Merge tag 'drm-intel-fixes-2015-07-15' into drm-intel-next-queued

Backmerge fixes since it's getting out of hand again with the massive
split due to atomic between -next and 4.2-rc. All the bugfixes in
4.2-rc are addressed already (by converting more towards atomic
instead of minimal duct-tape) so just always pick the version in next
for the conflicts in modeset code.

All the other conflicts are just adjacent lines changed.

Conflicts:
	drivers/gpu/drm/i915/i915_drv.h
	drivers/gpu/drm/i915/i915_gem_gtt.c
	drivers/gpu/drm/i915/intel_display.c
	drivers/gpu/drm/i915/intel_drv.h
	drivers/gpu/drm/i915/intel_ringbuffer.h

Signed-off-by: Daniel Vetter <daniel.vetter@intel.com>

4413 files changed, 354127 insertions, 130963 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index c0cc96bab9e7..6e973b8e3a3b 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -182,4 +182,6 @@ source "drivers/thunderbolt/Kconfig"
 
 source "drivers/android/Kconfig"
 
+source "drivers/nvdimm/Kconfig"
+
 endmenu
diff --git a/drivers/Makefile b/drivers/Makefile
index 46d2554be404..b64b49f6e01b 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -64,6 +64,7 @@ obj-$(CONFIG_FB_INTEL)          += video/fbdev/intelfb/
 
 obj-$(CONFIG_PARPORT)		+= parport/
 obj-y				+= base/ block/ misc/ mfd/ nfc/
+obj-$(CONFIG_LIBNVDIMM)		+= nvdimm/
 obj-$(CONFIG_DMA_SHARED_BUFFER) += dma-buf/
 obj-$(CONFIG_NUBUS)		+= nubus/
 obj-y				+= macintosh/
@@ -98,7 +99,6 @@ obj-$(CONFIG_USB_GADGET)	+= usb/
 obj-$(CONFIG_SERIO)		+= input/serio/
 obj-$(CONFIG_GAMEPORT)		+= input/gameport/
 obj-$(CONFIG_INPUT)		+= input/
-obj-$(CONFIG_I2O)		+= message/
 obj-$(CONFIG_RTC_LIB)		+= rtc/
 obj-y				+= i2c/ media/
 obj-$(CONFIG_PPS)		+= pps/
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index ab2cbb51c6aa..114cf48085ab 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -54,6 +54,9 @@ config ACPI_GENERIC_GSI
 config ACPI_SYSTEM_POWER_STATES_SUPPORT
 	bool
 
+config ACPI_CCA_REQUIRED
+	bool
+
 config ACPI_SLEEP
 	bool
 	depends on SUSPEND || HIBERNATION
@@ -62,7 +65,7 @@ config ACPI_SLEEP
 
 config ACPI_PROCFS_POWER
 	bool "Deprecated power /proc/acpi directories"
-	depends on PROC_FS
+	depends on X86 && PROC_FS
 	help
 	  For backwards compatibility, this option allows
           deprecated power /proc/acpi/ directories to exist, even when
@@ -77,6 +80,26 @@ config ACPI_PROCFS_POWER
 
 	  Say N to delete power /proc/acpi/ directories that have moved to /sys/
 
+config ACPI_REV_OVERRIDE_POSSIBLE
+	bool "Allow supported ACPI revision to be overriden"
+	depends on X86
+	default y
+	help
+	  The platform firmware on some systems expects Linux to return "5" as
+	  the supported ACPI revision which makes it expose system configuration
+	  information in a special way.
+
+	  For example, based on what ACPI exports as the supported revision,
+	  Dell XPS 13 (2015) configures its audio device to either work in HDA
+	  mode or in I2S mode, where the former is supposed to be used on Linux
+	  until the latter is fully supported (in the kernel as well as in user
+	  space).
+
+	  This option enables a DMI-based quirk for the above Dell machine (so
+	  that HDA audio is exposed by the platform firmware to the kernel) and
+	  makes it possible to force the kernel to return "5" as the supported
+	  ACPI revision via the "acpi_rev_override" command line switch.
+
 config ACPI_EC_DEBUGFS
 	tristate "EC read/write access through /sys/kernel/debug/ec"
 	default n
@@ -383,6 +406,32 @@ config ACPI_REDUCED_HARDWARE_ONLY
 
 	  If you are unsure what to do, do not enable this option.
 
+config ACPI_NFIT
+	tristate "ACPI NVDIMM Firmware Interface Table (NFIT)"
+	depends on PHYS_ADDR_T_64BIT
+	depends on BLK_DEV
+	select LIBNVDIMM
+	help
+	  Infrastructure to probe ACPI 6 compliant platforms for
+	  NVDIMMs (NFIT) and register a libnvdimm device tree.  In
+	  addition to storage devices this also enables libnvdimm to pass
+	  ACPI._DSM messages for platform/dimm configuration.
+
+	  To compile this driver as a module, choose M here:
+	  the module will be called nfit.
+
+config ACPI_NFIT_DEBUG
+	bool "NFIT DSM debug"
+	depends on ACPI_NFIT
+	depends on DYNAMIC_DEBUG
+	default n
+	help
+	  Enabling this option causes the nfit driver to dump the
+	  input and output buffers of _DSM operations on the ACPI0012
+	  device and its children.  This can be very verbose, so leave
+	  it disabled unless you are debugging a hardware / firmware
+	  issue.
+
 source "drivers/acpi/apei/Kconfig"
 
 config ACPI_EXTLOG
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index 8a063e276530..8321430d7f24 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -52,9 +52,6 @@ acpi-$(CONFIG_X86)		+= acpi_cmos_rtc.o
 acpi-$(CONFIG_DEBUG_FS)		+= debugfs.o
 acpi-$(CONFIG_ACPI_NUMA)	+= numa.o
 acpi-$(CONFIG_ACPI_PROCFS_POWER) += cm_sbs.o
-ifdef CONFIG_ACPI_VIDEO
-acpi-y				+= video_detect.o
-endif
 acpi-y				+= acpi_lpat.o
 acpi-$(CONFIG_ACPI_GENERIC_GSI) += gsi.o
 
@@ -71,6 +68,7 @@ obj-$(CONFIG_ACPI_PCI_SLOT)	+= pci_slot.o
 obj-$(CONFIG_ACPI_PROCESSOR)	+= processor.o
 obj-y				+= container.o
 obj-$(CONFIG_ACPI_THERMAL)	+= thermal.o
+obj-$(CONFIG_ACPI_NFIT)		+= nfit.o
 obj-y				+= acpi_memhotplug.o
 obj-$(CONFIG_ACPI_HOTPLUG_IOAPIC) += ioapic.o
 obj-$(CONFIG_ACPI_BATTERY)	+= battery.o
@@ -95,3 +93,5 @@ obj-$(CONFIG_ACPI_EXTLOG)	+= acpi_extlog.o
 obj-$(CONFIG_PMIC_OPREGION)	+= pmic/intel_pmic.o
 obj-$(CONFIG_CRC_PMIC_OPREGION) += pmic/intel_pmic_crc.o
 obj-$(CONFIG_XPOWER_PMIC_OPREGION) += pmic/intel_pmic_xpower.o
+
+video-objs			+= acpi_video.o video_detect.o
diff --git a/drivers/acpi/ac.c b/drivers/acpi/ac.c
index bbcc2b5a70d4..9b5354a2cd08 100644
--- a/drivers/acpi/ac.c
+++ b/drivers/acpi/ac.c
@@ -308,7 +308,7 @@ static int thinkpad_e530_quirk(const struct dmi_system_id *d)
 	return 0;
 }
 
-static struct dmi_system_id ac_dmi_table[] = {
+static const struct dmi_system_id ac_dmi_table[] = {
 	{
 	.callback = thinkpad_e530_quirk,
 	.ident = "thinkpad e530",
diff --git a/drivers/acpi/acpi_lpss.c b/drivers/acpi/acpi_lpss.c
index 37fb19047603..46b58abb08c5 100644
--- a/drivers/acpi/acpi_lpss.c
+++ b/drivers/acpi/acpi_lpss.c
@@ -129,50 +129,50 @@ static void byt_i2c_setup(struct lpss_private_data *pdata)
 	writel(0, pdata->mmio_base + LPSS_I2C_ENABLE);
 }
 
-static struct lpss_device_desc lpt_dev_desc = {
+static const struct lpss_device_desc lpt_dev_desc = {
 	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_LTR,
 	.prv_offset = 0x800,
 };
 
-static struct lpss_device_desc lpt_i2c_dev_desc = {
+static const struct lpss_device_desc lpt_i2c_dev_desc = {
 	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_LTR,
 	.prv_offset = 0x800,
 };
 
-static struct lpss_device_desc lpt_uart_dev_desc = {
+static const struct lpss_device_desc lpt_uart_dev_desc = {
 	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_LTR,
 	.clk_con_id = "baudclk",
 	.prv_offset = 0x800,
 	.setup = lpss_uart_setup,
 };
 
-static struct lpss_device_desc lpt_sdio_dev_desc = {
+static const struct lpss_device_desc lpt_sdio_dev_desc = {
 	.flags = LPSS_LTR,
 	.prv_offset = 0x1000,
 	.prv_size_override = 0x1018,
 };
 
-static struct lpss_device_desc byt_pwm_dev_desc = {
+static const struct lpss_device_desc byt_pwm_dev_desc = {
 	.flags = LPSS_SAVE_CTX,
 };
 
-static struct lpss_device_desc byt_uart_dev_desc = {
+static const struct lpss_device_desc byt_uart_dev_desc = {
 	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_SAVE_CTX,
 	.clk_con_id = "baudclk",
 	.prv_offset = 0x800,
 	.setup = lpss_uart_setup,
 };
 
-static struct lpss_device_desc byt_spi_dev_desc = {
+static const struct lpss_device_desc byt_spi_dev_desc = {
 	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_SAVE_CTX,
 	.prv_offset = 0x400,
 };
 
-static struct lpss_device_desc byt_sdio_dev_desc = {
+static const struct lpss_device_desc byt_sdio_dev_desc = {
 	.flags = LPSS_CLK,
 };
 
-static struct lpss_device_desc byt_i2c_dev_desc = {
+static const struct lpss_device_desc byt_i2c_dev_desc = {
 	.flags = LPSS_CLK | LPSS_SAVE_CTX,
 	.prv_offset = 0x800,
 	.setup = byt_i2c_setup,
@@ -323,14 +323,14 @@ out:
 static int acpi_lpss_create_device(struct acpi_device *adev,
 				   const struct acpi_device_id *id)
 {
-	struct lpss_device_desc *dev_desc;
+	const struct lpss_device_desc *dev_desc;
 	struct lpss_private_data *pdata;
 	struct resource_entry *rentry;
 	struct list_head resource_list;
 	struct platform_device *pdev;
 	int ret;
 
-	dev_desc = (struct lpss_device_desc *)id->driver_data;
+	dev_desc = (const struct lpss_device_desc *)id->driver_data;
 	if (!dev_desc) {
 		pdev = acpi_create_platform_device(adev);
 		return IS_ERR_OR_NULL(pdev) ? PTR_ERR(pdev) : 1;
@@ -352,13 +352,16 @@ static int acpi_lpss_create_device(struct acpi_device *adev,
 				pdata->mmio_size = resource_size(rentry->res);
 			pdata->mmio_base = ioremap(rentry->res->start,
 						   pdata->mmio_size);
-			if (!pdata->mmio_base)
-				goto err_out;
 			break;
 		}
 
 	acpi_dev_free_resource_list(&resource_list);
 
+	if (!pdata->mmio_base) {
+		ret = -ENOMEM;
+		goto err_out;
+	}
+
 	pdata->dev_desc = dev_desc;
 
 	if (dev_desc->setup)
diff --git a/drivers/acpi/acpi_pad.c b/drivers/acpi/acpi_pad.c
index 6bc9cbc01ad6..00b39802d7ec 100644
--- a/drivers/acpi/acpi_pad.c
+++ b/drivers/acpi/acpi_pad.c
@@ -105,7 +105,7 @@ static void round_robin_cpu(unsigned int tsk_index)
 	mutex_lock(&round_robin_lock);
 	cpumask_clear(tmp);
 	for_each_cpu(cpu, pad_busy_cpus)
-		cpumask_or(tmp, tmp, topology_thread_cpumask(cpu));
+		cpumask_or(tmp, tmp, topology_sibling_cpumask(cpu));
 	cpumask_andnot(tmp, cpu_online_mask, tmp);
 	/* avoid HT sibilings if possible */
 	if (cpumask_empty(tmp))
diff --git a/drivers/acpi/acpi_platform.c b/drivers/acpi/acpi_platform.c
index 4bf75597f732..06a67d5f2846 100644
--- a/drivers/acpi/acpi_platform.c
+++ b/drivers/acpi/acpi_platform.c
@@ -103,7 +103,7 @@ struct platform_device *acpi_create_platform_device(struct acpi_device *adev)
 	pdevinfo.res = resources;
 	pdevinfo.num_res = count;
 	pdevinfo.fwnode = acpi_fwnode_handle(adev);
-	pdevinfo.dma_mask = DMA_BIT_MASK(32);
+	pdevinfo.dma_mask = acpi_check_dma(adev, NULL) ? DMA_BIT_MASK(32) : 0;
 	pdev = platform_device_register_full(&pdevinfo);
 	if (IS_ERR(pdev))
 		dev_err(&adev->dev, "platform device creation failed: %ld\n",
diff --git a/drivers/acpi/acpi_processor.c b/drivers/acpi/acpi_processor.c
index 58f335ca2e75..92a5f738e370 100644
--- a/drivers/acpi/acpi_processor.c
+++ b/drivers/acpi/acpi_processor.c
@@ -170,7 +170,7 @@ static int acpi_processor_hotadd_init(struct acpi_processor *pr)
 	acpi_status status;
 	int ret;
 
-	if (pr->phys_id == PHYS_CPUID_INVALID)
+	if (invalid_phys_cpuid(pr->phys_id))
 		return -ENODEV;
 
 	status = acpi_evaluate_integer(pr->handle, "_STA", NULL, &sta);
@@ -215,8 +215,7 @@ static int acpi_processor_get_info(struct acpi_device *device)
 	union acpi_object object = { 0 };
 	struct acpi_buffer buffer = { sizeof(union acpi_object), &object };
 	struct acpi_processor *pr = acpi_driver_data(device);
-	phys_cpuid_t phys_id;
-	int cpu_index, device_declaration = 0;
+	int device_declaration = 0;
 	acpi_status status = AE_OK;
 	static int cpu0_initialized;
 	unsigned long long value;
@@ -263,29 +262,28 @@ static int acpi_processor_get_info(struct acpi_device *device)
 		pr->acpi_id = value;
 	}
 
-	phys_id = acpi_get_phys_id(pr->handle, device_declaration, pr->acpi_id);
-	if (phys_id == PHYS_CPUID_INVALID)
+	pr->phys_id = acpi_get_phys_id(pr->handle, device_declaration,
+					pr->acpi_id);
+	if (invalid_phys_cpuid(pr->phys_id))
 		acpi_handle_debug(pr->handle, "failed to get CPU physical ID.\n");
-	pr->phys_id = phys_id;
 
-	cpu_index = acpi_map_cpuid(pr->phys_id, pr->acpi_id);
+	pr->id = acpi_map_cpuid(pr->phys_id, pr->acpi_id);
 	if (!cpu0_initialized && !acpi_has_cpu_in_madt()) {
 		cpu0_initialized = 1;
 		/*
 		 * Handle UP system running SMP kernel, with no CPU
 		 * entry in MADT
 		 */
-		if ((cpu_index == -1) && (num_online_cpus() == 1))
-			cpu_index = 0;
+		if (invalid_logical_cpuid(pr->id) && (num_online_cpus() == 1))
+			pr->id = 0;
 	}
-	pr->id = cpu_index;
 
 	/*
 	 *  Extra Processor objects may be enumerated on MP systems with
 	 *  less than the max # of CPUs. They should be ignored _iff
 	 *  they are physically not present.
 	 */
-	if (pr->id == -1) {
+	if (invalid_logical_cpuid(pr->id)) {
 		int ret = acpi_processor_hotadd_init(pr);
 		if (ret)
 			return ret;
diff --git a/drivers/acpi/video.c b/drivers/acpi/acpi_video.c
index cc79d3fedfb2..8c2fe2f2f9fd 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/acpi_video.c
@@ -43,7 +43,7 @@
 #include <acpi/video.h>
 #include <asm/uaccess.h>
 
-#include "internal.h"
+#define PREFIX "ACPI: "
 
 #define ACPI_VIDEO_BUS_NAME		"Video Bus"
 #define ACPI_VIDEO_DEVICE_NAME		"Video Device"
@@ -78,26 +78,17 @@ module_param(brightness_switch_enabled, bool, 0644);
 static bool allow_duplicates;
 module_param(allow_duplicates, bool, 0644);
 
-/*
- * For Windows 8 systems: used to decide if video module
- * should skip registering backlight interface of its own.
- */
-enum {
-	NATIVE_BACKLIGHT_NOT_SET = -1,
-	NATIVE_BACKLIGHT_OFF,
-	NATIVE_BACKLIGHT_ON,
-};
-
-static int use_native_backlight_param = NATIVE_BACKLIGHT_NOT_SET;
-module_param_named(use_native_backlight, use_native_backlight_param, int, 0444);
-static int use_native_backlight_dmi = NATIVE_BACKLIGHT_NOT_SET;
+static int disable_backlight_sysfs_if = -1;
+module_param(disable_backlight_sysfs_if, int, 0444);
 
 static int register_count;
+static DEFINE_MUTEX(register_count_mutex);
 static struct mutex video_list_lock;
 static struct list_head video_bus_head;
 static int acpi_video_bus_add(struct acpi_device *device);
 static int acpi_video_bus_remove(struct acpi_device *device);
 static void acpi_video_bus_notify(struct acpi_device *device, u32 event);
+void acpi_video_detect_exit(void);
 
 static const struct acpi_device_id video_device_ids[] = {
 	{ACPI_VIDEO_HID, 0},
@@ -157,7 +148,6 @@ struct acpi_video_enumerated_device {
 struct acpi_video_bus {
 	struct acpi_device *device;
 	bool backlight_registered;
-	bool backlight_notifier_registered;
 	u8 dos_setting;
 	struct acpi_video_enumerated_device *attached_array;
 	u8 attached_count;
@@ -170,7 +160,6 @@ struct acpi_video_bus {
 	struct input_dev *input;
 	char phys[32];	/* for input device */
 	struct notifier_block pm_nb;
-	struct notifier_block backlight_nb;
 };
 
 struct acpi_video_device_flags {
@@ -241,24 +230,6 @@ static int acpi_video_get_next_level(struct acpi_video_device *device,
 				     u32 level_current, u32 event);
 static void acpi_video_switch_brightness(struct work_struct *work);
 
-static bool acpi_video_use_native_backlight(void)
-{
-	if (use_native_backlight_param != NATIVE_BACKLIGHT_NOT_SET)
-		return use_native_backlight_param;
-	else if (use_native_backlight_dmi != NATIVE_BACKLIGHT_NOT_SET)
-		return use_native_backlight_dmi;
-	return acpi_osi_is_win8();
-}
-
-bool acpi_video_verify_backlight_support(void)
-{
-	if (acpi_video_use_native_backlight() &&
-	    backlight_device_registered(BACKLIGHT_RAW))
-		return false;
-	return acpi_video_backlight_support();
-}
-EXPORT_SYMBOL_GPL(acpi_video_verify_backlight_support);
-
 /* backlight device sysfs support */
 static int acpi_video_get_brightness(struct backlight_device *bd)
 {
@@ -413,25 +384,21 @@ acpi_video_device_lcd_set_level(struct acpi_video_device *device, int level)
  */
 
 static int bqc_offset_aml_bug_workaround;
-static int __init video_set_bqc_offset(const struct dmi_system_id *d)
+static int video_set_bqc_offset(const struct dmi_system_id *d)
 {
 	bqc_offset_aml_bug_workaround = 9;
 	return 0;
 }
 
-static int __init video_disable_native_backlight(const struct dmi_system_id *d)
-{
-	use_native_backlight_dmi = NATIVE_BACKLIGHT_OFF;
-	return 0;
-}
-
-static int __init video_enable_native_backlight(const struct dmi_system_id *d)
+static int video_disable_backlight_sysfs_if(
+	const struct dmi_system_id *d)
 {
-	use_native_backlight_dmi = NATIVE_BACKLIGHT_ON;
+	if (disable_backlight_sysfs_if == -1)
+		disable_backlight_sysfs_if = 1;
 	return 0;
 }
 
-static struct dmi_system_id video_dmi_table[] __initdata = {
+static struct dmi_system_id video_dmi_table[] = {
 	/*
 	 * Broken _BQC workaround http://bugzilla.kernel.org/show_bug.cgi?id=13121
 	 */
@@ -477,110 +444,19 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 	},
 
 	/*
-	 * These models have a working acpi_video backlight control, and using
-	 * native backlight causes a regression where backlight does not work
-	 * when userspace is not handling brightness key events. Disable
-	 * native_backlight on these to fix this:
-	 * https://bugzilla.kernel.org/show_bug.cgi?id=81691
+	 * Some machines have a broken acpi-video interface for brightness
+	 * control, but still need an acpi_video_device_lcd_set_level() call
+	 * on resume to turn the backlight power on.  We Enable backlight
+	 * control on these systems, but do not register a backlight sysfs
+	 * as brightness control does not work.
 	 */
 	{
-	 .callback = video_disable_native_backlight,
-	 .ident = "ThinkPad T420",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T420"),
-		},
-	},
-	{
-	 .callback = video_disable_native_backlight,
-	 .ident = "ThinkPad T520",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T520"),
-		},
-	},
-	{
-	 .callback = video_disable_native_backlight,
-	 .ident = "ThinkPad X201s",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X201s"),
-		},
-	},
-
-	/* The native backlight controls do not work on some older machines */
-	{
-	 /* https://bugs.freedesktop.org/show_bug.cgi?id=81515 */
-	 .callback = video_disable_native_backlight,
-	 .ident = "HP ENVY 15 Notebook",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		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"),
-		},
-	},
-	{
-	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1186097 */
-	 .callback = video_disable_native_backlight,
-	 .ident = "SAMSUNG 3570R/370R/470R/450R/510R/4450RV",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
-		DMI_MATCH(DMI_PRODUCT_NAME, "3570R/370R/470R/450R/510R/4450RV"),
-		},
-	},
-	{
-	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1094948 */
-	 .callback = video_disable_native_backlight,
-	 .ident = "SAMSUNG 730U3E/740U3E",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
-		DMI_MATCH(DMI_PRODUCT_NAME, "730U3E/740U3E"),
-		},
-	},
-	{
-	 /* https://bugs.freedesktop.org/show_bug.cgi?id=87286 */
-	 .callback = video_disable_native_backlight,
-	 .ident = "SAMSUNG 900X3C/900X3D/900X3E/900X4C/900X4D",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
-		DMI_MATCH(DMI_PRODUCT_NAME, "900X3C/900X3D/900X3E/900X4C/900X4D"),
-		},
-	},
-
-	{
-	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1163574 */
-	 .callback = video_disable_native_backlight,
-	 .ident = "Dell XPS15 L521X",
+	 /* https://bugs.freedesktop.org/show_bug.cgi?id=82634 */
+	 .callback = video_disable_backlight_sysfs_if,
+	 .ident = "Toshiba Portege R830",
 	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-		DMI_MATCH(DMI_PRODUCT_NAME, "XPS L521X"),
-		},
-	},
-
-	/* Non win8 machines which need native backlight nevertheless */
-	{
-	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1187004 */
-	 .callback = video_enable_native_backlight,
-	 .ident = "Lenovo Ideapad Z570",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "102434U"),
+		DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "PORTEGE R830"),
 		},
 	},
 	{}
@@ -1382,7 +1258,7 @@ acpi_video_switch_brightness(struct work_struct *work)
 	int result = -EINVAL;
 
 	/* no warning message if acpi_backlight=vendor or a quirk is used */
-	if (!acpi_video_verify_backlight_support())
+	if (!device->backlight)
 		return;
 
 	if (!device->brightness)
@@ -1657,8 +1533,9 @@ static int acpi_video_resume(struct notifier_block *nb,
 
 	for (i = 0; i < video->attached_count; i++) {
 		video_device = video->attached_array[i].bind_info;
-		if (video_device && video_device->backlight)
-			acpi_video_set_brightness(video_device->backlight);
+		if (video_device && video_device->brightness)
+			acpi_video_device_lcd_set_level(video_device,
+					video_device->brightness->curr);
 	}
 
 	return NOTIFY_OK;
@@ -1707,6 +1584,10 @@ static void acpi_video_dev_register_backlight(struct acpi_video_device *device)
 	result = acpi_video_init_brightness(device);
 	if (result)
 		return;
+
+	if (disable_backlight_sysfs_if > 0)
+		return;
+
 	name = kasprintf(GFP_KERNEL, "acpi_video%d", count);
 	if (!name)
 		return;
@@ -1729,8 +1610,10 @@ static void acpi_video_dev_register_backlight(struct acpi_video_device *device)
 						      &acpi_backlight_ops,
 						      &props);
 	kfree(name);
-	if (IS_ERR(device->backlight))
+	if (IS_ERR(device->backlight)) {
+		device->backlight = NULL;
 		return;
+	}
 
 	/*
 	 * Save current brightness level in case we have to restore it
@@ -1787,7 +1670,7 @@ static int acpi_video_bus_register_backlight(struct acpi_video_bus *video)
 
 	acpi_video_run_bcl_for_osi(video);
 
-	if (!acpi_video_verify_backlight_support())
+	if (acpi_video_get_backlight_type() != acpi_backlight_video)
 		return 0;
 
 	mutex_lock(&video->device_list_lock);
@@ -1931,56 +1814,6 @@ static void acpi_video_bus_remove_notify_handler(struct acpi_video_bus *video)
 	video->input = NULL;
 }
 
-static int acpi_video_backlight_notify(struct notifier_block *nb,
-					unsigned long val, void *bd)
-{
-	struct backlight_device *backlight = bd;
-	struct acpi_video_bus *video;
-
-	/* acpi_video_verify_backlight_support only cares about raw devices */
-	if (backlight->props.type != BACKLIGHT_RAW)
-		return NOTIFY_DONE;
-
-	video = container_of(nb, struct acpi_video_bus, backlight_nb);
-
-	switch (val) {
-	case BACKLIGHT_REGISTERED:
-		if (!acpi_video_verify_backlight_support())
-			acpi_video_bus_unregister_backlight(video);
-		break;
-	case BACKLIGHT_UNREGISTERED:
-		acpi_video_bus_register_backlight(video);
-		break;
-	}
-
-	return NOTIFY_OK;
-}
-
-static int acpi_video_bus_add_backlight_notify_handler(
-						struct acpi_video_bus *video)
-{
-	int error;
-
-	video->backlight_nb.notifier_call = acpi_video_backlight_notify;
-	video->backlight_nb.priority = 0;
-	error = backlight_register_notifier(&video->backlight_nb);
-	if (error == 0)
-		video->backlight_notifier_registered = true;
-
-	return error;
-}
-
-static int acpi_video_bus_remove_backlight_notify_handler(
-						struct acpi_video_bus *video)
-{
-	if (!video->backlight_notifier_registered)
-		return 0;
-
-	video->backlight_notifier_registered = false;
-
-	return backlight_unregister_notifier(&video->backlight_nb);
-}
-
 static int acpi_video_bus_put_devices(struct acpi_video_bus *video)
 {
 	struct acpi_video_device *dev, *next;
@@ -2062,7 +1895,6 @@ static int acpi_video_bus_add(struct acpi_device *device)
 
 	acpi_video_bus_register_backlight(video);
 	acpi_video_bus_add_notify_handler(video);
-	acpi_video_bus_add_backlight_notify_handler(video);
 
 	return 0;
 
@@ -2086,7 +1918,6 @@ static int acpi_video_bus_remove(struct acpi_device *device)
 
 	video = acpi_driver_data(device);
 
-	acpi_video_bus_remove_backlight_notify_handler(video);
 	acpi_video_bus_remove_notify_handler(video);
 	acpi_video_bus_unregister_backlight(video);
 	acpi_video_bus_put_devices(video);
@@ -2134,22 +1965,25 @@ static int __init intel_opregion_present(void)
 
 int acpi_video_register(void)
 {
-	int ret;
+	int ret = 0;
 
+	mutex_lock(&register_count_mutex);
 	if (register_count) {
 		/*
 		 * if the function of acpi_video_register is already called,
 		 * don't register the acpi_vide_bus again and return no error.
 		 */
-		return 0;
+		goto leave;
 	}
 
 	mutex_init(&video_list_lock);
 	INIT_LIST_HEAD(&video_bus_head);
 
+	dmi_check_system(video_dmi_table);
+
 	ret = acpi_bus_register_driver(&acpi_video_bus);
 	if (ret)
-		return ret;
+		goto leave;
 
 	/*
 	 * When the acpi_video_bus is loaded successfully, increase
@@ -2157,24 +1991,20 @@ int acpi_video_register(void)
 	 */
 	register_count = 1;
 
-	return 0;
+leave:
+	mutex_unlock(&register_count_mutex);
+	return ret;
 }
 EXPORT_SYMBOL(acpi_video_register);
 
 void acpi_video_unregister(void)
 {
-	if (!register_count) {
-		/*
-		 * If the acpi video bus is already unloaded, don't
-		 * unload it again and return directly.
-		 */
-		return;
+	mutex_lock(&register_count_mutex);
+	if (register_count) {
+		acpi_bus_unregister_driver(&acpi_video_bus);
+		register_count = 0;
 	}
-	acpi_bus_unregister_driver(&acpi_video_bus);
-
-	register_count = 0;
-
-	return;
+	mutex_unlock(&register_count_mutex);
 }
 EXPORT_SYMBOL(acpi_video_unregister);
 
@@ -2182,15 +2012,15 @@ void acpi_video_unregister_backlight(void)
 {
 	struct acpi_video_bus *video;
 
-	if (!register_count)
-		return;
-
-	mutex_lock(&video_list_lock);
-	list_for_each_entry(video, &video_bus_head, entry)
-		acpi_video_bus_unregister_backlight(video);
-	mutex_unlock(&video_list_lock);
+	mutex_lock(&register_count_mutex);
+	if (register_count) {
+		mutex_lock(&video_list_lock);
+		list_for_each_entry(video, &video_bus_head, entry)
+			acpi_video_bus_unregister_backlight(video);
+		mutex_unlock(&video_list_lock);
+	}
+	mutex_unlock(&register_count_mutex);
 }
-EXPORT_SYMBOL(acpi_video_unregister_backlight);
 
 /*
  * This is kind of nasty. Hardware using Intel chipsets may require
@@ -2212,8 +2042,6 @@ static int __init acpi_video_init(void)
 	if (acpi_disabled)
 		return 0;
 
-	dmi_check_system(video_dmi_table);
-
 	if (intel_opregion_present())
 		return 0;
 
@@ -2222,6 +2050,7 @@ static int __init acpi_video_init(void)
 
 static void __exit acpi_video_exit(void)
 {
+	acpi_video_detect_exit();
 	acpi_video_unregister();
 
 	return;
diff --git a/drivers/acpi/acpica/accommon.h b/drivers/acpi/acpica/accommon.h
index 853aa2dbdb61..a8d8092ee391 100644
--- a/drivers/acpi/acpica/accommon.h
+++ b/drivers/acpi/acpica/accommon.h
@@ -59,5 +59,8 @@
 #include "acglobal.h"		/* All global variables */
 #include "achware.h"		/* Hardware defines and interfaces */
 #include "acutils.h"		/* Utility interfaces */
+#ifndef ACPI_USE_SYSTEM_CLIBRARY
+#include "acclib.h"		/* C library interfaces */
+#endif				/* !ACPI_USE_SYSTEM_CLIBRARY */
 
 #endif				/* __ACCOMMON_H__ */
diff --git a/drivers/acpi/acpica/acdebug.h b/drivers/acpi/acpica/acdebug.h
index 4169bb87a996..43685dd36c77 100644
--- a/drivers/acpi/acpica/acdebug.h
+++ b/drivers/acpi/acpica/acdebug.h
@@ -231,7 +231,9 @@ void acpi_db_open_debug_file(char *name);
 acpi_status acpi_db_load_acpi_table(char *filename);
 
 acpi_status
-acpi_db_get_table_from_file(char *filename, struct acpi_table_header **table);
+acpi_db_get_table_from_file(char *filename,
+			    struct acpi_table_header **table,
+			    u8 must_be_aml_table);
 
 /*
  * dbhistry - debugger HISTORY command
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index a0c478784314..53f96a370762 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -61,6 +61,8 @@ ACPI_GLOBAL(struct acpi_table_header, acpi_gbl_original_dsdt_header);
 
 #if (!ACPI_REDUCED_HARDWARE)
 ACPI_GLOBAL(struct acpi_table_facs *, acpi_gbl_FACS);
+ACPI_GLOBAL(struct acpi_table_facs *, acpi_gbl_facs32);
+ACPI_GLOBAL(struct acpi_table_facs *, acpi_gbl_facs64);
 
 #endif				/* !ACPI_REDUCED_HARDWARE */
 
diff --git a/drivers/acpi/acpica/acinterp.h b/drivers/acpi/acpica/acinterp.h
index 1886bde54b5d..7ac98000b46b 100644
--- a/drivers/acpi/acpica/acinterp.h
+++ b/drivers/acpi/acpica/acinterp.h
@@ -468,6 +468,8 @@ void acpi_ex_eisa_id_to_string(char *dest, u64 compressed_id);
 
 void acpi_ex_integer_to_string(char *dest, u64 value);
 
+void acpi_ex_pci_cls_to_string(char *dest, u8 class_code[3]);
+
 u8 acpi_is_valid_space_id(u8 space_id);
 
 /*
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index 87b27521fcac..bc600969c6a1 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -213,6 +213,7 @@ struct acpi_table_list {
 
 #define ACPI_TABLE_INDEX_DSDT           (0)
 #define ACPI_TABLE_INDEX_FACS           (1)
+#define ACPI_TABLE_INDEX_X_FACS         (2)
 
 struct acpi_find_context {
 	char *search_for;
@@ -352,11 +353,21 @@ struct acpi_package_info3 {
 	u16 reserved;
 };
 
+struct acpi_package_info4 {
+	u8 type;
+	u8 object_type1;
+	u8 count1;
+	u8 sub_object_types;
+	u8 pkg_count;
+	u16 reserved;
+};
+
 union acpi_predefined_info {
 	struct acpi_name_info info;
 	struct acpi_package_info ret_info;
 	struct acpi_package_info2 ret_info2;
 	struct acpi_package_info3 ret_info3;
+	struct acpi_package_info4 ret_info4;
 };
 
 /* Reset to default packing */
@@ -1165,4 +1176,9 @@ struct ah_uuid {
 	char *string;
 };
 
+struct ah_table {
+	char *signature;
+	char *description;
+};
+
 #endif				/* __ACLOCAL_H__ */
diff --git a/drivers/acpi/acpica/acnamesp.h b/drivers/acpi/acpica/acnamesp.h
index 952fbe0b7231..0dd088290d80 100644
--- a/drivers/acpi/acpica/acnamesp.h
+++ b/drivers/acpi/acpica/acnamesp.h
@@ -66,6 +66,7 @@
 #define ACPI_NS_PREFIX_IS_SCOPE     0x10
 #define ACPI_NS_EXTERNAL            0x20
 #define ACPI_NS_TEMPORARY           0x40
+#define ACPI_NS_OVERRIDE_IF_FOUND   0x80
 
 /* Flags for acpi_ns_walk_namespace */
 
diff --git a/drivers/acpi/acpica/acobject.h b/drivers/acpi/acpica/acobject.h
index 3e9720e1f34f..c81d98d09cac 100644
--- a/drivers/acpi/acpica/acobject.h
+++ b/drivers/acpi/acpica/acobject.h
@@ -335,6 +335,7 @@ struct acpi_object_reference {
 	void *object;		/* name_op=>HANDLE to obj, index_op=>union acpi_operand_object */
 	struct acpi_namespace_node *node;	/* ref_of or Namepath */
 	union acpi_operand_object **where;	/* Target of Index */
+	u8 *index_pointer;	/* Used for Buffers and Strings */
 	u32 value;		/* Used for Local/Arg/Index/ddb_handle */
 };
 
diff --git a/drivers/acpi/acpica/acparser.h b/drivers/acpi/acpica/acparser.h
index 74a390c6db16..0cdd2fce493a 100644
--- a/drivers/acpi/acpica/acparser.h
+++ b/drivers/acpi/acpica/acparser.h
@@ -70,6 +70,9 @@
  *
  *****************************************************************************/
 
+extern const u8 acpi_gbl_short_op_index[];
+extern const u8 acpi_gbl_long_op_index[];
+
 /*
  * psxface - Parser external interfaces
  */
diff --git a/drivers/acpi/acpica/acpredef.h b/drivers/acpi/acpica/acpredef.h
index a972d11c97c9..b9474b529fcb 100644
--- a/drivers/acpi/acpica/acpredef.h
+++ b/drivers/acpi/acpica/acpredef.h
@@ -105,6 +105,11 @@
  *      count = 0 (optional)
  *      (Used for _DLM)
  *
+ * ACPI_PTYPE2_VAR_VAR: Variable number of subpackages, each of either a
+ *      constant or variable length. The subpackages are preceded by a
+ *      constant number of objects.
+ *      (Used for _LPI, _RDI)
+ *
  * ACPI_PTYPE2_UUID_PAIR: Each subpackage is preceded by a UUID Buffer. The UUID
  *      defines the format of the package. Zero-length parent package is
  *      allowed.
@@ -123,7 +128,8 @@ enum acpi_return_package_types {
 	ACPI_PTYPE2_MIN = 8,
 	ACPI_PTYPE2_REV_FIXED = 9,
 	ACPI_PTYPE2_FIX_VAR = 10,
-	ACPI_PTYPE2_UUID_PAIR = 11
+	ACPI_PTYPE2_VAR_VAR = 11,
+	ACPI_PTYPE2_UUID_PAIR = 12
 };
 
 /* Support macros for users of the predefined info table */
@@ -172,7 +178,7 @@ enum acpi_return_package_types {
  * These are the names that can actually be evaluated via acpi_evaluate_object.
  * Not present in this table are the following:
  *
- *      1) Predefined/Reserved names that are never evaluated via
+ *      1) Predefined/Reserved names that are not usually evaluated via
  *         acpi_evaluate_object:
  *              _Lxx and _Exx GPE methods
  *              _Qxx EC methods
@@ -361,6 +367,9 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (4 Int) */
 	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_INTEGER, 4, 0, 0, 0),
 
+	{{"_BTH", METHOD_1ARGS(ACPI_TYPE_INTEGER),	/* ACPI 6.0 */
+	  METHOD_NO_RETURN_VALUE}},
+
 	{{"_BTM", METHOD_1ARGS(ACPI_TYPE_INTEGER),
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
@@ -390,6 +399,9 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_INTEGER | ACPI_RTYPE_BUFFER, 0,
 		     0, 0, 0),
 
+	{{"_CR3", METHOD_0ARGS,	/* ACPI 6.0 */
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
 	{{"_CRS", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
 
@@ -445,7 +457,7 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	{{"_DOS", METHOD_1ARGS(ACPI_TYPE_INTEGER),
 	  METHOD_NO_RETURN_VALUE}},
 
-	{{"_DSD", METHOD_0ARGS,
+	{{"_DSD", METHOD_0ARGS,	/* ACPI 6.0 */
 	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each: 1 Buf, 1 Pkg */
 	PACKAGE_INFO(ACPI_PTYPE2_UUID_PAIR, ACPI_RTYPE_BUFFER, 1,
 		     ACPI_RTYPE_PACKAGE, 1, 0),
@@ -604,6 +616,12 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (1 Int(rev), n Pkg (2 Int) */
 	PACKAGE_INFO(ACPI_PTYPE2_REV_FIXED, ACPI_RTYPE_INTEGER, 2, 0, 0, 0),
 
+	{{"_LPI", METHOD_0ARGS,	/* ACPI 6.0 */
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (3 Int, n Pkg (10 Int/Buf) */
+	PACKAGE_INFO(ACPI_PTYPE2_VAR_VAR, ACPI_RTYPE_INTEGER, 3,
+		     ACPI_RTYPE_INTEGER | ACPI_RTYPE_BUFFER | ACPI_RTYPE_STRING,
+		     10, 0),
+
 	{{"_MAT", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
 
@@ -624,6 +642,9 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 		       ACPI_TYPE_INTEGER),
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
+	{{"_MTL", METHOD_0ARGS,	/* ACPI 6.0 */
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
 	{{"_NTT", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
@@ -716,6 +737,10 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Refs) */
 	PACKAGE_INFO(ACPI_PTYPE1_VAR, ACPI_RTYPE_REFERENCE, 0, 0, 0, 0),
 
+	{{"_PRR", METHOD_0ARGS,	/* ACPI 6.0 */
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Fixed-length (1 Ref) */
+	PACKAGE_INFO(ACPI_PTYPE1_FIXED, ACPI_RTYPE_REFERENCE, 1, 0, 0, 0),
+
 	{{"_PRS", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
 
@@ -796,6 +821,11 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	{{"_PXM", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
+	{{"_RDI", METHOD_0ARGS,	/* ACPI 6.0 */
+	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (1 Int, n Pkg (m Ref)) */
+	PACKAGE_INFO(ACPI_PTYPE2_VAR_VAR, ACPI_RTYPE_INTEGER, 1,
+		     ACPI_RTYPE_REFERENCE, 0, 0),
+
 	{{"_REG", METHOD_2ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
 	  METHOD_NO_RETURN_VALUE}},
 
@@ -808,6 +838,9 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	{{"_ROM", METHOD_2ARGS(ACPI_TYPE_INTEGER, ACPI_TYPE_INTEGER),
 	  METHOD_RETURNS(ACPI_RTYPE_BUFFER)}},
 
+	{{"_RST", METHOD_0ARGS,	/* ACPI 6.0 */
+	  METHOD_NO_RETURN_VALUE}},
+
 	{{"_RTV", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
@@ -935,6 +968,9 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	{{"_TDL", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
+	{{"_TFP", METHOD_0ARGS,	/* ACPI 6.0 */
+	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
+
 	{{"_TIP", METHOD_1ARGS(ACPI_TYPE_INTEGER),
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
@@ -959,6 +995,9 @@ const union acpi_predefined_info acpi_gbl_predefined_methods[] = {
 	  METHOD_RETURNS(ACPI_RTYPE_PACKAGE)}},	/* Variable-length (Pkgs) each 5 Int with count */
 	PACKAGE_INFO(ACPI_PTYPE2_COUNT, ACPI_RTYPE_INTEGER, 5, 0, 0, 0),
 
+	{{"_TSN", METHOD_0ARGS,	/* ACPI 6.0 */
+	  METHOD_RETURNS(ACPI_RTYPE_REFERENCE)}},
+
 	{{"_TSP", METHOD_0ARGS,
 	  METHOD_RETURNS(ACPI_RTYPE_INTEGER)}},
 
diff --git a/drivers/acpi/acpica/acstruct.h b/drivers/acpi/acpica/acstruct.h
index 87c7860b3394..44997ca02ae2 100644
--- a/drivers/acpi/acpica/acstruct.h
+++ b/drivers/acpi/acpica/acstruct.h
@@ -82,6 +82,7 @@ struct acpi_walk_state {
 	u8 return_used;
 	u8 scope_depth;
 	u8 pass_number;		/* Parse pass during table load */
+	u8 namespace_override;	/* Override existing objects */
 	u8 result_size;		/* Total elements for the result stack */
 	u8 result_count;	/* Current number of occupied elements of result stack */
 	u32 aml_offset;
diff --git a/drivers/acpi/acpica/acutils.h b/drivers/acpi/acpica/acutils.h
index 2b3c5bd222f1..6de0d3573037 100644
--- a/drivers/acpi/acpica/acutils.h
+++ b/drivers/acpi/acpica/acutils.h
@@ -205,66 +205,6 @@ acpi_status acpi_ut_hardware_initialize(void);
 
 void acpi_ut_subsystem_shutdown(void);
 
-/*
- * utclib - Local implementations of C library functions
- */
-#ifndef ACPI_USE_SYSTEM_CLIBRARY
-
-acpi_size acpi_ut_strlen(const char *string);
-
-char *acpi_ut_strchr(const char *string, int ch);
-
-char *acpi_ut_strcpy(char *dst_string, const char *src_string);
-
-char *acpi_ut_strncpy(char *dst_string,
-		      const char *src_string, acpi_size count);
-
-int acpi_ut_memcmp(const char *buffer1, const char *buffer2, acpi_size count);
-
-int acpi_ut_strncmp(const char *string1, const char *string2, acpi_size count);
-
-int acpi_ut_strcmp(const char *string1, const char *string2);
-
-char *acpi_ut_strcat(char *dst_string, const char *src_string);
-
-char *acpi_ut_strncat(char *dst_string,
-		      const char *src_string, acpi_size count);
-
-u32 acpi_ut_strtoul(const char *string, char **terminator, u32 base);
-
-char *acpi_ut_strstr(char *string1, char *string2);
-
-void *acpi_ut_memcpy(void *dest, const void *src, acpi_size count);
-
-void *acpi_ut_memset(void *dest, u8 value, acpi_size count);
-
-int acpi_ut_to_upper(int c);
-
-int acpi_ut_to_lower(int c);
-
-extern const u8 _acpi_ctype[];
-
-#define _ACPI_XA     0x00	/* extra alphabetic - not supported */
-#define _ACPI_XS     0x40	/* extra space */
-#define _ACPI_BB     0x00	/* BEL, BS, etc. - not supported */
-#define _ACPI_CN     0x20	/* CR, FF, HT, NL, VT */
-#define _ACPI_DI     0x04	/* '0'-'9' */
-#define _ACPI_LO     0x02	/* 'a'-'z' */
-#define _ACPI_PU     0x10	/* punctuation */
-#define _ACPI_SP     0x08	/* space */
-#define _ACPI_UP     0x01	/* 'A'-'Z' */
-#define _ACPI_XD     0x80	/* '0'-'9', 'A'-'F', 'a'-'f' */
-
-#define ACPI_IS_DIGIT(c)  (_acpi_ctype[(unsigned char)(c)] & (_ACPI_DI))
-#define ACPI_IS_SPACE(c)  (_acpi_ctype[(unsigned char)(c)] & (_ACPI_SP))
-#define ACPI_IS_XDIGIT(c) (_acpi_ctype[(unsigned char)(c)] & (_ACPI_XD))
-#define ACPI_IS_UPPER(c)  (_acpi_ctype[(unsigned char)(c)] & (_ACPI_UP))
-#define ACPI_IS_LOWER(c)  (_acpi_ctype[(unsigned char)(c)] & (_ACPI_LO))
-#define ACPI_IS_PRINT(c)  (_acpi_ctype[(unsigned char)(c)] & (_ACPI_LO | _ACPI_UP | _ACPI_DI | _ACPI_XS | _ACPI_PU))
-#define ACPI_IS_ALPHA(c)  (_acpi_ctype[(unsigned char)(c)] & (_ACPI_LO | _ACPI_UP))
-
-#endif				/* !ACPI_USE_SYSTEM_CLIBRARY */
-
 #define ACPI_IS_ASCII(c)  ((c) < 0x80)
 
 /*
@@ -430,6 +370,10 @@ acpi_status
 acpi_ut_execute_CID(struct acpi_namespace_node *device_node,
 		    struct acpi_pnp_device_id_list ** return_cid_list);
 
+acpi_status
+acpi_ut_execute_CLS(struct acpi_namespace_node *device_node,
+		    struct acpi_pnp_device_id **return_id);
+
 /*
  * utlock - reader/writer locks
  */
diff --git a/drivers/acpi/acpica/dsfield.c b/drivers/acpi/acpica/dsfield.c
index 43b40de90484..20de148594fd 100644
--- a/drivers/acpi/acpica/dsfield.c
+++ b/drivers/acpi/acpica/dsfield.c
@@ -502,7 +502,7 @@ acpi_ds_create_field(union acpi_parse_object *op,
 		}
 	}
 
-	ACPI_MEMSET(&info, 0, sizeof(struct acpi_create_field_info));
+	memset(&info, 0, sizeof(struct acpi_create_field_info));
 
 	/* Second arg is the field flags */
 
diff --git a/drivers/acpi/acpica/dsinit.c b/drivers/acpi/acpica/dsinit.c
index bbe74bcebbae..95779e8ec3bb 100644
--- a/drivers/acpi/acpica/dsinit.c
+++ b/drivers/acpi/acpica/dsinit.c
@@ -207,7 +207,7 @@ acpi_ds_initialize_objects(u32 table_index,
 
 	/* Set all init info to zero */
 
-	ACPI_MEMSET(&info, 0, sizeof(struct acpi_init_walk_info));
+	memset(&info, 0, sizeof(struct acpi_init_walk_info));
 
 	info.owner_id = owner_id;
 	info.table_index = table_index;
diff --git a/drivers/acpi/acpica/dsmethod.c b/drivers/acpi/acpica/dsmethod.c
index d72565a3c646..85bb951430d9 100644
--- a/drivers/acpi/acpica/dsmethod.c
+++ b/drivers/acpi/acpica/dsmethod.c
@@ -116,6 +116,7 @@ acpi_ds_auto_serialize_method(struct acpi_namespace_node *node,
 	walk_state =
 	    acpi_ds_create_walk_state(node->owner_id, NULL, NULL, NULL);
 	if (!walk_state) {
+		acpi_ps_free_op(op);
 		return_ACPI_STATUS(AE_NO_MEMORY);
 	}
 
@@ -125,6 +126,7 @@ acpi_ds_auto_serialize_method(struct acpi_namespace_node *node,
 				  obj_desc->method.aml_length, NULL, 0);
 	if (ACPI_FAILURE(status)) {
 		acpi_ds_delete_walk_state(walk_state);
+		acpi_ps_free_op(op);
 		return_ACPI_STATUS(status);
 	}
 
@@ -133,9 +135,6 @@ acpi_ds_auto_serialize_method(struct acpi_namespace_node *node,
 	/* Parse the method, scan for creation of named objects */
 
 	status = acpi_ps_parse_aml(walk_state);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
 
 	acpi_ps_delete_parse_tree(op);
 	return_ACPI_STATUS(status);
diff --git a/drivers/acpi/acpica/dsobject.c b/drivers/acpi/acpica/dsobject.c
index 8a7b07b6adc8..2beb7fd674ae 100644
--- a/drivers/acpi/acpica/dsobject.c
+++ b/drivers/acpi/acpica/dsobject.c
@@ -339,8 +339,8 @@ acpi_ds_build_internal_buffer_obj(struct acpi_walk_state *walk_state,
 		/* Initialize buffer from the byte_list (if present) */
 
 		if (byte_list) {
-			ACPI_MEMCPY(obj_desc->buffer.pointer,
-				    byte_list->named.data, byte_list_length);
+			memcpy(obj_desc->buffer.pointer, byte_list->named.data,
+			       byte_list_length);
 		}
 	}
 
@@ -750,8 +750,7 @@ acpi_ds_init_object_from_op(struct acpi_walk_state *walk_state,
 	case ACPI_TYPE_STRING:
 
 		obj_desc->string.pointer = op->common.value.string;
-		obj_desc->string.length =
-		    (u32) ACPI_STRLEN(op->common.value.string);
+		obj_desc->string.length = (u32)strlen(op->common.value.string);
 
 		/*
 		 * The string is contained in the ACPI table, don't ever try
diff --git a/drivers/acpi/acpica/dsutils.c b/drivers/acpi/acpica/dsutils.c
index deeddd6d2f05..ebc577baeaf9 100644
--- a/drivers/acpi/acpica/dsutils.c
+++ b/drivers/acpi/acpica/dsutils.c
@@ -572,8 +572,8 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 					obj_desc =
 					    acpi_ut_create_string_object((acpi_size) name_length);
 
-					ACPI_STRNCPY(obj_desc->string.pointer,
-						     name_string, name_length);
+					strncpy(obj_desc->string.pointer,
+						name_string, name_length);
 					status = AE_OK;
 				} else {
 					/*
diff --git a/drivers/acpi/acpica/dswload.c b/drivers/acpi/acpica/dswload.c
index 843942fb4be5..845ff44919c3 100644
--- a/drivers/acpi/acpica/dswload.c
+++ b/drivers/acpi/acpica/dswload.c
@@ -315,10 +315,19 @@ acpi_ds_load1_begin_op(struct acpi_walk_state * walk_state,
 		flags = ACPI_NS_NO_UPSEARCH;
 		if ((walk_state->opcode != AML_SCOPE_OP) &&
 		    (!(walk_state->parse_flags & ACPI_PARSE_DEFERRED_OP))) {
-			flags |= ACPI_NS_ERROR_IF_FOUND;
-			ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
-					  "[%s] Cannot already exist\n",
-					  acpi_ut_get_type_name(object_type)));
+			if (walk_state->namespace_override) {
+				flags |= ACPI_NS_OVERRIDE_IF_FOUND;
+				ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
+						  "[%s] Override allowed\n",
+						  acpi_ut_get_type_name
+						  (object_type)));
+			} else {
+				flags |= ACPI_NS_ERROR_IF_FOUND;
+				ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
+						  "[%s] Cannot already exist\n",
+						  acpi_ut_get_type_name
+						  (object_type)));
+			}
 		} else {
 			ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
 					  "[%s] Both Find or Create allowed\n",
diff --git a/drivers/acpi/acpica/evgpeinit.c b/drivers/acpi/acpica/evgpeinit.c
index 8840296d5b20..ea4c0d3fca2d 100644
--- a/drivers/acpi/acpica/evgpeinit.c
+++ b/drivers/acpi/acpica/evgpeinit.c
@@ -377,7 +377,7 @@ acpi_ev_match_gpe_method(acpi_handle obj_handle,
 
 	/* 4) The last two characters of the name are the hex GPE Number */
 
-	gpe_number = ACPI_STRTOUL(&name[2], NULL, 16);
+	gpe_number = strtoul(&name[2], NULL, 16);
 	if (gpe_number == ACPI_UINT32_MAX) {
 
 		/* Conversion failed; invalid method, just ignore it */
diff --git a/drivers/acpi/acpica/exconfig.c b/drivers/acpi/acpica/exconfig.c
index 6e0df2b9d5a4..24a4c5c2b124 100644
--- a/drivers/acpi/acpica/exconfig.c
+++ b/drivers/acpi/acpica/exconfig.c
@@ -470,7 +470,7 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 			return_ACPI_STATUS(AE_NO_MEMORY);
 		}
 
-		ACPI_MEMCPY(table, table_header, length);
+		memcpy(table, table_header, length);
 		break;
 
 	default:
diff --git a/drivers/acpi/acpica/exconvrt.c b/drivers/acpi/acpica/exconvrt.c
index 89a976b4ccf2..075d654c837f 100644
--- a/drivers/acpi/acpica/exconvrt.c
+++ b/drivers/acpi/acpica/exconvrt.c
@@ -227,9 +227,8 @@ acpi_ex_convert_to_buffer(union acpi_operand_object *obj_desc,
 		/* Copy the integer to the buffer, LSB first */
 
 		new_buf = return_desc->buffer.pointer;
-		ACPI_MEMCPY(new_buf,
-			    &obj_desc->integer.value,
-			    acpi_gbl_integer_byte_width);
+		memcpy(new_buf,
+		       &obj_desc->integer.value, acpi_gbl_integer_byte_width);
 		break;
 
 	case ACPI_TYPE_STRING:
@@ -252,8 +251,8 @@ acpi_ex_convert_to_buffer(union acpi_operand_object *obj_desc,
 		/* Copy the string to the buffer */
 
 		new_buf = return_desc->buffer.pointer;
-		ACPI_STRNCPY((char *)new_buf, (char *)obj_desc->string.pointer,
-			     obj_desc->string.length);
+		strncpy((char *)new_buf, (char *)obj_desc->string.pointer,
+			obj_desc->string.length);
 		break;
 
 	default:
diff --git a/drivers/acpi/acpica/exdebug.c b/drivers/acpi/acpica/exdebug.c
index e67d0aca3fe6..815442bbd051 100644
--- a/drivers/acpi/acpica/exdebug.c
+++ b/drivers/acpi/acpica/exdebug.c
@@ -76,6 +76,8 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 {
 	u32 i;
 	u32 timer;
+	union acpi_operand_object *object_desc;
+	u32 value;
 
 	ACPI_FUNCTION_TRACE_PTR(ex_do_debug_object, source_desc);
 
@@ -254,8 +256,44 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 							 object)->object,
 							level + 4, 0);
 			} else {
-				acpi_ex_do_debug_object(source_desc->reference.
-							object, level + 4, 0);
+				object_desc = source_desc->reference.object;
+				value = source_desc->reference.value;
+
+				switch (object_desc->common.type) {
+				case ACPI_TYPE_BUFFER:
+
+					acpi_os_printf("Buffer[%u] = 0x%2.2X\n",
+						       value,
+						       *source_desc->reference.
+						       index_pointer);
+					break;
+
+				case ACPI_TYPE_STRING:
+
+					acpi_os_printf
+					    ("String[%u] = \"%c\" (0x%2.2X)\n",
+					     value,
+					     *source_desc->reference.
+					     index_pointer,
+					     *source_desc->reference.
+					     index_pointer);
+					break;
+
+				case ACPI_TYPE_PACKAGE:
+
+					acpi_os_printf("Package[%u] = ", value);
+					acpi_ex_do_debug_object(*source_desc->
+								reference.where,
+								level + 4, 0);
+					break;
+
+				default:
+
+					acpi_os_printf
+					    ("Unknown Reference object type %X\n",
+					     object_desc->common.type);
+					break;
+				}
 			}
 		}
 		break;
diff --git a/drivers/acpi/acpica/exdump.c b/drivers/acpi/acpica/exdump.c
index 1da52bef632e..401e7edcd419 100644
--- a/drivers/acpi/acpica/exdump.c
+++ b/drivers/acpi/acpica/exdump.c
@@ -224,7 +224,7 @@ static struct acpi_exdump_info acpi_ex_dump_index_field[5] = {
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(index_field.data_obj), "Data Object"}
 };
 
-static struct acpi_exdump_info acpi_ex_dump_reference[8] = {
+static struct acpi_exdump_info acpi_ex_dump_reference[9] = {
 	{ACPI_EXD_INIT, ACPI_EXD_TABLE_SIZE(acpi_ex_dump_reference), NULL},
 	{ACPI_EXD_UINT8, ACPI_EXD_OFFSET(reference.class), "Class"},
 	{ACPI_EXD_UINT8, ACPI_EXD_OFFSET(reference.target_type), "Target Type"},
@@ -232,6 +232,8 @@ static struct acpi_exdump_info acpi_ex_dump_reference[8] = {
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(reference.object), "Object Desc"},
 	{ACPI_EXD_NODE, ACPI_EXD_OFFSET(reference.node), "Node"},
 	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(reference.where), "Where"},
+	{ACPI_EXD_POINTER, ACPI_EXD_OFFSET(reference.index_pointer),
+	 "Index Pointer"},
 	{ACPI_EXD_REFERENCE, 0, NULL}
 };
 
@@ -1005,14 +1007,13 @@ static void acpi_ex_dump_reference_obj(union acpi_operand_object *obj_desc)
 	} else if (obj_desc->reference.object) {
 		if (ACPI_GET_DESCRIPTOR_TYPE(obj_desc) ==
 		    ACPI_DESC_TYPE_OPERAND) {
-			acpi_os_printf(" Target: %p",
+			acpi_os_printf("%22s %p", "Target :",
 				       obj_desc->reference.object);
 			if (obj_desc->reference.class == ACPI_REFCLASS_TABLE) {
 				acpi_os_printf(" Table Index: %X\n",
 					       obj_desc->reference.value);
 			} else {
-				acpi_os_printf(" Target: %p [%s]\n",
-					       obj_desc->reference.object,
+				acpi_os_printf(" [%s]\n",
 					       acpi_ut_get_type_name(((union
 								       acpi_operand_object
 								       *)
diff --git a/drivers/acpi/acpica/exfield.c b/drivers/acpi/acpica/exfield.c
index c161dd974f74..61fd9c7b88bc 100644
--- a/drivers/acpi/acpica/exfield.c
+++ b/drivers/acpi/acpica/exfield.c
@@ -428,7 +428,7 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 		}
 
 		buffer = buffer_desc->buffer.pointer;
-		ACPI_MEMCPY(buffer, source_desc->buffer.pointer, length);
+		memcpy(buffer, source_desc->buffer.pointer, length);
 
 		/* Lock entire transaction if requested */
 
diff --git a/drivers/acpi/acpica/exfldio.c b/drivers/acpi/acpica/exfldio.c
index 725a3746a2df..70b7bbbb860b 100644
--- a/drivers/acpi/acpica/exfldio.c
+++ b/drivers/acpi/acpica/exfldio.c
@@ -416,22 +416,22 @@ acpi_ex_field_datum_io(union acpi_operand_object *obj_desc,
 			 * Copy the data from the source buffer.
 			 * Length is the field width in bytes.
 			 */
-			ACPI_MEMCPY(value,
-				    (obj_desc->buffer_field.buffer_obj)->buffer.
-				    pointer +
-				    obj_desc->buffer_field.base_byte_offset +
-				    field_datum_byte_offset,
-				    obj_desc->common_field.access_byte_width);
+			memcpy(value,
+			       (obj_desc->buffer_field.buffer_obj)->buffer.
+			       pointer +
+			       obj_desc->buffer_field.base_byte_offset +
+			       field_datum_byte_offset,
+			       obj_desc->common_field.access_byte_width);
 		} else {
 			/*
 			 * Copy the data to the target buffer.
 			 * Length is the field width in bytes.
 			 */
-			ACPI_MEMCPY((obj_desc->buffer_field.buffer_obj)->buffer.
-				    pointer +
-				    obj_desc->buffer_field.base_byte_offset +
-				    field_datum_byte_offset, value,
-				    obj_desc->common_field.access_byte_width);
+			memcpy((obj_desc->buffer_field.buffer_obj)->buffer.
+			       pointer +
+			       obj_desc->buffer_field.base_byte_offset +
+			       field_datum_byte_offset, value,
+			       obj_desc->common_field.access_byte_width);
 		}
 
 		status = AE_OK;
@@ -703,7 +703,7 @@ acpi_ex_extract_from_field(union acpi_operand_object *obj_desc,
 		return_ACPI_STATUS(AE_BUFFER_OVERFLOW);
 	}
 
-	ACPI_MEMSET(buffer, 0, buffer_length);
+	memset(buffer, 0, buffer_length);
 	access_bit_width = ACPI_MUL_8(obj_desc->common_field.access_byte_width);
 
 	/* Handle the simple case here */
@@ -720,7 +720,7 @@ acpi_ex_extract_from_field(union acpi_operand_object *obj_desc,
 			status =
 			    acpi_ex_field_datum_io(obj_desc, 0, &raw_datum,
 						   ACPI_READ);
-			ACPI_MEMCPY(buffer, &raw_datum, buffer_length);
+			memcpy(buffer, &raw_datum, buffer_length);
 		}
 
 		return_ACPI_STATUS(status);
@@ -793,9 +793,9 @@ acpi_ex_extract_from_field(union acpi_operand_object *obj_desc,
 
 		/* Write merged datum to target buffer */
 
-		ACPI_MEMCPY(((char *)buffer) + buffer_offset, &merged_datum,
-			    ACPI_MIN(obj_desc->common_field.access_byte_width,
-				     buffer_length - buffer_offset));
+		memcpy(((char *)buffer) + buffer_offset, &merged_datum,
+		       ACPI_MIN(obj_desc->common_field.access_byte_width,
+				buffer_length - buffer_offset));
 
 		buffer_offset += obj_desc->common_field.access_byte_width;
 		merged_datum =
@@ -811,9 +811,9 @@ acpi_ex_extract_from_field(union acpi_operand_object *obj_desc,
 
 	/* Write the last datum to the buffer */
 
-	ACPI_MEMCPY(((char *)buffer) + buffer_offset, &merged_datum,
-		    ACPI_MIN(obj_desc->common_field.access_byte_width,
-			     buffer_length - buffer_offset));
+	memcpy(((char *)buffer) + buffer_offset, &merged_datum,
+	       ACPI_MIN(obj_desc->common_field.access_byte_width,
+			buffer_length - buffer_offset));
 
 	return_ACPI_STATUS(AE_OK);
 }
@@ -878,7 +878,7 @@ acpi_ex_insert_into_field(union acpi_operand_object *obj_desc,
 		 * at Byte zero. All unused (upper) bytes of the
 		 * buffer will be 0.
 		 */
-		ACPI_MEMCPY((char *)new_buffer, (char *)buffer, buffer_length);
+		memcpy((char *)new_buffer, (char *)buffer, buffer_length);
 		buffer = new_buffer;
 		buffer_length = required_length;
 	}
@@ -918,9 +918,9 @@ acpi_ex_insert_into_field(union acpi_operand_object *obj_desc,
 
 	/* Get initial Datum from the input buffer */
 
-	ACPI_MEMCPY(&raw_datum, buffer,
-		    ACPI_MIN(obj_desc->common_field.access_byte_width,
-			     buffer_length - buffer_offset));
+	memcpy(&raw_datum, buffer,
+	       ACPI_MIN(obj_desc->common_field.access_byte_width,
+			buffer_length - buffer_offset));
 
 	merged_datum =
 	    raw_datum << obj_desc->common_field.start_field_bit_offset;
@@ -970,9 +970,9 @@ acpi_ex_insert_into_field(union acpi_operand_object *obj_desc,
 		/* Get the next input datum from the buffer */
 
 		buffer_offset += obj_desc->common_field.access_byte_width;
-		ACPI_MEMCPY(&raw_datum, ((char *)buffer) + buffer_offset,
-			    ACPI_MIN(obj_desc->common_field.access_byte_width,
-				     buffer_length - buffer_offset));
+		memcpy(&raw_datum, ((char *)buffer) + buffer_offset,
+		       ACPI_MIN(obj_desc->common_field.access_byte_width,
+				buffer_length - buffer_offset));
 
 		merged_datum |=
 		    raw_datum << obj_desc->common_field.start_field_bit_offset;
diff --git a/drivers/acpi/acpica/exmisc.c b/drivers/acpi/acpica/exmisc.c
index b56fc9d6f48e..d02afece0f10 100644
--- a/drivers/acpi/acpica/exmisc.c
+++ b/drivers/acpi/acpica/exmisc.c
@@ -209,8 +209,8 @@ acpi_ex_concat_template(union acpi_operand_object *operand0,
 	 * end_tag descriptor is copied from Operand1.
 	 */
 	new_buf = return_desc->buffer.pointer;
-	ACPI_MEMCPY(new_buf, operand0->buffer.pointer, length0);
-	ACPI_MEMCPY(new_buf + length0, operand1->buffer.pointer, length1);
+	memcpy(new_buf, operand0->buffer.pointer, length0);
+	memcpy(new_buf + length0, operand1->buffer.pointer, length1);
 
 	/* Insert end_tag and set the checksum to zero, means "ignore checksum" */
 
@@ -318,14 +318,14 @@ acpi_ex_do_concatenate(union acpi_operand_object *operand0,
 
 		/* Copy the first integer, LSB first */
 
-		ACPI_MEMCPY(new_buf, &operand0->integer.value,
-			    acpi_gbl_integer_byte_width);
+		memcpy(new_buf, &operand0->integer.value,
+		       acpi_gbl_integer_byte_width);
 
 		/* Copy the second integer (LSB first) after the first */
 
-		ACPI_MEMCPY(new_buf + acpi_gbl_integer_byte_width,
-			    &local_operand1->integer.value,
-			    acpi_gbl_integer_byte_width);
+		memcpy(new_buf + acpi_gbl_integer_byte_width,
+		       &local_operand1->integer.value,
+		       acpi_gbl_integer_byte_width);
 		break;
 
 	case ACPI_TYPE_STRING:
@@ -346,9 +346,9 @@ acpi_ex_do_concatenate(union acpi_operand_object *operand0,
 
 		/* Concatenate the strings */
 
-		ACPI_STRCPY(new_buf, operand0->string.pointer);
-		ACPI_STRCPY(new_buf + operand0->string.length,
-			    local_operand1->string.pointer);
+		strcpy(new_buf, operand0->string.pointer);
+		strcpy(new_buf + operand0->string.length,
+		       local_operand1->string.pointer);
 		break;
 
 	case ACPI_TYPE_BUFFER:
@@ -369,11 +369,11 @@ acpi_ex_do_concatenate(union acpi_operand_object *operand0,
 
 		/* Concatenate the buffers */
 
-		ACPI_MEMCPY(new_buf, operand0->buffer.pointer,
-			    operand0->buffer.length);
-		ACPI_MEMCPY(new_buf + operand0->buffer.length,
-			    local_operand1->buffer.pointer,
-			    local_operand1->buffer.length);
+		memcpy(new_buf, operand0->buffer.pointer,
+		       operand0->buffer.length);
+		memcpy(new_buf + operand0->buffer.length,
+		       local_operand1->buffer.pointer,
+		       local_operand1->buffer.length);
 		break;
 
 	default:
@@ -660,9 +660,9 @@ acpi_ex_do_logical_op(u16 opcode,
 
 		/* Lexicographic compare: compare the data bytes */
 
-		compare = ACPI_MEMCMP(operand0->buffer.pointer,
-				      local_operand1->buffer.pointer,
-				      (length0 > length1) ? length1 : length0);
+		compare = memcmp(operand0->buffer.pointer,
+				 local_operand1->buffer.pointer,
+				 (length0 > length1) ? length1 : length0);
 
 		switch (opcode) {
 		case AML_LEQUAL_OP:	/* LEqual (Operand0, Operand1) */
diff --git a/drivers/acpi/acpica/exnames.c b/drivers/acpi/acpica/exnames.c
index 453b00c30177..20e87813c7d7 100644
--- a/drivers/acpi/acpica/exnames.c
+++ b/drivers/acpi/acpica/exnames.c
@@ -192,7 +192,7 @@ static acpi_status acpi_ex_name_segment(u8 ** in_aml_address, char *name_string)
 		char_buf[4] = '\0';
 
 		if (name_string) {
-			ACPI_STRCAT(name_string, char_buf);
+			strcat(name_string, char_buf);
 			ACPI_DEBUG_PRINT((ACPI_DB_NAMES,
 					  "Appended to - %s\n", name_string));
 		} else {
diff --git a/drivers/acpi/acpica/exoparg2.c b/drivers/acpi/acpica/exoparg2.c
index fcc618aa2061..b8944ebb1081 100644
--- a/drivers/acpi/acpica/exoparg2.c
+++ b/drivers/acpi/acpica/exoparg2.c
@@ -337,8 +337,8 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 		 * Copy the raw buffer data with no transform.
 		 * (NULL terminated already)
 		 */
-		ACPI_MEMCPY(return_desc->string.pointer,
-			    operand[0]->buffer.pointer, length);
+		memcpy(return_desc->string.pointer,
+		       operand[0]->buffer.pointer, length);
 		break;
 
 	case AML_CONCAT_RES_OP:
@@ -380,6 +380,8 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 
 			return_desc->reference.target_type =
 			    ACPI_TYPE_BUFFER_FIELD;
+			return_desc->reference.index_pointer =
+			    &(operand[0]->buffer.pointer[index]);
 			break;
 
 		case ACPI_TYPE_BUFFER:
@@ -391,6 +393,8 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 
 			return_desc->reference.target_type =
 			    ACPI_TYPE_BUFFER_FIELD;
+			return_desc->reference.index_pointer =
+			    &(operand[0]->buffer.pointer[index]);
 			break;
 
 		case ACPI_TYPE_PACKAGE:
diff --git a/drivers/acpi/acpica/exoparg3.c b/drivers/acpi/acpica/exoparg3.c
index 1c64a988cbee..fa100b3b92ee 100644
--- a/drivers/acpi/acpica/exoparg3.c
+++ b/drivers/acpi/acpica/exoparg3.c
@@ -237,8 +237,8 @@ acpi_status acpi_ex_opcode_3A_1T_1R(struct acpi_walk_state *walk_state)
 
 			/* We have a buffer, copy the portion requested */
 
-			ACPI_MEMCPY(buffer, operand[0]->string.pointer + index,
-				    length);
+			memcpy(buffer, operand[0]->string.pointer + index,
+			       length);
 		}
 
 		/* Set the length of the new String/Buffer */
diff --git a/drivers/acpi/acpica/exregion.c b/drivers/acpi/acpica/exregion.c
index f6c2f5499935..b4a5e44c00dd 100644
--- a/drivers/acpi/acpica/exregion.c
+++ b/drivers/acpi/acpica/exregion.c
@@ -517,15 +517,14 @@ acpi_ex_data_table_space_handler(u32 function,
 	switch (function) {
 	case ACPI_READ:
 
-		ACPI_MEMCPY(ACPI_CAST_PTR(char, value),
-			    ACPI_PHYSADDR_TO_PTR(address),
-			    ACPI_DIV_8(bit_width));
+		memcpy(ACPI_CAST_PTR(char, value),
+		       ACPI_PHYSADDR_TO_PTR(address), ACPI_DIV_8(bit_width));
 		break;
 
 	case ACPI_WRITE:
 
-		ACPI_MEMCPY(ACPI_PHYSADDR_TO_PTR(address),
-			    ACPI_CAST_PTR(char, value), ACPI_DIV_8(bit_width));
+		memcpy(ACPI_PHYSADDR_TO_PTR(address),
+		       ACPI_CAST_PTR(char, value), ACPI_DIV_8(bit_width));
 		break;
 
 	default:
diff --git a/drivers/acpi/acpica/exstorob.c b/drivers/acpi/acpica/exstorob.c
index 6fa3c8d8fc5f..e1d4f4d51b97 100644
--- a/drivers/acpi/acpica/exstorob.c
+++ b/drivers/acpi/acpica/exstorob.c
@@ -100,9 +100,9 @@ acpi_ex_store_buffer_to_buffer(union acpi_operand_object *source_desc,
 
 		/* Clear existing buffer and copy in the new one */
 
-		ACPI_MEMSET(target_desc->buffer.pointer, 0,
-			    target_desc->buffer.length);
-		ACPI_MEMCPY(target_desc->buffer.pointer, buffer, length);
+		memset(target_desc->buffer.pointer, 0,
+		       target_desc->buffer.length);
+		memcpy(target_desc->buffer.pointer, buffer, length);
 
 #ifdef ACPI_OBSOLETE_BEHAVIOR
 		/*
@@ -129,8 +129,8 @@ acpi_ex_store_buffer_to_buffer(union acpi_operand_object *source_desc,
 	} else {
 		/* Truncate the source, copy only what will fit */
 
-		ACPI_MEMCPY(target_desc->buffer.pointer, buffer,
-			    target_desc->buffer.length);
+		memcpy(target_desc->buffer.pointer, buffer,
+		       target_desc->buffer.length);
 
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
 				  "Truncating source buffer from %X to %X\n",
@@ -187,9 +187,9 @@ acpi_ex_store_string_to_string(union acpi_operand_object *source_desc,
 		 * String will fit in existing non-static buffer.
 		 * Clear old string and copy in the new one
 		 */
-		ACPI_MEMSET(target_desc->string.pointer, 0,
-			    (acpi_size) target_desc->string.length + 1);
-		ACPI_MEMCPY(target_desc->string.pointer, buffer, length);
+		memset(target_desc->string.pointer, 0,
+		       (acpi_size) target_desc->string.length + 1);
+		memcpy(target_desc->string.pointer, buffer, length);
 	} else {
 		/*
 		 * Free the current buffer, then allocate a new buffer
@@ -210,7 +210,7 @@ acpi_ex_store_string_to_string(union acpi_operand_object *source_desc,
 		}
 
 		target_desc->common.flags &= ~AOPOBJ_STATIC_POINTER;
-		ACPI_MEMCPY(target_desc->string.pointer, buffer, length);
+		memcpy(target_desc->string.pointer, buffer, length);
 	}
 
 	/* Set the new target length */
diff --git a/drivers/acpi/acpica/exutils.c b/drivers/acpi/acpica/exutils.c
index 3f4225e95d93..30c3f464fda5 100644
--- a/drivers/acpi/acpica/exutils.c
+++ b/drivers/acpi/acpica/exutils.c
@@ -380,6 +380,38 @@ void acpi_ex_integer_to_string(char *out_string, u64 value)
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_ex_pci_cls_to_string
+ *
+ * PARAMETERS:  out_string      - Where to put the converted string (7 bytes)
+ * PARAMETERS:  class_code      - PCI class code to be converted (3 bytes)
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Convert 3-bytes PCI class code to string representation.
+ *              Return buffer must be large enough to hold the string. The
+ *              string returned is always exactly of length
+ *              ACPI_PCICLS_STRING_SIZE (includes null terminator).
+ *
+ ******************************************************************************/
+
+void acpi_ex_pci_cls_to_string(char *out_string, u8 class_code[3])
+{
+
+	ACPI_FUNCTION_ENTRY();
+
+	/* All 3 bytes are hexadecimal */
+
+	out_string[0] = acpi_ut_hex_to_ascii_char((u64)class_code[0], 4);
+	out_string[1] = acpi_ut_hex_to_ascii_char((u64)class_code[0], 0);
+	out_string[2] = acpi_ut_hex_to_ascii_char((u64)class_code[1], 4);
+	out_string[3] = acpi_ut_hex_to_ascii_char((u64)class_code[1], 0);
+	out_string[4] = acpi_ut_hex_to_ascii_char((u64)class_code[2], 4);
+	out_string[5] = acpi_ut_hex_to_ascii_char((u64)class_code[2], 0);
+	out_string[6] = 0;
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_is_valid_space_id
  *
  * PARAMETERS:  space_id            - ID to be validated
diff --git a/drivers/acpi/acpica/hwpci.c b/drivers/acpi/acpica/hwpci.c
index c5214dec4988..f785ea788356 100644
--- a/drivers/acpi/acpica/hwpci.c
+++ b/drivers/acpi/acpica/hwpci.c
@@ -123,7 +123,7 @@ acpi_hw_derive_pci_id(struct acpi_pci_id *pci_id,
 		      acpi_handle root_pci_device, acpi_handle pci_region)
 {
 	acpi_status status;
-	struct acpi_pci_device *list_head = NULL;
+	struct acpi_pci_device *list_head;
 
 	ACPI_FUNCTION_TRACE(hw_derive_pci_id);
 
@@ -177,13 +177,13 @@ acpi_hw_build_pci_list(acpi_handle root_pci_device,
 	acpi_handle parent_device;
 	acpi_status status;
 	struct acpi_pci_device *list_element;
-	struct acpi_pci_device *list_head = NULL;
 
 	/*
 	 * Ascend namespace branch until the root_pci_device is reached, building
 	 * a list of device nodes. Loop will exit when either the PCI device is
 	 * found, or the root of the namespace is reached.
 	 */
+	*return_list_head = NULL;
 	current_device = pci_region;
 	while (1) {
 		status = acpi_get_parent(current_device, &parent_device);
@@ -198,7 +198,6 @@ acpi_hw_build_pci_list(acpi_handle root_pci_device,
 		/* Finished when we reach the PCI root device (PNP0A03 or PNP0A08) */
 
 		if (parent_device == root_pci_device) {
-			*return_list_head = list_head;
 			return (AE_OK);
 		}
 
@@ -213,9 +212,9 @@ acpi_hw_build_pci_list(acpi_handle root_pci_device,
 
 		/* Put new element at the head of the list */
 
-		list_element->next = list_head;
+		list_element->next = *return_list_head;
 		list_element->device = parent_device;
-		list_head = list_element;
+		*return_list_head = list_element;
 
 		current_device = parent_device;
 	}
diff --git a/drivers/acpi/acpica/hwxfsleep.c b/drivers/acpi/acpica/hwxfsleep.c
index 3b3767698827..52dfd0d050fa 100644
--- a/drivers/acpi/acpica/hwxfsleep.c
+++ b/drivers/acpi/acpica/hwxfsleep.c
@@ -50,6 +50,13 @@
 ACPI_MODULE_NAME("hwxfsleep")
 
 /* Local prototypes */
+#if (!ACPI_REDUCED_HARDWARE)
+static acpi_status
+acpi_hw_set_firmware_waking_vectors(struct acpi_table_facs *facs,
+				    acpi_physical_address physical_address,
+				    acpi_physical_address physical_address64);
+#endif
+
 static acpi_status acpi_hw_sleep_dispatch(u8 sleep_state, u32 function_id);
 
 /*
@@ -72,6 +79,7 @@ static struct acpi_sleep_functions acpi_sleep_dispatch[] = {
 
 /*
  * These functions are removed for the ACPI_REDUCED_HARDWARE case:
+ *      acpi_set_firmware_waking_vectors
  *      acpi_set_firmware_waking_vector
  *      acpi_set_firmware_waking_vector64
  *      acpi_enter_sleep_state_s4bios
@@ -80,20 +88,26 @@ static struct acpi_sleep_functions acpi_sleep_dispatch[] = {
 #if (!ACPI_REDUCED_HARDWARE)
 /*******************************************************************************
  *
- * FUNCTION:    acpi_set_firmware_waking_vector
+ * FUNCTION:    acpi_hw_set_firmware_waking_vectors
  *
- * PARAMETERS:  physical_address    - 32-bit physical address of ACPI real mode
+ * PARAMETERS:  facs                - Pointer to FACS table
+ *              physical_address    - 32-bit physical address of ACPI real mode
  *                                    entry point.
+ *              physical_address64  - 64-bit physical address of ACPI protected
+ *                                    mode entry point.
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Sets the 32-bit firmware_waking_vector field of the FACS
+ * DESCRIPTION: Sets the firmware_waking_vector fields of the FACS
  *
  ******************************************************************************/
 
-acpi_status acpi_set_firmware_waking_vector(u32 physical_address)
+static acpi_status
+acpi_hw_set_firmware_waking_vectors(struct acpi_table_facs *facs,
+				    acpi_physical_address physical_address,
+				    acpi_physical_address physical_address64)
 {
-	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vector);
+	ACPI_FUNCTION_TRACE(acpi_hw_set_firmware_waking_vectors);
 
 
 	/*
@@ -106,17 +120,92 @@ acpi_status acpi_set_firmware_waking_vector(u32 physical_address)
 
 	/* Set the 32-bit vector */
 
-	acpi_gbl_FACS->firmware_waking_vector = physical_address;
+	facs->firmware_waking_vector = (u32)physical_address;
 
-	/* Clear the 64-bit vector if it exists */
+	if (facs->length > 32) {
+		if (facs->version >= 1) {
 
-	if ((acpi_gbl_FACS->length > 32) && (acpi_gbl_FACS->version >= 1)) {
-		acpi_gbl_FACS->xfirmware_waking_vector = 0;
+			/* Set the 64-bit vector */
+
+			facs->xfirmware_waking_vector = physical_address64;
+		} else {
+			/* Clear the 64-bit vector if it exists */
+
+			facs->xfirmware_waking_vector = 0;
+		}
 	}
 
 	return_ACPI_STATUS(AE_OK);
 }
 
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_set_firmware_waking_vectors
+ *
+ * PARAMETERS:  physical_address    - 32-bit physical address of ACPI real mode
+ *                                    entry point.
+ *              physical_address64  - 64-bit physical address of ACPI protected
+ *                                    mode entry point.
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Sets the firmware_waking_vector fields of the FACS
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_set_firmware_waking_vectors(acpi_physical_address physical_address,
+				 acpi_physical_address physical_address64)
+{
+
+	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vectors);
+
+	/* If Hardware Reduced flag is set, there is no FACS */
+
+	if (acpi_gbl_reduced_hardware) {
+		return_ACPI_STATUS (AE_OK);
+	}
+
+	if (acpi_gbl_facs32) {
+		(void)acpi_hw_set_firmware_waking_vectors(acpi_gbl_facs32,
+							  physical_address,
+							  physical_address64);
+	}
+	if (acpi_gbl_facs64) {
+		(void)acpi_hw_set_firmware_waking_vectors(acpi_gbl_facs64,
+							  physical_address,
+							  physical_address64);
+	}
+
+	return_ACPI_STATUS(AE_OK);
+}
+
+ACPI_EXPORT_SYMBOL(acpi_set_firmware_waking_vectors)
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_set_firmware_waking_vector
+ *
+ * PARAMETERS:  physical_address    - 32-bit physical address of ACPI real mode
+ *                                    entry point.
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Sets the 32-bit firmware_waking_vector field of the FACS
+ *
+ ******************************************************************************/
+acpi_status acpi_set_firmware_waking_vector(u32 physical_address)
+{
+	acpi_status status;
+
+	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vector);
+
+	status = acpi_set_firmware_waking_vectors((acpi_physical_address)
+						  physical_address, 0);
+
+	return_ACPI_STATUS(status);
+}
+
 ACPI_EXPORT_SYMBOL(acpi_set_firmware_waking_vector)
 
 #if ACPI_MACHINE_WIDTH == 64
@@ -136,25 +225,19 @@ ACPI_EXPORT_SYMBOL(acpi_set_firmware_waking_vector)
  ******************************************************************************/
 acpi_status acpi_set_firmware_waking_vector64(u64 physical_address)
 {
-	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vector64);
-
-
-	/* Determine if the 64-bit vector actually exists */
+	acpi_status status;
 
-	if ((acpi_gbl_FACS->length <= 32) || (acpi_gbl_FACS->version < 1)) {
-		return_ACPI_STATUS(AE_NOT_EXIST);
-	}
+	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vector64);
 
-	/* Clear 32-bit vector, set the 64-bit X_ vector */
+	status = acpi_set_firmware_waking_vectors(0,
+						  (acpi_physical_address)
+						  physical_address);
 
-	acpi_gbl_FACS->firmware_waking_vector = 0;
-	acpi_gbl_FACS->xfirmware_waking_vector = physical_address;
-	return_ACPI_STATUS(AE_OK);
+	return_ACPI_STATUS(status);
 }
 
 ACPI_EXPORT_SYMBOL(acpi_set_firmware_waking_vector64)
 #endif
-
 /*******************************************************************************
  *
  * FUNCTION:    acpi_enter_sleep_state_s4bios
diff --git a/drivers/acpi/acpica/nsaccess.c b/drivers/acpi/acpica/nsaccess.c
index 24fa19a76d70..c687b9979fb2 100644
--- a/drivers/acpi/acpica/nsaccess.c
+++ b/drivers/acpi/acpica/nsaccess.c
@@ -102,7 +102,7 @@ acpi_status acpi_ns_root_initialize(void)
 
 		/* _OSI is optional for now, will be permanent later */
 
-		if (!ACPI_STRCMP(init_val->name, "_OSI")
+		if (!strcmp(init_val->name, "_OSI")
 		    && !acpi_gbl_create_osi_method) {
 			continue;
 		}
@@ -180,7 +180,7 @@ acpi_status acpi_ns_root_initialize(void)
 
 				/* Build an object around the static string */
 
-				obj_desc->string.length = (u32)ACPI_STRLEN(val);
+				obj_desc->string.length = (u32)strlen(val);
 				obj_desc->string.pointer = val;
 				obj_desc->common.flags |= AOPOBJ_STATIC_POINTER;
 				break;
@@ -203,7 +203,7 @@ acpi_status acpi_ns_root_initialize(void)
 
 				/* Special case for ACPI Global Lock */
 
-				if (ACPI_STRCMP(init_val->name, "_GL_") == 0) {
+				if (strcmp(init_val->name, "_GL_") == 0) {
 					acpi_gbl_global_lock_mutex = obj_desc;
 
 					/* Create additional counting semaphore for global lock */
@@ -304,7 +304,9 @@ acpi_ns_lookup(union acpi_generic_state *scope_info,
 		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
 
-	local_flags = flags & ~(ACPI_NS_ERROR_IF_FOUND | ACPI_NS_SEARCH_PARENT);
+	local_flags = flags &
+	    ~(ACPI_NS_ERROR_IF_FOUND | ACPI_NS_OVERRIDE_IF_FOUND |
+	      ACPI_NS_SEARCH_PARENT);
 	*return_node = ACPI_ENTRY_NOT_FOUND;
 	acpi_gbl_ns_lookup_count++;
 
@@ -547,6 +549,12 @@ acpi_ns_lookup(union acpi_generic_state *scope_info,
 			if (flags & ACPI_NS_ERROR_IF_FOUND) {
 				local_flags |= ACPI_NS_ERROR_IF_FOUND;
 			}
+
+			/* Set override flag according to caller */
+
+			if (flags & ACPI_NS_OVERRIDE_IF_FOUND) {
+				local_flags |= ACPI_NS_OVERRIDE_IF_FOUND;
+			}
 		}
 
 		/* Extract one ACPI name from the front of the pathname */
diff --git a/drivers/acpi/acpica/nsconvert.c b/drivers/acpi/acpica/nsconvert.c
index 1a8b39c8d969..da55a1c60da1 100644
--- a/drivers/acpi/acpica/nsconvert.c
+++ b/drivers/acpi/acpica/nsconvert.c
@@ -187,8 +187,8 @@ acpi_ns_convert_to_string(union acpi_operand_object *original_object,
 		 * Copy the raw buffer data with no transform. String is already NULL
 		 * terminated at Length+1.
 		 */
-		ACPI_MEMCPY(new_object->string.pointer,
-			    original_object->buffer.pointer, length);
+		memcpy(new_object->string.pointer,
+		       original_object->buffer.pointer, length);
 		break;
 
 	default:
@@ -251,9 +251,9 @@ acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
 			return (AE_NO_MEMORY);
 		}
 
-		ACPI_MEMCPY(new_object->buffer.pointer,
-			    original_object->string.pointer,
-			    original_object->string.length);
+		memcpy(new_object->buffer.pointer,
+		       original_object->string.pointer,
+		       original_object->string.length);
 		break;
 
 	case ACPI_TYPE_PACKAGE:
diff --git a/drivers/acpi/acpica/nsdump.c b/drivers/acpi/acpica/nsdump.c
index d259393505fa..0f1daba640e7 100644
--- a/drivers/acpi/acpica/nsdump.c
+++ b/drivers/acpi/acpica/nsdump.c
@@ -101,7 +101,7 @@ void acpi_ns_print_pathname(u32 num_segments, char *pathname)
 
 	while (num_segments) {
 		for (i = 0; i < 4; i++) {
-			ACPI_IS_PRINT(pathname[i]) ?
+			isprint((int)pathname[i]) ?
 			    acpi_os_printf("%c", pathname[i]) :
 			    acpi_os_printf("?");
 		}
diff --git a/drivers/acpi/acpica/nseval.c b/drivers/acpi/acpica/nseval.c
index 7bcc68f57afa..80670cb32b5a 100644
--- a/drivers/acpi/acpica/nseval.c
+++ b/drivers/acpi/acpica/nseval.c
@@ -59,15 +59,14 @@ acpi_ns_exec_module_code(union acpi_operand_object *method_obj,
  *
  * FUNCTION:    acpi_ns_evaluate
  *
- * PARAMETERS:  info            - Evaluation info block, contains:
+ * PARAMETERS:  info            - Evaluation info block, contains these fields
+ *                                and more:
  *                  prefix_node     - Prefix or Method/Object Node to execute
  *                  relative_path   - Name of method to execute, If NULL, the
  *                                    Node is the object to execute
  *                  parameters      - List of parameters to pass to the method,
  *                                    terminated by NULL. Params itself may be
  *                                    NULL if no parameters are being passed.
- *                  return_object   - Where to put method's return value (if
- *                                    any). If NULL, no value is returned.
  *                  parameter_type  - Type of Parameter list
  *                  return_object   - Where to put method's return value (if
  *                                    any). If NULL, no value is returned.
@@ -440,7 +439,7 @@ acpi_ns_exec_module_code(union acpi_operand_object *method_obj,
 
 	/* Initialize the evaluation information block */
 
-	ACPI_MEMSET(info, 0, sizeof(struct acpi_evaluate_info));
+	memset(info, 0, sizeof(struct acpi_evaluate_info));
 	info->prefix_node = parent_node;
 
 	/*
diff --git a/drivers/acpi/acpica/nsinit.c b/drivers/acpi/acpica/nsinit.c
index 4a85c4517988..b744a53618eb 100644
--- a/drivers/acpi/acpica/nsinit.c
+++ b/drivers/acpi/acpica/nsinit.c
@@ -90,7 +90,7 @@ acpi_status acpi_ns_initialize_objects(void)
 
 	/* Set all init info to zero */
 
-	ACPI_MEMSET(&info, 0, sizeof(struct acpi_init_walk_info));
+	memset(&info, 0, sizeof(struct acpi_init_walk_info));
 
 	/* Walk entire namespace from the supplied root */
 
@@ -566,7 +566,7 @@ acpi_ns_init_one_device(acpi_handle obj_handle,
 	ACPI_DEBUG_EXEC(acpi_ut_display_init_pathname
 			(ACPI_TYPE_METHOD, device_node, METHOD_NAME__INI));
 
-	ACPI_MEMSET(info, 0, sizeof(struct acpi_evaluate_info));
+	memset(info, 0, sizeof(struct acpi_evaluate_info));
 	info->prefix_node = device_node;
 	info->relative_pathname = METHOD_NAME__INI;
 	info->parameters = NULL;
diff --git a/drivers/acpi/acpica/nsparse.c b/drivers/acpi/acpica/nsparse.c
index c95a119767b5..57a4cfe547e4 100644
--- a/drivers/acpi/acpica/nsparse.c
+++ b/drivers/acpi/acpica/nsparse.c
@@ -117,6 +117,13 @@ acpi_ns_one_complete_parse(u32 pass_number,
 					       (u8) pass_number);
 	}
 
+	/* Found OSDT table, enable the namespace override feature */
+
+	if (ACPI_COMPARE_NAME(table->signature, ACPI_SIG_OSDT) &&
+	    pass_number == ACPI_IMODE_LOAD_PASS1) {
+		walk_state->namespace_override = TRUE;
+	}
+
 	if (ACPI_FAILURE(status)) {
 		acpi_ds_delete_walk_state(walk_state);
 		goto cleanup;
diff --git a/drivers/acpi/acpica/nsprepkg.c b/drivers/acpi/acpica/nsprepkg.c
index 8b79958b7aca..9bb251932b45 100644
--- a/drivers/acpi/acpica/nsprepkg.c
+++ b/drivers/acpi/acpica/nsprepkg.c
@@ -316,6 +316,13 @@ acpi_ns_check_package(struct acpi_evaluate_info *info,
 		    acpi_ns_check_package_list(info, package, elements, count);
 		break;
 
+	case ACPI_PTYPE2_VAR_VAR:
+		/*
+		 * Returns a variable list of packages, each with a variable list
+		 * of objects.
+		 */
+		break;
+
 	case ACPI_PTYPE2_UUID_PAIR:
 
 		/* The package must contain pairs of (UUID + type) */
@@ -487,6 +494,12 @@ acpi_ns_check_package_list(struct acpi_evaluate_info *info,
 			}
 			break;
 
+		case ACPI_PTYPE2_VAR_VAR:
+			/*
+			 * Each subpackage has a fixed or variable number of elements
+			 */
+			break;
+
 		case ACPI_PTYPE2_FIXED:
 
 			/* Each subpackage has a fixed length */
diff --git a/drivers/acpi/acpica/nsrepair.c b/drivers/acpi/acpica/nsrepair.c
index 151fcd95ba84..77d8103d0094 100644
--- a/drivers/acpi/acpica/nsrepair.c
+++ b/drivers/acpi/acpica/nsrepair.c
@@ -497,10 +497,10 @@ acpi_ns_remove_null_elements(struct acpi_evaluate_info *info,
 	case ACPI_PTYPE2_MIN:
 	case ACPI_PTYPE2_REV_FIXED:
 	case ACPI_PTYPE2_FIX_VAR:
-
 		break;
 
 	default:
+	case ACPI_PTYPE2_VAR_VAR:
 	case ACPI_PTYPE1_FIXED:
 	case ACPI_PTYPE1_OPTION:
 		return;
diff --git a/drivers/acpi/acpica/nsrepair2.c b/drivers/acpi/acpica/nsrepair2.c
index c30672d23878..0515a70f42a4 100644
--- a/drivers/acpi/acpica/nsrepair2.c
+++ b/drivers/acpi/acpica/nsrepair2.c
@@ -580,7 +580,7 @@ acpi_ns_repair_HID(struct acpi_evaluate_info *info,
 	 * # is a hex digit.
 	 */
 	for (dest = new_string->string.pointer; *source; dest++, source++) {
-		*dest = (char)ACPI_TOUPPER(*source);
+		*dest = (char)toupper((int)*source);
 	}
 
 	acpi_ut_remove_reference(return_object);
diff --git a/drivers/acpi/acpica/nssearch.c b/drivers/acpi/acpica/nssearch.c
index 4a9d4a66016e..d73904013830 100644
--- a/drivers/acpi/acpica/nssearch.c
+++ b/drivers/acpi/acpica/nssearch.c
@@ -325,8 +325,41 @@ acpi_ns_search_and_enter(u32 target_name,
 		 * If we found it AND the request specifies that a find is an error,
 		 * return the error
 		 */
-		if ((status == AE_OK) && (flags & ACPI_NS_ERROR_IF_FOUND)) {
-			status = AE_ALREADY_EXISTS;
+		if (status == AE_OK) {
+
+			/* The node was found in the namespace */
+
+			/*
+			 * If the namespace override feature is enabled for this node,
+			 * delete any existing attached sub-object and make the node
+			 * look like a new node that is owned by the override table.
+			 */
+			if (flags & ACPI_NS_OVERRIDE_IF_FOUND) {
+				ACPI_DEBUG_PRINT((ACPI_DB_NAMES,
+						  "Namespace override: %4.4s pass %u type %X Owner %X\n",
+						  ACPI_CAST_PTR(char,
+								&target_name),
+						  interpreter_mode,
+						  (*return_node)->type,
+						  walk_state->owner_id));
+
+				acpi_ns_delete_children(*return_node);
+				if (acpi_gbl_runtime_namespace_override) {
+					acpi_ut_remove_reference((*return_node)->object);
+					(*return_node)->object = NULL;
+					(*return_node)->owner_id =
+					    walk_state->owner_id;
+				} else {
+					acpi_ns_remove_node(*return_node);
+					*return_node = ACPI_ENTRY_NOT_FOUND;
+				}
+			}
+
+			/* Return an error if we don't expect to find the object */
+
+			else if (flags & ACPI_NS_ERROR_IF_FOUND) {
+				status = AE_ALREADY_EXISTS;
+			}
 		}
 #ifdef ACPI_ASL_COMPILER
 		if (*return_node && (*return_node)->type == ACPI_TYPE_ANY) {
diff --git a/drivers/acpi/acpica/nsutils.c b/drivers/acpi/acpica/nsutils.c
index 6ad02008c0c2..8d8104b8bd28 100644
--- a/drivers/acpi/acpica/nsutils.c
+++ b/drivers/acpi/acpica/nsutils.c
@@ -292,8 +292,7 @@ acpi_status acpi_ns_build_internal_name(struct acpi_namestring_info *info)
 			} else {
 				/* Convert the character to uppercase and save it */
 
-				result[i] =
-				    (char)ACPI_TOUPPER((int)*external_name);
+				result[i] = (char)toupper((int)*external_name);
 				external_name++;
 			}
 		}
diff --git a/drivers/acpi/acpica/nsxfeval.c b/drivers/acpi/acpica/nsxfeval.c
index b6030a2deee1..6ee1e52b903d 100644
--- a/drivers/acpi/acpica/nsxfeval.c
+++ b/drivers/acpi/acpica/nsxfeval.c
@@ -696,7 +696,7 @@ acpi_ns_get_device_callback(acpi_handle obj_handle,
 			return (AE_CTRL_DEPTH);
 		}
 
-		no_match = ACPI_STRCMP(hid->string, info->hid);
+		no_match = strcmp(hid->string, info->hid);
 		ACPI_FREE(hid);
 
 		if (no_match) {
@@ -715,8 +715,7 @@ acpi_ns_get_device_callback(acpi_handle obj_handle,
 
 			found = FALSE;
 			for (i = 0; i < cid->count; i++) {
-				if (ACPI_STRCMP(cid->ids[i].string, info->hid)
-				    == 0) {
+				if (strcmp(cid->ids[i].string, info->hid) == 0) {
 
 					/* Found a matching CID */
 
diff --git a/drivers/acpi/acpica/nsxfname.c b/drivers/acpi/acpica/nsxfname.c
index d66c326485d8..9ff643b9553f 100644
--- a/drivers/acpi/acpica/nsxfname.c
+++ b/drivers/acpi/acpica/nsxfname.c
@@ -114,7 +114,7 @@ acpi_get_handle(acpi_handle parent,
 
 		/* Special case for root-only, since we can't search for it */
 
-		if (!ACPI_STRCMP(pathname, ACPI_NS_ROOT_PATH)) {
+		if (!strcmp(pathname, ACPI_NS_ROOT_PATH)) {
 			*ret_handle =
 			    ACPI_CAST_PTR(acpi_handle, acpi_gbl_root_node);
 			return (AE_OK);
@@ -242,7 +242,7 @@ static char *acpi_ns_copy_device_id(struct acpi_pnp_device_id *dest,
 
 	/* Copy actual string and return a pointer to the next string area */
 
-	ACPI_MEMCPY(string_area, source->string, source->length);
+	memcpy(string_area, source->string, source->length);
 	return (string_area + source->length);
 }
 
@@ -260,7 +260,7 @@ static char *acpi_ns_copy_device_id(struct acpi_pnp_device_id *dest,
  *              control methods (Such as in the case of a device.)
  *
  * For Device and Processor objects, run the Device _HID, _UID, _CID, _SUB,
- * _STA, _ADR, _sx_w, and _sx_d methods.
+ * _CLS, _STA, _ADR, _sx_w, and _sx_d methods.
  *
  * Note: Allocates the return buffer, must be freed by the caller.
  *
@@ -276,11 +276,12 @@ acpi_get_object_info(acpi_handle handle,
 	struct acpi_pnp_device_id *hid = NULL;
 	struct acpi_pnp_device_id *uid = NULL;
 	struct acpi_pnp_device_id *sub = NULL;
+	struct acpi_pnp_device_id *cls = NULL;
 	char *next_id_string;
 	acpi_object_type type;
 	acpi_name name;
 	u8 param_count = 0;
-	u8 valid = 0;
+	u16 valid = 0;
 	u32 info_size;
 	u32 i;
 	acpi_status status;
@@ -320,7 +321,7 @@ acpi_get_object_info(acpi_handle handle,
 	if ((type == ACPI_TYPE_DEVICE) || (type == ACPI_TYPE_PROCESSOR)) {
 		/*
 		 * Get extra info for ACPI Device/Processor objects only:
-		 * Run the Device _HID, _UID, _SUB, and _CID methods.
+		 * Run the Device _HID, _UID, _SUB, _CID, and _CLS methods.
 		 *
 		 * Note: none of these methods are required, so they may or may
 		 * not be present for this device. The Info->Valid bitfield is used
@@ -363,6 +364,14 @@ acpi_get_object_info(acpi_handle handle,
 			     sizeof(struct acpi_pnp_device_id_list));
 			valid |= ACPI_VALID_CID;
 		}
+
+		/* Execute the Device._CLS method */
+
+		status = acpi_ut_execute_CLS(node, &cls);
+		if (ACPI_SUCCESS(status)) {
+			info_size += cls->length;
+			valid |= ACPI_VALID_CLS;
+		}
 	}
 
 	/*
@@ -486,6 +495,11 @@ acpi_get_object_info(acpi_handle handle,
 		}
 	}
 
+	if (cls) {
+		next_id_string = acpi_ns_copy_device_id(&info->class_code,
+							cls, next_id_string);
+	}
+
 	/* Copy the fixed-length data */
 
 	info->info_size = info_size;
@@ -510,6 +524,9 @@ cleanup:
 	if (cid_list) {
 		ACPI_FREE(cid_list);
 	}
+	if (cls) {
+		ACPI_FREE(cls);
+	}
 	return (status);
 }
 
@@ -620,7 +637,7 @@ acpi_status acpi_install_method(u8 *buffer)
 
 	/* Copy the method AML to the local buffer */
 
-	ACPI_MEMCPY(aml_buffer, aml_start, aml_length);
+	memcpy(aml_buffer, aml_start, aml_length);
 
 	/* Initialize the method object with the new method's information */
 
diff --git a/drivers/acpi/acpica/psopinfo.c b/drivers/acpi/acpica/psopinfo.c
index 20e1a35169fc..58310907fa7b 100644
--- a/drivers/acpi/acpica/psopinfo.c
+++ b/drivers/acpi/acpica/psopinfo.c
@@ -50,9 +50,6 @@
 #define _COMPONENT          ACPI_PARSER
 ACPI_MODULE_NAME("psopinfo")
 
-extern const u8 acpi_gbl_short_op_index[];
-extern const u8 acpi_gbl_long_op_index[];
-
 static const u8 acpi_gbl_argument_count[] =
     { 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 6 };
 
diff --git a/drivers/acpi/acpica/psutils.c b/drivers/acpi/acpica/psutils.c
index 960505ab409a..32440912023a 100644
--- a/drivers/acpi/acpica/psutils.c
+++ b/drivers/acpi/acpica/psutils.c
@@ -93,10 +93,9 @@ void acpi_ps_init_op(union acpi_parse_object *op, u16 opcode)
 	op->common.descriptor_type = ACPI_DESC_TYPE_PARSER;
 	op->common.aml_opcode = opcode;
 
-	ACPI_DISASM_ONLY_MEMBERS(ACPI_STRNCPY(op->common.aml_op_name,
-					      (acpi_ps_get_opcode_info
-					       (opcode))->name,
-					      sizeof(op->common.aml_op_name)));
+	ACPI_DISASM_ONLY_MEMBERS(strncpy(op->common.aml_op_name,
+					 (acpi_ps_get_opcode_info(opcode))->
+					 name, sizeof(op->common.aml_op_name)));
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/acpica/rscreate.c b/drivers/acpi/acpica/rscreate.c
index 15434e4c9b34..3fa829e96c2a 100644
--- a/drivers/acpi/acpica/rscreate.c
+++ b/drivers/acpi/acpica/rscreate.c
@@ -353,13 +353,13 @@ acpi_rs_create_pci_routing_table(union acpi_operand_object *package_object,
 				/* +1 to include null terminator */
 
 				user_prt->length +=
-				    (u32) ACPI_STRLEN(user_prt->source) + 1;
+				    (u32)strlen(user_prt->source) + 1;
 				break;
 
 			case ACPI_TYPE_STRING:
 
-				ACPI_STRCPY(user_prt->source,
-					    obj_desc->string.pointer);
+				strcpy(user_prt->source,
+				       obj_desc->string.pointer);
 
 				/*
 				 * Add to the Length field the length of the string
diff --git a/drivers/acpi/acpica/rsmisc.c b/drivers/acpi/acpica/rsmisc.c
index 1fe49d223663..ac37852e0821 100644
--- a/drivers/acpi/acpica/rsmisc.c
+++ b/drivers/acpi/acpica/rsmisc.c
@@ -119,7 +119,7 @@ acpi_rs_convert_aml_to_resource(struct acpi_resource *resource,
 			/*
 			 * Get the resource type and the initial (minimum) length
 			 */
-			ACPI_MEMSET(resource, 0, INIT_RESOURCE_LENGTH(info));
+			memset(resource, 0, INIT_RESOURCE_LENGTH(info));
 			resource->type = INIT_RESOURCE_TYPE(info);
 			resource->length = INIT_RESOURCE_LENGTH(info);
 			break;
@@ -324,13 +324,13 @@ acpi_rs_convert_aml_to_resource(struct acpi_resource *resource,
 
 		case ACPI_RSC_SET8:
 
-			ACPI_MEMSET(destination, info->aml_offset, info->value);
+			memset(destination, info->aml_offset, info->value);
 			break;
 
 		case ACPI_RSC_DATA8:
 
 			target = ACPI_ADD_PTR(char, resource, info->value);
-			ACPI_MEMCPY(destination, source, ACPI_GET16(target));
+			memcpy(destination, source, ACPI_GET16(target));
 			break;
 
 		case ACPI_RSC_ADDRESS:
@@ -502,7 +502,7 @@ acpi_rs_convert_resource_to_aml(struct acpi_resource *resource,
 		switch (info->opcode) {
 		case ACPI_RSC_INITSET:
 
-			ACPI_MEMSET(aml, 0, INIT_RESOURCE_LENGTH(info));
+			memset(aml, 0, INIT_RESOURCE_LENGTH(info));
 			aml_length = INIT_RESOURCE_LENGTH(info);
 			acpi_rs_set_resource_header(INIT_RESOURCE_TYPE(info),
 						    aml_length, aml);
diff --git a/drivers/acpi/acpica/rsutils.c b/drivers/acpi/acpica/rsutils.c
index ece3cd60cc6a..52b024df0052 100644
--- a/drivers/acpi/acpica/rsutils.c
+++ b/drivers/acpi/acpica/rsutils.c
@@ -148,7 +148,7 @@ acpi_rs_move_data(void *destination, void *source, u16 item_count, u8 move_type)
 		case ACPI_RSC_MOVE_SERIAL_VEN:
 		case ACPI_RSC_MOVE_SERIAL_RES:
 
-			ACPI_MEMCPY(destination, source, item_count);
+			memcpy(destination, source, item_count);
 			return;
 
 			/*
@@ -364,12 +364,11 @@ acpi_rs_get_resource_source(acpi_rs_length resource_length,
 		 * Zero the entire area of the buffer.
 		 */
 		total_length =
-		    (u32)
-		    ACPI_STRLEN(ACPI_CAST_PTR(char, &aml_resource_source[1])) +
+		    (u32)strlen(ACPI_CAST_PTR(char, &aml_resource_source[1])) +
 		    1;
-		total_length = (u32) ACPI_ROUND_UP_TO_NATIVE_WORD(total_length);
+		total_length = (u32)ACPI_ROUND_UP_TO_NATIVE_WORD(total_length);
 
-		ACPI_MEMSET(resource_source->string_ptr, 0, total_length);
+		memset(resource_source->string_ptr, 0, total_length);
 
 		/* Copy the resource_source string to the destination */
 
@@ -432,8 +431,8 @@ acpi_rs_set_resource_source(union aml_resource * aml,
 
 		/* Copy the resource_source string */
 
-		ACPI_STRCPY(ACPI_CAST_PTR(char, &aml_resource_source[1]),
-			    resource_source->string_ptr);
+		strcpy(ACPI_CAST_PTR(char, &aml_resource_source[1]),
+		       resource_source->string_ptr);
 
 		/*
 		 * Add the length of the string (+ 1 for null terminator) to the
diff --git a/drivers/acpi/acpica/rsxface.c b/drivers/acpi/acpica/rsxface.c
index 8e6276df0226..de51f836ef68 100644
--- a/drivers/acpi/acpica/rsxface.c
+++ b/drivers/acpi/acpica/rsxface.c
@@ -398,8 +398,8 @@ acpi_resource_to_address64(struct acpi_resource *resource,
 
 		/* Simple copy for 64 bit source */
 
-		ACPI_MEMCPY(out, &resource->data,
-			    sizeof(struct acpi_resource_address64));
+		memcpy(out, &resource->data,
+		       sizeof(struct acpi_resource_address64));
 		break;
 
 	default:
@@ -499,7 +499,7 @@ acpi_rs_match_vendor_resource(struct acpi_resource *resource, void *context)
 	 */
 	if ((vendor->byte_length < (ACPI_UUID_LENGTH + 1)) ||
 	    (vendor->uuid_subtype != info->uuid->subtype) ||
-	    (ACPI_MEMCMP(vendor->uuid, info->uuid->data, ACPI_UUID_LENGTH))) {
+	    (memcmp(vendor->uuid, info->uuid->data, ACPI_UUID_LENGTH))) {
 		return (AE_OK);
 	}
 
@@ -513,7 +513,7 @@ acpi_rs_match_vendor_resource(struct acpi_resource *resource, void *context)
 
 	/* Found the correct resource, copy and return it */
 
-	ACPI_MEMCPY(buffer->pointer, resource, resource->length);
+	memcpy(buffer->pointer, resource, resource->length);
 	buffer->length = resource->length;
 
 	/* Found the desired descriptor, terminate resource walk */
diff --git a/drivers/acpi/acpica/tbdata.c b/drivers/acpi/acpica/tbdata.c
index d7f8386455bd..5c9d5abf1588 100644
--- a/drivers/acpi/acpica/tbdata.c
+++ b/drivers/acpi/acpica/tbdata.c
@@ -73,7 +73,7 @@ acpi_tb_init_table_descriptor(struct acpi_table_desc *table_desc,
 	 * Initialize the table descriptor. Set the pointer to NULL, since the
 	 * table is not fully mapped at this time.
 	 */
-	ACPI_MEMSET(table_desc, 0, sizeof(struct acpi_table_desc));
+	memset(table_desc, 0, sizeof(struct acpi_table_desc));
 	table_desc->address = address;
 	table_desc->length = table->length;
 	table_desc->flags = flags;
@@ -465,9 +465,9 @@ acpi_status acpi_tb_resize_root_table_list(void)
 	/* Copy and free the previous table array */
 
 	if (acpi_gbl_root_table_list.tables) {
-		ACPI_MEMCPY(tables, acpi_gbl_root_table_list.tables,
-			    (acpi_size) table_count *
-			    sizeof(struct acpi_table_desc));
+		memcpy(tables, acpi_gbl_root_table_list.tables,
+		       (acpi_size) table_count *
+		       sizeof(struct acpi_table_desc));
 
 		if (acpi_gbl_root_table_list.flags & ACPI_ROOT_ORIGIN_ALLOCATED) {
 			ACPI_FREE(acpi_gbl_root_table_list.tables);
diff --git a/drivers/acpi/acpica/tbfadt.c b/drivers/acpi/acpica/tbfadt.c
index 7d2486005e3f..6253001b6375 100644
--- a/drivers/acpi/acpica/tbfadt.c
+++ b/drivers/acpi/acpica/tbfadt.c
@@ -350,9 +350,18 @@ void acpi_tb_parse_fadt(u32 table_index)
 	/* If Hardware Reduced flag is set, there is no FACS */
 
 	if (!acpi_gbl_reduced_hardware) {
-		acpi_tb_install_fixed_table((acpi_physical_address)
-					    acpi_gbl_FADT.Xfacs, ACPI_SIG_FACS,
-					    ACPI_TABLE_INDEX_FACS);
+		if (acpi_gbl_FADT.facs) {
+			acpi_tb_install_fixed_table((acpi_physical_address)
+						    acpi_gbl_FADT.facs,
+						    ACPI_SIG_FACS,
+						    ACPI_TABLE_INDEX_FACS);
+		}
+		if (acpi_gbl_FADT.Xfacs) {
+			acpi_tb_install_fixed_table((acpi_physical_address)
+						    acpi_gbl_FADT.Xfacs,
+						    ACPI_SIG_FACS,
+						    ACPI_TABLE_INDEX_X_FACS);
+		}
 	}
 }
 
@@ -389,12 +398,12 @@ void acpi_tb_create_local_fadt(struct acpi_table_header *table, u32 length)
 
 	/* Clear the entire local FADT */
 
-	ACPI_MEMSET(&acpi_gbl_FADT, 0, sizeof(struct acpi_table_fadt));
+	memset(&acpi_gbl_FADT, 0, sizeof(struct acpi_table_fadt));
 
 	/* Copy the original FADT, up to sizeof (struct acpi_table_fadt) */
 
-	ACPI_MEMCPY(&acpi_gbl_FADT, table,
-		    ACPI_MIN(length, sizeof(struct acpi_table_fadt)));
+	memcpy(&acpi_gbl_FADT, table,
+	       ACPI_MIN(length, sizeof(struct acpi_table_fadt)));
 
 	/* Take a copy of the Hardware Reduced flag */
 
@@ -491,13 +500,9 @@ static void acpi_tb_convert_fadt(void)
 	acpi_gbl_FADT.header.length = sizeof(struct acpi_table_fadt);
 
 	/*
-	 * Expand the 32-bit FACS and DSDT addresses to 64-bit as necessary.
+	 * Expand the 32-bit DSDT addresses to 64-bit as necessary.
 	 * Later ACPICA code will always use the X 64-bit field.
 	 */
-	acpi_gbl_FADT.Xfacs = acpi_tb_select_address("FACS",
-						     acpi_gbl_FADT.facs,
-						     acpi_gbl_FADT.Xfacs);
-
 	acpi_gbl_FADT.Xdsdt = acpi_tb_select_address("DSDT",
 						     acpi_gbl_FADT.dsdt,
 						     acpi_gbl_FADT.Xdsdt);
diff --git a/drivers/acpi/acpica/tbfind.c b/drivers/acpi/acpica/tbfind.c
index 0b879fcfef67..119c84ad9833 100644
--- a/drivers/acpi/acpica/tbfind.c
+++ b/drivers/acpi/acpica/tbfind.c
@@ -76,16 +76,16 @@ acpi_tb_find_table(char *signature,
 
 	/* Normalize the input strings */
 
-	ACPI_MEMSET(&header, 0, sizeof(struct acpi_table_header));
+	memset(&header, 0, sizeof(struct acpi_table_header));
 	ACPI_MOVE_NAME(header.signature, signature);
-	ACPI_STRNCPY(header.oem_id, oem_id, ACPI_OEM_ID_SIZE);
-	ACPI_STRNCPY(header.oem_table_id, oem_table_id, ACPI_OEM_TABLE_ID_SIZE);
+	strncpy(header.oem_id, oem_id, ACPI_OEM_ID_SIZE);
+	strncpy(header.oem_table_id, oem_table_id, ACPI_OEM_TABLE_ID_SIZE);
 
 	/* Search for the table */
 
 	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; ++i) {
-		if (ACPI_MEMCMP(&(acpi_gbl_root_table_list.tables[i].signature),
-				header.signature, ACPI_NAME_SIZE)) {
+		if (memcmp(&(acpi_gbl_root_table_list.tables[i].signature),
+			   header.signature, ACPI_NAME_SIZE)) {
 
 			/* Not the requested table */
 
@@ -112,21 +112,20 @@ acpi_tb_find_table(char *signature,
 
 		/* Check for table match on all IDs */
 
-		if (!ACPI_MEMCMP
+		if (!memcmp
 		    (acpi_gbl_root_table_list.tables[i].pointer->signature,
 		     header.signature, ACPI_NAME_SIZE) && (!oem_id[0]
 							   ||
-							   !ACPI_MEMCMP
+							   !memcmp
 							   (acpi_gbl_root_table_list.
 							    tables[i].pointer->
 							    oem_id,
 							    header.oem_id,
 							    ACPI_OEM_ID_SIZE))
 		    && (!oem_table_id[0]
-			|| !ACPI_MEMCMP(acpi_gbl_root_table_list.tables[i].
-					pointer->oem_table_id,
-					header.oem_table_id,
-					ACPI_OEM_TABLE_ID_SIZE))) {
+			|| !memcmp(acpi_gbl_root_table_list.tables[i].pointer->
+				   oem_table_id, header.oem_table_id,
+				   ACPI_OEM_TABLE_ID_SIZE))) {
 			*table_index = i;
 
 			ACPI_DEBUG_PRINT((ACPI_DB_TABLES,
diff --git a/drivers/acpi/acpica/tbinstal.c b/drivers/acpi/acpica/tbinstal.c
index 008a251780f4..15ea98e0068d 100644
--- a/drivers/acpi/acpica/tbinstal.c
+++ b/drivers/acpi/acpica/tbinstal.c
@@ -87,8 +87,8 @@ acpi_tb_compare_tables(struct acpi_table_desc *table_desc, u32 table_index)
 	 * not just the header.
 	 */
 	is_identical = (u8)((table_desc->length != table_length ||
-			     ACPI_MEMCMP(table_desc->pointer, table,
-					 table_length)) ? FALSE : TRUE);
+			     memcmp(table_desc->pointer, table, table_length)) ?
+			    FALSE : TRUE);
 
 	/* Release the acquired table */
 
@@ -289,8 +289,7 @@ acpi_tb_install_standard_table(acpi_physical_address address,
 		if ((new_table_desc.signature.ascii[0] != 0x00) &&
 		    (!ACPI_COMPARE_NAME
 		     (&new_table_desc.signature, ACPI_SIG_SSDT))
-		    && (ACPI_STRNCMP(new_table_desc.signature.ascii, "OEM", 3)))
-		{
+		    && (strncmp(new_table_desc.signature.ascii, "OEM", 3))) {
 			ACPI_BIOS_ERROR((AE_INFO,
 					 "Table has invalid signature [%4.4s] (0x%8.8X), "
 					 "must be SSDT or OEMx",
diff --git a/drivers/acpi/acpica/tbprint.c b/drivers/acpi/acpica/tbprint.c
index 77ba5c71c6e7..709d5112fc16 100644
--- a/drivers/acpi/acpica/tbprint.c
+++ b/drivers/acpi/acpica/tbprint.c
@@ -73,7 +73,7 @@ static void acpi_tb_fix_string(char *string, acpi_size length)
 {
 
 	while (length && *string) {
-		if (!ACPI_IS_PRINT(*string)) {
+		if (!isprint((int)*string)) {
 			*string = '?';
 		}
 		string++;
@@ -100,7 +100,7 @@ acpi_tb_cleanup_table_header(struct acpi_table_header *out_header,
 			     struct acpi_table_header *header)
 {
 
-	ACPI_MEMCPY(out_header, header, sizeof(struct acpi_table_header));
+	memcpy(out_header, header, sizeof(struct acpi_table_header));
 
 	acpi_tb_fix_string(out_header->signature, ACPI_NAME_SIZE);
 	acpi_tb_fix_string(out_header->oem_id, ACPI_OEM_ID_SIZE);
@@ -138,9 +138,9 @@ acpi_tb_print_table_header(acpi_physical_address address,
 
 		/* RSDP has no common fields */
 
-		ACPI_MEMCPY(local_header.oem_id,
-			    ACPI_CAST_PTR(struct acpi_table_rsdp,
-					  header)->oem_id, ACPI_OEM_ID_SIZE);
+		memcpy(local_header.oem_id,
+		       ACPI_CAST_PTR(struct acpi_table_rsdp, header)->oem_id,
+		       ACPI_OEM_ID_SIZE);
 		acpi_tb_fix_string(local_header.oem_id, ACPI_OEM_ID_SIZE);
 
 		ACPI_INFO((AE_INFO, "RSDP 0x%8.8X%8.8X %06X (v%.2d %-6.6s)",
diff --git a/drivers/acpi/acpica/tbutils.c b/drivers/acpi/acpica/tbutils.c
index 6559a58439c5..568ac0e4a3c6 100644
--- a/drivers/acpi/acpica/tbutils.c
+++ b/drivers/acpi/acpica/tbutils.c
@@ -68,7 +68,6 @@ acpi_tb_get_root_table_entry(u8 *table_entry, u32 table_entry_size);
 
 acpi_status acpi_tb_initialize_facs(void)
 {
-	acpi_status status;
 
 	/* If Hardware Reduced flag is set, there is no FACS */
 
@@ -77,11 +76,25 @@ acpi_status acpi_tb_initialize_facs(void)
 		return (AE_OK);
 	}
 
-	status = acpi_get_table_by_index(ACPI_TABLE_INDEX_FACS,
-					 ACPI_CAST_INDIRECT_PTR(struct
-								acpi_table_header,
-								&acpi_gbl_FACS));
-	return (status);
+	(void)acpi_get_table_by_index(ACPI_TABLE_INDEX_FACS,
+				      ACPI_CAST_INDIRECT_PTR(struct
+							     acpi_table_header,
+							     &acpi_gbl_facs32));
+	(void)acpi_get_table_by_index(ACPI_TABLE_INDEX_X_FACS,
+				      ACPI_CAST_INDIRECT_PTR(struct
+							     acpi_table_header,
+							     &acpi_gbl_facs64));
+
+	if (acpi_gbl_facs64
+	    && (!acpi_gbl_facs32 || !acpi_gbl_use32_bit_facs_addresses)) {
+		acpi_gbl_FACS = acpi_gbl_facs64;
+	} else if (acpi_gbl_facs32) {
+		acpi_gbl_FACS = acpi_gbl_facs32;
+	}
+
+	/* If there is no FACS, just continue. There was already an error msg */
+
+	return (AE_OK);
 }
 #endif				/* !ACPI_REDUCED_HARDWARE */
 
@@ -101,7 +114,7 @@ acpi_status acpi_tb_initialize_facs(void)
 u8 acpi_tb_tables_loaded(void)
 {
 
-	if (acpi_gbl_root_table_list.current_table_count >= 3) {
+	if (acpi_gbl_root_table_list.current_table_count >= 4) {
 		return (TRUE);
 	}
 
@@ -175,7 +188,7 @@ struct acpi_table_header *acpi_tb_copy_dsdt(u32 table_index)
 		return (NULL);
 	}
 
-	ACPI_MEMCPY(new_table, table_desc->pointer, table_desc->length);
+	memcpy(new_table, table_desc->pointer, table_desc->length);
 	acpi_tb_uninstall_table(table_desc);
 
 	acpi_tb_init_table_descriptor(&acpi_gbl_root_table_list.
@@ -357,11 +370,11 @@ acpi_status __init acpi_tb_parse_root_table(acpi_physical_address rsdp_address)
 	table_entry = ACPI_ADD_PTR(u8, table, sizeof(struct acpi_table_header));
 
 	/*
-	 * First two entries in the table array are reserved for the DSDT
-	 * and FACS, which are not actually present in the RSDT/XSDT - they
-	 * come from the FADT
+	 * First three entries in the table array are reserved for the DSDT
+	 * and 32bit/64bit FACS, which are not actually present in the
+	 * RSDT/XSDT - they come from the FADT
 	 */
-	acpi_gbl_root_table_list.current_table_count = 2;
+	acpi_gbl_root_table_list.current_table_count = 3;
 
 	/* Initialize the root table array from the RSDT/XSDT */
 
diff --git a/drivers/acpi/acpica/tbxface.c b/drivers/acpi/acpica/tbxface.c
index 60e94f87f27a..5559e2c70b15 100644
--- a/drivers/acpi/acpica/tbxface.c
+++ b/drivers/acpi/acpica/tbxface.c
@@ -119,9 +119,9 @@ acpi_initialize_tables(struct acpi_table_desc * initial_table_array,
 	} else {
 		/* Root Table Array has been statically allocated by the host */
 
-		ACPI_MEMSET(initial_table_array, 0,
-			    (acpi_size) initial_table_count *
-			    sizeof(struct acpi_table_desc));
+		memset(initial_table_array, 0,
+		       (acpi_size) initial_table_count *
+		       sizeof(struct acpi_table_desc));
 
 		acpi_gbl_root_table_list.tables = initial_table_array;
 		acpi_gbl_root_table_list.max_table_count = initial_table_count;
@@ -242,8 +242,9 @@ acpi_get_table_header(char *signature,
 				if (!header) {
 					return (AE_NO_MEMORY);
 				}
-				ACPI_MEMCPY(out_table_header, header,
-					    sizeof(struct acpi_table_header));
+
+				memcpy(out_table_header, header,
+				       sizeof(struct acpi_table_header));
 				acpi_os_unmap_memory(header,
 						     sizeof(struct
 							    acpi_table_header));
@@ -251,9 +252,9 @@ acpi_get_table_header(char *signature,
 				return (AE_NOT_FOUND);
 			}
 		} else {
-			ACPI_MEMCPY(out_table_header,
-				    acpi_gbl_root_table_list.tables[i].pointer,
-				    sizeof(struct acpi_table_header));
+			memcpy(out_table_header,
+			       acpi_gbl_root_table_list.tables[i].pointer,
+			       sizeof(struct acpi_table_header));
 		}
 		return (AE_OK);
 	}
diff --git a/drivers/acpi/acpica/tbxfload.c b/drivers/acpi/acpica/tbxfload.c
index aadb3002a2dd..9682d40ca6ff 100644
--- a/drivers/acpi/acpica/tbxfload.c
+++ b/drivers/acpi/acpica/tbxfload.c
@@ -150,8 +150,8 @@ static acpi_status acpi_tb_load_namespace(void)
 	 * Save the original DSDT header for detection of table corruption
 	 * and/or replacement of the DSDT from outside the OS.
 	 */
-	ACPI_MEMCPY(&acpi_gbl_original_dsdt_header, acpi_gbl_DSDT,
-		    sizeof(struct acpi_table_header));
+	memcpy(&acpi_gbl_original_dsdt_header, acpi_gbl_DSDT,
+	       sizeof(struct acpi_table_header));
 
 	(void)acpi_ut_release_mutex(ACPI_MTX_TABLES);
 
@@ -166,13 +166,18 @@ static acpi_status acpi_tb_load_namespace(void)
 
 	(void)acpi_ut_acquire_mutex(ACPI_MTX_TABLES);
 	for (i = 0; i < acpi_gbl_root_table_list.current_table_count; ++i) {
-		if ((!ACPI_COMPARE_NAME
+		if (!acpi_gbl_root_table_list.tables[i].address ||
+		    (!ACPI_COMPARE_NAME
 		     (&(acpi_gbl_root_table_list.tables[i].signature),
 		      ACPI_SIG_SSDT)
 		     &&
 		     !ACPI_COMPARE_NAME(&
 					(acpi_gbl_root_table_list.tables[i].
-					 signature), ACPI_SIG_PSDT))
+					 signature), ACPI_SIG_PSDT)
+		     &&
+		     !ACPI_COMPARE_NAME(&
+					(acpi_gbl_root_table_list.tables[i].
+					 signature), ACPI_SIG_OSDT))
 		    ||
 		    ACPI_FAILURE(acpi_tb_validate_table
 				 (&acpi_gbl_root_table_list.tables[i]))) {
@@ -219,9 +224,9 @@ acpi_install_table(acpi_physical_address address, u8 physical)
 	ACPI_FUNCTION_TRACE(acpi_install_table);
 
 	if (physical) {
-		flags = ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL;
-	} else {
 		flags = ACPI_TABLE_ORIGIN_INTERNAL_PHYSICAL;
+	} else {
+		flags = ACPI_TABLE_ORIGIN_EXTERNAL_VIRTUAL;
 	}
 
 	status = acpi_tb_install_standard_table(address, flags,
diff --git a/drivers/acpi/acpica/utalloc.c b/drivers/acpi/acpica/utalloc.c
index 61d8f6d186d1..7a4101f0685e 100644
--- a/drivers/acpi/acpica/utalloc.c
+++ b/drivers/acpi/acpica/utalloc.c
@@ -73,7 +73,7 @@ void *acpi_os_allocate_zeroed(acpi_size size)
 
 		/* Clear the memory block */
 
-		ACPI_MEMSET(allocation, 0, size);
+		memset(allocation, 0, size);
 	}
 
 	return (allocation);
@@ -181,7 +181,7 @@ acpi_status acpi_ut_delete_caches(void)
 	char buffer[7];
 
 	if (acpi_gbl_display_final_mem_stats) {
-		ACPI_STRCPY(buffer, "MEMORY");
+		strcpy(buffer, "MEMORY");
 		(void)acpi_db_display_statistics(buffer);
 	}
 #endif
@@ -337,6 +337,6 @@ acpi_ut_initialize_buffer(struct acpi_buffer * buffer,
 
 	/* Have a valid buffer, clear it */
 
-	ACPI_MEMSET(buffer->pointer, 0, required_length);
+	memset(buffer->pointer, 0, required_length);
 	return (AE_OK);
 }
diff --git a/drivers/acpi/acpica/utbuffer.c b/drivers/acpi/acpica/utbuffer.c
index a8c39643e618..01c8709ca586 100644
--- a/drivers/acpi/acpica/utbuffer.c
+++ b/drivers/acpi/acpica/utbuffer.c
@@ -159,7 +159,7 @@ void acpi_ut_dump_buffer(u8 *buffer, u32 count, u32 display, u32 base_offset)
 			}
 
 			buf_char = buffer[(acpi_size) i + j];
-			if (ACPI_IS_PRINT(buf_char)) {
+			if (isprint(buf_char)) {
 				acpi_os_printf("%c", buf_char);
 			} else {
 				acpi_os_printf(".");
@@ -319,7 +319,7 @@ acpi_ut_dump_buffer_to_file(ACPI_FILE file,
 			}
 
 			buf_char = buffer[(acpi_size) i + j];
-			if (ACPI_IS_PRINT(buf_char)) {
+			if (isprint(buf_char)) {
 				acpi_ut_file_printf(file, "%c", buf_char);
 			} else {
 				acpi_ut_file_printf(file, ".");
diff --git a/drivers/acpi/acpica/utcache.c b/drivers/acpi/acpica/utcache.c
index eacc5eee362e..0d21fbd99363 100644
--- a/drivers/acpi/acpica/utcache.c
+++ b/drivers/acpi/acpica/utcache.c
@@ -84,7 +84,7 @@ acpi_os_create_cache(char *cache_name,
 
 	/* Populate the cache object and return it */
 
-	ACPI_MEMSET(cache, 0, sizeof(struct acpi_memory_list));
+	memset(cache, 0, sizeof(struct acpi_memory_list));
 	cache->list_name = cache_name;
 	cache->object_size = object_size;
 	cache->max_depth = max_depth;
@@ -212,7 +212,7 @@ acpi_os_release_object(struct acpi_memory_list * cache, void *object)
 
 		/* Mark the object as cached */
 
-		ACPI_MEMSET(object, 0xCA, cache->object_size);
+		memset(object, 0xCA, cache->object_size);
 		ACPI_SET_DESCRIPTOR_TYPE(object, ACPI_DESC_TYPE_CACHED);
 
 		/* Put the object at the head of the cache list */
@@ -281,7 +281,7 @@ void *acpi_os_acquire_object(struct acpi_memory_list *cache)
 
 		/* Clear (zero) the previously used Object */
 
-		ACPI_MEMSET(object, 0, cache->object_size);
+		memset(object, 0, cache->object_size);
 	} else {
 		/* The cache is empty, create a new object */
 
diff --git a/drivers/acpi/acpica/utcopy.c b/drivers/acpi/acpica/utcopy.c
index c37ec5035f4c..257221d452c8 100644
--- a/drivers/acpi/acpica/utcopy.c
+++ b/drivers/acpi/acpica/utcopy.c
@@ -129,7 +129,7 @@ acpi_ut_copy_isimple_to_esimple(union acpi_operand_object *internal_object,
 
 	/* Always clear the external object */
 
-	ACPI_MEMSET(external_object, 0, sizeof(union acpi_object));
+	memset(external_object, 0, sizeof(union acpi_object));
 
 	/*
 	 * In general, the external object will be the same type as
@@ -149,9 +149,9 @@ acpi_ut_copy_isimple_to_esimple(union acpi_operand_object *internal_object,
 								  string.
 								  length + 1);
 
-		ACPI_MEMCPY((void *)data_space,
-			    (void *)internal_object->string.pointer,
-			    (acpi_size) internal_object->string.length + 1);
+		memcpy((void *)data_space,
+		       (void *)internal_object->string.pointer,
+		       (acpi_size) internal_object->string.length + 1);
 		break;
 
 	case ACPI_TYPE_BUFFER:
@@ -162,9 +162,9 @@ acpi_ut_copy_isimple_to_esimple(union acpi_operand_object *internal_object,
 		    ACPI_ROUND_UP_TO_NATIVE_WORD(internal_object->string.
 						 length);
 
-		ACPI_MEMCPY((void *)data_space,
-			    (void *)internal_object->buffer.pointer,
-			    internal_object->buffer.length);
+		memcpy((void *)data_space,
+		       (void *)internal_object->buffer.pointer,
+		       internal_object->buffer.length);
 		break;
 
 	case ACPI_TYPE_INTEGER:
@@ -502,9 +502,9 @@ acpi_ut_copy_esimple_to_isimple(union acpi_object *external_object,
 			goto error_exit;
 		}
 
-		ACPI_MEMCPY(internal_object->string.pointer,
-			    external_object->string.pointer,
-			    external_object->string.length);
+		memcpy(internal_object->string.pointer,
+		       external_object->string.pointer,
+		       external_object->string.length);
 
 		internal_object->string.length = external_object->string.length;
 		break;
@@ -517,9 +517,9 @@ acpi_ut_copy_esimple_to_isimple(union acpi_object *external_object,
 			goto error_exit;
 		}
 
-		ACPI_MEMCPY(internal_object->buffer.pointer,
-			    external_object->buffer.pointer,
-			    external_object->buffer.length);
+		memcpy(internal_object->buffer.pointer,
+		       external_object->buffer.pointer,
+		       external_object->buffer.length);
 
 		internal_object->buffer.length = external_object->buffer.length;
 
@@ -694,8 +694,8 @@ acpi_ut_copy_simple_object(union acpi_operand_object *source_desc,
 		copy_size = sizeof(struct acpi_namespace_node);
 	}
 
-	ACPI_MEMCPY(ACPI_CAST_PTR(char, dest_desc),
-		    ACPI_CAST_PTR(char, source_desc), copy_size);
+	memcpy(ACPI_CAST_PTR(char, dest_desc),
+	       ACPI_CAST_PTR(char, source_desc), copy_size);
 
 	/* Restore the saved fields */
 
@@ -725,9 +725,9 @@ acpi_ut_copy_simple_object(union acpi_operand_object *source_desc,
 
 			/* Copy the actual buffer data */
 
-			ACPI_MEMCPY(dest_desc->buffer.pointer,
-				    source_desc->buffer.pointer,
-				    source_desc->buffer.length);
+			memcpy(dest_desc->buffer.pointer,
+			       source_desc->buffer.pointer,
+			       source_desc->buffer.length);
 		}
 		break;
 
@@ -747,9 +747,9 @@ acpi_ut_copy_simple_object(union acpi_operand_object *source_desc,
 
 			/* Copy the actual string data */
 
-			ACPI_MEMCPY(dest_desc->string.pointer,
-				    source_desc->string.pointer,
-				    (acpi_size) source_desc->string.length + 1);
+			memcpy(dest_desc->string.pointer,
+			       source_desc->string.pointer,
+			       (acpi_size) source_desc->string.length + 1);
 		}
 		break;
 
diff --git a/drivers/acpi/acpica/utdebug.c b/drivers/acpi/acpica/utdebug.c
index 4f3f888d33bb..cd02693841db 100644
--- a/drivers/acpi/acpica/utdebug.c
+++ b/drivers/acpi/acpica/utdebug.c
@@ -111,8 +111,8 @@ void acpi_ut_track_stack_ptr(void)
  * RETURN:      Updated pointer to the function name
  *
  * DESCRIPTION: Remove the "Acpi" prefix from the function name, if present.
- *              This allows compiler macros such as __func__ to be used with no
- *              change to the debug output.
+ *              This allows compiler macros such as __func__ to be used
+ *              with no change to the debug output.
  *
  ******************************************************************************/
 
diff --git a/drivers/acpi/acpica/utfileio.c b/drivers/acpi/acpica/utfileio.c
index 7e1168be39fa..857af824337b 100644
--- a/drivers/acpi/acpica/utfileio.c
+++ b/drivers/acpi/acpica/utfileio.c
@@ -198,11 +198,8 @@ acpi_ut_read_table(FILE * fp,
 			     table_header.length, file_size);
 
 #ifdef ACPI_ASL_COMPILER
-			status = fl_check_for_ascii(fp, NULL, FALSE);
-			if (ACPI_SUCCESS(status)) {
-				acpi_os_printf
-				    ("File appears to be ASCII only, must be binary\n");
-			}
+			acpi_os_printf("File is corrupt or is ASCII text -- "
+				       "it must be a binary file\n");
 #endif
 			return (AE_BAD_HEADER);
 		}
@@ -315,7 +312,7 @@ acpi_ut_read_table_from_file(char *filename, struct acpi_table_header ** table)
 	/* Get the entire file */
 
 	fprintf(stderr,
-		"Loading Acpi table from file %10s - Length %.8u (%06X)\n",
+		"Reading ACPI table from file %10s - Length %.8u (0x%06X)\n",
 		filename, file_size, file_size);
 
 	status = acpi_ut_read_table(file, table, &table_length);
diff --git a/drivers/acpi/acpica/utglobal.c b/drivers/acpi/acpica/utglobal.c
index 5e8df9177da4..a72685c1e819 100644
--- a/drivers/acpi/acpica/utglobal.c
+++ b/drivers/acpi/acpica/utglobal.c
@@ -102,12 +102,19 @@ const struct acpi_predefined_names acpi_gbl_pre_defined_names[] = {
 	{"_SB_", ACPI_TYPE_DEVICE, NULL},
 	{"_SI_", ACPI_TYPE_LOCAL_SCOPE, NULL},
 	{"_TZ_", ACPI_TYPE_DEVICE, NULL},
-	{"_REV", ACPI_TYPE_INTEGER, (char *)ACPI_CA_SUPPORT_LEVEL},
+	/*
+	 * March, 2015:
+	 * The _REV object is in the process of being deprecated, because
+	 * other ACPI implementations permanently return 2. Thus, it
+	 * has little or no value. Return 2 for compatibility with
+	 * other ACPI implementations.
+	 */
+	{"_REV", ACPI_TYPE_INTEGER, ACPI_CAST_PTR(char, 2)},
 	{"_OS_", ACPI_TYPE_STRING, ACPI_OS_NAME},
-	{"_GL_", ACPI_TYPE_MUTEX, (char *)1},
+	{"_GL_", ACPI_TYPE_MUTEX, ACPI_CAST_PTR(char, 1)},
 
 #if !defined (ACPI_NO_METHOD_EXECUTION) || defined (ACPI_CONSTANT_EVAL_ONLY)
-	{"_OSI", ACPI_TYPE_METHOD, (char *)1},
+	{"_OSI", ACPI_TYPE_METHOD, ACPI_CAST_PTR(char, 1)},
 #endif
 
 	/* Table terminator */
diff --git a/drivers/acpi/acpica/uthex.c b/drivers/acpi/acpica/uthex.c
index aa448278ba28..fda8b3def81c 100644
--- a/drivers/acpi/acpica/uthex.c
+++ b/drivers/acpi/acpica/uthex.c
@@ -75,9 +75,9 @@ char acpi_ut_hex_to_ascii_char(u64 integer, u32 position)
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ut_hex_char_to_value
+ * FUNCTION:    acpi_ut_ascii_char_to_hex
  *
- * PARAMETERS:  ascii_char            - Hex character in Ascii
+ * PARAMETERS:  hex_char                - Hex character in Ascii
  *
  * RETURN:      The binary value of the ascii/hex character
  *
diff --git a/drivers/acpi/acpica/utids.c b/drivers/acpi/acpica/utids.c
index 27431cfc1c44..7956df1e263c 100644
--- a/drivers/acpi/acpica/utids.c
+++ b/drivers/acpi/acpica/utids.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Module Name: utids - support for device Ids - HID, UID, CID
+ * Module Name: utids - support for device Ids - HID, UID, CID, SUB, CLS
  *
  *****************************************************************************/
 
@@ -111,7 +111,7 @@ acpi_ut_execute_HID(struct acpi_namespace_node *device_node,
 	if (obj_desc->common.type == ACPI_TYPE_INTEGER) {
 		acpi_ex_eisa_id_to_string(hid->string, obj_desc->integer.value);
 	} else {
-		ACPI_STRCPY(hid->string, obj_desc->string.pointer);
+		strcpy(hid->string, obj_desc->string.pointer);
 	}
 
 	hid->length = length;
@@ -180,7 +180,7 @@ acpi_ut_execute_SUB(struct acpi_namespace_node *device_node,
 
 	/* Simply copy existing string */
 
-	ACPI_STRCPY(sub->string, obj_desc->string.pointer);
+	strcpy(sub->string, obj_desc->string.pointer);
 	sub->length = length;
 	*return_id = sub;
 
@@ -256,7 +256,7 @@ acpi_ut_execute_UID(struct acpi_namespace_node *device_node,
 	if (obj_desc->common.type == ACPI_TYPE_INTEGER) {
 		acpi_ex_integer_to_string(uid->string, obj_desc->integer.value);
 	} else {
-		ACPI_STRCPY(uid->string, obj_desc->string.pointer);
+		strcpy(uid->string, obj_desc->string.pointer);
 	}
 
 	uid->length = length;
@@ -393,8 +393,7 @@ acpi_ut_execute_CID(struct acpi_namespace_node *device_node,
 
 			/* Copy the String CID from the returned object */
 
-			ACPI_STRCPY(next_id_string,
-				    cid_objects[i]->string.pointer);
+			strcpy(next_id_string, cid_objects[i]->string.pointer);
 			length = cid_objects[i]->string.length + 1;
 		}
 
@@ -416,3 +415,92 @@ cleanup:
 	acpi_ut_remove_reference(obj_desc);
 	return_ACPI_STATUS(status);
 }
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ut_execute_CLS
+ *
+ * PARAMETERS:  device_node         - Node for the device
+ *              return_id           - Where the _CLS is returned
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Executes the _CLS control method that returns PCI-defined
+ *              class code of the device. The _CLS value is always a package
+ *              containing PCI class information as a list of integers.
+ *              The returned string has format "BBSSPP", where:
+ *                BB = Base-class code
+ *                SS = Sub-class code
+ *                PP = Programming Interface code
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_ut_execute_CLS(struct acpi_namespace_node *device_node,
+		    struct acpi_pnp_device_id **return_id)
+{
+	union acpi_operand_object *obj_desc;
+	union acpi_operand_object **cls_objects;
+	u32 count;
+	struct acpi_pnp_device_id *cls;
+	u32 length;
+	acpi_status status;
+	u8 class_code[3] = { 0, 0, 0 };
+
+	ACPI_FUNCTION_TRACE(ut_execute_CLS);
+
+	status = acpi_ut_evaluate_object(device_node, METHOD_NAME__CLS,
+					 ACPI_BTYPE_PACKAGE, &obj_desc);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	/* Get the size of the String to be returned, includes null terminator */
+
+	length = ACPI_PCICLS_STRING_SIZE;
+	cls_objects = obj_desc->package.elements;
+	count = obj_desc->package.count;
+
+	if (obj_desc->common.type == ACPI_TYPE_PACKAGE) {
+		if (count > 0
+		    && cls_objects[0]->common.type == ACPI_TYPE_INTEGER) {
+			class_code[0] = (u8)cls_objects[0]->integer.value;
+		}
+		if (count > 1
+		    && cls_objects[1]->common.type == ACPI_TYPE_INTEGER) {
+			class_code[1] = (u8)cls_objects[1]->integer.value;
+		}
+		if (count > 2
+		    && cls_objects[2]->common.type == ACPI_TYPE_INTEGER) {
+			class_code[2] = (u8)cls_objects[2]->integer.value;
+		}
+	}
+
+	/* Allocate a buffer for the CLS */
+
+	cls =
+	    ACPI_ALLOCATE_ZEROED(sizeof(struct acpi_pnp_device_id) +
+				 (acpi_size) length);
+	if (!cls) {
+		status = AE_NO_MEMORY;
+		goto cleanup;
+	}
+
+	/* Area for the string starts after PNP_DEVICE_ID struct */
+
+	cls->string =
+	    ACPI_ADD_PTR(char, cls, sizeof(struct acpi_pnp_device_id));
+
+	/* Simply copy existing string */
+
+	acpi_ex_pci_cls_to_string(cls->string, class_code);
+	cls->length = length;
+	*return_id = cls;
+
+cleanup:
+
+	/* On exit, we must delete the return object */
+
+	acpi_ut_remove_reference(obj_desc);
+	return_ACPI_STATUS(status);
+}
diff --git a/drivers/acpi/acpica/utmisc.c b/drivers/acpi/acpica/utmisc.c
index cbb7034d28d8..71b66537f826 100644
--- a/drivers/acpi/acpica/utmisc.c
+++ b/drivers/acpi/acpica/utmisc.c
@@ -66,9 +66,9 @@ u8 acpi_ut_is_pci_root_bridge(char *id)
 	 * Check if this is a PCI root bridge.
 	 * ACPI 3.0+: check for a PCI Express root also.
 	 */
-	if (!(ACPI_STRCMP(id,
-			  PCI_ROOT_HID_STRING)) ||
-	    !(ACPI_STRCMP(id, PCI_EXPRESS_ROOT_HID_STRING))) {
+	if (!(strcmp(id,
+		     PCI_ROOT_HID_STRING)) ||
+	    !(strcmp(id, PCI_EXPRESS_ROOT_HID_STRING))) {
 		return (TRUE);
 	}
 
@@ -97,7 +97,8 @@ u8 acpi_ut_is_aml_table(struct acpi_table_header *table)
 
 	if (ACPI_COMPARE_NAME(table->signature, ACPI_SIG_DSDT) ||
 	    ACPI_COMPARE_NAME(table->signature, ACPI_SIG_PSDT) ||
-	    ACPI_COMPARE_NAME(table->signature, ACPI_SIG_SSDT)) {
+	    ACPI_COMPARE_NAME(table->signature, ACPI_SIG_SSDT) ||
+	    ACPI_COMPARE_NAME(table->signature, ACPI_SIG_OSDT)) {
 		return (TRUE);
 	}
 
diff --git a/drivers/acpi/acpica/utosi.c b/drivers/acpi/acpica/utosi.c
index 44035abdbf29..8f3d203aed79 100644
--- a/drivers/acpi/acpica/utosi.c
+++ b/drivers/acpi/acpica/utosi.c
@@ -232,8 +232,7 @@ acpi_status acpi_ut_install_interface(acpi_string interface_name)
 		return (AE_NO_MEMORY);
 	}
 
-	interface_info->name =
-	    ACPI_ALLOCATE_ZEROED(ACPI_STRLEN(interface_name) + 1);
+	interface_info->name = ACPI_ALLOCATE_ZEROED(strlen(interface_name) + 1);
 	if (!interface_info->name) {
 		ACPI_FREE(interface_info);
 		return (AE_NO_MEMORY);
@@ -241,7 +240,7 @@ acpi_status acpi_ut_install_interface(acpi_string interface_name)
 
 	/* Initialize new info and insert at the head of the global list */
 
-	ACPI_STRCPY(interface_info->name, interface_name);
+	strcpy(interface_info->name, interface_name);
 	interface_info->flags = ACPI_OSI_DYNAMIC;
 	interface_info->next = acpi_gbl_supported_interfaces;
 
@@ -269,7 +268,7 @@ acpi_status acpi_ut_remove_interface(acpi_string interface_name)
 
 	previous_interface = next_interface = acpi_gbl_supported_interfaces;
 	while (next_interface) {
-		if (!ACPI_STRCMP(interface_name, next_interface->name)) {
+		if (!strcmp(interface_name, next_interface->name)) {
 
 			/* Found: name is in either the static list or was added at runtime */
 
@@ -373,7 +372,7 @@ struct acpi_interface_info *acpi_ut_get_interface(acpi_string interface_name)
 
 	next_interface = acpi_gbl_supported_interfaces;
 	while (next_interface) {
-		if (!ACPI_STRCMP(interface_name, next_interface->name)) {
+		if (!strcmp(interface_name, next_interface->name)) {
 			return (next_interface);
 		}
 
diff --git a/drivers/acpi/acpica/utpredef.c b/drivers/acpi/acpica/utpredef.c
index 29e449935a82..97898ed71b4b 100644
--- a/drivers/acpi/acpica/utpredef.c
+++ b/drivers/acpi/acpica/utpredef.c
@@ -148,7 +148,7 @@ void acpi_ut_get_expected_return_types(char *buffer, u32 expected_btypes)
 	u32 j;
 
 	if (!expected_btypes) {
-		ACPI_STRCPY(buffer, "NONE");
+		strcpy(buffer, "NONE");
 		return;
 	}
 
@@ -161,7 +161,7 @@ void acpi_ut_get_expected_return_types(char *buffer, u32 expected_btypes)
 		/* If one of the expected types, concatenate the name of this type */
 
 		if (expected_btypes & this_rtype) {
-			ACPI_STRCAT(buffer, &ut_rtype_names[i][j]);
+			strcat(buffer, &ut_rtype_names[i][j]);
 			j = 0;	/* Use name separator from now on */
 		}
 
diff --git a/drivers/acpi/acpica/utprint.c b/drivers/acpi/acpica/utprint.c
index 2be6bd4bdc09..b26297c5de49 100644
--- a/drivers/acpi/acpica/utprint.c
+++ b/drivers/acpi/acpica/utprint.c
@@ -180,7 +180,7 @@ const char *acpi_ut_scan_number(const char *string, u64 *number_ptr)
 {
 	u64 number = 0;
 
-	while (ACPI_IS_DIGIT(*string)) {
+	while (isdigit((int)*string)) {
 		number *= 10;
 		number += *(string++) - '0';
 	}
@@ -405,7 +405,7 @@ acpi_ut_vsnprintf(char *string,
 		/* Process width */
 
 		width = -1;
-		if (ACPI_IS_DIGIT(*format)) {
+		if (isdigit((int)*format)) {
 			format = acpi_ut_scan_number(format, &number);
 			width = (s32) number;
 		} else if (*format == '*') {
@@ -422,7 +422,7 @@ acpi_ut_vsnprintf(char *string,
 		precision = -1;
 		if (*format == '.') {
 			++format;
-			if (ACPI_IS_DIGIT(*format)) {
+			if (isdigit((int)*format)) {
 				format = acpi_ut_scan_number(format, &number);
 				precision = (s32) number;
 			} else if (*format == '*') {
diff --git a/drivers/acpi/acpica/utstring.c b/drivers/acpi/acpica/utstring.c
index 83b6c52490dc..8f3c883dfe0e 100644
--- a/drivers/acpi/acpica/utstring.c
+++ b/drivers/acpi/acpica/utstring.c
@@ -79,7 +79,7 @@ void acpi_ut_strlwr(char *src_string)
 	/* Walk entire string, lowercasing the letters */
 
 	for (string = src_string; *string; string++) {
-		*string = (char)ACPI_TOLOWER(*string);
+		*string = (char)tolower((int)*string);
 	}
 
 	return;
@@ -145,7 +145,7 @@ void acpi_ut_strupr(char *src_string)
 	/* Walk entire string, uppercasing the letters */
 
 	for (string = src_string; *string; string++) {
-		*string = (char)ACPI_TOUPPER(*string);
+		*string = (char)toupper((int)*string);
 	}
 
 	return;
@@ -202,7 +202,7 @@ acpi_status acpi_ut_strtoul64(char *string, u32 base, u64 *ret_integer)
 
 	/* Skip over any white space in the buffer */
 
-	while ((*string) && (ACPI_IS_SPACE(*string) || *string == '\t')) {
+	while ((*string) && (isspace((int)*string) || *string == '\t')) {
 		string++;
 	}
 
@@ -211,7 +211,7 @@ acpi_status acpi_ut_strtoul64(char *string, u32 base, u64 *ret_integer)
 		 * Base equal to ACPI_ANY_BASE means 'ToInteger operation case'.
 		 * We need to determine if it is decimal or hexadecimal.
 		 */
-		if ((*string == '0') && (ACPI_TOLOWER(*(string + 1)) == 'x')) {
+		if ((*string == '0') && (tolower((int)*(string + 1)) == 'x')) {
 			sign_of0x = 1;
 			base = 16;
 
@@ -224,7 +224,7 @@ acpi_status acpi_ut_strtoul64(char *string, u32 base, u64 *ret_integer)
 
 	/* Any string left? Check that '0x' is not followed by white space. */
 
-	if (!(*string) || ACPI_IS_SPACE(*string) || *string == '\t') {
+	if (!(*string) || isspace((int)*string) || *string == '\t') {
 		if (to_integer_op) {
 			goto error_exit;
 		} else {
@@ -241,7 +241,7 @@ acpi_status acpi_ut_strtoul64(char *string, u32 base, u64 *ret_integer)
 	/* Main loop: convert the string to a 32- or 64-bit integer */
 
 	while (*string) {
-		if (ACPI_IS_DIGIT(*string)) {
+		if (isdigit((int)*string)) {
 
 			/* Convert ASCII 0-9 to Decimal value */
 
@@ -252,8 +252,8 @@ acpi_status acpi_ut_strtoul64(char *string, u32 base, u64 *ret_integer)
 
 			term = 1;
 		} else {
-			this_digit = (u8)ACPI_TOUPPER(*string);
-			if (ACPI_IS_XDIGIT((char)this_digit)) {
+			this_digit = (u8)toupper((int)*string);
+			if (isxdigit((int)this_digit)) {
 
 				/* Convert ASCII Hex char to value */
 
@@ -404,7 +404,7 @@ void acpi_ut_print_string(char *string, u16 max_length)
 
 			/* Check for printable character or hex escape */
 
-			if (ACPI_IS_PRINT(string[i])) {
+			if (isprint((int)string[i])) {
 				/* This is a normal character */
 
 				acpi_os_printf("%c", (int)string[i]);
@@ -609,22 +609,22 @@ void ut_convert_backslashes(char *pathname)
 u8 acpi_ut_safe_strcpy(char *dest, acpi_size dest_size, char *source)
 {
 
-	if (ACPI_STRLEN(source) >= dest_size) {
+	if (strlen(source) >= dest_size) {
 		return (TRUE);
 	}
 
-	ACPI_STRCPY(dest, source);
+	strcpy(dest, source);
 	return (FALSE);
 }
 
 u8 acpi_ut_safe_strcat(char *dest, acpi_size dest_size, char *source)
 {
 
-	if ((ACPI_STRLEN(dest) + ACPI_STRLEN(source)) >= dest_size) {
+	if ((strlen(dest) + strlen(source)) >= dest_size) {
 		return (TRUE);
 	}
 
-	ACPI_STRCAT(dest, source);
+	strcat(dest, source);
 	return (FALSE);
 }
 
@@ -635,14 +635,13 @@ acpi_ut_safe_strncat(char *dest,
 {
 	acpi_size actual_transfer_length;
 
-	actual_transfer_length =
-	    ACPI_MIN(max_transfer_length, ACPI_STRLEN(source));
+	actual_transfer_length = ACPI_MIN(max_transfer_length, strlen(source));
 
-	if ((ACPI_STRLEN(dest) + actual_transfer_length) >= dest_size) {
+	if ((strlen(dest) + actual_transfer_length) >= dest_size) {
 		return (TRUE);
 	}
 
-	ACPI_STRNCAT(dest, source, max_transfer_length);
+	strncat(dest, source, max_transfer_length);
 	return (FALSE);
 }
 #endif
diff --git a/drivers/acpi/acpica/uttrack.c b/drivers/acpi/acpica/uttrack.c
index 130dd9f96f0f..9a7dc8196a5d 100644
--- a/drivers/acpi/acpica/uttrack.c
+++ b/drivers/acpi/acpica/uttrack.c
@@ -100,7 +100,7 @@ acpi_ut_create_list(char *list_name,
 		return (AE_NO_MEMORY);
 	}
 
-	ACPI_MEMSET(cache, 0, sizeof(struct acpi_memory_list));
+	memset(cache, 0, sizeof(struct acpi_memory_list));
 
 	cache->list_name = list_name;
 	cache->object_size = object_size;
@@ -402,7 +402,7 @@ acpi_ut_track_allocation(struct acpi_debug_mem_block *allocation,
 	allocation->component = component;
 	allocation->line = line;
 
-	ACPI_STRNCPY(allocation->module, module, ACPI_MAX_MODULE_NAME);
+	strncpy(allocation->module, module, ACPI_MAX_MODULE_NAME);
 	allocation->module[ACPI_MAX_MODULE_NAME - 1] = 0;
 
 	if (!element) {
@@ -497,7 +497,7 @@ acpi_ut_remove_allocation(struct acpi_debug_mem_block *allocation,
 
 	/* Mark the segment as deleted */
 
-	ACPI_MEMSET(&allocation->user_space, 0xEA, allocation->size);
+	memset(&allocation->user_space, 0xEA, allocation->size);
 
 	status = acpi_ut_release_mutex(ACPI_MTX_MEMORY);
 	return (status);
@@ -595,7 +595,7 @@ void acpi_ut_dump_allocations(u32 component, const char *module)
 	while (element) {
 		if ((element->component & component) &&
 		    ((module == NULL)
-		     || (0 == ACPI_STRCMP(module, element->module)))) {
+		     || (0 == strcmp(module, element->module)))) {
 			descriptor =
 			    ACPI_CAST_PTR(union acpi_descriptor,
 					  &element->user_space);
diff --git a/drivers/acpi/acpica/utxface.c b/drivers/acpi/acpica/utxface.c
index 0929187bdce0..51cf52d52243 100644
--- a/drivers/acpi/acpica/utxface.c
+++ b/drivers/acpi/acpica/utxface.c
@@ -234,8 +234,8 @@ acpi_status acpi_get_statistics(struct acpi_statistics *stats)
 	stats->sci_count = acpi_sci_count;
 	stats->gpe_count = acpi_gpe_count;
 
-	ACPI_MEMCPY(stats->fixed_event_count, acpi_fixed_event_count,
-		    sizeof(acpi_fixed_event_count));
+	memcpy(stats->fixed_event_count, acpi_fixed_event_count,
+	       sizeof(acpi_fixed_event_count));
 
 	/* Other counters */
 
@@ -322,7 +322,7 @@ acpi_status acpi_install_interface(acpi_string interface_name)
 
 	/* Parameter validation */
 
-	if (!interface_name || (ACPI_STRLEN(interface_name) == 0)) {
+	if (!interface_name || (strlen(interface_name) == 0)) {
 		return (AE_BAD_PARAMETER);
 	}
 
@@ -374,7 +374,7 @@ acpi_status acpi_remove_interface(acpi_string interface_name)
 
 	/* Parameter validation */
 
-	if (!interface_name || (ACPI_STRLEN(interface_name) == 0)) {
+	if (!interface_name || (strlen(interface_name) == 0)) {
 		return (AE_BAD_PARAMETER);
 	}
 
diff --git a/drivers/acpi/acpica/utxferror.c b/drivers/acpi/acpica/utxferror.c
index 306e785f9418..98d578753101 100644
--- a/drivers/acpi/acpica/utxferror.c
+++ b/drivers/acpi/acpica/utxferror.c
@@ -107,9 +107,16 @@ acpi_exception(const char *module_name,
 	va_list arg_list;
 
 	ACPI_MSG_REDIRECT_BEGIN;
-	acpi_os_printf(ACPI_MSG_EXCEPTION "%s, ",
-		       acpi_format_exception(status));
 
+	/* For AE_OK, just print the message */
+
+	if (ACPI_SUCCESS(status)) {
+		acpi_os_printf(ACPI_MSG_EXCEPTION);
+
+	} else {
+		acpi_os_printf(ACPI_MSG_EXCEPTION "%s, ",
+			       acpi_format_exception(status));
+	}
 	va_start(arg_list, format);
 	acpi_os_vprintf(format, arg_list);
 	ACPI_MSG_SUFFIX;
diff --git a/drivers/acpi/acpica/utxfinit.c b/drivers/acpi/acpica/utxfinit.c
index 083a76891889..42a32a66ef22 100644
--- a/drivers/acpi/acpica/utxfinit.c
+++ b/drivers/acpi/acpica/utxfinit.c
@@ -179,10 +179,12 @@ acpi_status __init acpi_enable_subsystem(u32 flags)
 	 * Obtain a permanent mapping for the FACS. This is required for the
 	 * Global Lock and the Firmware Waking Vector
 	 */
-	status = acpi_tb_initialize_facs();
-	if (ACPI_FAILURE(status)) {
-		ACPI_WARNING((AE_INFO, "Could not map the FACS table"));
-		return_ACPI_STATUS(status);
+	if (!(flags & ACPI_NO_FACS_INIT)) {
+		status = acpi_tb_initialize_facs();
+		if (ACPI_FAILURE(status)) {
+			ACPI_WARNING((AE_INFO, "Could not map the FACS table"));
+			return_ACPI_STATUS(status);
+		}
 	}
 #endif				/* !ACPI_REDUCED_HARDWARE */
 
diff --git a/drivers/acpi/apei/erst.c b/drivers/acpi/apei/erst.c
index ed65e9c4b5b0..3670bbab57a3 100644
--- a/drivers/acpi/apei/erst.c
+++ b/drivers/acpi/apei/erst.c
@@ -35,6 +35,7 @@
 #include <linux/nmi.h>
 #include <linux/hardirq.h>
 #include <linux/pstore.h>
+#include <linux/vmalloc.h>
 #include <acpi/apei.h>
 
 #include "apei-internal.h"
diff --git a/drivers/acpi/apei/ghes.c b/drivers/acpi/apei/ghes.c
index e82d0976a5d0..2bfd53cbfe80 100644
--- a/drivers/acpi/apei/ghes.c
+++ b/drivers/acpi/apei/ghes.c
@@ -729,10 +729,10 @@ static struct llist_head ghes_estatus_llist;
 static struct irq_work ghes_proc_irq_work;
 
 /*
- * NMI may be triggered on any CPU, so ghes_nmi_lock is used for
- * mutual exclusion.
+ * NMI may be triggered on any CPU, so ghes_in_nmi is used for
+ * having only one concurrent reader.
  */
-static DEFINE_RAW_SPINLOCK(ghes_nmi_lock);
+static atomic_t ghes_in_nmi = ATOMIC_INIT(0);
 
 static LIST_HEAD(ghes_nmi);
 
@@ -797,73 +797,75 @@ static void ghes_print_queued_estatus(void)
 	}
 }
 
+/* Save estatus for further processing in IRQ context */
+static void __process_error(struct ghes *ghes)
+{
+#ifdef CONFIG_ARCH_HAVE_NMI_SAFE_CMPXCHG
+	u32 len, node_len;
+	struct ghes_estatus_node *estatus_node;
+	struct acpi_hest_generic_status *estatus;
+
+	if (ghes_estatus_cached(ghes->estatus))
+		return;
+
+	len = cper_estatus_len(ghes->estatus);
+	node_len = GHES_ESTATUS_NODE_LEN(len);
+
+	estatus_node = (void *)gen_pool_alloc(ghes_estatus_pool, node_len);
+	if (!estatus_node)
+		return;
+
+	estatus_node->ghes = ghes;
+	estatus_node->generic = ghes->generic;
+	estatus = GHES_ESTATUS_FROM_NODE(estatus_node);
+	memcpy(estatus, ghes->estatus, len);
+	llist_add(&estatus_node->llnode, &ghes_estatus_llist);
+#endif
+}
+
+static void __ghes_panic(struct ghes *ghes)
+{
+	oops_begin();
+	ghes_print_queued_estatus();
+	__ghes_print_estatus(KERN_EMERG, ghes->generic, ghes->estatus);
+
+	/* reboot to log the error! */
+	if (panic_timeout == 0)
+		panic_timeout = ghes_panic_timeout;
+	panic("Fatal hardware error!");
+}
+
 static int ghes_notify_nmi(unsigned int cmd, struct pt_regs *regs)
 {
-	struct ghes *ghes, *ghes_global = NULL;
-	int sev, sev_global = -1;
-	int ret = NMI_DONE;
+	struct ghes *ghes;
+	int sev, ret = NMI_DONE;
+
+	if (!atomic_add_unless(&ghes_in_nmi, 1, 1))
+		return ret;
 
-	raw_spin_lock(&ghes_nmi_lock);
 	list_for_each_entry_rcu(ghes, &ghes_nmi, list) {
 		if (ghes_read_estatus(ghes, 1)) {
 			ghes_clear_estatus(ghes);
 			continue;
 		}
-		sev = ghes_severity(ghes->estatus->error_severity);
-		if (sev > sev_global) {
-			sev_global = sev;
-			ghes_global = ghes;
-		}
-		ret = NMI_HANDLED;
-	}
-
-	if (ret == NMI_DONE)
-		goto out;
 
-	if (sev_global >= GHES_SEV_PANIC) {
-		oops_begin();
-		ghes_print_queued_estatus();
-		__ghes_print_estatus(KERN_EMERG, ghes_global->generic,
-				     ghes_global->estatus);
-		/* reboot to log the error! */
-		if (panic_timeout == 0)
-			panic_timeout = ghes_panic_timeout;
-		panic("Fatal hardware error!");
-	}
+		sev = ghes_severity(ghes->estatus->error_severity);
+		if (sev >= GHES_SEV_PANIC)
+			__ghes_panic(ghes);
 
-	list_for_each_entry_rcu(ghes, &ghes_nmi, list) {
-#ifdef CONFIG_ARCH_HAVE_NMI_SAFE_CMPXCHG
-		u32 len, node_len;
-		struct ghes_estatus_node *estatus_node;
-		struct acpi_hest_generic_status *estatus;
-#endif
 		if (!(ghes->flags & GHES_TO_CLEAR))
 			continue;
-#ifdef CONFIG_ARCH_HAVE_NMI_SAFE_CMPXCHG
-		if (ghes_estatus_cached(ghes->estatus))
-			goto next;
-		/* Save estatus for further processing in IRQ context */
-		len = cper_estatus_len(ghes->estatus);
-		node_len = GHES_ESTATUS_NODE_LEN(len);
-		estatus_node = (void *)gen_pool_alloc(ghes_estatus_pool,
-						      node_len);
-		if (estatus_node) {
-			estatus_node->ghes = ghes;
-			estatus_node->generic = ghes->generic;
-			estatus = GHES_ESTATUS_FROM_NODE(estatus_node);
-			memcpy(estatus, ghes->estatus, len);
-			llist_add(&estatus_node->llnode, &ghes_estatus_llist);
-		}
-next:
-#endif
+
+		__process_error(ghes);
 		ghes_clear_estatus(ghes);
+
+		ret = NMI_HANDLED;
 	}
+
 #ifdef CONFIG_ARCH_HAVE_NMI_SAFE_CMPXCHG
 	irq_work_queue(&ghes_proc_irq_work);
 #endif
-
-out:
-	raw_spin_unlock(&ghes_nmi_lock);
+	atomic_dec(&ghes_in_nmi);
 	return ret;
 }
 
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index 63d43677f644..b3628cc01a53 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -70,6 +70,7 @@ MODULE_AUTHOR("Alexey Starikovskiy <astarikovskiy@suse.de>");
 MODULE_DESCRIPTION("ACPI Battery Driver");
 MODULE_LICENSE("GPL");
 
+static async_cookie_t async_cookie;
 static int battery_bix_broken_package;
 static int battery_notification_delay_ms;
 static unsigned int cache_time = 1000;
@@ -338,14 +339,6 @@ static enum power_supply_property energy_battery_props[] = {
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
 };
 
-#ifdef CONFIG_ACPI_PROCFS_POWER
-inline char *acpi_battery_units(struct acpi_battery *battery)
-{
-	return (battery->power_unit == ACPI_BATTERY_POWER_UNIT_MA) ?
-		"mA" : "mW";
-}
-#endif
-
 /* --------------------------------------------------------------------------
                                Battery Management
    -------------------------------------------------------------------------- */
@@ -354,14 +347,14 @@ struct acpi_offsets {
 	u8 mode;		/* int or string? */
 };
 
-static struct acpi_offsets state_offsets[] = {
+static const struct acpi_offsets state_offsets[] = {
 	{offsetof(struct acpi_battery, state), 0},
 	{offsetof(struct acpi_battery, rate_now), 0},
 	{offsetof(struct acpi_battery, capacity_now), 0},
 	{offsetof(struct acpi_battery, voltage_now), 0},
 };
 
-static struct acpi_offsets info_offsets[] = {
+static const struct acpi_offsets info_offsets[] = {
 	{offsetof(struct acpi_battery, power_unit), 0},
 	{offsetof(struct acpi_battery, design_capacity), 0},
 	{offsetof(struct acpi_battery, full_charge_capacity), 0},
@@ -377,7 +370,7 @@ static struct acpi_offsets info_offsets[] = {
 	{offsetof(struct acpi_battery, oem_info), 1},
 };
 
-static struct acpi_offsets extended_info_offsets[] = {
+static const struct acpi_offsets extended_info_offsets[] = {
 	{offsetof(struct acpi_battery, revision), 0},
 	{offsetof(struct acpi_battery, power_unit), 0},
 	{offsetof(struct acpi_battery, design_capacity), 0},
@@ -402,7 +395,7 @@ static struct acpi_offsets extended_info_offsets[] = {
 
 static int extract_package(struct acpi_battery *battery,
 			   union acpi_object *package,
-			   struct acpi_offsets *offsets, int num)
+			   const struct acpi_offsets *offsets, int num)
 {
 	int i;
 	union acpi_object *element;
@@ -792,6 +785,12 @@ static void acpi_battery_refresh(struct acpi_battery *battery)
 #ifdef CONFIG_ACPI_PROCFS_POWER
 static struct proc_dir_entry *acpi_battery_dir;
 
+static const char *acpi_battery_units(const struct acpi_battery *battery)
+{
+	return (battery->power_unit == ACPI_BATTERY_POWER_UNIT_MA) ?
+		"mA" : "mW";
+}
+
 static int acpi_battery_print_info(struct seq_file *seq, int result)
 {
 	struct acpi_battery *battery = seq->private;
@@ -1125,19 +1124,21 @@ static int battery_notify(struct notifier_block *nb,
 	return 0;
 }
 
-static int battery_bix_broken_package_quirk(const struct dmi_system_id *d)
+static int __init
+battery_bix_broken_package_quirk(const struct dmi_system_id *d)
 {
 	battery_bix_broken_package = 1;
 	return 0;
 }
 
-static int battery_notification_delay_quirk(const struct dmi_system_id *d)
+static int __init
+battery_notification_delay_quirk(const struct dmi_system_id *d)
 {
 	battery_notification_delay_ms = 1000;
 	return 0;
 }
 
-static struct dmi_system_id bat_dmi_table[] = {
+static const struct dmi_system_id bat_dmi_table[] __initconst = {
 	{
 		.callback = battery_bix_broken_package_quirk,
 		.ident = "NEC LZ750/LS",
@@ -1292,33 +1293,34 @@ static struct acpi_driver acpi_battery_driver = {
 
 static void __init acpi_battery_init_async(void *unused, async_cookie_t cookie)
 {
-	if (acpi_disabled)
-		return;
+	int result;
 
 	dmi_check_system(bat_dmi_table);
-	
+
 #ifdef CONFIG_ACPI_PROCFS_POWER
 	acpi_battery_dir = acpi_lock_battery_dir();
 	if (!acpi_battery_dir)
 		return;
 #endif
-	if (acpi_bus_register_driver(&acpi_battery_driver) < 0) {
+	result = acpi_bus_register_driver(&acpi_battery_driver);
 #ifdef CONFIG_ACPI_PROCFS_POWER
+	if (result < 0)
 		acpi_unlock_battery_dir(acpi_battery_dir);
 #endif
-		return;
-	}
-	return;
 }
 
 static int __init acpi_battery_init(void)
 {
-	async_schedule(acpi_battery_init_async, NULL);
+	if (acpi_disabled)
+		return -ENODEV;
+
+	async_cookie = async_schedule(acpi_battery_init_async, NULL);
 	return 0;
 }
 
 static void __exit acpi_battery_exit(void)
 {
+	async_synchronize_cookie(async_cookie);
 	acpi_bus_unregister_driver(&acpi_battery_driver);
 #ifdef CONFIG_ACPI_PROCFS_POWER
 	acpi_unlock_battery_dir(acpi_battery_dir);
diff --git a/drivers/acpi/blacklist.c b/drivers/acpi/blacklist.c
index 1d1791935c31..278dc4be992a 100644
--- a/drivers/acpi/blacklist.c
+++ b/drivers/acpi/blacklist.c
@@ -162,6 +162,15 @@ static int __init dmi_disable_osi_win8(const struct dmi_system_id *d)
 	acpi_osi_setup("!Windows 2012");
 	return 0;
 }
+#ifdef CONFIG_ACPI_REV_OVERRIDE_POSSIBLE
+static int __init dmi_enable_rev_override(const struct dmi_system_id *d)
+{
+	printk(KERN_NOTICE PREFIX "DMI detected: %s (force ACPI _REV to 5)\n",
+	       d->ident);
+	acpi_rev_override_setup(NULL);
+	return 0;
+}
+#endif
 
 static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 	{
@@ -325,6 +334,23 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 		     DMI_MATCH(DMI_PRODUCT_NAME, "1015PX"),
 		},
 	},
+
+#ifdef CONFIG_ACPI_REV_OVERRIDE_POSSIBLE
+	/*
+	 * DELL XPS 13 (2015) switches sound between HDA and I2S
+	 * depending on the ACPI _REV callback. If userspace supports
+	 * I2S sufficiently (or if you do not care about sound), you
+	 * can safely disable this quirk.
+	 */
+	{
+	 .callback = dmi_enable_rev_override,
+	 .ident = "DELL XPS 13 (2015)",
+	 .matches = {
+		      DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		      DMI_MATCH(DMI_PRODUCT_NAME, "XPS 13 9343"),
+		},
+	},
+#endif
 	{}
 };
 
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index c412fdb28d34..513e7230e3d0 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -470,6 +470,16 @@ static int __init acpi_bus_init_irq(void)
 	return 0;
 }
 
+/**
+ * acpi_early_init - Initialize ACPICA and populate the ACPI namespace.
+ *
+ * The ACPI tables are accessible after this, but the handling of events has not
+ * been initialized and the global lock is not available yet, so AML should not
+ * be executed at this point.
+ *
+ * Doing this before switching the EFI runtime services to virtual mode allows
+ * the EfiBootServices memory to be freed slightly earlier on boot.
+ */
 void __init acpi_early_init(void)
 {
 	acpi_status status;
@@ -533,26 +543,42 @@ void __init acpi_early_init(void)
 		acpi_gbl_FADT.sci_interrupt = acpi_sci_override_gsi;
 	}
 #endif
+	return;
+
+ error0:
+	disable_acpi();
+}
+
+/**
+ * acpi_subsystem_init - Finalize the early initialization of ACPI.
+ *
+ * Switch over the platform to the ACPI mode (if possible), initialize the
+ * handling of ACPI events, install the interrupt and global lock handlers.
+ *
+ * Doing this too early is generally unsafe, but at the same time it needs to be
+ * done before all things that really depend on ACPI.  The right spot appears to
+ * be before finalizing the EFI initialization.
+ */
+void __init acpi_subsystem_init(void)
+{
+	acpi_status status;
+
+	if (acpi_disabled)
+		return;
 
 	status = acpi_enable_subsystem(~ACPI_NO_ACPI_ENABLE);
 	if (ACPI_FAILURE(status)) {
 		printk(KERN_ERR PREFIX "Unable to enable ACPI\n");
-		goto error0;
+		disable_acpi();
+	} else {
+		/*
+		 * If the system is using ACPI then we can be reasonably
+		 * confident that any regulators are managed by the firmware
+		 * so tell the regulator core it has everything it needs to
+		 * know.
+		 */
+		regulator_has_full_constraints();
 	}
-
-	/*
-	 * If the system is using ACPI then we can be reasonably
-	 * confident that any regulators are managed by the firmware
-	 * so tell the regulator core it has everything it needs to
-	 * know.
-	 */
-	regulator_has_full_constraints();
-
-	return;
-
-      error0:
-	disable_acpi();
-	return;
 }
 
 static int __init acpi_bus_init(void)
diff --git a/drivers/acpi/device_pm.c b/drivers/acpi/device_pm.c
index 735db11a9b00..717afcdb5f4a 100644
--- a/drivers/acpi/device_pm.c
+++ b/drivers/acpi/device_pm.c
@@ -98,17 +98,16 @@ int acpi_device_get_power(struct acpi_device *device, int *state)
 
 		/*
 		 * The power resources settings may indicate a power state
-		 * shallower than the actual power state of the device.
+		 * shallower than the actual power state of the device, because
+		 * the same power resources may be referenced by other devices.
 		 *
-		 * Moreover, on systems predating ACPI 4.0, if the device
-		 * doesn't depend on any power resources and _PSC returns 3,
-		 * that means "power off".  We need to maintain compatibility
-		 * with those systems.
+		 * For systems predating ACPI 4.0 we assume that D3hot is the
+		 * deepest state that can be supported.
 		 */
 		if (psc > result && psc < ACPI_STATE_D3_COLD)
 			result = psc;
 		else if (result == ACPI_STATE_UNKNOWN)
-			result = psc > ACPI_STATE_D2 ? ACPI_STATE_D3_COLD : psc;
+			result = psc > ACPI_STATE_D2 ? ACPI_STATE_D3_HOT : psc;
 	}
 
 	/*
@@ -153,8 +152,8 @@ static int acpi_dev_pm_explicit_set(struct acpi_device *adev, int state)
  */
 int acpi_device_set_power(struct acpi_device *device, int state)
 {
+	int target_state = state;
 	int result = 0;
-	bool cut_power = false;
 
 	if (!device || !device->flags.power_manageable
 	    || (state < ACPI_STATE_D0) || (state > ACPI_STATE_D3_COLD))
@@ -169,11 +168,21 @@ int acpi_device_set_power(struct acpi_device *device, int state)
 		return 0;
 	}
 
-	if (!device->power.states[state].flags.valid) {
+	if (state == ACPI_STATE_D3_COLD) {
+		/*
+		 * For transitions to D3cold we need to execute _PS3 and then
+		 * possibly drop references to the power resources in use.
+		 */
+		state = ACPI_STATE_D3_HOT;
+		/* If _PR3 is not available, use D3hot as the target state. */
+		if (!device->power.states[ACPI_STATE_D3_COLD].flags.valid)
+			target_state = state;
+	} else if (!device->power.states[state].flags.valid) {
 		dev_warn(&device->dev, "Power state %s not supported\n",
 			 acpi_power_state_string(state));
 		return -ENODEV;
 	}
+
 	if (!device->power.flags.ignore_parent &&
 	    device->parent && (state < device->parent->power.state)) {
 		dev_warn(&device->dev,
@@ -183,39 +192,38 @@ int acpi_device_set_power(struct acpi_device *device, int state)
 		return -ENODEV;
 	}
 
-	/* For D3cold we should first transition into D3hot. */
-	if (state == ACPI_STATE_D3_COLD
-	    && device->power.states[ACPI_STATE_D3_COLD].flags.os_accessible) {
-		state = ACPI_STATE_D3_HOT;
-		cut_power = true;
-	}
-
-	if (state < device->power.state && state != ACPI_STATE_D0
-	    && device->power.state >= ACPI_STATE_D3_HOT) {
-		dev_warn(&device->dev,
-			 "Cannot transition to non-D0 state from D3\n");
-		return -ENODEV;
-	}
-
 	/*
 	 * Transition Power
 	 * ----------------
-	 * In accordance with the ACPI specification first apply power (via
-	 * power resources) and then evaluate _PSx.
+	 * In accordance with ACPI 6, _PSx is executed before manipulating power
+	 * resources, unless the target state is D0, in which case _PS0 is
+	 * supposed to be executed after turning the power resources on.
 	 */
-	if (device->power.flags.power_resources) {
-		result = acpi_power_transition(device, state);
+	if (state > ACPI_STATE_D0) {
+		/*
+		 * According to ACPI 6, devices cannot go from lower-power
+		 * (deeper) states to higher-power (shallower) states.
+		 */
+		if (state < device->power.state) {
+			dev_warn(&device->dev, "Cannot transition from %s to %s\n",
+				 acpi_power_state_string(device->power.state),
+				 acpi_power_state_string(state));
+			return -ENODEV;
+		}
+
+		result = acpi_dev_pm_explicit_set(device, state);
 		if (result)
 			goto end;
-	}
-	result = acpi_dev_pm_explicit_set(device, state);
-	if (result)
-		goto end;
 
-	if (cut_power) {
-		device->power.state = state;
-		state = ACPI_STATE_D3_COLD;
-		result = acpi_power_transition(device, state);
+		if (device->power.flags.power_resources)
+			result = acpi_power_transition(device, target_state);
+	} else {
+		if (device->power.flags.power_resources) {
+			result = acpi_power_transition(device, ACPI_STATE_D0);
+			if (result)
+				goto end;
+		}
+		result = acpi_dev_pm_explicit_set(device, ACPI_STATE_D0);
 	}
 
  end:
@@ -264,13 +272,24 @@ int acpi_bus_init_power(struct acpi_device *device)
 		return result;
 
 	if (state < ACPI_STATE_D3_COLD && device->power.flags.power_resources) {
+		/* Reference count the power resources. */
 		result = acpi_power_on_resources(device, state);
 		if (result)
 			return result;
 
-		result = acpi_dev_pm_explicit_set(device, state);
-		if (result)
-			return result;
+		if (state == ACPI_STATE_D0) {
+			/*
+			 * If _PSC is not present and the state inferred from
+			 * power resources appears to be D0, it still may be
+			 * necessary to execute _PS0 at this point, because
+			 * another device using the same power resources may
+			 * have been put into D0 previously and that's why we
+			 * see D0 here.
+			 */
+			result = acpi_dev_pm_explicit_set(device, state);
+			if (result)
+				return result;
+		}
 	} else if (state == ACPI_STATE_UNKNOWN) {
 		/*
 		 * No power resources and missing _PSC?  Cross fingers and make
@@ -603,12 +622,12 @@ int acpi_pm_device_sleep_state(struct device *dev, int *d_min_p, int d_max_in)
 	if (d_max_in < ACPI_STATE_D0 || d_max_in > ACPI_STATE_D3_COLD)
 		return -EINVAL;
 
-	if (d_max_in > ACPI_STATE_D3_HOT) {
+	if (d_max_in > ACPI_STATE_D2) {
 		enum pm_qos_flags_status stat;
 
 		stat = dev_pm_qos_flags(dev, PM_QOS_FLAG_NO_POWER_OFF);
 		if (stat == PM_QOS_FLAGS_ALL)
-			d_max_in = ACPI_STATE_D3_HOT;
+			d_max_in = ACPI_STATE_D2;
 	}
 
 	adev = ACPI_COMPANION(dev);
@@ -953,6 +972,7 @@ EXPORT_SYMBOL_GPL(acpi_subsys_prepare);
  */
 void acpi_subsys_complete(struct device *dev)
 {
+	pm_generic_complete(dev);
 	/*
 	 * If the device had been runtime-suspended before the system went into
 	 * the sleep state it is going out of and it has never been resumed till
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index 5e8fed448850..9d4761d2f6b7 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -59,6 +59,38 @@
 #define ACPI_EC_FLAG_BURST	0x10	/* burst mode */
 #define ACPI_EC_FLAG_SCI	0x20	/* EC-SCI occurred */
 
+/*
+ * The SCI_EVT clearing timing is not defined by the ACPI specification.
+ * This leads to lots of practical timing issues for the host EC driver.
+ * The following variations are defined (from the target EC firmware's
+ * perspective):
+ * STATUS: After indicating SCI_EVT edge triggered IRQ to the host, the
+ *         target can clear SCI_EVT at any time so long as the host can see
+ *         the indication by reading the status register (EC_SC). So the
+ *         host should re-check SCI_EVT after the first time the SCI_EVT
+ *         indication is seen, which is the same time the query request
+ *         (QR_EC) is written to the command register (EC_CMD). SCI_EVT set
+ *         at any later time could indicate another event. Normally such
+ *         kind of EC firmware has implemented an event queue and will
+ *         return 0x00 to indicate "no outstanding event".
+ * QUERY: After seeing the query request (QR_EC) written to the command
+ *        register (EC_CMD) by the host and having prepared the responding
+ *        event value in the data register (EC_DATA), the target can safely
+ *        clear SCI_EVT because the target can confirm that the current
+ *        event is being handled by the host. The host then should check
+ *        SCI_EVT right after reading the event response from the data
+ *        register (EC_DATA).
+ * EVENT: After seeing the event response read from the data register
+ *        (EC_DATA) by the host, the target can clear SCI_EVT. As the
+ *        target requires time to notice the change in the data register
+ *        (EC_DATA), the host may be required to wait additional guarding
+ *        time before checking the SCI_EVT again. Such guarding may not be
+ *        necessary if the host is notified via another IRQ.
+ */
+#define ACPI_EC_EVT_TIMING_STATUS	0x00
+#define ACPI_EC_EVT_TIMING_QUERY	0x01
+#define ACPI_EC_EVT_TIMING_EVENT	0x02
+
 /* EC commands */
 enum ec_command {
 	ACPI_EC_COMMAND_READ = 0x80,
@@ -70,13 +102,13 @@ enum ec_command {
 
 #define ACPI_EC_DELAY		500	/* Wait 500ms max. during EC ops */
 #define ACPI_EC_UDELAY_GLK	1000	/* Wait 1ms max. to get global lock */
-#define ACPI_EC_MSI_UDELAY	550	/* Wait 550us for MSI EC */
-#define ACPI_EC_UDELAY_POLL	1000	/* Wait 1ms for EC transaction polling */
+#define ACPI_EC_UDELAY_POLL	550	/* Wait 1ms for EC transaction polling */
 #define ACPI_EC_CLEAR_MAX	100	/* Maximum number of events to query
 					 * when trying to clear the EC */
 
 enum {
 	EC_FLAGS_QUERY_PENDING,		/* Query is pending */
+	EC_FLAGS_QUERY_GUARDING,	/* Guard for SCI_EVT check */
 	EC_FLAGS_HANDLERS_INSTALLED,	/* Handlers for GPE and
 					 * OpReg are installed */
 	EC_FLAGS_STARTED,		/* Driver is started */
@@ -93,6 +125,16 @@ static unsigned int ec_delay __read_mostly = ACPI_EC_DELAY;
 module_param(ec_delay, uint, 0644);
 MODULE_PARM_DESC(ec_delay, "Timeout(ms) waited until an EC command completes");
 
+static bool ec_busy_polling __read_mostly;
+module_param(ec_busy_polling, bool, 0644);
+MODULE_PARM_DESC(ec_busy_polling, "Use busy polling to advance EC transaction");
+
+static unsigned int ec_polling_guard __read_mostly = ACPI_EC_UDELAY_POLL;
+module_param(ec_polling_guard, uint, 0644);
+MODULE_PARM_DESC(ec_polling_guard, "Guard time(us) between EC accesses in polling modes");
+
+static unsigned int ec_event_clearing __read_mostly = ACPI_EC_EVT_TIMING_QUERY;
+
 /*
  * If the number of false interrupts per one transaction exceeds
  * this threshold, will think there is a GPE storm happened and
@@ -121,7 +163,6 @@ struct transaction {
 	u8 wlen;
 	u8 rlen;
 	u8 flags;
-	unsigned long timestamp;
 };
 
 static int acpi_ec_query(struct acpi_ec *ec, u8 *data);
@@ -130,7 +171,6 @@ static void advance_transaction(struct acpi_ec *ec);
 struct acpi_ec *boot_ec, *first_ec;
 EXPORT_SYMBOL(first_ec);
 
-static int EC_FLAGS_MSI; /* Out-of-spec MSI controller */
 static int EC_FLAGS_VALIDATE_ECDT; /* ASUStec ECDTs need to be validated */
 static int EC_FLAGS_SKIP_DSDT_SCAN; /* Not all BIOS survive early DSDT scan */
 static int EC_FLAGS_CLEAR_ON_RESUME; /* Needs acpi_ec_clear() on boot/resume */
@@ -218,7 +258,7 @@ static inline u8 acpi_ec_read_data(struct acpi_ec *ec)
 {
 	u8 x = inb(ec->data_addr);
 
-	ec->curr->timestamp = jiffies;
+	ec->timestamp = jiffies;
 	ec_dbg_raw("EC_DATA(R) = 0x%2.2x", x);
 	return x;
 }
@@ -227,14 +267,14 @@ static inline void acpi_ec_write_cmd(struct acpi_ec *ec, u8 command)
 {
 	ec_dbg_raw("EC_SC(W) = 0x%2.2x", command);
 	outb(command, ec->command_addr);
-	ec->curr->timestamp = jiffies;
+	ec->timestamp = jiffies;
 }
 
 static inline void acpi_ec_write_data(struct acpi_ec *ec, u8 data)
 {
 	ec_dbg_raw("EC_DATA(W) = 0x%2.2x", data);
 	outb(data, ec->data_addr);
-	ec->curr->timestamp = jiffies;
+	ec->timestamp = jiffies;
 }
 
 #ifdef DEBUG
@@ -267,7 +307,7 @@ static inline bool acpi_ec_is_gpe_raised(struct acpi_ec *ec)
 	acpi_event_status gpe_status = 0;
 
 	(void)acpi_get_gpe_status(NULL, ec->gpe, &gpe_status);
-	return (gpe_status & ACPI_EVENT_FLAG_SET) ? true : false;
+	return (gpe_status & ACPI_EVENT_FLAG_STATUS_SET) ? true : false;
 }
 
 static inline void acpi_ec_enable_gpe(struct acpi_ec *ec, bool open)
@@ -379,19 +419,49 @@ static bool acpi_ec_submit_flushable_request(struct acpi_ec *ec)
 static void acpi_ec_submit_query(struct acpi_ec *ec)
 {
 	if (!test_and_set_bit(EC_FLAGS_QUERY_PENDING, &ec->flags)) {
-		ec_dbg_req("Event started");
+		ec_dbg_evt("Command(%s) submitted/blocked",
+			   acpi_ec_cmd_string(ACPI_EC_COMMAND_QUERY));
+		ec->nr_pending_queries++;
 		schedule_work(&ec->work);
 	}
 }
 
 static void acpi_ec_complete_query(struct acpi_ec *ec)
 {
-	if (ec->curr->command == ACPI_EC_COMMAND_QUERY) {
+	if (test_bit(EC_FLAGS_QUERY_PENDING, &ec->flags)) {
 		clear_bit(EC_FLAGS_QUERY_PENDING, &ec->flags);
-		ec_dbg_req("Event stopped");
+		ec_dbg_evt("Command(%s) unblocked",
+			   acpi_ec_cmd_string(ACPI_EC_COMMAND_QUERY));
 	}
 }
 
+static bool acpi_ec_guard_event(struct acpi_ec *ec)
+{
+	if (ec_event_clearing == ACPI_EC_EVT_TIMING_STATUS ||
+	    ec_event_clearing == ACPI_EC_EVT_TIMING_QUERY ||
+	    !test_bit(EC_FLAGS_QUERY_PENDING, &ec->flags) ||
+	    (ec->curr && ec->curr->command == ACPI_EC_COMMAND_QUERY))
+		return false;
+
+	/*
+	 * Postpone the query submission to allow the firmware to proceed,
+	 * we shouldn't check SCI_EVT before the firmware reflagging it.
+	 */
+	return true;
+}
+
+static int ec_transaction_polled(struct acpi_ec *ec)
+{
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&ec->lock, flags);
+	if (ec->curr && (ec->curr->flags & ACPI_EC_COMMAND_POLL))
+		ret = 1;
+	spin_unlock_irqrestore(&ec->lock, flags);
+	return ret;
+}
+
 static int ec_transaction_completed(struct acpi_ec *ec)
 {
 	unsigned long flags;
@@ -404,6 +474,22 @@ static int ec_transaction_completed(struct acpi_ec *ec)
 	return ret;
 }
 
+static inline void ec_transaction_transition(struct acpi_ec *ec, unsigned long flag)
+{
+	ec->curr->flags |= flag;
+	if (ec->curr->command == ACPI_EC_COMMAND_QUERY) {
+		if (ec_event_clearing == ACPI_EC_EVT_TIMING_STATUS &&
+		    flag == ACPI_EC_COMMAND_POLL)
+			acpi_ec_complete_query(ec);
+		if (ec_event_clearing == ACPI_EC_EVT_TIMING_QUERY &&
+		    flag == ACPI_EC_COMMAND_COMPLETE)
+			acpi_ec_complete_query(ec);
+		if (ec_event_clearing == ACPI_EC_EVT_TIMING_EVENT &&
+		    flag == ACPI_EC_COMMAND_COMPLETE)
+			set_bit(EC_FLAGS_QUERY_GUARDING, &ec->flags);
+	}
+}
+
 static void advance_transaction(struct acpi_ec *ec)
 {
 	struct transaction *t;
@@ -420,6 +506,18 @@ static void advance_transaction(struct acpi_ec *ec)
 	acpi_ec_clear_gpe(ec);
 	status = acpi_ec_read_status(ec);
 	t = ec->curr;
+	/*
+	 * Another IRQ or a guarded polling mode advancement is detected,
+	 * the next QR_EC submission is then allowed.
+	 */
+	if (!t || !(t->flags & ACPI_EC_COMMAND_POLL)) {
+		if (ec_event_clearing == ACPI_EC_EVT_TIMING_EVENT &&
+		    (!ec->nr_pending_queries ||
+		     test_bit(EC_FLAGS_QUERY_GUARDING, &ec->flags))) {
+			clear_bit(EC_FLAGS_QUERY_GUARDING, &ec->flags);
+			acpi_ec_complete_query(ec);
+		}
+	}
 	if (!t)
 		goto err;
 	if (t->flags & ACPI_EC_COMMAND_POLL) {
@@ -432,17 +530,17 @@ static void advance_transaction(struct acpi_ec *ec)
 			if ((status & ACPI_EC_FLAG_OBF) == 1) {
 				t->rdata[t->ri++] = acpi_ec_read_data(ec);
 				if (t->rlen == t->ri) {
-					t->flags |= ACPI_EC_COMMAND_COMPLETE;
+					ec_transaction_transition(ec, ACPI_EC_COMMAND_COMPLETE);
 					if (t->command == ACPI_EC_COMMAND_QUERY)
-						ec_dbg_req("Command(%s) hardware completion",
-							   acpi_ec_cmd_string(t->command));
+						ec_dbg_evt("Command(%s) completed by hardware",
+							   acpi_ec_cmd_string(ACPI_EC_COMMAND_QUERY));
 					wakeup = true;
 				}
 			} else
 				goto err;
 		} else if (t->wlen == t->wi &&
 			   (status & ACPI_EC_FLAG_IBF) == 0) {
-			t->flags |= ACPI_EC_COMMAND_COMPLETE;
+			ec_transaction_transition(ec, ACPI_EC_COMMAND_COMPLETE);
 			wakeup = true;
 		}
 		goto out;
@@ -450,17 +548,15 @@ static void advance_transaction(struct acpi_ec *ec)
 		if (EC_FLAGS_QUERY_HANDSHAKE &&
 		    !(status & ACPI_EC_FLAG_SCI) &&
 		    (t->command == ACPI_EC_COMMAND_QUERY)) {
-			t->flags |= ACPI_EC_COMMAND_POLL;
-			acpi_ec_complete_query(ec);
+			ec_transaction_transition(ec, ACPI_EC_COMMAND_POLL);
 			t->rdata[t->ri++] = 0x00;
-			t->flags |= ACPI_EC_COMMAND_COMPLETE;
-			ec_dbg_req("Command(%s) software completion",
-				   acpi_ec_cmd_string(t->command));
+			ec_transaction_transition(ec, ACPI_EC_COMMAND_COMPLETE);
+			ec_dbg_evt("Command(%s) completed by software",
+				   acpi_ec_cmd_string(ACPI_EC_COMMAND_QUERY));
 			wakeup = true;
 		} else if ((status & ACPI_EC_FLAG_IBF) == 0) {
 			acpi_ec_write_cmd(ec, t->command);
-			t->flags |= ACPI_EC_COMMAND_POLL;
-			acpi_ec_complete_query(ec);
+			ec_transaction_transition(ec, ACPI_EC_COMMAND_POLL);
 		} else
 			goto err;
 		goto out;
@@ -490,8 +586,39 @@ static void start_transaction(struct acpi_ec *ec)
 {
 	ec->curr->irq_count = ec->curr->wi = ec->curr->ri = 0;
 	ec->curr->flags = 0;
-	ec->curr->timestamp = jiffies;
-	advance_transaction(ec);
+}
+
+static int ec_guard(struct acpi_ec *ec)
+{
+	unsigned long guard = usecs_to_jiffies(ec_polling_guard);
+	unsigned long timeout = ec->timestamp + guard;
+
+	do {
+		if (ec_busy_polling) {
+			/* Perform busy polling */
+			if (ec_transaction_completed(ec))
+				return 0;
+			udelay(jiffies_to_usecs(guard));
+		} else {
+			/*
+			 * Perform wait polling
+			 *
+			 * For SCI_EVT clearing timing of "event",
+			 * performing guarding before re-checking the
+			 * SCI_EVT. Otherwise, such guarding is not needed
+			 * due to the old practices.
+			 */
+			if (!ec_transaction_polled(ec) &&
+			    !acpi_ec_guard_event(ec))
+				break;
+			if (wait_event_timeout(ec->wait,
+					       ec_transaction_completed(ec),
+					       guard))
+				return 0;
+		}
+		/* Guard the register accesses for the polling modes */
+	} while (time_before(jiffies, timeout));
+	return -ETIME;
 }
 
 static int ec_poll(struct acpi_ec *ec)
@@ -502,25 +629,11 @@ static int ec_poll(struct acpi_ec *ec)
 	while (repeat--) {
 		unsigned long delay = jiffies +
 			msecs_to_jiffies(ec_delay);
-		unsigned long usecs = ACPI_EC_UDELAY_POLL;
 		do {
-			/* don't sleep with disabled interrupts */
-			if (EC_FLAGS_MSI || irqs_disabled()) {
-				usecs = ACPI_EC_MSI_UDELAY;
-				udelay(usecs);
-				if (ec_transaction_completed(ec))
-					return 0;
-			} else {
-				if (wait_event_timeout(ec->wait,
-						ec_transaction_completed(ec),
-						usecs_to_jiffies(usecs)))
-					return 0;
-			}
+			if (!ec_guard(ec))
+				return 0;
 			spin_lock_irqsave(&ec->lock, flags);
-			if (time_after(jiffies,
-					ec->curr->timestamp +
-					usecs_to_jiffies(usecs)))
-				advance_transaction(ec);
+			advance_transaction(ec);
 			spin_unlock_irqrestore(&ec->lock, flags);
 		} while (time_before(jiffies, delay));
 		pr_debug("controller reset, restart transaction\n");
@@ -537,8 +650,6 @@ static int acpi_ec_transaction_unlocked(struct acpi_ec *ec,
 	unsigned long tmp;
 	int ret = 0;
 
-	if (EC_FLAGS_MSI)
-		udelay(ACPI_EC_MSI_UDELAY);
 	/* start transaction */
 	spin_lock_irqsave(&ec->lock, tmp);
 	/* Enable GPE for command processing (IBF=0/OBF=1) */
@@ -552,7 +663,9 @@ static int acpi_ec_transaction_unlocked(struct acpi_ec *ec,
 	ec_dbg_req("Command(%s) started", acpi_ec_cmd_string(t->command));
 	start_transaction(ec);
 	spin_unlock_irqrestore(&ec->lock, tmp);
+
 	ret = ec_poll(ec);
+
 	spin_lock_irqsave(&ec->lock, tmp);
 	if (t->irq_count == ec_storm_threshold)
 		acpi_ec_clear_storm(ec, EC_FLAGS_COMMAND_STORM);
@@ -575,6 +688,7 @@ static int acpi_ec_transaction(struct acpi_ec *ec, struct transaction *t)
 		return -EINVAL;
 	if (t->rdata)
 		memset(t->rdata, 0, t->rlen);
+
 	mutex_lock(&ec->mutex);
 	if (ec->global_lock) {
 		status = acpi_acquire_global_lock(ACPI_EC_UDELAY_GLK, &glk);
@@ -586,8 +700,6 @@ static int acpi_ec_transaction(struct acpi_ec *ec, struct transaction *t)
 
 	status = acpi_ec_transaction_unlocked(ec, t);
 
-	if (test_bit(EC_FLAGS_COMMAND_STORM, &ec->flags))
-		msleep(1);
 	if (ec->global_lock)
 		acpi_release_global_lock(glk);
 unlock:
@@ -923,11 +1035,54 @@ static int acpi_ec_query(struct acpi_ec *ec, u8 *data)
 	return result;
 }
 
-static void acpi_ec_gpe_poller(struct work_struct *work)
+static void acpi_ec_check_event(struct acpi_ec *ec)
 {
+	unsigned long flags;
+
+	if (ec_event_clearing == ACPI_EC_EVT_TIMING_EVENT) {
+		if (ec_guard(ec)) {
+			spin_lock_irqsave(&ec->lock, flags);
+			/*
+			 * Take care of the SCI_EVT unless no one else is
+			 * taking care of it.
+			 */
+			if (!ec->curr)
+				advance_transaction(ec);
+			spin_unlock_irqrestore(&ec->lock, flags);
+		}
+	}
+}
+
+static void acpi_ec_event_handler(struct work_struct *work)
+{
+	unsigned long flags;
 	struct acpi_ec *ec = container_of(work, struct acpi_ec, work);
 
-	acpi_ec_query(ec, NULL);
+	ec_dbg_evt("Event started");
+
+	spin_lock_irqsave(&ec->lock, flags);
+	while (ec->nr_pending_queries) {
+		spin_unlock_irqrestore(&ec->lock, flags);
+		(void)acpi_ec_query(ec, NULL);
+		spin_lock_irqsave(&ec->lock, flags);
+		ec->nr_pending_queries--;
+		/*
+		 * Before exit, make sure that this work item can be
+		 * scheduled again. There might be QR_EC failures, leaving
+		 * EC_FLAGS_QUERY_PENDING uncleared and preventing this work
+		 * item from being scheduled again.
+		 */
+		if (!ec->nr_pending_queries) {
+			if (ec_event_clearing == ACPI_EC_EVT_TIMING_STATUS ||
+			    ec_event_clearing == ACPI_EC_EVT_TIMING_QUERY)
+				acpi_ec_complete_query(ec);
+		}
+	}
+	spin_unlock_irqrestore(&ec->lock, flags);
+
+	ec_dbg_evt("Event stopped");
+
+	acpi_ec_check_event(ec);
 }
 
 static u32 acpi_ec_gpe_handler(acpi_handle gpe_device,
@@ -961,7 +1116,7 @@ acpi_ec_space_handler(u32 function, acpi_physical_address address,
 	if (function != ACPI_READ && function != ACPI_WRITE)
 		return AE_BAD_PARAMETER;
 
-	if (EC_FLAGS_MSI || bits > 8)
+	if (ec_busy_polling || bits > 8)
 		acpi_ec_burst_enable(ec);
 
 	for (i = 0; i < bytes; ++i, ++address, ++value)
@@ -969,7 +1124,7 @@ acpi_ec_space_handler(u32 function, acpi_physical_address address,
 			acpi_ec_read(ec, address, value) :
 			acpi_ec_write(ec, address, *value);
 
-	if (EC_FLAGS_MSI || bits > 8)
+	if (ec_busy_polling || bits > 8)
 		acpi_ec_burst_disable(ec);
 
 	switch (result) {
@@ -1002,7 +1157,8 @@ static struct acpi_ec *make_acpi_ec(void)
 	init_waitqueue_head(&ec->wait);
 	INIT_LIST_HEAD(&ec->list);
 	spin_lock_init(&ec->lock);
-	INIT_WORK(&ec->work, acpi_ec_gpe_poller);
+	INIT_WORK(&ec->work, acpi_ec_event_handler);
+	ec->timestamp = jiffies;
 	return ec;
 }
 
@@ -1237,30 +1393,13 @@ static int ec_validate_ecdt(const struct dmi_system_id *id)
 	return 0;
 }
 
-/* MSI EC needs special treatment, enable it */
-static int ec_flag_msi(const struct dmi_system_id *id)
-{
-	pr_debug("Detected MSI hardware, enabling workarounds.\n");
-	EC_FLAGS_MSI = 1;
-	EC_FLAGS_VALIDATE_ECDT = 1;
-	return 0;
-}
-
-/*
- * Clevo M720 notebook actually works ok with IRQ mode, if we lifted
- * the GPE storm threshold back to 20
- */
-static int ec_enlarge_storm_threshold(const struct dmi_system_id *id)
-{
-	pr_debug("Setting the EC GPE storm threshold to 20\n");
-	ec_storm_threshold  = 20;
-	return 0;
-}
-
+#if 0
 /*
- * Acer EC firmware refuses to respond QR_EC when SCI_EVT is not set, for
- * which case, we complete the QR_EC without issuing it to the firmware.
- * https://bugzilla.kernel.org/show_bug.cgi?id=86211
+ * Some EC firmware variations refuses to respond QR_EC when SCI_EVT is not
+ * set, for which case, we complete the QR_EC without issuing it to the
+ * firmware.
+ * https://bugzilla.kernel.org/show_bug.cgi?id=82611
+ * https://bugzilla.kernel.org/show_bug.cgi?id=97381
  */
 static int ec_flag_query_handshake(const struct dmi_system_id *id)
 {
@@ -1268,6 +1407,7 @@ static int ec_flag_query_handshake(const struct dmi_system_id *id)
 	EC_FLAGS_QUERY_HANDSHAKE = 1;
 	return 0;
 }
+#endif
 
 /*
  * On some hardware it is necessary to clear events accumulated by the EC during
@@ -1290,6 +1430,7 @@ static int ec_clear_on_resume(const struct dmi_system_id *id)
 {
 	pr_debug("Detected system needing EC poll on resume.\n");
 	EC_FLAGS_CLEAR_ON_RESUME = 1;
+	ec_event_clearing = ACPI_EC_EVT_TIMING_STATUS;
 	return 0;
 }
 
@@ -1299,29 +1440,9 @@ static struct dmi_system_id ec_dmi_table[] __initdata = {
 	DMI_MATCH(DMI_BIOS_VENDOR, "COMPAL"),
 	DMI_MATCH(DMI_BOARD_NAME, "JFL92") }, NULL},
 	{
-	ec_flag_msi, "MSI hardware", {
-	DMI_MATCH(DMI_BIOS_VENDOR, "Micro-Star")}, NULL},
-	{
-	ec_flag_msi, "MSI hardware", {
-	DMI_MATCH(DMI_SYS_VENDOR, "Micro-Star")}, NULL},
-	{
-	ec_flag_msi, "MSI hardware", {
-	DMI_MATCH(DMI_CHASSIS_VENDOR, "MICRO-Star")}, NULL},
-	{
-	ec_flag_msi, "MSI hardware", {
-	DMI_MATCH(DMI_CHASSIS_VENDOR, "MICRO-STAR")}, NULL},
-	{
-	ec_flag_msi, "Quanta hardware", {
-	DMI_MATCH(DMI_SYS_VENDOR, "Quanta"),
-	DMI_MATCH(DMI_PRODUCT_NAME, "TW8/SW8/DW8"),}, NULL},
-	{
-	ec_flag_msi, "Quanta hardware", {
-	DMI_MATCH(DMI_SYS_VENDOR, "Quanta"),
-	DMI_MATCH(DMI_PRODUCT_NAME, "TW9/SW9"),}, NULL},
-	{
-	ec_flag_msi, "Clevo W350etq", {
-	DMI_MATCH(DMI_SYS_VENDOR, "CLEVO CO."),
-	DMI_MATCH(DMI_PRODUCT_NAME, "W35_37ET"),}, NULL},
+	ec_validate_ecdt, "MSI MS-171F", {
+	DMI_MATCH(DMI_SYS_VENDOR, "Micro-Star"),
+	DMI_MATCH(DMI_PRODUCT_NAME, "MS-171F"),}, NULL},
 	{
 	ec_validate_ecdt, "ASUS hardware", {
 	DMI_MATCH(DMI_BIOS_VENDOR, "ASUS") }, NULL},
@@ -1329,10 +1450,6 @@ static struct dmi_system_id ec_dmi_table[] __initdata = {
 	ec_validate_ecdt, "ASUS hardware", {
 	DMI_MATCH(DMI_BOARD_VENDOR, "ASUSTeK Computer Inc.") }, NULL},
 	{
-	ec_enlarge_storm_threshold, "CLEVO hardware", {
-	DMI_MATCH(DMI_SYS_VENDOR, "CLEVO Co."),
-	DMI_MATCH(DMI_PRODUCT_NAME, "M720T/M730T"),}, NULL},
-	{
 	ec_skip_dsdt_scan, "HP Folio 13", {
 	DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
 	DMI_MATCH(DMI_PRODUCT_NAME, "HP Folio 13"),}, NULL},
@@ -1343,9 +1460,6 @@ static struct dmi_system_id ec_dmi_table[] __initdata = {
 	{
 	ec_clear_on_resume, "Samsung hardware", {
 	DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD.")}, NULL},
-	{
-	ec_flag_query_handshake, "Acer hardware", {
-	DMI_MATCH(DMI_SYS_VENDOR, "Acer"), }, NULL},
 	{},
 };
 
@@ -1427,6 +1541,43 @@ error:
 	return -ENODEV;
 }
 
+static int param_set_event_clearing(const char *val, struct kernel_param *kp)
+{
+	int result = 0;
+
+	if (!strncmp(val, "status", sizeof("status") - 1)) {
+		ec_event_clearing = ACPI_EC_EVT_TIMING_STATUS;
+		pr_info("Assuming SCI_EVT clearing on EC_SC accesses\n");
+	} else if (!strncmp(val, "query", sizeof("query") - 1)) {
+		ec_event_clearing = ACPI_EC_EVT_TIMING_QUERY;
+		pr_info("Assuming SCI_EVT clearing on QR_EC writes\n");
+	} else if (!strncmp(val, "event", sizeof("event") - 1)) {
+		ec_event_clearing = ACPI_EC_EVT_TIMING_EVENT;
+		pr_info("Assuming SCI_EVT clearing on event reads\n");
+	} else
+		result = -EINVAL;
+	return result;
+}
+
+static int param_get_event_clearing(char *buffer, struct kernel_param *kp)
+{
+	switch (ec_event_clearing) {
+	case ACPI_EC_EVT_TIMING_STATUS:
+		return sprintf(buffer, "status");
+	case ACPI_EC_EVT_TIMING_QUERY:
+		return sprintf(buffer, "query");
+	case ACPI_EC_EVT_TIMING_EVENT:
+		return sprintf(buffer, "event");
+	default:
+		return sprintf(buffer, "invalid");
+	}
+	return 0;
+}
+
+module_param_call(ec_event_clearing, param_set_event_clearing, param_get_event_clearing,
+		  NULL, 0644);
+MODULE_PARM_DESC(ec_event_clearing, "Assumed SCI_EVT clearing timing");
+
 static struct acpi_driver acpi_ec_driver = {
 	.name = "ec",
 	.class = ACPI_EC_CLASS,
diff --git a/drivers/acpi/fan.c b/drivers/acpi/fan.c
index 7a36f02598a6..bea0bbaafa97 100644
--- a/drivers/acpi/fan.c
+++ b/drivers/acpi/fan.c
@@ -158,8 +158,9 @@ static int fan_get_state(struct acpi_device *device, unsigned long *state)
 	if (result)
 		return result;
 
-	*state = (acpi_state == ACPI_STATE_D3_COLD ? 0 :
-		 (acpi_state == ACPI_STATE_D0 ? 1 : -1));
+	*state = acpi_state == ACPI_STATE_D3_COLD
+			|| acpi_state == ACPI_STATE_D3_HOT ?
+		0 : (acpi_state == ACPI_STATE_D0 ? 1 : -1);
 	return 0;
 }
 
diff --git a/drivers/acpi/glue.c b/drivers/acpi/glue.c
index 39c485b0c25c..b9657af751d1 100644
--- a/drivers/acpi/glue.c
+++ b/drivers/acpi/glue.c
@@ -13,6 +13,7 @@
 #include <linux/slab.h>
 #include <linux/rwsem.h>
 #include <linux/acpi.h>
+#include <linux/dma-mapping.h>
 
 #include "internal.h"
 
@@ -167,6 +168,7 @@ int acpi_bind_one(struct device *dev, struct acpi_device *acpi_dev)
 	struct list_head *physnode_list;
 	unsigned int node_id;
 	int retval = -EINVAL;
+	bool coherent;
 
 	if (has_acpi_companion(dev)) {
 		if (acpi_dev) {
@@ -223,6 +225,9 @@ int acpi_bind_one(struct device *dev, struct acpi_device *acpi_dev)
 	if (!has_acpi_companion(dev))
 		ACPI_COMPANION_SET(dev, acpi_dev);
 
+	if (acpi_check_dma(acpi_dev, &coherent))
+		arch_setup_dma_ops(dev, 0, 0, NULL, coherent);
+
 	acpi_physnode_link_name(physical_node_name, node_id);
 	retval = sysfs_create_link(&acpi_dev->dev.kobj, &dev->kobj,
 				   physical_node_name);
diff --git a/drivers/acpi/hed.c b/drivers/acpi/hed.c
index aafe3ca829c2..a322710b5ba4 100644
--- a/drivers/acpi/hed.c
+++ b/drivers/acpi/hed.c
@@ -27,7 +27,7 @@
 #include <linux/acpi.h>
 #include <acpi/hed.h>
 
-static struct acpi_device_id acpi_hed_ids[] = {
+static const struct acpi_device_id acpi_hed_ids[] = {
 	{"PNP0C33", 0},
 	{"", 0},
 };
diff --git a/drivers/acpi/internal.h b/drivers/acpi/internal.h
index ba4a61e964be..4683a96932b9 100644
--- a/drivers/acpi/internal.h
+++ b/drivers/acpi/internal.h
@@ -58,6 +58,7 @@ void acpi_cmos_rtc_init(void);
 #else
 static inline void acpi_cmos_rtc_init(void) {}
 #endif
+int acpi_rev_override_setup(char *str);
 
 extern bool acpi_force_hot_remove;
 
@@ -138,6 +139,8 @@ struct acpi_ec {
 	struct transaction *curr;
 	spinlock_t lock;
 	struct work_struct work;
+	unsigned long timestamp;
+	unsigned long nr_pending_queries;
 };
 
 extern struct acpi_ec *first_ec;
@@ -182,15 +185,10 @@ static inline void suspend_nvs_restore(void) {}
 #endif
 
 /*--------------------------------------------------------------------------
-					Video
-  -------------------------------------------------------------------------- */
-#if defined(CONFIG_ACPI_VIDEO) || defined(CONFIG_ACPI_VIDEO_MODULE)
-bool acpi_osi_is_win8(void);
-#endif
-
-/*--------------------------------------------------------------------------
 				Device properties
   -------------------------------------------------------------------------- */
+#define ACPI_DT_NAMESPACE_HID	"PRP0001"
+
 void acpi_init_properties(struct acpi_device *adev);
 void acpi_free_properties(struct acpi_device *adev);
 
diff --git a/drivers/acpi/nfit.c b/drivers/acpi/nfit.c
new file mode 100644
index 000000000000..628a42c41ab1
--- /dev/null
+++ b/drivers/acpi/nfit.c
@@ -0,0 +1,1693 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#include <linux/list_sort.h>
+#include <linux/libnvdimm.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/ndctl.h>
+#include <linux/list.h>
+#include <linux/acpi.h>
+#include <linux/sort.h>
+#include <linux/pmem.h>
+#include <linux/io.h>
+#include "nfit.h"
+
+/*
+ * For readq() and writeq() on 32-bit builds, the hi-lo, lo-hi order is
+ * irrelevant.
+ */
+#include <asm-generic/io-64-nonatomic-hi-lo.h>
+
+static bool force_enable_dimms;
+module_param(force_enable_dimms, bool, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(force_enable_dimms, "Ignore _STA (ACPI DIMM device) status");
+
+static u8 nfit_uuid[NFIT_UUID_MAX][16];
+
+const u8 *to_nfit_uuid(enum nfit_uuids id)
+{
+	return nfit_uuid[id];
+}
+EXPORT_SYMBOL(to_nfit_uuid);
+
+static struct acpi_nfit_desc *to_acpi_nfit_desc(
+		struct nvdimm_bus_descriptor *nd_desc)
+{
+	return container_of(nd_desc, struct acpi_nfit_desc, nd_desc);
+}
+
+static struct acpi_device *to_acpi_dev(struct acpi_nfit_desc *acpi_desc)
+{
+	struct nvdimm_bus_descriptor *nd_desc = &acpi_desc->nd_desc;
+
+	/*
+	 * If provider == 'ACPI.NFIT' we can assume 'dev' is a struct
+	 * acpi_device.
+	 */
+	if (!nd_desc->provider_name
+			|| strcmp(nd_desc->provider_name, "ACPI.NFIT") != 0)
+		return NULL;
+
+	return to_acpi_device(acpi_desc->dev);
+}
+
+static int acpi_nfit_ctl(struct nvdimm_bus_descriptor *nd_desc,
+		struct nvdimm *nvdimm, unsigned int cmd, void *buf,
+		unsigned int buf_len)
+{
+	struct acpi_nfit_desc *acpi_desc = to_acpi_nfit_desc(nd_desc);
+	const struct nd_cmd_desc *desc = NULL;
+	union acpi_object in_obj, in_buf, *out_obj;
+	struct device *dev = acpi_desc->dev;
+	const char *cmd_name, *dimm_name;
+	unsigned long dsm_mask;
+	acpi_handle handle;
+	const u8 *uuid;
+	u32 offset;
+	int rc, i;
+
+	if (nvdimm) {
+		struct nfit_mem *nfit_mem = nvdimm_provider_data(nvdimm);
+		struct acpi_device *adev = nfit_mem->adev;
+
+		if (!adev)
+			return -ENOTTY;
+		dimm_name = nvdimm_name(nvdimm);
+		cmd_name = nvdimm_cmd_name(cmd);
+		dsm_mask = nfit_mem->dsm_mask;
+		desc = nd_cmd_dimm_desc(cmd);
+		uuid = to_nfit_uuid(NFIT_DEV_DIMM);
+		handle = adev->handle;
+	} else {
+		struct acpi_device *adev = to_acpi_dev(acpi_desc);
+
+		cmd_name = nvdimm_bus_cmd_name(cmd);
+		dsm_mask = nd_desc->dsm_mask;
+		desc = nd_cmd_bus_desc(cmd);
+		uuid = to_nfit_uuid(NFIT_DEV_BUS);
+		handle = adev->handle;
+		dimm_name = "bus";
+	}
+
+	if (!desc || (cmd && (desc->out_num + desc->in_num == 0)))
+		return -ENOTTY;
+
+	if (!test_bit(cmd, &dsm_mask))
+		return -ENOTTY;
+
+	in_obj.type = ACPI_TYPE_PACKAGE;
+	in_obj.package.count = 1;
+	in_obj.package.elements = &in_buf;
+	in_buf.type = ACPI_TYPE_BUFFER;
+	in_buf.buffer.pointer = buf;
+	in_buf.buffer.length = 0;
+
+	/* libnvdimm has already validated the input envelope */
+	for (i = 0; i < desc->in_num; i++)
+		in_buf.buffer.length += nd_cmd_in_size(nvdimm, cmd, desc,
+				i, buf);
+
+	if (IS_ENABLED(CONFIG_ACPI_NFIT_DEBUG)) {
+		dev_dbg(dev, "%s:%s cmd: %s input length: %d\n", __func__,
+				dimm_name, cmd_name, in_buf.buffer.length);
+		print_hex_dump_debug(cmd_name, DUMP_PREFIX_OFFSET, 4,
+				4, in_buf.buffer.pointer, min_t(u32, 128,
+					in_buf.buffer.length), true);
+	}
+
+	out_obj = acpi_evaluate_dsm(handle, uuid, 1, cmd, &in_obj);
+	if (!out_obj) {
+		dev_dbg(dev, "%s:%s _DSM failed cmd: %s\n", __func__, dimm_name,
+				cmd_name);
+		return -EINVAL;
+	}
+
+	if (out_obj->package.type != ACPI_TYPE_BUFFER) {
+		dev_dbg(dev, "%s:%s unexpected output object type cmd: %s type: %d\n",
+				__func__, dimm_name, cmd_name, out_obj->type);
+		rc = -EINVAL;
+		goto out;
+	}
+
+	if (IS_ENABLED(CONFIG_ACPI_NFIT_DEBUG)) {
+		dev_dbg(dev, "%s:%s cmd: %s output length: %d\n", __func__,
+				dimm_name, cmd_name, out_obj->buffer.length);
+		print_hex_dump_debug(cmd_name, DUMP_PREFIX_OFFSET, 4,
+				4, out_obj->buffer.pointer, min_t(u32, 128,
+					out_obj->buffer.length), true);
+	}
+
+	for (i = 0, offset = 0; i < desc->out_num; i++) {
+		u32 out_size = nd_cmd_out_size(nvdimm, cmd, desc, i, buf,
+				(u32 *) out_obj->buffer.pointer);
+
+		if (offset + out_size > out_obj->buffer.length) {
+			dev_dbg(dev, "%s:%s output object underflow cmd: %s field: %d\n",
+					__func__, dimm_name, cmd_name, i);
+			break;
+		}
+
+		if (in_buf.buffer.length + offset + out_size > buf_len) {
+			dev_dbg(dev, "%s:%s output overrun cmd: %s field: %d\n",
+					__func__, dimm_name, cmd_name, i);
+			rc = -ENXIO;
+			goto out;
+		}
+		memcpy(buf + in_buf.buffer.length + offset,
+				out_obj->buffer.pointer + offset, out_size);
+		offset += out_size;
+	}
+	if (offset + in_buf.buffer.length < buf_len) {
+		if (i >= 1) {
+			/*
+			 * status valid, return the number of bytes left
+			 * unfilled in the output buffer
+			 */
+			rc = buf_len - offset - in_buf.buffer.length;
+		} else {
+			dev_err(dev, "%s:%s underrun cmd: %s buf_len: %d out_len: %d\n",
+					__func__, dimm_name, cmd_name, buf_len,
+					offset);
+			rc = -ENXIO;
+		}
+	} else
+		rc = 0;
+
+ out:
+	ACPI_FREE(out_obj);
+
+	return rc;
+}
+
+static const char *spa_type_name(u16 type)
+{
+	static const char *to_name[] = {
+		[NFIT_SPA_VOLATILE] = "volatile",
+		[NFIT_SPA_PM] = "pmem",
+		[NFIT_SPA_DCR] = "dimm-control-region",
+		[NFIT_SPA_BDW] = "block-data-window",
+		[NFIT_SPA_VDISK] = "volatile-disk",
+		[NFIT_SPA_VCD] = "volatile-cd",
+		[NFIT_SPA_PDISK] = "persistent-disk",
+		[NFIT_SPA_PCD] = "persistent-cd",
+
+	};
+
+	if (type > NFIT_SPA_PCD)
+		return "unknown";
+
+	return to_name[type];
+}
+
+static int nfit_spa_type(struct acpi_nfit_system_address *spa)
+{
+	int i;
+
+	for (i = 0; i < NFIT_UUID_MAX; i++)
+		if (memcmp(to_nfit_uuid(i), spa->range_guid, 16) == 0)
+			return i;
+	return -1;
+}
+
+static bool add_spa(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_system_address *spa)
+{
+	struct device *dev = acpi_desc->dev;
+	struct nfit_spa *nfit_spa = devm_kzalloc(dev, sizeof(*nfit_spa),
+			GFP_KERNEL);
+
+	if (!nfit_spa)
+		return false;
+	INIT_LIST_HEAD(&nfit_spa->list);
+	nfit_spa->spa = spa;
+	list_add_tail(&nfit_spa->list, &acpi_desc->spas);
+	dev_dbg(dev, "%s: spa index: %d type: %s\n", __func__,
+			spa->range_index,
+			spa_type_name(nfit_spa_type(spa)));
+	return true;
+}
+
+static bool add_memdev(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_memory_map *memdev)
+{
+	struct device *dev = acpi_desc->dev;
+	struct nfit_memdev *nfit_memdev = devm_kzalloc(dev,
+			sizeof(*nfit_memdev), GFP_KERNEL);
+
+	if (!nfit_memdev)
+		return false;
+	INIT_LIST_HEAD(&nfit_memdev->list);
+	nfit_memdev->memdev = memdev;
+	list_add_tail(&nfit_memdev->list, &acpi_desc->memdevs);
+	dev_dbg(dev, "%s: memdev handle: %#x spa: %d dcr: %d\n",
+			__func__, memdev->device_handle, memdev->range_index,
+			memdev->region_index);
+	return true;
+}
+
+static bool add_dcr(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_control_region *dcr)
+{
+	struct device *dev = acpi_desc->dev;
+	struct nfit_dcr *nfit_dcr = devm_kzalloc(dev, sizeof(*nfit_dcr),
+			GFP_KERNEL);
+
+	if (!nfit_dcr)
+		return false;
+	INIT_LIST_HEAD(&nfit_dcr->list);
+	nfit_dcr->dcr = dcr;
+	list_add_tail(&nfit_dcr->list, &acpi_desc->dcrs);
+	dev_dbg(dev, "%s: dcr index: %d windows: %d\n", __func__,
+			dcr->region_index, dcr->windows);
+	return true;
+}
+
+static bool add_bdw(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_data_region *bdw)
+{
+	struct device *dev = acpi_desc->dev;
+	struct nfit_bdw *nfit_bdw = devm_kzalloc(dev, sizeof(*nfit_bdw),
+			GFP_KERNEL);
+
+	if (!nfit_bdw)
+		return false;
+	INIT_LIST_HEAD(&nfit_bdw->list);
+	nfit_bdw->bdw = bdw;
+	list_add_tail(&nfit_bdw->list, &acpi_desc->bdws);
+	dev_dbg(dev, "%s: bdw dcr: %d windows: %d\n", __func__,
+			bdw->region_index, bdw->windows);
+	return true;
+}
+
+static bool add_idt(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_interleave *idt)
+{
+	struct device *dev = acpi_desc->dev;
+	struct nfit_idt *nfit_idt = devm_kzalloc(dev, sizeof(*nfit_idt),
+			GFP_KERNEL);
+
+	if (!nfit_idt)
+		return false;
+	INIT_LIST_HEAD(&nfit_idt->list);
+	nfit_idt->idt = idt;
+	list_add_tail(&nfit_idt->list, &acpi_desc->idts);
+	dev_dbg(dev, "%s: idt index: %d num_lines: %d\n", __func__,
+			idt->interleave_index, idt->line_count);
+	return true;
+}
+
+static bool add_flush(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_flush_address *flush)
+{
+	struct device *dev = acpi_desc->dev;
+	struct nfit_flush *nfit_flush = devm_kzalloc(dev, sizeof(*nfit_flush),
+			GFP_KERNEL);
+
+	if (!nfit_flush)
+		return false;
+	INIT_LIST_HEAD(&nfit_flush->list);
+	nfit_flush->flush = flush;
+	list_add_tail(&nfit_flush->list, &acpi_desc->flushes);
+	dev_dbg(dev, "%s: nfit_flush handle: %d hint_count: %d\n", __func__,
+			flush->device_handle, flush->hint_count);
+	return true;
+}
+
+static void *add_table(struct acpi_nfit_desc *acpi_desc, void *table,
+		const void *end)
+{
+	struct device *dev = acpi_desc->dev;
+	struct acpi_nfit_header *hdr;
+	void *err = ERR_PTR(-ENOMEM);
+
+	if (table >= end)
+		return NULL;
+
+	hdr = table;
+	switch (hdr->type) {
+	case ACPI_NFIT_TYPE_SYSTEM_ADDRESS:
+		if (!add_spa(acpi_desc, table))
+			return err;
+		break;
+	case ACPI_NFIT_TYPE_MEMORY_MAP:
+		if (!add_memdev(acpi_desc, table))
+			return err;
+		break;
+	case ACPI_NFIT_TYPE_CONTROL_REGION:
+		if (!add_dcr(acpi_desc, table))
+			return err;
+		break;
+	case ACPI_NFIT_TYPE_DATA_REGION:
+		if (!add_bdw(acpi_desc, table))
+			return err;
+		break;
+	case ACPI_NFIT_TYPE_INTERLEAVE:
+		if (!add_idt(acpi_desc, table))
+			return err;
+		break;
+	case ACPI_NFIT_TYPE_FLUSH_ADDRESS:
+		if (!add_flush(acpi_desc, table))
+			return err;
+		break;
+	case ACPI_NFIT_TYPE_SMBIOS:
+		dev_dbg(dev, "%s: smbios\n", __func__);
+		break;
+	default:
+		dev_err(dev, "unknown table '%d' parsing nfit\n", hdr->type);
+		break;
+	}
+
+	return table + hdr->length;
+}
+
+static void nfit_mem_find_spa_bdw(struct acpi_nfit_desc *acpi_desc,
+		struct nfit_mem *nfit_mem)
+{
+	u32 device_handle = __to_nfit_memdev(nfit_mem)->device_handle;
+	u16 dcr = nfit_mem->dcr->region_index;
+	struct nfit_spa *nfit_spa;
+
+	list_for_each_entry(nfit_spa, &acpi_desc->spas, list) {
+		u16 range_index = nfit_spa->spa->range_index;
+		int type = nfit_spa_type(nfit_spa->spa);
+		struct nfit_memdev *nfit_memdev;
+
+		if (type != NFIT_SPA_BDW)
+			continue;
+
+		list_for_each_entry(nfit_memdev, &acpi_desc->memdevs, list) {
+			if (nfit_memdev->memdev->range_index != range_index)
+				continue;
+			if (nfit_memdev->memdev->device_handle != device_handle)
+				continue;
+			if (nfit_memdev->memdev->region_index != dcr)
+				continue;
+
+			nfit_mem->spa_bdw = nfit_spa->spa;
+			return;
+		}
+	}
+
+	dev_dbg(acpi_desc->dev, "SPA-BDW not found for SPA-DCR %d\n",
+			nfit_mem->spa_dcr->range_index);
+	nfit_mem->bdw = NULL;
+}
+
+static int nfit_mem_add(struct acpi_nfit_desc *acpi_desc,
+		struct nfit_mem *nfit_mem, struct acpi_nfit_system_address *spa)
+{
+	u16 dcr = __to_nfit_memdev(nfit_mem)->region_index;
+	struct nfit_memdev *nfit_memdev;
+	struct nfit_flush *nfit_flush;
+	struct nfit_dcr *nfit_dcr;
+	struct nfit_bdw *nfit_bdw;
+	struct nfit_idt *nfit_idt;
+	u16 idt_idx, range_index;
+
+	list_for_each_entry(nfit_dcr, &acpi_desc->dcrs, list) {
+		if (nfit_dcr->dcr->region_index != dcr)
+			continue;
+		nfit_mem->dcr = nfit_dcr->dcr;
+		break;
+	}
+
+	if (!nfit_mem->dcr) {
+		dev_dbg(acpi_desc->dev, "SPA %d missing:%s%s\n",
+				spa->range_index, __to_nfit_memdev(nfit_mem)
+				? "" : " MEMDEV", nfit_mem->dcr ? "" : " DCR");
+		return -ENODEV;
+	}
+
+	/*
+	 * We've found enough to create an nvdimm, optionally
+	 * find an associated BDW
+	 */
+	list_add(&nfit_mem->list, &acpi_desc->dimms);
+
+	list_for_each_entry(nfit_bdw, &acpi_desc->bdws, list) {
+		if (nfit_bdw->bdw->region_index != dcr)
+			continue;
+		nfit_mem->bdw = nfit_bdw->bdw;
+		break;
+	}
+
+	if (!nfit_mem->bdw)
+		return 0;
+
+	nfit_mem_find_spa_bdw(acpi_desc, nfit_mem);
+
+	if (!nfit_mem->spa_bdw)
+		return 0;
+
+	range_index = nfit_mem->spa_bdw->range_index;
+	list_for_each_entry(nfit_memdev, &acpi_desc->memdevs, list) {
+		if (nfit_memdev->memdev->range_index != range_index ||
+				nfit_memdev->memdev->region_index != dcr)
+			continue;
+		nfit_mem->memdev_bdw = nfit_memdev->memdev;
+		idt_idx = nfit_memdev->memdev->interleave_index;
+		list_for_each_entry(nfit_idt, &acpi_desc->idts, list) {
+			if (nfit_idt->idt->interleave_index != idt_idx)
+				continue;
+			nfit_mem->idt_bdw = nfit_idt->idt;
+			break;
+		}
+
+		list_for_each_entry(nfit_flush, &acpi_desc->flushes, list) {
+			if (nfit_flush->flush->device_handle !=
+					nfit_memdev->memdev->device_handle)
+				continue;
+			nfit_mem->nfit_flush = nfit_flush;
+			break;
+		}
+		break;
+	}
+
+	return 0;
+}
+
+static int nfit_mem_dcr_init(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_system_address *spa)
+{
+	struct nfit_mem *nfit_mem, *found;
+	struct nfit_memdev *nfit_memdev;
+	int type = nfit_spa_type(spa);
+	u16 dcr;
+
+	switch (type) {
+	case NFIT_SPA_DCR:
+	case NFIT_SPA_PM:
+		break;
+	default:
+		return 0;
+	}
+
+	list_for_each_entry(nfit_memdev, &acpi_desc->memdevs, list) {
+		int rc;
+
+		if (nfit_memdev->memdev->range_index != spa->range_index)
+			continue;
+		found = NULL;
+		dcr = nfit_memdev->memdev->region_index;
+		list_for_each_entry(nfit_mem, &acpi_desc->dimms, list)
+			if (__to_nfit_memdev(nfit_mem)->region_index == dcr) {
+				found = nfit_mem;
+				break;
+			}
+
+		if (found)
+			nfit_mem = found;
+		else {
+			nfit_mem = devm_kzalloc(acpi_desc->dev,
+					sizeof(*nfit_mem), GFP_KERNEL);
+			if (!nfit_mem)
+				return -ENOMEM;
+			INIT_LIST_HEAD(&nfit_mem->list);
+		}
+
+		if (type == NFIT_SPA_DCR) {
+			struct nfit_idt *nfit_idt;
+			u16 idt_idx;
+
+			/* multiple dimms may share a SPA when interleaved */
+			nfit_mem->spa_dcr = spa;
+			nfit_mem->memdev_dcr = nfit_memdev->memdev;
+			idt_idx = nfit_memdev->memdev->interleave_index;
+			list_for_each_entry(nfit_idt, &acpi_desc->idts, list) {
+				if (nfit_idt->idt->interleave_index != idt_idx)
+					continue;
+				nfit_mem->idt_dcr = nfit_idt->idt;
+				break;
+			}
+		} else {
+			/*
+			 * A single dimm may belong to multiple SPA-PM
+			 * ranges, record at least one in addition to
+			 * any SPA-DCR range.
+			 */
+			nfit_mem->memdev_pmem = nfit_memdev->memdev;
+		}
+
+		if (found)
+			continue;
+
+		rc = nfit_mem_add(acpi_desc, nfit_mem, spa);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static int nfit_mem_cmp(void *priv, struct list_head *_a, struct list_head *_b)
+{
+	struct nfit_mem *a = container_of(_a, typeof(*a), list);
+	struct nfit_mem *b = container_of(_b, typeof(*b), list);
+	u32 handleA, handleB;
+
+	handleA = __to_nfit_memdev(a)->device_handle;
+	handleB = __to_nfit_memdev(b)->device_handle;
+	if (handleA < handleB)
+		return -1;
+	else if (handleA > handleB)
+		return 1;
+	return 0;
+}
+
+static int nfit_mem_init(struct acpi_nfit_desc *acpi_desc)
+{
+	struct nfit_spa *nfit_spa;
+
+	/*
+	 * For each SPA-DCR or SPA-PMEM address range find its
+	 * corresponding MEMDEV(s).  From each MEMDEV find the
+	 * corresponding DCR.  Then, if we're operating on a SPA-DCR,
+	 * try to find a SPA-BDW and a corresponding BDW that references
+	 * the DCR.  Throw it all into an nfit_mem object.  Note, that
+	 * BDWs are optional.
+	 */
+	list_for_each_entry(nfit_spa, &acpi_desc->spas, list) {
+		int rc;
+
+		rc = nfit_mem_dcr_init(acpi_desc, nfit_spa->spa);
+		if (rc)
+			return rc;
+	}
+
+	list_sort(NULL, &acpi_desc->dimms, nfit_mem_cmp);
+
+	return 0;
+}
+
+static ssize_t revision_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nvdimm_bus *nvdimm_bus = to_nvdimm_bus(dev);
+	struct nvdimm_bus_descriptor *nd_desc = to_nd_desc(nvdimm_bus);
+	struct acpi_nfit_desc *acpi_desc = to_acpi_desc(nd_desc);
+
+	return sprintf(buf, "%d\n", acpi_desc->nfit->header.revision);
+}
+static DEVICE_ATTR_RO(revision);
+
+static struct attribute *acpi_nfit_attributes[] = {
+	&dev_attr_revision.attr,
+	NULL,
+};
+
+static struct attribute_group acpi_nfit_attribute_group = {
+	.name = "nfit",
+	.attrs = acpi_nfit_attributes,
+};
+
+const struct attribute_group *acpi_nfit_attribute_groups[] = {
+	&nvdimm_bus_attribute_group,
+	&acpi_nfit_attribute_group,
+	NULL,
+};
+EXPORT_SYMBOL_GPL(acpi_nfit_attribute_groups);
+
+static struct acpi_nfit_memory_map *to_nfit_memdev(struct device *dev)
+{
+	struct nvdimm *nvdimm = to_nvdimm(dev);
+	struct nfit_mem *nfit_mem = nvdimm_provider_data(nvdimm);
+
+	return __to_nfit_memdev(nfit_mem);
+}
+
+static struct acpi_nfit_control_region *to_nfit_dcr(struct device *dev)
+{
+	struct nvdimm *nvdimm = to_nvdimm(dev);
+	struct nfit_mem *nfit_mem = nvdimm_provider_data(nvdimm);
+
+	return nfit_mem->dcr;
+}
+
+static ssize_t handle_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct acpi_nfit_memory_map *memdev = to_nfit_memdev(dev);
+
+	return sprintf(buf, "%#x\n", memdev->device_handle);
+}
+static DEVICE_ATTR_RO(handle);
+
+static ssize_t phys_id_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct acpi_nfit_memory_map *memdev = to_nfit_memdev(dev);
+
+	return sprintf(buf, "%#x\n", memdev->physical_id);
+}
+static DEVICE_ATTR_RO(phys_id);
+
+static ssize_t vendor_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct acpi_nfit_control_region *dcr = to_nfit_dcr(dev);
+
+	return sprintf(buf, "%#x\n", dcr->vendor_id);
+}
+static DEVICE_ATTR_RO(vendor);
+
+static ssize_t rev_id_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct acpi_nfit_control_region *dcr = to_nfit_dcr(dev);
+
+	return sprintf(buf, "%#x\n", dcr->revision_id);
+}
+static DEVICE_ATTR_RO(rev_id);
+
+static ssize_t device_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct acpi_nfit_control_region *dcr = to_nfit_dcr(dev);
+
+	return sprintf(buf, "%#x\n", dcr->device_id);
+}
+static DEVICE_ATTR_RO(device);
+
+static ssize_t format_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct acpi_nfit_control_region *dcr = to_nfit_dcr(dev);
+
+	return sprintf(buf, "%#x\n", dcr->code);
+}
+static DEVICE_ATTR_RO(format);
+
+static ssize_t serial_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct acpi_nfit_control_region *dcr = to_nfit_dcr(dev);
+
+	return sprintf(buf, "%#x\n", dcr->serial_number);
+}
+static DEVICE_ATTR_RO(serial);
+
+static ssize_t flags_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	u16 flags = to_nfit_memdev(dev)->flags;
+
+	return sprintf(buf, "%s%s%s%s%s\n",
+			flags & ACPI_NFIT_MEM_SAVE_FAILED ? "save " : "",
+			flags & ACPI_NFIT_MEM_RESTORE_FAILED ? "restore " : "",
+			flags & ACPI_NFIT_MEM_FLUSH_FAILED ? "flush " : "",
+			flags & ACPI_NFIT_MEM_ARMED ? "arm " : "",
+			flags & ACPI_NFIT_MEM_HEALTH_OBSERVED ? "smart " : "");
+}
+static DEVICE_ATTR_RO(flags);
+
+static struct attribute *acpi_nfit_dimm_attributes[] = {
+	&dev_attr_handle.attr,
+	&dev_attr_phys_id.attr,
+	&dev_attr_vendor.attr,
+	&dev_attr_device.attr,
+	&dev_attr_format.attr,
+	&dev_attr_serial.attr,
+	&dev_attr_rev_id.attr,
+	&dev_attr_flags.attr,
+	NULL,
+};
+
+static umode_t acpi_nfit_dimm_attr_visible(struct kobject *kobj,
+		struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+
+	if (to_nfit_dcr(dev))
+		return a->mode;
+	else
+		return 0;
+}
+
+static struct attribute_group acpi_nfit_dimm_attribute_group = {
+	.name = "nfit",
+	.attrs = acpi_nfit_dimm_attributes,
+	.is_visible = acpi_nfit_dimm_attr_visible,
+};
+
+static const struct attribute_group *acpi_nfit_dimm_attribute_groups[] = {
+	&nvdimm_attribute_group,
+	&nd_device_attribute_group,
+	&acpi_nfit_dimm_attribute_group,
+	NULL,
+};
+
+static struct nvdimm *acpi_nfit_dimm_by_handle(struct acpi_nfit_desc *acpi_desc,
+		u32 device_handle)
+{
+	struct nfit_mem *nfit_mem;
+
+	list_for_each_entry(nfit_mem, &acpi_desc->dimms, list)
+		if (__to_nfit_memdev(nfit_mem)->device_handle == device_handle)
+			return nfit_mem->nvdimm;
+
+	return NULL;
+}
+
+static int acpi_nfit_add_dimm(struct acpi_nfit_desc *acpi_desc,
+		struct nfit_mem *nfit_mem, u32 device_handle)
+{
+	struct acpi_device *adev, *adev_dimm;
+	struct device *dev = acpi_desc->dev;
+	const u8 *uuid = to_nfit_uuid(NFIT_DEV_DIMM);
+	unsigned long long sta;
+	int i, rc = -ENODEV;
+	acpi_status status;
+
+	nfit_mem->dsm_mask = acpi_desc->dimm_dsm_force_en;
+	adev = to_acpi_dev(acpi_desc);
+	if (!adev)
+		return 0;
+
+	adev_dimm = acpi_find_child_device(adev, device_handle, false);
+	nfit_mem->adev = adev_dimm;
+	if (!adev_dimm) {
+		dev_err(dev, "no ACPI.NFIT device with _ADR %#x, disabling...\n",
+				device_handle);
+		return force_enable_dimms ? 0 : -ENODEV;
+	}
+
+	status = acpi_evaluate_integer(adev_dimm->handle, "_STA", NULL, &sta);
+	if (status == AE_NOT_FOUND) {
+		dev_dbg(dev, "%s missing _STA, assuming enabled...\n",
+				dev_name(&adev_dimm->dev));
+		rc = 0;
+	} else if (ACPI_FAILURE(status))
+		dev_err(dev, "%s failed to retrieve_STA, disabling...\n",
+				dev_name(&adev_dimm->dev));
+	else if ((sta & ACPI_STA_DEVICE_ENABLED) == 0)
+		dev_info(dev, "%s disabled by firmware\n",
+				dev_name(&adev_dimm->dev));
+	else
+		rc = 0;
+
+	for (i = ND_CMD_SMART; i <= ND_CMD_VENDOR; i++)
+		if (acpi_check_dsm(adev_dimm->handle, uuid, 1, 1ULL << i))
+			set_bit(i, &nfit_mem->dsm_mask);
+
+	return force_enable_dimms ? 0 : rc;
+}
+
+static int acpi_nfit_register_dimms(struct acpi_nfit_desc *acpi_desc)
+{
+	struct nfit_mem *nfit_mem;
+	int dimm_count = 0;
+
+	list_for_each_entry(nfit_mem, &acpi_desc->dimms, list) {
+		struct nvdimm *nvdimm;
+		unsigned long flags = 0;
+		u32 device_handle;
+		u16 mem_flags;
+		int rc;
+
+		device_handle = __to_nfit_memdev(nfit_mem)->device_handle;
+		nvdimm = acpi_nfit_dimm_by_handle(acpi_desc, device_handle);
+		if (nvdimm) {
+			/*
+			 * If for some reason we find multiple DCRs the
+			 * first one wins
+			 */
+			dev_err(acpi_desc->dev, "duplicate DCR detected: %s\n",
+					nvdimm_name(nvdimm));
+			continue;
+		}
+
+		if (nfit_mem->bdw && nfit_mem->memdev_pmem)
+			flags |= NDD_ALIASING;
+
+		mem_flags = __to_nfit_memdev(nfit_mem)->flags;
+		if (mem_flags & ACPI_NFIT_MEM_ARMED)
+			flags |= NDD_UNARMED;
+
+		rc = acpi_nfit_add_dimm(acpi_desc, nfit_mem, device_handle);
+		if (rc)
+			continue;
+
+		nvdimm = nvdimm_create(acpi_desc->nvdimm_bus, nfit_mem,
+				acpi_nfit_dimm_attribute_groups,
+				flags, &nfit_mem->dsm_mask);
+		if (!nvdimm)
+			return -ENOMEM;
+
+		nfit_mem->nvdimm = nvdimm;
+		dimm_count++;
+
+		if ((mem_flags & ACPI_NFIT_MEM_FAILED_MASK) == 0)
+			continue;
+
+		dev_info(acpi_desc->dev, "%s: failed: %s%s%s%s\n",
+				nvdimm_name(nvdimm),
+			mem_flags & ACPI_NFIT_MEM_SAVE_FAILED ? "save " : "",
+			mem_flags & ACPI_NFIT_MEM_RESTORE_FAILED ? "restore " : "",
+			mem_flags & ACPI_NFIT_MEM_FLUSH_FAILED ? "flush " : "",
+			mem_flags & ACPI_NFIT_MEM_ARMED ? "arm " : "");
+
+	}
+
+	return nvdimm_bus_check_dimm_count(acpi_desc->nvdimm_bus, dimm_count);
+}
+
+static void acpi_nfit_init_dsms(struct acpi_nfit_desc *acpi_desc)
+{
+	struct nvdimm_bus_descriptor *nd_desc = &acpi_desc->nd_desc;
+	const u8 *uuid = to_nfit_uuid(NFIT_DEV_BUS);
+	struct acpi_device *adev;
+	int i;
+
+	adev = to_acpi_dev(acpi_desc);
+	if (!adev)
+		return;
+
+	for (i = ND_CMD_ARS_CAP; i <= ND_CMD_ARS_STATUS; i++)
+		if (acpi_check_dsm(adev->handle, uuid, 1, 1ULL << i))
+			set_bit(i, &nd_desc->dsm_mask);
+}
+
+static ssize_t range_index_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+	struct nfit_spa *nfit_spa = nd_region_provider_data(nd_region);
+
+	return sprintf(buf, "%d\n", nfit_spa->spa->range_index);
+}
+static DEVICE_ATTR_RO(range_index);
+
+static struct attribute *acpi_nfit_region_attributes[] = {
+	&dev_attr_range_index.attr,
+	NULL,
+};
+
+static struct attribute_group acpi_nfit_region_attribute_group = {
+	.name = "nfit",
+	.attrs = acpi_nfit_region_attributes,
+};
+
+static const struct attribute_group *acpi_nfit_region_attribute_groups[] = {
+	&nd_region_attribute_group,
+	&nd_mapping_attribute_group,
+	&nd_device_attribute_group,
+	&nd_numa_attribute_group,
+	&acpi_nfit_region_attribute_group,
+	NULL,
+};
+
+/* enough info to uniquely specify an interleave set */
+struct nfit_set_info {
+	struct nfit_set_info_map {
+		u64 region_offset;
+		u32 serial_number;
+		u32 pad;
+	} mapping[0];
+};
+
+static size_t sizeof_nfit_set_info(int num_mappings)
+{
+	return sizeof(struct nfit_set_info)
+		+ num_mappings * sizeof(struct nfit_set_info_map);
+}
+
+static int cmp_map(const void *m0, const void *m1)
+{
+	const struct nfit_set_info_map *map0 = m0;
+	const struct nfit_set_info_map *map1 = m1;
+
+	return memcmp(&map0->region_offset, &map1->region_offset,
+			sizeof(u64));
+}
+
+/* Retrieve the nth entry referencing this spa */
+static struct acpi_nfit_memory_map *memdev_from_spa(
+		struct acpi_nfit_desc *acpi_desc, u16 range_index, int n)
+{
+	struct nfit_memdev *nfit_memdev;
+
+	list_for_each_entry(nfit_memdev, &acpi_desc->memdevs, list)
+		if (nfit_memdev->memdev->range_index == range_index)
+			if (n-- == 0)
+				return nfit_memdev->memdev;
+	return NULL;
+}
+
+static int acpi_nfit_init_interleave_set(struct acpi_nfit_desc *acpi_desc,
+		struct nd_region_desc *ndr_desc,
+		struct acpi_nfit_system_address *spa)
+{
+	int i, spa_type = nfit_spa_type(spa);
+	struct device *dev = acpi_desc->dev;
+	struct nd_interleave_set *nd_set;
+	u16 nr = ndr_desc->num_mappings;
+	struct nfit_set_info *info;
+
+	if (spa_type == NFIT_SPA_PM || spa_type == NFIT_SPA_VOLATILE)
+		/* pass */;
+	else
+		return 0;
+
+	nd_set = devm_kzalloc(dev, sizeof(*nd_set), GFP_KERNEL);
+	if (!nd_set)
+		return -ENOMEM;
+
+	info = devm_kzalloc(dev, sizeof_nfit_set_info(nr), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+	for (i = 0; i < nr; i++) {
+		struct nd_mapping *nd_mapping = &ndr_desc->nd_mapping[i];
+		struct nfit_set_info_map *map = &info->mapping[i];
+		struct nvdimm *nvdimm = nd_mapping->nvdimm;
+		struct nfit_mem *nfit_mem = nvdimm_provider_data(nvdimm);
+		struct acpi_nfit_memory_map *memdev = memdev_from_spa(acpi_desc,
+				spa->range_index, i);
+
+		if (!memdev || !nfit_mem->dcr) {
+			dev_err(dev, "%s: failed to find DCR\n", __func__);
+			return -ENODEV;
+		}
+
+		map->region_offset = memdev->region_offset;
+		map->serial_number = nfit_mem->dcr->serial_number;
+	}
+
+	sort(&info->mapping[0], nr, sizeof(struct nfit_set_info_map),
+			cmp_map, NULL);
+	nd_set->cookie = nd_fletcher64(info, sizeof_nfit_set_info(nr), 0);
+	ndr_desc->nd_set = nd_set;
+	devm_kfree(dev, info);
+
+	return 0;
+}
+
+static u64 to_interleave_offset(u64 offset, struct nfit_blk_mmio *mmio)
+{
+	struct acpi_nfit_interleave *idt = mmio->idt;
+	u32 sub_line_offset, line_index, line_offset;
+	u64 line_no, table_skip_count, table_offset;
+
+	line_no = div_u64_rem(offset, mmio->line_size, &sub_line_offset);
+	table_skip_count = div_u64_rem(line_no, mmio->num_lines, &line_index);
+	line_offset = idt->line_offset[line_index]
+		* mmio->line_size;
+	table_offset = table_skip_count * mmio->table_size;
+
+	return mmio->base_offset + line_offset + table_offset + sub_line_offset;
+}
+
+static void wmb_blk(struct nfit_blk *nfit_blk)
+{
+
+	if (nfit_blk->nvdimm_flush) {
+		/*
+		 * The first wmb() is needed to 'sfence' all previous writes
+		 * such that they are architecturally visible for the platform
+		 * buffer flush.  Note that we've already arranged for pmem
+		 * writes to avoid the cache via arch_memcpy_to_pmem().  The
+		 * final wmb() ensures ordering for the NVDIMM flush write.
+		 */
+		wmb();
+		writeq(1, nfit_blk->nvdimm_flush);
+		wmb();
+	} else
+		wmb_pmem();
+}
+
+static u64 read_blk_stat(struct nfit_blk *nfit_blk, unsigned int bw)
+{
+	struct nfit_blk_mmio *mmio = &nfit_blk->mmio[DCR];
+	u64 offset = nfit_blk->stat_offset + mmio->size * bw;
+
+	if (mmio->num_lines)
+		offset = to_interleave_offset(offset, mmio);
+
+	return readq(mmio->base + offset);
+}
+
+static void write_blk_ctl(struct nfit_blk *nfit_blk, unsigned int bw,
+		resource_size_t dpa, unsigned int len, unsigned int write)
+{
+	u64 cmd, offset;
+	struct nfit_blk_mmio *mmio = &nfit_blk->mmio[DCR];
+
+	enum {
+		BCW_OFFSET_MASK = (1ULL << 48)-1,
+		BCW_LEN_SHIFT = 48,
+		BCW_LEN_MASK = (1ULL << 8) - 1,
+		BCW_CMD_SHIFT = 56,
+	};
+
+	cmd = (dpa >> L1_CACHE_SHIFT) & BCW_OFFSET_MASK;
+	len = len >> L1_CACHE_SHIFT;
+	cmd |= ((u64) len & BCW_LEN_MASK) << BCW_LEN_SHIFT;
+	cmd |= ((u64) write) << BCW_CMD_SHIFT;
+
+	offset = nfit_blk->cmd_offset + mmio->size * bw;
+	if (mmio->num_lines)
+		offset = to_interleave_offset(offset, mmio);
+
+	writeq(cmd, mmio->base + offset);
+	wmb_blk(nfit_blk);
+
+	if (nfit_blk->dimm_flags & ND_BLK_DCR_LATCH)
+		readq(mmio->base + offset);
+}
+
+static int acpi_nfit_blk_single_io(struct nfit_blk *nfit_blk,
+		resource_size_t dpa, void *iobuf, size_t len, int rw,
+		unsigned int lane)
+{
+	struct nfit_blk_mmio *mmio = &nfit_blk->mmio[BDW];
+	unsigned int copied = 0;
+	u64 base_offset;
+	int rc;
+
+	base_offset = nfit_blk->bdw_offset + dpa % L1_CACHE_BYTES
+		+ lane * mmio->size;
+	write_blk_ctl(nfit_blk, lane, dpa, len, rw);
+	while (len) {
+		unsigned int c;
+		u64 offset;
+
+		if (mmio->num_lines) {
+			u32 line_offset;
+
+			offset = to_interleave_offset(base_offset + copied,
+					mmio);
+			div_u64_rem(offset, mmio->line_size, &line_offset);
+			c = min_t(size_t, len, mmio->line_size - line_offset);
+		} else {
+			offset = base_offset + nfit_blk->bdw_offset;
+			c = len;
+		}
+
+		if (rw)
+			memcpy_to_pmem(mmio->aperture + offset,
+					iobuf + copied, c);
+		else
+			memcpy_from_pmem(iobuf + copied,
+					mmio->aperture + offset, c);
+
+		copied += c;
+		len -= c;
+	}
+
+	if (rw)
+		wmb_blk(nfit_blk);
+
+	rc = read_blk_stat(nfit_blk, lane) ? -EIO : 0;
+	return rc;
+}
+
+static int acpi_nfit_blk_region_do_io(struct nd_blk_region *ndbr,
+		resource_size_t dpa, void *iobuf, u64 len, int rw)
+{
+	struct nfit_blk *nfit_blk = nd_blk_region_provider_data(ndbr);
+	struct nfit_blk_mmio *mmio = &nfit_blk->mmio[BDW];
+	struct nd_region *nd_region = nfit_blk->nd_region;
+	unsigned int lane, copied = 0;
+	int rc = 0;
+
+	lane = nd_region_acquire_lane(nd_region);
+	while (len) {
+		u64 c = min(len, mmio->size);
+
+		rc = acpi_nfit_blk_single_io(nfit_blk, dpa + copied,
+				iobuf + copied, c, rw, lane);
+		if (rc)
+			break;
+
+		copied += c;
+		len -= c;
+	}
+	nd_region_release_lane(nd_region, lane);
+
+	return rc;
+}
+
+static void nfit_spa_mapping_release(struct kref *kref)
+{
+	struct nfit_spa_mapping *spa_map = to_spa_map(kref);
+	struct acpi_nfit_system_address *spa = spa_map->spa;
+	struct acpi_nfit_desc *acpi_desc = spa_map->acpi_desc;
+
+	WARN_ON(!mutex_is_locked(&acpi_desc->spa_map_mutex));
+	dev_dbg(acpi_desc->dev, "%s: SPA%d\n", __func__, spa->range_index);
+	iounmap(spa_map->iomem);
+	release_mem_region(spa->address, spa->length);
+	list_del(&spa_map->list);
+	kfree(spa_map);
+}
+
+static struct nfit_spa_mapping *find_spa_mapping(
+		struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_system_address *spa)
+{
+	struct nfit_spa_mapping *spa_map;
+
+	WARN_ON(!mutex_is_locked(&acpi_desc->spa_map_mutex));
+	list_for_each_entry(spa_map, &acpi_desc->spa_maps, list)
+		if (spa_map->spa == spa)
+			return spa_map;
+
+	return NULL;
+}
+
+static void nfit_spa_unmap(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_system_address *spa)
+{
+	struct nfit_spa_mapping *spa_map;
+
+	mutex_lock(&acpi_desc->spa_map_mutex);
+	spa_map = find_spa_mapping(acpi_desc, spa);
+
+	if (spa_map)
+		kref_put(&spa_map->kref, nfit_spa_mapping_release);
+	mutex_unlock(&acpi_desc->spa_map_mutex);
+}
+
+static void __iomem *__nfit_spa_map(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_system_address *spa, enum spa_map_type type)
+{
+	resource_size_t start = spa->address;
+	resource_size_t n = spa->length;
+	struct nfit_spa_mapping *spa_map;
+	struct resource *res;
+
+	WARN_ON(!mutex_is_locked(&acpi_desc->spa_map_mutex));
+
+	spa_map = find_spa_mapping(acpi_desc, spa);
+	if (spa_map) {
+		kref_get(&spa_map->kref);
+		return spa_map->iomem;
+	}
+
+	spa_map = kzalloc(sizeof(*spa_map), GFP_KERNEL);
+	if (!spa_map)
+		return NULL;
+
+	INIT_LIST_HEAD(&spa_map->list);
+	spa_map->spa = spa;
+	kref_init(&spa_map->kref);
+	spa_map->acpi_desc = acpi_desc;
+
+	res = request_mem_region(start, n, dev_name(acpi_desc->dev));
+	if (!res)
+		goto err_mem;
+
+	if (type == SPA_MAP_APERTURE) {
+		/*
+		 * TODO: memremap_pmem() support, but that requires cache
+		 * flushing when the aperture is moved.
+		 */
+		spa_map->iomem = ioremap_wc(start, n);
+	} else
+		spa_map->iomem = ioremap_nocache(start, n);
+
+	if (!spa_map->iomem)
+		goto err_map;
+
+	list_add_tail(&spa_map->list, &acpi_desc->spa_maps);
+	return spa_map->iomem;
+
+ err_map:
+	release_mem_region(start, n);
+ err_mem:
+	kfree(spa_map);
+	return NULL;
+}
+
+/**
+ * nfit_spa_map - interleave-aware managed-mappings of acpi_nfit_system_address ranges
+ * @nvdimm_bus: NFIT-bus that provided the spa table entry
+ * @nfit_spa: spa table to map
+ * @type: aperture or control region
+ *
+ * In the case where block-data-window apertures and
+ * dimm-control-regions are interleaved they will end up sharing a
+ * single request_mem_region() + ioremap() for the address range.  In
+ * the style of devm nfit_spa_map() mappings are automatically dropped
+ * when all region devices referencing the same mapping are disabled /
+ * unbound.
+ */
+static void __iomem *nfit_spa_map(struct acpi_nfit_desc *acpi_desc,
+		struct acpi_nfit_system_address *spa, enum spa_map_type type)
+{
+	void __iomem *iomem;
+
+	mutex_lock(&acpi_desc->spa_map_mutex);
+	iomem = __nfit_spa_map(acpi_desc, spa, type);
+	mutex_unlock(&acpi_desc->spa_map_mutex);
+
+	return iomem;
+}
+
+static int nfit_blk_init_interleave(struct nfit_blk_mmio *mmio,
+		struct acpi_nfit_interleave *idt, u16 interleave_ways)
+{
+	if (idt) {
+		mmio->num_lines = idt->line_count;
+		mmio->line_size = idt->line_size;
+		if (interleave_ways == 0)
+			return -ENXIO;
+		mmio->table_size = mmio->num_lines * interleave_ways
+			* mmio->line_size;
+	}
+
+	return 0;
+}
+
+static int acpi_nfit_blk_get_flags(struct nvdimm_bus_descriptor *nd_desc,
+		struct nvdimm *nvdimm, struct nfit_blk *nfit_blk)
+{
+	struct nd_cmd_dimm_flags flags;
+	int rc;
+
+	memset(&flags, 0, sizeof(flags));
+	rc = nd_desc->ndctl(nd_desc, nvdimm, ND_CMD_DIMM_FLAGS, &flags,
+			sizeof(flags));
+
+	if (rc >= 0 && flags.status == 0)
+		nfit_blk->dimm_flags = flags.flags;
+	else if (rc == -ENOTTY) {
+		/* fall back to a conservative default */
+		nfit_blk->dimm_flags = ND_BLK_DCR_LATCH;
+		rc = 0;
+	} else
+		rc = -ENXIO;
+
+	return rc;
+}
+
+static int acpi_nfit_blk_region_enable(struct nvdimm_bus *nvdimm_bus,
+		struct device *dev)
+{
+	struct nvdimm_bus_descriptor *nd_desc = to_nd_desc(nvdimm_bus);
+	struct acpi_nfit_desc *acpi_desc = to_acpi_desc(nd_desc);
+	struct nd_blk_region *ndbr = to_nd_blk_region(dev);
+	struct nfit_flush *nfit_flush;
+	struct nfit_blk_mmio *mmio;
+	struct nfit_blk *nfit_blk;
+	struct nfit_mem *nfit_mem;
+	struct nvdimm *nvdimm;
+	int rc;
+
+	nvdimm = nd_blk_region_to_dimm(ndbr);
+	nfit_mem = nvdimm_provider_data(nvdimm);
+	if (!nfit_mem || !nfit_mem->dcr || !nfit_mem->bdw) {
+		dev_dbg(dev, "%s: missing%s%s%s\n", __func__,
+				nfit_mem ? "" : " nfit_mem",
+				(nfit_mem && nfit_mem->dcr) ? "" : " dcr",
+				(nfit_mem && nfit_mem->bdw) ? "" : " bdw");
+		return -ENXIO;
+	}
+
+	nfit_blk = devm_kzalloc(dev, sizeof(*nfit_blk), GFP_KERNEL);
+	if (!nfit_blk)
+		return -ENOMEM;
+	nd_blk_region_set_provider_data(ndbr, nfit_blk);
+	nfit_blk->nd_region = to_nd_region(dev);
+
+	/* map block aperture memory */
+	nfit_blk->bdw_offset = nfit_mem->bdw->offset;
+	mmio = &nfit_blk->mmio[BDW];
+	mmio->base = nfit_spa_map(acpi_desc, nfit_mem->spa_bdw,
+			SPA_MAP_APERTURE);
+	if (!mmio->base) {
+		dev_dbg(dev, "%s: %s failed to map bdw\n", __func__,
+				nvdimm_name(nvdimm));
+		return -ENOMEM;
+	}
+	mmio->size = nfit_mem->bdw->size;
+	mmio->base_offset = nfit_mem->memdev_bdw->region_offset;
+	mmio->idt = nfit_mem->idt_bdw;
+	mmio->spa = nfit_mem->spa_bdw;
+	rc = nfit_blk_init_interleave(mmio, nfit_mem->idt_bdw,
+			nfit_mem->memdev_bdw->interleave_ways);
+	if (rc) {
+		dev_dbg(dev, "%s: %s failed to init bdw interleave\n",
+				__func__, nvdimm_name(nvdimm));
+		return rc;
+	}
+
+	/* map block control memory */
+	nfit_blk->cmd_offset = nfit_mem->dcr->command_offset;
+	nfit_blk->stat_offset = nfit_mem->dcr->status_offset;
+	mmio = &nfit_blk->mmio[DCR];
+	mmio->base = nfit_spa_map(acpi_desc, nfit_mem->spa_dcr,
+			SPA_MAP_CONTROL);
+	if (!mmio->base) {
+		dev_dbg(dev, "%s: %s failed to map dcr\n", __func__,
+				nvdimm_name(nvdimm));
+		return -ENOMEM;
+	}
+	mmio->size = nfit_mem->dcr->window_size;
+	mmio->base_offset = nfit_mem->memdev_dcr->region_offset;
+	mmio->idt = nfit_mem->idt_dcr;
+	mmio->spa = nfit_mem->spa_dcr;
+	rc = nfit_blk_init_interleave(mmio, nfit_mem->idt_dcr,
+			nfit_mem->memdev_dcr->interleave_ways);
+	if (rc) {
+		dev_dbg(dev, "%s: %s failed to init dcr interleave\n",
+				__func__, nvdimm_name(nvdimm));
+		return rc;
+	}
+
+	rc = acpi_nfit_blk_get_flags(nd_desc, nvdimm, nfit_blk);
+	if (rc < 0) {
+		dev_dbg(dev, "%s: %s failed get DIMM flags\n",
+				__func__, nvdimm_name(nvdimm));
+		return rc;
+	}
+
+	nfit_flush = nfit_mem->nfit_flush;
+	if (nfit_flush && nfit_flush->flush->hint_count != 0) {
+		nfit_blk->nvdimm_flush = devm_ioremap_nocache(dev,
+				nfit_flush->flush->hint_address[0], 8);
+		if (!nfit_blk->nvdimm_flush)
+			return -ENOMEM;
+	}
+
+	if (!arch_has_pmem_api() && !nfit_blk->nvdimm_flush)
+		dev_warn(dev, "unable to guarantee persistence of writes\n");
+
+	if (mmio->line_size == 0)
+		return 0;
+
+	if ((u32) nfit_blk->cmd_offset % mmio->line_size
+			+ 8 > mmio->line_size) {
+		dev_dbg(dev, "cmd_offset crosses interleave boundary\n");
+		return -ENXIO;
+	} else if ((u32) nfit_blk->stat_offset % mmio->line_size
+			+ 8 > mmio->line_size) {
+		dev_dbg(dev, "stat_offset crosses interleave boundary\n");
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static void acpi_nfit_blk_region_disable(struct nvdimm_bus *nvdimm_bus,
+		struct device *dev)
+{
+	struct nvdimm_bus_descriptor *nd_desc = to_nd_desc(nvdimm_bus);
+	struct acpi_nfit_desc *acpi_desc = to_acpi_desc(nd_desc);
+	struct nd_blk_region *ndbr = to_nd_blk_region(dev);
+	struct nfit_blk *nfit_blk = nd_blk_region_provider_data(ndbr);
+	int i;
+
+	if (!nfit_blk)
+		return; /* never enabled */
+
+	/* auto-free BLK spa mappings */
+	for (i = 0; i < 2; i++) {
+		struct nfit_blk_mmio *mmio = &nfit_blk->mmio[i];
+
+		if (mmio->base)
+			nfit_spa_unmap(acpi_desc, mmio->spa);
+	}
+	nd_blk_region_set_provider_data(ndbr, NULL);
+	/* devm will free nfit_blk */
+}
+
+static int acpi_nfit_init_mapping(struct acpi_nfit_desc *acpi_desc,
+		struct nd_mapping *nd_mapping, struct nd_region_desc *ndr_desc,
+		struct acpi_nfit_memory_map *memdev,
+		struct acpi_nfit_system_address *spa)
+{
+	struct nvdimm *nvdimm = acpi_nfit_dimm_by_handle(acpi_desc,
+			memdev->device_handle);
+	struct nd_blk_region_desc *ndbr_desc;
+	struct nfit_mem *nfit_mem;
+	int blk_valid = 0;
+
+	if (!nvdimm) {
+		dev_err(acpi_desc->dev, "spa%d dimm: %#x not found\n",
+				spa->range_index, memdev->device_handle);
+		return -ENODEV;
+	}
+
+	nd_mapping->nvdimm = nvdimm;
+	switch (nfit_spa_type(spa)) {
+	case NFIT_SPA_PM:
+	case NFIT_SPA_VOLATILE:
+		nd_mapping->start = memdev->address;
+		nd_mapping->size = memdev->region_size;
+		break;
+	case NFIT_SPA_DCR:
+		nfit_mem = nvdimm_provider_data(nvdimm);
+		if (!nfit_mem || !nfit_mem->bdw) {
+			dev_dbg(acpi_desc->dev, "spa%d %s missing bdw\n",
+					spa->range_index, nvdimm_name(nvdimm));
+		} else {
+			nd_mapping->size = nfit_mem->bdw->capacity;
+			nd_mapping->start = nfit_mem->bdw->start_address;
+			ndr_desc->num_lanes = nfit_mem->bdw->windows;
+			blk_valid = 1;
+		}
+
+		ndr_desc->nd_mapping = nd_mapping;
+		ndr_desc->num_mappings = blk_valid;
+		ndbr_desc = to_blk_region_desc(ndr_desc);
+		ndbr_desc->enable = acpi_nfit_blk_region_enable;
+		ndbr_desc->disable = acpi_nfit_blk_region_disable;
+		ndbr_desc->do_io = acpi_desc->blk_do_io;
+		if (!nvdimm_blk_region_create(acpi_desc->nvdimm_bus, ndr_desc))
+			return -ENOMEM;
+		break;
+	}
+
+	return 0;
+}
+
+static int acpi_nfit_register_region(struct acpi_nfit_desc *acpi_desc,
+		struct nfit_spa *nfit_spa)
+{
+	static struct nd_mapping nd_mappings[ND_MAX_MAPPINGS];
+	struct acpi_nfit_system_address *spa = nfit_spa->spa;
+	struct nd_blk_region_desc ndbr_desc;
+	struct nd_region_desc *ndr_desc;
+	struct nfit_memdev *nfit_memdev;
+	struct nvdimm_bus *nvdimm_bus;
+	struct resource res;
+	int count = 0, rc;
+
+	if (spa->range_index == 0) {
+		dev_dbg(acpi_desc->dev, "%s: detected invalid spa index\n",
+				__func__);
+		return 0;
+	}
+
+	memset(&res, 0, sizeof(res));
+	memset(&nd_mappings, 0, sizeof(nd_mappings));
+	memset(&ndbr_desc, 0, sizeof(ndbr_desc));
+	res.start = spa->address;
+	res.end = res.start + spa->length - 1;
+	ndr_desc = &ndbr_desc.ndr_desc;
+	ndr_desc->res = &res;
+	ndr_desc->provider_data = nfit_spa;
+	ndr_desc->attr_groups = acpi_nfit_region_attribute_groups;
+	if (spa->flags & ACPI_NFIT_PROXIMITY_VALID)
+		ndr_desc->numa_node = acpi_map_pxm_to_online_node(
+						spa->proximity_domain);
+	else
+		ndr_desc->numa_node = NUMA_NO_NODE;
+
+	list_for_each_entry(nfit_memdev, &acpi_desc->memdevs, list) {
+		struct acpi_nfit_memory_map *memdev = nfit_memdev->memdev;
+		struct nd_mapping *nd_mapping;
+
+		if (memdev->range_index != spa->range_index)
+			continue;
+		if (count >= ND_MAX_MAPPINGS) {
+			dev_err(acpi_desc->dev, "spa%d exceeds max mappings %d\n",
+					spa->range_index, ND_MAX_MAPPINGS);
+			return -ENXIO;
+		}
+		nd_mapping = &nd_mappings[count++];
+		rc = acpi_nfit_init_mapping(acpi_desc, nd_mapping, ndr_desc,
+				memdev, spa);
+		if (rc)
+			return rc;
+	}
+
+	ndr_desc->nd_mapping = nd_mappings;
+	ndr_desc->num_mappings = count;
+	rc = acpi_nfit_init_interleave_set(acpi_desc, ndr_desc, spa);
+	if (rc)
+		return rc;
+
+	nvdimm_bus = acpi_desc->nvdimm_bus;
+	if (nfit_spa_type(spa) == NFIT_SPA_PM) {
+		if (!nvdimm_pmem_region_create(nvdimm_bus, ndr_desc))
+			return -ENOMEM;
+	} else if (nfit_spa_type(spa) == NFIT_SPA_VOLATILE) {
+		if (!nvdimm_volatile_region_create(nvdimm_bus, ndr_desc))
+			return -ENOMEM;
+	}
+	return 0;
+}
+
+static int acpi_nfit_register_regions(struct acpi_nfit_desc *acpi_desc)
+{
+	struct nfit_spa *nfit_spa;
+
+	list_for_each_entry(nfit_spa, &acpi_desc->spas, list) {
+		int rc = acpi_nfit_register_region(acpi_desc, nfit_spa);
+
+		if (rc)
+			return rc;
+	}
+	return 0;
+}
+
+int acpi_nfit_init(struct acpi_nfit_desc *acpi_desc, acpi_size sz)
+{
+	struct device *dev = acpi_desc->dev;
+	const void *end;
+	u8 *data;
+	int rc;
+
+	INIT_LIST_HEAD(&acpi_desc->spa_maps);
+	INIT_LIST_HEAD(&acpi_desc->spas);
+	INIT_LIST_HEAD(&acpi_desc->dcrs);
+	INIT_LIST_HEAD(&acpi_desc->bdws);
+	INIT_LIST_HEAD(&acpi_desc->idts);
+	INIT_LIST_HEAD(&acpi_desc->flushes);
+	INIT_LIST_HEAD(&acpi_desc->memdevs);
+	INIT_LIST_HEAD(&acpi_desc->dimms);
+	mutex_init(&acpi_desc->spa_map_mutex);
+
+	data = (u8 *) acpi_desc->nfit;
+	end = data + sz;
+	data += sizeof(struct acpi_table_nfit);
+	while (!IS_ERR_OR_NULL(data))
+		data = add_table(acpi_desc, data, end);
+
+	if (IS_ERR(data)) {
+		dev_dbg(dev, "%s: nfit table parsing error: %ld\n", __func__,
+				PTR_ERR(data));
+		return PTR_ERR(data);
+	}
+
+	if (nfit_mem_init(acpi_desc) != 0)
+		return -ENOMEM;
+
+	acpi_nfit_init_dsms(acpi_desc);
+
+	rc = acpi_nfit_register_dimms(acpi_desc);
+	if (rc)
+		return rc;
+
+	return acpi_nfit_register_regions(acpi_desc);
+}
+EXPORT_SYMBOL_GPL(acpi_nfit_init);
+
+static int acpi_nfit_add(struct acpi_device *adev)
+{
+	struct nvdimm_bus_descriptor *nd_desc;
+	struct acpi_nfit_desc *acpi_desc;
+	struct device *dev = &adev->dev;
+	struct acpi_table_header *tbl;
+	acpi_status status = AE_OK;
+	acpi_size sz;
+	int rc;
+
+	status = acpi_get_table_with_size("NFIT", 0, &tbl, &sz);
+	if (ACPI_FAILURE(status)) {
+		dev_err(dev, "failed to find NFIT\n");
+		return -ENXIO;
+	}
+
+	acpi_desc = devm_kzalloc(dev, sizeof(*acpi_desc), GFP_KERNEL);
+	if (!acpi_desc)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, acpi_desc);
+	acpi_desc->dev = dev;
+	acpi_desc->nfit = (struct acpi_table_nfit *) tbl;
+	acpi_desc->blk_do_io = acpi_nfit_blk_region_do_io;
+	nd_desc = &acpi_desc->nd_desc;
+	nd_desc->provider_name = "ACPI.NFIT";
+	nd_desc->ndctl = acpi_nfit_ctl;
+	nd_desc->attr_groups = acpi_nfit_attribute_groups;
+
+	acpi_desc->nvdimm_bus = nvdimm_bus_register(dev, nd_desc);
+	if (!acpi_desc->nvdimm_bus)
+		return -ENXIO;
+
+	rc = acpi_nfit_init(acpi_desc, sz);
+	if (rc) {
+		nvdimm_bus_unregister(acpi_desc->nvdimm_bus);
+		return rc;
+	}
+	return 0;
+}
+
+static int acpi_nfit_remove(struct acpi_device *adev)
+{
+	struct acpi_nfit_desc *acpi_desc = dev_get_drvdata(&adev->dev);
+
+	nvdimm_bus_unregister(acpi_desc->nvdimm_bus);
+	return 0;
+}
+
+static const struct acpi_device_id acpi_nfit_ids[] = {
+	{ "ACPI0012", 0 },
+	{ "", 0 },
+};
+MODULE_DEVICE_TABLE(acpi, acpi_nfit_ids);
+
+static struct acpi_driver acpi_nfit_driver = {
+	.name = KBUILD_MODNAME,
+	.ids = acpi_nfit_ids,
+	.ops = {
+		.add = acpi_nfit_add,
+		.remove = acpi_nfit_remove,
+	},
+};
+
+static __init int nfit_init(void)
+{
+	BUILD_BUG_ON(sizeof(struct acpi_table_nfit) != 40);
+	BUILD_BUG_ON(sizeof(struct acpi_nfit_system_address) != 56);
+	BUILD_BUG_ON(sizeof(struct acpi_nfit_memory_map) != 48);
+	BUILD_BUG_ON(sizeof(struct acpi_nfit_interleave) != 20);
+	BUILD_BUG_ON(sizeof(struct acpi_nfit_smbios) != 9);
+	BUILD_BUG_ON(sizeof(struct acpi_nfit_control_region) != 80);
+	BUILD_BUG_ON(sizeof(struct acpi_nfit_data_region) != 40);
+
+	acpi_str_to_uuid(UUID_VOLATILE_MEMORY, nfit_uuid[NFIT_SPA_VOLATILE]);
+	acpi_str_to_uuid(UUID_PERSISTENT_MEMORY, nfit_uuid[NFIT_SPA_PM]);
+	acpi_str_to_uuid(UUID_CONTROL_REGION, nfit_uuid[NFIT_SPA_DCR]);
+	acpi_str_to_uuid(UUID_DATA_REGION, nfit_uuid[NFIT_SPA_BDW]);
+	acpi_str_to_uuid(UUID_VOLATILE_VIRTUAL_DISK, nfit_uuid[NFIT_SPA_VDISK]);
+	acpi_str_to_uuid(UUID_VOLATILE_VIRTUAL_CD, nfit_uuid[NFIT_SPA_VCD]);
+	acpi_str_to_uuid(UUID_PERSISTENT_VIRTUAL_DISK, nfit_uuid[NFIT_SPA_PDISK]);
+	acpi_str_to_uuid(UUID_PERSISTENT_VIRTUAL_CD, nfit_uuid[NFIT_SPA_PCD]);
+	acpi_str_to_uuid(UUID_NFIT_BUS, nfit_uuid[NFIT_DEV_BUS]);
+	acpi_str_to_uuid(UUID_NFIT_DIMM, nfit_uuid[NFIT_DEV_DIMM]);
+
+	return acpi_bus_register_driver(&acpi_nfit_driver);
+}
+
+static __exit void nfit_exit(void)
+{
+	acpi_bus_unregister_driver(&acpi_nfit_driver);
+}
+
+module_init(nfit_init);
+module_exit(nfit_exit);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Intel Corporation");
diff --git a/drivers/acpi/nfit.h b/drivers/acpi/nfit.h
new file mode 100644
index 000000000000..79b6d83875c1
--- /dev/null
+++ b/drivers/acpi/nfit.h
@@ -0,0 +1,176 @@
+/*
+ * NVDIMM Firmware Interface Table - NFIT
+ *
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#ifndef __NFIT_H__
+#define __NFIT_H__
+#include <linux/libnvdimm.h>
+#include <linux/types.h>
+#include <linux/uuid.h>
+#include <linux/acpi.h>
+#include <acpi/acuuid.h>
+
+#define UUID_NFIT_BUS "2f10e7a4-9e91-11e4-89d3-123b93f75cba"
+#define UUID_NFIT_DIMM "4309ac30-0d11-11e4-9191-0800200c9a66"
+#define ACPI_NFIT_MEM_FAILED_MASK (ACPI_NFIT_MEM_SAVE_FAILED \
+		| ACPI_NFIT_MEM_RESTORE_FAILED | ACPI_NFIT_MEM_FLUSH_FAILED \
+		| ACPI_NFIT_MEM_ARMED)
+
+enum nfit_uuids {
+	NFIT_SPA_VOLATILE,
+	NFIT_SPA_PM,
+	NFIT_SPA_DCR,
+	NFIT_SPA_BDW,
+	NFIT_SPA_VDISK,
+	NFIT_SPA_VCD,
+	NFIT_SPA_PDISK,
+	NFIT_SPA_PCD,
+	NFIT_DEV_BUS,
+	NFIT_DEV_DIMM,
+	NFIT_UUID_MAX,
+};
+
+enum {
+	ND_BLK_DCR_LATCH = 2,
+};
+
+struct nfit_spa {
+	struct acpi_nfit_system_address *spa;
+	struct list_head list;
+};
+
+struct nfit_dcr {
+	struct acpi_nfit_control_region *dcr;
+	struct list_head list;
+};
+
+struct nfit_bdw {
+	struct acpi_nfit_data_region *bdw;
+	struct list_head list;
+};
+
+struct nfit_idt {
+	struct acpi_nfit_interleave *idt;
+	struct list_head list;
+};
+
+struct nfit_flush {
+	struct acpi_nfit_flush_address *flush;
+	struct list_head list;
+};
+
+struct nfit_memdev {
+	struct acpi_nfit_memory_map *memdev;
+	struct list_head list;
+};
+
+/* assembled tables for a given dimm/memory-device */
+struct nfit_mem {
+	struct nvdimm *nvdimm;
+	struct acpi_nfit_memory_map *memdev_dcr;
+	struct acpi_nfit_memory_map *memdev_pmem;
+	struct acpi_nfit_memory_map *memdev_bdw;
+	struct acpi_nfit_control_region *dcr;
+	struct acpi_nfit_data_region *bdw;
+	struct acpi_nfit_system_address *spa_dcr;
+	struct acpi_nfit_system_address *spa_bdw;
+	struct acpi_nfit_interleave *idt_dcr;
+	struct acpi_nfit_interleave *idt_bdw;
+	struct nfit_flush *nfit_flush;
+	struct list_head list;
+	struct acpi_device *adev;
+	unsigned long dsm_mask;
+};
+
+struct acpi_nfit_desc {
+	struct nvdimm_bus_descriptor nd_desc;
+	struct acpi_table_nfit *nfit;
+	struct mutex spa_map_mutex;
+	struct list_head spa_maps;
+	struct list_head memdevs;
+	struct list_head flushes;
+	struct list_head dimms;
+	struct list_head spas;
+	struct list_head dcrs;
+	struct list_head bdws;
+	struct list_head idts;
+	struct nvdimm_bus *nvdimm_bus;
+	struct device *dev;
+	unsigned long dimm_dsm_force_en;
+	int (*blk_do_io)(struct nd_blk_region *ndbr, resource_size_t dpa,
+			void *iobuf, u64 len, int rw);
+};
+
+enum nd_blk_mmio_selector {
+	BDW,
+	DCR,
+};
+
+struct nfit_blk {
+	struct nfit_blk_mmio {
+		union {
+			void __iomem *base;
+			void __pmem  *aperture;
+		};
+		u64 size;
+		u64 base_offset;
+		u32 line_size;
+		u32 num_lines;
+		u32 table_size;
+		struct acpi_nfit_interleave *idt;
+		struct acpi_nfit_system_address *spa;
+	} mmio[2];
+	struct nd_region *nd_region;
+	u64 bdw_offset; /* post interleave offset */
+	u64 stat_offset;
+	u64 cmd_offset;
+	void __iomem *nvdimm_flush;
+	u32 dimm_flags;
+};
+
+enum spa_map_type {
+	SPA_MAP_CONTROL,
+	SPA_MAP_APERTURE,
+};
+
+struct nfit_spa_mapping {
+	struct acpi_nfit_desc *acpi_desc;
+	struct acpi_nfit_system_address *spa;
+	struct list_head list;
+	struct kref kref;
+	void __iomem *iomem;
+};
+
+static inline struct nfit_spa_mapping *to_spa_map(struct kref *kref)
+{
+	return container_of(kref, struct nfit_spa_mapping, kref);
+}
+
+static inline struct acpi_nfit_memory_map *__to_nfit_memdev(
+		struct nfit_mem *nfit_mem)
+{
+	if (nfit_mem->memdev_dcr)
+		return nfit_mem->memdev_dcr;
+	return nfit_mem->memdev_pmem;
+}
+
+static inline struct acpi_nfit_desc *to_acpi_desc(
+		struct nvdimm_bus_descriptor *nd_desc)
+{
+	return container_of(nd_desc, struct acpi_nfit_desc, nd_desc);
+}
+
+const u8 *to_nfit_uuid(enum nfit_uuids id);
+int acpi_nfit_init(struct acpi_nfit_desc *nfit, acpi_size sz);
+extern const struct attribute_group *acpi_nfit_attribute_groups[];
+#endif /* __NFIT_H__ */
diff --git a/drivers/acpi/numa.c b/drivers/acpi/numa.c
index 1333cbdc3ea2..acaa3b4ea504 100644
--- a/drivers/acpi/numa.c
+++ b/drivers/acpi/numa.c
@@ -29,6 +29,8 @@
 #include <linux/errno.h>
 #include <linux/acpi.h>
 #include <linux/numa.h>
+#include <linux/nodemask.h>
+#include <linux/topology.h>
 
 #define PREFIX "ACPI: "
 
@@ -70,7 +72,12 @@ static void __acpi_map_pxm_to_node(int pxm, int node)
 
 int acpi_map_pxm_to_node(int pxm)
 {
-	int node = pxm_to_node_map[pxm];
+	int node;
+
+	if (pxm < 0 || pxm >= MAX_PXM_DOMAINS)
+		return NUMA_NO_NODE;
+
+	node = pxm_to_node_map[pxm];
 
 	if (node == NUMA_NO_NODE) {
 		if (nodes_weight(nodes_found_map) >= MAX_NUMNODES)
@@ -83,6 +90,45 @@ int acpi_map_pxm_to_node(int pxm)
 	return node;
 }
 
+/**
+ * acpi_map_pxm_to_online_node - Map proximity ID to online node
+ * @pxm: ACPI proximity ID
+ *
+ * This is similar to acpi_map_pxm_to_node(), but always returns an online
+ * node.  When the mapped node from a given proximity ID is offline, it
+ * looks up the node distance table and returns the nearest online node.
+ *
+ * ACPI device drivers, which are called after the NUMA initialization has
+ * completed in the kernel, can call this interface to obtain their device
+ * NUMA topology from ACPI tables.  Such drivers do not have to deal with
+ * offline nodes.  A node may be offline when a device proximity ID is
+ * unique, SRAT memory entry does not exist, or NUMA is disabled, ex.
+ * "numa=off" on x86.
+ */
+int acpi_map_pxm_to_online_node(int pxm)
+{
+	int node, n, dist, min_dist;
+
+	node = acpi_map_pxm_to_node(pxm);
+
+	if (node == NUMA_NO_NODE)
+		node = 0;
+
+	if (!node_online(node)) {
+		min_dist = INT_MAX;
+		for_each_online_node(n) {
+			dist = node_distance(node, n);
+			if (dist < min_dist) {
+				min_dist = dist;
+				node = n;
+			}
+		}
+	}
+
+	return node;
+}
+EXPORT_SYMBOL(acpi_map_pxm_to_online_node);
+
 static void __init
 acpi_table_print_srat_entry(struct acpi_subtable_header *header)
 {
@@ -328,8 +374,6 @@ int acpi_get_node(acpi_handle handle)
 	int pxm;
 
 	pxm = acpi_get_pxm(handle);
-	if (pxm < 0 || pxm >= MAX_PXM_DOMAINS)
-		return NUMA_NO_NODE;
 
 	return acpi_map_pxm_to_node(pxm);
 }
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index 7ccba395c9dd..3b8963f21b36 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -182,7 +182,7 @@ static void __init acpi_request_region (struct acpi_generic_address *gas,
 		request_mem_region(addr, length, desc);
 }
 
-static void __init acpi_reserve_resources(void)
+static int __init acpi_reserve_resources(void)
 {
 	acpi_request_region(&acpi_gbl_FADT.xpm1a_event_block, acpi_gbl_FADT.pm1_event_length,
 		"ACPI PM1a_EVT_BLK");
@@ -211,7 +211,10 @@ static void __init acpi_reserve_resources(void)
 	if (!(acpi_gbl_FADT.gpe1_block_length & 0x1))
 		acpi_request_region(&acpi_gbl_FADT.xgpe1_block,
 			       acpi_gbl_FADT.gpe1_block_length, "ACPI GPE1_BLK");
+
+	return 0;
 }
+fs_initcall_sync(acpi_reserve_resources);
 
 void acpi_os_printf(const char *fmt, ...)
 {
@@ -534,13 +537,26 @@ acpi_os_get_physical_address(void *virt, acpi_physical_address * phys)
 }
 #endif
 
+#ifdef CONFIG_ACPI_REV_OVERRIDE_POSSIBLE
+static bool acpi_rev_override;
+
+int __init acpi_rev_override_setup(char *str)
+{
+	acpi_rev_override = true;
+	return 1;
+}
+__setup("acpi_rev_override", acpi_rev_override_setup);
+#else
+#define acpi_rev_override	false
+#endif
+
 #define ACPI_MAX_OVERRIDE_LEN 100
 
 static char acpi_os_name[ACPI_MAX_OVERRIDE_LEN];
 
 acpi_status
 acpi_os_predefined_override(const struct acpi_predefined_names *init_val,
-			    acpi_string * new_val)
+			    char **new_val)
 {
 	if (!init_val || !new_val)
 		return AE_BAD_PARAMETER;
@@ -552,6 +568,11 @@ acpi_os_predefined_override(const struct acpi_predefined_names *init_val,
 		*new_val = acpi_os_name;
 	}
 
+	if (!memcmp(init_val->name, "_REV", 4) && acpi_rev_override) {
+		printk(KERN_INFO PREFIX "Overriding _REV return value to 5\n");
+		*new_val = (char *)5;
+	}
+
 	return AE_OK;
 }
 
@@ -1684,6 +1705,12 @@ int acpi_resources_are_enforced(void)
 }
 EXPORT_SYMBOL(acpi_resources_are_enforced);
 
+bool acpi_osi_is_win8(void)
+{
+	return acpi_gbl_osi_data >= ACPI_OSI_WIN_8;
+}
+EXPORT_SYMBOL(acpi_osi_is_win8);
+
 /*
  * Deallocate the memory for a spinlock.
  */
@@ -1842,7 +1869,6 @@ acpi_status __init acpi_os_initialize(void)
 
 acpi_status __init acpi_os_initialize1(void)
 {
-	acpi_reserve_resources();
 	kacpid_wq = alloc_workqueue("kacpid", 0, 1);
 	kacpi_notify_wq = alloc_workqueue("kacpi_notify", 0, 1);
 	kacpi_hotplug_wq = alloc_ordered_workqueue("kacpi_hotplug", 0);
diff --git a/drivers/acpi/pci_irq.c b/drivers/acpi/pci_irq.c
index b1def411c0b8..304eccb0ae5c 100644
--- a/drivers/acpi/pci_irq.c
+++ b/drivers/acpi/pci_irq.c
@@ -44,7 +44,6 @@
 ACPI_MODULE_NAME("pci_irq");
 
 struct acpi_prt_entry {
-	struct list_head	list;
 	struct acpi_pci_id	id;
 	u8			pin;
 	acpi_handle		link;
@@ -163,7 +162,7 @@ static int acpi_pci_irq_check_entry(acpi_handle handle, struct pci_dev *dev,
 {
 	int segment = pci_domain_nr(dev->bus);
 	int bus = dev->bus->number;
-	int device = PCI_SLOT(dev->devfn);
+	int device = pci_ari_enabled(dev->bus) ? 0 : PCI_SLOT(dev->devfn);
 	struct acpi_prt_entry *entry;
 
 	if (((prt->address >> 16) & 0xffff) != device ||
diff --git a/drivers/acpi/power.c b/drivers/acpi/power.c
index e0bcfb642b52..93eac53b5110 100644
--- a/drivers/acpi/power.c
+++ b/drivers/acpi/power.c
@@ -684,7 +684,8 @@ int acpi_power_get_inferred_state(struct acpi_device *device, int *state)
 		}
 	}
 
-	*state = ACPI_STATE_D3_COLD;
+	*state = device->power.states[ACPI_STATE_D3_COLD].flags.valid ?
+		ACPI_STATE_D3_COLD : ACPI_STATE_D3_HOT;
 	return 0;
 }
 
@@ -710,8 +711,6 @@ int acpi_power_transition(struct acpi_device *device, int state)
 	    || (device->power.state > ACPI_STATE_D3_COLD))
 		return -ENODEV;
 
-	/* TBD: Resources must be ordered. */
-
 	/*
 	 * First we reference all power resources required in the target list
 	 * (e.g. so the device doesn't lose power while transitioning).  Then,
@@ -761,6 +760,25 @@ static void acpi_power_sysfs_remove(struct acpi_device *device)
 	device_remove_file(&device->dev, &dev_attr_resource_in_use);
 }
 
+static void acpi_power_add_resource_to_list(struct acpi_power_resource *resource)
+{
+	mutex_lock(&power_resource_list_lock);
+
+	if (!list_empty(&acpi_power_resource_list)) {
+		struct acpi_power_resource *r;
+
+		list_for_each_entry(r, &acpi_power_resource_list, list_node)
+			if (r->order > resource->order) {
+				list_add_tail(&resource->list_node, &r->list_node);
+				goto out;
+			}
+	}
+	list_add_tail(&resource->list_node, &acpi_power_resource_list);
+
+ out:
+	mutex_unlock(&power_resource_list_lock);
+}
+
 int acpi_add_power_resource(acpi_handle handle)
 {
 	struct acpi_power_resource *resource;
@@ -811,9 +829,7 @@ int acpi_add_power_resource(acpi_handle handle)
 	if (!device_create_file(&device->dev, &dev_attr_resource_in_use))
 		device->remove = acpi_power_sysfs_remove;
 
-	mutex_lock(&power_resource_list_lock);
-	list_add(&resource->list_node, &acpi_power_resource_list);
-	mutex_unlock(&power_resource_list_lock);
+	acpi_power_add_resource_to_list(resource);
 	acpi_device_add_finalize(device);
 	return 0;
 
@@ -844,7 +860,22 @@ void acpi_resume_power_resources(void)
 		    && resource->ref_count) {
 			dev_info(&resource->device.dev, "Turning ON\n");
 			__acpi_power_on(resource);
-		} else if (state == ACPI_POWER_RESOURCE_STATE_ON
+		}
+
+		mutex_unlock(&resource->resource_lock);
+	}
+	list_for_each_entry_reverse(resource, &acpi_power_resource_list, list_node) {
+		int result, state;
+
+		mutex_lock(&resource->resource_lock);
+
+		result = acpi_power_get_state(resource->device.handle, &state);
+		if (result) {
+			mutex_unlock(&resource->resource_lock);
+			continue;
+		}
+
+		if (state == ACPI_POWER_RESOURCE_STATE_ON
 		    && !resource->ref_count) {
 			dev_info(&resource->device.dev, "Turning OFF\n");
 			__acpi_power_off(resource);
diff --git a/drivers/acpi/processor_core.c b/drivers/acpi/processor_core.c
index b1ec78b8a645..33a38d604630 100644
--- a/drivers/acpi/processor_core.c
+++ b/drivers/acpi/processor_core.c
@@ -184,7 +184,7 @@ phys_cpuid_t acpi_get_phys_id(acpi_handle handle, int type, u32 acpi_id)
 	phys_cpuid_t phys_id;
 
 	phys_id = map_mat_entry(handle, type, acpi_id);
-	if (phys_id == PHYS_CPUID_INVALID)
+	if (invalid_phys_cpuid(phys_id))
 		phys_id = map_madt_entry(type, acpi_id);
 
 	return phys_id;
@@ -196,7 +196,7 @@ int acpi_map_cpuid(phys_cpuid_t phys_id, u32 acpi_id)
 	int i;
 #endif
 
-	if (phys_id == PHYS_CPUID_INVALID) {
+	if (invalid_phys_cpuid(phys_id)) {
 		/*
 		 * On UP processor, there is no _MAT or MADT table.
 		 * So above phys_id is always set to PHYS_CPUID_INVALID.
@@ -215,12 +215,12 @@ int acpi_map_cpuid(phys_cpuid_t phys_id, u32 acpi_id)
 		 * Ignores phys_id and always returns 0 for the processor
 		 * handle with acpi id 0 if nr_cpu_ids is 1.
 		 * This should be the case if SMP tables are not found.
-		 * Return -1 for other CPU's handle.
+		 * Return -EINVAL for other CPU's handle.
 		 */
 		if (nr_cpu_ids <= 1 && acpi_id == 0)
 			return acpi_id;
 		else
-			return -1;
+			return -EINVAL;
 	}
 
 #ifdef CONFIG_SMP
@@ -233,7 +233,7 @@ int acpi_map_cpuid(phys_cpuid_t phys_id, u32 acpi_id)
 	if (phys_id == 0)
 		return phys_id;
 #endif
-	return -1;
+	return -ENODEV;
 }
 
 int acpi_get_cpuid(acpi_handle handle, int type, u32 acpi_id)
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index 39e0c8e36244..d540f42c9232 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -94,7 +94,7 @@ static int set_max_cstate(const struct dmi_system_id *id)
 	return 0;
 }
 
-static struct dmi_system_id processor_power_dmi_table[] = {
+static const struct dmi_system_id processor_power_dmi_table[] = {
 	{ set_max_cstate, "Clevo 5600D", {
 	  DMI_MATCH(DMI_BIOS_VENDOR,"Phoenix Technologies LTD"),
 	  DMI_MATCH(DMI_BIOS_VERSION,"SHE845M0.86C.0013.D.0302131307")},
diff --git a/drivers/acpi/processor_pdc.c b/drivers/acpi/processor_pdc.c
index e5dd80800930..7cfbda4d7c51 100644
--- a/drivers/acpi/processor_pdc.c
+++ b/drivers/acpi/processor_pdc.c
@@ -52,10 +52,7 @@ static bool __init processor_physically_present(acpi_handle handle)
 	type = (acpi_type == ACPI_TYPE_DEVICE) ? 1 : 0;
 	cpuid = acpi_get_cpuid(handle, type, acpi_id);
 
-	if (cpuid == -1)
-		return false;
-
-	return true;
+	return !invalid_logical_cpuid(cpuid);
 }
 
 static void acpi_set_pdc_bits(u32 *buf)
diff --git a/drivers/acpi/property.c b/drivers/acpi/property.c
index 0d083736e25b..7836e2e980f4 100644
--- a/drivers/acpi/property.c
+++ b/drivers/acpi/property.c
@@ -79,50 +79,51 @@ static bool acpi_properties_format_valid(const union acpi_object *properties)
 static void acpi_init_of_compatible(struct acpi_device *adev)
 {
 	const union acpi_object *of_compatible;
-	struct acpi_hardware_id *hwid;
-	bool acpi_of = false;
 	int ret;
 
-	/*
-	 * Check if the special PRP0001 ACPI ID is present and in that
-	 * case we fill in Device Tree compatible properties for this
-	 * device.
-	 */
-	list_for_each_entry(hwid, &adev->pnp.ids, list) {
-		if (!strcmp(hwid->id, "PRP0001")) {
-			acpi_of = true;
-			break;
-		}
-	}
-
-	if (!acpi_of)
-		return;
-
 	ret = acpi_dev_get_property_array(adev, "compatible", ACPI_TYPE_STRING,
 					  &of_compatible);
 	if (ret) {
 		ret = acpi_dev_get_property(adev, "compatible",
 					    ACPI_TYPE_STRING, &of_compatible);
 		if (ret) {
-			acpi_handle_warn(adev->handle,
-					 "PRP0001 requires compatible property\n");
+			if (adev->parent
+			    && adev->parent->flags.of_compatible_ok)
+				goto out;
+
 			return;
 		}
 	}
 	adev->data.of_compatible = of_compatible;
+
+ out:
+	adev->flags.of_compatible_ok = 1;
 }
 
 void acpi_init_properties(struct acpi_device *adev)
 {
 	struct acpi_buffer buf = { ACPI_ALLOCATE_BUFFER };
+	bool acpi_of = false;
+	struct acpi_hardware_id *hwid;
 	const union acpi_object *desc;
 	acpi_status status;
 	int i;
 
+	/*
+	 * Check if ACPI_DT_NAMESPACE_HID is present and inthat case we fill in
+	 * Device Tree compatible properties for this device.
+	 */
+	list_for_each_entry(hwid, &adev->pnp.ids, list) {
+		if (!strcmp(hwid->id, ACPI_DT_NAMESPACE_HID)) {
+			acpi_of = true;
+			break;
+		}
+	}
+
 	status = acpi_evaluate_object_typed(adev->handle, "_DSD", NULL, &buf,
 					    ACPI_TYPE_PACKAGE);
 	if (ACPI_FAILURE(status))
-		return;
+		goto out;
 
 	desc = buf.pointer;
 	if (desc->package.count % 2)
@@ -156,13 +157,20 @@ void acpi_init_properties(struct acpi_device *adev)
 		adev->data.pointer = buf.pointer;
 		adev->data.properties = properties;
 
-		acpi_init_of_compatible(adev);
-		return;
+		if (acpi_of)
+			acpi_init_of_compatible(adev);
+
+		goto out;
 	}
 
  fail:
-	dev_warn(&adev->dev, "Returned _DSD data is not valid, skipping\n");
+	dev_dbg(&adev->dev, "Returned _DSD data is not valid, skipping\n");
 	ACPI_FREE(buf.pointer);
+
+ out:
+	if (acpi_of && !adev->flags.of_compatible_ok)
+		acpi_handle_info(adev->handle,
+			 ACPI_DT_NAMESPACE_HID " requires 'compatible' property\n");
 }
 
 void acpi_free_properties(struct acpi_device *adev)
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index 03141aa4ea95..ec256352f423 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -11,6 +11,7 @@
 #include <linux/kthread.h>
 #include <linux/dmi.h>
 #include <linux/nls.h>
+#include <linux/dma-mapping.h>
 
 #include <asm/pgtable.h>
 
@@ -135,12 +136,13 @@ static int create_pnp_modalias(struct acpi_device *acpi_dev, char *modalias,
 	struct acpi_hardware_id *id;
 
 	/*
-	 * Since we skip PRP0001 from the modalias below, 0 should be returned
-	 * if PRP0001 is the only ACPI/PNP ID in the device's list.
+	 * Since we skip ACPI_DT_NAMESPACE_HID from the modalias below, 0 should
+	 * be returned if ACPI_DT_NAMESPACE_HID is the only ACPI/PNP ID in the
+	 * device's list.
 	 */
 	count = 0;
 	list_for_each_entry(id, &acpi_dev->pnp.ids, list)
-		if (strcmp(id->id, "PRP0001"))
+		if (strcmp(id->id, ACPI_DT_NAMESPACE_HID))
 			count++;
 
 	if (!count)
@@ -153,7 +155,7 @@ static int create_pnp_modalias(struct acpi_device *acpi_dev, char *modalias,
 	size -= len;
 
 	list_for_each_entry(id, &acpi_dev->pnp.ids, list) {
-		if (!strcmp(id->id, "PRP0001"))
+		if (!strcmp(id->id, ACPI_DT_NAMESPACE_HID))
 			continue;
 
 		count = snprintf(&modalias[len], size, "%s:", id->id);
@@ -177,7 +179,8 @@ static int create_pnp_modalias(struct acpi_device *acpi_dev, char *modalias,
  * @size: Size of the buffer.
  *
  * Expose DT compatible modalias as of:NnameTCcompatible.  This function should
- * only be called for devices having PRP0001 in their list of ACPI/PNP IDs.
+ * only be called for devices having ACPI_DT_NAMESPACE_HID in their list of
+ * ACPI/PNP IDs.
  */
 static int create_of_modalias(struct acpi_device *acpi_dev, char *modalias,
 			      int size)
@@ -980,9 +983,9 @@ static void acpi_device_remove_files(struct acpi_device *dev)
  * @adev: ACPI device object to match.
  * @of_match_table: List of device IDs to match against.
  *
- * If @dev has an ACPI companion which has the special PRP0001 device ID in its
- * list of identifiers and a _DSD object with the "compatible" property, use
- * that property to match against the given list of identifiers.
+ * If @dev has an ACPI companion which has ACPI_DT_NAMESPACE_HID in its list of
+ * identifiers and a _DSD object with the "compatible" property, use that
+ * property to match against the given list of identifiers.
  */
 static bool acpi_of_match_device(struct acpi_device *adev,
 				 const struct of_device_id *of_match_table)
@@ -1016,6 +1019,29 @@ static bool acpi_of_match_device(struct acpi_device *adev,
 	return false;
 }
 
+static bool __acpi_match_device_cls(const struct acpi_device_id *id,
+				    struct acpi_hardware_id *hwid)
+{
+	int i, msk, byte_shift;
+	char buf[3];
+
+	if (!id->cls)
+		return false;
+
+	/* Apply class-code bitmask, before checking each class-code byte */
+	for (i = 1; i <= 3; i++) {
+		byte_shift = 8 * (3 - i);
+		msk = (id->cls_msk >> byte_shift) & 0xFF;
+		if (!msk)
+			continue;
+
+		sprintf(buf, "%02x", (id->cls >> byte_shift) & msk);
+		if (strncmp(buf, &hwid->id[(i - 1) * 2], 2))
+			return false;
+	}
+	return true;
+}
+
 static const struct acpi_device_id *__acpi_match_device(
 	struct acpi_device *device,
 	const struct acpi_device_id *ids,
@@ -1033,19 +1059,22 @@ static const struct acpi_device_id *__acpi_match_device(
 
 	list_for_each_entry(hwid, &device->pnp.ids, list) {
 		/* First, check the ACPI/PNP IDs provided by the caller. */
-		for (id = ids; id->id[0]; id++)
-			if (!strcmp((char *) id->id, hwid->id))
+		for (id = ids; id->id[0] || id->cls; id++) {
+			if (id->id[0] && !strcmp((char *) id->id, hwid->id))
 				return id;
+			else if (id->cls && __acpi_match_device_cls(id, hwid))
+				return id;
+		}
 
 		/*
-		 * Next, check the special "PRP0001" ID and try to match the
+		 * Next, check ACPI_DT_NAMESPACE_HID and try to match the
 		 * "compatible" property if found.
 		 *
 		 * The id returned by the below is not valid, but the only
 		 * caller passing non-NULL of_ids here is only interested in
 		 * whether or not the return value is NULL.
 		 */
-		if (!strcmp("PRP0001", hwid->id)
+		if (!strcmp(ACPI_DT_NAMESPACE_HID, hwid->id)
 		    && acpi_of_match_device(device, of_ids))
 			return id;
 	}
@@ -1671,7 +1700,7 @@ static int acpi_bus_extract_wakeup_device_power_package(acpi_handle handle,
 
 static void acpi_wakeup_gpe_init(struct acpi_device *device)
 {
-	struct acpi_device_id button_device_ids[] = {
+	static const struct acpi_device_id button_device_ids[] = {
 		{"PNP0C0C", 0},
 		{"PNP0C0D", 0},
 		{"PNP0C0E", 0},
@@ -1766,15 +1795,9 @@ static void acpi_bus_init_power_state(struct acpi_device *device, int state)
 	if (acpi_has_method(device->handle, pathname))
 		ps->flags.explicit_set = 1;
 
-	/*
-	 * State is valid if there are means to put the device into it.
-	 * D3hot is only valid if _PR3 present.
-	 */
-	if (!list_empty(&ps->resources)
-	    || (ps->flags.explicit_set && state < ACPI_STATE_D3_HOT)) {
+	/* State is valid if there are means to put the device into it. */
+	if (!list_empty(&ps->resources) || ps->flags.explicit_set)
 		ps->flags.valid = 1;
-		ps->flags.os_accessible = 1;
-	}
 
 	ps->power = -1;		/* Unknown - driver assigned */
 	ps->latency = -1;	/* Unknown - driver assigned */
@@ -1810,21 +1833,13 @@ static void acpi_bus_get_power_flags(struct acpi_device *device)
 		acpi_bus_init_power_state(device, i);
 
 	INIT_LIST_HEAD(&device->power.states[ACPI_STATE_D3_COLD].resources);
+	if (!list_empty(&device->power.states[ACPI_STATE_D3_HOT].resources))
+		device->power.states[ACPI_STATE_D3_COLD].flags.valid = 1;
 
-	/* Set defaults for D0 and D3 states (always valid) */
+	/* Set defaults for D0 and D3hot states (always valid) */
 	device->power.states[ACPI_STATE_D0].flags.valid = 1;
 	device->power.states[ACPI_STATE_D0].power = 100;
-	device->power.states[ACPI_STATE_D3_COLD].flags.valid = 1;
-	device->power.states[ACPI_STATE_D3_COLD].power = 0;
-
-	/* Set D3cold's explicit_set flag if _PS3 exists. */
-	if (device->power.states[ACPI_STATE_D3_HOT].flags.explicit_set)
-		device->power.states[ACPI_STATE_D3_COLD].flags.explicit_set = 1;
-
-	/* Presence of _PS3 or _PRx means we can put the device into D3 cold */
-	if (device->power.states[ACPI_STATE_D3_HOT].flags.explicit_set ||
-			device->power.flags.power_resources)
-		device->power.states[ACPI_STATE_D3_COLD].flags.os_accessible = 1;
+	device->power.states[ACPI_STATE_D3_HOT].flags.valid = 1;
 
 	if (acpi_bus_init_power(device))
 		device->flags.power_manageable = 0;
@@ -1947,6 +1962,62 @@ bool acpi_dock_match(acpi_handle handle)
 	return acpi_has_method(handle, "_DCK");
 }
 
+static acpi_status
+acpi_backlight_cap_match(acpi_handle handle, u32 level, void *context,
+			  void **return_value)
+{
+	long *cap = context;
+
+	if (acpi_has_method(handle, "_BCM") &&
+	    acpi_has_method(handle, "_BCL")) {
+		ACPI_DEBUG_PRINT((ACPI_DB_INFO, "Found generic backlight "
+				  "support\n"));
+		*cap |= ACPI_VIDEO_BACKLIGHT;
+		if (!acpi_has_method(handle, "_BQC"))
+			printk(KERN_WARNING FW_BUG PREFIX "No _BQC method, "
+				"cannot determine initial brightness\n");
+		/* We have backlight support, no need to scan further */
+		return AE_CTRL_TERMINATE;
+	}
+	return 0;
+}
+
+/* Returns true if the ACPI object is a video device which can be
+ * handled by video.ko.
+ * The device will get a Linux specific CID added in scan.c to
+ * identify the device as an ACPI graphics device
+ * Be aware that the graphics device may not be physically present
+ * Use acpi_video_get_capabilities() to detect general ACPI video
+ * capabilities of present cards
+ */
+long acpi_is_video_device(acpi_handle handle)
+{
+	long video_caps = 0;
+
+	/* Is this device able to support video switching ? */
+	if (acpi_has_method(handle, "_DOD") || acpi_has_method(handle, "_DOS"))
+		video_caps |= ACPI_VIDEO_OUTPUT_SWITCHING;
+
+	/* Is this device able to retrieve a video ROM ? */
+	if (acpi_has_method(handle, "_ROM"))
+		video_caps |= ACPI_VIDEO_ROM_AVAILABLE;
+
+	/* Is this device able to configure which video head to be POSTed ? */
+	if (acpi_has_method(handle, "_VPO") &&
+	    acpi_has_method(handle, "_GPD") &&
+	    acpi_has_method(handle, "_SPD"))
+		video_caps |= ACPI_VIDEO_DEVICE_POSTING;
+
+	/* Only check for backlight functionality if one of the above hit. */
+	if (video_caps)
+		acpi_walk_namespace(ACPI_TYPE_DEVICE, handle,
+				    ACPI_UINT32_MAX, acpi_backlight_cap_match, NULL,
+				    &video_caps, NULL);
+
+	return video_caps;
+}
+EXPORT_SYMBOL(acpi_is_video_device);
+
 const char *acpi_device_hid(struct acpi_device *device)
 {
 	struct acpi_hardware_id *hid;
@@ -2056,6 +2127,8 @@ static void acpi_set_pnp_ids(acpi_handle handle, struct acpi_device_pnp *pnp,
 		if (info->valid & ACPI_VALID_UID)
 			pnp->unique_id = kstrdup(info->unique_id.string,
 							GFP_KERNEL);
+		if (info->valid & ACPI_VALID_CLS)
+			acpi_add_id(pnp, info->class_code.string);
 
 		kfree(info);
 
@@ -2109,6 +2182,39 @@ void acpi_free_pnp_ids(struct acpi_device_pnp *pnp)
 	kfree(pnp->unique_id);
 }
 
+static void acpi_init_coherency(struct acpi_device *adev)
+{
+	unsigned long long cca = 0;
+	acpi_status status;
+	struct acpi_device *parent = adev->parent;
+
+	if (parent && parent->flags.cca_seen) {
+		/*
+		 * From ACPI spec, OSPM will ignore _CCA if an ancestor
+		 * already saw one.
+		 */
+		adev->flags.cca_seen = 1;
+		cca = parent->flags.coherent_dma;
+	} else {
+		status = acpi_evaluate_integer(adev->handle, "_CCA",
+					       NULL, &cca);
+		if (ACPI_SUCCESS(status))
+			adev->flags.cca_seen = 1;
+		else if (!IS_ENABLED(CONFIG_ACPI_CCA_REQUIRED))
+			/*
+			 * If architecture does not specify that _CCA is
+			 * required for DMA-able devices (e.g. x86),
+			 * we default to _CCA=1.
+			 */
+			cca = 1;
+		else
+			acpi_handle_debug(adev->handle,
+					  "ACPI device is missing _CCA.\n");
+	}
+
+	adev->flags.coherent_dma = cca;
+}
+
 void acpi_init_device_object(struct acpi_device *device, acpi_handle handle,
 			     int type, unsigned long long sta)
 {
@@ -2127,6 +2233,7 @@ void acpi_init_device_object(struct acpi_device *device, acpi_handle handle,
 	device->flags.visited = false;
 	device_initialize(&device->dev);
 	dev_set_uevent_suppress(&device->dev, true);
+	acpi_init_coherency(device);
 }
 
 void acpi_device_add_finalize(struct acpi_device *device)
@@ -2405,7 +2512,7 @@ static void acpi_default_enumeration(struct acpi_device *device)
 }
 
 static const struct acpi_device_id generic_device_ids[] = {
-	{"PRP0001", },
+	{ACPI_DT_NAMESPACE_HID, },
 	{"", },
 };
 
@@ -2413,8 +2520,8 @@ static int acpi_generic_device_attach(struct acpi_device *adev,
 				      const struct acpi_device_id *not_used)
 {
 	/*
-	 * Since PRP0001 is the only ID handled here, the test below can be
-	 * unconditional.
+	 * Since ACPI_DT_NAMESPACE_HID is the only ID handled here, the test
+	 * below can be unconditional.
 	 */
 	if (adev->data.of_compatible)
 		acpi_default_enumeration(adev);
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index d24fa1964eb8..6d4e44ea74ac 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -800,7 +800,8 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
 				result =
 					thermal_zone_bind_cooling_device
 					(thermal, trip, cdev,
-					 THERMAL_NO_LIMIT, THERMAL_NO_LIMIT);
+					 THERMAL_NO_LIMIT, THERMAL_NO_LIMIT,
+					 THERMAL_WEIGHT_DEFAULT);
 			else
 				result =
 					thermal_zone_unbind_cooling_device
@@ -824,7 +825,8 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
 			if (bind)
 				result = thermal_zone_bind_cooling_device
 					(thermal, trip, cdev,
-					 THERMAL_NO_LIMIT, THERMAL_NO_LIMIT);
+					 THERMAL_NO_LIMIT, THERMAL_NO_LIMIT,
+					 THERMAL_WEIGHT_DEFAULT);
 			else
 				result = thermal_zone_unbind_cooling_device
 					(thermal, trip, cdev);
@@ -841,7 +843,8 @@ static int acpi_thermal_cooling_device_cb(struct thermal_zone_device *thermal,
 				result = thermal_zone_bind_cooling_device
 						(thermal, THERMAL_TRIPS_NONE,
 						 cdev, THERMAL_NO_LIMIT,
-						 THERMAL_NO_LIMIT);
+						 THERMAL_NO_LIMIT,
+						 THERMAL_WEIGHT_DEFAULT);
 			else
 				result = thermal_zone_unbind_cooling_device
 						(thermal, THERMAL_TRIPS_NONE,
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index cd49a3982b6a..67c548ad3764 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -712,3 +712,18 @@ bool acpi_check_dsm(acpi_handle handle, const u8 *uuid, int rev, u64 funcs)
 	return false;
 }
 EXPORT_SYMBOL(acpi_check_dsm);
+
+/*
+ * acpi_backlight= handling, this is done here rather then in video_detect.c
+ * because __setup cannot be used in modules.
+ */
+char acpi_video_backlight_string[16];
+EXPORT_SYMBOL(acpi_video_backlight_string);
+
+static int __init acpi_backlight(char *str)
+{
+	strlcpy(acpi_video_backlight_string, str,
+		sizeof(acpi_video_backlight_string));
+	return 1;
+}
+__setup("acpi_backlight=", acpi_backlight);
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index c42feb2bacd0..815f75ef2411 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -1,106 +1,61 @@
 /*
+ *  Copyright (C) 2015       Red Hat Inc.
+ *                           Hans de Goede <hdegoede@redhat.com>
  *  Copyright (C) 2008       SuSE Linux Products GmbH
  *                           Thomas Renninger <trenn@suse.de>
  *
  *  May be copied or modified under the terms of the GNU General Public License
  *
  * video_detect.c:
- * Provides acpi_is_video_device() for early scanning of ACPI devices in scan.c
- * There a Linux specific (Spec does not provide a HID for video devices) is
- * assigned
- *
  * After PCI devices are glued with ACPI devices
  * acpi_get_pci_dev() can be called to identify ACPI graphics
  * devices for which a real graphics card is plugged in
  *
- * Now acpi_video_get_capabilities() can be called to check which
- * capabilities the graphics cards plugged in support. The check for general
- * video capabilities will be triggered by the first caller of
- * acpi_video_get_capabilities(NULL); which will happen when the first
- * backlight switching supporting driver calls:
- * acpi_video_backlight_support();
- *
  * Depending on whether ACPI graphics extensions (cmp. ACPI spec Appendix B)
  * are available, video.ko should be used to handle the device.
  *
  * Otherwise vendor specific drivers like thinkpad_acpi, asus-laptop,
  * sony_acpi,... can take care about backlight brightness.
  *
- * If CONFIG_ACPI_VIDEO is neither set as "compiled in" (y) nor as a module (m)
- * this file will not be compiled, acpi_video_get_capabilities() and
- * acpi_video_backlight_support() will always return 0 and vendor specific
- * drivers always can handle backlight.
+ * Backlight drivers can use acpi_video_get_backlight_type() to determine
+ * which driver should handle the backlight.
  *
+ * If CONFIG_ACPI_VIDEO is neither set as "compiled in" (y) nor as a module (m)
+ * this file will not be compiled and acpi_video_get_backlight_type() will
+ * always return acpi_backlight_vendor.
  */
 
 #include <linux/export.h>
 #include <linux/acpi.h>
+#include <linux/backlight.h>
 #include <linux/dmi.h>
+#include <linux/module.h>
 #include <linux/pci.h>
-
-#include "internal.h"
+#include <linux/types.h>
+#include <acpi/video.h>
 
 ACPI_MODULE_NAME("video");
 #define _COMPONENT		ACPI_VIDEO_COMPONENT
 
-static long acpi_video_support;
-static bool acpi_video_caps_checked;
+void acpi_video_unregister_backlight(void);
 
-static acpi_status
-acpi_backlight_cap_match(acpi_handle handle, u32 level, void *context,
-			  void **return_value)
-{
-	long *cap = context;
+static bool backlight_notifier_registered;
+static struct notifier_block backlight_nb;
 
-	if (acpi_has_method(handle, "_BCM") &&
-	    acpi_has_method(handle, "_BCL")) {
-		ACPI_DEBUG_PRINT((ACPI_DB_INFO, "Found generic backlight "
-				  "support\n"));
-		*cap |= ACPI_VIDEO_BACKLIGHT;
-		if (!acpi_has_method(handle, "_BQC"))
-			printk(KERN_WARNING FW_BUG PREFIX "No _BQC method, "
-				"cannot determine initial brightness\n");
-		/* We have backlight support, no need to scan further */
-		return AE_CTRL_TERMINATE;
-	}
-	return 0;
-}
+static enum acpi_backlight_type acpi_backlight_cmdline = acpi_backlight_undef;
+static enum acpi_backlight_type acpi_backlight_dmi = acpi_backlight_undef;
 
-/* Returns true if the ACPI object is a video device which can be
- * handled by video.ko.
- * The device will get a Linux specific CID added in scan.c to
- * identify the device as an ACPI graphics device
- * Be aware that the graphics device may not be physically present
- * Use acpi_video_get_capabilities() to detect general ACPI video
- * capabilities of present cards
- */
-long acpi_is_video_device(acpi_handle handle)
+static void acpi_video_parse_cmdline(void)
 {
-	long video_caps = 0;
-
-	/* Is this device able to support video switching ? */
-	if (acpi_has_method(handle, "_DOD") || acpi_has_method(handle, "_DOS"))
-		video_caps |= ACPI_VIDEO_OUTPUT_SWITCHING;
-
-	/* Is this device able to retrieve a video ROM ? */
-	if (acpi_has_method(handle, "_ROM"))
-		video_caps |= ACPI_VIDEO_ROM_AVAILABLE;
-
-	/* Is this device able to configure which video head to be POSTed ? */
-	if (acpi_has_method(handle, "_VPO") &&
-	    acpi_has_method(handle, "_GPD") &&
-	    acpi_has_method(handle, "_SPD"))
-		video_caps |= ACPI_VIDEO_DEVICE_POSTING;
-
-	/* Only check for backlight functionality if one of the above hit. */
-	if (video_caps)
-		acpi_walk_namespace(ACPI_TYPE_DEVICE, handle,
-				    ACPI_UINT32_MAX, acpi_backlight_cap_match, NULL,
-				    &video_caps, NULL);
-
-	return video_caps;
+	if (!strcmp("vendor", acpi_video_backlight_string))
+		acpi_backlight_cmdline = acpi_backlight_vendor;
+	if (!strcmp("video", acpi_video_backlight_string))
+		acpi_backlight_cmdline = acpi_backlight_video;
+	if (!strcmp("native", acpi_video_backlight_string))
+		acpi_backlight_cmdline = acpi_backlight_native;
+	if (!strcmp("none", acpi_video_backlight_string))
+		acpi_backlight_cmdline = acpi_backlight_none;
 }
-EXPORT_SYMBOL(acpi_is_video_device);
 
 static acpi_status
 find_video(acpi_handle handle, u32 lvl, void *context, void **rv)
@@ -109,7 +64,7 @@ find_video(acpi_handle handle, u32 lvl, void *context, void **rv)
 	struct pci_dev *dev;
 	struct acpi_device *acpi_dev;
 
-	const struct acpi_device_id video_ids[] = {
+	static const struct acpi_device_id video_ids[] = {
 		{ACPI_VIDEO_HID, 0},
 		{"", 0},
 	};
@@ -130,11 +85,23 @@ find_video(acpi_handle handle, u32 lvl, void *context, void **rv)
  * buggy */
 static int video_detect_force_vendor(const struct dmi_system_id *d)
 {
-	acpi_video_support |= ACPI_VIDEO_BACKLIGHT_DMI_VENDOR;
+	acpi_backlight_dmi = acpi_backlight_vendor;
+	return 0;
+}
+
+static int video_detect_force_video(const struct dmi_system_id *d)
+{
+	acpi_backlight_dmi = acpi_backlight_video;
 	return 0;
 }
 
-static struct dmi_system_id video_detect_dmi_table[] = {
+static int video_detect_force_native(const struct dmi_system_id *d)
+{
+	acpi_backlight_dmi = acpi_backlight_native;
+	return 0;
+}
+
+static const struct dmi_system_id video_detect_dmi_table[] = {
 	/* On Samsung X360, the BIOS will set a flag (VDRV) if generic
 	 * ACPI backlight device is used. This flag will definitively break
 	 * the backlight interface (even the vendor interface) untill next
@@ -174,137 +141,209 @@ static struct dmi_system_id video_detect_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 5737"),
 		},
 	},
+
+	/*
+	 * These models have a working acpi_video backlight control, and using
+	 * native backlight causes a regression where backlight does not work
+	 * when userspace is not handling brightness key events. Disable
+	 * native_backlight on these to fix this:
+	 * https://bugzilla.kernel.org/show_bug.cgi?id=81691
+	 */
+	{
+	 .callback = video_detect_force_video,
+	 .ident = "ThinkPad T420",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T420"),
+		},
+	},
+	{
+	 .callback = video_detect_force_video,
+	 .ident = "ThinkPad T520",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T520"),
+		},
+	},
+	{
+	 .callback = video_detect_force_video,
+	 .ident = "ThinkPad X201s",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X201s"),
+		},
+	},
+
+	/* The native backlight controls do not work on some older machines */
+	{
+	 /* https://bugs.freedesktop.org/show_bug.cgi?id=81515 */
+	 .callback = video_detect_force_video,
+	 .ident = "HP ENVY 15 Notebook",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "HP ENVY 15 Notebook PC"),
+		},
+	},
+	{
+	 .callback = video_detect_force_video,
+	 .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_detect_force_video,
+	 .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"),
+		},
+	},
+	{
+	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1186097 */
+	 .callback = video_detect_force_video,
+	 .ident = "SAMSUNG 3570R/370R/470R/450R/510R/4450RV",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME,
+			  "3570R/370R/470R/450R/510R/4450RV"),
+		},
+	},
+	{
+	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1094948 */
+	 .callback = video_detect_force_video,
+	 .ident = "SAMSUNG 730U3E/740U3E",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "730U3E/740U3E"),
+		},
+	},
+	{
+	 /* https://bugs.freedesktop.org/show_bug.cgi?id=87286 */
+	 .callback = video_detect_force_video,
+	 .ident = "SAMSUNG 900X3C/900X3D/900X3E/900X4C/900X4D",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME,
+			  "900X3C/900X3D/900X3E/900X4C/900X4D"),
+		},
+	},
+	{
+	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1163574 */
+	 .callback = video_detect_force_video,
+	 .ident = "Dell XPS15 L521X",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "XPS L521X"),
+		},
+	},
+
+	/* Non win8 machines which need native backlight nevertheless */
+	{
+	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1187004 */
+	 .callback = video_detect_force_native,
+	 .ident = "Lenovo Ideapad Z570",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "102434U"),
+		},
+	},
+	{
+	 /* https://bugzilla.redhat.com/show_bug.cgi?id=1217249 */
+	 .callback = video_detect_force_native,
+	 .ident = "Apple MacBook Pro 12,1",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro12,1"),
+		},
+	},
 	{ },
 };
 
+static int acpi_video_backlight_notify(struct notifier_block *nb,
+				       unsigned long val, void *bd)
+{
+	struct backlight_device *backlight = bd;
+
+	/* A raw bl registering may change video -> native */
+	if (backlight->props.type == BACKLIGHT_RAW &&
+	    val == BACKLIGHT_REGISTERED &&
+	    acpi_video_get_backlight_type() != acpi_backlight_video)
+		acpi_video_unregister_backlight();
+
+	return NOTIFY_OK;
+}
+
 /*
- * Returns the video capabilities of a specific ACPI graphics device
+ * Determine which type of backlight interface to use on this system,
+ * First check cmdline, then dmi quirks, then do autodetect.
  *
- * if NULL is passed as argument all ACPI devices are enumerated and
- * all graphics capabilities of physically present devices are
- * summarized and returned. This is cached and done only once.
+ * The autodetect order is:
+ * 1) Is the acpi-video backlight interface supported ->
+ *  no, use a vendor interface
+ * 2) Is this a win8 "ready" BIOS and do we have a native interface ->
+ *  yes, use a native interface
+ * 3) Else use the acpi-video interface
+ *
+ * Arguably the native on win8 check should be done first, but that would
+ * be a behavior change, which may causes issues.
  */
-long acpi_video_get_capabilities(acpi_handle graphics_handle)
+enum acpi_backlight_type acpi_video_get_backlight_type(void)
 {
-	long caps = 0;
-	struct acpi_device *tmp_dev;
-	acpi_status status;
-
-	if (acpi_video_caps_checked && graphics_handle == NULL)
-		return acpi_video_support;
-
-	if (!graphics_handle) {
-		/* Only do the global walk through all graphics devices once */
-		acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
-				    ACPI_UINT32_MAX, find_video, NULL,
-				    &caps, NULL);
-		/* There might be boot param flags set already... */
-		acpi_video_support |= caps;
-		acpi_video_caps_checked = 1;
-		/* Add blacklists here. Be careful to use the right *DMI* bits
-		 * to still be able to override logic via boot params, e.g.:
-		 *
-		 *   if (dmi_name_in_vendors("XY")) {
-		 *	acpi_video_support |=
-		 *		ACPI_VIDEO_BACKLIGHT_DMI_VENDOR;
-		 *}
-		 */
+	static DEFINE_MUTEX(init_mutex);
+	static bool init_done;
+	static long video_caps;
 
+	/* Parse cmdline, dmi and acpi only once */
+	mutex_lock(&init_mutex);
+	if (!init_done) {
+		acpi_video_parse_cmdline();
 		dmi_check_system(video_detect_dmi_table);
-	} else {
-		status = acpi_bus_get_device(graphics_handle, &tmp_dev);
-		if (ACPI_FAILURE(status)) {
-			ACPI_EXCEPTION((AE_INFO, status, "Invalid device"));
-			return 0;
-		}
-		acpi_walk_namespace(ACPI_TYPE_DEVICE, graphics_handle,
+		acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
 				    ACPI_UINT32_MAX, find_video, NULL,
-				    &caps, NULL);
+				    &video_caps, NULL);
+		backlight_nb.notifier_call = acpi_video_backlight_notify;
+		backlight_nb.priority = 0;
+		if (backlight_register_notifier(&backlight_nb) == 0)
+			backlight_notifier_registered = true;
+		init_done = true;
 	}
-	ACPI_DEBUG_PRINT((ACPI_DB_INFO, "We have 0x%lX video support %s %s\n",
-			  graphics_handle ? caps : acpi_video_support,
-			  graphics_handle ? "on device " : "in general",
-			  graphics_handle ? acpi_device_bid(tmp_dev) : ""));
-	return caps;
-}
-EXPORT_SYMBOL(acpi_video_get_capabilities);
+	mutex_unlock(&init_mutex);
 
-static void acpi_video_caps_check(void)
-{
-	/*
-	 * We must check whether the ACPI graphics device is physically plugged
-	 * in. Therefore this must be called after binding PCI and ACPI devices
-	 */
-	if (!acpi_video_caps_checked)
-		acpi_video_get_capabilities(NULL);
-}
+	if (acpi_backlight_cmdline != acpi_backlight_undef)
+		return acpi_backlight_cmdline;
 
-bool acpi_osi_is_win8(void)
-{
-	return acpi_gbl_osi_data >= ACPI_OSI_WIN_8;
-}
-EXPORT_SYMBOL(acpi_osi_is_win8);
+	if (acpi_backlight_dmi != acpi_backlight_undef)
+		return acpi_backlight_dmi;
 
-/* Promote the vendor interface instead of the generic video module.
- * This function allow DMI blacklists to be implemented by externals
- * platform drivers instead of putting a big blacklist in video_detect.c
- * After calling this function you will probably want to call
- * acpi_video_unregister() to make sure the video module is not loaded
- */
-void acpi_video_dmi_promote_vendor(void)
-{
-	acpi_video_caps_check();
-	acpi_video_support |= ACPI_VIDEO_BACKLIGHT_DMI_VENDOR;
-}
-EXPORT_SYMBOL(acpi_video_dmi_promote_vendor);
-
-/* To be called when a driver who previously promoted the vendor
- * interface */
-void acpi_video_dmi_demote_vendor(void)
-{
-	acpi_video_caps_check();
-	acpi_video_support &= ~ACPI_VIDEO_BACKLIGHT_DMI_VENDOR;
-}
-EXPORT_SYMBOL(acpi_video_dmi_demote_vendor);
-
-/* Returns true if video.ko can do backlight switching */
-int acpi_video_backlight_support(void)
-{
-	acpi_video_caps_check();
-
-	/* First check for boot param -> highest prio */
-	if (acpi_video_support & ACPI_VIDEO_BACKLIGHT_FORCE_VENDOR)
-		return 0;
-	else if (acpi_video_support & ACPI_VIDEO_BACKLIGHT_FORCE_VIDEO)
-		return 1;
+	if (!(video_caps & ACPI_VIDEO_BACKLIGHT))
+		return acpi_backlight_vendor;
 
-	/* Then check for DMI blacklist -> second highest prio */
-	if (acpi_video_support & ACPI_VIDEO_BACKLIGHT_DMI_VENDOR)
-		return 0;
-	else if (acpi_video_support & ACPI_VIDEO_BACKLIGHT_DMI_VIDEO)
-		return 1;
+	if (acpi_osi_is_win8() && backlight_device_registered(BACKLIGHT_RAW))
+		return acpi_backlight_native;
 
-	/* Then go the default way */
-	return acpi_video_support & ACPI_VIDEO_BACKLIGHT;
+	return acpi_backlight_video;
 }
-EXPORT_SYMBOL(acpi_video_backlight_support);
+EXPORT_SYMBOL(acpi_video_get_backlight_type);
 
 /*
- * Use acpi_backlight=vendor/video to force that backlight switching
- * is processed by vendor specific acpi drivers or video.ko driver.
+ * Set the preferred backlight interface type based on DMI info.
+ * This function allows DMI blacklists to be implemented by external
+ * platform drivers instead of putting a big blacklist in video_detect.c
  */
-static int __init acpi_backlight(char *str)
+void acpi_video_set_dmi_backlight_type(enum acpi_backlight_type type)
 {
-	if (str == NULL || *str == '\0')
-		return 1;
-	else {
-		if (!strcmp("vendor", str))
-			acpi_video_support |=
-				ACPI_VIDEO_BACKLIGHT_FORCE_VENDOR;
-		if (!strcmp("video", str))
-			acpi_video_support |=
-				ACPI_VIDEO_BACKLIGHT_FORCE_VIDEO;
-	}
-	return 1;
+	acpi_backlight_dmi = type;
+	/* Remove acpi-video backlight interface if it is no longer desired */
+	if (acpi_video_get_backlight_type() != acpi_backlight_video)
+		acpi_video_unregister_backlight();
+}
+EXPORT_SYMBOL(acpi_video_set_dmi_backlight_type);
+
+void __exit acpi_video_detect_exit(void)
+{
+	if (backlight_notifier_registered)
+		backlight_unregister_notifier(&backlight_nb);
 }
-__setup("acpi_backlight=", acpi_backlight);
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index 9dca4b995be0..15e40ee62a94 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -48,7 +48,7 @@ config ATA_VERBOSE_ERROR
 
 config ATA_ACPI
 	bool "ATA ACPI Support"
-	depends on ACPI && PCI
+	depends on ACPI
 	default y
 	help
 	  This option adds support for ATA-related ACPI objects.
@@ -98,6 +98,15 @@ config SATA_AHCI_PLATFORM
 
 	  If unsure, say N.
 
+config AHCI_BRCMSTB
+	tristate "Broadcom STB AHCI SATA support"
+	depends on ARCH_BRCMSTB
+	help
+	  This option enables support for the AHCI SATA3 controller found on
+	  STB SoC's.
+
+	  If unsure, say N.
+
 config AHCI_DA850
 	tristate "DaVinci DA850 AHCI SATA support"
 	depends on ARCH_DAVINCI_DA850
@@ -124,6 +133,15 @@ config AHCI_IMX
 
 	  If unsure, say N.
 
+config AHCI_CEVA
+	tristate "CEVA AHCI SATA support"
+	depends on OF
+	help
+	  This option enables support for the CEVA AHCI SATA.
+	  It can be found on the Xilinx Zynq UltraScale+ MPSoC.
+
+	  If unsure, say N.
+
 config AHCI_MVEBU
 	tristate "Marvell EBU AHCI SATA support"
 	depends on ARCH_MVEBU
@@ -827,7 +845,6 @@ config PATA_AT32
 config PATA_AT91
 	tristate "PATA support for AT91SAM9260"
 	depends on ARM && SOC_AT91SAM9
-	depends on !ARCH_MULTIPLATFORM
 	help
 	  This option enables support for IDE devices on the Atmel AT91SAM9260 SoC.
 
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index 40f7865f20a1..af70919f7dde 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -10,6 +10,8 @@ obj-$(CONFIG_SATA_INIC162X)	+= sata_inic162x.o
 obj-$(CONFIG_SATA_SIL24)	+= sata_sil24.o
 obj-$(CONFIG_SATA_DWC)		+= sata_dwc_460ex.o
 obj-$(CONFIG_SATA_HIGHBANK)	+= sata_highbank.o libahci.o
+obj-$(CONFIG_AHCI_BRCMSTB)	+= ahci_brcmstb.o libahci.o libahci_platform.o
+obj-$(CONFIG_AHCI_CEVA)		+= ahci_ceva.o libahci.o libahci_platform.o
 obj-$(CONFIG_AHCI_DA850)	+= ahci_da850.o libahci.o libahci_platform.o
 obj-$(CONFIG_AHCI_IMX)		+= ahci_imx.o libahci.o libahci_platform.o
 obj-$(CONFIG_AHCI_MVEBU)	+= ahci_mvebu.o libahci.o libahci_platform.o
diff --git a/drivers/ata/acard-ahci.c b/drivers/ata/acard-ahci.c
index 12489ce863c4..ed6a30cd681a 100644
--- a/drivers/ata/acard-ahci.c
+++ b/drivers/ata/acard-ahci.c
@@ -433,6 +433,8 @@ static int acard_ahci_init_one(struct pci_dev *pdev, const struct pci_device_id
 	hpriv = devm_kzalloc(dev, sizeof(*hpriv), GFP_KERNEL);
 	if (!hpriv)
 		return -ENOMEM;
+
+	hpriv->irq = pdev->irq;
 	hpriv->flags |= (unsigned long)pi.private_data;
 
 	if (!(hpriv->flags & AHCI_HFLAG_NO_MSI))
@@ -498,7 +500,7 @@ static int acard_ahci_init_one(struct pci_dev *pdev, const struct pci_device_id
 	acard_ahci_pci_print_info(host);
 
 	pci_set_master(pdev);
-	return ahci_host_activate(host, pdev->irq, &acard_ahci_sht);
+	return ahci_host_activate(host, &acard_ahci_sht);
 }
 
 module_pci_driver(acard_ahci_pci_driver);
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index 65ee94454bbd..7e62751abfac 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -42,6 +42,7 @@
 #include <linux/device.h>
 #include <linux/dmi.h>
 #include <linux/gfp.h>
+#include <linux/msi.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_cmnd.h>
 #include <linux/libata.h>
@@ -52,6 +53,7 @@
 
 enum {
 	AHCI_PCI_BAR_STA2X11	= 0,
+	AHCI_PCI_BAR_CAVIUM	= 0,
 	AHCI_PCI_BAR_ENMOTUS	= 2,
 	AHCI_PCI_BAR_STANDARD	= 5,
 };
@@ -1288,17 +1290,60 @@ static inline void ahci_gtf_filter_workaround(struct ata_host *host)
 {}
 #endif
 
-static int ahci_init_interrupts(struct pci_dev *pdev, unsigned int n_ports,
-				struct ahci_host_priv *hpriv)
+/*
+ * ahci_init_msix() only implements single MSI-X support, not multiple
+ * MSI-X per-port interrupts. This is needed for host controllers that only
+ * have MSI-X support implemented, but no MSI or intx.
+ */
+static int ahci_init_msix(struct pci_dev *pdev, unsigned int n_ports,
+			  struct ahci_host_priv *hpriv)
 {
 	int rc, nvec;
+	struct msix_entry entry = {};
 
+	/* Do not init MSI-X if MSI is disabled for the device */
 	if (hpriv->flags & AHCI_HFLAG_NO_MSI)
-		goto intx;
+		return -ENODEV;
+
+	nvec = pci_msix_vec_count(pdev);
+	if (nvec < 0)
+		return nvec;
+
+	if (!nvec) {
+		rc = -ENODEV;
+		goto fail;
+	}
+
+	/*
+	 * There can be more than one vector (e.g. for error detection or
+	 * hdd hotplug). Only the first vector (entry.entry = 0) is used.
+	 */
+	rc = pci_enable_msix_exact(pdev, &entry, 1);
+	if (rc < 0)
+		goto fail;
+
+	hpriv->irq = entry.vector;
+
+	return 1;
+fail:
+	dev_err(&pdev->dev,
+		"failed to enable MSI-X with error %d, # of vectors: %d\n",
+		rc, nvec);
+
+	return rc;
+}
+
+static int ahci_init_msi(struct pci_dev *pdev, unsigned int n_ports,
+			struct ahci_host_priv *hpriv)
+{
+	int rc, nvec;
+
+	if (hpriv->flags & AHCI_HFLAG_NO_MSI)
+		return -ENODEV;
 
 	nvec = pci_msi_vec_count(pdev);
 	if (nvec < 0)
-		goto intx;
+		return nvec;
 
 	/*
 	 * If number of MSIs is less than number of ports then Sharing Last
@@ -1311,8 +1356,8 @@ static int ahci_init_interrupts(struct pci_dev *pdev, unsigned int n_ports,
 	rc = pci_enable_msi_exact(pdev, nvec);
 	if (rc == -ENOSPC)
 		goto single_msi;
-	else if (rc < 0)
-		goto intx;
+	if (rc < 0)
+		return rc;
 
 	/* fallback to single MSI mode if the controller enforced MRSM mode */
 	if (readl(hpriv->mmio + HOST_CTL) & HOST_MRSM) {
@@ -1324,15 +1369,42 @@ static int ahci_init_interrupts(struct pci_dev *pdev, unsigned int n_ports,
 	if (nvec > 1)
 		hpriv->flags |= AHCI_HFLAG_MULTI_MSI;
 
-	return nvec;
+	goto out;
 
 single_msi:
-	if (pci_enable_msi(pdev))
-		goto intx;
-	return 1;
+	nvec = 1;
+
+	rc = pci_enable_msi(pdev);
+	if (rc < 0)
+		return rc;
+out:
+	hpriv->irq = pdev->irq;
+
+	return nvec;
+}
+
+static int ahci_init_interrupts(struct pci_dev *pdev, unsigned int n_ports,
+				struct ahci_host_priv *hpriv)
+{
+	int nvec;
+
+	nvec = ahci_init_msi(pdev, n_ports, hpriv);
+	if (nvec >= 0)
+		return nvec;
+
+	/*
+	 * Currently, MSI-X support only implements single IRQ mode and
+	 * exists for controllers which can't do other types of IRQ. Only
+	 * set it up if MSI fails.
+	 */
+	nvec = ahci_init_msix(pdev, n_ports, hpriv);
+	if (nvec >= 0)
+		return nvec;
 
-intx:
+	/* lagacy intx interrupts */
 	pci_intx(pdev, 1);
+	hpriv->irq = pdev->irq;
+
 	return 0;
 }
 
@@ -1371,11 +1443,13 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev_info(&pdev->dev,
 			 "PDC42819 can only drive SATA devices with this driver\n");
 
-	/* Both Connext and Enmotus devices use non-standard BARs */
+	/* Some devices use non-standard BARs */
 	if (pdev->vendor == PCI_VENDOR_ID_STMICRO && pdev->device == 0xCC06)
 		ahci_pci_bar = AHCI_PCI_BAR_STA2X11;
 	else if (pdev->vendor == 0x1c44 && pdev->device == 0x8000)
 		ahci_pci_bar = AHCI_PCI_BAR_ENMOTUS;
+	else if (pdev->vendor == 0x177d && pdev->device == 0xa01c)
+		ahci_pci_bar = AHCI_PCI_BAR_CAVIUM;
 
 	/*
 	 * The JMicron chip 361/363 contains one SATA controller and one
@@ -1497,13 +1571,13 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	 */
 	n_ports = max(ahci_nr_ports(hpriv->cap), fls(hpriv->port_map));
 
-	ahci_init_interrupts(pdev, n_ports, hpriv);
-
 	host = ata_host_alloc_pinfo(&pdev->dev, ppi, n_ports);
 	if (!host)
 		return -ENOMEM;
 	host->private_data = hpriv;
 
+	ahci_init_interrupts(pdev, n_ports, hpriv);
+
 	if (!(hpriv->cap & HOST_CAP_SSS) || ahci_ignore_sss)
 		host->flags |= ATA_HOST_PARALLEL_SCAN;
 	else
@@ -1549,7 +1623,7 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	pci_set_master(pdev);
 
-	return ahci_host_activate(host, pdev->irq, &ahci_sht);
+	return ahci_host_activate(host, &ahci_sht);
 }
 
 module_pci_driver(ahci_pci_driver);
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 71262e08648e..5b8e8a0fab48 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -238,6 +238,8 @@ enum {
 	AHCI_HFLAG_MULTI_MSI		= (1 << 16), /* multiple PCI MSIs */
 	AHCI_HFLAG_NO_DEVSLP		= (1 << 17), /* no device sleep */
 	AHCI_HFLAG_NO_FBS		= (1 << 18), /* no FBS */
+	AHCI_HFLAG_EDGE_IRQ		= (1 << 19), /* HOST_IRQ_STAT behaves as
+							Edge Triggered */
 
 	/* ap->flags bits */
 
@@ -341,6 +343,7 @@ struct ahci_host_priv {
 	struct phy		**phys;
 	unsigned		nports;		/* Number of ports */
 	void			*plat_data;	/* Other platform data */
+	unsigned int		irq;		/* interrupt line */
 	/*
 	 * Optional ahci_start_engine override, if not set this gets set to the
 	 * default ahci_start_engine during ahci_save_initial_config, this can
@@ -393,8 +396,7 @@ void ahci_set_em_messages(struct ahci_host_priv *hpriv,
 			  struct ata_port_info *pi);
 int ahci_reset_em(struct ata_host *host);
 void ahci_print_info(struct ata_host *host, const char *scc_s);
-int ahci_host_activate(struct ata_host *host, int irq,
-		       struct scsi_host_template *sht);
+int ahci_host_activate(struct ata_host *host, struct scsi_host_template *sht);
 void ahci_error_handler(struct ata_port *ap);
 
 static inline void __iomem *__ahci_port_base(struct ata_host *host,
diff --git a/drivers/ata/ahci_brcmstb.c b/drivers/ata/ahci_brcmstb.c
new file mode 100644
index 000000000000..ce1e3a885981
--- /dev/null
+++ b/drivers/ata/ahci_brcmstb.c
@@ -0,0 +1,322 @@
+/*
+ * Broadcom SATA3 AHCI Controller Driver
+ *
+ * Copyright © 2009-2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/ahci_platform.h>
+#include <linux/compiler.h>
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/libata.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/string.h>
+
+#include "ahci.h"
+
+#define DRV_NAME					"brcm-ahci"
+
+#define SATA_TOP_CTRL_VERSION				0x0
+#define SATA_TOP_CTRL_BUS_CTRL				0x4
+ #define MMIO_ENDIAN_SHIFT				0 /* CPU->AHCI */
+ #define DMADESC_ENDIAN_SHIFT				2 /* AHCI->DDR */
+ #define DMADATA_ENDIAN_SHIFT				4 /* AHCI->DDR */
+ #define PIODATA_ENDIAN_SHIFT				6
+  #define ENDIAN_SWAP_NONE				0
+  #define ENDIAN_SWAP_FULL				2
+ #define OVERRIDE_HWINIT				BIT(16)
+#define SATA_TOP_CTRL_TP_CTRL				0x8
+#define SATA_TOP_CTRL_PHY_CTRL				0xc
+ #define SATA_TOP_CTRL_PHY_CTRL_1			0x0
+  #define SATA_TOP_CTRL_1_PHY_DEFAULT_POWER_STATE	BIT(14)
+ #define SATA_TOP_CTRL_PHY_CTRL_2			0x4
+  #define SATA_TOP_CTRL_2_SW_RST_MDIOREG		BIT(0)
+  #define SATA_TOP_CTRL_2_SW_RST_OOB			BIT(1)
+  #define SATA_TOP_CTRL_2_SW_RST_RX			BIT(2)
+  #define SATA_TOP_CTRL_2_SW_RST_TX			BIT(3)
+  #define SATA_TOP_CTRL_2_PHY_GLOBAL_RESET		BIT(14)
+ #define SATA_TOP_CTRL_PHY_OFFS				0x8
+ #define SATA_TOP_MAX_PHYS				2
+#define SATA_TOP_CTRL_SATA_TP_OUT			0x1c
+#define SATA_TOP_CTRL_CLIENT_INIT_CTRL			0x20
+
+/* On big-endian MIPS, buses are reversed to big endian, so switch them back */
+#if defined(CONFIG_MIPS) && defined(__BIG_ENDIAN)
+#define DATA_ENDIAN			 2 /* AHCI->DDR inbound accesses */
+#define MMIO_ENDIAN			 2 /* CPU->AHCI outbound accesses */
+#else
+#define DATA_ENDIAN			 0
+#define MMIO_ENDIAN			 0
+#endif
+
+#define BUS_CTRL_ENDIAN_CONF				\
+	((DATA_ENDIAN << DMADATA_ENDIAN_SHIFT) |	\
+	(DATA_ENDIAN << DMADESC_ENDIAN_SHIFT) |		\
+	(MMIO_ENDIAN << MMIO_ENDIAN_SHIFT))
+
+struct brcm_ahci_priv {
+	struct device *dev;
+	void __iomem *top_ctrl;
+	u32 port_mask;
+};
+
+static const struct ata_port_info ahci_brcm_port_info = {
+	.flags		= AHCI_FLAG_COMMON,
+	.pio_mask	= ATA_PIO4,
+	.udma_mask	= ATA_UDMA6,
+	.port_ops	= &ahci_platform_ops,
+};
+
+static inline u32 brcm_sata_readreg(void __iomem *addr)
+{
+	/*
+	 * MIPS endianness is configured by boot strap, which also reverses all
+	 * bus endianness (i.e., big-endian CPU + big endian bus ==> native
+	 * endian I/O).
+	 *
+	 * Other architectures (e.g., ARM) either do not support big endian, or
+	 * else leave I/O in little endian mode.
+	 */
+	if (IS_ENABLED(CONFIG_MIPS) && IS_ENABLED(__BIG_ENDIAN))
+		return __raw_readl(addr);
+	else
+		return readl_relaxed(addr);
+}
+
+static inline void brcm_sata_writereg(u32 val, void __iomem *addr)
+{
+	/* See brcm_sata_readreg() comments */
+	if (IS_ENABLED(CONFIG_MIPS) && IS_ENABLED(__BIG_ENDIAN))
+		__raw_writel(val, addr);
+	else
+		writel_relaxed(val, addr);
+}
+
+static void brcm_sata_phy_enable(struct brcm_ahci_priv *priv, int port)
+{
+	void __iomem *phyctrl = priv->top_ctrl + SATA_TOP_CTRL_PHY_CTRL +
+				(port * SATA_TOP_CTRL_PHY_OFFS);
+	void __iomem *p;
+	u32 reg;
+
+	/* clear PHY_DEFAULT_POWER_STATE */
+	p = phyctrl + SATA_TOP_CTRL_PHY_CTRL_1;
+	reg = brcm_sata_readreg(p);
+	reg &= ~SATA_TOP_CTRL_1_PHY_DEFAULT_POWER_STATE;
+	brcm_sata_writereg(reg, p);
+
+	/* reset the PHY digital logic */
+	p = phyctrl + SATA_TOP_CTRL_PHY_CTRL_2;
+	reg = brcm_sata_readreg(p);
+	reg &= ~(SATA_TOP_CTRL_2_SW_RST_MDIOREG | SATA_TOP_CTRL_2_SW_RST_OOB |
+		 SATA_TOP_CTRL_2_SW_RST_RX);
+	reg |= SATA_TOP_CTRL_2_SW_RST_TX;
+	brcm_sata_writereg(reg, p);
+	reg = brcm_sata_readreg(p);
+	reg |= SATA_TOP_CTRL_2_PHY_GLOBAL_RESET;
+	brcm_sata_writereg(reg, p);
+	reg = brcm_sata_readreg(p);
+	reg &= ~SATA_TOP_CTRL_2_PHY_GLOBAL_RESET;
+	brcm_sata_writereg(reg, p);
+	(void)brcm_sata_readreg(p);
+}
+
+static void brcm_sata_phy_disable(struct brcm_ahci_priv *priv, int port)
+{
+	void __iomem *phyctrl = priv->top_ctrl + SATA_TOP_CTRL_PHY_CTRL +
+				(port * SATA_TOP_CTRL_PHY_OFFS);
+	void __iomem *p;
+	u32 reg;
+
+	/* power-off the PHY digital logic */
+	p = phyctrl + SATA_TOP_CTRL_PHY_CTRL_2;
+	reg = brcm_sata_readreg(p);
+	reg |= (SATA_TOP_CTRL_2_SW_RST_MDIOREG | SATA_TOP_CTRL_2_SW_RST_OOB |
+		SATA_TOP_CTRL_2_SW_RST_RX | SATA_TOP_CTRL_2_SW_RST_TX |
+		SATA_TOP_CTRL_2_PHY_GLOBAL_RESET);
+	brcm_sata_writereg(reg, p);
+
+	/* set PHY_DEFAULT_POWER_STATE */
+	p = phyctrl + SATA_TOP_CTRL_PHY_CTRL_1;
+	reg = brcm_sata_readreg(p);
+	reg |= SATA_TOP_CTRL_1_PHY_DEFAULT_POWER_STATE;
+	brcm_sata_writereg(reg, p);
+}
+
+static void brcm_sata_phys_enable(struct brcm_ahci_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < SATA_TOP_MAX_PHYS; i++)
+		if (priv->port_mask & BIT(i))
+			brcm_sata_phy_enable(priv, i);
+}
+
+static void brcm_sata_phys_disable(struct brcm_ahci_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < SATA_TOP_MAX_PHYS; i++)
+		if (priv->port_mask & BIT(i))
+			brcm_sata_phy_disable(priv, i);
+}
+
+static u32 brcm_ahci_get_portmask(struct platform_device *pdev,
+				  struct brcm_ahci_priv *priv)
+{
+	void __iomem *ahci;
+	struct resource *res;
+	u32 impl;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "ahci");
+	ahci = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ahci))
+		return 0;
+
+	impl = readl(ahci + HOST_PORTS_IMPL);
+
+	if (fls(impl) > SATA_TOP_MAX_PHYS)
+		dev_warn(priv->dev, "warning: more ports than PHYs (%#x)\n",
+			 impl);
+	else if (!impl)
+		dev_info(priv->dev, "no ports found\n");
+
+	devm_iounmap(&pdev->dev, ahci);
+	devm_release_mem_region(&pdev->dev, res->start, resource_size(res));
+
+	return impl;
+}
+
+static void brcm_sata_init(struct brcm_ahci_priv *priv)
+{
+	/* Configure endianness */
+	brcm_sata_writereg(BUS_CTRL_ENDIAN_CONF,
+			   priv->top_ctrl + SATA_TOP_CTRL_BUS_CTRL);
+}
+
+static int brcm_ahci_suspend(struct device *dev)
+{
+	struct ata_host *host = dev_get_drvdata(dev);
+	struct ahci_host_priv *hpriv = host->private_data;
+	struct brcm_ahci_priv *priv = hpriv->plat_data;
+	int ret;
+
+	ret = ahci_platform_suspend(dev);
+	brcm_sata_phys_disable(priv);
+	return ret;
+}
+
+static int brcm_ahci_resume(struct device *dev)
+{
+	struct ata_host *host = dev_get_drvdata(dev);
+	struct ahci_host_priv *hpriv = host->private_data;
+	struct brcm_ahci_priv *priv = hpriv->plat_data;
+
+	brcm_sata_init(priv);
+	brcm_sata_phys_enable(priv);
+	return ahci_platform_resume(dev);
+}
+
+static struct scsi_host_template ahci_platform_sht = {
+	AHCI_SHT(DRV_NAME),
+};
+
+static int brcm_ahci_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct brcm_ahci_priv *priv;
+	struct ahci_host_priv *hpriv;
+	struct resource *res;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+	priv->dev = dev;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "top-ctrl");
+	priv->top_ctrl = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->top_ctrl))
+		return PTR_ERR(priv->top_ctrl);
+
+	brcm_sata_init(priv);
+
+	priv->port_mask = brcm_ahci_get_portmask(pdev, priv);
+	if (!priv->port_mask)
+		return -ENODEV;
+
+	brcm_sata_phys_enable(priv);
+
+	hpriv = ahci_platform_get_resources(pdev);
+	if (IS_ERR(hpriv))
+		return PTR_ERR(hpriv);
+	hpriv->plat_data = priv;
+
+	ret = ahci_platform_enable_resources(hpriv);
+	if (ret)
+		return ret;
+
+	ret = ahci_platform_init_host(pdev, hpriv, &ahci_brcm_port_info,
+				      &ahci_platform_sht);
+	if (ret)
+		return ret;
+
+	dev_info(dev, "Broadcom AHCI SATA3 registered\n");
+
+	return 0;
+}
+
+static int brcm_ahci_remove(struct platform_device *pdev)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	struct ahci_host_priv *hpriv = host->private_data;
+	struct brcm_ahci_priv *priv = hpriv->plat_data;
+	int ret;
+
+	ret = ata_platform_remove_one(pdev);
+	if (ret)
+		return ret;
+
+	brcm_sata_phys_disable(priv);
+
+	return 0;
+}
+
+static const struct of_device_id ahci_of_match[] = {
+	{.compatible = "brcm,bcm7445-ahci"},
+	{},
+};
+MODULE_DEVICE_TABLE(of, ahci_of_match);
+
+static SIMPLE_DEV_PM_OPS(ahci_brcm_pm_ops, brcm_ahci_suspend, brcm_ahci_resume);
+
+static struct platform_driver brcm_ahci_driver = {
+	.probe = brcm_ahci_probe,
+	.remove = brcm_ahci_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = ahci_of_match,
+		.pm = &ahci_brcm_pm_ops,
+	},
+};
+module_platform_driver(brcm_ahci_driver);
+
+MODULE_DESCRIPTION("Broadcom SATA3 AHCI Controller Driver");
+MODULE_AUTHOR("Brian Norris");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:sata-brcmstb");
diff --git a/drivers/ata/ahci_ceva.c b/drivers/ata/ahci_ceva.c
new file mode 100644
index 000000000000..207649d323c5
--- /dev/null
+++ b/drivers/ata/ahci_ceva.c
@@ -0,0 +1,238 @@
+/*
+ * Copyright (C) 2015 Xilinx, Inc.
+ * CEVA AHCI SATA platform driver
+ *
+ * based on the AHCI SATA platform driver by Jeff Garzik and Anton Vorontsov
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/ahci_platform.h>
+#include <linux/kernel.h>
+#include <linux/libata.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include "ahci.h"
+
+/* Vendor Specific Register Offsets */
+#define AHCI_VEND_PCFG  0xA4
+#define AHCI_VEND_PPCFG 0xA8
+#define AHCI_VEND_PP2C  0xAC
+#define AHCI_VEND_PP3C  0xB0
+#define AHCI_VEND_PP4C  0xB4
+#define AHCI_VEND_PP5C  0xB8
+#define AHCI_VEND_PAXIC 0xC0
+#define AHCI_VEND_PTC   0xC8
+
+/* Vendor Specific Register bit definitions */
+#define PAXIC_ADBW_BW64 0x1
+#define PAXIC_MAWIDD	(1 << 8)
+#define PAXIC_MARIDD	(1 << 16)
+#define PAXIC_OTL	(0x4 << 20)
+
+#define PCFG_TPSS_VAL	(0x32 << 16)
+#define PCFG_TPRS_VAL	(0x2 << 12)
+#define PCFG_PAD_VAL	0x2
+
+#define PPCFG_TTA	0x1FFFE
+#define PPCFG_PSSO_EN	(1 << 28)
+#define PPCFG_PSS_EN	(1 << 29)
+#define PPCFG_ESDF_EN	(1 << 31)
+
+#define PP2C_CIBGMN	0x0F
+#define PP2C_CIBGMX	(0x25 << 8)
+#define PP2C_CIBGN	(0x18 << 16)
+#define PP2C_CINMP	(0x29 << 24)
+
+#define PP3C_CWBGMN	0x04
+#define PP3C_CWBGMX	(0x0B << 8)
+#define PP3C_CWBGN	(0x08 << 16)
+#define PP3C_CWNMP	(0x0F << 24)
+
+#define PP4C_BMX	0x0a
+#define PP4C_BNM	(0x08 << 8)
+#define PP4C_SFD	(0x4a << 16)
+#define PP4C_PTST	(0x06 << 24)
+
+#define PP5C_RIT	0x60216
+#define PP5C_RCT	(0x7f0 << 20)
+
+#define PTC_RX_WM_VAL	0x40
+#define PTC_RSVD	(1 << 27)
+
+#define PORT0_BASE	0x100
+#define PORT1_BASE	0x180
+
+/* Port Control Register Bit Definitions */
+#define PORT_SCTL_SPD_GEN2	(0x2 << 4)
+#define PORT_SCTL_SPD_GEN1	(0x1 << 4)
+#define PORT_SCTL_IPM		(0x3 << 8)
+
+#define PORT_BASE	0x100
+#define PORT_OFFSET	0x80
+#define NR_PORTS	2
+#define DRV_NAME	"ahci-ceva"
+#define CEVA_FLAG_BROKEN_GEN2	1
+
+struct ceva_ahci_priv {
+	struct platform_device *ahci_pdev;
+	int flags;
+};
+
+static struct ata_port_operations ahci_ceva_ops = {
+	.inherits = &ahci_platform_ops,
+};
+
+static const struct ata_port_info ahci_ceva_port_info = {
+	.flags          = AHCI_FLAG_COMMON,
+	.pio_mask       = ATA_PIO4,
+	.udma_mask      = ATA_UDMA6,
+	.port_ops	= &ahci_ceva_ops,
+};
+
+static void ahci_ceva_setup(struct ahci_host_priv *hpriv)
+{
+	void __iomem *mmio = hpriv->mmio;
+	struct ceva_ahci_priv *cevapriv = hpriv->plat_data;
+	u32 tmp;
+	int i;
+
+	/*
+	 * AXI Data bus width to 64
+	 * Set Mem Addr Read, Write ID for data transfers
+	 * Transfer limit to 72 DWord
+	 */
+	tmp = PAXIC_ADBW_BW64 | PAXIC_MAWIDD | PAXIC_MARIDD | PAXIC_OTL;
+	writel(tmp, mmio + AHCI_VEND_PAXIC);
+
+	/* Set AHCI Enable */
+	tmp = readl(mmio + HOST_CTL);
+	tmp |= HOST_AHCI_EN;
+	writel(tmp, mmio + HOST_CTL);
+
+	for (i = 0; i < NR_PORTS; i++) {
+		/* TPSS TPRS scalars, CISE and Port Addr */
+		tmp = PCFG_TPSS_VAL | PCFG_TPRS_VAL | (PCFG_PAD_VAL + i);
+		writel(tmp, mmio + AHCI_VEND_PCFG);
+
+		/* Port Phy Cfg register enables */
+		tmp = PPCFG_TTA | PPCFG_PSS_EN | PPCFG_ESDF_EN;
+		writel(tmp, mmio + AHCI_VEND_PPCFG);
+
+		/* Phy Control OOB timing parameters COMINIT */
+		tmp = PP2C_CIBGMN | PP2C_CIBGMX | PP2C_CIBGN | PP2C_CINMP;
+		writel(tmp, mmio + AHCI_VEND_PP2C);
+
+		/* Phy Control OOB timing parameters COMWAKE */
+		tmp = PP3C_CWBGMN | PP3C_CWBGMX | PP3C_CWBGN | PP3C_CWNMP;
+		writel(tmp, mmio + AHCI_VEND_PP3C);
+
+		/* Phy Control Burst timing setting */
+		tmp = PP4C_BMX | PP4C_BNM | PP4C_SFD | PP4C_PTST;
+		writel(tmp, mmio + AHCI_VEND_PP4C);
+
+		/* Rate Change Timer and Retry Interval Timer setting */
+		tmp = PP5C_RIT | PP5C_RCT;
+		writel(tmp, mmio + AHCI_VEND_PP5C);
+
+		/* Rx Watermark setting  */
+		tmp = PTC_RX_WM_VAL | PTC_RSVD;
+		writel(tmp, mmio + AHCI_VEND_PTC);
+
+		/* Default to Gen 2 Speed and Gen 1 if Gen2 is broken */
+		tmp = PORT_SCTL_SPD_GEN2 | PORT_SCTL_IPM;
+		if (cevapriv->flags & CEVA_FLAG_BROKEN_GEN2)
+			tmp = PORT_SCTL_SPD_GEN1 | PORT_SCTL_IPM;
+		writel(tmp, mmio + PORT_SCR_CTL + PORT_BASE + PORT_OFFSET * i);
+	}
+}
+
+static struct scsi_host_template ahci_platform_sht = {
+	AHCI_SHT(DRV_NAME),
+};
+
+static int ceva_ahci_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device *dev = &pdev->dev;
+	struct ahci_host_priv *hpriv;
+	struct ceva_ahci_priv *cevapriv;
+	int rc;
+
+	cevapriv = devm_kzalloc(dev, sizeof(*cevapriv), GFP_KERNEL);
+	if (!cevapriv)
+		return -ENOMEM;
+
+	cevapriv->ahci_pdev = pdev;
+
+	hpriv = ahci_platform_get_resources(pdev);
+	if (IS_ERR(hpriv))
+		return PTR_ERR(hpriv);
+
+	rc = ahci_platform_enable_resources(hpriv);
+	if (rc)
+		return rc;
+
+	if (of_property_read_bool(np, "ceva,broken-gen2"))
+		cevapriv->flags = CEVA_FLAG_BROKEN_GEN2;
+
+	hpriv->plat_data = cevapriv;
+
+	/* CEVA specific initialization */
+	ahci_ceva_setup(hpriv);
+
+	rc = ahci_platform_init_host(pdev, hpriv, &ahci_ceva_port_info,
+					&ahci_platform_sht);
+	if (rc)
+		goto disable_resources;
+
+	return 0;
+
+disable_resources:
+	ahci_platform_disable_resources(hpriv);
+	return rc;
+}
+
+static int __maybe_unused ceva_ahci_suspend(struct device *dev)
+{
+	return ahci_platform_suspend_host(dev);
+}
+
+static int __maybe_unused ceva_ahci_resume(struct device *dev)
+{
+	return ahci_platform_resume_host(dev);
+}
+
+static SIMPLE_DEV_PM_OPS(ahci_ceva_pm_ops, ceva_ahci_suspend, ceva_ahci_resume);
+
+static const struct of_device_id ceva_ahci_of_match[] = {
+	{ .compatible = "ceva,ahci-1v84" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ceva_ahci_of_match);
+
+static struct platform_driver ceva_ahci_driver = {
+	.probe = ceva_ahci_probe,
+	.remove = ata_platform_remove_one,
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = ceva_ahci_of_match,
+		.pm = &ahci_ceva_pm_ops,
+	},
+};
+module_platform_driver(ceva_ahci_driver);
+
+MODULE_DESCRIPTION("CEVA AHCI SATA platform driver");
+MODULE_AUTHOR("Xilinx Inc.");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/ata/ahci_mvebu.c b/drivers/ata/ahci_mvebu.c
index 23716dd8a7ec..8490d37aee2a 100644
--- a/drivers/ata/ahci_mvebu.c
+++ b/drivers/ata/ahci_mvebu.c
@@ -45,7 +45,7 @@ static void ahci_mvebu_mbus_config(struct ahci_host_priv *hpriv,
 		writel((cs->mbus_attr << 8) |
 		       (dram->mbus_dram_target_id << 4) | 1,
 		       hpriv->mmio + AHCI_WINDOW_CTRL(i));
-		writel(cs->base, hpriv->mmio + AHCI_WINDOW_BASE(i));
+		writel(cs->base >> 16, hpriv->mmio + AHCI_WINDOW_BASE(i));
 		writel(((cs->size - 1) & 0xffff0000),
 		       hpriv->mmio + AHCI_WINDOW_SIZE(i));
 	}
@@ -62,6 +62,26 @@ static void ahci_mvebu_regret_option(struct ahci_host_priv *hpriv)
 	writel(0x80, hpriv->mmio + AHCI_VENDOR_SPECIFIC_0_DATA);
 }
 
+static int ahci_mvebu_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return ahci_platform_suspend_host(&pdev->dev);
+}
+
+static int ahci_mvebu_resume(struct platform_device *pdev)
+{
+	struct ata_host *host = platform_get_drvdata(pdev);
+	struct ahci_host_priv *hpriv = host->private_data;
+	const struct mbus_dram_target_info *dram;
+
+	dram = mv_mbus_dram_info();
+	if (dram)
+		ahci_mvebu_mbus_config(hpriv, dram);
+
+	ahci_mvebu_regret_option(hpriv);
+
+	return ahci_platform_resume_host(&pdev->dev);
+}
+
 static const struct ata_port_info ahci_mvebu_port_info = {
 	.flags	   = AHCI_FLAG_COMMON,
 	.pio_mask  = ATA_PIO4,
@@ -120,6 +140,8 @@ MODULE_DEVICE_TABLE(of, ahci_mvebu_of_match);
 static struct platform_driver ahci_mvebu_driver = {
 	.probe = ahci_mvebu_probe,
 	.remove = ata_platform_remove_one,
+	.suspend = ahci_mvebu_suspend,
+	.resume = ahci_mvebu_resume,
 	.driver = {
 		.name = DRV_NAME,
 		.of_match_table = ahci_mvebu_of_match,
diff --git a/drivers/ata/ahci_platform.c b/drivers/ata/ahci_platform.c
index 78d6ae0b90c4..1befb114c384 100644
--- a/drivers/ata/ahci_platform.c
+++ b/drivers/ata/ahci_platform.c
@@ -20,6 +20,8 @@
 #include <linux/platform_device.h>
 #include <linux/libata.h>
 #include <linux/ahci_platform.h>
+#include <linux/acpi.h>
+#include <linux/pci_ids.h>
 #include "ahci.h"
 
 #define DRV_NAME "ahci"
@@ -74,16 +76,24 @@ static const struct of_device_id ahci_of_match[] = {
 	{ .compatible = "ibm,476gtr-ahci", },
 	{ .compatible = "snps,dwc-ahci", },
 	{ .compatible = "hisilicon,hisi-ahci", },
+	{ .compatible = "fsl,qoriq-ahci", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, ahci_of_match);
 
+static const struct acpi_device_id ahci_acpi_match[] = {
+	{ ACPI_DEVICE_CLASS(PCI_CLASS_STORAGE_SATA_AHCI, 0xffffff) },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, ahci_acpi_match);
+
 static struct platform_driver ahci_driver = {
 	.probe = ahci_probe,
 	.remove = ata_platform_remove_one,
 	.driver = {
 		.name = DRV_NAME,
 		.of_match_table = ahci_of_match,
+		.acpi_match_table = ahci_acpi_match,
 		.pm = &ahci_pm_ops,
 	},
 };
diff --git a/drivers/ata/ahci_xgene.c b/drivers/ata/ahci_xgene.c
index 2b78510d94dd..e2c6d9e0c5ac 100644
--- a/drivers/ata/ahci_xgene.c
+++ b/drivers/ata/ahci_xgene.c
@@ -27,6 +27,7 @@
 #include <linux/platform_device.h>
 #include <linux/ahci_platform.h>
 #include <linux/of_address.h>
+#include <linux/of_device.h>
 #include <linux/of_irq.h>
 #include <linux/phy/phy.h>
 #include "ahci.h"
@@ -84,6 +85,11 @@
 /* Max retry for link down */
 #define MAX_LINK_DOWN_RETRY 3
 
+enum xgene_ahci_version {
+	XGENE_AHCI_V1 = 1,
+	XGENE_AHCI_V2,
+};
+
 struct xgene_ahci_context {
 	struct ahci_host_priv *hpriv;
 	struct device *dev;
@@ -542,7 +548,7 @@ softreset_retry:
 	return rc;
 }
 
-static struct ata_port_operations xgene_ahci_ops = {
+static struct ata_port_operations xgene_ahci_v1_ops = {
 	.inherits = &ahci_ops,
 	.host_stop = xgene_ahci_host_stop,
 	.hardreset = xgene_ahci_hardreset,
@@ -552,11 +558,25 @@ static struct ata_port_operations xgene_ahci_ops = {
 	.pmp_softreset = xgene_ahci_pmp_softreset
 };
 
-static const struct ata_port_info xgene_ahci_port_info = {
+static const struct ata_port_info xgene_ahci_v1_port_info = {
+	.flags = AHCI_FLAG_COMMON | ATA_FLAG_PMP,
+	.pio_mask = ATA_PIO4,
+	.udma_mask = ATA_UDMA6,
+	.port_ops = &xgene_ahci_v1_ops,
+};
+
+static struct ata_port_operations xgene_ahci_v2_ops = {
+	.inherits = &ahci_ops,
+	.host_stop = xgene_ahci_host_stop,
+	.hardreset = xgene_ahci_hardreset,
+	.read_id = xgene_ahci_read_id,
+};
+
+static const struct ata_port_info xgene_ahci_v2_port_info = {
 	.flags = AHCI_FLAG_COMMON | ATA_FLAG_PMP,
 	.pio_mask = ATA_PIO4,
 	.udma_mask = ATA_UDMA6,
-	.port_ops = &xgene_ahci_ops,
+	.port_ops = &xgene_ahci_v2_ops,
 };
 
 static int xgene_ahci_hw_init(struct ahci_host_priv *hpriv)
@@ -629,12 +649,32 @@ static struct scsi_host_template ahci_platform_sht = {
 	AHCI_SHT(DRV_NAME),
 };
 
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id xgene_ahci_acpi_match[] = {
+	{ "APMC0D0D", XGENE_AHCI_V1},
+	{ "APMC0D32", XGENE_AHCI_V2},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, xgene_ahci_acpi_match);
+#endif
+
+static const struct of_device_id xgene_ahci_of_match[] = {
+	{.compatible = "apm,xgene-ahci", .data = (void *) XGENE_AHCI_V1},
+	{.compatible = "apm,xgene-ahci-v2", .data = (void *) XGENE_AHCI_V2},
+	{},
+};
+MODULE_DEVICE_TABLE(of, xgene_ahci_of_match);
+
 static int xgene_ahci_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct ahci_host_priv *hpriv;
 	struct xgene_ahci_context *ctx;
 	struct resource *res;
+	const struct of_device_id *of_devid;
+	enum xgene_ahci_version version = XGENE_AHCI_V1;
+	const struct ata_port_info *ppi[] = { &xgene_ahci_v1_port_info,
+					      &xgene_ahci_v2_port_info };
 	int rc;
 
 	hpriv = ahci_platform_get_resources(pdev);
@@ -677,6 +717,35 @@ static int xgene_ahci_probe(struct platform_device *pdev)
 		ctx->csr_mux = csr;
 	}
 
+	of_devid = of_match_device(xgene_ahci_of_match, dev);
+	if (of_devid) {
+		if (of_devid->data)
+			version = (enum xgene_ahci_version) of_devid->data;
+	}
+#ifdef CONFIG_ACPI
+	else {
+		const struct acpi_device_id *acpi_id;
+		struct acpi_device_info *info;
+		acpi_status status;
+
+		acpi_id = acpi_match_device(xgene_ahci_acpi_match, &pdev->dev);
+		if (!acpi_id) {
+			dev_warn(&pdev->dev, "No node entry in ACPI table. Assume version1\n");
+			version = XGENE_AHCI_V1;
+		} else if (acpi_id->driver_data) {
+			version = (enum xgene_ahci_version) acpi_id->driver_data;
+			status = acpi_get_object_info(ACPI_HANDLE(&pdev->dev), &info);
+			if (ACPI_FAILURE(status)) {
+				dev_warn(&pdev->dev, "%s: Error reading device info. Assume version1\n",
+					__func__);
+				version = XGENE_AHCI_V1;
+			}
+			if (info->valid & ACPI_VALID_CID)
+				version = XGENE_AHCI_V2;
+		}
+	}
+#endif
+
 	dev_dbg(dev, "VAddr 0x%p Mmio VAddr 0x%p\n", ctx->csr_core,
 		hpriv->mmio);
 
@@ -704,9 +773,19 @@ static int xgene_ahci_probe(struct platform_device *pdev)
 	/* Configure the host controller */
 	xgene_ahci_hw_init(hpriv);
 skip_clk_phy:
-	hpriv->flags = AHCI_HFLAG_NO_PMP | AHCI_HFLAG_NO_NCQ;
 
-	rc = ahci_platform_init_host(pdev, hpriv, &xgene_ahci_port_info,
+	switch (version) {
+	case XGENE_AHCI_V1:
+		hpriv->flags = AHCI_HFLAG_NO_NCQ;
+		break;
+	case XGENE_AHCI_V2:
+		hpriv->flags |= AHCI_HFLAG_YES_FBS | AHCI_HFLAG_EDGE_IRQ;
+		break;
+	default:
+		break;
+	}
+
+	rc = ahci_platform_init_host(pdev, hpriv, ppi[version - 1],
 				     &ahci_platform_sht);
 	if (rc)
 		goto disable_resources;
@@ -719,20 +798,6 @@ disable_resources:
 	return rc;
 }
 
-#ifdef CONFIG_ACPI
-static const struct acpi_device_id xgene_ahci_acpi_match[] = {
-	{ "APMC0D0D", },
-	{ }
-};
-MODULE_DEVICE_TABLE(acpi, xgene_ahci_acpi_match);
-#endif
-
-static const struct of_device_id xgene_ahci_of_match[] = {
-	{.compatible = "apm,xgene-ahci"},
-	{},
-};
-MODULE_DEVICE_TABLE(of, xgene_ahci_of_match);
-
 static struct platform_driver xgene_ahci_driver = {
 	.probe = xgene_ahci_probe,
 	.remove = ata_platform_remove_one,
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 287c4ba0219f..d256a66158be 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -1825,11 +1825,38 @@ static irqreturn_t ahci_multi_irqs_intr(int irq, void *dev_instance)
 	return IRQ_WAKE_THREAD;
 }
 
-static irqreturn_t ahci_single_irq_intr(int irq, void *dev_instance)
+static u32 ahci_handle_port_intr(struct ata_host *host, u32 irq_masked)
+{
+	unsigned int i, handled = 0;
+
+	for (i = 0; i < host->n_ports; i++) {
+		struct ata_port *ap;
+
+		if (!(irq_masked & (1 << i)))
+			continue;
+
+		ap = host->ports[i];
+		if (ap) {
+			ahci_port_intr(ap);
+			VPRINTK("port %u\n", i);
+		} else {
+			VPRINTK("port %u (no irq)\n", i);
+			if (ata_ratelimit())
+				dev_warn(host->dev,
+					 "interrupt on disabled port %u\n", i);
+		}
+
+		handled = 1;
+	}
+
+	return handled;
+}
+
+static irqreturn_t ahci_single_edge_irq_intr(int irq, void *dev_instance)
 {
 	struct ata_host *host = dev_instance;
 	struct ahci_host_priv *hpriv;
-	unsigned int i, handled = 0;
+	unsigned int rc = 0;
 	void __iomem *mmio;
 	u32 irq_stat, irq_masked;
 
@@ -1847,25 +1874,44 @@ static irqreturn_t ahci_single_irq_intr(int irq, void *dev_instance)
 
 	spin_lock(&host->lock);
 
-	for (i = 0; i < host->n_ports; i++) {
-		struct ata_port *ap;
+	/*
+	 * HOST_IRQ_STAT behaves as edge triggered latch meaning that
+	 * it should be cleared before all the port events are cleared.
+	 */
+	writel(irq_stat, mmio + HOST_IRQ_STAT);
 
-		if (!(irq_masked & (1 << i)))
-			continue;
+	rc = ahci_handle_port_intr(host, irq_masked);
 
-		ap = host->ports[i];
-		if (ap) {
-			ahci_port_intr(ap);
-			VPRINTK("port %u\n", i);
-		} else {
-			VPRINTK("port %u (no irq)\n", i);
-			if (ata_ratelimit())
-				dev_warn(host->dev,
-					 "interrupt on disabled port %u\n", i);
-		}
+	spin_unlock(&host->lock);
 
-		handled = 1;
-	}
+	VPRINTK("EXIT\n");
+
+	return IRQ_RETVAL(rc);
+}
+
+static irqreturn_t ahci_single_level_irq_intr(int irq, void *dev_instance)
+{
+	struct ata_host *host = dev_instance;
+	struct ahci_host_priv *hpriv;
+	unsigned int rc = 0;
+	void __iomem *mmio;
+	u32 irq_stat, irq_masked;
+
+	VPRINTK("ENTER\n");
+
+	hpriv = host->private_data;
+	mmio = hpriv->mmio;
+
+	/* sigh.  0xffffffff is a valid return from h/w */
+	irq_stat = readl(mmio + HOST_IRQ_STAT);
+	if (!irq_stat)
+		return IRQ_NONE;
+
+	irq_masked = irq_stat & hpriv->port_map;
+
+	spin_lock(&host->lock);
+
+	rc = ahci_handle_port_intr(host, irq_masked);
 
 	/* HOST_IRQ_STAT behaves as level triggered latch meaning that
 	 * it should be cleared after all the port events are cleared;
@@ -1882,7 +1928,7 @@ static irqreturn_t ahci_single_irq_intr(int irq, void *dev_instance)
 
 	VPRINTK("EXIT\n");
 
-	return IRQ_RETVAL(handled);
+	return IRQ_RETVAL(rc);
 }
 
 unsigned int ahci_qc_issue(struct ata_queued_cmd *qc)
@@ -2297,7 +2343,7 @@ static int ahci_port_start(struct ata_port *ap)
 	/*
 	 * Switch to per-port locking in case each port has its own MSI vector.
 	 */
-	if ((hpriv->flags & AHCI_HFLAG_MULTI_MSI)) {
+	if (hpriv->flags & AHCI_HFLAG_MULTI_MSI) {
 		spin_lock_init(&pp->lock);
 		ap->lock = &pp->lock;
 	}
@@ -2425,7 +2471,10 @@ static int ahci_host_activate_multi_irqs(struct ata_host *host, int irq,
 	rc = ata_host_start(host);
 	if (rc)
 		return rc;
-
+	/*
+	 * Requests IRQs according to AHCI-1.1 when multiple MSIs were
+	 * allocated. That is one MSI per port, starting from @irq.
+	 */
 	for (i = 0; i < host->n_ports; i++) {
 		struct ahci_port_priv *pp = host->ports[i]->private_data;
 
@@ -2464,29 +2513,27 @@ out_free_irqs:
 /**
  *	ahci_host_activate - start AHCI host, request IRQs and register it
  *	@host: target ATA host
- *	@irq: base IRQ number to request
  *	@sht: scsi_host_template to use when registering the host
  *
- *	Similar to ata_host_activate, but requests IRQs according to AHCI-1.1
- *	when multiple MSIs were allocated. That is one MSI per port, starting
- *	from @irq.
- *
  *	LOCKING:
  *	Inherited from calling layer (may sleep).
  *
  *	RETURNS:
  *	0 on success, -errno otherwise.
  */
-int ahci_host_activate(struct ata_host *host, int irq,
-		       struct scsi_host_template *sht)
+int ahci_host_activate(struct ata_host *host, struct scsi_host_template *sht)
 {
 	struct ahci_host_priv *hpriv = host->private_data;
+	int irq = hpriv->irq;
 	int rc;
 
 	if (hpriv->flags & AHCI_HFLAG_MULTI_MSI)
 		rc = ahci_host_activate_multi_irqs(host, irq, sht);
+	else if (hpriv->flags & AHCI_HFLAG_EDGE_IRQ)
+		rc = ata_host_activate(host, irq, ahci_single_edge_irq_intr,
+				       IRQF_SHARED, sht);
 	else
-		rc = ata_host_activate(host, irq, ahci_single_irq_intr,
+		rc = ata_host_activate(host, irq, ahci_single_level_irq_intr,
 				       IRQF_SHARED, sht);
 	return rc;
 }
diff --git a/drivers/ata/libahci_platform.c b/drivers/ata/libahci_platform.c
index d89305d289f6..aaa761b9081c 100644
--- a/drivers/ata/libahci_platform.c
+++ b/drivers/ata/libahci_platform.c
@@ -518,6 +518,8 @@ int ahci_platform_init_host(struct platform_device *pdev,
 		return -EINVAL;
 	}
 
+	hpriv->irq = irq;
+
 	/* prepare host */
 	pi.private_data = (void *)(unsigned long)hpriv->flags;
 
@@ -588,7 +590,7 @@ int ahci_platform_init_host(struct platform_device *pdev,
 	ahci_init_controller(host);
 	ahci_print_info(host, "platform");
 
-	return ahci_host_activate(host, irq, sht);
+	return ahci_host_activate(host, sht);
 }
 EXPORT_SYMBOL_GPL(ahci_platform_init_host);
 
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index 577849c6611a..e83fc3d0da9c 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -3654,7 +3654,7 @@ int sata_link_resume(struct ata_link *link, const unsigned long *params,
  *	EH context.
  *
  *	RETURNS:
- *	0 on succes, -errno otherwise.
+ *	0 on success, -errno otherwise.
  */
 int sata_link_scr_lpm(struct ata_link *link, enum ata_lpm_policy policy,
 		      bool spm_wakeup)
@@ -4225,7 +4225,7 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	{ "PIONEER DVD-RW  DVR-216D",	NULL,	ATA_HORKAGE_NOSETXFER },
 
 	/* devices that don't properly handle queued TRIM commands */
-	{ "Micron_M500*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM |
+	{ "Micron_M500_*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM |
 						ATA_HORKAGE_ZERO_AFTER_TRIM, },
 	{ "Crucial_CT*M500*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM |
 						ATA_HORKAGE_ZERO_AFTER_TRIM, },
@@ -6456,12 +6456,7 @@ static int __init ata_parse_force_one(char **cur,
 				      struct ata_force_ent *force_ent,
 				      const char **reason)
 {
-	/* FIXME: Currently, there's no way to tag init const data and
-	 * using __initdata causes build failure on some versions of
-	 * gcc.  Once __initdataconst is implemented, add const to the
-	 * following structure.
-	 */
-	static struct ata_force_param force_tbl[] __initdata = {
+	static const struct ata_force_param force_tbl[] __initconst = {
 		{ "40c",	.cbl		= ATA_CBL_PATA40 },
 		{ "80c",	.cbl		= ATA_CBL_PATA80 },
 		{ "short40c",	.cbl		= ATA_CBL_PATA40_SHORT },
@@ -6472,6 +6467,8 @@ static int __init ata_parse_force_one(char **cur,
 		{ "3.0Gbps",	.spd_limit	= 2 },
 		{ "noncq",	.horkage_on	= ATA_HORKAGE_NONCQ },
 		{ "ncq",	.horkage_off	= ATA_HORKAGE_NONCQ },
+		{ "noncqtrim",	.horkage_on	= ATA_HORKAGE_NO_NCQ_TRIM },
+		{ "ncqtrim",	.horkage_off	= ATA_HORKAGE_NO_NCQ_TRIM },
 		{ "dump_id",	.horkage_on	= ATA_HORKAGE_DUMP_ID },
 		{ "pio0",	.xfer_mask	= 1 << (ATA_SHIFT_PIO + 0) },
 		{ "pio1",	.xfer_mask	= 1 << (ATA_SHIFT_PIO + 1) },
diff --git a/drivers/ata/libata-eh.c b/drivers/ata/libata-eh.c
index cf0022ec07f2..7465031a893c 100644
--- a/drivers/ata/libata-eh.c
+++ b/drivers/ata/libata-eh.c
@@ -1507,16 +1507,21 @@ unsigned int ata_read_log_page(struct ata_device *dev, u8 log,
 {
 	struct ata_taskfile tf;
 	unsigned int err_mask;
+	bool dma = false;
 
 	DPRINTK("read log page - log 0x%x, page 0x%x\n", log, page);
 
+retry:
 	ata_tf_init(dev, &tf);
-	if (dev->dma_mode && ata_id_has_read_log_dma_ext(dev->id)) {
+	if (dev->dma_mode && ata_id_has_read_log_dma_ext(dev->id) &&
+	    !(dev->horkage & ATA_HORKAGE_NO_NCQ_LOG)) {
 		tf.command = ATA_CMD_READ_LOG_DMA_EXT;
 		tf.protocol = ATA_PROT_DMA;
+		dma = true;
 	} else {
 		tf.command = ATA_CMD_READ_LOG_EXT;
 		tf.protocol = ATA_PROT_PIO;
+		dma = false;
 	}
 	tf.lbal = log;
 	tf.lbam = page;
@@ -1527,6 +1532,12 @@ unsigned int ata_read_log_page(struct ata_device *dev, u8 log,
 	err_mask = ata_exec_internal(dev, &tf, NULL, DMA_FROM_DEVICE,
 				     buf, sectors * ATA_SECT_SIZE, 0);
 
+	if (err_mask && dma) {
+		dev->horkage |= ATA_HORKAGE_NO_NCQ_LOG;
+		ata_dev_warn(dev, "READ LOG DMA EXT failed, trying unqueued\n");
+		goto retry;
+	}
+
 	DPRINTK("EXIT, err_mask=%x\n", err_mask);
 	return err_mask;
 }
diff --git a/drivers/ata/libata-transport.c b/drivers/ata/libata-transport.c
index 3227b7c8a05f..d6c37bcd416d 100644
--- a/drivers/ata/libata-transport.c
+++ b/drivers/ata/libata-transport.c
@@ -560,6 +560,27 @@ show_ata_dev_gscr(struct device *dev,
 
 static DEVICE_ATTR(gscr, S_IRUGO, show_ata_dev_gscr, NULL);
 
+static ssize_t
+show_ata_dev_trim(struct device *dev,
+		  struct device_attribute *attr, char *buf)
+{
+	struct ata_device *ata_dev = transport_class_to_dev(dev);
+	unsigned char *mode;
+
+	if (!ata_id_has_trim(ata_dev->id))
+		mode = "unsupported";
+	else if (ata_dev->horkage & ATA_HORKAGE_NO_NCQ_TRIM)
+			mode = "forced_unqueued";
+	else if (ata_fpdma_dsm_supported(ata_dev))
+		mode = "queued";
+	else
+		mode = "unqueued";
+
+	return snprintf(buf, 20, "%s\n", mode);
+}
+
+static DEVICE_ATTR(trim, S_IRUGO, show_ata_dev_trim, NULL);
+
 static DECLARE_TRANSPORT_CLASS(ata_dev_class,
 			       "ata_device", NULL, NULL, NULL);
 
@@ -733,6 +754,7 @@ struct scsi_transport_template *ata_attach_transport(void)
 	SETUP_DEV_ATTRIBUTE(ering);
 	SETUP_DEV_ATTRIBUTE(id);
 	SETUP_DEV_ATTRIBUTE(gscr);
+	SETUP_DEV_ATTRIBUTE(trim);
 	BUG_ON(count > ATA_DEV_ATTRS);
 	i->dev_attrs[count] = NULL;
 
diff --git a/drivers/ata/pata_at91.c b/drivers/ata/pata_at91.c
index 9e85937d36a9..ace0a4de3449 100644
--- a/drivers/ata/pata_at91.c
+++ b/drivers/ata/pata_at91.c
@@ -24,11 +24,13 @@
 #include <linux/ata.h>
 #include <linux/clk.h>
 #include <linux/libata.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mfd/syscon/atmel-smc.h>
 #include <linux/platform_device.h>
 #include <linux/ata_platform.h>
 #include <linux/platform_data/atmel.h>
+#include <linux/regmap.h>
 
-#include <mach/at91sam9_smc.h>
 #include <asm/gpio.h>
 
 #define DRV_NAME		"pata_at91"
@@ -57,6 +59,15 @@ struct smc_range {
 	int max;
 };
 
+struct regmap *smc;
+
+struct at91sam9_smc_generic_fields {
+	struct regmap_field *setup;
+	struct regmap_field *pulse;
+	struct regmap_field *cycle;
+	struct regmap_field *mode;
+} fields;
+
 /**
  * adjust_smc_value - adjust value for one of SMC registers.
  * @value: adjusted value
@@ -206,7 +217,6 @@ static void set_smc_timing(struct device *dev, struct ata_device *adev,
 {
 	int ret = 0;
 	int use_iordy;
-	struct sam9_smc_config smc;
 	unsigned int t6z;         /* data tristate time in ns */
 	unsigned int cycle;       /* SMC Cycle width in MCK ticks */
 	unsigned int setup;       /* SMC Setup width in MCK ticks */
@@ -244,19 +254,21 @@ static void set_smc_timing(struct device *dev, struct ata_device *adev,
 
 	dev_dbg(dev, "Use IORDY=%u, TDF Cycles=%u\n", use_iordy, tdf_cycles);
 
-	/* SMC Setup Register */
-	smc.nwe_setup = smc.nrd_setup = setup;
-	smc.ncs_write_setup = smc.ncs_read_setup = 0;
-	/* SMC Pulse Register */
-	smc.nwe_pulse = smc.nrd_pulse = pulse;
-	smc.ncs_write_pulse = smc.ncs_read_pulse = cs_pulse;
-	/* SMC Cycle Register */
-	smc.write_cycle = smc.read_cycle = cycle;
-	/* SMC Mode Register*/
-	smc.tdf_cycles = tdf_cycles;
-	smc.mode = info->mode;
-
-	sam9_smc_configure(0, info->cs, &smc);
+	regmap_fields_write(fields.setup, info->cs,
+			    AT91SAM9_SMC_NRDSETUP(setup) |
+			    AT91SAM9_SMC_NWESETUP(setup) |
+			    AT91SAM9_SMC_NCS_NRDSETUP(0) |
+			    AT91SAM9_SMC_NCS_WRSETUP(0));
+	regmap_fields_write(fields.pulse, info->cs,
+			    AT91SAM9_SMC_NRDPULSE(pulse) |
+			    AT91SAM9_SMC_NWEPULSE(pulse) |
+			    AT91SAM9_SMC_NCS_NRDPULSE(cs_pulse) |
+			    AT91SAM9_SMC_NCS_WRPULSE(cs_pulse));
+	regmap_fields_write(fields.cycle, info->cs,
+			    AT91SAM9_SMC_NRDCYCLE(cycle) |
+			    AT91SAM9_SMC_NWECYCLE(cycle));
+	regmap_fields_write(fields.mode, info->cs, info->mode |
+			    AT91_SMC_TDF_(tdf_cycles));
 }
 
 static void pata_at91_set_piomode(struct ata_port *ap, struct ata_device *adev)
@@ -280,21 +292,21 @@ static unsigned int pata_at91_data_xfer_noirq(struct ata_device *dev,
 {
 	struct at91_ide_info *info = dev->link->ap->host->private_data;
 	unsigned int consumed;
+	unsigned int mode;
 	unsigned long flags;
-	struct sam9_smc_config smc;
 
 	local_irq_save(flags);
-	sam9_smc_read_mode(0, info->cs, &smc);
+	regmap_fields_read(fields.mode, info->cs, &mode);
 
 	/* set 16bit mode before writing data */
-	smc.mode = (smc.mode & ~AT91_SMC_DBW) | AT91_SMC_DBW_16;
-	sam9_smc_write_mode(0, info->cs, &smc);
+	regmap_fields_write(fields.mode, info->cs, (mode & ~AT91_SMC_DBW) |
+			    AT91_SMC_DBW_16);
 
 	consumed = ata_sff_data_xfer(dev, buf, buflen, rw);
 
 	/* restore 8bit mode after data is written */
-	smc.mode = (smc.mode & ~AT91_SMC_DBW) | AT91_SMC_DBW_8;
-	sam9_smc_write_mode(0, info->cs, &smc);
+	regmap_fields_write(fields.mode, info->cs, (mode & ~AT91_SMC_DBW) |
+			    AT91_SMC_DBW_8);
 
 	local_irq_restore(flags);
 	return consumed;
@@ -312,6 +324,36 @@ static struct ata_port_operations pata_at91_port_ops = {
 	.cable_detect	= ata_cable_40wire,
 };
 
+static int at91sam9_smc_fields_init(struct device *dev)
+{
+	struct reg_field field = REG_FIELD(0, 0, 31);
+
+	field.id_size = 8;
+	field.id_offset = AT91SAM9_SMC_GENERIC_BLK_SZ;
+
+	field.reg = AT91SAM9_SMC_SETUP(AT91SAM9_SMC_GENERIC);
+	fields.setup = devm_regmap_field_alloc(dev, smc, field);
+	if (IS_ERR(fields.setup))
+		return PTR_ERR(fields.setup);
+
+	field.reg = AT91SAM9_SMC_PULSE(AT91SAM9_SMC_GENERIC);
+	fields.pulse = devm_regmap_field_alloc(dev, smc, field);
+	if (IS_ERR(fields.pulse))
+		return PTR_ERR(fields.pulse);
+
+	field.reg = AT91SAM9_SMC_CYCLE(AT91SAM9_SMC_GENERIC);
+	fields.cycle = devm_regmap_field_alloc(dev, smc, field);
+	if (IS_ERR(fields.cycle))
+		return PTR_ERR(fields.cycle);
+
+	field.reg = AT91SAM9_SMC_MODE(AT91SAM9_SMC_GENERIC);
+	fields.mode = devm_regmap_field_alloc(dev, smc, field);
+	if (IS_ERR(fields.mode))
+		return PTR_ERR(fields.mode);
+
+	return 0;
+}
+
 static int pata_at91_probe(struct platform_device *pdev)
 {
 	struct at91_cf_data *board = dev_get_platdata(&pdev->dev);
@@ -341,6 +383,14 @@ static int pata_at91_probe(struct platform_device *pdev)
 
 	irq = board->irq_pin;
 
+	smc = syscon_regmap_lookup_by_phandle(pdev->dev.of_node, "atmel,smc");
+	if (IS_ERR(smc))
+		return PTR_ERR(smc);
+
+	ret = at91sam9_smc_fields_init(dev);
+	if (ret < 0)
+		return ret;
+
 	/* init ata host */
 
 	host = ata_host_alloc(dev, 1);
diff --git a/drivers/ata/pata_hpt366.c b/drivers/ata/pata_hpt366.c
index cbc3de793d1d..0038dc4c06c7 100644
--- a/drivers/ata/pata_hpt366.c
+++ b/drivers/ata/pata_hpt366.c
@@ -352,7 +352,7 @@ static int hpt36x_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 	};
 	const struct ata_port_info *ppi[] = { &info_hpt366, NULL };
 
-	void *hpriv = NULL;
+	const void *hpriv = NULL;
 	u32 reg1;
 	int rc;
 
@@ -383,7 +383,7 @@ static int hpt36x_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 		break;
 	}
 	/* Now kick off ATA set up */
-	return ata_pci_bmdma_init_one(dev, ppi, &hpt36x_sht, hpriv, 0);
+	return ata_pci_bmdma_init_one(dev, ppi, &hpt36x_sht, (void *)hpriv, 0);
 }
 
 #ifdef CONFIG_PM_SLEEP
diff --git a/drivers/ata/pata_octeon_cf.c b/drivers/ata/pata_octeon_cf.c
index 80a80548ad0a..27245957eee3 100644
--- a/drivers/ata/pata_octeon_cf.c
+++ b/drivers/ata/pata_octeon_cf.c
@@ -1053,7 +1053,7 @@ static struct of_device_id octeon_cf_match[] = {
 	},
 	{},
 };
-MODULE_DEVICE_TABLE(of, octeon_i2c_match);
+MODULE_DEVICE_TABLE(of, octeon_cf_match);
 
 static struct platform_driver octeon_cf_driver = {
 	.probe		= octeon_cf_probe,
diff --git a/drivers/ata/pata_samsung_cf.c b/drivers/ata/pata_samsung_cf.c
index fa44eb2872db..cbb5a471eb9d 100644
--- a/drivers/ata/pata_samsung_cf.c
+++ b/drivers/ata/pata_samsung_cf.c
@@ -638,7 +638,7 @@ static const struct dev_pm_ops pata_s3c_pm_ops = {
 #endif
 
 /* driver device registration */
-static struct platform_device_id pata_s3c_driver_ids[] = {
+static const struct platform_device_id pata_s3c_driver_ids[] = {
 	{
 		.name		= "s3c64xx-pata",
 		.driver_data	= TYPE_S3C64XX,
diff --git a/drivers/ata/sata_highbank.c b/drivers/ata/sata_highbank.c
index 24e311fe2c1c..8638d575b2b9 100644
--- a/drivers/ata/sata_highbank.c
+++ b/drivers/ata/sata_highbank.c
@@ -499,6 +499,7 @@ static int ahci_highbank_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	hpriv->irq = irq;
 	hpriv->flags |= (unsigned long)pi.private_data;
 
 	hpriv->mmio = devm_ioremap(dev, mem->start, resource_size(mem));
@@ -568,7 +569,7 @@ static int ahci_highbank_probe(struct platform_device *pdev)
 	ahci_init_controller(host);
 	ahci_print_info(host, "platform");
 
-	rc = ahci_host_activate(host, irq, &ahci_highbank_platform_sht);
+	rc = ahci_host_activate(host, &ahci_highbank_platform_sht);
 	if (rc)
 		goto err0;
 
diff --git a/drivers/ata/sata_nv.c b/drivers/ata/sata_nv.c
index 7ece85f43020..734f563b8d37 100644
--- a/drivers/ata/sata_nv.c
+++ b/drivers/ata/sata_nv.c
@@ -599,7 +599,7 @@ MODULE_DEVICE_TABLE(pci, nv_pci_tbl);
 MODULE_VERSION(DRV_VERSION);
 
 static bool adma_enabled;
-static bool swncq_enabled = 1;
+static bool swncq_enabled = true;
 static bool msi_enabled;
 
 static void nv_adma_register_mode(struct ata_port *ap)
diff --git a/drivers/atm/he.c b/drivers/atm/he.c
index 93dca2e73bf5..a8da3a50e374 100644
--- a/drivers/atm/he.c
+++ b/drivers/atm/he.c
@@ -116,8 +116,8 @@ static bool disable64;
 static short nvpibits = -1;
 static short nvcibits = -1;
 static short rx_skb_reserve = 16;
-static bool irq_coalesce = 1;
-static bool sdh = 0;
+static bool irq_coalesce = true;
+static bool sdh;
 
 /* Read from EEPROM = 0000 0011b */
 static unsigned int readtab[] = {
diff --git a/drivers/atm/idt77105.c b/drivers/atm/idt77105.c
index 909c95bd7be2..feb023d7eebd 100644
--- a/drivers/atm/idt77105.c
+++ b/drivers/atm/idt77105.c
@@ -306,14 +306,12 @@ static int idt77105_start(struct atm_dev *dev)
 	if (start_timer) {
 		start_timer = 0;
                 
-		init_timer(&stats_timer);
+		setup_timer(&stats_timer, idt77105_stats_timer_func, 0UL);
 		stats_timer.expires = jiffies+IDT77105_STATS_TIMER_PERIOD;
-		stats_timer.function = idt77105_stats_timer_func;
 		add_timer(&stats_timer);
                 
-		init_timer(&restart_timer);
+		setup_timer(&restart_timer, idt77105_restart_timer_func, 0UL);
 		restart_timer.expires = jiffies+IDT77105_RESTART_TIMER_PERIOD;
-		restart_timer.function = idt77105_restart_timer_func;
 		add_timer(&restart_timer);
 	}
 	spin_unlock_irqrestore(&idt77105_priv_lock, flags);
diff --git a/drivers/atm/iphase.c b/drivers/atm/iphase.c
index 924f8e26789d..65e65903faa0 100644
--- a/drivers/atm/iphase.c
+++ b/drivers/atm/iphase.c
@@ -2618,7 +2618,7 @@ static void ia_close(struct atm_vcc *vcc)
         if (vcc->qos.txtp.traffic_class != ATM_NONE) {
            iadev->close_pending++;
 	   prepare_to_wait(&iadev->timeout_wait, &wait, TASK_UNINTERRUPTIBLE);
-	   schedule_timeout(50);
+	   schedule_timeout(msecs_to_jiffies(500));
 	   finish_wait(&iadev->timeout_wait, &wait);
            spin_lock_irqsave(&iadev->tx_lock, flags); 
            while((skb = skb_dequeue(&iadev->tx_backlog))) {
diff --git a/drivers/base/base.h b/drivers/base/base.h
index 251c5d30f963..fd3347d9f153 100644
--- a/drivers/base/base.h
+++ b/drivers/base/base.h
@@ -116,6 +116,7 @@ static inline int driver_match_device(struct device_driver *drv,
 {
 	return drv->bus->match ? drv->bus->match(dev, drv) : 1;
 }
+extern bool driver_allows_async_probing(struct device_driver *drv);
 
 extern int driver_add_groups(struct device_driver *drv,
 			     const struct attribute_group **groups);
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 79bc203f51ef..500592486e88 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -10,6 +10,7 @@
  *
  */
 
+#include <linux/async.h>
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/errno.h>
@@ -549,15 +550,12 @@ void bus_probe_device(struct device *dev)
 {
 	struct bus_type *bus = dev->bus;
 	struct subsys_interface *sif;
-	int ret;
 
 	if (!bus)
 		return;
 
-	if (bus->p->drivers_autoprobe) {
-		ret = device_attach(dev);
-		WARN_ON(ret < 0);
-	}
+	if (bus->p->drivers_autoprobe)
+		device_initial_probe(dev);
 
 	mutex_lock(&bus->p->mutex);
 	list_for_each_entry(sif, &bus->p->interfaces, node)
@@ -659,6 +657,17 @@ static ssize_t uevent_store(struct device_driver *drv, const char *buf,
 }
 static DRIVER_ATTR_WO(uevent);
 
+static void driver_attach_async(void *_drv, async_cookie_t cookie)
+{
+	struct device_driver *drv = _drv;
+	int ret;
+
+	ret = driver_attach(drv);
+
+	pr_debug("bus: '%s': driver %s async attach completed: %d\n",
+		 drv->bus->name, drv->name, ret);
+}
+
 /**
  * bus_add_driver - Add a driver to the bus.
  * @drv: driver.
@@ -691,9 +700,15 @@ int bus_add_driver(struct device_driver *drv)
 
 	klist_add_tail(&priv->knode_bus, &bus->p->klist_drivers);
 	if (drv->bus->p->drivers_autoprobe) {
-		error = driver_attach(drv);
-		if (error)
-			goto out_unregister;
+		if (driver_allows_async_probing(drv)) {
+			pr_debug("bus: '%s': probing driver %s asynchronously\n",
+				drv->bus->name, drv->name);
+			async_schedule(driver_attach_async, drv);
+		} else {
+			error = driver_attach(drv);
+			if (error)
+				goto out_unregister;
+		}
 	}
 	module_add_driver(drv->owner, drv);
 
diff --git a/drivers/base/cacheinfo.c b/drivers/base/cacheinfo.c
index 9c2ba1c97c42..764280a91776 100644
--- a/drivers/base/cacheinfo.c
+++ b/drivers/base/cacheinfo.c
@@ -179,7 +179,7 @@ static int detect_cache_attributes(unsigned int cpu)
 {
 	int ret;
 
-	if (init_cache_level(cpu))
+	if (init_cache_level(cpu) || !cache_leaves(cpu))
 		return -ENOENT;
 
 	per_cpu_cacheinfo(cpu) = kcalloc(cache_leaves(cpu),
@@ -191,12 +191,12 @@ static int detect_cache_attributes(unsigned int cpu)
 	if (ret)
 		goto free_ci;
 	/*
-	 * For systems using DT for cache hierarcy, of_node and shared_cpu_map
+	 * For systems using DT for cache hierarchy, 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) {
-		pr_warn("Unable to detect cache hierarcy from DT for CPU %d\n",
+		pr_warn("Unable to detect cache hierarchy from DT for CPU %d\n",
 			cpu);
 		goto free_ci;
 	}
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 21d13038534e..dafae6d2f7ac 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -1303,12 +1303,11 @@ const char *device_get_devnode(struct device *dev,
 		return dev_name(dev);
 
 	/* replace '!' in the name with '/' */
-	*tmp = kstrdup(dev_name(dev), GFP_KERNEL);
-	if (!*tmp)
+	s = kstrdup(dev_name(dev), GFP_KERNEL);
+	if (!s)
 		return NULL;
-	while ((s = strchr(*tmp, '!')))
-		s[0] = '/';
-	return *tmp;
+	strreplace(s, '!', '/');
+	return *tmp = s;
 }
 
 /**
diff --git a/drivers/base/cpu.c b/drivers/base/cpu.c
index f160ea44a86d..78720e706176 100644
--- a/drivers/base/cpu.c
+++ b/drivers/base/cpu.c
@@ -16,6 +16,7 @@
 #include <linux/acpi.h>
 #include <linux/of.h>
 #include <linux/cpufeature.h>
+#include <linux/tick.h>
 
 #include "base.h"
 
@@ -265,6 +266,30 @@ static ssize_t print_cpus_offline(struct device *dev,
 }
 static DEVICE_ATTR(offline, 0444, print_cpus_offline, NULL);
 
+static ssize_t print_cpus_isolated(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	int n = 0, len = PAGE_SIZE-2;
+
+	n = scnprintf(buf, len, "%*pbl\n", cpumask_pr_args(cpu_isolated_map));
+
+	return n;
+}
+static DEVICE_ATTR(isolated, 0444, print_cpus_isolated, NULL);
+
+#ifdef CONFIG_NO_HZ_FULL
+static ssize_t print_cpus_nohz_full(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	int n = 0, len = PAGE_SIZE-2;
+
+	n = scnprintf(buf, len, "%*pbl\n", cpumask_pr_args(tick_nohz_full_mask));
+
+	return n;
+}
+static DEVICE_ATTR(nohz_full, 0444, print_cpus_nohz_full, NULL);
+#endif
+
 static void cpu_device_release(struct device *dev)
 {
 	/*
@@ -431,6 +456,10 @@ static struct attribute *cpu_root_attrs[] = {
 	&cpu_attrs[2].attr.attr,
 	&dev_attr_kernel_max.attr,
 	&dev_attr_offline.attr,
+	&dev_attr_isolated.attr,
+#ifdef CONFIG_NO_HZ_FULL
+	&dev_attr_nohz_full.attr,
+#endif
 #ifdef CONFIG_GENERIC_CPU_AUTOPROBE
 	&dev_attr_modalias.attr,
 #endif
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index e843fdbe4925..a638bbb1a27a 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -141,7 +141,7 @@ static bool driver_deferred_probe_enable = false;
  * more than one device is probing at the same time, it is possible for one
  * probe to complete successfully while another is about to defer. If the second
  * depends on the first, then it will get put on the pending list after the
- * trigger event has already occured and will be stuck there.
+ * trigger event has already occurred and will be stuck there.
  *
  * The atomic 'deferred_trigger_count' is used to determine if a successful
  * trigger has occurred in the midst of probing a driver. If the trigger count
@@ -417,31 +417,107 @@ int driver_probe_device(struct device_driver *drv, struct device *dev)
 	return ret;
 }
 
-static int __device_attach(struct device_driver *drv, void *data)
+bool driver_allows_async_probing(struct device_driver *drv)
 {
-	struct device *dev = data;
+	switch (drv->probe_type) {
+	case PROBE_PREFER_ASYNCHRONOUS:
+		return true;
+
+	case PROBE_FORCE_SYNCHRONOUS:
+		return false;
+
+	default:
+		if (module_requested_async_probing(drv->owner))
+			return true;
+
+		return false;
+	}
+}
+
+struct device_attach_data {
+	struct device *dev;
+
+	/*
+	 * Indicates whether we are are considering asynchronous probing or
+	 * not. Only initial binding after device or driver registration
+	 * (including deferral processing) may be done asynchronously, the
+	 * rest is always synchronous, as we expect it is being done by
+	 * request from userspace.
+	 */
+	bool check_async;
+
+	/*
+	 * Indicates if we are binding synchronous or asynchronous drivers.
+	 * When asynchronous probing is enabled we'll execute 2 passes
+	 * over drivers: first pass doing synchronous probing and second
+	 * doing asynchronous probing (if synchronous did not succeed -
+	 * most likely because there was no driver requiring synchronous
+	 * probing - and we found asynchronous driver during first pass).
+	 * The 2 passes are done because we can't shoot asynchronous
+	 * probe for given device and driver from bus_for_each_drv() since
+	 * driver pointer is not guaranteed to stay valid once
+	 * bus_for_each_drv() iterates to the next driver on the bus.
+	 */
+	bool want_async;
+
+	/*
+	 * We'll set have_async to 'true' if, while scanning for matching
+	 * driver, we'll encounter one that requests asynchronous probing.
+	 */
+	bool have_async;
+};
+
+static int __device_attach_driver(struct device_driver *drv, void *_data)
+{
+	struct device_attach_data *data = _data;
+	struct device *dev = data->dev;
+	bool async_allowed;
+
+	/*
+	 * Check if device has already been claimed. This may
+	 * happen with driver loading, device discovery/registration,
+	 * and deferred probe processing happens all at once with
+	 * multiple threads.
+	 */
+	if (dev->driver)
+		return -EBUSY;
 
 	if (!driver_match_device(drv, dev))
 		return 0;
 
+	async_allowed = driver_allows_async_probing(drv);
+
+	if (async_allowed)
+		data->have_async = true;
+
+	if (data->check_async && async_allowed != data->want_async)
+		return 0;
+
 	return driver_probe_device(drv, dev);
 }
 
-/**
- * device_attach - try to attach device to a driver.
- * @dev: device.
- *
- * Walk the list of drivers that the bus has and call
- * driver_probe_device() for each pair. If a compatible
- * pair is found, break out and return.
- *
- * Returns 1 if the device was bound to a driver;
- * 0 if no matching driver was found;
- * -ENODEV if the device is not registered.
- *
- * When called for a USB interface, @dev->parent lock must be held.
- */
-int device_attach(struct device *dev)
+static void __device_attach_async_helper(void *_dev, async_cookie_t cookie)
+{
+	struct device *dev = _dev;
+	struct device_attach_data data = {
+		.dev		= dev,
+		.check_async	= true,
+		.want_async	= true,
+	};
+
+	device_lock(dev);
+
+	bus_for_each_drv(dev->bus, NULL, &data, __device_attach_driver);
+	dev_dbg(dev, "async probe completed\n");
+
+	pm_request_idle(dev);
+
+	device_unlock(dev);
+
+	put_device(dev);
+}
+
+static int __device_attach(struct device *dev, bool allow_async)
 {
 	int ret = 0;
 
@@ -459,15 +535,59 @@ int device_attach(struct device *dev)
 			ret = 0;
 		}
 	} else {
-		ret = bus_for_each_drv(dev->bus, NULL, dev, __device_attach);
-		pm_request_idle(dev);
+		struct device_attach_data data = {
+			.dev = dev,
+			.check_async = allow_async,
+			.want_async = false,
+		};
+
+		ret = bus_for_each_drv(dev->bus, NULL, &data,
+					__device_attach_driver);
+		if (!ret && allow_async && data.have_async) {
+			/*
+			 * If we could not find appropriate driver
+			 * synchronously and we are allowed to do
+			 * async probes and there are drivers that
+			 * want to probe asynchronously, we'll
+			 * try them.
+			 */
+			dev_dbg(dev, "scheduling asynchronous probe\n");
+			get_device(dev);
+			async_schedule(__device_attach_async_helper, dev);
+		} else {
+			pm_request_idle(dev);
+		}
 	}
 out_unlock:
 	device_unlock(dev);
 	return ret;
 }
+
+/**
+ * device_attach - try to attach device to a driver.
+ * @dev: device.
+ *
+ * Walk the list of drivers that the bus has and call
+ * driver_probe_device() for each pair. If a compatible
+ * pair is found, break out and return.
+ *
+ * Returns 1 if the device was bound to a driver;
+ * 0 if no matching driver was found;
+ * -ENODEV if the device is not registered.
+ *
+ * When called for a USB interface, @dev->parent lock must be held.
+ */
+int device_attach(struct device *dev)
+{
+	return __device_attach(dev, false);
+}
 EXPORT_SYMBOL_GPL(device_attach);
 
+void device_initial_probe(struct device *dev)
+{
+	__device_attach(dev, true);
+}
+
 static int __driver_attach(struct device *dev, void *data)
 {
 	struct device_driver *drv = data;
@@ -522,6 +642,9 @@ static void __device_release_driver(struct device *dev)
 
 	drv = dev->driver;
 	if (drv) {
+		if (driver_allows_async_probing(drv))
+			async_synchronize_full();
+
 		pm_runtime_get_sync(dev);
 
 		driver_sysfs_remove(dev);
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 171841ad1008..894bda114224 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -150,17 +150,17 @@ struct firmware_buf {
 	int page_array_size;
 	struct list_head pending_list;
 #endif
-	char fw_id[];
+	const char *fw_id;
 };
 
 struct fw_cache_entry {
 	struct list_head list;
-	char name[];
+	const char *name;
 };
 
 struct fw_name_devm {
 	unsigned long magic;
-	char name[];
+	const char *name;
 };
 
 #define to_fwbuf(d) container_of(d, struct firmware_buf, ref)
@@ -181,13 +181,17 @@ static struct firmware_buf *__allocate_fw_buf(const char *fw_name,
 {
 	struct firmware_buf *buf;
 
-	buf = kzalloc(sizeof(*buf) + strlen(fw_name) + 1, GFP_ATOMIC);
-
+	buf = kzalloc(sizeof(*buf), GFP_ATOMIC);
 	if (!buf)
-		return buf;
+		return NULL;
+
+	buf->fw_id = kstrdup_const(fw_name, GFP_ATOMIC);
+	if (!buf->fw_id) {
+		kfree(buf);
+		return NULL;
+	}
 
 	kref_init(&buf->ref);
-	strcpy(buf->fw_id, fw_name);
 	buf->fwc = fwc;
 	init_completion(&buf->completion);
 #ifdef CONFIG_FW_LOADER_USER_HELPER
@@ -257,6 +261,7 @@ static void __fw_free_buf(struct kref *ref)
 	} else
 #endif
 		vfree(buf->data);
+	kfree_const(buf->fw_id);
 	kfree(buf);
 }
 
@@ -320,9 +325,13 @@ fail:
 static int fw_get_filesystem_firmware(struct device *device,
 				       struct firmware_buf *buf)
 {
-	int i;
+	int i, len;
 	int rc = -ENOENT;
-	char *path = __getname();
+	char *path;
+
+	path = __getname();
+	if (!path)
+		return -ENOMEM;
 
 	for (i = 0; i < ARRAY_SIZE(fw_path); i++) {
 		struct file *file;
@@ -331,7 +340,12 @@ static int fw_get_filesystem_firmware(struct device *device,
 		if (!fw_path[i][0])
 			continue;
 
-		snprintf(path, PATH_MAX, "%s/%s", fw_path[i], buf->fw_id);
+		len = snprintf(path, PATH_MAX, "%s/%s",
+			       fw_path[i], buf->fw_id);
+		if (len >= PATH_MAX) {
+			rc = -ENAMETOOLONG;
+			break;
+		}
 
 		file = filp_open(path, O_RDONLY, 0);
 		if (IS_ERR(file))
@@ -392,6 +406,7 @@ static void fw_name_devm_release(struct device *dev, void *res)
 	if (fwn->magic == (unsigned long)&fw_cache)
 		pr_debug("%s: fw_name-%s devm-%p released\n",
 				__func__, fwn->name, res);
+	kfree_const(fwn->name);
 }
 
 static int fw_devm_match(struct device *dev, void *res,
@@ -422,13 +437,17 @@ static int fw_add_devm_name(struct device *dev, const char *name)
 	if (fwn)
 		return 1;
 
-	fwn = devres_alloc(fw_name_devm_release, sizeof(struct fw_name_devm) +
-			   strlen(name) + 1, GFP_KERNEL);
+	fwn = devres_alloc(fw_name_devm_release, sizeof(struct fw_name_devm),
+			   GFP_KERNEL);
 	if (!fwn)
 		return -ENOMEM;
+	fwn->name = kstrdup_const(name, GFP_KERNEL);
+	if (!fwn->name) {
+		kfree(fwn);
+		return -ENOMEM;
+	}
 
 	fwn->magic = (unsigned long)&fw_cache;
-	strcpy(fwn->name, name);
 	devres_add(dev, fwn);
 
 	return 0;
@@ -544,10 +563,8 @@ static void fw_dev_release(struct device *dev)
 	kfree(fw_priv);
 }
 
-static int firmware_uevent(struct device *dev, struct kobj_uevent_env *env)
+static int do_firmware_uevent(struct firmware_priv *fw_priv, struct kobj_uevent_env *env)
 {
-	struct firmware_priv *fw_priv = to_firmware_priv(dev);
-
 	if (add_uevent_var(env, "FIRMWARE=%s", fw_priv->buf->fw_id))
 		return -ENOMEM;
 	if (add_uevent_var(env, "TIMEOUT=%i", loading_timeout))
@@ -558,6 +575,18 @@ static int firmware_uevent(struct device *dev, struct kobj_uevent_env *env)
 	return 0;
 }
 
+static int firmware_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	struct firmware_priv *fw_priv = to_firmware_priv(dev);
+	int err = 0;
+
+	mutex_lock(&fw_lock);
+	if (fw_priv->buf)
+		err = do_firmware_uevent(fw_priv, env);
+	mutex_unlock(&fw_lock);
+	return err;
+}
+
 static struct class firmware_class = {
 	.name		= "firmware",
 	.class_attrs	= firmware_class_attrs,
@@ -1247,6 +1276,7 @@ static void request_firmware_work_func(struct work_struct *work)
 	put_device(fw_work->device); /* taken in request_firmware_nowait() */
 
 	module_put(fw_work->module);
+	kfree_const(fw_work->name);
 	kfree(fw_work);
 }
 
@@ -1286,7 +1316,11 @@ request_firmware_nowait(
 		return -ENOMEM;
 
 	fw_work->module = module;
-	fw_work->name = name;
+	fw_work->name = kstrdup_const(name, gfp);
+	if (!fw_work->name) {
+		kfree(fw_work);
+		return -ENOMEM;
+	}
 	fw_work->device = device;
 	fw_work->context = context;
 	fw_work->cont = cont;
@@ -1294,6 +1328,7 @@ request_firmware_nowait(
 		(uevent ? FW_OPT_UEVENT : FW_OPT_USERHELPER);
 
 	if (!try_module_get(module)) {
+		kfree_const(fw_work->name);
 		kfree(fw_work);
 		return -EFAULT;
 	}
@@ -1384,11 +1419,16 @@ static struct fw_cache_entry *alloc_fw_cache_entry(const char *name)
 {
 	struct fw_cache_entry *fce;
 
-	fce = kzalloc(sizeof(*fce) + strlen(name) + 1, GFP_ATOMIC);
+	fce = kzalloc(sizeof(*fce), GFP_ATOMIC);
 	if (!fce)
 		goto exit;
 
-	strcpy(fce->name, name);
+	fce->name = kstrdup_const(name, GFP_ATOMIC);
+	if (!fce->name) {
+		kfree(fce);
+		fce = NULL;
+		goto exit;
+	}
 exit:
 	return fce;
 }
@@ -1428,6 +1468,7 @@ found:
 
 static void free_fw_cache_entry(struct fw_cache_entry *fce)
 {
+	kfree_const(fce->name);
 	kfree(fce);
 }
 
diff --git a/drivers/base/init.c b/drivers/base/init.c
index da033d3bab3c..48c0e220acc0 100644
--- a/drivers/base/init.c
+++ b/drivers/base/init.c
@@ -8,6 +8,7 @@
 #include <linux/device.h>
 #include <linux/init.h>
 #include <linux/memory.h>
+#include <linux/of.h>
 
 #include "base.h"
 
@@ -34,4 +35,5 @@ void __init driver_init(void)
 	cpu_dev_init();
 	memory_dev_init();
 	container_dev_init();
+	of_core_init();
 }
diff --git a/drivers/base/node.c b/drivers/base/node.c
index a2aa65b4215d..31df474d72f4 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -359,12 +359,16 @@ int unregister_cpu_under_node(unsigned int cpu, unsigned int nid)
 #ifdef CONFIG_MEMORY_HOTPLUG_SPARSE
 #define page_initialized(page)  (page->lru.next)
 
-static int get_nid_for_pfn(unsigned long pfn)
+static int __init_refok get_nid_for_pfn(unsigned long pfn)
 {
 	struct page *page;
 
 	if (!pfn_valid_within(pfn))
 		return -1;
+#ifdef CONFIG_DEFERRED_STRUCT_PAGE_INIT
+	if (system_state == SYSTEM_BOOTING)
+		return early_pfn_to_nid(pfn);
+#endif
 	page = pfn_to_page(pfn);
 	if (!page_initialized(page))
 		return -1;
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index ebf034b97278..063f0ab15259 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -613,6 +613,19 @@ int __init_or_module __platform_driver_probe(struct platform_driver *drv,
 {
 	int retval, code;
 
+	if (drv->driver.probe_type == PROBE_PREFER_ASYNCHRONOUS) {
+		pr_err("%s: drivers registered with %s can not be probed asynchronously\n",
+			 drv->driver.name, __func__);
+		return -EINVAL;
+	}
+
+	/*
+	 * We have to run our probes synchronously because we check if
+	 * we find any devices to bind to and exit with error if there
+	 * are any.
+	 */
+	drv->driver.probe_type = PROBE_FORCE_SYNCHRONOUS;
+
 	/*
 	 * Prevent driver from requesting probe deferral to avoid further
 	 * futile probe attempts.
diff --git a/drivers/base/power/Makefile b/drivers/base/power/Makefile
index 1cb8544598d5..f94a6ccfe787 100644
--- a/drivers/base/power/Makefile
+++ b/drivers/base/power/Makefile
@@ -1,4 +1,4 @@
-obj-$(CONFIG_PM)	+= sysfs.o generic_ops.o common.o qos.o runtime.o
+obj-$(CONFIG_PM)	+= sysfs.o generic_ops.o common.o qos.o runtime.o wakeirq.o
 obj-$(CONFIG_PM_SLEEP)	+= main.o wakeup.o
 obj-$(CONFIG_PM_TRACE_RTC)	+= trace.o
 obj-$(CONFIG_PM_OPP)	+= opp.o
diff --git a/drivers/base/power/clock_ops.c b/drivers/base/power/clock_ops.c
index 7fdd0172605a..acef9f9f759a 100644
--- a/drivers/base/power/clock_ops.c
+++ b/drivers/base/power/clock_ops.c
@@ -15,6 +15,7 @@
 #include <linux/clkdev.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/pm_runtime.h>
 
 #ifdef CONFIG_PM
 
@@ -67,7 +68,8 @@ static void pm_clk_acquire(struct device *dev, struct pm_clock_entry *ce)
 	} else {
 		clk_prepare(ce->clk);
 		ce->status = PCE_STATUS_ACQUIRED;
-		dev_dbg(dev, "Clock %s managed by runtime PM.\n", ce->con_id);
+		dev_dbg(dev, "Clock %pC con_id %s managed by runtime PM.\n",
+			ce->clk, ce->con_id);
 	}
 }
 
@@ -93,7 +95,7 @@ static int __pm_clk_add(struct device *dev, const char *con_id,
 			return -ENOMEM;
 		}
 	} else {
-		if (IS_ERR(ce->clk) || !__clk_get(clk)) {
+		if (IS_ERR(clk) || !__clk_get(clk)) {
 			kfree(ce);
 			return -ENOENT;
 		}
@@ -367,6 +369,43 @@ static int pm_clk_notify(struct notifier_block *nb,
 	return 0;
 }
 
+int pm_clk_runtime_suspend(struct device *dev)
+{
+	int ret;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	ret = pm_generic_runtime_suspend(dev);
+	if (ret) {
+		dev_err(dev, "failed to suspend device\n");
+		return ret;
+	}
+
+	ret = pm_clk_suspend(dev);
+	if (ret) {
+		dev_err(dev, "failed to suspend clock\n");
+		pm_generic_runtime_resume(dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+int pm_clk_runtime_resume(struct device *dev)
+{
+	int ret;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	ret = pm_clk_resume(dev);
+	if (ret) {
+		dev_err(dev, "failed to resume clock\n");
+		return ret;
+	}
+
+	return pm_generic_runtime_resume(dev);
+}
+
 #else /* !CONFIG_PM */
 
 /**
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index 2327613d4539..0ee43c1056e0 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -6,6 +6,7 @@
  * This file is released under the GPLv2.
  */
 
+#include <linux/delay.h>
 #include <linux/kernel.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
@@ -19,6 +20,8 @@
 #include <linux/suspend.h>
 #include <linux/export.h>
 
+#define GENPD_RETRY_MAX_MS	250		/* Approximate */
+
 #define GENPD_DEV_CALLBACK(genpd, type, callback, dev)		\
 ({								\
 	type (*__routine)(struct device *__d); 			\
@@ -181,7 +184,7 @@ static void genpd_recalc_cpu_exit_latency(struct generic_pm_domain *genpd)
 	genpd->cpuidle_data->idle_state->exit_latency = usecs64;
 }
 
-static int genpd_power_on(struct generic_pm_domain *genpd)
+static int genpd_power_on(struct generic_pm_domain *genpd, bool timed)
 {
 	ktime_t time_start;
 	s64 elapsed_ns;
@@ -190,6 +193,9 @@ static int genpd_power_on(struct generic_pm_domain *genpd)
 	if (!genpd->power_on)
 		return 0;
 
+	if (!timed)
+		return genpd->power_on(genpd);
+
 	time_start = ktime_get();
 	ret = genpd->power_on(genpd);
 	if (ret)
@@ -208,7 +214,7 @@ static int genpd_power_on(struct generic_pm_domain *genpd)
 	return ret;
 }
 
-static int genpd_power_off(struct generic_pm_domain *genpd)
+static int genpd_power_off(struct generic_pm_domain *genpd, bool timed)
 {
 	ktime_t time_start;
 	s64 elapsed_ns;
@@ -217,6 +223,9 @@ static int genpd_power_off(struct generic_pm_domain *genpd)
 	if (!genpd->power_off)
 		return 0;
 
+	if (!timed)
+		return genpd->power_off(genpd);
+
 	time_start = ktime_get();
 	ret = genpd->power_off(genpd);
 	if (ret == -EBUSY)
@@ -305,7 +314,7 @@ static int __pm_genpd_poweron(struct generic_pm_domain *genpd)
 		}
 	}
 
-	ret = genpd_power_on(genpd);
+	ret = genpd_power_on(genpd, true);
 	if (ret)
 		goto err;
 
@@ -615,7 +624,7 @@ static int pm_genpd_poweroff(struct generic_pm_domain *genpd)
 		 * the pm_genpd_poweron() restore power for us (this shouldn't
 		 * happen very often).
 		 */
-		ret = genpd_power_off(genpd);
+		ret = genpd_power_off(genpd, true);
 		if (ret == -EBUSY) {
 			genpd_set_active(genpd);
 			goto out;
@@ -827,6 +836,7 @@ static bool genpd_dev_active_wakeup(struct generic_pm_domain *genpd,
 /**
  * pm_genpd_sync_poweroff - Synchronously power off a PM domain and its masters.
  * @genpd: PM domain to power off, if possible.
+ * @timed: True if latency measurements are allowed.
  *
  * Check if the given PM domain can be powered off (during system suspend or
  * hibernation) and do that if so.  Also, in that case propagate to its masters.
@@ -836,7 +846,8 @@ static bool genpd_dev_active_wakeup(struct generic_pm_domain *genpd,
  * executed sequentially, so it is guaranteed that it will never run twice in
  * parallel).
  */
-static void pm_genpd_sync_poweroff(struct generic_pm_domain *genpd)
+static void pm_genpd_sync_poweroff(struct generic_pm_domain *genpd,
+				   bool timed)
 {
 	struct gpd_link *link;
 
@@ -847,26 +858,28 @@ static void pm_genpd_sync_poweroff(struct generic_pm_domain *genpd)
 	    || atomic_read(&genpd->sd_count) > 0)
 		return;
 
-	genpd_power_off(genpd);
+	genpd_power_off(genpd, timed);
 
 	genpd->status = GPD_STATE_POWER_OFF;
 
 	list_for_each_entry(link, &genpd->slave_links, slave_node) {
 		genpd_sd_counter_dec(link->master);
-		pm_genpd_sync_poweroff(link->master);
+		pm_genpd_sync_poweroff(link->master, timed);
 	}
 }
 
 /**
  * pm_genpd_sync_poweron - Synchronously power on a PM domain and its masters.
  * @genpd: PM domain to power on.
+ * @timed: True if latency measurements are allowed.
  *
  * This function is only called in "noirq" and "syscore" stages of system power
  * transitions, so it need not acquire locks (all of the "noirq" callbacks are
  * executed sequentially, so it is guaranteed that it will never run twice in
  * parallel).
  */
-static void pm_genpd_sync_poweron(struct generic_pm_domain *genpd)
+static void pm_genpd_sync_poweron(struct generic_pm_domain *genpd,
+				  bool timed)
 {
 	struct gpd_link *link;
 
@@ -874,11 +887,11 @@ static void pm_genpd_sync_poweron(struct generic_pm_domain *genpd)
 		return;
 
 	list_for_each_entry(link, &genpd->slave_links, slave_node) {
-		pm_genpd_sync_poweron(link->master);
+		pm_genpd_sync_poweron(link->master, timed);
 		genpd_sd_counter_inc(link->master);
 	}
 
-	genpd_power_on(genpd);
+	genpd_power_on(genpd, timed);
 
 	genpd->status = GPD_STATE_ACTIVE;
 }
@@ -1056,7 +1069,7 @@ static int pm_genpd_suspend_noirq(struct device *dev)
 	 * the same PM domain, so it is not necessary to use locking here.
 	 */
 	genpd->suspended_count++;
-	pm_genpd_sync_poweroff(genpd);
+	pm_genpd_sync_poweroff(genpd, true);
 
 	return 0;
 }
@@ -1086,7 +1099,7 @@ static int pm_genpd_resume_noirq(struct device *dev)
 	 * guaranteed that this function will never run twice in parallel for
 	 * the same PM domain, so it is not necessary to use locking here.
 	 */
-	pm_genpd_sync_poweron(genpd);
+	pm_genpd_sync_poweron(genpd, true);
 	genpd->suspended_count--;
 
 	return genpd_start_dev(genpd, dev);
@@ -1300,7 +1313,7 @@ static int pm_genpd_restore_noirq(struct device *dev)
 			 * If the domain was off before the hibernation, make
 			 * sure it will be off going forward.
 			 */
-			genpd_power_off(genpd);
+			genpd_power_off(genpd, true);
 
 			return 0;
 		}
@@ -1309,7 +1322,7 @@ static int pm_genpd_restore_noirq(struct device *dev)
 	if (genpd->suspend_power_off)
 		return 0;
 
-	pm_genpd_sync_poweron(genpd);
+	pm_genpd_sync_poweron(genpd, true);
 
 	return genpd_start_dev(genpd, dev);
 }
@@ -1367,9 +1380,9 @@ static void genpd_syscore_switch(struct device *dev, bool suspend)
 
 	if (suspend) {
 		genpd->suspended_count++;
-		pm_genpd_sync_poweroff(genpd);
+		pm_genpd_sync_poweroff(genpd, false);
 	} else {
-		pm_genpd_sync_poweron(genpd);
+		pm_genpd_sync_poweron(genpd, false);
 		genpd->suspended_count--;
 	}
 }
@@ -2121,6 +2134,7 @@ EXPORT_SYMBOL_GPL(of_genpd_get_from_provider);
 static void genpd_dev_pm_detach(struct device *dev, bool power_off)
 {
 	struct generic_pm_domain *pd;
+	unsigned int i;
 	int ret = 0;
 
 	pd = pm_genpd_lookup_dev(dev);
@@ -2129,10 +2143,12 @@ static void genpd_dev_pm_detach(struct device *dev, bool power_off)
 
 	dev_dbg(dev, "removing from PM domain %s\n", pd->name);
 
-	while (1) {
+	for (i = 1; i < GENPD_RETRY_MAX_MS; i <<= 1) {
 		ret = pm_genpd_remove_device(pd, dev);
 		if (ret != -EAGAIN)
 			break;
+
+		mdelay(i);
 		cond_resched();
 	}
 
@@ -2173,6 +2189,7 @@ int genpd_dev_pm_attach(struct device *dev)
 {
 	struct of_phandle_args pd_args;
 	struct generic_pm_domain *pd;
+	unsigned int i;
 	int ret;
 
 	if (!dev->of_node)
@@ -2208,10 +2225,12 @@ int genpd_dev_pm_attach(struct device *dev)
 
 	dev_dbg(dev, "adding to PM domain %s\n", pd->name);
 
-	while (1) {
+	for (i = 1; i < GENPD_RETRY_MAX_MS; i <<= 1) {
 		ret = pm_genpd_add_device(pd, dev);
 		if (ret != -EAGAIN)
 			break;
+
+		mdelay(i);
 		cond_resched();
 	}
 
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 3d874eca7104..30b7bbfdc558 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -24,6 +24,7 @@
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
 #include <linux/pm-trace.h>
+#include <linux/pm_wakeirq.h>
 #include <linux/interrupt.h>
 #include <linux/sched.h>
 #include <linux/async.h>
@@ -587,6 +588,7 @@ void dpm_resume_noirq(pm_message_t state)
 	async_synchronize_full();
 	dpm_show_time(starttime, state, "noirq");
 	resume_device_irqs();
+	device_wakeup_disarm_wake_irqs();
 	cpuidle_resume();
 	trace_suspend_resume(TPS("dpm_resume_noirq"), state.event, false);
 }
@@ -920,9 +922,7 @@ static void device_complete(struct device *dev, pm_message_t state)
 
 	if (callback) {
 		pm_dev_dbg(dev, state, info);
-		trace_device_pm_callback_start(dev, info, state.event);
 		callback(dev);
-		trace_device_pm_callback_end(dev, 0);
 	}
 
 	device_unlock(dev);
@@ -954,7 +954,9 @@ void dpm_complete(pm_message_t state)
 		list_move(&dev->power.entry, &list);
 		mutex_unlock(&dpm_list_mtx);
 
+		trace_device_pm_callback_start(dev, "", state.event);
 		device_complete(dev, state);
+		trace_device_pm_callback_end(dev, 0);
 
 		mutex_lock(&dpm_list_mtx);
 		put_device(dev);
@@ -1104,6 +1106,7 @@ int dpm_suspend_noirq(pm_message_t state)
 
 	trace_suspend_resume(TPS("dpm_suspend_noirq"), state.event, true);
 	cpuidle_pause();
+	device_wakeup_arm_wake_irqs();
 	suspend_device_irqs();
 	mutex_lock(&dpm_list_mtx);
 	pm_transition = state;
@@ -1585,11 +1588,8 @@ static int device_prepare(struct device *dev, pm_message_t state)
 		callback = dev->driver->pm->prepare;
 	}
 
-	if (callback) {
-		trace_device_pm_callback_start(dev, info, state.event);
+	if (callback)
 		ret = callback(dev);
-		trace_device_pm_callback_end(dev, ret);
-	}
 
 	device_unlock(dev);
 
@@ -1631,7 +1631,9 @@ int dpm_prepare(pm_message_t state)
 		get_device(dev);
 		mutex_unlock(&dpm_list_mtx);
 
+		trace_device_pm_callback_start(dev, "", state.event);
 		error = device_prepare(dev, state);
+		trace_device_pm_callback_end(dev, error);
 
 		mutex_lock(&dpm_list_mtx);
 		if (error) {
diff --git a/drivers/base/power/power.h b/drivers/base/power/power.h
index b6b8a273c5da..f1a5d95e7b20 100644
--- a/drivers/base/power/power.h
+++ b/drivers/base/power/power.h
@@ -20,6 +20,46 @@ static inline void pm_runtime_early_init(struct device *dev)
 extern void pm_runtime_init(struct device *dev);
 extern void pm_runtime_remove(struct device *dev);
 
+struct wake_irq {
+	struct device *dev;
+	int irq;
+	bool dedicated_irq:1;
+};
+
+extern void dev_pm_arm_wake_irq(struct wake_irq *wirq);
+extern void dev_pm_disarm_wake_irq(struct wake_irq *wirq);
+
+#ifdef CONFIG_PM_SLEEP
+
+extern int device_wakeup_attach_irq(struct device *dev,
+				    struct wake_irq *wakeirq);
+extern void device_wakeup_detach_irq(struct device *dev);
+extern void device_wakeup_arm_wake_irqs(void);
+extern void device_wakeup_disarm_wake_irqs(void);
+
+#else
+
+static inline int
+device_wakeup_attach_irq(struct device *dev,
+			 struct wake_irq *wakeirq)
+{
+	return 0;
+}
+
+static inline void device_wakeup_detach_irq(struct device *dev)
+{
+}
+
+static inline void device_wakeup_arm_wake_irqs(void)
+{
+}
+
+static inline void device_wakeup_disarm_wake_irqs(void)
+{
+}
+
+#endif /* CONFIG_PM_SLEEP */
+
 /*
  * sysfs.c
  */
@@ -52,6 +92,14 @@ static inline void wakeup_sysfs_remove(struct device *dev) {}
 static inline int pm_qos_sysfs_add(struct device *dev) { return 0; }
 static inline void pm_qos_sysfs_remove(struct device *dev) {}
 
+static inline void dev_pm_arm_wake_irq(struct wake_irq *wirq)
+{
+}
+
+static inline void dev_pm_disarm_wake_irq(struct wake_irq *wirq)
+{
+}
+
 #endif
 
 #ifdef CONFIG_PM_SLEEP
diff --git a/drivers/base/power/runtime.c b/drivers/base/power/runtime.c
index 5070c4fe8542..e1a10a03df8e 100644
--- a/drivers/base/power/runtime.c
+++ b/drivers/base/power/runtime.c
@@ -10,6 +10,7 @@
 #include <linux/sched.h>
 #include <linux/export.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_wakeirq.h>
 #include <trace/events/rpm.h>
 #include "power.h"
 
@@ -514,6 +515,7 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 
 	callback = RPM_GET_CALLBACK(dev, runtime_suspend);
 
+	dev_pm_enable_wake_irq(dev);
 	retval = rpm_callback(callback, dev);
 	if (retval)
 		goto fail;
@@ -552,6 +554,7 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 	return retval;
 
  fail:
+	dev_pm_disable_wake_irq(dev);
 	__update_runtime_status(dev, RPM_ACTIVE);
 	dev->power.deferred_resume = false;
 	wake_up_all(&dev->power.wait_queue);
@@ -734,13 +737,16 @@ static int rpm_resume(struct device *dev, int rpmflags)
 
 	callback = RPM_GET_CALLBACK(dev, runtime_resume);
 
+	dev_pm_disable_wake_irq(dev);
 	retval = rpm_callback(callback, dev);
 	if (retval) {
 		__update_runtime_status(dev, RPM_SUSPENDED);
 		pm_runtime_cancel_pending(dev);
+		dev_pm_enable_wake_irq(dev);
 	} else {
  no_callback:
 		__update_runtime_status(dev, RPM_ACTIVE);
+		pm_runtime_mark_last_busy(dev);
 		if (parent)
 			atomic_inc(&parent->power.child_count);
 	}
diff --git a/drivers/base/power/wakeirq.c b/drivers/base/power/wakeirq.c
new file mode 100644
index 000000000000..eb6e67451dec
--- /dev/null
+++ b/drivers/base/power/wakeirq.c
@@ -0,0 +1,271 @@
+/*
+ * wakeirq.c - Device wakeirq helper functions
+ *
+ * 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.
+ */
+
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/pm_runtime.h>
+#include <linux/pm_wakeirq.h>
+
+#include "power.h"
+
+/**
+ * dev_pm_attach_wake_irq - Attach device interrupt as a wake IRQ
+ * @dev: Device entry
+ * @irq: Device wake-up capable interrupt
+ * @wirq: Wake irq specific data
+ *
+ * Internal function to attach either a device IO interrupt or a
+ * dedicated wake-up interrupt as a wake IRQ.
+ */
+static int dev_pm_attach_wake_irq(struct device *dev, int irq,
+				  struct wake_irq *wirq)
+{
+	unsigned long flags;
+	int err;
+
+	if (!dev || !wirq)
+		return -EINVAL;
+
+	spin_lock_irqsave(&dev->power.lock, flags);
+	if (dev_WARN_ONCE(dev, dev->power.wakeirq,
+			  "wake irq already initialized\n")) {
+		spin_unlock_irqrestore(&dev->power.lock, flags);
+		return -EEXIST;
+	}
+
+	err = device_wakeup_attach_irq(dev, wirq);
+	if (!err)
+		dev->power.wakeirq = wirq;
+
+	spin_unlock_irqrestore(&dev->power.lock, flags);
+	return err;
+}
+
+/**
+ * dev_pm_set_wake_irq - Attach device IO interrupt as wake IRQ
+ * @dev: Device entry
+ * @irq: Device IO interrupt
+ *
+ * Attach a device IO interrupt as a wake IRQ. The wake IRQ gets
+ * automatically configured for wake-up from suspend  based
+ * on the device specific sysfs wakeup entry. Typically called
+ * during driver probe after calling device_init_wakeup().
+ */
+int dev_pm_set_wake_irq(struct device *dev, int irq)
+{
+	struct wake_irq *wirq;
+	int err;
+
+	wirq = kzalloc(sizeof(*wirq), GFP_KERNEL);
+	if (!wirq)
+		return -ENOMEM;
+
+	wirq->dev = dev;
+	wirq->irq = irq;
+
+	err = dev_pm_attach_wake_irq(dev, irq, wirq);
+	if (err)
+		kfree(wirq);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(dev_pm_set_wake_irq);
+
+/**
+ * dev_pm_clear_wake_irq - Detach a device IO interrupt wake IRQ
+ * @dev: Device entry
+ *
+ * Detach a device wake IRQ and free resources.
+ *
+ * Note that it's OK for drivers to call this without calling
+ * dev_pm_set_wake_irq() as all the driver instances may not have
+ * a wake IRQ configured. This avoid adding wake IRQ specific
+ * checks into the drivers.
+ */
+void dev_pm_clear_wake_irq(struct device *dev)
+{
+	struct wake_irq *wirq = dev->power.wakeirq;
+	unsigned long flags;
+
+	if (!wirq)
+		return;
+
+	spin_lock_irqsave(&dev->power.lock, flags);
+	device_wakeup_detach_irq(dev);
+	dev->power.wakeirq = NULL;
+	spin_unlock_irqrestore(&dev->power.lock, flags);
+
+	if (wirq->dedicated_irq)
+		free_irq(wirq->irq, wirq);
+	kfree(wirq);
+}
+EXPORT_SYMBOL_GPL(dev_pm_clear_wake_irq);
+
+/**
+ * handle_threaded_wake_irq - Handler for dedicated wake-up interrupts
+ * @irq: Device specific dedicated wake-up interrupt
+ * @_wirq: Wake IRQ data
+ *
+ * Some devices have a separate wake-up interrupt in addition to the
+ * device IO interrupt. The wake-up interrupt signals that a device
+ * should be woken up from it's idle state. This handler uses device
+ * specific pm_runtime functions to wake the device, and then it's
+ * up to the device to do whatever it needs to. Note that as the
+ * device may need to restore context and start up regulators, we
+ * use a threaded IRQ.
+ *
+ * Also note that we are not resending the lost device interrupts.
+ * We assume that the wake-up interrupt just needs to wake-up the
+ * device, and then device's pm_runtime_resume() can deal with the
+ * situation.
+ */
+static irqreturn_t handle_threaded_wake_irq(int irq, void *_wirq)
+{
+	struct wake_irq *wirq = _wirq;
+	int res;
+
+	/* We don't want RPM_ASYNC or RPM_NOWAIT here */
+	res = pm_runtime_resume(wirq->dev);
+	if (res < 0)
+		dev_warn(wirq->dev,
+			 "wake IRQ with no resume: %i\n", res);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * dev_pm_set_dedicated_wake_irq - Request a dedicated wake-up interrupt
+ * @dev: Device entry
+ * @irq: Device wake-up interrupt
+ *
+ * Unless your hardware has separate wake-up interrupts in addition
+ * to the device IO interrupts, you don't need this.
+ *
+ * Sets up a threaded interrupt handler for a device that has
+ * a dedicated wake-up interrupt in addition to the device IO
+ * interrupt.
+ *
+ * The interrupt starts disabled, and needs to be managed for
+ * the device by the bus code or the device driver using
+ * dev_pm_enable_wake_irq() and dev_pm_disable_wake_irq()
+ * functions.
+ */
+int dev_pm_set_dedicated_wake_irq(struct device *dev, int irq)
+{
+	struct wake_irq *wirq;
+	int err;
+
+	wirq = kzalloc(sizeof(*wirq), GFP_KERNEL);
+	if (!wirq)
+		return -ENOMEM;
+
+	wirq->dev = dev;
+	wirq->irq = irq;
+	wirq->dedicated_irq = true;
+	irq_set_status_flags(irq, IRQ_NOAUTOEN);
+
+	/*
+	 * Consumer device may need to power up and restore state
+	 * so we use a threaded irq.
+	 */
+	err = request_threaded_irq(irq, NULL, handle_threaded_wake_irq,
+				   IRQF_ONESHOT, dev_name(dev), wirq);
+	if (err)
+		goto err_free;
+
+	err = dev_pm_attach_wake_irq(dev, irq, wirq);
+	if (err)
+		goto err_free_irq;
+
+	return err;
+
+err_free_irq:
+	free_irq(irq, wirq);
+err_free:
+	kfree(wirq);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(dev_pm_set_dedicated_wake_irq);
+
+/**
+ * dev_pm_enable_wake_irq - Enable device wake-up interrupt
+ * @dev: Device
+ *
+ * Called from the bus code or the device driver for
+ * runtime_suspend() to enable the wake-up interrupt while
+ * the device is running.
+ *
+ * Note that for runtime_suspend()) the wake-up interrupts
+ * should be unconditionally enabled unlike for suspend()
+ * that is conditional.
+ */
+void dev_pm_enable_wake_irq(struct device *dev)
+{
+	struct wake_irq *wirq = dev->power.wakeirq;
+
+	if (wirq && wirq->dedicated_irq)
+		enable_irq(wirq->irq);
+}
+EXPORT_SYMBOL_GPL(dev_pm_enable_wake_irq);
+
+/**
+ * dev_pm_disable_wake_irq - Disable device wake-up interrupt
+ * @dev: Device
+ *
+ * Called from the bus code or the device driver for
+ * runtime_resume() to disable the wake-up interrupt while
+ * the device is running.
+ */
+void dev_pm_disable_wake_irq(struct device *dev)
+{
+	struct wake_irq *wirq = dev->power.wakeirq;
+
+	if (wirq && wirq->dedicated_irq)
+		disable_irq_nosync(wirq->irq);
+}
+EXPORT_SYMBOL_GPL(dev_pm_disable_wake_irq);
+
+/**
+ * dev_pm_arm_wake_irq - Arm device wake-up
+ * @wirq: Device wake-up interrupt
+ *
+ * Sets up the wake-up event conditionally based on the
+ * device_may_wake().
+ */
+void dev_pm_arm_wake_irq(struct wake_irq *wirq)
+{
+	if (!wirq)
+		return;
+
+	if (device_may_wakeup(wirq->dev))
+		enable_irq_wake(wirq->irq);
+}
+
+/**
+ * dev_pm_disarm_wake_irq - Disarm device wake-up
+ * @wirq: Device wake-up interrupt
+ *
+ * Clears up the wake-up event conditionally based on the
+ * device_may_wake().
+ */
+void dev_pm_disarm_wake_irq(struct wake_irq *wirq)
+{
+	if (!wirq)
+		return;
+
+	if (device_may_wakeup(wirq->dev))
+		disable_irq_wake(wirq->irq);
+}
diff --git a/drivers/base/power/wakeup.c b/drivers/base/power/wakeup.c
index 77262009f89d..51f15bc15774 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -14,6 +14,7 @@
 #include <linux/suspend.h>
 #include <linux/seq_file.h>
 #include <linux/debugfs.h>
+#include <linux/pm_wakeirq.h>
 #include <trace/events/power.h>
 
 #include "power.h"
@@ -56,6 +57,11 @@ static LIST_HEAD(wakeup_sources);
 
 static DECLARE_WAIT_QUEUE_HEAD(wakeup_count_wait_queue);
 
+static struct wakeup_source deleted_ws = {
+	.name = "deleted",
+	.lock =  __SPIN_LOCK_UNLOCKED(deleted_ws.lock),
+};
+
 /**
  * wakeup_source_prepare - Prepare a new wakeup source for initialization.
  * @ws: Wakeup source to prepare.
@@ -107,6 +113,34 @@ void wakeup_source_drop(struct wakeup_source *ws)
 }
 EXPORT_SYMBOL_GPL(wakeup_source_drop);
 
+/*
+ * Record wakeup_source statistics being deleted into a dummy wakeup_source.
+ */
+static void wakeup_source_record(struct wakeup_source *ws)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&deleted_ws.lock, flags);
+
+	if (ws->event_count) {
+		deleted_ws.total_time =
+			ktime_add(deleted_ws.total_time, ws->total_time);
+		deleted_ws.prevent_sleep_time =
+			ktime_add(deleted_ws.prevent_sleep_time,
+				  ws->prevent_sleep_time);
+		deleted_ws.max_time =
+			ktime_compare(deleted_ws.max_time, ws->max_time) > 0 ?
+				deleted_ws.max_time : ws->max_time;
+		deleted_ws.event_count += ws->event_count;
+		deleted_ws.active_count += ws->active_count;
+		deleted_ws.relax_count += ws->relax_count;
+		deleted_ws.expire_count += ws->expire_count;
+		deleted_ws.wakeup_count += ws->wakeup_count;
+	}
+
+	spin_unlock_irqrestore(&deleted_ws.lock, flags);
+}
+
 /**
  * wakeup_source_destroy - Destroy a struct wakeup_source object.
  * @ws: Wakeup source to destroy.
@@ -119,6 +153,7 @@ void wakeup_source_destroy(struct wakeup_source *ws)
 		return;
 
 	wakeup_source_drop(ws);
+	wakeup_source_record(ws);
 	kfree(ws->name);
 	kfree(ws);
 }
@@ -239,6 +274,86 @@ int device_wakeup_enable(struct device *dev)
 EXPORT_SYMBOL_GPL(device_wakeup_enable);
 
 /**
+ * device_wakeup_attach_irq - Attach a wakeirq to a wakeup source
+ * @dev: Device to handle
+ * @wakeirq: Device specific wakeirq entry
+ *
+ * Attach a device wakeirq to the wakeup source so the device
+ * wake IRQ can be configured automatically for suspend and
+ * resume.
+ *
+ * Call under the device's power.lock lock.
+ */
+int device_wakeup_attach_irq(struct device *dev,
+			     struct wake_irq *wakeirq)
+{
+	struct wakeup_source *ws;
+
+	ws = dev->power.wakeup;
+	if (!ws) {
+		dev_err(dev, "forgot to call call device_init_wakeup?\n");
+		return -EINVAL;
+	}
+
+	if (ws->wakeirq)
+		return -EEXIST;
+
+	ws->wakeirq = wakeirq;
+	return 0;
+}
+
+/**
+ * device_wakeup_detach_irq - Detach a wakeirq from a wakeup source
+ * @dev: Device to handle
+ *
+ * Removes a device wakeirq from the wakeup source.
+ *
+ * Call under the device's power.lock lock.
+ */
+void device_wakeup_detach_irq(struct device *dev)
+{
+	struct wakeup_source *ws;
+
+	ws = dev->power.wakeup;
+	if (ws)
+		ws->wakeirq = NULL;
+}
+
+/**
+ * device_wakeup_arm_wake_irqs(void)
+ *
+ * Itereates over the list of device wakeirqs to arm them.
+ */
+void device_wakeup_arm_wake_irqs(void)
+{
+	struct wakeup_source *ws;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(ws, &wakeup_sources, entry) {
+		if (ws->wakeirq)
+			dev_pm_arm_wake_irq(ws->wakeirq);
+	}
+	rcu_read_unlock();
+}
+
+/**
+ * device_wakeup_disarm_wake_irqs(void)
+ *
+ * Itereates over the list of device wakeirqs to disarm them.
+ */
+void device_wakeup_disarm_wake_irqs(void)
+{
+	struct wakeup_source *ws;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(ws, &wakeup_sources, entry) {
+		if (ws->wakeirq)
+			dev_pm_disarm_wake_irq(ws->wakeirq);
+	}
+	rcu_read_unlock();
+}
+
+/**
  * device_wakeup_detach - Detach a device's wakeup source object from it.
  * @dev: Device to detach the wakeup source object from.
  *
@@ -351,6 +466,20 @@ int device_set_wakeup_enable(struct device *dev, bool enable)
 }
 EXPORT_SYMBOL_GPL(device_set_wakeup_enable);
 
+/**
+ * wakeup_source_not_registered - validate the given wakeup source.
+ * @ws: Wakeup source to be validated.
+ */
+static bool wakeup_source_not_registered(struct wakeup_source *ws)
+{
+	/*
+	 * Use timer struct to check if the given source is initialized
+	 * by wakeup_source_add.
+	 */
+	return ws->timer.function != pm_wakeup_timer_fn ||
+		   ws->timer.data != (unsigned long)ws;
+}
+
 /*
  * The functions below use the observation that each wakeup event starts a
  * period in which the system should not be suspended.  The moment this period
@@ -391,6 +520,10 @@ static void wakeup_source_activate(struct wakeup_source *ws)
 {
 	unsigned int cec;
 
+	if (WARN_ONCE(wakeup_source_not_registered(ws),
+			"unregistered wakeup source\n"))
+		return;
+
 	/*
 	 * active wakeup source should bring the system
 	 * out of PM_SUSPEND_FREEZE state
@@ -894,6 +1027,8 @@ static int wakeup_sources_stats_show(struct seq_file *m, void *unused)
 		print_wakeup_source_stats(m, ws);
 	rcu_read_unlock();
 
+	print_wakeup_source_stats(m, &deleted_ws);
+
 	return 0;
 }
 
diff --git a/drivers/base/property.c b/drivers/base/property.c
index 1d0b116cae95..f3f6d167f3f1 100644
--- a/drivers/base/property.c
+++ b/drivers/base/property.c
@@ -14,6 +14,7 @@
 #include <linux/export.h>
 #include <linux/kernel.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/property.h>
 
 /**
@@ -128,9 +129,9 @@ EXPORT_SYMBOL_GPL(device_property_present);
 bool fwnode_property_present(struct fwnode_handle *fwnode, const char *propname)
 {
 	if (is_of_node(fwnode))
-		return of_property_read_bool(of_node(fwnode), propname);
+		return of_property_read_bool(to_of_node(fwnode), propname);
 	else if (is_acpi_node(fwnode))
-		return !acpi_dev_prop_get(acpi_node(fwnode), propname, NULL);
+		return !acpi_dev_prop_get(to_acpi_node(fwnode), propname, NULL);
 
 	return !!pset_prop_get(to_pset(fwnode), propname);
 }
@@ -285,10 +286,10 @@ EXPORT_SYMBOL_GPL(device_property_read_string);
 ({ \
 	int _ret_; \
 	if (is_of_node(_fwnode_)) \
-		_ret_ = OF_DEV_PROP_READ_ARRAY(of_node(_fwnode_), _propname_, \
+		_ret_ = OF_DEV_PROP_READ_ARRAY(to_of_node(_fwnode_), _propname_, \
 					       _type_, _val_, _nval_); \
 	else if (is_acpi_node(_fwnode_)) \
-		_ret_ = acpi_dev_prop_read(acpi_node(_fwnode_), _propname_, \
+		_ret_ = acpi_dev_prop_read(to_acpi_node(_fwnode_), _propname_, \
 					   _proptype_, _val_, _nval_); \
 	else \
 		_ret_ = pset_prop_read_array(to_pset(_fwnode_), _propname_, \
@@ -424,11 +425,11 @@ int fwnode_property_read_string_array(struct fwnode_handle *fwnode,
 {
 	if (is_of_node(fwnode))
 		return val ?
-			of_property_read_string_array(of_node(fwnode), propname,
-						      val, nval) :
-			of_property_count_strings(of_node(fwnode), propname);
+			of_property_read_string_array(to_of_node(fwnode),
+						      propname, val, nval) :
+			of_property_count_strings(to_of_node(fwnode), propname);
 	else if (is_acpi_node(fwnode))
-		return acpi_dev_prop_read(acpi_node(fwnode), propname,
+		return acpi_dev_prop_read(to_acpi_node(fwnode), propname,
 					  DEV_PROP_STRING, val, nval);
 
 	return pset_prop_read_array(to_pset(fwnode), propname,
@@ -455,9 +456,9 @@ int fwnode_property_read_string(struct fwnode_handle *fwnode,
 				const char *propname, const char **val)
 {
 	if (is_of_node(fwnode))
-		return of_property_read_string(of_node(fwnode), propname, val);
+		return of_property_read_string(to_of_node(fwnode), propname, val);
 	else if (is_acpi_node(fwnode))
-		return acpi_dev_prop_read(acpi_node(fwnode), propname,
+		return acpi_dev_prop_read(to_acpi_node(fwnode), propname,
 					  DEV_PROP_STRING, val, 1);
 
 	return -ENXIO;
@@ -475,13 +476,13 @@ struct fwnode_handle *device_get_next_child_node(struct device *dev,
 	if (IS_ENABLED(CONFIG_OF) && dev->of_node) {
 		struct device_node *node;
 
-		node = of_get_next_available_child(dev->of_node, of_node(child));
+		node = of_get_next_available_child(dev->of_node, to_of_node(child));
 		if (node)
 			return &node->fwnode;
 	} else if (IS_ENABLED(CONFIG_ACPI)) {
 		struct acpi_device *node;
 
-		node = acpi_get_next_child(dev, acpi_node(child));
+		node = acpi_get_next_child(dev, to_acpi_node(child));
 		if (node)
 			return acpi_fwnode_handle(node);
 	}
@@ -500,7 +501,7 @@ EXPORT_SYMBOL_GPL(device_get_next_child_node);
 void fwnode_handle_put(struct fwnode_handle *fwnode)
 {
 	if (is_of_node(fwnode))
-		of_node_put(of_node(fwnode));
+		of_node_put(to_of_node(fwnode));
 }
 EXPORT_SYMBOL_GPL(fwnode_handle_put);
 
@@ -519,3 +520,16 @@ unsigned int device_get_child_node_count(struct device *dev)
 	return count;
 }
 EXPORT_SYMBOL_GPL(device_get_child_node_count);
+
+bool device_dma_is_coherent(struct device *dev)
+{
+	bool coherent = false;
+
+	if (IS_ENABLED(CONFIG_OF) && dev->of_node)
+		coherent = of_dma_is_coherent(dev->of_node);
+	else
+		acpi_check_dma(ACPI_COMPANION(dev), &coherent);
+
+	return coherent;
+}
+EXPORT_SYMBOL_GPL(device_dma_is_coherent);
diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
index a13587b5c2be..b2b2849fc6d3 100644
--- a/drivers/base/regmap/internal.h
+++ b/drivers/base/regmap/internal.h
@@ -131,7 +131,10 @@ struct regmap {
 	struct reg_default *reg_defaults;
 	const void *reg_defaults_raw;
 	void *cache;
+	/* if set, the cache contains newer data than the HW */
 	u32 cache_dirty;
+	/* if set, the HW registers are known to match map->reg_defaults */
+	bool no_sync_defaults;
 
 	struct reg_default *patch;
 	int patch_regs;
diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
index 7eb7b3b98794..b9862d741a56 100644
--- a/drivers/base/regmap/regcache.c
+++ b/drivers/base/regmap/regcache.c
@@ -249,6 +249,22 @@ int regcache_write(struct regmap *map,
 	return 0;
 }
 
+static bool regcache_reg_needs_sync(struct regmap *map, unsigned int reg,
+				    unsigned int val)
+{
+	int ret;
+
+	/* If we don't know the chip just got reset, then sync everything. */
+	if (!map->no_sync_defaults)
+		return true;
+
+	/* Is this the hardware default?  If so skip. */
+	ret = regcache_lookup_reg(map, reg);
+	if (ret >= 0 && val == map->reg_defaults[ret].def)
+		return false;
+	return true;
+}
+
 static int regcache_default_sync(struct regmap *map, unsigned int min,
 				 unsigned int max)
 {
@@ -266,9 +282,7 @@ static int regcache_default_sync(struct regmap *map, unsigned int min,
 		if (ret)
 			return ret;
 
-		/* Is this the hardware default?  If so skip. */
-		ret = regcache_lookup_reg(map, reg);
-		if (ret >= 0 && val == map->reg_defaults[ret].def)
+		if (!regcache_reg_needs_sync(map, reg, val))
 			continue;
 
 		map->cache_bypass = 1;
@@ -342,6 +356,7 @@ out:
 	/* Restore the bypass state */
 	map->async = false;
 	map->cache_bypass = bypass;
+	map->no_sync_defaults = false;
 	map->unlock(map->lock_arg);
 
 	regmap_async_complete(map);
@@ -397,6 +412,7 @@ out:
 	/* Restore the bypass state */
 	map->cache_bypass = bypass;
 	map->async = false;
+	map->no_sync_defaults = false;
 	map->unlock(map->lock_arg);
 
 	regmap_async_complete(map);
@@ -461,18 +477,23 @@ void regcache_cache_only(struct regmap *map, bool enable)
 EXPORT_SYMBOL_GPL(regcache_cache_only);
 
 /**
- * regcache_mark_dirty: Mark the register cache as dirty
+ * regcache_mark_dirty: Indicate that HW registers were reset to default values
  *
  * @map: map to mark
  *
- * Mark the register cache as dirty, for example due to the device
- * having been powered down for suspend.  If the cache is not marked
- * as dirty then the cache sync will be suppressed.
+ * Inform regcache that the device has been powered down or reset, so that
+ * on resume, regcache_sync() knows to write out all non-default values
+ * stored in the cache.
+ *
+ * If this function is not called, regcache_sync() will assume that
+ * the hardware state still matches the cache state, modulo any writes that
+ * happened when cache_only was true.
  */
 void regcache_mark_dirty(struct regmap *map)
 {
 	map->lock(map->lock_arg);
 	map->cache_dirty = true;
+	map->no_sync_defaults = true;
 	map->unlock(map->lock_arg);
 }
 EXPORT_SYMBOL_GPL(regcache_mark_dirty);
@@ -613,10 +634,7 @@ static int regcache_sync_block_single(struct regmap *map, void *block,
 			continue;
 
 		val = regcache_get_val(map, block, i);
-
-		/* Is this the hardware default?  If so skip. */
-		ret = regcache_lookup_reg(map, regtmp);
-		if (ret >= 0 && val == map->reg_defaults[ret].def)
+		if (!regcache_reg_needs_sync(map, regtmp, val))
 			continue;
 
 		map->cache_bypass = 1;
@@ -688,10 +706,7 @@ static int regcache_sync_block_raw(struct regmap *map, void *block,
 		}
 
 		val = regcache_get_val(map, block, i);
-
-		/* Is this the hardware default?  If so skip. */
-		ret = regcache_lookup_reg(map, regtmp);
-		if (ret >= 0 && val == map->reg_defaults[ret].def) {
+		if (!regcache_reg_needs_sync(map, regtmp, val)) {
 			ret = regcache_sync_block_raw_flush(map, &data,
 							    base, regtmp);
 			if (ret != 0)
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index a6c3f75b4b01..2597600a5d26 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -109,7 +109,7 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 		if (!d->chip->init_ack_masked)
 			continue;
 		/*
-		 * Ack all the masked interrupts uncondictionly,
+		 * Ack all the masked interrupts unconditionally,
 		 * OR if there is masked interrupt which hasn't been Acked,
 		 * it'll be ignored in irq handler, then may introduce irq storm
 		 */
@@ -306,19 +306,12 @@ static int regmap_irq_map(struct irq_domain *h, unsigned int virq,
 	irq_set_chip_data(virq, data);
 	irq_set_chip(virq, &data->irq_chip);
 	irq_set_nested_thread(virq, 1);
-
-	/* ARM needs us to explicitly flag the IRQ as valid
-	 * and will set them noprobe when we do so. */
-#ifdef CONFIG_ARM
-	set_irq_flags(virq, IRQF_VALID);
-#else
 	irq_set_noprobe(virq);
-#endif
 
 	return 0;
 }
 
-static struct irq_domain_ops regmap_domain_ops = {
+static const struct irq_domain_ops regmap_domain_ops = {
 	.map	= regmap_irq_map,
 	.xlate	= irq_domain_xlate_twocell,
 };
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 6273ff072f3e..7111d04f2621 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -945,11 +945,10 @@ EXPORT_SYMBOL_GPL(devm_regmap_init);
 static void regmap_field_init(struct regmap_field *rm_field,
 	struct regmap *regmap, struct reg_field reg_field)
 {
-	int field_bits = reg_field.msb - reg_field.lsb + 1;
 	rm_field->regmap = regmap;
 	rm_field->reg = reg_field.reg;
 	rm_field->shift = reg_field.lsb;
-	rm_field->mask = ((BIT(field_bits) - 1) << reg_field.lsb);
+	rm_field->mask = GENMASK(reg_field.msb, reg_field.lsb);
 	rm_field->id_size = reg_field.id_size;
 	rm_field->id_offset = reg_field.id_offset;
 }
@@ -2318,7 +2317,7 @@ int regmap_bulk_read(struct regmap *map, unsigned int reg, void *val,
 					  &ival);
 			if (ret != 0)
 				return ret;
-			memcpy(val + (i * val_bytes), &ival, val_bytes);
+			map->format.format_val(val + (i * val_bytes), ival, 0);
 		}
 	}
 
@@ -2583,10 +2582,7 @@ int regmap_register_patch(struct regmap *map, const struct reg_default *regs,
 	map->async = true;
 
 	ret = _regmap_multi_reg_write(map, regs, num_regs);
-	if (ret != 0)
-		goto out;
 
-out:
 	map->async = false;
 	map->cache_bypass = bypass;
 
@@ -2613,6 +2609,30 @@ int regmap_get_val_bytes(struct regmap *map)
 }
 EXPORT_SYMBOL_GPL(regmap_get_val_bytes);
 
+/**
+ * regmap_get_max_register(): Report the max register value
+ *
+ * Report the max register value, mainly intended to for use by
+ * generic infrastructure built on top of regmap.
+ */
+int regmap_get_max_register(struct regmap *map)
+{
+	return map->max_register ? map->max_register : -EINVAL;
+}
+EXPORT_SYMBOL_GPL(regmap_get_max_register);
+
+/**
+ * regmap_get_reg_stride(): Report the register address stride
+ *
+ * Report the register address stride, mainly intended to for use by
+ * generic infrastructure built on top of regmap.
+ */
+int regmap_get_reg_stride(struct regmap *map)
+{
+	return map->reg_stride;
+}
+EXPORT_SYMBOL_GPL(regmap_get_reg_stride);
+
 int regmap_parse_val(struct regmap *map, const void *buf,
 			unsigned int *val)
 {
diff --git a/drivers/base/topology.c b/drivers/base/topology.c
index 6491f45200a7..8b7d7f8e5851 100644
--- a/drivers/base/topology.c
+++ b/drivers/base/topology.c
@@ -61,7 +61,7 @@ static DEVICE_ATTR_RO(physical_package_id);
 define_id_show_func(core_id);
 static DEVICE_ATTR_RO(core_id);
 
-define_siblings_show_func(thread_siblings, thread_cpumask);
+define_siblings_show_func(thread_siblings, sibling_cpumask);
 static DEVICE_ATTR_RO(thread_siblings);
 static DEVICE_ATTR_RO(thread_siblings_list);
 
diff --git a/drivers/bcma/Kconfig b/drivers/bcma/Kconfig
index fc6ffcfa8061..be5fffb6da24 100644
--- a/drivers/bcma/Kconfig
+++ b/drivers/bcma/Kconfig
@@ -29,12 +29,6 @@ config BCMA_HOST_PCI
 	select BCMA_DRIVER_PCI
 	default y
 
-config BCMA_DRIVER_PCI_HOSTMODE
-	bool "Driver for PCI core working in hostmode"
-	depends on BCMA && MIPS && BCMA_HOST_PCI
-	help
-	  PCI core hostmode operation (external PCI bus).
-
 config BCMA_HOST_SOC
 	bool "Support for BCMA in a SoC"
 	depends on BCMA
@@ -61,6 +55,12 @@ config BCMA_DRIVER_PCI
 	  This driver is also prerequisite for a hostmode PCIe core
 	  support.
 
+config BCMA_DRIVER_PCI_HOSTMODE
+	bool "Driver for PCI core working in hostmode"
+	depends on BCMA && MIPS && BCMA_DRIVER_PCI
+	help
+	  PCI core hostmode operation (external PCI bus).
+
 config BCMA_DRIVER_MIPS
 	bool "BCMA Broadcom MIPS core driver"
 	depends on BCMA && MIPS
diff --git a/drivers/bcma/driver_gpio.c b/drivers/bcma/driver_gpio.c
index 74ccb02e0f10..5f6018e7cd4c 100644
--- a/drivers/bcma/driver_gpio.c
+++ b/drivers/bcma/driver_gpio.c
@@ -226,6 +226,7 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 		chip->of_node	= cc->core->dev.of_node;
 #endif
 	switch (bus->chipinfo.id) {
+	case BCMA_CHIP_ID_BCM4707:
 	case BCMA_CHIP_ID_BCM5357:
 	case BCMA_CHIP_ID_BCM53572:
 		chip->ngpio	= 32;
@@ -235,16 +236,17 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 	}
 
 	/*
-	 * On MIPS we register GPIO devices (LEDs, buttons) using absolute GPIO
-	 * pin numbers. We don't have Device Tree there and we can't really use
-	 * relative (per chip) numbers.
-	 * So let's use predictable base for BCM47XX and "random" for all other.
+	 * Register SoC GPIO devices with absolute GPIO pin base.
+	 * On MIPS, we don't have Device Tree and we can't use relative (per chip)
+	 * GPIO numbers.
+	 * On some ARM devices, user space may want to access some system GPIO
+	 * pins directly, which is easier to do with a predictable GPIO base.
 	 */
-#if IS_BUILTIN(CONFIG_BCM47XX)
-	chip->base		= bus->num * BCMA_GPIO_MAX_PINS;
-#else
-	chip->base		= -1;
-#endif
+	if (IS_BUILTIN(CONFIG_BCM47XX) ||
+	    cc->core->bus->hosttype == BCMA_HOSTTYPE_SOC)
+		chip->base		= bus->num * BCMA_GPIO_MAX_PINS;
+	else
+		chip->base		= -1;
 
 	err = bcma_gpio_irq_domain_init(cc);
 	if (err)
diff --git a/drivers/block/Kconfig b/drivers/block/Kconfig
index eb1fed5bd516..1b8094d4d7af 100644
--- a/drivers/block/Kconfig
+++ b/drivers/block/Kconfig
@@ -404,17 +404,6 @@ config BLK_DEV_RAM_DAX
 	  and will prevent RAM block device backing store memory from being
 	  allocated from highmem (only a problem for highmem systems).
 
-config BLK_DEV_PMEM
-	tristate "Persistent memory block device support"
-	help
-	  Saying Y here will allow you to use a contiguous range of reserved
-	  memory as one or more persistent block devices.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called 'pmem'.
-
-	  If unsure, say N.
-
 config CDROM_PKTCDVD
 	tristate "Packet writing on CD/DVD media"
 	depends on !UML
diff --git a/drivers/block/Makefile b/drivers/block/Makefile
index 9cc6c18a1c7e..02b688d1438d 100644
--- a/drivers/block/Makefile
+++ b/drivers/block/Makefile
@@ -14,7 +14,6 @@ obj-$(CONFIG_PS3_VRAM)		+= ps3vram.o
 obj-$(CONFIG_ATARI_FLOPPY)	+= ataflop.o
 obj-$(CONFIG_AMIGA_Z2RAM)	+= z2ram.o
 obj-$(CONFIG_BLK_DEV_RAM)	+= brd.o
-obj-$(CONFIG_BLK_DEV_PMEM)	+= pmem.o
 obj-$(CONFIG_BLK_DEV_LOOP)	+= loop.o
 obj-$(CONFIG_BLK_CPQ_DA)	+= cpqarray.o
 obj-$(CONFIG_BLK_CPQ_CISS_DA)  += cciss.o
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index ff20f192b0f6..0422c47261c3 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -139,8 +139,6 @@ static struct board_type products[] = {
 	{0x3214103C, "Smart Array E200i", &SA5_access},
 	{0x3215103C, "Smart Array E200i", &SA5_access},
 	{0x3237103C, "Smart Array E500", &SA5_access},
-	{0x3223103C, "Smart Array P800", &SA5_access},
-	{0x3234103C, "Smart Array P400", &SA5_access},
 	{0x323D103C, "Smart Array P700m", &SA5_access},
 };
 
@@ -574,8 +572,6 @@ static void cciss_procinit(ctlr_info_t *h)
 
 /* List of controllers which cannot be hard reset on kexec with reset_devices */
 static u32 unresettable_controller[] = {
-	0x324a103C, /* Smart Array P712m */
-	0x324b103C, /* SmartArray P711m */
 	0x3223103C, /* Smart Array P800 */
 	0x3234103C, /* Smart Array P400 */
 	0x3235103C, /* Smart Array P400i */
@@ -586,12 +582,32 @@ static u32 unresettable_controller[] = {
 	0x3215103C, /* Smart Array E200i */
 	0x3237103C, /* Smart Array E500 */
 	0x323D103C, /* Smart Array P700m */
+	0x40800E11, /* Smart Array 5i */
 	0x409C0E11, /* Smart Array 6400 */
 	0x409D0E11, /* Smart Array 6400 EM */
+	0x40700E11, /* Smart Array 5300 */
+	0x40820E11, /* Smart Array 532 */
+	0x40830E11, /* Smart Array 5312 */
+	0x409A0E11, /* Smart Array 641 */
+	0x409B0E11, /* Smart Array 642 */
+	0x40910E11, /* Smart Array 6i */
 };
 
 /* List of controllers which cannot even be soft reset */
 static u32 soft_unresettable_controller[] = {
+	0x40800E11, /* Smart Array 5i */
+	0x40700E11, /* Smart Array 5300 */
+	0x40820E11, /* Smart Array 532 */
+	0x40830E11, /* Smart Array 5312 */
+	0x409A0E11, /* Smart Array 641 */
+	0x409B0E11, /* Smart Array 642 */
+	0x40910E11, /* Smart Array 6i */
+	/* Exclude 640x boards.  These are two pci devices in one slot
+	 * which share a battery backed cache module.  One controls the
+	 * cache, the other accesses the cache through the one that controls
+	 * it.  If we reset the one controlling the cache, the other will
+	 * likely not be happy.  Just forbid resetting this conjoined mess.
+	 */
 	0x409C0E11, /* Smart Array 6400 */
 	0x409D0E11, /* Smart Array 6400 EM */
 };
@@ -4667,8 +4683,7 @@ static int cciss_kdump_hard_reset_controller(struct pci_dev *pdev)
 	 */
 	cciss_lookup_board_id(pdev, &board_id);
 	if (!ctlr_is_resettable(board_id)) {
-		dev_warn(&pdev->dev, "Cannot reset Smart Array 640x "
-				"due to shared cache module.");
+		dev_warn(&pdev->dev, "Controller not resettable\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/block/cciss_scsi.c b/drivers/block/cciss_scsi.c
index ecd845cd28d8..1537302e56e3 100644
--- a/drivers/block/cciss_scsi.c
+++ b/drivers/block/cciss_scsi.c
@@ -84,7 +84,6 @@ static struct scsi_host_template cciss_driver_template = {
 	.show_info		= cciss_scsi_show_info,
 	.queuecommand		= cciss_scsi_queue_command,
 	.this_id		= 7,
-	.cmd_per_lun		= 1,
 	.use_clustering		= DISABLE_CLUSTERING,
 	/* Can't have eh_bus_reset_handler or eh_host_reset_handler for cciss */
 	.eh_device_reset_handler= cciss_eh_device_reset_handler,
diff --git a/drivers/block/drbd/drbd_debugfs.c b/drivers/block/drbd/drbd_debugfs.c
index a6ee3d750c30..6b88a35fb048 100644
--- a/drivers/block/drbd/drbd_debugfs.c
+++ b/drivers/block/drbd/drbd_debugfs.c
@@ -419,14 +419,6 @@ static int in_flight_summary_show(struct seq_file *m, void *pos)
 	return 0;
 }
 
-/* simple_positive(file->f_path.dentry) respectively debugfs_positive(),
- * but neither is "reachable" from here.
- * So we have our own inline version of it above.  :-( */
-static inline int debugfs_positive(struct dentry *dentry)
-{
-        return d_really_is_positive(dentry) && !d_unhashed(dentry);
-}
-
 /* make sure at *open* time that the respective object won't go away. */
 static int drbd_single_open(struct file *file, int (*show)(struct seq_file *, void *),
 		                void *data, struct kref *kref,
@@ -444,7 +436,7 @@ static int drbd_single_open(struct file *file, int (*show)(struct seq_file *, vo
 	/* serialize with d_delete() */
 	mutex_lock(&d_inode(parent)->i_mutex);
 	/* Make sure the object is still alive */
-	if (debugfs_positive(file->f_path.dentry)
+	if (simple_positive(file->f_path.dentry)
 	&& kref_get_unless_zero(kref))
 		ret = 0;
 	mutex_unlock(&d_inode(parent)->i_mutex);
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index b905e9888b88..efd19c2da9c2 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -38,6 +38,7 @@
 #include <linux/mutex.h>
 #include <linux/major.h>
 #include <linux/blkdev.h>
+#include <linux/backing-dev.h>
 #include <linux/genhd.h>
 #include <linux/idr.h>
 #include <net/tcp.h>
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 81fde9ef7f8e..a1518539b858 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -2359,7 +2359,7 @@ static void drbd_cleanup(void)
  * @congested_data:	User data
  * @bdi_bits:		Bits the BDI flusher thread is currently interested in
  *
- * Returns 1<<BDI_async_congested and/or 1<<BDI_sync_congested if we are congested.
+ * Returns 1<<WB_async_congested and/or 1<<WB_sync_congested if we are congested.
  */
 static int drbd_congested(void *congested_data, int bdi_bits)
 {
@@ -2376,14 +2376,14 @@ static int drbd_congested(void *congested_data, int bdi_bits)
 	}
 
 	if (test_bit(CALLBACK_PENDING, &first_peer_device(device)->connection->flags)) {
-		r |= (1 << BDI_async_congested);
+		r |= (1 << WB_async_congested);
 		/* Without good local data, we would need to read from remote,
 		 * and that would need the worker thread as well, which is
 		 * currently blocked waiting for that usermode helper to
 		 * finish.
 		 */
 		if (!get_ldev_if_state(device, D_UP_TO_DATE))
-			r |= (1 << BDI_sync_congested);
+			r |= (1 << WB_sync_congested);
 		else
 			put_ldev(device);
 		r &= bdi_bits;
@@ -2399,9 +2399,9 @@ static int drbd_congested(void *congested_data, int bdi_bits)
 			reason = 'b';
 	}
 
-	if (bdi_bits & (1 << BDI_async_congested) &&
+	if (bdi_bits & (1 << WB_async_congested) &&
 	    test_bit(NET_CONGESTED, &first_peer_device(device)->connection->flags)) {
-		r |= (1 << BDI_async_congested);
+		r |= (1 << WB_async_congested);
 		reason = reason == 'b' ? 'a' : 'n';
 	}
 
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index cee20354ac37..c097909c589c 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -598,7 +598,7 @@ static struct socket *drbd_try_connect(struct drbd_connection *connection)
 	memcpy(&peer_in6, &connection->peer_addr, peer_addr_len);
 
 	what = "sock_create_kern";
-	err = sock_create_kern(((struct sockaddr *)&src_in6)->sa_family,
+	err = sock_create_kern(&init_net, ((struct sockaddr *)&src_in6)->sa_family,
 			       SOCK_STREAM, IPPROTO_TCP, &sock);
 	if (err < 0) {
 		sock = NULL;
@@ -693,7 +693,7 @@ static int prepare_listen_socket(struct drbd_connection *connection, struct acce
 	memcpy(&my_addr, &connection->my_addr, my_addr_len);
 
 	what = "sock_create_kern";
-	err = sock_create_kern(((struct sockaddr *)&my_addr)->sa_family,
+	err = sock_create_kern(&init_net, ((struct sockaddr *)&my_addr)->sa_family,
 			       SOCK_STREAM, IPPROTO_TCP, &s_listen);
 	if (err) {
 		s_listen = NULL;
diff --git a/drivers/block/loop.c b/drivers/block/loop.c
index d7173cb1ea76..f7a4c9d7f721 100644
--- a/drivers/block/loop.c
+++ b/drivers/block/loop.c
@@ -86,8 +86,6 @@ static DEFINE_MUTEX(loop_index_mutex);
 static int max_part;
 static int part_shift;
 
-static struct workqueue_struct *loop_wq;
-
 static int transfer_xor(struct loop_device *lo, int cmd,
 			struct page *raw_page, unsigned raw_off,
 			struct page *loop_page, unsigned loop_off,
@@ -476,6 +474,28 @@ static int loop_flush(struct loop_device *lo)
 	return loop_switch(lo, NULL);
 }
 
+static void loop_reread_partitions(struct loop_device *lo,
+				   struct block_device *bdev)
+{
+	int rc;
+
+	/*
+	 * bd_mutex has been held already in release path, so don't
+	 * acquire it if this function is called in such case.
+	 *
+	 * If the reread partition isn't from release path, lo_refcnt
+	 * must be at least one and it can only become zero when the
+	 * current holder is released.
+	 */
+	if (!atomic_read(&lo->lo_refcnt))
+		rc = __blkdev_reread_part(bdev);
+	else
+		rc = blkdev_reread_part(bdev);
+	if (rc)
+		pr_warn("%s: partition scan of loop%d (%s) failed (rc=%d)\n",
+			__func__, lo->lo_number, lo->lo_file_name, rc);
+}
+
 /*
  * loop_change_fd switched the backing store of a loopback device to
  * a new file. This is useful for operating system installers to free up
@@ -524,7 +544,7 @@ static int loop_change_fd(struct loop_device *lo, struct block_device *bdev,
 
 	fput(old_file);
 	if (lo->lo_flags & LO_FLAGS_PARTSCAN)
-		ioctl_by_bdev(bdev, BLKRRPART, 0);
+		loop_reread_partitions(lo, bdev);
 	return 0;
 
  out_putf:
@@ -568,7 +588,7 @@ static ssize_t loop_attr_backing_file_show(struct loop_device *lo, char *buf)
 
 	spin_lock_irq(&lo->lo_lock);
 	if (lo->lo_backing_file)
-		p = d_path(&lo->lo_backing_file->f_path, buf, PAGE_SIZE - 1);
+		p = file_path(lo->lo_backing_file, buf, PAGE_SIZE - 1);
 	spin_unlock_irq(&lo->lo_lock);
 
 	if (IS_ERR_OR_NULL(p))
@@ -725,6 +745,12 @@ static int loop_set_fd(struct loop_device *lo, fmode_t mode,
 	size = get_loop_size(lo, file);
 	if ((loff_t)(sector_t)size != size)
 		goto out_putf;
+	error = -ENOMEM;
+	lo->wq = alloc_workqueue("kloopd%d",
+			WQ_MEM_RECLAIM | WQ_HIGHPRI | WQ_UNBOUND, 16,
+			lo->lo_number);
+	if (!lo->wq)
+		goto out_putf;
 
 	error = 0;
 
@@ -755,7 +781,7 @@ static int loop_set_fd(struct loop_device *lo, fmode_t mode,
 	if (part_shift)
 		lo->lo_flags |= LO_FLAGS_PARTSCAN;
 	if (lo->lo_flags & LO_FLAGS_PARTSCAN)
-		ioctl_by_bdev(bdev, BLKRRPART, 0);
+		loop_reread_partitions(lo, bdev);
 
 	/* Grab the block_device to prevent its destruction after we
 	 * put /dev/loopXX inode. Later in loop_clr_fd() we bdput(bdev).
@@ -827,7 +853,7 @@ static int loop_clr_fd(struct loop_device *lo)
 	 * <dev>/do something like mkfs/losetup -d <dev> causing the losetup -d
 	 * command to fail with EBUSY.
 	 */
-	if (lo->lo_refcnt > 1) {
+	if (atomic_read(&lo->lo_refcnt) > 1) {
 		lo->lo_flags |= LO_FLAGS_AUTOCLEAR;
 		mutex_unlock(&lo->lo_ctl_mutex);
 		return 0;
@@ -836,6 +862,9 @@ static int loop_clr_fd(struct loop_device *lo)
 	if (filp == NULL)
 		return -EINVAL;
 
+	/* freeze request queue during the transition */
+	blk_mq_freeze_queue(lo->lo_queue);
+
 	spin_lock_irq(&lo->lo_lock);
 	lo->lo_state = Lo_rundown;
 	lo->lo_backing_file = NULL;
@@ -867,11 +896,15 @@ static int loop_clr_fd(struct loop_device *lo)
 	lo->lo_state = Lo_unbound;
 	/* This is safe: open() is still holding a reference. */
 	module_put(THIS_MODULE);
+	blk_mq_unfreeze_queue(lo->lo_queue);
+
 	if (lo->lo_flags & LO_FLAGS_PARTSCAN && bdev)
-		ioctl_by_bdev(bdev, BLKRRPART, 0);
+		loop_reread_partitions(lo, bdev);
 	lo->lo_flags = 0;
 	if (!part_shift)
 		lo->lo_disk->flags |= GENHD_FL_NO_PART_SCAN;
+	destroy_workqueue(lo->wq);
+	lo->wq = NULL;
 	mutex_unlock(&lo->lo_ctl_mutex);
 	/*
 	 * Need not hold lo_ctl_mutex to fput backing file.
@@ -943,7 +976,7 @@ loop_set_status(struct loop_device *lo, const struct loop_info64 *info)
 	     !(lo->lo_flags & LO_FLAGS_PARTSCAN)) {
 		lo->lo_flags |= LO_FLAGS_PARTSCAN;
 		lo->lo_disk->flags &= ~GENHD_FL_NO_PART_SCAN;
-		ioctl_by_bdev(lo->lo_device, BLKRRPART, 0);
+		loop_reread_partitions(lo, lo->lo_device);
 	}
 
 	lo->lo_encrypt_key_size = info->lo_encrypt_key_size;
@@ -1324,9 +1357,7 @@ static int lo_open(struct block_device *bdev, fmode_t mode)
 		goto out;
 	}
 
-	mutex_lock(&lo->lo_ctl_mutex);
-	lo->lo_refcnt++;
-	mutex_unlock(&lo->lo_ctl_mutex);
+	atomic_inc(&lo->lo_refcnt);
 out:
 	mutex_unlock(&loop_index_mutex);
 	return err;
@@ -1337,11 +1368,10 @@ static void lo_release(struct gendisk *disk, fmode_t mode)
 	struct loop_device *lo = disk->private_data;
 	int err;
 
-	mutex_lock(&lo->lo_ctl_mutex);
-
-	if (--lo->lo_refcnt)
-		goto out;
+	if (atomic_dec_return(&lo->lo_refcnt))
+		return;
 
+	mutex_lock(&lo->lo_ctl_mutex);
 	if (lo->lo_flags & LO_FLAGS_AUTOCLEAR) {
 		/*
 		 * In autoclear mode, stop the loop thread
@@ -1358,7 +1388,6 @@ static void lo_release(struct gendisk *disk, fmode_t mode)
 		loop_flush(lo);
 	}
 
-out:
 	mutex_unlock(&lo->lo_ctl_mutex);
 }
 
@@ -1425,9 +1454,13 @@ static int loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 		const struct blk_mq_queue_data *bd)
 {
 	struct loop_cmd *cmd = blk_mq_rq_to_pdu(bd->rq);
+	struct loop_device *lo = cmd->rq->q->queuedata;
 
 	blk_mq_start_request(bd->rq);
 
+	if (lo->lo_state != Lo_bound)
+		return -EIO;
+
 	if (cmd->rq->cmd_flags & REQ_WRITE) {
 		struct loop_device *lo = cmd->rq->q->queuedata;
 		bool need_sched = true;
@@ -1441,9 +1474,9 @@ static int loop_queue_rq(struct blk_mq_hw_ctx *hctx,
 		spin_unlock_irq(&lo->lo_lock);
 
 		if (need_sched)
-			queue_work(loop_wq, &lo->write_work);
+			queue_work(lo->wq, &lo->write_work);
 	} else {
-		queue_work(loop_wq, &cmd->read_work);
+		queue_work(lo->wq, &cmd->read_work);
 	}
 
 	return BLK_MQ_RQ_QUEUE_OK;
@@ -1455,9 +1488,6 @@ static void loop_handle_cmd(struct loop_cmd *cmd)
 	struct loop_device *lo = cmd->rq->q->queuedata;
 	int ret = -EIO;
 
-	if (lo->lo_state != Lo_bound)
-		goto failed;
-
 	if (write && (lo->lo_flags & LO_FLAGS_READ_ONLY))
 		goto failed;
 
@@ -1594,6 +1624,7 @@ static int loop_add(struct loop_device **l, int i)
 		disk->flags |= GENHD_FL_NO_PART_SCAN;
 	disk->flags |= GENHD_FL_EXT_DEVT;
 	mutex_init(&lo->lo_ctl_mutex);
+	atomic_set(&lo->lo_refcnt, 0);
 	lo->lo_number		= i;
 	spin_lock_init(&lo->lo_lock);
 	disk->major		= LOOP_MAJOR;
@@ -1711,7 +1742,7 @@ static long loop_control_ioctl(struct file *file, unsigned int cmd,
 			mutex_unlock(&lo->lo_ctl_mutex);
 			break;
 		}
-		if (lo->lo_refcnt > 0) {
+		if (atomic_read(&lo->lo_refcnt) > 0) {
 			ret = -EBUSY;
 			mutex_unlock(&lo->lo_ctl_mutex);
 			break;
@@ -1806,13 +1837,6 @@ static int __init loop_init(void)
 		goto misc_out;
 	}
 
-	loop_wq = alloc_workqueue("kloopd",
-			WQ_MEM_RECLAIM | WQ_HIGHPRI | WQ_UNBOUND, 0);
-	if (!loop_wq) {
-		err = -ENOMEM;
-		goto misc_out;
-	}
-
 	blk_register_region(MKDEV(LOOP_MAJOR, 0), range,
 				  THIS_MODULE, loop_probe, NULL, NULL);
 
@@ -1850,8 +1874,6 @@ static void __exit loop_exit(void)
 	blk_unregister_region(MKDEV(LOOP_MAJOR, 0), range);
 	unregister_blkdev(LOOP_MAJOR, "loop");
 
-	destroy_workqueue(loop_wq);
-
 	misc_deregister(&loop_misc);
 }
 
diff --git a/drivers/block/loop.h b/drivers/block/loop.h
index 301c27f8323f..25e8997ed246 100644
--- a/drivers/block/loop.h
+++ b/drivers/block/loop.h
@@ -28,7 +28,7 @@ struct loop_func_table;
 
 struct loop_device {
 	int		lo_number;
-	int		lo_refcnt;
+	atomic_t	lo_refcnt;
 	loff_t		lo_offset;
 	loff_t		lo_sizelimit;
 	int		lo_flags;
@@ -54,6 +54,7 @@ struct loop_device {
 	gfp_t		old_gfp_mask;
 
 	spinlock_t		lo_lock;
+	struct workqueue_struct *wq;
 	struct list_head	write_cmd_head;
 	struct work_struct	write_work;
 	bool			write_started;
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 3bd7ca9853a8..4a2ef09e6704 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -163,12 +163,6 @@ static bool mtip_check_surprise_removal(struct pci_dev *pdev)
 		else
 			dev_warn(&dd->pdev->dev,
 				"%s: dd->queue is NULL\n", __func__);
-		if (dd->port) {
-			set_bit(MTIP_PF_SR_CLEANUP_BIT, &dd->port->flags);
-			wake_up_interruptible(&dd->port->svc_wait);
-		} else
-			dev_warn(&dd->pdev->dev,
-				"%s: dd->port is NULL\n", __func__);
 		return true; /* device removed */
 	}
 
@@ -269,8 +263,11 @@ static int mtip_hba_reset(struct driver_data *dd)
 	/* Flush */
 	readl(dd->mmio + HOST_CTL);
 
-	/* Spin for up to 2 seconds, waiting for reset acknowledgement */
-	timeout = jiffies + msecs_to_jiffies(2000);
+	/*
+	 * Spin for up to 10 seconds waiting for reset acknowledgement. Spec
+	 * is 1 sec but in LUN failure conditions, up to 10 secs are required
+	 */
+	timeout = jiffies + msecs_to_jiffies(10000);
 	do {
 		mdelay(10);
 		if (test_bit(MTIP_DDF_REMOVE_PENDING_BIT, &dd->dd_flag))
@@ -623,8 +620,7 @@ static void mtip_handle_tfe(struct driver_data *dd)
 
 	set_bit(MTIP_PF_EH_ACTIVE_BIT, &port->flags);
 
-	if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags) &&
-			test_bit(MTIP_TAG_INTERNAL, port->allocated)) {
+	if (test_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags)) {
 		cmd = mtip_cmd_from_tag(dd, MTIP_TAG_INTERNAL);
 		dbg_printk(MTIP_DRV_NAME " TFE for the internal command\n");
 
@@ -896,6 +892,10 @@ static inline irqreturn_t mtip_handle_irq(struct driver_data *data)
 
 		/* Acknowledge the interrupt status on the port.*/
 		port_stat = readl(port->mmio + PORT_IRQ_STAT);
+		if (unlikely(port_stat == 0xFFFFFFFF)) {
+			mtip_check_surprise_removal(dd->pdev);
+			return IRQ_HANDLED;
+		}
 		writel(port_stat, port->mmio + PORT_IRQ_STAT);
 
 		/* Demux port status */
@@ -991,15 +991,10 @@ static bool mtip_pause_ncq(struct mtip_port *port,
 	reply = port->rxfis + RX_FIS_D2H_REG;
 	task_file_data = readl(port->mmio+PORT_TFDATA);
 
-	if (fis->command == ATA_CMD_SEC_ERASE_UNIT)
-		clear_bit(MTIP_DDF_SEC_LOCK_BIT, &port->dd->dd_flag);
-
 	if ((task_file_data & 1))
 		return false;
 
 	if (fis->command == ATA_CMD_SEC_ERASE_PREP) {
-		set_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
-		set_bit(MTIP_DDF_SEC_LOCK_BIT, &port->dd->dd_flag);
 		port->ic_pause_timer = jiffies;
 		return true;
 	} else if ((fis->command == ATA_CMD_DOWNLOAD_MICRO) &&
@@ -1011,8 +1006,10 @@ static bool mtip_pause_ncq(struct mtip_port *port,
 		((fis->command == 0xFC) &&
 			(fis->features == 0x27 || fis->features == 0x72 ||
 			 fis->features == 0x62 || fis->features == 0x26))) {
+		clear_bit(MTIP_DDF_SEC_LOCK_BIT, &port->dd->dd_flag);
 		/* Com reset after secure erase or lowlevel format */
 		mtip_restart_port(port);
+		clear_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
 		return false;
 	}
 
@@ -1112,9 +1109,10 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 	int_cmd = mtip_get_int_command(dd);
 
 	set_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
-	port->ic_pause_timer = 0;
 
-	clear_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
+	if (fis->command == ATA_CMD_SEC_ERASE_PREP)
+		set_bit(MTIP_PF_SE_ACTIVE_BIT, &port->flags);
+
 	clear_bit(MTIP_PF_DM_ACTIVE_BIT, &port->flags);
 
 	if (atomic == GFP_KERNEL) {
@@ -1251,11 +1249,11 @@ static int mtip_exec_internal_command(struct mtip_port *port,
 exec_ic_exit:
 	/* Clear the allocated and active bits for the internal command. */
 	mtip_put_int_command(dd, int_cmd);
+	clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
 	if (rv >= 0 && mtip_pause_ncq(port, fis)) {
 		/* NCQ paused */
 		return rv;
 	}
-	clear_bit(MTIP_PF_IC_ACTIVE_BIT, &port->flags);
 	wake_up_interruptible(&port->svc_wait);
 
 	return rv;
@@ -2625,18 +2623,6 @@ static ssize_t mtip_hw_read_registers(struct file *f, char __user *ubuf,
 				readl(dd->mmio + HOST_IRQ_STAT));
 	size += sprintf(&buf[size], "\n");
 
-	size += sprintf(&buf[size], "L/ Allocated     : [ 0x");
-
-	for (n = dd->slot_groups-1; n >= 0; n--) {
-		if (sizeof(long) > sizeof(u32))
-			group_allocated =
-				dd->port->allocated[n/2] >> (32*(n&1));
-		else
-			group_allocated = dd->port->allocated[n];
-		size += sprintf(&buf[size], "%08X ", group_allocated);
-	}
-	size += sprintf(&buf[size], "]\n");
-
 	size += sprintf(&buf[size], "L/ Commands in Q : [ 0x");
 
 	for (n = dd->slot_groups-1; n >= 0; n--) {
@@ -2780,48 +2766,6 @@ static void mtip_hw_debugfs_exit(struct driver_data *dd)
 		debugfs_remove_recursive(dd->dfs_node);
 }
 
-static int mtip_free_orphan(struct driver_data *dd)
-{
-	struct kobject *kobj;
-
-	if (dd->bdev) {
-		if (dd->bdev->bd_holders >= 1)
-			return -2;
-
-		bdput(dd->bdev);
-		dd->bdev = NULL;
-	}
-
-	mtip_hw_debugfs_exit(dd);
-
-	spin_lock(&rssd_index_lock);
-	ida_remove(&rssd_index_ida, dd->index);
-	spin_unlock(&rssd_index_lock);
-
-	if (!test_bit(MTIP_DDF_INIT_DONE_BIT, &dd->dd_flag) &&
-			test_bit(MTIP_DDF_REBUILD_FAILED_BIT, &dd->dd_flag)) {
-		put_disk(dd->disk);
-	} else {
-		if (dd->disk) {
-			kobj = kobject_get(&disk_to_dev(dd->disk)->kobj);
-			if (kobj) {
-				mtip_hw_sysfs_exit(dd, kobj);
-				kobject_put(kobj);
-			}
-			del_gendisk(dd->disk);
-			dd->disk = NULL;
-		}
-		if (dd->queue) {
-			dd->queue->queuedata = NULL;
-			blk_cleanup_queue(dd->queue);
-			blk_mq_free_tag_set(&dd->tags);
-			dd->queue = NULL;
-		}
-	}
-	kfree(dd);
-	return 0;
-}
-
 /*
  * Perform any init/resume time hardware setup
  *
@@ -2944,7 +2888,6 @@ static int mtip_ftl_rebuild_poll(struct driver_data *dd)
 			mtip_block_initialize(dd);
 			return 0;
 		}
-		ssleep(10);
 	} while (time_before(jiffies, timeout));
 
 	/* Check for timeout */
@@ -2969,7 +2912,6 @@ static int mtip_service_thread(void *data)
 	unsigned long slot, slot_start, slot_wrap;
 	unsigned int num_cmd_slots = dd->slot_groups * 32;
 	struct mtip_port *port = dd->port;
-	int ret;
 
 	while (1) {
 		if (kthread_should_stop() ||
@@ -2990,10 +2932,6 @@ static int mtip_service_thread(void *data)
 			test_bit(MTIP_PF_SVC_THD_STOP_BIT, &port->flags))
 			goto st_out;
 
-		/* If I am an orphan, start self cleanup */
-		if (test_bit(MTIP_PF_SR_CLEANUP_BIT, &port->flags))
-			break;
-
 		if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
 				&dd->dd_flag)))
 			goto st_out;
@@ -3047,26 +2985,6 @@ restart_eh:
 		}
 	}
 
-	/* wait for pci remove to exit */
-	while (1) {
-		if (test_bit(MTIP_DDF_REMOVE_DONE_BIT, &dd->dd_flag))
-			break;
-		msleep_interruptible(1000);
-		if (kthread_should_stop())
-			goto st_out;
-	}
-
-	while (1) {
-		ret = mtip_free_orphan(dd);
-		if (!ret) {
-			/* NOTE: All data structures are invalid, do not
-			 * access any here */
-			return 0;
-		}
-		msleep_interruptible(1000);
-		if (kthread_should_stop())
-			goto st_out;
-	}
 st_out:
 	return 0;
 }
@@ -3394,6 +3312,7 @@ static int mtip_hw_exit(struct driver_data *dd)
 	/* Release the IRQ. */
 	irq_set_affinity_hint(dd->pdev->irq, NULL);
 	devm_free_irq(&dd->pdev->dev, dd->pdev->irq, dd);
+	msleep(1000);
 
 	/* Free dma regions */
 	mtip_dma_free(dd);
@@ -3699,6 +3618,26 @@ static const struct block_device_operations mtip_block_ops = {
 	.owner		= THIS_MODULE
 };
 
+static inline bool is_se_active(struct driver_data *dd)
+{
+	if (unlikely(test_bit(MTIP_PF_SE_ACTIVE_BIT, &dd->port->flags))) {
+		if (dd->port->ic_pause_timer) {
+			unsigned long to = dd->port->ic_pause_timer +
+							msecs_to_jiffies(1000);
+			if (time_after(jiffies, to)) {
+				clear_bit(MTIP_PF_SE_ACTIVE_BIT,
+							&dd->port->flags);
+				clear_bit(MTIP_DDF_SEC_LOCK_BIT, &dd->dd_flag);
+				dd->port->ic_pause_timer = 0;
+				wake_up_interruptible(&dd->port->svc_wait);
+				return false;
+			}
+		}
+		return true;
+	}
+	return false;
+}
+
 /*
  * Block layer make request function.
  *
@@ -3716,6 +3655,9 @@ static int mtip_submit_request(struct blk_mq_hw_ctx *hctx, struct request *rq)
 	struct mtip_cmd *cmd = blk_mq_rq_to_pdu(rq);
 	unsigned int nents;
 
+	if (is_se_active(dd))
+		return -ENODATA;
+
 	if (unlikely(dd->dd_flag & MTIP_DDF_STOP_IO)) {
 		if (unlikely(test_bit(MTIP_DDF_REMOVE_PENDING_BIT,
 							&dd->dd_flag))) {
@@ -3900,7 +3842,8 @@ static int mtip_block_initialize(struct driver_data *dd)
 
 	dd->disk->driverfs_dev	= &dd->pdev->dev;
 	dd->disk->major		= dd->major;
-	dd->disk->first_minor	= dd->instance * MTIP_MAX_MINORS;
+	dd->disk->first_minor	= index * MTIP_MAX_MINORS;
+	dd->disk->minors 	= MTIP_MAX_MINORS;
 	dd->disk->fops		= &mtip_block_ops;
 	dd->disk->private_data	= dd;
 	dd->index		= index;
@@ -4066,52 +4009,51 @@ static int mtip_block_remove(struct driver_data *dd)
 {
 	struct kobject *kobj;
 
-	if (!dd->sr) {
-		mtip_hw_debugfs_exit(dd);
+	mtip_hw_debugfs_exit(dd);
 
-		if (dd->mtip_svc_handler) {
-			set_bit(MTIP_PF_SVC_THD_STOP_BIT, &dd->port->flags);
-			wake_up_interruptible(&dd->port->svc_wait);
-			kthread_stop(dd->mtip_svc_handler);
-		}
+	if (dd->mtip_svc_handler) {
+		set_bit(MTIP_PF_SVC_THD_STOP_BIT, &dd->port->flags);
+		wake_up_interruptible(&dd->port->svc_wait);
+		kthread_stop(dd->mtip_svc_handler);
+	}
 
-		/* Clean up the sysfs attributes, if created */
-		if (test_bit(MTIP_DDF_INIT_DONE_BIT, &dd->dd_flag)) {
-			kobj = kobject_get(&disk_to_dev(dd->disk)->kobj);
-			if (kobj) {
-				mtip_hw_sysfs_exit(dd, kobj);
-				kobject_put(kobj);
-			}
+	/* Clean up the sysfs attributes, if created */
+	if (test_bit(MTIP_DDF_INIT_DONE_BIT, &dd->dd_flag)) {
+		kobj = kobject_get(&disk_to_dev(dd->disk)->kobj);
+		if (kobj) {
+			mtip_hw_sysfs_exit(dd, kobj);
+			kobject_put(kobj);
 		}
+	}
 
+	if (!dd->sr)
 		mtip_standby_drive(dd);
-
-		/*
-		 * Delete our gendisk structure. This also removes the device
-		 * from /dev
-		 */
-		if (dd->bdev) {
-			bdput(dd->bdev);
-			dd->bdev = NULL;
-		}
-		if (dd->disk) {
-			if (dd->disk->queue) {
-				del_gendisk(dd->disk);
-				blk_cleanup_queue(dd->queue);
-				blk_mq_free_tag_set(&dd->tags);
-				dd->queue = NULL;
-			} else
-				put_disk(dd->disk);
-		}
-		dd->disk  = NULL;
-
-		spin_lock(&rssd_index_lock);
-		ida_remove(&rssd_index_ida, dd->index);
-		spin_unlock(&rssd_index_lock);
-	} else {
+	else
 		dev_info(&dd->pdev->dev, "device %s surprise removal\n",
 						dd->disk->disk_name);
+
+	/*
+	 * Delete our gendisk structure. This also removes the device
+	 * from /dev
+	 */
+	if (dd->bdev) {
+		bdput(dd->bdev);
+		dd->bdev = NULL;
 	}
+	if (dd->disk) {
+		del_gendisk(dd->disk);
+		if (dd->disk->queue) {
+			blk_cleanup_queue(dd->queue);
+			blk_mq_free_tag_set(&dd->tags);
+			dd->queue = NULL;
+		}
+		put_disk(dd->disk);
+	}
+	dd->disk  = NULL;
+
+	spin_lock(&rssd_index_lock);
+	ida_remove(&rssd_index_ida, dd->index);
+	spin_unlock(&rssd_index_lock);
 
 	/* De-initialize the protocol layer. */
 	mtip_hw_exit(dd);
@@ -4140,12 +4082,12 @@ static int mtip_block_shutdown(struct driver_data *dd)
 		dev_info(&dd->pdev->dev,
 			"Shutting down %s ...\n", dd->disk->disk_name);
 
+		del_gendisk(dd->disk);
 		if (dd->disk->queue) {
-			del_gendisk(dd->disk);
 			blk_cleanup_queue(dd->queue);
 			blk_mq_free_tag_set(&dd->tags);
-		} else
-			put_disk(dd->disk);
+		}
+		put_disk(dd->disk);
 		dd->disk  = NULL;
 		dd->queue = NULL;
 	}
@@ -4507,6 +4449,7 @@ static void mtip_pci_remove(struct pci_dev *pdev)
 			"Completion workers still active!\n");
 	}
 
+	blk_mq_stop_hw_queues(dd->queue);
 	/* Clean up the block layer. */
 	mtip_block_remove(dd);
 
@@ -4524,10 +4467,7 @@ static void mtip_pci_remove(struct pci_dev *pdev)
 	list_del_init(&dd->remove_list);
 	spin_unlock_irqrestore(&dev_lock, flags);
 
-	if (!dd->sr)
-		kfree(dd);
-	else
-		set_bit(MTIP_DDF_REMOVE_DONE_BIT, &dd->dd_flag);
+	kfree(dd);
 
 	pcim_iounmap_regions(pdev, 1 << MTIP_ABAR);
 	pci_set_drvdata(pdev, NULL);
diff --git a/drivers/block/mtip32xx/mtip32xx.h b/drivers/block/mtip32xx/mtip32xx.h
index ba1b31ee22ec..3274784008eb 100644
--- a/drivers/block/mtip32xx/mtip32xx.h
+++ b/drivers/block/mtip32xx/mtip32xx.h
@@ -142,7 +142,6 @@ enum {
 	MTIP_PF_SVC_THD_ACTIVE_BIT  = 4,
 	MTIP_PF_ISSUE_CMDS_BIT      = 5,
 	MTIP_PF_REBUILD_BIT         = 6,
-	MTIP_PF_SR_CLEANUP_BIT      = 7,
 	MTIP_PF_SVC_THD_STOP_BIT    = 8,
 
 	/* below are bit numbers in 'dd_flag' defined in driver_data */
@@ -150,7 +149,6 @@ enum {
 	MTIP_DDF_REMOVE_PENDING_BIT = 1,
 	MTIP_DDF_OVER_TEMP_BIT      = 2,
 	MTIP_DDF_WRITE_PROTECT_BIT  = 3,
-	MTIP_DDF_REMOVE_DONE_BIT    = 4,
 	MTIP_DDF_CLEANUP_BIT        = 5,
 	MTIP_DDF_RESUME_BIT         = 6,
 	MTIP_DDF_INIT_DONE_BIT      = 7,
@@ -412,19 +410,13 @@ struct mtip_port {
 	 * by the DMA when the driver issues internal commands.
 	 */
 	dma_addr_t sector_buffer_dma;
-	/*
-	 * Bit significant, used to determine if a command slot has
-	 * been allocated. i.e. the slot is in use.  Bits are cleared
-	 * when the command slot and all associated data structures
-	 * are no longer needed.
-	 */
+
 	u16 *log_buf;
 	dma_addr_t log_buf_dma;
 
 	u8 *smart_buf;
 	dma_addr_t smart_buf_dma;
 
-	unsigned long allocated[SLOTBITS_IN_LONGS];
 	/*
 	 * used to queue commands when an internal command is in progress
 	 * or error handling is active
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index 39e5f7fae3ef..0e385d8e9b86 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -230,29 +230,40 @@ static int nbd_send_req(struct nbd_device *nbd, struct request *req)
 	int result, flags;
 	struct nbd_request request;
 	unsigned long size = blk_rq_bytes(req);
+	u32 type;
+
+	if (req->cmd_type == REQ_TYPE_DRV_PRIV)
+		type = NBD_CMD_DISC;
+	else if (req->cmd_flags & REQ_DISCARD)
+		type = NBD_CMD_TRIM;
+	else if (req->cmd_flags & REQ_FLUSH)
+		type = NBD_CMD_FLUSH;
+	else if (rq_data_dir(req) == WRITE)
+		type = NBD_CMD_WRITE;
+	else
+		type = NBD_CMD_READ;
 
 	memset(&request, 0, sizeof(request));
 	request.magic = htonl(NBD_REQUEST_MAGIC);
-	request.type = htonl(nbd_cmd(req));
-
-	if (nbd_cmd(req) != NBD_CMD_FLUSH && nbd_cmd(req) != NBD_CMD_DISC) {
+	request.type = htonl(type);
+	if (type != NBD_CMD_FLUSH && type != NBD_CMD_DISC) {
 		request.from = cpu_to_be64((u64)blk_rq_pos(req) << 9);
 		request.len = htonl(size);
 	}
 	memcpy(request.handle, &req, sizeof(req));
 
 	dev_dbg(nbd_to_dev(nbd), "request %p: sending control (%s@%llu,%uB)\n",
-		req, nbdcmd_to_ascii(nbd_cmd(req)),
+		req, nbdcmd_to_ascii(type),
 		(unsigned long long)blk_rq_pos(req) << 9, blk_rq_bytes(req));
 	result = sock_xmit(nbd, 1, &request, sizeof(request),
-			(nbd_cmd(req) == NBD_CMD_WRITE) ? MSG_MORE : 0);
+			(type == NBD_CMD_WRITE) ? MSG_MORE : 0);
 	if (result <= 0) {
 		dev_err(disk_to_dev(nbd->disk),
 			"Send control failed (result %d)\n", result);
 		return -EIO;
 	}
 
-	if (nbd_cmd(req) == NBD_CMD_WRITE) {
+	if (type == NBD_CMD_WRITE) {
 		struct req_iterator iter;
 		struct bio_vec bvec;
 		/*
@@ -352,7 +363,7 @@ static struct request *nbd_read_stat(struct nbd_device *nbd)
 	}
 
 	dev_dbg(nbd_to_dev(nbd), "request %p: got reply\n", req);
-	if (nbd_cmd(req) == NBD_CMD_READ) {
+	if (rq_data_dir(req) != WRITE) {
 		struct req_iterator iter;
 		struct bio_vec bvec;
 
@@ -452,23 +463,11 @@ static void nbd_handle_req(struct nbd_device *nbd, struct request *req)
 	if (req->cmd_type != REQ_TYPE_FS)
 		goto error_out;
 
-	nbd_cmd(req) = NBD_CMD_READ;
-	if (rq_data_dir(req) == WRITE) {
-		if ((req->cmd_flags & REQ_DISCARD)) {
-			WARN_ON(!(nbd->flags & NBD_FLAG_SEND_TRIM));
-			nbd_cmd(req) = NBD_CMD_TRIM;
-		} else
-			nbd_cmd(req) = NBD_CMD_WRITE;
-		if (nbd->flags & NBD_FLAG_READ_ONLY) {
-			dev_err(disk_to_dev(nbd->disk),
-				"Write on read-only\n");
-			goto error_out;
-		}
-	}
-
-	if (req->cmd_flags & REQ_FLUSH) {
-		BUG_ON(unlikely(blk_rq_sectors(req)));
-		nbd_cmd(req) = NBD_CMD_FLUSH;
+	if (rq_data_dir(req) == WRITE &&
+	    (nbd->flags & NBD_FLAG_READ_ONLY)) {
+		dev_err(disk_to_dev(nbd->disk),
+			"Write on read-only\n");
+		goto error_out;
 	}
 
 	req->errors = 0;
@@ -592,8 +591,7 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 		fsync_bdev(bdev);
 		mutex_lock(&nbd->tx_lock);
 		blk_rq_init(NULL, &sreq);
-		sreq.cmd_type = REQ_TYPE_SPECIAL;
-		nbd_cmd(&sreq) = NBD_CMD_DISC;
+		sreq.cmd_type = REQ_TYPE_DRV_PRIV;
 
 		/* Check again after getting mutex back.  */
 		if (!nbd->sock)
@@ -713,7 +711,7 @@ static int __nbd_ioctl(struct block_device *bdev, struct nbd_device *nbd,
 		bdev->bd_inode->i_size = 0;
 		set_capacity(nbd->disk, 0);
 		if (max_part > 0)
-			ioctl_by_bdev(bdev, BLKRRPART, 0);
+			blkdev_reread_part(bdev);
 		if (nbd->disconnect) /* user requested, ignore socket errors */
 			return 0;
 		return nbd->harderror;
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index 65cd61a4145e..69de41a87b74 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -99,7 +99,7 @@ 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 = {
+static const struct kernel_param_ops null_queue_mode_param_ops = {
 	.set	= null_set_queue_mode,
 	.get	= param_get_int,
 };
@@ -127,7 +127,7 @@ static int null_set_irqmode(const char *str, const struct kernel_param *kp)
 					NULL_IRQ_TIMER);
 }
 
-static struct kernel_param_ops null_irqmode_param_ops = {
+static const struct kernel_param_ops null_irqmode_param_ops = {
 	.set	= null_set_irqmode,
 	.get	= param_get_int,
 };
@@ -243,6 +243,17 @@ static enum hrtimer_restart null_cmd_timer_expired(struct hrtimer *timer)
 			cmd = container_of(entry, struct nullb_cmd, ll_list);
 			entry = entry->next;
 			end_cmd(cmd);
+
+			if (cmd->rq) {
+				struct request_queue *q = cmd->rq->q;
+
+				if (!q->mq_ops && blk_queue_stopped(q)) {
+					spin_lock(q->queue_lock);
+					if (blk_queue_stopped(q))
+						blk_start_queue(q);
+					spin_unlock(q->queue_lock);
+				}
+			}
 		} while (entry);
 	}
 
@@ -257,7 +268,7 @@ static void null_cmd_end_timer(struct nullb_cmd *cmd)
 	if (llist_add(&cmd->ll_list, &cq->list)) {
 		ktime_t kt = ktime_set(0, completion_nsec);
 
-		hrtimer_start(&cq->timer, kt, HRTIMER_MODE_REL);
+		hrtimer_start(&cq->timer, kt, HRTIMER_MODE_REL_PINNED);
 	}
 
 	put_cpu();
@@ -334,6 +345,7 @@ static int null_rq_prep_fn(struct request_queue *q, struct request *req)
 		req->special = cmd;
 		return BLKPREP_OK;
 	}
+	blk_stop_queue(q);
 
 	return BLKPREP_DEFER;
 }
diff --git a/drivers/block/nvme-core.c b/drivers/block/nvme-core.c
index 85b8036deaa3..d1d6141920d3 100644
--- a/drivers/block/nvme-core.c
+++ b/drivers/block/nvme-core.c
@@ -29,6 +29,7 @@
 #include <linux/kdev_t.h>
 #include <linux/kthread.h>
 #include <linux/kernel.h>
+#include <linux/list_sort.h>
 #include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
@@ -80,6 +81,7 @@ static wait_queue_head_t nvme_kthread_wait;
 static struct class *nvme_class;
 
 static void nvme_reset_failed_dev(struct work_struct *ws);
+static int nvme_reset(struct nvme_dev *dev);
 static int nvme_process_cq(struct nvme_queue *nvmeq);
 
 struct async_cmd_info {
@@ -102,6 +104,7 @@ struct nvme_queue {
 	spinlock_t q_lock;
 	struct nvme_command *sq_cmds;
 	volatile struct nvme_completion *cqes;
+	struct blk_mq_tags **tags;
 	dma_addr_t sq_dma_addr;
 	dma_addr_t cq_dma_addr;
 	u32 __iomem *q_db;
@@ -114,7 +117,6 @@ struct nvme_queue {
 	u8 cq_phase;
 	u8 cqe_seen;
 	struct async_cmd_info cmdinfo;
-	struct blk_mq_hw_ctx *hctx;
 };
 
 /*
@@ -182,12 +184,22 @@ static int nvme_admin_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
 	struct nvme_dev *dev = data;
 	struct nvme_queue *nvmeq = dev->queues[0];
 
-	WARN_ON(nvmeq->hctx);
-	nvmeq->hctx = hctx;
+	WARN_ON(hctx_idx != 0);
+	WARN_ON(dev->admin_tagset.tags[0] != hctx->tags);
+	WARN_ON(nvmeq->tags);
+
 	hctx->driver_data = nvmeq;
+	nvmeq->tags = &dev->admin_tagset.tags[0];
 	return 0;
 }
 
+static void nvme_admin_exit_hctx(struct blk_mq_hw_ctx *hctx, unsigned int hctx_idx)
+{
+	struct nvme_queue *nvmeq = hctx->driver_data;
+
+	nvmeq->tags = NULL;
+}
+
 static int nvme_admin_init_request(void *data, struct request *req,
 				unsigned int hctx_idx, unsigned int rq_idx,
 				unsigned int numa_node)
@@ -201,27 +213,16 @@ static int nvme_admin_init_request(void *data, struct request *req,
 	return 0;
 }
 
-static void nvme_exit_hctx(struct blk_mq_hw_ctx *hctx, unsigned int hctx_idx)
-{
-	struct nvme_queue *nvmeq = hctx->driver_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;
+	struct nvme_queue *nvmeq = dev->queues[hctx_idx + 1];
 
-	/* 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);
+	if (!nvmeq->tags)
+		nvmeq->tags = &dev->tagset.tags[hctx_idx];
 
+	WARN_ON(dev->tagset.tags[hctx_idx] != hctx->tags);
 	hctx->driver_data = nvmeq;
 	return 0;
 }
@@ -307,9 +308,16 @@ static void async_req_completion(struct nvme_queue *nvmeq, void *ctx,
 
 	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);
+	if (status != NVME_SC_SUCCESS)
+		return;
+
+	switch (result & 0xff07) {
+	case NVME_AER_NOTICE_NS_CHANGED:
+		dev_info(nvmeq->q_dmadev, "rescanning\n");
+		schedule_work(&nvmeq->dev->scan_work);
+	default:
+		dev_warn(nvmeq->q_dmadev, "async event result %08x\n", result);
+	}
 }
 
 static void abort_completion(struct nvme_queue *nvmeq, void *ctx,
@@ -320,7 +328,7 @@ static void abort_completion(struct nvme_queue *nvmeq, void *ctx,
 	u16 status = le16_to_cpup(&cqe->status) >> 1;
 	u32 result = le32_to_cpup(&cqe->result);
 
-	blk_mq_free_hctx_request(nvmeq->hctx, req);
+	blk_mq_free_request(req);
 
 	dev_warn(nvmeq->q_dmadev, "Abort status:%x result:%x", status, result);
 	++nvmeq->dev->abort_limit;
@@ -333,14 +341,13 @@ static void async_completion(struct nvme_queue *nvmeq, void *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);
+	blk_mq_free_request(cmdinfo->req);
 }
 
 static inline struct nvme_cmd_info *get_cmd_from_tag(struct nvme_queue *nvmeq,
 				  unsigned int tag)
 {
-	struct blk_mq_hw_ctx *hctx = nvmeq->hctx;
-	struct request *req = blk_mq_tag_to_rq(hctx->tags, tag);
+	struct request *req = blk_mq_tag_to_rq(*nvmeq->tags, tag);
 
 	return blk_mq_rq_to_pdu(req);
 }
@@ -445,7 +452,7 @@ static struct nvme_iod *nvme_alloc_iod(struct request *rq, struct nvme_dev *dev,
 				(unsigned long) rq, gfp);
 }
 
-void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
+static void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
 {
 	const int last_prp = dev->page_size / 8 - 1;
 	int i;
@@ -605,22 +612,33 @@ static void req_completion(struct nvme_queue *nvmeq, void *ctx,
 			spin_unlock_irqrestore(req->q->queue_lock, flags);
 			return;
 		}
-		req->errors = nvme_error_status(status);
+		if (req->cmd_type == REQ_TYPE_DRV_PRIV) {
+			if (cmd_rq->ctx == CMD_CTX_CANCELLED)
+				req->errors = -EINTR;
+			else
+				req->errors = status;
+		} else {
+			req->errors = nvme_error_status(status);
+		}
 	} else
 		req->errors = 0;
+	if (req->cmd_type == REQ_TYPE_DRV_PRIV) {
+		u32 result = le32_to_cpup(&cqe->result);
+		req->special = (void *)(uintptr_t)result;
+	}
 
 	if (cmd_rq->aborted)
-		dev_warn(&nvmeq->dev->pci_dev->dev,
+		dev_warn(nvmeq->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,
+		dma_unmap_sg(nvmeq->dev->dev, iod->sg, iod->nents,
 			rq_data_dir(req) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
 		if (blk_integrity_rq(req)) {
 			if (!rq_data_dir(req))
 				nvme_dif_remap(req, nvme_dif_complete);
-			dma_unmap_sg(&nvmeq->dev->pci_dev->dev, iod->meta_sg, 1,
+			dma_unmap_sg(nvmeq->dev->dev, iod->meta_sg, 1,
 				rq_data_dir(req) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
 		}
 	}
@@ -630,8 +648,8 @@ static void req_completion(struct nvme_queue *nvmeq, void *ctx,
 }
 
 /* length is in bytes.  gfp flags indicates whether we may sleep. */
-int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod, int total_len,
-								gfp_t gfp)
+static int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod,
+		int total_len, gfp_t gfp)
 {
 	struct dma_pool *pool;
 	int length = total_len;
@@ -709,6 +727,23 @@ int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod, int total_len,
 	return total_len;
 }
 
+static void nvme_submit_priv(struct nvme_queue *nvmeq, struct request *req,
+		struct nvme_iod *iod)
+{
+	struct nvme_command *cmnd = &nvmeq->sq_cmds[nvmeq->sq_tail];
+
+	memcpy(cmnd, req->cmd, sizeof(struct nvme_command));
+	cmnd->rw.command_id = req->tag;
+	if (req->nr_phys_segments) {
+		cmnd->rw.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
+		cmnd->rw.prp2 = cpu_to_le64(iod->first_dma);
+	}
+
+	if (++nvmeq->sq_tail == nvmeq->q_depth)
+		nvmeq->sq_tail = 0;
+	writel(nvmeq->sq_tail, nvmeq->q_db);
+}
+
 /*
  * 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
@@ -807,11 +842,15 @@ static int nvme_submit_iod(struct nvme_queue *nvmeq, struct nvme_iod *iod,
 	return 0;
 }
 
+/*
+ * NOTE: ns is NULL when called on the admin queue.
+ */
 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 nvme_dev *dev = nvmeq->dev;
 	struct request *req = bd->rq;
 	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
 	struct nvme_iod *iod;
@@ -822,15 +861,16 @@ static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 	 * unless this namespace is formated such that the metadata can be
 	 * stripped/generated by the controller with PRACT=1.
 	 */
-	if (ns->ms && !blk_integrity_rq(req)) {
-		if (!(ns->pi_type && ns->ms == 8)) {
+	if (ns && ns->ms && !blk_integrity_rq(req)) {
+		if (!(ns->pi_type && ns->ms == 8) &&
+					req->cmd_type != REQ_TYPE_DRV_PRIV) {
 			req->errors = -EFAULT;
 			blk_mq_complete_request(req);
 			return BLK_MQ_RQ_QUEUE_OK;
 		}
 	}
 
-	iod = nvme_alloc_iod(req, ns->dev, GFP_ATOMIC);
+	iod = nvme_alloc_iod(req, dev, GFP_ATOMIC);
 	if (!iod)
 		return BLK_MQ_RQ_QUEUE_BUSY;
 
@@ -841,8 +881,7 @@ static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 		 * as it is not worth having a special pool for these or
 		 * additional cases to handle freeing the iod.
 		 */
-		range = dma_pool_alloc(nvmeq->dev->prp_small_pool,
-						GFP_ATOMIC,
+		range = dma_pool_alloc(dev->prp_small_pool, GFP_ATOMIC,
 						&iod->first_dma);
 		if (!range)
 			goto retry_cmd;
@@ -860,9 +899,8 @@ static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 			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);
+                    nvme_setup_prps(dev, iod, blk_rq_bytes(req), GFP_ATOMIC)) {
+			dma_unmap_sg(dev->dev, iod->sg, iod->nents, dma_dir);
 			goto retry_cmd;
 		}
 		if (blk_integrity_rq(req)) {
@@ -884,7 +922,9 @@ static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 
 	nvme_set_info(cmd, iod, req_completion);
 	spin_lock_irq(&nvmeq->q_lock);
-	if (req->cmd_flags & REQ_DISCARD)
+	if (req->cmd_type == REQ_TYPE_DRV_PRIV)
+		nvme_submit_priv(nvmeq, req, iod);
+	else 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);
@@ -896,10 +936,10 @@ static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 	return BLK_MQ_RQ_QUEUE_OK;
 
  error_cmd:
-	nvme_free_iod(nvmeq->dev, iod);
+	nvme_free_iod(dev, iod);
 	return BLK_MQ_RQ_QUEUE_ERROR;
  retry_cmd:
-	nvme_free_iod(nvmeq->dev, iod);
+	nvme_free_iod(dev, iod);
 	return BLK_MQ_RQ_QUEUE_BUSY;
 }
 
@@ -942,15 +982,6 @@ static int nvme_process_cq(struct nvme_queue *nvmeq)
 	return 1;
 }
 
-/* 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)
-{
-	WARN_ON_ONCE(1);
-	return BLK_MQ_RQ_QUEUE_ERROR;
-}
-
 static irqreturn_t nvme_irq(int irq, void *data)
 {
 	irqreturn_t result;
@@ -972,46 +1003,61 @@ static irqreturn_t nvme_irq_check(int irq, void *data)
 	return IRQ_WAKE_THREAD;
 }
 
-struct sync_cmd_info {
-	struct task_struct *task;
-	u32 result;
-	int status;
-};
-
-static void sync_completion(struct nvme_queue *nvmeq, void *ctx,
-						struct nvme_completion *cqe)
-{
-	struct sync_cmd_info *cmdinfo = ctx;
-	cmdinfo->result = le32_to_cpup(&cqe->result);
-	cmdinfo->status = le16_to_cpup(&cqe->status) >> 1;
-	wake_up_process(cmdinfo->task);
-}
-
 /*
  * 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 request *req, struct nvme_command *cmd,
-						u32 *result, unsigned timeout)
+int __nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
+		void *buffer, void __user *ubuffer, unsigned bufflen,
+		u32 *result, unsigned timeout)
 {
-	struct sync_cmd_info cmdinfo;
-	struct nvme_cmd_info *cmd_rq = blk_mq_rq_to_pdu(req);
-	struct nvme_queue *nvmeq = cmd_rq->nvmeq;
+	bool write = cmd->common.opcode & 1;
+	struct bio *bio = NULL;
+	struct request *req;
+	int ret;
 
-	cmdinfo.task = current;
-	cmdinfo.status = -EINTR;
+	req = blk_mq_alloc_request(q, write, GFP_KERNEL, false);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
 
-	cmd->common.command_id = req->tag;
+	req->cmd_type = REQ_TYPE_DRV_PRIV;
+	req->cmd_flags |= REQ_FAILFAST_DRIVER;
+	req->__data_len = 0;
+	req->__sector = (sector_t) -1;
+	req->bio = req->biotail = NULL;
 
-	nvme_set_info(cmd_rq, &cmdinfo, sync_completion);
+	req->timeout = timeout ? timeout : ADMIN_TIMEOUT;
 
-	set_current_state(TASK_UNINTERRUPTIBLE);
-	nvme_submit_cmd(nvmeq, cmd);
-	schedule();
+	req->cmd = (unsigned char *)cmd;
+	req->cmd_len = sizeof(struct nvme_command);
+	req->special = (void *)0;
+
+	if (buffer && bufflen) {
+		ret = blk_rq_map_kern(q, req, buffer, bufflen, __GFP_WAIT);
+		if (ret)
+			goto out;
+	} else if (ubuffer && bufflen) {
+		ret = blk_rq_map_user(q, req, NULL, ubuffer, bufflen, __GFP_WAIT);
+		if (ret)
+			goto out;
+		bio = req->bio;
+	}
 
+	blk_execute_rq(req->q, NULL, req, 0);
+	if (bio)
+		blk_rq_unmap_user(bio);
 	if (result)
-		*result = cmdinfo.result;
-	return cmdinfo.status;
+		*result = (u32)(uintptr_t)req->special;
+	ret = req->errors;
+ out:
+	blk_mq_free_request(req);
+	return ret;
+}
+
+int nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
+		void *buffer, unsigned bufflen)
+{
+	return __nvme_submit_sync_cmd(q, cmd, buffer, NULL, bufflen, NULL, 0);
 }
 
 static int nvme_submit_async_admin_req(struct nvme_dev *dev)
@@ -1033,7 +1079,7 @@ static int nvme_submit_async_admin_req(struct nvme_dev *dev)
 	c.common.opcode = nvme_admin_async_event;
 	c.common.command_id = req->tag;
 
-	blk_mq_free_hctx_request(nvmeq->hctx, req);
+	blk_mq_free_request(req);
 	return __nvme_submit_cmd(nvmeq, &c);
 }
 
@@ -1060,41 +1106,6 @@ static int nvme_submit_admin_async_cmd(struct nvme_dev *dev,
 	return nvme_submit_cmd(nvmeq, cmd);
 }
 
-static int __nvme_submit_admin_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
-						u32 *result, unsigned 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_admin_cmd(struct nvme_dev *dev, struct nvme_command *cmd,
-								u32 *result)
-{
-	return __nvme_submit_admin_cmd(dev, cmd, result, ADMIN_TIMEOUT);
-}
-
-int nvme_submit_io_cmd(struct nvme_dev *dev, struct nvme_ns *ns,
-					struct nvme_command *cmd, u32 *result)
-{
-	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)
 {
 	struct nvme_command c;
@@ -1103,7 +1114,7 @@ static int adapter_delete_queue(struct nvme_dev *dev, u8 opcode, u16 id)
 	c.delete_queue.opcode = opcode;
 	c.delete_queue.qid = cpu_to_le16(id);
 
-	return nvme_submit_admin_cmd(dev, &c, NULL);
+	return nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0);
 }
 
 static int adapter_alloc_cq(struct nvme_dev *dev, u16 qid,
@@ -1112,6 +1123,10 @@ static int adapter_alloc_cq(struct nvme_dev *dev, u16 qid,
 	struct nvme_command c;
 	int flags = NVME_QUEUE_PHYS_CONTIG | NVME_CQ_IRQ_ENABLED;
 
+	/*
+	 * Note: we (ab)use the fact the the prp fields survive if no data
+	 * is attached to the request.
+	 */
 	memset(&c, 0, sizeof(c));
 	c.create_cq.opcode = nvme_admin_create_cq;
 	c.create_cq.prp1 = cpu_to_le64(nvmeq->cq_dma_addr);
@@ -1120,7 +1135,7 @@ 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);
 
-	return nvme_submit_admin_cmd(dev, &c, NULL);
+	return nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0);
 }
 
 static int adapter_alloc_sq(struct nvme_dev *dev, u16 qid,
@@ -1129,6 +1144,10 @@ static int adapter_alloc_sq(struct nvme_dev *dev, u16 qid,
 	struct nvme_command c;
 	int flags = NVME_QUEUE_PHYS_CONTIG | NVME_SQ_PRIO_MEDIUM;
 
+	/*
+	 * Note: we (ab)use the fact the the prp fields survive if no data
+	 * is attached to the request.
+	 */
 	memset(&c, 0, sizeof(c));
 	c.create_sq.opcode = nvme_admin_create_sq;
 	c.create_sq.prp1 = cpu_to_le64(nvmeq->sq_dma_addr);
@@ -1137,7 +1156,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);
 
-	return nvme_submit_admin_cmd(dev, &c, NULL);
+	return nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0);
 }
 
 static int adapter_delete_cq(struct nvme_dev *dev, u16 cqid)
@@ -1150,18 +1169,45 @@ static int adapter_delete_sq(struct nvme_dev *dev, u16 sqid)
 	return adapter_delete_queue(dev, nvme_admin_delete_sq, sqid);
 }
 
-int nvme_identify(struct nvme_dev *dev, unsigned nsid, unsigned cns,
-							dma_addr_t dma_addr)
+int nvme_identify_ctrl(struct nvme_dev *dev, struct nvme_id_ctrl **id)
 {
-	struct nvme_command c;
+	struct nvme_command c = { };
+	int error;
 
-	memset(&c, 0, sizeof(c));
+	/* gcc-4.4.4 (at least) has issues with initializers and anon unions */
 	c.identify.opcode = nvme_admin_identify;
-	c.identify.nsid = cpu_to_le32(nsid);
-	c.identify.prp1 = cpu_to_le64(dma_addr);
-	c.identify.cns = cpu_to_le32(cns);
+	c.identify.cns = cpu_to_le32(1);
+
+	*id = kmalloc(sizeof(struct nvme_id_ctrl), GFP_KERNEL);
+	if (!*id)
+		return -ENOMEM;
+
+	error = nvme_submit_sync_cmd(dev->admin_q, &c, *id,
+			sizeof(struct nvme_id_ctrl));
+	if (error)
+		kfree(*id);
+	return error;
+}
+
+int nvme_identify_ns(struct nvme_dev *dev, unsigned nsid,
+		struct nvme_id_ns **id)
+{
+	struct nvme_command c = { };
+	int error;
+
+	/* gcc-4.4.4 (at least) has issues with initializers and anon unions */
+	c.identify.opcode = nvme_admin_identify,
+	c.identify.nsid = cpu_to_le32(nsid),
+
+	*id = kmalloc(sizeof(struct nvme_id_ns), GFP_KERNEL);
+	if (!*id)
+		return -ENOMEM;
 
-	return nvme_submit_admin_cmd(dev, &c, NULL);
+	error = nvme_submit_sync_cmd(dev->admin_q, &c, *id,
+			sizeof(struct nvme_id_ns));
+	if (error)
+		kfree(*id);
+	return error;
 }
 
 int nvme_get_features(struct nvme_dev *dev, unsigned fid, unsigned nsid,
@@ -1175,7 +1221,8 @@ int nvme_get_features(struct nvme_dev *dev, unsigned fid, unsigned nsid,
 	c.features.prp1 = cpu_to_le64(dma_addr);
 	c.features.fid = cpu_to_le32(fid);
 
-	return nvme_submit_admin_cmd(dev, &c, result);
+	return __nvme_submit_sync_cmd(dev->admin_q, &c, NULL, NULL, 0,
+			result, 0);
 }
 
 int nvme_set_features(struct nvme_dev *dev, unsigned fid, unsigned dword11,
@@ -1189,7 +1236,30 @@ int nvme_set_features(struct nvme_dev *dev, unsigned fid, unsigned dword11,
 	c.features.fid = cpu_to_le32(fid);
 	c.features.dword11 = cpu_to_le32(dword11);
 
-	return nvme_submit_admin_cmd(dev, &c, result);
+	return __nvme_submit_sync_cmd(dev->admin_q, &c, NULL, NULL, 0,
+			result, 0);
+}
+
+int nvme_get_log_page(struct nvme_dev *dev, struct nvme_smart_log **log)
+{
+	struct nvme_command c = { };
+	int error;
+
+	c.common.opcode = nvme_admin_get_log_page,
+	c.common.nsid = cpu_to_le32(0xFFFFFFFF),
+	c.common.cdw10[0] = cpu_to_le32(
+			(((sizeof(struct nvme_smart_log) / 4) - 1) << 16) |
+			 NVME_LOG_SMART),
+
+	*log = kmalloc(sizeof(struct nvme_smart_log), GFP_KERNEL);
+	if (!*log)
+		return -ENOMEM;
+
+	error = nvme_submit_sync_cmd(dev->admin_q, &c, *log,
+			sizeof(struct nvme_smart_log));
+	if (error)
+		kfree(*log);
+	return error;
 }
 
 /**
@@ -1214,8 +1284,7 @@ static void nvme_abort_req(struct request *req)
 		if (work_busy(&dev->reset_work))
 			goto out;
 		list_del_init(&dev->node);
-		dev_warn(&dev->pci_dev->dev,
-			"I/O %d QID %d timeout, reset controller\n",
+		dev_warn(dev->dev, "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);
@@ -1254,8 +1323,7 @@ static void nvme_abort_req(struct request *req)
 	}
 }
 
-static void nvme_cancel_queue_ios(struct blk_mq_hw_ctx *hctx,
-				struct request *req, void *data, bool reserved)
+static void nvme_cancel_queue_ios(struct request *req, void *data, bool reserved)
 {
 	struct nvme_queue *nvmeq = data;
 	void *ctx;
@@ -1352,11 +1420,9 @@ 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);
-	if (hctx && hctx->tags)
-		blk_mq_tag_busy_iter(hctx, nvme_cancel_queue_ios, nvmeq);
+	if (nvmeq->tags && *nvmeq->tags)
+		blk_mq_all_tag_busy_iter(*nvmeq->tags, nvme_cancel_queue_ios, nvmeq);
 	spin_unlock_irq(&nvmeq->q_lock);
 }
 
@@ -1384,22 +1450,21 @@ static void nvme_disable_queue(struct nvme_dev *dev, int qid)
 static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 							int depth)
 {
-	struct device *dmadev = &dev->pci_dev->dev;
 	struct nvme_queue *nvmeq = kzalloc(sizeof(*nvmeq), GFP_KERNEL);
 	if (!nvmeq)
 		return NULL;
 
-	nvmeq->cqes = dma_zalloc_coherent(dmadev, CQ_SIZE(depth),
+	nvmeq->cqes = dma_zalloc_coherent(dev->dev, CQ_SIZE(depth),
 					  &nvmeq->cq_dma_addr, GFP_KERNEL);
 	if (!nvmeq->cqes)
 		goto free_nvmeq;
 
-	nvmeq->sq_cmds = dma_alloc_coherent(dmadev, SQ_SIZE(depth),
+	nvmeq->sq_cmds = dma_alloc_coherent(dev->dev, SQ_SIZE(depth),
 					&nvmeq->sq_dma_addr, GFP_KERNEL);
 	if (!nvmeq->sq_cmds)
 		goto free_cqdma;
 
-	nvmeq->q_dmadev = dmadev;
+	nvmeq->q_dmadev = dev->dev;
 	nvmeq->dev = dev;
 	snprintf(nvmeq->irqname, sizeof(nvmeq->irqname), "nvme%dq%d",
 			dev->instance, qid);
@@ -1409,13 +1474,17 @@ static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 	nvmeq->q_db = &dev->dbs[qid * 2 * dev->db_stride];
 	nvmeq->q_depth = depth;
 	nvmeq->qid = qid;
-	dev->queue_count++;
+	nvmeq->cq_vector = -1;
 	dev->queues[qid] = nvmeq;
 
+	/* make sure queue descriptor is set before queue count, for kthread */
+	mb();
+	dev->queue_count++;
+
 	return nvmeq;
 
  free_cqdma:
-	dma_free_coherent(dmadev, CQ_SIZE(depth), (void *)nvmeq->cqes,
+	dma_free_coherent(dev->dev, CQ_SIZE(depth), (void *)nvmeq->cqes,
 							nvmeq->cq_dma_addr);
  free_nvmeq:
 	kfree(nvmeq);
@@ -1487,7 +1556,7 @@ static int nvme_wait_ready(struct nvme_dev *dev, u64 cap, bool enabled)
 		if (fatal_signal_pending(current))
 			return -EINTR;
 		if (time_after(jiffies, timeout)) {
-			dev_err(&dev->pci_dev->dev,
+			dev_err(dev->dev,
 				"Device not ready; aborting %s\n", enabled ?
 						"initialisation" : "reset");
 			return -ENODEV;
@@ -1537,7 +1606,7 @@ static int nvme_shutdown_ctrl(struct nvme_dev *dev)
 		if (fatal_signal_pending(current))
 			return -EINTR;
 		if (time_after(jiffies, timeout)) {
-			dev_err(&dev->pci_dev->dev,
+			dev_err(dev->dev,
 				"Device shutdown incomplete; abort shutdown\n");
 			return -ENODEV;
 		}
@@ -1547,10 +1616,10 @@ static int nvme_shutdown_ctrl(struct nvme_dev *dev)
 }
 
 static struct blk_mq_ops nvme_mq_admin_ops = {
-	.queue_rq	= nvme_admin_queue_rq,
+	.queue_rq	= nvme_queue_rq,
 	.map_queue	= blk_mq_map_queue,
 	.init_hctx	= nvme_admin_init_hctx,
-	.exit_hctx	= nvme_exit_hctx,
+	.exit_hctx      = nvme_admin_exit_hctx,
 	.init_request	= nvme_admin_init_request,
 	.timeout	= nvme_timeout,
 };
@@ -1559,7 +1628,6 @@ 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,
 };
@@ -1580,7 +1648,7 @@ static int nvme_alloc_admin_tags(struct nvme_dev *dev)
 		dev->admin_tagset.queue_depth = NVME_AQ_DEPTH - 1;
 		dev->admin_tagset.reserved_tags = 1;
 		dev->admin_tagset.timeout = ADMIN_TIMEOUT;
-		dev->admin_tagset.numa_node = dev_to_node(&dev->pci_dev->dev);
+		dev->admin_tagset.numa_node = dev_to_node(dev->dev);
 		dev->admin_tagset.cmd_size = nvme_cmd_size(dev);
 		dev->admin_tagset.driver_data = dev;
 
@@ -1594,6 +1662,7 @@ static int nvme_alloc_admin_tags(struct nvme_dev *dev)
 		}
 		if (!blk_get_queue(dev->admin_q)) {
 			nvme_dev_remove_admin(dev);
+			dev->admin_q = NULL;
 			return -ENODEV;
 		}
 	} else
@@ -1613,14 +1682,14 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	unsigned dev_page_max = NVME_CAP_MPSMAX(cap) + 12;
 
 	if (page_shift < dev_page_min) {
-		dev_err(&dev->pci_dev->dev,
+		dev_err(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,
+		dev_info(dev->dev,
 				"Device maximum page size (%u) smaller than "
 				"host (%u); enabling work-around\n",
 				1 << dev_page_max, 1 << page_shift);
@@ -1658,8 +1727,10 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 
 	nvmeq->cq_vector = 0;
 	result = queue_request_irq(dev, nvmeq, nvmeq->irqname);
-	if (result)
+	if (result) {
+		nvmeq->cq_vector = -1;
 		goto free_nvmeq;
+	}
 
 	return result;
 
@@ -1668,131 +1739,51 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	return result;
 }
 
-struct nvme_iod *nvme_map_user_pages(struct nvme_dev *dev, int write,
-				unsigned long addr, unsigned length)
-{
-	int i, err, count, nents, offset;
-	struct scatterlist *sg;
-	struct page **pages;
-	struct nvme_iod *iod;
-
-	if (addr & 3)
-		return ERR_PTR(-EINVAL);
-	if (!length || length > INT_MAX - PAGE_SIZE)
-		return ERR_PTR(-EINVAL);
-
-	offset = offset_in_page(addr);
-	count = DIV_ROUND_UP(offset + length, PAGE_SIZE);
-	pages = kcalloc(count, sizeof(*pages), GFP_KERNEL);
-	if (!pages)
-		return ERR_PTR(-ENOMEM);
-
-	err = get_user_pages_fast(addr, count, 1, pages);
-	if (err < count) {
-		count = err;
-		err = -EFAULT;
-		goto put_pages;
-	}
-
-	err = -ENOMEM;
-	iod = __nvme_alloc_iod(count, length, dev, 0, GFP_KERNEL);
-	if (!iod)
-		goto put_pages;
-
-	sg = iod->sg;
-	sg_init_table(sg, count);
-	for (i = 0; i < count; i++) {
-		sg_set_page(&sg[i], pages[i],
-			    min_t(unsigned, length, PAGE_SIZE - offset),
-			    offset);
-		length -= (PAGE_SIZE - offset);
-		offset = 0;
-	}
-	sg_mark_end(&sg[i - 1]);
-	iod->nents = count;
-
-	nents = dma_map_sg(&dev->pci_dev->dev, sg, count,
-				write ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-	if (!nents)
-		goto free_iod;
-
-	kfree(pages);
-	return iod;
-
- free_iod:
-	kfree(iod);
- put_pages:
-	for (i = 0; i < count; i++)
-		put_page(pages[i]);
-	kfree(pages);
-	return ERR_PTR(err);
-}
-
-void nvme_unmap_user_pages(struct nvme_dev *dev, int write,
-			struct nvme_iod *iod)
-{
-	int i;
-
-	dma_unmap_sg(&dev->pci_dev->dev, iod->sg, iod->nents,
-				write ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-
-	for (i = 0; i < iod->nents; i++)
-		put_page(sg_page(&iod->sg[i]));
-}
-
 static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
 {
 	struct nvme_dev *dev = ns->dev;
 	struct nvme_user_io io;
 	struct nvme_command c;
-	unsigned length, meta_len, prp_len;
+	unsigned length, meta_len;
 	int status, write;
-	struct nvme_iod *iod;
 	dma_addr_t meta_dma = 0;
 	void *meta = NULL;
+	void __user *metadata;
 
 	if (copy_from_user(&io, uio, sizeof(io)))
 		return -EFAULT;
-	length = (io.nblocks + 1) << ns->lba_shift;
-	meta_len = (io.nblocks + 1) * ns->ms;
-
-	if (meta_len && ((io.metadata & 3) || !io.metadata) && !ns->ext)
-		return -EINVAL;
-	else if (meta_len && ns->ext) {
-		length += meta_len;
-		meta_len = 0;
-	}
-
-	write = io.opcode & 1;
 
 	switch (io.opcode) {
 	case nvme_cmd_write:
 	case nvme_cmd_read:
 	case nvme_cmd_compare:
-		iod = nvme_map_user_pages(dev, write, io.addr, length);
 		break;
 	default:
 		return -EINVAL;
 	}
 
-	if (IS_ERR(iod))
-		return PTR_ERR(iod);
+	length = (io.nblocks + 1) << ns->lba_shift;
+	meta_len = (io.nblocks + 1) * ns->ms;
+	metadata = (void __user *)(unsigned long)io.metadata;
+	write = io.opcode & 1;
 
-	prp_len = nvme_setup_prps(dev, iod, length, GFP_KERNEL);
-	if (length != prp_len) {
-		status = -ENOMEM;
-		goto unmap;
+	if (ns->ext) {
+		length += meta_len;
+		meta_len = 0;
 	}
 	if (meta_len) {
-		meta = dma_alloc_coherent(&dev->pci_dev->dev, meta_len,
+		if (((io.metadata & 3) || !io.metadata) && !ns->ext)
+			return -EINVAL;
+
+		meta = dma_alloc_coherent(dev->dev, meta_len,
 						&meta_dma, GFP_KERNEL);
+
 		if (!meta) {
 			status = -ENOMEM;
 			goto unmap;
 		}
 		if (write) {
-			if (copy_from_user(meta, (void __user *)io.metadata,
-								meta_len)) {
+			if (copy_from_user(meta, metadata, meta_len)) {
 				status = -EFAULT;
 				goto unmap;
 			}
@@ -1810,20 +1801,17 @@ static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
 	c.rw.reftag = cpu_to_le32(io.reftag);
 	c.rw.apptag = cpu_to_le16(io.apptag);
 	c.rw.appmask = cpu_to_le16(io.appmask);
-	c.rw.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
-	c.rw.prp2 = cpu_to_le64(iod->first_dma);
 	c.rw.metadata = cpu_to_le64(meta_dma);
-	status = nvme_submit_io_cmd(dev, ns, &c, NULL);
+
+	status = __nvme_submit_sync_cmd(ns->queue, &c, NULL,
+			(void __user *)io.addr, length, NULL, 0);
  unmap:
-	nvme_unmap_user_pages(dev, write, iod);
-	nvme_free_iod(dev, iod);
 	if (meta) {
 		if (status == NVME_SC_SUCCESS && !write) {
-			if (copy_to_user((void __user *)io.metadata, meta,
-								meta_len))
+			if (copy_to_user(metadata, meta, meta_len))
 				status = -EFAULT;
 		}
-		dma_free_coherent(&dev->pci_dev->dev, meta_len, meta, meta_dma);
+		dma_free_coherent(dev->dev, meta_len, meta, meta_dma);
 	}
 	return status;
 }
@@ -1833,9 +1821,8 @@ static int nvme_user_cmd(struct nvme_dev *dev, struct nvme_ns *ns,
 {
 	struct nvme_passthru_cmd cmd;
 	struct nvme_command c;
-	int status, length;
-	struct nvme_iod *uninitialized_var(iod);
-	unsigned timeout;
+	unsigned timeout = 0;
+	int status;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
@@ -1855,46 +1842,17 @@ static int nvme_user_cmd(struct nvme_dev *dev, struct nvme_ns *ns,
 	c.common.cdw10[4] = cpu_to_le32(cmd.cdw14);
 	c.common.cdw10[5] = cpu_to_le32(cmd.cdw15);
 
-	length = cmd.data_len;
-	if (cmd.data_len) {
-		iod = nvme_map_user_pages(dev, cmd.opcode & 1, cmd.addr,
-								length);
-		if (IS_ERR(iod))
-			return PTR_ERR(iod);
-		length = nvme_setup_prps(dev, iod, length, GFP_KERNEL);
-		c.common.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
-		c.common.prp2 = cpu_to_le64(iod->first_dma);
-	}
-
-	timeout = cmd.timeout_ms ? msecs_to_jiffies(cmd.timeout_ms) :
-								ADMIN_TIMEOUT;
-
-	if (length != cmd.data_len)
-		status = -ENOMEM;
-	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.timeout_ms)
+		timeout = msecs_to_jiffies(cmd.timeout_ms);
 
-	if (cmd.data_len) {
-		nvme_unmap_user_pages(dev, cmd.opcode & 1, iod);
-		nvme_free_iod(dev, iod);
+	status = __nvme_submit_sync_cmd(ns ? ns->queue : dev->admin_q, &c,
+			NULL, (void __user *)cmd.addr, cmd.data_len,
+			&cmd.result, timeout);
+	if (status >= 0) {
+		if (put_user(cmd.result, &ucmd->result))
+			return -EFAULT;
 	}
 
-	if ((status >= 0) && copy_to_user(&ucmd->result, &cmd.result,
-							sizeof(cmd.result)))
-		status = -EFAULT;
-
 	return status;
 }
 
@@ -1986,23 +1944,18 @@ 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;
 	u8 lbaf, pi_type;
 	u16 old_ms;
 	unsigned short bs;
 
-	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_ns(dev, ns->ns_id, &id)) {
+		dev_warn(dev->dev, "%s: Identify failure nvme%dn%d\n", __func__,
+						dev->instance, ns->ns_id);
+		return -ENODEV;
 	}
-	if (nvme_identify(dev, ns->ns_id, 0, dma_addr)) {
-		dev_warn(&dev->pci_dev->dev,
-			"identify failed ns:%d, setting capacity to 0\n",
-			ns->ns_id);
-		memset(id, 0, sizeof(*id));
+	if (id->ncap == 0) {
+		kfree(id);
+		return -ENODEV;
 	}
 
 	old_ms = ns->ms;
@@ -2036,7 +1989,7 @@ static int nvme_revalidate_disk(struct gendisk *disk)
 								!ns->ext)
 		nvme_init_integrity(ns);
 
-	if (id->ncap == 0 || (ns->ms && !blk_get_integrity(disk)))
+	if (ns->ms && !blk_get_integrity(disk))
 		set_capacity(disk, 0);
 	else
 		set_capacity(disk, le64_to_cpup(&id->nsze) << (ns->lba_shift - 9));
@@ -2044,7 +1997,7 @@ static int nvme_revalidate_disk(struct gendisk *disk)
 	if (dev->oncs & NVME_CTRL_ONCS_DSM)
 		nvme_config_discard(ns);
 
-	dma_free_coherent(&dev->pci_dev->dev, 4096, id, dma_addr);
+	kfree(id);
 	return 0;
 }
 
@@ -2071,7 +2024,7 @@ static int nvme_kthread(void *data)
 				if (work_busy(&dev->reset_work))
 					continue;
 				list_del_init(&dev->node);
-				dev_warn(&dev->pci_dev->dev,
+				dev_warn(dev->dev,
 					"Failed status: %x, reset controller\n",
 					readl(&dev->bar->csts));
 				dev->reset_workfn = nvme_reset_failed_dev;
@@ -2103,7 +2056,7 @@ static void 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 node = dev_to_node(dev->dev);
 
 	ns = kzalloc_node(sizeof(*ns), GFP_KERNEL, node);
 	if (!ns)
@@ -2151,11 +2104,16 @@ static void nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid)
 	 * requires it.
 	 */
 	set_capacity(disk, 0);
-	nvme_revalidate_disk(ns->disk);
+	if (nvme_revalidate_disk(ns->disk))
+		goto out_free_disk;
+
 	add_disk(ns->disk);
 	if (ns->ms)
 		revalidate_disk(ns->disk);
 	return;
+ out_free_disk:
+	kfree(disk);
+	list_del(&ns->list);
  out_free_queue:
 	blk_cleanup_queue(ns->queue);
  out_free_ns:
@@ -2186,8 +2144,7 @@ static int set_queue_count(struct nvme_dev *dev, int count)
 	if (status < 0)
 		return status;
 	if (status > 0) {
-		dev_err(&dev->pci_dev->dev, "Could not set queue count (%d)\n",
-									status);
+		dev_err(dev->dev, "Could not set queue count (%d)\n", status);
 		return 0;
 	}
 	return min(result & 0xffff, result >> 16) + 1;
@@ -2201,7 +2158,7 @@ static size_t db_bar_size(struct nvme_dev *dev, unsigned nr_io_queues)
 static int nvme_setup_io_queues(struct nvme_dev *dev)
 {
 	struct nvme_queue *adminq = dev->queues[0];
-	struct pci_dev *pdev = dev->pci_dev;
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
 	int result, i, vecs, nr_io_queues, size;
 
 	nr_io_queues = num_possible_cpus();
@@ -2259,8 +2216,10 @@ 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)
+	if (result) {
+		adminq->cq_vector = -1;
 		goto free_queues;
+	}
 
 	/* Free previously allocated queues that are no longer usable */
 	nvme_free_queues(dev, nr_io_queues + 1);
@@ -2273,6 +2232,99 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	return result;
 }
 
+static void nvme_free_namespace(struct nvme_ns *ns)
+{
+	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);
+}
+
+static int ns_cmp(void *priv, struct list_head *a, struct list_head *b)
+{
+	struct nvme_ns *nsa = container_of(a, struct nvme_ns, list);
+	struct nvme_ns *nsb = container_of(b, struct nvme_ns, list);
+
+	return nsa->ns_id - nsb->ns_id;
+}
+
+static struct nvme_ns *nvme_find_ns(struct nvme_dev *dev, unsigned nsid)
+{
+	struct nvme_ns *ns;
+
+	list_for_each_entry(ns, &dev->namespaces, list) {
+		if (ns->ns_id == nsid)
+			return ns;
+		if (ns->ns_id > nsid)
+			break;
+	}
+	return NULL;
+}
+
+static inline bool nvme_io_incapable(struct nvme_dev *dev)
+{
+	return (!dev->bar || readl(&dev->bar->csts) & NVME_CSTS_CFS ||
+							dev->online_queues < 2);
+}
+
+static void nvme_ns_remove(struct nvme_ns *ns)
+{
+	bool kill = nvme_io_incapable(ns->dev) && !blk_queue_dying(ns->queue);
+
+	if (kill)
+		blk_set_queue_dying(ns->queue);
+	if (ns->disk->flags & GENHD_FL_UP) {
+		if (blk_get_integrity(ns->disk))
+			blk_integrity_unregister(ns->disk);
+		del_gendisk(ns->disk);
+	}
+	if (kill || !blk_queue_dying(ns->queue)) {
+		blk_mq_abort_requeue_list(ns->queue);
+		blk_cleanup_queue(ns->queue);
+        }
+}
+
+static void nvme_scan_namespaces(struct nvme_dev *dev, unsigned nn)
+{
+	struct nvme_ns *ns, *next;
+	unsigned i;
+
+	for (i = 1; i <= nn; i++) {
+		ns = nvme_find_ns(dev, i);
+		if (ns) {
+			if (revalidate_disk(ns->disk)) {
+				nvme_ns_remove(ns);
+				nvme_free_namespace(ns);
+			}
+		} else
+			nvme_alloc_ns(dev, i);
+	}
+	list_for_each_entry_safe(ns, next, &dev->namespaces, list) {
+		if (ns->ns_id > nn) {
+			nvme_ns_remove(ns);
+			nvme_free_namespace(ns);
+		}
+	}
+	list_sort(NULL, &dev->namespaces, ns_cmp);
+}
+
+static void nvme_dev_scan(struct work_struct *work)
+{
+	struct nvme_dev *dev = container_of(work, struct nvme_dev, scan_work);
+	struct nvme_id_ctrl *ctrl;
+
+	if (!dev->tagset.tags)
+		return;
+	if (nvme_identify_ctrl(dev, &ctrl))
+		return;
+	nvme_scan_namespaces(dev, le32_to_cpup(&ctrl->nn));
+	kfree(ctrl);
+}
+
 /*
  * Return: error value if an error occurred setting up the queues or calling
  * Identify Device.  0 if these succeeded, even if adding some of the
@@ -2281,26 +2333,18 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
  */
 static int nvme_dev_add(struct nvme_dev *dev)
 {
-	struct pci_dev *pdev = dev->pci_dev;
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
 	int res;
-	unsigned nn, i;
+	unsigned nn;
 	struct nvme_id_ctrl *ctrl;
-	void *mem;
-	dma_addr_t dma_addr;
 	int shift = NVME_CAP_MPSMIN(readq(&dev->bar->cap)) + 12;
 
-	mem = dma_alloc_coherent(&pdev->dev, 4096, &dma_addr, GFP_KERNEL);
-	if (!mem)
-		return -ENOMEM;
-
-	res = nvme_identify(dev, 0, 1, dma_addr);
+	res = nvme_identify_ctrl(dev, &ctrl);
 	if (res) {
-		dev_err(&pdev->dev, "Identify Controller failed (%d)\n", res);
-		dma_free_coherent(&dev->pci_dev->dev, 4096, mem, dma_addr);
+		dev_err(dev->dev, "Identify Controller failed (%d)\n", res);
 		return -EIO;
 	}
 
-	ctrl = mem;
 	nn = le32_to_cpup(&ctrl->nn);
 	dev->oncs = le16_to_cpup(&ctrl->oncs);
 	dev->abort_limit = ctrl->acl + 1;
@@ -2322,24 +2366,23 @@ static int nvme_dev_add(struct nvme_dev *dev)
 		} else
 			dev->max_hw_sectors = max_hw_sectors;
 	}
-	dma_free_coherent(&dev->pci_dev->dev, 4096, mem, dma_addr);
+	kfree(ctrl);
 
-	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 =
+	if (!dev->tagset.tags) {
+		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->dev);
+		dev->tagset.queue_depth =
 				min_t(int, dev->q_depth, BLK_MQ_MAX_DEPTH) - 1;
-	dev->tagset.cmd_size = nvme_cmd_size(dev);
-	dev->tagset.flags = BLK_MQ_F_SHOULD_MERGE;
-	dev->tagset.driver_data = dev;
-
-	if (blk_mq_alloc_tag_set(&dev->tagset))
-		return 0;
-
-	for (i = 1; i <= nn; i++)
-		nvme_alloc_ns(dev, i);
+		dev->tagset.cmd_size = nvme_cmd_size(dev);
+		dev->tagset.flags = BLK_MQ_F_SHOULD_MERGE;
+		dev->tagset.driver_data = dev;
 
+		if (blk_mq_alloc_tag_set(&dev->tagset))
+			return 0;
+	}
+	schedule_work(&dev->scan_work);
 	return 0;
 }
 
@@ -2347,7 +2390,7 @@ static int nvme_dev_map(struct nvme_dev *dev)
 {
 	u64 cap;
 	int bars, result = -ENOMEM;
-	struct pci_dev *pdev = dev->pci_dev;
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
 
 	if (pci_enable_device_mem(pdev))
 		return result;
@@ -2361,8 +2404,8 @@ static int nvme_dev_map(struct nvme_dev *dev)
 	if (pci_request_selected_regions(pdev, bars, "nvme"))
 		goto disable_pci;
 
-	if (dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64)) &&
-	    dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32)))
+	if (dma_set_mask_and_coherent(dev->dev, DMA_BIT_MASK(64)) &&
+	    dma_set_mask_and_coherent(dev->dev, DMA_BIT_MASK(32)))
 		goto disable;
 
 	dev->bar = ioremap(pci_resource_start(pdev, 0), 8192);
@@ -2403,19 +2446,21 @@ static int nvme_dev_map(struct nvme_dev *dev)
 
 static void nvme_dev_unmap(struct nvme_dev *dev)
 {
-	if (dev->pci_dev->msi_enabled)
-		pci_disable_msi(dev->pci_dev);
-	else if (dev->pci_dev->msix_enabled)
-		pci_disable_msix(dev->pci_dev);
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
+
+	if (pdev->msi_enabled)
+		pci_disable_msi(pdev);
+	else if (pdev->msix_enabled)
+		pci_disable_msix(pdev);
 
 	if (dev->bar) {
 		iounmap(dev->bar);
 		dev->bar = NULL;
-		pci_release_regions(dev->pci_dev);
+		pci_release_regions(pdev);
 	}
 
-	if (pci_is_enabled(dev->pci_dev))
-		pci_disable_device(dev->pci_dev);
+	if (pci_is_enabled(pdev))
+		pci_disable_device(pdev);
 }
 
 struct nvme_delq_ctx {
@@ -2534,7 +2579,7 @@ static void nvme_disable_io_queues(struct nvme_dev *dev)
 					&worker, "nvme%d", dev->instance);
 
 	if (IS_ERR(kworker_task)) {
-		dev_err(&dev->pci_dev->dev,
+		dev_err(dev->dev,
 			"Failed to create queue del task\n");
 		for (i = dev->queue_count - 1; i > 0; i--)
 			nvme_disable_queue(dev, i);
@@ -2585,9 +2630,9 @@ static void nvme_freeze_queues(struct nvme_dev *dev)
 	list_for_each_entry(ns, &dev->namespaces, list) {
 		blk_mq_freeze_queue_start(ns->queue);
 
-		spin_lock(ns->queue->queue_lock);
+		spin_lock_irq(ns->queue->queue_lock);
 		queue_flag_set(QUEUE_FLAG_STOPPED, ns->queue);
-		spin_unlock(ns->queue->queue_lock);
+		spin_unlock_irq(ns->queue->queue_lock);
 
 		blk_mq_cancel_requeue_work(ns->queue);
 		blk_mq_stop_hw_queues(ns->queue);
@@ -2637,29 +2682,19 @@ static void nvme_dev_remove(struct nvme_dev *dev)
 {
 	struct nvme_ns *ns;
 
-	list_for_each_entry(ns, &dev->namespaces, list) {
-		if (ns->disk->flags & GENHD_FL_UP) {
-			if (blk_get_integrity(ns->disk))
-				blk_integrity_unregister(ns->disk);
-			del_gendisk(ns->disk);
-		}
-		if (!blk_queue_dying(ns->queue)) {
-			blk_mq_abort_requeue_list(ns->queue);
-			blk_cleanup_queue(ns->queue);
-		}
-	}
+	list_for_each_entry(ns, &dev->namespaces, list)
+		nvme_ns_remove(ns);
 }
 
 static int nvme_setup_prp_pools(struct nvme_dev *dev)
 {
-	struct device *dmadev = &dev->pci_dev->dev;
-	dev->prp_page_pool = dma_pool_create("prp list page", dmadev,
+	dev->prp_page_pool = dma_pool_create("prp list page", dev->dev,
 						PAGE_SIZE, PAGE_SIZE, 0);
 	if (!dev->prp_page_pool)
 		return -ENOMEM;
 
 	/* Optimisation for I/Os between 4k and 128k */
-	dev->prp_small_pool = dma_pool_create("prp list 256", dmadev,
+	dev->prp_small_pool = dma_pool_create("prp list 256", dev->dev,
 						256, 256, 0);
 	if (!dev->prp_small_pool) {
 		dma_pool_destroy(dev->prp_page_pool);
@@ -2707,28 +2742,22 @@ static void nvme_free_namespaces(struct nvme_dev *dev)
 {
 	struct nvme_ns *ns, *next;
 
-	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);
-	}
+	list_for_each_entry_safe(ns, next, &dev->namespaces, list)
+		nvme_free_namespace(ns);
 }
 
 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);
+	put_device(dev->dev);
 	put_device(dev->device);
 	nvme_free_namespaces(dev);
 	nvme_release_instance(dev);
-	blk_mq_free_tag_set(&dev->tagset);
-	blk_put_queue(dev->admin_q);
+	if (dev->tagset.tags)
+		blk_mq_free_tag_set(&dev->tagset);
+	if (dev->admin_q)
+		blk_put_queue(dev->admin_q);
 	kfree(dev->queues);
 	kfree(dev->entry);
 	kfree(dev);
@@ -2779,6 +2808,9 @@ static long nvme_dev_ioctl(struct file *f, unsigned int cmd, unsigned long arg)
 			return -ENOTTY;
 		ns = list_first_entry(&dev->namespaces, struct nvme_ns, list);
 		return nvme_user_cmd(dev, ns, (void __user *)arg);
+	case NVME_IOCTL_RESET:
+		dev_warn(dev->dev, "resetting controller\n");
+		return nvme_reset(dev);
 	default:
 		return -ENOTTY;
 	}
@@ -2800,11 +2832,11 @@ static void nvme_set_irq_hints(struct nvme_dev *dev)
 	for (i = 0; i < dev->online_queues; i++) {
 		nvmeq = dev->queues[i];
 
-		if (!nvmeq->hctx)
+		if (!nvmeq->tags || !(*nvmeq->tags))
 			continue;
 
 		irq_set_affinity_hint(dev->entry[nvmeq->cq_vector].vector,
-							nvmeq->hctx->cpumask);
+					blk_mq_tags_cpumask(*nvmeq->tags));
 	}
 }
 
@@ -2856,6 +2888,9 @@ static int nvme_dev_start(struct nvme_dev *dev)
 
  free_tags:
 	nvme_dev_remove_admin(dev);
+	blk_put_queue(dev->admin_q);
+	dev->admin_q = NULL;
+	dev->queues[0]->tags = NULL;
  disable:
 	nvme_disable_queue(dev, 0);
 	nvme_dev_list_remove(dev);
@@ -2867,7 +2902,7 @@ static int nvme_dev_start(struct nvme_dev *dev)
 static int nvme_remove_dead_ctrl(void *arg)
 {
 	struct nvme_dev *dev = (struct nvme_dev *)arg;
-	struct pci_dev *pdev = dev->pci_dev;
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
 
 	if (pci_get_drvdata(pdev))
 		pci_stop_and_remove_bus_device_locked(pdev);
@@ -2897,24 +2932,43 @@ static int nvme_dev_resume(struct nvme_dev *dev)
 		spin_unlock(&dev_list_lock);
 	} else {
 		nvme_unfreeze_queues(dev);
+		nvme_dev_add(dev);
 		nvme_set_irq_hints(dev);
 	}
 	return 0;
 }
 
+static void nvme_dead_ctrl(struct nvme_dev *dev)
+{
+	dev_warn(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))) {
+		dev_err(dev->dev,
+			"Failed to start controller remove task\n");
+		kref_put(&dev->kref, nvme_free_dev);
+	}
+}
+
 static void nvme_dev_reset(struct nvme_dev *dev)
 {
+	bool in_probe = work_busy(&dev->probe_work);
+
 	nvme_dev_shutdown(dev);
-	if (nvme_dev_resume(dev)) {
-		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))) {
-			dev_err(&dev->pci_dev->dev,
-				"Failed to start controller remove task\n");
-			kref_put(&dev->kref, nvme_free_dev);
-		}
+
+	/* Synchronize with device probe so that work will see failure status
+	 * and exit gracefully without trying to schedule another reset */
+	flush_work(&dev->probe_work);
+
+	/* Fail this device if reset occured during probe to avoid
+	 * infinite initialization loops. */
+	if (in_probe) {
+		nvme_dead_ctrl(dev);
+		return;
 	}
+	/* Schedule device resume asynchronously so the reset work is available
+	 * to cleanup errors that may occur during reinitialization */
+	schedule_work(&dev->probe_work);
 }
 
 static void nvme_reset_failed_dev(struct work_struct *ws)
@@ -2929,6 +2983,45 @@ static void nvme_reset_workfn(struct work_struct *work)
 	dev->reset_workfn(work);
 }
 
+static int nvme_reset(struct nvme_dev *dev)
+{
+	int ret = -EBUSY;
+
+	if (!dev->admin_q || blk_queue_dying(dev->admin_q))
+		return -ENODEV;
+
+	spin_lock(&dev_list_lock);
+	if (!work_pending(&dev->reset_work)) {
+		dev->reset_workfn = nvme_reset_failed_dev;
+		queue_work(nvme_workq, &dev->reset_work);
+		ret = 0;
+	}
+	spin_unlock(&dev_list_lock);
+
+	if (!ret) {
+		flush_work(&dev->reset_work);
+		flush_work(&dev->probe_work);
+		return 0;
+	}
+
+	return ret;
+}
+
+static ssize_t nvme_sysfs_reset(struct device *dev,
+				struct device_attribute *attr, const char *buf,
+				size_t count)
+{
+	struct nvme_dev *ndev = dev_get_drvdata(dev);
+	int ret;
+
+	ret = nvme_reset(ndev);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+static DEVICE_ATTR(reset_controller, S_IWUSR, NULL, nvme_sysfs_reset);
+
 static void nvme_async_probe(struct work_struct *work);
 static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
@@ -2954,7 +3047,7 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	INIT_LIST_HEAD(&dev->namespaces);
 	dev->reset_workfn = nvme_reset_failed_dev;
 	INIT_WORK(&dev->reset_work, nvme_reset_workfn);
-	dev->pci_dev = pci_dev_get(pdev);
+	dev->dev = get_device(&pdev->dev);
 	pci_set_drvdata(pdev, dev);
 	result = nvme_set_instance(dev);
 	if (result)
@@ -2973,18 +3066,27 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto release_pools;
 	}
 	get_device(dev->device);
+	dev_set_drvdata(dev->device, dev);
+
+	result = device_create_file(dev->device, &dev_attr_reset_controller);
+	if (result)
+		goto put_dev;
 
 	INIT_LIST_HEAD(&dev->node);
+	INIT_WORK(&dev->scan_work, nvme_dev_scan);
 	INIT_WORK(&dev->probe_work, nvme_async_probe);
 	schedule_work(&dev->probe_work);
 	return 0;
 
+ put_dev:
+	device_destroy(nvme_class, MKDEV(nvme_char_major, dev->instance));
+	put_device(dev->device);
  release_pools:
 	nvme_release_prp_pools(dev);
  release:
 	nvme_release_instance(dev);
  put_pci:
-	pci_dev_put(dev->pci_dev);
+	put_device(dev->dev);
  free:
 	kfree(dev->queues);
 	kfree(dev->entry);
@@ -2995,24 +3097,9 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 static void nvme_async_probe(struct work_struct *work)
 {
 	struct nvme_dev *dev = container_of(work, struct nvme_dev, probe_work);
-	int result;
-
-	result = nvme_dev_start(dev);
-	if (result)
-		goto reset;
 
-	if (dev->online_queues > 1)
-		result = nvme_dev_add(dev);
-	if (result)
-		goto reset;
-
-	nvme_set_irq_hints(dev);
-	return;
- reset:
-	if (!work_busy(&dev->reset_work)) {
-		dev->reset_workfn = nvme_reset_failed_dev;
-		queue_work(nvme_workq, &dev->reset_work);
-	}
+	if (nvme_dev_resume(dev) && !work_busy(&dev->reset_work))
+		nvme_dead_ctrl(dev);
 }
 
 static void nvme_reset_notify(struct pci_dev *pdev, bool prepare)
@@ -3042,8 +3129,10 @@ static void nvme_remove(struct pci_dev *pdev)
 	pci_set_drvdata(pdev, NULL);
 	flush_work(&dev->probe_work);
 	flush_work(&dev->reset_work);
-	nvme_dev_shutdown(dev);
+	flush_work(&dev->scan_work);
+	device_remove_file(dev->device, &dev_attr_reset_controller);
 	nvme_dev_remove(dev);
+	nvme_dev_shutdown(dev);
 	nvme_dev_remove_admin(dev);
 	device_destroy(nvme_class, MKDEV(nvme_char_major, dev->instance));
 	nvme_free_queues(dev, 0);
diff --git a/drivers/block/nvme-scsi.c b/drivers/block/nvme-scsi.c
index 44f2514fb775..e5a63f06fb0f 100644
--- a/drivers/block/nvme-scsi.c
+++ b/drivers/block/nvme-scsi.c
@@ -41,15 +41,13 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/types.h>
+#include <asm/unaligned.h>
 #include <scsi/sg.h>
 #include <scsi/scsi.h>
 
 
 static int sg_version_num = 30534;	/* 2 digits for each component */
 
-#define SNTI_TRANSLATION_SUCCESS			0
-#define SNTI_INTERNAL_ERROR				1
-
 /* VPD Page Codes */
 #define VPD_SUPPORTED_PAGES				0x00
 #define VPD_SERIAL_NUMBER				0x80
@@ -58,49 +56,14 @@ static int sg_version_num = 30534;	/* 2 digits for each component */
 #define VPD_BLOCK_LIMITS				0xB0
 #define VPD_BLOCK_DEV_CHARACTERISTICS			0xB1
 
-/* CDB offsets */
-#define REPORT_LUNS_CDB_ALLOC_LENGTH_OFFSET		6
-#define REPORT_LUNS_SR_OFFSET				2
-#define READ_CAP_16_CDB_ALLOC_LENGTH_OFFSET		10
-#define REQUEST_SENSE_CDB_ALLOC_LENGTH_OFFSET		4
-#define REQUEST_SENSE_DESC_OFFSET			1
-#define REQUEST_SENSE_DESC_MASK				0x01
-#define DESCRIPTOR_FORMAT_SENSE_DATA_TYPE		1
-#define INQUIRY_EVPD_BYTE_OFFSET			1
-#define INQUIRY_PAGE_CODE_BYTE_OFFSET			2
-#define INQUIRY_EVPD_BIT_MASK				1
-#define INQUIRY_CDB_ALLOCATION_LENGTH_OFFSET		3
-#define START_STOP_UNIT_CDB_IMMED_OFFSET		1
-#define START_STOP_UNIT_CDB_IMMED_MASK			0x1
-#define START_STOP_UNIT_CDB_POWER_COND_MOD_OFFSET	3
-#define START_STOP_UNIT_CDB_POWER_COND_MOD_MASK		0xF
-#define START_STOP_UNIT_CDB_POWER_COND_OFFSET		4
-#define START_STOP_UNIT_CDB_POWER_COND_MASK		0xF0
-#define START_STOP_UNIT_CDB_NO_FLUSH_OFFSET		4
-#define START_STOP_UNIT_CDB_NO_FLUSH_MASK		0x4
-#define START_STOP_UNIT_CDB_START_OFFSET		4
-#define START_STOP_UNIT_CDB_START_MASK			0x1
-#define WRITE_BUFFER_CDB_MODE_OFFSET			1
-#define WRITE_BUFFER_CDB_MODE_MASK			0x1F
-#define WRITE_BUFFER_CDB_BUFFER_ID_OFFSET		2
-#define WRITE_BUFFER_CDB_BUFFER_OFFSET_OFFSET		3
-#define WRITE_BUFFER_CDB_PARM_LIST_LENGTH_OFFSET	6
-#define FORMAT_UNIT_CDB_FORMAT_PROT_INFO_OFFSET		1
-#define FORMAT_UNIT_CDB_FORMAT_PROT_INFO_MASK		0xC0
-#define FORMAT_UNIT_CDB_FORMAT_PROT_INFO_SHIFT		6
-#define FORMAT_UNIT_CDB_LONG_LIST_OFFSET		1
-#define FORMAT_UNIT_CDB_LONG_LIST_MASK			0x20
-#define FORMAT_UNIT_CDB_FORMAT_DATA_OFFSET		1
-#define FORMAT_UNIT_CDB_FORMAT_DATA_MASK		0x10
+/* format unit paramter list offsets */
 #define FORMAT_UNIT_SHORT_PARM_LIST_LEN			4
 #define FORMAT_UNIT_LONG_PARM_LIST_LEN			8
 #define FORMAT_UNIT_PROT_INT_OFFSET			3
 #define FORMAT_UNIT_PROT_FIELD_USAGE_OFFSET		0
 #define FORMAT_UNIT_PROT_FIELD_USAGE_MASK		0x07
-#define UNMAP_CDB_PARAM_LIST_LENGTH_OFFSET		7
 
 /* Misc. defines */
-#define NIBBLE_SHIFT					4
 #define FIXED_SENSE_DATA				0x70
 #define DESC_FORMAT_SENSE_DATA				0x72
 #define FIXED_SENSE_DATA_ADD_LENGTH			10
@@ -144,27 +107,6 @@ static int sg_version_num = 30534;	/* 2 digits for each component */
 #define EXTENDED_INQUIRY_DATA_PAGE_LENGTH		0x3C
 #define RESERVED_FIELD					0
 
-/* SCSI READ/WRITE Defines */
-#define IO_CDB_WP_MASK					0xE0
-#define IO_CDB_WP_SHIFT					5
-#define IO_CDB_FUA_MASK					0x8
-#define IO_6_CDB_LBA_OFFSET				0
-#define IO_6_CDB_LBA_MASK				0x001FFFFF
-#define IO_6_CDB_TX_LEN_OFFSET				4
-#define IO_6_DEFAULT_TX_LEN				256
-#define IO_10_CDB_LBA_OFFSET				2
-#define IO_10_CDB_TX_LEN_OFFSET				7
-#define IO_10_CDB_WP_OFFSET				1
-#define IO_10_CDB_FUA_OFFSET				1
-#define IO_12_CDB_LBA_OFFSET				2
-#define IO_12_CDB_TX_LEN_OFFSET				6
-#define IO_12_CDB_WP_OFFSET				1
-#define IO_12_CDB_FUA_OFFSET				1
-#define IO_16_CDB_FUA_OFFSET				1
-#define IO_16_CDB_WP_OFFSET				1
-#define IO_16_CDB_LBA_OFFSET				2
-#define IO_16_CDB_TX_LEN_OFFSET				10
-
 /* Mode Sense/Select defines */
 #define MODE_PAGE_INFO_EXCEP				0x1C
 #define MODE_PAGE_CACHING				0x08
@@ -179,23 +121,14 @@ static int sg_version_num = 30534;	/* 2 digits for each component */
 #define MODE_PAGE_INF_EXC_LEN				0x0C
 #define MODE_PAGE_ALL_LEN				0x54
 #define MODE_SENSE6_MPH_SIZE				4
-#define MODE_SENSE6_ALLOC_LEN_OFFSET			4
-#define MODE_SENSE_PAGE_CONTROL_OFFSET			2
 #define MODE_SENSE_PAGE_CONTROL_MASK			0xC0
 #define MODE_SENSE_PAGE_CODE_OFFSET			2
 #define MODE_SENSE_PAGE_CODE_MASK			0x3F
-#define MODE_SENSE_LLBAA_OFFSET				1
 #define MODE_SENSE_LLBAA_MASK				0x10
 #define MODE_SENSE_LLBAA_SHIFT				4
-#define MODE_SENSE_DBD_OFFSET				1
 #define MODE_SENSE_DBD_MASK				8
 #define MODE_SENSE_DBD_SHIFT				3
 #define MODE_SENSE10_MPH_SIZE				8
-#define MODE_SENSE10_ALLOC_LEN_OFFSET			7
-#define MODE_SELECT_CDB_PAGE_FORMAT_OFFSET		1
-#define MODE_SELECT_CDB_SAVE_PAGES_OFFSET		1
-#define MODE_SELECT_6_CDB_PARAM_LIST_LENGTH_OFFSET	4
-#define MODE_SELECT_10_CDB_PARAM_LIST_LENGTH_OFFSET	7
 #define MODE_SELECT_CDB_PAGE_FORMAT_MASK		0x10
 #define MODE_SELECT_CDB_SAVE_PAGES_MASK			0x1
 #define MODE_SELECT_6_BD_OFFSET				3
@@ -221,14 +154,11 @@ static int sg_version_num = 30534;	/* 2 digits for each component */
 #define LOG_PAGE_SUPPORTED_LOG_PAGES_LENGTH		0x07
 #define LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE		0x2F
 #define LOG_PAGE_TEMPERATURE_PAGE			0x0D
-#define LOG_SENSE_CDB_SP_OFFSET				1
 #define LOG_SENSE_CDB_SP_NOT_ENABLED			0
-#define LOG_SENSE_CDB_PC_OFFSET				2
 #define LOG_SENSE_CDB_PC_MASK				0xC0
 #define LOG_SENSE_CDB_PC_SHIFT				6
 #define LOG_SENSE_CDB_PC_CUMULATIVE_VALUES		1
 #define LOG_SENSE_CDB_PAGE_CODE_MASK			0x3F
-#define LOG_SENSE_CDB_ALLOC_LENGTH_OFFSET		7
 #define REMAINING_INFO_EXCP_PAGE_LENGTH			0x8
 #define LOG_INFO_EXCP_PAGE_LENGTH			0xC
 #define REMAINING_TEMP_PAGE_LENGTH			0xC
@@ -278,77 +208,11 @@ static int sg_version_num = 30534;	/* 2 digits for each component */
 #define SCSI_ASCQ_POWER_LOSS_EXPECTED			0x08
 #define SCSI_ASCQ_INVALID_LUN_ID			0x09
 
-/**
- * DEVICE_SPECIFIC_PARAMETER in mode parameter header (see sbc2r16) to
- * enable DPOFUA support type 0x10 value.
- */
-#define DEVICE_SPECIFIC_PARAMETER			0
-#define VPD_ID_DESCRIPTOR_LENGTH sizeof(VPD_IDENTIFICATION_DESCRIPTOR)
-
-/* MACROs to extract information from CDBs */
-
-#define GET_OPCODE(cdb)		cdb[0]
-
-#define GET_U8_FROM_CDB(cdb, index) (cdb[index] << 0)
-
-#define GET_U16_FROM_CDB(cdb, index) ((cdb[index] << 8) | (cdb[index + 1] << 0))
-
-#define GET_U24_FROM_CDB(cdb, index) ((cdb[index] << 16) | \
-(cdb[index + 1] <<  8) | \
-(cdb[index + 2] <<  0))
-
-#define GET_U32_FROM_CDB(cdb, index) ((cdb[index] << 24) | \
-(cdb[index + 1] << 16) | \
-(cdb[index + 2] <<  8) | \
-(cdb[index + 3] <<  0))
-
-#define GET_U64_FROM_CDB(cdb, index) ((((u64)cdb[index]) << 56) | \
-(((u64)cdb[index + 1]) << 48) | \
-(((u64)cdb[index + 2]) << 40) | \
-(((u64)cdb[index + 3]) << 32) | \
-(((u64)cdb[index + 4]) << 24) | \
-(((u64)cdb[index + 5]) << 16) | \
-(((u64)cdb[index + 6]) <<  8) | \
-(((u64)cdb[index + 7]) <<  0))
-
-/* Inquiry Helper Macros */
-#define GET_INQ_EVPD_BIT(cdb) \
-((GET_U8_FROM_CDB(cdb, INQUIRY_EVPD_BYTE_OFFSET) &		\
-INQUIRY_EVPD_BIT_MASK) ? 1 : 0)
-
-#define GET_INQ_PAGE_CODE(cdb)					\
-(GET_U8_FROM_CDB(cdb, INQUIRY_PAGE_CODE_BYTE_OFFSET))
-
-#define GET_INQ_ALLOC_LENGTH(cdb)				\
-(GET_U16_FROM_CDB(cdb, INQUIRY_CDB_ALLOCATION_LENGTH_OFFSET))
-
-/* Report LUNs Helper Macros */
-#define GET_REPORT_LUNS_ALLOC_LENGTH(cdb)			\
-(GET_U32_FROM_CDB(cdb, REPORT_LUNS_CDB_ALLOC_LENGTH_OFFSET))
-
-/* Read Capacity Helper Macros */
-#define GET_READ_CAP_16_ALLOC_LENGTH(cdb)			\
-(GET_U32_FROM_CDB(cdb, READ_CAP_16_CDB_ALLOC_LENGTH_OFFSET))
-
-#define IS_READ_CAP_16(cdb)					\
-((cdb[0] == SERVICE_ACTION_IN_16 && cdb[1] == SAI_READ_CAPACITY_16) ? 1 : 0)
-
-/* Request Sense Helper Macros */
-#define GET_REQUEST_SENSE_ALLOC_LENGTH(cdb)			\
-(GET_U8_FROM_CDB(cdb, REQUEST_SENSE_CDB_ALLOC_LENGTH_OFFSET))
-
-/* Mode Sense Helper Macros */
-#define GET_MODE_SENSE_DBD(cdb)					\
-((GET_U8_FROM_CDB(cdb, MODE_SENSE_DBD_OFFSET) & MODE_SENSE_DBD_MASK) >>	\
-MODE_SENSE_DBD_SHIFT)
-
-#define GET_MODE_SENSE_LLBAA(cdb)				\
-((GET_U8_FROM_CDB(cdb, MODE_SENSE_LLBAA_OFFSET) &		\
-MODE_SENSE_LLBAA_MASK) >> MODE_SENSE_LLBAA_SHIFT)
-
-#define GET_MODE_SENSE_MPH_SIZE(cdb10)				\
-(cdb10 ? MODE_SENSE10_MPH_SIZE : MODE_SENSE6_MPH_SIZE)
-
+/* copied from drivers/usb/gadget/function/storage_common.h */
+static inline u32 get_unaligned_be24(u8 *buf)
+{
+	return 0xffffff & (u32) get_unaligned_be32(buf - 1);
+}
 
 /* Struct to gather data that needs to be extracted from a SCSI CDB.
    Not conforming to any particular CDB variant, but compatible with all. */
@@ -369,8 +233,6 @@ struct nvme_trans_io_cdb {
 static int nvme_trans_copy_to_user(struct sg_io_hdr *hdr, void *from,
 								unsigned long n)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
-	unsigned long not_copied;
 	int i;
 	void *index = from;
 	size_t remaining = n;
@@ -380,29 +242,25 @@ static int nvme_trans_copy_to_user(struct sg_io_hdr *hdr, void *from,
 		struct sg_iovec sgl;
 
 		for (i = 0; i < hdr->iovec_count; i++) {
-			not_copied = copy_from_user(&sgl, hdr->dxferp +
+			if (copy_from_user(&sgl, hdr->dxferp +
 						i * sizeof(struct sg_iovec),
-						sizeof(struct sg_iovec));
-			if (not_copied)
+						sizeof(struct sg_iovec)))
 				return -EFAULT;
 			xfer_len = min(remaining, sgl.iov_len);
-			not_copied = copy_to_user(sgl.iov_base, index,
-								xfer_len);
-			if (not_copied) {
-				res = -EFAULT;
-				break;
-			}
+			if (copy_to_user(sgl.iov_base, index, xfer_len))
+				return -EFAULT;
+
 			index += xfer_len;
 			remaining -= xfer_len;
 			if (remaining == 0)
 				break;
 		}
-		return res;
+		return 0;
 	}
-	not_copied = copy_to_user(hdr->dxferp, from, n);
-	if (not_copied)
-		res = -EFAULT;
-	return res;
+
+	if (copy_to_user(hdr->dxferp, from, n))
+		return -EFAULT;
+	return 0;
 }
 
 /* Copy data from userspace memory */
@@ -410,8 +268,6 @@ static int nvme_trans_copy_to_user(struct sg_io_hdr *hdr, void *from,
 static int nvme_trans_copy_from_user(struct sg_io_hdr *hdr, void *to,
 								unsigned long n)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
-	unsigned long not_copied;
 	int i;
 	void *index = to;
 	size_t remaining = n;
@@ -421,30 +277,24 @@ static int nvme_trans_copy_from_user(struct sg_io_hdr *hdr, void *to,
 		struct sg_iovec sgl;
 
 		for (i = 0; i < hdr->iovec_count; i++) {
-			not_copied = copy_from_user(&sgl, hdr->dxferp +
+			if (copy_from_user(&sgl, hdr->dxferp +
 						i * sizeof(struct sg_iovec),
-						sizeof(struct sg_iovec));
-			if (not_copied)
+						sizeof(struct sg_iovec)))
 				return -EFAULT;
 			xfer_len = min(remaining, sgl.iov_len);
-			not_copied = copy_from_user(index, sgl.iov_base,
-								xfer_len);
-			if (not_copied) {
-				res = -EFAULT;
-				break;
-			}
+			if (copy_from_user(index, sgl.iov_base, xfer_len))
+				return -EFAULT;
 			index += xfer_len;
 			remaining -= xfer_len;
 			if (remaining == 0)
 				break;
 		}
-		return res;
+		return 0;
 	}
 
-	not_copied = copy_from_user(to, hdr->dxferp, n);
-	if (not_copied)
-		res = -EFAULT;
-	return res;
+	if (copy_from_user(to, hdr->dxferp, n))
+		return -EFAULT;
+	return 0;
 }
 
 /* Status/Sense Buffer Writeback */
@@ -452,7 +302,6 @@ static int nvme_trans_copy_from_user(struct sg_io_hdr *hdr, void *to,
 static int nvme_trans_completion(struct sg_io_hdr *hdr, u8 status, u8 sense_key,
 				 u8 asc, u8 ascq)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
 	u8 xfer_len;
 	u8 resp[DESC_FMT_SENSE_DATA_SIZE];
 
@@ -477,25 +326,29 @@ static int nvme_trans_completion(struct sg_io_hdr *hdr, u8 status, u8 sense_key,
 		xfer_len = min_t(u8, hdr->mx_sb_len, DESC_FMT_SENSE_DATA_SIZE);
 		hdr->sb_len_wr = xfer_len;
 		if (copy_to_user(hdr->sbp, resp, xfer_len) > 0)
-			res = -EFAULT;
+			return -EFAULT;
 	}
 
-	return res;
+	return 0;
 }
 
+/*
+ * Take a status code from a lowlevel routine, and if it was a positive NVMe
+ * error code update the sense data based on it.  In either case the passed
+ * in value is returned again, unless an -EFAULT from copy_to_user overrides
+ * it.
+ */
 static int nvme_trans_status_code(struct sg_io_hdr *hdr, int nvme_sc)
 {
 	u8 status, sense_key, asc, ascq;
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 
 	/* For non-nvme (Linux) errors, simply return the error code */
 	if (nvme_sc < 0)
 		return nvme_sc;
 
 	/* Mask DNR, More, and reserved fields */
-	nvme_sc &= 0x7FF;
-
-	switch (nvme_sc) {
+	switch (nvme_sc & 0x7FF) {
 	/* Generic Command Status */
 	case NVME_SC_SUCCESS:
 		status = SAM_STAT_GOOD;
@@ -662,8 +515,7 @@ static int nvme_trans_status_code(struct sg_io_hdr *hdr, int nvme_sc)
 	}
 
 	res = nvme_trans_completion(hdr, status, sense_key, asc, ascq);
-
-	return res;
+	return res ? res : nvme_sc;
 }
 
 /* INQUIRY Helper Functions */
@@ -673,10 +525,8 @@ static int nvme_trans_standard_inquiry_page(struct nvme_ns *ns,
 					int alloc_len)
 {
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
 	struct nvme_id_ns *id_ns;
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int nvme_sc;
 	int xfer_len;
 	u8 resp_data_format = 0x02;
@@ -684,31 +534,17 @@ static int nvme_trans_standard_inquiry_page(struct nvme_ns *ns,
 	u8 cmdque = 0x01 << 1;
 	u8 fw_offset = sizeof(dev->firmware_rev);
 
-	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
-				&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out_dma;
-	}
-
 	/* nvme ns identify - use DPS value for PROTECT field */
-	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
-	/*
-	 * If nvme_sc was -ve, res will be -ve here.
-	 * If nvme_sc was +ve, the status would bace been translated, and res
-	 *  can only be 0 or -ve.
-	 *    - If 0 && nvme_sc > 0, then go into next if where res gets nvme_sc
-	 *    - If -ve, return because its a Linux error.
-	 */
 	if (res)
-		goto out_free;
-	if (nvme_sc) {
-		res = nvme_sc;
-		goto out_free;
-	}
-	id_ns = mem;
-	(id_ns->dps) ? (protect = 0x01) : (protect = 0);
+		return res;
+
+	if (id_ns->dps)
+		protect = 0x01;
+	else
+		protect = 0;
+	kfree(id_ns);
 
 	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
 	inq_response[2] = VERSION_SPC_4;
@@ -725,20 +561,13 @@ static int nvme_trans_standard_inquiry_page(struct nvme_ns *ns,
 	strncpy(&inq_response[32], dev->firmware_rev + fw_offset, 4);
 
 	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
-	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-
- out_free:
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
-			  dma_addr);
- out_dma:
-	return res;
+	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
 }
 
 static int nvme_trans_supported_vpd_pages(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr, u8 *inq_response,
 					int alloc_len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
 	int xfer_len;
 
 	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
@@ -752,9 +581,7 @@ static int nvme_trans_supported_vpd_pages(struct nvme_ns *ns,
 	inq_response[9] = INQ_BDEV_LIMITS_PAGE;
 
 	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
-	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-
-	return res;
+	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
 }
 
 static int nvme_trans_unit_serial_page(struct nvme_ns *ns,
@@ -762,7 +589,6 @@ static int nvme_trans_unit_serial_page(struct nvme_ns *ns,
 					int alloc_len)
 {
 	struct nvme_dev *dev = ns->dev;
-	int res = SNTI_TRANSLATION_SUCCESS;
 	int xfer_len;
 
 	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
@@ -771,53 +597,42 @@ static int nvme_trans_unit_serial_page(struct nvme_ns *ns,
 	strncpy(&inq_response[4], dev->serial, INQ_SERIAL_NUMBER_LENGTH);
 
 	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
-	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-
-	return res;
+	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
 }
 
 static int nvme_trans_device_id_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					u8 *inq_response, int alloc_len)
 {
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int nvme_sc;
 	int xfer_len;
 	__be32 tmp_id = cpu_to_be32(ns->ns_id);
 
-	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
-					&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out_dma;
-	}
-
 	memset(inq_response, 0, alloc_len);
 	inq_response[1] = INQ_DEVICE_IDENTIFICATION_PAGE;    /* Page Code */
 	if (readl(&dev->bar->vs) >= NVME_VS(1, 1)) {
-		struct nvme_id_ns *id_ns = mem;
-		void *eui = id_ns->eui64;
-		int len = sizeof(id_ns->eui64);
+		struct nvme_id_ns *id_ns;
+		void *eui;
+		int len;
 
-		nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+		nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
 		res = nvme_trans_status_code(hdr, nvme_sc);
 		if (res)
-			goto out_free;
-		if (nvme_sc) {
-			res = nvme_sc;
-			goto out_free;
-		}
+			return res;
 
+		eui = id_ns->eui64;
+		len = sizeof(id_ns->eui64);
 		if (readl(&dev->bar->vs) >= NVME_VS(1, 2)) {
 			if (bitmap_empty(eui, len * 8)) {
 				eui = id_ns->nguid;
 				len = sizeof(id_ns->nguid);
 			}
 		}
-		if (bitmap_empty(eui, len * 8))
+		if (bitmap_empty(eui, len * 8)) {
+			kfree(id_ns);
 			goto scsi_string;
+		}
 
 		inq_response[3] = 4 + len; /* Page Length */
 		/* Designation Descriptor start */
@@ -826,14 +641,14 @@ static int nvme_trans_device_id_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		inq_response[6] = 0x00;    /* Rsvd */
 		inq_response[7] = len;     /* Designator Length */
 		memcpy(&inq_response[8], eui, len);
+		kfree(id_ns);
 	} else {
  scsi_string:
 		if (alloc_len < 72) {
-			res = nvme_trans_completion(hdr,
+			return nvme_trans_completion(hdr,
 					SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-			goto out_free;
 		}
 		inq_response[3] = 0x48;    /* Page Length */
 		/* Designation Descriptor start */
@@ -842,30 +657,22 @@ static int nvme_trans_device_id_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		inq_response[6] = 0x00;    /* Rsvd */
 		inq_response[7] = 0x44;    /* Designator Length */
 
-		sprintf(&inq_response[8], "%04x", dev->pci_dev->vendor);
+		sprintf(&inq_response[8], "%04x", to_pci_dev(dev->dev)->vendor);
 		memcpy(&inq_response[12], dev->model, sizeof(dev->model));
 		sprintf(&inq_response[52], "%04x", tmp_id);
 		memcpy(&inq_response[56], dev->serial, sizeof(dev->serial));
 	}
 	xfer_len = alloc_len;
-	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
-
- out_free:
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
-			  dma_addr);
- out_dma:
-	return res;
+	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
 }
 
 static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					int alloc_len)
 {
 	u8 *inq_response;
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int nvme_sc;
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
 	struct nvme_id_ctrl *id_ctrl;
 	struct nvme_id_ns *id_ns;
 	int xfer_len;
@@ -878,45 +685,32 @@ static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	u8 luiclr = 0x01;
 
 	inq_response = kmalloc(EXTENDED_INQUIRY_DATA_PAGE_LENGTH, GFP_KERNEL);
-	if (inq_response == NULL) {
-		res = -ENOMEM;
-		goto out_mem;
-	}
-
-	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
-							&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out_dma;
-	}
+	if (inq_response == NULL)
+		return -ENOMEM;
 
-	/* nvme ns identify */
-	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
-		goto out_free;
-	if (nvme_sc) {
-		res = nvme_sc;
-		goto out_free;
-	}
-	id_ns = mem;
-	spt = spt_lut[(id_ns->dpc) & 0x07] << 3;
-	(id_ns->dps) ? (protect = 0x01) : (protect = 0);
+		goto out_free_inq;
+
+	spt = spt_lut[id_ns->dpc & 0x07] << 3;
+	if (id_ns->dps)
+		protect = 0x01;
+	else
+		protect = 0;
+	kfree(id_ns);
+
 	grd_chk = protect << 2;
 	app_chk = protect << 1;
 	ref_chk = protect;
 
-	/* nvme controller identify */
-	nvme_sc = nvme_identify(dev, 0, 1, dma_addr);
+	nvme_sc = nvme_identify_ctrl(dev, &id_ctrl);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
-		goto out_free;
-	if (nvme_sc) {
-		res = nvme_sc;
-		goto out_free;
-	}
-	id_ctrl = mem;
+		goto out_free_inq;
+
 	v_sup = id_ctrl->vwc;
+	kfree(id_ctrl);
 
 	memset(inq_response, 0, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
 	inq_response[1] = INQ_EXTENDED_INQUIRY_DATA_PAGE;    /* Page Code */
@@ -932,12 +726,8 @@ static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	xfer_len = min(alloc_len, EXTENDED_INQUIRY_DATA_PAGE_LENGTH);
 	res = nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
 
- out_free:
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
-			  dma_addr);
- out_dma:
+ out_free_inq:
 	kfree(inq_response);
- out_mem:
 	return res;
 }
 
@@ -965,7 +755,7 @@ static int nvme_trans_bdev_char_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					int alloc_len)
 {
 	u8 *inq_response;
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int xfer_len;
 
 	inq_response = kzalloc(EXTENDED_INQUIRY_DATA_PAGE_LENGTH, GFP_KERNEL);
@@ -994,7 +784,7 @@ static int nvme_trans_bdev_char_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 static int nvme_trans_log_supp_pages(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					int alloc_len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int xfer_len;
 	u8 *log_response;
 
@@ -1022,47 +812,30 @@ static int nvme_trans_log_supp_pages(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 static int nvme_trans_log_info_exceptions(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr, int alloc_len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int xfer_len;
 	u8 *log_response;
-	struct nvme_command c;
 	struct nvme_dev *dev = ns->dev;
 	struct nvme_smart_log *smart_log;
-	dma_addr_t dma_addr;
-	void *mem;
 	u8 temp_c;
 	u16 temp_k;
 
 	log_response = kzalloc(LOG_INFO_EXCP_PAGE_LENGTH, GFP_KERNEL);
-	if (log_response == NULL) {
-		res = -ENOMEM;
-		goto out_mem;
-	}
+	if (log_response == NULL)
+		return -ENOMEM;
 
-	mem = dma_alloc_coherent(&dev->pci_dev->dev,
-					sizeof(struct nvme_smart_log),
-					&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out_dma;
-	}
+	res = nvme_get_log_page(dev, &smart_log);
+	if (res < 0)
+		goto out_free_response;
 
-	/* Get SMART Log Page */
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = nvme_admin_get_log_page;
-	c.common.nsid = cpu_to_le32(0xFFFFFFFF);
-	c.common.prp1 = cpu_to_le64(dma_addr);
-	c.common.cdw10[0] = cpu_to_le32((((sizeof(struct nvme_smart_log) /
-			BYTES_TO_DWORDS) - 1) << 16) | NVME_LOG_SMART);
-	res = nvme_submit_admin_cmd(dev, &c, NULL);
 	if (res != NVME_SC_SUCCESS) {
 		temp_c = LOG_TEMP_UNKNOWN;
 	} else {
-		smart_log = mem;
 		temp_k = (smart_log->temperature[1] << 8) +
 				(smart_log->temperature[0]);
 		temp_c = temp_k - KELVIN_TEMP_FACTOR;
 	}
+	kfree(smart_log);
 
 	log_response[0] = LOG_PAGE_INFORMATIONAL_EXCEPTIONS_PAGE;
 	/* Subpage=0x00, Page Length MSB=0 */
@@ -1078,59 +851,39 @@ static int nvme_trans_log_info_exceptions(struct nvme_ns *ns,
 	xfer_len = min(alloc_len, LOG_INFO_EXCP_PAGE_LENGTH);
 	res = nvme_trans_copy_to_user(hdr, log_response, xfer_len);
 
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_smart_log),
-			  mem, dma_addr);
- out_dma:
+ out_free_response:
 	kfree(log_response);
- out_mem:
 	return res;
 }
 
 static int nvme_trans_log_temperature(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					int alloc_len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int xfer_len;
 	u8 *log_response;
-	struct nvme_command c;
 	struct nvme_dev *dev = ns->dev;
 	struct nvme_smart_log *smart_log;
-	dma_addr_t dma_addr;
-	void *mem;
 	u32 feature_resp;
 	u8 temp_c_cur, temp_c_thresh;
 	u16 temp_k;
 
 	log_response = kzalloc(LOG_TEMP_PAGE_LENGTH, GFP_KERNEL);
-	if (log_response == NULL) {
-		res = -ENOMEM;
-		goto out_mem;
-	}
+	if (log_response == NULL)
+		return -ENOMEM;
 
-	mem = dma_alloc_coherent(&dev->pci_dev->dev,
-					sizeof(struct nvme_smart_log),
-					&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out_dma;
-	}
+	res = nvme_get_log_page(dev, &smart_log);
+	if (res < 0)
+		goto out_free_response;
 
-	/* Get SMART Log Page */
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = nvme_admin_get_log_page;
-	c.common.nsid = cpu_to_le32(0xFFFFFFFF);
-	c.common.prp1 = cpu_to_le64(dma_addr);
-	c.common.cdw10[0] = cpu_to_le32((((sizeof(struct nvme_smart_log) /
-			BYTES_TO_DWORDS) - 1) << 16) | NVME_LOG_SMART);
-	res = nvme_submit_admin_cmd(dev, &c, NULL);
 	if (res != NVME_SC_SUCCESS) {
 		temp_c_cur = LOG_TEMP_UNKNOWN;
 	} else {
-		smart_log = mem;
 		temp_k = (smart_log->temperature[1] << 8) +
 				(smart_log->temperature[0]);
 		temp_c_cur = temp_k - KELVIN_TEMP_FACTOR;
 	}
+	kfree(smart_log);
 
 	/* Get Features for Temp Threshold */
 	res = nvme_get_features(dev, NVME_FEAT_TEMP_THRESH, 0, 0,
@@ -1159,11 +912,8 @@ static int nvme_trans_log_temperature(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	xfer_len = min(alloc_len, LOG_TEMP_PAGE_LENGTH);
 	res = nvme_trans_copy_to_user(hdr, log_response, xfer_len);
 
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_smart_log),
-			  mem, dma_addr);
- out_dma:
+ out_free_response:
 	kfree(log_response);
- out_mem:
 	return res;
 }
 
@@ -1174,59 +924,45 @@ static int nvme_trans_fill_mode_parm_hdr(u8 *resp, int len, u8 cdb10, u8 llbaa,
 {
 	/* Quick check to make sure I don't stomp on my own memory... */
 	if ((cdb10 && len < 8) || (!cdb10 && len < 4))
-		return SNTI_INTERNAL_ERROR;
+		return -EINVAL;
 
 	if (cdb10) {
 		resp[0] = (mode_data_length & 0xFF00) >> 8;
 		resp[1] = (mode_data_length & 0x00FF);
-		/* resp[2] and [3] are zero */
+		resp[3] = 0x10 /* DPOFUA */;
 		resp[4] = llbaa;
 		resp[5] = RESERVED_FIELD;
 		resp[6] = (blk_desc_len & 0xFF00) >> 8;
 		resp[7] = (blk_desc_len & 0x00FF);
 	} else {
 		resp[0] = (mode_data_length & 0x00FF);
-		/* resp[1] and [2] are zero */
+		resp[2] = 0x10 /* DPOFUA */;
 		resp[3] = (blk_desc_len & 0x00FF);
 	}
 
-	return SNTI_TRANSLATION_SUCCESS;
+	return 0;
 }
 
 static int nvme_trans_fill_blk_desc(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 				    u8 *resp, int len, u8 llbaa)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int nvme_sc;
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
 	struct nvme_id_ns *id_ns;
 	u8 flbas;
 	u32 lba_length;
 
 	if (llbaa == 0 && len < MODE_PAGE_BLK_DES_LEN)
-		return SNTI_INTERNAL_ERROR;
+		return -EINVAL;
 	else if (llbaa > 0 && len < MODE_PAGE_LLBAA_BLK_DES_LEN)
-		return SNTI_INTERNAL_ERROR;
-
-	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
-							&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out;
-	}
+		return -EINVAL;
 
-	/* nvme ns identify */
-	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
-		goto out_dma;
-	if (nvme_sc) {
-		res = nvme_sc;
-		goto out_dma;
-	}
-	id_ns = mem;
+		return res;
+
 	flbas = (id_ns->flbas) & 0x0F;
 	lba_length = (1 << (id_ns->lbaf[flbas].ds));
 
@@ -1246,10 +982,7 @@ static int nvme_trans_fill_blk_desc(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		memcpy(&resp[12], &tmp_len, sizeof(u32));
 	}
 
- out_dma:
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
-			  dma_addr);
- out:
+	kfree(id_ns);
 	return res;
 }
 
@@ -1258,7 +991,7 @@ static int nvme_trans_fill_control_page(struct nvme_ns *ns,
 					int len)
 {
 	if (len < MODE_PAGE_CONTROL_LEN)
-		return SNTI_INTERNAL_ERROR;
+		return -EINVAL;
 
 	resp[0] = MODE_PAGE_CONTROL;
 	resp[1] = MODE_PAGE_CONTROL_LEN_FIELD;
@@ -1272,78 +1005,69 @@ static int nvme_trans_fill_control_page(struct nvme_ns *ns,
 	resp[9] = 0xFF;
 	/* Bytes 10,11: Extended selftest completion time = 0x0000 */
 
-	return SNTI_TRANSLATION_SUCCESS;
+	return 0;
 }
 
 static int nvme_trans_fill_caching_page(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr,
 					u8 *resp, int len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res = 0;
 	int nvme_sc;
 	struct nvme_dev *dev = ns->dev;
 	u32 feature_resp;
 	u8 vwc;
 
 	if (len < MODE_PAGE_CACHING_LEN)
-		return SNTI_INTERNAL_ERROR;
+		return -EINVAL;
 
 	nvme_sc = nvme_get_features(dev, NVME_FEAT_VOLATILE_WC, 0, 0,
 								&feature_resp);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
-		goto out;
-	if (nvme_sc) {
-		res = nvme_sc;
-		goto out;
-	}
+		return res;
+
 	vwc = feature_resp & 0x00000001;
 
 	resp[0] = MODE_PAGE_CACHING;
 	resp[1] = MODE_PAGE_CACHING_LEN_FIELD;
 	resp[2] = vwc << 2;
-
- out:
-	return res;
+	return 0;
 }
 
 static int nvme_trans_fill_pow_cnd_page(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr, u8 *resp,
 					int len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
-
 	if (len < MODE_PAGE_POW_CND_LEN)
-		return SNTI_INTERNAL_ERROR;
+		return -EINVAL;
 
 	resp[0] = MODE_PAGE_POWER_CONDITION;
 	resp[1] = MODE_PAGE_POW_CND_LEN_FIELD;
 	/* All other bytes are zero */
 
-	return res;
+	return 0;
 }
 
 static int nvme_trans_fill_inf_exc_page(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr, u8 *resp,
 					int len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
-
 	if (len < MODE_PAGE_INF_EXC_LEN)
-		return SNTI_INTERNAL_ERROR;
+		return -EINVAL;
 
 	resp[0] = MODE_PAGE_INFO_EXCEP;
 	resp[1] = MODE_PAGE_INF_EXC_LEN_FIELD;
 	resp[2] = 0x88;
 	/* All other bytes are zero */
 
-	return res;
+	return 0;
 }
 
 static int nvme_trans_fill_all_pages(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 				     u8 *resp, int len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	u16 mode_pages_offset_1 = 0;
 	u16 mode_pages_offset_2, mode_pages_offset_3, mode_pages_offset_4;
 
@@ -1353,23 +1077,18 @@ static int nvme_trans_fill_all_pages(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 
 	res = nvme_trans_fill_caching_page(ns, hdr, &resp[mode_pages_offset_1],
 					MODE_PAGE_CACHING_LEN);
-	if (res != SNTI_TRANSLATION_SUCCESS)
-		goto out;
+	if (res)
+		return res;
 	res = nvme_trans_fill_control_page(ns, hdr, &resp[mode_pages_offset_2],
 					MODE_PAGE_CONTROL_LEN);
-	if (res != SNTI_TRANSLATION_SUCCESS)
-		goto out;
+	if (res)
+		return res;
 	res = nvme_trans_fill_pow_cnd_page(ns, hdr, &resp[mode_pages_offset_3],
 					MODE_PAGE_POW_CND_LEN);
-	if (res != SNTI_TRANSLATION_SUCCESS)
-		goto out;
-	res = nvme_trans_fill_inf_exc_page(ns, hdr, &resp[mode_pages_offset_4],
+	if (res)
+		return res;
+	return nvme_trans_fill_inf_exc_page(ns, hdr, &resp[mode_pages_offset_4],
 					MODE_PAGE_INF_EXC_LEN);
-	if (res != SNTI_TRANSLATION_SUCCESS)
-		goto out;
-
- out:
-	return res;
 }
 
 static inline int nvme_trans_get_blk_desc_len(u8 dbd, u8 llbaa)
@@ -1390,7 +1109,7 @@ static int nvme_trans_mode_page_create(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr, u8 *, int),
 					u16 mode_pages_tot_len)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int xfer_len;
 	u8 *response;
 	u8 dbd, llbaa;
@@ -1399,9 +1118,10 @@ static int nvme_trans_mode_page_create(struct nvme_ns *ns,
 	u16 mode_pages_offset_1;
 	u16 blk_desc_len, blk_desc_offset, mode_data_length;
 
-	dbd = GET_MODE_SENSE_DBD(cmd);
-	llbaa = GET_MODE_SENSE_LLBAA(cmd);
-	mph_size = GET_MODE_SENSE_MPH_SIZE(cdb10);
+	dbd = (cmd[1] & MODE_SENSE_DBD_MASK) >> MODE_SENSE_DBD_SHIFT;
+	llbaa = (cmd[1] & MODE_SENSE_LLBAA_MASK) >> MODE_SENSE_LLBAA_SHIFT;
+	mph_size = cdb10 ? MODE_SENSE10_MPH_SIZE : MODE_SENSE6_MPH_SIZE;
+
 	blk_desc_len = nvme_trans_get_blk_desc_len(dbd, llbaa);
 
 	resp_size = mph_size + blk_desc_len + mode_pages_tot_len;
@@ -1419,18 +1139,18 @@ static int nvme_trans_mode_page_create(struct nvme_ns *ns,
 
 	res = nvme_trans_fill_mode_parm_hdr(&response[0], mph_size, cdb10,
 					llbaa, mode_data_length, blk_desc_len);
-	if (res != SNTI_TRANSLATION_SUCCESS)
+	if (res)
 		goto out_free;
 	if (blk_desc_len > 0) {
 		res = nvme_trans_fill_blk_desc(ns, hdr,
 					       &response[blk_desc_offset],
 					       blk_desc_len, llbaa);
-		if (res != SNTI_TRANSLATION_SUCCESS)
+		if (res)
 			goto out_free;
 	}
 	res = mode_page_fill_func(ns, hdr, &response[mode_pages_offset_1],
 					mode_pages_tot_len);
-	if (res != SNTI_TRANSLATION_SUCCESS)
+	if (res)
 		goto out_free;
 
 	xfer_len = min(alloc_len, resp_size);
@@ -1485,33 +1205,20 @@ static void nvme_trans_fill_read_cap(u8 *response, struct nvme_id_ns *id_ns,
 static int nvme_trans_power_state(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 						u8 pc, u8 pcmod, u8 start)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int nvme_sc;
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
 	struct nvme_id_ctrl *id_ctrl;
 	int lowest_pow_st;	/* max npss = lowest power consumption */
 	unsigned ps_desired = 0;
 
-	/* NVMe Controller Identify */
-	mem = dma_alloc_coherent(&dev->pci_dev->dev,
-				sizeof(struct nvme_id_ctrl),
-				&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out;
-	}
-	nvme_sc = nvme_identify(dev, 0, 1, dma_addr);
+	nvme_sc = nvme_identify_ctrl(dev, &id_ctrl);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
-		goto out_dma;
-	if (nvme_sc) {
-		res = nvme_sc;
-		goto out_dma;
-	}
-	id_ctrl = mem;
+		return res;
+
 	lowest_pow_st = max(POWER_STATE_0, (int)(id_ctrl->npss - 1));
+	kfree(id_ctrl);
 
 	switch (pc) {
 	case NVME_POWER_STATE_START_VALID:
@@ -1551,79 +1258,48 @@ static int nvme_trans_power_state(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	}
 	nvme_sc = nvme_set_features(dev, NVME_FEAT_POWER_MGMT, ps_desired, 0,
 				    NULL);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		goto out_dma;
-	if (nvme_sc)
-		res = nvme_sc;
- out_dma:
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ctrl), mem,
-			  dma_addr);
- out:
-	return res;
+	return nvme_trans_status_code(hdr, nvme_sc);
 }
 
-/* Write Buffer Helper Functions */
-/* Also using this for Format Unit with hdr passed as NULL, and buffer_id, 0 */
+static int nvme_trans_send_activate_fw_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					u8 buffer_id)
+{
+	struct nvme_command c;
+	int nvme_sc;
 
-static int nvme_trans_send_fw_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = nvme_admin_activate_fw;
+	c.common.cdw10[0] = cpu_to_le32(buffer_id | NVME_FWACT_REPL_ACTV);
+
+	nvme_sc = nvme_submit_sync_cmd(ns->queue, &c, NULL, 0);
+	return nvme_trans_status_code(hdr, nvme_sc);
+}
+
+static int nvme_trans_send_download_fw_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					u8 opcode, u32 tot_len, u32 offset,
 					u8 buffer_id)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
 	int nvme_sc;
 	struct nvme_dev *dev = ns->dev;
 	struct nvme_command c;
-	struct nvme_iod *iod = NULL;
-	unsigned length;
 
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = opcode;
-	if (opcode == nvme_admin_download_fw) {
-		if (hdr->iovec_count > 0) {
-			/* Assuming SGL is not allowed for this command */
-			res = nvme_trans_completion(hdr,
-						SAM_STAT_CHECK_CONDITION,
-						ILLEGAL_REQUEST,
-						SCSI_ASC_INVALID_CDB,
-						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-			goto out;
-		}
-		iod = nvme_map_user_pages(dev, DMA_TO_DEVICE,
-				(unsigned long)hdr->dxferp, tot_len);
-		if (IS_ERR(iod)) {
-			res = PTR_ERR(iod);
-			goto out;
-		}
-		length = nvme_setup_prps(dev, iod, tot_len, GFP_KERNEL);
-		if (length != tot_len) {
-			res = -ENOMEM;
-			goto out_unmap;
-		}
-
-		c.dlfw.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
-		c.dlfw.prp2 = cpu_to_le64(iod->first_dma);
-		c.dlfw.numd = cpu_to_le32((tot_len/BYTES_TO_DWORDS) - 1);
-		c.dlfw.offset = cpu_to_le32(offset/BYTES_TO_DWORDS);
-	} else if (opcode == nvme_admin_activate_fw) {
-		u32 cdw10 = buffer_id | NVME_FWACT_REPL_ACTV;
-		c.common.cdw10[0] = cpu_to_le32(cdw10);
+	if (hdr->iovec_count > 0) {
+		/* Assuming SGL is not allowed for this command */
+		return nvme_trans_completion(hdr,
+					SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST,
+					SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 	}
 
-	nvme_sc = nvme_submit_admin_cmd(dev, &c, NULL);
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		goto out_unmap;
-	if (nvme_sc)
-		res = nvme_sc;
-
- out_unmap:
-	if (opcode == nvme_admin_download_fw) {
-		nvme_unmap_user_pages(dev, DMA_TO_DEVICE, iod);
-		nvme_free_iod(dev, iod);
-	}
- out:
-	return res;
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = nvme_admin_download_fw;
+	c.dlfw.numd = cpu_to_le32((tot_len/BYTES_TO_DWORDS) - 1);
+	c.dlfw.offset = cpu_to_le32(offset/BYTES_TO_DWORDS);
+
+	nvme_sc = __nvme_submit_sync_cmd(dev->admin_q, &c, NULL,
+			hdr->dxferp, tot_len, NULL, 0);
+	return nvme_trans_status_code(hdr, nvme_sc);
 }
 
 /* Mode Select Helper Functions */
@@ -1686,7 +1362,7 @@ static void nvme_trans_modesel_save_bd(struct nvme_ns *ns, u8 *parm_list,
 static int nvme_trans_modesel_get_mp(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					u8 *mode_page, u8 page_code)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res = 0;
 	int nvme_sc;
 	struct nvme_dev *dev = ns->dev;
 	unsigned dword11;
@@ -1697,12 +1373,6 @@ static int nvme_trans_modesel_get_mp(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		nvme_sc = nvme_set_features(dev, NVME_FEAT_VOLATILE_WC, dword11,
 					    0, NULL);
 		res = nvme_trans_status_code(hdr, nvme_sc);
-		if (res)
-			break;
-		if (nvme_sc) {
-			res = nvme_sc;
-			break;
-		}
 		break;
 	case MODE_PAGE_CONTROL:
 		break;
@@ -1714,8 +1384,6 @@ static int nvme_trans_modesel_get_mp(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 						ILLEGAL_REQUEST,
 						SCSI_ASC_INVALID_PARAMETER,
 						SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-			if (!res)
-				res = SNTI_INTERNAL_ERROR;
 			break;
 		}
 		break;
@@ -1723,8 +1391,6 @@ static int nvme_trans_modesel_get_mp(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		if (!res)
-			res = SNTI_INTERNAL_ERROR;
 		break;
 	}
 
@@ -1735,7 +1401,7 @@ static int nvme_trans_modesel_data(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					u8 *cmd, u16 parm_list_len, u8 pf,
 					u8 sp, u8 cdb10)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	u8 *parm_list;
 	u16 bd_len;
 	u8 llbaa = 0;
@@ -1751,7 +1417,7 @@ static int nvme_trans_modesel_data(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	}
 
 	res = nvme_trans_copy_from_user(hdr, parm_list, parm_list_len);
-	if (res != SNTI_TRANSLATION_SUCCESS)
+	if (res)
 		goto out_mem;
 
 	nvme_trans_modesel_get_bd_len(parm_list, cdb10, &bd_len, &llbaa);
@@ -1789,7 +1455,7 @@ static int nvme_trans_modesel_data(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		mp_size = parm_list[index + 1] + 2;
 		res = nvme_trans_modesel_get_mp(ns, hdr, &parm_list[index],
 								page_code);
-		if (res != SNTI_TRANSLATION_SUCCESS)
+		if (res)
 			break;
 		index += mp_size;
 	} while (index < parm_list_len);
@@ -1805,12 +1471,9 @@ static int nvme_trans_modesel_data(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 static int nvme_trans_fmt_set_blk_size_count(struct nvme_ns *ns,
 					     struct sg_io_hdr *hdr)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res = 0;
 	int nvme_sc;
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
-	struct nvme_id_ns *id_ns;
 	u8 flbas;
 
 	/*
@@ -1821,22 +1484,12 @@ static int nvme_trans_fmt_set_blk_size_count(struct nvme_ns *ns,
 	 */
 
 	if (ns->mode_select_num_blocks == 0 || ns->mode_select_block_len == 0) {
-		mem = dma_alloc_coherent(&dev->pci_dev->dev,
-			sizeof(struct nvme_id_ns), &dma_addr, GFP_KERNEL);
-		if (mem == NULL) {
-			res = -ENOMEM;
-			goto out;
-		}
-		/* nvme ns identify */
-		nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+		struct nvme_id_ns *id_ns;
+
+		nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
 		res = nvme_trans_status_code(hdr, nvme_sc);
 		if (res)
-			goto out_dma;
-		if (nvme_sc) {
-			res = nvme_sc;
-			goto out_dma;
-		}
-		id_ns = mem;
+			return res;
 
 		if (ns->mode_select_num_blocks == 0)
 			ns->mode_select_num_blocks = le64_to_cpu(id_ns->ncap);
@@ -1845,18 +1498,17 @@ static int nvme_trans_fmt_set_blk_size_count(struct nvme_ns *ns,
 			ns->mode_select_block_len =
 						(1 << (id_ns->lbaf[flbas].ds));
 		}
- out_dma:
-		dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
-				  mem, dma_addr);
+
+		kfree(id_ns);
 	}
- out:
-	return res;
+
+	return 0;
 }
 
 static int nvme_trans_fmt_get_parm_header(struct sg_io_hdr *hdr, u8 len,
 					u8 format_prot_info, u8 *nvme_pf_code)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	u8 *parm_list;
 	u8 pf_usage, pf_code;
 
@@ -1866,7 +1518,7 @@ static int nvme_trans_fmt_get_parm_header(struct sg_io_hdr *hdr, u8 len,
 		goto out;
 	}
 	res = nvme_trans_copy_from_user(hdr, parm_list, len);
-	if (res != SNTI_TRANSLATION_SUCCESS)
+	if (res)
 		goto out_mem;
 
 	if ((parm_list[FORMAT_UNIT_IMMED_OFFSET] &
@@ -1916,11 +1568,9 @@ static int nvme_trans_fmt_get_parm_header(struct sg_io_hdr *hdr, u8 len,
 static int nvme_trans_fmt_send_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 				   u8 prot_info)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int nvme_sc;
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
 	struct nvme_id_ns *id_ns;
 	u8 i;
 	u8 flbas, nlbaf;
@@ -1929,22 +1579,11 @@ static int nvme_trans_fmt_send_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	struct nvme_command c;
 
 	/* Loop thru LBAF's in id_ns to match reqd lbaf, put in cdw10 */
-	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
-							&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out;
-	}
-	/* nvme ns identify */
-	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
-		goto out_dma;
-	if (nvme_sc) {
-		res = nvme_sc;
-		goto out_dma;
-	}
-	id_ns = mem;
+		return res;
+
 	flbas = (id_ns->flbas) & 0x0F;
 	nlbaf = id_ns->nlbaf;
 
@@ -1972,69 +1611,13 @@ static int nvme_trans_fmt_send_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	c.format.nsid = cpu_to_le32(ns->ns_id);
 	c.format.cdw10 = cpu_to_le32(cdw10);
 
-	nvme_sc = nvme_submit_admin_cmd(dev, &c, NULL);
+	nvme_sc = nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0);
 	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		goto out_dma;
-	if (nvme_sc)
-		res = nvme_sc;
 
- out_dma:
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
-			  dma_addr);
- out:
+	kfree(id_ns);
 	return res;
 }
 
-/* Read/Write Helper Functions */
-
-static inline void nvme_trans_get_io_cdb6(u8 *cmd,
-					struct nvme_trans_io_cdb *cdb_info)
-{
-	cdb_info->fua = 0;
-	cdb_info->prot_info = 0;
-	cdb_info->lba = GET_U32_FROM_CDB(cmd, IO_6_CDB_LBA_OFFSET) &
-					IO_6_CDB_LBA_MASK;
-	cdb_info->xfer_len = GET_U8_FROM_CDB(cmd, IO_6_CDB_TX_LEN_OFFSET);
-
-	/* sbc3r27 sec 5.32 - TRANSFER LEN of 0 implies a 256 Block transfer */
-	if (cdb_info->xfer_len == 0)
-		cdb_info->xfer_len = IO_6_DEFAULT_TX_LEN;
-}
-
-static inline void nvme_trans_get_io_cdb10(u8 *cmd,
-					struct nvme_trans_io_cdb *cdb_info)
-{
-	cdb_info->fua = GET_U8_FROM_CDB(cmd, IO_10_CDB_FUA_OFFSET) &
-					IO_CDB_FUA_MASK;
-	cdb_info->prot_info = GET_U8_FROM_CDB(cmd, IO_10_CDB_WP_OFFSET) &
-					IO_CDB_WP_MASK >> IO_CDB_WP_SHIFT;
-	cdb_info->lba = GET_U32_FROM_CDB(cmd, IO_10_CDB_LBA_OFFSET);
-	cdb_info->xfer_len = GET_U16_FROM_CDB(cmd, IO_10_CDB_TX_LEN_OFFSET);
-}
-
-static inline void nvme_trans_get_io_cdb12(u8 *cmd,
-					struct nvme_trans_io_cdb *cdb_info)
-{
-	cdb_info->fua = GET_U8_FROM_CDB(cmd, IO_12_CDB_FUA_OFFSET) &
-					IO_CDB_FUA_MASK;
-	cdb_info->prot_info = GET_U8_FROM_CDB(cmd, IO_12_CDB_WP_OFFSET) &
-					IO_CDB_WP_MASK >> IO_CDB_WP_SHIFT;
-	cdb_info->lba = GET_U32_FROM_CDB(cmd, IO_12_CDB_LBA_OFFSET);
-	cdb_info->xfer_len = GET_U32_FROM_CDB(cmd, IO_12_CDB_TX_LEN_OFFSET);
-}
-
-static inline void nvme_trans_get_io_cdb16(u8 *cmd,
-					struct nvme_trans_io_cdb *cdb_info)
-{
-	cdb_info->fua = GET_U8_FROM_CDB(cmd, IO_16_CDB_FUA_OFFSET) &
-					IO_CDB_FUA_MASK;
-	cdb_info->prot_info = GET_U8_FROM_CDB(cmd, IO_16_CDB_WP_OFFSET) &
-					IO_CDB_WP_MASK >> IO_CDB_WP_SHIFT;
-	cdb_info->lba = GET_U64_FROM_CDB(cmd, IO_16_CDB_LBA_OFFSET);
-	cdb_info->xfer_len = GET_U32_FROM_CDB(cmd, IO_16_CDB_TX_LEN_OFFSET);
-}
-
 static inline u32 nvme_trans_io_get_num_cmds(struct sg_io_hdr *hdr,
 					struct nvme_trans_io_cdb *cdb_info,
 					u32 max_blocks)
@@ -2064,11 +1647,8 @@ static u16 nvme_trans_io_get_control(struct nvme_ns *ns,
 static int nvme_trans_do_nvme_io(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 				struct nvme_trans_io_cdb *cdb_info, u8 is_write)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
-	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
+	int nvme_sc = NVME_SC_SUCCESS;
 	u32 num_cmds;
-	struct nvme_iod *iod;
 	u64 unit_len;
 	u64 unit_num_blocks;	/* Number of blocks to xfer in each nvme cmd */
 	u32 retcode;
@@ -2119,45 +1699,20 @@ static int nvme_trans_do_nvme_io(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		control = nvme_trans_io_get_control(ns, cdb_info);
 		c.rw.control = cpu_to_le16(control);
 
-		iod = nvme_map_user_pages(dev,
-			(is_write) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
-			(unsigned long)next_mapping_addr, unit_len);
-		if (IS_ERR(iod)) {
-			res = PTR_ERR(iod);
-			goto out;
-		}
-		retcode = nvme_setup_prps(dev, iod, unit_len, GFP_KERNEL);
-		if (retcode != unit_len) {
-			nvme_unmap_user_pages(dev,
-				(is_write) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
-				iod);
-			nvme_free_iod(dev, iod);
-			res = -ENOMEM;
-			goto out;
+		if (get_capacity(ns->disk) - unit_num_blocks <
+				cdb_info->lba + nvme_offset) {
+			nvme_sc = NVME_SC_LBA_RANGE;
+			break;
 		}
-		c.rw.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
-		c.rw.prp2 = cpu_to_le64(iod->first_dma);
+		nvme_sc = __nvme_submit_sync_cmd(ns->queue, &c, NULL,
+				next_mapping_addr, unit_len, NULL, 0);
+		if (nvme_sc)
+			break;
 
 		nvme_offset += unit_num_blocks;
-
-		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,
-				iod);
-			nvme_free_iod(dev, iod);
-			res = nvme_trans_status_code(hdr, nvme_sc);
-			goto out;
-		}
-		nvme_unmap_user_pages(dev,
-				(is_write) ? DMA_TO_DEVICE : DMA_FROM_DEVICE,
-				iod);
-		nvme_free_iod(dev, iod);
 	}
-	res = nvme_trans_status_code(hdr, NVME_SC_SUCCESS);
 
- out:
-	return res;
+	return nvme_trans_status_code(hdr, nvme_sc);
 }
 
 
@@ -2166,8 +1721,8 @@ static int nvme_trans_do_nvme_io(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 static int nvme_trans_io(struct nvme_ns *ns, struct sg_io_hdr *hdr, u8 is_write,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
-	struct nvme_trans_io_cdb cdb_info;
+	int res = 0;
+	struct nvme_trans_io_cdb cdb_info = { 0, };
 	u8 opcode = cmd[0];
 	u64 xfer_bytes;
 	u64 sum_iov_len = 0;
@@ -2175,27 +1730,52 @@ static int nvme_trans_io(struct nvme_ns *ns, struct sg_io_hdr *hdr, u8 is_write,
 	int i;
 	size_t not_copied;
 
-	/* Extract Fields from CDB */
+	/*
+	 * The FUA and WPROTECT fields are not supported in 6-byte CDBs,
+	 * but always in the same place for all others.
+	 */
+	switch (opcode) {
+	case WRITE_6:
+	case READ_6:
+		break;
+	default:
+		cdb_info.fua = cmd[1] & 0x8;
+		cdb_info.prot_info = (cmd[1] & 0xe0) >> 5;
+		if (cdb_info.prot_info && !ns->pi_type) {
+			return nvme_trans_completion(hdr,
+					SAM_STAT_CHECK_CONDITION,
+					ILLEGAL_REQUEST,
+					SCSI_ASC_INVALID_CDB,
+					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+		}
+	}
+
 	switch (opcode) {
 	case WRITE_6:
 	case READ_6:
-		nvme_trans_get_io_cdb6(cmd, &cdb_info);
+		cdb_info.lba = get_unaligned_be24(&cmd[1]);
+		cdb_info.xfer_len = cmd[4];
+		if (cdb_info.xfer_len == 0)
+			cdb_info.xfer_len = 256;
 		break;
 	case WRITE_10:
 	case READ_10:
-		nvme_trans_get_io_cdb10(cmd, &cdb_info);
+		cdb_info.lba = get_unaligned_be32(&cmd[2]);
+		cdb_info.xfer_len = get_unaligned_be16(&cmd[7]);
 		break;
 	case WRITE_12:
 	case READ_12:
-		nvme_trans_get_io_cdb12(cmd, &cdb_info);
+		cdb_info.lba = get_unaligned_be32(&cmd[2]);
+		cdb_info.xfer_len = get_unaligned_be32(&cmd[6]);
 		break;
 	case WRITE_16:
 	case READ_16:
-		nvme_trans_get_io_cdb16(cmd, &cdb_info);
+		cdb_info.lba = get_unaligned_be64(&cmd[2]);
+		cdb_info.xfer_len = get_unaligned_be32(&cmd[10]);
 		break;
 	default:
 		/* Will never really reach here */
-		res = SNTI_INTERNAL_ERROR;
+		res = -EIO;
 		goto out;
 	}
 
@@ -2237,7 +1817,7 @@ static int nvme_trans_io(struct nvme_ns *ns, struct sg_io_hdr *hdr, u8 is_write,
 
 	/* Send NVMe IO Command(s) */
 	res = nvme_trans_do_nvme_io(ns, hdr, &cdb_info, is_write);
-	if (res != SNTI_TRANSLATION_SUCCESS)
+	if (res)
 		goto out;
 
  out:
@@ -2247,15 +1827,15 @@ static int nvme_trans_io(struct nvme_ns *ns, struct sg_io_hdr *hdr, u8 is_write,
 static int nvme_trans_inquiry(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res = 0;
 	u8 evpd;
 	u8 page_code;
 	int alloc_len;
 	u8 *inq_response;
 
-	evpd = GET_INQ_EVPD_BIT(cmd);
-	page_code = GET_INQ_PAGE_CODE(cmd);
-	alloc_len = GET_INQ_ALLOC_LENGTH(cmd);
+	evpd = cmd[1] & 0x01;
+	page_code = cmd[2];
+	alloc_len = get_unaligned_be16(&cmd[3]);
 
 	inq_response = kmalloc(max(alloc_len, STANDARD_INQUIRY_LENGTH),
 				GFP_KERNEL);
@@ -2316,29 +1896,27 @@ static int nvme_trans_inquiry(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 static int nvme_trans_log_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	u16 alloc_len;
-	u8 sp;
 	u8 pc;
 	u8 page_code;
 
-	sp = GET_U8_FROM_CDB(cmd, LOG_SENSE_CDB_SP_OFFSET);
-	if (sp != LOG_SENSE_CDB_SP_NOT_ENABLED) {
+	if (cmd[1] != LOG_SENSE_CDB_SP_NOT_ENABLED) {
 		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 		goto out;
 	}
-	pc = GET_U8_FROM_CDB(cmd, LOG_SENSE_CDB_PC_OFFSET);
-	page_code = pc & LOG_SENSE_CDB_PAGE_CODE_MASK;
-	pc = (pc & LOG_SENSE_CDB_PC_MASK) >> LOG_SENSE_CDB_PC_SHIFT;
+
+	page_code = cmd[2] & LOG_SENSE_CDB_PAGE_CODE_MASK;
+	pc = (cmd[2] & LOG_SENSE_CDB_PC_MASK) >> LOG_SENSE_CDB_PC_SHIFT;
 	if (pc != LOG_SENSE_CDB_PC_CUMULATIVE_VALUES) {
 		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 		goto out;
 	}
-	alloc_len = GET_U16_FROM_CDB(cmd, LOG_SENSE_CDB_ALLOC_LENGTH_OFFSET);
+	alloc_len = get_unaligned_be16(&cmd[7]);
 	switch (page_code) {
 	case LOG_PAGE_SUPPORTED_LOG_PAGES_PAGE:
 		res = nvme_trans_log_supp_pages(ns, hdr, alloc_len);
@@ -2363,24 +1941,18 @@ static int nvme_trans_log_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 static int nvme_trans_mode_select(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
 	u8 cdb10 = 0;
 	u16 parm_list_len;
 	u8 page_format;
 	u8 save_pages;
 
-	page_format = GET_U8_FROM_CDB(cmd, MODE_SELECT_CDB_PAGE_FORMAT_OFFSET);
-	page_format &= MODE_SELECT_CDB_PAGE_FORMAT_MASK;
+	page_format = cmd[1] & MODE_SELECT_CDB_PAGE_FORMAT_MASK;
+	save_pages = cmd[1] & MODE_SELECT_CDB_SAVE_PAGES_MASK;
 
-	save_pages = GET_U8_FROM_CDB(cmd, MODE_SELECT_CDB_SAVE_PAGES_OFFSET);
-	save_pages &= MODE_SELECT_CDB_SAVE_PAGES_MASK;
-
-	if (GET_OPCODE(cmd) == MODE_SELECT) {
-		parm_list_len = GET_U8_FROM_CDB(cmd,
-				MODE_SELECT_6_CDB_PARAM_LIST_LENGTH_OFFSET);
+	if (cmd[0] == MODE_SELECT) {
+		parm_list_len = cmd[4];
 	} else {
-		parm_list_len = GET_U16_FROM_CDB(cmd,
-				MODE_SELECT_10_CDB_PARAM_LIST_LENGTH_OFFSET);
+		parm_list_len = cmd[7];
 		cdb10 = 1;
 	}
 
@@ -2389,42 +1961,36 @@ static int nvme_trans_mode_select(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		 * According to SPC-4 r24, a paramter list length field of 0
 		 * shall not be considered an error
 		 */
-		res = nvme_trans_modesel_data(ns, hdr, cmd, parm_list_len,
+		return nvme_trans_modesel_data(ns, hdr, cmd, parm_list_len,
 						page_format, save_pages, cdb10);
 	}
 
-	return res;
+	return 0;
 }
 
 static int nvme_trans_mode_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res = 0;
 	u16 alloc_len;
 	u8 cdb10 = 0;
-	u8 page_code;
-	u8 pc;
 
-	if (GET_OPCODE(cmd) == MODE_SENSE) {
-		alloc_len = GET_U8_FROM_CDB(cmd, MODE_SENSE6_ALLOC_LEN_OFFSET);
+	if (cmd[0] == MODE_SENSE) {
+		alloc_len = cmd[4];
 	} else {
-		alloc_len = GET_U16_FROM_CDB(cmd,
-						MODE_SENSE10_ALLOC_LEN_OFFSET);
+		alloc_len = get_unaligned_be16(&cmd[7]);
 		cdb10 = 1;
 	}
 
-	pc = GET_U8_FROM_CDB(cmd, MODE_SENSE_PAGE_CONTROL_OFFSET) &
-						MODE_SENSE_PAGE_CONTROL_MASK;
-	if (pc != MODE_SENSE_PC_CURRENT_VALUES) {
+	if ((cmd[2] & MODE_SENSE_PAGE_CONTROL_MASK) !=
+			MODE_SENSE_PC_CURRENT_VALUES) {
 		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 		goto out;
 	}
 
-	page_code = GET_U8_FROM_CDB(cmd, MODE_SENSE_PAGE_CODE_OFFSET) &
-					MODE_SENSE_PAGE_CODE_MASK;
-	switch (page_code) {
+	switch (cmd[2] & MODE_SENSE_PAGE_CODE_MASK) {
 	case MODE_PAGE_CACHING:
 		res = nvme_trans_mode_page_create(ns, hdr, cmd, alloc_len,
 						cdb10,
@@ -2467,47 +2033,34 @@ static int nvme_trans_mode_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 }
 
 static int nvme_trans_read_capacity(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
+							u8 *cmd, u8 cdb16)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int nvme_sc;
-	u32 alloc_len = READ_CAP_10_RESP_SIZE;
-	u32 resp_size = READ_CAP_10_RESP_SIZE;
+	u32 alloc_len;
+	u32 resp_size;
 	u32 xfer_len;
-	u8 cdb16;
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
 	struct nvme_id_ns *id_ns;
 	u8 *response;
 
-	cdb16 = IS_READ_CAP_16(cmd);
 	if (cdb16) {
-		alloc_len = GET_READ_CAP_16_ALLOC_LENGTH(cmd);
+		alloc_len = get_unaligned_be32(&cmd[10]);
 		resp_size = READ_CAP_16_RESP_SIZE;
+	} else {
+		alloc_len = READ_CAP_10_RESP_SIZE;
+		resp_size = READ_CAP_10_RESP_SIZE;
 	}
 
-	mem = dma_alloc_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns),
-							&dma_addr, GFP_KERNEL);
-	if (mem == NULL) {
-		res = -ENOMEM;
-		goto out;
-	}
-	/* nvme ns identify */
-	nvme_sc = nvme_identify(dev, ns->ns_id, 0, dma_addr);
+	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
-		goto out_dma;
-	if (nvme_sc) {
-		res = nvme_sc;
-		goto out_dma;
-	}
-	id_ns = mem;
+		return res;	
 
 	response = kzalloc(resp_size, GFP_KERNEL);
 	if (response == NULL) {
 		res = -ENOMEM;
-		goto out_dma;
+		goto out_free_id;
 	}
 	nvme_trans_fill_read_cap(response, id_ns, cdb16);
 
@@ -2515,72 +2068,53 @@ static int nvme_trans_read_capacity(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
 
 	kfree(response);
- out_dma:
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ns), mem,
-			  dma_addr);
- out:
+ out_free_id:
+	kfree(id_ns);
 	return res;
 }
 
 static int nvme_trans_report_luns(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	int nvme_sc;
 	u32 alloc_len, xfer_len, resp_size;
-	u8 select_report;
 	u8 *response;
 	struct nvme_dev *dev = ns->dev;
-	dma_addr_t dma_addr;
-	void *mem;
 	struct nvme_id_ctrl *id_ctrl;
 	u32 ll_length, lun_id;
 	u8 lun_id_offset = REPORT_LUNS_FIRST_LUN_OFFSET;
 	__be32 tmp_len;
 
-	alloc_len = GET_REPORT_LUNS_ALLOC_LENGTH(cmd);
-	select_report = GET_U8_FROM_CDB(cmd, REPORT_LUNS_SR_OFFSET);
-
-	if ((select_report != ALL_LUNS_RETURNED) &&
-	    (select_report != ALL_WELL_KNOWN_LUNS_RETURNED) &&
-	    (select_report != RESTRICTED_LUNS_RETURNED)) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+	switch (cmd[2]) {
+	default:
+		return nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		goto out;
-	} else {
-		/* NVMe Controller Identify */
-		mem = dma_alloc_coherent(&dev->pci_dev->dev,
-					sizeof(struct nvme_id_ctrl),
-					&dma_addr, GFP_KERNEL);
-		if (mem == NULL) {
-			res = -ENOMEM;
-			goto out;
-		}
-		nvme_sc = nvme_identify(dev, 0, 1, dma_addr);
+	case ALL_LUNS_RETURNED:
+	case ALL_WELL_KNOWN_LUNS_RETURNED:
+	case RESTRICTED_LUNS_RETURNED:
+		nvme_sc = nvme_identify_ctrl(dev, &id_ctrl);
 		res = nvme_trans_status_code(hdr, nvme_sc);
 		if (res)
-			goto out_dma;
-		if (nvme_sc) {
-			res = nvme_sc;
-			goto out_dma;
-		}
-		id_ctrl = mem;
+			return res;
+
 		ll_length = le32_to_cpu(id_ctrl->nn) * LUN_ENTRY_SIZE;
 		resp_size = ll_length + LUN_DATA_HEADER_SIZE;
 
+		alloc_len = get_unaligned_be32(&cmd[6]);
 		if (alloc_len < resp_size) {
 			res = nvme_trans_completion(hdr,
 					SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-			goto out_dma;
+			goto out_free_id;
 		}
 
 		response = kzalloc(resp_size, GFP_KERNEL);
 		if (response == NULL) {
 			res = -ENOMEM;
-			goto out_dma;
+			goto out_free_id;
 		}
 
 		/* The first LUN ID will always be 0 per the SAM spec */
@@ -2601,24 +2135,21 @@ static int nvme_trans_report_luns(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	res = nvme_trans_copy_to_user(hdr, response, xfer_len);
 
 	kfree(response);
- out_dma:
-	dma_free_coherent(&dev->pci_dev->dev, sizeof(struct nvme_id_ctrl), mem,
-			  dma_addr);
- out:
+ out_free_id:
+	kfree(id_ctrl);
 	return res;
 }
 
 static int nvme_trans_request_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	u8 alloc_len, xfer_len, resp_size;
 	u8 desc_format;
 	u8 *response;
 
-	alloc_len = GET_REQUEST_SENSE_ALLOC_LENGTH(cmd);
-	desc_format = GET_U8_FROM_CDB(cmd, REQUEST_SENSE_DESC_OFFSET);
-	desc_format &= REQUEST_SENSE_DESC_MASK;
+	desc_format = cmd[1] & 0x01;
+	alloc_len = cmd[4];
 
 	resp_size = ((desc_format) ? (DESC_FMT_SENSE_DATA_SIZE) :
 					(FIXED_FMT_SENSE_DATA_SIZE));
@@ -2628,7 +2159,7 @@ static int nvme_trans_request_sense(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		goto out;
 	}
 
-	if (desc_format == DESCRIPTOR_FORMAT_SENSE_DATA_TYPE) {
+	if (desc_format) {
 		/* Descriptor Format Sense Data */
 		response[0] = DESC_FORMAT_SENSE_DATA;
 		response[1] = NO_SENSE;
@@ -2667,95 +2198,58 @@ static int nvme_trans_security_protocol(struct nvme_ns *ns,
 				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 }
 
-static int nvme_trans_start_stop(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-							u8 *cmd)
+static int nvme_trans_synchronize_cache(struct nvme_ns *ns,
+					struct sg_io_hdr *hdr)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
 	int nvme_sc;
 	struct nvme_command c;
-	u8 immed, pcmod, pc, no_flush, start;
 
-	immed = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_IMMED_OFFSET);
-	pcmod = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_POWER_COND_MOD_OFFSET);
-	pc = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_POWER_COND_OFFSET);
-	no_flush = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_NO_FLUSH_OFFSET);
-	start = GET_U8_FROM_CDB(cmd, START_STOP_UNIT_CDB_START_OFFSET);
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = nvme_cmd_flush;
+	c.common.nsid = cpu_to_le32(ns->ns_id);
 
-	immed &= START_STOP_UNIT_CDB_IMMED_MASK;
-	pcmod &= START_STOP_UNIT_CDB_POWER_COND_MOD_MASK;
-	pc = (pc & START_STOP_UNIT_CDB_POWER_COND_MASK) >> NIBBLE_SHIFT;
-	no_flush &= START_STOP_UNIT_CDB_NO_FLUSH_MASK;
-	start &= START_STOP_UNIT_CDB_START_MASK;
+	nvme_sc = nvme_submit_sync_cmd(ns->queue, &c, NULL, 0);
+	return nvme_trans_status_code(hdr, nvme_sc);
+}
+
+static int nvme_trans_start_stop(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+							u8 *cmd)
+{
+	u8 immed, pcmod, pc, no_flush, start;
+
+	immed = cmd[1] & 0x01;
+	pcmod = cmd[3] & 0x0f;
+	pc = (cmd[4] & 0xf0) >> 4;
+	no_flush = cmd[4] & 0x04;
+	start = cmd[4] & 0x01;
 
 	if (immed != 0) {
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+		return nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 	} else {
 		if (no_flush == 0) {
 			/* Issue NVME FLUSH command prior to START STOP UNIT */
-			memset(&c, 0, sizeof(c));
-			c.common.opcode = nvme_cmd_flush;
-			c.common.nsid = cpu_to_le32(ns->ns_id);
-
-			nvme_sc = nvme_submit_io_cmd(ns->dev, ns, &c, NULL);
-			res = nvme_trans_status_code(hdr, nvme_sc);
+			int res = nvme_trans_synchronize_cache(ns, hdr);
 			if (res)
-				goto out;
-			if (nvme_sc) {
-				res = nvme_sc;
-				goto out;
-			}
+				return res;
 		}
 		/* Setup the expected power state transition */
-		res = nvme_trans_power_state(ns, hdr, pc, pcmod, start);
+		return nvme_trans_power_state(ns, hdr, pc, pcmod, start);
 	}
-
- out:
-	return res;
-}
-
-static int nvme_trans_synchronize_cache(struct nvme_ns *ns,
-					struct sg_io_hdr *hdr, u8 *cmd)
-{
-	int res = SNTI_TRANSLATION_SUCCESS;
-	int nvme_sc;
-	struct nvme_command c;
-
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = nvme_cmd_flush;
-	c.common.nsid = cpu_to_le32(ns->ns_id);
-
-	nvme_sc = nvme_submit_io_cmd(ns->dev, ns, &c, NULL);
-
-	res = nvme_trans_status_code(hdr, nvme_sc);
-	if (res)
-		goto out;
-	if (nvme_sc)
-		res = nvme_sc;
-
- out:
-	return res;
 }
 
 static int nvme_trans_format_unit(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res;
 	u8 parm_hdr_len = 0;
 	u8 nvme_pf_code = 0;
 	u8 format_prot_info, long_list, format_data;
 
-	format_prot_info = GET_U8_FROM_CDB(cmd,
-				FORMAT_UNIT_CDB_FORMAT_PROT_INFO_OFFSET);
-	long_list = GET_U8_FROM_CDB(cmd, FORMAT_UNIT_CDB_LONG_LIST_OFFSET);
-	format_data = GET_U8_FROM_CDB(cmd, FORMAT_UNIT_CDB_FORMAT_DATA_OFFSET);
-
-	format_prot_info = (format_prot_info &
-				FORMAT_UNIT_CDB_FORMAT_PROT_INFO_MASK) >>
-				FORMAT_UNIT_CDB_FORMAT_PROT_INFO_SHIFT;
-	long_list &= FORMAT_UNIT_CDB_LONG_LIST_MASK;
-	format_data &= FORMAT_UNIT_CDB_FORMAT_DATA_MASK;
+	format_prot_info = (cmd[1] & 0xc0) >> 6;
+	long_list = cmd[1] & 0x20;
+	format_data = cmd[1] & 0x10;
 
 	if (format_data != 0) {
 		if (format_prot_info != 0) {
@@ -2779,16 +2273,16 @@ static int nvme_trans_format_unit(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	if (parm_hdr_len > 0) {
 		res = nvme_trans_fmt_get_parm_header(hdr, parm_hdr_len,
 					format_prot_info, &nvme_pf_code);
-		if (res != SNTI_TRANSLATION_SUCCESS)
+		if (res)
 			goto out;
 	}
 
 	/* Attempt to activate any previously downloaded firmware image */
-	res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_activate_fw, 0, 0, 0);
+	res = nvme_trans_send_activate_fw_cmd(ns, hdr, 0);
 
 	/* Determine Block size and count and send format command */
 	res = nvme_trans_fmt_set_blk_size_count(ns, hdr);
-	if (res != SNTI_TRANSLATION_SUCCESS)
+	if (res)
 		goto out;
 
 	res = nvme_trans_fmt_send_cmd(ns, hdr, nvme_pf_code);
@@ -2801,28 +2295,24 @@ static int nvme_trans_test_unit_ready(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr,
 					u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
 	struct nvme_dev *dev = ns->dev;
 
 	if (!(readl(&dev->bar->csts) & NVME_CSTS_RDY))
-		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
+		return nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					    NOT_READY, SCSI_ASC_LUN_NOT_READY,
 					    SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 	else
-		res = nvme_trans_completion(hdr, SAM_STAT_GOOD, NO_SENSE, 0, 0);
-
-	return res;
+		return nvme_trans_completion(hdr, SAM_STAT_GOOD, NO_SENSE, 0, 0);
 }
 
 static int nvme_trans_write_buffer(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	int res = SNTI_TRANSLATION_SUCCESS;
+	int res = 0;
 	u32 buffer_offset, parm_list_length;
 	u8 buffer_id, mode;
 
-	parm_list_length =
-		GET_U24_FROM_CDB(cmd, WRITE_BUFFER_CDB_PARM_LIST_LENGTH_OFFSET);
+	parm_list_length = get_unaligned_be24(&cmd[6]);
 	if (parm_list_length % BYTES_TO_DWORDS != 0) {
 		/* NVMe expects Firmware file to be a whole number of DWORDS */
 		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
@@ -2830,38 +2320,32 @@ static int nvme_trans_write_buffer(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 		goto out;
 	}
-	buffer_id = GET_U8_FROM_CDB(cmd, WRITE_BUFFER_CDB_BUFFER_ID_OFFSET);
+	buffer_id = cmd[2];
 	if (buffer_id > NVME_MAX_FIRMWARE_SLOT) {
 		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
 					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 		goto out;
 	}
-	mode = GET_U8_FROM_CDB(cmd, WRITE_BUFFER_CDB_MODE_OFFSET) &
-						WRITE_BUFFER_CDB_MODE_MASK;
-	buffer_offset =
-		GET_U24_FROM_CDB(cmd, WRITE_BUFFER_CDB_BUFFER_OFFSET_OFFSET);
+	mode = cmd[1] & 0x1f;
+	buffer_offset = get_unaligned_be24(&cmd[3]);
 
 	switch (mode) {
 	case DOWNLOAD_SAVE_ACTIVATE:
-		res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_download_fw,
+		res = nvme_trans_send_download_fw_cmd(ns, hdr, nvme_admin_download_fw,
 						parm_list_length, buffer_offset,
 						buffer_id);
-		if (res != SNTI_TRANSLATION_SUCCESS)
+		if (res)
 			goto out;
-		res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_activate_fw,
-						parm_list_length, buffer_offset,
-						buffer_id);
+		res = nvme_trans_send_activate_fw_cmd(ns, hdr, buffer_id);
 		break;
 	case DOWNLOAD_SAVE_DEFER_ACTIVATE:
-		res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_download_fw,
+		res = nvme_trans_send_download_fw_cmd(ns, hdr, nvme_admin_download_fw,
 						parm_list_length, buffer_offset,
 						buffer_id);
 		break;
 	case ACTIVATE_DEFERRED_MICROCODE:
-		res = nvme_trans_send_fw_cmd(ns, hdr, nvme_admin_activate_fw,
-						parm_list_length, buffer_offset,
-						buffer_id);
+		res = nvme_trans_send_activate_fw_cmd(ns, hdr, buffer_id);
 		break;
 	default:
 		res = nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
@@ -2890,15 +2374,13 @@ struct scsi_unmap_parm_list {
 static int nvme_trans_unmap(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 							u8 *cmd)
 {
-	struct nvme_dev *dev = ns->dev;
 	struct scsi_unmap_parm_list *plist;
 	struct nvme_dsm_range *range;
 	struct nvme_command c;
-	int i, nvme_sc, res = -ENOMEM;
+	int i, nvme_sc, res;
 	u16 ndesc, list_len;
-	dma_addr_t dma_addr;
 
-	list_len = GET_U16_FROM_CDB(cmd, UNMAP_CDB_PARAM_LIST_LENGTH_OFFSET);
+	list_len = get_unaligned_be16(&cmd[7]);
 	if (!list_len)
 		return -EINVAL;
 
@@ -2907,7 +2389,7 @@ static int nvme_trans_unmap(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		return -ENOMEM;
 
 	res = nvme_trans_copy_from_user(hdr, plist, list_len);
-	if (res != SNTI_TRANSLATION_SUCCESS)
+	if (res)
 		goto out;
 
 	ndesc = be16_to_cpu(plist->unmap_blk_desc_data_len) >> 4;
@@ -2916,10 +2398,11 @@ static int nvme_trans_unmap(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		goto out;
 	}
 
-	range = dma_alloc_coherent(&dev->pci_dev->dev, ndesc * sizeof(*range),
-							&dma_addr, GFP_KERNEL);
-	if (!range)
+	range = kcalloc(ndesc, sizeof(*range), GFP_KERNEL);
+	if (!range) {
+		res = -ENOMEM;
 		goto out;
+	}
 
 	for (i = 0; i < ndesc; i++) {
 		range[i].nlb = cpu_to_le32(be32_to_cpu(plist->desc[i].nlb));
@@ -2930,15 +2413,14 @@ static int nvme_trans_unmap(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	memset(&c, 0, sizeof(c));
 	c.dsm.opcode = nvme_cmd_dsm;
 	c.dsm.nsid = cpu_to_le32(ns->ns_id);
-	c.dsm.prp1 = cpu_to_le64(dma_addr);
 	c.dsm.nr = cpu_to_le32(ndesc - 1);
 	c.dsm.attributes = cpu_to_le32(NVME_DSMGMT_AD);
 
-	nvme_sc = nvme_submit_io_cmd(dev, ns, &c, NULL);
+	nvme_sc = nvme_submit_sync_cmd(ns->queue, &c, range,
+			ndesc * sizeof(*range));
 	res = nvme_trans_status_code(hdr, nvme_sc);
 
-	dma_free_coherent(&dev->pci_dev->dev, ndesc * sizeof(*range),
-							range, dma_addr);
+	kfree(range);
  out:
 	kfree(plist);
 	return res;
@@ -2993,13 +2475,16 @@ static int nvme_scsi_translate(struct nvme_ns *ns, struct sg_io_hdr *hdr)
 		retcode = nvme_trans_mode_sense(ns, hdr, cmd);
 		break;
 	case READ_CAPACITY:
-		retcode = nvme_trans_read_capacity(ns, hdr, cmd);
+		retcode = nvme_trans_read_capacity(ns, hdr, cmd, 0);
 		break;
 	case SERVICE_ACTION_IN_16:
-		if (IS_READ_CAP_16(cmd))
-			retcode = nvme_trans_read_capacity(ns, hdr, cmd);
-		else
+		switch (cmd[1]) {
+		case SAI_READ_CAPACITY_16:
+			retcode = nvme_trans_read_capacity(ns, hdr, cmd, 1);
+			break;
+		default:
 			goto out;
+		}
 		break;
 	case REPORT_LUNS:
 		retcode = nvme_trans_report_luns(ns, hdr, cmd);
@@ -3015,7 +2500,7 @@ static int nvme_scsi_translate(struct nvme_ns *ns, struct sg_io_hdr *hdr)
 		retcode = nvme_trans_start_stop(ns, hdr, cmd);
 		break;
 	case SYNCHRONIZE_CACHE:
-		retcode = nvme_trans_synchronize_cache(ns, hdr, cmd);
+		retcode = nvme_trans_synchronize_cache(ns, hdr);
 		break;
 	case FORMAT_UNIT:
 		retcode = nvme_trans_format_unit(ns, hdr, cmd);
@@ -3053,15 +2538,16 @@ int nvme_sg_io(struct nvme_ns *ns, struct sg_io_hdr __user *u_hdr)
 	if (hdr.cmd_len > BLK_MAX_CDB)
 		return -EINVAL;
 
+	/*
+	 * A positive return code means a NVMe status, which has been
+	 * translated to sense data.
+	 */
 	retcode = nvme_scsi_translate(ns, &hdr);
 	if (retcode < 0)
 		return retcode;
-	if (retcode > 0)
-		retcode = SNTI_TRANSLATION_SUCCESS;
 	if (copy_to_user(u_hdr, &hdr, sizeof(sg_io_hdr_t)) > 0)
 		return -EFAULT;
-
-	return retcode;
+	return 0;
 }
 
 int nvme_sg_get_version_num(int __user *ip)
diff --git a/drivers/block/paride/paride.c b/drivers/block/paride/paride.c
index 48c50f11f63b..0e287993b778 100644
--- a/drivers/block/paride/paride.c
+++ b/drivers/block/paride/paride.c
@@ -30,6 +30,7 @@
 #include <linux/wait.h>
 #include <linux/sched.h>	/* TASK_* */
 #include <linux/parport.h>
+#include <linux/slab.h>
 
 #include "paride.h"
 
@@ -244,17 +245,19 @@ void paride_unregister(PIP * pr)
 
 EXPORT_SYMBOL(paride_unregister);
 
-static int pi_register_parport(PIA * pi, int verbose)
+static int pi_register_parport(PIA *pi, int verbose, int unit)
 {
 	struct parport *port;
+	struct pardev_cb par_cb;
 
 	port = parport_find_base(pi->port);
 	if (!port)
 		return 0;
-
-	pi->pardev = parport_register_device(port,
-					     pi->device, NULL,
-					     pi_wake_up, NULL, 0, (void *) pi);
+	memset(&par_cb, 0, sizeof(par_cb));
+	par_cb.wakeup = pi_wake_up;
+	par_cb.private = (void *)pi;
+	pi->pardev = parport_register_dev_model(port, pi->device, &par_cb,
+						unit);
 	parport_put_port(port);
 	if (!pi->pardev)
 		return 0;
@@ -311,7 +314,7 @@ static int pi_probe_unit(PIA * pi, int unit, char *scratch, int verbose)
 		e = pi->proto->max_units;
 	}
 
-	if (!pi_register_parport(pi, verbose))
+	if (!pi_register_parport(pi, verbose, s))
 		return 0;
 
 	if (pi->proto->test_port) {
@@ -432,3 +435,45 @@ int pi_init(PIA * pi, int autoprobe, int port, int mode,
 }
 
 EXPORT_SYMBOL(pi_init);
+
+static int pi_probe(struct pardevice *par_dev)
+{
+	struct device_driver *drv = par_dev->dev.driver;
+	int len = strlen(drv->name);
+
+	if (strncmp(par_dev->name, drv->name, len))
+		return -ENODEV;
+
+	return 0;
+}
+
+void *pi_register_driver(char *name)
+{
+	struct parport_driver *parp_drv;
+	int ret;
+
+	parp_drv = kzalloc(sizeof(*parp_drv), GFP_KERNEL);
+	if (!parp_drv)
+		return NULL;
+
+	parp_drv->name = name;
+	parp_drv->probe = pi_probe;
+	parp_drv->devmodel = true;
+
+	ret = parport_register_driver(parp_drv);
+	if (ret) {
+		kfree(parp_drv);
+		return NULL;
+	}
+	return (void *)parp_drv;
+}
+EXPORT_SYMBOL(pi_register_driver);
+
+void pi_unregister_driver(void *_drv)
+{
+	struct parport_driver *drv = _drv;
+
+	parport_unregister_driver(drv);
+	kfree(drv);
+}
+EXPORT_SYMBOL(pi_unregister_driver);
diff --git a/drivers/block/paride/paride.h b/drivers/block/paride/paride.h
index 2bddbf45518b..ddb9e589da7f 100644
--- a/drivers/block/paride/paride.h
+++ b/drivers/block/paride/paride.h
@@ -165,6 +165,8 @@ typedef struct pi_protocol PIP;
 
 extern int paride_register( PIP * );
 extern void paride_unregister ( PIP * );
+void *pi_register_driver(char *);
+void pi_unregister_driver(void *);
 
 #endif /* __DRIVERS_PARIDE_H__ */
 /* end of paride.h */
diff --git a/drivers/block/paride/pcd.c b/drivers/block/paride/pcd.c
index 3b7c9f1be484..93362362aa55 100644
--- a/drivers/block/paride/pcd.c
+++ b/drivers/block/paride/pcd.c
@@ -221,6 +221,7 @@ static int pcd_busy;		/* request being processed ? */
 static int pcd_sector;		/* address of next requested sector */
 static int pcd_count;		/* number of blocks still to do */
 static char *pcd_buf;		/* buffer for request in progress */
+static void *par_drv;		/* reference of parport driver */
 
 /* kernel glue structures */
 
@@ -690,6 +691,12 @@ static int pcd_detect(void)
 	printk("%s: %s version %s, major %d, nice %d\n",
 	       name, name, PCD_VERSION, major, nice);
 
+	par_drv = pi_register_driver(name);
+	if (!par_drv) {
+		pr_err("failed to register %s driver\n", name);
+		return -1;
+	}
+
 	k = 0;
 	if (pcd_drive_count == 0) { /* nothing spec'd - so autoprobe for 1 */
 		cd = pcd;
@@ -723,6 +730,7 @@ static int pcd_detect(void)
 	printk("%s: No CD-ROM drive found\n", name);
 	for (unit = 0, cd = pcd; unit < PCD_UNITS; unit++, cd++)
 		put_disk(cd->disk);
+	pi_unregister_driver(par_drv);
 	return -1;
 }
 
@@ -984,6 +992,7 @@ static void __exit pcd_exit(void)
 	}
 	blk_cleanup_queue(pcd_queue);
 	unregister_blkdev(major, name);
+	pi_unregister_driver(par_drv);
 }
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/block/paride/pd.c b/drivers/block/paride/pd.c
index d48715b287e6..b9242d78283d 100644
--- a/drivers/block/paride/pd.c
+++ b/drivers/block/paride/pd.c
@@ -247,6 +247,8 @@ static char *pd_errs[17] = { "ERR", "INDEX", "ECC", "DRQ", "SEEK", "WRERR",
 	"IDNF", "MC", "UNC", "???", "TMO"
 };
 
+static void *par_drv;		/* reference of parport driver */
+
 static inline int status_reg(struct pd_unit *disk)
 {
 	return pi_read_regr(disk->pi, 1, 6);
@@ -442,7 +444,7 @@ static char *pd_buf;		/* buffer for request in progress */
 
 static enum action do_pd_io_start(void)
 {
-	if (pd_req->cmd_type == REQ_TYPE_SPECIAL) {
+	if (pd_req->cmd_type == REQ_TYPE_DRV_PRIV) {
 		phase = pd_special;
 		return pd_special();
 	}
@@ -725,7 +727,7 @@ static int pd_special_command(struct pd_unit *disk,
 	if (IS_ERR(rq))
 		return PTR_ERR(rq);
 
-	rq->cmd_type = REQ_TYPE_SPECIAL;
+	rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	rq->special = func;
 
 	err = blk_execute_rq(disk->gd->queue, disk->gd, rq, 0);
@@ -872,6 +874,12 @@ static int pd_detect(void)
 			pd_drive_count++;
 	}
 
+	par_drv = pi_register_driver(name);
+	if (!par_drv) {
+		pr_err("failed to register %s driver\n", name);
+		return -1;
+	}
+
 	if (pd_drive_count == 0) { /* nothing spec'd - so autoprobe for 1 */
 		disk = pd;
 		if (pi_init(disk->pi, 1, -1, -1, -1, -1, -1, pd_scratch,
@@ -902,8 +910,10 @@ static int pd_detect(void)
 			found = 1;
 		}
 	}
-	if (!found)
+	if (!found) {
 		printk("%s: no valid drive found\n", name);
+		pi_unregister_driver(par_drv);
+	}
 	return found;
 }
 
diff --git a/drivers/block/paride/pf.c b/drivers/block/paride/pf.c
index 9a15fd3c9349..7a7d977a76c5 100644
--- a/drivers/block/paride/pf.c
+++ b/drivers/block/paride/pf.c
@@ -264,6 +264,7 @@ static int pf_cmd;		/* current command READ/WRITE */
 static struct pf_unit *pf_current;/* unit of current request */
 static int pf_mask;		/* stopper for pseudo-int */
 static char *pf_buf;		/* buffer for request in progress */
+static void *par_drv;		/* reference of parport driver */
 
 /* kernel glue structures */
 
@@ -703,6 +704,11 @@ static int pf_detect(void)
 	printk("%s: %s version %s, major %d, cluster %d, nice %d\n",
 	       name, name, PF_VERSION, major, cluster, nice);
 
+	par_drv = pi_register_driver(name);
+	if (!par_drv) {
+		pr_err("failed to register %s driver\n", name);
+		return -1;
+	}
 	k = 0;
 	if (pf_drive_count == 0) {
 		if (pi_init(pf->pi, 1, -1, -1, -1, -1, -1, pf_scratch, PI_PF,
@@ -735,6 +741,7 @@ static int pf_detect(void)
 	printk("%s: No ATAPI disk detected\n", name);
 	for (pf = units, unit = 0; unit < PF_UNITS; pf++, unit++)
 		put_disk(pf->disk);
+	pi_unregister_driver(par_drv);
 	return -1;
 }
 
diff --git a/drivers/block/paride/pg.c b/drivers/block/paride/pg.c
index 876d0c3eaf58..bfbd4c852dd9 100644
--- a/drivers/block/paride/pg.c
+++ b/drivers/block/paride/pg.c
@@ -227,6 +227,7 @@ static int pg_identify(struct pg *dev, int log);
 static char pg_scratch[512];	/* scratch block buffer */
 
 static struct class *pg_class;
+static void *par_drv;		/* reference of parport driver */
 
 /* kernel glue structures */
 
@@ -481,6 +482,12 @@ static int pg_detect(void)
 
 	printk("%s: %s version %s, major %d\n", name, name, PG_VERSION, major);
 
+	par_drv = pi_register_driver(name);
+	if (!par_drv) {
+		pr_err("failed to register %s driver\n", name);
+		return -1;
+	}
+
 	k = 0;
 	if (pg_drive_count == 0) {
 		if (pi_init(dev->pi, 1, -1, -1, -1, -1, -1, pg_scratch,
@@ -511,6 +518,7 @@ static int pg_detect(void)
 	if (k)
 		return 0;
 
+	pi_unregister_driver(par_drv);
 	printk("%s: No ATAPI device detected\n", name);
 	return -1;
 }
diff --git a/drivers/block/paride/pt.c b/drivers/block/paride/pt.c
index 2596042eb987..1740d75e8a32 100644
--- a/drivers/block/paride/pt.c
+++ b/drivers/block/paride/pt.c
@@ -232,6 +232,7 @@ static int pt_identify(struct pt_unit *tape);
 static struct pt_unit pt[PT_UNITS];
 
 static char pt_scratch[512];	/* scratch block buffer */
+static void *par_drv;		/* reference of parport driver */
 
 /* kernel glue structures */
 
@@ -605,6 +606,12 @@ static int pt_detect(void)
 
 	printk("%s: %s version %s, major %d\n", name, name, PT_VERSION, major);
 
+	par_drv = pi_register_driver(name);
+	if (!par_drv) {
+		pr_err("failed to register %s driver\n", name);
+		return -1;
+	}
+
 	specified = 0;
 	for (unit = 0; unit < PT_UNITS; unit++) {
 		struct pt_unit *tape = &pt[unit];
@@ -644,6 +651,7 @@ static int pt_detect(void)
 	if (found)
 		return 0;
 
+	pi_unregister_driver(par_drv);
 	printk("%s: No ATAPI tape drive detected\n", name);
 	return -1;
 }
diff --git a/drivers/block/pktcdvd.c b/drivers/block/pktcdvd.c
index 09e628dafd9d..4c20c228184c 100644
--- a/drivers/block/pktcdvd.c
+++ b/drivers/block/pktcdvd.c
@@ -61,6 +61,7 @@
 #include <linux/freezer.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
+#include <linux/backing-dev.h>
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_ioctl.h>
 #include <scsi/scsi.h>
diff --git a/drivers/block/ps3vram.c b/drivers/block/ps3vram.c
index ef45cfb98fd2..b1612eb16172 100644
--- a/drivers/block/ps3vram.c
+++ b/drivers/block/ps3vram.c
@@ -1,5 +1,5 @@
 /*
- * ps3vram - Use extra PS3 video ram as MTD block device.
+ * ps3vram - Use extra PS3 video ram as block device.
  *
  * Copyright 2009 Sony Corporation
  *
@@ -73,8 +73,8 @@ struct ps3vram_priv {
 
 	u64 memory_handle;
 	u64 context_handle;
-	u32 *ctrl;
-	void *reports;
+	u32 __iomem *ctrl;
+	void __iomem *reports;
 	u8 *xdr_buf;
 
 	u32 *fifo_base;
@@ -104,7 +104,7 @@ static char *size = "256M";
 module_param(size, charp, 0);
 MODULE_PARM_DESC(size, "memory size");
 
-static u32 *ps3vram_get_notifier(void *reports, int notifier)
+static u32 __iomem *ps3vram_get_notifier(void __iomem *reports, int notifier)
 {
 	return reports + DMA_NOTIFIER_OFFSET_BASE +
 	       DMA_NOTIFIER_SIZE * notifier;
@@ -113,22 +113,22 @@ static u32 *ps3vram_get_notifier(void *reports, int notifier)
 static void ps3vram_notifier_reset(struct ps3_system_bus_device *dev)
 {
 	struct ps3vram_priv *priv = ps3_system_bus_get_drvdata(dev);
-	u32 *notify = ps3vram_get_notifier(priv->reports, NOTIFIER);
+	u32 __iomem *notify = ps3vram_get_notifier(priv->reports, NOTIFIER);
 	int i;
 
 	for (i = 0; i < 4; i++)
-		notify[i] = 0xffffffff;
+		iowrite32be(0xffffffff, notify + i);
 }
 
 static int ps3vram_notifier_wait(struct ps3_system_bus_device *dev,
 				 unsigned int timeout_ms)
 {
 	struct ps3vram_priv *priv = ps3_system_bus_get_drvdata(dev);
-	u32 *notify = ps3vram_get_notifier(priv->reports, NOTIFIER);
+	u32 __iomem *notify = ps3vram_get_notifier(priv->reports, NOTIFIER);
 	unsigned long timeout;
 
 	for (timeout = 20; timeout; timeout--) {
-		if (!notify[3])
+		if (!ioread32be(notify + 3))
 			return 0;
 		udelay(10);
 	}
@@ -136,7 +136,7 @@ static int ps3vram_notifier_wait(struct ps3_system_bus_device *dev,
 	timeout = jiffies + msecs_to_jiffies(timeout_ms);
 
 	do {
-		if (!notify[3])
+		if (!ioread32be(notify + 3))
 			return 0;
 		msleep(1);
 	} while (time_before(jiffies, timeout));
@@ -148,8 +148,8 @@ static void ps3vram_init_ring(struct ps3_system_bus_device *dev)
 {
 	struct ps3vram_priv *priv = ps3_system_bus_get_drvdata(dev);
 
-	priv->ctrl[CTRL_PUT] = FIFO_BASE + FIFO_OFFSET;
-	priv->ctrl[CTRL_GET] = FIFO_BASE + FIFO_OFFSET;
+	iowrite32be(FIFO_BASE + FIFO_OFFSET, priv->ctrl + CTRL_PUT);
+	iowrite32be(FIFO_BASE + FIFO_OFFSET, priv->ctrl + CTRL_GET);
 }
 
 static int ps3vram_wait_ring(struct ps3_system_bus_device *dev,
@@ -159,14 +159,14 @@ static int ps3vram_wait_ring(struct ps3_system_bus_device *dev,
 	unsigned long timeout = jiffies + msecs_to_jiffies(timeout_ms);
 
 	do {
-		if (priv->ctrl[CTRL_PUT] == priv->ctrl[CTRL_GET])
+		if (ioread32be(priv->ctrl + CTRL_PUT) == ioread32be(priv->ctrl + CTRL_GET))
 			return 0;
 		msleep(1);
 	} while (time_before(jiffies, timeout));
 
 	dev_warn(&dev->core, "FIFO timeout (%08x/%08x/%08x)\n",
-		 priv->ctrl[CTRL_PUT], priv->ctrl[CTRL_GET],
-		 priv->ctrl[CTRL_TOP]);
+		 ioread32be(priv->ctrl + CTRL_PUT), ioread32be(priv->ctrl + CTRL_GET),
+		 ioread32be(priv->ctrl + CTRL_TOP));
 
 	return -ETIMEDOUT;
 }
@@ -189,7 +189,7 @@ static void ps3vram_rewind_ring(struct ps3_system_bus_device *dev)
 
 	ps3vram_out_ring(priv, 0x20000000 | (FIFO_BASE + FIFO_OFFSET));
 
-	priv->ctrl[CTRL_PUT] = FIFO_BASE + FIFO_OFFSET;
+	iowrite32be(FIFO_BASE + FIFO_OFFSET, priv->ctrl + CTRL_PUT);
 
 	/* asking the HV for a blit will kick the FIFO */
 	status = lv1_gpu_fb_blit(priv->context_handle, 0, 0, 0, 0);
@@ -207,8 +207,8 @@ static void ps3vram_fire_ring(struct ps3_system_bus_device *dev)
 
 	mutex_lock(&ps3_gpu_mutex);
 
-	priv->ctrl[CTRL_PUT] = FIFO_BASE + FIFO_OFFSET +
-			       (priv->fifo_ptr - priv->fifo_base) * sizeof(u32);
+	iowrite32be(FIFO_BASE + FIFO_OFFSET + (priv->fifo_ptr - priv->fifo_base)
+		* sizeof(u32), priv->ctrl + CTRL_PUT);
 
 	/* asking the HV for a blit will kick the FIFO */
 	status = lv1_gpu_fb_blit(priv->context_handle, 0, 0, 0, 0);
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index ec6c5c6e1ac9..d94529d5c8e9 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -346,6 +346,7 @@ struct rbd_device {
 	struct rbd_image_header	header;
 	unsigned long		flags;		/* possibly lock protected */
 	struct rbd_spec		*spec;
+	struct rbd_options	*opts;
 
 	char			*header_name;
 
@@ -724,34 +725,36 @@ static struct rbd_client *rbd_client_find(struct ceph_options *ceph_opts)
 }
 
 /*
- * mount options
+ * (Per device) rbd map options
  */
 enum {
+	Opt_queue_depth,
 	Opt_last_int,
 	/* int args above */
 	Opt_last_string,
 	/* string args above */
 	Opt_read_only,
 	Opt_read_write,
-	/* Boolean args above */
-	Opt_last_bool,
+	Opt_err
 };
 
 static match_table_t rbd_opts_tokens = {
+	{Opt_queue_depth, "queue_depth=%d"},
 	/* int args above */
 	/* string args above */
 	{Opt_read_only, "read_only"},
 	{Opt_read_only, "ro"},		/* Alternate spelling */
 	{Opt_read_write, "read_write"},
 	{Opt_read_write, "rw"},		/* Alternate spelling */
-	/* Boolean args above */
-	{-1, NULL}
+	{Opt_err, NULL}
 };
 
 struct rbd_options {
+	int	queue_depth;
 	bool	read_only;
 };
 
+#define RBD_QUEUE_DEPTH_DEFAULT	BLKDEV_MAX_RQ
 #define RBD_READ_ONLY_DEFAULT	false
 
 static int parse_rbd_opts_token(char *c, void *private)
@@ -761,27 +764,27 @@ static int parse_rbd_opts_token(char *c, void *private)
 	int token, intval, ret;
 
 	token = match_token(c, rbd_opts_tokens, argstr);
-	if (token < 0)
-		return -EINVAL;
-
 	if (token < Opt_last_int) {
 		ret = match_int(&argstr[0], &intval);
 		if (ret < 0) {
-			pr_err("bad mount option arg (not int) "
-			       "at '%s'\n", c);
+			pr_err("bad mount option arg (not int) at '%s'\n", c);
 			return ret;
 		}
 		dout("got int token %d val %d\n", token, intval);
 	} else if (token > Opt_last_int && token < Opt_last_string) {
-		dout("got string token %d val %s\n", token,
-		     argstr[0].from);
-	} else if (token > Opt_last_string && token < Opt_last_bool) {
-		dout("got Boolean token %d\n", token);
+		dout("got string token %d val %s\n", token, argstr[0].from);
 	} else {
 		dout("got token %d\n", token);
 	}
 
 	switch (token) {
+	case Opt_queue_depth:
+		if (intval < 1) {
+			pr_err("queue_depth out of range\n");
+			return -EINVAL;
+		}
+		rbd_opts->queue_depth = intval;
+		break;
 	case Opt_read_only:
 		rbd_opts->read_only = true;
 		break;
@@ -789,9 +792,10 @@ static int parse_rbd_opts_token(char *c, void *private)
 		rbd_opts->read_only = false;
 		break;
 	default:
-		rbd_assert(false);
-		break;
+		/* libceph prints "bad option" msg */
+		return -EINVAL;
 	}
+
 	return 0;
 }
 
@@ -1563,22 +1567,39 @@ static void rbd_obj_request_end(struct rbd_obj_request *obj_request)
 /*
  * Wait for an object request to complete.  If interrupted, cancel the
  * underlying osd request.
+ *
+ * @timeout: in jiffies, 0 means "wait forever"
  */
-static int rbd_obj_request_wait(struct rbd_obj_request *obj_request)
+static int __rbd_obj_request_wait(struct rbd_obj_request *obj_request,
+				  unsigned long timeout)
 {
-	int ret;
+	long ret;
 
 	dout("%s %p\n", __func__, obj_request);
-
-	ret = wait_for_completion_interruptible(&obj_request->completion);
-	if (ret < 0) {
-		dout("%s %p interrupted\n", __func__, obj_request);
+	ret = wait_for_completion_interruptible_timeout(
+					&obj_request->completion,
+					ceph_timeout_jiffies(timeout));
+	if (ret <= 0) {
+		if (ret == 0)
+			ret = -ETIMEDOUT;
 		rbd_obj_request_end(obj_request);
-		return ret;
+	} else {
+		ret = 0;
 	}
 
-	dout("%s %p done\n", __func__, obj_request);
-	return 0;
+	dout("%s %p ret %d\n", __func__, obj_request, (int)ret);
+	return ret;
+}
+
+static int rbd_obj_request_wait(struct rbd_obj_request *obj_request)
+{
+	return __rbd_obj_request_wait(obj_request, 0);
+}
+
+static int rbd_obj_request_wait_timeout(struct rbd_obj_request *obj_request,
+					unsigned long timeout)
+{
+	return __rbd_obj_request_wait(obj_request, timeout);
 }
 
 static void rbd_img_request_complete(struct rbd_img_request *img_request)
@@ -2001,11 +2022,11 @@ static struct rbd_obj_request *rbd_obj_request_create(const char *object_name,
 	rbd_assert(obj_request_type_valid(type));
 
 	size = strlen(object_name) + 1;
-	name = kmalloc(size, GFP_KERNEL);
+	name = kmalloc(size, GFP_NOIO);
 	if (!name)
 		return NULL;
 
-	obj_request = kmem_cache_zalloc(rbd_obj_request_cache, GFP_KERNEL);
+	obj_request = kmem_cache_zalloc(rbd_obj_request_cache, GFP_NOIO);
 	if (!obj_request) {
 		kfree(name);
 		return NULL;
@@ -2376,7 +2397,7 @@ static void rbd_img_obj_request_fill(struct rbd_obj_request *obj_request,
 	}
 
 	if (opcode == CEPH_OSD_OP_DELETE)
-		osd_req_op_init(osd_request, num_ops, opcode);
+		osd_req_op_init(osd_request, num_ops, opcode, 0);
 	else
 		osd_req_op_extent_init(osd_request, num_ops, opcode,
 				       offset, length, 0, 0);
@@ -2848,7 +2869,7 @@ static int rbd_img_obj_exists_submit(struct rbd_obj_request *obj_request)
 		goto out;
 	stat_request->callback = rbd_img_obj_exists_callback;
 
-	osd_req_op_init(stat_request->osd_req, 0, CEPH_OSD_OP_STAT);
+	osd_req_op_init(stat_request->osd_req, 0, CEPH_OSD_OP_STAT, 0);
 	osd_req_op_raw_data_in_pages(stat_request->osd_req, 0, pages, size, 0,
 					false, false);
 	rbd_osd_req_format_read(stat_request);
@@ -3122,6 +3143,7 @@ static struct rbd_obj_request *rbd_obj_watch_request_helper(
 						bool watch)
 {
 	struct ceph_osd_client *osdc = &rbd_dev->rbd_client->client->osdc;
+	struct ceph_options *opts = osdc->client->options;
 	struct rbd_obj_request *obj_request;
 	int ret;
 
@@ -3148,7 +3170,7 @@ static struct rbd_obj_request *rbd_obj_watch_request_helper(
 	if (ret)
 		goto out;
 
-	ret = rbd_obj_request_wait(obj_request);
+	ret = rbd_obj_request_wait_timeout(obj_request, opts->mount_timeout);
 	if (ret)
 		goto out;
 
@@ -3750,10 +3772,9 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 
 	memset(&rbd_dev->tag_set, 0, sizeof(rbd_dev->tag_set));
 	rbd_dev->tag_set.ops = &rbd_mq_ops;
-	rbd_dev->tag_set.queue_depth = BLKDEV_MAX_RQ;
+	rbd_dev->tag_set.queue_depth = rbd_dev->opts->queue_depth;
 	rbd_dev->tag_set.numa_node = NUMA_NO_NODE;
-	rbd_dev->tag_set.flags =
-		BLK_MQ_F_SHOULD_MERGE | BLK_MQ_F_SG_MERGE;
+	rbd_dev->tag_set.flags = BLK_MQ_F_SHOULD_MERGE | BLK_MQ_F_SG_MERGE;
 	rbd_dev->tag_set.nr_hw_queues = 1;
 	rbd_dev->tag_set.cmd_size = sizeof(struct work_struct);
 
@@ -3773,6 +3794,7 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 	/* set io sizes to object size */
 	segment_size = rbd_obj_bytes(&rbd_dev->header);
 	blk_queue_max_hw_sectors(q, segment_size / SECTOR_SIZE);
+	blk_queue_max_segments(q, segment_size / SECTOR_SIZE);
 	blk_queue_max_segment_size(q, segment_size);
 	blk_queue_io_min(q, segment_size);
 	blk_queue_io_opt(q, segment_size);
@@ -4044,7 +4066,8 @@ static void rbd_spec_free(struct kref *kref)
 }
 
 static struct rbd_device *rbd_dev_create(struct rbd_client *rbdc,
-				struct rbd_spec *spec)
+					 struct rbd_spec *spec,
+					 struct rbd_options *opts)
 {
 	struct rbd_device *rbd_dev;
 
@@ -4058,8 +4081,9 @@ static struct rbd_device *rbd_dev_create(struct rbd_client *rbdc,
 	INIT_LIST_HEAD(&rbd_dev->node);
 	init_rwsem(&rbd_dev->header_rwsem);
 
-	rbd_dev->spec = spec;
 	rbd_dev->rbd_client = rbdc;
+	rbd_dev->spec = spec;
+	rbd_dev->opts = opts;
 
 	/* Initialize the layout used for all rbd requests */
 
@@ -4075,6 +4099,7 @@ static void rbd_dev_destroy(struct rbd_device *rbd_dev)
 {
 	rbd_put_client(rbd_dev->rbd_client);
 	rbd_spec_put(rbd_dev->spec);
+	kfree(rbd_dev->opts);
 	kfree(rbd_dev);
 }
 
@@ -4933,6 +4958,7 @@ static int rbd_add_parse_args(const char *buf,
 		goto out_mem;
 
 	rbd_opts->read_only = RBD_READ_ONLY_DEFAULT;
+	rbd_opts->queue_depth = RBD_QUEUE_DEPTH_DEFAULT;
 
 	copts = ceph_parse_options(options, mon_addrs,
 					mon_addrs + mon_addrs_size - 1,
@@ -4963,8 +4989,8 @@ out_err:
  */
 static int rbd_add_get_pool_id(struct rbd_client *rbdc, const char *pool_name)
 {
+	struct ceph_options *opts = rbdc->client->options;
 	u64 newest_epoch;
-	unsigned long timeout = rbdc->client->options->mount_timeout * HZ;
 	int tries = 0;
 	int ret;
 
@@ -4979,7 +5005,8 @@ again:
 		if (rbdc->client->osdc.osdmap->epoch < newest_epoch) {
 			ceph_monc_request_next_osdmap(&rbdc->client->monc);
 			(void) ceph_monc_wait_osdmap(&rbdc->client->monc,
-						     newest_epoch, timeout);
+						     newest_epoch,
+						     opts->mount_timeout);
 			goto again;
 		} else {
 			/* the osdmap we have is new enough */
@@ -5148,7 +5175,7 @@ static int rbd_dev_probe_parent(struct rbd_device *rbd_dev)
 	rbdc = __rbd_get_client(rbd_dev->rbd_client);
 
 	ret = -ENOMEM;
-	parent = rbd_dev_create(rbdc, parent_spec);
+	parent = rbd_dev_create(rbdc, parent_spec, NULL);
 	if (!parent)
 		goto out_err;
 
@@ -5394,9 +5421,6 @@ static ssize_t do_rbd_add(struct bus_type *bus,
 	rc = rbd_add_parse_args(buf, &ceph_opts, &rbd_opts, &spec);
 	if (rc < 0)
 		goto err_out_module;
-	read_only = rbd_opts->read_only;
-	kfree(rbd_opts);
-	rbd_opts = NULL;	/* done with this */
 
 	rbdc = rbd_get_client(ceph_opts);
 	if (IS_ERR(rbdc)) {
@@ -5422,11 +5446,12 @@ static ssize_t do_rbd_add(struct bus_type *bus,
 		goto err_out_client;
 	}
 
-	rbd_dev = rbd_dev_create(rbdc, spec);
+	rbd_dev = rbd_dev_create(rbdc, spec, rbd_opts);
 	if (!rbd_dev)
 		goto err_out_client;
 	rbdc = NULL;		/* rbd_dev now owns this */
 	spec = NULL;		/* rbd_dev now owns this */
+	rbd_opts = NULL;	/* rbd_dev now owns this */
 
 	rc = rbd_dev_image_probe(rbd_dev, true);
 	if (rc < 0)
@@ -5434,6 +5459,7 @@ static ssize_t do_rbd_add(struct bus_type *bus,
 
 	/* If we are mapping a snapshot it must be marked read-only */
 
+	read_only = rbd_dev->opts->read_only;
 	if (rbd_dev->spec->snap_id != CEPH_NOSNAP)
 		read_only = true;
 	rbd_dev->mapping.read_only = read_only;
@@ -5458,6 +5484,7 @@ err_out_client:
 	rbd_put_client(rbdc);
 err_out_args:
 	rbd_spec_put(spec);
+	kfree(rbd_opts);
 err_out_module:
 	module_put(THIS_MODULE);
 
diff --git a/drivers/block/sx8.c b/drivers/block/sx8.c
index 5d552857de41..59c91d49b14b 100644
--- a/drivers/block/sx8.c
+++ b/drivers/block/sx8.c
@@ -620,7 +620,7 @@ static int carm_array_info (struct carm_host *host, unsigned int array_idx)
 	spin_unlock_irq(&host->lock);
 
 	DPRINTK("blk_execute_rq_nowait, tag == %u\n", idx);
-	crq->rq->cmd_type = REQ_TYPE_SPECIAL;
+	crq->rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	crq->rq->special = crq;
 	blk_execute_rq_nowait(host->oob_q, NULL, crq->rq, true, NULL);
 
@@ -661,7 +661,7 @@ static int carm_send_special (struct carm_host *host, carm_sspc_t func)
 	crq->msg_bucket = (u32) rc;
 
 	DPRINTK("blk_execute_rq_nowait, tag == %u\n", idx);
-	crq->rq->cmd_type = REQ_TYPE_SPECIAL;
+	crq->rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	crq->rq->special = crq;
 	blk_execute_rq_nowait(host->oob_q, NULL, crq->rq, true, NULL);
 
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 5ea2f0bbbc7c..d4d05f064d39 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -124,7 +124,7 @@ static inline void virtblk_request_done(struct request *req)
 		req->resid_len = virtio32_to_cpu(vblk->vdev, vbr->in_hdr.residual);
 		req->sense_len = virtio32_to_cpu(vblk->vdev, vbr->in_hdr.sense_len);
 		req->errors = virtio32_to_cpu(vblk->vdev, vbr->in_hdr.errors);
-	} else if (req->cmd_type == REQ_TYPE_SPECIAL) {
+	} else if (req->cmd_type == REQ_TYPE_DRV_PRIV) {
 		req->errors = (error != 0);
 	}
 
@@ -188,7 +188,7 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx,
 			vbr->out_hdr.sector = 0;
 			vbr->out_hdr.ioprio = cpu_to_virtio32(vblk->vdev, req_get_ioprio(vbr->req));
 			break;
-		case REQ_TYPE_SPECIAL:
+		case REQ_TYPE_DRV_PRIV:
 			vbr->out_hdr.type = cpu_to_virtio32(vblk->vdev, VIRTIO_BLK_T_GET_ID);
 			vbr->out_hdr.sector = 0;
 			vbr->out_hdr.ioprio = cpu_to_virtio32(vblk->vdev, req_get_ioprio(vbr->req));
@@ -251,7 +251,7 @@ static int virtblk_get_id(struct gendisk *disk, char *id_str)
 		return PTR_ERR(req);
 	}
 
-	req->cmd_type = REQ_TYPE_SPECIAL;
+	req->cmd_type = REQ_TYPE_DRV_PRIV;
 	err = blk_execute_rq(vblk->disk->queue, vblk->disk, req, false);
 	blk_put_request(req);
 
diff --git a/drivers/block/xen-blkback/blkback.c b/drivers/block/xen-blkback/blkback.c
index 713fc9ff1149..ced96777b677 100644
--- a/drivers/block/xen-blkback/blkback.c
+++ b/drivers/block/xen-blkback/blkback.c
@@ -84,6 +84,13 @@ MODULE_PARM_DESC(max_persistent_grants,
                  "Maximum number of grants to map persistently");
 
 /*
+ * Maximum order of pages to be used for the shared ring between front and
+ * backend, 4KB page granularity is used.
+ */
+unsigned int xen_blkif_max_ring_order = XENBUS_MAX_RING_PAGE_ORDER;
+module_param_named(max_ring_page_order, xen_blkif_max_ring_order, int, S_IRUGO);
+MODULE_PARM_DESC(max_ring_page_order, "Maximum order of pages to be used for the shared ring");
+/*
  * The LRU mechanism to clean the lists of persistent grants needs to
  * be executed periodically. The time interval between consecutive executions
  * of the purge mechanism is set in ms.
@@ -729,7 +736,7 @@ static void xen_blkbk_unmap_and_respond(struct pending_req *req)
 	struct grant_page **pages = req->segments;
 	unsigned int invcount;
 
-	invcount = xen_blkbk_unmap_prepare(blkif, pages, req->nr_pages,
+	invcount = xen_blkbk_unmap_prepare(blkif, pages, req->nr_segs,
 					   req->unmap, req->unmap_pages);
 
 	work->data = req;
@@ -915,7 +922,7 @@ static int xen_blkbk_map_seg(struct pending_req *pending_req)
 	int rc;
 
 	rc = xen_blkbk_map(pending_req->blkif, pending_req->segments,
-			   pending_req->nr_pages,
+			   pending_req->nr_segs,
 	                   (pending_req->operation != BLKIF_OP_READ));
 
 	return rc;
@@ -931,7 +938,7 @@ static int xen_blkbk_parse_indirect(struct blkif_request *req,
 	int indirect_grefs, rc, n, nseg, i;
 	struct blkif_request_segment *segments = NULL;
 
-	nseg = pending_req->nr_pages;
+	nseg = pending_req->nr_segs;
 	indirect_grefs = INDIRECT_PAGES(nseg);
 	BUG_ON(indirect_grefs > BLKIF_MAX_INDIRECT_PAGES_PER_REQUEST);
 
@@ -1251,7 +1258,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 	pending_req->id        = req->u.rw.id;
 	pending_req->operation = req_operation;
 	pending_req->status    = BLKIF_RSP_OKAY;
-	pending_req->nr_pages  = nseg;
+	pending_req->nr_segs   = nseg;
 
 	if (req->operation != BLKIF_OP_INDIRECT) {
 		preq.dev               = req->u.rw.handle;
@@ -1372,7 +1379,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 
  fail_flush:
 	xen_blkbk_unmap(blkif, pending_req->segments,
-	                pending_req->nr_pages);
+	                pending_req->nr_segs);
  fail_response:
 	/* Haven't submitted any bio's yet. */
 	make_response(blkif, req->u.rw.id, req_operation, BLKIF_RSP_ERROR);
@@ -1438,6 +1445,12 @@ static int __init xen_blkif_init(void)
 	if (!xen_domain())
 		return -ENODEV;
 
+	if (xen_blkif_max_ring_order > XENBUS_MAX_RING_PAGE_ORDER) {
+		pr_info("Invalid max_ring_order (%d), will use default max: %d.\n",
+			xen_blkif_max_ring_order, XENBUS_MAX_RING_PAGE_ORDER);
+		xen_blkif_max_ring_order = XENBUS_MAX_RING_PAGE_ORDER;
+	}
+
 	rc = xen_blkif_interface_init();
 	if (rc)
 		goto failed_init;
diff --git a/drivers/block/xen-blkback/common.h b/drivers/block/xen-blkback/common.h
index f620b5d3f77c..45a044a53d1e 100644
--- a/drivers/block/xen-blkback/common.h
+++ b/drivers/block/xen-blkback/common.h
@@ -44,6 +44,7 @@
 #include <xen/interface/io/blkif.h>
 #include <xen/interface/io/protocols.h>
 
+extern unsigned int xen_blkif_max_ring_order;
 /*
  * This is the maximum number of segments that would be allowed in indirect
  * requests. This value will also be passed to the frontend.
@@ -248,7 +249,7 @@ struct backend_info;
 #define PERSISTENT_GNT_WAS_ACTIVE	1
 
 /* Number of requests that we can fit in a ring */
-#define XEN_BLKIF_REQS			32
+#define XEN_BLKIF_REQS_PER_PAGE		32
 
 struct persistent_gnt {
 	struct page *page;
@@ -320,6 +321,7 @@ struct xen_blkif {
 	struct work_struct	free_work;
 	/* Thread shutdown wait queue. */
 	wait_queue_head_t	shutdown_wq;
+	unsigned int nr_ring_pages;
 };
 
 struct seg_buf {
@@ -343,7 +345,7 @@ struct grant_page {
 struct pending_req {
 	struct xen_blkif	*blkif;
 	u64			id;
-	int			nr_pages;
+	int			nr_segs;
 	atomic_t		pendcnt;
 	unsigned short		operation;
 	int			status;
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 6ab69ad61ee1..deb3f001791f 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -25,6 +25,7 @@
 
 /* Enlarge the array size in order to fully show blkback name. */
 #define BLKBACK_NAME_LEN (20)
+#define RINGREF_NAME_LEN (20)
 
 struct backend_info {
 	struct xenbus_device	*dev;
@@ -124,8 +125,6 @@ static void xen_update_blkif_status(struct xen_blkif *blkif)
 static struct xen_blkif *xen_blkif_alloc(domid_t domid)
 {
 	struct xen_blkif *blkif;
-	struct pending_req *req, *n;
-	int i, j;
 
 	BUILD_BUG_ON(MAX_INDIRECT_PAGES > BLKIF_MAX_INDIRECT_PAGES_PER_REQUEST);
 
@@ -151,55 +150,15 @@ static struct xen_blkif *xen_blkif_alloc(domid_t domid)
 
 	INIT_LIST_HEAD(&blkif->pending_free);
 	INIT_WORK(&blkif->free_work, xen_blkif_deferred_free);
-
-	for (i = 0; i < XEN_BLKIF_REQS; i++) {
-		req = kzalloc(sizeof(*req), GFP_KERNEL);
-		if (!req)
-			goto fail;
-		list_add_tail(&req->free_list,
-		              &blkif->pending_free);
-		for (j = 0; j < MAX_INDIRECT_SEGMENTS; j++) {
-			req->segments[j] = kzalloc(sizeof(*req->segments[0]),
-			                           GFP_KERNEL);
-			if (!req->segments[j])
-				goto fail;
-		}
-		for (j = 0; j < MAX_INDIRECT_PAGES; j++) {
-			req->indirect_pages[j] = kzalloc(sizeof(*req->indirect_pages[0]),
-			                                 GFP_KERNEL);
-			if (!req->indirect_pages[j])
-				goto fail;
-		}
-	}
 	spin_lock_init(&blkif->pending_free_lock);
 	init_waitqueue_head(&blkif->pending_free_wq);
 	init_waitqueue_head(&blkif->shutdown_wq);
 
 	return blkif;
-
-fail:
-	list_for_each_entry_safe(req, n, &blkif->pending_free, free_list) {
-		list_del(&req->free_list);
-		for (j = 0; j < MAX_INDIRECT_SEGMENTS; j++) {
-			if (!req->segments[j])
-				break;
-			kfree(req->segments[j]);
-		}
-		for (j = 0; j < MAX_INDIRECT_PAGES; j++) {
-			if (!req->indirect_pages[j])
-				break;
-			kfree(req->indirect_pages[j]);
-		}
-		kfree(req);
-	}
-
-	kmem_cache_free(xen_blkif_cachep, blkif);
-
-	return ERR_PTR(-ENOMEM);
 }
 
-static int xen_blkif_map(struct xen_blkif *blkif, grant_ref_t gref,
-			 unsigned int evtchn)
+static int xen_blkif_map(struct xen_blkif *blkif, grant_ref_t *gref,
+			 unsigned int nr_grefs, unsigned int evtchn)
 {
 	int err;
 
@@ -207,7 +166,7 @@ static int xen_blkif_map(struct xen_blkif *blkif, grant_ref_t gref,
 	if (blkif->irq)
 		return 0;
 
-	err = xenbus_map_ring_valloc(blkif->be->dev, &gref, 1,
+	err = xenbus_map_ring_valloc(blkif->be->dev, gref, nr_grefs,
 				     &blkif->blk_ring);
 	if (err < 0)
 		return err;
@@ -217,21 +176,21 @@ static int xen_blkif_map(struct xen_blkif *blkif, grant_ref_t gref,
 	{
 		struct blkif_sring *sring;
 		sring = (struct blkif_sring *)blkif->blk_ring;
-		BACK_RING_INIT(&blkif->blk_rings.native, sring, PAGE_SIZE);
+		BACK_RING_INIT(&blkif->blk_rings.native, sring, PAGE_SIZE * nr_grefs);
 		break;
 	}
 	case BLKIF_PROTOCOL_X86_32:
 	{
 		struct blkif_x86_32_sring *sring_x86_32;
 		sring_x86_32 = (struct blkif_x86_32_sring *)blkif->blk_ring;
-		BACK_RING_INIT(&blkif->blk_rings.x86_32, sring_x86_32, PAGE_SIZE);
+		BACK_RING_INIT(&blkif->blk_rings.x86_32, sring_x86_32, PAGE_SIZE * nr_grefs);
 		break;
 	}
 	case BLKIF_PROTOCOL_X86_64:
 	{
 		struct blkif_x86_64_sring *sring_x86_64;
 		sring_x86_64 = (struct blkif_x86_64_sring *)blkif->blk_ring;
-		BACK_RING_INIT(&blkif->blk_rings.x86_64, sring_x86_64, PAGE_SIZE);
+		BACK_RING_INIT(&blkif->blk_rings.x86_64, sring_x86_64, PAGE_SIZE * nr_grefs);
 		break;
 	}
 	default:
@@ -312,7 +271,7 @@ static void xen_blkif_free(struct xen_blkif *blkif)
 		i++;
 	}
 
-	WARN_ON(i != XEN_BLKIF_REQS);
+	WARN_ON(i != (XEN_BLKIF_REQS_PER_PAGE * blkif->nr_ring_pages));
 
 	kmem_cache_free(xen_blkif_cachep, blkif);
 }
@@ -597,6 +556,11 @@ static int xen_blkbk_probe(struct xenbus_device *dev,
 	if (err)
 		goto fail;
 
+	err = xenbus_printf(XBT_NIL, dev->nodename, "max-ring-page-order", "%u",
+			    xen_blkif_max_ring_order);
+	if (err)
+		pr_warn("%s write out 'max-ring-page-order' failed\n", __func__);
+
 	err = xenbus_switch_state(dev, XenbusStateInitWait);
 	if (err)
 		goto fail;
@@ -860,22 +824,66 @@ again:
 static int connect_ring(struct backend_info *be)
 {
 	struct xenbus_device *dev = be->dev;
-	unsigned long ring_ref;
-	unsigned int evtchn;
+	unsigned int ring_ref[XENBUS_MAX_RING_PAGES];
+	unsigned int evtchn, nr_grefs, ring_page_order;
 	unsigned int pers_grants;
 	char protocol[64] = "";
-	int err;
+	struct pending_req *req, *n;
+	int err, i, j;
 
 	pr_debug("%s %s\n", __func__, dev->otherend);
 
-	err = xenbus_gather(XBT_NIL, dev->otherend, "ring-ref", "%lu",
-			    &ring_ref, "event-channel", "%u", &evtchn, NULL);
-	if (err) {
-		xenbus_dev_fatal(dev, err,
-				 "reading %s/ring-ref and event-channel",
+	err = xenbus_scanf(XBT_NIL, dev->otherend, "event-channel", "%u",
+			  &evtchn);
+	if (err != 1) {
+		err = -EINVAL;
+		xenbus_dev_fatal(dev, err, "reading %s/event-channel",
 				 dev->otherend);
 		return err;
 	}
+	pr_info("event-channel %u\n", evtchn);
+
+	err = xenbus_scanf(XBT_NIL, dev->otherend, "ring-page-order", "%u",
+			  &ring_page_order);
+	if (err != 1) {
+		err = xenbus_scanf(XBT_NIL, dev->otherend, "ring-ref",
+				  "%u", &ring_ref[0]);
+		if (err != 1) {
+			err = -EINVAL;
+			xenbus_dev_fatal(dev, err, "reading %s/ring-ref",
+					 dev->otherend);
+			return err;
+		}
+		nr_grefs = 1;
+		pr_info("%s:using single page: ring-ref %d\n", dev->otherend,
+			ring_ref[0]);
+	} else {
+		unsigned int i;
+
+		if (ring_page_order > xen_blkif_max_ring_order) {
+			err = -EINVAL;
+			xenbus_dev_fatal(dev, err, "%s/request %d ring page order exceed max:%d",
+					 dev->otherend, ring_page_order,
+					 xen_blkif_max_ring_order);
+			return err;
+		}
+
+		nr_grefs = 1 << ring_page_order;
+		for (i = 0; i < nr_grefs; i++) {
+			char ring_ref_name[RINGREF_NAME_LEN];
+
+			snprintf(ring_ref_name, RINGREF_NAME_LEN, "ring-ref%u", i);
+			err = xenbus_scanf(XBT_NIL, dev->otherend, ring_ref_name,
+					   "%u", &ring_ref[i]);
+			if (err != 1) {
+				err = -EINVAL;
+				xenbus_dev_fatal(dev, err, "reading %s/%s",
+						 dev->otherend, ring_ref_name);
+				return err;
+			}
+			pr_info("ring-ref%u: %u\n", i, ring_ref[i]);
+		}
+	}
 
 	be->blkif->blk_protocol = BLKIF_PROTOCOL_DEFAULT;
 	err = xenbus_gather(XBT_NIL, dev->otherend, "protocol",
@@ -900,20 +908,55 @@ static int connect_ring(struct backend_info *be)
 
 	be->blkif->vbd.feature_gnt_persistent = pers_grants;
 	be->blkif->vbd.overflow_max_grants = 0;
+	be->blkif->nr_ring_pages = nr_grefs;
 
-	pr_info("ring-ref %ld, event-channel %d, protocol %d (%s) %s\n",
-		ring_ref, evtchn, be->blkif->blk_protocol, protocol,
+	pr_info("ring-pages:%d, event-channel %d, protocol %d (%s) %s\n",
+		nr_grefs, evtchn, be->blkif->blk_protocol, protocol,
 		pers_grants ? "persistent grants" : "");
 
+	for (i = 0; i < nr_grefs * XEN_BLKIF_REQS_PER_PAGE; i++) {
+		req = kzalloc(sizeof(*req), GFP_KERNEL);
+		if (!req)
+			goto fail;
+		list_add_tail(&req->free_list, &be->blkif->pending_free);
+		for (j = 0; j < MAX_INDIRECT_SEGMENTS; j++) {
+			req->segments[j] = kzalloc(sizeof(*req->segments[0]), GFP_KERNEL);
+			if (!req->segments[j])
+				goto fail;
+		}
+		for (j = 0; j < MAX_INDIRECT_PAGES; j++) {
+			req->indirect_pages[j] = kzalloc(sizeof(*req->indirect_pages[0]),
+							 GFP_KERNEL);
+			if (!req->indirect_pages[j])
+				goto fail;
+		}
+	}
+
 	/* Map the shared frame, irq etc. */
-	err = xen_blkif_map(be->blkif, ring_ref, evtchn);
+	err = xen_blkif_map(be->blkif, ring_ref, nr_grefs, evtchn);
 	if (err) {
-		xenbus_dev_fatal(dev, err, "mapping ring-ref %lu port %u",
-				 ring_ref, evtchn);
+		xenbus_dev_fatal(dev, err, "mapping ring-ref port %u", evtchn);
 		return err;
 	}
 
 	return 0;
+
+fail:
+	list_for_each_entry_safe(req, n, &be->blkif->pending_free, free_list) {
+		list_del(&req->free_list);
+		for (j = 0; j < MAX_INDIRECT_SEGMENTS; j++) {
+			if (!req->segments[j])
+				break;
+			kfree(req->segments[j]);
+		}
+		for (j = 0; j < MAX_INDIRECT_PAGES; j++) {
+			if (!req->indirect_pages[j])
+				break;
+			kfree(req->indirect_pages[j]);
+		}
+		kfree(req);
+	}
+	return -ENOMEM;
 }
 
 static const struct xenbus_device_id xen_blkbk_ids[] = {
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index 2c61cf8c6f61..6d89ed35d80c 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -98,7 +98,21 @@ static unsigned int xen_blkif_max_segments = 32;
 module_param_named(max, xen_blkif_max_segments, int, S_IRUGO);
 MODULE_PARM_DESC(max, "Maximum amount of segments in indirect requests (default is 32)");
 
-#define BLK_RING_SIZE __CONST_RING_SIZE(blkif, PAGE_SIZE)
+/*
+ * Maximum order of pages to be used for the shared ring between front and
+ * backend, 4KB page granularity is used.
+ */
+static unsigned int xen_blkif_max_ring_order;
+module_param_named(max_ring_page_order, xen_blkif_max_ring_order, int, S_IRUGO);
+MODULE_PARM_DESC(max_ring_page_order, "Maximum order of pages to be used for the shared ring");
+
+#define BLK_RING_SIZE(info) __CONST_RING_SIZE(blkif, PAGE_SIZE * (info)->nr_ring_pages)
+#define BLK_MAX_RING_SIZE __CONST_RING_SIZE(blkif, PAGE_SIZE * XENBUS_MAX_RING_PAGES)
+/*
+ * ring-ref%i i=(-1UL) would take 11 characters + 'ring-ref' is 8, so 19
+ * characters are enough. Define to 20 to keep consist with backend.
+ */
+#define RINGREF_NAME_LEN (20)
 
 /*
  * We have one of these per vbd, whether ide, scsi or 'other'.  They
@@ -114,13 +128,14 @@ struct blkfront_info
 	int vdevice;
 	blkif_vdev_t handle;
 	enum blkif_state connected;
-	int ring_ref;
+	int ring_ref[XENBUS_MAX_RING_PAGES];
+	unsigned int nr_ring_pages;
 	struct blkif_front_ring ring;
 	unsigned int evtchn, irq;
 	struct request_queue *rq;
 	struct work_struct work;
 	struct gnttab_free_callback callback;
-	struct blk_shadow shadow[BLK_RING_SIZE];
+	struct blk_shadow shadow[BLK_MAX_RING_SIZE];
 	struct list_head grants;
 	struct list_head indirect_pages;
 	unsigned int persistent_gnts_c;
@@ -139,8 +154,6 @@ static unsigned int nr_minors;
 static unsigned long *minors;
 static DEFINE_SPINLOCK(minor_lock);
 
-#define MAXIMUM_OUTSTANDING_BLOCK_REQS \
-	(BLKIF_MAX_SEGMENTS_PER_REQUEST * BLK_RING_SIZE)
 #define GRANT_INVALID_REF	0
 
 #define PARTS_PER_DISK		16
@@ -170,7 +183,7 @@ static int blkfront_setup_indirect(struct blkfront_info *info);
 static int get_id_from_freelist(struct blkfront_info *info)
 {
 	unsigned long free = info->shadow_free;
-	BUG_ON(free >= BLK_RING_SIZE);
+	BUG_ON(free >= BLK_RING_SIZE(info));
 	info->shadow_free = info->shadow[free].req.u.rw.id;
 	info->shadow[free].req.u.rw.id = 0x0fffffee; /* debug */
 	return free;
@@ -983,7 +996,7 @@ static void blkif_free(struct blkfront_info *info, int suspend)
 		}
 	}
 
-	for (i = 0; i < BLK_RING_SIZE; i++) {
+	for (i = 0; i < BLK_RING_SIZE(info); i++) {
 		/*
 		 * Clear persistent grants present in requests already
 		 * on the shared ring
@@ -1033,12 +1046,15 @@ free_shadow:
 	flush_work(&info->work);
 
 	/* Free resources associated with old device channel. */
-	if (info->ring_ref != GRANT_INVALID_REF) {
-		gnttab_end_foreign_access(info->ring_ref, 0,
-					  (unsigned long)info->ring.sring);
-		info->ring_ref = GRANT_INVALID_REF;
-		info->ring.sring = NULL;
+	for (i = 0; i < info->nr_ring_pages; i++) {
+		if (info->ring_ref[i] != GRANT_INVALID_REF) {
+			gnttab_end_foreign_access(info->ring_ref[i], 0, 0);
+			info->ring_ref[i] = GRANT_INVALID_REF;
+		}
 	}
+	free_pages((unsigned long)info->ring.sring, get_order(info->nr_ring_pages * PAGE_SIZE));
+	info->ring.sring = NULL;
+
 	if (info->irq)
 		unbind_from_irqhandler(info->irq, info);
 	info->evtchn = info->irq = 0;
@@ -1058,12 +1074,6 @@ static void blkif_completion(struct blk_shadow *s, struct blkfront_info *info,
 		s->req.u.indirect.nr_segments : s->req.u.rw.nr_segments;
 
 	if (bret->operation == BLKIF_OP_READ && info->feature_persistent) {
-		/*
-		 * Copy the data received from the backend into the bvec.
-		 * Since bv_offset can be different than 0, and bv_len different
-		 * than PAGE_SIZE, we have to keep track of the current offset,
-		 * to be sure we are copying the data from the right shared page.
-		 */
 		for_each_sg(s->sg, sg, nseg, i) {
 			BUG_ON(sg->offset + sg->length > PAGE_SIZE);
 			shared_data = kmap_atomic(
@@ -1157,7 +1167,7 @@ static irqreturn_t blkif_interrupt(int irq, void *dev_id)
 		 * never have given to it (we stamp it up to BLK_RING_SIZE -
 		 * look in get_id_from_freelist.
 		 */
-		if (id >= BLK_RING_SIZE) {
+		if (id >= BLK_RING_SIZE(info)) {
 			WARN(1, "%s: response to %s has incorrect id (%ld)\n",
 			     info->gd->disk_name, op_name(bret->operation), id);
 			/* We can't safely get the 'struct request' as
@@ -1245,26 +1255,30 @@ static int setup_blkring(struct xenbus_device *dev,
 			 struct blkfront_info *info)
 {
 	struct blkif_sring *sring;
-	grant_ref_t gref;
-	int err;
+	int err, i;
+	unsigned long ring_size = info->nr_ring_pages * PAGE_SIZE;
+	grant_ref_t gref[XENBUS_MAX_RING_PAGES];
 
-	info->ring_ref = GRANT_INVALID_REF;
+	for (i = 0; i < info->nr_ring_pages; i++)
+		info->ring_ref[i] = GRANT_INVALID_REF;
 
-	sring = (struct blkif_sring *)__get_free_page(GFP_NOIO | __GFP_HIGH);
+	sring = (struct blkif_sring *)__get_free_pages(GFP_NOIO | __GFP_HIGH,
+						       get_order(ring_size));
 	if (!sring) {
 		xenbus_dev_fatal(dev, -ENOMEM, "allocating shared ring");
 		return -ENOMEM;
 	}
 	SHARED_RING_INIT(sring);
-	FRONT_RING_INIT(&info->ring, sring, PAGE_SIZE);
+	FRONT_RING_INIT(&info->ring, sring, ring_size);
 
-	err = xenbus_grant_ring(dev, info->ring.sring, 1, &gref);
+	err = xenbus_grant_ring(dev, info->ring.sring, info->nr_ring_pages, gref);
 	if (err < 0) {
-		free_page((unsigned long)sring);
+		free_pages((unsigned long)sring, get_order(ring_size));
 		info->ring.sring = NULL;
 		goto fail;
 	}
-	info->ring_ref = gref;
+	for (i = 0; i < info->nr_ring_pages; i++)
+		info->ring_ref[i] = gref[i];
 
 	err = xenbus_alloc_evtchn(dev, &info->evtchn);
 	if (err)
@@ -1292,7 +1306,18 @@ static int talk_to_blkback(struct xenbus_device *dev,
 {
 	const char *message = NULL;
 	struct xenbus_transaction xbt;
-	int err;
+	int err, i;
+	unsigned int max_page_order = 0;
+	unsigned int ring_page_order = 0;
+
+	err = xenbus_scanf(XBT_NIL, info->xbdev->otherend,
+			   "max-ring-page-order", "%u", &max_page_order);
+	if (err != 1)
+		info->nr_ring_pages = 1;
+	else {
+		ring_page_order = min(xen_blkif_max_ring_order, max_page_order);
+		info->nr_ring_pages = 1 << ring_page_order;
+	}
 
 	/* Create shared ring, alloc event channel. */
 	err = setup_blkring(dev, info);
@@ -1306,11 +1331,32 @@ again:
 		goto destroy_blkring;
 	}
 
-	err = xenbus_printf(xbt, dev->nodename,
-			    "ring-ref", "%u", info->ring_ref);
-	if (err) {
-		message = "writing ring-ref";
-		goto abort_transaction;
+	if (info->nr_ring_pages == 1) {
+		err = xenbus_printf(xbt, dev->nodename,
+				    "ring-ref", "%u", info->ring_ref[0]);
+		if (err) {
+			message = "writing ring-ref";
+			goto abort_transaction;
+		}
+	} else {
+		err = xenbus_printf(xbt, dev->nodename,
+				    "ring-page-order", "%u", ring_page_order);
+		if (err) {
+			message = "writing ring-page-order";
+			goto abort_transaction;
+		}
+
+		for (i = 0; i < info->nr_ring_pages; i++) {
+			char ring_ref_name[RINGREF_NAME_LEN];
+
+			snprintf(ring_ref_name, RINGREF_NAME_LEN, "ring-ref%u", i);
+			err = xenbus_printf(xbt, dev->nodename, ring_ref_name,
+					    "%u", info->ring_ref[i]);
+			if (err) {
+				message = "writing ring-ref";
+				goto abort_transaction;
+			}
+		}
 	}
 	err = xenbus_printf(xbt, dev->nodename,
 			    "event-channel", "%u", info->evtchn);
@@ -1338,6 +1384,9 @@ again:
 		goto destroy_blkring;
 	}
 
+	for (i = 0; i < BLK_RING_SIZE(info); i++)
+		info->shadow[i].req.u.rw.id = i+1;
+	info->shadow[BLK_RING_SIZE(info)-1].req.u.rw.id = 0x0fffffff;
 	xenbus_switch_state(dev, XenbusStateInitialised);
 
 	return 0;
@@ -1361,7 +1410,7 @@ again:
 static int blkfront_probe(struct xenbus_device *dev,
 			  const struct xenbus_device_id *id)
 {
-	int err, vdevice, i;
+	int err, vdevice;
 	struct blkfront_info *info;
 
 	/* FIXME: Use dynamic device id if this is not set. */
@@ -1422,21 +1471,10 @@ static int blkfront_probe(struct xenbus_device *dev,
 	info->connected = BLKIF_STATE_DISCONNECTED;
 	INIT_WORK(&info->work, blkif_restart_queue);
 
-	for (i = 0; i < BLK_RING_SIZE; i++)
-		info->shadow[i].req.u.rw.id = i+1;
-	info->shadow[BLK_RING_SIZE-1].req.u.rw.id = 0x0fffffff;
-
 	/* Front end dir is a number, which is used as the id. */
 	info->handle = simple_strtoul(strrchr(dev->nodename, '/')+1, NULL, 0);
 	dev_set_drvdata(&dev->dev, info);
 
-	err = talk_to_blkback(dev, info);
-	if (err) {
-		kfree(info);
-		dev_set_drvdata(&dev->dev, NULL);
-		return err;
-	}
-
 	return 0;
 }
 
@@ -1476,10 +1514,10 @@ static int blkif_recover(struct blkfront_info *info)
 
 	/* Stage 2: Set up free list. */
 	memset(&info->shadow, 0, sizeof(info->shadow));
-	for (i = 0; i < BLK_RING_SIZE; i++)
+	for (i = 0; i < BLK_RING_SIZE(info); i++)
 		info->shadow[i].req.u.rw.id = i+1;
 	info->shadow_free = info->ring.req_prod_pvt;
-	info->shadow[BLK_RING_SIZE-1].req.u.rw.id = 0x0fffffff;
+	info->shadow[BLK_RING_SIZE(info)-1].req.u.rw.id = 0x0fffffff;
 
 	rc = blkfront_setup_indirect(info);
 	if (rc) {
@@ -1491,7 +1529,7 @@ static int blkif_recover(struct blkfront_info *info)
 	blk_queue_max_segments(info->rq, segs);
 	bio_list_init(&bio_list);
 	INIT_LIST_HEAD(&requests);
-	for (i = 0; i < BLK_RING_SIZE; i++) {
+	for (i = 0; i < BLK_RING_SIZE(info); i++) {
 		/* Not in use? */
 		if (!copy[i].request)
 			continue;
@@ -1697,7 +1735,7 @@ static int blkfront_setup_indirect(struct blkfront_info *info)
 		segs = info->max_indirect_segments;
 	}
 
-	err = fill_grant_buffer(info, (segs + INDIRECT_GREFS(segs)) * BLK_RING_SIZE);
+	err = fill_grant_buffer(info, (segs + INDIRECT_GREFS(segs)) * BLK_RING_SIZE(info));
 	if (err)
 		goto out_of_memory;
 
@@ -1707,7 +1745,7 @@ static int blkfront_setup_indirect(struct blkfront_info *info)
 		 * grants, we need to allocate a set of pages that can be
 		 * used for mapping indirect grefs
 		 */
-		int num = INDIRECT_GREFS(segs) * BLK_RING_SIZE;
+		int num = INDIRECT_GREFS(segs) * BLK_RING_SIZE(info);
 
 		BUG_ON(!list_empty(&info->indirect_pages));
 		for (i = 0; i < num; i++) {
@@ -1718,7 +1756,7 @@ static int blkfront_setup_indirect(struct blkfront_info *info)
 		}
 	}
 
-	for (i = 0; i < BLK_RING_SIZE; i++) {
+	for (i = 0; i < BLK_RING_SIZE(info); i++) {
 		info->shadow[i].grants_used = kzalloc(
 			sizeof(info->shadow[i].grants_used[0]) * segs,
 			GFP_NOIO);
@@ -1740,7 +1778,7 @@ static int blkfront_setup_indirect(struct blkfront_info *info)
 	return 0;
 
 out_of_memory:
-	for (i = 0; i < BLK_RING_SIZE; i++) {
+	for (i = 0; i < BLK_RING_SIZE(info); i++) {
 		kfree(info->shadow[i].grants_used);
 		info->shadow[i].grants_used = NULL;
 		kfree(info->shadow[i].sg);
@@ -1906,8 +1944,15 @@ static void blkback_changed(struct xenbus_device *dev,
 	dev_dbg(&dev->dev, "blkfront:blkback_changed to state %d.\n", backend_state);
 
 	switch (backend_state) {
-	case XenbusStateInitialising:
 	case XenbusStateInitWait:
+		if (dev->state != XenbusStateInitialising)
+			break;
+		if (talk_to_blkback(dev, info)) {
+			kfree(info);
+			dev_set_drvdata(&dev->dev, NULL);
+			break;
+		}
+	case XenbusStateInitialising:
 	case XenbusStateInitialised:
 	case XenbusStateReconfiguring:
 	case XenbusStateReconfigured:
@@ -2091,6 +2136,12 @@ static int __init xlblk_init(void)
 	if (!xen_domain())
 		return -ENODEV;
 
+	if (xen_blkif_max_ring_order > XENBUS_MAX_RING_PAGE_ORDER) {
+		pr_info("Invalid max_ring_order (%d), will use default max: %d.\n",
+			xen_blkif_max_ring_order, XENBUS_MAX_RING_PAGE_ORDER);
+		xen_blkif_max_ring_order = 0;
+	}
+
 	if (!xen_has_pv_disk_devices())
 		return -ENODEV;
 
diff --git a/drivers/block/zram/Kconfig b/drivers/block/zram/Kconfig
index 6489c0fd0ea6..386ba3d1a6ee 100644
--- a/drivers/block/zram/Kconfig
+++ b/drivers/block/zram/Kconfig
@@ -23,12 +23,4 @@ config ZRAM_LZ4_COMPRESS
 	default n
 	help
 	  This option enables LZ4 compression algorithm support. Compression
-	  algorithm can be changed using `comp_algorithm' device attribute.
-
-config ZRAM_DEBUG
-	bool "Compressed RAM block device debug support"
-	depends on ZRAM
-	default n
-	help
-	  This option adds additional debugging code to the compressed
-	  RAM block device driver.
+	  algorithm can be changed using `comp_algorithm' device attribute.
\ No newline at end of file
diff --git a/drivers/block/zram/zcomp.c b/drivers/block/zram/zcomp.c
index f1ff39a3d1c1..965d1afb0eaa 100644
--- a/drivers/block/zram/zcomp.c
+++ b/drivers/block/zram/zcomp.c
@@ -274,7 +274,7 @@ ssize_t zcomp_available_show(const char *comp, char *buf)
 	int i = 0;
 
 	while (backends[i]) {
-		if (sysfs_streq(comp, backends[i]->name))
+		if (!strcmp(comp, backends[i]->name))
 			sz += scnprintf(buf + sz, PAGE_SIZE - sz - 2,
 					"[%s] ", backends[i]->name);
 		else
@@ -286,6 +286,11 @@ ssize_t zcomp_available_show(const char *comp, char *buf)
 	return sz;
 }
 
+bool zcomp_available_algorithm(const char *comp)
+{
+	return find_backend(comp) != NULL;
+}
+
 bool zcomp_set_max_streams(struct zcomp *comp, int num_strm)
 {
 	return comp->set_max_streams(comp, num_strm);
diff --git a/drivers/block/zram/zcomp.h b/drivers/block/zram/zcomp.h
index c59d1fca72c0..46e2b9f8f1f0 100644
--- a/drivers/block/zram/zcomp.h
+++ b/drivers/block/zram/zcomp.h
@@ -51,6 +51,7 @@ struct zcomp {
 };
 
 ssize_t zcomp_available_show(const char *comp, char *buf);
+bool zcomp_available_algorithm(const char *comp);
 
 struct zcomp *zcomp_create(const char *comp, int max_strm);
 void zcomp_destroy(struct zcomp *comp);
diff --git a/drivers/block/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index 8dcbced0eafd..fb655e8d1e3b 100644
--- a/drivers/block/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -15,10 +15,6 @@
 #define KMSG_COMPONENT "zram"
 #define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
 
-#ifdef CONFIG_ZRAM_DEBUG
-#define DEBUG
-#endif
-
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/bio.h>
@@ -32,12 +28,16 @@
 #include <linux/string.h>
 #include <linux/vmalloc.h>
 #include <linux/err.h>
+#include <linux/idr.h>
+#include <linux/sysfs.h>
 
 #include "zram_drv.h"
 
-/* Globals */
+static DEFINE_IDR(zram_index_idr);
+/* idr index must be protected */
+static DEFINE_MUTEX(zram_index_mutex);
+
 static int zram_major;
-static struct zram *zram_devices;
 static const char *default_compressor = "lzo";
 
 /* Module params (documentation at end) */
@@ -53,7 +53,7 @@ static inline void deprecated_attr_warn(const char *name)
 }
 
 #define ZRAM_ATTR_RO(name)						\
-static ssize_t 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);				\
@@ -74,33 +74,117 @@ static inline struct zram *dev_to_zram(struct device *dev)
 	return (struct zram *)dev_to_disk(dev)->private_data;
 }
 
-static ssize_t compact_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t len)
+/* flag operations require table entry bit_spin_lock() being held */
+static int zram_test_flag(struct zram_meta *meta, u32 index,
+			enum zram_pageflags flag)
 {
-	unsigned long nr_migrated;
-	struct zram *zram = dev_to_zram(dev);
-	struct zram_meta *meta;
+	return meta->table[index].value & BIT(flag);
+}
 
-	down_read(&zram->init_lock);
-	if (!init_done(zram)) {
-		up_read(&zram->init_lock);
-		return -EINVAL;
-	}
+static void zram_set_flag(struct zram_meta *meta, u32 index,
+			enum zram_pageflags flag)
+{
+	meta->table[index].value |= BIT(flag);
+}
 
-	meta = zram->meta;
-	nr_migrated = zs_compact(meta->mem_pool);
-	atomic64_add(nr_migrated, &zram->stats.num_migrated);
-	up_read(&zram->init_lock);
+static void zram_clear_flag(struct zram_meta *meta, u32 index,
+			enum zram_pageflags flag)
+{
+	meta->table[index].value &= ~BIT(flag);
+}
 
-	return len;
+static size_t zram_get_obj_size(struct zram_meta *meta, u32 index)
+{
+	return meta->table[index].value & (BIT(ZRAM_FLAG_SHIFT) - 1);
 }
 
-static ssize_t disksize_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static void zram_set_obj_size(struct zram_meta *meta,
+					u32 index, size_t size)
 {
-	struct zram *zram = dev_to_zram(dev);
+	unsigned long flags = meta->table[index].value >> ZRAM_FLAG_SHIFT;
 
-	return scnprintf(buf, PAGE_SIZE, "%llu\n", zram->disksize);
+	meta->table[index].value = (flags << ZRAM_FLAG_SHIFT) | size;
+}
+
+static inline int is_partial_io(struct bio_vec *bvec)
+{
+	return bvec->bv_len != PAGE_SIZE;
+}
+
+/*
+ * Check if request is within bounds and aligned on zram logical blocks.
+ */
+static inline int valid_io_request(struct zram *zram,
+		sector_t start, unsigned int size)
+{
+	u64 end, bound;
+
+	/* unaligned request */
+	if (unlikely(start & (ZRAM_SECTOR_PER_LOGICAL_BLOCK - 1)))
+		return 0;
+	if (unlikely(size & (ZRAM_LOGICAL_BLOCK_SIZE - 1)))
+		return 0;
+
+	end = start + (size >> SECTOR_SHIFT);
+	bound = zram->disksize >> SECTOR_SHIFT;
+	/* out of range range */
+	if (unlikely(start >= bound || end > bound || start > end))
+		return 0;
+
+	/* I/O request is valid */
+	return 1;
+}
+
+static void update_position(u32 *index, int *offset, struct bio_vec *bvec)
+{
+	if (*offset + bvec->bv_len >= PAGE_SIZE)
+		(*index)++;
+	*offset = (*offset + bvec->bv_len) % PAGE_SIZE;
+}
+
+static inline void update_used_max(struct zram *zram,
+					const unsigned long pages)
+{
+	unsigned long old_max, cur_max;
+
+	old_max = atomic_long_read(&zram->stats.max_used_pages);
+
+	do {
+		cur_max = old_max;
+		if (pages > cur_max)
+			old_max = atomic_long_cmpxchg(
+				&zram->stats.max_used_pages, cur_max, pages);
+	} while (old_max != cur_max);
+}
+
+static int page_zero_filled(void *ptr)
+{
+	unsigned int pos;
+	unsigned long *page;
+
+	page = (unsigned long *)ptr;
+
+	for (pos = 0; pos != PAGE_SIZE / sizeof(*page); pos++) {
+		if (page[pos])
+			return 0;
+	}
+
+	return 1;
+}
+
+static void handle_zero_page(struct bio_vec *bvec)
+{
+	struct page *page = bvec->bv_page;
+	void *user_mem;
+
+	user_mem = kmap_atomic(page);
+	if (is_partial_io(bvec))
+		memset(user_mem + bvec->bv_offset, 0, bvec->bv_len);
+	else
+		clear_page(user_mem);
+	kunmap_atomic(user_mem);
+
+	flush_dcache_page(page);
 }
 
 static ssize_t initstate_show(struct device *dev,
@@ -116,6 +200,14 @@ static ssize_t initstate_show(struct device *dev,
 	return scnprintf(buf, PAGE_SIZE, "%u\n", val);
 }
 
+static ssize_t disksize_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct zram *zram = dev_to_zram(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", zram->disksize);
+}
+
 static ssize_t orig_data_size_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -143,19 +235,6 @@ static ssize_t mem_used_total_show(struct device *dev,
 	return scnprintf(buf, PAGE_SIZE, "%llu\n", val << PAGE_SHIFT);
 }
 
-static ssize_t max_comp_streams_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
-{
-	int val;
-	struct zram *zram = dev_to_zram(dev);
-
-	down_read(&zram->init_lock);
-	val = zram->max_comp_streams;
-	up_read(&zram->init_lock);
-
-	return scnprintf(buf, PAGE_SIZE, "%d\n", val);
-}
-
 static ssize_t mem_limit_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -225,6 +304,19 @@ static ssize_t mem_used_max_store(struct device *dev,
 	return len;
 }
 
+static ssize_t max_comp_streams_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int val;
+	struct zram *zram = dev_to_zram(dev);
+
+	down_read(&zram->init_lock);
+	val = zram->max_comp_streams;
+	up_read(&zram->init_lock);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", val);
+}
+
 static ssize_t max_comp_streams_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t len)
 {
@@ -271,6 +363,8 @@ static ssize_t comp_algorithm_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t len)
 {
 	struct zram *zram = dev_to_zram(dev);
+	size_t sz;
+
 	down_write(&zram->init_lock);
 	if (init_done(zram)) {
 		up_write(&zram->init_lock);
@@ -278,69 +372,108 @@ static ssize_t comp_algorithm_store(struct device *dev,
 		return -EBUSY;
 	}
 	strlcpy(zram->compressor, buf, sizeof(zram->compressor));
+
+	/* ignore trailing newline */
+	sz = strlen(zram->compressor);
+	if (sz > 0 && zram->compressor[sz - 1] == '\n')
+		zram->compressor[sz - 1] = 0x00;
+
+	if (!zcomp_available_algorithm(zram->compressor))
+		len = -EINVAL;
+
 	up_write(&zram->init_lock);
 	return len;
 }
 
-/* flag operations needs meta->tb_lock */
-static int zram_test_flag(struct zram_meta *meta, u32 index,
-			enum zram_pageflags flag)
+static ssize_t compact_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
 {
-	return meta->table[index].value & BIT(flag);
-}
+	unsigned long nr_migrated;
+	struct zram *zram = dev_to_zram(dev);
+	struct zram_meta *meta;
 
-static void zram_set_flag(struct zram_meta *meta, u32 index,
-			enum zram_pageflags flag)
-{
-	meta->table[index].value |= BIT(flag);
-}
+	down_read(&zram->init_lock);
+	if (!init_done(zram)) {
+		up_read(&zram->init_lock);
+		return -EINVAL;
+	}
 
-static void zram_clear_flag(struct zram_meta *meta, u32 index,
-			enum zram_pageflags flag)
-{
-	meta->table[index].value &= ~BIT(flag);
+	meta = zram->meta;
+	nr_migrated = zs_compact(meta->mem_pool);
+	atomic64_add(nr_migrated, &zram->stats.num_migrated);
+	up_read(&zram->init_lock);
+
+	return len;
 }
 
-static size_t zram_get_obj_size(struct zram_meta *meta, u32 index)
+static ssize_t io_stat_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
 {
-	return meta->table[index].value & (BIT(ZRAM_FLAG_SHIFT) - 1);
+	struct zram *zram = dev_to_zram(dev);
+	ssize_t ret;
+
+	down_read(&zram->init_lock);
+	ret = scnprintf(buf, PAGE_SIZE,
+			"%8llu %8llu %8llu %8llu\n",
+			(u64)atomic64_read(&zram->stats.failed_reads),
+			(u64)atomic64_read(&zram->stats.failed_writes),
+			(u64)atomic64_read(&zram->stats.invalid_io),
+			(u64)atomic64_read(&zram->stats.notify_free));
+	up_read(&zram->init_lock);
+
+	return ret;
 }
 
-static void zram_set_obj_size(struct zram_meta *meta,
-					u32 index, size_t size)
+static ssize_t mm_stat_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
 {
-	unsigned long flags = meta->table[index].value >> ZRAM_FLAG_SHIFT;
+	struct zram *zram = dev_to_zram(dev);
+	u64 orig_size, mem_used = 0;
+	long max_used;
+	ssize_t ret;
 
-	meta->table[index].value = (flags << ZRAM_FLAG_SHIFT) | size;
+	down_read(&zram->init_lock);
+	if (init_done(zram))
+		mem_used = zs_get_total_pages(zram->meta->mem_pool);
+
+	orig_size = atomic64_read(&zram->stats.pages_stored);
+	max_used = atomic_long_read(&zram->stats.max_used_pages);
+
+	ret = scnprintf(buf, PAGE_SIZE,
+			"%8llu %8llu %8llu %8lu %8ld %8llu %8llu\n",
+			orig_size << PAGE_SHIFT,
+			(u64)atomic64_read(&zram->stats.compr_data_size),
+			mem_used << PAGE_SHIFT,
+			zram->limit_pages << PAGE_SHIFT,
+			max_used << PAGE_SHIFT,
+			(u64)atomic64_read(&zram->stats.zero_pages),
+			(u64)atomic64_read(&zram->stats.num_migrated));
+	up_read(&zram->init_lock);
+
+	return ret;
 }
 
-static inline int is_partial_io(struct bio_vec *bvec)
+static DEVICE_ATTR_RO(io_stat);
+static DEVICE_ATTR_RO(mm_stat);
+ZRAM_ATTR_RO(num_reads);
+ZRAM_ATTR_RO(num_writes);
+ZRAM_ATTR_RO(failed_reads);
+ZRAM_ATTR_RO(failed_writes);
+ZRAM_ATTR_RO(invalid_io);
+ZRAM_ATTR_RO(notify_free);
+ZRAM_ATTR_RO(zero_pages);
+ZRAM_ATTR_RO(compr_data_size);
+
+static inline bool zram_meta_get(struct zram *zram)
 {
-	return bvec->bv_len != PAGE_SIZE;
+	if (atomic_inc_not_zero(&zram->refcount))
+		return true;
+	return false;
 }
 
-/*
- * Check if request is within bounds and aligned on zram logical blocks.
- */
-static inline int valid_io_request(struct zram *zram,
-		sector_t start, unsigned int size)
+static inline void zram_meta_put(struct zram *zram)
 {
-	u64 end, bound;
-
-	/* unaligned request */
-	if (unlikely(start & (ZRAM_SECTOR_PER_LOGICAL_BLOCK - 1)))
-		return 0;
-	if (unlikely(size & (ZRAM_LOGICAL_BLOCK_SIZE - 1)))
-		return 0;
-
-	end = start + (size >> SECTOR_SHIFT);
-	bound = zram->disksize >> SECTOR_SHIFT;
-	/* out of range range */
-	if (unlikely(start >= bound || end > bound || start > end))
-		return 0;
-
-	/* I/O request is valid */
-	return 1;
+	atomic_dec(&zram->refcount);
 }
 
 static void zram_meta_free(struct zram_meta *meta, u64 disksize)
@@ -394,56 +527,6 @@ out_error:
 	return NULL;
 }
 
-static inline bool zram_meta_get(struct zram *zram)
-{
-	if (atomic_inc_not_zero(&zram->refcount))
-		return true;
-	return false;
-}
-
-static inline void zram_meta_put(struct zram *zram)
-{
-	atomic_dec(&zram->refcount);
-}
-
-static void update_position(u32 *index, int *offset, struct bio_vec *bvec)
-{
-	if (*offset + bvec->bv_len >= PAGE_SIZE)
-		(*index)++;
-	*offset = (*offset + bvec->bv_len) % PAGE_SIZE;
-}
-
-static int page_zero_filled(void *ptr)
-{
-	unsigned int pos;
-	unsigned long *page;
-
-	page = (unsigned long *)ptr;
-
-	for (pos = 0; pos != PAGE_SIZE / sizeof(*page); pos++) {
-		if (page[pos])
-			return 0;
-	}
-
-	return 1;
-}
-
-static void handle_zero_page(struct bio_vec *bvec)
-{
-	struct page *page = bvec->bv_page;
-	void *user_mem;
-
-	user_mem = kmap_atomic(page);
-	if (is_partial_io(bvec))
-		memset(user_mem + bvec->bv_offset, 0, bvec->bv_len);
-	else
-		clear_page(user_mem);
-	kunmap_atomic(user_mem);
-
-	flush_dcache_page(page);
-}
-
-
 /*
  * To protect concurrent access to the same index entry,
  * caller should hold this table index entry's bit_spinlock to
@@ -561,21 +644,6 @@ out_cleanup:
 	return ret;
 }
 
-static inline void update_used_max(struct zram *zram,
-					const unsigned long pages)
-{
-	unsigned long old_max, cur_max;
-
-	old_max = atomic_long_read(&zram->stats.max_used_pages);
-
-	do {
-		cur_max = old_max;
-		if (pages > cur_max)
-			old_max = atomic_long_cmpxchg(
-				&zram->stats.max_used_pages, cur_max, pages);
-	} while (old_max != cur_max);
-}
-
 static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 			   int offset)
 {
@@ -585,8 +653,7 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	struct page *page;
 	unsigned char *user_mem, *cmem, *src, *uncmem = NULL;
 	struct zram_meta *meta = zram->meta;
-	struct zcomp_strm *zstrm;
-	bool locked = false;
+	struct zcomp_strm *zstrm = NULL;
 	unsigned long alloced_pages;
 
 	page = bvec->bv_page;
@@ -606,7 +673,6 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	}
 
 	zstrm = zcomp_strm_find(zram->comp);
-	locked = true;
 	user_mem = kmap_atomic(page);
 
 	if (is_partial_io(bvec)) {
@@ -678,7 +744,7 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	}
 
 	zcomp_strm_release(zram->comp, zstrm);
-	locked = false;
+	zstrm = NULL;
 	zs_unmap_object(meta->mem_pool, handle);
 
 	/*
@@ -696,42 +762,13 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	atomic64_add(clen, &zram->stats.compr_data_size);
 	atomic64_inc(&zram->stats.pages_stored);
 out:
-	if (locked)
+	if (zstrm)
 		zcomp_strm_release(zram->comp, zstrm);
 	if (is_partial_io(bvec))
 		kfree(uncmem);
 	return ret;
 }
 
-static int zram_bvec_rw(struct zram *zram, struct bio_vec *bvec, u32 index,
-			int offset, int rw)
-{
-	unsigned long start_time = jiffies;
-	int ret;
-
-	generic_start_io_acct(rw, bvec->bv_len >> SECTOR_SHIFT,
-			&zram->disk->part0);
-
-	if (rw == READ) {
-		atomic64_inc(&zram->stats.num_reads);
-		ret = zram_bvec_read(zram, bvec, index, offset);
-	} else {
-		atomic64_inc(&zram->stats.num_writes);
-		ret = zram_bvec_write(zram, bvec, index, offset);
-	}
-
-	generic_end_io_acct(rw, &zram->disk->part0, start_time);
-
-	if (unlikely(ret)) {
-		if (rw == READ)
-			atomic64_inc(&zram->stats.failed_reads);
-		else
-			atomic64_inc(&zram->stats.failed_writes);
-	}
-
-	return ret;
-}
-
 /*
  * zram_bio_discard - handler on discard request
  * @index: physical block index in PAGE_SIZE units
@@ -771,149 +808,32 @@ static void zram_bio_discard(struct zram *zram, u32 index,
 	}
 }
 
-static void zram_reset_device(struct zram *zram)
-{
-	struct zram_meta *meta;
-	struct zcomp *comp;
-	u64 disksize;
-
-	down_write(&zram->init_lock);
-
-	zram->limit_pages = 0;
-
-	if (!init_done(zram)) {
-		up_write(&zram->init_lock);
-		return;
-	}
-
-	meta = zram->meta;
-	comp = zram->comp;
-	disksize = zram->disksize;
-	/*
-	 * Refcount will go down to 0 eventually and r/w handler
-	 * cannot handle further I/O so it will bail out by
-	 * check zram_meta_get.
-	 */
-	zram_meta_put(zram);
-	/*
-	 * We want to free zram_meta in process context to avoid
-	 * deadlock between reclaim path and any other locks.
-	 */
-	wait_event(zram->io_done, atomic_read(&zram->refcount) == 0);
-
-	/* Reset stats */
-	memset(&zram->stats, 0, sizeof(zram->stats));
-	zram->disksize = 0;
-	zram->max_comp_streams = 1;
-	set_capacity(zram->disk, 0);
-
-	up_write(&zram->init_lock);
-	/* I/O operation under all of CPU are done so let's free */
-	zram_meta_free(meta, disksize);
-	zcomp_destroy(comp);
-}
-
-static ssize_t disksize_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t len)
-{
-	u64 disksize;
-	struct zcomp *comp;
-	struct zram_meta *meta;
-	struct zram *zram = dev_to_zram(dev);
-	int err;
-
-	disksize = memparse(buf, NULL);
-	if (!disksize)
-		return -EINVAL;
-
-	disksize = PAGE_ALIGN(disksize);
-	meta = zram_meta_alloc(zram->disk->first_minor, disksize);
-	if (!meta)
-		return -ENOMEM;
-
-	comp = zcomp_create(zram->compressor, zram->max_comp_streams);
-	if (IS_ERR(comp)) {
-		pr_info("Cannot initialise %s compressing backend\n",
-				zram->compressor);
-		err = PTR_ERR(comp);
-		goto out_free_meta;
-	}
-
-	down_write(&zram->init_lock);
-	if (init_done(zram)) {
-		pr_info("Cannot change disksize for initialized device\n");
-		err = -EBUSY;
-		goto out_destroy_comp;
-	}
-
-	init_waitqueue_head(&zram->io_done);
-	atomic_set(&zram->refcount, 1);
-	zram->meta = meta;
-	zram->comp = comp;
-	zram->disksize = disksize;
-	set_capacity(zram->disk, zram->disksize >> SECTOR_SHIFT);
-	up_write(&zram->init_lock);
-
-	/*
-	 * Revalidate disk out of the init_lock to avoid lockdep splat.
-	 * It's okay because disk's capacity is protected by init_lock
-	 * so that revalidate_disk always sees up-to-date capacity.
-	 */
-	revalidate_disk(zram->disk);
-
-	return len;
-
-out_destroy_comp:
-	up_write(&zram->init_lock);
-	zcomp_destroy(comp);
-out_free_meta:
-	zram_meta_free(meta, disksize);
-	return err;
-}
-
-static ssize_t reset_store(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t len)
+static int zram_bvec_rw(struct zram *zram, struct bio_vec *bvec, u32 index,
+			int offset, int rw)
 {
+	unsigned long start_time = jiffies;
 	int ret;
-	unsigned short do_reset;
-	struct zram *zram;
-	struct block_device *bdev;
 
-	zram = dev_to_zram(dev);
-	bdev = bdget_disk(zram->disk, 0);
-
-	if (!bdev)
-		return -ENOMEM;
+	generic_start_io_acct(rw, bvec->bv_len >> SECTOR_SHIFT,
+			&zram->disk->part0);
 
-	mutex_lock(&bdev->bd_mutex);
-	/* Do not reset an active device! */
-	if (bdev->bd_openers) {
-		ret = -EBUSY;
-		goto out;
+	if (rw == READ) {
+		atomic64_inc(&zram->stats.num_reads);
+		ret = zram_bvec_read(zram, bvec, index, offset);
+	} else {
+		atomic64_inc(&zram->stats.num_writes);
+		ret = zram_bvec_write(zram, bvec, index, offset);
 	}
 
-	ret = kstrtou16(buf, 10, &do_reset);
-	if (ret)
-		goto out;
+	generic_end_io_acct(rw, &zram->disk->part0, start_time);
 
-	if (!do_reset) {
-		ret = -EINVAL;
-		goto out;
+	if (unlikely(ret)) {
+		if (rw == READ)
+			atomic64_inc(&zram->stats.failed_reads);
+		else
+			atomic64_inc(&zram->stats.failed_writes);
 	}
 
-	/* Make sure all pending I/O is finished */
-	fsync_bdev(bdev);
-	zram_reset_device(zram);
-
-	mutex_unlock(&bdev->bd_mutex);
-	revalidate_disk(zram->disk);
-	bdput(bdev);
-
-	return len;
-
-out:
-	mutex_unlock(&bdev->bd_mutex);
-	bdput(bdev);
 	return ret;
 }
 
@@ -1053,80 +973,185 @@ out:
 	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 void zram_reset_device(struct zram *zram)
+{
+	struct zram_meta *meta;
+	struct zcomp *comp;
+	u64 disksize;
 
-static DEVICE_ATTR_WO(compact);
-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);
+	down_write(&zram->init_lock);
 
-static ssize_t io_stat_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+	zram->limit_pages = 0;
+
+	if (!init_done(zram)) {
+		up_write(&zram->init_lock);
+		return;
+	}
+
+	meta = zram->meta;
+	comp = zram->comp;
+	disksize = zram->disksize;
+	/*
+	 * Refcount will go down to 0 eventually and r/w handler
+	 * cannot handle further I/O so it will bail out by
+	 * check zram_meta_get.
+	 */
+	zram_meta_put(zram);
+	/*
+	 * We want to free zram_meta in process context to avoid
+	 * deadlock between reclaim path and any other locks.
+	 */
+	wait_event(zram->io_done, atomic_read(&zram->refcount) == 0);
+
+	/* Reset stats */
+	memset(&zram->stats, 0, sizeof(zram->stats));
+	zram->disksize = 0;
+	zram->max_comp_streams = 1;
+
+	set_capacity(zram->disk, 0);
+	part_stat_set_all(&zram->disk->part0, 0);
+
+	up_write(&zram->init_lock);
+	/* I/O operation under all of CPU are done so let's free */
+	zram_meta_free(meta, disksize);
+	zcomp_destroy(comp);
+}
+
+static ssize_t disksize_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
 {
+	u64 disksize;
+	struct zcomp *comp;
+	struct zram_meta *meta;
 	struct zram *zram = dev_to_zram(dev);
-	ssize_t ret;
+	int err;
 
-	down_read(&zram->init_lock);
-	ret = scnprintf(buf, PAGE_SIZE,
-			"%8llu %8llu %8llu %8llu\n",
-			(u64)atomic64_read(&zram->stats.failed_reads),
-			(u64)atomic64_read(&zram->stats.failed_writes),
-			(u64)atomic64_read(&zram->stats.invalid_io),
-			(u64)atomic64_read(&zram->stats.notify_free));
-	up_read(&zram->init_lock);
+	disksize = memparse(buf, NULL);
+	if (!disksize)
+		return -EINVAL;
 
-	return ret;
+	disksize = PAGE_ALIGN(disksize);
+	meta = zram_meta_alloc(zram->disk->first_minor, disksize);
+	if (!meta)
+		return -ENOMEM;
+
+	comp = zcomp_create(zram->compressor, zram->max_comp_streams);
+	if (IS_ERR(comp)) {
+		pr_info("Cannot initialise %s compressing backend\n",
+				zram->compressor);
+		err = PTR_ERR(comp);
+		goto out_free_meta;
+	}
+
+	down_write(&zram->init_lock);
+	if (init_done(zram)) {
+		pr_info("Cannot change disksize for initialized device\n");
+		err = -EBUSY;
+		goto out_destroy_comp;
+	}
+
+	init_waitqueue_head(&zram->io_done);
+	atomic_set(&zram->refcount, 1);
+	zram->meta = meta;
+	zram->comp = comp;
+	zram->disksize = disksize;
+	set_capacity(zram->disk, zram->disksize >> SECTOR_SHIFT);
+	up_write(&zram->init_lock);
+
+	/*
+	 * Revalidate disk out of the init_lock to avoid lockdep splat.
+	 * It's okay because disk's capacity is protected by init_lock
+	 * so that revalidate_disk always sees up-to-date capacity.
+	 */
+	revalidate_disk(zram->disk);
+
+	return len;
+
+out_destroy_comp:
+	up_write(&zram->init_lock);
+	zcomp_destroy(comp);
+out_free_meta:
+	zram_meta_free(meta, disksize);
+	return err;
 }
 
-static ssize_t mm_stat_show(struct device *dev,
-		struct device_attribute *attr, char *buf)
+static ssize_t reset_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
 {
-	struct zram *zram = dev_to_zram(dev);
-	u64 orig_size, mem_used = 0;
-	long max_used;
-	ssize_t ret;
+	int ret;
+	unsigned short do_reset;
+	struct zram *zram;
+	struct block_device *bdev;
 
-	down_read(&zram->init_lock);
-	if (init_done(zram))
-		mem_used = zs_get_total_pages(zram->meta->mem_pool);
+	ret = kstrtou16(buf, 10, &do_reset);
+	if (ret)
+		return ret;
 
-	orig_size = atomic64_read(&zram->stats.pages_stored);
-	max_used = atomic_long_read(&zram->stats.max_used_pages);
+	if (!do_reset)
+		return -EINVAL;
 
-	ret = scnprintf(buf, PAGE_SIZE,
-			"%8llu %8llu %8llu %8lu %8ld %8llu %8llu\n",
-			orig_size << PAGE_SHIFT,
-			(u64)atomic64_read(&zram->stats.compr_data_size),
-			mem_used << PAGE_SHIFT,
-			zram->limit_pages << PAGE_SHIFT,
-			max_used << PAGE_SHIFT,
-			(u64)atomic64_read(&zram->stats.zero_pages),
-			(u64)atomic64_read(&zram->stats.num_migrated));
-	up_read(&zram->init_lock);
+	zram = dev_to_zram(dev);
+	bdev = bdget_disk(zram->disk, 0);
+	if (!bdev)
+		return -ENOMEM;
+
+	mutex_lock(&bdev->bd_mutex);
+	/* Do not reset an active device or claimed device */
+	if (bdev->bd_openers || zram->claim) {
+		mutex_unlock(&bdev->bd_mutex);
+		bdput(bdev);
+		return -EBUSY;
+	}
+
+	/* From now on, anyone can't open /dev/zram[0-9] */
+	zram->claim = true;
+	mutex_unlock(&bdev->bd_mutex);
+
+	/* Make sure all the pending I/O are finished */
+	fsync_bdev(bdev);
+	zram_reset_device(zram);
+	revalidate_disk(zram->disk);
+	bdput(bdev);
+
+	mutex_lock(&bdev->bd_mutex);
+	zram->claim = false;
+	mutex_unlock(&bdev->bd_mutex);
+
+	return len;
+}
+
+static int zram_open(struct block_device *bdev, fmode_t mode)
+{
+	int ret = 0;
+	struct zram *zram;
+
+	WARN_ON(!mutex_is_locked(&bdev->bd_mutex));
+
+	zram = bdev->bd_disk->private_data;
+	/* zram was claimed to reset so open request fails */
+	if (zram->claim)
+		ret = -EBUSY;
 
 	return ret;
 }
 
-static DEVICE_ATTR_RO(io_stat);
-static DEVICE_ATTR_RO(mm_stat);
-ZRAM_ATTR_RO(num_reads);
-ZRAM_ATTR_RO(num_writes);
-ZRAM_ATTR_RO(failed_reads);
-ZRAM_ATTR_RO(failed_writes);
-ZRAM_ATTR_RO(invalid_io);
-ZRAM_ATTR_RO(notify_free);
-ZRAM_ATTR_RO(zero_pages);
-ZRAM_ATTR_RO(compr_data_size);
+static const struct block_device_operations zram_devops = {
+	.open = zram_open,
+	.swap_slot_free_notify = zram_slot_free_notify,
+	.rw_page = zram_rw_page,
+	.owner = THIS_MODULE
+};
+
+static DEVICE_ATTR_WO(compact);
+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);
 
 static struct attribute *zram_disk_attrs[] = {
 	&dev_attr_disksize.attr,
@@ -1156,10 +1181,24 @@ static struct attribute_group zram_disk_attr_group = {
 	.attrs = zram_disk_attrs,
 };
 
-static int create_device(struct zram *zram, int device_id)
+/*
+ * Allocate and initialize new zram device. the function returns
+ * '>= 0' device_id upon success, and negative value otherwise.
+ */
+static int zram_add(void)
 {
+	struct zram *zram;
 	struct request_queue *queue;
-	int ret = -ENOMEM;
+	int ret, device_id;
+
+	zram = kzalloc(sizeof(struct zram), GFP_KERNEL);
+	if (!zram)
+		return -ENOMEM;
+
+	ret = idr_alloc(&zram_index_idr, zram, 0, 0, GFP_KERNEL);
+	if (ret < 0)
+		goto out_free_dev;
+	device_id = ret;
 
 	init_rwsem(&zram->init_lock);
 
@@ -1167,12 +1206,13 @@ static int create_device(struct zram *zram, int device_id)
 	if (!queue) {
 		pr_err("Error allocating disk queue for device %d\n",
 			device_id);
-		goto out;
+		ret = -ENOMEM;
+		goto out_free_idr;
 	}
 
 	blk_queue_make_request(queue, zram_make_request);
 
-	 /* gendisk structure */
+	/* gendisk structure */
 	zram->disk = alloc_disk(1);
 	if (!zram->disk) {
 		pr_warn("Error allocating disk structure for device %d\n",
@@ -1230,90 +1270,177 @@ static int create_device(struct zram *zram, int device_id)
 	strlcpy(zram->compressor, default_compressor, sizeof(zram->compressor));
 	zram->meta = NULL;
 	zram->max_comp_streams = 1;
-	return 0;
+
+	pr_info("Added device: %s\n", zram->disk->disk_name);
+	return device_id;
 
 out_free_disk:
 	del_gendisk(zram->disk);
 	put_disk(zram->disk);
 out_free_queue:
 	blk_cleanup_queue(queue);
-out:
+out_free_idr:
+	idr_remove(&zram_index_idr, device_id);
+out_free_dev:
+	kfree(zram);
 	return ret;
 }
 
-static void destroy_devices(unsigned int nr)
+static int zram_remove(struct zram *zram)
+{
+	struct block_device *bdev;
+
+	bdev = bdget_disk(zram->disk, 0);
+	if (!bdev)
+		return -ENOMEM;
+
+	mutex_lock(&bdev->bd_mutex);
+	if (bdev->bd_openers || zram->claim) {
+		mutex_unlock(&bdev->bd_mutex);
+		bdput(bdev);
+		return -EBUSY;
+	}
+
+	zram->claim = true;
+	mutex_unlock(&bdev->bd_mutex);
+
+	/*
+	 * Remove sysfs first, so no one will perform a disksize
+	 * store while we destroy the devices. This also helps during
+	 * hot_remove -- zram_reset_device() is the last holder of
+	 * ->init_lock, no later/concurrent disksize_store() or any
+	 * other sysfs handlers are possible.
+	 */
+	sysfs_remove_group(&disk_to_dev(zram->disk)->kobj,
+			&zram_disk_attr_group);
+
+	/* Make sure all the pending I/O are finished */
+	fsync_bdev(bdev);
+	zram_reset_device(zram);
+	bdput(bdev);
+
+	pr_info("Removed device: %s\n", zram->disk->disk_name);
+
+	idr_remove(&zram_index_idr, zram->disk->first_minor);
+	blk_cleanup_queue(zram->disk->queue);
+	del_gendisk(zram->disk);
+	put_disk(zram->disk);
+	kfree(zram);
+	return 0;
+}
+
+/* zram-control sysfs attributes */
+static ssize_t hot_add_show(struct class *class,
+			struct class_attribute *attr,
+			char *buf)
+{
+	int ret;
+
+	mutex_lock(&zram_index_mutex);
+	ret = zram_add();
+	mutex_unlock(&zram_index_mutex);
+
+	if (ret < 0)
+		return ret;
+	return scnprintf(buf, PAGE_SIZE, "%d\n", ret);
+}
+
+static ssize_t hot_remove_store(struct class *class,
+			struct class_attribute *attr,
+			const char *buf,
+			size_t count)
 {
 	struct zram *zram;
-	unsigned int i;
+	int ret, dev_id;
 
-	for (i = 0; i < nr; i++) {
-		zram = &zram_devices[i];
-		/*
-		 * Remove sysfs first, so no one will perform a disksize
-		 * store while we destroy the devices
-		 */
-		sysfs_remove_group(&disk_to_dev(zram->disk)->kobj,
-				&zram_disk_attr_group);
+	/* dev_id is gendisk->first_minor, which is `int' */
+	ret = kstrtoint(buf, 10, &dev_id);
+	if (ret)
+		return ret;
+	if (dev_id < 0)
+		return -EINVAL;
 
-		zram_reset_device(zram);
+	mutex_lock(&zram_index_mutex);
 
-		blk_cleanup_queue(zram->disk->queue);
-		del_gendisk(zram->disk);
-		put_disk(zram->disk);
-	}
+	zram = idr_find(&zram_index_idr, dev_id);
+	if (zram)
+		ret = zram_remove(zram);
+	else
+		ret = -ENODEV;
+
+	mutex_unlock(&zram_index_mutex);
+	return ret ? ret : count;
+}
+
+static struct class_attribute zram_control_class_attrs[] = {
+	__ATTR_RO(hot_add),
+	__ATTR_WO(hot_remove),
+	__ATTR_NULL,
+};
+
+static struct class zram_control_class = {
+	.name		= "zram-control",
+	.owner		= THIS_MODULE,
+	.class_attrs	= zram_control_class_attrs,
+};
+
+static int zram_remove_cb(int id, void *ptr, void *data)
+{
+	zram_remove(ptr);
+	return 0;
+}
 
-	kfree(zram_devices);
+static void destroy_devices(void)
+{
+	class_unregister(&zram_control_class);
+	idr_for_each(&zram_index_idr, &zram_remove_cb, NULL);
+	idr_destroy(&zram_index_idr);
 	unregister_blkdev(zram_major, "zram");
-	pr_info("Destroyed %u device(s)\n", nr);
 }
 
 static int __init zram_init(void)
 {
-	int ret, dev_id;
+	int ret;
 
-	if (num_devices > max_num_devices) {
-		pr_warn("Invalid value for num_devices: %u\n",
-				num_devices);
-		return -EINVAL;
+	ret = class_register(&zram_control_class);
+	if (ret) {
+		pr_warn("Unable to register zram-control class\n");
+		return ret;
 	}
 
 	zram_major = register_blkdev(0, "zram");
 	if (zram_major <= 0) {
 		pr_warn("Unable to get major number\n");
+		class_unregister(&zram_control_class);
 		return -EBUSY;
 	}
 
-	/* Allocate the device array and initialize each one */
-	zram_devices = kzalloc(num_devices * sizeof(struct zram), GFP_KERNEL);
-	if (!zram_devices) {
-		unregister_blkdev(zram_major, "zram");
-		return -ENOMEM;
-	}
-
-	for (dev_id = 0; dev_id < num_devices; dev_id++) {
-		ret = create_device(&zram_devices[dev_id], dev_id);
-		if (ret)
+	while (num_devices != 0) {
+		mutex_lock(&zram_index_mutex);
+		ret = zram_add();
+		mutex_unlock(&zram_index_mutex);
+		if (ret < 0)
 			goto out_error;
+		num_devices--;
 	}
 
-	pr_info("Created %u device(s)\n", num_devices);
 	return 0;
 
 out_error:
-	destroy_devices(dev_id);
+	destroy_devices();
 	return ret;
 }
 
 static void __exit zram_exit(void)
 {
-	destroy_devices(num_devices);
+	destroy_devices();
 }
 
 module_init(zram_init);
 module_exit(zram_exit);
 
 module_param(num_devices, uint, 0);
-MODULE_PARM_DESC(num_devices, "Number of zram devices");
+MODULE_PARM_DESC(num_devices, "Number of pre-created zram devices");
 
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Nitin Gupta <ngupta@vflare.org>");
diff --git a/drivers/block/zram/zram_drv.h b/drivers/block/zram/zram_drv.h
index 570c598f4ce9..6dbe2df506bf 100644
--- a/drivers/block/zram/zram_drv.h
+++ b/drivers/block/zram/zram_drv.h
@@ -20,12 +20,6 @@
 
 #include "zcomp.h"
 
-/*
- * Some arbitrary value. This is just to catch
- * invalid value for num_devices module parameter.
- */
-static const unsigned max_num_devices = 32;
-
 /*-- Configurable parameters */
 
 /*
@@ -121,5 +115,9 @@ struct zram {
 	 */
 	u64 disksize;	/* bytes */
 	char compressor[10];
+	/*
+	 * zram is claimed so open request will be failed
+	 */
+	bool claim; /* Protected by bdev->bd_mutex */
 };
 #endif
diff --git a/drivers/bluetooth/Kconfig b/drivers/bluetooth/Kconfig
index ed5c2738bea2..2e777071e1dc 100644
--- a/drivers/bluetooth/Kconfig
+++ b/drivers/bluetooth/Kconfig
@@ -9,6 +9,10 @@ config BT_BCM
 	tristate
 	select FW_LOADER
 
+config BT_RTL
+	tristate
+	select FW_LOADER
+
 config BT_HCIBTUSB
 	tristate "HCI USB driver"
 	depends on USB
@@ -32,6 +36,17 @@ config BT_HCIBTUSB_BCM
 
 	  Say Y here to compile support for Broadcom protocol.
 
+config BT_HCIBTUSB_RTL
+	bool "Realtek protocol support"
+	depends on BT_HCIBTUSB
+	select BT_RTL
+	default y
+	help
+	  The Realtek protocol support enables firmware and configuration
+	  download support for Realtek Bluetooth controllers.
+
+	  Say Y here to compile support for Realtek protocol.
+
 config BT_HCIBTSDIO
 	tristate "HCI SDIO driver"
 	depends on MMC
diff --git a/drivers/bluetooth/Makefile b/drivers/bluetooth/Makefile
index dd0d9c40b999..f40e194e7080 100644
--- a/drivers/bluetooth/Makefile
+++ b/drivers/bluetooth/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_BT_MRVL)		+= btmrvl.o
 obj-$(CONFIG_BT_MRVL_SDIO)	+= btmrvl_sdio.o
 obj-$(CONFIG_BT_WILINK)		+= btwilink.o
 obj-$(CONFIG_BT_BCM)		+= btbcm.o
+obj-$(CONFIG_BT_RTL)		+= btrtl.o
 
 btmrvl-y			:= btmrvl_main.o
 btmrvl-$(CONFIG_DEBUG_FS)	+= btmrvl_debugfs.o
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index 8c81af6dbe06..e527a3e13939 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -80,6 +80,7 @@ static const struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x0489, 0xe057) },
 	{ USB_DEVICE(0x0489, 0xe056) },
 	{ USB_DEVICE(0x0489, 0xe05f) },
+	{ USB_DEVICE(0x0489, 0xe076) },
 	{ USB_DEVICE(0x0489, 0xe078) },
 	{ USB_DEVICE(0x04c5, 0x1330) },
 	{ USB_DEVICE(0x04CA, 0x3004) },
@@ -88,6 +89,7 @@ static const struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x04CA, 0x3007) },
 	{ USB_DEVICE(0x04CA, 0x3008) },
 	{ USB_DEVICE(0x04CA, 0x300b) },
+	{ USB_DEVICE(0x04CA, 0x300d) },
 	{ USB_DEVICE(0x04CA, 0x300f) },
 	{ USB_DEVICE(0x04CA, 0x3010) },
 	{ USB_DEVICE(0x0930, 0x0219) },
@@ -113,6 +115,7 @@ static const struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x13d3, 0x3408) },
 	{ USB_DEVICE(0x13d3, 0x3423) },
 	{ USB_DEVICE(0x13d3, 0x3432) },
+	{ USB_DEVICE(0x13d3, 0x3474) },
 
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xE02C) },
@@ -137,6 +140,7 @@ static const struct usb_device_id ath3k_blist_tbl[] = {
 	{ USB_DEVICE(0x0489, 0xe056), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe057), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe05f), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe076), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe078), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04c5, 0x1330), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3004), .driver_info = BTUSB_ATH3012 },
@@ -145,6 +149,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, 0x300d), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x300f), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3010), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
@@ -170,6 +175,7 @@ static const struct usb_device_id ath3k_blist_tbl[] = {
 	{ USB_DEVICE(0x13d3, 0x3408), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x13d3, 0x3423), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x13d3, 0x3432), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3474), .driver_info = BTUSB_ATH3012 },
 
 	/* Atheros AR5BBU22 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xE036), .driver_info = BTUSB_ATH3012 },
diff --git a/drivers/bluetooth/bt3c_cs.c b/drivers/bluetooth/bt3c_cs.c
index 6de97b3871b0..7aab65427d38 100644
--- a/drivers/bluetooth/bt3c_cs.c
+++ b/drivers/bluetooth/bt3c_cs.c
@@ -202,9 +202,8 @@ static void bt3c_write_wakeup(struct bt3c_info *info)
 		/* Send frame */
 		len = bt3c_write(iobase, 256, skb->data, skb->len);
 
-		if (len != skb->len) {
+		if (len != skb->len)
 			BT_ERR("Very strange");
-		}
 
 		kfree_skb(skb);
 
diff --git a/drivers/bluetooth/btbcm.c b/drivers/bluetooth/btbcm.c
index 4bba86677adc..1e1a4323a71f 100644
--- a/drivers/bluetooth/btbcm.c
+++ b/drivers/bluetooth/btbcm.c
@@ -33,6 +33,7 @@
 #define VERSION "0.1"
 
 #define BDADDR_BCM20702A0 (&(bdaddr_t) {{0x00, 0xa0, 0x02, 0x70, 0x20, 0x00}})
+#define BDADDR_BCM4324B3 (&(bdaddr_t) {{0x00, 0x00, 0x00, 0xb3, 0x24, 0x43}})
 
 int btbcm_check_bdaddr(struct hci_dev *hdev)
 {
@@ -55,17 +56,19 @@ int btbcm_check_bdaddr(struct hci_dev *hdev)
 	}
 
 	bda = (struct hci_rp_read_bd_addr *)skb->data;
-	if (bda->status) {
-		BT_ERR("%s: BCM: Device address result failed (%02x)",
-		       hdev->name, bda->status);
-		kfree_skb(skb);
-		return -bt_to_errno(bda->status);
-	}
 
-	/* The address 00:20:70:02:A0:00 indicates a BCM20702A0 controller
+	/* Check if the address indicates a controller with either an
+	 * invalid or default address. In both cases the device needs
+	 * to be marked as not having a valid address.
+	 *
+	 * The address 00:20:70:02:A0:00 indicates a BCM20702A0 controller
 	 * with no configured address.
+	 *
+	 * The address 43:24:B3:00:00:00 indicates a BCM4324B3 controller
+	 * with waiting for configuration state.
 	 */
-	if (!bacmp(&bda->bdaddr, BDADDR_BCM20702A0)) {
+	if (!bacmp(&bda->bdaddr, BDADDR_BCM20702A0) ||
+	    !bacmp(&bda->bdaddr, BDADDR_BCM4324B3)) {
 		BT_INFO("%s: BCM: Using default device address (%pMR)",
 			hdev->name, &bda->bdaddr);
 		set_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks);
@@ -95,21 +98,14 @@ int btbcm_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 }
 EXPORT_SYMBOL_GPL(btbcm_set_bdaddr);
 
-int btbcm_patchram(struct hci_dev *hdev, const char *firmware)
+int btbcm_patchram(struct hci_dev *hdev, const struct firmware *fw)
 {
 	const struct hci_command_hdr *cmd;
-	const struct firmware *fw;
 	const u8 *fw_ptr;
 	size_t fw_size;
 	struct sk_buff *skb;
 	u16 opcode;
-	int err;
-
-	err = request_firmware(&fw, firmware, &hdev->dev);
-	if (err < 0) {
-		BT_INFO("%s: BCM: Patch %s not found", hdev->name, firmware);
-		return err;
-	}
+	int err = 0;
 
 	/* Start Download */
 	skb = __hci_cmd_sync(hdev, 0xfc2e, 0, NULL, HCI_INIT_TIMEOUT);
@@ -135,8 +131,7 @@ int btbcm_patchram(struct hci_dev *hdev, const char *firmware)
 		fw_size -= sizeof(*cmd);
 
 		if (fw_size < cmd->plen) {
-			BT_ERR("%s: BCM: Patch %s is corrupted", hdev->name,
-			       firmware);
+			BT_ERR("%s: BCM: Patch is corrupted", hdev->name);
 			err = -EINVAL;
 			goto done;
 		}
@@ -162,7 +157,6 @@ int btbcm_patchram(struct hci_dev *hdev, const char *firmware)
 	msleep(250);
 
 done:
-	release_firmware(fw);
 	return err;
 }
 EXPORT_SYMBOL(btbcm_patchram);
@@ -248,9 +242,101 @@ static const struct {
 	const char *name;
 } bcm_uart_subver_table[] = {
 	{ 0x410e, "BCM43341B0"	},	/* 002.001.014 */
+	{ 0x4406, "BCM4324B3"	},	/* 002.004.006 */
+	{ 0x610c, "BCM4354"	},	/* 003.001.012 */
 	{ }
 };
 
+int btbcm_initialize(struct hci_dev *hdev, char *fw_name, size_t len)
+{
+	u16 subver, rev;
+	const char *hw_name = NULL;
+	struct sk_buff *skb;
+	struct hci_rp_read_local_version *ver;
+	int i, err;
+
+	/* Reset */
+	err = btbcm_reset(hdev);
+	if (err)
+		return err;
+
+	/* Read Local Version Info */
+	skb = btbcm_read_local_version(hdev);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	ver = (struct hci_rp_read_local_version *)skb->data;
+	rev = le16_to_cpu(ver->hci_rev);
+	subver = le16_to_cpu(ver->lmp_subver);
+	kfree_skb(skb);
+
+	/* Read Verbose Config Version Info */
+	skb = btbcm_read_verbose_config(hdev);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	BT_INFO("%s: BCM: chip id %u", hdev->name, skb->data[1]);
+	kfree_skb(skb);
+
+	switch ((rev & 0xf000) >> 12) {
+	case 0:
+	case 1:
+	case 3:
+		for (i = 0; bcm_uart_subver_table[i].name; i++) {
+			if (subver == bcm_uart_subver_table[i].subver) {
+				hw_name = bcm_uart_subver_table[i].name;
+				break;
+			}
+		}
+
+		snprintf(fw_name, len, "brcm/%s.hcd", hw_name ? : "BCM");
+		break;
+	default:
+		return 0;
+	}
+
+	BT_INFO("%s: %s (%3.3u.%3.3u.%3.3u) build %4.4u", hdev->name,
+		hw_name ? : "BCM", (subver & 0x7000) >> 13,
+		(subver & 0x1f00) >> 8, (subver & 0x00ff), rev & 0x0fff);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btbcm_initialize);
+
+int btbcm_finalize(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+	struct hci_rp_read_local_version *ver;
+	u16 subver, rev;
+	int err;
+
+	/* Reset */
+	err = btbcm_reset(hdev);
+	if (err)
+		return err;
+
+	/* Read Local Version Info */
+	skb = btbcm_read_local_version(hdev);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	ver = (struct hci_rp_read_local_version *)skb->data;
+	rev = le16_to_cpu(ver->hci_rev);
+	subver = le16_to_cpu(ver->lmp_subver);
+	kfree_skb(skb);
+
+	BT_INFO("%s: BCM (%3.3u.%3.3u.%3.3u) build %4.4u", hdev->name,
+		(subver & 0x7000) >> 13, (subver & 0x1f00) >> 8,
+		(subver & 0x00ff), rev & 0x0fff);
+
+	btbcm_check_bdaddr(hdev);
+
+	set_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btbcm_finalize);
+
 static const struct {
 	u16 subver;
 	const char *name;
@@ -271,6 +357,7 @@ static const struct {
 int btbcm_setup_patchram(struct hci_dev *hdev)
 {
 	char fw_name[64];
+	const struct firmware *fw;
 	u16 subver, rev, pid, vid;
 	const char *hw_name = NULL;
 	struct sk_buff *skb;
@@ -302,6 +389,7 @@ int btbcm_setup_patchram(struct hci_dev *hdev)
 
 	switch ((rev & 0xf000) >> 12) {
 	case 0:
+	case 3:
 		for (i = 0; bcm_uart_subver_table[i].name; i++) {
 			if (subver == bcm_uart_subver_table[i].subver) {
 				hw_name = bcm_uart_subver_table[i].name;
@@ -341,9 +429,15 @@ int btbcm_setup_patchram(struct hci_dev *hdev)
 		hw_name ? : "BCM", (subver & 0x7000) >> 13,
 		(subver & 0x1f00) >> 8, (subver & 0x00ff), rev & 0x0fff);
 
-	err = btbcm_patchram(hdev, fw_name);
-	if (err == -ENOENT)
+	err = request_firmware(&fw, fw_name, &hdev->dev);
+	if (err < 0) {
+		BT_INFO("%s: BCM: Patch %s not found", hdev->name, fw_name);
 		return 0;
+	}
+
+	btbcm_patchram(hdev, fw);
+
+	release_firmware(fw);
 
 	/* Reset */
 	err = btbcm_reset(hdev);
diff --git a/drivers/bluetooth/btbcm.h b/drivers/bluetooth/btbcm.h
index eb6ab5f9483d..d9e6b41658e5 100644
--- a/drivers/bluetooth/btbcm.h
+++ b/drivers/bluetooth/btbcm.h
@@ -21,15 +21,61 @@
  *
  */
 
+#define BCM_UART_CLOCK_48MHZ	0x01
+#define BCM_UART_CLOCK_24MHZ	0x02
+
+struct bcm_update_uart_baud_rate {
+	__le16 zero;
+	__le32 baud_rate;
+} __packed;
+
+struct bcm_write_uart_clock_setting {
+	__u8 type;
+} __packed;
+
+struct bcm_set_sleep_mode {
+	__u8 sleep_mode;
+	__u8 idle_host;
+	__u8 idle_dev;
+	__u8 bt_wake_active;
+	__u8 host_wake_active;
+	__u8 allow_host_sleep;
+	__u8 combine_modes;
+	__u8 tristate_control;
+	__u8 usb_auto_sleep;
+	__u8 usb_resume_timeout;
+	__u8 pulsed_host_wake;
+	__u8 break_to_host;
+} __packed;
+
+struct bcm_set_pcm_int_params {
+	__u8 routing;
+	__u8 rate;
+	__u8 frame_sync;
+	__u8 sync_mode;
+	__u8 clock_mode;
+} __packed;
+
+struct bcm_set_pcm_format_params {
+	__u8 lsb_first;
+	__u8 fill_value;
+	__u8 fill_method;
+	__u8 fill_num;
+	__u8 right_justify;
+} __packed;
+
 #if IS_ENABLED(CONFIG_BT_BCM)
 
 int btbcm_check_bdaddr(struct hci_dev *hdev);
 int btbcm_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr);
-int btbcm_patchram(struct hci_dev *hdev, const char *firmware);
+int btbcm_patchram(struct hci_dev *hdev, const struct firmware *fw);
 
 int btbcm_setup_patchram(struct hci_dev *hdev);
 int btbcm_setup_apple(struct hci_dev *hdev);
 
+int btbcm_initialize(struct hci_dev *hdev, char *fw_name, size_t len);
+int btbcm_finalize(struct hci_dev *hdev);
+
 #else
 
 static inline int btbcm_check_bdaddr(struct hci_dev *hdev)
@@ -42,7 +88,7 @@ static inline int btbcm_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 	return -EOPNOTSUPP;
 }
 
-static inline int btbcm_patchram(struct hci_dev *hdev, const char *firmware)
+static inline int btbcm_patchram(struct hci_dev *hdev, const struct firmware *fw)
 {
 	return -EOPNOTSUPP;
 }
@@ -57,4 +103,15 @@ static inline int btbcm_setup_apple(struct hci_dev *hdev)
 	return 0;
 }
 
+static inline int btbcm_initialize(struct hci_dev *hdev, char *fw_name,
+				   size_t len)
+{
+	return 0;
+}
+
+static inline int btbcm_finalize(struct hci_dev *hdev)
+{
+	return 0;
+}
+
 #endif
diff --git a/drivers/bluetooth/btintel.c b/drivers/bluetooth/btintel.c
index 2d43d4279b00..828f2f8d1568 100644
--- a/drivers/bluetooth/btintel.c
+++ b/drivers/bluetooth/btintel.c
@@ -53,12 +53,6 @@ int btintel_check_bdaddr(struct hci_dev *hdev)
 	}
 
 	bda = (struct hci_rp_read_bd_addr *)skb->data;
-	if (bda->status) {
-		BT_ERR("%s: Intel device address result failed (%02x)",
-		       hdev->name, bda->status);
-		kfree_skb(skb);
-		return -bt_to_errno(bda->status);
-	}
 
 	/* For some Intel based controllers, the default Bluetooth device
 	 * address 00:03:19:9E:8B:00 can be found. These controllers are
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 01d6da577eeb..b9a811900f6a 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -1217,7 +1217,7 @@ static void btmrvl_sdio_dump_firmware(struct btmrvl_private *priv)
 	unsigned int reg, reg_start, reg_end;
 	enum rdwr_status stat;
 	u8 *dbg_ptr, *end_ptr, *fw_dump_data, *fw_dump_ptr;
-	u8 dump_num, idx, i, read_reg, doneflag = 0;
+	u8 dump_num = 0, idx, i, read_reg, doneflag = 0;
 	u32 memory_size, fw_dump_len = 0;
 
 	/* dump sdio register first */
diff --git a/drivers/bluetooth/btrtl.c b/drivers/bluetooth/btrtl.c
new file mode 100644
index 000000000000..84288938f7f2
--- /dev/null
+++ b/drivers/bluetooth/btrtl.c
@@ -0,0 +1,390 @@
+/*
+ *  Bluetooth support for Realtek devices
+ *
+ *  Copyright (C) 2015 Endless Mobile, Inc.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/firmware.h>
+#include <asm/unaligned.h>
+#include <linux/usb.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "btrtl.h"
+
+#define VERSION "0.1"
+
+#define RTL_EPATCH_SIGNATURE	"Realtech"
+#define RTL_ROM_LMP_3499	0x3499
+#define RTL_ROM_LMP_8723A	0x1200
+#define RTL_ROM_LMP_8723B	0x8723
+#define RTL_ROM_LMP_8821A	0x8821
+#define RTL_ROM_LMP_8761A	0x8761
+
+static int rtl_read_rom_version(struct hci_dev *hdev, u8 *version)
+{
+	struct rtl_rom_version_evt *rom_version;
+	struct sk_buff *skb;
+
+	/* Read RTL ROM version command */
+	skb = __hci_cmd_sync(hdev, 0xfc6d, 0, NULL, HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s: Read ROM version failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return PTR_ERR(skb);
+	}
+
+	if (skb->len != sizeof(*rom_version)) {
+		BT_ERR("%s: RTL version event length mismatch", hdev->name);
+		kfree_skb(skb);
+		return -EIO;
+	}
+
+	rom_version = (struct rtl_rom_version_evt *)skb->data;
+	BT_INFO("%s: rom_version status=%x version=%x",
+		hdev->name, rom_version->status, rom_version->version);
+
+	*version = rom_version->version;
+
+	kfree_skb(skb);
+	return 0;
+}
+
+static int rtl8723b_parse_firmware(struct hci_dev *hdev, u16 lmp_subver,
+				   const struct firmware *fw,
+				   unsigned char **_buf)
+{
+	const u8 extension_sig[] = { 0x51, 0x04, 0xfd, 0x77 };
+	struct rtl_epatch_header *epatch_info;
+	unsigned char *buf;
+	int i, ret, len;
+	size_t min_size;
+	u8 opcode, length, data, rom_version = 0;
+	int project_id = -1;
+	const unsigned char *fwptr, *chip_id_base;
+	const unsigned char *patch_length_base, *patch_offset_base;
+	u32 patch_offset = 0;
+	u16 patch_length, num_patches;
+	const u16 project_id_to_lmp_subver[] = {
+		RTL_ROM_LMP_8723A,
+		RTL_ROM_LMP_8723B,
+		RTL_ROM_LMP_8821A,
+		RTL_ROM_LMP_8761A
+	};
+
+	ret = rtl_read_rom_version(hdev, &rom_version);
+	if (ret)
+		return ret;
+
+	min_size = sizeof(struct rtl_epatch_header) + sizeof(extension_sig) + 3;
+	if (fw->size < min_size)
+		return -EINVAL;
+
+	fwptr = fw->data + fw->size - sizeof(extension_sig);
+	if (memcmp(fwptr, extension_sig, sizeof(extension_sig)) != 0) {
+		BT_ERR("%s: extension section signature mismatch", hdev->name);
+		return -EINVAL;
+	}
+
+	/* Loop from the end of the firmware parsing instructions, until
+	 * we find an instruction that identifies the "project ID" for the
+	 * hardware supported by this firwmare file.
+	 * Once we have that, we double-check that that project_id is suitable
+	 * for the hardware we are working with.
+	 */
+	while (fwptr >= fw->data + (sizeof(struct rtl_epatch_header) + 3)) {
+		opcode = *--fwptr;
+		length = *--fwptr;
+		data = *--fwptr;
+
+		BT_DBG("check op=%x len=%x data=%x", opcode, length, data);
+
+		if (opcode == 0xff) /* EOF */
+			break;
+
+		if (length == 0) {
+			BT_ERR("%s: found instruction with length 0",
+			       hdev->name);
+			return -EINVAL;
+		}
+
+		if (opcode == 0 && length == 1) {
+			project_id = data;
+			break;
+		}
+
+		fwptr -= length;
+	}
+
+	if (project_id < 0) {
+		BT_ERR("%s: failed to find version instruction", hdev->name);
+		return -EINVAL;
+	}
+
+	if (project_id >= ARRAY_SIZE(project_id_to_lmp_subver)) {
+		BT_ERR("%s: unknown project id %d", hdev->name, project_id);
+		return -EINVAL;
+	}
+
+	if (lmp_subver != project_id_to_lmp_subver[project_id]) {
+		BT_ERR("%s: firmware is for %x but this is a %x", hdev->name,
+		       project_id_to_lmp_subver[project_id], lmp_subver);
+		return -EINVAL;
+	}
+
+	epatch_info = (struct rtl_epatch_header *)fw->data;
+	if (memcmp(epatch_info->signature, RTL_EPATCH_SIGNATURE, 8) != 0) {
+		BT_ERR("%s: bad EPATCH signature", hdev->name);
+		return -EINVAL;
+	}
+
+	num_patches = le16_to_cpu(epatch_info->num_patches);
+	BT_DBG("fw_version=%x, num_patches=%d",
+	       le32_to_cpu(epatch_info->fw_version), num_patches);
+
+	/* After the rtl_epatch_header there is a funky patch metadata section.
+	 * Assuming 2 patches, the layout is:
+	 * ChipID1 ChipID2 PatchLength1 PatchLength2 PatchOffset1 PatchOffset2
+	 *
+	 * Find the right patch for this chip.
+	 */
+	min_size += 8 * num_patches;
+	if (fw->size < min_size)
+		return -EINVAL;
+
+	chip_id_base = fw->data + sizeof(struct rtl_epatch_header);
+	patch_length_base = chip_id_base + (sizeof(u16) * num_patches);
+	patch_offset_base = patch_length_base + (sizeof(u16) * num_patches);
+	for (i = 0; i < num_patches; i++) {
+		u16 chip_id = get_unaligned_le16(chip_id_base +
+						 (i * sizeof(u16)));
+		if (chip_id == rom_version + 1) {
+			patch_length = get_unaligned_le16(patch_length_base +
+							  (i * sizeof(u16)));
+			patch_offset = get_unaligned_le32(patch_offset_base +
+							  (i * sizeof(u32)));
+			break;
+		}
+	}
+
+	if (!patch_offset) {
+		BT_ERR("%s: didn't find patch for chip id %d",
+		       hdev->name, rom_version);
+		return -EINVAL;
+	}
+
+	BT_DBG("length=%x offset=%x index %d", patch_length, patch_offset, i);
+	min_size = patch_offset + patch_length;
+	if (fw->size < min_size)
+		return -EINVAL;
+
+	/* Copy the firmware into a new buffer and write the version at
+	 * the end.
+	 */
+	len = patch_length;
+	buf = kmemdup(fw->data + patch_offset, patch_length, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	memcpy(buf + patch_length - 4, &epatch_info->fw_version, 4);
+
+	*_buf = buf;
+	return len;
+}
+
+static int rtl_download_firmware(struct hci_dev *hdev,
+				 const unsigned char *data, int fw_len)
+{
+	struct rtl_download_cmd *dl_cmd;
+	int frag_num = fw_len / RTL_FRAG_LEN + 1;
+	int frag_len = RTL_FRAG_LEN;
+	int ret = 0;
+	int i;
+
+	dl_cmd = kmalloc(sizeof(struct rtl_download_cmd), GFP_KERNEL);
+	if (!dl_cmd)
+		return -ENOMEM;
+
+	for (i = 0; i < frag_num; i++) {
+		struct sk_buff *skb;
+
+		BT_DBG("download fw (%d/%d)", i, frag_num);
+
+		dl_cmd->index = i;
+		if (i == (frag_num - 1)) {
+			dl_cmd->index |= 0x80; /* data end */
+			frag_len = fw_len % RTL_FRAG_LEN;
+		}
+		memcpy(dl_cmd->data, data, frag_len);
+
+		/* Send download command */
+		skb = __hci_cmd_sync(hdev, 0xfc20, frag_len + 1, dl_cmd,
+				     HCI_INIT_TIMEOUT);
+		if (IS_ERR(skb)) {
+			BT_ERR("%s: download fw command failed (%ld)",
+			       hdev->name, PTR_ERR(skb));
+			ret = -PTR_ERR(skb);
+			goto out;
+		}
+
+		if (skb->len != sizeof(struct rtl_download_response)) {
+			BT_ERR("%s: download fw event length mismatch",
+			       hdev->name);
+			kfree_skb(skb);
+			ret = -EIO;
+			goto out;
+		}
+
+		kfree_skb(skb);
+		data += RTL_FRAG_LEN;
+	}
+
+out:
+	kfree(dl_cmd);
+	return ret;
+}
+
+static int btrtl_setup_rtl8723a(struct hci_dev *hdev)
+{
+	const struct firmware *fw;
+	int ret;
+
+	BT_INFO("%s: rtl: loading rtl_bt/rtl8723a_fw.bin", hdev->name);
+	ret = request_firmware(&fw, "rtl_bt/rtl8723a_fw.bin", &hdev->dev);
+	if (ret < 0) {
+		BT_ERR("%s: Failed to load rtl_bt/rtl8723a_fw.bin", hdev->name);
+		return ret;
+	}
+
+	if (fw->size < 8) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Check that the firmware doesn't have the epatch signature
+	 * (which is only for RTL8723B and newer).
+	 */
+	if (!memcmp(fw->data, RTL_EPATCH_SIGNATURE, 8)) {
+		BT_ERR("%s: unexpected EPATCH signature!", hdev->name);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = rtl_download_firmware(hdev, fw->data, fw->size);
+
+out:
+	release_firmware(fw);
+	return ret;
+}
+
+static int btrtl_setup_rtl8723b(struct hci_dev *hdev, u16 lmp_subver,
+				const char *fw_name)
+{
+	unsigned char *fw_data = NULL;
+	const struct firmware *fw;
+	int ret;
+
+	BT_INFO("%s: rtl: loading %s", hdev->name, fw_name);
+	ret = request_firmware(&fw, fw_name, &hdev->dev);
+	if (ret < 0) {
+		BT_ERR("%s: Failed to load %s", hdev->name, fw_name);
+		return ret;
+	}
+
+	ret = rtl8723b_parse_firmware(hdev, lmp_subver, fw, &fw_data);
+	if (ret < 0)
+		goto out;
+
+	ret = rtl_download_firmware(hdev, fw_data, ret);
+	kfree(fw_data);
+	if (ret < 0)
+		goto out;
+
+out:
+	release_firmware(fw);
+	return ret;
+}
+
+static struct sk_buff *btrtl_read_local_version(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
+			     HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION failed (%ld)",
+		       hdev->name, PTR_ERR(skb));
+		return skb;
+	}
+
+	if (skb->len != sizeof(struct hci_rp_read_local_version)) {
+		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION event length mismatch",
+		       hdev->name);
+		kfree_skb(skb);
+		return ERR_PTR(-EIO);
+	}
+
+	return skb;
+}
+
+int btrtl_setup_realtek(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+	struct hci_rp_read_local_version *resp;
+	u16 lmp_subver;
+
+	skb = btrtl_read_local_version(hdev);
+	if (IS_ERR(skb))
+		return -PTR_ERR(skb);
+
+	resp = (struct hci_rp_read_local_version *)skb->data;
+	BT_INFO("%s: rtl: examining hci_ver=%02x hci_rev=%04x lmp_ver=%02x "
+		"lmp_subver=%04x", hdev->name, resp->hci_ver, resp->hci_rev,
+		resp->lmp_ver, resp->lmp_subver);
+
+	lmp_subver = le16_to_cpu(resp->lmp_subver);
+	kfree_skb(skb);
+
+	/* Match a set of subver values that correspond to stock firmware,
+	 * which is not compatible with standard btusb.
+	 * If matched, upload an alternative firmware that does conform to
+	 * standard btusb. Once that firmware is uploaded, the subver changes
+	 * to a different value.
+	 */
+	switch (lmp_subver) {
+	case RTL_ROM_LMP_8723A:
+	case RTL_ROM_LMP_3499:
+		return btrtl_setup_rtl8723a(hdev);
+	case RTL_ROM_LMP_8723B:
+		return btrtl_setup_rtl8723b(hdev, lmp_subver,
+					    "rtl_bt/rtl8723b_fw.bin");
+	case RTL_ROM_LMP_8821A:
+		return btrtl_setup_rtl8723b(hdev, lmp_subver,
+					    "rtl_bt/rtl8821a_fw.bin");
+	case RTL_ROM_LMP_8761A:
+		return btrtl_setup_rtl8723b(hdev, lmp_subver,
+					    "rtl_bt/rtl8761a_fw.bin");
+	default:
+		BT_INFO("rtl: assuming no firmware upload needed.");
+		return 0;
+	}
+}
+EXPORT_SYMBOL_GPL(btrtl_setup_realtek);
+
+MODULE_AUTHOR("Daniel Drake <drake@endlessm.com>");
+MODULE_DESCRIPTION("Bluetooth support for Realtek devices ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/bluetooth/btrtl.h b/drivers/bluetooth/btrtl.h
new file mode 100644
index 000000000000..38ffe4890cd1
--- /dev/null
+++ b/drivers/bluetooth/btrtl.h
@@ -0,0 +1,52 @@
+/*
+ *  Bluetooth support for Realtek devices
+ *
+ *  Copyright (C) 2015 Endless Mobile, Inc.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ */
+
+#define RTL_FRAG_LEN 252
+
+struct rtl_download_cmd {
+	__u8 index;
+	__u8 data[RTL_FRAG_LEN];
+} __packed;
+
+struct rtl_download_response {
+	__u8 status;
+	__u8 index;
+} __packed;
+
+struct rtl_rom_version_evt {
+	__u8 status;
+	__u8 version;
+} __packed;
+
+struct rtl_epatch_header {
+	__u8 signature[8];
+	__le32 fw_version;
+	__le16 num_patches;
+} __packed;
+
+#if IS_ENABLED(CONFIG_BT_RTL)
+
+int btrtl_setup_realtek(struct hci_dev *hdev);
+
+#else
+
+static inline int btrtl_setup_realtek(struct hci_dev *hdev)
+{
+	return -EOPNOTSUPP;
+}
+
+#endif
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 3c10d4dfe9a7..b4cf8d9c9dac 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -31,13 +31,14 @@
 
 #include "btintel.h"
 #include "btbcm.h"
+#include "btrtl.h"
 
 #define VERSION "0.8"
 
 static bool disable_scofix;
 static bool force_scofix;
 
-static bool reset = 1;
+static bool reset = true;
 
 static struct usb_driver btusb_driver;
 
@@ -178,6 +179,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x0489, 0xe056), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe057), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe05f), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0489, 0xe076), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0489, 0xe078), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04c5, 0x1330), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3004), .driver_info = BTUSB_ATH3012 },
@@ -186,6 +188,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, 0x300d), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x300f), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x04ca, 0x3010), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
@@ -211,6 +214,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x13d3, 0x3408), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x13d3, 0x3423), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x13d3, 0x3432), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x13d3, 0x3474), .driver_info = BTUSB_ATH3012 },
 
 	/* Atheros AR5BBU12 with sflash firmware */
 	{ USB_DEVICE(0x0489, 0xe02c), .driver_info = BTUSB_IGNORE },
@@ -265,7 +269,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x0e5e, 0x6622), .driver_info = BTUSB_BROKEN_ISOC },
 
 	/* Roper Class 1 Bluetooth Dongle (Silicon Wave based) */
-	{ USB_DEVICE(0x1300, 0x0001), .driver_info = BTUSB_SWAVE },
+	{ USB_DEVICE(0x1310, 0x0001), .driver_info = BTUSB_SWAVE },
 
 	/* Digianswer devices */
 	{ USB_DEVICE(0x08fd, 0x0001), .driver_info = BTUSB_DIGIANSWER },
@@ -330,6 +334,7 @@ static const struct usb_device_id blacklist_table[] = {
 #define BTUSB_FIRMWARE_LOADED	7
 #define BTUSB_FIRMWARE_FAILED	8
 #define BTUSB_BOOTING		9
+#define BTUSB_RESET_RESUME	10
 
 struct btusb_data {
 	struct hci_dev       *hdev;
@@ -1298,28 +1303,6 @@ static void btusb_waker(struct work_struct *work)
 	usb_autopm_put_interface(data->intf);
 }
 
-static struct sk_buff *btusb_read_local_version(struct hci_dev *hdev)
-{
-	struct sk_buff *skb;
-
-	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
-			     HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
-		return skb;
-	}
-
-	if (skb->len != sizeof(struct hci_rp_read_local_version)) {
-		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION event length mismatch",
-		       hdev->name);
-		kfree_skb(skb);
-		return ERR_PTR(-EIO);
-	}
-
-	return skb;
-}
-
 static int btusb_setup_bcm92035(struct hci_dev *hdev)
 {
 	struct sk_buff *skb;
@@ -1340,408 +1323,40 @@ static int btusb_setup_csr(struct hci_dev *hdev)
 {
 	struct hci_rp_read_local_version *rp;
 	struct sk_buff *skb;
-	int ret;
 
 	BT_DBG("%s", hdev->name);
 
-	skb = btusb_read_local_version(hdev);
-	if (IS_ERR(skb))
-		return -PTR_ERR(skb);
-
-	rp = (struct hci_rp_read_local_version *)skb->data;
-
-	if (!rp->status) {
-		if (le16_to_cpu(rp->manufacturer) != 10) {
-			/* Clear the reset quirk since this is not an actual
-			 * early Bluetooth 1.1 device from CSR.
-			 */
-			clear_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
-
-			/* These fake CSR controllers have all a broken
-			 * stored link key handling and so just disable it.
-			 */
-			set_bit(HCI_QUIRK_BROKEN_STORED_LINK_KEY,
-				&hdev->quirks);
-		}
-	}
-
-	ret = -bt_to_errno(rp->status);
-
-	kfree_skb(skb);
-
-	return ret;
-}
-
-#define RTL_FRAG_LEN 252
-
-struct rtl_download_cmd {
-	__u8 index;
-	__u8 data[RTL_FRAG_LEN];
-} __packed;
-
-struct rtl_download_response {
-	__u8 status;
-	__u8 index;
-} __packed;
-
-struct rtl_rom_version_evt {
-	__u8 status;
-	__u8 version;
-} __packed;
-
-struct rtl_epatch_header {
-	__u8 signature[8];
-	__le32 fw_version;
-	__le16 num_patches;
-} __packed;
-
-#define RTL_EPATCH_SIGNATURE	"Realtech"
-#define RTL_ROM_LMP_3499	0x3499
-#define RTL_ROM_LMP_8723A	0x1200
-#define RTL_ROM_LMP_8723B	0x8723
-#define RTL_ROM_LMP_8821A	0x8821
-#define RTL_ROM_LMP_8761A	0x8761
-
-static int rtl_read_rom_version(struct hci_dev *hdev, u8 *version)
-{
-	struct rtl_rom_version_evt *rom_version;
-	struct sk_buff *skb;
-	int ret;
-
-	/* Read RTL ROM version command */
-	skb = __hci_cmd_sync(hdev, 0xfc6d, 0, NULL, HCI_INIT_TIMEOUT);
+	skb = __hci_cmd_sync(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL,
+			     HCI_INIT_TIMEOUT);
 	if (IS_ERR(skb)) {
-		BT_ERR("%s: Read ROM version failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
-		return PTR_ERR(skb);
+		int err = PTR_ERR(skb);
+		BT_ERR("%s: CSR: Local version failed (%d)", hdev->name, err);
+		return err;
 	}
 
-	if (skb->len != sizeof(*rom_version)) {
-		BT_ERR("%s: RTL version event length mismatch", hdev->name);
+	if (skb->len != sizeof(struct hci_rp_read_local_version)) {
+		BT_ERR("%s: CSR: Local version length mismatch", hdev->name);
 		kfree_skb(skb);
 		return -EIO;
 	}
 
-	rom_version = (struct rtl_rom_version_evt *)skb->data;
-	BT_INFO("%s: rom_version status=%x version=%x",
-		hdev->name, rom_version->status, rom_version->version);
-
-	ret = rom_version->status;
-	if (ret == 0)
-		*version = rom_version->version;
-
-	kfree_skb(skb);
-	return ret;
-}
-
-static int rtl8723b_parse_firmware(struct hci_dev *hdev, u16 lmp_subver,
-				   const struct firmware *fw,
-				   unsigned char **_buf)
-{
-	const u8 extension_sig[] = { 0x51, 0x04, 0xfd, 0x77 };
-	struct rtl_epatch_header *epatch_info;
-	unsigned char *buf;
-	int i, ret, len;
-	size_t min_size;
-	u8 opcode, length, data, rom_version = 0;
-	int project_id = -1;
-	const unsigned char *fwptr, *chip_id_base;
-	const unsigned char *patch_length_base, *patch_offset_base;
-	u32 patch_offset = 0;
-	u16 patch_length, num_patches;
-	const u16 project_id_to_lmp_subver[] = {
-		RTL_ROM_LMP_8723A,
-		RTL_ROM_LMP_8723B,
-		RTL_ROM_LMP_8821A,
-		RTL_ROM_LMP_8761A
-	};
-
-	ret = rtl_read_rom_version(hdev, &rom_version);
-	if (ret)
-		return -bt_to_errno(ret);
-
-	min_size = sizeof(struct rtl_epatch_header) + sizeof(extension_sig) + 3;
-	if (fw->size < min_size)
-		return -EINVAL;
-
-	fwptr = fw->data + fw->size - sizeof(extension_sig);
-	if (memcmp(fwptr, extension_sig, sizeof(extension_sig)) != 0) {
-		BT_ERR("%s: extension section signature mismatch", hdev->name);
-		return -EINVAL;
-	}
-
-	/* Loop from the end of the firmware parsing instructions, until
-	 * we find an instruction that identifies the "project ID" for the
-	 * hardware supported by this firwmare file.
-	 * Once we have that, we double-check that that project_id is suitable
-	 * for the hardware we are working with.
-	 */
-	while (fwptr >= fw->data + (sizeof(struct rtl_epatch_header) + 3)) {
-		opcode = *--fwptr;
-		length = *--fwptr;
-		data = *--fwptr;
-
-		BT_DBG("check op=%x len=%x data=%x", opcode, length, data);
-
-		if (opcode == 0xff) /* EOF */
-			break;
-
-		if (length == 0) {
-			BT_ERR("%s: found instruction with length 0",
-			       hdev->name);
-			return -EINVAL;
-		}
-
-		if (opcode == 0 && length == 1) {
-			project_id = data;
-			break;
-		}
-
-		fwptr -= length;
-	}
-
-	if (project_id < 0) {
-		BT_ERR("%s: failed to find version instruction", hdev->name);
-		return -EINVAL;
-	}
-
-	if (project_id >= ARRAY_SIZE(project_id_to_lmp_subver)) {
-		BT_ERR("%s: unknown project id %d", hdev->name, project_id);
-		return -EINVAL;
-	}
-
-	if (lmp_subver != project_id_to_lmp_subver[project_id]) {
-		BT_ERR("%s: firmware is for %x but this is a %x", hdev->name,
-		       project_id_to_lmp_subver[project_id], lmp_subver);
-		return -EINVAL;
-	}
-
-	epatch_info = (struct rtl_epatch_header *)fw->data;
-	if (memcmp(epatch_info->signature, RTL_EPATCH_SIGNATURE, 8) != 0) {
-		BT_ERR("%s: bad EPATCH signature", hdev->name);
-		return -EINVAL;
-	}
-
-	num_patches = le16_to_cpu(epatch_info->num_patches);
-	BT_DBG("fw_version=%x, num_patches=%d",
-	       le32_to_cpu(epatch_info->fw_version), num_patches);
-
-	/* After the rtl_epatch_header there is a funky patch metadata section.
-	 * Assuming 2 patches, the layout is:
-	 * ChipID1 ChipID2 PatchLength1 PatchLength2 PatchOffset1 PatchOffset2
-	 *
-	 * Find the right patch for this chip.
-	 */
-	min_size += 8 * num_patches;
-	if (fw->size < min_size)
-		return -EINVAL;
-
-	chip_id_base = fw->data + sizeof(struct rtl_epatch_header);
-	patch_length_base = chip_id_base + (sizeof(u16) * num_patches);
-	patch_offset_base = patch_length_base + (sizeof(u16) * num_patches);
-	for (i = 0; i < num_patches; i++) {
-		u16 chip_id = get_unaligned_le16(chip_id_base +
-						 (i * sizeof(u16)));
-		if (chip_id == rom_version + 1) {
-			patch_length = get_unaligned_le16(patch_length_base +
-							  (i * sizeof(u16)));
-			patch_offset = get_unaligned_le32(patch_offset_base +
-							  (i * sizeof(u32)));
-			break;
-		}
-	}
-
-	if (!patch_offset) {
-		BT_ERR("%s: didn't find patch for chip id %d",
-		       hdev->name, rom_version);
-		return -EINVAL;
-	}
-
-	BT_DBG("length=%x offset=%x index %d", patch_length, patch_offset, i);
-	min_size = patch_offset + patch_length;
-	if (fw->size < min_size)
-		return -EINVAL;
-
-	/* Copy the firmware into a new buffer and write the version at
-	 * the end.
-	 */
-	len = patch_length;
-	buf = kmemdup(fw->data + patch_offset, patch_length, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	memcpy(buf + patch_length - 4, &epatch_info->fw_version, 4);
-
-	*_buf = buf;
-	return len;
-}
-
-static int rtl_download_firmware(struct hci_dev *hdev,
-				 const unsigned char *data, int fw_len)
-{
-	struct rtl_download_cmd *dl_cmd;
-	int frag_num = fw_len / RTL_FRAG_LEN + 1;
-	int frag_len = RTL_FRAG_LEN;
-	int ret = 0;
-	int i;
-
-	dl_cmd = kmalloc(sizeof(struct rtl_download_cmd), GFP_KERNEL);
-	if (!dl_cmd)
-		return -ENOMEM;
-
-	for (i = 0; i < frag_num; i++) {
-		struct rtl_download_response *dl_resp;
-		struct sk_buff *skb;
-
-		BT_DBG("download fw (%d/%d)", i, frag_num);
-
-		dl_cmd->index = i;
-		if (i == (frag_num - 1)) {
-			dl_cmd->index |= 0x80; /* data end */
-			frag_len = fw_len % RTL_FRAG_LEN;
-		}
-		memcpy(dl_cmd->data, data, frag_len);
-
-		/* Send download command */
-		skb = __hci_cmd_sync(hdev, 0xfc20, frag_len + 1, dl_cmd,
-				     HCI_INIT_TIMEOUT);
-		if (IS_ERR(skb)) {
-			BT_ERR("%s: download fw command failed (%ld)",
-			       hdev->name, PTR_ERR(skb));
-			ret = -PTR_ERR(skb);
-			goto out;
-		}
-
-		if (skb->len != sizeof(*dl_resp)) {
-			BT_ERR("%s: download fw event length mismatch",
-			       hdev->name);
-			kfree_skb(skb);
-			ret = -EIO;
-			goto out;
-		}
-
-		dl_resp = (struct rtl_download_response *)skb->data;
-		if (dl_resp->status != 0) {
-			kfree_skb(skb);
-			ret = bt_to_errno(dl_resp->status);
-			goto out;
-		}
-
-		kfree_skb(skb);
-		data += RTL_FRAG_LEN;
-	}
-
-out:
-	kfree(dl_cmd);
-	return ret;
-}
-
-static int btusb_setup_rtl8723a(struct hci_dev *hdev)
-{
-	struct btusb_data *data = dev_get_drvdata(&hdev->dev);
-	struct usb_device *udev = interface_to_usbdev(data->intf);
-	const struct firmware *fw;
-	int ret;
-
-	BT_INFO("%s: rtl: loading rtl_bt/rtl8723a_fw.bin", hdev->name);
-	ret = request_firmware(&fw, "rtl_bt/rtl8723a_fw.bin", &udev->dev);
-	if (ret < 0) {
-		BT_ERR("%s: Failed to load rtl_bt/rtl8723a_fw.bin", hdev->name);
-		return ret;
-	}
-
-	if (fw->size < 8) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	/* Check that the firmware doesn't have the epatch signature
-	 * (which is only for RTL8723B and newer).
-	 */
-	if (!memcmp(fw->data, RTL_EPATCH_SIGNATURE, 8)) {
-		BT_ERR("%s: unexpected EPATCH signature!", hdev->name);
-		ret = -EINVAL;
-		goto out;
-	}
-
-	ret = rtl_download_firmware(hdev, fw->data, fw->size);
-
-out:
-	release_firmware(fw);
-	return ret;
-}
+	rp = (struct hci_rp_read_local_version *)skb->data;
 
-static int btusb_setup_rtl8723b(struct hci_dev *hdev, u16 lmp_subver,
-				const char *fw_name)
-{
-	struct btusb_data *data = dev_get_drvdata(&hdev->dev);
-	struct usb_device *udev = interface_to_usbdev(data->intf);
-	unsigned char *fw_data = NULL;
-	const struct firmware *fw;
-	int ret;
+	if (le16_to_cpu(rp->manufacturer) != 10) {
+		/* Clear the reset quirk since this is not an actual
+		 * early Bluetooth 1.1 device from CSR.
+		 */
+		clear_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 
-	BT_INFO("%s: rtl: loading %s", hdev->name, fw_name);
-	ret = request_firmware(&fw, fw_name, &udev->dev);
-	if (ret < 0) {
-		BT_ERR("%s: Failed to load %s", hdev->name, fw_name);
-		return ret;
+		/* These fake CSR controllers have all a broken
+		 * stored link key handling and so just disable it.
+		 */
+		set_bit(HCI_QUIRK_BROKEN_STORED_LINK_KEY, &hdev->quirks);
 	}
 
-	ret = rtl8723b_parse_firmware(hdev, lmp_subver, fw, &fw_data);
-	if (ret < 0)
-		goto out;
-
-	ret = rtl_download_firmware(hdev, fw_data, ret);
-	kfree(fw_data);
-	if (ret < 0)
-		goto out;
-
-out:
-	release_firmware(fw);
-	return ret;
-}
-
-static int btusb_setup_realtek(struct hci_dev *hdev)
-{
-	struct sk_buff *skb;
-	struct hci_rp_read_local_version *resp;
-	u16 lmp_subver;
-
-	skb = btusb_read_local_version(hdev);
-	if (IS_ERR(skb))
-		return -PTR_ERR(skb);
-
-	resp = (struct hci_rp_read_local_version *)skb->data;
-	BT_INFO("%s: rtl: examining hci_ver=%02x hci_rev=%04x lmp_ver=%02x "
-		"lmp_subver=%04x", hdev->name, resp->hci_ver, resp->hci_rev,
-		resp->lmp_ver, resp->lmp_subver);
-
-	lmp_subver = le16_to_cpu(resp->lmp_subver);
 	kfree_skb(skb);
 
-	/* Match a set of subver values that correspond to stock firmware,
-	 * which is not compatible with standard btusb.
-	 * If matched, upload an alternative firmware that does conform to
-	 * standard btusb. Once that firmware is uploaded, the subver changes
-	 * to a different value.
-	 */
-	switch (lmp_subver) {
-	case RTL_ROM_LMP_8723A:
-	case RTL_ROM_LMP_3499:
-		return btusb_setup_rtl8723a(hdev);
-	case RTL_ROM_LMP_8723B:
-		return btusb_setup_rtl8723b(hdev, lmp_subver,
-					    "rtl_bt/rtl8723b_fw.bin");
-	case RTL_ROM_LMP_8821A:
-		return btusb_setup_rtl8723b(hdev, lmp_subver,
-					    "rtl_bt/rtl8821a_fw.bin");
-	case RTL_ROM_LMP_8761A:
-		return btusb_setup_rtl8723b(hdev, lmp_subver,
-					    "rtl_bt/rtl8761a_fw.bin");
-	default:
-		BT_INFO("rtl: assuming no firmware upload needed.");
-		return 0;
-	}
+	return 0;
 }
 
 static const struct firmware *btusb_setup_intel_get_fw(struct hci_dev *hdev,
@@ -1951,12 +1566,6 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 	}
 
 	ver = (struct intel_version *)skb->data;
-	if (ver->status) {
-		BT_ERR("%s Intel fw version event failed (%02x)", hdev->name,
-		       ver->status);
-		kfree_skb(skb);
-		return -bt_to_errno(ver->status);
-	}
 
 	BT_INFO("%s: read Intel version: %02x%02x%02x%02x%02x%02x%02x%02x%02x",
 		hdev->name, ver->hw_platform, ver->hw_variant,
@@ -1990,6 +1599,8 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 	}
 	fw_ptr = fw->data;
 
+	kfree_skb(skb);
+
 	/* This Intel specific command enables the manufacturer mode of the
 	 * controller.
 	 *
@@ -2004,15 +1615,6 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 		return PTR_ERR(skb);
 	}
 
-	if (skb->data[0]) {
-		u8 evt_status = skb->data[0];
-
-		BT_ERR("%s enable Intel manufacturer mode event failed (%02x)",
-		       hdev->name, evt_status);
-		kfree_skb(skb);
-		release_firmware(fw);
-		return -bt_to_errno(evt_status);
-	}
 	kfree_skb(skb);
 
 	disable_patch = 1;
@@ -2331,6 +1933,7 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	struct intel_boot_params *params;
 	const struct firmware *fw;
 	const u8 *fw_ptr;
+	u32 frag_len;
 	char fwname[64];
 	ktime_t calltime, delta, rettime;
 	unsigned long long duration;
@@ -2358,13 +1961,6 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	}
 
 	ver = (struct intel_version *)skb->data;
-	if (ver->status) {
-		BT_ERR("%s: Intel version command failure (%02x)",
-		       hdev->name, ver->status);
-		err = -bt_to_errno(ver->status);
-		kfree_skb(skb);
-		return err;
-	}
 
 	/* The hardware platform number has a fixed value of 0x37 and
 	 * for now only accept this single value.
@@ -2439,13 +2035,6 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	}
 
 	params = (struct intel_boot_params *)skb->data;
-	if (params->status) {
-		BT_ERR("%s: Intel boot parameters command failure (%02x)",
-		       hdev->name, params->status);
-		err = -bt_to_errno(params->status);
-		kfree_skb(skb);
-		return err;
-	}
 
 	BT_INFO("%s: Device revision is %u", hdev->name,
 		le16_to_cpu(params->dev_revid));
@@ -2495,6 +2084,12 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 
 	BT_INFO("%s: Found device firmware: %s", hdev->name, fwname);
 
+	/* Save the DDC file name for later use to apply once the firmware
+	 * downloading is done.
+	 */
+	snprintf(fwname, sizeof(fwname), "intel/ibt-11-%u.ddc",
+		 le16_to_cpu(params->dev_revid));
+
 	kfree_skb(skb);
 
 	if (fw->size < 644) {
@@ -2537,24 +2132,33 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	}
 
 	fw_ptr = fw->data + 644;
+	frag_len = 0;
 
 	while (fw_ptr - fw->data < fw->size) {
-		struct hci_command_hdr *cmd = (void *)fw_ptr;
-		u8 cmd_len;
+		struct hci_command_hdr *cmd = (void *)(fw_ptr + frag_len);
 
-		cmd_len = sizeof(*cmd) + cmd->plen;
+		frag_len += sizeof(*cmd) + cmd->plen;
 
-		/* Send each command from the firmware data buffer as
-		 * a single Data fragment.
+		/* The paramter length of the secure send command requires
+		 * a 4 byte alignment. It happens so that the firmware file
+		 * contains proper Intel_NOP commands to align the fragments
+		 * as needed.
+		 *
+		 * Send set of commands with 4 byte alignment from the
+		 * firmware data buffer as a single Data fragement.
 		 */
-		err = btusb_intel_secure_send(hdev, 0x01, cmd_len, fw_ptr);
-		if (err < 0) {
-			BT_ERR("%s: Failed to send firmware data (%d)",
-			       hdev->name, err);
-			goto done;
-		}
+		if (!(frag_len % 4)) {
+			err = btusb_intel_secure_send(hdev, 0x01, frag_len,
+						      fw_ptr);
+			if (err < 0) {
+				BT_ERR("%s: Failed to send firmware data (%d)",
+				       hdev->name, err);
+				goto done;
+			}
 
-		fw_ptr += cmd_len;
+			fw_ptr += frag_len;
+			frag_len = 0;
+		}
 	}
 
 	set_bit(BTUSB_FIRMWARE_LOADED, &data->flags);
@@ -2647,6 +2251,43 @@ done:
 
 	clear_bit(BTUSB_BOOTLOADER, &data->flags);
 
+	/* Once the device is running in operational mode, it needs to apply
+	 * the device configuration (DDC) parameters.
+	 *
+	 * The device can work without DDC parameters, so even if it fails
+	 * to load the file, no need to fail the setup.
+	 */
+	err = request_firmware_direct(&fw, fwname, &hdev->dev);
+	if (err < 0)
+		return 0;
+
+	BT_INFO("%s: Found Intel DDC parameters: %s", hdev->name, fwname);
+
+	fw_ptr = fw->data;
+
+	/* DDC file contains one or more DDC structure which has
+	 * Length (1 byte), DDC ID (2 bytes), and DDC value (Length - 2).
+	 */
+	while (fw->size > fw_ptr - fw->data) {
+		u8 cmd_plen = fw_ptr[0] + sizeof(u8);
+
+		skb = __hci_cmd_sync(hdev, 0xfc8b, cmd_plen, fw_ptr,
+				     HCI_INIT_TIMEOUT);
+		if (IS_ERR(skb)) {
+			BT_ERR("%s: Failed to send Intel_Write_DDC (%ld)",
+			       hdev->name, PTR_ERR(skb));
+			release_firmware(fw);
+			return PTR_ERR(skb);
+		}
+
+		fw_ptr += cmd_plen;
+		kfree_skb(skb);
+	}
+
+	release_firmware(fw);
+
+	BT_INFO("%s: Applying Intel DDC parameters completed", hdev->name);
+
 	return 0;
 }
 
@@ -2678,13 +2319,6 @@ static void btusb_hw_error_intel(struct hci_dev *hdev, u8 code)
 		return;
 	}
 
-	if (skb->data[0] != 0x00) {
-		BT_ERR("%s: Exception info command failure (%02x)",
-		       hdev->name, skb->data[0]);
-		kfree_skb(skb);
-		return;
-	}
-
 	BT_ERR("%s: Exception info %s", hdev->name, (char *)(skb->data + 1));
 
 	kfree_skb(skb);
@@ -2792,6 +2426,7 @@ struct qca_device_info {
 static const struct qca_device_info qca_devices_table[] = {
 	{ 0x00000100, 20, 4, 10 }, /* Rome 1.0 */
 	{ 0x00000101, 20, 4, 10 }, /* Rome 1.1 */
+	{ 0x00000200, 28, 4, 18 }, /* Rome 2.0 */
 	{ 0x00000201, 28, 4, 18 }, /* Rome 2.1 */
 	{ 0x00000300, 28, 4, 18 }, /* Rome 3.0 */
 	{ 0x00000302, 28, 4, 18 }, /* Rome 3.2 */
@@ -3175,8 +2810,17 @@ static int btusb_probe(struct usb_interface *intf,
 		hdev->set_bdaddr = btusb_set_bdaddr_ath3012;
 	}
 
-	if (id->driver_info & BTUSB_REALTEK)
-		hdev->setup = btusb_setup_realtek;
+#ifdef CONFIG_BT_HCIBTUSB_RTL
+	if (id->driver_info & BTUSB_REALTEK) {
+		hdev->setup = btrtl_setup_realtek;
+
+		/* Realtek devices lose their updated firmware over suspend,
+		 * but the USB hub doesn't notice any status change.
+		 * Explicitly request a device reset on resume.
+		 */
+		set_bit(BTUSB_RESET_RESUME, &data->flags);
+	}
+#endif
 
 	if (id->driver_info & BTUSB_AMP) {
 		/* AMP controllers do not support SCO packets */
@@ -3308,6 +2952,14 @@ static int btusb_suspend(struct usb_interface *intf, pm_message_t message)
 	btusb_stop_traffic(data);
 	usb_kill_anchored_urbs(&data->tx_anchor);
 
+	/* Optionally request a device reset on resume, but only when
+	 * wakeups are disabled. If wakeups are enabled we assume the
+	 * device will stay powered up throughout suspend.
+	 */
+	if (test_bit(BTUSB_RESET_RESUME, &data->flags) &&
+	    !device_may_wakeup(&data->udev->dev))
+		data->udev->reset_resume = 1;
+
 	return 0;
 }
 
diff --git a/drivers/bluetooth/btwilink.c b/drivers/bluetooth/btwilink.c
index 55c135b7757a..7a722df97343 100644
--- a/drivers/bluetooth/btwilink.c
+++ b/drivers/bluetooth/btwilink.c
@@ -22,7 +22,7 @@
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  *
  */
-#define DEBUG
+
 #include <linux/platform_device.h>
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
diff --git a/drivers/bluetooth/hci_ath.c b/drivers/bluetooth/hci_ath.c
index ec8fa0e0f036..6da5e4ca13ea 100644
--- a/drivers/bluetooth/hci_ath.c
+++ b/drivers/bluetooth/hci_ath.c
@@ -192,6 +192,7 @@ static int ath_recv(struct hci_uart *hu, const void *data, int count)
 	if (IS_ERR(ath->rx_skb)) {
 		int err = PTR_ERR(ath->rx_skb);
 		BT_ERR("%s: Frame reassembly failed (%d)", hu->hdev->name, err);
+		ath->rx_skb = NULL;
 		return err;
 	}
 
diff --git a/drivers/bluetooth/hci_bcm.c b/drivers/bluetooth/hci_bcm.c
index 1ec0b4a5ffa6..23523e140a9a 100644
--- a/drivers/bluetooth/hci_bcm.c
+++ b/drivers/bluetooth/hci_bcm.c
@@ -24,6 +24,7 @@
 #include <linux/kernel.h>
 #include <linux/errno.h>
 #include <linux/skbuff.h>
+#include <linux/firmware.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -36,6 +37,55 @@ struct bcm_data {
 	struct sk_buff_head txq;
 };
 
+static int bcm_set_baudrate(struct hci_uart *hu, unsigned int speed)
+{
+	struct hci_dev *hdev = hu->hdev;
+	struct sk_buff *skb;
+	struct bcm_update_uart_baud_rate param;
+
+	if (speed > 3000000) {
+		struct bcm_write_uart_clock_setting clock;
+
+		clock.type = BCM_UART_CLOCK_48MHZ;
+
+		BT_DBG("%s: Set Controller clock (%d)", hdev->name, clock.type);
+
+		/* This Broadcom specific command changes the UART's controller
+		 * clock for baud rate > 3000000.
+		 */
+		skb = __hci_cmd_sync(hdev, 0xfc45, 1, &clock, HCI_INIT_TIMEOUT);
+		if (IS_ERR(skb)) {
+			int err = PTR_ERR(skb);
+			BT_ERR("%s: BCM: failed to write clock command (%d)",
+			       hdev->name, err);
+			return err;
+		}
+
+		kfree_skb(skb);
+	}
+
+	BT_DBG("%s: Set Controller UART speed to %d bit/s", hdev->name, speed);
+
+	param.zero = cpu_to_le16(0);
+	param.baud_rate = cpu_to_le32(speed);
+
+	/* This Broadcom specific command changes the UART's controller baud
+	 * rate.
+	 */
+	skb = __hci_cmd_sync(hdev, 0xfc18, sizeof(param), &param,
+			     HCI_INIT_TIMEOUT);
+	if (IS_ERR(skb)) {
+		int err = PTR_ERR(skb);
+		BT_ERR("%s: BCM: failed to write update baudrate command (%d)",
+		       hdev->name, err);
+		return err;
+	}
+
+	kfree_skb(skb);
+
+	return 0;
+}
+
 static int bcm_open(struct hci_uart *hu)
 {
 	struct bcm_data *bcm;
@@ -79,11 +129,62 @@ static int bcm_flush(struct hci_uart *hu)
 
 static int bcm_setup(struct hci_uart *hu)
 {
+	char fw_name[64];
+	const struct firmware *fw;
+	unsigned int speed;
+	int err;
+
 	BT_DBG("hu %p", hu);
 
 	hu->hdev->set_bdaddr = btbcm_set_bdaddr;
 
-	return btbcm_setup_patchram(hu->hdev);
+	err = btbcm_initialize(hu->hdev, fw_name, sizeof(fw_name));
+	if (err)
+		return err;
+
+	err = request_firmware(&fw, fw_name, &hu->hdev->dev);
+	if (err < 0) {
+		BT_INFO("%s: BCM: Patch %s not found", hu->hdev->name, fw_name);
+		return 0;
+	}
+
+	err = btbcm_patchram(hu->hdev, fw);
+	if (err) {
+		BT_INFO("%s: BCM: Patch failed (%d)", hu->hdev->name, err);
+		goto finalize;
+	}
+
+	/* Init speed if any */
+	if (hu->init_speed)
+		speed = hu->init_speed;
+	else if (hu->proto->init_speed)
+		speed = hu->proto->init_speed;
+	else
+		speed = 0;
+
+	if (speed)
+		hci_uart_set_baudrate(hu, speed);
+
+	/* Operational speed if any */
+	if (hu->oper_speed)
+		speed = hu->oper_speed;
+	else if (hu->proto->oper_speed)
+		speed = hu->proto->oper_speed;
+	else
+		speed = 0;
+
+	if (speed) {
+		err = bcm_set_baudrate(hu, speed);
+		if (!err)
+			hci_uart_set_baudrate(hu, speed);
+	}
+
+finalize:
+	release_firmware(fw);
+
+	err = btbcm_finalize(hu->hdev);
+
+	return err;
 }
 
 static const struct h4_recv_pkt bcm_recv_pkts[] = {
@@ -104,6 +205,7 @@ static int bcm_recv(struct hci_uart *hu, const void *data, int count)
 	if (IS_ERR(bcm->rx_skb)) {
 		int err = PTR_ERR(bcm->rx_skb);
 		BT_ERR("%s: Frame reassembly failed (%d)", hu->hdev->name, err);
+		bcm->rx_skb = NULL;
 		return err;
 	}
 
@@ -133,10 +235,13 @@ static struct sk_buff *bcm_dequeue(struct hci_uart *hu)
 static const struct hci_uart_proto bcm_proto = {
 	.id		= HCI_UART_BCM,
 	.name		= "BCM",
+	.init_speed	= 115200,
+	.oper_speed	= 4000000,
 	.open		= bcm_open,
 	.close		= bcm_close,
 	.flush		= bcm_flush,
 	.setup		= bcm_setup,
+	.set_baudrate	= bcm_set_baudrate,
 	.recv		= bcm_recv,
 	.enqueue	= bcm_enqueue,
 	.dequeue	= bcm_dequeue,
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index dc8e3d4356a0..d0b615a932d1 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -47,8 +47,8 @@
 
 #include "hci_uart.h"
 
-static bool txcrc = 1;
-static bool hciextn = 1;
+static bool txcrc = true;
+static bool hciextn = true;
 
 #define BCSP_TXWINSIZE	4
 
@@ -436,7 +436,7 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 			break;
 		default:
 			memcpy(skb_put(bcsp->rx_skb, 1), &byte, 1);
-			if ((bcsp->rx_skb-> data[0] & 0x40) != 0 && 
+			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
 					bcsp->rx_state != BCSP_W4_CRC)
 				bcsp_crc_update(&bcsp->message_crc, byte);
 			bcsp->rx_count--;
@@ -447,24 +447,24 @@ static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char
 		switch (byte) {
 		case 0xdc:
 			memcpy(skb_put(bcsp->rx_skb, 1), &c0, 1);
-			if ((bcsp->rx_skb-> data[0] & 0x40) != 0 && 
+			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
 					bcsp->rx_state != BCSP_W4_CRC)
-				bcsp_crc_update(&bcsp-> message_crc, 0xc0);
+				bcsp_crc_update(&bcsp->message_crc, 0xc0);
 			bcsp->rx_esc_state = BCSP_ESCSTATE_NOESC;
 			bcsp->rx_count--;
 			break;
 
 		case 0xdd:
 			memcpy(skb_put(bcsp->rx_skb, 1), &db, 1);
-			if ((bcsp->rx_skb-> data[0] & 0x40) != 0 && 
+			if ((bcsp->rx_skb->data[0] & 0x40) != 0 &&
 					bcsp->rx_state != BCSP_W4_CRC) 
-				bcsp_crc_update(&bcsp-> message_crc, 0xdb);
+				bcsp_crc_update(&bcsp->message_crc, 0xdb);
 			bcsp->rx_esc_state = BCSP_ESCSTATE_NOESC;
 			bcsp->rx_count--;
 			break;
 
 		default:
-			BT_ERR ("Invalid byte %02x after esc byte", byte);
+			BT_ERR("Invalid byte %02x after esc byte", byte);
 			kfree_skb(bcsp->rx_skb);
 			bcsp->rx_skb = NULL;
 			bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
@@ -527,7 +527,7 @@ static void bcsp_complete_rx_pkt(struct hci_uart *hu)
 
 				hci_recv_frame(hu->hdev, bcsp->rx_skb);
 			} else {
-				BT_ERR ("Packet for unknown channel (%u %s)",
+				BT_ERR("Packet for unknown channel (%u %s)",
 					bcsp->rx_skb->data[1] & 0x0f,
 					bcsp->rx_skb->data[0] & 0x80 ? 
 					"reliable" : "unreliable");
@@ -587,7 +587,7 @@ static int bcsp_recv(struct hci_uart *hu, const void *data, int count)
 			}
 			if (bcsp->rx_skb->data[0] & 0x80	/* reliable pkt */
 			    		&& (bcsp->rx_skb->data[0] & 0x07) != bcsp->rxseq_txack) {
-				BT_ERR ("Out-of-order packet arrived, got %u expected %u",
+				BT_ERR("Out-of-order packet arrived, got %u expected %u",
 					bcsp->rx_skb->data[0] & 0x07, bcsp->rxseq_txack);
 
 				kfree_skb(bcsp->rx_skb);
diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index f7190f01e135..57faddc53645 100644
--- a/drivers/bluetooth/hci_h4.c
+++ b/drivers/bluetooth/hci_h4.c
@@ -133,6 +133,7 @@ static int h4_recv(struct hci_uart *hu, const void *data, int count)
 	if (IS_ERR(h4->rx_skb)) {
 		int err = PTR_ERR(h4->rx_skb);
 		BT_ERR("%s: Frame reassembly failed (%d)", hu->hdev->name, err);
+		h4->rx_skb = NULL;
 		return err;
 	}
 
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index 5c9a73f02664..177dd69fdd95 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -40,6 +40,7 @@
 #include <linux/signal.h>
 #include <linux/ioctl.h>
 #include <linux/skbuff.h>
+#include <linux/firmware.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -265,11 +266,133 @@ static int hci_uart_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	return 0;
 }
 
+/* Flow control or un-flow control the device */
+void hci_uart_set_flow_control(struct hci_uart *hu, bool enable)
+{
+	struct tty_struct *tty = hu->tty;
+	struct ktermios ktermios;
+	int status;
+	unsigned int set = 0;
+	unsigned int clear = 0;
+
+	if (enable) {
+		/* Disable hardware flow control */
+		ktermios = tty->termios;
+		ktermios.c_cflag &= ~CRTSCTS;
+		status = tty_set_termios(tty, &ktermios);
+		BT_DBG("Disabling hardware flow control: %s",
+		       status ? "failed" : "success");
+
+		/* Clear RTS to prevent the device from sending */
+		/* Most UARTs need OUT2 to enable interrupts */
+		status = tty->driver->ops->tiocmget(tty);
+		BT_DBG("Current tiocm 0x%x", status);
+
+		set &= ~(TIOCM_OUT2 | TIOCM_RTS);
+		clear = ~set;
+		set &= TIOCM_DTR | TIOCM_RTS | TIOCM_OUT1 |
+		       TIOCM_OUT2 | TIOCM_LOOP;
+		clear &= TIOCM_DTR | TIOCM_RTS | TIOCM_OUT1 |
+			 TIOCM_OUT2 | TIOCM_LOOP;
+		status = tty->driver->ops->tiocmset(tty, set, clear);
+		BT_DBG("Clearing RTS: %s", status ? "failed" : "success");
+	} else {
+		/* Set RTS to allow the device to send again */
+		status = tty->driver->ops->tiocmget(tty);
+		BT_DBG("Current tiocm 0x%x", status);
+
+		set |= (TIOCM_OUT2 | TIOCM_RTS);
+		clear = ~set;
+		set &= TIOCM_DTR | TIOCM_RTS | TIOCM_OUT1 |
+		       TIOCM_OUT2 | TIOCM_LOOP;
+		clear &= TIOCM_DTR | TIOCM_RTS | TIOCM_OUT1 |
+			 TIOCM_OUT2 | TIOCM_LOOP;
+		status = tty->driver->ops->tiocmset(tty, set, clear);
+		BT_DBG("Setting RTS: %s", status ? "failed" : "success");
+
+		/* Re-enable hardware flow control */
+		ktermios = tty->termios;
+		ktermios.c_cflag |= CRTSCTS;
+		status = tty_set_termios(tty, &ktermios);
+		BT_DBG("Enabling hardware flow control: %s",
+		       status ? "failed" : "success");
+	}
+}
+
+void hci_uart_set_speeds(struct hci_uart *hu, unsigned int init_speed,
+			 unsigned int oper_speed)
+{
+	hu->init_speed = init_speed;
+	hu->oper_speed = oper_speed;
+}
+
+void hci_uart_init_tty(struct hci_uart *hu)
+{
+	struct tty_struct *tty = hu->tty;
+	struct ktermios ktermios;
+
+	/* Bring the UART into a known 8 bits no parity hw fc state */
+	ktermios = tty->termios;
+	ktermios.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP |
+			      INLCR | IGNCR | ICRNL | IXON);
+	ktermios.c_oflag &= ~OPOST;
+	ktermios.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+	ktermios.c_cflag &= ~(CSIZE | PARENB);
+	ktermios.c_cflag |= CS8;
+	ktermios.c_cflag |= CRTSCTS;
+
+	/* tty_set_termios() return not checked as it is always 0 */
+	tty_set_termios(tty, &ktermios);
+}
+
+void hci_uart_set_baudrate(struct hci_uart *hu, unsigned int speed)
+{
+	struct tty_struct *tty = hu->tty;
+	struct ktermios ktermios;
+
+	ktermios = tty->termios;
+	ktermios.c_cflag &= ~CBAUD;
+	tty_termios_encode_baud_rate(&ktermios, speed, speed);
+
+	/* tty_set_termios() return not checked as it is always 0 */
+	tty_set_termios(tty, &ktermios);
+
+	BT_DBG("%s: New tty speeds: %d/%d", hu->hdev->name,
+	       tty->termios.c_ispeed, tty->termios.c_ospeed);
+}
+
 static int hci_uart_setup(struct hci_dev *hdev)
 {
 	struct hci_uart *hu = hci_get_drvdata(hdev);
 	struct hci_rp_read_local_version *ver;
 	struct sk_buff *skb;
+	unsigned int speed;
+	int err;
+
+	/* Init speed if any */
+	if (hu->init_speed)
+		speed = hu->init_speed;
+	else if (hu->proto->init_speed)
+		speed = hu->proto->init_speed;
+	else
+		speed = 0;
+
+	if (speed)
+		hci_uart_set_baudrate(hu, speed);
+
+	/* Operational speed if any */
+	if (hu->oper_speed)
+		speed = hu->oper_speed;
+	else if (hu->proto->oper_speed)
+		speed = hu->proto->oper_speed;
+	else
+		speed = 0;
+
+	if (hu->proto->set_baudrate && speed) {
+		err = hu->proto->set_baudrate(hu, speed);
+		if (!err)
+			hci_uart_set_baudrate(hu, speed);
+	}
 
 	if (hu->proto->setup)
 		return hu->proto->setup(hu);
diff --git a/drivers/bluetooth/hci_uart.h b/drivers/bluetooth/hci_uart.h
index 72120a5ba13c..ce9c670956f5 100644
--- a/drivers/bluetooth/hci_uart.h
+++ b/drivers/bluetooth/hci_uart.h
@@ -58,10 +58,13 @@ struct hci_uart;
 struct hci_uart_proto {
 	unsigned int id;
 	const char *name;
+	unsigned int init_speed;
+	unsigned int oper_speed;
 	int (*open)(struct hci_uart *hu);
 	int (*close)(struct hci_uart *hu);
 	int (*flush)(struct hci_uart *hu);
 	int (*setup)(struct hci_uart *hu);
+	int (*set_baudrate)(struct hci_uart *hu, unsigned int speed);
 	int (*recv)(struct hci_uart *hu, const void *data, int len);
 	int (*enqueue)(struct hci_uart *hu, struct sk_buff *skb);
 	struct sk_buff *(*dequeue)(struct hci_uart *hu);
@@ -82,6 +85,9 @@ struct hci_uart {
 	struct sk_buff		*tx_skb;
 	unsigned long		tx_state;
 	spinlock_t		rx_lock;
+
+	unsigned int init_speed;
+	unsigned int oper_speed;
 };
 
 /* HCI_UART proto flag bits */
@@ -96,6 +102,11 @@ int hci_uart_register_proto(const struct hci_uart_proto *p);
 int hci_uart_unregister_proto(const struct hci_uart_proto *p);
 int hci_uart_tx_wakeup(struct hci_uart *hu);
 int hci_uart_init_ready(struct hci_uart *hu);
+void hci_uart_init_tty(struct hci_uart *hu);
+void hci_uart_set_baudrate(struct hci_uart *hu, unsigned int speed);
+void hci_uart_set_flow_control(struct hci_uart *hu, bool enable);
+void hci_uart_set_speeds(struct hci_uart *hu, unsigned int init_speed,
+			 unsigned int oper_speed);
 
 #ifdef CONFIG_BT_HCIUART_H4
 int h4_init(void);
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index 6653473f2757..78653db2ef2b 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -366,7 +366,7 @@ static const struct file_operations vhci_fops = {
 	.llseek		= no_llseek,
 };
 
-static struct miscdevice vhci_miscdev= {
+static struct miscdevice vhci_miscdev = {
 	.name	= "vhci",
 	.fops	= &vhci_fops,
 	.minor	= VHCI_MINOR,
diff --git a/drivers/bus/Kconfig b/drivers/bus/Kconfig
index a1d4af6df3f5..1a82f3a17681 100644
--- a/drivers/bus/Kconfig
+++ b/drivers/bus/Kconfig
@@ -7,21 +7,24 @@ menu "Bus devices"
 config ARM_CCI
 	bool
 
+config ARM_CCI_PMU
+	bool
+	select ARM_CCI
+
 config ARM_CCI400_COMMON
 	bool
 	select ARM_CCI
 
 config ARM_CCI400_PMU
 	bool "ARM CCI400 PMU support"
-	default y
-	depends on ARM || ARM64
-	depends on HW_PERF_EVENTS
+	depends on (ARM && CPU_V7) || ARM64
+	depends on PERF_EVENTS
 	select ARM_CCI400_COMMON
+	select ARM_CCI_PMU
 	help
-	  Support for PMU events monitoring on the ARM CCI cache coherent
-	  interconnect.
-
-	  If unsure, say Y
+	  Support for PMU events monitoring on the ARM CCI-400 (cache coherent
+	  interconnect). CCI-400 supports counting events related to the
+	  connected slave/master interfaces.
 
 config ARM_CCI400_PORT_CTRL
 	bool
@@ -31,6 +34,20 @@ config ARM_CCI400_PORT_CTRL
 	  Low level power management driver for CCI400 cache coherent
 	  interconnect for ARM platforms.
 
+config ARM_CCI500_PMU
+	bool "ARM CCI500 PMU support"
+	default y
+	depends on (ARM && CPU_V7) || ARM64
+	depends on PERF_EVENTS
+	select ARM_CCI_PMU
+	help
+	  Support for PMU events monitoring on the ARM CCI-500 cache coherent
+	  interconnect. CCI-500 provides 8 independent event counters, which
+	  can count events pertaining to the slave/master interfaces as well
+	  as the internal events to the CCI.
+
+	  If unsure, say Y
+
 config ARM_CCN
 	bool "ARM CCN driver support"
 	depends on ARM || ARM64
diff --git a/drivers/bus/arm-cci.c b/drivers/bus/arm-cci.c
index 5340604b23a4..577cc4bf6a9d 100644
--- a/drivers/bus/arm-cci.c
+++ b/drivers/bus/arm-cci.c
@@ -52,12 +52,15 @@ static const struct of_device_id arm_cci_matches[] = {
 #ifdef CONFIG_ARM_CCI400_COMMON
 	{.compatible = "arm,cci-400", .data = CCI400_PORTS_DATA },
 #endif
+#ifdef CONFIG_ARM_CCI500_PMU
+	{ .compatible = "arm,cci-500", },
+#endif
 	{},
 };
 
-#ifdef CONFIG_ARM_CCI400_PMU
+#ifdef CONFIG_ARM_CCI_PMU
 
-#define DRIVER_NAME		"CCI-400"
+#define DRIVER_NAME		"ARM-CCI"
 #define DRIVER_NAME_PMU		DRIVER_NAME " PMU"
 
 #define CCI_PMCR		0x0100
@@ -77,20 +80,21 @@ static const struct of_device_id arm_cci_matches[] = {
 
 #define CCI_PMU_OVRFLW_FLAG	1
 
-#define CCI_PMU_CNTR_BASE(idx)	((idx) * SZ_4K)
-
-#define CCI_PMU_CNTR_MASK	((1ULL << 32) -1)
+#define CCI_PMU_CNTR_SIZE(model)	((model)->cntr_size)
+#define CCI_PMU_CNTR_BASE(model, idx)	((idx) * CCI_PMU_CNTR_SIZE(model))
+#define CCI_PMU_CNTR_MASK		((1ULL << 32) -1)
+#define CCI_PMU_CNTR_LAST(cci_pmu)	(cci_pmu->num_cntrs - 1)
 
-#define CCI_PMU_EVENT_MASK		0xffUL
-#define CCI_PMU_EVENT_SOURCE(event)	((event >> 5) & 0x7)
-#define CCI_PMU_EVENT_CODE(event)	(event & 0x1f)
-
-#define CCI_PMU_MAX_HW_EVENTS 5   /* CCI PMU has 4 counters + 1 cycle counter */
+#define CCI_PMU_MAX_HW_CNTRS(model) \
+	((model)->num_hw_cntrs + (model)->fixed_hw_cntrs)
 
 /* Types of interfaces that can generate events */
 enum {
 	CCI_IF_SLAVE,
 	CCI_IF_MASTER,
+#ifdef CONFIG_ARM_CCI500_PMU
+	CCI_IF_GLOBAL,
+#endif
 	CCI_IF_MAX,
 };
 
@@ -100,14 +104,30 @@ struct event_range {
 };
 
 struct cci_pmu_hw_events {
-	struct perf_event *events[CCI_PMU_MAX_HW_EVENTS];
-	unsigned long used_mask[BITS_TO_LONGS(CCI_PMU_MAX_HW_EVENTS)];
+	struct perf_event **events;
+	unsigned long *used_mask;
 	raw_spinlock_t pmu_lock;
 };
 
+struct cci_pmu;
+/*
+ * struct cci_pmu_model:
+ * @fixed_hw_cntrs - Number of fixed event counters
+ * @num_hw_cntrs - Maximum number of programmable event counters
+ * @cntr_size - Size of an event counter mapping
+ */
 struct cci_pmu_model {
 	char *name;
+	u32 fixed_hw_cntrs;
+	u32 num_hw_cntrs;
+	u32 cntr_size;
+	u64 nformat_attrs;
+	u64 nevent_attrs;
+	struct dev_ext_attribute *format_attrs;
+	struct dev_ext_attribute *event_attrs;
 	struct event_range event_ranges[CCI_IF_MAX];
+	int (*validate_hw_event)(struct cci_pmu *, unsigned long);
+	int (*get_event_idx)(struct cci_pmu *, struct cci_pmu_hw_events *, unsigned long);
 };
 
 static struct cci_pmu_model cci_pmu_models[];
@@ -116,33 +136,59 @@ struct cci_pmu {
 	void __iomem *base;
 	struct pmu pmu;
 	int nr_irqs;
-	int irqs[CCI_PMU_MAX_HW_EVENTS];
+	int *irqs;
 	unsigned long active_irqs;
 	const struct cci_pmu_model *model;
 	struct cci_pmu_hw_events hw_events;
 	struct platform_device *plat_device;
-	int num_events;
+	int num_cntrs;
 	atomic_t active_events;
 	struct mutex reserve_mutex;
+	struct notifier_block cpu_nb;
 	cpumask_t cpus;
 };
-static struct cci_pmu *pmu;
 
 #define to_cci_pmu(c)	(container_of(c, struct cci_pmu, pmu))
 
+enum cci_models {
+#ifdef CONFIG_ARM_CCI400_PMU
+	CCI400_R0,
+	CCI400_R1,
+#endif
+#ifdef CONFIG_ARM_CCI500_PMU
+	CCI500_R0,
+#endif
+	CCI_MODEL_MAX
+};
+
+static ssize_t cci_pmu_format_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+static ssize_t cci_pmu_event_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+#define CCI_EXT_ATTR_ENTRY(_name, _func, _config) \
+	{ __ATTR(_name, S_IRUGO, _func, NULL), (void *)_config }
+
+#define CCI_FORMAT_EXT_ATTR_ENTRY(_name, _config) \
+	CCI_EXT_ATTR_ENTRY(_name, cci_pmu_format_show, (char *)_config)
+#define CCI_EVENT_EXT_ATTR_ENTRY(_name, _config) \
+	CCI_EXT_ATTR_ENTRY(_name, cci_pmu_event_show, (unsigned long)_config)
+
+/* CCI400 PMU Specific definitions */
+
+#ifdef CONFIG_ARM_CCI400_PMU
+
 /* Port ids */
-#define CCI_PORT_S0	0
-#define CCI_PORT_S1	1
-#define CCI_PORT_S2	2
-#define CCI_PORT_S3	3
-#define CCI_PORT_S4	4
-#define CCI_PORT_M0	5
-#define CCI_PORT_M1	6
-#define CCI_PORT_M2	7
-
-#define CCI_REV_R0		0
-#define CCI_REV_R1		1
-#define CCI_REV_R1_PX		5
+#define CCI400_PORT_S0		0
+#define CCI400_PORT_S1		1
+#define CCI400_PORT_S2		2
+#define CCI400_PORT_S3		3
+#define CCI400_PORT_S4		4
+#define CCI400_PORT_M0		5
+#define CCI400_PORT_M1		6
+#define CCI400_PORT_M2		7
+
+#define CCI400_R1_PX		5
 
 /*
  * Instead of an event id to monitor CCI cycles, a dedicated counter is
@@ -150,12 +196,11 @@ static struct cci_pmu *pmu;
  * make use of this event in hardware.
  */
 enum cci400_perf_events {
-	CCI_PMU_CYCLES = 0xff
+	CCI400_PMU_CYCLES = 0xff
 };
 
-#define CCI_PMU_CYCLE_CNTR_IDX		0
-#define CCI_PMU_CNTR0_IDX		1
-#define CCI_PMU_CNTR_LAST(cci_pmu)	(CCI_PMU_CYCLE_CNTR_IDX + cci_pmu->num_events - 1)
+#define CCI400_PMU_CYCLE_CNTR_IDX	0
+#define CCI400_PMU_CNTR0_IDX		1
 
 /*
  * CCI PMU event id is an 8-bit value made of two parts - bits 7:5 for one of 8
@@ -169,37 +214,173 @@ enum cci400_perf_events {
  * the different revisions and are used to validate the event to be monitored.
  */
 
-#define CCI_REV_R0_SLAVE_PORT_MIN_EV	0x00
-#define CCI_REV_R0_SLAVE_PORT_MAX_EV	0x13
-#define CCI_REV_R0_MASTER_PORT_MIN_EV	0x14
-#define CCI_REV_R0_MASTER_PORT_MAX_EV	0x1a
+#define CCI400_PMU_EVENT_MASK		0xffUL
+#define CCI400_PMU_EVENT_SOURCE_SHIFT	5
+#define CCI400_PMU_EVENT_SOURCE_MASK	0x7
+#define CCI400_PMU_EVENT_CODE_SHIFT	0
+#define CCI400_PMU_EVENT_CODE_MASK	0x1f
+#define CCI400_PMU_EVENT_SOURCE(event) \
+	((event >> CCI400_PMU_EVENT_SOURCE_SHIFT) & \
+			CCI400_PMU_EVENT_SOURCE_MASK)
+#define CCI400_PMU_EVENT_CODE(event) \
+	((event >> CCI400_PMU_EVENT_CODE_SHIFT) & CCI400_PMU_EVENT_CODE_MASK)
+
+#define CCI400_R0_SLAVE_PORT_MIN_EV	0x00
+#define CCI400_R0_SLAVE_PORT_MAX_EV	0x13
+#define CCI400_R0_MASTER_PORT_MIN_EV	0x14
+#define CCI400_R0_MASTER_PORT_MAX_EV	0x1a
+
+#define CCI400_R1_SLAVE_PORT_MIN_EV	0x00
+#define CCI400_R1_SLAVE_PORT_MAX_EV	0x14
+#define CCI400_R1_MASTER_PORT_MIN_EV	0x00
+#define CCI400_R1_MASTER_PORT_MAX_EV	0x11
+
+#define CCI400_CYCLE_EVENT_EXT_ATTR_ENTRY(_name, _config) \
+	CCI_EXT_ATTR_ENTRY(_name, cci400_pmu_cycle_event_show, \
+					(unsigned long)_config)
+
+static ssize_t cci400_pmu_cycle_event_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static struct dev_ext_attribute cci400_pmu_format_attrs[] = {
+	CCI_FORMAT_EXT_ATTR_ENTRY(event, "config:0-4"),
+	CCI_FORMAT_EXT_ATTR_ENTRY(source, "config:5-7"),
+};
+
+static struct dev_ext_attribute cci400_r0_pmu_event_attrs[] = {
+	/* Slave events */
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_any, 0x0),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_device, 0x01),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_normal_or_nonshareable, 0x2),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_inner_or_outershareable, 0x3),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_cache_maintenance, 0x4),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_mem_barrier, 0x5),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_sync_barrier, 0x6),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_dvm_msg, 0x7),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_dvm_msg_sync, 0x8),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_stall_tt_full, 0x9),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_r_data_last_hs_snoop, 0xA),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_r_data_stall_rvalids_h_rready_l, 0xB),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_any, 0xC),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_device, 0xD),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_normal_or_nonshareable, 0xE),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_inner_or_outershare_wback_wclean, 0xF),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_write_unique, 0x10),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_write_line_unique, 0x11),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_evict, 0x12),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_stall_tt_full, 0x13),
+	/* Master events */
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_retry_speculative_fetch, 0x14),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_addr_hazard, 0x15),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_id_hazard, 0x16),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_tt_full, 0x17),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_barrier_hazard, 0x18),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall_barrier_hazard, 0x19),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall_tt_full, 0x1A),
+	/* Special event for cycles counter */
+	CCI400_CYCLE_EVENT_EXT_ATTR_ENTRY(cycles, 0xff),
+};
+
+static struct dev_ext_attribute cci400_r1_pmu_event_attrs[] = {
+	/* Slave events */
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_any, 0x0),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_device, 0x01),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_normal_or_nonshareable, 0x2),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_inner_or_outershareable, 0x3),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_cache_maintenance, 0x4),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_mem_barrier, 0x5),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_sync_barrier, 0x6),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_dvm_msg, 0x7),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_dvm_msg_sync, 0x8),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_stall_tt_full, 0x9),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_r_data_last_hs_snoop, 0xA),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_r_data_stall_rvalids_h_rready_l, 0xB),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_any, 0xC),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_device, 0xD),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_normal_or_nonshareable, 0xE),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_inner_or_outershare_wback_wclean, 0xF),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_write_unique, 0x10),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_write_line_unique, 0x11),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_evict, 0x12),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_stall_tt_full, 0x13),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_stall_slave_id_hazard, 0x14),
+	/* Master events */
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_retry_speculative_fetch, 0x0),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_stall_cycle_addr_hazard, 0x1),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_master_id_hazard, 0x2),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_hi_prio_rtq_full, 0x3),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_barrier_hazard, 0x4),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall_barrier_hazard, 0x5),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall_wtq_full, 0x6),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_low_prio_rtq_full, 0x7),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_mid_prio_rtq_full, 0x8),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_qvn_vn0, 0x9),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_qvn_vn1, 0xA),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_qvn_vn2, 0xB),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall_qvn_vn3, 0xC),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall_qvn_vn0, 0xD),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall_qvn_vn1, 0xE),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall_qvn_vn2, 0xF),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall_qvn_vn3, 0x10),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_unique_or_line_unique_addr_hazard, 0x11),
+	/* Special event for cycles counter */
+	CCI400_CYCLE_EVENT_EXT_ATTR_ENTRY(cycles, 0xff),
+};
 
-#define CCI_REV_R1_SLAVE_PORT_MIN_EV	0x00
-#define CCI_REV_R1_SLAVE_PORT_MAX_EV	0x14
-#define CCI_REV_R1_MASTER_PORT_MIN_EV	0x00
-#define CCI_REV_R1_MASTER_PORT_MAX_EV	0x11
+static ssize_t cci400_pmu_cycle_event_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct dev_ext_attribute *eattr = container_of(attr,
+				struct dev_ext_attribute, attr);
+	return snprintf(buf, PAGE_SIZE, "config=0x%lx\n", (unsigned long)eattr->var);
+}
 
-static int pmu_validate_hw_event(unsigned long hw_event)
+static int cci400_get_event_idx(struct cci_pmu *cci_pmu,
+				struct cci_pmu_hw_events *hw,
+				unsigned long cci_event)
 {
-	u8 ev_source = CCI_PMU_EVENT_SOURCE(hw_event);
-	u8 ev_code = CCI_PMU_EVENT_CODE(hw_event);
+	int idx;
+
+	/* cycles event idx is fixed */
+	if (cci_event == CCI400_PMU_CYCLES) {
+		if (test_and_set_bit(CCI400_PMU_CYCLE_CNTR_IDX, hw->used_mask))
+			return -EAGAIN;
+
+		return CCI400_PMU_CYCLE_CNTR_IDX;
+	}
+
+	for (idx = CCI400_PMU_CNTR0_IDX; idx <= CCI_PMU_CNTR_LAST(cci_pmu); ++idx)
+		if (!test_and_set_bit(idx, hw->used_mask))
+			return idx;
+
+	/* No counters available */
+	return -EAGAIN;
+}
+
+static int cci400_validate_hw_event(struct cci_pmu *cci_pmu, unsigned long hw_event)
+{
+	u8 ev_source = CCI400_PMU_EVENT_SOURCE(hw_event);
+	u8 ev_code = CCI400_PMU_EVENT_CODE(hw_event);
 	int if_type;
 
-	if (hw_event & ~CCI_PMU_EVENT_MASK)
+	if (hw_event & ~CCI400_PMU_EVENT_MASK)
 		return -ENOENT;
 
+	if (hw_event == CCI400_PMU_CYCLES)
+		return hw_event;
+
 	switch (ev_source) {
-	case CCI_PORT_S0:
-	case CCI_PORT_S1:
-	case CCI_PORT_S2:
-	case CCI_PORT_S3:
-	case CCI_PORT_S4:
+	case CCI400_PORT_S0:
+	case CCI400_PORT_S1:
+	case CCI400_PORT_S2:
+	case CCI400_PORT_S3:
+	case CCI400_PORT_S4:
 		/* Slave Interface */
 		if_type = CCI_IF_SLAVE;
 		break;
-	case CCI_PORT_M0:
-	case CCI_PORT_M1:
-	case CCI_PORT_M2:
+	case CCI400_PORT_M0:
+	case CCI400_PORT_M1:
+	case CCI400_PORT_M2:
 		/* Master Interface */
 		if_type = CCI_IF_MASTER;
 		break;
@@ -207,87 +388,291 @@ static int pmu_validate_hw_event(unsigned long hw_event)
 		return -ENOENT;
 	}
 
-	if (ev_code >= pmu->model->event_ranges[if_type].min &&
-		ev_code <= pmu->model->event_ranges[if_type].max)
+	if (ev_code >= cci_pmu->model->event_ranges[if_type].min &&
+		ev_code <= cci_pmu->model->event_ranges[if_type].max)
 		return hw_event;
 
 	return -ENOENT;
 }
 
-static int probe_cci_revision(void)
+static int probe_cci400_revision(void)
 {
 	int rev;
 	rev = readl_relaxed(cci_ctrl_base + CCI_PID2) & CCI_PID2_REV_MASK;
 	rev >>= CCI_PID2_REV_SHIFT;
 
-	if (rev < CCI_REV_R1_PX)
-		return CCI_REV_R0;
+	if (rev < CCI400_R1_PX)
+		return CCI400_R0;
 	else
-		return CCI_REV_R1;
+		return CCI400_R1;
 }
 
 static const struct cci_pmu_model *probe_cci_model(struct platform_device *pdev)
 {
 	if (platform_has_secure_cci_access())
-		return &cci_pmu_models[probe_cci_revision()];
+		return &cci_pmu_models[probe_cci400_revision()];
 	return NULL;
 }
+#else	/* !CONFIG_ARM_CCI400_PMU */
+static inline struct cci_pmu_model *probe_cci_model(struct platform_device *pdev)
+{
+	return NULL;
+}
+#endif	/* CONFIG_ARM_CCI400_PMU */
+
+#ifdef CONFIG_ARM_CCI500_PMU
+
+/*
+ * CCI500 provides 8 independent event counters that can count
+ * any of the events available.
+ *
+ * CCI500 PMU event id is an 9-bit value made of two parts.
+ *	 bits [8:5] - Source for the event
+ *		      0x0-0x6 - Slave interfaces
+ *		      0x8-0xD - Master interfaces
+ *		      0xf     - Global Events
+ *		      0x7,0xe - Reserved
+ *
+ *	 bits [4:0] - Event code (specific to type of interface)
+ */
+
+/* Port ids */
+#define CCI500_PORT_S0			0x0
+#define CCI500_PORT_S1			0x1
+#define CCI500_PORT_S2			0x2
+#define CCI500_PORT_S3			0x3
+#define CCI500_PORT_S4			0x4
+#define CCI500_PORT_S5			0x5
+#define CCI500_PORT_S6			0x6
+
+#define CCI500_PORT_M0			0x8
+#define CCI500_PORT_M1			0x9
+#define CCI500_PORT_M2			0xa
+#define CCI500_PORT_M3			0xb
+#define CCI500_PORT_M4			0xc
+#define CCI500_PORT_M5			0xd
+
+#define CCI500_PORT_GLOBAL 		0xf
+
+#define CCI500_PMU_EVENT_MASK		0x1ffUL
+#define CCI500_PMU_EVENT_SOURCE_SHIFT	0x5
+#define CCI500_PMU_EVENT_SOURCE_MASK	0xf
+#define CCI500_PMU_EVENT_CODE_SHIFT	0x0
+#define CCI500_PMU_EVENT_CODE_MASK	0x1f
+
+#define CCI500_PMU_EVENT_SOURCE(event)	\
+	((event >> CCI500_PMU_EVENT_SOURCE_SHIFT) & CCI500_PMU_EVENT_SOURCE_MASK)
+#define CCI500_PMU_EVENT_CODE(event)	\
+	((event >> CCI500_PMU_EVENT_CODE_SHIFT) & CCI500_PMU_EVENT_CODE_MASK)
+
+#define CCI500_SLAVE_PORT_MIN_EV	0x00
+#define CCI500_SLAVE_PORT_MAX_EV	0x1f
+#define CCI500_MASTER_PORT_MIN_EV	0x00
+#define CCI500_MASTER_PORT_MAX_EV	0x06
+#define CCI500_GLOBAL_PORT_MIN_EV	0x00
+#define CCI500_GLOBAL_PORT_MAX_EV	0x0f
+
+
+#define CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(_name, _config) \
+	CCI_EXT_ATTR_ENTRY(_name, cci500_pmu_global_event_show, \
+					(unsigned long) _config)
+
+static ssize_t cci500_pmu_global_event_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+static struct dev_ext_attribute cci500_pmu_format_attrs[] = {
+	CCI_FORMAT_EXT_ATTR_ENTRY(event, "config:0-4"),
+	CCI_FORMAT_EXT_ATTR_ENTRY(source, "config:5-8"),
+};
+
+static struct dev_ext_attribute cci500_pmu_event_attrs[] = {
+	/* Slave events */
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_arvalid, 0x0),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_dev, 0x1),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_nonshareable, 0x2),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_shareable_non_alloc, 0x3),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_shareable_alloc, 0x4),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_invalidate, 0x5),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_cache_maint, 0x6),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_dvm_msg, 0x7),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_rval, 0x8),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_hs_rlast_snoop, 0x9),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_hs_awalid, 0xA),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_dev, 0xB),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_non_shareable, 0xC),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_share_wb, 0xD),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_share_wlu, 0xE),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_share_wunique, 0xF),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_evict, 0x10),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_wrevict, 0x11),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_w_data_beat, 0x12),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_srq_acvalid, 0x13),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_srq_read, 0x14),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_srq_clean, 0x15),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_srq_data_transfer_low, 0x16),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rrq_stall_arvalid, 0x17),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_r_data_stall, 0x18),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_wrq_stall, 0x19),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_w_data_stall, 0x1A),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_w_resp_stall, 0x1B),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_srq_stall, 0x1C),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_s_data_stall, 0x1D),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_rq_stall_ot_limit, 0x1E),
+	CCI_EVENT_EXT_ATTR_ENTRY(si_r_stall_arbit, 0x1F),
+
+	/* Master events */
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_r_data_beat_any, 0x0),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_w_data_beat_any, 0x1),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_rrq_stall, 0x2),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_r_data_stall, 0x3),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_wrq_stall, 0x4),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_w_data_stall, 0x5),
+	CCI_EVENT_EXT_ATTR_ENTRY(mi_w_resp_stall, 0x6),
+
+	/* Global events */
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_access_filter_bank_0_1, 0x0),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_access_filter_bank_2_3, 0x1),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_access_filter_bank_4_5, 0x2),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_access_filter_bank_6_7, 0x3),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_access_miss_filter_bank_0_1, 0x4),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_access_miss_filter_bank_2_3, 0x5),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_access_miss_filter_bank_4_5, 0x6),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_access_miss_filter_bank_6_7, 0x7),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_back_invalidation, 0x8),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_stall_alloc_busy, 0x9),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_stall_tt_full, 0xA),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_wrq, 0xB),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_cd_hs, 0xC),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_rq_stall_addr_hazard, 0xD),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snopp_rq_stall_tt_full, 0xE),
+	CCI500_GLOBAL_EVENT_EXT_ATTR_ENTRY(cci_snoop_rq_tzmp1_prot, 0xF),
+};
+
+static ssize_t cci500_pmu_global_event_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct dev_ext_attribute *eattr = container_of(attr,
+					struct dev_ext_attribute, attr);
+	/* Global events have single fixed source code */
+	return snprintf(buf, PAGE_SIZE, "event=0x%lx,source=0x%x\n",
+				(unsigned long)eattr->var, CCI500_PORT_GLOBAL);
+}
+
+static int cci500_validate_hw_event(struct cci_pmu *cci_pmu,
+					unsigned long hw_event)
+{
+	u32 ev_source = CCI500_PMU_EVENT_SOURCE(hw_event);
+	u32 ev_code = CCI500_PMU_EVENT_CODE(hw_event);
+	int if_type;
+
+	if (hw_event & ~CCI500_PMU_EVENT_MASK)
+		return -ENOENT;
+
+	switch (ev_source) {
+	case CCI500_PORT_S0:
+	case CCI500_PORT_S1:
+	case CCI500_PORT_S2:
+	case CCI500_PORT_S3:
+	case CCI500_PORT_S4:
+	case CCI500_PORT_S5:
+	case CCI500_PORT_S6:
+		if_type = CCI_IF_SLAVE;
+		break;
+	case CCI500_PORT_M0:
+	case CCI500_PORT_M1:
+	case CCI500_PORT_M2:
+	case CCI500_PORT_M3:
+	case CCI500_PORT_M4:
+	case CCI500_PORT_M5:
+		if_type = CCI_IF_MASTER;
+		break;
+	case CCI500_PORT_GLOBAL:
+		if_type = CCI_IF_GLOBAL;
+		break;
+	default:
+		return -ENOENT;
+	}
+
+	if (ev_code >= cci_pmu->model->event_ranges[if_type].min &&
+		ev_code <= cci_pmu->model->event_ranges[if_type].max)
+		return hw_event;
+
+	return -ENOENT;
+}
+#endif	/* CONFIG_ARM_CCI500_PMU */
+
+static ssize_t cci_pmu_format_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct dev_ext_attribute *eattr = container_of(attr,
+				struct dev_ext_attribute, attr);
+	return snprintf(buf, PAGE_SIZE, "%s\n", (char *)eattr->var);
+}
+
+static ssize_t cci_pmu_event_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct dev_ext_attribute *eattr = container_of(attr,
+				struct dev_ext_attribute, attr);
+	/* source parameter is mandatory for normal PMU events */
+	return snprintf(buf, PAGE_SIZE, "source=?,event=0x%lx\n",
+					 (unsigned long)eattr->var);
+}
 
 static int pmu_is_valid_counter(struct cci_pmu *cci_pmu, int idx)
 {
-	return CCI_PMU_CYCLE_CNTR_IDX <= idx &&
-		idx <= CCI_PMU_CNTR_LAST(cci_pmu);
+	return 0 <= idx && idx <= CCI_PMU_CNTR_LAST(cci_pmu);
 }
 
-static u32 pmu_read_register(int idx, unsigned int offset)
+static u32 pmu_read_register(struct cci_pmu *cci_pmu, int idx, unsigned int offset)
 {
-	return readl_relaxed(pmu->base + CCI_PMU_CNTR_BASE(idx) + offset);
+	return readl_relaxed(cci_pmu->base +
+			     CCI_PMU_CNTR_BASE(cci_pmu->model, idx) + offset);
 }
 
-static void pmu_write_register(u32 value, int idx, unsigned int offset)
+static void pmu_write_register(struct cci_pmu *cci_pmu, u32 value,
+			       int idx, unsigned int offset)
 {
-	return writel_relaxed(value, pmu->base + CCI_PMU_CNTR_BASE(idx) + offset);
+	return writel_relaxed(value, cci_pmu->base +
+			      CCI_PMU_CNTR_BASE(cci_pmu->model, idx) + offset);
 }
 
-static void pmu_disable_counter(int idx)
+static void pmu_disable_counter(struct cci_pmu *cci_pmu, int idx)
 {
-	pmu_write_register(0, idx, CCI_PMU_CNTR_CTRL);
+	pmu_write_register(cci_pmu, 0, idx, CCI_PMU_CNTR_CTRL);
 }
 
-static void pmu_enable_counter(int idx)
+static void pmu_enable_counter(struct cci_pmu *cci_pmu, int idx)
 {
-	pmu_write_register(1, idx, CCI_PMU_CNTR_CTRL);
+	pmu_write_register(cci_pmu, 1, idx, CCI_PMU_CNTR_CTRL);
 }
 
-static void pmu_set_event(int idx, unsigned long event)
+static void pmu_set_event(struct cci_pmu *cci_pmu, int idx, unsigned long event)
 {
-	pmu_write_register(event, idx, CCI_PMU_EVT_SEL);
+	pmu_write_register(cci_pmu, event, idx, CCI_PMU_EVT_SEL);
 }
 
+/*
+ * Returns the number of programmable counters actually implemented
+ * by the cci
+ */
 static u32 pmu_get_max_counters(void)
 {
-	u32 n_cnts = (readl_relaxed(cci_ctrl_base + CCI_PMCR) &
-		      CCI_PMCR_NCNT_MASK) >> CCI_PMCR_NCNT_SHIFT;
-
-	/* add 1 for cycle counter */
-	return n_cnts + 1;
+	return (readl_relaxed(cci_ctrl_base + CCI_PMCR) &
+		CCI_PMCR_NCNT_MASK) >> CCI_PMCR_NCNT_SHIFT;
 }
 
 static int pmu_get_event_idx(struct cci_pmu_hw_events *hw, struct perf_event *event)
 {
 	struct cci_pmu *cci_pmu = to_cci_pmu(event->pmu);
-	struct hw_perf_event *hw_event = &event->hw;
-	unsigned long cci_event = hw_event->config_base;
+	unsigned long cci_event = event->hw.config_base;
 	int idx;
 
-	if (cci_event == CCI_PMU_CYCLES) {
-		if (test_and_set_bit(CCI_PMU_CYCLE_CNTR_IDX, hw->used_mask))
-			return -EAGAIN;
+	if (cci_pmu->model->get_event_idx)
+		return cci_pmu->model->get_event_idx(cci_pmu, hw, cci_event);
 
-		return CCI_PMU_CYCLE_CNTR_IDX;
-	}
-
-	for (idx = CCI_PMU_CNTR0_IDX; idx <= CCI_PMU_CNTR_LAST(cci_pmu); ++idx)
+	/* Generic code to find an unused idx from the mask */
+	for(idx = 0; idx <= CCI_PMU_CNTR_LAST(cci_pmu); idx++)
 		if (!test_and_set_bit(idx, hw->used_mask))
 			return idx;
 
@@ -297,18 +682,13 @@ static int pmu_get_event_idx(struct cci_pmu_hw_events *hw, struct perf_event *ev
 
 static int pmu_map_event(struct perf_event *event)
 {
-	int mapping;
-	unsigned long config = event->attr.config;
+	struct cci_pmu *cci_pmu = to_cci_pmu(event->pmu);
 
-	if (event->attr.type < PERF_TYPE_MAX)
+	if (event->attr.type < PERF_TYPE_MAX ||
+			!cci_pmu->model->validate_hw_event)
 		return -ENOENT;
 
-	if (config == CCI_PMU_CYCLES)
-		mapping = config;
-	else
-		mapping = pmu_validate_hw_event(config);
-
-	return mapping;
+	return	cci_pmu->model->validate_hw_event(cci_pmu, event->attr.config);
 }
 
 static int pmu_request_irq(struct cci_pmu *cci_pmu, irq_handler_t handler)
@@ -319,7 +699,7 @@ static int pmu_request_irq(struct cci_pmu *cci_pmu, irq_handler_t handler)
 	if (unlikely(!pmu_device))
 		return -ENODEV;
 
-	if (pmu->nr_irqs < 1) {
+	if (cci_pmu->nr_irqs < 1) {
 		dev_err(&pmu_device->dev, "no irqs for CCI PMUs defined\n");
 		return -ENODEV;
 	}
@@ -331,16 +711,16 @@ static int pmu_request_irq(struct cci_pmu *cci_pmu, irq_handler_t handler)
 	 *
 	 * This should allow handling of non-unique interrupt for the counters.
 	 */
-	for (i = 0; i < pmu->nr_irqs; i++) {
-		int err = request_irq(pmu->irqs[i], handler, IRQF_SHARED,
+	for (i = 0; i < cci_pmu->nr_irqs; i++) {
+		int err = request_irq(cci_pmu->irqs[i], handler, IRQF_SHARED,
 				"arm-cci-pmu", cci_pmu);
 		if (err) {
 			dev_err(&pmu_device->dev, "unable to request IRQ%d for ARM CCI PMU counters\n",
-				pmu->irqs[i]);
+				cci_pmu->irqs[i]);
 			return err;
 		}
 
-		set_bit(i, &pmu->active_irqs);
+		set_bit(i, &cci_pmu->active_irqs);
 	}
 
 	return 0;
@@ -350,11 +730,11 @@ static void pmu_free_irq(struct cci_pmu *cci_pmu)
 {
 	int i;
 
-	for (i = 0; i < pmu->nr_irqs; i++) {
-		if (!test_and_clear_bit(i, &pmu->active_irqs))
+	for (i = 0; i < cci_pmu->nr_irqs; i++) {
+		if (!test_and_clear_bit(i, &cci_pmu->active_irqs))
 			continue;
 
-		free_irq(pmu->irqs[i], cci_pmu);
+		free_irq(cci_pmu->irqs[i], cci_pmu);
 	}
 }
 
@@ -369,7 +749,7 @@ static u32 pmu_read_counter(struct perf_event *event)
 		dev_err(&cci_pmu->plat_device->dev, "Invalid CCI PMU counter %d\n", idx);
 		return 0;
 	}
-	value = pmu_read_register(idx, CCI_PMU_CNTR);
+	value = pmu_read_register(cci_pmu, idx, CCI_PMU_CNTR);
 
 	return value;
 }
@@ -383,7 +763,7 @@ static void pmu_write_counter(struct perf_event *event, u32 value)
 	if (unlikely(!pmu_is_valid_counter(cci_pmu, idx)))
 		dev_err(&cci_pmu->plat_device->dev, "Invalid CCI PMU counter %d\n", idx);
 	else
-		pmu_write_register(value, idx, CCI_PMU_CNTR);
+		pmu_write_register(cci_pmu, value, idx, CCI_PMU_CNTR);
 }
 
 static u64 pmu_event_update(struct perf_event *event)
@@ -427,7 +807,7 @@ static irqreturn_t pmu_handle_irq(int irq_num, void *dev)
 {
 	unsigned long flags;
 	struct cci_pmu *cci_pmu = dev;
-	struct cci_pmu_hw_events *events = &pmu->hw_events;
+	struct cci_pmu_hw_events *events = &cci_pmu->hw_events;
 	int idx, handled = IRQ_NONE;
 
 	raw_spin_lock_irqsave(&events->pmu_lock, flags);
@@ -436,7 +816,7 @@ static irqreturn_t pmu_handle_irq(int irq_num, void *dev)
 	 * This should work regardless of whether we have per-counter overflow
 	 * interrupt or a combined overflow interrupt.
 	 */
-	for (idx = CCI_PMU_CYCLE_CNTR_IDX; idx <= CCI_PMU_CNTR_LAST(cci_pmu); idx++) {
+	for (idx = 0; idx <= CCI_PMU_CNTR_LAST(cci_pmu); idx++) {
 		struct perf_event *event = events->events[idx];
 		struct hw_perf_event *hw_counter;
 
@@ -446,11 +826,12 @@ static irqreturn_t pmu_handle_irq(int irq_num, void *dev)
 		hw_counter = &event->hw;
 
 		/* Did this counter overflow? */
-		if (!(pmu_read_register(idx, CCI_PMU_OVRFLW) &
+		if (!(pmu_read_register(cci_pmu, idx, CCI_PMU_OVRFLW) &
 		      CCI_PMU_OVRFLW_FLAG))
 			continue;
 
-		pmu_write_register(CCI_PMU_OVRFLW_FLAG, idx, CCI_PMU_OVRFLW);
+		pmu_write_register(cci_pmu, CCI_PMU_OVRFLW_FLAG, idx,
+							CCI_PMU_OVRFLW);
 
 		pmu_event_update(event);
 		pmu_event_set_period(event);
@@ -492,7 +873,7 @@ static void cci_pmu_enable(struct pmu *pmu)
 {
 	struct cci_pmu *cci_pmu = to_cci_pmu(pmu);
 	struct cci_pmu_hw_events *hw_events = &cci_pmu->hw_events;
-	int enabled = bitmap_weight(hw_events->used_mask, cci_pmu->num_events);
+	int enabled = bitmap_weight(hw_events->used_mask, cci_pmu->num_cntrs);
 	unsigned long flags;
 	u32 val;
 
@@ -523,6 +904,16 @@ static void cci_pmu_disable(struct pmu *pmu)
 	raw_spin_unlock_irqrestore(&hw_events->pmu_lock, flags);
 }
 
+/*
+ * Check if the idx represents a non-programmable counter.
+ * All the fixed event counters are mapped before the programmable
+ * counters.
+ */
+static bool pmu_fixed_hw_idx(struct cci_pmu *cci_pmu, int idx)
+{
+	return (idx >= 0) && (idx < cci_pmu->model->fixed_hw_cntrs);
+}
+
 static void cci_pmu_start(struct perf_event *event, int pmu_flags)
 {
 	struct cci_pmu *cci_pmu = to_cci_pmu(event->pmu);
@@ -547,12 +938,12 @@ static void cci_pmu_start(struct perf_event *event, int pmu_flags)
 
 	raw_spin_lock_irqsave(&hw_events->pmu_lock, flags);
 
-	/* Configure the event to count, unless you are counting cycles */
-	if (idx != CCI_PMU_CYCLE_CNTR_IDX)
-		pmu_set_event(idx, hwc->config_base);
+	/* Configure the counter unless you are counting a fixed event */
+	if (!pmu_fixed_hw_idx(cci_pmu, idx))
+		pmu_set_event(cci_pmu, idx, hwc->config_base);
 
 	pmu_event_set_period(event);
-	pmu_enable_counter(idx);
+	pmu_enable_counter(cci_pmu, idx);
 
 	raw_spin_unlock_irqrestore(&hw_events->pmu_lock, flags);
 }
@@ -575,7 +966,7 @@ static void cci_pmu_stop(struct perf_event *event, int pmu_flags)
 	 * We always reprogram the counter, so ignore PERF_EF_UPDATE. See
 	 * cci_pmu_start()
 	 */
-	pmu_disable_counter(idx);
+	pmu_disable_counter(cci_pmu, idx);
 	pmu_event_update(event);
 	hwc->state |= PERF_HES_STOPPED | PERF_HES_UPTODATE;
 }
@@ -655,13 +1046,16 @@ static int
 validate_group(struct perf_event *event)
 {
 	struct perf_event *sibling, *leader = event->group_leader;
+	struct cci_pmu *cci_pmu = to_cci_pmu(event->pmu);
+	unsigned long mask[BITS_TO_LONGS(cci_pmu->num_cntrs)];
 	struct cci_pmu_hw_events fake_pmu = {
 		/*
 		 * Initialise the fake PMU. We only need to populate the
 		 * used_mask for the purposes of validation.
 		 */
-		.used_mask = { 0 },
+		.used_mask = mask,
 	};
+	memset(mask, 0, BITS_TO_LONGS(cci_pmu->num_cntrs) * sizeof(unsigned long));
 
 	if (!validate_event(event->pmu, &fake_pmu, leader))
 		return -EINVAL;
@@ -779,20 +1173,27 @@ static int cci_pmu_event_init(struct perf_event *event)
 	return err;
 }
 
-static ssize_t pmu_attr_cpumask_show(struct device *dev,
+static ssize_t pmu_cpumask_attr_show(struct device *dev,
 				     struct device_attribute *attr, char *buf)
 {
+	struct dev_ext_attribute *eattr = container_of(attr,
+					struct dev_ext_attribute, attr);
+	struct cci_pmu *cci_pmu = eattr->var;
+
 	int n = scnprintf(buf, PAGE_SIZE - 1, "%*pbl",
-			  cpumask_pr_args(&pmu->cpus));
+			  cpumask_pr_args(&cci_pmu->cpus));
 	buf[n++] = '\n';
 	buf[n] = '\0';
 	return n;
 }
 
-static DEVICE_ATTR(cpumask, S_IRUGO, pmu_attr_cpumask_show, NULL);
+static struct dev_ext_attribute pmu_cpumask_attr = {
+	__ATTR(cpumask, S_IRUGO, pmu_cpumask_attr_show, NULL),
+	NULL,		/* Populated in cci_pmu_init */
+};
 
 static struct attribute *pmu_attrs[] = {
-	&dev_attr_cpumask.attr,
+	&pmu_cpumask_attr.attr.attr,
 	NULL,
 };
 
@@ -800,14 +1201,78 @@ static struct attribute_group pmu_attr_group = {
 	.attrs = pmu_attrs,
 };
 
+static struct attribute_group pmu_format_attr_group = {
+	.name = "format",
+	.attrs = NULL,		/* Filled in cci_pmu_init_attrs */
+};
+
+static struct attribute_group pmu_event_attr_group = {
+	.name = "events",
+	.attrs = NULL,		/* Filled in cci_pmu_init_attrs */
+};
+
 static const struct attribute_group *pmu_attr_groups[] = {
 	&pmu_attr_group,
+	&pmu_format_attr_group,
+	&pmu_event_attr_group,
 	NULL
 };
 
+static struct attribute **alloc_attrs(struct platform_device *pdev,
+				int n, struct dev_ext_attribute *source)
+{
+	int i;
+	struct attribute **attrs;
+
+	/* Alloc n + 1 (for terminating NULL) */
+	attrs  = devm_kcalloc(&pdev->dev, n + 1, sizeof(struct attribute *),
+								GFP_KERNEL);
+	if (!attrs)
+		return attrs;
+	for(i = 0; i < n; i++)
+		attrs[i] = &source[i].attr.attr;
+	return attrs;
+}
+
+static int cci_pmu_init_attrs(struct cci_pmu *cci_pmu, struct platform_device *pdev)
+{
+	const struct cci_pmu_model *model = cci_pmu->model;
+	struct attribute **attrs;
+
+	/*
+	 * All allocations below are managed, hence doesn't need to be
+	 * free'd explicitly in case of an error.
+	 */
+
+	if (model->nevent_attrs) {
+		attrs = alloc_attrs(pdev, model->nevent_attrs,
+						model->event_attrs);
+		if (!attrs)
+			return -ENOMEM;
+		pmu_event_attr_group.attrs = attrs;
+	}
+	if (model->nformat_attrs) {
+		attrs = alloc_attrs(pdev, model->nformat_attrs,
+						 model->format_attrs);
+		if (!attrs)
+			return -ENOMEM;
+		pmu_format_attr_group.attrs = attrs;
+	}
+	pmu_cpumask_attr.var = cci_pmu;
+
+	return 0;
+}
+
 static int cci_pmu_init(struct cci_pmu *cci_pmu, struct platform_device *pdev)
 {
 	char *name = cci_pmu->model->name;
+	u32 num_cntrs;
+	int rc;
+
+	rc = cci_pmu_init_attrs(cci_pmu, pdev);
+	if (rc)
+		return rc;
+
 	cci_pmu->pmu = (struct pmu) {
 		.name		= cci_pmu->model->name,
 		.task_ctx_nr	= perf_invalid_context,
@@ -823,7 +1288,15 @@ static int cci_pmu_init(struct cci_pmu *cci_pmu, struct platform_device *pdev)
 	};
 
 	cci_pmu->plat_device = pdev;
-	cci_pmu->num_events = pmu_get_max_counters();
+	num_cntrs = pmu_get_max_counters();
+	if (num_cntrs > cci_pmu->model->num_hw_cntrs) {
+		dev_warn(&pdev->dev,
+			"PMU implements more counters(%d) than supported by"
+			" the model(%d), truncated.",
+			num_cntrs, cci_pmu->model->num_hw_cntrs);
+		num_cntrs = cci_pmu->model->num_hw_cntrs;
+	}
+	cci_pmu->num_cntrs = num_cntrs + cci_pmu->model->fixed_hw_cntrs;
 
 	return perf_pmu_register(&cci_pmu->pmu, name, -1);
 }
@@ -831,12 +1304,14 @@ static int cci_pmu_init(struct cci_pmu *cci_pmu, struct platform_device *pdev)
 static int cci_pmu_cpu_notifier(struct notifier_block *self,
 				unsigned long action, void *hcpu)
 {
+	struct cci_pmu *cci_pmu = container_of(self,
+					struct cci_pmu, cpu_nb);
 	unsigned int cpu = (long)hcpu;
 	unsigned int target;
 
 	switch (action & ~CPU_TASKS_FROZEN) {
 	case CPU_DOWN_PREPARE:
-		if (!cpumask_test_and_clear_cpu(cpu, &pmu->cpus))
+		if (!cpumask_test_and_clear_cpu(cpu, &cci_pmu->cpus))
 			break;
 		target = cpumask_any_but(cpu_online_mask, cpu);
 		if (target < 0) // UP, last CPU
@@ -845,7 +1320,7 @@ static int cci_pmu_cpu_notifier(struct notifier_block *self,
 		 * TODO: migrate context once core races on event->ctx have
 		 * been fixed.
 		 */
-		cpumask_set_cpu(target, &pmu->cpus);
+		cpumask_set_cpu(target, &cci_pmu->cpus);
 	default:
 		break;
 	}
@@ -853,57 +1328,103 @@ static int cci_pmu_cpu_notifier(struct notifier_block *self,
 	return NOTIFY_OK;
 }
 
-static struct notifier_block cci_pmu_cpu_nb = {
-	.notifier_call	= cci_pmu_cpu_notifier,
-	/*
-	 * to migrate uncore events, our notifier should be executed
-	 * before perf core's notifier.
-	 */
-	.priority	= CPU_PRI_PERF + 1,
-};
-
 static struct cci_pmu_model cci_pmu_models[] = {
-	[CCI_REV_R0] = {
+#ifdef CONFIG_ARM_CCI400_PMU
+	[CCI400_R0] = {
 		.name = "CCI_400",
+		.fixed_hw_cntrs = 1,	/* Cycle counter */
+		.num_hw_cntrs = 4,
+		.cntr_size = SZ_4K,
+		.format_attrs = cci400_pmu_format_attrs,
+		.nformat_attrs = ARRAY_SIZE(cci400_pmu_format_attrs),
+		.event_attrs = cci400_r0_pmu_event_attrs,
+		.nevent_attrs = ARRAY_SIZE(cci400_r0_pmu_event_attrs),
 		.event_ranges = {
 			[CCI_IF_SLAVE] = {
-				CCI_REV_R0_SLAVE_PORT_MIN_EV,
-				CCI_REV_R0_SLAVE_PORT_MAX_EV,
+				CCI400_R0_SLAVE_PORT_MIN_EV,
+				CCI400_R0_SLAVE_PORT_MAX_EV,
 			},
 			[CCI_IF_MASTER] = {
-				CCI_REV_R0_MASTER_PORT_MIN_EV,
-				CCI_REV_R0_MASTER_PORT_MAX_EV,
+				CCI400_R0_MASTER_PORT_MIN_EV,
+				CCI400_R0_MASTER_PORT_MAX_EV,
 			},
 		},
+		.validate_hw_event = cci400_validate_hw_event,
+		.get_event_idx = cci400_get_event_idx,
 	},
-	[CCI_REV_R1] = {
+	[CCI400_R1] = {
 		.name = "CCI_400_r1",
+		.fixed_hw_cntrs = 1,	/* Cycle counter */
+		.num_hw_cntrs = 4,
+		.cntr_size = SZ_4K,
+		.format_attrs = cci400_pmu_format_attrs,
+		.nformat_attrs = ARRAY_SIZE(cci400_pmu_format_attrs),
+		.event_attrs = cci400_r1_pmu_event_attrs,
+		.nevent_attrs = ARRAY_SIZE(cci400_r1_pmu_event_attrs),
 		.event_ranges = {
 			[CCI_IF_SLAVE] = {
-				CCI_REV_R1_SLAVE_PORT_MIN_EV,
-				CCI_REV_R1_SLAVE_PORT_MAX_EV,
+				CCI400_R1_SLAVE_PORT_MIN_EV,
+				CCI400_R1_SLAVE_PORT_MAX_EV,
 			},
 			[CCI_IF_MASTER] = {
-				CCI_REV_R1_MASTER_PORT_MIN_EV,
-				CCI_REV_R1_MASTER_PORT_MAX_EV,
+				CCI400_R1_MASTER_PORT_MIN_EV,
+				CCI400_R1_MASTER_PORT_MAX_EV,
 			},
 		},
+		.validate_hw_event = cci400_validate_hw_event,
+		.get_event_idx = cci400_get_event_idx,
 	},
+#endif
+#ifdef CONFIG_ARM_CCI500_PMU
+	[CCI500_R0] = {
+		.name = "CCI_500",
+		.fixed_hw_cntrs = 0,
+		.num_hw_cntrs = 8,
+		.cntr_size = SZ_64K,
+		.format_attrs = cci500_pmu_format_attrs,
+		.nformat_attrs = ARRAY_SIZE(cci500_pmu_format_attrs),
+		.event_attrs = cci500_pmu_event_attrs,
+		.nevent_attrs = ARRAY_SIZE(cci500_pmu_event_attrs),
+		.event_ranges = {
+			[CCI_IF_SLAVE] = {
+				CCI500_SLAVE_PORT_MIN_EV,
+				CCI500_SLAVE_PORT_MAX_EV,
+			},
+			[CCI_IF_MASTER] = {
+				CCI500_MASTER_PORT_MIN_EV,
+				CCI500_MASTER_PORT_MAX_EV,
+			},
+			[CCI_IF_GLOBAL] = {
+				CCI500_GLOBAL_PORT_MIN_EV,
+				CCI500_GLOBAL_PORT_MAX_EV,
+			},
+		},
+		.validate_hw_event = cci500_validate_hw_event,
+	},
+#endif
 };
 
 static const struct of_device_id arm_cci_pmu_matches[] = {
+#ifdef CONFIG_ARM_CCI400_PMU
 	{
 		.compatible = "arm,cci-400-pmu",
 		.data	= NULL,
 	},
 	{
 		.compatible = "arm,cci-400-pmu,r0",
-		.data	= &cci_pmu_models[CCI_REV_R0],
+		.data	= &cci_pmu_models[CCI400_R0],
 	},
 	{
 		.compatible = "arm,cci-400-pmu,r1",
-		.data	= &cci_pmu_models[CCI_REV_R1],
+		.data	= &cci_pmu_models[CCI400_R1],
+	},
+#endif
+#ifdef CONFIG_ARM_CCI500_PMU
+	{
+		.compatible = "arm,cci-500-pmu,r0",
+		.data = &cci_pmu_models[CCI500_R0],
 	},
+#endif
 	{},
 };
 
@@ -932,68 +1453,114 @@ static bool is_duplicate_irq(int irq, int *irqs, int nr_irqs)
 	return false;
 }
 
-static int cci_pmu_probe(struct platform_device *pdev)
+static struct cci_pmu *cci_pmu_alloc(struct platform_device *pdev)
 {
-	struct resource *res;
-	int i, ret, irq;
+	struct cci_pmu *cci_pmu;
 	const struct cci_pmu_model *model;
 
+	/*
+	 * All allocations are devm_* hence we don't have to free
+	 * them explicitly on an error, as it would end up in driver
+	 * detach.
+	 */
 	model = get_cci_model(pdev);
 	if (!model) {
 		dev_warn(&pdev->dev, "CCI PMU version not supported\n");
-		return -ENODEV;
+		return ERR_PTR(-ENODEV);
 	}
 
-	pmu = devm_kzalloc(&pdev->dev, sizeof(*pmu), GFP_KERNEL);
-	if (!pmu)
-		return -ENOMEM;
+	cci_pmu = devm_kzalloc(&pdev->dev, sizeof(*cci_pmu), GFP_KERNEL);
+	if (!cci_pmu)
+		return ERR_PTR(-ENOMEM);
+
+	cci_pmu->model = model;
+	cci_pmu->irqs = devm_kcalloc(&pdev->dev, CCI_PMU_MAX_HW_CNTRS(model),
+					sizeof(*cci_pmu->irqs), GFP_KERNEL);
+	if (!cci_pmu->irqs)
+		return ERR_PTR(-ENOMEM);
+	cci_pmu->hw_events.events = devm_kcalloc(&pdev->dev,
+					     CCI_PMU_MAX_HW_CNTRS(model),
+					     sizeof(*cci_pmu->hw_events.events),
+					     GFP_KERNEL);
+	if (!cci_pmu->hw_events.events)
+		return ERR_PTR(-ENOMEM);
+	cci_pmu->hw_events.used_mask = devm_kcalloc(&pdev->dev,
+						BITS_TO_LONGS(CCI_PMU_MAX_HW_CNTRS(model)),
+						sizeof(*cci_pmu->hw_events.used_mask),
+						GFP_KERNEL);
+	if (!cci_pmu->hw_events.used_mask)
+		return ERR_PTR(-ENOMEM);
+
+	return cci_pmu;
+}
+
+
+static int cci_pmu_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct cci_pmu *cci_pmu;
+	int i, ret, irq;
+
+	cci_pmu = cci_pmu_alloc(pdev);
+	if (IS_ERR(cci_pmu))
+		return PTR_ERR(cci_pmu);
 
-	pmu->model = model;
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	pmu->base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(pmu->base))
+	cci_pmu->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(cci_pmu->base))
 		return -ENOMEM;
 
 	/*
-	 * CCI PMU has 5 overflow signals - one per counter; but some may be tied
+	 * CCI PMU has one overflow interrupt per counter; but some may be tied
 	 * together to a common interrupt.
 	 */
-	pmu->nr_irqs = 0;
-	for (i = 0; i < CCI_PMU_MAX_HW_EVENTS; i++) {
+	cci_pmu->nr_irqs = 0;
+	for (i = 0; i < CCI_PMU_MAX_HW_CNTRS(cci_pmu->model); i++) {
 		irq = platform_get_irq(pdev, i);
 		if (irq < 0)
 			break;
 
-		if (is_duplicate_irq(irq, pmu->irqs, pmu->nr_irqs))
+		if (is_duplicate_irq(irq, cci_pmu->irqs, cci_pmu->nr_irqs))
 			continue;
 
-		pmu->irqs[pmu->nr_irqs++] = irq;
+		cci_pmu->irqs[cci_pmu->nr_irqs++] = irq;
 	}
 
 	/*
 	 * Ensure that the device tree has as many interrupts as the number
 	 * of counters.
 	 */
-	if (i < CCI_PMU_MAX_HW_EVENTS) {
+	if (i < CCI_PMU_MAX_HW_CNTRS(cci_pmu->model)) {
 		dev_warn(&pdev->dev, "In-correct number of interrupts: %d, should be %d\n",
-			i, CCI_PMU_MAX_HW_EVENTS);
+			i, CCI_PMU_MAX_HW_CNTRS(cci_pmu->model));
 		return -EINVAL;
 	}
 
-	raw_spin_lock_init(&pmu->hw_events.pmu_lock);
-	mutex_init(&pmu->reserve_mutex);
-	atomic_set(&pmu->active_events, 0);
-	cpumask_set_cpu(smp_processor_id(), &pmu->cpus);
+	raw_spin_lock_init(&cci_pmu->hw_events.pmu_lock);
+	mutex_init(&cci_pmu->reserve_mutex);
+	atomic_set(&cci_pmu->active_events, 0);
+	cpumask_set_cpu(smp_processor_id(), &cci_pmu->cpus);
+
+	cci_pmu->cpu_nb = (struct notifier_block) {
+		.notifier_call	= cci_pmu_cpu_notifier,
+		/*
+		 * to migrate uncore events, our notifier should be executed
+		 * before perf core's notifier.
+		 */
+		.priority	= CPU_PRI_PERF + 1,
+	};
 
-	ret = register_cpu_notifier(&cci_pmu_cpu_nb);
+	ret = register_cpu_notifier(&cci_pmu->cpu_nb);
 	if (ret)
 		return ret;
 
-	ret = cci_pmu_init(pmu, pdev);
-	if (ret)
+	ret = cci_pmu_init(cci_pmu, pdev);
+	if (ret) {
+		unregister_cpu_notifier(&cci_pmu->cpu_nb);
 		return ret;
+	}
 
-	pr_info("ARM %s PMU driver probed", pmu->model->name);
+	pr_info("ARM %s PMU driver probed", cci_pmu->model->name);
 	return 0;
 }
 
@@ -1032,14 +1599,14 @@ static int __init cci_platform_init(void)
 	return platform_driver_register(&cci_platform_driver);
 }
 
-#else /* !CONFIG_ARM_CCI400_PMU */
+#else /* !CONFIG_ARM_CCI_PMU */
 
 static int __init cci_platform_init(void)
 {
 	return 0;
 }
 
-#endif /* CONFIG_ARM_CCI400_PMU */
+#endif /* CONFIG_ARM_CCI_PMU */
 
 #ifdef CONFIG_ARM_CCI400_PORT_CTRL
 
diff --git a/drivers/bus/arm-ccn.c b/drivers/bus/arm-ccn.c
index aaa0f2a87118..7d9879e166cf 100644
--- a/drivers/bus/arm-ccn.c
+++ b/drivers/bus/arm-ccn.c
@@ -166,13 +166,17 @@ struct arm_ccn_dt {
 
 	struct hrtimer hrtimer;
 
+	cpumask_t cpu;
+	struct notifier_block cpu_nb;
+
 	struct pmu pmu;
 };
 
 struct arm_ccn {
 	struct device *dev;
 	void __iomem *base;
-	unsigned irq_used:1;
+	unsigned int irq;
+
 	unsigned sbas_present:1;
 	unsigned sbsx_present:1;
 
@@ -212,7 +216,7 @@ static int arm_ccn_node_to_xp_port(int node)
 
 static void arm_ccn_pmu_config_set(u64 *config, u32 node_xp, u32 type, u32 port)
 {
-	*config &= ~((0xff << 0) | (0xff << 8) | (0xff << 24));
+	*config &= ~((0xff << 0) | (0xff << 8) | (0x3 << 24));
 	*config |= (node_xp << 0) | (type << 8) | (port << 24);
 }
 
@@ -336,6 +340,23 @@ static ssize_t arm_ccn_pmu_event_show(struct device *dev,
 	if (event->mask)
 		res += snprintf(buf + res, PAGE_SIZE - res, ",mask=0x%x",
 				event->mask);
+
+	/* Arguments required by an event */
+	switch (event->type) {
+	case CCN_TYPE_CYCLES:
+		break;
+	case CCN_TYPE_XP:
+		res += snprintf(buf + res, PAGE_SIZE - res,
+				",xp=?,port=?,vc=?,dir=?");
+		if (event->event == CCN_EVENT_WATCHPOINT)
+			res += snprintf(buf + res, PAGE_SIZE - res,
+					",cmp_l=?,cmp_h=?,mask=?");
+		break;
+	default:
+		res += snprintf(buf + res, PAGE_SIZE - res, ",node=?");
+		break;
+	}
+
 	res += snprintf(buf + res, PAGE_SIZE - res, "\n");
 
 	return res;
@@ -521,6 +542,25 @@ static struct attribute_group arm_ccn_pmu_cmp_mask_attr_group = {
 	.attrs = arm_ccn_pmu_cmp_mask_attrs,
 };
 
+static ssize_t arm_ccn_pmu_cpumask_show(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	struct arm_ccn *ccn = pmu_to_arm_ccn(dev_get_drvdata(dev));
+
+	return cpumap_print_to_pagebuf(true, buf, &ccn->dt.cpu);
+}
+
+static struct device_attribute arm_ccn_pmu_cpumask_attr =
+		__ATTR(cpumask, S_IRUGO, arm_ccn_pmu_cpumask_show, NULL);
+
+static struct attribute *arm_ccn_pmu_cpumask_attrs[] = {
+	&arm_ccn_pmu_cpumask_attr.attr,
+	NULL,
+};
+
+static struct attribute_group arm_ccn_pmu_cpumask_attr_group = {
+	.attrs = arm_ccn_pmu_cpumask_attrs,
+};
 
 /*
  * Default poll period is 10ms, which is way over the top anyway,
@@ -542,6 +582,7 @@ static const struct attribute_group *arm_ccn_pmu_attr_groups[] = {
 	&arm_ccn_pmu_events_attr_group,
 	&arm_ccn_pmu_format_attr_group,
 	&arm_ccn_pmu_cmp_mask_attr_group,
+	&arm_ccn_pmu_cpumask_attr_group,
 	NULL
 };
 
@@ -587,7 +628,65 @@ static int arm_ccn_pmu_type_eq(u32 a, u32 b)
 	return 0;
 }
 
-static void arm_ccn_pmu_event_destroy(struct perf_event *event)
+static int arm_ccn_pmu_event_alloc(struct perf_event *event)
+{
+	struct arm_ccn *ccn = pmu_to_arm_ccn(event->pmu);
+	struct hw_perf_event *hw = &event->hw;
+	u32 node_xp, type, event_id;
+	struct arm_ccn_component *source;
+	int bit;
+
+	node_xp = CCN_CONFIG_NODE(event->attr.config);
+	type = CCN_CONFIG_TYPE(event->attr.config);
+	event_id = CCN_CONFIG_EVENT(event->attr.config);
+
+	/* Allocate the cycle counter */
+	if (type == CCN_TYPE_CYCLES) {
+		if (test_and_set_bit(CCN_IDX_PMU_CYCLE_COUNTER,
+				ccn->dt.pmu_counters_mask))
+			return -EAGAIN;
+
+		hw->idx = CCN_IDX_PMU_CYCLE_COUNTER;
+		ccn->dt.pmu_counters[CCN_IDX_PMU_CYCLE_COUNTER].event = event;
+
+		return 0;
+	}
+
+	/* Allocate an event counter */
+	hw->idx = arm_ccn_pmu_alloc_bit(ccn->dt.pmu_counters_mask,
+			CCN_NUM_PMU_EVENT_COUNTERS);
+	if (hw->idx < 0) {
+		dev_dbg(ccn->dev, "No more counters available!\n");
+		return -EAGAIN;
+	}
+
+	if (type == CCN_TYPE_XP)
+		source = &ccn->xp[node_xp];
+	else
+		source = &ccn->node[node_xp];
+	ccn->dt.pmu_counters[hw->idx].source = source;
+
+	/* Allocate an event source or a watchpoint */
+	if (type == CCN_TYPE_XP && event_id == CCN_EVENT_WATCHPOINT)
+		bit = arm_ccn_pmu_alloc_bit(source->xp.dt_cmp_mask,
+				CCN_NUM_XP_WATCHPOINTS);
+	else
+		bit = arm_ccn_pmu_alloc_bit(source->pmu_events_mask,
+				CCN_NUM_PMU_EVENTS);
+	if (bit < 0) {
+		dev_dbg(ccn->dev, "No more event sources/watchpoints on node/XP %d!\n",
+				node_xp);
+		clear_bit(hw->idx, ccn->dt.pmu_counters_mask);
+		return -EAGAIN;
+	}
+	hw->config_base = bit;
+
+	ccn->dt.pmu_counters[hw->idx].event = event;
+
+	return 0;
+}
+
+static void arm_ccn_pmu_event_release(struct perf_event *event)
 {
 	struct arm_ccn *ccn = pmu_to_arm_ccn(event->pmu);
 	struct hw_perf_event *hw = &event->hw;
@@ -616,15 +715,14 @@ static int arm_ccn_pmu_event_init(struct perf_event *event)
 	struct arm_ccn *ccn;
 	struct hw_perf_event *hw = &event->hw;
 	u32 node_xp, type, event_id;
-	int valid, bit;
-	struct arm_ccn_component *source;
+	int valid;
 	int i;
+	struct perf_event *sibling;
 
 	if (event->attr.type != event->pmu->type)
 		return -ENOENT;
 
 	ccn = pmu_to_arm_ccn(event->pmu);
-	event->destroy = arm_ccn_pmu_event_destroy;
 
 	if (hw->sample_period) {
 		dev_warn(ccn->dev, "Sampling not supported!\n");
@@ -642,6 +740,16 @@ static int arm_ccn_pmu_event_init(struct perf_event *event)
 		dev_warn(ccn->dev, "Can't provide per-task data!\n");
 		return -EOPNOTSUPP;
 	}
+	/*
+	 * Many perf core operations (eg. events rotation) operate on a
+	 * single CPU context. This is obvious for CPU PMUs, where one
+	 * expects the same sets of events being observed on all CPUs,
+	 * but can lead to issues for off-core PMUs, like CCN, where each
+	 * event could be theoretically assigned to a different CPU. To
+	 * mitigate this, we enforce CPU assignment to one, selected
+	 * processor (the one described in the "cpumask" attribute).
+	 */
+	event->cpu = cpumask_first(&ccn->dt.cpu);
 
 	node_xp = CCN_CONFIG_NODE(event->attr.config);
 	type = CCN_CONFIG_TYPE(event->attr.config);
@@ -711,48 +819,20 @@ static int arm_ccn_pmu_event_init(struct perf_event *event)
 				node_xp, type, port);
 	}
 
-	/* Allocate the cycle counter */
-	if (type == CCN_TYPE_CYCLES) {
-		if (test_and_set_bit(CCN_IDX_PMU_CYCLE_COUNTER,
-				ccn->dt.pmu_counters_mask))
-			return -EAGAIN;
-
-		hw->idx = CCN_IDX_PMU_CYCLE_COUNTER;
-		ccn->dt.pmu_counters[CCN_IDX_PMU_CYCLE_COUNTER].event = event;
-
-		return 0;
-	}
-
-	/* Allocate an event counter */
-	hw->idx = arm_ccn_pmu_alloc_bit(ccn->dt.pmu_counters_mask,
-			CCN_NUM_PMU_EVENT_COUNTERS);
-	if (hw->idx < 0) {
-		dev_warn(ccn->dev, "No more counters available!\n");
-		return -EAGAIN;
-	}
-
-	if (type == CCN_TYPE_XP)
-		source = &ccn->xp[node_xp];
-	else
-		source = &ccn->node[node_xp];
-	ccn->dt.pmu_counters[hw->idx].source = source;
-
-	/* Allocate an event source or a watchpoint */
-	if (type == CCN_TYPE_XP && event_id == CCN_EVENT_WATCHPOINT)
-		bit = arm_ccn_pmu_alloc_bit(source->xp.dt_cmp_mask,
-				CCN_NUM_XP_WATCHPOINTS);
-	else
-		bit = arm_ccn_pmu_alloc_bit(source->pmu_events_mask,
-				CCN_NUM_PMU_EVENTS);
-	if (bit < 0) {
-		dev_warn(ccn->dev, "No more event sources/watchpoints on node/XP %d!\n",
-				node_xp);
-		clear_bit(hw->idx, ccn->dt.pmu_counters_mask);
-		return -EAGAIN;
-	}
-	hw->config_base = bit;
+	/*
+	 * We must NOT create groups containing mixed PMUs, although software
+	 * events are acceptable (for example to create a CCN group
+	 * periodically read when a hrtimer aka cpu-clock leader triggers).
+	 */
+	if (event->group_leader->pmu != event->pmu &&
+			!is_software_event(event->group_leader))
+		return -EINVAL;
 
-	ccn->dt.pmu_counters[hw->idx].event = event;
+	list_for_each_entry(sibling, &event->group_leader->sibling_list,
+			group_entry)
+		if (sibling->pmu != event->pmu &&
+				!is_software_event(sibling))
+			return -EINVAL;
 
 	return 0;
 }
@@ -835,9 +915,14 @@ static void arm_ccn_pmu_event_start(struct perf_event *event, int flags)
 			arm_ccn_pmu_read_counter(ccn, hw->idx));
 	hw->state = 0;
 
-	if (!ccn->irq_used)
+	/*
+	 * Pin the timer, so that the overflows are handled by the chosen
+	 * event->cpu (this is the same one as presented in "cpumask"
+	 * attribute).
+	 */
+	if (!ccn->irq)
 		hrtimer_start(&ccn->dt.hrtimer, arm_ccn_pmu_timer_period(),
-				HRTIMER_MODE_REL);
+				HRTIMER_MODE_REL_PINNED);
 
 	/* Set the DT bus input, engaging the counter */
 	arm_ccn_pmu_xp_dt_config(event, 1);
@@ -852,7 +937,7 @@ static void arm_ccn_pmu_event_stop(struct perf_event *event, int flags)
 	/* Disable counting, setting the DT bus to pass-through mode */
 	arm_ccn_pmu_xp_dt_config(event, 0);
 
-	if (!ccn->irq_used)
+	if (!ccn->irq)
 		hrtimer_cancel(&ccn->dt.hrtimer);
 
 	/* Let the DT bus drain */
@@ -1014,8 +1099,13 @@ static void arm_ccn_pmu_event_config(struct perf_event *event)
 
 static int arm_ccn_pmu_event_add(struct perf_event *event, int flags)
 {
+	int err;
 	struct hw_perf_event *hw = &event->hw;
 
+	err = arm_ccn_pmu_event_alloc(event);
+	if (err)
+		return err;
+
 	arm_ccn_pmu_event_config(event);
 
 	hw->state = PERF_HES_STOPPED;
@@ -1029,6 +1119,8 @@ static int arm_ccn_pmu_event_add(struct perf_event *event, int flags)
 static void arm_ccn_pmu_event_del(struct perf_event *event, int flags)
 {
 	arm_ccn_pmu_event_stop(event, PERF_EF_UPDATE);
+
+	arm_ccn_pmu_event_release(event);
 }
 
 static void arm_ccn_pmu_event_read(struct perf_event *event)
@@ -1079,12 +1171,39 @@ static enum hrtimer_restart arm_ccn_pmu_timer_handler(struct hrtimer *hrtimer)
 }
 
 
+static int arm_ccn_pmu_cpu_notifier(struct notifier_block *nb,
+		unsigned long action, void *hcpu)
+{
+	struct arm_ccn_dt *dt = container_of(nb, struct arm_ccn_dt, cpu_nb);
+	struct arm_ccn *ccn = container_of(dt, struct arm_ccn, dt);
+	unsigned int cpu = (long)hcpu; /* for (long) see kernel/cpu.c */
+	unsigned int target;
+
+	switch (action & ~CPU_TASKS_FROZEN) {
+	case CPU_DOWN_PREPARE:
+		if (!cpumask_test_and_clear_cpu(cpu, &dt->cpu))
+			break;
+		target = cpumask_any_but(cpu_online_mask, cpu);
+		if (target < 0)
+			break;
+		perf_pmu_migrate_context(&dt->pmu, cpu, target);
+		cpumask_set_cpu(target, &dt->cpu);
+		WARN_ON(irq_set_affinity(ccn->irq, &dt->cpu) != 0);
+	default:
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+
 static DEFINE_IDA(arm_ccn_pmu_ida);
 
 static int arm_ccn_pmu_init(struct arm_ccn *ccn)
 {
 	int i;
 	char *name;
+	int err;
 
 	/* Initialize DT subsystem */
 	ccn->dt.base = ccn->base + CCN_REGION_SIZE;
@@ -1136,20 +1255,58 @@ static int arm_ccn_pmu_init(struct arm_ccn *ccn)
 	};
 
 	/* No overflow interrupt? Have to use a timer instead. */
-	if (!ccn->irq_used) {
+	if (!ccn->irq) {
 		dev_info(ccn->dev, "No access to interrupts, using timer.\n");
 		hrtimer_init(&ccn->dt.hrtimer, CLOCK_MONOTONIC,
 				HRTIMER_MODE_REL);
 		ccn->dt.hrtimer.function = arm_ccn_pmu_timer_handler;
 	}
 
-	return perf_pmu_register(&ccn->dt.pmu, name, -1);
+	/* Pick one CPU which we will use to collect data from CCN... */
+	cpumask_set_cpu(smp_processor_id(), &ccn->dt.cpu);
+
+	/*
+	 * ... and change the selection when it goes offline. Priority is
+	 * picked to have a chance to migrate events before perf is notified.
+	 */
+	ccn->dt.cpu_nb.notifier_call = arm_ccn_pmu_cpu_notifier;
+	ccn->dt.cpu_nb.priority = CPU_PRI_PERF + 1,
+	err = register_cpu_notifier(&ccn->dt.cpu_nb);
+	if (err)
+		goto error_cpu_notifier;
+
+	/* Also make sure that the overflow interrupt is handled by this CPU */
+	if (ccn->irq) {
+		err = irq_set_affinity(ccn->irq, &ccn->dt.cpu);
+		if (err) {
+			dev_err(ccn->dev, "Failed to set interrupt affinity!\n");
+			goto error_set_affinity;
+		}
+	}
+
+	err = perf_pmu_register(&ccn->dt.pmu, name, -1);
+	if (err)
+		goto error_pmu_register;
+
+	return 0;
+
+error_pmu_register:
+error_set_affinity:
+	unregister_cpu_notifier(&ccn->dt.cpu_nb);
+error_cpu_notifier:
+	ida_simple_remove(&arm_ccn_pmu_ida, ccn->dt.id);
+	for (i = 0; i < ccn->num_xps; i++)
+		writel(0, ccn->xp[i].base + CCN_XP_DT_CONTROL);
+	writel(0, ccn->dt.base + CCN_DT_PMCR);
+	return err;
 }
 
 static void arm_ccn_pmu_cleanup(struct arm_ccn *ccn)
 {
 	int i;
 
+	irq_set_affinity(ccn->irq, cpu_possible_mask);
+	unregister_cpu_notifier(&ccn->dt.cpu_nb);
 	for (i = 0; i < ccn->num_xps; i++)
 		writel(0, ccn->xp[i].base + CCN_XP_DT_CONTROL);
 	writel(0, ccn->dt.base + CCN_DT_PMCR);
@@ -1285,6 +1442,7 @@ static int arm_ccn_probe(struct platform_device *pdev)
 {
 	struct arm_ccn *ccn;
 	struct resource *res;
+	unsigned int irq;
 	int err;
 
 	ccn = devm_kzalloc(&pdev->dev, sizeof(*ccn), GFP_KERNEL);
@@ -1309,6 +1467,7 @@ static int arm_ccn_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res)
 		return -EINVAL;
+	irq = res->start;
 
 	/* Check if we can use the interrupt */
 	writel(CCN_MN_ERRINT_STATUS__PMU_EVENTS__DISABLE,
@@ -1318,13 +1477,12 @@ static int arm_ccn_probe(struct platform_device *pdev)
 		/* Can set 'disable' bits, so can acknowledge interrupts */
 		writel(CCN_MN_ERRINT_STATUS__PMU_EVENTS__ENABLE,
 				ccn->base + CCN_MN_ERRINT_STATUS);
-		err = devm_request_irq(ccn->dev, res->start,
-				arm_ccn_irq_handler, 0, dev_name(ccn->dev),
-				ccn);
+		err = devm_request_irq(ccn->dev, irq, arm_ccn_irq_handler, 0,
+				dev_name(ccn->dev), ccn);
 		if (err)
 			return err;
 
-		ccn->irq_used = 1;
+		ccn->irq = irq;
 	}
 
 
diff --git a/drivers/bus/brcmstb_gisb.c b/drivers/bus/brcmstb_gisb.c
index 738612c45266..f364fa4d24eb 100644
--- a/drivers/bus/brcmstb_gisb.c
+++ b/drivers/bus/brcmstb_gisb.c
@@ -91,6 +91,7 @@ static const int gisb_offsets_bcm7445[] = {
 struct brcmstb_gisb_arb_device {
 	void __iomem	*base;
 	const int	*gisb_offsets;
+	bool		big_endian;
 	struct mutex	lock;
 	struct list_head next;
 	u32 valid_mask;
@@ -108,7 +109,10 @@ static u32 gisb_read(struct brcmstb_gisb_arb_device *gdev, int reg)
 	if (offset == -1)
 		return 1;
 
-	return ioread32(gdev->base + offset);
+	if (gdev->big_endian)
+		return ioread32be(gdev->base + offset);
+	else
+		return ioread32(gdev->base + offset);
 }
 
 static void gisb_write(struct brcmstb_gisb_arb_device *gdev, u32 val, int reg)
@@ -117,7 +121,11 @@ static void gisb_write(struct brcmstb_gisb_arb_device *gdev, u32 val, int reg)
 
 	if (offset == -1)
 		return;
-	iowrite32(val, gdev->base + reg);
+
+	if (gdev->big_endian)
+		iowrite32be(val, gdev->base + reg);
+	else
+		iowrite32(val, gdev->base + reg);
 }
 
 static ssize_t gisb_arb_get_timeout(struct device *dev,
@@ -296,6 +304,7 @@ static int __init brcmstb_gisb_arb_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 	gdev->gisb_offsets = of_id->data;
+	gdev->big_endian = of_device_is_big_endian(dn);
 
 	err = devm_request_irq(&pdev->dev, timeout_irq,
 				brcmstb_gisb_timeout_handler, 0, pdev->name,
diff --git a/drivers/bus/mvebu-mbus.c b/drivers/bus/mvebu-mbus.c
index fb9ec6221730..c43c3d2baf73 100644
--- a/drivers/bus/mvebu-mbus.c
+++ b/drivers/bus/mvebu-mbus.c
@@ -57,8 +57,8 @@
 #include <linux/of_address.h>
 #include <linux/debugfs.h>
 #include <linux/log2.h>
-#include <linux/syscore_ops.h>
 #include <linux/memblock.h>
+#include <linux/syscore_ops.h>
 
 /*
  * DDR target is the same on all platforms.
@@ -70,6 +70,7 @@
  */
 #define WIN_CTRL_OFF		0x0000
 #define   WIN_CTRL_ENABLE       BIT(0)
+/* Only on HW I/O coherency capable platforms */
 #define   WIN_CTRL_SYNCBARRIER  BIT(1)
 #define   WIN_CTRL_TGT_MASK     0xf0
 #define   WIN_CTRL_TGT_SHIFT    4
@@ -102,9 +103,7 @@
 
 /* Relative to mbusbridge_base */
 #define MBUS_BRIDGE_CTRL_OFF	0x0
-#define  MBUS_BRIDGE_SIZE_MASK  0xffff0000
 #define MBUS_BRIDGE_BASE_OFF	0x4
-#define  MBUS_BRIDGE_BASE_MASK  0xffff0000
 
 /* Maximum number of windows, for all known platforms */
 #define MBUS_WINS_MAX           20
@@ -154,13 +153,39 @@ struct mvebu_mbus_state {
 
 static struct mvebu_mbus_state mbus_state;
 
+/*
+ * We provide two variants of the mv_mbus_dram_info() function:
+ *
+ * - The normal one, where the described DRAM ranges may overlap with
+ *   the I/O windows, but for which the DRAM ranges are guaranteed to
+ *   have a power of two size. Such ranges are suitable for the DMA
+ *   masters that only DMA between the RAM and the device, which is
+ *   actually all devices except the crypto engines.
+ *
+ * - The 'nooverlap' one, where the described DRAM ranges are
+ *   guaranteed to not overlap with the I/O windows, but for which the
+ *   DRAM ranges will not have power of two sizes. They will only be
+ *   aligned on a 64 KB boundary, and have a size multiple of 64
+ *   KB. Such ranges are suitable for the DMA masters that DMA between
+ *   the crypto SRAM (which is mapped through an I/O window) and a
+ *   device. This is the case for the crypto engines.
+ */
+
 static struct mbus_dram_target_info mvebu_mbus_dram_info;
+static struct mbus_dram_target_info mvebu_mbus_dram_info_nooverlap;
+
 const struct mbus_dram_target_info *mv_mbus_dram_info(void)
 {
 	return &mvebu_mbus_dram_info;
 }
 EXPORT_SYMBOL_GPL(mv_mbus_dram_info);
 
+const struct mbus_dram_target_info *mv_mbus_dram_info_nooverlap(void)
+{
+	return &mvebu_mbus_dram_info_nooverlap;
+}
+EXPORT_SYMBOL_GPL(mv_mbus_dram_info_nooverlap);
+
 /* Checks whether the given window has remap capability */
 static bool mvebu_mbus_window_is_remappable(struct mvebu_mbus_state *mbus,
 					    const int win)
@@ -323,8 +348,9 @@ static int mvebu_mbus_setup_window(struct mvebu_mbus_state *mbus,
 	ctrl = ((size - 1) & WIN_CTRL_SIZE_MASK) |
 		(attr << WIN_CTRL_ATTR_SHIFT)    |
 		(target << WIN_CTRL_TGT_SHIFT)   |
-		WIN_CTRL_SYNCBARRIER             |
 		WIN_CTRL_ENABLE;
+	if (mbus->hw_io_coherency)
+		ctrl |= WIN_CTRL_SYNCBARRIER;
 
 	writel(base & WIN_BASE_LOW, addr + WIN_BASE_OFF);
 	writel(ctrl, addr + WIN_CTRL_OFF);
@@ -592,7 +618,7 @@ mvebu_mbus_find_bridge_hole(uint64_t *start, uint64_t *end)
 		 * This part of the memory is above 4 GB, so we don't
 		 * care for the MBus bridge hole.
 		 */
-		if (r->base >= 0x100000000)
+		if (r->base >= 0x100000000ULL)
 			continue;
 
 		/*
@@ -604,50 +630,33 @@ mvebu_mbus_find_bridge_hole(uint64_t *start, uint64_t *end)
 	}
 
 	*start = s;
-	*end = 0x100000000;
+	*end = 0x100000000ULL;
 }
 
+/*
+ * This function fills in the mvebu_mbus_dram_info_nooverlap data
+ * structure, by looking at the mvebu_mbus_dram_info data, and
+ * removing the parts of it that overlap with I/O windows.
+ */
 static void __init
-mvebu_mbus_default_setup_cpu_target(struct mvebu_mbus_state *mbus)
+mvebu_mbus_setup_cpu_target_nooverlap(struct mvebu_mbus_state *mbus)
 {
-	int i;
-	int cs;
 	uint64_t mbus_bridge_base, mbus_bridge_end;
-
-	mvebu_mbus_dram_info.mbus_dram_target_id = TARGET_DDR;
+	int cs_nooverlap = 0;
+	int i;
 
 	mvebu_mbus_find_bridge_hole(&mbus_bridge_base, &mbus_bridge_end);
 
-	for (i = 0, cs = 0; i < 4; i++) {
-		u64 base = readl(mbus->sdramwins_base + DDR_BASE_CS_OFF(i));
-		u64 size = readl(mbus->sdramwins_base + DDR_SIZE_CS_OFF(i));
-		u64 end;
+	for (i = 0; i < mvebu_mbus_dram_info.num_cs; i++) {
 		struct mbus_dram_window *w;
+		u64 base, size, end;
 
-		/* Ignore entries that are not enabled */
-		if (!(size & DDR_SIZE_ENABLED))
-			continue;
-
-		/*
-		 * Ignore entries whose base address is above 2^32,
-		 * since devices cannot DMA to such high addresses
-		 */
-		if (base & DDR_BASE_CS_HIGH_MASK)
-			continue;
-
-		base = base & DDR_BASE_CS_LOW_MASK;
-		size = (size | ~DDR_SIZE_MASK) + 1;
+		w = &mvebu_mbus_dram_info.cs[i];
+		base = w->base;
+		size = w->size;
 		end = base + size;
 
 		/*
-		 * Adjust base/size of the current CS to make sure it
-		 * doesn't overlap with the MBus bridge hole. This is
-		 * particularly important for devices that do DMA from
-		 * DRAM to a SRAM mapped in a MBus window, such as the
-		 * CESA cryptographic engine.
-		 */
-
-		/*
 		 * The CS is fully enclosed inside the MBus bridge
 		 * area, so ignore it.
 		 */
@@ -670,7 +679,7 @@ mvebu_mbus_default_setup_cpu_target(struct mvebu_mbus_state *mbus)
 		if (base < mbus_bridge_base && end > mbus_bridge_base)
 			size -= end - mbus_bridge_base;
 
-		w = &mvebu_mbus_dram_info.cs[cs++];
+		w = &mvebu_mbus_dram_info_nooverlap.cs[cs_nooverlap++];
 		w->cs_index = i;
 		w->mbus_attr = 0xf & ~(1 << i);
 		if (mbus->hw_io_coherency)
@@ -678,6 +687,42 @@ mvebu_mbus_default_setup_cpu_target(struct mvebu_mbus_state *mbus)
 		w->base = base;
 		w->size = size;
 	}
+
+	mvebu_mbus_dram_info_nooverlap.mbus_dram_target_id = TARGET_DDR;
+	mvebu_mbus_dram_info_nooverlap.num_cs = cs_nooverlap;
+}
+
+static void __init
+mvebu_mbus_default_setup_cpu_target(struct mvebu_mbus_state *mbus)
+{
+	int i;
+	int cs;
+
+	mvebu_mbus_dram_info.mbus_dram_target_id = TARGET_DDR;
+
+	for (i = 0, cs = 0; i < 4; i++) {
+		u32 base = readl(mbus->sdramwins_base + DDR_BASE_CS_OFF(i));
+		u32 size = readl(mbus->sdramwins_base + DDR_SIZE_CS_OFF(i));
+
+		/*
+		 * We only take care of entries for which the chip
+		 * select is enabled, and that don't have high base
+		 * address bits set (devices can only access the first
+		 * 32 bits of the memory).
+		 */
+		if ((size & DDR_SIZE_ENABLED) &&
+		    !(base & DDR_BASE_CS_HIGH_MASK)) {
+			struct mbus_dram_window *w;
+
+			w = &mvebu_mbus_dram_info.cs[cs++];
+			w->cs_index = i;
+			w->mbus_attr = 0xf & ~(1 << i);
+			if (mbus->hw_io_coherency)
+				w->mbus_attr |= ATTR_HW_COHERENCY;
+			w->base = base & DDR_BASE_CS_LOW_MASK;
+			w->size = (size | ~DDR_SIZE_MASK) + 1;
+		}
+	}
 	mvebu_mbus_dram_info.num_cs = cs;
 }
 
@@ -1035,6 +1080,7 @@ static int __init mvebu_mbus_common_init(struct mvebu_mbus_state *mbus,
 		mvebu_mbus_disable_window(mbus, win);
 
 	mbus->soc->setup_cpu_target(mbus);
+	mvebu_mbus_setup_cpu_target_nooverlap(mbus);
 
 	if (is_coherent)
 		writel(UNIT_SYNC_BARRIER_ALL,
diff --git a/drivers/bus/omap_l3_noc.c b/drivers/bus/omap_l3_noc.c
index ebee57d715d2..5012e3ad1225 100644
--- a/drivers/bus/omap_l3_noc.c
+++ b/drivers/bus/omap_l3_noc.c
@@ -301,7 +301,7 @@ static int omap_l3_probe(struct platform_device *pdev)
 	return ret;
 }
 
-#ifdef	CONFIG_PM
+#ifdef	CONFIG_PM_SLEEP
 
 /**
  * l3_resume_noirq() - resume function for l3_noc
@@ -347,7 +347,7 @@ static int l3_resume_noirq(struct device *dev)
 }
 
 static const struct dev_pm_ops l3_dev_pm_ops = {
-	.resume_noirq		= l3_resume_noirq,
+	SET_NOIRQ_SYSTEM_SLEEP_PM_OPS(NULL, l3_resume_noirq)
 };
 
 #define L3_DEV_PM_OPS (&l3_dev_pm_ops)
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index a4af8221751e..a043107da2af 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -590,14 +590,6 @@ config DEVPORT
 
 source "drivers/s390/char/Kconfig"
 
-config MSM_SMD_PKT
-	bool "Enable device interface for some SMD packet ports"
-	default n
-	depends on MSM_SMD
-	help
-	  Enables userspace clients to read and write to some packet SMD
-	  ports via device interface for MSM chipset.
-
 config TILE_SROM
 	bool "Character-device access via hypervisor to the Tilera SPI ROM"
 	depends on TILE
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index d06cde26031b..d8a7579300d2 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -9,7 +9,6 @@ obj-$(CONFIG_ATARI_DSP56K)	+= dsp56k.o
 obj-$(CONFIG_VIRTIO_CONSOLE)	+= virtio_console.o
 obj-$(CONFIG_RAW_DRIVER)	+= raw.o
 obj-$(CONFIG_SGI_SNSC)		+= snsc.o snsc_event.o
-obj-$(CONFIG_MSM_SMD_PKT)	+= msm_smd_pkt.o
 obj-$(CONFIG_MSPEC)		+= mspec.o
 obj-$(CONFIG_MMTIMER)		+= mmtimer.o
 obj-$(CONFIG_UV_MMTIMER)	+= uv_mmtimer.o
@@ -36,7 +35,6 @@ else
   obj-$(CONFIG_NVRAM)	+= nvram.o
 endif
 obj-$(CONFIG_TOSHIBA)		+= toshiba.o
-obj-$(CONFIG_I8K)		+= i8k.o
 obj-$(CONFIG_DS1620)		+= ds1620.o
 obj-$(CONFIG_HW_RANDOM)		+= hw_random/
 obj-$(CONFIG_PPDEV)		+= ppdev.o
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index 4734d02ca899..1341a94cc779 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -581,7 +581,7 @@ static inline int needs_ilk_vtd_wa(void)
 	/* Query intel_iommu to see if we need the workaround. Presumably that
 	 * was loaded first.
 	 */
-	if ((gpu_devid == PCI_DEVICE_ID_INTEL_IRONLAKE_M_HB ||
+	if ((gpu_devid == PCI_DEVICE_ID_INTEL_IRONLAKE_D_IG ||
 	     gpu_devid == PCI_DEVICE_ID_INTEL_IRONLAKE_M_IG) &&
 	     intel_iommu_gfx_mapped)
 		return 1;
diff --git a/drivers/char/hw_random/via-rng.c b/drivers/char/hw_random/via-rng.c
index a3bebef255ad..0c98a9d51a24 100644
--- a/drivers/char/hw_random/via-rng.c
+++ b/drivers/char/hw_random/via-rng.c
@@ -33,7 +33,7 @@
 #include <asm/io.h>
 #include <asm/msr.h>
 #include <asm/cpufeature.h>
-#include <asm/i387.h>
+#include <asm/fpu/api.h>
 
 
 
diff --git a/drivers/char/ipmi/ipmi_powernv.c b/drivers/char/ipmi/ipmi_powernv.c
index 8753b0f6a317..9b409c0f14f7 100644
--- a/drivers/char/ipmi/ipmi_powernv.c
+++ b/drivers/char/ipmi/ipmi_powernv.c
@@ -15,6 +15,8 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/interrupt.h>
 
 #include <asm/opal.h>
 
@@ -23,8 +25,7 @@ struct ipmi_smi_powernv {
 	u64			interface_id;
 	struct ipmi_device_id	ipmi_id;
 	ipmi_smi_t		intf;
-	u64			event;
-	struct notifier_block	event_nb;
+	unsigned int		irq;
 
 	/**
 	 * We assume that there can only be one outstanding request, so
@@ -197,15 +198,12 @@ static struct ipmi_smi_handlers ipmi_powernv_smi_handlers = {
 	.poll			= ipmi_powernv_poll,
 };
 
-static int ipmi_opal_event(struct notifier_block *nb,
-			  unsigned long events, void *change)
+static irqreturn_t ipmi_opal_event(int irq, void *data)
 {
-	struct ipmi_smi_powernv *smi = container_of(nb,
-					struct ipmi_smi_powernv, event_nb);
+	struct ipmi_smi_powernv *smi = data;
 
-	if (events & smi->event)
-		ipmi_powernv_recv(smi);
-	return 0;
+	ipmi_powernv_recv(smi);
+	return IRQ_HANDLED;
 }
 
 static int ipmi_powernv_probe(struct platform_device *pdev)
@@ -240,13 +238,16 @@ static int ipmi_powernv_probe(struct platform_device *pdev)
 		goto err_free;
 	}
 
-	ipmi->event = 1ull << prop;
-	ipmi->event_nb.notifier_call = ipmi_opal_event;
+	ipmi->irq = irq_of_parse_and_map(dev->of_node, 0);
+	if (!ipmi->irq) {
+		dev_info(dev, "Unable to map irq from device tree\n");
+		ipmi->irq = opal_event_request(prop);
+	}
 
-	rc = opal_notifier_register(&ipmi->event_nb);
-	if (rc) {
-		dev_warn(dev, "OPAL notifier registration failed (%d)\n", rc);
-		goto err_free;
+	if (request_irq(ipmi->irq, ipmi_opal_event, IRQ_TYPE_LEVEL_HIGH,
+				"opal-ipmi", ipmi)) {
+		dev_warn(dev, "Unable to request irq\n");
+		goto err_dispose;
 	}
 
 	ipmi->opal_msg = devm_kmalloc(dev,
@@ -271,7 +272,9 @@ static int ipmi_powernv_probe(struct platform_device *pdev)
 err_free_msg:
 	devm_kfree(dev, ipmi->opal_msg);
 err_unregister:
-	opal_notifier_unregister(&ipmi->event_nb);
+	free_irq(ipmi->irq, ipmi);
+err_dispose:
+	irq_dispose_mapping(ipmi->irq);
 err_free:
 	devm_kfree(dev, ipmi);
 	return rc;
@@ -282,7 +285,9 @@ 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);
+	free_irq(smi->irq, smi);
+	irq_dispose_mapping(smi->irq);
+
 	return 0;
 }
 
diff --git a/drivers/char/ipmi/ipmi_watchdog.c b/drivers/char/ipmi/ipmi_watchdog.c
index 37b8be7cba95..0ac3bd1a5497 100644
--- a/drivers/char/ipmi/ipmi_watchdog.c
+++ b/drivers/char/ipmi/ipmi_watchdog.c
@@ -208,7 +208,7 @@ static int set_param_timeout(const char *val, const struct kernel_param *kp)
 	return rv;
 }
 
-static struct kernel_param_ops param_ops_timeout = {
+static const struct kernel_param_ops param_ops_timeout = {
 	.set = set_param_timeout,
 	.get = param_get_int,
 };
@@ -270,14 +270,14 @@ static int set_param_wdog_ifnum(const char *val, const struct kernel_param *kp)
 	return 0;
 }
 
-static struct kernel_param_ops param_ops_wdog_ifnum = {
+static const struct kernel_param_ops param_ops_wdog_ifnum = {
 	.set = set_param_wdog_ifnum,
 	.get = param_get_int,
 };
 
 #define param_check_wdog_ifnum param_check_int
 
-static struct kernel_param_ops param_ops_str = {
+static const struct kernel_param_ops param_ops_str = {
 	.set = set_param_str,
 	.get = get_param_str,
 };
diff --git a/drivers/char/misc.c b/drivers/char/misc.c
index 9fd5a91e0d81..fdb0f9b3fe45 100644
--- a/drivers/char/misc.c
+++ b/drivers/char/misc.c
@@ -117,14 +117,14 @@ static int misc_open(struct inode * inode, struct file * file)
 	const struct file_operations *new_fops = NULL;
 
 	mutex_lock(&misc_mtx);
-	
+
 	list_for_each_entry(c, &misc_list, list) {
 		if (c->minor == minor) {
-			new_fops = fops_get(c->fops);		
+			new_fops = fops_get(c->fops);
 			break;
 		}
 	}
-		
+
 	if (!new_fops) {
 		mutex_unlock(&misc_mtx);
 		request_module("char-major-%d-%d", MISC_MAJOR, minor);
@@ -167,7 +167,7 @@ static const struct file_operations misc_fops = {
 /**
  *	misc_register	-	register a miscellaneous device
  *	@misc: device structure
- *	
+ *
  *	Register a miscellaneous device with the kernel. If the minor
  *	number is set to %MISC_DYNAMIC_MINOR a minor number is assigned
  *	and placed in the minor field of the structure. For other cases
@@ -181,17 +181,18 @@ static const struct file_operations misc_fops = {
  *	A zero is returned on success and a negative errno code for
  *	failure.
  */
- 
+
 int misc_register(struct miscdevice * misc)
 {
 	dev_t dev;
 	int err = 0;
+	bool is_dynamic = (misc->minor == MISC_DYNAMIC_MINOR);
 
 	INIT_LIST_HEAD(&misc->list);
 
 	mutex_lock(&misc_mtx);
 
-	if (misc->minor == MISC_DYNAMIC_MINOR) {
+	if (is_dynamic) {
 		int i = find_first_zero_bit(misc_minors, DYNAMIC_MINORS);
 		if (i >= DYNAMIC_MINORS) {
 			err = -EBUSY;
@@ -216,9 +217,13 @@ int misc_register(struct miscdevice * misc)
 		device_create_with_groups(misc_class, misc->parent, dev,
 					  misc, misc->groups, "%s", misc->name);
 	if (IS_ERR(misc->this_device)) {
-		int i = DYNAMIC_MINORS - misc->minor - 1;
-		if (i < DYNAMIC_MINORS && i >= 0)
-			clear_bit(i, misc_minors);
+		if (is_dynamic) {
+			int i = DYNAMIC_MINORS - misc->minor - 1;
+
+			if (i < DYNAMIC_MINORS && i >= 0)
+				clear_bit(i, misc_minors);
+			misc->minor = MISC_DYNAMIC_MINOR;
+		}
 		err = PTR_ERR(misc->this_device);
 		goto out;
 	}
diff --git a/drivers/char/msm_smd_pkt.c b/drivers/char/msm_smd_pkt.c
deleted file mode 100644
index ba82a06d9684..000000000000
--- a/drivers/char/msm_smd_pkt.c
+++ /dev/null
@@ -1,465 +0,0 @@
-/* Copyright (c) 2008-2010, Code Aurora Forum. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 and
- * only version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- * 02110-1301, USA.
- *
- */
-/*
- * SMD Packet Driver -- Provides userspace interface to SMD packet ports.
- */
-
-#include <linux/slab.h>
-#include <linux/cdev.h>
-#include <linux/module.h>
-#include <linux/fs.h>
-#include <linux/device.h>
-#include <linux/sched.h>
-#include <linux/mutex.h>
-#include <linux/delay.h>
-#include <linux/uaccess.h>
-#include <linux/workqueue.h>
-#include <linux/poll.h>
-
-#include <mach/msm_smd.h>
-
-#define NUM_SMD_PKT_PORTS 9
-#define DEVICE_NAME "smdpkt"
-#define MAX_BUF_SIZE 2048
-
-struct smd_pkt_dev {
-	struct cdev cdev;
-	struct device *devicep;
-
-	struct smd_channel *ch;
-	int open_count;
-	struct mutex ch_lock;
-	struct mutex rx_lock;
-	struct mutex tx_lock;
-	wait_queue_head_t ch_read_wait_queue;
-	wait_queue_head_t ch_opened_wait_queue;
-
-	int i;
-
-	unsigned char tx_buf[MAX_BUF_SIZE];
-	unsigned char rx_buf[MAX_BUF_SIZE];
-	int remote_open;
-
-} *smd_pkt_devp[NUM_SMD_PKT_PORTS];
-
-struct class *smd_pkt_classp;
-static dev_t smd_pkt_number;
-
-static int msm_smd_pkt_debug_enable;
-module_param_named(debug_enable, msm_smd_pkt_debug_enable,
-		   int, S_IRUGO | S_IWUSR | S_IWGRP);
-
-#ifdef DEBUG
-#define D_DUMP_BUFFER(prestr, cnt, buf) do {			\
-		int i;						\
-		if (msm_smd_pkt_debug_enable) {			\
-			pr_debug("%s", prestr);			\
-			for (i = 0; i < cnt; i++)		\
-				pr_debug("%.2x", buf[i]);	\
-			pr_debug("\n");				\
-		}						\
-	} while (0)
-#else
-#define D_DUMP_BUFFER(prestr, cnt, buf) do {} while (0)
-#endif
-
-#ifdef DEBUG
-#define DBG(x...) do {			\
-	if (msm_smd_pkt_debug_enable)	\
-		pr_debug(x);		\
-	} while (0)
-#else
-#define DBG(x...) do {} while (0)
-#endif
-
-static void check_and_wakeup_reader(struct smd_pkt_dev *smd_pkt_devp)
-{
-	int sz;
-
-	if (!smd_pkt_devp || !smd_pkt_devp->ch)
-		return;
-
-	sz = smd_cur_packet_size(smd_pkt_devp->ch);
-	if (sz == 0) {
-		DBG("no packet\n");
-		return;
-	}
-	if (sz > smd_read_avail(smd_pkt_devp->ch)) {
-		DBG("incomplete packet\n");
-		return;
-	}
-
-	DBG("waking up reader\n");
-	wake_up_interruptible(&smd_pkt_devp->ch_read_wait_queue);
-}
-
-static int smd_pkt_read(struct file *file, char __user *buf,
-			size_t count, loff_t *ppos)
-{
-	int r, bytes_read;
-	struct smd_pkt_dev *smd_pkt_devp;
-	struct smd_channel *chl;
-
-	DBG("read %d bytes\n", count);
-	if (count > MAX_BUF_SIZE)
-		return -EINVAL;
-
-	smd_pkt_devp = file->private_data;
-	if (!smd_pkt_devp || !smd_pkt_devp->ch)
-		return -EINVAL;
-
-	chl = smd_pkt_devp->ch;
-wait_for_packet:
-	r = wait_event_interruptible(smd_pkt_devp->ch_read_wait_queue,
-				     (smd_cur_packet_size(chl) > 0 &&
-				      smd_read_avail(chl) >=
-				      smd_cur_packet_size(chl)));
-
-	if (r < 0) {
-		if (r != -ERESTARTSYS)
-			pr_err("wait returned %d\n", r);
-		return r;
-	}
-
-	mutex_lock(&smd_pkt_devp->rx_lock);
-
-	bytes_read = smd_cur_packet_size(smd_pkt_devp->ch);
-	if (bytes_read == 0 ||
-	    bytes_read < smd_read_avail(smd_pkt_devp->ch)) {
-		mutex_unlock(&smd_pkt_devp->rx_lock);
-		DBG("Nothing to read\n");
-		goto wait_for_packet;
-	}
-
-	if (bytes_read > count) {
-		mutex_unlock(&smd_pkt_devp->rx_lock);
-		pr_info("packet size %d > buffer size %d", bytes_read, count);
-		return -EINVAL;
-	}
-
-	r = smd_read(smd_pkt_devp->ch, smd_pkt_devp->rx_buf, bytes_read);
-	if (r != bytes_read) {
-		mutex_unlock(&smd_pkt_devp->rx_lock);
-		pr_err("smd_read failed to read %d bytes: %d\n", bytes_read, r);
-		return -EIO;
-	}
-
-	D_DUMP_BUFFER("read: ", bytes_read, smd_pkt_devp->rx_buf);
-	r = copy_to_user(buf, smd_pkt_devp->rx_buf, bytes_read);
-	mutex_unlock(&smd_pkt_devp->rx_lock);
-	if (r) {
-		pr_err("copy_to_user failed %d\n", r);
-		return -EFAULT;
-	}
-
-	DBG("read complete %d bytes\n", bytes_read);
-	check_and_wakeup_reader(smd_pkt_devp);
-
-	return bytes_read;
-}
-
-static int smd_pkt_write(struct file *file, const char __user *buf,
-			 size_t count, loff_t *ppos)
-{
-	int r;
-	struct smd_pkt_dev *smd_pkt_devp;
-
-	if (count > MAX_BUF_SIZE)
-		return -EINVAL;
-
-	DBG("writing %d bytes\n", count);
-
-	smd_pkt_devp = file->private_data;
-	if (!smd_pkt_devp || !smd_pkt_devp->ch)
-		return -EINVAL;
-
-	mutex_lock(&smd_pkt_devp->tx_lock);
-	if (smd_write_avail(smd_pkt_devp->ch) < count) {
-		mutex_unlock(&smd_pkt_devp->tx_lock);
-		DBG("Not enough space to write\n");
-		return -ENOMEM;
-	}
-
-	D_DUMP_BUFFER("write: ", count, buf);
-	r = copy_from_user(smd_pkt_devp->tx_buf, buf, count);
-	if (r) {
-		mutex_unlock(&smd_pkt_devp->tx_lock);
-		pr_err("copy_from_user failed %d\n", r);
-		return -EFAULT;
-	}
-
-	r = smd_write(smd_pkt_devp->ch, smd_pkt_devp->tx_buf, count);
-	if (r != count) {
-		mutex_unlock(&smd_pkt_devp->tx_lock);
-		pr_err("smd_write failed to write %d bytes: %d.\n", count, r);
-		return -EIO;
-	}
-	mutex_unlock(&smd_pkt_devp->tx_lock);
-
-	DBG("wrote %d bytes\n", count);
-	return count;
-}
-
-static unsigned int smd_pkt_poll(struct file *file, poll_table *wait)
-{
-	struct smd_pkt_dev *smd_pkt_devp;
-	unsigned int mask = 0;
-
-	smd_pkt_devp = file->private_data;
-	if (!smd_pkt_devp)
-		return POLLERR;
-
-	DBG("poll waiting\n");
-	poll_wait(file, &smd_pkt_devp->ch_read_wait_queue, wait);
-	if (smd_read_avail(smd_pkt_devp->ch))
-		mask |= POLLIN | POLLRDNORM;
-
-	DBG("poll return\n");
-	return mask;
-}
-
-static void smd_pkt_ch_notify(void *priv, unsigned event)
-{
-	struct smd_pkt_dev *smd_pkt_devp = priv;
-
-	if (smd_pkt_devp->ch == 0)
-		return;
-
-	switch (event) {
-	case SMD_EVENT_DATA:
-		DBG("data\n");
-		check_and_wakeup_reader(smd_pkt_devp);
-		break;
-
-	case SMD_EVENT_OPEN:
-		DBG("remote open\n");
-		smd_pkt_devp->remote_open = 1;
-		wake_up_interruptible(&smd_pkt_devp->ch_opened_wait_queue);
-		break;
-
-	case SMD_EVENT_CLOSE:
-		smd_pkt_devp->remote_open = 0;
-		pr_info("remote closed\n");
-		break;
-
-	default:
-		pr_err("unknown event %d\n", event);
-		break;
-	}
-}
-
-static char *smd_pkt_dev_name[] = {
-	"smdcntl0",
-	"smdcntl1",
-	"smdcntl2",
-	"smdcntl3",
-	"smdcntl4",
-	"smdcntl5",
-	"smdcntl6",
-	"smdcntl7",
-	"smd22",
-};
-
-static char *smd_ch_name[] = {
-	"DATA5_CNTL",
-	"DATA6_CNTL",
-	"DATA7_CNTL",
-	"DATA8_CNTL",
-	"DATA9_CNTL",
-	"DATA12_CNTL",
-	"DATA13_CNTL",
-	"DATA14_CNTL",
-	"DATA22",
-};
-
-static int smd_pkt_open(struct inode *inode, struct file *file)
-{
-	int r = 0;
-	struct smd_pkt_dev *smd_pkt_devp;
-
-	smd_pkt_devp = container_of(inode->i_cdev, struct smd_pkt_dev, cdev);
-	if (!smd_pkt_devp)
-		return -EINVAL;
-
-	file->private_data = smd_pkt_devp;
-
-	mutex_lock(&smd_pkt_devp->ch_lock);
-	if (smd_pkt_devp->open_count == 0) {
-		r = smd_open(smd_ch_name[smd_pkt_devp->i],
-			     &smd_pkt_devp->ch, smd_pkt_devp,
-			     smd_pkt_ch_notify);
-		if (r < 0) {
-			pr_err("smd_open failed for %s, %d\n",
-			       smd_ch_name[smd_pkt_devp->i], r);
-			goto out;
-		}
-
-		r = wait_event_interruptible_timeout(
-				smd_pkt_devp->ch_opened_wait_queue,
-				smd_pkt_devp->remote_open,
-				msecs_to_jiffies(2 * HZ));
-		if (r == 0)
-			r = -ETIMEDOUT;
-
-		if (r < 0) {
-			pr_err("wait returned %d\n", r);
-			smd_close(smd_pkt_devp->ch);
-			smd_pkt_devp->ch = 0;
-		} else {
-			smd_pkt_devp->open_count++;
-			r = 0;
-		}
-	}
-out:
-	mutex_unlock(&smd_pkt_devp->ch_lock);
-	return r;
-}
-
-static int smd_pkt_release(struct inode *inode, struct file *file)
-{
-	int r = 0;
-	struct smd_pkt_dev *smd_pkt_devp = file->private_data;
-
-	if (!smd_pkt_devp)
-		return -EINVAL;
-
-	mutex_lock(&smd_pkt_devp->ch_lock);
-	if (--smd_pkt_devp->open_count == 0) {
-		r = smd_close(smd_pkt_devp->ch);
-		smd_pkt_devp->ch = 0;
-	}
-	mutex_unlock(&smd_pkt_devp->ch_lock);
-
-	return r;
-}
-
-static const struct file_operations smd_pkt_fops = {
-	.owner = THIS_MODULE,
-	.open = smd_pkt_open,
-	.release = smd_pkt_release,
-	.read = smd_pkt_read,
-	.write = smd_pkt_write,
-	.poll = smd_pkt_poll,
-};
-
-static int __init smd_pkt_init(void)
-{
-	int i;
-	int r;
-
-	r = alloc_chrdev_region(&smd_pkt_number, 0,
-				NUM_SMD_PKT_PORTS, DEVICE_NAME);
-	if (r) {
-		pr_err("alloc_chrdev_region() failed %d\n", r);
-		return r;
-	}
-
-	smd_pkt_classp = class_create(THIS_MODULE, DEVICE_NAME);
-	if (IS_ERR(smd_pkt_classp)) {
-		r = PTR_ERR(smd_pkt_classp);
-		pr_err("class_create() failed %d\n", r);
-		goto unreg_chardev;
-	}
-
-	for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
-		smd_pkt_devp[i] = kzalloc(sizeof(struct smd_pkt_dev),
-					  GFP_KERNEL);
-		if (!smd_pkt_devp[i]) {
-			pr_err("kmalloc() failed\n");
-			goto clean_cdevs;
-		}
-
-		smd_pkt_devp[i]->i = i;
-
-		init_waitqueue_head(&smd_pkt_devp[i]->ch_read_wait_queue);
-		smd_pkt_devp[i]->remote_open = 0;
-		init_waitqueue_head(&smd_pkt_devp[i]->ch_opened_wait_queue);
-
-		mutex_init(&smd_pkt_devp[i]->ch_lock);
-		mutex_init(&smd_pkt_devp[i]->rx_lock);
-		mutex_init(&smd_pkt_devp[i]->tx_lock);
-
-		cdev_init(&smd_pkt_devp[i]->cdev, &smd_pkt_fops);
-		smd_pkt_devp[i]->cdev.owner = THIS_MODULE;
-
-		r = cdev_add(&smd_pkt_devp[i]->cdev,
-			     (smd_pkt_number + i), 1);
-		if (r) {
-			pr_err("cdev_add() failed %d\n", r);
-			kfree(smd_pkt_devp[i]);
-			goto clean_cdevs;
-		}
-
-		smd_pkt_devp[i]->devicep =
-			device_create(smd_pkt_classp, NULL,
-				      (smd_pkt_number + i), NULL,
-				      smd_pkt_dev_name[i]);
-		if (IS_ERR(smd_pkt_devp[i]->devicep)) {
-			r = PTR_ERR(smd_pkt_devp[i]->devicep);
-			pr_err("device_create() failed %d\n", r);
-			cdev_del(&smd_pkt_devp[i]->cdev);
-			kfree(smd_pkt_devp[i]);
-			goto clean_cdevs;
-		}
-
-	}
-
-	pr_info("SMD Packet Port Driver Initialized.\n");
-	return 0;
-
-clean_cdevs:
-	if (i > 0) {
-		while (--i >= 0) {
-			mutex_destroy(&smd_pkt_devp[i]->ch_lock);
-			mutex_destroy(&smd_pkt_devp[i]->rx_lock);
-			mutex_destroy(&smd_pkt_devp[i]->tx_lock);
-			cdev_del(&smd_pkt_devp[i]->cdev);
-			kfree(smd_pkt_devp[i]);
-			device_destroy(smd_pkt_classp,
-				       MKDEV(MAJOR(smd_pkt_number), i));
-		}
-	}
-
-	class_destroy(smd_pkt_classp);
-unreg_chardev:
-	unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
-	return r;
-}
-module_init(smd_pkt_init);
-
-static void __exit smd_pkt_cleanup(void)
-{
-	int i;
-
-	for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
-		mutex_destroy(&smd_pkt_devp[i]->ch_lock);
-		mutex_destroy(&smd_pkt_devp[i]->rx_lock);
-		mutex_destroy(&smd_pkt_devp[i]->tx_lock);
-		cdev_del(&smd_pkt_devp[i]->cdev);
-		kfree(smd_pkt_devp[i]);
-		device_destroy(smd_pkt_classp,
-			       MKDEV(MAJOR(smd_pkt_number), i));
-	}
-
-	class_destroy(smd_pkt_classp);
-	unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
-}
-module_exit(smd_pkt_cleanup);
-
-MODULE_DESCRIPTION("MSM Shared Memory Packet Port");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/char/pcmcia/cm4040_cs.c b/drivers/char/pcmcia/cm4040_cs.c
index 8dd48a2be911..fc061f7c2bd1 100644
--- a/drivers/char/pcmcia/cm4040_cs.c
+++ b/drivers/char/pcmcia/cm4040_cs.c
@@ -532,9 +532,8 @@ static int reader_config(struct pcmcia_device *link, int devno)
 
 	fail_rc = pcmcia_enable_device(link);
 	if (fail_rc != 0) {
-		dev_printk(KERN_INFO, &link->dev,
-			   "pcmcia_enable_device failed 0x%x\n",
-			   fail_rc);
+		dev_info(&link->dev, "pcmcia_enable_device failed 0x%x\n",
+			 fail_rc);
 		goto cs_release;
 	}
 
diff --git a/drivers/char/pcmcia/synclink_cs.c b/drivers/char/pcmcia/synclink_cs.c
index 0ea9986059af..7680d5213ff8 100644
--- a/drivers/char/pcmcia/synclink_cs.c
+++ b/drivers/char/pcmcia/synclink_cs.c
@@ -437,7 +437,7 @@ static int mgslpc_device_count = 0;
  * .text section address and breakpoint on module load.
  * This is useful for use with gdb and add-symbol-file command.
  */
-static bool break_on_load=0;
+static bool break_on_load;
 
 /*
  * Driver major number, defaults to zero to get auto
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 9cd6968e2f92..d0da5d852d41 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -409,6 +409,9 @@ static DECLARE_WAIT_QUEUE_HEAD(random_write_wait);
 static DECLARE_WAIT_QUEUE_HEAD(urandom_init_wait);
 static struct fasync_struct *fasync;
 
+static DEFINE_SPINLOCK(random_ready_list_lock);
+static LIST_HEAD(random_ready_list);
+
 /**********************************************************************
  *
  * OS independent entropy store.   Here are the functions which handle
@@ -589,6 +592,22 @@ static void fast_mix(struct fast_pool *f)
 	f->count++;
 }
 
+static void process_random_ready_list(void)
+{
+	unsigned long flags;
+	struct random_ready_callback *rdy, *tmp;
+
+	spin_lock_irqsave(&random_ready_list_lock, flags);
+	list_for_each_entry_safe(rdy, tmp, &random_ready_list, list) {
+		struct module *owner = rdy->owner;
+
+		list_del_init(&rdy->list);
+		rdy->func(rdy);
+		module_put(owner);
+	}
+	spin_unlock_irqrestore(&random_ready_list_lock, flags);
+}
+
 /*
  * Credit (or debit) the entropy store with n bits of entropy.
  * Use credit_entropy_bits_safe() if the value comes from userspace
@@ -660,7 +679,8 @@ retry:
 		r->entropy_total = 0;
 		if (r == &nonblocking_pool) {
 			prandom_reseed_late();
-			wake_up_interruptible(&urandom_init_wait);
+			process_random_ready_list();
+			wake_up_all(&urandom_init_wait);
 			pr_notice("random: %s pool is initialized\n", r->name);
 		}
 	}
@@ -1245,6 +1265,64 @@ void get_random_bytes(void *buf, int nbytes)
 EXPORT_SYMBOL(get_random_bytes);
 
 /*
+ * Add a callback function that will be invoked when the nonblocking
+ * pool is initialised.
+ *
+ * returns: 0 if callback is successfully added
+ *	    -EALREADY if pool is already initialised (callback not called)
+ *	    -ENOENT if module for callback is not alive
+ */
+int add_random_ready_callback(struct random_ready_callback *rdy)
+{
+	struct module *owner;
+	unsigned long flags;
+	int err = -EALREADY;
+
+	if (likely(nonblocking_pool.initialized))
+		return err;
+
+	owner = rdy->owner;
+	if (!try_module_get(owner))
+		return -ENOENT;
+
+	spin_lock_irqsave(&random_ready_list_lock, flags);
+	if (nonblocking_pool.initialized)
+		goto out;
+
+	owner = NULL;
+
+	list_add(&rdy->list, &random_ready_list);
+	err = 0;
+
+out:
+	spin_unlock_irqrestore(&random_ready_list_lock, flags);
+
+	module_put(owner);
+
+	return err;
+}
+EXPORT_SYMBOL(add_random_ready_callback);
+
+/*
+ * Delete a previously registered readiness callback function.
+ */
+void del_random_ready_callback(struct random_ready_callback *rdy)
+{
+	unsigned long flags;
+	struct module *owner = NULL;
+
+	spin_lock_irqsave(&random_ready_list_lock, flags);
+	if (!list_empty(&rdy->list)) {
+		list_del_init(&rdy->list);
+		owner = rdy->owner;
+	}
+	spin_unlock_irqrestore(&random_ready_list_lock, flags);
+
+	module_put(owner);
+}
+EXPORT_SYMBOL(del_random_ready_callback);
+
+/*
  * This function will use the architecture-specific hardware random
  * number generator if it is available.  The arch-specific hw RNG will
  * almost certainly be faster than what we can do in software, but it
diff --git a/drivers/char/raw.c b/drivers/char/raw.c
index 5fc291c6157e..60316fbaf295 100644
--- a/drivers/char/raw.c
+++ b/drivers/char/raw.c
@@ -12,6 +12,7 @@
 #include <linux/fs.h>
 #include <linux/major.h>
 #include <linux/blkdev.h>
+#include <linux/backing-dev.h>
 #include <linux/module.h>
 #include <linux/raw.h>
 #include <linux/capability.h>
diff --git a/drivers/char/snsc.c b/drivers/char/snsc.c
index 8bab59292a0d..8a80ead8d316 100644
--- a/drivers/char/snsc.c
+++ b/drivers/char/snsc.c
@@ -198,7 +198,7 @@ scdrv_read(struct file *file, char __user *buf, size_t count, loff_t *f_pos)
 		add_wait_queue(&sd->sd_rq, &wait);
 		spin_unlock_irqrestore(&sd->sd_rlock, flags);
 
-		schedule_timeout(SCDRV_TIMEOUT);
+		schedule_timeout(msecs_to_jiffies(SCDRV_TIMEOUT));
 
 		remove_wait_queue(&sd->sd_rq, &wait);
 		if (signal_pending(current)) {
@@ -294,7 +294,7 @@ scdrv_write(struct file *file, const char __user *buf,
 		add_wait_queue(&sd->sd_wq, &wait);
 		spin_unlock_irqrestore(&sd->sd_wlock, flags);
 
-		schedule_timeout(SCDRV_TIMEOUT);
+		schedule_timeout(msecs_to_jiffies(SCDRV_TIMEOUT));
 
 		remove_wait_queue(&sd->sd_wq, &wait);
 		if (signal_pending(current)) {
diff --git a/drivers/char/tpm/tpm_crb.c b/drivers/char/tpm/tpm_crb.c
index b26ceee3585e..44f9d20c19ac 100644
--- a/drivers/char/tpm/tpm_crb.c
+++ b/drivers/char/tpm/tpm_crb.c
@@ -267,7 +267,7 @@ static int crb_acpi_add(struct acpi_device *device)
 
 	memcpy_fromio(&pa, &priv->cca->cmd_pa, 8);
 	pa = le64_to_cpu(pa);
-	priv->cmd = devm_ioremap_nocache(dev, le64_to_cpu(pa),
+	priv->cmd = devm_ioremap_nocache(dev, pa,
 					 ioread32(&priv->cca->cmd_size));
 	if (!priv->cmd) {
 		dev_err(dev, "ioremap of the command buffer failed\n");
@@ -276,7 +276,7 @@ static int crb_acpi_add(struct acpi_device *device)
 
 	memcpy_fromio(&pa, &priv->cca->rsp_pa, 8);
 	pa = le64_to_cpu(pa);
-	priv->rsp = devm_ioremap_nocache(dev, le64_to_cpu(pa),
+	priv->rsp = devm_ioremap_nocache(dev, pa,
 					 ioread32(&priv->cca->rsp_size));
 	if (!priv->rsp) {
 		dev_err(dev, "ioremap of the response buffer failed\n");
diff --git a/drivers/char/tpm/tpm_ibmvtpm.c b/drivers/char/tpm/tpm_ibmvtpm.c
index 42ffa5e7a1e0..27ebf9511cb4 100644
--- a/drivers/char/tpm/tpm_ibmvtpm.c
+++ b/drivers/char/tpm/tpm_ibmvtpm.c
@@ -578,6 +578,9 @@ static int tpm_ibmvtpm_probe(struct vio_dev *vio_dev,
 		goto cleanup;
 	}
 
+	ibmvtpm->dev = dev;
+	ibmvtpm->vdev = vio_dev;
+
 	crq_q = &ibmvtpm->crq_queue;
 	crq_q->crq_addr = (struct ibmvtpm_crq *)get_zeroed_page(GFP_KERNEL);
 	if (!crq_q->crq_addr) {
@@ -622,8 +625,6 @@ static int tpm_ibmvtpm_probe(struct vio_dev *vio_dev,
 
 	crq_q->index = 0;
 
-	ibmvtpm->dev = dev;
-	ibmvtpm->vdev = vio_dev;
 	TPM_VPRIV(chip) = (void *)ibmvtpm;
 
 	spin_lock_init(&ibmvtpm->rtce_lock);
diff --git a/drivers/char/tpm/tpm_of.c b/drivers/char/tpm/tpm_of.c
index c002d1bd9caf..eebe6256918f 100644
--- a/drivers/char/tpm/tpm_of.c
+++ b/drivers/char/tpm/tpm_of.c
@@ -49,7 +49,7 @@ int read_log(struct tpm_bios_log *log)
 
 	basep = of_get_property(np, "linux,sml-base", NULL);
 	if (basep == NULL) {
-		pr_err(KERN_ERR "%s: ERROR - SML not found\n", __func__);
+		pr_err("%s: ERROR - SML not found\n", __func__);
 		goto cleanup_eio;
 	}
 
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index 50754d203310..d2406fe25533 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -1492,8 +1492,8 @@ static int add_port(struct ports_device *portdev, u32 id)
 		 * Finally, create the debugfs file that we can use to
 		 * inspect a port's state at any time
 		 */
-		sprintf(debugfs_name, "vport%up%u",
-			port->portdev->vdev->index, id);
+		snprintf(debugfs_name, sizeof(debugfs_name), "vport%up%u",
+			 port->portdev->vdev->index, id);
 		port->debugfs_file = debugfs_create_file(debugfs_name, 0444,
 							 pdrvdata.debugfs_dir,
 							 port,
diff --git a/drivers/char/xilinx_hwicap/buffer_icap.c b/drivers/char/xilinx_hwicap/buffer_icap.c
index 05d897764f02..53c3882e4981 100644
--- a/drivers/char/xilinx_hwicap/buffer_icap.c
+++ b/drivers/char/xilinx_hwicap/buffer_icap.c
@@ -270,7 +270,7 @@ int buffer_icap_set_configuration(struct hwicap_drvdata *drvdata, u32 *data,
 	int status;
 	s32 buffer_count = 0;
 	s32 num_writes = 0;
-	bool dirty = 0;
+	bool dirty = false;
 	u32 i;
 	void __iomem *base_address = drvdata->base_address;
 
@@ -279,7 +279,7 @@ int buffer_icap_set_configuration(struct hwicap_drvdata *drvdata, u32 *data,
 
 		/* Copy data to bram */
 		buffer_icap_set_bram(base_address, buffer_count, data[i]);
-		dirty = 1;
+		dirty = true;
 
 		if (buffer_count < XHI_MAX_BUFFER_INTS - 1) {
 			buffer_count++;
@@ -299,7 +299,7 @@ int buffer_icap_set_configuration(struct hwicap_drvdata *drvdata, u32 *data,
 
 		buffer_count = 0;
 		num_writes++;
-		dirty = 0;
+		dirty = false;
 	}
 
 	/* Write unwritten data to ICAP */
diff --git a/drivers/char/xillybus/Kconfig b/drivers/char/xillybus/Kconfig
index b53bdf12da0d..b302684d86c1 100644
--- a/drivers/char/xillybus/Kconfig
+++ b/drivers/char/xillybus/Kconfig
@@ -24,7 +24,7 @@ config XILLYBUS_PCIE
 
 config XILLYBUS_OF
 	tristate "Xillybus over Device Tree"
-	depends on OF_ADDRESS && OF_IRQ
+	depends on OF_ADDRESS && OF_IRQ && HAS_DMA
 	help
 	  Set to M if you want Xillybus to find its resources from the
 	  Open Firmware Flattened Device Tree. If the target is an embedded
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 9897f353bf1a..42f7120ca9ce 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -78,6 +78,23 @@ config COMMON_CLK_SI570
 	  This driver supports Silicon Labs 570/571/598/599 programmable
 	  clock generators.
 
+config COMMON_CLK_CDCE925
+	tristate "Clock driver for TI CDCE925 devices"
+	depends on I2C
+	depends on OF
+	select REGMAP_I2C
+	help
+	---help---
+	  This driver supports the TI CDCE925 programmable clock synthesizer.
+	  The chip contains two PLLs with spread-spectrum clocking support and
+	  five output dividers. The driver only supports the following setup,
+	  and uses a fixed setting for the output muxes.
+	  Y1 is derived from the input clock
+	  Y2 and Y3 derive from PLL1
+	  Y4 and Y5 derive from PLL2
+	  Given a target output frequency, the driver will set the PLL and
+	  divider to best approximate the desired output.
+
 config COMMON_CLK_S2MPS11
 	tristate "Clock driver for S2MPS1X/S5M8767 MFD"
 	depends on MFD_SEC_CORE
@@ -150,11 +167,13 @@ config COMMON_CLK_CDCE706
 	---help---
 	  This driver supports TI CDCE706 programmable 3-PLL clock synthesizer.
 
+source "drivers/clk/bcm/Kconfig"
+source "drivers/clk/hisilicon/Kconfig"
 source "drivers/clk/qcom/Kconfig"
 
 endmenu
 
-source "drivers/clk/bcm/Kconfig"
 source "drivers/clk/mvebu/Kconfig"
 
 source "drivers/clk/samsung/Kconfig"
+source "drivers/clk/tegra/Kconfig"
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 3d00c25382c5..c4cf075a2320 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -24,7 +24,7 @@ obj-$(CONFIG_COMMON_CLK_CDCE706)	+= clk-cdce706.o
 obj-$(CONFIG_ARCH_CLPS711X)		+= clk-clps711x.o
 obj-$(CONFIG_ARCH_EFM32)		+= clk-efm32gg.o
 obj-$(CONFIG_ARCH_HIGHBANK)		+= clk-highbank.o
-obj-$(CONFIG_MACH_LOONGSON1)		+= clk-ls1x.o
+obj-$(CONFIG_MACH_LOONGSON32)		+= clk-ls1x.o
 obj-$(CONFIG_COMMON_CLK_MAX_GEN)	+= clk-max-gen.o
 obj-$(CONFIG_COMMON_CLK_MAX77686)	+= clk-max77686.o
 obj-$(CONFIG_COMMON_CLK_MAX77802)	+= clk-max77802.o
@@ -38,6 +38,8 @@ obj-$(CONFIG_COMMON_CLK_RK808)		+= clk-rk808.o
 obj-$(CONFIG_COMMON_CLK_S2MPS11)	+= clk-s2mps11.o
 obj-$(CONFIG_COMMON_CLK_SI5351)		+= clk-si5351.o
 obj-$(CONFIG_COMMON_CLK_SI570)		+= clk-si570.o
+obj-$(CONFIG_COMMON_CLK_CDCE925)	+= clk-cdce925.o
+obj-$(CONFIG_ARCH_STM32)		+= clk-stm32f4.o
 obj-$(CONFIG_CLK_TWL6040)		+= clk-twl6040.o
 obj-$(CONFIG_ARCH_U300)			+= clk-u300.o
 obj-$(CONFIG_ARCH_VT8500)		+= clk-vt8500.o
@@ -45,17 +47,20 @@ obj-$(CONFIG_COMMON_CLK_WM831X)		+= clk-wm831x.o
 obj-$(CONFIG_COMMON_CLK_XGENE)		+= clk-xgene.o
 obj-$(CONFIG_COMMON_CLK_PWM)		+= clk-pwm.o
 obj-$(CONFIG_COMMON_CLK_AT91)		+= at91/
-obj-$(CONFIG_ARCH_BCM_MOBILE)		+= bcm/
+obj-$(CONFIG_ARCH_BCM)			+= bcm/
 obj-$(CONFIG_ARCH_BERLIN)		+= berlin/
-obj-$(CONFIG_ARCH_HI3xxx)		+= hisilicon/
-obj-$(CONFIG_ARCH_HIP04)		+= hisilicon/
-obj-$(CONFIG_ARCH_HIX5HD2)		+= hisilicon/
+obj-$(CONFIG_ARCH_HISI)			+= hisilicon/
+obj-$(CONFIG_ARCH_MXC)			+= imx/
+obj-$(CONFIG_MACH_INGENIC)		+= ingenic/
 obj-$(CONFIG_COMMON_CLK_KEYSTONE)	+= keystone/
+obj-$(CONFIG_ARCH_MEDIATEK)		+= mediatek/
 ifeq ($(CONFIG_COMMON_CLK), y)
 obj-$(CONFIG_ARCH_MMP)			+= mmp/
 endif
 obj-$(CONFIG_PLAT_ORION)		+= mvebu/
+obj-$(CONFIG_ARCH_MESON)		+= meson/
 obj-$(CONFIG_ARCH_MXS)			+= mxs/
+obj-$(CONFIG_ARCH_LPC18XX)		+= nxp/
 obj-$(CONFIG_MACH_PISTACHIO)		+= pistachio/
 obj-$(CONFIG_COMMON_CLK_PXA)		+= pxa/
 obj-$(CONFIG_COMMON_CLK_QCOM)		+= qcom/
@@ -72,4 +77,6 @@ obj-$(CONFIG_ARCH_OMAP2PLUS)		+= ti/
 obj-$(CONFIG_ARCH_U8500)		+= ux500/
 obj-$(CONFIG_COMMON_CLK_VERSATILE)	+= versatile/
 obj-$(CONFIG_X86)			+= x86/
+obj-$(CONFIG_ARCH_ZX)			+= zte/
 obj-$(CONFIG_ARCH_ZYNQ)			+= zynq/
+obj-$(CONFIG_H8300)		+= h8300/
diff --git a/drivers/clk/at91/clk-h32mx.c b/drivers/clk/at91/clk-h32mx.c
index 152dcb3f7b5f..61566bcefa53 100644
--- a/drivers/clk/at91/clk-h32mx.c
+++ b/drivers/clk/at91/clk-h32mx.c
@@ -116,8 +116,10 @@ void __init of_sama5d4_clk_h32mx_setup(struct device_node *np,
 	h32mxclk->pmc = pmc;
 
 	clk = clk_register(NULL, &h32mxclk->hw);
-	if (!clk)
+	if (!clk) {
+		kfree(h32mxclk);
 		return;
+	}
 
 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
 }
diff --git a/drivers/clk/at91/clk-main.c b/drivers/clk/at91/clk-main.c
index 59fa3cc96c9e..27dfa965cfed 100644
--- a/drivers/clk/at91/clk-main.c
+++ b/drivers/clk/at91/clk-main.c
@@ -171,8 +171,10 @@ at91_clk_register_main_osc(struct at91_pmc *pmc,
 	irq_set_status_flags(osc->irq, IRQ_NOAUTOEN);
 	ret = request_irq(osc->irq, clk_main_osc_irq_handler,
 			  IRQF_TRIGGER_HIGH, name, osc);
-	if (ret)
+	if (ret) {
+		kfree(osc);
 		return ERR_PTR(ret);
+	}
 
 	if (bypass)
 		pmc_write(pmc, AT91_CKGR_MOR,
@@ -614,7 +616,7 @@ void __init of_at91sam9x5_clk_main_setup(struct device_node *np,
 	const char *name = np->name;
 	int i;
 
-	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	num_parents = of_clk_get_parent_count(np);
 	if (num_parents <= 0 || num_parents > 2)
 		return;
 
diff --git a/drivers/clk/at91/clk-master.c b/drivers/clk/at91/clk-master.c
index c1af80bcdf20..5b3ded5205a2 100644
--- a/drivers/clk/at91/clk-master.c
+++ b/drivers/clk/at91/clk-master.c
@@ -165,12 +165,16 @@ at91_clk_register_master(struct at91_pmc *pmc, unsigned int irq,
 	irq_set_status_flags(master->irq, IRQ_NOAUTOEN);
 	ret = request_irq(master->irq, clk_master_irq_handler,
 			  IRQF_TRIGGER_HIGH, "clk-master", master);
-	if (ret)
+	if (ret) {
+		kfree(master);
 		return ERR_PTR(ret);
+	}
 
 	clk = clk_register(NULL, &master->hw);
-	if (IS_ERR(clk))
+	if (IS_ERR(clk)) {
+		free_irq(master->irq, master);
 		kfree(master);
+	}
 
 	return clk;
 }
@@ -224,7 +228,7 @@ of_at91_clk_master_setup(struct device_node *np, struct at91_pmc *pmc,
 	const char *name = np->name;
 	struct clk_master_characteristics *characteristics;
 
-	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	num_parents = of_clk_get_parent_count(np);
 	if (num_parents <= 0 || num_parents > MASTER_SOURCE_MAX)
 		return;
 
diff --git a/drivers/clk/at91/clk-peripheral.c b/drivers/clk/at91/clk-peripheral.c
index 597fed423d7d..df2c1afa52b4 100644
--- a/drivers/clk/at91/clk-peripheral.c
+++ b/drivers/clk/at91/clk-peripheral.c
@@ -29,7 +29,7 @@
 #define PERIPHERAL_RSHIFT_MASK	0x3
 #define PERIPHERAL_RSHIFT(val)	(((val) >> 16) & PERIPHERAL_RSHIFT_MASK)
 
-#define PERIPHERAL_MAX_SHIFT	4
+#define PERIPHERAL_MAX_SHIFT	3
 
 struct clk_peripheral {
 	struct clk_hw hw;
@@ -242,7 +242,7 @@ static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw,
 		return *parent_rate;
 
 	if (periph->range.max) {
-		for (; shift < PERIPHERAL_MAX_SHIFT; shift++) {
+		for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 			cur_rate = *parent_rate >> shift;
 			if (cur_rate <= periph->range.max)
 				break;
@@ -254,7 +254,7 @@ static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw,
 
 	best_diff = cur_rate - rate;
 	best_rate = cur_rate;
-	for (; shift < PERIPHERAL_MAX_SHIFT; shift++) {
+	for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 		cur_rate = *parent_rate >> shift;
 		if (cur_rate < rate)
 			cur_diff = rate - cur_rate;
@@ -289,7 +289,7 @@ static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw,
 	if (periph->range.max && rate > periph->range.max)
 		return -EINVAL;
 
-	for (shift = 0; shift < PERIPHERAL_MAX_SHIFT; shift++) {
+	for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 		if (parent_rate >> shift == rate) {
 			periph->auto_div = false;
 			periph->div = shift;
diff --git a/drivers/clk/at91/clk-pll.c b/drivers/clk/at91/clk-pll.c
index 6ec79dbc0840..18b60f4895a6 100644
--- a/drivers/clk/at91/clk-pll.c
+++ b/drivers/clk/at91/clk-pll.c
@@ -173,8 +173,7 @@ static long clk_pll_get_best_div_mul(struct clk_pll *pll, unsigned long rate,
 	int i = 0;
 
 	/* Check if parent_rate is a valid input rate */
-	if (parent_rate < characteristics->input.min ||
-	    parent_rate > characteristics->input.max)
+	if (parent_rate < characteristics->input.min)
 		return -ERANGE;
 
 	/*
@@ -187,6 +186,15 @@ static long clk_pll_get_best_div_mul(struct clk_pll *pll, unsigned long rate,
 	if (!mindiv)
 		mindiv = 1;
 
+	if (parent_rate > characteristics->input.max) {
+		tmpdiv = DIV_ROUND_UP(parent_rate, characteristics->input.max);
+		if (tmpdiv > PLL_DIV_MAX)
+			return -ERANGE;
+
+		if (tmpdiv > mindiv)
+			mindiv = tmpdiv;
+	}
+
 	/*
 	 * Calculate the maximum divider which is limited by PLL register
 	 * layout (limited by the MUL or DIV field size).
@@ -338,12 +346,16 @@ at91_clk_register_pll(struct at91_pmc *pmc, unsigned int irq, const char *name,
 	irq_set_status_flags(pll->irq, IRQ_NOAUTOEN);
 	ret = request_irq(pll->irq, clk_pll_irq_handler, IRQF_TRIGGER_HIGH,
 			  id ? "clk-pllb" : "clk-plla", pll);
-	if (ret)
+	if (ret) {
+		kfree(pll);
 		return ERR_PTR(ret);
+	}
 
 	clk = clk_register(NULL, &pll->hw);
-	if (IS_ERR(clk))
+	if (IS_ERR(clk)) {
+		free_irq(pll->irq, pll);
 		kfree(pll);
+	}
 
 	return clk;
 }
diff --git a/drivers/clk/at91/clk-programmable.c b/drivers/clk/at91/clk-programmable.c
index 86c8a073dcc3..8c86c0f7847a 100644
--- a/drivers/clk/at91/clk-programmable.c
+++ b/drivers/clk/at91/clk-programmable.c
@@ -237,7 +237,7 @@ of_at91_clk_prog_setup(struct device_node *np, struct at91_pmc *pmc,
 	const char *name;
 	struct device_node *progclknp;
 
-	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	num_parents = of_clk_get_parent_count(np);
 	if (num_parents <= 0 || num_parents > PROG_SOURCE_MAX)
 		return;
 
diff --git a/drivers/clk/at91/clk-slow.c b/drivers/clk/at91/clk-slow.c
index 2f13bd5246b5..98a84a865fe1 100644
--- a/drivers/clk/at91/clk-slow.c
+++ b/drivers/clk/at91/clk-slow.c
@@ -373,7 +373,7 @@ void __init of_at91sam9x5_clk_slow_setup(struct device_node *np,
 	const char *name = np->name;
 	int i;
 
-	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	num_parents = of_clk_get_parent_count(np);
 	if (num_parents <= 0 || num_parents > 2)
 		return;
 
@@ -451,7 +451,7 @@ void __init of_at91sam9260_clk_slow_setup(struct device_node *np,
 	const char *name = np->name;
 	int i;
 
-	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	num_parents = of_clk_get_parent_count(np);
 	if (num_parents != 2)
 		return;
 
diff --git a/drivers/clk/at91/clk-smd.c b/drivers/clk/at91/clk-smd.c
index 144d47ecfe63..3817ea865ca2 100644
--- a/drivers/clk/at91/clk-smd.c
+++ b/drivers/clk/at91/clk-smd.c
@@ -150,7 +150,7 @@ void __init of_at91sam9x5_clk_smd_setup(struct device_node *np,
 	const char *parent_names[SMD_SOURCE_MAX];
 	const char *name = np->name;
 
-	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	num_parents = of_clk_get_parent_count(np);
 	if (num_parents <= 0 || num_parents > SMD_SOURCE_MAX)
 		return;
 
diff --git a/drivers/clk/at91/clk-system.c b/drivers/clk/at91/clk-system.c
index a76d03fd577b..58008b3e8bc1 100644
--- a/drivers/clk/at91/clk-system.c
+++ b/drivers/clk/at91/clk-system.c
@@ -130,13 +130,17 @@ at91_clk_register_system(struct at91_pmc *pmc, const char *name,
 		irq_set_status_flags(sys->irq, IRQ_NOAUTOEN);
 		ret = request_irq(sys->irq, clk_system_irq_handler,
 				IRQF_TRIGGER_HIGH, name, sys);
-		if (ret)
+		if (ret) {
+			kfree(sys);
 			return ERR_PTR(ret);
+		}
 	}
 
 	clk = clk_register(NULL, &sys->hw);
-	if (IS_ERR(clk))
+	if (IS_ERR(clk)) {
+		free_irq(sys->irq, sys);
 		kfree(sys);
+	}
 
 	return clk;
 }
diff --git a/drivers/clk/at91/clk-usb.c b/drivers/clk/at91/clk-usb.c
index 0b7c3e8840ba..b0cbd2b1ff59 100644
--- a/drivers/clk/at91/clk-usb.c
+++ b/drivers/clk/at91/clk-usb.c
@@ -378,7 +378,7 @@ void __init of_at91sam9x5_clk_usb_setup(struct device_node *np,
 	const char *parent_names[USB_SOURCE_MAX];
 	const char *name = np->name;
 
-	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	num_parents = of_clk_get_parent_count(np);
 	if (num_parents <= 0 || num_parents > USB_SOURCE_MAX)
 		return;
 
diff --git a/drivers/clk/at91/clk-utmi.c b/drivers/clk/at91/clk-utmi.c
index ae3263bc1476..30dd697b1668 100644
--- a/drivers/clk/at91/clk-utmi.c
+++ b/drivers/clk/at91/clk-utmi.c
@@ -118,12 +118,16 @@ at91_clk_register_utmi(struct at91_pmc *pmc, unsigned int irq,
 	irq_set_status_flags(utmi->irq, IRQ_NOAUTOEN);
 	ret = request_irq(utmi->irq, clk_utmi_irq_handler,
 			  IRQF_TRIGGER_HIGH, "clk-utmi", utmi);
-	if (ret)
+	if (ret) {
+		kfree(utmi);
 		return ERR_PTR(ret);
+	}
 
 	clk = clk_register(NULL, &utmi->hw);
-	if (IS_ERR(clk))
+	if (IS_ERR(clk)) {
+		free_irq(utmi->irq, utmi);
 		kfree(utmi);
+	}
 
 	return clk;
 }
diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
index 3f27d21fb729..39be2be82b0a 100644
--- a/drivers/clk/at91/pmc.c
+++ b/drivers/clk/at91/pmc.c
@@ -153,7 +153,7 @@ static int pmc_irq_domain_xlate(struct irq_domain *d,
 	return 0;
 }
 
-static struct irq_domain_ops pmc_irq_ops = {
+static const struct irq_domain_ops pmc_irq_ops = {
 	.map	= pmc_irq_map,
 	.xlate	= pmc_irq_domain_xlate,
 };
diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index 69abb08cf146..eb8e5dc9076d 100644
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -121,7 +121,7 @@ extern void __init of_at91sam9x5_clk_smd_setup(struct device_node *np,
 					       struct at91_pmc *pmc);
 #endif
 
-#if defined(CONFIG_HAVE_AT91_SMD)
+#if defined(CONFIG_HAVE_AT91_H32MX)
 extern void __init of_sama5d4_clk_h32mx_setup(struct device_node *np,
 					      struct at91_pmc *pmc);
 #endif
diff --git a/drivers/clk/bcm/Kconfig b/drivers/clk/bcm/Kconfig
index 75506e53075b..88febf53b276 100644
--- a/drivers/clk/bcm/Kconfig
+++ b/drivers/clk/bcm/Kconfig
@@ -7,3 +7,12 @@ config CLK_BCM_KONA
 	  Enable common clock framework support for Broadcom SoCs
 	  using "Kona" style clock control units, including those
 	  in the BCM281xx and BCM21664 families.
+
+config COMMON_CLK_IPROC
+	bool "Broadcom iProc clock support"
+	depends on ARCH_BCM_IPROC
+	depends on COMMON_CLK
+	default ARCH_BCM_IPROC
+	help
+	  Enable common clock framework support for Broadcom SoCs
+	  based on the iProc architecture
diff --git a/drivers/clk/bcm/Makefile b/drivers/clk/bcm/Makefile
index 6297d05a9a10..8a7a477862c7 100644
--- a/drivers/clk/bcm/Makefile
+++ b/drivers/clk/bcm/Makefile
@@ -2,3 +2,5 @@ obj-$(CONFIG_CLK_BCM_KONA)	+= clk-kona.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-kona-setup.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-bcm281xx.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-bcm21664.o
+obj-$(CONFIG_COMMON_CLK_IPROC)	+= clk-iproc-armpll.o clk-iproc-pll.o clk-iproc-asiu.o
+obj-$(CONFIG_ARCH_BCM_CYGNUS)	+= clk-cygnus.o
diff --git a/drivers/clk/bcm/clk-cygnus.c b/drivers/clk/bcm/clk-cygnus.c
new file mode 100644
index 000000000000..316c60337661
--- /dev/null
+++ b/drivers/clk/bcm/clk-cygnus.c
@@ -0,0 +1,265 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/clkdev.h>
+#include <linux/of_address.h>
+#include <linux/delay.h>
+
+#include <dt-bindings/clock/bcm-cygnus.h>
+#include "clk-iproc.h"
+
+#define reg_val(o, s, w) { .offset = o, .shift = s, .width = w, }
+
+#define aon_val(o, pw, ps, is) { .offset = o, .pwr_width = pw, \
+	.pwr_shift = ps, .iso_shift = is }
+
+#define sw_ctrl_val(o, s) { .offset = o, .shift = s, }
+
+#define asiu_div_val(o, es, hs, hw, ls, lw) \
+		{ .offset = o, .en_shift = es, .high_shift = hs, \
+		.high_width = hw, .low_shift = ls, .low_width = lw }
+
+#define reset_val(o, rs, prs, kis, kiw, kps, kpw, kas, kaw) { .offset = o, \
+	.reset_shift = rs, .p_reset_shift = prs, .ki_shift = kis, \
+	.ki_width = kiw, .kp_shift = kps, .kp_width = kpw, .ka_shift = kas, \
+	.ka_width = kaw }
+
+#define vco_ctrl_val(uo, lo) { .u_offset = uo, .l_offset = lo }
+
+#define enable_val(o, es, hs, bs) { .offset = o, .enable_shift = es, \
+	.hold_shift = hs, .bypass_shift = bs }
+
+#define asiu_gate_val(o, es) { .offset = o, .en_shift = es }
+
+static void __init cygnus_armpll_init(struct device_node *node)
+{
+	iproc_armpll_setup(node);
+}
+CLK_OF_DECLARE(cygnus_armpll, "brcm,cygnus-armpll", cygnus_armpll_init);
+
+static const struct iproc_pll_ctrl genpll = {
+	.flags = IPROC_CLK_AON | IPROC_CLK_PLL_HAS_NDIV_FRAC |
+		IPROC_CLK_PLL_NEEDS_SW_CFG,
+	.aon = aon_val(0x0, 2, 1, 0),
+	.reset = reset_val(0x0, 11, 10, 4, 3, 0, 4, 7, 3),
+	.sw_ctrl = sw_ctrl_val(0x10, 31),
+	.ndiv_int = reg_val(0x10, 20, 10),
+	.ndiv_frac = reg_val(0x10, 0, 20),
+	.pdiv = reg_val(0x14, 0, 4),
+	.vco_ctrl = vco_ctrl_val(0x18, 0x1c),
+	.status = reg_val(0x28, 12, 1),
+};
+
+static const struct iproc_clk_ctrl genpll_clk[] = {
+	[BCM_CYGNUS_GENPLL_AXI21_CLK] = {
+		.channel = BCM_CYGNUS_GENPLL_AXI21_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x4, 6, 0, 12),
+		.mdiv = reg_val(0x20, 0, 8),
+	},
+	[BCM_CYGNUS_GENPLL_250MHZ_CLK] = {
+		.channel = BCM_CYGNUS_GENPLL_250MHZ_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x4, 7, 1, 13),
+		.mdiv = reg_val(0x20, 10, 8),
+	},
+	[BCM_CYGNUS_GENPLL_IHOST_SYS_CLK] = {
+		.channel = BCM_CYGNUS_GENPLL_IHOST_SYS_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x4, 8, 2, 14),
+		.mdiv = reg_val(0x20, 20, 8),
+	},
+	[BCM_CYGNUS_GENPLL_ENET_SW_CLK] = {
+		.channel = BCM_CYGNUS_GENPLL_ENET_SW_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x4, 9, 3, 15),
+		.mdiv = reg_val(0x24, 0, 8),
+	},
+	[BCM_CYGNUS_GENPLL_AUDIO_125_CLK] = {
+		.channel = BCM_CYGNUS_GENPLL_AUDIO_125_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x4, 10, 4, 16),
+		.mdiv = reg_val(0x24, 10, 8),
+	},
+	[BCM_CYGNUS_GENPLL_CAN_CLK] = {
+		.channel = BCM_CYGNUS_GENPLL_CAN_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x4, 11, 5, 17),
+		.mdiv = reg_val(0x24, 20, 8),
+	},
+};
+
+static void __init cygnus_genpll_clk_init(struct device_node *node)
+{
+	iproc_pll_clk_setup(node, &genpll, NULL, 0, genpll_clk,
+			    ARRAY_SIZE(genpll_clk));
+}
+CLK_OF_DECLARE(cygnus_genpll, "brcm,cygnus-genpll", cygnus_genpll_clk_init);
+
+static const struct iproc_pll_ctrl lcpll0 = {
+	.flags = IPROC_CLK_AON | IPROC_CLK_PLL_NEEDS_SW_CFG,
+	.aon = aon_val(0x0, 2, 5, 4),
+	.reset = reset_val(0x0, 31, 30, 27, 3, 23, 4, 19, 4),
+	.sw_ctrl = sw_ctrl_val(0x4, 31),
+	.ndiv_int = reg_val(0x4, 16, 10),
+	.pdiv = reg_val(0x4, 26, 4),
+	.vco_ctrl = vco_ctrl_val(0x10, 0x14),
+	.status = reg_val(0x18, 12, 1),
+};
+
+static const struct iproc_clk_ctrl lcpll0_clk[] = {
+	[BCM_CYGNUS_LCPLL0_PCIE_PHY_REF_CLK] = {
+		.channel = BCM_CYGNUS_LCPLL0_PCIE_PHY_REF_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x0, 7, 1, 13),
+		.mdiv = reg_val(0x8, 0, 8),
+	},
+	[BCM_CYGNUS_LCPLL0_DDR_PHY_CLK] = {
+		.channel = BCM_CYGNUS_LCPLL0_DDR_PHY_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x0, 8, 2, 14),
+		.mdiv = reg_val(0x8, 10, 8),
+	},
+	[BCM_CYGNUS_LCPLL0_SDIO_CLK] = {
+		.channel = BCM_CYGNUS_LCPLL0_SDIO_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x0, 9, 3, 15),
+		.mdiv = reg_val(0x8, 20, 8),
+	},
+	[BCM_CYGNUS_LCPLL0_USB_PHY_REF_CLK] = {
+		.channel = BCM_CYGNUS_LCPLL0_USB_PHY_REF_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x0, 10, 4, 16),
+		.mdiv = reg_val(0xc, 0, 8),
+	},
+	[BCM_CYGNUS_LCPLL0_SMART_CARD_CLK] = {
+		.channel = BCM_CYGNUS_LCPLL0_SMART_CARD_CLK,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x0, 11, 5, 17),
+		.mdiv = reg_val(0xc, 10, 8),
+	},
+	[BCM_CYGNUS_LCPLL0_CH5_UNUSED] = {
+		.channel = BCM_CYGNUS_LCPLL0_CH5_UNUSED,
+		.flags = IPROC_CLK_AON,
+		.enable = enable_val(0x0, 12, 6, 18),
+		.mdiv = reg_val(0xc, 20, 8),
+	},
+};
+
+static void __init cygnus_lcpll0_clk_init(struct device_node *node)
+{
+	iproc_pll_clk_setup(node, &lcpll0, NULL, 0, lcpll0_clk,
+			    ARRAY_SIZE(lcpll0_clk));
+}
+CLK_OF_DECLARE(cygnus_lcpll0, "brcm,cygnus-lcpll0", cygnus_lcpll0_clk_init);
+
+/*
+ * MIPI PLL VCO frequency parameter table
+ */
+static const struct iproc_pll_vco_param mipipll_vco_params[] = {
+	/* rate (Hz) ndiv_int ndiv_frac pdiv */
+	{ 750000000UL,   30,     0,        1 },
+	{ 1000000000UL,  40,     0,        1 },
+	{ 1350000000ul,  54,     0,        1 },
+	{ 2000000000UL,  80,     0,        1 },
+	{ 2100000000UL,  84,     0,        1 },
+	{ 2250000000UL,  90,     0,        1 },
+	{ 2500000000UL,  100,    0,        1 },
+	{ 2700000000UL,  54,     0,        0 },
+	{ 2975000000UL,  119,    0,        1 },
+	{ 3100000000UL,  124,    0,        1 },
+	{ 3150000000UL,  126,    0,        1 },
+};
+
+static const struct iproc_pll_ctrl mipipll = {
+	.flags = IPROC_CLK_PLL_ASIU | IPROC_CLK_PLL_HAS_NDIV_FRAC |
+		 IPROC_CLK_NEEDS_READ_BACK,
+	.aon = aon_val(0x0, 4, 17, 16),
+	.asiu = asiu_gate_val(0x0, 3),
+	.reset = reset_val(0x0, 11, 10, 4, 3, 0, 4, 7, 4),
+	.ndiv_int = reg_val(0x10, 20, 10),
+	.ndiv_frac = reg_val(0x10, 0, 20),
+	.pdiv = reg_val(0x14, 0, 4),
+	.vco_ctrl = vco_ctrl_val(0x18, 0x1c),
+	.status = reg_val(0x28, 12, 1),
+};
+
+static const struct iproc_clk_ctrl mipipll_clk[] = {
+	[BCM_CYGNUS_MIPIPLL_CH0_UNUSED] = {
+		.channel = BCM_CYGNUS_MIPIPLL_CH0_UNUSED,
+		.flags = IPROC_CLK_NEEDS_READ_BACK,
+		.enable = enable_val(0x4, 12, 6, 18),
+		.mdiv = reg_val(0x20, 0, 8),
+	},
+	[BCM_CYGNUS_MIPIPLL_CH1_LCD] = {
+		.channel = BCM_CYGNUS_MIPIPLL_CH1_LCD,
+		.flags = IPROC_CLK_NEEDS_READ_BACK,
+		.enable = enable_val(0x4, 13, 7, 19),
+		.mdiv = reg_val(0x20, 10, 8),
+	},
+	[BCM_CYGNUS_MIPIPLL_CH2_V3D] = {
+		.channel = BCM_CYGNUS_MIPIPLL_CH2_V3D,
+		.flags = IPROC_CLK_NEEDS_READ_BACK,
+		.enable = enable_val(0x4, 14, 8, 20),
+		.mdiv = reg_val(0x20, 20, 8),
+	},
+	[BCM_CYGNUS_MIPIPLL_CH3_UNUSED] = {
+		.channel = BCM_CYGNUS_MIPIPLL_CH3_UNUSED,
+		.flags = IPROC_CLK_NEEDS_READ_BACK,
+		.enable = enable_val(0x4, 15, 9, 21),
+		.mdiv = reg_val(0x24, 0, 8),
+	},
+	[BCM_CYGNUS_MIPIPLL_CH4_UNUSED] = {
+		.channel = BCM_CYGNUS_MIPIPLL_CH4_UNUSED,
+		.flags = IPROC_CLK_NEEDS_READ_BACK,
+		.enable = enable_val(0x4, 16, 10, 22),
+		.mdiv = reg_val(0x24, 10, 8),
+	},
+	[BCM_CYGNUS_MIPIPLL_CH5_UNUSED] = {
+		.channel = BCM_CYGNUS_MIPIPLL_CH5_UNUSED,
+		.flags = IPROC_CLK_NEEDS_READ_BACK,
+		.enable = enable_val(0x4, 17, 11, 23),
+		.mdiv = reg_val(0x24, 20, 8),
+	},
+};
+
+static void __init cygnus_mipipll_clk_init(struct device_node *node)
+{
+	iproc_pll_clk_setup(node, &mipipll, mipipll_vco_params,
+			    ARRAY_SIZE(mipipll_vco_params), mipipll_clk,
+			    ARRAY_SIZE(mipipll_clk));
+}
+CLK_OF_DECLARE(cygnus_mipipll, "brcm,cygnus-mipipll", cygnus_mipipll_clk_init);
+
+static const struct iproc_asiu_div asiu_div[] = {
+	[BCM_CYGNUS_ASIU_KEYPAD_CLK] = asiu_div_val(0x0, 31, 16, 10, 0, 10),
+	[BCM_CYGNUS_ASIU_ADC_CLK] = asiu_div_val(0x4, 31, 16, 10, 0, 10),
+	[BCM_CYGNUS_ASIU_PWM_CLK] = asiu_div_val(0x8, 31, 16, 10, 0, 10),
+};
+
+static const struct iproc_asiu_gate asiu_gate[] = {
+	[BCM_CYGNUS_ASIU_KEYPAD_CLK] = asiu_gate_val(0x0, 7),
+	[BCM_CYGNUS_ASIU_ADC_CLK] = asiu_gate_val(0x0, 9),
+	[BCM_CYGNUS_ASIU_PWM_CLK] = asiu_gate_val(IPROC_CLK_INVALID_OFFSET, 0),
+};
+
+static void __init cygnus_asiu_init(struct device_node *node)
+{
+	iproc_asiu_setup(node, asiu_div, asiu_gate, ARRAY_SIZE(asiu_div));
+}
+CLK_OF_DECLARE(cygnus_asiu_clk, "brcm,cygnus-asiu-clk", cygnus_asiu_init);
diff --git a/drivers/clk/bcm/clk-iproc-armpll.c b/drivers/clk/bcm/clk-iproc-armpll.c
new file mode 100644
index 000000000000..a196ee28a17a
--- /dev/null
+++ b/drivers/clk/bcm/clk-iproc-armpll.c
@@ -0,0 +1,282 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/clkdev.h>
+#include <linux/of_address.h>
+
+#define IPROC_CLK_MAX_FREQ_POLICY                    0x3
+#define IPROC_CLK_POLICY_FREQ_OFFSET                 0x008
+#define IPROC_CLK_POLICY_FREQ_POLICY_FREQ_SHIFT      8
+#define IPROC_CLK_POLICY_FREQ_POLICY_FREQ_MASK       0x7
+
+#define IPROC_CLK_PLLARMA_OFFSET                     0xc00
+#define IPROC_CLK_PLLARMA_LOCK_SHIFT                 28
+#define IPROC_CLK_PLLARMA_PDIV_SHIFT                 24
+#define IPROC_CLK_PLLARMA_PDIV_MASK                  0xf
+#define IPROC_CLK_PLLARMA_NDIV_INT_SHIFT             8
+#define IPROC_CLK_PLLARMA_NDIV_INT_MASK              0x3ff
+
+#define IPROC_CLK_PLLARMB_OFFSET                     0xc04
+#define IPROC_CLK_PLLARMB_NDIV_FRAC_MASK             0xfffff
+
+#define IPROC_CLK_PLLARMC_OFFSET                     0xc08
+#define IPROC_CLK_PLLARMC_BYPCLK_EN_SHIFT            8
+#define IPROC_CLK_PLLARMC_MDIV_MASK                  0xff
+
+#define IPROC_CLK_PLLARMCTL5_OFFSET                  0xc20
+#define IPROC_CLK_PLLARMCTL5_H_MDIV_MASK             0xff
+
+#define IPROC_CLK_PLLARM_OFFSET_OFFSET               0xc24
+#define IPROC_CLK_PLLARM_SW_CTL_SHIFT                29
+#define IPROC_CLK_PLLARM_NDIV_INT_OFFSET_SHIFT       20
+#define IPROC_CLK_PLLARM_NDIV_INT_OFFSET_MASK        0xff
+#define IPROC_CLK_PLLARM_NDIV_FRAC_OFFSET_MASK       0xfffff
+
+#define IPROC_CLK_ARM_DIV_OFFSET                     0xe00
+#define IPROC_CLK_ARM_DIV_PLL_SELECT_OVERRIDE_SHIFT  4
+#define IPROC_CLK_ARM_DIV_ARM_PLL_SELECT_MASK        0xf
+
+#define IPROC_CLK_POLICY_DBG_OFFSET                  0xec0
+#define IPROC_CLK_POLICY_DBG_ACT_FREQ_SHIFT          12
+#define IPROC_CLK_POLICY_DBG_ACT_FREQ_MASK           0x7
+
+enum iproc_arm_pll_fid {
+	ARM_PLL_FID_CRYSTAL_CLK   = 0,
+	ARM_PLL_FID_SYS_CLK       = 2,
+	ARM_PLL_FID_CH0_SLOW_CLK  = 6,
+	ARM_PLL_FID_CH1_FAST_CLK  = 7
+};
+
+struct iproc_arm_pll {
+	struct clk_hw hw;
+	void __iomem *base;
+	unsigned long rate;
+};
+
+#define to_iproc_arm_pll(hw) container_of(hw, struct iproc_arm_pll, hw)
+
+static unsigned int __get_fid(struct iproc_arm_pll *pll)
+{
+	u32 val;
+	unsigned int policy, fid, active_fid;
+
+	val = readl(pll->base + IPROC_CLK_ARM_DIV_OFFSET);
+	if (val & (1 << IPROC_CLK_ARM_DIV_PLL_SELECT_OVERRIDE_SHIFT))
+		policy = val & IPROC_CLK_ARM_DIV_ARM_PLL_SELECT_MASK;
+	else
+		policy = 0;
+
+	/* something is seriously wrong */
+	BUG_ON(policy > IPROC_CLK_MAX_FREQ_POLICY);
+
+	val = readl(pll->base + IPROC_CLK_POLICY_FREQ_OFFSET);
+	fid = (val >> (IPROC_CLK_POLICY_FREQ_POLICY_FREQ_SHIFT * policy)) &
+		IPROC_CLK_POLICY_FREQ_POLICY_FREQ_MASK;
+
+	val = readl(pll->base + IPROC_CLK_POLICY_DBG_OFFSET);
+	active_fid = IPROC_CLK_POLICY_DBG_ACT_FREQ_MASK &
+		(val >> IPROC_CLK_POLICY_DBG_ACT_FREQ_SHIFT);
+	if (fid != active_fid) {
+		pr_debug("%s: fid override %u->%u\n", __func__,	fid,
+				active_fid);
+		fid = active_fid;
+	}
+
+	pr_debug("%s: active fid: %u\n", __func__, fid);
+
+	return fid;
+}
+
+/*
+ * Determine the mdiv (post divider) based on the frequency ID being used.
+ * There are 4 sources that can be used to derive the output clock rate:
+ *    - 25 MHz Crystal
+ *    - System clock
+ *    - PLL channel 0 (slow clock)
+ *    - PLL channel 1 (fast clock)
+ */
+static int __get_mdiv(struct iproc_arm_pll *pll)
+{
+	unsigned int fid;
+	int mdiv;
+	u32 val;
+
+	fid = __get_fid(pll);
+
+	switch (fid) {
+	case ARM_PLL_FID_CRYSTAL_CLK:
+	case ARM_PLL_FID_SYS_CLK:
+		mdiv = 1;
+		break;
+
+	case ARM_PLL_FID_CH0_SLOW_CLK:
+		val = readl(pll->base + IPROC_CLK_PLLARMC_OFFSET);
+		mdiv = val & IPROC_CLK_PLLARMC_MDIV_MASK;
+		if (mdiv == 0)
+			mdiv = 256;
+		break;
+
+	case ARM_PLL_FID_CH1_FAST_CLK:
+		val = readl(pll->base +	IPROC_CLK_PLLARMCTL5_OFFSET);
+		mdiv = val & IPROC_CLK_PLLARMCTL5_H_MDIV_MASK;
+		if (mdiv == 0)
+			mdiv = 256;
+		break;
+
+	default:
+		mdiv = -EFAULT;
+	}
+
+	return mdiv;
+}
+
+static unsigned int __get_ndiv(struct iproc_arm_pll *pll)
+{
+	u32 val;
+	unsigned int ndiv_int, ndiv_frac, ndiv;
+
+	val = readl(pll->base + IPROC_CLK_PLLARM_OFFSET_OFFSET);
+	if (val & (1 << IPROC_CLK_PLLARM_SW_CTL_SHIFT)) {
+		/*
+		 * offset mode is active. Read the ndiv from the PLLARM OFFSET
+		 * register
+		 */
+		ndiv_int = (val >> IPROC_CLK_PLLARM_NDIV_INT_OFFSET_SHIFT) &
+			IPROC_CLK_PLLARM_NDIV_INT_OFFSET_MASK;
+		if (ndiv_int == 0)
+			ndiv_int = 256;
+
+		ndiv_frac = val & IPROC_CLK_PLLARM_NDIV_FRAC_OFFSET_MASK;
+	} else {
+		/* offset mode not active */
+		val = readl(pll->base + IPROC_CLK_PLLARMA_OFFSET);
+		ndiv_int = (val >> IPROC_CLK_PLLARMA_NDIV_INT_SHIFT) &
+			IPROC_CLK_PLLARMA_NDIV_INT_MASK;
+		if (ndiv_int == 0)
+			ndiv_int = 1024;
+
+		val = readl(pll->base + IPROC_CLK_PLLARMB_OFFSET);
+		ndiv_frac = val & IPROC_CLK_PLLARMB_NDIV_FRAC_MASK;
+	}
+
+	ndiv = (ndiv_int << 20) | ndiv_frac;
+
+	return ndiv;
+}
+
+/*
+ * The output frequency of the ARM PLL is calculated based on the ARM PLL
+ * divider values:
+ *   pdiv = ARM PLL pre-divider
+ *   ndiv = ARM PLL multiplier
+ *   mdiv = ARM PLL post divider
+ *
+ * The frequency is calculated by:
+ *   ((ndiv * parent clock rate) / pdiv) / mdiv
+ */
+static unsigned long iproc_arm_pll_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct iproc_arm_pll *pll = to_iproc_arm_pll(hw);
+	u32 val;
+	int mdiv;
+	u64 ndiv;
+	unsigned int pdiv;
+
+	/* in bypass mode, use parent rate */
+	val = readl(pll->base + IPROC_CLK_PLLARMC_OFFSET);
+	if (val & (1 << IPROC_CLK_PLLARMC_BYPCLK_EN_SHIFT)) {
+		pll->rate = parent_rate;
+		return pll->rate;
+	}
+
+	/* PLL needs to be locked */
+	val = readl(pll->base + IPROC_CLK_PLLARMA_OFFSET);
+	if (!(val & (1 << IPROC_CLK_PLLARMA_LOCK_SHIFT))) {
+		pll->rate = 0;
+		return 0;
+	}
+
+	pdiv = (val >> IPROC_CLK_PLLARMA_PDIV_SHIFT) &
+		IPROC_CLK_PLLARMA_PDIV_MASK;
+	if (pdiv == 0)
+		pdiv = 16;
+
+	ndiv = __get_ndiv(pll);
+	mdiv = __get_mdiv(pll);
+	if (mdiv <= 0) {
+		pll->rate = 0;
+		return 0;
+	}
+	pll->rate = (ndiv * parent_rate) >> 20;
+	pll->rate = (pll->rate / pdiv) / mdiv;
+
+	pr_debug("%s: ARM PLL rate: %lu. parent rate: %lu\n", __func__,
+		 pll->rate, parent_rate);
+	pr_debug("%s: ndiv_int: %u, pdiv: %u, mdiv: %d\n", __func__,
+		 (unsigned int)(ndiv >> 20), pdiv, mdiv);
+
+	return pll->rate;
+}
+
+static const struct clk_ops iproc_arm_pll_ops = {
+	.recalc_rate = iproc_arm_pll_recalc_rate,
+};
+
+void __init iproc_armpll_setup(struct device_node *node)
+{
+	int ret;
+	struct clk *clk;
+	struct iproc_arm_pll *pll;
+	struct clk_init_data init;
+	const char *parent_name;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (WARN_ON(!pll))
+		return;
+
+	pll->base = of_iomap(node, 0);
+	if (WARN_ON(!pll->base))
+		goto err_free_pll;
+
+	init.name = node->name;
+	init.ops = &iproc_arm_pll_ops;
+	init.flags = 0;
+	parent_name = of_clk_get_parent_name(node, 0);
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+	pll->hw.init = &init;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (WARN_ON(IS_ERR(clk)))
+		goto err_iounmap;
+
+	ret = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (WARN_ON(ret))
+		goto err_clk_unregister;
+
+	return;
+
+err_clk_unregister:
+	clk_unregister(clk);
+err_iounmap:
+	iounmap(pll->base);
+err_free_pll:
+	kfree(pll);
+}
diff --git a/drivers/clk/bcm/clk-iproc-asiu.c b/drivers/clk/bcm/clk-iproc-asiu.c
new file mode 100644
index 000000000000..f630e1bbdcfe
--- /dev/null
+++ b/drivers/clk/bcm/clk-iproc-asiu.c
@@ -0,0 +1,272 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/clkdev.h>
+#include <linux/of_address.h>
+#include <linux/delay.h>
+
+#include "clk-iproc.h"
+
+struct iproc_asiu;
+
+struct iproc_asiu_clk {
+	struct clk_hw hw;
+	const char *name;
+	struct iproc_asiu *asiu;
+	unsigned long rate;
+	struct iproc_asiu_div div;
+	struct iproc_asiu_gate gate;
+};
+
+struct iproc_asiu {
+	void __iomem *div_base;
+	void __iomem *gate_base;
+
+	struct clk_onecell_data clk_data;
+	struct iproc_asiu_clk *clks;
+};
+
+#define to_asiu_clk(hw) container_of(hw, struct iproc_asiu_clk, hw)
+
+static int iproc_asiu_clk_enable(struct clk_hw *hw)
+{
+	struct iproc_asiu_clk *clk = to_asiu_clk(hw);
+	struct iproc_asiu *asiu = clk->asiu;
+	u32 val;
+
+	/* some clocks at the ASIU level are always enabled */
+	if (clk->gate.offset == IPROC_CLK_INVALID_OFFSET)
+		return 0;
+
+	val = readl(asiu->gate_base + clk->gate.offset);
+	val |= (1 << clk->gate.en_shift);
+	writel(val, asiu->gate_base + clk->gate.offset);
+
+	return 0;
+}
+
+static void iproc_asiu_clk_disable(struct clk_hw *hw)
+{
+	struct iproc_asiu_clk *clk = to_asiu_clk(hw);
+	struct iproc_asiu *asiu = clk->asiu;
+	u32 val;
+
+	/* some clocks at the ASIU level are always enabled */
+	if (clk->gate.offset == IPROC_CLK_INVALID_OFFSET)
+		return;
+
+	val = readl(asiu->gate_base + clk->gate.offset);
+	val &= ~(1 << clk->gate.en_shift);
+	writel(val, asiu->gate_base + clk->gate.offset);
+}
+
+static unsigned long iproc_asiu_clk_recalc_rate(struct clk_hw *hw,
+						unsigned long parent_rate)
+{
+	struct iproc_asiu_clk *clk = to_asiu_clk(hw);
+	struct iproc_asiu *asiu = clk->asiu;
+	u32 val;
+	unsigned int div_h, div_l;
+
+	if (parent_rate == 0) {
+		clk->rate = 0;
+		return 0;
+	}
+
+	/* if clock divisor is not enabled, simply return parent rate */
+	val = readl(asiu->div_base + clk->div.offset);
+	if ((val & (1 << clk->div.en_shift)) == 0) {
+		clk->rate = parent_rate;
+		return parent_rate;
+	}
+
+	/* clock rate = parent rate / (high_div + 1) + (low_div + 1) */
+	div_h = (val >> clk->div.high_shift) & bit_mask(clk->div.high_width);
+	div_h++;
+	div_l = (val >> clk->div.low_shift) & bit_mask(clk->div.low_width);
+	div_l++;
+
+	clk->rate = parent_rate / (div_h + div_l);
+	pr_debug("%s: rate: %lu. parent rate: %lu div_h: %u div_l: %u\n",
+		 __func__, clk->rate, parent_rate, div_h, div_l);
+
+	return clk->rate;
+}
+
+static long iproc_asiu_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+				      unsigned long *parent_rate)
+{
+	unsigned int div;
+
+	if (rate == 0 || *parent_rate == 0)
+		return -EINVAL;
+
+	if (rate == *parent_rate)
+		return *parent_rate;
+
+	div = DIV_ROUND_UP(*parent_rate, rate);
+	if (div < 2)
+		return *parent_rate;
+
+	return *parent_rate / div;
+}
+
+static int iproc_asiu_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+				   unsigned long parent_rate)
+{
+	struct iproc_asiu_clk *clk = to_asiu_clk(hw);
+	struct iproc_asiu *asiu = clk->asiu;
+	unsigned int div, div_h, div_l;
+	u32 val;
+
+	if (rate == 0 || parent_rate == 0)
+		return -EINVAL;
+
+	/* simply disable the divisor if one wants the same rate as parent */
+	if (rate == parent_rate) {
+		val = readl(asiu->div_base + clk->div.offset);
+		val &= ~(1 << clk->div.en_shift);
+		writel(val, asiu->div_base + clk->div.offset);
+		return 0;
+	}
+
+	div = DIV_ROUND_UP(parent_rate, rate);
+	if (div < 2)
+		return -EINVAL;
+
+	div_h = div_l = div >> 1;
+	div_h--;
+	div_l--;
+
+	val = readl(asiu->div_base + clk->div.offset);
+	val |= 1 << clk->div.en_shift;
+	if (div_h) {
+		val &= ~(bit_mask(clk->div.high_width)
+			 << clk->div.high_shift);
+		val |= div_h << clk->div.high_shift;
+	} else {
+		val &= ~(bit_mask(clk->div.high_width)
+			 << clk->div.high_shift);
+	}
+	if (div_l) {
+		val &= ~(bit_mask(clk->div.low_width) << clk->div.low_shift);
+		val |= div_l << clk->div.low_shift;
+	} else {
+		val &= ~(bit_mask(clk->div.low_width) << clk->div.low_shift);
+	}
+	writel(val, asiu->div_base + clk->div.offset);
+
+	return 0;
+}
+
+static const struct clk_ops iproc_asiu_ops = {
+	.enable = iproc_asiu_clk_enable,
+	.disable = iproc_asiu_clk_disable,
+	.recalc_rate = iproc_asiu_clk_recalc_rate,
+	.round_rate = iproc_asiu_clk_round_rate,
+	.set_rate = iproc_asiu_clk_set_rate,
+};
+
+void __init iproc_asiu_setup(struct device_node *node,
+			     const struct iproc_asiu_div *div,
+			     const struct iproc_asiu_gate *gate,
+			     unsigned int num_clks)
+{
+	int i, ret;
+	struct iproc_asiu *asiu;
+
+	if (WARN_ON(!gate || !div))
+		return;
+
+	asiu = kzalloc(sizeof(*asiu), GFP_KERNEL);
+	if (WARN_ON(!asiu))
+		return;
+
+	asiu->clk_data.clk_num = num_clks;
+	asiu->clk_data.clks = kcalloc(num_clks, sizeof(*asiu->clk_data.clks),
+				      GFP_KERNEL);
+	if (WARN_ON(!asiu->clk_data.clks))
+		goto err_clks;
+
+	asiu->clks = kcalloc(num_clks, sizeof(*asiu->clks), GFP_KERNEL);
+	if (WARN_ON(!asiu->clks))
+		goto err_asiu_clks;
+
+	asiu->div_base = of_iomap(node, 0);
+	if (WARN_ON(!asiu->div_base))
+		goto err_iomap_div;
+
+	asiu->gate_base = of_iomap(node, 1);
+	if (WARN_ON(!asiu->gate_base))
+		goto err_iomap_gate;
+
+	for (i = 0; i < num_clks; i++) {
+		struct clk_init_data init;
+		struct clk *clk;
+		const char *parent_name;
+		struct iproc_asiu_clk *asiu_clk;
+		const char *clk_name;
+
+		ret = of_property_read_string_index(node, "clock-output-names",
+						    i, &clk_name);
+		if (WARN_ON(ret))
+			goto err_clk_register;
+
+		asiu_clk = &asiu->clks[i];
+		asiu_clk->name = clk_name;
+		asiu_clk->asiu = asiu;
+		asiu_clk->div = div[i];
+		asiu_clk->gate = gate[i];
+		init.name = clk_name;
+		init.ops = &iproc_asiu_ops;
+		init.flags = 0;
+		parent_name = of_clk_get_parent_name(node, 0);
+		init.parent_names = (parent_name ? &parent_name : NULL);
+		init.num_parents = (parent_name ? 1 : 0);
+		asiu_clk->hw.init = &init;
+
+		clk = clk_register(NULL, &asiu_clk->hw);
+		if (WARN_ON(IS_ERR(clk)))
+			goto err_clk_register;
+		asiu->clk_data.clks[i] = clk;
+	}
+
+	ret = of_clk_add_provider(node, of_clk_src_onecell_get,
+				  &asiu->clk_data);
+	if (WARN_ON(ret))
+		goto err_clk_register;
+
+	return;
+
+err_clk_register:
+	for (i = 0; i < num_clks; i++)
+		clk_unregister(asiu->clk_data.clks[i]);
+	iounmap(asiu->gate_base);
+
+err_iomap_gate:
+	iounmap(asiu->div_base);
+
+err_iomap_div:
+	kfree(asiu->clks);
+
+err_asiu_clks:
+	kfree(asiu->clk_data.clks);
+
+err_clks:
+	kfree(asiu);
+}
diff --git a/drivers/clk/bcm/clk-iproc-pll.c b/drivers/clk/bcm/clk-iproc-pll.c
new file mode 100644
index 000000000000..2dda4e8295a9
--- /dev/null
+++ b/drivers/clk/bcm/clk-iproc-pll.c
@@ -0,0 +1,711 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/clkdev.h>
+#include <linux/of_address.h>
+#include <linux/delay.h>
+
+#include "clk-iproc.h"
+
+#define PLL_VCO_HIGH_SHIFT 19
+#define PLL_VCO_LOW_SHIFT  30
+
+/* number of delay loops waiting for PLL to lock */
+#define LOCK_DELAY 100
+
+/* number of VCO frequency bands */
+#define NUM_FREQ_BANDS 8
+
+#define NUM_KP_BANDS 3
+enum kp_band {
+	KP_BAND_MID = 0,
+	KP_BAND_HIGH,
+	KP_BAND_HIGH_HIGH
+};
+
+static const unsigned int kp_table[NUM_KP_BANDS][NUM_FREQ_BANDS] = {
+	{ 5, 6, 6, 7, 7, 8, 9, 10 },
+	{ 4, 4, 5, 5, 6, 7, 8, 9  },
+	{ 4, 5, 5, 6, 7, 8, 9, 10 },
+};
+
+static const unsigned long ref_freq_table[NUM_FREQ_BANDS][2] = {
+	{ 10000000,  12500000  },
+	{ 12500000,  15000000  },
+	{ 15000000,  20000000  },
+	{ 20000000,  25000000  },
+	{ 25000000,  50000000  },
+	{ 50000000,  75000000  },
+	{ 75000000,  100000000 },
+	{ 100000000, 125000000 },
+};
+
+enum vco_freq_range {
+	VCO_LOW       = 700000000U,
+	VCO_MID       = 1200000000U,
+	VCO_HIGH      = 2200000000U,
+	VCO_HIGH_HIGH = 3100000000U,
+	VCO_MAX       = 4000000000U,
+};
+
+struct iproc_pll;
+
+struct iproc_clk {
+	struct clk_hw hw;
+	const char *name;
+	struct iproc_pll *pll;
+	unsigned long rate;
+	const struct iproc_clk_ctrl *ctrl;
+};
+
+struct iproc_pll {
+	void __iomem *pll_base;
+	void __iomem *pwr_base;
+	void __iomem *asiu_base;
+
+	const struct iproc_pll_ctrl *ctrl;
+	const struct iproc_pll_vco_param *vco_param;
+	unsigned int num_vco_entries;
+
+	struct clk_onecell_data clk_data;
+	struct iproc_clk *clks;
+};
+
+#define to_iproc_clk(hw) container_of(hw, struct iproc_clk, hw)
+
+/*
+ * Based on the target frequency, find a match from the VCO frequency parameter
+ * table and return its index
+ */
+static int pll_get_rate_index(struct iproc_pll *pll, unsigned int target_rate)
+{
+	int i;
+
+	for (i = 0; i < pll->num_vco_entries; i++)
+		if (target_rate == pll->vco_param[i].rate)
+			break;
+
+	if (i >= pll->num_vco_entries)
+		return -EINVAL;
+
+	return i;
+}
+
+static int get_kp(unsigned long ref_freq, enum kp_band kp_index)
+{
+	int i;
+
+	if (ref_freq < ref_freq_table[0][0])
+		return -EINVAL;
+
+	for (i = 0; i < NUM_FREQ_BANDS; i++) {
+		if (ref_freq >= ref_freq_table[i][0] &&
+		    ref_freq < ref_freq_table[i][1])
+			return kp_table[kp_index][i];
+	}
+	return -EINVAL;
+}
+
+static int pll_wait_for_lock(struct iproc_pll *pll)
+{
+	int i;
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+
+	for (i = 0; i < LOCK_DELAY; i++) {
+		u32 val = readl(pll->pll_base + ctrl->status.offset);
+
+		if (val & (1 << ctrl->status.shift))
+			return 0;
+		udelay(10);
+	}
+
+	return -EIO;
+}
+
+static void __pll_disable(struct iproc_pll *pll)
+{
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+	u32 val;
+
+	if (ctrl->flags & IPROC_CLK_PLL_ASIU) {
+		val = readl(pll->asiu_base + ctrl->asiu.offset);
+		val &= ~(1 << ctrl->asiu.en_shift);
+		writel(val, pll->asiu_base + ctrl->asiu.offset);
+	}
+
+	/* latch input value so core power can be shut down */
+	val = readl(pll->pwr_base + ctrl->aon.offset);
+	val |= (1 << ctrl->aon.iso_shift);
+	writel(val, pll->pwr_base + ctrl->aon.offset);
+
+	/* power down the core */
+	val &= ~(bit_mask(ctrl->aon.pwr_width) << ctrl->aon.pwr_shift);
+	writel(val, pll->pwr_base + ctrl->aon.offset);
+}
+
+static int __pll_enable(struct iproc_pll *pll)
+{
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+	u32 val;
+
+	/* power up the PLL and make sure it's not latched */
+	val = readl(pll->pwr_base + ctrl->aon.offset);
+	val |= bit_mask(ctrl->aon.pwr_width) << ctrl->aon.pwr_shift;
+	val &= ~(1 << ctrl->aon.iso_shift);
+	writel(val, pll->pwr_base + ctrl->aon.offset);
+
+	/* certain PLLs also need to be ungated from the ASIU top level */
+	if (ctrl->flags & IPROC_CLK_PLL_ASIU) {
+		val = readl(pll->asiu_base + ctrl->asiu.offset);
+		val |= (1 << ctrl->asiu.en_shift);
+		writel(val, pll->asiu_base + ctrl->asiu.offset);
+	}
+
+	return 0;
+}
+
+static void __pll_put_in_reset(struct iproc_pll *pll)
+{
+	u32 val;
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+	const struct iproc_pll_reset_ctrl *reset = &ctrl->reset;
+
+	val = readl(pll->pll_base + reset->offset);
+	val &= ~(1 << reset->reset_shift | 1 << reset->p_reset_shift);
+	writel(val, pll->pll_base + reset->offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + reset->offset);
+}
+
+static void __pll_bring_out_reset(struct iproc_pll *pll, unsigned int kp,
+				  unsigned int ka, unsigned int ki)
+{
+	u32 val;
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+	const struct iproc_pll_reset_ctrl *reset = &ctrl->reset;
+
+	val = readl(pll->pll_base + reset->offset);
+	val &= ~(bit_mask(reset->ki_width) << reset->ki_shift |
+		 bit_mask(reset->kp_width) << reset->kp_shift |
+		 bit_mask(reset->ka_width) << reset->ka_shift);
+	val |=  ki << reset->ki_shift | kp << reset->kp_shift |
+		ka << reset->ka_shift;
+	val |= 1 << reset->reset_shift | 1 << reset->p_reset_shift;
+	writel(val, pll->pll_base + reset->offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + reset->offset);
+}
+
+static int pll_set_rate(struct iproc_clk *clk, unsigned int rate_index,
+			unsigned long parent_rate)
+{
+	struct iproc_pll *pll = clk->pll;
+	const struct iproc_pll_vco_param *vco = &pll->vco_param[rate_index];
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+	int ka = 0, ki, kp, ret;
+	unsigned long rate = vco->rate;
+	u32 val;
+	enum kp_band kp_index;
+	unsigned long ref_freq;
+
+	/*
+	 * reference frequency = parent frequency / PDIV
+	 * If PDIV = 0, then it becomes a multiplier (x2)
+	 */
+	if (vco->pdiv == 0)
+		ref_freq = parent_rate * 2;
+	else
+		ref_freq = parent_rate / vco->pdiv;
+
+	/* determine Ki and Kp index based on target VCO frequency */
+	if (rate >= VCO_LOW && rate < VCO_HIGH) {
+		ki = 4;
+		kp_index = KP_BAND_MID;
+	} else if (rate >= VCO_HIGH && rate && rate < VCO_HIGH_HIGH) {
+		ki = 3;
+		kp_index = KP_BAND_HIGH;
+	} else if (rate >= VCO_HIGH_HIGH && rate < VCO_MAX) {
+		ki = 3;
+		kp_index = KP_BAND_HIGH_HIGH;
+	} else {
+		pr_err("%s: pll: %s has invalid rate: %lu\n", __func__,
+				clk->name, rate);
+		return -EINVAL;
+	}
+
+	kp = get_kp(ref_freq, kp_index);
+	if (kp < 0) {
+		pr_err("%s: pll: %s has invalid kp\n", __func__, clk->name);
+		return kp;
+	}
+
+	ret = __pll_enable(pll);
+	if (ret) {
+		pr_err("%s: pll: %s fails to enable\n", __func__, clk->name);
+		return ret;
+	}
+
+	/* put PLL in reset */
+	__pll_put_in_reset(pll);
+
+	writel(0, pll->pll_base + ctrl->vco_ctrl.u_offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + ctrl->vco_ctrl.u_offset);
+	val = readl(pll->pll_base + ctrl->vco_ctrl.l_offset);
+
+	if (rate >= VCO_LOW && rate < VCO_MID)
+		val |= (1 << PLL_VCO_LOW_SHIFT);
+
+	if (rate < VCO_HIGH)
+		val &= ~(1 << PLL_VCO_HIGH_SHIFT);
+	else
+		val |= (1 << PLL_VCO_HIGH_SHIFT);
+
+	writel(val, pll->pll_base + ctrl->vco_ctrl.l_offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + ctrl->vco_ctrl.l_offset);
+
+	/* program integer part of NDIV */
+	val = readl(pll->pll_base + ctrl->ndiv_int.offset);
+	val &= ~(bit_mask(ctrl->ndiv_int.width) << ctrl->ndiv_int.shift);
+	val |= vco->ndiv_int << ctrl->ndiv_int.shift;
+	writel(val, pll->pll_base + ctrl->ndiv_int.offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + ctrl->ndiv_int.offset);
+
+	/* program fractional part of NDIV */
+	if (ctrl->flags & IPROC_CLK_PLL_HAS_NDIV_FRAC) {
+		val = readl(pll->pll_base + ctrl->ndiv_frac.offset);
+		val &= ~(bit_mask(ctrl->ndiv_frac.width) <<
+			 ctrl->ndiv_frac.shift);
+		val |= vco->ndiv_frac << ctrl->ndiv_frac.shift;
+		writel(val, pll->pll_base + ctrl->ndiv_frac.offset);
+		if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+			readl(pll->pll_base + ctrl->ndiv_frac.offset);
+	}
+
+	/* program PDIV */
+	val = readl(pll->pll_base + ctrl->pdiv.offset);
+	val &= ~(bit_mask(ctrl->pdiv.width) << ctrl->pdiv.shift);
+	val |= vco->pdiv << ctrl->pdiv.shift;
+	writel(val, pll->pll_base + ctrl->pdiv.offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + ctrl->pdiv.offset);
+
+	__pll_bring_out_reset(pll, kp, ka, ki);
+
+	ret = pll_wait_for_lock(pll);
+	if (ret < 0) {
+		pr_err("%s: pll: %s failed to lock\n", __func__, clk->name);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int iproc_pll_enable(struct clk_hw *hw)
+{
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	struct iproc_pll *pll = clk->pll;
+
+	return __pll_enable(pll);
+}
+
+static void iproc_pll_disable(struct clk_hw *hw)
+{
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	struct iproc_pll *pll = clk->pll;
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+
+	if (ctrl->flags & IPROC_CLK_AON)
+		return;
+
+	__pll_disable(pll);
+}
+
+static unsigned long iproc_pll_recalc_rate(struct clk_hw *hw,
+					   unsigned long parent_rate)
+{
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	struct iproc_pll *pll = clk->pll;
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+	u32 val;
+	u64 ndiv;
+	unsigned int ndiv_int, ndiv_frac, pdiv;
+
+	if (parent_rate == 0)
+		return 0;
+
+	/* PLL needs to be locked */
+	val = readl(pll->pll_base + ctrl->status.offset);
+	if ((val & (1 << ctrl->status.shift)) == 0) {
+		clk->rate = 0;
+		return 0;
+	}
+
+	/*
+	 * PLL output frequency =
+	 *
+	 * ((ndiv_int + ndiv_frac / 2^20) * (parent clock rate / pdiv)
+	 */
+	val = readl(pll->pll_base + ctrl->ndiv_int.offset);
+	ndiv_int = (val >> ctrl->ndiv_int.shift) &
+		bit_mask(ctrl->ndiv_int.width);
+	ndiv = (u64)ndiv_int << ctrl->ndiv_int.shift;
+
+	if (ctrl->flags & IPROC_CLK_PLL_HAS_NDIV_FRAC) {
+		val = readl(pll->pll_base + ctrl->ndiv_frac.offset);
+		ndiv_frac = (val >> ctrl->ndiv_frac.shift) &
+			bit_mask(ctrl->ndiv_frac.width);
+
+		if (ndiv_frac != 0)
+			ndiv = ((u64)ndiv_int << ctrl->ndiv_int.shift) |
+				ndiv_frac;
+	}
+
+	val = readl(pll->pll_base + ctrl->pdiv.offset);
+	pdiv = (val >> ctrl->pdiv.shift) & bit_mask(ctrl->pdiv.width);
+
+	clk->rate = (ndiv * parent_rate) >> ctrl->ndiv_int.shift;
+
+	if (pdiv == 0)
+		clk->rate *= 2;
+	else
+		clk->rate /= pdiv;
+
+	return clk->rate;
+}
+
+static long iproc_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long *parent_rate)
+{
+	unsigned i;
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	struct iproc_pll *pll = clk->pll;
+
+	if (rate == 0 || *parent_rate == 0 || !pll->vco_param)
+		return -EINVAL;
+
+	for (i = 0; i < pll->num_vco_entries; i++) {
+		if (rate <= pll->vco_param[i].rate)
+			break;
+	}
+
+	if (i == pll->num_vco_entries)
+		i--;
+
+	return pll->vco_param[i].rate;
+}
+
+static int iproc_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	struct iproc_pll *pll = clk->pll;
+	int rate_index, ret;
+
+	rate_index = pll_get_rate_index(pll, rate);
+	if (rate_index < 0)
+		return rate_index;
+
+	ret = pll_set_rate(clk, rate_index, parent_rate);
+	return ret;
+}
+
+static const struct clk_ops iproc_pll_ops = {
+	.enable = iproc_pll_enable,
+	.disable = iproc_pll_disable,
+	.recalc_rate = iproc_pll_recalc_rate,
+	.round_rate = iproc_pll_round_rate,
+	.set_rate = iproc_pll_set_rate,
+};
+
+static int iproc_clk_enable(struct clk_hw *hw)
+{
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	const struct iproc_clk_ctrl *ctrl = clk->ctrl;
+	struct iproc_pll *pll = clk->pll;
+	u32 val;
+
+	/* channel enable is active low */
+	val = readl(pll->pll_base + ctrl->enable.offset);
+	val &= ~(1 << ctrl->enable.enable_shift);
+	writel(val, pll->pll_base + ctrl->enable.offset);
+
+	/* also make sure channel is not held */
+	val = readl(pll->pll_base + ctrl->enable.offset);
+	val &= ~(1 << ctrl->enable.hold_shift);
+	writel(val, pll->pll_base + ctrl->enable.offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + ctrl->enable.offset);
+
+	return 0;
+}
+
+static void iproc_clk_disable(struct clk_hw *hw)
+{
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	const struct iproc_clk_ctrl *ctrl = clk->ctrl;
+	struct iproc_pll *pll = clk->pll;
+	u32 val;
+
+	if (ctrl->flags & IPROC_CLK_AON)
+		return;
+
+	val = readl(pll->pll_base + ctrl->enable.offset);
+	val |= 1 << ctrl->enable.enable_shift;
+	writel(val, pll->pll_base + ctrl->enable.offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + ctrl->enable.offset);
+}
+
+static unsigned long iproc_clk_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	const struct iproc_clk_ctrl *ctrl = clk->ctrl;
+	struct iproc_pll *pll = clk->pll;
+	u32 val;
+	unsigned int mdiv;
+
+	if (parent_rate == 0)
+		return 0;
+
+	val = readl(pll->pll_base + ctrl->mdiv.offset);
+	mdiv = (val >> ctrl->mdiv.shift) & bit_mask(ctrl->mdiv.width);
+	if (mdiv == 0)
+		mdiv = 256;
+
+	clk->rate = parent_rate / mdiv;
+
+	return clk->rate;
+}
+
+static long iproc_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *parent_rate)
+{
+	unsigned int div;
+
+	if (rate == 0 || *parent_rate == 0)
+		return -EINVAL;
+
+	if (rate == *parent_rate)
+		return *parent_rate;
+
+	div = DIV_ROUND_UP(*parent_rate, rate);
+	if (div < 2)
+		return *parent_rate;
+
+	if (div > 256)
+		div = 256;
+
+	return *parent_rate / div;
+}
+
+static int iproc_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct iproc_clk *clk = to_iproc_clk(hw);
+	const struct iproc_clk_ctrl *ctrl = clk->ctrl;
+	struct iproc_pll *pll = clk->pll;
+	u32 val;
+	unsigned int div;
+
+	if (rate == 0 || parent_rate == 0)
+		return -EINVAL;
+
+	div = DIV_ROUND_UP(parent_rate, rate);
+	if (div > 256)
+		return -EINVAL;
+
+	val = readl(pll->pll_base + ctrl->mdiv.offset);
+	if (div == 256) {
+		val &= ~(bit_mask(ctrl->mdiv.width) << ctrl->mdiv.shift);
+	} else {
+		val &= ~(bit_mask(ctrl->mdiv.width) << ctrl->mdiv.shift);
+		val |= div << ctrl->mdiv.shift;
+	}
+	writel(val, pll->pll_base + ctrl->mdiv.offset);
+	if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+		readl(pll->pll_base + ctrl->mdiv.offset);
+	clk->rate = parent_rate / div;
+
+	return 0;
+}
+
+static const struct clk_ops iproc_clk_ops = {
+	.enable = iproc_clk_enable,
+	.disable = iproc_clk_disable,
+	.recalc_rate = iproc_clk_recalc_rate,
+	.round_rate = iproc_clk_round_rate,
+	.set_rate = iproc_clk_set_rate,
+};
+
+/**
+ * Some PLLs require the PLL SW override bit to be set before changes can be
+ * applied to the PLL
+ */
+static void iproc_pll_sw_cfg(struct iproc_pll *pll)
+{
+	const struct iproc_pll_ctrl *ctrl = pll->ctrl;
+
+	if (ctrl->flags & IPROC_CLK_PLL_NEEDS_SW_CFG) {
+		u32 val;
+
+		val = readl(pll->pll_base + ctrl->sw_ctrl.offset);
+		val |= BIT(ctrl->sw_ctrl.shift);
+		writel(val, pll->pll_base + ctrl->sw_ctrl.offset);
+		if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK))
+			readl(pll->pll_base + ctrl->sw_ctrl.offset);
+	}
+}
+
+void __init iproc_pll_clk_setup(struct device_node *node,
+				const struct iproc_pll_ctrl *pll_ctrl,
+				const struct iproc_pll_vco_param *vco,
+				unsigned int num_vco_entries,
+				const struct iproc_clk_ctrl *clk_ctrl,
+				unsigned int num_clks)
+{
+	int i, ret;
+	struct clk *clk;
+	struct iproc_pll *pll;
+	struct iproc_clk *iclk;
+	struct clk_init_data init;
+	const char *parent_name;
+
+	if (WARN_ON(!pll_ctrl) || WARN_ON(!clk_ctrl))
+		return;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (WARN_ON(!pll))
+		return;
+
+	pll->clk_data.clk_num = num_clks;
+	pll->clk_data.clks = kcalloc(num_clks, sizeof(*pll->clk_data.clks),
+				     GFP_KERNEL);
+	if (WARN_ON(!pll->clk_data.clks))
+		goto err_clk_data;
+
+	pll->clks = kcalloc(num_clks, sizeof(*pll->clks), GFP_KERNEL);
+	if (WARN_ON(!pll->clks))
+		goto err_clks;
+
+	pll->pll_base = of_iomap(node, 0);
+	if (WARN_ON(!pll->pll_base))
+		goto err_pll_iomap;
+
+	pll->pwr_base = of_iomap(node, 1);
+	if (WARN_ON(!pll->pwr_base))
+		goto err_pwr_iomap;
+
+	/* some PLLs require gating control at the top ASIU level */
+	if (pll_ctrl->flags & IPROC_CLK_PLL_ASIU) {
+		pll->asiu_base = of_iomap(node, 2);
+		if (WARN_ON(!pll->asiu_base))
+			goto err_asiu_iomap;
+	}
+
+	/* initialize and register the PLL itself */
+	pll->ctrl = pll_ctrl;
+
+	iclk = &pll->clks[0];
+	iclk->pll = pll;
+	iclk->name = node->name;
+
+	init.name = node->name;
+	init.ops = &iproc_pll_ops;
+	init.flags = 0;
+	parent_name = of_clk_get_parent_name(node, 0);
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+	iclk->hw.init = &init;
+
+	if (vco) {
+		pll->num_vco_entries = num_vco_entries;
+		pll->vco_param = vco;
+	}
+
+	iproc_pll_sw_cfg(pll);
+
+	clk = clk_register(NULL, &iclk->hw);
+	if (WARN_ON(IS_ERR(clk)))
+		goto err_pll_register;
+
+	pll->clk_data.clks[0] = clk;
+
+	/* now initialize and register all leaf clocks */
+	for (i = 1; i < num_clks; i++) {
+		const char *clk_name;
+
+		memset(&init, 0, sizeof(init));
+		parent_name = node->name;
+
+		ret = of_property_read_string_index(node, "clock-output-names",
+						    i, &clk_name);
+		if (WARN_ON(ret))
+			goto err_clk_register;
+
+		iclk = &pll->clks[i];
+		iclk->name = clk_name;
+		iclk->pll = pll;
+		iclk->ctrl = &clk_ctrl[i];
+
+		init.name = clk_name;
+		init.ops = &iproc_clk_ops;
+		init.flags = 0;
+		init.parent_names = (parent_name ? &parent_name : NULL);
+		init.num_parents = (parent_name ? 1 : 0);
+		iclk->hw.init = &init;
+
+		clk = clk_register(NULL, &iclk->hw);
+		if (WARN_ON(IS_ERR(clk)))
+			goto err_clk_register;
+
+		pll->clk_data.clks[i] = clk;
+	}
+
+	ret = of_clk_add_provider(node, of_clk_src_onecell_get, &pll->clk_data);
+	if (WARN_ON(ret))
+		goto err_clk_register;
+
+	return;
+
+err_clk_register:
+	for (i = 0; i < num_clks; i++)
+		clk_unregister(pll->clk_data.clks[i]);
+
+err_pll_register:
+	if (pll->asiu_base)
+		iounmap(pll->asiu_base);
+
+err_asiu_iomap:
+	iounmap(pll->pwr_base);
+
+err_pwr_iomap:
+	iounmap(pll->pll_base);
+
+err_pll_iomap:
+	kfree(pll->clks);
+
+err_clks:
+	kfree(pll->clk_data.clks);
+
+err_clk_data:
+	kfree(pll);
+}
diff --git a/drivers/clk/bcm/clk-iproc.h b/drivers/clk/bcm/clk-iproc.h
new file mode 100644
index 000000000000..d834b7abd5c6
--- /dev/null
+++ b/drivers/clk/bcm/clk-iproc.h
@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _CLK_IPROC_H
+#define _CLK_IPROC_H
+
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/of.h>
+#include <linux/clk-provider.h>
+
+#define IPROC_CLK_NAME_LEN 25
+#define IPROC_CLK_INVALID_OFFSET 0xffffffff
+#define bit_mask(width) ((1 << (width)) - 1)
+
+/* clocks that should not be disabled at runtime */
+#define IPROC_CLK_AON BIT(0)
+
+/* PLL that requires gating through ASIU */
+#define IPROC_CLK_PLL_ASIU BIT(1)
+
+/* PLL that has fractional part of the NDIV */
+#define IPROC_CLK_PLL_HAS_NDIV_FRAC BIT(2)
+
+/*
+ * Some of the iProc PLL/clocks may have an ASIC bug that requires read back
+ * of the same register following the write to flush the write transaction into
+ * the intended register
+ */
+#define IPROC_CLK_NEEDS_READ_BACK BIT(3)
+
+/*
+ * Some PLLs require the PLL SW override bit to be set before changes can be
+ * applied to the PLL
+ */
+#define IPROC_CLK_PLL_NEEDS_SW_CFG BIT(4)
+
+/*
+ * Parameters for VCO frequency configuration
+ *
+ * VCO frequency =
+ * ((ndiv_int + ndiv_frac / 2^20) * (ref freqeuncy  / pdiv)
+ */
+struct iproc_pll_vco_param {
+	unsigned long rate;
+	unsigned int ndiv_int;
+	unsigned int ndiv_frac;
+	unsigned int pdiv;
+};
+
+struct iproc_clk_reg_op {
+	unsigned int offset;
+	unsigned int shift;
+	unsigned int width;
+};
+
+/*
+ * Clock gating control at the top ASIU level
+ */
+struct iproc_asiu_gate {
+	unsigned int offset;
+	unsigned int en_shift;
+};
+
+/*
+ * Control of powering on/off of a PLL
+ *
+ * Before powering off a PLL, input isolation (ISO) needs to be enabled
+ */
+struct iproc_pll_aon_pwr_ctrl {
+	unsigned int offset;
+	unsigned int pwr_width;
+	unsigned int pwr_shift;
+	unsigned int iso_shift;
+};
+
+/*
+ * Control of the PLL reset, with Ki, Kp, and Ka parameters
+ */
+struct iproc_pll_reset_ctrl {
+	unsigned int offset;
+	unsigned int reset_shift;
+	unsigned int p_reset_shift;
+	unsigned int ki_shift;
+	unsigned int ki_width;
+	unsigned int kp_shift;
+	unsigned int kp_width;
+	unsigned int ka_shift;
+	unsigned int ka_width;
+};
+
+/*
+ * To enable SW control of the PLL
+ */
+struct iproc_pll_sw_ctrl {
+	unsigned int offset;
+	unsigned int shift;
+};
+
+struct iproc_pll_vco_ctrl {
+	unsigned int u_offset;
+	unsigned int l_offset;
+};
+
+/*
+ * Main PLL control parameters
+ */
+struct iproc_pll_ctrl {
+	unsigned long flags;
+	struct iproc_pll_aon_pwr_ctrl aon;
+	struct iproc_asiu_gate asiu;
+	struct iproc_pll_reset_ctrl reset;
+	struct iproc_pll_sw_ctrl sw_ctrl;
+	struct iproc_clk_reg_op ndiv_int;
+	struct iproc_clk_reg_op ndiv_frac;
+	struct iproc_clk_reg_op pdiv;
+	struct iproc_pll_vco_ctrl vco_ctrl;
+	struct iproc_clk_reg_op status;
+};
+
+/*
+ * Controls enabling/disabling a PLL derived clock
+ */
+struct iproc_clk_enable_ctrl {
+	unsigned int offset;
+	unsigned int enable_shift;
+	unsigned int hold_shift;
+	unsigned int bypass_shift;
+};
+
+/*
+ * Main clock control parameters for clocks derived from the PLLs
+ */
+struct iproc_clk_ctrl {
+	unsigned int channel;
+	unsigned long flags;
+	struct iproc_clk_enable_ctrl enable;
+	struct iproc_clk_reg_op mdiv;
+};
+
+/*
+ * Divisor of the ASIU clocks
+ */
+struct iproc_asiu_div {
+	unsigned int offset;
+	unsigned int en_shift;
+	unsigned int high_shift;
+	unsigned int high_width;
+	unsigned int low_shift;
+	unsigned int low_width;
+};
+
+void __init iproc_armpll_setup(struct device_node *node);
+void __init iproc_pll_clk_setup(struct device_node *node,
+				const struct iproc_pll_ctrl *pll_ctrl,
+				const struct iproc_pll_vco_param *vco,
+				unsigned int num_vco_entries,
+				const struct iproc_clk_ctrl *clk_ctrl,
+				unsigned int num_clks);
+void __init iproc_asiu_setup(struct device_node *node,
+			     const struct iproc_asiu_div *div,
+			     const struct iproc_asiu_gate *gate,
+			     unsigned int num_clks);
+
+#endif /* _CLK_IPROC_H */
diff --git a/drivers/clk/bcm/clk-kona-setup.c b/drivers/clk/bcm/clk-kona-setup.c
index e5aededdd322..deaa7f962b84 100644
--- a/drivers/clk/bcm/clk-kona-setup.c
+++ b/drivers/clk/bcm/clk-kona-setup.c
@@ -21,8 +21,6 @@
 #define selector_clear_exists(sel)	((sel)->width = 0)
 #define trigger_clear_exists(trig)	FLAG_CLEAR(trig, TRIG, EXISTS)
 
-LIST_HEAD(ccu_list);	/* The list of set up CCUs */
-
 /* Validity checking */
 
 static bool ccu_data_offsets_valid(struct ccu_data *ccu)
@@ -773,7 +771,6 @@ static void kona_ccu_teardown(struct ccu_data *ccu)
 
 	of_clk_del_provider(ccu->node);	/* safe if never added */
 	ccu_clks_teardown(ccu);
-	list_del(&ccu->links);
 	of_node_put(ccu->node);
 	ccu->node = NULL;
 	iounmap(ccu->base);
@@ -847,7 +844,6 @@ void __init kona_dt_ccu_setup(struct ccu_data *ccu,
 		goto out_err;
 	}
 	ccu->node = of_node_get(node);
-	list_add_tail(&ccu->links, &ccu_list);
 
 	/*
 	 * Set up each defined kona clock and save the result in
diff --git a/drivers/clk/bcm/clk-kona.c b/drivers/clk/bcm/clk-kona.c
index a0ef4f75d457..79a98506c433 100644
--- a/drivers/clk/bcm/clk-kona.c
+++ b/drivers/clk/bcm/clk-kona.c
@@ -1240,7 +1240,7 @@ static bool __kona_clk_init(struct kona_clk *bcm_clk)
 	default:
 		BUG();
 	}
-	return -EINVAL;
+	return false;
 }
 
 /* Set a CCU and all its clocks into their desired initial state */
diff --git a/drivers/clk/bcm/clk-kona.h b/drivers/clk/bcm/clk-kona.h
index 6849a64baf6d..906576ec97b6 100644
--- a/drivers/clk/bcm/clk-kona.h
+++ b/drivers/clk/bcm/clk-kona.h
@@ -480,7 +480,6 @@ struct ccu_data {
 	spinlock_t lock;	/* serialization lock */
 	bool write_enabled;	/* write access is currently enabled */
 	struct ccu_policy policy;
-	struct list_head links;	/* for ccu_list */
 	struct device_node *node;
 	struct clk_onecell_data clk_data;
 	const char *name;
@@ -492,7 +491,6 @@ struct ccu_data {
 #define KONA_CCU_COMMON(_prefix, _name, _ccuname)			    \
 	.name		= #_name "_ccu",				    \
 	.lock		= __SPIN_LOCK_UNLOCKED(_name ## _ccu_data.lock),    \
-	.links		= LIST_HEAD_INIT(_name ## _ccu_data.links),	    \
 	.clk_data	= {						    \
 		.clk_num = _prefix ## _ ## _ccuname ## _CCU_CLOCK_COUNT,    \
 	}
diff --git a/drivers/clk/berlin/berlin2-pll.c b/drivers/clk/berlin/berlin2-pll.c
index bdc506b03824..f4b8d324b083 100644
--- a/drivers/clk/berlin/berlin2-pll.c
+++ b/drivers/clk/berlin/berlin2-pll.c
@@ -25,14 +25,7 @@
 #include <asm/div64.h>
 
 #include "berlin2-div.h"
-
-struct berlin2_pll_map {
-	const u8 vcodiv[16];
-	u8 mult;
-	u8 fbdiv_shift;
-	u8 rfdiv_shift;
-	u8 divsel_shift;
-};
+#include "berlin2-pll.h"
 
 struct berlin2_pll {
 	struct clk_hw hw;
diff --git a/drivers/clk/berlin/bg2.c b/drivers/clk/berlin/bg2.c
index 515fb133495c..73153fc45ee9 100644
--- a/drivers/clk/berlin/bg2.c
+++ b/drivers/clk/berlin/bg2.c
@@ -502,12 +502,13 @@ static const struct berlin2_gate_data bg2_gates[] __initconst = {
 
 static void __init berlin2_clock_setup(struct device_node *np)
 {
+	struct device_node *parent_np = of_get_parent(np);
 	const char *parent_names[9];
 	struct clk *clk;
 	u8 avpll_flags = 0;
 	int n;
 
-	gbase = of_iomap(np, 0);
+	gbase = of_iomap(parent_np, 0);
 	if (!gbase)
 		return;
 
@@ -685,7 +686,5 @@ static void __init berlin2_clock_setup(struct device_node *np)
 bg2_fail:
 	iounmap(gbase);
 }
-CLK_OF_DECLARE(berlin2_clock, "marvell,berlin2-chip-ctrl",
-	       berlin2_clock_setup);
-CLK_OF_DECLARE(berlin2cd_clock, "marvell,berlin2cd-chip-ctrl",
+CLK_OF_DECLARE(berlin2_clk, "marvell,berlin2-clk",
 	       berlin2_clock_setup);
diff --git a/drivers/clk/berlin/bg2q.c b/drivers/clk/berlin/bg2q.c
index 440ef81ab15c..221f40c2b850 100644
--- a/drivers/clk/berlin/bg2q.c
+++ b/drivers/clk/berlin/bg2q.c
@@ -290,18 +290,19 @@ static const struct berlin2_gate_data bg2q_gates[] __initconst = {
 
 static void __init berlin2q_clock_setup(struct device_node *np)
 {
+	struct device_node *parent_np = of_get_parent(np);
 	const char *parent_names[9];
 	struct clk *clk;
 	int n;
 
-	gbase = of_iomap(np, 0);
+	gbase = of_iomap(parent_np, 0);
 	if (!gbase) {
 		pr_err("%s: Unable to map global base\n", np->full_name);
 		return;
 	}
 
 	/* BG2Q CPU PLL is not part of global registers */
-	cpupll_base = of_iomap(np, 1);
+	cpupll_base = of_iomap(parent_np, 1);
 	if (!cpupll_base) {
 		pr_err("%s: Unable to map cpupll base\n", np->full_name);
 		iounmap(gbase);
@@ -384,5 +385,5 @@ bg2q_fail:
 	iounmap(cpupll_base);
 	iounmap(gbase);
 }
-CLK_OF_DECLARE(berlin2q_clock, "marvell,berlin2q-chip-ctrl",
+CLK_OF_DECLARE(berlin2q_clk, "marvell,berlin2q-clk",
 	       berlin2q_clock_setup);
diff --git a/drivers/clk/clk-asm9260.c b/drivers/clk/clk-asm9260.c
index 88f4ff6916fe..90897af8d9f7 100644
--- a/drivers/clk/clk-asm9260.c
+++ b/drivers/clk/clk-asm9260.c
@@ -274,7 +274,7 @@ static void __init asm9260_acc_init(struct device_node *np)
 	u32 accuracy = 0;
 
 	base = of_io_request_and_map(np, 0, np->name);
-	if (!base)
+	if (IS_ERR(base))
 		panic("%s: unable to map resource", np->name);
 
 	/* register pll */
diff --git a/drivers/clk/clk-axm5516.c b/drivers/clk/clk-axm5516.c
index 0f6368ceec4c..c7c91a5ecf8b 100644
--- a/drivers/clk/clk-axm5516.c
+++ b/drivers/clk/clk-axm5516.c
@@ -556,7 +556,7 @@ static int axmclk_probe(struct platform_device *pdev)
 		return PTR_ERR(regmap);
 
 	num_clks = ARRAY_SIZE(axmclk_clocks);
-	pr_info("axmclk: supporting %u clocks\n", num_clks);
+	pr_info("axmclk: supporting %zu clocks\n", num_clks);
 	priv = devm_kzalloc(dev, sizeof(*priv) + sizeof(*priv->clks) * num_clks,
 			    GFP_KERNEL);
 	if (!priv)
diff --git a/drivers/clk/clk-cdce706.c b/drivers/clk/clk-cdce706.c
index b8e4f8a822e9..f01164fada5d 100644
--- a/drivers/clk/clk-cdce706.c
+++ b/drivers/clk/clk-cdce706.c
@@ -94,7 +94,7 @@ static const char * const cdce706_source_name[] = {
 	"clk_in0", "clk_in1",
 };
 
-static const char *cdce706_clkin_name[] = {
+static const char * const cdce706_clkin_name[] = {
 	"clk_in",
 };
 
@@ -102,7 +102,7 @@ static const char * const cdce706_pll_name[] = {
 	"pll1", "pll2", "pll3",
 };
 
-static const char *cdce706_divider_parent_name[] = {
+static const char * const cdce706_divider_parent_name[] = {
 	"clk_in", "pll1", "pll2", "pll2", "pll3",
 };
 
@@ -666,6 +666,7 @@ static int cdce706_probe(struct i2c_client *client,
 
 static int cdce706_remove(struct i2c_client *client)
 {
+	of_clk_del_provider(client->dev.of_node);
 	return 0;
 }
 
diff --git a/drivers/clk/clk-cdce925.c b/drivers/clk/clk-cdce925.c
new file mode 100644
index 000000000000..85fafb41e6ca
--- /dev/null
+++ b/drivers/clk/clk-cdce925.c
@@ -0,0 +1,749 @@
+/*
+ * Driver for TI Dual PLL CDCE925 clock synthesizer
+ *
+ * This driver always connects the Y1 to the input clock, Y2/Y3 to PLL1
+ * and Y4/Y5 to PLL2. PLL frequency is set on a first-come-first-serve
+ * basis. Clients can directly request any frequency that the chip can
+ * deliver using the standard clk framework. In addition, the device can
+ * be configured and activated via the devicetree.
+ *
+ * Copyright (C) 2014, Topic Embedded Products
+ * Licenced under GPL
+ */
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/gcd.h>
+
+/* The chip has 2 PLLs which can be routed through dividers to 5 outputs.
+ * Model this as 2 PLL clocks which are parents to the outputs.
+ */
+#define NUMBER_OF_PLLS	2
+#define NUMBER_OF_OUTPUTS	5
+
+#define CDCE925_REG_GLOBAL1	0x01
+#define CDCE925_REG_Y1SPIPDIVH	0x02
+#define CDCE925_REG_PDIVL	0x03
+#define CDCE925_REG_XCSEL	0x05
+/* PLL parameters start at 0x10, steps of 0x10 */
+#define CDCE925_OFFSET_PLL	0x10
+/* Add CDCE925_OFFSET_PLL * (pll) to these registers before sending */
+#define CDCE925_PLL_MUX_OUTPUTS	0x14
+#define CDCE925_PLL_MULDIV	0x18
+
+#define CDCE925_PLL_FREQUENCY_MIN	 80000000ul
+#define CDCE925_PLL_FREQUENCY_MAX	230000000ul
+struct clk_cdce925_chip;
+
+struct clk_cdce925_output {
+	struct clk_hw hw;
+	struct clk_cdce925_chip *chip;
+	u8 index;
+	u16 pdiv; /* 1..127 for Y2-Y5; 1..1023 for Y1 */
+};
+#define to_clk_cdce925_output(_hw) \
+	container_of(_hw, struct clk_cdce925_output, hw)
+
+struct clk_cdce925_pll {
+	struct clk_hw hw;
+	struct clk_cdce925_chip *chip;
+	u8 index;
+	u16 m;   /* 1..511 */
+	u16 n;   /* 1..4095 */
+};
+#define to_clk_cdce925_pll(_hw)	container_of(_hw, struct clk_cdce925_pll, hw)
+
+struct clk_cdce925_chip {
+	struct regmap *regmap;
+	struct i2c_client *i2c_client;
+	struct clk_cdce925_pll pll[NUMBER_OF_PLLS];
+	struct clk_cdce925_output clk[NUMBER_OF_OUTPUTS];
+	struct clk *dt_clk[NUMBER_OF_OUTPUTS];
+	struct clk_onecell_data onecell;
+};
+
+/* ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** ** */
+
+static unsigned long cdce925_pll_calculate_rate(unsigned long parent_rate,
+	u16 n, u16 m)
+{
+	if ((!m || !n) || (m == n))
+		return parent_rate; /* In bypass mode runs at same frequency */
+	return mult_frac(parent_rate, (unsigned long)n, (unsigned long)m);
+}
+
+static unsigned long cdce925_pll_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	/* Output frequency of PLL is Fout = (Fin/Pdiv)*(N/M) */
+	struct clk_cdce925_pll *data = to_clk_cdce925_pll(hw);
+
+	return cdce925_pll_calculate_rate(parent_rate, data->n, data->m);
+}
+
+static void cdce925_pll_find_rate(unsigned long rate,
+		unsigned long parent_rate, u16 *n, u16 *m)
+{
+	unsigned long un;
+	unsigned long um;
+	unsigned long g;
+
+	if (rate <= parent_rate) {
+		/* Can always deliver parent_rate in bypass mode */
+		rate = parent_rate;
+		*n = 0;
+		*m = 0;
+	} else {
+		/* In PLL mode, need to apply min/max range */
+		if (rate < CDCE925_PLL_FREQUENCY_MIN)
+			rate = CDCE925_PLL_FREQUENCY_MIN;
+		else if (rate > CDCE925_PLL_FREQUENCY_MAX)
+			rate = CDCE925_PLL_FREQUENCY_MAX;
+
+		g = gcd(rate, parent_rate);
+		um = parent_rate / g;
+		un = rate / g;
+		/* When outside hw range, reduce to fit (rounding errors) */
+		while ((un > 4095) || (um > 511)) {
+			un >>= 1;
+			um >>= 1;
+		}
+		if (un == 0)
+			un = 1;
+		if (um == 0)
+			um = 1;
+
+		*n = un;
+		*m = um;
+	}
+}
+
+static long cdce925_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *parent_rate)
+{
+	u16 n, m;
+
+	cdce925_pll_find_rate(rate, *parent_rate, &n, &m);
+	return (long)cdce925_pll_calculate_rate(*parent_rate, n, m);
+}
+
+static int cdce925_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_cdce925_pll *data = to_clk_cdce925_pll(hw);
+
+	if (!rate || (rate == parent_rate)) {
+		data->m = 0; /* Bypass mode */
+		data->n = 0;
+		return 0;
+	}
+
+	if ((rate < CDCE925_PLL_FREQUENCY_MIN) ||
+		(rate > CDCE925_PLL_FREQUENCY_MAX)) {
+		pr_debug("%s: rate %lu outside PLL range.\n", __func__, rate);
+		return -EINVAL;
+	}
+
+	if (rate < parent_rate) {
+		pr_debug("%s: rate %lu less than parent rate %lu.\n", __func__,
+			rate, parent_rate);
+		return -EINVAL;
+	}
+
+	cdce925_pll_find_rate(rate, parent_rate, &data->n, &data->m);
+	return 0;
+}
+
+
+/* calculate p = max(0, 4 - int(log2 (n/m))) */
+static u8 cdce925_pll_calc_p(u16 n, u16 m)
+{
+	u8 p;
+	u16 r = n / m;
+
+	if (r >= 16)
+		return 0;
+	p = 4;
+	while (r > 1) {
+		r >>= 1;
+		--p;
+	}
+	return p;
+}
+
+/* Returns VCO range bits for VCO1_0_RANGE */
+static u8 cdce925_pll_calc_range_bits(struct clk_hw *hw, u16 n, u16 m)
+{
+	struct clk *parent = clk_get_parent(hw->clk);
+	unsigned long rate = clk_get_rate(parent);
+
+	rate = mult_frac(rate, (unsigned long)n, (unsigned long)m);
+	if (rate >= 175000000)
+		return 0x3;
+	if (rate >= 150000000)
+		return 0x02;
+	if (rate >= 125000000)
+		return 0x01;
+	return 0x00;
+}
+
+/* I2C clock, hence everything must happen in (un)prepare because this
+ * may sleep */
+static int cdce925_pll_prepare(struct clk_hw *hw)
+{
+	struct clk_cdce925_pll *data = to_clk_cdce925_pll(hw);
+	u16 n = data->n;
+	u16 m = data->m;
+	u16 r;
+	u8 q;
+	u8 p;
+	u16 nn;
+	u8 pll[4]; /* Bits are spread out over 4 byte registers */
+	u8 reg_ofs = data->index * CDCE925_OFFSET_PLL;
+	unsigned i;
+
+	if ((!m || !n) || (m == n)) {
+		/* Set PLL mux to bypass mode, leave the rest as is */
+		regmap_update_bits(data->chip->regmap,
+			reg_ofs + CDCE925_PLL_MUX_OUTPUTS, 0x80, 0x80);
+	} else {
+		/* According to data sheet: */
+		/* p = max(0, 4 - int(log2 (n/m))) */
+		p = cdce925_pll_calc_p(n, m);
+		/* nn = n * 2^p */
+		nn = n * BIT(p);
+		/* q = int(nn/m) */
+		q = nn / m;
+		if ((q < 16) || (1 > 64)) {
+			pr_debug("%s invalid q=%d\n", __func__, q);
+			return -EINVAL;
+		}
+		r = nn - (m*q);
+		if (r > 511) {
+			pr_debug("%s invalid r=%d\n", __func__, r);
+			return -EINVAL;
+		}
+		pr_debug("%s n=%d m=%d p=%d q=%d r=%d\n", __func__,
+			n, m, p, q, r);
+		/* encode into register bits */
+		pll[0] = n >> 4;
+		pll[1] = ((n & 0x0F) << 4) | ((r >> 5) & 0x0F);
+		pll[2] = ((r & 0x1F) << 3) | ((q >> 3) & 0x07);
+		pll[3] = ((q & 0x07) << 5) | (p << 2) |
+				cdce925_pll_calc_range_bits(hw, n, m);
+		/* Write to registers */
+		for (i = 0; i < ARRAY_SIZE(pll); ++i)
+			regmap_write(data->chip->regmap,
+				reg_ofs + CDCE925_PLL_MULDIV + i, pll[i]);
+		/* Enable PLL */
+		regmap_update_bits(data->chip->regmap,
+			reg_ofs + CDCE925_PLL_MUX_OUTPUTS, 0x80, 0x00);
+	}
+
+	return 0;
+}
+
+static void cdce925_pll_unprepare(struct clk_hw *hw)
+{
+	struct clk_cdce925_pll *data = to_clk_cdce925_pll(hw);
+	u8 reg_ofs = data->index * CDCE925_OFFSET_PLL;
+
+	regmap_update_bits(data->chip->regmap,
+			reg_ofs + CDCE925_PLL_MUX_OUTPUTS, 0x80, 0x80);
+}
+
+static const struct clk_ops cdce925_pll_ops = {
+	.prepare = cdce925_pll_prepare,
+	.unprepare = cdce925_pll_unprepare,
+	.recalc_rate = cdce925_pll_recalc_rate,
+	.round_rate = cdce925_pll_round_rate,
+	.set_rate = cdce925_pll_set_rate,
+};
+
+
+static void cdce925_clk_set_pdiv(struct clk_cdce925_output *data, u16 pdiv)
+{
+	switch (data->index) {
+	case 0:
+		regmap_update_bits(data->chip->regmap,
+			CDCE925_REG_Y1SPIPDIVH,
+			0x03, (pdiv >> 8) & 0x03);
+		regmap_write(data->chip->regmap, 0x03, pdiv & 0xFF);
+		break;
+	case 1:
+		regmap_update_bits(data->chip->regmap, 0x16, 0x7F, pdiv);
+		break;
+	case 2:
+		regmap_update_bits(data->chip->regmap, 0x17, 0x7F, pdiv);
+		break;
+	case 3:
+		regmap_update_bits(data->chip->regmap, 0x26, 0x7F, pdiv);
+		break;
+	case 4:
+		regmap_update_bits(data->chip->regmap, 0x27, 0x7F, pdiv);
+		break;
+	}
+}
+
+static void cdce925_clk_activate(struct clk_cdce925_output *data)
+{
+	switch (data->index) {
+	case 0:
+		regmap_update_bits(data->chip->regmap,
+			CDCE925_REG_Y1SPIPDIVH, 0x0c, 0x0c);
+		break;
+	case 1:
+	case 2:
+		regmap_update_bits(data->chip->regmap, 0x14, 0x03, 0x03);
+		break;
+	case 3:
+	case 4:
+		regmap_update_bits(data->chip->regmap, 0x24, 0x03, 0x03);
+		break;
+	}
+}
+
+static int cdce925_clk_prepare(struct clk_hw *hw)
+{
+	struct clk_cdce925_output *data = to_clk_cdce925_output(hw);
+
+	cdce925_clk_set_pdiv(data, data->pdiv);
+	cdce925_clk_activate(data);
+	return 0;
+}
+
+static void cdce925_clk_unprepare(struct clk_hw *hw)
+{
+	struct clk_cdce925_output *data = to_clk_cdce925_output(hw);
+
+	/* Disable clock by setting divider to "0" */
+	cdce925_clk_set_pdiv(data, 0);
+}
+
+static unsigned long cdce925_clk_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_cdce925_output *data = to_clk_cdce925_output(hw);
+
+	if (data->pdiv)
+		return parent_rate / data->pdiv;
+	return 0;
+}
+
+static u16 cdce925_calc_divider(unsigned long rate,
+		unsigned long parent_rate)
+{
+	unsigned long divider;
+
+	if (!rate)
+		return 0;
+	if (rate >= parent_rate)
+		return 1;
+
+	divider = DIV_ROUND_CLOSEST(parent_rate, rate);
+	if (divider > 0x7F)
+		divider = 0x7F;
+
+	return (u16)divider;
+}
+
+static unsigned long cdce925_clk_best_parent_rate(
+	struct clk_hw *hw, unsigned long rate)
+{
+	struct clk *pll = clk_get_parent(hw->clk);
+	struct clk *root = clk_get_parent(pll);
+	unsigned long root_rate = clk_get_rate(root);
+	unsigned long best_rate_error = rate;
+	u16 pdiv_min;
+	u16 pdiv_max;
+	u16 pdiv_best;
+	u16 pdiv_now;
+
+	if (root_rate % rate == 0)
+		return root_rate; /* Don't need the PLL, use bypass */
+
+	pdiv_min = (u16)max(1ul, DIV_ROUND_UP(CDCE925_PLL_FREQUENCY_MIN, rate));
+	pdiv_max = (u16)min(127ul, CDCE925_PLL_FREQUENCY_MAX / rate);
+
+	if (pdiv_min > pdiv_max)
+		return 0; /* No can do? */
+
+	pdiv_best = pdiv_min;
+	for (pdiv_now = pdiv_min; pdiv_now < pdiv_max; ++pdiv_now) {
+		unsigned long target_rate = rate * pdiv_now;
+		long pll_rate = clk_round_rate(pll, target_rate);
+		unsigned long actual_rate;
+		unsigned long rate_error;
+
+		if (pll_rate <= 0)
+			continue;
+		actual_rate = pll_rate / pdiv_now;
+		rate_error = abs((long)actual_rate - (long)rate);
+		if (rate_error < best_rate_error) {
+			pdiv_best = pdiv_now;
+			best_rate_error = rate_error;
+		}
+		/* TODO: Consider PLL frequency based on smaller n/m values
+		 * and pick the better one if the error is equal */
+	}
+
+	return rate * pdiv_best;
+}
+
+static long cdce925_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *parent_rate)
+{
+	unsigned long l_parent_rate = *parent_rate;
+	u16 divider = cdce925_calc_divider(rate, l_parent_rate);
+
+	if (l_parent_rate / divider != rate) {
+		l_parent_rate = cdce925_clk_best_parent_rate(hw, rate);
+		divider = cdce925_calc_divider(rate, l_parent_rate);
+		*parent_rate = l_parent_rate;
+	}
+
+	if (divider)
+		return (long)(l_parent_rate / divider);
+	return 0;
+}
+
+static int cdce925_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_cdce925_output *data = to_clk_cdce925_output(hw);
+
+	data->pdiv = cdce925_calc_divider(rate, parent_rate);
+
+	return 0;
+}
+
+static const struct clk_ops cdce925_clk_ops = {
+	.prepare = cdce925_clk_prepare,
+	.unprepare = cdce925_clk_unprepare,
+	.recalc_rate = cdce925_clk_recalc_rate,
+	.round_rate = cdce925_clk_round_rate,
+	.set_rate = cdce925_clk_set_rate,
+};
+
+
+static u16 cdce925_y1_calc_divider(unsigned long rate,
+		unsigned long parent_rate)
+{
+	unsigned long divider;
+
+	if (!rate)
+		return 0;
+	if (rate >= parent_rate)
+		return 1;
+
+	divider = DIV_ROUND_CLOSEST(parent_rate, rate);
+	if (divider > 0x3FF) /* Y1 has 10-bit divider */
+		divider = 0x3FF;
+
+	return (u16)divider;
+}
+
+static long cdce925_clk_y1_round_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *parent_rate)
+{
+	unsigned long l_parent_rate = *parent_rate;
+	u16 divider = cdce925_y1_calc_divider(rate, l_parent_rate);
+
+	if (divider)
+		return (long)(l_parent_rate / divider);
+	return 0;
+}
+
+static int cdce925_clk_y1_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_cdce925_output *data = to_clk_cdce925_output(hw);
+
+	data->pdiv = cdce925_y1_calc_divider(rate, parent_rate);
+
+	return 0;
+}
+
+static const struct clk_ops cdce925_clk_y1_ops = {
+	.prepare = cdce925_clk_prepare,
+	.unprepare = cdce925_clk_unprepare,
+	.recalc_rate = cdce925_clk_recalc_rate,
+	.round_rate = cdce925_clk_y1_round_rate,
+	.set_rate = cdce925_clk_y1_set_rate,
+};
+
+
+static struct regmap_config cdce925_regmap_config = {
+	.name = "configuration0",
+	.reg_bits = 8,
+	.val_bits = 8,
+	.cache_type = REGCACHE_RBTREE,
+	.max_register = 0x2F,
+};
+
+#define CDCE925_I2C_COMMAND_BLOCK_TRANSFER	0x00
+#define CDCE925_I2C_COMMAND_BYTE_TRANSFER	0x80
+
+static int cdce925_regmap_i2c_write(
+	void *context, const void *data, size_t count)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+	int ret;
+	u8 reg_data[2];
+
+	if (count != 2)
+		return -ENOTSUPP;
+
+	/* First byte is command code */
+	reg_data[0] = CDCE925_I2C_COMMAND_BYTE_TRANSFER | ((u8 *)data)[0];
+	reg_data[1] = ((u8 *)data)[1];
+
+	dev_dbg(&i2c->dev, "%s(%zu) %#x %#x\n", __func__, count,
+			reg_data[0], reg_data[1]);
+
+	ret = i2c_master_send(i2c, reg_data, count);
+	if (likely(ret == count))
+		return 0;
+	else if (ret < 0)
+		return ret;
+	else
+		return -EIO;
+}
+
+static int cdce925_regmap_i2c_read(void *context,
+	   const void *reg, size_t reg_size, void *val, size_t val_size)
+{
+	struct device *dev = context;
+	struct i2c_client *i2c = to_i2c_client(dev);
+	struct i2c_msg xfer[2];
+	int ret;
+	u8 reg_data[2];
+
+	if (reg_size != 1)
+		return -ENOTSUPP;
+
+	xfer[0].addr = i2c->addr;
+	xfer[0].flags = 0;
+	xfer[0].buf = reg_data;
+	if (val_size == 1) {
+		reg_data[0] =
+			CDCE925_I2C_COMMAND_BYTE_TRANSFER | ((u8 *)reg)[0];
+		xfer[0].len = 1;
+	} else {
+		reg_data[0] =
+			CDCE925_I2C_COMMAND_BLOCK_TRANSFER | ((u8 *)reg)[0];
+		reg_data[1] = val_size;
+		xfer[0].len = 2;
+	}
+
+	xfer[1].addr = i2c->addr;
+	xfer[1].flags = I2C_M_RD;
+	xfer[1].len = val_size;
+	xfer[1].buf = val;
+
+	ret = i2c_transfer(i2c->adapter, xfer, 2);
+	if (likely(ret == 2)) {
+		dev_dbg(&i2c->dev, "%s(%zu, %zu) %#x %#x\n", __func__,
+				reg_size, val_size, reg_data[0], *((u8 *)val));
+		return 0;
+	} else if (ret < 0)
+		return ret;
+	else
+		return -EIO;
+}
+
+/* The CDCE925 uses a funky way to read/write registers. Bulk mode is
+ * just weird, so just use the single byte mode exclusively. */
+static struct regmap_bus regmap_cdce925_bus = {
+	.write = cdce925_regmap_i2c_write,
+	.read = cdce925_regmap_i2c_read,
+};
+
+static int cdce925_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct clk_cdce925_chip *data;
+	struct device_node *node = client->dev.of_node;
+	const char *parent_name;
+	const char *pll_clk_name[NUMBER_OF_PLLS] = {NULL,};
+	struct clk_init_data init;
+	struct clk *clk;
+	u32 value;
+	int i;
+	int err;
+	struct device_node *np_output;
+	char child_name[6];
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+	data = devm_kzalloc(&client->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->i2c_client = client;
+	data->regmap = devm_regmap_init(&client->dev, &regmap_cdce925_bus,
+			&client->dev, &cdce925_regmap_config);
+	if (IS_ERR(data->regmap)) {
+		dev_err(&client->dev, "failed to allocate register map\n");
+		return PTR_ERR(data->regmap);
+	}
+	i2c_set_clientdata(client, data);
+
+	parent_name = of_clk_get_parent_name(node, 0);
+	if (!parent_name) {
+		dev_err(&client->dev, "missing parent clock\n");
+		return -ENODEV;
+	}
+	dev_dbg(&client->dev, "parent is: %s\n", parent_name);
+
+	if (of_property_read_u32(node, "xtal-load-pf", &value) == 0)
+		regmap_write(data->regmap,
+			CDCE925_REG_XCSEL, (value << 3) & 0xF8);
+	/* PWDN bit */
+	regmap_update_bits(data->regmap, CDCE925_REG_GLOBAL1, BIT(4), 0);
+
+	/* Set input source for Y1 to be the XTAL */
+	regmap_update_bits(data->regmap, 0x02, BIT(7), 0);
+
+	init.ops = &cdce925_pll_ops;
+	init.flags = 0;
+	init.parent_names = &parent_name;
+	init.num_parents = parent_name ? 1 : 0;
+
+	/* Register PLL clocks */
+	for (i = 0; i < NUMBER_OF_PLLS; ++i) {
+		pll_clk_name[i] = kasprintf(GFP_KERNEL, "%s.pll%d",
+			client->dev.of_node->name, i);
+		init.name = pll_clk_name[i];
+		data->pll[i].chip = data;
+		data->pll[i].hw.init = &init;
+		data->pll[i].index = i;
+		clk = devm_clk_register(&client->dev, &data->pll[i].hw);
+		if (IS_ERR(clk)) {
+			dev_err(&client->dev, "Failed register PLL %d\n", i);
+			err = PTR_ERR(clk);
+			goto error;
+		}
+		sprintf(child_name, "PLL%d", i+1);
+		np_output = of_get_child_by_name(node, child_name);
+		if (!np_output)
+			continue;
+		if (!of_property_read_u32(np_output,
+			"clock-frequency", &value)) {
+			err = clk_set_rate(clk, value);
+			if (err)
+				dev_err(&client->dev,
+					"unable to set PLL frequency %ud\n",
+					value);
+		}
+		if (!of_property_read_u32(np_output,
+			"spread-spectrum", &value)) {
+			u8 flag = of_property_read_bool(np_output,
+				"spread-spectrum-center") ? 0x80 : 0x00;
+			regmap_update_bits(data->regmap,
+				0x16 + (i*CDCE925_OFFSET_PLL),
+				0x80, flag);
+			regmap_update_bits(data->regmap,
+				0x12 + (i*CDCE925_OFFSET_PLL),
+				0x07, value & 0x07);
+		}
+	}
+
+	/* Register output clock Y1 */
+	init.ops = &cdce925_clk_y1_ops;
+	init.flags = 0;
+	init.num_parents = 1;
+	init.parent_names = &parent_name; /* Mux Y1 to input */
+	init.name = kasprintf(GFP_KERNEL, "%s.Y1", client->dev.of_node->name);
+	data->clk[0].chip = data;
+	data->clk[0].hw.init = &init;
+	data->clk[0].index = 0;
+	data->clk[0].pdiv = 1;
+	clk = devm_clk_register(&client->dev, &data->clk[0].hw);
+	kfree(init.name); /* clock framework made a copy of the name */
+	if (IS_ERR(clk)) {
+		dev_err(&client->dev, "clock registration Y1 failed\n");
+		err = PTR_ERR(clk);
+		goto error;
+	}
+	data->dt_clk[0] = clk;
+
+	/* Register output clocks Y2 .. Y5*/
+	init.ops = &cdce925_clk_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.num_parents = 1;
+	for (i = 1; i < NUMBER_OF_OUTPUTS; ++i) {
+		init.name = kasprintf(GFP_KERNEL, "%s.Y%d",
+			client->dev.of_node->name, i+1);
+		data->clk[i].chip = data;
+		data->clk[i].hw.init = &init;
+		data->clk[i].index = i;
+		data->clk[i].pdiv = 1;
+		switch (i) {
+		case 1:
+		case 2:
+			/* Mux Y2/3 to PLL1 */
+			init.parent_names = &pll_clk_name[0];
+			break;
+		case 3:
+		case 4:
+			/* Mux Y4/5 to PLL2 */
+			init.parent_names = &pll_clk_name[1];
+			break;
+		}
+		clk = devm_clk_register(&client->dev, &data->clk[i].hw);
+		kfree(init.name); /* clock framework made a copy of the name */
+		if (IS_ERR(clk)) {
+			dev_err(&client->dev, "clock registration failed\n");
+			err = PTR_ERR(clk);
+			goto error;
+		}
+		data->dt_clk[i] = clk;
+	}
+
+	/* Register the output clocks */
+	data->onecell.clk_num = NUMBER_OF_OUTPUTS;
+	data->onecell.clks = data->dt_clk;
+	err = of_clk_add_provider(client->dev.of_node, of_clk_src_onecell_get,
+		&data->onecell);
+	if (err)
+		dev_err(&client->dev, "unable to add OF clock provider\n");
+
+	err = 0;
+
+error:
+	for (i = 0; i < NUMBER_OF_PLLS; ++i)
+		/* clock framework made a copy of the name */
+		kfree(pll_clk_name[i]);
+
+	return err;
+}
+
+static const struct i2c_device_id cdce925_id[] = {
+	{ "cdce925", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cdce925_id);
+
+static const struct of_device_id clk_cdce925_of_match[] = {
+	{ .compatible = "ti,cdce925" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, clk_cdce925_of_match);
+
+static struct i2c_driver cdce925_driver = {
+	.driver = {
+		.name = "cdce925",
+		.of_match_table = of_match_ptr(clk_cdce925_of_match),
+	},
+	.probe		= cdce925_probe,
+	.id_table	= cdce925_id,
+};
+module_i2c_driver(cdce925_driver);
+
+MODULE_AUTHOR("Mike Looijmans <mike.looijmans@topic.nl>");
+MODULE_DESCRIPTION("cdce925 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/clk/clk-composite.c b/drivers/clk/clk-composite.c
index 956b7e54fa1c..616f5aef3c26 100644
--- a/drivers/clk/clk-composite.c
+++ b/drivers/clk/clk-composite.c
@@ -188,7 +188,7 @@ static void clk_composite_disable(struct clk_hw *hw)
 }
 
 struct clk *clk_register_composite(struct device *dev, const char *name,
-			const char **parent_names, int num_parents,
+			const char * const *parent_names, int num_parents,
 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
@@ -200,10 +200,8 @@ struct clk *clk_register_composite(struct device *dev, const char *name,
 	struct clk_ops *clk_composite_ops;
 
 	composite = kzalloc(sizeof(*composite), GFP_KERNEL);
-	if (!composite) {
-		pr_err("%s: could not allocate composite clk\n", __func__);
+	if (!composite)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	init.name = name;
 	init.flags = flags | CLK_IS_BASIC;
diff --git a/drivers/clk/clk-conf.c b/drivers/clk/clk-conf.c
index 48a65b2b4027..43a218f35b19 100644
--- a/drivers/clk/clk-conf.c
+++ b/drivers/clk/clk-conf.c
@@ -106,8 +106,9 @@ static int __set_clk_rates(struct device_node *node, bool clk_supplier)
 
 			rc = clk_set_rate(clk, rate);
 			if (rc < 0)
-				pr_err("clk: couldn't set %s clock rate: %d\n",
-				       __clk_get_name(clk), rc);
+				pr_err("clk: couldn't set %s clk rate to %d (%d), current rate: %ld\n",
+				       __clk_get_name(clk), rate, rc,
+				       clk_get_rate(clk));
 			clk_put(clk);
 		}
 		index++;
@@ -124,7 +125,7 @@ static int __set_clk_rates(struct device_node *node, bool clk_supplier)
  * and sets any specified clock parents and rates. The @clk_supplier argument
  * should be set to true if @node may be also a clock supplier of any clock
  * listed in its 'assigned-clocks' or 'assigned-clock-parents' properties.
- * If @clk_supplier is false the function exits returnning 0 as soon as it
+ * If @clk_supplier is false the function exits returning 0 as soon as it
  * determines the @node is also a supplier of any of the clocks.
  */
 int of_clk_set_defaults(struct device_node *node, bool clk_supplier)
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index 25006a8bb8e6..706b5783c360 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -430,11 +430,9 @@ static struct clk *_register_divider(struct device *dev, const char *name,
 	}
 
 	/* allocate the divider */
-	div = kzalloc(sizeof(struct clk_divider), GFP_KERNEL);
-	if (!div) {
-		pr_err("%s: could not allocate divider clk\n", __func__);
+	div = kzalloc(sizeof(*div), GFP_KERNEL);
+	if (!div)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	init.name = name;
 	init.ops = &clk_divider_ops;
diff --git a/drivers/clk/clk-fixed-factor.c b/drivers/clk/clk-fixed-factor.c
index d9e3f671c2ea..fccabe497f6e 100644
--- a/drivers/clk/clk-fixed-factor.c
+++ b/drivers/clk/clk-fixed-factor.c
@@ -55,10 +55,16 @@ static long clk_factor_round_rate(struct clk_hw *hw, unsigned long rate,
 static int clk_factor_set_rate(struct clk_hw *hw, unsigned long rate,
 				unsigned long parent_rate)
 {
+	/*
+	 * We must report success but we can do so unconditionally because
+	 * clk_factor_round_rate returns values that ensure this call is a
+	 * nop.
+	 */
+
 	return 0;
 }
 
-struct clk_ops clk_fixed_factor_ops = {
+const struct clk_ops clk_fixed_factor_ops = {
 	.round_rate = clk_factor_round_rate,
 	.set_rate = clk_factor_set_rate,
 	.recalc_rate = clk_factor_recalc_rate,
@@ -74,10 +80,8 @@ struct clk *clk_register_fixed_factor(struct device *dev, const char *name,
 	struct clk *clk;
 
 	fix = kmalloc(sizeof(*fix), GFP_KERNEL);
-	if (!fix) {
-		pr_err("%s: could not allocate fixed factor clk\n", __func__);
+	if (!fix)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	/* struct clk_fixed_factor assignments */
 	fix->mult = mult;
diff --git a/drivers/clk/clk-fixed-rate.c b/drivers/clk/clk-fixed-rate.c
index 0fc56ab6e844..f85ec8d1711f 100644
--- a/drivers/clk/clk-fixed-rate.c
+++ b/drivers/clk/clk-fixed-rate.c
@@ -65,11 +65,9 @@ struct clk *clk_register_fixed_rate_with_accuracy(struct device *dev,
 	struct clk_init_data init;
 
 	/* allocate fixed-rate clock */
-	fixed = kzalloc(sizeof(struct clk_fixed_rate), GFP_KERNEL);
-	if (!fixed) {
-		pr_err("%s: could not allocate fixed clk\n", __func__);
+	fixed = kzalloc(sizeof(*fixed), GFP_KERNEL);
+	if (!fixed)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	init.name = name;
 	init.ops = &clk_fixed_rate_ops;
diff --git a/drivers/clk/clk-fractional-divider.c b/drivers/clk/clk-fractional-divider.c
index 6aa72d9d79ba..140eb5844dc4 100644
--- a/drivers/clk/clk-fractional-divider.c
+++ b/drivers/clk/clk-fractional-divider.c
@@ -109,10 +109,8 @@ struct clk *clk_register_fractional_divider(struct device *dev,
 	struct clk *clk;
 
 	fd = kzalloc(sizeof(*fd), GFP_KERNEL);
-	if (!fd) {
-		dev_err(dev, "could not allocate fractional divider clk\n");
+	if (!fd)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	init.name = name;
 	init.ops = &clk_fractional_divider_ops;
diff --git a/drivers/clk/clk-gate.c b/drivers/clk/clk-gate.c
index 3f0e4200cb5d..551dd0672794 100644
--- a/drivers/clk/clk-gate.c
+++ b/drivers/clk/clk-gate.c
@@ -135,11 +135,9 @@ struct clk *clk_register_gate(struct device *dev, const char *name,
 	}
 
 	/* allocate the gate */
-	gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL);
-	if (!gate) {
-		pr_err("%s: could not allocate gated clk\n", __func__);
+	gate = kzalloc(sizeof(*gate), GFP_KERNEL);
+	if (!gate)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	init.name = name;
 	init.ops = &clk_gate_ops;
diff --git a/drivers/clk/clk-gpio-gate.c b/drivers/clk/clk-gpio-gate.c
index a71cabedda93..f564e624fb93 100644
--- a/drivers/clk/clk-gpio-gate.c
+++ b/drivers/clk/clk-gpio-gate.c
@@ -189,7 +189,7 @@ static struct clk *of_clk_gpio_gate_delayed_register_get(
 /**
  * of_gpio_gate_clk_setup() - Setup function for gpio controlled clock
  */
-void __init of_gpio_gate_clk_setup(struct device_node *node)
+static void __init of_gpio_gate_clk_setup(struct device_node *node)
 {
 	struct clk_gpio_gate_delayed_register_data *data;
 
@@ -203,6 +203,5 @@ void __init of_gpio_gate_clk_setup(struct device_node *node)
 
 	of_clk_add_provider(node, of_clk_gpio_gate_delayed_register_get, data);
 }
-EXPORT_SYMBOL_GPL(of_gpio_gate_clk_setup);
 CLK_OF_DECLARE(gpio_gate_clk, "gpio-gate-clock", of_gpio_gate_clk_setup);
 #endif
diff --git a/drivers/clk/clk-ls1x.c b/drivers/clk/clk-ls1x.c
index ca80103ac188..d4c61985f448 100644
--- a/drivers/clk/clk-ls1x.c
+++ b/drivers/clk/clk-ls1x.c
@@ -80,9 +80,9 @@ 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", };
+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)
 {
diff --git a/drivers/clk/clk-max-gen.c b/drivers/clk/clk-max-gen.c
index 6505049d50f1..35af9cb6da4f 100644
--- a/drivers/clk/clk-max-gen.c
+++ b/drivers/clk/clk-max-gen.c
@@ -31,6 +31,8 @@
 #include <linux/of.h>
 #include <linux/export.h>
 
+#include "clk-max-gen.h"
+
 struct max_gen_clk {
 	struct regmap *regmap;
 	u32 mask;
diff --git a/drivers/clk/clk-max77686.c b/drivers/clk/clk-max77686.c
index 86cdb3a28629..446c2fe76dc2 100644
--- a/drivers/clk/clk-max77686.c
+++ b/drivers/clk/clk-max77686.c
@@ -23,6 +23,7 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/mfd/max77686.h>
 #include <linux/mfd/max77686-private.h>
diff --git a/drivers/clk/clk-max77802.c b/drivers/clk/clk-max77802.c
index 0729dc723a8f..74c49b93a6eb 100644
--- a/drivers/clk/clk-max77802.c
+++ b/drivers/clk/clk-max77802.c
@@ -22,6 +22,7 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/mfd/max77686-private.h>
 #include <linux/clk-provider.h>
diff --git a/drivers/clk/clk-moxart.c b/drivers/clk/clk-moxart.c
index 30a3b6999e10..5181b89c3cb2 100644
--- a/drivers/clk/clk-moxart.c
+++ b/drivers/clk/clk-moxart.c
@@ -15,7 +15,7 @@
 #include <linux/of_address.h>
 #include <linux/clkdev.h>
 
-void __init moxart_of_pll_clk_init(struct device_node *node)
+static void __init moxart_of_pll_clk_init(struct device_node *node)
 {
 	static void __iomem *base;
 	struct clk *clk, *ref_clk;
@@ -53,7 +53,7 @@ void __init moxart_of_pll_clk_init(struct device_node *node)
 CLK_OF_DECLARE(moxart_pll_clock, "moxa,moxart-pll-clock",
 	       moxart_of_pll_clk_init);
 
-void __init moxart_of_apb_clk_init(struct device_node *node)
+static void __init moxart_of_apb_clk_init(struct device_node *node)
 {
 	static void __iomem *base;
 	struct clk *clk, *pll_clk;
diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index 69a094c3783d..6066a01b20ea 100644
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -114,7 +114,8 @@ const struct clk_ops clk_mux_ro_ops = {
 EXPORT_SYMBOL_GPL(clk_mux_ro_ops);
 
 struct clk *clk_register_mux_table(struct device *dev, const char *name,
-		const char **parent_names, u8 num_parents, unsigned long flags,
+		const char * const *parent_names, u8 num_parents,
+		unsigned long flags,
 		void __iomem *reg, u8 shift, u32 mask,
 		u8 clk_mux_flags, u32 *table, spinlock_t *lock)
 {
@@ -166,7 +167,8 @@ struct clk *clk_register_mux_table(struct device *dev, const char *name,
 EXPORT_SYMBOL_GPL(clk_register_mux_table);
 
 struct clk *clk_register_mux(struct device *dev, const char *name,
-		const char **parent_names, u8 num_parents, unsigned long flags,
+		const char * const *parent_names, u8 num_parents,
+		unsigned long flags,
 		void __iomem *reg, u8 shift, u8 width,
 		u8 clk_mux_flags, spinlock_t *lock)
 {
diff --git a/drivers/clk/clk-nomadik.c b/drivers/clk/clk-nomadik.c
index 05e04ce0f148..c9487179f25f 100644
--- a/drivers/clk/clk-nomadik.c
+++ b/drivers/clk/clk-nomadik.c
@@ -503,8 +503,7 @@ static int __init nomadik_src_clk_init_debugfs(void)
 			    NULL, NULL, &nomadik_src_clk_debugfs_ops);
 	return 0;
 }
-
-module_init(nomadik_src_clk_init_debugfs);
+device_initcall(nomadik_src_clk_init_debugfs);
 
 #endif
 
diff --git a/drivers/clk/clk-s2mps11.c b/drivers/clk/clk-s2mps11.c
index bfa1e64e267d..9b13a303d3f8 100644
--- a/drivers/clk/clk-s2mps11.c
+++ b/drivers/clk/clk-s2mps11.c
@@ -242,14 +242,12 @@ static int s2mps11_clk_probe(struct platform_device *pdev)
 			goto err_reg;
 		}
 
-		s2mps11_clk->lookup = clkdev_alloc(s2mps11_clk->clk,
+		s2mps11_clk->lookup = clkdev_create(s2mps11_clk->clk,
 					s2mps11_name(s2mps11_clk), NULL);
 		if (!s2mps11_clk->lookup) {
 			ret = -ENOMEM;
 			goto err_lup;
 		}
-
-		clkdev_add(s2mps11_clk->lookup);
 	}
 
 	for (i = 0; i < S2MPS11_CLKS_NUM; i++) {
diff --git a/drivers/clk/clk-si5351.c b/drivers/clk/clk-si5351.c
index 30335d3b99af..e39e1e680b3c 100644
--- a/drivers/clk/clk-si5351.c
+++ b/drivers/clk/clk-si5351.c
@@ -552,7 +552,8 @@ static const struct clk_ops si5351_pll_ops = {
  * MSx_P2[19:0] = 128 * b - c * floor(128 * b/c) = (128*b) mod c
  * MSx_P3[19:0] = c
  *
- * MS[6,7] are integer (P1) divide only, P2 = 0, P3 = 0
+ * MS[6,7] are integer (P1) divide only, P1 = divide value,
+ * P2 and P3 are not applicable
  *
  * for 150MHz < fOUT <= 160MHz:
  *
@@ -606,9 +607,6 @@ static unsigned long si5351_msynth_recalc_rate(struct clk_hw *hw,
 	if (!hwdata->params.valid)
 		si5351_read_parameters(hwdata->drvdata, reg, &hwdata->params);
 
-	if (hwdata->params.p3 == 0)
-		return parent_rate;
-
 	/*
 	 * multisync0-5: fOUT = (128 * P3 * fIN) / (P1*P3 + P2 + 512*P3)
 	 * multisync6-7: fOUT = fIN / P1
@@ -616,6 +614,8 @@ static unsigned long si5351_msynth_recalc_rate(struct clk_hw *hw,
 	rate = parent_rate;
 	if (hwdata->num > 5) {
 		m = hwdata->params.p1;
+	} else if (hwdata->params.p3 == 0) {
+		return parent_rate;
 	} else if ((si5351_reg_read(hwdata->drvdata, reg + 2) &
 		    SI5351_OUTPUT_CLK_DIVBY4) == SI5351_OUTPUT_CLK_DIVBY4) {
 		m = 4;
@@ -679,6 +679,16 @@ static long si5351_msynth_round_rate(struct clk_hw *hw, unsigned long rate,
 		c = 1;
 
 		*parent_rate = a * rate;
+	} else if (hwdata->num >= 6) {
+		/* determine the closest integer divider */
+		a = DIV_ROUND_CLOSEST(*parent_rate, rate);
+		if (a < SI5351_MULTISYNTH_A_MIN)
+			a = SI5351_MULTISYNTH_A_MIN;
+		if (a > SI5351_MULTISYNTH67_A_MAX)
+			a = SI5351_MULTISYNTH67_A_MAX;
+
+		b = 0;
+		c = 1;
 	} else {
 		unsigned long rfrac, denom;
 
@@ -692,9 +702,7 @@ static long si5351_msynth_round_rate(struct clk_hw *hw, unsigned long rate,
 		a = *parent_rate / rate;
 		if (a < SI5351_MULTISYNTH_A_MIN)
 			a = SI5351_MULTISYNTH_A_MIN;
-		if (hwdata->num >= 6 && a > SI5351_MULTISYNTH67_A_MAX)
-			a = SI5351_MULTISYNTH67_A_MAX;
-		else if (a > SI5351_MULTISYNTH_A_MAX)
+		if (a > SI5351_MULTISYNTH_A_MAX)
 			a = SI5351_MULTISYNTH_A_MAX;
 
 		/* find best approximation for b/c = fVCO mod fOUT */
@@ -723,6 +731,10 @@ static long si5351_msynth_round_rate(struct clk_hw *hw, unsigned long rate,
 		hwdata->params.p3 = 1;
 		hwdata->params.p2 = 0;
 		hwdata->params.p1 = 0;
+	} else if (hwdata->num >= 6) {
+		hwdata->params.p3 = 0;
+		hwdata->params.p2 = 0;
+		hwdata->params.p1 = a;
 	} else {
 		hwdata->params.p3  = c;
 		hwdata->params.p2  = (128 * b) % c;
diff --git a/drivers/clk/clk-stm32f4.c b/drivers/clk/clk-stm32f4.c
new file mode 100644
index 000000000000..3f6f7ad39490
--- /dev/null
+++ b/drivers/clk/clk-stm32f4.c
@@ -0,0 +1,380 @@
+/*
+ * Author: Daniel Thompson <daniel.thompson@linaro.org>
+ *
+ * Inspired by clk-asm9260.c .
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#define STM32F4_RCC_PLLCFGR		0x04
+#define STM32F4_RCC_CFGR		0x08
+#define STM32F4_RCC_AHB1ENR		0x30
+#define STM32F4_RCC_AHB2ENR		0x34
+#define STM32F4_RCC_AHB3ENR		0x38
+#define STM32F4_RCC_APB1ENR		0x40
+#define STM32F4_RCC_APB2ENR		0x44
+
+struct stm32f4_gate_data {
+	u8	offset;
+	u8	bit_idx;
+	const char *name;
+	const char *parent_name;
+	unsigned long flags;
+};
+
+static const struct stm32f4_gate_data stm32f4_gates[] __initconst = {
+	{ STM32F4_RCC_AHB1ENR,  0,	"gpioa",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  1,	"gpiob",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  2,	"gpioc",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  3,	"gpiod",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  4,	"gpioe",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  5,	"gpiof",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  6,	"gpiog",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  7,	"gpioh",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  8,	"gpioi",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR,  9,	"gpioj",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 10,	"gpiok",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 12,	"crc",		"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 18,	"bkpsra",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 20,	"ccmdatam",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 21,	"dma1",		"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 22,	"dma2",		"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 23,	"dma2d",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 25,	"ethmac",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 26,	"ethmactx",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 27,	"ethmacrx",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 28,	"ethmacptp",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 29,	"otghs",	"ahb_div" },
+	{ STM32F4_RCC_AHB1ENR, 30,	"otghsulpi",	"ahb_div" },
+
+	{ STM32F4_RCC_AHB2ENR,  0,	"dcmi",		"ahb_div" },
+	{ STM32F4_RCC_AHB2ENR,  4,	"cryp",		"ahb_div" },
+	{ STM32F4_RCC_AHB2ENR,  5,	"hash",		"ahb_div" },
+	{ STM32F4_RCC_AHB2ENR,  6,	"rng",		"pll48" },
+	{ STM32F4_RCC_AHB2ENR,  7,	"otgfs",	"pll48" },
+
+	{ STM32F4_RCC_AHB3ENR,  0,	"fmc",		"ahb_div",
+		CLK_IGNORE_UNUSED },
+
+	{ STM32F4_RCC_APB1ENR,  0,	"tim2",		"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR,  1,	"tim3",		"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR,  2,	"tim4",		"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR,  3,	"tim5",		"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR,  4,	"tim6",		"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR,  5,	"tim7",		"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR,  6,	"tim12",	"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR,  7,	"tim13",	"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR,  8,	"tim14",	"apb1_mul" },
+	{ STM32F4_RCC_APB1ENR, 11,	"wwdg",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 14,	"spi2",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 15,	"spi3",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 17,	"uart2",	"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 18,	"uart3",	"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 19,	"uart4",	"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 20,	"uart5",	"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 21,	"i2c1",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 22,	"i2c2",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 23,	"i2c3",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 25,	"can1",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 26,	"can2",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 28,	"pwr",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 29,	"dac",		"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 30,	"uart7",	"apb1_div" },
+	{ STM32F4_RCC_APB1ENR, 31,	"uart8",	"apb1_div" },
+
+	{ STM32F4_RCC_APB2ENR,  0,	"tim1",		"apb2_mul" },
+	{ STM32F4_RCC_APB2ENR,  1,	"tim8",		"apb2_mul" },
+	{ STM32F4_RCC_APB2ENR,  4,	"usart1",	"apb2_div" },
+	{ STM32F4_RCC_APB2ENR,  5,	"usart6",	"apb2_div" },
+	{ STM32F4_RCC_APB2ENR,  8,	"adc1",		"apb2_div" },
+	{ STM32F4_RCC_APB2ENR,  9,	"adc2",		"apb2_div" },
+	{ STM32F4_RCC_APB2ENR, 10,	"adc3",		"apb2_div" },
+	{ STM32F4_RCC_APB2ENR, 11,	"sdio",		"pll48" },
+	{ STM32F4_RCC_APB2ENR, 12,	"spi1",		"apb2_div" },
+	{ STM32F4_RCC_APB2ENR, 13,	"spi4",		"apb2_div" },
+	{ STM32F4_RCC_APB2ENR, 14,	"syscfg",	"apb2_div" },
+	{ STM32F4_RCC_APB2ENR, 16,	"tim9",		"apb2_mul" },
+	{ STM32F4_RCC_APB2ENR, 17,	"tim10",	"apb2_mul" },
+	{ STM32F4_RCC_APB2ENR, 18,	"tim11",	"apb2_mul" },
+	{ STM32F4_RCC_APB2ENR, 20,	"spi5",		"apb2_div" },
+	{ STM32F4_RCC_APB2ENR, 21,	"spi6",		"apb2_div" },
+	{ STM32F4_RCC_APB2ENR, 22,	"sai1",		"apb2_div" },
+	{ STM32F4_RCC_APB2ENR, 26,	"ltdc",		"apb2_div" },
+};
+
+/*
+ * MAX_CLKS is the maximum value in the enumeration below plus the combined
+ * hweight of stm32f42xx_gate_map (plus one).
+ */
+#define MAX_CLKS 74
+
+enum { SYSTICK, FCLK };
+
+/*
+ * This bitmask tells us which bit offsets (0..192) on STM32F4[23]xxx
+ * have gate bits associated with them. Its combined hweight is 71.
+ */
+static const u64 stm32f42xx_gate_map[] = { 0x000000f17ef417ffull,
+					   0x0000000000000001ull,
+					   0x04777f33f6fec9ffull };
+
+static struct clk *clks[MAX_CLKS];
+static DEFINE_SPINLOCK(stm32f4_clk_lock);
+static void __iomem *base;
+
+/*
+ * "Multiplier" device for APBx clocks.
+ *
+ * The APBx dividers are power-of-two dividers and, if *not* running in 1:1
+ * mode, they also tap out the one of the low order state bits to run the
+ * timers. ST datasheets represent this feature as a (conditional) clock
+ * multiplier.
+ */
+struct clk_apb_mul {
+	struct clk_hw hw;
+	u8 bit_idx;
+};
+
+#define to_clk_apb_mul(_hw) container_of(_hw, struct clk_apb_mul, hw)
+
+static unsigned long clk_apb_mul_recalc_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	struct clk_apb_mul *am = to_clk_apb_mul(hw);
+
+	if (readl(base + STM32F4_RCC_CFGR) & BIT(am->bit_idx))
+		return parent_rate * 2;
+
+	return parent_rate;
+}
+
+static long clk_apb_mul_round_rate(struct clk_hw *hw, unsigned long rate,
+				   unsigned long *prate)
+{
+	struct clk_apb_mul *am = to_clk_apb_mul(hw);
+	unsigned long mult = 1;
+
+	if (readl(base + STM32F4_RCC_CFGR) & BIT(am->bit_idx))
+		mult = 2;
+
+	if (__clk_get_flags(hw->clk) & CLK_SET_RATE_PARENT) {
+		unsigned long best_parent = rate / mult;
+
+		*prate =
+		    __clk_round_rate(__clk_get_parent(hw->clk), best_parent);
+	}
+
+	return *prate * mult;
+}
+
+static int clk_apb_mul_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	/*
+	 * We must report success but we can do so unconditionally because
+	 * clk_apb_mul_round_rate returns values that ensure this call is a
+	 * nop.
+	 */
+
+	return 0;
+}
+
+static const struct clk_ops clk_apb_mul_factor_ops = {
+	.round_rate = clk_apb_mul_round_rate,
+	.set_rate = clk_apb_mul_set_rate,
+	.recalc_rate = clk_apb_mul_recalc_rate,
+};
+
+static struct clk *clk_register_apb_mul(struct device *dev, const char *name,
+					const char *parent_name,
+					unsigned long flags, u8 bit_idx)
+{
+	struct clk_apb_mul *am;
+	struct clk_init_data init;
+	struct clk *clk;
+
+	am = kzalloc(sizeof(*am), GFP_KERNEL);
+	if (!am)
+		return ERR_PTR(-ENOMEM);
+
+	am->bit_idx = bit_idx;
+	am->hw.init = &init;
+
+	init.name = name;
+	init.ops = &clk_apb_mul_factor_ops;
+	init.flags = flags;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	clk = clk_register(dev, &am->hw);
+
+	if (IS_ERR(clk))
+		kfree(am);
+
+	return clk;
+}
+
+/*
+ * Decode current PLL state and (statically) model the state we inherit from
+ * the bootloader.
+ */
+static void stm32f4_rcc_register_pll(const char *hse_clk, const char *hsi_clk)
+{
+	unsigned long pllcfgr = readl(base + STM32F4_RCC_PLLCFGR);
+
+	unsigned long pllm   = pllcfgr & 0x3f;
+	unsigned long plln   = (pllcfgr >> 6) & 0x1ff;
+	unsigned long pllp   = BIT(((pllcfgr >> 16) & 3) + 1);
+	const char   *pllsrc = pllcfgr & BIT(22) ? hse_clk : hsi_clk;
+	unsigned long pllq   = (pllcfgr >> 24) & 0xf;
+
+	clk_register_fixed_factor(NULL, "vco", pllsrc, 0, plln, pllm);
+	clk_register_fixed_factor(NULL, "pll", "vco", 0, 1, pllp);
+	clk_register_fixed_factor(NULL, "pll48", "vco", 0, 1, pllq);
+}
+
+/*
+ * Converts the primary and secondary indices (as they appear in DT) to an
+ * offset into our struct clock array.
+ */
+static int stm32f4_rcc_lookup_clk_idx(u8 primary, u8 secondary)
+{
+	u64 table[ARRAY_SIZE(stm32f42xx_gate_map)];
+
+	if (primary == 1) {
+		if (WARN_ON(secondary > FCLK))
+			return -EINVAL;
+		return secondary;
+	}
+
+	memcpy(table, stm32f42xx_gate_map, sizeof(table));
+
+	/* only bits set in table can be used as indices */
+	if (WARN_ON(secondary >= BITS_PER_BYTE * sizeof(table) ||
+		    0 == (table[BIT_ULL_WORD(secondary)] &
+			  BIT_ULL_MASK(secondary))))
+		return -EINVAL;
+
+	/* mask out bits above our current index */
+	table[BIT_ULL_WORD(secondary)] &=
+	    GENMASK_ULL(secondary % BITS_PER_LONG_LONG, 0);
+
+	return FCLK + hweight64(table[0]) +
+	       (BIT_ULL_WORD(secondary) >= 1 ? hweight64(table[1]) : 0) +
+	       (BIT_ULL_WORD(secondary) >= 2 ? hweight64(table[2]) : 0);
+}
+
+static struct clk *
+stm32f4_rcc_lookup_clk(struct of_phandle_args *clkspec, void *data)
+{
+	int i = stm32f4_rcc_lookup_clk_idx(clkspec->args[0], clkspec->args[1]);
+
+	if (i < 0)
+		return ERR_PTR(-EINVAL);
+
+	return clks[i];
+}
+
+static const char *sys_parents[] __initdata =   { "hsi", NULL, "pll" };
+
+static const struct clk_div_table ahb_div_table[] = {
+	{ 0x0,   1 }, { 0x1,   1 }, { 0x2,   1 }, { 0x3,   1 },
+	{ 0x4,   1 }, { 0x5,   1 }, { 0x6,   1 }, { 0x7,   1 },
+	{ 0x8,   2 }, { 0x9,   4 }, { 0xa,   8 }, { 0xb,  16 },
+	{ 0xc,  64 }, { 0xd, 128 }, { 0xe, 256 }, { 0xf, 512 },
+	{ 0 },
+};
+
+static const struct clk_div_table apb_div_table[] = {
+	{ 0,  1 }, { 0,  1 }, { 0,  1 }, { 0,  1 },
+	{ 4,  2 }, { 5,  4 }, { 6,  8 }, { 7, 16 },
+	{ 0 },
+};
+
+static void __init stm32f4_rcc_init(struct device_node *np)
+{
+	const char *hse_clk;
+	int n;
+
+	base = of_iomap(np, 0);
+	if (!base) {
+		pr_err("%s: unable to map resource", np->name);
+		return;
+	}
+
+	hse_clk = of_clk_get_parent_name(np, 0);
+
+	clk_register_fixed_rate_with_accuracy(NULL, "hsi", NULL, 0,
+			16000000, 160000);
+	stm32f4_rcc_register_pll(hse_clk, "hsi");
+
+	sys_parents[1] = hse_clk;
+	clk_register_mux_table(
+	    NULL, "sys", sys_parents, ARRAY_SIZE(sys_parents), 0,
+	    base + STM32F4_RCC_CFGR, 0, 3, 0, NULL, &stm32f4_clk_lock);
+
+	clk_register_divider_table(NULL, "ahb_div", "sys",
+				   CLK_SET_RATE_PARENT, base + STM32F4_RCC_CFGR,
+				   4, 4, 0, ahb_div_table, &stm32f4_clk_lock);
+
+	clk_register_divider_table(NULL, "apb1_div", "ahb_div",
+				   CLK_SET_RATE_PARENT, base + STM32F4_RCC_CFGR,
+				   10, 3, 0, apb_div_table, &stm32f4_clk_lock);
+	clk_register_apb_mul(NULL, "apb1_mul", "apb1_div",
+			     CLK_SET_RATE_PARENT, 12);
+
+	clk_register_divider_table(NULL, "apb2_div", "ahb_div",
+				   CLK_SET_RATE_PARENT, base + STM32F4_RCC_CFGR,
+				   13, 3, 0, apb_div_table, &stm32f4_clk_lock);
+	clk_register_apb_mul(NULL, "apb2_mul", "apb2_div",
+			     CLK_SET_RATE_PARENT, 15);
+
+	clks[SYSTICK] = clk_register_fixed_factor(NULL, "systick", "ahb_div",
+						  0, 1, 8);
+	clks[FCLK] = clk_register_fixed_factor(NULL, "fclk", "ahb_div",
+					       0, 1, 1);
+
+	for (n = 0; n < ARRAY_SIZE(stm32f4_gates); n++) {
+		const struct stm32f4_gate_data *gd = &stm32f4_gates[n];
+		unsigned int secondary =
+		    8 * (gd->offset - STM32F4_RCC_AHB1ENR) + gd->bit_idx;
+		int idx = stm32f4_rcc_lookup_clk_idx(0, secondary);
+
+		if (idx < 0)
+			goto fail;
+
+		clks[idx] = clk_register_gate(
+		    NULL, gd->name, gd->parent_name, gd->flags,
+		    base + gd->offset, gd->bit_idx, 0, &stm32f4_clk_lock);
+
+		if (IS_ERR(clks[n])) {
+			pr_err("%s: Unable to register leaf clock %s\n",
+			       np->full_name, gd->name);
+			goto fail;
+		}
+	}
+
+	of_clk_add_provider(np, stm32f4_rcc_lookup_clk, NULL);
+	return;
+fail:
+	iounmap(base);
+}
+CLK_OF_DECLARE(stm32f4_rcc, "st,stm32f42xx-rcc", stm32f4_rcc_init);
diff --git a/drivers/clk/clk-u300.c b/drivers/clk/clk-u300.c
index 406bfc1375b2..18bf5e576b93 100644
--- a/drivers/clk/clk-u300.c
+++ b/drivers/clk/clk-u300.c
@@ -12,6 +12,7 @@
 #include <linux/clk-provider.h>
 #include <linux/spinlock.h>
 #include <linux/of.h>
+#include <linux/platform_data/clk-u300.h>
 
 /* APP side SYSCON registers */
 /* CLK Control Register 16bit (R/W) */
diff --git a/drivers/clk/clk-xgene.c b/drivers/clk/clk-xgene.c
index dd8a62d8f11f..f26b3ac36b27 100644
--- a/drivers/clk/clk-xgene.c
+++ b/drivers/clk/clk-xgene.c
@@ -42,12 +42,12 @@
 
 static DEFINE_SPINLOCK(clk_lock);
 
-static inline u32 xgene_clk_read(void *csr)
+static inline u32 xgene_clk_read(void __iomem *csr)
 {
 	return readl_relaxed(csr);
 }
 
-static inline void xgene_clk_write(u32 data, void *csr)
+static inline void xgene_clk_write(u32 data, void __iomem *csr)
 {
 	return writel_relaxed(data, csr);
 }
@@ -119,7 +119,7 @@ static unsigned long xgene_clk_pll_recalc_rate(struct clk_hw *hw,
 	return fvco / nout;
 }
 
-const struct clk_ops xgene_clk_pll_ops = {
+static const struct clk_ops xgene_clk_pll_ops = {
 	.is_enabled = xgene_clk_pll_is_enabled,
 	.recalc_rate = xgene_clk_pll_recalc_rate,
 };
@@ -167,7 +167,7 @@ static void xgene_pllclk_init(struct device_node *np, enum xgene_pll_type pll_ty
 {
         const char *clk_name = np->full_name;
         struct clk *clk;
-        void *reg;
+        void __iomem *reg;
 
         reg = of_iomap(np, 0);
         if (reg == NULL) {
@@ -222,20 +222,22 @@ static int xgene_clk_enable(struct clk_hw *hw)
 	struct xgene_clk *pclk = to_xgene_clk(hw);
 	unsigned long flags = 0;
 	u32 data;
+	phys_addr_t reg;
 
 	if (pclk->lock)
 		spin_lock_irqsave(pclk->lock, flags);
 
 	if (pclk->param.csr_reg != NULL) {
 		pr_debug("%s clock enabled\n", pclk->name);
+		reg = __pa(pclk->param.csr_reg);
 		/* First enable the clock */
 		data = xgene_clk_read(pclk->param.csr_reg +
 					pclk->param.reg_clk_offset);
 		data |= pclk->param.reg_clk_mask;
 		xgene_clk_write(data, pclk->param.csr_reg +
 					pclk->param.reg_clk_offset);
-		pr_debug("%s clock PADDR base 0x%016LX clk offset 0x%08X mask 0x%08X value 0x%08X\n",
-			pclk->name, __pa(pclk->param.csr_reg),
+		pr_debug("%s clock PADDR base %pa clk offset 0x%08X mask 0x%08X value 0x%08X\n",
+			pclk->name, &reg,
 			pclk->param.reg_clk_offset, pclk->param.reg_clk_mask,
 			data);
 
@@ -245,8 +247,8 @@ static int xgene_clk_enable(struct clk_hw *hw)
 		data &= ~pclk->param.reg_csr_mask;
 		xgene_clk_write(data, pclk->param.csr_reg +
 					pclk->param.reg_csr_offset);
-		pr_debug("%s CSR RESET PADDR base 0x%016LX csr offset 0x%08X mask 0x%08X value 0x%08X\n",
-			pclk->name, __pa(pclk->param.csr_reg),
+		pr_debug("%s CSR RESET PADDR base %pa csr offset 0x%08X mask 0x%08X value 0x%08X\n",
+			pclk->name, &reg,
 			pclk->param.reg_csr_offset, pclk->param.reg_csr_mask,
 			data);
 	}
@@ -386,7 +388,7 @@ static long xgene_clk_round_rate(struct clk_hw *hw, unsigned long rate,
 	return parent_rate / divider;
 }
 
-const struct clk_ops xgene_clk_ops = {
+static const struct clk_ops xgene_clk_ops = {
 	.enable = xgene_clk_enable,
 	.disable = xgene_clk_disable,
 	.is_enabled = xgene_clk_is_enabled,
@@ -456,7 +458,7 @@ static void __init xgene_devclk_init(struct device_node *np)
 	parameters.csr_reg = NULL;
 	parameters.divider_reg = NULL;
 	for (i = 0; i < 2; i++) {
-		void *map_res;
+		void __iomem *map_res;
 		rc = of_address_to_resource(np, i, &res);
 		if (rc != 0) {
 			if (i == 0) {
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index 5b0f41868b42..ddb4b541016f 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -21,6 +21,7 @@
 #include <linux/device.h>
 #include <linux/init.h>
 #include <linux/sched.h>
+#include <linux/clkdev.h>
 
 #include "clk.h"
 
@@ -37,13 +38,6 @@ static HLIST_HEAD(clk_root_list);
 static HLIST_HEAD(clk_orphan_list);
 static LIST_HEAD(clk_notifier_list);
 
-static long clk_core_get_accuracy(struct clk_core *clk);
-static unsigned long clk_core_get_rate(struct clk_core *clk);
-static int clk_core_get_phase(struct clk_core *clk);
-static bool clk_core_is_prepared(struct clk_core *clk);
-static bool clk_core_is_enabled(struct clk_core *clk);
-static struct clk_core *clk_core_lookup(const char *name);
-
 /***    private data structures    ***/
 
 struct clk_core {
@@ -68,11 +62,11 @@ struct clk_core {
 	int			phase;
 	struct hlist_head	children;
 	struct hlist_node	child_node;
-	struct hlist_node	debug_node;
 	struct hlist_head	clks;
 	unsigned int		notifier_count;
 #ifdef CONFIG_DEBUG_FS
 	struct dentry		*dentry;
+	struct hlist_node	debug_node;
 #endif
 	struct kref		ref;
 };
@@ -145,382 +139,71 @@ static void clk_enable_unlock(unsigned long flags)
 	spin_unlock_irqrestore(&enable_lock, flags);
 }
 
-/***        debugfs support        ***/
-
-#ifdef CONFIG_DEBUG_FS
-#include <linux/debugfs.h>
-
-static struct dentry *rootdir;
-static int inited = 0;
-static DEFINE_MUTEX(clk_debug_lock);
-static HLIST_HEAD(clk_debug_list);
-
-static struct hlist_head *all_lists[] = {
-	&clk_root_list,
-	&clk_orphan_list,
-	NULL,
-};
-
-static struct hlist_head *orphan_list[] = {
-	&clk_orphan_list,
-	NULL,
-};
-
-static void clk_summary_show_one(struct seq_file *s, struct clk_core *c,
-				 int level)
-{
-	if (!c)
-		return;
-
-	seq_printf(s, "%*s%-*s %11d %12d %11lu %10lu %-3d\n",
-		   level * 3 + 1, "",
-		   30 - level * 3, c->name,
-		   c->enable_count, c->prepare_count, clk_core_get_rate(c),
-		   clk_core_get_accuracy(c), clk_core_get_phase(c));
-}
-
-static void clk_summary_show_subtree(struct seq_file *s, struct clk_core *c,
-				     int level)
-{
-	struct clk_core *child;
-
-	if (!c)
-		return;
-
-	clk_summary_show_one(s, c, level);
-
-	hlist_for_each_entry(child, &c->children, child_node)
-		clk_summary_show_subtree(s, child, level + 1);
-}
-
-static int clk_summary_show(struct seq_file *s, void *data)
-{
-	struct clk_core *c;
-	struct hlist_head **lists = (struct hlist_head **)s->private;
-
-	seq_puts(s, "   clock                         enable_cnt  prepare_cnt        rate   accuracy   phase\n");
-	seq_puts(s, "----------------------------------------------------------------------------------------\n");
-
-	clk_prepare_lock();
-
-	for (; *lists; lists++)
-		hlist_for_each_entry(c, *lists, child_node)
-			clk_summary_show_subtree(s, c, 0);
-
-	clk_prepare_unlock();
-
-	return 0;
-}
-
-
-static int clk_summary_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, clk_summary_show, inode->i_private);
-}
-
-static const struct file_operations clk_summary_fops = {
-	.open		= clk_summary_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-static void clk_dump_one(struct seq_file *s, struct clk_core *c, int level)
-{
-	if (!c)
-		return;
-
-	seq_printf(s, "\"%s\": { ", c->name);
-	seq_printf(s, "\"enable_count\": %d,", c->enable_count);
-	seq_printf(s, "\"prepare_count\": %d,", c->prepare_count);
-	seq_printf(s, "\"rate\": %lu", clk_core_get_rate(c));
-	seq_printf(s, "\"accuracy\": %lu", clk_core_get_accuracy(c));
-	seq_printf(s, "\"phase\": %d", clk_core_get_phase(c));
-}
-
-static void clk_dump_subtree(struct seq_file *s, struct clk_core *c, int level)
-{
-	struct clk_core *child;
-
-	if (!c)
-		return;
-
-	clk_dump_one(s, c, level);
-
-	hlist_for_each_entry(child, &c->children, child_node) {
-		seq_printf(s, ",");
-		clk_dump_subtree(s, child, level + 1);
-	}
-
-	seq_printf(s, "}");
-}
-
-static int clk_dump(struct seq_file *s, void *data)
-{
-	struct clk_core *c;
-	bool first_node = true;
-	struct hlist_head **lists = (struct hlist_head **)s->private;
-
-	seq_printf(s, "{");
-
-	clk_prepare_lock();
-
-	for (; *lists; lists++) {
-		hlist_for_each_entry(c, *lists, child_node) {
-			if (!first_node)
-				seq_puts(s, ",");
-			first_node = false;
-			clk_dump_subtree(s, c, 0);
-		}
-	}
-
-	clk_prepare_unlock();
-
-	seq_printf(s, "}");
-	return 0;
-}
-
-
-static int clk_dump_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, clk_dump, inode->i_private);
-}
-
-static const struct file_operations clk_dump_fops = {
-	.open		= clk_dump_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-static int clk_debug_create_one(struct clk_core *clk, struct dentry *pdentry)
-{
-	struct dentry *d;
-	int ret = -ENOMEM;
-
-	if (!clk || !pdentry) {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	d = debugfs_create_dir(clk->name, pdentry);
-	if (!d)
-		goto out;
-
-	clk->dentry = d;
-
-	d = debugfs_create_u32("clk_rate", S_IRUGO, clk->dentry,
-			(u32 *)&clk->rate);
-	if (!d)
-		goto err_out;
-
-	d = debugfs_create_u32("clk_accuracy", S_IRUGO, clk->dentry,
-			(u32 *)&clk->accuracy);
-	if (!d)
-		goto err_out;
-
-	d = debugfs_create_u32("clk_phase", S_IRUGO, clk->dentry,
-			(u32 *)&clk->phase);
-	if (!d)
-		goto err_out;
-
-	d = debugfs_create_x32("clk_flags", S_IRUGO, clk->dentry,
-			(u32 *)&clk->flags);
-	if (!d)
-		goto err_out;
-
-	d = debugfs_create_u32("clk_prepare_count", S_IRUGO, clk->dentry,
-			(u32 *)&clk->prepare_count);
-	if (!d)
-		goto err_out;
-
-	d = debugfs_create_u32("clk_enable_count", S_IRUGO, clk->dentry,
-			(u32 *)&clk->enable_count);
-	if (!d)
-		goto err_out;
-
-	d = debugfs_create_u32("clk_notifier_count", S_IRUGO, clk->dentry,
-			(u32 *)&clk->notifier_count);
-	if (!d)
-		goto err_out;
-
-	if (clk->ops->debug_init) {
-		ret = clk->ops->debug_init(clk->hw, clk->dentry);
-		if (ret)
-			goto err_out;
-	}
-
-	ret = 0;
-	goto out;
-
-err_out:
-	debugfs_remove_recursive(clk->dentry);
-	clk->dentry = NULL;
-out:
-	return ret;
-}
-
-/**
- * clk_debug_register - add a clk node to the debugfs clk tree
- * @clk: the clk being added to the debugfs clk tree
- *
- * Dynamically adds a clk to the debugfs clk tree if debugfs has been
- * initialized.  Otherwise it bails out early since the debugfs clk tree
- * will be created lazily by clk_debug_init as part of a late_initcall.
- */
-static int clk_debug_register(struct clk_core *clk)
-{
-	int ret = 0;
-
-	mutex_lock(&clk_debug_lock);
-	hlist_add_head(&clk->debug_node, &clk_debug_list);
-
-	if (!inited)
-		goto unlock;
-
-	ret = clk_debug_create_one(clk, rootdir);
-unlock:
-	mutex_unlock(&clk_debug_lock);
-
-	return ret;
-}
-
- /**
- * clk_debug_unregister - remove a clk node from the debugfs clk tree
- * @clk: the clk being removed from the debugfs clk tree
- *
- * Dynamically removes a clk and all it's children clk nodes from the
- * debugfs clk tree if clk->dentry points to debugfs created by
- * clk_debug_register in __clk_init.
- */
-static void clk_debug_unregister(struct clk_core *clk)
+static bool clk_core_is_prepared(struct clk_core *core)
 {
-	mutex_lock(&clk_debug_lock);
-	hlist_del_init(&clk->debug_node);
-	debugfs_remove_recursive(clk->dentry);
-	clk->dentry = NULL;
-	mutex_unlock(&clk_debug_lock);
-}
-
-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 (hw->core->dentry)
-		d = debugfs_create_file(name, mode, hw->core->dentry, data,
-					fops);
+	/*
+	 * .is_prepared is optional for clocks that can prepare
+	 * fall back to software usage counter if it is missing
+	 */
+	if (!core->ops->is_prepared)
+		return core->prepare_count;
 
-	return d;
+	return core->ops->is_prepared(core->hw);
 }
-EXPORT_SYMBOL_GPL(clk_debugfs_add_file);
 
-/**
- * clk_debug_init - lazily create the debugfs clk tree visualization
- *
- * clks are often initialized very early during boot before memory can
- * be dynamically allocated and well before debugfs is setup.
- * clk_debug_init walks the clk tree hierarchy while holding
- * prepare_lock and creates the topology as part of a late_initcall,
- * thus insuring that clks initialized very early will still be
- * represented in the debugfs clk tree.  This function should only be
- * called once at boot-time, and all other clks added dynamically will
- * be done so with clk_debug_register.
- */
-static int __init clk_debug_init(void)
+static bool clk_core_is_enabled(struct clk_core *core)
 {
-	struct clk_core *clk;
-	struct dentry *d;
-
-	rootdir = debugfs_create_dir("clk", NULL);
-
-	if (!rootdir)
-		return -ENOMEM;
-
-	d = debugfs_create_file("clk_summary", S_IRUGO, rootdir, &all_lists,
-				&clk_summary_fops);
-	if (!d)
-		return -ENOMEM;
-
-	d = debugfs_create_file("clk_dump", S_IRUGO, rootdir, &all_lists,
-				&clk_dump_fops);
-	if (!d)
-		return -ENOMEM;
-
-	d = debugfs_create_file("clk_orphan_summary", S_IRUGO, rootdir,
-				&orphan_list, &clk_summary_fops);
-	if (!d)
-		return -ENOMEM;
-
-	d = debugfs_create_file("clk_orphan_dump", S_IRUGO, rootdir,
-				&orphan_list, &clk_dump_fops);
-	if (!d)
-		return -ENOMEM;
-
-	mutex_lock(&clk_debug_lock);
-	hlist_for_each_entry(clk, &clk_debug_list, debug_node)
-		clk_debug_create_one(clk, rootdir);
-
-	inited = 1;
-	mutex_unlock(&clk_debug_lock);
+	/*
+	 * .is_enabled is only mandatory for clocks that gate
+	 * fall back to software usage counter if .is_enabled is missing
+	 */
+	if (!core->ops->is_enabled)
+		return core->enable_count;
 
-	return 0;
-}
-late_initcall(clk_debug_init);
-#else
-static inline int clk_debug_register(struct clk_core *clk) { return 0; }
-static inline void clk_debug_reparent(struct clk_core *clk,
-				      struct clk_core *new_parent)
-{
+	return core->ops->is_enabled(core->hw);
 }
-static inline void clk_debug_unregister(struct clk_core *clk)
-{
-}
-#endif
 
-/* caller must hold prepare_lock */
-static void clk_unprepare_unused_subtree(struct clk_core *clk)
+static void clk_unprepare_unused_subtree(struct clk_core *core)
 {
 	struct clk_core *child;
 
 	lockdep_assert_held(&prepare_lock);
 
-	hlist_for_each_entry(child, &clk->children, child_node)
+	hlist_for_each_entry(child, &core->children, child_node)
 		clk_unprepare_unused_subtree(child);
 
-	if (clk->prepare_count)
+	if (core->prepare_count)
 		return;
 
-	if (clk->flags & CLK_IGNORE_UNUSED)
+	if (core->flags & CLK_IGNORE_UNUSED)
 		return;
 
-	if (clk_core_is_prepared(clk)) {
-		trace_clk_unprepare(clk);
-		if (clk->ops->unprepare_unused)
-			clk->ops->unprepare_unused(clk->hw);
-		else if (clk->ops->unprepare)
-			clk->ops->unprepare(clk->hw);
-		trace_clk_unprepare_complete(clk);
+	if (clk_core_is_prepared(core)) {
+		trace_clk_unprepare(core);
+		if (core->ops->unprepare_unused)
+			core->ops->unprepare_unused(core->hw);
+		else if (core->ops->unprepare)
+			core->ops->unprepare(core->hw);
+		trace_clk_unprepare_complete(core);
 	}
 }
 
-/* caller must hold prepare_lock */
-static void clk_disable_unused_subtree(struct clk_core *clk)
+static void clk_disable_unused_subtree(struct clk_core *core)
 {
 	struct clk_core *child;
 	unsigned long flags;
 
 	lockdep_assert_held(&prepare_lock);
 
-	hlist_for_each_entry(child, &clk->children, child_node)
+	hlist_for_each_entry(child, &core->children, child_node)
 		clk_disable_unused_subtree(child);
 
 	flags = clk_enable_lock();
 
-	if (clk->enable_count)
+	if (core->enable_count)
 		goto unlock_out;
 
-	if (clk->flags & CLK_IGNORE_UNUSED)
+	if (core->flags & CLK_IGNORE_UNUSED)
 		goto unlock_out;
 
 	/*
@@ -528,13 +211,13 @@ static void clk_disable_unused_subtree(struct clk_core *clk)
 	 * sequence.  call .disable_unused if available, otherwise fall
 	 * back to .disable
 	 */
-	if (clk_core_is_enabled(clk)) {
-		trace_clk_disable(clk);
-		if (clk->ops->disable_unused)
-			clk->ops->disable_unused(clk->hw);
-		else if (clk->ops->disable)
-			clk->ops->disable(clk->hw);
-		trace_clk_disable_complete(clk);
+	if (clk_core_is_enabled(core)) {
+		trace_clk_disable(core);
+		if (core->ops->disable_unused)
+			core->ops->disable_unused(core->hw);
+		else if (core->ops->disable)
+			core->ops->disable(core->hw);
+		trace_clk_disable_complete(core);
 	}
 
 unlock_out:
@@ -551,7 +234,7 @@ __setup("clk_ignore_unused", clk_ignore_unused_setup);
 
 static int clk_disable_unused(void)
 {
-	struct clk_core *clk;
+	struct clk_core *core;
 
 	if (clk_ignore_unused) {
 		pr_warn("clk: Not disabling unused clocks\n");
@@ -560,17 +243,17 @@ static int clk_disable_unused(void)
 
 	clk_prepare_lock();
 
-	hlist_for_each_entry(clk, &clk_root_list, child_node)
-		clk_disable_unused_subtree(clk);
+	hlist_for_each_entry(core, &clk_root_list, child_node)
+		clk_disable_unused_subtree(core);
 
-	hlist_for_each_entry(clk, &clk_orphan_list, child_node)
-		clk_disable_unused_subtree(clk);
+	hlist_for_each_entry(core, &clk_orphan_list, child_node)
+		clk_disable_unused_subtree(core);
 
-	hlist_for_each_entry(clk, &clk_root_list, child_node)
-		clk_unprepare_unused_subtree(clk);
+	hlist_for_each_entry(core, &clk_root_list, child_node)
+		clk_unprepare_unused_subtree(core);
 
-	hlist_for_each_entry(clk, &clk_orphan_list, child_node)
-		clk_unprepare_unused_subtree(clk);
+	hlist_for_each_entry(core, &clk_orphan_list, child_node)
+		clk_unprepare_unused_subtree(core);
 
 	clk_prepare_unlock();
 
@@ -608,18 +291,61 @@ struct clk *__clk_get_parent(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(__clk_get_parent);
 
-static struct clk_core *clk_core_get_parent_by_index(struct clk_core *clk,
+static struct clk_core *__clk_lookup_subtree(const char *name,
+					     struct clk_core *core)
+{
+	struct clk_core *child;
+	struct clk_core *ret;
+
+	if (!strcmp(core->name, name))
+		return core;
+
+	hlist_for_each_entry(child, &core->children, child_node) {
+		ret = __clk_lookup_subtree(name, child);
+		if (ret)
+			return ret;
+	}
+
+	return NULL;
+}
+
+static struct clk_core *clk_core_lookup(const char *name)
+{
+	struct clk_core *root_clk;
+	struct clk_core *ret;
+
+	if (!name)
+		return NULL;
+
+	/* search the 'proper' clk tree first */
+	hlist_for_each_entry(root_clk, &clk_root_list, child_node) {
+		ret = __clk_lookup_subtree(name, root_clk);
+		if (ret)
+			return ret;
+	}
+
+	/* if not found, then search the orphan tree */
+	hlist_for_each_entry(root_clk, &clk_orphan_list, child_node) {
+		ret = __clk_lookup_subtree(name, root_clk);
+		if (ret)
+			return ret;
+	}
+
+	return NULL;
+}
+
+static struct clk_core *clk_core_get_parent_by_index(struct clk_core *core,
 							 u8 index)
 {
-	if (!clk || index >= clk->num_parents)
+	if (!core || index >= core->num_parents)
 		return NULL;
-	else if (!clk->parents)
-		return clk_core_lookup(clk->parent_names[index]);
-	else if (!clk->parents[index])
-		return clk->parents[index] =
-			clk_core_lookup(clk->parent_names[index]);
+	else if (!core->parents)
+		return clk_core_lookup(core->parent_names[index]);
+	else if (!core->parents[index])
+		return core->parents[index] =
+			clk_core_lookup(core->parent_names[index]);
 	else
-		return clk->parents[index];
+		return core->parents[index];
 }
 
 struct clk *clk_get_parent_by_index(struct clk *clk, u8 index)
@@ -640,21 +366,21 @@ unsigned int __clk_get_enable_count(struct clk *clk)
 	return !clk ? 0 : clk->core->enable_count;
 }
 
-static unsigned long clk_core_get_rate_nolock(struct clk_core *clk)
+static unsigned long clk_core_get_rate_nolock(struct clk_core *core)
 {
 	unsigned long ret;
 
-	if (!clk) {
+	if (!core) {
 		ret = 0;
 		goto out;
 	}
 
-	ret = clk->rate;
+	ret = core->rate;
 
-	if (clk->flags & CLK_IS_ROOT)
+	if (core->flags & CLK_IS_ROOT)
 		goto out;
 
-	if (!clk->parent)
+	if (!core->parent)
 		ret = 0;
 
 out:
@@ -670,12 +396,12 @@ unsigned long __clk_get_rate(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(__clk_get_rate);
 
-static unsigned long __clk_get_accuracy(struct clk_core *clk)
+static unsigned long __clk_get_accuracy(struct clk_core *core)
 {
-	if (!clk)
+	if (!core)
 		return 0;
 
-	return clk->accuracy;
+	return core->accuracy;
 }
 
 unsigned long __clk_get_flags(struct clk *clk)
@@ -684,27 +410,6 @@ unsigned long __clk_get_flags(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(__clk_get_flags);
 
-static bool clk_core_is_prepared(struct clk_core *clk)
-{
-	int ret;
-
-	if (!clk)
-		return false;
-
-	/*
-	 * .is_prepared is optional for clocks that can prepare
-	 * fall back to software usage counter if it is missing
-	 */
-	if (!clk->ops->is_prepared) {
-		ret = clk->prepare_count ? 1 : 0;
-		goto out;
-	}
-
-	ret = clk->ops->is_prepared(clk->hw);
-out:
-	return !!ret;
-}
-
 bool __clk_is_prepared(struct clk *clk)
 {
 	if (!clk)
@@ -713,27 +418,6 @@ bool __clk_is_prepared(struct clk *clk)
 	return clk_core_is_prepared(clk->core);
 }
 
-static bool clk_core_is_enabled(struct clk_core *clk)
-{
-	int ret;
-
-	if (!clk)
-		return false;
-
-	/*
-	 * .is_enabled is only mandatory for clocks that gate
-	 * fall back to software usage counter if .is_enabled is missing
-	 */
-	if (!clk->ops->is_enabled) {
-		ret = clk->enable_count ? 1 : 0;
-		goto out;
-	}
-
-	ret = clk->ops->is_enabled(clk->hw);
-out:
-	return !!ret;
-}
-
 bool __clk_is_enabled(struct clk *clk)
 {
 	if (!clk)
@@ -743,49 +427,6 @@ bool __clk_is_enabled(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(__clk_is_enabled);
 
-static struct clk_core *__clk_lookup_subtree(const char *name,
-					     struct clk_core *clk)
-{
-	struct clk_core *child;
-	struct clk_core *ret;
-
-	if (!strcmp(clk->name, name))
-		return clk;
-
-	hlist_for_each_entry(child, &clk->children, child_node) {
-		ret = __clk_lookup_subtree(name, child);
-		if (ret)
-			return ret;
-	}
-
-	return NULL;
-}
-
-static struct clk_core *clk_core_lookup(const char *name)
-{
-	struct clk_core *root_clk;
-	struct clk_core *ret;
-
-	if (!name)
-		return NULL;
-
-	/* search the 'proper' clk tree first */
-	hlist_for_each_entry(root_clk, &clk_root_list, child_node) {
-		ret = __clk_lookup_subtree(name, root_clk);
-		if (ret)
-			return ret;
-	}
-
-	/* if not found, then search the orphan tree */
-	hlist_for_each_entry(root_clk, &clk_orphan_list, child_node) {
-		ret = __clk_lookup_subtree(name, root_clk);
-		if (ret)
-			return ret;
-	}
-
-	return NULL;
-}
-
 static bool mux_is_better_rate(unsigned long rate, unsigned long now,
 			   unsigned long best, unsigned long flags)
 {
@@ -853,7 +494,7 @@ struct clk *__clk_lookup(const char *name)
 	return !core ? NULL : core->hw->clk;
 }
 
-static void clk_core_get_boundaries(struct clk_core *clk,
+static void clk_core_get_boundaries(struct clk_core *core,
 				    unsigned long *min_rate,
 				    unsigned long *max_rate)
 {
@@ -862,10 +503,10 @@ static void clk_core_get_boundaries(struct clk_core *clk,
 	*min_rate = 0;
 	*max_rate = ULONG_MAX;
 
-	hlist_for_each_entry(clk_user, &clk->clks, clks_node)
+	hlist_for_each_entry(clk_user, &core->clks, clks_node)
 		*min_rate = max(*min_rate, clk_user->min_rate);
 
-	hlist_for_each_entry(clk_user, &clk->clks, clks_node)
+	hlist_for_each_entry(clk_user, &core->clks, clks_node)
 		*max_rate = min(*max_rate, clk_user->max_rate);
 }
 
@@ -901,26 +542,28 @@ EXPORT_SYMBOL_GPL(__clk_mux_determine_rate_closest);
 
 /***        clk api        ***/
 
-static void clk_core_unprepare(struct clk_core *clk)
+static void clk_core_unprepare(struct clk_core *core)
 {
-	if (!clk)
+	lockdep_assert_held(&prepare_lock);
+
+	if (!core)
 		return;
 
-	if (WARN_ON(clk->prepare_count == 0))
+	if (WARN_ON(core->prepare_count == 0))
 		return;
 
-	if (--clk->prepare_count > 0)
+	if (--core->prepare_count > 0)
 		return;
 
-	WARN_ON(clk->enable_count > 0);
+	WARN_ON(core->enable_count > 0);
 
-	trace_clk_unprepare(clk);
+	trace_clk_unprepare(core);
 
-	if (clk->ops->unprepare)
-		clk->ops->unprepare(clk->hw);
+	if (core->ops->unprepare)
+		core->ops->unprepare(core->hw);
 
-	trace_clk_unprepare_complete(clk);
-	clk_core_unprepare(clk->parent);
+	trace_clk_unprepare_complete(core);
+	clk_core_unprepare(core->parent);
 }
 
 /**
@@ -945,32 +588,34 @@ void clk_unprepare(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(clk_unprepare);
 
-static int clk_core_prepare(struct clk_core *clk)
+static int clk_core_prepare(struct clk_core *core)
 {
 	int ret = 0;
 
-	if (!clk)
+	lockdep_assert_held(&prepare_lock);
+
+	if (!core)
 		return 0;
 
-	if (clk->prepare_count == 0) {
-		ret = clk_core_prepare(clk->parent);
+	if (core->prepare_count == 0) {
+		ret = clk_core_prepare(core->parent);
 		if (ret)
 			return ret;
 
-		trace_clk_prepare(clk);
+		trace_clk_prepare(core);
 
-		if (clk->ops->prepare)
-			ret = clk->ops->prepare(clk->hw);
+		if (core->ops->prepare)
+			ret = core->ops->prepare(core->hw);
 
-		trace_clk_prepare_complete(clk);
+		trace_clk_prepare_complete(core);
 
 		if (ret) {
-			clk_core_unprepare(clk->parent);
+			clk_core_unprepare(core->parent);
 			return ret;
 		}
 	}
 
-	clk->prepare_count++;
+	core->prepare_count++;
 
 	return 0;
 }
@@ -1002,33 +647,27 @@ int clk_prepare(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(clk_prepare);
 
-static void clk_core_disable(struct clk_core *clk)
+static void clk_core_disable(struct clk_core *core)
 {
-	if (!clk)
+	lockdep_assert_held(&enable_lock);
+
+	if (!core)
 		return;
 
-	if (WARN_ON(clk->enable_count == 0))
+	if (WARN_ON(core->enable_count == 0))
 		return;
 
-	if (--clk->enable_count > 0)
+	if (--core->enable_count > 0)
 		return;
 
-	trace_clk_disable(clk);
+	trace_clk_disable(core);
 
-	if (clk->ops->disable)
-		clk->ops->disable(clk->hw);
+	if (core->ops->disable)
+		core->ops->disable(core->hw);
 
-	trace_clk_disable_complete(clk);
+	trace_clk_disable_complete(core);
 
-	clk_core_disable(clk->parent);
-}
-
-static void __clk_disable(struct clk *clk)
-{
-	if (!clk)
-		return;
-
-	clk_core_disable(clk->core);
+	clk_core_disable(core->parent);
 }
 
 /**
@@ -1051,52 +690,46 @@ void clk_disable(struct clk *clk)
 		return;
 
 	flags = clk_enable_lock();
-	__clk_disable(clk);
+	clk_core_disable(clk->core);
 	clk_enable_unlock(flags);
 }
 EXPORT_SYMBOL_GPL(clk_disable);
 
-static int clk_core_enable(struct clk_core *clk)
+static int clk_core_enable(struct clk_core *core)
 {
 	int ret = 0;
 
-	if (!clk)
+	lockdep_assert_held(&enable_lock);
+
+	if (!core)
 		return 0;
 
-	if (WARN_ON(clk->prepare_count == 0))
+	if (WARN_ON(core->prepare_count == 0))
 		return -ESHUTDOWN;
 
-	if (clk->enable_count == 0) {
-		ret = clk_core_enable(clk->parent);
+	if (core->enable_count == 0) {
+		ret = clk_core_enable(core->parent);
 
 		if (ret)
 			return ret;
 
-		trace_clk_enable(clk);
+		trace_clk_enable(core);
 
-		if (clk->ops->enable)
-			ret = clk->ops->enable(clk->hw);
+		if (core->ops->enable)
+			ret = core->ops->enable(core->hw);
 
-		trace_clk_enable_complete(clk);
+		trace_clk_enable_complete(core);
 
 		if (ret) {
-			clk_core_disable(clk->parent);
+			clk_core_disable(core->parent);
 			return ret;
 		}
 	}
 
-	clk->enable_count++;
+	core->enable_count++;
 	return 0;
 }
 
-static int __clk_enable(struct clk *clk)
-{
-	if (!clk)
-		return 0;
-
-	return clk_core_enable(clk->core);
-}
-
 /**
  * clk_enable - ungate a clock
  * @clk: the clk being ungated
@@ -1115,15 +748,18 @@ int clk_enable(struct clk *clk)
 	unsigned long flags;
 	int ret;
 
+	if (!clk)
+		return 0;
+
 	flags = clk_enable_lock();
-	ret = __clk_enable(clk);
+	ret = clk_core_enable(clk->core);
 	clk_enable_unlock(flags);
 
 	return ret;
 }
 EXPORT_SYMBOL_GPL(clk_enable);
 
-static unsigned long clk_core_round_rate_nolock(struct clk_core *clk,
+static unsigned long clk_core_round_rate_nolock(struct clk_core *core,
 						unsigned long rate,
 						unsigned long min_rate,
 						unsigned long max_rate)
@@ -1134,25 +770,25 @@ static unsigned long clk_core_round_rate_nolock(struct clk_core *clk,
 
 	lockdep_assert_held(&prepare_lock);
 
-	if (!clk)
+	if (!core)
 		return 0;
 
-	parent = clk->parent;
+	parent = core->parent;
 	if (parent)
 		parent_rate = parent->rate;
 
-	if (clk->ops->determine_rate) {
+	if (core->ops->determine_rate) {
 		parent_hw = parent ? parent->hw : NULL;
-		return clk->ops->determine_rate(clk->hw, rate,
+		return core->ops->determine_rate(core->hw, rate,
 						min_rate, max_rate,
 						&parent_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_core_round_rate_nolock(clk->parent, rate, min_rate,
+	} else if (core->ops->round_rate)
+		return core->ops->round_rate(core->hw, rate, &parent_rate);
+	else if (core->flags & CLK_SET_RATE_PARENT)
+		return clk_core_round_rate_nolock(core->parent, rate, min_rate,
 						  max_rate);
 	else
-		return clk->rate;
+		return core->rate;
 }
 
 /**
@@ -1162,8 +798,7 @@ static unsigned long clk_core_round_rate_nolock(struct clk_core *clk,
  * @min_rate: returned rate must be greater than this rate
  * @max_rate: returned rate must be less than this rate
  *
- * Caller must hold prepare_lock.  Useful for clk_ops such as .set_rate and
- * .determine_rate.
+ * Useful for clk_ops such as .set_rate and .determine_rate.
  */
 unsigned long __clk_determine_rate(struct clk_hw *hw,
 				   unsigned long rate,
@@ -1182,7 +817,7 @@ EXPORT_SYMBOL_GPL(__clk_determine_rate);
  * @clk: round the rate of this clock
  * @rate: the rate which is to be rounded
  *
- * Caller must hold prepare_lock.  Useful for clk_ops such as .set_rate
+ * Useful for clk_ops such as .set_rate
  */
 unsigned long __clk_round_rate(struct clk *clk, unsigned long rate)
 {
@@ -1224,7 +859,7 @@ EXPORT_SYMBOL_GPL(clk_round_rate);
 
 /**
  * __clk_notify - call clk notifier chain
- * @clk: struct clk * that is changing rate
+ * @core: clk that is changing rate
  * @msg: clk notifier type (see include/linux/clk.h)
  * @old_rate: old clk rate
  * @new_rate: new clk rate
@@ -1236,7 +871,7 @@ EXPORT_SYMBOL_GPL(clk_round_rate);
  * called if all went well, or NOTIFY_STOP or NOTIFY_BAD immediately if
  * a driver returns that.
  */
-static int __clk_notify(struct clk_core *clk, unsigned long msg,
+static int __clk_notify(struct clk_core *core, unsigned long msg,
 		unsigned long old_rate, unsigned long new_rate)
 {
 	struct clk_notifier *cn;
@@ -1247,7 +882,7 @@ static int __clk_notify(struct clk_core *clk, unsigned long msg,
 	cnd.new_rate = new_rate;
 
 	list_for_each_entry(cn, &clk_notifier_list, node) {
-		if (cn->clk->core == clk) {
+		if (cn->clk->core == core) {
 			cnd.clk = cn->clk;
 			ret = srcu_notifier_call_chain(&cn->notifier_head, msg,
 					&cnd);
@@ -1259,44 +894,42 @@ static int __clk_notify(struct clk_core *clk, unsigned long msg,
 
 /**
  * __clk_recalc_accuracies
- * @clk: first clk in the subtree
+ * @core: first clk in the subtree
  *
  * Walks the subtree of clks starting with clk and recalculates accuracies as
  * it goes.  Note that if a clk does not implement the .recalc_accuracy
- * callback then it is assumed that the clock will take on the accuracy of it's
+ * callback then it is assumed that the clock will take on the accuracy of its
  * parent.
- *
- * Caller must hold prepare_lock.
  */
-static void __clk_recalc_accuracies(struct clk_core *clk)
+static void __clk_recalc_accuracies(struct clk_core *core)
 {
 	unsigned long parent_accuracy = 0;
 	struct clk_core *child;
 
 	lockdep_assert_held(&prepare_lock);
 
-	if (clk->parent)
-		parent_accuracy = clk->parent->accuracy;
+	if (core->parent)
+		parent_accuracy = core->parent->accuracy;
 
-	if (clk->ops->recalc_accuracy)
-		clk->accuracy = clk->ops->recalc_accuracy(clk->hw,
+	if (core->ops->recalc_accuracy)
+		core->accuracy = core->ops->recalc_accuracy(core->hw,
 							  parent_accuracy);
 	else
-		clk->accuracy = parent_accuracy;
+		core->accuracy = parent_accuracy;
 
-	hlist_for_each_entry(child, &clk->children, child_node)
+	hlist_for_each_entry(child, &core->children, child_node)
 		__clk_recalc_accuracies(child);
 }
 
-static long clk_core_get_accuracy(struct clk_core *clk)
+static long clk_core_get_accuracy(struct clk_core *core)
 {
 	unsigned long accuracy;
 
 	clk_prepare_lock();
-	if (clk && (clk->flags & CLK_GET_ACCURACY_NOCACHE))
-		__clk_recalc_accuracies(clk);
+	if (core && (core->flags & CLK_GET_ACCURACY_NOCACHE))
+		__clk_recalc_accuracies(core);
 
-	accuracy = __clk_get_accuracy(clk);
+	accuracy = __clk_get_accuracy(core);
 	clk_prepare_unlock();
 
 	return accuracy;
@@ -1320,17 +953,17 @@ long clk_get_accuracy(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(clk_get_accuracy);
 
-static unsigned long clk_recalc(struct clk_core *clk,
+static unsigned long clk_recalc(struct clk_core *core,
 				unsigned long parent_rate)
 {
-	if (clk->ops->recalc_rate)
-		return clk->ops->recalc_rate(clk->hw, parent_rate);
+	if (core->ops->recalc_rate)
+		return core->ops->recalc_rate(core->hw, parent_rate);
 	return parent_rate;
 }
 
 /**
  * __clk_recalc_rates
- * @clk: first clk in the subtree
+ * @core: first clk in the subtree
  * @msg: notification type (see include/linux/clk.h)
  *
  * Walks the subtree of clks starting with clk and recalculates rates as it
@@ -1339,10 +972,8 @@ static unsigned long clk_recalc(struct clk_core *clk,
  *
  * clk_recalc_rates also propagates the POST_RATE_CHANGE notification,
  * if necessary.
- *
- * Caller must hold prepare_lock.
  */
-static void __clk_recalc_rates(struct clk_core *clk, unsigned long msg)
+static void __clk_recalc_rates(struct clk_core *core, unsigned long msg)
 {
 	unsigned long old_rate;
 	unsigned long parent_rate = 0;
@@ -1350,34 +981,34 @@ static void __clk_recalc_rates(struct clk_core *clk, unsigned long msg)
 
 	lockdep_assert_held(&prepare_lock);
 
-	old_rate = clk->rate;
+	old_rate = core->rate;
 
-	if (clk->parent)
-		parent_rate = clk->parent->rate;
+	if (core->parent)
+		parent_rate = core->parent->rate;
 
-	clk->rate = clk_recalc(clk, parent_rate);
+	core->rate = clk_recalc(core, parent_rate);
 
 	/*
 	 * ignore NOTIFY_STOP and NOTIFY_BAD return values for POST_RATE_CHANGE
 	 * & ABORT_RATE_CHANGE notifiers
 	 */
-	if (clk->notifier_count && msg)
-		__clk_notify(clk, msg, old_rate, clk->rate);
+	if (core->notifier_count && msg)
+		__clk_notify(core, msg, old_rate, core->rate);
 
-	hlist_for_each_entry(child, &clk->children, child_node)
+	hlist_for_each_entry(child, &core->children, child_node)
 		__clk_recalc_rates(child, msg);
 }
 
-static unsigned long clk_core_get_rate(struct clk_core *clk)
+static unsigned long clk_core_get_rate(struct clk_core *core)
 {
 	unsigned long rate;
 
 	clk_prepare_lock();
 
-	if (clk && (clk->flags & CLK_GET_RATE_NOCACHE))
-		__clk_recalc_rates(clk, 0);
+	if (core && (core->flags & CLK_GET_RATE_NOCACHE))
+		__clk_recalc_rates(core, 0);
 
-	rate = clk_core_get_rate_nolock(clk);
+	rate = clk_core_get_rate_nolock(core);
 	clk_prepare_unlock();
 
 	return rate;
@@ -1400,15 +1031,15 @@ unsigned long clk_get_rate(struct clk *clk)
 }
 EXPORT_SYMBOL_GPL(clk_get_rate);
 
-static int clk_fetch_parent_index(struct clk_core *clk,
+static int clk_fetch_parent_index(struct clk_core *core,
 				  struct clk_core *parent)
 {
 	int i;
 
-	if (!clk->parents) {
-		clk->parents = kcalloc(clk->num_parents,
+	if (!core->parents) {
+		core->parents = kcalloc(core->num_parents,
 					sizeof(struct clk *), GFP_KERNEL);
-		if (!clk->parents)
+		if (!core->parents)
 			return -ENOMEM;
 	}
 
@@ -1417,15 +1048,15 @@ static int clk_fetch_parent_index(struct clk_core *clk,
 	 * or if not yet cached, use string name comparison and cache
 	 * them now to avoid future calls to clk_core_lookup.
 	 */
-	for (i = 0; i < clk->num_parents; i++) {
-		if (clk->parents[i] == parent)
+	for (i = 0; i < core->num_parents; i++) {
+		if (core->parents[i] == parent)
 			return i;
 
-		if (clk->parents[i])
+		if (core->parents[i])
 			continue;
 
-		if (!strcmp(clk->parent_names[i], parent->name)) {
-			clk->parents[i] = clk_core_lookup(parent->name);
+		if (!strcmp(core->parent_names[i], parent->name)) {
+			core->parents[i] = clk_core_lookup(parent->name);
 			return i;
 		}
 	}
@@ -1433,28 +1064,28 @@ static int clk_fetch_parent_index(struct clk_core *clk,
 	return -EINVAL;
 }
 
-static void clk_reparent(struct clk_core *clk, struct clk_core *new_parent)
+static void clk_reparent(struct clk_core *core, struct clk_core *new_parent)
 {
-	hlist_del(&clk->child_node);
+	hlist_del(&core->child_node);
 
 	if (new_parent) {
 		/* avoid duplicate POST_RATE_CHANGE notifications */
-		if (new_parent->new_child == clk)
+		if (new_parent->new_child == core)
 			new_parent->new_child = NULL;
 
-		hlist_add_head(&clk->child_node, &new_parent->children);
+		hlist_add_head(&core->child_node, &new_parent->children);
 	} else {
-		hlist_add_head(&clk->child_node, &clk_orphan_list);
+		hlist_add_head(&core->child_node, &clk_orphan_list);
 	}
 
-	clk->parent = new_parent;
+	core->parent = new_parent;
 }
 
-static struct clk_core *__clk_set_parent_before(struct clk_core *clk,
+static struct clk_core *__clk_set_parent_before(struct clk_core *core,
 					   struct clk_core *parent)
 {
 	unsigned long flags;
-	struct clk_core *old_parent = clk->parent;
+	struct clk_core *old_parent = core->parent;
 
 	/*
 	 * Migrate prepare state between parents and prevent race with
@@ -1473,17 +1104,17 @@ static struct clk_core *__clk_set_parent_before(struct clk_core *clk,
 	 *
 	 * See also: Comment for clk_set_parent() below.
 	 */
-	if (clk->prepare_count) {
+	if (core->prepare_count) {
 		clk_core_prepare(parent);
 		flags = clk_enable_lock();
 		clk_core_enable(parent);
-		clk_core_enable(clk);
+		clk_core_enable(core);
 		clk_enable_unlock(flags);
 	}
 
 	/* update the clk tree topology */
 	flags = clk_enable_lock();
-	clk_reparent(clk, parent);
+	clk_reparent(core, parent);
 	clk_enable_unlock(flags);
 
 	return old_parent;
@@ -1508,31 +1139,31 @@ static void __clk_set_parent_after(struct clk_core *core,
 	}
 }
 
-static int __clk_set_parent(struct clk_core *clk, struct clk_core *parent,
+static int __clk_set_parent(struct clk_core *core, struct clk_core *parent,
 			    u8 p_index)
 {
 	unsigned long flags;
 	int ret = 0;
 	struct clk_core *old_parent;
 
-	old_parent = __clk_set_parent_before(clk, parent);
+	old_parent = __clk_set_parent_before(core, parent);
 
-	trace_clk_set_parent(clk, parent);
+	trace_clk_set_parent(core, parent);
 
 	/* change clock input source */
-	if (parent && clk->ops->set_parent)
-		ret = clk->ops->set_parent(clk->hw, p_index);
+	if (parent && core->ops->set_parent)
+		ret = core->ops->set_parent(core->hw, p_index);
 
-	trace_clk_set_parent_complete(clk, parent);
+	trace_clk_set_parent_complete(core, parent);
 
 	if (ret) {
 		flags = clk_enable_lock();
-		clk_reparent(clk, old_parent);
+		clk_reparent(core, old_parent);
 		clk_enable_unlock(flags);
 
-		if (clk->prepare_count) {
+		if (core->prepare_count) {
 			flags = clk_enable_lock();
-			clk_core_disable(clk);
+			clk_core_disable(core);
 			clk_core_disable(parent);
 			clk_enable_unlock(flags);
 			clk_core_unprepare(parent);
@@ -1540,14 +1171,14 @@ static int __clk_set_parent(struct clk_core *clk, struct clk_core *parent,
 		return ret;
 	}
 
-	__clk_set_parent_after(clk, parent, old_parent);
+	__clk_set_parent_after(core, parent, old_parent);
 
 	return 0;
 }
 
 /**
  * __clk_speculate_rates
- * @clk: first clk in the subtree
+ * @core: first clk in the subtree
  * @parent_rate: the "future" rate of clk's parent
  *
  * Walks the subtree of clks starting with clk, speculating rates as it
@@ -1558,10 +1189,8 @@ static int __clk_set_parent(struct clk_core *clk, struct clk_core *parent,
  * subtree have subscribed to the notifications.  Note that if a clk does not
  * implement the .recalc_rate callback then it is assumed that the clock will
  * take on the rate of its parent.
- *
- * Caller must hold prepare_lock.
  */
-static int __clk_speculate_rates(struct clk_core *clk,
+static int __clk_speculate_rates(struct clk_core *core,
 				 unsigned long parent_rate)
 {
 	struct clk_core *child;
@@ -1570,19 +1199,19 @@ static int __clk_speculate_rates(struct clk_core *clk,
 
 	lockdep_assert_held(&prepare_lock);
 
-	new_rate = clk_recalc(clk, parent_rate);
+	new_rate = clk_recalc(core, parent_rate);
 
 	/* abort rate change if a driver returns NOTIFY_BAD or NOTIFY_STOP */
-	if (clk->notifier_count)
-		ret = __clk_notify(clk, PRE_RATE_CHANGE, clk->rate, new_rate);
+	if (core->notifier_count)
+		ret = __clk_notify(core, PRE_RATE_CHANGE, core->rate, new_rate);
 
 	if (ret & NOTIFY_STOP_MASK) {
 		pr_debug("%s: clk notifier callback for clock %s aborted with error %d\n",
-				__func__, clk->name, ret);
+				__func__, core->name, ret);
 		goto out;
 	}
 
-	hlist_for_each_entry(child, &clk->children, child_node) {
+	hlist_for_each_entry(child, &core->children, child_node) {
 		ret = __clk_speculate_rates(child, new_rate);
 		if (ret & NOTIFY_STOP_MASK)
 			break;
@@ -1592,20 +1221,20 @@ out:
 	return ret;
 }
 
-static void clk_calc_subtree(struct clk_core *clk, unsigned long new_rate,
+static void clk_calc_subtree(struct clk_core *core, unsigned long new_rate,
 			     struct clk_core *new_parent, u8 p_index)
 {
 	struct clk_core *child;
 
-	clk->new_rate = new_rate;
-	clk->new_parent = new_parent;
-	clk->new_parent_index = p_index;
+	core->new_rate = new_rate;
+	core->new_parent = new_parent;
+	core->new_parent_index = p_index;
 	/* include clk in new parent's PRE_RATE_CHANGE notifications */
-	clk->new_child = NULL;
-	if (new_parent && new_parent != clk->parent)
-		new_parent->new_child = clk;
+	core->new_child = NULL;
+	if (new_parent && new_parent != core->parent)
+		new_parent->new_child = core;
 
-	hlist_for_each_entry(child, &clk->children, child_node) {
+	hlist_for_each_entry(child, &core->children, child_node) {
 		child->new_rate = clk_recalc(child, new_rate);
 		clk_calc_subtree(child, child->new_rate, NULL, 0);
 	}
@@ -1615,10 +1244,10 @@ static void clk_calc_subtree(struct clk_core *clk, unsigned long new_rate,
  * calculate the new rates returning the topmost clock that has to be
  * changed.
  */
-static struct clk_core *clk_calc_new_rates(struct clk_core *clk,
+static struct clk_core *clk_calc_new_rates(struct clk_core *core,
 					   unsigned long rate)
 {
-	struct clk_core *top = clk;
+	struct clk_core *top = core;
 	struct clk_core *old_parent, *parent;
 	struct clk_hw *parent_hw;
 	unsigned long best_parent_rate = 0;
@@ -1629,20 +1258,20 @@ static struct clk_core *clk_calc_new_rates(struct clk_core *clk,
 	long ret;
 
 	/* sanity */
-	if (IS_ERR_OR_NULL(clk))
+	if (IS_ERR_OR_NULL(core))
 		return NULL;
 
 	/* save parent rate, if it exists */
-	parent = old_parent = clk->parent;
+	parent = old_parent = core->parent;
 	if (parent)
 		best_parent_rate = parent->rate;
 
-	clk_core_get_boundaries(clk, &min_rate, &max_rate);
+	clk_core_get_boundaries(core, &min_rate, &max_rate);
 
 	/* find the closest rate and parent clk/rate */
-	if (clk->ops->determine_rate) {
+	if (core->ops->determine_rate) {
 		parent_hw = parent ? parent->hw : NULL;
-		ret = clk->ops->determine_rate(clk->hw, rate,
+		ret = core->ops->determine_rate(core->hw, rate,
 					       min_rate,
 					       max_rate,
 					       &best_parent_rate,
@@ -1652,8 +1281,8 @@ static struct clk_core *clk_calc_new_rates(struct clk_core *clk,
 
 		new_rate = ret;
 		parent = parent_hw ? parent_hw->core : NULL;
-	} else if (clk->ops->round_rate) {
-		ret = clk->ops->round_rate(clk->hw, rate,
+	} else if (core->ops->round_rate) {
+		ret = core->ops->round_rate(core->hw, rate,
 					   &best_parent_rate);
 		if (ret < 0)
 			return NULL;
@@ -1661,9 +1290,9 @@ static struct clk_core *clk_calc_new_rates(struct clk_core *clk,
 		new_rate = ret;
 		if (new_rate < min_rate || new_rate > max_rate)
 			return NULL;
-	} else if (!parent || !(clk->flags & CLK_SET_RATE_PARENT)) {
+	} else if (!parent || !(core->flags & CLK_SET_RATE_PARENT)) {
 		/* pass-through clock without adjustable parent */
-		clk->new_rate = clk->rate;
+		core->new_rate = core->rate;
 		return NULL;
 	} else {
 		/* pass-through clock with adjustable parent */
@@ -1674,28 +1303,28 @@ static struct clk_core *clk_calc_new_rates(struct clk_core *clk,
 
 	/* some clocks must be gated to change parent */
 	if (parent != old_parent &&
-	    (clk->flags & CLK_SET_PARENT_GATE) && clk->prepare_count) {
+	    (core->flags & CLK_SET_PARENT_GATE) && core->prepare_count) {
 		pr_debug("%s: %s not gated but wants to reparent\n",
-			 __func__, clk->name);
+			 __func__, core->name);
 		return NULL;
 	}
 
 	/* try finding the new parent index */
-	if (parent && clk->num_parents > 1) {
-		p_index = clk_fetch_parent_index(clk, parent);
+	if (parent && core->num_parents > 1) {
+		p_index = clk_fetch_parent_index(core, parent);
 		if (p_index < 0) {
 			pr_debug("%s: clk %s can not be parent of clk %s\n",
-				 __func__, parent->name, clk->name);
+				 __func__, parent->name, core->name);
 			return NULL;
 		}
 	}
 
-	if ((clk->flags & CLK_SET_RATE_PARENT) && parent &&
+	if ((core->flags & CLK_SET_RATE_PARENT) && parent &&
 	    best_parent_rate != parent->rate)
 		top = clk_calc_new_rates(parent, best_parent_rate);
 
 out:
-	clk_calc_subtree(clk, new_rate, parent, p_index);
+	clk_calc_subtree(core, new_rate, parent, p_index);
 
 	return top;
 }
@@ -1705,33 +1334,33 @@ out:
  * so that in case of an error we can walk down the whole tree again and
  * abort the change.
  */
-static struct clk_core *clk_propagate_rate_change(struct clk_core *clk,
+static struct clk_core *clk_propagate_rate_change(struct clk_core *core,
 						  unsigned long event)
 {
 	struct clk_core *child, *tmp_clk, *fail_clk = NULL;
 	int ret = NOTIFY_DONE;
 
-	if (clk->rate == clk->new_rate)
+	if (core->rate == core->new_rate)
 		return NULL;
 
-	if (clk->notifier_count) {
-		ret = __clk_notify(clk, event, clk->rate, clk->new_rate);
+	if (core->notifier_count) {
+		ret = __clk_notify(core, event, core->rate, core->new_rate);
 		if (ret & NOTIFY_STOP_MASK)
-			fail_clk = clk;
+			fail_clk = core;
 	}
 
-	hlist_for_each_entry(child, &clk->children, child_node) {
+	hlist_for_each_entry(child, &core->children, child_node) {
 		/* Skip children who will be reparented to another clock */
-		if (child->new_parent && child->new_parent != clk)
+		if (child->new_parent && child->new_parent != core)
 			continue;
 		tmp_clk = clk_propagate_rate_change(child, event);
 		if (tmp_clk)
 			fail_clk = tmp_clk;
 	}
 
-	/* handle the new child who might not be in clk->children yet */
-	if (clk->new_child) {
-		tmp_clk = clk_propagate_rate_change(clk->new_child, event);
+	/* handle the new child who might not be in core->children yet */
+	if (core->new_child) {
+		tmp_clk = clk_propagate_rate_change(core->new_child, event);
 		if (tmp_clk)
 			fail_clk = tmp_clk;
 	}
@@ -1743,7 +1372,7 @@ static struct clk_core *clk_propagate_rate_change(struct clk_core *clk,
  * walk down a subtree and set the new rates notifying the rate
  * change on the way
  */
-static void clk_change_rate(struct clk_core *clk)
+static void clk_change_rate(struct clk_core *core)
 {
 	struct clk_core *child;
 	struct hlist_node *tmp;
@@ -1752,77 +1381,80 @@ static void clk_change_rate(struct clk_core *clk)
 	bool skip_set_rate = false;
 	struct clk_core *old_parent;
 
-	old_rate = clk->rate;
+	old_rate = core->rate;
 
-	if (clk->new_parent)
-		best_parent_rate = clk->new_parent->rate;
-	else if (clk->parent)
-		best_parent_rate = clk->parent->rate;
+	if (core->new_parent)
+		best_parent_rate = core->new_parent->rate;
+	else if (core->parent)
+		best_parent_rate = core->parent->rate;
 
-	if (clk->new_parent && clk->new_parent != clk->parent) {
-		old_parent = __clk_set_parent_before(clk, clk->new_parent);
-		trace_clk_set_parent(clk, clk->new_parent);
+	if (core->new_parent && core->new_parent != core->parent) {
+		old_parent = __clk_set_parent_before(core, core->new_parent);
+		trace_clk_set_parent(core, core->new_parent);
 
-		if (clk->ops->set_rate_and_parent) {
+		if (core->ops->set_rate_and_parent) {
 			skip_set_rate = true;
-			clk->ops->set_rate_and_parent(clk->hw, clk->new_rate,
+			core->ops->set_rate_and_parent(core->hw, core->new_rate,
 					best_parent_rate,
-					clk->new_parent_index);
-		} else if (clk->ops->set_parent) {
-			clk->ops->set_parent(clk->hw, clk->new_parent_index);
+					core->new_parent_index);
+		} else if (core->ops->set_parent) {
+			core->ops->set_parent(core->hw, core->new_parent_index);
 		}
 
-		trace_clk_set_parent_complete(clk, clk->new_parent);
-		__clk_set_parent_after(clk, clk->new_parent, old_parent);
+		trace_clk_set_parent_complete(core, core->new_parent);
+		__clk_set_parent_after(core, core->new_parent, old_parent);
 	}
 
-	trace_clk_set_rate(clk, clk->new_rate);
+	trace_clk_set_rate(core, core->new_rate);
 
-	if (!skip_set_rate && clk->ops->set_rate)
-		clk->ops->set_rate(clk->hw, clk->new_rate, best_parent_rate);
+	if (!skip_set_rate && core->ops->set_rate)
+		core->ops->set_rate(core->hw, core->new_rate, best_parent_rate);
 
-	trace_clk_set_rate_complete(clk, clk->new_rate);
+	trace_clk_set_rate_complete(core, core->new_rate);
 
-	clk->rate = clk_recalc(clk, best_parent_rate);
+	core->rate = clk_recalc(core, best_parent_rate);
 
-	if (clk->notifier_count && old_rate != clk->rate)
-		__clk_notify(clk, POST_RATE_CHANGE, old_rate, clk->rate);
+	if (core->notifier_count && old_rate != core->rate)
+		__clk_notify(core, POST_RATE_CHANGE, old_rate, core->rate);
+
+	if (core->flags & CLK_RECALC_NEW_RATES)
+		(void)clk_calc_new_rates(core, core->new_rate);
 
 	/*
 	 * Use safe iteration, as change_rate can actually swap parents
 	 * for certain clock types.
 	 */
-	hlist_for_each_entry_safe(child, tmp, &clk->children, child_node) {
+	hlist_for_each_entry_safe(child, tmp, &core->children, child_node) {
 		/* Skip children who will be reparented to another clock */
-		if (child->new_parent && child->new_parent != clk)
+		if (child->new_parent && child->new_parent != core)
 			continue;
 		clk_change_rate(child);
 	}
 
-	/* handle the new child who might not be in clk->children yet */
-	if (clk->new_child)
-		clk_change_rate(clk->new_child);
+	/* handle the new child who might not be in core->children yet */
+	if (core->new_child)
+		clk_change_rate(core->new_child);
 }
 
-static int clk_core_set_rate_nolock(struct clk_core *clk,
+static int clk_core_set_rate_nolock(struct clk_core *core,
 				    unsigned long req_rate)
 {
 	struct clk_core *top, *fail_clk;
 	unsigned long rate = req_rate;
 	int ret = 0;
 
-	if (!clk)
+	if (!core)
 		return 0;
 
 	/* bail early if nothing to do */
-	if (rate == clk_core_get_rate_nolock(clk))
+	if (rate == clk_core_get_rate_nolock(core))
 		return 0;
 
-	if ((clk->flags & CLK_SET_RATE_GATE) && clk->prepare_count)
+	if ((core->flags & CLK_SET_RATE_GATE) && core->prepare_count)
 		return -EBUSY;
 
 	/* calculate new rates and get the topmost changed clock */
-	top = clk_calc_new_rates(clk, rate);
+	top = clk_calc_new_rates(core, rate);
 	if (!top)
 		return -EINVAL;
 
@@ -1838,7 +1470,7 @@ static int clk_core_set_rate_nolock(struct clk_core *clk,
 	/* change the rates */
 	clk_change_rate(top);
 
-	clk->req_rate = req_rate;
+	core->req_rate = req_rate;
 
 	return ret;
 }
@@ -1977,55 +1609,63 @@ EXPORT_SYMBOL_GPL(clk_get_parent);
  * .parents array exists, and if so use it to avoid an expensive tree
  * traversal.  If .parents does not exist then walk the tree.
  */
-static struct clk_core *__clk_init_parent(struct clk_core *clk)
+static struct clk_core *__clk_init_parent(struct clk_core *core)
 {
 	struct clk_core *ret = NULL;
 	u8 index;
 
 	/* handle the trivial cases */
 
-	if (!clk->num_parents)
+	if (!core->num_parents)
 		goto out;
 
-	if (clk->num_parents == 1) {
-		if (IS_ERR_OR_NULL(clk->parent))
-			clk->parent = clk_core_lookup(clk->parent_names[0]);
-		ret = clk->parent;
+	if (core->num_parents == 1) {
+		if (IS_ERR_OR_NULL(core->parent))
+			core->parent = clk_core_lookup(core->parent_names[0]);
+		ret = core->parent;
 		goto out;
 	}
 
-	if (!clk->ops->get_parent) {
-		WARN(!clk->ops->get_parent,
+	if (!core->ops->get_parent) {
+		WARN(!core->ops->get_parent,
 			"%s: multi-parent clocks must implement .get_parent\n",
 			__func__);
 		goto out;
 	};
 
 	/*
-	 * Do our best to cache parent clocks in clk->parents.  This prevents
-	 * unnecessary and expensive lookups.  We don't set clk->parent here;
+	 * Do our best to cache parent clocks in core->parents.  This prevents
+	 * unnecessary and expensive lookups.  We don't set core->parent here;
 	 * that is done by the calling function.
 	 */
 
-	index = clk->ops->get_parent(clk->hw);
+	index = core->ops->get_parent(core->hw);
 
-	if (!clk->parents)
-		clk->parents =
-			kcalloc(clk->num_parents, sizeof(struct clk *),
+	if (!core->parents)
+		core->parents =
+			kcalloc(core->num_parents, sizeof(struct clk *),
 					GFP_KERNEL);
 
-	ret = clk_core_get_parent_by_index(clk, index);
+	ret = clk_core_get_parent_by_index(core, index);
 
 out:
 	return ret;
 }
 
-static void clk_core_reparent(struct clk_core *clk,
+static void clk_core_reparent(struct clk_core *core,
 				  struct clk_core *new_parent)
 {
-	clk_reparent(clk, new_parent);
-	__clk_recalc_accuracies(clk);
-	__clk_recalc_rates(clk, POST_RATE_CHANGE);
+	clk_reparent(core, new_parent);
+	__clk_recalc_accuracies(core);
+	__clk_recalc_rates(core, POST_RATE_CHANGE);
+}
+
+void clk_hw_reparent(struct clk_hw *hw, struct clk_hw *new_parent)
+{
+	if (!hw)
+		return;
+
+	clk_core_reparent(hw->core, !new_parent ? NULL : new_parent->core);
 }
 
 /**
@@ -2062,61 +1702,61 @@ bool clk_has_parent(struct clk *clk, struct clk *parent)
 }
 EXPORT_SYMBOL_GPL(clk_has_parent);
 
-static int clk_core_set_parent(struct clk_core *clk, struct clk_core *parent)
+static int clk_core_set_parent(struct clk_core *core, struct clk_core *parent)
 {
 	int ret = 0;
 	int p_index = 0;
 	unsigned long p_rate = 0;
 
-	if (!clk)
+	if (!core)
 		return 0;
 
 	/* prevent racing with updates to the clock topology */
 	clk_prepare_lock();
 
-	if (clk->parent == parent)
+	if (core->parent == parent)
 		goto out;
 
 	/* verify ops for for multi-parent clks */
-	if ((clk->num_parents > 1) && (!clk->ops->set_parent)) {
+	if ((core->num_parents > 1) && (!core->ops->set_parent)) {
 		ret = -ENOSYS;
 		goto out;
 	}
 
 	/* check that we are allowed to re-parent if the clock is in use */
-	if ((clk->flags & CLK_SET_PARENT_GATE) && clk->prepare_count) {
+	if ((core->flags & CLK_SET_PARENT_GATE) && core->prepare_count) {
 		ret = -EBUSY;
 		goto out;
 	}
 
 	/* try finding the new parent index */
 	if (parent) {
-		p_index = clk_fetch_parent_index(clk, parent);
+		p_index = clk_fetch_parent_index(core, parent);
 		p_rate = parent->rate;
 		if (p_index < 0) {
 			pr_debug("%s: clk %s can not be parent of clk %s\n",
-					__func__, parent->name, clk->name);
+					__func__, parent->name, core->name);
 			ret = p_index;
 			goto out;
 		}
 	}
 
 	/* propagate PRE_RATE_CHANGE notifications */
-	ret = __clk_speculate_rates(clk, p_rate);
+	ret = __clk_speculate_rates(core, p_rate);
 
 	/* abort if a driver objects */
 	if (ret & NOTIFY_STOP_MASK)
 		goto out;
 
 	/* do the re-parent */
-	ret = __clk_set_parent(clk, parent, p_index);
+	ret = __clk_set_parent(core, parent, p_index);
 
 	/* propagate rate an accuracy recalculation accordingly */
 	if (ret) {
-		__clk_recalc_rates(clk, ABORT_RATE_CHANGE);
+		__clk_recalc_rates(core, ABORT_RATE_CHANGE);
 	} else {
-		__clk_recalc_rates(clk, POST_RATE_CHANGE);
-		__clk_recalc_accuracies(clk);
+		__clk_recalc_rates(core, POST_RATE_CHANGE);
+		__clk_recalc_accuracies(core);
 	}
 
 out:
@@ -2201,21 +1841,16 @@ int clk_set_phase(struct clk *clk, int degrees)
 }
 EXPORT_SYMBOL_GPL(clk_set_phase);
 
-static int clk_core_get_phase(struct clk_core *clk)
+static int clk_core_get_phase(struct clk_core *core)
 {
-	int ret = 0;
-
-	if (!clk)
-		goto out;
+	int ret;
 
 	clk_prepare_lock();
-	ret = clk->phase;
+	ret = core->phase;
 	clk_prepare_unlock();
 
-out:
 	return ret;
 }
-EXPORT_SYMBOL_GPL(clk_get_phase);
 
 /**
  * clk_get_phase - return the phase shift of a clock signal
@@ -2231,6 +1866,7 @@ int clk_get_phase(struct clk *clk)
 
 	return clk_core_get_phase(clk->core);
 }
+EXPORT_SYMBOL_GPL(clk_get_phase);
 
 /**
  * clk_is_match - check if two clk's point to the same hardware clock
@@ -2258,6 +1894,337 @@ bool clk_is_match(const struct clk *p, const struct clk *q)
 }
 EXPORT_SYMBOL_GPL(clk_is_match);
 
+/***        debugfs support        ***/
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+
+static struct dentry *rootdir;
+static int inited = 0;
+static DEFINE_MUTEX(clk_debug_lock);
+static HLIST_HEAD(clk_debug_list);
+
+static struct hlist_head *all_lists[] = {
+	&clk_root_list,
+	&clk_orphan_list,
+	NULL,
+};
+
+static struct hlist_head *orphan_list[] = {
+	&clk_orphan_list,
+	NULL,
+};
+
+static void clk_summary_show_one(struct seq_file *s, struct clk_core *c,
+				 int level)
+{
+	if (!c)
+		return;
+
+	seq_printf(s, "%*s%-*s %11d %12d %11lu %10lu %-3d\n",
+		   level * 3 + 1, "",
+		   30 - level * 3, c->name,
+		   c->enable_count, c->prepare_count, clk_core_get_rate(c),
+		   clk_core_get_accuracy(c), clk_core_get_phase(c));
+}
+
+static void clk_summary_show_subtree(struct seq_file *s, struct clk_core *c,
+				     int level)
+{
+	struct clk_core *child;
+
+	if (!c)
+		return;
+
+	clk_summary_show_one(s, c, level);
+
+	hlist_for_each_entry(child, &c->children, child_node)
+		clk_summary_show_subtree(s, child, level + 1);
+}
+
+static int clk_summary_show(struct seq_file *s, void *data)
+{
+	struct clk_core *c;
+	struct hlist_head **lists = (struct hlist_head **)s->private;
+
+	seq_puts(s, "   clock                         enable_cnt  prepare_cnt        rate   accuracy   phase\n");
+	seq_puts(s, "----------------------------------------------------------------------------------------\n");
+
+	clk_prepare_lock();
+
+	for (; *lists; lists++)
+		hlist_for_each_entry(c, *lists, child_node)
+			clk_summary_show_subtree(s, c, 0);
+
+	clk_prepare_unlock();
+
+	return 0;
+}
+
+
+static int clk_summary_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, clk_summary_show, inode->i_private);
+}
+
+static const struct file_operations clk_summary_fops = {
+	.open		= clk_summary_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void clk_dump_one(struct seq_file *s, struct clk_core *c, int level)
+{
+	if (!c)
+		return;
+
+	/* This should be JSON format, i.e. elements separated with a comma */
+	seq_printf(s, "\"%s\": { ", c->name);
+	seq_printf(s, "\"enable_count\": %d,", c->enable_count);
+	seq_printf(s, "\"prepare_count\": %d,", c->prepare_count);
+	seq_printf(s, "\"rate\": %lu,", clk_core_get_rate(c));
+	seq_printf(s, "\"accuracy\": %lu,", clk_core_get_accuracy(c));
+	seq_printf(s, "\"phase\": %d", clk_core_get_phase(c));
+}
+
+static void clk_dump_subtree(struct seq_file *s, struct clk_core *c, int level)
+{
+	struct clk_core *child;
+
+	if (!c)
+		return;
+
+	clk_dump_one(s, c, level);
+
+	hlist_for_each_entry(child, &c->children, child_node) {
+		seq_printf(s, ",");
+		clk_dump_subtree(s, child, level + 1);
+	}
+
+	seq_printf(s, "}");
+}
+
+static int clk_dump(struct seq_file *s, void *data)
+{
+	struct clk_core *c;
+	bool first_node = true;
+	struct hlist_head **lists = (struct hlist_head **)s->private;
+
+	seq_printf(s, "{");
+
+	clk_prepare_lock();
+
+	for (; *lists; lists++) {
+		hlist_for_each_entry(c, *lists, child_node) {
+			if (!first_node)
+				seq_puts(s, ",");
+			first_node = false;
+			clk_dump_subtree(s, c, 0);
+		}
+	}
+
+	clk_prepare_unlock();
+
+	seq_puts(s, "}\n");
+	return 0;
+}
+
+
+static int clk_dump_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, clk_dump, inode->i_private);
+}
+
+static const struct file_operations clk_dump_fops = {
+	.open		= clk_dump_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int clk_debug_create_one(struct clk_core *core, struct dentry *pdentry)
+{
+	struct dentry *d;
+	int ret = -ENOMEM;
+
+	if (!core || !pdentry) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	d = debugfs_create_dir(core->name, pdentry);
+	if (!d)
+		goto out;
+
+	core->dentry = d;
+
+	d = debugfs_create_u32("clk_rate", S_IRUGO, core->dentry,
+			(u32 *)&core->rate);
+	if (!d)
+		goto err_out;
+
+	d = debugfs_create_u32("clk_accuracy", S_IRUGO, core->dentry,
+			(u32 *)&core->accuracy);
+	if (!d)
+		goto err_out;
+
+	d = debugfs_create_u32("clk_phase", S_IRUGO, core->dentry,
+			(u32 *)&core->phase);
+	if (!d)
+		goto err_out;
+
+	d = debugfs_create_x32("clk_flags", S_IRUGO, core->dentry,
+			(u32 *)&core->flags);
+	if (!d)
+		goto err_out;
+
+	d = debugfs_create_u32("clk_prepare_count", S_IRUGO, core->dentry,
+			(u32 *)&core->prepare_count);
+	if (!d)
+		goto err_out;
+
+	d = debugfs_create_u32("clk_enable_count", S_IRUGO, core->dentry,
+			(u32 *)&core->enable_count);
+	if (!d)
+		goto err_out;
+
+	d = debugfs_create_u32("clk_notifier_count", S_IRUGO, core->dentry,
+			(u32 *)&core->notifier_count);
+	if (!d)
+		goto err_out;
+
+	if (core->ops->debug_init) {
+		ret = core->ops->debug_init(core->hw, core->dentry);
+		if (ret)
+			goto err_out;
+	}
+
+	ret = 0;
+	goto out;
+
+err_out:
+	debugfs_remove_recursive(core->dentry);
+	core->dentry = NULL;
+out:
+	return ret;
+}
+
+/**
+ * clk_debug_register - add a clk node to the debugfs clk directory
+ * @core: the clk being added to the debugfs clk directory
+ *
+ * Dynamically adds a clk to the debugfs clk directory if debugfs has been
+ * initialized.  Otherwise it bails out early since the debugfs clk directory
+ * will be created lazily by clk_debug_init as part of a late_initcall.
+ */
+static int clk_debug_register(struct clk_core *core)
+{
+	int ret = 0;
+
+	mutex_lock(&clk_debug_lock);
+	hlist_add_head(&core->debug_node, &clk_debug_list);
+
+	if (!inited)
+		goto unlock;
+
+	ret = clk_debug_create_one(core, rootdir);
+unlock:
+	mutex_unlock(&clk_debug_lock);
+
+	return ret;
+}
+
+ /**
+ * clk_debug_unregister - remove a clk node from the debugfs clk directory
+ * @core: the clk being removed from the debugfs clk directory
+ *
+ * Dynamically removes a clk and all its child nodes from the
+ * debugfs clk directory if clk->dentry points to debugfs created by
+ * clk_debug_register in __clk_init.
+ */
+static void clk_debug_unregister(struct clk_core *core)
+{
+	mutex_lock(&clk_debug_lock);
+	hlist_del_init(&core->debug_node);
+	debugfs_remove_recursive(core->dentry);
+	core->dentry = NULL;
+	mutex_unlock(&clk_debug_lock);
+}
+
+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 (hw->core->dentry)
+		d = debugfs_create_file(name, mode, hw->core->dentry, data,
+					fops);
+
+	return d;
+}
+EXPORT_SYMBOL_GPL(clk_debugfs_add_file);
+
+/**
+ * clk_debug_init - lazily populate the debugfs clk directory
+ *
+ * clks are often initialized very early during boot before memory can be
+ * dynamically allocated and well before debugfs is setup. This function
+ * populates the debugfs clk directory once at boot-time when we know that
+ * debugfs is setup. It should only be called once at boot-time, all other clks
+ * added dynamically will be done so with clk_debug_register.
+ */
+static int __init clk_debug_init(void)
+{
+	struct clk_core *core;
+	struct dentry *d;
+
+	rootdir = debugfs_create_dir("clk", NULL);
+
+	if (!rootdir)
+		return -ENOMEM;
+
+	d = debugfs_create_file("clk_summary", S_IRUGO, rootdir, &all_lists,
+				&clk_summary_fops);
+	if (!d)
+		return -ENOMEM;
+
+	d = debugfs_create_file("clk_dump", S_IRUGO, rootdir, &all_lists,
+				&clk_dump_fops);
+	if (!d)
+		return -ENOMEM;
+
+	d = debugfs_create_file("clk_orphan_summary", S_IRUGO, rootdir,
+				&orphan_list, &clk_summary_fops);
+	if (!d)
+		return -ENOMEM;
+
+	d = debugfs_create_file("clk_orphan_dump", S_IRUGO, rootdir,
+				&orphan_list, &clk_dump_fops);
+	if (!d)
+		return -ENOMEM;
+
+	mutex_lock(&clk_debug_lock);
+	hlist_for_each_entry(core, &clk_debug_list, debug_node)
+		clk_debug_create_one(core, rootdir);
+
+	inited = 1;
+	mutex_unlock(&clk_debug_lock);
+
+	return 0;
+}
+late_initcall(clk_debug_init);
+#else
+static inline int clk_debug_register(struct clk_core *core) { return 0; }
+static inline void clk_debug_reparent(struct clk_core *core,
+				      struct clk_core *new_parent)
+{
+}
+static inline void clk_debug_unregister(struct clk_core *core)
+{
+}
+#endif
+
 /**
  * __clk_init - initialize the data structures in a struct clk
  * @dev:	device initializing this clk, placeholder for now
@@ -2271,67 +2238,67 @@ static int __clk_init(struct device *dev, struct clk *clk_user)
 	int i, ret = 0;
 	struct clk_core *orphan;
 	struct hlist_node *tmp2;
-	struct clk_core *clk;
+	struct clk_core *core;
 	unsigned long rate;
 
 	if (!clk_user)
 		return -EINVAL;
 
-	clk = clk_user->core;
+	core = clk_user->core;
 
 	clk_prepare_lock();
 
 	/* check to see if a clock with this name is already registered */
-	if (clk_core_lookup(clk->name)) {
+	if (clk_core_lookup(core->name)) {
 		pr_debug("%s: clk %s already initialized\n",
-				__func__, clk->name);
+				__func__, core->name);
 		ret = -EEXIST;
 		goto out;
 	}
 
 	/* check that clk_ops are sane.  See Documentation/clk.txt */
-	if (clk->ops->set_rate &&
-	    !((clk->ops->round_rate || clk->ops->determine_rate) &&
-	      clk->ops->recalc_rate)) {
+	if (core->ops->set_rate &&
+	    !((core->ops->round_rate || core->ops->determine_rate) &&
+	      core->ops->recalc_rate)) {
 		pr_warning("%s: %s must implement .round_rate or .determine_rate in addition to .recalc_rate\n",
-				__func__, clk->name);
+				__func__, core->name);
 		ret = -EINVAL;
 		goto out;
 	}
 
-	if (clk->ops->set_parent && !clk->ops->get_parent) {
+	if (core->ops->set_parent && !core->ops->get_parent) {
 		pr_warning("%s: %s must implement .get_parent & .set_parent\n",
-				__func__, clk->name);
+				__func__, core->name);
 		ret = -EINVAL;
 		goto out;
 	}
 
-	if (clk->ops->set_rate_and_parent &&
-			!(clk->ops->set_parent && clk->ops->set_rate)) {
+	if (core->ops->set_rate_and_parent &&
+			!(core->ops->set_parent && core->ops->set_rate)) {
 		pr_warn("%s: %s must implement .set_parent & .set_rate\n",
-				__func__, clk->name);
+				__func__, core->name);
 		ret = -EINVAL;
 		goto out;
 	}
 
 	/* throw a WARN if any entries in parent_names are NULL */
-	for (i = 0; i < clk->num_parents; i++)
-		WARN(!clk->parent_names[i],
+	for (i = 0; i < core->num_parents; i++)
+		WARN(!core->parent_names[i],
 				"%s: invalid NULL in %s's .parent_names\n",
-				__func__, clk->name);
+				__func__, core->name);
 
 	/*
 	 * Allocate an array of struct clk *'s to avoid unnecessary string
 	 * look-ups of clk's possible parents.  This can fail for clocks passed
-	 * in to clk_init during early boot; thus any access to clk->parents[]
+	 * in to clk_init during early boot; thus any access to core->parents[]
 	 * must always check for a NULL pointer and try to populate it if
 	 * necessary.
 	 *
-	 * If clk->parents is not NULL we skip this entire block.  This allows
-	 * for clock drivers to statically initialize clk->parents.
+	 * If core->parents is not NULL we skip this entire block.  This allows
+	 * for clock drivers to statically initialize core->parents.
 	 */
-	if (clk->num_parents > 1 && !clk->parents) {
-		clk->parents = kcalloc(clk->num_parents, sizeof(struct clk *),
+	if (core->num_parents > 1 && !core->parents) {
+		core->parents = kcalloc(core->num_parents, sizeof(struct clk *),
 					GFP_KERNEL);
 		/*
 		 * clk_core_lookup returns NULL for parents that have not been
@@ -2339,16 +2306,16 @@ static int __clk_init(struct device *dev, struct clk *clk_user)
 		 * for a NULL pointer.  We can always perform lazy lookups for
 		 * missing parents later on.
 		 */
-		if (clk->parents)
-			for (i = 0; i < clk->num_parents; i++)
-				clk->parents[i] =
-					clk_core_lookup(clk->parent_names[i]);
+		if (core->parents)
+			for (i = 0; i < core->num_parents; i++)
+				core->parents[i] =
+					clk_core_lookup(core->parent_names[i]);
 	}
 
-	clk->parent = __clk_init_parent(clk);
+	core->parent = __clk_init_parent(core);
 
 	/*
-	 * Populate clk->parent if parent has already been __clk_init'd.  If
+	 * Populate core->parent if parent has already been __clk_init'd.  If
 	 * parent has not yet been __clk_init'd then place clk in the orphan
 	 * list.  If clk has set the CLK_IS_ROOT flag then place it in the root
 	 * clk list.
@@ -2357,13 +2324,13 @@ static int __clk_init(struct device *dev, struct clk *clk_user)
 	 * clocks and re-parent any that are children of the clock currently
 	 * being clk_init'd.
 	 */
-	if (clk->parent)
-		hlist_add_head(&clk->child_node,
-				&clk->parent->children);
-	else if (clk->flags & CLK_IS_ROOT)
-		hlist_add_head(&clk->child_node, &clk_root_list);
+	if (core->parent)
+		hlist_add_head(&core->child_node,
+				&core->parent->children);
+	else if (core->flags & CLK_IS_ROOT)
+		hlist_add_head(&core->child_node, &clk_root_list);
 	else
-		hlist_add_head(&clk->child_node, &clk_orphan_list);
+		hlist_add_head(&core->child_node, &clk_orphan_list);
 
 	/*
 	 * Set clk's accuracy.  The preferred method is to use
@@ -2372,23 +2339,23 @@ static int __clk_init(struct device *dev, struct clk *clk_user)
 	 * parent (or is orphaned) then accuracy is set to zero (perfect
 	 * clock).
 	 */
-	if (clk->ops->recalc_accuracy)
-		clk->accuracy = clk->ops->recalc_accuracy(clk->hw,
-					__clk_get_accuracy(clk->parent));
-	else if (clk->parent)
-		clk->accuracy = clk->parent->accuracy;
+	if (core->ops->recalc_accuracy)
+		core->accuracy = core->ops->recalc_accuracy(core->hw,
+					__clk_get_accuracy(core->parent));
+	else if (core->parent)
+		core->accuracy = core->parent->accuracy;
 	else
-		clk->accuracy = 0;
+		core->accuracy = 0;
 
 	/*
 	 * Set clk's phase.
 	 * Since a phase is by definition relative to its parent, just
 	 * query the current clock phase, or just assume it's in phase.
 	 */
-	if (clk->ops->get_phase)
-		clk->phase = clk->ops->get_phase(clk->hw);
+	if (core->ops->get_phase)
+		core->phase = core->ops->get_phase(core->hw);
 	else
-		clk->phase = 0;
+		core->phase = 0;
 
 	/*
 	 * Set clk's rate.  The preferred method is to use .recalc_rate.  For
@@ -2396,14 +2363,14 @@ static int __clk_init(struct device *dev, struct clk *clk_user)
 	 * parent's rate.  If a clock doesn't have a parent (or is orphaned)
 	 * then rate is set to zero.
 	 */
-	if (clk->ops->recalc_rate)
-		rate = clk->ops->recalc_rate(clk->hw,
-				clk_core_get_rate_nolock(clk->parent));
-	else if (clk->parent)
-		rate = clk->parent->rate;
+	if (core->ops->recalc_rate)
+		rate = core->ops->recalc_rate(core->hw,
+				clk_core_get_rate_nolock(core->parent));
+	else if (core->parent)
+		rate = core->parent->rate;
 	else
 		rate = 0;
-	clk->rate = clk->req_rate = rate;
+	core->rate = core->req_rate = rate;
 
 	/*
 	 * walk the list of orphan clocks and reparent any that are children of
@@ -2412,14 +2379,14 @@ static int __clk_init(struct device *dev, struct clk *clk_user)
 	hlist_for_each_entry_safe(orphan, tmp2, &clk_orphan_list, child_node) {
 		if (orphan->num_parents && orphan->ops->get_parent) {
 			i = orphan->ops->get_parent(orphan->hw);
-			if (!strcmp(clk->name, orphan->parent_names[i]))
-				clk_core_reparent(orphan, clk);
+			if (!strcmp(core->name, orphan->parent_names[i]))
+				clk_core_reparent(orphan, core);
 			continue;
 		}
 
 		for (i = 0; i < orphan->num_parents; i++)
-			if (!strcmp(clk->name, orphan->parent_names[i])) {
-				clk_core_reparent(orphan, clk);
+			if (!strcmp(core->name, orphan->parent_names[i])) {
+				clk_core_reparent(orphan, core);
 				break;
 			}
 	 }
@@ -2432,15 +2399,15 @@ static int __clk_init(struct device *dev, struct clk *clk_user)
 	 * Please consider other ways of solving initialization problems before
 	 * using this callback, as its use is discouraged.
 	 */
-	if (clk->ops->init)
-		clk->ops->init(clk->hw);
+	if (core->ops->init)
+		core->ops->init(core->hw);
 
-	kref_init(&clk->ref);
+	kref_init(&core->ref);
 out:
 	clk_prepare_unlock();
 
 	if (!ret)
-		clk_debug_register(clk);
+		clk_debug_register(core);
 
 	return ret;
 }
@@ -2486,63 +2453,58 @@ void __clk_free_clk(struct clk *clk)
  *
  * clk_register is the primary interface for populating the clock tree with new
  * clock nodes.  It returns a pointer to the newly allocated struct clk which
- * cannot be dereferenced by driver code but may be used in conjuction with the
+ * cannot be dereferenced by driver code but may be used in conjunction with the
  * rest of the clock API.  In the event of an error clk_register will return an
  * error code; drivers must test for an error code after calling clk_register.
  */
 struct clk *clk_register(struct device *dev, struct clk_hw *hw)
 {
 	int i, ret;
-	struct clk_core *clk;
+	struct clk_core *core;
 
-	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
-	if (!clk) {
-		pr_err("%s: could not allocate clk\n", __func__);
+	core = kzalloc(sizeof(*core), GFP_KERNEL);
+	if (!core) {
 		ret = -ENOMEM;
 		goto fail_out;
 	}
 
-	clk->name = kstrdup_const(hw->init->name, GFP_KERNEL);
-	if (!clk->name) {
-		pr_err("%s: could not allocate clk->name\n", __func__);
+	core->name = kstrdup_const(hw->init->name, GFP_KERNEL);
+	if (!core->name) {
 		ret = -ENOMEM;
 		goto fail_name;
 	}
-	clk->ops = hw->init->ops;
+	core->ops = hw->init->ops;
 	if (dev && dev->driver)
-		clk->owner = dev->driver->owner;
-	clk->hw = hw;
-	clk->flags = hw->init->flags;
-	clk->num_parents = hw->init->num_parents;
-	hw->core = clk;
+		core->owner = dev->driver->owner;
+	core->hw = hw;
+	core->flags = hw->init->flags;
+	core->num_parents = hw->init->num_parents;
+	hw->core = core;
 
 	/* allocate local copy in case parent_names is __initdata */
-	clk->parent_names = kcalloc(clk->num_parents, sizeof(char *),
+	core->parent_names = kcalloc(core->num_parents, sizeof(char *),
 					GFP_KERNEL);
 
-	if (!clk->parent_names) {
-		pr_err("%s: could not allocate clk->parent_names\n", __func__);
+	if (!core->parent_names) {
 		ret = -ENOMEM;
 		goto fail_parent_names;
 	}
 
 
 	/* copy each string name in case parent_names is __initdata */
-	for (i = 0; i < clk->num_parents; i++) {
-		clk->parent_names[i] = kstrdup_const(hw->init->parent_names[i],
+	for (i = 0; i < core->num_parents; i++) {
+		core->parent_names[i] = kstrdup_const(hw->init->parent_names[i],
 						GFP_KERNEL);
-		if (!clk->parent_names[i]) {
-			pr_err("%s: could not copy parent_names\n", __func__);
+		if (!core->parent_names[i]) {
 			ret = -ENOMEM;
 			goto fail_parent_names_copy;
 		}
 	}
 
-	INIT_HLIST_HEAD(&clk->clks);
+	INIT_HLIST_HEAD(&core->clks);
 
 	hw->clk = __clk_create_clk(hw, NULL, NULL);
 	if (IS_ERR(hw->clk)) {
-		pr_err("%s: could not allocate per-user clk\n", __func__);
 		ret = PTR_ERR(hw->clk);
 		goto fail_parent_names_copy;
 	}
@@ -2556,35 +2518,32 @@ struct clk *clk_register(struct device *dev, struct clk_hw *hw)
 
 fail_parent_names_copy:
 	while (--i >= 0)
-		kfree_const(clk->parent_names[i]);
-	kfree(clk->parent_names);
+		kfree_const(core->parent_names[i]);
+	kfree(core->parent_names);
 fail_parent_names:
-	kfree_const(clk->name);
+	kfree_const(core->name);
 fail_name:
-	kfree(clk);
+	kfree(core);
 fail_out:
 	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL_GPL(clk_register);
 
-/*
- * Free memory allocated for a clock.
- * Caller must hold prepare_lock.
- */
+/* Free memory allocated for a clock. */
 static void __clk_release(struct kref *ref)
 {
-	struct clk_core *clk = container_of(ref, struct clk_core, ref);
-	int i = clk->num_parents;
+	struct clk_core *core = container_of(ref, struct clk_core, ref);
+	int i = core->num_parents;
 
 	lockdep_assert_held(&prepare_lock);
 
-	kfree(clk->parents);
+	kfree(core->parents);
 	while (--i >= 0)
-		kfree_const(clk->parent_names[i]);
+		kfree_const(core->parent_names[i]);
 
-	kfree(clk->parent_names);
-	kfree_const(clk->name);
-	kfree(clk);
+	kfree(core->parent_names);
+	kfree_const(core->name);
+	kfree(core);
 }
 
 /*
@@ -3068,6 +3027,27 @@ const char *of_clk_get_parent_name(struct device_node *np, int index)
 }
 EXPORT_SYMBOL_GPL(of_clk_get_parent_name);
 
+/**
+ * of_clk_parent_fill() - Fill @parents with names of @np's parents and return
+ * number of parents
+ * @np: Device node pointer associated with clock provider
+ * @parents: pointer to char array that hold the parents' names
+ * @size: size of the @parents array
+ *
+ * Return: number of parents for the clock node.
+ */
+int of_clk_parent_fill(struct device_node *np, const char **parents,
+		       unsigned int size)
+{
+	unsigned int i = 0;
+
+	while (i < size && (parents[i] = of_clk_get_parent_name(np, i)) != NULL)
+		i++;
+
+	return i;
+}
+EXPORT_SYMBOL_GPL(of_clk_parent_fill);
+
 struct clock_provider {
 	of_clk_init_cb_t clk_init_cb;
 	struct device_node *np;
diff --git a/drivers/clk/clkdev.c b/drivers/clk/clkdev.c
index 1fcb6ef2cdac..c0eaf0973bd2 100644
--- a/drivers/clk/clkdev.c
+++ b/drivers/clk/clkdev.c
@@ -177,7 +177,7 @@ struct clk *clk_get_sys(const char *dev_id, const char *con_id)
 	if (!cl)
 		goto out;
 
-	clk = __clk_create_clk(__clk_get_hw(cl->clk), dev_id, con_id);
+	clk = __clk_create_clk(cl->clk_hw, dev_id, con_id);
 	if (IS_ERR(clk))
 		goto out;
 
@@ -215,18 +215,26 @@ void clk_put(struct clk *clk)
 }
 EXPORT_SYMBOL(clk_put);
 
-void clkdev_add(struct clk_lookup *cl)
+static void __clkdev_add(struct clk_lookup *cl)
 {
 	mutex_lock(&clocks_mutex);
 	list_add_tail(&cl->node, &clocks);
 	mutex_unlock(&clocks_mutex);
 }
+
+void clkdev_add(struct clk_lookup *cl)
+{
+	if (!cl->clk_hw)
+		cl->clk_hw = __clk_get_hw(cl->clk);
+	__clkdev_add(cl);
+}
 EXPORT_SYMBOL(clkdev_add);
 
-void __init clkdev_add_table(struct clk_lookup *cl, size_t num)
+void clkdev_add_table(struct clk_lookup *cl, size_t num)
 {
 	mutex_lock(&clocks_mutex);
 	while (num--) {
+		cl->clk_hw = __clk_get_hw(cl->clk);
 		list_add_tail(&cl->node, &clocks);
 		cl++;
 	}
@@ -243,7 +251,7 @@ struct clk_lookup_alloc {
 };
 
 static struct clk_lookup * __init_refok
-vclkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt,
+vclkdev_alloc(struct clk_hw *hw, const char *con_id, const char *dev_fmt,
 	va_list ap)
 {
 	struct clk_lookup_alloc *cla;
@@ -252,7 +260,7 @@ vclkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt,
 	if (!cla)
 		return NULL;
 
-	cla->cl.clk = clk;
+	cla->cl.clk_hw = hw;
 	if (con_id) {
 		strlcpy(cla->con_id, con_id, sizeof(cla->con_id));
 		cla->cl.con_id = cla->con_id;
@@ -266,6 +274,19 @@ vclkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt,
 	return &cla->cl;
 }
 
+static struct clk_lookup *
+vclkdev_create(struct clk_hw *hw, const char *con_id, const char *dev_fmt,
+	va_list ap)
+{
+	struct clk_lookup *cl;
+
+	cl = vclkdev_alloc(hw, con_id, dev_fmt, ap);
+	if (cl)
+		__clkdev_add(cl);
+
+	return cl;
+}
+
 struct clk_lookup * __init_refok
 clkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt, ...)
 {
@@ -273,28 +294,49 @@ clkdev_alloc(struct clk *clk, const char *con_id, const char *dev_fmt, ...)
 	va_list ap;
 
 	va_start(ap, dev_fmt);
-	cl = vclkdev_alloc(clk, con_id, dev_fmt, ap);
+	cl = vclkdev_alloc(__clk_get_hw(clk), con_id, dev_fmt, ap);
 	va_end(ap);
 
 	return cl;
 }
 EXPORT_SYMBOL(clkdev_alloc);
 
-int clk_add_alias(const char *alias, const char *alias_dev_name, char *id,
-	struct device *dev)
+/**
+ * clkdev_create - allocate and add a clkdev lookup structure
+ * @clk: struct clk to associate with all clk_lookups
+ * @con_id: connection ID string on device
+ * @dev_fmt: format string describing device name
+ *
+ * Returns a clk_lookup structure, which can be later unregistered and
+ * freed.
+ */
+struct clk_lookup *clkdev_create(struct clk *clk, const char *con_id,
+	const char *dev_fmt, ...)
 {
-	struct clk *r = clk_get(dev, id);
+	struct clk_lookup *cl;
+	va_list ap;
+
+	va_start(ap, dev_fmt);
+	cl = vclkdev_create(__clk_get_hw(clk), con_id, dev_fmt, ap);
+	va_end(ap);
+
+	return cl;
+}
+EXPORT_SYMBOL_GPL(clkdev_create);
+
+int clk_add_alias(const char *alias, const char *alias_dev_name,
+	const char *con_id, struct device *dev)
+{
+	struct clk *r = clk_get(dev, con_id);
 	struct clk_lookup *l;
 
 	if (IS_ERR(r))
 		return PTR_ERR(r);
 
-	l = clkdev_alloc(r, alias, alias_dev_name);
+	l = clkdev_create(r, alias, "%s", alias_dev_name);
 	clk_put(r);
-	if (!l)
-		return -ENODEV;
-	clkdev_add(l);
-	return 0;
+
+	return l ? 0 : -ENODEV;
 }
 EXPORT_SYMBOL(clk_add_alias);
 
@@ -334,15 +376,10 @@ int clk_register_clkdev(struct clk *clk, const char *con_id,
 		return PTR_ERR(clk);
 
 	va_start(ap, dev_fmt);
-	cl = vclkdev_alloc(clk, con_id, dev_fmt, ap);
+	cl = vclkdev_create(__clk_get_hw(clk), con_id, dev_fmt, ap);
 	va_end(ap);
 
-	if (!cl)
-		return -ENOMEM;
-
-	clkdev_add(cl);
-
-	return 0;
+	return cl ? 0 : -ENOMEM;
 }
 EXPORT_SYMBOL(clk_register_clkdev);
 
@@ -365,8 +402,8 @@ int clk_register_clkdevs(struct clk *clk, struct clk_lookup *cl, size_t num)
 		return PTR_ERR(clk);
 
 	for (i = 0; i < num; i++, cl++) {
-		cl->clk = clk;
-		clkdev_add(cl);
+		cl->clk_hw = __clk_get_hw(clk);
+		__clkdev_add(cl);
 	}
 
 	return 0;
diff --git a/drivers/clk/h8300/Makefile b/drivers/clk/h8300/Makefile
new file mode 100644
index 000000000000..b86427c31fca
--- /dev/null
+++ b/drivers/clk/h8300/Makefile
@@ -0,0 +1,2 @@
+obj-y += clk-div.o
+obj-$(CONFIG_H8S2678) += clk-h8s2678.o
diff --git a/drivers/clk/h8300/clk-div.c b/drivers/clk/h8300/clk-div.c
new file mode 100644
index 000000000000..56f9eba91b83
--- /dev/null
+++ b/drivers/clk/h8300/clk-div.c
@@ -0,0 +1,53 @@
+/*
+ * H8/300 divide clock driver
+ *
+ * Copyright 2015 Yoshinori Sato <ysato@users.sourceforge.jp>
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+static DEFINE_SPINLOCK(clklock);
+
+static void __init h8300_div_clk_setup(struct device_node *node)
+{
+	unsigned int num_parents;
+	struct clk *clk;
+	const char *clk_name = node->name;
+	const char *parent_name;
+	void __iomem *divcr = NULL;
+	int width;
+
+	num_parents = of_clk_get_parent_count(node);
+	if (num_parents < 1) {
+		pr_err("%s: no parent found", clk_name);
+		return;
+	}
+
+	divcr = of_iomap(node, 0);
+	if (divcr == NULL) {
+		pr_err("%s: failed to map divide register", clk_name);
+		goto error;
+	}
+
+	parent_name = of_clk_get_parent_name(node, 0);
+	of_property_read_u32(node, "renesas,width", &width);
+	clk = clk_register_divider(NULL, clk_name, parent_name,
+				   CLK_SET_RATE_GATE, divcr, 0, width,
+				   CLK_DIVIDER_POWER_OF_TWO, &clklock);
+	if (!IS_ERR(clk)) {
+		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+		return;
+	}
+	pr_err("%s: failed to register %s div clock (%ld)\n",
+	       __func__, clk_name, PTR_ERR(clk));
+error:
+	if (divcr)
+		iounmap(divcr);
+}
+
+CLK_OF_DECLARE(h8300_div_clk, "renesas,h8300-div-clock", h8300_div_clk_setup);
diff --git a/drivers/clk/h8300/clk-h8s2678.c b/drivers/clk/h8300/clk-h8s2678.c
new file mode 100644
index 000000000000..4701b093e497
--- /dev/null
+++ b/drivers/clk/h8300/clk-h8s2678.c
@@ -0,0 +1,146 @@
+/*
+ * H8S2678 clock driver
+ *
+ * Copyright 2015 Yoshinori Sato <ysato@users.sourceforge.jp>
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/of_address.h>
+
+static DEFINE_SPINLOCK(clklock);
+
+#define MAX_FREQ 33333333
+#define MIN_FREQ  8000000
+
+struct pll_clock {
+	struct clk_hw hw;
+	void __iomem *sckcr;
+	void __iomem *pllcr;
+};
+
+#define to_pll_clock(_hw) container_of(_hw, struct pll_clock, hw)
+
+static unsigned long pll_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct pll_clock *pll_clock = to_pll_clock(hw);
+	int mul = 1 << (ctrl_inb((unsigned long)pll_clock->pllcr) & 3);
+
+	return parent_rate * mul;
+}
+
+static long pll_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *prate)
+{
+	int i, m = -1;
+	long offset[3];
+
+	if (rate > MAX_FREQ)
+		rate = MAX_FREQ;
+	if (rate < MIN_FREQ)
+		rate = MIN_FREQ;
+
+	for (i = 0; i < 3; i++)
+		offset[i] = abs(rate - (*prate * (1 << i)));
+	for (i = 0; i < 3; i++)
+		if (m < 0)
+			m = i;
+		else
+			m = (offset[i] < offset[m])?i:m;
+
+	return *prate * (1 << m);
+}
+
+static int pll_set_rate(struct clk_hw *hw, unsigned long rate,
+			unsigned long parent_rate)
+{
+	int pll;
+	unsigned char val;
+	unsigned long flags;
+	struct pll_clock *pll_clock = to_pll_clock(hw);
+
+	pll = ((rate / parent_rate) / 2) & 0x03;
+	spin_lock_irqsave(&clklock, flags);
+	val = ctrl_inb((unsigned long)pll_clock->sckcr);
+	val |= 0x08;
+	ctrl_outb(val, (unsigned long)pll_clock->sckcr);
+	val = ctrl_inb((unsigned long)pll_clock->pllcr);
+	val &= ~0x03;
+	val |= pll;
+	ctrl_outb(val, (unsigned long)pll_clock->pllcr);
+	spin_unlock_irqrestore(&clklock, flags);
+	return 0;
+}
+
+static const struct clk_ops pll_ops = {
+	.recalc_rate = pll_recalc_rate,
+	.round_rate = pll_round_rate,
+	.set_rate = pll_set_rate,
+};
+
+static void __init h8s2678_pll_clk_setup(struct device_node *node)
+{
+	unsigned int num_parents;
+	struct clk *clk;
+	const char *clk_name = node->name;
+	const char *parent_name;
+	struct pll_clock *pll_clock;
+	struct clk_init_data init;
+
+	num_parents = of_clk_get_parent_count(node);
+	if (num_parents < 1) {
+		pr_err("%s: no parent found", clk_name);
+		return;
+	}
+
+
+	pll_clock = kzalloc(sizeof(struct pll_clock), GFP_KERNEL);
+	if (!pll_clock) {
+		pr_err("%s: failed to alloc memory", clk_name);
+		return;
+	}
+
+	pll_clock->sckcr = of_iomap(node, 0);
+	if (pll_clock->sckcr == NULL) {
+		pr_err("%s: failed to map divide register", clk_name);
+		goto free_clock;
+	}
+
+	pll_clock->pllcr = of_iomap(node, 1);
+	if (pll_clock->pllcr == NULL) {
+		pr_err("%s: failed to map multiply register", clk_name);
+		goto unmap_sckcr;
+	}
+
+	parent_name = of_clk_get_parent_name(node, 0);
+	init.name = clk_name;
+	init.ops = &pll_ops;
+	init.flags = CLK_IS_BASIC;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	pll_clock->hw.init = &init;
+
+	clk = clk_register(NULL, &pll_clock->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to register %s div clock (%ld)\n",
+		       __func__, clk_name, PTR_ERR(clk));
+		goto unmap_pllcr;
+	}
+
+	of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	return;
+
+unmap_pllcr:
+	iounmap(pll_clock->pllcr);
+unmap_sckcr:
+	iounmap(pll_clock->sckcr);
+free_clock:
+	kfree(pll_clock);
+}
+
+CLK_OF_DECLARE(h8s2678_div_clk, "renesas,h8s2678-pll-clock",
+	       h8s2678_pll_clk_setup);
diff --git a/drivers/clk/hisilicon/Kconfig b/drivers/clk/hisilicon/Kconfig
new file mode 100644
index 000000000000..b4165ba75d9f
--- /dev/null
+++ b/drivers/clk/hisilicon/Kconfig
@@ -0,0 +1,6 @@
+config COMMON_CLK_HI6220
+	bool "Hi6220 Clock Driver"
+	depends on ARCH_HISI || COMPILE_TEST
+	default ARCH_HISI
+	help
+	  Build the Hisilicon Hi6220 clock driver based on the common clock framework.
diff --git a/drivers/clk/hisilicon/Makefile b/drivers/clk/hisilicon/Makefile
index 038c02f4d0e7..48f0116a032a 100644
--- a/drivers/clk/hisilicon/Makefile
+++ b/drivers/clk/hisilicon/Makefile
@@ -2,8 +2,9 @@
 # Hisilicon Clock specific Makefile
 #
 
-obj-y	+= clk.o clkgate-separated.o
+obj-y	+= clk.o clkgate-separated.o clkdivider-hi6220.o
 
 obj-$(CONFIG_ARCH_HI3xxx)	+= clk-hi3620.o
 obj-$(CONFIG_ARCH_HIP04)	+= clk-hip04.o
 obj-$(CONFIG_ARCH_HIX5HD2)	+= clk-hix5hd2.o
+obj-$(CONFIG_COMMON_CLK_HI6220)	+= clk-hi6220.o
diff --git a/drivers/clk/hisilicon/clk-hi3620.c b/drivers/clk/hisilicon/clk-hi3620.c
index 472dd2cb10b3..715d34a5ef9b 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 *const timer0_mux_p[] __initconst = { "osc32k", "timerclk01", };
+static const char *const timer1_mux_p[] __initconst = { "osc32k", "timerclk01", };
+static const char *const timer2_mux_p[] __initconst = { "osc32k", "timerclk23", };
+static const char *const timer3_mux_p[] __initconst = { "osc32k", "timerclk23", };
+static const char *const timer4_mux_p[] __initconst = { "osc32k", "timerclk45", };
+static const char *const timer5_mux_p[] __initconst = { "osc32k", "timerclk45", };
+static const char *const timer6_mux_p[] __initconst = { "osc32k", "timerclk67", };
+static const char *const timer7_mux_p[] __initconst = { "osc32k", "timerclk67", };
+static const char *const timer8_mux_p[] __initconst = { "osc32k", "timerclk89", };
+static const char *const timer9_mux_p[] __initconst = { "osc32k", "timerclk89", };
+static const char *const uart0_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *const uart1_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *const uart2_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *const uart3_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *const uart4_mux_p[] __initconst = { "osc26m", "pclk", };
+static const char *const spi0_mux_p[] __initconst = { "osc26m", "rclk_cfgaxi", };
+static const char *const spi1_mux_p[] __initconst = { "osc26m", "rclk_cfgaxi", };
+static const char *const 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 *const saxi_mux_p[] __initconst = { "armpll3", "armpll2", };
+static const char *const pwm0_mux_p[] __initconst = { "osc32k", "osc26m", };
+static const char *const pwm1_mux_p[] __initconst = { "osc32k", "osc26m", };
+static const char *const sd_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const mmc1_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const mmc1_mux2_p[] __initconst = { "osc26m", "mmc1_div", };
+static const char *const g2d_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const venc_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const vdec_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const vpp_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const edc0_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const 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 *const edc1_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const 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 *const rclk_hsic_p[] __initconst = { "armpll3", "armpll2", };
+static const char *const mmc2_mux_p[] __initconst = { "armpll2", "armpll3", };
+static const char *const mmc3_mux_p[] __initconst = { "armpll2", "armpll3", };
 
 
 /* fixed rate clocks */
diff --git a/drivers/clk/hisilicon/clk-hi6220.c b/drivers/clk/hisilicon/clk-hi6220.c
new file mode 100644
index 000000000000..4563343b6420
--- /dev/null
+++ b/drivers/clk/hisilicon/clk-hi6220.c
@@ -0,0 +1,284 @@
+/*
+ * Hisilicon Hi6220 clock driver
+ *
+ * Copyright (c) 2015 Hisilicon Limited.
+ *
+ * Author: Bintian Wang <bintian.wang@huawei.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/slab.h>
+
+#include <dt-bindings/clock/hi6220-clock.h>
+
+#include "clk.h"
+
+
+/* clocks in AO (always on) controller */
+static struct hisi_fixed_rate_clock hi6220_fixed_rate_clks[] __initdata = {
+	{ HI6220_REF32K,	"ref32k",	NULL, CLK_IS_ROOT, 32764,     },
+	{ HI6220_CLK_TCXO,	"clk_tcxo",	NULL, CLK_IS_ROOT, 19200000,  },
+	{ HI6220_MMC1_PAD,	"mmc1_pad",	NULL, CLK_IS_ROOT, 100000000, },
+	{ HI6220_MMC2_PAD,	"mmc2_pad",	NULL, CLK_IS_ROOT, 100000000, },
+	{ HI6220_MMC0_PAD,	"mmc0_pad",	NULL, CLK_IS_ROOT, 200000000, },
+	{ HI6220_PLL_BBP,	"bbppll0",	NULL, CLK_IS_ROOT, 245760000, },
+	{ HI6220_PLL_GPU,	"gpupll",	NULL, CLK_IS_ROOT, 1000000000,},
+	{ HI6220_PLL1_DDR,	"ddrpll1",	NULL, CLK_IS_ROOT, 1066000000,},
+	{ HI6220_PLL_SYS,	"syspll",	NULL, CLK_IS_ROOT, 1200000000,},
+	{ HI6220_PLL_SYS_MEDIA,	"media_syspll",	NULL, CLK_IS_ROOT, 1200000000,},
+	{ HI6220_DDR_SRC,	"ddr_sel_src",  NULL, CLK_IS_ROOT, 1200000000,},
+	{ HI6220_PLL_MEDIA,	"media_pll",    NULL, CLK_IS_ROOT, 1440000000,},
+	{ HI6220_PLL_DDR,	"ddrpll0",      NULL, CLK_IS_ROOT, 1600000000,},
+};
+
+static struct hisi_fixed_factor_clock hi6220_fixed_factor_clks[] __initdata = {
+	{ HI6220_300M,         "clk_300m",    "syspll",          1, 4, 0, },
+	{ HI6220_150M,         "clk_150m",    "clk_300m",        1, 2, 0, },
+	{ HI6220_PICOPHY_SRC,  "picophy_src", "clk_150m",        1, 4, 0, },
+	{ HI6220_MMC0_SRC_SEL, "mmc0srcsel",  "mmc0_sel",        1, 8, 0, },
+	{ HI6220_MMC1_SRC_SEL, "mmc1srcsel",  "mmc1_sel",        1, 8, 0, },
+	{ HI6220_MMC2_SRC_SEL, "mmc2srcsel",  "mmc2_sel",        1, 8, 0, },
+	{ HI6220_VPU_CODEC,    "vpucodec",    "codec_jpeg_aclk", 1, 2, 0, },
+	{ HI6220_MMC0_SMP,     "mmc0_sample", "mmc0_sel",        1, 8, 0, },
+	{ HI6220_MMC1_SMP,     "mmc1_sample", "mmc1_sel",        1, 8, 0, },
+	{ HI6220_MMC2_SMP,     "mmc2_sample", "mmc2_sel",        1, 8, 0, },
+};
+
+static struct hisi_gate_clock hi6220_separated_gate_clks_ao[] __initdata = {
+	{ HI6220_WDT0_PCLK,   "wdt0_pclk",   "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 12, 0, },
+	{ HI6220_WDT1_PCLK,   "wdt1_pclk",   "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 13, 0, },
+	{ HI6220_WDT2_PCLK,   "wdt2_pclk",   "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 14, 0, },
+	{ HI6220_TIMER0_PCLK, "timer0_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 15, 0, },
+	{ HI6220_TIMER1_PCLK, "timer1_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 16, 0, },
+	{ HI6220_TIMER2_PCLK, "timer2_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 17, 0, },
+	{ HI6220_TIMER3_PCLK, "timer3_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 18, 0, },
+	{ HI6220_TIMER4_PCLK, "timer4_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 19, 0, },
+	{ HI6220_TIMER5_PCLK, "timer5_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 20, 0, },
+	{ HI6220_TIMER6_PCLK, "timer6_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 21, 0, },
+	{ HI6220_TIMER7_PCLK, "timer7_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 22, 0, },
+	{ HI6220_TIMER8_PCLK, "timer8_pclk", "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 23, 0, },
+	{ HI6220_UART0_PCLK,  "uart0_pclk",  "clk_tcxo", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x630, 24, 0, },
+};
+
+static void __init hi6220_clk_ao_init(struct device_node *np)
+{
+	struct hisi_clock_data *clk_data_ao;
+
+	clk_data_ao = hisi_clk_init(np, HI6220_AO_NR_CLKS);
+	if (!clk_data_ao)
+		return;
+
+	hisi_clk_register_fixed_rate(hi6220_fixed_rate_clks,
+				ARRAY_SIZE(hi6220_fixed_rate_clks), clk_data_ao);
+
+	hisi_clk_register_fixed_factor(hi6220_fixed_factor_clks,
+				ARRAY_SIZE(hi6220_fixed_factor_clks), clk_data_ao);
+
+	hisi_clk_register_gate_sep(hi6220_separated_gate_clks_ao,
+				ARRAY_SIZE(hi6220_separated_gate_clks_ao), clk_data_ao);
+}
+CLK_OF_DECLARE(hi6220_clk_ao, "hisilicon,hi6220-aoctrl", hi6220_clk_ao_init);
+
+
+/* clocks in sysctrl */
+static const char *mmc0_mux0_p[] __initdata = { "pll_ddr_gate", "syspll", };
+static const char *mmc0_mux1_p[] __initdata = { "mmc0_mux0", "pll_media_gate", };
+static const char *mmc0_src_p[] __initdata = { "mmc0srcsel", "mmc0_div", };
+static const char *mmc1_mux0_p[] __initdata = { "pll_ddr_gate", "syspll", };
+static const char *mmc1_mux1_p[] __initdata = { "mmc1_mux0", "pll_media_gate", };
+static const char *mmc1_src_p[]  __initdata = { "mmc1srcsel", "mmc1_div", };
+static const char *mmc2_mux0_p[] __initdata = { "pll_ddr_gate", "syspll", };
+static const char *mmc2_mux1_p[] __initdata = { "mmc2_mux0", "pll_media_gate", };
+static const char *mmc2_src_p[]  __initdata = { "mmc2srcsel", "mmc2_div", };
+static const char *mmc0_sample_in[] __initdata = { "mmc0_sample", "mmc0_pad", };
+static const char *mmc1_sample_in[] __initdata = { "mmc1_sample", "mmc1_pad", };
+static const char *mmc2_sample_in[] __initdata = { "mmc2_sample", "mmc2_pad", };
+static const char *uart1_src[] __initdata = { "clk_tcxo", "clk_150m", };
+static const char *uart2_src[] __initdata = { "clk_tcxo", "clk_150m", };
+static const char *uart3_src[] __initdata = { "clk_tcxo", "clk_150m", };
+static const char *uart4_src[] __initdata = { "clk_tcxo", "clk_150m", };
+static const char *hifi_src[] __initdata = { "syspll", "pll_media_gate", };
+
+static struct hisi_gate_clock hi6220_separated_gate_clks_sys[] __initdata = {
+	{ HI6220_MMC0_CLK,      "mmc0_clk",      "mmc0_src",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x200, 0,  0, },
+	{ HI6220_MMC0_CIUCLK,   "mmc0_ciuclk",   "mmc0_smp_in",    CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x200, 0,  0, },
+	{ HI6220_MMC1_CLK,      "mmc1_clk",      "mmc1_src",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x200, 1,  0, },
+	{ HI6220_MMC1_CIUCLK,   "mmc1_ciuclk",   "mmc1_smp_in",    CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x200, 1,  0, },
+	{ HI6220_MMC2_CLK,      "mmc2_clk",      "mmc2_src",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x200, 2,  0, },
+	{ HI6220_MMC2_CIUCLK,   "mmc2_ciuclk",   "mmc2_smp_in",    CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x200, 2,  0, },
+	{ HI6220_USBOTG_HCLK,   "usbotg_hclk",   "clk_bus",        CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x200, 4,  0, },
+	{ HI6220_CLK_PICOPHY,   "clk_picophy",   "cs_dapb",        CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x200, 5,  0, },
+	{ HI6220_HIFI,          "hifi_clk",      "hifi_div",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x210, 0,  0, },
+	{ HI6220_DACODEC_PCLK,  "dacodec_pclk",  "clk_bus",        CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x210, 5,  0, },
+	{ HI6220_EDMAC_ACLK,    "edmac_aclk",    "clk_bus",        CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x220, 2,  0, },
+	{ HI6220_CS_ATB,        "cs_atb",        "cs_atb_div",     CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 0,  0, },
+	{ HI6220_I2C0_CLK,      "i2c0_clk",      "clk_150m",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 1,  0, },
+	{ HI6220_I2C1_CLK,      "i2c1_clk",      "clk_150m",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 2,  0, },
+	{ HI6220_I2C2_CLK,      "i2c2_clk",      "clk_150m",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 3,  0, },
+	{ HI6220_I2C3_CLK,      "i2c3_clk",      "clk_150m",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 4,  0, },
+	{ HI6220_UART1_PCLK,    "uart1_pclk",    "uart1_src",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 5,  0, },
+	{ HI6220_UART2_PCLK,    "uart2_pclk",    "uart2_src",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 6,  0, },
+	{ HI6220_UART3_PCLK,    "uart3_pclk",    "uart3_src",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 7,  0, },
+	{ HI6220_UART4_PCLK,    "uart4_pclk",    "uart4_src",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 8,  0, },
+	{ HI6220_SPI_CLK,       "spi_clk",       "clk_150m",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 9,  0, },
+	{ HI6220_TSENSOR_CLK,   "tsensor_clk",   "clk_bus",        CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x230, 12, 0, },
+	{ HI6220_MMU_CLK,       "mmu_clk",       "ddrc_axi1",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x240, 11, 0, },
+	{ HI6220_HIFI_SEL,      "hifi_sel",      "hifi_src",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 0,  0, },
+	{ HI6220_MMC0_SYSPLL,   "mmc0_syspll",   "syspll",         CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 1,  0, },
+	{ HI6220_MMC1_SYSPLL,   "mmc1_syspll",   "syspll",         CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 2,  0, },
+	{ HI6220_MMC2_SYSPLL,   "mmc2_syspll",   "syspll",         CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 3,  0, },
+	{ HI6220_MMC0_SEL,      "mmc0_sel",      "mmc0_mux1",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 6,  0, },
+	{ HI6220_MMC1_SEL,      "mmc1_sel",      "mmc1_mux1",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 7,  0, },
+	{ HI6220_BBPPLL_SEL,    "bbppll_sel",    "pll0_bbp_gate",  CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 9,  0, },
+	{ HI6220_MEDIA_PLL_SRC, "media_pll_src", "pll_media_gate", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 10, 0, },
+	{ HI6220_MMC2_SEL,      "mmc2_sel",      "mmc2_mux1",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 11, 0, },
+	{ HI6220_CS_ATB_SYSPLL, "cs_atb_syspll", "syspll",         CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x270, 12, 0, },
+};
+
+static struct hisi_mux_clock hi6220_mux_clks_sys[] __initdata = {
+	{ HI6220_MMC0_SRC,    "mmc0_src",    mmc0_src_p,     ARRAY_SIZE(mmc0_src_p),     CLK_SET_RATE_PARENT, 0x4,   0,  1, 0, },
+	{ HI6220_MMC0_SMP_IN, "mmc0_smp_in", mmc0_sample_in, ARRAY_SIZE(mmc0_sample_in), CLK_SET_RATE_PARENT, 0x4,   0,  1, 0, },
+	{ HI6220_MMC1_SRC,    "mmc1_src",    mmc1_src_p,     ARRAY_SIZE(mmc1_src_p),     CLK_SET_RATE_PARENT, 0x4,   2,  1, 0, },
+	{ HI6220_MMC1_SMP_IN, "mmc1_smp_in", mmc1_sample_in, ARRAY_SIZE(mmc1_sample_in), CLK_SET_RATE_PARENT, 0x4,   2,  1, 0, },
+	{ HI6220_MMC2_SRC,    "mmc2_src",    mmc2_src_p,     ARRAY_SIZE(mmc2_src_p),     CLK_SET_RATE_PARENT, 0x4,   4,  1, 0, },
+	{ HI6220_MMC2_SMP_IN, "mmc2_smp_in", mmc2_sample_in, ARRAY_SIZE(mmc2_sample_in), CLK_SET_RATE_PARENT, 0x4,   4,  1, 0, },
+	{ HI6220_HIFI_SRC,    "hifi_src",    hifi_src,       ARRAY_SIZE(hifi_src),       CLK_SET_RATE_PARENT, 0x400, 0,  1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_UART1_SRC,   "uart1_src",   uart1_src,      ARRAY_SIZE(uart1_src),      CLK_SET_RATE_PARENT, 0x400, 1,  1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_UART2_SRC,   "uart2_src",   uart2_src,      ARRAY_SIZE(uart2_src),      CLK_SET_RATE_PARENT, 0x400, 2,  1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_UART3_SRC,   "uart3_src",   uart3_src,      ARRAY_SIZE(uart3_src),      CLK_SET_RATE_PARENT, 0x400, 3,  1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_UART4_SRC,   "uart4_src",   uart4_src,      ARRAY_SIZE(uart4_src),      CLK_SET_RATE_PARENT, 0x400, 4,  1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_MMC0_MUX0,   "mmc0_mux0",   mmc0_mux0_p,    ARRAY_SIZE(mmc0_mux0_p),    CLK_SET_RATE_PARENT, 0x400, 5,  1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_MMC1_MUX0,   "mmc1_mux0",   mmc1_mux0_p,    ARRAY_SIZE(mmc1_mux0_p),    CLK_SET_RATE_PARENT, 0x400, 11, 1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_MMC2_MUX0,   "mmc2_mux0",   mmc2_mux0_p,    ARRAY_SIZE(mmc2_mux0_p),    CLK_SET_RATE_PARENT, 0x400, 12, 1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_MMC0_MUX1,   "mmc0_mux1",   mmc0_mux1_p,    ARRAY_SIZE(mmc0_mux1_p),    CLK_SET_RATE_PARENT, 0x400, 13, 1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_MMC1_MUX1,   "mmc1_mux1",   mmc1_mux1_p,    ARRAY_SIZE(mmc1_mux1_p),    CLK_SET_RATE_PARENT, 0x400, 14, 1, CLK_MUX_HIWORD_MASK,},
+	{ HI6220_MMC2_MUX1,   "mmc2_mux1",   mmc2_mux1_p,    ARRAY_SIZE(mmc2_mux1_p),    CLK_SET_RATE_PARENT, 0x400, 15, 1, CLK_MUX_HIWORD_MASK,},
+};
+
+static struct hi6220_divider_clock hi6220_div_clks_sys[] __initdata = {
+	{ HI6220_CLK_BUS,     "clk_bus",     "clk_300m",      CLK_SET_RATE_PARENT, 0x490, 0,  4, 7, },
+	{ HI6220_MMC0_DIV,    "mmc0_div",    "mmc0_syspll",   CLK_SET_RATE_PARENT, 0x494, 0,  6, 7, },
+	{ HI6220_MMC1_DIV,    "mmc1_div",    "mmc1_syspll",   CLK_SET_RATE_PARENT, 0x498, 0,  6, 7, },
+	{ HI6220_MMC2_DIV,    "mmc2_div",    "mmc2_syspll",   CLK_SET_RATE_PARENT, 0x49c, 0,  6, 7, },
+	{ HI6220_HIFI_DIV,    "hifi_div",    "hifi_sel",      CLK_SET_RATE_PARENT, 0x4a0, 0,  4, 7, },
+	{ HI6220_BBPPLL0_DIV, "bbppll0_div", "bbppll_sel",    CLK_SET_RATE_PARENT, 0x4a0, 8,  6, 15,},
+	{ HI6220_CS_DAPB,     "cs_dapb",     "picophy_src",   CLK_SET_RATE_PARENT, 0x4a0, 24, 2, 31,},
+	{ HI6220_CS_ATB_DIV,  "cs_atb_div",  "cs_atb_syspll", CLK_SET_RATE_PARENT, 0x4a4, 0,  4, 7, },
+};
+
+static void __init hi6220_clk_sys_init(struct device_node *np)
+{
+	struct hisi_clock_data *clk_data;
+
+	clk_data = hisi_clk_init(np, HI6220_SYS_NR_CLKS);
+	if (!clk_data)
+		return;
+
+	hisi_clk_register_gate_sep(hi6220_separated_gate_clks_sys,
+			ARRAY_SIZE(hi6220_separated_gate_clks_sys), clk_data);
+
+	hisi_clk_register_mux(hi6220_mux_clks_sys,
+			ARRAY_SIZE(hi6220_mux_clks_sys), clk_data);
+
+	hi6220_clk_register_divider(hi6220_div_clks_sys,
+			ARRAY_SIZE(hi6220_div_clks_sys), clk_data);
+}
+CLK_OF_DECLARE(hi6220_clk_sys, "hisilicon,hi6220-sysctrl", hi6220_clk_sys_init);
+
+
+/* clocks in media controller */
+static const char *clk_1000_1200_src[] __initdata = { "pll_gpu_gate", "media_syspll_src", };
+static const char *clk_1440_1200_src[] __initdata = { "media_syspll_src", "media_pll_src", };
+static const char *clk_1000_1440_src[] __initdata = { "pll_gpu_gate", "media_pll_src", };
+
+static struct hisi_gate_clock hi6220_separated_gate_clks_media[] __initdata = {
+	{ HI6220_DSI_PCLK,       "dsi_pclk",         "vpucodec",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 0,  0, },
+	{ HI6220_G3D_PCLK,       "g3d_pclk",         "vpucodec",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 1,  0, },
+	{ HI6220_ACLK_CODEC_VPU, "aclk_codec_vpu",   "ade_core_src",  CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 3,  0, },
+	{ HI6220_ISP_SCLK,       "isp_sclk",         "isp_sclk_src",  CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 5,  0, },
+	{ HI6220_ADE_CORE,	 "ade_core",	     "ade_core_src",  CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 6,  0, },
+	{ HI6220_MED_MMU,        "media_mmu",        "mmu_clk",       CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 8,  0, },
+	{ HI6220_CFG_CSI4PHY,    "cfg_csi4phy",      "clk_tcxo",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 9,  0, },
+	{ HI6220_CFG_CSI2PHY,    "cfg_csi2phy",      "clk_tcxo",      CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 10, 0, },
+	{ HI6220_ISP_SCLK_GATE,  "isp_sclk_gate",    "media_pll_src", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 11, 0, },
+	{ HI6220_ISP_SCLK_GATE1, "isp_sclk_gate1",   "media_pll_src", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 12, 0, },
+	{ HI6220_ADE_CORE_GATE,  "ade_core_gate",    "media_pll_src", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 14, 0, },
+	{ HI6220_CODEC_VPU_GATE, "codec_vpu_gate",   "clk_1000_1440", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 15, 0, },
+	{ HI6220_MED_SYSPLL,     "media_syspll_src", "media_syspll",  CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x520, 17, 0, },
+};
+
+static struct hisi_mux_clock hi6220_mux_clks_media[] __initdata = {
+	{ HI6220_1440_1200, "clk_1440_1200", clk_1440_1200_src, ARRAY_SIZE(clk_1440_1200_src), CLK_SET_RATE_PARENT, 0x51c, 0, 1, 0, },
+	{ HI6220_1000_1200, "clk_1000_1200", clk_1000_1200_src, ARRAY_SIZE(clk_1000_1200_src), CLK_SET_RATE_PARENT, 0x51c, 1, 1, 0, },
+	{ HI6220_1000_1440, "clk_1000_1440", clk_1000_1440_src, ARRAY_SIZE(clk_1000_1440_src), CLK_SET_RATE_PARENT, 0x51c, 6, 1, 0, },
+};
+
+static struct hi6220_divider_clock hi6220_div_clks_media[] __initdata = {
+	{ HI6220_CODEC_JPEG,    "codec_jpeg_aclk", "media_pll_src",  CLK_SET_RATE_PARENT, 0xcbc, 0,  4, 23, },
+	{ HI6220_ISP_SCLK_SRC,  "isp_sclk_src",    "isp_sclk_gate",  CLK_SET_RATE_PARENT, 0xcbc, 8,  4, 15, },
+	{ HI6220_ISP_SCLK1,     "isp_sclk1",       "isp_sclk_gate1", CLK_SET_RATE_PARENT, 0xcbc, 24, 4, 31, },
+	{ HI6220_ADE_CORE_SRC,  "ade_core_src",    "ade_core_gate",  CLK_SET_RATE_PARENT, 0xcc0, 16, 3, 23, },
+	{ HI6220_ADE_PIX_SRC,   "ade_pix_src",     "clk_1440_1200",  CLK_SET_RATE_PARENT, 0xcc0, 24, 6, 31, },
+	{ HI6220_G3D_CLK,       "g3d_clk",         "clk_1000_1200",  CLK_SET_RATE_PARENT, 0xcc4, 8,  4, 15, },
+	{ HI6220_CODEC_VPU_SRC, "codec_vpu_src",   "codec_vpu_gate", CLK_SET_RATE_PARENT, 0xcc4, 24, 6, 31, },
+};
+
+static void __init hi6220_clk_media_init(struct device_node *np)
+{
+	struct hisi_clock_data *clk_data;
+
+	clk_data = hisi_clk_init(np, HI6220_MEDIA_NR_CLKS);
+	if (!clk_data)
+		return;
+
+	hisi_clk_register_gate_sep(hi6220_separated_gate_clks_media,
+				ARRAY_SIZE(hi6220_separated_gate_clks_media), clk_data);
+
+	hisi_clk_register_mux(hi6220_mux_clks_media,
+				ARRAY_SIZE(hi6220_mux_clks_media), clk_data);
+
+	hi6220_clk_register_divider(hi6220_div_clks_media,
+				ARRAY_SIZE(hi6220_div_clks_media), clk_data);
+}
+CLK_OF_DECLARE(hi6220_clk_media, "hisilicon,hi6220-mediactrl", hi6220_clk_media_init);
+
+
+/* clocks in pmctrl */
+static struct hisi_gate_clock hi6220_gate_clks_power[] __initdata = {
+	{ HI6220_PLL_GPU_GATE,   "pll_gpu_gate",   "gpupll",    CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x8,  0,  0, },
+	{ HI6220_PLL1_DDR_GATE,  "pll1_ddr_gate",  "ddrpll1",   CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x10, 0,  0, },
+	{ HI6220_PLL_DDR_GATE,   "pll_ddr_gate",   "ddrpll0",   CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x18, 0,  0, },
+	{ HI6220_PLL_MEDIA_GATE, "pll_media_gate", "media_pll", CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x38, 0,  0, },
+	{ HI6220_PLL0_BBP_GATE,  "pll0_bbp_gate",  "bbppll0",   CLK_SET_RATE_PARENT|CLK_IGNORE_UNUSED, 0x48, 0,  0, },
+};
+
+static struct hi6220_divider_clock hi6220_div_clks_power[] __initdata = {
+	{ HI6220_DDRC_SRC,  "ddrc_src",  "ddr_sel_src", CLK_SET_RATE_PARENT, 0x5a8, 0, 4, 0, },
+	{ HI6220_DDRC_AXI1, "ddrc_axi1", "ddrc_src",    CLK_SET_RATE_PARENT, 0x5a8, 8, 2, 0, },
+};
+
+static void __init hi6220_clk_power_init(struct device_node *np)
+{
+	struct hisi_clock_data *clk_data;
+
+	clk_data = hisi_clk_init(np, HI6220_POWER_NR_CLKS);
+	if (!clk_data)
+		return;
+
+	hisi_clk_register_gate(hi6220_gate_clks_power,
+				ARRAY_SIZE(hi6220_gate_clks_power), clk_data);
+
+	hi6220_clk_register_divider(hi6220_div_clks_power,
+				ARRAY_SIZE(hi6220_div_clks_power), clk_data);
+}
+CLK_OF_DECLARE(hi6220_clk_power, "hisilicon,hi6220-pmctrl", hi6220_clk_power_init);
diff --git a/drivers/clk/hisilicon/clk-hix5hd2.c b/drivers/clk/hisilicon/clk-hix5hd2.c
index f1d239435826..0aaf29da8491 100644
--- a/drivers/clk/hisilicon/clk-hix5hd2.c
+++ b/drivers/clk/hisilicon/clk-hix5hd2.c
@@ -46,15 +46,15 @@ static struct hisi_fixed_rate_clock hix5hd2_fixed_rate_clks[] __initdata = {
 	{ HIX5HD2_FIXED_83M, "83m", NULL, CLK_IS_ROOT, 83333333, },
 };
 
-static const char *sfc_mux_p[] __initdata = {
+static const char *const sfc_mux_p[] __initconst = {
 		"24m", "150m", "200m", "100m", "75m", };
 static u32 sfc_mux_table[] = {0, 4, 5, 6, 7};
 
-static const char *sdio_mux_p[] __initdata = {
+static const char *const sdio_mux_p[] __initconst = {
 		"75m", "100m", "50m", "15m", };
 static u32 sdio_mux_table[] = {0, 1, 2, 3};
 
-static const char *fephy_mux_p[] __initdata = { "25m", "125m"};
+static const char *const fephy_mux_p[] __initconst = { "25m", "125m"};
 static u32 fephy_mux_table[] = {0, 1};
 
 
@@ -252,8 +252,9 @@ static struct clk_ops clk_complex_ops = {
 	.disable = clk_complex_disable,
 };
 
-void __init hix5hd2_clk_register_complex(struct hix5hd2_complex_clock *clks,
-					 int nums, struct hisi_clock_data *data)
+static void __init
+hix5hd2_clk_register_complex(struct hix5hd2_complex_clock *clks, int nums,
+			     struct hisi_clock_data *data)
 {
 	void __iomem *base = data->base;
 	int i;
diff --git a/drivers/clk/hisilicon/clk.c b/drivers/clk/hisilicon/clk.c
index a078e84f7b05..c90a89739b03 100644
--- a/drivers/clk/hisilicon/clk.c
+++ b/drivers/clk/hisilicon/clk.c
@@ -232,3 +232,32 @@ void __init hisi_clk_register_gate_sep(struct hisi_gate_clock *clks,
 		data->clk_data.clks[clks[i].id] = clk;
 	}
 }
+
+void __init hi6220_clk_register_divider(struct hi6220_divider_clock *clks,
+					int nums, struct hisi_clock_data *data)
+{
+	struct clk *clk;
+	void __iomem *base = data->base;
+	int i;
+
+	for (i = 0; i < nums; i++) {
+		clk = hi6220_register_clkdiv(NULL, clks[i].name,
+						clks[i].parent_name,
+						clks[i].flags,
+						base + clks[i].offset,
+						clks[i].shift,
+						clks[i].width,
+						clks[i].mask_bit,
+						&hisi_clk_lock);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+
+		if (clks[i].alias)
+			clk_register_clkdev(clk, clks[i].alias, NULL);
+
+		data->clk_data.clks[clks[i].id] = clk;
+	}
+}
diff --git a/drivers/clk/hisilicon/clk.h b/drivers/clk/hisilicon/clk.h
index 31083ffc0650..b56fbc1c5f27 100644
--- a/drivers/clk/hisilicon/clk.h
+++ b/drivers/clk/hisilicon/clk.h
@@ -55,7 +55,7 @@ struct hisi_fixed_factor_clock {
 struct hisi_mux_clock {
 	unsigned int		id;
 	const char		*name;
-	const char		**parent_names;
+	const char		*const *parent_names;
 	u8			num_parents;
 	unsigned long		flags;
 	unsigned long		offset;
@@ -79,6 +79,18 @@ struct hisi_divider_clock {
 	const char		*alias;
 };
 
+struct hi6220_divider_clock {
+	unsigned int		id;
+	const char		*name;
+	const char		*parent_name;
+	unsigned long		flags;
+	unsigned long		offset;
+	u8			shift;
+	u8			width;
+	u32			mask_bit;
+	const char		*alias;
+};
+
 struct hisi_gate_clock {
 	unsigned int		id;
 	const char		*name;
@@ -94,18 +106,23 @@ struct clk *hisi_register_clkgate_sep(struct device *, const char *,
 				const char *, unsigned long,
 				void __iomem *, u8,
 				u8, spinlock_t *);
+struct clk *hi6220_register_clkdiv(struct device *dev, const char *name,
+	const char *parent_name, unsigned long flags, void __iomem *reg,
+	u8 shift, u8 width, u32 mask_bit, spinlock_t *lock);
 
-struct hisi_clock_data __init *hisi_clk_init(struct device_node *, int);
-void __init hisi_clk_register_fixed_rate(struct hisi_fixed_rate_clock *,
-					int, struct hisi_clock_data *);
-void __init hisi_clk_register_fixed_factor(struct hisi_fixed_factor_clock *,
-					int, struct hisi_clock_data *);
-void __init hisi_clk_register_mux(struct hisi_mux_clock *, int,
+struct hisi_clock_data *hisi_clk_init(struct device_node *, int);
+void hisi_clk_register_fixed_rate(struct hisi_fixed_rate_clock *,
+				int, struct hisi_clock_data *);
+void hisi_clk_register_fixed_factor(struct hisi_fixed_factor_clock *,
+				int, struct hisi_clock_data *);
+void hisi_clk_register_mux(struct hisi_mux_clock *, int,
 				struct hisi_clock_data *);
-void __init hisi_clk_register_divider(struct hisi_divider_clock *,
+void hisi_clk_register_divider(struct hisi_divider_clock *,
+				int, struct hisi_clock_data *);
+void hisi_clk_register_gate(struct hisi_gate_clock *,
+				int, struct hisi_clock_data *);
+void hisi_clk_register_gate_sep(struct hisi_gate_clock *,
+				int, struct hisi_clock_data *);
+void hi6220_clk_register_divider(struct hi6220_divider_clock *,
 				int, struct hisi_clock_data *);
-void __init hisi_clk_register_gate(struct hisi_gate_clock *,
-					int, struct hisi_clock_data *);
-void __init hisi_clk_register_gate_sep(struct hisi_gate_clock *,
-					int, struct hisi_clock_data *);
 #endif	/* __HISI_CLK_H */
diff --git a/drivers/clk/hisilicon/clkdivider-hi6220.c b/drivers/clk/hisilicon/clkdivider-hi6220.c
new file mode 100644
index 000000000000..113eee8ed23a
--- /dev/null
+++ b/drivers/clk/hisilicon/clkdivider-hi6220.c
@@ -0,0 +1,156 @@
+/*
+ * Hisilicon hi6220 SoC divider clock driver
+ *
+ * Copyright (c) 2015 Hisilicon Limited.
+ *
+ * Author: Bintian Wang <bintian.wang@huawei.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+
+#define div_mask(width)	((1 << (width)) - 1)
+
+/**
+ * struct hi6220_clk_divider - divider clock for hi6220
+ *
+ * @hw:		handle between common and hardware-specific interfaces
+ * @reg:	register containing divider
+ * @shift:	shift to the divider bit field
+ * @width:	width of the divider bit field
+ * @mask:	mask for setting divider rate
+ * @table:	the div table that the divider supports
+ * @lock:	register lock
+ */
+struct hi6220_clk_divider {
+	struct clk_hw	hw;
+	void __iomem	*reg;
+	u8		shift;
+	u8		width;
+	u32		mask;
+	const struct clk_div_table *table;
+	spinlock_t	*lock;
+};
+
+#define to_hi6220_clk_divider(_hw)	\
+	container_of(_hw, struct hi6220_clk_divider, hw)
+
+static unsigned long hi6220_clkdiv_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	unsigned int val;
+	struct hi6220_clk_divider *dclk = to_hi6220_clk_divider(hw);
+
+	val = readl_relaxed(dclk->reg) >> dclk->shift;
+	val &= div_mask(dclk->width);
+
+	return divider_recalc_rate(hw, parent_rate, val, dclk->table,
+				   CLK_DIVIDER_ROUND_CLOSEST);
+}
+
+static long hi6220_clkdiv_round_rate(struct clk_hw *hw, unsigned long rate,
+					unsigned long *prate)
+{
+	struct hi6220_clk_divider *dclk = to_hi6220_clk_divider(hw);
+
+	return divider_round_rate(hw, rate, prate, dclk->table,
+				  dclk->width, CLK_DIVIDER_ROUND_CLOSEST);
+}
+
+static int hi6220_clkdiv_set_rate(struct clk_hw *hw, unsigned long rate,
+					unsigned long parent_rate)
+{
+	int value;
+	unsigned long flags = 0;
+	u32 data;
+	struct hi6220_clk_divider *dclk = to_hi6220_clk_divider(hw);
+
+	value = divider_get_val(rate, parent_rate, dclk->table,
+				dclk->width, CLK_DIVIDER_ROUND_CLOSEST);
+
+	if (dclk->lock)
+		spin_lock_irqsave(dclk->lock, flags);
+
+	data = readl_relaxed(dclk->reg);
+	data &= ~(div_mask(dclk->width) << dclk->shift);
+	data |= value << dclk->shift;
+	data |= dclk->mask;
+
+	writel_relaxed(data, dclk->reg);
+
+	if (dclk->lock)
+		spin_unlock_irqrestore(dclk->lock, flags);
+
+	return 0;
+}
+
+static const struct clk_ops hi6220_clkdiv_ops = {
+	.recalc_rate = hi6220_clkdiv_recalc_rate,
+	.round_rate = hi6220_clkdiv_round_rate,
+	.set_rate = hi6220_clkdiv_set_rate,
+};
+
+struct clk *hi6220_register_clkdiv(struct device *dev, const char *name,
+	const char *parent_name, unsigned long flags, void __iomem *reg,
+	u8 shift, u8 width, u32 mask_bit, spinlock_t *lock)
+{
+	struct hi6220_clk_divider *div;
+	struct clk *clk;
+	struct clk_init_data init;
+	struct clk_div_table *table;
+	u32 max_div, min_div;
+	int i;
+
+	/* allocate the divider */
+	div = kzalloc(sizeof(*div), GFP_KERNEL);
+	if (!div)
+		return ERR_PTR(-ENOMEM);
+
+	/* Init the divider table */
+	max_div = div_mask(width) + 1;
+	min_div = 1;
+
+	table = kcalloc(max_div + 1, sizeof(*table), GFP_KERNEL);
+	if (!table) {
+		kfree(div);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	for (i = 0; i < max_div; i++) {
+		table[i].div = min_div + i;
+		table[i].val = table[i].div - 1;
+	}
+
+	init.name = name;
+	init.ops = &hi6220_clkdiv_ops;
+	init.flags = flags;
+	init.parent_names = parent_name ? &parent_name : NULL;
+	init.num_parents = parent_name ? 1 : 0;
+
+	/* struct hi6220_clk_divider assignments */
+	div->reg = reg;
+	div->shift = shift;
+	div->width = width;
+	div->mask = mask_bit ? BIT(mask_bit) : 0;
+	div->lock = lock;
+	div->hw.init = &init;
+	div->table = table;
+
+	/* register the clock */
+	clk = clk_register(dev, &div->hw);
+	if (IS_ERR(clk)) {
+		kfree(table);
+		kfree(div);
+	}
+
+	return clk;
+}
diff --git a/drivers/clk/imx/Makefile b/drivers/clk/imx/Makefile
new file mode 100644
index 000000000000..75fae169ce8f
--- /dev/null
+++ b/drivers/clk/imx/Makefile
@@ -0,0 +1,26 @@
+
+obj-y += \
+	clk.o \
+	clk-busy.o \
+	clk-cpu.o \
+	clk-fixup-div.o \
+	clk-fixup-mux.o \
+	clk-gate-exclusive.o \
+	clk-gate2.o \
+	clk-pllv1.o \
+	clk-pllv2.o \
+	clk-pllv3.o \
+	clk-pfd.o
+
+obj-$(CONFIG_SOC_IMX1)   += clk-imx1.o
+obj-$(CONFIG_SOC_IMX21)  += clk-imx21.o
+obj-$(CONFIG_SOC_IMX25)  += clk-imx25.o
+obj-$(CONFIG_SOC_IMX27)  += clk-imx27.o
+obj-$(CONFIG_SOC_IMX31)  += clk-imx31.o
+obj-$(CONFIG_SOC_IMX35)  += clk-imx35.o
+obj-$(CONFIG_SOC_IMX5)   += clk-imx51-imx53.o
+obj-$(CONFIG_SOC_IMX6Q)  += clk-imx6q.o
+obj-$(CONFIG_SOC_IMX6SL) += clk-imx6sl.o
+obj-$(CONFIG_SOC_IMX6SX) += clk-imx6sx.o
+obj-$(CONFIG_SOC_IMX7D)  += clk-imx7d.o
+obj-$(CONFIG_SOC_VF610)  += clk-vf610.o
diff --git a/drivers/clk/imx/clk-busy.c b/drivers/clk/imx/clk-busy.c
new file mode 100644
index 000000000000..4bb1bc419b79
--- /dev/null
+++ b/drivers/clk/imx/clk-busy.c
@@ -0,0 +1,189 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ * Copyright 2012 Linaro Ltd.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/err.h>
+#include "clk.h"
+
+static int clk_busy_wait(void __iomem *reg, u8 shift)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(10);
+
+	while (readl_relaxed(reg) & (1 << shift))
+		if (time_after(jiffies, timeout))
+			return -ETIMEDOUT;
+
+	return 0;
+}
+
+struct clk_busy_divider {
+	struct clk_divider div;
+	const struct clk_ops *div_ops;
+	void __iomem *reg;
+	u8 shift;
+};
+
+static inline struct clk_busy_divider *to_clk_busy_divider(struct clk_hw *hw)
+{
+	struct clk_divider *div = container_of(hw, struct clk_divider, hw);
+
+	return container_of(div, struct clk_busy_divider, div);
+}
+
+static unsigned long clk_busy_divider_recalc_rate(struct clk_hw *hw,
+						  unsigned long parent_rate)
+{
+	struct clk_busy_divider *busy = to_clk_busy_divider(hw);
+
+	return busy->div_ops->recalc_rate(&busy->div.hw, parent_rate);
+}
+
+static long clk_busy_divider_round_rate(struct clk_hw *hw, unsigned long rate,
+					unsigned long *prate)
+{
+	struct clk_busy_divider *busy = to_clk_busy_divider(hw);
+
+	return busy->div_ops->round_rate(&busy->div.hw, rate, prate);
+}
+
+static int clk_busy_divider_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_busy_divider *busy = to_clk_busy_divider(hw);
+	int ret;
+
+	ret = busy->div_ops->set_rate(&busy->div.hw, rate, parent_rate);
+	if (!ret)
+		ret = clk_busy_wait(busy->reg, busy->shift);
+
+	return ret;
+}
+
+static struct clk_ops clk_busy_divider_ops = {
+	.recalc_rate = clk_busy_divider_recalc_rate,
+	.round_rate = clk_busy_divider_round_rate,
+	.set_rate = clk_busy_divider_set_rate,
+};
+
+struct clk *imx_clk_busy_divider(const char *name, const char *parent_name,
+				 void __iomem *reg, u8 shift, u8 width,
+				 void __iomem *busy_reg, u8 busy_shift)
+{
+	struct clk_busy_divider *busy;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	busy = kzalloc(sizeof(*busy), GFP_KERNEL);
+	if (!busy)
+		return ERR_PTR(-ENOMEM);
+
+	busy->reg = busy_reg;
+	busy->shift = busy_shift;
+
+	busy->div.reg = reg;
+	busy->div.shift = shift;
+	busy->div.width = width;
+	busy->div.lock = &imx_ccm_lock;
+	busy->div_ops = &clk_divider_ops;
+
+	init.name = name;
+	init.ops = &clk_busy_divider_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	busy->div.hw.init = &init;
+
+	clk = clk_register(NULL, &busy->div.hw);
+	if (IS_ERR(clk))
+		kfree(busy);
+
+	return clk;
+}
+
+struct clk_busy_mux {
+	struct clk_mux mux;
+	const struct clk_ops *mux_ops;
+	void __iomem *reg;
+	u8 shift;
+};
+
+static inline struct clk_busy_mux *to_clk_busy_mux(struct clk_hw *hw)
+{
+	struct clk_mux *mux = container_of(hw, struct clk_mux, hw);
+
+	return container_of(mux, struct clk_busy_mux, mux);
+}
+
+static u8 clk_busy_mux_get_parent(struct clk_hw *hw)
+{
+	struct clk_busy_mux *busy = to_clk_busy_mux(hw);
+
+	return busy->mux_ops->get_parent(&busy->mux.hw);
+}
+
+static int clk_busy_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_busy_mux *busy = to_clk_busy_mux(hw);
+	int ret;
+
+	ret = busy->mux_ops->set_parent(&busy->mux.hw, index);
+	if (!ret)
+		ret = clk_busy_wait(busy->reg, busy->shift);
+
+	return ret;
+}
+
+static struct clk_ops clk_busy_mux_ops = {
+	.get_parent = clk_busy_mux_get_parent,
+	.set_parent = clk_busy_mux_set_parent,
+};
+
+struct clk *imx_clk_busy_mux(const char *name, void __iomem *reg, u8 shift,
+			     u8 width, void __iomem *busy_reg, u8 busy_shift,
+			     const char **parent_names, int num_parents)
+{
+	struct clk_busy_mux *busy;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	busy = kzalloc(sizeof(*busy), GFP_KERNEL);
+	if (!busy)
+		return ERR_PTR(-ENOMEM);
+
+	busy->reg = busy_reg;
+	busy->shift = busy_shift;
+
+	busy->mux.reg = reg;
+	busy->mux.shift = shift;
+	busy->mux.mask = BIT(width) - 1;
+	busy->mux.lock = &imx_ccm_lock;
+	busy->mux_ops = &clk_mux_ops;
+
+	init.name = name;
+	init.ops = &clk_busy_mux_ops;
+	init.flags = 0;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+
+	busy->mux.hw.init = &init;
+
+	clk = clk_register(NULL, &busy->mux.hw);
+	if (IS_ERR(clk))
+		kfree(busy);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-cpu.c b/drivers/clk/imx/clk-cpu.c
new file mode 100644
index 000000000000..9d46eac87f45
--- /dev/null
+++ b/drivers/clk/imx/clk-cpu.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2014 Lucas Stach <l.stach@pengutronix.de>, Pengutronix
+ *
+ * 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.
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include "clk.h"
+
+struct clk_cpu {
+	struct clk_hw	hw;
+	struct clk	*div;
+	struct clk	*mux;
+	struct clk	*pll;
+	struct clk	*step;
+};
+
+static inline struct clk_cpu *to_clk_cpu(struct clk_hw *hw)
+{
+	return container_of(hw, struct clk_cpu, hw);
+}
+
+static unsigned long clk_cpu_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct clk_cpu *cpu = to_clk_cpu(hw);
+
+	return clk_get_rate(cpu->div);
+}
+
+static long clk_cpu_round_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long *prate)
+{
+	struct clk_cpu *cpu = to_clk_cpu(hw);
+
+	return clk_round_rate(cpu->pll, rate);
+}
+
+static int clk_cpu_set_rate(struct clk_hw *hw, unsigned long rate,
+			    unsigned long parent_rate)
+{
+	struct clk_cpu *cpu = to_clk_cpu(hw);
+	int ret;
+
+	/* switch to PLL bypass clock */
+	ret = clk_set_parent(cpu->mux, cpu->step);
+	if (ret)
+		return ret;
+
+	/* reprogram PLL */
+	ret = clk_set_rate(cpu->pll, rate);
+	if (ret) {
+		clk_set_parent(cpu->mux, cpu->pll);
+		return ret;
+	}
+	/* switch back to PLL clock */
+	clk_set_parent(cpu->mux, cpu->pll);
+
+	/* Ensure the divider is what we expect */
+	clk_set_rate(cpu->div, rate);
+
+	return 0;
+}
+
+static const struct clk_ops clk_cpu_ops = {
+	.recalc_rate	= clk_cpu_recalc_rate,
+	.round_rate	= clk_cpu_round_rate,
+	.set_rate	= clk_cpu_set_rate,
+};
+
+struct clk *imx_clk_cpu(const char *name, const char *parent_name,
+		struct clk *div, struct clk *mux, struct clk *pll,
+		struct clk *step)
+{
+	struct clk_cpu *cpu;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	cpu = kzalloc(sizeof(*cpu), GFP_KERNEL);
+	if (!cpu)
+		return ERR_PTR(-ENOMEM);
+
+	cpu->div = div;
+	cpu->mux = mux;
+	cpu->pll = pll;
+	cpu->step = step;
+
+	init.name = name;
+	init.ops = &clk_cpu_ops;
+	init.flags = 0;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	cpu->hw.init = &init;
+
+	clk = clk_register(NULL, &cpu->hw);
+	if (IS_ERR(clk))
+		kfree(cpu);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-fixup-div.c b/drivers/clk/imx/clk-fixup-div.c
new file mode 100644
index 000000000000..21db020b1f2d
--- /dev/null
+++ b/drivers/clk/imx/clk-fixup-div.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2013 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include "clk.h"
+
+#define to_clk_div(_hw) container_of(_hw, struct clk_divider, hw)
+#define div_mask(d)	((1 << (d->width)) - 1)
+
+/**
+ * struct clk_fixup_div - imx integer fixup divider clock
+ * @divider: the parent class
+ * @ops: pointer to clk_ops of parent class
+ * @fixup: a hook to fixup the write value
+ *
+ * The imx fixup divider clock is a subclass of basic clk_divider
+ * with an addtional fixup hook.
+ */
+struct clk_fixup_div {
+	struct clk_divider divider;
+	const struct clk_ops *ops;
+	void (*fixup)(u32 *val);
+};
+
+static inline struct clk_fixup_div *to_clk_fixup_div(struct clk_hw *hw)
+{
+	struct clk_divider *divider = to_clk_div(hw);
+
+	return container_of(divider, struct clk_fixup_div, divider);
+}
+
+static unsigned long clk_fixup_div_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct clk_fixup_div *fixup_div = to_clk_fixup_div(hw);
+
+	return fixup_div->ops->recalc_rate(&fixup_div->divider.hw, parent_rate);
+}
+
+static long clk_fixup_div_round_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long *prate)
+{
+	struct clk_fixup_div *fixup_div = to_clk_fixup_div(hw);
+
+	return fixup_div->ops->round_rate(&fixup_div->divider.hw, rate, prate);
+}
+
+static int clk_fixup_div_set_rate(struct clk_hw *hw, unsigned long rate,
+			    unsigned long parent_rate)
+{
+	struct clk_fixup_div *fixup_div = to_clk_fixup_div(hw);
+	struct clk_divider *div = to_clk_div(hw);
+	unsigned int divider, value;
+	unsigned long flags = 0;
+	u32 val;
+
+	divider = parent_rate / rate;
+
+	/* Zero based divider */
+	value = divider - 1;
+
+	if (value > div_mask(div))
+		value = div_mask(div);
+
+	spin_lock_irqsave(div->lock, flags);
+
+	val = readl(div->reg);
+	val &= ~(div_mask(div) << div->shift);
+	val |= value << div->shift;
+	fixup_div->fixup(&val);
+	writel(val, div->reg);
+
+	spin_unlock_irqrestore(div->lock, flags);
+
+	return 0;
+}
+
+static const struct clk_ops clk_fixup_div_ops = {
+	.recalc_rate = clk_fixup_div_recalc_rate,
+	.round_rate = clk_fixup_div_round_rate,
+	.set_rate = clk_fixup_div_set_rate,
+};
+
+struct clk *imx_clk_fixup_divider(const char *name, const char *parent,
+				  void __iomem *reg, u8 shift, u8 width,
+				  void (*fixup)(u32 *val))
+{
+	struct clk_fixup_div *fixup_div;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	if (!fixup)
+		return ERR_PTR(-EINVAL);
+
+	fixup_div = kzalloc(sizeof(*fixup_div), GFP_KERNEL);
+	if (!fixup_div)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &clk_fixup_div_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = parent ? &parent : NULL;
+	init.num_parents = parent ? 1 : 0;
+
+	fixup_div->divider.reg = reg;
+	fixup_div->divider.shift = shift;
+	fixup_div->divider.width = width;
+	fixup_div->divider.lock = &imx_ccm_lock;
+	fixup_div->divider.hw.init = &init;
+	fixup_div->ops = &clk_divider_ops;
+	fixup_div->fixup = fixup;
+
+	clk = clk_register(NULL, &fixup_div->divider.hw);
+	if (IS_ERR(clk))
+		kfree(fixup_div);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-fixup-mux.c b/drivers/clk/imx/clk-fixup-mux.c
new file mode 100644
index 000000000000..0d40b35c557c
--- /dev/null
+++ b/drivers/clk/imx/clk-fixup-mux.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2013 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include "clk.h"
+
+#define to_clk_mux(_hw) container_of(_hw, struct clk_mux, hw)
+
+/**
+ * struct clk_fixup_mux - imx integer fixup multiplexer clock
+ * @mux: the parent class
+ * @ops: pointer to clk_ops of parent class
+ * @fixup: a hook to fixup the write value
+ *
+ * The imx fixup multiplexer clock is a subclass of basic clk_mux
+ * with an addtional fixup hook.
+ */
+struct clk_fixup_mux {
+	struct clk_mux mux;
+	const struct clk_ops *ops;
+	void (*fixup)(u32 *val);
+};
+
+static inline struct clk_fixup_mux *to_clk_fixup_mux(struct clk_hw *hw)
+{
+	struct clk_mux *mux = to_clk_mux(hw);
+
+	return container_of(mux, struct clk_fixup_mux, mux);
+}
+
+static u8 clk_fixup_mux_get_parent(struct clk_hw *hw)
+{
+	struct clk_fixup_mux *fixup_mux = to_clk_fixup_mux(hw);
+
+	return fixup_mux->ops->get_parent(&fixup_mux->mux.hw);
+}
+
+static int clk_fixup_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_fixup_mux *fixup_mux = to_clk_fixup_mux(hw);
+	struct clk_mux *mux = to_clk_mux(hw);
+	unsigned long flags = 0;
+	u32 val;
+
+	spin_lock_irqsave(mux->lock, flags);
+
+	val = readl(mux->reg);
+	val &= ~(mux->mask << mux->shift);
+	val |= index << mux->shift;
+	fixup_mux->fixup(&val);
+	writel(val, mux->reg);
+
+	spin_unlock_irqrestore(mux->lock, flags);
+
+	return 0;
+}
+
+static const struct clk_ops clk_fixup_mux_ops = {
+	.get_parent = clk_fixup_mux_get_parent,
+	.set_parent = clk_fixup_mux_set_parent,
+};
+
+struct clk *imx_clk_fixup_mux(const char *name, void __iomem *reg,
+			      u8 shift, u8 width, const char **parents,
+			      int num_parents, void (*fixup)(u32 *val))
+{
+	struct clk_fixup_mux *fixup_mux;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	if (!fixup)
+		return ERR_PTR(-EINVAL);
+
+	fixup_mux = kzalloc(sizeof(*fixup_mux), GFP_KERNEL);
+	if (!fixup_mux)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &clk_fixup_mux_ops;
+	init.parent_names = parents;
+	init.num_parents = num_parents;
+	init.flags = 0;
+
+	fixup_mux->mux.reg = reg;
+	fixup_mux->mux.shift = shift;
+	fixup_mux->mux.mask = BIT(width) - 1;
+	fixup_mux->mux.lock = &imx_ccm_lock;
+	fixup_mux->mux.hw.init = &init;
+	fixup_mux->ops = &clk_mux_ops;
+	fixup_mux->fixup = fixup;
+
+	clk = clk_register(NULL, &fixup_mux->mux.hw);
+	if (IS_ERR(clk))
+		kfree(fixup_mux);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-gate-exclusive.c b/drivers/clk/imx/clk-gate-exclusive.c
new file mode 100644
index 000000000000..c12f5f2e04dc
--- /dev/null
+++ b/drivers/clk/imx/clk-gate-exclusive.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright 2014 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include "clk.h"
+
+/**
+ * struct clk_gate_exclusive - i.MX specific gate clock which is mutually
+ * exclusive with other gate clocks
+ *
+ * @gate: the parent class
+ * @exclusive_mask: mask of gate bits which are mutually exclusive to this
+ *	gate clock
+ *
+ * The imx exclusive gate clock is a subclass of basic clk_gate
+ * with an addtional mask to indicate which other gate bits in the same
+ * register is mutually exclusive to this gate clock.
+ */
+struct clk_gate_exclusive {
+	struct clk_gate gate;
+	u32 exclusive_mask;
+};
+
+static int clk_gate_exclusive_enable(struct clk_hw *hw)
+{
+	struct clk_gate *gate = container_of(hw, struct clk_gate, hw);
+	struct clk_gate_exclusive *exgate = container_of(gate,
+					struct clk_gate_exclusive, gate);
+	u32 val = readl(gate->reg);
+
+	if (val & exgate->exclusive_mask)
+		return -EBUSY;
+
+	return clk_gate_ops.enable(hw);
+}
+
+static void clk_gate_exclusive_disable(struct clk_hw *hw)
+{
+	clk_gate_ops.disable(hw);
+}
+
+static int clk_gate_exclusive_is_enabled(struct clk_hw *hw)
+{
+	return clk_gate_ops.is_enabled(hw);
+}
+
+static const struct clk_ops clk_gate_exclusive_ops = {
+	.enable = clk_gate_exclusive_enable,
+	.disable = clk_gate_exclusive_disable,
+	.is_enabled = clk_gate_exclusive_is_enabled,
+};
+
+struct clk *imx_clk_gate_exclusive(const char *name, const char *parent,
+	 void __iomem *reg, u8 shift, u32 exclusive_mask)
+{
+	struct clk_gate_exclusive *exgate;
+	struct clk_gate *gate;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	if (exclusive_mask == 0)
+		return ERR_PTR(-EINVAL);
+
+	exgate = kzalloc(sizeof(*exgate), GFP_KERNEL);
+	if (!exgate)
+		return ERR_PTR(-ENOMEM);
+	gate = &exgate->gate;
+
+	init.name = name;
+	init.ops = &clk_gate_exclusive_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = parent ? &parent : NULL;
+	init.num_parents = parent ? 1 : 0;
+
+	gate->reg = reg;
+	gate->bit_idx = shift;
+	gate->lock = &imx_ccm_lock;
+	gate->hw.init = &init;
+	exgate->exclusive_mask = exclusive_mask;
+
+	clk = clk_register(NULL, &gate->hw);
+	if (IS_ERR(clk))
+		kfree(exgate);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-gate2.c b/drivers/clk/imx/clk-gate2.c
new file mode 100644
index 000000000000..8935bff99fe7
--- /dev/null
+++ b/drivers/clk/imx/clk-gate2.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) 2010-2011 Canonical Ltd <jeremy.kerr@canonical.com>
+ * Copyright (C) 2011-2012 Mike Turquette, Linaro Ltd <mturquette@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Gated clock implementation
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/string.h>
+#include "clk.h"
+
+/**
+ * DOC: basic gatable clock which can gate and ungate it's ouput
+ *
+ * Traits of this clock:
+ * prepare - clk_(un)prepare only ensures parent is (un)prepared
+ * enable - clk_enable and clk_disable are functional & control gating
+ * rate - inherits rate from parent.  No clk_set_rate support
+ * parent - fixed parent.  No clk_set_parent support
+ */
+
+struct clk_gate2 {
+	struct clk_hw hw;
+	void __iomem	*reg;
+	u8		bit_idx;
+	u8		flags;
+	spinlock_t	*lock;
+	unsigned int	*share_count;
+};
+
+#define to_clk_gate2(_hw) container_of(_hw, struct clk_gate2, hw)
+
+static int clk_gate2_enable(struct clk_hw *hw)
+{
+	struct clk_gate2 *gate = to_clk_gate2(hw);
+	u32 reg;
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(gate->lock, flags);
+
+	if (gate->share_count && (*gate->share_count)++ > 0)
+		goto out;
+
+	reg = readl(gate->reg);
+	reg |= 3 << gate->bit_idx;
+	writel(reg, gate->reg);
+
+out:
+	spin_unlock_irqrestore(gate->lock, flags);
+
+	return 0;
+}
+
+static void clk_gate2_disable(struct clk_hw *hw)
+{
+	struct clk_gate2 *gate = to_clk_gate2(hw);
+	u32 reg;
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(gate->lock, flags);
+
+	if (gate->share_count) {
+		if (WARN_ON(*gate->share_count == 0))
+			goto out;
+		else if (--(*gate->share_count) > 0)
+			goto out;
+	}
+
+	reg = readl(gate->reg);
+	reg &= ~(3 << gate->bit_idx);
+	writel(reg, gate->reg);
+
+out:
+	spin_unlock_irqrestore(gate->lock, flags);
+}
+
+static int clk_gate2_reg_is_enabled(void __iomem *reg, u8 bit_idx)
+{
+	u32 val = readl(reg);
+
+	if (((val >> bit_idx) & 1) == 1)
+		return 1;
+
+	return 0;
+}
+
+static int clk_gate2_is_enabled(struct clk_hw *hw)
+{
+	struct clk_gate2 *gate = to_clk_gate2(hw);
+
+	return clk_gate2_reg_is_enabled(gate->reg, gate->bit_idx);
+}
+
+static void clk_gate2_disable_unused(struct clk_hw *hw)
+{
+	struct clk_gate2 *gate = to_clk_gate2(hw);
+	unsigned long flags = 0;
+	u32 reg;
+
+	spin_lock_irqsave(gate->lock, flags);
+
+	if (!gate->share_count || *gate->share_count == 0) {
+		reg = readl(gate->reg);
+		reg &= ~(3 << gate->bit_idx);
+		writel(reg, gate->reg);
+	}
+
+	spin_unlock_irqrestore(gate->lock, flags);
+}
+
+static struct clk_ops clk_gate2_ops = {
+	.enable = clk_gate2_enable,
+	.disable = clk_gate2_disable,
+	.disable_unused = clk_gate2_disable_unused,
+	.is_enabled = clk_gate2_is_enabled,
+};
+
+struct clk *clk_register_gate2(struct device *dev, const char *name,
+		const char *parent_name, unsigned long flags,
+		void __iomem *reg, u8 bit_idx,
+		u8 clk_gate2_flags, spinlock_t *lock,
+		unsigned int *share_count)
+{
+	struct clk_gate2 *gate;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	gate = kzalloc(sizeof(struct clk_gate2), GFP_KERNEL);
+	if (!gate)
+		return ERR_PTR(-ENOMEM);
+
+	/* struct clk_gate2 assignments */
+	gate->reg = reg;
+	gate->bit_idx = bit_idx;
+	gate->flags = clk_gate2_flags;
+	gate->lock = lock;
+	gate->share_count = share_count;
+
+	init.name = name;
+	init.ops = &clk_gate2_ops;
+	init.flags = flags;
+	init.parent_names = parent_name ? &parent_name : NULL;
+	init.num_parents = parent_name ? 1 : 0;
+
+	gate->hw.init = &init;
+
+	clk = clk_register(dev, &gate->hw);
+	if (IS_ERR(clk))
+		kfree(gate);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-imx1.c b/drivers/clk/imx/clk-imx1.c
new file mode 100644
index 000000000000..c2647fa19f28
--- /dev/null
+++ b/drivers/clk/imx/clk-imx1.c
@@ -0,0 +1,122 @@
+/*
+ *  Copyright (C) 2008 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <dt-bindings/clock/imx1-clock.h>
+#include <soc/imx/timer.h>
+#include <asm/irq.h>
+
+#include "clk.h"
+
+#define MX1_CCM_BASE_ADDR	0x0021b000
+#define MX1_TIM1_BASE_ADDR	0x00220000
+#define MX1_TIM1_INT		(NR_IRQS_LEGACY + 59)
+
+static const char *prem_sel_clks[] = { "clk32_premult", "clk16m", };
+static const char *clko_sel_clks[] = { "per1", "hclk", "clk48m", "clk16m",
+				       "prem", "fclk", };
+
+static struct clk *clk[IMX1_CLK_MAX];
+static struct clk_onecell_data clk_data;
+
+static void __iomem *ccm __initdata;
+#define CCM_CSCR	(ccm + 0x0000)
+#define CCM_MPCTL0	(ccm + 0x0004)
+#define CCM_SPCTL0	(ccm + 0x000c)
+#define CCM_PCDR	(ccm + 0x0020)
+#define SCM_GCCR	(ccm + 0x0810)
+
+static void __init _mx1_clocks_init(unsigned long fref)
+{
+	clk[IMX1_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+	clk[IMX1_CLK_CLK32] = imx_obtain_fixed_clock("clk32", fref);
+	clk[IMX1_CLK_CLK16M_EXT] = imx_clk_fixed("clk16m_ext", 16000000);
+	clk[IMX1_CLK_CLK16M] = imx_clk_gate("clk16m", "clk16m_ext", CCM_CSCR, 17);
+	clk[IMX1_CLK_CLK32_PREMULT] = imx_clk_fixed_factor("clk32_premult", "clk32", 512, 1);
+	clk[IMX1_CLK_PREM] = imx_clk_mux("prem", CCM_CSCR, 16, 1, prem_sel_clks, ARRAY_SIZE(prem_sel_clks));
+	clk[IMX1_CLK_MPLL] = imx_clk_pllv1(IMX_PLLV1_IMX1, "mpll", "clk32_premult", CCM_MPCTL0);
+	clk[IMX1_CLK_MPLL_GATE] = imx_clk_gate("mpll_gate", "mpll", CCM_CSCR, 0);
+	clk[IMX1_CLK_SPLL] = imx_clk_pllv1(IMX_PLLV1_IMX1, "spll", "prem", CCM_SPCTL0);
+	clk[IMX1_CLK_SPLL_GATE] = imx_clk_gate("spll_gate", "spll", CCM_CSCR, 1);
+	clk[IMX1_CLK_MCU] = imx_clk_divider("mcu", "clk32_premult", CCM_CSCR, 15, 1);
+	clk[IMX1_CLK_FCLK] = imx_clk_divider("fclk", "mpll_gate", CCM_CSCR, 15, 1);
+	clk[IMX1_CLK_HCLK] = imx_clk_divider("hclk", "spll_gate", CCM_CSCR, 10, 4);
+	clk[IMX1_CLK_CLK48M] = imx_clk_divider("clk48m", "spll_gate", CCM_CSCR, 26, 3);
+	clk[IMX1_CLK_PER1] = imx_clk_divider("per1", "spll_gate", CCM_PCDR, 0, 4);
+	clk[IMX1_CLK_PER2] = imx_clk_divider("per2", "spll_gate", CCM_PCDR, 4, 4);
+	clk[IMX1_CLK_PER3] = imx_clk_divider("per3", "spll_gate", CCM_PCDR, 16, 7);
+	clk[IMX1_CLK_CLKO] = imx_clk_mux("clko", CCM_CSCR, 29, 3, clko_sel_clks, ARRAY_SIZE(clko_sel_clks));
+	clk[IMX1_CLK_UART3_GATE] = imx_clk_gate("uart3_gate", "hclk", SCM_GCCR, 6);
+	clk[IMX1_CLK_SSI2_GATE] = imx_clk_gate("ssi2_gate", "hclk", SCM_GCCR, 5);
+	clk[IMX1_CLK_BROM_GATE] = imx_clk_gate("brom_gate", "hclk", SCM_GCCR, 4);
+	clk[IMX1_CLK_DMA_GATE] = imx_clk_gate("dma_gate", "hclk", SCM_GCCR, 3);
+	clk[IMX1_CLK_CSI_GATE] = imx_clk_gate("csi_gate", "hclk", SCM_GCCR, 2);
+	clk[IMX1_CLK_MMA_GATE] = imx_clk_gate("mma_gate", "hclk", SCM_GCCR, 1);
+	clk[IMX1_CLK_USBD_GATE] = imx_clk_gate("usbd_gate", "clk48m", SCM_GCCR, 0);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+}
+
+int __init mx1_clocks_init(unsigned long fref)
+{
+	ccm = ioremap(MX1_CCM_BASE_ADDR, SZ_4K);
+	BUG_ON(!ccm);
+
+	_mx1_clocks_init(fref);
+
+	clk_register_clkdev(clk[IMX1_CLK_PER1], "per", "imx-gpt.0");
+	clk_register_clkdev(clk[IMX1_CLK_HCLK], "ipg", "imx-gpt.0");
+	clk_register_clkdev(clk[IMX1_CLK_DMA_GATE], "ahb", "imx1-dma");
+	clk_register_clkdev(clk[IMX1_CLK_HCLK], "ipg", "imx1-dma");
+	clk_register_clkdev(clk[IMX1_CLK_PER1], "per", "imx1-uart.0");
+	clk_register_clkdev(clk[IMX1_CLK_HCLK], "ipg", "imx1-uart.0");
+	clk_register_clkdev(clk[IMX1_CLK_PER1], "per", "imx1-uart.1");
+	clk_register_clkdev(clk[IMX1_CLK_HCLK], "ipg", "imx1-uart.1");
+	clk_register_clkdev(clk[IMX1_CLK_PER1], "per", "imx1-uart.2");
+	clk_register_clkdev(clk[IMX1_CLK_UART3_GATE], "ipg", "imx1-uart.2");
+	clk_register_clkdev(clk[IMX1_CLK_HCLK], NULL, "imx1-i2c.0");
+	clk_register_clkdev(clk[IMX1_CLK_PER2], "per", "imx1-cspi.0");
+	clk_register_clkdev(clk[IMX1_CLK_DUMMY], "ipg", "imx1-cspi.0");
+	clk_register_clkdev(clk[IMX1_CLK_PER2], "per", "imx1-cspi.1");
+	clk_register_clkdev(clk[IMX1_CLK_DUMMY], "ipg", "imx1-cspi.1");
+	clk_register_clkdev(clk[IMX1_CLK_PER2], "per", "imx1-fb.0");
+	clk_register_clkdev(clk[IMX1_CLK_DUMMY], "ipg", "imx1-fb.0");
+	clk_register_clkdev(clk[IMX1_CLK_DUMMY], "ahb", "imx1-fb.0");
+
+	mxc_timer_init(MX1_TIM1_BASE_ADDR, MX1_TIM1_INT, GPT_TYPE_IMX1);
+
+	return 0;
+}
+
+static void __init mx1_clocks_init_dt(struct device_node *np)
+{
+	ccm = of_iomap(np, 0);
+	BUG_ON(!ccm);
+
+	_mx1_clocks_init(32768);
+
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+}
+CLK_OF_DECLARE(imx1_ccm, "fsl,imx1-ccm", mx1_clocks_init_dt);
diff --git a/drivers/clk/imx/clk-imx21.c b/drivers/clk/imx/clk-imx21.c
new file mode 100644
index 000000000000..dba987e3b89f
--- /dev/null
+++ b/drivers/clk/imx/clk-imx21.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008 Juergen Beisert, kernel@pengutronix.de
+ * Copyright 2008 Martin Fuzzey, mfuzzey@gmail.com
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <dt-bindings/clock/imx21-clock.h>
+#include <soc/imx/timer.h>
+#include <asm/irq.h>
+
+#include "clk.h"
+
+#define MX21_CCM_BASE_ADDR	0x10027000
+#define MX21_GPT1_BASE_ADDR	0x10003000
+#define MX21_INT_GPT1		(NR_IRQS_LEGACY + 26)
+
+static void __iomem *ccm __initdata;
+
+/* Register offsets */
+#define CCM_CSCR	(ccm + 0x00)
+#define CCM_MPCTL0	(ccm + 0x04)
+#define CCM_SPCTL0	(ccm + 0x0c)
+#define CCM_PCDR0	(ccm + 0x18)
+#define CCM_PCDR1	(ccm + 0x1c)
+#define CCM_PCCR0	(ccm + 0x20)
+#define CCM_PCCR1	(ccm + 0x24)
+
+static const char *mpll_osc_sel_clks[] = { "ckih_gate", "ckih_div1p5", };
+static const char *mpll_sel_clks[] = { "fpm_gate", "mpll_osc_sel", };
+static const char *spll_sel_clks[] = { "fpm_gate", "mpll_osc_sel", };
+static const char *ssi_sel_clks[] = { "spll_gate", "mpll_gate", };
+
+static struct clk *clk[IMX21_CLK_MAX];
+static struct clk_onecell_data clk_data;
+
+static void __init _mx21_clocks_init(unsigned long lref, unsigned long href)
+{
+	BUG_ON(!ccm);
+
+	clk[IMX21_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+	clk[IMX21_CLK_CKIL] = imx_obtain_fixed_clock("ckil", lref);
+	clk[IMX21_CLK_CKIH] = imx_obtain_fixed_clock("ckih", href);
+	clk[IMX21_CLK_FPM] = imx_clk_fixed_factor("fpm", "ckil", 512, 1);
+	clk[IMX21_CLK_CKIH_DIV1P5] = imx_clk_fixed_factor("ckih_div1p5", "ckih_gate", 2, 3);
+
+	clk[IMX21_CLK_MPLL_GATE] = imx_clk_gate("mpll_gate", "mpll", CCM_CSCR, 0);
+	clk[IMX21_CLK_SPLL_GATE] = imx_clk_gate("spll_gate", "spll", CCM_CSCR, 1);
+	clk[IMX21_CLK_FPM_GATE] = imx_clk_gate("fpm_gate", "fpm", CCM_CSCR, 2);
+	clk[IMX21_CLK_CKIH_GATE] = imx_clk_gate_dis("ckih_gate", "ckih", CCM_CSCR, 3);
+	clk[IMX21_CLK_MPLL_OSC_SEL] = imx_clk_mux("mpll_osc_sel", CCM_CSCR, 4, 1, mpll_osc_sel_clks, ARRAY_SIZE(mpll_osc_sel_clks));
+	clk[IMX21_CLK_IPG] = imx_clk_divider("ipg", "hclk", CCM_CSCR, 9, 1);
+	clk[IMX21_CLK_HCLK] = imx_clk_divider("hclk", "fclk", CCM_CSCR, 10, 4);
+	clk[IMX21_CLK_MPLL_SEL] = imx_clk_mux("mpll_sel", CCM_CSCR, 16, 1, mpll_sel_clks, ARRAY_SIZE(mpll_sel_clks));
+	clk[IMX21_CLK_SPLL_SEL] = imx_clk_mux("spll_sel", CCM_CSCR, 17, 1, spll_sel_clks, ARRAY_SIZE(spll_sel_clks));
+	clk[IMX21_CLK_SSI1_SEL] = imx_clk_mux("ssi1_sel", CCM_CSCR, 19, 1, ssi_sel_clks, ARRAY_SIZE(ssi_sel_clks));
+	clk[IMX21_CLK_SSI2_SEL] = imx_clk_mux("ssi2_sel", CCM_CSCR, 20, 1, ssi_sel_clks, ARRAY_SIZE(ssi_sel_clks));
+	clk[IMX21_CLK_USB_DIV] = imx_clk_divider("usb_div", "spll_gate", CCM_CSCR, 26, 3);
+	clk[IMX21_CLK_FCLK] = imx_clk_divider("fclk", "mpll_gate", CCM_CSCR, 29, 3);
+
+	clk[IMX21_CLK_MPLL] = imx_clk_pllv1(IMX_PLLV1_IMX21, "mpll", "mpll_sel", CCM_MPCTL0);
+
+	clk[IMX21_CLK_SPLL] = imx_clk_pllv1(IMX_PLLV1_IMX21, "spll", "spll_sel", CCM_SPCTL0);
+
+	clk[IMX21_CLK_NFC_DIV] = imx_clk_divider("nfc_div", "fclk", CCM_PCDR0, 12, 4);
+	clk[IMX21_CLK_SSI1_DIV] = imx_clk_divider("ssi1_div", "ssi1_sel", CCM_PCDR0, 16, 6);
+	clk[IMX21_CLK_SSI2_DIV] = imx_clk_divider("ssi2_div", "ssi2_sel", CCM_PCDR0, 26, 6);
+
+	clk[IMX21_CLK_PER1] = imx_clk_divider("per1", "mpll_gate", CCM_PCDR1, 0, 6);
+	clk[IMX21_CLK_PER2] = imx_clk_divider("per2", "mpll_gate", CCM_PCDR1, 8, 6);
+	clk[IMX21_CLK_PER3] = imx_clk_divider("per3", "mpll_gate", CCM_PCDR1, 16, 6);
+	clk[IMX21_CLK_PER4] = imx_clk_divider("per4", "mpll_gate", CCM_PCDR1, 24, 6);
+
+	clk[IMX21_CLK_UART1_IPG_GATE] = imx_clk_gate("uart1_ipg_gate", "ipg", CCM_PCCR0, 0);
+	clk[IMX21_CLK_UART2_IPG_GATE] = imx_clk_gate("uart2_ipg_gate", "ipg", CCM_PCCR0, 1);
+	clk[IMX21_CLK_UART3_IPG_GATE] = imx_clk_gate("uart3_ipg_gate", "ipg", CCM_PCCR0, 2);
+	clk[IMX21_CLK_UART4_IPG_GATE] = imx_clk_gate("uart4_ipg_gate", "ipg", CCM_PCCR0, 3);
+	clk[IMX21_CLK_CSPI1_IPG_GATE] = imx_clk_gate("cspi1_ipg_gate", "ipg", CCM_PCCR0, 4);
+	clk[IMX21_CLK_CSPI2_IPG_GATE] = imx_clk_gate("cspi2_ipg_gate", "ipg", CCM_PCCR0, 5);
+	clk[IMX21_CLK_SSI1_GATE] = imx_clk_gate("ssi1_gate", "ipg", CCM_PCCR0, 6);
+	clk[IMX21_CLK_SSI2_GATE] = imx_clk_gate("ssi2_gate", "ipg", CCM_PCCR0, 7);
+	clk[IMX21_CLK_SDHC1_IPG_GATE] = imx_clk_gate("sdhc1_ipg_gate", "ipg", CCM_PCCR0, 9);
+	clk[IMX21_CLK_SDHC2_IPG_GATE] = imx_clk_gate("sdhc2_ipg_gate", "ipg", CCM_PCCR0, 10);
+	clk[IMX21_CLK_GPIO_GATE] = imx_clk_gate("gpio_gate", "ipg", CCM_PCCR0, 11);
+	clk[IMX21_CLK_I2C_GATE] = imx_clk_gate("i2c_gate", "ipg", CCM_PCCR0, 12);
+	clk[IMX21_CLK_DMA_GATE] = imx_clk_gate("dma_gate", "ipg", CCM_PCCR0, 13);
+	clk[IMX21_CLK_USB_GATE] = imx_clk_gate("usb_gate", "usb_div", CCM_PCCR0, 14);
+	clk[IMX21_CLK_EMMA_GATE] = imx_clk_gate("emma_gate", "ipg", CCM_PCCR0, 15);
+	clk[IMX21_CLK_SSI2_BAUD_GATE] = imx_clk_gate("ssi2_baud_gate", "ipg", CCM_PCCR0, 16);
+	clk[IMX21_CLK_SSI1_BAUD_GATE] = imx_clk_gate("ssi1_baud_gate", "ipg", CCM_PCCR0, 17);
+	clk[IMX21_CLK_LCDC_IPG_GATE] = imx_clk_gate("lcdc_ipg_gate", "ipg", CCM_PCCR0, 18);
+	clk[IMX21_CLK_NFC_GATE] = imx_clk_gate("nfc_gate", "nfc_div", CCM_PCCR0, 19);
+	clk[IMX21_CLK_SLCDC_HCLK_GATE] = imx_clk_gate("slcdc_hclk_gate", "hclk", CCM_PCCR0, 21);
+	clk[IMX21_CLK_PER4_GATE] = imx_clk_gate("per4_gate", "per4", CCM_PCCR0, 22);
+	clk[IMX21_CLK_BMI_GATE] = imx_clk_gate("bmi_gate", "hclk", CCM_PCCR0, 23);
+	clk[IMX21_CLK_USB_HCLK_GATE] = imx_clk_gate("usb_hclk_gate", "hclk", CCM_PCCR0, 24);
+	clk[IMX21_CLK_SLCDC_GATE] = imx_clk_gate("slcdc_gate", "hclk", CCM_PCCR0, 25);
+	clk[IMX21_CLK_LCDC_HCLK_GATE] = imx_clk_gate("lcdc_hclk_gate", "hclk", CCM_PCCR0, 26);
+	clk[IMX21_CLK_EMMA_HCLK_GATE] = imx_clk_gate("emma_hclk_gate", "hclk", CCM_PCCR0, 27);
+	clk[IMX21_CLK_BROM_GATE] = imx_clk_gate("brom_gate", "hclk", CCM_PCCR0, 28);
+	clk[IMX21_CLK_DMA_HCLK_GATE] = imx_clk_gate("dma_hclk_gate", "hclk", CCM_PCCR0, 30);
+	clk[IMX21_CLK_CSI_HCLK_GATE] = imx_clk_gate("csi_hclk_gate", "hclk", CCM_PCCR0, 31);
+
+	clk[IMX21_CLK_CSPI3_IPG_GATE] = imx_clk_gate("cspi3_ipg_gate", "ipg", CCM_PCCR1, 23);
+	clk[IMX21_CLK_WDOG_GATE] = imx_clk_gate("wdog_gate", "ipg", CCM_PCCR1, 24);
+	clk[IMX21_CLK_GPT1_IPG_GATE] = imx_clk_gate("gpt1_ipg_gate", "ipg", CCM_PCCR1, 25);
+	clk[IMX21_CLK_GPT2_IPG_GATE] = imx_clk_gate("gpt2_ipg_gate", "ipg", CCM_PCCR1, 26);
+	clk[IMX21_CLK_GPT3_IPG_GATE] = imx_clk_gate("gpt3_ipg_gate", "ipg", CCM_PCCR1, 27);
+	clk[IMX21_CLK_PWM_IPG_GATE] = imx_clk_gate("pwm_ipg_gate", "ipg", CCM_PCCR1, 28);
+	clk[IMX21_CLK_RTC_GATE] = imx_clk_gate("rtc_gate", "ipg", CCM_PCCR1, 29);
+	clk[IMX21_CLK_KPP_GATE] = imx_clk_gate("kpp_gate", "ipg", CCM_PCCR1, 30);
+	clk[IMX21_CLK_OWIRE_GATE] = imx_clk_gate("owire_gate", "ipg", CCM_PCCR1, 31);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+}
+
+int __init mx21_clocks_init(unsigned long lref, unsigned long href)
+{
+	ccm = ioremap(MX21_CCM_BASE_ADDR, SZ_2K);
+
+	_mx21_clocks_init(lref, href);
+
+	clk_register_clkdev(clk[IMX21_CLK_PER1], "per", "imx21-uart.0");
+	clk_register_clkdev(clk[IMX21_CLK_UART1_IPG_GATE], "ipg", "imx21-uart.0");
+	clk_register_clkdev(clk[IMX21_CLK_PER1], "per", "imx21-uart.1");
+	clk_register_clkdev(clk[IMX21_CLK_UART2_IPG_GATE], "ipg", "imx21-uart.1");
+	clk_register_clkdev(clk[IMX21_CLK_PER1], "per", "imx21-uart.2");
+	clk_register_clkdev(clk[IMX21_CLK_UART3_IPG_GATE], "ipg", "imx21-uart.2");
+	clk_register_clkdev(clk[IMX21_CLK_PER1], "per", "imx21-uart.3");
+	clk_register_clkdev(clk[IMX21_CLK_UART4_IPG_GATE], "ipg", "imx21-uart.3");
+	clk_register_clkdev(clk[IMX21_CLK_GPT1_IPG_GATE], "ipg", "imx-gpt.0");
+	clk_register_clkdev(clk[IMX21_CLK_PER1], "per", "imx-gpt.0");
+	clk_register_clkdev(clk[IMX21_CLK_PER2], "per", "imx21-cspi.0");
+	clk_register_clkdev(clk[IMX21_CLK_CSPI1_IPG_GATE], "ipg", "imx21-cspi.0");
+	clk_register_clkdev(clk[IMX21_CLK_PER2], "per", "imx21-cspi.1");
+	clk_register_clkdev(clk[IMX21_CLK_CSPI2_IPG_GATE], "ipg", "imx21-cspi.1");
+	clk_register_clkdev(clk[IMX21_CLK_PER2], "per", "imx21-cspi.2");
+	clk_register_clkdev(clk[IMX21_CLK_CSPI3_IPG_GATE], "ipg", "imx21-cspi.2");
+	clk_register_clkdev(clk[IMX21_CLK_PER3], "per", "imx21-fb.0");
+	clk_register_clkdev(clk[IMX21_CLK_LCDC_IPG_GATE], "ipg", "imx21-fb.0");
+	clk_register_clkdev(clk[IMX21_CLK_LCDC_HCLK_GATE], "ahb", "imx21-fb.0");
+	clk_register_clkdev(clk[IMX21_CLK_USB_GATE], "per", "imx21-hcd.0");
+	clk_register_clkdev(clk[IMX21_CLK_USB_HCLK_GATE], "ahb", "imx21-hcd.0");
+	clk_register_clkdev(clk[IMX21_CLK_NFC_GATE], NULL, "imx21-nand.0");
+	clk_register_clkdev(clk[IMX21_CLK_DMA_HCLK_GATE], "ahb", "imx21-dma");
+	clk_register_clkdev(clk[IMX21_CLK_DMA_GATE], "ipg", "imx21-dma");
+	clk_register_clkdev(clk[IMX21_CLK_WDOG_GATE], NULL, "imx2-wdt.0");
+	clk_register_clkdev(clk[IMX21_CLK_I2C_GATE], NULL, "imx21-i2c.0");
+	clk_register_clkdev(clk[IMX21_CLK_OWIRE_GATE], NULL, "mxc_w1.0");
+
+	mxc_timer_init(MX21_GPT1_BASE_ADDR, MX21_INT_GPT1, GPT_TYPE_IMX21);
+
+	return 0;
+}
+
+static void __init mx21_clocks_init_dt(struct device_node *np)
+{
+	ccm = of_iomap(np, 0);
+
+	_mx21_clocks_init(32768, 26000000);
+
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+}
+CLK_OF_DECLARE(imx27_ccm, "fsl,imx21-ccm", mx21_clocks_init_dt);
diff --git a/drivers/clk/imx/clk-imx25.c b/drivers/clk/imx/clk-imx25.c
new file mode 100644
index 000000000000..ec1a4c1dacf1
--- /dev/null
+++ b/drivers/clk/imx/clk-imx25.c
@@ -0,0 +1,262 @@
+/*
+ * Copyright (C) 2009 by Sascha Hauer, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#include "clk.h"
+
+#define CCM_MPCTL	0x00
+#define CCM_UPCTL	0x04
+#define CCM_CCTL	0x08
+#define CCM_CGCR0	0x0C
+#define CCM_CGCR1	0x10
+#define CCM_CGCR2	0x14
+#define CCM_PCDR0	0x18
+#define CCM_PCDR1	0x1C
+#define CCM_PCDR2	0x20
+#define CCM_PCDR3	0x24
+#define CCM_RCSR	0x28
+#define CCM_CRDR	0x2C
+#define CCM_DCVR0	0x30
+#define CCM_DCVR1	0x34
+#define CCM_DCVR2	0x38
+#define CCM_DCVR3	0x3c
+#define CCM_LTR0	0x40
+#define CCM_LTR1	0x44
+#define CCM_LTR2	0x48
+#define CCM_LTR3	0x4c
+#define CCM_MCR		0x64
+
+#define ccm(x)	(ccm_base + (x))
+
+static struct clk_onecell_data clk_data;
+
+static const char *cpu_sel_clks[] = { "mpll", "mpll_cpu_3_4", };
+static const char *per_sel_clks[] = { "ahb", "upll", };
+static const char *cko_sel_clks[] = { "dummy", "osc", "cpu", "ahb",
+				      "ipg", "dummy", "dummy", "dummy",
+				      "dummy", "dummy", "per0", "per2",
+				      "per13", "per14", "usbotg_ahb", "dummy",};
+
+enum mx25_clks {
+	dummy, osc, mpll, upll, mpll_cpu_3_4, cpu_sel, cpu, ahb, usb_div, ipg,
+	per0_sel, per1_sel, per2_sel, per3_sel, per4_sel, per5_sel, per6_sel,
+	per7_sel, per8_sel, per9_sel, per10_sel, per11_sel, per12_sel,
+	per13_sel, per14_sel, per15_sel, per0, per1, per2, per3, per4, per5,
+	per6, per7, per8, per9, per10, per11, per12, per13, per14, per15,
+	csi_ipg_per, epit_ipg_per, esai_ipg_per, esdhc1_ipg_per, esdhc2_ipg_per,
+	gpt_ipg_per, i2c_ipg_per, lcdc_ipg_per, nfc_ipg_per, owire_ipg_per,
+	pwm_ipg_per, sim1_ipg_per, sim2_ipg_per, ssi1_ipg_per, ssi2_ipg_per,
+	uart_ipg_per, ata_ahb, reserved1, csi_ahb, emi_ahb, esai_ahb, esdhc1_ahb,
+	esdhc2_ahb, fec_ahb, lcdc_ahb, rtic_ahb, sdma_ahb, slcdc_ahb, usbotg_ahb,
+	reserved2, reserved3, reserved4, reserved5, can1_ipg, can2_ipg,	csi_ipg,
+	cspi1_ipg, cspi2_ipg, cspi3_ipg, dryice_ipg, ect_ipg, epit1_ipg, epit2_ipg,
+	reserved6, esdhc1_ipg, esdhc2_ipg, fec_ipg, reserved7, reserved8, reserved9,
+	gpt1_ipg, gpt2_ipg, gpt3_ipg, gpt4_ipg, reserved10, reserved11, reserved12,
+	iim_ipg, reserved13, reserved14, kpp_ipg, lcdc_ipg, reserved15, pwm1_ipg,
+	pwm2_ipg, pwm3_ipg, pwm4_ipg, rngb_ipg, reserved16, scc_ipg, sdma_ipg,
+	sim1_ipg, sim2_ipg, slcdc_ipg, spba_ipg, ssi1_ipg, ssi2_ipg, tsc_ipg,
+	uart1_ipg, uart2_ipg, uart3_ipg, uart4_ipg, uart5_ipg, reserved17,
+	wdt_ipg, cko_div, cko_sel, cko, clk_max
+};
+
+static struct clk *clk[clk_max];
+
+static int __init __mx25_clocks_init(unsigned long osc_rate,
+				     void __iomem *ccm_base)
+{
+	BUG_ON(!ccm_base);
+
+	clk[dummy] = imx_clk_fixed("dummy", 0);
+	clk[osc] = imx_clk_fixed("osc", osc_rate);
+	clk[mpll] = imx_clk_pllv1(IMX_PLLV1_IMX25, "mpll", "osc", ccm(CCM_MPCTL));
+	clk[upll] = imx_clk_pllv1(IMX_PLLV1_IMX25, "upll", "osc", ccm(CCM_UPCTL));
+	clk[mpll_cpu_3_4] = imx_clk_fixed_factor("mpll_cpu_3_4", "mpll", 3, 4);
+	clk[cpu_sel] = imx_clk_mux("cpu_sel", ccm(CCM_CCTL), 14, 1, cpu_sel_clks, ARRAY_SIZE(cpu_sel_clks));
+	clk[cpu] = imx_clk_divider("cpu", "cpu_sel", ccm(CCM_CCTL), 30, 2);
+	clk[ahb] = imx_clk_divider("ahb", "cpu", ccm(CCM_CCTL), 28, 2);
+	clk[usb_div] = imx_clk_divider("usb_div", "upll", ccm(CCM_CCTL), 16, 6); 
+	clk[ipg] = imx_clk_fixed_factor("ipg", "ahb", 1, 2);
+	clk[per0_sel] = imx_clk_mux("per0_sel", ccm(CCM_MCR), 0, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per1_sel] = imx_clk_mux("per1_sel", ccm(CCM_MCR), 1, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per2_sel] = imx_clk_mux("per2_sel", ccm(CCM_MCR), 2, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per3_sel] = imx_clk_mux("per3_sel", ccm(CCM_MCR), 3, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per4_sel] = imx_clk_mux("per4_sel", ccm(CCM_MCR), 4, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per5_sel] = imx_clk_mux("per5_sel", ccm(CCM_MCR), 5, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per6_sel] = imx_clk_mux("per6_sel", ccm(CCM_MCR), 6, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per7_sel] = imx_clk_mux("per7_sel", ccm(CCM_MCR), 7, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per8_sel] = imx_clk_mux("per8_sel", ccm(CCM_MCR), 8, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per9_sel] = imx_clk_mux("per9_sel", ccm(CCM_MCR), 9, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per10_sel] = imx_clk_mux("per10_sel", ccm(CCM_MCR), 10, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per11_sel] = imx_clk_mux("per11_sel", ccm(CCM_MCR), 11, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per12_sel] = imx_clk_mux("per12_sel", ccm(CCM_MCR), 12, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per13_sel] = imx_clk_mux("per13_sel", ccm(CCM_MCR), 13, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per14_sel] = imx_clk_mux("per14_sel", ccm(CCM_MCR), 14, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[per15_sel] = imx_clk_mux("per15_sel", ccm(CCM_MCR), 15, 1, per_sel_clks, ARRAY_SIZE(per_sel_clks));
+	clk[cko_div] = imx_clk_divider("cko_div", "cko_sel", ccm(CCM_MCR), 24, 6);
+	clk[cko_sel] = imx_clk_mux("cko_sel", ccm(CCM_MCR), 20, 4, cko_sel_clks, ARRAY_SIZE(cko_sel_clks));
+	clk[cko] = imx_clk_gate("cko", "cko_div", ccm(CCM_MCR),  30);
+	clk[per0] = imx_clk_divider("per0", "per0_sel", ccm(CCM_PCDR0), 0, 6);
+	clk[per1] = imx_clk_divider("per1", "per1_sel", ccm(CCM_PCDR0), 8, 6);
+	clk[per2] = imx_clk_divider("per2", "per2_sel", ccm(CCM_PCDR0), 16, 6);
+	clk[per3] = imx_clk_divider("per3", "per3_sel", ccm(CCM_PCDR0), 24, 6);
+	clk[per4] = imx_clk_divider("per4", "per4_sel", ccm(CCM_PCDR1), 0, 6);
+	clk[per5] = imx_clk_divider("per5", "per5_sel", ccm(CCM_PCDR1), 8, 6);
+	clk[per6] = imx_clk_divider("per6", "per6_sel", ccm(CCM_PCDR1), 16, 6);
+	clk[per7] = imx_clk_divider("per7", "per7_sel", ccm(CCM_PCDR1), 24, 6);
+	clk[per8] = imx_clk_divider("per8", "per8_sel", ccm(CCM_PCDR2), 0, 6);
+	clk[per9] = imx_clk_divider("per9", "per9_sel", ccm(CCM_PCDR2), 8, 6);
+	clk[per10] = imx_clk_divider("per10", "per10_sel", ccm(CCM_PCDR2), 16, 6);
+	clk[per11] = imx_clk_divider("per11", "per11_sel", ccm(CCM_PCDR2), 24, 6);
+	clk[per12] = imx_clk_divider("per12", "per12_sel", ccm(CCM_PCDR3), 0, 6);
+	clk[per13] = imx_clk_divider("per13", "per13_sel", ccm(CCM_PCDR3), 8, 6);
+	clk[per14] = imx_clk_divider("per14", "per14_sel", ccm(CCM_PCDR3), 16, 6);
+	clk[per15] = imx_clk_divider("per15", "per15_sel", ccm(CCM_PCDR3), 24, 6);
+	clk[csi_ipg_per] = imx_clk_gate("csi_ipg_per", "per0", ccm(CCM_CGCR0), 0);
+	clk[epit_ipg_per] = imx_clk_gate("epit_ipg_per", "per1", ccm(CCM_CGCR0),  1);
+	clk[esai_ipg_per] = imx_clk_gate("esai_ipg_per", "per2", ccm(CCM_CGCR0),  2);
+	clk[esdhc1_ipg_per] = imx_clk_gate("esdhc1_ipg_per", "per3", ccm(CCM_CGCR0),  3);
+	clk[esdhc2_ipg_per] = imx_clk_gate("esdhc2_ipg_per", "per4", ccm(CCM_CGCR0),  4);
+	clk[gpt_ipg_per] = imx_clk_gate("gpt_ipg_per", "per5", ccm(CCM_CGCR0),  5);
+	clk[i2c_ipg_per] = imx_clk_gate("i2c_ipg_per", "per6", ccm(CCM_CGCR0),  6);
+	clk[lcdc_ipg_per] = imx_clk_gate("lcdc_ipg_per", "per7", ccm(CCM_CGCR0),  7);
+	clk[nfc_ipg_per] = imx_clk_gate("nfc_ipg_per", "per8", ccm(CCM_CGCR0),  8);
+	clk[owire_ipg_per] = imx_clk_gate("owire_ipg_per", "per9", ccm(CCM_CGCR0),  9);
+	clk[pwm_ipg_per] = imx_clk_gate("pwm_ipg_per", "per10", ccm(CCM_CGCR0),  10);
+	clk[sim1_ipg_per] = imx_clk_gate("sim1_ipg_per", "per11", ccm(CCM_CGCR0),  11);
+	clk[sim2_ipg_per] = imx_clk_gate("sim2_ipg_per", "per12", ccm(CCM_CGCR0),  12);
+	clk[ssi1_ipg_per] = imx_clk_gate("ssi1_ipg_per", "per13", ccm(CCM_CGCR0), 13);
+	clk[ssi2_ipg_per] = imx_clk_gate("ssi2_ipg_per", "per14", ccm(CCM_CGCR0), 14);
+	clk[uart_ipg_per] = imx_clk_gate("uart_ipg_per", "per15", ccm(CCM_CGCR0), 15);
+	clk[ata_ahb] = imx_clk_gate("ata_ahb", "ahb", ccm(CCM_CGCR0), 16);
+	/* CCM_CGCR0(17): reserved */
+	clk[csi_ahb] = imx_clk_gate("csi_ahb", "ahb", ccm(CCM_CGCR0), 18);
+	clk[emi_ahb] = imx_clk_gate("emi_ahb", "ahb", ccm(CCM_CGCR0), 19);
+	clk[esai_ahb] = imx_clk_gate("esai_ahb", "ahb", ccm(CCM_CGCR0), 20);
+	clk[esdhc1_ahb] = imx_clk_gate("esdhc1_ahb", "ahb", ccm(CCM_CGCR0), 21);
+	clk[esdhc2_ahb] = imx_clk_gate("esdhc2_ahb", "ahb", ccm(CCM_CGCR0), 22);
+	clk[fec_ahb] = imx_clk_gate("fec_ahb", "ahb", ccm(CCM_CGCR0), 23);
+	clk[lcdc_ahb] = imx_clk_gate("lcdc_ahb", "ahb", ccm(CCM_CGCR0), 24);
+	clk[rtic_ahb] = imx_clk_gate("rtic_ahb", "ahb", ccm(CCM_CGCR0), 25);
+	clk[sdma_ahb] = imx_clk_gate("sdma_ahb", "ahb", ccm(CCM_CGCR0), 26);
+	clk[slcdc_ahb] = imx_clk_gate("slcdc_ahb", "ahb", ccm(CCM_CGCR0), 27);
+	clk[usbotg_ahb] = imx_clk_gate("usbotg_ahb", "ahb", ccm(CCM_CGCR0), 28);
+	/* CCM_CGCR0(29-31): reserved */
+	/* CCM_CGCR1(0): reserved in datasheet, used as audmux in FSL kernel */
+	clk[can1_ipg] = imx_clk_gate("can1_ipg", "ipg", ccm(CCM_CGCR1),  2);
+	clk[can2_ipg] = imx_clk_gate("can2_ipg", "ipg", ccm(CCM_CGCR1),  3);
+	clk[csi_ipg] = imx_clk_gate("csi_ipg", "ipg", ccm(CCM_CGCR1),  4);
+	clk[cspi1_ipg] = imx_clk_gate("cspi1_ipg", "ipg", ccm(CCM_CGCR1),  5);
+	clk[cspi2_ipg] = imx_clk_gate("cspi2_ipg", "ipg", ccm(CCM_CGCR1),  6);
+	clk[cspi3_ipg] = imx_clk_gate("cspi3_ipg", "ipg", ccm(CCM_CGCR1),  7);
+	clk[dryice_ipg] = imx_clk_gate("dryice_ipg", "ipg", ccm(CCM_CGCR1),  8);
+	clk[ect_ipg] = imx_clk_gate("ect_ipg", "ipg", ccm(CCM_CGCR1),  9);
+	clk[epit1_ipg] = imx_clk_gate("epit1_ipg", "ipg", ccm(CCM_CGCR1),  10);
+	clk[epit2_ipg] = imx_clk_gate("epit2_ipg", "ipg", ccm(CCM_CGCR1),  11);
+	/* CCM_CGCR1(12): reserved in datasheet, used as esai in FSL kernel */
+	clk[esdhc1_ipg] = imx_clk_gate("esdhc1_ipg", "ipg", ccm(CCM_CGCR1), 13);
+	clk[esdhc2_ipg] = imx_clk_gate("esdhc2_ipg", "ipg", ccm(CCM_CGCR1), 14);
+	clk[fec_ipg] = imx_clk_gate("fec_ipg", "ipg", ccm(CCM_CGCR1), 15);
+	/* CCM_CGCR1(16): reserved in datasheet, used as gpio1 in FSL kernel */
+	/* CCM_CGCR1(17): reserved in datasheet, used as gpio2 in FSL kernel */
+	/* CCM_CGCR1(18): reserved in datasheet, used as gpio3 in FSL kernel */
+	clk[gpt1_ipg] = imx_clk_gate("gpt1_ipg", "ipg", ccm(CCM_CGCR1), 19);
+	clk[gpt2_ipg] = imx_clk_gate("gpt2_ipg", "ipg", ccm(CCM_CGCR1), 20);
+	clk[gpt3_ipg] = imx_clk_gate("gpt3_ipg", "ipg", ccm(CCM_CGCR1), 21);
+	clk[gpt4_ipg] = imx_clk_gate("gpt4_ipg", "ipg", ccm(CCM_CGCR1), 22);
+	/* CCM_CGCR1(23): reserved in datasheet, used as i2c1 in FSL kernel */
+	/* CCM_CGCR1(24): reserved in datasheet, used as i2c2 in FSL kernel */
+	/* CCM_CGCR1(25): reserved in datasheet, used as i2c3 in FSL kernel */
+	clk[iim_ipg] = imx_clk_gate("iim_ipg", "ipg", ccm(CCM_CGCR1), 26);
+	/* CCM_CGCR1(27): reserved in datasheet, used as iomuxc in FSL kernel */
+	/* CCM_CGCR1(28): reserved in datasheet, used as kpp in FSL kernel */
+	clk[kpp_ipg] = imx_clk_gate("kpp_ipg", "ipg", ccm(CCM_CGCR1), 28);
+	clk[lcdc_ipg] = imx_clk_gate("lcdc_ipg", "ipg", ccm(CCM_CGCR1), 29);
+	/* CCM_CGCR1(30): reserved in datasheet, used as owire in FSL kernel */
+	clk[pwm1_ipg] = imx_clk_gate("pwm1_ipg", "ipg", ccm(CCM_CGCR1), 31);
+	clk[pwm2_ipg] = imx_clk_gate("pwm2_ipg", "ipg", ccm(CCM_CGCR2),  0);
+	clk[pwm3_ipg] = imx_clk_gate("pwm3_ipg", "ipg", ccm(CCM_CGCR2),  1);
+	clk[pwm4_ipg] = imx_clk_gate("pwm4_ipg", "ipg", ccm(CCM_CGCR2),  2);
+	clk[rngb_ipg] = imx_clk_gate("rngb_ipg", "ipg", ccm(CCM_CGCR2),  3);
+	/* CCM_CGCR2(4): reserved in datasheet, used as rtic in FSL kernel */
+	clk[scc_ipg] = imx_clk_gate("scc_ipg", "ipg", ccm(CCM_CGCR2),  5);
+	clk[sdma_ipg] = imx_clk_gate("sdma_ipg", "ipg", ccm(CCM_CGCR2),  6);
+	clk[sim1_ipg] = imx_clk_gate("sim1_ipg", "ipg", ccm(CCM_CGCR2),  7);
+	clk[sim2_ipg] = imx_clk_gate("sim2_ipg", "ipg", ccm(CCM_CGCR2),  8);
+	clk[slcdc_ipg] = imx_clk_gate("slcdc_ipg", "ipg", ccm(CCM_CGCR2),  9);
+	clk[spba_ipg] = imx_clk_gate("spba_ipg", "ipg", ccm(CCM_CGCR2),  10);
+	clk[ssi1_ipg] = imx_clk_gate("ssi1_ipg", "ipg", ccm(CCM_CGCR2), 11);
+	clk[ssi2_ipg] = imx_clk_gate("ssi2_ipg", "ipg", ccm(CCM_CGCR2), 12);
+	clk[tsc_ipg] = imx_clk_gate("tsc_ipg", "ipg", ccm(CCM_CGCR2), 13);
+	clk[uart1_ipg] = imx_clk_gate("uart1_ipg", "ipg", ccm(CCM_CGCR2), 14);
+	clk[uart2_ipg] = imx_clk_gate("uart2_ipg", "ipg", ccm(CCM_CGCR2), 15);
+	clk[uart3_ipg] = imx_clk_gate("uart3_ipg", "ipg", ccm(CCM_CGCR2), 16);
+	clk[uart4_ipg] = imx_clk_gate("uart4_ipg", "ipg", ccm(CCM_CGCR2), 17);
+	clk[uart5_ipg] = imx_clk_gate("uart5_ipg", "ipg", ccm(CCM_CGCR2), 18);
+	/* CCM_CGCR2(19): reserved in datasheet, but used as wdt in FSL kernel */
+	clk[wdt_ipg] = imx_clk_gate("wdt_ipg", "ipg", ccm(CCM_CGCR2), 19);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	clk_prepare_enable(clk[emi_ahb]);
+
+	/* Clock source for gpt must be derived from AHB */
+	clk_set_parent(clk[per5_sel], clk[ahb]);
+
+	/*
+	 * Let's initially set up CLKO parent as ipg, since this configuration
+	 * is used on some imx25 board designs to clock the audio codec.
+	 */
+	clk_set_parent(clk[cko_sel], clk[ipg]);
+
+	return 0;
+}
+
+static void __init mx25_clocks_init_dt(struct device_node *np)
+{
+	struct device_node *refnp;
+	unsigned long osc_rate = 24000000;
+	void __iomem *ccm;
+
+	/* retrieve the freqency of fixed clocks from device tree */
+	for_each_compatible_node(refnp, NULL, "fixed-clock") {
+		u32 rate;
+		if (of_property_read_u32(refnp, "clock-frequency", &rate))
+			continue;
+
+		if (of_device_is_compatible(refnp, "fsl,imx-osc"))
+			osc_rate = rate;
+	}
+
+	ccm = of_iomap(np, 0);
+	__mx25_clocks_init(osc_rate, ccm);
+
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+}
+CLK_OF_DECLARE(imx25_ccm, "fsl,imx25-ccm", mx25_clocks_init_dt);
diff --git a/drivers/clk/imx/clk-imx27.c b/drivers/clk/imx/clk-imx27.c
new file mode 100644
index 000000000000..d9d50d54ef2a
--- /dev/null
+++ b/drivers/clk/imx/clk-imx27.c
@@ -0,0 +1,263 @@
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <dt-bindings/clock/imx27-clock.h>
+#include <soc/imx/revision.h>
+#include <soc/imx/timer.h>
+#include <asm/irq.h>
+
+#include "clk.h"
+
+#define MX27_CCM_BASE_ADDR	0x10027000
+#define MX27_GPT1_BASE_ADDR	0x10003000
+#define MX27_INT_GPT1		(NR_IRQS_LEGACY + 26)
+
+static void __iomem *ccm __initdata;
+
+/* Register offsets */
+#define CCM_CSCR		(ccm + 0x00)
+#define CCM_MPCTL0		(ccm + 0x04)
+#define CCM_MPCTL1		(ccm + 0x08)
+#define CCM_SPCTL0		(ccm + 0x0c)
+#define CCM_SPCTL1		(ccm + 0x10)
+#define CCM_PCDR0		(ccm + 0x18)
+#define CCM_PCDR1		(ccm + 0x1c)
+#define CCM_PCCR0		(ccm + 0x20)
+#define CCM_PCCR1		(ccm + 0x24)
+#define CCM_CCSR		(ccm + 0x28)
+
+static const char *vpu_sel_clks[] = { "spll", "mpll_main2", };
+static const char *cpu_sel_clks[] = { "mpll_main2", "mpll", };
+static const char *mpll_sel_clks[] = { "fpm", "mpll_osc_sel", };
+static const char *mpll_osc_sel_clks[] = { "ckih_gate", "ckih_div1p5", };
+static const char *clko_sel_clks[] = {
+	"ckil", "fpm", "ckih_gate", "ckih_gate",
+	"ckih_gate", "mpll", "spll", "cpu_div",
+	"ahb", "ipg", "per1_div", "per2_div",
+	"per3_div", "per4_div", "ssi1_div", "ssi2_div",
+	"nfc_div", "mshc_div", "vpu_div", "60m",
+	"32k", "usb_div", "dptc",
+};
+
+static const char *ssi_sel_clks[] = { "spll_gate", "mpll", };
+
+static struct clk *clk[IMX27_CLK_MAX];
+static struct clk_onecell_data clk_data;
+
+static void __init _mx27_clocks_init(unsigned long fref)
+{
+	BUG_ON(!ccm);
+
+	clk[IMX27_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+	clk[IMX27_CLK_CKIH] = imx_clk_fixed("ckih", fref);
+	clk[IMX27_CLK_CKIL] = imx_clk_fixed("ckil", 32768);
+	clk[IMX27_CLK_FPM] = imx_clk_fixed_factor("fpm", "ckil", 1024, 1);
+	clk[IMX27_CLK_CKIH_DIV1P5] = imx_clk_fixed_factor("ckih_div1p5", "ckih_gate", 2, 3);
+	clk[IMX27_CLK_CKIH_GATE] = imx_clk_gate_dis("ckih_gate", "ckih", CCM_CSCR, 3);
+	clk[IMX27_CLK_MPLL_OSC_SEL] = imx_clk_mux("mpll_osc_sel", CCM_CSCR, 4, 1, mpll_osc_sel_clks, ARRAY_SIZE(mpll_osc_sel_clks));
+	clk[IMX27_CLK_MPLL_SEL] = imx_clk_mux("mpll_sel", CCM_CSCR, 16, 1, mpll_sel_clks, ARRAY_SIZE(mpll_sel_clks));
+	clk[IMX27_CLK_MPLL] = imx_clk_pllv1(IMX_PLLV1_IMX27, "mpll", "mpll_sel", CCM_MPCTL0);
+	clk[IMX27_CLK_SPLL] = imx_clk_pllv1(IMX_PLLV1_IMX27, "spll", "ckih_gate", CCM_SPCTL0);
+	clk[IMX27_CLK_SPLL_GATE] = imx_clk_gate("spll_gate", "spll", CCM_CSCR, 1);
+	clk[IMX27_CLK_MPLL_MAIN2] = imx_clk_fixed_factor("mpll_main2", "mpll", 2, 3);
+
+	if (mx27_revision() >= IMX_CHIP_REVISION_2_0) {
+		clk[IMX27_CLK_AHB] = imx_clk_divider("ahb", "mpll_main2", CCM_CSCR, 8, 2);
+		clk[IMX27_CLK_IPG] = imx_clk_fixed_factor("ipg", "ahb", 1, 2);
+	} else {
+		clk[IMX27_CLK_AHB] = imx_clk_divider("ahb", "mpll_main2", CCM_CSCR, 9, 4);
+		clk[IMX27_CLK_IPG] = imx_clk_divider("ipg", "ahb", CCM_CSCR, 8, 1);
+	}
+
+	clk[IMX27_CLK_MSHC_DIV] = imx_clk_divider("mshc_div", "ahb", CCM_PCDR0, 0, 6);
+	clk[IMX27_CLK_NFC_DIV] = imx_clk_divider("nfc_div", "ahb", CCM_PCDR0, 6, 4);
+	clk[IMX27_CLK_PER1_DIV] = imx_clk_divider("per1_div", "mpll_main2", CCM_PCDR1, 0, 6);
+	clk[IMX27_CLK_PER2_DIV] = imx_clk_divider("per2_div", "mpll_main2", CCM_PCDR1, 8, 6);
+	clk[IMX27_CLK_PER3_DIV] = imx_clk_divider("per3_div", "mpll_main2", CCM_PCDR1, 16, 6);
+	clk[IMX27_CLK_PER4_DIV] = imx_clk_divider("per4_div", "mpll_main2", CCM_PCDR1, 24, 6);
+	clk[IMX27_CLK_VPU_SEL] = imx_clk_mux("vpu_sel", CCM_CSCR, 21, 1, vpu_sel_clks, ARRAY_SIZE(vpu_sel_clks));
+	clk[IMX27_CLK_VPU_DIV] = imx_clk_divider("vpu_div", "vpu_sel", CCM_PCDR0, 10, 6);
+	clk[IMX27_CLK_USB_DIV] = imx_clk_divider("usb_div", "spll_gate", CCM_CSCR, 28, 3);
+	clk[IMX27_CLK_CPU_SEL] = imx_clk_mux("cpu_sel", CCM_CSCR, 15, 1, cpu_sel_clks, ARRAY_SIZE(cpu_sel_clks));
+	clk[IMX27_CLK_CLKO_SEL] = imx_clk_mux("clko_sel", CCM_CCSR, 0, 5, clko_sel_clks, ARRAY_SIZE(clko_sel_clks));
+
+	if (mx27_revision() >= IMX_CHIP_REVISION_2_0)
+		clk[IMX27_CLK_CPU_DIV] = imx_clk_divider("cpu_div", "cpu_sel", CCM_CSCR, 12, 2);
+	else
+		clk[IMX27_CLK_CPU_DIV] = imx_clk_divider("cpu_div", "cpu_sel", CCM_CSCR, 13, 3);
+
+	clk[IMX27_CLK_CLKO_DIV] = imx_clk_divider("clko_div", "clko_sel", CCM_PCDR0, 22, 3);
+	clk[IMX27_CLK_SSI1_SEL] = imx_clk_mux("ssi1_sel", CCM_CSCR, 22, 1, ssi_sel_clks, ARRAY_SIZE(ssi_sel_clks));
+	clk[IMX27_CLK_SSI2_SEL] = imx_clk_mux("ssi2_sel", CCM_CSCR, 23, 1, ssi_sel_clks, ARRAY_SIZE(ssi_sel_clks));
+	clk[IMX27_CLK_SSI1_DIV] = imx_clk_divider("ssi1_div", "ssi1_sel", CCM_PCDR0, 16, 6);
+	clk[IMX27_CLK_SSI2_DIV] = imx_clk_divider("ssi2_div", "ssi2_sel", CCM_PCDR0, 26, 6);
+	clk[IMX27_CLK_CLKO_EN] = imx_clk_gate("clko_en", "clko_div", CCM_PCCR0, 0);
+	clk[IMX27_CLK_SSI2_IPG_GATE] = imx_clk_gate("ssi2_ipg_gate", "ipg", CCM_PCCR0, 0);
+	clk[IMX27_CLK_SSI1_IPG_GATE] = imx_clk_gate("ssi1_ipg_gate", "ipg", CCM_PCCR0, 1);
+	clk[IMX27_CLK_SLCDC_IPG_GATE] = imx_clk_gate("slcdc_ipg_gate", "ipg", CCM_PCCR0, 2);
+	clk[IMX27_CLK_SDHC3_IPG_GATE] = imx_clk_gate("sdhc3_ipg_gate", "ipg", CCM_PCCR0, 3);
+	clk[IMX27_CLK_SDHC2_IPG_GATE] = imx_clk_gate("sdhc2_ipg_gate", "ipg", CCM_PCCR0, 4);
+	clk[IMX27_CLK_SDHC1_IPG_GATE] = imx_clk_gate("sdhc1_ipg_gate", "ipg", CCM_PCCR0, 5);
+	clk[IMX27_CLK_SCC_IPG_GATE] = imx_clk_gate("scc_ipg_gate", "ipg", CCM_PCCR0, 6);
+	clk[IMX27_CLK_SAHARA_IPG_GATE] = imx_clk_gate("sahara_ipg_gate", "ipg", CCM_PCCR0, 7);
+	clk[IMX27_CLK_RTIC_IPG_GATE] = imx_clk_gate("rtic_ipg_gate", "ipg", CCM_PCCR0, 8);
+	clk[IMX27_CLK_RTC_IPG_GATE] = imx_clk_gate("rtc_ipg_gate", "ipg", CCM_PCCR0, 9);
+	clk[IMX27_CLK_PWM_IPG_GATE] = imx_clk_gate("pwm_ipg_gate", "ipg", CCM_PCCR0, 11);
+	clk[IMX27_CLK_OWIRE_IPG_GATE] = imx_clk_gate("owire_ipg_gate", "ipg", CCM_PCCR0, 12);
+	clk[IMX27_CLK_MSHC_IPG_GATE] = imx_clk_gate("mshc_ipg_gate", "ipg", CCM_PCCR0, 13);
+	clk[IMX27_CLK_LCDC_IPG_GATE] = imx_clk_gate("lcdc_ipg_gate", "ipg", CCM_PCCR0, 14);
+	clk[IMX27_CLK_KPP_IPG_GATE] = imx_clk_gate("kpp_ipg_gate", "ipg", CCM_PCCR0, 15);
+	clk[IMX27_CLK_IIM_IPG_GATE] = imx_clk_gate("iim_ipg_gate", "ipg", CCM_PCCR0, 16);
+	clk[IMX27_CLK_I2C2_IPG_GATE] = imx_clk_gate("i2c2_ipg_gate", "ipg", CCM_PCCR0, 17);
+	clk[IMX27_CLK_I2C1_IPG_GATE] = imx_clk_gate("i2c1_ipg_gate", "ipg", CCM_PCCR0, 18);
+	clk[IMX27_CLK_GPT6_IPG_GATE] = imx_clk_gate("gpt6_ipg_gate", "ipg", CCM_PCCR0, 19);
+	clk[IMX27_CLK_GPT5_IPG_GATE] = imx_clk_gate("gpt5_ipg_gate", "ipg", CCM_PCCR0, 20);
+	clk[IMX27_CLK_GPT4_IPG_GATE] = imx_clk_gate("gpt4_ipg_gate", "ipg", CCM_PCCR0, 21);
+	clk[IMX27_CLK_GPT3_IPG_GATE] = imx_clk_gate("gpt3_ipg_gate", "ipg", CCM_PCCR0, 22);
+	clk[IMX27_CLK_GPT2_IPG_GATE] = imx_clk_gate("gpt2_ipg_gate", "ipg", CCM_PCCR0, 23);
+	clk[IMX27_CLK_GPT1_IPG_GATE] = imx_clk_gate("gpt1_ipg_gate", "ipg", CCM_PCCR0, 24);
+	clk[IMX27_CLK_GPIO_IPG_GATE] = imx_clk_gate("gpio_ipg_gate", "ipg", CCM_PCCR0, 25);
+	clk[IMX27_CLK_FEC_IPG_GATE] = imx_clk_gate("fec_ipg_gate", "ipg", CCM_PCCR0, 26);
+	clk[IMX27_CLK_EMMA_IPG_GATE] = imx_clk_gate("emma_ipg_gate", "ipg", CCM_PCCR0, 27);
+	clk[IMX27_CLK_DMA_IPG_GATE] = imx_clk_gate("dma_ipg_gate", "ipg", CCM_PCCR0, 28);
+	clk[IMX27_CLK_CSPI3_IPG_GATE] = imx_clk_gate("cspi3_ipg_gate", "ipg", CCM_PCCR0, 29);
+	clk[IMX27_CLK_CSPI2_IPG_GATE] = imx_clk_gate("cspi2_ipg_gate", "ipg", CCM_PCCR0, 30);
+	clk[IMX27_CLK_CSPI1_IPG_GATE] = imx_clk_gate("cspi1_ipg_gate", "ipg", CCM_PCCR0, 31);
+	clk[IMX27_CLK_MSHC_BAUD_GATE] = imx_clk_gate("mshc_baud_gate", "mshc_div", CCM_PCCR1, 2);
+	clk[IMX27_CLK_NFC_BAUD_GATE] = imx_clk_gate("nfc_baud_gate", "nfc_div", CCM_PCCR1,  3);
+	clk[IMX27_CLK_SSI2_BAUD_GATE] = imx_clk_gate("ssi2_baud_gate", "ssi2_div", CCM_PCCR1,  4);
+	clk[IMX27_CLK_SSI1_BAUD_GATE] = imx_clk_gate("ssi1_baud_gate", "ssi1_div", CCM_PCCR1,  5);
+	clk[IMX27_CLK_VPU_BAUD_GATE] = imx_clk_gate("vpu_baud_gate", "vpu_div", CCM_PCCR1,  6);
+	clk[IMX27_CLK_PER4_GATE] = imx_clk_gate("per4_gate", "per4_div", CCM_PCCR1,  7);
+	clk[IMX27_CLK_PER3_GATE] = imx_clk_gate("per3_gate", "per3_div", CCM_PCCR1,  8);
+	clk[IMX27_CLK_PER2_GATE] = imx_clk_gate("per2_gate", "per2_div", CCM_PCCR1,  9);
+	clk[IMX27_CLK_PER1_GATE] = imx_clk_gate("per1_gate", "per1_div", CCM_PCCR1, 10);
+	clk[IMX27_CLK_USB_AHB_GATE] = imx_clk_gate("usb_ahb_gate", "ahb", CCM_PCCR1, 11);
+	clk[IMX27_CLK_SLCDC_AHB_GATE] = imx_clk_gate("slcdc_ahb_gate", "ahb", CCM_PCCR1, 12);
+	clk[IMX27_CLK_SAHARA_AHB_GATE] = imx_clk_gate("sahara_ahb_gate", "ahb", CCM_PCCR1, 13);
+	clk[IMX27_CLK_RTIC_AHB_GATE] = imx_clk_gate("rtic_ahb_gate", "ahb", CCM_PCCR1, 14);
+	clk[IMX27_CLK_LCDC_AHB_GATE] = imx_clk_gate("lcdc_ahb_gate", "ahb", CCM_PCCR1, 15);
+	clk[IMX27_CLK_VPU_AHB_GATE] = imx_clk_gate("vpu_ahb_gate", "ahb", CCM_PCCR1, 16);
+	clk[IMX27_CLK_FEC_AHB_GATE] = imx_clk_gate("fec_ahb_gate", "ahb", CCM_PCCR1, 17);
+	clk[IMX27_CLK_EMMA_AHB_GATE] = imx_clk_gate("emma_ahb_gate", "ahb", CCM_PCCR1, 18);
+	clk[IMX27_CLK_EMI_AHB_GATE] = imx_clk_gate("emi_ahb_gate", "ahb", CCM_PCCR1, 19);
+	clk[IMX27_CLK_DMA_AHB_GATE] = imx_clk_gate("dma_ahb_gate", "ahb", CCM_PCCR1, 20);
+	clk[IMX27_CLK_CSI_AHB_GATE] = imx_clk_gate("csi_ahb_gate", "ahb", CCM_PCCR1, 21);
+	clk[IMX27_CLK_BROM_AHB_GATE] = imx_clk_gate("brom_ahb_gate", "ahb", CCM_PCCR1, 22);
+	clk[IMX27_CLK_ATA_AHB_GATE] = imx_clk_gate("ata_ahb_gate", "ahb", CCM_PCCR1, 23);
+	clk[IMX27_CLK_WDOG_IPG_GATE] = imx_clk_gate("wdog_ipg_gate", "ipg", CCM_PCCR1, 24);
+	clk[IMX27_CLK_USB_IPG_GATE] = imx_clk_gate("usb_ipg_gate", "ipg", CCM_PCCR1, 25);
+	clk[IMX27_CLK_UART6_IPG_GATE] = imx_clk_gate("uart6_ipg_gate", "ipg", CCM_PCCR1, 26);
+	clk[IMX27_CLK_UART5_IPG_GATE] = imx_clk_gate("uart5_ipg_gate", "ipg", CCM_PCCR1, 27);
+	clk[IMX27_CLK_UART4_IPG_GATE] = imx_clk_gate("uart4_ipg_gate", "ipg", CCM_PCCR1, 28);
+	clk[IMX27_CLK_UART3_IPG_GATE] = imx_clk_gate("uart3_ipg_gate", "ipg", CCM_PCCR1, 29);
+	clk[IMX27_CLK_UART2_IPG_GATE] = imx_clk_gate("uart2_ipg_gate", "ipg", CCM_PCCR1, 30);
+	clk[IMX27_CLK_UART1_IPG_GATE] = imx_clk_gate("uart1_ipg_gate", "ipg", CCM_PCCR1, 31);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	clk_register_clkdev(clk[IMX27_CLK_CPU_DIV], NULL, "cpu0");
+
+	clk_prepare_enable(clk[IMX27_CLK_EMI_AHB_GATE]);
+
+	imx_print_silicon_rev("i.MX27", mx27_revision());
+}
+
+int __init mx27_clocks_init(unsigned long fref)
+{
+	ccm = ioremap(MX27_CCM_BASE_ADDR, SZ_4K);
+
+	_mx27_clocks_init(fref);
+
+	clk_register_clkdev(clk[IMX27_CLK_UART1_IPG_GATE], "ipg", "imx21-uart.0");
+	clk_register_clkdev(clk[IMX27_CLK_PER1_GATE], "per", "imx21-uart.0");
+	clk_register_clkdev(clk[IMX27_CLK_UART2_IPG_GATE], "ipg", "imx21-uart.1");
+	clk_register_clkdev(clk[IMX27_CLK_PER1_GATE], "per", "imx21-uart.1");
+	clk_register_clkdev(clk[IMX27_CLK_UART3_IPG_GATE], "ipg", "imx21-uart.2");
+	clk_register_clkdev(clk[IMX27_CLK_PER1_GATE], "per", "imx21-uart.2");
+	clk_register_clkdev(clk[IMX27_CLK_UART4_IPG_GATE], "ipg", "imx21-uart.3");
+	clk_register_clkdev(clk[IMX27_CLK_PER1_GATE], "per", "imx21-uart.3");
+	clk_register_clkdev(clk[IMX27_CLK_UART5_IPG_GATE], "ipg", "imx21-uart.4");
+	clk_register_clkdev(clk[IMX27_CLK_PER1_GATE], "per", "imx21-uart.4");
+	clk_register_clkdev(clk[IMX27_CLK_UART6_IPG_GATE], "ipg", "imx21-uart.5");
+	clk_register_clkdev(clk[IMX27_CLK_PER1_GATE], "per", "imx21-uart.5");
+	clk_register_clkdev(clk[IMX27_CLK_GPT1_IPG_GATE], "ipg", "imx-gpt.0");
+	clk_register_clkdev(clk[IMX27_CLK_PER1_GATE], "per", "imx-gpt.0");
+	clk_register_clkdev(clk[IMX27_CLK_PER2_GATE], "per", "imx21-mmc.0");
+	clk_register_clkdev(clk[IMX27_CLK_SDHC1_IPG_GATE], "ipg", "imx21-mmc.0");
+	clk_register_clkdev(clk[IMX27_CLK_PER2_GATE], "per", "imx21-mmc.1");
+	clk_register_clkdev(clk[IMX27_CLK_SDHC2_IPG_GATE], "ipg", "imx21-mmc.1");
+	clk_register_clkdev(clk[IMX27_CLK_PER2_GATE], "per", "imx21-mmc.2");
+	clk_register_clkdev(clk[IMX27_CLK_SDHC2_IPG_GATE], "ipg", "imx21-mmc.2");
+	clk_register_clkdev(clk[IMX27_CLK_PER2_GATE], "per", "imx27-cspi.0");
+	clk_register_clkdev(clk[IMX27_CLK_CSPI1_IPG_GATE], "ipg", "imx27-cspi.0");
+	clk_register_clkdev(clk[IMX27_CLK_PER2_GATE], "per", "imx27-cspi.1");
+	clk_register_clkdev(clk[IMX27_CLK_CSPI2_IPG_GATE], "ipg", "imx27-cspi.1");
+	clk_register_clkdev(clk[IMX27_CLK_PER2_GATE], "per", "imx27-cspi.2");
+	clk_register_clkdev(clk[IMX27_CLK_CSPI3_IPG_GATE], "ipg", "imx27-cspi.2");
+	clk_register_clkdev(clk[IMX27_CLK_PER3_GATE], "per", "imx21-fb.0");
+	clk_register_clkdev(clk[IMX27_CLK_LCDC_IPG_GATE], "ipg", "imx21-fb.0");
+	clk_register_clkdev(clk[IMX27_CLK_LCDC_AHB_GATE], "ahb", "imx21-fb.0");
+	clk_register_clkdev(clk[IMX27_CLK_CSI_AHB_GATE], "ahb", "imx27-camera.0");
+	clk_register_clkdev(clk[IMX27_CLK_PER4_GATE], "per", "imx27-camera.0");
+	clk_register_clkdev(clk[IMX27_CLK_USB_DIV], "per", "imx-udc-mx27");
+	clk_register_clkdev(clk[IMX27_CLK_USB_IPG_GATE], "ipg", "imx-udc-mx27");
+	clk_register_clkdev(clk[IMX27_CLK_USB_AHB_GATE], "ahb", "imx-udc-mx27");
+	clk_register_clkdev(clk[IMX27_CLK_USB_DIV], "per", "mxc-ehci.0");
+	clk_register_clkdev(clk[IMX27_CLK_USB_IPG_GATE], "ipg", "mxc-ehci.0");
+	clk_register_clkdev(clk[IMX27_CLK_USB_AHB_GATE], "ahb", "mxc-ehci.0");
+	clk_register_clkdev(clk[IMX27_CLK_USB_DIV], "per", "mxc-ehci.1");
+	clk_register_clkdev(clk[IMX27_CLK_USB_IPG_GATE], "ipg", "mxc-ehci.1");
+	clk_register_clkdev(clk[IMX27_CLK_USB_AHB_GATE], "ahb", "mxc-ehci.1");
+	clk_register_clkdev(clk[IMX27_CLK_USB_DIV], "per", "mxc-ehci.2");
+	clk_register_clkdev(clk[IMX27_CLK_USB_IPG_GATE], "ipg", "mxc-ehci.2");
+	clk_register_clkdev(clk[IMX27_CLK_USB_AHB_GATE], "ahb", "mxc-ehci.2");
+	clk_register_clkdev(clk[IMX27_CLK_SSI1_IPG_GATE], NULL, "imx-ssi.0");
+	clk_register_clkdev(clk[IMX27_CLK_SSI2_IPG_GATE], NULL, "imx-ssi.1");
+	clk_register_clkdev(clk[IMX27_CLK_NFC_BAUD_GATE], NULL, "imx27-nand.0");
+	clk_register_clkdev(clk[IMX27_CLK_VPU_BAUD_GATE], "per", "coda-imx27.0");
+	clk_register_clkdev(clk[IMX27_CLK_VPU_AHB_GATE], "ahb", "coda-imx27.0");
+	clk_register_clkdev(clk[IMX27_CLK_DMA_AHB_GATE], "ahb", "imx27-dma");
+	clk_register_clkdev(clk[IMX27_CLK_DMA_IPG_GATE], "ipg", "imx27-dma");
+	clk_register_clkdev(clk[IMX27_CLK_FEC_IPG_GATE], "ipg", "imx27-fec.0");
+	clk_register_clkdev(clk[IMX27_CLK_FEC_AHB_GATE], "ahb", "imx27-fec.0");
+	clk_register_clkdev(clk[IMX27_CLK_WDOG_IPG_GATE], NULL, "imx2-wdt.0");
+	clk_register_clkdev(clk[IMX27_CLK_I2C1_IPG_GATE], NULL, "imx21-i2c.0");
+	clk_register_clkdev(clk[IMX27_CLK_I2C2_IPG_GATE], NULL, "imx21-i2c.1");
+	clk_register_clkdev(clk[IMX27_CLK_OWIRE_IPG_GATE], NULL, "mxc_w1.0");
+	clk_register_clkdev(clk[IMX27_CLK_KPP_IPG_GATE], NULL, "imx-keypad");
+	clk_register_clkdev(clk[IMX27_CLK_EMMA_AHB_GATE], "emma-ahb", "imx27-camera.0");
+	clk_register_clkdev(clk[IMX27_CLK_EMMA_IPG_GATE], "emma-ipg", "imx27-camera.0");
+	clk_register_clkdev(clk[IMX27_CLK_EMMA_AHB_GATE], "ahb", "m2m-emmaprp.0");
+	clk_register_clkdev(clk[IMX27_CLK_EMMA_IPG_GATE], "ipg", "m2m-emmaprp.0");
+
+	mxc_timer_init(MX27_GPT1_BASE_ADDR, MX27_INT_GPT1, GPT_TYPE_IMX21);
+
+	return 0;
+}
+
+static void __init mx27_clocks_init_dt(struct device_node *np)
+{
+	struct device_node *refnp;
+	u32 fref = 26000000; /* default */
+
+	for_each_compatible_node(refnp, NULL, "fixed-clock") {
+		if (!of_device_is_compatible(refnp, "fsl,imx-osc26m"))
+			continue;
+
+		if (!of_property_read_u32(refnp, "clock-frequency", &fref))
+			break;
+	}
+
+	ccm = of_iomap(np, 0);
+
+	_mx27_clocks_init(fref);
+
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+}
+CLK_OF_DECLARE(imx27_ccm, "fsl,imx27-ccm", mx27_clocks_init_dt);
diff --git a/drivers/clk/imx/clk-imx31.c b/drivers/clk/imx/clk-imx31.c
new file mode 100644
index 000000000000..fe66c40b7be2
--- /dev/null
+++ b/drivers/clk/imx/clk-imx31.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (C) 2012 Sascha Hauer <kernel@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, 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.
+ */
+
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <soc/imx/revision.h>
+#include <soc/imx/timer.h>
+#include <asm/irq.h>
+
+#include "clk.h"
+
+#define MX31_CCM_BASE_ADDR	0x53f80000
+#define MX31_GPT1_BASE_ADDR	0x53f90000
+#define MX31_INT_GPT		(NR_IRQS_LEGACY + 29)
+
+#define MXC_CCM_CCMR		0x00
+#define MXC_CCM_PDR0		0x04
+#define MXC_CCM_PDR1		0x08
+#define MXC_CCM_MPCTL		0x10
+#define MXC_CCM_UPCTL		0x14
+#define MXC_CCM_SRPCTL		0x18
+#define MXC_CCM_CGR0		0x20
+#define MXC_CCM_CGR1		0x24
+#define MXC_CCM_CGR2		0x28
+#define MXC_CCM_PMCR0		0x5c
+
+static const char *mcu_main_sel[] = { "spll", "mpll", };
+static const char *per_sel[] = { "per_div", "ipg", };
+static const char *csi_sel[] = { "upll", "spll", };
+static const char *fir_sel[] = { "mcu_main", "upll", "spll" };
+
+enum mx31_clks {
+	dummy, ckih, ckil, mpll, spll, upll, mcu_main, hsp, ahb, nfc, ipg,
+	per_div, per, csi, fir, csi_div, usb_div_pre, usb_div_post, fir_div_pre,
+	fir_div_post, sdhc1_gate, sdhc2_gate, gpt_gate, epit1_gate, epit2_gate,
+	iim_gate, ata_gate, sdma_gate, cspi3_gate, rng_gate, uart1_gate,
+	uart2_gate, ssi1_gate, i2c1_gate, i2c2_gate, i2c3_gate, hantro_gate,
+	mstick1_gate, mstick2_gate, csi_gate, rtc_gate, wdog_gate, pwm_gate,
+	sim_gate, ect_gate, usb_gate, kpp_gate, ipu_gate, uart3_gate,
+	uart4_gate, uart5_gate, owire_gate, ssi2_gate, cspi1_gate, cspi2_gate,
+	gacc_gate, emi_gate, rtic_gate, firi_gate, clk_max
+};
+
+static struct clk *clk[clk_max];
+static struct clk_onecell_data clk_data;
+
+int __init mx31_clocks_init(unsigned long fref)
+{
+	void __iomem *base;
+	struct device_node *np;
+
+	base = ioremap(MX31_CCM_BASE_ADDR, SZ_4K);
+	BUG_ON(!base);
+
+	clk[dummy] = imx_clk_fixed("dummy", 0);
+	clk[ckih] = imx_clk_fixed("ckih", fref);
+	clk[ckil] = imx_clk_fixed("ckil", 32768);
+	clk[mpll] = imx_clk_pllv1(IMX_PLLV1_IMX31, "mpll", "ckih", base + MXC_CCM_MPCTL);
+	clk[spll] = imx_clk_pllv1(IMX_PLLV1_IMX31, "spll", "ckih", base + MXC_CCM_SRPCTL);
+	clk[upll] = imx_clk_pllv1(IMX_PLLV1_IMX31, "upll", "ckih", base + MXC_CCM_UPCTL);
+	clk[mcu_main] = imx_clk_mux("mcu_main", base + MXC_CCM_PMCR0, 31, 1, mcu_main_sel, ARRAY_SIZE(mcu_main_sel));
+	clk[hsp] = imx_clk_divider("hsp", "mcu_main", base + MXC_CCM_PDR0, 11, 3);
+	clk[ahb] = imx_clk_divider("ahb", "mcu_main", base + MXC_CCM_PDR0, 3, 3);
+	clk[nfc] = imx_clk_divider("nfc", "ahb", base + MXC_CCM_PDR0, 8, 3);
+	clk[ipg] = imx_clk_divider("ipg", "ahb", base + MXC_CCM_PDR0, 6, 2);
+	clk[per_div] = imx_clk_divider("per_div", "upll", base + MXC_CCM_PDR0, 16, 5);
+	clk[per] = imx_clk_mux("per", base + MXC_CCM_CCMR, 24, 1, per_sel, ARRAY_SIZE(per_sel));
+	clk[csi] = imx_clk_mux("csi_sel", base + MXC_CCM_CCMR, 25, 1, csi_sel, ARRAY_SIZE(csi_sel));
+	clk[fir] = imx_clk_mux("fir_sel", base + MXC_CCM_CCMR, 11, 2, fir_sel, ARRAY_SIZE(fir_sel));
+	clk[csi_div] = imx_clk_divider("csi_div", "csi_sel", base + MXC_CCM_PDR0, 23, 9);
+	clk[usb_div_pre] = imx_clk_divider("usb_div_pre", "upll", base + MXC_CCM_PDR1, 30, 2);
+	clk[usb_div_post] = imx_clk_divider("usb_div_post", "usb_div_pre", base + MXC_CCM_PDR1, 27, 3);
+	clk[fir_div_pre] = imx_clk_divider("fir_div_pre", "fir_sel", base + MXC_CCM_PDR1, 24, 3);
+	clk[fir_div_post] = imx_clk_divider("fir_div_post", "fir_div_pre", base + MXC_CCM_PDR1, 23, 6);
+	clk[sdhc1_gate] = imx_clk_gate2("sdhc1_gate", "per", base + MXC_CCM_CGR0, 0);
+	clk[sdhc2_gate] = imx_clk_gate2("sdhc2_gate", "per", base + MXC_CCM_CGR0, 2);
+	clk[gpt_gate] = imx_clk_gate2("gpt_gate", "per", base + MXC_CCM_CGR0, 4);
+	clk[epit1_gate] = imx_clk_gate2("epit1_gate", "per", base + MXC_CCM_CGR0, 6);
+	clk[epit2_gate] = imx_clk_gate2("epit2_gate", "per", base + MXC_CCM_CGR0, 8);
+	clk[iim_gate] = imx_clk_gate2("iim_gate", "ipg", base + MXC_CCM_CGR0, 10);
+	clk[ata_gate] = imx_clk_gate2("ata_gate", "ipg", base + MXC_CCM_CGR0, 12);
+	clk[sdma_gate] = imx_clk_gate2("sdma_gate", "ahb", base + MXC_CCM_CGR0, 14);
+	clk[cspi3_gate] = imx_clk_gate2("cspi3_gate", "ipg", base + MXC_CCM_CGR0, 16);
+	clk[rng_gate] = imx_clk_gate2("rng_gate", "ipg", base + MXC_CCM_CGR0, 18);
+	clk[uart1_gate] = imx_clk_gate2("uart1_gate", "per", base + MXC_CCM_CGR0, 20);
+	clk[uart2_gate] = imx_clk_gate2("uart2_gate", "per", base + MXC_CCM_CGR0, 22);
+	clk[ssi1_gate] = imx_clk_gate2("ssi1_gate", "spll", base + MXC_CCM_CGR0, 24);
+	clk[i2c1_gate] = imx_clk_gate2("i2c1_gate", "per", base + MXC_CCM_CGR0, 26);
+	clk[i2c2_gate] = imx_clk_gate2("i2c2_gate", "per", base + MXC_CCM_CGR0, 28);
+	clk[i2c3_gate] = imx_clk_gate2("i2c3_gate", "per", base + MXC_CCM_CGR0, 30);
+	clk[hantro_gate] = imx_clk_gate2("hantro_gate", "per", base + MXC_CCM_CGR1, 0);
+	clk[mstick1_gate] = imx_clk_gate2("mstick1_gate", "per", base + MXC_CCM_CGR1, 2);
+	clk[mstick2_gate] = imx_clk_gate2("mstick2_gate", "per", base + MXC_CCM_CGR1, 4);
+	clk[csi_gate] = imx_clk_gate2("csi_gate", "csi_div", base + MXC_CCM_CGR1, 6);
+	clk[rtc_gate] = imx_clk_gate2("rtc_gate", "ipg", base + MXC_CCM_CGR1, 8);
+	clk[wdog_gate] = imx_clk_gate2("wdog_gate", "ipg", base + MXC_CCM_CGR1, 10);
+	clk[pwm_gate] = imx_clk_gate2("pwm_gate", "per", base + MXC_CCM_CGR1, 12);
+	clk[sim_gate] = imx_clk_gate2("sim_gate", "per", base + MXC_CCM_CGR1, 14);
+	clk[ect_gate] = imx_clk_gate2("ect_gate", "per", base + MXC_CCM_CGR1, 16);
+	clk[usb_gate] = imx_clk_gate2("usb_gate", "ahb", base + MXC_CCM_CGR1, 18);
+	clk[kpp_gate] = imx_clk_gate2("kpp_gate", "ipg", base + MXC_CCM_CGR1, 20);
+	clk[ipu_gate] = imx_clk_gate2("ipu_gate", "hsp", base + MXC_CCM_CGR1, 22);
+	clk[uart3_gate] = imx_clk_gate2("uart3_gate", "per", base + MXC_CCM_CGR1, 24);
+	clk[uart4_gate] = imx_clk_gate2("uart4_gate", "per", base + MXC_CCM_CGR1, 26);
+	clk[uart5_gate] = imx_clk_gate2("uart5_gate", "per", base + MXC_CCM_CGR1, 28);
+	clk[owire_gate] = imx_clk_gate2("owire_gate", "per", base + MXC_CCM_CGR1, 30);
+	clk[ssi2_gate] = imx_clk_gate2("ssi2_gate", "spll", base + MXC_CCM_CGR2, 0);
+	clk[cspi1_gate] = imx_clk_gate2("cspi1_gate", "ipg", base + MXC_CCM_CGR2, 2);
+	clk[cspi2_gate] = imx_clk_gate2("cspi2_gate", "ipg", base + MXC_CCM_CGR2, 4);
+	clk[gacc_gate] = imx_clk_gate2("gacc_gate", "per", base + MXC_CCM_CGR2, 6);
+	clk[emi_gate] = imx_clk_gate2("emi_gate", "ahb", base + MXC_CCM_CGR2, 8);
+	clk[rtic_gate] = imx_clk_gate2("rtic_gate", "ahb", base + MXC_CCM_CGR2, 10);
+	clk[firi_gate] = imx_clk_gate2("firi_gate", "upll", base+MXC_CCM_CGR2, 12);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx31-ccm");
+
+	if (np) {
+		clk_data.clks = clk;
+		clk_data.clk_num = ARRAY_SIZE(clk);
+		of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+	}
+
+	clk_register_clkdev(clk[gpt_gate], "per", "imx-gpt.0");
+	clk_register_clkdev(clk[ipg], "ipg", "imx-gpt.0");
+	clk_register_clkdev(clk[cspi1_gate], NULL, "imx31-cspi.0");
+	clk_register_clkdev(clk[cspi2_gate], NULL, "imx31-cspi.1");
+	clk_register_clkdev(clk[cspi3_gate], NULL, "imx31-cspi.2");
+	clk_register_clkdev(clk[pwm_gate], "pwm", NULL);
+	clk_register_clkdev(clk[wdog_gate], NULL, "imx2-wdt.0");
+	clk_register_clkdev(clk[rtc_gate], NULL, "imx21-rtc");
+	clk_register_clkdev(clk[epit1_gate], "epit", NULL);
+	clk_register_clkdev(clk[epit2_gate], "epit", NULL);
+	clk_register_clkdev(clk[nfc], NULL, "imx27-nand.0");
+	clk_register_clkdev(clk[ipu_gate], NULL, "ipu-core");
+	clk_register_clkdev(clk[ipu_gate], NULL, "mx3_sdc_fb");
+	clk_register_clkdev(clk[kpp_gate], NULL, "imx-keypad");
+	clk_register_clkdev(clk[usb_div_post], "per", "mxc-ehci.0");
+	clk_register_clkdev(clk[usb_gate], "ahb", "mxc-ehci.0");
+	clk_register_clkdev(clk[ipg], "ipg", "mxc-ehci.0");
+	clk_register_clkdev(clk[usb_div_post], "per", "mxc-ehci.1");
+	clk_register_clkdev(clk[usb_gate], "ahb", "mxc-ehci.1");
+	clk_register_clkdev(clk[ipg], "ipg", "mxc-ehci.1");
+	clk_register_clkdev(clk[usb_div_post], "per", "mxc-ehci.2");
+	clk_register_clkdev(clk[usb_gate], "ahb", "mxc-ehci.2");
+	clk_register_clkdev(clk[ipg], "ipg", "mxc-ehci.2");
+	clk_register_clkdev(clk[usb_div_post], "per", "imx-udc-mx27");
+	clk_register_clkdev(clk[usb_gate], "ahb", "imx-udc-mx27");
+	clk_register_clkdev(clk[ipg], "ipg", "imx-udc-mx27");
+	clk_register_clkdev(clk[csi_gate], NULL, "mx3-camera.0");
+	/* i.mx31 has the i.mx21 type uart */
+	clk_register_clkdev(clk[uart1_gate], "per", "imx21-uart.0");
+	clk_register_clkdev(clk[ipg], "ipg", "imx21-uart.0");
+	clk_register_clkdev(clk[uart2_gate], "per", "imx21-uart.1");
+	clk_register_clkdev(clk[ipg], "ipg", "imx21-uart.1");
+	clk_register_clkdev(clk[uart3_gate], "per", "imx21-uart.2");
+	clk_register_clkdev(clk[ipg], "ipg", "imx21-uart.2");
+	clk_register_clkdev(clk[uart4_gate], "per", "imx21-uart.3");
+	clk_register_clkdev(clk[ipg], "ipg", "imx21-uart.3");
+	clk_register_clkdev(clk[uart5_gate], "per", "imx21-uart.4");
+	clk_register_clkdev(clk[ipg], "ipg", "imx21-uart.4");
+	clk_register_clkdev(clk[i2c1_gate], NULL, "imx21-i2c.0");
+	clk_register_clkdev(clk[i2c2_gate], NULL, "imx21-i2c.1");
+	clk_register_clkdev(clk[i2c3_gate], NULL, "imx21-i2c.2");
+	clk_register_clkdev(clk[owire_gate], NULL, "mxc_w1.0");
+	clk_register_clkdev(clk[sdhc1_gate], NULL, "imx31-mmc.0");
+	clk_register_clkdev(clk[sdhc2_gate], NULL, "imx31-mmc.1");
+	clk_register_clkdev(clk[ssi1_gate], NULL, "imx-ssi.0");
+	clk_register_clkdev(clk[ssi2_gate], NULL, "imx-ssi.1");
+	clk_register_clkdev(clk[firi_gate], "firi", NULL);
+	clk_register_clkdev(clk[ata_gate], NULL, "pata_imx");
+	clk_register_clkdev(clk[rtic_gate], "rtic", NULL);
+	clk_register_clkdev(clk[rng_gate], NULL, "mxc_rnga");
+	clk_register_clkdev(clk[sdma_gate], NULL, "imx31-sdma");
+	clk_register_clkdev(clk[iim_gate], "iim", NULL);
+
+	clk_set_parent(clk[csi], clk[upll]);
+	clk_prepare_enable(clk[emi_gate]);
+	clk_prepare_enable(clk[iim_gate]);
+	mx31_revision();
+	clk_disable_unprepare(clk[iim_gate]);
+
+	mxc_timer_init(MX31_GPT1_BASE_ADDR, MX31_INT_GPT, GPT_TYPE_IMX31);
+
+	return 0;
+}
+
+int __init mx31_clocks_init_dt(void)
+{
+	struct device_node *np;
+	u32 fref = 26000000; /* default */
+
+	for_each_compatible_node(np, NULL, "fixed-clock") {
+		if (!of_device_is_compatible(np, "fsl,imx-osc26m"))
+			continue;
+
+		if (!of_property_read_u32(np, "clock-frequency", &fref))
+			break;
+	}
+
+	return mx31_clocks_init(fref);
+}
diff --git a/drivers/clk/imx/clk-imx35.c b/drivers/clk/imx/clk-imx35.c
new file mode 100644
index 000000000000..69138ba3dec7
--- /dev/null
+++ b/drivers/clk/imx/clk-imx35.c
@@ -0,0 +1,310 @@
+/*
+ * Copyright (C) 2012 Sascha Hauer, Pengutronix <s.hauer@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+#include <linux/mm.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/err.h>
+#include <soc/imx/revision.h>
+#include <soc/imx/timer.h>
+#include <asm/irq.h>
+
+#include "clk.h"
+
+#define MX35_CCM_BASE_ADDR	0x53f80000
+#define MX35_GPT1_BASE_ADDR	0x53f90000
+#define MX35_INT_GPT		(NR_IRQS_LEGACY + 29)
+
+#define MXC_CCM_PDR0		0x04
+#define MX35_CCM_PDR2		0x0c
+#define MX35_CCM_PDR3		0x10
+#define MX35_CCM_PDR4		0x14
+#define MX35_CCM_MPCTL		0x1c
+#define MX35_CCM_PPCTL		0x20
+#define MX35_CCM_CGR0		0x2c
+#define MX35_CCM_CGR1		0x30
+#define MX35_CCM_CGR2		0x34
+#define MX35_CCM_CGR3		0x38
+
+struct arm_ahb_div {
+	unsigned char arm, ahb, sel;
+};
+
+static struct arm_ahb_div clk_consumer[] = {
+	{ .arm = 1, .ahb = 4, .sel = 0},
+	{ .arm = 1, .ahb = 3, .sel = 1},
+	{ .arm = 2, .ahb = 2, .sel = 0},
+	{ .arm = 0, .ahb = 0, .sel = 0},
+	{ .arm = 0, .ahb = 0, .sel = 0},
+	{ .arm = 0, .ahb = 0, .sel = 0},
+	{ .arm = 4, .ahb = 1, .sel = 0},
+	{ .arm = 1, .ahb = 5, .sel = 0},
+	{ .arm = 1, .ahb = 8, .sel = 0},
+	{ .arm = 1, .ahb = 6, .sel = 1},
+	{ .arm = 2, .ahb = 4, .sel = 0},
+	{ .arm = 0, .ahb = 0, .sel = 0},
+	{ .arm = 0, .ahb = 0, .sel = 0},
+	{ .arm = 0, .ahb = 0, .sel = 0},
+	{ .arm = 4, .ahb = 2, .sel = 0},
+	{ .arm = 0, .ahb = 0, .sel = 0},
+};
+
+static char hsp_div_532[] = { 4, 8, 3, 0 };
+static char hsp_div_400[] = { 3, 6, 3, 0 };
+
+static struct clk_onecell_data clk_data;
+
+static const char *std_sel[] = {"ppll", "arm"};
+static const char *ipg_per_sel[] = {"ahb_per_div", "arm_per_div"};
+
+enum mx35_clks {
+	ckih, mpll, ppll, mpll_075, arm, hsp, hsp_div, hsp_sel, ahb, ipg,
+	arm_per_div, ahb_per_div, ipg_per, uart_sel, uart_div, esdhc_sel,
+	esdhc1_div, esdhc2_div, esdhc3_div, spdif_sel, spdif_div_pre,
+	spdif_div_post, ssi_sel, ssi1_div_pre, ssi1_div_post, ssi2_div_pre,
+	ssi2_div_post, usb_sel, usb_div, nfc_div, asrc_gate, pata_gate,
+	audmux_gate, can1_gate, can2_gate, cspi1_gate, cspi2_gate, ect_gate,
+	edio_gate, emi_gate, epit1_gate, epit2_gate, esai_gate, esdhc1_gate,
+	esdhc2_gate, esdhc3_gate, fec_gate, gpio1_gate, gpio2_gate, gpio3_gate,
+	gpt_gate, i2c1_gate, i2c2_gate, i2c3_gate, iomuxc_gate, ipu_gate,
+	kpp_gate, mlb_gate, mshc_gate, owire_gate, pwm_gate, rngc_gate,
+	rtc_gate, rtic_gate, scc_gate, sdma_gate, spba_gate, spdif_gate,
+	ssi1_gate, ssi2_gate, uart1_gate, uart2_gate, uart3_gate, usbotg_gate,
+	wdog_gate, max_gate, admux_gate, csi_gate, csi_div, csi_sel, iim_gate,
+	gpu2d_gate, clk_max
+};
+
+static struct clk *clk[clk_max];
+
+int __init mx35_clocks_init(void)
+{
+	void __iomem *base;
+	u32 pdr0, consumer_sel, hsp_sel;
+	struct arm_ahb_div *aad;
+	unsigned char *hsp_div;
+
+	base = ioremap(MX35_CCM_BASE_ADDR, SZ_4K);
+	BUG_ON(!base);
+
+	pdr0 = __raw_readl(base + MXC_CCM_PDR0);
+	consumer_sel = (pdr0 >> 16) & 0xf;
+	aad = &clk_consumer[consumer_sel];
+	if (!aad->arm) {
+		pr_err("i.MX35 clk: illegal consumer mux selection 0x%x\n", consumer_sel);
+		/*
+		 * We are basically stuck. Continue with a default entry and hope we
+		 * get far enough to actually show the above message
+		 */
+		aad = &clk_consumer[0];
+	}
+
+	clk[ckih] = imx_clk_fixed("ckih", 24000000);
+	clk[mpll] = imx_clk_pllv1(IMX_PLLV1_IMX35, "mpll", "ckih", base + MX35_CCM_MPCTL);
+	clk[ppll] = imx_clk_pllv1(IMX_PLLV1_IMX35, "ppll", "ckih", base + MX35_CCM_PPCTL);
+
+	clk[mpll] = imx_clk_fixed_factor("mpll_075", "mpll", 3, 4);
+
+	if (aad->sel)
+		clk[arm] = imx_clk_fixed_factor("arm", "mpll_075", 1, aad->arm);
+	else
+		clk[arm] = imx_clk_fixed_factor("arm", "mpll", 1, aad->arm);
+
+	if (clk_get_rate(clk[arm]) > 400000000)
+		hsp_div = hsp_div_532;
+	else
+		hsp_div = hsp_div_400;
+
+	hsp_sel = (pdr0 >> 20) & 0x3;
+	if (!hsp_div[hsp_sel]) {
+		pr_err("i.MX35 clk: illegal hsp clk selection 0x%x\n", hsp_sel);
+		hsp_sel = 0;
+	}
+
+	clk[hsp] = imx_clk_fixed_factor("hsp", "arm", 1, hsp_div[hsp_sel]);
+
+	clk[ahb] = imx_clk_fixed_factor("ahb", "arm", 1, aad->ahb);
+	clk[ipg] = imx_clk_fixed_factor("ipg", "ahb", 1, 2);
+
+	clk[arm_per_div] = imx_clk_divider("arm_per_div", "arm", base + MX35_CCM_PDR4, 16, 6);
+	clk[ahb_per_div] = imx_clk_divider("ahb_per_div", "ahb", base + MXC_CCM_PDR0, 12, 3);
+	clk[ipg_per] = imx_clk_mux("ipg_per", base + MXC_CCM_PDR0, 26, 1, ipg_per_sel, ARRAY_SIZE(ipg_per_sel));
+
+	clk[uart_sel] = imx_clk_mux("uart_sel", base + MX35_CCM_PDR3, 14, 1, std_sel, ARRAY_SIZE(std_sel));
+	clk[uart_div] = imx_clk_divider("uart_div", "uart_sel", base + MX35_CCM_PDR4, 10, 6);
+
+	clk[esdhc_sel] = imx_clk_mux("esdhc_sel", base + MX35_CCM_PDR4, 9, 1, std_sel, ARRAY_SIZE(std_sel));
+	clk[esdhc1_div] = imx_clk_divider("esdhc1_div", "esdhc_sel", base + MX35_CCM_PDR3, 0, 6);
+	clk[esdhc2_div] = imx_clk_divider("esdhc2_div", "esdhc_sel", base + MX35_CCM_PDR3, 8, 6);
+	clk[esdhc3_div] = imx_clk_divider("esdhc3_div", "esdhc_sel", base + MX35_CCM_PDR3, 16, 6);
+
+	clk[spdif_sel] = imx_clk_mux("spdif_sel", base + MX35_CCM_PDR3, 22, 1, std_sel, ARRAY_SIZE(std_sel));
+	clk[spdif_div_pre] = imx_clk_divider("spdif_div_pre", "spdif_sel", base + MX35_CCM_PDR3, 29, 3); /* divide by 1 not allowed */ 
+	clk[spdif_div_post] = imx_clk_divider("spdif_div_post", "spdif_div_pre", base + MX35_CCM_PDR3, 23, 6);
+
+	clk[ssi_sel] = imx_clk_mux("ssi_sel", base + MX35_CCM_PDR2, 6, 1, std_sel, ARRAY_SIZE(std_sel));
+	clk[ssi1_div_pre] = imx_clk_divider("ssi1_div_pre", "ssi_sel", base + MX35_CCM_PDR2, 24, 3);
+	clk[ssi1_div_post] = imx_clk_divider("ssi1_div_post", "ssi1_div_pre", base + MX35_CCM_PDR2, 0, 6);
+	clk[ssi2_div_pre] = imx_clk_divider("ssi2_div_pre", "ssi_sel", base + MX35_CCM_PDR2, 27, 3);
+	clk[ssi2_div_post] = imx_clk_divider("ssi2_div_post", "ssi2_div_pre", base + MX35_CCM_PDR2, 8, 6);
+
+	clk[usb_sel] = imx_clk_mux("usb_sel", base + MX35_CCM_PDR4, 9, 1, std_sel, ARRAY_SIZE(std_sel));
+	clk[usb_div] = imx_clk_divider("usb_div", "usb_sel", base + MX35_CCM_PDR4, 22, 6);
+
+	clk[nfc_div] = imx_clk_divider("nfc_div", "ahb", base + MX35_CCM_PDR4, 28, 4);
+
+	clk[csi_sel] = imx_clk_mux("csi_sel", base + MX35_CCM_PDR2, 7, 1, std_sel, ARRAY_SIZE(std_sel));
+	clk[csi_div] = imx_clk_divider("csi_div", "csi_sel", base + MX35_CCM_PDR2, 16, 6);
+
+	clk[asrc_gate] = imx_clk_gate2("asrc_gate", "ipg", base + MX35_CCM_CGR0,  0);
+	clk[pata_gate] = imx_clk_gate2("pata_gate", "ipg", base + MX35_CCM_CGR0,  2);
+	clk[audmux_gate] = imx_clk_gate2("audmux_gate", "ipg", base + MX35_CCM_CGR0,  4);
+	clk[can1_gate] = imx_clk_gate2("can1_gate", "ipg", base + MX35_CCM_CGR0,  6);
+	clk[can2_gate] = imx_clk_gate2("can2_gate", "ipg", base + MX35_CCM_CGR0,  8);
+	clk[cspi1_gate] = imx_clk_gate2("cspi1_gate", "ipg", base + MX35_CCM_CGR0, 10);
+	clk[cspi2_gate] = imx_clk_gate2("cspi2_gate", "ipg", base + MX35_CCM_CGR0, 12);
+	clk[ect_gate] = imx_clk_gate2("ect_gate", "ipg", base + MX35_CCM_CGR0, 14);
+	clk[edio_gate] = imx_clk_gate2("edio_gate",   "ipg", base + MX35_CCM_CGR0, 16);
+	clk[emi_gate] = imx_clk_gate2("emi_gate", "ipg", base + MX35_CCM_CGR0, 18);
+	clk[epit1_gate] = imx_clk_gate2("epit1_gate", "ipg", base + MX35_CCM_CGR0, 20);
+	clk[epit2_gate] = imx_clk_gate2("epit2_gate", "ipg", base + MX35_CCM_CGR0, 22);
+	clk[esai_gate] = imx_clk_gate2("esai_gate",   "ipg", base + MX35_CCM_CGR0, 24);
+	clk[esdhc1_gate] = imx_clk_gate2("esdhc1_gate", "esdhc1_div", base + MX35_CCM_CGR0, 26);
+	clk[esdhc2_gate] = imx_clk_gate2("esdhc2_gate", "esdhc2_div", base + MX35_CCM_CGR0, 28);
+	clk[esdhc3_gate] = imx_clk_gate2("esdhc3_gate", "esdhc3_div", base + MX35_CCM_CGR0, 30);
+
+	clk[fec_gate] = imx_clk_gate2("fec_gate", "ipg", base + MX35_CCM_CGR1,  0);
+	clk[gpio1_gate] = imx_clk_gate2("gpio1_gate", "ipg", base + MX35_CCM_CGR1,  2);
+	clk[gpio2_gate] = imx_clk_gate2("gpio2_gate", "ipg", base + MX35_CCM_CGR1,  4);
+	clk[gpio3_gate] = imx_clk_gate2("gpio3_gate", "ipg", base + MX35_CCM_CGR1,  6);
+	clk[gpt_gate] = imx_clk_gate2("gpt_gate", "ipg", base + MX35_CCM_CGR1,  8);
+	clk[i2c1_gate] = imx_clk_gate2("i2c1_gate", "ipg_per", base + MX35_CCM_CGR1, 10);
+	clk[i2c2_gate] = imx_clk_gate2("i2c2_gate", "ipg_per", base + MX35_CCM_CGR1, 12);
+	clk[i2c3_gate] = imx_clk_gate2("i2c3_gate", "ipg_per", base + MX35_CCM_CGR1, 14);
+	clk[iomuxc_gate] = imx_clk_gate2("iomuxc_gate", "ipg", base + MX35_CCM_CGR1, 16);
+	clk[ipu_gate] = imx_clk_gate2("ipu_gate", "hsp", base + MX35_CCM_CGR1, 18);
+	clk[kpp_gate] = imx_clk_gate2("kpp_gate", "ipg", base + MX35_CCM_CGR1, 20);
+	clk[mlb_gate] = imx_clk_gate2("mlb_gate", "ahb", base + MX35_CCM_CGR1, 22);
+	clk[mshc_gate] = imx_clk_gate2("mshc_gate", "dummy", base + MX35_CCM_CGR1, 24);
+	clk[owire_gate] = imx_clk_gate2("owire_gate", "ipg_per", base + MX35_CCM_CGR1, 26);
+	clk[pwm_gate] = imx_clk_gate2("pwm_gate", "ipg_per", base + MX35_CCM_CGR1, 28);
+	clk[rngc_gate] = imx_clk_gate2("rngc_gate", "ipg", base + MX35_CCM_CGR1, 30);
+
+	clk[rtc_gate] = imx_clk_gate2("rtc_gate", "ipg", base + MX35_CCM_CGR2,  0);
+	clk[rtic_gate] = imx_clk_gate2("rtic_gate", "ahb", base + MX35_CCM_CGR2,  2);
+	clk[scc_gate] = imx_clk_gate2("scc_gate", "ipg", base + MX35_CCM_CGR2,  4);
+	clk[sdma_gate] = imx_clk_gate2("sdma_gate", "ahb", base + MX35_CCM_CGR2,  6);
+	clk[spba_gate] = imx_clk_gate2("spba_gate", "ipg", base + MX35_CCM_CGR2,  8);
+	clk[spdif_gate] = imx_clk_gate2("spdif_gate", "spdif_div_post", base + MX35_CCM_CGR2, 10);
+	clk[ssi1_gate] = imx_clk_gate2("ssi1_gate", "ssi1_div_post", base + MX35_CCM_CGR2, 12);
+	clk[ssi2_gate] = imx_clk_gate2("ssi2_gate", "ssi2_div_post", base + MX35_CCM_CGR2, 14);
+	clk[uart1_gate] = imx_clk_gate2("uart1_gate", "uart_div", base + MX35_CCM_CGR2, 16);
+	clk[uart2_gate] = imx_clk_gate2("uart2_gate", "uart_div", base + MX35_CCM_CGR2, 18);
+	clk[uart3_gate] = imx_clk_gate2("uart3_gate", "uart_div", base + MX35_CCM_CGR2, 20);
+	clk[usbotg_gate] = imx_clk_gate2("usbotg_gate", "ahb", base + MX35_CCM_CGR2, 22);
+	clk[wdog_gate] = imx_clk_gate2("wdog_gate", "ipg", base + MX35_CCM_CGR2, 24);
+	clk[max_gate] = imx_clk_gate2("max_gate", "dummy", base + MX35_CCM_CGR2, 26);
+	clk[admux_gate] = imx_clk_gate2("admux_gate", "ipg", base + MX35_CCM_CGR2, 30);
+
+	clk[csi_gate] = imx_clk_gate2("csi_gate", "csi_div", base + MX35_CCM_CGR3,  0);
+	clk[iim_gate] = imx_clk_gate2("iim_gate", "ipg", base + MX35_CCM_CGR3,  2);
+	clk[gpu2d_gate] = imx_clk_gate2("gpu2d_gate", "ahb", base + MX35_CCM_CGR3,  4);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	clk_register_clkdev(clk[pata_gate], NULL, "pata_imx");
+	clk_register_clkdev(clk[can1_gate], NULL, "flexcan.0");
+	clk_register_clkdev(clk[can2_gate], NULL, "flexcan.1");
+	clk_register_clkdev(clk[cspi1_gate], "per", "imx35-cspi.0");
+	clk_register_clkdev(clk[cspi1_gate], "ipg", "imx35-cspi.0");
+	clk_register_clkdev(clk[cspi2_gate], "per", "imx35-cspi.1");
+	clk_register_clkdev(clk[cspi2_gate], "ipg", "imx35-cspi.1");
+	clk_register_clkdev(clk[epit1_gate], NULL, "imx-epit.0");
+	clk_register_clkdev(clk[epit2_gate], NULL, "imx-epit.1");
+	clk_register_clkdev(clk[esdhc1_gate], "per", "sdhci-esdhc-imx35.0");
+	clk_register_clkdev(clk[ipg], "ipg", "sdhci-esdhc-imx35.0");
+	clk_register_clkdev(clk[ahb], "ahb", "sdhci-esdhc-imx35.0");
+	clk_register_clkdev(clk[esdhc2_gate], "per", "sdhci-esdhc-imx35.1");
+	clk_register_clkdev(clk[ipg], "ipg", "sdhci-esdhc-imx35.1");
+	clk_register_clkdev(clk[ahb], "ahb", "sdhci-esdhc-imx35.1");
+	clk_register_clkdev(clk[esdhc3_gate], "per", "sdhci-esdhc-imx35.2");
+	clk_register_clkdev(clk[ipg], "ipg", "sdhci-esdhc-imx35.2");
+	clk_register_clkdev(clk[ahb], "ahb", "sdhci-esdhc-imx35.2");
+	/* i.mx35 has the i.mx27 type fec */
+	clk_register_clkdev(clk[fec_gate], NULL, "imx27-fec.0");
+	clk_register_clkdev(clk[gpt_gate], "per", "imx-gpt.0");
+	clk_register_clkdev(clk[ipg], "ipg", "imx-gpt.0");
+	clk_register_clkdev(clk[i2c1_gate], NULL, "imx21-i2c.0");
+	clk_register_clkdev(clk[i2c2_gate], NULL, "imx21-i2c.1");
+	clk_register_clkdev(clk[i2c3_gate], NULL, "imx21-i2c.2");
+	clk_register_clkdev(clk[ipu_gate], NULL, "ipu-core");
+	clk_register_clkdev(clk[ipu_gate], NULL, "mx3_sdc_fb");
+	clk_register_clkdev(clk[kpp_gate], NULL, "imx-keypad");
+	clk_register_clkdev(clk[owire_gate], NULL, "mxc_w1");
+	clk_register_clkdev(clk[sdma_gate], NULL, "imx35-sdma");
+	clk_register_clkdev(clk[ssi1_gate], NULL, "imx-ssi.0");
+	clk_register_clkdev(clk[ssi2_gate], NULL, "imx-ssi.1");
+	/* i.mx35 has the i.mx21 type uart */
+	clk_register_clkdev(clk[uart1_gate], "per", "imx21-uart.0");
+	clk_register_clkdev(clk[ipg], "ipg", "imx21-uart.0");
+	clk_register_clkdev(clk[uart2_gate], "per", "imx21-uart.1");
+	clk_register_clkdev(clk[ipg], "ipg", "imx21-uart.1");
+	clk_register_clkdev(clk[uart3_gate], "per", "imx21-uart.2");
+	clk_register_clkdev(clk[ipg], "ipg", "imx21-uart.2");
+	clk_register_clkdev(clk[usb_div], "per", "mxc-ehci.0");
+	clk_register_clkdev(clk[ipg], "ipg", "mxc-ehci.0");
+	clk_register_clkdev(clk[usbotg_gate], "ahb", "mxc-ehci.0");
+	clk_register_clkdev(clk[usb_div], "per", "mxc-ehci.1");
+	clk_register_clkdev(clk[ipg], "ipg", "mxc-ehci.1");
+	clk_register_clkdev(clk[usbotg_gate], "ahb", "mxc-ehci.1");
+	clk_register_clkdev(clk[usb_div], "per", "mxc-ehci.2");
+	clk_register_clkdev(clk[ipg], "ipg", "mxc-ehci.2");
+	clk_register_clkdev(clk[usbotg_gate], "ahb", "mxc-ehci.2");
+	clk_register_clkdev(clk[usb_div], "per", "imx-udc-mx27");
+	clk_register_clkdev(clk[ipg], "ipg", "imx-udc-mx27");
+	clk_register_clkdev(clk[usbotg_gate], "ahb", "imx-udc-mx27");
+	clk_register_clkdev(clk[wdog_gate], NULL, "imx2-wdt.0");
+	clk_register_clkdev(clk[nfc_div], NULL, "imx25-nand.0");
+	clk_register_clkdev(clk[csi_gate], NULL, "mx3-camera.0");
+	clk_register_clkdev(clk[admux_gate], "audmux", NULL);
+
+	clk_prepare_enable(clk[spba_gate]);
+	clk_prepare_enable(clk[gpio1_gate]);
+	clk_prepare_enable(clk[gpio2_gate]);
+	clk_prepare_enable(clk[gpio3_gate]);
+	clk_prepare_enable(clk[iim_gate]);
+	clk_prepare_enable(clk[emi_gate]);
+	clk_prepare_enable(clk[max_gate]);
+	clk_prepare_enable(clk[iomuxc_gate]);
+
+	/*
+	 * SCC is needed to boot via mmc after a watchdog reset. The clock code
+	 * before conversion to common clk also enabled UART1 (which isn't
+	 * handled here and not needed for mmc) and IIM (which is enabled
+	 * unconditionally above).
+	 */
+	clk_prepare_enable(clk[scc_gate]);
+
+	imx_print_silicon_rev("i.MX35", mx35_revision());
+
+	mxc_timer_init(MX35_GPT1_BASE_ADDR, MX35_INT_GPT, GPT_TYPE_IMX31);
+
+	return 0;
+}
+
+static void __init mx35_clocks_init_dt(struct device_node *ccm_node)
+{
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(ccm_node, of_clk_src_onecell_get, &clk_data);
+
+	mx35_clocks_init();
+}
+CLK_OF_DECLARE(imx35, "fsl,imx35-ccm", mx35_clocks_init_dt);
diff --git a/drivers/clk/imx/clk-imx51-imx53.c b/drivers/clk/imx/clk-imx51-imx53.c
new file mode 100644
index 000000000000..a7e4f394be0d
--- /dev/null
+++ b/drivers/clk/imx/clk-imx51-imx53.c
@@ -0,0 +1,570 @@
+/*
+ * Copyright (C) 2011 Sascha Hauer, Pengutronix <s.hauer@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+#include <linux/mm.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/clkdev.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <soc/imx/revision.h>
+#include <dt-bindings/clock/imx5-clock.h>
+
+#include "clk.h"
+
+#define MX51_DPLL1_BASE		0x83f80000
+#define MX51_DPLL2_BASE		0x83f84000
+#define MX51_DPLL3_BASE		0x83f88000
+
+#define MX53_DPLL1_BASE		0x63f80000
+#define MX53_DPLL2_BASE		0x63f84000
+#define MX53_DPLL3_BASE		0x63f88000
+#define MX53_DPLL4_BASE		0x63f8c000
+
+#define MXC_CCM_CCR		(ccm_base + 0x00)
+#define MXC_CCM_CCDR		(ccm_base + 0x04)
+#define MXC_CCM_CSR		(ccm_base + 0x08)
+#define MXC_CCM_CCSR		(ccm_base + 0x0c)
+#define MXC_CCM_CACRR		(ccm_base + 0x10)
+#define MXC_CCM_CBCDR		(ccm_base + 0x14)
+#define MXC_CCM_CBCMR		(ccm_base + 0x18)
+#define MXC_CCM_CSCMR1		(ccm_base + 0x1c)
+#define MXC_CCM_CSCMR2		(ccm_base + 0x20)
+#define MXC_CCM_CSCDR1		(ccm_base + 0x24)
+#define MXC_CCM_CS1CDR		(ccm_base + 0x28)
+#define MXC_CCM_CS2CDR		(ccm_base + 0x2c)
+#define MXC_CCM_CDCDR		(ccm_base + 0x30)
+#define MXC_CCM_CHSCDR		(ccm_base + 0x34)
+#define MXC_CCM_CSCDR2		(ccm_base + 0x38)
+#define MXC_CCM_CSCDR3		(ccm_base + 0x3c)
+#define MXC_CCM_CSCDR4		(ccm_base + 0x40)
+#define MXC_CCM_CWDR		(ccm_base + 0x44)
+#define MXC_CCM_CDHIPR		(ccm_base + 0x48)
+#define MXC_CCM_CDCR		(ccm_base + 0x4c)
+#define MXC_CCM_CTOR		(ccm_base + 0x50)
+#define MXC_CCM_CLPCR		(ccm_base + 0x54)
+#define MXC_CCM_CISR		(ccm_base + 0x58)
+#define MXC_CCM_CIMR		(ccm_base + 0x5c)
+#define MXC_CCM_CCOSR		(ccm_base + 0x60)
+#define MXC_CCM_CGPR		(ccm_base + 0x64)
+#define MXC_CCM_CCGR0		(ccm_base + 0x68)
+#define MXC_CCM_CCGR1		(ccm_base + 0x6c)
+#define MXC_CCM_CCGR2		(ccm_base + 0x70)
+#define MXC_CCM_CCGR3		(ccm_base + 0x74)
+#define MXC_CCM_CCGR4		(ccm_base + 0x78)
+#define MXC_CCM_CCGR5		(ccm_base + 0x7c)
+#define MXC_CCM_CCGR6		(ccm_base + 0x80)
+#define MXC_CCM_CCGR7		(ccm_base + 0x84)
+
+/* Low-power Audio Playback Mode clock */
+static const char *lp_apm_sel[] = { "osc", };
+
+/* This is used multiple times */
+static const char *standard_pll_sel[] = { "pll1_sw", "pll2_sw", "pll3_sw", "lp_apm", };
+static const char *periph_apm_sel[] = { "pll1_sw", "pll3_sw", "lp_apm", };
+static const char *main_bus_sel[] = { "pll2_sw", "periph_apm", };
+static const char *per_lp_apm_sel[] = { "main_bus", "lp_apm", };
+static const char *per_root_sel[] = { "per_podf", "ipg", };
+static const char *esdhc_c_sel[] = { "esdhc_a_podf", "esdhc_b_podf", };
+static const char *esdhc_d_sel[] = { "esdhc_a_podf", "esdhc_b_podf", };
+static const char *ssi_apm_sels[] = { "ckih1", "lp_amp", "ckih2", };
+static const char *ssi_clk_sels[] = { "pll1_sw", "pll2_sw", "pll3_sw", "ssi_apm", };
+static const char *ssi3_clk_sels[] = { "ssi1_root_gate", "ssi2_root_gate", };
+static const char *ssi_ext1_com_sels[] = { "ssi_ext1_podf", "ssi1_root_gate", };
+static const char *ssi_ext2_com_sels[] = { "ssi_ext2_podf", "ssi2_root_gate", };
+static const char *emi_slow_sel[] = { "main_bus", "ahb", };
+static const char *usb_phy_sel_str[] = { "osc", "usb_phy_podf", };
+static const char *mx51_ipu_di0_sel[] = { "di_pred", "osc", "ckih1", "tve_di", };
+static const char *mx53_ipu_di0_sel[] = { "di_pred", "osc", "ckih1", "di_pll4_podf", "dummy", "ldb_di0_gate", };
+static const char *mx53_ldb_di0_sel[] = { "pll3_sw", "pll4_sw", };
+static const char *mx51_ipu_di1_sel[] = { "di_pred", "osc", "ckih1", "tve_di", "ipp_di1", };
+static const char *mx53_ipu_di1_sel[] = { "di_pred", "osc", "ckih1", "tve_di", "ipp_di1", "ldb_di1_gate", };
+static const char *mx53_ldb_di1_sel[] = { "pll3_sw", "pll4_sw", };
+static const char *mx51_tve_ext_sel[] = { "osc", "ckih1", };
+static const char *mx53_tve_ext_sel[] = { "pll4_sw", "ckih1", };
+static const char *mx51_tve_sel[] = { "tve_pred", "tve_ext_sel", };
+static const char *ipu_sel[] = { "axi_a", "axi_b", "emi_slow_gate", "ahb", };
+static const char *gpu3d_sel[] = { "axi_a", "axi_b", "emi_slow_gate", "ahb" };
+static const char *gpu2d_sel[] = { "axi_a", "axi_b", "emi_slow_gate", "ahb" };
+static const char *vpu_sel[] = { "axi_a", "axi_b", "emi_slow_gate", "ahb", };
+static const char *mx53_can_sel[] = { "ipg", "ckih1", "ckih2", "lp_apm", };
+static const char *mx53_cko1_sel[] = {
+	"cpu_podf", "pll1_sw", "pll2_sw", "pll3_sw",
+	"emi_slow_podf", "pll4_sw", "nfc_podf", "dummy",
+	"di_pred", "dummy", "dummy", "ahb",
+	"ipg", "per_root", "ckil", "dummy",};
+static const char *mx53_cko2_sel[] = {
+	"dummy"/* dptc_core */, "dummy"/* dptc_perich */,
+	"dummy", "esdhc_a_podf",
+	"usboh3_podf", "dummy"/* wrck_clk_root */,
+	"ecspi_podf", "dummy"/* pll1_ref_clk */,
+	"esdhc_b_podf", "dummy"/* ddr_clk_root */,
+	"dummy"/* arm_axi_clk_root */, "dummy"/* usb_phy_out */,
+	"vpu_sel", "ipu_sel",
+	"osc", "ckih1",
+	"dummy", "esdhc_c_sel",
+	"ssi1_root_podf", "ssi2_root_podf",
+	"dummy", "dummy",
+	"dummy"/* lpsr_clk_root */, "dummy"/* pgc_clk_root */,
+	"dummy"/* tve_out */, "usb_phy_sel",
+	"tve_sel", "lp_apm",
+	"uart_root", "dummy"/* spdif0_clk_root */,
+	"dummy", "dummy", };
+static const char *mx51_spdif_xtal_sel[] = { "osc", "ckih", "ckih2", };
+static const char *mx53_spdif_xtal_sel[] = { "osc", "ckih", "ckih2", "pll4_sw", };
+static const char *spdif_sel[] = { "pll1_sw", "pll2_sw", "pll3_sw", "spdif_xtal_sel", };
+static const char *spdif0_com_sel[] = { "spdif0_podf", "ssi1_root_gate", };
+static const char *mx51_spdif1_com_sel[] = { "spdif1_podf", "ssi2_root_gate", };
+static const char *step_sels[] = { "lp_apm", };
+static const char *cpu_podf_sels[] = { "pll1_sw", "step_sel" };
+
+static struct clk *clk[IMX5_CLK_END];
+static struct clk_onecell_data clk_data;
+
+static void __init mx5_clocks_common_init(void __iomem *ccm_base)
+{
+	clk[IMX5_CLK_DUMMY]		= imx_clk_fixed("dummy", 0);
+	clk[IMX5_CLK_CKIL]		= imx_obtain_fixed_clock("ckil", 0);
+	clk[IMX5_CLK_OSC]		= imx_obtain_fixed_clock("osc", 0);
+	clk[IMX5_CLK_CKIH1]		= imx_obtain_fixed_clock("ckih1", 0);
+	clk[IMX5_CLK_CKIH2]		= imx_obtain_fixed_clock("ckih2", 0);
+
+	clk[IMX5_CLK_PERIPH_APM]	= imx_clk_mux("periph_apm", MXC_CCM_CBCMR, 12, 2,
+						periph_apm_sel, ARRAY_SIZE(periph_apm_sel));
+	clk[IMX5_CLK_MAIN_BUS]		= imx_clk_mux("main_bus", MXC_CCM_CBCDR, 25, 1,
+						main_bus_sel, ARRAY_SIZE(main_bus_sel));
+	clk[IMX5_CLK_PER_LP_APM]	= imx_clk_mux("per_lp_apm", MXC_CCM_CBCMR, 1, 1,
+						per_lp_apm_sel, ARRAY_SIZE(per_lp_apm_sel));
+	clk[IMX5_CLK_PER_PRED1]		= imx_clk_divider("per_pred1", "per_lp_apm", MXC_CCM_CBCDR, 6, 2);
+	clk[IMX5_CLK_PER_PRED2]		= imx_clk_divider("per_pred2", "per_pred1", MXC_CCM_CBCDR, 3, 3);
+	clk[IMX5_CLK_PER_PODF]		= imx_clk_divider("per_podf", "per_pred2", MXC_CCM_CBCDR, 0, 3);
+	clk[IMX5_CLK_PER_ROOT]		= imx_clk_mux("per_root", MXC_CCM_CBCMR, 0, 1,
+						per_root_sel, ARRAY_SIZE(per_root_sel));
+	clk[IMX5_CLK_AHB]		= imx_clk_divider("ahb", "main_bus", MXC_CCM_CBCDR, 10, 3);
+	clk[IMX5_CLK_AHB_MAX]		= imx_clk_gate2("ahb_max", "ahb", MXC_CCM_CCGR0, 28);
+	clk[IMX5_CLK_AIPS_TZ1]		= imx_clk_gate2("aips_tz1", "ahb", MXC_CCM_CCGR0, 24);
+	clk[IMX5_CLK_AIPS_TZ2]		= imx_clk_gate2("aips_tz2", "ahb", MXC_CCM_CCGR0, 26);
+	clk[IMX5_CLK_TMAX1]		= imx_clk_gate2("tmax1", "ahb", MXC_CCM_CCGR1, 0);
+	clk[IMX5_CLK_TMAX2]		= imx_clk_gate2("tmax2", "ahb", MXC_CCM_CCGR1, 2);
+	clk[IMX5_CLK_TMAX3]		= imx_clk_gate2("tmax3", "ahb", MXC_CCM_CCGR1, 4);
+	clk[IMX5_CLK_SPBA]		= imx_clk_gate2("spba", "ipg", MXC_CCM_CCGR5, 0);
+	clk[IMX5_CLK_IPG]		= imx_clk_divider("ipg", "ahb", MXC_CCM_CBCDR, 8, 2);
+	clk[IMX5_CLK_AXI_A]		= imx_clk_divider("axi_a", "main_bus", MXC_CCM_CBCDR, 16, 3);
+	clk[IMX5_CLK_AXI_B]		= imx_clk_divider("axi_b", "main_bus", MXC_CCM_CBCDR, 19, 3);
+	clk[IMX5_CLK_UART_SEL]		= imx_clk_mux("uart_sel", MXC_CCM_CSCMR1, 24, 2,
+						standard_pll_sel, ARRAY_SIZE(standard_pll_sel));
+	clk[IMX5_CLK_UART_PRED]		= imx_clk_divider("uart_pred", "uart_sel", MXC_CCM_CSCDR1, 3, 3);
+	clk[IMX5_CLK_UART_ROOT]		= imx_clk_divider("uart_root", "uart_pred", MXC_CCM_CSCDR1, 0, 3);
+
+	clk[IMX5_CLK_ESDHC_A_SEL]	= imx_clk_mux("esdhc_a_sel", MXC_CCM_CSCMR1, 20, 2,
+						standard_pll_sel, ARRAY_SIZE(standard_pll_sel));
+	clk[IMX5_CLK_ESDHC_B_SEL]	= imx_clk_mux("esdhc_b_sel", MXC_CCM_CSCMR1, 16, 2,
+						standard_pll_sel, ARRAY_SIZE(standard_pll_sel));
+	clk[IMX5_CLK_ESDHC_A_PRED]	= imx_clk_divider("esdhc_a_pred", "esdhc_a_sel", MXC_CCM_CSCDR1, 16, 3);
+	clk[IMX5_CLK_ESDHC_A_PODF]	= imx_clk_divider("esdhc_a_podf", "esdhc_a_pred", MXC_CCM_CSCDR1, 11, 3);
+	clk[IMX5_CLK_ESDHC_B_PRED]	= imx_clk_divider("esdhc_b_pred", "esdhc_b_sel", MXC_CCM_CSCDR1, 22, 3);
+	clk[IMX5_CLK_ESDHC_B_PODF]	= imx_clk_divider("esdhc_b_podf", "esdhc_b_pred", MXC_CCM_CSCDR1, 19, 3);
+	clk[IMX5_CLK_ESDHC_C_SEL]	= imx_clk_mux("esdhc_c_sel", MXC_CCM_CSCMR1, 19, 1, esdhc_c_sel, ARRAY_SIZE(esdhc_c_sel));
+	clk[IMX5_CLK_ESDHC_D_SEL]	= imx_clk_mux("esdhc_d_sel", MXC_CCM_CSCMR1, 18, 1, esdhc_d_sel, ARRAY_SIZE(esdhc_d_sel));
+
+	clk[IMX5_CLK_EMI_SEL]		= imx_clk_mux("emi_sel", MXC_CCM_CBCDR, 26, 1,
+						emi_slow_sel, ARRAY_SIZE(emi_slow_sel));
+	clk[IMX5_CLK_EMI_SLOW_PODF]	= imx_clk_divider("emi_slow_podf", "emi_sel", MXC_CCM_CBCDR, 22, 3);
+	clk[IMX5_CLK_NFC_PODF]		= imx_clk_divider("nfc_podf", "emi_slow_podf", MXC_CCM_CBCDR, 13, 3);
+	clk[IMX5_CLK_ECSPI_SEL]		= imx_clk_mux("ecspi_sel", MXC_CCM_CSCMR1, 4, 2,
+						standard_pll_sel, ARRAY_SIZE(standard_pll_sel));
+	clk[IMX5_CLK_ECSPI_PRED]	= imx_clk_divider("ecspi_pred", "ecspi_sel", MXC_CCM_CSCDR2, 25, 3);
+	clk[IMX5_CLK_ECSPI_PODF]	= imx_clk_divider("ecspi_podf", "ecspi_pred", MXC_CCM_CSCDR2, 19, 6);
+	clk[IMX5_CLK_USBOH3_SEL]	= imx_clk_mux("usboh3_sel", MXC_CCM_CSCMR1, 22, 2,
+						standard_pll_sel, ARRAY_SIZE(standard_pll_sel));
+	clk[IMX5_CLK_USBOH3_PRED]	= imx_clk_divider("usboh3_pred", "usboh3_sel", MXC_CCM_CSCDR1, 8, 3);
+	clk[IMX5_CLK_USBOH3_PODF]	= imx_clk_divider("usboh3_podf", "usboh3_pred", MXC_CCM_CSCDR1, 6, 2);
+	clk[IMX5_CLK_USB_PHY_PRED]	= imx_clk_divider("usb_phy_pred", "pll3_sw", MXC_CCM_CDCDR, 3, 3);
+	clk[IMX5_CLK_USB_PHY_PODF]	= imx_clk_divider("usb_phy_podf", "usb_phy_pred", MXC_CCM_CDCDR, 0, 3);
+	clk[IMX5_CLK_USB_PHY_SEL]	= imx_clk_mux("usb_phy_sel", MXC_CCM_CSCMR1, 26, 1,
+						usb_phy_sel_str, ARRAY_SIZE(usb_phy_sel_str));
+	clk[IMX5_CLK_STEP_SEL]		= imx_clk_mux("step_sel", MXC_CCM_CCSR, 7, 2, step_sels, ARRAY_SIZE(step_sels));
+	clk[IMX5_CLK_CPU_PODF_SEL]	= imx_clk_mux("cpu_podf_sel", MXC_CCM_CCSR, 2, 1, cpu_podf_sels, ARRAY_SIZE(cpu_podf_sels));
+	clk[IMX5_CLK_CPU_PODF]		= imx_clk_divider("cpu_podf", "cpu_podf_sel", MXC_CCM_CACRR, 0, 3);
+	clk[IMX5_CLK_DI_PRED]		= imx_clk_divider("di_pred", "pll3_sw", MXC_CCM_CDCDR, 6, 3);
+	clk[IMX5_CLK_IIM_GATE]		= imx_clk_gate2("iim_gate", "ipg", MXC_CCM_CCGR0, 30);
+	clk[IMX5_CLK_UART1_IPG_GATE]	= imx_clk_gate2("uart1_ipg_gate", "ipg", MXC_CCM_CCGR1, 6);
+	clk[IMX5_CLK_UART1_PER_GATE]	= imx_clk_gate2("uart1_per_gate", "uart_root", MXC_CCM_CCGR1, 8);
+	clk[IMX5_CLK_UART2_IPG_GATE]	= imx_clk_gate2("uart2_ipg_gate", "ipg", MXC_CCM_CCGR1, 10);
+	clk[IMX5_CLK_UART2_PER_GATE]	= imx_clk_gate2("uart2_per_gate", "uart_root", MXC_CCM_CCGR1, 12);
+	clk[IMX5_CLK_UART3_IPG_GATE]	= imx_clk_gate2("uart3_ipg_gate", "ipg", MXC_CCM_CCGR1, 14);
+	clk[IMX5_CLK_UART3_PER_GATE]	= imx_clk_gate2("uart3_per_gate", "uart_root", MXC_CCM_CCGR1, 16);
+	clk[IMX5_CLK_I2C1_GATE]		= imx_clk_gate2("i2c1_gate", "per_root", MXC_CCM_CCGR1, 18);
+	clk[IMX5_CLK_I2C2_GATE]		= imx_clk_gate2("i2c2_gate", "per_root", MXC_CCM_CCGR1, 20);
+	clk[IMX5_CLK_PWM1_IPG_GATE]	= imx_clk_gate2("pwm1_ipg_gate", "ipg", MXC_CCM_CCGR2, 10);
+	clk[IMX5_CLK_PWM1_HF_GATE]	= imx_clk_gate2("pwm1_hf_gate", "per_root", MXC_CCM_CCGR2, 12);
+	clk[IMX5_CLK_PWM2_IPG_GATE]	= imx_clk_gate2("pwm2_ipg_gate", "ipg", MXC_CCM_CCGR2, 14);
+	clk[IMX5_CLK_PWM2_HF_GATE]	= imx_clk_gate2("pwm2_hf_gate", "per_root", MXC_CCM_CCGR2, 16);
+	clk[IMX5_CLK_GPT_IPG_GATE]	= imx_clk_gate2("gpt_ipg_gate", "ipg", MXC_CCM_CCGR2, 18);
+	clk[IMX5_CLK_GPT_HF_GATE]	= imx_clk_gate2("gpt_hf_gate", "per_root", MXC_CCM_CCGR2, 20);
+	clk[IMX5_CLK_FEC_GATE]		= imx_clk_gate2("fec_gate", "ipg", MXC_CCM_CCGR2, 24);
+	clk[IMX5_CLK_USBOH3_GATE]	= imx_clk_gate2("usboh3_gate", "ipg", MXC_CCM_CCGR2, 26);
+	clk[IMX5_CLK_USBOH3_PER_GATE]	= imx_clk_gate2("usboh3_per_gate", "usboh3_podf", MXC_CCM_CCGR2, 28);
+	clk[IMX5_CLK_ESDHC1_IPG_GATE]	= imx_clk_gate2("esdhc1_ipg_gate", "ipg", MXC_CCM_CCGR3, 0);
+	clk[IMX5_CLK_ESDHC2_IPG_GATE]	= imx_clk_gate2("esdhc2_ipg_gate", "ipg", MXC_CCM_CCGR3, 4);
+	clk[IMX5_CLK_ESDHC3_IPG_GATE]	= imx_clk_gate2("esdhc3_ipg_gate", "ipg", MXC_CCM_CCGR3, 8);
+	clk[IMX5_CLK_ESDHC4_IPG_GATE]	= imx_clk_gate2("esdhc4_ipg_gate", "ipg", MXC_CCM_CCGR3, 12);
+	clk[IMX5_CLK_SSI1_IPG_GATE]	= imx_clk_gate2("ssi1_ipg_gate", "ipg", MXC_CCM_CCGR3, 16);
+	clk[IMX5_CLK_SSI2_IPG_GATE]	= imx_clk_gate2("ssi2_ipg_gate", "ipg", MXC_CCM_CCGR3, 20);
+	clk[IMX5_CLK_SSI3_IPG_GATE]	= imx_clk_gate2("ssi3_ipg_gate", "ipg", MXC_CCM_CCGR3, 24);
+	clk[IMX5_CLK_ECSPI1_IPG_GATE]	= imx_clk_gate2("ecspi1_ipg_gate", "ipg", MXC_CCM_CCGR4, 18);
+	clk[IMX5_CLK_ECSPI1_PER_GATE]	= imx_clk_gate2("ecspi1_per_gate", "ecspi_podf", MXC_CCM_CCGR4, 20);
+	clk[IMX5_CLK_ECSPI2_IPG_GATE]	= imx_clk_gate2("ecspi2_ipg_gate", "ipg", MXC_CCM_CCGR4, 22);
+	clk[IMX5_CLK_ECSPI2_PER_GATE]	= imx_clk_gate2("ecspi2_per_gate", "ecspi_podf", MXC_CCM_CCGR4, 24);
+	clk[IMX5_CLK_CSPI_IPG_GATE]	= imx_clk_gate2("cspi_ipg_gate", "ipg", MXC_CCM_CCGR4, 26);
+	clk[IMX5_CLK_SDMA_GATE]		= imx_clk_gate2("sdma_gate", "ipg", MXC_CCM_CCGR4, 30);
+	clk[IMX5_CLK_EMI_FAST_GATE]	= imx_clk_gate2("emi_fast_gate", "dummy", MXC_CCM_CCGR5, 14);
+	clk[IMX5_CLK_EMI_SLOW_GATE]	= imx_clk_gate2("emi_slow_gate", "emi_slow_podf", MXC_CCM_CCGR5, 16);
+	clk[IMX5_CLK_IPU_SEL]		= imx_clk_mux("ipu_sel", MXC_CCM_CBCMR, 6, 2, ipu_sel, ARRAY_SIZE(ipu_sel));
+	clk[IMX5_CLK_IPU_GATE]		= imx_clk_gate2("ipu_gate", "ipu_sel", MXC_CCM_CCGR5, 10);
+	clk[IMX5_CLK_NFC_GATE]		= imx_clk_gate2("nfc_gate", "nfc_podf", MXC_CCM_CCGR5, 20);
+	clk[IMX5_CLK_IPU_DI0_GATE]	= imx_clk_gate2("ipu_di0_gate", "ipu_di0_sel", MXC_CCM_CCGR6, 10);
+	clk[IMX5_CLK_IPU_DI1_GATE]	= imx_clk_gate2("ipu_di1_gate", "ipu_di1_sel", MXC_CCM_CCGR6, 12);
+	clk[IMX5_CLK_GPU3D_SEL]		= imx_clk_mux("gpu3d_sel", MXC_CCM_CBCMR, 4, 2, gpu3d_sel, ARRAY_SIZE(gpu3d_sel));
+	clk[IMX5_CLK_GPU2D_SEL]		= imx_clk_mux("gpu2d_sel", MXC_CCM_CBCMR, 16, 2, gpu2d_sel, ARRAY_SIZE(gpu2d_sel));
+	clk[IMX5_CLK_GPU3D_GATE]	= imx_clk_gate2("gpu3d_gate", "gpu3d_sel", MXC_CCM_CCGR5, 2);
+	clk[IMX5_CLK_GARB_GATE]		= imx_clk_gate2("garb_gate", "axi_a", MXC_CCM_CCGR5, 4);
+	clk[IMX5_CLK_GPU2D_GATE]	= imx_clk_gate2("gpu2d_gate", "gpu2d_sel", MXC_CCM_CCGR6, 14);
+	clk[IMX5_CLK_VPU_SEL]		= imx_clk_mux("vpu_sel", MXC_CCM_CBCMR, 14, 2, vpu_sel, ARRAY_SIZE(vpu_sel));
+	clk[IMX5_CLK_VPU_GATE]		= imx_clk_gate2("vpu_gate", "vpu_sel", MXC_CCM_CCGR5, 6);
+	clk[IMX5_CLK_VPU_REFERENCE_GATE] = imx_clk_gate2("vpu_reference_gate", "osc", MXC_CCM_CCGR5, 8);
+	clk[IMX5_CLK_UART4_IPG_GATE]	= imx_clk_gate2("uart4_ipg_gate", "ipg", MXC_CCM_CCGR7, 8);
+	clk[IMX5_CLK_UART4_PER_GATE]	= imx_clk_gate2("uart4_per_gate", "uart_root", MXC_CCM_CCGR7, 10);
+	clk[IMX5_CLK_UART5_IPG_GATE]	= imx_clk_gate2("uart5_ipg_gate", "ipg", MXC_CCM_CCGR7, 12);
+	clk[IMX5_CLK_UART5_PER_GATE]	= imx_clk_gate2("uart5_per_gate", "uart_root", MXC_CCM_CCGR7, 14);
+	clk[IMX5_CLK_GPC_DVFS]		= imx_clk_gate2("gpc_dvfs", "dummy", MXC_CCM_CCGR5, 24);
+
+	clk[IMX5_CLK_SSI_APM]		= imx_clk_mux("ssi_apm", MXC_CCM_CSCMR1, 8, 2, ssi_apm_sels, ARRAY_SIZE(ssi_apm_sels));
+	clk[IMX5_CLK_SSI1_ROOT_SEL]	= imx_clk_mux("ssi1_root_sel", MXC_CCM_CSCMR1, 14, 2, ssi_clk_sels, ARRAY_SIZE(ssi_clk_sels));
+	clk[IMX5_CLK_SSI2_ROOT_SEL]	= imx_clk_mux("ssi2_root_sel", MXC_CCM_CSCMR1, 12, 2, ssi_clk_sels, ARRAY_SIZE(ssi_clk_sels));
+	clk[IMX5_CLK_SSI3_ROOT_SEL]	= imx_clk_mux("ssi3_root_sel", MXC_CCM_CSCMR1, 11, 1, ssi3_clk_sels, ARRAY_SIZE(ssi3_clk_sels));
+	clk[IMX5_CLK_SSI_EXT1_SEL]	= imx_clk_mux("ssi_ext1_sel", MXC_CCM_CSCMR1, 28, 2, ssi_clk_sels, ARRAY_SIZE(ssi_clk_sels));
+	clk[IMX5_CLK_SSI_EXT2_SEL]	= imx_clk_mux("ssi_ext2_sel", MXC_CCM_CSCMR1, 30, 2, ssi_clk_sels, ARRAY_SIZE(ssi_clk_sels));
+	clk[IMX5_CLK_SSI_EXT1_COM_SEL]	= imx_clk_mux("ssi_ext1_com_sel", MXC_CCM_CSCMR1, 0, 1, ssi_ext1_com_sels, ARRAY_SIZE(ssi_ext1_com_sels));
+	clk[IMX5_CLK_SSI_EXT2_COM_SEL]	= imx_clk_mux("ssi_ext2_com_sel", MXC_CCM_CSCMR1, 1, 1, ssi_ext2_com_sels, ARRAY_SIZE(ssi_ext2_com_sels));
+	clk[IMX5_CLK_SSI1_ROOT_PRED]	= imx_clk_divider("ssi1_root_pred", "ssi1_root_sel", MXC_CCM_CS1CDR, 6, 3);
+	clk[IMX5_CLK_SSI1_ROOT_PODF]	= imx_clk_divider("ssi1_root_podf", "ssi1_root_pred", MXC_CCM_CS1CDR, 0, 6);
+	clk[IMX5_CLK_SSI2_ROOT_PRED]	= imx_clk_divider("ssi2_root_pred", "ssi2_root_sel", MXC_CCM_CS2CDR, 6, 3);
+	clk[IMX5_CLK_SSI2_ROOT_PODF]	= imx_clk_divider("ssi2_root_podf", "ssi2_root_pred", MXC_CCM_CS2CDR, 0, 6);
+	clk[IMX5_CLK_SSI_EXT1_PRED]	= imx_clk_divider("ssi_ext1_pred", "ssi_ext1_sel", MXC_CCM_CS1CDR, 22, 3);
+	clk[IMX5_CLK_SSI_EXT1_PODF]	= imx_clk_divider("ssi_ext1_podf", "ssi_ext1_pred", MXC_CCM_CS1CDR, 16, 6);
+	clk[IMX5_CLK_SSI_EXT2_PRED]	= imx_clk_divider("ssi_ext2_pred", "ssi_ext2_sel", MXC_CCM_CS2CDR, 22, 3);
+	clk[IMX5_CLK_SSI_EXT2_PODF]	= imx_clk_divider("ssi_ext2_podf", "ssi_ext2_pred", MXC_CCM_CS2CDR, 16, 6);
+	clk[IMX5_CLK_SSI1_ROOT_GATE]	= imx_clk_gate2("ssi1_root_gate", "ssi1_root_podf", MXC_CCM_CCGR3, 18);
+	clk[IMX5_CLK_SSI2_ROOT_GATE]	= imx_clk_gate2("ssi2_root_gate", "ssi2_root_podf", MXC_CCM_CCGR3, 22);
+	clk[IMX5_CLK_SSI3_ROOT_GATE]	= imx_clk_gate2("ssi3_root_gate", "ssi3_root_sel", MXC_CCM_CCGR3, 26);
+	clk[IMX5_CLK_SSI_EXT1_GATE]	= imx_clk_gate2("ssi_ext1_gate", "ssi_ext1_com_sel", MXC_CCM_CCGR3, 28);
+	clk[IMX5_CLK_SSI_EXT2_GATE]	= imx_clk_gate2("ssi_ext2_gate", "ssi_ext2_com_sel", MXC_CCM_CCGR3, 30);
+	clk[IMX5_CLK_EPIT1_IPG_GATE]	= imx_clk_gate2("epit1_ipg_gate", "ipg", MXC_CCM_CCGR2, 2);
+	clk[IMX5_CLK_EPIT1_HF_GATE]	= imx_clk_gate2("epit1_hf_gate", "per_root", MXC_CCM_CCGR2, 4);
+	clk[IMX5_CLK_EPIT2_IPG_GATE]	= imx_clk_gate2("epit2_ipg_gate", "ipg", MXC_CCM_CCGR2, 6);
+	clk[IMX5_CLK_EPIT2_HF_GATE]	= imx_clk_gate2("epit2_hf_gate", "per_root", MXC_CCM_CCGR2, 8);
+	clk[IMX5_CLK_OWIRE_GATE]	= imx_clk_gate2("owire_gate", "per_root", MXC_CCM_CCGR2, 22);
+	clk[IMX5_CLK_SRTC_GATE]		= imx_clk_gate2("srtc_gate", "per_root", MXC_CCM_CCGR4, 28);
+	clk[IMX5_CLK_PATA_GATE]		= imx_clk_gate2("pata_gate", "ipg", MXC_CCM_CCGR4, 0);
+	clk[IMX5_CLK_SPDIF0_SEL]	= imx_clk_mux("spdif0_sel", MXC_CCM_CSCMR2, 0, 2, spdif_sel, ARRAY_SIZE(spdif_sel));
+	clk[IMX5_CLK_SPDIF0_PRED]	= imx_clk_divider("spdif0_pred", "spdif0_sel", MXC_CCM_CDCDR, 25, 3);
+	clk[IMX5_CLK_SPDIF0_PODF]	= imx_clk_divider("spdif0_podf", "spdif0_pred", MXC_CCM_CDCDR, 19, 6);
+	clk[IMX5_CLK_SPDIF0_COM_SEL]	= imx_clk_mux_flags("spdif0_com_sel", MXC_CCM_CSCMR2, 4, 1,
+						spdif0_com_sel, ARRAY_SIZE(spdif0_com_sel), CLK_SET_RATE_PARENT);
+	clk[IMX5_CLK_SPDIF0_GATE]	= imx_clk_gate2("spdif0_gate", "spdif0_com_sel", MXC_CCM_CCGR5, 26);
+	clk[IMX5_CLK_SPDIF_IPG_GATE]	= imx_clk_gate2("spdif_ipg_gate", "ipg", MXC_CCM_CCGR5, 30);
+	clk[IMX5_CLK_SAHARA_IPG_GATE]	= imx_clk_gate2("sahara_ipg_gate", "ipg", MXC_CCM_CCGR4, 14);
+	clk[IMX5_CLK_SATA_REF]		= imx_clk_fixed_factor("sata_ref", "usb_phy1_gate", 1, 1);
+
+	clk_register_clkdev(clk[IMX5_CLK_CPU_PODF], NULL, "cpu0");
+	clk_register_clkdev(clk[IMX5_CLK_GPC_DVFS], "gpc_dvfs", NULL);
+
+	/* Set SDHC parents to be PLL2 */
+	clk_set_parent(clk[IMX5_CLK_ESDHC_A_SEL], clk[IMX5_CLK_PLL2_SW]);
+	clk_set_parent(clk[IMX5_CLK_ESDHC_B_SEL], clk[IMX5_CLK_PLL2_SW]);
+
+	/* move usb phy clk to 24MHz */
+	clk_set_parent(clk[IMX5_CLK_USB_PHY_SEL], clk[IMX5_CLK_OSC]);
+
+	clk_prepare_enable(clk[IMX5_CLK_GPC_DVFS]);
+	clk_prepare_enable(clk[IMX5_CLK_AHB_MAX]); /* esdhc3 */
+	clk_prepare_enable(clk[IMX5_CLK_AIPS_TZ1]);
+	clk_prepare_enable(clk[IMX5_CLK_AIPS_TZ2]); /* fec */
+	clk_prepare_enable(clk[IMX5_CLK_SPBA]);
+	clk_prepare_enable(clk[IMX5_CLK_EMI_FAST_GATE]); /* fec */
+	clk_prepare_enable(clk[IMX5_CLK_EMI_SLOW_GATE]); /* eim */
+	clk_prepare_enable(clk[IMX5_CLK_MIPI_HSC1_GATE]);
+	clk_prepare_enable(clk[IMX5_CLK_MIPI_HSC2_GATE]);
+	clk_prepare_enable(clk[IMX5_CLK_MIPI_ESC_GATE]);
+	clk_prepare_enable(clk[IMX5_CLK_MIPI_HSP_GATE]);
+	clk_prepare_enable(clk[IMX5_CLK_TMAX1]);
+	clk_prepare_enable(clk[IMX5_CLK_TMAX2]); /* esdhc2, fec */
+	clk_prepare_enable(clk[IMX5_CLK_TMAX3]); /* esdhc1, esdhc4 */
+}
+
+static void __init mx50_clocks_init(struct device_node *np)
+{
+	void __iomem *ccm_base;
+	void __iomem *pll_base;
+	unsigned long r;
+
+	pll_base = ioremap(MX53_DPLL1_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL1_SW]		= imx_clk_pllv2("pll1_sw", "osc", pll_base);
+
+	pll_base = ioremap(MX53_DPLL2_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL2_SW]		= imx_clk_pllv2("pll2_sw", "osc", pll_base);
+
+	pll_base = ioremap(MX53_DPLL3_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL3_SW]		= imx_clk_pllv2("pll3_sw", "osc", pll_base);
+
+	ccm_base = of_iomap(np, 0);
+	WARN_ON(!ccm_base);
+
+	mx5_clocks_common_init(ccm_base);
+
+	clk[IMX5_CLK_LP_APM]		= imx_clk_mux("lp_apm", MXC_CCM_CCSR, 10, 1,
+						lp_apm_sel, ARRAY_SIZE(lp_apm_sel));
+	clk[IMX5_CLK_ESDHC1_PER_GATE]	= imx_clk_gate2("esdhc1_per_gate", "esdhc_a_podf", MXC_CCM_CCGR3, 2);
+	clk[IMX5_CLK_ESDHC2_PER_GATE]	= imx_clk_gate2("esdhc2_per_gate", "esdhc_c_sel", MXC_CCM_CCGR3, 6);
+	clk[IMX5_CLK_ESDHC3_PER_GATE]	= imx_clk_gate2("esdhc3_per_gate", "esdhc_b_podf", MXC_CCM_CCGR3, 10);
+	clk[IMX5_CLK_ESDHC4_PER_GATE]	= imx_clk_gate2("esdhc4_per_gate", "esdhc_d_sel", MXC_CCM_CCGR3, 14);
+	clk[IMX5_CLK_USB_PHY1_GATE]	= imx_clk_gate2("usb_phy1_gate", "usb_phy_sel", MXC_CCM_CCGR4, 10);
+	clk[IMX5_CLK_USB_PHY2_GATE]	= imx_clk_gate2("usb_phy2_gate", "usb_phy_sel", MXC_CCM_CCGR4, 12);
+	clk[IMX5_CLK_I2C3_GATE]		= imx_clk_gate2("i2c3_gate", "per_root", MXC_CCM_CCGR1, 22);
+
+	clk[IMX5_CLK_CKO1_SEL]		= imx_clk_mux("cko1_sel", MXC_CCM_CCOSR, 0, 4,
+						mx53_cko1_sel, ARRAY_SIZE(mx53_cko1_sel));
+	clk[IMX5_CLK_CKO1_PODF]		= imx_clk_divider("cko1_podf", "cko1_sel", MXC_CCM_CCOSR, 4, 3);
+	clk[IMX5_CLK_CKO1]		= imx_clk_gate2("cko1", "cko1_podf", MXC_CCM_CCOSR, 7);
+
+	clk[IMX5_CLK_CKO2_SEL]		= imx_clk_mux("cko2_sel", MXC_CCM_CCOSR, 16, 5,
+						mx53_cko2_sel, ARRAY_SIZE(mx53_cko2_sel));
+	clk[IMX5_CLK_CKO2_PODF]		= imx_clk_divider("cko2_podf", "cko2_sel", MXC_CCM_CCOSR, 21, 3);
+	clk[IMX5_CLK_CKO2]		= imx_clk_gate2("cko2", "cko2_podf", MXC_CCM_CCOSR, 24);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	/* set SDHC root clock to 200MHZ*/
+	clk_set_rate(clk[IMX5_CLK_ESDHC_A_PODF], 200000000);
+	clk_set_rate(clk[IMX5_CLK_ESDHC_B_PODF], 200000000);
+
+	clk_prepare_enable(clk[IMX5_CLK_IIM_GATE]);
+	imx_print_silicon_rev("i.MX50", IMX_CHIP_REVISION_1_1);
+	clk_disable_unprepare(clk[IMX5_CLK_IIM_GATE]);
+
+	r = clk_round_rate(clk[IMX5_CLK_USBOH3_PER_GATE], 54000000);
+	clk_set_rate(clk[IMX5_CLK_USBOH3_PER_GATE], r);
+}
+CLK_OF_DECLARE(imx50_ccm, "fsl,imx50-ccm", mx50_clocks_init);
+
+static void __init mx51_clocks_init(struct device_node *np)
+{
+	void __iomem *ccm_base;
+	void __iomem *pll_base;
+	u32 val;
+
+	pll_base = ioremap(MX51_DPLL1_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL1_SW]		= imx_clk_pllv2("pll1_sw", "osc", pll_base);
+
+	pll_base = ioremap(MX51_DPLL2_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL2_SW]		= imx_clk_pllv2("pll2_sw", "osc", pll_base);
+
+	pll_base = ioremap(MX51_DPLL3_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL3_SW]		= imx_clk_pllv2("pll3_sw", "osc", pll_base);
+
+	ccm_base = of_iomap(np, 0);
+	WARN_ON(!ccm_base);
+
+	mx5_clocks_common_init(ccm_base);
+
+	clk[IMX5_CLK_LP_APM]		= imx_clk_mux("lp_apm", MXC_CCM_CCSR, 9, 1,
+						lp_apm_sel, ARRAY_SIZE(lp_apm_sel));
+	clk[IMX5_CLK_IPU_DI0_SEL]	= imx_clk_mux("ipu_di0_sel", MXC_CCM_CSCMR2, 26, 3,
+						mx51_ipu_di0_sel, ARRAY_SIZE(mx51_ipu_di0_sel));
+	clk[IMX5_CLK_IPU_DI1_SEL]	= imx_clk_mux("ipu_di1_sel", MXC_CCM_CSCMR2, 29, 3,
+						mx51_ipu_di1_sel, ARRAY_SIZE(mx51_ipu_di1_sel));
+	clk[IMX5_CLK_TVE_EXT_SEL]	= imx_clk_mux_flags("tve_ext_sel", MXC_CCM_CSCMR1, 6, 1,
+						mx51_tve_ext_sel, ARRAY_SIZE(mx51_tve_ext_sel), CLK_SET_RATE_PARENT);
+	clk[IMX5_CLK_TVE_SEL]		= imx_clk_mux("tve_sel", MXC_CCM_CSCMR1, 7, 1,
+						mx51_tve_sel, ARRAY_SIZE(mx51_tve_sel));
+	clk[IMX5_CLK_TVE_GATE]		= imx_clk_gate2("tve_gate", "tve_sel", MXC_CCM_CCGR2, 30);
+	clk[IMX5_CLK_TVE_PRED]		= imx_clk_divider("tve_pred", "pll3_sw", MXC_CCM_CDCDR, 28, 3);
+	clk[IMX5_CLK_ESDHC1_PER_GATE]	= imx_clk_gate2("esdhc1_per_gate", "esdhc_a_podf", MXC_CCM_CCGR3, 2);
+	clk[IMX5_CLK_ESDHC2_PER_GATE]	= imx_clk_gate2("esdhc2_per_gate", "esdhc_b_podf", MXC_CCM_CCGR3, 6);
+	clk[IMX5_CLK_ESDHC3_PER_GATE]	= imx_clk_gate2("esdhc3_per_gate", "esdhc_c_sel", MXC_CCM_CCGR3, 10);
+	clk[IMX5_CLK_ESDHC4_PER_GATE]	= imx_clk_gate2("esdhc4_per_gate", "esdhc_d_sel", MXC_CCM_CCGR3, 14);
+	clk[IMX5_CLK_USB_PHY_GATE]	= imx_clk_gate2("usb_phy_gate", "usb_phy_sel", MXC_CCM_CCGR2, 0);
+	clk[IMX5_CLK_HSI2C_GATE]	= imx_clk_gate2("hsi2c_gate", "ipg", MXC_CCM_CCGR1, 22);
+	clk[IMX5_CLK_MIPI_HSC1_GATE]	= imx_clk_gate2("mipi_hsc1_gate", "ipg", MXC_CCM_CCGR4, 6);
+	clk[IMX5_CLK_MIPI_HSC2_GATE]	= imx_clk_gate2("mipi_hsc2_gate", "ipg", MXC_CCM_CCGR4, 8);
+	clk[IMX5_CLK_MIPI_ESC_GATE]	= imx_clk_gate2("mipi_esc_gate", "ipg", MXC_CCM_CCGR4, 10);
+	clk[IMX5_CLK_MIPI_HSP_GATE]	= imx_clk_gate2("mipi_hsp_gate", "ipg", MXC_CCM_CCGR4, 12);
+	clk[IMX5_CLK_SPDIF_XTAL_SEL]	= imx_clk_mux("spdif_xtal_sel", MXC_CCM_CSCMR1, 2, 2,
+						mx51_spdif_xtal_sel, ARRAY_SIZE(mx51_spdif_xtal_sel));
+	clk[IMX5_CLK_SPDIF1_SEL]	= imx_clk_mux("spdif1_sel", MXC_CCM_CSCMR2, 2, 2,
+						spdif_sel, ARRAY_SIZE(spdif_sel));
+	clk[IMX5_CLK_SPDIF1_PRED]	= imx_clk_divider("spdif1_pred", "spdif1_sel", MXC_CCM_CDCDR, 16, 3);
+	clk[IMX5_CLK_SPDIF1_PODF]	= imx_clk_divider("spdif1_podf", "spdif1_pred", MXC_CCM_CDCDR, 9, 6);
+	clk[IMX5_CLK_SPDIF1_COM_SEL]	= imx_clk_mux("spdif1_com_sel", MXC_CCM_CSCMR2, 5, 1,
+						mx51_spdif1_com_sel, ARRAY_SIZE(mx51_spdif1_com_sel));
+	clk[IMX5_CLK_SPDIF1_GATE]	= imx_clk_gate2("spdif1_gate", "spdif1_com_sel", MXC_CCM_CCGR5, 28);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	/* set the usboh3 parent to pll2_sw */
+	clk_set_parent(clk[IMX5_CLK_USBOH3_SEL], clk[IMX5_CLK_PLL2_SW]);
+
+	/* set SDHC root clock to 166.25MHZ*/
+	clk_set_rate(clk[IMX5_CLK_ESDHC_A_PODF], 166250000);
+	clk_set_rate(clk[IMX5_CLK_ESDHC_B_PODF], 166250000);
+
+	clk_prepare_enable(clk[IMX5_CLK_IIM_GATE]);
+	imx_print_silicon_rev("i.MX51", mx51_revision());
+	clk_disable_unprepare(clk[IMX5_CLK_IIM_GATE]);
+
+	/*
+	 * Reference Manual says: Functionality of CCDR[18] and CLPCR[23] is no
+	 * longer supported. Set to one for better power saving.
+	 *
+	 * The effect of not setting these bits is that MIPI clocks can't be
+	 * enabled without the IPU clock being enabled aswell.
+	 */
+	val = readl(MXC_CCM_CCDR);
+	val |= 1 << 18;
+	writel(val, MXC_CCM_CCDR);
+
+	val = readl(MXC_CCM_CLPCR);
+	val |= 1 << 23;
+	writel(val, MXC_CCM_CLPCR);
+}
+CLK_OF_DECLARE(imx51_ccm, "fsl,imx51-ccm", mx51_clocks_init);
+
+static void __init mx53_clocks_init(struct device_node *np)
+{
+	void __iomem *ccm_base;
+	void __iomem *pll_base;
+	unsigned long r;
+
+	pll_base = ioremap(MX53_DPLL1_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL1_SW]		= imx_clk_pllv2("pll1_sw", "osc", pll_base);
+
+	pll_base = ioremap(MX53_DPLL2_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL2_SW]		= imx_clk_pllv2("pll2_sw", "osc", pll_base);
+
+	pll_base = ioremap(MX53_DPLL3_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL3_SW]		= imx_clk_pllv2("pll3_sw", "osc", pll_base);
+
+	pll_base = ioremap(MX53_DPLL4_BASE, SZ_16K);
+	WARN_ON(!pll_base);
+	clk[IMX5_CLK_PLL4_SW]		= imx_clk_pllv2("pll4_sw", "osc", pll_base);
+
+	ccm_base = of_iomap(np, 0);
+	WARN_ON(!ccm_base);
+
+	mx5_clocks_common_init(ccm_base);
+
+	clk[IMX5_CLK_LP_APM]		= imx_clk_mux("lp_apm", MXC_CCM_CCSR, 10, 1,
+						lp_apm_sel, ARRAY_SIZE(lp_apm_sel));
+	clk[IMX5_CLK_LDB_DI1_DIV_3_5]	= imx_clk_fixed_factor("ldb_di1_div_3_5", "ldb_di1_sel", 2, 7);
+	clk[IMX5_CLK_LDB_DI1_DIV]	= imx_clk_divider_flags("ldb_di1_div", "ldb_di1_div_3_5", MXC_CCM_CSCMR2, 11, 1, 0);
+	clk[IMX5_CLK_LDB_DI1_SEL]	= imx_clk_mux_flags("ldb_di1_sel", MXC_CCM_CSCMR2, 9, 1,
+						mx53_ldb_di1_sel, ARRAY_SIZE(mx53_ldb_di1_sel), CLK_SET_RATE_PARENT);
+	clk[IMX5_CLK_DI_PLL4_PODF]	= imx_clk_divider("di_pll4_podf", "pll4_sw", MXC_CCM_CDCDR, 16, 3);
+	clk[IMX5_CLK_LDB_DI0_DIV_3_5]	= imx_clk_fixed_factor("ldb_di0_div_3_5", "ldb_di0_sel", 2, 7);
+	clk[IMX5_CLK_LDB_DI0_DIV]	= imx_clk_divider_flags("ldb_di0_div", "ldb_di0_div_3_5", MXC_CCM_CSCMR2, 10, 1, 0);
+	clk[IMX5_CLK_LDB_DI0_SEL]	= imx_clk_mux_flags("ldb_di0_sel", MXC_CCM_CSCMR2, 8, 1,
+						mx53_ldb_di0_sel, ARRAY_SIZE(mx53_ldb_di0_sel), CLK_SET_RATE_PARENT);
+	clk[IMX5_CLK_LDB_DI0_GATE]	= imx_clk_gate2("ldb_di0_gate", "ldb_di0_div", MXC_CCM_CCGR6, 28);
+	clk[IMX5_CLK_LDB_DI1_GATE]	= imx_clk_gate2("ldb_di1_gate", "ldb_di1_div", MXC_CCM_CCGR6, 30);
+	clk[IMX5_CLK_IPU_DI0_SEL]	= imx_clk_mux("ipu_di0_sel", MXC_CCM_CSCMR2, 26, 3,
+						mx53_ipu_di0_sel, ARRAY_SIZE(mx53_ipu_di0_sel));
+	clk[IMX5_CLK_IPU_DI1_SEL]	= imx_clk_mux("ipu_di1_sel", MXC_CCM_CSCMR2, 29, 3,
+						mx53_ipu_di1_sel, ARRAY_SIZE(mx53_ipu_di1_sel));
+	clk[IMX5_CLK_TVE_EXT_SEL]	= imx_clk_mux_flags("tve_ext_sel", MXC_CCM_CSCMR1, 6, 1,
+						mx53_tve_ext_sel, ARRAY_SIZE(mx53_tve_ext_sel), CLK_SET_RATE_PARENT);
+	clk[IMX5_CLK_TVE_GATE]		= imx_clk_gate2("tve_gate", "tve_pred", MXC_CCM_CCGR2, 30);
+	clk[IMX5_CLK_TVE_PRED]		= imx_clk_divider("tve_pred", "tve_ext_sel", MXC_CCM_CDCDR, 28, 3);
+	clk[IMX5_CLK_ESDHC1_PER_GATE]	= imx_clk_gate2("esdhc1_per_gate", "esdhc_a_podf", MXC_CCM_CCGR3, 2);
+	clk[IMX5_CLK_ESDHC2_PER_GATE]	= imx_clk_gate2("esdhc2_per_gate", "esdhc_c_sel", MXC_CCM_CCGR3, 6);
+	clk[IMX5_CLK_ESDHC3_PER_GATE]	= imx_clk_gate2("esdhc3_per_gate", "esdhc_b_podf", MXC_CCM_CCGR3, 10);
+	clk[IMX5_CLK_ESDHC4_PER_GATE]	= imx_clk_gate2("esdhc4_per_gate", "esdhc_d_sel", MXC_CCM_CCGR3, 14);
+	clk[IMX5_CLK_USB_PHY1_GATE]	= imx_clk_gate2("usb_phy1_gate", "usb_phy_sel", MXC_CCM_CCGR4, 10);
+	clk[IMX5_CLK_USB_PHY2_GATE]	= imx_clk_gate2("usb_phy2_gate", "usb_phy_sel", MXC_CCM_CCGR4, 12);
+	clk[IMX5_CLK_CAN_SEL]		= imx_clk_mux("can_sel", MXC_CCM_CSCMR2, 6, 2,
+						mx53_can_sel, ARRAY_SIZE(mx53_can_sel));
+	clk[IMX5_CLK_CAN1_SERIAL_GATE]	= imx_clk_gate2("can1_serial_gate", "can_sel", MXC_CCM_CCGR6, 22);
+	clk[IMX5_CLK_CAN1_IPG_GATE]	= imx_clk_gate2("can1_ipg_gate", "ipg", MXC_CCM_CCGR6, 20);
+	clk[IMX5_CLK_OCRAM]		= imx_clk_gate2("ocram", "ahb", MXC_CCM_CCGR6, 2);
+	clk[IMX5_CLK_CAN2_SERIAL_GATE]	= imx_clk_gate2("can2_serial_gate", "can_sel", MXC_CCM_CCGR4, 8);
+	clk[IMX5_CLK_CAN2_IPG_GATE]	= imx_clk_gate2("can2_ipg_gate", "ipg", MXC_CCM_CCGR4, 6);
+	clk[IMX5_CLK_I2C3_GATE]		= imx_clk_gate2("i2c3_gate", "per_root", MXC_CCM_CCGR1, 22);
+	clk[IMX5_CLK_SATA_GATE]		= imx_clk_gate2("sata_gate", "ipg", MXC_CCM_CCGR4, 2);
+
+	clk[IMX5_CLK_CKO1_SEL]		= imx_clk_mux("cko1_sel", MXC_CCM_CCOSR, 0, 4,
+						mx53_cko1_sel, ARRAY_SIZE(mx53_cko1_sel));
+	clk[IMX5_CLK_CKO1_PODF]		= imx_clk_divider("cko1_podf", "cko1_sel", MXC_CCM_CCOSR, 4, 3);
+	clk[IMX5_CLK_CKO1]		= imx_clk_gate2("cko1", "cko1_podf", MXC_CCM_CCOSR, 7);
+
+	clk[IMX5_CLK_CKO2_SEL]		= imx_clk_mux("cko2_sel", MXC_CCM_CCOSR, 16, 5,
+						mx53_cko2_sel, ARRAY_SIZE(mx53_cko2_sel));
+	clk[IMX5_CLK_CKO2_PODF]		= imx_clk_divider("cko2_podf", "cko2_sel", MXC_CCM_CCOSR, 21, 3);
+	clk[IMX5_CLK_CKO2]		= imx_clk_gate2("cko2", "cko2_podf", MXC_CCM_CCOSR, 24);
+	clk[IMX5_CLK_SPDIF_XTAL_SEL]	= imx_clk_mux("spdif_xtal_sel", MXC_CCM_CSCMR1, 2, 2,
+						mx53_spdif_xtal_sel, ARRAY_SIZE(mx53_spdif_xtal_sel));
+	clk[IMX5_CLK_ARM]		= imx_clk_cpu("arm", "cpu_podf",
+						clk[IMX5_CLK_CPU_PODF],
+						clk[IMX5_CLK_CPU_PODF_SEL],
+						clk[IMX5_CLK_PLL1_SW],
+						clk[IMX5_CLK_STEP_SEL]);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	/* set SDHC root clock to 200MHZ*/
+	clk_set_rate(clk[IMX5_CLK_ESDHC_A_PODF], 200000000);
+	clk_set_rate(clk[IMX5_CLK_ESDHC_B_PODF], 200000000);
+
+	/* move can bus clk to 24MHz */
+	clk_set_parent(clk[IMX5_CLK_CAN_SEL], clk[IMX5_CLK_LP_APM]);
+
+	/* make sure step clock is running from 24MHz */
+	clk_set_parent(clk[IMX5_CLK_STEP_SEL], clk[IMX5_CLK_LP_APM]);
+
+	clk_prepare_enable(clk[IMX5_CLK_IIM_GATE]);
+	imx_print_silicon_rev("i.MX53", mx53_revision());
+	clk_disable_unprepare(clk[IMX5_CLK_IIM_GATE]);
+
+	r = clk_round_rate(clk[IMX5_CLK_USBOH3_PER_GATE], 54000000);
+	clk_set_rate(clk[IMX5_CLK_USBOH3_PER_GATE], r);
+}
+CLK_OF_DECLARE(imx53_ccm, "fsl,imx53-ccm", mx53_clocks_init);
diff --git a/drivers/clk/imx/clk-imx6q.c b/drivers/clk/imx/clk-imx6q.c
new file mode 100644
index 000000000000..d046f8e43de8
--- /dev/null
+++ b/drivers/clk/imx/clk-imx6q.c
@@ -0,0 +1,538 @@
+/*
+ * Copyright 2011-2013 Freescale Semiconductor, Inc.
+ * Copyright 2011 Linaro Ltd.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <soc/imx/revision.h>
+#include <dt-bindings/clock/imx6qdl-clock.h>
+
+#include "clk.h"
+
+static const char *step_sels[]	= { "osc", "pll2_pfd2_396m", };
+static const char *pll1_sw_sels[]	= { "pll1_sys", "step", };
+static const char *periph_pre_sels[]	= { "pll2_bus", "pll2_pfd2_396m", "pll2_pfd0_352m", "pll2_198m", };
+static const char *periph_clk2_sels[]	= { "pll3_usb_otg", "osc", "osc", "dummy", };
+static const char *periph2_clk2_sels[]	= { "pll3_usb_otg", "pll2_bus", };
+static const char *periph_sels[]	= { "periph_pre", "periph_clk2", };
+static const char *periph2_sels[]	= { "periph2_pre", "periph2_clk2", };
+static const char *axi_sels[]		= { "periph", "pll2_pfd2_396m", "periph", "pll3_pfd1_540m", };
+static const char *audio_sels[]	= { "pll4_audio_div", "pll3_pfd2_508m", "pll3_pfd3_454m", "pll3_usb_otg", };
+static const char *gpu_axi_sels[]	= { "axi", "ahb", };
+static const char *gpu2d_core_sels[]	= { "axi", "pll3_usb_otg", "pll2_pfd0_352m", "pll2_pfd2_396m", };
+static const char *gpu3d_core_sels[]	= { "mmdc_ch0_axi", "pll3_usb_otg", "pll2_pfd1_594m", "pll2_pfd2_396m", };
+static const char *gpu3d_shader_sels[] = { "mmdc_ch0_axi", "pll3_usb_otg", "pll2_pfd1_594m", "pll3_pfd0_720m", };
+static const char *ipu_sels[]		= { "mmdc_ch0_axi", "pll2_pfd2_396m", "pll3_120m", "pll3_pfd1_540m", };
+static const char *ldb_di_sels[]	= { "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd2_396m", "mmdc_ch1_axi", "pll3_usb_otg", };
+static const char *ipu_di_pre_sels[]	= { "mmdc_ch0_axi", "pll3_usb_otg", "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll3_pfd1_540m", };
+static const char *ipu1_di0_sels[]	= { "ipu1_di0_pre", "dummy", "dummy", "ldb_di0", "ldb_di1", };
+static const char *ipu1_di1_sels[]	= { "ipu1_di1_pre", "dummy", "dummy", "ldb_di0", "ldb_di1", };
+static const char *ipu2_di0_sels[]	= { "ipu2_di0_pre", "dummy", "dummy", "ldb_di0", "ldb_di1", };
+static const char *ipu2_di1_sels[]	= { "ipu2_di1_pre", "dummy", "dummy", "ldb_di0", "ldb_di1", };
+static const char *hsi_tx_sels[]	= { "pll3_120m", "pll2_pfd2_396m", };
+static const char *pcie_axi_sels[]	= { "axi", "ahb", };
+static const char *ssi_sels[]		= { "pll3_pfd2_508m", "pll3_pfd3_454m", "pll4_audio_div", };
+static const char *usdhc_sels[]	= { "pll2_pfd2_396m", "pll2_pfd0_352m", };
+static const char *enfc_sels[]	= { "pll2_pfd0_352m", "pll2_bus", "pll3_usb_otg", "pll2_pfd2_396m", };
+static const char *eim_sels[]		= { "pll2_pfd2_396m", "pll3_usb_otg", "axi", "pll2_pfd0_352m", };
+static const char *eim_slow_sels[]      = { "axi", "pll3_usb_otg", "pll2_pfd2_396m", "pll2_pfd0_352m", };
+static const char *vdo_axi_sels[]	= { "axi", "ahb", };
+static const char *vpu_axi_sels[]	= { "axi", "pll2_pfd2_396m", "pll2_pfd0_352m", };
+static const char *cko1_sels[]	= { "pll3_usb_otg", "pll2_bus", "pll1_sys", "pll5_video_div",
+				    "dummy", "axi", "enfc", "ipu1_di0", "ipu1_di1", "ipu2_di0",
+				    "ipu2_di1", "ahb", "ipg", "ipg_per", "ckil", "pll4_audio_div", };
+static const char *cko2_sels[] = {
+	"mmdc_ch0_axi", "mmdc_ch1_axi", "usdhc4", "usdhc1",
+	"gpu2d_axi", "dummy", "ecspi_root", "gpu3d_axi",
+	"usdhc3", "dummy", "arm", "ipu1",
+	"ipu2", "vdo_axi", "osc", "gpu2d_core",
+	"gpu3d_core", "usdhc2", "ssi1", "ssi2",
+	"ssi3", "gpu3d_shader", "vpu_axi", "can_root",
+	"ldb_di0", "ldb_di1", "esai_extal", "eim_slow",
+	"uart_serial", "spdif", "asrc", "hsi_tx",
+};
+static const char *cko_sels[] = { "cko1", "cko2", };
+static const char *lvds_sels[] = {
+	"dummy", "dummy", "dummy", "dummy", "dummy", "dummy",
+	"pll4_audio", "pll5_video", "pll8_mlb", "enet_ref",
+	"pcie_ref_125m", "sata_ref_100m",
+};
+static const char *pll_bypass_src_sels[] = { "osc", "lvds1_in", "lvds2_in", "dummy", };
+static const char *pll1_bypass_sels[] = { "pll1", "pll1_bypass_src", };
+static const char *pll2_bypass_sels[] = { "pll2", "pll2_bypass_src", };
+static const char *pll3_bypass_sels[] = { "pll3", "pll3_bypass_src", };
+static const char *pll4_bypass_sels[] = { "pll4", "pll4_bypass_src", };
+static const char *pll5_bypass_sels[] = { "pll5", "pll5_bypass_src", };
+static const char *pll6_bypass_sels[] = { "pll6", "pll6_bypass_src", };
+static const char *pll7_bypass_sels[] = { "pll7", "pll7_bypass_src", };
+
+static struct clk *clk[IMX6QDL_CLK_END];
+static struct clk_onecell_data clk_data;
+
+static unsigned int const clks_init_on[] __initconst = {
+	IMX6QDL_CLK_MMDC_CH0_AXI,
+	IMX6QDL_CLK_ROM,
+	IMX6QDL_CLK_ARM,
+};
+
+static struct clk_div_table clk_enet_ref_table[] = {
+	{ .val = 0, .div = 20, },
+	{ .val = 1, .div = 10, },
+	{ .val = 2, .div = 5, },
+	{ .val = 3, .div = 4, },
+	{ /* sentinel */ }
+};
+
+static struct clk_div_table post_div_table[] = {
+	{ .val = 2, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 0, .div = 4, },
+	{ /* sentinel */ }
+};
+
+static struct clk_div_table video_div_table[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 2, .div = 1, },
+	{ .val = 3, .div = 4, },
+	{ /* sentinel */ }
+};
+
+static unsigned int share_count_esai;
+static unsigned int share_count_asrc;
+static unsigned int share_count_ssi1;
+static unsigned int share_count_ssi2;
+static unsigned int share_count_ssi3;
+static unsigned int share_count_mipi_core_cfg;
+
+static inline int clk_on_imx6q(void)
+{
+	return of_machine_is_compatible("fsl,imx6q");
+}
+
+static inline int clk_on_imx6dl(void)
+{
+	return of_machine_is_compatible("fsl,imx6dl");
+}
+
+static void __init imx6q_clocks_init(struct device_node *ccm_node)
+{
+	struct device_node *np;
+	void __iomem *base;
+	int i;
+	int ret;
+
+	clk[IMX6QDL_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+	clk[IMX6QDL_CLK_CKIL] = imx_obtain_fixed_clock("ckil", 0);
+	clk[IMX6QDL_CLK_CKIH] = imx_obtain_fixed_clock("ckih1", 0);
+	clk[IMX6QDL_CLK_OSC] = imx_obtain_fixed_clock("osc", 0);
+	/* Clock source from external clock via CLK1/2 PADs */
+	clk[IMX6QDL_CLK_ANACLK1] = imx_obtain_fixed_clock("anaclk1", 0);
+	clk[IMX6QDL_CLK_ANACLK2] = imx_obtain_fixed_clock("anaclk2", 0);
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx6q-anatop");
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+
+	/* Audio/video PLL post dividers do not work on i.MX6q revision 1.0 */
+	if (clk_on_imx6q() && imx_get_soc_revision() == IMX_CHIP_REVISION_1_0) {
+		post_div_table[1].div = 1;
+		post_div_table[2].div = 1;
+		video_div_table[1].div = 1;
+		video_div_table[3].div = 1;
+	}
+
+	clk[IMX6QDL_PLL1_BYPASS_SRC] = imx_clk_mux("pll1_bypass_src", base + 0x00, 14, 2, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[IMX6QDL_PLL2_BYPASS_SRC] = imx_clk_mux("pll2_bypass_src", base + 0x30, 14, 2, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[IMX6QDL_PLL3_BYPASS_SRC] = imx_clk_mux("pll3_bypass_src", base + 0x10, 14, 2, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[IMX6QDL_PLL4_BYPASS_SRC] = imx_clk_mux("pll4_bypass_src", base + 0x70, 14, 2, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[IMX6QDL_PLL5_BYPASS_SRC] = imx_clk_mux("pll5_bypass_src", base + 0xa0, 14, 2, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[IMX6QDL_PLL6_BYPASS_SRC] = imx_clk_mux("pll6_bypass_src", base + 0xe0, 14, 2, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[IMX6QDL_PLL7_BYPASS_SRC] = imx_clk_mux("pll7_bypass_src", base + 0x20, 14, 2, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+
+	/*                                    type               name    parent_name        base         div_mask */
+	clk[IMX6QDL_CLK_PLL1] = imx_clk_pllv3(IMX_PLLV3_SYS,     "pll1", "pll1_bypass_src", base + 0x00, 0x7f);
+	clk[IMX6QDL_CLK_PLL2] = imx_clk_pllv3(IMX_PLLV3_GENERIC, "pll2", "pll2_bypass_src", base + 0x30, 0x1);
+	clk[IMX6QDL_CLK_PLL3] = imx_clk_pllv3(IMX_PLLV3_USB,     "pll3", "pll3_bypass_src", base + 0x10, 0x3);
+	clk[IMX6QDL_CLK_PLL4] = imx_clk_pllv3(IMX_PLLV3_AV,      "pll4", "pll4_bypass_src", base + 0x70, 0x7f);
+	clk[IMX6QDL_CLK_PLL5] = imx_clk_pllv3(IMX_PLLV3_AV,      "pll5", "pll5_bypass_src", base + 0xa0, 0x7f);
+	clk[IMX6QDL_CLK_PLL6] = imx_clk_pllv3(IMX_PLLV3_ENET,    "pll6", "pll6_bypass_src", base + 0xe0, 0x3);
+	clk[IMX6QDL_CLK_PLL7] = imx_clk_pllv3(IMX_PLLV3_USB,     "pll7", "pll7_bypass_src", base + 0x20, 0x3);
+
+	clk[IMX6QDL_PLL1_BYPASS] = imx_clk_mux_flags("pll1_bypass", base + 0x00, 16, 1, pll1_bypass_sels, ARRAY_SIZE(pll1_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_PLL2_BYPASS] = imx_clk_mux_flags("pll2_bypass", base + 0x30, 16, 1, pll2_bypass_sels, ARRAY_SIZE(pll2_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_PLL3_BYPASS] = imx_clk_mux_flags("pll3_bypass", base + 0x10, 16, 1, pll3_bypass_sels, ARRAY_SIZE(pll3_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_PLL4_BYPASS] = imx_clk_mux_flags("pll4_bypass", base + 0x70, 16, 1, pll4_bypass_sels, ARRAY_SIZE(pll4_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_PLL5_BYPASS] = imx_clk_mux_flags("pll5_bypass", base + 0xa0, 16, 1, pll5_bypass_sels, ARRAY_SIZE(pll5_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_PLL6_BYPASS] = imx_clk_mux_flags("pll6_bypass", base + 0xe0, 16, 1, pll6_bypass_sels, ARRAY_SIZE(pll6_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_PLL7_BYPASS] = imx_clk_mux_flags("pll7_bypass", base + 0x20, 16, 1, pll7_bypass_sels, ARRAY_SIZE(pll7_bypass_sels), CLK_SET_RATE_PARENT);
+
+	/* Do not bypass PLLs initially */
+	clk_set_parent(clk[IMX6QDL_PLL1_BYPASS], clk[IMX6QDL_CLK_PLL1]);
+	clk_set_parent(clk[IMX6QDL_PLL2_BYPASS], clk[IMX6QDL_CLK_PLL2]);
+	clk_set_parent(clk[IMX6QDL_PLL3_BYPASS], clk[IMX6QDL_CLK_PLL3]);
+	clk_set_parent(clk[IMX6QDL_PLL4_BYPASS], clk[IMX6QDL_CLK_PLL4]);
+	clk_set_parent(clk[IMX6QDL_PLL5_BYPASS], clk[IMX6QDL_CLK_PLL5]);
+	clk_set_parent(clk[IMX6QDL_PLL6_BYPASS], clk[IMX6QDL_CLK_PLL6]);
+	clk_set_parent(clk[IMX6QDL_PLL7_BYPASS], clk[IMX6QDL_CLK_PLL7]);
+
+	clk[IMX6QDL_CLK_PLL1_SYS]      = imx_clk_gate("pll1_sys",      "pll1_bypass", base + 0x00, 13);
+	clk[IMX6QDL_CLK_PLL2_BUS]      = imx_clk_gate("pll2_bus",      "pll2_bypass", base + 0x30, 13);
+	clk[IMX6QDL_CLK_PLL3_USB_OTG]  = imx_clk_gate("pll3_usb_otg",  "pll3_bypass", base + 0x10, 13);
+	clk[IMX6QDL_CLK_PLL4_AUDIO]    = imx_clk_gate("pll4_audio",    "pll4_bypass", base + 0x70, 13);
+	clk[IMX6QDL_CLK_PLL5_VIDEO]    = imx_clk_gate("pll5_video",    "pll5_bypass", base + 0xa0, 13);
+	clk[IMX6QDL_CLK_PLL6_ENET]     = imx_clk_gate("pll6_enet",     "pll6_bypass", base + 0xe0, 13);
+	clk[IMX6QDL_CLK_PLL7_USB_HOST] = imx_clk_gate("pll7_usb_host", "pll7_bypass", base + 0x20, 13);
+
+	/*
+	 * Bit 20 is the reserved and read-only bit, we do this only for:
+	 * - Do nothing for usbphy clk_enable/disable
+	 * - Keep refcount when do usbphy clk_enable/disable, in that case,
+	 * the clk framework may need to enable/disable usbphy's parent
+	 */
+	clk[IMX6QDL_CLK_USBPHY1] = imx_clk_gate("usbphy1", "pll3_usb_otg", base + 0x10, 20);
+	clk[IMX6QDL_CLK_USBPHY2] = imx_clk_gate("usbphy2", "pll7_usb_host", base + 0x20, 20);
+
+	/*
+	 * usbphy*_gate needs to be on after system boots up, and software
+	 * never needs to control it anymore.
+	 */
+	clk[IMX6QDL_CLK_USBPHY1_GATE] = imx_clk_gate("usbphy1_gate", "dummy", base + 0x10, 6);
+	clk[IMX6QDL_CLK_USBPHY2_GATE] = imx_clk_gate("usbphy2_gate", "dummy", base + 0x20, 6);
+
+	clk[IMX6QDL_CLK_SATA_REF] = imx_clk_fixed_factor("sata_ref", "pll6_enet", 1, 5);
+	clk[IMX6QDL_CLK_PCIE_REF] = imx_clk_fixed_factor("pcie_ref", "pll6_enet", 1, 4);
+
+	clk[IMX6QDL_CLK_SATA_REF_100M] = imx_clk_gate("sata_ref_100m", "sata_ref", base + 0xe0, 20);
+	clk[IMX6QDL_CLK_PCIE_REF_125M] = imx_clk_gate("pcie_ref_125m", "pcie_ref", base + 0xe0, 19);
+
+	clk[IMX6QDL_CLK_ENET_REF] = clk_register_divider_table(NULL, "enet_ref", "pll6_enet", 0,
+			base + 0xe0, 0, 2, 0, clk_enet_ref_table,
+			&imx_ccm_lock);
+
+	clk[IMX6QDL_CLK_LVDS1_SEL] = imx_clk_mux("lvds1_sel", base + 0x160, 0, 5, lvds_sels, ARRAY_SIZE(lvds_sels));
+	clk[IMX6QDL_CLK_LVDS2_SEL] = imx_clk_mux("lvds2_sel", base + 0x160, 5, 5, lvds_sels, ARRAY_SIZE(lvds_sels));
+
+	/*
+	 * lvds1_gate and lvds2_gate are pseudo-gates.  Both can be
+	 * independently configured as clock inputs or outputs.  We treat
+	 * the "output_enable" bit as a gate, even though it's really just
+	 * enabling clock output.
+	 */
+	clk[IMX6QDL_CLK_LVDS1_GATE] = imx_clk_gate_exclusive("lvds1_gate", "lvds1_sel", base + 0x160, 10, BIT(12));
+	clk[IMX6QDL_CLK_LVDS2_GATE] = imx_clk_gate_exclusive("lvds2_gate", "lvds2_sel", base + 0x160, 11, BIT(13));
+
+	clk[IMX6QDL_CLK_LVDS1_IN] = imx_clk_gate_exclusive("lvds1_in", "anaclk1", base + 0x160, 12, BIT(10));
+	clk[IMX6QDL_CLK_LVDS2_IN] = imx_clk_gate_exclusive("lvds2_in", "anaclk2", base + 0x160, 13, BIT(11));
+
+	/*                                            name              parent_name        reg       idx */
+	clk[IMX6QDL_CLK_PLL2_PFD0_352M] = imx_clk_pfd("pll2_pfd0_352m", "pll2_bus",     base + 0x100, 0);
+	clk[IMX6QDL_CLK_PLL2_PFD1_594M] = imx_clk_pfd("pll2_pfd1_594m", "pll2_bus",     base + 0x100, 1);
+	clk[IMX6QDL_CLK_PLL2_PFD2_396M] = imx_clk_pfd("pll2_pfd2_396m", "pll2_bus",     base + 0x100, 2);
+	clk[IMX6QDL_CLK_PLL3_PFD0_720M] = imx_clk_pfd("pll3_pfd0_720m", "pll3_usb_otg", base + 0xf0,  0);
+	clk[IMX6QDL_CLK_PLL3_PFD1_540M] = imx_clk_pfd("pll3_pfd1_540m", "pll3_usb_otg", base + 0xf0,  1);
+	clk[IMX6QDL_CLK_PLL3_PFD2_508M] = imx_clk_pfd("pll3_pfd2_508m", "pll3_usb_otg", base + 0xf0,  2);
+	clk[IMX6QDL_CLK_PLL3_PFD3_454M] = imx_clk_pfd("pll3_pfd3_454m", "pll3_usb_otg", base + 0xf0,  3);
+
+	/*                                                name         parent_name     mult div */
+	clk[IMX6QDL_CLK_PLL2_198M] = imx_clk_fixed_factor("pll2_198m", "pll2_pfd2_396m", 1, 2);
+	clk[IMX6QDL_CLK_PLL3_120M] = imx_clk_fixed_factor("pll3_120m", "pll3_usb_otg",   1, 4);
+	clk[IMX6QDL_CLK_PLL3_80M]  = imx_clk_fixed_factor("pll3_80m",  "pll3_usb_otg",   1, 6);
+	clk[IMX6QDL_CLK_PLL3_60M]  = imx_clk_fixed_factor("pll3_60m",  "pll3_usb_otg",   1, 8);
+	clk[IMX6QDL_CLK_TWD]       = imx_clk_fixed_factor("twd",       "arm",            1, 2);
+	clk[IMX6QDL_CLK_GPT_3M]    = imx_clk_fixed_factor("gpt_3m",    "osc",            1, 8);
+	clk[IMX6QDL_CLK_VIDEO_27M] = imx_clk_fixed_factor("video_27m", "pll3_pfd1_540m", 1, 20);
+	if (clk_on_imx6dl()) {
+		clk[IMX6QDL_CLK_GPU2D_AXI] = imx_clk_fixed_factor("gpu2d_axi", "mmdc_ch0_axi_podf", 1, 1);
+		clk[IMX6QDL_CLK_GPU3D_AXI] = imx_clk_fixed_factor("gpu3d_axi", "mmdc_ch0_axi_podf", 1, 1);
+	}
+
+	clk[IMX6QDL_CLK_PLL4_POST_DIV] = clk_register_divider_table(NULL, "pll4_post_div", "pll4_audio", CLK_SET_RATE_PARENT, base + 0x70, 19, 2, 0, post_div_table, &imx_ccm_lock);
+	clk[IMX6QDL_CLK_PLL4_AUDIO_DIV] = clk_register_divider(NULL, "pll4_audio_div", "pll4_post_div", CLK_SET_RATE_PARENT, base + 0x170, 15, 1, 0, &imx_ccm_lock);
+	clk[IMX6QDL_CLK_PLL5_POST_DIV] = clk_register_divider_table(NULL, "pll5_post_div", "pll5_video", CLK_SET_RATE_PARENT, base + 0xa0, 19, 2, 0, post_div_table, &imx_ccm_lock);
+	clk[IMX6QDL_CLK_PLL5_VIDEO_DIV] = clk_register_divider_table(NULL, "pll5_video_div", "pll5_post_div", CLK_SET_RATE_PARENT, base + 0x170, 30, 2, 0, video_div_table, &imx_ccm_lock);
+
+	np = ccm_node;
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+
+	/*                                              name                reg       shift width parent_names     num_parents */
+	clk[IMX6QDL_CLK_STEP]             = imx_clk_mux("step",	            base + 0xc,  8,  1, step_sels,	   ARRAY_SIZE(step_sels));
+	clk[IMX6QDL_CLK_PLL1_SW]          = imx_clk_mux("pll1_sw",	    base + 0xc,  2,  1, pll1_sw_sels,      ARRAY_SIZE(pll1_sw_sels));
+	clk[IMX6QDL_CLK_PERIPH_PRE]       = imx_clk_mux("periph_pre",       base + 0x18, 18, 2, periph_pre_sels,   ARRAY_SIZE(periph_pre_sels));
+	clk[IMX6QDL_CLK_PERIPH2_PRE]      = imx_clk_mux("periph2_pre",      base + 0x18, 21, 2, periph_pre_sels,   ARRAY_SIZE(periph_pre_sels));
+	clk[IMX6QDL_CLK_PERIPH_CLK2_SEL]  = imx_clk_mux("periph_clk2_sel",  base + 0x18, 12, 2, periph_clk2_sels,  ARRAY_SIZE(periph_clk2_sels));
+	clk[IMX6QDL_CLK_PERIPH2_CLK2_SEL] = imx_clk_mux("periph2_clk2_sel", base + 0x18, 20, 1, periph2_clk2_sels, ARRAY_SIZE(periph2_clk2_sels));
+	clk[IMX6QDL_CLK_AXI_SEL]          = imx_clk_mux("axi_sel",          base + 0x14, 6,  2, axi_sels,          ARRAY_SIZE(axi_sels));
+	clk[IMX6QDL_CLK_ESAI_SEL]         = imx_clk_mux("esai_sel",         base + 0x20, 19, 2, audio_sels,        ARRAY_SIZE(audio_sels));
+	clk[IMX6QDL_CLK_ASRC_SEL]         = imx_clk_mux("asrc_sel",         base + 0x30, 7,  2, audio_sels,        ARRAY_SIZE(audio_sels));
+	clk[IMX6QDL_CLK_SPDIF_SEL]        = imx_clk_mux("spdif_sel",        base + 0x30, 20, 2, audio_sels,        ARRAY_SIZE(audio_sels));
+	if (clk_on_imx6q()) {
+		clk[IMX6QDL_CLK_GPU2D_AXI]        = imx_clk_mux("gpu2d_axi",        base + 0x18, 0,  1, gpu_axi_sels,      ARRAY_SIZE(gpu_axi_sels));
+		clk[IMX6QDL_CLK_GPU3D_AXI]        = imx_clk_mux("gpu3d_axi",        base + 0x18, 1,  1, gpu_axi_sels,      ARRAY_SIZE(gpu_axi_sels));
+	}
+	clk[IMX6QDL_CLK_GPU2D_CORE_SEL]   = imx_clk_mux("gpu2d_core_sel",   base + 0x18, 16, 2, gpu2d_core_sels,   ARRAY_SIZE(gpu2d_core_sels));
+	clk[IMX6QDL_CLK_GPU3D_CORE_SEL]   = imx_clk_mux("gpu3d_core_sel",   base + 0x18, 4,  2, gpu3d_core_sels,   ARRAY_SIZE(gpu3d_core_sels));
+	clk[IMX6QDL_CLK_GPU3D_SHADER_SEL] = imx_clk_mux("gpu3d_shader_sel", base + 0x18, 8,  2, gpu3d_shader_sels, ARRAY_SIZE(gpu3d_shader_sels));
+	clk[IMX6QDL_CLK_IPU1_SEL]         = imx_clk_mux("ipu1_sel",         base + 0x3c, 9,  2, ipu_sels,          ARRAY_SIZE(ipu_sels));
+	clk[IMX6QDL_CLK_IPU2_SEL]         = imx_clk_mux("ipu2_sel",         base + 0x3c, 14, 2, ipu_sels,          ARRAY_SIZE(ipu_sels));
+	clk[IMX6QDL_CLK_LDB_DI0_SEL]      = imx_clk_mux_flags("ldb_di0_sel", base + 0x2c, 9,  3, ldb_di_sels,      ARRAY_SIZE(ldb_di_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_LDB_DI1_SEL]      = imx_clk_mux_flags("ldb_di1_sel", base + 0x2c, 12, 3, ldb_di_sels,      ARRAY_SIZE(ldb_di_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_IPU1_DI0_PRE_SEL] = imx_clk_mux_flags("ipu1_di0_pre_sel", base + 0x34, 6,  3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_IPU1_DI1_PRE_SEL] = imx_clk_mux_flags("ipu1_di1_pre_sel", base + 0x34, 15, 3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_IPU2_DI0_PRE_SEL] = imx_clk_mux_flags("ipu2_di0_pre_sel", base + 0x38, 6,  3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_IPU2_DI1_PRE_SEL] = imx_clk_mux_flags("ipu2_di1_pre_sel", base + 0x38, 15, 3, ipu_di_pre_sels,   ARRAY_SIZE(ipu_di_pre_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_IPU1_DI0_SEL]     = imx_clk_mux_flags("ipu1_di0_sel",     base + 0x34, 0,  3, ipu1_di0_sels,     ARRAY_SIZE(ipu1_di0_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_IPU1_DI1_SEL]     = imx_clk_mux_flags("ipu1_di1_sel",     base + 0x34, 9,  3, ipu1_di1_sels,     ARRAY_SIZE(ipu1_di1_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_IPU2_DI0_SEL]     = imx_clk_mux_flags("ipu2_di0_sel",     base + 0x38, 0,  3, ipu2_di0_sels,     ARRAY_SIZE(ipu2_di0_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_IPU2_DI1_SEL]     = imx_clk_mux_flags("ipu2_di1_sel",     base + 0x38, 9,  3, ipu2_di1_sels,     ARRAY_SIZE(ipu2_di1_sels), CLK_SET_RATE_PARENT);
+	clk[IMX6QDL_CLK_HSI_TX_SEL]       = imx_clk_mux("hsi_tx_sel",       base + 0x30, 28, 1, hsi_tx_sels,       ARRAY_SIZE(hsi_tx_sels));
+	clk[IMX6QDL_CLK_PCIE_AXI_SEL]     = imx_clk_mux("pcie_axi_sel",     base + 0x18, 10, 1, pcie_axi_sels,     ARRAY_SIZE(pcie_axi_sels));
+	clk[IMX6QDL_CLK_SSI1_SEL]         = imx_clk_fixup_mux("ssi1_sel",   base + 0x1c, 10, 2, ssi_sels,          ARRAY_SIZE(ssi_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_SSI2_SEL]         = imx_clk_fixup_mux("ssi2_sel",   base + 0x1c, 12, 2, ssi_sels,          ARRAY_SIZE(ssi_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_SSI3_SEL]         = imx_clk_fixup_mux("ssi3_sel",   base + 0x1c, 14, 2, ssi_sels,          ARRAY_SIZE(ssi_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_USDHC1_SEL]       = imx_clk_fixup_mux("usdhc1_sel", base + 0x1c, 16, 1, usdhc_sels,        ARRAY_SIZE(usdhc_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_USDHC2_SEL]       = imx_clk_fixup_mux("usdhc2_sel", base + 0x1c, 17, 1, usdhc_sels,        ARRAY_SIZE(usdhc_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_USDHC3_SEL]       = imx_clk_fixup_mux("usdhc3_sel", base + 0x1c, 18, 1, usdhc_sels,        ARRAY_SIZE(usdhc_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_USDHC4_SEL]       = imx_clk_fixup_mux("usdhc4_sel", base + 0x1c, 19, 1, usdhc_sels,        ARRAY_SIZE(usdhc_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_ENFC_SEL]         = imx_clk_mux("enfc_sel",         base + 0x2c, 16, 2, enfc_sels,         ARRAY_SIZE(enfc_sels));
+	clk[IMX6QDL_CLK_EIM_SEL]          = imx_clk_fixup_mux("eim_sel",      base + 0x1c, 27, 2, eim_sels,        ARRAY_SIZE(eim_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_EIM_SLOW_SEL]     = imx_clk_fixup_mux("eim_slow_sel", base + 0x1c, 29, 2, eim_slow_sels,   ARRAY_SIZE(eim_slow_sels), imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_VDO_AXI_SEL]      = imx_clk_mux("vdo_axi_sel",      base + 0x18, 11, 1, vdo_axi_sels,      ARRAY_SIZE(vdo_axi_sels));
+	clk[IMX6QDL_CLK_VPU_AXI_SEL]      = imx_clk_mux("vpu_axi_sel",      base + 0x18, 14, 2, vpu_axi_sels,      ARRAY_SIZE(vpu_axi_sels));
+	clk[IMX6QDL_CLK_CKO1_SEL]         = imx_clk_mux("cko1_sel",         base + 0x60, 0,  4, cko1_sels,         ARRAY_SIZE(cko1_sels));
+	clk[IMX6QDL_CLK_CKO2_SEL]         = imx_clk_mux("cko2_sel",         base + 0x60, 16, 5, cko2_sels,         ARRAY_SIZE(cko2_sels));
+	clk[IMX6QDL_CLK_CKO]              = imx_clk_mux("cko",              base + 0x60, 8, 1,  cko_sels,          ARRAY_SIZE(cko_sels));
+
+	/*                                          name         reg      shift width busy: reg, shift parent_names  num_parents */
+	clk[IMX6QDL_CLK_PERIPH]  = imx_clk_busy_mux("periph",  base + 0x14, 25,  1,   base + 0x48, 5,  periph_sels,  ARRAY_SIZE(periph_sels));
+	clk[IMX6QDL_CLK_PERIPH2] = imx_clk_busy_mux("periph2", base + 0x14, 26,  1,   base + 0x48, 3,  periph2_sels, ARRAY_SIZE(periph2_sels));
+
+	/*                                                  name                parent_name          reg       shift width */
+	clk[IMX6QDL_CLK_PERIPH_CLK2]      = imx_clk_divider("periph_clk2",      "periph_clk2_sel",   base + 0x14, 27, 3);
+	clk[IMX6QDL_CLK_PERIPH2_CLK2]     = imx_clk_divider("periph2_clk2",     "periph2_clk2_sel",  base + 0x14, 0,  3);
+	clk[IMX6QDL_CLK_IPG]              = imx_clk_divider("ipg",              "ahb",               base + 0x14, 8,  2);
+	clk[IMX6QDL_CLK_IPG_PER]          = imx_clk_fixup_divider("ipg_per",    "ipg",               base + 0x1c, 0,  6, imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_ESAI_PRED]        = imx_clk_divider("esai_pred",        "esai_sel",          base + 0x28, 9,  3);
+	clk[IMX6QDL_CLK_ESAI_PODF]        = imx_clk_divider("esai_podf",        "esai_pred",         base + 0x28, 25, 3);
+	clk[IMX6QDL_CLK_ASRC_PRED]        = imx_clk_divider("asrc_pred",        "asrc_sel",          base + 0x30, 12, 3);
+	clk[IMX6QDL_CLK_ASRC_PODF]        = imx_clk_divider("asrc_podf",        "asrc_pred",         base + 0x30, 9,  3);
+	clk[IMX6QDL_CLK_SPDIF_PRED]       = imx_clk_divider("spdif_pred",       "spdif_sel",         base + 0x30, 25, 3);
+	clk[IMX6QDL_CLK_SPDIF_PODF]       = imx_clk_divider("spdif_podf",       "spdif_pred",        base + 0x30, 22, 3);
+	clk[IMX6QDL_CLK_CAN_ROOT]         = imx_clk_divider("can_root",         "pll3_60m",          base + 0x20, 2,  6);
+	clk[IMX6QDL_CLK_ECSPI_ROOT]       = imx_clk_divider("ecspi_root",       "pll3_60m",          base + 0x38, 19, 6);
+	clk[IMX6QDL_CLK_GPU2D_CORE_PODF]  = imx_clk_divider("gpu2d_core_podf",  "gpu2d_core_sel",    base + 0x18, 23, 3);
+	clk[IMX6QDL_CLK_GPU3D_CORE_PODF]  = imx_clk_divider("gpu3d_core_podf",  "gpu3d_core_sel",    base + 0x18, 26, 3);
+	clk[IMX6QDL_CLK_GPU3D_SHADER]     = imx_clk_divider("gpu3d_shader",     "gpu3d_shader_sel",  base + 0x18, 29, 3);
+	clk[IMX6QDL_CLK_IPU1_PODF]        = imx_clk_divider("ipu1_podf",        "ipu1_sel",          base + 0x3c, 11, 3);
+	clk[IMX6QDL_CLK_IPU2_PODF]        = imx_clk_divider("ipu2_podf",        "ipu2_sel",          base + 0x3c, 16, 3);
+	clk[IMX6QDL_CLK_LDB_DI0_DIV_3_5]  = imx_clk_fixed_factor("ldb_di0_div_3_5", "ldb_di0_sel", 2, 7);
+	clk[IMX6QDL_CLK_LDB_DI0_PODF]     = imx_clk_divider_flags("ldb_di0_podf", "ldb_di0_div_3_5", base + 0x20, 10, 1, 0);
+	clk[IMX6QDL_CLK_LDB_DI1_DIV_3_5]  = imx_clk_fixed_factor("ldb_di1_div_3_5", "ldb_di1_sel", 2, 7);
+	clk[IMX6QDL_CLK_LDB_DI1_PODF]     = imx_clk_divider_flags("ldb_di1_podf", "ldb_di1_div_3_5", base + 0x20, 11, 1, 0);
+	clk[IMX6QDL_CLK_IPU1_DI0_PRE]     = imx_clk_divider("ipu1_di0_pre",     "ipu1_di0_pre_sel",  base + 0x34, 3,  3);
+	clk[IMX6QDL_CLK_IPU1_DI1_PRE]     = imx_clk_divider("ipu1_di1_pre",     "ipu1_di1_pre_sel",  base + 0x34, 12, 3);
+	clk[IMX6QDL_CLK_IPU2_DI0_PRE]     = imx_clk_divider("ipu2_di0_pre",     "ipu2_di0_pre_sel",  base + 0x38, 3,  3);
+	clk[IMX6QDL_CLK_IPU2_DI1_PRE]     = imx_clk_divider("ipu2_di1_pre",     "ipu2_di1_pre_sel",  base + 0x38, 12, 3);
+	clk[IMX6QDL_CLK_HSI_TX_PODF]      = imx_clk_divider("hsi_tx_podf",      "hsi_tx_sel",        base + 0x30, 29, 3);
+	clk[IMX6QDL_CLK_SSI1_PRED]        = imx_clk_divider("ssi1_pred",        "ssi1_sel",          base + 0x28, 6,  3);
+	clk[IMX6QDL_CLK_SSI1_PODF]        = imx_clk_divider("ssi1_podf",        "ssi1_pred",         base + 0x28, 0,  6);
+	clk[IMX6QDL_CLK_SSI2_PRED]        = imx_clk_divider("ssi2_pred",        "ssi2_sel",          base + 0x2c, 6,  3);
+	clk[IMX6QDL_CLK_SSI2_PODF]        = imx_clk_divider("ssi2_podf",        "ssi2_pred",         base + 0x2c, 0,  6);
+	clk[IMX6QDL_CLK_SSI3_PRED]        = imx_clk_divider("ssi3_pred",        "ssi3_sel",          base + 0x28, 22, 3);
+	clk[IMX6QDL_CLK_SSI3_PODF]        = imx_clk_divider("ssi3_podf",        "ssi3_pred",         base + 0x28, 16, 6);
+	clk[IMX6QDL_CLK_UART_SERIAL_PODF] = imx_clk_divider("uart_serial_podf", "pll3_80m",          base + 0x24, 0,  6);
+	clk[IMX6QDL_CLK_USDHC1_PODF]      = imx_clk_divider("usdhc1_podf",      "usdhc1_sel",        base + 0x24, 11, 3);
+	clk[IMX6QDL_CLK_USDHC2_PODF]      = imx_clk_divider("usdhc2_podf",      "usdhc2_sel",        base + 0x24, 16, 3);
+	clk[IMX6QDL_CLK_USDHC3_PODF]      = imx_clk_divider("usdhc3_podf",      "usdhc3_sel",        base + 0x24, 19, 3);
+	clk[IMX6QDL_CLK_USDHC4_PODF]      = imx_clk_divider("usdhc4_podf",      "usdhc4_sel",        base + 0x24, 22, 3);
+	clk[IMX6QDL_CLK_ENFC_PRED]        = imx_clk_divider("enfc_pred",        "enfc_sel",          base + 0x2c, 18, 3);
+	clk[IMX6QDL_CLK_ENFC_PODF]        = imx_clk_divider("enfc_podf",        "enfc_pred",         base + 0x2c, 21, 6);
+	clk[IMX6QDL_CLK_EIM_PODF]         = imx_clk_fixup_divider("eim_podf",   "eim_sel",           base + 0x1c, 20, 3, imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_EIM_SLOW_PODF]    = imx_clk_fixup_divider("eim_slow_podf", "eim_slow_sel",   base + 0x1c, 23, 3, imx_cscmr1_fixup);
+	clk[IMX6QDL_CLK_VPU_AXI_PODF]     = imx_clk_divider("vpu_axi_podf",     "vpu_axi_sel",       base + 0x24, 25, 3);
+	clk[IMX6QDL_CLK_CKO1_PODF]        = imx_clk_divider("cko1_podf",        "cko1_sel",          base + 0x60, 4,  3);
+	clk[IMX6QDL_CLK_CKO2_PODF]        = imx_clk_divider("cko2_podf",        "cko2_sel",          base + 0x60, 21, 3);
+
+	/*                                                        name                 parent_name    reg        shift width busy: reg, shift */
+	clk[IMX6QDL_CLK_AXI]               = imx_clk_busy_divider("axi",               "axi_sel",     base + 0x14, 16,  3,   base + 0x48, 0);
+	clk[IMX6QDL_CLK_MMDC_CH0_AXI_PODF] = imx_clk_busy_divider("mmdc_ch0_axi_podf", "periph",      base + 0x14, 19,  3,   base + 0x48, 4);
+	clk[IMX6QDL_CLK_MMDC_CH1_AXI_PODF] = imx_clk_busy_divider("mmdc_ch1_axi_podf", "periph2",     base + 0x14, 3,   3,   base + 0x48, 2);
+	clk[IMX6QDL_CLK_ARM]               = imx_clk_busy_divider("arm",               "pll1_sw",     base + 0x10, 0,   3,   base + 0x48, 16);
+	clk[IMX6QDL_CLK_AHB]               = imx_clk_busy_divider("ahb",               "periph",      base + 0x14, 10,  3,   base + 0x48, 1);
+
+	/*                                            name             parent_name          reg         shift */
+	clk[IMX6QDL_CLK_APBH_DMA]     = imx_clk_gate2("apbh_dma",      "usdhc3",            base + 0x68, 4);
+	clk[IMX6QDL_CLK_ASRC]         = imx_clk_gate2_shared("asrc",         "asrc_podf",   base + 0x68, 6, &share_count_asrc);
+	clk[IMX6QDL_CLK_ASRC_IPG]     = imx_clk_gate2_shared("asrc_ipg",     "ahb",         base + 0x68, 6, &share_count_asrc);
+	clk[IMX6QDL_CLK_ASRC_MEM]     = imx_clk_gate2_shared("asrc_mem",     "ahb",         base + 0x68, 6, &share_count_asrc);
+	clk[IMX6QDL_CLK_CAN1_IPG]     = imx_clk_gate2("can1_ipg",      "ipg",               base + 0x68, 14);
+	clk[IMX6QDL_CLK_CAN1_SERIAL]  = imx_clk_gate2("can1_serial",   "can_root",          base + 0x68, 16);
+	clk[IMX6QDL_CLK_CAN2_IPG]     = imx_clk_gate2("can2_ipg",      "ipg",               base + 0x68, 18);
+	clk[IMX6QDL_CLK_CAN2_SERIAL]  = imx_clk_gate2("can2_serial",   "can_root",          base + 0x68, 20);
+	clk[IMX6QDL_CLK_ECSPI1]       = imx_clk_gate2("ecspi1",        "ecspi_root",        base + 0x6c, 0);
+	clk[IMX6QDL_CLK_ECSPI2]       = imx_clk_gate2("ecspi2",        "ecspi_root",        base + 0x6c, 2);
+	clk[IMX6QDL_CLK_ECSPI3]       = imx_clk_gate2("ecspi3",        "ecspi_root",        base + 0x6c, 4);
+	clk[IMX6QDL_CLK_ECSPI4]       = imx_clk_gate2("ecspi4",        "ecspi_root",        base + 0x6c, 6);
+	if (clk_on_imx6dl())
+		clk[IMX6DL_CLK_I2C4]  = imx_clk_gate2("i2c4",          "ipg_per",           base + 0x6c, 8);
+	else
+		clk[IMX6Q_CLK_ECSPI5] = imx_clk_gate2("ecspi5",        "ecspi_root",        base + 0x6c, 8);
+	clk[IMX6QDL_CLK_ENET]         = imx_clk_gate2("enet",          "ipg",               base + 0x6c, 10);
+	clk[IMX6QDL_CLK_ESAI_EXTAL]   = imx_clk_gate2_shared("esai_extal",   "esai_podf",   base + 0x6c, 16, &share_count_esai);
+	clk[IMX6QDL_CLK_ESAI_IPG]     = imx_clk_gate2_shared("esai_ipg",   "ahb",           base + 0x6c, 16, &share_count_esai);
+	clk[IMX6QDL_CLK_ESAI_MEM]     = imx_clk_gate2_shared("esai_mem", "ahb",             base + 0x6c, 16, &share_count_esai);
+	clk[IMX6QDL_CLK_GPT_IPG]      = imx_clk_gate2("gpt_ipg",       "ipg",               base + 0x6c, 20);
+	clk[IMX6QDL_CLK_GPT_IPG_PER]  = imx_clk_gate2("gpt_ipg_per",   "ipg_per",           base + 0x6c, 22);
+	if (clk_on_imx6dl())
+		/*
+		 * The multiplexer and divider of imx6q clock gpu3d_shader get
+		 * redefined/reused as gpu2d_core_sel and gpu2d_core_podf on imx6dl.
+		 */
+		clk[IMX6QDL_CLK_GPU2D_CORE] = imx_clk_gate2("gpu2d_core", "gpu3d_shader", base + 0x6c, 24);
+	else
+		clk[IMX6QDL_CLK_GPU2D_CORE] = imx_clk_gate2("gpu2d_core", "gpu2d_core_podf", base + 0x6c, 24);
+	clk[IMX6QDL_CLK_GPU3D_CORE]   = imx_clk_gate2("gpu3d_core",    "gpu3d_core_podf",   base + 0x6c, 26);
+	clk[IMX6QDL_CLK_HDMI_IAHB]    = imx_clk_gate2("hdmi_iahb",     "ahb",               base + 0x70, 0);
+	clk[IMX6QDL_CLK_HDMI_ISFR]    = imx_clk_gate2("hdmi_isfr",     "video_27m",         base + 0x70, 4);
+	clk[IMX6QDL_CLK_I2C1]         = imx_clk_gate2("i2c1",          "ipg_per",           base + 0x70, 6);
+	clk[IMX6QDL_CLK_I2C2]         = imx_clk_gate2("i2c2",          "ipg_per",           base + 0x70, 8);
+	clk[IMX6QDL_CLK_I2C3]         = imx_clk_gate2("i2c3",          "ipg_per",           base + 0x70, 10);
+	clk[IMX6QDL_CLK_IIM]          = imx_clk_gate2("iim",           "ipg",               base + 0x70, 12);
+	clk[IMX6QDL_CLK_ENFC]         = imx_clk_gate2("enfc",          "enfc_podf",         base + 0x70, 14);
+	clk[IMX6QDL_CLK_VDOA]         = imx_clk_gate2("vdoa",          "vdo_axi",           base + 0x70, 26);
+	clk[IMX6QDL_CLK_IPU1]         = imx_clk_gate2("ipu1",          "ipu1_podf",         base + 0x74, 0);
+	clk[IMX6QDL_CLK_IPU1_DI0]     = imx_clk_gate2("ipu1_di0",      "ipu1_di0_sel",      base + 0x74, 2);
+	clk[IMX6QDL_CLK_IPU1_DI1]     = imx_clk_gate2("ipu1_di1",      "ipu1_di1_sel",      base + 0x74, 4);
+	clk[IMX6QDL_CLK_IPU2]         = imx_clk_gate2("ipu2",          "ipu2_podf",         base + 0x74, 6);
+	clk[IMX6QDL_CLK_IPU2_DI0]     = imx_clk_gate2("ipu2_di0",      "ipu2_di0_sel",      base + 0x74, 8);
+	clk[IMX6QDL_CLK_LDB_DI0]      = imx_clk_gate2("ldb_di0",       "ldb_di0_podf",      base + 0x74, 12);
+	clk[IMX6QDL_CLK_LDB_DI1]      = imx_clk_gate2("ldb_di1",       "ldb_di1_podf",      base + 0x74, 14);
+	clk[IMX6QDL_CLK_IPU2_DI1]     = imx_clk_gate2("ipu2_di1",      "ipu2_di1_sel",      base + 0x74, 10);
+	clk[IMX6QDL_CLK_HSI_TX]       = imx_clk_gate2_shared("hsi_tx", "hsi_tx_podf",       base + 0x74, 16, &share_count_mipi_core_cfg);
+	clk[IMX6QDL_CLK_MIPI_CORE_CFG] = imx_clk_gate2_shared("mipi_core_cfg", "video_27m", base + 0x74, 16, &share_count_mipi_core_cfg);
+	clk[IMX6QDL_CLK_MIPI_IPG]     = imx_clk_gate2_shared("mipi_ipg", "ipg",             base + 0x74, 16, &share_count_mipi_core_cfg);
+	if (clk_on_imx6dl())
+		/*
+		 * The multiplexer and divider of the imx6q clock gpu2d get
+		 * redefined/reused as mlb_sys_sel and mlb_sys_clk_podf on imx6dl.
+		 */
+		clk[IMX6QDL_CLK_MLB] = imx_clk_gate2("mlb",            "gpu2d_core_podf",   base + 0x74, 18);
+	else
+		clk[IMX6QDL_CLK_MLB] = imx_clk_gate2("mlb",            "axi",               base + 0x74, 18);
+	clk[IMX6QDL_CLK_MMDC_CH0_AXI] = imx_clk_gate2("mmdc_ch0_axi",  "mmdc_ch0_axi_podf", base + 0x74, 20);
+	clk[IMX6QDL_CLK_MMDC_CH1_AXI] = imx_clk_gate2("mmdc_ch1_axi",  "mmdc_ch1_axi_podf", base + 0x74, 22);
+	clk[IMX6QDL_CLK_OCRAM]        = imx_clk_gate2("ocram",         "ahb",               base + 0x74, 28);
+	clk[IMX6QDL_CLK_OPENVG_AXI]   = imx_clk_gate2("openvg_axi",    "axi",               base + 0x74, 30);
+	clk[IMX6QDL_CLK_PCIE_AXI]     = imx_clk_gate2("pcie_axi",      "pcie_axi_sel",      base + 0x78, 0);
+	clk[IMX6QDL_CLK_PER1_BCH]     = imx_clk_gate2("per1_bch",      "usdhc3",            base + 0x78, 12);
+	clk[IMX6QDL_CLK_PWM1]         = imx_clk_gate2("pwm1",          "ipg_per",           base + 0x78, 16);
+	clk[IMX6QDL_CLK_PWM2]         = imx_clk_gate2("pwm2",          "ipg_per",           base + 0x78, 18);
+	clk[IMX6QDL_CLK_PWM3]         = imx_clk_gate2("pwm3",          "ipg_per",           base + 0x78, 20);
+	clk[IMX6QDL_CLK_PWM4]         = imx_clk_gate2("pwm4",          "ipg_per",           base + 0x78, 22);
+	clk[IMX6QDL_CLK_GPMI_BCH_APB] = imx_clk_gate2("gpmi_bch_apb",  "usdhc3",            base + 0x78, 24);
+	clk[IMX6QDL_CLK_GPMI_BCH]     = imx_clk_gate2("gpmi_bch",      "usdhc4",            base + 0x78, 26);
+	clk[IMX6QDL_CLK_GPMI_IO]      = imx_clk_gate2("gpmi_io",       "enfc",              base + 0x78, 28);
+	clk[IMX6QDL_CLK_GPMI_APB]     = imx_clk_gate2("gpmi_apb",      "usdhc3",            base + 0x78, 30);
+	clk[IMX6QDL_CLK_ROM]          = imx_clk_gate2("rom",           "ahb",               base + 0x7c, 0);
+	clk[IMX6QDL_CLK_SATA]         = imx_clk_gate2("sata",          "ahb",               base + 0x7c, 4);
+	clk[IMX6QDL_CLK_SDMA]         = imx_clk_gate2("sdma",          "ahb",               base + 0x7c, 6);
+	clk[IMX6QDL_CLK_SPBA]         = imx_clk_gate2("spba",          "ipg",               base + 0x7c, 12);
+	clk[IMX6QDL_CLK_SPDIF]        = imx_clk_gate2("spdif",         "spdif_podf",        base + 0x7c, 14);
+	clk[IMX6QDL_CLK_SSI1_IPG]     = imx_clk_gate2_shared("ssi1_ipg",      "ipg",        base + 0x7c, 18, &share_count_ssi1);
+	clk[IMX6QDL_CLK_SSI2_IPG]     = imx_clk_gate2_shared("ssi2_ipg",      "ipg",        base + 0x7c, 20, &share_count_ssi2);
+	clk[IMX6QDL_CLK_SSI3_IPG]     = imx_clk_gate2_shared("ssi3_ipg",      "ipg",        base + 0x7c, 22, &share_count_ssi3);
+	clk[IMX6QDL_CLK_SSI1]         = imx_clk_gate2_shared("ssi1",          "ssi1_podf",  base + 0x7c, 18, &share_count_ssi1);
+	clk[IMX6QDL_CLK_SSI2]         = imx_clk_gate2_shared("ssi2",          "ssi2_podf",  base + 0x7c, 20, &share_count_ssi2);
+	clk[IMX6QDL_CLK_SSI3]         = imx_clk_gate2_shared("ssi3",          "ssi3_podf",  base + 0x7c, 22, &share_count_ssi3);
+	clk[IMX6QDL_CLK_UART_IPG]     = imx_clk_gate2("uart_ipg",      "ipg",               base + 0x7c, 24);
+	clk[IMX6QDL_CLK_UART_SERIAL]  = imx_clk_gate2("uart_serial",   "uart_serial_podf",  base + 0x7c, 26);
+	clk[IMX6QDL_CLK_USBOH3]       = imx_clk_gate2("usboh3",        "ipg",               base + 0x80, 0);
+	clk[IMX6QDL_CLK_USDHC1]       = imx_clk_gate2("usdhc1",        "usdhc1_podf",       base + 0x80, 2);
+	clk[IMX6QDL_CLK_USDHC2]       = imx_clk_gate2("usdhc2",        "usdhc2_podf",       base + 0x80, 4);
+	clk[IMX6QDL_CLK_USDHC3]       = imx_clk_gate2("usdhc3",        "usdhc3_podf",       base + 0x80, 6);
+	clk[IMX6QDL_CLK_USDHC4]       = imx_clk_gate2("usdhc4",        "usdhc4_podf",       base + 0x80, 8);
+	clk[IMX6QDL_CLK_EIM_SLOW]     = imx_clk_gate2("eim_slow",      "eim_slow_podf",     base + 0x80, 10);
+	clk[IMX6QDL_CLK_VDO_AXI]      = imx_clk_gate2("vdo_axi",       "vdo_axi_sel",       base + 0x80, 12);
+	clk[IMX6QDL_CLK_VPU_AXI]      = imx_clk_gate2("vpu_axi",       "vpu_axi_podf",      base + 0x80, 14);
+	clk[IMX6QDL_CLK_CKO1]         = imx_clk_gate("cko1",           "cko1_podf",         base + 0x60, 7);
+	clk[IMX6QDL_CLK_CKO2]         = imx_clk_gate("cko2",           "cko2_podf",         base + 0x60, 24);
+
+	/*
+	 * The gpt_3m clock is not available on i.MX6Q TO1.0.  Let's point it
+	 * to clock gpt_ipg_per to ease the gpt driver code.
+	 */
+	if (clk_on_imx6q() && imx_get_soc_revision() == IMX_CHIP_REVISION_1_0)
+		clk[IMX6QDL_CLK_GPT_3M] = clk[IMX6QDL_CLK_GPT_IPG_PER];
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	clk_register_clkdev(clk[IMX6QDL_CLK_ENET_REF], "enet_ref", NULL);
+
+	if ((imx_get_soc_revision() != IMX_CHIP_REVISION_1_0) ||
+	    clk_on_imx6dl()) {
+		clk_set_parent(clk[IMX6QDL_CLK_LDB_DI0_SEL], clk[IMX6QDL_CLK_PLL5_VIDEO_DIV]);
+		clk_set_parent(clk[IMX6QDL_CLK_LDB_DI1_SEL], clk[IMX6QDL_CLK_PLL5_VIDEO_DIV]);
+	}
+
+	clk_set_parent(clk[IMX6QDL_CLK_IPU1_DI0_PRE_SEL], clk[IMX6QDL_CLK_PLL5_VIDEO_DIV]);
+	clk_set_parent(clk[IMX6QDL_CLK_IPU1_DI1_PRE_SEL], clk[IMX6QDL_CLK_PLL5_VIDEO_DIV]);
+	clk_set_parent(clk[IMX6QDL_CLK_IPU2_DI0_PRE_SEL], clk[IMX6QDL_CLK_PLL5_VIDEO_DIV]);
+	clk_set_parent(clk[IMX6QDL_CLK_IPU2_DI1_PRE_SEL], clk[IMX6QDL_CLK_PLL5_VIDEO_DIV]);
+	clk_set_parent(clk[IMX6QDL_CLK_IPU1_DI0_SEL], clk[IMX6QDL_CLK_IPU1_DI0_PRE]);
+	clk_set_parent(clk[IMX6QDL_CLK_IPU1_DI1_SEL], clk[IMX6QDL_CLK_IPU1_DI1_PRE]);
+	clk_set_parent(clk[IMX6QDL_CLK_IPU2_DI0_SEL], clk[IMX6QDL_CLK_IPU2_DI0_PRE]);
+	clk_set_parent(clk[IMX6QDL_CLK_IPU2_DI1_SEL], clk[IMX6QDL_CLK_IPU2_DI1_PRE]);
+
+	/*
+	 * The gpmi needs 100MHz frequency in the EDO/Sync mode,
+	 * We can not get the 100MHz from the pll2_pfd0_352m.
+	 * So choose pll2_pfd2_396m as enfc_sel's parent.
+	 */
+	clk_set_parent(clk[IMX6QDL_CLK_ENFC_SEL], clk[IMX6QDL_CLK_PLL2_PFD2_396M]);
+
+	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
+		clk_prepare_enable(clk[clks_init_on[i]]);
+
+	if (IS_ENABLED(CONFIG_USB_MXS_PHY)) {
+		clk_prepare_enable(clk[IMX6QDL_CLK_USBPHY1_GATE]);
+		clk_prepare_enable(clk[IMX6QDL_CLK_USBPHY2_GATE]);
+	}
+
+	/*
+	 * Let's initially set up CLKO with OSC24M, since this configuration
+	 * is widely used by imx6q board designs to clock audio codec.
+	 */
+	ret = clk_set_parent(clk[IMX6QDL_CLK_CKO2_SEL], clk[IMX6QDL_CLK_OSC]);
+	if (!ret)
+		ret = clk_set_parent(clk[IMX6QDL_CLK_CKO], clk[IMX6QDL_CLK_CKO2]);
+	if (ret)
+		pr_warn("failed to set up CLKO: %d\n", ret);
+
+	/* Audio-related clocks configuration */
+	clk_set_parent(clk[IMX6QDL_CLK_SPDIF_SEL], clk[IMX6QDL_CLK_PLL3_PFD3_454M]);
+
+	/* All existing boards with PCIe use LVDS1 */
+	if (IS_ENABLED(CONFIG_PCI_IMX6))
+		clk_set_parent(clk[IMX6QDL_CLK_LVDS1_SEL], clk[IMX6QDL_CLK_SATA_REF_100M]);
+}
+CLK_OF_DECLARE(imx6q, "fsl,imx6q-ccm", imx6q_clocks_init);
diff --git a/drivers/clk/imx/clk-imx6sl.c b/drivers/clk/imx/clk-imx6sl.c
new file mode 100644
index 000000000000..a0d4cf26cfa9
--- /dev/null
+++ b/drivers/clk/imx/clk-imx6sl.c
@@ -0,0 +1,443 @@
+/*
+ * Copyright 2013-2014 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <dt-bindings/clock/imx6sl-clock.h>
+
+#include "clk.h"
+
+#define CCSR			0xc
+#define BM_CCSR_PLL1_SW_CLK_SEL	(1 << 2)
+#define CACRR			0x10
+#define CDHIPR			0x48
+#define BM_CDHIPR_ARM_PODF_BUSY	(1 << 16)
+#define ARM_WAIT_DIV_396M	2
+#define ARM_WAIT_DIV_792M	4
+#define ARM_WAIT_DIV_996M	6
+
+#define PLL_ARM			0x0
+#define BM_PLL_ARM_DIV_SELECT	(0x7f << 0)
+#define BM_PLL_ARM_POWERDOWN	(1 << 12)
+#define BM_PLL_ARM_ENABLE	(1 << 13)
+#define BM_PLL_ARM_LOCK		(1 << 31)
+#define PLL_ARM_DIV_792M	66
+
+static const char *step_sels[]		= { "osc", "pll2_pfd2", };
+static const char *pll1_sw_sels[]	= { "pll1_sys", "step", };
+static const char *ocram_alt_sels[]	= { "pll2_pfd2", "pll3_pfd1", };
+static const char *ocram_sels[]		= { "periph", "ocram_alt_sels", };
+static const char *pre_periph_sels[]	= { "pll2_bus", "pll2_pfd2", "pll2_pfd0", "pll2_198m", };
+static const char *periph_clk2_sels[]	= { "pll3_usb_otg", "osc", "osc", "dummy", };
+static const char *periph2_clk2_sels[]	= { "pll3_usb_otg", "pll2_bus", };
+static const char *periph_sels[]	= { "pre_periph_sel", "periph_clk2_podf", };
+static const char *periph2_sels[]	= { "pre_periph2_sel", "periph2_clk2_podf", };
+static const char *csi_sels[]		= { "osc", "pll2_pfd2", "pll3_120m", "pll3_pfd1", };
+static const char *lcdif_axi_sels[]	= { "pll2_bus", "pll2_pfd2", "pll3_usb_otg", "pll3_pfd1", };
+static const char *usdhc_sels[]		= { "pll2_pfd2", "pll2_pfd0", };
+static const char *ssi_sels[]		= { "pll3_pfd2", "pll3_pfd3", "pll4_audio_div", "dummy", };
+static const char *perclk_sels[]	= { "ipg", "osc", };
+static const char *pxp_axi_sels[]	= { "pll2_bus", "pll3_usb_otg", "pll5_video_div", "pll2_pfd0", "pll2_pfd2", "pll3_pfd3", };
+static const char *epdc_axi_sels[]	= { "pll2_bus", "pll3_usb_otg", "pll5_video_div", "pll2_pfd0", "pll2_pfd2", "pll3_pfd2", };
+static const char *gpu2d_ovg_sels[]	= { "pll3_pfd1", "pll3_usb_otg", "pll2_bus", "pll2_pfd2", };
+static const char *gpu2d_sels[]		= { "pll2_pfd2", "pll3_usb_otg", "pll3_pfd1", "pll2_bus", };
+static const char *lcdif_pix_sels[]	= { "pll2_bus", "pll3_usb_otg", "pll5_video_div", "pll2_pfd0", "pll3_pfd0", "pll3_pfd1", };
+static const char *epdc_pix_sels[]	= { "pll2_bus", "pll3_usb_otg", "pll5_video_div", "pll2_pfd0", "pll2_pfd1", "pll3_pfd1", };
+static const char *audio_sels[]		= { "pll4_audio_div", "pll3_pfd2", "pll3_pfd3", "pll3_usb_otg", };
+static const char *ecspi_sels[]		= { "pll3_60m", "osc", };
+static const char *uart_sels[]		= { "pll3_80m", "osc", };
+static const char *lvds_sels[]		= {
+	"pll1_sys", "pll2_bus", "pll2_pfd0", "pll2_pfd1", "pll2_pfd2", "dummy", "pll4_audio", "pll5_video",
+	"dummy", "enet_ref", "dummy", "dummy", "pll3_usb_otg", "pll7_usb_host", "pll3_pfd0", "pll3_pfd1",
+	"pll3_pfd2", "pll3_pfd3", "osc", "dummy", "dummy", "dummy", "dummy", "dummy",
+	 "dummy", "dummy", "dummy", "dummy", "dummy", "dummy", "dummy", "dummy",
+};
+static const char *pll_bypass_src_sels[] = { "osc", "lvds1_in", };
+static const char *pll1_bypass_sels[]	= { "pll1", "pll1_bypass_src", };
+static const char *pll2_bypass_sels[]	= { "pll2", "pll2_bypass_src", };
+static const char *pll3_bypass_sels[]	= { "pll3", "pll3_bypass_src", };
+static const char *pll4_bypass_sels[]	= { "pll4", "pll4_bypass_src", };
+static const char *pll5_bypass_sels[]	= { "pll5", "pll5_bypass_src", };
+static const char *pll6_bypass_sels[]	= { "pll6", "pll6_bypass_src", };
+static const char *pll7_bypass_sels[]	= { "pll7", "pll7_bypass_src", };
+
+static struct clk_div_table clk_enet_ref_table[] = {
+	{ .val = 0, .div = 20, },
+	{ .val = 1, .div = 10, },
+	{ .val = 2, .div = 5, },
+	{ .val = 3, .div = 4, },
+	{ }
+};
+
+static struct clk_div_table post_div_table[] = {
+	{ .val = 2, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 0, .div = 4, },
+	{ }
+};
+
+static struct clk_div_table video_div_table[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 2, .div = 1, },
+	{ .val = 3, .div = 4, },
+	{ }
+};
+
+static unsigned int share_count_ssi1;
+static unsigned int share_count_ssi2;
+static unsigned int share_count_ssi3;
+
+static struct clk *clks[IMX6SL_CLK_END];
+static struct clk_onecell_data clk_data;
+static void __iomem *ccm_base;
+static void __iomem *anatop_base;
+
+static const u32 clks_init_on[] __initconst = {
+	IMX6SL_CLK_IPG, IMX6SL_CLK_ARM, IMX6SL_CLK_MMDC_ROOT,
+};
+
+/*
+ * ERR005311 CCM: After exit from WAIT mode, unwanted interrupt(s) taken
+ *           during WAIT mode entry process could cause cache memory
+ *           corruption.
+ *
+ * Software workaround:
+ *     To prevent this issue from occurring, software should ensure that the
+ * ARM to IPG clock ratio is less than 12:5 (that is < 2.4x), before
+ * entering WAIT mode.
+ *
+ * This function will set the ARM clk to max value within the 12:5 limit.
+ * As IPG clock is fixed at 66MHz(so ARM freq must not exceed 158.4MHz),
+ * ARM freq are one of below setpoints: 396MHz, 792MHz and 996MHz, since
+ * the clk APIs can NOT be called in idle thread(may cause kernel schedule
+ * as there is sleep function in PLL wait function), so here we just slow
+ * down ARM to below freq according to previous freq:
+ *
+ * run mode      wait mode
+ * 396MHz   ->   132MHz;
+ * 792MHz   ->   158.4MHz;
+ * 996MHz   ->   142.3MHz;
+ */
+static int imx6sl_get_arm_divider_for_wait(void)
+{
+	if (readl_relaxed(ccm_base + CCSR) & BM_CCSR_PLL1_SW_CLK_SEL) {
+		return ARM_WAIT_DIV_396M;
+	} else {
+		if ((readl_relaxed(anatop_base + PLL_ARM) &
+			BM_PLL_ARM_DIV_SELECT) == PLL_ARM_DIV_792M)
+			return ARM_WAIT_DIV_792M;
+		else
+			return ARM_WAIT_DIV_996M;
+	}
+}
+
+static void imx6sl_enable_pll_arm(bool enable)
+{
+	static u32 saved_pll_arm;
+	u32 val;
+
+	if (enable) {
+		saved_pll_arm = val = readl_relaxed(anatop_base + PLL_ARM);
+		val |= BM_PLL_ARM_ENABLE;
+		val &= ~BM_PLL_ARM_POWERDOWN;
+		writel_relaxed(val, anatop_base + PLL_ARM);
+		while (!(__raw_readl(anatop_base + PLL_ARM) & BM_PLL_ARM_LOCK))
+			;
+	} else {
+		 writel_relaxed(saved_pll_arm, anatop_base + PLL_ARM);
+	}
+}
+
+void imx6sl_set_wait_clk(bool enter)
+{
+	static unsigned long saved_arm_div;
+	int arm_div_for_wait = imx6sl_get_arm_divider_for_wait();
+
+	/*
+	 * According to hardware design, arm podf change need
+	 * PLL1 clock enabled.
+	 */
+	if (arm_div_for_wait == ARM_WAIT_DIV_396M)
+		imx6sl_enable_pll_arm(true);
+
+	if (enter) {
+		saved_arm_div = readl_relaxed(ccm_base + CACRR);
+		writel_relaxed(arm_div_for_wait, ccm_base + CACRR);
+	} else {
+		writel_relaxed(saved_arm_div, ccm_base + CACRR);
+	}
+	while (__raw_readl(ccm_base + CDHIPR) & BM_CDHIPR_ARM_PODF_BUSY)
+		;
+
+	if (arm_div_for_wait == ARM_WAIT_DIV_396M)
+		imx6sl_enable_pll_arm(false);
+}
+
+static void __init imx6sl_clocks_init(struct device_node *ccm_node)
+{
+	struct device_node *np;
+	void __iomem *base;
+	int i;
+	int ret;
+
+	clks[IMX6SL_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+	clks[IMX6SL_CLK_CKIL] = imx_obtain_fixed_clock("ckil", 0);
+	clks[IMX6SL_CLK_OSC] = imx_obtain_fixed_clock("osc", 0);
+	/* Clock source from external clock via CLK1 PAD */
+	clks[IMX6SL_CLK_ANACLK1] = imx_obtain_fixed_clock("anaclk1", 0);
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx6sl-anatop");
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+	anatop_base = base;
+
+	clks[IMX6SL_PLL1_BYPASS_SRC] = imx_clk_mux("pll1_bypass_src", base + 0x00, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SL_PLL2_BYPASS_SRC] = imx_clk_mux("pll2_bypass_src", base + 0x30, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SL_PLL3_BYPASS_SRC] = imx_clk_mux("pll3_bypass_src", base + 0x10, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SL_PLL4_BYPASS_SRC] = imx_clk_mux("pll4_bypass_src", base + 0x70, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SL_PLL5_BYPASS_SRC] = imx_clk_mux("pll5_bypass_src", base + 0xa0, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SL_PLL6_BYPASS_SRC] = imx_clk_mux("pll6_bypass_src", base + 0xe0, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SL_PLL7_BYPASS_SRC] = imx_clk_mux("pll7_bypass_src", base + 0x20, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+
+	/*                                    type               name    parent_name        base         div_mask */
+	clks[IMX6SL_CLK_PLL1] = imx_clk_pllv3(IMX_PLLV3_SYS,     "pll1", "pll1_bypass_src", base + 0x00, 0x7f);
+	clks[IMX6SL_CLK_PLL2] = imx_clk_pllv3(IMX_PLLV3_GENERIC, "pll2", "pll2_bypass_src", base + 0x30, 0x1);
+	clks[IMX6SL_CLK_PLL3] = imx_clk_pllv3(IMX_PLLV3_USB,     "pll3", "pll3_bypass_src", base + 0x10, 0x3);
+	clks[IMX6SL_CLK_PLL4] = imx_clk_pllv3(IMX_PLLV3_AV,      "pll4", "pll4_bypass_src", base + 0x70, 0x7f);
+	clks[IMX6SL_CLK_PLL5] = imx_clk_pllv3(IMX_PLLV3_AV,      "pll5", "pll5_bypass_src", base + 0xa0, 0x7f);
+	clks[IMX6SL_CLK_PLL6] = imx_clk_pllv3(IMX_PLLV3_ENET,    "pll6", "pll6_bypass_src", base + 0xe0, 0x3);
+	clks[IMX6SL_CLK_PLL7] = imx_clk_pllv3(IMX_PLLV3_USB,     "pll7", "pll7_bypass_src", base + 0x20, 0x3);
+
+	clks[IMX6SL_PLL1_BYPASS] = imx_clk_mux_flags("pll1_bypass", base + 0x00, 16, 1, pll1_bypass_sels, ARRAY_SIZE(pll1_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SL_PLL2_BYPASS] = imx_clk_mux_flags("pll2_bypass", base + 0x30, 16, 1, pll2_bypass_sels, ARRAY_SIZE(pll2_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SL_PLL3_BYPASS] = imx_clk_mux_flags("pll3_bypass", base + 0x10, 16, 1, pll3_bypass_sels, ARRAY_SIZE(pll3_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SL_PLL4_BYPASS] = imx_clk_mux_flags("pll4_bypass", base + 0x70, 16, 1, pll4_bypass_sels, ARRAY_SIZE(pll4_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SL_PLL5_BYPASS] = imx_clk_mux_flags("pll5_bypass", base + 0xa0, 16, 1, pll5_bypass_sels, ARRAY_SIZE(pll5_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SL_PLL6_BYPASS] = imx_clk_mux_flags("pll6_bypass", base + 0xe0, 16, 1, pll6_bypass_sels, ARRAY_SIZE(pll6_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SL_PLL7_BYPASS] = imx_clk_mux_flags("pll7_bypass", base + 0x20, 16, 1, pll7_bypass_sels, ARRAY_SIZE(pll7_bypass_sels), CLK_SET_RATE_PARENT);
+
+	/* Do not bypass PLLs initially */
+	clk_set_parent(clks[IMX6SL_PLL1_BYPASS], clks[IMX6SL_CLK_PLL1]);
+	clk_set_parent(clks[IMX6SL_PLL2_BYPASS], clks[IMX6SL_CLK_PLL2]);
+	clk_set_parent(clks[IMX6SL_PLL3_BYPASS], clks[IMX6SL_CLK_PLL3]);
+	clk_set_parent(clks[IMX6SL_PLL4_BYPASS], clks[IMX6SL_CLK_PLL4]);
+	clk_set_parent(clks[IMX6SL_PLL5_BYPASS], clks[IMX6SL_CLK_PLL5]);
+	clk_set_parent(clks[IMX6SL_PLL6_BYPASS], clks[IMX6SL_CLK_PLL6]);
+	clk_set_parent(clks[IMX6SL_PLL7_BYPASS], clks[IMX6SL_CLK_PLL7]);
+
+	clks[IMX6SL_CLK_PLL1_SYS]      = imx_clk_gate("pll1_sys",      "pll1_bypass", base + 0x00, 13);
+	clks[IMX6SL_CLK_PLL2_BUS]      = imx_clk_gate("pll2_bus",      "pll2_bypass", base + 0x30, 13);
+	clks[IMX6SL_CLK_PLL3_USB_OTG]  = imx_clk_gate("pll3_usb_otg",  "pll3_bypass", base + 0x10, 13);
+	clks[IMX6SL_CLK_PLL4_AUDIO]    = imx_clk_gate("pll4_audio",    "pll4_bypass", base + 0x70, 13);
+	clks[IMX6SL_CLK_PLL5_VIDEO]    = imx_clk_gate("pll5_video",    "pll5_bypass", base + 0xa0, 13);
+	clks[IMX6SL_CLK_PLL6_ENET]     = imx_clk_gate("pll6_enet",     "pll6_bypass", base + 0xe0, 13);
+	clks[IMX6SL_CLK_PLL7_USB_HOST] = imx_clk_gate("pll7_usb_host", "pll7_bypass", base + 0x20, 13);
+
+	clks[IMX6SL_CLK_LVDS1_SEL] = imx_clk_mux("lvds1_sel", base + 0x160, 0, 5, lvds_sels, ARRAY_SIZE(lvds_sels));
+	clks[IMX6SL_CLK_LVDS1_OUT] = imx_clk_gate_exclusive("lvds1_out", "lvds1_sel", base + 0x160, 10, BIT(12));
+	clks[IMX6SL_CLK_LVDS1_IN] = imx_clk_gate_exclusive("lvds1_in", "anaclk1", base + 0x160, 12, BIT(10));
+
+	/*
+	 * usbphy1 and usbphy2 are implemented as dummy gates using reserve
+	 * bit 20.  They are used by phy driver to keep the refcount of
+	 * parent PLL correct. usbphy1_gate and usbphy2_gate only needs to be
+	 * turned on during boot, and software will not need to control it
+	 * anymore after that.
+	 */
+	clks[IMX6SL_CLK_USBPHY1]      = imx_clk_gate("usbphy1",      "pll3_usb_otg",  base + 0x10, 20);
+	clks[IMX6SL_CLK_USBPHY2]      = imx_clk_gate("usbphy2",      "pll7_usb_host", base + 0x20, 20);
+	clks[IMX6SL_CLK_USBPHY1_GATE] = imx_clk_gate("usbphy1_gate", "dummy",         base + 0x10, 6);
+	clks[IMX6SL_CLK_USBPHY2_GATE] = imx_clk_gate("usbphy2_gate", "dummy",         base + 0x20, 6);
+
+	/*                                                           dev   name              parent_name      flags                reg        shift width div: flags, div_table lock */
+	clks[IMX6SL_CLK_PLL4_POST_DIV]  = clk_register_divider_table(NULL, "pll4_post_div",  "pll4_audio",    CLK_SET_RATE_PARENT, base + 0x70,  19, 2,   0, post_div_table, &imx_ccm_lock);
+	clks[IMX6SL_CLK_PLL4_AUDIO_DIV] =       clk_register_divider(NULL, "pll4_audio_div", "pll4_post_div", CLK_SET_RATE_PARENT, base + 0x170, 15, 1,   0, &imx_ccm_lock);
+	clks[IMX6SL_CLK_PLL5_POST_DIV]  = clk_register_divider_table(NULL, "pll5_post_div",  "pll5_video",    CLK_SET_RATE_PARENT, base + 0xa0,  19, 2,   0, post_div_table, &imx_ccm_lock);
+	clks[IMX6SL_CLK_PLL5_VIDEO_DIV] = clk_register_divider_table(NULL, "pll5_video_div", "pll5_post_div", CLK_SET_RATE_PARENT, base + 0x170, 30, 2,   0, video_div_table, &imx_ccm_lock);
+	clks[IMX6SL_CLK_ENET_REF]       = clk_register_divider_table(NULL, "enet_ref",       "pll6_enet",     0,                   base + 0xe0,  0,  2,   0, clk_enet_ref_table, &imx_ccm_lock);
+
+	/*                                       name         parent_name     reg           idx */
+	clks[IMX6SL_CLK_PLL2_PFD0] = imx_clk_pfd("pll2_pfd0", "pll2_bus",     base + 0x100, 0);
+	clks[IMX6SL_CLK_PLL2_PFD1] = imx_clk_pfd("pll2_pfd1", "pll2_bus",     base + 0x100, 1);
+	clks[IMX6SL_CLK_PLL2_PFD2] = imx_clk_pfd("pll2_pfd2", "pll2_bus",     base + 0x100, 2);
+	clks[IMX6SL_CLK_PLL3_PFD0] = imx_clk_pfd("pll3_pfd0", "pll3_usb_otg", base + 0xf0,  0);
+	clks[IMX6SL_CLK_PLL3_PFD1] = imx_clk_pfd("pll3_pfd1", "pll3_usb_otg", base + 0xf0,  1);
+	clks[IMX6SL_CLK_PLL3_PFD2] = imx_clk_pfd("pll3_pfd2", "pll3_usb_otg", base + 0xf0,  2);
+	clks[IMX6SL_CLK_PLL3_PFD3] = imx_clk_pfd("pll3_pfd3", "pll3_usb_otg", base + 0xf0,  3);
+
+	/*                                                name         parent_name     mult div */
+	clks[IMX6SL_CLK_PLL2_198M] = imx_clk_fixed_factor("pll2_198m", "pll2_pfd2",      1, 2);
+	clks[IMX6SL_CLK_PLL3_120M] = imx_clk_fixed_factor("pll3_120m", "pll3_usb_otg",   1, 4);
+	clks[IMX6SL_CLK_PLL3_80M]  = imx_clk_fixed_factor("pll3_80m",  "pll3_usb_otg",   1, 6);
+	clks[IMX6SL_CLK_PLL3_60M]  = imx_clk_fixed_factor("pll3_60m",  "pll3_usb_otg",   1, 8);
+
+	np = ccm_node;
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+	ccm_base = base;
+
+	/*                                              name                reg       shift width parent_names     num_parents */
+	clks[IMX6SL_CLK_STEP]             = imx_clk_mux("step",             base + 0xc,  8,  1, step_sels,         ARRAY_SIZE(step_sels));
+	clks[IMX6SL_CLK_PLL1_SW]          = imx_clk_mux("pll1_sw",          base + 0xc,  2,  1, pll1_sw_sels,      ARRAY_SIZE(pll1_sw_sels));
+	clks[IMX6SL_CLK_OCRAM_ALT_SEL]    = imx_clk_mux("ocram_alt_sel",    base + 0x14, 7,  1, ocram_alt_sels,    ARRAY_SIZE(ocram_alt_sels));
+	clks[IMX6SL_CLK_OCRAM_SEL]        = imx_clk_mux("ocram_sel",        base + 0x14, 6,  1, ocram_sels,        ARRAY_SIZE(ocram_sels));
+	clks[IMX6SL_CLK_PRE_PERIPH2_SEL]  = imx_clk_mux("pre_periph2_sel",  base + 0x18, 21, 2, pre_periph_sels,   ARRAY_SIZE(pre_periph_sels));
+	clks[IMX6SL_CLK_PRE_PERIPH_SEL]   = imx_clk_mux("pre_periph_sel",   base + 0x18, 18, 2, pre_periph_sels,   ARRAY_SIZE(pre_periph_sels));
+	clks[IMX6SL_CLK_PERIPH2_CLK2_SEL] = imx_clk_mux("periph2_clk2_sel", base + 0x18, 20, 1, periph2_clk2_sels, ARRAY_SIZE(periph2_clk2_sels));
+	clks[IMX6SL_CLK_PERIPH_CLK2_SEL]  = imx_clk_mux("periph_clk2_sel",  base + 0x18, 12, 2, periph_clk2_sels,  ARRAY_SIZE(periph_clk2_sels));
+	clks[IMX6SL_CLK_CSI_SEL]          = imx_clk_mux("csi_sel",          base + 0x3c, 9,  2, csi_sels,          ARRAY_SIZE(csi_sels));
+	clks[IMX6SL_CLK_LCDIF_AXI_SEL]    = imx_clk_mux("lcdif_axi_sel",    base + 0x3c, 14, 2, lcdif_axi_sels,    ARRAY_SIZE(lcdif_axi_sels));
+	clks[IMX6SL_CLK_USDHC1_SEL]       = imx_clk_fixup_mux("usdhc1_sel", base + 0x1c, 16, 1, usdhc_sels,        ARRAY_SIZE(usdhc_sels),  imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_USDHC2_SEL]       = imx_clk_fixup_mux("usdhc2_sel", base + 0x1c, 17, 1, usdhc_sels,        ARRAY_SIZE(usdhc_sels),  imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_USDHC3_SEL]       = imx_clk_fixup_mux("usdhc3_sel", base + 0x1c, 18, 1, usdhc_sels,        ARRAY_SIZE(usdhc_sels),  imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_USDHC4_SEL]       = imx_clk_fixup_mux("usdhc4_sel", base + 0x1c, 19, 1, usdhc_sels,        ARRAY_SIZE(usdhc_sels),  imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_SSI1_SEL]         = imx_clk_fixup_mux("ssi1_sel",   base + 0x1c, 10, 2, ssi_sels,          ARRAY_SIZE(ssi_sels),    imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_SSI2_SEL]         = imx_clk_fixup_mux("ssi2_sel",   base + 0x1c, 12, 2, ssi_sels,          ARRAY_SIZE(ssi_sels),    imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_SSI3_SEL]         = imx_clk_fixup_mux("ssi3_sel",   base + 0x1c, 14, 2, ssi_sels,          ARRAY_SIZE(ssi_sels),    imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_PERCLK_SEL]       = imx_clk_fixup_mux("perclk_sel", base + 0x1c, 6,  1, perclk_sels,       ARRAY_SIZE(perclk_sels), imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_PXP_AXI_SEL]      = imx_clk_mux("pxp_axi_sel",      base + 0x34, 6,  3, pxp_axi_sels,      ARRAY_SIZE(pxp_axi_sels));
+	clks[IMX6SL_CLK_EPDC_AXI_SEL]     = imx_clk_mux("epdc_axi_sel",     base + 0x34, 15, 3, epdc_axi_sels,     ARRAY_SIZE(epdc_axi_sels));
+	clks[IMX6SL_CLK_GPU2D_OVG_SEL]    = imx_clk_mux("gpu2d_ovg_sel",    base + 0x18, 4,  2, gpu2d_ovg_sels,    ARRAY_SIZE(gpu2d_ovg_sels));
+	clks[IMX6SL_CLK_GPU2D_SEL]        = imx_clk_mux("gpu2d_sel",        base + 0x18, 8,  2, gpu2d_sels,        ARRAY_SIZE(gpu2d_sels));
+	clks[IMX6SL_CLK_LCDIF_PIX_SEL]    = imx_clk_mux("lcdif_pix_sel",    base + 0x38, 6,  3, lcdif_pix_sels,    ARRAY_SIZE(lcdif_pix_sels));
+	clks[IMX6SL_CLK_EPDC_PIX_SEL]     = imx_clk_mux("epdc_pix_sel",     base + 0x38, 15, 3, epdc_pix_sels,     ARRAY_SIZE(epdc_pix_sels));
+	clks[IMX6SL_CLK_SPDIF0_SEL]       = imx_clk_mux("spdif0_sel",       base + 0x30, 20, 2, audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SL_CLK_SPDIF1_SEL]       = imx_clk_mux("spdif1_sel",       base + 0x30, 7,  2, audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SL_CLK_EXTERN_AUDIO_SEL] = imx_clk_mux("extern_audio_sel", base + 0x20, 19, 2, audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SL_CLK_ECSPI_SEL]        = imx_clk_mux("ecspi_sel",        base + 0x38, 18, 1, ecspi_sels,        ARRAY_SIZE(ecspi_sels));
+	clks[IMX6SL_CLK_UART_SEL]         = imx_clk_mux("uart_sel",         base + 0x24, 6,  1, uart_sels,         ARRAY_SIZE(uart_sels));
+
+	/*                                          name       reg        shift width busy: reg, shift parent_names  num_parents */
+	clks[IMX6SL_CLK_PERIPH]  = imx_clk_busy_mux("periph",  base + 0x14, 25,  1,   base + 0x48, 5,  periph_sels,  ARRAY_SIZE(periph_sels));
+	clks[IMX6SL_CLK_PERIPH2] = imx_clk_busy_mux("periph2", base + 0x14, 26,  1,   base + 0x48, 3,  periph2_sels, ARRAY_SIZE(periph2_sels));
+
+	/*                                                   name                 parent_name          reg       shift width */
+	clks[IMX6SL_CLK_OCRAM_PODF]        = imx_clk_divider("ocram_podf",        "ocram_sel",         base + 0x14, 16, 3);
+	clks[IMX6SL_CLK_PERIPH_CLK2_PODF]  = imx_clk_divider("periph_clk2_podf",  "periph_clk2_sel",   base + 0x14, 27, 3);
+	clks[IMX6SL_CLK_PERIPH2_CLK2_PODF] = imx_clk_divider("periph2_clk2_podf", "periph2_clk2_sel",  base + 0x14, 0,  3);
+	clks[IMX6SL_CLK_IPG]               = imx_clk_divider("ipg",               "ahb",               base + 0x14, 8,  2);
+	clks[IMX6SL_CLK_CSI_PODF]          = imx_clk_divider("csi_podf",          "csi_sel",           base + 0x3c, 11, 3);
+	clks[IMX6SL_CLK_LCDIF_AXI_PODF]    = imx_clk_divider("lcdif_axi_podf",    "lcdif_axi_sel",     base + 0x3c, 16, 3);
+	clks[IMX6SL_CLK_USDHC1_PODF]       = imx_clk_divider("usdhc1_podf",       "usdhc1_sel",        base + 0x24, 11, 3);
+	clks[IMX6SL_CLK_USDHC2_PODF]       = imx_clk_divider("usdhc2_podf",       "usdhc2_sel",        base + 0x24, 16, 3);
+	clks[IMX6SL_CLK_USDHC3_PODF]       = imx_clk_divider("usdhc3_podf",       "usdhc3_sel",        base + 0x24, 19, 3);
+	clks[IMX6SL_CLK_USDHC4_PODF]       = imx_clk_divider("usdhc4_podf",       "usdhc4_sel",        base + 0x24, 22, 3);
+	clks[IMX6SL_CLK_SSI1_PRED]         = imx_clk_divider("ssi1_pred",         "ssi1_sel",          base + 0x28, 6,  3);
+	clks[IMX6SL_CLK_SSI1_PODF]         = imx_clk_divider("ssi1_podf",         "ssi1_pred",         base + 0x28, 0,  6);
+	clks[IMX6SL_CLK_SSI2_PRED]         = imx_clk_divider("ssi2_pred",         "ssi2_sel",          base + 0x2c, 6,  3);
+	clks[IMX6SL_CLK_SSI2_PODF]         = imx_clk_divider("ssi2_podf",         "ssi2_pred",         base + 0x2c, 0,  6);
+	clks[IMX6SL_CLK_SSI3_PRED]         = imx_clk_divider("ssi3_pred",         "ssi3_sel",          base + 0x28, 22, 3);
+	clks[IMX6SL_CLK_SSI3_PODF]         = imx_clk_divider("ssi3_podf",         "ssi3_pred",         base + 0x28, 16, 6);
+	clks[IMX6SL_CLK_PERCLK]            = imx_clk_fixup_divider("perclk",      "perclk_sel",        base + 0x1c, 0,  6, imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_PXP_AXI_PODF]      = imx_clk_divider("pxp_axi_podf",      "pxp_axi_sel",       base + 0x34, 3,  3);
+	clks[IMX6SL_CLK_EPDC_AXI_PODF]     = imx_clk_divider("epdc_axi_podf",     "epdc_axi_sel",      base + 0x34, 12, 3);
+	clks[IMX6SL_CLK_GPU2D_OVG_PODF]    = imx_clk_divider("gpu2d_ovg_podf",    "gpu2d_ovg_sel",     base + 0x18, 26, 3);
+	clks[IMX6SL_CLK_GPU2D_PODF]        = imx_clk_divider("gpu2d_podf",        "gpu2d_sel",         base + 0x18, 29, 3);
+	clks[IMX6SL_CLK_LCDIF_PIX_PRED]    = imx_clk_divider("lcdif_pix_pred",    "lcdif_pix_sel",     base + 0x38, 3,  3);
+	clks[IMX6SL_CLK_EPDC_PIX_PRED]     = imx_clk_divider("epdc_pix_pred",     "epdc_pix_sel",      base + 0x38, 12, 3);
+	clks[IMX6SL_CLK_LCDIF_PIX_PODF]    = imx_clk_fixup_divider("lcdif_pix_podf", "lcdif_pix_pred", base + 0x1c, 20, 3, imx_cscmr1_fixup);
+	clks[IMX6SL_CLK_EPDC_PIX_PODF]     = imx_clk_divider("epdc_pix_podf",     "epdc_pix_pred",     base + 0x18, 23, 3);
+	clks[IMX6SL_CLK_SPDIF0_PRED]       = imx_clk_divider("spdif0_pred",       "spdif0_sel",        base + 0x30, 25, 3);
+	clks[IMX6SL_CLK_SPDIF0_PODF]       = imx_clk_divider("spdif0_podf",       "spdif0_pred",       base + 0x30, 22, 3);
+	clks[IMX6SL_CLK_SPDIF1_PRED]       = imx_clk_divider("spdif1_pred",       "spdif1_sel",        base + 0x30, 12, 3);
+	clks[IMX6SL_CLK_SPDIF1_PODF]       = imx_clk_divider("spdif1_podf",       "spdif1_pred",       base + 0x30, 9,  3);
+	clks[IMX6SL_CLK_EXTERN_AUDIO_PRED] = imx_clk_divider("extern_audio_pred", "extern_audio_sel",  base + 0x28, 9,  3);
+	clks[IMX6SL_CLK_EXTERN_AUDIO_PODF] = imx_clk_divider("extern_audio_podf", "extern_audio_pred", base + 0x28, 25, 3);
+	clks[IMX6SL_CLK_ECSPI_ROOT]        = imx_clk_divider("ecspi_root",        "ecspi_sel",         base + 0x38, 19, 6);
+	clks[IMX6SL_CLK_UART_ROOT]         = imx_clk_divider("uart_root",         "uart_sel",          base + 0x24, 0,  6);
+
+	/*                                                name         parent_name reg       shift width busy: reg, shift */
+	clks[IMX6SL_CLK_AHB]       = imx_clk_busy_divider("ahb",       "periph",  base + 0x14, 10, 3,    base + 0x48, 1);
+	clks[IMX6SL_CLK_MMDC_ROOT] = imx_clk_busy_divider("mmdc",      "periph2", base + 0x14, 3,  3,    base + 0x48, 2);
+	clks[IMX6SL_CLK_ARM]       = imx_clk_busy_divider("arm",       "pll1_sw", base + 0x10, 0,  3,    base + 0x48, 16);
+
+	/*                                            name            parent_name          reg         shift */
+	clks[IMX6SL_CLK_ECSPI1]       = imx_clk_gate2("ecspi1",       "ecspi_root",        base + 0x6c, 0);
+	clks[IMX6SL_CLK_ECSPI2]       = imx_clk_gate2("ecspi2",       "ecspi_root",        base + 0x6c, 2);
+	clks[IMX6SL_CLK_ECSPI3]       = imx_clk_gate2("ecspi3",       "ecspi_root",        base + 0x6c, 4);
+	clks[IMX6SL_CLK_ECSPI4]       = imx_clk_gate2("ecspi4",       "ecspi_root",        base + 0x6c, 6);
+	clks[IMX6SL_CLK_ENET]         = imx_clk_gate2("enet",         "ipg",               base + 0x6c, 10);
+	clks[IMX6SL_CLK_EPIT1]        = imx_clk_gate2("epit1",        "perclk",            base + 0x6c, 12);
+	clks[IMX6SL_CLK_EPIT2]        = imx_clk_gate2("epit2",        "perclk",            base + 0x6c, 14);
+	clks[IMX6SL_CLK_EXTERN_AUDIO] = imx_clk_gate2("extern_audio", "extern_audio_podf", base + 0x6c, 16);
+	clks[IMX6SL_CLK_GPT]          = imx_clk_gate2("gpt",          "perclk",            base + 0x6c, 20);
+	clks[IMX6SL_CLK_GPT_SERIAL]   = imx_clk_gate2("gpt_serial",   "perclk",            base + 0x6c, 22);
+	clks[IMX6SL_CLK_GPU2D_OVG]    = imx_clk_gate2("gpu2d_ovg",    "gpu2d_ovg_podf",    base + 0x6c, 26);
+	clks[IMX6SL_CLK_I2C1]         = imx_clk_gate2("i2c1",         "perclk",            base + 0x70, 6);
+	clks[IMX6SL_CLK_I2C2]         = imx_clk_gate2("i2c2",         "perclk",            base + 0x70, 8);
+	clks[IMX6SL_CLK_I2C3]         = imx_clk_gate2("i2c3",         "perclk",            base + 0x70, 10);
+	clks[IMX6SL_CLK_OCOTP]        = imx_clk_gate2("ocotp",        "ipg",               base + 0x70, 12);
+	clks[IMX6SL_CLK_CSI]          = imx_clk_gate2("csi",          "csi_podf",          base + 0x74, 0);
+	clks[IMX6SL_CLK_PXP_AXI]      = imx_clk_gate2("pxp_axi",      "pxp_axi_podf",      base + 0x74, 2);
+	clks[IMX6SL_CLK_EPDC_AXI]     = imx_clk_gate2("epdc_axi",     "epdc_axi_podf",     base + 0x74, 4);
+	clks[IMX6SL_CLK_LCDIF_AXI]    = imx_clk_gate2("lcdif_axi",    "lcdif_axi_podf",    base + 0x74, 6);
+	clks[IMX6SL_CLK_LCDIF_PIX]    = imx_clk_gate2("lcdif_pix",    "lcdif_pix_podf",    base + 0x74, 8);
+	clks[IMX6SL_CLK_EPDC_PIX]     = imx_clk_gate2("epdc_pix",     "epdc_pix_podf",     base + 0x74, 10);
+	clks[IMX6SL_CLK_OCRAM]        = imx_clk_gate2("ocram",        "ocram_podf",        base + 0x74, 28);
+	clks[IMX6SL_CLK_PWM1]         = imx_clk_gate2("pwm1",         "perclk",            base + 0x78, 16);
+	clks[IMX6SL_CLK_PWM2]         = imx_clk_gate2("pwm2",         "perclk",            base + 0x78, 18);
+	clks[IMX6SL_CLK_PWM3]         = imx_clk_gate2("pwm3",         "perclk",            base + 0x78, 20);
+	clks[IMX6SL_CLK_PWM4]         = imx_clk_gate2("pwm4",         "perclk",            base + 0x78, 22);
+	clks[IMX6SL_CLK_SDMA]         = imx_clk_gate2("sdma",         "ipg",               base + 0x7c, 6);
+	clks[IMX6SL_CLK_SPBA]         = imx_clk_gate2("spba",         "ipg",               base + 0x7c, 12);
+	clks[IMX6SL_CLK_SPDIF]        = imx_clk_gate2("spdif",        "spdif0_podf",       base + 0x7c, 14);
+	clks[IMX6SL_CLK_SSI1_IPG]     = imx_clk_gate2_shared("ssi1_ipg",     "ipg",        base + 0x7c, 18, &share_count_ssi1);
+	clks[IMX6SL_CLK_SSI2_IPG]     = imx_clk_gate2_shared("ssi2_ipg",     "ipg",        base + 0x7c, 20, &share_count_ssi2);
+	clks[IMX6SL_CLK_SSI3_IPG]     = imx_clk_gate2_shared("ssi3_ipg",     "ipg",        base + 0x7c, 22, &share_count_ssi3);
+	clks[IMX6SL_CLK_SSI1]         = imx_clk_gate2_shared("ssi1",         "ssi1_podf",  base + 0x7c, 18, &share_count_ssi1);
+	clks[IMX6SL_CLK_SSI2]         = imx_clk_gate2_shared("ssi2",         "ssi2_podf",  base + 0x7c, 20, &share_count_ssi2);
+	clks[IMX6SL_CLK_SSI3]         = imx_clk_gate2_shared("ssi3",         "ssi3_podf",  base + 0x7c, 22, &share_count_ssi3);
+	clks[IMX6SL_CLK_UART]         = imx_clk_gate2("uart",         "ipg",               base + 0x7c, 24);
+	clks[IMX6SL_CLK_UART_SERIAL]  = imx_clk_gate2("uart_serial",  "uart_root",         base + 0x7c, 26);
+	clks[IMX6SL_CLK_USBOH3]       = imx_clk_gate2("usboh3",       "ipg",               base + 0x80, 0);
+	clks[IMX6SL_CLK_USDHC1]       = imx_clk_gate2("usdhc1",       "usdhc1_podf",       base + 0x80, 2);
+	clks[IMX6SL_CLK_USDHC2]       = imx_clk_gate2("usdhc2",       "usdhc2_podf",       base + 0x80, 4);
+	clks[IMX6SL_CLK_USDHC3]       = imx_clk_gate2("usdhc3",       "usdhc3_podf",       base + 0x80, 6);
+	clks[IMX6SL_CLK_USDHC4]       = imx_clk_gate2("usdhc4",       "usdhc4_podf",       base + 0x80, 8);
+
+	imx_check_clocks(clks, ARRAY_SIZE(clks));
+
+	clk_data.clks = clks;
+	clk_data.clk_num = ARRAY_SIZE(clks);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	/* Ensure the AHB clk is at 132MHz. */
+	ret = clk_set_rate(clks[IMX6SL_CLK_AHB], 132000000);
+	if (ret)
+		pr_warn("%s: failed to set AHB clock rate %d!\n",
+			__func__, ret);
+
+	/*
+	 * Make sure those always on clocks are enabled to maintain the correct
+	 * usecount and enabling/disabling of parent PLLs.
+	 */
+	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
+		clk_prepare_enable(clks[clks_init_on[i]]);
+
+	if (IS_ENABLED(CONFIG_USB_MXS_PHY)) {
+		clk_prepare_enable(clks[IMX6SL_CLK_USBPHY1_GATE]);
+		clk_prepare_enable(clks[IMX6SL_CLK_USBPHY2_GATE]);
+	}
+
+	/* Audio-related clocks configuration */
+	clk_set_parent(clks[IMX6SL_CLK_SPDIF0_SEL], clks[IMX6SL_CLK_PLL3_PFD3]);
+
+	/* set PLL5 video as lcdif pix parent clock */
+	clk_set_parent(clks[IMX6SL_CLK_LCDIF_PIX_SEL],
+			clks[IMX6SL_CLK_PLL5_VIDEO_DIV]);
+
+	clk_set_parent(clks[IMX6SL_CLK_LCDIF_AXI_SEL],
+		       clks[IMX6SL_CLK_PLL2_PFD2]);
+}
+CLK_OF_DECLARE(imx6sl, "fsl,imx6sl-ccm", imx6sl_clocks_init);
diff --git a/drivers/clk/imx/clk-imx6sx.c b/drivers/clk/imx/clk-imx6sx.c
new file mode 100644
index 000000000000..5b95c2c2bf52
--- /dev/null
+++ b/drivers/clk/imx/clk-imx6sx.c
@@ -0,0 +1,561 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <dt-bindings/clock/imx6sx-clock.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/types.h>
+
+#include "clk.h"
+
+#define CCDR    0x4
+#define BM_CCM_CCDR_MMDC_CH0_MASK       (0x2 << 16)
+
+static const char *step_sels[]		= { "osc", "pll2_pfd2_396m", };
+static const char *pll1_sw_sels[]	= { "pll1_sys", "step", };
+static const char *periph_pre_sels[]	= { "pll2_bus", "pll2_pfd2_396m", "pll2_pfd0_352m", "pll2_198m", };
+static const char *periph2_pre_sels[]	= { "pll2_bus", "pll2_pfd2_396m", "pll2_pfd0_352m", "pll4_audio_div", };
+static const char *periph_clk2_sels[]	= { "pll3_usb_otg", "osc", "osc", };
+static const char *periph2_clk2_sels[]	= { "pll3_usb_otg", "osc", };
+static const char *periph_sels[]	= { "periph_pre", "periph_clk2", };
+static const char *periph2_sels[]	= { "periph2_pre", "periph2_clk2", };
+static const char *ocram_sels[]		= { "periph", "pll2_pfd2_396m", "periph", "pll3_pfd1_540m", };
+static const char *audio_sels[]		= { "pll4_audio_div", "pll3_pfd2_508m", "pll5_video_div", "pll3_usb_otg", };
+static const char *gpu_axi_sels[]	= { "pll2_pfd2_396m", "pll3_pfd0_720m", "pll3_pfd1_540m", "pll2_bus", };
+static const char *gpu_core_sels[]	= { "pll3_pfd1_540m", "pll3_pfd0_720m", "pll2_bus", "pll2_pfd2_396m", };
+static const char *ldb_di0_div_sels[]	= { "ldb_di0_div_3_5", "ldb_di0_div_7", };
+static const char *ldb_di1_div_sels[]	= { "ldb_di1_div_3_5", "ldb_di1_div_7", };
+static const char *ldb_di0_sels[]	= { "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll2_pfd3_594m", "pll2_pfd1_594m", "pll3_pfd3_454m", };
+static const char *ldb_di1_sels[]	= { "pll3_usb_otg", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll2_bus", "pll3_pfd3_454m", "pll3_pfd2_508m", };
+static const char *pcie_axi_sels[]	= { "axi", "ahb", };
+static const char *ssi_sels[]		= { "pll3_pfd2_508m", "pll5_video_div", "pll4_audio_div", };
+static const char *qspi1_sels[]		= { "pll3_usb_otg", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll2_bus", "pll3_pfd3_454m", "pll3_pfd2_508m", };
+static const char *perclk_sels[]	= { "ipg", "osc", };
+static const char *usdhc_sels[]		= { "pll2_pfd2_396m", "pll2_pfd0_352m", };
+static const char *vid_sels[]		= { "pll3_pfd1_540m", "pll3_usb_otg", "pll3_pfd3_454m", "pll4_audio_div", "pll5_video_div", };
+static const char *can_sels[]		= { "pll3_60m", "osc", "pll3_80m", "dummy", };
+static const char *uart_sels[]		= { "pll3_80m", "osc", };
+static const char *qspi2_sels[]		= { "pll2_pfd0_352m", "pll2_bus", "pll3_usb_otg", "pll2_pfd2_396m", "pll3_pfd3_454m", "dummy", "dummy", "dummy", };
+static const char *enet_pre_sels[]	= { "pll2_bus", "pll3_usb_otg", "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll3_pfd2_508m", };
+static const char *enet_sels[]		= { "enet_podf", "ipp_di0", "ipp_di1", "ldb_di0", "ldb_di1", };
+static const char *m4_pre_sels[]	= { "pll2_bus", "pll3_usb_otg", "osc", "pll2_pfd0_352m", "pll2_pfd2_396m", "pll3_pfd3_454m", };
+static const char *m4_sels[]		= { "m4_pre_sel", "ipp_di0", "ipp_di1", "ldb_di0", "ldb_di1", };
+static const char *eim_slow_sels[]	= { "ocram", "pll3_usb_otg", "pll2_pfd2_396m", "pll2_pfd0_352m", };
+static const char *ecspi_sels[]		= { "pll3_60m", "osc", };
+static const char *lcdif1_pre_sels[]	= { "pll2_bus", "pll3_pfd3_454m", "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd1_594m", "pll3_pfd1_540m", };
+static const char *lcdif1_sels[]	= { "lcdif1_podf", "ipp_di0", "ipp_di1", "ldb_di0", "ldb_di1", };
+static const char *lcdif2_pre_sels[]	= { "pll2_bus", "pll3_pfd3_454m", "pll5_video_div", "pll2_pfd0_352m", "pll2_pfd3_594m", "pll3_pfd1_540m", };
+static const char *lcdif2_sels[]	= { "lcdif2_podf", "ipp_di0", "ipp_di1", "ldb_di0", "ldb_di1", };
+static const char *display_sels[]	= { "pll2_bus", "pll2_pfd2_396m", "pll3_usb_otg", "pll3_pfd1_540m", };
+static const char *csi_sels[]		= { "osc", "pll2_pfd2_396m", "pll3_120m", "pll3_pfd1_540m", };
+static const char *cko1_sels[]		= {
+	"pll3_usb_otg", "pll2_bus", "pll1_sys", "pll5_video_div",
+	"dummy", "ocram", "dummy", "pxp_axi", "epdc_axi", "lcdif_pix",
+	"epdc_pix", "ahb", "ipg", "perclk", "ckil", "pll4_audio_div",
+};
+static const char *cko2_sels[]		= {
+	"dummy", "mmdc_p0_fast", "usdhc4", "usdhc1", "dummy", "wrck",
+	"ecspi_root", "dummy", "usdhc3", "pcie", "arm", "csi_core",
+	"lcdif_axi", "dummy", "osc", "dummy", "gpu2d_ovg_core",
+	"usdhc2", "ssi1", "ssi2", "ssi3", "gpu2d_core", "dummy",
+	"dummy", "dummy", "dummy", "esai_extal", "eim_slow", "uart_serial",
+	"spdif", "asrc", "dummy",
+};
+static const char *cko_sels[] = { "cko1", "cko2", };
+static const char *lvds_sels[]	= {
+	"arm", "pll1_sys", "dummy", "dummy", "dummy", "dummy", "dummy", "pll5_video_div",
+	"dummy", "dummy", "pcie_ref_125m", "dummy", "usbphy1", "usbphy2",
+};
+static const char *pll_bypass_src_sels[] = { "osc", "lvds1_in", };
+static const char *pll1_bypass_sels[] = { "pll1", "pll1_bypass_src", };
+static const char *pll2_bypass_sels[] = { "pll2", "pll2_bypass_src", };
+static const char *pll3_bypass_sels[] = { "pll3", "pll3_bypass_src", };
+static const char *pll4_bypass_sels[] = { "pll4", "pll4_bypass_src", };
+static const char *pll5_bypass_sels[] = { "pll5", "pll5_bypass_src", };
+static const char *pll6_bypass_sels[] = { "pll6", "pll6_bypass_src", };
+static const char *pll7_bypass_sels[] = { "pll7", "pll7_bypass_src", };
+
+static struct clk *clks[IMX6SX_CLK_CLK_END];
+static struct clk_onecell_data clk_data;
+
+static int const clks_init_on[] __initconst = {
+	IMX6SX_CLK_AIPS_TZ1, IMX6SX_CLK_AIPS_TZ2, IMX6SX_CLK_AIPS_TZ3,
+	IMX6SX_CLK_IPMUX1, IMX6SX_CLK_IPMUX2, IMX6SX_CLK_IPMUX3,
+	IMX6SX_CLK_WAKEUP, IMX6SX_CLK_MMDC_P0_FAST, IMX6SX_CLK_MMDC_P0_IPG,
+	IMX6SX_CLK_ROM, IMX6SX_CLK_ARM, IMX6SX_CLK_IPG, IMX6SX_CLK_OCRAM,
+	IMX6SX_CLK_PER2_MAIN, IMX6SX_CLK_PERCLK, IMX6SX_CLK_M4,
+	IMX6SX_CLK_QSPI1, IMX6SX_CLK_QSPI2, IMX6SX_CLK_UART_IPG,
+	IMX6SX_CLK_UART_SERIAL, IMX6SX_CLK_I2C3, IMX6SX_CLK_ECSPI5,
+	IMX6SX_CLK_CAN1_IPG, IMX6SX_CLK_CAN1_SERIAL, IMX6SX_CLK_CAN2_IPG,
+	IMX6SX_CLK_CAN2_SERIAL, IMX6SX_CLK_CANFD, IMX6SX_CLK_EPIT1,
+	IMX6SX_CLK_EPIT2,
+};
+
+static struct clk_div_table clk_enet_ref_table[] = {
+	{ .val = 0, .div = 20, },
+	{ .val = 1, .div = 10, },
+	{ .val = 2, .div = 5, },
+	{ .val = 3, .div = 4, },
+	{ }
+};
+
+static struct clk_div_table post_div_table[] = {
+	{ .val = 2, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 0, .div = 4, },
+	{ }
+};
+
+static struct clk_div_table video_div_table[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 2, .div = 1, },
+	{ .val = 3, .div = 4, },
+	{ }
+};
+
+static u32 share_count_asrc;
+static u32 share_count_audio;
+static u32 share_count_esai;
+static u32 share_count_ssi1;
+static u32 share_count_ssi2;
+static u32 share_count_ssi3;
+
+static void __init imx6sx_clocks_init(struct device_node *ccm_node)
+{
+	struct device_node *np;
+	void __iomem *base;
+	int i;
+
+	clks[IMX6SX_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+
+	clks[IMX6SX_CLK_CKIL] = of_clk_get_by_name(ccm_node, "ckil");
+	clks[IMX6SX_CLK_OSC] = of_clk_get_by_name(ccm_node, "osc");
+
+	/* ipp_di clock is external input */
+	clks[IMX6SX_CLK_IPP_DI0] = of_clk_get_by_name(ccm_node, "ipp_di0");
+	clks[IMX6SX_CLK_IPP_DI1] = of_clk_get_by_name(ccm_node, "ipp_di1");
+
+	/* Clock source from external clock via CLK1 PAD */
+	clks[IMX6SX_CLK_ANACLK1] = imx_obtain_fixed_clock("anaclk1", 0);
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx6sx-anatop");
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+
+	clks[IMX6SX_PLL1_BYPASS_SRC] = imx_clk_mux("pll1_bypass_src", base + 0x00, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SX_PLL2_BYPASS_SRC] = imx_clk_mux("pll2_bypass_src", base + 0x30, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SX_PLL3_BYPASS_SRC] = imx_clk_mux("pll3_bypass_src", base + 0x10, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SX_PLL4_BYPASS_SRC] = imx_clk_mux("pll4_bypass_src", base + 0x70, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SX_PLL5_BYPASS_SRC] = imx_clk_mux("pll5_bypass_src", base + 0xa0, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SX_PLL6_BYPASS_SRC] = imx_clk_mux("pll6_bypass_src", base + 0xe0, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clks[IMX6SX_PLL7_BYPASS_SRC] = imx_clk_mux("pll7_bypass_src", base + 0x20, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+
+	/*                                    type               name    parent_name        base         div_mask */
+	clks[IMX6SX_CLK_PLL1] = imx_clk_pllv3(IMX_PLLV3_SYS,     "pll1", "pll1_bypass_src", base + 0x00, 0x7f);
+	clks[IMX6SX_CLK_PLL2] = imx_clk_pllv3(IMX_PLLV3_GENERIC, "pll2", "pll2_bypass_src", base + 0x30, 0x1);
+	clks[IMX6SX_CLK_PLL3] = imx_clk_pllv3(IMX_PLLV3_USB,     "pll3", "pll3_bypass_src", base + 0x10, 0x3);
+	clks[IMX6SX_CLK_PLL4] = imx_clk_pllv3(IMX_PLLV3_AV,      "pll4", "pll4_bypass_src", base + 0x70, 0x7f);
+	clks[IMX6SX_CLK_PLL5] = imx_clk_pllv3(IMX_PLLV3_AV,      "pll5", "pll5_bypass_src", base + 0xa0, 0x7f);
+	clks[IMX6SX_CLK_PLL6] = imx_clk_pllv3(IMX_PLLV3_ENET,    "pll6", "pll6_bypass_src", base + 0xe0, 0x3);
+	clks[IMX6SX_CLK_PLL7] = imx_clk_pllv3(IMX_PLLV3_USB,     "pll7", "pll7_bypass_src", base + 0x20, 0x3);
+
+	clks[IMX6SX_PLL1_BYPASS] = imx_clk_mux_flags("pll1_bypass", base + 0x00, 16, 1, pll1_bypass_sels, ARRAY_SIZE(pll1_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_PLL2_BYPASS] = imx_clk_mux_flags("pll2_bypass", base + 0x30, 16, 1, pll2_bypass_sels, ARRAY_SIZE(pll2_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_PLL3_BYPASS] = imx_clk_mux_flags("pll3_bypass", base + 0x10, 16, 1, pll3_bypass_sels, ARRAY_SIZE(pll3_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_PLL4_BYPASS] = imx_clk_mux_flags("pll4_bypass", base + 0x70, 16, 1, pll4_bypass_sels, ARRAY_SIZE(pll4_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_PLL5_BYPASS] = imx_clk_mux_flags("pll5_bypass", base + 0xa0, 16, 1, pll5_bypass_sels, ARRAY_SIZE(pll5_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_PLL6_BYPASS] = imx_clk_mux_flags("pll6_bypass", base + 0xe0, 16, 1, pll6_bypass_sels, ARRAY_SIZE(pll6_bypass_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_PLL7_BYPASS] = imx_clk_mux_flags("pll7_bypass", base + 0x20, 16, 1, pll7_bypass_sels, ARRAY_SIZE(pll7_bypass_sels), CLK_SET_RATE_PARENT);
+
+	/* Do not bypass PLLs initially */
+	clk_set_parent(clks[IMX6SX_PLL1_BYPASS], clks[IMX6SX_CLK_PLL1]);
+	clk_set_parent(clks[IMX6SX_PLL2_BYPASS], clks[IMX6SX_CLK_PLL2]);
+	clk_set_parent(clks[IMX6SX_PLL3_BYPASS], clks[IMX6SX_CLK_PLL3]);
+	clk_set_parent(clks[IMX6SX_PLL4_BYPASS], clks[IMX6SX_CLK_PLL4]);
+	clk_set_parent(clks[IMX6SX_PLL5_BYPASS], clks[IMX6SX_CLK_PLL5]);
+	clk_set_parent(clks[IMX6SX_PLL6_BYPASS], clks[IMX6SX_CLK_PLL6]);
+	clk_set_parent(clks[IMX6SX_PLL7_BYPASS], clks[IMX6SX_CLK_PLL7]);
+
+	clks[IMX6SX_CLK_PLL1_SYS]      = imx_clk_gate("pll1_sys",      "pll1_bypass", base + 0x00, 13);
+	clks[IMX6SX_CLK_PLL2_BUS]      = imx_clk_gate("pll2_bus",      "pll2_bypass", base + 0x30, 13);
+	clks[IMX6SX_CLK_PLL3_USB_OTG]  = imx_clk_gate("pll3_usb_otg",  "pll3_bypass", base + 0x10, 13);
+	clks[IMX6SX_CLK_PLL4_AUDIO]    = imx_clk_gate("pll4_audio",    "pll4_bypass", base + 0x70, 13);
+	clks[IMX6SX_CLK_PLL5_VIDEO]    = imx_clk_gate("pll5_video",    "pll5_bypass", base + 0xa0, 13);
+	clks[IMX6SX_CLK_PLL6_ENET]     = imx_clk_gate("pll6_enet",     "pll6_bypass", base + 0xe0, 13);
+	clks[IMX6SX_CLK_PLL7_USB_HOST] = imx_clk_gate("pll7_usb_host", "pll7_bypass", base + 0x20, 13);
+
+	/*
+	 * Bit 20 is the reserved and read-only bit, we do this only for:
+	 * - Do nothing for usbphy clk_enable/disable
+	 * - Keep refcount when do usbphy clk_enable/disable, in that case,
+	 * the clk framework may need to enable/disable usbphy's parent
+	 */
+	clks[IMX6SX_CLK_USBPHY1] = imx_clk_gate("usbphy1", "pll3_usb_otg",  base + 0x10, 20);
+	clks[IMX6SX_CLK_USBPHY2] = imx_clk_gate("usbphy2", "pll7_usb_host", base + 0x20, 20);
+
+	/*
+	 * usbphy*_gate needs to be on after system boots up, and software
+	 * never needs to control it anymore.
+	 */
+	clks[IMX6SX_CLK_USBPHY1_GATE] = imx_clk_gate("usbphy1_gate", "dummy", base + 0x10, 6);
+	clks[IMX6SX_CLK_USBPHY2_GATE] = imx_clk_gate("usbphy2_gate", "dummy", base + 0x20, 6);
+
+	/* FIXME 100MHz is used for pcie ref for all imx6 pcie, excepted imx6q */
+	clks[IMX6SX_CLK_PCIE_REF] = imx_clk_fixed_factor("pcie_ref", "pll6_enet", 1, 5);
+	clks[IMX6SX_CLK_PCIE_REF_125M] = imx_clk_gate("pcie_ref_125m", "pcie_ref", base + 0xe0, 19);
+
+	clks[IMX6SX_CLK_LVDS1_OUT] = imx_clk_gate_exclusive("lvds1_out", "lvds1_sel", base + 0x160, 10, BIT(12));
+	clks[IMX6SX_CLK_LVDS1_IN]  = imx_clk_gate_exclusive("lvds1_in",  "anaclk1",   base + 0x160, 12, BIT(10));
+
+	clks[IMX6SX_CLK_ENET_REF] = clk_register_divider_table(NULL, "enet_ref", "pll6_enet", 0,
+			base + 0xe0, 0, 2, 0, clk_enet_ref_table,
+			&imx_ccm_lock);
+	clks[IMX6SX_CLK_ENET2_REF] = clk_register_divider_table(NULL, "enet2_ref", "pll6_enet", 0,
+			base + 0xe0, 2, 2, 0, clk_enet_ref_table,
+			&imx_ccm_lock);
+	clks[IMX6SX_CLK_ENET2_REF_125M] = imx_clk_gate("enet2_ref_125m", "enet2_ref", base + 0xe0, 20);
+
+	clks[IMX6SX_CLK_ENET_PTP_REF] = imx_clk_fixed_factor("enet_ptp_ref", "pll6_enet", 1, 20);
+	clks[IMX6SX_CLK_ENET_PTP] = imx_clk_gate("enet_ptp_25m", "enet_ptp_ref", base + 0xe0, 21);
+
+	/*                                       name              parent_name     reg           idx */
+	clks[IMX6SX_CLK_PLL2_PFD0] = imx_clk_pfd("pll2_pfd0_352m", "pll2_bus",     base + 0x100, 0);
+	clks[IMX6SX_CLK_PLL2_PFD1] = imx_clk_pfd("pll2_pfd1_594m", "pll2_bus",     base + 0x100, 1);
+	clks[IMX6SX_CLK_PLL2_PFD2] = imx_clk_pfd("pll2_pfd2_396m", "pll2_bus",     base + 0x100, 2);
+	clks[IMX6SX_CLK_PLL2_PFD3] = imx_clk_pfd("pll2_pfd3_594m", "pll2_bus",     base + 0x100, 3);
+	clks[IMX6SX_CLK_PLL3_PFD0] = imx_clk_pfd("pll3_pfd0_720m", "pll3_usb_otg", base + 0xf0,  0);
+	clks[IMX6SX_CLK_PLL3_PFD1] = imx_clk_pfd("pll3_pfd1_540m", "pll3_usb_otg", base + 0xf0,  1);
+	clks[IMX6SX_CLK_PLL3_PFD2] = imx_clk_pfd("pll3_pfd2_508m", "pll3_usb_otg", base + 0xf0,  2);
+	clks[IMX6SX_CLK_PLL3_PFD3] = imx_clk_pfd("pll3_pfd3_454m", "pll3_usb_otg", base + 0xf0,  3);
+
+	/*                                                name         parent_name       mult div */
+	clks[IMX6SX_CLK_PLL2_198M] = imx_clk_fixed_factor("pll2_198m", "pll2_pfd2_396m", 1,   2);
+	clks[IMX6SX_CLK_PLL3_120M] = imx_clk_fixed_factor("pll3_120m", "pll3_usb_otg",   1,   4);
+	clks[IMX6SX_CLK_PLL3_80M]  = imx_clk_fixed_factor("pll3_80m",  "pll3_usb_otg",   1,   6);
+	clks[IMX6SX_CLK_PLL3_60M]  = imx_clk_fixed_factor("pll3_60m",  "pll3_usb_otg",   1,   8);
+	clks[IMX6SX_CLK_TWD]       = imx_clk_fixed_factor("twd",       "arm",            1,   2);
+	clks[IMX6SX_CLK_GPT_3M]    = imx_clk_fixed_factor("gpt_3m",    "osc",            1,   8);
+
+	clks[IMX6SX_CLK_PLL4_POST_DIV]  = clk_register_divider_table(NULL, "pll4_post_div", "pll4_audio",
+				CLK_SET_RATE_PARENT, base + 0x70, 19, 2, 0, post_div_table, &imx_ccm_lock);
+	clks[IMX6SX_CLK_PLL4_AUDIO_DIV] = clk_register_divider(NULL, "pll4_audio_div", "pll4_post_div",
+				CLK_SET_RATE_PARENT, base + 0x170, 15, 1, 0, &imx_ccm_lock);
+	clks[IMX6SX_CLK_PLL5_POST_DIV]  = clk_register_divider_table(NULL, "pll5_post_div", "pll5_video",
+				CLK_SET_RATE_PARENT, base + 0xa0, 19, 2, 0, post_div_table, &imx_ccm_lock);
+	clks[IMX6SX_CLK_PLL5_VIDEO_DIV] = clk_register_divider_table(NULL, "pll5_video_div", "pll5_post_div",
+				CLK_SET_RATE_PARENT, base + 0x170, 30, 2, 0, video_div_table, &imx_ccm_lock);
+
+	/*                                                name                reg           shift   width   parent_names       num_parents */
+	clks[IMX6SX_CLK_LVDS1_SEL]          = imx_clk_mux("lvds1_sel",        base + 0x160, 0,      5,      lvds_sels,         ARRAY_SIZE(lvds_sels));
+
+	np = ccm_node;
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+
+	/*                                                name                reg           shift   width   parent_names       num_parents */
+	clks[IMX6SX_CLK_STEP]               = imx_clk_mux("step",             base + 0xc,   8,      1,      step_sels,         ARRAY_SIZE(step_sels));
+	clks[IMX6SX_CLK_PLL1_SW]            = imx_clk_mux("pll1_sw",          base + 0xc,   2,      1,      pll1_sw_sels,      ARRAY_SIZE(pll1_sw_sels));
+	clks[IMX6SX_CLK_OCRAM_SEL]          = imx_clk_mux("ocram_sel",        base + 0x14,  6,      2,      ocram_sels,        ARRAY_SIZE(ocram_sels));
+	clks[IMX6SX_CLK_PERIPH_PRE]         = imx_clk_mux("periph_pre",       base + 0x18,  18,     2,      periph_pre_sels,   ARRAY_SIZE(periph_pre_sels));
+	clks[IMX6SX_CLK_PERIPH2_PRE]        = imx_clk_mux("periph2_pre",      base + 0x18,  21,     2,      periph2_pre_sels,   ARRAY_SIZE(periph2_pre_sels));
+	clks[IMX6SX_CLK_PERIPH_CLK2_SEL]    = imx_clk_mux("periph_clk2_sel",  base + 0x18,  12,     2,      periph_clk2_sels,  ARRAY_SIZE(periph_clk2_sels));
+	clks[IMX6SX_CLK_PERIPH2_CLK2_SEL]   = imx_clk_mux("periph2_clk2_sel", base + 0x18,  20,     1,      periph2_clk2_sels, ARRAY_SIZE(periph2_clk2_sels));
+	clks[IMX6SX_CLK_PCIE_AXI_SEL]       = imx_clk_mux("pcie_axi_sel",     base + 0x18,  10,     1,      pcie_axi_sels,     ARRAY_SIZE(pcie_axi_sels));
+	clks[IMX6SX_CLK_GPU_AXI_SEL]        = imx_clk_mux("gpu_axi_sel",      base + 0x18,  8,      2,      gpu_axi_sels,      ARRAY_SIZE(gpu_axi_sels));
+	clks[IMX6SX_CLK_GPU_CORE_SEL]       = imx_clk_mux("gpu_core_sel",     base + 0x18,  4,      2,      gpu_core_sels,     ARRAY_SIZE(gpu_core_sels));
+	clks[IMX6SX_CLK_EIM_SLOW_SEL]       = imx_clk_mux("eim_slow_sel",     base + 0x1c,  29,     2,      eim_slow_sels,     ARRAY_SIZE(eim_slow_sels));
+	clks[IMX6SX_CLK_USDHC1_SEL]         = imx_clk_mux("usdhc1_sel",       base + 0x1c,  16,     1,      usdhc_sels,        ARRAY_SIZE(usdhc_sels));
+	clks[IMX6SX_CLK_USDHC2_SEL]         = imx_clk_mux("usdhc2_sel",       base + 0x1c,  17,     1,      usdhc_sels,        ARRAY_SIZE(usdhc_sels));
+	clks[IMX6SX_CLK_USDHC3_SEL]         = imx_clk_mux("usdhc3_sel",       base + 0x1c,  18,     1,      usdhc_sels,        ARRAY_SIZE(usdhc_sels));
+	clks[IMX6SX_CLK_USDHC4_SEL]         = imx_clk_mux("usdhc4_sel",       base + 0x1c,  19,     1,      usdhc_sels,        ARRAY_SIZE(usdhc_sels));
+	clks[IMX6SX_CLK_SSI3_SEL]           = imx_clk_mux("ssi3_sel",         base + 0x1c,  14,     2,      ssi_sels,          ARRAY_SIZE(ssi_sels));
+	clks[IMX6SX_CLK_SSI2_SEL]           = imx_clk_mux("ssi2_sel",         base + 0x1c,  12,     2,      ssi_sels,          ARRAY_SIZE(ssi_sels));
+	clks[IMX6SX_CLK_SSI1_SEL]           = imx_clk_mux("ssi1_sel",         base + 0x1c,  10,     2,      ssi_sels,          ARRAY_SIZE(ssi_sels));
+	clks[IMX6SX_CLK_QSPI1_SEL]          = imx_clk_mux_flags("qspi1_sel", base + 0x1c,  7, 3, qspi1_sels, ARRAY_SIZE(qspi1_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_PERCLK_SEL]         = imx_clk_mux("perclk_sel",       base + 0x1c,  6,      1,      perclk_sels,       ARRAY_SIZE(perclk_sels));
+	clks[IMX6SX_CLK_VID_SEL]            = imx_clk_mux("vid_sel",          base + 0x20,  21,     3,      vid_sels,          ARRAY_SIZE(vid_sels));
+	clks[IMX6SX_CLK_ESAI_SEL]           = imx_clk_mux("esai_sel",         base + 0x20,  19,     2,      audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SX_CLK_CAN_SEL]            = imx_clk_mux("can_sel",          base + 0x20,  8,      2,      can_sels,          ARRAY_SIZE(can_sels));
+	clks[IMX6SX_CLK_UART_SEL]           = imx_clk_mux("uart_sel",         base + 0x24,  6,      1,      uart_sels,         ARRAY_SIZE(uart_sels));
+	clks[IMX6SX_CLK_QSPI2_SEL]          = imx_clk_mux_flags("qspi2_sel", base + 0x2c, 15, 3, qspi2_sels, ARRAY_SIZE(qspi2_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_SPDIF_SEL]          = imx_clk_mux("spdif_sel",        base + 0x30,  20,     2,      audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SX_CLK_AUDIO_SEL]          = imx_clk_mux("audio_sel",        base + 0x30,  7,      2,      audio_sels,        ARRAY_SIZE(audio_sels));
+	clks[IMX6SX_CLK_ENET_PRE_SEL]       = imx_clk_mux("enet_pre_sel",     base + 0x34,  15,     3,      enet_pre_sels,     ARRAY_SIZE(enet_pre_sels));
+	clks[IMX6SX_CLK_ENET_SEL]           = imx_clk_mux("enet_sel",         base + 0x34,  9,      3,      enet_sels,         ARRAY_SIZE(enet_sels));
+	clks[IMX6SX_CLK_M4_PRE_SEL]         = imx_clk_mux("m4_pre_sel",       base + 0x34,  6,      3,      m4_pre_sels,       ARRAY_SIZE(m4_pre_sels));
+	clks[IMX6SX_CLK_M4_SEL]             = imx_clk_mux("m4_sel",           base + 0x34,  0,      3,      m4_sels,           ARRAY_SIZE(m4_sels));
+	clks[IMX6SX_CLK_ECSPI_SEL]          = imx_clk_mux("ecspi_sel",        base + 0x38,  18,     1,      ecspi_sels,        ARRAY_SIZE(ecspi_sels));
+	clks[IMX6SX_CLK_LCDIF2_PRE_SEL]     = imx_clk_mux("lcdif2_pre_sel",   base + 0x38,  6,      3,      lcdif2_pre_sels,   ARRAY_SIZE(lcdif2_pre_sels));
+	clks[IMX6SX_CLK_LCDIF2_SEL]         = imx_clk_mux("lcdif2_sel",       base + 0x38,  0,      3,      lcdif2_sels,       ARRAY_SIZE(lcdif2_sels));
+	clks[IMX6SX_CLK_DISPLAY_SEL]        = imx_clk_mux("display_sel",      base + 0x3c,  14,     2,      display_sels,      ARRAY_SIZE(display_sels));
+	clks[IMX6SX_CLK_CSI_SEL]            = imx_clk_mux("csi_sel",          base + 0x3c,  9,      2,      csi_sels,          ARRAY_SIZE(csi_sels));
+	clks[IMX6SX_CLK_CKO1_SEL]           = imx_clk_mux("cko1_sel",         base + 0x60,  0,      4,      cko1_sels,         ARRAY_SIZE(cko1_sels));
+	clks[IMX6SX_CLK_CKO2_SEL]           = imx_clk_mux("cko2_sel",         base + 0x60,  16,     5,      cko2_sels,         ARRAY_SIZE(cko2_sels));
+	clks[IMX6SX_CLK_CKO]                = imx_clk_mux("cko",              base + 0x60,  8,      1,      cko_sels,          ARRAY_SIZE(cko_sels));
+
+	clks[IMX6SX_CLK_LDB_DI1_DIV_SEL]    = imx_clk_mux_flags("ldb_di1_div_sel", base + 0x20, 11, 1, ldb_di1_div_sels, ARRAY_SIZE(ldb_di1_div_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LDB_DI0_DIV_SEL]    = imx_clk_mux_flags("ldb_di0_div_sel", base + 0x20, 10, 1, ldb_di0_div_sels, ARRAY_SIZE(ldb_di0_div_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LDB_DI1_SEL]        = imx_clk_mux_flags("ldb_di1_sel",     base + 0x2c, 12, 3, ldb_di1_sels,      ARRAY_SIZE(ldb_di1_sels),    CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LDB_DI0_SEL]        = imx_clk_mux_flags("ldb_di0_sel",     base + 0x2c, 9,  3, ldb_di0_sels,      ARRAY_SIZE(ldb_di0_sels),    CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LCDIF1_PRE_SEL]     = imx_clk_mux_flags("lcdif1_pre_sel",  base + 0x38, 15, 3, lcdif1_pre_sels,   ARRAY_SIZE(lcdif1_pre_sels), CLK_SET_RATE_PARENT);
+	clks[IMX6SX_CLK_LCDIF1_SEL]         = imx_clk_mux_flags("lcdif1_sel",      base + 0x38, 9,  3, lcdif1_sels,       ARRAY_SIZE(lcdif1_sels),     CLK_SET_RATE_PARENT);
+
+	/*                                                    name              parent_name          reg          shift width */
+	clks[IMX6SX_CLK_PERIPH_CLK2]        = imx_clk_divider("periph_clk2",    "periph_clk2_sel",   base + 0x14, 27,   3);
+	clks[IMX6SX_CLK_PERIPH2_CLK2]       = imx_clk_divider("periph2_clk2",   "periph2_clk2_sel",  base + 0x14, 0,    3);
+	clks[IMX6SX_CLK_IPG]                = imx_clk_divider("ipg",            "ahb",               base + 0x14, 8,    2);
+	clks[IMX6SX_CLK_GPU_CORE_PODF]      = imx_clk_divider("gpu_core_podf",  "gpu_core_sel",      base + 0x18, 29,   3);
+	clks[IMX6SX_CLK_GPU_AXI_PODF]       = imx_clk_divider("gpu_axi_podf",   "gpu_axi_sel",       base + 0x18, 26,   3);
+	clks[IMX6SX_CLK_LCDIF1_PODF]        = imx_clk_divider("lcdif1_podf",    "lcdif1_pred",       base + 0x18, 23,   3);
+	clks[IMX6SX_CLK_QSPI1_PODF]         = imx_clk_divider("qspi1_podf",     "qspi1_sel",         base + 0x1c, 26,   3);
+	clks[IMX6SX_CLK_EIM_SLOW_PODF]      = imx_clk_divider("eim_slow_podf",  "eim_slow_sel",      base + 0x1c, 23,   3);
+	clks[IMX6SX_CLK_LCDIF2_PODF]        = imx_clk_divider("lcdif2_podf",    "lcdif2_pred",       base + 0x1c, 20,   3);
+	clks[IMX6SX_CLK_PERCLK]             = imx_clk_divider("perclk",         "perclk_sel",        base + 0x1c, 0,    6);
+	clks[IMX6SX_CLK_VID_PODF]           = imx_clk_divider("vid_podf",       "vid_sel",           base + 0x20, 24,   2);
+	clks[IMX6SX_CLK_CAN_PODF]           = imx_clk_divider("can_podf",       "can_sel",           base + 0x20, 2,    6);
+	clks[IMX6SX_CLK_USDHC4_PODF]        = imx_clk_divider("usdhc4_podf",    "usdhc4_sel",        base + 0x24, 22,   3);
+	clks[IMX6SX_CLK_USDHC3_PODF]        = imx_clk_divider("usdhc3_podf",    "usdhc3_sel",        base + 0x24, 19,   3);
+	clks[IMX6SX_CLK_USDHC2_PODF]        = imx_clk_divider("usdhc2_podf",    "usdhc2_sel",        base + 0x24, 16,   3);
+	clks[IMX6SX_CLK_USDHC1_PODF]        = imx_clk_divider("usdhc1_podf",    "usdhc1_sel",        base + 0x24, 11,   3);
+	clks[IMX6SX_CLK_UART_PODF]          = imx_clk_divider("uart_podf",      "uart_sel",          base + 0x24, 0,    6);
+	clks[IMX6SX_CLK_ESAI_PRED]          = imx_clk_divider("esai_pred",      "esai_sel",          base + 0x28, 9,    3);
+	clks[IMX6SX_CLK_ESAI_PODF]          = imx_clk_divider("esai_podf",      "esai_pred",         base + 0x28, 25,   3);
+	clks[IMX6SX_CLK_SSI3_PRED]          = imx_clk_divider("ssi3_pred",      "ssi3_sel",          base + 0x28, 22,   3);
+	clks[IMX6SX_CLK_SSI3_PODF]          = imx_clk_divider("ssi3_podf",      "ssi3_pred",         base + 0x28, 16,   6);
+	clks[IMX6SX_CLK_SSI1_PRED]          = imx_clk_divider("ssi1_pred",      "ssi1_sel",          base + 0x28, 6,    3);
+	clks[IMX6SX_CLK_SSI1_PODF]          = imx_clk_divider("ssi1_podf",      "ssi1_pred",         base + 0x28, 0,    6);
+	clks[IMX6SX_CLK_QSPI2_PRED]         = imx_clk_divider("qspi2_pred",     "qspi2_sel",         base + 0x2c, 18,   3);
+	clks[IMX6SX_CLK_QSPI2_PODF]         = imx_clk_divider("qspi2_podf",     "qspi2_pred",        base + 0x2c, 21,   6);
+	clks[IMX6SX_CLK_SSI2_PRED]          = imx_clk_divider("ssi2_pred",      "ssi2_sel",          base + 0x2c, 6,    3);
+	clks[IMX6SX_CLK_SSI2_PODF]          = imx_clk_divider("ssi2_podf",      "ssi2_pred",         base + 0x2c, 0,    6);
+	clks[IMX6SX_CLK_SPDIF_PRED]         = imx_clk_divider("spdif_pred",     "spdif_sel",         base + 0x30, 25,   3);
+	clks[IMX6SX_CLK_SPDIF_PODF]         = imx_clk_divider("spdif_podf",     "spdif_pred",        base + 0x30, 22,   3);
+	clks[IMX6SX_CLK_AUDIO_PRED]         = imx_clk_divider("audio_pred",     "audio_sel",         base + 0x30, 12,   3);
+	clks[IMX6SX_CLK_AUDIO_PODF]         = imx_clk_divider("audio_podf",     "audio_pred",        base + 0x30, 9,    3);
+	clks[IMX6SX_CLK_ENET_PODF]          = imx_clk_divider("enet_podf",      "enet_pre_sel",      base + 0x34, 12,   3);
+	clks[IMX6SX_CLK_M4_PODF]            = imx_clk_divider("m4_podf",        "m4_sel",            base + 0x34, 3,    3);
+	clks[IMX6SX_CLK_ECSPI_PODF]         = imx_clk_divider("ecspi_podf",     "ecspi_sel",         base + 0x38, 19,   6);
+	clks[IMX6SX_CLK_LCDIF1_PRED]        = imx_clk_divider("lcdif1_pred",    "lcdif1_pre_sel",    base + 0x38, 12,   3);
+	clks[IMX6SX_CLK_LCDIF2_PRED]        = imx_clk_divider("lcdif2_pred",    "lcdif2_pre_sel",    base + 0x38, 3,    3);
+	clks[IMX6SX_CLK_DISPLAY_PODF]       = imx_clk_divider("display_podf",   "display_sel",       base + 0x3c, 16,   3);
+	clks[IMX6SX_CLK_CSI_PODF]           = imx_clk_divider("csi_podf",       "csi_sel",           base + 0x3c, 11,   3);
+	clks[IMX6SX_CLK_CKO1_PODF]          = imx_clk_divider("cko1_podf",      "cko1_sel",          base + 0x60, 4,    3);
+	clks[IMX6SX_CLK_CKO2_PODF]          = imx_clk_divider("cko2_podf",      "cko2_sel",          base + 0x60, 21,   3);
+
+	clks[IMX6SX_CLK_LDB_DI0_DIV_3_5]    = imx_clk_fixed_factor("ldb_di0_div_3_5", "ldb_di0_sel", 2, 7);
+	clks[IMX6SX_CLK_LDB_DI0_DIV_7]      = imx_clk_fixed_factor("ldb_di0_div_7",   "ldb_di0_sel", 1, 7);
+	clks[IMX6SX_CLK_LDB_DI1_DIV_3_5]    = imx_clk_fixed_factor("ldb_di1_div_3_5", "ldb_di1_sel", 2, 7);
+	clks[IMX6SX_CLK_LDB_DI1_DIV_7]      = imx_clk_fixed_factor("ldb_di1_div_7",   "ldb_di1_sel", 1, 7);
+
+	/*                                               name        reg          shift width busy: reg,   shift parent_names       num_parents */
+	clks[IMX6SX_CLK_PERIPH]       = imx_clk_busy_mux("periph",   base + 0x14, 25,   1,    base + 0x48, 5,    periph_sels,       ARRAY_SIZE(periph_sels));
+	clks[IMX6SX_CLK_PERIPH2]      = imx_clk_busy_mux("periph2",  base + 0x14, 26,   1,    base + 0x48, 3,    periph2_sels,      ARRAY_SIZE(periph2_sels));
+	/*                                                   name             parent_name    reg          shift width busy: reg,   shift */
+	clks[IMX6SX_CLK_OCRAM_PODF]   = imx_clk_busy_divider("ocram_podf",    "ocram_sel",   base + 0x14, 16,   3,    base + 0x48, 0);
+	clks[IMX6SX_CLK_AHB]          = imx_clk_busy_divider("ahb",           "periph",      base + 0x14, 10,   3,    base + 0x48, 1);
+	clks[IMX6SX_CLK_MMDC_PODF]    = imx_clk_busy_divider("mmdc_podf",     "periph2",     base + 0x14, 3,    3,    base + 0x48, 2);
+	clks[IMX6SX_CLK_ARM]          = imx_clk_busy_divider("arm",           "pll1_sw",     base + 0x10, 0,    3,    base + 0x48, 16);
+
+	/*                                            name             parent_name          reg         shift */
+	/* CCGR0 */
+	clks[IMX6SX_CLK_AIPS_TZ1]     = imx_clk_gate2("aips_tz1",      "ahb",               base + 0x68, 0);
+	clks[IMX6SX_CLK_AIPS_TZ2]     = imx_clk_gate2("aips_tz2",      "ahb",               base + 0x68, 2);
+	clks[IMX6SX_CLK_APBH_DMA]     = imx_clk_gate2("apbh_dma",      "usdhc3",            base + 0x68, 4);
+	clks[IMX6SX_CLK_ASRC_MEM]     = imx_clk_gate2_shared("asrc_mem", "ahb",             base + 0x68, 6, &share_count_asrc);
+	clks[IMX6SX_CLK_ASRC_IPG]     = imx_clk_gate2_shared("asrc_ipg", "ahb",             base + 0x68, 6, &share_count_asrc);
+	clks[IMX6SX_CLK_CAAM_MEM]     = imx_clk_gate2("caam_mem",      "ahb",               base + 0x68, 8);
+	clks[IMX6SX_CLK_CAAM_ACLK]    = imx_clk_gate2("caam_aclk",     "ahb",               base + 0x68, 10);
+	clks[IMX6SX_CLK_CAAM_IPG]     = imx_clk_gate2("caam_ipg",      "ipg",               base + 0x68, 12);
+	clks[IMX6SX_CLK_CAN1_IPG]     = imx_clk_gate2("can1_ipg",      "ipg",               base + 0x68, 14);
+	clks[IMX6SX_CLK_CAN1_SERIAL]  = imx_clk_gate2("can1_serial",   "can_podf",          base + 0x68, 16);
+	clks[IMX6SX_CLK_CAN2_IPG]     = imx_clk_gate2("can2_ipg",      "ipg",               base + 0x68, 18);
+	clks[IMX6SX_CLK_CAN2_SERIAL]  = imx_clk_gate2("can2_serial",   "can_podf",          base + 0x68, 20);
+	clks[IMX6SX_CLK_DCIC1]        = imx_clk_gate2("dcic1",         "display_podf",      base + 0x68, 24);
+	clks[IMX6SX_CLK_DCIC2]        = imx_clk_gate2("dcic2",         "display_podf",      base + 0x68, 26);
+	clks[IMX6SX_CLK_AIPS_TZ3]     = imx_clk_gate2("aips_tz3",      "ahb",               base + 0x68, 30);
+
+	/* CCGR1 */
+	clks[IMX6SX_CLK_ECSPI1]       = imx_clk_gate2("ecspi1",        "ecspi_podf",        base + 0x6c, 0);
+	clks[IMX6SX_CLK_ECSPI2]       = imx_clk_gate2("ecspi2",        "ecspi_podf",        base + 0x6c, 2);
+	clks[IMX6SX_CLK_ECSPI3]       = imx_clk_gate2("ecspi3",        "ecspi_podf",        base + 0x6c, 4);
+	clks[IMX6SX_CLK_ECSPI4]       = imx_clk_gate2("ecspi4",        "ecspi_podf",        base + 0x6c, 6);
+	clks[IMX6SX_CLK_ECSPI5]       = imx_clk_gate2("ecspi5",        "ecspi_podf",        base + 0x6c, 8);
+	clks[IMX6SX_CLK_EPIT1]        = imx_clk_gate2("epit1",         "perclk",            base + 0x6c, 12);
+	clks[IMX6SX_CLK_EPIT2]        = imx_clk_gate2("epit2",         "perclk",            base + 0x6c, 14);
+	clks[IMX6SX_CLK_ESAI_EXTAL]   = imx_clk_gate2_shared("esai_extal", "esai_podf",     base + 0x6c, 16, &share_count_esai);
+	clks[IMX6SX_CLK_ESAI_IPG]     = imx_clk_gate2_shared("esai_ipg",   "ahb",           base + 0x6c, 16, &share_count_esai);
+	clks[IMX6SX_CLK_ESAI_MEM]     = imx_clk_gate2_shared("esai_mem",   "ahb",           base + 0x6c, 16, &share_count_esai);
+	clks[IMX6SX_CLK_WAKEUP]       = imx_clk_gate2("wakeup",        "ipg",               base + 0x6c, 18);
+	clks[IMX6SX_CLK_GPT_BUS]      = imx_clk_gate2("gpt_bus",       "perclk",            base + 0x6c, 20);
+	clks[IMX6SX_CLK_GPT_SERIAL]   = imx_clk_gate2("gpt_serial",    "perclk",            base + 0x6c, 22);
+	clks[IMX6SX_CLK_GPU]          = imx_clk_gate2("gpu",           "gpu_core_podf",     base + 0x6c, 26);
+	clks[IMX6SX_CLK_CANFD]        = imx_clk_gate2("canfd",         "can_podf",          base + 0x6c, 30);
+
+	/* CCGR2 */
+	clks[IMX6SX_CLK_CSI]          = imx_clk_gate2("csi",           "csi_podf",          base + 0x70, 2);
+	clks[IMX6SX_CLK_I2C1]         = imx_clk_gate2("i2c1",          "perclk",            base + 0x70, 6);
+	clks[IMX6SX_CLK_I2C2]         = imx_clk_gate2("i2c2",          "perclk",            base + 0x70, 8);
+	clks[IMX6SX_CLK_I2C3]         = imx_clk_gate2("i2c3",          "perclk",            base + 0x70, 10);
+	clks[IMX6SX_CLK_OCOTP]        = imx_clk_gate2("ocotp",         "ipg",               base + 0x70, 12);
+	clks[IMX6SX_CLK_IOMUXC]       = imx_clk_gate2("iomuxc",        "lcdif1_podf",       base + 0x70, 14);
+	clks[IMX6SX_CLK_IPMUX1]       = imx_clk_gate2("ipmux1",        "ahb",               base + 0x70, 16);
+	clks[IMX6SX_CLK_IPMUX2]       = imx_clk_gate2("ipmux2",        "ahb",               base + 0x70, 18);
+	clks[IMX6SX_CLK_IPMUX3]       = imx_clk_gate2("ipmux3",        "ahb",               base + 0x70, 20);
+	clks[IMX6SX_CLK_TZASC1]       = imx_clk_gate2("tzasc1",        "mmdc_podf",         base + 0x70, 22);
+	clks[IMX6SX_CLK_LCDIF_APB]    = imx_clk_gate2("lcdif_apb",     "display_podf",      base + 0x70, 28);
+	clks[IMX6SX_CLK_PXP_AXI]      = imx_clk_gate2("pxp_axi",       "display_podf",      base + 0x70, 30);
+
+	/* CCGR3 */
+	clks[IMX6SX_CLK_M4]           = imx_clk_gate2("m4",            "m4_podf",           base + 0x74, 2);
+	clks[IMX6SX_CLK_ENET]         = imx_clk_gate2("enet",          "ipg",               base + 0x74, 4);
+	clks[IMX6SX_CLK_ENET_AHB]     = imx_clk_gate2("enet_ahb",      "enet_sel",          base + 0x74, 4);
+	clks[IMX6SX_CLK_DISPLAY_AXI]  = imx_clk_gate2("display_axi",   "display_podf",      base + 0x74, 6);
+	clks[IMX6SX_CLK_LCDIF2_PIX]   = imx_clk_gate2("lcdif2_pix",    "lcdif2_sel",        base + 0x74, 8);
+	clks[IMX6SX_CLK_LCDIF1_PIX]   = imx_clk_gate2("lcdif1_pix",    "lcdif1_sel",        base + 0x74, 10);
+	clks[IMX6SX_CLK_LDB_DI0]      = imx_clk_gate2("ldb_di0",       "ldb_di0_div_sel",   base + 0x74, 12);
+	clks[IMX6SX_CLK_QSPI1]        = imx_clk_gate2("qspi1",         "qspi1_podf",        base + 0x74, 14);
+	clks[IMX6SX_CLK_MLB]          = imx_clk_gate2("mlb",           "ahb",               base + 0x74, 18);
+	clks[IMX6SX_CLK_MMDC_P0_FAST] = imx_clk_gate2("mmdc_p0_fast",  "mmdc_podf",         base + 0x74, 20);
+	clks[IMX6SX_CLK_MMDC_P0_IPG]  = imx_clk_gate2("mmdc_p0_ipg",   "ipg",               base + 0x74, 24);
+	clks[IMX6SX_CLK_OCRAM]        = imx_clk_gate2("ocram",         "ocram_podf",        base + 0x74, 28);
+
+	/* CCGR4 */
+	clks[IMX6SX_CLK_PCIE_AXI]     = imx_clk_gate2("pcie_axi",      "display_podf",      base + 0x78, 0);
+	clks[IMX6SX_CLK_QSPI2]        = imx_clk_gate2("qspi2",         "qspi2_podf",        base + 0x78, 10);
+	clks[IMX6SX_CLK_PER1_BCH]     = imx_clk_gate2("per1_bch",      "usdhc3",            base + 0x78, 12);
+	clks[IMX6SX_CLK_PER2_MAIN]    = imx_clk_gate2("per2_main",     "ahb",               base + 0x78, 14);
+	clks[IMX6SX_CLK_PWM1]         = imx_clk_gate2("pwm1",          "perclk",            base + 0x78, 16);
+	clks[IMX6SX_CLK_PWM2]         = imx_clk_gate2("pwm2",          "perclk",            base + 0x78, 18);
+	clks[IMX6SX_CLK_PWM3]         = imx_clk_gate2("pwm3",          "perclk",            base + 0x78, 20);
+	clks[IMX6SX_CLK_PWM4]         = imx_clk_gate2("pwm4",          "perclk",            base + 0x78, 22);
+	clks[IMX6SX_CLK_GPMI_BCH_APB] = imx_clk_gate2("gpmi_bch_apb",  "usdhc3",            base + 0x78, 24);
+	clks[IMX6SX_CLK_GPMI_BCH]     = imx_clk_gate2("gpmi_bch",      "usdhc4",            base + 0x78, 26);
+	clks[IMX6SX_CLK_GPMI_IO]      = imx_clk_gate2("gpmi_io",       "qspi2_podf",        base + 0x78, 28);
+	clks[IMX6SX_CLK_GPMI_APB]     = imx_clk_gate2("gpmi_apb",      "usdhc3",            base + 0x78, 30);
+
+	/* CCGR5 */
+	clks[IMX6SX_CLK_ROM]          = imx_clk_gate2("rom",           "ahb",               base + 0x7c, 0);
+	clks[IMX6SX_CLK_SDMA]         = imx_clk_gate2("sdma",          "ahb",               base + 0x7c, 6);
+	clks[IMX6SX_CLK_SPBA]         = imx_clk_gate2("spba",          "ipg",               base + 0x7c, 12);
+	clks[IMX6SX_CLK_AUDIO]        = imx_clk_gate2_shared("audio",  "audio_podf",        base + 0x7c, 14, &share_count_audio);
+	clks[IMX6SX_CLK_SPDIF]        = imx_clk_gate2_shared("spdif",  "spdif_podf",        base + 0x7c, 14, &share_count_audio);
+	clks[IMX6SX_CLK_SSI1_IPG]     = imx_clk_gate2_shared("ssi1_ipg",      "ipg",        base + 0x7c, 18, &share_count_ssi1);
+	clks[IMX6SX_CLK_SSI2_IPG]     = imx_clk_gate2_shared("ssi2_ipg",      "ipg",        base + 0x7c, 20, &share_count_ssi2);
+	clks[IMX6SX_CLK_SSI3_IPG]     = imx_clk_gate2_shared("ssi3_ipg",      "ipg",        base + 0x7c, 22, &share_count_ssi3);
+	clks[IMX6SX_CLK_SSI1]         = imx_clk_gate2_shared("ssi1",          "ssi1_podf",  base + 0x7c, 18, &share_count_ssi1);
+	clks[IMX6SX_CLK_SSI2]         = imx_clk_gate2_shared("ssi2",          "ssi2_podf",  base + 0x7c, 20, &share_count_ssi2);
+	clks[IMX6SX_CLK_SSI3]         = imx_clk_gate2_shared("ssi3",          "ssi3_podf",  base + 0x7c, 22, &share_count_ssi3);
+	clks[IMX6SX_CLK_UART_IPG]     = imx_clk_gate2("uart_ipg",      "ipg",               base + 0x7c, 24);
+	clks[IMX6SX_CLK_UART_SERIAL]  = imx_clk_gate2("uart_serial",   "uart_podf",         base + 0x7c, 26);
+	clks[IMX6SX_CLK_SAI1_IPG]     = imx_clk_gate2("sai1_ipg",      "ipg",               base + 0x7c, 28);
+	clks[IMX6SX_CLK_SAI2_IPG]     = imx_clk_gate2("sai2_ipg",      "ipg",               base + 0x7c, 30);
+	clks[IMX6SX_CLK_SAI1]         = imx_clk_gate2("sai1",          "ssi1_podf",         base + 0x7c, 28);
+	clks[IMX6SX_CLK_SAI2]         = imx_clk_gate2("sai2",          "ssi2_podf",         base + 0x7c, 30);
+
+	/* CCGR6 */
+	clks[IMX6SX_CLK_USBOH3]       = imx_clk_gate2("usboh3",        "ipg",               base + 0x80, 0);
+	clks[IMX6SX_CLK_USDHC1]       = imx_clk_gate2("usdhc1",        "usdhc1_podf",       base + 0x80, 2);
+	clks[IMX6SX_CLK_USDHC2]       = imx_clk_gate2("usdhc2",        "usdhc2_podf",       base + 0x80, 4);
+	clks[IMX6SX_CLK_USDHC3]       = imx_clk_gate2("usdhc3",        "usdhc3_podf",       base + 0x80, 6);
+	clks[IMX6SX_CLK_USDHC4]       = imx_clk_gate2("usdhc4",        "usdhc4_podf",       base + 0x80, 8);
+	clks[IMX6SX_CLK_EIM_SLOW]     = imx_clk_gate2("eim_slow",      "eim_slow_podf",     base + 0x80, 10);
+	clks[IMX6SX_CLK_PWM8]         = imx_clk_gate2("pwm8",          "perclk",            base + 0x80, 16);
+	clks[IMX6SX_CLK_VADC]         = imx_clk_gate2("vadc",          "vid_podf",          base + 0x80, 20);
+	clks[IMX6SX_CLK_GIS]          = imx_clk_gate2("gis",           "display_podf",      base + 0x80, 22);
+	clks[IMX6SX_CLK_I2C4]         = imx_clk_gate2("i2c4",          "perclk",            base + 0x80, 24);
+	clks[IMX6SX_CLK_PWM5]         = imx_clk_gate2("pwm5",          "perclk",            base + 0x80, 26);
+	clks[IMX6SX_CLK_PWM6]         = imx_clk_gate2("pwm6",          "perclk",            base + 0x80, 28);
+	clks[IMX6SX_CLK_PWM7]         = imx_clk_gate2("pwm7",          "perclk",            base + 0x80, 30);
+
+	clks[IMX6SX_CLK_CKO1]         = imx_clk_gate("cko1",           "cko1_podf",         base + 0x60, 7);
+	clks[IMX6SX_CLK_CKO2]         = imx_clk_gate("cko2",           "cko2_podf",         base + 0x60, 24);
+
+	/* mask handshake of mmdc */
+	writel_relaxed(BM_CCM_CCDR_MMDC_CH0_MASK, base + CCDR);
+
+	imx_check_clocks(clks, ARRAY_SIZE(clks));
+
+	clk_data.clks = clks;
+	clk_data.clk_num = ARRAY_SIZE(clks);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
+		clk_prepare_enable(clks[clks_init_on[i]]);
+
+	if (IS_ENABLED(CONFIG_USB_MXS_PHY)) {
+		clk_prepare_enable(clks[IMX6SX_CLK_USBPHY1_GATE]);
+		clk_prepare_enable(clks[IMX6SX_CLK_USBPHY2_GATE]);
+	}
+
+	/* Set the default 132MHz for EIM module */
+	clk_set_parent(clks[IMX6SX_CLK_EIM_SLOW_SEL], clks[IMX6SX_CLK_PLL2_PFD2]);
+	clk_set_rate(clks[IMX6SX_CLK_EIM_SLOW], 132000000);
+
+	/* set parent clock for LCDIF1 pixel clock */
+	clk_set_parent(clks[IMX6SX_CLK_LCDIF1_PRE_SEL], clks[IMX6SX_CLK_PLL5_VIDEO_DIV]);
+	clk_set_parent(clks[IMX6SX_CLK_LCDIF1_SEL], clks[IMX6SX_CLK_LCDIF1_PODF]);
+
+	/* Set the parent clks of PCIe lvds1 and pcie_axi to be pcie ref, axi */
+	if (clk_set_parent(clks[IMX6SX_CLK_LVDS1_SEL], clks[IMX6SX_CLK_PCIE_REF_125M]))
+		pr_err("Failed to set pcie bus parent clk.\n");
+	if (clk_set_parent(clks[IMX6SX_CLK_PCIE_AXI_SEL], clks[IMX6SX_CLK_AXI]))
+		pr_err("Failed to set pcie parent clk.\n");
+
+	/*
+	 * Init enet system AHB clock, set to 200MHz
+	 * pll2_pfd2_396m-> ENET_PODF-> ENET_AHB
+	 */
+	clk_set_parent(clks[IMX6SX_CLK_ENET_PRE_SEL], clks[IMX6SX_CLK_PLL2_PFD2]);
+	clk_set_parent(clks[IMX6SX_CLK_ENET_SEL], clks[IMX6SX_CLK_ENET_PODF]);
+	clk_set_rate(clks[IMX6SX_CLK_ENET_PODF], 200000000);
+	clk_set_rate(clks[IMX6SX_CLK_ENET_REF], 125000000);
+	clk_set_rate(clks[IMX6SX_CLK_ENET2_REF], 125000000);
+
+	/* Audio clocks */
+	clk_set_rate(clks[IMX6SX_CLK_PLL4_AUDIO_DIV], 393216000);
+
+	clk_set_parent(clks[IMX6SX_CLK_SPDIF_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_rate(clks[IMX6SX_CLK_SPDIF_PODF], 98304000);
+
+	clk_set_parent(clks[IMX6SX_CLK_AUDIO_SEL], clks[IMX6SX_CLK_PLL3_USB_OTG]);
+	clk_set_rate(clks[IMX6SX_CLK_AUDIO_PODF], 24000000);
+
+	clk_set_parent(clks[IMX6SX_CLK_SSI1_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_parent(clks[IMX6SX_CLK_SSI2_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_parent(clks[IMX6SX_CLK_SSI3_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_rate(clks[IMX6SX_CLK_SSI1_PODF], 24576000);
+	clk_set_rate(clks[IMX6SX_CLK_SSI2_PODF], 24576000);
+	clk_set_rate(clks[IMX6SX_CLK_SSI3_PODF], 24576000);
+
+	clk_set_parent(clks[IMX6SX_CLK_ESAI_SEL], clks[IMX6SX_CLK_PLL4_AUDIO_DIV]);
+	clk_set_rate(clks[IMX6SX_CLK_ESAI_PODF], 24576000);
+
+	/* Set parent clock for vadc */
+	clk_set_parent(clks[IMX6SX_CLK_VID_SEL], clks[IMX6SX_CLK_PLL3_USB_OTG]);
+
+	/* default parent of can_sel clock is invalid, manually set it here */
+	clk_set_parent(clks[IMX6SX_CLK_CAN_SEL], clks[IMX6SX_CLK_PLL3_60M]);
+
+	/* Update gpu clock from default 528M to 720M */
+	clk_set_parent(clks[IMX6SX_CLK_GPU_CORE_SEL], clks[IMX6SX_CLK_PLL3_PFD0]);
+	clk_set_parent(clks[IMX6SX_CLK_GPU_AXI_SEL], clks[IMX6SX_CLK_PLL3_PFD0]);
+
+	clk_set_parent(clks[IMX6SX_CLK_QSPI1_SEL], clks[IMX6SX_CLK_PLL2_BUS]);
+	clk_set_parent(clks[IMX6SX_CLK_QSPI2_SEL], clks[IMX6SX_CLK_PLL2_BUS]);
+}
+CLK_OF_DECLARE(imx6sx, "fsl,imx6sx-ccm", imx6sx_clocks_init);
diff --git a/drivers/clk/imx/clk-imx7d.c b/drivers/clk/imx/clk-imx7d.c
new file mode 100644
index 000000000000..71f3a94b472c
--- /dev/null
+++ b/drivers/clk/imx/clk-imx7d.c
@@ -0,0 +1,860 @@
+/*
+ * Copyright (C) 2014-2015 Freescale Semiconductor, Inc.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <dt-bindings/clock/imx7d-clock.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/types.h>
+
+#include "clk.h"
+
+static struct clk *clks[IMX7D_CLK_END];
+static const char *arm_a7_sel[] = { "osc", "pll_arm_main_clk",
+	"pll_enet_500m_clk", "pll_dram_main_clk",
+	"pll_sys_main_clk", "pll_sys_pfd0_392m_clk", "pll_audio_main_clk",
+	"pll_usb_main_clk", };
+
+static const char *arm_m4_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_250m_clk", "pll_sys_pfd2_270m_clk",
+	"pll_dram_533m_clk", "pll_audio_main_clk", "pll_video_main_clk",
+	"pll_usb_main_clk", };
+
+static const char *arm_m0_sel[] = { "osc", "pll_sys_main_120m_clk",
+	"pll_enet_125m_clk", "pll_sys_pfd2_135m_clk",
+	"pll_dram_533m_clk", "pll_audio_main_clk", "pll_video_main_clk",
+	"pll_usb_main_clk", };
+
+static const char *axi_sel[] = { "osc", "pll_sys_pfd1_332m_clk",
+	"pll_dram_533m_clk", "pll_enet_250m_clk", "pll_sys_pfd5_clk",
+	"pll_audio_main_clk", "pll_video_main_clk", "pll_sys_pfd7_clk", };
+
+static const char *disp_axi_sel[] = { "osc", "pll_sys_pfd1_332m_clk",
+	"pll_dram_533m_clk", "pll_enet_250m_clk", "pll_sys_pfd6_clk",
+	"pll_sys_pfd7_clk", "pll_audio_main_clk", "pll_video_main_clk", };
+
+static const char *enet_axi_sel[] = { "osc", "pll_sys_pfd2_270m_clk",
+	"pll_dram_533m_clk", "pll_enet_250m_clk",
+	"pll_sys_main_240m_clk", "pll_audio_main_clk", "pll_video_main_clk",
+	"pll_sys_pfd4_clk", };
+
+static const char *nand_usdhc_bus_sel[] = { "osc", "pll_sys_pfd2_270m_clk",
+	"pll_dram_533m_clk", "pll_sys_main_240m_clk",
+	"pll_sys_pfd2_135m_clk", "pll_sys_pfd6_clk", "pll_enet_250m_clk",
+	"pll_audio_main_clk", };
+
+static const char *ahb_channel_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_dram_533m_clk", "pll_sys_pfd0_392m_clk",
+	"pll_enet_125m_clk", "pll_usb_main_clk", "pll_audio_main_clk",
+	"pll_video_main_clk", };
+
+static const char *dram_phym_sel[] = { "pll_dram_main_clk",
+	"dram_phym_alt_clk", };
+
+static const char *dram_sel[] = { "pll_dram_main_clk",
+	"dram_alt_clk", };
+
+static const char *dram_phym_alt_sel[] = { "osc", "pll_dram_533m_clk",
+	"pll_sys_main_clk", "pll_enet_500m_clk",
+	"pll_usb_main_clk", "pll_sys_pfd7_clk", "pll_audio_main_clk",
+	"pll_video_main_clk", };
+
+static const char *dram_alt_sel[] = { "osc", "pll_dram_533m_clk",
+	"pll_sys_main_clk", "pll_enet_500m_clk",
+	"pll_enet_250m_clk", "pll_sys_pfd0_392m_clk",
+	"pll_audio_main_clk", "pll_sys_pfd2_270m_clk", };
+
+static const char *usb_hsic_sel[] = { "osc", "pll_sys_main_clk",
+	"pll_usb_main_clk", "pll_sys_pfd3_clk", "pll_sys_pfd4_clk",
+	"pll_sys_pfd5_clk", "pll_sys_pfd6_clk", "pll_sys_pfd7_clk", };
+
+static const char *pcie_ctrl_sel[] = { "osc", "pll_enet_250m_clk",
+	"pll_sys_main_240m_clk", "pll_sys_pfd2_270m_clk",
+	"pll_dram_533m_clk", "pll_enet_500m_clk",
+	"pll_sys_pfd1_332m_clk", "pll_sys_pfd6_clk", };
+
+static const char *pcie_phy_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_enet_500m_clk", "ext_clk_1", "ext_clk_2", "ext_clk_3",
+	"ext_clk_4", "pll_sys_pfd0_392m_clk", };
+
+static const char *epdc_pixel_sel[] = { "osc", "pll_sys_pfd1_332m_clk",
+	"pll_dram_533m_clk", "pll_sys_main_clk", "pll_sys_pfd5_clk",
+	"pll_sys_pfd6_clk", "pll_sys_pfd7_clk", "pll_video_main_clk", };
+
+static const char *lcdif_pixel_sel[] = { "osc", "pll_sys_pfd5_clk",
+	"pll_dram_533m_clk", "ext_clk_3", "pll_sys_pfd4_clk",
+	"pll_sys_pfd2_270m_clk", "pll_video_main_clk",
+	"pll_usb_main_clk", };
+
+static const char *mipi_dsi_sel[] = { "osc", "pll_sys_pfd5_clk",
+	"pll_sys_pfd3_clk", "pll_sys_main_clk", "pll_sys_pfd0_196m_clk",
+	"pll_dram_533m_clk", "pll_video_main_clk", "pll_audio_main_clk", };
+
+static const char *mipi_csi_sel[] = { "osc", "pll_sys_pfd4_clk",
+	"pll_sys_pfd3_clk", "pll_sys_main_clk", "pll_sys_pfd0_196m_clk",
+	"pll_dram_533m_clk", "pll_video_main_clk", "pll_audio_main_clk", };
+
+static const char *mipi_dphy_sel[] = { "osc", "pll_sys_main_120m_clk",
+	"pll_dram_533m_clk", "pll_sys_pfd5_clk", "ref_1m_clk", "ext_clk_2",
+	"pll_video_main_clk", "ext_clk_3", };
+
+static const char *sai1_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_audio_main_clk", "pll_dram_533m_clk", "pll_video_main_clk",
+	"pll_sys_pfd4_clk", "pll_enet_125m_clk", "ext_clk_2", };
+
+static const char *sai2_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_audio_main_clk", "pll_dram_533m_clk", "pll_video_main_clk",
+	"pll_sys_pfd4_clk", "pll_enet_125m_clk", "ext_clk_2", };
+
+static const char *sai3_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_audio_main_clk", "pll_dram_533m_clk", "pll_video_main_clk",
+	"pll_sys_pfd4_clk", "pll_enet_125m_clk", "ext_clk_3", };
+
+static const char *spdif_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_audio_main_clk", "pll_dram_533m_clk", "pll_video_main_clk",
+	"pll_sys_pfd4_clk", "pll_enet_125m_clk", "ext_3_clk", };
+
+static const char *enet1_ref_sel[] = { "osc", "pll_enet_125m_clk",
+	"pll_enet_50m_clk", "pll_enet_25m_clk",
+	"pll_sys_main_120m_clk", "pll_audio_main_clk", "pll_video_main_clk",
+	"ext_clk_4", };
+
+static const char *enet1_time_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_audio_main_clk", "ext_clk_1", "ext_clk_2", "ext_clk_3",
+	"ext_clk_4", "pll_video_main_clk", };
+
+static const char *enet2_ref_sel[] = { "osc", "pll_enet_125m_clk",
+	"pll_enet_50m_clk", "pll_enet_25m_clk",
+	"pll_sys_main_120m_clk", "pll_audio_main_clk", "pll_video_main_clk",
+	"ext_clk_4", };
+
+static const char *enet2_time_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_audio_main_clk", "ext_clk_1", "ext_clk_2", "ext_clk_3",
+	"ext_clk_4", "pll_video_main_clk", };
+
+static const char *enet_phy_ref_sel[] = { "osc", "pll_enet_25m_clk",
+	"pll_enet_50m_clk", "pll_enet_125m_clk",
+	"pll_dram_533m_clk", "pll_audio_main_clk", "pll_video_main_clk",
+	"pll_sys_pfd3_clk", };
+
+static const char *eim_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_sys_main_120m_clk", "pll_dram_533m_clk",
+	"pll_sys_pfd2_270m_clk", "pll_sys_pfd3_clk", "pll_enet_125m_clk",
+	"pll_usb_main_clk", };
+
+static const char *nand_sel[] = { "osc", "pll_sys_main_clk",
+	"pll_dram_533m_clk", "pll_sys_pfd0_392m_clk", "pll_sys_pfd3_clk",
+	"pll_enet_500m_clk", "pll_enet_250m_clk",
+	"pll_video_main_clk", };
+
+static const char *qspi_sel[] = { "osc", "pll_sys_pfd4_clk",
+	"pll_dram_533m_clk", "pll_enet_500m_clk", "pll_sys_pfd3_clk",
+	"pll_sys_pfd2_270m_clk", "pll_sys_pfd6_clk", "pll_sys_pfd7_clk", };
+
+static const char *usdhc1_sel[] = { "osc", "pll_sys_pfd0_392m_clk",
+	"pll_dram_533m_clk", "pll_enet_500m_clk", "pll_sys_pfd4_clk",
+	"pll_sys_pfd2_270m_clk", "pll_sys_pfd6_clk", "pll_sys_pfd7_clk", };
+
+static const char *usdhc2_sel[] = { "osc", "pll_sys_pfd0_392m_clk",
+	"pll_dram_533m_clk", "pll_enet_500m_clk", "pll_sys_pfd4_clk",
+	"pll_sys_pfd2_270m_clk", "pll_sys_pfd6_clk", "pll_sys_pfd7_clk", };
+
+static const char *usdhc3_sel[] = { "osc", "pll_sys_pfd0_392m_clk",
+	"pll_dram_533m_clk", "pll_enet_500m_clk", "pll_sys_pfd4_clk",
+	"pll_sys_pfd2_270m_clk", "pll_sys_pfd6_clk", "pll_sys_pfd7_clk", };
+
+static const char *can1_sel[] = { "osc", "pll_sys_main_120m_clk",
+	"pll_dram_533m_clk", "pll_sys_main_clk",
+	"pll_enet_40m_clk", "pll_usb_main_clk", "ext_clk_1",
+	"ext_clk_4", };
+
+static const char *can2_sel[] = { "osc", "pll_sys_main_120m_clk",
+	"pll_dram_533m_clk", "pll_sys_main_clk",
+	"pll_enet_40m_clk", "pll_usb_main_clk", "ext_clk_1",
+	"ext_clk_3", };
+
+static const char *i2c1_sel[] = { "osc", "pll_sys_main_120m_clk",
+	"pll_enet_50m_clk", "pll_dram_533m_clk",
+	"pll_audio_main_clk", "pll_video_main_clk", "pll_usb_main_clk",
+	"pll_sys_pfd2_135m_clk", };
+
+static const char *i2c2_sel[] = { "osc", "pll_sys_main_120m_clk",
+	"pll_enet_50m_clk", "pll_dram_533m_clk",
+	"pll_audio_main_clk", "pll_video_main_clk", "pll_usb_main_clk",
+	"pll_sys_pfd2_135m_clk", };
+
+static const char *i2c3_sel[] = { "osc", "pll_sys_main_120m_clk",
+	"pll_enet_50m_clk", "pll_dram_533m_clk",
+	"pll_audio_main_clk", "pll_video_main_clk", "pll_usb_main_clk",
+	"pll_sys_pfd2_135m_clk", };
+
+static const char *i2c4_sel[] = { "osc", "pll_sys_main_120m_clk",
+	"pll_enet_50m_clk", "pll_dram_533m_clk",
+	"pll_audio_main_clk", "pll_video_main_clk", "pll_usb_main_clk",
+	"pll_sys_pfd2_135m_clk", };
+
+static const char *uart1_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_enet_100m_clk",
+	"pll_sys_main_clk", "ext_clk_2", "ext_clk_4",
+	"pll_usb_main_clk", };
+
+static const char *uart2_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_enet_100m_clk",
+	"pll_sys_main_clk", "ext_clk_2", "ext_clk_3",
+	"pll_usb_main_clk", };
+
+static const char *uart3_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_enet_100m_clk",
+	"pll_sys_main_clk", "ext_clk_2", "ext_clk_4",
+	"pll_usb_main_clk", };
+
+static const char *uart4_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_enet_100m_clk",
+	"pll_sys_main_clk", "ext_clk_2", "ext_clk_3",
+	"pll_usb_main_clk", };
+
+static const char *uart5_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_enet_100m_clk",
+	"pll_sys_main_clk", "ext_clk_2", "ext_clk_4",
+	"pll_usb_main_clk", };
+
+static const char *uart6_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_enet_100m_clk",
+	"pll_sys_main_clk", "ext_clk_2", "ext_clk_3",
+	"pll_usb_main_clk", };
+
+static const char *uart7_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_enet_100m_clk",
+	"pll_sys_main_clk", "ext_clk_2", "ext_clk_4",
+	"pll_usb_main_clk", };
+
+static const char *ecspi1_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_sys_main_120m_clk",
+	"pll_sys_main_clk", "pll_sys_pfd4_clk", "pll_enet_250m_clk",
+	"pll_usb_main_clk", };
+
+static const char *ecspi2_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_sys_main_120m_clk",
+	"pll_sys_main_clk", "pll_sys_pfd4_clk", "pll_enet_250m_clk",
+	"pll_usb_main_clk", };
+
+static const char *ecspi3_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_sys_main_120m_clk",
+	"pll_sys_main_clk", "pll_sys_pfd4_clk", "pll_enet_250m_clk",
+	"pll_usb_main_clk", };
+
+static const char *ecspi4_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_enet_40m_clk", "pll_sys_main_120m_clk",
+	"pll_sys_main_clk", "pll_sys_pfd4_clk", "pll_enet_250m_clk",
+	"pll_usb_main_clk", };
+
+static const char *pwm1_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_main_clk",
+	"ext_clk_1", "ref_1m_clk", "pll_video_main_clk", };
+
+static const char *pwm2_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_main_clk",
+	"ext_clk_1", "ref_1m_clk", "pll_video_main_clk", };
+
+static const char *pwm3_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_main_clk",
+	"ext_clk_2", "ref_1m_clk", "pll_video_main_clk", };
+
+static const char *pwm4_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_main_clk",
+	"ext_clk_2", "ref_1m_clk", "pll_video_main_clk", };
+
+static const char *flextimer1_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_main_clk",
+	"ext_clk_3", "ref_1m_clk", "pll_video_main_clk", };
+
+static const char *flextimer2_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_main_120m_clk", "pll_enet_40m_clk", "pll_audio_main_clk",
+	"ext_clk_3", "ref_1m_clk", "pll_video_main_clk", };
+
+static const char *sim1_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_sys_main_120m_clk", "pll_dram_533m_clk",
+	"pll_usb_main_clk", "pll_audio_main_clk", "pll_enet_125m_clk",
+	"pll_sys_pfd7_clk", };
+
+static const char *sim2_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_sys_main_120m_clk", "pll_dram_533m_clk",
+	"pll_usb_main_clk", "pll_video_main_clk", "pll_enet_125m_clk",
+	"pll_sys_pfd7_clk", };
+
+static const char *gpt1_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_pfd0_392m_clk", "pll_enet_40m_clk", "pll_video_main_clk",
+	"ref_1m_clk", "pll_audio_main_clk", "ext_clk_1", };
+
+static const char *gpt2_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_pfd0_392m_clk", "pll_enet_40m_clk", "pll_video_main_clk",
+	"ref_1m_clk", "pll_audio_main_clk", "ext_clk_2", };
+
+static const char *gpt3_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_pfd0_392m_clk", "pll_enet_40m_clk", "pll_video_main_clk",
+	"ref_1m_clk", "pll_audio_main_clk", "ext_clk_3", };
+
+static const char *gpt4_sel[] = { "osc", "pll_enet_100m_clk",
+	"pll_sys_pfd0_392m_clk", "pll_enet_40m_clk", "pll_video_main_clk",
+	"ref_1m_clk", "pll_audio_main_clk", "ext_clk_4", };
+
+static const char *trace_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_sys_main_120m_clk", "pll_dram_533m_clk",
+	"pll_enet_125m_clk", "pll_usb_main_clk", "ext_clk_2",
+	"ext_clk_3", };
+
+static const char *wdog_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_sys_main_120m_clk", "pll_dram_533m_clk",
+	"pll_enet_125m_clk", "pll_usb_main_clk", "ref_1m_clk",
+	"pll_sys_pfd1_166m_clk", };
+
+static const char *csi_mclk_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_sys_main_120m_clk", "pll_dram_533m_clk",
+	"pll_enet_125m_clk", "pll_audio_main_clk", "pll_video_main_clk",
+	"pll_usb_main_clk", };
+
+static const char *audio_mclk_sel[] = { "osc", "pll_sys_pfd2_135m_clk",
+	"pll_sys_main_120m_clk", "pll_dram_533m_clk",
+	"pll_enet_125m_clk", "pll_audio_main_clk", "pll_video_main_clk",
+	"pll_usb_main_clk", };
+
+static const char *wrclk_sel[] = { "osc", "pll_enet_40m_clk",
+	"pll_dram_533m_clk", "pll_usb_main_clk",
+	"pll_sys_main_240m_clk", "pll_sys_pfd2_270m_clk",
+	"pll_enet_500m_clk", "pll_sys_pfd7_clk", };
+
+static const char *clko1_sel[] = { "osc", "pll_sys_main_clk",
+	"pll_sys_main_240m_clk", "pll_sys_pfd0_196m_clk", "pll_sys_pfd3_clk",
+	"pll_enet_500m_clk", "pll_dram_533m_clk", "ref_1m_clk", };
+
+static const char *clko2_sel[] = { "osc", "pll_sys_main_240m_clk",
+	"pll_sys_pfd0_392m_clk", "pll_sys_pfd1_166m_clk", "pll_sys_pfd4_clk",
+	"pll_audio_main_clk", "pll_video_main_clk", "osc_32k_clk", };
+
+static const char *lvds1_sel[] = { "pll_arm_main_clk",
+	"pll_sys_main_clk", "pll_sys_pfd0_392m_clk", "pll_sys_pfd1_332m_clk",
+	"pll_sys_pfd2_270m_clk", "pll_sys_pfd3_clk", "pll_sys_pfd4_clk",
+	"pll_sys_pfd5_clk", "pll_sys_pfd6_clk", "pll_sys_pfd7_clk",
+	"pll_audio_main_clk", "pll_video_main_clk", "pll_enet_500m_clk",
+	"pll_enet_250m_clk", "pll_enet_125m_clk", "pll_enet_100m_clk",
+	"pll_enet_50m_clk", "pll_enet_40m_clk", "pll_enet_25m_clk",
+	"pll_dram_main_clk", };
+
+static const char *pll_bypass_src_sel[] = { "osc", "dummy", };
+static const char *pll_arm_bypass_sel[] = { "pll_arm_main", "pll_arm_main_src", };
+static const char *pll_dram_bypass_sel[] = { "pll_dram_main", "pll_dram_main_src", };
+static const char *pll_sys_bypass_sel[] = { "pll_sys_main", "pll_sys_main_src", };
+static const char *pll_enet_bypass_sel[] = { "pll_enet_main", "pll_enet_main_src", };
+static const char *pll_audio_bypass_sel[] = { "pll_audio_main", "pll_audio_main_src", };
+static const char *pll_video_bypass_sel[] = { "pll_video_main", "pll_video_main_src", };
+
+static struct clk_onecell_data clk_data;
+
+static void __init imx7d_clocks_init(struct device_node *ccm_node)
+{
+	struct device_node *np;
+	void __iomem *base;
+	int i;
+
+	clks[IMX7D_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+	clks[IMX7D_OSC_24M_CLK] = of_clk_get_by_name(ccm_node, "osc");
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx7d-anatop");
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+
+	clks[IMX7D_PLL_ARM_MAIN_SRC]  = imx_clk_mux("pll_arm_main_src", base + 0x60, 14, 2, pll_bypass_src_sel, ARRAY_SIZE(pll_bypass_src_sel));
+	clks[IMX7D_PLL_DRAM_MAIN_SRC] = imx_clk_mux("pll_dram_main_src", base + 0x70, 14, 2, pll_bypass_src_sel, ARRAY_SIZE(pll_bypass_src_sel));
+	clks[IMX7D_PLL_SYS_MAIN_SRC]  = imx_clk_mux("pll_sys_main_src", base + 0xb0, 14, 2, pll_bypass_src_sel, ARRAY_SIZE(pll_bypass_src_sel));
+	clks[IMX7D_PLL_ENET_MAIN_SRC] = imx_clk_mux("pll_enet_main_src", base + 0xe0, 14, 2, pll_bypass_src_sel, ARRAY_SIZE(pll_bypass_src_sel));
+	clks[IMX7D_PLL_AUDIO_MAIN_SRC] = imx_clk_mux("pll_audio_main_src", base + 0xf0, 14, 2, pll_bypass_src_sel, ARRAY_SIZE(pll_bypass_src_sel));
+	clks[IMX7D_PLL_VIDEO_MAIN_SRC] = imx_clk_mux("pll_video_main_src", base + 0x130, 14, 2, pll_bypass_src_sel, ARRAY_SIZE(pll_bypass_src_sel));
+
+	clks[IMX7D_PLL_ARM_MAIN]  = imx_clk_pllv3(IMX_PLLV3_SYS, "pll_arm_main", "pll_arm_main_src", base + 0x60, 0x7f);
+	clks[IMX7D_PLL_DRAM_MAIN] = imx_clk_pllv3(IMX_PLLV3_SYS, "pll_dram_main", "pll_dram_main_src", base + 0x70, 0x7f);
+	clks[IMX7D_PLL_SYS_MAIN]  = imx_clk_pllv3(IMX_PLLV3_GENERIC, "pll_sys_main", "pll_sys_main_src", base + 0xb0, 0x1);
+	clks[IMX7D_PLL_ENET_MAIN] = imx_clk_pllv3(IMX_PLLV3_ENET_IMX7, "pll_enet_main", "pll_enet_main_src", base + 0xe0, 0x0);
+	clks[IMX7D_PLL_AUDIO_MAIN] = imx_clk_pllv3(IMX_PLLV3_AV, "pll_audio_main", "pll_audio_main_src", base + 0xf0, 0x7f);
+	clks[IMX7D_PLL_VIDEO_MAIN] = imx_clk_pllv3(IMX_PLLV3_AV, "pll_video_main", "pll_video_main_src", base + 0x130, 0x7f);
+
+	clks[IMX7D_PLL_ARM_MAIN_BYPASS]  = imx_clk_mux_flags("pll_arm_main_bypass", base + 0x60, 16, 1, pll_arm_bypass_sel, ARRAY_SIZE(pll_arm_bypass_sel), CLK_SET_RATE_PARENT);
+	clks[IMX7D_PLL_DRAM_MAIN_BYPASS] = imx_clk_mux_flags("pll_dram_main_bypass", base + 0x70, 16, 1, pll_dram_bypass_sel, ARRAY_SIZE(pll_dram_bypass_sel), CLK_SET_RATE_PARENT);
+	clks[IMX7D_PLL_SYS_MAIN_BYPASS]  = imx_clk_mux_flags("pll_sys_main_bypass", base + 0xb0, 16, 1, pll_sys_bypass_sel, ARRAY_SIZE(pll_sys_bypass_sel), CLK_SET_RATE_PARENT);
+	clks[IMX7D_PLL_ENET_MAIN_BYPASS] = imx_clk_mux_flags("pll_enet_main_bypass", base + 0xe0, 16, 1, pll_enet_bypass_sel, ARRAY_SIZE(pll_enet_bypass_sel), CLK_SET_RATE_PARENT);
+	clks[IMX7D_PLL_AUDIO_MAIN_BYPASS] = imx_clk_mux_flags("pll_audio_main_bypass", base + 0xf0, 16, 1, pll_audio_bypass_sel, ARRAY_SIZE(pll_audio_bypass_sel), CLK_SET_RATE_PARENT);
+	clks[IMX7D_PLL_VIDEO_MAIN_BYPASS] = imx_clk_mux_flags("pll_video_main_bypass", base + 0x130, 16, 1, pll_video_bypass_sel, ARRAY_SIZE(pll_video_bypass_sel), CLK_SET_RATE_PARENT);
+
+	clk_set_parent(clks[IMX7D_PLL_ARM_MAIN_BYPASS], clks[IMX7D_PLL_ARM_MAIN]);
+	clk_set_parent(clks[IMX7D_PLL_DRAM_MAIN_BYPASS], clks[IMX7D_PLL_DRAM_MAIN]);
+	clk_set_parent(clks[IMX7D_PLL_SYS_MAIN_BYPASS], clks[IMX7D_PLL_SYS_MAIN]);
+	clk_set_parent(clks[IMX7D_PLL_ENET_MAIN_BYPASS], clks[IMX7D_PLL_ENET_MAIN]);
+	clk_set_parent(clks[IMX7D_PLL_AUDIO_MAIN_BYPASS], clks[IMX7D_PLL_AUDIO_MAIN]);
+	clk_set_parent(clks[IMX7D_PLL_VIDEO_MAIN_BYPASS], clks[IMX7D_PLL_VIDEO_MAIN]);
+
+	clks[IMX7D_PLL_ARM_MAIN_CLK] = imx_clk_gate("pll_arm_main_clk", "pll_arm_main_bypass", base + 0x60, 13);
+	clks[IMX7D_PLL_DRAM_MAIN_CLK] = imx_clk_gate("pll_dram_main_clk", "pll_dram_main_bypass", base + 0x70, 13);
+	clks[IMX7D_PLL_SYS_MAIN_CLK] = imx_clk_gate("pll_sys_main_clk", "pll_sys_main_bypass", base + 0xb0, 13);
+	clks[IMX7D_PLL_AUDIO_MAIN_CLK] = imx_clk_gate("pll_audio_main_clk", "pll_audio_main_bypass", base + 0xf0, 13);
+	clks[IMX7D_PLL_VIDEO_MAIN_CLK] = imx_clk_gate("pll_video_main_clk", "pll_video_main_bypass", base + 0x130, 13);
+
+	clks[IMX7D_PLL_SYS_PFD0_392M_CLK] = imx_clk_pfd("pll_sys_pfd0_392m_clk", "pll_sys_main_clk", base + 0xc0, 0);
+	clks[IMX7D_PLL_SYS_PFD1_332M_CLK] = imx_clk_pfd("pll_sys_pfd1_332m_clk", "pll_sys_main_clk", base + 0xc0, 1);
+	clks[IMX7D_PLL_SYS_PFD2_270M_CLK] = imx_clk_pfd("pll_sys_pfd2_270m_clk", "pll_sys_main_clk", base + 0xc0, 2);
+
+	clks[IMX7D_PLL_SYS_PFD3_CLK] = imx_clk_pfd("pll_sys_pfd3_clk", "pll_sys_main_clk", base + 0xc0, 3);
+	clks[IMX7D_PLL_SYS_PFD4_CLK] = imx_clk_pfd("pll_sys_pfd4_clk", "pll_sys_main_clk", base + 0xd0, 0);
+	clks[IMX7D_PLL_SYS_PFD5_CLK] = imx_clk_pfd("pll_sys_pfd5_clk", "pll_sys_main_clk", base + 0xd0, 1);
+	clks[IMX7D_PLL_SYS_PFD6_CLK] = imx_clk_pfd("pll_sys_pfd6_clk", "pll_sys_main_clk", base + 0xd0, 2);
+	clks[IMX7D_PLL_SYS_PFD7_CLK] = imx_clk_pfd("pll_sys_pfd7_clk", "pll_sys_main_clk", base + 0xd0, 3);
+
+	clks[IMX7D_PLL_SYS_MAIN_480M] = imx_clk_fixed_factor("pll_sys_main_480m", "pll_sys_main_clk", 1, 1);
+	clks[IMX7D_PLL_SYS_MAIN_240M] = imx_clk_fixed_factor("pll_sys_main_240m", "pll_sys_main_clk", 1, 2);
+	clks[IMX7D_PLL_SYS_MAIN_120M] = imx_clk_fixed_factor("pll_sys_main_120m", "pll_sys_main_clk", 1, 4);
+	clks[IMX7D_PLL_DRAM_MAIN_533M] = imx_clk_fixed_factor("pll_dram_533m", "pll_dram_main_clk", 1, 2);
+
+	clks[IMX7D_PLL_SYS_MAIN_480M_CLK] = imx_clk_gate_dis("pll_sys_main_480m_clk", "pll_sys_main_480m", base + 0xb0, 4);
+	clks[IMX7D_PLL_SYS_MAIN_240M_CLK] = imx_clk_gate_dis("pll_sys_main_240m_clk", "pll_sys_main_240m", base + 0xb0, 5);
+	clks[IMX7D_PLL_SYS_MAIN_120M_CLK] = imx_clk_gate_dis("pll_sys_main_120m_clk", "pll_sys_main_120m", base + 0xb0, 6);
+	clks[IMX7D_PLL_DRAM_MAIN_533M_CLK] = imx_clk_gate("pll_dram_533m_clk", "pll_dram_533m", base + 0x70, 12);
+
+	clks[IMX7D_PLL_SYS_PFD0_196M] = imx_clk_fixed_factor("pll_sys_pfd0_196m", "pll_sys_pfd0_392m_clk", 1, 2);
+	clks[IMX7D_PLL_SYS_PFD1_166M] = imx_clk_fixed_factor("pll_sys_pfd1_166m", "pll_sys_pfd1_332m_clk", 1, 2);
+	clks[IMX7D_PLL_SYS_PFD2_135M] = imx_clk_fixed_factor("pll_sys_pfd2_135m", "pll_sys_pfd2_270m_clk", 1, 2);
+
+	clks[IMX7D_PLL_SYS_PFD0_196M_CLK] = imx_clk_gate_dis("pll_sys_pfd0_196m_clk", "pll_sys_pfd0_196m", base + 0xb0, 26);
+	clks[IMX7D_PLL_SYS_PFD1_166M_CLK] = imx_clk_gate_dis("pll_sys_pfd1_166m_clk", "pll_sys_pfd1_166m", base + 0xb0, 27);
+	clks[IMX7D_PLL_SYS_PFD2_135M_CLK] = imx_clk_gate_dis("pll_sys_pfd2_135m_clk", "pll_sys_pfd2_135m", base + 0xb0, 28);
+
+	clks[IMX7D_PLL_ENET_MAIN_CLK] = imx_clk_fixed_factor("pll_enet_main_clk", "pll_enet_main_bypass", 1, 1);
+	clks[IMX7D_PLL_ENET_MAIN_500M] = imx_clk_fixed_factor("pll_enet_500m", "pll_enet_main_clk", 1, 2);
+	clks[IMX7D_PLL_ENET_MAIN_250M] = imx_clk_fixed_factor("pll_enet_250m", "pll_enet_main_clk", 1, 4);
+	clks[IMX7D_PLL_ENET_MAIN_125M] = imx_clk_fixed_factor("pll_enet_125m", "pll_enet_main_clk", 1, 8);
+	clks[IMX7D_PLL_ENET_MAIN_100M] = imx_clk_fixed_factor("pll_enet_100m", "pll_enet_main_clk", 1, 10);
+	clks[IMX7D_PLL_ENET_MAIN_50M] = imx_clk_fixed_factor("pll_enet_50m", "pll_enet_main_clk", 1, 20);
+	clks[IMX7D_PLL_ENET_MAIN_40M] = imx_clk_fixed_factor("pll_enet_40m", "pll_enet_main_clk", 1, 25);
+	clks[IMX7D_PLL_ENET_MAIN_25M] = imx_clk_fixed_factor("pll_enet_25m", "pll_enet_main_clk", 1, 40);
+
+	clks[IMX7D_PLL_ENET_MAIN_500M_CLK] = imx_clk_gate("pll_enet_500m_clk", "pll_enet_500m", base + 0xe0, 12);
+	clks[IMX7D_PLL_ENET_MAIN_250M_CLK] = imx_clk_gate("pll_enet_250m_clk", "pll_enet_250m", base + 0xe0, 11);
+	clks[IMX7D_PLL_ENET_MAIN_125M_CLK] = imx_clk_gate("pll_enet_125m_clk", "pll_enet_125m", base + 0xe0, 10);
+	clks[IMX7D_PLL_ENET_MAIN_100M_CLK] = imx_clk_gate("pll_enet_100m_clk", "pll_enet_100m", base + 0xe0, 9);
+	clks[IMX7D_PLL_ENET_MAIN_50M_CLK]  = imx_clk_gate("pll_enet_50m_clk", "pll_enet_50m", base + 0xe0, 8);
+	clks[IMX7D_PLL_ENET_MAIN_40M_CLK]  = imx_clk_gate("pll_enet_40m_clk", "pll_enet_40m", base + 0xe0, 7);
+	clks[IMX7D_PLL_ENET_MAIN_25M_CLK]  = imx_clk_gate("pll_enet_25m_clk", "pll_enet_25m", base + 0xe0, 6);
+
+	clks[IMX7D_LVDS1_OUT_SEL] = imx_clk_mux("lvds1_sel", base + 0x170, 0, 5, lvds1_sel, ARRAY_SIZE(lvds1_sel));
+	clks[IMX7D_LVDS1_OUT_CLK] = imx_clk_gate_exclusive("lvds1_out", "lvds1_sel", base + 0x170, 5, BIT(6));
+
+	np = ccm_node;
+	base = of_iomap(np, 0);
+	WARN_ON(!base);
+
+	clks[IMX7D_ARM_A7_ROOT_SRC] = imx_clk_mux("arm_a7_src", base + 0x8000, 24, 3, arm_a7_sel, ARRAY_SIZE(arm_a7_sel));
+	clks[IMX7D_ARM_M4_ROOT_SRC] = imx_clk_mux("arm_m4_src", base + 0x8080, 24, 3, arm_m4_sel, ARRAY_SIZE(arm_m4_sel));
+	clks[IMX7D_ARM_M0_ROOT_SRC] = imx_clk_mux("arm_m0_src", base + 0x8100, 24, 3, arm_m0_sel, ARRAY_SIZE(arm_m0_sel));
+	clks[IMX7D_MAIN_AXI_ROOT_SRC] = imx_clk_mux("axi_src", base + 0x8800, 24, 3, axi_sel, ARRAY_SIZE(axi_sel));
+	clks[IMX7D_DISP_AXI_ROOT_SRC] = imx_clk_mux("disp_axi_src", base + 0x8880, 24, 3, disp_axi_sel, ARRAY_SIZE(disp_axi_sel));
+	clks[IMX7D_ENET_AXI_ROOT_SRC] = imx_clk_mux("enet_axi_src", base + 0x8900, 24, 3, enet_axi_sel, ARRAY_SIZE(enet_axi_sel));
+	clks[IMX7D_NAND_USDHC_BUS_ROOT_SRC] = imx_clk_mux("nand_usdhc_src", base + 0x8980, 24, 3, nand_usdhc_bus_sel, ARRAY_SIZE(nand_usdhc_bus_sel));
+	clks[IMX7D_AHB_CHANNEL_ROOT_SRC] = imx_clk_mux("ahb_src", base + 0x9000, 24, 3, ahb_channel_sel, ARRAY_SIZE(ahb_channel_sel));
+	clks[IMX7D_DRAM_PHYM_ROOT_SRC] = imx_clk_mux("dram_phym_src", base + 0x9800, 24, 1, dram_phym_sel, ARRAY_SIZE(dram_phym_sel));
+	clks[IMX7D_DRAM_ROOT_SRC] = imx_clk_mux("dram_src", base + 0x9880, 24, 1, dram_sel, ARRAY_SIZE(dram_sel));
+	clks[IMX7D_DRAM_PHYM_ALT_ROOT_SRC] = imx_clk_mux("dram_phym_alt_src", base + 0xa000, 24, 3, dram_phym_alt_sel, ARRAY_SIZE(dram_phym_alt_sel));
+	clks[IMX7D_DRAM_ALT_ROOT_SRC]  = imx_clk_mux("dram_alt_src", base + 0xa080, 24, 3, dram_alt_sel, ARRAY_SIZE(dram_alt_sel));
+	clks[IMX7D_USB_HSIC_ROOT_SRC] = imx_clk_mux("usb_hsic_src", base + 0xa100, 24, 3, usb_hsic_sel, ARRAY_SIZE(usb_hsic_sel));
+	clks[IMX7D_PCIE_CTRL_ROOT_SRC] = imx_clk_mux("pcie_ctrl_src", base + 0xa180, 24, 3, pcie_ctrl_sel, ARRAY_SIZE(pcie_ctrl_sel));
+	clks[IMX7D_PCIE_PHY_ROOT_SRC] = imx_clk_mux("pcie_phy_src", base + 0xa200, 24, 3, pcie_phy_sel, ARRAY_SIZE(pcie_phy_sel));
+	clks[IMX7D_EPDC_PIXEL_ROOT_SRC] = imx_clk_mux("epdc_pixel_src", base + 0xa280, 24, 3, epdc_pixel_sel, ARRAY_SIZE(epdc_pixel_sel));
+	clks[IMX7D_LCDIF_PIXEL_ROOT_SRC] = imx_clk_mux("lcdif_pixel_src", base + 0xa300, 24, 3, lcdif_pixel_sel, ARRAY_SIZE(lcdif_pixel_sel));
+	clks[IMX7D_MIPI_DSI_ROOT_SRC] = imx_clk_mux("mipi_dsi_src", base + 0xa380, 24, 3,  mipi_dsi_sel, ARRAY_SIZE(mipi_dsi_sel));
+	clks[IMX7D_MIPI_CSI_ROOT_SRC] = imx_clk_mux("mipi_csi_src", base + 0xa400, 24, 3, mipi_csi_sel, ARRAY_SIZE(mipi_csi_sel));
+	clks[IMX7D_MIPI_DPHY_ROOT_SRC] = imx_clk_mux("mipi_dphy_src", base + 0xa480, 24, 3, mipi_dphy_sel, ARRAY_SIZE(mipi_dphy_sel));
+	clks[IMX7D_SAI1_ROOT_SRC] = imx_clk_mux("sai1_src", base + 0xa500, 24, 3, sai1_sel, ARRAY_SIZE(sai1_sel));
+	clks[IMX7D_SAI2_ROOT_SRC] = imx_clk_mux("sai2_src", base + 0xa580, 24, 3, sai2_sel, ARRAY_SIZE(sai2_sel));
+	clks[IMX7D_SAI3_ROOT_SRC] = imx_clk_mux("sai3_src", base + 0xa600, 24, 3, sai3_sel, ARRAY_SIZE(sai3_sel));
+	clks[IMX7D_SPDIF_ROOT_SRC] = imx_clk_mux("spdif_src", base + 0xa680, 24, 3, spdif_sel, ARRAY_SIZE(spdif_sel));
+	clks[IMX7D_ENET1_REF_ROOT_SRC] = imx_clk_mux("enet1_ref_src", base + 0xa700, 24, 3, enet1_ref_sel, ARRAY_SIZE(enet1_ref_sel));
+	clks[IMX7D_ENET1_TIME_ROOT_SRC] = imx_clk_mux("enet1_time_src", base + 0xa780, 24, 3, enet1_time_sel, ARRAY_SIZE(enet1_time_sel));
+	clks[IMX7D_ENET2_REF_ROOT_SRC] = imx_clk_mux("enet2_ref_src", base + 0xa800, 24, 3, enet2_ref_sel, ARRAY_SIZE(enet2_ref_sel));
+	clks[IMX7D_ENET2_TIME_ROOT_SRC] = imx_clk_mux("enet2_time_src", base + 0xa880, 24, 3, enet2_time_sel, ARRAY_SIZE(enet2_time_sel));
+	clks[IMX7D_ENET_PHY_REF_ROOT_SRC] = imx_clk_mux("enet_phy_ref_src", base + 0xa900, 24, 3, enet_phy_ref_sel, ARRAY_SIZE(enet_phy_ref_sel));
+	clks[IMX7D_EIM_ROOT_SRC] = imx_clk_mux("eim_src", base + 0xa980, 24, 3, eim_sel, ARRAY_SIZE(eim_sel));
+	clks[IMX7D_NAND_ROOT_SRC] = imx_clk_mux("nand_src", base + 0xaa00, 24, 3, nand_sel, ARRAY_SIZE(nand_sel));
+	clks[IMX7D_QSPI_ROOT_SRC] = imx_clk_mux("qspi_src", base + 0xaa80, 24, 3, qspi_sel, ARRAY_SIZE(qspi_sel));
+	clks[IMX7D_USDHC1_ROOT_SRC] = imx_clk_mux("usdhc1_src", base + 0xab00, 24, 3, usdhc1_sel, ARRAY_SIZE(usdhc1_sel));
+	clks[IMX7D_USDHC2_ROOT_SRC] = imx_clk_mux("usdhc2_src", base + 0xab80, 24, 3, usdhc2_sel, ARRAY_SIZE(usdhc2_sel));
+	clks[IMX7D_USDHC3_ROOT_SRC] = imx_clk_mux("usdhc3_src", base + 0xac00, 24, 3, usdhc3_sel, ARRAY_SIZE(usdhc3_sel));
+	clks[IMX7D_CAN1_ROOT_SRC] = imx_clk_mux("can1_src", base + 0xac80, 24, 3, can1_sel, ARRAY_SIZE(can1_sel));
+	clks[IMX7D_CAN2_ROOT_SRC] = imx_clk_mux("can2_src", base + 0xad00, 24, 3, can2_sel, ARRAY_SIZE(can2_sel));
+	clks[IMX7D_I2C1_ROOT_SRC] = imx_clk_mux("i2c1_src", base + 0xad80, 24, 3, i2c1_sel, ARRAY_SIZE(i2c1_sel));
+	clks[IMX7D_I2C2_ROOT_SRC] = imx_clk_mux("i2c2_src", base + 0xae00, 24, 3, i2c2_sel, ARRAY_SIZE(i2c2_sel));
+	clks[IMX7D_I2C3_ROOT_SRC] = imx_clk_mux("i2c3_src", base + 0xae80, 24, 3, i2c3_sel, ARRAY_SIZE(i2c3_sel));
+	clks[IMX7D_I2C4_ROOT_SRC] = imx_clk_mux("i2c4_src", base + 0xaf00, 24, 3, i2c4_sel, ARRAY_SIZE(i2c4_sel));
+	clks[IMX7D_UART1_ROOT_SRC] = imx_clk_mux("uart1_src", base + 0xaf80, 24, 3, uart1_sel, ARRAY_SIZE(uart1_sel));
+	clks[IMX7D_UART2_ROOT_SRC] = imx_clk_mux("uart2_src", base + 0xb000, 24, 3, uart2_sel, ARRAY_SIZE(uart2_sel));
+	clks[IMX7D_UART3_ROOT_SRC] = imx_clk_mux("uart3_src", base + 0xb080, 24, 3, uart3_sel, ARRAY_SIZE(uart3_sel));
+	clks[IMX7D_UART4_ROOT_SRC] = imx_clk_mux("uart4_src", base + 0xb100, 24, 3, uart4_sel, ARRAY_SIZE(uart4_sel));
+	clks[IMX7D_UART5_ROOT_SRC] = imx_clk_mux("uart5_src", base + 0xb180, 24, 3, uart5_sel, ARRAY_SIZE(uart5_sel));
+	clks[IMX7D_UART6_ROOT_SRC] = imx_clk_mux("uart6_src", base + 0xb200, 24, 3, uart6_sel, ARRAY_SIZE(uart6_sel));
+	clks[IMX7D_UART7_ROOT_SRC] = imx_clk_mux("uart7_src", base + 0xb280, 24, 3, uart7_sel, ARRAY_SIZE(uart7_sel));
+	clks[IMX7D_ECSPI1_ROOT_SRC] = imx_clk_mux("ecspi1_src", base + 0xb300, 24, 3, ecspi1_sel, ARRAY_SIZE(ecspi1_sel));
+	clks[IMX7D_ECSPI2_ROOT_SRC] = imx_clk_mux("ecspi2_src", base + 0xb380, 24, 3, ecspi2_sel, ARRAY_SIZE(ecspi2_sel));
+	clks[IMX7D_ECSPI3_ROOT_SRC] = imx_clk_mux("ecspi3_src", base + 0xb400, 24, 3, ecspi3_sel, ARRAY_SIZE(ecspi3_sel));
+	clks[IMX7D_ECSPI4_ROOT_SRC] = imx_clk_mux("ecspi4_src", base + 0xb480, 24, 3, ecspi4_sel, ARRAY_SIZE(ecspi4_sel));
+	clks[IMX7D_PWM1_ROOT_SRC] = imx_clk_mux("pwm1_src", base + 0xb500, 24, 3, pwm1_sel, ARRAY_SIZE(pwm1_sel));
+	clks[IMX7D_PWM2_ROOT_SRC] = imx_clk_mux("pwm2_src", base + 0xb580, 24, 3, pwm2_sel, ARRAY_SIZE(pwm2_sel));
+	clks[IMX7D_PWM3_ROOT_SRC] = imx_clk_mux("pwm3_src", base + 0xb600, 24, 3, pwm3_sel, ARRAY_SIZE(pwm3_sel));
+	clks[IMX7D_PWM4_ROOT_SRC] = imx_clk_mux("pwm4_src", base + 0xb680, 24, 3, pwm4_sel, ARRAY_SIZE(pwm4_sel));
+	clks[IMX7D_FLEXTIMER1_ROOT_SRC] = imx_clk_mux("flextimer1_src", base + 0xb700, 24, 3, flextimer1_sel, ARRAY_SIZE(flextimer1_sel));
+	clks[IMX7D_FLEXTIMER2_ROOT_SRC] = imx_clk_mux("flextimer2_src", base + 0xb780, 24, 3, flextimer2_sel, ARRAY_SIZE(flextimer2_sel));
+	clks[IMX7D_SIM1_ROOT_SRC] = imx_clk_mux("sim1_src", base + 0xb800, 24, 3, sim1_sel, ARRAY_SIZE(sim1_sel));
+	clks[IMX7D_SIM2_ROOT_SRC] = imx_clk_mux("sim2_src", base + 0xb880, 24, 3, sim2_sel, ARRAY_SIZE(sim2_sel));
+	clks[IMX7D_GPT1_ROOT_SRC] = imx_clk_mux("gpt1_src", base + 0xb900, 24, 3, gpt1_sel, ARRAY_SIZE(gpt1_sel));
+	clks[IMX7D_GPT2_ROOT_SRC] = imx_clk_mux("gpt2_src", base + 0xb980, 24, 3, gpt2_sel, ARRAY_SIZE(gpt2_sel));
+	clks[IMX7D_GPT3_ROOT_SRC] = imx_clk_mux("gpt3_src", base + 0xba00, 24, 3, gpt3_sel, ARRAY_SIZE(gpt3_sel));
+	clks[IMX7D_GPT4_ROOT_SRC] = imx_clk_mux("gpt4_src", base + 0xba80, 24, 3, gpt4_sel, ARRAY_SIZE(gpt4_sel));
+	clks[IMX7D_TRACE_ROOT_SRC] = imx_clk_mux("trace_src", base + 0xbb00, 24, 3, trace_sel, ARRAY_SIZE(trace_sel));
+	clks[IMX7D_WDOG_ROOT_SRC] = imx_clk_mux("wdog_src", base + 0xbb80, 24, 3, wdog_sel, ARRAY_SIZE(wdog_sel));
+	clks[IMX7D_CSI_MCLK_ROOT_SRC] = imx_clk_mux("csi_mclk_src", base + 0xbc00, 24, 3, csi_mclk_sel, ARRAY_SIZE(csi_mclk_sel));
+	clks[IMX7D_AUDIO_MCLK_ROOT_SRC] = imx_clk_mux("audio_mclk_src", base + 0xbc80, 24, 3, audio_mclk_sel, ARRAY_SIZE(audio_mclk_sel));
+	clks[IMX7D_WRCLK_ROOT_SRC] = imx_clk_mux("wrclk_src", base + 0xbd00, 24, 3, wrclk_sel, ARRAY_SIZE(wrclk_sel));
+	clks[IMX7D_CLKO1_ROOT_SRC] = imx_clk_mux("clko1_src", base + 0xbd80, 24, 3, clko1_sel, ARRAY_SIZE(clko1_sel));
+	clks[IMX7D_CLKO2_ROOT_SRC] = imx_clk_mux("clko2_src", base + 0xbe00, 24, 3, clko2_sel, ARRAY_SIZE(clko2_sel));
+
+	clks[IMX7D_ARM_A7_ROOT_CG] = imx_clk_gate("arm_a7_cg", "arm_a7_src", base + 0x8000, 28);
+	clks[IMX7D_ARM_M4_ROOT_CG] = imx_clk_gate("arm_m4_cg", "arm_m4_src", base + 0x8080, 28);
+	clks[IMX7D_ARM_M0_ROOT_CG] = imx_clk_gate("arm_m0_cg", "arm_m0_src", base + 0x8100, 28);
+	clks[IMX7D_MAIN_AXI_ROOT_CG] = imx_clk_gate("axi_cg", "axi_src", base + 0x8800, 28);
+	clks[IMX7D_DISP_AXI_ROOT_CG] = imx_clk_gate("disp_axi_cg", "disp_axi_src", base + 0x8880, 28);
+	clks[IMX7D_ENET_AXI_ROOT_CG] = imx_clk_gate("enet_axi_cg", "enet_axi_src", base + 0x8900, 28);
+	clks[IMX7D_NAND_USDHC_BUS_ROOT_CG] = imx_clk_gate("nand_usdhc_cg", "nand_usdhc_src", base + 0x8980, 28);
+	clks[IMX7D_AHB_CHANNEL_ROOT_CG] = imx_clk_gate("ahb_cg", "ahb_src", base + 0x9000, 28);
+	clks[IMX7D_DRAM_PHYM_ROOT_CG] = imx_clk_gate("dram_phym_cg", "dram_phym_src", base + 0x9800, 28);
+	clks[IMX7D_DRAM_ROOT_CG] = imx_clk_gate("dram_cg", "dram_src", base + 0x9880, 28);
+	clks[IMX7D_DRAM_PHYM_ALT_ROOT_CG] = imx_clk_gate("dram_phym_alt_cg", "dram_phym_alt_src", base + 0xa000, 28);
+	clks[IMX7D_DRAM_ALT_ROOT_CG] = imx_clk_gate("dram_alt_cg", "dram_alt_src", base + 0xa080, 28);
+	clks[IMX7D_USB_HSIC_ROOT_CG] = imx_clk_gate("usb_hsic_cg", "usb_hsic_src", base + 0xa100, 28);
+	clks[IMX7D_PCIE_CTRL_ROOT_CG] = imx_clk_gate("pcie_ctrl_cg", "pcie_ctrl_src", base + 0xa180, 28);
+	clks[IMX7D_PCIE_PHY_ROOT_CG] = imx_clk_gate("pcie_phy_cg", "pcie_phy_src", base + 0xa200, 28);
+	clks[IMX7D_EPDC_PIXEL_ROOT_CG] = imx_clk_gate("epdc_pixel_cg", "epdc_pixel_src", base + 0xa280, 28);
+	clks[IMX7D_LCDIF_PIXEL_ROOT_CG] = imx_clk_gate("lcdif_pixel_cg", "lcdif_pixel_src", base + 0xa300, 28);
+	clks[IMX7D_MIPI_DSI_ROOT_CG] = imx_clk_gate("mipi_dsi_cg", "mipi_dsi_src", base + 0xa380, 28);
+	clks[IMX7D_MIPI_CSI_ROOT_CG] = imx_clk_gate("mipi_csi_cg", "mipi_csi_src", base + 0xa400, 28);
+	clks[IMX7D_MIPI_DPHY_ROOT_CG] = imx_clk_gate("mipi_dphy_cg", "mipi_dphy_src", base + 0xa480, 28);
+	clks[IMX7D_SAI1_ROOT_CG] = imx_clk_gate("sai1_cg", "sai1_src", base + 0xa500, 28);
+	clks[IMX7D_SAI2_ROOT_CG] = imx_clk_gate("sai2_cg", "sai2_src", base + 0xa580, 28);
+	clks[IMX7D_SAI3_ROOT_CG] = imx_clk_gate("sai3_cg", "sai3_src", base + 0xa600, 28);
+	clks[IMX7D_SPDIF_ROOT_CG] = imx_clk_gate("spdif_cg", "spdif_src", base + 0xa680, 28);
+	clks[IMX7D_ENET1_REF_ROOT_CG] = imx_clk_gate("enet1_ref_cg", "enet1_ref_src", base + 0xa700, 28);
+	clks[IMX7D_ENET1_TIME_ROOT_CG] = imx_clk_gate("enet1_time_cg", "enet1_time_src", base + 0xa780, 28);
+	clks[IMX7D_ENET2_REF_ROOT_CG] = imx_clk_gate("enet2_ref_cg", "enet2_ref_src", base + 0xa800, 28);
+	clks[IMX7D_ENET2_TIME_ROOT_CG] = imx_clk_gate("enet2_time_cg", "enet2_time_src", base + 0xa880, 28);
+	clks[IMX7D_ENET_PHY_REF_ROOT_CG] = imx_clk_gate("enet_phy_ref_cg", "enet_phy_ref_src", base + 0xa900, 28);
+	clks[IMX7D_EIM_ROOT_CG] = imx_clk_gate("eim_cg", "eim_src", base + 0xa980, 28);
+	clks[IMX7D_NAND_ROOT_CG] = imx_clk_gate("nand_cg", "nand_src", base + 0xaa00, 28);
+	clks[IMX7D_QSPI_ROOT_CG] = imx_clk_gate("qspi_cg", "qspi_src", base + 0xaa80, 28);
+	clks[IMX7D_USDHC1_ROOT_CG] = imx_clk_gate("usdhc1_cg", "usdhc1_src", base + 0xab00, 28);
+	clks[IMX7D_USDHC2_ROOT_CG] = imx_clk_gate("usdhc2_cg", "usdhc2_src", base + 0xab80, 28);
+	clks[IMX7D_USDHC3_ROOT_CG] = imx_clk_gate("usdhc3_cg", "usdhc3_src", base + 0xac00, 28);
+	clks[IMX7D_CAN1_ROOT_CG] = imx_clk_gate("can1_cg", "can1_src", base + 0xac80, 28);
+	clks[IMX7D_CAN2_ROOT_CG] = imx_clk_gate("can2_cg", "can2_src", base + 0xad00, 28);
+	clks[IMX7D_I2C1_ROOT_CG] = imx_clk_gate("i2c1_cg", "i2c1_src", base + 0xad80, 28);
+	clks[IMX7D_I2C2_ROOT_CG] = imx_clk_gate("i2c2_cg", "i2c2_src", base + 0xae00, 28);
+	clks[IMX7D_I2C3_ROOT_CG] = imx_clk_gate("i2c3_cg", "i2c3_src", base + 0xae80, 28);
+	clks[IMX7D_I2C4_ROOT_CG] = imx_clk_gate("i2c4_cg", "i2c4_src", base + 0xaf00, 28);
+	clks[IMX7D_UART1_ROOT_CG] = imx_clk_gate("uart1_cg", "uart1_src", base + 0xaf80, 28);
+	clks[IMX7D_UART2_ROOT_CG] = imx_clk_gate("uart2_cg", "uart2_src", base + 0xb000, 28);
+	clks[IMX7D_UART3_ROOT_CG] = imx_clk_gate("uart3_cg", "uart3_src", base + 0xb080, 28);
+	clks[IMX7D_UART4_ROOT_CG] = imx_clk_gate("uart4_cg", "uart4_src", base + 0xb100, 28);
+	clks[IMX7D_UART5_ROOT_CG] = imx_clk_gate("uart5_cg", "uart5_src", base + 0xb180, 28);
+	clks[IMX7D_UART6_ROOT_CG] = imx_clk_gate("uart6_cg", "uart6_src", base + 0xb200, 28);
+	clks[IMX7D_UART7_ROOT_CG] = imx_clk_gate("uart7_cg", "uart7_src", base + 0xb280, 28);
+	clks[IMX7D_ECSPI1_ROOT_CG] = imx_clk_gate("ecspi1_cg", "ecspi1_src", base + 0xb300, 28);
+	clks[IMX7D_ECSPI2_ROOT_CG] = imx_clk_gate("ecspi2_cg", "ecspi2_src", base + 0xb380, 28);
+	clks[IMX7D_ECSPI3_ROOT_CG] = imx_clk_gate("ecspi3_cg", "ecspi3_src", base + 0xb400, 28);
+	clks[IMX7D_ECSPI4_ROOT_CG] = imx_clk_gate("ecspi4_cg", "ecspi4_src", base + 0xb480, 28);
+	clks[IMX7D_PWM1_ROOT_CG] = imx_clk_gate("pwm1_cg", "pwm1_src", base + 0xb500, 28);
+	clks[IMX7D_PWM2_ROOT_CG] = imx_clk_gate("pwm2_cg", "pwm2_src", base + 0xb580, 28);
+	clks[IMX7D_PWM3_ROOT_CG] = imx_clk_gate("pwm3_cg", "pwm3_src", base + 0xb600, 28);
+	clks[IMX7D_PWM4_ROOT_CG] = imx_clk_gate("pwm4_cg", "pwm4_src", base + 0xb680, 28);
+	clks[IMX7D_FLEXTIMER1_ROOT_CG] = imx_clk_gate("flextimer1_cg", "flextimer1_src", base + 0xb700, 28);
+	clks[IMX7D_FLEXTIMER2_ROOT_CG] = imx_clk_gate("flextimer2_cg", "flextimer2_src", base + 0xb780, 28);
+	clks[IMX7D_SIM1_ROOT_CG] = imx_clk_gate("sim1_cg", "sim1_src", base + 0xb800, 28);
+	clks[IMX7D_SIM2_ROOT_CG] = imx_clk_gate("sim2_cg", "sim2_src", base + 0xb880, 28);
+	clks[IMX7D_GPT1_ROOT_CG] = imx_clk_gate("gpt1_cg", "gpt1_src", base + 0xb900, 28);
+	clks[IMX7D_GPT2_ROOT_CG] = imx_clk_gate("gpt2_cg", "gpt2_src", base + 0xb980, 28);
+	clks[IMX7D_GPT3_ROOT_CG] = imx_clk_gate("gpt3_cg", "gpt3_src", base + 0xbA00, 28);
+	clks[IMX7D_GPT4_ROOT_CG] = imx_clk_gate("gpt4_cg", "gpt4_src", base + 0xbA80, 28);
+	clks[IMX7D_TRACE_ROOT_CG] = imx_clk_gate("trace_cg", "trace_src", base + 0xbb00, 28);
+	clks[IMX7D_WDOG_ROOT_CG] = imx_clk_gate("wdog_cg", "wdog_src", base + 0xbb80, 28);
+	clks[IMX7D_CSI_MCLK_ROOT_CG] = imx_clk_gate("csi_mclk_cg", "csi_mclk_src", base + 0xbc00, 28);
+	clks[IMX7D_AUDIO_MCLK_ROOT_CG] = imx_clk_gate("audio_mclk_cg", "audio_mclk_src", base + 0xbc80, 28);
+	clks[IMX7D_WRCLK_ROOT_CG] = imx_clk_gate("wrclk_cg", "wrclk_src", base + 0xbd00, 28);
+	clks[IMX7D_CLKO1_ROOT_CG] = imx_clk_gate("clko1_cg", "clko1_src", base + 0xbd80, 28);
+	clks[IMX7D_CLKO2_ROOT_CG] = imx_clk_gate("clko2_cg", "clko2_src", base + 0xbe00, 28);
+
+	clks[IMX7D_MAIN_AXI_ROOT_PRE_DIV] = imx_clk_divider("axi_pre_div", "axi_cg", base + 0x8800, 16, 3);
+	clks[IMX7D_DISP_AXI_ROOT_PRE_DIV] = imx_clk_divider("disp_axi_pre_div", "disp_axi_cg", base + 0x8880, 16, 3);
+	clks[IMX7D_ENET_AXI_ROOT_PRE_DIV] = imx_clk_divider("enet_axi_pre_div", "enet_axi_cg", base + 0x8900, 16, 3);
+	clks[IMX7D_NAND_USDHC_BUS_ROOT_PRE_DIV] = imx_clk_divider("nand_usdhc_pre_div", "nand_usdhc_cg", base + 0x8980, 16, 3);
+	clks[IMX7D_AHB_CHANNEL_ROOT_PRE_DIV] = imx_clk_divider("ahb_pre_div", "ahb_cg", base + 0x9000, 16, 3);
+	clks[IMX7D_DRAM_PHYM_ALT_ROOT_PRE_DIV] = imx_clk_divider("dram_phym_alt_pre_div", "dram_phym_alt_cg", base + 0xa000, 16, 3);
+	clks[IMX7D_DRAM_ALT_ROOT_PRE_DIV] = imx_clk_divider("dram_alt_pre_div", "dram_alt_cg", base + 0xa080, 16, 3);
+	clks[IMX7D_USB_HSIC_ROOT_PRE_DIV] = imx_clk_divider("usb_hsic_pre_div", "usb_hsic_cg", base + 0xa100, 16, 3);
+	clks[IMX7D_PCIE_CTRL_ROOT_PRE_DIV] = imx_clk_divider("pcie_ctrl_pre_div", "pcie_ctrl_cg", base + 0xa180, 16, 3);
+	clks[IMX7D_PCIE_PHY_ROOT_PRE_DIV] = imx_clk_divider("pcie_phy_pre_div", "pcie_phy_cg", base + 0xa200, 16, 3);
+	clks[IMX7D_EPDC_PIXEL_ROOT_PRE_DIV] = imx_clk_divider("epdc_pixel_pre_div", "epdc_pixel_cg", base + 0xa280, 16, 3);
+	clks[IMX7D_LCDIF_PIXEL_ROOT_PRE_DIV] = imx_clk_divider("lcdif_pixel_pre_div", "lcdif_pixel_cg", base + 0xa300, 16, 3);
+	clks[IMX7D_MIPI_DSI_ROOT_PRE_DIV] = imx_clk_divider("mipi_dsi_pre_div", "mipi_dsi_cg", base + 0xa380, 16, 3);
+	clks[IMX7D_MIPI_CSI_ROOT_PRE_DIV] = imx_clk_divider("mipi_csi_pre_div", "mipi_csi_cg", base + 0xa400, 16, 3);
+	clks[IMX7D_MIPI_DPHY_ROOT_PRE_DIV] = imx_clk_divider("mipi_dphy_pre_div", "mipi_dphy_cg", base + 0xa480, 16, 3);
+	clks[IMX7D_SAI1_ROOT_PRE_DIV] = imx_clk_divider("sai1_pre_div", "sai1_cg", base + 0xa500, 16, 3);
+	clks[IMX7D_SAI2_ROOT_PRE_DIV] = imx_clk_divider("sai2_pre_div", "sai2_cg", base + 0xa580, 16, 3);
+	clks[IMX7D_SAI3_ROOT_PRE_DIV] = imx_clk_divider("sai3_pre_div", "sai3_cg", base + 0xa600, 16, 3);
+	clks[IMX7D_SPDIF_ROOT_PRE_DIV] = imx_clk_divider("spdif_pre_div", "spdif_cg", base + 0xa680, 16, 3);
+	clks[IMX7D_ENET1_REF_ROOT_PRE_DIV] = imx_clk_divider("enet1_ref_pre_div", "enet1_ref_cg", base + 0xa700, 16, 3);
+	clks[IMX7D_ENET1_TIME_ROOT_PRE_DIV] = imx_clk_divider("enet1_time_pre_div", "enet1_time_cg", base + 0xa780, 16, 3);
+	clks[IMX7D_ENET2_REF_ROOT_PRE_DIV] = imx_clk_divider("enet2_ref_pre_div", "enet2_ref_cg", base + 0xa800, 16, 3);
+	clks[IMX7D_ENET2_TIME_ROOT_PRE_DIV] = imx_clk_divider("enet2_time_pre_div", "enet2_time_cg", base + 0xa880, 16, 3);
+	clks[IMX7D_ENET_PHY_REF_ROOT_PRE_DIV] = imx_clk_divider("enet_phy_ref_pre_div", "enet_phy_ref_cg", base + 0xa900, 16, 3);
+	clks[IMX7D_EIM_ROOT_PRE_DIV] = imx_clk_divider("eim_pre_div", "eim_cg", base + 0xa980, 16, 3);
+	clks[IMX7D_NAND_ROOT_PRE_DIV] = imx_clk_divider("nand_pre_div", "nand_cg", base + 0xaa00, 16, 3);
+	clks[IMX7D_QSPI_ROOT_PRE_DIV] = imx_clk_divider("qspi_pre_div", "qspi_cg", base + 0xaa80, 16, 3);
+	clks[IMX7D_USDHC1_ROOT_PRE_DIV] = imx_clk_divider("usdhc1_pre_div", "usdhc1_cg", base + 0xab00, 16, 3);
+	clks[IMX7D_USDHC2_ROOT_PRE_DIV] = imx_clk_divider("usdhc2_pre_div", "usdhc2_cg", base + 0xab80, 16, 3);
+	clks[IMX7D_USDHC3_ROOT_PRE_DIV] = imx_clk_divider("usdhc3_pre_div", "usdhc3_cg", base + 0xac00, 16, 3);
+	clks[IMX7D_CAN1_ROOT_PRE_DIV] = imx_clk_divider("can1_pre_div", "can1_cg", base + 0xac80, 16, 3);
+	clks[IMX7D_CAN2_ROOT_PRE_DIV] = imx_clk_divider("can2_pre_div", "can2_cg", base + 0xad00, 16, 3);
+	clks[IMX7D_I2C1_ROOT_PRE_DIV] = imx_clk_divider("i2c1_pre_div", "i2c1_cg", base + 0xad80, 16, 3);
+	clks[IMX7D_I2C2_ROOT_PRE_DIV] = imx_clk_divider("i2c2_pre_div", "i2c2_cg", base + 0xae00, 16, 3);
+	clks[IMX7D_I2C3_ROOT_PRE_DIV] = imx_clk_divider("i2c3_pre_div", "i2c3_cg", base + 0xae80, 16, 3);
+	clks[IMX7D_I2C4_ROOT_PRE_DIV] = imx_clk_divider("i2c4_pre_div", "i2c4_cg", base + 0xaf00, 16, 3);
+	clks[IMX7D_UART1_ROOT_PRE_DIV] = imx_clk_divider("uart1_pre_div", "uart1_cg", base + 0xaf80, 16, 3);
+	clks[IMX7D_UART2_ROOT_PRE_DIV] = imx_clk_divider("uart2_pre_div", "uart2_cg", base + 0xb000, 16, 3);
+	clks[IMX7D_UART3_ROOT_PRE_DIV] = imx_clk_divider("uart3_pre_div", "uart3_cg", base + 0xb080, 16, 3);
+	clks[IMX7D_UART4_ROOT_PRE_DIV] = imx_clk_divider("uart4_pre_div", "uart4_cg", base + 0xb100, 16, 3);
+	clks[IMX7D_UART5_ROOT_PRE_DIV] = imx_clk_divider("uart5_pre_div", "uart5_cg", base + 0xb180, 16, 3);
+	clks[IMX7D_UART6_ROOT_PRE_DIV] = imx_clk_divider("uart6_pre_div", "uart6_cg", base + 0xb200, 16, 3);
+	clks[IMX7D_UART7_ROOT_PRE_DIV] = imx_clk_divider("uart7_pre_div", "uart7_cg", base + 0xb280, 16, 3);
+	clks[IMX7D_ECSPI1_ROOT_PRE_DIV] = imx_clk_divider("ecspi1_pre_div", "ecspi1_cg", base + 0xb300, 16, 3);
+	clks[IMX7D_ECSPI2_ROOT_PRE_DIV] = imx_clk_divider("ecspi2_pre_div", "ecspi2_cg", base + 0xb380, 16, 3);
+	clks[IMX7D_ECSPI3_ROOT_PRE_DIV] = imx_clk_divider("ecspi3_pre_div", "ecspi3_cg", base + 0xb400, 16, 3);
+	clks[IMX7D_ECSPI4_ROOT_PRE_DIV] = imx_clk_divider("ecspi4_pre_div", "ecspi4_cg", base + 0xb480, 16, 3);
+	clks[IMX7D_PWM1_ROOT_PRE_DIV] = imx_clk_divider("pwm1_pre_div", "pwm1_cg", base + 0xb500, 16, 3);
+	clks[IMX7D_PWM2_ROOT_PRE_DIV] = imx_clk_divider("pwm2_pre_div", "pwm2_cg", base + 0xb580, 16, 3);
+	clks[IMX7D_PWM3_ROOT_PRE_DIV] = imx_clk_divider("pwm3_pre_div", "pwm3_cg", base + 0xb600, 16, 3);
+	clks[IMX7D_PWM4_ROOT_PRE_DIV] = imx_clk_divider("pwm4_pre_div", "pwm4_cg", base + 0xb680, 16, 3);
+	clks[IMX7D_FLEXTIMER1_ROOT_PRE_DIV] = imx_clk_divider("flextimer1_pre_div", "flextimer1_cg", base + 0xb700, 16, 3);
+	clks[IMX7D_FLEXTIMER2_ROOT_PRE_DIV] = imx_clk_divider("flextimer2_pre_div", "flextimer2_cg", base + 0xb780, 16, 3);
+	clks[IMX7D_SIM1_ROOT_PRE_DIV] = imx_clk_divider("sim1_pre_div", "sim1_cg", base + 0xb800, 16, 3);
+	clks[IMX7D_SIM2_ROOT_PRE_DIV] = imx_clk_divider("sim2_pre_div", "sim2_cg", base + 0xb880, 16, 3);
+	clks[IMX7D_GPT1_ROOT_PRE_DIV] = imx_clk_divider("gpt1_pre_div", "gpt1_cg", base + 0xb900, 16, 3);
+	clks[IMX7D_GPT2_ROOT_PRE_DIV] = imx_clk_divider("gpt2_pre_div", "gpt2_cg", base + 0xb980, 16, 3);
+	clks[IMX7D_GPT3_ROOT_PRE_DIV] = imx_clk_divider("gpt3_pre_div", "gpt3_cg", base + 0xba00, 16, 3);
+	clks[IMX7D_GPT4_ROOT_PRE_DIV] = imx_clk_divider("gpt4_pre_div", "gpt4_cg", base + 0xba80, 16, 3);
+	clks[IMX7D_TRACE_ROOT_PRE_DIV] = imx_clk_divider("trace_pre_div", "trace_cg", base + 0xbb00, 16, 3);
+	clks[IMX7D_WDOG_ROOT_PRE_DIV] = imx_clk_divider("wdog_pre_div", "wdog_cg", base + 0xbb80, 16, 3);
+	clks[IMX7D_CSI_MCLK_ROOT_PRE_DIV] = imx_clk_divider("csi_mclk_pre_div", "csi_mclk_cg", base + 0xbc00, 16, 3);
+	clks[IMX7D_AUDIO_MCLK_ROOT_PRE_DIV] = imx_clk_divider("audio_mclk_pre_div", "audio_mclk_cg", base + 0xbc80, 16, 3);
+	clks[IMX7D_WRCLK_ROOT_PRE_DIV] = imx_clk_divider("wrclk_pre_div", "wrclk_cg", base + 0xbd00, 16, 3);
+	clks[IMX7D_CLKO1_ROOT_PRE_DIV] = imx_clk_divider("clko1_pre_div", "clko1_cg", base + 0xbd80, 16, 3);
+	clks[IMX7D_CLKO2_ROOT_PRE_DIV] = imx_clk_divider("clko2_pre_div", "clko2_cg", base + 0xbe00, 16, 3);
+
+	clks[IMX7D_ARM_A7_ROOT_DIV] = imx_clk_divider("arm_a7_div", "arm_a7_cg", base + 0x8000, 0, 3);
+	clks[IMX7D_ARM_M4_ROOT_DIV] = imx_clk_divider("arm_m4_div", "arm_m4_cg", base + 0x8080, 0, 3);
+	clks[IMX7D_ARM_M0_ROOT_DIV] = imx_clk_divider("arm_m0_div", "arm_m0_cg", base + 0x8100, 0, 3);
+	clks[IMX7D_MAIN_AXI_ROOT_DIV] = imx_clk_divider("axi_post_div", "axi_pre_div", base + 0x8800, 0, 6);
+	clks[IMX7D_DISP_AXI_ROOT_DIV] = imx_clk_divider("disp_axi_post_div", "disp_axi_pre_div", base + 0x8880, 0, 6);
+	clks[IMX7D_ENET_AXI_ROOT_DIV] = imx_clk_divider("enet_axi_post_div", "enet_axi_pre_div", base + 0x8900, 0, 6);
+	clks[IMX7D_NAND_USDHC_BUS_ROOT_DIV] = imx_clk_divider("nand_usdhc_post_div", "nand_usdhc_pre_div", base + 0x8980, 0, 6);
+	clks[IMX7D_AHB_CHANNEL_ROOT_DIV] = imx_clk_divider("ahb_post_div", "ahb_pre_div", base + 0x9000, 0, 6);
+	clks[IMX7D_DRAM_ROOT_DIV] = imx_clk_divider("dram_post_div", "dram_cg", base + 0x9880, 0, 3);
+	clks[IMX7D_DRAM_PHYM_ALT_ROOT_DIV] = imx_clk_divider("dram_phym_alt_post_div", "dram_phym_alt_pre_div", base + 0xa000, 0, 3);
+	clks[IMX7D_DRAM_ALT_ROOT_DIV] = imx_clk_divider("dram_alt_post_div", "dram_alt_pre_div", base + 0xa080, 0, 3);
+	clks[IMX7D_USB_HSIC_ROOT_DIV] = imx_clk_divider("usb_hsic_post_div", "usb_hsic_pre_div", base + 0xa100, 0, 6);
+	clks[IMX7D_PCIE_CTRL_ROOT_DIV] = imx_clk_divider("pcie_ctrl_post_div", "pcie_ctrl_pre_div", base + 0xa180, 0, 6);
+	clks[IMX7D_PCIE_PHY_ROOT_DIV] = imx_clk_divider("pcie_phy_post_div", "pcie_phy_pre_div", base + 0xa200, 0, 6);
+	clks[IMX7D_EPDC_PIXEL_ROOT_DIV] = imx_clk_divider("epdc_pixel_post_div", "epdc_pixel_pre_div", base + 0xa280, 0, 6);
+	clks[IMX7D_LCDIF_PIXEL_ROOT_DIV] = imx_clk_divider("lcdif_pixel_post_div", "lcdif_pixel_pre_div", base + 0xa300, 0, 6);
+	clks[IMX7D_MIPI_DSI_ROOT_DIV] = imx_clk_divider("mipi_dsi_post_div", "mipi_dsi_pre_div", base + 0xa380, 0, 6);
+	clks[IMX7D_MIPI_CSI_ROOT_DIV] = imx_clk_divider("mipi_csi_post_div", "mipi_csi_pre_div", base + 0xa400, 0, 6);
+	clks[IMX7D_MIPI_DPHY_ROOT_DIV] = imx_clk_divider("mipi_dphy_post_div", "mipi_csi_dphy_div", base + 0xa480, 0, 6);
+	clks[IMX7D_SAI1_ROOT_DIV] = imx_clk_divider("sai1_post_div", "sai1_pre_div", base + 0xa500, 0, 6);
+	clks[IMX7D_SAI2_ROOT_DIV] = imx_clk_divider("sai2_post_div", "sai2_pre_div", base + 0xa580, 0, 6);
+	clks[IMX7D_SAI3_ROOT_DIV] = imx_clk_divider("sai3_post_div", "sai3_pre_div", base + 0xa600, 0, 6);
+	clks[IMX7D_SPDIF_ROOT_DIV] = imx_clk_divider("spdif_post_div", "spdif_pre_div", base + 0xa680, 0, 6);
+	clks[IMX7D_ENET1_REF_ROOT_DIV] = imx_clk_divider("enet1_ref_post_div", "enet1_ref_pre_div", base + 0xa700, 0, 6);
+	clks[IMX7D_ENET1_TIME_ROOT_DIV] = imx_clk_divider("enet1_time_post_div", "enet1_time_pre_div", base + 0xa780, 0, 6);
+	clks[IMX7D_ENET2_REF_ROOT_DIV] = imx_clk_divider("enet2_ref_post_div", "enet2_ref_pre_div", base + 0xa800, 0, 6);
+	clks[IMX7D_ENET2_TIME_ROOT_DIV] = imx_clk_divider("enet2_time_post_div", "enet2_time_pre_div", base + 0xa880, 0, 6);
+	clks[IMX7D_ENET_PHY_REF_ROOT_DIV] = imx_clk_divider("enet_phy_ref_post_div", "enet_phy_ref_pre_div", base + 0xa900, 0, 6);
+	clks[IMX7D_EIM_ROOT_DIV] = imx_clk_divider("eim_post_div", "eim_pre_div", base + 0xa980, 0, 6);
+	clks[IMX7D_NAND_ROOT_DIV] = imx_clk_divider("nand_post_div", "nand_pre_div", base + 0xaa00, 0, 6);
+	clks[IMX7D_QSPI_ROOT_DIV] = imx_clk_divider("qspi_post_div", "qspi_pre_div", base + 0xaa80, 0, 6);
+	clks[IMX7D_USDHC1_ROOT_DIV] = imx_clk_divider("usdhc1_post_div", "usdhc1_pre_div", base + 0xab00, 0, 6);
+	clks[IMX7D_USDHC2_ROOT_DIV] = imx_clk_divider("usdhc2_post_div", "usdhc2_pre_div", base + 0xab80, 0, 6);
+	clks[IMX7D_USDHC3_ROOT_DIV] = imx_clk_divider("usdhc3_post_div", "usdhc3_pre_div", base + 0xac00, 0, 6);
+	clks[IMX7D_CAN1_ROOT_DIV] = imx_clk_divider("can1_post_div", "can1_pre_div", base + 0xac80, 0, 6);
+	clks[IMX7D_CAN2_ROOT_DIV] = imx_clk_divider("can2_post_div", "can2_pre_div", base + 0xad00, 0, 6);
+	clks[IMX7D_I2C1_ROOT_DIV] = imx_clk_divider("i2c1_post_div", "i2c1_pre_div", base + 0xad80, 0, 6);
+	clks[IMX7D_I2C2_ROOT_DIV] = imx_clk_divider("i2c2_post_div", "i2c2_pre_div", base + 0xae00, 0, 6);
+	clks[IMX7D_I2C3_ROOT_DIV] = imx_clk_divider("i2c3_post_div", "i2c3_pre_div", base + 0xae80, 0, 6);
+	clks[IMX7D_I2C4_ROOT_DIV] = imx_clk_divider("i2c4_post_div", "i2c4_pre_div", base + 0xaf00, 0, 6);
+	clks[IMX7D_UART1_ROOT_DIV] = imx_clk_divider("uart1_post_div", "uart1_pre_div", base + 0xaf80, 0, 6);
+	clks[IMX7D_UART2_ROOT_DIV] = imx_clk_divider("uart2_post_div", "uart2_pre_div", base + 0xb000, 0, 6);
+	clks[IMX7D_UART3_ROOT_DIV] = imx_clk_divider("uart3_post_div", "uart3_pre_div", base + 0xb080, 0, 6);
+	clks[IMX7D_UART4_ROOT_DIV] = imx_clk_divider("uart4_post_div", "uart4_pre_div", base + 0xb100, 0, 6);
+	clks[IMX7D_UART5_ROOT_DIV] = imx_clk_divider("uart5_post_div", "uart5_pre_div", base + 0xb180, 0, 6);
+	clks[IMX7D_UART6_ROOT_DIV] = imx_clk_divider("uart6_post_div", "uart6_pre_div", base + 0xb200, 0, 6);
+	clks[IMX7D_UART7_ROOT_DIV] = imx_clk_divider("uart7_post_div", "uart7_pre_div", base + 0xb280, 0, 6);
+	clks[IMX7D_ECSPI1_ROOT_DIV] = imx_clk_divider("ecspi1_post_div", "ecspi1_pre_div", base + 0xb300, 0, 6);
+	clks[IMX7D_ECSPI2_ROOT_DIV] = imx_clk_divider("ecspi2_post_div", "ecspi2_pre_div", base + 0xb380, 0, 6);
+	clks[IMX7D_ECSPI3_ROOT_DIV] = imx_clk_divider("ecspi3_post_div", "ecspi3_pre_div", base + 0xb400, 0, 6);
+	clks[IMX7D_ECSPI4_ROOT_DIV] = imx_clk_divider("ecspi4_post_div", "ecspi4_pre_div", base + 0xb480, 0, 6);
+	clks[IMX7D_PWM1_ROOT_DIV] = imx_clk_divider("pwm1_post_div", "pwm1_pre_div", base + 0xb500, 0, 6);
+	clks[IMX7D_PWM2_ROOT_DIV] = imx_clk_divider("pwm2_post_div", "pwm2_pre_div", base + 0xb580, 0, 6);
+	clks[IMX7D_PWM3_ROOT_DIV] = imx_clk_divider("pwm3_post_div", "pwm3_pre_div", base + 0xb600, 0, 6);
+	clks[IMX7D_PWM4_ROOT_DIV] = imx_clk_divider("pwm4_post_div", "pwm4_pre_div", base + 0xb680, 0, 6);
+	clks[IMX7D_FLEXTIMER1_ROOT_DIV] = imx_clk_divider("flextimer1_post_div", "flextimer1_pre_div", base + 0xb700, 0, 6);
+	clks[IMX7D_FLEXTIMER2_ROOT_DIV] = imx_clk_divider("flextimer2_post_div", "flextimer2_pre_div", base + 0xb780, 0, 6);
+	clks[IMX7D_SIM1_ROOT_DIV] = imx_clk_divider("sim1_post_div", "sim1_pre_div", base + 0xb800, 0, 6);
+	clks[IMX7D_SIM2_ROOT_DIV] = imx_clk_divider("sim2_post_div", "sim2_pre_div", base + 0xb880, 0, 6);
+	clks[IMX7D_GPT1_ROOT_DIV] = imx_clk_divider("gpt1_post_div", "gpt1_pre_div", base + 0xb900, 0, 6);
+	clks[IMX7D_GPT2_ROOT_DIV] = imx_clk_divider("gpt2_post_div", "gpt2_pre_div", base + 0xb980, 0, 6);
+	clks[IMX7D_GPT3_ROOT_DIV] = imx_clk_divider("gpt3_post_div", "gpt3_pre_div", base + 0xba00, 0, 6);
+	clks[IMX7D_GPT4_ROOT_DIV] = imx_clk_divider("gpt4_post_div", "gpt4_pre_div", base + 0xba80, 0, 6);
+	clks[IMX7D_TRACE_ROOT_DIV] = imx_clk_divider("trace_post_div", "trace_pre_div", base + 0xbb00, 0, 6);
+	clks[IMX7D_WDOG_ROOT_DIV] = imx_clk_divider("wdog_post_div", "wdog_pre_div", base + 0xbb80, 0, 6);
+	clks[IMX7D_CSI_MCLK_ROOT_DIV] = imx_clk_divider("csi_mclk_post_div", "csi_mclk_pre_div", base + 0xbc00, 0, 6);
+	clks[IMX7D_AUDIO_MCLK_ROOT_DIV] = imx_clk_divider("audio_mclk_post_div", "audio_mclk_pre_div", base + 0xbc80, 0, 6);
+	clks[IMX7D_WRCLK_ROOT_DIV] = imx_clk_divider("wrclk_post_div", "wrclk_pre_div", base + 0xbd00, 0, 6);
+	clks[IMX7D_CLKO1_ROOT_DIV] = imx_clk_divider("clko1_post_div", "clko1_pre_div", base + 0xbd80, 0, 6);
+	clks[IMX7D_CLKO2_ROOT_DIV] = imx_clk_divider("clko2_post_div", "clko2_pre_div", base + 0xbe00, 0, 6);
+
+	clks[IMX7D_ARM_A7_ROOT_CLK] = imx_clk_gate2("arm_a7_root_clk", "arm_a7_div", base + 0x4000, 0);
+	clks[IMX7D_ARM_M4_ROOT_CLK] = imx_clk_gate2("arm_m4_root_clk", "arm_m4_div", base + 0x4010, 0);
+	clks[IMX7D_ARM_M0_ROOT_CLK] = imx_clk_gate2("arm_m0_root_clk", "arm_m0_div", base + 0x4020, 0);
+	clks[IMX7D_MAIN_AXI_ROOT_CLK] = imx_clk_gate2("main_axi_root_clk", "axi_post_div", base + 0x4040, 0);
+	clks[IMX7D_DISP_AXI_ROOT_CLK] = imx_clk_gate2("disp_axi_root_clk", "disp_axi_post_div", base + 0x4050, 0);
+	clks[IMX7D_ENET_AXI_ROOT_CLK] = imx_clk_gate2("enet_axi_root_clk", "enet_axi_post_div", base + 0x4060, 0);
+	clks[IMX7D_OCRAM_CLK] = imx_clk_gate2("ocram_clk", "axi_post_div", base + 0x4110, 0);
+	clks[IMX7D_OCRAM_S_CLK] = imx_clk_gate2("ocram_s_clk", "ahb_post_div", base + 0x4120, 0);
+	clks[IMX7D_NAND_USDHC_BUS_ROOT_CLK] = imx_clk_gate2("nand_usdhc_root_clk", "nand_usdhc_post_div", base + 0x4130, 0);
+	clks[IMX7D_AHB_CHANNEL_ROOT_CLK] = imx_clk_gate2("ahb_root_clk", "ahb_post_div", base + 0x4200, 0);
+	clks[IMX7D_DRAM_ROOT_CLK] = imx_clk_gate2("dram_root_clk", "dram_post_div", base + 0x4130, 0);
+	clks[IMX7D_DRAM_PHYM_ROOT_CLK] = imx_clk_gate2("dram_phym_root_clk", "dram_phym_cg", base + 0x4130, 0);
+	clks[IMX7D_DRAM_PHYM_ALT_ROOT_CLK] = imx_clk_gate2("dram_phym_alt_root_clk", "dram_phym_alt_post_div", base + 0x4130, 0);
+	clks[IMX7D_DRAM_ALT_ROOT_CLK] = imx_clk_gate2("dram_alt_root_clk", "dram_alt_post_div", base + 0x4130, 0);
+	clks[IMX7D_USB_HSIC_ROOT_CLK] = imx_clk_gate2("usb_hsic_root_clk", "usb_hsic_post_div", base + 0x4420, 0);
+	clks[IMX7D_PCIE_CTRL_ROOT_CLK] = imx_clk_gate2("pcie_ctrl_root_clk", "pcie_ctrl_post_div", base + 0x4600, 0);
+	clks[IMX7D_PCIE_PHY_ROOT_CLK] = imx_clk_gate2("pcie_phy_root_clk", "pcie_phy_post_div", base + 0x4600, 0);
+	clks[IMX7D_EPDC_PIXEL_ROOT_CLK] = imx_clk_gate2("epdc_pixel_root_clk", "epdc_pixel_post_div", base + 0x44a0, 0);
+	clks[IMX7D_LCDIF_PIXEL_ROOT_CLK] = imx_clk_gate2("lcdif_pixel_root_clk", "lcdif_pixel_post_div", base + 0x44b0, 0);
+	clks[IMX7D_MIPI_DSI_ROOT_CLK] = imx_clk_gate2("mipi_dsi_root_clk", "mipi_dsi_post_div", base + 0x4650, 0);
+	clks[IMX7D_MIPI_CSI_ROOT_CLK] = imx_clk_gate2("mipi_csi_root_clk", "mipi_csi_post_div", base + 0x4640, 0);
+	clks[IMX7D_MIPI_DPHY_ROOT_CLK] = imx_clk_gate2("mipi_dphy_root_clk", "mipi_dphy_post_div", base + 0x4660, 0);
+	clks[IMX7D_SAI1_ROOT_CLK] = imx_clk_gate2("sai1_root_clk", "sai1_post_div", base + 0x48c0, 0);
+	clks[IMX7D_SAI2_ROOT_CLK] = imx_clk_gate2("sai2_root_clk", "sai2_post_div", base + 0x48d0, 0);
+	clks[IMX7D_SAI3_ROOT_CLK] = imx_clk_gate2("sai3_root_clk", "sai3_post_div", base + 0x48e0, 0);
+	clks[IMX7D_SPDIF_ROOT_CLK] = imx_clk_gate2("spdif_root_clk", "spdif_post_div", base + 0x44d0, 0);
+	clks[IMX7D_ENET1_REF_ROOT_CLK] = imx_clk_gate2("enet1_ref_root_clk", "enet1_ref_post_div", base + 0x44e0, 0);
+	clks[IMX7D_ENET1_TIME_ROOT_CLK] = imx_clk_gate2("enet1_time_root_clk", "enet1_time_post_div", base + 0x44f0, 0);
+	clks[IMX7D_ENET2_REF_ROOT_CLK] = imx_clk_gate2("enet2_ref_root_clk", "enet2_ref_post_div", base + 0x4500, 0);
+	clks[IMX7D_ENET2_TIME_ROOT_CLK] = imx_clk_gate2("enet2_time_root_clk", "enet2_time_post_div", base + 0x4510, 0);
+	clks[IMX7D_ENET_PHY_REF_ROOT_CLK] = imx_clk_gate2("enet_phy_ref_root_clk", "enet_phy_ref_post_div", base + 0x4520, 0);
+	clks[IMX7D_EIM_ROOT_CLK] = imx_clk_gate2("eim_root_clk", "eim_post_div", base + 0x4160, 0);
+	clks[IMX7D_NAND_ROOT_CLK] = imx_clk_gate2("nand_root_clk", "nand_post_div", base + 0x4140, 0);
+	clks[IMX7D_QSPI_ROOT_CLK] = imx_clk_gate2("qspi_root_clk", "qspi_post_div", base + 0x4150, 0);
+	clks[IMX7D_USDHC1_ROOT_CLK] = imx_clk_gate2("usdhc1_root_clk", "usdhc1_post_div", base + 0x46c0, 0);
+	clks[IMX7D_USDHC2_ROOT_CLK] = imx_clk_gate2("usdhc2_root_clk", "usdhc2_post_div", base + 0x46d0, 0);
+	clks[IMX7D_USDHC3_ROOT_CLK] = imx_clk_gate2("usdhc3_root_clk", "usdhc3_post_div", base + 0x46e0, 0);
+	clks[IMX7D_CAN1_ROOT_CLK] = imx_clk_gate2("can1_root_clk", "can1_post_div", base + 0x4740, 0);
+	clks[IMX7D_CAN2_ROOT_CLK] = imx_clk_gate2("can2_root_clk", "can2_post_div", base + 0x4750, 0);
+	clks[IMX7D_I2C1_ROOT_CLK] = imx_clk_gate2("i2c1_root_clk", "i2c1_post_div", base + 0x4880, 0);
+	clks[IMX7D_I2C2_ROOT_CLK] = imx_clk_gate2("i2c2_root_clk", "i2c2_post_div", base + 0x4890, 0);
+	clks[IMX7D_I2C3_ROOT_CLK] = imx_clk_gate2("i2c3_root_clk", "i2c3_post_div", base + 0x48a0, 0);
+	clks[IMX7D_I2C4_ROOT_CLK] = imx_clk_gate2("i2c4_root_clk", "i2c4_post_div", base + 0x48b0, 0);
+	clks[IMX7D_UART1_ROOT_CLK] = imx_clk_gate2("uart1_root_clk", "uart1_post_div", base + 0x4940, 0);
+	clks[IMX7D_UART2_ROOT_CLK] = imx_clk_gate2("uart2_root_clk", "uart2_post_div", base + 0x4950, 0);
+	clks[IMX7D_UART3_ROOT_CLK] = imx_clk_gate2("uart3_root_clk", "uart3_post_div", base + 0x4960, 0);
+	clks[IMX7D_UART4_ROOT_CLK] = imx_clk_gate2("uart4_root_clk", "uart4_post_div", base + 0x4970, 0);
+	clks[IMX7D_UART5_ROOT_CLK] = imx_clk_gate2("uart5_root_clk", "uart5_post_div", base + 0x4980, 0);
+	clks[IMX7D_UART6_ROOT_CLK] = imx_clk_gate2("uart6_root_clk", "uart6_post_div", base + 0x4990, 0);
+	clks[IMX7D_UART7_ROOT_CLK] = imx_clk_gate2("uart7_root_clk", "uart7_post_div", base + 0x49a0, 0);
+	clks[IMX7D_ECSPI1_ROOT_CLK] = imx_clk_gate2("ecspi1_root_clk", "ecspi1_post_div", base + 0x4780, 0);
+	clks[IMX7D_ECSPI2_ROOT_CLK] = imx_clk_gate2("ecspi2_root_clk", "ecspi2_post_div", base + 0x4790, 0);
+	clks[IMX7D_ECSPI3_ROOT_CLK] = imx_clk_gate2("ecspi3_root_clk", "ecspi3_post_div", base + 0x47a0, 0);
+	clks[IMX7D_ECSPI4_ROOT_CLK] = imx_clk_gate2("ecspi4_root_clk", "ecspi4_post_div", base + 0x47b0, 0);
+	clks[IMX7D_PWM1_ROOT_CLK] = imx_clk_gate2("pwm1_root_clk", "pwm1_post_div", base + 0x4840, 0);
+	clks[IMX7D_PWM2_ROOT_CLK] = imx_clk_gate2("pwm2_root_clk", "pwm2_post_div", base + 0x4850, 0);
+	clks[IMX7D_PWM3_ROOT_CLK] = imx_clk_gate2("pwm3_root_clk", "pwm3_post_div", base + 0x4860, 0);
+	clks[IMX7D_PWM4_ROOT_CLK] = imx_clk_gate2("pwm4_root_clk", "pwm4_post_div", base + 0x4870, 0);
+	clks[IMX7D_FLEXTIMER1_ROOT_CLK] = imx_clk_gate2("flextimer1_root_clk", "flextimer1_post_div", base + 0x4800, 0);
+	clks[IMX7D_FLEXTIMER2_ROOT_CLK] = imx_clk_gate2("flextimer2_root_clk", "flextimer2_post_div", base + 0x4810, 0);
+	clks[IMX7D_SIM1_ROOT_CLK] = imx_clk_gate2("sim1_root_clk", "sim1_post_div", base + 0x4900, 0);
+	clks[IMX7D_SIM2_ROOT_CLK] = imx_clk_gate2("sim2_root_clk", "sim2_post_div", base + 0x4910, 0);
+	clks[IMX7D_GPT1_ROOT_CLK] = imx_clk_gate2("gpt1_root_clk", "gpt1_post_div", base + 0x47c0, 0);
+	clks[IMX7D_GPT2_ROOT_CLK] = imx_clk_gate2("gpt2_root_clk", "gpt2_post_div", base + 0x47d0, 0);
+	clks[IMX7D_GPT3_ROOT_CLK] = imx_clk_gate2("gpt3_root_clk", "gpt3_post_div", base + 0x47e0, 0);
+	clks[IMX7D_GPT4_ROOT_CLK] = imx_clk_gate2("gpt4_root_clk", "gpt4_post_div", base + 0x47f0, 0);
+	clks[IMX7D_TRACE_ROOT_CLK] = imx_clk_gate2("trace_root_clk", "trace_post_div", base + 0x4300, 0);
+	clks[IMX7D_WDOG1_ROOT_CLK] = imx_clk_gate2("wdog1_root_clk", "wdog_post_div", base + 0x49c0, 0);
+	clks[IMX7D_WDOG2_ROOT_CLK] = imx_clk_gate2("wdog2_root_clk", "wdog_post_div", base + 0x49d0, 0);
+	clks[IMX7D_WDOG3_ROOT_CLK] = imx_clk_gate2("wdog3_root_clk", "wdog_post_div", base + 0x49e0, 0);
+	clks[IMX7D_WDOG4_ROOT_CLK] = imx_clk_gate2("wdog4_root_clk", "wdog_post_div", base + 0x49f0, 0);
+	clks[IMX7D_CSI_MCLK_ROOT_CLK] = imx_clk_gate2("csi_mclk_root_clk", "csi_mclk_post_div", base + 0x4490, 0);
+	clks[IMX7D_AUDIO_MCLK_ROOT_CLK] = imx_clk_gate2("audio_mclk_root_clk", "audio_mclk_post_div", base + 0x4790, 0);
+	clks[IMX7D_WRCLK_ROOT_CLK] = imx_clk_gate2("wrclk_root_clk", "wrclk_post_div", base + 0x47a0, 0);
+
+	clks[IMX7D_GPT_3M_CLK] = imx_clk_fixed_factor("gpt_3m", "osc", 1, 8);
+
+	for (i = 0; i < ARRAY_SIZE(clks); i++)
+		if (IS_ERR(clks[i]))
+			pr_err("i.MX7D clk %d: register failed with %ld\n",
+					i, PTR_ERR(clks[i]));
+
+	clk_data.clks = clks;
+	clk_data.clk_num = ARRAY_SIZE(clks);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	/* TO BE FIXED LATER
+	 * Enable all clock to bring up imx7, otherwise system will be halt and block
+	 * the other part upstream Because imx7d clock design changed, clock framework
+	 * need do a little modify.
+	 * Dong Aisheng is working on this. After that, this part need be changed.
+	 */
+	for (i = 0; i < IMX7D_CLK_END; i++)
+		clk_prepare_enable(clks[i]);
+
+	/* use old gpt clk setting, gpt1 root clk must be twice as gpt counter freq */
+	clk_set_parent(clks[IMX7D_GPT1_ROOT_SRC], clks[IMX7D_OSC_24M_CLK]);
+
+	/*
+	 * init enet clock source:
+	 * 	AXI clock source is 250MHz
+	 *	Phy refrence clock is 25MHz
+	 *	1588 time clock source is 100MHz
+	 */
+	clk_set_parent(clks[IMX7D_ENET_AXI_ROOT_SRC], clks[IMX7D_PLL_ENET_MAIN_250M_CLK]);
+	clk_set_parent(clks[IMX7D_ENET_PHY_REF_ROOT_SRC], clks[IMX7D_PLL_ENET_MAIN_25M_CLK]);
+	clk_set_parent(clks[IMX7D_ENET1_TIME_ROOT_SRC], clks[IMX7D_PLL_ENET_MAIN_100M_CLK]);
+	clk_set_parent(clks[IMX7D_ENET2_TIME_ROOT_SRC], clks[IMX7D_PLL_ENET_MAIN_100M_CLK]);
+
+	/* set uart module clock's parent clock source that must be great then 80MHz */
+	clk_set_parent(clks[IMX7D_UART1_ROOT_SRC], clks[IMX7D_OSC_24M_CLK]);
+
+}
+CLK_OF_DECLARE(imx7d, "fsl,imx7d-ccm", imx7d_clocks_init);
diff --git a/drivers/clk/imx/clk-pfd.c b/drivers/clk/imx/clk-pfd.c
new file mode 100644
index 000000000000..0b0f6f66ec56
--- /dev/null
+++ b/drivers/clk/imx/clk-pfd.c
@@ -0,0 +1,158 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ * Copyright 2012 Linaro Ltd.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include "clk.h"
+
+/**
+ * struct clk_pfd - IMX PFD clock
+ * @clk_hw:	clock source
+ * @reg:	PFD register address
+ * @idx:	the index of PFD encoded in the register
+ *
+ * PFD clock found on i.MX6 series.  Each register for PFD has 4 clk_pfd
+ * data encoded, and member idx is used to specify the one.  And each
+ * register has SET, CLR and TOG registers at offset 0x4 0x8 and 0xc.
+ */
+struct clk_pfd {
+	struct clk_hw	hw;
+	void __iomem	*reg;
+	u8		idx;
+};
+
+#define to_clk_pfd(_hw) container_of(_hw, struct clk_pfd, hw)
+
+#define SET	0x4
+#define CLR	0x8
+#define OTG	0xc
+
+static int clk_pfd_enable(struct clk_hw *hw)
+{
+	struct clk_pfd *pfd = to_clk_pfd(hw);
+
+	writel_relaxed(1 << ((pfd->idx + 1) * 8 - 1), pfd->reg + CLR);
+
+	return 0;
+}
+
+static void clk_pfd_disable(struct clk_hw *hw)
+{
+	struct clk_pfd *pfd = to_clk_pfd(hw);
+
+	writel_relaxed(1 << ((pfd->idx + 1) * 8 - 1), pfd->reg + SET);
+}
+
+static unsigned long clk_pfd_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct clk_pfd *pfd = to_clk_pfd(hw);
+	u64 tmp = parent_rate;
+	u8 frac = (readl_relaxed(pfd->reg) >> (pfd->idx * 8)) & 0x3f;
+
+	tmp *= 18;
+	do_div(tmp, frac);
+
+	return tmp;
+}
+
+static long clk_pfd_round_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long *prate)
+{
+	u64 tmp = *prate;
+	u8 frac;
+
+	tmp = tmp * 18 + rate / 2;
+	do_div(tmp, rate);
+	frac = tmp;
+	if (frac < 12)
+		frac = 12;
+	else if (frac > 35)
+		frac = 35;
+	tmp = *prate;
+	tmp *= 18;
+	do_div(tmp, frac);
+
+	return tmp;
+}
+
+static int clk_pfd_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_pfd *pfd = to_clk_pfd(hw);
+	u64 tmp = parent_rate;
+	u8 frac;
+
+	tmp = tmp * 18 + rate / 2;
+	do_div(tmp, rate);
+	frac = tmp;
+	if (frac < 12)
+		frac = 12;
+	else if (frac > 35)
+		frac = 35;
+
+	writel_relaxed(0x3f << (pfd->idx * 8), pfd->reg + CLR);
+	writel_relaxed(frac << (pfd->idx * 8), pfd->reg + SET);
+
+	return 0;
+}
+
+static int clk_pfd_is_enabled(struct clk_hw *hw)
+{
+	struct clk_pfd *pfd = to_clk_pfd(hw);
+
+	if (readl_relaxed(pfd->reg) & (1 << ((pfd->idx + 1) * 8 - 1)))
+		return 0;
+
+	return 1;
+}
+
+static const struct clk_ops clk_pfd_ops = {
+	.enable		= clk_pfd_enable,
+	.disable	= clk_pfd_disable,
+	.recalc_rate	= clk_pfd_recalc_rate,
+	.round_rate	= clk_pfd_round_rate,
+	.set_rate	= clk_pfd_set_rate,
+	.is_enabled     = clk_pfd_is_enabled,
+};
+
+struct clk *imx_clk_pfd(const char *name, const char *parent_name,
+			void __iomem *reg, u8 idx)
+{
+	struct clk_pfd *pfd;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pfd = kzalloc(sizeof(*pfd), GFP_KERNEL);
+	if (!pfd)
+		return ERR_PTR(-ENOMEM);
+
+	pfd->reg = reg;
+	pfd->idx = idx;
+
+	init.name = name;
+	init.ops = &clk_pfd_ops;
+	init.flags = 0;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	pfd->hw.init = &init;
+
+	clk = clk_register(NULL, &pfd->hw);
+	if (IS_ERR(clk))
+		kfree(pfd);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-pllv1.c b/drivers/clk/imx/clk-pllv1.c
new file mode 100644
index 000000000000..c34ad8a611dd
--- /dev/null
+++ b/drivers/clk/imx/clk-pllv1.c
@@ -0,0 +1,141 @@
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+
+#include "clk.h"
+
+/**
+ * pll v1
+ *
+ * @clk_hw	clock source
+ * @parent	the parent clock name
+ * @base	base address of pll registers
+ *
+ * PLL clock version 1, found on i.MX1/21/25/27/31/35
+ */
+
+#define MFN_BITS	(10)
+#define MFN_SIGN	(BIT(MFN_BITS - 1))
+#define MFN_MASK	(MFN_SIGN - 1)
+
+struct clk_pllv1 {
+	struct clk_hw	hw;
+	void __iomem	*base;
+	enum imx_pllv1_type type;
+};
+
+#define to_clk_pllv1(clk) (container_of(clk, struct clk_pllv1, clk))
+
+static inline bool is_imx1_pllv1(struct clk_pllv1 *pll)
+{
+	return pll->type == IMX_PLLV1_IMX1;
+}
+
+static inline bool is_imx21_pllv1(struct clk_pllv1 *pll)
+{
+	return pll->type == IMX_PLLV1_IMX21;
+}
+
+static inline bool is_imx27_pllv1(struct clk_pllv1 *pll)
+{
+	return pll->type == IMX_PLLV1_IMX27;
+}
+
+static inline bool mfn_is_negative(struct clk_pllv1 *pll, unsigned int mfn)
+{
+	return !is_imx1_pllv1(pll) && !is_imx21_pllv1(pll) && (mfn & MFN_SIGN);
+}
+
+static unsigned long clk_pllv1_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_pllv1 *pll = to_clk_pllv1(hw);
+	long long ll;
+	int mfn_abs;
+	unsigned int mfi, mfn, mfd, pd;
+	u32 reg;
+	unsigned long rate;
+
+	reg = readl(pll->base);
+
+	/*
+	 * Get the resulting clock rate from a PLL register value and the input
+	 * frequency. PLLs with this register layout can be found on i.MX1,
+	 * i.MX21, i.MX27 and i,MX31
+	 *
+	 *                  mfi + mfn / (mfd + 1)
+	 *  f = 2 * f_ref * --------------------
+	 *                        pd + 1
+	 */
+
+	mfi = (reg >> 10) & 0xf;
+	mfn = reg & 0x3ff;
+	mfd = (reg >> 16) & 0x3ff;
+	pd =  (reg >> 26) & 0xf;
+
+	mfi = mfi <= 5 ? 5 : mfi;
+
+	mfn_abs = mfn;
+
+	/*
+	 * On all i.MXs except i.MX1 and i.MX21 mfn is a 10bit
+	 * 2's complements number.
+	 * On i.MX27 the bit 9 is the sign bit.
+	 */
+	if (mfn_is_negative(pll, mfn)) {
+		if (is_imx27_pllv1(pll))
+			mfn_abs = mfn & MFN_MASK;
+		else
+			mfn_abs = BIT(MFN_BITS) - mfn;
+	}
+
+	rate = parent_rate * 2;
+	rate /= pd + 1;
+
+	ll = (unsigned long long)rate * mfn_abs;
+
+	do_div(ll, mfd + 1);
+
+	if (mfn_is_negative(pll, mfn))
+		ll = -ll;
+
+	ll = (rate * mfi) + ll;
+
+	return ll;
+}
+
+static struct clk_ops clk_pllv1_ops = {
+	.recalc_rate = clk_pllv1_recalc_rate,
+};
+
+struct clk *imx_clk_pllv1(enum imx_pllv1_type type, const char *name,
+		const char *parent, void __iomem *base)
+{
+	struct clk_pllv1 *pll;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pll = kmalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll)
+		return ERR_PTR(-ENOMEM);
+
+	pll->base = base;
+	pll->type = type;
+
+	init.name = name;
+	init.ops = &clk_pllv1_ops;
+	init.flags = 0;
+	init.parent_names = &parent;
+	init.num_parents = 1;
+
+	pll->hw.init = &init;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-pllv2.c b/drivers/clk/imx/clk-pllv2.c
new file mode 100644
index 000000000000..20889d59b44d
--- /dev/null
+++ b/drivers/clk/imx/clk-pllv2.c
@@ -0,0 +1,266 @@
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+
+#include <asm/div64.h>
+
+#include "clk.h"
+
+#define to_clk_pllv2(clk) (container_of(clk, struct clk_pllv2, clk))
+
+/* PLL Register Offsets */
+#define MXC_PLL_DP_CTL			0x00
+#define MXC_PLL_DP_CONFIG		0x04
+#define MXC_PLL_DP_OP			0x08
+#define MXC_PLL_DP_MFD			0x0C
+#define MXC_PLL_DP_MFN			0x10
+#define MXC_PLL_DP_MFNMINUS		0x14
+#define MXC_PLL_DP_MFNPLUS		0x18
+#define MXC_PLL_DP_HFS_OP		0x1C
+#define MXC_PLL_DP_HFS_MFD		0x20
+#define MXC_PLL_DP_HFS_MFN		0x24
+#define MXC_PLL_DP_MFN_TOGC		0x28
+#define MXC_PLL_DP_DESTAT		0x2c
+
+/* PLL Register Bit definitions */
+#define MXC_PLL_DP_CTL_MUL_CTRL		0x2000
+#define MXC_PLL_DP_CTL_DPDCK0_2_EN	0x1000
+#define MXC_PLL_DP_CTL_DPDCK0_2_OFFSET	12
+#define MXC_PLL_DP_CTL_ADE		0x800
+#define MXC_PLL_DP_CTL_REF_CLK_DIV	0x400
+#define MXC_PLL_DP_CTL_REF_CLK_SEL_MASK	(3 << 8)
+#define MXC_PLL_DP_CTL_REF_CLK_SEL_OFFSET	8
+#define MXC_PLL_DP_CTL_HFSM		0x80
+#define MXC_PLL_DP_CTL_PRE		0x40
+#define MXC_PLL_DP_CTL_UPEN		0x20
+#define MXC_PLL_DP_CTL_RST		0x10
+#define MXC_PLL_DP_CTL_RCP		0x8
+#define MXC_PLL_DP_CTL_PLM		0x4
+#define MXC_PLL_DP_CTL_BRM0		0x2
+#define MXC_PLL_DP_CTL_LRF		0x1
+
+#define MXC_PLL_DP_CONFIG_BIST		0x8
+#define MXC_PLL_DP_CONFIG_SJC_CE	0x4
+#define MXC_PLL_DP_CONFIG_AREN		0x2
+#define MXC_PLL_DP_CONFIG_LDREQ		0x1
+
+#define MXC_PLL_DP_OP_MFI_OFFSET	4
+#define MXC_PLL_DP_OP_MFI_MASK		(0xF << 4)
+#define MXC_PLL_DP_OP_PDF_OFFSET	0
+#define MXC_PLL_DP_OP_PDF_MASK		0xF
+
+#define MXC_PLL_DP_MFD_OFFSET		0
+#define MXC_PLL_DP_MFD_MASK		0x07FFFFFF
+
+#define MXC_PLL_DP_MFN_OFFSET		0x0
+#define MXC_PLL_DP_MFN_MASK		0x07FFFFFF
+
+#define MXC_PLL_DP_MFN_TOGC_TOG_DIS	(1 << 17)
+#define MXC_PLL_DP_MFN_TOGC_TOG_EN	(1 << 16)
+#define MXC_PLL_DP_MFN_TOGC_CNT_OFFSET	0x0
+#define MXC_PLL_DP_MFN_TOGC_CNT_MASK	0xFFFF
+
+#define MXC_PLL_DP_DESTAT_TOG_SEL	(1 << 31)
+#define MXC_PLL_DP_DESTAT_MFN		0x07FFFFFF
+
+#define MAX_DPLL_WAIT_TRIES	1000 /* 1000 * udelay(1) = 1ms */
+
+struct clk_pllv2 {
+	struct clk_hw	hw;
+	void __iomem	*base;
+};
+
+static unsigned long __clk_pllv2_recalc_rate(unsigned long parent_rate,
+		u32 dp_ctl, u32 dp_op, u32 dp_mfd, u32 dp_mfn)
+{
+	long mfi, mfn, mfd, pdf, ref_clk, mfn_abs;
+	unsigned long dbl;
+	s64 temp;
+
+	dbl = dp_ctl & MXC_PLL_DP_CTL_DPDCK0_2_EN;
+
+	pdf = dp_op & MXC_PLL_DP_OP_PDF_MASK;
+	mfi = (dp_op & MXC_PLL_DP_OP_MFI_MASK) >> MXC_PLL_DP_OP_MFI_OFFSET;
+	mfi = (mfi <= 5) ? 5 : mfi;
+	mfd = dp_mfd & MXC_PLL_DP_MFD_MASK;
+	mfn = mfn_abs = dp_mfn & MXC_PLL_DP_MFN_MASK;
+	/* Sign extend to 32-bits */
+	if (mfn >= 0x04000000) {
+		mfn |= 0xFC000000;
+		mfn_abs = -mfn;
+	}
+
+	ref_clk = 2 * parent_rate;
+	if (dbl != 0)
+		ref_clk *= 2;
+
+	ref_clk /= (pdf + 1);
+	temp = (u64) ref_clk * mfn_abs;
+	do_div(temp, mfd + 1);
+	if (mfn < 0)
+		temp = -temp;
+	temp = (ref_clk * mfi) + temp;
+
+	return temp;
+}
+
+static unsigned long clk_pllv2_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	u32 dp_op, dp_mfd, dp_mfn, dp_ctl;
+	void __iomem *pllbase;
+	struct clk_pllv2 *pll = to_clk_pllv2(hw);
+
+	pllbase = pll->base;
+
+	dp_ctl = __raw_readl(pllbase + MXC_PLL_DP_CTL);
+	dp_op = __raw_readl(pllbase + MXC_PLL_DP_OP);
+	dp_mfd = __raw_readl(pllbase + MXC_PLL_DP_MFD);
+	dp_mfn = __raw_readl(pllbase + MXC_PLL_DP_MFN);
+
+	return __clk_pllv2_recalc_rate(parent_rate, dp_ctl, dp_op, dp_mfd, dp_mfn);
+}
+
+static int __clk_pllv2_set_rate(unsigned long rate, unsigned long parent_rate,
+		u32 *dp_op, u32 *dp_mfd, u32 *dp_mfn)
+{
+	u32 reg;
+	long mfi, pdf, mfn, mfd = 999999;
+	s64 temp64;
+	unsigned long quad_parent_rate;
+
+	quad_parent_rate = 4 * parent_rate;
+	pdf = mfi = -1;
+	while (++pdf < 16 && mfi < 5)
+		mfi = rate * (pdf+1) / quad_parent_rate;
+	if (mfi > 15)
+		return -EINVAL;
+	pdf--;
+
+	temp64 = rate * (pdf + 1) - quad_parent_rate * mfi;
+	do_div(temp64, quad_parent_rate / 1000000);
+	mfn = (long)temp64;
+
+	reg = mfi << 4 | pdf;
+
+	*dp_op = reg;
+	*dp_mfd = mfd;
+	*dp_mfn = mfn;
+
+	return 0;
+}
+
+static int clk_pllv2_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_pllv2 *pll = to_clk_pllv2(hw);
+	void __iomem *pllbase;
+	u32 dp_ctl, dp_op, dp_mfd, dp_mfn;
+	int ret;
+
+	pllbase = pll->base;
+
+
+	ret = __clk_pllv2_set_rate(rate, parent_rate, &dp_op, &dp_mfd, &dp_mfn);
+	if (ret)
+		return ret;
+
+	dp_ctl = __raw_readl(pllbase + MXC_PLL_DP_CTL);
+	/* use dpdck0_2 */
+	__raw_writel(dp_ctl | 0x1000L, pllbase + MXC_PLL_DP_CTL);
+
+	__raw_writel(dp_op, pllbase + MXC_PLL_DP_OP);
+	__raw_writel(dp_mfd, pllbase + MXC_PLL_DP_MFD);
+	__raw_writel(dp_mfn, pllbase + MXC_PLL_DP_MFN);
+
+	return 0;
+}
+
+static long clk_pllv2_round_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *prate)
+{
+	u32 dp_op, dp_mfd, dp_mfn;
+
+	__clk_pllv2_set_rate(rate, *prate, &dp_op, &dp_mfd, &dp_mfn);
+	return __clk_pllv2_recalc_rate(*prate, MXC_PLL_DP_CTL_DPDCK0_2_EN,
+			dp_op, dp_mfd, dp_mfn);
+}
+
+static int clk_pllv2_prepare(struct clk_hw *hw)
+{
+	struct clk_pllv2 *pll = to_clk_pllv2(hw);
+	u32 reg;
+	void __iomem *pllbase;
+	int i = 0;
+
+	pllbase = pll->base;
+	reg = __raw_readl(pllbase + MXC_PLL_DP_CTL) | MXC_PLL_DP_CTL_UPEN;
+	__raw_writel(reg, pllbase + MXC_PLL_DP_CTL);
+
+	/* Wait for lock */
+	do {
+		reg = __raw_readl(pllbase + MXC_PLL_DP_CTL);
+		if (reg & MXC_PLL_DP_CTL_LRF)
+			break;
+
+		udelay(1);
+	} while (++i < MAX_DPLL_WAIT_TRIES);
+
+	if (i == MAX_DPLL_WAIT_TRIES) {
+		pr_err("MX5: pll locking failed\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void clk_pllv2_unprepare(struct clk_hw *hw)
+{
+	struct clk_pllv2 *pll = to_clk_pllv2(hw);
+	u32 reg;
+	void __iomem *pllbase;
+
+	pllbase = pll->base;
+	reg = __raw_readl(pllbase + MXC_PLL_DP_CTL) & ~MXC_PLL_DP_CTL_UPEN;
+	__raw_writel(reg, pllbase + MXC_PLL_DP_CTL);
+}
+
+static struct clk_ops clk_pllv2_ops = {
+	.prepare = clk_pllv2_prepare,
+	.unprepare = clk_pllv2_unprepare,
+	.recalc_rate = clk_pllv2_recalc_rate,
+	.round_rate = clk_pllv2_round_rate,
+	.set_rate = clk_pllv2_set_rate,
+};
+
+struct clk *imx_clk_pllv2(const char *name, const char *parent,
+		void __iomem *base)
+{
+	struct clk_pllv2 *pll;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll)
+		return ERR_PTR(-ENOMEM);
+
+	pll->base = base;
+
+	init.name = name;
+	init.ops = &clk_pllv2_ops;
+	init.flags = 0;
+	init.parent_names = &parent;
+	init.num_parents = 1;
+
+	pll->hw.init = &init;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-pllv3.c b/drivers/clk/imx/clk-pllv3.c
new file mode 100644
index 000000000000..f0d15fb9d783
--- /dev/null
+++ b/drivers/clk/imx/clk-pllv3.c
@@ -0,0 +1,338 @@
+/*
+ * Copyright 2012 Freescale Semiconductor, Inc.
+ * Copyright 2012 Linaro Ltd.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/err.h>
+#include "clk.h"
+
+#define PLL_NUM_OFFSET		0x10
+#define PLL_DENOM_OFFSET	0x20
+
+#define BM_PLL_POWER		(0x1 << 12)
+#define BM_PLL_LOCK		(0x1 << 31)
+#define IMX7_ENET_PLL_POWER	(0x1 << 5)
+
+/**
+ * struct clk_pllv3 - IMX PLL clock version 3
+ * @clk_hw:	 clock source
+ * @base:	 base address of PLL registers
+ * @powerup_set: set POWER bit to power up the PLL
+ * @powerdown:   pll powerdown offset bit
+ * @div_mask:	 mask of divider bits
+ * @div_shift:	 shift of divider bits
+ *
+ * IMX PLL clock version 3, found on i.MX6 series.  Divider for pllv3
+ * is actually a multiplier, and always sits at bit 0.
+ */
+struct clk_pllv3 {
+	struct clk_hw	hw;
+	void __iomem	*base;
+	bool		powerup_set;
+	u32		powerdown;
+	u32		div_mask;
+	u32		div_shift;
+};
+
+#define to_clk_pllv3(_hw) container_of(_hw, struct clk_pllv3, hw)
+
+static int clk_pllv3_wait_lock(struct clk_pllv3 *pll)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(10);
+	u32 val = readl_relaxed(pll->base) & pll->powerdown;
+
+	/* No need to wait for lock when pll is not powered up */
+	if ((pll->powerup_set && !val) || (!pll->powerup_set && val))
+		return 0;
+
+	/* Wait for PLL to lock */
+	do {
+		if (readl_relaxed(pll->base) & BM_PLL_LOCK)
+			break;
+		if (time_after(jiffies, timeout))
+			break;
+		usleep_range(50, 500);
+	} while (1);
+
+	return readl_relaxed(pll->base) & BM_PLL_LOCK ? 0 : -ETIMEDOUT;
+}
+
+static int clk_pllv3_prepare(struct clk_hw *hw)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	u32 val;
+
+	val = readl_relaxed(pll->base);
+	if (pll->powerup_set)
+		val |= BM_PLL_POWER;
+	else
+		val &= ~BM_PLL_POWER;
+	writel_relaxed(val, pll->base);
+
+	return clk_pllv3_wait_lock(pll);
+}
+
+static void clk_pllv3_unprepare(struct clk_hw *hw)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	u32 val;
+
+	val = readl_relaxed(pll->base);
+	if (pll->powerup_set)
+		val &= ~BM_PLL_POWER;
+	else
+		val |= BM_PLL_POWER;
+	writel_relaxed(val, pll->base);
+}
+
+static unsigned long clk_pllv3_recalc_rate(struct clk_hw *hw,
+					   unsigned long parent_rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	u32 div = (readl_relaxed(pll->base) >> pll->div_shift)  & pll->div_mask;
+
+	return (div == 1) ? parent_rate * 22 : parent_rate * 20;
+}
+
+static long clk_pllv3_round_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long *prate)
+{
+	unsigned long parent_rate = *prate;
+
+	return (rate >= parent_rate * 22) ? parent_rate * 22 :
+					    parent_rate * 20;
+}
+
+static int clk_pllv3_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	u32 val, div;
+
+	if (rate == parent_rate * 22)
+		div = 1;
+	else if (rate == parent_rate * 20)
+		div = 0;
+	else
+		return -EINVAL;
+
+	val = readl_relaxed(pll->base);
+	val &= ~(pll->div_mask << pll->div_shift);
+	val |= (div << pll->div_shift);
+	writel_relaxed(val, pll->base);
+
+	return clk_pllv3_wait_lock(pll);
+}
+
+static const struct clk_ops clk_pllv3_ops = {
+	.prepare	= clk_pllv3_prepare,
+	.unprepare	= clk_pllv3_unprepare,
+	.recalc_rate	= clk_pllv3_recalc_rate,
+	.round_rate	= clk_pllv3_round_rate,
+	.set_rate	= clk_pllv3_set_rate,
+};
+
+static unsigned long clk_pllv3_sys_recalc_rate(struct clk_hw *hw,
+					       unsigned long parent_rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	u32 div = readl_relaxed(pll->base) & pll->div_mask;
+
+	return parent_rate * div / 2;
+}
+
+static long clk_pllv3_sys_round_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long *prate)
+{
+	unsigned long parent_rate = *prate;
+	unsigned long min_rate = parent_rate * 54 / 2;
+	unsigned long max_rate = parent_rate * 108 / 2;
+	u32 div;
+
+	if (rate > max_rate)
+		rate = max_rate;
+	else if (rate < min_rate)
+		rate = min_rate;
+	div = rate * 2 / parent_rate;
+
+	return parent_rate * div / 2;
+}
+
+static int clk_pllv3_sys_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	unsigned long min_rate = parent_rate * 54 / 2;
+	unsigned long max_rate = parent_rate * 108 / 2;
+	u32 val, div;
+
+	if (rate < min_rate || rate > max_rate)
+		return -EINVAL;
+
+	div = rate * 2 / parent_rate;
+	val = readl_relaxed(pll->base);
+	val &= ~pll->div_mask;
+	val |= div;
+	writel_relaxed(val, pll->base);
+
+	return clk_pllv3_wait_lock(pll);
+}
+
+static const struct clk_ops clk_pllv3_sys_ops = {
+	.prepare	= clk_pllv3_prepare,
+	.unprepare	= clk_pllv3_unprepare,
+	.recalc_rate	= clk_pllv3_sys_recalc_rate,
+	.round_rate	= clk_pllv3_sys_round_rate,
+	.set_rate	= clk_pllv3_sys_set_rate,
+};
+
+static unsigned long clk_pllv3_av_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	u32 mfn = readl_relaxed(pll->base + PLL_NUM_OFFSET);
+	u32 mfd = readl_relaxed(pll->base + PLL_DENOM_OFFSET);
+	u32 div = readl_relaxed(pll->base) & pll->div_mask;
+
+	return (parent_rate * div) + ((parent_rate / mfd) * mfn);
+}
+
+static long clk_pllv3_av_round_rate(struct clk_hw *hw, unsigned long rate,
+				    unsigned long *prate)
+{
+	unsigned long parent_rate = *prate;
+	unsigned long min_rate = parent_rate * 27;
+	unsigned long max_rate = parent_rate * 54;
+	u32 div;
+	u32 mfn, mfd = 1000000;
+	u64 temp64;
+
+	if (rate > max_rate)
+		rate = max_rate;
+	else if (rate < min_rate)
+		rate = min_rate;
+
+	div = rate / parent_rate;
+	temp64 = (u64) (rate - div * parent_rate);
+	temp64 *= mfd;
+	do_div(temp64, parent_rate);
+	mfn = temp64;
+
+	return parent_rate * div + parent_rate / mfd * mfn;
+}
+
+static int clk_pllv3_av_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	unsigned long min_rate = parent_rate * 27;
+	unsigned long max_rate = parent_rate * 54;
+	u32 val, div;
+	u32 mfn, mfd = 1000000;
+	u64 temp64;
+
+	if (rate < min_rate || rate > max_rate)
+		return -EINVAL;
+
+	div = rate / parent_rate;
+	temp64 = (u64) (rate - div * parent_rate);
+	temp64 *= mfd;
+	do_div(temp64, parent_rate);
+	mfn = temp64;
+
+	val = readl_relaxed(pll->base);
+	val &= ~pll->div_mask;
+	val |= div;
+	writel_relaxed(val, pll->base);
+	writel_relaxed(mfn, pll->base + PLL_NUM_OFFSET);
+	writel_relaxed(mfd, pll->base + PLL_DENOM_OFFSET);
+
+	return clk_pllv3_wait_lock(pll);
+}
+
+static const struct clk_ops clk_pllv3_av_ops = {
+	.prepare	= clk_pllv3_prepare,
+	.unprepare	= clk_pllv3_unprepare,
+	.recalc_rate	= clk_pllv3_av_recalc_rate,
+	.round_rate	= clk_pllv3_av_round_rate,
+	.set_rate	= clk_pllv3_av_set_rate,
+};
+
+static unsigned long clk_pllv3_enet_recalc_rate(struct clk_hw *hw,
+						unsigned long parent_rate)
+{
+	return 500000000;
+}
+
+static const struct clk_ops clk_pllv3_enet_ops = {
+	.prepare	= clk_pllv3_prepare,
+	.unprepare	= clk_pllv3_unprepare,
+	.recalc_rate	= clk_pllv3_enet_recalc_rate,
+};
+
+struct clk *imx_clk_pllv3(enum imx_pllv3_type type, const char *name,
+			  const char *parent_name, void __iomem *base,
+			  u32 div_mask)
+{
+	struct clk_pllv3 *pll;
+	const struct clk_ops *ops;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll)
+		return ERR_PTR(-ENOMEM);
+
+	pll->powerdown = BM_PLL_POWER;
+
+	switch (type) {
+	case IMX_PLLV3_SYS:
+		ops = &clk_pllv3_sys_ops;
+		break;
+	case IMX_PLLV3_USB_VF610:
+		pll->div_shift = 1;
+	case IMX_PLLV3_USB:
+		ops = &clk_pllv3_ops;
+		pll->powerup_set = true;
+		break;
+	case IMX_PLLV3_AV:
+		ops = &clk_pllv3_av_ops;
+		break;
+	case IMX_PLLV3_ENET_IMX7:
+		pll->powerdown = IMX7_ENET_PLL_POWER;
+	case IMX_PLLV3_ENET:
+		ops = &clk_pllv3_enet_ops;
+		break;
+	default:
+		ops = &clk_pllv3_ops;
+	}
+	pll->base = base;
+	pll->div_mask = div_mask;
+
+	init.name = name;
+	init.ops = ops;
+	init.flags = 0;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	pll->hw.init = &init;
+
+	clk = clk_register(NULL, &pll->hw);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
diff --git a/drivers/clk/imx/clk-vf610.c b/drivers/clk/imx/clk-vf610.c
new file mode 100644
index 000000000000..bff45ead7389
--- /dev/null
+++ b/drivers/clk/imx/clk-vf610.c
@@ -0,0 +1,416 @@
+/*
+ * Copyright 2012-2013 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ */
+
+#include <linux/of_address.h>
+#include <linux/clk.h>
+#include <dt-bindings/clock/vf610-clock.h>
+
+#include "clk.h"
+
+#define CCM_CCR			(ccm_base + 0x00)
+#define CCM_CSR			(ccm_base + 0x04)
+#define CCM_CCSR		(ccm_base + 0x08)
+#define CCM_CACRR		(ccm_base + 0x0c)
+#define CCM_CSCMR1		(ccm_base + 0x10)
+#define CCM_CSCDR1		(ccm_base + 0x14)
+#define CCM_CSCDR2		(ccm_base + 0x18)
+#define CCM_CSCDR3		(ccm_base + 0x1c)
+#define CCM_CSCMR2		(ccm_base + 0x20)
+#define CCM_CSCDR4		(ccm_base + 0x24)
+#define CCM_CLPCR		(ccm_base + 0x2c)
+#define CCM_CISR		(ccm_base + 0x30)
+#define CCM_CIMR		(ccm_base + 0x34)
+#define CCM_CGPR		(ccm_base + 0x3c)
+#define CCM_CCGR0		(ccm_base + 0x40)
+#define CCM_CCGR1		(ccm_base + 0x44)
+#define CCM_CCGR2		(ccm_base + 0x48)
+#define CCM_CCGR3		(ccm_base + 0x4c)
+#define CCM_CCGR4		(ccm_base + 0x50)
+#define CCM_CCGR5		(ccm_base + 0x54)
+#define CCM_CCGR6		(ccm_base + 0x58)
+#define CCM_CCGR7		(ccm_base + 0x5c)
+#define CCM_CCGR8		(ccm_base + 0x60)
+#define CCM_CCGR9		(ccm_base + 0x64)
+#define CCM_CCGR10		(ccm_base + 0x68)
+#define CCM_CCGR11		(ccm_base + 0x6c)
+#define CCM_CMEOR0		(ccm_base + 0x70)
+#define CCM_CMEOR1		(ccm_base + 0x74)
+#define CCM_CMEOR2		(ccm_base + 0x78)
+#define CCM_CMEOR3		(ccm_base + 0x7c)
+#define CCM_CMEOR4		(ccm_base + 0x80)
+#define CCM_CMEOR5		(ccm_base + 0x84)
+#define CCM_CPPDSR		(ccm_base + 0x88)
+#define CCM_CCOWR		(ccm_base + 0x8c)
+#define CCM_CCPGR0		(ccm_base + 0x90)
+#define CCM_CCPGR1		(ccm_base + 0x94)
+#define CCM_CCPGR2		(ccm_base + 0x98)
+#define CCM_CCPGR3		(ccm_base + 0x9c)
+
+#define CCM_CCGRx_CGn(n)	((n) * 2)
+
+#define PFD_PLL1_BASE		(anatop_base + 0x2b0)
+#define PFD_PLL2_BASE		(anatop_base + 0x100)
+#define PFD_PLL3_BASE		(anatop_base + 0xf0)
+#define PLL1_CTRL		(anatop_base + 0x270)
+#define PLL2_CTRL		(anatop_base + 0x30)
+#define PLL3_CTRL		(anatop_base + 0x10)
+#define PLL4_CTRL		(anatop_base + 0x70)
+#define PLL5_CTRL		(anatop_base + 0xe0)
+#define PLL6_CTRL		(anatop_base + 0xa0)
+#define PLL7_CTRL		(anatop_base + 0x20)
+#define ANA_MISC1		(anatop_base + 0x160)
+
+static void __iomem *anatop_base;
+static void __iomem *ccm_base;
+
+/* sources for multiplexer clocks, this is used multiple times */
+static const char *fast_sels[]	= { "firc", "fxosc", };
+static const char *slow_sels[]	= { "sirc_32k", "sxosc", };
+static const char *pll1_sels[]	= { "pll1_sys", "pll1_pfd1", "pll1_pfd2", "pll1_pfd3", "pll1_pfd4", };
+static const char *pll2_sels[]	= { "pll2_bus", "pll2_pfd1", "pll2_pfd2", "pll2_pfd3", "pll2_pfd4", };
+static const char *pll_bypass_src_sels[] = { "fast_clk_sel", "lvds1_in", };
+static const char *pll1_bypass_sels[] = { "pll1", "pll1_bypass_src", };
+static const char *pll2_bypass_sels[] = { "pll2", "pll2_bypass_src", };
+static const char *pll3_bypass_sels[] = { "pll3", "pll3_bypass_src", };
+static const char *pll4_bypass_sels[] = { "pll4", "pll4_bypass_src", };
+static const char *pll5_bypass_sels[] = { "pll5", "pll5_bypass_src", };
+static const char *pll6_bypass_sels[] = { "pll6", "pll6_bypass_src", };
+static const char *pll7_bypass_sels[] = { "pll7", "pll7_bypass_src", };
+static const char *sys_sels[]	= { "fast_clk_sel", "slow_clk_sel", "pll2_pfd_sel", "pll2_bus", "pll1_pfd_sel", "pll3_usb_otg", };
+static const char *ddr_sels[]	= { "pll2_pfd2", "sys_sel", };
+static const char *rmii_sels[]	= { "enet_ext", "audio_ext", "enet_50m", "enet_25m", };
+static const char *enet_ts_sels[]	= { "enet_ext", "fxosc", "audio_ext", "usb", "enet_ts", "enet_25m", "enet_50m", };
+static const char *esai_sels[]	= { "audio_ext", "mlb", "spdif_rx", "pll4_audio_div", };
+static const char *sai_sels[]	= { "audio_ext", "mlb", "spdif_rx", "pll4_audio_div", };
+static const char *nfc_sels[]	= { "platform_bus", "pll1_pfd1", "pll3_pfd1", "pll3_pfd3", };
+static const char *qspi_sels[]	= { "pll3_usb_otg", "pll3_pfd4", "pll2_pfd4", "pll1_pfd4", };
+static const char *esdhc_sels[]	= { "pll3_usb_otg", "pll3_pfd3", "pll1_pfd3", "platform_bus", };
+static const char *dcu_sels[]	= { "pll1_pfd2", "pll3_usb_otg", };
+static const char *gpu_sels[]	= { "pll2_pfd2", "pll3_pfd2", };
+static const char *vadc_sels[]	= { "pll6_video_div", "pll3_usb_otg_div", "pll3_usb_otg", };
+/* FTM counter clock source, not module clock */
+static const char *ftm_ext_sels[]	= {"sirc_128k", "sxosc", "fxosc_half", "audio_ext", };
+static const char *ftm_fix_sels[]	= { "sxosc", "ipg_bus", };
+
+
+static struct clk_div_table pll4_audio_div_table[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 6 },
+	{ .val = 3, .div = 8 },
+	{ .val = 4, .div = 10 },
+	{ .val = 5, .div = 12 },
+	{ .val = 6, .div = 14 },
+	{ .val = 7, .div = 16 },
+	{ }
+};
+
+static struct clk *clk[VF610_CLK_END];
+static struct clk_onecell_data clk_data;
+
+static unsigned int const clks_init_on[] __initconst = {
+	VF610_CLK_SYS_BUS,
+	VF610_CLK_DDR_SEL,
+	VF610_CLK_DAP,
+};
+
+static struct clk * __init vf610_get_fixed_clock(
+				struct device_node *ccm_node, const char *name)
+{
+	struct clk *clk = of_clk_get_by_name(ccm_node, name);
+
+	/* Backward compatibility if device tree is missing clks assignments */
+	if (IS_ERR(clk))
+		clk = imx_obtain_fixed_clock(name, 0);
+	return clk;
+};
+
+static void __init vf610_clocks_init(struct device_node *ccm_node)
+{
+	struct device_node *np;
+	int i;
+
+	clk[VF610_CLK_DUMMY] = imx_clk_fixed("dummy", 0);
+	clk[VF610_CLK_SIRC_128K] = imx_clk_fixed("sirc_128k", 128000);
+	clk[VF610_CLK_SIRC_32K] = imx_clk_fixed("sirc_32k", 32000);
+	clk[VF610_CLK_FIRC] = imx_clk_fixed("firc", 24000000);
+
+	clk[VF610_CLK_SXOSC] = vf610_get_fixed_clock(ccm_node, "sxosc");
+	clk[VF610_CLK_FXOSC] = vf610_get_fixed_clock(ccm_node, "fxosc");
+	clk[VF610_CLK_AUDIO_EXT] = vf610_get_fixed_clock(ccm_node, "audio_ext");
+	clk[VF610_CLK_ENET_EXT] = vf610_get_fixed_clock(ccm_node, "enet_ext");
+
+	/* Clock source from external clock via LVDs PAD */
+	clk[VF610_CLK_ANACLK1] = vf610_get_fixed_clock(ccm_node, "anaclk1");
+
+	clk[VF610_CLK_FXOSC_HALF] = imx_clk_fixed_factor("fxosc_half", "fxosc", 1, 2);
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,vf610-anatop");
+	anatop_base = of_iomap(np, 0);
+	BUG_ON(!anatop_base);
+
+	np = ccm_node;
+	ccm_base = of_iomap(np, 0);
+	BUG_ON(!ccm_base);
+
+	clk[VF610_CLK_SLOW_CLK_SEL] = imx_clk_mux("slow_clk_sel", CCM_CCSR, 4, 1, slow_sels, ARRAY_SIZE(slow_sels));
+	clk[VF610_CLK_FASK_CLK_SEL] = imx_clk_mux("fast_clk_sel", CCM_CCSR, 5, 1, fast_sels, ARRAY_SIZE(fast_sels));
+
+	clk[VF610_CLK_PLL1_BYPASS_SRC] = imx_clk_mux("pll1_bypass_src", PLL1_CTRL, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[VF610_CLK_PLL2_BYPASS_SRC] = imx_clk_mux("pll2_bypass_src", PLL2_CTRL, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[VF610_CLK_PLL3_BYPASS_SRC] = imx_clk_mux("pll3_bypass_src", PLL3_CTRL, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[VF610_CLK_PLL4_BYPASS_SRC] = imx_clk_mux("pll4_bypass_src", PLL4_CTRL, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[VF610_CLK_PLL5_BYPASS_SRC] = imx_clk_mux("pll5_bypass_src", PLL5_CTRL, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[VF610_CLK_PLL6_BYPASS_SRC] = imx_clk_mux("pll6_bypass_src", PLL6_CTRL, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+	clk[VF610_CLK_PLL7_BYPASS_SRC] = imx_clk_mux("pll7_bypass_src", PLL7_CTRL, 14, 1, pll_bypass_src_sels, ARRAY_SIZE(pll_bypass_src_sels));
+
+	clk[VF610_CLK_PLL1] = imx_clk_pllv3(IMX_PLLV3_GENERIC, "pll1", "pll1_bypass_src", PLL1_CTRL, 0x1);
+	clk[VF610_CLK_PLL2] = imx_clk_pllv3(IMX_PLLV3_GENERIC, "pll2", "pll2_bypass_src", PLL2_CTRL, 0x1);
+	clk[VF610_CLK_PLL3] = imx_clk_pllv3(IMX_PLLV3_USB_VF610,     "pll3", "pll3_bypass_src", PLL3_CTRL, 0x2);
+	clk[VF610_CLK_PLL4] = imx_clk_pllv3(IMX_PLLV3_AV,      "pll4", "pll4_bypass_src", PLL4_CTRL, 0x7f);
+	clk[VF610_CLK_PLL5] = imx_clk_pllv3(IMX_PLLV3_ENET,    "pll5", "pll5_bypass_src", PLL5_CTRL, 0x3);
+	clk[VF610_CLK_PLL6] = imx_clk_pllv3(IMX_PLLV3_AV,      "pll6", "pll6_bypass_src", PLL6_CTRL, 0x7f);
+	clk[VF610_CLK_PLL7] = imx_clk_pllv3(IMX_PLLV3_USB_VF610,     "pll7", "pll7_bypass_src", PLL7_CTRL, 0x2);
+
+	clk[VF610_PLL1_BYPASS] = imx_clk_mux_flags("pll1_bypass", PLL1_CTRL, 16, 1, pll1_bypass_sels, ARRAY_SIZE(pll1_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[VF610_PLL2_BYPASS] = imx_clk_mux_flags("pll2_bypass", PLL2_CTRL, 16, 1, pll2_bypass_sels, ARRAY_SIZE(pll2_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[VF610_PLL3_BYPASS] = imx_clk_mux_flags("pll3_bypass", PLL3_CTRL, 16, 1, pll3_bypass_sels, ARRAY_SIZE(pll3_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[VF610_PLL4_BYPASS] = imx_clk_mux_flags("pll4_bypass", PLL4_CTRL, 16, 1, pll4_bypass_sels, ARRAY_SIZE(pll4_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[VF610_PLL5_BYPASS] = imx_clk_mux_flags("pll5_bypass", PLL5_CTRL, 16, 1, pll5_bypass_sels, ARRAY_SIZE(pll5_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[VF610_PLL6_BYPASS] = imx_clk_mux_flags("pll6_bypass", PLL6_CTRL, 16, 1, pll6_bypass_sels, ARRAY_SIZE(pll6_bypass_sels), CLK_SET_RATE_PARENT);
+	clk[VF610_PLL7_BYPASS] = imx_clk_mux_flags("pll7_bypass", PLL7_CTRL, 16, 1, pll7_bypass_sels, ARRAY_SIZE(pll7_bypass_sels), CLK_SET_RATE_PARENT);
+
+	/* Do not bypass PLLs initially */
+	clk_set_parent(clk[VF610_PLL1_BYPASS], clk[VF610_CLK_PLL1]);
+	clk_set_parent(clk[VF610_PLL2_BYPASS], clk[VF610_CLK_PLL2]);
+	clk_set_parent(clk[VF610_PLL3_BYPASS], clk[VF610_CLK_PLL3]);
+	clk_set_parent(clk[VF610_PLL4_BYPASS], clk[VF610_CLK_PLL4]);
+	clk_set_parent(clk[VF610_PLL5_BYPASS], clk[VF610_CLK_PLL5]);
+	clk_set_parent(clk[VF610_PLL6_BYPASS], clk[VF610_CLK_PLL6]);
+	clk_set_parent(clk[VF610_PLL7_BYPASS], clk[VF610_CLK_PLL7]);
+
+	clk[VF610_CLK_PLL1_SYS]      = imx_clk_gate("pll1_sys",      "pll1_bypass", PLL1_CTRL, 13);
+	clk[VF610_CLK_PLL2_BUS]      = imx_clk_gate("pll2_bus",      "pll2_bypass", PLL2_CTRL, 13);
+	clk[VF610_CLK_PLL3_USB_OTG]  = imx_clk_gate("pll3_usb_otg",  "pll3_bypass", PLL3_CTRL, 13);
+	clk[VF610_CLK_PLL4_AUDIO]    = imx_clk_gate("pll4_audio",    "pll4_bypass", PLL4_CTRL, 13);
+	clk[VF610_CLK_PLL5_ENET]     = imx_clk_gate("pll5_enet",     "pll5_bypass", PLL5_CTRL, 13);
+	clk[VF610_CLK_PLL6_VIDEO]    = imx_clk_gate("pll6_video",    "pll6_bypass", PLL6_CTRL, 13);
+	clk[VF610_CLK_PLL7_USB_HOST] = imx_clk_gate("pll7_usb_host", "pll7_bypass", PLL7_CTRL, 13);
+
+	clk[VF610_CLK_LVDS1_IN]  = imx_clk_gate_exclusive("lvds1_in", "anaclk1", ANA_MISC1, 12, BIT(10));
+
+	clk[VF610_CLK_PLL1_PFD1] = imx_clk_pfd("pll1_pfd1", "pll1_sys", PFD_PLL1_BASE, 0);
+	clk[VF610_CLK_PLL1_PFD2] = imx_clk_pfd("pll1_pfd2", "pll1_sys", PFD_PLL1_BASE, 1);
+	clk[VF610_CLK_PLL1_PFD3] = imx_clk_pfd("pll1_pfd3", "pll1_sys", PFD_PLL1_BASE, 2);
+	clk[VF610_CLK_PLL1_PFD4] = imx_clk_pfd("pll1_pfd4", "pll1_sys", PFD_PLL1_BASE, 3);
+
+	clk[VF610_CLK_PLL2_PFD1] = imx_clk_pfd("pll2_pfd1", "pll2_bus", PFD_PLL2_BASE, 0);
+	clk[VF610_CLK_PLL2_PFD2] = imx_clk_pfd("pll2_pfd2", "pll2_bus", PFD_PLL2_BASE, 1);
+	clk[VF610_CLK_PLL2_PFD3] = imx_clk_pfd("pll2_pfd3", "pll2_bus", PFD_PLL2_BASE, 2);
+	clk[VF610_CLK_PLL2_PFD4] = imx_clk_pfd("pll2_pfd4", "pll2_bus", PFD_PLL2_BASE, 3);
+
+	clk[VF610_CLK_PLL3_PFD1] = imx_clk_pfd("pll3_pfd1", "pll3_usb_otg", PFD_PLL3_BASE, 0);
+	clk[VF610_CLK_PLL3_PFD2] = imx_clk_pfd("pll3_pfd2", "pll3_usb_otg", PFD_PLL3_BASE, 1);
+	clk[VF610_CLK_PLL3_PFD3] = imx_clk_pfd("pll3_pfd3", "pll3_usb_otg", PFD_PLL3_BASE, 2);
+	clk[VF610_CLK_PLL3_PFD4] = imx_clk_pfd("pll3_pfd4", "pll3_usb_otg", PFD_PLL3_BASE, 3);
+
+	clk[VF610_CLK_PLL1_PFD_SEL] = imx_clk_mux("pll1_pfd_sel", CCM_CCSR, 16, 3, pll1_sels, 5);
+	clk[VF610_CLK_PLL2_PFD_SEL] = imx_clk_mux("pll2_pfd_sel", CCM_CCSR, 19, 3, pll2_sels, 5);
+	clk[VF610_CLK_SYS_SEL] = imx_clk_mux("sys_sel", CCM_CCSR, 0, 3, sys_sels, ARRAY_SIZE(sys_sels));
+	clk[VF610_CLK_DDR_SEL] = imx_clk_mux("ddr_sel", CCM_CCSR, 6, 1, ddr_sels, ARRAY_SIZE(ddr_sels));
+	clk[VF610_CLK_SYS_BUS] = imx_clk_divider("sys_bus", "sys_sel", CCM_CACRR, 0, 3);
+	clk[VF610_CLK_PLATFORM_BUS] = imx_clk_divider("platform_bus", "sys_bus", CCM_CACRR, 3, 3);
+	clk[VF610_CLK_IPG_BUS] = imx_clk_divider("ipg_bus", "platform_bus", CCM_CACRR, 11, 2);
+
+	clk[VF610_CLK_PLL3_MAIN_DIV] = imx_clk_divider("pll3_usb_otg_div", "pll3_usb_otg", CCM_CACRR, 20, 1);
+	clk[VF610_CLK_PLL4_MAIN_DIV] = clk_register_divider_table(NULL, "pll4_audio_div", "pll4_audio", 0, CCM_CACRR, 6, 3, 0, pll4_audio_div_table, &imx_ccm_lock);
+	clk[VF610_CLK_PLL6_MAIN_DIV] = imx_clk_divider("pll6_video_div", "pll6_video", CCM_CACRR, 21, 1);
+
+	clk[VF610_CLK_USBPHY0] = imx_clk_gate("usbphy0", "pll3_usb_otg", PLL3_CTRL, 6);
+	clk[VF610_CLK_USBPHY1] = imx_clk_gate("usbphy1", "pll7_usb_host", PLL7_CTRL, 6);
+
+	clk[VF610_CLK_USBC0] = imx_clk_gate2("usbc0", "ipg_bus", CCM_CCGR1, CCM_CCGRx_CGn(4));
+	clk[VF610_CLK_USBC1] = imx_clk_gate2("usbc1", "ipg_bus", CCM_CCGR7, CCM_CCGRx_CGn(4));
+
+	clk[VF610_CLK_QSPI0_SEL] = imx_clk_mux("qspi0_sel", CCM_CSCMR1, 22, 2, qspi_sels, 4);
+	clk[VF610_CLK_QSPI0_EN] = imx_clk_gate("qspi0_en", "qspi0_sel", CCM_CSCDR3, 4);
+	clk[VF610_CLK_QSPI0_X4_DIV] = imx_clk_divider("qspi0_x4", "qspi0_en", CCM_CSCDR3, 0, 2);
+	clk[VF610_CLK_QSPI0_X2_DIV] = imx_clk_divider("qspi0_x2", "qspi0_x4", CCM_CSCDR3, 2, 1);
+	clk[VF610_CLK_QSPI0_X1_DIV] = imx_clk_divider("qspi0_x1", "qspi0_x2", CCM_CSCDR3, 3, 1);
+	clk[VF610_CLK_QSPI0] = imx_clk_gate2("qspi0", "qspi0_x1", CCM_CCGR2, CCM_CCGRx_CGn(4));
+
+	clk[VF610_CLK_QSPI1_SEL] = imx_clk_mux("qspi1_sel", CCM_CSCMR1, 24, 2, qspi_sels, 4);
+	clk[VF610_CLK_QSPI1_EN] = imx_clk_gate("qspi1_en", "qspi1_sel", CCM_CSCDR3, 12);
+	clk[VF610_CLK_QSPI1_X4_DIV] = imx_clk_divider("qspi1_x4", "qspi1_en", CCM_CSCDR3, 8, 2);
+	clk[VF610_CLK_QSPI1_X2_DIV] = imx_clk_divider("qspi1_x2", "qspi1_x4", CCM_CSCDR3, 10, 1);
+	clk[VF610_CLK_QSPI1_X1_DIV] = imx_clk_divider("qspi1_x1", "qspi1_x2", CCM_CSCDR3, 11, 1);
+	clk[VF610_CLK_QSPI1] = imx_clk_gate2("qspi1", "qspi1_x1", CCM_CCGR8, CCM_CCGRx_CGn(4));
+
+	clk[VF610_CLK_ENET_50M] = imx_clk_fixed_factor("enet_50m", "pll5_enet", 1, 10);
+	clk[VF610_CLK_ENET_25M] = imx_clk_fixed_factor("enet_25m", "pll5_enet", 1, 20);
+	clk[VF610_CLK_ENET_SEL] = imx_clk_mux("enet_sel", CCM_CSCMR2, 4, 2, rmii_sels, 4);
+	clk[VF610_CLK_ENET_TS_SEL] = imx_clk_mux("enet_ts_sel", CCM_CSCMR2, 0, 3, enet_ts_sels, 7);
+	clk[VF610_CLK_ENET] = imx_clk_gate("enet", "enet_sel", CCM_CSCDR1, 24);
+	clk[VF610_CLK_ENET_TS] = imx_clk_gate("enet_ts", "enet_ts_sel", CCM_CSCDR1, 23);
+	clk[VF610_CLK_ENET0] = imx_clk_gate2("enet0", "ipg_bus", CCM_CCGR9, CCM_CCGRx_CGn(0));
+	clk[VF610_CLK_ENET1] = imx_clk_gate2("enet1", "ipg_bus", CCM_CCGR9, CCM_CCGRx_CGn(1));
+
+	clk[VF610_CLK_PIT] = imx_clk_gate2("pit", "ipg_bus", CCM_CCGR1, CCM_CCGRx_CGn(7));
+
+	clk[VF610_CLK_UART0] = imx_clk_gate2("uart0", "ipg_bus", CCM_CCGR0, CCM_CCGRx_CGn(7));
+	clk[VF610_CLK_UART1] = imx_clk_gate2("uart1", "ipg_bus", CCM_CCGR0, CCM_CCGRx_CGn(8));
+	clk[VF610_CLK_UART2] = imx_clk_gate2("uart2", "ipg_bus", CCM_CCGR0, CCM_CCGRx_CGn(9));
+	clk[VF610_CLK_UART3] = imx_clk_gate2("uart3", "ipg_bus", CCM_CCGR0, CCM_CCGRx_CGn(10));
+	clk[VF610_CLK_UART4] = imx_clk_gate2("uart4", "ipg_bus", CCM_CCGR6, CCM_CCGRx_CGn(9));
+	clk[VF610_CLK_UART5] = imx_clk_gate2("uart5", "ipg_bus", CCM_CCGR6, CCM_CCGRx_CGn(10));
+
+	clk[VF610_CLK_I2C0] = imx_clk_gate2("i2c0", "ipg_bus", CCM_CCGR4, CCM_CCGRx_CGn(6));
+	clk[VF610_CLK_I2C1] = imx_clk_gate2("i2c1", "ipg_bus", CCM_CCGR4, CCM_CCGRx_CGn(7));
+	clk[VF610_CLK_I2C2] = imx_clk_gate2("i2c2", "ipg_bus", CCM_CCGR10, CCM_CCGRx_CGn(6));
+	clk[VF610_CLK_I2C3] = imx_clk_gate2("i2c3", "ipg_bus", CCM_CCGR10, CCM_CCGRx_CGn(7));
+
+	clk[VF610_CLK_DSPI0] = imx_clk_gate2("dspi0", "ipg_bus", CCM_CCGR0, CCM_CCGRx_CGn(12));
+	clk[VF610_CLK_DSPI1] = imx_clk_gate2("dspi1", "ipg_bus", CCM_CCGR0, CCM_CCGRx_CGn(13));
+	clk[VF610_CLK_DSPI2] = imx_clk_gate2("dspi2", "ipg_bus", CCM_CCGR6, CCM_CCGRx_CGn(12));
+	clk[VF610_CLK_DSPI3] = imx_clk_gate2("dspi3", "ipg_bus", CCM_CCGR6, CCM_CCGRx_CGn(13));
+
+	clk[VF610_CLK_WDT] = imx_clk_gate2("wdt", "ipg_bus", CCM_CCGR1, CCM_CCGRx_CGn(14));
+
+	clk[VF610_CLK_ESDHC0_SEL] = imx_clk_mux("esdhc0_sel", CCM_CSCMR1, 16, 2, esdhc_sels, 4);
+	clk[VF610_CLK_ESDHC0_EN] = imx_clk_gate("esdhc0_en", "esdhc0_sel", CCM_CSCDR2, 28);
+	clk[VF610_CLK_ESDHC0_DIV] = imx_clk_divider("esdhc0_div", "esdhc0_en", CCM_CSCDR2, 16, 4);
+	clk[VF610_CLK_ESDHC0] = imx_clk_gate2("eshc0", "esdhc0_div", CCM_CCGR7, CCM_CCGRx_CGn(1));
+
+	clk[VF610_CLK_ESDHC1_SEL] = imx_clk_mux("esdhc1_sel", CCM_CSCMR1, 18, 2, esdhc_sels, 4);
+	clk[VF610_CLK_ESDHC1_EN] = imx_clk_gate("esdhc1_en", "esdhc1_sel", CCM_CSCDR2, 29);
+	clk[VF610_CLK_ESDHC1_DIV] = imx_clk_divider("esdhc1_div", "esdhc1_en", CCM_CSCDR2, 20, 4);
+	clk[VF610_CLK_ESDHC1] = imx_clk_gate2("eshc1", "esdhc1_div", CCM_CCGR7, CCM_CCGRx_CGn(2));
+
+	/*
+	 * ftm_ext_clk and ftm_fix_clk are FTM timer counter's
+	 * selectable clock sources, both use a common enable bit
+	 * in CCM_CSCDR1, selecting "dummy" clock as parent of
+	 * "ftm0_ext_fix" make it serve only for enable/disable.
+	 */
+	clk[VF610_CLK_FTM0_EXT_SEL] = imx_clk_mux("ftm0_ext_sel", CCM_CSCMR2, 6, 2, ftm_ext_sels, 4);
+	clk[VF610_CLK_FTM0_FIX_SEL] = imx_clk_mux("ftm0_fix_sel", CCM_CSCMR2, 14, 1, ftm_fix_sels, 2);
+	clk[VF610_CLK_FTM0_EXT_FIX_EN] = imx_clk_gate("ftm0_ext_fix_en", "dummy", CCM_CSCDR1, 25);
+	clk[VF610_CLK_FTM1_EXT_SEL] = imx_clk_mux("ftm1_ext_sel", CCM_CSCMR2, 8, 2, ftm_ext_sels, 4);
+	clk[VF610_CLK_FTM1_FIX_SEL] = imx_clk_mux("ftm1_fix_sel", CCM_CSCMR2, 15, 1, ftm_fix_sels, 2);
+	clk[VF610_CLK_FTM1_EXT_FIX_EN] = imx_clk_gate("ftm1_ext_fix_en", "dummy", CCM_CSCDR1, 26);
+	clk[VF610_CLK_FTM2_EXT_SEL] = imx_clk_mux("ftm2_ext_sel", CCM_CSCMR2, 10, 2, ftm_ext_sels, 4);
+	clk[VF610_CLK_FTM2_FIX_SEL] = imx_clk_mux("ftm2_fix_sel", CCM_CSCMR2, 16, 1, ftm_fix_sels, 2);
+	clk[VF610_CLK_FTM2_EXT_FIX_EN] = imx_clk_gate("ftm2_ext_fix_en", "dummy", CCM_CSCDR1, 27);
+	clk[VF610_CLK_FTM3_EXT_SEL] = imx_clk_mux("ftm3_ext_sel", CCM_CSCMR2, 12, 2, ftm_ext_sels, 4);
+	clk[VF610_CLK_FTM3_FIX_SEL] = imx_clk_mux("ftm3_fix_sel", CCM_CSCMR2, 17, 1, ftm_fix_sels, 2);
+	clk[VF610_CLK_FTM3_EXT_FIX_EN] = imx_clk_gate("ftm3_ext_fix_en", "dummy", CCM_CSCDR1, 28);
+
+	/* ftm(n)_clk are FTM module operation clock */
+	clk[VF610_CLK_FTM0] = imx_clk_gate2("ftm0", "ipg_bus", CCM_CCGR1, CCM_CCGRx_CGn(8));
+	clk[VF610_CLK_FTM1] = imx_clk_gate2("ftm1", "ipg_bus", CCM_CCGR1, CCM_CCGRx_CGn(9));
+	clk[VF610_CLK_FTM2] = imx_clk_gate2("ftm2", "ipg_bus", CCM_CCGR7, CCM_CCGRx_CGn(8));
+	clk[VF610_CLK_FTM3] = imx_clk_gate2("ftm3", "ipg_bus", CCM_CCGR7, CCM_CCGRx_CGn(9));
+
+	clk[VF610_CLK_DCU0_SEL] = imx_clk_mux("dcu0_sel", CCM_CSCMR1, 28, 1, dcu_sels, 2);
+	clk[VF610_CLK_DCU0_EN] = imx_clk_gate("dcu0_en", "dcu0_sel", CCM_CSCDR3, 19);
+	clk[VF610_CLK_DCU0_DIV] = imx_clk_divider("dcu0_div", "dcu0_en", CCM_CSCDR3, 16, 3);
+	clk[VF610_CLK_DCU0] = imx_clk_gate2("dcu0", "dcu0_div", CCM_CCGR3, CCM_CCGRx_CGn(8));
+	clk[VF610_CLK_DCU1_SEL] = imx_clk_mux("dcu1_sel", CCM_CSCMR1, 29, 1, dcu_sels, 2);
+	clk[VF610_CLK_DCU1_EN] = imx_clk_gate("dcu1_en", "dcu1_sel", CCM_CSCDR3, 23);
+	clk[VF610_CLK_DCU1_DIV] = imx_clk_divider("dcu1_div", "dcu1_en", CCM_CSCDR3, 20, 3);
+	clk[VF610_CLK_DCU1] = imx_clk_gate2("dcu1", "dcu1_div", CCM_CCGR9, CCM_CCGRx_CGn(8));
+
+	clk[VF610_CLK_ESAI_SEL] = imx_clk_mux("esai_sel", CCM_CSCMR1, 20, 2, esai_sels, 4);
+	clk[VF610_CLK_ESAI_EN] = imx_clk_gate("esai_en", "esai_sel", CCM_CSCDR2, 30);
+	clk[VF610_CLK_ESAI_DIV] = imx_clk_divider("esai_div", "esai_en", CCM_CSCDR2, 24, 4);
+	clk[VF610_CLK_ESAI] = imx_clk_gate2("esai", "esai_div", CCM_CCGR4, CCM_CCGRx_CGn(2));
+
+	clk[VF610_CLK_SAI0_SEL] = imx_clk_mux("sai0_sel", CCM_CSCMR1, 0, 2, sai_sels, 4);
+	clk[VF610_CLK_SAI0_EN] = imx_clk_gate("sai0_en", "sai0_sel", CCM_CSCDR1, 16);
+	clk[VF610_CLK_SAI0_DIV] = imx_clk_divider("sai0_div", "sai0_en", CCM_CSCDR1, 0, 4);
+	clk[VF610_CLK_SAI0] = imx_clk_gate2("sai0", "sai0_div", CCM_CCGR0, CCM_CCGRx_CGn(15));
+
+	clk[VF610_CLK_SAI1_SEL] = imx_clk_mux("sai1_sel", CCM_CSCMR1, 2, 2, sai_sels, 4);
+	clk[VF610_CLK_SAI1_EN] = imx_clk_gate("sai1_en", "sai1_sel", CCM_CSCDR1, 17);
+	clk[VF610_CLK_SAI1_DIV] = imx_clk_divider("sai1_div", "sai1_en", CCM_CSCDR1, 4, 4);
+	clk[VF610_CLK_SAI1] = imx_clk_gate2("sai1", "sai1_div", CCM_CCGR1, CCM_CCGRx_CGn(0));
+
+	clk[VF610_CLK_SAI2_SEL] = imx_clk_mux("sai2_sel", CCM_CSCMR1, 4, 2, sai_sels, 4);
+	clk[VF610_CLK_SAI2_EN] = imx_clk_gate("sai2_en", "sai2_sel", CCM_CSCDR1, 18);
+	clk[VF610_CLK_SAI2_DIV] = imx_clk_divider("sai2_div", "sai2_en", CCM_CSCDR1, 8, 4);
+	clk[VF610_CLK_SAI2] = imx_clk_gate2("sai2", "sai2_div", CCM_CCGR1, CCM_CCGRx_CGn(1));
+
+	clk[VF610_CLK_SAI3_SEL] = imx_clk_mux("sai3_sel", CCM_CSCMR1, 6, 2, sai_sels, 4);
+	clk[VF610_CLK_SAI3_EN] = imx_clk_gate("sai3_en", "sai3_sel", CCM_CSCDR1, 19);
+	clk[VF610_CLK_SAI3_DIV] = imx_clk_divider("sai3_div", "sai3_en", CCM_CSCDR1, 12, 4);
+	clk[VF610_CLK_SAI3] = imx_clk_gate2("sai3", "sai3_div", CCM_CCGR1, CCM_CCGRx_CGn(2));
+
+	clk[VF610_CLK_NFC_SEL] = imx_clk_mux("nfc_sel", CCM_CSCMR1, 12, 2, nfc_sels, 4);
+	clk[VF610_CLK_NFC_EN] = imx_clk_gate("nfc_en", "nfc_sel", CCM_CSCDR2, 9);
+	clk[VF610_CLK_NFC_PRE_DIV] = imx_clk_divider("nfc_pre_div", "nfc_en", CCM_CSCDR3, 13, 3);
+	clk[VF610_CLK_NFC_FRAC_DIV] = imx_clk_divider("nfc_frac_div", "nfc_pre_div", CCM_CSCDR2, 4, 4);
+	clk[VF610_CLK_NFC] = imx_clk_gate2("nfc", "nfc_frac_div", CCM_CCGR10, CCM_CCGRx_CGn(0));
+
+	clk[VF610_CLK_GPU_SEL] = imx_clk_mux("gpu_sel", CCM_CSCMR1, 14, 1, gpu_sels, 2);
+	clk[VF610_CLK_GPU_EN] = imx_clk_gate("gpu_en", "gpu_sel", CCM_CSCDR2, 10);
+	clk[VF610_CLK_GPU2D] = imx_clk_gate2("gpu", "gpu_en", CCM_CCGR8, CCM_CCGRx_CGn(15));
+
+	clk[VF610_CLK_VADC_SEL] = imx_clk_mux("vadc_sel", CCM_CSCMR1, 8, 2, vadc_sels, 3);
+	clk[VF610_CLK_VADC_EN] = imx_clk_gate("vadc_en", "vadc_sel", CCM_CSCDR1, 22);
+	clk[VF610_CLK_VADC_DIV] = imx_clk_divider("vadc_div", "vadc_en", CCM_CSCDR1, 20, 2);
+	clk[VF610_CLK_VADC_DIV_HALF] = imx_clk_fixed_factor("vadc_div_half", "vadc_div", 1, 2);
+	clk[VF610_CLK_VADC] = imx_clk_gate2("vadc", "vadc_div", CCM_CCGR8, CCM_CCGRx_CGn(7));
+
+	clk[VF610_CLK_ADC0] = imx_clk_gate2("adc0", "ipg_bus", CCM_CCGR1, CCM_CCGRx_CGn(11));
+	clk[VF610_CLK_ADC1] = imx_clk_gate2("adc1", "ipg_bus", CCM_CCGR7, CCM_CCGRx_CGn(11));
+	clk[VF610_CLK_DAC0] = imx_clk_gate2("dac0", "ipg_bus", CCM_CCGR8, CCM_CCGRx_CGn(12));
+	clk[VF610_CLK_DAC1] = imx_clk_gate2("dac1", "ipg_bus", CCM_CCGR8, CCM_CCGRx_CGn(13));
+
+	clk[VF610_CLK_ASRC] = imx_clk_gate2("asrc", "ipg_bus", CCM_CCGR4, CCM_CCGRx_CGn(1));
+
+	clk[VF610_CLK_FLEXCAN0_EN] = imx_clk_gate("flexcan0_en", "ipg_bus", CCM_CSCDR2, 11);
+	clk[VF610_CLK_FLEXCAN0] = imx_clk_gate2("flexcan0", "flexcan0_en", CCM_CCGR0, CCM_CCGRx_CGn(0));
+	clk[VF610_CLK_FLEXCAN1_EN] = imx_clk_gate("flexcan1_en", "ipg_bus", CCM_CSCDR2, 12);
+	clk[VF610_CLK_FLEXCAN1] = imx_clk_gate2("flexcan1", "flexcan1_en", CCM_CCGR9, CCM_CCGRx_CGn(4));
+
+	clk[VF610_CLK_DMAMUX0] = imx_clk_gate2("dmamux0", "platform_bus", CCM_CCGR0, CCM_CCGRx_CGn(4));
+	clk[VF610_CLK_DMAMUX1] = imx_clk_gate2("dmamux1", "platform_bus", CCM_CCGR0, CCM_CCGRx_CGn(5));
+	clk[VF610_CLK_DMAMUX2] = imx_clk_gate2("dmamux2", "platform_bus", CCM_CCGR6, CCM_CCGRx_CGn(1));
+	clk[VF610_CLK_DMAMUX3] = imx_clk_gate2("dmamux3", "platform_bus", CCM_CCGR6, CCM_CCGRx_CGn(2));
+
+	clk[VF610_CLK_SNVS] = imx_clk_gate2("snvs-rtc", "ipg_bus", CCM_CCGR6, CCM_CCGRx_CGn(7));
+	clk[VF610_CLK_DAP] = imx_clk_gate("dap", "platform_bus", CCM_CCSR, 24);
+
+	imx_check_clocks(clk, ARRAY_SIZE(clk));
+
+	clk_set_parent(clk[VF610_CLK_QSPI0_SEL], clk[VF610_CLK_PLL1_PFD4]);
+	clk_set_rate(clk[VF610_CLK_QSPI0_X4_DIV], clk_get_rate(clk[VF610_CLK_QSPI0_SEL]) / 2);
+	clk_set_rate(clk[VF610_CLK_QSPI0_X2_DIV], clk_get_rate(clk[VF610_CLK_QSPI0_X4_DIV]) / 2);
+	clk_set_rate(clk[VF610_CLK_QSPI0_X1_DIV], clk_get_rate(clk[VF610_CLK_QSPI0_X2_DIV]) / 2);
+
+	clk_set_parent(clk[VF610_CLK_QSPI1_SEL], clk[VF610_CLK_PLL1_PFD4]);
+	clk_set_rate(clk[VF610_CLK_QSPI1_X4_DIV], clk_get_rate(clk[VF610_CLK_QSPI1_SEL]) / 2);
+	clk_set_rate(clk[VF610_CLK_QSPI1_X2_DIV], clk_get_rate(clk[VF610_CLK_QSPI1_X4_DIV]) / 2);
+	clk_set_rate(clk[VF610_CLK_QSPI1_X1_DIV], clk_get_rate(clk[VF610_CLK_QSPI1_X2_DIV]) / 2);
+
+	clk_set_parent(clk[VF610_CLK_SAI0_SEL], clk[VF610_CLK_AUDIO_EXT]);
+	clk_set_parent(clk[VF610_CLK_SAI1_SEL], clk[VF610_CLK_AUDIO_EXT]);
+	clk_set_parent(clk[VF610_CLK_SAI2_SEL], clk[VF610_CLK_AUDIO_EXT]);
+	clk_set_parent(clk[VF610_CLK_SAI3_SEL], clk[VF610_CLK_AUDIO_EXT]);
+
+	for (i = 0; i < ARRAY_SIZE(clks_init_on); i++)
+		clk_prepare_enable(clk[clks_init_on[i]]);
+
+	/* Add the clocks to provider list */
+	clk_data.clks = clk;
+	clk_data.clk_num = ARRAY_SIZE(clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+}
+CLK_OF_DECLARE(vf610, "fsl,vf610-ccm", vf610_clocks_init);
diff --git a/drivers/clk/imx/clk.c b/drivers/clk/imx/clk.c
new file mode 100644
index 000000000000..df12b5307175
--- /dev/null
+++ b/drivers/clk/imx/clk.c
@@ -0,0 +1,75 @@
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include "clk.h"
+
+DEFINE_SPINLOCK(imx_ccm_lock);
+
+void __init imx_check_clocks(struct clk *clks[], unsigned int count)
+{
+	unsigned i;
+
+	for (i = 0; i < count; i++)
+		if (IS_ERR(clks[i]))
+			pr_err("i.MX clk %u: register failed with %ld\n",
+			       i, PTR_ERR(clks[i]));
+}
+
+static struct clk * __init imx_obtain_fixed_clock_from_dt(const char *name)
+{
+	struct of_phandle_args phandle;
+	struct clk *clk = ERR_PTR(-ENODEV);
+	char *path;
+
+	path = kasprintf(GFP_KERNEL, "/clocks/%s", name);
+	if (!path)
+		return ERR_PTR(-ENOMEM);
+
+	phandle.np = of_find_node_by_path(path);
+	kfree(path);
+
+	if (phandle.np) {
+		clk = of_clk_get_from_provider(&phandle);
+		of_node_put(phandle.np);
+	}
+	return clk;
+}
+
+struct clk * __init imx_obtain_fixed_clock(
+			const char *name, unsigned long rate)
+{
+	struct clk *clk;
+
+	clk = imx_obtain_fixed_clock_from_dt(name);
+	if (IS_ERR(clk))
+		clk = imx_clk_fixed(name, rate);
+	return clk;
+}
+
+/*
+ * This fixups the register CCM_CSCMR1 write value.
+ * The write/read/divider values of the aclk_podf field
+ * of that register have the relationship described by
+ * the following table:
+ *
+ * write value       read value        divider
+ * 3b'000            3b'110            7
+ * 3b'001            3b'111            8
+ * 3b'010            3b'100            5
+ * 3b'011            3b'101            6
+ * 3b'100            3b'010            3
+ * 3b'101            3b'011            4
+ * 3b'110            3b'000            1
+ * 3b'111            3b'001            2(default)
+ *
+ * That's why we do the xor operation below.
+ */
+#define CSCMR1_FIXUP	0x00600000
+
+void imx_cscmr1_fixup(u32 *val)
+{
+	*val ^= CSCMR1_FIXUP;
+	return;
+}
diff --git a/drivers/clk/imx/clk.h b/drivers/clk/imx/clk.h
new file mode 100644
index 000000000000..1049b0c7d818
--- /dev/null
+++ b/drivers/clk/imx/clk.h
@@ -0,0 +1,149 @@
+#ifndef __MACH_IMX_CLK_H
+#define __MACH_IMX_CLK_H
+
+#include <linux/spinlock.h>
+#include <linux/clk-provider.h>
+
+extern spinlock_t imx_ccm_lock;
+
+void imx_check_clocks(struct clk *clks[], unsigned int count);
+
+extern void imx_cscmr1_fixup(u32 *val);
+
+enum imx_pllv1_type {
+	IMX_PLLV1_IMX1,
+	IMX_PLLV1_IMX21,
+	IMX_PLLV1_IMX25,
+	IMX_PLLV1_IMX27,
+	IMX_PLLV1_IMX31,
+	IMX_PLLV1_IMX35,
+};
+
+struct clk *imx_clk_pllv1(enum imx_pllv1_type type, const char *name,
+		const char *parent, void __iomem *base);
+
+struct clk *imx_clk_pllv2(const char *name, const char *parent,
+		void __iomem *base);
+
+enum imx_pllv3_type {
+	IMX_PLLV3_GENERIC,
+	IMX_PLLV3_SYS,
+	IMX_PLLV3_USB,
+	IMX_PLLV3_USB_VF610,
+	IMX_PLLV3_AV,
+	IMX_PLLV3_ENET,
+	IMX_PLLV3_ENET_IMX7,
+};
+
+struct clk *imx_clk_pllv3(enum imx_pllv3_type type, const char *name,
+		const char *parent_name, void __iomem *base, u32 div_mask);
+
+struct clk *clk_register_gate2(struct device *dev, const char *name,
+		const char *parent_name, unsigned long flags,
+		void __iomem *reg, u8 bit_idx,
+		u8 clk_gate_flags, spinlock_t *lock,
+		unsigned int *share_count);
+
+struct clk * imx_obtain_fixed_clock(
+			const char *name, unsigned long rate);
+
+struct clk *imx_clk_gate_exclusive(const char *name, const char *parent,
+	 void __iomem *reg, u8 shift, u32 exclusive_mask);
+
+static inline struct clk *imx_clk_gate2(const char *name, const char *parent,
+		void __iomem *reg, u8 shift)
+{
+	return clk_register_gate2(NULL, name, parent, CLK_SET_RATE_PARENT, reg,
+			shift, 0, &imx_ccm_lock, NULL);
+}
+
+static inline struct clk *imx_clk_gate2_shared(const char *name,
+		const char *parent, void __iomem *reg, u8 shift,
+		unsigned int *share_count)
+{
+	return clk_register_gate2(NULL, name, parent, CLK_SET_RATE_PARENT, reg,
+			shift, 0, &imx_ccm_lock, share_count);
+}
+
+struct clk *imx_clk_pfd(const char *name, const char *parent_name,
+		void __iomem *reg, u8 idx);
+
+struct clk *imx_clk_busy_divider(const char *name, const char *parent_name,
+				 void __iomem *reg, u8 shift, u8 width,
+				 void __iomem *busy_reg, u8 busy_shift);
+
+struct clk *imx_clk_busy_mux(const char *name, void __iomem *reg, u8 shift,
+			     u8 width, void __iomem *busy_reg, u8 busy_shift,
+			     const char **parent_names, int num_parents);
+
+struct clk *imx_clk_fixup_divider(const char *name, const char *parent,
+				  void __iomem *reg, u8 shift, u8 width,
+				  void (*fixup)(u32 *val));
+
+struct clk *imx_clk_fixup_mux(const char *name, void __iomem *reg,
+			      u8 shift, u8 width, const char **parents,
+			      int num_parents, void (*fixup)(u32 *val));
+
+static inline struct clk *imx_clk_fixed(const char *name, int rate)
+{
+	return clk_register_fixed_rate(NULL, name, NULL, CLK_IS_ROOT, rate);
+}
+
+static inline struct clk *imx_clk_divider(const char *name, const char *parent,
+		void __iomem *reg, u8 shift, u8 width)
+{
+	return clk_register_divider(NULL, name, parent, CLK_SET_RATE_PARENT,
+			reg, shift, width, 0, &imx_ccm_lock);
+}
+
+static inline struct clk *imx_clk_divider_flags(const char *name,
+		const char *parent, void __iomem *reg, u8 shift, u8 width,
+		unsigned long flags)
+{
+	return clk_register_divider(NULL, name, parent, flags,
+			reg, shift, width, 0, &imx_ccm_lock);
+}
+
+static inline struct clk *imx_clk_gate(const char *name, const char *parent,
+		void __iomem *reg, u8 shift)
+{
+	return clk_register_gate(NULL, name, parent, CLK_SET_RATE_PARENT, reg,
+			shift, 0, &imx_ccm_lock);
+}
+
+static inline struct clk *imx_clk_gate_dis(const char *name, const char *parent,
+		void __iomem *reg, u8 shift)
+{
+	return clk_register_gate(NULL, name, parent, CLK_SET_RATE_PARENT, reg,
+			shift, CLK_GATE_SET_TO_DISABLE, &imx_ccm_lock);
+}
+
+static inline struct clk *imx_clk_mux(const char *name, void __iomem *reg,
+		u8 shift, u8 width, const char **parents, int num_parents)
+{
+	return clk_register_mux(NULL, name, parents, num_parents,
+			CLK_SET_RATE_NO_REPARENT, reg, shift,
+			width, 0, &imx_ccm_lock);
+}
+
+static inline struct clk *imx_clk_mux_flags(const char *name,
+		void __iomem *reg, u8 shift, u8 width, const char **parents,
+		int num_parents, unsigned long flags)
+{
+	return clk_register_mux(NULL, name, parents, num_parents,
+			flags | CLK_SET_RATE_NO_REPARENT, reg, shift, width, 0,
+			&imx_ccm_lock);
+}
+
+static inline struct clk *imx_clk_fixed_factor(const char *name,
+		const char *parent, unsigned int mult, unsigned int div)
+{
+	return clk_register_fixed_factor(NULL, name, parent,
+			CLK_SET_RATE_PARENT, mult, div);
+}
+
+struct clk *imx_clk_cpu(const char *name, const char *parent_name,
+		struct clk *div, struct clk *mux, struct clk *pll,
+		struct clk *step);
+
+#endif
diff --git a/drivers/clk/ingenic/Makefile b/drivers/clk/ingenic/Makefile
new file mode 100644
index 000000000000..cd47b0664c2b
--- /dev/null
+++ b/drivers/clk/ingenic/Makefile
@@ -0,0 +1,3 @@
+obj-y				+= cgu.o
+obj-$(CONFIG_MACH_JZ4740)	+= jz4740-cgu.o
+obj-$(CONFIG_MACH_JZ4780)	+= jz4780-cgu.o
diff --git a/drivers/clk/ingenic/cgu.c b/drivers/clk/ingenic/cgu.c
new file mode 100644
index 000000000000..b936cdd1a13c
--- /dev/null
+++ b/drivers/clk/ingenic/cgu.c
@@ -0,0 +1,711 @@
+/*
+ * Ingenic SoC CGU driver
+ *
+ * Copyright (c) 2013-2015 Imagination Technologies
+ * Author: Paul Burton <paul.burton@imgtec.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/bitops.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include "cgu.h"
+
+#define MHZ (1000 * 1000)
+
+/**
+ * ingenic_cgu_gate_get() - get the value of clock gate register bit
+ * @cgu: reference to the CGU whose registers should be read
+ * @info: info struct describing the gate bit
+ *
+ * Retrieves the state of the clock gate bit described by info. The
+ * caller must hold cgu->lock.
+ *
+ * Return: true if the gate bit is set, else false.
+ */
+static inline bool
+ingenic_cgu_gate_get(struct ingenic_cgu *cgu,
+		     const struct ingenic_cgu_gate_info *info)
+{
+	return readl(cgu->base + info->reg) & BIT(info->bit);
+}
+
+/**
+ * ingenic_cgu_gate_set() - set the value of clock gate register bit
+ * @cgu: reference to the CGU whose registers should be modified
+ * @info: info struct describing the gate bit
+ * @val: non-zero to gate a clock, otherwise zero
+ *
+ * Sets the given gate bit in order to gate or ungate a clock.
+ *
+ * The caller must hold cgu->lock.
+ */
+static inline void
+ingenic_cgu_gate_set(struct ingenic_cgu *cgu,
+		     const struct ingenic_cgu_gate_info *info, bool val)
+{
+	u32 clkgr = readl(cgu->base + info->reg);
+
+	if (val)
+		clkgr |= BIT(info->bit);
+	else
+		clkgr &= ~BIT(info->bit);
+
+	writel(clkgr, cgu->base + info->reg);
+}
+
+/*
+ * PLL operations
+ */
+
+static unsigned long
+ingenic_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	const struct ingenic_cgu_pll_info *pll_info;
+	unsigned m, n, od_enc, od;
+	bool bypass, enable;
+	unsigned long flags;
+	u32 ctl;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+	BUG_ON(clk_info->type != CGU_CLK_PLL);
+	pll_info = &clk_info->pll;
+
+	spin_lock_irqsave(&cgu->lock, flags);
+	ctl = readl(cgu->base + pll_info->reg);
+	spin_unlock_irqrestore(&cgu->lock, flags);
+
+	m = (ctl >> pll_info->m_shift) & GENMASK(pll_info->m_bits - 1, 0);
+	m += pll_info->m_offset;
+	n = (ctl >> pll_info->n_shift) & GENMASK(pll_info->n_bits - 1, 0);
+	n += pll_info->n_offset;
+	od_enc = ctl >> pll_info->od_shift;
+	od_enc &= GENMASK(pll_info->od_bits - 1, 0);
+	bypass = !!(ctl & BIT(pll_info->bypass_bit));
+	enable = !!(ctl & BIT(pll_info->enable_bit));
+
+	if (bypass)
+		return parent_rate;
+
+	if (!enable)
+		return 0;
+
+	for (od = 0; od < pll_info->od_max; od++) {
+		if (pll_info->od_encoding[od] == od_enc)
+			break;
+	}
+	BUG_ON(od == pll_info->od_max);
+	od++;
+
+	return div_u64((u64)parent_rate * m, n * od);
+}
+
+static unsigned long
+ingenic_pll_calc(const struct ingenic_cgu_clk_info *clk_info,
+		 unsigned long rate, unsigned long parent_rate,
+		 unsigned *pm, unsigned *pn, unsigned *pod)
+{
+	const struct ingenic_cgu_pll_info *pll_info;
+	unsigned m, n, od;
+
+	pll_info = &clk_info->pll;
+	od = 1;
+
+	/*
+	 * The frequency after the input divider must be between 10 and 50 MHz.
+	 * The highest divider yields the best resolution.
+	 */
+	n = parent_rate / (10 * MHZ);
+	n = min_t(unsigned, n, 1 << clk_info->pll.n_bits);
+	n = max_t(unsigned, n, pll_info->n_offset);
+
+	m = (rate / MHZ) * od * n / (parent_rate / MHZ);
+	m = min_t(unsigned, m, 1 << clk_info->pll.m_bits);
+	m = max_t(unsigned, m, pll_info->m_offset);
+
+	if (pm)
+		*pm = m;
+	if (pn)
+		*pn = n;
+	if (pod)
+		*pod = od;
+
+	return div_u64((u64)parent_rate * m, n * od);
+}
+
+static long
+ingenic_pll_round_rate(struct clk_hw *hw, unsigned long req_rate,
+		       unsigned long *prate)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+	BUG_ON(clk_info->type != CGU_CLK_PLL);
+
+	return ingenic_pll_calc(clk_info, req_rate, *prate, NULL, NULL, NULL);
+}
+
+static int
+ingenic_pll_set_rate(struct clk_hw *hw, unsigned long req_rate,
+		     unsigned long parent_rate)
+{
+	const unsigned timeout = 100;
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	const struct ingenic_cgu_pll_info *pll_info;
+	unsigned long rate, flags;
+	unsigned m, n, od, i;
+	u32 ctl;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+	BUG_ON(clk_info->type != CGU_CLK_PLL);
+	pll_info = &clk_info->pll;
+
+	rate = ingenic_pll_calc(clk_info, req_rate, parent_rate,
+			       &m, &n, &od);
+	if (rate != req_rate)
+		pr_info("ingenic-cgu: request '%s' rate %luHz, actual %luHz\n",
+			clk_info->name, req_rate, rate);
+
+	spin_lock_irqsave(&cgu->lock, flags);
+	ctl = readl(cgu->base + pll_info->reg);
+
+	ctl &= ~(GENMASK(pll_info->m_bits - 1, 0) << pll_info->m_shift);
+	ctl |= (m - pll_info->m_offset) << pll_info->m_shift;
+
+	ctl &= ~(GENMASK(pll_info->n_bits - 1, 0) << pll_info->n_shift);
+	ctl |= (n - pll_info->n_offset) << pll_info->n_shift;
+
+	ctl &= ~(GENMASK(pll_info->od_bits - 1, 0) << pll_info->od_shift);
+	ctl |= pll_info->od_encoding[od - 1] << pll_info->od_shift;
+
+	ctl &= ~BIT(pll_info->bypass_bit);
+	ctl |= BIT(pll_info->enable_bit);
+
+	writel(ctl, cgu->base + pll_info->reg);
+
+	/* wait for the PLL to stabilise */
+	for (i = 0; i < timeout; i++) {
+		ctl = readl(cgu->base + pll_info->reg);
+		if (ctl & BIT(pll_info->stable_bit))
+			break;
+		mdelay(1);
+	}
+
+	spin_unlock_irqrestore(&cgu->lock, flags);
+
+	if (i == timeout)
+		return -EBUSY;
+
+	return 0;
+}
+
+static const struct clk_ops ingenic_pll_ops = {
+	.recalc_rate = ingenic_pll_recalc_rate,
+	.round_rate = ingenic_pll_round_rate,
+	.set_rate = ingenic_pll_set_rate,
+};
+
+/*
+ * Operations for all non-PLL clocks
+ */
+
+static u8 ingenic_clk_get_parent(struct clk_hw *hw)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	u32 reg;
+	u8 i, hw_idx, idx = 0;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+
+	if (clk_info->type & CGU_CLK_MUX) {
+		reg = readl(cgu->base + clk_info->mux.reg);
+		hw_idx = (reg >> clk_info->mux.shift) &
+			 GENMASK(clk_info->mux.bits - 1, 0);
+
+		/*
+		 * Convert the hardware index to the parent index by skipping
+		 * over any -1's in the parents array.
+		 */
+		for (i = 0; i < hw_idx; i++) {
+			if (clk_info->parents[i] != -1)
+				idx++;
+		}
+	}
+
+	return idx;
+}
+
+static int ingenic_clk_set_parent(struct clk_hw *hw, u8 idx)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	unsigned long flags;
+	u8 curr_idx, hw_idx, num_poss;
+	u32 reg, mask;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+
+	if (clk_info->type & CGU_CLK_MUX) {
+		/*
+		 * Convert the parent index to the hardware index by adding
+		 * 1 for any -1 in the parents array preceding the given
+		 * index. That is, we want the index of idx'th entry in
+		 * clk_info->parents which does not equal -1.
+		 */
+		hw_idx = curr_idx = 0;
+		num_poss = 1 << clk_info->mux.bits;
+		for (; hw_idx < num_poss; hw_idx++) {
+			if (clk_info->parents[hw_idx] == -1)
+				continue;
+			if (curr_idx == idx)
+				break;
+			curr_idx++;
+		}
+
+		/* idx should always be a valid parent */
+		BUG_ON(curr_idx != idx);
+
+		mask = GENMASK(clk_info->mux.bits - 1, 0);
+		mask <<= clk_info->mux.shift;
+
+		spin_lock_irqsave(&cgu->lock, flags);
+
+		/* write the register */
+		reg = readl(cgu->base + clk_info->mux.reg);
+		reg &= ~mask;
+		reg |= hw_idx << clk_info->mux.shift;
+		writel(reg, cgu->base + clk_info->mux.reg);
+
+		spin_unlock_irqrestore(&cgu->lock, flags);
+		return 0;
+	}
+
+	return idx ? -EINVAL : 0;
+}
+
+static unsigned long
+ingenic_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	unsigned long rate = parent_rate;
+	u32 div_reg, div;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+
+	if (clk_info->type & CGU_CLK_DIV) {
+		div_reg = readl(cgu->base + clk_info->div.reg);
+		div = (div_reg >> clk_info->div.shift) &
+		      GENMASK(clk_info->div.bits - 1, 0);
+		div += 1;
+
+		rate /= div;
+	}
+
+	return rate;
+}
+
+static unsigned
+ingenic_clk_calc_div(const struct ingenic_cgu_clk_info *clk_info,
+		     unsigned long parent_rate, unsigned long req_rate)
+{
+	unsigned div;
+
+	/* calculate the divide */
+	div = DIV_ROUND_UP(parent_rate, req_rate);
+
+	/* and impose hardware constraints */
+	div = min_t(unsigned, div, 1 << clk_info->div.bits);
+	div = max_t(unsigned, div, 1);
+
+	return div;
+}
+
+static long
+ingenic_clk_round_rate(struct clk_hw *hw, unsigned long req_rate,
+		       unsigned long *parent_rate)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	long rate = *parent_rate;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+
+	if (clk_info->type & CGU_CLK_DIV)
+		rate /= ingenic_clk_calc_div(clk_info, *parent_rate, req_rate);
+	else if (clk_info->type & CGU_CLK_FIXDIV)
+		rate /= clk_info->fixdiv.div;
+
+	return rate;
+}
+
+static int
+ingenic_clk_set_rate(struct clk_hw *hw, unsigned long req_rate,
+		     unsigned long parent_rate)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	const unsigned timeout = 100;
+	unsigned long rate, flags;
+	unsigned div, i;
+	u32 reg, mask;
+	int ret = 0;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+
+	if (clk_info->type & CGU_CLK_DIV) {
+		div = ingenic_clk_calc_div(clk_info, parent_rate, req_rate);
+		rate = parent_rate / div;
+
+		if (rate != req_rate)
+			return -EINVAL;
+
+		spin_lock_irqsave(&cgu->lock, flags);
+		reg = readl(cgu->base + clk_info->div.reg);
+
+		/* update the divide */
+		mask = GENMASK(clk_info->div.bits - 1, 0);
+		reg &= ~(mask << clk_info->div.shift);
+		reg |= (div - 1) << clk_info->div.shift;
+
+		/* clear the stop bit */
+		if (clk_info->div.stop_bit != -1)
+			reg &= ~BIT(clk_info->div.stop_bit);
+
+		/* set the change enable bit */
+		if (clk_info->div.ce_bit != -1)
+			reg |= BIT(clk_info->div.ce_bit);
+
+		/* update the hardware */
+		writel(reg, cgu->base + clk_info->div.reg);
+
+		/* wait for the change to take effect */
+		if (clk_info->div.busy_bit != -1) {
+			for (i = 0; i < timeout; i++) {
+				reg = readl(cgu->base + clk_info->div.reg);
+				if (!(reg & BIT(clk_info->div.busy_bit)))
+					break;
+				mdelay(1);
+			}
+			if (i == timeout)
+				ret = -EBUSY;
+		}
+
+		spin_unlock_irqrestore(&cgu->lock, flags);
+		return ret;
+	}
+
+	return -EINVAL;
+}
+
+static int ingenic_clk_enable(struct clk_hw *hw)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	unsigned long flags;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+
+	if (clk_info->type & CGU_CLK_GATE) {
+		/* ungate the clock */
+		spin_lock_irqsave(&cgu->lock, flags);
+		ingenic_cgu_gate_set(cgu, &clk_info->gate, false);
+		spin_unlock_irqrestore(&cgu->lock, flags);
+	}
+
+	return 0;
+}
+
+static void ingenic_clk_disable(struct clk_hw *hw)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	unsigned long flags;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+
+	if (clk_info->type & CGU_CLK_GATE) {
+		/* gate the clock */
+		spin_lock_irqsave(&cgu->lock, flags);
+		ingenic_cgu_gate_set(cgu, &clk_info->gate, true);
+		spin_unlock_irqrestore(&cgu->lock, flags);
+	}
+}
+
+static int ingenic_clk_is_enabled(struct clk_hw *hw)
+{
+	struct ingenic_clk *ingenic_clk = to_ingenic_clk(hw);
+	struct ingenic_cgu *cgu = ingenic_clk->cgu;
+	const struct ingenic_cgu_clk_info *clk_info;
+	unsigned long flags;
+	int enabled = 1;
+
+	clk_info = &cgu->clock_info[ingenic_clk->idx];
+
+	if (clk_info->type & CGU_CLK_GATE) {
+		spin_lock_irqsave(&cgu->lock, flags);
+		enabled = !ingenic_cgu_gate_get(cgu, &clk_info->gate);
+		spin_unlock_irqrestore(&cgu->lock, flags);
+	}
+
+	return enabled;
+}
+
+static const struct clk_ops ingenic_clk_ops = {
+	.get_parent = ingenic_clk_get_parent,
+	.set_parent = ingenic_clk_set_parent,
+
+	.recalc_rate = ingenic_clk_recalc_rate,
+	.round_rate = ingenic_clk_round_rate,
+	.set_rate = ingenic_clk_set_rate,
+
+	.enable = ingenic_clk_enable,
+	.disable = ingenic_clk_disable,
+	.is_enabled = ingenic_clk_is_enabled,
+};
+
+/*
+ * Setup functions.
+ */
+
+static int ingenic_register_clock(struct ingenic_cgu *cgu, unsigned idx)
+{
+	const struct ingenic_cgu_clk_info *clk_info = &cgu->clock_info[idx];
+	struct clk_init_data clk_init;
+	struct ingenic_clk *ingenic_clk = NULL;
+	struct clk *clk, *parent;
+	const char *parent_names[4];
+	unsigned caps, i, num_possible;
+	int err = -EINVAL;
+
+	BUILD_BUG_ON(ARRAY_SIZE(clk_info->parents) > ARRAY_SIZE(parent_names));
+
+	if (clk_info->type == CGU_CLK_EXT) {
+		clk = of_clk_get_by_name(cgu->np, clk_info->name);
+		if (IS_ERR(clk)) {
+			pr_err("%s: no external clock '%s' provided\n",
+			       __func__, clk_info->name);
+			err = -ENODEV;
+			goto out;
+		}
+		err = clk_register_clkdev(clk, clk_info->name, NULL);
+		if (err) {
+			clk_put(clk);
+			goto out;
+		}
+		cgu->clocks.clks[idx] = clk;
+		return 0;
+	}
+
+	if (!clk_info->type) {
+		pr_err("%s: no clock type specified for '%s'\n", __func__,
+		       clk_info->name);
+		goto out;
+	}
+
+	ingenic_clk = kzalloc(sizeof(*ingenic_clk), GFP_KERNEL);
+	if (!ingenic_clk) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	ingenic_clk->hw.init = &clk_init;
+	ingenic_clk->cgu = cgu;
+	ingenic_clk->idx = idx;
+
+	clk_init.name = clk_info->name;
+	clk_init.flags = 0;
+	clk_init.parent_names = parent_names;
+
+	caps = clk_info->type;
+
+	if (caps & (CGU_CLK_MUX | CGU_CLK_CUSTOM)) {
+		clk_init.num_parents = 0;
+
+		if (caps & CGU_CLK_MUX)
+			num_possible = 1 << clk_info->mux.bits;
+		else
+			num_possible = ARRAY_SIZE(clk_info->parents);
+
+		for (i = 0; i < num_possible; i++) {
+			if (clk_info->parents[i] == -1)
+				continue;
+
+			parent = cgu->clocks.clks[clk_info->parents[i]];
+			parent_names[clk_init.num_parents] =
+				__clk_get_name(parent);
+			clk_init.num_parents++;
+		}
+
+		BUG_ON(!clk_init.num_parents);
+		BUG_ON(clk_init.num_parents > ARRAY_SIZE(parent_names));
+	} else {
+		BUG_ON(clk_info->parents[0] == -1);
+		clk_init.num_parents = 1;
+		parent = cgu->clocks.clks[clk_info->parents[0]];
+		parent_names[0] = __clk_get_name(parent);
+	}
+
+	if (caps & CGU_CLK_CUSTOM) {
+		clk_init.ops = clk_info->custom.clk_ops;
+
+		caps &= ~CGU_CLK_CUSTOM;
+
+		if (caps) {
+			pr_err("%s: custom clock may not be combined with type 0x%x\n",
+			       __func__, caps);
+			goto out;
+		}
+	} else if (caps & CGU_CLK_PLL) {
+		clk_init.ops = &ingenic_pll_ops;
+
+		caps &= ~CGU_CLK_PLL;
+
+		if (caps) {
+			pr_err("%s: PLL may not be combined with type 0x%x\n",
+			       __func__, caps);
+			goto out;
+		}
+	} else {
+		clk_init.ops = &ingenic_clk_ops;
+	}
+
+	/* nothing to do for gates or fixed dividers */
+	caps &= ~(CGU_CLK_GATE | CGU_CLK_FIXDIV);
+
+	if (caps & CGU_CLK_MUX) {
+		if (!(caps & CGU_CLK_MUX_GLITCHFREE))
+			clk_init.flags |= CLK_SET_PARENT_GATE;
+
+		caps &= ~(CGU_CLK_MUX | CGU_CLK_MUX_GLITCHFREE);
+	}
+
+	if (caps & CGU_CLK_DIV) {
+		caps &= ~CGU_CLK_DIV;
+	} else {
+		/* pass rate changes to the parent clock */
+		clk_init.flags |= CLK_SET_RATE_PARENT;
+	}
+
+	if (caps) {
+		pr_err("%s: unknown clock type 0x%x\n", __func__, caps);
+		goto out;
+	}
+
+	clk = clk_register(NULL, &ingenic_clk->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to register clock '%s'\n", __func__,
+		       clk_info->name);
+		err = PTR_ERR(clk);
+		goto out;
+	}
+
+	err = clk_register_clkdev(clk, clk_info->name, NULL);
+	if (err)
+		goto out;
+
+	cgu->clocks.clks[idx] = clk;
+out:
+	if (err)
+		kfree(ingenic_clk);
+	return err;
+}
+
+struct ingenic_cgu *
+ingenic_cgu_new(const struct ingenic_cgu_clk_info *clock_info,
+		unsigned num_clocks, struct device_node *np)
+{
+	struct ingenic_cgu *cgu;
+
+	cgu = kzalloc(sizeof(*cgu), GFP_KERNEL);
+	if (!cgu)
+		goto err_out;
+
+	cgu->base = of_iomap(np, 0);
+	if (!cgu->base) {
+		pr_err("%s: failed to map CGU registers\n", __func__);
+		goto err_out_free;
+	}
+
+	cgu->np = np;
+	cgu->clock_info = clock_info;
+	cgu->clocks.clk_num = num_clocks;
+
+	spin_lock_init(&cgu->lock);
+
+	return cgu;
+
+err_out_free:
+	kfree(cgu);
+err_out:
+	return NULL;
+}
+
+int ingenic_cgu_register_clocks(struct ingenic_cgu *cgu)
+{
+	unsigned i;
+	int err;
+
+	cgu->clocks.clks = kcalloc(cgu->clocks.clk_num, sizeof(struct clk *),
+				   GFP_KERNEL);
+	if (!cgu->clocks.clks) {
+		err = -ENOMEM;
+		goto err_out;
+	}
+
+	for (i = 0; i < cgu->clocks.clk_num; i++) {
+		err = ingenic_register_clock(cgu, i);
+		if (err)
+			goto err_out_unregister;
+	}
+
+	err = of_clk_add_provider(cgu->np, of_clk_src_onecell_get,
+				  &cgu->clocks);
+	if (err)
+		goto err_out_unregister;
+
+	return 0;
+
+err_out_unregister:
+	for (i = 0; i < cgu->clocks.clk_num; i++) {
+		if (!cgu->clocks.clks[i])
+			continue;
+		if (cgu->clock_info[i].type & CGU_CLK_EXT)
+			clk_put(cgu->clocks.clks[i]);
+		else
+			clk_unregister(cgu->clocks.clks[i]);
+	}
+	kfree(cgu->clocks.clks);
+err_out:
+	return err;
+}
diff --git a/drivers/clk/ingenic/cgu.h b/drivers/clk/ingenic/cgu.h
new file mode 100644
index 000000000000..99347e2b97e8
--- /dev/null
+++ b/drivers/clk/ingenic/cgu.h
@@ -0,0 +1,223 @@
+/*
+ * Ingenic SoC CGU driver
+ *
+ * Copyright (c) 2013-2015 Imagination Technologies
+ * Author: Paul Burton <paul.burton@imgtec.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.
+ */
+
+#ifndef __DRIVERS_CLK_INGENIC_CGU_H__
+#define __DRIVERS_CLK_INGENIC_CGU_H__
+
+#include <linux/bitops.h>
+#include <linux/of.h>
+#include <linux/spinlock.h>
+
+/**
+ * struct ingenic_cgu_pll_info - information about a PLL
+ * @reg: the offset of the PLL's control register within the CGU
+ * @m_shift: the number of bits to shift the multiplier value by (ie. the
+ *           index of the lowest bit of the multiplier value in the PLL's
+ *           control register)
+ * @m_bits: the size of the multiplier field in bits
+ * @m_offset: the multiplier value which encodes to 0 in the PLL's control
+ *            register
+ * @n_shift: the number of bits to shift the divider value by (ie. the
+ *           index of the lowest bit of the divider value in the PLL's
+ *           control register)
+ * @n_bits: the size of the divider field in bits
+ * @n_offset: the divider value which encodes to 0 in the PLL's control
+ *            register
+ * @od_shift: the number of bits to shift the post-VCO divider value by (ie.
+ *            the index of the lowest bit of the post-VCO divider value in
+ *            the PLL's control register)
+ * @od_bits: the size of the post-VCO divider field in bits
+ * @od_max: the maximum post-VCO divider value
+ * @od_encoding: a pointer to an array mapping post-VCO divider values to
+ *               their encoded values in the PLL control register, or -1 for
+ *               unsupported values
+ * @bypass_bit: the index of the bypass bit in the PLL control register
+ * @enable_bit: the index of the enable bit in the PLL control register
+ * @stable_bit: the index of the stable bit in the PLL control register
+ */
+struct ingenic_cgu_pll_info {
+	unsigned reg;
+	const s8 *od_encoding;
+	u8 m_shift, m_bits, m_offset;
+	u8 n_shift, n_bits, n_offset;
+	u8 od_shift, od_bits, od_max;
+	u8 bypass_bit;
+	u8 enable_bit;
+	u8 stable_bit;
+};
+
+/**
+ * struct ingenic_cgu_mux_info - information about a clock mux
+ * @reg: offset of the mux control register within the CGU
+ * @shift: number of bits to shift the mux value by (ie. the index of
+ *         the lowest bit of the mux value within its control register)
+ * @bits: the size of the mux value in bits
+ */
+struct ingenic_cgu_mux_info {
+	unsigned reg;
+	u8 shift;
+	u8 bits;
+};
+
+/**
+ * struct ingenic_cgu_div_info - information about a divider
+ * @reg: offset of the divider control register within the CGU
+ * @shift: number of bits to shift the divide value by (ie. the index of
+ *         the lowest bit of the divide value within its control register)
+ * @bits: the size of the divide value in bits
+ * @ce_bit: the index of the change enable bit within reg, or -1 if there
+ *          isn't one
+ * @busy_bit: the index of the busy bit within reg, or -1 if there isn't one
+ * @stop_bit: the index of the stop bit within reg, or -1 if there isn't one
+ */
+struct ingenic_cgu_div_info {
+	unsigned reg;
+	u8 shift;
+	u8 bits;
+	s8 ce_bit;
+	s8 busy_bit;
+	s8 stop_bit;
+};
+
+/**
+ * struct ingenic_cgu_fixdiv_info - information about a fixed divider
+ * @div: the divider applied to the parent clock
+ */
+struct ingenic_cgu_fixdiv_info {
+	unsigned div;
+};
+
+/**
+ * struct ingenic_cgu_gate_info - information about a clock gate
+ * @reg: offset of the gate control register within the CGU
+ * @bit: offset of the bit in the register that controls the gate
+ */
+struct ingenic_cgu_gate_info {
+	unsigned reg;
+	u8 bit;
+};
+
+/**
+ * struct ingenic_cgu_custom_info - information about a custom (SoC) clock
+ * @clk_ops: custom clock operation callbacks
+ */
+struct ingenic_cgu_custom_info {
+	struct clk_ops *clk_ops;
+};
+
+/**
+ * struct ingenic_cgu_clk_info - information about a clock
+ * @name: name of the clock
+ * @type: a bitmask formed from CGU_CLK_* values
+ * @parents: an array of the indices of potential parents of this clock
+ *           within the clock_info array of the CGU, or -1 in entries
+ *           which correspond to no valid parent
+ * @pll: information valid if type includes CGU_CLK_PLL
+ * @gate: information valid if type includes CGU_CLK_GATE
+ * @mux: information valid if type includes CGU_CLK_MUX
+ * @div: information valid if type includes CGU_CLK_DIV
+ * @fixdiv: information valid if type includes CGU_CLK_FIXDIV
+ * @custom: information valid if type includes CGU_CLK_CUSTOM
+ */
+struct ingenic_cgu_clk_info {
+	const char *name;
+
+	enum {
+		CGU_CLK_NONE		= 0,
+		CGU_CLK_EXT		= BIT(0),
+		CGU_CLK_PLL		= BIT(1),
+		CGU_CLK_GATE		= BIT(2),
+		CGU_CLK_MUX		= BIT(3),
+		CGU_CLK_MUX_GLITCHFREE	= BIT(4),
+		CGU_CLK_DIV		= BIT(5),
+		CGU_CLK_FIXDIV		= BIT(6),
+		CGU_CLK_CUSTOM		= BIT(7),
+	} type;
+
+	int parents[4];
+
+	union {
+		struct ingenic_cgu_pll_info pll;
+
+		struct {
+			struct ingenic_cgu_gate_info gate;
+			struct ingenic_cgu_mux_info mux;
+			struct ingenic_cgu_div_info div;
+			struct ingenic_cgu_fixdiv_info fixdiv;
+		};
+
+		struct ingenic_cgu_custom_info custom;
+	};
+};
+
+/**
+ * struct ingenic_cgu - data about the CGU
+ * @np: the device tree node that caused the CGU to be probed
+ * @base: the ioremap'ed base address of the CGU registers
+ * @clock_info: an array containing information about implemented clocks
+ * @clocks: used to provide clocks to DT, allows lookup of struct clk*
+ * @lock: lock to be held whilst manipulating CGU registers
+ */
+struct ingenic_cgu {
+	struct device_node *np;
+	void __iomem *base;
+
+	const struct ingenic_cgu_clk_info *clock_info;
+	struct clk_onecell_data clocks;
+
+	spinlock_t lock;
+};
+
+/**
+ * struct ingenic_clk - private data for a clock
+ * @hw: see Documentation/clk.txt
+ * @cgu: a pointer to the CGU data
+ * @idx: the index of this clock in cgu->clock_info
+ */
+struct ingenic_clk {
+	struct clk_hw hw;
+	struct ingenic_cgu *cgu;
+	unsigned idx;
+};
+
+#define to_ingenic_clk(_hw) container_of(_hw, struct ingenic_clk, hw)
+
+/**
+ * ingenic_cgu_new() - create a new CGU instance
+ * @clock_info: an array of clock information structures describing the clocks
+ *              which are implemented by the CGU
+ * @num_clocks: the number of entries in clock_info
+ * @np: the device tree node which causes this CGU to be probed
+ *
+ * Return: a pointer to the CGU instance if initialisation is successful,
+ *         otherwise NULL.
+ */
+struct ingenic_cgu *
+ingenic_cgu_new(const struct ingenic_cgu_clk_info *clock_info,
+		unsigned num_clocks, struct device_node *np);
+
+/**
+ * ingenic_cgu_register_clocks() - Registers the clocks
+ * @cgu: pointer to cgu data
+ *
+ * Register the clocks described by the CGU with the common clock framework.
+ *
+ * Return: 0 on success or -errno if unsuccesful.
+ */
+int ingenic_cgu_register_clocks(struct ingenic_cgu *cgu);
+
+#endif /* __DRIVERS_CLK_INGENIC_CGU_H__ */
diff --git a/drivers/clk/ingenic/jz4740-cgu.c b/drivers/clk/ingenic/jz4740-cgu.c
new file mode 100644
index 000000000000..305a26c2a800
--- /dev/null
+++ b/drivers/clk/ingenic/jz4740-cgu.c
@@ -0,0 +1,303 @@
+/*
+ * Ingenic JZ4740 SoC CGU driver
+ *
+ * Copyright (c) 2015 Imagination Technologies
+ * Author: Paul Burton <paul.burton@imgtec.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/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/of.h>
+#include <dt-bindings/clock/jz4740-cgu.h>
+#include <asm/mach-jz4740/clock.h>
+#include "cgu.h"
+
+/* CGU register offsets */
+#define CGU_REG_CPCCR		0x00
+#define CGU_REG_LCR		0x04
+#define CGU_REG_CPPCR		0x10
+#define CGU_REG_CLKGR		0x20
+#define CGU_REG_SCR		0x24
+#define CGU_REG_I2SCDR		0x60
+#define CGU_REG_LPCDR		0x64
+#define CGU_REG_MSCCDR		0x68
+#define CGU_REG_UHCCDR		0x6c
+#define CGU_REG_SSICDR		0x74
+
+/* bits within a PLL control register */
+#define PLLCTL_M_SHIFT		23
+#define PLLCTL_M_MASK		(0x1ff << PLLCTL_M_SHIFT)
+#define PLLCTL_N_SHIFT		18
+#define PLLCTL_N_MASK		(0x1f << PLLCTL_N_SHIFT)
+#define PLLCTL_OD_SHIFT		16
+#define PLLCTL_OD_MASK		(0x3 << PLLCTL_OD_SHIFT)
+#define PLLCTL_STABLE		(1 << 10)
+#define PLLCTL_BYPASS		(1 << 9)
+#define PLLCTL_ENABLE		(1 << 8)
+
+/* bits within the LCR register */
+#define LCR_SLEEP		(1 << 0)
+
+/* bits within the CLKGR register */
+#define CLKGR_UDC		(1 << 11)
+
+static struct ingenic_cgu *cgu;
+
+static const s8 pll_od_encoding[4] = {
+	0x0, 0x1, -1, 0x3,
+};
+
+static const struct ingenic_cgu_clk_info jz4740_cgu_clocks[] = {
+
+	/* External clocks */
+
+	[JZ4740_CLK_EXT] = { "ext", CGU_CLK_EXT },
+	[JZ4740_CLK_RTC] = { "rtc", CGU_CLK_EXT },
+
+	[JZ4740_CLK_PLL] = {
+		"pll", CGU_CLK_PLL,
+		.parents = { JZ4740_CLK_EXT, -1, -1, -1 },
+		.pll = {
+			.reg = CGU_REG_CPPCR,
+			.m_shift = 23,
+			.m_bits = 9,
+			.m_offset = 2,
+			.n_shift = 18,
+			.n_bits = 5,
+			.n_offset = 2,
+			.od_shift = 16,
+			.od_bits = 2,
+			.od_max = 4,
+			.od_encoding = pll_od_encoding,
+			.stable_bit = 10,
+			.bypass_bit = 9,
+			.enable_bit = 8,
+		},
+	},
+
+	/* Muxes & dividers */
+
+	[JZ4740_CLK_PLL_HALF] = {
+		"pll half", CGU_CLK_DIV,
+		.parents = { JZ4740_CLK_PLL, -1, -1, -1 },
+		.div = { CGU_REG_CPCCR, 21, 1, -1, -1, -1 },
+	},
+
+	[JZ4740_CLK_CCLK] = {
+		"cclk", CGU_CLK_DIV,
+		.parents = { JZ4740_CLK_PLL, -1, -1, -1 },
+		.div = { CGU_REG_CPCCR, 0, 4, 22, -1, -1 },
+	},
+
+	[JZ4740_CLK_HCLK] = {
+		"hclk", CGU_CLK_DIV,
+		.parents = { JZ4740_CLK_PLL, -1, -1, -1 },
+		.div = { CGU_REG_CPCCR, 4, 4, 22, -1, -1 },
+	},
+
+	[JZ4740_CLK_PCLK] = {
+		"pclk", CGU_CLK_DIV,
+		.parents = { JZ4740_CLK_PLL, -1, -1, -1 },
+		.div = { CGU_REG_CPCCR, 8, 4, 22, -1, -1 },
+	},
+
+	[JZ4740_CLK_MCLK] = {
+		"mclk", CGU_CLK_DIV,
+		.parents = { JZ4740_CLK_PLL, -1, -1, -1 },
+		.div = { CGU_REG_CPCCR, 12, 4, 22, -1, -1 },
+	},
+
+	[JZ4740_CLK_LCD] = {
+		"lcd", CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_PLL_HALF, -1, -1, -1 },
+		.div = { CGU_REG_CPCCR, 16, 5, 22, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 10 },
+	},
+
+	[JZ4740_CLK_LCD_PCLK] = {
+		"lcd_pclk", CGU_CLK_DIV,
+		.parents = { JZ4740_CLK_PLL_HALF, -1, -1, -1 },
+		.div = { CGU_REG_LPCDR, 0, 11, -1, -1, -1 },
+	},
+
+	[JZ4740_CLK_I2S] = {
+		"i2s", CGU_CLK_MUX | CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_EXT, JZ4740_CLK_PLL_HALF, -1, -1 },
+		.mux = { CGU_REG_CPCCR, 31, 1 },
+		.div = { CGU_REG_I2SCDR, 0, 8, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 6 },
+	},
+
+	[JZ4740_CLK_SPI] = {
+		"spi", CGU_CLK_MUX | CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_EXT, JZ4740_CLK_PLL, -1, -1 },
+		.mux = { CGU_REG_SSICDR, 31, 1 },
+		.div = { CGU_REG_SSICDR, 0, 4, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 4 },
+	},
+
+	[JZ4740_CLK_MMC] = {
+		"mmc", CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_PLL_HALF, -1, -1, -1 },
+		.div = { CGU_REG_MSCCDR, 0, 5, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 7 },
+	},
+
+	[JZ4740_CLK_UHC] = {
+		"uhc", CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_PLL_HALF, -1, -1, -1 },
+		.div = { CGU_REG_UHCCDR, 0, 4, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 14 },
+	},
+
+	[JZ4740_CLK_UDC] = {
+		"udc", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { JZ4740_CLK_EXT, JZ4740_CLK_PLL_HALF, -1, -1 },
+		.mux = { CGU_REG_CPCCR, 29, 1 },
+		.div = { CGU_REG_CPCCR, 23, 6, -1, -1, -1 },
+		.gate = { CGU_REG_SCR, 6 },
+	},
+
+	/* Gate-only clocks */
+
+	[JZ4740_CLK_UART0] = {
+		"uart0", CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_EXT, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 0 },
+	},
+
+	[JZ4740_CLK_UART1] = {
+		"uart1", CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_EXT, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 15 },
+	},
+
+	[JZ4740_CLK_DMA] = {
+		"dma", CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_PCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 12 },
+	},
+
+	[JZ4740_CLK_IPU] = {
+		"ipu", CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_PCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 13 },
+	},
+
+	[JZ4740_CLK_ADC] = {
+		"adc", CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_EXT, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 8 },
+	},
+
+	[JZ4740_CLK_I2C] = {
+		"i2c", CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_EXT, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 3 },
+	},
+
+	[JZ4740_CLK_AIC] = {
+		"aic", CGU_CLK_GATE,
+		.parents = { JZ4740_CLK_EXT, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR, 5 },
+	},
+};
+
+static void __init jz4740_cgu_init(struct device_node *np)
+{
+	int retval;
+
+	cgu = ingenic_cgu_new(jz4740_cgu_clocks,
+			      ARRAY_SIZE(jz4740_cgu_clocks), np);
+	if (!cgu) {
+		pr_err("%s: failed to initialise CGU\n", __func__);
+		return;
+	}
+
+	retval = ingenic_cgu_register_clocks(cgu);
+	if (retval)
+		pr_err("%s: failed to register CGU Clocks\n", __func__);
+}
+CLK_OF_DECLARE(jz4740_cgu, "ingenic,jz4740-cgu", jz4740_cgu_init);
+
+void jz4740_clock_set_wait_mode(enum jz4740_wait_mode mode)
+{
+	uint32_t lcr = readl(cgu->base + CGU_REG_LCR);
+
+	switch (mode) {
+	case JZ4740_WAIT_MODE_IDLE:
+		lcr &= ~LCR_SLEEP;
+		break;
+
+	case JZ4740_WAIT_MODE_SLEEP:
+		lcr |= LCR_SLEEP;
+		break;
+	}
+
+	writel(lcr, cgu->base + CGU_REG_LCR);
+}
+
+void jz4740_clock_udc_disable_auto_suspend(void)
+{
+	uint32_t clkgr = readl(cgu->base + CGU_REG_CLKGR);
+
+	clkgr &= ~CLKGR_UDC;
+	writel(clkgr, cgu->base + CGU_REG_CLKGR);
+}
+EXPORT_SYMBOL_GPL(jz4740_clock_udc_disable_auto_suspend);
+
+void jz4740_clock_udc_enable_auto_suspend(void)
+{
+	uint32_t clkgr = readl(cgu->base + CGU_REG_CLKGR);
+
+	clkgr |= CLKGR_UDC;
+	writel(clkgr, cgu->base + CGU_REG_CLKGR);
+}
+EXPORT_SYMBOL_GPL(jz4740_clock_udc_enable_auto_suspend);
+
+#define JZ_CLOCK_GATE_UART0	BIT(0)
+#define JZ_CLOCK_GATE_TCU	BIT(1)
+#define JZ_CLOCK_GATE_DMAC	BIT(12)
+
+void jz4740_clock_suspend(void)
+{
+	uint32_t clkgr, cppcr;
+
+	clkgr = readl(cgu->base + CGU_REG_CLKGR);
+	clkgr |= JZ_CLOCK_GATE_TCU | JZ_CLOCK_GATE_DMAC | JZ_CLOCK_GATE_UART0;
+	writel(clkgr, cgu->base + CGU_REG_CLKGR);
+
+	cppcr = readl(cgu->base + CGU_REG_CPPCR);
+	cppcr &= ~BIT(jz4740_cgu_clocks[JZ4740_CLK_PLL].pll.enable_bit);
+	writel(cppcr, cgu->base + CGU_REG_CPPCR);
+}
+
+void jz4740_clock_resume(void)
+{
+	uint32_t clkgr, cppcr, stable;
+
+	cppcr = readl(cgu->base + CGU_REG_CPPCR);
+	cppcr |= BIT(jz4740_cgu_clocks[JZ4740_CLK_PLL].pll.enable_bit);
+	writel(cppcr, cgu->base + CGU_REG_CPPCR);
+
+	stable = BIT(jz4740_cgu_clocks[JZ4740_CLK_PLL].pll.stable_bit);
+	do {
+		cppcr = readl(cgu->base + CGU_REG_CPPCR);
+	} while (!(cppcr & stable));
+
+	clkgr = readl(cgu->base + CGU_REG_CLKGR);
+	clkgr &= ~JZ_CLOCK_GATE_TCU;
+	clkgr &= ~JZ_CLOCK_GATE_DMAC;
+	clkgr &= ~JZ_CLOCK_GATE_UART0;
+	writel(clkgr, cgu->base + CGU_REG_CLKGR);
+}
diff --git a/drivers/clk/ingenic/jz4780-cgu.c b/drivers/clk/ingenic/jz4780-cgu.c
new file mode 100644
index 000000000000..431f962300b6
--- /dev/null
+++ b/drivers/clk/ingenic/jz4780-cgu.c
@@ -0,0 +1,733 @@
+/*
+ * Ingenic JZ4780 SoC CGU driver
+ *
+ * Copyright (c) 2013-2015 Imagination Technologies
+ * Author: Paul Burton <paul.burton@imgtec.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/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/of.h>
+#include <dt-bindings/clock/jz4780-cgu.h>
+#include "cgu.h"
+
+/* CGU register offsets */
+#define CGU_REG_CLOCKCONTROL	0x00
+#define CGU_REG_PLLCONTROL	0x0c
+#define CGU_REG_APLL		0x10
+#define CGU_REG_MPLL		0x14
+#define CGU_REG_EPLL		0x18
+#define CGU_REG_VPLL		0x1c
+#define CGU_REG_CLKGR0		0x20
+#define CGU_REG_OPCR		0x24
+#define CGU_REG_CLKGR1		0x28
+#define CGU_REG_DDRCDR		0x2c
+#define CGU_REG_VPUCDR		0x30
+#define CGU_REG_USBPCR		0x3c
+#define CGU_REG_USBRDT		0x40
+#define CGU_REG_USBVBFIL	0x44
+#define CGU_REG_USBPCR1		0x48
+#define CGU_REG_LP0CDR		0x54
+#define CGU_REG_I2SCDR		0x60
+#define CGU_REG_LP1CDR		0x64
+#define CGU_REG_MSC0CDR		0x68
+#define CGU_REG_UHCCDR		0x6c
+#define CGU_REG_SSICDR		0x74
+#define CGU_REG_CIMCDR		0x7c
+#define CGU_REG_PCMCDR		0x84
+#define CGU_REG_GPUCDR		0x88
+#define CGU_REG_HDMICDR		0x8c
+#define CGU_REG_MSC1CDR		0xa4
+#define CGU_REG_MSC2CDR		0xa8
+#define CGU_REG_BCHCDR		0xac
+#define CGU_REG_CLOCKSTATUS	0xd4
+
+/* bits within the OPCR register */
+#define OPCR_SPENDN0		(1 << 7)
+#define OPCR_SPENDN1		(1 << 6)
+
+/* bits within the USBPCR register */
+#define USBPCR_USB_MODE		BIT(31)
+#define USBPCR_IDPULLUP_MASK	(0x3 << 28)
+#define USBPCR_COMMONONN	BIT(25)
+#define USBPCR_VBUSVLDEXT	BIT(24)
+#define USBPCR_VBUSVLDEXTSEL	BIT(23)
+#define USBPCR_POR		BIT(22)
+#define USBPCR_OTG_DISABLE	BIT(20)
+#define USBPCR_COMPDISTUNE_MASK	(0x7 << 17)
+#define USBPCR_OTGTUNE_MASK	(0x7 << 14)
+#define USBPCR_SQRXTUNE_MASK	(0x7 << 11)
+#define USBPCR_TXFSLSTUNE_MASK	(0xf << 7)
+#define USBPCR_TXPREEMPHTUNE	BIT(6)
+#define USBPCR_TXHSXVTUNE_MASK	(0x3 << 4)
+#define USBPCR_TXVREFTUNE_MASK	0xf
+
+/* bits within the USBPCR1 register */
+#define USBPCR1_REFCLKSEL_SHIFT	26
+#define USBPCR1_REFCLKSEL_MASK	(0x3 << USBPCR1_REFCLKSEL_SHIFT)
+#define USBPCR1_REFCLKSEL_CORE	(0x2 << USBPCR1_REFCLKSEL_SHIFT)
+#define USBPCR1_REFCLKDIV_SHIFT	24
+#define USBPCR1_REFCLKDIV_MASK	(0x3 << USBPCR1_REFCLKDIV_SHIFT)
+#define USBPCR1_REFCLKDIV_19_2	(0x3 << USBPCR1_REFCLKDIV_SHIFT)
+#define USBPCR1_REFCLKDIV_48	(0x2 << USBPCR1_REFCLKDIV_SHIFT)
+#define USBPCR1_REFCLKDIV_24	(0x1 << USBPCR1_REFCLKDIV_SHIFT)
+#define USBPCR1_REFCLKDIV_12	(0x0 << USBPCR1_REFCLKDIV_SHIFT)
+#define USBPCR1_USB_SEL		BIT(28)
+#define USBPCR1_WORD_IF0	BIT(19)
+#define USBPCR1_WORD_IF1	BIT(18)
+
+/* bits within the USBRDT register */
+#define USBRDT_VBFIL_LD_EN	BIT(25)
+#define USBRDT_USBRDT_MASK	0x7fffff
+
+/* bits within the USBVBFIL register */
+#define USBVBFIL_IDDIGFIL_SHIFT	16
+#define USBVBFIL_IDDIGFIL_MASK	(0xffff << USBVBFIL_IDDIGFIL_SHIFT)
+#define USBVBFIL_USBVBFIL_MASK	(0xffff)
+
+static struct ingenic_cgu *cgu;
+
+static u8 jz4780_otg_phy_get_parent(struct clk_hw *hw)
+{
+	/* we only use CLKCORE, revisit if that ever changes */
+	return 0;
+}
+
+static int jz4780_otg_phy_set_parent(struct clk_hw *hw, u8 idx)
+{
+	unsigned long flags;
+	u32 usbpcr1;
+
+	if (idx > 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&cgu->lock, flags);
+
+	usbpcr1 = readl(cgu->base + CGU_REG_USBPCR1);
+	usbpcr1 &= ~USBPCR1_REFCLKSEL_MASK;
+	/* we only use CLKCORE */
+	usbpcr1 |= USBPCR1_REFCLKSEL_CORE;
+	writel(usbpcr1, cgu->base + CGU_REG_USBPCR1);
+
+	spin_unlock_irqrestore(&cgu->lock, flags);
+	return 0;
+}
+
+static unsigned long jz4780_otg_phy_recalc_rate(struct clk_hw *hw,
+						unsigned long parent_rate)
+{
+	u32 usbpcr1;
+	unsigned refclk_div;
+
+	usbpcr1 = readl(cgu->base + CGU_REG_USBPCR1);
+	refclk_div = usbpcr1 & USBPCR1_REFCLKDIV_MASK;
+
+	switch (refclk_div) {
+	case USBPCR1_REFCLKDIV_12:
+		return 12000000;
+
+	case USBPCR1_REFCLKDIV_24:
+		return 24000000;
+
+	case USBPCR1_REFCLKDIV_48:
+		return 48000000;
+
+	case USBPCR1_REFCLKDIV_19_2:
+		return 19200000;
+	}
+
+	BUG();
+	return parent_rate;
+}
+
+static long jz4780_otg_phy_round_rate(struct clk_hw *hw, unsigned long req_rate,
+				      unsigned long *parent_rate)
+{
+	if (req_rate < 15600000)
+		return 12000000;
+
+	if (req_rate < 21600000)
+		return 19200000;
+
+	if (req_rate < 36000000)
+		return 24000000;
+
+	return 48000000;
+}
+
+static int jz4780_otg_phy_set_rate(struct clk_hw *hw, unsigned long req_rate,
+				   unsigned long parent_rate)
+{
+	unsigned long flags;
+	u32 usbpcr1, div_bits;
+
+	switch (req_rate) {
+	case 12000000:
+		div_bits = USBPCR1_REFCLKDIV_12;
+		break;
+
+	case 19200000:
+		div_bits = USBPCR1_REFCLKDIV_19_2;
+		break;
+
+	case 24000000:
+		div_bits = USBPCR1_REFCLKDIV_24;
+		break;
+
+	case 48000000:
+		div_bits = USBPCR1_REFCLKDIV_48;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&cgu->lock, flags);
+
+	usbpcr1 = readl(cgu->base + CGU_REG_USBPCR1);
+	usbpcr1 &= ~USBPCR1_REFCLKDIV_MASK;
+	usbpcr1 |= div_bits;
+	writel(usbpcr1, cgu->base + CGU_REG_USBPCR1);
+
+	spin_unlock_irqrestore(&cgu->lock, flags);
+	return 0;
+}
+
+static struct clk_ops jz4780_otg_phy_ops = {
+	.get_parent = jz4780_otg_phy_get_parent,
+	.set_parent = jz4780_otg_phy_set_parent,
+
+	.recalc_rate = jz4780_otg_phy_recalc_rate,
+	.round_rate = jz4780_otg_phy_round_rate,
+	.set_rate = jz4780_otg_phy_set_rate,
+};
+
+static const s8 pll_od_encoding[16] = {
+	0x0, 0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7,
+	0x8, 0x9, 0xa, 0xb, 0xc, 0xd, 0xe, 0xf,
+};
+
+static const struct ingenic_cgu_clk_info jz4780_cgu_clocks[] = {
+
+	/* External clocks */
+
+	[JZ4780_CLK_EXCLK] = { "ext", CGU_CLK_EXT },
+	[JZ4780_CLK_RTCLK] = { "rtc", CGU_CLK_EXT },
+
+	/* PLLs */
+
+#define DEF_PLL(name) { \
+	.reg = CGU_REG_ ## name, \
+	.m_shift = 19, \
+	.m_bits = 13, \
+	.m_offset = 1, \
+	.n_shift = 13, \
+	.n_bits = 6, \
+	.n_offset = 1, \
+	.od_shift = 9, \
+	.od_bits = 4, \
+	.od_max = 16, \
+	.od_encoding = pll_od_encoding, \
+	.stable_bit = 6, \
+	.bypass_bit = 1, \
+	.enable_bit = 0, \
+}
+
+	[JZ4780_CLK_APLL] = {
+		"apll", CGU_CLK_PLL,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.pll = DEF_PLL(APLL),
+	},
+
+	[JZ4780_CLK_MPLL] = {
+		"mpll", CGU_CLK_PLL,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.pll = DEF_PLL(MPLL),
+	},
+
+	[JZ4780_CLK_EPLL] = {
+		"epll", CGU_CLK_PLL,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.pll = DEF_PLL(EPLL),
+	},
+
+	[JZ4780_CLK_VPLL] = {
+		"vpll", CGU_CLK_PLL,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.pll = DEF_PLL(VPLL),
+	},
+
+#undef DEF_PLL
+
+	/* Custom (SoC-specific) OTG PHY */
+
+	[JZ4780_CLK_OTGPHY] = {
+		"otg_phy", CGU_CLK_CUSTOM,
+		.parents = { -1, -1, JZ4780_CLK_EXCLK, -1 },
+		.custom = { &jz4780_otg_phy_ops },
+	},
+
+	/* Muxes & dividers */
+
+	[JZ4780_CLK_SCLKA] = {
+		"sclk_a", CGU_CLK_MUX,
+		.parents = { -1, JZ4780_CLK_APLL, JZ4780_CLK_EXCLK,
+			     JZ4780_CLK_RTCLK },
+		.mux = { CGU_REG_CLOCKCONTROL, 30, 2 },
+	},
+
+	[JZ4780_CLK_CPUMUX] = {
+		"cpumux", CGU_CLK_MUX,
+		.parents = { -1, JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_EPLL },
+		.mux = { CGU_REG_CLOCKCONTROL, 28, 2 },
+	},
+
+	[JZ4780_CLK_CPU] = {
+		"cpu", CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_CPUMUX, -1, -1, -1 },
+		.div = { CGU_REG_CLOCKCONTROL, 0, 4, 22, -1, -1 },
+	},
+
+	[JZ4780_CLK_L2CACHE] = {
+		"l2cache", CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_CPUMUX, -1, -1, -1 },
+		.div = { CGU_REG_CLOCKCONTROL, 4, 4, -1, -1, -1 },
+	},
+
+	[JZ4780_CLK_AHB0] = {
+		"ahb0", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { -1, JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_EPLL },
+		.mux = { CGU_REG_CLOCKCONTROL, 26, 2 },
+		.div = { CGU_REG_CLOCKCONTROL, 8, 4, 21, -1, -1 },
+	},
+
+	[JZ4780_CLK_AHB2PMUX] = {
+		"ahb2_apb_mux", CGU_CLK_MUX,
+		.parents = { -1, JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_RTCLK },
+		.mux = { CGU_REG_CLOCKCONTROL, 24, 2 },
+	},
+
+	[JZ4780_CLK_AHB2] = {
+		"ahb2", CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_AHB2PMUX, -1, -1, -1 },
+		.div = { CGU_REG_CLOCKCONTROL, 12, 4, 20, -1, -1 },
+	},
+
+	[JZ4780_CLK_PCLK] = {
+		"pclk", CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_AHB2PMUX, -1, -1, -1 },
+		.div = { CGU_REG_CLOCKCONTROL, 16, 4, 20, -1, -1 },
+	},
+
+	[JZ4780_CLK_DDR] = {
+		"ddr", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { -1, JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL, -1 },
+		.mux = { CGU_REG_DDRCDR, 30, 2 },
+		.div = { CGU_REG_DDRCDR, 0, 4, 29, 28, 27 },
+	},
+
+	[JZ4780_CLK_VPU] = {
+		"vpu", CGU_CLK_MUX | CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_EPLL, -1 },
+		.mux = { CGU_REG_VPUCDR, 30, 2 },
+		.div = { CGU_REG_VPUCDR, 0, 4, 29, 28, 27 },
+		.gate = { CGU_REG_CLKGR1, 2 },
+	},
+
+	[JZ4780_CLK_I2SPLL] = {
+		"i2s_pll", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_EPLL, -1, -1 },
+		.mux = { CGU_REG_I2SCDR, 30, 1 },
+		.div = { CGU_REG_I2SCDR, 0, 8, 29, 28, 27 },
+	},
+
+	[JZ4780_CLK_I2S] = {
+		"i2s", CGU_CLK_MUX,
+		.parents = { JZ4780_CLK_EXCLK, JZ4780_CLK_I2SPLL, -1, -1 },
+		.mux = { CGU_REG_I2SCDR, 31, 1 },
+	},
+
+	[JZ4780_CLK_LCD0PIXCLK] = {
+		"lcd0pixclk", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_VPLL, -1 },
+		.mux = { CGU_REG_LP0CDR, 30, 2 },
+		.div = { CGU_REG_LP0CDR, 0, 8, 28, 27, 26 },
+	},
+
+	[JZ4780_CLK_LCD1PIXCLK] = {
+		"lcd1pixclk", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_VPLL, -1 },
+		.mux = { CGU_REG_LP1CDR, 30, 2 },
+		.div = { CGU_REG_LP1CDR, 0, 8, 28, 27, 26 },
+	},
+
+	[JZ4780_CLK_MSCMUX] = {
+		"msc_mux", CGU_CLK_MUX,
+		.parents = { -1, JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL, -1 },
+		.mux = { CGU_REG_MSC0CDR, 30, 2 },
+	},
+
+	[JZ4780_CLK_MSC0] = {
+		"msc0", CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_MSCMUX, -1, -1, -1 },
+		.div = { CGU_REG_MSC0CDR, 0, 8, 29, 28, 27 },
+		.gate = { CGU_REG_CLKGR0, 3 },
+	},
+
+	[JZ4780_CLK_MSC1] = {
+		"msc1", CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_MSCMUX, -1, -1, -1 },
+		.div = { CGU_REG_MSC1CDR, 0, 8, 29, 28, 27 },
+		.gate = { CGU_REG_CLKGR0, 11 },
+	},
+
+	[JZ4780_CLK_MSC2] = {
+		"msc2", CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_MSCMUX, -1, -1, -1 },
+		.div = { CGU_REG_MSC2CDR, 0, 8, 29, 28, 27 },
+		.gate = { CGU_REG_CLKGR0, 12 },
+	},
+
+	[JZ4780_CLK_UHC] = {
+		"uhc", CGU_CLK_MUX | CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_EPLL, JZ4780_CLK_OTGPHY },
+		.mux = { CGU_REG_UHCCDR, 30, 2 },
+		.div = { CGU_REG_UHCCDR, 0, 8, 29, 28, 27 },
+		.gate = { CGU_REG_CLKGR0, 24 },
+	},
+
+	[JZ4780_CLK_SSIPLL] = {
+		"ssi_pll", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL, -1, -1 },
+		.mux = { CGU_REG_SSICDR, 30, 1 },
+		.div = { CGU_REG_SSICDR, 0, 8, 29, 28, 27 },
+	},
+
+	[JZ4780_CLK_SSI] = {
+		"ssi", CGU_CLK_MUX,
+		.parents = { JZ4780_CLK_EXCLK, JZ4780_CLK_SSIPLL, -1, -1 },
+		.mux = { CGU_REG_SSICDR, 31, 1 },
+	},
+
+	[JZ4780_CLK_CIMMCLK] = {
+		"cim_mclk", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL, -1, -1 },
+		.mux = { CGU_REG_CIMCDR, 31, 1 },
+		.div = { CGU_REG_CIMCDR, 0, 8, 30, 29, 28 },
+	},
+
+	[JZ4780_CLK_PCMPLL] = {
+		"pcm_pll", CGU_CLK_MUX | CGU_CLK_DIV,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_EPLL, JZ4780_CLK_VPLL },
+		.mux = { CGU_REG_PCMCDR, 29, 2 },
+		.div = { CGU_REG_PCMCDR, 0, 8, 28, 27, 26 },
+	},
+
+	[JZ4780_CLK_PCM] = {
+		"pcm", CGU_CLK_MUX | CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, JZ4780_CLK_PCMPLL, -1, -1 },
+		.mux = { CGU_REG_PCMCDR, 31, 1 },
+		.gate = { CGU_REG_CLKGR1, 3 },
+	},
+
+	[JZ4780_CLK_GPU] = {
+		"gpu", CGU_CLK_MUX | CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { -1, JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_EPLL },
+		.mux = { CGU_REG_GPUCDR, 30, 2 },
+		.div = { CGU_REG_GPUCDR, 0, 4, 29, 28, 27 },
+		.gate = { CGU_REG_CLKGR1, 4 },
+	},
+
+	[JZ4780_CLK_HDMI] = {
+		"hdmi", CGU_CLK_MUX | CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_VPLL, -1 },
+		.mux = { CGU_REG_HDMICDR, 30, 2 },
+		.div = { CGU_REG_HDMICDR, 0, 8, 29, 28, 26 },
+		.gate = { CGU_REG_CLKGR1, 9 },
+	},
+
+	[JZ4780_CLK_BCH] = {
+		"bch", CGU_CLK_MUX | CGU_CLK_DIV | CGU_CLK_GATE,
+		.parents = { -1, JZ4780_CLK_SCLKA, JZ4780_CLK_MPLL,
+			     JZ4780_CLK_EPLL },
+		.mux = { CGU_REG_BCHCDR, 30, 2 },
+		.div = { CGU_REG_BCHCDR, 0, 4, 29, 28, 27 },
+		.gate = { CGU_REG_CLKGR0, 1 },
+	},
+
+	/* Gate-only clocks */
+
+	[JZ4780_CLK_NEMC] = {
+		"nemc", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_AHB2, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 0 },
+	},
+
+	[JZ4780_CLK_OTG0] = {
+		"otg0", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 2 },
+	},
+
+	[JZ4780_CLK_SSI0] = {
+		"ssi0", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_SSI, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 4 },
+	},
+
+	[JZ4780_CLK_SMB0] = {
+		"smb0", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_PCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 5 },
+	},
+
+	[JZ4780_CLK_SMB1] = {
+		"smb1", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_PCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 6 },
+	},
+
+	[JZ4780_CLK_SCC] = {
+		"scc", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 7 },
+	},
+
+	[JZ4780_CLK_AIC] = {
+		"aic", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 8 },
+	},
+
+	[JZ4780_CLK_TSSI0] = {
+		"tssi0", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 9 },
+	},
+
+	[JZ4780_CLK_OWI] = {
+		"owi", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 10 },
+	},
+
+	[JZ4780_CLK_KBC] = {
+		"kbc", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 13 },
+	},
+
+	[JZ4780_CLK_SADC] = {
+		"sadc", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 14 },
+	},
+
+	[JZ4780_CLK_UART0] = {
+		"uart0", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 15 },
+	},
+
+	[JZ4780_CLK_UART1] = {
+		"uart1", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 16 },
+	},
+
+	[JZ4780_CLK_UART2] = {
+		"uart2", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 17 },
+	},
+
+	[JZ4780_CLK_UART3] = {
+		"uart3", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 18 },
+	},
+
+	[JZ4780_CLK_SSI1] = {
+		"ssi1", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_SSI, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 19 },
+	},
+
+	[JZ4780_CLK_SSI2] = {
+		"ssi2", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_SSI, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 20 },
+	},
+
+	[JZ4780_CLK_PDMA] = {
+		"pdma", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 21 },
+	},
+
+	[JZ4780_CLK_GPS] = {
+		"gps", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 22 },
+	},
+
+	[JZ4780_CLK_MAC] = {
+		"mac", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 23 },
+	},
+
+	[JZ4780_CLK_SMB2] = {
+		"smb2", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_PCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 24 },
+	},
+
+	[JZ4780_CLK_CIM] = {
+		"cim", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 26 },
+	},
+
+	[JZ4780_CLK_LCD] = {
+		"lcd", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 28 },
+	},
+
+	[JZ4780_CLK_TVE] = {
+		"tve", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_LCD, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 27 },
+	},
+
+	[JZ4780_CLK_IPU] = {
+		"ipu", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 29 },
+	},
+
+	[JZ4780_CLK_DDR0] = {
+		"ddr0", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_DDR, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 30 },
+	},
+
+	[JZ4780_CLK_DDR1] = {
+		"ddr1", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_DDR, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR0, 31 },
+	},
+
+	[JZ4780_CLK_SMB3] = {
+		"smb3", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_PCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 0 },
+	},
+
+	[JZ4780_CLK_TSSI1] = {
+		"tssi1", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 1 },
+	},
+
+	[JZ4780_CLK_COMPRESS] = {
+		"compress", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 5 },
+	},
+
+	[JZ4780_CLK_AIC1] = {
+		"aic1", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 6 },
+	},
+
+	[JZ4780_CLK_GPVLC] = {
+		"gpvlc", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 7 },
+	},
+
+	[JZ4780_CLK_OTG1] = {
+		"otg1", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 8 },
+	},
+
+	[JZ4780_CLK_UART4] = {
+		"uart4", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 10 },
+	},
+
+	[JZ4780_CLK_AHBMON] = {
+		"ahb_mon", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 11 },
+	},
+
+	[JZ4780_CLK_SMB4] = {
+		"smb4", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_PCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 12 },
+	},
+
+	[JZ4780_CLK_DES] = {
+		"des", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 13 },
+	},
+
+	[JZ4780_CLK_X2D] = {
+		"x2d", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_EXCLK, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 14 },
+	},
+
+	[JZ4780_CLK_CORE1] = {
+		"core1", CGU_CLK_GATE,
+		.parents = { JZ4780_CLK_CPU, -1, -1, -1 },
+		.gate = { CGU_REG_CLKGR1, 15 },
+	},
+
+};
+
+static void __init jz4780_cgu_init(struct device_node *np)
+{
+	int retval;
+
+	cgu = ingenic_cgu_new(jz4780_cgu_clocks,
+			      ARRAY_SIZE(jz4780_cgu_clocks), np);
+	if (!cgu) {
+		pr_err("%s: failed to initialise CGU\n", __func__);
+		return;
+	}
+
+	retval = ingenic_cgu_register_clocks(cgu);
+	if (retval) {
+		pr_err("%s: failed to register CGU Clocks\n", __func__);
+		return;
+	}
+}
+CLK_OF_DECLARE(jz4780_cgu, "ingenic,jz4780-cgu", jz4780_cgu_init);
diff --git a/drivers/clk/keystone/pll.c b/drivers/clk/keystone/pll.c
index 0dd8a4b12747..4a375ead70e9 100644
--- a/drivers/clk/keystone/pll.c
+++ b/drivers/clk/keystone/pll.c
@@ -37,7 +37,8 @@
  *	Main PLL or any other PLLs in the device such as ARM PLL, DDR PLL
  *	or PA PLL available on keystone2. These PLLs are controlled by
  *	this register. Main PLL is controlled by a PLL controller.
- * @pllm: PLL register map address
+ * @pllm: PLL register map address for multiplier bits
+ * @pllod: PLL register map address for post divider bits
  * @pll_ctl0: PLL controller map address
  * @pllm_lower_mask: multiplier lower mask
  * @pllm_upper_mask: multiplier upper mask
@@ -53,6 +54,7 @@ struct clk_pll_data {
 	u32 phy_pllm;
 	u32 phy_pll_ctl0;
 	void __iomem *pllm;
+	void __iomem *pllod;
 	void __iomem *pll_ctl0;
 	u32 pllm_lower_mask;
 	u32 pllm_upper_mask;
@@ -102,7 +104,11 @@ static unsigned long clk_pllclk_recalc(struct clk_hw *hw,
 		/* read post divider from od bits*/
 		postdiv = ((val & pll_data->clkod_mask) >>
 				 pll_data->clkod_shift) + 1;
-	else
+	else if (pll_data->pllod) {
+		postdiv = readl(pll_data->pllod);
+		postdiv = ((postdiv & pll_data->clkod_mask) >>
+				pll_data->clkod_shift) + 1;
+	} else
 		postdiv = pll_data->postdiv;
 
 	rate /= (prediv + 1);
@@ -172,12 +178,21 @@ static void __init _of_pll_clk_init(struct device_node *node, bool pllctrl)
 		/* assume the PLL has output divider register bits */
 		pll_data->clkod_mask = CLKOD_MASK;
 		pll_data->clkod_shift = CLKOD_SHIFT;
+
+		/*
+		 * Check if there is an post-divider register. If not
+		 * assume od bits are part of control register.
+		 */
+		i = of_property_match_string(node, "reg-names",
+					     "post-divider");
+		pll_data->pllod = of_iomap(node, i);
 	}
 
 	i = of_property_match_string(node, "reg-names", "control");
 	pll_data->pll_ctl0 = of_iomap(node, i);
 	if (!pll_data->pll_ctl0) {
 		pr_err("%s: ioremap failed\n", __func__);
+		iounmap(pll_data->pllod);
 		goto out;
 	}
 
@@ -193,6 +208,7 @@ static void __init _of_pll_clk_init(struct device_node *node, bool pllctrl)
 		pll_data->pllm = of_iomap(node, i);
 		if (!pll_data->pllm) {
 			iounmap(pll_data->pll_ctl0);
+			iounmap(pll_data->pllod);
 			goto out;
 		}
 	}
diff --git a/drivers/clk/mediatek/Makefile b/drivers/clk/mediatek/Makefile
new file mode 100644
index 000000000000..8e4b2a4635b9
--- /dev/null
+++ b/drivers/clk/mediatek/Makefile
@@ -0,0 +1,4 @@
+obj-y += clk-mtk.o clk-pll.o clk-gate.o
+obj-$(CONFIG_RESET_CONTROLLER) += reset.o
+obj-y += clk-mt8135.o
+obj-y += clk-mt8173.o
diff --git a/drivers/clk/mediatek/clk-gate.c b/drivers/clk/mediatek/clk-gate.c
new file mode 100644
index 000000000000..57020368a693
--- /dev/null
+++ b/drivers/clk/mediatek/clk-gate.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: James Liao <jamesjj.liao@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/of.h>
+#include <linux/of_address.h>
+
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/clkdev.h>
+
+#include "clk-mtk.h"
+#include "clk-gate.h"
+
+static int mtk_cg_bit_is_cleared(struct clk_hw *hw)
+{
+	struct mtk_clk_gate *cg = to_clk_gate(hw);
+	u32 val;
+
+	regmap_read(cg->regmap, cg->sta_ofs, &val);
+
+	val &= BIT(cg->bit);
+
+	return val == 0;
+}
+
+static int mtk_cg_bit_is_set(struct clk_hw *hw)
+{
+	struct mtk_clk_gate *cg = to_clk_gate(hw);
+	u32 val;
+
+	regmap_read(cg->regmap, cg->sta_ofs, &val);
+
+	val &= BIT(cg->bit);
+
+	return val != 0;
+}
+
+static void mtk_cg_set_bit(struct clk_hw *hw)
+{
+	struct mtk_clk_gate *cg = to_clk_gate(hw);
+
+	regmap_write(cg->regmap, cg->set_ofs, BIT(cg->bit));
+}
+
+static void mtk_cg_clr_bit(struct clk_hw *hw)
+{
+	struct mtk_clk_gate *cg = to_clk_gate(hw);
+
+	regmap_write(cg->regmap, cg->clr_ofs, BIT(cg->bit));
+}
+
+static int mtk_cg_enable(struct clk_hw *hw)
+{
+	mtk_cg_clr_bit(hw);
+
+	return 0;
+}
+
+static void mtk_cg_disable(struct clk_hw *hw)
+{
+	mtk_cg_set_bit(hw);
+}
+
+static int mtk_cg_enable_inv(struct clk_hw *hw)
+{
+	mtk_cg_set_bit(hw);
+
+	return 0;
+}
+
+static void mtk_cg_disable_inv(struct clk_hw *hw)
+{
+	mtk_cg_clr_bit(hw);
+}
+
+const struct clk_ops mtk_clk_gate_ops_setclr = {
+	.is_enabled	= mtk_cg_bit_is_cleared,
+	.enable		= mtk_cg_enable,
+	.disable	= mtk_cg_disable,
+};
+
+const struct clk_ops mtk_clk_gate_ops_setclr_inv = {
+	.is_enabled	= mtk_cg_bit_is_set,
+	.enable		= mtk_cg_enable_inv,
+	.disable	= mtk_cg_disable_inv,
+};
+
+struct clk *mtk_clk_register_gate(
+		const char *name,
+		const char *parent_name,
+		struct regmap *regmap,
+		int set_ofs,
+		int clr_ofs,
+		int sta_ofs,
+		u8 bit,
+		const struct clk_ops *ops)
+{
+	struct mtk_clk_gate *cg;
+	struct clk *clk;
+	struct clk_init_data init = {};
+
+	cg = kzalloc(sizeof(*cg), GFP_KERNEL);
+	if (!cg)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = parent_name ? &parent_name : NULL;
+	init.num_parents = parent_name ? 1 : 0;
+	init.ops = ops;
+
+	cg->regmap = regmap;
+	cg->set_ofs = set_ofs;
+	cg->clr_ofs = clr_ofs;
+	cg->sta_ofs = sta_ofs;
+	cg->bit = bit;
+
+	cg->hw.init = &init;
+
+	clk = clk_register(NULL, &cg->hw);
+	if (IS_ERR(clk))
+		kfree(cg);
+
+	return clk;
+}
diff --git a/drivers/clk/mediatek/clk-gate.h b/drivers/clk/mediatek/clk-gate.h
new file mode 100644
index 000000000000..6b6780b1e9c5
--- /dev/null
+++ b/drivers/clk/mediatek/clk-gate.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: James Liao <jamesjj.liao@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.
+ */
+
+#ifndef __DRV_CLK_GATE_H
+#define __DRV_CLK_GATE_H
+
+#include <linux/regmap.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+
+struct mtk_clk_gate {
+	struct clk_hw	hw;
+	struct regmap	*regmap;
+	int		set_ofs;
+	int		clr_ofs;
+	int		sta_ofs;
+	u8		bit;
+};
+
+static inline struct mtk_clk_gate *to_clk_gate(struct clk_hw *hw)
+{
+	return container_of(hw, struct mtk_clk_gate, hw);
+}
+
+extern const struct clk_ops mtk_clk_gate_ops_setclr;
+extern const struct clk_ops mtk_clk_gate_ops_setclr_inv;
+
+struct clk *mtk_clk_register_gate(
+		const char *name,
+		const char *parent_name,
+		struct regmap *regmap,
+		int set_ofs,
+		int clr_ofs,
+		int sta_ofs,
+		u8 bit,
+		const struct clk_ops *ops);
+
+#endif /* __DRV_CLK_GATE_H */
diff --git a/drivers/clk/mediatek/clk-mt8135.c b/drivers/clk/mediatek/clk-mt8135.c
new file mode 100644
index 000000000000..08b4b849b491
--- /dev/null
+++ b/drivers/clk/mediatek/clk-mt8135.c
@@ -0,0 +1,644 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: James Liao <jamesjj.liao@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/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/mfd/syscon.h>
+#include <dt-bindings/clock/mt8135-clk.h>
+
+#include "clk-mtk.h"
+#include "clk-gate.h"
+
+static DEFINE_SPINLOCK(mt8135_clk_lock);
+
+static const struct mtk_fixed_factor root_clk_alias[] __initconst = {
+	FACTOR(CLK_TOP_DSI0_LNTC_DSICLK, "dsi0_lntc_dsiclk", "clk_null", 1, 1),
+	FACTOR(CLK_TOP_HDMITX_CLKDIG_CTS, "hdmitx_clkdig_cts", "clk_null", 1, 1),
+	FACTOR(CLK_TOP_CLKPH_MCK, "clkph_mck", "clk_null", 1, 1),
+	FACTOR(CLK_TOP_CPUM_TCK_IN, "cpum_tck_in", "clk_null", 1, 1),
+};
+
+static const struct mtk_fixed_factor top_divs[] __initconst = {
+	FACTOR(CLK_TOP_MAINPLL_806M, "mainpll_806m", "mainpll", 1, 2),
+	FACTOR(CLK_TOP_MAINPLL_537P3M, "mainpll_537p3m", "mainpll", 1, 3),
+	FACTOR(CLK_TOP_MAINPLL_322P4M, "mainpll_322p4m", "mainpll", 1, 5),
+	FACTOR(CLK_TOP_MAINPLL_230P3M, "mainpll_230p3m", "mainpll", 1, 7),
+
+	FACTOR(CLK_TOP_UNIVPLL_624M, "univpll_624m", "univpll", 1, 2),
+	FACTOR(CLK_TOP_UNIVPLL_416M, "univpll_416m", "univpll", 1, 3),
+	FACTOR(CLK_TOP_UNIVPLL_249P6M, "univpll_249p6m", "univpll", 1, 5),
+	FACTOR(CLK_TOP_UNIVPLL_178P3M, "univpll_178p3m", "univpll", 1, 7),
+	FACTOR(CLK_TOP_UNIVPLL_48M, "univpll_48m", "univpll", 1, 26),
+
+	FACTOR(CLK_TOP_MMPLL_D2, "mmpll_d2", "mmpll", 1, 2),
+	FACTOR(CLK_TOP_MMPLL_D3, "mmpll_d3", "mmpll", 1, 3),
+	FACTOR(CLK_TOP_MMPLL_D5, "mmpll_d5", "mmpll", 1, 5),
+	FACTOR(CLK_TOP_MMPLL_D7, "mmpll_d7", "mmpll", 1, 7),
+	FACTOR(CLK_TOP_MMPLL_D4, "mmpll_d4", "mmpll_d2", 1, 2),
+	FACTOR(CLK_TOP_MMPLL_D6, "mmpll_d6", "mmpll_d3", 1, 2),
+
+	FACTOR(CLK_TOP_SYSPLL_D2, "syspll_d2", "mainpll_806m", 1, 1),
+	FACTOR(CLK_TOP_SYSPLL_D4, "syspll_d4", "mainpll_806m", 1, 2),
+	FACTOR(CLK_TOP_SYSPLL_D6, "syspll_d6", "mainpll_806m", 1, 3),
+	FACTOR(CLK_TOP_SYSPLL_D8, "syspll_d8", "mainpll_806m", 1, 4),
+	FACTOR(CLK_TOP_SYSPLL_D10, "syspll_d10", "mainpll_806m", 1, 5),
+	FACTOR(CLK_TOP_SYSPLL_D12, "syspll_d12", "mainpll_806m", 1, 6),
+	FACTOR(CLK_TOP_SYSPLL_D16, "syspll_d16", "mainpll_806m", 1, 8),
+	FACTOR(CLK_TOP_SYSPLL_D24, "syspll_d24", "mainpll_806m", 1, 12),
+
+	FACTOR(CLK_TOP_SYSPLL_D3, "syspll_d3", "mainpll_537p3m", 1, 1),
+
+	FACTOR(CLK_TOP_SYSPLL_D2P5, "syspll_d2p5", "mainpll_322p4m", 2, 1),
+	FACTOR(CLK_TOP_SYSPLL_D5, "syspll_d5", "mainpll_322p4m", 1, 1),
+
+	FACTOR(CLK_TOP_SYSPLL_D3P5, "syspll_d3p5", "mainpll_230p3m", 2, 1),
+
+	FACTOR(CLK_TOP_UNIVPLL1_D2, "univpll1_d2", "univpll_624m", 1, 2),
+	FACTOR(CLK_TOP_UNIVPLL1_D4, "univpll1_d4", "univpll_624m", 1, 4),
+	FACTOR(CLK_TOP_UNIVPLL1_D6, "univpll1_d6", "univpll_624m", 1, 6),
+	FACTOR(CLK_TOP_UNIVPLL1_D8, "univpll1_d8", "univpll_624m", 1, 8),
+	FACTOR(CLK_TOP_UNIVPLL1_D10, "univpll1_d10", "univpll_624m", 1, 10),
+
+	FACTOR(CLK_TOP_UNIVPLL2_D2, "univpll2_d2", "univpll_416m", 1, 2),
+	FACTOR(CLK_TOP_UNIVPLL2_D4, "univpll2_d4", "univpll_416m", 1, 4),
+	FACTOR(CLK_TOP_UNIVPLL2_D6, "univpll2_d6", "univpll_416m", 1, 6),
+	FACTOR(CLK_TOP_UNIVPLL2_D8, "univpll2_d8", "univpll_416m", 1, 8),
+
+	FACTOR(CLK_TOP_UNIVPLL_D3, "univpll_d3", "univpll_416m", 1, 1),
+	FACTOR(CLK_TOP_UNIVPLL_D5, "univpll_d5", "univpll_249p6m", 1, 1),
+	FACTOR(CLK_TOP_UNIVPLL_D7, "univpll_d7", "univpll_178p3m", 1, 1),
+	FACTOR(CLK_TOP_UNIVPLL_D10, "univpll_d10", "univpll_249p6m", 1, 2),
+	FACTOR(CLK_TOP_UNIVPLL_D26, "univpll_d26", "univpll_48m", 1, 1),
+
+	FACTOR(CLK_TOP_APLL, "apll_ck", "audpll", 1, 1),
+	FACTOR(CLK_TOP_APLL_D4, "apll_d4", "audpll", 1, 4),
+	FACTOR(CLK_TOP_APLL_D8, "apll_d8", "audpll", 1, 8),
+	FACTOR(CLK_TOP_APLL_D16, "apll_d16", "audpll", 1, 16),
+	FACTOR(CLK_TOP_APLL_D24, "apll_d24", "audpll", 1, 24),
+
+	FACTOR(CLK_TOP_LVDSPLL_D2, "lvdspll_d2", "lvdspll", 1, 2),
+	FACTOR(CLK_TOP_LVDSPLL_D4, "lvdspll_d4", "lvdspll", 1, 4),
+	FACTOR(CLK_TOP_LVDSPLL_D8, "lvdspll_d8", "lvdspll", 1, 8),
+
+	FACTOR(CLK_TOP_LVDSTX_CLKDIG_CT, "lvdstx_clkdig_cts", "lvdspll", 1, 1),
+	FACTOR(CLK_TOP_VPLL_DPIX, "vpll_dpix_ck", "lvdspll", 1, 1),
+
+	FACTOR(CLK_TOP_TVHDMI_H, "tvhdmi_h_ck", "tvdpll", 1, 1),
+
+	FACTOR(CLK_TOP_HDMITX_CLKDIG_D2, "hdmitx_clkdig_d2", "hdmitx_clkdig_cts", 1, 2),
+	FACTOR(CLK_TOP_HDMITX_CLKDIG_D3, "hdmitx_clkdig_d3", "hdmitx_clkdig_cts", 1, 3),
+
+	FACTOR(CLK_TOP_TVHDMI_D2, "tvhdmi_d2", "tvhdmi_h_ck", 1, 2),
+	FACTOR(CLK_TOP_TVHDMI_D4, "tvhdmi_d4", "tvhdmi_h_ck", 1, 4),
+
+	FACTOR(CLK_TOP_MEMPLL_MCK_D4, "mempll_mck_d4", "clkph_mck", 1, 4),
+};
+
+static const char * const axi_parents[] __initconst = {
+	"clk26m",
+	"syspll_d3",
+	"syspll_d4",
+	"syspll_d6",
+	"univpll_d5",
+	"univpll2_d2",
+	"syspll_d3p5"
+};
+
+static const char * const smi_parents[] __initconst = {
+	"clk26m",
+	"clkph_mck",
+	"syspll_d2p5",
+	"syspll_d3",
+	"syspll_d8",
+	"univpll_d5",
+	"univpll1_d2",
+	"univpll1_d6",
+	"mmpll_d3",
+	"mmpll_d4",
+	"mmpll_d5",
+	"mmpll_d6",
+	"mmpll_d7",
+	"vdecpll",
+	"lvdspll"
+};
+
+static const char * const mfg_parents[] __initconst = {
+	"clk26m",
+	"univpll1_d4",
+	"syspll_d2",
+	"syspll_d2p5",
+	"syspll_d3",
+	"univpll_d5",
+	"univpll1_d2",
+	"mmpll_d2",
+	"mmpll_d3",
+	"mmpll_d4",
+	"mmpll_d5",
+	"mmpll_d6",
+	"mmpll_d7"
+};
+
+static const char * const irda_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d8",
+	"univpll1_d6"
+};
+
+static const char * const cam_parents[] __initconst = {
+	"clk26m",
+	"syspll_d3",
+	"syspll_d3p5",
+	"syspll_d4",
+	"univpll_d5",
+	"univpll2_d2",
+	"univpll_d7",
+	"univpll1_d4"
+};
+
+static const char * const aud_intbus_parents[] __initconst = {
+	"clk26m",
+	"syspll_d6",
+	"univpll_d10"
+};
+
+static const char * const jpg_parents[] __initconst = {
+	"clk26m",
+	"syspll_d5",
+	"syspll_d4",
+	"syspll_d3",
+	"univpll_d7",
+	"univpll2_d2",
+	"univpll_d5"
+};
+
+static const char * const disp_parents[] __initconst = {
+	"clk26m",
+	"syspll_d3p5",
+	"syspll_d3",
+	"univpll2_d2",
+	"univpll_d5",
+	"univpll1_d2",
+	"lvdspll",
+	"vdecpll"
+};
+
+static const char * const msdc30_parents[] __initconst = {
+	"clk26m",
+	"syspll_d6",
+	"syspll_d5",
+	"univpll1_d4",
+	"univpll2_d4",
+	"msdcpll"
+};
+
+static const char * const usb20_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d6",
+	"univpll1_d10"
+};
+
+static const char * const venc_parents[] __initconst = {
+	"clk26m",
+	"syspll_d3",
+	"syspll_d8",
+	"univpll_d5",
+	"univpll1_d6",
+	"mmpll_d4",
+	"mmpll_d5",
+	"mmpll_d6"
+};
+
+static const char * const spi_parents[] __initconst = {
+	"clk26m",
+	"syspll_d6",
+	"syspll_d8",
+	"syspll_d10",
+	"univpll1_d6",
+	"univpll1_d8"
+};
+
+static const char * const uart_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d8"
+};
+
+static const char * const mem_parents[] __initconst = {
+	"clk26m",
+	"clkph_mck"
+};
+
+static const char * const camtg_parents[] __initconst = {
+	"clk26m",
+	"univpll_d26",
+	"univpll1_d6",
+	"syspll_d16",
+	"syspll_d8"
+};
+
+static const char * const audio_parents[] __initconst = {
+	"clk26m",
+	"syspll_d24"
+};
+
+static const char * const fix_parents[] __initconst = {
+	"rtc32k",
+	"clk26m",
+	"univpll_d5",
+	"univpll_d7",
+	"univpll1_d2",
+	"univpll1_d4",
+	"univpll1_d6",
+	"univpll1_d8"
+};
+
+static const char * const vdec_parents[] __initconst = {
+	"clk26m",
+	"vdecpll",
+	"clkph_mck",
+	"syspll_d2p5",
+	"syspll_d3",
+	"syspll_d3p5",
+	"syspll_d4",
+	"syspll_d5",
+	"syspll_d6",
+	"syspll_d8",
+	"univpll1_d2",
+	"univpll2_d2",
+	"univpll_d7",
+	"univpll_d10",
+	"univpll2_d4",
+	"lvdspll"
+};
+
+static const char * const ddrphycfg_parents[] __initconst = {
+	"clk26m",
+	"axi_sel",
+	"syspll_d12"
+};
+
+static const char * const dpilvds_parents[] __initconst = {
+	"clk26m",
+	"lvdspll",
+	"lvdspll_d2",
+	"lvdspll_d4",
+	"lvdspll_d8"
+};
+
+static const char * const pmicspi_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d6",
+	"syspll_d8",
+	"syspll_d10",
+	"univpll1_d10",
+	"mempll_mck_d4",
+	"univpll_d26",
+	"syspll_d24"
+};
+
+static const char * const smi_mfg_as_parents[] __initconst = {
+	"clk26m",
+	"smi_sel",
+	"mfg_sel",
+	"mem_sel"
+};
+
+static const char * const gcpu_parents[] __initconst = {
+	"clk26m",
+	"syspll_d4",
+	"univpll_d7",
+	"syspll_d5",
+	"syspll_d6"
+};
+
+static const char * const dpi1_parents[] __initconst = {
+	"clk26m",
+	"tvhdmi_h_ck",
+	"tvhdmi_d2",
+	"tvhdmi_d4"
+};
+
+static const char * const cci_parents[] __initconst = {
+	"clk26m",
+	"mainpll_537p3m",
+	"univpll_d3",
+	"syspll_d2p5",
+	"syspll_d3",
+	"syspll_d5"
+};
+
+static const char * const apll_parents[] __initconst = {
+	"clk26m",
+	"apll_ck",
+	"apll_d4",
+	"apll_d8",
+	"apll_d16",
+	"apll_d24"
+};
+
+static const char * const hdmipll_parents[] __initconst = {
+	"clk26m",
+	"hdmitx_clkdig_cts",
+	"hdmitx_clkdig_d2",
+	"hdmitx_clkdig_d3"
+};
+
+static const struct mtk_composite top_muxes[] __initconst = {
+	/* CLK_CFG_0 */
+	MUX_GATE(CLK_TOP_AXI_SEL, "axi_sel", axi_parents,
+		0x0140, 0, 3, INVALID_MUX_GATE_BIT),
+	MUX_GATE(CLK_TOP_SMI_SEL, "smi_sel", smi_parents, 0x0140, 8, 4, 15),
+	MUX_GATE(CLK_TOP_MFG_SEL, "mfg_sel", mfg_parents, 0x0140, 16, 4, 23),
+	MUX_GATE(CLK_TOP_IRDA_SEL, "irda_sel", irda_parents, 0x0140, 24, 2, 31),
+	/* CLK_CFG_1 */
+	MUX_GATE(CLK_TOP_CAM_SEL, "cam_sel", cam_parents, 0x0144, 0, 3, 7),
+	MUX_GATE(CLK_TOP_AUD_INTBUS_SEL, "aud_intbus_sel", aud_intbus_parents,
+		0x0144, 8, 2, 15),
+	MUX_GATE(CLK_TOP_JPG_SEL, "jpg_sel", jpg_parents, 0x0144, 16, 3, 23),
+	MUX_GATE(CLK_TOP_DISP_SEL, "disp_sel", disp_parents, 0x0144, 24, 3, 31),
+	/* CLK_CFG_2 */
+	MUX_GATE(CLK_TOP_MSDC30_1_SEL, "msdc30_1_sel", msdc30_parents, 0x0148, 0, 3, 7),
+	MUX_GATE(CLK_TOP_MSDC30_2_SEL, "msdc30_2_sel", msdc30_parents, 0x0148, 8, 3, 15),
+	MUX_GATE(CLK_TOP_MSDC30_3_SEL, "msdc30_3_sel", msdc30_parents, 0x0148, 16, 3, 23),
+	MUX_GATE(CLK_TOP_MSDC30_4_SEL, "msdc30_4_sel", msdc30_parents, 0x0148, 24, 3, 31),
+	/* CLK_CFG_3 */
+	MUX_GATE(CLK_TOP_USB20_SEL, "usb20_sel", usb20_parents, 0x014c, 0, 2, 7),
+	/* CLK_CFG_4 */
+	MUX_GATE(CLK_TOP_VENC_SEL, "venc_sel", venc_parents, 0x0150, 8, 3, 15),
+	MUX_GATE(CLK_TOP_SPI_SEL, "spi_sel", spi_parents, 0x0150, 16, 3, 23),
+	MUX_GATE(CLK_TOP_UART_SEL, "uart_sel", uart_parents, 0x0150, 24, 2, 31),
+	/* CLK_CFG_6 */
+	MUX_GATE(CLK_TOP_MEM_SEL, "mem_sel", mem_parents, 0x0158, 0, 2, 7),
+	MUX_GATE(CLK_TOP_CAMTG_SEL, "camtg_sel", camtg_parents, 0x0158, 8, 3, 15),
+	MUX_GATE(CLK_TOP_AUDIO_SEL, "audio_sel", audio_parents, 0x0158, 24, 2, 31),
+	/* CLK_CFG_7 */
+	MUX_GATE(CLK_TOP_FIX_SEL, "fix_sel", fix_parents, 0x015c, 0, 3, 7),
+	MUX_GATE(CLK_TOP_VDEC_SEL, "vdec_sel", vdec_parents, 0x015c, 8, 4, 15),
+	MUX_GATE(CLK_TOP_DDRPHYCFG_SEL, "ddrphycfg_sel", ddrphycfg_parents,
+		0x015c, 16, 2, 23),
+	MUX_GATE(CLK_TOP_DPILVDS_SEL, "dpilvds_sel", dpilvds_parents, 0x015c, 24, 3, 31),
+	/* CLK_CFG_8 */
+	MUX_GATE(CLK_TOP_PMICSPI_SEL, "pmicspi_sel", pmicspi_parents, 0x0164, 0, 3, 7),
+	MUX_GATE(CLK_TOP_MSDC30_0_SEL, "msdc30_0_sel", msdc30_parents, 0x0164, 8, 3, 15),
+	MUX_GATE(CLK_TOP_SMI_MFG_AS_SEL, "smi_mfg_as_sel", smi_mfg_as_parents,
+		0x0164, 16, 2, 23),
+	MUX_GATE(CLK_TOP_GCPU_SEL, "gcpu_sel", gcpu_parents, 0x0164, 24, 3, 31),
+	/* CLK_CFG_9 */
+	MUX_GATE(CLK_TOP_DPI1_SEL, "dpi1_sel", dpi1_parents, 0x0168, 0, 2, 7),
+	MUX_GATE(CLK_TOP_CCI_SEL, "cci_sel", cci_parents, 0x0168, 8, 3, 15),
+	MUX_GATE(CLK_TOP_APLL_SEL, "apll_sel", apll_parents, 0x0168, 16, 3, 23),
+	MUX_GATE(CLK_TOP_HDMIPLL_SEL, "hdmipll_sel", hdmipll_parents, 0x0168, 24, 2, 31),
+};
+
+static const struct mtk_gate_regs infra_cg_regs = {
+	.set_ofs = 0x0040,
+	.clr_ofs = 0x0044,
+	.sta_ofs = 0x0048,
+};
+
+#define GATE_ICG(_id, _name, _parent, _shift) {	\
+		.id = _id,					\
+		.name = _name,					\
+		.parent_name = _parent,				\
+		.regs = &infra_cg_regs,				\
+		.shift = _shift,				\
+		.ops = &mtk_clk_gate_ops_setclr,		\
+	}
+
+static const struct mtk_gate infra_clks[] __initconst = {
+	GATE_ICG(CLK_INFRA_PMIC_WRAP, "pmic_wrap_ck", "axi_sel", 23),
+	GATE_ICG(CLK_INFRA_PMICSPI, "pmicspi_ck", "pmicspi_sel", 22),
+	GATE_ICG(CLK_INFRA_CCIF1_AP_CTRL, "ccif1_ap_ctrl", "axi_sel", 21),
+	GATE_ICG(CLK_INFRA_CCIF0_AP_CTRL, "ccif0_ap_ctrl", "axi_sel", 20),
+	GATE_ICG(CLK_INFRA_KP, "kp_ck", "axi_sel", 16),
+	GATE_ICG(CLK_INFRA_CPUM, "cpum_ck", "cpum_tck_in", 15),
+	GATE_ICG(CLK_INFRA_M4U, "m4u_ck", "mem_sel", 8),
+	GATE_ICG(CLK_INFRA_MFGAXI, "mfgaxi_ck", "axi_sel", 7),
+	GATE_ICG(CLK_INFRA_DEVAPC, "devapc_ck", "axi_sel", 6),
+	GATE_ICG(CLK_INFRA_AUDIO, "audio_ck", "aud_intbus_sel", 5),
+	GATE_ICG(CLK_INFRA_MFG_BUS, "mfg_bus_ck", "axi_sel", 2),
+	GATE_ICG(CLK_INFRA_SMI, "smi_ck", "smi_sel", 1),
+	GATE_ICG(CLK_INFRA_DBGCLK, "dbgclk_ck", "axi_sel", 0),
+};
+
+static const struct mtk_gate_regs peri0_cg_regs = {
+	.set_ofs = 0x0008,
+	.clr_ofs = 0x0010,
+	.sta_ofs = 0x0018,
+};
+
+static const struct mtk_gate_regs peri1_cg_regs = {
+	.set_ofs = 0x000c,
+	.clr_ofs = 0x0014,
+	.sta_ofs = 0x001c,
+};
+
+#define GATE_PERI0(_id, _name, _parent, _shift) {	\
+		.id = _id,					\
+		.name = _name,					\
+		.parent_name = _parent,				\
+		.regs = &peri0_cg_regs,				\
+		.shift = _shift,				\
+		.ops = &mtk_clk_gate_ops_setclr,		\
+	}
+
+#define GATE_PERI1(_id, _name, _parent, _shift) {	\
+		.id = _id,					\
+		.name = _name,					\
+		.parent_name = _parent,				\
+		.regs = &peri1_cg_regs,				\
+		.shift = _shift,				\
+		.ops = &mtk_clk_gate_ops_setclr,		\
+	}
+
+static const struct mtk_gate peri_gates[] __initconst = {
+	/* PERI0 */
+	GATE_PERI0(CLK_PERI_I2C5, "i2c5_ck", "axi_sel", 31),
+	GATE_PERI0(CLK_PERI_I2C4, "i2c4_ck", "axi_sel", 30),
+	GATE_PERI0(CLK_PERI_I2C3, "i2c3_ck", "axi_sel", 29),
+	GATE_PERI0(CLK_PERI_I2C2, "i2c2_ck", "axi_sel", 28),
+	GATE_PERI0(CLK_PERI_I2C1, "i2c1_ck", "axi_sel", 27),
+	GATE_PERI0(CLK_PERI_I2C0, "i2c0_ck", "axi_sel", 26),
+	GATE_PERI0(CLK_PERI_UART3, "uart3_ck", "axi_sel", 25),
+	GATE_PERI0(CLK_PERI_UART2, "uart2_ck", "axi_sel", 24),
+	GATE_PERI0(CLK_PERI_UART1, "uart1_ck", "axi_sel", 23),
+	GATE_PERI0(CLK_PERI_UART0, "uart0_ck", "axi_sel", 22),
+	GATE_PERI0(CLK_PERI_IRDA, "irda_ck", "irda_sel", 21),
+	GATE_PERI0(CLK_PERI_NLI, "nli_ck", "axi_sel", 20),
+	GATE_PERI0(CLK_PERI_MD_HIF, "md_hif_ck", "axi_sel", 19),
+	GATE_PERI0(CLK_PERI_AP_HIF, "ap_hif_ck", "axi_sel", 18),
+	GATE_PERI0(CLK_PERI_MSDC30_3, "msdc30_3_ck", "msdc30_4_sel", 17),
+	GATE_PERI0(CLK_PERI_MSDC30_2, "msdc30_2_ck", "msdc30_3_sel", 16),
+	GATE_PERI0(CLK_PERI_MSDC30_1, "msdc30_1_ck", "msdc30_2_sel", 15),
+	GATE_PERI0(CLK_PERI_MSDC20_2, "msdc20_2_ck", "msdc30_1_sel", 14),
+	GATE_PERI0(CLK_PERI_MSDC20_1, "msdc20_1_ck", "msdc30_0_sel", 13),
+	GATE_PERI0(CLK_PERI_AP_DMA, "ap_dma_ck", "axi_sel", 12),
+	GATE_PERI0(CLK_PERI_USB1, "usb1_ck", "usb20_sel", 11),
+	GATE_PERI0(CLK_PERI_USB0, "usb0_ck", "usb20_sel", 10),
+	GATE_PERI0(CLK_PERI_PWM, "pwm_ck", "axi_sel", 9),
+	GATE_PERI0(CLK_PERI_PWM7, "pwm7_ck", "axi_sel", 8),
+	GATE_PERI0(CLK_PERI_PWM6, "pwm6_ck", "axi_sel", 7),
+	GATE_PERI0(CLK_PERI_PWM5, "pwm5_ck", "axi_sel", 6),
+	GATE_PERI0(CLK_PERI_PWM4, "pwm4_ck", "axi_sel", 5),
+	GATE_PERI0(CLK_PERI_PWM3, "pwm3_ck", "axi_sel", 4),
+	GATE_PERI0(CLK_PERI_PWM2, "pwm2_ck", "axi_sel", 3),
+	GATE_PERI0(CLK_PERI_PWM1, "pwm1_ck", "axi_sel", 2),
+	GATE_PERI0(CLK_PERI_THERM, "therm_ck", "axi_sel", 1),
+	GATE_PERI0(CLK_PERI_NFI, "nfi_ck", "axi_sel", 0),
+	/* PERI1 */
+	GATE_PERI1(CLK_PERI_USBSLV, "usbslv_ck", "axi_sel", 8),
+	GATE_PERI1(CLK_PERI_USB1_MCU, "usb1_mcu_ck", "axi_sel", 7),
+	GATE_PERI1(CLK_PERI_USB0_MCU, "usb0_mcu_ck", "axi_sel", 6),
+	GATE_PERI1(CLK_PERI_GCPU, "gcpu_ck", "gcpu_sel", 5),
+	GATE_PERI1(CLK_PERI_FHCTL, "fhctl_ck", "clk26m", 4),
+	GATE_PERI1(CLK_PERI_SPI1, "spi1_ck", "spi_sel", 3),
+	GATE_PERI1(CLK_PERI_AUXADC, "auxadc_ck", "clk26m", 2),
+	GATE_PERI1(CLK_PERI_PERI_PWRAP, "peri_pwrap_ck", "axi_sel", 1),
+	GATE_PERI1(CLK_PERI_I2C6, "i2c6_ck", "axi_sel", 0),
+};
+
+static const char * const uart_ck_sel_parents[] __initconst = {
+	"clk26m",
+	"uart_sel",
+};
+
+static const struct mtk_composite peri_clks[] __initconst = {
+	MUX(CLK_PERI_UART0_SEL, "uart0_ck_sel", uart_ck_sel_parents, 0x40c, 0, 1),
+	MUX(CLK_PERI_UART1_SEL, "uart1_ck_sel", uart_ck_sel_parents, 0x40c, 1, 1),
+	MUX(CLK_PERI_UART2_SEL, "uart2_ck_sel", uart_ck_sel_parents, 0x40c, 2, 1),
+	MUX(CLK_PERI_UART3_SEL, "uart3_ck_sel", uart_ck_sel_parents, 0x40c, 3, 1),
+};
+
+static void __init mtk_topckgen_init(struct device_node *node)
+{
+	struct clk_onecell_data *clk_data;
+	void __iomem *base;
+	int r;
+
+	base = of_iomap(node, 0);
+	if (!base) {
+		pr_err("%s(): ioremap failed\n", __func__);
+		return;
+	}
+
+	clk_data = mtk_alloc_clk_data(CLK_TOP_NR_CLK);
+
+	mtk_clk_register_factors(root_clk_alias, ARRAY_SIZE(root_clk_alias), clk_data);
+	mtk_clk_register_factors(top_divs, ARRAY_SIZE(top_divs), clk_data);
+	mtk_clk_register_composites(top_muxes, ARRAY_SIZE(top_muxes), base,
+			&mt8135_clk_lock, clk_data);
+
+	clk_prepare_enable(clk_data->clks[CLK_TOP_CCI_SEL]);
+
+	r = of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+	if (r)
+		pr_err("%s(): could not register clock provider: %d\n",
+			__func__, r);
+}
+CLK_OF_DECLARE(mtk_topckgen, "mediatek,mt8135-topckgen", mtk_topckgen_init);
+
+static void __init mtk_infrasys_init(struct device_node *node)
+{
+	struct clk_onecell_data *clk_data;
+	int r;
+
+	clk_data = mtk_alloc_clk_data(CLK_INFRA_NR_CLK);
+
+	mtk_clk_register_gates(node, infra_clks, ARRAY_SIZE(infra_clks),
+						clk_data);
+
+	clk_prepare_enable(clk_data->clks[CLK_INFRA_M4U]);
+
+	r = of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+	if (r)
+		pr_err("%s(): could not register clock provider: %d\n",
+			__func__, r);
+
+	mtk_register_reset_controller(node, 2, 0x30);
+}
+CLK_OF_DECLARE(mtk_infrasys, "mediatek,mt8135-infracfg", mtk_infrasys_init);
+
+static void __init mtk_pericfg_init(struct device_node *node)
+{
+	struct clk_onecell_data *clk_data;
+	int r;
+	void __iomem *base;
+
+	base = of_iomap(node, 0);
+	if (!base) {
+		pr_err("%s(): ioremap failed\n", __func__);
+		return;
+	}
+
+	clk_data = mtk_alloc_clk_data(CLK_PERI_NR_CLK);
+
+	mtk_clk_register_gates(node, peri_gates, ARRAY_SIZE(peri_gates),
+						clk_data);
+	mtk_clk_register_composites(peri_clks, ARRAY_SIZE(peri_clks), base,
+			&mt8135_clk_lock, clk_data);
+
+	r = of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+	if (r)
+		pr_err("%s(): could not register clock provider: %d\n",
+			__func__, r);
+
+	mtk_register_reset_controller(node, 2, 0);
+}
+CLK_OF_DECLARE(mtk_pericfg, "mediatek,mt8135-pericfg", mtk_pericfg_init);
+
+#define MT8135_PLL_FMAX		(2000 * MHZ)
+#define CON0_MT8135_RST_BAR	BIT(27)
+
+#define PLL(_id, _name, _reg, _pwr_reg, _en_mask, _flags, _pcwbits, _pd_reg, _pd_shift, _tuner_reg, _pcw_reg, _pcw_shift) { \
+		.id = _id,						\
+		.name = _name,						\
+		.reg = _reg,						\
+		.pwr_reg = _pwr_reg,					\
+		.en_mask = _en_mask,					\
+		.flags = _flags,					\
+		.rst_bar_mask = CON0_MT8135_RST_BAR,			\
+		.fmax = MT8135_PLL_FMAX,				\
+		.pcwbits = _pcwbits,					\
+		.pd_reg = _pd_reg,					\
+		.pd_shift = _pd_shift,					\
+		.tuner_reg = _tuner_reg,				\
+		.pcw_reg = _pcw_reg,					\
+		.pcw_shift = _pcw_shift,				\
+	}
+
+static const struct mtk_pll_data plls[] = {
+	PLL(CLK_APMIXED_ARMPLL1, "armpll1", 0x200, 0x218, 0x80000001, 0, 21, 0x204, 24, 0x0, 0x204, 0),
+	PLL(CLK_APMIXED_ARMPLL2, "armpll2", 0x2cc, 0x2e4, 0x80000001, 0, 21, 0x2d0, 24, 0x0, 0x2d0, 0),
+	PLL(CLK_APMIXED_MAINPLL, "mainpll", 0x21c, 0x234, 0xf0000001, HAVE_RST_BAR, 21, 0x21c, 6, 0x0, 0x220, 0),
+	PLL(CLK_APMIXED_UNIVPLL, "univpll", 0x238, 0x250, 0xf3000001, HAVE_RST_BAR, 7, 0x238, 6, 0x0, 0x238, 9),
+	PLL(CLK_APMIXED_MMPLL, "mmpll", 0x254, 0x26c, 0xf0000001, HAVE_RST_BAR, 21, 0x254, 6, 0x0, 0x258, 0),
+	PLL(CLK_APMIXED_MSDCPLL, "msdcpll", 0x278, 0x290, 0x80000001, 0, 21, 0x278, 6, 0x0, 0x27c, 0),
+	PLL(CLK_APMIXED_TVDPLL, "tvdpll", 0x294, 0x2ac, 0x80000001, 0, 31, 0x294, 6, 0x0, 0x298, 0),
+	PLL(CLK_APMIXED_LVDSPLL, "lvdspll", 0x2b0, 0x2c8,	0x80000001, 0, 21, 0x2b0, 6, 0x0, 0x2b4, 0),
+	PLL(CLK_APMIXED_AUDPLL, "audpll", 0x2e8, 0x300, 0x80000001, 0, 31, 0x2e8, 6, 0x2f8, 0x2ec, 0),
+	PLL(CLK_APMIXED_VDECPLL, "vdecpll", 0x304, 0x31c,	0x80000001, 0, 21, 0x2b0, 6, 0x0, 0x308, 0),
+};
+
+static void __init mtk_apmixedsys_init(struct device_node *node)
+{
+	struct clk_onecell_data *clk_data;
+
+	clk_data = mtk_alloc_clk_data(CLK_APMIXED_NR_CLK);
+	if (!clk_data)
+		return;
+
+	mtk_clk_register_plls(node, plls, ARRAY_SIZE(plls), clk_data);
+}
+CLK_OF_DECLARE(mtk_apmixedsys, "mediatek,mt8135-apmixedsys",
+		mtk_apmixedsys_init);
diff --git a/drivers/clk/mediatek/clk-mt8173.c b/drivers/clk/mediatek/clk-mt8173.c
new file mode 100644
index 000000000000..8b6523d15fb8
--- /dev/null
+++ b/drivers/clk/mediatek/clk-mt8173.c
@@ -0,0 +1,846 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: James Liao <jamesjj.liao@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/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/mfd/syscon.h>
+
+#include "clk-mtk.h"
+#include "clk-gate.h"
+
+#include <dt-bindings/clock/mt8173-clk.h>
+
+static DEFINE_SPINLOCK(mt8173_clk_lock);
+
+static const struct mtk_fixed_factor root_clk_alias[] __initconst = {
+	FACTOR(CLK_TOP_CLKPH_MCK_O, "clkph_mck_o", "clk_null", 1, 1),
+	FACTOR(CLK_TOP_DPI, "dpi_ck", "clk_null", 1, 1),
+	FACTOR(CLK_TOP_USB_SYSPLL_125M, "usb_syspll_125m", "clk_null", 1, 1),
+	FACTOR(CLK_TOP_HDMITX_DIG_CTS, "hdmitx_dig_cts", "clk_null", 1, 1),
+};
+
+static const struct mtk_fixed_factor top_divs[] __initconst = {
+	FACTOR(CLK_TOP_ARMCA7PLL_754M, "armca7pll_754m", "armca7pll", 1, 2),
+	FACTOR(CLK_TOP_ARMCA7PLL_502M, "armca7pll_502m", "armca7pll", 1, 3),
+
+	FACTOR(CLK_TOP_MAIN_H546M, "main_h546m", "mainpll", 1, 2),
+	FACTOR(CLK_TOP_MAIN_H364M, "main_h364m", "mainpll", 1, 3),
+	FACTOR(CLK_TOP_MAIN_H218P4M, "main_h218p4m", "mainpll", 1, 5),
+	FACTOR(CLK_TOP_MAIN_H156M, "main_h156m", "mainpll", 1, 7),
+
+	FACTOR(CLK_TOP_TVDPLL_445P5M, "tvdpll_445p5m", "tvdpll", 1, 4),
+	FACTOR(CLK_TOP_TVDPLL_594M, "tvdpll_594m", "tvdpll", 1, 3),
+
+	FACTOR(CLK_TOP_UNIV_624M, "univ_624m", "univpll", 1, 2),
+	FACTOR(CLK_TOP_UNIV_416M, "univ_416m", "univpll", 1, 3),
+	FACTOR(CLK_TOP_UNIV_249P6M, "univ_249p6m", "univpll", 1, 5),
+	FACTOR(CLK_TOP_UNIV_178P3M, "univ_178p3m", "univpll", 1, 7),
+	FACTOR(CLK_TOP_UNIV_48M, "univ_48m", "univpll", 1, 26),
+
+	FACTOR(CLK_TOP_CLKRTC_EXT, "clkrtc_ext", "clk32k", 1, 1),
+	FACTOR(CLK_TOP_CLKRTC_INT, "clkrtc_int", "clk26m", 1, 793),
+	FACTOR(CLK_TOP_FPC, "fpc_ck", "clk26m", 1, 1),
+
+	FACTOR(CLK_TOP_HDMITXPLL_D2, "hdmitxpll_d2", "hdmitx_dig_cts", 1, 2),
+	FACTOR(CLK_TOP_HDMITXPLL_D3, "hdmitxpll_d3", "hdmitx_dig_cts", 1, 3),
+
+	FACTOR(CLK_TOP_ARMCA7PLL_D2, "armca7pll_d2", "armca7pll_754m", 1, 1),
+	FACTOR(CLK_TOP_ARMCA7PLL_D3, "armca7pll_d3", "armca7pll_502m", 1, 1),
+
+	FACTOR(CLK_TOP_APLL1, "apll1_ck", "apll1", 1, 1),
+	FACTOR(CLK_TOP_APLL2, "apll2_ck", "apll2", 1, 1),
+
+	FACTOR(CLK_TOP_DMPLL, "dmpll_ck", "clkph_mck_o", 1, 1),
+	FACTOR(CLK_TOP_DMPLL_D2, "dmpll_d2", "clkph_mck_o", 1, 2),
+	FACTOR(CLK_TOP_DMPLL_D4, "dmpll_d4", "clkph_mck_o", 1, 4),
+	FACTOR(CLK_TOP_DMPLL_D8, "dmpll_d8", "clkph_mck_o", 1, 8),
+	FACTOR(CLK_TOP_DMPLL_D16, "dmpll_d16", "clkph_mck_o", 1, 16),
+
+	FACTOR(CLK_TOP_LVDSPLL_D2, "lvdspll_d2", "lvdspll", 1, 2),
+	FACTOR(CLK_TOP_LVDSPLL_D4, "lvdspll_d4", "lvdspll", 1, 4),
+	FACTOR(CLK_TOP_LVDSPLL_D8, "lvdspll_d8", "lvdspll", 1, 8),
+
+	FACTOR(CLK_TOP_MMPLL, "mmpll_ck", "mmpll", 1, 1),
+	FACTOR(CLK_TOP_MMPLL_D2, "mmpll_d2", "mmpll", 1, 2),
+
+	FACTOR(CLK_TOP_MSDCPLL, "msdcpll_ck", "msdcpll", 1, 1),
+	FACTOR(CLK_TOP_MSDCPLL_D2, "msdcpll_d2", "msdcpll", 1, 2),
+	FACTOR(CLK_TOP_MSDCPLL_D4, "msdcpll_d4", "msdcpll", 1, 4),
+	FACTOR(CLK_TOP_MSDCPLL2, "msdcpll2_ck", "msdcpll2", 1, 1),
+	FACTOR(CLK_TOP_MSDCPLL2_D2, "msdcpll2_d2", "msdcpll2", 1, 2),
+	FACTOR(CLK_TOP_MSDCPLL2_D4, "msdcpll2_d4", "msdcpll2", 1, 4),
+
+	FACTOR(CLK_TOP_SYSPLL_D2, "syspll_d2", "main_h546m", 1, 1),
+	FACTOR(CLK_TOP_SYSPLL1_D2, "syspll1_d2", "main_h546m", 1, 2),
+	FACTOR(CLK_TOP_SYSPLL1_D4, "syspll1_d4", "main_h546m", 1, 4),
+	FACTOR(CLK_TOP_SYSPLL1_D8, "syspll1_d8", "main_h546m", 1, 8),
+	FACTOR(CLK_TOP_SYSPLL1_D16, "syspll1_d16", "main_h546m", 1, 16),
+	FACTOR(CLK_TOP_SYSPLL_D3, "syspll_d3", "main_h364m", 1, 1),
+	FACTOR(CLK_TOP_SYSPLL2_D2, "syspll2_d2", "main_h364m", 1, 2),
+	FACTOR(CLK_TOP_SYSPLL2_D4, "syspll2_d4", "main_h364m", 1, 4),
+	FACTOR(CLK_TOP_SYSPLL_D5, "syspll_d5", "main_h218p4m", 1, 1),
+	FACTOR(CLK_TOP_SYSPLL3_D2, "syspll3_d2", "main_h218p4m", 1, 2),
+	FACTOR(CLK_TOP_SYSPLL3_D4, "syspll3_d4", "main_h218p4m", 1, 4),
+	FACTOR(CLK_TOP_SYSPLL_D7, "syspll_d7", "main_h156m", 1, 1),
+	FACTOR(CLK_TOP_SYSPLL4_D2, "syspll4_d2", "main_h156m", 1, 2),
+	FACTOR(CLK_TOP_SYSPLL4_D4, "syspll4_d4", "main_h156m", 1, 4),
+
+	FACTOR(CLK_TOP_TVDPLL, "tvdpll_ck", "tvdpll_594m", 1, 1),
+	FACTOR(CLK_TOP_TVDPLL_D2, "tvdpll_d2", "tvdpll_594m", 1, 2),
+	FACTOR(CLK_TOP_TVDPLL_D4, "tvdpll_d4", "tvdpll_594m", 1, 4),
+	FACTOR(CLK_TOP_TVDPLL_D8, "tvdpll_d8", "tvdpll_594m", 1, 8),
+	FACTOR(CLK_TOP_TVDPLL_D16, "tvdpll_d16", "tvdpll_594m", 1, 16),
+
+	FACTOR(CLK_TOP_UNIVPLL_D2, "univpll_d2", "univ_624m", 1, 1),
+	FACTOR(CLK_TOP_UNIVPLL1_D2, "univpll1_d2", "univ_624m", 1, 2),
+	FACTOR(CLK_TOP_UNIVPLL1_D4, "univpll1_d4", "univ_624m", 1, 4),
+	FACTOR(CLK_TOP_UNIVPLL1_D8, "univpll1_d8", "univ_624m", 1, 8),
+	FACTOR(CLK_TOP_UNIVPLL_D3, "univpll_d3", "univ_416m", 1, 1),
+	FACTOR(CLK_TOP_UNIVPLL2_D2, "univpll2_d2", "univ_416m", 1, 2),
+	FACTOR(CLK_TOP_UNIVPLL2_D4, "univpll2_d4", "univ_416m", 1, 4),
+	FACTOR(CLK_TOP_UNIVPLL2_D8, "univpll2_d8", "univ_416m", 1, 8),
+	FACTOR(CLK_TOP_UNIVPLL_D5, "univpll_d5", "univ_249p6m", 1, 1),
+	FACTOR(CLK_TOP_UNIVPLL3_D2, "univpll3_d2", "univ_249p6m", 1, 2),
+	FACTOR(CLK_TOP_UNIVPLL3_D4, "univpll3_d4", "univ_249p6m", 1, 4),
+	FACTOR(CLK_TOP_UNIVPLL3_D8, "univpll3_d8", "univ_249p6m", 1, 8),
+	FACTOR(CLK_TOP_UNIVPLL_D7, "univpll_d7", "univ_178p3m", 1, 1),
+	FACTOR(CLK_TOP_UNIVPLL_D26, "univpll_d26", "univ_48m", 1, 1),
+	FACTOR(CLK_TOP_UNIVPLL_D52, "univpll_d52", "univ_48m", 1, 2),
+
+	FACTOR(CLK_TOP_VCODECPLL, "vcodecpll_ck", "vcodecpll", 1, 3),
+	FACTOR(CLK_TOP_VCODECPLL_370P5, "vcodecpll_370p5", "vcodecpll", 1, 4),
+
+	FACTOR(CLK_TOP_VENCPLL, "vencpll_ck", "vencpll", 1, 1),
+	FACTOR(CLK_TOP_VENCPLL_D2, "vencpll_d2", "vencpll", 1, 2),
+	FACTOR(CLK_TOP_VENCPLL_D4, "vencpll_d4", "vencpll", 1, 4),
+};
+
+static const char * const axi_parents[] __initconst = {
+	"clk26m",
+	"syspll1_d2",
+	"syspll_d5",
+	"syspll1_d4",
+	"univpll_d5",
+	"univpll2_d2",
+	"dmpll_d2",
+	"dmpll_d4"
+};
+
+static const char * const mem_parents[] __initconst = {
+	"clk26m",
+	"dmpll_ck"
+};
+
+static const char * const ddrphycfg_parents[] __initconst = {
+	"clk26m",
+	"syspll1_d8"
+};
+
+static const char * const mm_parents[] __initconst = {
+	"clk26m",
+	"vencpll_d2",
+	"main_h364m",
+	"syspll1_d2",
+	"syspll_d5",
+	"syspll1_d4",
+	"univpll1_d2",
+	"univpll2_d2",
+	"dmpll_d2"
+};
+
+static const char * const pwm_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d4",
+	"univpll3_d2",
+	"univpll1_d4"
+};
+
+static const char * const vdec_parents[] __initconst = {
+	"clk26m",
+	"vcodecpll_ck",
+	"tvdpll_445p5m",
+	"univpll_d3",
+	"vencpll_d2",
+	"syspll_d3",
+	"univpll1_d2",
+	"mmpll_d2",
+	"dmpll_d2",
+	"dmpll_d4"
+};
+
+static const char * const venc_parents[] __initconst = {
+	"clk26m",
+	"vcodecpll_ck",
+	"tvdpll_445p5m",
+	"univpll_d3",
+	"vencpll_d2",
+	"syspll_d3",
+	"univpll1_d2",
+	"univpll2_d2",
+	"dmpll_d2",
+	"dmpll_d4"
+};
+
+static const char * const mfg_parents[] __initconst = {
+	"clk26m",
+	"mmpll_ck",
+	"dmpll_ck",
+	"clk26m",
+	"clk26m",
+	"clk26m",
+	"clk26m",
+	"clk26m",
+	"clk26m",
+	"syspll_d3",
+	"syspll1_d2",
+	"syspll_d5",
+	"univpll_d3",
+	"univpll1_d2",
+	"univpll_d5",
+	"univpll2_d2"
+};
+
+static const char * const camtg_parents[] __initconst = {
+	"clk26m",
+	"univpll_d26",
+	"univpll2_d2",
+	"syspll3_d2",
+	"syspll3_d4",
+	"univpll1_d4"
+};
+
+static const char * const uart_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d8"
+};
+
+static const char * const spi_parents[] __initconst = {
+	"clk26m",
+	"syspll3_d2",
+	"syspll1_d4",
+	"syspll4_d2",
+	"univpll3_d2",
+	"univpll2_d4",
+	"univpll1_d8"
+};
+
+static const char * const usb20_parents[] __initconst = {
+	"clk26m",
+	"univpll1_d8",
+	"univpll3_d4"
+};
+
+static const char * const usb30_parents[] __initconst = {
+	"clk26m",
+	"univpll3_d2",
+	"usb_syspll_125m",
+	"univpll2_d4"
+};
+
+static const char * const msdc50_0_h_parents[] __initconst = {
+	"clk26m",
+	"syspll1_d2",
+	"syspll2_d2",
+	"syspll4_d2",
+	"univpll_d5",
+	"univpll1_d4"
+};
+
+static const char * const msdc50_0_parents[] __initconst = {
+	"clk26m",
+	"msdcpll_ck",
+	"msdcpll_d2",
+	"univpll1_d4",
+	"syspll2_d2",
+	"syspll_d7",
+	"msdcpll_d4",
+	"vencpll_d4",
+	"tvdpll_ck",
+	"univpll_d2",
+	"univpll1_d2",
+	"mmpll_ck",
+	"msdcpll2_ck",
+	"msdcpll2_d2",
+	"msdcpll2_d4"
+};
+
+static const char * const msdc30_1_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d2",
+	"msdcpll_d4",
+	"univpll1_d4",
+	"syspll2_d2",
+	"syspll_d7",
+	"univpll_d7",
+	"vencpll_d4"
+};
+
+static const char * const msdc30_2_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d2",
+	"msdcpll_d4",
+	"univpll1_d4",
+	"syspll2_d2",
+	"syspll_d7",
+	"univpll_d7",
+	"vencpll_d2"
+};
+
+static const char * const msdc30_3_parents[] __initconst = {
+	"clk26m",
+	"msdcpll2_ck",
+	"msdcpll2_d2",
+	"univpll2_d2",
+	"msdcpll2_d4",
+	"msdcpll_d4",
+	"univpll1_d4",
+	"syspll2_d2",
+	"syspll_d7",
+	"univpll_d7",
+	"vencpll_d4",
+	"msdcpll_ck",
+	"msdcpll_d2",
+	"msdcpll_d4"
+};
+
+static const char * const audio_parents[] __initconst = {
+	"clk26m",
+	"syspll3_d4",
+	"syspll4_d4",
+	"syspll1_d16"
+};
+
+static const char * const aud_intbus_parents[] __initconst = {
+	"clk26m",
+	"syspll1_d4",
+	"syspll4_d2",
+	"univpll3_d2",
+	"univpll2_d8",
+	"dmpll_d4",
+	"dmpll_d8"
+};
+
+static const char * const pmicspi_parents[] __initconst = {
+	"clk26m",
+	"syspll1_d8",
+	"syspll3_d4",
+	"syspll1_d16",
+	"univpll3_d4",
+	"univpll_d26",
+	"dmpll_d8",
+	"dmpll_d16"
+};
+
+static const char * const scp_parents[] __initconst = {
+	"clk26m",
+	"syspll1_d2",
+	"univpll_d5",
+	"syspll_d5",
+	"dmpll_d2",
+	"dmpll_d4"
+};
+
+static const char * const atb_parents[] __initconst = {
+	"clk26m",
+	"syspll1_d2",
+	"univpll_d5",
+	"dmpll_d2"
+};
+
+static const char * const venc_lt_parents[] __initconst = {
+	"clk26m",
+	"univpll_d3",
+	"vcodecpll_ck",
+	"tvdpll_445p5m",
+	"vencpll_d2",
+	"syspll_d3",
+	"univpll1_d2",
+	"univpll2_d2",
+	"syspll1_d2",
+	"univpll_d5",
+	"vcodecpll_370p5",
+	"dmpll_ck"
+};
+
+static const char * const dpi0_parents[] __initconst = {
+	"clk26m",
+	"tvdpll_d2",
+	"tvdpll_d4",
+	"clk26m",
+	"clk26m",
+	"tvdpll_d8",
+	"tvdpll_d16"
+};
+
+static const char * const irda_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d4",
+	"syspll2_d4"
+};
+
+static const char * const cci400_parents[] __initconst = {
+	"clk26m",
+	"vencpll_ck",
+	"armca7pll_754m",
+	"armca7pll_502m",
+	"univpll_d2",
+	"syspll_d2",
+	"msdcpll_ck",
+	"dmpll_ck"
+};
+
+static const char * const aud_1_parents[] __initconst = {
+	"clk26m",
+	"apll1_ck",
+	"univpll2_d4",
+	"univpll2_d8"
+};
+
+static const char * const aud_2_parents[] __initconst = {
+	"clk26m",
+	"apll2_ck",
+	"univpll2_d4",
+	"univpll2_d8"
+};
+
+static const char * const mem_mfg_in_parents[] __initconst = {
+	"clk26m",
+	"mmpll_ck",
+	"dmpll_ck",
+	"clk26m"
+};
+
+static const char * const axi_mfg_in_parents[] __initconst = {
+	"clk26m",
+	"axi_sel",
+	"dmpll_d2"
+};
+
+static const char * const scam_parents[] __initconst = {
+	"clk26m",
+	"syspll3_d2",
+	"univpll2_d4",
+	"dmpll_d4"
+};
+
+static const char * const spinfi_ifr_parents[] __initconst = {
+	"clk26m",
+	"univpll2_d8",
+	"univpll3_d4",
+	"syspll4_d2",
+	"univpll2_d4",
+	"univpll3_d2",
+	"syspll1_d4",
+	"univpll1_d4"
+};
+
+static const char * const hdmi_parents[] __initconst = {
+	"clk26m",
+	"hdmitx_dig_cts",
+	"hdmitxpll_d2",
+	"hdmitxpll_d3"
+};
+
+static const char * const dpilvds_parents[] __initconst = {
+	"clk26m",
+	"lvdspll",
+	"lvdspll_d2",
+	"lvdspll_d4",
+	"lvdspll_d8",
+	"fpc_ck"
+};
+
+static const char * const msdc50_2_h_parents[] __initconst = {
+	"clk26m",
+	"syspll1_d2",
+	"syspll2_d2",
+	"syspll4_d2",
+	"univpll_d5",
+	"univpll1_d4"
+};
+
+static const char * const hdcp_parents[] __initconst = {
+	"clk26m",
+	"syspll4_d2",
+	"syspll3_d4",
+	"univpll2_d4"
+};
+
+static const char * const hdcp_24m_parents[] __initconst = {
+	"clk26m",
+	"univpll_d26",
+	"univpll_d52",
+	"univpll2_d8"
+};
+
+static const char * const rtc_parents[] __initconst = {
+	"clkrtc_int",
+	"clkrtc_ext",
+	"clk26m",
+	"univpll3_d8"
+};
+
+static const char * const i2s0_m_ck_parents[] __initconst = {
+	"apll1_div1",
+	"apll2_div1"
+};
+
+static const char * const i2s1_m_ck_parents[] __initconst = {
+	"apll1_div2",
+	"apll2_div2"
+};
+
+static const char * const i2s2_m_ck_parents[] __initconst = {
+	"apll1_div3",
+	"apll2_div3"
+};
+
+static const char * const i2s3_m_ck_parents[] __initconst = {
+	"apll1_div4",
+	"apll2_div4"
+};
+
+static const char * const i2s3_b_ck_parents[] __initconst = {
+	"apll1_div5",
+	"apll2_div5"
+};
+
+static const struct mtk_composite top_muxes[] __initconst = {
+	/* CLK_CFG_0 */
+	MUX(CLK_TOP_AXI_SEL, "axi_sel", axi_parents, 0x0040, 0, 3),
+	MUX(CLK_TOP_MEM_SEL, "mem_sel", mem_parents, 0x0040, 8, 1),
+	MUX_GATE(CLK_TOP_DDRPHYCFG_SEL, "ddrphycfg_sel", ddrphycfg_parents, 0x0040, 16, 1, 23),
+	MUX_GATE(CLK_TOP_MM_SEL, "mm_sel", mm_parents, 0x0040, 24, 4, 31),
+	/* CLK_CFG_1 */
+	MUX_GATE(CLK_TOP_PWM_SEL, "pwm_sel", pwm_parents, 0x0050, 0, 2, 7),
+	MUX_GATE(CLK_TOP_VDEC_SEL, "vdec_sel", vdec_parents, 0x0050, 8, 4, 15),
+	MUX_GATE(CLK_TOP_VENC_SEL, "venc_sel", venc_parents, 0x0050, 16, 4, 23),
+	MUX_GATE(CLK_TOP_MFG_SEL, "mfg_sel", mfg_parents, 0x0050, 24, 4, 31),
+	/* CLK_CFG_2 */
+	MUX_GATE(CLK_TOP_CAMTG_SEL, "camtg_sel", camtg_parents, 0x0060, 0, 3, 7),
+	MUX_GATE(CLK_TOP_UART_SEL, "uart_sel", uart_parents, 0x0060, 8, 1, 15),
+	MUX_GATE(CLK_TOP_SPI_SEL, "spi_sel", spi_parents, 0x0060, 16, 3, 23),
+	MUX_GATE(CLK_TOP_USB20_SEL, "usb20_sel", usb20_parents, 0x0060, 24, 2, 31),
+	/* CLK_CFG_3 */
+	MUX_GATE(CLK_TOP_USB30_SEL, "usb30_sel", usb30_parents, 0x0070, 0, 2, 7),
+	MUX_GATE(CLK_TOP_MSDC50_0_H_SEL, "msdc50_0_h_sel", msdc50_0_h_parents, 0x0070, 8, 3, 15),
+	MUX_GATE(CLK_TOP_MSDC50_0_SEL, "msdc50_0_sel", msdc50_0_parents, 0x0070, 16, 4, 23),
+	MUX_GATE(CLK_TOP_MSDC30_1_SEL, "msdc30_1_sel", msdc30_1_parents, 0x0070, 24, 3, 31),
+	/* CLK_CFG_4 */
+	MUX_GATE(CLK_TOP_MSDC30_2_SEL, "msdc30_2_sel", msdc30_2_parents, 0x0080, 0, 3, 7),
+	MUX_GATE(CLK_TOP_MSDC30_3_SEL, "msdc30_3_sel", msdc30_3_parents, 0x0080, 8, 4, 15),
+	MUX_GATE(CLK_TOP_AUDIO_SEL, "audio_sel", audio_parents, 0x0080, 16, 2, 23),
+	MUX_GATE(CLK_TOP_AUD_INTBUS_SEL, "aud_intbus_sel", aud_intbus_parents, 0x0080, 24, 3, 31),
+	/* CLK_CFG_5 */
+	MUX_GATE(CLK_TOP_PMICSPI_SEL, "pmicspi_sel", pmicspi_parents, 0x0090, 0, 3, 7 /* 7:5 */),
+	MUX_GATE(CLK_TOP_SCP_SEL, "scp_sel", scp_parents, 0x0090, 8, 3, 15),
+	MUX_GATE(CLK_TOP_ATB_SEL, "atb_sel", atb_parents, 0x0090, 16, 2, 23),
+	MUX_GATE(CLK_TOP_VENC_LT_SEL, "venclt_sel", venc_lt_parents, 0x0090, 24, 4, 31),
+	/* CLK_CFG_6 */
+	MUX_GATE(CLK_TOP_DPI0_SEL, "dpi0_sel", dpi0_parents, 0x00a0, 0, 3, 7),
+	MUX_GATE(CLK_TOP_IRDA_SEL, "irda_sel", irda_parents, 0x00a0, 8, 2, 15),
+	MUX_GATE(CLK_TOP_CCI400_SEL, "cci400_sel", cci400_parents, 0x00a0, 16, 3, 23),
+	MUX_GATE(CLK_TOP_AUD_1_SEL, "aud_1_sel", aud_1_parents, 0x00a0, 24, 2, 31),
+	/* CLK_CFG_7 */
+	MUX_GATE(CLK_TOP_AUD_2_SEL, "aud_2_sel", aud_2_parents, 0x00b0, 0, 2, 7),
+	MUX_GATE(CLK_TOP_MEM_MFG_IN_SEL, "mem_mfg_in_sel", mem_mfg_in_parents, 0x00b0, 8, 2, 15),
+	MUX_GATE(CLK_TOP_AXI_MFG_IN_SEL, "axi_mfg_in_sel", axi_mfg_in_parents, 0x00b0, 16, 2, 23),
+	MUX_GATE(CLK_TOP_SCAM_SEL, "scam_sel", scam_parents, 0x00b0, 24, 2, 31),
+	/* CLK_CFG_12 */
+	MUX_GATE(CLK_TOP_SPINFI_IFR_SEL, "spinfi_ifr_sel", spinfi_ifr_parents, 0x00c0, 0, 3, 7),
+	MUX_GATE(CLK_TOP_HDMI_SEL, "hdmi_sel", hdmi_parents, 0x00c0, 8, 2, 15),
+	MUX_GATE(CLK_TOP_DPILVDS_SEL, "dpilvds_sel", dpilvds_parents, 0x00c0, 24, 3, 31),
+	/* CLK_CFG_13 */
+	MUX_GATE(CLK_TOP_MSDC50_2_H_SEL, "msdc50_2_h_sel", msdc50_2_h_parents, 0x00d0, 0, 3, 7),
+	MUX_GATE(CLK_TOP_HDCP_SEL, "hdcp_sel", hdcp_parents, 0x00d0, 8, 2, 15),
+	MUX_GATE(CLK_TOP_HDCP_24M_SEL, "hdcp_24m_sel", hdcp_24m_parents, 0x00d0, 16, 2, 23),
+	MUX(CLK_TOP_RTC_SEL, "rtc_sel", rtc_parents, 0x00d0, 24, 2),
+
+	DIV_GATE(CLK_TOP_APLL1_DIV0, "apll1_div0", "aud_1_sel", 0x12c, 8, 0x120, 4, 24),
+	DIV_GATE(CLK_TOP_APLL1_DIV1, "apll1_div1", "aud_1_sel", 0x12c, 9, 0x124, 8, 0),
+	DIV_GATE(CLK_TOP_APLL1_DIV2, "apll1_div2", "aud_1_sel", 0x12c, 10, 0x124, 8, 8),
+	DIV_GATE(CLK_TOP_APLL1_DIV3, "apll1_div3", "aud_1_sel", 0x12c, 11, 0x124, 8, 16),
+	DIV_GATE(CLK_TOP_APLL1_DIV4, "apll1_div4", "aud_1_sel", 0x12c, 12, 0x124, 8, 24),
+	DIV_GATE(CLK_TOP_APLL1_DIV5, "apll1_div5", "apll1_div4", 0x12c, 13, 0x12c, 4, 0),
+
+	DIV_GATE(CLK_TOP_APLL2_DIV0, "apll2_div0", "aud_2_sel", 0x12c, 16, 0x120, 4, 28),
+	DIV_GATE(CLK_TOP_APLL2_DIV1, "apll2_div1", "aud_2_sel", 0x12c, 17, 0x128, 8, 0),
+	DIV_GATE(CLK_TOP_APLL2_DIV2, "apll2_div2", "aud_2_sel", 0x12c, 18, 0x128, 8, 8),
+	DIV_GATE(CLK_TOP_APLL2_DIV3, "apll2_div3", "aud_2_sel", 0x12c, 19, 0x128, 8, 16),
+	DIV_GATE(CLK_TOP_APLL2_DIV4, "apll2_div4", "aud_2_sel", 0x12c, 20, 0x128, 8, 24),
+	DIV_GATE(CLK_TOP_APLL2_DIV5, "apll2_div5", "apll2_div4", 0x12c, 21, 0x12c, 4, 4),
+
+	MUX(CLK_TOP_I2S0_M_SEL, "i2s0_m_ck_sel", i2s0_m_ck_parents, 0x120, 4, 1),
+	MUX(CLK_TOP_I2S1_M_SEL, "i2s1_m_ck_sel", i2s1_m_ck_parents, 0x120, 5, 1),
+	MUX(CLK_TOP_I2S2_M_SEL, "i2s2_m_ck_sel", i2s2_m_ck_parents, 0x120, 6, 1),
+	MUX(CLK_TOP_I2S3_M_SEL, "i2s3_m_ck_sel", i2s3_m_ck_parents, 0x120, 7, 1),
+	MUX(CLK_TOP_I2S3_B_SEL, "i2s3_b_ck_sel", i2s3_b_ck_parents, 0x120, 8, 1),
+};
+
+static const struct mtk_gate_regs infra_cg_regs = {
+	.set_ofs = 0x0040,
+	.clr_ofs = 0x0044,
+	.sta_ofs = 0x0048,
+};
+
+#define GATE_ICG(_id, _name, _parent, _shift) {	\
+		.id = _id,					\
+		.name = _name,					\
+		.parent_name = _parent,				\
+		.regs = &infra_cg_regs,				\
+		.shift = _shift,				\
+		.ops = &mtk_clk_gate_ops_setclr,		\
+	}
+
+static const struct mtk_gate infra_clks[] __initconst = {
+	GATE_ICG(CLK_INFRA_DBGCLK, "infra_dbgclk", "axi_sel", 0),
+	GATE_ICG(CLK_INFRA_SMI, "infra_smi", "mm_sel", 1),
+	GATE_ICG(CLK_INFRA_AUDIO, "infra_audio", "aud_intbus_sel", 5),
+	GATE_ICG(CLK_INFRA_GCE, "infra_gce", "axi_sel", 6),
+	GATE_ICG(CLK_INFRA_L2C_SRAM, "infra_l2c_sram", "axi_sel", 7),
+	GATE_ICG(CLK_INFRA_M4U, "infra_m4u", "mem_sel", 8),
+	GATE_ICG(CLK_INFRA_CPUM, "infra_cpum", "clk_null", 15),
+	GATE_ICG(CLK_INFRA_KP, "infra_kp", "axi_sel", 16),
+	GATE_ICG(CLK_INFRA_CEC, "infra_cec", "clk26m", 18),
+	GATE_ICG(CLK_INFRA_PMICSPI, "infra_pmicspi", "pmicspi_sel", 22),
+	GATE_ICG(CLK_INFRA_PMICWRAP, "infra_pmicwrap", "axi_sel", 23),
+};
+
+static const struct mtk_gate_regs peri0_cg_regs = {
+	.set_ofs = 0x0008,
+	.clr_ofs = 0x0010,
+	.sta_ofs = 0x0018,
+};
+
+static const struct mtk_gate_regs peri1_cg_regs = {
+	.set_ofs = 0x000c,
+	.clr_ofs = 0x0014,
+	.sta_ofs = 0x001c,
+};
+
+#define GATE_PERI0(_id, _name, _parent, _shift) {	\
+		.id = _id,					\
+		.name = _name,					\
+		.parent_name = _parent,				\
+		.regs = &peri0_cg_regs,				\
+		.shift = _shift,				\
+		.ops = &mtk_clk_gate_ops_setclr,		\
+	}
+
+#define GATE_PERI1(_id, _name, _parent, _shift) {	\
+		.id = _id,					\
+		.name = _name,					\
+		.parent_name = _parent,				\
+		.regs = &peri1_cg_regs,				\
+		.shift = _shift,				\
+		.ops = &mtk_clk_gate_ops_setclr,		\
+	}
+
+static const struct mtk_gate peri_gates[] __initconst = {
+	/* PERI0 */
+	GATE_PERI0(CLK_PERI_NFI, "peri_nfi", "axi_sel", 0),
+	GATE_PERI0(CLK_PERI_THERM, "peri_therm", "axi_sel", 1),
+	GATE_PERI0(CLK_PERI_PWM1, "peri_pwm1", "axi_sel", 2),
+	GATE_PERI0(CLK_PERI_PWM2, "peri_pwm2", "axi_sel", 3),
+	GATE_PERI0(CLK_PERI_PWM3, "peri_pwm3", "axi_sel", 4),
+	GATE_PERI0(CLK_PERI_PWM4, "peri_pwm4", "axi_sel", 5),
+	GATE_PERI0(CLK_PERI_PWM5, "peri_pwm5", "axi_sel", 6),
+	GATE_PERI0(CLK_PERI_PWM6, "peri_pwm6", "axi_sel", 7),
+	GATE_PERI0(CLK_PERI_PWM7, "peri_pwm7", "axi_sel", 8),
+	GATE_PERI0(CLK_PERI_PWM, "peri_pwm", "axi_sel", 9),
+	GATE_PERI0(CLK_PERI_USB0, "peri_usb0", "usb20_sel", 10),
+	GATE_PERI0(CLK_PERI_USB1, "peri_usb1", "usb20_sel", 11),
+	GATE_PERI0(CLK_PERI_AP_DMA, "peri_ap_dma", "axi_sel", 12),
+	GATE_PERI0(CLK_PERI_MSDC30_0, "peri_msdc30_0", "msdc50_0_sel", 13),
+	GATE_PERI0(CLK_PERI_MSDC30_1, "peri_msdc30_1", "msdc30_1_sel", 14),
+	GATE_PERI0(CLK_PERI_MSDC30_2, "peri_msdc30_2", "msdc30_2_sel", 15),
+	GATE_PERI0(CLK_PERI_MSDC30_3, "peri_msdc30_3", "msdc30_3_sel", 16),
+	GATE_PERI0(CLK_PERI_NLI_ARB, "peri_nli_arb", "axi_sel", 17),
+	GATE_PERI0(CLK_PERI_IRDA, "peri_irda", "irda_sel", 18),
+	GATE_PERI0(CLK_PERI_UART0, "peri_uart0", "axi_sel", 19),
+	GATE_PERI0(CLK_PERI_UART1, "peri_uart1", "axi_sel", 20),
+	GATE_PERI0(CLK_PERI_UART2, "peri_uart2", "axi_sel", 21),
+	GATE_PERI0(CLK_PERI_UART3, "peri_uart3", "axi_sel", 22),
+	GATE_PERI0(CLK_PERI_I2C0, "peri_i2c0", "axi_sel", 23),
+	GATE_PERI0(CLK_PERI_I2C1, "peri_i2c1", "axi_sel", 24),
+	GATE_PERI0(CLK_PERI_I2C2, "peri_i2c2", "axi_sel", 25),
+	GATE_PERI0(CLK_PERI_I2C3, "peri_i2c3", "axi_sel", 26),
+	GATE_PERI0(CLK_PERI_I2C4, "peri_i2c4", "axi_sel", 27),
+	GATE_PERI0(CLK_PERI_AUXADC, "peri_auxadc", "clk26m", 28),
+	GATE_PERI0(CLK_PERI_SPI0, "peri_spi0", "spi_sel", 29),
+	GATE_PERI0(CLK_PERI_I2C5, "peri_i2c5", "axi_sel", 30),
+	GATE_PERI0(CLK_PERI_NFIECC, "peri_nfiecc", "axi_sel", 31),
+	/* PERI1 */
+	GATE_PERI1(CLK_PERI_SPI, "peri_spi", "spi_sel", 0),
+	GATE_PERI1(CLK_PERI_IRRX, "peri_irrx", "spi_sel", 1),
+	GATE_PERI1(CLK_PERI_I2C6, "peri_i2c6", "axi_sel", 2),
+};
+
+static const char * const uart_ck_sel_parents[] __initconst = {
+	"clk26m",
+	"uart_sel",
+};
+
+static const struct mtk_composite peri_clks[] __initconst = {
+	MUX(CLK_PERI_UART0_SEL, "uart0_ck_sel", uart_ck_sel_parents, 0x40c, 0, 1),
+	MUX(CLK_PERI_UART1_SEL, "uart1_ck_sel", uart_ck_sel_parents, 0x40c, 1, 1),
+	MUX(CLK_PERI_UART2_SEL, "uart2_ck_sel", uart_ck_sel_parents, 0x40c, 2, 1),
+	MUX(CLK_PERI_UART3_SEL, "uart3_ck_sel", uart_ck_sel_parents, 0x40c, 3, 1),
+};
+
+static struct clk_onecell_data *mt8173_top_clk_data __initdata;
+static struct clk_onecell_data *mt8173_pll_clk_data __initdata;
+
+static void __init mtk_clk_enable_critical(void)
+{
+	if (!mt8173_top_clk_data || !mt8173_pll_clk_data)
+		return;
+
+	clk_prepare_enable(mt8173_pll_clk_data->clks[CLK_APMIXED_ARMCA15PLL]);
+	clk_prepare_enable(mt8173_pll_clk_data->clks[CLK_APMIXED_ARMCA7PLL]);
+	clk_prepare_enable(mt8173_top_clk_data->clks[CLK_TOP_MEM_SEL]);
+	clk_prepare_enable(mt8173_top_clk_data->clks[CLK_TOP_DDRPHYCFG_SEL]);
+	clk_prepare_enable(mt8173_top_clk_data->clks[CLK_TOP_CCI400_SEL]);
+	clk_prepare_enable(mt8173_top_clk_data->clks[CLK_TOP_RTC_SEL]);
+}
+
+static void __init mtk_topckgen_init(struct device_node *node)
+{
+	struct clk_onecell_data *clk_data;
+	void __iomem *base;
+	int r;
+
+	base = of_iomap(node, 0);
+	if (!base) {
+		pr_err("%s(): ioremap failed\n", __func__);
+		return;
+	}
+
+	mt8173_top_clk_data = clk_data = mtk_alloc_clk_data(CLK_TOP_NR_CLK);
+
+	mtk_clk_register_factors(root_clk_alias, ARRAY_SIZE(root_clk_alias), clk_data);
+	mtk_clk_register_factors(top_divs, ARRAY_SIZE(top_divs), clk_data);
+	mtk_clk_register_composites(top_muxes, ARRAY_SIZE(top_muxes), base,
+			&mt8173_clk_lock, clk_data);
+
+	r = of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+	if (r)
+		pr_err("%s(): could not register clock provider: %d\n",
+			__func__, r);
+
+	mtk_clk_enable_critical();
+}
+CLK_OF_DECLARE(mtk_topckgen, "mediatek,mt8173-topckgen", mtk_topckgen_init);
+
+static void __init mtk_infrasys_init(struct device_node *node)
+{
+	struct clk_onecell_data *clk_data;
+	int r;
+
+	clk_data = mtk_alloc_clk_data(CLK_INFRA_NR_CLK);
+
+	mtk_clk_register_gates(node, infra_clks, ARRAY_SIZE(infra_clks),
+						clk_data);
+
+	r = of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+	if (r)
+		pr_err("%s(): could not register clock provider: %d\n",
+			__func__, r);
+
+	mtk_register_reset_controller(node, 2, 0x30);
+}
+CLK_OF_DECLARE(mtk_infrasys, "mediatek,mt8173-infracfg", mtk_infrasys_init);
+
+static void __init mtk_pericfg_init(struct device_node *node)
+{
+	struct clk_onecell_data *clk_data;
+	int r;
+	void __iomem *base;
+
+	base = of_iomap(node, 0);
+	if (!base) {
+		pr_err("%s(): ioremap failed\n", __func__);
+		return;
+	}
+
+	clk_data = mtk_alloc_clk_data(CLK_PERI_NR_CLK);
+
+	mtk_clk_register_gates(node, peri_gates, ARRAY_SIZE(peri_gates),
+						clk_data);
+	mtk_clk_register_composites(peri_clks, ARRAY_SIZE(peri_clks), base,
+			&mt8173_clk_lock, clk_data);
+
+	r = of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+	if (r)
+		pr_err("%s(): could not register clock provider: %d\n",
+			__func__, r);
+
+	mtk_register_reset_controller(node, 2, 0);
+}
+CLK_OF_DECLARE(mtk_pericfg, "mediatek,mt8173-pericfg", mtk_pericfg_init);
+
+#define MT8173_PLL_FMAX		(3000UL * MHZ)
+
+#define CON0_MT8173_RST_BAR	BIT(24)
+
+#define PLL(_id, _name, _reg, _pwr_reg, _en_mask, _flags, _pcwbits, _pd_reg, _pd_shift, \
+			_tuner_reg, _pcw_reg, _pcw_shift) { \
+		.id = _id,						\
+		.name = _name,						\
+		.reg = _reg,						\
+		.pwr_reg = _pwr_reg,					\
+		.en_mask = _en_mask,					\
+		.flags = _flags,					\
+		.rst_bar_mask = CON0_MT8173_RST_BAR,			\
+		.fmax = MT8173_PLL_FMAX,				\
+		.pcwbits = _pcwbits,					\
+		.pd_reg = _pd_reg,					\
+		.pd_shift = _pd_shift,					\
+		.tuner_reg = _tuner_reg,				\
+		.pcw_reg = _pcw_reg,					\
+		.pcw_shift = _pcw_shift,				\
+	}
+
+static const struct mtk_pll_data plls[] = {
+	PLL(CLK_APMIXED_ARMCA15PLL, "armca15pll", 0x200, 0x20c, 0x00000001, 0, 21, 0x204, 24, 0x0, 0x204, 0),
+	PLL(CLK_APMIXED_ARMCA7PLL, "armca7pll", 0x210, 0x21c, 0x00000001, 0, 21, 0x214, 24, 0x0, 0x214, 0),
+	PLL(CLK_APMIXED_MAINPLL, "mainpll", 0x220, 0x22c, 0xf0000101, HAVE_RST_BAR, 21, 0x220, 4, 0x0, 0x224, 0),
+	PLL(CLK_APMIXED_UNIVPLL, "univpll", 0x230, 0x23c, 0xfe000001, HAVE_RST_BAR, 7, 0x230, 4, 0x0, 0x234, 14),
+	PLL(CLK_APMIXED_MMPLL, "mmpll", 0x240, 0x24c, 0x00000001, 0, 21, 0x244, 24, 0x0, 0x244, 0),
+	PLL(CLK_APMIXED_MSDCPLL, "msdcpll", 0x250, 0x25c, 0x00000001, 0, 21, 0x250, 4, 0x0, 0x254, 0),
+	PLL(CLK_APMIXED_VENCPLL, "vencpll", 0x260, 0x26c, 0x00000001, 0, 21, 0x260, 4, 0x0, 0x264, 0),
+	PLL(CLK_APMIXED_TVDPLL, "tvdpll", 0x270, 0x27c, 0x00000001, 0, 21, 0x270, 4, 0x0, 0x274, 0),
+	PLL(CLK_APMIXED_MPLL, "mpll", 0x280, 0x28c, 0x00000001, 0, 21, 0x280, 4, 0x0, 0x284, 0),
+	PLL(CLK_APMIXED_VCODECPLL, "vcodecpll", 0x290, 0x29c, 0x00000001, 0, 21, 0x290, 4, 0x0, 0x294, 0),
+	PLL(CLK_APMIXED_APLL1, "apll1", 0x2a0, 0x2b0, 0x00000001, 0, 31, 0x2a0, 4, 0x2a4, 0x2a4, 0),
+	PLL(CLK_APMIXED_APLL2, "apll2", 0x2b4, 0x2c4, 0x00000001, 0, 31, 0x2b4, 4, 0x2b8, 0x2b8, 0),
+	PLL(CLK_APMIXED_LVDSPLL, "lvdspll", 0x2d0, 0x2dc, 0x00000001, 0, 21, 0x2d0, 4, 0x0, 0x2d4, 0),
+	PLL(CLK_APMIXED_MSDCPLL2, "msdcpll2", 0x2f0, 0x2fc, 0x00000001, 0, 21, 0x2f0, 4, 0x0, 0x2f4, 0),
+};
+
+static void __init mtk_apmixedsys_init(struct device_node *node)
+{
+	struct clk_onecell_data *clk_data;
+
+	mt8173_pll_clk_data = clk_data = mtk_alloc_clk_data(CLK_APMIXED_NR_CLK);
+	if (!clk_data)
+		return;
+
+	mtk_clk_register_plls(node, plls, ARRAY_SIZE(plls), clk_data);
+
+	mtk_clk_enable_critical();
+}
+CLK_OF_DECLARE(mtk_apmixedsys, "mediatek,mt8173-apmixedsys",
+		mtk_apmixedsys_init);
diff --git a/drivers/clk/mediatek/clk-mtk.c b/drivers/clk/mediatek/clk-mtk.c
new file mode 100644
index 000000000000..18444aea63c9
--- /dev/null
+++ b/drivers/clk/mediatek/clk-mtk.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: James Liao <jamesjj.liao@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/of.h>
+#include <linux/of_address.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/clkdev.h>
+#include <linux/mfd/syscon.h>
+
+#include "clk-mtk.h"
+#include "clk-gate.h"
+
+struct clk_onecell_data *mtk_alloc_clk_data(unsigned int clk_num)
+{
+	int i;
+	struct clk_onecell_data *clk_data;
+
+	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
+	if (!clk_data)
+		return NULL;
+
+	clk_data->clks = kcalloc(clk_num, sizeof(*clk_data->clks), GFP_KERNEL);
+	if (!clk_data->clks)
+		goto err_out;
+
+	clk_data->clk_num = clk_num;
+
+	for (i = 0; i < clk_num; i++)
+		clk_data->clks[i] = ERR_PTR(-ENOENT);
+
+	return clk_data;
+err_out:
+	kfree(clk_data);
+
+	return NULL;
+}
+
+void mtk_clk_register_factors(const struct mtk_fixed_factor *clks, int num,
+		struct clk_onecell_data *clk_data)
+{
+	int i;
+	struct clk *clk;
+
+	for (i = 0; i < num; i++) {
+		const struct mtk_fixed_factor *ff = &clks[i];
+
+		clk = clk_register_fixed_factor(NULL, ff->name, ff->parent_name,
+				CLK_SET_RATE_PARENT, ff->mult, ff->div);
+
+		if (IS_ERR(clk)) {
+			pr_err("Failed to register clk %s: %ld\n",
+					ff->name, PTR_ERR(clk));
+			continue;
+		}
+
+		if (clk_data)
+			clk_data->clks[ff->id] = clk;
+	}
+}
+
+int mtk_clk_register_gates(struct device_node *node, const struct mtk_gate *clks,
+		int num, struct clk_onecell_data *clk_data)
+{
+	int i;
+	struct clk *clk;
+	struct regmap *regmap;
+
+	if (!clk_data)
+		return -ENOMEM;
+
+	regmap = syscon_node_to_regmap(node);
+	if (IS_ERR(regmap)) {
+		pr_err("Cannot find regmap for %s: %ld\n", node->full_name,
+				PTR_ERR(regmap));
+		return PTR_ERR(regmap);
+	}
+
+	for (i = 0; i < num; i++) {
+		const struct mtk_gate *gate = &clks[i];
+
+		clk = mtk_clk_register_gate(gate->name, gate->parent_name,
+				regmap,
+				gate->regs->set_ofs,
+				gate->regs->clr_ofs,
+				gate->regs->sta_ofs,
+				gate->shift, gate->ops);
+
+		if (IS_ERR(clk)) {
+			pr_err("Failed to register clk %s: %ld\n",
+					gate->name, PTR_ERR(clk));
+			continue;
+		}
+
+		clk_data->clks[gate->id] = clk;
+	}
+
+	return 0;
+}
+
+struct clk *mtk_clk_register_composite(const struct mtk_composite *mc,
+		void __iomem *base, spinlock_t *lock)
+{
+	struct clk *clk;
+	struct clk_mux *mux = NULL;
+	struct clk_gate *gate = NULL;
+	struct clk_divider *div = NULL;
+	struct clk_hw *mux_hw = NULL, *gate_hw = NULL, *div_hw = NULL;
+	const struct clk_ops *mux_ops = NULL, *gate_ops = NULL, *div_ops = NULL;
+	const char * const *parent_names;
+	const char *parent;
+	int num_parents;
+	int ret;
+
+	if (mc->mux_shift >= 0) {
+		mux = kzalloc(sizeof(*mux), GFP_KERNEL);
+		if (!mux)
+			return ERR_PTR(-ENOMEM);
+
+		mux->reg = base + mc->mux_reg;
+		mux->mask = BIT(mc->mux_width) - 1;
+		mux->shift = mc->mux_shift;
+		mux->lock = lock;
+
+		mux_hw = &mux->hw;
+		mux_ops = &clk_mux_ops;
+
+		parent_names = mc->parent_names;
+		num_parents = mc->num_parents;
+	} else {
+		parent = mc->parent;
+		parent_names = &parent;
+		num_parents = 1;
+	}
+
+	if (mc->gate_shift >= 0) {
+		gate = kzalloc(sizeof(*gate), GFP_KERNEL);
+		if (!gate) {
+			ret = -ENOMEM;
+			goto err_out;
+		}
+
+		gate->reg = base + mc->gate_reg;
+		gate->bit_idx = mc->gate_shift;
+		gate->flags = CLK_GATE_SET_TO_DISABLE;
+		gate->lock = lock;
+
+		gate_hw = &gate->hw;
+		gate_ops = &clk_gate_ops;
+	}
+
+	if (mc->divider_shift >= 0) {
+		div = kzalloc(sizeof(*div), GFP_KERNEL);
+		if (!div) {
+			ret = -ENOMEM;
+			goto err_out;
+		}
+
+		div->reg = base + mc->divider_reg;
+		div->shift = mc->divider_shift;
+		div->width = mc->divider_width;
+		div->lock = lock;
+
+		div_hw = &div->hw;
+		div_ops = &clk_divider_ops;
+	}
+
+	clk = clk_register_composite(NULL, mc->name, parent_names, num_parents,
+		mux_hw, mux_ops,
+		div_hw, div_ops,
+		gate_hw, gate_ops,
+		mc->flags);
+
+	if (IS_ERR(clk)) {
+		kfree(gate);
+		kfree(mux);
+	}
+
+	return clk;
+err_out:
+	kfree(mux);
+
+	return ERR_PTR(ret);
+}
+
+void mtk_clk_register_composites(const struct mtk_composite *mcs,
+		int num, void __iomem *base, spinlock_t *lock,
+		struct clk_onecell_data *clk_data)
+{
+	struct clk *clk;
+	int i;
+
+	for (i = 0; i < num; i++) {
+		const struct mtk_composite *mc = &mcs[i];
+
+		clk = mtk_clk_register_composite(mc, base, lock);
+
+		if (IS_ERR(clk)) {
+			pr_err("Failed to register clk %s: %ld\n",
+					mc->name, PTR_ERR(clk));
+			continue;
+		}
+
+		if (clk_data)
+			clk_data->clks[mc->id] = clk;
+	}
+}
diff --git a/drivers/clk/mediatek/clk-mtk.h b/drivers/clk/mediatek/clk-mtk.h
new file mode 100644
index 000000000000..9dda9d8ad10b
--- /dev/null
+++ b/drivers/clk/mediatek/clk-mtk.h
@@ -0,0 +1,169 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: James Liao <jamesjj.liao@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.
+ */
+
+#ifndef __DRV_CLK_MTK_H
+#define __DRV_CLK_MTK_H
+
+#include <linux/regmap.h>
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+
+#define MAX_MUX_GATE_BIT	31
+#define INVALID_MUX_GATE_BIT	(MAX_MUX_GATE_BIT + 1)
+
+#define MHZ (1000 * 1000)
+
+struct mtk_fixed_factor {
+	int id;
+	const char *name;
+	const char *parent_name;
+	int mult;
+	int div;
+};
+
+#define FACTOR(_id, _name, _parent, _mult, _div) {	\
+		.id = _id,				\
+		.name = _name,				\
+		.parent_name = _parent,			\
+		.mult = _mult,				\
+		.div = _div,				\
+	}
+
+extern void mtk_clk_register_factors(const struct mtk_fixed_factor *clks,
+		int num, struct clk_onecell_data *clk_data);
+
+struct mtk_composite {
+	int id;
+	const char *name;
+	const char * const *parent_names;
+	const char *parent;
+	unsigned flags;
+
+	uint32_t mux_reg;
+	uint32_t divider_reg;
+	uint32_t gate_reg;
+
+	signed char mux_shift;
+	signed char mux_width;
+	signed char gate_shift;
+
+	signed char divider_shift;
+	signed char divider_width;
+
+	signed char num_parents;
+};
+
+#define MUX_GATE(_id, _name, _parents, _reg, _shift, _width, _gate) {	\
+		.id = _id,						\
+		.name = _name,						\
+		.mux_reg = _reg,					\
+		.mux_shift = _shift,					\
+		.mux_width = _width,					\
+		.gate_reg = _reg,					\
+		.gate_shift = _gate,					\
+		.divider_shift = -1,					\
+		.parent_names = _parents,				\
+		.num_parents = ARRAY_SIZE(_parents),			\
+		.flags = CLK_SET_RATE_PARENT,				\
+	}
+
+#define MUX(_id, _name, _parents, _reg, _shift, _width) {		\
+		.id = _id,						\
+		.name = _name,						\
+		.mux_reg = _reg,					\
+		.mux_shift = _shift,					\
+		.mux_width = _width,					\
+		.gate_shift = -1,					\
+		.divider_shift = -1,					\
+		.parent_names = _parents,				\
+		.num_parents = ARRAY_SIZE(_parents),			\
+		.flags = CLK_SET_RATE_PARENT,				\
+	}
+
+#define DIV_GATE(_id, _name, _parent, _gate_reg, _gate_shift, _div_reg, _div_width, _div_shift) {	\
+		.id = _id,						\
+		.parent = _parent,					\
+		.name = _name,						\
+		.divider_reg = _div_reg,				\
+		.divider_shift = _div_shift,				\
+		.divider_width = _div_width,				\
+		.gate_reg = _gate_reg,					\
+		.gate_shift = _gate_shift,				\
+		.mux_shift = -1,					\
+		.flags = 0,						\
+	}
+
+struct clk *mtk_clk_register_composite(const struct mtk_composite *mc,
+		void __iomem *base, spinlock_t *lock);
+
+void mtk_clk_register_composites(const struct mtk_composite *mcs,
+		int num, void __iomem *base, spinlock_t *lock,
+		struct clk_onecell_data *clk_data);
+
+struct mtk_gate_regs {
+	u32 sta_ofs;
+	u32 clr_ofs;
+	u32 set_ofs;
+};
+
+struct mtk_gate {
+	int id;
+	const char *name;
+	const char *parent_name;
+	const struct mtk_gate_regs *regs;
+	int shift;
+	const struct clk_ops *ops;
+};
+
+int mtk_clk_register_gates(struct device_node *node, const struct mtk_gate *clks,
+		int num, struct clk_onecell_data *clk_data);
+
+struct clk_onecell_data *mtk_alloc_clk_data(unsigned int clk_num);
+
+#define HAVE_RST_BAR	BIT(0)
+
+struct mtk_pll_data {
+	int id;
+	const char *name;
+	uint32_t reg;
+	uint32_t pwr_reg;
+	uint32_t en_mask;
+	uint32_t pd_reg;
+	uint32_t tuner_reg;
+	int pd_shift;
+	unsigned int flags;
+	const struct clk_ops *ops;
+	u32 rst_bar_mask;
+	unsigned long fmax;
+	int pcwbits;
+	uint32_t pcw_reg;
+	int pcw_shift;
+};
+
+void __init mtk_clk_register_plls(struct device_node *node,
+		const struct mtk_pll_data *plls, int num_plls,
+		struct clk_onecell_data *clk_data);
+
+#ifdef CONFIG_RESET_CONTROLLER
+void mtk_register_reset_controller(struct device_node *np,
+			unsigned int num_regs, int regofs);
+#else
+static inline void mtk_register_reset_controller(struct device_node *np,
+			unsigned int num_regs, int regofs)
+{
+}
+#endif
+
+#endif /* __DRV_CLK_MTK_H */
diff --git a/drivers/clk/mediatek/clk-pll.c b/drivers/clk/mediatek/clk-pll.c
new file mode 100644
index 000000000000..44409e98c52f
--- /dev/null
+++ b/drivers/clk/mediatek/clk-pll.c
@@ -0,0 +1,332 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: James Liao <jamesjj.liao@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/of.h>
+#include <linux/of_address.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/clkdev.h>
+#include <linux/delay.h>
+
+#include "clk-mtk.h"
+
+#define REG_CON0		0
+#define REG_CON1		4
+
+#define CON0_BASE_EN		BIT(0)
+#define CON0_PWR_ON		BIT(0)
+#define CON0_ISO_EN		BIT(1)
+#define CON0_PCW_CHG		BIT(31)
+
+#define AUDPLL_TUNER_EN		BIT(31)
+
+#define POSTDIV_MASK		0x7
+#define INTEGER_BITS		7
+
+/*
+ * MediaTek PLLs are configured through their pcw value. The pcw value describes
+ * a divider in the PLL feedback loop which consists of 7 bits for the integer
+ * part and the remaining bits (if present) for the fractional part. Also they
+ * have a 3 bit power-of-two post divider.
+ */
+
+struct mtk_clk_pll {
+	struct clk_hw	hw;
+	void __iomem	*base_addr;
+	void __iomem	*pd_addr;
+	void __iomem	*pwr_addr;
+	void __iomem	*tuner_addr;
+	void __iomem	*pcw_addr;
+	const struct mtk_pll_data *data;
+};
+
+static inline struct mtk_clk_pll *to_mtk_clk_pll(struct clk_hw *hw)
+{
+	return container_of(hw, struct mtk_clk_pll, hw);
+}
+
+static int mtk_pll_is_prepared(struct clk_hw *hw)
+{
+	struct mtk_clk_pll *pll = to_mtk_clk_pll(hw);
+
+	return (readl(pll->base_addr + REG_CON0) & CON0_BASE_EN) != 0;
+}
+
+static unsigned long __mtk_pll_recalc_rate(struct mtk_clk_pll *pll, u32 fin,
+		u32 pcw, int postdiv)
+{
+	int pcwbits = pll->data->pcwbits;
+	int pcwfbits;
+	u64 vco;
+	u8 c = 0;
+
+	/* The fractional part of the PLL divider. */
+	pcwfbits = pcwbits > INTEGER_BITS ? pcwbits - INTEGER_BITS : 0;
+
+	vco = (u64)fin * pcw;
+
+	if (pcwfbits && (vco & GENMASK(pcwfbits - 1, 0)))
+		c = 1;
+
+	vco >>= pcwfbits;
+
+	if (c)
+		vco++;
+
+	return ((unsigned long)vco + postdiv - 1) / postdiv;
+}
+
+static void mtk_pll_set_rate_regs(struct mtk_clk_pll *pll, u32 pcw,
+		int postdiv)
+{
+	u32 con1, pd, val;
+	int pll_en;
+
+	/* set postdiv */
+	pd = readl(pll->pd_addr);
+	pd &= ~(POSTDIV_MASK << pll->data->pd_shift);
+	pd |= (ffs(postdiv) - 1) << pll->data->pd_shift;
+	writel(pd, pll->pd_addr);
+
+	pll_en = readl(pll->base_addr + REG_CON0) & CON0_BASE_EN;
+
+	/* set pcw */
+	val = readl(pll->pcw_addr);
+
+	val &= ~GENMASK(pll->data->pcw_shift + pll->data->pcwbits - 1,
+			pll->data->pcw_shift);
+	val |= pcw << pll->data->pcw_shift;
+	writel(val, pll->pcw_addr);
+
+	con1 = readl(pll->base_addr + REG_CON1);
+
+	if (pll_en)
+		con1 |= CON0_PCW_CHG;
+
+	writel(con1, pll->base_addr + REG_CON1);
+	if (pll->tuner_addr)
+		writel(con1 + 1, pll->tuner_addr);
+
+	if (pll_en)
+		udelay(20);
+}
+
+/*
+ * mtk_pll_calc_values - calculate good values for a given input frequency.
+ * @pll:	The pll
+ * @pcw:	The pcw value (output)
+ * @postdiv:	The post divider (output)
+ * @freq:	The desired target frequency
+ * @fin:	The input frequency
+ *
+ */
+static void mtk_pll_calc_values(struct mtk_clk_pll *pll, u32 *pcw, u32 *postdiv,
+		u32 freq, u32 fin)
+{
+	unsigned long fmin = 1000 * MHZ;
+	u64 _pcw;
+	u32 val;
+
+	if (freq > pll->data->fmax)
+		freq = pll->data->fmax;
+
+	for (val = 0; val < 4; val++) {
+		*postdiv = 1 << val;
+		if (freq * *postdiv >= fmin)
+			break;
+	}
+
+	/* _pcw = freq * postdiv / fin * 2^pcwfbits */
+	_pcw = ((u64)freq << val) << (pll->data->pcwbits - INTEGER_BITS);
+	do_div(_pcw, fin);
+
+	*pcw = (u32)_pcw;
+}
+
+static int mtk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct mtk_clk_pll *pll = to_mtk_clk_pll(hw);
+	u32 pcw = 0;
+	u32 postdiv;
+
+	mtk_pll_calc_values(pll, &pcw, &postdiv, rate, parent_rate);
+	mtk_pll_set_rate_regs(pll, pcw, postdiv);
+
+	return 0;
+}
+
+static unsigned long mtk_pll_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct mtk_clk_pll *pll = to_mtk_clk_pll(hw);
+	u32 postdiv;
+	u32 pcw;
+
+	postdiv = (readl(pll->pd_addr) >> pll->data->pd_shift) & POSTDIV_MASK;
+	postdiv = 1 << postdiv;
+
+	pcw = readl(pll->pcw_addr) >> pll->data->pcw_shift;
+	pcw &= GENMASK(pll->data->pcwbits - 1, 0);
+
+	return __mtk_pll_recalc_rate(pll, parent_rate, pcw, postdiv);
+}
+
+static long mtk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *prate)
+{
+	struct mtk_clk_pll *pll = to_mtk_clk_pll(hw);
+	u32 pcw = 0;
+	int postdiv;
+
+	mtk_pll_calc_values(pll, &pcw, &postdiv, rate, *prate);
+
+	return __mtk_pll_recalc_rate(pll, *prate, pcw, postdiv);
+}
+
+static int mtk_pll_prepare(struct clk_hw *hw)
+{
+	struct mtk_clk_pll *pll = to_mtk_clk_pll(hw);
+	u32 r;
+
+	r = readl(pll->pwr_addr) | CON0_PWR_ON;
+	writel(r, pll->pwr_addr);
+	udelay(1);
+
+	r = readl(pll->pwr_addr) & ~CON0_ISO_EN;
+	writel(r, pll->pwr_addr);
+	udelay(1);
+
+	r = readl(pll->base_addr + REG_CON0);
+	r |= pll->data->en_mask;
+	writel(r, pll->base_addr + REG_CON0);
+
+	if (pll->tuner_addr) {
+		r = readl(pll->tuner_addr) | AUDPLL_TUNER_EN;
+		writel(r, pll->tuner_addr);
+	}
+
+	udelay(20);
+
+	if (pll->data->flags & HAVE_RST_BAR) {
+		r = readl(pll->base_addr + REG_CON0);
+		r |= pll->data->rst_bar_mask;
+		writel(r, pll->base_addr + REG_CON0);
+	}
+
+	return 0;
+}
+
+static void mtk_pll_unprepare(struct clk_hw *hw)
+{
+	struct mtk_clk_pll *pll = to_mtk_clk_pll(hw);
+	u32 r;
+
+	if (pll->data->flags & HAVE_RST_BAR) {
+		r = readl(pll->base_addr + REG_CON0);
+		r &= ~pll->data->rst_bar_mask;
+		writel(r, pll->base_addr + REG_CON0);
+	}
+
+	if (pll->tuner_addr) {
+		r = readl(pll->tuner_addr) & ~AUDPLL_TUNER_EN;
+		writel(r, pll->tuner_addr);
+	}
+
+	r = readl(pll->base_addr + REG_CON0);
+	r &= ~CON0_BASE_EN;
+	writel(r, pll->base_addr + REG_CON0);
+
+	r = readl(pll->pwr_addr) | CON0_ISO_EN;
+	writel(r, pll->pwr_addr);
+
+	r = readl(pll->pwr_addr) & ~CON0_PWR_ON;
+	writel(r, pll->pwr_addr);
+}
+
+static const struct clk_ops mtk_pll_ops = {
+	.is_prepared	= mtk_pll_is_prepared,
+	.prepare	= mtk_pll_prepare,
+	.unprepare	= mtk_pll_unprepare,
+	.recalc_rate	= mtk_pll_recalc_rate,
+	.round_rate	= mtk_pll_round_rate,
+	.set_rate	= mtk_pll_set_rate,
+};
+
+static struct clk *mtk_clk_register_pll(const struct mtk_pll_data *data,
+		void __iomem *base)
+{
+	struct mtk_clk_pll *pll;
+	struct clk_init_data init = {};
+	struct clk *clk;
+	const char *parent_name = "clk26m";
+
+	pll = kzalloc(sizeof(*pll), GFP_KERNEL);
+	if (!pll)
+		return ERR_PTR(-ENOMEM);
+
+	pll->base_addr = base + data->reg;
+	pll->pwr_addr = base + data->pwr_reg;
+	pll->pd_addr = base + data->pd_reg;
+	pll->pcw_addr = base + data->pcw_reg;
+	if (data->tuner_reg)
+		pll->tuner_addr = base + data->tuner_reg;
+	pll->hw.init = &init;
+	pll->data = data;
+
+	init.name = data->name;
+	init.ops = &mtk_pll_ops;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	clk = clk_register(NULL, &pll->hw);
+
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+void __init mtk_clk_register_plls(struct device_node *node,
+		const struct mtk_pll_data *plls, int num_plls, struct clk_onecell_data *clk_data)
+{
+	void __iomem *base;
+	int r, i;
+	struct clk *clk;
+
+	base = of_iomap(node, 0);
+	if (!base) {
+		pr_err("%s(): ioremap failed\n", __func__);
+		return;
+	}
+
+	for (i = 0; i < num_plls; i++) {
+		const struct mtk_pll_data *pll = &plls[i];
+
+		clk = mtk_clk_register_pll(pll, base);
+
+		if (IS_ERR(clk)) {
+			pr_err("Failed to register clk %s: %ld\n",
+					pll->name, PTR_ERR(clk));
+			continue;
+		}
+
+		clk_data->clks[pll->id] = clk;
+	}
+
+	r = of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+	if (r)
+		pr_err("%s(): could not register clock provider: %d\n",
+			__func__, r);
+}
diff --git a/drivers/clk/mediatek/reset.c b/drivers/clk/mediatek/reset.c
new file mode 100644
index 000000000000..9e9fe4b19ac4
--- /dev/null
+++ b/drivers/clk/mediatek/reset.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/reset-controller.h>
+#include <linux/slab.h>
+
+#include "clk-mtk.h"
+
+struct mtk_reset {
+	struct regmap *regmap;
+	int regofs;
+	struct reset_controller_dev rcdev;
+};
+
+static int mtk_reset_assert(struct reset_controller_dev *rcdev,
+			      unsigned long id)
+{
+	struct mtk_reset *data = container_of(rcdev, struct mtk_reset, rcdev);
+
+	return regmap_update_bits(data->regmap, data->regofs + ((id / 32) << 2),
+			BIT(id % 32), ~0);
+}
+
+static int mtk_reset_deassert(struct reset_controller_dev *rcdev,
+				unsigned long id)
+{
+	struct mtk_reset *data = container_of(rcdev, struct mtk_reset, rcdev);
+
+	return regmap_update_bits(data->regmap, data->regofs + ((id / 32) << 2),
+			BIT(id % 32), 0);
+}
+
+static int mtk_reset(struct reset_controller_dev *rcdev,
+			      unsigned long id)
+{
+	int ret;
+
+	ret = mtk_reset_assert(rcdev, id);
+	if (ret)
+		return ret;
+
+	return mtk_reset_deassert(rcdev, id);
+}
+
+static struct reset_control_ops mtk_reset_ops = {
+	.assert = mtk_reset_assert,
+	.deassert = mtk_reset_deassert,
+	.reset = mtk_reset,
+};
+
+void mtk_register_reset_controller(struct device_node *np,
+			unsigned int num_regs, int regofs)
+{
+	struct mtk_reset *data;
+	int ret;
+	struct regmap *regmap;
+
+	regmap = syscon_node_to_regmap(np);
+	if (IS_ERR(regmap)) {
+		pr_err("Cannot find regmap for %s: %ld\n", np->full_name,
+				PTR_ERR(regmap));
+		return;
+	}
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return;
+
+	data->regmap = regmap;
+	data->regofs = regofs;
+	data->rcdev.owner = THIS_MODULE;
+	data->rcdev.nr_resets = num_regs * 32;
+	data->rcdev.ops = &mtk_reset_ops;
+	data->rcdev.of_node = np;
+
+	ret = reset_controller_register(&data->rcdev);
+	if (ret) {
+		pr_err("could not register reset controller: %d\n", ret);
+		kfree(data);
+		return;
+	}
+}
diff --git a/drivers/clk/meson/Makefile b/drivers/clk/meson/Makefile
new file mode 100644
index 000000000000..6d45531df9ab
--- /dev/null
+++ b/drivers/clk/meson/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for Meson specific clk
+#
+
+obj-y += clkc.o clk-pll.o clk-cpu.o
+obj-y += meson8b-clkc.o
diff --git a/drivers/clk/meson/clk-cpu.c b/drivers/clk/meson/clk-cpu.c
new file mode 100644
index 000000000000..71ad493b94df
--- /dev/null
+++ b/drivers/clk/meson/clk-cpu.c
@@ -0,0 +1,242 @@
+/*
+ * Copyright (c) 2015 Endless Mobile, Inc.
+ * Author: Carlo Caione <carlo@endlessm.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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 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/>.
+ */
+
+/*
+ * CPU clock path:
+ *
+ *                           +-[/N]-----|3|
+ *             MUX2  +--[/3]-+----------|2| MUX1
+ * [sys_pll]---|1|   |--[/2]------------|1|-|1|
+ *             | |---+------------------|0| | |----- [a5_clk]
+ *          +--|0|                          | |
+ * [xtal]---+-------------------------------|0|
+ *
+ *
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/clk-provider.h>
+
+#define MESON_CPU_CLK_CNTL1		0x00
+#define MESON_CPU_CLK_CNTL		0x40
+
+#define MESON_CPU_CLK_MUX1		BIT(7)
+#define MESON_CPU_CLK_MUX2		BIT(0)
+
+#define MESON_N_WIDTH			9
+#define MESON_N_SHIFT			20
+#define MESON_SEL_WIDTH			2
+#define MESON_SEL_SHIFT			2
+
+#include "clkc.h"
+
+struct meson_clk_cpu {
+	struct notifier_block		clk_nb;
+	const struct clk_div_table	*div_table;
+	struct clk_hw			hw;
+	void __iomem			*base;
+	u16				reg_off;
+};
+#define to_meson_clk_cpu_hw(_hw) container_of(_hw, struct meson_clk_cpu, hw)
+#define to_meson_clk_cpu_nb(_nb) container_of(_nb, struct meson_clk_cpu, clk_nb)
+
+static long meson_clk_cpu_round_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long *prate)
+{
+	struct meson_clk_cpu *clk_cpu = to_meson_clk_cpu_hw(hw);
+
+	return divider_round_rate(hw, rate, prate, clk_cpu->div_table,
+				  MESON_N_WIDTH, CLK_DIVIDER_ROUND_CLOSEST);
+}
+
+static int meson_clk_cpu_set_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long parent_rate)
+{
+	struct meson_clk_cpu *clk_cpu = to_meson_clk_cpu_hw(hw);
+	unsigned int div, sel, N = 0;
+	u32 reg;
+
+	div = DIV_ROUND_UP(parent_rate, rate);
+
+	if (div <= 3) {
+		sel = div - 1;
+	} else {
+		sel = 3;
+		N = div / 2;
+	}
+
+	reg = readl(clk_cpu->base + clk_cpu->reg_off + MESON_CPU_CLK_CNTL1);
+	reg = PARM_SET(MESON_N_WIDTH, MESON_N_SHIFT, reg, N);
+	writel(reg, clk_cpu->base + clk_cpu->reg_off + MESON_CPU_CLK_CNTL1);
+
+	reg = readl(clk_cpu->base + clk_cpu->reg_off + MESON_CPU_CLK_CNTL);
+	reg = PARM_SET(MESON_SEL_WIDTH, MESON_SEL_SHIFT, reg, sel);
+	writel(reg, clk_cpu->base + clk_cpu->reg_off + MESON_CPU_CLK_CNTL);
+
+	return 0;
+}
+
+static unsigned long meson_clk_cpu_recalc_rate(struct clk_hw *hw,
+					       unsigned long parent_rate)
+{
+	struct meson_clk_cpu *clk_cpu = to_meson_clk_cpu_hw(hw);
+	unsigned int N, sel;
+	unsigned int div = 1;
+	u32 reg;
+
+	reg = readl(clk_cpu->base + clk_cpu->reg_off + MESON_CPU_CLK_CNTL1);
+	N = PARM_GET(MESON_N_WIDTH, MESON_N_SHIFT, reg);
+
+	reg = readl(clk_cpu->base + clk_cpu->reg_off + MESON_CPU_CLK_CNTL);
+	sel = PARM_GET(MESON_SEL_WIDTH, MESON_SEL_SHIFT, reg);
+
+	if (sel < 3)
+		div = sel + 1;
+	else
+		div = 2 * N;
+
+	return parent_rate / div;
+}
+
+static int meson_clk_cpu_pre_rate_change(struct meson_clk_cpu *clk_cpu,
+					 struct clk_notifier_data *ndata)
+{
+	u32 cpu_clk_cntl;
+
+	/* switch MUX1 to xtal */
+	cpu_clk_cntl = readl(clk_cpu->base + clk_cpu->reg_off
+				+ MESON_CPU_CLK_CNTL);
+	cpu_clk_cntl &= ~MESON_CPU_CLK_MUX1;
+	writel(cpu_clk_cntl, clk_cpu->base + clk_cpu->reg_off
+				+ MESON_CPU_CLK_CNTL);
+	udelay(100);
+
+	/* switch MUX2 to sys-pll */
+	cpu_clk_cntl |= MESON_CPU_CLK_MUX2;
+	writel(cpu_clk_cntl, clk_cpu->base + clk_cpu->reg_off
+				+ MESON_CPU_CLK_CNTL);
+
+	return 0;
+}
+
+static int meson_clk_cpu_post_rate_change(struct meson_clk_cpu *clk_cpu,
+					  struct clk_notifier_data *ndata)
+{
+	u32 cpu_clk_cntl;
+
+	/* switch MUX1 to divisors' output */
+	cpu_clk_cntl = readl(clk_cpu->base + clk_cpu->reg_off
+				+ MESON_CPU_CLK_CNTL);
+	cpu_clk_cntl |= MESON_CPU_CLK_MUX1;
+	writel(cpu_clk_cntl, clk_cpu->base + clk_cpu->reg_off
+				+ MESON_CPU_CLK_CNTL);
+	udelay(100);
+
+	return 0;
+}
+
+/*
+ * This clock notifier is called when the frequency of the of the parent
+ * PLL clock is to be changed. We use the xtal input as temporary parent
+ * while the PLL frequency is stabilized.
+ */
+static int meson_clk_cpu_notifier_cb(struct notifier_block *nb,
+				     unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct meson_clk_cpu *clk_cpu = to_meson_clk_cpu_nb(nb);
+	int ret = 0;
+
+	if (event == PRE_RATE_CHANGE)
+		ret = meson_clk_cpu_pre_rate_change(clk_cpu, ndata);
+	else if (event == POST_RATE_CHANGE)
+		ret = meson_clk_cpu_post_rate_change(clk_cpu, ndata);
+
+	return notifier_from_errno(ret);
+}
+
+static const struct clk_ops meson_clk_cpu_ops = {
+	.recalc_rate	= meson_clk_cpu_recalc_rate,
+	.round_rate	= meson_clk_cpu_round_rate,
+	.set_rate	= meson_clk_cpu_set_rate,
+};
+
+struct clk *meson_clk_register_cpu(const struct clk_conf *clk_conf,
+				   void __iomem *reg_base,
+				   spinlock_t *lock)
+{
+	struct clk *clk;
+	struct clk *pclk;
+	struct meson_clk_cpu *clk_cpu;
+	struct clk_init_data init;
+	int ret;
+
+	clk_cpu = kzalloc(sizeof(*clk_cpu), GFP_KERNEL);
+	if (!clk_cpu)
+		return ERR_PTR(-ENOMEM);
+
+	clk_cpu->base = reg_base;
+	clk_cpu->reg_off = clk_conf->reg_off;
+	clk_cpu->div_table = clk_conf->conf.div_table;
+	clk_cpu->clk_nb.notifier_call = meson_clk_cpu_notifier_cb;
+
+	init.name = clk_conf->clk_name;
+	init.ops = &meson_clk_cpu_ops;
+	init.flags = clk_conf->flags | CLK_GET_RATE_NOCACHE;
+	init.flags |= CLK_SET_RATE_PARENT;
+	init.parent_names = clk_conf->clks_parent;
+	init.num_parents = 1;
+
+	clk_cpu->hw.init = &init;
+
+	pclk = __clk_lookup(clk_conf->clks_parent[0]);
+	if (!pclk) {
+		pr_err("%s: could not lookup parent clock %s\n",
+				__func__, clk_conf->clks_parent[0]);
+		ret = -EINVAL;
+		goto free_clk;
+	}
+
+	ret = clk_notifier_register(pclk, &clk_cpu->clk_nb);
+	if (ret) {
+		pr_err("%s: failed to register clock notifier for %s\n",
+				__func__, clk_conf->clk_name);
+		goto free_clk;
+	}
+
+	clk = clk_register(NULL, &clk_cpu->hw);
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		goto unregister_clk_nb;
+	}
+
+	return clk;
+
+unregister_clk_nb:
+	clk_notifier_unregister(pclk, &clk_cpu->clk_nb);
+free_clk:
+	kfree(clk_cpu);
+
+	return ERR_PTR(ret);
+}
+
diff --git a/drivers/clk/meson/clk-pll.c b/drivers/clk/meson/clk-pll.c
new file mode 100644
index 000000000000..664edf0708ea
--- /dev/null
+++ b/drivers/clk/meson/clk-pll.c
@@ -0,0 +1,227 @@
+/*
+ * Copyright (c) 2015 Endless Mobile, Inc.
+ * Author: Carlo Caione <carlo@endlessm.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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 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/>.
+ */
+
+/*
+ * In the most basic form, a Meson PLL is composed as follows:
+ *
+ *                     PLL
+ *      +------------------------------+
+ *      |                              |
+ * in -----[ /N ]---[ *M ]---[ >>OD ]----->> out
+ *      |         ^        ^           |
+ *      +------------------------------+
+ *                |        |
+ *               FREF     VCO
+ *
+ * out = (in * M / N) >> OD
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+
+#include "clkc.h"
+
+#define MESON_PLL_RESET				BIT(29)
+#define MESON_PLL_LOCK				BIT(31)
+
+struct meson_clk_pll {
+	struct clk_hw	hw;
+	void __iomem	*base;
+	struct pll_conf	*conf;
+	unsigned int	rate_count;
+	spinlock_t	*lock;
+};
+#define to_meson_clk_pll(_hw) container_of(_hw, struct meson_clk_pll, hw)
+
+static unsigned long meson_clk_pll_recalc_rate(struct clk_hw *hw,
+						unsigned long parent_rate)
+{
+	struct meson_clk_pll *pll = to_meson_clk_pll(hw);
+	struct parm *p;
+	unsigned long parent_rate_mhz = parent_rate / 1000000;
+	unsigned long rate_mhz;
+	u16 n, m, od;
+	u32 reg;
+
+	p = &pll->conf->n;
+	reg = readl(pll->base + p->reg_off);
+	n = PARM_GET(p->width, p->shift, reg);
+
+	p = &pll->conf->m;
+	reg = readl(pll->base + p->reg_off);
+	m = PARM_GET(p->width, p->shift, reg);
+
+	p = &pll->conf->od;
+	reg = readl(pll->base + p->reg_off);
+	od = PARM_GET(p->width, p->shift, reg);
+
+	rate_mhz = (parent_rate_mhz * m / n) >> od;
+
+	return rate_mhz * 1000000;
+}
+
+static long meson_clk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long *parent_rate)
+{
+	struct meson_clk_pll *pll = to_meson_clk_pll(hw);
+	const struct pll_rate_table *rate_table = pll->conf->rate_table;
+	int i;
+
+	for (i = 0; i < pll->rate_count; i++) {
+		if (rate <= rate_table[i].rate)
+			return rate_table[i].rate;
+	}
+
+	/* else return the smallest value */
+	return rate_table[0].rate;
+}
+
+static const struct pll_rate_table *meson_clk_get_pll_settings(struct meson_clk_pll *pll,
+							       unsigned long rate)
+{
+	const struct pll_rate_table *rate_table = pll->conf->rate_table;
+	int i;
+
+	for (i = 0; i < pll->rate_count; i++) {
+		if (rate == rate_table[i].rate)
+			return &rate_table[i];
+	}
+	return NULL;
+}
+
+static int meson_clk_pll_wait_lock(struct meson_clk_pll *pll,
+				   struct parm *p_n)
+{
+	int delay = 24000000;
+	u32 reg;
+
+	while (delay > 0) {
+		reg = readl(pll->base + p_n->reg_off);
+
+		if (reg & MESON_PLL_LOCK)
+			return 0;
+		delay--;
+	}
+	return -ETIMEDOUT;
+}
+
+static int meson_clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long parent_rate)
+{
+	struct meson_clk_pll *pll = to_meson_clk_pll(hw);
+	struct parm *p;
+	const struct pll_rate_table *rate_set;
+	unsigned long old_rate;
+	int ret = 0;
+	u32 reg;
+
+	if (parent_rate == 0 || rate == 0)
+		return -EINVAL;
+
+	old_rate = rate;
+
+	rate_set = meson_clk_get_pll_settings(pll, rate);
+	if (!rate_set)
+		return -EINVAL;
+
+	/* PLL reset */
+	p = &pll->conf->n;
+	reg = readl(pll->base + p->reg_off);
+	writel(reg | MESON_PLL_RESET, pll->base + p->reg_off);
+
+	reg = PARM_SET(p->width, p->shift, reg, rate_set->n);
+	writel(reg, pll->base + p->reg_off);
+
+	p = &pll->conf->m;
+	reg = readl(pll->base + p->reg_off);
+	reg = PARM_SET(p->width, p->shift, reg, rate_set->m);
+	writel(reg, pll->base + p->reg_off);
+
+	p = &pll->conf->od;
+	reg = readl(pll->base + p->reg_off);
+	reg = PARM_SET(p->width, p->shift, reg, rate_set->od);
+	writel(reg, pll->base + p->reg_off);
+
+	p = &pll->conf->n;
+	ret = meson_clk_pll_wait_lock(pll, p);
+	if (ret) {
+		pr_warn("%s: pll did not lock, trying to restore old rate %lu\n",
+			__func__, old_rate);
+		meson_clk_pll_set_rate(hw, old_rate, parent_rate);
+	}
+
+	return ret;
+}
+
+static const struct clk_ops meson_clk_pll_ops = {
+	.recalc_rate	= meson_clk_pll_recalc_rate,
+	.round_rate	= meson_clk_pll_round_rate,
+	.set_rate	= meson_clk_pll_set_rate,
+};
+
+static const struct clk_ops meson_clk_pll_ro_ops = {
+	.recalc_rate	= meson_clk_pll_recalc_rate,
+};
+
+struct clk *meson_clk_register_pll(const struct clk_conf *clk_conf,
+				   void __iomem *reg_base,
+				   spinlock_t *lock)
+{
+	struct clk *clk;
+	struct meson_clk_pll *clk_pll;
+	struct clk_init_data init;
+
+	clk_pll = kzalloc(sizeof(*clk_pll), GFP_KERNEL);
+	if (!clk_pll)
+		return ERR_PTR(-ENOMEM);
+
+	clk_pll->base = reg_base + clk_conf->reg_off;
+	clk_pll->lock = lock;
+	clk_pll->conf = clk_conf->conf.pll;
+
+	init.name = clk_conf->clk_name;
+	init.flags = clk_conf->flags | CLK_GET_RATE_NOCACHE;
+
+	init.parent_names = &clk_conf->clks_parent[0];
+	init.num_parents = 1;
+	init.ops = &meson_clk_pll_ro_ops;
+
+	/* If no rate_table is specified we assume the PLL is read-only */
+	if (clk_pll->conf->rate_table) {
+		int len;
+
+		for (len = 0; clk_pll->conf->rate_table[len].rate != 0; )
+			len++;
+
+		 clk_pll->rate_count = len;
+		 init.ops = &meson_clk_pll_ops;
+	}
+
+	clk_pll->hw.init = &init;
+
+	clk = clk_register(NULL, &clk_pll->hw);
+	if (IS_ERR(clk))
+		kfree(clk_pll);
+
+	return clk;
+}
diff --git a/drivers/clk/meson/clkc.c b/drivers/clk/meson/clkc.c
new file mode 100644
index 000000000000..b8c511c5e7a7
--- /dev/null
+++ b/drivers/clk/meson/clkc.c
@@ -0,0 +1,250 @@
+/*
+ * Copyright (c) 2015 Endless Mobile, Inc.
+ * Author: Carlo Caione <carlo@endlessm.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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 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/clk-provider.h>
+#include <linux/mfd/syscon.h>
+#include <linux/slab.h>
+
+#include "clkc.h"
+
+static DEFINE_SPINLOCK(clk_lock);
+
+static struct clk **clks;
+static struct clk_onecell_data clk_data;
+
+struct clk ** __init meson_clk_init(struct device_node *np,
+				   unsigned long nr_clks)
+{
+	clks = kcalloc(nr_clks, sizeof(*clks), GFP_KERNEL);
+	if (!clks)
+		return ERR_PTR(-ENOMEM);
+
+	clk_data.clks = clks;
+	clk_data.clk_num = nr_clks;
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	return clks;
+}
+
+static void meson_clk_add_lookup(struct clk *clk, unsigned int id)
+{
+	if (clks && id)
+		clks[id] = clk;
+}
+
+static struct clk * __init
+meson_clk_register_composite(const struct clk_conf *clk_conf,
+			     void __iomem *clk_base)
+{
+	struct clk *clk;
+	struct clk_mux *mux = NULL;
+	struct clk_divider *div = NULL;
+	struct clk_gate *gate = NULL;
+	const struct clk_ops *mux_ops = NULL;
+	const struct composite_conf *composite_conf;
+
+	composite_conf = clk_conf->conf.composite;
+
+	if (clk_conf->num_parents > 1) {
+		mux = kzalloc(sizeof(*mux), GFP_KERNEL);
+		if (!mux)
+			return ERR_PTR(-ENOMEM);
+
+		mux->reg = clk_base + clk_conf->reg_off
+				+ composite_conf->mux_parm.reg_off;
+		mux->shift = composite_conf->mux_parm.shift;
+		mux->mask = BIT(composite_conf->mux_parm.width) - 1;
+		mux->flags = composite_conf->mux_flags;
+		mux->lock = &clk_lock;
+		mux->table = composite_conf->mux_table;
+		mux_ops = (composite_conf->mux_flags & CLK_MUX_READ_ONLY) ?
+			  &clk_mux_ro_ops : &clk_mux_ops;
+	}
+
+	if (MESON_PARM_APPLICABLE(&composite_conf->div_parm)) {
+		div = kzalloc(sizeof(*div), GFP_KERNEL);
+		if (!div) {
+			clk = ERR_PTR(-ENOMEM);
+			goto error;
+		}
+
+		div->reg = clk_base + clk_conf->reg_off
+				+ composite_conf->div_parm.reg_off;
+		div->shift = composite_conf->div_parm.shift;
+		div->width = composite_conf->div_parm.width;
+		div->lock = &clk_lock;
+		div->flags = composite_conf->div_flags;
+		div->table = composite_conf->div_table;
+	}
+
+	if (MESON_PARM_APPLICABLE(&composite_conf->gate_parm)) {
+		gate = kzalloc(sizeof(*gate), GFP_KERNEL);
+		if (!gate) {
+			clk = ERR_PTR(-ENOMEM);
+			goto error;
+		}
+
+		gate->reg = clk_base + clk_conf->reg_off
+				+ composite_conf->div_parm.reg_off;
+		gate->bit_idx = composite_conf->gate_parm.shift;
+		gate->flags = composite_conf->gate_flags;
+		gate->lock = &clk_lock;
+	}
+
+	clk = clk_register_composite(NULL, clk_conf->clk_name,
+				    clk_conf->clks_parent,
+				    clk_conf->num_parents,
+				    mux ? &mux->hw : NULL, mux_ops,
+				    div ? &div->hw : NULL, &clk_divider_ops,
+				    gate ? &gate->hw : NULL, &clk_gate_ops,
+				    clk_conf->flags);
+	if (IS_ERR(clk))
+		goto error;
+
+	return clk;
+
+error:
+	kfree(gate);
+	kfree(div);
+	kfree(mux);
+
+	return clk;
+}
+
+static struct clk * __init
+meson_clk_register_fixed_factor(const struct clk_conf *clk_conf,
+				void __iomem *clk_base)
+{
+	struct clk *clk;
+	const struct fixed_fact_conf *fixed_fact_conf;
+	const struct parm *p;
+	unsigned int mult, div;
+	u32 reg;
+
+	fixed_fact_conf = &clk_conf->conf.fixed_fact;
+
+	mult = clk_conf->conf.fixed_fact.mult;
+	div = clk_conf->conf.fixed_fact.div;
+
+	if (!mult) {
+		mult = 1;
+		p = &fixed_fact_conf->mult_parm;
+		if (MESON_PARM_APPLICABLE(p)) {
+			reg = readl(clk_base + clk_conf->reg_off + p->reg_off);
+			mult = PARM_GET(p->width, p->shift, reg);
+		}
+	}
+
+	if (!div) {
+		div = 1;
+		p = &fixed_fact_conf->div_parm;
+		if (MESON_PARM_APPLICABLE(p)) {
+			reg = readl(clk_base + clk_conf->reg_off + p->reg_off);
+			mult = PARM_GET(p->width, p->shift, reg);
+		}
+	}
+
+	clk = clk_register_fixed_factor(NULL,
+			clk_conf->clk_name,
+			clk_conf->clks_parent[0],
+			clk_conf->flags,
+			mult, div);
+
+	return clk;
+}
+
+static struct clk * __init
+meson_clk_register_fixed_rate(const struct clk_conf *clk_conf,
+			      void __iomem *clk_base)
+{
+	struct clk *clk;
+	const struct fixed_rate_conf *fixed_rate_conf;
+	const struct parm *r;
+	unsigned long rate;
+	u32 reg;
+
+	fixed_rate_conf = &clk_conf->conf.fixed_rate;
+	rate = fixed_rate_conf->rate;
+
+	if (!rate) {
+		r = &fixed_rate_conf->rate_parm;
+		reg = readl(clk_base + clk_conf->reg_off + r->reg_off);
+		rate = PARM_GET(r->width, r->shift, reg);
+	}
+
+	rate *= 1000000;
+
+	clk = clk_register_fixed_rate(NULL,
+			clk_conf->clk_name,
+			clk_conf->num_parents
+				? clk_conf->clks_parent[0] : NULL,
+			clk_conf->flags, rate);
+
+	return clk;
+}
+
+void __init meson_clk_register_clks(const struct clk_conf *clk_confs,
+				    size_t nr_confs,
+				    void __iomem *clk_base)
+{
+	unsigned int i;
+	struct clk *clk = NULL;
+
+	for (i = 0; i < nr_confs; i++) {
+		const struct clk_conf *clk_conf = &clk_confs[i];
+
+		switch (clk_conf->clk_type) {
+		case CLK_FIXED_RATE:
+			clk = meson_clk_register_fixed_rate(clk_conf,
+							    clk_base);
+			break;
+		case CLK_FIXED_FACTOR:
+			clk = meson_clk_register_fixed_factor(clk_conf,
+							      clk_base);
+			break;
+		case CLK_COMPOSITE:
+			clk = meson_clk_register_composite(clk_conf,
+							   clk_base);
+			break;
+		case CLK_CPU:
+			clk = meson_clk_register_cpu(clk_conf, clk_base,
+						     &clk_lock);
+			break;
+		case CLK_PLL:
+			clk = meson_clk_register_pll(clk_conf, clk_base,
+						     &clk_lock);
+			break;
+		default:
+			clk = NULL;
+		}
+
+		if (!clk) {
+			pr_err("%s: unknown clock type %d\n", __func__,
+			       clk_conf->clk_type);
+			continue;
+		}
+
+		if (IS_ERR(clk)) {
+			pr_warn("%s: Unable to create %s clock\n", __func__,
+				clk_conf->clk_name);
+			continue;
+		}
+
+		meson_clk_add_lookup(clk, clk_conf->clk_id);
+	}
+}
diff --git a/drivers/clk/meson/clkc.h b/drivers/clk/meson/clkc.h
new file mode 100644
index 000000000000..609ae92cc13f
--- /dev/null
+++ b/drivers/clk/meson/clkc.h
@@ -0,0 +1,187 @@
+/*
+ * Copyright (c) 2015 Endless Mobile, Inc.
+ * Author: Carlo Caione <carlo@endlessm.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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __CLKC_H
+#define __CLKC_H
+
+#define PMASK(width)			GENMASK(width - 1, 0)
+#define SETPMASK(width, shift)		GENMASK(shift + width - 1, shift)
+#define CLRPMASK(width, shift)		(~SETPMASK(width, shift))
+
+#define PARM_GET(width, shift, reg)					\
+	(((reg) & SETPMASK(width, shift)) >> (shift))
+#define PARM_SET(width, shift, reg, val)				\
+	(((reg) & CLRPMASK(width, shift)) | (val << (shift)))
+
+#define MESON_PARM_APPLICABLE(p)		(!!((p)->width))
+
+struct parm {
+	u16	reg_off;
+	u8	shift;
+	u8	width;
+};
+#define PARM(_r, _s, _w)						\
+	{								\
+		.reg_off	= (_r),					\
+		.shift		= (_s),					\
+		.width		= (_w),					\
+	}								\
+
+struct pll_rate_table {
+	unsigned long	rate;
+	u16		m;
+	u16		n;
+	u16		od;
+};
+#define PLL_RATE(_r, _m, _n, _od)					\
+	{								\
+		.rate		= (_r),					\
+		.m		= (_m),					\
+		.n		= (_n),					\
+		.od		= (_od),				\
+	}								\
+
+struct pll_conf {
+	const struct pll_rate_table	*rate_table;
+	struct parm			m;
+	struct parm			n;
+	struct parm			od;
+};
+
+struct fixed_fact_conf {
+	unsigned int	div;
+	unsigned int	mult;
+	struct parm	div_parm;
+	struct parm	mult_parm;
+};
+
+struct fixed_rate_conf {
+	unsigned long	rate;
+	struct parm	rate_parm;
+};
+
+struct composite_conf {
+	struct parm		mux_parm;
+	struct parm		div_parm;
+	struct parm		gate_parm;
+	struct clk_div_table	*div_table;
+	u32			*mux_table;
+	u8			mux_flags;
+	u8			div_flags;
+	u8			gate_flags;
+};
+
+#define PNAME(x) static const char *x[]
+
+enum clk_type {
+	CLK_FIXED_FACTOR,
+	CLK_FIXED_RATE,
+	CLK_COMPOSITE,
+	CLK_CPU,
+	CLK_PLL,
+};
+
+struct clk_conf {
+	u16				reg_off;
+	enum clk_type			clk_type;
+	unsigned int			clk_id;
+	const char			*clk_name;
+	const char			**clks_parent;
+	int				num_parents;
+	unsigned long			flags;
+	union {
+		struct fixed_fact_conf		fixed_fact;
+		struct fixed_rate_conf		fixed_rate;
+		const struct composite_conf		*composite;
+		struct pll_conf			*pll;
+		const struct clk_div_table	*div_table;
+	} conf;
+};
+
+#define FIXED_RATE_P(_ro, _ci, _cn, _f, _c)				\
+	{								\
+		.reg_off			= (_ro),		\
+		.clk_type			= CLK_FIXED_RATE,	\
+		.clk_id				= (_ci),		\
+		.clk_name			= (_cn),		\
+		.flags				= (_f),			\
+		.conf.fixed_rate.rate_parm	= _c,			\
+	}								\
+
+#define FIXED_RATE(_ci, _cn, _f, _r)					\
+	{								\
+		.clk_type			= CLK_FIXED_RATE,	\
+		.clk_id				= (_ci),		\
+		.clk_name			= (_cn),		\
+		.flags				= (_f),			\
+		.conf.fixed_rate.rate		= (_r),			\
+	}								\
+
+#define PLL(_ro, _ci, _cn, _cp, _f, _c)					\
+	{								\
+		.reg_off			= (_ro),		\
+		.clk_type			= CLK_PLL,		\
+		.clk_id				= (_ci),		\
+		.clk_name			= (_cn),		\
+		.clks_parent			= (_cp),		\
+		.num_parents			= ARRAY_SIZE(_cp),	\
+		.flags				= (_f),			\
+		.conf.pll			= (_c),			\
+	}								\
+
+#define FIXED_FACTOR_DIV(_ci, _cn, _cp, _f, _d)				\
+	{								\
+		.clk_type			= CLK_FIXED_FACTOR,	\
+		.clk_id				= (_ci),		\
+		.clk_name			= (_cn),		\
+		.clks_parent			= (_cp),		\
+		.num_parents			= ARRAY_SIZE(_cp),	\
+		.conf.fixed_fact.div		= (_d),			\
+	}								\
+
+#define CPU(_ro, _ci, _cn, _cp, _dt)					\
+	{								\
+		.reg_off			= (_ro),		\
+		.clk_type			= CLK_CPU,		\
+		.clk_id				= (_ci),		\
+		.clk_name			= (_cn),		\
+		.clks_parent			= (_cp),		\
+		.num_parents			= ARRAY_SIZE(_cp),	\
+		.conf.div_table			= (_dt),		\
+	}								\
+
+#define COMPOSITE(_ro, _ci, _cn, _cp, _f, _c)				\
+	{								\
+		.reg_off			= (_ro),		\
+		.clk_type			= CLK_COMPOSITE,	\
+		.clk_id				= (_ci),		\
+		.clk_name			= (_cn),		\
+		.clks_parent			= (_cp),		\
+		.num_parents			= ARRAY_SIZE(_cp),	\
+		.flags				= (_f),			\
+		.conf.composite			= (_c),			\
+	}								\
+
+struct clk **meson_clk_init(struct device_node *np, unsigned long nr_clks);
+void meson_clk_register_clks(const struct clk_conf *clk_confs,
+			     unsigned int nr_confs, void __iomem *clk_base);
+struct clk *meson_clk_register_cpu(const struct clk_conf *clk_conf,
+				   void __iomem *reg_base, spinlock_t *lock);
+struct clk *meson_clk_register_pll(const struct clk_conf *clk_conf,
+				   void __iomem *reg_base, spinlock_t *lock);
+
+#endif /* __CLKC_H */
diff --git a/drivers/clk/meson/meson8b-clkc.c b/drivers/clk/meson/meson8b-clkc.c
new file mode 100644
index 000000000000..61f6d55c4ac7
--- /dev/null
+++ b/drivers/clk/meson/meson8b-clkc.c
@@ -0,0 +1,196 @@
+/*
+ * Copyright (c) 2015 Endless Mobile, Inc.
+ * Author: Carlo Caione <carlo@endlessm.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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 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-provider.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <dt-bindings/clock/meson8b-clkc.h>
+
+#include "clkc.h"
+
+#define MESON8B_REG_CTL0_ADDR		0x0000
+#define MESON8B_REG_SYS_CPU_CNTL1	0x015c
+#define MESON8B_REG_HHI_MPEG		0x0174
+#define MESON8B_REG_MALI		0x01b0
+#define MESON8B_REG_PLL_FIXED		0x0280
+#define MESON8B_REG_PLL_SYS		0x0300
+#define MESON8B_REG_PLL_VID		0x0320
+
+static const struct pll_rate_table sys_pll_rate_table[] = {
+	PLL_RATE(312000000, 52, 1, 2),
+	PLL_RATE(336000000, 56, 1, 2),
+	PLL_RATE(360000000, 60, 1, 2),
+	PLL_RATE(384000000, 64, 1, 2),
+	PLL_RATE(408000000, 68, 1, 2),
+	PLL_RATE(432000000, 72, 1, 2),
+	PLL_RATE(456000000, 76, 1, 2),
+	PLL_RATE(480000000, 80, 1, 2),
+	PLL_RATE(504000000, 84, 1, 2),
+	PLL_RATE(528000000, 88, 1, 2),
+	PLL_RATE(552000000, 92, 1, 2),
+	PLL_RATE(576000000, 96, 1, 2),
+	PLL_RATE(600000000, 50, 1, 1),
+	PLL_RATE(624000000, 52, 1, 1),
+	PLL_RATE(648000000, 54, 1, 1),
+	PLL_RATE(672000000, 56, 1, 1),
+	PLL_RATE(696000000, 58, 1, 1),
+	PLL_RATE(720000000, 60, 1, 1),
+	PLL_RATE(744000000, 62, 1, 1),
+	PLL_RATE(768000000, 64, 1, 1),
+	PLL_RATE(792000000, 66, 1, 1),
+	PLL_RATE(816000000, 68, 1, 1),
+	PLL_RATE(840000000, 70, 1, 1),
+	PLL_RATE(864000000, 72, 1, 1),
+	PLL_RATE(888000000, 74, 1, 1),
+	PLL_RATE(912000000, 76, 1, 1),
+	PLL_RATE(936000000, 78, 1, 1),
+	PLL_RATE(960000000, 80, 1, 1),
+	PLL_RATE(984000000, 82, 1, 1),
+	PLL_RATE(1008000000, 84, 1, 1),
+	PLL_RATE(1032000000, 86, 1, 1),
+	PLL_RATE(1056000000, 88, 1, 1),
+	PLL_RATE(1080000000, 90, 1, 1),
+	PLL_RATE(1104000000, 92, 1, 1),
+	PLL_RATE(1128000000, 94, 1, 1),
+	PLL_RATE(1152000000, 96, 1, 1),
+	PLL_RATE(1176000000, 98, 1, 1),
+	PLL_RATE(1200000000, 50, 1, 0),
+	PLL_RATE(1224000000, 51, 1, 0),
+	PLL_RATE(1248000000, 52, 1, 0),
+	PLL_RATE(1272000000, 53, 1, 0),
+	PLL_RATE(1296000000, 54, 1, 0),
+	PLL_RATE(1320000000, 55, 1, 0),
+	PLL_RATE(1344000000, 56, 1, 0),
+	PLL_RATE(1368000000, 57, 1, 0),
+	PLL_RATE(1392000000, 58, 1, 0),
+	PLL_RATE(1416000000, 59, 1, 0),
+	PLL_RATE(1440000000, 60, 1, 0),
+	PLL_RATE(1464000000, 61, 1, 0),
+	PLL_RATE(1488000000, 62, 1, 0),
+	PLL_RATE(1512000000, 63, 1, 0),
+	PLL_RATE(1536000000, 64, 1, 0),
+	{ /* sentinel */ },
+};
+
+static const struct clk_div_table cpu_div_table[] = {
+	{ .val = 1, .div = 1 },
+	{ .val = 2, .div = 2 },
+	{ .val = 3, .div = 3 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 6 },
+	{ .val = 4, .div = 8 },
+	{ .val = 5, .div = 10 },
+	{ .val = 6, .div = 12 },
+	{ .val = 7, .div = 14 },
+	{ .val = 8, .div = 16 },
+	{ /* sentinel */ },
+};
+
+PNAME(p_xtal)		= { "xtal" };
+PNAME(p_fclk_div)	= { "fixed_pll" };
+PNAME(p_cpu_clk)	= { "sys_pll" };
+PNAME(p_clk81)		= { "fclk_div3", "fclk_div4", "fclk_div5" };
+PNAME(p_mali)		= { "fclk_div3", "fclk_div4", "fclk_div5",
+			    "fclk_div7", "zero" };
+
+static u32 mux_table_clk81[]	= { 6, 5, 7 };
+static u32 mux_table_mali[]	= { 6, 5, 7, 4, 0 };
+
+static struct pll_conf pll_confs = {
+	.m		= PARM(0x00, 0,  9),
+	.n		= PARM(0x00, 9,  5),
+	.od		= PARM(0x00, 16, 2),
+};
+
+static struct pll_conf sys_pll_conf = {
+	.m		= PARM(0x00, 0,  9),
+	.n		= PARM(0x00, 9,  5),
+	.od		= PARM(0x00, 16, 2),
+	.rate_table	= sys_pll_rate_table,
+};
+
+static const struct composite_conf clk81_conf __initconst = {
+	.mux_table		= mux_table_clk81,
+	.mux_flags		= CLK_MUX_READ_ONLY,
+	.mux_parm		= PARM(0x00, 12, 3),
+	.div_parm		= PARM(0x00, 0, 7),
+	.gate_parm		= PARM(0x00, 7, 1),
+};
+
+static const struct composite_conf mali_conf __initconst = {
+	.mux_table		= mux_table_mali,
+	.mux_parm		= PARM(0x00, 9, 3),
+	.div_parm		= PARM(0x00, 0, 7),
+	.gate_parm		= PARM(0x00, 8, 1),
+};
+
+static const struct clk_conf meson8b_xtal_conf __initconst =
+	FIXED_RATE_P(MESON8B_REG_CTL0_ADDR, CLKID_XTAL, "xtal",
+		     CLK_IS_ROOT, PARM(0x00, 4, 7));
+
+static const struct clk_conf meson8b_clk_confs[] __initconst = {
+	FIXED_RATE(CLKID_ZERO, "zero", CLK_IS_ROOT, 0),
+	PLL(MESON8B_REG_PLL_FIXED, CLKID_PLL_FIXED, "fixed_pll",
+	    p_xtal, 0, &pll_confs),
+	PLL(MESON8B_REG_PLL_VID, CLKID_PLL_VID, "vid_pll",
+	    p_xtal, 0, &pll_confs),
+	PLL(MESON8B_REG_PLL_SYS, CLKID_PLL_SYS, "sys_pll",
+	    p_xtal, 0, &sys_pll_conf),
+	FIXED_FACTOR_DIV(CLKID_FCLK_DIV2, "fclk_div2", p_fclk_div, 0, 2),
+	FIXED_FACTOR_DIV(CLKID_FCLK_DIV3, "fclk_div3", p_fclk_div, 0, 3),
+	FIXED_FACTOR_DIV(CLKID_FCLK_DIV4, "fclk_div4", p_fclk_div, 0, 4),
+	FIXED_FACTOR_DIV(CLKID_FCLK_DIV5, "fclk_div5", p_fclk_div, 0, 5),
+	FIXED_FACTOR_DIV(CLKID_FCLK_DIV7, "fclk_div7", p_fclk_div, 0, 7),
+	CPU(MESON8B_REG_SYS_CPU_CNTL1, CLKID_CPUCLK, "a5_clk", p_cpu_clk,
+	    cpu_div_table),
+	COMPOSITE(MESON8B_REG_HHI_MPEG, CLKID_CLK81, "clk81", p_clk81,
+		  CLK_SET_RATE_NO_REPARENT | CLK_IGNORE_UNUSED, &clk81_conf),
+	COMPOSITE(MESON8B_REG_MALI, CLKID_MALI, "mali", p_mali,
+		  CLK_IGNORE_UNUSED, &mali_conf),
+};
+
+static void __init meson8b_clkc_init(struct device_node *np)
+{
+	void __iomem *clk_base;
+
+	if (!meson_clk_init(np, CLK_NR_CLKS))
+		return;
+
+	/* XTAL */
+	clk_base = of_iomap(np, 0);
+	if (!clk_base) {
+		pr_err("%s: Unable to map xtal base\n", __func__);
+		return;
+	}
+
+	meson_clk_register_clks(&meson8b_xtal_conf, 1, clk_base);
+	iounmap(clk_base);
+
+	/*  Generic clocks and PLLs */
+	clk_base = of_iomap(np, 1);
+	if (!clk_base) {
+		pr_err("%s: Unable to map clk base\n", __func__);
+		return;
+	}
+
+	meson_clk_register_clks(meson8b_clk_confs,
+				ARRAY_SIZE(meson8b_clk_confs),
+				clk_base);
+}
+CLK_OF_DECLARE(meson8b_clock, "amlogic,meson8b-clkc", meson8b_clkc_init);
diff --git a/drivers/clk/mmp/Makefile b/drivers/clk/mmp/Makefile
index 3caaf7cc169c..9d4bc41e4239 100644
--- a/drivers/clk/mmp/Makefile
+++ b/drivers/clk/mmp/Makefile
@@ -12,3 +12,5 @@ obj-$(CONFIG_MACH_MMP2_DT) += clk-of-mmp2.o
 obj-$(CONFIG_CPU_PXA168) += clk-pxa168.o
 obj-$(CONFIG_CPU_PXA910) += clk-pxa910.o
 obj-$(CONFIG_CPU_MMP2) += clk-mmp2.o
+
+obj-y += clk-of-pxa1928.o
diff --git a/drivers/clk/mmp/clk-apbc.c b/drivers/clk/mmp/clk-apbc.c
index d14120eaa71f..09d41c717c52 100644
--- a/drivers/clk/mmp/clk-apbc.c
+++ b/drivers/clk/mmp/clk-apbc.c
@@ -115,7 +115,7 @@ static void clk_apbc_unprepare(struct clk_hw *hw)
 		spin_unlock_irqrestore(apbc->lock, flags);
 }
 
-struct clk_ops clk_apbc_ops = {
+static struct clk_ops clk_apbc_ops = {
 	.prepare = clk_apbc_prepare,
 	.unprepare = clk_apbc_unprepare,
 };
diff --git a/drivers/clk/mmp/clk-apmu.c b/drivers/clk/mmp/clk-apmu.c
index abe182b2377f..cdcf2d7f321e 100644
--- a/drivers/clk/mmp/clk-apmu.c
+++ b/drivers/clk/mmp/clk-apmu.c
@@ -61,7 +61,7 @@ static void clk_apmu_disable(struct clk_hw *hw)
 		spin_unlock_irqrestore(apmu->lock, flags);
 }
 
-struct clk_ops clk_apmu_ops = {
+static struct clk_ops clk_apmu_ops = {
 	.enable = clk_apmu_enable,
 	.disable = clk_apmu_disable,
 };
diff --git a/drivers/clk/mmp/clk-mmp2.c b/drivers/clk/mmp/clk-mmp2.c
index 5c90a4230fa3..09d2832fbd78 100644
--- a/drivers/clk/mmp/clk-mmp2.c
+++ b/drivers/clk/mmp/clk-mmp2.c
@@ -63,10 +63,8 @@ static struct mmp_clk_factor_masks uart_factor_masks = {
 };
 
 static struct mmp_clk_factor_tbl uart_factor_tbl[] = {
-	{.num = 14634, .den = 2165},	/*14.745MHZ */
+	{.num = 8125, .den = 1536},	/*14.745MHZ */
 	{.num = 3521, .den = 689},	/*19.23MHZ */
-	{.num = 9679, .den = 5728},	/*58.9824MHZ */
-	{.num = 15850, .den = 9451},	/*59.429MHZ */
 };
 
 static const char *uart_parent[] = {"uart_pll", "vctcxo"};
diff --git a/drivers/clk/mmp/clk-of-mmp2.c b/drivers/clk/mmp/clk-of-mmp2.c
index 2cbc2b43ae52..251533d87c65 100644
--- a/drivers/clk/mmp/clk-of-mmp2.c
+++ b/drivers/clk/mmp/clk-of-mmp2.c
@@ -30,6 +30,7 @@
 #define APBC_TWSI4	0x7c
 #define APBC_TWSI5	0x80
 #define APBC_KPC	0x18
+#define APBC_TIMER	0x24
 #define APBC_UART0	0x2c
 #define APBC_UART1	0x30
 #define APBC_UART2	0x34
@@ -98,10 +99,8 @@ static struct mmp_clk_factor_masks uart_factor_masks = {
 };
 
 static struct mmp_clk_factor_tbl uart_factor_tbl[] = {
-	{.num = 14634, .den = 2165},	/*14.745MHZ */
+	{.num = 8125, .den = 1536},	/*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)
@@ -134,6 +133,9 @@ 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(timer_lock);
+static const char *timer_parent_names[] = {"clk32", "vctcxo_2", "vctcxo_4", "vctcxo"};
+
 static DEFINE_SPINLOCK(reset_lock);
 
 static struct mmp_param_mux_clk apbc_mux_clks[] = {
@@ -145,6 +147,7 @@ static struct mmp_param_mux_clk apbc_mux_clks[] = {
 	{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, "timer_mux", timer_parent_names, ARRAY_SIZE(timer_parent_names), CLK_SET_RATE_PARENT, APBC_TIMER, 4, 3, 0, &timer_lock},
 };
 
 static struct mmp_param_gate_clk apbc_gate_clks[] = {
@@ -170,6 +173,7 @@ static struct mmp_param_gate_clk apbc_gate_clks[] = {
 	{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},
+	{MMP2_CLK_TIMER, "timer_clk", "timer_mux", CLK_SET_RATE_PARENT, APBC_TIMER, 0x7, 0x3, 0x0, 0, &timer_lock},
 };
 
 static void mmp2_apb_periph_clk_init(struct mmp2_clk_unit *pxa_unit)
diff --git a/drivers/clk/mmp/clk-of-pxa168.c b/drivers/clk/mmp/clk-of-pxa168.c
index 5b1810dc4bd2..64eaf4141c69 100644
--- a/drivers/clk/mmp/clk-of-pxa168.c
+++ b/drivers/clk/mmp/clk-of-pxa168.c
@@ -32,6 +32,7 @@
 #define APBC_PWM1	0x10
 #define APBC_PWM2	0x14
 #define APBC_PWM3	0x18
+#define APBC_TIMER	0x34
 #define APBC_SSP0	0x81c
 #define APBC_SSP1	0x820
 #define APBC_SSP2	0x84c
@@ -58,6 +59,7 @@ 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},
+	{PXA168_CLK_USB_PLL, "usb_pll", NULL, CLK_IS_ROOT, 480000000},
 };
 
 static struct mmp_param_fixed_factor_clk fixed_factor_clks[] = {
@@ -70,6 +72,7 @@ static struct mmp_param_fixed_factor_clk fixed_factor_clks[] = {
 	{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_192, "pll1_192", "pll1_96", 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},
@@ -119,6 +122,9 @@ 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(timer_lock);
+static const char *timer_parent_names[] = {"pll1_48", "clk32", "pll1_96", "pll1_192"};
+
 static DEFINE_SPINLOCK(reset_lock);
 
 static struct mmp_param_mux_clk apbc_mux_clks[] = {
@@ -130,6 +136,7 @@ static struct mmp_param_mux_clk apbc_mux_clks[] = {
 	{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},
+	{0, "timer_mux", timer_parent_names, ARRAY_SIZE(timer_parent_names), CLK_SET_RATE_PARENT, APBC_TIMER, 4, 3, 0, &timer_lock},
 };
 
 static struct mmp_param_gate_clk apbc_gate_clks[] = {
@@ -151,6 +158,7 @@ static struct mmp_param_gate_clk apbc_gate_clks[] = {
 	{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},
+	{PXA168_CLK_TIMER, "timer_clk", "timer_mux", CLK_SET_RATE_PARENT, APBC_TIMER, 0x3, 0x3, 0x0, 0, &timer_lock},
 };
 
 static void pxa168_apb_periph_clk_init(struct pxa168_clk_unit *pxa_unit)
diff --git a/drivers/clk/mmp/clk-of-pxa1928.c b/drivers/clk/mmp/clk-of-pxa1928.c
new file mode 100644
index 000000000000..433a5ae1eae0
--- /dev/null
+++ b/drivers/clk/mmp/clk-of-pxa1928.c
@@ -0,0 +1,265 @@
+/*
+ * pxa1928 clock framework source file
+ *
+ * Copyright (C) 2015 Linaro, Ltd.
+ * Rob Herring <robh@kernel.org>
+ *
+ * Based on drivers/clk/mmp/clk-of-mmp2.c:
+ * 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/kernel.h>
+#include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include <dt-bindings/clock/marvell,pxa1928.h>
+
+#include "clk.h"
+#include "reset.h"
+
+#define MPMU_UART_PLL	0x14
+
+struct pxa1928_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[] = {
+	{0, "clk32", NULL, CLK_IS_ROOT, 32768},
+	{0, "vctcxo", NULL, CLK_IS_ROOT, 26000000},
+	{0, "pll1_624", NULL, CLK_IS_ROOT, 624000000},
+	{0, "pll5p", NULL, CLK_IS_ROOT, 832000000},
+	{0, "pll5", NULL, CLK_IS_ROOT, 1248000000},
+	{0, "usb_pll", NULL, CLK_IS_ROOT, 480000000},
+};
+
+static struct mmp_param_fixed_factor_clk fixed_factor_clks[] = {
+	{0, "pll1_d2", "pll1_624", 1, 2, 0},
+	{0, "pll1_d9", "pll1_624", 1, 9, 0},
+	{0, "pll1_d12", "pll1_624", 1, 12, 0},
+	{0, "pll1_d16", "pll1_624", 1, 16, 0},
+	{0, "pll1_d20", "pll1_624", 1, 20, 0},
+	{0, "pll1_416", "pll1_624", 2, 3, 0},
+	{0, "vctcxo_d2", "vctcxo", 1, 2, 0},
+	{0, "vctcxo_d4", "vctcxo", 1, 4, 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 = 832, .den = 234},	/*58.5MHZ */
+	{.num = 1, .den = 1},		/*26MHZ */
+};
+
+static void pxa1928_pll_init(struct pxa1928_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_416",
+				CLK_SET_RATE_PARENT,
+				pxa_unit->mpmu_base + MPMU_UART_PLL,
+				&uart_factor_masks, uart_factor_tbl,
+				ARRAY_SIZE(uart_factor_tbl), NULL);
+}
+
+static DEFINE_SPINLOCK(uart0_lock);
+static DEFINE_SPINLOCK(uart1_lock);
+static DEFINE_SPINLOCK(uart2_lock);
+static DEFINE_SPINLOCK(uart3_lock);
+static const char *uart_parent_names[] = {"uart_pll", "vctcxo"};
+
+static DEFINE_SPINLOCK(ssp0_lock);
+static DEFINE_SPINLOCK(ssp1_lock);
+static const char *ssp_parent_names[] = {"vctcxo_d4", "vctcxo_d2", "vctcxo", "pll1_d12"};
+
+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, PXA1928_CLK_UART0 * 4, 4, 3, 0, &uart0_lock},
+	{0, "uart1_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, PXA1928_CLK_UART1 * 4, 4, 3, 0, &uart1_lock},
+	{0, "uart2_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, PXA1928_CLK_UART2 * 4, 4, 3, 0, &uart2_lock},
+	{0, "uart3_mux", uart_parent_names, ARRAY_SIZE(uart_parent_names), CLK_SET_RATE_PARENT, PXA1928_CLK_UART3 * 4, 4, 3, 0, &uart3_lock},
+	{0, "ssp0_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, PXA1928_CLK_SSP0 * 4, 4, 3, 0, &ssp0_lock},
+	{0, "ssp1_mux", ssp_parent_names, ARRAY_SIZE(ssp_parent_names), CLK_SET_RATE_PARENT, PXA1928_CLK_SSP1 * 4, 4, 3, 0, &ssp1_lock},
+};
+
+static struct mmp_param_gate_clk apbc_gate_clks[] = {
+	{PXA1928_CLK_TWSI0, "twsi0_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_TWSI0 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_TWSI1, "twsi1_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_TWSI1 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_TWSI2, "twsi2_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_TWSI2 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_TWSI3, "twsi3_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_TWSI3 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_TWSI4, "twsi4_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_TWSI4 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_TWSI5, "twsi5_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_TWSI5 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_GPIO, "gpio_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_GPIO * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_KPC, "kpc_clk", "clk32", CLK_SET_RATE_PARENT, PXA1928_CLK_KPC * 4, 0x3, 0x3, 0x0, MMP_CLK_GATE_NEED_DELAY, NULL},
+	{PXA1928_CLK_RTC, "rtc_clk", "clk32", CLK_SET_RATE_PARENT, PXA1928_CLK_RTC * 4, 0x83, 0x83, 0x0, MMP_CLK_GATE_NEED_DELAY, NULL},
+	{PXA1928_CLK_PWM0, "pwm0_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_PWM0 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_PWM1, "pwm1_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_PWM1 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_PWM2, "pwm2_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_PWM2 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	{PXA1928_CLK_PWM3, "pwm3_clk", "vctcxo", CLK_SET_RATE_PARENT, PXA1928_CLK_PWM3 * 4, 0x3, 0x3, 0x0, 0, &reset_lock},
+	/* The gate clocks has mux parent. */
+	{PXA1928_CLK_UART0, "uart0_clk", "uart0_mux", CLK_SET_RATE_PARENT, PXA1928_CLK_UART0 * 4, 0x3, 0x3, 0x0, 0, &uart0_lock},
+	{PXA1928_CLK_UART1, "uart1_clk", "uart1_mux", CLK_SET_RATE_PARENT, PXA1928_CLK_UART1 * 4, 0x3, 0x3, 0x0, 0, &uart1_lock},
+	{PXA1928_CLK_UART2, "uart2_clk", "uart2_mux", CLK_SET_RATE_PARENT, PXA1928_CLK_UART2 * 4, 0x3, 0x3, 0x0, 0, &uart2_lock},
+	{PXA1928_CLK_UART3, "uart3_clk", "uart3_mux", CLK_SET_RATE_PARENT, PXA1928_CLK_UART3 * 4, 0x3, 0x3, 0x0, 0, &uart3_lock},
+	{PXA1928_CLK_SSP0, "ssp0_clk", "ssp0_mux", CLK_SET_RATE_PARENT, PXA1928_CLK_SSP0 * 4, 0x3, 0x3, 0x0, 0, &ssp0_lock},
+	{PXA1928_CLK_SSP1, "ssp1_clk", "ssp1_mux", CLK_SET_RATE_PARENT, PXA1928_CLK_SSP1 * 4, 0x3, 0x3, 0x0, 0, &ssp1_lock},
+};
+
+static void pxa1928_apb_periph_clk_init(struct pxa1928_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 DEFINE_SPINLOCK(sdh2_lock);
+static DEFINE_SPINLOCK(sdh3_lock);
+static DEFINE_SPINLOCK(sdh4_lock);
+static const char *sdh_parent_names[] = {"pll1_624", "pll5p", "pll5", "pll1_416"};
+
+static DEFINE_SPINLOCK(usb_lock);
+
+static struct mmp_param_mux_clk apmu_mux_clks[] = {
+	{0, "sdh_mux", sdh_parent_names, ARRAY_SIZE(sdh_parent_names), CLK_SET_RATE_PARENT, PXA1928_CLK_SDH0 * 4, 8, 2, 0, &sdh0_lock},
+};
+
+static struct mmp_param_div_clk apmu_div_clks[] = {
+	{0, "sdh_div", "sdh_mux", 0, PXA1928_CLK_SDH0 * 4, 10, 4, CLK_DIVIDER_ONE_BASED, &sdh0_lock},
+};
+
+static struct mmp_param_gate_clk apmu_gate_clks[] = {
+	{PXA1928_CLK_USB, "usb_clk", "usb_pll", 0, PXA1928_CLK_USB * 4, 0x9, 0x9, 0x0, 0, &usb_lock},
+	{PXA1928_CLK_HSIC, "hsic_clk", "usb_pll", 0, PXA1928_CLK_HSIC * 4, 0x9, 0x9, 0x0, 0, &usb_lock},
+	/* The gate clocks has mux parent. */
+	{PXA1928_CLK_SDH0, "sdh0_clk", "sdh_div", CLK_SET_RATE_PARENT, PXA1928_CLK_SDH0 * 4, 0x1b, 0x1b, 0x0, 0, &sdh0_lock},
+	{PXA1928_CLK_SDH1, "sdh1_clk", "sdh_div", CLK_SET_RATE_PARENT, PXA1928_CLK_SDH1 * 4, 0x1b, 0x1b, 0x0, 0, &sdh1_lock},
+	{PXA1928_CLK_SDH2, "sdh2_clk", "sdh_div", CLK_SET_RATE_PARENT, PXA1928_CLK_SDH2 * 4, 0x1b, 0x1b, 0x0, 0, &sdh2_lock},
+	{PXA1928_CLK_SDH3, "sdh3_clk", "sdh_div", CLK_SET_RATE_PARENT, PXA1928_CLK_SDH3 * 4, 0x1b, 0x1b, 0x0, 0, &sdh3_lock},
+	{PXA1928_CLK_SDH4, "sdh4_clk", "sdh_div", CLK_SET_RATE_PARENT, PXA1928_CLK_SDH4 * 4, 0x1b, 0x1b, 0x0, 0, &sdh4_lock},
+};
+
+static void pxa1928_axi_periph_clk_init(struct pxa1928_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 pxa1928_clk_reset_init(struct device_node *np,
+				struct pxa1928_clk_unit *pxa_unit)
+{
+	struct mmp_clk_reset_cell *cells;
+	int i, base, nr_resets;
+
+	nr_resets = ARRAY_SIZE(apbc_gate_clks);
+	cells = kcalloc(nr_resets, sizeof(*cells), GFP_KERNEL);
+	if (!cells)
+		return;
+
+	base = 0;
+	for (i = 0; i < nr_resets; 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;
+	}
+
+	mmp_clk_reset_register(np, cells, nr_resets);
+}
+
+static void __init pxa1928_mpmu_clk_init(struct device_node *np)
+{
+	struct pxa1928_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;
+	}
+
+	pxa1928_pll_init(pxa_unit);
+}
+CLK_OF_DECLARE(pxa1928_mpmu_clk, "marvell,pxa1928-mpmu", pxa1928_mpmu_clk_init);
+
+static void __init pxa1928_apmu_clk_init(struct device_node *np)
+{
+	struct pxa1928_clk_unit *pxa_unit;
+
+	pxa_unit = kzalloc(sizeof(*pxa_unit), GFP_KERNEL);
+	if (!pxa_unit)
+		return;
+
+	pxa_unit->apmu_base = of_iomap(np, 0);
+	if (!pxa_unit->apmu_base) {
+		pr_err("failed to map apmu registers\n");
+		return;
+	}
+
+	mmp_clk_init(np, &pxa_unit->unit, PXA1928_APMU_NR_CLKS);
+
+	pxa1928_axi_periph_clk_init(pxa_unit);
+}
+CLK_OF_DECLARE(pxa1928_apmu_clk, "marvell,pxa1928-apmu", pxa1928_apmu_clk_init);
+
+static void __init pxa1928_apbc_clk_init(struct device_node *np)
+{
+	struct pxa1928_clk_unit *pxa_unit;
+
+	pxa_unit = kzalloc(sizeof(*pxa_unit), GFP_KERNEL);
+	if (!pxa_unit)
+		return;
+
+	pxa_unit->apbc_base = of_iomap(np, 0);
+	if (!pxa_unit->apbc_base) {
+		pr_err("failed to map apbc registers\n");
+		return;
+	}
+
+	mmp_clk_init(np, &pxa_unit->unit, PXA1928_APBC_NR_CLKS);
+
+	pxa1928_apb_periph_clk_init(pxa_unit);
+	pxa1928_clk_reset_init(np, pxa_unit);
+}
+CLK_OF_DECLARE(pxa1928_apbc_clk, "marvell,pxa1928-apbc", pxa1928_apbc_clk_init);
diff --git a/drivers/clk/mmp/clk-of-pxa910.c b/drivers/clk/mmp/clk-of-pxa910.c
index 5e3c80dad336..13d6173326a4 100644
--- a/drivers/clk/mmp/clk-of-pxa910.c
+++ b/drivers/clk/mmp/clk-of-pxa910.c
@@ -35,6 +35,8 @@
 #define APBC_SSP0	0x1c
 #define APBC_SSP1	0x20
 #define APBC_SSP2	0x4c
+#define APBC_TIMER0	0x30
+#define APBC_TIMER1	0x44
 #define APBCP_TWSI1	0x28
 #define APBCP_UART2	0x1c
 #define APMU_SDH0	0x54
@@ -57,6 +59,7 @@ 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},
+	{PXA910_CLK_USB_PLL, "usb_pll", NULL, CLK_IS_ROOT, 480000000},
 };
 
 static struct mmp_param_fixed_factor_clk fixed_factor_clks[] = {
@@ -69,6 +72,7 @@ static struct mmp_param_fixed_factor_clk fixed_factor_clks[] = {
 	{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_192, "pll1_192", "pll1_96", 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},
@@ -115,6 +119,10 @@ 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(timer0_lock);
+static DEFINE_SPINLOCK(timer1_lock);
+static const char *timer_parent_names[] = {"pll1_48", "clk32", "pll1_96"};
+
 static DEFINE_SPINLOCK(reset_lock);
 
 static struct mmp_param_mux_clk apbc_mux_clks[] = {
@@ -122,6 +130,8 @@ static struct mmp_param_mux_clk apbc_mux_clks[] = {
 	{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},
+	{0, "timer0_mux", timer_parent_names, ARRAY_SIZE(timer_parent_names), CLK_SET_RATE_PARENT, APBC_TIMER0, 4, 3, 0, &timer0_lock},
+	{0, "timer1_mux", timer_parent_names, ARRAY_SIZE(timer_parent_names), CLK_SET_RATE_PARENT, APBC_TIMER1, 4, 3, 0, &timer1_lock},
 };
 
 static struct mmp_param_mux_clk apbcp_mux_clks[] = {
@@ -142,6 +152,8 @@ static struct mmp_param_gate_clk apbc_gate_clks[] = {
 	{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},
+	{PXA910_CLK_TIMER0, "timer0_clk", "timer0_mux", CLK_SET_RATE_PARENT, APBC_TIMER0, 0x3, 0x3, 0x0, 0, &timer0_lock},
+	{PXA910_CLK_TIMER1, "timer1_clk", "timer1_mux", CLK_SET_RATE_PARENT, APBC_TIMER1, 0x3, 0x3, 0x0, 0, &timer1_lock},
 };
 
 static struct mmp_param_gate_clk apbcp_gate_clks[] = {
diff --git a/drivers/clk/mvebu/armada-370.c b/drivers/clk/mvebu/armada-370.c
index 756f0f39d6a3..2c7c1085f883 100644
--- a/drivers/clk/mvebu/armada-370.c
+++ b/drivers/clk/mvebu/armada-370.c
@@ -163,6 +163,7 @@ static const struct clk_gating_soc_desc a370_gating_desc[] __initconst = {
 	{ "pex1", "pex1_en", 9, 0 },
 	{ "sata0", NULL, 15, 0 },
 	{ "sdio", NULL, 17, 0 },
+	{ "crypto", NULL, 23, CLK_IGNORE_UNUSED },
 	{ "tdm", NULL, 25, 0 },
 	{ "ddr", NULL, 28, CLK_IGNORE_UNUSED },
 	{ "sata1", NULL, 30, 0 },
diff --git a/drivers/clk/mxs/clk-imx23.c b/drivers/clk/mxs/clk-imx23.c
index 22d136aa699f..32216f9b7f03 100644
--- a/drivers/clk/mxs/clk-imx23.c
+++ b/drivers/clk/mxs/clk-imx23.c
@@ -77,12 +77,12 @@ static void __init clk_misc_init(void)
 	writel_relaxed(30 << BP_FRAC_IOFRAC, FRAC + SET);
 }
 
-static const char *sel_pll[]  __initdata = { "pll", "ref_xtal", };
-static const char *sel_cpu[]  __initdata = { "ref_cpu", "ref_xtal", };
-static const char *sel_pix[]  __initdata = { "ref_pix", "ref_xtal", };
-static const char *sel_io[]   __initdata = { "ref_io", "ref_xtal", };
-static const char *cpu_sels[] __initdata = { "cpu_pll", "cpu_xtal", };
-static const char *emi_sels[] __initdata = { "emi_pll", "emi_xtal", };
+static const char *const sel_pll[]  __initconst = { "pll", "ref_xtal", };
+static const char *const sel_cpu[]  __initconst = { "ref_cpu", "ref_xtal", };
+static const char *const sel_pix[]  __initconst = { "ref_pix", "ref_xtal", };
+static const char *const sel_io[]   __initconst = { "ref_io", "ref_xtal", };
+static const char *const cpu_sels[] __initconst = { "cpu_pll", "cpu_xtal", };
+static const char *const emi_sels[] __initconst = { "emi_pll", "emi_xtal", };
 
 enum imx23_clk {
 	ref_xtal, pll, ref_cpu, ref_emi, ref_pix, ref_io, saif_sel,
diff --git a/drivers/clk/mxs/clk-imx28.c b/drivers/clk/mxs/clk-imx28.c
index b1be3746ce95..a68670868baa 100644
--- a/drivers/clk/mxs/clk-imx28.c
+++ b/drivers/clk/mxs/clk-imx28.c
@@ -125,15 +125,15 @@ static void __init clk_misc_init(void)
 	writel_relaxed(val, FRAC0);
 }
 
-static const char *sel_cpu[]  __initdata = { "ref_cpu", "ref_xtal", };
-static const char *sel_io0[]  __initdata = { "ref_io0", "ref_xtal", };
-static const char *sel_io1[]  __initdata = { "ref_io1", "ref_xtal", };
-static const char *sel_pix[]  __initdata = { "ref_pix", "ref_xtal", };
-static const char *sel_gpmi[] __initdata = { "ref_gpmi", "ref_xtal", };
-static const char *sel_pll0[] __initdata = { "pll0", "ref_xtal", };
-static const char *cpu_sels[] __initdata = { "cpu_pll", "cpu_xtal", };
-static const char *emi_sels[] __initdata = { "emi_pll", "emi_xtal", };
-static const char *ptp_sels[] __initdata = { "ref_xtal", "pll0", };
+static const char *const sel_cpu[]  __initconst = { "ref_cpu", "ref_xtal", };
+static const char *const sel_io0[]  __initconst = { "ref_io0", "ref_xtal", };
+static const char *const sel_io1[]  __initconst = { "ref_io1", "ref_xtal", };
+static const char *const sel_pix[]  __initconst = { "ref_pix", "ref_xtal", };
+static const char *const sel_gpmi[] __initconst = { "ref_gpmi", "ref_xtal", };
+static const char *const sel_pll0[] __initconst = { "pll0", "ref_xtal", };
+static const char *const cpu_sels[] __initconst = { "cpu_pll", "cpu_xtal", };
+static const char *const emi_sels[] __initconst = { "emi_pll", "emi_xtal", };
+static const char *const ptp_sels[] __initconst = { "ref_xtal", "pll0", };
 
 enum imx28_clk {
 	ref_xtal, pll0, pll1, pll2, ref_cpu, ref_emi, ref_io0, ref_io1,
diff --git a/drivers/clk/mxs/clk.h b/drivers/clk/mxs/clk.h
index ef10ad9b5daa..f07d821dd75d 100644
--- a/drivers/clk/mxs/clk.h
+++ b/drivers/clk/mxs/clk.h
@@ -49,7 +49,7 @@ static inline struct clk *mxs_clk_gate(const char *name,
 }
 
 static inline struct clk *mxs_clk_mux(const char *name, void __iomem *reg,
-		u8 shift, u8 width, const char **parent_names, int num_parents)
+		u8 shift, u8 width, const char *const *parent_names, int num_parents)
 {
 	return clk_register_mux(NULL, name, parent_names, num_parents,
 				CLK_SET_RATE_PARENT | CLK_SET_RATE_NO_REPARENT,
diff --git a/drivers/clk/nxp/Makefile b/drivers/clk/nxp/Makefile
new file mode 100644
index 000000000000..7f608b0ad7b4
--- /dev/null
+++ b/drivers/clk/nxp/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_ARCH_LPC18XX)	+= clk-lpc18xx-cgu.o
+obj-$(CONFIG_ARCH_LPC18XX)	+= clk-lpc18xx-ccu.o
diff --git a/drivers/clk/nxp/clk-lpc18xx-ccu.c b/drivers/clk/nxp/clk-lpc18xx-ccu.c
new file mode 100644
index 000000000000..eeaee97da110
--- /dev/null
+++ b/drivers/clk/nxp/clk-lpc18xx-ccu.c
@@ -0,0 +1,293 @@
+/*
+ * Clk driver for NXP LPC18xx/LPC43xx Clock Control Unit (CCU)
+ *
+ * Copyright (C) 2015 Joachim Eastwood <manabian@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/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+
+#include <dt-bindings/clock/lpc18xx-ccu.h>
+
+/* Bit defines for CCU branch configuration register */
+#define LPC18XX_CCU_RUN		BIT(0)
+#define LPC18XX_CCU_AUTO	BIT(1)
+#define LPC18XX_CCU_DIV		BIT(5)
+#define LPC18XX_CCU_DIVSTAT	BIT(27)
+
+/* CCU branch feature bits */
+#define CCU_BRANCH_IS_BUS	BIT(0)
+#define CCU_BRANCH_HAVE_DIV2	BIT(1)
+
+#define to_clk_gate(_hw) container_of(_hw, struct clk_gate, hw)
+
+struct lpc18xx_branch_clk_data {
+	const char **name;
+	int num;
+};
+
+struct lpc18xx_clk_branch {
+	const char *base_name;
+	const char *name;
+	u16 offset;
+	u16 flags;
+	struct clk *clk;
+	struct clk_gate gate;
+};
+
+static struct lpc18xx_clk_branch clk_branches[] = {
+	{"base_apb3_clk", "apb3_bus",		CLK_APB3_BUS,		CCU_BRANCH_IS_BUS},
+	{"base_apb3_clk", "apb3_i2c1",		CLK_APB3_I2C1,		0},
+	{"base_apb3_clk", "apb3_dac",		CLK_APB3_DAC,		0},
+	{"base_apb3_clk", "apb3_adc0",		CLK_APB3_ADC0,		0},
+	{"base_apb3_clk", "apb3_adc1",		CLK_APB3_ADC1,		0},
+	{"base_apb3_clk", "apb3_can0",		CLK_APB3_CAN0,		0},
+
+	{"base_apb1_clk", "apb1_bus",		CLK_APB1_BUS,		CCU_BRANCH_IS_BUS},
+	{"base_apb1_clk", "apb1_mc_pwm",	CLK_APB1_MOTOCON_PWM,	0},
+	{"base_apb1_clk", "apb1_i2c0",		CLK_APB1_I2C0,		0},
+	{"base_apb1_clk", "apb1_i2s",		CLK_APB1_I2S,		0},
+	{"base_apb1_clk", "apb1_can1",		CLK_APB1_CAN1,		0},
+
+	{"base_spifi_clk", "spifi",		CLK_SPIFI,		0},
+
+	{"base_cpu_clk", "cpu_bus",		CLK_CPU_BUS,		CCU_BRANCH_IS_BUS},
+	{"base_cpu_clk", "cpu_spifi",		CLK_CPU_SPIFI,		0},
+	{"base_cpu_clk", "cpu_gpio",		CLK_CPU_GPIO,		0},
+	{"base_cpu_clk", "cpu_lcd",		CLK_CPU_LCD,		0},
+	{"base_cpu_clk", "cpu_ethernet",	CLK_CPU_ETHERNET,	0},
+	{"base_cpu_clk", "cpu_usb0",		CLK_CPU_USB0,		0},
+	{"base_cpu_clk", "cpu_emc",		CLK_CPU_EMC,		0},
+	{"base_cpu_clk", "cpu_sdio",		CLK_CPU_SDIO,		0},
+	{"base_cpu_clk", "cpu_dma",		CLK_CPU_DMA,		0},
+	{"base_cpu_clk", "cpu_core",		CLK_CPU_CORE,		0},
+	{"base_cpu_clk", "cpu_sct",		CLK_CPU_SCT,		0},
+	{"base_cpu_clk", "cpu_usb1",		CLK_CPU_USB1,		0},
+	{"base_cpu_clk", "cpu_emcdiv",		CLK_CPU_EMCDIV,		CCU_BRANCH_HAVE_DIV2},
+	{"base_cpu_clk", "cpu_flasha",		CLK_CPU_FLASHA,		CCU_BRANCH_HAVE_DIV2},
+	{"base_cpu_clk", "cpu_flashb",		CLK_CPU_FLASHB,		CCU_BRANCH_HAVE_DIV2},
+	{"base_cpu_clk", "cpu_m0app",		CLK_CPU_M0APP,		CCU_BRANCH_HAVE_DIV2},
+	{"base_cpu_clk", "cpu_adchs",		CLK_CPU_ADCHS,		CCU_BRANCH_HAVE_DIV2},
+	{"base_cpu_clk", "cpu_eeprom",		CLK_CPU_EEPROM,		CCU_BRANCH_HAVE_DIV2},
+	{"base_cpu_clk", "cpu_wwdt",		CLK_CPU_WWDT,		0},
+	{"base_cpu_clk", "cpu_uart0",		CLK_CPU_UART0,		0},
+	{"base_cpu_clk", "cpu_uart1",		CLK_CPU_UART1,		0},
+	{"base_cpu_clk", "cpu_ssp0",		CLK_CPU_SSP0,		0},
+	{"base_cpu_clk", "cpu_timer0",		CLK_CPU_TIMER0,		0},
+	{"base_cpu_clk", "cpu_timer1",		CLK_CPU_TIMER1,		0},
+	{"base_cpu_clk", "cpu_scu",		CLK_CPU_SCU,		0},
+	{"base_cpu_clk", "cpu_creg",		CLK_CPU_CREG,		0},
+	{"base_cpu_clk", "cpu_ritimer",		CLK_CPU_RITIMER,	0},
+	{"base_cpu_clk", "cpu_uart2",		CLK_CPU_UART2,		0},
+	{"base_cpu_clk", "cpu_uart3",		CLK_CPU_UART3,		0},
+	{"base_cpu_clk", "cpu_timer2",		CLK_CPU_TIMER2,		0},
+	{"base_cpu_clk", "cpu_timer3",		CLK_CPU_TIMER3,		0},
+	{"base_cpu_clk", "cpu_ssp1",		CLK_CPU_SSP1,		0},
+	{"base_cpu_clk", "cpu_qei",		CLK_CPU_QEI,		0},
+
+	{"base_periph_clk", "periph_bus",	CLK_PERIPH_BUS,		CCU_BRANCH_IS_BUS},
+	{"base_periph_clk", "periph_core",	CLK_PERIPH_CORE,	0},
+	{"base_periph_clk", "periph_sgpio",	CLK_PERIPH_SGPIO,	0},
+
+	{"base_usb0_clk",  "usb0",		CLK_USB0,		0},
+	{"base_usb1_clk",  "usb1",		CLK_USB1,		0},
+	{"base_spi_clk",   "spi",		CLK_SPI,		0},
+	{"base_adchs_clk", "adchs",		CLK_ADCHS,		0},
+
+	{"base_audio_clk", "audio",		CLK_AUDIO,		0},
+	{"base_uart3_clk", "apb2_uart3",	CLK_APB2_UART3,		0},
+	{"base_uart2_clk", "apb2_uart2",	CLK_APB2_UART2,		0},
+	{"base_uart1_clk", "apb0_uart1",	CLK_APB0_UART1,		0},
+	{"base_uart0_clk", "apb0_uart0",	CLK_APB0_UART0,		0},
+	{"base_ssp1_clk",  "apb2_ssp1",		CLK_APB2_SSP1,		0},
+	{"base_ssp0_clk",  "apb0_ssp0",		CLK_APB0_SSP0,		0},
+	{"base_sdio_clk",  "sdio",		CLK_SDIO,		0},
+};
+
+static struct clk *lpc18xx_ccu_branch_clk_get(struct of_phandle_args *clkspec,
+					      void *data)
+{
+	struct lpc18xx_branch_clk_data *clk_data = data;
+	unsigned int offset = clkspec->args[0];
+	int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(clk_branches); i++) {
+		if (clk_branches[i].offset != offset)
+			continue;
+
+		for (j = 0; j < clk_data->num; j++) {
+			if (!strcmp(clk_branches[i].base_name, clk_data->name[j]))
+				return clk_branches[i].clk;
+		}
+	}
+
+	pr_err("%s: invalid clock offset %d\n", __func__, offset);
+
+	return ERR_PTR(-EINVAL);
+}
+
+static int lpc18xx_ccu_gate_endisable(struct clk_hw *hw, bool enable)
+{
+	struct clk_gate *gate = to_clk_gate(hw);
+	u32 val;
+
+	/*
+	 * Divider field is write only, so divider stat field must
+	 * be read so divider field can be set accordingly.
+	 */
+	val = clk_readl(gate->reg);
+	if (val & LPC18XX_CCU_DIVSTAT)
+		val |= LPC18XX_CCU_DIV;
+
+	if (enable) {
+		val |= LPC18XX_CCU_RUN;
+	} else {
+		/*
+		 * To safely disable a branch clock a squence of two separate
+		 * writes must be used. First write should set the AUTO bit
+		 * and the next write should clear the RUN bit.
+		 */
+		val |= LPC18XX_CCU_AUTO;
+		clk_writel(val, gate->reg);
+
+		val &= ~LPC18XX_CCU_RUN;
+	}
+
+	clk_writel(val, gate->reg);
+
+	return 0;
+}
+
+static int lpc18xx_ccu_gate_enable(struct clk_hw *hw)
+{
+	return lpc18xx_ccu_gate_endisable(hw, true);
+}
+
+static void lpc18xx_ccu_gate_disable(struct clk_hw *hw)
+{
+	lpc18xx_ccu_gate_endisable(hw, false);
+}
+
+static int lpc18xx_ccu_gate_is_enabled(struct clk_hw *hw)
+{
+	struct clk_gate *gate = to_clk_gate(hw);
+
+	return clk_readl(gate->reg) & LPC18XX_CCU_RUN;
+}
+
+static const struct clk_ops lpc18xx_ccu_gate_ops = {
+	.enable		= lpc18xx_ccu_gate_enable,
+	.disable	= lpc18xx_ccu_gate_disable,
+	.is_enabled	= lpc18xx_ccu_gate_is_enabled,
+};
+
+static void lpc18xx_ccu_register_branch_gate_div(struct lpc18xx_clk_branch *branch,
+						 void __iomem *reg_base,
+						 const char *parent)
+{
+	const struct clk_ops *div_ops = NULL;
+	struct clk_divider *div = NULL;
+	struct clk_hw *div_hw = NULL;
+
+	if (branch->flags & CCU_BRANCH_HAVE_DIV2) {
+		div = kzalloc(sizeof(*div), GFP_KERNEL);
+		if (!div)
+			return;
+
+		div->reg = branch->offset + reg_base;
+		div->flags = CLK_DIVIDER_READ_ONLY;
+		div->shift = 27;
+		div->width = 1;
+
+		div_hw = &div->hw;
+		div_ops = &clk_divider_ops;
+	}
+
+	branch->gate.reg = branch->offset + reg_base;
+	branch->gate.bit_idx = 0;
+
+	branch->clk = clk_register_composite(NULL, branch->name, &parent, 1,
+					     NULL, NULL,
+					     div_hw, div_ops,
+					     &branch->gate.hw, &lpc18xx_ccu_gate_ops, 0);
+	if (IS_ERR(branch->clk)) {
+		kfree(div);
+		pr_warn("%s: failed to register %s\n", __func__, branch->name);
+		return;
+	}
+
+	/* Grab essential branch clocks for CPU and SDRAM */
+	switch (branch->offset) {
+	case CLK_CPU_EMC:
+	case CLK_CPU_CORE:
+	case CLK_CPU_CREG:
+	case CLK_CPU_EMCDIV:
+		clk_prepare_enable(branch->clk);
+	}
+}
+
+static void lpc18xx_ccu_register_branch_clks(void __iomem *reg_base,
+					     const char *base_name)
+{
+	const char *parent = base_name;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(clk_branches); i++) {
+		if (strcmp(clk_branches[i].base_name, base_name))
+			continue;
+
+		lpc18xx_ccu_register_branch_gate_div(&clk_branches[i], reg_base,
+						     parent);
+
+		if (clk_branches[i].flags & CCU_BRANCH_IS_BUS)
+			parent = clk_branches[i].name;
+	}
+}
+
+static void __init lpc18xx_ccu_init(struct device_node *np)
+{
+	struct lpc18xx_branch_clk_data *clk_data;
+	void __iomem *reg_base;
+	int i, ret;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_warn("%s: failed to map address range\n", __func__);
+		return;
+	}
+
+	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
+	if (!clk_data)
+		return;
+
+	clk_data->num = of_property_count_strings(np, "clock-names");
+	clk_data->name = kcalloc(clk_data->num, sizeof(char *), GFP_KERNEL);
+	if (!clk_data->name) {
+		kfree(clk_data);
+		return;
+	}
+
+	for (i = 0; i < clk_data->num; i++) {
+		ret = of_property_read_string_index(np, "clock-names", i,
+						    &clk_data->name[i]);
+		if (ret) {
+			pr_warn("%s: failed to get clock name at idx %d\n",
+				__func__, i);
+			continue;
+		}
+
+		lpc18xx_ccu_register_branch_clks(reg_base, clk_data->name[i]);
+	}
+
+	of_clk_add_provider(np, lpc18xx_ccu_branch_clk_get, clk_data);
+}
+CLK_OF_DECLARE(lpc18xx_ccu, "nxp,lpc1850-ccu", lpc18xx_ccu_init);
diff --git a/drivers/clk/nxp/clk-lpc18xx-cgu.c b/drivers/clk/nxp/clk-lpc18xx-cgu.c
new file mode 100644
index 000000000000..81e9e1c788f4
--- /dev/null
+++ b/drivers/clk/nxp/clk-lpc18xx-cgu.c
@@ -0,0 +1,635 @@
+/*
+ * Clk driver for NXP LPC18xx/LPC43xx Clock Generation Unit (CGU)
+ *
+ * Copyright (C) 2015 Joachim Eastwood <manabian@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/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include <dt-bindings/clock/lpc18xx-cgu.h>
+
+/* Clock Generation Unit (CGU) registers */
+#define LPC18XX_CGU_XTAL_OSC_CTRL	0x018
+#define LPC18XX_CGU_PLL0USB_STAT	0x01c
+#define LPC18XX_CGU_PLL0USB_CTRL	0x020
+#define LPC18XX_CGU_PLL0USB_MDIV	0x024
+#define LPC18XX_CGU_PLL0USB_NP_DIV	0x028
+#define LPC18XX_CGU_PLL0AUDIO_STAT	0x02c
+#define LPC18XX_CGU_PLL0AUDIO_CTRL	0x030
+#define LPC18XX_CGU_PLL0AUDIO_MDIV	0x034
+#define LPC18XX_CGU_PLL0AUDIO_NP_DIV	0x038
+#define LPC18XX_CGU_PLL0AUDIO_FRAC	0x03c
+#define LPC18XX_CGU_PLL1_STAT		0x040
+#define LPC18XX_CGU_PLL1_CTRL		0x044
+#define  LPC18XX_PLL1_CTRL_FBSEL	BIT(6)
+#define  LPC18XX_PLL1_CTRL_DIRECT	BIT(7)
+#define LPC18XX_CGU_IDIV_CTRL(n)	(0x048 + (n) * sizeof(u32))
+#define LPC18XX_CGU_BASE_CLK(id)	(0x05c + (id) * sizeof(u32))
+#define LPC18XX_CGU_PLL_CTRL_OFFSET	0x4
+
+/* PLL0 bits common to both audio and USB PLL */
+#define LPC18XX_PLL0_STAT_LOCK		BIT(0)
+#define LPC18XX_PLL0_CTRL_PD		BIT(0)
+#define LPC18XX_PLL0_CTRL_BYPASS	BIT(1)
+#define LPC18XX_PLL0_CTRL_DIRECTI	BIT(2)
+#define LPC18XX_PLL0_CTRL_DIRECTO	BIT(3)
+#define LPC18XX_PLL0_CTRL_CLKEN		BIT(4)
+#define LPC18XX_PLL0_MDIV_MDEC_MASK	0x1ffff
+#define LPC18XX_PLL0_MDIV_SELP_SHIFT	17
+#define LPC18XX_PLL0_MDIV_SELI_SHIFT	22
+#define LPC18XX_PLL0_MSEL_MAX		BIT(15)
+
+/* Register value that gives PLL0 post/pre dividers equal to 1 */
+#define LPC18XX_PLL0_NP_DIVS_1		0x00302062
+
+enum {
+	CLK_SRC_OSC32,
+	CLK_SRC_IRC,
+	CLK_SRC_ENET_RX_CLK,
+	CLK_SRC_ENET_TX_CLK,
+	CLK_SRC_GP_CLKIN,
+	CLK_SRC_RESERVED1,
+	CLK_SRC_OSC,
+	CLK_SRC_PLL0USB,
+	CLK_SRC_PLL0AUDIO,
+	CLK_SRC_PLL1,
+	CLK_SRC_RESERVED2,
+	CLK_SRC_RESERVED3,
+	CLK_SRC_IDIVA,
+	CLK_SRC_IDIVB,
+	CLK_SRC_IDIVC,
+	CLK_SRC_IDIVD,
+	CLK_SRC_IDIVE,
+	CLK_SRC_MAX
+};
+
+static const char *clk_src_names[CLK_SRC_MAX] = {
+	[CLK_SRC_OSC32]		= "osc32",
+	[CLK_SRC_IRC]		= "irc",
+	[CLK_SRC_ENET_RX_CLK]	= "enet_rx_clk",
+	[CLK_SRC_ENET_TX_CLK]	= "enet_tx_clk",
+	[CLK_SRC_GP_CLKIN]	= "gp_clkin",
+	[CLK_SRC_OSC]		= "osc",
+	[CLK_SRC_PLL0USB]	= "pll0usb",
+	[CLK_SRC_PLL0AUDIO]	= "pll0audio",
+	[CLK_SRC_PLL1]		= "pll1",
+	[CLK_SRC_IDIVA]		= "idiva",
+	[CLK_SRC_IDIVB]		= "idivb",
+	[CLK_SRC_IDIVC]		= "idivc",
+	[CLK_SRC_IDIVD]		= "idivd",
+	[CLK_SRC_IDIVE]		= "idive",
+};
+
+static const char *clk_base_names[BASE_CLK_MAX] = {
+	[BASE_SAFE_CLK]		= "base_safe_clk",
+	[BASE_USB0_CLK]		= "base_usb0_clk",
+	[BASE_PERIPH_CLK]	= "base_periph_clk",
+	[BASE_USB1_CLK]		= "base_usb1_clk",
+	[BASE_CPU_CLK]		= "base_cpu_clk",
+	[BASE_SPIFI_CLK]	= "base_spifi_clk",
+	[BASE_SPI_CLK]		= "base_spi_clk",
+	[BASE_PHY_RX_CLK]	= "base_phy_rx_clk",
+	[BASE_PHY_TX_CLK]	= "base_phy_tx_clk",
+	[BASE_APB1_CLK]		= "base_apb1_clk",
+	[BASE_APB3_CLK]		= "base_apb3_clk",
+	[BASE_LCD_CLK]		= "base_lcd_clk",
+	[BASE_ADCHS_CLK]	= "base_adchs_clk",
+	[BASE_SDIO_CLK]		= "base_sdio_clk",
+	[BASE_SSP0_CLK]		= "base_ssp0_clk",
+	[BASE_SSP1_CLK]		= "base_ssp1_clk",
+	[BASE_UART0_CLK]	= "base_uart0_clk",
+	[BASE_UART1_CLK]	= "base_uart1_clk",
+	[BASE_UART2_CLK]	= "base_uart2_clk",
+	[BASE_UART3_CLK]	= "base_uart3_clk",
+	[BASE_OUT_CLK]		= "base_out_clk",
+	[BASE_AUDIO_CLK]	= "base_audio_clk",
+	[BASE_CGU_OUT0_CLK]	= "base_cgu_out0_clk",
+	[BASE_CGU_OUT1_CLK]	= "base_cgu_out1_clk",
+};
+
+static u32 lpc18xx_cgu_pll0_src_ids[] = {
+	CLK_SRC_OSC32, CLK_SRC_IRC, CLK_SRC_ENET_RX_CLK,
+	CLK_SRC_ENET_TX_CLK, CLK_SRC_GP_CLKIN, CLK_SRC_OSC,
+	CLK_SRC_PLL1, CLK_SRC_IDIVA, CLK_SRC_IDIVB, CLK_SRC_IDIVC,
+	CLK_SRC_IDIVD, CLK_SRC_IDIVE,
+};
+
+static u32 lpc18xx_cgu_pll1_src_ids[] = {
+	CLK_SRC_OSC32, CLK_SRC_IRC, CLK_SRC_ENET_RX_CLK,
+	CLK_SRC_ENET_TX_CLK, CLK_SRC_GP_CLKIN, CLK_SRC_OSC,
+	CLK_SRC_PLL0USB, CLK_SRC_PLL0AUDIO, CLK_SRC_IDIVA,
+	CLK_SRC_IDIVB, CLK_SRC_IDIVC, CLK_SRC_IDIVD, CLK_SRC_IDIVE,
+};
+
+static u32 lpc18xx_cgu_idiva_src_ids[] = {
+	CLK_SRC_OSC32, CLK_SRC_IRC, CLK_SRC_ENET_RX_CLK,
+	CLK_SRC_ENET_TX_CLK, CLK_SRC_GP_CLKIN, CLK_SRC_OSC,
+	CLK_SRC_PLL0USB, CLK_SRC_PLL0AUDIO, CLK_SRC_PLL1
+};
+
+static u32 lpc18xx_cgu_idivbcde_src_ids[] = {
+	CLK_SRC_OSC32, CLK_SRC_IRC, CLK_SRC_ENET_RX_CLK,
+	CLK_SRC_ENET_TX_CLK, CLK_SRC_GP_CLKIN, CLK_SRC_OSC,
+	CLK_SRC_PLL0AUDIO, CLK_SRC_PLL1, CLK_SRC_IDIVA,
+};
+
+static u32 lpc18xx_cgu_base_irc_src_ids[] = {CLK_SRC_IRC};
+
+static u32 lpc18xx_cgu_base_usb0_src_ids[] = {CLK_SRC_PLL0USB};
+
+static u32 lpc18xx_cgu_base_common_src_ids[] = {
+	CLK_SRC_OSC32, CLK_SRC_IRC, CLK_SRC_ENET_RX_CLK,
+	CLK_SRC_ENET_TX_CLK, CLK_SRC_GP_CLKIN, CLK_SRC_OSC,
+	CLK_SRC_PLL0AUDIO, CLK_SRC_PLL1, CLK_SRC_IDIVA,
+	CLK_SRC_IDIVB, CLK_SRC_IDIVC, CLK_SRC_IDIVD, CLK_SRC_IDIVE,
+};
+
+static u32 lpc18xx_cgu_base_all_src_ids[] = {
+	CLK_SRC_OSC32, CLK_SRC_IRC, CLK_SRC_ENET_RX_CLK,
+	CLK_SRC_ENET_TX_CLK, CLK_SRC_GP_CLKIN, CLK_SRC_OSC,
+	CLK_SRC_PLL0USB, CLK_SRC_PLL0AUDIO, CLK_SRC_PLL1,
+	CLK_SRC_IDIVA, CLK_SRC_IDIVB, CLK_SRC_IDIVC,
+	CLK_SRC_IDIVD, CLK_SRC_IDIVE,
+};
+
+struct lpc18xx_cgu_src_clk_div {
+	u8 clk_id;
+	u8 n_parents;
+	struct clk_divider	div;
+	struct clk_mux		mux;
+	struct clk_gate		gate;
+};
+
+#define LPC1XX_CGU_SRC_CLK_DIV(_id, _width, _table)	\
+{							\
+	.clk_id = CLK_SRC_ ##_id,			\
+	.n_parents = ARRAY_SIZE(lpc18xx_cgu_ ##_table),	\
+	.div = {					\
+		.shift = 2,				\
+		.width = _width,			\
+	},						\
+	.mux = {					\
+		.mask = 0x1f,				\
+		.shift = 24,				\
+		.table = lpc18xx_cgu_ ##_table,		\
+	},						\
+	.gate = {					\
+		.bit_idx = 0,				\
+		.flags = CLK_GATE_SET_TO_DISABLE,	\
+	},						\
+}
+
+static struct lpc18xx_cgu_src_clk_div lpc18xx_cgu_src_clk_divs[] = {
+	LPC1XX_CGU_SRC_CLK_DIV(IDIVA, 2, idiva_src_ids),
+	LPC1XX_CGU_SRC_CLK_DIV(IDIVB, 4, idivbcde_src_ids),
+	LPC1XX_CGU_SRC_CLK_DIV(IDIVC, 4, idivbcde_src_ids),
+	LPC1XX_CGU_SRC_CLK_DIV(IDIVD, 4, idivbcde_src_ids),
+	LPC1XX_CGU_SRC_CLK_DIV(IDIVE, 8, idivbcde_src_ids),
+};
+
+struct lpc18xx_cgu_base_clk {
+	u8 clk_id;
+	u8 n_parents;
+	struct clk_mux mux;
+	struct clk_gate gate;
+};
+
+#define LPC1XX_CGU_BASE_CLK(_id, _table, _flags)	\
+{							\
+	.clk_id = BASE_ ##_id ##_CLK,			\
+	.n_parents = ARRAY_SIZE(lpc18xx_cgu_ ##_table),	\
+	.mux = {					\
+		.mask = 0x1f,				\
+		.shift = 24,				\
+		.table = lpc18xx_cgu_ ##_table,		\
+		.flags = _flags,			\
+	},						\
+	.gate = {					\
+		.bit_idx = 0,				\
+		.flags = CLK_GATE_SET_TO_DISABLE,	\
+	},						\
+}
+
+static struct lpc18xx_cgu_base_clk lpc18xx_cgu_base_clks[] = {
+	LPC1XX_CGU_BASE_CLK(SAFE,	base_irc_src_ids, CLK_MUX_READ_ONLY),
+	LPC1XX_CGU_BASE_CLK(USB0,	base_usb0_src_ids,   0),
+	LPC1XX_CGU_BASE_CLK(PERIPH,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(USB1,	base_all_src_ids,    0),
+	LPC1XX_CGU_BASE_CLK(CPU,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(SPIFI,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(SPI,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(PHY_RX,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(PHY_TX,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(APB1,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(APB3,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(LCD,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(ADCHS,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(SDIO,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(SSP0,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(SSP1,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(UART0,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(UART1,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(UART2,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(UART3,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(OUT,	base_all_src_ids,    0),
+	{ /* 21 reserved */ },
+	{ /* 22 reserved */ },
+	{ /* 23 reserved */ },
+	{ /* 24 reserved */ },
+	LPC1XX_CGU_BASE_CLK(AUDIO,	base_common_src_ids, 0),
+	LPC1XX_CGU_BASE_CLK(CGU_OUT0,	base_all_src_ids,    0),
+	LPC1XX_CGU_BASE_CLK(CGU_OUT1,	base_all_src_ids,    0),
+};
+
+struct lpc18xx_pll {
+	struct		clk_hw hw;
+	void __iomem	*reg;
+	spinlock_t	*lock;
+	u8		flags;
+};
+
+#define to_lpc_pll(hw) container_of(hw, struct lpc18xx_pll, hw)
+
+struct lpc18xx_cgu_pll_clk {
+	u8 clk_id;
+	u8 n_parents;
+	u8 reg_offset;
+	struct clk_mux mux;
+	struct clk_gate gate;
+	struct lpc18xx_pll pll;
+	const struct clk_ops *pll_ops;
+};
+
+#define LPC1XX_CGU_CLK_PLL(_id, _table, _pll_ops)	\
+{							\
+	.clk_id = CLK_SRC_ ##_id,			\
+	.n_parents = ARRAY_SIZE(lpc18xx_cgu_ ##_table),	\
+	.reg_offset = LPC18XX_CGU_ ##_id ##_STAT,	\
+	.mux = {					\
+		.mask = 0x1f,				\
+		.shift = 24,				\
+		.table = lpc18xx_cgu_ ##_table,		\
+	},						\
+	.gate = {					\
+		.bit_idx = 0,				\
+		.flags = CLK_GATE_SET_TO_DISABLE,	\
+	},						\
+	.pll_ops = &lpc18xx_ ##_pll_ops,		\
+}
+
+/*
+ * PLL0 uses a special register value encoding. The compute functions below
+ * are taken or derived from the LPC1850 user manual (section 12.6.3.3).
+ */
+
+/* Compute PLL0 multiplier from decoded version */
+static u32 lpc18xx_pll0_mdec2msel(u32 x)
+{
+	int i;
+
+	switch (x) {
+	case 0x18003: return 1;
+	case 0x10003: return 2;
+	default:
+		for (i = LPC18XX_PLL0_MSEL_MAX + 1; x != 0x4000 && i > 0; i--)
+			x = ((x ^ x >> 14) & 1) | (x << 1 & 0x7fff);
+		return i;
+	}
+}
+/* Compute PLL0 decoded multiplier from binary version */
+static u32 lpc18xx_pll0_msel2mdec(u32 msel)
+{
+	u32 i, x = 0x4000;
+
+	switch (msel) {
+	case 0: return 0;
+	case 1: return 0x18003;
+	case 2: return 0x10003;
+	default:
+		for (i = msel; i <= LPC18XX_PLL0_MSEL_MAX; i++)
+			x = ((x ^ x >> 1) & 1) << 14 | (x >> 1 & 0xffff);
+		return x;
+	}
+}
+
+/* Compute PLL0 bandwidth SELI reg from multiplier */
+static u32 lpc18xx_pll0_msel2seli(u32 msel)
+{
+	u32 tmp;
+
+	if (msel > 16384) return 1;
+	if (msel >  8192) return 2;
+	if (msel >  2048) return 4;
+	if (msel >=  501) return 8;
+	if (msel >=   60) {
+		tmp = 1024 / (msel + 9);
+		return ((1024 == (tmp * (msel + 9))) == 0) ? tmp * 4 : (tmp + 1) * 4;
+	}
+
+	return (msel & 0x3c) + 4;
+}
+
+/* Compute PLL0 bandwidth SELP reg from multiplier */
+static u32 lpc18xx_pll0_msel2selp(u32 msel)
+{
+	if (msel < 60)
+		return (msel >> 1) + 1;
+
+	return 31;
+}
+
+static unsigned long lpc18xx_pll0_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	struct lpc18xx_pll *pll = to_lpc_pll(hw);
+	u32 ctrl, mdiv, msel, npdiv;
+
+	ctrl = clk_readl(pll->reg + LPC18XX_CGU_PLL0USB_CTRL);
+	mdiv = clk_readl(pll->reg + LPC18XX_CGU_PLL0USB_MDIV);
+	npdiv = clk_readl(pll->reg + LPC18XX_CGU_PLL0USB_NP_DIV);
+
+	if (ctrl & LPC18XX_PLL0_CTRL_BYPASS)
+		return parent_rate;
+
+	if (npdiv != LPC18XX_PLL0_NP_DIVS_1) {
+		pr_warn("%s: pre/post dividers not supported\n", __func__);
+		return 0;
+	}
+
+	msel = lpc18xx_pll0_mdec2msel(mdiv & LPC18XX_PLL0_MDIV_MDEC_MASK);
+	if (msel)
+		return 2 * msel * parent_rate;
+
+	pr_warn("%s: unable to calculate rate\n", __func__);
+
+	return 0;
+}
+
+static long lpc18xx_pll0_round_rate(struct clk_hw *hw, unsigned long rate,
+				    unsigned long *prate)
+{
+	unsigned long m;
+
+	if (*prate < rate) {
+		pr_warn("%s: pll dividers not supported\n", __func__);
+		return -EINVAL;
+	}
+
+	m = DIV_ROUND_UP_ULL(*prate, rate * 2);
+	if (m <= 0 && m > LPC18XX_PLL0_MSEL_MAX) {
+		pr_warn("%s: unable to support rate %lu\n", __func__, rate);
+		return -EINVAL;
+	}
+
+	return 2 * *prate * m;
+}
+
+static int lpc18xx_pll0_set_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long parent_rate)
+{
+	struct lpc18xx_pll *pll = to_lpc_pll(hw);
+	u32 ctrl, stat, m;
+	int retry = 3;
+
+	if (parent_rate < rate) {
+		pr_warn("%s: pll dividers not supported\n", __func__);
+		return -EINVAL;
+	}
+
+	m = DIV_ROUND_UP_ULL(parent_rate, rate * 2);
+	if (m <= 0 && m > LPC18XX_PLL0_MSEL_MAX) {
+		pr_warn("%s: unable to support rate %lu\n", __func__, rate);
+		return -EINVAL;
+	}
+
+	m  = lpc18xx_pll0_msel2mdec(m);
+	m |= lpc18xx_pll0_msel2selp(m) << LPC18XX_PLL0_MDIV_SELP_SHIFT;
+	m |= lpc18xx_pll0_msel2seli(m) << LPC18XX_PLL0_MDIV_SELI_SHIFT;
+
+	/* Power down PLL, disable clk output and dividers */
+	ctrl = clk_readl(pll->reg + LPC18XX_CGU_PLL0USB_CTRL);
+	ctrl |= LPC18XX_PLL0_CTRL_PD;
+	ctrl &= ~(LPC18XX_PLL0_CTRL_BYPASS | LPC18XX_PLL0_CTRL_DIRECTI |
+		  LPC18XX_PLL0_CTRL_DIRECTO | LPC18XX_PLL0_CTRL_CLKEN);
+	clk_writel(ctrl, pll->reg + LPC18XX_CGU_PLL0USB_CTRL);
+
+	/* Configure new PLL settings */
+	clk_writel(m, pll->reg + LPC18XX_CGU_PLL0USB_MDIV);
+	clk_writel(LPC18XX_PLL0_NP_DIVS_1, pll->reg + LPC18XX_CGU_PLL0USB_NP_DIV);
+
+	/* Power up PLL and wait for lock */
+	ctrl &= ~LPC18XX_PLL0_CTRL_PD;
+	clk_writel(ctrl, pll->reg + LPC18XX_CGU_PLL0USB_CTRL);
+	do {
+		udelay(10);
+		stat = clk_readl(pll->reg + LPC18XX_CGU_PLL0USB_STAT);
+		if (stat & LPC18XX_PLL0_STAT_LOCK) {
+			ctrl |= LPC18XX_PLL0_CTRL_CLKEN;
+			clk_writel(ctrl, pll->reg + LPC18XX_CGU_PLL0USB_CTRL);
+
+			return 0;
+		}
+	} while (retry--);
+
+	pr_warn("%s: unable to lock pll\n", __func__);
+
+	return -EINVAL;
+}
+
+static const struct clk_ops lpc18xx_pll0_ops = {
+	.recalc_rate	= lpc18xx_pll0_recalc_rate,
+	.round_rate	= lpc18xx_pll0_round_rate,
+	.set_rate	= lpc18xx_pll0_set_rate,
+};
+
+static unsigned long lpc18xx_pll1_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	struct lpc18xx_pll *pll = to_lpc_pll(hw);
+	u16 msel, nsel, psel;
+	bool direct, fbsel;
+	u32 stat, ctrl;
+
+	stat = clk_readl(pll->reg + LPC18XX_CGU_PLL1_STAT);
+	ctrl = clk_readl(pll->reg + LPC18XX_CGU_PLL1_CTRL);
+
+	direct = (ctrl & LPC18XX_PLL1_CTRL_DIRECT) ? true : false;
+	fbsel = (ctrl & LPC18XX_PLL1_CTRL_FBSEL) ? true : false;
+
+	msel = ((ctrl >> 16) & 0xff) + 1;
+	nsel = ((ctrl >> 12) & 0x3) + 1;
+
+	if (direct || fbsel)
+		return msel * (parent_rate / nsel);
+
+	psel = (ctrl >>  8) & 0x3;
+	psel = 1 << psel;
+
+	return (msel / (2 * psel)) * (parent_rate / nsel);
+}
+
+static const struct clk_ops lpc18xx_pll1_ops = {
+	.recalc_rate = lpc18xx_pll1_recalc_rate,
+};
+
+static struct lpc18xx_cgu_pll_clk lpc18xx_cgu_src_clk_plls[] = {
+	LPC1XX_CGU_CLK_PLL(PLL0USB,	pll0_src_ids, pll0_ops),
+	LPC1XX_CGU_CLK_PLL(PLL0AUDIO,	pll0_src_ids, pll0_ops),
+	LPC1XX_CGU_CLK_PLL(PLL1,	pll1_src_ids, pll1_ops),
+};
+
+static void lpc18xx_fill_parent_names(const char **parent, u32 *id, int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++)
+		parent[i] = clk_src_names[id[i]];
+}
+
+static struct clk *lpc18xx_cgu_register_div(struct lpc18xx_cgu_src_clk_div *clk,
+					    void __iomem *base, int n)
+{
+	void __iomem *reg = base + LPC18XX_CGU_IDIV_CTRL(n);
+	const char *name = clk_src_names[clk->clk_id];
+	const char *parents[CLK_SRC_MAX];
+
+	clk->div.reg = reg;
+	clk->mux.reg = reg;
+	clk->gate.reg = reg;
+
+	lpc18xx_fill_parent_names(parents, clk->mux.table, clk->n_parents);
+
+	return clk_register_composite(NULL, name, parents, clk->n_parents,
+				      &clk->mux.hw, &clk_mux_ops,
+				      &clk->div.hw, &clk_divider_ops,
+				      &clk->gate.hw, &clk_gate_ops, 0);
+}
+
+
+static struct clk *lpc18xx_register_base_clk(struct lpc18xx_cgu_base_clk *clk,
+					     void __iomem *reg_base, int n)
+{
+	void __iomem *reg = reg_base + LPC18XX_CGU_BASE_CLK(n);
+	const char *name = clk_base_names[clk->clk_id];
+	const char *parents[CLK_SRC_MAX];
+
+	if (clk->n_parents == 0)
+		return ERR_PTR(-ENOENT);
+
+	clk->mux.reg = reg;
+	clk->gate.reg = reg;
+
+	lpc18xx_fill_parent_names(parents, clk->mux.table, clk->n_parents);
+
+	/* SAFE_CLK can not be turned off */
+	if (n == BASE_SAFE_CLK)
+		return clk_register_composite(NULL, name, parents, clk->n_parents,
+					      &clk->mux.hw, &clk_mux_ops,
+					      NULL, NULL, NULL, NULL, 0);
+
+	return clk_register_composite(NULL, name, parents, clk->n_parents,
+				      &clk->mux.hw, &clk_mux_ops,
+				      NULL,  NULL,
+				      &clk->gate.hw, &clk_gate_ops, 0);
+}
+
+
+static struct clk *lpc18xx_cgu_register_pll(struct lpc18xx_cgu_pll_clk *clk,
+					    void __iomem *base)
+{
+	const char *name = clk_src_names[clk->clk_id];
+	const char *parents[CLK_SRC_MAX];
+
+	clk->pll.reg  = base;
+	clk->mux.reg  = base + clk->reg_offset + LPC18XX_CGU_PLL_CTRL_OFFSET;
+	clk->gate.reg = base + clk->reg_offset + LPC18XX_CGU_PLL_CTRL_OFFSET;
+
+	lpc18xx_fill_parent_names(parents, clk->mux.table, clk->n_parents);
+
+	return clk_register_composite(NULL, name, parents, clk->n_parents,
+				      &clk->mux.hw, &clk_mux_ops,
+				      &clk->pll.hw, clk->pll_ops,
+				      &clk->gate.hw, &clk_gate_ops, 0);
+}
+
+static void __init lpc18xx_cgu_register_source_clks(struct device_node *np,
+						    void __iomem *base)
+{
+	const char *parents[CLK_SRC_MAX];
+	struct clk *clk;
+	int i;
+
+	/* Register the internal 12 MHz RC oscillator (IRC) */
+	clk = clk_register_fixed_rate(NULL, clk_src_names[CLK_SRC_IRC],
+				      NULL, CLK_IS_ROOT, 12000000);
+	if (IS_ERR(clk))
+		pr_warn("%s: failed to register irc clk\n", __func__);
+
+	/* Register crystal oscillator controlller */
+	parents[0] = of_clk_get_parent_name(np, 0);
+	clk = clk_register_gate(NULL, clk_src_names[CLK_SRC_OSC], parents[0],
+				0, base + LPC18XX_CGU_XTAL_OSC_CTRL,
+				0, CLK_GATE_SET_TO_DISABLE, NULL);
+	if (IS_ERR(clk))
+		pr_warn("%s: failed to register osc clk\n", __func__);
+
+	/* Register all PLLs */
+	for (i = 0; i < ARRAY_SIZE(lpc18xx_cgu_src_clk_plls); i++) {
+		clk = lpc18xx_cgu_register_pll(&lpc18xx_cgu_src_clk_plls[i],
+						   base);
+		if (IS_ERR(clk))
+			pr_warn("%s: failed to register pll (%d)\n", __func__, i);
+	}
+
+	/* Register all clock dividers A-E */
+	for (i = 0; i < ARRAY_SIZE(lpc18xx_cgu_src_clk_divs); i++) {
+		clk = lpc18xx_cgu_register_div(&lpc18xx_cgu_src_clk_divs[i],
+					       base, i);
+		if (IS_ERR(clk))
+			pr_warn("%s: failed to register div %d\n", __func__, i);
+	}
+}
+
+static struct clk *clk_base[BASE_CLK_MAX];
+static struct clk_onecell_data clk_base_data = {
+	.clks = clk_base,
+	.clk_num = BASE_CLK_MAX,
+};
+
+static void __init lpc18xx_cgu_register_base_clks(void __iomem *reg_base)
+{
+	int i;
+
+	for (i = BASE_SAFE_CLK; i < BASE_CLK_MAX; i++) {
+		clk_base[i] = lpc18xx_register_base_clk(&lpc18xx_cgu_base_clks[i],
+							reg_base, i);
+		if (IS_ERR(clk_base[i]) && PTR_ERR(clk_base[i]) != -ENOENT)
+			pr_warn("%s: register base clk %d failed\n", __func__, i);
+	}
+}
+
+static void __init lpc18xx_cgu_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_warn("%s: failed to map address range\n", __func__);
+		return;
+	}
+
+	lpc18xx_cgu_register_source_clks(np, reg_base);
+	lpc18xx_cgu_register_base_clks(reg_base);
+
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_base_data);
+}
+CLK_OF_DECLARE(lpc18xx_cgu, "nxp,lpc1850-cgu", lpc18xx_cgu_init);
diff --git a/drivers/clk/pistachio/clk-pll.c b/drivers/clk/pistachio/clk-pll.c
index de537560bf70..e17dada0dd21 100644
--- a/drivers/clk/pistachio/clk-pll.c
+++ b/drivers/clk/pistachio/clk-pll.c
@@ -6,9 +6,12 @@
  * version 2, as published by the Free Software Foundation.
  */
 
+#define pr_fmt(fmt) "%s: " fmt, __func__
+
 #include <linux/clk-provider.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
+#include <linux/printk.h>
 #include <linux/slab.h>
 
 #include "clk.h"
@@ -50,6 +53,18 @@
 #define PLL_CTRL4			0x10
 #define PLL_FRAC_CTRL4_BYPASS		BIT(28)
 
+#define MIN_PFD				9600000UL
+#define MIN_VCO_LA			400000000UL
+#define MAX_VCO_LA			1600000000UL
+#define MIN_VCO_FRAC_INT		600000000UL
+#define MAX_VCO_FRAC_INT		1600000000UL
+#define MIN_VCO_FRAC_FRAC		600000000UL
+#define MAX_VCO_FRAC_FRAC		2400000000UL
+#define MIN_OUTPUT_LA			8000000UL
+#define MAX_OUTPUT_LA			1600000000UL
+#define MIN_OUTPUT_FRAC			12000000UL
+#define MAX_OUTPUT_FRAC			1600000000UL
+
 struct pistachio_clk_pll {
 	struct clk_hw hw;
 	void __iomem *base;
@@ -67,6 +82,12 @@ static inline void pll_writel(struct pistachio_clk_pll *pll, u32 val, u32 reg)
 	writel(val, pll->base + reg);
 }
 
+static inline void pll_lock(struct pistachio_clk_pll *pll)
+{
+	while (!(pll_readl(pll, PLL_STATUS) & PLL_STATUS_LOCK))
+		cpu_relax();
+}
+
 static inline u32 do_div_round_closest(u64 dividend, u32 divisor)
 {
 	dividend += divisor / 2;
@@ -124,6 +145,8 @@ static int pll_gf40lp_frac_enable(struct clk_hw *hw)
 	val &= ~PLL_FRAC_CTRL4_BYPASS;
 	pll_writel(pll, val, PLL_CTRL4);
 
+	pll_lock(pll);
+
 	return 0;
 }
 
@@ -149,16 +172,29 @@ static int pll_gf40lp_frac_set_rate(struct clk_hw *hw, unsigned long rate,
 {
 	struct pistachio_clk_pll *pll = to_pistachio_pll(hw);
 	struct pistachio_pll_rate_table *params;
-	bool was_enabled;
-	u32 val;
+	int enabled = pll_gf40lp_frac_is_enabled(hw);
+	u32 val, vco, old_postdiv1, old_postdiv2;
+	const char *name = __clk_get_name(hw->clk);
+
+	if (rate < MIN_OUTPUT_FRAC || rate > MAX_OUTPUT_FRAC)
+		return -EINVAL;
 
 	params = pll_get_params(pll, parent_rate, rate);
-	if (!params)
+	if (!params || !params->refdiv)
 		return -EINVAL;
 
-	was_enabled = pll_gf40lp_frac_is_enabled(hw);
-	if (!was_enabled)
-		pll_gf40lp_frac_enable(hw);
+	vco = params->fref * params->fbdiv / params->refdiv;
+	if (vco < MIN_VCO_FRAC_FRAC || vco > MAX_VCO_FRAC_FRAC)
+		pr_warn("%s: VCO %u is out of range %lu..%lu\n", name, vco,
+			MIN_VCO_FRAC_FRAC, MAX_VCO_FRAC_FRAC);
+
+	val = params->fref / params->refdiv;
+	if (val < MIN_PFD)
+		pr_warn("%s: PFD %u is too low (min %lu)\n",
+			name, val, MIN_PFD);
+	if (val > vco / 16)
+		pr_warn("%s: PFD %u is too high (max %u)\n",
+			name, val, vco / 16);
 
 	val = pll_readl(pll, PLL_CTRL1);
 	val &= ~((PLL_CTRL1_REFDIV_MASK << PLL_CTRL1_REFDIV_SHIFT) |
@@ -168,6 +204,19 @@ static int pll_gf40lp_frac_set_rate(struct clk_hw *hw, unsigned long rate,
 	pll_writel(pll, val, PLL_CTRL1);
 
 	val = pll_readl(pll, PLL_CTRL2);
+
+	old_postdiv1 = (val >> PLL_FRAC_CTRL2_POSTDIV1_SHIFT) &
+		       PLL_FRAC_CTRL2_POSTDIV1_MASK;
+	old_postdiv2 = (val >> PLL_FRAC_CTRL2_POSTDIV2_SHIFT) &
+		       PLL_FRAC_CTRL2_POSTDIV2_MASK;
+	if (enabled &&
+	    (params->postdiv1 != old_postdiv1 ||
+	     params->postdiv2 != old_postdiv2))
+		pr_warn("%s: changing postdiv while PLL is enabled\n", name);
+
+	if (params->postdiv2 > params->postdiv1)
+		pr_warn("%s: postdiv2 should not exceed postdiv1\n", name);
+
 	val &= ~((PLL_FRAC_CTRL2_FRAC_MASK << PLL_FRAC_CTRL2_FRAC_SHIFT) |
 		 (PLL_FRAC_CTRL2_POSTDIV1_MASK <<
 		  PLL_FRAC_CTRL2_POSTDIV1_SHIFT) |
@@ -178,11 +227,8 @@ static int pll_gf40lp_frac_set_rate(struct clk_hw *hw, unsigned long rate,
 		(params->postdiv2 << PLL_FRAC_CTRL2_POSTDIV2_SHIFT);
 	pll_writel(pll, val, PLL_CTRL2);
 
-	while (!(pll_readl(pll, PLL_STATUS) & PLL_STATUS_LOCK))
-		cpu_relax();
-
-	if (!was_enabled)
-		pll_gf40lp_frac_disable(hw);
+	if (enabled)
+		pll_lock(pll);
 
 	return 0;
 }
@@ -241,6 +287,8 @@ static int pll_gf40lp_laint_enable(struct clk_hw *hw)
 	val &= ~PLL_INT_CTRL2_BYPASS;
 	pll_writel(pll, val, PLL_CTRL2);
 
+	pll_lock(pll);
+
 	return 0;
 }
 
@@ -266,18 +314,44 @@ static int pll_gf40lp_laint_set_rate(struct clk_hw *hw, unsigned long rate,
 {
 	struct pistachio_clk_pll *pll = to_pistachio_pll(hw);
 	struct pistachio_pll_rate_table *params;
-	bool was_enabled;
-	u32 val;
+	int enabled = pll_gf40lp_laint_is_enabled(hw);
+	u32 val, vco, old_postdiv1, old_postdiv2;
+	const char *name = __clk_get_name(hw->clk);
+
+	if (rate < MIN_OUTPUT_LA || rate > MAX_OUTPUT_LA)
+		return -EINVAL;
 
 	params = pll_get_params(pll, parent_rate, rate);
-	if (!params)
+	if (!params || !params->refdiv)
 		return -EINVAL;
 
-	was_enabled = pll_gf40lp_laint_is_enabled(hw);
-	if (!was_enabled)
-		pll_gf40lp_laint_enable(hw);
+	vco = params->fref * params->fbdiv / params->refdiv;
+	if (vco < MIN_VCO_LA || vco > MAX_VCO_LA)
+		pr_warn("%s: VCO %u is out of range %lu..%lu\n", name, vco,
+			MIN_VCO_LA, MAX_VCO_LA);
+
+	val = params->fref / params->refdiv;
+	if (val < MIN_PFD)
+		pr_warn("%s: PFD %u is too low (min %lu)\n",
+			name, val, MIN_PFD);
+	if (val > vco / 16)
+		pr_warn("%s: PFD %u is too high (max %u)\n",
+			name, val, vco / 16);
 
 	val = pll_readl(pll, PLL_CTRL1);
+
+	old_postdiv1 = (val >> PLL_INT_CTRL1_POSTDIV1_SHIFT) &
+		       PLL_INT_CTRL1_POSTDIV1_MASK;
+	old_postdiv2 = (val >> PLL_INT_CTRL1_POSTDIV2_SHIFT) &
+		       PLL_INT_CTRL1_POSTDIV2_MASK;
+	if (enabled &&
+	    (params->postdiv1 != old_postdiv1 ||
+	     params->postdiv2 != old_postdiv2))
+		pr_warn("%s: changing postdiv while PLL is enabled\n", name);
+
+	if (params->postdiv2 > params->postdiv1)
+		pr_warn("%s: postdiv2 should not exceed postdiv1\n", name);
+
 	val &= ~((PLL_CTRL1_REFDIV_MASK << PLL_CTRL1_REFDIV_SHIFT) |
 		 (PLL_CTRL1_FBDIV_MASK << PLL_CTRL1_FBDIV_SHIFT) |
 		 (PLL_INT_CTRL1_POSTDIV1_MASK << PLL_INT_CTRL1_POSTDIV1_SHIFT) |
@@ -288,11 +362,8 @@ static int pll_gf40lp_laint_set_rate(struct clk_hw *hw, unsigned long rate,
 		(params->postdiv2 << PLL_INT_CTRL1_POSTDIV2_SHIFT);
 	pll_writel(pll, val, PLL_CTRL1);
 
-	while (!(pll_readl(pll, PLL_STATUS) & PLL_STATUS_LOCK))
-		cpu_relax();
-
-	if (!was_enabled)
-		pll_gf40lp_laint_disable(hw);
+	if (enabled)
+		pll_lock(pll);
 
 	return 0;
 }
diff --git a/drivers/clk/pxa/clk-pxa.h b/drivers/clk/pxa/clk-pxa.h
index b04c5b9c0ea8..d1de805df867 100644
--- a/drivers/clk/pxa/clk-pxa.h
+++ b/drivers/clk/pxa/clk-pxa.h
@@ -14,7 +14,7 @@
 #define _CLK_PXA_
 
 #define PARENTS(name) \
-	static const char *name ## _parents[] __initdata
+	static const char *const name ## _parents[] __initconst
 #define MUX_RO_RATE_RO_OPS(name, clk_name)			\
 	static struct clk_hw name ## _mux_hw;			\
 	static struct clk_hw name ## _rate_hw;			\
@@ -72,7 +72,7 @@ struct desc_clk_cken {
 	const char *name;
 	const char *dev_id;
 	const char *con_id;
-	const char **parent_names;
+	const char * const *parent_names;
 	struct clk_fixed_factor lp;
 	struct clk_fixed_factor hp;
 	struct clk_gate gate;
diff --git a/drivers/clk/pxa/clk-pxa27x.c b/drivers/clk/pxa/clk-pxa27x.c
index 5f9b54b024b9..9a31b77eed23 100644
--- a/drivers/clk/pxa/clk-pxa27x.c
+++ b/drivers/clk/pxa/clk-pxa27x.c
@@ -353,6 +353,34 @@ static u8 clk_pxa27x_memory_get_parent(struct clk_hw *hw)
 PARENTS(clk_pxa27x_memory) = { "osc_13mhz", "system_bus", "run" };
 MUX_RO_RATE_RO_OPS(clk_pxa27x_memory, "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, "pxa27x-gpio", "osc_32_768khz"),
+	DUMMY_CLK(NULL, "sa1100-rtc", "osc_32_768khz"),
+	DUMMY_CLK("UARTCLK", "pxa2xx-ir", "STUART"),
+};
+
+static void __init pxa27x_dummy_clocks_init(void)
+{
+	struct clk *clk;
+	struct dummy_clk *d;
+	const char *name;
+	int i;
+
+	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);
+	}
+}
+
 static void __init pxa27x_base_clocks_init(void)
 {
 	pxa27x_register_plls();
@@ -362,12 +390,12 @@ static void __init pxa27x_base_clocks_init(void)
 	clk_register_clk_pxa27x_lcd_base();
 }
 
-static int __init pxa27x_clocks_init(void)
+int __init pxa27x_clocks_init(void)
 {
 	pxa27x_base_clocks_init();
+	pxa27x_dummy_clocks_init();
 	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)
 {
diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index b95d17fbb8d7..92936f0912d2 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -530,19 +530,16 @@ static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	struct freq_tbl f = *rcg->freq_tbl;
 	const struct frac_entry *frac = frac_table_pixel;
-	unsigned long request, src_rate;
+	unsigned long request;
 	int delta = 100000;
 	u32 mask = BIT(rcg->hid_width) - 1;
 	u32 hid_div;
-	int index = qcom_find_src_index(hw, rcg->parent_map, f.src);
-	struct clk *parent = clk_get_parent_by_index(hw->clk, index);
 
 	for (; frac->num; frac++) {
 		request = (rate * frac->den) / frac->num;
 
-		src_rate = __clk_round_rate(parent, request);
-		if ((src_rate < (request - delta)) ||
-			(src_rate > (request + delta)))
+		if ((parent_rate < (request - delta)) ||
+			(parent_rate > (request + delta)))
 			continue;
 
 		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
diff --git a/drivers/clk/qcom/gcc-ipq806x.c b/drivers/clk/qcom/gcc-ipq806x.c
index a50936a17376..563969942a1d 100644
--- a/drivers/clk/qcom/gcc-ipq806x.c
+++ b/drivers/clk/qcom/gcc-ipq806x.c
@@ -140,12 +140,47 @@ static struct clk_regmap pll14_vote = {
 	},
 };
 
+#define NSS_PLL_RATE(f, _l, _m, _n, i) \
+	{  \
+		.freq = f,  \
+		.l = _l, \
+		.m = _m, \
+		.n = _n, \
+		.ibits = i, \
+	}
+
+static struct pll_freq_tbl pll18_freq_tbl[] = {
+	NSS_PLL_RATE(550000000, 44, 0, 1, 0x01495625),
+	NSS_PLL_RATE(733000000, 58, 16, 25, 0x014b5625),
+};
+
+static struct clk_pll pll18 = {
+	.l_reg = 0x31a4,
+	.m_reg = 0x31a8,
+	.n_reg = 0x31ac,
+	.config_reg = 0x31b4,
+	.mode_reg = 0x31a0,
+	.status_reg = 0x31b8,
+	.status_bit = 16,
+	.post_div_shift = 16,
+	.post_div_width = 1,
+	.freq_tbl = pll18_freq_tbl,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pll18",
+		.parent_names = (const char *[]){ "pxo" },
+		.num_parents = 1,
+		.ops = &clk_pll_ops,
+	},
+};
+
 enum {
 	P_PXO,
 	P_PLL8,
 	P_PLL3,
 	P_PLL0,
 	P_CXO,
+	P_PLL14,
+	P_PLL18,
 };
 
 static const struct parent_map gcc_pxo_pll8_map[] = {
@@ -197,6 +232,22 @@ static const char *gcc_pxo_pll8_pll0_map[] = {
 	"pll0_vote",
 };
 
+static const struct parent_map gcc_pxo_pll8_pll14_pll18_pll0_map[] = {
+	{ P_PXO, 0 },
+	{ P_PLL8, 4 },
+	{ P_PLL0, 2 },
+	{ P_PLL14, 5 },
+	{ P_PLL18, 1 }
+};
+
+static const char *gcc_pxo_pll8_pll14_pll18_pll0[] = {
+	"pxo",
+	"pll8_vote",
+	"pll0_vote",
+	"pll14",
+	"pll18",
+};
+
 static struct freq_tbl clk_tbl_gsbi_uart[] = {
 	{  1843200, P_PLL8, 2,  6, 625 },
 	{  3686400, P_PLL8, 2, 12, 625 },
@@ -2202,6 +2253,472 @@ static struct clk_branch ebi2_aon_clk = {
 	},
 };
 
+static const struct freq_tbl clk_tbl_gmac[] = {
+	{ 133000000, P_PLL0, 1,  50, 301 },
+	{ 266000000, P_PLL0, 1, 127, 382 },
+	{ }
+};
+
+static struct clk_dyn_rcg gmac_core1_src = {
+	.ns_reg[0] = 0x3cac,
+	.ns_reg[1] = 0x3cb0,
+	.md_reg[0] = 0x3ca4,
+	.md_reg[1] = 0x3ca8,
+	.bank_reg = 0x3ca0,
+	.mn[0] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.mn[1] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.s[0] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.s[1] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.p[0] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.p[1] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.mux_sel_bit = 0,
+	.freq_tbl = clk_tbl_gmac,
+	.clkr = {
+		.enable_reg = 0x3ca0,
+		.enable_mask = BIT(1),
+		.hw.init = &(struct clk_init_data){
+			.name = "gmac_core1_src",
+			.parent_names = gcc_pxo_pll8_pll14_pll18_pll0,
+			.num_parents = 5,
+			.ops = &clk_dyn_rcg_ops,
+		},
+	},
+};
+
+static struct clk_branch gmac_core1_clk = {
+	.halt_reg = 0x3c20,
+	.halt_bit = 4,
+	.hwcg_reg = 0x3cb4,
+	.hwcg_bit = 6,
+	.clkr = {
+		.enable_reg = 0x3cb4,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gmac_core1_clk",
+			.parent_names = (const char *[]){
+				"gmac_core1_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_dyn_rcg gmac_core2_src = {
+	.ns_reg[0] = 0x3ccc,
+	.ns_reg[1] = 0x3cd0,
+	.md_reg[0] = 0x3cc4,
+	.md_reg[1] = 0x3cc8,
+	.bank_reg = 0x3ca0,
+	.mn[0] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.mn[1] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.s[0] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.s[1] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.p[0] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.p[1] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.mux_sel_bit = 0,
+	.freq_tbl = clk_tbl_gmac,
+	.clkr = {
+		.enable_reg = 0x3cc0,
+		.enable_mask = BIT(1),
+		.hw.init = &(struct clk_init_data){
+			.name = "gmac_core2_src",
+			.parent_names = gcc_pxo_pll8_pll14_pll18_pll0,
+			.num_parents = 5,
+			.ops = &clk_dyn_rcg_ops,
+		},
+	},
+};
+
+static struct clk_branch gmac_core2_clk = {
+	.halt_reg = 0x3c20,
+	.halt_bit = 5,
+	.hwcg_reg = 0x3cd4,
+	.hwcg_bit = 6,
+	.clkr = {
+		.enable_reg = 0x3cd4,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gmac_core2_clk",
+			.parent_names = (const char *[]){
+				"gmac_core2_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_dyn_rcg gmac_core3_src = {
+	.ns_reg[0] = 0x3cec,
+	.ns_reg[1] = 0x3cf0,
+	.md_reg[0] = 0x3ce4,
+	.md_reg[1] = 0x3ce8,
+	.bank_reg = 0x3ce0,
+	.mn[0] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.mn[1] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.s[0] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.s[1] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.p[0] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.p[1] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.mux_sel_bit = 0,
+	.freq_tbl = clk_tbl_gmac,
+	.clkr = {
+		.enable_reg = 0x3ce0,
+		.enable_mask = BIT(1),
+		.hw.init = &(struct clk_init_data){
+			.name = "gmac_core3_src",
+			.parent_names = gcc_pxo_pll8_pll14_pll18_pll0,
+			.num_parents = 5,
+			.ops = &clk_dyn_rcg_ops,
+		},
+	},
+};
+
+static struct clk_branch gmac_core3_clk = {
+	.halt_reg = 0x3c20,
+	.halt_bit = 6,
+	.hwcg_reg = 0x3cf4,
+	.hwcg_bit = 6,
+	.clkr = {
+		.enable_reg = 0x3cf4,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gmac_core3_clk",
+			.parent_names = (const char *[]){
+				"gmac_core3_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static struct clk_dyn_rcg gmac_core4_src = {
+	.ns_reg[0] = 0x3d0c,
+	.ns_reg[1] = 0x3d10,
+	.md_reg[0] = 0x3d04,
+	.md_reg[1] = 0x3d08,
+	.bank_reg = 0x3d00,
+	.mn[0] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.mn[1] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.s[0] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.s[1] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.p[0] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.p[1] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.mux_sel_bit = 0,
+	.freq_tbl = clk_tbl_gmac,
+	.clkr = {
+		.enable_reg = 0x3d00,
+		.enable_mask = BIT(1),
+		.hw.init = &(struct clk_init_data){
+			.name = "gmac_core4_src",
+			.parent_names = gcc_pxo_pll8_pll14_pll18_pll0,
+			.num_parents = 5,
+			.ops = &clk_dyn_rcg_ops,
+		},
+	},
+};
+
+static struct clk_branch gmac_core4_clk = {
+	.halt_reg = 0x3c20,
+	.halt_bit = 7,
+	.hwcg_reg = 0x3d14,
+	.hwcg_bit = 6,
+	.clkr = {
+		.enable_reg = 0x3d14,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gmac_core4_clk",
+			.parent_names = (const char *[]){
+				"gmac_core4_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static const struct freq_tbl clk_tbl_nss_tcm[] = {
+	{ 266000000, P_PLL0, 3, 0, 0 },
+	{ 400000000, P_PLL0, 2, 0, 0 },
+	{ }
+};
+
+static struct clk_dyn_rcg nss_tcm_src = {
+	.ns_reg[0] = 0x3dc4,
+	.ns_reg[1] = 0x3dc8,
+	.bank_reg = 0x3dc0,
+	.s[0] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.s[1] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.p[0] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 4,
+	},
+	.p[1] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 4,
+	},
+	.mux_sel_bit = 0,
+	.freq_tbl = clk_tbl_nss_tcm,
+	.clkr = {
+		.enable_reg = 0x3dc0,
+		.enable_mask = BIT(1),
+		.hw.init = &(struct clk_init_data){
+			.name = "nss_tcm_src",
+			.parent_names = gcc_pxo_pll8_pll14_pll18_pll0,
+			.num_parents = 5,
+			.ops = &clk_dyn_rcg_ops,
+		},
+	},
+};
+
+static struct clk_branch nss_tcm_clk = {
+	.halt_reg = 0x3c20,
+	.halt_bit = 14,
+	.clkr = {
+		.enable_reg = 0x3dd0,
+		.enable_mask = BIT(6) | BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "nss_tcm_clk",
+			.parent_names = (const char *[]){
+				"nss_tcm_src",
+			},
+			.num_parents = 1,
+			.ops = &clk_branch_ops,
+			.flags = CLK_SET_RATE_PARENT,
+		},
+	},
+};
+
+static const struct freq_tbl clk_tbl_nss[] = {
+	{ 110000000, P_PLL18, 1, 1, 5 },
+	{ 275000000, P_PLL18, 2, 0, 0 },
+	{ 550000000, P_PLL18, 1, 0, 0 },
+	{ 733000000, P_PLL18, 1, 0, 0 },
+	{ }
+};
+
+static struct clk_dyn_rcg ubi32_core1_src_clk = {
+	.ns_reg[0] = 0x3d2c,
+	.ns_reg[1] = 0x3d30,
+	.md_reg[0] = 0x3d24,
+	.md_reg[1] = 0x3d28,
+	.bank_reg = 0x3d20,
+	.mn[0] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.mn[1] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.s[0] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.s[1] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.p[0] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.p[1] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.mux_sel_bit = 0,
+	.freq_tbl = clk_tbl_nss,
+	.clkr = {
+		.enable_reg = 0x3d20,
+		.enable_mask = BIT(1),
+		.hw.init = &(struct clk_init_data){
+			.name = "ubi32_core1_src_clk",
+			.parent_names = gcc_pxo_pll8_pll14_pll18_pll0,
+			.num_parents = 5,
+			.ops = &clk_dyn_rcg_ops,
+			.flags = CLK_SET_RATE_PARENT | CLK_GET_RATE_NOCACHE,
+		},
+	},
+};
+
+static struct clk_dyn_rcg ubi32_core2_src_clk = {
+	.ns_reg[0] = 0x3d4c,
+	.ns_reg[1] = 0x3d50,
+	.md_reg[0] = 0x3d44,
+	.md_reg[1] = 0x3d48,
+	.bank_reg = 0x3d40,
+	.mn[0] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.mn[1] = {
+		.mnctr_en_bit = 8,
+		.mnctr_reset_bit = 7,
+		.mnctr_mode_shift = 5,
+		.n_val_shift = 16,
+		.m_val_shift = 16,
+		.width = 8,
+	},
+	.s[0] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.s[1] = {
+		.src_sel_shift = 0,
+		.parent_map = gcc_pxo_pll8_pll14_pll18_pll0_map,
+	},
+	.p[0] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.p[1] = {
+		.pre_div_shift = 3,
+		.pre_div_width = 2,
+	},
+	.mux_sel_bit = 0,
+	.freq_tbl = clk_tbl_nss,
+	.clkr = {
+		.enable_reg = 0x3d40,
+		.enable_mask = BIT(1),
+		.hw.init = &(struct clk_init_data){
+			.name = "ubi32_core2_src_clk",
+			.parent_names = gcc_pxo_pll8_pll14_pll18_pll0,
+			.num_parents = 5,
+			.ops = &clk_dyn_rcg_ops,
+			.flags = CLK_SET_RATE_PARENT | CLK_GET_RATE_NOCACHE,
+		},
+	},
+};
+
 static struct clk_regmap *gcc_ipq806x_clks[] = {
 	[PLL0] = &pll0.clkr,
 	[PLL0_VOTE] = &pll0_vote,
@@ -2211,6 +2728,7 @@ static struct clk_regmap *gcc_ipq806x_clks[] = {
 	[PLL8_VOTE] = &pll8_vote,
 	[PLL14] = &pll14.clkr,
 	[PLL14_VOTE] = &pll14_vote,
+	[PLL18] = &pll18.clkr,
 	[GSBI1_UART_SRC] = &gsbi1_uart_src.clkr,
 	[GSBI1_UART_CLK] = &gsbi1_uart_clk.clkr,
 	[GSBI2_UART_SRC] = &gsbi2_uart_src.clkr,
@@ -2307,6 +2825,18 @@ static struct clk_regmap *gcc_ipq806x_clks[] = {
 	[USB_FS1_SYSTEM_CLK] = &usb_fs1_sys_clk.clkr,
 	[EBI2_CLK] = &ebi2_clk.clkr,
 	[EBI2_AON_CLK] = &ebi2_aon_clk.clkr,
+	[GMAC_CORE1_CLK_SRC] = &gmac_core1_src.clkr,
+	[GMAC_CORE1_CLK] = &gmac_core1_clk.clkr,
+	[GMAC_CORE2_CLK_SRC] = &gmac_core2_src.clkr,
+	[GMAC_CORE2_CLK] = &gmac_core2_clk.clkr,
+	[GMAC_CORE3_CLK_SRC] = &gmac_core3_src.clkr,
+	[GMAC_CORE3_CLK] = &gmac_core3_clk.clkr,
+	[GMAC_CORE4_CLK_SRC] = &gmac_core4_src.clkr,
+	[GMAC_CORE4_CLK] = &gmac_core4_clk.clkr,
+	[UBI32_CORE1_CLK_SRC] = &ubi32_core1_src_clk.clkr,
+	[UBI32_CORE2_CLK_SRC] = &ubi32_core2_src_clk.clkr,
+	[NSSTCM_CLK_SRC] = &nss_tcm_src.clkr,
+	[NSSTCM_CLK] = &nss_tcm_clk.clkr,
 };
 
 static const struct qcom_reset_map gcc_ipq806x_resets[] = {
@@ -2425,6 +2955,48 @@ static const struct qcom_reset_map gcc_ipq806x_resets[] = {
 	[USB30_1_PHY_RESET] = { 0x3b58, 0 },
 	[NSSFB0_RESET] = { 0x3b60, 6 },
 	[NSSFB1_RESET] = { 0x3b60, 7 },
+	[UBI32_CORE1_CLKRST_CLAMP_RESET] = { 0x3d3c, 3},
+	[UBI32_CORE1_CLAMP_RESET] = { 0x3d3c, 2 },
+	[UBI32_CORE1_AHB_RESET] = { 0x3d3c, 1 },
+	[UBI32_CORE1_AXI_RESET] = { 0x3d3c, 0 },
+	[UBI32_CORE2_CLKRST_CLAMP_RESET] = { 0x3d5c, 3 },
+	[UBI32_CORE2_CLAMP_RESET] = { 0x3d5c, 2 },
+	[UBI32_CORE2_AHB_RESET] = { 0x3d5c, 1 },
+	[UBI32_CORE2_AXI_RESET] = { 0x3d5c, 0 },
+	[GMAC_CORE1_RESET] = { 0x3cbc, 0 },
+	[GMAC_CORE2_RESET] = { 0x3cdc, 0 },
+	[GMAC_CORE3_RESET] = { 0x3cfc, 0 },
+	[GMAC_CORE4_RESET] = { 0x3d1c, 0 },
+	[GMAC_AHB_RESET] = { 0x3e24, 0 },
+	[NSS_CH0_RST_RX_CLK_N_RESET] = { 0x3b60, 0 },
+	[NSS_CH0_RST_TX_CLK_N_RESET] = { 0x3b60, 1 },
+	[NSS_CH0_RST_RX_125M_N_RESET] = { 0x3b60, 2 },
+	[NSS_CH0_HW_RST_RX_125M_N_RESET] = { 0x3b60, 3 },
+	[NSS_CH0_RST_TX_125M_N_RESET] = { 0x3b60, 4 },
+	[NSS_CH1_RST_RX_CLK_N_RESET] = { 0x3b60, 5 },
+	[NSS_CH1_RST_TX_CLK_N_RESET] = { 0x3b60, 6 },
+	[NSS_CH1_RST_RX_125M_N_RESET] = { 0x3b60, 7 },
+	[NSS_CH1_HW_RST_RX_125M_N_RESET] = { 0x3b60, 8 },
+	[NSS_CH1_RST_TX_125M_N_RESET] = { 0x3b60, 9 },
+	[NSS_CH2_RST_RX_CLK_N_RESET] = { 0x3b60, 10 },
+	[NSS_CH2_RST_TX_CLK_N_RESET] = { 0x3b60, 11 },
+	[NSS_CH2_RST_RX_125M_N_RESET] = { 0x3b60, 12 },
+	[NSS_CH2_HW_RST_RX_125M_N_RESET] = { 0x3b60, 13 },
+	[NSS_CH2_RST_TX_125M_N_RESET] = { 0x3b60, 14 },
+	[NSS_CH3_RST_RX_CLK_N_RESET] = { 0x3b60, 15 },
+	[NSS_CH3_RST_TX_CLK_N_RESET] = { 0x3b60, 16 },
+	[NSS_CH3_RST_RX_125M_N_RESET] = { 0x3b60, 17 },
+	[NSS_CH3_HW_RST_RX_125M_N_RESET] = { 0x3b60, 18 },
+	[NSS_CH3_RST_TX_125M_N_RESET] = { 0x3b60, 19 },
+	[NSS_RST_RX_250M_125M_N_RESET] = { 0x3b60, 20 },
+	[NSS_RST_TX_250M_125M_N_RESET] = { 0x3b60, 21 },
+	[NSS_QSGMII_TXPI_RST_N_RESET] = { 0x3b60, 22 },
+	[NSS_QSGMII_CDR_RST_N_RESET] = { 0x3b60, 23 },
+	[NSS_SGMII2_CDR_RST_N_RESET] = { 0x3b60, 24 },
+	[NSS_SGMII3_CDR_RST_N_RESET] = { 0x3b60, 25 },
+	[NSS_CAL_PRBS_RST_N_RESET] = { 0x3b60, 26 },
+	[NSS_LCKDT_RST_N_RESET] = { 0x3b60, 27 },
+	[NSS_SRDS_N_RESET] = { 0x3b60, 28 },
 };
 
 static const struct regmap_config gcc_ipq806x_regmap_config = {
@@ -2453,6 +3025,8 @@ static int gcc_ipq806x_probe(struct platform_device *pdev)
 {
 	struct clk *clk;
 	struct device *dev = &pdev->dev;
+	struct regmap *regmap;
+	int ret;
 
 	/* Temporary until RPM clocks supported */
 	clk = clk_register_fixed_rate(dev, "cxo", NULL, CLK_IS_ROOT, 25000000);
@@ -2463,7 +3037,25 @@ static int gcc_ipq806x_probe(struct platform_device *pdev)
 	if (IS_ERR(clk))
 		return PTR_ERR(clk);
 
-	return qcom_cc_probe(pdev, &gcc_ipq806x_desc);
+	ret = qcom_cc_probe(pdev, &gcc_ipq806x_desc);
+	if (ret)
+		return ret;
+
+	regmap = dev_get_regmap(dev, NULL);
+	if (!regmap)
+		return -ENODEV;
+
+	/* Setup PLL18 static bits */
+	regmap_update_bits(regmap, 0x31a4, 0xffffffc0, 0x40000400);
+	regmap_write(regmap, 0x31b0, 0x3080);
+
+	/* Set GMAC footswitch sleep/wakeup values */
+	regmap_write(regmap, 0x3cb8, 8);
+	regmap_write(regmap, 0x3cd8, 8);
+	regmap_write(regmap, 0x3cf8, 8);
+	regmap_write(regmap, 0x3d18, 8);
+
+	return 0;
 }
 
 static int gcc_ipq806x_remove(struct platform_device *pdev)
diff --git a/drivers/clk/rockchip/clk-cpu.c b/drivers/clk/rockchip/clk-cpu.c
index 8539c4fd34cc..fb7721bd37e6 100644
--- a/drivers/clk/rockchip/clk-cpu.c
+++ b/drivers/clk/rockchip/clk-cpu.c
@@ -231,7 +231,7 @@ static int rockchip_cpuclk_notifier_cb(struct notifier_block *nb,
 }
 
 struct clk *rockchip_clk_register_cpuclk(const char *name,
-			const char **parent_names, u8 num_parents,
+			const char *const *parent_names, u8 num_parents,
 			const struct rockchip_cpuclk_reg_data *reg_data,
 			const struct rockchip_cpuclk_rate_table *rates,
 			int nrates, void __iomem *reg_base, spinlock_t *lock)
diff --git a/drivers/clk/rockchip/clk-mmc-phase.c b/drivers/clk/rockchip/clk-mmc-phase.c
index c842e3b60f21..e9f8df324e7c 100644
--- a/drivers/clk/rockchip/clk-mmc-phase.c
+++ b/drivers/clk/rockchip/clk-mmc-phase.c
@@ -120,7 +120,7 @@ static const struct clk_ops rockchip_mmc_clk_ops = {
 };
 
 struct clk *rockchip_clk_register_mmc(const char *name,
-				const char **parent_names, u8 num_parents,
+				const char *const *parent_names, u8 num_parents,
 				void __iomem *reg, int shift)
 {
 	struct clk_init_data init;
diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c
index f8d3baf275b2..76027261f7ed 100644
--- a/drivers/clk/rockchip/clk-pll.c
+++ b/drivers/clk/rockchip/clk-pll.c
@@ -329,10 +329,10 @@ static const struct clk_ops rockchip_rk3066_pll_clk_ops = {
  */
 
 struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
-		const char *name, const char **parent_names, u8 num_parents,
-		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,
+		const char *name, const char *const *parent_names,
+		u8 num_parents, 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,
 		u8 clk_pll_flags, spinlock_t *lock)
 {
 	const char *pll_parents[3];
diff --git a/drivers/clk/rockchip/clk-rk3188.c b/drivers/clk/rockchip/clk-rk3188.c
index 556ce041d371..e4f9d472f1ff 100644
--- a/drivers/clk/rockchip/clk-rk3188.c
+++ b/drivers/clk/rockchip/clk-rk3188.c
@@ -26,7 +26,7 @@ enum rk3188_plls {
 	apll, cpll, dpll, gpll,
 };
 
-struct rockchip_pll_rate_table rk3188_pll_rates[] = {
+static struct rockchip_pll_rate_table rk3188_pll_rates[] = {
 	RK3066_PLL_RATE(2208000000, 1, 92, 1),
 	RK3066_PLL_RATE(2184000000, 1, 91, 1),
 	RK3066_PLL_RATE(2160000000, 1, 90, 1),
diff --git a/drivers/clk/rockchip/clk-rk3288.c b/drivers/clk/rockchip/clk-rk3288.c
index d17eb4528a28..4f817ed9e6ee 100644
--- a/drivers/clk/rockchip/clk-rk3288.c
+++ b/drivers/clk/rockchip/clk-rk3288.c
@@ -27,7 +27,7 @@ enum rk3288_plls {
 	apll, dpll, cpll, gpll, npll,
 };
 
-struct rockchip_pll_rate_table rk3288_pll_rates[] = {
+static struct rockchip_pll_rate_table rk3288_pll_rates[] = {
 	RK3066_PLL_RATE(2208000000, 1, 92, 1),
 	RK3066_PLL_RATE(2184000000, 1, 91, 1),
 	RK3066_PLL_RATE(2160000000, 1, 90, 1),
diff --git a/drivers/clk/rockchip/clk.c b/drivers/clk/rockchip/clk.c
index edb5d489ae61..052b94db0ff9 100644
--- a/drivers/clk/rockchip/clk.c
+++ b/drivers/clk/rockchip/clk.c
@@ -39,7 +39,7 @@
  * sometimes without one of those components.
  */
 static struct clk *rockchip_clk_register_branch(const char *name,
-		const char **parent_names, u8 num_parents, void __iomem *base,
+		const char *const *parent_names, u8 num_parents, void __iomem *base,
 		int muxdiv_offset, u8 mux_shift, u8 mux_width, u8 mux_flags,
 		u8 div_shift, u8 div_width, u8 div_flags,
 		struct clk_div_table *div_table, int gate_offset,
@@ -103,8 +103,8 @@ static struct clk *rockchip_clk_register_branch(const char *name,
 }
 
 static struct clk *rockchip_clk_register_frac_branch(const char *name,
-		const char **parent_names, u8 num_parents, void __iomem *base,
-		int muxdiv_offset, u8 div_flags,
+		const char *const *parent_names, u8 num_parents,
+		void __iomem *base, int muxdiv_offset, u8 div_flags,
 		int gate_offset, u8 gate_shift, u8 gate_flags,
 		unsigned long flags, spinlock_t *lock)
 {
@@ -297,7 +297,7 @@ void __init rockchip_clk_register_branches(
 }
 
 void __init rockchip_clk_register_armclk(unsigned int lookup_id,
-			const char *name, const char **parent_names,
+			const char *name, const char *const *parent_names,
 			u8 num_parents,
 			const struct rockchip_cpuclk_reg_data *reg_data,
 			const struct rockchip_cpuclk_rate_table *rates,
diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h
index e63cafe893e1..6b092673048a 100644
--- a/drivers/clk/rockchip/clk.h
+++ b/drivers/clk/rockchip/clk.h
@@ -108,7 +108,7 @@ struct rockchip_pll_rate_table {
 struct rockchip_pll_clock {
 	unsigned int		id;
 	const char		*name;
-	const char		**parent_names;
+	const char		*const *parent_names;
 	u8			num_parents;
 	unsigned long		flags;
 	int			con_offset;
@@ -140,10 +140,10 @@ struct rockchip_pll_clock {
 	}
 
 struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
-		const char *name, const char **parent_names, u8 num_parents,
-		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,
+		const char *name, const char *const *parent_names,
+		u8 num_parents, 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,
 		u8 clk_pll_flags, spinlock_t *lock);
 
 struct rockchip_cpuclk_clksel {
@@ -173,16 +173,16 @@ struct rockchip_cpuclk_reg_data {
 };
 
 struct clk *rockchip_clk_register_cpuclk(const char *name,
-			const char **parent_names, u8 num_parents,
+			const char *const *parent_names, u8 num_parents,
 			const struct rockchip_cpuclk_reg_data *reg_data,
 			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,
+				const char *const *parent_names, u8 num_parents,
 				void __iomem *reg, int shift);
 
-#define PNAME(x) static const char *x[] __initdata
+#define PNAME(x) static const char *const x[] __initconst
 
 enum rockchip_clk_branch_type {
 	branch_composite,
@@ -197,7 +197,7 @@ struct rockchip_clk_branch {
 	unsigned int			id;
 	enum rockchip_clk_branch_type	branch_type;
 	const char			*name;
-	const char			**parent_names;
+	const char			*const *parent_names;
 	u8				num_parents;
 	unsigned long			flags;
 	int				muxdiv_offset;
@@ -403,7 +403,7 @@ void rockchip_clk_register_branches(struct rockchip_clk_branch *clk_list,
 void rockchip_clk_register_plls(struct rockchip_pll_clock *pll_list,
 				unsigned int nr_pll, int grf_lock_offset);
 void rockchip_clk_register_armclk(unsigned int lookup_id, const char *name,
-			const char **parent_names, u8 num_parents,
+			const char *const *parent_names, u8 num_parents,
 			const struct rockchip_cpuclk_reg_data *reg_data,
 			const struct rockchip_cpuclk_rate_table *rates,
 			int nrates);
diff --git a/drivers/clk/samsung/Makefile b/drivers/clk/samsung/Makefile
index a17683b2cf27..5f6833ea355d 100644
--- a/drivers/clk/samsung/Makefile
+++ b/drivers/clk/samsung/Makefile
@@ -2,7 +2,7 @@
 # Samsung Clock specific Makefile
 #
 
-obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-pll.o
+obj-$(CONFIG_COMMON_CLK)	+= clk.o clk-pll.o clk-cpu.o
 obj-$(CONFIG_SOC_EXYNOS3250)	+= clk-exynos3250.o
 obj-$(CONFIG_ARCH_EXYNOS4)	+= clk-exynos4.o
 obj-$(CONFIG_SOC_EXYNOS4415)	+= clk-exynos4415.o
diff --git a/drivers/clk/samsung/clk-cpu.c b/drivers/clk/samsung/clk-cpu.c
new file mode 100644
index 000000000000..3a1fe07cfe9e
--- /dev/null
+++ b/drivers/clk/samsung/clk-cpu.c
@@ -0,0 +1,349 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Thomas Abraham <thomas.ab@samsung.com>
+ *
+ * Copyright (c) 2015 Samsung Electronics Co., Ltd.
+ * Bartlomiej Zolnierkiewicz <b.zolnierkie@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This file contains the utility function to register CPU clock for Samsung
+ * Exynos platforms. A CPU clock is defined as a clock supplied to a CPU or a
+ * group of CPUs. The CPU clock is typically derived from a hierarchy of clock
+ * blocks which includes mux and divider blocks. There are a number of other
+ * auxiliary clocks supplied to the CPU domain such as the debug blocks and AXI
+ * clock for CPU domain. The rates of these auxiliary clocks are related to the
+ * CPU clock rate and this relation is usually specified in the hardware manual
+ * of the SoC or supplied after the SoC characterization.
+ *
+ * The below implementation of the CPU clock allows the rate changes of the CPU
+ * clock and the corresponding rate changes of the auxillary clocks of the CPU
+ * domain. The platform clock driver provides a clock register configuration
+ * for each configurable rate which is then used to program the clock hardware
+ * registers to acheive a fast co-oridinated rate change for all the CPU domain
+ * clocks.
+ *
+ * On a rate change request for the CPU clock, the rate change is propagated
+ * upto the PLL supplying the clock to the CPU domain clock blocks. While the
+ * CPU domain PLL is reconfigured, the CPU domain clocks are driven using an
+ * alternate clock source. If required, the alternate clock source is divided
+ * down in order to keep the output clock rate within the previous OPP limits.
+*/
+
+#include <linux/errno.h>
+#include "clk-cpu.h"
+
+#define E4210_SRC_CPU		0x0
+#define E4210_STAT_CPU		0x200
+#define E4210_DIV_CPU0		0x300
+#define E4210_DIV_CPU1		0x304
+#define E4210_DIV_STAT_CPU0	0x400
+#define E4210_DIV_STAT_CPU1	0x404
+
+#define E4210_DIV0_RATIO0_MASK	0x7
+#define E4210_DIV1_HPM_MASK	(0x7 << 4)
+#define E4210_DIV1_COPY_MASK	(0x7 << 0)
+#define E4210_MUX_HPM_MASK	(1 << 20)
+#define E4210_DIV0_ATB_SHIFT	16
+#define E4210_DIV0_ATB_MASK	(DIV_MASK << E4210_DIV0_ATB_SHIFT)
+
+#define MAX_DIV			8
+#define DIV_MASK		7
+#define DIV_MASK_ALL		0xffffffff
+#define MUX_MASK		7
+
+/*
+ * Helper function to wait until divider(s) have stabilized after the divider
+ * value has changed.
+ */
+static void wait_until_divider_stable(void __iomem *div_reg, unsigned long mask)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(10);
+
+	do {
+		if (!(readl(div_reg) & mask))
+			return;
+	} while (time_before(jiffies, timeout));
+
+	if (!(readl(div_reg) & mask))
+		return;
+
+	pr_err("%s: timeout in divider stablization\n", __func__);
+}
+
+/*
+ * Helper function to wait until mux has stabilized after the mux selection
+ * value was changed.
+ */
+static void wait_until_mux_stable(void __iomem *mux_reg, u32 mux_pos,
+					unsigned long mux_value)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(10);
+
+	do {
+		if (((readl(mux_reg) >> mux_pos) & MUX_MASK) == mux_value)
+			return;
+	} while (time_before(jiffies, timeout));
+
+	if (((readl(mux_reg) >> mux_pos) & MUX_MASK) == mux_value)
+		return;
+
+	pr_err("%s: re-parenting mux timed-out\n", __func__);
+}
+
+/* common round rate callback useable for all types of CPU clocks */
+static long exynos_cpuclk_round_rate(struct clk_hw *hw,
+			unsigned long drate, unsigned long *prate)
+{
+	struct clk *parent = __clk_get_parent(hw->clk);
+	*prate = __clk_round_rate(parent, drate);
+	return *prate;
+}
+
+/* common recalc rate callback useable for all types of CPU clocks */
+static unsigned long exynos_cpuclk_recalc_rate(struct clk_hw *hw,
+			unsigned long parent_rate)
+{
+	/*
+	 * The CPU clock output (armclk) rate is the same as its parent
+	 * rate. Although there exist certain dividers inside the CPU
+	 * clock block that could be used to divide the parent clock,
+	 * the driver does not make use of them currently, except during
+	 * frequency transitions.
+	 */
+	return parent_rate;
+}
+
+static const struct clk_ops exynos_cpuclk_clk_ops = {
+	.recalc_rate = exynos_cpuclk_recalc_rate,
+	.round_rate = exynos_cpuclk_round_rate,
+};
+
+/*
+ * Helper function to set the 'safe' dividers for the CPU clock. The parameters
+ * div and mask contain the divider value and the register bit mask of the
+ * dividers to be programmed.
+ */
+static void exynos_set_safe_div(void __iomem *base, unsigned long div,
+					unsigned long mask)
+{
+	unsigned long div0;
+
+	div0 = readl(base + E4210_DIV_CPU0);
+	div0 = (div0 & ~mask) | (div & mask);
+	writel(div0, base + E4210_DIV_CPU0);
+	wait_until_divider_stable(base + E4210_DIV_STAT_CPU0, mask);
+}
+
+/* handler for pre-rate change notification from parent clock */
+static int exynos_cpuclk_pre_rate_change(struct clk_notifier_data *ndata,
+			struct exynos_cpuclk *cpuclk, void __iomem *base)
+{
+	const struct exynos_cpuclk_cfg_data *cfg_data = cpuclk->cfg;
+	unsigned long alt_prate = clk_get_rate(cpuclk->alt_parent);
+	unsigned long alt_div = 0, alt_div_mask = DIV_MASK;
+	unsigned long div0, div1 = 0, mux_reg;
+
+	/* find out the divider values to use for clock data */
+	while ((cfg_data->prate * 1000) != ndata->new_rate) {
+		if (cfg_data->prate == 0)
+			return -EINVAL;
+		cfg_data++;
+	}
+
+	spin_lock(cpuclk->lock);
+
+	/*
+	 * For the selected PLL clock frequency, get the pre-defined divider
+	 * values. If the clock for sclk_hpm is not sourced from apll, then
+	 * the values for DIV_COPY and DIV_HPM dividers need not be set.
+	 */
+	div0 = cfg_data->div0;
+	if (test_bit(CLK_CPU_HAS_DIV1, &cpuclk->flags)) {
+		div1 = cfg_data->div1;
+		if (readl(base + E4210_SRC_CPU) & E4210_MUX_HPM_MASK)
+			div1 = readl(base + E4210_DIV_CPU1) &
+				(E4210_DIV1_HPM_MASK | E4210_DIV1_COPY_MASK);
+	}
+
+	/*
+	 * If the old parent clock speed is less than the clock speed of
+	 * the alternate parent, then it should be ensured that at no point
+	 * the armclk speed is more than the old_prate until the dividers are
+	 * set.  Also workaround the issue of the dividers being set to lower
+	 * values before the parent clock speed is set to new lower speed
+	 * (this can result in too high speed of armclk output clocks).
+	 */
+	if (alt_prate > ndata->old_rate || ndata->old_rate > ndata->new_rate) {
+		unsigned long tmp_rate = min(ndata->old_rate, ndata->new_rate);
+
+		alt_div = DIV_ROUND_UP(alt_prate, tmp_rate) - 1;
+		WARN_ON(alt_div >= MAX_DIV);
+
+		if (test_bit(CLK_CPU_NEEDS_DEBUG_ALT_DIV, &cpuclk->flags)) {
+			/*
+			 * In Exynos4210, ATB clock parent is also mout_core. So
+			 * ATB clock also needs to be mantained at safe speed.
+			 */
+			alt_div |= E4210_DIV0_ATB_MASK;
+			alt_div_mask |= E4210_DIV0_ATB_MASK;
+		}
+		exynos_set_safe_div(base, alt_div, alt_div_mask);
+		div0 |= alt_div;
+	}
+
+	/* select sclk_mpll as the alternate parent */
+	mux_reg = readl(base + E4210_SRC_CPU);
+	writel(mux_reg | (1 << 16), base + E4210_SRC_CPU);
+	wait_until_mux_stable(base + E4210_STAT_CPU, 16, 2);
+
+	/* alternate parent is active now. set the dividers */
+	writel(div0, base + E4210_DIV_CPU0);
+	wait_until_divider_stable(base + E4210_DIV_STAT_CPU0, DIV_MASK_ALL);
+
+	if (test_bit(CLK_CPU_HAS_DIV1, &cpuclk->flags)) {
+		writel(div1, base + E4210_DIV_CPU1);
+		wait_until_divider_stable(base + E4210_DIV_STAT_CPU1,
+				DIV_MASK_ALL);
+	}
+
+	spin_unlock(cpuclk->lock);
+	return 0;
+}
+
+/* handler for post-rate change notification from parent clock */
+static int exynos_cpuclk_post_rate_change(struct clk_notifier_data *ndata,
+			struct exynos_cpuclk *cpuclk, void __iomem *base)
+{
+	const struct exynos_cpuclk_cfg_data *cfg_data = cpuclk->cfg;
+	unsigned long div = 0, div_mask = DIV_MASK;
+	unsigned long mux_reg;
+
+	/* find out the divider values to use for clock data */
+	if (test_bit(CLK_CPU_NEEDS_DEBUG_ALT_DIV, &cpuclk->flags)) {
+		while ((cfg_data->prate * 1000) != ndata->new_rate) {
+			if (cfg_data->prate == 0)
+				return -EINVAL;
+			cfg_data++;
+		}
+	}
+
+	spin_lock(cpuclk->lock);
+
+	/* select mout_apll as the alternate parent */
+	mux_reg = readl(base + E4210_SRC_CPU);
+	writel(mux_reg & ~(1 << 16), base + E4210_SRC_CPU);
+	wait_until_mux_stable(base + E4210_STAT_CPU, 16, 1);
+
+	if (test_bit(CLK_CPU_NEEDS_DEBUG_ALT_DIV, &cpuclk->flags)) {
+		div |= (cfg_data->div0 & E4210_DIV0_ATB_MASK);
+		div_mask |= E4210_DIV0_ATB_MASK;
+	}
+
+	exynos_set_safe_div(base, div, div_mask);
+	spin_unlock(cpuclk->lock);
+	return 0;
+}
+
+/*
+ * This notifier function is called for the pre-rate and post-rate change
+ * notifications of the parent clock of cpuclk.
+ */
+static int exynos_cpuclk_notifier_cb(struct notifier_block *nb,
+				unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct exynos_cpuclk *cpuclk;
+	void __iomem *base;
+	int err = 0;
+
+	cpuclk = container_of(nb, struct exynos_cpuclk, clk_nb);
+	base = cpuclk->ctrl_base;
+
+	if (event == PRE_RATE_CHANGE)
+		err = exynos_cpuclk_pre_rate_change(ndata, cpuclk, base);
+	else if (event == POST_RATE_CHANGE)
+		err = exynos_cpuclk_post_rate_change(ndata, cpuclk, base);
+
+	return notifier_from_errno(err);
+}
+
+/* helper function to register a CPU clock */
+int __init exynos_register_cpu_clock(struct samsung_clk_provider *ctx,
+		unsigned int lookup_id, const char *name, const char *parent,
+		const char *alt_parent, unsigned long offset,
+		const struct exynos_cpuclk_cfg_data *cfg,
+		unsigned long num_cfgs, unsigned long flags)
+{
+	struct exynos_cpuclk *cpuclk;
+	struct clk_init_data init;
+	struct clk *clk;
+	int ret = 0;
+
+	cpuclk = kzalloc(sizeof(*cpuclk), GFP_KERNEL);
+	if (!cpuclk)
+		return -ENOMEM;
+
+	init.name = name;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = &parent;
+	init.num_parents = 1;
+	init.ops = &exynos_cpuclk_clk_ops;
+
+	cpuclk->hw.init = &init;
+	cpuclk->ctrl_base = ctx->reg_base + offset;
+	cpuclk->lock = &ctx->lock;
+	cpuclk->flags = flags;
+	cpuclk->clk_nb.notifier_call = exynos_cpuclk_notifier_cb;
+
+	cpuclk->alt_parent = __clk_lookup(alt_parent);
+	if (!cpuclk->alt_parent) {
+		pr_err("%s: could not lookup alternate parent %s\n",
+				__func__, alt_parent);
+		ret = -EINVAL;
+		goto free_cpuclk;
+	}
+
+	clk = __clk_lookup(parent);
+	if (!clk) {
+		pr_err("%s: could not lookup parent clock %s\n",
+				__func__, parent);
+		ret = -EINVAL;
+		goto free_cpuclk;
+	}
+
+	ret = clk_notifier_register(clk, &cpuclk->clk_nb);
+	if (ret) {
+		pr_err("%s: failed to register clock notifier for %s\n",
+				__func__, name);
+		goto free_cpuclk;
+	}
+
+	cpuclk->cfg = kmemdup(cfg, sizeof(*cfg) * num_cfgs, GFP_KERNEL);
+	if (!cpuclk->cfg) {
+		pr_err("%s: could not allocate memory for cpuclk data\n",
+				__func__);
+		ret = -ENOMEM;
+		goto unregister_clk_nb;
+	}
+
+	clk = clk_register(NULL, &cpuclk->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: could not register cpuclk %s\n", __func__,	name);
+		ret = PTR_ERR(clk);
+		goto free_cpuclk_data;
+	}
+
+	samsung_clk_add_lookup(ctx, clk, lookup_id);
+	return 0;
+
+free_cpuclk_data:
+	kfree(cpuclk->cfg);
+unregister_clk_nb:
+	clk_notifier_unregister(__clk_lookup(parent), &cpuclk->clk_nb);
+free_cpuclk:
+	kfree(cpuclk);
+	return ret;
+}
diff --git a/drivers/clk/samsung/clk-cpu.h b/drivers/clk/samsung/clk-cpu.h
new file mode 100644
index 000000000000..37874d3c3165
--- /dev/null
+++ b/drivers/clk/samsung/clk-cpu.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Common Clock Framework support for all PLL's in Samsung platforms
+*/
+
+#ifndef __SAMSUNG_CLK_CPU_H
+#define __SAMSUNG_CLK_CPU_H
+
+#include "clk.h"
+
+/**
+ * struct exynos_cpuclk_data: config data to setup cpu clocks.
+ * @prate: frequency of the primary parent clock (in KHz).
+ * @div0: value to be programmed in the div_cpu0 register.
+ * @div1: value to be programmed in the div_cpu1 register.
+ *
+ * This structure holds the divider configuration data for dividers in the CPU
+ * clock domain. The parent frequency at which these divider values are valid is
+ * specified in @prate. The @prate is the frequency of the primary parent clock.
+ * For CPU clock domains that do not have a DIV1 register, the @div1 member
+ * value is not used.
+ */
+struct exynos_cpuclk_cfg_data {
+	unsigned long	prate;
+	unsigned long	div0;
+	unsigned long	div1;
+};
+
+/**
+ * struct exynos_cpuclk: information about clock supplied to a CPU core.
+ * @hw:	handle between CCF and CPU clock.
+ * @alt_parent: alternate parent clock to use when switching the speed
+ *	of the primary parent clock.
+ * @ctrl_base:	base address of the clock controller.
+ * @lock: cpu clock domain register access lock.
+ * @cfg: cpu clock rate configuration data.
+ * @num_cfgs: number of array elements in @cfg array.
+ * @clk_nb: clock notifier registered for changes in clock speed of the
+ *	primary parent clock.
+ * @flags: configuration flags for the CPU clock.
+ *
+ * This structure holds information required for programming the CPU clock for
+ * various clock speeds.
+ */
+struct exynos_cpuclk {
+	struct clk_hw				hw;
+	struct clk				*alt_parent;
+	void __iomem				*ctrl_base;
+	spinlock_t				*lock;
+	const struct exynos_cpuclk_cfg_data	*cfg;
+	const unsigned long			num_cfgs;
+	struct notifier_block			clk_nb;
+	unsigned long				flags;
+
+/* The CPU clock registers has DIV1 configuration register */
+#define CLK_CPU_HAS_DIV1		(1 << 0)
+/* When ALT parent is active, debug clocks need safe divider values */
+#define CLK_CPU_NEEDS_DEBUG_ALT_DIV	(1 << 1)
+};
+
+extern int __init exynos_register_cpu_clock(struct samsung_clk_provider *ctx,
+			unsigned int lookup_id, const char *name,
+			const char *parent, const char *alt_parent,
+			unsigned long offset,
+			const struct exynos_cpuclk_cfg_data *cfg,
+			unsigned long num_cfgs, unsigned long flags);
+
+#endif /* __SAMSUNG_CLK_CPU_H */
diff --git a/drivers/clk/samsung/clk-exynos4.c b/drivers/clk/samsung/clk-exynos4.c
index 714d6ba782c8..cae2c048488d 100644
--- a/drivers/clk/samsung/clk-exynos4.c
+++ b/drivers/clk/samsung/clk-exynos4.c
@@ -19,6 +19,7 @@
 #include <linux/syscore_ops.h>
 
 #include "clk.h"
+#include "clk-cpu.h"
 
 /* Exynos4 clock controller register offsets */
 #define SRC_LEFTBUS		0x4200
@@ -534,7 +535,8 @@ static struct samsung_fixed_factor_clock exynos4x12_fixed_factor_clks[] __initda
 /* list of mux clocks supported in all exynos4 soc's */
 static struct samsung_mux_clock exynos4_mux_clks[] __initdata = {
 	MUX_FA(CLK_MOUT_APLL, "mout_apll", mout_apll_p, SRC_CPU, 0, 1,
-			CLK_SET_RATE_PARENT, 0, "mout_apll"),
+			CLK_SET_RATE_PARENT | CLK_RECALC_NEW_RATES, 0,
+			"mout_apll"),
 	MUX(CLK_MOUT_HDMI, "mout_hdmi", mout_hdmi_p, SRC_TV, 0, 1),
 	MUX(0, "mout_mfc1", sclk_evpll_p, SRC_MFC, 4, 1),
 	MUX(0, "mout_mfc", mout_mfc_p, SRC_MFC, 8, 1),
@@ -1378,6 +1380,22 @@ static void __init exynos4x12_core_down_clock(void)
 	__raw_writel(0x0, reg_base + E4X12_PWR_CTRL2);
 }
 
+#define E4210_CPU_DIV0(apll, pclk_dbg, atb, periph, corem1, corem0)	\
+		(((apll) << 24) | ((pclk_dbg) << 20) | ((atb) << 16) |	\
+		((periph) << 12) | ((corem1) << 8) | ((corem0) <<  4))
+#define E4210_CPU_DIV1(hpm, copy)					\
+		(((hpm) << 4) | ((copy) << 0))
+
+static const struct exynos_cpuclk_cfg_data e4210_armclk_d[] __initconst = {
+	{ 1200000, E4210_CPU_DIV0(7, 1, 4, 3, 7, 3), E4210_CPU_DIV1(0, 5), },
+	{ 1000000, E4210_CPU_DIV0(7, 1, 4, 3, 7, 3), E4210_CPU_DIV1(0, 4), },
+	{  800000, E4210_CPU_DIV0(7, 1, 3, 3, 7, 3), E4210_CPU_DIV1(0, 3), },
+	{  500000, E4210_CPU_DIV0(7, 1, 3, 3, 7, 3), E4210_CPU_DIV1(0, 3), },
+	{  400000, E4210_CPU_DIV0(7, 1, 3, 3, 7, 3), E4210_CPU_DIV1(0, 3), },
+	{  200000, E4210_CPU_DIV0(0, 1, 1, 1, 3, 1), E4210_CPU_DIV1(0, 3), },
+	{  0 },
+};
+
 /* register exynos4 clocks */
 static void __init exynos4_clk_init(struct device_node *np,
 				    enum exynos4_soc soc)
@@ -1455,6 +1473,10 @@ static void __init exynos4_clk_init(struct device_node *np,
 		samsung_clk_register_fixed_factor(ctx,
 			exynos4210_fixed_factor_clks,
 			ARRAY_SIZE(exynos4210_fixed_factor_clks));
+		exynos_register_cpu_clock(ctx, CLK_ARM_CLK, "armclk",
+			mout_core_p4210[0], mout_core_p4210[1], 0x14200,
+			e4210_armclk_d, ARRAY_SIZE(e4210_armclk_d),
+			CLK_CPU_NEEDS_DEBUG_ALT_DIV | CLK_CPU_HAS_DIV1);
 	} else {
 		samsung_clk_register_mux(ctx, exynos4x12_mux_clks,
 			ARRAY_SIZE(exynos4x12_mux_clks));
diff --git a/drivers/clk/samsung/clk-exynos5260.c b/drivers/clk/samsung/clk-exynos5260.c
index e2e5193d1049..06f96eb7cf93 100644
--- a/drivers/clk/samsung/clk-exynos5260.c
+++ b/drivers/clk/samsung/clk-exynos5260.c
@@ -94,7 +94,7 @@ PNAME(mout_aud_pll_user_p) = {"fin_pll", "fout_aud_pll"};
 PNAME(mout_sclk_aud_i2s_p) = {"mout_aud_pll_user", "ioclk_i2s_cdclk"};
 PNAME(mout_sclk_aud_pcm_p) = {"mout_aud_pll_user", "ioclk_pcm_extclk"};
 
-struct samsung_mux_clock aud_mux_clks[] __initdata = {
+static struct samsung_mux_clock aud_mux_clks[] __initdata = {
 	MUX(AUD_MOUT_AUD_PLL_USER, "mout_aud_pll_user", mout_aud_pll_user_p,
 			MUX_SEL_AUD, 0, 1),
 	MUX(AUD_MOUT_SCLK_AUD_I2S, "mout_sclk_aud_i2s", mout_sclk_aud_i2s_p,
@@ -103,7 +103,7 @@ struct samsung_mux_clock aud_mux_clks[] __initdata = {
 			MUX_SEL_AUD, 8, 1),
 };
 
-struct samsung_div_clock aud_div_clks[] __initdata = {
+static struct samsung_div_clock aud_div_clks[] __initdata = {
 	DIV(AUD_DOUT_ACLK_AUD_131, "dout_aclk_aud_131", "mout_aud_pll_user",
 			DIV_AUD0, 0, 4),
 
@@ -115,7 +115,7 @@ struct samsung_div_clock aud_div_clks[] __initdata = {
 			DIV_AUD1, 12, 4),
 };
 
-struct samsung_gate_clock aud_gate_clks[] __initdata = {
+static struct samsung_gate_clock aud_gate_clks[] __initdata = {
 	GATE(AUD_SCLK_I2S, "sclk_aud_i2s", "dout_sclk_aud_i2s",
 			EN_SCLK_AUD, 0, CLK_SET_RATE_PARENT, 0),
 	GATE(AUD_SCLK_PCM, "sclk_aud_pcm", "dout_sclk_aud_pcm",
@@ -135,7 +135,7 @@ struct samsung_gate_clock aud_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_aud_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.mux_clks = aud_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(aud_mux_clks);
@@ -203,7 +203,7 @@ PNAME(mout_phyclk_mipi_dphy_4lmrxclk_esc0_user_p) = {"fin_pll",
 PNAME(mout_sclk_hdmi_spdif_p) = {"fin_pll", "ioclk_spdif_extclk",
 			"dout_aclk_peri_aud", "phyclk_hdmi_phy_ref_cko"};
 
-struct samsung_mux_clock disp_mux_clks[] __initdata = {
+static struct samsung_mux_clock disp_mux_clks[] __initdata = {
 	MUX(DISP_MOUT_ACLK_DISP_333_USER, "mout_aclk_disp_333_user",
 			mout_aclk_disp_333_user_p,
 			MUX_SEL_DISP0, 0, 1),
@@ -272,7 +272,7 @@ struct samsung_mux_clock disp_mux_clks[] __initdata = {
 			MUX_SEL_DISP4, 4, 2),
 };
 
-struct samsung_div_clock disp_div_clks[] __initdata = {
+static struct samsung_div_clock disp_div_clks[] __initdata = {
 	DIV(DISP_DOUT_PCLK_DISP_111, "dout_pclk_disp_111",
 			"mout_aclk_disp_222_user",
 			DIV_DISP, 8, 4),
@@ -285,7 +285,7 @@ struct samsung_div_clock disp_div_clks[] __initdata = {
 			DIV_DISP, 16, 4),
 };
 
-struct samsung_gate_clock disp_gate_clks[] __initdata = {
+static struct samsung_gate_clock disp_gate_clks[] __initdata = {
 	GATE(DISP_MOUT_HDMI_PHY_PIXEL_USER, "sclk_hdmi_link_i_pixel",
 			"mout_phyclk_hdmi_phy_pixel_clko_user",
 			EN_SCLK_DISP0, 26, CLK_SET_RATE_PARENT, 0),
@@ -325,7 +325,7 @@ struct samsung_gate_clock disp_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_disp_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.mux_clks = disp_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(disp_mux_clks);
@@ -363,13 +363,13 @@ static unsigned long egl_clk_regs[] __initdata = {
 PNAME(mout_egl_b_p) = {"mout_egl_pll", "dout_bus_pll"};
 PNAME(mout_egl_pll_p) = {"fin_pll", "fout_egl_pll"};
 
-struct samsung_mux_clock egl_mux_clks[] __initdata = {
+static struct samsung_mux_clock egl_mux_clks[] __initdata = {
 	MUX(EGL_MOUT_EGL_PLL, "mout_egl_pll", mout_egl_pll_p,
 			MUX_SEL_EGL, 4, 1),
 	MUX(EGL_MOUT_EGL_B, "mout_egl_b", mout_egl_b_p, MUX_SEL_EGL, 16, 1),
 };
 
-struct samsung_div_clock egl_div_clks[] __initdata = {
+static struct samsung_div_clock egl_div_clks[] __initdata = {
 	DIV(EGL_DOUT_EGL1, "dout_egl1", "mout_egl_b", DIV_EGL, 0, 3),
 	DIV(EGL_DOUT_EGL2, "dout_egl2", "dout_egl1", DIV_EGL, 4, 3),
 	DIV(EGL_DOUT_ACLK_EGL, "dout_aclk_egl", "dout_egl2", DIV_EGL, 8, 3),
@@ -389,7 +389,7 @@ static struct samsung_pll_clock egl_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_egl_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.pll_clks = egl_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(egl_pll_clks);
@@ -433,7 +433,7 @@ PNAME(mout_phyclk_usbdrd30_pipe_pclk_user_p) = {"fin_pll",
 PNAME(mout_phyclk_usbdrd30_phyclock_user_p) = {"fin_pll",
 			"phyclk_usbdrd30_udrd30_phyclock"};
 
-struct samsung_mux_clock fsys_mux_clks[] __initdata = {
+static struct samsung_mux_clock fsys_mux_clks[] __initdata = {
 	MUX(FSYS_MOUT_PHYCLK_USBDRD30_PHYCLOCK_USER,
 			"mout_phyclk_usbdrd30_phyclock_user",
 			mout_phyclk_usbdrd30_phyclock_user_p,
@@ -456,7 +456,7 @@ struct samsung_mux_clock fsys_mux_clks[] __initdata = {
 			MUX_SEL_FSYS1, 16, 1),
 };
 
-struct samsung_gate_clock fsys_gate_clks[] __initdata = {
+static struct samsung_gate_clock fsys_gate_clks[] __initdata = {
 	GATE(FSYS_PHYCLK_USBHOST20, "phyclk_usbhost20_phyclock",
 			"mout_phyclk_usbdrd30_phyclock_user",
 			EN_SCLK_FSYS, 1, 0, 0),
@@ -491,7 +491,7 @@ struct samsung_gate_clock fsys_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_fsys_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.mux_clks = fsys_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(fsys_mux_clks);
@@ -537,18 +537,18 @@ static unsigned long g2d_clk_regs[] __initdata = {
 
 PNAME(mout_aclk_g2d_333_user_p) = {"fin_pll", "dout_aclk_g2d_333"};
 
-struct samsung_mux_clock g2d_mux_clks[] __initdata = {
+static struct samsung_mux_clock g2d_mux_clks[] __initdata = {
 	MUX(G2D_MOUT_ACLK_G2D_333_USER, "mout_aclk_g2d_333_user",
 			mout_aclk_g2d_333_user_p,
 			MUX_SEL_G2D, 0, 1),
 };
 
-struct samsung_div_clock g2d_div_clks[] __initdata = {
+static struct samsung_div_clock g2d_div_clks[] __initdata = {
 	DIV(G2D_DOUT_PCLK_G2D_83, "dout_pclk_g2d_83", "mout_aclk_g2d_333_user",
 			DIV_G2D, 0, 3),
 };
 
-struct samsung_gate_clock g2d_gate_clks[] __initdata = {
+static struct samsung_gate_clock g2d_gate_clks[] __initdata = {
 	GATE(G2D_CLK_G2D, "clk_g2d", "mout_aclk_g2d_333_user",
 			EN_IP_G2D, 4, 0, 0),
 	GATE(G2D_CLK_JPEG, "clk_jpeg", "mout_aclk_g2d_333_user",
@@ -580,7 +580,7 @@ struct samsung_gate_clock g2d_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_g2d_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.mux_clks = g2d_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(g2d_mux_clks);
@@ -617,17 +617,17 @@ static unsigned long g3d_clk_regs[] __initdata = {
 
 PNAME(mout_g3d_pll_p) = {"fin_pll", "fout_g3d_pll"};
 
-struct samsung_mux_clock g3d_mux_clks[] __initdata = {
+static struct samsung_mux_clock g3d_mux_clks[] __initdata = {
 	MUX(G3D_MOUT_G3D_PLL, "mout_g3d_pll", mout_g3d_pll_p,
 			MUX_SEL_G3D, 0, 1),
 };
 
-struct samsung_div_clock g3d_div_clks[] __initdata = {
+static struct samsung_div_clock g3d_div_clks[] __initdata = {
 	DIV(G3D_DOUT_PCLK_G3D, "dout_pclk_g3d", "dout_aclk_g3d", DIV_G3D, 0, 3),
 	DIV(G3D_DOUT_ACLK_G3D, "dout_aclk_g3d", "mout_g3d_pll", DIV_G3D, 4, 3),
 };
 
-struct samsung_gate_clock g3d_gate_clks[] __initdata = {
+static struct samsung_gate_clock g3d_gate_clks[] __initdata = {
 	GATE(G3D_CLK_G3D, "clk_g3d", "dout_aclk_g3d", EN_IP_G3D, 2, 0, 0),
 	GATE(G3D_CLK_G3D_HPM, "clk_g3d_hpm", "dout_aclk_g3d",
 			EN_IP_G3D, 3, 0, 0),
@@ -641,7 +641,7 @@ static struct samsung_pll_clock g3d_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_g3d_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.pll_clks = g3d_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(g3d_pll_clks);
@@ -694,7 +694,7 @@ PNAME(mout_aclk_m2m_400_user_p) = {"fin_pll", "dout_aclk_gscl_400"};
 PNAME(mout_aclk_gscl_fimc_user_p) = {"fin_pll", "dout_aclk_gscl_400"};
 PNAME(mout_aclk_csis_p) = {"dout_aclk_csis_200", "mout_aclk_gscl_fimc_user"};
 
-struct samsung_mux_clock gscl_mux_clks[] __initdata = {
+static struct samsung_mux_clock gscl_mux_clks[] __initdata = {
 	MUX(GSCL_MOUT_ACLK_GSCL_333_USER, "mout_aclk_gscl_333_user",
 			mout_aclk_gscl_333_user_p,
 			MUX_SEL_GSCL, 0, 1),
@@ -708,7 +708,7 @@ struct samsung_mux_clock gscl_mux_clks[] __initdata = {
 			MUX_SEL_GSCL, 24, 1),
 };
 
-struct samsung_div_clock gscl_div_clks[] __initdata = {
+static struct samsung_div_clock gscl_div_clks[] __initdata = {
 	DIV(GSCL_DOUT_PCLK_M2M_100, "dout_pclk_m2m_100",
 			"mout_aclk_m2m_400_user",
 			DIV_GSCL, 0, 3),
@@ -717,7 +717,7 @@ struct samsung_div_clock gscl_div_clks[] __initdata = {
 			DIV_GSCL, 4, 3),
 };
 
-struct samsung_gate_clock gscl_gate_clks[] __initdata = {
+static struct samsung_gate_clock gscl_gate_clks[] __initdata = {
 	GATE(GSCL_SCLK_CSIS0_WRAP, "sclk_csis0_wrap", "dout_aclk_csis_200",
 			EN_SCLK_GSCL_FIMC, 0, CLK_SET_RATE_PARENT, 0),
 	GATE(GSCL_SCLK_CSIS1_WRAP, "sclk_csis1_wrap", "dout_aclk_csis_200",
@@ -776,7 +776,7 @@ struct samsung_gate_clock gscl_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_gscl_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.mux_clks = gscl_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(gscl_mux_clks);
@@ -813,14 +813,14 @@ static unsigned long isp_clk_regs[] __initdata = {
 PNAME(mout_isp_400_user_p) = {"fin_pll", "dout_aclk_isp1_400"};
 PNAME(mout_isp_266_user_p)	 = {"fin_pll", "dout_aclk_isp1_266"};
 
-struct samsung_mux_clock isp_mux_clks[] __initdata = {
+static struct samsung_mux_clock isp_mux_clks[] __initdata = {
 	MUX(ISP_MOUT_ISP_266_USER, "mout_isp_266_user", mout_isp_266_user_p,
 			MUX_SEL_ISP0, 0, 1),
 	MUX(ISP_MOUT_ISP_400_USER, "mout_isp_400_user", mout_isp_400_user_p,
 			MUX_SEL_ISP0, 4, 1),
 };
 
-struct samsung_div_clock isp_div_clks[] __initdata = {
+static struct samsung_div_clock isp_div_clks[] __initdata = {
 	DIV(ISP_DOUT_PCLK_ISP_66, "dout_pclk_isp_66", "mout_kfc",
 			DIV_ISP, 0, 3),
 	DIV(ISP_DOUT_PCLK_ISP_133, "dout_pclk_isp_133", "mout_kfc",
@@ -832,7 +832,7 @@ struct samsung_div_clock isp_div_clks[] __initdata = {
 	DIV(ISP_DOUT_SCLK_MPWM, "dout_sclk_mpwm", "mout_kfc", DIV_ISP, 20, 2),
 };
 
-struct samsung_gate_clock isp_gate_clks[] __initdata = {
+static struct samsung_gate_clock isp_gate_clks[] __initdata = {
 	GATE(ISP_CLK_GIC, "clk_isp_gic", "mout_aclk_isp1_266",
 			EN_IP_ISP0, 15, 0, 0),
 
@@ -895,7 +895,7 @@ struct samsung_gate_clock isp_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_isp_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.mux_clks = isp_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(isp_mux_clks);
@@ -934,13 +934,13 @@ static unsigned long kfc_clk_regs[] __initdata = {
 PNAME(mout_kfc_pll_p) = {"fin_pll", "fout_kfc_pll"};
 PNAME(mout_kfc_p)	 = {"mout_kfc_pll", "dout_media_pll"};
 
-struct samsung_mux_clock kfc_mux_clks[] __initdata = {
+static struct samsung_mux_clock kfc_mux_clks[] __initdata = {
 	MUX(KFC_MOUT_KFC_PLL, "mout_kfc_pll", mout_kfc_pll_p,
 			MUX_SEL_KFC0, 0, 1),
 	MUX(KFC_MOUT_KFC, "mout_kfc", mout_kfc_p, MUX_SEL_KFC2, 0, 1),
 };
 
-struct samsung_div_clock kfc_div_clks[] __initdata = {
+static struct samsung_div_clock kfc_div_clks[] __initdata = {
 	DIV(KFC_DOUT_KFC1, "dout_kfc1", "mout_kfc", DIV_KFC, 0, 3),
 	DIV(KFC_DOUT_KFC2, "dout_kfc2", "dout_kfc1", DIV_KFC, 4, 3),
 	DIV(KFC_DOUT_KFC_ATCLK, "dout_kfc_atclk", "dout_kfc2", DIV_KFC, 8, 3),
@@ -959,7 +959,7 @@ static struct samsung_pll_clock kfc_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_kfc_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.pll_clks = kfc_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(kfc_pll_clks);
@@ -993,18 +993,18 @@ static unsigned long mfc_clk_regs[] __initdata = {
 
 PNAME(mout_aclk_mfc_333_user_p) = {"fin_pll", "dout_aclk_mfc_333"};
 
-struct samsung_mux_clock mfc_mux_clks[] __initdata = {
+static struct samsung_mux_clock mfc_mux_clks[] __initdata = {
 	MUX(MFC_MOUT_ACLK_MFC_333_USER, "mout_aclk_mfc_333_user",
 			mout_aclk_mfc_333_user_p,
 			MUX_SEL_MFC, 0, 1),
 };
 
-struct samsung_div_clock mfc_div_clks[] __initdata = {
+static struct samsung_div_clock mfc_div_clks[] __initdata = {
 	DIV(MFC_DOUT_PCLK_MFC_83, "dout_pclk_mfc_83", "mout_aclk_mfc_333_user",
 			DIV_MFC, 0, 3),
 };
 
-struct samsung_gate_clock mfc_gate_clks[] __initdata = {
+static struct samsung_gate_clock mfc_gate_clks[] __initdata = {
 	GATE(MFC_CLK_MFC, "clk_mfc", "mout_aclk_mfc_333_user",
 			EN_IP_MFC, 1, 0, 0),
 	GATE(MFC_CLK_SMMU2_MFCM0, "clk_smmu2_mfcm0", "mout_aclk_mfc_333_user",
@@ -1015,7 +1015,7 @@ struct samsung_gate_clock mfc_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_mfc_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.mux_clks = mfc_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(mfc_mux_clks);
@@ -1078,7 +1078,7 @@ PNAME(mout_mif_drex2x_p) = {"dout_mem_pll", "dout_bus_pll"};
 PNAME(mout_clkm_phy_p) = {"mout_mif_drex", "dout_media_pll"};
 PNAME(mout_clk2x_phy_p) = {"mout_mif_drex2x", "dout_media_pll"};
 
-struct samsung_mux_clock mif_mux_clks[] __initdata = {
+static struct samsung_mux_clock mif_mux_clks[] __initdata = {
 	MUX(MIF_MOUT_MEM_PLL, "mout_mem_pll", mout_mem_pll_p,
 			MUX_SEL_MIF, 0, 1),
 	MUX(MIF_MOUT_BUS_PLL, "mout_bus_pll", mout_bus_pll_p,
@@ -1095,7 +1095,7 @@ struct samsung_mux_clock mif_mux_clks[] __initdata = {
 			MUX_SEL_MIF, 24, 1),
 };
 
-struct samsung_div_clock mif_div_clks[] __initdata = {
+static struct samsung_div_clock mif_div_clks[] __initdata = {
 	DIV(MIF_DOUT_MEDIA_PLL, "dout_media_pll", "mout_media_pll",
 			DIV_MIF, 0, 3),
 	DIV(MIF_DOUT_MEM_PLL, "dout_mem_pll", "mout_mem_pll",
@@ -1114,7 +1114,7 @@ struct samsung_div_clock mif_div_clks[] __initdata = {
 			DIV_MIF, 28, 4),
 };
 
-struct samsung_gate_clock mif_gate_clks[] __initdata = {
+static struct samsung_gate_clock mif_gate_clks[] __initdata = {
 	GATE(MIF_CLK_LPDDR3PHY_WRAP0, "clk_lpddr3phy_wrap0", "dout_clk2x_phy",
 			EN_IP_MIF, 12, CLK_IGNORE_UNUSED, 0),
 	GATE(MIF_CLK_LPDDR3PHY_WRAP1, "clk_lpddr3phy_wrap1", "dout_clk2x_phy",
@@ -1162,7 +1162,7 @@ static struct samsung_pll_clock mif_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_mif_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.pll_clks = mif_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(mif_pll_clks);
@@ -1221,7 +1221,7 @@ PNAME(mout_sclk_i2scod_p) = {"ioclk_i2s_cdclk", "fin_pll", "dout_aclk_peri_aud",
 PNAME(mout_sclk_spdif_p) = {"ioclk_spdif_extclk", "fin_pll",
 			"dout_aclk_peri_aud", "phyclk_hdmi_phy_ref_cko"};
 
-struct samsung_mux_clock peri_mux_clks[] __initdata = {
+static struct samsung_mux_clock peri_mux_clks[] __initdata = {
 	MUX(PERI_MOUT_SCLK_PCM, "mout_sclk_pcm", mout_sclk_pcm_p,
 			MUX_SEL_PERI1, 4, 2),
 	MUX(PERI_MOUT_SCLK_I2SCOD, "mout_sclk_i2scod", mout_sclk_i2scod_p,
@@ -1230,12 +1230,12 @@ struct samsung_mux_clock peri_mux_clks[] __initdata = {
 			MUX_SEL_PERI1, 20, 2),
 };
 
-struct samsung_div_clock peri_div_clks[] __initdata = {
+static struct samsung_div_clock peri_div_clks[] __initdata = {
 	DIV(PERI_DOUT_PCM, "dout_pcm", "mout_sclk_pcm", DIV_PERI, 0, 8),
 	DIV(PERI_DOUT_I2S, "dout_i2s", "mout_sclk_i2scod", DIV_PERI, 8, 6),
 };
 
-struct samsung_gate_clock peri_gate_clks[] __initdata = {
+static struct samsung_gate_clock peri_gate_clks[] __initdata = {
 	GATE(PERI_SCLK_PCM1, "sclk_pcm1", "dout_pcm", EN_SCLK_PERI, 0,
 			CLK_SET_RATE_PARENT, 0),
 	GATE(PERI_SCLK_I2S, "sclk_i2s", "dout_i2s", EN_SCLK_PERI, 1,
@@ -1370,7 +1370,7 @@ struct samsung_gate_clock peri_gate_clks[] __initdata = {
 
 static void __init exynos5260_clk_peri_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.mux_clks = peri_mux_clks;
 	cmu.nr_mux_clks = ARRAY_SIZE(peri_mux_clks);
@@ -1432,7 +1432,7 @@ static unsigned long top_clk_regs[] __initdata = {
 };
 
 /* fixed rate clocks generated inside the soc */
-struct samsung_fixed_rate_clock fixed_rate_clks[] __initdata = {
+static struct samsung_fixed_rate_clock fixed_rate_clks[] __initdata = {
 	FRATE(PHYCLK_DPTX_PHY_CH3_TXD_CLK, "phyclk_dptx_phy_ch3_txd_clk", NULL,
 			CLK_IS_ROOT, 270000000),
 	FRATE(PHYCLK_DPTX_PHY_CH2_TXD_CLK, "phyclk_dptx_phy_ch2_txd_clk", NULL,
@@ -1519,7 +1519,7 @@ PNAME(mout_sclk_fsys_mmc1_sdclkin_b_p) = {"mout_sclk_fsys_mmc1_sdclkin_a",
 PNAME(mout_sclk_fsys_mmc2_sdclkin_b_p) = {"mout_sclk_fsys_mmc2_sdclkin_a",
 			"mout_mediatop_pll_user"};
 
-struct samsung_mux_clock top_mux_clks[] __initdata = {
+static struct samsung_mux_clock top_mux_clks[] __initdata = {
 	MUX(TOP_MOUT_MEDIATOP_PLL_USER, "mout_mediatop_pll_user",
 			mout_mediatop_pll_user_p,
 			MUX_SEL_TOP_PLL0, 0, 1),
@@ -1679,7 +1679,7 @@ struct samsung_mux_clock top_mux_clks[] __initdata = {
 			MUX_SEL_TOP_GSCL, 20, 1),
 };
 
-struct samsung_div_clock top_div_clks[] __initdata = {
+static struct samsung_div_clock top_div_clks[] __initdata = {
 	DIV(TOP_DOUT_ACLK_G2D_333, "dout_aclk_g2d_333", "mout_aclk_g2d_333",
 			DIV_TOP_G2D_MFC, 0, 3),
 	DIV(TOP_DOUT_ACLK_MFC_333, "dout_aclk_mfc_333", "mout_aclk_mfc_333",
@@ -1800,7 +1800,7 @@ struct samsung_div_clock top_div_clks[] __initdata = {
 
 };
 
-struct samsung_gate_clock top_gate_clks[] __initdata = {
+static struct samsung_gate_clock top_gate_clks[] __initdata = {
 	GATE(TOP_SCLK_MMC0, "sclk_fsys_mmc0_sdclkin",
 			"dout_sclk_fsys_mmc0_sdclkin_b",
 			EN_SCLK_TOP, 7, CLK_SET_RATE_PARENT, 0),
@@ -1826,7 +1826,7 @@ static struct samsung_pll_clock top_pll_clks[] __initdata = {
 
 static void __init exynos5260_clk_top_init(struct device_node *np)
 {
-	struct samsung_cmu_info cmu = {0};
+	struct samsung_cmu_info cmu = { NULL };
 
 	cmu.pll_clks = top_pll_clks;
 	cmu.nr_pll_clks =  ARRAY_SIZE(top_pll_clks);
diff --git a/drivers/clk/samsung/clk-exynos5420.c b/drivers/clk/samsung/clk-exynos5420.c
index bea4a173eef5..a1d731ca8f48 100644
--- a/drivers/clk/samsung/clk-exynos5420.c
+++ b/drivers/clk/samsung/clk-exynos5420.c
@@ -504,7 +504,7 @@ static struct samsung_fixed_factor_clock
 	FFACTOR(0, "ff_dout_spll2", "mout_sclk_spll", 1, 2, 0),
 };
 
-struct samsung_mux_clock exynos5800_mux_clks[] __initdata = {
+static struct samsung_mux_clock exynos5800_mux_clks[] __initdata = {
 	MUX(0, "mout_aclk400_isp", mout_group3_5800_p, SRC_TOP0, 0, 3),
 	MUX(0, "mout_aclk400_mscl", mout_group3_5800_p, SRC_TOP0, 4, 3),
 	MUX(0, "mout_aclk400_wcore", mout_group2_5800_p, SRC_TOP0, 16, 3),
@@ -553,7 +553,7 @@ struct samsung_mux_clock exynos5800_mux_clks[] __initdata = {
 	MUX(0, "mout_fimd1", mout_group2_p, SRC_DISP10, 4, 3),
 };
 
-struct samsung_div_clock exynos5800_div_clks[] __initdata = {
+static struct samsung_div_clock exynos5800_div_clks[] __initdata = {
 	DIV(0, "dout_aclk400_wcore", "mout_aclk400_wcore", DIV_TOP0, 16, 3),
 
 	DIV(0, "dout_aclk550_cam", "mout_aclk550_cam",
@@ -569,14 +569,14 @@ struct samsung_div_clock exynos5800_div_clks[] __initdata = {
 	DIV(0, "dout_sclk_sw", "sclk_spll", DIV_TOP9, 24, 6),
 };
 
-struct samsung_gate_clock exynos5800_gate_clks[] __initdata = {
+static struct samsung_gate_clock exynos5800_gate_clks[] __initdata = {
 	GATE(CLK_ACLK550_CAM, "aclk550_cam", "mout_user_aclk550_cam",
 				GATE_BUS_TOP, 24, 0, 0),
 	GATE(CLK_ACLK432_SCALER, "aclk432_scaler", "mout_user_aclk432_scaler",
 				GATE_BUS_TOP, 27, 0, 0),
 };
 
-struct samsung_mux_clock exynos5420_mux_clks[] __initdata = {
+static struct samsung_mux_clock exynos5420_mux_clks[] __initdata = {
 	MUX(0, "sclk_bpll", mout_bpll_p, TOP_SPARE2, 0, 1),
 	MUX(0, "mout_aclk400_wcore_bpll", mout_aclk400_wcore_bpll_p,
 				TOP_SPARE2, 4, 1),
@@ -606,7 +606,7 @@ struct samsung_mux_clock exynos5420_mux_clks[] __initdata = {
 	MUX(0, "mout_fimd1", mout_group3_p, SRC_DISP10, 4, 1),
 };
 
-struct samsung_div_clock exynos5420_div_clks[] __initdata = {
+static struct samsung_div_clock exynos5420_div_clks[] __initdata = {
 	DIV(0, "dout_aclk400_wcore", "mout_aclk400_wcore_bpll",
 			DIV_TOP0, 16, 3),
 };
diff --git a/drivers/clk/samsung/clk-exynos5433.c b/drivers/clk/samsung/clk-exynos5433.c
index 9e04ae2bb4d7..39c95649d3d0 100644
--- a/drivers/clk/samsung/clk-exynos5433.c
+++ b/drivers/clk/samsung/clk-exynos5433.c
@@ -835,6 +835,7 @@ static unsigned long cpif_clk_regs[] __initdata = {
 	MPHY_PLL_CON1,
 	MPHY_PLL_FREQ_DET,
 	MUX_SEL_CPIF0,
+	DIV_CPIF,
 	ENABLE_SCLK_CPIF,
 };
 
@@ -1389,7 +1390,7 @@ static struct samsung_gate_clock mif_gate_clks[] __initdata = {
 
 	/* ENABLE_ACLK_MIF2 */
 	GATE(CLK_ACLK_MIFND_266, "aclk_mifnd_266", "div_aclk_mif_266",
-			ENABLE_ACLK_MIF2, 20, 0, 0),
+			ENABLE_ACLK_MIF2, 20, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_ACLK_PPMU_DREX1S3, "aclk_ppmu_drex1s3", "div_aclk_drex1",
 			ENABLE_ACLK_MIF2, 17, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_ACLK_PPMU_DREX1S1, "aclk_ppmu_drex1s1", "div_aclk_drex1",
@@ -1832,39 +1833,39 @@ static struct samsung_gate_clock peris_gate_clks[] __initdata = {
 
 	/* ENABLE_PCLK_PERIS_SECURE_TZPC */
 	GATE(CLK_PCLK_TZPC12, "pclk_tzpc12", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 12, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 12, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC11, "pclk_tzpc11", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 11, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 11, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC10, "pclk_tzpc10", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 10, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 10, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC9, "pclk_tzpc9", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 9, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 9, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC8, "pclk_tzpc8", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 8, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 8, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC7, "pclk_tzpc7", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 7, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 7, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC6, "pclk_tzpc6", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 6, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 6, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC5, "pclk_tzpc5", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 5, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 5, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC4, "pclk_tzpc4", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 4, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 4, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC3, "pclk_tzpc3", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 3, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 3, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC2, "pclk_tzpc2", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 2, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 2, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC1, "pclk_tzpc1", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 1, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 1, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_PCLK_TZPC0, "pclk_tzpc0", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_TZPC, 0, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_TZPC, 0, CLK_IGNORE_UNUSED, 0),
 
 	/* ENABLE_PCLK_PERIS_SECURE_SECKEY_APBIF */
 	GATE(CLK_PCLK_SECKEY_APBIF, "pclk_seckey_apbif", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_SECKEY_APBIF, 0, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_SECKEY_APBIF, 0, CLK_IGNORE_UNUSED, 0),
 
 	/* ENABLE_PCLK_PERIS_SECURE_CHIPID_APBIF */
 	GATE(CLK_PCLK_CHIPID_APBIF, "pclk_chipid_apbif", "aclk_peris_66",
-			ENABLE_PCLK_PERIS_SECURE_CHIPID_APBIF, 0, 0, 0),
+			ENABLE_PCLK_PERIS_SECURE_CHIPID_APBIF, 0, CLK_IGNORE_UNUSED, 0),
 
 	/* ENABLE_PCLK_PERIS_SECURE_TOPRTC */
 	GATE(CLK_PCLK_TOPRTC, "pclk_toprtc", "aclk_peris_66",
@@ -1895,11 +1896,11 @@ static struct samsung_gate_clock peris_gate_clks[] __initdata = {
 
 	/* ENABLE_SCLK_PERIS_SECURE_SECKEY */
 	GATE(CLK_SCLK_SECKEY, "sclk_seckey", "oscclk_efuse_common",
-			ENABLE_SCLK_PERIS_SECURE_SECKEY, 0, 0, 0),
+			ENABLE_SCLK_PERIS_SECURE_SECKEY, 0, CLK_IGNORE_UNUSED, 0),
 
 	/* ENABLE_SCLK_PERIS_SECURE_CHIPID */
 	GATE(CLK_SCLK_CHIPID, "sclk_chipid", "oscclk_efuse_common",
-			ENABLE_SCLK_PERIS_SECURE_CHIPID, 0, 0, 0),
+			ENABLE_SCLK_PERIS_SECURE_CHIPID, 0, CLK_IGNORE_UNUSED, 0),
 
 	/* ENABLE_SCLK_PERIS_SECURE_TOPRTC */
 	GATE(CLK_SCLK_TOPRTC, "sclk_toprtc", "oscclk_efuse_common",
@@ -3286,10 +3287,10 @@ static struct samsung_pll_clock g3d_pll_clks[] __initdata = {
 
 static struct samsung_mux_clock g3d_mux_clks[] __initdata = {
 	/* MUX_SEL_G3D */
-	MUX(CLK_MOUT_ACLK_G3D_400, "mout_aclk_g3d_400", mout_aclk_g3d_400_p,
-			MUX_SEL_G3D, 8, 1),
-	MUX(CLK_MOUT_G3D_PLL, "mout_g3d_pll", mout_g3d_pll_p,
-			MUX_SEL_G3D, 0, 1),
+	MUX_F(CLK_MOUT_ACLK_G3D_400, "mout_aclk_g3d_400", mout_aclk_g3d_400_p,
+			MUX_SEL_G3D, 8, 1, CLK_SET_RATE_PARENT, 0),
+	MUX_F(CLK_MOUT_G3D_PLL, "mout_g3d_pll", mout_g3d_pll_p,
+			MUX_SEL_G3D, 0, 1, CLK_SET_RATE_PARENT, 0),
 };
 
 static struct samsung_div_clock g3d_div_clks[] __initdata = {
@@ -3298,8 +3299,8 @@ static struct samsung_div_clock g3d_div_clks[] __initdata = {
 			8, 2),
 	DIV(CLK_DIV_PCLK_G3D, "div_pclk_g3d", "div_aclk_g3d", DIV_G3D,
 			4, 3),
-	DIV(CLK_DIV_ACLK_G3D, "div_aclk_g3d", "mout_aclk_g3d_400", DIV_G3D,
-			0, 3),
+	DIV_F(CLK_DIV_ACLK_G3D, "div_aclk_g3d", "mout_aclk_g3d_400", DIV_G3D,
+			0, 3, CLK_SET_RATE_PARENT, 0),
 };
 
 static struct samsung_gate_clock g3d_gate_clks[] __initdata = {
@@ -3309,9 +3310,9 @@ static struct samsung_gate_clock g3d_gate_clks[] __initdata = {
 	GATE(CLK_ACLK_BTS_G3D0, "aclk_bts_g3d0", "div_aclk_g3d",
 			ENABLE_ACLK_G3D, 6, 0, 0),
 	GATE(CLK_ACLK_ASYNCAPBS_G3D, "aclk_asyncapbs_g3d", "div_pclk_g3d",
-			ENABLE_ACLK_G3D, 5, 0, 0),
+			ENABLE_ACLK_G3D, 5, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_ACLK_ASYNCAPBM_G3D, "aclk_asyncapbm_g3d", "div_aclk_g3d",
-			ENABLE_ACLK_G3D, 4, 0, 0),
+			ENABLE_ACLK_G3D, 4, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_ACLK_AHB2APB_G3DP, "aclk_ahb2apb_g3dp", "div_pclk_g3d",
 			ENABLE_ACLK_G3D, 3, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_ACLK_G3DNP_150, "aclk_g3dnp_150", "div_pclk_g3d",
@@ -3319,7 +3320,7 @@ static struct samsung_gate_clock g3d_gate_clks[] __initdata = {
 	GATE(CLK_ACLK_G3DND_600, "aclk_g3dnd_600", "div_aclk_g3d",
 			ENABLE_ACLK_G3D, 1, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_ACLK_G3D, "aclk_g3d", "div_aclk_g3d",
-			ENABLE_ACLK_G3D, 0, 0, 0),
+			ENABLE_ACLK_G3D, 0, CLK_SET_RATE_PARENT, 0),
 
 	/* ENABLE_PCLK_G3D */
 	GATE(CLK_PCLK_BTS_G3D1, "pclk_bts_g3d1", "div_pclk_g3d",
@@ -3582,7 +3583,7 @@ static struct samsung_pll_clock apollo_pll_clks[] __initdata = {
 static struct samsung_mux_clock apollo_mux_clks[] __initdata = {
 	/* MUX_SEL_APOLLO0 */
 	MUX_F(CLK_MOUT_APOLLO_PLL, "mout_apollo_pll", mout_apollo_pll_p,
-			MUX_SEL_APOLLO0, 0, 1, 0, CLK_MUX_READ_ONLY),
+			MUX_SEL_APOLLO0, 0, 1, CLK_SET_RATE_PARENT, 0),
 
 	/* MUX_SEL_APOLLO1 */
 	MUX(CLK_MOUT_BUS_PLL_APOLLO_USER, "mout_bus_pll_apollo_user",
@@ -3590,7 +3591,7 @@ static struct samsung_mux_clock apollo_mux_clks[] __initdata = {
 
 	/* MUX_SEL_APOLLO2 */
 	MUX_F(CLK_MOUT_APOLLO, "mout_apollo", mout_apollo_p, MUX_SEL_APOLLO2,
-			0, 1, 0, CLK_MUX_READ_ONLY),
+			0, 1, CLK_SET_RATE_PARENT, 0),
 };
 
 static struct samsung_div_clock apollo_div_clks[] __initdata = {
@@ -3611,11 +3612,9 @@ static struct samsung_div_clock apollo_div_clks[] __initdata = {
 			DIV_APOLLO0, 8, 3, CLK_GET_RATE_NOCACHE,
 			CLK_DIVIDER_READ_ONLY),
 	DIV_F(CLK_DIV_APOLLO2, "div_apollo2", "div_apollo1",
-			DIV_APOLLO0, 4, 3, CLK_GET_RATE_NOCACHE,
-			CLK_DIVIDER_READ_ONLY),
+			DIV_APOLLO0, 4, 3, CLK_SET_RATE_PARENT, 0),
 	DIV_F(CLK_DIV_APOLLO1, "div_apollo1", "mout_apollo",
-			DIV_APOLLO0, 0, 3, CLK_GET_RATE_NOCACHE,
-			CLK_DIVIDER_READ_ONLY),
+			DIV_APOLLO0, 0, 3, CLK_SET_RATE_PARENT, 0),
 
 	/* DIV_APOLLO1 */
 	DIV_F(CLK_DIV_SCLK_HPM_APOLLO, "div_sclk_hpm_apollo", "mout_apollo",
@@ -3666,7 +3665,8 @@ static struct samsung_gate_clock apollo_gate_clks[] __initdata = {
 	GATE(CLK_SCLK_HPM_APOLLO, "sclk_hpm_apollo", "div_sclk_hpm_apollo",
 			ENABLE_SCLK_APOLLO, 1, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_SCLK_APOLLO, "sclk_apollo", "div_apollo2",
-			ENABLE_SCLK_APOLLO, 0, CLK_IGNORE_UNUSED, 0),
+			ENABLE_SCLK_APOLLO, 0,
+			CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0),
 };
 
 static struct samsung_cmu_info apollo_cmu_info __initdata = {
@@ -3775,7 +3775,7 @@ static struct samsung_pll_clock atlas_pll_clks[] __initdata = {
 static struct samsung_mux_clock atlas_mux_clks[] __initdata = {
 	/* MUX_SEL_ATLAS0 */
 	MUX_F(CLK_MOUT_ATLAS_PLL, "mout_atlas_pll", mout_atlas_pll_p,
-			MUX_SEL_ATLAS0, 0, 1, 0, CLK_MUX_READ_ONLY),
+			MUX_SEL_ATLAS0, 0, 1, CLK_SET_RATE_PARENT, 0),
 
 	/* MUX_SEL_ATLAS1 */
 	MUX(CLK_MOUT_BUS_PLL_ATLAS_USER, "mout_bus_pll_atlas_user",
@@ -3783,7 +3783,7 @@ static struct samsung_mux_clock atlas_mux_clks[] __initdata = {
 
 	/* MUX_SEL_ATLAS2 */
 	MUX_F(CLK_MOUT_ATLAS, "mout_atlas", mout_atlas_p, MUX_SEL_ATLAS2,
-			0, 1, 0, CLK_MUX_READ_ONLY),
+			0, 1, CLK_SET_RATE_PARENT, 0),
 };
 
 static struct samsung_div_clock atlas_div_clks[] __initdata = {
@@ -3804,11 +3804,9 @@ static struct samsung_div_clock atlas_div_clks[] __initdata = {
 			DIV_ATLAS0, 8, 3, CLK_GET_RATE_NOCACHE,
 			CLK_DIVIDER_READ_ONLY),
 	DIV_F(CLK_DIV_ATLAS2, "div_atlas2", "div_atlas1",
-			DIV_ATLAS0, 4, 3, CLK_GET_RATE_NOCACHE,
-			CLK_DIVIDER_READ_ONLY),
+			DIV_ATLAS0, 4, 3, CLK_SET_RATE_PARENT, 0),
 	DIV_F(CLK_DIV_ATLAS1, "div_atlas1", "mout_atlas",
-			DIV_ATLAS0, 0, 3, CLK_GET_RATE_NOCACHE,
-			CLK_DIVIDER_READ_ONLY),
+			DIV_ATLAS0, 0, 3, CLK_SET_RATE_PARENT, 0),
 
 	/* DIV_ATLAS1 */
 	DIV_F(CLK_DIV_SCLK_HPM_ATLAS, "div_sclk_hpm_atlas", "mout_atlas",
@@ -3885,7 +3883,8 @@ static struct samsung_gate_clock atlas_gate_clks[] __initdata = {
 	GATE(CLK_ATCLK, "atclk", "div_atclk_atlas",
 			ENABLE_SCLK_ATLAS, 1, CLK_IGNORE_UNUSED, 0),
 	GATE(CLK_SCLK_ATLAS, "sclk_atlas", "div_atlas2",
-			ENABLE_SCLK_ATLAS, 0, CLK_IGNORE_UNUSED, 0),
+			ENABLE_SCLK_ATLAS, 0,
+			CLK_IGNORE_UNUSED | CLK_SET_RATE_PARENT, 0),
 };
 
 static struct samsung_cmu_info atlas_cmu_info __initdata = {
diff --git a/drivers/clk/samsung/clk-pll.c b/drivers/clk/samsung/clk-pll.c
index 9d70e5c03804..bebc61b5fce1 100644
--- a/drivers/clk/samsung/clk-pll.c
+++ b/drivers/clk/samsung/clk-pll.c
@@ -1156,7 +1156,7 @@ static const struct clk_ops samsung_pll2650xx_clk_min_ops = {
 };
 
 static void __init _samsung_clk_register_pll(struct samsung_clk_provider *ctx,
-				struct samsung_pll_clock *pll_clk,
+				const struct samsung_pll_clock *pll_clk,
 				void __iomem *base)
 {
 	struct samsung_clk_pll *pll;
@@ -1303,7 +1303,7 @@ static void __init _samsung_clk_register_pll(struct samsung_clk_provider *ctx,
 }
 
 void __init samsung_clk_register_pll(struct samsung_clk_provider *ctx,
-			struct samsung_pll_clock *pll_list,
+			const struct samsung_pll_clock *pll_list,
 			unsigned int nr_pll, void __iomem *base)
 {
 	int cnt;
diff --git a/drivers/clk/samsung/clk-s3c2410-dclk.c b/drivers/clk/samsung/clk-s3c2410-dclk.c
index f4f29ed6bd25..e56df5064889 100644
--- a/drivers/clk/samsung/clk-s3c2410-dclk.c
+++ b/drivers/clk/samsung/clk-s3c2410-dclk.c
@@ -81,13 +81,13 @@ static int s3c24xx_clkout_set_parent(struct clk_hw *hw, u8 index)
 	return ret;
 }
 
-const struct clk_ops s3c24xx_clkout_ops = {
+static const struct clk_ops s3c24xx_clkout_ops = {
 	.get_parent = s3c24xx_clkout_get_parent,
 	.set_parent = s3c24xx_clkout_set_parent,
 	.determine_rate = __clk_mux_determine_rate,
 };
 
-struct clk *s3c24xx_register_clkout(struct device *dev, const char *name,
+static struct clk *s3c24xx_register_clkout(struct device *dev, const char *name,
 		const char **parent_names, u8 num_parents,
 		u8 shift, u32 mask)
 {
@@ -404,7 +404,7 @@ static struct s3c24xx_dclk_drv_data dclk_variants[] = {
 	},
 };
 
-static struct platform_device_id s3c24xx_dclk_driver_ids[] = {
+static const struct platform_device_id s3c24xx_dclk_driver_ids[] = {
 	{
 		.name		= "s3c2410-dclk",
 		.driver_data	= (kernel_ulong_t)&dclk_variants[S3C2410],
diff --git a/drivers/clk/samsung/clk-s5pv210.c b/drivers/clk/samsung/clk-s5pv210.c
index e668e479a697..cf7e8fa7b624 100644
--- a/drivers/clk/samsung/clk-s5pv210.c
+++ b/drivers/clk/samsung/clk-s5pv210.c
@@ -169,44 +169,44 @@ static inline void s5pv210_clk_sleep_init(void) { }
 #endif
 
 /* Mux parent lists. */
-static const char *fin_pll_p[] __initdata = {
+static const char *const fin_pll_p[] __initconst = {
 	"xxti",
 	"xusbxti"
 };
 
-static const char *mout_apll_p[] __initdata = {
+static const char *const mout_apll_p[] __initconst = {
 	"fin_pll",
 	"fout_apll"
 };
 
-static const char *mout_mpll_p[] __initdata = {
+static const char *const mout_mpll_p[] __initconst = {
 	"fin_pll",
 	"fout_mpll"
 };
 
-static const char *mout_epll_p[] __initdata = {
+static const char *const mout_epll_p[] __initconst = {
 	"fin_pll",
 	"fout_epll"
 };
 
-static const char *mout_vpllsrc_p[] __initdata = {
+static const char *const mout_vpllsrc_p[] __initconst = {
 	"fin_pll",
 	"sclk_hdmi27m"
 };
 
-static const char *mout_vpll_p[] __initdata = {
+static const char *const mout_vpll_p[] __initconst = {
 	"mout_vpllsrc",
 	"fout_vpll"
 };
 
-static const char *mout_group1_p[] __initdata = {
+static const char *const mout_group1_p[] __initconst = {
 	"dout_a2m",
 	"mout_mpll",
 	"mout_epll",
 	"mout_vpll"
 };
 
-static const char *mout_group2_p[] __initdata = {
+static const char *const mout_group2_p[] __initconst = {
 	"xxti",
 	"xusbxti",
 	"sclk_hdmi27m",
@@ -218,7 +218,7 @@ static const char *mout_group2_p[] __initdata = {
 	"mout_vpll",
 };
 
-static const char *mout_audio0_p[] __initdata = {
+static const char *const mout_audio0_p[] __initconst = {
 	"xxti",
 	"pcmcdclk0",
 	"sclk_hdmi27m",
@@ -230,7 +230,7 @@ static const char *mout_audio0_p[] __initdata = {
 	"mout_vpll",
 };
 
-static const char *mout_audio1_p[] __initdata = {
+static const char *const mout_audio1_p[] __initconst = {
 	"i2scdclk1",
 	"pcmcdclk1",
 	"sclk_hdmi27m",
@@ -242,7 +242,7 @@ static const char *mout_audio1_p[] __initdata = {
 	"mout_vpll",
 };
 
-static const char *mout_audio2_p[] __initdata = {
+static const char *const mout_audio2_p[] __initconst = {
 	"i2scdclk2",
 	"pcmcdclk2",
 	"sclk_hdmi27m",
@@ -254,63 +254,63 @@ static const char *mout_audio2_p[] __initdata = {
 	"mout_vpll",
 };
 
-static const char *mout_spdif_p[] __initdata = {
+static const char *const mout_spdif_p[] __initconst = {
 	"dout_audio0",
 	"dout_audio1",
 	"dout_audio3",
 };
 
-static const char *mout_group3_p[] __initdata = {
+static const char *const mout_group3_p[] __initconst = {
 	"mout_apll",
 	"mout_mpll"
 };
 
-static const char *mout_group4_p[] __initdata = {
+static const char *const mout_group4_p[] __initconst = {
 	"mout_mpll",
 	"dout_a2m"
 };
 
-static const char *mout_flash_p[] __initdata = {
+static const char *const mout_flash_p[] __initconst = {
 	"dout_hclkd",
 	"dout_hclkp"
 };
 
-static const char *mout_dac_p[] __initdata = {
+static const char *const mout_dac_p[] __initconst = {
 	"mout_vpll",
 	"sclk_hdmiphy"
 };
 
-static const char *mout_hdmi_p[] __initdata = {
+static const char *const mout_hdmi_p[] __initconst = {
 	"sclk_hdmiphy",
 	"dout_tblk"
 };
 
-static const char *mout_mixer_p[] __initdata = {
+static const char *const mout_mixer_p[] __initconst = {
 	"mout_dac",
 	"mout_hdmi"
 };
 
-static const char *mout_vpll_6442_p[] __initdata = {
+static const char *const mout_vpll_6442_p[] __initconst = {
 	"fin_pll",
 	"fout_vpll"
 };
 
-static const char *mout_mixer_6442_p[] __initdata = {
+static const char *const mout_mixer_6442_p[] __initconst = {
 	"mout_vpll",
 	"dout_mixer"
 };
 
-static const char *mout_d0sync_6442_p[] __initdata = {
+static const char *const mout_d0sync_6442_p[] __initconst = {
 	"mout_dsys",
 	"div_apll"
 };
 
-static const char *mout_d1sync_6442_p[] __initdata = {
+static const char *const mout_d1sync_6442_p[] __initconst = {
 	"mout_psys",
 	"div_apll"
 };
 
-static const char *mout_group2_6442_p[] __initdata = {
+static const char *const mout_group2_6442_p[] __initconst = {
 	"fin_pll",
 	"none",
 	"none",
@@ -322,7 +322,7 @@ static const char *mout_group2_6442_p[] __initdata = {
 	"mout_vpll",
 };
 
-static const char *mout_audio0_6442_p[] __initdata = {
+static const char *const mout_audio0_6442_p[] __initconst = {
 	"fin_pll",
 	"pcmcdclk0",
 	"none",
@@ -334,7 +334,7 @@ static const char *mout_audio0_6442_p[] __initdata = {
 	"mout_vpll",
 };
 
-static const char *mout_audio1_6442_p[] __initdata = {
+static const char *const mout_audio1_6442_p[] __initconst = {
 	"i2scdclk1",
 	"pcmcdclk1",
 	"none",
@@ -347,7 +347,7 @@ static const char *mout_audio1_6442_p[] __initdata = {
 	"fin_pll",
 };
 
-static const char *mout_clksel_p[] __initdata = {
+static const char *const mout_clksel_p[] __initconst = {
 	"fout_apll_clkout",
 	"fout_mpll_clkout",
 	"fout_epll",
@@ -370,7 +370,7 @@ static const char *mout_clksel_p[] __initdata = {
 	"div_dclk"
 };
 
-static const char *mout_clksel_6442_p[] __initdata = {
+static const char *const mout_clksel_6442_p[] __initconst = {
 	"fout_apll_clkout",
 	"fout_mpll_clkout",
 	"fout_epll",
@@ -393,7 +393,7 @@ static const char *mout_clksel_6442_p[] __initdata = {
 	"div_dclk"
 };
 
-static const char *mout_clkout_p[] __initdata = {
+static const char *const mout_clkout_p[] __initconst = {
 	"dout_clkout",
 	"none",
 	"xxti",
@@ -401,20 +401,20 @@ static const char *mout_clkout_p[] __initdata = {
 };
 
 /* Common fixed factor clocks. */
-static struct samsung_fixed_factor_clock ffactor_clks[] __initdata = {
+static const struct samsung_fixed_factor_clock ffactor_clks[] __initconst = {
 	FFACTOR(FOUT_APLL_CLKOUT, "fout_apll_clkout", "fout_apll", 1, 4, 0),
 	FFACTOR(FOUT_MPLL_CLKOUT, "fout_mpll_clkout", "fout_mpll", 1, 2, 0),
 	FFACTOR(DOUT_APLL_CLKOUT, "dout_apll_clkout", "dout_apll", 1, 4, 0),
 };
 
 /* PLL input mux (fin_pll), which needs to be registered before PLLs. */
-static struct samsung_mux_clock early_mux_clks[] __initdata = {
+static const struct samsung_mux_clock early_mux_clks[] __initconst = {
 	MUX_F(FIN_PLL, "fin_pll", fin_pll_p, OM_STAT, 0, 1,
 					CLK_MUX_READ_ONLY, 0),
 };
 
 /* Common clock muxes. */
-static struct samsung_mux_clock mux_clks[] __initdata = {
+static const struct samsung_mux_clock mux_clks[] __initconst = {
 	MUX(MOUT_FLASH, "mout_flash", mout_flash_p, CLK_SRC0, 28, 1),
 	MUX(MOUT_PSYS, "mout_psys", mout_group4_p, CLK_SRC0, 24, 1),
 	MUX(MOUT_DSYS, "mout_dsys", mout_group4_p, CLK_SRC0, 20, 1),
@@ -427,7 +427,7 @@ static struct samsung_mux_clock mux_clks[] __initdata = {
 };
 
 /* S5PV210-specific clock muxes. */
-static struct samsung_mux_clock s5pv210_mux_clks[] __initdata = {
+static const struct samsung_mux_clock s5pv210_mux_clks[] __initconst = {
 	MUX(MOUT_VPLL, "mout_vpll", mout_vpll_p, CLK_SRC0, 12, 1),
 
 	MUX(MOUT_VPLLSRC, "mout_vpllsrc", mout_vpllsrc_p, CLK_SRC1, 28, 1),
@@ -472,7 +472,7 @@ static struct samsung_mux_clock s5pv210_mux_clks[] __initdata = {
 };
 
 /* S5P6442-specific clock muxes. */
-static struct samsung_mux_clock s5p6442_mux_clks[] __initdata = {
+static const struct samsung_mux_clock s5p6442_mux_clks[] __initconst = {
 	MUX(MOUT_VPLL, "mout_vpll", mout_vpll_6442_p, CLK_SRC0, 12, 1),
 
 	MUX(MOUT_FIMD, "mout_fimd", mout_group2_6442_p, CLK_SRC1, 20, 4),
@@ -504,7 +504,7 @@ static struct samsung_mux_clock s5p6442_mux_clks[] __initdata = {
 };
 
 /* S5PV210-specific fixed rate clocks generated inside the SoC. */
-static struct samsung_fixed_rate_clock s5pv210_frate_clks[] __initdata = {
+static const struct samsung_fixed_rate_clock s5pv210_frate_clks[] __initconst = {
 	FRATE(SCLK_HDMI27M, "sclk_hdmi27m", NULL, CLK_IS_ROOT, 27000000),
 	FRATE(SCLK_HDMIPHY, "sclk_hdmiphy", NULL, CLK_IS_ROOT, 27000000),
 	FRATE(SCLK_USBPHY0, "sclk_usbphy0", NULL, CLK_IS_ROOT, 48000000),
@@ -512,12 +512,12 @@ static struct samsung_fixed_rate_clock s5pv210_frate_clks[] __initdata = {
 };
 
 /* S5P6442-specific fixed rate clocks generated inside the SoC. */
-static struct samsung_fixed_rate_clock s5p6442_frate_clks[] __initdata = {
+static const struct samsung_fixed_rate_clock s5p6442_frate_clks[] __initconst = {
 	FRATE(SCLK_USBPHY0, "sclk_usbphy0", NULL, CLK_IS_ROOT, 30000000),
 };
 
 /* Common clock dividers. */
-static struct samsung_div_clock div_clks[] __initdata = {
+static const struct samsung_div_clock div_clks[] __initconst = {
 	DIV(DOUT_PCLKP, "dout_pclkp", "dout_hclkp", CLK_DIV0, 28, 3),
 	DIV(DOUT_PCLKD, "dout_pclkd", "dout_hclkd", CLK_DIV0, 20, 3),
 	DIV(DOUT_A2M, "dout_a2m", "mout_apll", CLK_DIV0, 4, 3),
@@ -549,7 +549,7 @@ static struct samsung_div_clock div_clks[] __initdata = {
 };
 
 /* S5PV210-specific clock dividers. */
-static struct samsung_div_clock s5pv210_div_clks[] __initdata = {
+static const struct samsung_div_clock s5pv210_div_clks[] __initconst = {
 	DIV(DOUT_HCLKP, "dout_hclkp", "mout_psys", CLK_DIV0, 24, 4),
 	DIV(DOUT_HCLKD, "dout_hclkd", "mout_dsys", CLK_DIV0, 16, 4),
 	DIV(DOUT_PCLKM, "dout_pclkm", "dout_hclkm", CLK_DIV0, 12, 3),
@@ -578,7 +578,7 @@ static struct samsung_div_clock s5pv210_div_clks[] __initdata = {
 };
 
 /* S5P6442-specific clock dividers. */
-static struct samsung_div_clock s5p6442_div_clks[] __initdata = {
+static const struct samsung_div_clock s5p6442_div_clks[] __initconst = {
 	DIV(DOUT_HCLKP, "dout_hclkp", "mout_d1sync", CLK_DIV0, 24, 4),
 	DIV(DOUT_HCLKD, "dout_hclkd", "mout_d0sync", CLK_DIV0, 16, 4),
 
@@ -586,7 +586,7 @@ static struct samsung_div_clock s5p6442_div_clks[] __initdata = {
 };
 
 /* Common clock gates. */
-static struct samsung_gate_clock gate_clks[] __initdata = {
+static const struct samsung_gate_clock gate_clks[] __initconst = {
 	GATE(CLK_ROTATOR, "rotator", "dout_hclkd", CLK_GATE_IP0, 29, 0, 0),
 	GATE(CLK_FIMC2, "fimc2", "dout_hclkd", CLK_GATE_IP0, 26, 0, 0),
 	GATE(CLK_FIMC1, "fimc1", "dout_hclkd", CLK_GATE_IP0, 25, 0, 0),
@@ -666,7 +666,7 @@ static struct samsung_gate_clock gate_clks[] __initdata = {
 };
 
 /* S5PV210-specific clock gates. */
-static struct samsung_gate_clock s5pv210_gate_clks[] __initdata = {
+static const struct samsung_gate_clock s5pv210_gate_clks[] __initconst = {
 	GATE(CLK_CSIS, "clk_csis", "dout_hclkd", CLK_GATE_IP0, 31, 0, 0),
 	GATE(CLK_MFC, "mfc", "dout_hclkm", CLK_GATE_IP0, 16, 0, 0),
 	GATE(CLK_G2D, "g2d", "dout_hclkd", CLK_GATE_IP0, 12, 0, 0),
@@ -728,7 +728,7 @@ static struct samsung_gate_clock s5pv210_gate_clks[] __initdata = {
 };
 
 /* S5P6442-specific clock gates. */
-static struct samsung_gate_clock s5p6442_gate_clks[] __initdata = {
+static const struct samsung_gate_clock s5p6442_gate_clks[] __initconst = {
 	GATE(CLK_JPEG, "jpeg", "dout_hclkd", CLK_GATE_IP0, 28, 0, 0),
 	GATE(CLK_MFC, "mfc", "dout_hclkd", CLK_GATE_IP0, 16, 0, 0),
 	GATE(CLK_G2D, "g2d", "dout_hclkd", CLK_GATE_IP0, 12, 0, 0),
@@ -748,14 +748,14 @@ static struct samsung_gate_clock s5p6442_gate_clks[] __initdata = {
  * Clock aliases for legacy clkdev look-up.
  * NOTE: Needed only to support legacy board files.
  */
-static struct samsung_clock_alias s5pv210_aliases[] = {
+static const struct samsung_clock_alias s5pv210_aliases[] __initconst = {
 	ALIAS(DOUT_APLL, NULL, "armclk"),
 	ALIAS(DOUT_HCLKM, NULL, "hclk_msys"),
 	ALIAS(MOUT_DMC0, NULL, "sclk_dmc0"),
 };
 
 /* S5PV210-specific PLLs. */
-static struct samsung_pll_clock s5pv210_pll_clks[] __initdata = {
+static const struct samsung_pll_clock s5pv210_pll_clks[] __initconst = {
 	[apll] = PLL(pll_4508, FOUT_APLL, "fout_apll", "fin_pll",
 						APLL_LOCK, APLL_CON0, NULL),
 	[mpll] = PLL(pll_4502, FOUT_MPLL, "fout_mpll", "fin_pll",
@@ -767,7 +767,7 @@ static struct samsung_pll_clock s5pv210_pll_clks[] __initdata = {
 };
 
 /* S5P6442-specific PLLs. */
-static struct samsung_pll_clock s5p6442_pll_clks[] __initdata = {
+static const struct samsung_pll_clock s5p6442_pll_clks[] __initconst = {
 	[apll] = PLL(pll_4502, FOUT_APLL, "fout_apll", "fin_pll",
 						APLL_LOCK, APLL_CON0, NULL),
 	[mpll] = PLL(pll_4502, FOUT_MPLL, "fout_mpll", "fin_pll",
diff --git a/drivers/clk/samsung/clk.c b/drivers/clk/samsung/clk.c
index 9e1f88c04fd4..0117238391d6 100644
--- a/drivers/clk/samsung/clk.c
+++ b/drivers/clk/samsung/clk.c
@@ -98,7 +98,7 @@ void samsung_clk_add_lookup(struct samsung_clk_provider *ctx, struct clk *clk,
 
 /* register a list of aliases */
 void __init samsung_clk_register_alias(struct samsung_clk_provider *ctx,
-				struct samsung_clock_alias *list,
+				const struct samsung_clock_alias *list,
 				unsigned int nr_clk)
 {
 	struct clk *clk;
@@ -132,7 +132,8 @@ void __init samsung_clk_register_alias(struct samsung_clk_provider *ctx,
 
 /* register a list of fixed clocks */
 void __init samsung_clk_register_fixed_rate(struct samsung_clk_provider *ctx,
-		struct samsung_fixed_rate_clock *list, unsigned int nr_clk)
+		const struct samsung_fixed_rate_clock *list,
+		unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx, ret;
@@ -161,7 +162,7 @@ void __init samsung_clk_register_fixed_rate(struct samsung_clk_provider *ctx,
 
 /* register a list of fixed factor clocks */
 void __init samsung_clk_register_fixed_factor(struct samsung_clk_provider *ctx,
-		struct samsung_fixed_factor_clock *list, unsigned int nr_clk)
+		const struct samsung_fixed_factor_clock *list, unsigned int nr_clk)
 {
 	struct clk *clk;
 	unsigned int idx;
@@ -181,7 +182,7 @@ void __init samsung_clk_register_fixed_factor(struct samsung_clk_provider *ctx,
 
 /* register a list of mux clocks */
 void __init samsung_clk_register_mux(struct samsung_clk_provider *ctx,
-				struct samsung_mux_clock *list,
+				const struct samsung_mux_clock *list,
 				unsigned int nr_clk)
 {
 	struct clk *clk;
@@ -213,7 +214,7 @@ void __init samsung_clk_register_mux(struct samsung_clk_provider *ctx,
 
 /* register a list of div clocks */
 void __init samsung_clk_register_div(struct samsung_clk_provider *ctx,
-				struct samsung_div_clock *list,
+				const struct samsung_div_clock *list,
 				unsigned int nr_clk)
 {
 	struct clk *clk;
@@ -252,7 +253,7 @@ void __init samsung_clk_register_div(struct samsung_clk_provider *ctx,
 
 /* register a list of gate clocks */
 void __init samsung_clk_register_gate(struct samsung_clk_provider *ctx,
-				struct samsung_gate_clock *list,
+				const struct samsung_gate_clock *list,
 				unsigned int nr_clk)
 {
 	struct clk *clk;
@@ -389,7 +390,7 @@ struct samsung_clk_provider * __init samsung_cmu_register_one(
 
 	ctx = samsung_clk_init(np, reg_base, cmu->nr_clk_ids);
 	if (!ctx) {
-		panic("%s: unable to alllocate ctx\n", __func__);
+		panic("%s: unable to allocate ctx\n", __func__);
 		return ctx;
 	}
 
diff --git a/drivers/clk/samsung/clk.h b/drivers/clk/samsung/clk.h
index e4c75383cea7..b775fc29caa5 100644
--- a/drivers/clk/samsung/clk.h
+++ b/drivers/clk/samsung/clk.h
@@ -121,7 +121,7 @@ struct samsung_mux_clock {
 	unsigned int		id;
 	const char		*dev_name;
 	const char		*name;
-	const char		**parent_names;
+	const char		*const *parent_names;
 	u8			num_parents;
 	unsigned long		flags;
 	unsigned long		offset;
@@ -368,28 +368,28 @@ extern void __init samsung_clk_of_register_fixed_ext(
 extern void samsung_clk_add_lookup(struct samsung_clk_provider *ctx,
 			struct clk *clk, unsigned int id);
 
-extern void samsung_clk_register_alias(struct samsung_clk_provider *ctx,
-			struct samsung_clock_alias *list,
+extern void __init samsung_clk_register_alias(struct samsung_clk_provider *ctx,
+			const struct samsung_clock_alias *list,
 			unsigned int nr_clk);
 extern void __init samsung_clk_register_fixed_rate(
 			struct samsung_clk_provider *ctx,
-			struct samsung_fixed_rate_clock *clk_list,
+			const struct samsung_fixed_rate_clock *clk_list,
 			unsigned int nr_clk);
 extern void __init samsung_clk_register_fixed_factor(
 			struct samsung_clk_provider *ctx,
-			struct samsung_fixed_factor_clock *list,
+			const struct samsung_fixed_factor_clock *list,
 			unsigned int nr_clk);
 extern void __init samsung_clk_register_mux(struct samsung_clk_provider *ctx,
-			struct samsung_mux_clock *clk_list,
+			const struct samsung_mux_clock *clk_list,
 			unsigned int nr_clk);
 extern void __init samsung_clk_register_div(struct samsung_clk_provider *ctx,
-			struct samsung_div_clock *clk_list,
+			const struct samsung_div_clock *clk_list,
 			unsigned int nr_clk);
 extern void __init samsung_clk_register_gate(struct samsung_clk_provider *ctx,
-			struct samsung_gate_clock *clk_list,
+			const struct samsung_gate_clock *clk_list,
 			unsigned int nr_clk);
 extern void __init samsung_clk_register_pll(struct samsung_clk_provider *ctx,
-			struct samsung_pll_clock *pll_list,
+			const struct samsung_pll_clock *pll_list,
 			unsigned int nr_clk, void __iomem *base);
 
 extern struct samsung_clk_provider __init *samsung_cmu_register_one(
diff --git a/drivers/clk/shmobile/clk-emev2.c b/drivers/clk/shmobile/clk-emev2.c
index 6c7c929c7765..5b60beb7d0eb 100644
--- a/drivers/clk/shmobile/clk-emev2.c
+++ b/drivers/clk/shmobile/clk-emev2.c
@@ -34,7 +34,7 @@
 static DEFINE_SPINLOCK(lock);
 
 /* not pretty, but hey */
-void __iomem *smu_base;
+static void __iomem *smu_base;
 
 static void __init emev2_smu_write(unsigned long value, int offs)
 {
diff --git a/drivers/clk/sirf/Makefile b/drivers/clk/sirf/Makefile
index 36b8e203f6e7..09b4210d9124 100644
--- a/drivers/clk/sirf/Makefile
+++ b/drivers/clk/sirf/Makefile
@@ -2,4 +2,4 @@
 # Makefile for sirf specific clk
 #
 
-obj-$(CONFIG_ARCH_SIRF) += clk-prima2.o clk-atlas6.o
+obj-$(CONFIG_ARCH_SIRF) += clk-prima2.o clk-atlas6.o clk-atlas7.o
diff --git a/drivers/clk/sirf/clk-atlas7.c b/drivers/clk/sirf/clk-atlas7.c
new file mode 100644
index 000000000000..db8ab691dbf6
--- /dev/null
+++ b/drivers/clk/sirf/clk-atlas7.c
@@ -0,0 +1,1632 @@
+/*
+ * Clock tree for CSR SiRFAtlas7
+ *
+ * Copyright (c) 2014 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/bitops.h>
+#include <linux/io.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/of_address.h>
+#include <linux/reset-controller.h>
+#include <linux/slab.h>
+
+#define SIRFSOC_CLKC_MEMPLL_AB_FREQ          0x0000
+#define SIRFSOC_CLKC_MEMPLL_AB_SSC           0x0004
+#define SIRFSOC_CLKC_MEMPLL_AB_CTRL0         0x0008
+#define SIRFSOC_CLKC_MEMPLL_AB_CTRL1         0x000c
+#define SIRFSOC_CLKC_MEMPLL_AB_STATUS        0x0010
+#define SIRFSOC_CLKC_MEMPLL_AB_SSRAM_ADDR    0x0014
+#define SIRFSOC_CLKC_MEMPLL_AB_SSRAM_DATA    0x0018
+
+#define SIRFSOC_CLKC_CPUPLL_AB_FREQ          0x001c
+#define SIRFSOC_CLKC_CPUPLL_AB_SSC           0x0020
+#define SIRFSOC_CLKC_CPUPLL_AB_CTRL0         0x0024
+#define SIRFSOC_CLKC_CPUPLL_AB_CTRL1         0x0028
+#define SIRFSOC_CLKC_CPUPLL_AB_STATUS        0x002c
+
+#define SIRFSOC_CLKC_SYS0PLL_AB_FREQ         0x0030
+#define SIRFSOC_CLKC_SYS0PLL_AB_SSC          0x0034
+#define SIRFSOC_CLKC_SYS0PLL_AB_CTRL0        0x0038
+#define SIRFSOC_CLKC_SYS0PLL_AB_CTRL1        0x003c
+#define SIRFSOC_CLKC_SYS0PLL_AB_STATUS       0x0040
+
+#define SIRFSOC_CLKC_SYS1PLL_AB_FREQ         0x0044
+#define SIRFSOC_CLKC_SYS1PLL_AB_SSC          0x0048
+#define SIRFSOC_CLKC_SYS1PLL_AB_CTRL0        0x004c
+#define SIRFSOC_CLKC_SYS1PLL_AB_CTRL1        0x0050
+#define SIRFSOC_CLKC_SYS1PLL_AB_STATUS       0x0054
+
+#define SIRFSOC_CLKC_SYS2PLL_AB_FREQ         0x0058
+#define SIRFSOC_CLKC_SYS2PLL_AB_SSC          0x005c
+#define SIRFSOC_CLKC_SYS2PLL_AB_CTRL0        0x0060
+#define SIRFSOC_CLKC_SYS2PLL_AB_CTRL1        0x0064
+#define SIRFSOC_CLKC_SYS2PLL_AB_STATUS       0x0068
+
+#define SIRFSOC_CLKC_SYS3PLL_AB_FREQ         0x006c
+#define SIRFSOC_CLKC_SYS3PLL_AB_SSC          0x0070
+#define SIRFSOC_CLKC_SYS3PLL_AB_CTRL0        0x0074
+#define SIRFSOC_CLKC_SYS3PLL_AB_CTRL1        0x0078
+#define SIRFSOC_CLKC_SYS3PLL_AB_STATUS       0x007c
+
+#define SIRFSOC_ABPLL_CTRL0_SSEN     0x00001000
+#define SIRFSOC_ABPLL_CTRL0_BYPASS   0x00000010
+#define SIRFSOC_ABPLL_CTRL0_RESET    0x00000001
+
+#define SIRFSOC_CLKC_AUDIO_DTO_INC           0x0088
+#define SIRFSOC_CLKC_DISP0_DTO_INC           0x008c
+#define SIRFSOC_CLKC_DISP1_DTO_INC           0x0090
+
+#define SIRFSOC_CLKC_AUDIO_DTO_SRC           0x0094
+#define SIRFSOC_CLKC_AUDIO_DTO_ENA           0x0098
+#define SIRFSOC_CLKC_AUDIO_DTO_DROFF         0x009c
+
+#define SIRFSOC_CLKC_DISP0_DTO_SRC           0x00a0
+#define SIRFSOC_CLKC_DISP0_DTO_ENA           0x00a4
+#define SIRFSOC_CLKC_DISP0_DTO_DROFF         0x00a8
+
+#define SIRFSOC_CLKC_DISP1_DTO_SRC           0x00ac
+#define SIRFSOC_CLKC_DISP1_DTO_ENA           0x00b0
+#define SIRFSOC_CLKC_DISP1_DTO_DROFF         0x00b4
+
+#define SIRFSOC_CLKC_I2S_CLK_SEL             0x00b8
+#define SIRFSOC_CLKC_I2S_SEL_STAT            0x00bc
+
+#define SIRFSOC_CLKC_USBPHY_CLKDIV_CFG       0x00c0
+#define SIRFSOC_CLKC_USBPHY_CLKDIV_ENA       0x00c4
+#define SIRFSOC_CLKC_USBPHY_CLK_SEL          0x00c8
+#define SIRFSOC_CLKC_USBPHY_CLK_SEL_STAT     0x00cc
+
+#define SIRFSOC_CLKC_BTSS_CLKDIV_CFG         0x00d0
+#define SIRFSOC_CLKC_BTSS_CLKDIV_ENA         0x00d4
+#define SIRFSOC_CLKC_BTSS_CLK_SEL            0x00d8
+#define SIRFSOC_CLKC_BTSS_CLK_SEL_STAT       0x00dc
+
+#define SIRFSOC_CLKC_RGMII_CLKDIV_CFG        0x00e0
+#define SIRFSOC_CLKC_RGMII_CLKDIV_ENA        0x00e4
+#define SIRFSOC_CLKC_RGMII_CLK_SEL           0x00e8
+#define SIRFSOC_CLKC_RGMII_CLK_SEL_STAT      0x00ec
+
+#define SIRFSOC_CLKC_CPU_CLKDIV_CFG          0x00f0
+#define SIRFSOC_CLKC_CPU_CLKDIV_ENA          0x00f4
+#define SIRFSOC_CLKC_CPU_CLK_SEL             0x00f8
+#define SIRFSOC_CLKC_CPU_CLK_SEL_STAT        0x00fc
+
+#define SIRFSOC_CLKC_SDPHY01_CLKDIV_CFG      0x0100
+#define SIRFSOC_CLKC_SDPHY01_CLKDIV_ENA      0x0104
+#define SIRFSOC_CLKC_SDPHY01_CLK_SEL         0x0108
+#define SIRFSOC_CLKC_SDPHY01_CLK_SEL_STAT    0x010c
+
+#define SIRFSOC_CLKC_SDPHY23_CLKDIV_CFG      0x0110
+#define SIRFSOC_CLKC_SDPHY23_CLKDIV_ENA      0x0114
+#define SIRFSOC_CLKC_SDPHY23_CLK_SEL         0x0118
+#define SIRFSOC_CLKC_SDPHY23_CLK_SEL_STAT    0x011c
+
+#define SIRFSOC_CLKC_SDPHY45_CLKDIV_CFG      0x0120
+#define SIRFSOC_CLKC_SDPHY45_CLKDIV_ENA      0x0124
+#define SIRFSOC_CLKC_SDPHY45_CLK_SEL         0x0128
+#define SIRFSOC_CLKC_SDPHY45_CLK_SEL_STAT    0x012c
+
+#define SIRFSOC_CLKC_SDPHY67_CLKDIV_CFG      0x0130
+#define SIRFSOC_CLKC_SDPHY67_CLKDIV_ENA      0x0134
+#define SIRFSOC_CLKC_SDPHY67_CLK_SEL         0x0138
+#define SIRFSOC_CLKC_SDPHY67_CLK_SEL_STAT    0x013c
+
+#define SIRFSOC_CLKC_CAN_CLKDIV_CFG          0x0140
+#define SIRFSOC_CLKC_CAN_CLKDIV_ENA          0x0144
+#define SIRFSOC_CLKC_CAN_CLK_SEL             0x0148
+#define SIRFSOC_CLKC_CAN_CLK_SEL_STAT        0x014c
+
+#define SIRFSOC_CLKC_DEINT_CLKDIV_CFG        0x0150
+#define SIRFSOC_CLKC_DEINT_CLKDIV_ENA        0x0154
+#define SIRFSOC_CLKC_DEINT_CLK_SEL           0x0158
+#define SIRFSOC_CLKC_DEINT_CLK_SEL_STAT      0x015c
+
+#define SIRFSOC_CLKC_NAND_CLKDIV_CFG         0x0160
+#define SIRFSOC_CLKC_NAND_CLKDIV_ENA         0x0164
+#define SIRFSOC_CLKC_NAND_CLK_SEL            0x0168
+#define SIRFSOC_CLKC_NAND_CLK_SEL_STAT       0x016c
+
+#define SIRFSOC_CLKC_DISP0_CLKDIV_CFG        0x0170
+#define SIRFSOC_CLKC_DISP0_CLKDIV_ENA        0x0174
+#define SIRFSOC_CLKC_DISP0_CLK_SEL           0x0178
+#define SIRFSOC_CLKC_DISP0_CLK_SEL_STAT      0x017c
+
+#define SIRFSOC_CLKC_DISP1_CLKDIV_CFG        0x0180
+#define SIRFSOC_CLKC_DISP1_CLKDIV_ENA        0x0184
+#define SIRFSOC_CLKC_DISP1_CLK_SEL           0x0188
+#define SIRFSOC_CLKC_DISP1_CLK_SEL_STAT      0x018c
+
+#define SIRFSOC_CLKC_GPU_CLKDIV_CFG          0x0190
+#define SIRFSOC_CLKC_GPU_CLKDIV_ENA          0x0194
+#define SIRFSOC_CLKC_GPU_CLK_SEL             0x0198
+#define SIRFSOC_CLKC_GPU_CLK_SEL_STAT        0x019c
+
+#define SIRFSOC_CLKC_GNSS_CLKDIV_CFG         0x01a0
+#define SIRFSOC_CLKC_GNSS_CLKDIV_ENA         0x01a4
+#define SIRFSOC_CLKC_GNSS_CLK_SEL            0x01a8
+#define SIRFSOC_CLKC_GNSS_CLK_SEL_STAT       0x01ac
+
+#define SIRFSOC_CLKC_SHARED_DIVIDER_CFG0     0x01b0
+#define SIRFSOC_CLKC_SHARED_DIVIDER_CFG1     0x01b4
+#define SIRFSOC_CLKC_SHARED_DIVIDER_ENA      0x01b8
+
+#define SIRFSOC_CLKC_SYS_CLK_SEL             0x01bc
+#define SIRFSOC_CLKC_SYS_CLK_SEL_STAT        0x01c0
+#define SIRFSOC_CLKC_IO_CLK_SEL              0x01c4
+#define SIRFSOC_CLKC_IO_CLK_SEL_STAT         0x01c8
+#define SIRFSOC_CLKC_G2D_CLK_SEL             0x01cc
+#define SIRFSOC_CLKC_G2D_CLK_SEL_STAT        0x01d0
+#define SIRFSOC_CLKC_JPENC_CLK_SEL           0x01d4
+#define SIRFSOC_CLKC_JPENC_CLK_SEL_STAT      0x01d8
+#define SIRFSOC_CLKC_VDEC_CLK_SEL            0x01dc
+#define SIRFSOC_CLKC_VDEC_CLK_SEL_STAT       0x01e0
+#define SIRFSOC_CLKC_GMAC_CLK_SEL            0x01e4
+#define SIRFSOC_CLKC_GMAC_CLK_SEL_STAT       0x01e8
+#define SIRFSOC_CLKC_USB_CLK_SEL             0x01ec
+#define SIRFSOC_CLKC_USB_CLK_SEL_STAT        0x01f0
+#define SIRFSOC_CLKC_KAS_CLK_SEL             0x01f4
+#define SIRFSOC_CLKC_KAS_CLK_SEL_STAT        0x01f8
+#define SIRFSOC_CLKC_SEC_CLK_SEL             0x01fc
+#define SIRFSOC_CLKC_SEC_CLK_SEL_STAT        0x0200
+#define SIRFSOC_CLKC_SDR_CLK_SEL             0x0204
+#define SIRFSOC_CLKC_SDR_CLK_SEL_STAT        0x0208
+#define SIRFSOC_CLKC_VIP_CLK_SEL             0x020c
+#define SIRFSOC_CLKC_VIP_CLK_SEL_STAT        0x0210
+#define SIRFSOC_CLKC_NOCD_CLK_SEL            0x0214
+#define SIRFSOC_CLKC_NOCD_CLK_SEL_STAT       0x0218
+#define SIRFSOC_CLKC_NOCR_CLK_SEL            0x021c
+#define SIRFSOC_CLKC_NOCR_CLK_SEL_STAT       0x0220
+#define SIRFSOC_CLKC_TPIU_CLK_SEL            0x0224
+#define SIRFSOC_CLKC_TPIU_CLK_SEL_STAT       0x0228
+
+#define SIRFSOC_CLKC_ROOT_CLK_EN0_SET        0x022c
+#define SIRFSOC_CLKC_ROOT_CLK_EN0_CLR        0x0230
+#define SIRFSOC_CLKC_ROOT_CLK_EN0_STAT       0x0234
+#define SIRFSOC_CLKC_ROOT_CLK_EN1_SET        0x0238
+#define SIRFSOC_CLKC_ROOT_CLK_EN1_CLR        0x023c
+#define SIRFSOC_CLKC_ROOT_CLK_EN1_STAT       0x0240
+
+#define SIRFSOC_CLKC_LEAF_CLK_EN0_SET        0x0244
+#define SIRFSOC_CLKC_LEAF_CLK_EN0_CLR        0x0248
+#define SIRFSOC_CLKC_LEAF_CLK_EN0_STAT       0x024c
+
+#define SIRFSOC_CLKC_RSTC_A7_SW_RST          0x0308
+
+#define SIRFSOC_CLKC_LEAF_CLK_EN1_SET        0x04a0
+#define SIRFSOC_CLKC_LEAF_CLK_EN2_SET        0x04b8
+#define SIRFSOC_CLKC_LEAF_CLK_EN3_SET        0x04d0
+#define SIRFSOC_CLKC_LEAF_CLK_EN4_SET        0x04e8
+#define SIRFSOC_CLKC_LEAF_CLK_EN5_SET        0x0500
+#define SIRFSOC_CLKC_LEAF_CLK_EN6_SET        0x0518
+#define SIRFSOC_CLKC_LEAF_CLK_EN7_SET        0x0530
+#define SIRFSOC_CLKC_LEAF_CLK_EN8_SET        0x0548
+
+
+static void __iomem *sirfsoc_clk_vbase;
+static struct clk_onecell_data clk_data;
+
+static const struct clk_div_table pll_div_table[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 8 },
+	{ .val = 4, .div = 16 },
+	{ .val = 5, .div = 32 },
+};
+
+struct clk_pll {
+	struct clk_hw hw;
+	u16 regofs;  /* register offset */
+};
+#define to_pllclk(_hw) container_of(_hw, struct clk_pll, hw)
+
+struct clk_dto {
+	struct clk_hw hw;
+	u16 inc_offset;  /* dto increment offset */
+	u16 src_offset;  /* dto src offset */
+};
+#define to_dtoclk(_hw) container_of(_hw, struct clk_dto, hw)
+
+struct clk_unit {
+	struct clk_hw hw;
+	u16 regofs;
+	u16 bit;
+	spinlock_t *lock;
+};
+#define to_unitclk(_hw) container_of(_hw, struct clk_unit, hw)
+
+struct atlas7_div_init_data {
+	const char *div_name;
+	const char *parent_name;
+	const char *gate_name;
+	unsigned long flags;
+	u8 divider_flags;
+	u8 gate_flags;
+	u32 div_offset;
+	u8 shift;
+	u8 width;
+	u32 gate_offset;
+	u8 gate_bit;
+	spinlock_t *lock;
+};
+
+struct atlas7_mux_init_data {
+	const char *mux_name;
+	const char * const *parent_names;
+	u8 parent_num;
+	unsigned long flags;
+	u8 mux_flags;
+	u32 mux_offset;
+	u8 shift;
+	u8 width;
+};
+
+struct atlas7_unit_init_data {
+	u32 index;
+	const char *unit_name;
+	const char *parent_name;
+	unsigned long flags;
+	u32 regofs;
+	u8 bit;
+	spinlock_t *lock;
+};
+
+struct atlas7_reset_desc {
+	const char *name;
+	u32 clk_ofs;
+	u8  clk_bit;
+	u32 rst_ofs;
+	u8  rst_bit;
+	spinlock_t *lock;
+};
+
+static DEFINE_SPINLOCK(cpupll_ctrl1_lock);
+static DEFINE_SPINLOCK(mempll_ctrl1_lock);
+static DEFINE_SPINLOCK(sys0pll_ctrl1_lock);
+static DEFINE_SPINLOCK(sys1pll_ctrl1_lock);
+static DEFINE_SPINLOCK(sys2pll_ctrl1_lock);
+static DEFINE_SPINLOCK(sys3pll_ctrl1_lock);
+static DEFINE_SPINLOCK(usbphy_div_lock);
+static DEFINE_SPINLOCK(btss_div_lock);
+static DEFINE_SPINLOCK(rgmii_div_lock);
+static DEFINE_SPINLOCK(cpu_div_lock);
+static DEFINE_SPINLOCK(sdphy01_div_lock);
+static DEFINE_SPINLOCK(sdphy23_div_lock);
+static DEFINE_SPINLOCK(sdphy45_div_lock);
+static DEFINE_SPINLOCK(sdphy67_div_lock);
+static DEFINE_SPINLOCK(can_div_lock);
+static DEFINE_SPINLOCK(deint_div_lock);
+static DEFINE_SPINLOCK(nand_div_lock);
+static DEFINE_SPINLOCK(disp0_div_lock);
+static DEFINE_SPINLOCK(disp1_div_lock);
+static DEFINE_SPINLOCK(gpu_div_lock);
+static DEFINE_SPINLOCK(gnss_div_lock);
+/* gate register shared */
+static DEFINE_SPINLOCK(share_div_lock);
+static DEFINE_SPINLOCK(root0_gate_lock);
+static DEFINE_SPINLOCK(root1_gate_lock);
+static DEFINE_SPINLOCK(leaf0_gate_lock);
+static DEFINE_SPINLOCK(leaf1_gate_lock);
+static DEFINE_SPINLOCK(leaf2_gate_lock);
+static DEFINE_SPINLOCK(leaf3_gate_lock);
+static DEFINE_SPINLOCK(leaf4_gate_lock);
+static DEFINE_SPINLOCK(leaf5_gate_lock);
+static DEFINE_SPINLOCK(leaf6_gate_lock);
+static DEFINE_SPINLOCK(leaf7_gate_lock);
+static DEFINE_SPINLOCK(leaf8_gate_lock);
+
+static inline unsigned long clkc_readl(unsigned reg)
+{
+	return readl(sirfsoc_clk_vbase + reg);
+}
+
+static inline void clkc_writel(u32 val, unsigned reg)
+{
+	writel(val, sirfsoc_clk_vbase + reg);
+}
+
+/*
+*  ABPLL
+*  integer mode: Fvco = Fin * 2 * NF / NR
+*  Spread Spectrum mode: Fvco = Fin * SSN / NR
+*  SSN = 2^24 / (256 * ((ssdiv >> ssdepth) << ssdepth) + (ssmod << ssdepth))
+*/
+static unsigned long pll_clk_recalc_rate(struct clk_hw *hw,
+	unsigned long parent_rate)
+{
+	unsigned long fin = parent_rate;
+	struct clk_pll *clk = to_pllclk(hw);
+	u64 rate;
+	u32 regctrl0 = clkc_readl(clk->regofs + SIRFSOC_CLKC_MEMPLL_AB_CTRL0 -
+			SIRFSOC_CLKC_MEMPLL_AB_FREQ);
+	u32 regfreq = clkc_readl(clk->regofs);
+	u32 regssc = clkc_readl(clk->regofs + SIRFSOC_CLKC_MEMPLL_AB_SSC -
+			SIRFSOC_CLKC_MEMPLL_AB_FREQ);
+	u32 nr = (regfreq  >> 16 & (BIT(3) - 1)) + 1;
+	u32 nf = (regfreq & (BIT(9) - 1)) + 1;
+	u32 ssdiv = regssc >> 8 & (BIT(12) - 1);
+	u32 ssdepth = regssc >> 20 & (BIT(2) - 1);
+	u32 ssmod = regssc & (BIT(8) - 1);
+
+	if (regctrl0 & SIRFSOC_ABPLL_CTRL0_BYPASS)
+		return fin;
+
+	if (regctrl0 & SIRFSOC_ABPLL_CTRL0_SSEN) {
+		rate = fin;
+		rate *= 1 << 24;
+		do_div(rate, (256 * ((ssdiv >> ssdepth) << ssdepth)
+			+ (ssmod << ssdepth)));
+	} else {
+		rate = 2 * fin;
+		rate *= nf;
+		do_div(rate, nr);
+	}
+	return rate;
+}
+
+static const struct clk_ops ab_pll_ops = {
+	.recalc_rate = pll_clk_recalc_rate,
+};
+
+static const char * const pll_clk_parents[] = {
+	"xin",
+};
+
+static struct clk_init_data clk_cpupll_init = {
+	.name = "cpupll_vco",
+	.ops = &ab_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_pll clk_cpupll = {
+	.regofs = SIRFSOC_CLKC_CPUPLL_AB_FREQ,
+	.hw = {
+		.init = &clk_cpupll_init,
+	},
+};
+
+static struct clk_init_data clk_mempll_init = {
+	.name = "mempll_vco",
+	.ops = &ab_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_pll clk_mempll = {
+	.regofs = SIRFSOC_CLKC_MEMPLL_AB_FREQ,
+	.hw = {
+		.init = &clk_mempll_init,
+	},
+};
+
+static struct clk_init_data clk_sys0pll_init = {
+	.name = "sys0pll_vco",
+	.ops = &ab_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_pll clk_sys0pll = {
+	.regofs = SIRFSOC_CLKC_SYS0PLL_AB_FREQ,
+	.hw = {
+		.init = &clk_sys0pll_init,
+	},
+};
+
+static struct clk_init_data clk_sys1pll_init = {
+	.name = "sys1pll_vco",
+	.ops = &ab_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_pll clk_sys1pll = {
+	.regofs = SIRFSOC_CLKC_SYS1PLL_AB_FREQ,
+	.hw = {
+		.init = &clk_sys1pll_init,
+	},
+};
+
+static struct clk_init_data clk_sys2pll_init = {
+	.name = "sys2pll_vco",
+	.ops = &ab_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_pll clk_sys2pll = {
+	.regofs = SIRFSOC_CLKC_SYS2PLL_AB_FREQ,
+	.hw = {
+		.init = &clk_sys2pll_init,
+	},
+};
+
+static struct clk_init_data clk_sys3pll_init = {
+	.name = "sys3pll_vco",
+	.ops = &ab_pll_ops,
+	.parent_names = pll_clk_parents,
+	.num_parents = ARRAY_SIZE(pll_clk_parents),
+};
+
+static struct clk_pll clk_sys3pll = {
+	.regofs = SIRFSOC_CLKC_SYS3PLL_AB_FREQ,
+	.hw = {
+		.init = &clk_sys3pll_init,
+	},
+};
+
+/*
+ *  DTO in clkc, default enable double resolution mode
+ *  double resolution mode:fout = fin * finc / 2^29
+ *  normal mode:fout = fin * finc / 2^28
+ */
+static int dto_clk_is_enabled(struct clk_hw *hw)
+{
+	struct clk_dto *clk = to_dtoclk(hw);
+	int reg;
+
+	reg = clk->src_offset + SIRFSOC_CLKC_AUDIO_DTO_ENA - SIRFSOC_CLKC_AUDIO_DTO_SRC;
+
+	return !!(clkc_readl(reg) & BIT(0));
+}
+
+static int dto_clk_enable(struct clk_hw *hw)
+{
+	u32 val, reg;
+	struct clk_dto *clk = to_dtoclk(hw);
+
+	reg = clk->src_offset + SIRFSOC_CLKC_AUDIO_DTO_ENA - SIRFSOC_CLKC_AUDIO_DTO_SRC;
+
+	val = clkc_readl(reg) | BIT(0);
+	clkc_writel(val, reg);
+	return 0;
+}
+
+static void dto_clk_disable(struct clk_hw *hw)
+{
+	u32 val, reg;
+	struct clk_dto *clk = to_dtoclk(hw);
+
+	reg = clk->src_offset + SIRFSOC_CLKC_AUDIO_DTO_ENA - SIRFSOC_CLKC_AUDIO_DTO_SRC;
+
+	val = clkc_readl(reg) & ~BIT(0);
+	clkc_writel(val, reg);
+}
+
+static unsigned long dto_clk_recalc_rate(struct clk_hw *hw,
+	unsigned long parent_rate)
+{
+	u64 rate = parent_rate;
+	struct clk_dto *clk = to_dtoclk(hw);
+	u32 finc = clkc_readl(clk->inc_offset);
+	u32 droff = clkc_readl(clk->src_offset + SIRFSOC_CLKC_AUDIO_DTO_DROFF - SIRFSOC_CLKC_AUDIO_DTO_SRC);
+
+	rate *= finc;
+	if (droff & BIT(0))
+		/* Double resolution off */
+		do_div(rate, 1 << 28);
+	else
+		do_div(rate, 1 << 29);
+
+	return rate;
+}
+
+static long dto_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+	unsigned long *parent_rate)
+{
+	u64 dividend = rate * (1 << 29);
+
+	do_div(dividend, *parent_rate);
+	dividend *= *parent_rate;
+	do_div(dividend, 1 << 29);
+
+	return dividend;
+}
+
+static int dto_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+	unsigned long parent_rate)
+{
+	u64 dividend = rate * (1 << 29);
+	struct clk_dto *clk = to_dtoclk(hw);
+
+	do_div(dividend, parent_rate);
+	clkc_writel(0, clk->src_offset + SIRFSOC_CLKC_AUDIO_DTO_DROFF - SIRFSOC_CLKC_AUDIO_DTO_SRC);
+	clkc_writel(dividend, clk->inc_offset);
+
+	return 0;
+}
+
+static u8 dto_clk_get_parent(struct clk_hw *hw)
+{
+	struct clk_dto *clk = to_dtoclk(hw);
+
+	return clkc_readl(clk->src_offset);
+}
+
+/*
+ *   dto need CLK_SET_PARENT_GATE
+ */
+static int dto_clk_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct clk_dto *clk = to_dtoclk(hw);
+
+	clkc_writel(index, clk->src_offset);
+	return 0;
+}
+
+static const struct clk_ops dto_ops = {
+	.is_enabled = dto_clk_is_enabled,
+	.enable = dto_clk_enable,
+	.disable = dto_clk_disable,
+	.recalc_rate = dto_clk_recalc_rate,
+	.round_rate = dto_clk_round_rate,
+	.set_rate = dto_clk_set_rate,
+	.get_parent = dto_clk_get_parent,
+	.set_parent = dto_clk_set_parent,
+};
+
+/* dto parent clock as syspllvco/clk1 */
+static const char * const audiodto_clk_parents[] = {
+	"sys0pll_clk1",
+	"sys1pll_clk1",
+	"sys3pll_clk1",
+};
+
+static struct clk_init_data clk_audiodto_init = {
+	.name = "audio_dto",
+	.ops = &dto_ops,
+	.parent_names = audiodto_clk_parents,
+	.num_parents = ARRAY_SIZE(audiodto_clk_parents),
+};
+
+static struct clk_dto clk_audio_dto = {
+	.inc_offset = SIRFSOC_CLKC_AUDIO_DTO_INC,
+	.src_offset = SIRFSOC_CLKC_AUDIO_DTO_SRC,
+	.hw = {
+		.init = &clk_audiodto_init,
+	},
+};
+
+static const char * const disp0dto_clk_parents[] = {
+	"sys0pll_clk1",
+	"sys1pll_clk1",
+	"sys3pll_clk1",
+};
+
+static struct clk_init_data clk_disp0dto_init = {
+	.name = "disp0_dto",
+	.ops = &dto_ops,
+	.parent_names = disp0dto_clk_parents,
+	.num_parents = ARRAY_SIZE(disp0dto_clk_parents),
+};
+
+static struct clk_dto clk_disp0_dto = {
+	.inc_offset = SIRFSOC_CLKC_DISP0_DTO_INC,
+	.src_offset = SIRFSOC_CLKC_DISP0_DTO_SRC,
+	.hw = {
+		.init = &clk_disp0dto_init,
+	},
+};
+
+static const char * const disp1dto_clk_parents[] = {
+	"sys0pll_clk1",
+	"sys1pll_clk1",
+	"sys3pll_clk1",
+};
+
+static struct clk_init_data clk_disp1dto_init = {
+	.name = "disp1_dto",
+	.ops = &dto_ops,
+	.parent_names = disp1dto_clk_parents,
+	.num_parents = ARRAY_SIZE(disp1dto_clk_parents),
+};
+
+static struct clk_dto clk_disp1_dto = {
+	.inc_offset = SIRFSOC_CLKC_DISP1_DTO_INC,
+	.src_offset = SIRFSOC_CLKC_DISP1_DTO_SRC,
+	.hw = {
+		.init = &clk_disp1dto_init,
+	},
+};
+
+static struct atlas7_div_init_data divider_list[] __initdata = {
+	/* div_name, parent_name, gate_name, clk_flag, divider_flag, gate_flag, div_offset, shift, wdith, gate_offset, bit_enable, lock */
+	{ "sys0pll_qa1", "sys0pll_fixdiv", "sys0pll_a1", 0, 0, 0, SIRFSOC_CLKC_USBPHY_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_USBPHY_CLKDIV_ENA, 0, &usbphy_div_lock },
+	{ "sys1pll_qa1", "sys1pll_fixdiv", "sys1pll_a1", 0, 0, 0, SIRFSOC_CLKC_USBPHY_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_USBPHY_CLKDIV_ENA, 4, &usbphy_div_lock },
+	{ "sys2pll_qa1", "sys2pll_fixdiv", "sys2pll_a1", 0, 0, 0, SIRFSOC_CLKC_USBPHY_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_USBPHY_CLKDIV_ENA, 8, &usbphy_div_lock },
+	{ "sys3pll_qa1", "sys3pll_fixdiv", "sys3pll_a1", 0, 0, 0, SIRFSOC_CLKC_USBPHY_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_USBPHY_CLKDIV_ENA, 12, &usbphy_div_lock },
+	{ "sys0pll_qa2", "sys0pll_fixdiv", "sys0pll_a2", 0, 0, 0, SIRFSOC_CLKC_BTSS_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_BTSS_CLKDIV_ENA, 0, &btss_div_lock },
+	{ "sys1pll_qa2", "sys1pll_fixdiv", "sys1pll_a2", 0, 0, 0, SIRFSOC_CLKC_BTSS_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_BTSS_CLKDIV_ENA, 4, &btss_div_lock },
+	{ "sys2pll_qa2", "sys2pll_fixdiv", "sys2pll_a2", 0, 0, 0, SIRFSOC_CLKC_BTSS_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_BTSS_CLKDIV_ENA, 8, &btss_div_lock },
+	{ "sys3pll_qa2", "sys3pll_fixdiv", "sys3pll_a2", 0, 0, 0, SIRFSOC_CLKC_BTSS_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_BTSS_CLKDIV_ENA, 12, &btss_div_lock },
+	{ "sys0pll_qa3", "sys0pll_fixdiv", "sys0pll_a3", 0, 0, 0, SIRFSOC_CLKC_RGMII_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_RGMII_CLKDIV_ENA, 0, &rgmii_div_lock },
+	{ "sys1pll_qa3", "sys1pll_fixdiv", "sys1pll_a3", 0, 0, 0, SIRFSOC_CLKC_RGMII_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_RGMII_CLKDIV_ENA, 4, &rgmii_div_lock },
+	{ "sys2pll_qa3", "sys2pll_fixdiv", "sys2pll_a3", 0, 0, 0, SIRFSOC_CLKC_RGMII_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_RGMII_CLKDIV_ENA, 8, &rgmii_div_lock },
+	{ "sys3pll_qa3", "sys3pll_fixdiv", "sys3pll_a3", 0, 0, 0, SIRFSOC_CLKC_RGMII_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_RGMII_CLKDIV_ENA, 12, &rgmii_div_lock },
+	{ "sys0pll_qa4", "sys0pll_fixdiv", "sys0pll_a4", 0, 0, 0, SIRFSOC_CLKC_CPU_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_CPU_CLKDIV_ENA, 0, &cpu_div_lock },
+	{ "sys1pll_qa4", "sys1pll_fixdiv", "sys1pll_a4", 0, 0, CLK_IGNORE_UNUSED, SIRFSOC_CLKC_CPU_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_CPU_CLKDIV_ENA, 4, &cpu_div_lock },
+	{ "sys0pll_qa5", "sys0pll_fixdiv", "sys0pll_a5", 0, 0, 0, SIRFSOC_CLKC_SDPHY01_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_SDPHY01_CLKDIV_ENA, 0, &sdphy01_div_lock },
+	{ "sys1pll_qa5", "sys1pll_fixdiv", "sys1pll_a5", 0, 0, 0, SIRFSOC_CLKC_SDPHY01_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_SDPHY01_CLKDIV_ENA, 4, &sdphy01_div_lock },
+	{ "sys2pll_qa5", "sys2pll_fixdiv", "sys2pll_a5", 0, 0, 0, SIRFSOC_CLKC_SDPHY01_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_SDPHY01_CLKDIV_ENA, 8, &sdphy01_div_lock },
+	{ "sys3pll_qa5", "sys3pll_fixdiv", "sys3pll_a5", 0, 0, 0, SIRFSOC_CLKC_SDPHY01_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_SDPHY01_CLKDIV_ENA, 12, &sdphy01_div_lock },
+	{ "sys0pll_qa6", "sys0pll_fixdiv", "sys0pll_a6", 0, 0, 0, SIRFSOC_CLKC_SDPHY23_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_SDPHY23_CLKDIV_ENA, 0, &sdphy23_div_lock },
+	{ "sys1pll_qa6", "sys1pll_fixdiv", "sys1pll_a6", 0, 0, 0, SIRFSOC_CLKC_SDPHY23_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_SDPHY23_CLKDIV_ENA, 4, &sdphy23_div_lock },
+	{ "sys2pll_qa6", "sys2pll_fixdiv", "sys2pll_a6", 0, 0, 0, SIRFSOC_CLKC_SDPHY23_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_SDPHY23_CLKDIV_ENA, 8, &sdphy23_div_lock },
+	{ "sys3pll_qa6", "sys3pll_fixdiv", "sys3pll_a6", 0, 0, 0, SIRFSOC_CLKC_SDPHY23_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_SDPHY23_CLKDIV_ENA, 12, &sdphy23_div_lock },
+	{ "sys0pll_qa7", "sys0pll_fixdiv", "sys0pll_a7", 0, 0, 0, SIRFSOC_CLKC_SDPHY45_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_SDPHY45_CLKDIV_ENA, 0, &sdphy45_div_lock },
+	{ "sys1pll_qa7", "sys1pll_fixdiv", "sys1pll_a7", 0, 0, 0, SIRFSOC_CLKC_SDPHY45_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_SDPHY45_CLKDIV_ENA, 4, &sdphy45_div_lock },
+	{ "sys2pll_qa7", "sys2pll_fixdiv", "sys2pll_a7", 0, 0, 0, SIRFSOC_CLKC_SDPHY45_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_SDPHY45_CLKDIV_ENA, 8, &sdphy45_div_lock },
+	{ "sys3pll_qa7", "sys3pll_fixdiv", "sys3pll_a7", 0, 0, 0, SIRFSOC_CLKC_SDPHY45_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_SDPHY45_CLKDIV_ENA, 12, &sdphy45_div_lock },
+	{ "sys0pll_qa8", "sys0pll_fixdiv", "sys0pll_a8", 0, 0, 0, SIRFSOC_CLKC_SDPHY67_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_SDPHY67_CLKDIV_ENA, 0, &sdphy67_div_lock },
+	{ "sys1pll_qa8", "sys1pll_fixdiv", "sys1pll_a8", 0, 0, 0, SIRFSOC_CLKC_SDPHY67_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_SDPHY67_CLKDIV_ENA, 4, &sdphy67_div_lock },
+	{ "sys2pll_qa8", "sys2pll_fixdiv", "sys2pll_a8", 0, 0, 0, SIRFSOC_CLKC_SDPHY67_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_SDPHY67_CLKDIV_ENA, 8, &sdphy67_div_lock },
+	{ "sys3pll_qa8", "sys3pll_fixdiv", "sys3pll_a8", 0, 0, 0, SIRFSOC_CLKC_SDPHY67_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_SDPHY67_CLKDIV_ENA, 12, &sdphy67_div_lock },
+	{ "sys0pll_qa9", "sys0pll_fixdiv", "sys0pll_a9", 0, 0, 0, SIRFSOC_CLKC_CAN_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_CAN_CLKDIV_ENA, 0, &can_div_lock },
+	{ "sys1pll_qa9", "sys1pll_fixdiv", "sys1pll_a9", 0, 0, 0, SIRFSOC_CLKC_CAN_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_CAN_CLKDIV_ENA, 4, &can_div_lock },
+	{ "sys2pll_qa9", "sys2pll_fixdiv", "sys2pll_a9", 0, 0, 0, SIRFSOC_CLKC_CAN_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_CAN_CLKDIV_ENA, 8, &can_div_lock },
+	{ "sys3pll_qa9", "sys3pll_fixdiv", "sys3pll_a9", 0, 0, 0, SIRFSOC_CLKC_CAN_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_CAN_CLKDIV_ENA, 12, &can_div_lock },
+	{ "sys0pll_qa10", "sys0pll_fixdiv", "sys0pll_a10", 0, 0, 0, SIRFSOC_CLKC_DEINT_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_DEINT_CLKDIV_ENA, 0, &deint_div_lock },
+	{ "sys1pll_qa10", "sys1pll_fixdiv", "sys1pll_a10", 0, 0, 0, SIRFSOC_CLKC_DEINT_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_DEINT_CLKDIV_ENA, 4, &deint_div_lock },
+	{ "sys2pll_qa10", "sys2pll_fixdiv", "sys2pll_a10", 0, 0, 0, SIRFSOC_CLKC_DEINT_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_DEINT_CLKDIV_ENA, 8, &deint_div_lock },
+	{ "sys3pll_qa10", "sys3pll_fixdiv", "sys3pll_a10", 0, 0, 0, SIRFSOC_CLKC_DEINT_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_DEINT_CLKDIV_ENA, 12, &deint_div_lock },
+	{ "sys0pll_qa11", "sys0pll_fixdiv", "sys0pll_a11", 0, 0, 0, SIRFSOC_CLKC_NAND_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_NAND_CLKDIV_ENA, 0, &nand_div_lock },
+	{ "sys1pll_qa11", "sys1pll_fixdiv", "sys1pll_a11", 0, 0, 0, SIRFSOC_CLKC_NAND_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_NAND_CLKDIV_ENA, 4, &nand_div_lock },
+	{ "sys2pll_qa11", "sys2pll_fixdiv", "sys2pll_a11", 0, 0, 0, SIRFSOC_CLKC_NAND_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_NAND_CLKDIV_ENA, 8, &nand_div_lock },
+	{ "sys3pll_qa11", "sys3pll_fixdiv", "sys3pll_a11", 0, 0, 0, SIRFSOC_CLKC_NAND_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_NAND_CLKDIV_ENA, 12, &nand_div_lock },
+	{ "sys0pll_qa12", "sys0pll_fixdiv", "sys0pll_a12", 0, 0, 0, SIRFSOC_CLKC_DISP0_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_DISP0_CLKDIV_ENA, 0, &disp0_div_lock },
+	{ "sys1pll_qa12", "sys1pll_fixdiv", "sys1pll_a12", 0, 0, 0, SIRFSOC_CLKC_DISP0_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_DISP0_CLKDIV_ENA, 4, &disp0_div_lock },
+	{ "sys2pll_qa12", "sys2pll_fixdiv", "sys2pll_a12", 0, 0, 0, SIRFSOC_CLKC_DISP0_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_DISP0_CLKDIV_ENA, 8, &disp0_div_lock },
+	{ "sys3pll_qa12", "sys3pll_fixdiv", "sys3pll_a12", 0, 0, 0, SIRFSOC_CLKC_DISP0_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_DISP0_CLKDIV_ENA, 12, &disp0_div_lock },
+	{ "sys0pll_qa13", "sys0pll_fixdiv", "sys0pll_a13", 0, 0, 0, SIRFSOC_CLKC_DISP1_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_DISP1_CLKDIV_ENA, 0, &disp1_div_lock },
+	{ "sys1pll_qa13", "sys1pll_fixdiv", "sys1pll_a13", 0, 0, 0, SIRFSOC_CLKC_DISP1_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_DISP1_CLKDIV_ENA, 4, &disp1_div_lock },
+	{ "sys2pll_qa13", "sys2pll_fixdiv", "sys2pll_a13", 0, 0, 0, SIRFSOC_CLKC_DISP1_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_DISP1_CLKDIV_ENA, 8, &disp1_div_lock },
+	{ "sys3pll_qa13", "sys3pll_fixdiv", "sys3pll_a13", 0, 0, 0, SIRFSOC_CLKC_DISP1_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_DISP1_CLKDIV_ENA, 12, &disp1_div_lock },
+	{ "sys0pll_qa14", "sys0pll_fixdiv", "sys0pll_a14", 0, 0, 0, SIRFSOC_CLKC_GPU_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_GPU_CLKDIV_ENA, 0, &gpu_div_lock },
+	{ "sys1pll_qa14", "sys1pll_fixdiv", "sys1pll_a14", 0, 0, 0, SIRFSOC_CLKC_GPU_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_GPU_CLKDIV_ENA, 4, &gpu_div_lock },
+	{ "sys2pll_qa14", "sys2pll_fixdiv", "sys2pll_a14", 0, 0, 0, SIRFSOC_CLKC_GPU_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_GPU_CLKDIV_ENA, 8, &gpu_div_lock },
+	{ "sys3pll_qa14", "sys3pll_fixdiv", "sys3pll_a14", 0, 0, 0, SIRFSOC_CLKC_GPU_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_GPU_CLKDIV_ENA, 12, &gpu_div_lock },
+	{ "sys0pll_qa15", "sys0pll_fixdiv", "sys0pll_a15", 0, 0, 0, SIRFSOC_CLKC_GNSS_CLKDIV_CFG, 0, 6, SIRFSOC_CLKC_GNSS_CLKDIV_ENA, 0, &gnss_div_lock },
+	{ "sys1pll_qa15", "sys1pll_fixdiv", "sys1pll_a15", 0, 0, 0, SIRFSOC_CLKC_GNSS_CLKDIV_CFG, 8, 6, SIRFSOC_CLKC_GNSS_CLKDIV_ENA, 4, &gnss_div_lock },
+	{ "sys2pll_qa15", "sys2pll_fixdiv", "sys2pll_a15", 0, 0, 0, SIRFSOC_CLKC_GNSS_CLKDIV_CFG, 16, 6, SIRFSOC_CLKC_GNSS_CLKDIV_ENA, 8, &gnss_div_lock },
+	{ "sys3pll_qa15", "sys3pll_fixdiv", "sys3pll_a15", 0, 0, 0, SIRFSOC_CLKC_GNSS_CLKDIV_CFG, 24, 6, SIRFSOC_CLKC_GNSS_CLKDIV_ENA, 12, &gnss_div_lock },
+	{ "sys1pll_qa18", "sys1pll_fixdiv", "sys1pll_a18", 0, 0, 0, SIRFSOC_CLKC_SHARED_DIVIDER_CFG0, 24, 6, SIRFSOC_CLKC_SHARED_DIVIDER_ENA, 12, &share_div_lock },
+	{ "sys1pll_qa19", "sys1pll_fixdiv", "sys1pll_a19", 0, 0, CLK_IGNORE_UNUSED, SIRFSOC_CLKC_SHARED_DIVIDER_CFG0, 16, 6, SIRFSOC_CLKC_SHARED_DIVIDER_ENA, 8, &share_div_lock },
+	{ "sys1pll_qa20", "sys1pll_fixdiv", "sys1pll_a20", 0, 0, 0, SIRFSOC_CLKC_SHARED_DIVIDER_CFG0, 8, 6, SIRFSOC_CLKC_SHARED_DIVIDER_ENA, 4, &share_div_lock },
+	{ "sys2pll_qa20", "sys2pll_fixdiv", "sys2pll_a20", 0, 0, 0, SIRFSOC_CLKC_SHARED_DIVIDER_CFG0, 0, 6, SIRFSOC_CLKC_SHARED_DIVIDER_ENA, 0, &share_div_lock },
+	{ "sys1pll_qa17", "sys1pll_fixdiv", "sys1pll_a17", 0, 0, CLK_IGNORE_UNUSED, SIRFSOC_CLKC_SHARED_DIVIDER_CFG1, 8, 6, SIRFSOC_CLKC_SHARED_DIVIDER_ENA, 20, &share_div_lock },
+	{ "sys0pll_qa20", "sys0pll_fixdiv", "sys0pll_a20", 0, 0, 0, SIRFSOC_CLKC_SHARED_DIVIDER_CFG1, 0, 6, SIRFSOC_CLKC_SHARED_DIVIDER_ENA, 16, &share_div_lock },
+};
+
+static const char * const i2s_clk_parents[] = {
+	"xin",
+	"xinw",
+	"audio_dto",
+	/* "pwm_i2s01" */
+};
+
+static const char * const usbphy_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a1",
+	"sys1pll_a1",
+	"sys2pll_a1",
+	"sys3pll_a1",
+};
+
+static const char * const btss_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a2",
+	"sys1pll_a2",
+	"sys2pll_a2",
+	"sys3pll_a2",
+};
+
+static const char * const rgmii_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a3",
+	"sys1pll_a3",
+	"sys2pll_a3",
+	"sys3pll_a3",
+};
+
+static const char * const cpu_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a4",
+	"sys1pll_a4",
+	"cpupll_clk1",
+};
+
+static const char * const sdphy01_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a5",
+	"sys1pll_a5",
+	"sys2pll_a5",
+	"sys3pll_a5",
+};
+
+static const char * const sdphy23_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a6",
+	"sys1pll_a6",
+	"sys2pll_a6",
+	"sys3pll_a6",
+};
+
+static const char * const sdphy45_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a7",
+	"sys1pll_a7",
+	"sys2pll_a7",
+	"sys3pll_a7",
+};
+
+static const char * const sdphy67_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a8",
+	"sys1pll_a8",
+	"sys2pll_a8",
+	"sys3pll_a8",
+};
+
+static const char * const can_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a9",
+	"sys1pll_a9",
+	"sys2pll_a9",
+	"sys3pll_a9",
+};
+
+static const char * const deint_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a10",
+	"sys1pll_a10",
+	"sys2pll_a10",
+	"sys3pll_a10",
+};
+
+static const char * const nand_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a11",
+	"sys1pll_a11",
+	"sys2pll_a11",
+	"sys3pll_a11",
+};
+
+static const char * const disp0_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a12",
+	"sys1pll_a12",
+	"sys2pll_a12",
+	"sys3pll_a12",
+	"disp0_dto",
+};
+
+static const char * const disp1_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a13",
+	"sys1pll_a13",
+	"sys2pll_a13",
+	"sys3pll_a13",
+	"disp1_dto",
+};
+
+static const char * const gpu_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a14",
+	"sys1pll_a14",
+	"sys2pll_a14",
+	"sys3pll_a14",
+};
+
+static const char * const gnss_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys0pll_a15",
+	"sys1pll_a15",
+	"sys2pll_a15",
+	"sys3pll_a15",
+};
+
+static const char * const sys_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const io_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const g2d_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const jpenc_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const vdec_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const gmac_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const usb_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const kas_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const sec_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const sdr_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const vip_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const nocd_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const nocr_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static const char * const tpiu_clk_parents[] = {
+	"xin",
+	"xinw",
+	"sys2pll_a20",
+	"sys1pll_a20",
+	"sys1pll_a19",
+	"sys1pll_a18",
+	"sys0pll_a20",
+	"sys1pll_a17",
+};
+
+static struct atlas7_mux_init_data mux_list[] __initdata = {
+	/* mux_name, parent_names, parent_num, flags, mux_flags, mux_offset, shift, width */
+	{ "i2s_mux", i2s_clk_parents, ARRAY_SIZE(i2s_clk_parents), 0, 0, SIRFSOC_CLKC_I2S_CLK_SEL, 0, 2 },
+	{ "usbphy_mux", usbphy_clk_parents, ARRAY_SIZE(usbphy_clk_parents), 0, 0, SIRFSOC_CLKC_I2S_CLK_SEL, 0, 3 },
+	{ "btss_mux", btss_clk_parents, ARRAY_SIZE(btss_clk_parents), 0, 0, SIRFSOC_CLKC_BTSS_CLK_SEL, 0, 3 },
+	{ "rgmii_mux", rgmii_clk_parents, ARRAY_SIZE(rgmii_clk_parents), 0, 0, SIRFSOC_CLKC_RGMII_CLK_SEL, 0, 3 },
+	{ "cpu_mux", cpu_clk_parents, ARRAY_SIZE(cpu_clk_parents), 0, 0, SIRFSOC_CLKC_CPU_CLK_SEL, 0, 3 },
+	{ "sdphy01_mux", sdphy01_clk_parents, ARRAY_SIZE(sdphy01_clk_parents), 0, 0, SIRFSOC_CLKC_SDPHY01_CLK_SEL, 0, 3 },
+	{ "sdphy23_mux", sdphy23_clk_parents, ARRAY_SIZE(sdphy23_clk_parents), 0, 0, SIRFSOC_CLKC_SDPHY23_CLK_SEL, 0, 3 },
+	{ "sdphy45_mux", sdphy45_clk_parents, ARRAY_SIZE(sdphy45_clk_parents), 0, 0, SIRFSOC_CLKC_SDPHY45_CLK_SEL, 0, 3 },
+	{ "sdphy67_mux", sdphy67_clk_parents, ARRAY_SIZE(sdphy67_clk_parents), 0, 0, SIRFSOC_CLKC_SDPHY67_CLK_SEL, 0, 3 },
+	{ "can_mux", can_clk_parents, ARRAY_SIZE(can_clk_parents), 0, 0, SIRFSOC_CLKC_CAN_CLK_SEL, 0, 3 },
+	{ "deint_mux", deint_clk_parents, ARRAY_SIZE(deint_clk_parents), 0, 0, SIRFSOC_CLKC_DEINT_CLK_SEL, 0, 3 },
+	{ "nand_mux", nand_clk_parents, ARRAY_SIZE(nand_clk_parents), 0, 0, SIRFSOC_CLKC_NAND_CLK_SEL, 0, 3 },
+	{ "disp0_mux", disp0_clk_parents, ARRAY_SIZE(disp0_clk_parents), 0, 0, SIRFSOC_CLKC_DISP0_CLK_SEL, 0, 3 },
+	{ "disp1_mux", disp1_clk_parents, ARRAY_SIZE(disp1_clk_parents), 0, 0, SIRFSOC_CLKC_DISP1_CLK_SEL, 0, 3 },
+	{ "gpu_mux", gpu_clk_parents, ARRAY_SIZE(gpu_clk_parents), 0, 0, SIRFSOC_CLKC_GPU_CLK_SEL, 0, 3 },
+	{ "gnss_mux", gnss_clk_parents, ARRAY_SIZE(gnss_clk_parents), 0, 0, SIRFSOC_CLKC_GNSS_CLK_SEL, 0, 3 },
+	{ "sys_mux", sys_clk_parents, ARRAY_SIZE(sys_clk_parents), 0, 0, SIRFSOC_CLKC_SYS_CLK_SEL, 0, 3 },
+	{ "io_mux", io_clk_parents, ARRAY_SIZE(io_clk_parents), 0, 0, SIRFSOC_CLKC_IO_CLK_SEL, 0, 3 },
+	{ "g2d_mux", g2d_clk_parents, ARRAY_SIZE(g2d_clk_parents), 0, 0, SIRFSOC_CLKC_G2D_CLK_SEL, 0, 3 },
+	{ "jpenc_mux", jpenc_clk_parents, ARRAY_SIZE(jpenc_clk_parents), 0, 0, SIRFSOC_CLKC_JPENC_CLK_SEL, 0, 3 },
+	{ "vdec_mux", vdec_clk_parents, ARRAY_SIZE(vdec_clk_parents), 0, 0, SIRFSOC_CLKC_VDEC_CLK_SEL, 0, 3 },
+	{ "gmac_mux", gmac_clk_parents, ARRAY_SIZE(gmac_clk_parents), 0, 0, SIRFSOC_CLKC_GMAC_CLK_SEL, 0, 3 },
+	{ "usb_mux", usb_clk_parents, ARRAY_SIZE(usb_clk_parents), 0, 0, SIRFSOC_CLKC_USB_CLK_SEL, 0, 3 },
+	{ "kas_mux", kas_clk_parents, ARRAY_SIZE(kas_clk_parents), 0, 0, SIRFSOC_CLKC_KAS_CLK_SEL, 0, 3 },
+	{ "sec_mux", sec_clk_parents, ARRAY_SIZE(sec_clk_parents), 0, 0, SIRFSOC_CLKC_SEC_CLK_SEL, 0, 3 },
+	{ "sdr_mux", sdr_clk_parents, ARRAY_SIZE(sdr_clk_parents), 0, 0, SIRFSOC_CLKC_SDR_CLK_SEL, 0, 3 },
+	{ "vip_mux", vip_clk_parents, ARRAY_SIZE(vip_clk_parents), 0, 0, SIRFSOC_CLKC_VIP_CLK_SEL, 0, 3 },
+	{ "nocd_mux", nocd_clk_parents, ARRAY_SIZE(nocd_clk_parents), 0, 0, SIRFSOC_CLKC_NOCD_CLK_SEL, 0, 3 },
+	{ "nocr_mux", nocr_clk_parents, ARRAY_SIZE(nocr_clk_parents), 0, 0, SIRFSOC_CLKC_NOCR_CLK_SEL, 0, 3 },
+	{ "tpiu_mux", tpiu_clk_parents, ARRAY_SIZE(tpiu_clk_parents), 0, 0, SIRFSOC_CLKC_TPIU_CLK_SEL, 0, 3 },
+};
+
+	/* new unit should add start from the tail of list */
+static struct atlas7_unit_init_data unit_list[] __initdata = {
+	/* unit_name, parent_name, flags, regofs, bit, lock */
+	{ 0, "audmscm_kas", "kas_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 0, &root0_gate_lock },
+	{ 1, "gnssm_gnss", "gnss_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 1, &root0_gate_lock },
+	{ 2, "gpum_gpu", "gpu_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 2, &root0_gate_lock },
+	{ 3, "mediam_g2d", "g2d_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 3, &root0_gate_lock },
+	{ 4, "mediam_jpenc", "jpenc_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 4, &root0_gate_lock },
+	{ 5, "vdifm_disp0", "disp0_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 5, &root0_gate_lock },
+	{ 6, "vdifm_disp1", "disp1_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 6, &root0_gate_lock },
+	{ 7, "audmscm_i2s", "i2s_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 8, &root0_gate_lock },
+	{ 8, "audmscm_io", "io_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 11, &root0_gate_lock },
+	{ 9, "vdifm_io", "io_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 12, &root0_gate_lock },
+	{ 10, "gnssm_io", "io_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 13, &root0_gate_lock },
+	{ 11, "mediam_io", "io_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 14, &root0_gate_lock },
+	{ 12, "btm_io", "io_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 17, &root0_gate_lock },
+	{ 13, "mediam_sdphy01", "sdphy01_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 18, &root0_gate_lock },
+	{ 14, "vdifm_sdphy23", "sdphy23_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 19, &root0_gate_lock },
+	{ 15, "vdifm_sdphy45", "sdphy45_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 20, &root0_gate_lock },
+	{ 16, "vdifm_sdphy67", "sdphy67_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 21, &root0_gate_lock },
+	{ 17, "audmscm_xin", "xin", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 22, &root0_gate_lock },
+	{ 18, "mediam_nand", "nand_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 27, &root0_gate_lock },
+	{ 19, "gnssm_sec", "sec_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 28, &root0_gate_lock },
+	{ 20, "cpum_cpu", "cpu_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 29, &root0_gate_lock },
+	{ 21, "gnssm_xin", "xin", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 30, &root0_gate_lock },
+	{ 22, "vdifm_vip", "vip_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN0_SET, 31, &root0_gate_lock },
+	{ 23, "btm_btss", "btss_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 0, &root1_gate_lock },
+	{ 24, "mediam_usbphy", "usbphy_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 1, &root1_gate_lock },
+	{ 25, "rtcm_kas", "kas_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 2, &root1_gate_lock },
+	{ 26, "audmscm_nocd", "nocd_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 3, &root1_gate_lock },
+	{ 27, "vdifm_nocd", "nocd_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 4, &root1_gate_lock },
+	{ 28, "gnssm_nocd", "nocd_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 5, &root1_gate_lock },
+	{ 29, "mediam_nocd", "nocd_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 6, &root1_gate_lock },
+	{ 30, "cpum_nocd", "nocd_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 8, &root1_gate_lock },
+	{ 31, "gpum_nocd", "nocd_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 9, &root1_gate_lock },
+	{ 32, "audmscm_nocr", "nocr_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 11, &root1_gate_lock },
+	{ 33, "vdifm_nocr", "nocr_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 12, &root1_gate_lock },
+	{ 34, "gnssm_nocr", "nocr_mux", CLK_IGNORE_UNUSED, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 13, &root1_gate_lock },
+	{ 35, "mediam_nocr", "nocr_mux", CLK_IGNORE_UNUSED, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 14, &root1_gate_lock },
+	{ 36, "ddrm_nocr", "nocr_mux", CLK_IGNORE_UNUSED, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 15, &root1_gate_lock },
+	{ 37, "cpum_tpiu", "tpiu_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 16, &root1_gate_lock },
+	{ 38, "gpum_nocr", "nocr_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 17, &root1_gate_lock },
+	{ 39, "gnssm_rgmii", "rgmii_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 20, &root1_gate_lock },
+	{ 40, "mediam_vdec", "vdec_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 21, &root1_gate_lock },
+	{ 41, "gpum_sdr", "sdr_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 22, &root1_gate_lock },
+	{ 42, "vdifm_deint", "deint_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 23, &root1_gate_lock },
+	{ 43, "gnssm_can", "can_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 26, &root1_gate_lock },
+	{ 44, "mediam_usb", "usb_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 28, &root1_gate_lock },
+	{ 45, "gnssm_gmac", "gmac_mux", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 29, &root1_gate_lock },
+	{ 46, "cvd_io", "audmscm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 0, &leaf1_gate_lock },
+	{ 47, "timer_io", "audmscm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 1, &leaf1_gate_lock },
+	{ 48, "pulse_io", "audmscm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 2, &leaf1_gate_lock },
+	{ 49, "tsc_io", "audmscm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 3, &leaf1_gate_lock },
+	{ 50, "tsc_xin", "audmscm_xin", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 21, &leaf1_gate_lock },
+	{ 51, "ioctop_io", "audmscm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 4, &leaf1_gate_lock },
+	{ 52, "rsc_io", "audmscm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 5, &leaf1_gate_lock },
+	{ 53, "dvm_io", "audmscm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 6, &leaf1_gate_lock },
+	{ 54, "lvds_xin", "audmscm_xin", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 7, &leaf1_gate_lock },
+	{ 55, "kas_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 8, &leaf1_gate_lock },
+	{ 56, "ac97_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 9, &leaf1_gate_lock },
+	{ 57, "usp0_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 10, &leaf1_gate_lock },
+	{ 58, "usp1_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 11, &leaf1_gate_lock },
+	{ 59, "usp2_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 12, &leaf1_gate_lock },
+	{ 60, "dmac2_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 13, &leaf1_gate_lock },
+	{ 61, "dmac3_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 14, &leaf1_gate_lock },
+	{ 62, "audioif_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 15, &leaf1_gate_lock },
+	{ 63, "i2s1_kas", "audmscm_kas", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 17, &leaf1_gate_lock },
+	{ 64, "thaudmscm_io", "audmscm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 22, &leaf1_gate_lock },
+	{ 65, "analogtest_xin", "audmscm_xin", 0, SIRFSOC_CLKC_LEAF_CLK_EN1_SET, 23, &leaf1_gate_lock },
+	{ 66, "sys2pci_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 0, &leaf2_gate_lock },
+	{ 67, "pciarb_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 1, &leaf2_gate_lock },
+	{ 68, "pcicopy_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 2, &leaf2_gate_lock },
+	{ 69, "rom_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 3, &leaf2_gate_lock },
+	{ 70, "sdio23_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 4, &leaf2_gate_lock },
+	{ 71, "sdio45_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 5, &leaf2_gate_lock },
+	{ 72, "sdio67_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 6, &leaf2_gate_lock },
+	{ 73, "vip1_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 7, &leaf2_gate_lock },
+	{ 74, "vip1_vip", "vdifm_vip", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 16, &leaf2_gate_lock },
+	{ 75, "sdio23_sdphy23", "vdifm_sdphy23", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 8, &leaf2_gate_lock },
+	{ 76, "sdio45_sdphy45", "vdifm_sdphy45", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 9, &leaf2_gate_lock },
+	{ 77, "sdio67_sdphy67", "vdifm_sdphy67", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 10, &leaf2_gate_lock },
+	{ 78, "vpp0_disp0", "vdifm_disp0", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 11, &leaf2_gate_lock },
+	{ 79, "lcd0_disp0", "vdifm_disp0", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 12, &leaf2_gate_lock },
+	{ 80, "vpp1_disp1", "vdifm_disp1", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 13, &leaf2_gate_lock },
+	{ 81, "lcd1_disp1", "vdifm_disp1", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 14, &leaf2_gate_lock },
+	{ 82, "dcu_deint", "vdifm_deint", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 15, &leaf2_gate_lock },
+	{ 83, "vdifm_dapa_r_nocr", "vdifm_nocr", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 17, &leaf2_gate_lock },
+	{ 84, "gpio1_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 18, &leaf2_gate_lock },
+	{ 85, "thvdifm_io", "vdifm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN2_SET, 19, &leaf2_gate_lock },
+	{ 86, "gmac_rgmii", "gnssm_rgmii", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 0, &leaf3_gate_lock },
+	{ 87, "gmac_gmac", "gnssm_gmac", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 1, &leaf3_gate_lock },
+	{ 88, "uart1_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 2, &leaf3_gate_lock },
+	{ 89, "dmac0_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 3, &leaf3_gate_lock },
+	{ 90, "uart0_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 4, &leaf3_gate_lock },
+	{ 91, "uart2_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 5, &leaf3_gate_lock },
+	{ 92, "uart3_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 6, &leaf3_gate_lock },
+	{ 93, "uart4_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 7, &leaf3_gate_lock },
+	{ 94, "uart5_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 8, &leaf3_gate_lock },
+	{ 95, "spi1_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 9, &leaf3_gate_lock },
+	{ 96, "gnss_gnss", "gnssm_gnss", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 10, &leaf3_gate_lock },
+	{ 97, "canbus1_can", "gnssm_can", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 12, &leaf3_gate_lock },
+	{ 98, "ccsec_sec", "gnssm_sec", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 15, &leaf3_gate_lock },
+	{ 99,  "ccpub_sec", "gnssm_sec", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 16, &leaf3_gate_lock },
+	{ 100, "gnssm_dapa_r_nocr", "gnssm_nocr", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 13, &leaf3_gate_lock },
+	{ 101, "thgnssm_io", "gnssm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN3_SET, 14, &leaf3_gate_lock },
+	{ 102, "media_vdec", "mediam_vdec", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 0, &leaf4_gate_lock },
+	{ 103, "media_jpenc", "mediam_jpenc", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 1, &leaf4_gate_lock },
+	{ 104, "g2d_g2d", "mediam_g2d", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 2, &leaf4_gate_lock },
+	{ 105, "i2c0_io", "mediam_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 3, &leaf4_gate_lock },
+	{ 106, "i2c1_io", "mediam_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 4, &leaf4_gate_lock },
+	{ 107, "gpio0_io", "mediam_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 5, &leaf4_gate_lock },
+	{ 108, "nand_io", "mediam_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 6, &leaf4_gate_lock },
+	{ 109, "sdio01_io", "mediam_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 7, &leaf4_gate_lock },
+	{ 110, "sys2pci2_io", "mediam_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 8, &leaf4_gate_lock },
+	{ 111, "sdio01_sdphy01", "mediam_sdphy01", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 9, &leaf4_gate_lock },
+	{ 112, "nand_nand", "mediam_nand", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 10, &leaf4_gate_lock },
+	{ 113, "usb0_usb", "mediam_usb", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 11, &leaf4_gate_lock },
+	{ 114, "usb1_usb", "mediam_usb", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 12, &leaf4_gate_lock },
+	{ 115, "usbphy0_usbphy", "mediam_usbphy", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 13, &leaf4_gate_lock },
+	{ 116, "usbphy1_usbphy", "mediam_usbphy", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 14, &leaf4_gate_lock },
+	{ 117, "thmediam_io", "mediam_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN4_SET, 15, &leaf4_gate_lock },
+	{ 118, "memc_mem", "mempll_clk1", CLK_IGNORE_UNUSED, SIRFSOC_CLKC_LEAF_CLK_EN5_SET, 0, &leaf5_gate_lock },
+	{ 119, "dapa_mem", "mempll_clk1", 0, SIRFSOC_CLKC_LEAF_CLK_EN5_SET, 1, &leaf5_gate_lock },
+	{ 120, "nocddrm_nocr", "ddrm_nocr", 0, SIRFSOC_CLKC_LEAF_CLK_EN5_SET, 2, &leaf5_gate_lock },
+	{ 121, "thddrm_nocr", "ddrm_nocr", 0, SIRFSOC_CLKC_LEAF_CLK_EN5_SET, 3, &leaf5_gate_lock },
+	{ 122, "spram1_cpudiv2", "cpum_cpu", 0, SIRFSOC_CLKC_LEAF_CLK_EN6_SET, 0, &leaf6_gate_lock },
+	{ 123, "spram2_cpudiv2", "cpum_cpu", 0, SIRFSOC_CLKC_LEAF_CLK_EN6_SET, 1, &leaf6_gate_lock },
+	{ 124, "coresight_cpudiv2", "cpum_cpu", 0, SIRFSOC_CLKC_LEAF_CLK_EN6_SET, 2, &leaf6_gate_lock },
+	{ 125, "thcpum_cpudiv4", "cpum_cpu", 0, SIRFSOC_CLKC_LEAF_CLK_EN6_SET, 3, &leaf6_gate_lock },
+	{ 126, "graphic_gpu", "gpum_gpu", 0, SIRFSOC_CLKC_LEAF_CLK_EN7_SET, 0, &leaf7_gate_lock },
+	{ 127, "vss_sdr", "gpum_sdr", 0, SIRFSOC_CLKC_LEAF_CLK_EN7_SET, 1, &leaf7_gate_lock },
+	{ 128, "thgpum_nocr", "gpum_nocr", 0, SIRFSOC_CLKC_LEAF_CLK_EN7_SET, 2, &leaf7_gate_lock },
+	{ 129, "a7ca_btss", "btm_btss", 0, SIRFSOC_CLKC_LEAF_CLK_EN8_SET, 1, &leaf8_gate_lock },
+	{ 130, "dmac4_io", "btm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN8_SET, 2, &leaf8_gate_lock },
+	{ 131, "uart6_io", "btm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN8_SET, 3, &leaf8_gate_lock },
+	{ 132, "usp3_io", "btm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN8_SET, 4, &leaf8_gate_lock },
+	{ 133, "a7ca_io", "btm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN8_SET, 5, &leaf8_gate_lock },
+	{ 134, "noc_btm_io", "btm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN8_SET, 6, &leaf8_gate_lock },
+	{ 135, "thbtm_io", "btm_io", 0, SIRFSOC_CLKC_LEAF_CLK_EN8_SET, 7, &leaf8_gate_lock },
+	{ 136, "btslow", "xinw_fixdiv_btslow", 0, SIRFSOC_CLKC_ROOT_CLK_EN1_SET, 25, &root1_gate_lock },
+	{ 137, "a7ca_btslow", "btslow", 0, SIRFSOC_CLKC_LEAF_CLK_EN8_SET, 0, &leaf8_gate_lock },
+};
+
+static struct clk *atlas7_clks[ARRAY_SIZE(unit_list)];
+
+static int unit_clk_is_enabled(struct clk_hw *hw)
+{
+	struct clk_unit *clk = to_unitclk(hw);
+	u32 reg;
+
+	reg = clk->regofs + SIRFSOC_CLKC_ROOT_CLK_EN0_STAT - SIRFSOC_CLKC_ROOT_CLK_EN0_SET;
+
+	return !!(clkc_readl(reg) & BIT(clk->bit));
+}
+
+static int unit_clk_enable(struct clk_hw *hw)
+{
+	u32 reg;
+	struct clk_unit *clk = to_unitclk(hw);
+	unsigned long flags;
+
+	reg = clk->regofs;
+
+	spin_lock_irqsave(clk->lock, flags);
+	clkc_writel(BIT(clk->bit), reg);
+	spin_unlock_irqrestore(clk->lock, flags);
+	return 0;
+}
+
+static void unit_clk_disable(struct clk_hw *hw)
+{
+	u32  reg;
+	struct clk_unit *clk = to_unitclk(hw);
+	unsigned long flags;
+
+	reg = clk->regofs + SIRFSOC_CLKC_ROOT_CLK_EN0_CLR - SIRFSOC_CLKC_ROOT_CLK_EN0_SET;
+
+	spin_lock_irqsave(clk->lock, flags);
+	clkc_writel(BIT(clk->bit), reg);
+	spin_unlock_irqrestore(clk->lock, flags);
+}
+
+static const struct clk_ops unit_clk_ops = {
+	.is_enabled = unit_clk_is_enabled,
+	.enable = unit_clk_enable,
+	.disable = unit_clk_disable,
+};
+
+static struct clk * __init
+atlas7_unit_clk_register(struct device *dev, const char *name,
+		 const char * const parent_name, unsigned long flags,
+		 u32 regofs, u8 bit, spinlock_t *lock)
+{
+	struct clk *clk;
+	struct clk_unit *unit;
+	struct clk_init_data init;
+
+	unit = kzalloc(sizeof(*unit), GFP_KERNEL);
+	if (!unit)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+	init.ops = &unit_clk_ops;
+	init.flags = flags;
+
+	unit->hw.init = &init;
+	unit->regofs = regofs;
+	unit->bit = bit;
+	unit->lock = lock;
+
+	clk = clk_register(dev, &unit->hw);
+	if (IS_ERR(clk))
+		kfree(unit);
+
+	return clk;
+}
+
+static struct atlas7_reset_desc atlas7_reset_unit[] = {
+	{ "PWM", 0x0244, 0, 0x0320, 0, &leaf0_gate_lock }, /* 0-5 */
+	{ "THCGUM", 0x0244, 3, 0x0320, 1, &leaf0_gate_lock },
+	{ "CVD", 0x04A0, 0, 0x032C, 0, &leaf1_gate_lock },
+	{ "TIMER", 0x04A0, 1, 0x032C, 1, &leaf1_gate_lock },
+	{ "PULSEC", 0x04A0, 2, 0x032C, 2, &leaf1_gate_lock },
+	{ "TSC", 0x04A0, 3, 0x032C, 3, &leaf1_gate_lock },
+	{ "IOCTOP", 0x04A0, 4, 0x032C, 4, &leaf1_gate_lock }, /* 6-10 */
+	{ "RSC", 0x04A0, 5, 0x032C, 5, &leaf1_gate_lock },
+	{ "DVM", 0x04A0, 6, 0x032C, 6, &leaf1_gate_lock },
+	{ "LVDS", 0x04A0, 7, 0x032C, 7, &leaf1_gate_lock },
+	{ "KAS", 0x04A0, 8, 0x032C, 8, &leaf1_gate_lock },
+	{ "AC97", 0x04A0, 9, 0x032C, 9, &leaf1_gate_lock }, /* 11-15 */
+	{ "USP0", 0x04A0, 10, 0x032C, 10, &leaf1_gate_lock },
+	{ "USP1", 0x04A0, 11, 0x032C, 11, &leaf1_gate_lock },
+	{ "USP2", 0x04A0, 12, 0x032C, 12, &leaf1_gate_lock },
+	{ "DMAC2", 0x04A0, 13, 0x032C, 13, &leaf1_gate_lock },
+	{ "DMAC3", 0x04A0, 14, 0x032C, 14, &leaf1_gate_lock }, /* 16-20 */
+	{ "AUDIO", 0x04A0, 15, 0x032C, 15, &leaf1_gate_lock },
+	{ "I2S1", 0x04A0, 17, 0x032C, 16, &leaf1_gate_lock },
+	{ "PMU_AUDIO", 0x04A0, 22, 0x032C, 17, &leaf1_gate_lock },
+	{ "THAUDMSCM", 0x04A0, 23, 0x032C, 18, &leaf1_gate_lock },
+	{ "SYS2PCI", 0x04B8, 0, 0x0338, 0, &leaf2_gate_lock }, /* 21-25 */
+	{ "PCIARB", 0x04B8, 1, 0x0338, 1, &leaf2_gate_lock },
+	{ "PCICOPY", 0x04B8, 2, 0x0338, 2, &leaf2_gate_lock },
+	{ "ROM", 0x04B8, 3, 0x0338, 3, &leaf2_gate_lock },
+	{ "SDIO23", 0x04B8, 4, 0x0338, 4, &leaf2_gate_lock },
+	{ "SDIO45", 0x04B8, 5, 0x0338, 5, &leaf2_gate_lock }, /* 26-30 */
+	{ "SDIO67", 0x04B8, 6, 0x0338, 6, &leaf2_gate_lock },
+	{ "VIP1", 0x04B8, 7, 0x0338, 7, &leaf2_gate_lock },
+	{ "VPP0", 0x04B8, 11, 0x0338, 8, &leaf2_gate_lock },
+	{ "LCD0", 0x04B8, 12, 0x0338, 9, &leaf2_gate_lock },
+	{ "VPP1", 0x04B8, 13, 0x0338, 10, &leaf2_gate_lock }, /* 31-35 */
+	{ "LCD1", 0x04B8, 14, 0x0338, 11, &leaf2_gate_lock },
+	{ "DCU", 0x04B8, 15, 0x0338, 12, &leaf2_gate_lock },
+	{ "GPIO", 0x04B8, 18, 0x0338, 13, &leaf2_gate_lock },
+	{ "DAPA_VDIFM", 0x04B8, 17, 0x0338, 15, &leaf2_gate_lock },
+	{ "THVDIFM", 0x04B8, 19, 0x0338, 16, &leaf2_gate_lock }, /* 36-40 */
+	{ "RGMII", 0x04D0, 0, 0x0344, 0, &leaf3_gate_lock },
+	{ "GMAC", 0x04D0, 1, 0x0344, 1, &leaf3_gate_lock },
+	{ "UART1", 0x04D0, 2, 0x0344, 2, &leaf3_gate_lock },
+	{ "DMAC0", 0x04D0, 3, 0x0344, 3, &leaf3_gate_lock },
+	{ "UART0", 0x04D0, 4, 0x0344, 4, &leaf3_gate_lock }, /* 41-45 */
+	{ "UART2", 0x04D0, 5, 0x0344, 5, &leaf3_gate_lock },
+	{ "UART3", 0x04D0, 6, 0x0344, 6, &leaf3_gate_lock },
+	{ "UART4", 0x04D0, 7, 0x0344, 7, &leaf3_gate_lock },
+	{ "UART5", 0x04D0, 8, 0x0344, 8, &leaf3_gate_lock },
+	{ "SPI1", 0x04D0, 9, 0x0344, 9, &leaf3_gate_lock }, /* 46-50 */
+	{ "GNSS_SYS_M0", 0x04D0, 10, 0x0344, 10, &leaf3_gate_lock },
+	{ "CANBUS1", 0x04D0, 12, 0x0344, 11, &leaf3_gate_lock },
+	{ "CCSEC", 0x04D0, 15, 0x0344, 12, &leaf3_gate_lock },
+	{ "CCPUB", 0x04D0, 16, 0x0344, 13, &leaf3_gate_lock },
+	{ "DAPA_GNSSM", 0x04D0, 13, 0x0344, 14, &leaf3_gate_lock }, /* 51-55 */
+	{ "THGNSSM", 0x04D0, 14, 0x0344, 15, &leaf3_gate_lock },
+	{ "VDEC", 0x04E8, 0, 0x0350, 0, &leaf4_gate_lock },
+	{ "JPENC", 0x04E8, 1, 0x0350, 1, &leaf4_gate_lock },
+	{ "G2D", 0x04E8, 2, 0x0350, 2, &leaf4_gate_lock },
+	{ "I2C0", 0x04E8, 3, 0x0350, 3, &leaf4_gate_lock }, /* 56-60 */
+	{ "I2C1", 0x04E8, 4, 0x0350, 4, &leaf4_gate_lock },
+	{ "GPIO0", 0x04E8, 5, 0x0350, 5, &leaf4_gate_lock },
+	{ "NAND", 0x04E8, 6, 0x0350, 6, &leaf4_gate_lock },
+	{ "SDIO01", 0x04E8, 7, 0x0350, 7, &leaf4_gate_lock },
+	{ "SYS2PCI2", 0x04E8, 8, 0x0350, 8, &leaf4_gate_lock }, /* 61-65 */
+	{ "USB0", 0x04E8, 11, 0x0350, 9, &leaf4_gate_lock },
+	{ "USB1", 0x04E8, 12, 0x0350, 10, &leaf4_gate_lock },
+	{ "THMEDIAM", 0x04E8, 15, 0x0350, 11, &leaf4_gate_lock },
+	{ "MEMC_DDRPHY", 0x0500, 0, 0x035C, 0, &leaf5_gate_lock },
+	{ "MEMC_UPCTL", 0x0500, 0, 0x035C, 1, &leaf5_gate_lock }, /* 66-70 */
+	{ "DAPA_MEM", 0x0500, 1, 0x035C, 2, &leaf5_gate_lock },
+	{ "MEMC_MEMDIV", 0x0500, 0, 0x035C, 3, &leaf5_gate_lock },
+	{ "THDDRM", 0x0500, 3, 0x035C, 4, &leaf5_gate_lock },
+	{ "CORESIGHT", 0x0518, 3, 0x0368, 13, &leaf6_gate_lock },
+	{ "THCPUM", 0x0518, 4, 0x0368, 17, &leaf6_gate_lock }, /* 71-75 */
+	{ "GRAPHIC", 0x0530, 0, 0x0374, 0, &leaf7_gate_lock },
+	{ "VSS_SDR", 0x0530, 1, 0x0374, 1, &leaf7_gate_lock },
+	{ "THGPUM", 0x0530, 2, 0x0374, 2, &leaf7_gate_lock },
+	{ "DMAC4", 0x0548, 2, 0x0380, 1, &leaf8_gate_lock },
+	{ "UART6", 0x0548, 3, 0x0380, 2, &leaf8_gate_lock }, /* 76- */
+	{ "USP3", 0x0548, 4, 0x0380, 3, &leaf8_gate_lock },
+	{ "THBTM", 0x0548, 5, 0x0380, 5, &leaf8_gate_lock },
+	{ "A7CA", 0x0548, 1, 0x0380, 0, &leaf8_gate_lock },
+	{ "A7CA_APB", 0x0548, 5, 0x0380, 4, &leaf8_gate_lock },
+};
+
+static int atlas7_reset_module(struct reset_controller_dev *rcdev,
+					unsigned long reset_idx)
+{
+	struct atlas7_reset_desc *reset = &atlas7_reset_unit[reset_idx];
+	unsigned long flags;
+
+	/*
+	 * HW suggest unit reset sequence:
+	 * assert sw reset (0)
+	 * setting sw clk_en to if the clock was disabled before reset
+	 * delay 16 clocks
+	 * disable clock (sw clk_en = 0)
+	 * de-assert reset (1)
+	 * after this sequence, restore clock or not is decided by SW
+	 */
+
+	spin_lock_irqsave(reset->lock, flags);
+	/* clock enable or not */
+	if (clkc_readl(reset->clk_ofs + 8) & (1 << reset->clk_bit)) {
+		clkc_writel(1 << reset->rst_bit, reset->rst_ofs + 4);
+		udelay(2);
+		clkc_writel(1 << reset->clk_bit, reset->clk_ofs + 4);
+		clkc_writel(1 << reset->rst_bit, reset->rst_ofs);
+		/* restore clock enable */
+		clkc_writel(1 << reset->clk_bit, reset->clk_ofs);
+	} else {
+		clkc_writel(1 << reset->rst_bit, reset->rst_ofs + 4);
+		clkc_writel(1 << reset->clk_bit, reset->clk_ofs);
+		udelay(2);
+		clkc_writel(1 << reset->clk_bit, reset->clk_ofs + 4);
+		clkc_writel(1 << reset->rst_bit, reset->rst_ofs);
+	}
+	spin_unlock_irqrestore(reset->lock, flags);
+
+	return 0;
+}
+
+static struct reset_control_ops atlas7_rst_ops = {
+	.reset = atlas7_reset_module,
+};
+
+static struct reset_controller_dev atlas7_rst_ctlr = {
+	.ops = &atlas7_rst_ops,
+	.owner = THIS_MODULE,
+	.of_reset_n_cells = 1,
+};
+
+static void __init atlas7_clk_init(struct device_node *np)
+{
+	struct clk *clk;
+	struct atlas7_div_init_data *div;
+	struct atlas7_mux_init_data *mux;
+	struct atlas7_unit_init_data *unit;
+	int i;
+	int ret;
+
+	sirfsoc_clk_vbase = of_iomap(np, 0);
+	if (!sirfsoc_clk_vbase)
+		panic("unable to map clkc registers\n");
+
+	of_node_put(np);
+
+	clk = clk_register(NULL, &clk_cpupll.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_mempll.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_sys0pll.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_sys1pll.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_sys2pll.hw);
+	BUG_ON(!clk);
+	clk = clk_register(NULL, &clk_sys3pll.hw);
+	BUG_ON(!clk);
+
+	clk = clk_register_divider_table(NULL, "cpupll_div1", "cpupll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_CPUPLL_AB_CTRL1, 0, 3, 0,
+			 pll_div_table, &cpupll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "cpupll_div2", "cpupll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_CPUPLL_AB_CTRL1, 4, 3, 0,
+			 pll_div_table, &cpupll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "cpupll_div3", "cpupll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_CPUPLL_AB_CTRL1, 8, 3, 0,
+			 pll_div_table, &cpupll_ctrl1_lock);
+	BUG_ON(!clk);
+
+	clk = clk_register_divider_table(NULL, "mempll_div1", "mempll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_MEMPLL_AB_CTRL1, 0, 3, 0,
+			 pll_div_table, &mempll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "mempll_div2", "mempll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_MEMPLL_AB_CTRL1, 4, 3, 0,
+			 pll_div_table, &mempll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "mempll_div3", "mempll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_MEMPLL_AB_CTRL1, 8, 3, 0,
+			 pll_div_table, &mempll_ctrl1_lock);
+	BUG_ON(!clk);
+
+	clk = clk_register_divider_table(NULL, "sys0pll_div1", "sys0pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS0PLL_AB_CTRL1, 0, 3, 0,
+			 pll_div_table, &sys0pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "sys0pll_div2", "sys0pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS0PLL_AB_CTRL1, 4, 3, 0,
+			 pll_div_table, &sys0pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "sys0pll_div3", "sys0pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS0PLL_AB_CTRL1, 8, 3, 0,
+			 pll_div_table, &sys0pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_fixed_factor(NULL, "sys0pll_fixdiv", "sys0pll_vco",
+					CLK_SET_RATE_PARENT, 1, 2);
+
+	clk = clk_register_divider_table(NULL, "sys1pll_div1", "sys1pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS1PLL_AB_CTRL1, 0, 3, 0,
+			 pll_div_table, &sys1pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "sys1pll_div2", "sys1pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS1PLL_AB_CTRL1, 4, 3, 0,
+			 pll_div_table, &sys1pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "sys1pll_div3", "sys1pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS1PLL_AB_CTRL1, 8, 3, 0,
+			 pll_div_table, &sys1pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_fixed_factor(NULL, "sys1pll_fixdiv", "sys1pll_vco",
+					CLK_SET_RATE_PARENT, 1, 2);
+
+	clk = clk_register_divider_table(NULL, "sys2pll_div1", "sys2pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS2PLL_AB_CTRL1, 0, 3, 0,
+			 pll_div_table, &sys2pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "sys2pll_div2", "sys2pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS2PLL_AB_CTRL1, 4, 3, 0,
+			 pll_div_table, &sys2pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "sys2pll_div3", "sys2pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS2PLL_AB_CTRL1, 8, 3, 0,
+			 pll_div_table, &sys2pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_fixed_factor(NULL, "sys2pll_fixdiv", "sys2pll_vco",
+					CLK_SET_RATE_PARENT, 1, 2);
+
+	clk = clk_register_divider_table(NULL, "sys3pll_div1", "sys3pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS3PLL_AB_CTRL1, 0, 3, 0,
+			 pll_div_table, &sys3pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "sys3pll_div2", "sys3pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS3PLL_AB_CTRL1, 4, 3, 0,
+			 pll_div_table, &sys3pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_divider_table(NULL, "sys3pll_div3", "sys3pll_vco", 0,
+			 sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS3PLL_AB_CTRL1, 8, 3, 0,
+			 pll_div_table, &sys3pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_fixed_factor(NULL, "sys3pll_fixdiv", "sys3pll_vco",
+					CLK_SET_RATE_PARENT, 1, 2);
+
+	BUG_ON(!clk);
+	clk = clk_register_fixed_factor(NULL, "xinw_fixdiv_btslow", "xinw",
+					CLK_SET_RATE_PARENT, 1, 4);
+
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "cpupll_clk1", "cpupll_div1",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_CPUPLL_AB_CTRL1,
+				12, 0, &cpupll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "cpupll_clk2", "cpupll_div2",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_CPUPLL_AB_CTRL1,
+				13, 0, &cpupll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "cpupll_clk3", "cpupll_div3",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_CPUPLL_AB_CTRL1,
+				14, 0, &cpupll_ctrl1_lock);
+	BUG_ON(!clk);
+
+	clk = clk_register_gate(NULL, "mempll_clk1", "mempll_div1",
+		CLK_SET_RATE_PARENT | CLK_IGNORE_UNUSED,
+		sirfsoc_clk_vbase + SIRFSOC_CLKC_MEMPLL_AB_CTRL1,
+		12, 0, &mempll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "mempll_clk2", "mempll_div2",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_MEMPLL_AB_CTRL1,
+				13, 0, &mempll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "mempll_clk3", "mempll_div3",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_MEMPLL_AB_CTRL1,
+				14, 0, &mempll_ctrl1_lock);
+	BUG_ON(!clk);
+
+	clk = clk_register_gate(NULL, "sys0pll_clk1", "sys0pll_div1",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS0PLL_AB_CTRL1,
+				12, 0, &sys0pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "sys0pll_clk2", "sys0pll_div2",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS0PLL_AB_CTRL1,
+				13, 0, &sys0pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "sys0pll_clk3", "sys0pll_div3",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS0PLL_AB_CTRL1,
+				14, 0, &sys0pll_ctrl1_lock);
+	BUG_ON(!clk);
+
+	clk = clk_register_gate(NULL, "sys1pll_clk1", "sys1pll_div1",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS1PLL_AB_CTRL1,
+				12, 0, &sys1pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "sys1pll_clk2", "sys1pll_div2",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS1PLL_AB_CTRL1,
+				13, 0, &sys1pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "sys1pll_clk3", "sys1pll_div3",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS1PLL_AB_CTRL1,
+				14, 0, &sys1pll_ctrl1_lock);
+	BUG_ON(!clk);
+
+	clk = clk_register_gate(NULL, "sys2pll_clk1", "sys2pll_div1",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS2PLL_AB_CTRL1,
+				12, 0, &sys2pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "sys2pll_clk2", "sys2pll_div2",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS2PLL_AB_CTRL1,
+				13, 0, &sys2pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "sys2pll_clk3", "sys2pll_div3",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS2PLL_AB_CTRL1,
+				14, 0, &sys2pll_ctrl1_lock);
+	BUG_ON(!clk);
+
+	clk = clk_register_gate(NULL, "sys3pll_clk1", "sys3pll_div1",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS3PLL_AB_CTRL1,
+				12, 0, &sys3pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "sys3pll_clk2", "sys3pll_div2",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS3PLL_AB_CTRL1,
+				13, 0, &sys3pll_ctrl1_lock);
+	BUG_ON(!clk);
+	clk = clk_register_gate(NULL, "sys3pll_clk3", "sys3pll_div3",
+		CLK_SET_RATE_PARENT, sirfsoc_clk_vbase + SIRFSOC_CLKC_SYS3PLL_AB_CTRL1,
+				14, 0, &sys3pll_ctrl1_lock);
+	BUG_ON(!clk);
+
+	clk = clk_register(NULL, &clk_audio_dto.hw);
+	BUG_ON(!clk);
+
+	clk = clk_register(NULL, &clk_disp0_dto.hw);
+	BUG_ON(!clk);
+
+	clk = clk_register(NULL, &clk_disp1_dto.hw);
+	BUG_ON(!clk);
+
+	for (i = 0; i < ARRAY_SIZE(divider_list); i++) {
+		div = &divider_list[i];
+		clk = clk_register_divider(NULL, div->div_name,
+			div->parent_name, div->divider_flags, sirfsoc_clk_vbase + div->div_offset,
+			div->shift, div->width, 0, div->lock);
+		BUG_ON(!clk);
+		clk = clk_register_gate(NULL, div->gate_name, div->div_name,
+			div->gate_flags, sirfsoc_clk_vbase + div->gate_offset,
+				div->gate_bit, 0, div->lock);
+		BUG_ON(!clk);
+	}
+	/* ignore selector status register check */
+	for (i = 0; i < ARRAY_SIZE(mux_list); i++) {
+		mux = &mux_list[i];
+		clk = clk_register_mux(NULL, mux->mux_name, mux->parent_names,
+			       mux->parent_num, mux->flags,
+			       sirfsoc_clk_vbase + mux->mux_offset,
+			       mux->shift, mux->width,
+			       mux->mux_flags, NULL);
+		BUG_ON(!clk);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(unit_list); i++) {
+		unit = &unit_list[i];
+		atlas7_clks[i] = atlas7_unit_clk_register(NULL, unit->unit_name, unit->parent_name,
+				unit->flags, unit->regofs, unit->bit, unit->lock);
+		BUG_ON(!atlas7_clks[i]);
+	}
+
+	clk_data.clks = atlas7_clks;
+	clk_data.clk_num = ARRAY_SIZE(unit_list);
+
+	ret = of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+	BUG_ON(ret);
+
+	atlas7_rst_ctlr.of_node = np;
+	atlas7_rst_ctlr.nr_resets = ARRAY_SIZE(atlas7_reset_unit);
+	reset_controller_register(&atlas7_rst_ctlr);
+}
+CLK_OF_DECLARE(atlas7_clk, "sirf,atlas7-car", atlas7_clk_init);
diff --git a/drivers/clk/sirf/clk-common.c b/drivers/clk/sirf/clk-common.c
index 37af51c5f213..9fc285d784d3 100644
--- a/drivers/clk/sirf/clk-common.c
+++ b/drivers/clk/sirf/clk-common.c
@@ -10,8 +10,8 @@
 #define KHZ     1000
 #define MHZ     (KHZ * KHZ)
 
-static void *sirfsoc_clk_vbase;
-static void *sirfsoc_rsc_vbase;
+static void __iomem *sirfsoc_clk_vbase;
+static void __iomem *sirfsoc_rsc_vbase;
 static struct clk_onecell_data clk_data;
 
 /*
@@ -188,7 +188,7 @@ static struct clk_ops std_pll_ops = {
 	.set_rate = pll_clk_set_rate,
 };
 
-static const char *pll_clk_parents[] = {
+static const char * const pll_clk_parents[] = {
 	"osc",
 };
 
@@ -284,7 +284,7 @@ static struct clk_hw usb_pll_clk_hw = {
  * clock domains - cpu, mem, sys/io, dsp, gfx
  */
 
-static const char *dmn_clk_parents[] = {
+static const char * const dmn_clk_parents[] = {
 	"rtc",
 	"osc",
 	"pll1",
@@ -673,7 +673,7 @@ static void std_clk_disable(struct clk_hw *hw)
 	clkc_writel(val, reg);
 }
 
-static const char *std_clk_io_parents[] = {
+static const char * const std_clk_io_parents[] = {
 	"io",
 };
 
@@ -949,7 +949,7 @@ static struct clk_std clk_pulse = {
 	},
 };
 
-static const char *std_clk_dsp_parents[] = {
+static const char * const std_clk_dsp_parents[] = {
 	"dsp",
 };
 
@@ -981,7 +981,7 @@ static struct clk_std clk_mf = {
 	},
 };
 
-static const char *std_clk_sys_parents[] = {
+static const char * const std_clk_sys_parents[] = {
 	"sys",
 };
 
@@ -999,7 +999,7 @@ static struct clk_std clk_security = {
 	},
 };
 
-static const char *std_clk_usb_parents[] = {
+static const char * const std_clk_usb_parents[] = {
 	"usb_pll",
 };
 
diff --git a/drivers/clk/socfpga/Makefile b/drivers/clk/socfpga/Makefile
index 7e2d15a0c7b8..d8bb239753a4 100644
--- a/drivers/clk/socfpga/Makefile
+++ b/drivers/clk/socfpga/Makefile
@@ -2,3 +2,4 @@ obj-y += clk.o
 obj-y += clk-gate.o
 obj-y += clk-pll.o
 obj-y += clk-periph.o
+obj-y += clk-pll-a10.o clk-periph-a10.o clk-gate-a10.o
diff --git a/drivers/clk/socfpga/clk-gate-a10.c b/drivers/clk/socfpga/clk-gate-a10.c
new file mode 100644
index 000000000000..83c6780ff4b2
--- /dev/null
+++ b/drivers/clk/socfpga/clk-gate-a10.c
@@ -0,0 +1,190 @@
+/*
+ * Copyright (C) 2015 Altera Corporation. All rights reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+
+#include "clk.h"
+
+#define streq(a, b) (strcmp((a), (b)) == 0)
+
+#define to_socfpga_gate_clk(p) container_of(p, struct socfpga_gate_clk, hw.hw)
+
+/* SDMMC Group for System Manager defines */
+#define SYSMGR_SDMMCGRP_CTRL_OFFSET	0x28
+
+static unsigned long socfpga_gate_clk_recalc_rate(struct clk_hw *hwclk,
+	unsigned long parent_rate)
+{
+	struct socfpga_gate_clk *socfpgaclk = to_socfpga_gate_clk(hwclk);
+	u32 div = 1, val;
+
+	if (socfpgaclk->fixed_div)
+		div = socfpgaclk->fixed_div;
+	else if (socfpgaclk->div_reg) {
+		val = readl(socfpgaclk->div_reg) >> socfpgaclk->shift;
+		val &= div_mask(socfpgaclk->width);
+		div = (1 << val);
+	}
+
+	return parent_rate / div;
+}
+
+static int socfpga_clk_prepare(struct clk_hw *hwclk)
+{
+	struct socfpga_gate_clk *socfpgaclk = to_socfpga_gate_clk(hwclk);
+	int i;
+	u32 hs_timing;
+	u32 clk_phase[2];
+
+	if (socfpgaclk->clk_phase[0] || socfpgaclk->clk_phase[1]) {
+		for (i = 0; i < ARRAY_SIZE(clk_phase); i++) {
+			switch (socfpgaclk->clk_phase[i]) {
+			case 0:
+				clk_phase[i] = 0;
+				break;
+			case 45:
+				clk_phase[i] = 1;
+				break;
+			case 90:
+				clk_phase[i] = 2;
+				break;
+			case 135:
+				clk_phase[i] = 3;
+				break;
+			case 180:
+				clk_phase[i] = 4;
+				break;
+			case 225:
+				clk_phase[i] = 5;
+				break;
+			case 270:
+				clk_phase[i] = 6;
+				break;
+			case 315:
+				clk_phase[i] = 7;
+				break;
+			default:
+				clk_phase[i] = 0;
+				break;
+			}
+		}
+
+		hs_timing = SYSMGR_SDMMC_CTRL_SET(clk_phase[0], clk_phase[1]);
+		if (!IS_ERR(socfpgaclk->sys_mgr_base_addr))
+			regmap_write(socfpgaclk->sys_mgr_base_addr,
+				     SYSMGR_SDMMCGRP_CTRL_OFFSET, hs_timing);
+		else
+			pr_err("%s: cannot set clk_phase because sys_mgr_base_addr is not available!\n",
+					__func__);
+	}
+	return 0;
+}
+
+static struct clk_ops gateclk_ops = {
+	.prepare = socfpga_clk_prepare,
+	.recalc_rate = socfpga_gate_clk_recalc_rate,
+};
+
+static void __init __socfpga_gate_init(struct device_node *node,
+	const struct clk_ops *ops)
+{
+	u32 clk_gate[2];
+	u32 div_reg[3];
+	u32 clk_phase[2];
+	u32 fixed_div;
+	struct clk *clk;
+	struct socfpga_gate_clk *socfpga_clk;
+	const char *clk_name = node->name;
+	const char *parent_name[SOCFPGA_MAX_PARENTS];
+	struct clk_init_data init;
+	int rc;
+	int i = 0;
+
+	socfpga_clk = kzalloc(sizeof(*socfpga_clk), GFP_KERNEL);
+	if (WARN_ON(!socfpga_clk))
+		return;
+
+	rc = of_property_read_u32_array(node, "clk-gate", clk_gate, 2);
+	if (rc)
+		clk_gate[0] = 0;
+
+	if (clk_gate[0]) {
+		socfpga_clk->hw.reg = clk_mgr_a10_base_addr + clk_gate[0];
+		socfpga_clk->hw.bit_idx = clk_gate[1];
+
+		gateclk_ops.enable = clk_gate_ops.enable;
+		gateclk_ops.disable = clk_gate_ops.disable;
+	}
+
+	rc = of_property_read_u32(node, "fixed-divider", &fixed_div);
+	if (rc)
+		socfpga_clk->fixed_div = 0;
+	else
+		socfpga_clk->fixed_div = fixed_div;
+
+	rc = of_property_read_u32_array(node, "div-reg", div_reg, 3);
+	if (!rc) {
+		socfpga_clk->div_reg = clk_mgr_a10_base_addr + div_reg[0];
+		socfpga_clk->shift = div_reg[1];
+		socfpga_clk->width = div_reg[2];
+	} else {
+		socfpga_clk->div_reg = NULL;
+	}
+
+	rc = of_property_read_u32_array(node, "clk-phase", clk_phase, 2);
+	if (!rc) {
+		socfpga_clk->clk_phase[0] = clk_phase[0];
+		socfpga_clk->clk_phase[1] = clk_phase[1];
+
+		socfpga_clk->sys_mgr_base_addr =
+			syscon_regmap_lookup_by_compatible("altr,sys-mgr");
+		if (IS_ERR(socfpga_clk->sys_mgr_base_addr)) {
+			pr_err("%s: failed to find altr,sys-mgr regmap!\n",
+					__func__);
+			return;
+		}
+	}
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+
+	init.name = clk_name;
+	init.ops = ops;
+	init.flags = 0;
+	while (i < SOCFPGA_MAX_PARENTS && (parent_name[i] =
+			of_clk_get_parent_name(node, i)) != NULL)
+		i++;
+
+	init.parent_names = parent_name;
+	init.num_parents = i;
+	socfpga_clk->hw.hw.init = &init;
+
+	clk = clk_register(NULL, &socfpga_clk->hw.hw);
+	if (WARN_ON(IS_ERR(clk))) {
+		kfree(socfpga_clk);
+		return;
+	}
+	rc = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (WARN_ON(rc))
+		return;
+}
+
+void __init socfpga_a10_gate_init(struct device_node *node)
+{
+	__socfpga_gate_init(node, &gateclk_ops);
+}
diff --git a/drivers/clk/socfpga/clk-gate.c b/drivers/clk/socfpga/clk-gate.c
index dd3a78c64795..82449cd76fd7 100644
--- a/drivers/clk/socfpga/clk-gate.c
+++ b/drivers/clk/socfpga/clk-gate.c
@@ -32,14 +32,10 @@
 #define SOCFPGA_MMC_CLK			"sdmmc_clk"
 #define SOCFPGA_GPIO_DB_CLK_OFFSET	0xA8
 
-#define streq(a, b) (strcmp((a), (b)) == 0)
-
 #define to_socfpga_gate_clk(p) container_of(p, struct socfpga_gate_clk, hw.hw)
 
 /* SDMMC Group for System Manager defines */
 #define SYSMGR_SDMMCGRP_CTRL_OFFSET    0x108
-#define SYSMGR_SDMMC_CTRL_SET(smplsel, drvsel) \
-	((((smplsel) & 0x7) << 3) | (((drvsel) & 0x7) << 0))
 
 static u8 socfpga_clk_get_parent(struct clk_hw *hwclk)
 {
@@ -194,7 +190,6 @@ static void __init __socfpga_gate_init(struct device_node *node,
 	const char *parent_name[SOCFPGA_MAX_PARENTS];
 	struct clk_init_data init;
 	int rc;
-	int i = 0;
 
 	socfpga_clk = kzalloc(sizeof(*socfpga_clk), GFP_KERNEL);
 	if (WARN_ON(!socfpga_clk))
@@ -224,7 +219,7 @@ static void __init __socfpga_gate_init(struct device_node *node,
 		socfpga_clk->shift = div_reg[1];
 		socfpga_clk->width = div_reg[2];
 	} else {
-		socfpga_clk->div_reg = 0;
+		socfpga_clk->div_reg = NULL;
 	}
 
 	rc = of_property_read_u32_array(node, "clk-phase", clk_phase, 2);
@@ -238,12 +233,9 @@ static void __init __socfpga_gate_init(struct device_node *node,
 	init.name = clk_name;
 	init.ops = ops;
 	init.flags = 0;
-	while (i < SOCFPGA_MAX_PARENTS && (parent_name[i] =
-			of_clk_get_parent_name(node, i)) != NULL)
-		i++;
 
+	init.num_parents = of_clk_parent_fill(node, parent_name, SOCFPGA_MAX_PARENTS);
 	init.parent_names = parent_name;
-	init.num_parents = i;
 	socfpga_clk->hw.hw.init = &init;
 
 	clk = clk_register(NULL, &socfpga_clk->hw.hw);
diff --git a/drivers/clk/socfpga/clk-periph-a10.c b/drivers/clk/socfpga/clk-periph-a10.c
new file mode 100644
index 000000000000..9d0181b5a6a4
--- /dev/null
+++ b/drivers/clk/socfpga/clk-periph-a10.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) 2015 Altera Corporation. All rights reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
+
+#include "clk.h"
+
+#define CLK_MGR_FREE_SHIFT		16
+#define CLK_MGR_FREE_MASK		0x7
+
+#define SOCFPGA_MPU_FREE_CLK		"mpu_free_clk"
+#define SOCFPGA_NOC_FREE_CLK		"noc_free_clk"
+#define SOCFPGA_SDMMC_FREE_CLK		"sdmmc_free_clk"
+#define to_socfpga_periph_clk(p) container_of(p, struct socfpga_periph_clk, hw.hw)
+
+static unsigned long clk_periclk_recalc_rate(struct clk_hw *hwclk,
+					     unsigned long parent_rate)
+{
+	struct socfpga_periph_clk *socfpgaclk = to_socfpga_periph_clk(hwclk);
+	u32 div;
+
+	if (socfpgaclk->fixed_div) {
+		div = socfpgaclk->fixed_div;
+	} else if (socfpgaclk->div_reg) {
+		div = readl(socfpgaclk->div_reg) >> socfpgaclk->shift;
+		div &= div_mask(socfpgaclk->width);
+		div += 1;
+	} else {
+		div = ((readl(socfpgaclk->hw.reg) & 0x7ff) + 1);
+	}
+
+	return parent_rate / div;
+}
+
+static u8 clk_periclk_get_parent(struct clk_hw *hwclk)
+{
+	struct socfpga_periph_clk *socfpgaclk = to_socfpga_periph_clk(hwclk);
+	u32 clk_src;
+
+	clk_src = readl(socfpgaclk->hw.reg);
+	if (streq(hwclk->init->name, SOCFPGA_MPU_FREE_CLK) ||
+	    streq(hwclk->init->name, SOCFPGA_NOC_FREE_CLK) ||
+	    streq(hwclk->init->name, SOCFPGA_SDMMC_FREE_CLK))
+		return (clk_src >> CLK_MGR_FREE_SHIFT) &
+			CLK_MGR_FREE_MASK;
+	else
+		return 0;
+}
+
+static const struct clk_ops periclk_ops = {
+	.recalc_rate = clk_periclk_recalc_rate,
+	.get_parent = clk_periclk_get_parent,
+};
+
+static __init void __socfpga_periph_init(struct device_node *node,
+	const struct clk_ops *ops)
+{
+	u32 reg;
+	struct clk *clk;
+	struct socfpga_periph_clk *periph_clk;
+	const char *clk_name = node->name;
+	const char *parent_name;
+	struct clk_init_data init;
+	int rc;
+	u32 fixed_div;
+	u32 div_reg[3];
+
+	of_property_read_u32(node, "reg", &reg);
+
+	periph_clk = kzalloc(sizeof(*periph_clk), GFP_KERNEL);
+	if (WARN_ON(!periph_clk))
+		return;
+
+	periph_clk->hw.reg = clk_mgr_a10_base_addr + reg;
+
+	rc = of_property_read_u32_array(node, "div-reg", div_reg, 3);
+	if (!rc) {
+		periph_clk->div_reg = clk_mgr_a10_base_addr + div_reg[0];
+		periph_clk->shift = div_reg[1];
+		periph_clk->width = div_reg[2];
+	} else {
+		periph_clk->div_reg = NULL;
+	}
+
+	rc = of_property_read_u32(node, "fixed-divider", &fixed_div);
+	if (rc)
+		periph_clk->fixed_div = 0;
+	else
+		periph_clk->fixed_div = fixed_div;
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+
+	init.name = clk_name;
+	init.ops = ops;
+	init.flags = 0;
+
+	parent_name = of_clk_get_parent_name(node, 0);
+	init.num_parents = 1;
+	init.parent_names = &parent_name;
+
+	periph_clk->hw.hw.init = &init;
+
+	clk = clk_register(NULL, &periph_clk->hw.hw);
+	if (WARN_ON(IS_ERR(clk))) {
+		kfree(periph_clk);
+		return;
+	}
+	rc = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (rc < 0) {
+		pr_err("Could not register clock provider for node:%s\n",
+		       clk_name);
+		goto err_clk;
+	}
+
+	return;
+
+err_clk:
+	clk_unregister(clk);
+}
+
+void __init socfpga_a10_periph_init(struct device_node *node)
+{
+	__socfpga_periph_init(node, &periclk_ops);
+}
diff --git a/drivers/clk/socfpga/clk-periph.c b/drivers/clk/socfpga/clk-periph.c
index 46531c34ec9b..83aeaa219d14 100644
--- a/drivers/clk/socfpga/clk-periph.c
+++ b/drivers/clk/socfpga/clk-periph.c
@@ -76,7 +76,7 @@ static __init void __socfpga_periph_init(struct device_node *node,
 		periph_clk->shift = div_reg[1];
 		periph_clk->width = div_reg[2];
 	} else {
-		periph_clk->div_reg = 0;
+		periph_clk->div_reg = NULL;
 	}
 
 	rc = of_property_read_u32(node, "fixed-divider", &fixed_div);
diff --git a/drivers/clk/socfpga/clk-pll-a10.c b/drivers/clk/socfpga/clk-pll-a10.c
new file mode 100644
index 000000000000..1178b11babca
--- /dev/null
+++ b/drivers/clk/socfpga/clk-pll-a10.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2015 Altera Corporation. All rights reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/clk-provider.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "clk.h"
+
+/* Clock Manager offsets */
+#define CLK_MGR_PLL_CLK_SRC_SHIFT	8
+#define CLK_MGR_PLL_CLK_SRC_MASK	0x3
+
+/* Clock bypass bits */
+#define SOCFPGA_PLL_BG_PWRDWN		0
+#define SOCFPGA_PLL_PWR_DOWN		1
+#define SOCFPGA_PLL_EXT_ENA		2
+#define SOCFPGA_PLL_DIVF_MASK		0x00001FFF
+#define SOCFPGA_PLL_DIVF_SHIFT	0
+#define SOCFPGA_PLL_DIVQ_MASK		0x003F0000
+#define SOCFPGA_PLL_DIVQ_SHIFT	16
+#define SOCFGPA_MAX_PARENTS	5
+
+#define SOCFPGA_MAIN_PLL_CLK		"main_pll"
+#define SOCFPGA_PERIP_PLL_CLK		"periph_pll"
+
+#define to_socfpga_clk(p) container_of(p, struct socfpga_pll, hw.hw)
+
+void __iomem *clk_mgr_a10_base_addr;
+
+static unsigned long clk_pll_recalc_rate(struct clk_hw *hwclk,
+					 unsigned long parent_rate)
+{
+	struct socfpga_pll *socfpgaclk = to_socfpga_clk(hwclk);
+	unsigned long divf, divq, reg;
+	unsigned long long vco_freq;
+
+	/* read VCO1 reg for numerator and denominator */
+	reg = readl(socfpgaclk->hw.reg + 0x4);
+	divf = (reg & SOCFPGA_PLL_DIVF_MASK) >> SOCFPGA_PLL_DIVF_SHIFT;
+	divq = (reg & SOCFPGA_PLL_DIVQ_MASK) >> SOCFPGA_PLL_DIVQ_SHIFT;
+	vco_freq = (unsigned long long)parent_rate * (divf + 1);
+	do_div(vco_freq, (1 + divq));
+	return (unsigned long)vco_freq;
+}
+
+static u8 clk_pll_get_parent(struct clk_hw *hwclk)
+{
+	struct socfpga_pll *socfpgaclk = to_socfpga_clk(hwclk);
+	u32 pll_src;
+
+	pll_src = readl(socfpgaclk->hw.reg);
+
+	return (pll_src >> CLK_MGR_PLL_CLK_SRC_SHIFT) &
+		CLK_MGR_PLL_CLK_SRC_MASK;
+}
+
+static struct clk_ops clk_pll_ops = {
+	.recalc_rate = clk_pll_recalc_rate,
+	.get_parent = clk_pll_get_parent,
+};
+
+static struct __init clk * __socfpga_pll_init(struct device_node *node,
+	const struct clk_ops *ops)
+{
+	u32 reg;
+	struct clk *clk;
+	struct socfpga_pll *pll_clk;
+	const char *clk_name = node->name;
+	const char *parent_name[SOCFGPA_MAX_PARENTS];
+	struct clk_init_data init;
+	struct device_node *clkmgr_np;
+	int rc;
+	int i = 0;
+
+	of_property_read_u32(node, "reg", &reg);
+
+	pll_clk = kzalloc(sizeof(*pll_clk), GFP_KERNEL);
+	if (WARN_ON(!pll_clk))
+		return NULL;
+
+	clkmgr_np = of_find_compatible_node(NULL, NULL, "altr,clk-mgr");
+	clk_mgr_a10_base_addr = of_iomap(clkmgr_np, 0);
+	BUG_ON(!clk_mgr_a10_base_addr);
+	pll_clk->hw.reg = clk_mgr_a10_base_addr + reg;
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+
+	init.name = clk_name;
+	init.ops = ops;
+	init.flags = 0;
+
+	while (i < SOCFGPA_MAX_PARENTS && (parent_name[i] =
+			of_clk_get_parent_name(node, i)) != NULL)
+		i++;
+	init.num_parents = i;
+	init.parent_names = parent_name;
+	pll_clk->hw.hw.init = &init;
+
+	pll_clk->hw.bit_idx = SOCFPGA_PLL_EXT_ENA;
+	clk_pll_ops.enable = clk_gate_ops.enable;
+	clk_pll_ops.disable = clk_gate_ops.disable;
+
+	clk = clk_register(NULL, &pll_clk->hw.hw);
+	if (WARN_ON(IS_ERR(clk))) {
+		kfree(pll_clk);
+		return NULL;
+	}
+	rc = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	return clk;
+}
+
+void __init socfpga_a10_pll_init(struct device_node *node)
+{
+	__socfpga_pll_init(node, &clk_pll_ops);
+}
diff --git a/drivers/clk/socfpga/clk-pll.c b/drivers/clk/socfpga/clk-pll.c
index de6da957a09d..8f26b5234947 100644
--- a/drivers/clk/socfpga/clk-pll.c
+++ b/drivers/clk/socfpga/clk-pll.c
@@ -92,7 +92,6 @@ static __init struct clk *__socfpga_pll_init(struct device_node *node,
 	struct clk_init_data init;
 	struct device_node *clkmgr_np;
 	int rc;
-	int i = 0;
 
 	of_property_read_u32(node, "reg", &reg);
 
@@ -111,11 +110,7 @@ static __init struct clk *__socfpga_pll_init(struct device_node *node,
 	init.ops = ops;
 	init.flags = 0;
 
-	while (i < SOCFPGA_MAX_PARENTS && (parent_name[i] =
-			of_clk_get_parent_name(node, i)) != NULL)
-		i++;
-
-	init.num_parents = i;
+	init.num_parents = of_clk_parent_fill(node, parent_name, SOCFPGA_MAX_PARENTS);
 	init.parent_names = parent_name;
 	pll_clk->hw.hw.init = &init;
 
diff --git a/drivers/clk/socfpga/clk.c b/drivers/clk/socfpga/clk.c
index 43db947e5f0e..7564d2e35f32 100644
--- a/drivers/clk/socfpga/clk.c
+++ b/drivers/clk/socfpga/clk.c
@@ -24,4 +24,9 @@
 CLK_OF_DECLARE(socfpga_pll_clk, "altr,socfpga-pll-clock", socfpga_pll_init);
 CLK_OF_DECLARE(socfpga_perip_clk, "altr,socfpga-perip-clk", socfpga_periph_init);
 CLK_OF_DECLARE(socfpga_gate_clk, "altr,socfpga-gate-clk", socfpga_gate_init);
-
+CLK_OF_DECLARE(socfpga_a10_pll_clk, "altr,socfpga-a10-pll-clock",
+	       socfpga_a10_pll_init);
+CLK_OF_DECLARE(socfpga_a10_perip_clk, "altr,socfpga-a10-perip-clk",
+	       socfpga_a10_periph_init);
+CLK_OF_DECLARE(socfpga_a10_gate_clk, "altr,socfpga-a10-gate-clk",
+	       socfpga_a10_gate_init);
diff --git a/drivers/clk/socfpga/clk.h b/drivers/clk/socfpga/clk.h
index d291f60c46e1..603973ab7e29 100644
--- a/drivers/clk/socfpga/clk.h
+++ b/drivers/clk/socfpga/clk.h
@@ -26,14 +26,22 @@
 #define CLKMGR_L4SRC		0x70
 #define CLKMGR_PERPLL_SRC	0xAC
 
-#define SOCFPGA_MAX_PARENTS		3
+#define SOCFPGA_MAX_PARENTS		5
 #define div_mask(width) ((1 << (width)) - 1)
 
+#define streq(a, b) (strcmp((a), (b)) == 0)
+#define SYSMGR_SDMMC_CTRL_SET(smplsel, drvsel) \
+	((((smplsel) & 0x7) << 3) | (((drvsel) & 0x7) << 0))
+
 extern void __iomem *clk_mgr_base_addr;
+extern void __iomem *clk_mgr_a10_base_addr;
 
 void __init socfpga_pll_init(struct device_node *node);
 void __init socfpga_periph_init(struct device_node *node);
 void __init socfpga_gate_init(struct device_node *node);
+void socfpga_a10_pll_init(struct device_node *node);
+void socfpga_a10_periph_init(struct device_node *node);
+void socfpga_a10_gate_init(struct device_node *node);
 
 struct socfpga_pll {
 	struct clk_gate	hw;
@@ -44,6 +52,7 @@ struct socfpga_gate_clk {
 	char *parent_name;
 	u32 fixed_div;
 	void __iomem *div_reg;
+	struct regmap *sys_mgr_base_addr;
 	u32 width;	/* only valid if div_reg != 0 */
 	u32 shift;	/* only valid if div_reg != 0 */
 	u32 clk_phase[2];
diff --git a/drivers/clk/st/clk-flexgen.c b/drivers/clk/st/clk-flexgen.c
index bf12a25eb3a2..8dd8cce27361 100644
--- a/drivers/clk/st/clk-flexgen.c
+++ b/drivers/clk/st/clk-flexgen.c
@@ -116,7 +116,7 @@ static long flexgen_round_rate(struct clk_hw *hw, unsigned long rate,
 	return *prate / div;
 }
 
-unsigned long flexgen_recalc_rate(struct clk_hw *hw,
+static unsigned long flexgen_recalc_rate(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct flexgen *flexgen = to_flexgen(hw);
@@ -174,7 +174,7 @@ static const struct clk_ops flexgen_ops = {
 	.set_rate = flexgen_set_rate,
 };
 
-struct clk *clk_register_flexgen(const char *name,
+static struct clk *clk_register_flexgen(const char *name,
 				const char **parent_names, u8 num_parents,
 				void __iomem *reg, spinlock_t *lock, u32 idx,
 				unsigned long flexgen_flags) {
@@ -190,7 +190,7 @@ struct clk *clk_register_flexgen(const char *name,
 
 	init.name = name;
 	init.ops = &flexgen_ops;
-	init.flags = CLK_IS_BASIC | flexgen_flags;
+	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE | flexgen_flags;
 	init.parent_names = parent_names;
 	init.num_parents = num_parents;
 
@@ -245,7 +245,7 @@ static const char ** __init flexgen_get_parents(struct device_node *np,
 	const char **parents;
 	int nparents, i;
 
-	nparents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	nparents = of_clk_get_parent_count(np);
 	if (WARN_ON(nparents <= 0))
 		return NULL;
 
@@ -260,7 +260,7 @@ static const char ** __init flexgen_get_parents(struct device_node *np,
 	return parents;
 }
 
-void __init st_of_flexgen_setup(struct device_node *np)
+static void __init st_of_flexgen_setup(struct device_node *np)
 {
 	struct device_node *pnode;
 	void __iomem *reg;
@@ -303,6 +303,8 @@ void __init st_of_flexgen_setup(struct device_node *np)
 	if (!rlock)
 		goto err;
 
+	spin_lock_init(rlock);
+
 	for (i = 0; i < clk_data->clk_num; i++) {
 		struct clk *clk;
 		const char *clk_name;
diff --git a/drivers/clk/st/clkgen-fsyn.c b/drivers/clk/st/clkgen-fsyn.c
index a917c4c7eaa9..d9eb2e1d8471 100644
--- a/drivers/clk/st/clkgen-fsyn.c
+++ b/drivers/clk/st/clkgen-fsyn.c
@@ -340,7 +340,7 @@ static const struct clkgen_quadfs_data st_fs660c32_C_407 = {
 		    CLKGEN_FIELD(0x30c, 0xf, 20),
 		    CLKGEN_FIELD(0x310, 0xf, 20) },
 	.lockstatus_present = true,
-	.lock_status = CLKGEN_FIELD(0x2A0, 0x1, 24),
+	.lock_status = CLKGEN_FIELD(0x2f0, 0x1, 24),
 	.powerup_polarity = 1,
 	.standby_polarity = 1,
 	.pll_ops	= &st_quadfs_pll_c32_ops,
@@ -489,10 +489,10 @@ static int quadfs_pll_is_enabled(struct clk_hw *hw)
 	struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
 	u32 npda = CLKGEN_READ(pll, npda);
 
-	return !!npda;
+	return pll->data->powerup_polarity ? !npda : !!npda;
 }
 
-int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs,
+static int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs,
 			   unsigned long *rate)
 {
 	unsigned long nd = fs->ndiv + 16; /* ndiv value */
@@ -519,7 +519,7 @@ static unsigned long quadfs_pll_fs660c32_recalc_rate(struct clk_hw *hw,
 	return rate;
 }
 
-int clk_fs660c32_vco_get_params(unsigned long input,
+static int clk_fs660c32_vco_get_params(unsigned long input,
 				unsigned long output, struct stm_fs *fs)
 {
 /* Formula
@@ -635,7 +635,7 @@ static struct clk * __init st_clk_register_quadfs_pll(
 
 	init.name = name;
 	init.ops = quadfs->pll_ops;
-	init.flags = CLK_IS_BASIC;
+	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
 	init.parent_names = &parent_name;
 	init.num_parents = 1;
 
@@ -774,7 +774,7 @@ static void quadfs_fsynth_disable(struct clk_hw *hw)
 	if (fs->lock)
 		spin_lock_irqsave(fs->lock, flags);
 
-	CLKGEN_WRITE(fs, nsb[fs->chan], !fs->data->standby_polarity);
+	CLKGEN_WRITE(fs, nsb[fs->chan], fs->data->standby_polarity);
 
 	if (fs->lock)
 		spin_unlock_irqrestore(fs->lock, flags);
@@ -1082,10 +1082,6 @@ static const struct of_device_id quadfs_of_match[] = {
 		.compatible = "st,stih407-quadfs660-D",
 		.data = &st_fs660c32_D_407
 	},
-	{
-		.compatible = "st,stih407-quadfs660-D",
-		.data = (void *)&st_fs660c32_D_407
-	},
 	{}
 };
 
diff --git a/drivers/clk/st/clkgen-mux.c b/drivers/clk/st/clkgen-mux.c
index fdcff10f6d30..717c4a91a17b 100644
--- a/drivers/clk/st/clkgen-mux.c
+++ b/drivers/clk/st/clkgen-mux.c
@@ -26,7 +26,7 @@ static const char ** __init clkgen_mux_get_parents(struct device_node *np,
 	const char **parents;
 	int nparents, i;
 
-	nparents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	nparents = of_clk_get_parent_count(np);
 	if (WARN_ON(nparents <= 0))
 		return ERR_PTR(-EINVAL);
 
@@ -131,7 +131,7 @@ static int clkgena_divmux_is_enabled(struct clk_hw *hw)
 	return (s8)clk_mux_ops.get_parent(mux_hw) > 0;
 }
 
-u8 clkgena_divmux_get_parent(struct clk_hw *hw)
+static u8 clkgena_divmux_get_parent(struct clk_hw *hw)
 {
 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 	struct clk_hw *mux_hw = &genamux->mux.hw;
@@ -168,7 +168,7 @@ static int clkgena_divmux_set_parent(struct clk_hw *hw, u8 index)
 	return 0;
 }
 
-unsigned long clkgena_divmux_recalc_rate(struct clk_hw *hw,
+static unsigned long clkgena_divmux_recalc_rate(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
@@ -215,7 +215,7 @@ static const struct clk_ops clkgena_divmux_ops = {
 /**
  * clk_register_genamux - register a genamux clock with the clock framework
  */
-struct clk *clk_register_genamux(const char *name,
+static struct clk *clk_register_genamux(const char *name,
 				const char **parent_names, u8 num_parents,
 				void __iomem *reg,
 				const struct clkgena_divmux_data *muxdata,
@@ -237,7 +237,7 @@ struct clk *clk_register_genamux(const char *name,
 
 	init.name = name;
 	init.ops = &clkgena_divmux_ops;
-	init.flags = CLK_IS_BASIC;
+	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
 	init.parent_names = parent_names;
 	init.num_parents = num_parents;
 
@@ -385,7 +385,7 @@ static void __iomem * __init clkgen_get_register_base(
 	return reg;
 }
 
-void __init st_of_clkgena_divmux_setup(struct device_node *np)
+static void __init st_of_clkgena_divmux_setup(struct device_node *np)
 {
 	const struct of_device_id *match;
 	const struct clkgena_divmux_data *data;
@@ -485,7 +485,7 @@ static const struct of_device_id clkgena_prediv_of_match[] = {
 	{}
 };
 
-void __init st_of_clkgena_prediv_setup(struct device_node *np)
+static void __init st_of_clkgena_prediv_setup(struct device_node *np)
 {
 	const struct of_device_id *match;
 	void __iomem *reg;
@@ -513,7 +513,8 @@ void __init st_of_clkgena_prediv_setup(struct device_node *np)
 					  0, &clk_name))
 		return;
 
-	clk = clk_register_divider_table(NULL, clk_name, parent_name, 0,
+	clk = clk_register_divider_table(NULL, clk_name, parent_name,
+					 CLK_GET_RATE_NOCACHE,
 					 reg + data->offset, data->shift, 1,
 					 0, data->table, NULL);
 	if (IS_ERR(clk))
@@ -582,7 +583,7 @@ static struct clkgen_mux_data stih416_a9_mux_data = {
 };
 static struct clkgen_mux_data stih407_a9_mux_data = {
 	.offset = 0x1a4,
-	.shift = 1,
+	.shift = 0,
 	.width = 2,
 };
 
@@ -622,7 +623,7 @@ static const struct of_device_id mux_of_match[] = {
 	{}
 };
 
-void __init st_of_clkgen_mux_setup(struct device_node *np)
+static void __init st_of_clkgen_mux_setup(struct device_node *np)
 {
 	const struct of_device_id *match;
 	struct clk *clk;
@@ -699,7 +700,7 @@ static const struct of_device_id vcc_of_match[] = {
 	{}
 };
 
-void __init st_of_clkgen_vcc_setup(struct device_node *np)
+static void __init st_of_clkgen_vcc_setup(struct device_node *np)
 {
 	const struct of_device_id *match;
 	void __iomem *reg;
@@ -786,7 +787,8 @@ void __init st_of_clkgen_vcc_setup(struct device_node *np)
 					     &mux->hw, &clk_mux_ops,
 					     &div->hw, &clk_divider_ops,
 					     &gate->hw, &clk_gate_ops,
-					     data->clk_flags);
+					     data->clk_flags |
+					     CLK_GET_RATE_NOCACHE);
 		if (IS_ERR(clk)) {
 			kfree(gate);
 			kfree(div);
diff --git a/drivers/clk/st/clkgen-pll.c b/drivers/clk/st/clkgen-pll.c
index d204ba85db3a..72d1c27eaffa 100644
--- a/drivers/clk/st/clkgen-pll.c
+++ b/drivers/clk/st/clkgen-pll.c
@@ -270,7 +270,7 @@ static int clkgen_pll_is_enabled(struct clk_hw *hw)
 	return !poweroff;
 }
 
-unsigned long recalc_stm_pll800c65(struct clk_hw *hw,
+static unsigned long recalc_stm_pll800c65(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -297,7 +297,7 @@ unsigned long recalc_stm_pll800c65(struct clk_hw *hw,
 
 }
 
-unsigned long recalc_stm_pll1600c65(struct clk_hw *hw,
+static unsigned long recalc_stm_pll1600c65(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -321,7 +321,7 @@ unsigned long recalc_stm_pll1600c65(struct clk_hw *hw,
 	return rate;
 }
 
-unsigned long recalc_stm_pll3200c32(struct clk_hw *hw,
+static unsigned long recalc_stm_pll3200c32(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -343,7 +343,7 @@ unsigned long recalc_stm_pll3200c32(struct clk_hw *hw,
 	return rate;
 }
 
-unsigned long recalc_stm_pll1200c32(struct clk_hw *hw,
+static unsigned long recalc_stm_pll1200c32(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -406,7 +406,7 @@ static struct clk * __init clkgen_pll_register(const char *parent_name,
 	init.name = clk_name;
 	init.ops = pll_data->ops;
 
-	init.flags = CLK_IS_BASIC;
+	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
 	init.parent_names = &parent_name;
 	init.num_parents  = 1;
 
@@ -544,7 +544,7 @@ CLK_OF_DECLARE(clkgena_c65_plls,
 	       "st,clkgena-plls-c65", clkgena_c65_pll_setup);
 
 static struct clk * __init clkgen_odf_register(const char *parent_name,
-					       void * __iomem reg,
+					       void __iomem *reg,
 					       struct clkgen_pll_data *pll_data,
 					       int odf,
 					       spinlock_t *odf_lock,
diff --git a/drivers/clk/sunxi/clk-mod0.c b/drivers/clk/sunxi/clk-mod0.c
index ec8f5a1fca09..9d028aec58e5 100644
--- a/drivers/clk/sunxi/clk-mod0.c
+++ b/drivers/clk/sunxi/clk-mod0.c
@@ -128,7 +128,7 @@ static struct platform_driver sun4i_a10_mod0_clk_driver = {
 	},
 	.probe = sun4i_a10_mod0_clk_probe,
 };
-module_platform_driver(sun4i_a10_mod0_clk_driver);
+builtin_platform_driver(sun4i_a10_mod0_clk_driver);
 
 static const struct factors_data sun9i_a80_mod0_data __initconst = {
 	.enable = 31,
diff --git a/drivers/clk/sunxi/clk-sun9i-core.c b/drivers/clk/sunxi/clk-sun9i-core.c
index d8da77d72861..887f4ea161bb 100644
--- a/drivers/clk/sunxi/clk-sun9i-core.c
+++ b/drivers/clk/sunxi/clk-sun9i-core.c
@@ -93,7 +93,7 @@ static void __init sun9i_a80_pll4_setup(struct device_node *node)
 	void __iomem *reg;
 
 	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
-	if (!reg) {
+	if (IS_ERR(reg)) {
 		pr_err("Could not get registers for a80-pll4-clk: %s\n",
 		       node->name);
 		return;
@@ -154,7 +154,7 @@ static void __init sun9i_a80_gt_setup(struct device_node *node)
 	struct clk *gt;
 
 	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
-	if (!reg) {
+	if (IS_ERR(reg)) {
 		pr_err("Could not get registers for a80-gt-clk: %s\n",
 		       node->name);
 		return;
@@ -218,7 +218,7 @@ static void __init sun9i_a80_ahb_setup(struct device_node *node)
 	void __iomem *reg;
 
 	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
-	if (!reg) {
+	if (IS_ERR(reg)) {
 		pr_err("Could not get registers for a80-ahb-clk: %s\n",
 		       node->name);
 		return;
@@ -244,7 +244,7 @@ static void __init sun9i_a80_apb0_setup(struct device_node *node)
 	void __iomem *reg;
 
 	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
-	if (!reg) {
+	if (IS_ERR(reg)) {
 		pr_err("Could not get registers for a80-apb0-clk: %s\n",
 		       node->name);
 		return;
@@ -310,7 +310,7 @@ static void __init sun9i_a80_apb1_setup(struct device_node *node)
 	void __iomem *reg;
 
 	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
-	if (!reg) {
+	if (IS_ERR(reg)) {
 		pr_err("Could not get registers for a80-apb1-clk: %s\n",
 		       node->name);
 		return;
diff --git a/drivers/clk/sunxi/clk-sunxi.c b/drivers/clk/sunxi/clk-sunxi.c
index 7e1e2bd189b6..abf7b37faf73 100644
--- a/drivers/clk/sunxi/clk-sunxi.c
+++ b/drivers/clk/sunxi/clk-sunxi.c
@@ -198,6 +198,8 @@ static void __init sun6i_ahb1_clk_setup(struct device_node *node)
 	int i = 0;
 
 	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
+	if (IS_ERR(reg))
+		return;
 
 	/* we have a mux, we will have >1 parents */
 	while (i < SUN6I_AHB1_MAX_PARENTS &&
@@ -1389,6 +1391,7 @@ 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(sun6i_a31s_clk_init, "allwinner,sun6i-a31s", sun6i_init_clocks);
 CLK_OF_DECLARE(sun8i_a23_clk_init, "allwinner,sun8i-a23", sun6i_init_clocks);
+CLK_OF_DECLARE(sun8i_a33_clk_init, "allwinner,sun8i-a33", sun6i_init_clocks);
 
 static void __init sun9i_init_clocks(struct device_node *node)
 {
diff --git a/drivers/clk/sunxi/clk-usb.c b/drivers/clk/sunxi/clk-usb.c
index a86ed2f8d7af..3a25f9588e67 100644
--- a/drivers/clk/sunxi/clk-usb.c
+++ b/drivers/clk/sunxi/clk-usb.c
@@ -204,6 +204,17 @@ static void __init sun6i_a31_usb_setup(struct device_node *node)
 }
 CLK_OF_DECLARE(sun6i_a31_usb, "allwinner,sun6i-a31-usb-clk", sun6i_a31_usb_setup);
 
+static const struct usb_clk_data sun8i_a23_usb_clk_data __initconst = {
+	.clk_mask = BIT(16) | BIT(11) | BIT(10) | BIT(9) | BIT(8),
+	.reset_mask = BIT(2) | BIT(1) | BIT(0),
+};
+
+static void __init sun8i_a23_usb_setup(struct device_node *node)
+{
+	sunxi_usb_clk_setup(node, &sun8i_a23_usb_clk_data, &sun4i_a10_usb_lock);
+}
+CLK_OF_DECLARE(sun8i_a23_usb, "allwinner,sun8i-a23-usb-clk", sun8i_a23_usb_setup);
+
 static const struct usb_clk_data sun9i_a80_usb_mod_data __initconst = {
 	.clk_mask = BIT(6) | BIT(5) | BIT(4) | BIT(3) | BIT(2) | BIT(1),
 	.reset_mask = BIT(19) | BIT(18) | BIT(17),
diff --git a/drivers/clk/tegra/Kconfig b/drivers/clk/tegra/Kconfig
new file mode 100644
index 000000000000..1ba30d1e14f2
--- /dev/null
+++ b/drivers/clk/tegra/Kconfig
@@ -0,0 +1,3 @@
+config TEGRA_CLK_EMC
+	def_bool y
+	depends on TEGRA124_EMC
diff --git a/drivers/clk/tegra/Makefile b/drivers/clk/tegra/Makefile
index edb8358fa6ce..aec862ba7a17 100644
--- a/drivers/clk/tegra/Makefile
+++ b/drivers/clk/tegra/Makefile
@@ -11,6 +11,7 @@ obj-y					+= clk-tegra-periph.o
 obj-y					+= clk-tegra-pmc.o
 obj-y					+= clk-tegra-fixed.o
 obj-y					+= clk-tegra-super-gen4.o
+obj-$(CONFIG_TEGRA_CLK_EMC)		+= clk-emc.o
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += clk-tegra20.o
 obj-$(CONFIG_ARCH_TEGRA_3x_SOC)         += clk-tegra30.o
 obj-$(CONFIG_ARCH_TEGRA_114_SOC)	+= clk-tegra114.o
diff --git a/drivers/clk/tegra/clk-emc.c b/drivers/clk/tegra/clk-emc.c
new file mode 100644
index 000000000000..7649685c86bc
--- /dev/null
+++ b/drivers/clk/tegra/clk-emc.c
@@ -0,0 +1,538 @@
+/*
+ * drivers/clk/tegra/clk-emc.c
+ *
+ * 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/clk-provider.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/sort.h>
+#include <linux/string.h>
+
+#include <soc/tegra/fuse.h>
+#include <soc/tegra/emc.h>
+
+#include "clk.h"
+
+#define CLK_SOURCE_EMC 0x19c
+
+#define CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR_SHIFT 0
+#define CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR_MASK 0xff
+#define CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR(x) (((x) & CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR_MASK) << \
+					      CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR_SHIFT)
+
+#define CLK_SOURCE_EMC_EMC_2X_CLK_SRC_SHIFT 29
+#define CLK_SOURCE_EMC_EMC_2X_CLK_SRC_MASK 0x7
+#define CLK_SOURCE_EMC_EMC_2X_CLK_SRC(x) (((x) & CLK_SOURCE_EMC_EMC_2X_CLK_SRC_MASK) << \
+					  CLK_SOURCE_EMC_EMC_2X_CLK_SRC_SHIFT)
+
+static const char * const emc_parent_clk_names[] = {
+	"pll_m", "pll_c", "pll_p", "clk_m", "pll_m_ud",
+	"pll_c2", "pll_c3", "pll_c_ud"
+};
+
+/*
+ * List of clock sources for various parents the EMC clock can have.
+ * When we change the timing to a timing with a parent that has the same
+ * clock source as the current parent, we must first change to a backup
+ * timing that has a different clock source.
+ */
+
+#define EMC_SRC_PLL_M 0
+#define EMC_SRC_PLL_C 1
+#define EMC_SRC_PLL_P 2
+#define EMC_SRC_CLK_M 3
+#define EMC_SRC_PLL_C2 4
+#define EMC_SRC_PLL_C3 5
+
+static const char emc_parent_clk_sources[] = {
+	EMC_SRC_PLL_M, EMC_SRC_PLL_C, EMC_SRC_PLL_P, EMC_SRC_CLK_M,
+	EMC_SRC_PLL_M, EMC_SRC_PLL_C2, EMC_SRC_PLL_C3, EMC_SRC_PLL_C
+};
+
+struct emc_timing {
+	unsigned long rate, parent_rate;
+	u8 parent_index;
+	struct clk *parent;
+	u32 ram_code;
+};
+
+struct tegra_clk_emc {
+	struct clk_hw hw;
+	void __iomem *clk_regs;
+	struct clk *prev_parent;
+	bool changing_timing;
+
+	struct device_node *emc_node;
+	struct tegra_emc *emc;
+
+	int num_timings;
+	struct emc_timing *timings;
+	spinlock_t *lock;
+};
+
+/* Common clock framework callback implementations */
+
+static unsigned long emc_recalc_rate(struct clk_hw *hw,
+				     unsigned long parent_rate)
+{
+	struct tegra_clk_emc *tegra;
+	u32 val, div;
+
+	tegra = container_of(hw, struct tegra_clk_emc, hw);
+
+	/*
+	 * CCF wrongly assumes that the parent won't change during set_rate,
+	 * so get the parent rate explicitly.
+	 */
+	parent_rate = __clk_get_rate(__clk_get_parent(hw->clk));
+
+	val = readl(tegra->clk_regs + CLK_SOURCE_EMC);
+	div = val & CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR_MASK;
+
+	return parent_rate / (div + 2) * 2;
+}
+
+/*
+ * Rounds up unless no higher rate exists, in which case down. This way is
+ * safer since things have EMC rate floors. Also don't touch parent_rate
+ * since we don't want the CCF to play with our parent clocks.
+ */
+static long emc_determine_rate(struct clk_hw *hw, unsigned long rate,
+			       unsigned long min_rate,
+			       unsigned long max_rate,
+			       unsigned long *best_parent_rate,
+			       struct clk_hw **best_parent_hw)
+{
+	struct tegra_clk_emc *tegra;
+	u8 ram_code = tegra_read_ram_code();
+	struct emc_timing *timing = NULL;
+	int i;
+
+	tegra = container_of(hw, struct tegra_clk_emc, hw);
+
+	for (i = 0; i < tegra->num_timings; i++) {
+		if (tegra->timings[i].ram_code != ram_code)
+			continue;
+
+		timing = tegra->timings + i;
+
+		if (timing->rate > max_rate) {
+			i = min(i, 1);
+			return tegra->timings[i - 1].rate;
+		}
+
+		if (timing->rate < min_rate)
+			continue;
+
+		if (timing->rate >= rate)
+			return timing->rate;
+	}
+
+	if (timing)
+		return timing->rate;
+
+	return __clk_get_rate(hw->clk);
+}
+
+static u8 emc_get_parent(struct clk_hw *hw)
+{
+	struct tegra_clk_emc *tegra;
+	u32 val;
+
+	tegra = container_of(hw, struct tegra_clk_emc, hw);
+
+	val = readl(tegra->clk_regs + CLK_SOURCE_EMC);
+
+	return (val >> CLK_SOURCE_EMC_EMC_2X_CLK_SRC_SHIFT)
+		& CLK_SOURCE_EMC_EMC_2X_CLK_SRC_MASK;
+}
+
+static struct tegra_emc *emc_ensure_emc_driver(struct tegra_clk_emc *tegra)
+{
+	struct platform_device *pdev;
+
+	if (tegra->emc)
+		return tegra->emc;
+
+	if (!tegra->emc_node)
+		return NULL;
+
+	pdev = of_find_device_by_node(tegra->emc_node);
+	if (!pdev) {
+		pr_err("%s: could not get external memory controller\n",
+		       __func__);
+		return NULL;
+	}
+
+	of_node_put(tegra->emc_node);
+	tegra->emc_node = NULL;
+
+	tegra->emc = platform_get_drvdata(pdev);
+	if (!tegra->emc) {
+		pr_err("%s: cannot find EMC driver\n", __func__);
+		return NULL;
+	}
+
+	return tegra->emc;
+}
+
+static int emc_set_timing(struct tegra_clk_emc *tegra,
+			  struct emc_timing *timing)
+{
+	int err;
+	u8 div;
+	u32 car_value;
+	unsigned long flags = 0;
+	struct tegra_emc *emc = emc_ensure_emc_driver(tegra);
+
+	if (!emc)
+		return -ENOENT;
+
+	pr_debug("going to rate %ld prate %ld p %s\n", timing->rate,
+		 timing->parent_rate, __clk_get_name(timing->parent));
+
+	if (emc_get_parent(&tegra->hw) == timing->parent_index &&
+	    clk_get_rate(timing->parent) != timing->parent_rate) {
+		BUG();
+		return -EINVAL;
+	}
+
+	tegra->changing_timing = true;
+
+	err = clk_set_rate(timing->parent, timing->parent_rate);
+	if (err) {
+		pr_err("cannot change parent %s rate to %ld: %d\n",
+		       __clk_get_name(timing->parent), timing->parent_rate,
+		       err);
+
+		return err;
+	}
+
+	err = clk_prepare_enable(timing->parent);
+	if (err) {
+		pr_err("cannot enable parent clock: %d\n", err);
+		return err;
+	}
+
+	div = timing->parent_rate / (timing->rate / 2) - 2;
+
+	err = tegra_emc_prepare_timing_change(emc, timing->rate);
+	if (err)
+		return err;
+
+	spin_lock_irqsave(tegra->lock, flags);
+
+	car_value = readl(tegra->clk_regs + CLK_SOURCE_EMC);
+
+	car_value &= ~CLK_SOURCE_EMC_EMC_2X_CLK_SRC(~0);
+	car_value |= CLK_SOURCE_EMC_EMC_2X_CLK_SRC(timing->parent_index);
+
+	car_value &= ~CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR(~0);
+	car_value |= CLK_SOURCE_EMC_EMC_2X_CLK_DIVISOR(div);
+
+	writel(car_value, tegra->clk_regs + CLK_SOURCE_EMC);
+
+	spin_unlock_irqrestore(tegra->lock, flags);
+
+	tegra_emc_complete_timing_change(emc, timing->rate);
+
+	clk_hw_reparent(&tegra->hw, __clk_get_hw(timing->parent));
+	clk_disable_unprepare(tegra->prev_parent);
+
+	tegra->prev_parent = timing->parent;
+	tegra->changing_timing = false;
+
+	return 0;
+}
+
+/*
+ * Get backup timing to use as an intermediate step when a change between
+ * two timings with the same clock source has been requested. First try to
+ * find a timing with a higher clock rate to avoid a rate below any set rate
+ * floors. If that is not possible, find a lower rate.
+ */
+static struct emc_timing *get_backup_timing(struct tegra_clk_emc *tegra,
+					    int timing_index)
+{
+	int i;
+	u32 ram_code = tegra_read_ram_code();
+	struct emc_timing *timing;
+
+	for (i = timing_index+1; i < tegra->num_timings; i++) {
+		timing = tegra->timings + i;
+		if (timing->ram_code != ram_code)
+			continue;
+
+		if (emc_parent_clk_sources[timing->parent_index] !=
+		    emc_parent_clk_sources[
+		      tegra->timings[timing_index].parent_index])
+			return timing;
+	}
+
+	for (i = timing_index-1; i >= 0; --i) {
+		timing = tegra->timings + i;
+		if (timing->ram_code != ram_code)
+			continue;
+
+		if (emc_parent_clk_sources[timing->parent_index] !=
+		    emc_parent_clk_sources[
+		      tegra->timings[timing_index].parent_index])
+			return timing;
+	}
+
+	return NULL;
+}
+
+static int emc_set_rate(struct clk_hw *hw, unsigned long rate,
+			unsigned long parent_rate)
+{
+	struct tegra_clk_emc *tegra;
+	struct emc_timing *timing = NULL;
+	int i, err;
+	u32 ram_code = tegra_read_ram_code();
+
+	tegra = container_of(hw, struct tegra_clk_emc, hw);
+
+	if (__clk_get_rate(hw->clk) == rate)
+		return 0;
+
+	/*
+	 * When emc_set_timing changes the parent rate, CCF will propagate
+	 * that downward to us, so ignore any set_rate calls while a rate
+	 * change is already going on.
+	 */
+	if (tegra->changing_timing)
+		return 0;
+
+	for (i = 0; i < tegra->num_timings; i++) {
+		if (tegra->timings[i].rate == rate &&
+		    tegra->timings[i].ram_code == ram_code) {
+			timing = tegra->timings + i;
+			break;
+		}
+	}
+
+	if (!timing) {
+		pr_err("cannot switch to rate %ld without emc table\n", rate);
+		return -EINVAL;
+	}
+
+	if (emc_parent_clk_sources[emc_get_parent(hw)] ==
+	    emc_parent_clk_sources[timing->parent_index] &&
+	    clk_get_rate(timing->parent) != timing->parent_rate) {
+		/*
+		 * Parent clock source not changed but parent rate has changed,
+		 * need to temporarily switch to another parent
+		 */
+
+		struct emc_timing *backup_timing;
+
+		backup_timing = get_backup_timing(tegra, i);
+		if (!backup_timing) {
+			pr_err("cannot find backup timing\n");
+			return -EINVAL;
+		}
+
+		pr_debug("using %ld as backup rate when going to %ld\n",
+			 backup_timing->rate, rate);
+
+		err = emc_set_timing(tegra, backup_timing);
+		if (err) {
+			pr_err("cannot set backup timing: %d\n", err);
+			return err;
+		}
+	}
+
+	return emc_set_timing(tegra, timing);
+}
+
+/* Initialization and deinitialization */
+
+static int load_one_timing_from_dt(struct tegra_clk_emc *tegra,
+				   struct emc_timing *timing,
+				   struct device_node *node)
+{
+	int err, i;
+	u32 tmp;
+
+	err = of_property_read_u32(node, "clock-frequency", &tmp);
+	if (err) {
+		pr_err("timing %s: failed to read rate\n", node->full_name);
+		return err;
+	}
+
+	timing->rate = tmp;
+
+	err = of_property_read_u32(node, "nvidia,parent-clock-frequency", &tmp);
+	if (err) {
+		pr_err("timing %s: failed to read parent rate\n",
+		       node->full_name);
+		return err;
+	}
+
+	timing->parent_rate = tmp;
+
+	timing->parent = of_clk_get_by_name(node, "emc-parent");
+	if (IS_ERR(timing->parent)) {
+		pr_err("timing %s: failed to get parent clock\n",
+		       node->full_name);
+		return PTR_ERR(timing->parent);
+	}
+
+	timing->parent_index = 0xff;
+	for (i = 0; i < ARRAY_SIZE(emc_parent_clk_names); i++) {
+		if (!strcmp(emc_parent_clk_names[i],
+			    __clk_get_name(timing->parent))) {
+			timing->parent_index = i;
+			break;
+		}
+	}
+	if (timing->parent_index == 0xff) {
+		pr_err("timing %s: %s is not a valid parent\n",
+		       node->full_name, __clk_get_name(timing->parent));
+		clk_put(timing->parent);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cmp_timings(const void *_a, const void *_b)
+{
+	const struct emc_timing *a = _a;
+	const struct emc_timing *b = _b;
+
+	if (a->rate < b->rate)
+		return -1;
+	else if (a->rate == b->rate)
+		return 0;
+	else
+		return 1;
+}
+
+static int load_timings_from_dt(struct tegra_clk_emc *tegra,
+				struct device_node *node,
+				u32 ram_code)
+{
+	struct device_node *child;
+	int child_count = of_get_child_count(node);
+	int i = 0, err;
+
+	tegra->timings = kcalloc(child_count, sizeof(struct emc_timing),
+				 GFP_KERNEL);
+	if (!tegra->timings)
+		return -ENOMEM;
+
+	tegra->num_timings = child_count;
+
+	for_each_child_of_node(node, child) {
+		struct emc_timing *timing = tegra->timings + (i++);
+
+		err = load_one_timing_from_dt(tegra, timing, child);
+		if (err)
+			return err;
+
+		timing->ram_code = ram_code;
+	}
+
+	sort(tegra->timings, tegra->num_timings, sizeof(struct emc_timing),
+	     cmp_timings, NULL);
+
+	return 0;
+}
+
+static const struct clk_ops tegra_clk_emc_ops = {
+	.recalc_rate = emc_recalc_rate,
+	.determine_rate = emc_determine_rate,
+	.set_rate = emc_set_rate,
+	.get_parent = emc_get_parent,
+};
+
+struct clk *tegra_clk_register_emc(void __iomem *base, struct device_node *np,
+				   spinlock_t *lock)
+{
+	struct tegra_clk_emc *tegra;
+	struct clk_init_data init;
+	struct device_node *node;
+	u32 node_ram_code;
+	struct clk *clk;
+	int err;
+
+	tegra = kcalloc(1, sizeof(*tegra), GFP_KERNEL);
+	if (!tegra)
+		return ERR_PTR(-ENOMEM);
+
+	tegra->clk_regs = base;
+	tegra->lock = lock;
+
+	tegra->num_timings = 0;
+
+	for_each_child_of_node(np, node) {
+		err = of_property_read_u32(node, "nvidia,ram-code",
+					   &node_ram_code);
+		if (err) {
+			of_node_put(node);
+			continue;
+		}
+
+		/*
+		 * Store timings for all ram codes as we cannot read the
+		 * fuses until the apbmisc driver is loaded.
+		 */
+		err = load_timings_from_dt(tegra, node, node_ram_code);
+		if (err)
+			return ERR_PTR(err);
+		of_node_put(node);
+		break;
+	}
+
+	if (tegra->num_timings == 0)
+		pr_warn("%s: no memory timings registered\n", __func__);
+
+	tegra->emc_node = of_parse_phandle(np,
+			"nvidia,external-memory-controller", 0);
+	if (!tegra->emc_node)
+		pr_warn("%s: couldn't find node for EMC driver\n", __func__);
+
+	init.name = "emc";
+	init.ops = &tegra_clk_emc_ops;
+	init.flags = 0;
+	init.parent_names = emc_parent_clk_names;
+	init.num_parents = ARRAY_SIZE(emc_parent_clk_names);
+
+	tegra->hw.init = &init;
+
+	clk = clk_register(NULL, &tegra->hw);
+	if (IS_ERR(clk))
+		return clk;
+
+	tegra->prev_parent = clk_get_parent_by_index(
+		tegra->hw.clk, emc_get_parent(&tegra->hw));
+	tegra->changing_timing = false;
+
+	/* Allow debugging tools to see the EMC clock */
+	clk_register_clkdev(clk, "emc", "tegra-clk-debug");
+
+	clk_prepare_enable(clk);
+
+	return clk;
+};
diff --git a/drivers/clk/tegra/clk-tegra124.c b/drivers/clk/tegra/clk-tegra124.c
index 11f857cd5f6a..e8cca3eac007 100644
--- a/drivers/clk/tegra/clk-tegra124.c
+++ b/drivers/clk/tegra/clk-tegra124.c
@@ -152,11 +152,6 @@ static unsigned long tegra124_input_freq[] = {
 	[12] = 260000000,
 };
 
-static const char *mux_pllmcp_clkm[] = {
-	"pll_m", "pll_c", "pll_p", "clk_m", "pll_m_ud", "pll_c2", "pll_c3",
-};
-#define mux_pllmcp_clkm_idx NULL
-
 static struct div_nmp pllxc_nmp = {
 	.divm_shift = 0,
 	.divm_width = 8,
@@ -791,7 +786,6 @@ static struct tegra_clk tegra124_clks[tegra_clk_max] __initdata = {
 	[tegra_clk_i2c2] = { .dt_id = TEGRA124_CLK_I2C2, .present = true },
 	[tegra_clk_uartc] = { .dt_id = TEGRA124_CLK_UARTC, .present = true },
 	[tegra_clk_mipi_cal] = { .dt_id = TEGRA124_CLK_MIPI_CAL, .present = true },
-	[tegra_clk_emc] = { .dt_id = TEGRA124_CLK_EMC, .present = true },
 	[tegra_clk_usb2] = { .dt_id = TEGRA124_CLK_USB2, .present = true },
 	[tegra_clk_usb3] = { .dt_id = TEGRA124_CLK_USB3, .present = true },
 	[tegra_clk_vde_8] = { .dt_id = TEGRA124_CLK_VDE, .present = true },
@@ -1127,13 +1121,7 @@ static __init void tegra124_periph_clk_init(void __iomem *clk_base,
 					     periph_clk_enb_refcnt);
 	clks[TEGRA124_CLK_DSIB] = clk;
 
-	/* emc mux */
-	clk = clk_register_mux(NULL, "emc_mux", mux_pllmcp_clkm,
-			       ARRAY_SIZE(mux_pllmcp_clkm), 0,
-			       clk_base + CLK_SOURCE_EMC,
-			       29, 3, 0, &emc_lock);
-
-	clk = tegra_clk_register_mc("mc", "emc_mux", clk_base + CLK_SOURCE_EMC,
+	clk = tegra_clk_register_mc("mc", "emc", clk_base + CLK_SOURCE_EMC,
 				    &emc_lock);
 	clks[TEGRA124_CLK_MC] = clk;
 
@@ -1389,7 +1377,6 @@ static struct tegra_clk_init_table common_init_table[] __initdata = {
 	{TEGRA124_CLK_XUSB_HOST_SRC, TEGRA124_CLK_PLL_RE_OUT, 112000000, 0},
 	{TEGRA124_CLK_SATA, TEGRA124_CLK_PLL_P, 104000000, 0},
 	{TEGRA124_CLK_SATA_OOB, TEGRA124_CLK_PLL_P, 204000000, 0},
-	{TEGRA124_CLK_EMC, TEGRA124_CLK_CLK_MAX, 0, 1},
 	{TEGRA124_CLK_MSELECT, TEGRA124_CLK_CLK_MAX, 0, 1},
 	{TEGRA124_CLK_CSITE, TEGRA124_CLK_CLK_MAX, 0, 1},
 	{TEGRA124_CLK_TSENSOR, TEGRA124_CLK_CLK_M, 400000, 0},
@@ -1513,6 +1500,10 @@ static void __init tegra124_132_clock_init_post(struct device_node *np)
 	tegra_super_clk_gen4_init(clk_base, pmc_base, tegra124_clks,
 				  &pll_x_params);
 	tegra_add_of_provider(np);
+
+	clks[TEGRA124_CLK_EMC] = tegra_clk_register_emc(clk_base, np,
+							&emc_lock);
+
 	tegra_register_devclks(devclks, ARRAY_SIZE(devclks));
 
 	tegra_cpu_car_ops = &tegra124_cpu_car_ops;
diff --git a/drivers/clk/tegra/clk-tegra30.c b/drivers/clk/tegra/clk-tegra30.c
index 4b26509fc218..0af3e834dd24 100644
--- a/drivers/clk/tegra/clk-tegra30.c
+++ b/drivers/clk/tegra/clk-tegra30.c
@@ -679,7 +679,7 @@ static struct tegra_devclk devclks[] __initdata = {
 	{ .dev_id = "tegra30-dam.1", .dt_id = TEGRA30_CLK_DAM1 },
 	{ .dev_id = "tegra30-dam.2", .dt_id = TEGRA30_CLK_DAM2 },
 	{ .con_id = "hda", .dev_id = "tegra30-hda", .dt_id = TEGRA30_CLK_HDA },
-	{ .con_id = "hda2codec", .dev_id = "tegra30-hda", .dt_id = TEGRA30_CLK_HDA2CODEC_2X },
+	{ .con_id = "hda2codec_2x", .dev_id = "tegra30-hda", .dt_id = TEGRA30_CLK_HDA2CODEC_2X },
 	{ .dev_id = "spi_tegra.0", .dt_id = TEGRA30_CLK_SBC1 },
 	{ .dev_id = "spi_tegra.1", .dt_id = TEGRA30_CLK_SBC2 },
 	{ .dev_id = "spi_tegra.2", .dt_id = TEGRA30_CLK_SBC3 },
diff --git a/drivers/clk/tegra/clk.h b/drivers/clk/tegra/clk.h
index d6ac00647faf..75ddc8ff8bd4 100644
--- a/drivers/clk/tegra/clk.h
+++ b/drivers/clk/tegra/clk.h
@@ -623,6 +623,18 @@ void tegra_super_clk_gen4_init(void __iomem *clk_base,
 			void __iomem *pmc_base, struct tegra_clk *tegra_clks,
 			struct tegra_clk_pll_params *pll_params);
 
+#ifdef CONFIG_TEGRA_CLK_EMC
+struct clk *tegra_clk_register_emc(void __iomem *base, struct device_node *np,
+				   spinlock_t *lock);
+#else
+static inline struct clk *tegra_clk_register_emc(void __iomem *base,
+						 struct device_node *np,
+						 spinlock_t *lock)
+{
+	return NULL;
+}
+#endif
+
 void tegra114_clock_tune_cpu_trimmers_high(void);
 void tegra114_clock_tune_cpu_trimmers_low(void);
 void tegra114_clock_tune_cpu_trimmers_init(void);
diff --git a/drivers/clk/ti/clk-7xx.c b/drivers/clk/ti/clk-7xx.c
index 5d2217ae4478..63b8323df918 100644
--- a/drivers/clk/ti/clk-7xx.c
+++ b/drivers/clk/ti/clk-7xx.c
@@ -305,13 +305,14 @@ static struct ti_dt_clk dra7xx_clks[] = {
 	DT_CLK("4882c000.timer", "timer_sys_ck", "timer_sys_clk_div"),
 	DT_CLK("4882e000.timer", "timer_sys_ck", "timer_sys_clk_div"),
 	DT_CLK(NULL, "sys_clkin", "sys_clkin1"),
+	DT_CLK(NULL, "dss_deshdcp_clk", "dss_deshdcp_clk"),
 	{ .node_name = NULL },
 };
 
 int __init dra7xx_dt_clk_init(void)
 {
 	int rc;
-	struct clk *abe_dpll_mux, *sys_clkin2, *dpll_ck;
+	struct clk *abe_dpll_mux, *sys_clkin2, *dpll_ck, *hdcp_ck;
 
 	ti_dt_clocks_register(dra7xx_clks);
 
@@ -347,5 +348,10 @@ int __init dra7xx_dt_clk_init(void)
 	if (rc)
 		pr_err("%s: failed to set USB_DPLL M2 OUT\n", __func__);
 
+	hdcp_ck = clk_get_sys(NULL, "dss_deshdcp_clk");
+	rc = clk_prepare_enable(hdcp_ck);
+	if (rc)
+		pr_err("%s: failed to set dss_deshdcp_clk\n", __func__);
+
 	return rc;
 }
diff --git a/drivers/clk/ti/clk-dra7-atl.c b/drivers/clk/ti/clk-dra7-atl.c
index d86bc46b93bd..19e543a32e2b 100644
--- a/drivers/clk/ti/clk-dra7-atl.c
+++ b/drivers/clk/ti/clk-dra7-atl.c
@@ -155,7 +155,7 @@ static int atl_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }
 
-const struct clk_ops atl_clk_ops = {
+static const struct clk_ops atl_clk_ops = {
 	.enable		= atl_clk_enable,
 	.disable	= atl_clk_disable,
 	.is_enabled	= atl_clk_is_enabled,
@@ -167,7 +167,7 @@ const struct clk_ops atl_clk_ops = {
 static void __init of_dra7_atl_clock_setup(struct device_node *node)
 {
 	struct dra7_atl_desc *clk_hw = NULL;
-	struct clk_init_data init = { 0 };
+	struct clk_init_data init = { NULL };
 	const char **parent_names = NULL;
 	struct clk *clk;
 
@@ -252,6 +252,11 @@ static int of_dra7_atl_clk_probe(struct platform_device *pdev)
 		}
 
 		clk = of_clk_get_from_provider(&clkspec);
+		if (IS_ERR(clk)) {
+			pr_err("%s: failed to get atl clock %d from provider\n",
+			       __func__, i);
+			return PTR_ERR(clk);
+		}
 
 		cdesc = to_atl_desc(__clk_get_hw(clk));
 		cdesc->cinfo = cinfo;
diff --git a/drivers/clk/ti/clk.c b/drivers/clk/ti/clk.c
index 0ebe5c51062b..64bb5e8a3b8c 100644
--- a/drivers/clk/ti/clk.c
+++ b/drivers/clk/ti/clk.c
@@ -122,14 +122,14 @@ void __iomem *ti_clk_get_reg_addr(struct device_node *node, int index)
 
 	if (i == CLK_MAX_MEMMAPS) {
 		pr_err("clk-provider not found for %s!\n", node->name);
-		return ERR_PTR(-ENOENT);
+		return IOMEM_ERR_PTR(-ENOENT);
 	}
 
 	reg->index = i;
 
 	if (of_property_read_u32_index(node, "reg", index, &val)) {
 		pr_err("%s must have reg[%d]!\n", node->name, index);
-		return ERR_PTR(-EINVAL);
+		return IOMEM_ERR_PTR(-EINVAL);
 	}
 
 	reg->offset = val;
diff --git a/drivers/clk/ti/clockdomain.c b/drivers/clk/ti/clockdomain.c
index 35fe1085480c..b82ef07f3403 100644
--- a/drivers/clk/ti/clockdomain.c
+++ b/drivers/clk/ti/clockdomain.c
@@ -32,7 +32,7 @@ static void __init of_ti_clockdomain_setup(struct device_node *node)
 	int i;
 	int num_clks;
 
-	num_clks = of_count_phandle_with_args(node, "clocks", "#clock-cells");
+	num_clks = of_clk_get_parent_count(node);
 
 	for (i = 0; i < num_clks; i++) {
 		clk = of_clk_get(node, i);
diff --git a/drivers/clk/ti/dpll.c b/drivers/clk/ti/dpll.c
index 11478a501c30..2aacf7a3bcae 100644
--- a/drivers/clk/ti/dpll.c
+++ b/drivers/clk/ti/dpll.c
@@ -177,7 +177,7 @@ cleanup:
 }
 
 #if defined(CONFIG_ARCH_OMAP3) && defined(CONFIG_ATAGS)
-void __iomem *_get_reg(u8 module, u16 offset)
+static void __iomem *_get_reg(u8 module, u16 offset)
 {
 	u32 reg;
 	struct clk_omap_reg *reg_setup;
diff --git a/drivers/clk/ti/fapll.c b/drivers/clk/ti/fapll.c
index ffcd8e09e85b..730aa62454a2 100644
--- a/drivers/clk/ti/fapll.c
+++ b/drivers/clk/ti/fapll.c
@@ -621,13 +621,13 @@ static void __init ti_fapll_setup(struct device_node *node)
 
 		/* Check for hardwired audio_pll_clk1 */
 		if (is_audio_pll_clk1(freq)) {
-			freq = 0;
-			div = 0;
+			freq = NULL;
+			div = NULL;
 		} else {
 			/* Does the synthesizer have a FREQ register? */
 			v = readl_relaxed(freq);
 			if (!v)
-				freq = 0;
+				freq = NULL;
 		}
 		synth_clk = ti_fapll_synth_setup(fd, freq, div, output_instance,
 						 output_name, node->name,
diff --git a/drivers/clk/ux500/u8500_clk.c b/drivers/clk/ux500/u8500_clk.c
index 80069c370a47..4626b97b7d83 100644
--- a/drivers/clk/ux500/u8500_clk.c
+++ b/drivers/clk/ux500/u8500_clk.c
@@ -116,11 +116,12 @@ void u8500_clk_init(u32 clkrst1_base, u32 clkrst2_base, u32 clkrst3_base,
 	clk_register_clkdev(clk, NULL, "hdmi");
 	clk_register_clkdev(clk, "hdmi", "mcde");
 
-	clk = clk_reg_prcmu_gate("apeatclk", NULL, PRCMU_APEATCLK, CLK_IS_ROOT);
+	clk = clk_reg_prcmu_scalable("apeatclk", NULL, PRCMU_APEATCLK, 0,
+				     CLK_IS_ROOT|CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "apeat");
 
-	clk = clk_reg_prcmu_gate("apetraceclk", NULL, PRCMU_APETRACECLK,
-				CLK_IS_ROOT);
+	clk = clk_reg_prcmu_scalable("apetraceclk", NULL, PRCMU_APETRACECLK, 0,
+				CLK_IS_ROOT|CLK_SET_RATE_GATE);
 	clk_register_clkdev(clk, NULL, "apetrace");
 
 	clk = clk_reg_prcmu_gate("mcdeclk", NULL, PRCMU_MCDECLK, CLK_IS_ROOT);
diff --git a/drivers/clk/ux500/u8500_of_clk.c b/drivers/clk/ux500/u8500_of_clk.c
index 7b55ef89baa5..e319ef912dc6 100644
--- a/drivers/clk/ux500/u8500_of_clk.c
+++ b/drivers/clk/ux500/u8500_of_clk.c
@@ -166,8 +166,8 @@ void u8500_of_clk_init(u32 clkrst1_base, u32 clkrst2_base, u32 clkrst3_base,
 	clk = clk_reg_prcmu_gate("apeatclk", NULL, PRCMU_APEATCLK, CLK_IS_ROOT);
 	prcmu_clk[PRCMU_APEATCLK] = clk;
 
-	clk = clk_reg_prcmu_gate("apetraceclk", NULL, PRCMU_APETRACECLK,
-				CLK_IS_ROOT);
+	clk = clk_reg_prcmu_scalable("apetraceclk", NULL, PRCMU_APETRACECLK, 0,
+				CLK_IS_ROOT|CLK_SET_RATE_GATE);
 	prcmu_clk[PRCMU_APETRACECLK] = clk;
 
 	clk = clk_reg_prcmu_gate("mcdeclk", NULL, PRCMU_MCDECLK, CLK_IS_ROOT);
diff --git a/drivers/clk/versatile/clk-sp810.c b/drivers/clk/versatile/clk-sp810.c
index c6e86a9a2aa3..a96dd8e53fdb 100644
--- a/drivers/clk/versatile/clk-sp810.c
+++ b/drivers/clk/versatile/clk-sp810.c
@@ -135,7 +135,7 @@ static struct clk *clk_sp810_timerclken_of_get(struct of_phandle_args *clkspec,
 	return sp810->timerclken[clkspec->args[0]].clk;
 }
 
-void __init clk_sp810_of_setup(struct device_node *node)
+static void __init clk_sp810_of_setup(struct device_node *node)
 {
 	struct clk_sp810 *sp810 = kzalloc(sizeof(*sp810), GFP_KERNEL);
 	const char *parent_names[2];
@@ -156,7 +156,7 @@ void __init clk_sp810_of_setup(struct device_node *node)
 			"timclk");
 	parent_names[1] = of_clk_get_parent_name(node, sp810->timclk_index);
 
-	if (parent_names[0] <= 0 || parent_names[1] <= 0) {
+	if (!parent_names[0] || !parent_names[1]) {
 		pr_warn("Failed to obtain parent clocks for SP810!\n");
 		return;
 	}
diff --git a/drivers/clk/zte/Makefile b/drivers/clk/zte/Makefile
new file mode 100644
index 000000000000..95b707c18108
--- /dev/null
+++ b/drivers/clk/zte/Makefile
@@ -0,0 +1,2 @@
+obj-y := clk-pll.o
+obj-$(CONFIG_SOC_ZX296702) += clk-zx296702.o
diff --git a/drivers/clk/zte/clk-pll.c b/drivers/clk/zte/clk-pll.c
new file mode 100644
index 000000000000..c3b221ae6cd7
--- /dev/null
+++ b/drivers/clk/zte/clk-pll.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright 2014 Linaro Ltd.
+ * Copyright (C) 2014 ZTE 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/clk-provider.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "clk.h"
+
+#define to_clk_zx_pll(_hw) container_of(_hw, struct clk_zx_pll, hw)
+
+#define CFG0_CFG1_OFFSET 4
+#define LOCK_FLAG BIT(30)
+#define POWER_DOWN BIT(31)
+
+static int rate_to_idx(struct clk_zx_pll *zx_pll, unsigned long rate)
+{
+	const struct zx_pll_config *config = zx_pll->lookup_table;
+	int i;
+
+	for (i = 0; i < zx_pll->count; i++) {
+		if (config[i].rate > rate)
+			return i > 0 ? i - 1 : 0;
+
+		if (config[i].rate == rate)
+			return i;
+	}
+
+	return i - 1;
+}
+
+static int hw_to_idx(struct clk_zx_pll *zx_pll)
+{
+	const struct zx_pll_config *config = zx_pll->lookup_table;
+	u32 hw_cfg0, hw_cfg1;
+	int i;
+
+	hw_cfg0 = readl_relaxed(zx_pll->reg_base);
+	hw_cfg1 = readl_relaxed(zx_pll->reg_base + CFG0_CFG1_OFFSET);
+
+	/* For matching the value in lookup table */
+	hw_cfg0 &= ~LOCK_FLAG;
+	hw_cfg0 |= POWER_DOWN;
+
+	for (i = 0; i < zx_pll->count; i++) {
+		if (hw_cfg0 == config[i].cfg0 && hw_cfg1 == config[i].cfg1)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static unsigned long zx_pll_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
+	int idx;
+
+	idx = hw_to_idx(zx_pll);
+	if (unlikely(idx == -EINVAL))
+		return 0;
+
+	return zx_pll->lookup_table[idx].rate;
+}
+
+static long zx_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+			      unsigned long *prate)
+{
+	struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
+	int idx;
+
+	idx = rate_to_idx(zx_pll, rate);
+
+	return zx_pll->lookup_table[idx].rate;
+}
+
+static int zx_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+			   unsigned long parent_rate)
+{
+	/* Assume current cpu is not running on current PLL */
+	struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
+	const struct zx_pll_config *config;
+	int idx;
+
+	idx = rate_to_idx(zx_pll, rate);
+	config = &zx_pll->lookup_table[idx];
+
+	writel_relaxed(config->cfg0, zx_pll->reg_base);
+	writel_relaxed(config->cfg1, zx_pll->reg_base + CFG0_CFG1_OFFSET);
+
+	return 0;
+}
+
+static int zx_pll_enable(struct clk_hw *hw)
+{
+	struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
+	u32 reg;
+
+	reg = readl_relaxed(zx_pll->reg_base);
+	writel_relaxed(reg & ~POWER_DOWN, zx_pll->reg_base);
+
+	return readl_relaxed_poll_timeout(zx_pll->reg_base, reg,
+					  reg & LOCK_FLAG, 0, 100);
+}
+
+static void zx_pll_disable(struct clk_hw *hw)
+{
+	struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
+	u32 reg;
+
+	reg = readl_relaxed(zx_pll->reg_base);
+	writel_relaxed(reg | POWER_DOWN, zx_pll->reg_base);
+}
+
+static int zx_pll_is_enabled(struct clk_hw *hw)
+{
+	struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
+	u32 reg;
+
+	reg = readl_relaxed(zx_pll->reg_base);
+
+	return !(reg & POWER_DOWN);
+}
+
+static const struct clk_ops zx_pll_ops = {
+	.recalc_rate = zx_pll_recalc_rate,
+	.round_rate = zx_pll_round_rate,
+	.set_rate = zx_pll_set_rate,
+	.enable = zx_pll_enable,
+	.disable = zx_pll_disable,
+	.is_enabled = zx_pll_is_enabled,
+};
+
+struct clk *clk_register_zx_pll(const char *name, const char *parent_name,
+	unsigned long flags, void __iomem *reg_base,
+	const struct zx_pll_config *lookup_table, int count, spinlock_t *lock)
+{
+	struct clk_zx_pll *zx_pll;
+	struct clk *clk;
+	struct clk_init_data init;
+
+	zx_pll = kzalloc(sizeof(*zx_pll), GFP_KERNEL);
+	if (!zx_pll)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &zx_pll_ops;
+	init.flags = flags;
+	init.parent_names = parent_name ? &parent_name : NULL;
+	init.num_parents = parent_name ? 1 : 0;
+
+	zx_pll->reg_base = reg_base;
+	zx_pll->lookup_table = lookup_table;
+	zx_pll->count = count;
+	zx_pll->lock = lock;
+	zx_pll->hw.init = &init;
+
+	clk = clk_register(NULL, &zx_pll->hw);
+	if (IS_ERR(clk))
+		kfree(zx_pll);
+
+	return clk;
+}
diff --git a/drivers/clk/zte/clk-zx296702.c b/drivers/clk/zte/clk-zx296702.c
new file mode 100644
index 000000000000..929d033594af
--- /dev/null
+++ b/drivers/clk/zte/clk-zx296702.c
@@ -0,0 +1,657 @@
+/*
+ * Copyright 2014 Linaro Ltd.
+ * Copyright (C) 2014 ZTE 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/clk-provider.h>
+#include <linux/of_address.h>
+#include <dt-bindings/clock/zx296702-clock.h>
+#include "clk.h"
+
+static DEFINE_SPINLOCK(reg_lock);
+
+static void __iomem *topcrm_base;
+static void __iomem *lsp0crpm_base;
+static void __iomem *lsp1crpm_base;
+
+static struct clk *topclk[ZX296702_TOPCLK_END];
+static struct clk *lsp0clk[ZX296702_LSP0CLK_END];
+static struct clk *lsp1clk[ZX296702_LSP1CLK_END];
+
+static struct clk_onecell_data topclk_data;
+static struct clk_onecell_data lsp0clk_data;
+static struct clk_onecell_data lsp1clk_data;
+
+#define CLK_MUX			(topcrm_base + 0x04)
+#define CLK_DIV			(topcrm_base + 0x08)
+#define CLK_EN0			(topcrm_base + 0x0c)
+#define CLK_EN1			(topcrm_base + 0x10)
+#define VOU_LOCAL_CLKEN		(topcrm_base + 0x68)
+#define VOU_LOCAL_CLKSEL	(topcrm_base + 0x70)
+#define VOU_LOCAL_DIV2_SET	(topcrm_base + 0x74)
+#define CLK_MUX1		(topcrm_base + 0x8c)
+
+#define CLK_SDMMC1		(lsp0crpm_base + 0x0c)
+
+#define CLK_UART0		(lsp1crpm_base + 0x20)
+#define CLK_UART1		(lsp1crpm_base + 0x24)
+#define CLK_SDMMC0		(lsp1crpm_base + 0x2c)
+
+static const struct zx_pll_config pll_a9_config[] = {
+	{ .rate = 700000000, .cfg0 = 0x800405d1, .cfg1 = 0x04555555 },
+	{ .rate = 800000000, .cfg0 = 0x80040691, .cfg1 = 0x04aaaaaa },
+	{ .rate = 900000000, .cfg0 = 0x80040791, .cfg1 = 0x04000000 },
+	{ .rate = 1000000000, .cfg0 = 0x80040851, .cfg1 = 0x04555555 },
+	{ .rate = 1100000000, .cfg0 = 0x80040911, .cfg1 = 0x04aaaaaa },
+	{ .rate = 1200000000, .cfg0 = 0x80040a11, .cfg1 = 0x04000000 },
+};
+
+static const struct clk_div_table main_hlk_div[] = {
+	{ .val = 1, .div = 2, },
+	{ .val = 3, .div = 4, },
+	{ /* sentinel */ }
+};
+
+static const struct clk_div_table a9_as1_aclk_divider[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 3, .div = 4, },
+	{ /* sentinel */ }
+};
+
+static const struct clk_div_table sec_wclk_divider[] = {
+	{ .val = 0, .div = 1, },
+	{ .val = 1, .div = 2, },
+	{ .val = 3, .div = 4, },
+	{ .val = 5, .div = 6, },
+	{ .val = 7, .div = 8, },
+	{ /* sentinel */ }
+};
+
+static const char * matrix_aclk_sel[] = {
+	"pll_mm0_198M",
+	"osc",
+	"clk_148M5",
+	"pll_lsp_104M",
+};
+
+static const char * a9_wclk_sel[] = {
+	"pll_a9",
+	"osc",
+	"clk_500",
+	"clk_250",
+};
+
+static const char * a9_as1_aclk_sel[] = {
+	"clk_250",
+	"osc",
+	"pll_mm0_396M",
+	"pll_mac_333M",
+};
+
+static const char * a9_trace_clkin_sel[] = {
+	"clk_74M25",
+	"pll_mm1_108M",
+	"clk_125",
+	"clk_148M5",
+};
+
+static const char * decppu_aclk_sel[] = {
+	"clk_250",
+	"pll_mm0_198M",
+	"pll_lsp_104M",
+	"pll_audio_294M912",
+};
+
+static const char * vou_main_wclk_sel[] = {
+	"clk_148M5",
+	"clk_74M25",
+	"clk_27",
+	"pll_mm1_54M",
+};
+
+static const char * vou_scaler_wclk_sel[] = {
+	"clk_250",
+	"pll_mac_333M",
+	"pll_audio_294M912",
+	"pll_mm0_198M",
+};
+
+static const char * r2d_wclk_sel[] = {
+	"pll_audio_294M912",
+	"pll_mac_333M",
+	"pll_a9_350M",
+	"pll_mm0_396M",
+};
+
+static const char * ddr_wclk_sel[] = {
+	"pll_mac_333M",
+	"pll_ddr_266M",
+	"pll_audio_294M912",
+	"pll_mm0_198M",
+};
+
+static const char * nand_wclk_sel[] = {
+	"pll_lsp_104M",
+	"osc",
+};
+
+static const char * lsp_26_wclk_sel[] = {
+	"pll_lsp_26M",
+	"osc",
+};
+
+static const char * vl0_sel[] = {
+	"vou_main_channel_div",
+	"vou_aux_channel_div",
+};
+
+static const char * hdmi_sel[] = {
+	"vou_main_channel_wclk",
+	"vou_aux_channel_wclk",
+};
+
+static const char * sdmmc0_wclk_sel[] = {
+	"lsp1_104M_wclk",
+	"lsp1_26M_wclk",
+};
+
+static const char * sdmmc1_wclk_sel[] = {
+	"lsp0_104M_wclk",
+	"lsp0_26M_wclk",
+};
+
+static const char * uart_wclk_sel[] = {
+	"lsp1_104M_wclk",
+	"lsp1_26M_wclk",
+};
+
+static inline struct clk *zx_divtbl(const char *name, const char *parent,
+				    void __iomem *reg, u8 shift, u8 width,
+				    const struct clk_div_table *table)
+{
+	return clk_register_divider_table(NULL, name, parent, 0, reg, shift,
+					  width, 0, table, &reg_lock);
+}
+
+static inline struct clk *zx_div(const char *name, const char *parent,
+				 void __iomem *reg, u8 shift, u8 width)
+{
+	return clk_register_divider(NULL, name, parent, 0,
+				    reg, shift, width, 0, &reg_lock);
+}
+
+static inline struct clk *zx_mux(const char *name, const char **parents,
+		int num_parents, void __iomem *reg, u8 shift, u8 width)
+{
+	return clk_register_mux(NULL, name, parents, num_parents,
+				0, reg, shift, width, 0, &reg_lock);
+}
+
+static inline struct clk *zx_gate(const char *name, const char *parent,
+				  void __iomem *reg, u8 shift)
+{
+	return clk_register_gate(NULL, name, parent, CLK_IGNORE_UNUSED,
+				 reg, shift, 0, &reg_lock);
+}
+
+static void __init zx296702_top_clocks_init(struct device_node *np)
+{
+	struct clk **clk = topclk;
+	int i;
+
+	topcrm_base = of_iomap(np, 0);
+	WARN_ON(!topcrm_base);
+
+	clk[ZX296702_OSC] =
+		clk_register_fixed_rate(NULL, "osc", NULL, CLK_IS_ROOT,
+				30000000);
+	clk[ZX296702_PLL_A9] =
+		clk_register_zx_pll("pll_a9", "osc", 0, topcrm_base
+				+ 0x01c, pll_a9_config,
+				ARRAY_SIZE(pll_a9_config), &reg_lock);
+
+	/* TODO: pll_a9_350M look like changeble follow a9 pll */
+	clk[ZX296702_PLL_A9_350M] =
+		clk_register_fixed_rate(NULL, "pll_a9_350M", "osc", 0,
+				350000000);
+	clk[ZX296702_PLL_MAC_1000M] =
+		clk_register_fixed_rate(NULL, "pll_mac_1000M", "osc", 0,
+				1000000000);
+	clk[ZX296702_PLL_MAC_333M] =
+		clk_register_fixed_rate(NULL, "pll_mac_333M",	 "osc", 0,
+				333000000);
+	clk[ZX296702_PLL_MM0_1188M] =
+		clk_register_fixed_rate(NULL, "pll_mm0_1188M", "osc", 0,
+				1188000000);
+	clk[ZX296702_PLL_MM0_396M] =
+		clk_register_fixed_rate(NULL, "pll_mm0_396M",  "osc", 0,
+				396000000);
+	clk[ZX296702_PLL_MM0_198M] =
+		clk_register_fixed_rate(NULL, "pll_mm0_198M",  "osc", 0,
+				198000000);
+	clk[ZX296702_PLL_MM1_108M] =
+		clk_register_fixed_rate(NULL, "pll_mm1_108M",  "osc", 0,
+				108000000);
+	clk[ZX296702_PLL_MM1_72M] =
+		clk_register_fixed_rate(NULL, "pll_mm1_72M",	 "osc", 0,
+				72000000);
+	clk[ZX296702_PLL_MM1_54M] =
+		clk_register_fixed_rate(NULL, "pll_mm1_54M",	 "osc", 0,
+				54000000);
+	clk[ZX296702_PLL_LSP_104M] =
+		clk_register_fixed_rate(NULL, "pll_lsp_104M",  "osc", 0,
+				104000000);
+	clk[ZX296702_PLL_LSP_26M] =
+		clk_register_fixed_rate(NULL, "pll_lsp_26M",	 "osc", 0,
+				26000000);
+	clk[ZX296702_PLL_DDR_266M] =
+		clk_register_fixed_rate(NULL, "pll_ddr_266M",	 "osc", 0,
+				266000000);
+	clk[ZX296702_PLL_AUDIO_294M912] =
+		clk_register_fixed_rate(NULL, "pll_audio_294M912", "osc", 0,
+				294912000);
+
+	/* bus clock */
+	clk[ZX296702_MATRIX_ACLK] =
+		zx_mux("matrix_aclk", matrix_aclk_sel,
+				ARRAY_SIZE(matrix_aclk_sel), CLK_MUX, 2, 2);
+	clk[ZX296702_MAIN_HCLK] =
+		zx_divtbl("main_hclk", "matrix_aclk", CLK_DIV, 0, 2,
+				main_hlk_div);
+	clk[ZX296702_MAIN_PCLK] =
+		zx_divtbl("main_pclk", "matrix_aclk", CLK_DIV, 2, 2,
+				main_hlk_div);
+
+	/* cpu clock */
+	clk[ZX296702_CLK_500] =
+		clk_register_fixed_factor(NULL, "clk_500", "pll_mac_1000M", 0,
+				1, 2);
+	clk[ZX296702_CLK_250] =
+		clk_register_fixed_factor(NULL, "clk_250", "pll_mac_1000M", 0,
+				1, 4);
+	clk[ZX296702_CLK_125] =
+		clk_register_fixed_factor(NULL, "clk_125", "clk_250", 0, 1, 2);
+	clk[ZX296702_CLK_148M5] =
+		clk_register_fixed_factor(NULL, "clk_148M5", "pll_mm0_1188M", 0,
+				1, 8);
+	clk[ZX296702_CLK_74M25] =
+		clk_register_fixed_factor(NULL, "clk_74M25", "pll_mm0_1188M", 0,
+				1, 16);
+	clk[ZX296702_A9_WCLK] =
+		zx_mux("a9_wclk", a9_wclk_sel, ARRAY_SIZE(a9_wclk_sel), CLK_MUX,
+				0, 2);
+	clk[ZX296702_A9_AS1_ACLK_MUX] =
+		zx_mux("a9_as1_aclk_mux", a9_as1_aclk_sel,
+				ARRAY_SIZE(a9_as1_aclk_sel), CLK_MUX, 4, 2);
+	clk[ZX296702_A9_TRACE_CLKIN_MUX] =
+		zx_mux("a9_trace_clkin_mux", a9_trace_clkin_sel,
+				ARRAY_SIZE(a9_trace_clkin_sel), CLK_MUX1, 0, 2);
+	clk[ZX296702_A9_AS1_ACLK_DIV] =
+		zx_divtbl("a9_as1_aclk_div", "a9_as1_aclk_mux", CLK_DIV, 4, 2,
+				a9_as1_aclk_divider);
+
+	/* multi-media clock */
+	clk[ZX296702_CLK_2] =
+		clk_register_fixed_factor(NULL, "clk_2", "pll_mm1_72M", 0,
+				1, 36);
+	clk[ZX296702_CLK_27] =
+		clk_register_fixed_factor(NULL, "clk_27", "pll_mm1_54M", 0,
+				1, 2);
+	clk[ZX296702_DECPPU_ACLK_MUX] =
+		zx_mux("decppu_aclk_mux", decppu_aclk_sel,
+				ARRAY_SIZE(decppu_aclk_sel), CLK_MUX, 6, 2);
+	clk[ZX296702_PPU_ACLK_MUX] =
+		zx_mux("ppu_aclk_mux", decppu_aclk_sel,
+				ARRAY_SIZE(decppu_aclk_sel), CLK_MUX, 8, 2);
+	clk[ZX296702_MALI400_ACLK_MUX] =
+		zx_mux("mali400_aclk_mux", decppu_aclk_sel,
+				ARRAY_SIZE(decppu_aclk_sel), CLK_MUX, 12, 2);
+	clk[ZX296702_VOU_ACLK_MUX] =
+		zx_mux("vou_aclk_mux", decppu_aclk_sel,
+				ARRAY_SIZE(decppu_aclk_sel), CLK_MUX, 10, 2);
+	clk[ZX296702_VOU_MAIN_WCLK_MUX] =
+		zx_mux("vou_main_wclk_mux", vou_main_wclk_sel,
+				ARRAY_SIZE(vou_main_wclk_sel), CLK_MUX, 14, 2);
+	clk[ZX296702_VOU_AUX_WCLK_MUX] =
+		zx_mux("vou_aux_wclk_mux", vou_main_wclk_sel,
+				ARRAY_SIZE(vou_main_wclk_sel), CLK_MUX, 16, 2);
+	clk[ZX296702_VOU_SCALER_WCLK_MUX] =
+		zx_mux("vou_scaler_wclk_mux", vou_scaler_wclk_sel,
+				ARRAY_SIZE(vou_scaler_wclk_sel), CLK_MUX,
+				18, 2);
+	clk[ZX296702_R2D_ACLK_MUX] =
+		zx_mux("r2d_aclk_mux", decppu_aclk_sel,
+				ARRAY_SIZE(decppu_aclk_sel), CLK_MUX, 20, 2);
+	clk[ZX296702_R2D_WCLK_MUX] =
+		zx_mux("r2d_wclk_mux", r2d_wclk_sel,
+				ARRAY_SIZE(r2d_wclk_sel), CLK_MUX, 22, 2);
+
+	/* other clock */
+	clk[ZX296702_CLK_50] =
+		clk_register_fixed_factor(NULL, "clk_50", "pll_mac_1000M",
+				0, 1, 20);
+	clk[ZX296702_CLK_25] =
+		clk_register_fixed_factor(NULL, "clk_25", "pll_mac_1000M",
+				0, 1, 40);
+	clk[ZX296702_CLK_12] =
+		clk_register_fixed_factor(NULL, "clk_12", "pll_mm1_72M",
+				0, 1, 6);
+	clk[ZX296702_CLK_16M384] =
+		clk_register_fixed_factor(NULL, "clk_16M384",
+				"pll_audio_294M912", 0, 1, 18);
+	clk[ZX296702_CLK_32K768] =
+		clk_register_fixed_factor(NULL, "clk_32K768", "clk_16M384",
+				0, 1, 500);
+	clk[ZX296702_SEC_WCLK_DIV] =
+		zx_divtbl("sec_wclk_div", "pll_lsp_104M", CLK_DIV, 6, 3,
+				sec_wclk_divider);
+	clk[ZX296702_DDR_WCLK_MUX] =
+		zx_mux("ddr_wclk_mux", ddr_wclk_sel,
+				ARRAY_SIZE(ddr_wclk_sel), CLK_MUX, 24, 2);
+	clk[ZX296702_NAND_WCLK_MUX] =
+		zx_mux("nand_wclk_mux", nand_wclk_sel,
+				ARRAY_SIZE(nand_wclk_sel), CLK_MUX, 24, 2);
+	clk[ZX296702_LSP_26_WCLK_MUX] =
+		zx_mux("lsp_26_wclk_mux", lsp_26_wclk_sel,
+				ARRAY_SIZE(lsp_26_wclk_sel), CLK_MUX, 27, 1);
+
+	/* gates */
+	clk[ZX296702_A9_AS0_ACLK] =
+		zx_gate("a9_as0_aclk",	"matrix_aclk",		CLK_EN0, 0);
+	clk[ZX296702_A9_AS1_ACLK] =
+		zx_gate("a9_as1_aclk",	"a9_as1_aclk_div",	CLK_EN0, 1);
+	clk[ZX296702_A9_TRACE_CLKIN] =
+		zx_gate("a9_trace_clkin", "a9_trace_clkin_mux",	CLK_EN0, 2);
+	clk[ZX296702_DECPPU_AXI_M_ACLK] =
+		zx_gate("decppu_axi_m_aclk", "decppu_aclk_mux", CLK_EN0, 3);
+	clk[ZX296702_DECPPU_AHB_S_HCLK] =
+		zx_gate("decppu_ahb_s_hclk",	"main_hclk",	CLK_EN0, 4);
+	clk[ZX296702_PPU_AXI_M_ACLK] =
+		zx_gate("ppu_axi_m_aclk",	"ppu_aclk_mux",	CLK_EN0, 5);
+	clk[ZX296702_PPU_AHB_S_HCLK] =
+		zx_gate("ppu_ahb_s_hclk",	"main_hclk",	CLK_EN0, 6);
+	clk[ZX296702_VOU_AXI_M_ACLK] =
+		zx_gate("vou_axi_m_aclk",	"vou_aclk_mux",	CLK_EN0, 7);
+	clk[ZX296702_VOU_APB_PCLK] =
+		zx_gate("vou_apb_pclk",	"main_pclk",		CLK_EN0, 8);
+	clk[ZX296702_VOU_MAIN_CHANNEL_WCLK] =
+		zx_gate("vou_main_channel_wclk", "vou_main_wclk_mux",
+				CLK_EN0, 9);
+	clk[ZX296702_VOU_AUX_CHANNEL_WCLK] =
+		zx_gate("vou_aux_channel_wclk", "vou_aux_wclk_mux",
+				CLK_EN0, 10);
+	clk[ZX296702_VOU_HDMI_OSCLK_CEC] =
+		zx_gate("vou_hdmi_osclk_cec", "clk_2",		CLK_EN0, 11);
+	clk[ZX296702_VOU_SCALER_WCLK] =
+		zx_gate("vou_scaler_wclk", "vou_scaler_wclk_mux", CLK_EN0, 12);
+	clk[ZX296702_MALI400_AXI_M_ACLK] =
+		zx_gate("mali400_axi_m_aclk", "mali400_aclk_mux", CLK_EN0, 13);
+	clk[ZX296702_MALI400_APB_PCLK] =
+		zx_gate("mali400_apb_pclk",	"main_pclk",	CLK_EN0, 14);
+	clk[ZX296702_R2D_WCLK] =
+		zx_gate("r2d_wclk",		"r2d_wclk_mux",	CLK_EN0, 15);
+	clk[ZX296702_R2D_AXI_M_ACLK] =
+		zx_gate("r2d_axi_m_aclk",	"r2d_aclk_mux",	CLK_EN0, 16);
+	clk[ZX296702_R2D_AHB_HCLK] =
+		zx_gate("r2d_ahb_hclk",		"main_hclk",	CLK_EN0, 17);
+	clk[ZX296702_DDR3_AXI_S0_ACLK] =
+		zx_gate("ddr3_axi_s0_aclk",	"matrix_aclk",	CLK_EN0, 18);
+	clk[ZX296702_DDR3_APB_PCLK] =
+		zx_gate("ddr3_apb_pclk",	"main_pclk",	CLK_EN0, 19);
+	clk[ZX296702_DDR3_WCLK] =
+		zx_gate("ddr3_wclk",		"ddr_wclk_mux",	CLK_EN0, 20);
+	clk[ZX296702_USB20_0_AHB_HCLK] =
+		zx_gate("usb20_0_ahb_hclk",	"main_hclk",	CLK_EN0, 21);
+	clk[ZX296702_USB20_0_EXTREFCLK] =
+		zx_gate("usb20_0_extrefclk",	"clk_12",	CLK_EN0, 22);
+	clk[ZX296702_USB20_1_AHB_HCLK] =
+		zx_gate("usb20_1_ahb_hclk",	"main_hclk",	CLK_EN0, 23);
+	clk[ZX296702_USB20_1_EXTREFCLK] =
+		zx_gate("usb20_1_extrefclk",	"clk_12",	CLK_EN0, 24);
+	clk[ZX296702_USB20_2_AHB_HCLK] =
+		zx_gate("usb20_2_ahb_hclk",	"main_hclk",	CLK_EN0, 25);
+	clk[ZX296702_USB20_2_EXTREFCLK] =
+		zx_gate("usb20_2_extrefclk",	"clk_12",	CLK_EN0, 26);
+	clk[ZX296702_GMAC_AXI_M_ACLK] =
+		zx_gate("gmac_axi_m_aclk",	"matrix_aclk",	CLK_EN0, 27);
+	clk[ZX296702_GMAC_APB_PCLK] =
+		zx_gate("gmac_apb_pclk",	"main_pclk",	CLK_EN0, 28);
+	clk[ZX296702_GMAC_125_CLKIN] =
+		zx_gate("gmac_125_clkin",	"clk_125",	CLK_EN0, 29);
+	clk[ZX296702_GMAC_RMII_CLKIN] =
+		zx_gate("gmac_rmii_clkin",	"clk_50",	CLK_EN0, 30);
+	clk[ZX296702_GMAC_25M_CLK] =
+		zx_gate("gmac_25M_clk",		"clk_25",	CLK_EN0, 31);
+	clk[ZX296702_NANDFLASH_AHB_HCLK] =
+		zx_gate("nandflash_ahb_hclk", "main_hclk",	CLK_EN1, 0);
+	clk[ZX296702_NANDFLASH_WCLK] =
+		zx_gate("nandflash_wclk",     "nand_wclk_mux",	CLK_EN1, 1);
+	clk[ZX296702_LSP0_APB_PCLK] =
+		zx_gate("lsp0_apb_pclk",	"main_pclk",	CLK_EN1, 2);
+	clk[ZX296702_LSP0_AHB_HCLK] =
+		zx_gate("lsp0_ahb_hclk",	"main_hclk",	CLK_EN1, 3);
+	clk[ZX296702_LSP0_26M_WCLK] =
+		zx_gate("lsp0_26M_wclk",   "lsp_26_wclk_mux",	CLK_EN1, 4);
+	clk[ZX296702_LSP0_104M_WCLK] =
+		zx_gate("lsp0_104M_wclk",	"pll_lsp_104M",	CLK_EN1, 5);
+	clk[ZX296702_LSP0_16M384_WCLK] =
+		zx_gate("lsp0_16M384_wclk",	"clk_16M384",	CLK_EN1, 6);
+	clk[ZX296702_LSP1_APB_PCLK] =
+		zx_gate("lsp1_apb_pclk",	"main_pclk",	CLK_EN1, 7);
+	/* FIXME: wclk enable bit is bit8. We hack it as reserved 31 for
+	 * UART does not work after parent clk is disabled/enabled */
+	clk[ZX296702_LSP1_26M_WCLK] =
+		zx_gate("lsp1_26M_wclk",     "lsp_26_wclk_mux",	CLK_EN1, 31);
+	clk[ZX296702_LSP1_104M_WCLK] =
+		zx_gate("lsp1_104M_wclk",    "pll_lsp_104M",	CLK_EN1, 9);
+	clk[ZX296702_LSP1_32K_CLK] =
+		zx_gate("lsp1_32K_clk",	"clk_32K768",		CLK_EN1, 10);
+	clk[ZX296702_AON_HCLK] =
+		zx_gate("aon_hclk",		"main_hclk",	CLK_EN1, 11);
+	clk[ZX296702_SYS_CTRL_PCLK] =
+		zx_gate("sys_ctrl_pclk",	"main_pclk",	CLK_EN1, 12);
+	clk[ZX296702_DMA_PCLK] =
+		zx_gate("dma_pclk",		"main_pclk",	CLK_EN1, 13);
+	clk[ZX296702_DMA_ACLK] =
+		zx_gate("dma_aclk",		"matrix_aclk",	CLK_EN1, 14);
+	clk[ZX296702_SEC_HCLK] =
+		zx_gate("sec_hclk",		"main_hclk",	CLK_EN1, 15);
+	clk[ZX296702_AES_WCLK] =
+		zx_gate("aes_wclk",		"sec_wclk_div",	CLK_EN1, 16);
+	clk[ZX296702_DES_WCLK] =
+		zx_gate("des_wclk",		"sec_wclk_div",	CLK_EN1, 17);
+	clk[ZX296702_IRAM_ACLK] =
+		zx_gate("iram_aclk",		"matrix_aclk",	CLK_EN1, 18);
+	clk[ZX296702_IROM_ACLK] =
+		zx_gate("irom_aclk",		"matrix_aclk",	CLK_EN1, 19);
+	clk[ZX296702_BOOT_CTRL_HCLK] =
+		zx_gate("boot_ctrl_hclk",	"main_hclk",	CLK_EN1, 20);
+	clk[ZX296702_EFUSE_CLK_30] =
+		zx_gate("efuse_clk_30",	"osc",			CLK_EN1, 21);
+
+	/* TODO: add VOU Local clocks */
+	clk[ZX296702_VOU_MAIN_CHANNEL_DIV] =
+		zx_div("vou_main_channel_div", "vou_main_channel_wclk",
+				VOU_LOCAL_DIV2_SET, 1, 1);
+	clk[ZX296702_VOU_AUX_CHANNEL_DIV] =
+		zx_div("vou_aux_channel_div", "vou_aux_channel_wclk",
+				VOU_LOCAL_DIV2_SET, 0, 1);
+	clk[ZX296702_VOU_TV_ENC_HD_DIV] =
+		zx_div("vou_tv_enc_hd_div", "vou_tv_enc_hd_mux",
+				VOU_LOCAL_DIV2_SET, 3, 1);
+	clk[ZX296702_VOU_TV_ENC_SD_DIV] =
+		zx_div("vou_tv_enc_sd_div", "vou_tv_enc_sd_mux",
+				VOU_LOCAL_DIV2_SET, 2, 1);
+	clk[ZX296702_VL0_MUX] =
+		zx_mux("vl0_mux", vl0_sel, ARRAY_SIZE(vl0_sel),
+				VOU_LOCAL_CLKSEL, 8, 1);
+	clk[ZX296702_VL1_MUX] =
+		zx_mux("vl1_mux", vl0_sel, ARRAY_SIZE(vl0_sel),
+				VOU_LOCAL_CLKSEL, 9, 1);
+	clk[ZX296702_VL2_MUX] =
+		zx_mux("vl2_mux", vl0_sel, ARRAY_SIZE(vl0_sel),
+				VOU_LOCAL_CLKSEL, 10, 1);
+	clk[ZX296702_GL0_MUX] =
+		zx_mux("gl0_mux", vl0_sel, ARRAY_SIZE(vl0_sel),
+				VOU_LOCAL_CLKSEL, 5, 1);
+	clk[ZX296702_GL1_MUX] =
+		zx_mux("gl1_mux", vl0_sel, ARRAY_SIZE(vl0_sel),
+				VOU_LOCAL_CLKSEL, 6, 1);
+	clk[ZX296702_GL2_MUX] =
+		zx_mux("gl2_mux", vl0_sel, ARRAY_SIZE(vl0_sel),
+				VOU_LOCAL_CLKSEL, 7, 1);
+	clk[ZX296702_WB_MUX] =
+		zx_mux("wb_mux",  vl0_sel, ARRAY_SIZE(vl0_sel),
+				VOU_LOCAL_CLKSEL, 11, 1);
+	clk[ZX296702_HDMI_MUX] =
+		zx_mux("hdmi_mux", hdmi_sel, ARRAY_SIZE(hdmi_sel),
+				VOU_LOCAL_CLKSEL, 4, 1);
+	clk[ZX296702_VOU_TV_ENC_HD_MUX] =
+		zx_mux("vou_tv_enc_hd_mux", hdmi_sel, ARRAY_SIZE(hdmi_sel),
+				VOU_LOCAL_CLKSEL, 3, 1);
+	clk[ZX296702_VOU_TV_ENC_SD_MUX] =
+		zx_mux("vou_tv_enc_sd_mux", hdmi_sel, ARRAY_SIZE(hdmi_sel),
+				VOU_LOCAL_CLKSEL, 2, 1);
+	clk[ZX296702_VL0_CLK] =
+		zx_gate("vl0_clk", "vl0_mux", VOU_LOCAL_CLKEN, 8);
+	clk[ZX296702_VL1_CLK] =
+		zx_gate("vl1_clk", "vl1_mux", VOU_LOCAL_CLKEN, 9);
+	clk[ZX296702_VL2_CLK] =
+		zx_gate("vl2_clk", "vl2_mux", VOU_LOCAL_CLKEN, 10);
+	clk[ZX296702_GL0_CLK] =
+		zx_gate("gl0_clk", "gl0_mux", VOU_LOCAL_CLKEN, 5);
+	clk[ZX296702_GL1_CLK] =
+		zx_gate("gl1_clk", "gl1_mux", VOU_LOCAL_CLKEN, 6);
+	clk[ZX296702_GL2_CLK] =
+		zx_gate("gl2_clk", "gl2_mux", VOU_LOCAL_CLKEN, 7);
+	clk[ZX296702_WB_CLK] =
+		zx_gate("wb_clk", "wb_mux", VOU_LOCAL_CLKEN, 11);
+	clk[ZX296702_CL_CLK] =
+		zx_gate("cl_clk", "vou_main_channel_div", VOU_LOCAL_CLKEN, 12);
+	clk[ZX296702_MAIN_MIX_CLK] =
+		zx_gate("main_mix_clk", "vou_main_channel_div",
+				VOU_LOCAL_CLKEN, 4);
+	clk[ZX296702_AUX_MIX_CLK] =
+		zx_gate("aux_mix_clk", "vou_aux_channel_div",
+				VOU_LOCAL_CLKEN, 3);
+	clk[ZX296702_HDMI_CLK] =
+		zx_gate("hdmi_clk", "hdmi_mux", VOU_LOCAL_CLKEN, 2);
+	clk[ZX296702_VOU_TV_ENC_HD_DAC_CLK] =
+		zx_gate("vou_tv_enc_hd_dac_clk", "vou_tv_enc_hd_div",
+				VOU_LOCAL_CLKEN, 1);
+	clk[ZX296702_VOU_TV_ENC_SD_DAC_CLK] =
+		zx_gate("vou_tv_enc_sd_dac_clk", "vou_tv_enc_sd_div",
+				VOU_LOCAL_CLKEN, 0);
+
+	/* CA9 PERIPHCLK = a9_wclk / 2 */
+	clk[ZX296702_A9_PERIPHCLK] =
+		clk_register_fixed_factor(NULL, "a9_periphclk", "a9_wclk",
+				0, 1, 2);
+
+	for (i = 0; i < ARRAY_SIZE(topclk); i++) {
+		if (IS_ERR(clk[i])) {
+			pr_err("zx296702 clk %d: register failed with %ld\n",
+				i, PTR_ERR(clk[i]));
+			return;
+		}
+	}
+
+	topclk_data.clks = topclk;
+	topclk_data.clk_num = ARRAY_SIZE(topclk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &topclk_data);
+}
+CLK_OF_DECLARE(zx296702_top_clk, "zte,zx296702-topcrm-clk",
+		zx296702_top_clocks_init);
+
+static void __init zx296702_lsp0_clocks_init(struct device_node *np)
+{
+	struct clk **clk = lsp0clk;
+	int i;
+
+	lsp0crpm_base = of_iomap(np, 0);
+	WARN_ON(!lsp0crpm_base);
+
+	/* SDMMC1 */
+	clk[ZX296702_SDMMC1_WCLK_MUX] =
+		zx_mux("sdmmc1_wclk_mux", sdmmc1_wclk_sel,
+				ARRAY_SIZE(sdmmc1_wclk_sel), CLK_SDMMC1, 4, 1);
+	clk[ZX296702_SDMMC1_WCLK_DIV] =
+		zx_div("sdmmc1_wclk_div", "sdmmc1_wclk_mux", CLK_SDMMC1, 12, 4);
+	clk[ZX296702_SDMMC1_WCLK] =
+		zx_gate("sdmmc1_wclk", "sdmmc1_wclk_div", CLK_SDMMC1, 1);
+	clk[ZX296702_SDMMC1_PCLK] =
+		zx_gate("sdmmc1_pclk", "lsp1_apb_pclk", CLK_SDMMC1, 0);
+
+	for (i = 0; i < ARRAY_SIZE(lsp0clk); i++) {
+		if (IS_ERR(clk[i])) {
+			pr_err("zx296702 clk %d: register failed with %ld\n",
+				i, PTR_ERR(clk[i]));
+			return;
+		}
+	}
+
+	lsp0clk_data.clks = lsp0clk;
+	lsp0clk_data.clk_num = ARRAY_SIZE(lsp0clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &lsp0clk_data);
+}
+CLK_OF_DECLARE(zx296702_lsp0_clk, "zte,zx296702-lsp0crpm-clk",
+		zx296702_lsp0_clocks_init);
+
+static void __init zx296702_lsp1_clocks_init(struct device_node *np)
+{
+	struct clk **clk = lsp1clk;
+	int i;
+
+	lsp1crpm_base = of_iomap(np, 0);
+	WARN_ON(!lsp1crpm_base);
+
+	/* UART0 */
+	clk[ZX296702_UART0_WCLK_MUX] =
+		zx_mux("uart0_wclk_mux", uart_wclk_sel,
+				ARRAY_SIZE(uart_wclk_sel), CLK_UART0, 4, 1);
+	/* FIXME: uart wclk enable bit is bit1 in. We hack it as reserved 31 for
+	 * UART does not work after parent clk is disabled/enabled */
+	clk[ZX296702_UART0_WCLK] =
+		zx_gate("uart0_wclk", "uart0_wclk_mux", CLK_UART0, 31);
+	clk[ZX296702_UART0_PCLK] =
+		zx_gate("uart0_pclk", "lsp1_apb_pclk", CLK_UART0, 0);
+
+	/* UART1 */
+	clk[ZX296702_UART1_WCLK_MUX] =
+		zx_mux("uart1_wclk_mux", uart_wclk_sel,
+				ARRAY_SIZE(uart_wclk_sel), CLK_UART1, 4, 1);
+	clk[ZX296702_UART1_WCLK] =
+		zx_gate("uart1_wclk", "uart1_wclk_mux", CLK_UART1, 1);
+	clk[ZX296702_UART1_PCLK] =
+		zx_gate("uart1_pclk", "lsp1_apb_pclk", CLK_UART1, 0);
+
+	/* SDMMC0 */
+	clk[ZX296702_SDMMC0_WCLK_MUX] =
+		zx_mux("sdmmc0_wclk_mux", sdmmc0_wclk_sel,
+				ARRAY_SIZE(sdmmc0_wclk_sel), CLK_SDMMC0, 4, 1);
+	clk[ZX296702_SDMMC0_WCLK_DIV] =
+		zx_div("sdmmc0_wclk_div", "sdmmc0_wclk_mux", CLK_SDMMC0, 12, 4);
+	clk[ZX296702_SDMMC0_WCLK] =
+		zx_gate("sdmmc0_wclk", "sdmmc0_wclk_div", CLK_SDMMC0, 1);
+	clk[ZX296702_SDMMC0_PCLK] =
+		zx_gate("sdmmc0_pclk", "lsp1_apb_pclk", CLK_SDMMC0, 0);
+
+	for (i = 0; i < ARRAY_SIZE(lsp1clk); i++) {
+		if (IS_ERR(clk[i])) {
+			pr_err("zx296702 clk %d: register failed with %ld\n",
+				i, PTR_ERR(clk[i]));
+			return;
+		}
+	}
+
+	lsp1clk_data.clks = lsp1clk;
+	lsp1clk_data.clk_num = ARRAY_SIZE(lsp1clk);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &lsp1clk_data);
+}
+CLK_OF_DECLARE(zx296702_lsp1_clk, "zte,zx296702-lsp1crpm-clk",
+		zx296702_lsp1_clocks_init);
diff --git a/drivers/clk/zte/clk.h b/drivers/clk/zte/clk.h
new file mode 100644
index 000000000000..0914a82d0535
--- /dev/null
+++ b/drivers/clk/zte/clk.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright 2015 Linaro Ltd.
+ * Copyright (C) 2014 ZTE 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.
+ */
+
+#ifndef __ZTE_CLK_H
+#define __ZTE_CLK_H
+#include <linux/clk-provider.h>
+#include <linux/spinlock.h>
+
+struct zx_pll_config {
+	unsigned long rate;
+	u32 cfg0;
+	u32 cfg1;
+};
+
+struct clk_zx_pll {
+	struct clk_hw hw;
+	void __iomem *reg_base;
+	const struct zx_pll_config *lookup_table; /* order by rate asc */
+	int count;
+	spinlock_t *lock;
+};
+
+struct clk *clk_register_zx_pll(const char *name, const char *parent_name,
+	unsigned long flags, void __iomem *reg_base,
+	const struct zx_pll_config *lookup_table, int count, spinlock_t *lock);
+#endif
diff --git a/drivers/clk/zynq/clkc.c b/drivers/clk/zynq/clkc.c
index 40cb113be6af..de614384bb44 100644
--- a/drivers/clk/zynq/clkc.c
+++ b/drivers/clk/zynq/clkc.c
@@ -85,22 +85,29 @@ static DEFINE_SPINLOCK(canmioclk_lock);
 static DEFINE_SPINLOCK(dbgclk_lock);
 static DEFINE_SPINLOCK(aperclk_lock);
 
-static const char *armpll_parents[] __initdata = {"armpll_int", "ps_clk"};
-static const char *ddrpll_parents[] __initdata = {"ddrpll_int", "ps_clk"};
-static const char *iopll_parents[] __initdata = {"iopll_int", "ps_clk"};
+static const char *const armpll_parents[] __initconst = {"armpll_int",
+	"ps_clk"};
+static const char *const ddrpll_parents[] __initconst = {"ddrpll_int",
+	"ps_clk"};
+static const char *const iopll_parents[] __initconst = {"iopll_int",
+	"ps_clk"};
 static const char *gem0_mux_parents[] __initdata = {"gem0_div1", "dummy_name"};
 static const char *gem1_mux_parents[] __initdata = {"gem1_div1", "dummy_name"};
-static const char *can0_mio_mux2_parents[] __initdata = {"can0_gate",
+static const char *const can0_mio_mux2_parents[] __initconst = {"can0_gate",
 	"can0_mio_mux"};
-static const char *can1_mio_mux2_parents[] __initdata = {"can1_gate",
+static const char *const can1_mio_mux2_parents[] __initconst = {"can1_gate",
 	"can1_mio_mux"};
 static const char *dbg_emio_mux_parents[] __initdata = {"dbg_div",
 	"dummy_name"};
 
-static const char *dbgtrc_emio_input_names[] __initdata = {"trace_emio_clk"};
-static const char *gem0_emio_input_names[] __initdata = {"gem0_emio_clk"};
-static const char *gem1_emio_input_names[] __initdata = {"gem1_emio_clk"};
-static const char *swdt_ext_clk_input_names[] __initdata = {"swdt_ext_clk"};
+static const char *const dbgtrc_emio_input_names[] __initconst = {
+	"trace_emio_clk"};
+static const char *const gem0_emio_input_names[] __initconst = {
+	"gem0_emio_clk"};
+static const char *const gem1_emio_input_names[] __initconst = {
+	"gem1_emio_clk"};
+static const char *const swdt_ext_clk_input_names[] __initconst = {
+	"swdt_ext_clk"};
 
 static void __init zynq_clk_register_fclk(enum zynq_clk fclk,
 		const char *clk_name, void __iomem *fclk_ctrl_reg,
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 51d7865fdddb..4e57730e0be4 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -106,6 +106,16 @@ config CLKSRC_EFM32
 	  Support to use the timers of EFM32 SoCs as clock source and clock
 	  event device.
 
+config CLKSRC_LPC32XX
+	bool
+	select CLKSRC_MMIO
+	select CLKSRC_OF
+
+config CLKSRC_STM32
+	bool "Clocksource for STM32 SoCs" if !ARCH_STM32
+	depends on OF && ARM && (ARCH_STM32 || COMPILE_TEST)
+	select CLKSRC_MMIO
+
 config ARM_ARCH_TIMER
 	bool
 	select CLKSRC_OF if OF
@@ -132,6 +142,12 @@ config ARM_GLOBAL_TIMER
 	help
 	  This options enables support for the ARM global timer unit
 
+config ARM_TIMER_SP804
+	bool "Support for Dual Timer SP804 module"
+	depends on GENERIC_SCHED_CLOCK && CLKDEV_LOOKUP
+	select CLKSRC_MMIO
+	select CLKSRC_OF if OF
+
 config CLKSRC_ARM_GLOBAL_TIMER_SCHED_CLOCK
 	bool
 	depends on ARM_GLOBAL_TIMER
@@ -139,6 +155,13 @@ config CLKSRC_ARM_GLOBAL_TIMER_SCHED_CLOCK
 	help
 	 Use ARM global timer clock source as sched_clock
 
+config ARMV7M_SYSTICK
+	bool
+	select CLKSRC_OF if OF
+	select CLKSRC_MMIO
+	help
+	  This options enables support for the ARMv7M system timer unit
+
 config ATMEL_PIT
 	select CLKSRC_OF if OF
 	def_bool SOC_AT91SAM9 || SOC_SAMA5
@@ -258,4 +281,16 @@ config CLKSRC_PXA
 	help
 	  This enables OST0 support available on PXA and SA-11x0
 	  platforms.
+
+config H8300_TMR16
+        bool
+
+config H8300_TPU
+        bool
+
+config CLKSRC_IMX_GPT
+	bool "Clocksource using i.MX GPT" if COMPILE_TEST
+	depends on ARM && CLKDEV_LOOKUP
+	select CLKSRC_MMIO
+
 endmenu
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 5b85f6adb258..f228354961ca 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -36,7 +36,9 @@ obj-$(CONFIG_ARCH_NSPIRE)	+= zevio-timer.o
 obj-$(CONFIG_ARCH_BCM_MOBILE)	+= bcm_kona_timer.o
 obj-$(CONFIG_CADENCE_TTC_TIMER)	+= cadence_ttc_timer.o
 obj-$(CONFIG_CLKSRC_EFM32)	+= time-efm32.o
+obj-$(CONFIG_CLKSRC_STM32)	+= timer-stm32.o
 obj-$(CONFIG_CLKSRC_EXYNOS_MCT)	+= exynos_mct.o
+obj-$(CONFIG_CLKSRC_LPC32XX)	+= time-lpc32xx.o
 obj-$(CONFIG_CLKSRC_SAMSUNG_PWM)	+= samsung_pwm_timer.o
 obj-$(CONFIG_FSL_FTM_TIMER)	+= fsl_ftm_timer.o
 obj-$(CONFIG_VF_PIT_TIMER)	+= vf_pit_timer.o
@@ -45,10 +47,16 @@ obj-$(CONFIG_MTK_TIMER)		+= mtk_timer.o
 
 obj-$(CONFIG_ARM_ARCH_TIMER)		+= arm_arch_timer.o
 obj-$(CONFIG_ARM_GLOBAL_TIMER)		+= arm_global_timer.o
+obj-$(CONFIG_ARMV7M_SYSTICK)		+= armv7m_systick.o
+obj-$(CONFIG_ARM_TIMER_SP804)		+= timer-sp804.o
 obj-$(CONFIG_CLKSRC_METAG_GENERIC)	+= metag_generic.o
 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
+obj-$(CONFIG_CLKSRC_IMX_GPT)		+= timer-imx-gpt.o
 obj-$(CONFIG_ASM9260_TIMER)		+= asm9260_timer.o
+obj-$(CONFIG_H8300)			+= h8300_timer8.o
+obj-$(CONFIG_H8300_TMR16)		+= h8300_timer16.o
+obj-$(CONFIG_H8300_TPU)			+= h8300_tpu.o
diff --git a/drivers/clocksource/armv7m_systick.c b/drivers/clocksource/armv7m_systick.c
new file mode 100644
index 000000000000..addfd2c64f54
--- /dev/null
+++ b/drivers/clocksource/armv7m_systick.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (C) Maxime Coquelin 2015
+ * Author:  Maxime Coquelin <mcoquelin.stm32@gmail.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/clocksource.h>
+#include <linux/clockchips.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/clk.h>
+#include <linux/bitops.h>
+
+#define SYST_CSR	0x00
+#define SYST_RVR	0x04
+#define SYST_CVR	0x08
+#define SYST_CALIB	0x0c
+
+#define SYST_CSR_ENABLE BIT(0)
+
+#define SYSTICK_LOAD_RELOAD_MASK 0x00FFFFFF
+
+static void __init system_timer_of_register(struct device_node *np)
+{
+	struct clk *clk = NULL;
+	void __iomem *base;
+	u32 rate;
+	int ret;
+
+	base = of_iomap(np, 0);
+	if (!base) {
+		pr_warn("system-timer: invalid base address\n");
+		return;
+	}
+
+	ret = of_property_read_u32(np, "clock-frequency", &rate);
+	if (ret) {
+		clk = of_clk_get(np, 0);
+		if (IS_ERR(clk))
+			goto out_unmap;
+
+		ret = clk_prepare_enable(clk);
+		if (ret)
+			goto out_clk_put;
+
+		rate = clk_get_rate(clk);
+		if (!rate)
+			goto out_clk_disable;
+	}
+
+	writel_relaxed(SYSTICK_LOAD_RELOAD_MASK, base + SYST_RVR);
+	writel_relaxed(SYST_CSR_ENABLE, base + SYST_CSR);
+
+	ret = clocksource_mmio_init(base + SYST_CVR, "arm_system_timer", rate,
+			200, 24, clocksource_mmio_readl_down);
+	if (ret) {
+		pr_err("failed to init clocksource (%d)\n", ret);
+		if (clk)
+			goto out_clk_disable;
+		else
+			goto out_unmap;
+	}
+
+	pr_info("ARM System timer initialized as clocksource\n");
+
+	return;
+
+out_clk_disable:
+	clk_disable_unprepare(clk);
+out_clk_put:
+	clk_put(clk);
+out_unmap:
+	iounmap(base);
+	pr_warn("ARM System timer register failed (%d)\n", ret);
+}
+
+CLOCKSOURCE_OF_DECLARE(arm_systick, "arm,armv7m-systick",
+			system_timer_of_register);
diff --git a/drivers/clocksource/asm9260_timer.c b/drivers/clocksource/asm9260_timer.c
index 2c9c993727c8..4c2ba59897e8 100644
--- a/drivers/clocksource/asm9260_timer.c
+++ b/drivers/clocksource/asm9260_timer.c
@@ -178,7 +178,7 @@ static void __init asm9260_timer_init(struct device_node *np)
 	unsigned long rate;
 
 	priv.base = of_io_request_and_map(np, 0, np->name);
-	if (!priv.base)
+	if (IS_ERR(priv.base))
 		panic("%s: unable to map resource", np->name);
 
 	clk = of_clk_get(np, 0);
diff --git a/drivers/clocksource/exynos_mct.c b/drivers/clocksource/exynos_mct.c
index 83564c9cfdbe..9064ff743598 100644
--- a/drivers/clocksource/exynos_mct.c
+++ b/drivers/clocksource/exynos_mct.c
@@ -209,7 +209,7 @@ static void exynos4_frc_resume(struct clocksource *cs)
 	exynos4_mct_frc_start();
 }
 
-struct clocksource mct_frc = {
+static struct clocksource mct_frc = {
 	.name		= "mct-frc",
 	.rating		= 400,
 	.read		= exynos4_frc_read,
@@ -413,7 +413,7 @@ static inline void exynos4_tick_set_mode(enum clock_event_mode mode,
 	}
 }
 
-static int exynos4_mct_tick_clear(struct mct_clock_event_device *mevt)
+static void exynos4_mct_tick_clear(struct mct_clock_event_device *mevt)
 {
 	struct clock_event_device *evt = &mevt->evt;
 
@@ -426,12 +426,8 @@ static int exynos4_mct_tick_clear(struct mct_clock_event_device *mevt)
 		exynos4_mct_tick_stop(mevt);
 
 	/* Clear the MCT tick interrupt */
-	if (readl_relaxed(reg_base + mevt->base + MCT_L_INT_CSTAT_OFFSET) & 1) {
+	if (readl_relaxed(reg_base + mevt->base + MCT_L_INT_CSTAT_OFFSET) & 1)
 		exynos4_mct_write(0x1, mevt->base + MCT_L_INT_CSTAT_OFFSET);
-		return 1;
-	} else {
-		return 0;
-	}
 }
 
 static irqreturn_t exynos4_mct_tick_isr(int irq, void *dev_id)
@@ -466,15 +462,12 @@ static int exynos4_local_timer_setup(struct clock_event_device *evt)
 	exynos4_mct_write(TICK_BASE_CNT, mevt->base + MCT_L_TCNTB_OFFSET);
 
 	if (mct_int_type == MCT_INT_SPI) {
-		evt->irq = mct_irqs[MCT_L0_IRQ + cpu];
-		if (request_irq(evt->irq, exynos4_mct_tick_isr,
-				IRQF_TIMER | IRQF_NOBALANCING,
-				evt->name, mevt)) {
-			pr_err("exynos-mct: cannot register IRQ %d\n",
-				evt->irq);
+
+		if (evt->irq == -1)
 			return -EIO;
-		}
-		irq_force_affinity(mct_irqs[MCT_L0_IRQ + cpu], cpumask_of(cpu));
+
+		irq_force_affinity(evt->irq, cpumask_of(cpu));
+		enable_irq(evt->irq);
 	} else {
 		enable_percpu_irq(mct_irqs[MCT_L0_IRQ], 0);
 	}
@@ -487,10 +480,12 @@ static int exynos4_local_timer_setup(struct clock_event_device *evt)
 static void exynos4_local_timer_stop(struct clock_event_device *evt)
 {
 	evt->set_mode(CLOCK_EVT_MODE_UNUSED, evt);
-	if (mct_int_type == MCT_INT_SPI)
-		free_irq(evt->irq, this_cpu_ptr(&percpu_mct_tick));
-	else
+	if (mct_int_type == MCT_INT_SPI) {
+		if (evt->irq != -1)
+			disable_irq_nosync(evt->irq);
+	} else {
 		disable_percpu_irq(mct_irqs[MCT_L0_IRQ]);
+	}
 }
 
 static int exynos4_mct_cpu_notify(struct notifier_block *self,
@@ -522,7 +517,7 @@ static struct notifier_block exynos4_mct_cpu_nb = {
 
 static void __init exynos4_timer_resources(struct device_node *np, void __iomem *base)
 {
-	int err;
+	int err, cpu;
 	struct mct_clock_event_device *mevt = this_cpu_ptr(&percpu_mct_tick);
 	struct clk *mct_clk, *tick_clk;
 
@@ -549,7 +544,25 @@ static void __init exynos4_timer_resources(struct device_node *np, void __iomem
 		WARN(err, "MCT: can't request IRQ %d (%d)\n",
 		     mct_irqs[MCT_L0_IRQ], err);
 	} else {
-		irq_set_affinity(mct_irqs[MCT_L0_IRQ], cpumask_of(0));
+		for_each_possible_cpu(cpu) {
+			int mct_irq = mct_irqs[MCT_L0_IRQ + cpu];
+			struct mct_clock_event_device *pcpu_mevt =
+				per_cpu_ptr(&percpu_mct_tick, cpu);
+
+			pcpu_mevt->evt.irq = -1;
+
+			irq_set_status_flags(mct_irq, IRQ_NOAUTOEN);
+			if (request_irq(mct_irq,
+					exynos4_mct_tick_isr,
+					IRQF_TIMER | IRQF_NOBALANCING,
+					pcpu_mevt->name, pcpu_mevt)) {
+				pr_err("exynos-mct: cannot register IRQ (cpu%d)\n",
+									cpu);
+
+				continue;
+			}
+			pcpu_mevt->evt.irq = mct_irq;
+		}
 	}
 
 	err = register_cpu_notifier(&exynos4_mct_cpu_nb);
@@ -564,18 +577,6 @@ out_irq:
 	free_percpu_irq(mct_irqs[MCT_L0_IRQ], &percpu_mct_tick);
 }
 
-void __init mct_init(void __iomem *base, int irq_g0, int irq_l0, int irq_l1)
-{
-	mct_irqs[MCT_G0_IRQ] = irq_g0;
-	mct_irqs[MCT_L0_IRQ] = irq_l0;
-	mct_irqs[MCT_L1_IRQ] = irq_l1;
-	mct_int_type = MCT_INT_SPI;
-
-	exynos4_timer_resources(NULL, base);
-	exynos4_clocksource_init();
-	exynos4_clockevent_init();
-}
-
 static void __init mct_init_dt(struct device_node *np, unsigned int int_type)
 {
 	u32 nr_irqs, i;
diff --git a/drivers/clocksource/h8300_timer16.c b/drivers/clocksource/h8300_timer16.c
new file mode 100644
index 000000000000..82941c1e9e33
--- /dev/null
+++ b/drivers/clocksource/h8300_timer16.c
@@ -0,0 +1,254 @@
+/*
+ *  H8/300 16bit Timer driver
+ *
+ *  Copyright 2015 Yoshinori Sato <ysato@users.sourcefoge.jp>
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/param.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/clocksource.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/of.h>
+
+#include <asm/segment.h>
+#include <asm/irq.h>
+
+#define TSTR	0
+#define TSNC	1
+#define TMDR	2
+#define TOLR	3
+#define TISRA	4
+#define TISRB	5
+#define TISRC	6
+
+#define TCR	0
+#define TIOR	1
+#define TCNT	2
+#define GRA	4
+#define GRB	6
+
+#define FLAG_REPROGRAM (1 << 0)
+#define FLAG_SKIPEVENT (1 << 1)
+#define FLAG_IRQCONTEXT (1 << 2)
+#define FLAG_STARTED (1 << 3)
+
+#define ONESHOT  0
+#define PERIODIC 1
+
+#define RELATIVE 0
+#define ABSOLUTE 1
+
+struct timer16_priv {
+	struct platform_device *pdev;
+	struct clocksource cs;
+	struct irqaction irqaction;
+	unsigned long total_cycles;
+	unsigned long mapbase;
+	unsigned long mapcommon;
+	unsigned long flags;
+	unsigned short gra;
+	unsigned short cs_enabled;
+	unsigned char enb;
+	unsigned char imfa;
+	unsigned char imiea;
+	unsigned char ovf;
+	raw_spinlock_t lock;
+	struct clk *clk;
+};
+
+static unsigned long timer16_get_counter(struct timer16_priv *p)
+{
+	unsigned long v1, v2, v3;
+	int o1, o2;
+
+	o1 = ctrl_inb(p->mapcommon + TISRC) & p->ovf;
+
+	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
+	do {
+		o2 = o1;
+		v1 = ctrl_inw(p->mapbase + TCNT);
+		v2 = ctrl_inw(p->mapbase + TCNT);
+		v3 = ctrl_inw(p->mapbase + TCNT);
+		o1 = ctrl_inb(p->mapcommon + TISRC) & p->ovf;
+	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
+			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
+
+	v2 |= 0x10000;
+	return v2;
+}
+
+
+static irqreturn_t timer16_interrupt(int irq, void *dev_id)
+{
+	struct timer16_priv *p = (struct timer16_priv *)dev_id;
+
+	ctrl_outb(ctrl_inb(p->mapcommon + TISRA) & ~p->imfa,
+		  p->mapcommon + TISRA);
+	p->total_cycles += 0x10000;
+
+	return IRQ_HANDLED;
+}
+
+static inline struct timer16_priv *cs_to_priv(struct clocksource *cs)
+{
+	return container_of(cs, struct timer16_priv, cs);
+}
+
+static cycle_t timer16_clocksource_read(struct clocksource *cs)
+{
+	struct timer16_priv *p = cs_to_priv(cs);
+	unsigned long flags, raw;
+	unsigned long value;
+
+	raw_spin_lock_irqsave(&p->lock, flags);
+	value = p->total_cycles;
+	raw = timer16_get_counter(p);
+	raw_spin_unlock_irqrestore(&p->lock, flags);
+
+	return value + raw;
+}
+
+static int timer16_enable(struct clocksource *cs)
+{
+	struct timer16_priv *p = cs_to_priv(cs);
+
+	WARN_ON(p->cs_enabled);
+
+	p->total_cycles = 0;
+	ctrl_outw(0x0000, p->mapbase + TCNT);
+	ctrl_outb(0x83, p->mapbase + TCR);
+	ctrl_outb(ctrl_inb(p->mapcommon + TSTR) | p->enb,
+		  p->mapcommon + TSTR);
+
+	p->cs_enabled = true;
+	return 0;
+}
+
+static void timer16_disable(struct clocksource *cs)
+{
+	struct timer16_priv *p = cs_to_priv(cs);
+
+	WARN_ON(!p->cs_enabled);
+
+	ctrl_outb(ctrl_inb(p->mapcommon + TSTR) & ~p->enb,
+		  p->mapcommon + TSTR);
+
+	p->cs_enabled = false;
+}
+
+#define REG_CH   0
+#define REG_COMM 1
+
+static int timer16_setup(struct timer16_priv *p, struct platform_device *pdev)
+{
+	struct resource *res[2];
+	int ret, irq;
+	unsigned int ch;
+
+	memset(p, 0, sizeof(*p));
+	p->pdev = pdev;
+
+	res[REG_CH] = platform_get_resource(p->pdev,
+					    IORESOURCE_MEM, REG_CH);
+	res[REG_COMM] = platform_get_resource(p->pdev,
+					      IORESOURCE_MEM, REG_COMM);
+	if (!res[REG_CH] || !res[REG_COMM]) {
+		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
+	}
+	irq = platform_get_irq(p->pdev, 0);
+	if (irq < 0) {
+		dev_err(&p->pdev->dev, "failed to get irq\n");
+		return irq;
+	}
+
+	p->clk = clk_get(&p->pdev->dev, "fck");
+	if (IS_ERR(p->clk)) {
+		dev_err(&p->pdev->dev, "can't get clk\n");
+		return PTR_ERR(p->clk);
+	}
+	of_property_read_u32(p->pdev->dev.of_node, "renesas,channel", &ch);
+
+	p->pdev = pdev;
+	p->mapbase = res[REG_CH]->start;
+	p->mapcommon = res[REG_COMM]->start;
+	p->enb = 1 << ch;
+	p->imfa = 1 << ch;
+	p->imiea = 1 << (4 + ch);
+	p->cs.name = pdev->name;
+	p->cs.rating = 200;
+	p->cs.read = timer16_clocksource_read;
+	p->cs.enable = timer16_enable;
+	p->cs.disable = timer16_disable;
+	p->cs.mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8);
+	p->cs.flags = CLOCK_SOURCE_IS_CONTINUOUS;
+
+	ret = request_irq(irq, timer16_interrupt,
+			  IRQF_TIMER, pdev->name, p);
+	if (ret < 0) {
+		dev_err(&p->pdev->dev, "failed to request irq %d\n", irq);
+		return ret;
+	}
+
+	clocksource_register_hz(&p->cs, clk_get_rate(p->clk) / 8);
+
+	return 0;
+}
+
+static int timer16_probe(struct platform_device *pdev)
+{
+	struct timer16_priv *p = platform_get_drvdata(pdev);
+
+	if (p) {
+		dev_info(&pdev->dev, "kept as earlytimer\n");
+		return 0;
+	}
+
+	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
+	if (!p)
+		return -ENOMEM;
+
+	return timer16_setup(p, pdev);
+}
+
+static int timer16_remove(struct platform_device *pdev)
+{
+	return -EBUSY;
+}
+
+static const struct of_device_id timer16_of_table[] = {
+	{ .compatible = "renesas,16bit-timer" },
+	{ }
+};
+static struct platform_driver timer16_driver = {
+	.probe		= timer16_probe,
+	.remove		= timer16_remove,
+	.driver		= {
+		.name	= "h8300h-16timer",
+		.of_match_table = of_match_ptr(timer16_of_table),
+	}
+};
+
+static int __init timer16_init(void)
+{
+	return platform_driver_register(&timer16_driver);
+}
+
+static void __exit timer16_exit(void)
+{
+	platform_driver_unregister(&timer16_driver);
+}
+
+subsys_initcall(timer16_init);
+module_exit(timer16_exit);
+MODULE_AUTHOR("Yoshinori Sato");
+MODULE_DESCRIPTION("H8/300H 16bit Timer Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/clocksource/h8300_timer8.c b/drivers/clocksource/h8300_timer8.c
new file mode 100644
index 000000000000..0214cb3a7f5e
--- /dev/null
+++ b/drivers/clocksource/h8300_timer8.c
@@ -0,0 +1,313 @@
+/*
+ *  linux/arch/h8300/kernel/cpu/timer/timer8.c
+ *
+ *  Yoshinori Sato <ysato@users.sourcefoge.jp>
+ *
+ *  8bit Timer driver
+ *
+ */
+
+#include <linux/errno.h>
+#include <linux/sched.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/clockchips.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/of.h>
+
+#include <asm/irq.h>
+
+#define _8TCR	0
+#define _8TCSR	2
+#define TCORA	4
+#define TCORB	6
+#define _8TCNT	8
+
+#define FLAG_REPROGRAM (1 << 0)
+#define FLAG_SKIPEVENT (1 << 1)
+#define FLAG_IRQCONTEXT (1 << 2)
+#define FLAG_STARTED (1 << 3)
+
+#define ONESHOT  0
+#define PERIODIC 1
+
+#define RELATIVE 0
+#define ABSOLUTE 1
+
+struct timer8_priv {
+	struct platform_device *pdev;
+	struct clock_event_device ced;
+	struct irqaction irqaction;
+	unsigned long mapbase;
+	raw_spinlock_t lock;
+	unsigned long flags;
+	unsigned int rate;
+	unsigned int tcora;
+	struct clk *pclk;
+};
+
+static unsigned long timer8_get_counter(struct timer8_priv *p)
+{
+	unsigned long v1, v2, v3;
+	int o1, o2;
+
+	o1 = ctrl_inb(p->mapbase + _8TCSR) & 0x20;
+
+	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
+	do {
+		o2 = o1;
+		v1 = ctrl_inw(p->mapbase + _8TCNT);
+		v2 = ctrl_inw(p->mapbase + _8TCNT);
+		v3 = ctrl_inw(p->mapbase + _8TCNT);
+		o1 = ctrl_inb(p->mapbase + _8TCSR) & 0x20;
+	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
+			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
+
+	v2 |= o1 << 10;
+	return v2;
+}
+
+static irqreturn_t timer8_interrupt(int irq, void *dev_id)
+{
+	struct timer8_priv *p = dev_id;
+
+	ctrl_outb(ctrl_inb(p->mapbase + _8TCSR) & ~0x40,
+		  p->mapbase + _8TCSR);
+	p->flags |= FLAG_IRQCONTEXT;
+	ctrl_outw(p->tcora, p->mapbase + TCORA);
+	if (!(p->flags & FLAG_SKIPEVENT)) {
+		if (p->ced.mode == CLOCK_EVT_MODE_ONESHOT)
+			ctrl_outw(0x0000, p->mapbase + _8TCR);
+		p->ced.event_handler(&p->ced);
+	}
+	p->flags &= ~(FLAG_SKIPEVENT | FLAG_IRQCONTEXT);
+
+	return IRQ_HANDLED;
+}
+
+static void timer8_set_next(struct timer8_priv *p, unsigned long delta)
+{
+	unsigned long flags;
+	unsigned long now;
+
+	raw_spin_lock_irqsave(&p->lock, flags);
+	if (delta >= 0x10000)
+		dev_warn(&p->pdev->dev, "delta out of range\n");
+	now = timer8_get_counter(p);
+	p->tcora = delta;
+	ctrl_outb(ctrl_inb(p->mapbase + _8TCR) | 0x40, p->mapbase + _8TCR);
+	if (delta > now)
+		ctrl_outw(delta, p->mapbase + TCORA);
+	else
+		ctrl_outw(now + 1, p->mapbase + TCORA);
+
+	raw_spin_unlock_irqrestore(&p->lock, flags);
+}
+
+static int timer8_enable(struct timer8_priv *p)
+{
+	p->rate = clk_get_rate(p->pclk) / 64;
+	ctrl_outw(0xffff, p->mapbase + TCORA);
+	ctrl_outw(0x0000, p->mapbase + _8TCNT);
+	ctrl_outw(0x0c02, p->mapbase + _8TCR);
+
+	return 0;
+}
+
+static int timer8_start(struct timer8_priv *p)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&p->lock, flags);
+
+	if (!(p->flags & FLAG_STARTED))
+		ret = timer8_enable(p);
+
+	if (ret)
+		goto out;
+	p->flags |= FLAG_STARTED;
+
+ out:
+	raw_spin_unlock_irqrestore(&p->lock, flags);
+
+	return ret;
+}
+
+static void timer8_stop(struct timer8_priv *p)
+{
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&p->lock, flags);
+
+	ctrl_outw(0x0000, p->mapbase + _8TCR);
+
+	raw_spin_unlock_irqrestore(&p->lock, flags);
+}
+
+static inline struct timer8_priv *ced_to_priv(struct clock_event_device *ced)
+{
+	return container_of(ced, struct timer8_priv, ced);
+}
+
+static void timer8_clock_event_start(struct timer8_priv *p, int periodic)
+{
+	struct clock_event_device *ced = &p->ced;
+
+	timer8_start(p);
+
+	ced->shift = 32;
+	ced->mult = div_sc(p->rate, NSEC_PER_SEC, ced->shift);
+	ced->max_delta_ns = clockevent_delta2ns(0xffff, ced);
+	ced->min_delta_ns = clockevent_delta2ns(0x0001, ced);
+
+	timer8_set_next(p, periodic?(p->rate + HZ/2) / HZ:0x10000);
+}
+
+static void timer8_clock_event_mode(enum clock_event_mode mode,
+				    struct clock_event_device *ced)
+{
+	struct timer8_priv *p = ced_to_priv(ced);
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		dev_info(&p->pdev->dev, "used for periodic clock events\n");
+		timer8_stop(p);
+		timer8_clock_event_start(p, PERIODIC);
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+		dev_info(&p->pdev->dev, "used for oneshot clock events\n");
+		timer8_stop(p);
+		timer8_clock_event_start(p, ONESHOT);
+		break;
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_UNUSED:
+		timer8_stop(p);
+		break;
+	default:
+		break;
+	}
+}
+
+static int timer8_clock_event_next(unsigned long delta,
+				   struct clock_event_device *ced)
+{
+	struct timer8_priv *p = ced_to_priv(ced);
+
+	BUG_ON(ced->mode != CLOCK_EVT_MODE_ONESHOT);
+	timer8_set_next(p, delta - 1);
+
+	return 0;
+}
+
+static int timer8_setup(struct timer8_priv *p,
+			struct platform_device *pdev)
+{
+	struct resource *res;
+	int irq;
+	int ret;
+
+	memset(p, 0, sizeof(*p));
+	p->pdev = pdev;
+
+	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
+	}
+
+	irq = platform_get_irq(p->pdev, 0);
+	if (irq < 0) {
+		dev_err(&p->pdev->dev, "failed to get irq\n");
+		return -ENXIO;
+	}
+
+	p->mapbase = res->start;
+
+	p->irqaction.name = dev_name(&p->pdev->dev);
+	p->irqaction.handler = timer8_interrupt;
+	p->irqaction.dev_id = p;
+	p->irqaction.flags = IRQF_TIMER;
+
+	p->pclk = clk_get(&p->pdev->dev, "fck");
+	if (IS_ERR(p->pclk)) {
+		dev_err(&p->pdev->dev, "can't get clk\n");
+		return PTR_ERR(p->pclk);
+	}
+
+	p->ced.name = pdev->name;
+	p->ced.features = CLOCK_EVT_FEAT_PERIODIC |
+		CLOCK_EVT_FEAT_ONESHOT;
+	p->ced.rating = 200;
+	p->ced.cpumask = cpumask_of(0);
+	p->ced.set_next_event = timer8_clock_event_next;
+	p->ced.set_mode = timer8_clock_event_mode;
+
+	ret = setup_irq(irq, &p->irqaction);
+	if (ret < 0) {
+		dev_err(&p->pdev->dev,
+			"failed to request irq %d\n", irq);
+		return ret;
+	}
+	clockevents_register_device(&p->ced);
+	platform_set_drvdata(pdev, p);
+
+	return 0;
+}
+
+static int timer8_probe(struct platform_device *pdev)
+{
+	struct timer8_priv *p = platform_get_drvdata(pdev);
+
+	if (p) {
+		dev_info(&pdev->dev, "kept as earlytimer\n");
+		return 0;
+	}
+
+	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
+	if (!p)
+		return -ENOMEM;
+
+	return timer8_setup(p, pdev);
+}
+
+static int timer8_remove(struct platform_device *pdev)
+{
+	return -EBUSY;
+}
+
+static const struct of_device_id timer8_of_table[] __maybe_unused = {
+	{ .compatible = "renesas,8bit-timer" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, timer8_of_table);
+static struct platform_driver timer8_driver = {
+	.probe		= timer8_probe,
+	.remove		= timer8_remove,
+	.driver		= {
+		.name	= "h8300-8timer",
+		.of_match_table = of_match_ptr(timer8_of_table),
+	}
+};
+
+static int __init timer8_init(void)
+{
+	return platform_driver_register(&timer8_driver);
+}
+
+static void __exit timer8_exit(void)
+{
+	platform_driver_unregister(&timer8_driver);
+}
+
+subsys_initcall(timer8_init);
+module_exit(timer8_exit);
+MODULE_AUTHOR("Yoshinori Sato");
+MODULE_DESCRIPTION("H8/300 8bit Timer Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/clocksource/h8300_tpu.c b/drivers/clocksource/h8300_tpu.c
new file mode 100644
index 000000000000..64195fdd78bf
--- /dev/null
+++ b/drivers/clocksource/h8300_tpu.c
@@ -0,0 +1,207 @@
+/*
+ *  H8/300 TPU Driver
+ *
+ *  Copyright 2015 Yoshinori Sato <ysato@users.sourcefoge.jp>
+ *
+ */
+
+#include <linux/errno.h>
+#include <linux/sched.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/clocksource.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/of.h>
+
+#include <asm/irq.h>
+
+#define TCR	0
+#define TMDR	1
+#define TIOR	2
+#define TER	4
+#define TSR	5
+#define TCNT	6
+#define TGRA	8
+#define TGRB	10
+#define TGRC	12
+#define TGRD	14
+
+struct tpu_priv {
+	struct platform_device *pdev;
+	struct clocksource cs;
+	struct clk *clk;
+	unsigned long mapbase1;
+	unsigned long mapbase2;
+	raw_spinlock_t lock;
+	unsigned int cs_enabled;
+};
+
+static inline unsigned long read_tcnt32(struct tpu_priv *p)
+{
+	unsigned long tcnt;
+
+	tcnt = ctrl_inw(p->mapbase1 + TCNT) << 16;
+	tcnt |= ctrl_inw(p->mapbase2 + TCNT);
+	return tcnt;
+}
+
+static int tpu_get_counter(struct tpu_priv *p, unsigned long long *val)
+{
+	unsigned long v1, v2, v3;
+	int o1, o2;
+
+	o1 = ctrl_inb(p->mapbase1 + TSR) & 0x10;
+
+	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
+	do {
+		o2 = o1;
+		v1 = read_tcnt32(p);
+		v2 = read_tcnt32(p);
+		v3 = read_tcnt32(p);
+		o1 = ctrl_inb(p->mapbase1 + TSR) & 0x10;
+	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
+			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
+
+	*val = v2;
+	return o1;
+}
+
+static inline struct tpu_priv *cs_to_priv(struct clocksource *cs)
+{
+	return container_of(cs, struct tpu_priv, cs);
+}
+
+static cycle_t tpu_clocksource_read(struct clocksource *cs)
+{
+	struct tpu_priv *p = cs_to_priv(cs);
+	unsigned long flags;
+	unsigned long long value;
+
+	raw_spin_lock_irqsave(&p->lock, flags);
+	if (tpu_get_counter(p, &value))
+		value += 0x100000000;
+	raw_spin_unlock_irqrestore(&p->lock, flags);
+
+	return value;
+}
+
+static int tpu_clocksource_enable(struct clocksource *cs)
+{
+	struct tpu_priv *p = cs_to_priv(cs);
+
+	WARN_ON(p->cs_enabled);
+
+	ctrl_outw(0, p->mapbase1 + TCNT);
+	ctrl_outw(0, p->mapbase2 + TCNT);
+	ctrl_outb(0x0f, p->mapbase1 + TCR);
+	ctrl_outb(0x03, p->mapbase2 + TCR);
+
+	p->cs_enabled = true;
+	return 0;
+}
+
+static void tpu_clocksource_disable(struct clocksource *cs)
+{
+	struct tpu_priv *p = cs_to_priv(cs);
+
+	WARN_ON(!p->cs_enabled);
+
+	ctrl_outb(0, p->mapbase1 + TCR);
+	ctrl_outb(0, p->mapbase2 + TCR);
+	p->cs_enabled = false;
+}
+
+#define CH_L 0
+#define CH_H 1
+
+static int __init tpu_setup(struct tpu_priv *p, struct platform_device *pdev)
+{
+	struct resource *res[2];
+
+	memset(p, 0, sizeof(*p));
+	p->pdev = pdev;
+
+	res[CH_L] = platform_get_resource(p->pdev, IORESOURCE_MEM, CH_L);
+	res[CH_H] = platform_get_resource(p->pdev, IORESOURCE_MEM, CH_H);
+	if (!res[CH_L] || !res[CH_H]) {
+		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
+		return -ENXIO;
+	}
+
+	p->clk = clk_get(&p->pdev->dev, "fck");
+	if (IS_ERR(p->clk)) {
+		dev_err(&p->pdev->dev, "can't get clk\n");
+		return PTR_ERR(p->clk);
+	}
+
+	p->mapbase1 = res[CH_L]->start;
+	p->mapbase2 = res[CH_H]->start;
+
+	p->cs.name = pdev->name;
+	p->cs.rating = 200;
+	p->cs.read = tpu_clocksource_read;
+	p->cs.enable = tpu_clocksource_enable;
+	p->cs.disable = tpu_clocksource_disable;
+	p->cs.mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8);
+	p->cs.flags = CLOCK_SOURCE_IS_CONTINUOUS;
+	clocksource_register_hz(&p->cs, clk_get_rate(p->clk) / 64);
+	platform_set_drvdata(pdev, p);
+
+	return 0;
+}
+
+static int tpu_probe(struct platform_device *pdev)
+{
+	struct tpu_priv *p = platform_get_drvdata(pdev);
+
+	if (p) {
+		dev_info(&pdev->dev, "kept as earlytimer\n");
+		return 0;
+	}
+
+	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
+	if (!p)
+		return -ENOMEM;
+
+	return tpu_setup(p, pdev);
+}
+
+static int tpu_remove(struct platform_device *pdev)
+{
+	return -EBUSY;
+}
+
+static const struct of_device_id tpu_of_table[] = {
+	{ .compatible = "renesas,tpu" },
+	{ }
+};
+
+static struct platform_driver tpu_driver = {
+	.probe		= tpu_probe,
+	.remove		= tpu_remove,
+	.driver		= {
+		.name	= "h8s-tpu",
+		.of_match_table = of_match_ptr(tpu_of_table),
+	}
+};
+
+static int __init tpu_init(void)
+{
+	return platform_driver_register(&tpu_driver);
+}
+
+static void __exit tpu_exit(void)
+{
+	platform_driver_unregister(&tpu_driver);
+}
+
+subsys_initcall(tpu_init);
+module_exit(tpu_exit);
+MODULE_AUTHOR("Yoshinori Sato");
+MODULE_DESCRIPTION("H8S Timer Pulse Unit Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/clocksource/qcom-timer.c b/drivers/clocksource/qcom-timer.c
index 098c542e5c53..cba2d015564c 100644
--- a/drivers/clocksource/qcom-timer.c
+++ b/drivers/clocksource/qcom-timer.c
@@ -40,8 +40,6 @@
 
 #define GPT_HZ 32768
 
-#define MSM_DGT_SHIFT 5
-
 static void __iomem *event_base;
 static void __iomem *sts_base;
 
@@ -232,7 +230,6 @@ err:
 	register_current_timer_delay(&msm_delay_timer);
 }
 
-#ifdef CONFIG_ARCH_QCOM
 static void __init msm_dt_timer_init(struct device_node *np)
 {
 	u32 freq;
@@ -285,59 +282,3 @@ static void __init msm_dt_timer_init(struct device_node *np)
 }
 CLOCKSOURCE_OF_DECLARE(kpss_timer, "qcom,kpss-timer", msm_dt_timer_init);
 CLOCKSOURCE_OF_DECLARE(scss_timer, "qcom,scss-timer", msm_dt_timer_init);
-#else
-
-static int __init msm_timer_map(phys_addr_t addr, u32 event, u32 source,
-				u32 sts)
-{
-	void __iomem *base;
-
-	base = ioremap(addr, SZ_256);
-	if (!base) {
-		pr_err("Failed to map timer base\n");
-		return -ENOMEM;
-	}
-	event_base = base + event;
-	source_base = base + source;
-	if (sts)
-		sts_base = base + sts;
-
-	return 0;
-}
-
-static notrace cycle_t msm_read_timer_count_shift(struct clocksource *cs)
-{
-	/*
-	 * Shift timer count down by a constant due to unreliable lower bits
-	 * on some targets.
-	 */
-	return msm_read_timer_count(cs) >> MSM_DGT_SHIFT;
-}
-
-void __init msm7x01_timer_init(void)
-{
-	struct clocksource *cs = &msm_clocksource;
-
-	if (msm_timer_map(0xc0100000, 0x0, 0x10, 0x0))
-		return;
-	cs->read = msm_read_timer_count_shift;
-	cs->mask = CLOCKSOURCE_MASK((32 - MSM_DGT_SHIFT));
-	/* 600 KHz */
-	msm_timer_init(19200000 >> MSM_DGT_SHIFT, 32 - MSM_DGT_SHIFT, 7,
-			false);
-}
-
-void __init msm7x30_timer_init(void)
-{
-	if (msm_timer_map(0xc0100000, 0x4, 0x24, 0x80))
-		return;
-	msm_timer_init(24576000 / 4, 32, 1, false);
-}
-
-void __init qsd8x50_timer_init(void)
-{
-	if (msm_timer_map(0xAC100000, 0x0, 0x10, 0x34))
-		return;
-	msm_timer_init(19200000 / 4, 32, 7, false);
-}
-#endif
diff --git a/drivers/clocksource/time-lpc32xx.c b/drivers/clocksource/time-lpc32xx.c
new file mode 100644
index 000000000000..a1c06a2bc77c
--- /dev/null
+++ b/drivers/clocksource/time-lpc32xx.c
@@ -0,0 +1,272 @@
+/*
+ * Clocksource driver for NXP LPC32xx/18xx/43xx timer
+ *
+ * Copyright (C) 2015 Joachim Eastwood <manabian@gmail.com>
+ *
+ * Based on:
+ * time-efm32 Copyright (C) 2013 Pengutronix
+ * mach-lpc32xx/timer.c Copyright (C) 2009 - 2010 NXP Semiconductors
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ *
+ */
+
+#define pr_fmt(fmt) "%s: " fmt, __func__
+
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/clocksource.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/sched_clock.h>
+
+#define LPC32XX_TIMER_IR		0x000
+#define  LPC32XX_TIMER_IR_MR0INT	BIT(0)
+#define LPC32XX_TIMER_TCR		0x004
+#define  LPC32XX_TIMER_TCR_CEN		BIT(0)
+#define  LPC32XX_TIMER_TCR_CRST		BIT(1)
+#define LPC32XX_TIMER_TC		0x008
+#define LPC32XX_TIMER_PR		0x00c
+#define LPC32XX_TIMER_MCR		0x014
+#define  LPC32XX_TIMER_MCR_MR0I		BIT(0)
+#define  LPC32XX_TIMER_MCR_MR0R		BIT(1)
+#define  LPC32XX_TIMER_MCR_MR0S		BIT(2)
+#define LPC32XX_TIMER_MR0		0x018
+#define LPC32XX_TIMER_CTCR		0x070
+
+struct lpc32xx_clock_event_ddata {
+	struct clock_event_device evtdev;
+	void __iomem *base;
+};
+
+/* Needed for the sched clock */
+static void __iomem *clocksource_timer_counter;
+
+static u64 notrace lpc32xx_read_sched_clock(void)
+{
+	return readl(clocksource_timer_counter);
+}
+
+static int lpc32xx_clkevt_next_event(unsigned long delta,
+				     struct clock_event_device *evtdev)
+{
+	struct lpc32xx_clock_event_ddata *ddata =
+		container_of(evtdev, struct lpc32xx_clock_event_ddata, evtdev);
+
+	/*
+	 * Place timer in reset and program the delta in the prescale
+	 * register (PR). When the prescale counter matches the value
+	 * in PR the counter register is incremented and the compare
+	 * match will trigger. After setup the timer is released from
+	 * reset and enabled.
+	 */
+	writel_relaxed(LPC32XX_TIMER_TCR_CRST, ddata->base + LPC32XX_TIMER_TCR);
+	writel_relaxed(delta, ddata->base + LPC32XX_TIMER_PR);
+	writel_relaxed(LPC32XX_TIMER_TCR_CEN, ddata->base + LPC32XX_TIMER_TCR);
+
+	return 0;
+}
+
+static int lpc32xx_clkevt_shutdown(struct clock_event_device *evtdev)
+{
+	struct lpc32xx_clock_event_ddata *ddata =
+		container_of(evtdev, struct lpc32xx_clock_event_ddata, evtdev);
+
+	/* Disable the timer */
+	writel_relaxed(0, ddata->base + LPC32XX_TIMER_TCR);
+
+	return 0;
+}
+
+static int lpc32xx_clkevt_oneshot(struct clock_event_device *evtdev)
+{
+	/*
+	 * When using oneshot, we must also disable the timer
+	 * to wait for the first call to set_next_event().
+	 */
+	return lpc32xx_clkevt_shutdown(evtdev);
+}
+
+static irqreturn_t lpc32xx_clock_event_handler(int irq, void *dev_id)
+{
+	struct lpc32xx_clock_event_ddata *ddata = dev_id;
+
+	/* Clear match on channel 0 */
+	writel_relaxed(LPC32XX_TIMER_IR_MR0INT, ddata->base + LPC32XX_TIMER_IR);
+
+	ddata->evtdev.event_handler(&ddata->evtdev);
+
+	return IRQ_HANDLED;
+}
+
+static struct lpc32xx_clock_event_ddata lpc32xx_clk_event_ddata = {
+	.evtdev = {
+		.name			= "lpc3220 clockevent",
+		.features		= CLOCK_EVT_FEAT_ONESHOT,
+		.rating			= 300,
+		.set_next_event		= lpc32xx_clkevt_next_event,
+		.set_state_shutdown	= lpc32xx_clkevt_shutdown,
+		.set_state_oneshot	= lpc32xx_clkevt_oneshot,
+	},
+};
+
+static int __init lpc32xx_clocksource_init(struct device_node *np)
+{
+	void __iomem *base;
+	unsigned long rate;
+	struct clk *clk;
+	int ret;
+
+	clk = of_clk_get_by_name(np, "timerclk");
+	if (IS_ERR(clk)) {
+		pr_err("clock get failed (%lu)\n", PTR_ERR(clk));
+		return PTR_ERR(clk);
+	}
+
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		pr_err("clock enable failed (%d)\n", ret);
+		goto err_clk_enable;
+	}
+
+	base = of_iomap(np, 0);
+	if (!base) {
+		pr_err("unable to map registers\n");
+		ret = -EADDRNOTAVAIL;
+		goto err_iomap;
+	}
+
+	/*
+	 * Disable and reset timer then set it to free running timer
+	 * mode (CTCR) with no prescaler (PR) or match operations (MCR).
+	 * After setup the timer is released from reset and enabled.
+	 */
+	writel_relaxed(LPC32XX_TIMER_TCR_CRST, base + LPC32XX_TIMER_TCR);
+	writel_relaxed(0, base + LPC32XX_TIMER_PR);
+	writel_relaxed(0, base + LPC32XX_TIMER_MCR);
+	writel_relaxed(0, base + LPC32XX_TIMER_CTCR);
+	writel_relaxed(LPC32XX_TIMER_TCR_CEN, base + LPC32XX_TIMER_TCR);
+
+	rate = clk_get_rate(clk);
+	ret = clocksource_mmio_init(base + LPC32XX_TIMER_TC, "lpc3220 timer",
+				    rate, 300, 32, clocksource_mmio_readl_up);
+	if (ret) {
+		pr_err("failed to init clocksource (%d)\n", ret);
+		goto err_clocksource_init;
+	}
+
+	clocksource_timer_counter = base + LPC32XX_TIMER_TC;
+	sched_clock_register(lpc32xx_read_sched_clock, 32, rate);
+
+	return 0;
+
+err_clocksource_init:
+	iounmap(base);
+err_iomap:
+	clk_disable_unprepare(clk);
+err_clk_enable:
+	clk_put(clk);
+	return ret;
+}
+
+static int __init lpc32xx_clockevent_init(struct device_node *np)
+{
+	void __iomem *base;
+	unsigned long rate;
+	struct clk *clk;
+	int ret, irq;
+
+	clk = of_clk_get_by_name(np, "timerclk");
+	if (IS_ERR(clk)) {
+		pr_err("clock get failed (%lu)\n", PTR_ERR(clk));
+		return PTR_ERR(clk);
+	}
+
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		pr_err("clock enable failed (%d)\n", ret);
+		goto err_clk_enable;
+	}
+
+	base = of_iomap(np, 0);
+	if (!base) {
+		pr_err("unable to map registers\n");
+		ret = -EADDRNOTAVAIL;
+		goto err_iomap;
+	}
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq) {
+		pr_err("get irq failed\n");
+		ret = -ENOENT;
+		goto err_irq;
+	}
+
+	/*
+	 * Disable timer and clear any pending interrupt (IR) on match
+	 * channel 0 (MR0). Configure a compare match value of 1 on MR0
+	 * and enable interrupt, reset on match and stop on match (MCR).
+	 */
+	writel_relaxed(0, base + LPC32XX_TIMER_TCR);
+	writel_relaxed(0, base + LPC32XX_TIMER_CTCR);
+	writel_relaxed(LPC32XX_TIMER_IR_MR0INT, base + LPC32XX_TIMER_IR);
+	writel_relaxed(1, base + LPC32XX_TIMER_MR0);
+	writel_relaxed(LPC32XX_TIMER_MCR_MR0I | LPC32XX_TIMER_MCR_MR0R |
+		       LPC32XX_TIMER_MCR_MR0S, base + LPC32XX_TIMER_MCR);
+
+	rate = clk_get_rate(clk);
+	lpc32xx_clk_event_ddata.base = base;
+	clockevents_config_and_register(&lpc32xx_clk_event_ddata.evtdev,
+					rate, 1, -1);
+
+	ret = request_irq(irq, lpc32xx_clock_event_handler,
+			  IRQF_TIMER | IRQF_IRQPOLL, "lpc3220 clockevent",
+			  &lpc32xx_clk_event_ddata);
+	if (ret) {
+		pr_err("request irq failed\n");
+		goto err_irq;
+	}
+
+	return 0;
+
+err_irq:
+	iounmap(base);
+err_iomap:
+	clk_disable_unprepare(clk);
+err_clk_enable:
+	clk_put(clk);
+	return ret;
+}
+
+/*
+ * This function asserts that we have exactly one clocksource and one
+ * clock_event_device in the end.
+ */
+static void __init lpc32xx_timer_init(struct device_node *np)
+{
+	static int has_clocksource, has_clockevent;
+	int ret;
+
+	if (!has_clocksource) {
+		ret = lpc32xx_clocksource_init(np);
+		if (!ret) {
+			has_clocksource = 1;
+			return;
+		}
+	}
+
+	if (!has_clockevent) {
+		ret = lpc32xx_clockevent_init(np);
+		if (!ret) {
+			has_clockevent = 1;
+			return;
+		}
+	}
+}
+CLOCKSOURCE_OF_DECLARE(lpc32xx_timer, "nxp,lpc3220-timer", lpc32xx_timer_init);
diff --git a/drivers/clocksource/timer-imx-gpt.c b/drivers/clocksource/timer-imx-gpt.c
new file mode 100644
index 000000000000..2d59038dec43
--- /dev/null
+++ b/drivers/clocksource/timer-imx-gpt.c
@@ -0,0 +1,541 @@
+/*
+ *  linux/arch/arm/plat-mxc/time.c
+ *
+ *  Copyright (C) 2000-2001 Deep Blue Solutions
+ *  Copyright (C) 2002 Shane Nay (shane@minirl.com)
+ *  Copyright (C) 2006-2007 Pavel Pisa (ppisa@pikron.com)
+ *  Copyright (C) 2008 Juergen Beisert (kernel@pengutronix.de)
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/clockchips.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/sched_clock.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <soc/imx/timer.h>
+
+/*
+ * There are 4 versions of the timer hardware on Freescale MXC hardware.
+ *  - MX1/MXL
+ *  - MX21, MX27.
+ *  - MX25, MX31, MX35, MX37, MX51, MX6Q(rev1.0)
+ *  - MX6DL, MX6SX, MX6Q(rev1.1+)
+ */
+
+/* defines common for all i.MX */
+#define MXC_TCTL		0x00
+#define MXC_TCTL_TEN		(1 << 0) /* Enable module */
+#define MXC_TPRER		0x04
+
+/* MX1, MX21, MX27 */
+#define MX1_2_TCTL_CLK_PCLK1	(1 << 1)
+#define MX1_2_TCTL_IRQEN	(1 << 4)
+#define MX1_2_TCTL_FRR		(1 << 8)
+#define MX1_2_TCMP		0x08
+#define MX1_2_TCN		0x10
+#define MX1_2_TSTAT		0x14
+
+/* MX21, MX27 */
+#define MX2_TSTAT_CAPT		(1 << 1)
+#define MX2_TSTAT_COMP		(1 << 0)
+
+/* MX31, MX35, MX25, MX5, MX6 */
+#define V2_TCTL_WAITEN		(1 << 3) /* Wait enable mode */
+#define V2_TCTL_CLK_IPG		(1 << 6)
+#define V2_TCTL_CLK_PER		(2 << 6)
+#define V2_TCTL_CLK_OSC_DIV8	(5 << 6)
+#define V2_TCTL_FRR		(1 << 9)
+#define V2_TCTL_24MEN		(1 << 10)
+#define V2_TPRER_PRE24M		12
+#define V2_IR			0x0c
+#define V2_TSTAT		0x08
+#define V2_TSTAT_OF1		(1 << 0)
+#define V2_TCN			0x24
+#define V2_TCMP			0x10
+
+#define V2_TIMER_RATE_OSC_DIV8	3000000
+
+struct imx_timer {
+	enum imx_gpt_type type;
+	void __iomem *base;
+	int irq;
+	struct clk *clk_per;
+	struct clk *clk_ipg;
+	const struct imx_gpt_data *gpt;
+	struct clock_event_device ced;
+	enum clock_event_mode cem;
+	struct irqaction act;
+};
+
+struct imx_gpt_data {
+	int reg_tstat;
+	int reg_tcn;
+	int reg_tcmp;
+	void (*gpt_setup_tctl)(struct imx_timer *imxtm);
+	void (*gpt_irq_enable)(struct imx_timer *imxtm);
+	void (*gpt_irq_disable)(struct imx_timer *imxtm);
+	void (*gpt_irq_acknowledge)(struct imx_timer *imxtm);
+	int (*set_next_event)(unsigned long evt,
+			      struct clock_event_device *ced);
+};
+
+static inline struct imx_timer *to_imx_timer(struct clock_event_device *ced)
+{
+	return container_of(ced, struct imx_timer, ced);
+}
+
+static void imx1_gpt_irq_disable(struct imx_timer *imxtm)
+{
+	unsigned int tmp;
+
+	tmp = readl_relaxed(imxtm->base + MXC_TCTL);
+	writel_relaxed(tmp & ~MX1_2_TCTL_IRQEN, imxtm->base + MXC_TCTL);
+}
+#define imx21_gpt_irq_disable imx1_gpt_irq_disable
+
+static void imx31_gpt_irq_disable(struct imx_timer *imxtm)
+{
+	writel_relaxed(0, imxtm->base + V2_IR);
+}
+#define imx6dl_gpt_irq_disable imx31_gpt_irq_disable
+
+static void imx1_gpt_irq_enable(struct imx_timer *imxtm)
+{
+	unsigned int tmp;
+
+	tmp = readl_relaxed(imxtm->base + MXC_TCTL);
+	writel_relaxed(tmp | MX1_2_TCTL_IRQEN, imxtm->base + MXC_TCTL);
+}
+#define imx21_gpt_irq_enable imx1_gpt_irq_enable
+
+static void imx31_gpt_irq_enable(struct imx_timer *imxtm)
+{
+	writel_relaxed(1<<0, imxtm->base + V2_IR);
+}
+#define imx6dl_gpt_irq_enable imx31_gpt_irq_enable
+
+static void imx1_gpt_irq_acknowledge(struct imx_timer *imxtm)
+{
+	writel_relaxed(0, imxtm->base + MX1_2_TSTAT);
+}
+
+static void imx21_gpt_irq_acknowledge(struct imx_timer *imxtm)
+{
+	writel_relaxed(MX2_TSTAT_CAPT | MX2_TSTAT_COMP,
+				imxtm->base + MX1_2_TSTAT);
+}
+
+static void imx31_gpt_irq_acknowledge(struct imx_timer *imxtm)
+{
+	writel_relaxed(V2_TSTAT_OF1, imxtm->base + V2_TSTAT);
+}
+#define imx6dl_gpt_irq_acknowledge imx31_gpt_irq_acknowledge
+
+static void __iomem *sched_clock_reg;
+
+static u64 notrace mxc_read_sched_clock(void)
+{
+	return sched_clock_reg ? readl_relaxed(sched_clock_reg) : 0;
+}
+
+static struct delay_timer imx_delay_timer;
+
+static unsigned long imx_read_current_timer(void)
+{
+	return readl_relaxed(sched_clock_reg);
+}
+
+static int __init mxc_clocksource_init(struct imx_timer *imxtm)
+{
+	unsigned int c = clk_get_rate(imxtm->clk_per);
+	void __iomem *reg = imxtm->base + imxtm->gpt->reg_tcn;
+
+	imx_delay_timer.read_current_timer = &imx_read_current_timer;
+	imx_delay_timer.freq = c;
+	register_current_timer_delay(&imx_delay_timer);
+
+	sched_clock_reg = reg;
+
+	sched_clock_register(mxc_read_sched_clock, 32, c);
+	return clocksource_mmio_init(reg, "mxc_timer1", c, 200, 32,
+			clocksource_mmio_readl_up);
+}
+
+/* clock event */
+
+static int mx1_2_set_next_event(unsigned long evt,
+			      struct clock_event_device *ced)
+{
+	struct imx_timer *imxtm = to_imx_timer(ced);
+	unsigned long tcmp;
+
+	tcmp = readl_relaxed(imxtm->base + MX1_2_TCN) + evt;
+
+	writel_relaxed(tcmp, imxtm->base + MX1_2_TCMP);
+
+	return (int)(tcmp - readl_relaxed(imxtm->base + MX1_2_TCN)) < 0 ?
+				-ETIME : 0;
+}
+
+static int v2_set_next_event(unsigned long evt,
+			      struct clock_event_device *ced)
+{
+	struct imx_timer *imxtm = to_imx_timer(ced);
+	unsigned long tcmp;
+
+	tcmp = readl_relaxed(imxtm->base + V2_TCN) + evt;
+
+	writel_relaxed(tcmp, imxtm->base + V2_TCMP);
+
+	return evt < 0x7fffffff &&
+		(int)(tcmp - readl_relaxed(imxtm->base + V2_TCN)) < 0 ?
+				-ETIME : 0;
+}
+
+#ifdef DEBUG
+static const char *clock_event_mode_label[] = {
+	[CLOCK_EVT_MODE_PERIODIC] = "CLOCK_EVT_MODE_PERIODIC",
+	[CLOCK_EVT_MODE_ONESHOT]  = "CLOCK_EVT_MODE_ONESHOT",
+	[CLOCK_EVT_MODE_SHUTDOWN] = "CLOCK_EVT_MODE_SHUTDOWN",
+	[CLOCK_EVT_MODE_UNUSED]   = "CLOCK_EVT_MODE_UNUSED",
+	[CLOCK_EVT_MODE_RESUME]   = "CLOCK_EVT_MODE_RESUME",
+};
+#endif /* DEBUG */
+
+static void mxc_set_mode(enum clock_event_mode mode,
+				struct clock_event_device *ced)
+{
+	struct imx_timer *imxtm = to_imx_timer(ced);
+	unsigned long flags;
+
+	/*
+	 * The timer interrupt generation is disabled at least
+	 * for enough time to call mxc_set_next_event()
+	 */
+	local_irq_save(flags);
+
+	/* Disable interrupt in GPT module */
+	imxtm->gpt->gpt_irq_disable(imxtm);
+
+	if (mode != imxtm->cem) {
+		u32 tcn = readl_relaxed(imxtm->base + imxtm->gpt->reg_tcn);
+		/* Set event time into far-far future */
+		writel_relaxed(tcn - 3, imxtm->base + imxtm->gpt->reg_tcmp);
+
+		/* Clear pending interrupt */
+		imxtm->gpt->gpt_irq_acknowledge(imxtm);
+	}
+
+#ifdef DEBUG
+	printk(KERN_INFO "mxc_set_mode: changing mode from %s to %s\n",
+		clock_event_mode_label[imxtm->cem],
+		clock_event_mode_label[mode]);
+#endif /* DEBUG */
+
+	/* Remember timer mode */
+	imxtm->cem = mode;
+	local_irq_restore(flags);
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		printk(KERN_ERR"mxc_set_mode: Periodic mode is not "
+				"supported for i.MX\n");
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+	/*
+	 * Do not put overhead of interrupt enable/disable into
+	 * mxc_set_next_event(), the core has about 4 minutes
+	 * to call mxc_set_next_event() or shutdown clock after
+	 * mode switching
+	 */
+		local_irq_save(flags);
+		imxtm->gpt->gpt_irq_enable(imxtm);
+		local_irq_restore(flags);
+		break;
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_RESUME:
+		/* Left event sources disabled, no more interrupts appear */
+		break;
+	}
+}
+
+/*
+ * IRQ handler for the timer
+ */
+static irqreturn_t mxc_timer_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *ced = dev_id;
+	struct imx_timer *imxtm = to_imx_timer(ced);
+	uint32_t tstat;
+
+	tstat = readl_relaxed(imxtm->base + imxtm->gpt->reg_tstat);
+
+	imxtm->gpt->gpt_irq_acknowledge(imxtm);
+
+	ced->event_handler(ced);
+
+	return IRQ_HANDLED;
+}
+
+static int __init mxc_clockevent_init(struct imx_timer *imxtm)
+{
+	struct clock_event_device *ced = &imxtm->ced;
+	struct irqaction *act = &imxtm->act;
+
+	imxtm->cem = CLOCK_EVT_MODE_UNUSED;
+
+	ced->name = "mxc_timer1";
+	ced->features = CLOCK_EVT_FEAT_ONESHOT;
+	ced->set_mode = mxc_set_mode;
+	ced->set_next_event = imxtm->gpt->set_next_event;
+	ced->rating = 200;
+	ced->cpumask = cpumask_of(0);
+	clockevents_config_and_register(ced, clk_get_rate(imxtm->clk_per),
+					0xff, 0xfffffffe);
+
+	act->name = "i.MX Timer Tick";
+	act->flags = IRQF_TIMER | IRQF_IRQPOLL;
+	act->handler = mxc_timer_interrupt;
+	act->dev_id = ced;
+
+	return setup_irq(imxtm->irq, act);
+}
+
+static void imx1_gpt_setup_tctl(struct imx_timer *imxtm)
+{
+	u32 tctl_val;
+
+	tctl_val = MX1_2_TCTL_FRR | MX1_2_TCTL_CLK_PCLK1 | MXC_TCTL_TEN;
+	writel_relaxed(tctl_val, imxtm->base + MXC_TCTL);
+}
+#define imx21_gpt_setup_tctl imx1_gpt_setup_tctl
+
+static void imx31_gpt_setup_tctl(struct imx_timer *imxtm)
+{
+	u32 tctl_val;
+
+	tctl_val = V2_TCTL_FRR | V2_TCTL_WAITEN | MXC_TCTL_TEN;
+	if (clk_get_rate(imxtm->clk_per) == V2_TIMER_RATE_OSC_DIV8)
+		tctl_val |= V2_TCTL_CLK_OSC_DIV8;
+	else
+		tctl_val |= V2_TCTL_CLK_PER;
+
+	writel_relaxed(tctl_val, imxtm->base + MXC_TCTL);
+}
+
+static void imx6dl_gpt_setup_tctl(struct imx_timer *imxtm)
+{
+	u32 tctl_val;
+
+	tctl_val = V2_TCTL_FRR | V2_TCTL_WAITEN | MXC_TCTL_TEN;
+	if (clk_get_rate(imxtm->clk_per) == V2_TIMER_RATE_OSC_DIV8) {
+		tctl_val |= V2_TCTL_CLK_OSC_DIV8;
+		/* 24 / 8 = 3 MHz */
+		writel_relaxed(7 << V2_TPRER_PRE24M, imxtm->base + MXC_TPRER);
+		tctl_val |= V2_TCTL_24MEN;
+	} else {
+		tctl_val |= V2_TCTL_CLK_PER;
+	}
+
+	writel_relaxed(tctl_val, imxtm->base + MXC_TCTL);
+}
+
+static const struct imx_gpt_data imx1_gpt_data = {
+	.reg_tstat = MX1_2_TSTAT,
+	.reg_tcn = MX1_2_TCN,
+	.reg_tcmp = MX1_2_TCMP,
+	.gpt_irq_enable = imx1_gpt_irq_enable,
+	.gpt_irq_disable = imx1_gpt_irq_disable,
+	.gpt_irq_acknowledge = imx1_gpt_irq_acknowledge,
+	.gpt_setup_tctl = imx1_gpt_setup_tctl,
+	.set_next_event = mx1_2_set_next_event,
+};
+
+static const struct imx_gpt_data imx21_gpt_data = {
+	.reg_tstat = MX1_2_TSTAT,
+	.reg_tcn = MX1_2_TCN,
+	.reg_tcmp = MX1_2_TCMP,
+	.gpt_irq_enable = imx21_gpt_irq_enable,
+	.gpt_irq_disable = imx21_gpt_irq_disable,
+	.gpt_irq_acknowledge = imx21_gpt_irq_acknowledge,
+	.gpt_setup_tctl = imx21_gpt_setup_tctl,
+	.set_next_event = mx1_2_set_next_event,
+};
+
+static const struct imx_gpt_data imx31_gpt_data = {
+	.reg_tstat = V2_TSTAT,
+	.reg_tcn = V2_TCN,
+	.reg_tcmp = V2_TCMP,
+	.gpt_irq_enable = imx31_gpt_irq_enable,
+	.gpt_irq_disable = imx31_gpt_irq_disable,
+	.gpt_irq_acknowledge = imx31_gpt_irq_acknowledge,
+	.gpt_setup_tctl = imx31_gpt_setup_tctl,
+	.set_next_event = v2_set_next_event,
+};
+
+static const struct imx_gpt_data imx6dl_gpt_data = {
+	.reg_tstat = V2_TSTAT,
+	.reg_tcn = V2_TCN,
+	.reg_tcmp = V2_TCMP,
+	.gpt_irq_enable = imx6dl_gpt_irq_enable,
+	.gpt_irq_disable = imx6dl_gpt_irq_disable,
+	.gpt_irq_acknowledge = imx6dl_gpt_irq_acknowledge,
+	.gpt_setup_tctl = imx6dl_gpt_setup_tctl,
+	.set_next_event = v2_set_next_event,
+};
+
+static void __init _mxc_timer_init(struct imx_timer *imxtm)
+{
+	switch (imxtm->type) {
+	case GPT_TYPE_IMX1:
+		imxtm->gpt = &imx1_gpt_data;
+		break;
+	case GPT_TYPE_IMX21:
+		imxtm->gpt = &imx21_gpt_data;
+		break;
+	case GPT_TYPE_IMX31:
+		imxtm->gpt = &imx31_gpt_data;
+		break;
+	case GPT_TYPE_IMX6DL:
+		imxtm->gpt = &imx6dl_gpt_data;
+		break;
+	default:
+		BUG();
+	}
+
+	if (IS_ERR(imxtm->clk_per)) {
+		pr_err("i.MX timer: unable to get clk\n");
+		return;
+	}
+
+	if (!IS_ERR(imxtm->clk_ipg))
+		clk_prepare_enable(imxtm->clk_ipg);
+
+	clk_prepare_enable(imxtm->clk_per);
+
+	/*
+	 * Initialise to a known state (all timers off, and timing reset)
+	 */
+
+	writel_relaxed(0, imxtm->base + MXC_TCTL);
+	writel_relaxed(0, imxtm->base + MXC_TPRER); /* see datasheet note */
+
+	imxtm->gpt->gpt_setup_tctl(imxtm);
+
+	/* init and register the timer to the framework */
+	mxc_clocksource_init(imxtm);
+	mxc_clockevent_init(imxtm);
+}
+
+void __init mxc_timer_init(unsigned long pbase, int irq, enum imx_gpt_type type)
+{
+	struct imx_timer *imxtm;
+
+	imxtm = kzalloc(sizeof(*imxtm), GFP_KERNEL);
+	BUG_ON(!imxtm);
+
+	imxtm->clk_per = clk_get_sys("imx-gpt.0", "per");
+	imxtm->clk_ipg = clk_get_sys("imx-gpt.0", "ipg");
+
+	imxtm->base = ioremap(pbase, SZ_4K);
+	BUG_ON(!imxtm->base);
+
+	imxtm->type = type;
+
+	_mxc_timer_init(imxtm);
+}
+
+static void __init mxc_timer_init_dt(struct device_node *np,  enum imx_gpt_type type)
+{
+	struct imx_timer *imxtm;
+	static int initialized;
+
+	/* Support one instance only */
+	if (initialized)
+		return;
+
+	imxtm = kzalloc(sizeof(*imxtm), GFP_KERNEL);
+	BUG_ON(!imxtm);
+
+	imxtm->base = of_iomap(np, 0);
+	WARN_ON(!imxtm->base);
+	imxtm->irq = irq_of_parse_and_map(np, 0);
+
+	imxtm->clk_ipg = of_clk_get_by_name(np, "ipg");
+
+	/* Try osc_per first, and fall back to per otherwise */
+	imxtm->clk_per = of_clk_get_by_name(np, "osc_per");
+	if (IS_ERR(imxtm->clk_per))
+		imxtm->clk_per = of_clk_get_by_name(np, "per");
+
+	imxtm->type = type;
+
+	_mxc_timer_init(imxtm);
+
+	initialized = 1;
+}
+
+static void __init imx1_timer_init_dt(struct device_node *np)
+{
+	mxc_timer_init_dt(np, GPT_TYPE_IMX1);
+}
+
+static void __init imx21_timer_init_dt(struct device_node *np)
+{
+	mxc_timer_init_dt(np, GPT_TYPE_IMX21);
+}
+
+static void __init imx31_timer_init_dt(struct device_node *np)
+{
+	enum imx_gpt_type type = GPT_TYPE_IMX31;
+
+	/*
+	 * We were using the same compatible string for i.MX6Q/D and i.MX6DL/S
+	 * GPT device, while they actually have different programming model.
+	 * This is a workaround to keep the existing i.MX6DL/S DTBs continue
+	 * working with the new kernel.
+	 */
+	if (of_machine_is_compatible("fsl,imx6dl"))
+		type = GPT_TYPE_IMX6DL;
+
+	mxc_timer_init_dt(np, type);
+}
+
+static void __init imx6dl_timer_init_dt(struct device_node *np)
+{
+	mxc_timer_init_dt(np, GPT_TYPE_IMX6DL);
+}
+
+CLOCKSOURCE_OF_DECLARE(imx1_timer, "fsl,imx1-gpt", imx1_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx21_timer, "fsl,imx21-gpt", imx21_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx27_timer, "fsl,imx27-gpt", imx21_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx31_timer, "fsl,imx31-gpt", imx31_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx25_timer, "fsl,imx25-gpt", imx31_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx50_timer, "fsl,imx50-gpt", imx31_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx51_timer, "fsl,imx51-gpt", imx31_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx53_timer, "fsl,imx53-gpt", imx31_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx6q_timer, "fsl,imx6q-gpt", imx31_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx6dl_timer, "fsl,imx6dl-gpt", imx6dl_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx6sl_timer, "fsl,imx6sl-gpt", imx6dl_timer_init_dt);
+CLOCKSOURCE_OF_DECLARE(imx6sx_timer, "fsl,imx6sx-gpt", imx6dl_timer_init_dt);
diff --git a/drivers/clocksource/timer-integrator-ap.c b/drivers/clocksource/timer-integrator-ap.c
index b9efd30513d5..a68866e0ecd4 100644
--- a/drivers/clocksource/timer-integrator-ap.c
+++ b/drivers/clocksource/timer-integrator-ap.c
@@ -26,7 +26,8 @@
 #include <linux/clockchips.h>
 #include <linux/interrupt.h>
 #include <linux/sched_clock.h>
-#include <asm/hardware/arm_timer.h>
+
+#include "timer-sp.h"
 
 static void __iomem * sched_clk_base;
 
@@ -166,7 +167,7 @@ static void __init integrator_ap_timer_init_of(struct device_node *node)
 	struct device_node *sec_node;
 
 	base = of_io_request_and_map(node, 0, "integrator-timer");
-	if (!base)
+	if (IS_ERR(base))
 		return;
 
 	clk = of_clk_get(node, 0);
diff --git a/drivers/clocksource/timer-sp.h b/drivers/clocksource/timer-sp.h
new file mode 100644
index 000000000000..050d88561e9c
--- /dev/null
+++ b/drivers/clocksource/timer-sp.h
@@ -0,0 +1,30 @@
+/*
+ * ARM timer implementation, found in Integrator, Versatile and Realview
+ * platforms.  Not all platforms support all registers and bits in these
+ * registers, so we mark them with A for Integrator AP, C for Integrator
+ * CP, V for Versatile and R for Realview.
+ *
+ * Integrator AP has 16-bit timers, Integrator CP, Versatile and Realview
+ * can have 16-bit or 32-bit selectable via a bit in the control register.
+ *
+ * Every SP804 contains two identical timers.
+ */
+#define TIMER_1_BASE	0x00
+#define TIMER_2_BASE	0x20
+
+#define TIMER_LOAD	0x00			/* ACVR rw */
+#define TIMER_VALUE	0x04			/* ACVR ro */
+#define TIMER_CTRL	0x08			/* ACVR rw */
+#define TIMER_CTRL_ONESHOT	(1 << 0)	/*  CVR */
+#define TIMER_CTRL_32BIT	(1 << 1)	/*  CVR */
+#define TIMER_CTRL_DIV1		(0 << 2)	/* ACVR */
+#define TIMER_CTRL_DIV16	(1 << 2)	/* ACVR */
+#define TIMER_CTRL_DIV256	(2 << 2)	/* ACVR */
+#define TIMER_CTRL_IE		(1 << 5)	/*   VR */
+#define TIMER_CTRL_PERIODIC	(1 << 6)	/* ACVR */
+#define TIMER_CTRL_ENABLE	(1 << 7)	/* ACVR */
+
+#define TIMER_INTCLR	0x0c			/* ACVR wo */
+#define TIMER_RIS	0x10			/*  CVR ro */
+#define TIMER_MIS	0x14			/*  CVR ro */
+#define TIMER_BGLOAD	0x18			/*  CVR rw */
diff --git a/drivers/clocksource/timer-sp804.c b/drivers/clocksource/timer-sp804.c
new file mode 100644
index 000000000000..ca02503f17d1
--- /dev/null
+++ b/drivers/clocksource/timer-sp804.c
@@ -0,0 +1,310 @@
+/*
+ *  linux/drivers/clocksource/timer-sp.c
+ *
+ *  Copyright (C) 1999 - 2003 ARM Limited
+ *  Copyright (C) 2000 Deep Blue Solutions Ltd
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License 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/clk.h>
+#include <linux/clocksource.h>
+#include <linux/clockchips.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/sched_clock.h>
+
+#include <clocksource/timer-sp804.h>
+
+#include "timer-sp.h"
+
+static long __init sp804_get_clock_rate(struct clk *clk)
+{
+	long rate;
+	int err;
+
+	err = clk_prepare(clk);
+	if (err) {
+		pr_err("sp804: clock failed to prepare: %d\n", err);
+		clk_put(clk);
+		return err;
+	}
+
+	err = clk_enable(clk);
+	if (err) {
+		pr_err("sp804: clock failed to enable: %d\n", err);
+		clk_unprepare(clk);
+		clk_put(clk);
+		return err;
+	}
+
+	rate = clk_get_rate(clk);
+	if (rate < 0) {
+		pr_err("sp804: clock failed to get rate: %ld\n", rate);
+		clk_disable(clk);
+		clk_unprepare(clk);
+		clk_put(clk);
+	}
+
+	return rate;
+}
+
+static void __iomem *sched_clock_base;
+
+static u64 notrace sp804_read(void)
+{
+	return ~readl_relaxed(sched_clock_base + TIMER_VALUE);
+}
+
+void __init sp804_timer_disable(void __iomem *base)
+{
+	writel(0, base + TIMER_CTRL);
+}
+
+void __init __sp804_clocksource_and_sched_clock_init(void __iomem *base,
+						     const char *name,
+						     struct clk *clk,
+						     int use_sched_clock)
+{
+	long rate;
+
+	if (!clk) {
+		clk = clk_get_sys("sp804", name);
+		if (IS_ERR(clk)) {
+			pr_err("sp804: clock not found: %d\n",
+			       (int)PTR_ERR(clk));
+			return;
+		}
+	}
+
+	rate = sp804_get_clock_rate(clk);
+
+	if (rate < 0)
+		return;
+
+	/* setup timer 0 as free-running clocksource */
+	writel(0, base + TIMER_CTRL);
+	writel(0xffffffff, base + TIMER_LOAD);
+	writel(0xffffffff, base + TIMER_VALUE);
+	writel(TIMER_CTRL_32BIT | TIMER_CTRL_ENABLE | TIMER_CTRL_PERIODIC,
+		base + TIMER_CTRL);
+
+	clocksource_mmio_init(base + TIMER_VALUE, name,
+		rate, 200, 32, clocksource_mmio_readl_down);
+
+	if (use_sched_clock) {
+		sched_clock_base = base;
+		sched_clock_register(sp804_read, 32, rate);
+	}
+}
+
+
+static void __iomem *clkevt_base;
+static unsigned long clkevt_reload;
+
+/*
+ * IRQ handler for the timer
+ */
+static irqreturn_t sp804_timer_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = dev_id;
+
+	/* clear the interrupt */
+	writel(1, clkevt_base + TIMER_INTCLR);
+
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static void sp804_set_mode(enum clock_event_mode mode,
+	struct clock_event_device *evt)
+{
+	unsigned long ctrl = TIMER_CTRL_32BIT | TIMER_CTRL_IE;
+
+	writel(ctrl, clkevt_base + TIMER_CTRL);
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		writel(clkevt_reload, clkevt_base + TIMER_LOAD);
+		ctrl |= TIMER_CTRL_PERIODIC | TIMER_CTRL_ENABLE;
+		break;
+
+	case CLOCK_EVT_MODE_ONESHOT:
+		/* period set, and timer enabled in 'next_event' hook */
+		ctrl |= TIMER_CTRL_ONESHOT;
+		break;
+
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	default:
+		break;
+	}
+
+	writel(ctrl, clkevt_base + TIMER_CTRL);
+}
+
+static int sp804_set_next_event(unsigned long next,
+	struct clock_event_device *evt)
+{
+	unsigned long ctrl = readl(clkevt_base + TIMER_CTRL);
+
+	writel(next, clkevt_base + TIMER_LOAD);
+	writel(ctrl | TIMER_CTRL_ENABLE, clkevt_base + TIMER_CTRL);
+
+	return 0;
+}
+
+static struct clock_event_device sp804_clockevent = {
+	.features       = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT |
+		CLOCK_EVT_FEAT_DYNIRQ,
+	.set_mode	= sp804_set_mode,
+	.set_next_event	= sp804_set_next_event,
+	.rating		= 300,
+};
+
+static struct irqaction sp804_timer_irq = {
+	.name		= "timer",
+	.flags		= IRQF_TIMER | IRQF_IRQPOLL,
+	.handler	= sp804_timer_interrupt,
+	.dev_id		= &sp804_clockevent,
+};
+
+void __init __sp804_clockevents_init(void __iomem *base, unsigned int irq, struct clk *clk, const char *name)
+{
+	struct clock_event_device *evt = &sp804_clockevent;
+	long rate;
+
+	if (!clk)
+		clk = clk_get_sys("sp804", name);
+	if (IS_ERR(clk)) {
+		pr_err("sp804: %s clock not found: %d\n", name,
+			(int)PTR_ERR(clk));
+		return;
+	}
+
+	rate = sp804_get_clock_rate(clk);
+	if (rate < 0)
+		return;
+
+	clkevt_base = base;
+	clkevt_reload = DIV_ROUND_CLOSEST(rate, HZ);
+	evt->name = name;
+	evt->irq = irq;
+	evt->cpumask = cpu_possible_mask;
+
+	writel(0, base + TIMER_CTRL);
+
+	setup_irq(irq, &sp804_timer_irq);
+	clockevents_config_and_register(evt, rate, 0xf, 0xffffffff);
+}
+
+static void __init sp804_of_init(struct device_node *np)
+{
+	static bool initialized = false;
+	void __iomem *base;
+	int irq;
+	u32 irq_num = 0;
+	struct clk *clk1, *clk2;
+	const char *name = of_get_property(np, "compatible", NULL);
+
+	base = of_iomap(np, 0);
+	if (WARN_ON(!base))
+		return;
+
+	/* Ensure timers are disabled */
+	writel(0, base + TIMER_CTRL);
+	writel(0, base + TIMER_2_BASE + TIMER_CTRL);
+
+	if (initialized || !of_device_is_available(np))
+		goto err;
+
+	clk1 = of_clk_get(np, 0);
+	if (IS_ERR(clk1))
+		clk1 = NULL;
+
+	/* Get the 2nd clock if the timer has 3 timer clocks */
+	if (of_count_phandle_with_args(np, "clocks", "#clock-cells") == 3) {
+		clk2 = of_clk_get(np, 1);
+		if (IS_ERR(clk2)) {
+			pr_err("sp804: %s clock not found: %d\n", np->name,
+				(int)PTR_ERR(clk2));
+			clk2 = NULL;
+		}
+	} else
+		clk2 = clk1;
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (irq <= 0)
+		goto err;
+
+	of_property_read_u32(np, "arm,sp804-has-irq", &irq_num);
+	if (irq_num == 2) {
+		__sp804_clockevents_init(base + TIMER_2_BASE, irq, clk2, name);
+		__sp804_clocksource_and_sched_clock_init(base, name, clk1, 1);
+	} else {
+		__sp804_clockevents_init(base, irq, clk1 , name);
+		__sp804_clocksource_and_sched_clock_init(base + TIMER_2_BASE,
+							 name, clk2, 1);
+	}
+	initialized = true;
+
+	return;
+err:
+	iounmap(base);
+}
+CLOCKSOURCE_OF_DECLARE(sp804, "arm,sp804", sp804_of_init);
+
+static void __init integrator_cp_of_init(struct device_node *np)
+{
+	static int init_count = 0;
+	void __iomem *base;
+	int irq;
+	const char *name = of_get_property(np, "compatible", NULL);
+	struct clk *clk;
+
+	base = of_iomap(np, 0);
+	if (WARN_ON(!base))
+		return;
+	clk = of_clk_get(np, 0);
+	if (WARN_ON(IS_ERR(clk)))
+		return;
+
+	/* Ensure timer is disabled */
+	writel(0, base + TIMER_CTRL);
+
+	if (init_count == 2 || !of_device_is_available(np))
+		goto err;
+
+	if (!init_count)
+		__sp804_clocksource_and_sched_clock_init(base, name, clk, 0);
+	else {
+		irq = irq_of_parse_and_map(np, 0);
+		if (irq <= 0)
+			goto err;
+
+		__sp804_clockevents_init(base, irq, clk, name);
+	}
+
+	init_count++;
+	return;
+err:
+	iounmap(base);
+}
+CLOCKSOURCE_OF_DECLARE(intcp, "arm,integrator-cp-timer", integrator_cp_of_init);
diff --git a/drivers/clocksource/timer-stm32.c b/drivers/clocksource/timer-stm32.c
new file mode 100644
index 000000000000..a97e8b50701c
--- /dev/null
+++ b/drivers/clocksource/timer-stm32.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (C) Maxime Coquelin 2015
+ * Author:  Maxime Coquelin <mcoquelin.stm32@gmail.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Inspired by time-efm32.c from Uwe Kleine-Koenig
+ */
+
+#include <linux/kernel.h>
+#include <linux/clocksource.h>
+#include <linux/clockchips.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/clk.h>
+#include <linux/reset.h>
+
+#define TIM_CR1		0x00
+#define TIM_DIER	0x0c
+#define TIM_SR		0x10
+#define TIM_EGR		0x14
+#define TIM_PSC		0x28
+#define TIM_ARR		0x2c
+
+#define TIM_CR1_CEN	BIT(0)
+#define TIM_CR1_OPM	BIT(3)
+#define TIM_CR1_ARPE	BIT(7)
+
+#define TIM_DIER_UIE	BIT(0)
+
+#define TIM_SR_UIF	BIT(0)
+
+#define TIM_EGR_UG	BIT(0)
+
+struct stm32_clock_event_ddata {
+	struct clock_event_device evtdev;
+	unsigned periodic_top;
+	void __iomem *base;
+};
+
+static void stm32_clock_event_set_mode(enum clock_event_mode mode,
+				       struct clock_event_device *evtdev)
+{
+	struct stm32_clock_event_ddata *data =
+		container_of(evtdev, struct stm32_clock_event_ddata, evtdev);
+	void *base = data->base;
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		writel_relaxed(data->periodic_top, base + TIM_ARR);
+		writel_relaxed(TIM_CR1_ARPE | TIM_CR1_CEN, base + TIM_CR1);
+		break;
+
+	case CLOCK_EVT_MODE_ONESHOT:
+	default:
+		writel_relaxed(0, base + TIM_CR1);
+		break;
+	}
+}
+
+static int stm32_clock_event_set_next_event(unsigned long evt,
+					    struct clock_event_device *evtdev)
+{
+	struct stm32_clock_event_ddata *data =
+		container_of(evtdev, struct stm32_clock_event_ddata, evtdev);
+
+	writel_relaxed(evt, data->base + TIM_ARR);
+	writel_relaxed(TIM_CR1_ARPE | TIM_CR1_OPM | TIM_CR1_CEN,
+		       data->base + TIM_CR1);
+
+	return 0;
+}
+
+static irqreturn_t stm32_clock_event_handler(int irq, void *dev_id)
+{
+	struct stm32_clock_event_ddata *data = dev_id;
+
+	writel_relaxed(0, data->base + TIM_SR);
+
+	data->evtdev.event_handler(&data->evtdev);
+
+	return IRQ_HANDLED;
+}
+
+static struct stm32_clock_event_ddata clock_event_ddata = {
+	.evtdev = {
+		.name = "stm32 clockevent",
+		.features = CLOCK_EVT_FEAT_ONESHOT | CLOCK_EVT_FEAT_PERIODIC,
+		.set_mode = stm32_clock_event_set_mode,
+		.set_next_event = stm32_clock_event_set_next_event,
+		.rating = 200,
+	},
+};
+
+static void __init stm32_clockevent_init(struct device_node *np)
+{
+	struct stm32_clock_event_ddata *data = &clock_event_ddata;
+	struct clk *clk;
+	struct reset_control *rstc;
+	unsigned long rate, max_delta;
+	int irq, ret, bits, prescaler = 1;
+
+	clk = of_clk_get(np, 0);
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		pr_err("failed to get clock for clockevent (%d)\n", ret);
+		goto err_clk_get;
+	}
+
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		pr_err("failed to enable timer clock for clockevent (%d)\n",
+		       ret);
+		goto err_clk_enable;
+	}
+
+	rate = clk_get_rate(clk);
+
+	rstc = of_reset_control_get(np, NULL);
+	if (!IS_ERR(rstc)) {
+		reset_control_assert(rstc);
+		reset_control_deassert(rstc);
+	}
+
+	data->base = of_iomap(np, 0);
+	if (!data->base) {
+		pr_err("failed to map registers for clockevent\n");
+		goto err_iomap;
+	}
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq) {
+		pr_err("%s: failed to get irq.\n", np->full_name);
+		goto err_get_irq;
+	}
+
+	/* Detect whether the timer is 16 or 32 bits */
+	writel_relaxed(~0U, data->base + TIM_ARR);
+	max_delta = readl_relaxed(data->base + TIM_ARR);
+	if (max_delta == ~0U) {
+		prescaler = 1;
+		bits = 32;
+	} else {
+		prescaler = 1024;
+		bits = 16;
+	}
+	writel_relaxed(0, data->base + TIM_ARR);
+
+	writel_relaxed(prescaler - 1, data->base + TIM_PSC);
+	writel_relaxed(TIM_EGR_UG, data->base + TIM_EGR);
+	writel_relaxed(TIM_DIER_UIE, data->base + TIM_DIER);
+	writel_relaxed(0, data->base + TIM_SR);
+
+	data->periodic_top = DIV_ROUND_CLOSEST(rate, prescaler * HZ);
+
+	clockevents_config_and_register(&data->evtdev,
+					DIV_ROUND_CLOSEST(rate, prescaler),
+					0x1, max_delta);
+
+	ret = request_irq(irq, stm32_clock_event_handler, IRQF_TIMER,
+			"stm32 clockevent", data);
+	if (ret) {
+		pr_err("%s: failed to request irq.\n", np->full_name);
+		goto err_get_irq;
+	}
+
+	pr_info("%s: STM32 clockevent driver initialized (%d bits)\n",
+			np->full_name, bits);
+
+	return;
+
+err_get_irq:
+	iounmap(data->base);
+err_iomap:
+	clk_disable_unprepare(clk);
+err_clk_enable:
+	clk_put(clk);
+err_clk_get:
+	return;
+}
+
+CLOCKSOURCE_OF_DECLARE(stm32, "st,stm32-timer", stm32_clockevent_init);
diff --git a/drivers/clocksource/timer-sun5i.c b/drivers/clocksource/timer-sun5i.c
index 28aa4b7bb602..0ffb4ea7c925 100644
--- a/drivers/clocksource/timer-sun5i.c
+++ b/drivers/clocksource/timer-sun5i.c
@@ -324,7 +324,7 @@ static void __init sun5i_timer_init(struct device_node *node)
 	int irq;
 
 	timer_base = of_io_request_and_map(node, 0, of_node_full_name(node));
-	if (!timer_base)
+	if (IS_ERR(timer_base))
 		panic("Can't map registers");
 
 	irq = irq_of_parse_and_map(node, 0);
diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 4f3dbc8cf729..cc8a71c267b8 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -5,7 +5,7 @@
 # big LITTLE core layer and glue drivers
 config ARM_BIG_LITTLE_CPUFREQ
 	tristate "Generic ARM big LITTLE CPUfreq driver"
-	depends on ARM && BIG_LITTLE && ARM_CPU_TOPOLOGY && HAVE_CLK
+	depends on (ARM_CPU_TOPOLOGY || ARM64) && HAVE_CLK
 	select PM_OPP
 	help
 	  This enables the Generic CPUfreq driver for ARM big.LITTLE platforms.
@@ -36,17 +36,6 @@ config ARM_EXYNOS_CPUFREQ
 
 	  If in doubt, say N.
 
-config ARM_EXYNOS4210_CPUFREQ
-	bool "SAMSUNG EXYNOS4210"
-	depends on CPU_EXYNOS4210
-	depends on ARM_EXYNOS_CPUFREQ
-	default y
-	help
-	  This adds the CPUFreq driver for Samsung EXYNOS4210
-	  SoC (S5PV310 or S5PC210).
-
-	  If in doubt, say N.
-
 config ARM_EXYNOS4X12_CPUFREQ
 	bool "SAMSUNG EXYNOS4x12"
 	depends on SOC_EXYNOS4212 || SOC_EXYNOS4412
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index cdce92ae2e8b..2169bf792db7 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -54,7 +54,6 @@ obj-$(CONFIG_ARCH_DAVINCI)		+= davinci-cpufreq.o
 obj-$(CONFIG_UX500_SOC_DB8500)		+= dbx500-cpufreq.o
 obj-$(CONFIG_ARM_EXYNOS_CPUFREQ)	+= arm-exynos-cpufreq.o
 arm-exynos-cpufreq-y					:= exynos-cpufreq.o
-arm-exynos-cpufreq-$(CONFIG_ARM_EXYNOS4210_CPUFREQ)	+= exynos4210-cpufreq.o
 arm-exynos-cpufreq-$(CONFIG_ARM_EXYNOS4X12_CPUFREQ)	+= exynos4x12-cpufreq.o
 arm-exynos-cpufreq-$(CONFIG_ARM_EXYNOS5250_CPUFREQ)	+= exynos5250-cpufreq.o
 obj-$(CONFIG_ARM_EXYNOS5440_CPUFREQ)	+= exynos5440-cpufreq.o
diff --git a/drivers/cpufreq/acpi-cpufreq.c b/drivers/cpufreq/acpi-cpufreq.c
index b0c18ed8d83f..0136dfcdabf0 100644
--- a/drivers/cpufreq/acpi-cpufreq.c
+++ b/drivers/cpufreq/acpi-cpufreq.c
@@ -699,13 +699,14 @@ static int acpi_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	dmi_check_system(sw_any_bug_dmi_table);
 	if (bios_with_sw_any_bug && !policy_is_shared(policy)) {
 		policy->shared_type = CPUFREQ_SHARED_TYPE_ALL;
-		cpumask_copy(policy->cpus, cpu_core_mask(cpu));
+		cpumask_copy(policy->cpus, topology_core_cpumask(cpu));
 	}
 
 	if (check_amd_hwpstate_cpu(cpu) && !acpi_pstate_strict) {
 		cpumask_clear(policy->cpus);
 		cpumask_set_cpu(cpu, policy->cpus);
-		cpumask_copy(data->freqdomain_cpus, cpu_sibling_mask(cpu));
+		cpumask_copy(data->freqdomain_cpus,
+			     topology_sibling_cpumask(cpu));
 		policy->shared_type = CPUFREQ_SHARED_TYPE_HW;
 		pr_info_once(PFX "overriding BIOS provided _PSD data\n");
 	}
diff --git a/drivers/cpufreq/arm_big_little.c b/drivers/cpufreq/arm_big_little.c
index e1a6ba66a7f5..f1e42f8ce0fc 100644
--- a/drivers/cpufreq/arm_big_little.c
+++ b/drivers/cpufreq/arm_big_little.c
@@ -31,7 +31,6 @@
 #include <linux/slab.h>
 #include <linux/topology.h>
 #include <linux/types.h>
-#include <asm/bL_switcher.h>
 
 #include "arm_big_little.h"
 
@@ -41,12 +40,16 @@
 #define MAX_CLUSTERS	2
 
 #ifdef CONFIG_BL_SWITCHER
+#include <asm/bL_switcher.h>
 static bool bL_switching_enabled;
 #define is_bL_switching_enabled()	bL_switching_enabled
 #define set_switching_enabled(x)	(bL_switching_enabled = (x))
 #else
 #define is_bL_switching_enabled()	false
 #define set_switching_enabled(x)	do { } while (0)
+#define bL_switch_request(...)		do { } while (0)
+#define bL_switcher_put_enabled()	do { } while (0)
+#define bL_switcher_get_enabled()	do { } while (0)
 #endif
 
 #define ACTUAL_FREQ(cluster, freq)  ((cluster == A7_CLUSTER) ? freq << 1 : freq)
@@ -186,6 +189,15 @@ bL_cpufreq_set_rate(u32 cpu, u32 old_cluster, u32 new_cluster, u32 rate)
 		mutex_unlock(&cluster_lock[old_cluster]);
 	}
 
+	/*
+	 * FIXME: clk_set_rate has to handle the case where clk_change_rate
+	 * can fail due to hardware or firmware issues. Until the clk core
+	 * layer is fixed, we can check here. In most of the cases we will
+	 * be reading only the cached value anyway. This needs to  be removed
+	 * once clk core is fixed.
+	 */
+	if (bL_cpufreq_get_rate(cpu) != new_rate)
+		return -EIO;
 	return 0;
 }
 
@@ -322,7 +334,6 @@ static void put_cluster_clk_and_freq_table(struct device *cpu_dev)
 static int _get_cluster_clk_and_freq_table(struct device *cpu_dev)
 {
 	u32 cluster = raw_cpu_to_cluster(cpu_dev->id);
-	char name[14] = "cpu-cluster.";
 	int ret;
 
 	if (freq_table[cluster])
@@ -342,8 +353,7 @@ static int _get_cluster_clk_and_freq_table(struct device *cpu_dev)
 		goto free_opp_table;
 	}
 
-	name[12] = cluster + '0';
-	clk[cluster] = clk_get(cpu_dev, name);
+	clk[cluster] = clk_get(cpu_dev, NULL);
 	if (!IS_ERR(clk[cluster])) {
 		dev_dbg(cpu_dev, "%s: clk: %p & freq table: %p, cluster: %d\n",
 				__func__, clk[cluster], freq_table[cluster],
@@ -506,6 +516,7 @@ static struct cpufreq_driver bL_cpufreq_driver = {
 	.attr			= cpufreq_generic_attr,
 };
 
+#ifdef CONFIG_BL_SWITCHER
 static int bL_cpufreq_switcher_notifier(struct notifier_block *nfb,
 					unsigned long action, void *_arg)
 {
@@ -538,6 +549,20 @@ static struct notifier_block bL_switcher_notifier = {
 	.notifier_call = bL_cpufreq_switcher_notifier,
 };
 
+static int __bLs_register_notifier(void)
+{
+	return bL_switcher_register_notifier(&bL_switcher_notifier);
+}
+
+static int __bLs_unregister_notifier(void)
+{
+	return bL_switcher_unregister_notifier(&bL_switcher_notifier);
+}
+#else
+static int __bLs_register_notifier(void) { return 0; }
+static int __bLs_unregister_notifier(void) { return 0; }
+#endif
+
 int bL_cpufreq_register(struct cpufreq_arm_bL_ops *ops)
 {
 	int ret, i;
@@ -555,8 +580,7 @@ int bL_cpufreq_register(struct cpufreq_arm_bL_ops *ops)
 
 	arm_bL_ops = ops;
 
-	ret = bL_switcher_get_enabled();
-	set_switching_enabled(ret);
+	set_switching_enabled(bL_switcher_get_enabled());
 
 	for (i = 0; i < MAX_CLUSTERS; i++)
 		mutex_init(&cluster_lock[i]);
@@ -567,7 +591,7 @@ int bL_cpufreq_register(struct cpufreq_arm_bL_ops *ops)
 				__func__, ops->name, ret);
 		arm_bL_ops = NULL;
 	} else {
-		ret = bL_switcher_register_notifier(&bL_switcher_notifier);
+		ret = __bLs_register_notifier();
 		if (ret) {
 			cpufreq_unregister_driver(&bL_cpufreq_driver);
 			arm_bL_ops = NULL;
@@ -591,7 +615,7 @@ void bL_cpufreq_unregister(struct cpufreq_arm_bL_ops *ops)
 	}
 
 	bL_switcher_get_enabled();
-	bL_switcher_unregister_notifier(&bL_switcher_notifier);
+	__bLs_unregister_notifier();
 	cpufreq_unregister_driver(&bL_cpufreq_driver);
 	bL_switcher_put_enabled();
 	pr_info("%s: Un-registered platform driver: %s\n", __func__,
diff --git a/drivers/cpufreq/cpufreq-dt.c b/drivers/cpufreq/cpufreq-dt.c
index bab67db54b7e..528a82bf5038 100644
--- a/drivers/cpufreq/cpufreq-dt.c
+++ b/drivers/cpufreq/cpufreq-dt.c
@@ -416,6 +416,7 @@ static struct platform_driver dt_cpufreq_platdrv = {
 };
 module_platform_driver(dt_cpufreq_platdrv);
 
+MODULE_ALIAS("platform:cpufreq-dt");
 MODULE_AUTHOR("Viresh Kumar <viresh.kumar@linaro.org>");
 MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
 MODULE_DESCRIPTION("Generic cpufreq driver");
diff --git a/drivers/cpufreq/cpufreq-nforce2.c b/drivers/cpufreq/cpufreq-nforce2.c
index a2258090b58b..db69eeb501a7 100644
--- a/drivers/cpufreq/cpufreq-nforce2.c
+++ b/drivers/cpufreq/cpufreq-nforce2.c
@@ -414,7 +414,7 @@ static int nforce2_detect_chipset(void)
  * nforce2_init - initializes the nForce2 CPUFreq driver
  *
  * Initializes the nForce2 FSB support. Returns -ENODEV on unsupported
- * devices, -EINVAL on problems during initiatization, and zero on
+ * devices, -EINVAL on problems during initialization, and zero on
  * success.
  */
 static int __init nforce2_init(void)
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 8ae655c364f4..b612411655f9 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -31,10 +31,62 @@
 #include <linux/tick.h>
 #include <trace/events/power.h>
 
-/* Macros to iterate over lists */
-/* Iterate over online CPUs policies */
 static LIST_HEAD(cpufreq_policy_list);
-#define for_each_policy(__policy)				\
+
+static inline bool policy_is_inactive(struct cpufreq_policy *policy)
+{
+	return cpumask_empty(policy->cpus);
+}
+
+static bool suitable_policy(struct cpufreq_policy *policy, bool active)
+{
+	return active == !policy_is_inactive(policy);
+}
+
+/* Finds Next Acive/Inactive policy */
+static struct cpufreq_policy *next_policy(struct cpufreq_policy *policy,
+					  bool active)
+{
+	do {
+		policy = list_next_entry(policy, policy_list);
+
+		/* No more policies in the list */
+		if (&policy->policy_list == &cpufreq_policy_list)
+			return NULL;
+	} while (!suitable_policy(policy, active));
+
+	return policy;
+}
+
+static struct cpufreq_policy *first_policy(bool active)
+{
+	struct cpufreq_policy *policy;
+
+	/* No policies in the list */
+	if (list_empty(&cpufreq_policy_list))
+		return NULL;
+
+	policy = list_first_entry(&cpufreq_policy_list, typeof(*policy),
+				  policy_list);
+
+	if (!suitable_policy(policy, active))
+		policy = next_policy(policy, active);
+
+	return policy;
+}
+
+/* Macros to iterate over CPU policies */
+#define for_each_suitable_policy(__policy, __active)	\
+	for (__policy = first_policy(__active);		\
+	     __policy;					\
+	     __policy = next_policy(__policy, __active))
+
+#define for_each_active_policy(__policy)		\
+	for_each_suitable_policy(__policy, true)
+#define for_each_inactive_policy(__policy)		\
+	for_each_suitable_policy(__policy, false)
+
+#define for_each_policy(__policy)			\
 	list_for_each_entry(__policy, &cpufreq_policy_list, policy_list)
 
 /* Iterate over governors */
@@ -49,13 +101,9 @@ static LIST_HEAD(cpufreq_governor_list);
  */
 static struct cpufreq_driver *cpufreq_driver;
 static DEFINE_PER_CPU(struct cpufreq_policy *, cpufreq_cpu_data);
-static DEFINE_PER_CPU(struct cpufreq_policy *, cpufreq_cpu_data_fallback);
 static DEFINE_RWLOCK(cpufreq_driver_lock);
 DEFINE_MUTEX(cpufreq_governor_lock);
 
-/* This one keeps track of the previously set governor of a removed CPU */
-static DEFINE_PER_CPU(char[CPUFREQ_NAME_LEN], cpufreq_cpu_governor);
-
 /* Flag to suspend/resume CPUFreq governors */
 static bool cpufreq_suspended;
 
@@ -178,7 +226,7 @@ int cpufreq_generic_init(struct cpufreq_policy *policy,
 	policy->cpuinfo.transition_latency = transition_latency;
 
 	/*
-	 * The driver only supports the SMP configuartion where all processors
+	 * The driver only supports the SMP configuration where all processors
 	 * share the clock and voltage and clock.
 	 */
 	cpumask_setall(policy->cpus);
@@ -187,10 +235,18 @@ int cpufreq_generic_init(struct cpufreq_policy *policy,
 }
 EXPORT_SYMBOL_GPL(cpufreq_generic_init);
 
-unsigned int cpufreq_generic_get(unsigned int cpu)
+/* Only for cpufreq core internal use */
+struct cpufreq_policy *cpufreq_cpu_get_raw(unsigned int cpu)
 {
 	struct cpufreq_policy *policy = per_cpu(cpufreq_cpu_data, cpu);
 
+	return policy && cpumask_test_cpu(cpu, policy->cpus) ? policy : NULL;
+}
+
+unsigned int cpufreq_generic_get(unsigned int cpu)
+{
+	struct cpufreq_policy *policy = cpufreq_cpu_get_raw(cpu);
+
 	if (!policy || IS_ERR(policy->clk)) {
 		pr_err("%s: No %s associated to cpu: %d\n",
 		       __func__, policy ? "clk" : "policy", cpu);
@@ -201,18 +257,29 @@ unsigned int cpufreq_generic_get(unsigned int cpu)
 }
 EXPORT_SYMBOL_GPL(cpufreq_generic_get);
 
-/* Only for cpufreq core internal use */
-struct cpufreq_policy *cpufreq_cpu_get_raw(unsigned int cpu)
-{
-	return per_cpu(cpufreq_cpu_data, cpu);
-}
-
+/**
+ * cpufreq_cpu_get: returns policy for a cpu and marks it busy.
+ *
+ * @cpu: cpu to find policy for.
+ *
+ * This returns policy for 'cpu', returns NULL if it doesn't exist.
+ * It also increments the kobject reference count to mark it busy and so would
+ * require a corresponding call to cpufreq_cpu_put() to decrement it back.
+ * If corresponding call cpufreq_cpu_put() isn't made, the policy wouldn't be
+ * freed as that depends on the kobj count.
+ *
+ * It also takes a read-lock of 'cpufreq_rwsem' and doesn't put it back if a
+ * valid policy is found. This is done to make sure the driver doesn't get
+ * unregistered while the policy is being used.
+ *
+ * Return: A valid policy on success, otherwise NULL on failure.
+ */
 struct cpufreq_policy *cpufreq_cpu_get(unsigned int cpu)
 {
 	struct cpufreq_policy *policy = NULL;
 	unsigned long flags;
 
-	if (cpu >= nr_cpu_ids)
+	if (WARN_ON(cpu >= nr_cpu_ids))
 		return NULL;
 
 	if (!down_read_trylock(&cpufreq_rwsem))
@@ -223,7 +290,7 @@ struct cpufreq_policy *cpufreq_cpu_get(unsigned int cpu)
 
 	if (cpufreq_driver) {
 		/* get the CPU */
-		policy = per_cpu(cpufreq_cpu_data, cpu);
+		policy = cpufreq_cpu_get_raw(cpu);
 		if (policy)
 			kobject_get(&policy->kobj);
 	}
@@ -237,6 +304,16 @@ struct cpufreq_policy *cpufreq_cpu_get(unsigned int cpu)
 }
 EXPORT_SYMBOL_GPL(cpufreq_cpu_get);
 
+/**
+ * cpufreq_cpu_put: Decrements the usage count of a policy
+ *
+ * @policy: policy earlier returned by cpufreq_cpu_get().
+ *
+ * This decrements the kobject reference count incremented earlier by calling
+ * cpufreq_cpu_get().
+ *
+ * It also drops the read-lock of 'cpufreq_rwsem' taken at cpufreq_cpu_get().
+ */
 void cpufreq_cpu_put(struct cpufreq_policy *policy)
 {
 	kobject_put(&policy->kobj);
@@ -798,11 +875,18 @@ static ssize_t store(struct kobject *kobj, struct attribute *attr,
 
 	down_write(&policy->rwsem);
 
+	/* Updating inactive policies is invalid, so avoid doing that. */
+	if (unlikely(policy_is_inactive(policy))) {
+		ret = -EBUSY;
+		goto unlock_policy_rwsem;
+	}
+
 	if (fattr->store)
 		ret = fattr->store(policy, buf, count);
 	else
 		ret = -EIO;
 
+unlock_policy_rwsem:
 	up_write(&policy->rwsem);
 
 	up_read(&cpufreq_rwsem);
@@ -873,28 +957,67 @@ void cpufreq_sysfs_remove_file(const struct attribute *attr)
 }
 EXPORT_SYMBOL(cpufreq_sysfs_remove_file);
 
-/* symlink affected CPUs */
+static int add_cpu_dev_symlink(struct cpufreq_policy *policy, int cpu)
+{
+	struct device *cpu_dev;
+
+	pr_debug("%s: Adding symlink for CPU: %u\n", __func__, cpu);
+
+	if (!policy)
+		return 0;
+
+	cpu_dev = get_cpu_device(cpu);
+	if (WARN_ON(!cpu_dev))
+		return 0;
+
+	return sysfs_create_link(&cpu_dev->kobj, &policy->kobj, "cpufreq");
+}
+
+static void remove_cpu_dev_symlink(struct cpufreq_policy *policy, int cpu)
+{
+	struct device *cpu_dev;
+
+	pr_debug("%s: Removing symlink for CPU: %u\n", __func__, cpu);
+
+	cpu_dev = get_cpu_device(cpu);
+	if (WARN_ON(!cpu_dev))
+		return;
+
+	sysfs_remove_link(&cpu_dev->kobj, "cpufreq");
+}
+
+/* Add/remove symlinks for all related CPUs */
 static int cpufreq_add_dev_symlink(struct cpufreq_policy *policy)
 {
 	unsigned int j;
 	int ret = 0;
 
-	for_each_cpu(j, policy->cpus) {
-		struct device *cpu_dev;
-
-		if (j == policy->cpu)
+	/* Some related CPUs might not be present (physically hotplugged) */
+	for_each_cpu_and(j, policy->related_cpus, cpu_present_mask) {
+		if (j == policy->kobj_cpu)
 			continue;
 
-		pr_debug("Adding link for CPU: %u\n", j);
-		cpu_dev = get_cpu_device(j);
-		ret = sysfs_create_link(&cpu_dev->kobj, &policy->kobj,
-					"cpufreq");
+		ret = add_cpu_dev_symlink(policy, j);
 		if (ret)
 			break;
 	}
+
 	return ret;
 }
 
+static void cpufreq_remove_dev_symlink(struct cpufreq_policy *policy)
+{
+	unsigned int j;
+
+	/* Some related CPUs might not be present (physically hotplugged) */
+	for_each_cpu_and(j, policy->related_cpus, cpu_present_mask) {
+		if (j == policy->kobj_cpu)
+			continue;
+
+		remove_cpu_dev_symlink(policy, j);
+	}
+}
+
 static int cpufreq_add_dev_interface(struct cpufreq_policy *policy,
 				     struct device *dev)
 {
@@ -937,7 +1060,7 @@ static void cpufreq_init_policy(struct cpufreq_policy *policy)
 	memcpy(&new_policy, policy, sizeof(*policy));
 
 	/* Update governor of new_policy to the governor used before hotplug */
-	gov = find_governor(per_cpu(cpufreq_cpu_governor, policy->cpu));
+	gov = find_governor(policy->last_governor);
 	if (gov)
 		pr_debug("Restoring governor %s for cpu %d\n",
 				policy->governor->name, policy->cpu);
@@ -963,7 +1086,10 @@ static int cpufreq_add_policy_cpu(struct cpufreq_policy *policy,
 				  unsigned int cpu, struct device *dev)
 {
 	int ret = 0;
-	unsigned long flags;
+
+	/* Has this CPU been taken care of already? */
+	if (cpumask_test_cpu(cpu, policy->cpus))
+		return 0;
 
 	if (has_target()) {
 		ret = __cpufreq_governor(policy, CPUFREQ_GOV_STOP);
@@ -974,13 +1100,7 @@ static int cpufreq_add_policy_cpu(struct cpufreq_policy *policy,
 	}
 
 	down_write(&policy->rwsem);
-
-	write_lock_irqsave(&cpufreq_driver_lock, flags);
-
 	cpumask_set_cpu(cpu, policy->cpus);
-	per_cpu(cpufreq_cpu_data, cpu) = policy;
-	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
-
 	up_write(&policy->rwsem);
 
 	if (has_target()) {
@@ -994,7 +1114,7 @@ static int cpufreq_add_policy_cpu(struct cpufreq_policy *policy,
 		}
 	}
 
-	return sysfs_create_link(&dev->kobj, &policy->kobj, "cpufreq");
+	return 0;
 }
 
 static struct cpufreq_policy *cpufreq_policy_restore(unsigned int cpu)
@@ -1003,20 +1123,25 @@ static struct cpufreq_policy *cpufreq_policy_restore(unsigned int cpu)
 	unsigned long flags;
 
 	read_lock_irqsave(&cpufreq_driver_lock, flags);
-
-	policy = per_cpu(cpufreq_cpu_data_fallback, cpu);
-
+	policy = per_cpu(cpufreq_cpu_data, cpu);
 	read_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
-	if (policy)
-		policy->governor = NULL;
+	if (likely(policy)) {
+		/* Policy should be inactive here */
+		WARN_ON(!policy_is_inactive(policy));
+
+		down_write(&policy->rwsem);
+		policy->cpu = cpu;
+		up_write(&policy->rwsem);
+	}
 
 	return policy;
 }
 
-static struct cpufreq_policy *cpufreq_policy_alloc(void)
+static struct cpufreq_policy *cpufreq_policy_alloc(struct device *dev)
 {
 	struct cpufreq_policy *policy;
+	int ret;
 
 	policy = kzalloc(sizeof(*policy), GFP_KERNEL);
 	if (!policy)
@@ -1028,6 +1153,13 @@ static struct cpufreq_policy *cpufreq_policy_alloc(void)
 	if (!zalloc_cpumask_var(&policy->related_cpus, GFP_KERNEL))
 		goto err_free_cpumask;
 
+	ret = kobject_init_and_add(&policy->kobj, &ktype_cpufreq, &dev->kobj,
+				   "cpufreq");
+	if (ret) {
+		pr_err("%s: failed to init policy->kobj: %d\n", __func__, ret);
+		goto err_free_rcpumask;
+	}
+
 	INIT_LIST_HEAD(&policy->policy_list);
 	init_rwsem(&policy->rwsem);
 	spin_lock_init(&policy->transition_lock);
@@ -1035,8 +1167,15 @@ static struct cpufreq_policy *cpufreq_policy_alloc(void)
 	init_completion(&policy->kobj_unregister);
 	INIT_WORK(&policy->update, handle_update);
 
+	policy->cpu = dev->id;
+
+	/* Set this once on allocation */
+	policy->kobj_cpu = dev->id;
+
 	return policy;
 
+err_free_rcpumask:
+	free_cpumask_var(policy->related_cpus);
 err_free_cpumask:
 	free_cpumask_var(policy->cpus);
 err_free_policy:
@@ -1045,18 +1184,20 @@ err_free_policy:
 	return NULL;
 }
 
-static void cpufreq_policy_put_kobj(struct cpufreq_policy *policy)
+static void cpufreq_policy_put_kobj(struct cpufreq_policy *policy, bool notify)
 {
 	struct kobject *kobj;
 	struct completion *cmp;
 
-	blocking_notifier_call_chain(&cpufreq_policy_notifier_list,
-			CPUFREQ_REMOVE_POLICY, policy);
+	if (notify)
+		blocking_notifier_call_chain(&cpufreq_policy_notifier_list,
+					     CPUFREQ_REMOVE_POLICY, policy);
 
-	down_read(&policy->rwsem);
+	down_write(&policy->rwsem);
+	cpufreq_remove_dev_symlink(policy);
 	kobj = &policy->kobj;
 	cmp = &policy->kobj_unregister;
-	up_read(&policy->rwsem);
+	up_write(&policy->rwsem);
 	kobject_put(kobj);
 
 	/*
@@ -1069,68 +1210,64 @@ static void cpufreq_policy_put_kobj(struct cpufreq_policy *policy)
 	pr_debug("wait complete\n");
 }
 
-static void cpufreq_policy_free(struct cpufreq_policy *policy)
-{
-	free_cpumask_var(policy->related_cpus);
-	free_cpumask_var(policy->cpus);
-	kfree(policy);
-}
-
-static int update_policy_cpu(struct cpufreq_policy *policy, unsigned int cpu,
-			     struct device *cpu_dev)
+static void cpufreq_policy_free(struct cpufreq_policy *policy, bool notify)
 {
-	int ret;
-
-	if (WARN_ON(cpu == policy->cpu))
-		return 0;
+	unsigned long flags;
+	int cpu;
 
-	/* Move kobject to the new policy->cpu */
-	ret = kobject_move(&policy->kobj, &cpu_dev->kobj);
-	if (ret) {
-		pr_err("%s: Failed to move kobj: %d\n", __func__, ret);
-		return ret;
-	}
+	/* Remove policy from list */
+	write_lock_irqsave(&cpufreq_driver_lock, flags);
+	list_del(&policy->policy_list);
 
-	down_write(&policy->rwsem);
-	policy->cpu = cpu;
-	up_write(&policy->rwsem);
+	for_each_cpu(cpu, policy->related_cpus)
+		per_cpu(cpufreq_cpu_data, cpu) = NULL;
+	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
-	return 0;
+	cpufreq_policy_put_kobj(policy, notify);
+	free_cpumask_var(policy->related_cpus);
+	free_cpumask_var(policy->cpus);
+	kfree(policy);
 }
 
-static int __cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
+/**
+ * cpufreq_add_dev - add a CPU device
+ *
+ * Adds the cpufreq interface for a CPU device.
+ *
+ * The Oracle says: try running cpufreq registration/unregistration concurrently
+ * with with cpu hotplugging and all hell will break loose. Tried to clean this
+ * mess up, but more thorough testing is needed. - Mathieu
+ */
+static int cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
 {
 	unsigned int j, cpu = dev->id;
 	int ret = -ENOMEM;
 	struct cpufreq_policy *policy;
 	unsigned long flags;
-	bool recover_policy = cpufreq_suspended;
-
-	if (cpu_is_offline(cpu))
-		return 0;
+	bool recover_policy = !sif;
 
 	pr_debug("adding CPU %u\n", cpu);
 
-	/* check whether a different CPU already registered this
-	 * CPU because it is in the same boat. */
-	policy = cpufreq_cpu_get_raw(cpu);
-	if (unlikely(policy))
-		return 0;
+	/*
+	 * Only possible if 'cpu' wasn't physically present earlier and we are
+	 * here from subsys_interface add callback. A hotplug notifier will
+	 * follow and we will handle it like logical CPU hotplug then. For now,
+	 * just create the sysfs link.
+	 */
+	if (cpu_is_offline(cpu))
+		return add_cpu_dev_symlink(per_cpu(cpufreq_cpu_data, cpu), cpu);
 
 	if (!down_read_trylock(&cpufreq_rwsem))
 		return 0;
 
-	/* Check if this cpu was hot-unplugged earlier and has siblings */
-	read_lock_irqsave(&cpufreq_driver_lock, flags);
-	for_each_policy(policy) {
-		if (cpumask_test_cpu(cpu, policy->related_cpus)) {
-			read_unlock_irqrestore(&cpufreq_driver_lock, flags);
-			ret = cpufreq_add_policy_cpu(policy, cpu, dev);
-			up_read(&cpufreq_rwsem);
-			return ret;
-		}
+	/* Check if this CPU already has a policy to manage it */
+	policy = per_cpu(cpufreq_cpu_data, cpu);
+	if (policy && !policy_is_inactive(policy)) {
+		WARN_ON(!cpumask_test_cpu(cpu, policy->related_cpus));
+		ret = cpufreq_add_policy_cpu(policy, cpu, dev);
+		up_read(&cpufreq_rwsem);
+		return ret;
 	}
-	read_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 	/*
 	 * Restore the saved policy when doing light-weight init and fall back
@@ -1139,22 +1276,11 @@ static int __cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
 	policy = recover_policy ? cpufreq_policy_restore(cpu) : NULL;
 	if (!policy) {
 		recover_policy = false;
-		policy = cpufreq_policy_alloc();
+		policy = cpufreq_policy_alloc(dev);
 		if (!policy)
 			goto nomem_out;
 	}
 
-	/*
-	 * In the resume path, since we restore a saved policy, the assignment
-	 * to policy->cpu is like an update of the existing policy, rather than
-	 * the creation of a brand new one. So we need to perform this update
-	 * by invoking update_policy_cpu().
-	 */
-	if (recover_policy && cpu != policy->cpu)
-		WARN_ON(update_policy_cpu(policy, cpu, dev));
-	else
-		policy->cpu = cpu;
-
 	cpumask_copy(policy->cpus, cpumask_of(cpu));
 
 	/* call driver. From then on the cpufreq must be able
@@ -1181,21 +1307,12 @@ static int __cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
 		policy->user_policy.min = policy->min;
 		policy->user_policy.max = policy->max;
 
-		/* prepare interface data */
-		ret = kobject_init_and_add(&policy->kobj, &ktype_cpufreq,
-					   &dev->kobj, "cpufreq");
-		if (ret) {
-			pr_err("%s: failed to init policy->kobj: %d\n",
-			       __func__, ret);
-			goto err_init_policy_kobj;
-		}
+		write_lock_irqsave(&cpufreq_driver_lock, flags);
+		for_each_cpu(j, policy->related_cpus)
+			per_cpu(cpufreq_cpu_data, j) = policy;
+		write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 	}
 
-	write_lock_irqsave(&cpufreq_driver_lock, flags);
-	for_each_cpu(j, policy->cpus)
-		per_cpu(cpufreq_cpu_data, j) = policy;
-	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
-
 	if (cpufreq_driver->get && !cpufreq_driver->setpolicy) {
 		policy->cur = cpufreq_driver->get(policy->cpu);
 		if (!policy->cur) {
@@ -1253,11 +1370,11 @@ static int __cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
 			goto err_out_unregister;
 		blocking_notifier_call_chain(&cpufreq_policy_notifier_list,
 				CPUFREQ_CREATE_POLICY, policy);
-	}
 
-	write_lock_irqsave(&cpufreq_driver_lock, flags);
-	list_add(&policy->policy_list, &cpufreq_policy_list);
-	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
+		write_lock_irqsave(&cpufreq_driver_lock, flags);
+		list_add(&policy->policy_list, &cpufreq_policy_list);
+		write_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	}
 
 	cpufreq_init_policy(policy);
 
@@ -1281,68 +1398,28 @@ static int __cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
 
 err_out_unregister:
 err_get_freq:
-	write_lock_irqsave(&cpufreq_driver_lock, flags);
-	for_each_cpu(j, policy->cpus)
-		per_cpu(cpufreq_cpu_data, j) = NULL;
-	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
-
-	if (!recover_policy) {
-		kobject_put(&policy->kobj);
-		wait_for_completion(&policy->kobj_unregister);
-	}
-err_init_policy_kobj:
 	up_write(&policy->rwsem);
 
 	if (cpufreq_driver->exit)
 		cpufreq_driver->exit(policy);
 err_set_policy_cpu:
-	if (recover_policy) {
-		/* Do not leave stale fallback data behind. */
-		per_cpu(cpufreq_cpu_data_fallback, cpu) = NULL;
-		cpufreq_policy_put_kobj(policy);
-	}
-	cpufreq_policy_free(policy);
-
+	cpufreq_policy_free(policy, recover_policy);
 nomem_out:
 	up_read(&cpufreq_rwsem);
 
 	return ret;
 }
 
-/**
- * cpufreq_add_dev - add a CPU device
- *
- * Adds the cpufreq interface for a CPU device.
- *
- * The Oracle says: try running cpufreq registration/unregistration concurrently
- * with with cpu hotplugging and all hell will break loose. Tried to clean this
- * mess up, but more thorough testing is needed. - Mathieu
- */
-static int cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
-{
-	return __cpufreq_add_dev(dev, sif);
-}
-
 static int __cpufreq_remove_dev_prepare(struct device *dev,
 					struct subsys_interface *sif)
 {
-	unsigned int cpu = dev->id, cpus;
-	int ret;
-	unsigned long flags;
+	unsigned int cpu = dev->id;
+	int ret = 0;
 	struct cpufreq_policy *policy;
 
 	pr_debug("%s: unregistering CPU %u\n", __func__, cpu);
 
-	write_lock_irqsave(&cpufreq_driver_lock, flags);
-
-	policy = per_cpu(cpufreq_cpu_data, cpu);
-
-	/* Save the policy somewhere when doing a light-weight tear-down */
-	if (cpufreq_suspended)
-		per_cpu(cpufreq_cpu_data_fallback, cpu) = policy;
-
-	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
-
+	policy = cpufreq_cpu_get_raw(cpu);
 	if (!policy) {
 		pr_debug("%s: No cpu_data found\n", __func__);
 		return -EINVAL;
@@ -1354,108 +1431,75 @@ static int __cpufreq_remove_dev_prepare(struct device *dev,
 			pr_err("%s: Failed to stop governor\n", __func__);
 			return ret;
 		}
-
-		strncpy(per_cpu(cpufreq_cpu_governor, cpu),
-			policy->governor->name, CPUFREQ_NAME_LEN);
 	}
 
-	down_read(&policy->rwsem);
-	cpus = cpumask_weight(policy->cpus);
-	up_read(&policy->rwsem);
+	down_write(&policy->rwsem);
+	cpumask_clear_cpu(cpu, policy->cpus);
 
-	if (cpu != policy->cpu) {
-		sysfs_remove_link(&dev->kobj, "cpufreq");
-	} else if (cpus > 1) {
+	if (policy_is_inactive(policy)) {
+		if (has_target())
+			strncpy(policy->last_governor, policy->governor->name,
+				CPUFREQ_NAME_LEN);
+	} else if (cpu == policy->cpu) {
 		/* Nominate new CPU */
-		int new_cpu = cpumask_any_but(policy->cpus, cpu);
-		struct device *cpu_dev = get_cpu_device(new_cpu);
+		policy->cpu = cpumask_any(policy->cpus);
+	}
+	up_write(&policy->rwsem);
 
-		sysfs_remove_link(&cpu_dev->kobj, "cpufreq");
-		ret = update_policy_cpu(policy, new_cpu, cpu_dev);
-		if (ret) {
-			if (sysfs_create_link(&cpu_dev->kobj, &policy->kobj,
-					      "cpufreq"))
-				pr_err("%s: Failed to restore kobj link to cpu:%d\n",
-				       __func__, cpu_dev->id);
-			return ret;
-		}
+	/* Start governor again for active policy */
+	if (!policy_is_inactive(policy)) {
+		if (has_target()) {
+			ret = __cpufreq_governor(policy, CPUFREQ_GOV_START);
+			if (!ret)
+				ret = __cpufreq_governor(policy, CPUFREQ_GOV_LIMITS);
 
-		if (!cpufreq_suspended)
-			pr_debug("%s: policy Kobject moved to cpu: %d from: %d\n",
-				 __func__, new_cpu, cpu);
+			if (ret)
+				pr_err("%s: Failed to start governor\n", __func__);
+		}
 	} else if (cpufreq_driver->stop_cpu) {
 		cpufreq_driver->stop_cpu(policy);
 	}
 
-	return 0;
+	return ret;
 }
 
 static int __cpufreq_remove_dev_finish(struct device *dev,
 				       struct subsys_interface *sif)
 {
-	unsigned int cpu = dev->id, cpus;
+	unsigned int cpu = dev->id;
 	int ret;
-	unsigned long flags;
-	struct cpufreq_policy *policy;
-
-	write_lock_irqsave(&cpufreq_driver_lock, flags);
-	policy = per_cpu(cpufreq_cpu_data, cpu);
-	per_cpu(cpufreq_cpu_data, cpu) = NULL;
-	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
+	struct cpufreq_policy *policy = per_cpu(cpufreq_cpu_data, cpu);
 
 	if (!policy) {
 		pr_debug("%s: No cpu_data found\n", __func__);
 		return -EINVAL;
 	}
 
-	down_write(&policy->rwsem);
-	cpus = cpumask_weight(policy->cpus);
-
-	if (cpus > 1)
-		cpumask_clear_cpu(cpu, policy->cpus);
-	up_write(&policy->rwsem);
+	/* Only proceed for inactive policies */
+	if (!policy_is_inactive(policy))
+		return 0;
 
 	/* If cpu is last user of policy, free policy */
-	if (cpus == 1) {
-		if (has_target()) {
-			ret = __cpufreq_governor(policy,
-					CPUFREQ_GOV_POLICY_EXIT);
-			if (ret) {
-				pr_err("%s: Failed to exit governor\n",
-				       __func__);
-				return ret;
-			}
-		}
-
-		if (!cpufreq_suspended)
-			cpufreq_policy_put_kobj(policy);
-
-		/*
-		 * Perform the ->exit() even during light-weight tear-down,
-		 * since this is a core component, and is essential for the
-		 * subsequent light-weight ->init() to succeed.
-		 */
-		if (cpufreq_driver->exit)
-			cpufreq_driver->exit(policy);
-
-		/* Remove policy from list of active policies */
-		write_lock_irqsave(&cpufreq_driver_lock, flags);
-		list_del(&policy->policy_list);
-		write_unlock_irqrestore(&cpufreq_driver_lock, flags);
-
-		if (!cpufreq_suspended)
-			cpufreq_policy_free(policy);
-	} else if (has_target()) {
-		ret = __cpufreq_governor(policy, CPUFREQ_GOV_START);
-		if (!ret)
-			ret = __cpufreq_governor(policy, CPUFREQ_GOV_LIMITS);
-
+	if (has_target()) {
+		ret = __cpufreq_governor(policy, CPUFREQ_GOV_POLICY_EXIT);
 		if (ret) {
-			pr_err("%s: Failed to start governor\n", __func__);
+			pr_err("%s: Failed to exit governor\n", __func__);
 			return ret;
 		}
 	}
 
+	/*
+	 * Perform the ->exit() even during light-weight tear-down,
+	 * since this is a core component, and is essential for the
+	 * subsequent light-weight ->init() to succeed.
+	 */
+	if (cpufreq_driver->exit)
+		cpufreq_driver->exit(policy);
+
+	/* Free the policy only if the driver is getting removed. */
+	if (sif)
+		cpufreq_policy_free(policy, true);
+
 	return 0;
 }
 
@@ -1469,8 +1513,33 @@ static int cpufreq_remove_dev(struct device *dev, struct subsys_interface *sif)
 	unsigned int cpu = dev->id;
 	int ret;
 
-	if (cpu_is_offline(cpu))
+	/*
+	 * Only possible if 'cpu' is getting physically removed now. A hotplug
+	 * notifier should have already been called and we just need to remove
+	 * link or free policy here.
+	 */
+	if (cpu_is_offline(cpu)) {
+		struct cpufreq_policy *policy = per_cpu(cpufreq_cpu_data, cpu);
+		struct cpumask mask;
+
+		if (!policy)
+			return 0;
+
+		cpumask_copy(&mask, policy->related_cpus);
+		cpumask_clear_cpu(cpu, &mask);
+
+		/*
+		 * Free policy only if all policy->related_cpus are removed
+		 * physically.
+		 */
+		if (cpumask_intersects(&mask, cpu_present_mask)) {
+			remove_cpu_dev_symlink(policy, cpu);
+			return 0;
+		}
+
+		cpufreq_policy_free(policy, true);
 		return 0;
+	}
 
 	ret = __cpufreq_remove_dev_prepare(dev, sif);
 
@@ -1567,6 +1636,10 @@ static unsigned int __cpufreq_get(struct cpufreq_policy *policy)
 
 	ret_freq = cpufreq_driver->get(policy->cpu);
 
+	/* Updating inactive policies is invalid, so avoid doing that. */
+	if (unlikely(policy_is_inactive(policy)))
+		return ret_freq;
+
 	if (ret_freq && policy->cur &&
 		!(cpufreq_driver->flags & CPUFREQ_CONST_LOOPS)) {
 		/* verify no discrepancy between actual and
@@ -1656,7 +1729,7 @@ void cpufreq_suspend(void)
 
 	pr_debug("%s: Suspending Governors\n", __func__);
 
-	for_each_policy(policy) {
+	for_each_active_policy(policy) {
 		if (__cpufreq_governor(policy, CPUFREQ_GOV_STOP))
 			pr_err("%s: Failed to stop governor for policy: %p\n",
 				__func__, policy);
@@ -1690,7 +1763,7 @@ void cpufreq_resume(void)
 
 	pr_debug("%s: Resuming Governors\n", __func__);
 
-	for_each_policy(policy) {
+	for_each_active_policy(policy) {
 		if (cpufreq_driver->resume && cpufreq_driver->resume(policy))
 			pr_err("%s: Failed to resume driver: %p\n", __func__,
 				policy);
@@ -1891,7 +1964,7 @@ static int __target_index(struct cpufreq_policy *policy,
 		 * Failed after setting to intermediate freq? Driver should have
 		 * reverted back to initial frequency and so should we. Check
 		 * here for intermediate_freq instead of get_intermediate, in
-		 * case we have't switched to intermediate freq at all.
+		 * case we haven't switched to intermediate freq at all.
 		 */
 		if (unlikely(retval && intermediate_freq)) {
 			freqs.old = intermediate_freq;
@@ -2092,7 +2165,8 @@ EXPORT_SYMBOL_GPL(cpufreq_register_governor);
 
 void cpufreq_unregister_governor(struct cpufreq_governor *governor)
 {
-	int cpu;
+	struct cpufreq_policy *policy;
+	unsigned long flags;
 
 	if (!governor)
 		return;
@@ -2100,12 +2174,15 @@ void cpufreq_unregister_governor(struct cpufreq_governor *governor)
 	if (cpufreq_disabled())
 		return;
 
-	for_each_present_cpu(cpu) {
-		if (cpu_online(cpu))
-			continue;
-		if (!strcmp(per_cpu(cpufreq_cpu_governor, cpu), governor->name))
-			strcpy(per_cpu(cpufreq_cpu_governor, cpu), "\0");
+	/* clear last_governor for all inactive policies */
+	read_lock_irqsave(&cpufreq_driver_lock, flags);
+	for_each_inactive_policy(policy) {
+		if (!strcmp(policy->last_governor, governor->name)) {
+			policy->governor = NULL;
+			strcpy(policy->last_governor, "\0");
+		}
 	}
+	read_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
 	mutex_lock(&cpufreq_governor_mutex);
 	list_del(&governor->governor_list);
@@ -2304,7 +2381,7 @@ static int cpufreq_cpu_callback(struct notifier_block *nfb,
 	if (dev) {
 		switch (action & ~CPU_TASKS_FROZEN) {
 		case CPU_ONLINE:
-			__cpufreq_add_dev(dev, NULL);
+			cpufreq_add_dev(dev, NULL);
 			break;
 
 		case CPU_DOWN_PREPARE:
@@ -2316,7 +2393,7 @@ static int cpufreq_cpu_callback(struct notifier_block *nfb,
 			break;
 
 		case CPU_DOWN_FAILED:
-			__cpufreq_add_dev(dev, NULL);
+			cpufreq_add_dev(dev, NULL);
 			break;
 		}
 	}
@@ -2336,7 +2413,7 @@ static int cpufreq_boost_set_sw(int state)
 	struct cpufreq_policy *policy;
 	int ret = -EINVAL;
 
-	for_each_policy(policy) {
+	for_each_active_policy(policy) {
 		freq_table = cpufreq_frequency_get_table(policy->cpu);
 		if (freq_table) {
 			ret = cpufreq_frequency_table_cpuinfo(policy,
diff --git a/drivers/cpufreq/cpufreq_conservative.c b/drivers/cpufreq/cpufreq_conservative.c
index 25a70d06c5bf..c86a10c30912 100644
--- a/drivers/cpufreq/cpufreq_conservative.c
+++ b/drivers/cpufreq/cpufreq_conservative.c
@@ -148,6 +148,10 @@ static int dbs_cpufreq_notifier(struct notifier_block *nb, unsigned long val,
 	return 0;
 }
 
+static struct notifier_block cs_cpufreq_notifier_block = {
+	.notifier_call = dbs_cpufreq_notifier,
+};
+
 /************************** sysfs interface ************************/
 static struct common_dbs_data cs_dbs_cdata;
 
@@ -317,7 +321,7 @@ static struct attribute_group cs_attr_group_gov_pol = {
 
 /************************** sysfs end ************************/
 
-static int cs_init(struct dbs_data *dbs_data)
+static int cs_init(struct dbs_data *dbs_data, bool notify)
 {
 	struct cs_dbs_tuners *tuners;
 
@@ -336,25 +340,25 @@ static int cs_init(struct dbs_data *dbs_data)
 	dbs_data->tuners = tuners;
 	dbs_data->min_sampling_rate = MIN_SAMPLING_RATE_RATIO *
 		jiffies_to_usecs(10);
-	mutex_init(&dbs_data->mutex);
+
+	if (notify)
+		cpufreq_register_notifier(&cs_cpufreq_notifier_block,
+					  CPUFREQ_TRANSITION_NOTIFIER);
+
 	return 0;
 }
 
-static void cs_exit(struct dbs_data *dbs_data)
+static void cs_exit(struct dbs_data *dbs_data, bool notify)
 {
+	if (notify)
+		cpufreq_unregister_notifier(&cs_cpufreq_notifier_block,
+					    CPUFREQ_TRANSITION_NOTIFIER);
+
 	kfree(dbs_data->tuners);
 }
 
 define_get_cpu_dbs_routines(cs_cpu_dbs_info);
 
-static struct notifier_block cs_cpufreq_notifier_block = {
-	.notifier_call = dbs_cpufreq_notifier,
-};
-
-static struct cs_ops cs_ops = {
-	.notifier_block = &cs_cpufreq_notifier_block,
-};
-
 static struct common_dbs_data cs_dbs_cdata = {
 	.governor = GOV_CONSERVATIVE,
 	.attr_group_gov_sys = &cs_attr_group_gov_sys,
@@ -363,9 +367,9 @@ static struct common_dbs_data cs_dbs_cdata = {
 	.get_cpu_dbs_info_s = get_cpu_dbs_info_s,
 	.gov_dbs_timer = cs_dbs_timer,
 	.gov_check_cpu = cs_check_cpu,
-	.gov_ops = &cs_ops,
 	.init = cs_init,
 	.exit = cs_exit,
+	.mutex = __MUTEX_INITIALIZER(cs_dbs_cdata.mutex),
 };
 
 static int cs_cpufreq_governor_dbs(struct cpufreq_policy *policy,
diff --git a/drivers/cpufreq/cpufreq_governor.c b/drivers/cpufreq/cpufreq_governor.c
index 1b44496b2d2b..57a39f8a92b7 100644
--- a/drivers/cpufreq/cpufreq_governor.c
+++ b/drivers/cpufreq/cpufreq_governor.c
@@ -239,211 +239,242 @@ static void set_sampling_rate(struct dbs_data *dbs_data,
 	}
 }
 
-int cpufreq_governor_dbs(struct cpufreq_policy *policy,
-		struct common_dbs_data *cdata, unsigned int event)
+static int cpufreq_governor_init(struct cpufreq_policy *policy,
+				 struct dbs_data *dbs_data,
+				 struct common_dbs_data *cdata)
 {
-	struct dbs_data *dbs_data;
-	struct od_cpu_dbs_info_s *od_dbs_info = NULL;
-	struct cs_cpu_dbs_info_s *cs_dbs_info = NULL;
-	struct od_ops *od_ops = NULL;
-	struct od_dbs_tuners *od_tuners = NULL;
-	struct cs_dbs_tuners *cs_tuners = NULL;
-	struct cpu_dbs_common_info *cpu_cdbs;
-	unsigned int sampling_rate, latency, ignore_nice, j, cpu = policy->cpu;
-	int io_busy = 0;
-	int rc;
-
-	if (have_governor_per_policy())
-		dbs_data = policy->governor_data;
-	else
-		dbs_data = cdata->gdbs_data;
-
-	WARN_ON(!dbs_data && (event != CPUFREQ_GOV_POLICY_INIT));
-
-	switch (event) {
-	case CPUFREQ_GOV_POLICY_INIT:
-		if (have_governor_per_policy()) {
-			WARN_ON(dbs_data);
-		} else if (dbs_data) {
-			dbs_data->usage_count++;
-			policy->governor_data = dbs_data;
-			return 0;
-		}
-
-		dbs_data = kzalloc(sizeof(*dbs_data), GFP_KERNEL);
-		if (!dbs_data) {
-			pr_err("%s: POLICY_INIT: kzalloc failed\n", __func__);
-			return -ENOMEM;
-		}
+	unsigned int latency;
+	int ret;
 
-		dbs_data->cdata = cdata;
-		dbs_data->usage_count = 1;
-		rc = cdata->init(dbs_data);
-		if (rc) {
-			pr_err("%s: POLICY_INIT: init() failed\n", __func__);
-			kfree(dbs_data);
-			return rc;
-		}
+	if (dbs_data) {
+		if (WARN_ON(have_governor_per_policy()))
+			return -EINVAL;
+		dbs_data->usage_count++;
+		policy->governor_data = dbs_data;
+		return 0;
+	}
 
-		if (!have_governor_per_policy())
-			WARN_ON(cpufreq_get_global_kobject());
+	dbs_data = kzalloc(sizeof(*dbs_data), GFP_KERNEL);
+	if (!dbs_data)
+		return -ENOMEM;
 
-		rc = sysfs_create_group(get_governor_parent_kobj(policy),
-				get_sysfs_attr(dbs_data));
-		if (rc) {
-			cdata->exit(dbs_data);
-			kfree(dbs_data);
-			return rc;
-		}
+	dbs_data->cdata = cdata;
+	dbs_data->usage_count = 1;
 
-		policy->governor_data = dbs_data;
+	ret = cdata->init(dbs_data, !policy->governor->initialized);
+	if (ret)
+		goto free_dbs_data;
 
-		/* policy latency is in ns. Convert it to us first */
-		latency = policy->cpuinfo.transition_latency / 1000;
-		if (latency == 0)
-			latency = 1;
+	/* policy latency is in ns. Convert it to us first */
+	latency = policy->cpuinfo.transition_latency / 1000;
+	if (latency == 0)
+		latency = 1;
 
-		/* Bring kernel and HW constraints together */
-		dbs_data->min_sampling_rate = max(dbs_data->min_sampling_rate,
-				MIN_LATENCY_MULTIPLIER * latency);
-		set_sampling_rate(dbs_data, max(dbs_data->min_sampling_rate,
+	/* Bring kernel and HW constraints together */
+	dbs_data->min_sampling_rate = max(dbs_data->min_sampling_rate,
+					  MIN_LATENCY_MULTIPLIER * latency);
+	set_sampling_rate(dbs_data, max(dbs_data->min_sampling_rate,
 					latency * LATENCY_MULTIPLIER));
 
-		if ((cdata->governor == GOV_CONSERVATIVE) &&
-				(!policy->governor->initialized)) {
-			struct cs_ops *cs_ops = dbs_data->cdata->gov_ops;
-
-			cpufreq_register_notifier(cs_ops->notifier_block,
-					CPUFREQ_TRANSITION_NOTIFIER);
+	if (!have_governor_per_policy()) {
+		if (WARN_ON(cpufreq_get_global_kobject())) {
+			ret = -EINVAL;
+			goto cdata_exit;
 		}
+		cdata->gdbs_data = dbs_data;
+	}
 
-		if (!have_governor_per_policy())
-			cdata->gdbs_data = dbs_data;
+	ret = sysfs_create_group(get_governor_parent_kobj(policy),
+				 get_sysfs_attr(dbs_data));
+	if (ret)
+		goto put_kobj;
 
-		return 0;
-	case CPUFREQ_GOV_POLICY_EXIT:
-		if (!--dbs_data->usage_count) {
-			sysfs_remove_group(get_governor_parent_kobj(policy),
-					get_sysfs_attr(dbs_data));
+	policy->governor_data = dbs_data;
 
-			if (!have_governor_per_policy())
-				cpufreq_put_global_kobject();
+	return 0;
 
-			if ((dbs_data->cdata->governor == GOV_CONSERVATIVE) &&
-				(policy->governor->initialized == 1)) {
-				struct cs_ops *cs_ops = dbs_data->cdata->gov_ops;
+put_kobj:
+	if (!have_governor_per_policy()) {
+		cdata->gdbs_data = NULL;
+		cpufreq_put_global_kobject();
+	}
+cdata_exit:
+	cdata->exit(dbs_data, !policy->governor->initialized);
+free_dbs_data:
+	kfree(dbs_data);
+	return ret;
+}
+
+static void cpufreq_governor_exit(struct cpufreq_policy *policy,
+				  struct dbs_data *dbs_data)
+{
+	struct common_dbs_data *cdata = dbs_data->cdata;
 
-				cpufreq_unregister_notifier(cs_ops->notifier_block,
-						CPUFREQ_TRANSITION_NOTIFIER);
-			}
+	policy->governor_data = NULL;
+	if (!--dbs_data->usage_count) {
+		sysfs_remove_group(get_governor_parent_kobj(policy),
+				   get_sysfs_attr(dbs_data));
 
-			cdata->exit(dbs_data);
-			kfree(dbs_data);
+		if (!have_governor_per_policy()) {
 			cdata->gdbs_data = NULL;
+			cpufreq_put_global_kobject();
 		}
 
-		policy->governor_data = NULL;
-		return 0;
+		cdata->exit(dbs_data, policy->governor->initialized == 1);
+		kfree(dbs_data);
 	}
+}
 
-	cpu_cdbs = dbs_data->cdata->get_cpu_cdbs(cpu);
+static int cpufreq_governor_start(struct cpufreq_policy *policy,
+				  struct dbs_data *dbs_data)
+{
+	struct common_dbs_data *cdata = dbs_data->cdata;
+	unsigned int sampling_rate, ignore_nice, j, cpu = policy->cpu;
+	struct cpu_dbs_common_info *cpu_cdbs = cdata->get_cpu_cdbs(cpu);
+	int io_busy = 0;
+
+	if (!policy->cur)
+		return -EINVAL;
+
+	if (cdata->governor == GOV_CONSERVATIVE) {
+		struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 
-	if (dbs_data->cdata->governor == GOV_CONSERVATIVE) {
-		cs_tuners = dbs_data->tuners;
-		cs_dbs_info = dbs_data->cdata->get_cpu_dbs_info_s(cpu);
 		sampling_rate = cs_tuners->sampling_rate;
 		ignore_nice = cs_tuners->ignore_nice_load;
 	} else {
-		od_tuners = dbs_data->tuners;
-		od_dbs_info = dbs_data->cdata->get_cpu_dbs_info_s(cpu);
+		struct od_dbs_tuners *od_tuners = dbs_data->tuners;
+
 		sampling_rate = od_tuners->sampling_rate;
 		ignore_nice = od_tuners->ignore_nice_load;
-		od_ops = dbs_data->cdata->gov_ops;
 		io_busy = od_tuners->io_is_busy;
 	}
 
-	switch (event) {
-	case CPUFREQ_GOV_START:
-		if (!policy->cur)
-			return -EINVAL;
+	for_each_cpu(j, policy->cpus) {
+		struct cpu_dbs_common_info *j_cdbs = cdata->get_cpu_cdbs(j);
+		unsigned int prev_load;
 
-		mutex_lock(&dbs_data->mutex);
+		j_cdbs->cpu = j;
+		j_cdbs->cur_policy = policy;
+		j_cdbs->prev_cpu_idle =
+			get_cpu_idle_time(j, &j_cdbs->prev_cpu_wall, io_busy);
 
-		for_each_cpu(j, policy->cpus) {
-			struct cpu_dbs_common_info *j_cdbs =
-				dbs_data->cdata->get_cpu_cdbs(j);
-			unsigned int prev_load;
+		prev_load = (unsigned int)(j_cdbs->prev_cpu_wall -
+					    j_cdbs->prev_cpu_idle);
+		j_cdbs->prev_load = 100 * prev_load /
+				    (unsigned int)j_cdbs->prev_cpu_wall;
 
-			j_cdbs->cpu = j;
-			j_cdbs->cur_policy = policy;
-			j_cdbs->prev_cpu_idle = get_cpu_idle_time(j,
-					       &j_cdbs->prev_cpu_wall, io_busy);
+		if (ignore_nice)
+			j_cdbs->prev_cpu_nice = kcpustat_cpu(j).cpustat[CPUTIME_NICE];
 
-			prev_load = (unsigned int)
-				(j_cdbs->prev_cpu_wall - j_cdbs->prev_cpu_idle);
-			j_cdbs->prev_load = 100 * prev_load /
-					(unsigned int) j_cdbs->prev_cpu_wall;
+		mutex_init(&j_cdbs->timer_mutex);
+		INIT_DEFERRABLE_WORK(&j_cdbs->work, cdata->gov_dbs_timer);
+	}
 
-			if (ignore_nice)
-				j_cdbs->prev_cpu_nice =
-					kcpustat_cpu(j).cpustat[CPUTIME_NICE];
+	if (cdata->governor == GOV_CONSERVATIVE) {
+		struct cs_cpu_dbs_info_s *cs_dbs_info =
+			cdata->get_cpu_dbs_info_s(cpu);
 
-			mutex_init(&j_cdbs->timer_mutex);
-			INIT_DEFERRABLE_WORK(&j_cdbs->work,
-					     dbs_data->cdata->gov_dbs_timer);
-		}
+		cs_dbs_info->down_skip = 0;
+		cs_dbs_info->enable = 1;
+		cs_dbs_info->requested_freq = policy->cur;
+	} else {
+		struct od_ops *od_ops = cdata->gov_ops;
+		struct od_cpu_dbs_info_s *od_dbs_info = cdata->get_cpu_dbs_info_s(cpu);
 
-		if (dbs_data->cdata->governor == GOV_CONSERVATIVE) {
-			cs_dbs_info->down_skip = 0;
-			cs_dbs_info->enable = 1;
-			cs_dbs_info->requested_freq = policy->cur;
-		} else {
-			od_dbs_info->rate_mult = 1;
-			od_dbs_info->sample_type = OD_NORMAL_SAMPLE;
-			od_ops->powersave_bias_init_cpu(cpu);
-		}
+		od_dbs_info->rate_mult = 1;
+		od_dbs_info->sample_type = OD_NORMAL_SAMPLE;
+		od_ops->powersave_bias_init_cpu(cpu);
+	}
 
-		mutex_unlock(&dbs_data->mutex);
+	/* Initiate timer time stamp */
+	cpu_cdbs->time_stamp = ktime_get();
 
-		/* Initiate timer time stamp */
-		cpu_cdbs->time_stamp = ktime_get();
+	gov_queue_work(dbs_data, policy, delay_for_sampling_rate(sampling_rate),
+		       true);
+	return 0;
+}
 
-		gov_queue_work(dbs_data, policy,
-				delay_for_sampling_rate(sampling_rate), true);
-		break;
+static void cpufreq_governor_stop(struct cpufreq_policy *policy,
+				  struct dbs_data *dbs_data)
+{
+	struct common_dbs_data *cdata = dbs_data->cdata;
+	unsigned int cpu = policy->cpu;
+	struct cpu_dbs_common_info *cpu_cdbs = cdata->get_cpu_cdbs(cpu);
 
-	case CPUFREQ_GOV_STOP:
-		if (dbs_data->cdata->governor == GOV_CONSERVATIVE)
-			cs_dbs_info->enable = 0;
+	if (cdata->governor == GOV_CONSERVATIVE) {
+		struct cs_cpu_dbs_info_s *cs_dbs_info =
+			cdata->get_cpu_dbs_info_s(cpu);
 
-		gov_cancel_work(dbs_data, policy);
+		cs_dbs_info->enable = 0;
+	}
 
-		mutex_lock(&dbs_data->mutex);
-		mutex_destroy(&cpu_cdbs->timer_mutex);
-		cpu_cdbs->cur_policy = NULL;
+	gov_cancel_work(dbs_data, policy);
 
-		mutex_unlock(&dbs_data->mutex);
+	mutex_destroy(&cpu_cdbs->timer_mutex);
+	cpu_cdbs->cur_policy = NULL;
+}
 
-		break;
+static void cpufreq_governor_limits(struct cpufreq_policy *policy,
+				    struct dbs_data *dbs_data)
+{
+	struct common_dbs_data *cdata = dbs_data->cdata;
+	unsigned int cpu = policy->cpu;
+	struct cpu_dbs_common_info *cpu_cdbs = cdata->get_cpu_cdbs(cpu);
+
+	if (!cpu_cdbs->cur_policy)
+		return;
+
+	mutex_lock(&cpu_cdbs->timer_mutex);
+	if (policy->max < cpu_cdbs->cur_policy->cur)
+		__cpufreq_driver_target(cpu_cdbs->cur_policy, policy->max,
+					CPUFREQ_RELATION_H);
+	else if (policy->min > cpu_cdbs->cur_policy->cur)
+		__cpufreq_driver_target(cpu_cdbs->cur_policy, policy->min,
+					CPUFREQ_RELATION_L);
+	dbs_check_cpu(dbs_data, cpu);
+	mutex_unlock(&cpu_cdbs->timer_mutex);
+}
+
+int cpufreq_governor_dbs(struct cpufreq_policy *policy,
+			 struct common_dbs_data *cdata, unsigned int event)
+{
+	struct dbs_data *dbs_data;
+	int ret = 0;
 
+	/* Lock governor to block concurrent initialization of governor */
+	mutex_lock(&cdata->mutex);
+
+	if (have_governor_per_policy())
+		dbs_data = policy->governor_data;
+	else
+		dbs_data = cdata->gdbs_data;
+
+	if (WARN_ON(!dbs_data && (event != CPUFREQ_GOV_POLICY_INIT))) {
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	switch (event) {
+	case CPUFREQ_GOV_POLICY_INIT:
+		ret = cpufreq_governor_init(policy, dbs_data, cdata);
+		break;
+	case CPUFREQ_GOV_POLICY_EXIT:
+		cpufreq_governor_exit(policy, dbs_data);
+		break;
+	case CPUFREQ_GOV_START:
+		ret = cpufreq_governor_start(policy, dbs_data);
+		break;
+	case CPUFREQ_GOV_STOP:
+		cpufreq_governor_stop(policy, dbs_data);
+		break;
 	case CPUFREQ_GOV_LIMITS:
-		mutex_lock(&dbs_data->mutex);
-		if (!cpu_cdbs->cur_policy) {
-			mutex_unlock(&dbs_data->mutex);
-			break;
-		}
-		mutex_lock(&cpu_cdbs->timer_mutex);
-		if (policy->max < cpu_cdbs->cur_policy->cur)
-			__cpufreq_driver_target(cpu_cdbs->cur_policy,
-					policy->max, CPUFREQ_RELATION_H);
-		else if (policy->min > cpu_cdbs->cur_policy->cur)
-			__cpufreq_driver_target(cpu_cdbs->cur_policy,
-					policy->min, CPUFREQ_RELATION_L);
-		dbs_check_cpu(dbs_data, cpu);
-		mutex_unlock(&cpu_cdbs->timer_mutex);
-		mutex_unlock(&dbs_data->mutex);
+		cpufreq_governor_limits(policy, dbs_data);
 		break;
 	}
-	return 0;
+
+unlock:
+	mutex_unlock(&cdata->mutex);
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(cpufreq_governor_dbs);
diff --git a/drivers/cpufreq/cpufreq_governor.h b/drivers/cpufreq/cpufreq_governor.h
index cc401d147e72..34736f5e869d 100644
--- a/drivers/cpufreq/cpufreq_governor.h
+++ b/drivers/cpufreq/cpufreq_governor.h
@@ -208,11 +208,16 @@ struct common_dbs_data {
 	void *(*get_cpu_dbs_info_s)(int cpu);
 	void (*gov_dbs_timer)(struct work_struct *work);
 	void (*gov_check_cpu)(int cpu, unsigned int load);
-	int (*init)(struct dbs_data *dbs_data);
-	void (*exit)(struct dbs_data *dbs_data);
+	int (*init)(struct dbs_data *dbs_data, bool notify);
+	void (*exit)(struct dbs_data *dbs_data, bool notify);
 
 	/* Governor specific ops, see below */
 	void *gov_ops;
+
+	/*
+	 * Protects governor's data (struct dbs_data and struct common_dbs_data)
+	 */
+	struct mutex mutex;
 };
 
 /* Governor Per policy data */
@@ -221,9 +226,6 @@ struct dbs_data {
 	unsigned int min_sampling_rate;
 	int usage_count;
 	void *tuners;
-
-	/* dbs_mutex protects dbs_enable in governor start/stop */
-	struct mutex mutex;
 };
 
 /* Governor specific ops, will be passed to dbs_data->gov_ops */
@@ -234,10 +236,6 @@ struct od_ops {
 	void (*freq_increase)(struct cpufreq_policy *policy, unsigned int freq);
 };
 
-struct cs_ops {
-	struct notifier_block *notifier_block;
-};
-
 static inline int delay_for_sampling_rate(unsigned int sampling_rate)
 {
 	int delay = usecs_to_jiffies(sampling_rate);
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index ad3f38fd3eb9..3c1e10f2304c 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -475,7 +475,7 @@ static struct attribute_group od_attr_group_gov_pol = {
 
 /************************** sysfs end ************************/
 
-static int od_init(struct dbs_data *dbs_data)
+static int od_init(struct dbs_data *dbs_data, bool notify)
 {
 	struct od_dbs_tuners *tuners;
 	u64 idle_time;
@@ -513,11 +513,10 @@ static int od_init(struct dbs_data *dbs_data)
 	tuners->io_is_busy = should_io_be_busy();
 
 	dbs_data->tuners = tuners;
-	mutex_init(&dbs_data->mutex);
 	return 0;
 }
 
-static void od_exit(struct dbs_data *dbs_data)
+static void od_exit(struct dbs_data *dbs_data, bool notify)
 {
 	kfree(dbs_data->tuners);
 }
@@ -541,6 +540,7 @@ static struct common_dbs_data od_dbs_cdata = {
 	.gov_ops = &od_ops,
 	.init = od_init,
 	.exit = od_exit,
+	.mutex = __MUTEX_INITIALIZER(od_dbs_cdata.mutex),
 };
 
 static void od_set_powersave_bias(unsigned int powersave_bias)
diff --git a/drivers/cpufreq/exynos-cpufreq.c b/drivers/cpufreq/exynos-cpufreq.c
index 82d2fbb20f7e..ae5b2bd3a978 100644
--- a/drivers/cpufreq/exynos-cpufreq.c
+++ b/drivers/cpufreq/exynos-cpufreq.c
@@ -10,6 +10,7 @@
 */
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/err.h>
 #include <linux/clk.h>
 #include <linux/io.h>
@@ -168,10 +169,7 @@ static int exynos_cpufreq_probe(struct platform_device *pdev)
 
 	exynos_info->dev = &pdev->dev;
 
-	if (of_machine_is_compatible("samsung,exynos4210")) {
-		exynos_info->type = EXYNOS_SOC_4210;
-		ret = exynos4210_cpufreq_init(exynos_info);
-	} else if (of_machine_is_compatible("samsung,exynos4212")) {
+	if (of_machine_is_compatible("samsung,exynos4212")) {
 		exynos_info->type = EXYNOS_SOC_4212;
 		ret = exynos4x12_cpufreq_init(exynos_info);
 	} else if (of_machine_is_compatible("samsung,exynos4412")) {
diff --git a/drivers/cpufreq/exynos-cpufreq.h b/drivers/cpufreq/exynos-cpufreq.h
index 9f2062a7cc02..a3855e4d913d 100644
--- a/drivers/cpufreq/exynos-cpufreq.h
+++ b/drivers/cpufreq/exynos-cpufreq.h
@@ -18,7 +18,6 @@ enum cpufreq_level_index {
 };
 
 enum exynos_soc_type {
-	EXYNOS_SOC_4210,
 	EXYNOS_SOC_4212,
 	EXYNOS_SOC_4412,
 	EXYNOS_SOC_5250,
@@ -53,14 +52,6 @@ struct exynos_dvfs_info {
 	void __iomem	*cmu_regs;
 };
 
-#ifdef CONFIG_ARM_EXYNOS4210_CPUFREQ
-extern int exynos4210_cpufreq_init(struct exynos_dvfs_info *);
-#else
-static inline int exynos4210_cpufreq_init(struct exynos_dvfs_info *info)
-{
-	return -EOPNOTSUPP;
-}
-#endif
 #ifdef CONFIG_ARM_EXYNOS4X12_CPUFREQ
 extern int exynos4x12_cpufreq_init(struct exynos_dvfs_info *);
 #else
diff --git a/drivers/cpufreq/exynos4210-cpufreq.c b/drivers/cpufreq/exynos4210-cpufreq.c
deleted file mode 100644
index 843ec824fd91..000000000000
--- a/drivers/cpufreq/exynos4210-cpufreq.c
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
- * Copyright (c) 2010-2011 Samsung Electronics Co., Ltd.
- *		http://www.samsung.com
- *
- * EXYNOS4210 - CPU frequency scaling support
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
-*/
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/err.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-#include <linux/slab.h>
-#include <linux/cpufreq.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
-
-#include "exynos-cpufreq.h"
-
-static struct clk *cpu_clk;
-static struct clk *moutcore;
-static struct clk *mout_mpll;
-static struct clk *mout_apll;
-static struct exynos_dvfs_info *cpufreq;
-
-static unsigned int exynos4210_volt_table[] = {
-	1250000, 1150000, 1050000, 975000, 950000,
-};
-
-static struct cpufreq_frequency_table exynos4210_freq_table[] = {
-	{0, L0, 1200 * 1000},
-	{0, L1, 1000 * 1000},
-	{0, L2,  800 * 1000},
-	{0, L3,  500 * 1000},
-	{0, L4,  200 * 1000},
-	{0, 0, CPUFREQ_TABLE_END},
-};
-
-static struct apll_freq apll_freq_4210[] = {
-	/*
-	 * values:
-	 * freq
-	 * clock divider for CORE, COREM0, COREM1, PERIPH, ATB, PCLK_DBG, APLL, RESERVED
-	 * clock divider for COPY, HPM, RESERVED
-	 * PLL M, P, S
-	 */
-	APLL_FREQ(1200, 0, 3, 7, 3, 4, 1, 7, 0, 5, 0, 0, 150, 3, 1),
-	APLL_FREQ(1000, 0, 3, 7, 3, 4, 1, 7, 0, 4, 0, 0, 250, 6, 1),
-	APLL_FREQ(800,  0, 3, 7, 3, 3, 1, 7, 0, 3, 0, 0, 200, 6, 1),
-	APLL_FREQ(500,  0, 3, 7, 3, 3, 1, 7, 0, 3, 0, 0, 250, 6, 2),
-	APLL_FREQ(200,  0, 1, 3, 1, 3, 1, 0, 0, 3, 0, 0, 200, 6, 3),
-};
-
-static void exynos4210_set_clkdiv(unsigned int div_index)
-{
-	unsigned int tmp;
-
-	/* Change Divider - CPU0 */
-
-	tmp = apll_freq_4210[div_index].clk_div_cpu0;
-
-	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU);
-
-	do {
-		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU);
-	} while (tmp & 0x1111111);
-
-	/* Change Divider - CPU1 */
-
-	tmp = apll_freq_4210[div_index].clk_div_cpu1;
-
-	__raw_writel(tmp, cpufreq->cmu_regs + EXYNOS4_CLKDIV_CPU1);
-
-	do {
-		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKDIV_STATCPU1);
-	} while (tmp & 0x11);
-}
-
-static void exynos4210_set_apll(unsigned int index)
-{
-	unsigned int tmp, freq = apll_freq_4210[index].freq;
-
-	/* MUX_CORE_SEL = MPLL, ARMCLK uses MPLL for lock time */
-	clk_set_parent(moutcore, mout_mpll);
-
-	do {
-		tmp = (__raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU)
-			>> EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT);
-		tmp &= 0x7;
-	} while (tmp != 0x2);
-
-	clk_set_rate(mout_apll, freq * 1000);
-
-	/* MUX_CORE_SEL = APLL */
-	clk_set_parent(moutcore, mout_apll);
-
-	do {
-		tmp = __raw_readl(cpufreq->cmu_regs + EXYNOS4_CLKMUX_STATCPU);
-		tmp &= EXYNOS4_CLKMUX_STATCPU_MUXCORE_MASK;
-	} while (tmp != (0x1 << EXYNOS4_CLKSRC_CPU_MUXCORE_SHIFT));
-}
-
-static void exynos4210_set_frequency(unsigned int old_index,
-				     unsigned int new_index)
-{
-	if (old_index > new_index) {
-		exynos4210_set_clkdiv(new_index);
-		exynos4210_set_apll(new_index);
-	} else if (old_index < new_index) {
-		exynos4210_set_apll(new_index);
-		exynos4210_set_clkdiv(new_index);
-	}
-}
-
-int exynos4210_cpufreq_init(struct exynos_dvfs_info *info)
-{
-	struct device_node *np;
-	unsigned long rate;
-
-	/*
-	 * HACK: This is a temporary workaround to get access to clock
-	 * controller registers directly and remove static mappings and
-	 * dependencies on platform headers. It is necessary to enable
-	 * Exynos multi-platform support and will be removed together with
-	 * this whole driver as soon as Exynos gets migrated to use
-	 * cpufreq-dt driver.
-	 */
-	np = of_find_compatible_node(NULL, NULL, "samsung,exynos4210-clock");
-	if (!np) {
-		pr_err("%s: failed to find clock controller DT node\n",
-			__func__);
-		return -ENODEV;
-	}
-
-	info->cmu_regs = of_iomap(np, 0);
-	if (!info->cmu_regs) {
-		pr_err("%s: failed to map CMU registers\n", __func__);
-		return -EFAULT;
-	}
-
-	cpu_clk = clk_get(NULL, "armclk");
-	if (IS_ERR(cpu_clk))
-		return PTR_ERR(cpu_clk);
-
-	moutcore = clk_get(NULL, "moutcore");
-	if (IS_ERR(moutcore))
-		goto err_moutcore;
-
-	mout_mpll = clk_get(NULL, "mout_mpll");
-	if (IS_ERR(mout_mpll))
-		goto err_mout_mpll;
-
-	rate = clk_get_rate(mout_mpll) / 1000;
-
-	mout_apll = clk_get(NULL, "mout_apll");
-	if (IS_ERR(mout_apll))
-		goto err_mout_apll;
-
-	info->mpll_freq_khz = rate;
-	/* 800Mhz */
-	info->pll_safe_idx = L2;
-	info->cpu_clk = cpu_clk;
-	info->volt_table = exynos4210_volt_table;
-	info->freq_table = exynos4210_freq_table;
-	info->set_freq = exynos4210_set_frequency;
-
-	cpufreq = info;
-
-	return 0;
-
-err_mout_apll:
-	clk_put(mout_mpll);
-err_mout_mpll:
-	clk_put(moutcore);
-err_moutcore:
-	clk_put(cpu_clk);
-
-	pr_debug("%s: failed initialization\n", __func__);
-	return -EINVAL;
-}
diff --git a/drivers/cpufreq/gx-suspmod.c b/drivers/cpufreq/gx-suspmod.c
index 1d723dc8880c..3488c9c175eb 100644
--- a/drivers/cpufreq/gx-suspmod.c
+++ b/drivers/cpufreq/gx-suspmod.c
@@ -144,7 +144,7 @@ module_param(max_duration, int, 0444);
 
 
 /**
- * we can detect a core multipiler from dir0_lsb
+ * we can detect a core multiplier from dir0_lsb
  * from GX1 datasheet p.56,
  *	MULT[3:0]:
  *	0000 = SYSCLK multiplied by 4 (test only)
@@ -346,7 +346,7 @@ static int cpufreq_gx_verify(struct cpufreq_policy *policy)
 
 	/* it needs to be assured that at least one supported frequency is
 	 * within policy->min and policy->max. If it is not, policy->max
-	 * needs to be increased until one freuqency is supported.
+	 * needs to be increased until one frequency is supported.
 	 * policy->min may not be decreased, though. This way we guarantee a
 	 * specific processing capacity.
 	 */
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index 6414661ac1c4..15ada47bb720 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -26,6 +26,7 @@
 #include <linux/fs.h>
 #include <linux/debugfs.h>
 #include <linux/acpi.h>
+#include <linux/vmalloc.h>
 #include <trace/events/power.h>
 
 #include <asm/div64.h>
@@ -48,9 +49,9 @@ static inline int32_t mul_fp(int32_t x, int32_t y)
 	return ((int64_t)x * (int64_t)y) >> FRAC_BITS;
 }
 
-static inline int32_t div_fp(int32_t x, int32_t y)
+static inline int32_t div_fp(s64 x, s64 y)
 {
-	return div_s64((int64_t)x << FRAC_BITS, y);
+	return div64_s64((int64_t)x << FRAC_BITS, y);
 }
 
 static inline int ceiling_fp(int32_t x)
@@ -68,6 +69,7 @@ struct sample {
 	int32_t core_pct_busy;
 	u64 aperf;
 	u64 mperf;
+	u64 tsc;
 	int freq;
 	ktime_t time;
 };
@@ -109,6 +111,7 @@ struct cpudata {
 	ktime_t last_sample_time;
 	u64	prev_aperf;
 	u64	prev_mperf;
+	u64	prev_tsc;
 	struct sample sample;
 };
 
@@ -396,7 +399,7 @@ static ssize_t store_no_turbo(struct kobject *a, struct attribute *b,
 
 	update_turbo_state();
 	if (limits.turbo_disabled) {
-		pr_warn("Turbo disabled by BIOS or unavailable on processor\n");
+		pr_warn("intel_pstate: Turbo disabled by BIOS or unavailable on processor\n");
 		return -EPERM;
 	}
 
@@ -484,7 +487,7 @@ static void __init intel_pstate_sysfs_expose_params(void)
 static void intel_pstate_hwp_enable(void)
 {
 	hwp_active++;
-	pr_info("intel_pstate HWP enabled\n");
+	pr_info("intel_pstate: HWP enabled\n");
 
 	wrmsrl( MSR_PM_ENABLE, 0x1);
 }
@@ -535,7 +538,7 @@ static void byt_set_pstate(struct cpudata *cpudata, int pstate)
 
 	val |= vid;
 
-	wrmsrl(MSR_IA32_PERF_CTL, val);
+	wrmsrl_on_cpu(cpudata->cpu, MSR_IA32_PERF_CTL, val);
 }
 
 #define BYT_BCLK_FREQS 5
@@ -704,19 +707,20 @@ static void intel_pstate_get_min_max(struct cpudata *cpu, int *min, int *max)
 	*min = clamp_t(int, min_perf, cpu->pstate.min_pstate, max_perf);
 }
 
-static void intel_pstate_set_pstate(struct cpudata *cpu, int pstate)
+static void intel_pstate_set_pstate(struct cpudata *cpu, int pstate, bool force)
 {
 	int max_perf, min_perf;
 
-	update_turbo_state();
-
-	intel_pstate_get_min_max(cpu, &min_perf, &max_perf);
+	if (force) {
+		update_turbo_state();
 
-	pstate = clamp_t(int, pstate, min_perf, max_perf);
+		intel_pstate_get_min_max(cpu, &min_perf, &max_perf);
 
-	if (pstate == cpu->pstate.current_pstate)
-		return;
+		pstate = clamp_t(int, pstate, min_perf, max_perf);
 
+		if (pstate == cpu->pstate.current_pstate)
+			return;
+	}
 	trace_cpu_frequency(pstate * cpu->pstate.scaling, cpu->cpu);
 
 	cpu->pstate.current_pstate = pstate;
@@ -733,7 +737,7 @@ static void intel_pstate_get_cpu_pstates(struct cpudata *cpu)
 
 	if (pstate_funcs.get_vid)
 		pstate_funcs.get_vid(cpu);
-	intel_pstate_set_pstate(cpu, cpu->pstate.min_pstate);
+	intel_pstate_set_pstate(cpu, cpu->pstate.min_pstate, false);
 }
 
 static inline void intel_pstate_calc_busy(struct cpudata *cpu)
@@ -756,23 +760,28 @@ static inline void intel_pstate_sample(struct cpudata *cpu)
 {
 	u64 aperf, mperf;
 	unsigned long flags;
+	u64 tsc;
 
 	local_irq_save(flags);
 	rdmsrl(MSR_IA32_APERF, aperf);
 	rdmsrl(MSR_IA32_MPERF, mperf);
+	tsc = native_read_tsc();
 	local_irq_restore(flags);
 
 	cpu->last_sample_time = cpu->sample.time;
 	cpu->sample.time = ktime_get();
 	cpu->sample.aperf = aperf;
 	cpu->sample.mperf = mperf;
+	cpu->sample.tsc =  tsc;
 	cpu->sample.aperf -= cpu->prev_aperf;
 	cpu->sample.mperf -= cpu->prev_mperf;
+	cpu->sample.tsc -= cpu->prev_tsc;
 
 	intel_pstate_calc_busy(cpu);
 
 	cpu->prev_aperf = aperf;
 	cpu->prev_mperf = mperf;
+	cpu->prev_tsc = tsc;
 }
 
 static inline void intel_hwp_set_sample_time(struct cpudata *cpu)
@@ -794,7 +803,7 @@ static inline void intel_pstate_set_sample_time(struct cpudata *cpu)
 static inline int32_t intel_pstate_get_scaled_busy(struct cpudata *cpu)
 {
 	int32_t core_busy, max_pstate, current_pstate, sample_ratio;
-	u32 duration_us;
+	s64 duration_us;
 	u32 sample_time;
 
 	/*
@@ -821,8 +830,8 @@ static inline int32_t intel_pstate_get_scaled_busy(struct cpudata *cpu)
 	 * 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);
+	duration_us = ktime_us_delta(cpu->sample.time,
+				     cpu->last_sample_time);
 	if (duration_us > sample_time * 3) {
 		sample_ratio = div_fp(int_tofp(sample_time),
 				      int_tofp(duration_us));
@@ -837,6 +846,10 @@ static inline void intel_pstate_adjust_busy_pstate(struct cpudata *cpu)
 	int32_t busy_scaled;
 	struct _pid *pid;
 	signed int ctl;
+	int from;
+	struct sample *sample;
+
+	from = cpu->pstate.current_pstate;
 
 	pid = &cpu->pid;
 	busy_scaled = intel_pstate_get_scaled_busy(cpu);
@@ -844,7 +857,17 @@ static inline void intel_pstate_adjust_busy_pstate(struct cpudata *cpu)
 	ctl = pid_calc(pid, busy_scaled);
 
 	/* Negative values of ctl increase the pstate and vice versa */
-	intel_pstate_set_pstate(cpu, cpu->pstate.current_pstate - ctl);
+	intel_pstate_set_pstate(cpu, cpu->pstate.current_pstate - ctl, true);
+
+	sample = &cpu->sample;
+	trace_pstate_sample(fp_toint(sample->core_pct_busy),
+		fp_toint(busy_scaled),
+		from,
+		cpu->pstate.current_pstate,
+		sample->mperf,
+		sample->aperf,
+		sample->tsc,
+		sample->freq);
 }
 
 static void intel_hwp_timer_func(unsigned long __data)
@@ -858,21 +881,11 @@ static void intel_hwp_timer_func(unsigned long __data)
 static void intel_pstate_timer_func(unsigned long __data)
 {
 	struct cpudata *cpu = (struct cpudata *) __data;
-	struct sample *sample;
 
 	intel_pstate_sample(cpu);
 
-	sample = &cpu->sample;
-
 	intel_pstate_adjust_busy_pstate(cpu);
 
-	trace_pstate_sample(fp_toint(sample->core_pct_busy),
-			fp_toint(intel_pstate_get_scaled_busy(cpu)),
-			cpu->pstate.current_pstate,
-			sample->mperf,
-			sample->aperf,
-			sample->freq);
-
 	intel_pstate_set_sample_time(cpu);
 }
 
@@ -935,7 +948,7 @@ static int intel_pstate_init_cpu(unsigned int cpunum)
 
 	add_timer_on(&cpu->timer, cpunum);
 
-	pr_debug("Intel pstate controlling: cpu %d\n", cpunum);
+	pr_debug("intel_pstate: controlling: cpu %d\n", cpunum);
 
 	return 0;
 }
@@ -1001,13 +1014,13 @@ static void intel_pstate_stop_cpu(struct cpufreq_policy *policy)
 	int cpu_num = policy->cpu;
 	struct cpudata *cpu = all_cpu_data[cpu_num];
 
-	pr_info("intel_pstate CPU %d exiting\n", cpu_num);
+	pr_debug("intel_pstate: CPU %d exiting\n", cpu_num);
 
 	del_timer_sync(&all_cpu_data[cpu_num]->timer);
 	if (hwp_active)
 		return;
 
-	intel_pstate_set_pstate(cpu, cpu->pstate.min_pstate);
+	intel_pstate_set_pstate(cpu, cpu->pstate.min_pstate, false);
 }
 
 static int intel_pstate_cpu_init(struct cpufreq_policy *policy)
diff --git a/drivers/cpufreq/loongson2_cpufreq.c b/drivers/cpufreq/loongson2_cpufreq.c
index fc897babab55..e362860c2b50 100644
--- a/drivers/cpufreq/loongson2_cpufreq.c
+++ b/drivers/cpufreq/loongson2_cpufreq.c
@@ -3,7 +3,7 @@
  *
  * The 2E revision of loongson processor not support this feature.
  *
- * Copyright (C) 2006 - 2008 Lemote Inc. & Insititute of Computing Technology
+ * Copyright (C) 2006 - 2008 Lemote Inc. & Institute of Computing Technology
  * Author: Yanhua, yanh@lemote.com
  *
  * This file is subject to the terms and conditions of the GNU General Public
diff --git a/drivers/cpufreq/ls1x-cpufreq.c b/drivers/cpufreq/ls1x-cpufreq.c
index f0913eee2f50..262581b3318d 100644
--- a/drivers/cpufreq/ls1x-cpufreq.c
+++ b/drivers/cpufreq/ls1x-cpufreq.c
@@ -17,8 +17,8 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 
-#include <asm/mach-loongson1/cpufreq.h>
-#include <asm/mach-loongson1/loongson1.h>
+#include <cpufreq.h>
+#include <loongson1.h>
 
 static struct {
 	struct device *dev;
diff --git a/drivers/cpufreq/p4-clockmod.c b/drivers/cpufreq/p4-clockmod.c
index 529cfd92158f..5dd95dab580d 100644
--- a/drivers/cpufreq/p4-clockmod.c
+++ b/drivers/cpufreq/p4-clockmod.c
@@ -172,7 +172,7 @@ static int cpufreq_p4_cpu_init(struct cpufreq_policy *policy)
 	unsigned int i;
 
 #ifdef CONFIG_SMP
-	cpumask_copy(policy->cpus, cpu_sibling_mask(policy->cpu));
+	cpumask_copy(policy->cpus, topology_sibling_cpumask(policy->cpu));
 #endif
 
 	/* Errata workaround */
diff --git a/drivers/cpufreq/powernow-k8.c b/drivers/cpufreq/powernow-k8.c
index f9ce7e4bf0fe..5c035d04d827 100644
--- a/drivers/cpufreq/powernow-k8.c
+++ b/drivers/cpufreq/powernow-k8.c
@@ -57,13 +57,6 @@ static DEFINE_PER_CPU(struct powernow_k8_data *, powernow_data);
 
 static struct cpufreq_driver cpufreq_amd64_driver;
 
-#ifndef CONFIG_SMP
-static inline const struct cpumask *cpu_core_mask(int cpu)
-{
-	return cpumask_of(0);
-}
-#endif
-
 /* Return a frequency in MHz, given an input fid */
 static u32 find_freq_from_fid(u32 fid)
 {
@@ -620,7 +613,7 @@ static int fill_powernow_table(struct powernow_k8_data *data,
 
 	pr_debug("cfid 0x%x, cvid 0x%x\n", data->currfid, data->currvid);
 	data->powernow_table = powernow_table;
-	if (cpumask_first(cpu_core_mask(data->cpu)) == data->cpu)
+	if (cpumask_first(topology_core_cpumask(data->cpu)) == data->cpu)
 		print_basics(data);
 
 	for (j = 0; j < data->numps; j++)
@@ -784,7 +777,7 @@ static int powernow_k8_cpu_init_acpi(struct powernow_k8_data *data)
 		CPUFREQ_TABLE_END;
 	data->powernow_table = powernow_table;
 
-	if (cpumask_first(cpu_core_mask(data->cpu)) == data->cpu)
+	if (cpumask_first(topology_core_cpumask(data->cpu)) == data->cpu)
 		print_basics(data);
 
 	/* notify BIOS that we exist */
@@ -1090,7 +1083,7 @@ static int powernowk8_cpu_init(struct cpufreq_policy *pol)
 	if (rc != 0)
 		goto err_out_exit_acpi;
 
-	cpumask_copy(pol->cpus, cpu_core_mask(pol->cpu));
+	cpumask_copy(pol->cpus, topology_core_cpumask(pol->cpu));
 	data->available_cores = pol->cpus;
 
 	/* min/max the cpu is capable of */
diff --git a/drivers/cpufreq/pxa2xx-cpufreq.c b/drivers/cpufreq/pxa2xx-cpufreq.c
index e24269ab4e9b..1d99c97defa9 100644
--- a/drivers/cpufreq/pxa2xx-cpufreq.c
+++ b/drivers/cpufreq/pxa2xx-cpufreq.c
@@ -56,7 +56,7 @@ module_param(pxa27x_maxfreq, uint, 0);
 MODULE_PARM_DESC(pxa27x_maxfreq, "Set the pxa27x maxfreq in MHz"
 		 "(typically 624=>pxa270, 416=>pxa271, 520=>pxa272)");
 
-typedef struct {
+struct pxa_freqs {
 	unsigned int khz;
 	unsigned int membus;
 	unsigned int cccr;
@@ -64,7 +64,7 @@ typedef struct {
 	unsigned int cclkcfg;
 	int vmin;
 	int vmax;
-} pxa_freqs_t;
+};
 
 /* Define the refresh period in mSec for the SDRAM and the number of rows */
 #define SDRAM_TREF	64	/* standard 64ms SDRAM */
@@ -86,7 +86,7 @@ static unsigned int sdram_rows;
 /* Use the run mode frequencies for the CPUFREQ_POLICY_PERFORMANCE policy */
 #define CCLKCFG			CCLKCFG_TURBO | CCLKCFG_FCS
 
-static pxa_freqs_t pxa255_run_freqs[] =
+static const struct pxa_freqs pxa255_run_freqs[] =
 {
 	/* CPU   MEMBUS  CCCR  DIV2 CCLKCFG	           run  turbo PXbus SDRAM */
 	{ 99500,  99500, 0x121, 1,  CCLKCFG, -1, -1},	/*  99,   99,   50,   50  */
@@ -98,7 +98,7 @@ static pxa_freqs_t pxa255_run_freqs[] =
 };
 
 /* Use the turbo mode frequencies for the CPUFREQ_POLICY_POWERSAVE policy */
-static pxa_freqs_t pxa255_turbo_freqs[] =
+static const struct pxa_freqs pxa255_turbo_freqs[] =
 {
 	/* CPU   MEMBUS  CCCR  DIV2 CCLKCFG	   run  turbo PXbus SDRAM */
 	{ 99500, 99500,  0x121, 1,  CCLKCFG, -1, -1},	/*  99,   99,   50,   50  */
@@ -153,7 +153,7 @@ MODULE_PARM_DESC(pxa255_turbo_table, "Selects the frequency table (0 = run table
    ((HT) ? CCLKCFG_HALFTURBO : 0) | \
    ((T)  ? CCLKCFG_TURBO : 0))
 
-static pxa_freqs_t pxa27x_freqs[] = {
+static struct pxa_freqs pxa27x_freqs[] = {
 	{104000, 104000, PXA27x_CCCR(1,	 8, 2), 0, CCLKCFG2(1, 0, 1),  900000, 1705000 },
 	{156000, 104000, PXA27x_CCCR(1,	 8, 3), 0, CCLKCFG2(1, 0, 1), 1000000, 1705000 },
 	{208000, 208000, PXA27x_CCCR(0, 16, 2), 1, CCLKCFG2(0, 0, 1), 1180000, 1705000 },
@@ -171,7 +171,7 @@ extern unsigned get_clk_frequency_khz(int info);
 
 #ifdef CONFIG_REGULATOR
 
-static int pxa_cpufreq_change_voltage(pxa_freqs_t *pxa_freq)
+static int pxa_cpufreq_change_voltage(const struct pxa_freqs *pxa_freq)
 {
 	int ret = 0;
 	int vmin, vmax;
@@ -202,7 +202,7 @@ static void __init pxa_cpufreq_init_voltages(void)
 	}
 }
 #else
-static int pxa_cpufreq_change_voltage(pxa_freqs_t *pxa_freq)
+static int pxa_cpufreq_change_voltage(struct pxa_freqs *pxa_freq)
 {
 	return 0;
 }
@@ -211,7 +211,7 @@ static void __init pxa_cpufreq_init_voltages(void) { }
 #endif
 
 static void find_freq_tables(struct cpufreq_frequency_table **freq_table,
-			     pxa_freqs_t **pxa_freqs)
+			     const struct pxa_freqs **pxa_freqs)
 {
 	if (cpu_is_pxa25x()) {
 		if (!pxa255_turbo_table) {
@@ -270,7 +270,7 @@ static unsigned int pxa_cpufreq_get(unsigned int cpu)
 static int pxa_set_target(struct cpufreq_policy *policy, unsigned int idx)
 {
 	struct cpufreq_frequency_table *pxa_freqs_table;
-	pxa_freqs_t *pxa_freq_settings;
+	const struct pxa_freqs *pxa_freq_settings;
 	unsigned long flags;
 	unsigned int new_freq_cpu, new_freq_mem;
 	unsigned int unused, preset_mdrefr, postset_mdrefr, cclkcfg;
@@ -361,7 +361,7 @@ static int pxa_cpufreq_init(struct cpufreq_policy *policy)
 	int i;
 	unsigned int freq;
 	struct cpufreq_frequency_table *pxa255_freq_table;
-	pxa_freqs_t *pxa255_freqs;
+	const struct pxa_freqs *pxa255_freqs;
 
 	/* try to guess pxa27x cpu */
 	if (cpu_is_pxa27x())
diff --git a/drivers/cpufreq/qoriq-cpufreq.c b/drivers/cpufreq/qoriq-cpufreq.c
index 88b21ae0d6b0..358f0752c31e 100644
--- a/drivers/cpufreq/qoriq-cpufreq.c
+++ b/drivers/cpufreq/qoriq-cpufreq.c
@@ -27,11 +27,11 @@
 
 /**
  * struct cpu_data
- * @parent: the parent node of cpu clock
+ * @pclk: the parent clock of cpu
  * @table: frequency table
  */
 struct cpu_data {
-	struct device_node *parent;
+	struct clk **pclk;
 	struct cpufreq_frequency_table *table;
 };
 
@@ -196,7 +196,7 @@ static void freq_table_sort(struct cpufreq_frequency_table *freq_table,
 
 static int qoriq_cpufreq_cpu_init(struct cpufreq_policy *policy)
 {
-	struct device_node *np;
+	struct device_node *np, *pnode;
 	int i, count, ret;
 	u32 freq, mask;
 	struct clk *clk;
@@ -219,17 +219,23 @@ static int qoriq_cpufreq_cpu_init(struct cpufreq_policy *policy)
 		goto err_nomem2;
 	}
 
-	data->parent = of_parse_phandle(np, "clocks", 0);
-	if (!data->parent) {
+	pnode = of_parse_phandle(np, "clocks", 0);
+	if (!pnode) {
 		pr_err("%s: could not get clock information\n", __func__);
 		goto err_nomem2;
 	}
 
-	count = of_property_count_strings(data->parent, "clock-names");
+	count = of_property_count_strings(pnode, "clock-names");
+	data->pclk = kcalloc(count, sizeof(struct clk *), GFP_KERNEL);
+	if (!data->pclk) {
+		pr_err("%s: no memory\n", __func__);
+		goto err_node;
+	}
+
 	table = kcalloc(count + 1, sizeof(*table), GFP_KERNEL);
 	if (!table) {
 		pr_err("%s: no memory\n", __func__);
-		goto err_node;
+		goto err_pclk;
 	}
 
 	if (fmask)
@@ -238,7 +244,8 @@ static int qoriq_cpufreq_cpu_init(struct cpufreq_policy *policy)
 		mask = 0x0;
 
 	for (i = 0; i < count; i++) {
-		clk = of_clk_get(data->parent, i);
+		clk = of_clk_get(pnode, i);
+		data->pclk[i] = clk;
 		freq = clk_get_rate(clk);
 		/*
 		 * the clock is valid if its frequency is not masked
@@ -273,13 +280,16 @@ static int qoriq_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	policy->cpuinfo.transition_latency = u64temp + 1;
 
 	of_node_put(np);
+	of_node_put(pnode);
 
 	return 0;
 
 err_nomem1:
 	kfree(table);
+err_pclk:
+	kfree(data->pclk);
 err_node:
-	of_node_put(data->parent);
+	of_node_put(pnode);
 err_nomem2:
 	policy->driver_data = NULL;
 	kfree(data);
@@ -293,7 +303,7 @@ static int __exit qoriq_cpufreq_cpu_exit(struct cpufreq_policy *policy)
 {
 	struct cpu_data *data = policy->driver_data;
 
-	of_node_put(data->parent);
+	kfree(data->pclk);
 	kfree(data->table);
 	kfree(data);
 	policy->driver_data = NULL;
@@ -307,7 +317,7 @@ static int qoriq_cpufreq_target(struct cpufreq_policy *policy,
 	struct clk *parent;
 	struct cpu_data *data = policy->driver_data;
 
-	parent = of_clk_get(data->parent, data->table[index].driver_data);
+	parent = data->pclk[data->table[index].driver_data];
 	return clk_set_parent(policy->clk, parent);
 }
 
diff --git a/drivers/cpufreq/s5pv210-cpufreq.c b/drivers/cpufreq/s5pv210-cpufreq.c
index b0dac7d6ba31..9e231f52150c 100644
--- a/drivers/cpufreq/s5pv210-cpufreq.c
+++ b/drivers/cpufreq/s5pv210-cpufreq.c
@@ -659,4 +659,4 @@ static struct platform_driver s5pv210_cpufreq_platdrv = {
 	},
 	.probe = s5pv210_cpufreq_probe,
 };
-module_platform_driver(s5pv210_cpufreq_platdrv);
+builtin_platform_driver(s5pv210_cpufreq_platdrv);
diff --git a/drivers/cpufreq/speedstep-ich.c b/drivers/cpufreq/speedstep-ich.c
index e56d632a8b21..37555c6b86a7 100644
--- a/drivers/cpufreq/speedstep-ich.c
+++ b/drivers/cpufreq/speedstep-ich.c
@@ -292,7 +292,7 @@ static int speedstep_cpu_init(struct cpufreq_policy *policy)
 
 	/* only run on CPU to be set, or on its sibling */
 #ifdef CONFIG_SMP
-	cpumask_copy(policy->cpus, cpu_sibling_mask(policy->cpu));
+	cpumask_copy(policy->cpus, topology_sibling_cpumask(policy->cpu));
 #endif
 	policy_cpu = cpumask_any_and(policy->cpus, cpu_online_mask);
 
diff --git a/drivers/cpuidle/cpuidle-at91.c b/drivers/cpuidle/cpuidle-at91.c
index f2446c78d87c..9c5853b6ca4a 100644
--- a/drivers/cpuidle/cpuidle-at91.c
+++ b/drivers/cpuidle/cpuidle-at91.c
@@ -62,5 +62,4 @@ static struct platform_driver at91_cpuidle_driver = {
 	},
 	.probe = at91_cpuidle_probe,
 };
-
-module_platform_driver(at91_cpuidle_driver);
+builtin_platform_driver(at91_cpuidle_driver);
diff --git a/drivers/cpuidle/cpuidle-big_little.c b/drivers/cpuidle/cpuidle-big_little.c
index 40c34faffe59..db2ede565f1a 100644
--- a/drivers/cpuidle/cpuidle-big_little.c
+++ b/drivers/cpuidle/cpuidle-big_little.c
@@ -108,13 +108,7 @@ static int notrace bl_powerdown_finisher(unsigned long arg)
 	unsigned int cpu = MPIDR_AFFINITY_LEVEL(mpidr, 0);
 
 	mcpm_set_entry_vector(cpu, cluster, cpu_resume);
-
-	/*
-	 * Residency value passed to mcpm_cpu_suspend back-end
-	 * has to be given clear semantics. Set to 0 as a
-	 * temporary value.
-	 */
-	mcpm_cpu_suspend(0);
+	mcpm_cpu_suspend();
 
 	/* return value != 0 means failure */
 	return 1;
diff --git a/drivers/cpuidle/cpuidle-calxeda.c b/drivers/cpuidle/cpuidle-calxeda.c
index 9445e6cc02be..c13feec89ea1 100644
--- a/drivers/cpuidle/cpuidle-calxeda.c
+++ b/drivers/cpuidle/cpuidle-calxeda.c
@@ -75,5 +75,4 @@ static struct platform_driver calxeda_cpuidle_plat_driver = {
         },
         .probe = calxeda_cpuidle_probe,
 };
-
-module_platform_driver(calxeda_cpuidle_plat_driver);
+builtin_platform_driver(calxeda_cpuidle_plat_driver);
diff --git a/drivers/cpuidle/cpuidle-powernv.c b/drivers/cpuidle/cpuidle-powernv.c
index 59372077ec7c..845bafcfa792 100644
--- a/drivers/cpuidle/cpuidle-powernv.c
+++ b/drivers/cpuidle/cpuidle-powernv.c
@@ -29,18 +29,25 @@ struct cpuidle_driver powernv_idle_driver = {
 
 static int max_idle_state;
 static struct cpuidle_state *cpuidle_state_table;
+static u64 snooze_timeout;
+static bool snooze_timeout_en;
 
 static int snooze_loop(struct cpuidle_device *dev,
 			struct cpuidle_driver *drv,
 			int index)
 {
+	u64 snooze_exit_time;
+
 	local_irq_enable();
 	set_thread_flag(TIF_POLLING_NRFLAG);
 
+	snooze_exit_time = get_tb() + snooze_timeout;
 	ppc64_runlatch_off();
 	while (!need_resched()) {
 		HMT_low();
 		HMT_very_low();
+		if (snooze_timeout_en && get_tb() > snooze_exit_time)
+			break;
 	}
 
 	HMT_medium();
@@ -60,6 +67,8 @@ static int nap_loop(struct cpuidle_device *dev,
 	return index;
 }
 
+/* Register for fastsleep only in oneshot mode of broadcast */
+#ifdef CONFIG_TICK_ONESHOT
 static int fastsleep_loop(struct cpuidle_device *dev,
 				struct cpuidle_driver *drv,
 				int index)
@@ -83,7 +92,7 @@ static int fastsleep_loop(struct cpuidle_device *dev,
 
 	return index;
 }
-
+#endif
 /*
  * States for dedicated partition case.
  */
@@ -209,7 +218,14 @@ static int powernv_add_idle_states(void)
 			powernv_states[nr_idle_states].flags = 0;
 			powernv_states[nr_idle_states].target_residency = 100;
 			powernv_states[nr_idle_states].enter = &nap_loop;
-		} else if (flags[i] & OPAL_PM_SLEEP_ENABLED ||
+		}
+
+		/*
+		 * All cpuidle states with CPUIDLE_FLAG_TIMER_STOP set must come
+		 * within this config dependency check.
+		 */
+#ifdef CONFIG_TICK_ONESHOT
+		if (flags[i] & OPAL_PM_SLEEP_ENABLED ||
 			flags[i] & OPAL_PM_SLEEP_ENABLED_ER1) {
 			/* Add FASTSLEEP state */
 			strcpy(powernv_states[nr_idle_states].name, "FastSleep");
@@ -218,7 +234,7 @@ static int powernv_add_idle_states(void)
 			powernv_states[nr_idle_states].target_residency = 300000;
 			powernv_states[nr_idle_states].enter = &fastsleep_loop;
 		}
-
+#endif
 		powernv_states[nr_idle_states].exit_latency =
 				((unsigned int)latency_ns[i]) / 1000;
 
@@ -252,6 +268,11 @@ static int powernv_idle_probe(void)
 		cpuidle_state_table = powernv_states;
 		/* Device tree can indicate more idle states */
 		max_idle_state = powernv_add_idle_states();
+		if (max_idle_state > 1) {
+			snooze_timeout_en = true;
+			snooze_timeout = powernv_states[1].target_residency *
+					 tb_ticks_per_usec;
+		}
  	} else
  		return -ENODEV;
 
diff --git a/drivers/cpuidle/cpuidle-pseries.c b/drivers/cpuidle/cpuidle-pseries.c
index bb9e2b6f3ecc..07135e009d8b 100644
--- a/drivers/cpuidle/cpuidle-pseries.c
+++ b/drivers/cpuidle/cpuidle-pseries.c
@@ -27,6 +27,8 @@ struct cpuidle_driver pseries_idle_driver = {
 
 static int max_idle_state;
 static struct cpuidle_state *cpuidle_state_table;
+static u64 snooze_timeout;
+static bool snooze_timeout_en;
 
 static inline void idle_loop_prolog(unsigned long *in_purr)
 {
@@ -58,14 +60,18 @@ static int snooze_loop(struct cpuidle_device *dev,
 			int index)
 {
 	unsigned long in_purr;
+	u64 snooze_exit_time;
 
 	idle_loop_prolog(&in_purr);
 	local_irq_enable();
 	set_thread_flag(TIF_POLLING_NRFLAG);
+	snooze_exit_time = get_tb() + snooze_timeout;
 
 	while (!need_resched()) {
 		HMT_low();
 		HMT_very_low();
+		if (snooze_timeout_en && get_tb() > snooze_exit_time)
+			break;
 	}
 
 	HMT_medium();
@@ -244,6 +250,11 @@ static int pseries_idle_probe(void)
 	} else
 		return -ENODEV;
 
+	if (max_idle_state > 1) {
+		snooze_timeout_en = true;
+		snooze_timeout = cpuidle_state_table[1].target_residency *
+				 tb_ticks_per_usec;
+	}
 	return 0;
 }
 
diff --git a/drivers/cpuidle/cpuidle-zynq.c b/drivers/cpuidle/cpuidle-zynq.c
index 543292b1d38e..6f4257fc56e5 100644
--- a/drivers/cpuidle/cpuidle-zynq.c
+++ b/drivers/cpuidle/cpuidle-zynq.c
@@ -73,5 +73,4 @@ static struct platform_driver zynq_cpuidle_driver = {
 	},
 	.probe = zynq_cpuidle_probe,
 };
-
-module_platform_driver(zynq_cpuidle_driver);
+builtin_platform_driver(zynq_cpuidle_driver);
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 61c417b9e53f..e8e2775c3821 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -65,7 +65,7 @@ int cpuidle_play_dead(void)
 		return -ENODEV;
 
 	/* Find lowest-power state that supports long-term idle */
-	for (i = drv->state_count - 1; i >= CPUIDLE_DRIVER_STATE_START; i--)
+	for (i = drv->state_count - 1; i >= 0; i--)
 		if (drv->states[i].enter_dead)
 			return drv->states[i].enter_dead(dev, i);
 
@@ -73,16 +73,21 @@ int cpuidle_play_dead(void)
 }
 
 static int find_deepest_state(struct cpuidle_driver *drv,
-			      struct cpuidle_device *dev, bool freeze)
+			      struct cpuidle_device *dev,
+			      unsigned int max_latency,
+			      unsigned int forbidden_flags,
+			      bool freeze)
 {
 	unsigned int latency_req = 0;
-	int i, ret = freeze ? -1 : CPUIDLE_DRIVER_STATE_START - 1;
+	int i, ret = -ENXIO;
 
-	for (i = CPUIDLE_DRIVER_STATE_START; i < drv->state_count; i++) {
+	for (i = 0; i < drv->state_count; i++) {
 		struct cpuidle_state *s = &drv->states[i];
 		struct cpuidle_state_usage *su = &dev->states_usage[i];
 
 		if (s->disabled || su->disable || s->exit_latency <= latency_req
+		    || s->exit_latency > max_latency
+		    || (s->flags & forbidden_flags)
 		    || (freeze && !s->enter_freeze))
 			continue;
 
@@ -92,6 +97,7 @@ static int find_deepest_state(struct cpuidle_driver *drv,
 	return ret;
 }
 
+#ifdef CONFIG_SUSPEND
 /**
  * cpuidle_find_deepest_state - Find the deepest available idle state.
  * @drv: cpuidle driver for the given CPU.
@@ -100,7 +106,7 @@ static int find_deepest_state(struct cpuidle_driver *drv,
 int cpuidle_find_deepest_state(struct cpuidle_driver *drv,
 			       struct cpuidle_device *dev)
 {
-	return find_deepest_state(drv, dev, false);
+	return find_deepest_state(drv, dev, UINT_MAX, 0, false);
 }
 
 static void enter_freeze_proper(struct cpuidle_driver *drv,
@@ -139,18 +145,19 @@ int cpuidle_enter_freeze(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	 * that interrupts won't be enabled when it exits and allows the tick to
 	 * be frozen safely.
 	 */
-	index = find_deepest_state(drv, dev, true);
+	index = find_deepest_state(drv, dev, UINT_MAX, 0, true);
 	if (index >= 0)
 		enter_freeze_proper(drv, dev, index);
 
 	return index;
 }
+#endif /* CONFIG_SUSPEND */
 
 /**
  * cpuidle_enter_state - enter the state and update stats
  * @dev: cpuidle device for this cpu
  * @drv: cpuidle driver for this cpu
- * @next_state: index into drv->states of the state to enter
+ * @index: index into the states table in @drv of the state to enter
  */
 int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
 			int index)
@@ -167,8 +174,18 @@ int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
 	 * local timer will be shut down.  If a local timer is used from another
 	 * CPU as a broadcast timer, this call may fail if it is not available.
 	 */
-	if (broadcast && tick_broadcast_enter())
-		return -EBUSY;
+	if (broadcast && tick_broadcast_enter()) {
+		index = find_deepest_state(drv, dev, target_state->exit_latency,
+					   CPUIDLE_FLAG_TIMER_STOP, false);
+		if (index < 0) {
+			default_idle_call();
+			return -EBUSY;
+		}
+		target_state = &drv->states[index];
+	}
+
+	/* Take note of the planned idle state. */
+	sched_idle_set_state(target_state);
 
 	trace_cpu_idle_rcuidle(index, dev->cpu);
 	time_start = ktime_get();
@@ -178,6 +195,9 @@ int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
 	time_end = ktime_get();
 	trace_cpu_idle_rcuidle(PWR_EVENT_EXIT, dev->cpu);
 
+	/* The cpu is no longer idle or about to enter idle. */
+	sched_idle_set_state(NULL);
+
 	if (broadcast) {
 		if (WARN_ON_ONCE(!irqs_disabled()))
 			local_irq_disable();
@@ -249,7 +269,7 @@ int cpuidle_enter(struct cpuidle_driver *drv, struct cpuidle_device *dev,
  */
 void cpuidle_reflect(struct cpuidle_device *dev, int index)
 {
-	if (cpuidle_curr_governor->reflect)
+	if (cpuidle_curr_governor->reflect && index >= 0)
 		cpuidle_curr_governor->reflect(dev, index);
 }
 
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index b8a5fa15ca24..22e4463d1787 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -367,9 +367,9 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 static void menu_reflect(struct cpuidle_device *dev, int index)
 {
 	struct menu_device *data = this_cpu_ptr(&menu_devices);
+
 	data->last_state_idx = index;
-	if (index >= 0)
-		data->needs_update = 1;
+	data->needs_update = 1;
 }
 
 /**
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index 033c0c86f6ec..4044125fb5d5 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -162,10 +162,10 @@ config CRYPTO_GHASH_S390
 config CRYPTO_DEV_MV_CESA
 	tristate "Marvell's Cryptographic Engine"
 	depends on PLAT_ORION
-	select CRYPTO_ALGAPI
 	select CRYPTO_AES
-	select CRYPTO_BLKCIPHER2
+	select CRYPTO_BLKCIPHER
 	select CRYPTO_HASH
+	select SRAM
 	help
 	  This driver allows you to utilize the Cryptographic Engines and
 	  Security Accelerator (CESA) which can be found on the Marvell Orion
@@ -173,10 +173,27 @@ config CRYPTO_DEV_MV_CESA
 
 	  Currently the driver supports AES in ECB and CBC mode without DMA.
 
+config CRYPTO_DEV_MARVELL_CESA
+	tristate "New Marvell's Cryptographic Engine driver"
+	depends on PLAT_ORION || ARCH_MVEBU
+	select CRYPTO_AES
+	select CRYPTO_DES
+	select CRYPTO_BLKCIPHER
+	select CRYPTO_HASH
+	select SRAM
+	help
+	  This driver allows you to utilize the Cryptographic Engines and
+	  Security Accelerator (CESA) which can be found on the Armada 370.
+	  This driver supports CPU offload through DMA transfers.
+
+	  This driver is aimed at replacing the mv_cesa driver. This will only
+	  happen once it has received proper testing.
+
 config CRYPTO_DEV_NIAGARA2
        tristate "Niagara2 Stream Processing Unit driver"
        select CRYPTO_DES
-       select CRYPTO_ALGAPI
+       select CRYPTO_BLKCIPHER
+       select CRYPTO_HASH
        depends on SPARC64
        help
 	  Each core of a Niagara2 processor contains a Stream
@@ -189,7 +206,6 @@ config CRYPTO_DEV_NIAGARA2
 config CRYPTO_DEV_HIFN_795X
 	tristate "Driver HIFN 795x crypto accelerator chips"
 	select CRYPTO_DES
-	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
 	select HW_RANDOM if CRYPTO_DEV_HIFN_795X_RNG
 	depends on PCI
@@ -208,8 +224,10 @@ source drivers/crypto/caam/Kconfig
 
 config CRYPTO_DEV_TALITOS
 	tristate "Talitos Freescale Security Engine (SEC)"
-	select CRYPTO_ALGAPI
+	select CRYPTO_AEAD
 	select CRYPTO_AUTHENC
+	select CRYPTO_BLKCIPHER
+	select CRYPTO_HASH
 	select HW_RANDOM
 	depends on FSL_SOC
 	help
@@ -222,11 +240,29 @@ config CRYPTO_DEV_TALITOS
 	  To compile this driver as a module, choose M here: the module
 	  will be called talitos.
 
+config CRYPTO_DEV_TALITOS1
+	bool "SEC1 (SEC 1.0 and SEC Lite 1.2)"
+	depends on CRYPTO_DEV_TALITOS
+	depends on PPC_8xx || PPC_82xx
+	default y
+	help
+	  Say 'Y' here to use the Freescale Security Engine (SEC) version 1.0
+	  found on MPC82xx or the Freescale Security Engine (SEC Lite)
+	  version 1.2 found on MPC8xx
+
+config CRYPTO_DEV_TALITOS2
+	bool "SEC2+ (SEC version 2.0 or upper)"
+	depends on CRYPTO_DEV_TALITOS
+	default y if !PPC_8xx
+	help
+	  Say 'Y' here to use the Freescale Security Engine (SEC)
+	  version 2 and following as found on MPC83xx, MPC85xx, etc ...
+
 config CRYPTO_DEV_IXP4XX
 	tristate "Driver for IXP4xx crypto hardware acceleration"
 	depends on ARCH_IXP4XX && IXP4XX_QMGR && IXP4XX_NPE
 	select CRYPTO_DES
-	select CRYPTO_ALGAPI
+	select CRYPTO_AEAD
 	select CRYPTO_AUTHENC
 	select CRYPTO_BLKCIPHER
 	help
@@ -236,7 +272,6 @@ config CRYPTO_DEV_PPC4XX
 	tristate "Driver AMCC PPC4xx crypto accelerator"
 	depends on PPC && 4xx
 	select CRYPTO_HASH
-	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
 	help
 	  This option allows you to have support for AMCC crypto acceleration.
@@ -257,7 +292,7 @@ config CRYPTO_DEV_OMAP_AES
 	tristate "Support for OMAP AES hw engine"
 	depends on ARCH_OMAP2 || ARCH_OMAP3 || ARCH_OMAP2PLUS
 	select CRYPTO_AES
-	select CRYPTO_BLKCIPHER2
+	select CRYPTO_BLKCIPHER
 	help
 	  OMAP processors have AES module accelerator. Select this if you
 	  want to use the OMAP module for AES algorithms.
@@ -266,7 +301,7 @@ config CRYPTO_DEV_OMAP_DES
 	tristate "Support for OMAP DES3DES hw engine"
 	depends on ARCH_OMAP2PLUS
 	select CRYPTO_DES
-	select CRYPTO_BLKCIPHER2
+	select CRYPTO_BLKCIPHER
 	help
 	  OMAP processors have DES/3DES module accelerator. Select this if you
 	  want to use the OMAP module for DES and 3DES algorithms. Currently
@@ -276,9 +311,10 @@ config CRYPTO_DEV_OMAP_DES
 config CRYPTO_DEV_PICOXCELL
 	tristate "Support for picoXcell IPSEC and Layer2 crypto engines"
 	depends on ARCH_PICOXCELL && HAVE_CLK
+	select CRYPTO_AEAD
 	select CRYPTO_AES
 	select CRYPTO_AUTHENC
-	select CRYPTO_ALGAPI
+	select CRYPTO_BLKCIPHER
 	select CRYPTO_DES
 	select CRYPTO_CBC
 	select CRYPTO_ECB
@@ -304,7 +340,6 @@ config CRYPTO_DEV_S5P
 	tristate "Support for Samsung S5PV210/Exynos crypto accelerator"
 	depends on ARCH_S5PV210 || ARCH_EXYNOS
 	select CRYPTO_AES
-	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
 	help
 	  This option allows you to have support for S5P crypto acceleration.
@@ -312,11 +347,13 @@ config CRYPTO_DEV_S5P
 	  algorithms execution.
 
 config CRYPTO_DEV_NX
-	bool "Support for IBM Power7+ in-Nest cryptographic acceleration"
-	depends on PPC64 && IBMVIO && !CPU_LITTLE_ENDIAN
-	default n
+	bool "Support for IBM PowerPC Nest (NX) cryptographic acceleration"
+	depends on PPC64
 	help
-	  Support for Power7+ in-Nest cryptographic acceleration.
+	  This enables support for the NX hardware cryptographic accelerator
+	  coprocessor that is in IBM PowerPC P7+ or later processors.  This
+	  does not actually enable any drivers, it only allows you to select
+	  which acceleration type (encryption and/or compression) to enable.
 
 if CRYPTO_DEV_NX
 	source "drivers/crypto/nx/Kconfig"
@@ -325,7 +362,6 @@ endif
 config CRYPTO_DEV_UX500
 	tristate "Driver for ST-Ericsson UX500 crypto hardware acceleration"
 	depends on ARCH_U8500
-	select CRYPTO_ALGAPI
 	help
 	  Driver for ST-Ericsson UX500 crypto engine.
 
@@ -343,10 +379,7 @@ config CRYPTO_DEV_BFIN_CRC
 config CRYPTO_DEV_ATMEL_AES
 	tristate "Support for Atmel AES hw accelerator"
 	depends on ARCH_AT91
-	select CRYPTO_CBC
-	select CRYPTO_ECB
 	select CRYPTO_AES
-	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
 	select AT_HDMAC
 	help
@@ -361,9 +394,6 @@ config CRYPTO_DEV_ATMEL_TDES
 	tristate "Support for Atmel DES/TDES hw accelerator"
 	depends on ARCH_AT91
 	select CRYPTO_DES
-	select CRYPTO_CBC
-	select CRYPTO_ECB
-	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
 	help
 	  Some Atmel processors have DES/TDES hw accelerator.
@@ -376,10 +406,7 @@ config CRYPTO_DEV_ATMEL_TDES
 config CRYPTO_DEV_ATMEL_SHA
 	tristate "Support for Atmel SHA hw accelerator"
 	depends on ARCH_AT91
-	select CRYPTO_SHA1
-	select CRYPTO_SHA256
-	select CRYPTO_SHA512
-	select CRYPTO_ALGAPI
+	select CRYPTO_HASH
 	help
 	  Some Atmel processors have SHA1/SHA224/SHA256/SHA384/SHA512
 	  hw accelerator.
@@ -392,7 +419,6 @@ config CRYPTO_DEV_ATMEL_SHA
 config CRYPTO_DEV_CCP
 	bool "Support for AMD Cryptographic Coprocessor"
 	depends on ((X86 && PCI) || (ARM64 && (OF_ADDRESS || ACPI))) && HAS_IOMEM
-	default n
 	help
 	  The AMD Cryptographic Coprocessor provides hardware support
 	  for encryption, hashing and related operations.
@@ -404,13 +430,11 @@ endif
 config CRYPTO_DEV_MXS_DCP
 	tristate "Support for Freescale MXS DCP"
 	depends on ARCH_MXS
-	select CRYPTO_SHA1
-	select CRYPTO_SHA256
 	select CRYPTO_CBC
 	select CRYPTO_ECB
 	select CRYPTO_AES
 	select CRYPTO_BLKCIPHER
-	select CRYPTO_ALGAPI
+	select CRYPTO_HASH
 	help
 	  The Freescale i.MX23/i.MX28 has SHA1/SHA256 and AES128 CBC/ECB
 	  co-processor on the die.
@@ -429,7 +453,6 @@ config CRYPTO_DEV_QCE
 	select CRYPTO_CBC
 	select CRYPTO_XTS
 	select CRYPTO_CTR
-	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
 	help
 	  This driver supports Qualcomm crypto engine accelerator
@@ -439,7 +462,6 @@ config CRYPTO_DEV_QCE
 config CRYPTO_DEV_VMX
 	bool "Support for VMX cryptographic acceleration instructions"
 	depends on PPC64
-	default n
 	help
 	  Support for VMX cryptographic acceleration instructions.
 
@@ -449,7 +471,6 @@ config CRYPTO_DEV_IMGTEC_HASH
 	tristate "Imagination Technologies hardware hash accelerator"
 	depends on MIPS || COMPILE_TEST
 	depends on HAS_DMA
-	select CRYPTO_ALGAPI
 	select CRYPTO_MD5
 	select CRYPTO_SHA1
 	select CRYPTO_SHA256
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index fb84be7e6be5..e35c07a8da85 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_CRYPTO_DEV_HIFN_795X) += hifn_795x.o
 obj-$(CONFIG_CRYPTO_DEV_IMGTEC_HASH) += img-hash.o
 obj-$(CONFIG_CRYPTO_DEV_IXP4XX) += ixp4xx_crypto.o
 obj-$(CONFIG_CRYPTO_DEV_MV_CESA) += mv_cesa.o
+obj-$(CONFIG_CRYPTO_DEV_MARVELL_CESA) += marvell/
 obj-$(CONFIG_CRYPTO_DEV_MXS_DCP) += mxs-dcp.o
 obj-$(CONFIG_CRYPTO_DEV_NIAGARA2) += n2_crypto.o
 n2_crypto-y := n2_core.o n2_asm.o
diff --git a/drivers/crypto/caam/Kconfig b/drivers/crypto/caam/Kconfig
index e7555ff4cafd..e286e285aa8a 100644
--- a/drivers/crypto/caam/Kconfig
+++ b/drivers/crypto/caam/Kconfig
@@ -45,7 +45,6 @@ config CRYPTO_DEV_FSL_CAAM_RINGSIZE
 config CRYPTO_DEV_FSL_CAAM_INTC
 	bool "Job Ring interrupt coalescing"
 	depends on CRYPTO_DEV_FSL_CAAM_JR
-	default n
 	help
 	  Enable the Job Ring's interrupt coalescing feature.
 
@@ -77,8 +76,9 @@ config CRYPTO_DEV_FSL_CAAM_CRYPTO_API
 	tristate "Register algorithm implementations with the Crypto API"
 	depends on CRYPTO_DEV_FSL_CAAM && CRYPTO_DEV_FSL_CAAM_JR
 	default y
-	select CRYPTO_ALGAPI
+	select CRYPTO_AEAD
 	select CRYPTO_AUTHENC
+	select CRYPTO_BLKCIPHER
 	help
 	  Selecting this will offload crypto for users of the
 	  scatterlist crypto API (such as the linux native IPSec
@@ -115,7 +115,6 @@ config CRYPTO_DEV_FSL_CAAM_RNG_API
 config CRYPTO_DEV_FSL_CAAM_DEBUG
 	bool "Enable debug output in CAAM driver"
 	depends on CRYPTO_DEV_FSL_CAAM
-	default n
 	help
 	  Selecting this will enable printing of various debug
 	  information in the CAAM driver.
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index 29071a156cbe..daca933a82ec 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -65,6 +65,10 @@
 /* max IV is max of AES_BLOCK_SIZE, DES3_EDE_BLOCK_SIZE */
 #define CAAM_MAX_IV_LENGTH		16
 
+#define AEAD_DESC_JOB_IO_LEN		(DESC_JOB_IO_LEN + CAAM_CMD_SZ * 2)
+#define GCM_DESC_JOB_IO_LEN		(AEAD_DESC_JOB_IO_LEN + \
+					 CAAM_CMD_SZ * 4)
+
 /* length of descriptors text */
 #define DESC_AEAD_BASE			(4 * CAAM_CMD_SZ)
 #define DESC_AEAD_ENC_LEN		(DESC_AEAD_BASE + 15 * CAAM_CMD_SZ)
@@ -79,18 +83,16 @@
 #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_GCM_ENC_LEN		(DESC_GCM_BASE + 16 * CAAM_CMD_SZ)
+#define DESC_GCM_DEC_LEN		(DESC_GCM_BASE + 12 * 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_RFC4106_ENC_LEN		(DESC_RFC4106_BASE + 10 * CAAM_CMD_SZ)
+#define DESC_RFC4106_DEC_LEN		(DESC_RFC4106_BASE + 10 * 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_RFC4543_ENC_LEN		(DESC_RFC4543_BASE + 11 * CAAM_CMD_SZ)
+#define DESC_RFC4543_DEC_LEN		(DESC_RFC4543_BASE + 12 * CAAM_CMD_SZ)
 
 #define DESC_ABLKCIPHER_BASE		(3 * CAAM_CMD_SZ)
 #define DESC_ABLKCIPHER_ENC_LEN		(DESC_ABLKCIPHER_BASE + \
@@ -98,8 +100,7 @@
 #define DESC_ABLKCIPHER_DEC_LEN		(DESC_ABLKCIPHER_BASE + \
 					 15 * CAAM_CMD_SZ)
 
-#define DESC_MAX_USED_BYTES		(DESC_RFC4543_GIVENC_LEN + \
-					 CAAM_MAX_KEY_SIZE)
+#define DESC_MAX_USED_BYTES		(CAAM_DESC_BYTES_MAX - DESC_JOB_IO_LEN)
 #define DESC_MAX_USED_LEN		(DESC_MAX_USED_BYTES / CAAM_CMD_SZ)
 
 #ifdef DEBUG
@@ -258,7 +259,7 @@ static void init_sh_desc_key_aead(u32 *desc, struct caam_ctx *ctx,
 
 static int aead_null_set_sh_desc(struct crypto_aead *aead)
 {
-	struct aead_tfm *tfm = &aead->base.crt_aead;
+	unsigned int ivsize = crypto_aead_ivsize(aead);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
 	struct device *jrdev = ctx->jrdev;
 	bool keys_fit_inline = false;
@@ -273,7 +274,7 @@ static int aead_null_set_sh_desc(struct crypto_aead *aead)
 	    ctx->split_key_pad_len <= CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
-	/* aead_encrypt shared descriptor */
+	/* old_aead_encrypt shared descriptor */
 	desc = ctx->sh_desc_enc;
 
 	init_sh_desc(desc, HDR_SHARE_SERIAL);
@@ -362,7 +363,7 @@ static int aead_null_set_sh_desc(struct crypto_aead *aead)
 
 	desc = ctx->sh_desc_dec;
 
-	/* aead_decrypt shared descriptor */
+	/* old_aead_decrypt shared descriptor */
 	init_sh_desc(desc, HDR_SHARE_SERIAL);
 
 	/* Skip if already shared */
@@ -383,7 +384,7 @@ static int aead_null_set_sh_desc(struct crypto_aead *aead)
 
 	/* assoclen + cryptlen = seqinlen - ivsize - authsize */
 	append_math_sub_imm_u32(desc, REG3, SEQINLEN, IMM,
-				ctx->authsize + tfm->ivsize);
+				ctx->authsize + 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);
@@ -449,7 +450,7 @@ static int aead_null_set_sh_desc(struct crypto_aead *aead)
 
 static int aead_set_sh_desc(struct crypto_aead *aead)
 {
-	struct aead_tfm *tfm = &aead->base.crt_aead;
+	unsigned int ivsize = crypto_aead_ivsize(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);
@@ -496,7 +497,7 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	    CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
-	/* aead_encrypt shared descriptor */
+	/* old_aead_encrypt shared descriptor */
 	desc = ctx->sh_desc_enc;
 
 	/* Note: Context registers are saved. */
@@ -510,7 +511,7 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	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);
+	append_math_sub_imm_u32(desc, REG2, SEQINLEN, IMM, ivsize);
 
 	/* assoclen = (assoclen + cryptlen) - cryptlen */
 	append_math_sub(desc, VARSEQINLEN, REG2, REG3, CAAM_CMD_SZ);
@@ -518,7 +519,7 @@ 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, ctx1_iv_off);
+	aead_append_ld_iv(desc, ivsize, ctx1_iv_off);
 
 	/* Load Counter into CONTEXT1 reg */
 	if (is_rfc3686)
@@ -565,7 +566,7 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	    CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
-	/* aead_decrypt shared descriptor */
+	/* old_aead_decrypt shared descriptor */
 	desc = ctx->sh_desc_dec;
 
 	/* Note: Context registers are saved. */
@@ -577,7 +578,7 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 
 	/* assoclen + cryptlen = seqinlen - ivsize - authsize */
 	append_math_sub_imm_u32(desc, REG3, SEQINLEN, IMM,
-				ctx->authsize + tfm->ivsize);
+				ctx->authsize + 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);
@@ -586,7 +587,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);
 
-	aead_append_ld_iv(desc, tfm->ivsize, ctx1_iv_off);
+	aead_append_ld_iv(desc, ivsize, ctx1_iv_off);
 
 	/* Load Counter into CONTEXT1 reg */
 	if (is_rfc3686)
@@ -645,20 +646,20 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	/* Generate IV */
 	geniv = NFIFOENTRY_STYPE_PAD | NFIFOENTRY_DEST_DECO |
 		NFIFOENTRY_DTYPE_MSG | NFIFOENTRY_LC1 |
-		NFIFOENTRY_PTYPE_RND | (tfm->ivsize << NFIFOENTRY_DLEN_SHIFT);
+		NFIFOENTRY_PTYPE_RND | (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 |
 		    (ctx1_iv_off << MOVE_OFFSET_SHIFT) |
-		    (tfm->ivsize << MOVE_LEN_SHIFT));
+		    (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 |
 		    (ctx1_iv_off << MOVE_OFFSET_SHIFT) |
-		    (tfm->ivsize << MOVE_LEN_SHIFT));
+		    (ivsize << MOVE_LEN_SHIFT));
 
 	/* Return to encryption */
 	append_operation(desc, ctx->class2_alg_type |
@@ -676,10 +677,10 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 
 	/* Copy iv from outfifo to class 2 fifo */
 	moveiv = NFIFOENTRY_STYPE_OFIFO | NFIFOENTRY_DEST_CLASS2 |
-		 NFIFOENTRY_DTYPE_MSG | (tfm->ivsize << NFIFOENTRY_DLEN_SHIFT);
+		 NFIFOENTRY_DTYPE_MSG | (ivsize << NFIFOENTRY_DLEN_SHIFT);
 	append_load_imm_u32(desc, moveiv, LDST_CLASS_IND_CCB |
 			    LDST_SRCDST_WORD_INFO_FIFO | LDST_IMM);
-	append_load_imm_u32(desc, tfm->ivsize, LDST_CLASS_2_CCB |
+	append_load_imm_u32(desc, ivsize, LDST_CLASS_2_CCB |
 			    LDST_SRCDST_WORD_DATASZ_REG | LDST_IMM);
 
 	/* Load Counter into CONTEXT1 reg */
@@ -698,7 +699,7 @@ static int aead_set_sh_desc(struct crypto_aead *aead)
 	append_math_add(desc, VARSEQOUTLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
 
 	/* Not need to reload iv */
-	append_seq_fifo_load(desc, tfm->ivsize,
+	append_seq_fifo_load(desc, ivsize,
 			     FIFOLD_CLASS_SKIP);
 
 	/* Will read cryptlen */
@@ -738,7 +739,6 @@ static int aead_setauthsize(struct crypto_aead *authenc,
 
 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;
@@ -754,7 +754,7 @@ static int gcm_set_sh_desc(struct crypto_aead *aead)
 	 * Job Descriptor and Shared Descriptor
 	 * must fit into the 64-word Descriptor h/w Buffer
 	 */
-	if (DESC_GCM_ENC_LEN + DESC_JOB_IO_LEN +
+	if (DESC_GCM_ENC_LEN + GCM_DESC_JOB_IO_LEN +
 	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
@@ -777,34 +777,34 @@ static int gcm_set_sh_desc(struct crypto_aead *aead)
 	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);
+	/* if assoclen + cryptlen is ZERO, skip to ICV write */
+	append_math_sub(desc, VARSEQOUTLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+	zero_assoc_jump_cmd2 = append_jump(desc, JUMP_TEST_ALL |
+						 JUMP_COND_MATH_Z);
 
-	/* assoclen + cryptlen = seqinlen - ivsize */
-	append_math_sub_imm_u32(desc, REG2, SEQINLEN, IMM, tfm->ivsize);
+	/* if assoclen is ZERO, skip reading the assoc data */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
+	zero_assoc_jump_cmd1 = append_jump(desc, JUMP_TEST_ALL |
+						 JUMP_COND_MATH_Z);
 
-	/* assoclen = (assoclen + cryptlen) - cryptlen */
-	append_math_sub(desc, REG1, REG2, REG3, CAAM_CMD_SZ);
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG3, CAAM_CMD_SZ);
+
+	/* skip assoc data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_SKIP | FIFOLDST_VLF);
+
+	/* cryptlen = seqinlen - assoclen */
+	append_math_sub(desc, VARSEQOUTLEN, SEQINLEN, 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);
+	append_math_sub(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
 
 	/* write encrypted data */
 	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
@@ -814,31 +814,17 @@ static int gcm_set_sh_desc(struct crypto_aead *aead)
 			     FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST1);
 
 	/* jump the zero-payload commands */
-	append_jump(desc, JUMP_TEST_ALL | 7);
+	append_jump(desc, JUMP_TEST_ALL | 2);
 
 	/* 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 */
+	/* There is no 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 |
@@ -862,7 +848,7 @@ static int gcm_set_sh_desc(struct crypto_aead *aead)
 	 * must all fit into the 64-word Descriptor h/w Buffer
 	 */
 	keys_fit_inline = false;
-	if (DESC_GCM_DEC_LEN + DESC_JOB_IO_LEN +
+	if (DESC_GCM_DEC_LEN + GCM_DESC_JOB_IO_LEN +
 	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
@@ -886,33 +872,30 @@ static int gcm_set_sh_desc(struct crypto_aead *aead)
 	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);
+	/* if assoclen is ZERO, skip reading the assoc data */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
+	zero_assoc_jump_cmd1 = 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);
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG3, CAAM_CMD_SZ);
 
-	/* 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);
+	/* skip assoc data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_SKIP | FIFOLDST_VLF);
 
-	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);
+	/* cryptlen = seqoutlen - assoclen */
+	append_math_sub(desc, VARSEQINLEN, SEQOUTLEN, REG0, CAAM_CMD_SZ);
+
+	/* jump to zero-payload command if cryptlen is zero */
+	zero_payload_jump_cmd = append_jump(desc, JUMP_TEST_ALL |
+					    JUMP_COND_MATH_Z);
+
+	append_math_sub(desc, VARSEQOUTLEN, SEQOUTLEN, REG0, CAAM_CMD_SZ);
 
 	/* store encrypted data */
 	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
@@ -921,21 +904,9 @@ static int gcm_set_sh_desc(struct crypto_aead *aead)
 	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);
@@ -968,13 +939,11 @@ static int gcm_setauthsize(struct crypto_aead *authenc, unsigned int authsize)
 
 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 *key_jump_cmd;
 	u32 *desc;
-	u32 geniv;
 
 	if (!ctx->enckeylen || !ctx->authsize)
 		return 0;
@@ -984,7 +953,7 @@ static int rfc4106_set_sh_desc(struct crypto_aead *aead)
 	 * Job Descriptor and Shared Descriptor
 	 * must fit into the 64-word Descriptor h/w Buffer
 	 */
-	if (DESC_RFC4106_ENC_LEN + DESC_JOB_IO_LEN +
+	if (DESC_RFC4106_ENC_LEN + GCM_DESC_JOB_IO_LEN +
 	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
@@ -1007,29 +976,21 @@ static int rfc4106_set_sh_desc(struct crypto_aead *aead)
 	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, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
 	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);
+	/* Skip assoc data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_SKIP | FIFOLDST_VLF);
 
 	/* Read assoc data */
 	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
 			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_FLUSH1);
 
+	/* cryptlen = seqoutlen - assoclen */
+	append_math_sub(desc, VARSEQOUTLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+
 	/* Will read cryptlen bytes */
-	append_math_add(desc, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
+	append_math_sub(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
 
 	/* Write encrypted data */
 	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
@@ -1083,30 +1044,21 @@ static int rfc4106_set_sh_desc(struct crypto_aead *aead)
 	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);
+	append_math_add(desc, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
+	append_math_add(desc, VARSEQOUTLEN, ZERO, 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);
+	/* Skip assoc data */
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_SKIP | FIFOLDST_VLF);
 
 	/* Read assoc data */
 	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | FIFOLDST_VLF |
 			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_FLUSH1);
 
+	/* Will write cryptlen bytes */
+	append_math_sub(desc, VARSEQOUTLEN, SEQOUTLEN, REG0, CAAM_CMD_SZ);
+
 	/* Will read cryptlen bytes */
-	append_math_add(desc, VARSEQINLEN, ZERO, REG2, CAAM_CMD_SZ);
+	append_math_sub(desc, VARSEQINLEN, SEQOUTLEN, REG0, CAAM_CMD_SZ);
 
 	/* Store payload data */
 	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
@@ -1132,107 +1084,6 @@ static int rfc4106_set_sh_desc(struct crypto_aead *aead)
 		       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;
 }
 
@@ -1249,14 +1100,12 @@ static int rfc4106_setauthsize(struct crypto_aead *authenc,
 
 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 *key_jump_cmd;
 	u32 *read_move_cmd, *write_move_cmd;
 	u32 *desc;
-	u32 geniv;
 
 	if (!ctx->enckeylen || !ctx->authsize)
 		return 0;
@@ -1266,7 +1115,7 @@ static int rfc4543_set_sh_desc(struct crypto_aead *aead)
 	 * Job Descriptor and Shared Descriptor
 	 * must fit into the 64-word Descriptor h/w Buffer
 	 */
-	if (DESC_RFC4543_ENC_LEN + DESC_JOB_IO_LEN +
+	if (DESC_RFC4543_ENC_LEN + GCM_DESC_JOB_IO_LEN +
 	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
@@ -1289,48 +1138,8 @@ static int rfc4543_set_sh_desc(struct crypto_aead *aead)
 	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);
+	/* assoclen + cryptlen = seqinlen */
+	append_math_sub(desc, REG3, SEQINLEN, REG0, CAAM_CMD_SZ);
 
 	/*
 	 * MOVE_LEN opcode is not available in all SEC HW revisions,
@@ -1342,16 +1151,13 @@ static int rfc4543_set_sh_desc(struct crypto_aead *aead)
 	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 */
+	/* Will read assoclen + cryptlen bytes */
+	append_math_sub(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
 
-	/* Read and write cryptlen bytes */
+	/* Will write assoclen + cryptlen bytes */
+	append_math_sub(desc, VARSEQOUTLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+
+	/* Read and write assoclen + cryptlen bytes */
 	aead_append_src_dst(desc, FIFOLD_TYPE_AAD);
 
 	set_move_tgt_here(desc, read_move_cmd);
@@ -1382,7 +1188,7 @@ static int rfc4543_set_sh_desc(struct crypto_aead *aead)
 	 * must all fit into the 64-word Descriptor h/w Buffer
 	 */
 	keys_fit_inline = false;
-	if (DESC_RFC4543_DEC_LEN + DESC_JOB_IO_LEN +
+	if (DESC_RFC4543_DEC_LEN + GCM_DESC_JOB_IO_LEN +
 	    ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
 		keys_fit_inline = true;
 
@@ -1405,28 +1211,8 @@ static int rfc4543_set_sh_desc(struct crypto_aead *aead)
 	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);
+	/* assoclen + cryptlen = seqoutlen */
+	append_math_sub(desc, REG3, SEQOUTLEN, REG0, CAAM_CMD_SZ);
 
 	/*
 	 * MOVE_LEN opcode is not available in all SEC HW revisions,
@@ -1438,40 +1224,16 @@ static int rfc4543_set_sh_desc(struct crypto_aead *aead)
 	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 read assoclen + cryptlen bytes */
+	append_math_sub(desc, VARSEQINLEN, SEQOUTLEN, REG0, 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 */
+	/* Will write assoclen + cryptlen bytes */
+	append_math_sub(desc, VARSEQOUTLEN, SEQOUTLEN, REG0, CAAM_CMD_SZ);
 
 	/* Store payload data */
 	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | FIFOLDST_VLF);
 
-	/* In-snoop cryptlen data */
+	/* In-snoop assoclen + cryptlen data */
 	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_BOTH | FIFOLDST_VLF |
 			     FIFOLD_TYPE_AAD | FIFOLD_TYPE_LAST2FLUSH1);
 
@@ -1499,135 +1261,6 @@ static int rfc4543_set_sh_desc(struct crypto_aead *aead)
 		       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;
 }
 
@@ -2100,7 +1733,7 @@ struct aead_edesc {
 	int sec4_sg_bytes;
 	dma_addr_t sec4_sg_dma;
 	struct sec4_sg_entry *sec4_sg;
-	u32 hw_desc[0];
+	u32 hw_desc[];
 };
 
 /*
@@ -2154,6 +1787,16 @@ static void aead_unmap(struct device *dev,
 		       struct aead_edesc *edesc,
 		       struct aead_request *req)
 {
+	caam_unmap(dev, req->src, req->dst,
+		   edesc->src_nents, edesc->src_chained, edesc->dst_nents,
+		   edesc->dst_chained, 0, 0,
+		   edesc->sec4_sg_dma, edesc->sec4_sg_bytes);
+}
+
+static void old_aead_unmap(struct device *dev,
+			   struct aead_edesc *edesc,
+			   struct aead_request *req)
+{
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	int ivsize = crypto_aead_ivsize(aead);
 
@@ -2184,6 +1827,28 @@ static void aead_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
 {
 	struct aead_request *req = context;
 	struct aead_edesc *edesc;
+
+#ifdef DEBUG
+	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	edesc = container_of(desc, struct aead_edesc, hw_desc[0]);
+
+	if (err)
+		caam_jr_strstatus(jrdev, err);
+
+	aead_unmap(jrdev, edesc, req);
+
+	kfree(edesc);
+
+	aead_request_complete(req, err);
+}
+
+static void old_aead_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
+				  void *context)
+{
+	struct aead_request *req = context;
+	struct aead_edesc *edesc;
 #ifdef DEBUG
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
@@ -2198,7 +1863,7 @@ static void aead_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
 	if (err)
 		caam_jr_strstatus(jrdev, err);
 
-	aead_unmap(jrdev, edesc, req);
+	old_aead_unmap(jrdev, edesc, req);
 
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "assoc  @"__stringify(__LINE__)": ",
@@ -2223,6 +1888,34 @@ static void aead_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 {
 	struct aead_request *req = context;
 	struct aead_edesc *edesc;
+
+#ifdef DEBUG
+	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	edesc = container_of(desc, struct aead_edesc, hw_desc[0]);
+
+	if (err)
+		caam_jr_strstatus(jrdev, err);
+
+	aead_unmap(jrdev, edesc, req);
+
+	/*
+	 * verify hw auth check passed else return -EBADMSG
+	 */
+	if ((err & JRSTA_CCBERR_ERRID_MASK) == JRSTA_CCBERR_ERRID_ICVCHK)
+		err = -EBADMSG;
+
+	kfree(edesc);
+
+	aead_request_complete(req, err);
+}
+
+static void old_aead_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
+				  void *context)
+{
+	struct aead_request *req = context;
+	struct aead_edesc *edesc;
 #ifdef DEBUG
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
@@ -2246,7 +1939,7 @@ static void aead_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 	if (err)
 		caam_jr_strstatus(jrdev, err);
 
-	aead_unmap(jrdev, edesc, req);
+	old_aead_unmap(jrdev, edesc, req);
 
 	/*
 	 * verify hw auth check passed else return -EBADMSG
@@ -2342,10 +2035,10 @@ static void ablkcipher_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 /*
  * Fill in aead job descriptor
  */
-static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
-			  struct aead_edesc *edesc,
-			  struct aead_request *req,
-			  bool all_contig, bool encrypt)
+static void old_init_aead_job(u32 *sh_desc, dma_addr_t ptr,
+			      struct aead_edesc *edesc,
+			      struct aead_request *req,
+			      bool all_contig, bool encrypt)
 {
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
@@ -2425,6 +2118,97 @@ static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
 }
 
 /*
+ * Fill in aead job descriptor
+ */
+static void init_aead_job(struct aead_request *req,
+			  struct aead_edesc *edesc,
+			  bool all_contig, bool encrypt)
+{
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	int authsize = ctx->authsize;
+	u32 *desc = edesc->hw_desc;
+	u32 out_options, in_options;
+	dma_addr_t dst_dma, src_dma;
+	int len, sec4_sg_index = 0;
+	dma_addr_t ptr;
+	u32 *sh_desc;
+
+	sh_desc = encrypt ? ctx->sh_desc_enc : ctx->sh_desc_dec;
+	ptr = encrypt ? ctx->sh_desc_enc_dma : ctx->sh_desc_dec_dma;
+
+	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->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->assoclen + req->cryptlen,
+			  in_options);
+
+	dst_dma = src_dma;
+	out_options = in_options;
+
+	if (unlikely(req->src != req->dst)) {
+		if (!edesc->dst_nents) {
+			dst_dma = sg_dma_address(req->dst);
+		} else {
+			dst_dma = edesc->sec4_sg_dma +
+				  sec4_sg_index *
+				  sizeof(struct sec4_sg_entry);
+			out_options = LDST_SGF;
+		}
+	}
+
+	if (encrypt)
+		append_seq_out_ptr(desc, dst_dma,
+				   req->assoclen + req->cryptlen + authsize,
+				   out_options);
+	else
+		append_seq_out_ptr(desc, dst_dma,
+				   req->assoclen + req->cryptlen - authsize,
+				   out_options);
+
+	/* REG3 = assoclen */
+	append_math_add_imm_u32(desc, REG3, ZERO, IMM, req->assoclen);
+}
+
+static void init_gcm_job(struct aead_request *req,
+			 struct aead_edesc *edesc,
+			 bool all_contig, bool encrypt)
+{
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	unsigned int ivsize = crypto_aead_ivsize(aead);
+	u32 *desc = edesc->hw_desc;
+	bool generic_gcm = (ivsize == 12);
+	unsigned int last;
+
+	init_aead_job(req, edesc, all_contig, encrypt);
+
+	/* BUG This should not be specific to generic GCM. */
+	last = 0;
+	if (encrypt && generic_gcm && !(req->assoclen + req->cryptlen))
+		last = FIFOLD_TYPE_LAST1;
+
+	/* Read GCM IV */
+	append_cmd(desc, CMD_FIFO_LOAD | FIFOLD_CLASS_CLASS1 | IMMEDIATE |
+			 FIFOLD_TYPE_IV | FIFOLD_TYPE_FLUSH1 | 12 | last);
+	/* Append Salt */
+	if (!generic_gcm)
+		append_data(desc, ctx->key + ctx->enckeylen, 4);
+	/* Append IV */
+	append_data(desc, req->iv, ivsize);
+	/* End of blank commands */
+}
+
+/*
  * Fill in aead givencrypt job descriptor
  */
 static void init_aead_giv_job(u32 *sh_desc, dma_addr_t ptr,
@@ -2608,9 +2392,10 @@ static void init_ablkcipher_giv_job(u32 *sh_desc, dma_addr_t ptr,
 /*
  * allocate and map the aead extended descriptor
  */
-static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
-					   int desc_bytes, bool *all_contig_ptr,
-					   bool encrypt)
+static struct aead_edesc *old_aead_edesc_alloc(struct aead_request *req,
+					       int desc_bytes,
+					       bool *all_contig_ptr,
+					       bool encrypt)
 {
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
@@ -2713,10 +2498,8 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 	sec4_sg_index = 0;
 	if (!all_contig) {
 		if (!is_gcm) {
-			sg_to_sec4_sg(req->assoc,
-				      assoc_nents,
-				      edesc->sec4_sg +
-				      sec4_sg_index, 0);
+			sg_to_sec4_sg_len(req->assoc, req->assoclen,
+					  edesc->sec4_sg + sec4_sg_index);
 			sec4_sg_index += assoc_nents;
 		}
 
@@ -2725,10 +2508,8 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 		sec4_sg_index += 1;
 
 		if (is_gcm) {
-			sg_to_sec4_sg(req->assoc,
-				      assoc_nents,
-				      edesc->sec4_sg +
-				      sec4_sg_index, 0);
+			sg_to_sec4_sg_len(req->assoc, req->assoclen,
+					  edesc->sec4_sg + sec4_sg_index);
 			sec4_sg_index += assoc_nents;
 		}
 
@@ -2752,7 +2533,124 @@ static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
 	return edesc;
 }
 
-static int aead_encrypt(struct aead_request *req)
+/*
+ * allocate and map the aead extended descriptor
+ */
+static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
+					   int desc_bytes, bool *all_contig_ptr,
+					   bool encrypt)
+{
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	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;
+	struct aead_edesc *edesc;
+	int sgc;
+	bool all_contig = true;
+	bool src_chained = false, dst_chained = false;
+	int sec4_sg_index, sec4_sg_len = 0, sec4_sg_bytes;
+	unsigned int authsize = ctx->authsize;
+
+	if (unlikely(req->dst != req->src)) {
+		src_nents = sg_count(req->src, req->assoclen + req->cryptlen,
+				     &src_chained);
+		dst_nents = sg_count(req->dst,
+				     req->assoclen + req->cryptlen +
+					(encrypt ? authsize : (-authsize)),
+				     &dst_chained);
+	} else {
+		src_nents = sg_count(req->src,
+				     req->assoclen + req->cryptlen +
+					(encrypt ? authsize : 0),
+				     &src_chained);
+	}
+
+	/* Check if data are contiguous. */
+	all_contig = !src_nents;
+	if (!all_contig) {
+		src_nents = src_nents ? : 1;
+		sec4_sg_len = src_nents;
+	}
+
+	sec4_sg_len += dst_nents;
+
+	sec4_sg_bytes = sec4_sg_len * sizeof(struct sec4_sg_entry);
+
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kzalloc(sizeof(struct aead_edesc) + desc_bytes +
+			sec4_sg_bytes, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	if (likely(req->src == req->dst)) {
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_BIDIRECTIONAL, src_chained);
+		if (unlikely(!sgc)) {
+			dev_err(jrdev, "unable to map source\n");
+			kfree(edesc);
+			return ERR_PTR(-ENOMEM);
+		}
+	} else {
+		sgc = dma_map_sg_chained(jrdev, req->src, src_nents ? : 1,
+					 DMA_TO_DEVICE, src_chained);
+		if (unlikely(!sgc)) {
+			dev_err(jrdev, "unable to map source\n");
+			kfree(edesc);
+			return ERR_PTR(-ENOMEM);
+		}
+
+		sgc = dma_map_sg_chained(jrdev, req->dst, dst_nents ? : 1,
+					 DMA_FROM_DEVICE, dst_chained);
+		if (unlikely(!sgc)) {
+			dev_err(jrdev, "unable to map destination\n");
+			dma_unmap_sg_chained(jrdev, req->src, src_nents ? : 1,
+					     DMA_TO_DEVICE, src_chained);
+			kfree(edesc);
+			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 = (void *)edesc + sizeof(struct aead_edesc) +
+			 desc_bytes;
+	*all_contig_ptr = all_contig;
+
+	sec4_sg_index = 0;
+	if (!all_contig) {
+		sg_to_sec4_sg_last(req->src, src_nents,
+			      edesc->sec4_sg + sec4_sg_index, 0);
+		sec4_sg_index += src_nents;
+	}
+	if (dst_nents) {
+		sg_to_sec4_sg_last(req->dst, dst_nents,
+				   edesc->sec4_sg + sec4_sg_index, 0);
+	}
+
+	if (!sec4_sg_bytes)
+		return edesc;
+
+	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");
+		aead_unmap(jrdev, edesc, req);
+		kfree(edesc);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	edesc->sec4_sg_bytes = sec4_sg_bytes;
+
+	return edesc;
+}
+
+static int gcm_encrypt(struct aead_request *req)
 {
 	struct aead_edesc *edesc;
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
@@ -2763,14 +2661,12 @@ static int aead_encrypt(struct aead_request *req)
 	int ret = 0;
 
 	/* allocate extended descriptor */
-	edesc = aead_edesc_alloc(req, DESC_JOB_IO_LEN *
-				 CAAM_CMD_SZ, &all_contig, true);
+	edesc = aead_edesc_alloc(req, GCM_DESC_JOB_IO_LEN, &all_contig, true);
 	if (IS_ERR(edesc))
 		return PTR_ERR(edesc);
 
 	/* Create and submit job descriptor */
-	init_aead_job(ctx->sh_desc_enc, ctx->sh_desc_enc_dma, edesc, req,
-		      all_contig, true);
+	init_gcm_job(req, edesc, all_contig, true);
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "aead jobdesc@"__stringify(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->hw_desc,
@@ -2789,7 +2685,7 @@ static int aead_encrypt(struct aead_request *req)
 	return ret;
 }
 
-static int aead_decrypt(struct aead_request *req)
+static int old_aead_encrypt(struct aead_request *req)
 {
 	struct aead_edesc *edesc;
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
@@ -2800,8 +2696,80 @@ static int aead_decrypt(struct aead_request *req)
 	int ret = 0;
 
 	/* allocate extended descriptor */
-	edesc = aead_edesc_alloc(req, DESC_JOB_IO_LEN *
-				 CAAM_CMD_SZ, &all_contig, false);
+	edesc = old_aead_edesc_alloc(req, DESC_JOB_IO_LEN *
+				     CAAM_CMD_SZ, &all_contig, true);
+	if (IS_ERR(edesc))
+		return PTR_ERR(edesc);
+
+	/* Create and submit job descriptor */
+	old_init_aead_job(ctx->sh_desc_enc, ctx->sh_desc_enc_dma, edesc, req,
+			  all_contig, true);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "aead 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, old_aead_encrypt_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		old_aead_unmap(jrdev, edesc, req);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+static int gcm_decrypt(struct aead_request *req)
+{
+	struct aead_edesc *edesc;
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	bool all_contig;
+	u32 *desc;
+	int ret = 0;
+
+	/* allocate extended descriptor */
+	edesc = aead_edesc_alloc(req, GCM_DESC_JOB_IO_LEN, &all_contig, false);
+	if (IS_ERR(edesc))
+		return PTR_ERR(edesc);
+
+	/* Create and submit job descriptor*/
+	init_gcm_job(req, edesc, all_contig, false);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "aead 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, aead_decrypt_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		aead_unmap(jrdev, edesc, req);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+static int old_aead_decrypt(struct aead_request *req)
+{
+	struct aead_edesc *edesc;
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	bool all_contig;
+	u32 *desc;
+	int ret = 0;
+
+	/* allocate extended descriptor */
+	edesc = old_aead_edesc_alloc(req, DESC_JOB_IO_LEN *
+				     CAAM_CMD_SZ, &all_contig, false);
 	if (IS_ERR(edesc))
 		return PTR_ERR(edesc);
 
@@ -2812,8 +2780,8 @@ static int aead_decrypt(struct aead_request *req)
 #endif
 
 	/* Create and submit job descriptor*/
-	init_aead_job(ctx->sh_desc_dec,
-		      ctx->sh_desc_dec_dma, edesc, req, all_contig, false);
+	old_init_aead_job(ctx->sh_desc_dec,
+			  ctx->sh_desc_dec_dma, edesc, req, all_contig, false);
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "aead jobdesc@"__stringify(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->hw_desc,
@@ -2821,11 +2789,11 @@ static int aead_decrypt(struct aead_request *req)
 #endif
 
 	desc = edesc->hw_desc;
-	ret = caam_jr_enqueue(jrdev, desc, aead_decrypt_done, req);
+	ret = caam_jr_enqueue(jrdev, desc, old_aead_decrypt_done, req);
 	if (!ret) {
 		ret = -EINPROGRESS;
 	} else {
-		aead_unmap(jrdev, edesc, req);
+		old_aead_unmap(jrdev, edesc, req);
 		kfree(edesc);
 	}
 
@@ -2953,8 +2921,8 @@ static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
 	sec4_sg_index = 0;
 	if (!(contig & GIV_SRC_CONTIG)) {
 		if (!is_gcm) {
-			sg_to_sec4_sg(req->assoc, assoc_nents,
-				      edesc->sec4_sg + sec4_sg_index, 0);
+			sg_to_sec4_sg_len(req->assoc, req->assoclen,
+					  edesc->sec4_sg + sec4_sg_index);
 			sec4_sg_index += assoc_nents;
 		}
 
@@ -2963,8 +2931,8 @@ static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
 		sec4_sg_index += 1;
 
 		if (is_gcm) {
-			sg_to_sec4_sg(req->assoc, assoc_nents,
-				      edesc->sec4_sg + sec4_sg_index, 0);
+			sg_to_sec4_sg_len(req->assoc, req->assoclen,
+					  edesc->sec4_sg + sec4_sg_index);
 			sec4_sg_index += assoc_nents;
 		}
 
@@ -2999,7 +2967,7 @@ static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
 	return edesc;
 }
 
-static int aead_givencrypt(struct aead_givcrypt_request *areq)
+static int old_aead_givencrypt(struct aead_givcrypt_request *areq)
 {
 	struct aead_request *req = &areq->areq;
 	struct aead_edesc *edesc;
@@ -3033,11 +3001,11 @@ static int aead_givencrypt(struct aead_givcrypt_request *areq)
 #endif
 
 	desc = edesc->hw_desc;
-	ret = caam_jr_enqueue(jrdev, desc, aead_encrypt_done, req);
+	ret = caam_jr_enqueue(jrdev, desc, old_aead_encrypt_done, req);
 	if (!ret) {
 		ret = -EINPROGRESS;
 	} else {
-		aead_unmap(jrdev, edesc, req);
+		old_aead_unmap(jrdev, edesc, req);
 		kfree(edesc);
 	}
 
@@ -3046,7 +3014,7 @@ static int aead_givencrypt(struct aead_givcrypt_request *areq)
 
 static int aead_null_givencrypt(struct aead_givcrypt_request *areq)
 {
-	return aead_encrypt(&areq->areq);
+	return old_aead_encrypt(&areq->areq);
 }
 
 /*
@@ -3379,11 +3347,7 @@ struct caam_alg_template {
 	u32 type;
 	union {
 		struct ablkcipher_alg ablkcipher;
-		struct aead_alg aead;
-		struct blkcipher_alg blkcipher;
-		struct cipher_alg cipher;
-		struct compress_alg compress;
-		struct rng_alg rng;
+		struct old_aead_alg aead;
 	} template_u;
 	u32 class1_alg_type;
 	u32 class2_alg_type;
@@ -3400,8 +3364,8 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
 			.givencrypt = aead_null_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = NULL_IV_SIZE,
@@ -3419,8 +3383,8 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
 			.givencrypt = aead_null_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = NULL_IV_SIZE,
@@ -3438,8 +3402,8 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
 			.givencrypt = aead_null_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = NULL_IV_SIZE,
@@ -3458,8 +3422,8 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
 			.givencrypt = aead_null_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = NULL_IV_SIZE,
@@ -3478,8 +3442,8 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
 			.givencrypt = aead_null_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = NULL_IV_SIZE,
@@ -3498,8 +3462,8 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
 			.givencrypt = aead_null_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = NULL_IV_SIZE,
@@ -3518,9 +3482,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = MD5_DIGEST_SIZE,
@@ -3537,9 +3501,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = SHA1_DIGEST_SIZE,
@@ -3556,9 +3520,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = SHA224_DIGEST_SIZE,
@@ -3576,9 +3540,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = SHA256_DIGEST_SIZE,
@@ -3596,9 +3560,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = SHA384_DIGEST_SIZE,
@@ -3617,9 +3581,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = SHA512_DIGEST_SIZE,
@@ -3637,9 +3601,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = MD5_DIGEST_SIZE,
@@ -3656,9 +3620,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = SHA1_DIGEST_SIZE,
@@ -3675,9 +3639,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = SHA224_DIGEST_SIZE,
@@ -3695,9 +3659,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = SHA256_DIGEST_SIZE,
@@ -3715,9 +3679,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = SHA384_DIGEST_SIZE,
@@ -3735,9 +3699,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = SHA512_DIGEST_SIZE,
@@ -3755,9 +3719,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = MD5_DIGEST_SIZE,
@@ -3774,9 +3738,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = SHA1_DIGEST_SIZE,
@@ -3793,9 +3757,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = SHA224_DIGEST_SIZE,
@@ -3813,9 +3777,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = SHA256_DIGEST_SIZE,
@@ -3833,9 +3797,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = SHA384_DIGEST_SIZE,
@@ -3853,9 +3817,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = SHA512_DIGEST_SIZE,
@@ -3873,9 +3837,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = CTR_RFC3686_IV_SIZE,
 			.maxauthsize = MD5_DIGEST_SIZE,
@@ -3892,9 +3856,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = CTR_RFC3686_IV_SIZE,
 			.maxauthsize = SHA1_DIGEST_SIZE,
@@ -3911,9 +3875,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = CTR_RFC3686_IV_SIZE,
 			.maxauthsize = SHA224_DIGEST_SIZE,
@@ -3931,9 +3895,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = CTR_RFC3686_IV_SIZE,
 			.maxauthsize = SHA256_DIGEST_SIZE,
@@ -3951,9 +3915,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = CTR_RFC3686_IV_SIZE,
 			.maxauthsize = SHA384_DIGEST_SIZE,
@@ -3971,9 +3935,9 @@ static struct caam_alg_template driver_algs[] = {
 		.template_aead = {
 			.setkey = aead_setkey,
 			.setauthsize = aead_setauthsize,
-			.encrypt = aead_encrypt,
-			.decrypt = aead_decrypt,
-			.givencrypt = aead_givencrypt,
+			.encrypt = old_aead_encrypt,
+			.decrypt = old_aead_decrypt,
+			.givencrypt = old_aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = CTR_RFC3686_IV_SIZE,
 			.maxauthsize = SHA512_DIGEST_SIZE,
@@ -3983,58 +3947,6 @@ static struct caam_alg_template driver_algs[] = {
 				   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)",
@@ -4124,21 +4036,84 @@ static struct caam_alg_template driver_algs[] = {
 	}
 };
 
-struct caam_crypto_alg {
-	struct list_head entry;
+struct caam_alg_entry {
 	int class1_alg_type;
 	int class2_alg_type;
 	int alg_op;
+};
+
+struct caam_aead_alg {
+	struct aead_alg aead;
+	struct caam_alg_entry caam;
+	bool registered;
+};
+
+static struct caam_aead_alg driver_aeads[] = {
+	{
+		.aead = {
+			.base = {
+				.cra_name = "rfc4106(gcm(aes))",
+				.cra_driver_name = "rfc4106-gcm-aes-caam",
+				.cra_blocksize = 1,
+			},
+			.setkey = rfc4106_setkey,
+			.setauthsize = rfc4106_setauthsize,
+			.encrypt = gcm_encrypt,
+			.decrypt = gcm_decrypt,
+			.ivsize = 8,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.caam = {
+			.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_GCM,
+		},
+	},
+	{
+		.aead = {
+			.base = {
+				.cra_name = "rfc4543(gcm(aes))",
+				.cra_driver_name = "rfc4543-gcm-aes-caam",
+				.cra_blocksize = 1,
+			},
+			.setkey = rfc4543_setkey,
+			.setauthsize = rfc4543_setauthsize,
+			.encrypt = gcm_encrypt,
+			.decrypt = gcm_decrypt,
+			.ivsize = 8,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.caam = {
+			.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_GCM,
+		},
+	},
+	/* Galois Counter Mode */
+	{
+		.aead = {
+			.base = {
+				.cra_name = "gcm(aes)",
+				.cra_driver_name = "gcm-aes-caam",
+				.cra_blocksize = 1,
+			},
+			.setkey = gcm_setkey,
+			.setauthsize = gcm_setauthsize,
+			.encrypt = gcm_encrypt,
+			.decrypt = gcm_decrypt,
+			.ivsize = 12,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.caam = {
+			.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_GCM,
+		},
+	},
+};
+
+struct caam_crypto_alg {
 	struct crypto_alg crypto_alg;
+	struct list_head entry;
+	struct caam_alg_entry caam;
 };
 
-static int caam_cra_init(struct crypto_tfm *tfm)
+static int caam_init_common(struct caam_ctx *ctx, struct caam_alg_entry *caam)
 {
-	struct crypto_alg *alg = tfm->__crt_alg;
-	struct caam_crypto_alg *caam_alg =
-		 container_of(alg, struct caam_crypto_alg, crypto_alg);
-	struct caam_ctx *ctx = crypto_tfm_ctx(tfm);
-
 	ctx->jrdev = caam_jr_alloc();
 	if (IS_ERR(ctx->jrdev)) {
 		pr_err("Job Ring Device allocation for transform failed\n");
@@ -4146,17 +4121,35 @@ static int caam_cra_init(struct crypto_tfm *tfm)
 	}
 
 	/* copy descriptor header template value */
-	ctx->class1_alg_type = OP_TYPE_CLASS1_ALG | caam_alg->class1_alg_type;
-	ctx->class2_alg_type = OP_TYPE_CLASS2_ALG | caam_alg->class2_alg_type;
-	ctx->alg_op = OP_TYPE_CLASS2_ALG | caam_alg->alg_op;
+	ctx->class1_alg_type = OP_TYPE_CLASS1_ALG | caam->class1_alg_type;
+	ctx->class2_alg_type = OP_TYPE_CLASS2_ALG | caam->class2_alg_type;
+	ctx->alg_op = OP_TYPE_CLASS2_ALG | caam->alg_op;
 
 	return 0;
 }
 
-static void caam_cra_exit(struct crypto_tfm *tfm)
+static int caam_cra_init(struct crypto_tfm *tfm)
 {
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct caam_crypto_alg *caam_alg =
+		 container_of(alg, struct caam_crypto_alg, crypto_alg);
 	struct caam_ctx *ctx = crypto_tfm_ctx(tfm);
 
+	return caam_init_common(ctx, &caam_alg->caam);
+}
+
+static int caam_aead_init(struct crypto_aead *tfm)
+{
+	struct aead_alg *alg = crypto_aead_alg(tfm);
+	struct caam_aead_alg *caam_alg =
+		 container_of(alg, struct caam_aead_alg, aead);
+	struct caam_ctx *ctx = crypto_aead_ctx(tfm);
+
+	return caam_init_common(ctx, &caam_alg->caam);
+}
+
+static void caam_exit_common(struct caam_ctx *ctx)
+{
 	if (ctx->sh_desc_enc_dma &&
 	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_enc_dma))
 		dma_unmap_single(ctx->jrdev, ctx->sh_desc_enc_dma,
@@ -4179,10 +4172,28 @@ static void caam_cra_exit(struct crypto_tfm *tfm)
 	caam_jr_free(ctx->jrdev);
 }
 
+static void caam_cra_exit(struct crypto_tfm *tfm)
+{
+	caam_exit_common(crypto_tfm_ctx(tfm));
+}
+
+static void caam_aead_exit(struct crypto_aead *tfm)
+{
+	caam_exit_common(crypto_aead_ctx(tfm));
+}
+
 static void __exit caam_algapi_exit(void)
 {
 
 	struct caam_crypto_alg *t_alg, *n;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(driver_aeads); i++) {
+		struct caam_aead_alg *t_alg = driver_aeads + i;
+
+		if (t_alg->registered)
+			crypto_unregister_aead(&t_alg->aead);
+	}
 
 	if (!alg_list.next)
 		return;
@@ -4235,13 +4246,26 @@ static struct caam_crypto_alg *caam_alg_alloc(struct caam_alg_template
 		break;
 	}
 
-	t_alg->class1_alg_type = template->class1_alg_type;
-	t_alg->class2_alg_type = template->class2_alg_type;
-	t_alg->alg_op = template->alg_op;
+	t_alg->caam.class1_alg_type = template->class1_alg_type;
+	t_alg->caam.class2_alg_type = template->class2_alg_type;
+	t_alg->caam.alg_op = template->alg_op;
 
 	return t_alg;
 }
 
+static void caam_aead_alg_init(struct caam_aead_alg *t_alg)
+{
+	struct aead_alg *alg = &t_alg->aead;
+
+	alg->base.cra_module = THIS_MODULE;
+	alg->base.cra_priority = CAAM_CRA_PRIORITY;
+	alg->base.cra_ctxsize = sizeof(struct caam_ctx);
+	alg->base.cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_KERN_DRIVER_ONLY;
+
+	alg->init = caam_aead_init;
+	alg->exit = caam_aead_exit;
+}
+
 static int __init caam_algapi_init(void)
 {
 	struct device_node *dev_node;
@@ -4249,6 +4273,7 @@ static int __init caam_algapi_init(void)
 	struct device *ctrldev;
 	void *priv;
 	int i = 0, err = 0;
+	bool registered = false;
 
 	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
 	if (!dev_node) {
@@ -4295,10 +4320,30 @@ static int __init caam_algapi_init(void)
 			pr_warn("%s alg registration failed\n",
 				t_alg->crypto_alg.cra_driver_name);
 			kfree(t_alg);
-		} else
-			list_add_tail(&t_alg->entry, &alg_list);
+			continue;
+		}
+
+		list_add_tail(&t_alg->entry, &alg_list);
+		registered = true;
 	}
-	if (!list_empty(&alg_list))
+
+	for (i = 0; i < ARRAY_SIZE(driver_aeads); i++) {
+		struct caam_aead_alg *t_alg = driver_aeads + i;
+
+		caam_aead_alg_init(t_alg);
+
+		err = crypto_register_aead(&t_alg->aead);
+		if (err) {
+			pr_warn("%s alg registration failed\n",
+				t_alg->aead.base.cra_driver_name);
+			continue;
+		}
+
+		t_alg->registered = true;
+		registered = true;
+	}
+
+	if (registered)
 		pr_info("caam algorithms registered in /proc/crypto\n");
 
 	return err;
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
index ba0532efd3ae..dae1e8099969 100644
--- a/drivers/crypto/caam/caamhash.c
+++ b/drivers/crypto/caam/caamhash.c
@@ -835,17 +835,17 @@ static int ahash_update_ctx(struct ahash_request *req)
 			src_map_to_sec4_sg(jrdev, req->src, src_nents,
 					   edesc->sec4_sg + sec4_sg_src_index,
 					   chained);
-			if (*next_buflen) {
+			if (*next_buflen)
 				scatterwalk_map_and_copy(next_buf, req->src,
 							 to_hash - *buflen,
 							 *next_buflen, 0);
-				state->current_buf = !state->current_buf;
-			}
 		} else {
 			(edesc->sec4_sg + sec4_sg_src_index - 1)->len |=
 							SEC4_SG_LEN_FIN;
 		}
 
+		state->current_buf = !state->current_buf;
+
 		sh_len = desc_len(sh_desc);
 		desc = edesc->hw_desc;
 		init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER |
@@ -1268,9 +1268,10 @@ static int ahash_update_no_ctx(struct ahash_request *req)
 			scatterwalk_map_and_copy(next_buf, req->src,
 						 to_hash - *buflen,
 						 *next_buflen, 0);
-			state->current_buf = !state->current_buf;
 		}
 
+		state->current_buf = !state->current_buf;
+
 		sh_len = desc_len(sh_desc);
 		desc = edesc->hw_desc;
 		init_job_desc_shared(desc, ptr, sh_len, HDR_SHARE_DEFER |
@@ -1544,6 +1545,8 @@ static int ahash_init(struct ahash_request *req)
 
 	state->current_buf = 0;
 	state->buf_dma = 0;
+	state->buflen_0 = 0;
+	state->buflen_1 = 0;
 
 	return 0;
 }
diff --git a/drivers/crypto/caam/caamrng.c b/drivers/crypto/caam/caamrng.c
index 26a544b505f1..5095337205b8 100644
--- a/drivers/crypto/caam/caamrng.c
+++ b/drivers/crypto/caam/caamrng.c
@@ -56,7 +56,7 @@
 
 /* Buffer, its dma address and lock */
 struct buf_data {
-	u8 buf[RN_BUF_SIZE];
+	u8 buf[RN_BUF_SIZE] ____cacheline_aligned;
 	dma_addr_t addr;
 	struct completion filled;
 	u32 hw_desc[DESC_JOB_O_LEN];
diff --git a/drivers/crypto/caam/compat.h b/drivers/crypto/caam/compat.h
index acd7743e2603..f57f395db33f 100644
--- a/drivers/crypto/caam/compat.h
+++ b/drivers/crypto/caam/compat.h
@@ -32,7 +32,7 @@
 #include <crypto/des.h>
 #include <crypto/sha.h>
 #include <crypto/md5.h>
-#include <crypto/aead.h>
+#include <crypto/internal/aead.h>
 #include <crypto/authenc.h>
 #include <crypto/scatterwalk.h>
 #include <crypto/internal/skcipher.h>
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index efba4ccd4fac..efacab7539ef 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -301,7 +301,7 @@ static int caam_remove(struct platform_device *pdev)
 #endif
 
 	/* Unmap controller region */
-	iounmap(&ctrl);
+	iounmap(ctrl);
 
 	return ret;
 }
@@ -496,7 +496,7 @@ static int caam_probe(struct platform_device *pdev)
 					sizeof(struct platform_device *) * rspec,
 					GFP_KERNEL);
 	if (ctrlpriv->jrpdev == NULL) {
-		iounmap(&ctrl);
+		iounmap(ctrl);
 		return -ENOMEM;
 	}
 
diff --git a/drivers/crypto/caam/regs.h b/drivers/crypto/caam/regs.h
index 378ddc17f60e..672c97489505 100644
--- a/drivers/crypto/caam/regs.h
+++ b/drivers/crypto/caam/regs.h
@@ -83,35 +83,35 @@
 #endif
 #endif
 
+/*
+ * The only users of these wr/rd_reg64 functions is the Job Ring (JR).
+ * The DMA address registers in the JR are a pair of 32-bit registers.
+ * The layout is:
+ *
+ *    base + 0x0000 : most-significant 32 bits
+ *    base + 0x0004 : least-significant 32 bits
+ *
+ * The 32-bit version of this core therefore has to write to base + 0x0004
+ * to set the 32-bit wide DMA address. This seems to be independent of the
+ * endianness of the written/read data.
+ */
+
 #ifndef CONFIG_64BIT
-#ifdef __BIG_ENDIAN
-static inline void wr_reg64(u64 __iomem *reg, u64 data)
-{
-	wr_reg32((u32 __iomem *)reg, (data & 0xffffffff00000000ull) >> 32);
-	wr_reg32((u32 __iomem *)reg + 1, data & 0x00000000ffffffffull);
-}
+#define REG64_MS32(reg) ((u32 __iomem *)(reg))
+#define REG64_LS32(reg) ((u32 __iomem *)(reg) + 1)
 
-static inline u64 rd_reg64(u64 __iomem *reg)
-{
-	return (((u64)rd_reg32((u32 __iomem *)reg)) << 32) |
-		((u64)rd_reg32((u32 __iomem *)reg + 1));
-}
-#else
-#ifdef __LITTLE_ENDIAN
 static inline void wr_reg64(u64 __iomem *reg, u64 data)
 {
-	wr_reg32((u32 __iomem *)reg + 1, (data & 0xffffffff00000000ull) >> 32);
-	wr_reg32((u32 __iomem *)reg, data & 0x00000000ffffffffull);
+	wr_reg32(REG64_MS32(reg), data >> 32);
+	wr_reg32(REG64_LS32(reg), data);
 }
 
 static inline u64 rd_reg64(u64 __iomem *reg)
 {
-	return (((u64)rd_reg32((u32 __iomem *)reg + 1)) << 32) |
-		((u64)rd_reg32((u32 __iomem *)reg));
+	return ((u64)rd_reg32(REG64_MS32(reg)) << 32 |
+		(u64)rd_reg32(REG64_LS32(reg)));
 }
 #endif
-#endif
-#endif
 
 /*
  * jr_outentry
diff --git a/drivers/crypto/caam/sg_sw_sec4.h b/drivers/crypto/caam/sg_sw_sec4.h
index 3b918218aa4c..b68b74cc7b77 100644
--- a/drivers/crypto/caam/sg_sw_sec4.h
+++ b/drivers/crypto/caam/sg_sw_sec4.h
@@ -55,6 +55,21 @@ static inline void sg_to_sec4_sg_last(struct scatterlist *sg, int sg_count,
 	sec4_sg_ptr->len |= SEC4_SG_LEN_FIN;
 }
 
+static inline struct sec4_sg_entry *sg_to_sec4_sg_len(
+	struct scatterlist *sg, unsigned int total,
+	struct sec4_sg_entry *sec4_sg_ptr)
+{
+	do {
+		unsigned int len = min(sg_dma_len(sg), total);
+
+		dma_to_sec4_sg_one(sec4_sg_ptr, sg_dma_address(sg), len, 0);
+		sec4_sg_ptr++;
+		sg = sg_next(sg);
+		total -= len;
+	} while (total);
+	return sec4_sg_ptr - 1;
+}
+
 /* count number of elements in scatterlist */
 static inline int __sg_count(struct scatterlist *sg_list, int nbytes,
 			     bool *chained)
@@ -85,34 +100,41 @@ static inline int sg_count(struct scatterlist *sg_list, int nbytes,
 	return sg_nents;
 }
 
-static int dma_map_sg_chained(struct device *dev, struct scatterlist *sg,
-			      unsigned int nents, enum dma_data_direction dir,
-			      bool chained)
+static inline void dma_unmap_sg_chained(
+	struct device *dev, struct scatterlist *sg, unsigned int nents,
+	enum dma_data_direction dir, bool chained)
 {
 	if (unlikely(chained)) {
 		int i;
 		for (i = 0; i < nents; i++) {
-			dma_map_sg(dev, sg, 1, dir);
+			dma_unmap_sg(dev, sg, 1, dir);
 			sg = sg_next(sg);
 		}
-	} else {
-		dma_map_sg(dev, sg, nents, dir);
+	} else if (nents) {
+		dma_unmap_sg(dev, sg, nents, dir);
 	}
-	return nents;
 }
 
-static int dma_unmap_sg_chained(struct device *dev, struct scatterlist *sg,
-				unsigned int nents, enum dma_data_direction dir,
-				bool chained)
+static inline int dma_map_sg_chained(
+	struct device *dev, struct scatterlist *sg, unsigned int nents,
+	enum dma_data_direction dir, bool chained)
 {
+	struct scatterlist *first = sg;
+
 	if (unlikely(chained)) {
 		int i;
 		for (i = 0; i < nents; i++) {
-			dma_unmap_sg(dev, sg, 1, dir);
+			if (!dma_map_sg(dev, sg, 1, dir)) {
+				dma_unmap_sg_chained(dev, first, i, dir,
+						     chained);
+				nents = 0;
+				break;
+			}
+
 			sg = sg_next(sg);
 		}
-	} else {
-		dma_unmap_sg(dev, sg, nents, dir);
-	}
+	} else
+		nents = dma_map_sg(dev, sg, nents, dir);
+
 	return nents;
 }
diff --git a/drivers/crypto/ccp/Kconfig b/drivers/crypto/ccp/Kconfig
index 7639ffc36c68..ae38f6b6cc10 100644
--- a/drivers/crypto/ccp/Kconfig
+++ b/drivers/crypto/ccp/Kconfig
@@ -13,7 +13,6 @@ config CRYPTO_DEV_CCP_CRYPTO
 	tristate "Encryption and hashing acceleration support"
 	depends on CRYPTO_DEV_CCP_DD
 	default m
-	select CRYPTO_ALGAPI
 	select CRYPTO_HASH
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AUTHENC
diff --git a/drivers/crypto/ccp/ccp-ops.c b/drivers/crypto/ccp/ccp-ops.c
index 71f2e3c89424..d09c6c4af4aa 100644
--- a/drivers/crypto/ccp/ccp-ops.c
+++ b/drivers/crypto/ccp/ccp-ops.c
@@ -52,8 +52,7 @@ struct ccp_dm_workarea {
 
 struct ccp_sg_workarea {
 	struct scatterlist *sg;
-	unsigned int nents;
-	unsigned int length;
+	int nents;
 
 	struct scatterlist *dma_sg;
 	struct device *dma_dev;
@@ -496,8 +495,10 @@ static int ccp_init_sg_workarea(struct ccp_sg_workarea *wa, struct device *dev,
 	if (!sg)
 		return 0;
 
-	wa->nents = sg_nents(sg);
-	wa->length = sg->length;
+	wa->nents = sg_nents_for_len(sg, len);
+	if (wa->nents < 0)
+		return wa->nents;
+
 	wa->bytes_left = len;
 	wa->sg_used = 0;
 
diff --git a/drivers/crypto/ccp/ccp-platform.c b/drivers/crypto/ccp/ccp-platform.c
index b1c20b2b5647..f2e6de361fd1 100644
--- a/drivers/crypto/ccp/ccp-platform.c
+++ b/drivers/crypto/ccp/ccp-platform.c
@@ -90,58 +90,6 @@ static struct resource *ccp_find_mmio_area(struct ccp_device *ccp)
 	return NULL;
 }
 
-#ifdef CONFIG_ACPI
-static int ccp_acpi_support(struct ccp_device *ccp)
-{
-	struct ccp_platform *ccp_platform = ccp->dev_specific;
-	struct acpi_device *adev = ACPI_COMPANION(ccp->dev);
-	acpi_handle handle;
-	acpi_status status;
-	unsigned long long data;
-	int cca;
-
-	/* Retrieve the device cache coherency value */
-	handle = adev->handle;
-	do {
-		status = acpi_evaluate_integer(handle, "_CCA", NULL, &data);
-		if (!ACPI_FAILURE(status)) {
-			cca = data;
-			break;
-		}
-	} while (!ACPI_FAILURE(status));
-
-	if (ACPI_FAILURE(status)) {
-		dev_err(ccp->dev, "error obtaining acpi coherency value\n");
-		return -EINVAL;
-	}
-
-	ccp_platform->coherent = !!cca;
-
-	return 0;
-}
-#else	/* CONFIG_ACPI */
-static int ccp_acpi_support(struct ccp_device *ccp)
-{
-	return -EINVAL;
-}
-#endif
-
-#ifdef CONFIG_OF
-static int ccp_of_support(struct ccp_device *ccp)
-{
-	struct ccp_platform *ccp_platform = ccp->dev_specific;
-
-	ccp_platform->coherent = of_dma_is_coherent(ccp->dev->of_node);
-
-	return 0;
-}
-#else
-static int ccp_of_support(struct ccp_device *ccp)
-{
-	return -EINVAL;
-}
-#endif
-
 static int ccp_platform_probe(struct platform_device *pdev)
 {
 	struct ccp_device *ccp;
@@ -174,21 +122,13 @@ static int ccp_platform_probe(struct platform_device *pdev)
 	}
 	ccp->io_regs = ccp->io_map;
 
-	if (!dev->dma_mask)
-		dev->dma_mask = &dev->coherent_dma_mask;
 	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(48));
 	if (ret) {
 		dev_err(dev, "dma_set_mask_and_coherent failed (%d)\n", ret);
 		goto e_err;
 	}
 
-	if (ccp_platform->use_acpi)
-		ret = ccp_acpi_support(ccp);
-	else
-		ret = ccp_of_support(ccp);
-	if (ret)
-		goto e_err;
-
+	ccp_platform->coherent = device_dma_is_coherent(ccp->dev);
 	if (ccp_platform->coherent)
 		ccp->axcache = CACHE_WB_NO_ALLOC;
 	else
diff --git a/drivers/crypto/ixp4xx_crypto.c b/drivers/crypto/ixp4xx_crypto.c
index 48f453555f1f..7ba495f75370 100644
--- a/drivers/crypto/ixp4xx_crypto.c
+++ b/drivers/crypto/ixp4xx_crypto.c
@@ -25,7 +25,7 @@
 #include <crypto/aes.h>
 #include <crypto/sha.h>
 #include <crypto/algapi.h>
-#include <crypto/aead.h>
+#include <crypto/internal/aead.h>
 #include <crypto/authenc.h>
 #include <crypto/scatterwalk.h>
 
@@ -575,7 +575,8 @@ static int init_tfm_ablk(struct crypto_tfm *tfm)
 
 static int init_tfm_aead(struct crypto_tfm *tfm)
 {
-	tfm->crt_aead.reqsize = sizeof(struct aead_ctx);
+	crypto_aead_set_reqsize(__crypto_aead_cast(tfm),
+				sizeof(struct aead_ctx));
 	return init_tfm(tfm);
 }
 
@@ -1096,7 +1097,7 @@ static int aead_setup(struct crypto_aead *tfm, unsigned int authsize)
 {
 	struct ixp_ctx *ctx = crypto_aead_ctx(tfm);
 	u32 *flags = &tfm->base.crt_flags;
-	unsigned digest_len = crypto_aead_alg(tfm)->maxauthsize;
+	unsigned digest_len = crypto_aead_maxauthsize(tfm);
 	int ret;
 
 	if (!ctx->enckey_len && !ctx->authkey_len)
@@ -1138,7 +1139,7 @@ out:
 
 static int aead_setauthsize(struct crypto_aead *tfm, unsigned int authsize)
 {
-	int max = crypto_aead_alg(tfm)->maxauthsize >> 2;
+	int max = crypto_aead_maxauthsize(tfm) >> 2;
 
 	if ((authsize>>2) < 1 || (authsize>>2) > max || (authsize & 3))
 		return -EINVAL;
diff --git a/drivers/crypto/marvell/Makefile b/drivers/crypto/marvell/Makefile
new file mode 100644
index 000000000000..0c12b13574dc
--- /dev/null
+++ b/drivers/crypto/marvell/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_CRYPTO_DEV_MARVELL_CESA) += marvell-cesa.o
+marvell-cesa-objs := cesa.o cipher.o hash.o tdma.o
diff --git a/drivers/crypto/marvell/cesa.c b/drivers/crypto/marvell/cesa.c
new file mode 100644
index 000000000000..1c6f98dd88f4
--- /dev/null
+++ b/drivers/crypto/marvell/cesa.c
@@ -0,0 +1,547 @@
+/*
+ * Support for Marvell's Cryptographic Engine and Security Accelerator (CESA)
+ * that can be found on the following platform: Orion, Kirkwood, Armada. This
+ * driver supports the TDMA engine on platforms on which it is available.
+ *
+ * Author: Boris Brezillon <boris.brezillon@free-electrons.com>
+ * Author: Arnaud Ebalard <arno@natisbad.org>
+ *
+ * This work is based on an initial version written by
+ * Sebastian Andrzej Siewior < sebastian at breakpoint dot cc >
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/genalloc.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kthread.h>
+#include <linux/mbus.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+
+#include "cesa.h"
+
+static int allhwsupport = !IS_ENABLED(CONFIG_CRYPTO_DEV_MV_CESA);
+module_param_named(allhwsupport, allhwsupport, int, 0444);
+MODULE_PARM_DESC(allhwsupport, "Enable support for all hardware (even it if overlaps with the mv_cesa driver)");
+
+struct mv_cesa_dev *cesa_dev;
+
+static void mv_cesa_dequeue_req_unlocked(struct mv_cesa_engine *engine)
+{
+	struct crypto_async_request *req, *backlog;
+	struct mv_cesa_ctx *ctx;
+
+	spin_lock_bh(&cesa_dev->lock);
+	backlog = crypto_get_backlog(&cesa_dev->queue);
+	req = crypto_dequeue_request(&cesa_dev->queue);
+	engine->req = req;
+	spin_unlock_bh(&cesa_dev->lock);
+
+	if (!req)
+		return;
+
+	if (backlog)
+		backlog->complete(backlog, -EINPROGRESS);
+
+	ctx = crypto_tfm_ctx(req->tfm);
+	ctx->ops->prepare(req, engine);
+	ctx->ops->step(req);
+}
+
+static irqreturn_t mv_cesa_int(int irq, void *priv)
+{
+	struct mv_cesa_engine *engine = priv;
+	struct crypto_async_request *req;
+	struct mv_cesa_ctx *ctx;
+	u32 status, mask;
+	irqreturn_t ret = IRQ_NONE;
+
+	while (true) {
+		int res;
+
+		mask = mv_cesa_get_int_mask(engine);
+		status = readl(engine->regs + CESA_SA_INT_STATUS);
+
+		if (!(status & mask))
+			break;
+
+		/*
+		 * TODO: avoid clearing the FPGA_INT_STATUS if this not
+		 * relevant on some platforms.
+		 */
+		writel(~status, engine->regs + CESA_SA_FPGA_INT_STATUS);
+		writel(~status, engine->regs + CESA_SA_INT_STATUS);
+
+		ret = IRQ_HANDLED;
+		spin_lock_bh(&engine->lock);
+		req = engine->req;
+		spin_unlock_bh(&engine->lock);
+		if (req) {
+			ctx = crypto_tfm_ctx(req->tfm);
+			res = ctx->ops->process(req, status & mask);
+			if (res != -EINPROGRESS) {
+				spin_lock_bh(&engine->lock);
+				engine->req = NULL;
+				mv_cesa_dequeue_req_unlocked(engine);
+				spin_unlock_bh(&engine->lock);
+				ctx->ops->cleanup(req);
+				local_bh_disable();
+				req->complete(req, res);
+				local_bh_enable();
+			} else {
+				ctx->ops->step(req);
+			}
+		}
+	}
+
+	return ret;
+}
+
+int mv_cesa_queue_req(struct crypto_async_request *req)
+{
+	int ret;
+	int i;
+
+	spin_lock_bh(&cesa_dev->lock);
+	ret = crypto_enqueue_request(&cesa_dev->queue, req);
+	spin_unlock_bh(&cesa_dev->lock);
+
+	if (ret != -EINPROGRESS)
+		return ret;
+
+	for (i = 0; i < cesa_dev->caps->nengines; i++) {
+		spin_lock_bh(&cesa_dev->engines[i].lock);
+		if (!cesa_dev->engines[i].req)
+			mv_cesa_dequeue_req_unlocked(&cesa_dev->engines[i]);
+		spin_unlock_bh(&cesa_dev->engines[i].lock);
+	}
+
+	return -EINPROGRESS;
+}
+
+static int mv_cesa_add_algs(struct mv_cesa_dev *cesa)
+{
+	int ret;
+	int i, j;
+
+	for (i = 0; i < cesa->caps->ncipher_algs; i++) {
+		ret = crypto_register_alg(cesa->caps->cipher_algs[i]);
+		if (ret)
+			goto err_unregister_crypto;
+	}
+
+	for (i = 0; i < cesa->caps->nahash_algs; i++) {
+		ret = crypto_register_ahash(cesa->caps->ahash_algs[i]);
+		if (ret)
+			goto err_unregister_ahash;
+	}
+
+	return 0;
+
+err_unregister_ahash:
+	for (j = 0; j < i; j++)
+		crypto_unregister_ahash(cesa->caps->ahash_algs[j]);
+	i = cesa->caps->ncipher_algs;
+
+err_unregister_crypto:
+	for (j = 0; j < i; j++)
+		crypto_unregister_alg(cesa->caps->cipher_algs[j]);
+
+	return ret;
+}
+
+static void mv_cesa_remove_algs(struct mv_cesa_dev *cesa)
+{
+	int i;
+
+	for (i = 0; i < cesa->caps->nahash_algs; i++)
+		crypto_unregister_ahash(cesa->caps->ahash_algs[i]);
+
+	for (i = 0; i < cesa->caps->ncipher_algs; i++)
+		crypto_unregister_alg(cesa->caps->cipher_algs[i]);
+}
+
+static struct crypto_alg *orion_cipher_algs[] = {
+	&mv_cesa_ecb_des_alg,
+	&mv_cesa_cbc_des_alg,
+	&mv_cesa_ecb_des3_ede_alg,
+	&mv_cesa_cbc_des3_ede_alg,
+	&mv_cesa_ecb_aes_alg,
+	&mv_cesa_cbc_aes_alg,
+};
+
+static struct ahash_alg *orion_ahash_algs[] = {
+	&mv_md5_alg,
+	&mv_sha1_alg,
+	&mv_ahmac_md5_alg,
+	&mv_ahmac_sha1_alg,
+};
+
+static struct crypto_alg *armada_370_cipher_algs[] = {
+	&mv_cesa_ecb_des_alg,
+	&mv_cesa_cbc_des_alg,
+	&mv_cesa_ecb_des3_ede_alg,
+	&mv_cesa_cbc_des3_ede_alg,
+	&mv_cesa_ecb_aes_alg,
+	&mv_cesa_cbc_aes_alg,
+};
+
+static struct ahash_alg *armada_370_ahash_algs[] = {
+	&mv_md5_alg,
+	&mv_sha1_alg,
+	&mv_sha256_alg,
+	&mv_ahmac_md5_alg,
+	&mv_ahmac_sha1_alg,
+	&mv_ahmac_sha256_alg,
+};
+
+static const struct mv_cesa_caps orion_caps = {
+	.nengines = 1,
+	.cipher_algs = orion_cipher_algs,
+	.ncipher_algs = ARRAY_SIZE(orion_cipher_algs),
+	.ahash_algs = orion_ahash_algs,
+	.nahash_algs = ARRAY_SIZE(orion_ahash_algs),
+	.has_tdma = false,
+};
+
+static const struct mv_cesa_caps kirkwood_caps = {
+	.nengines = 1,
+	.cipher_algs = orion_cipher_algs,
+	.ncipher_algs = ARRAY_SIZE(orion_cipher_algs),
+	.ahash_algs = orion_ahash_algs,
+	.nahash_algs = ARRAY_SIZE(orion_ahash_algs),
+	.has_tdma = true,
+};
+
+static const struct mv_cesa_caps armada_370_caps = {
+	.nengines = 1,
+	.cipher_algs = armada_370_cipher_algs,
+	.ncipher_algs = ARRAY_SIZE(armada_370_cipher_algs),
+	.ahash_algs = armada_370_ahash_algs,
+	.nahash_algs = ARRAY_SIZE(armada_370_ahash_algs),
+	.has_tdma = true,
+};
+
+static const struct mv_cesa_caps armada_xp_caps = {
+	.nengines = 2,
+	.cipher_algs = armada_370_cipher_algs,
+	.ncipher_algs = ARRAY_SIZE(armada_370_cipher_algs),
+	.ahash_algs = armada_370_ahash_algs,
+	.nahash_algs = ARRAY_SIZE(armada_370_ahash_algs),
+	.has_tdma = true,
+};
+
+static const struct of_device_id mv_cesa_of_match_table[] = {
+	{ .compatible = "marvell,orion-crypto", .data = &orion_caps },
+	{ .compatible = "marvell,kirkwood-crypto", .data = &kirkwood_caps },
+	{ .compatible = "marvell,dove-crypto", .data = &kirkwood_caps },
+	{ .compatible = "marvell,armada-370-crypto", .data = &armada_370_caps },
+	{ .compatible = "marvell,armada-xp-crypto", .data = &armada_xp_caps },
+	{ .compatible = "marvell,armada-375-crypto", .data = &armada_xp_caps },
+	{ .compatible = "marvell,armada-38x-crypto", .data = &armada_xp_caps },
+	{}
+};
+MODULE_DEVICE_TABLE(of, mv_cesa_of_match_table);
+
+static void
+mv_cesa_conf_mbus_windows(struct mv_cesa_engine *engine,
+			  const struct mbus_dram_target_info *dram)
+{
+	void __iomem *iobase = engine->regs;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		writel(0, iobase + CESA_TDMA_WINDOW_CTRL(i));
+		writel(0, iobase + CESA_TDMA_WINDOW_BASE(i));
+	}
+
+	for (i = 0; i < dram->num_cs; i++) {
+		const struct mbus_dram_window *cs = dram->cs + i;
+
+		writel(((cs->size - 1) & 0xffff0000) |
+		       (cs->mbus_attr << 8) |
+		       (dram->mbus_dram_target_id << 4) | 1,
+		       iobase + CESA_TDMA_WINDOW_CTRL(i));
+		writel(cs->base, iobase + CESA_TDMA_WINDOW_BASE(i));
+	}
+}
+
+static int mv_cesa_dev_dma_init(struct mv_cesa_dev *cesa)
+{
+	struct device *dev = cesa->dev;
+	struct mv_cesa_dev_dma *dma;
+
+	if (!cesa->caps->has_tdma)
+		return 0;
+
+	dma = devm_kzalloc(dev, sizeof(*dma), GFP_KERNEL);
+	if (!dma)
+		return -ENOMEM;
+
+	dma->tdma_desc_pool = dmam_pool_create("tdma_desc", dev,
+					sizeof(struct mv_cesa_tdma_desc),
+					16, 0);
+	if (!dma->tdma_desc_pool)
+		return -ENOMEM;
+
+	dma->op_pool = dmam_pool_create("cesa_op", dev,
+					sizeof(struct mv_cesa_op_ctx), 16, 0);
+	if (!dma->op_pool)
+		return -ENOMEM;
+
+	dma->cache_pool = dmam_pool_create("cesa_cache", dev,
+					   CESA_MAX_HASH_BLOCK_SIZE, 1, 0);
+	if (!dma->cache_pool)
+		return -ENOMEM;
+
+	dma->padding_pool = dmam_pool_create("cesa_padding", dev, 72, 1, 0);
+	if (!dma->cache_pool)
+		return -ENOMEM;
+
+	cesa->dma = dma;
+
+	return 0;
+}
+
+static int mv_cesa_get_sram(struct platform_device *pdev, int idx)
+{
+	struct mv_cesa_dev *cesa = platform_get_drvdata(pdev);
+	struct mv_cesa_engine *engine = &cesa->engines[idx];
+	const char *res_name = "sram";
+	struct resource *res;
+
+	engine->pool = of_gen_pool_get(cesa->dev->of_node,
+				       "marvell,crypto-srams", idx);
+	if (engine->pool) {
+		engine->sram = gen_pool_dma_alloc(engine->pool,
+						  cesa->sram_size,
+						  &engine->sram_dma);
+		if (engine->sram)
+			return 0;
+
+		engine->pool = NULL;
+		return -ENOMEM;
+	}
+
+	if (cesa->caps->nengines > 1) {
+		if (!idx)
+			res_name = "sram0";
+		else
+			res_name = "sram1";
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   res_name);
+	if (!res || resource_size(res) < cesa->sram_size)
+		return -EINVAL;
+
+	engine->sram = devm_ioremap_resource(cesa->dev, res);
+	if (IS_ERR(engine->sram))
+		return PTR_ERR(engine->sram);
+
+	engine->sram_dma = phys_to_dma(cesa->dev,
+				       (phys_addr_t)res->start);
+
+	return 0;
+}
+
+static void mv_cesa_put_sram(struct platform_device *pdev, int idx)
+{
+	struct mv_cesa_dev *cesa = platform_get_drvdata(pdev);
+	struct mv_cesa_engine *engine = &cesa->engines[idx];
+
+	if (!engine->pool)
+		return;
+
+	gen_pool_free(engine->pool, (unsigned long)engine->sram,
+		      cesa->sram_size);
+}
+
+static int mv_cesa_probe(struct platform_device *pdev)
+{
+	const struct mv_cesa_caps *caps = &orion_caps;
+	const struct mbus_dram_target_info *dram;
+	const struct of_device_id *match;
+	struct device *dev = &pdev->dev;
+	struct mv_cesa_dev *cesa;
+	struct mv_cesa_engine *engines;
+	struct resource *res;
+	int irq, ret, i;
+	u32 sram_size;
+
+	if (cesa_dev) {
+		dev_err(&pdev->dev, "Only one CESA device authorized\n");
+		return -EEXIST;
+	}
+
+	if (dev->of_node) {
+		match = of_match_node(mv_cesa_of_match_table, dev->of_node);
+		if (!match || !match->data)
+			return -ENOTSUPP;
+
+		caps = match->data;
+	}
+
+	if ((caps == &orion_caps || caps == &kirkwood_caps) && !allhwsupport)
+		return -ENOTSUPP;
+
+	cesa = devm_kzalloc(dev, sizeof(*cesa), GFP_KERNEL);
+	if (!cesa)
+		return -ENOMEM;
+
+	cesa->caps = caps;
+	cesa->dev = dev;
+
+	sram_size = CESA_SA_DEFAULT_SRAM_SIZE;
+	of_property_read_u32(cesa->dev->of_node, "marvell,crypto-sram-size",
+			     &sram_size);
+	if (sram_size < CESA_SA_MIN_SRAM_SIZE)
+		sram_size = CESA_SA_MIN_SRAM_SIZE;
+
+	cesa->sram_size = sram_size;
+	cesa->engines = devm_kzalloc(dev, caps->nengines * sizeof(*engines),
+				     GFP_KERNEL);
+	if (!cesa->engines)
+		return -ENOMEM;
+
+	spin_lock_init(&cesa->lock);
+	crypto_init_queue(&cesa->queue, 50);
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "regs");
+	cesa->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(cesa->regs))
+		return -ENOMEM;
+
+	ret = mv_cesa_dev_dma_init(cesa);
+	if (ret)
+		return ret;
+
+	dram = mv_mbus_dram_info_nooverlap();
+
+	platform_set_drvdata(pdev, cesa);
+
+	for (i = 0; i < caps->nengines; i++) {
+		struct mv_cesa_engine *engine = &cesa->engines[i];
+		char res_name[7];
+
+		engine->id = i;
+		spin_lock_init(&engine->lock);
+
+		ret = mv_cesa_get_sram(pdev, i);
+		if (ret)
+			goto err_cleanup;
+
+		irq = platform_get_irq(pdev, i);
+		if (irq < 0) {
+			ret = irq;
+			goto err_cleanup;
+		}
+
+		/*
+		 * Not all platforms can gate the CESA clocks: do not complain
+		 * if the clock does not exist.
+		 */
+		snprintf(res_name, sizeof(res_name), "cesa%d", i);
+		engine->clk = devm_clk_get(dev, res_name);
+		if (IS_ERR(engine->clk)) {
+			engine->clk = devm_clk_get(dev, NULL);
+			if (IS_ERR(engine->clk))
+				engine->clk = NULL;
+		}
+
+		snprintf(res_name, sizeof(res_name), "cesaz%d", i);
+		engine->zclk = devm_clk_get(dev, res_name);
+		if (IS_ERR(engine->zclk))
+			engine->zclk = NULL;
+
+		ret = clk_prepare_enable(engine->clk);
+		if (ret)
+			goto err_cleanup;
+
+		ret = clk_prepare_enable(engine->zclk);
+		if (ret)
+			goto err_cleanup;
+
+		engine->regs = cesa->regs + CESA_ENGINE_OFF(i);
+
+		if (dram && cesa->caps->has_tdma)
+			mv_cesa_conf_mbus_windows(&cesa->engines[i], dram);
+
+		writel(0, cesa->engines[i].regs + CESA_SA_INT_STATUS);
+		writel(CESA_SA_CFG_STOP_DIG_ERR,
+		       cesa->engines[i].regs + CESA_SA_CFG);
+		writel(engine->sram_dma & CESA_SA_SRAM_MSK,
+		       cesa->engines[i].regs + CESA_SA_DESC_P0);
+
+		ret = devm_request_threaded_irq(dev, irq, NULL, mv_cesa_int,
+						IRQF_ONESHOT,
+						dev_name(&pdev->dev),
+						&cesa->engines[i]);
+		if (ret)
+			goto err_cleanup;
+	}
+
+	cesa_dev = cesa;
+
+	ret = mv_cesa_add_algs(cesa);
+	if (ret) {
+		cesa_dev = NULL;
+		goto err_cleanup;
+	}
+
+	dev_info(dev, "CESA device successfully registered\n");
+
+	return 0;
+
+err_cleanup:
+	for (i = 0; i < caps->nengines; i++) {
+		clk_disable_unprepare(cesa->engines[i].zclk);
+		clk_disable_unprepare(cesa->engines[i].clk);
+		mv_cesa_put_sram(pdev, i);
+	}
+
+	return ret;
+}
+
+static int mv_cesa_remove(struct platform_device *pdev)
+{
+	struct mv_cesa_dev *cesa = platform_get_drvdata(pdev);
+	int i;
+
+	mv_cesa_remove_algs(cesa);
+
+	for (i = 0; i < cesa->caps->nengines; i++) {
+		clk_disable_unprepare(cesa->engines[i].zclk);
+		clk_disable_unprepare(cesa->engines[i].clk);
+		mv_cesa_put_sram(pdev, i);
+	}
+
+	return 0;
+}
+
+static struct platform_driver marvell_cesa = {
+	.probe		= mv_cesa_probe,
+	.remove		= mv_cesa_remove,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "marvell-cesa",
+		.of_match_table = mv_cesa_of_match_table,
+	},
+};
+module_platform_driver(marvell_cesa);
+
+MODULE_ALIAS("platform:mv_crypto");
+MODULE_AUTHOR("Boris Brezillon <boris.brezillon@free-electrons.com>");
+MODULE_AUTHOR("Arnaud Ebalard <arno@natisbad.org>");
+MODULE_DESCRIPTION("Support for Marvell's cryptographic engine");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/crypto/marvell/cesa.h b/drivers/crypto/marvell/cesa.h
new file mode 100644
index 000000000000..b60698b30d30
--- /dev/null
+++ b/drivers/crypto/marvell/cesa.h
@@ -0,0 +1,791 @@
+#ifndef __MARVELL_CESA_H__
+#define __MARVELL_CESA_H__
+
+#include <crypto/algapi.h>
+#include <crypto/hash.h>
+#include <crypto/internal/hash.h>
+
+#include <linux/crypto.h>
+#include <linux/dmapool.h>
+
+#define CESA_ENGINE_OFF(i)			(((i) * 0x2000))
+
+#define CESA_TDMA_BYTE_CNT			0x800
+#define CESA_TDMA_SRC_ADDR			0x810
+#define CESA_TDMA_DST_ADDR			0x820
+#define CESA_TDMA_NEXT_ADDR			0x830
+
+#define CESA_TDMA_CONTROL			0x840
+#define CESA_TDMA_DST_BURST			GENMASK(2, 0)
+#define CESA_TDMA_DST_BURST_32B			3
+#define CESA_TDMA_DST_BURST_128B		4
+#define CESA_TDMA_OUT_RD_EN			BIT(4)
+#define CESA_TDMA_SRC_BURST			GENMASK(8, 6)
+#define CESA_TDMA_SRC_BURST_32B			(3 << 6)
+#define CESA_TDMA_SRC_BURST_128B		(4 << 6)
+#define CESA_TDMA_CHAIN				BIT(9)
+#define CESA_TDMA_BYTE_SWAP			BIT(11)
+#define CESA_TDMA_NO_BYTE_SWAP			BIT(11)
+#define CESA_TDMA_EN				BIT(12)
+#define CESA_TDMA_FETCH_ND			BIT(13)
+#define CESA_TDMA_ACT				BIT(14)
+
+#define CESA_TDMA_CUR				0x870
+#define CESA_TDMA_ERROR_CAUSE			0x8c8
+#define CESA_TDMA_ERROR_MSK			0x8cc
+
+#define CESA_TDMA_WINDOW_BASE(x)		(((x) * 0x8) + 0xa00)
+#define CESA_TDMA_WINDOW_CTRL(x)		(((x) * 0x8) + 0xa04)
+
+#define CESA_IVDIG(x)				(0xdd00 + ((x) * 4) +	\
+						 (((x) < 5) ? 0 : 0x14))
+
+#define CESA_SA_CMD				0xde00
+#define CESA_SA_CMD_EN_CESA_SA_ACCL0		BIT(0)
+#define CESA_SA_CMD_EN_CESA_SA_ACCL1		BIT(1)
+#define CESA_SA_CMD_DISABLE_SEC			BIT(2)
+
+#define CESA_SA_DESC_P0				0xde04
+
+#define CESA_SA_DESC_P1				0xde14
+
+#define CESA_SA_CFG				0xde08
+#define CESA_SA_CFG_STOP_DIG_ERR		GENMASK(1, 0)
+#define CESA_SA_CFG_DIG_ERR_CONT		0
+#define CESA_SA_CFG_DIG_ERR_SKIP		1
+#define CESA_SA_CFG_DIG_ERR_STOP		3
+#define CESA_SA_CFG_CH0_W_IDMA			BIT(7)
+#define CESA_SA_CFG_CH1_W_IDMA			BIT(8)
+#define CESA_SA_CFG_ACT_CH0_IDMA		BIT(9)
+#define CESA_SA_CFG_ACT_CH1_IDMA		BIT(10)
+#define CESA_SA_CFG_MULTI_PKT			BIT(11)
+#define CESA_SA_CFG_PARA_DIS			BIT(13)
+
+#define CESA_SA_ACCEL_STATUS			0xde0c
+#define CESA_SA_ST_ACT_0			BIT(0)
+#define CESA_SA_ST_ACT_1			BIT(1)
+
+/*
+ * CESA_SA_FPGA_INT_STATUS looks like a FPGA leftover and is documented only
+ * in Errata 4.12. It looks like that it was part of an IRQ-controller in FPGA
+ * and someone forgot to remove  it while switching to the core and moving to
+ * CESA_SA_INT_STATUS.
+ */
+#define CESA_SA_FPGA_INT_STATUS			0xdd68
+#define CESA_SA_INT_STATUS			0xde20
+#define CESA_SA_INT_AUTH_DONE			BIT(0)
+#define CESA_SA_INT_DES_E_DONE			BIT(1)
+#define CESA_SA_INT_AES_E_DONE			BIT(2)
+#define CESA_SA_INT_AES_D_DONE			BIT(3)
+#define CESA_SA_INT_ENC_DONE			BIT(4)
+#define CESA_SA_INT_ACCEL0_DONE			BIT(5)
+#define CESA_SA_INT_ACCEL1_DONE			BIT(6)
+#define CESA_SA_INT_ACC0_IDMA_DONE		BIT(7)
+#define CESA_SA_INT_ACC1_IDMA_DONE		BIT(8)
+#define CESA_SA_INT_IDMA_DONE			BIT(9)
+#define CESA_SA_INT_IDMA_OWN_ERR		BIT(10)
+
+#define CESA_SA_INT_MSK				0xde24
+
+#define CESA_SA_DESC_CFG_OP_MAC_ONLY		0
+#define CESA_SA_DESC_CFG_OP_CRYPT_ONLY		1
+#define CESA_SA_DESC_CFG_OP_MAC_CRYPT		2
+#define CESA_SA_DESC_CFG_OP_CRYPT_MAC		3
+#define CESA_SA_DESC_CFG_OP_MSK			GENMASK(1, 0)
+#define CESA_SA_DESC_CFG_MACM_SHA256		(1 << 4)
+#define CESA_SA_DESC_CFG_MACM_HMAC_SHA256	(3 << 4)
+#define CESA_SA_DESC_CFG_MACM_MD5		(4 << 4)
+#define CESA_SA_DESC_CFG_MACM_SHA1		(5 << 4)
+#define CESA_SA_DESC_CFG_MACM_HMAC_MD5		(6 << 4)
+#define CESA_SA_DESC_CFG_MACM_HMAC_SHA1		(7 << 4)
+#define CESA_SA_DESC_CFG_MACM_MSK		GENMASK(6, 4)
+#define CESA_SA_DESC_CFG_CRYPTM_DES		(1 << 8)
+#define CESA_SA_DESC_CFG_CRYPTM_3DES		(2 << 8)
+#define CESA_SA_DESC_CFG_CRYPTM_AES		(3 << 8)
+#define CESA_SA_DESC_CFG_CRYPTM_MSK		GENMASK(9, 8)
+#define CESA_SA_DESC_CFG_DIR_ENC		(0 << 12)
+#define CESA_SA_DESC_CFG_DIR_DEC		(1 << 12)
+#define CESA_SA_DESC_CFG_CRYPTCM_ECB		(0 << 16)
+#define CESA_SA_DESC_CFG_CRYPTCM_CBC		(1 << 16)
+#define CESA_SA_DESC_CFG_CRYPTCM_MSK		BIT(16)
+#define CESA_SA_DESC_CFG_3DES_EEE		(0 << 20)
+#define CESA_SA_DESC_CFG_3DES_EDE		(1 << 20)
+#define CESA_SA_DESC_CFG_AES_LEN_128		(0 << 24)
+#define CESA_SA_DESC_CFG_AES_LEN_192		(1 << 24)
+#define CESA_SA_DESC_CFG_AES_LEN_256		(2 << 24)
+#define CESA_SA_DESC_CFG_AES_LEN_MSK		GENMASK(25, 24)
+#define CESA_SA_DESC_CFG_NOT_FRAG		(0 << 30)
+#define CESA_SA_DESC_CFG_FIRST_FRAG		(1 << 30)
+#define CESA_SA_DESC_CFG_LAST_FRAG		(2 << 30)
+#define CESA_SA_DESC_CFG_MID_FRAG		(3 << 30)
+#define CESA_SA_DESC_CFG_FRAG_MSK		GENMASK(31, 30)
+
+/*
+ * /-----------\ 0
+ * | ACCEL CFG |	4 * 8
+ * |-----------| 0x20
+ * | CRYPT KEY |	8 * 4
+ * |-----------| 0x40
+ * |  IV   IN  |	4 * 4
+ * |-----------| 0x40 (inplace)
+ * |  IV BUF   |	4 * 4
+ * |-----------| 0x80
+ * |  DATA IN  |	16 * x (max ->max_req_size)
+ * |-----------| 0x80 (inplace operation)
+ * |  DATA OUT |	16 * x (max ->max_req_size)
+ * \-----------/ SRAM size
+ */
+
+/*
+ * Hashing memory map:
+ * /-----------\ 0
+ * | ACCEL CFG |        4 * 8
+ * |-----------| 0x20
+ * | Inner IV  |        8 * 4
+ * |-----------| 0x40
+ * | Outer IV  |        8 * 4
+ * |-----------| 0x60
+ * | Output BUF|        8 * 4
+ * |-----------| 0x80
+ * |  DATA IN  |        64 * x (max ->max_req_size)
+ * \-----------/ SRAM size
+ */
+
+#define CESA_SA_CFG_SRAM_OFFSET			0x00
+#define CESA_SA_DATA_SRAM_OFFSET		0x80
+
+#define CESA_SA_CRYPT_KEY_SRAM_OFFSET		0x20
+#define CESA_SA_CRYPT_IV_SRAM_OFFSET		0x40
+
+#define CESA_SA_MAC_IIV_SRAM_OFFSET		0x20
+#define CESA_SA_MAC_OIV_SRAM_OFFSET		0x40
+#define CESA_SA_MAC_DIG_SRAM_OFFSET		0x60
+
+#define CESA_SA_DESC_CRYPT_DATA(offset)					\
+	cpu_to_le32((CESA_SA_DATA_SRAM_OFFSET + (offset)) |		\
+		    ((CESA_SA_DATA_SRAM_OFFSET + (offset)) << 16))
+
+#define CESA_SA_DESC_CRYPT_IV(offset)					\
+	cpu_to_le32((CESA_SA_CRYPT_IV_SRAM_OFFSET + (offset)) |	\
+		    ((CESA_SA_CRYPT_IV_SRAM_OFFSET + (offset)) << 16))
+
+#define CESA_SA_DESC_CRYPT_KEY(offset)					\
+	cpu_to_le32(CESA_SA_CRYPT_KEY_SRAM_OFFSET + (offset))
+
+#define CESA_SA_DESC_MAC_DATA(offset)					\
+	cpu_to_le32(CESA_SA_DATA_SRAM_OFFSET + (offset))
+#define CESA_SA_DESC_MAC_DATA_MSK		GENMASK(15, 0)
+
+#define CESA_SA_DESC_MAC_TOTAL_LEN(total_len)	cpu_to_le32((total_len) << 16)
+#define CESA_SA_DESC_MAC_TOTAL_LEN_MSK		GENMASK(31, 16)
+
+#define CESA_SA_DESC_MAC_SRC_TOTAL_LEN_MAX	0xffff
+
+#define CESA_SA_DESC_MAC_DIGEST(offset)					\
+	cpu_to_le32(CESA_SA_MAC_DIG_SRAM_OFFSET + (offset))
+#define CESA_SA_DESC_MAC_DIGEST_MSK		GENMASK(15, 0)
+
+#define CESA_SA_DESC_MAC_FRAG_LEN(frag_len)	cpu_to_le32((frag_len) << 16)
+#define CESA_SA_DESC_MAC_FRAG_LEN_MSK		GENMASK(31, 16)
+
+#define CESA_SA_DESC_MAC_IV(offset)					\
+	cpu_to_le32((CESA_SA_MAC_IIV_SRAM_OFFSET + (offset)) |		\
+		    ((CESA_SA_MAC_OIV_SRAM_OFFSET + (offset)) << 16))
+
+#define CESA_SA_SRAM_SIZE			2048
+#define CESA_SA_SRAM_PAYLOAD_SIZE		(cesa_dev->sram_size - \
+						 CESA_SA_DATA_SRAM_OFFSET)
+
+#define CESA_SA_DEFAULT_SRAM_SIZE		2048
+#define CESA_SA_MIN_SRAM_SIZE			1024
+
+#define CESA_SA_SRAM_MSK			(2048 - 1)
+
+#define CESA_MAX_HASH_BLOCK_SIZE		64
+#define CESA_HASH_BLOCK_SIZE_MSK		(CESA_MAX_HASH_BLOCK_SIZE - 1)
+
+/**
+ * struct mv_cesa_sec_accel_desc - security accelerator descriptor
+ * @config:	engine config
+ * @enc_p:	input and output data pointers for a cipher operation
+ * @enc_len:	cipher operation length
+ * @enc_key_p:	cipher key pointer
+ * @enc_iv:	cipher IV pointers
+ * @mac_src_p:	input pointer and total hash length
+ * @mac_digest:	digest pointer and hash operation length
+ * @mac_iv:	hmac IV pointers
+ *
+ * Structure passed to the CESA engine to describe the crypto operation
+ * to be executed.
+ */
+struct mv_cesa_sec_accel_desc {
+	u32 config;
+	u32 enc_p;
+	u32 enc_len;
+	u32 enc_key_p;
+	u32 enc_iv;
+	u32 mac_src_p;
+	u32 mac_digest;
+	u32 mac_iv;
+};
+
+/**
+ * struct mv_cesa_blkcipher_op_ctx - cipher operation context
+ * @key:	cipher key
+ * @iv:		cipher IV
+ *
+ * Context associated to a cipher operation.
+ */
+struct mv_cesa_blkcipher_op_ctx {
+	u32 key[8];
+	u32 iv[4];
+};
+
+/**
+ * struct mv_cesa_hash_op_ctx - hash or hmac operation context
+ * @key:	cipher key
+ * @iv:		cipher IV
+ *
+ * Context associated to an hash or hmac operation.
+ */
+struct mv_cesa_hash_op_ctx {
+	u32 iv[16];
+	u32 hash[8];
+};
+
+/**
+ * struct mv_cesa_op_ctx - crypto operation context
+ * @desc:	CESA descriptor
+ * @ctx:	context associated to the crypto operation
+ *
+ * Context associated to a crypto operation.
+ */
+struct mv_cesa_op_ctx {
+	struct mv_cesa_sec_accel_desc desc;
+	union {
+		struct mv_cesa_blkcipher_op_ctx blkcipher;
+		struct mv_cesa_hash_op_ctx hash;
+	} ctx;
+};
+
+/* TDMA descriptor flags */
+#define CESA_TDMA_DST_IN_SRAM			BIT(31)
+#define CESA_TDMA_SRC_IN_SRAM			BIT(30)
+#define CESA_TDMA_TYPE_MSK			GENMASK(29, 0)
+#define CESA_TDMA_DUMMY				0
+#define CESA_TDMA_DATA				1
+#define CESA_TDMA_OP				2
+
+/**
+ * struct mv_cesa_tdma_desc - TDMA descriptor
+ * @byte_cnt:	number of bytes to transfer
+ * @src:	DMA address of the source
+ * @dst:	DMA address of the destination
+ * @next_dma:	DMA address of the next TDMA descriptor
+ * @cur_dma:	DMA address of this TDMA descriptor
+ * @next:	pointer to the next TDMA descriptor
+ * @op:		CESA operation attached to this TDMA descriptor
+ * @data:	raw data attached to this TDMA descriptor
+ * @flags:	flags describing the TDMA transfer. See the
+ *		"TDMA descriptor flags" section above
+ *
+ * TDMA descriptor used to create a transfer chain describing a crypto
+ * operation.
+ */
+struct mv_cesa_tdma_desc {
+	u32 byte_cnt;
+	u32 src;
+	u32 dst;
+	u32 next_dma;
+	u32 cur_dma;
+	struct mv_cesa_tdma_desc *next;
+	union {
+		struct mv_cesa_op_ctx *op;
+		void *data;
+	};
+	u32 flags;
+};
+
+/**
+ * struct mv_cesa_sg_dma_iter - scatter-gather iterator
+ * @dir:	transfer direction
+ * @sg:		scatter list
+ * @offset:	current position in the scatter list
+ * @op_offset:	current position in the crypto operation
+ *
+ * Iterator used to iterate over a scatterlist while creating a TDMA chain for
+ * a crypto operation.
+ */
+struct mv_cesa_sg_dma_iter {
+	enum dma_data_direction dir;
+	struct scatterlist *sg;
+	unsigned int offset;
+	unsigned int op_offset;
+};
+
+/**
+ * struct mv_cesa_dma_iter - crypto operation iterator
+ * @len:	the crypto operation length
+ * @offset:	current position in the crypto operation
+ * @op_len:	sub-operation length (the crypto engine can only act on 2kb
+ *		chunks)
+ *
+ * Iterator used to create a TDMA chain for a given crypto operation.
+ */
+struct mv_cesa_dma_iter {
+	unsigned int len;
+	unsigned int offset;
+	unsigned int op_len;
+};
+
+/**
+ * struct mv_cesa_tdma_chain - TDMA chain
+ * @first:	first entry in the TDMA chain
+ * @last:	last entry in the TDMA chain
+ *
+ * Stores a TDMA chain for a specific crypto operation.
+ */
+struct mv_cesa_tdma_chain {
+	struct mv_cesa_tdma_desc *first;
+	struct mv_cesa_tdma_desc *last;
+};
+
+struct mv_cesa_engine;
+
+/**
+ * struct mv_cesa_caps - CESA device capabilities
+ * @engines:		number of engines
+ * @has_tdma:		whether this device has a TDMA block
+ * @cipher_algs:	supported cipher algorithms
+ * @ncipher_algs:	number of supported cipher algorithms
+ * @ahash_algs:		supported hash algorithms
+ * @nahash_algs:	number of supported hash algorithms
+ *
+ * Structure used to describe CESA device capabilities.
+ */
+struct mv_cesa_caps {
+	int nengines;
+	bool has_tdma;
+	struct crypto_alg **cipher_algs;
+	int ncipher_algs;
+	struct ahash_alg **ahash_algs;
+	int nahash_algs;
+};
+
+/**
+ * struct mv_cesa_dev_dma - DMA pools
+ * @tdma_desc_pool:	TDMA desc pool
+ * @op_pool:		crypto operation pool
+ * @cache_pool:		data cache pool (used by hash implementation when the
+ *			hash request is smaller than the hash block size)
+ * @padding_pool:	padding pool (used by hash implementation when hardware
+ *			padding cannot be used)
+ *
+ * Structure containing the different DMA pools used by this driver.
+ */
+struct mv_cesa_dev_dma {
+	struct dma_pool *tdma_desc_pool;
+	struct dma_pool *op_pool;
+	struct dma_pool *cache_pool;
+	struct dma_pool *padding_pool;
+};
+
+/**
+ * struct mv_cesa_dev - CESA device
+ * @caps:	device capabilities
+ * @regs:	device registers
+ * @sram_size:	usable SRAM size
+ * @lock:	device lock
+ * @queue:	crypto request queue
+ * @engines:	array of engines
+ * @dma:	dma pools
+ *
+ * Structure storing CESA device information.
+ */
+struct mv_cesa_dev {
+	const struct mv_cesa_caps *caps;
+	void __iomem *regs;
+	struct device *dev;
+	unsigned int sram_size;
+	spinlock_t lock;
+	struct crypto_queue queue;
+	struct mv_cesa_engine *engines;
+	struct mv_cesa_dev_dma *dma;
+};
+
+/**
+ * struct mv_cesa_engine - CESA engine
+ * @id:			engine id
+ * @regs:		engine registers
+ * @sram:		SRAM memory region
+ * @sram_dma:		DMA address of the SRAM memory region
+ * @lock:		engine lock
+ * @req:		current crypto request
+ * @clk:		engine clk
+ * @zclk:		engine zclk
+ * @max_req_len:	maximum chunk length (useful to create the TDMA chain)
+ * @int_mask:		interrupt mask cache
+ * @pool:		memory pool pointing to the memory region reserved in
+ *			SRAM
+ *
+ * Structure storing CESA engine information.
+ */
+struct mv_cesa_engine {
+	int id;
+	void __iomem *regs;
+	void __iomem *sram;
+	dma_addr_t sram_dma;
+	spinlock_t lock;
+	struct crypto_async_request *req;
+	struct clk *clk;
+	struct clk *zclk;
+	size_t max_req_len;
+	u32 int_mask;
+	struct gen_pool *pool;
+};
+
+/**
+ * struct mv_cesa_req_ops - CESA request operations
+ * @prepare:	prepare a request to be executed on the specified engine
+ * @process:	process a request chunk result (should return 0 if the
+ *		operation, -EINPROGRESS if it needs more steps or an error
+ *		code)
+ * @step:	launch the crypto operation on the next chunk
+ * @cleanup:	cleanup the crypto request (release associated data)
+ */
+struct mv_cesa_req_ops {
+	void (*prepare)(struct crypto_async_request *req,
+			struct mv_cesa_engine *engine);
+	int (*process)(struct crypto_async_request *req, u32 status);
+	void (*step)(struct crypto_async_request *req);
+	void (*cleanup)(struct crypto_async_request *req);
+};
+
+/**
+ * struct mv_cesa_ctx - CESA operation context
+ * @ops:	crypto operations
+ *
+ * Base context structure inherited by operation specific ones.
+ */
+struct mv_cesa_ctx {
+	const struct mv_cesa_req_ops *ops;
+};
+
+/**
+ * struct mv_cesa_hash_ctx - CESA hash operation context
+ * @base:	base context structure
+ *
+ * Hash context structure.
+ */
+struct mv_cesa_hash_ctx {
+	struct mv_cesa_ctx base;
+};
+
+/**
+ * struct mv_cesa_hash_ctx - CESA hmac operation context
+ * @base:	base context structure
+ * @iv:		initialization vectors
+ *
+ * HMAC context structure.
+ */
+struct mv_cesa_hmac_ctx {
+	struct mv_cesa_ctx base;
+	u32 iv[16];
+};
+
+/**
+ * enum mv_cesa_req_type - request type definitions
+ * @CESA_STD_REQ:	standard request
+ * @CESA_DMA_REQ:	DMA request
+ */
+enum mv_cesa_req_type {
+	CESA_STD_REQ,
+	CESA_DMA_REQ,
+};
+
+/**
+ * struct mv_cesa_req - CESA request
+ * @type:	request type
+ * @engine:	engine associated with this request
+ */
+struct mv_cesa_req {
+	enum mv_cesa_req_type type;
+	struct mv_cesa_engine *engine;
+};
+
+/**
+ * struct mv_cesa_tdma_req - CESA TDMA request
+ * @base:	base information
+ * @chain:	TDMA chain
+ */
+struct mv_cesa_tdma_req {
+	struct mv_cesa_req base;
+	struct mv_cesa_tdma_chain chain;
+};
+
+/**
+ * struct mv_cesa_sg_std_iter - CESA scatter-gather iterator for standard
+ *				requests
+ * @iter:	sg mapping iterator
+ * @offset:	current offset in the SG entry mapped in memory
+ */
+struct mv_cesa_sg_std_iter {
+	struct sg_mapping_iter iter;
+	unsigned int offset;
+};
+
+/**
+ * struct mv_cesa_ablkcipher_std_req - cipher standard request
+ * @base:	base information
+ * @op:		operation context
+ * @offset:	current operation offset
+ * @size:	size of the crypto operation
+ */
+struct mv_cesa_ablkcipher_std_req {
+	struct mv_cesa_req base;
+	struct mv_cesa_op_ctx op;
+	unsigned int offset;
+	unsigned int size;
+	bool skip_ctx;
+};
+
+/**
+ * struct mv_cesa_ablkcipher_req - cipher request
+ * @req:	type specific request information
+ * @src_nents:	number of entries in the src sg list
+ * @dst_nents:	number of entries in the dest sg list
+ */
+struct mv_cesa_ablkcipher_req {
+	union {
+		struct mv_cesa_req base;
+		struct mv_cesa_tdma_req dma;
+		struct mv_cesa_ablkcipher_std_req std;
+	} req;
+	int src_nents;
+	int dst_nents;
+};
+
+/**
+ * struct mv_cesa_ahash_std_req - standard hash request
+ * @base:	base information
+ * @offset:	current operation offset
+ */
+struct mv_cesa_ahash_std_req {
+	struct mv_cesa_req base;
+	unsigned int offset;
+};
+
+/**
+ * struct mv_cesa_ahash_dma_req - DMA hash request
+ * @base:		base information
+ * @padding:		padding buffer
+ * @padding_dma:	DMA address of the padding buffer
+ * @cache_dma:		DMA address of the cache buffer
+ */
+struct mv_cesa_ahash_dma_req {
+	struct mv_cesa_tdma_req base;
+	u8 *padding;
+	dma_addr_t padding_dma;
+	dma_addr_t cache_dma;
+};
+
+/**
+ * struct mv_cesa_ahash_req - hash request
+ * @req:		type specific request information
+ * @cache:		cache buffer
+ * @cache_ptr:		write pointer in the cache buffer
+ * @len:		hash total length
+ * @src_nents:		number of entries in the scatterlist
+ * @last_req:		define whether the current operation is the last one
+ *			or not
+ * @state:		hash state
+ */
+struct mv_cesa_ahash_req {
+	union {
+		struct mv_cesa_req base;
+		struct mv_cesa_ahash_dma_req dma;
+		struct mv_cesa_ahash_std_req std;
+	} req;
+	struct mv_cesa_op_ctx op_tmpl;
+	u8 *cache;
+	unsigned int cache_ptr;
+	u64 len;
+	int src_nents;
+	bool last_req;
+	__be32 state[8];
+};
+
+/* CESA functions */
+
+extern struct mv_cesa_dev *cesa_dev;
+
+static inline void mv_cesa_update_op_cfg(struct mv_cesa_op_ctx *op,
+					 u32 cfg, u32 mask)
+{
+	op->desc.config &= cpu_to_le32(~mask);
+	op->desc.config |= cpu_to_le32(cfg);
+}
+
+static inline u32 mv_cesa_get_op_cfg(struct mv_cesa_op_ctx *op)
+{
+	return le32_to_cpu(op->desc.config);
+}
+
+static inline void mv_cesa_set_op_cfg(struct mv_cesa_op_ctx *op, u32 cfg)
+{
+	op->desc.config = cpu_to_le32(cfg);
+}
+
+static inline void mv_cesa_adjust_op(struct mv_cesa_engine *engine,
+				     struct mv_cesa_op_ctx *op)
+{
+	u32 offset = engine->sram_dma & CESA_SA_SRAM_MSK;
+
+	op->desc.enc_p = CESA_SA_DESC_CRYPT_DATA(offset);
+	op->desc.enc_key_p = CESA_SA_DESC_CRYPT_KEY(offset);
+	op->desc.enc_iv = CESA_SA_DESC_CRYPT_IV(offset);
+	op->desc.mac_src_p &= ~CESA_SA_DESC_MAC_DATA_MSK;
+	op->desc.mac_src_p |= CESA_SA_DESC_MAC_DATA(offset);
+	op->desc.mac_digest &= ~CESA_SA_DESC_MAC_DIGEST_MSK;
+	op->desc.mac_digest |= CESA_SA_DESC_MAC_DIGEST(offset);
+	op->desc.mac_iv = CESA_SA_DESC_MAC_IV(offset);
+}
+
+static inline void mv_cesa_set_crypt_op_len(struct mv_cesa_op_ctx *op, int len)
+{
+	op->desc.enc_len = cpu_to_le32(len);
+}
+
+static inline void mv_cesa_set_mac_op_total_len(struct mv_cesa_op_ctx *op,
+						int len)
+{
+	op->desc.mac_src_p &= ~CESA_SA_DESC_MAC_TOTAL_LEN_MSK;
+	op->desc.mac_src_p |= CESA_SA_DESC_MAC_TOTAL_LEN(len);
+}
+
+static inline void mv_cesa_set_mac_op_frag_len(struct mv_cesa_op_ctx *op,
+					       int len)
+{
+	op->desc.mac_digest &= ~CESA_SA_DESC_MAC_FRAG_LEN_MSK;
+	op->desc.mac_digest |= CESA_SA_DESC_MAC_FRAG_LEN(len);
+}
+
+static inline void mv_cesa_set_int_mask(struct mv_cesa_engine *engine,
+					u32 int_mask)
+{
+	if (int_mask == engine->int_mask)
+		return;
+
+	writel(int_mask, engine->regs + CESA_SA_INT_MSK);
+	engine->int_mask = int_mask;
+}
+
+static inline u32 mv_cesa_get_int_mask(struct mv_cesa_engine *engine)
+{
+	return engine->int_mask;
+}
+
+int mv_cesa_queue_req(struct crypto_async_request *req);
+
+/* TDMA functions */
+
+static inline void mv_cesa_req_dma_iter_init(struct mv_cesa_dma_iter *iter,
+					     unsigned int len)
+{
+	iter->len = len;
+	iter->op_len = min(len, CESA_SA_SRAM_PAYLOAD_SIZE);
+	iter->offset = 0;
+}
+
+static inline void mv_cesa_sg_dma_iter_init(struct mv_cesa_sg_dma_iter *iter,
+					    struct scatterlist *sg,
+					    enum dma_data_direction dir)
+{
+	iter->op_offset = 0;
+	iter->offset = 0;
+	iter->sg = sg;
+	iter->dir = dir;
+}
+
+static inline unsigned int
+mv_cesa_req_dma_iter_transfer_len(struct mv_cesa_dma_iter *iter,
+				  struct mv_cesa_sg_dma_iter *sgiter)
+{
+	return min(iter->op_len - sgiter->op_offset,
+		   sg_dma_len(sgiter->sg) - sgiter->offset);
+}
+
+bool mv_cesa_req_dma_iter_next_transfer(struct mv_cesa_dma_iter *chain,
+					struct mv_cesa_sg_dma_iter *sgiter,
+					unsigned int len);
+
+static inline bool mv_cesa_req_dma_iter_next_op(struct mv_cesa_dma_iter *iter)
+{
+	iter->offset += iter->op_len;
+	iter->op_len = min(iter->len - iter->offset,
+			   CESA_SA_SRAM_PAYLOAD_SIZE);
+
+	return iter->op_len;
+}
+
+void mv_cesa_dma_step(struct mv_cesa_tdma_req *dreq);
+
+static inline int mv_cesa_dma_process(struct mv_cesa_tdma_req *dreq,
+				      u32 status)
+{
+	if (!(status & CESA_SA_INT_ACC0_IDMA_DONE))
+		return -EINPROGRESS;
+
+	if (status & CESA_SA_INT_IDMA_OWN_ERR)
+		return -EINVAL;
+
+	return 0;
+}
+
+void mv_cesa_dma_prepare(struct mv_cesa_tdma_req *dreq,
+			 struct mv_cesa_engine *engine);
+
+void mv_cesa_dma_cleanup(struct mv_cesa_tdma_req *dreq);
+
+static inline void
+mv_cesa_tdma_desc_iter_init(struct mv_cesa_tdma_chain *chain)
+{
+	memset(chain, 0, sizeof(*chain));
+}
+
+struct mv_cesa_op_ctx *mv_cesa_dma_add_op(struct mv_cesa_tdma_chain *chain,
+					const struct mv_cesa_op_ctx *op_templ,
+					bool skip_ctx,
+					gfp_t flags);
+
+int mv_cesa_dma_add_data_transfer(struct mv_cesa_tdma_chain *chain,
+				  dma_addr_t dst, dma_addr_t src, u32 size,
+				  u32 flags, gfp_t gfp_flags);
+
+int mv_cesa_dma_add_dummy_launch(struct mv_cesa_tdma_chain *chain,
+				 u32 flags);
+
+int mv_cesa_dma_add_dummy_end(struct mv_cesa_tdma_chain *chain, u32 flags);
+
+int mv_cesa_dma_add_op_transfers(struct mv_cesa_tdma_chain *chain,
+				 struct mv_cesa_dma_iter *dma_iter,
+				 struct mv_cesa_sg_dma_iter *sgiter,
+				 gfp_t gfp_flags);
+
+/* Algorithm definitions */
+
+extern struct ahash_alg mv_md5_alg;
+extern struct ahash_alg mv_sha1_alg;
+extern struct ahash_alg mv_sha256_alg;
+extern struct ahash_alg mv_ahmac_md5_alg;
+extern struct ahash_alg mv_ahmac_sha1_alg;
+extern struct ahash_alg mv_ahmac_sha256_alg;
+
+extern struct crypto_alg mv_cesa_ecb_des_alg;
+extern struct crypto_alg mv_cesa_cbc_des_alg;
+extern struct crypto_alg mv_cesa_ecb_des3_ede_alg;
+extern struct crypto_alg mv_cesa_cbc_des3_ede_alg;
+extern struct crypto_alg mv_cesa_ecb_aes_alg;
+extern struct crypto_alg mv_cesa_cbc_aes_alg;
+
+#endif /* __MARVELL_CESA_H__ */
diff --git a/drivers/crypto/marvell/cipher.c b/drivers/crypto/marvell/cipher.c
new file mode 100644
index 000000000000..0745cf3b9c0e
--- /dev/null
+++ b/drivers/crypto/marvell/cipher.c
@@ -0,0 +1,797 @@
+/*
+ * Cipher algorithms supported by the CESA: DES, 3DES and AES.
+ *
+ * Author: Boris Brezillon <boris.brezillon@free-electrons.com>
+ * Author: Arnaud Ebalard <arno@natisbad.org>
+ *
+ * This work is based on an initial version written by
+ * Sebastian Andrzej Siewior < sebastian at breakpoint dot cc >
+ *
+ * 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 <crypto/aes.h>
+#include <crypto/des.h>
+
+#include "cesa.h"
+
+struct mv_cesa_des_ctx {
+	struct mv_cesa_ctx base;
+	u8 key[DES_KEY_SIZE];
+};
+
+struct mv_cesa_des3_ctx {
+	struct mv_cesa_ctx base;
+	u8 key[DES3_EDE_KEY_SIZE];
+};
+
+struct mv_cesa_aes_ctx {
+	struct mv_cesa_ctx base;
+	struct crypto_aes_ctx aes;
+};
+
+struct mv_cesa_ablkcipher_dma_iter {
+	struct mv_cesa_dma_iter base;
+	struct mv_cesa_sg_dma_iter src;
+	struct mv_cesa_sg_dma_iter dst;
+};
+
+static inline void
+mv_cesa_ablkcipher_req_iter_init(struct mv_cesa_ablkcipher_dma_iter *iter,
+				 struct ablkcipher_request *req)
+{
+	mv_cesa_req_dma_iter_init(&iter->base, req->nbytes);
+	mv_cesa_sg_dma_iter_init(&iter->src, req->src, DMA_TO_DEVICE);
+	mv_cesa_sg_dma_iter_init(&iter->dst, req->dst, DMA_FROM_DEVICE);
+}
+
+static inline bool
+mv_cesa_ablkcipher_req_iter_next_op(struct mv_cesa_ablkcipher_dma_iter *iter)
+{
+	iter->src.op_offset = 0;
+	iter->dst.op_offset = 0;
+
+	return mv_cesa_req_dma_iter_next_op(&iter->base);
+}
+
+static inline void
+mv_cesa_ablkcipher_dma_cleanup(struct ablkcipher_request *req)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+
+	if (req->dst != req->src) {
+		dma_unmap_sg(cesa_dev->dev, req->dst, creq->dst_nents,
+			     DMA_FROM_DEVICE);
+		dma_unmap_sg(cesa_dev->dev, req->src, creq->src_nents,
+			     DMA_TO_DEVICE);
+	} else {
+		dma_unmap_sg(cesa_dev->dev, req->src, creq->src_nents,
+			     DMA_BIDIRECTIONAL);
+	}
+	mv_cesa_dma_cleanup(&creq->req.dma);
+}
+
+static inline void mv_cesa_ablkcipher_cleanup(struct ablkcipher_request *req)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		mv_cesa_ablkcipher_dma_cleanup(req);
+}
+
+static void mv_cesa_ablkcipher_std_step(struct ablkcipher_request *req)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+	struct mv_cesa_ablkcipher_std_req *sreq = &creq->req.std;
+	struct mv_cesa_engine *engine = sreq->base.engine;
+	size_t  len = min_t(size_t, req->nbytes - sreq->offset,
+			    CESA_SA_SRAM_PAYLOAD_SIZE);
+
+	len = sg_pcopy_to_buffer(req->src, creq->src_nents,
+				 engine->sram + CESA_SA_DATA_SRAM_OFFSET,
+				 len, sreq->offset);
+
+	sreq->size = len;
+	mv_cesa_set_crypt_op_len(&sreq->op, len);
+
+	/* FIXME: only update enc_len field */
+	if (!sreq->skip_ctx) {
+		memcpy(engine->sram, &sreq->op, sizeof(sreq->op));
+		sreq->skip_ctx = true;
+	} else {
+		memcpy(engine->sram, &sreq->op, sizeof(sreq->op.desc));
+	}
+
+	mv_cesa_set_int_mask(engine, CESA_SA_INT_ACCEL0_DONE);
+	writel(CESA_SA_CFG_PARA_DIS, engine->regs + CESA_SA_CFG);
+	writel(CESA_SA_CMD_EN_CESA_SA_ACCL0, engine->regs + CESA_SA_CMD);
+}
+
+static int mv_cesa_ablkcipher_std_process(struct ablkcipher_request *req,
+					  u32 status)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+	struct mv_cesa_ablkcipher_std_req *sreq = &creq->req.std;
+	struct mv_cesa_engine *engine = sreq->base.engine;
+	size_t len;
+
+	len = sg_pcopy_from_buffer(req->dst, creq->dst_nents,
+				   engine->sram + CESA_SA_DATA_SRAM_OFFSET,
+				   sreq->size, sreq->offset);
+
+	sreq->offset += len;
+	if (sreq->offset < req->nbytes)
+		return -EINPROGRESS;
+
+	return 0;
+}
+
+static int mv_cesa_ablkcipher_process(struct crypto_async_request *req,
+				      u32 status)
+{
+	struct ablkcipher_request *ablkreq = ablkcipher_request_cast(req);
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(ablkreq);
+	struct mv_cesa_ablkcipher_std_req *sreq = &creq->req.std;
+	struct mv_cesa_engine *engine = sreq->base.engine;
+	int ret;
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		ret = mv_cesa_dma_process(&creq->req.dma, status);
+	else
+		ret = mv_cesa_ablkcipher_std_process(ablkreq, status);
+
+	if (ret)
+		return ret;
+
+	memcpy(ablkreq->info, engine->sram + CESA_SA_CRYPT_IV_SRAM_OFFSET,
+	       crypto_ablkcipher_ivsize(crypto_ablkcipher_reqtfm(ablkreq)));
+
+	return 0;
+}
+
+static void mv_cesa_ablkcipher_step(struct crypto_async_request *req)
+{
+	struct ablkcipher_request *ablkreq = ablkcipher_request_cast(req);
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(ablkreq);
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		mv_cesa_dma_step(&creq->req.dma);
+	else
+		mv_cesa_ablkcipher_std_step(ablkreq);
+}
+
+static inline void
+mv_cesa_ablkcipher_dma_prepare(struct ablkcipher_request *req)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+	struct mv_cesa_tdma_req *dreq = &creq->req.dma;
+
+	mv_cesa_dma_prepare(dreq, dreq->base.engine);
+}
+
+static inline void
+mv_cesa_ablkcipher_std_prepare(struct ablkcipher_request *req)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+	struct mv_cesa_ablkcipher_std_req *sreq = &creq->req.std;
+	struct mv_cesa_engine *engine = sreq->base.engine;
+
+	sreq->size = 0;
+	sreq->offset = 0;
+	mv_cesa_adjust_op(engine, &sreq->op);
+	memcpy(engine->sram, &sreq->op, sizeof(sreq->op));
+}
+
+static inline void mv_cesa_ablkcipher_prepare(struct crypto_async_request *req,
+					      struct mv_cesa_engine *engine)
+{
+	struct ablkcipher_request *ablkreq = ablkcipher_request_cast(req);
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(ablkreq);
+
+	creq->req.base.engine = engine;
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		mv_cesa_ablkcipher_dma_prepare(ablkreq);
+	else
+		mv_cesa_ablkcipher_std_prepare(ablkreq);
+}
+
+static inline void
+mv_cesa_ablkcipher_req_cleanup(struct crypto_async_request *req)
+{
+	struct ablkcipher_request *ablkreq = ablkcipher_request_cast(req);
+
+	mv_cesa_ablkcipher_cleanup(ablkreq);
+}
+
+static const struct mv_cesa_req_ops mv_cesa_ablkcipher_req_ops = {
+	.step = mv_cesa_ablkcipher_step,
+	.process = mv_cesa_ablkcipher_process,
+	.prepare = mv_cesa_ablkcipher_prepare,
+	.cleanup = mv_cesa_ablkcipher_req_cleanup,
+};
+
+static int mv_cesa_ablkcipher_cra_init(struct crypto_tfm *tfm)
+{
+	struct mv_cesa_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	ctx->base.ops = &mv_cesa_ablkcipher_req_ops;
+
+	tfm->crt_ablkcipher.reqsize = sizeof(struct mv_cesa_ablkcipher_req);
+
+	return 0;
+}
+
+static int mv_cesa_aes_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			      unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct mv_cesa_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+	int remaining;
+	int offset;
+	int ret;
+	int i;
+
+	ret = crypto_aes_expand_key(&ctx->aes, key, len);
+	if (ret) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return ret;
+	}
+
+	remaining = (ctx->aes.key_length - 16) / 4;
+	offset = ctx->aes.key_length + 24 - remaining;
+	for (i = 0; i < remaining; i++)
+		ctx->aes.key_dec[4 + i] =
+			cpu_to_le32(ctx->aes.key_enc[offset + i]);
+
+	return 0;
+}
+
+static int mv_cesa_des_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
+			      unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct mv_cesa_des_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 tmp[DES_EXPKEY_WORDS];
+	int ret;
+
+	if (len != DES_KEY_SIZE) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	ret = des_ekey(tmp, key);
+	if (!ret && (tfm->crt_flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		tfm->crt_flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, DES_KEY_SIZE);
+
+	return 0;
+}
+
+static int mv_cesa_des3_ede_setkey(struct crypto_ablkcipher *cipher,
+				   const u8 *key, unsigned int len)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct mv_cesa_des_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (len != DES3_EDE_KEY_SIZE) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, DES3_EDE_KEY_SIZE);
+
+	return 0;
+}
+
+static int mv_cesa_ablkcipher_dma_req_init(struct ablkcipher_request *req,
+				const struct mv_cesa_op_ctx *op_templ)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		      GFP_KERNEL : GFP_ATOMIC;
+	struct mv_cesa_tdma_req *dreq = &creq->req.dma;
+	struct mv_cesa_ablkcipher_dma_iter iter;
+	struct mv_cesa_tdma_chain chain;
+	bool skip_ctx = false;
+	int ret;
+
+	dreq->base.type = CESA_DMA_REQ;
+	dreq->chain.first = NULL;
+	dreq->chain.last = NULL;
+
+	if (req->src != req->dst) {
+		ret = dma_map_sg(cesa_dev->dev, req->src, creq->src_nents,
+				 DMA_TO_DEVICE);
+		if (!ret)
+			return -ENOMEM;
+
+		ret = dma_map_sg(cesa_dev->dev, req->dst, creq->dst_nents,
+				 DMA_FROM_DEVICE);
+		if (!ret) {
+			ret = -ENOMEM;
+			goto err_unmap_src;
+		}
+	} else {
+		ret = dma_map_sg(cesa_dev->dev, req->src, creq->src_nents,
+				 DMA_BIDIRECTIONAL);
+		if (!ret)
+			return -ENOMEM;
+	}
+
+	mv_cesa_tdma_desc_iter_init(&chain);
+	mv_cesa_ablkcipher_req_iter_init(&iter, req);
+
+	do {
+		struct mv_cesa_op_ctx *op;
+
+		op = mv_cesa_dma_add_op(&chain, op_templ, skip_ctx, flags);
+		if (IS_ERR(op)) {
+			ret = PTR_ERR(op);
+			goto err_free_tdma;
+		}
+		skip_ctx = true;
+
+		mv_cesa_set_crypt_op_len(op, iter.base.op_len);
+
+		/* Add input transfers */
+		ret = mv_cesa_dma_add_op_transfers(&chain, &iter.base,
+						   &iter.src, flags);
+		if (ret)
+			goto err_free_tdma;
+
+		/* Add dummy desc to launch the crypto operation */
+		ret = mv_cesa_dma_add_dummy_launch(&chain, flags);
+		if (ret)
+			goto err_free_tdma;
+
+		/* Add output transfers */
+		ret = mv_cesa_dma_add_op_transfers(&chain, &iter.base,
+						   &iter.dst, flags);
+		if (ret)
+			goto err_free_tdma;
+
+	} while (mv_cesa_ablkcipher_req_iter_next_op(&iter));
+
+	dreq->chain = chain;
+
+	return 0;
+
+err_free_tdma:
+	mv_cesa_dma_cleanup(dreq);
+	if (req->dst != req->src)
+		dma_unmap_sg(cesa_dev->dev, req->dst, creq->dst_nents,
+			     DMA_FROM_DEVICE);
+
+err_unmap_src:
+	dma_unmap_sg(cesa_dev->dev, req->src, creq->src_nents,
+		     req->dst != req->src ? DMA_TO_DEVICE : DMA_BIDIRECTIONAL);
+
+	return ret;
+}
+
+static inline int
+mv_cesa_ablkcipher_std_req_init(struct ablkcipher_request *req,
+				const struct mv_cesa_op_ctx *op_templ)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+	struct mv_cesa_ablkcipher_std_req *sreq = &creq->req.std;
+
+	sreq->base.type = CESA_STD_REQ;
+	sreq->op = *op_templ;
+	sreq->skip_ctx = false;
+
+	return 0;
+}
+
+static int mv_cesa_ablkcipher_req_init(struct ablkcipher_request *req,
+				       struct mv_cesa_op_ctx *tmpl)
+{
+	struct mv_cesa_ablkcipher_req *creq = ablkcipher_request_ctx(req);
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	unsigned int blksize = crypto_ablkcipher_blocksize(tfm);
+	int ret;
+
+	if (!IS_ALIGNED(req->nbytes, blksize))
+		return -EINVAL;
+
+	creq->src_nents = sg_nents_for_len(req->src, req->nbytes);
+	creq->dst_nents = sg_nents_for_len(req->dst, req->nbytes);
+
+	mv_cesa_update_op_cfg(tmpl, CESA_SA_DESC_CFG_OP_CRYPT_ONLY,
+			      CESA_SA_DESC_CFG_OP_MSK);
+
+	/* TODO: add a threshold for DMA usage */
+	if (cesa_dev->caps->has_tdma)
+		ret = mv_cesa_ablkcipher_dma_req_init(req, tmpl);
+	else
+		ret = mv_cesa_ablkcipher_std_req_init(req, tmpl);
+
+	return ret;
+}
+
+static int mv_cesa_des_op(struct ablkcipher_request *req,
+			  struct mv_cesa_op_ctx *tmpl)
+{
+	struct mv_cesa_des_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	int ret;
+
+	mv_cesa_update_op_cfg(tmpl, CESA_SA_DESC_CFG_CRYPTM_DES,
+			      CESA_SA_DESC_CFG_CRYPTM_MSK);
+
+	memcpy(tmpl->ctx.blkcipher.key, ctx->key, DES_KEY_SIZE);
+
+	ret = mv_cesa_ablkcipher_req_init(req, tmpl);
+	if (ret)
+		return ret;
+
+	ret = mv_cesa_queue_req(&req->base);
+	if (ret && ret != -EINPROGRESS)
+		mv_cesa_ablkcipher_cleanup(req);
+
+	return ret;
+}
+
+static int mv_cesa_ecb_des_encrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl,
+			   CESA_SA_DESC_CFG_CRYPTCM_ECB |
+			   CESA_SA_DESC_CFG_DIR_ENC);
+
+	return mv_cesa_des_op(req, &tmpl);
+}
+
+static int mv_cesa_ecb_des_decrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl,
+			   CESA_SA_DESC_CFG_CRYPTCM_ECB |
+			   CESA_SA_DESC_CFG_DIR_DEC);
+
+	return mv_cesa_des_op(req, &tmpl);
+}
+
+struct crypto_alg mv_cesa_ecb_des_alg = {
+	.cra_name = "ecb(des)",
+	.cra_driver_name = "mv-ecb-des",
+	.cra_priority = 300,
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+		     CRYPTO_ALG_KERN_DRIVER_ONLY | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = DES_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct mv_cesa_des_ctx),
+	.cra_alignmask = 0,
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_module = THIS_MODULE,
+	.cra_init = mv_cesa_ablkcipher_cra_init,
+	.cra_u = {
+		.ablkcipher = {
+			.min_keysize = DES_KEY_SIZE,
+			.max_keysize = DES_KEY_SIZE,
+			.setkey = mv_cesa_des_setkey,
+			.encrypt = mv_cesa_ecb_des_encrypt,
+			.decrypt = mv_cesa_ecb_des_decrypt,
+		},
+	},
+};
+
+static int mv_cesa_cbc_des_op(struct ablkcipher_request *req,
+			      struct mv_cesa_op_ctx *tmpl)
+{
+	mv_cesa_update_op_cfg(tmpl, CESA_SA_DESC_CFG_CRYPTCM_CBC,
+			      CESA_SA_DESC_CFG_CRYPTCM_MSK);
+
+	memcpy(tmpl->ctx.blkcipher.iv, req->info, DES_BLOCK_SIZE);
+
+	return mv_cesa_des_op(req, tmpl);
+}
+
+static int mv_cesa_cbc_des_encrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_DIR_ENC);
+
+	return mv_cesa_cbc_des_op(req, &tmpl);
+}
+
+static int mv_cesa_cbc_des_decrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_DIR_DEC);
+
+	return mv_cesa_cbc_des_op(req, &tmpl);
+}
+
+struct crypto_alg mv_cesa_cbc_des_alg = {
+	.cra_name = "cbc(des)",
+	.cra_driver_name = "mv-cbc-des",
+	.cra_priority = 300,
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+		     CRYPTO_ALG_KERN_DRIVER_ONLY | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = DES_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct mv_cesa_des_ctx),
+	.cra_alignmask = 0,
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_module = THIS_MODULE,
+	.cra_init = mv_cesa_ablkcipher_cra_init,
+	.cra_u = {
+		.ablkcipher = {
+			.min_keysize = DES_KEY_SIZE,
+			.max_keysize = DES_KEY_SIZE,
+			.ivsize	     = DES_BLOCK_SIZE,
+			.setkey = mv_cesa_des_setkey,
+			.encrypt = mv_cesa_cbc_des_encrypt,
+			.decrypt = mv_cesa_cbc_des_decrypt,
+		},
+	},
+};
+
+static int mv_cesa_des3_op(struct ablkcipher_request *req,
+			   struct mv_cesa_op_ctx *tmpl)
+{
+	struct mv_cesa_des3_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	int ret;
+
+	mv_cesa_update_op_cfg(tmpl, CESA_SA_DESC_CFG_CRYPTM_3DES,
+			      CESA_SA_DESC_CFG_CRYPTM_MSK);
+
+	memcpy(tmpl->ctx.blkcipher.key, ctx->key, DES3_EDE_KEY_SIZE);
+
+	ret = mv_cesa_ablkcipher_req_init(req, tmpl);
+	if (ret)
+		return ret;
+
+	ret = mv_cesa_queue_req(&req->base);
+	if (ret && ret != -EINPROGRESS)
+		mv_cesa_ablkcipher_cleanup(req);
+
+	return ret;
+}
+
+static int mv_cesa_ecb_des3_ede_encrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl,
+			   CESA_SA_DESC_CFG_CRYPTCM_ECB |
+			   CESA_SA_DESC_CFG_3DES_EDE |
+			   CESA_SA_DESC_CFG_DIR_ENC);
+
+	return mv_cesa_des3_op(req, &tmpl);
+}
+
+static int mv_cesa_ecb_des3_ede_decrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl,
+			   CESA_SA_DESC_CFG_CRYPTCM_ECB |
+			   CESA_SA_DESC_CFG_3DES_EDE |
+			   CESA_SA_DESC_CFG_DIR_DEC);
+
+	return mv_cesa_des3_op(req, &tmpl);
+}
+
+struct crypto_alg mv_cesa_ecb_des3_ede_alg = {
+	.cra_name = "ecb(des3_ede)",
+	.cra_driver_name = "mv-ecb-des3-ede",
+	.cra_priority = 300,
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+		     CRYPTO_ALG_KERN_DRIVER_ONLY | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct mv_cesa_des3_ctx),
+	.cra_alignmask = 0,
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_module = THIS_MODULE,
+	.cra_init = mv_cesa_ablkcipher_cra_init,
+	.cra_u = {
+		.ablkcipher = {
+			.min_keysize = DES3_EDE_KEY_SIZE,
+			.max_keysize = DES3_EDE_KEY_SIZE,
+			.ivsize	     = DES3_EDE_BLOCK_SIZE,
+			.setkey = mv_cesa_des3_ede_setkey,
+			.encrypt = mv_cesa_ecb_des3_ede_encrypt,
+			.decrypt = mv_cesa_ecb_des3_ede_decrypt,
+		},
+	},
+};
+
+static int mv_cesa_cbc_des3_op(struct ablkcipher_request *req,
+			       struct mv_cesa_op_ctx *tmpl)
+{
+	memcpy(tmpl->ctx.blkcipher.iv, req->info, DES3_EDE_BLOCK_SIZE);
+
+	return mv_cesa_des3_op(req, tmpl);
+}
+
+static int mv_cesa_cbc_des3_ede_encrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl,
+			   CESA_SA_DESC_CFG_CRYPTCM_CBC |
+			   CESA_SA_DESC_CFG_3DES_EDE |
+			   CESA_SA_DESC_CFG_DIR_ENC);
+
+	return mv_cesa_cbc_des3_op(req, &tmpl);
+}
+
+static int mv_cesa_cbc_des3_ede_decrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl,
+			   CESA_SA_DESC_CFG_CRYPTCM_CBC |
+			   CESA_SA_DESC_CFG_3DES_EDE |
+			   CESA_SA_DESC_CFG_DIR_DEC);
+
+	return mv_cesa_cbc_des3_op(req, &tmpl);
+}
+
+struct crypto_alg mv_cesa_cbc_des3_ede_alg = {
+	.cra_name = "cbc(des3_ede)",
+	.cra_driver_name = "mv-cbc-des3-ede",
+	.cra_priority = 300,
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+		     CRYPTO_ALG_KERN_DRIVER_ONLY | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct mv_cesa_des3_ctx),
+	.cra_alignmask = 0,
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_module = THIS_MODULE,
+	.cra_init = mv_cesa_ablkcipher_cra_init,
+	.cra_u = {
+		.ablkcipher = {
+			.min_keysize = DES3_EDE_KEY_SIZE,
+			.max_keysize = DES3_EDE_KEY_SIZE,
+			.ivsize	     = DES3_EDE_BLOCK_SIZE,
+			.setkey = mv_cesa_des3_ede_setkey,
+			.encrypt = mv_cesa_cbc_des3_ede_encrypt,
+			.decrypt = mv_cesa_cbc_des3_ede_decrypt,
+		},
+	},
+};
+
+static int mv_cesa_aes_op(struct ablkcipher_request *req,
+			  struct mv_cesa_op_ctx *tmpl)
+{
+	struct mv_cesa_aes_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	int ret, i;
+	u32 *key;
+	u32 cfg;
+
+	cfg = CESA_SA_DESC_CFG_CRYPTM_AES;
+
+	if (mv_cesa_get_op_cfg(tmpl) & CESA_SA_DESC_CFG_DIR_DEC)
+		key = ctx->aes.key_dec;
+	else
+		key = ctx->aes.key_enc;
+
+	for (i = 0; i < ctx->aes.key_length / sizeof(u32); i++)
+		tmpl->ctx.blkcipher.key[i] = cpu_to_le32(key[i]);
+
+	if (ctx->aes.key_length == 24)
+		cfg |= CESA_SA_DESC_CFG_AES_LEN_192;
+	else if (ctx->aes.key_length == 32)
+		cfg |= CESA_SA_DESC_CFG_AES_LEN_256;
+
+	mv_cesa_update_op_cfg(tmpl, cfg,
+			      CESA_SA_DESC_CFG_CRYPTM_MSK |
+			      CESA_SA_DESC_CFG_AES_LEN_MSK);
+
+	ret = mv_cesa_ablkcipher_req_init(req, tmpl);
+	if (ret)
+		return ret;
+
+	ret = mv_cesa_queue_req(&req->base);
+	if (ret && ret != -EINPROGRESS)
+		mv_cesa_ablkcipher_cleanup(req);
+
+	return ret;
+}
+
+static int mv_cesa_ecb_aes_encrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl,
+			   CESA_SA_DESC_CFG_CRYPTCM_ECB |
+			   CESA_SA_DESC_CFG_DIR_ENC);
+
+	return mv_cesa_aes_op(req, &tmpl);
+}
+
+static int mv_cesa_ecb_aes_decrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl,
+			   CESA_SA_DESC_CFG_CRYPTCM_ECB |
+			   CESA_SA_DESC_CFG_DIR_DEC);
+
+	return mv_cesa_aes_op(req, &tmpl);
+}
+
+struct crypto_alg mv_cesa_ecb_aes_alg = {
+	.cra_name = "ecb(aes)",
+	.cra_driver_name = "mv-ecb-aes",
+	.cra_priority = 300,
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+		     CRYPTO_ALG_KERN_DRIVER_ONLY | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = AES_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct mv_cesa_aes_ctx),
+	.cra_alignmask = 0,
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_module = THIS_MODULE,
+	.cra_init = mv_cesa_ablkcipher_cra_init,
+	.cra_u = {
+		.ablkcipher = {
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.setkey = mv_cesa_aes_setkey,
+			.encrypt = mv_cesa_ecb_aes_encrypt,
+			.decrypt = mv_cesa_ecb_aes_decrypt,
+		},
+	},
+};
+
+static int mv_cesa_cbc_aes_op(struct ablkcipher_request *req,
+			      struct mv_cesa_op_ctx *tmpl)
+{
+	mv_cesa_update_op_cfg(tmpl, CESA_SA_DESC_CFG_CRYPTCM_CBC,
+			      CESA_SA_DESC_CFG_CRYPTCM_MSK);
+	memcpy(tmpl->ctx.blkcipher.iv, req->info, AES_BLOCK_SIZE);
+
+	return mv_cesa_aes_op(req, tmpl);
+}
+
+static int mv_cesa_cbc_aes_encrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_DIR_ENC);
+
+	return mv_cesa_cbc_aes_op(req, &tmpl);
+}
+
+static int mv_cesa_cbc_aes_decrypt(struct ablkcipher_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_DIR_DEC);
+
+	return mv_cesa_cbc_aes_op(req, &tmpl);
+}
+
+struct crypto_alg mv_cesa_cbc_aes_alg = {
+	.cra_name = "cbc(aes)",
+	.cra_driver_name = "mv-cbc-aes",
+	.cra_priority = 300,
+	.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+		     CRYPTO_ALG_KERN_DRIVER_ONLY | CRYPTO_ALG_ASYNC,
+	.cra_blocksize = AES_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct mv_cesa_aes_ctx),
+	.cra_alignmask = 0,
+	.cra_type = &crypto_ablkcipher_type,
+	.cra_module = THIS_MODULE,
+	.cra_init = mv_cesa_ablkcipher_cra_init,
+	.cra_u = {
+		.ablkcipher = {
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_BLOCK_SIZE,
+			.setkey = mv_cesa_aes_setkey,
+			.encrypt = mv_cesa_cbc_aes_encrypt,
+			.decrypt = mv_cesa_cbc_aes_decrypt,
+		},
+	},
+};
diff --git a/drivers/crypto/marvell/hash.c b/drivers/crypto/marvell/hash.c
new file mode 100644
index 000000000000..ae9272eb9c1a
--- /dev/null
+++ b/drivers/crypto/marvell/hash.c
@@ -0,0 +1,1441 @@
+/*
+ * Hash algorithms supported by the CESA: MD5, SHA1 and SHA256.
+ *
+ * Author: Boris Brezillon <boris.brezillon@free-electrons.com>
+ * Author: Arnaud Ebalard <arno@natisbad.org>
+ *
+ * This work is based on an initial version written by
+ * Sebastian Andrzej Siewior < sebastian at breakpoint dot cc >
+ *
+ * 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 <crypto/md5.h>
+#include <crypto/sha.h>
+
+#include "cesa.h"
+
+struct mv_cesa_ahash_dma_iter {
+	struct mv_cesa_dma_iter base;
+	struct mv_cesa_sg_dma_iter src;
+};
+
+static inline void
+mv_cesa_ahash_req_iter_init(struct mv_cesa_ahash_dma_iter *iter,
+			    struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	unsigned int len = req->nbytes;
+
+	if (!creq->last_req)
+		len = (len + creq->cache_ptr) & ~CESA_HASH_BLOCK_SIZE_MSK;
+
+	mv_cesa_req_dma_iter_init(&iter->base, len);
+	mv_cesa_sg_dma_iter_init(&iter->src, req->src, DMA_TO_DEVICE);
+	iter->src.op_offset = creq->cache_ptr;
+}
+
+static inline bool
+mv_cesa_ahash_req_iter_next_op(struct mv_cesa_ahash_dma_iter *iter)
+{
+	iter->src.op_offset = 0;
+
+	return mv_cesa_req_dma_iter_next_op(&iter->base);
+}
+
+static inline int mv_cesa_ahash_dma_alloc_cache(struct mv_cesa_ahash_req *creq,
+						gfp_t flags)
+{
+	struct mv_cesa_ahash_dma_req *dreq = &creq->req.dma;
+
+	creq->cache = dma_pool_alloc(cesa_dev->dma->cache_pool, flags,
+				     &dreq->cache_dma);
+	if (!creq->cache)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static inline int mv_cesa_ahash_std_alloc_cache(struct mv_cesa_ahash_req *creq,
+						gfp_t flags)
+{
+	creq->cache = kzalloc(CESA_MAX_HASH_BLOCK_SIZE, flags);
+	if (!creq->cache)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int mv_cesa_ahash_alloc_cache(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		      GFP_KERNEL : GFP_ATOMIC;
+	int ret;
+
+	if (creq->cache)
+		return 0;
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		ret = mv_cesa_ahash_dma_alloc_cache(creq, flags);
+	else
+		ret = mv_cesa_ahash_std_alloc_cache(creq, flags);
+
+	return ret;
+}
+
+static inline void mv_cesa_ahash_dma_free_cache(struct mv_cesa_ahash_req *creq)
+{
+	dma_pool_free(cesa_dev->dma->cache_pool, creq->cache,
+		      creq->req.dma.cache_dma);
+}
+
+static inline void mv_cesa_ahash_std_free_cache(struct mv_cesa_ahash_req *creq)
+{
+	kfree(creq->cache);
+}
+
+static void mv_cesa_ahash_free_cache(struct mv_cesa_ahash_req *creq)
+{
+	if (!creq->cache)
+		return;
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		mv_cesa_ahash_dma_free_cache(creq);
+	else
+		mv_cesa_ahash_std_free_cache(creq);
+
+	creq->cache = NULL;
+}
+
+static int mv_cesa_ahash_dma_alloc_padding(struct mv_cesa_ahash_dma_req *req,
+					   gfp_t flags)
+{
+	if (req->padding)
+		return 0;
+
+	req->padding = dma_pool_alloc(cesa_dev->dma->padding_pool, flags,
+				      &req->padding_dma);
+	if (!req->padding)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void mv_cesa_ahash_dma_free_padding(struct mv_cesa_ahash_dma_req *req)
+{
+	if (!req->padding)
+		return;
+
+	dma_pool_free(cesa_dev->dma->padding_pool, req->padding,
+		      req->padding_dma);
+	req->padding = NULL;
+}
+
+static inline void mv_cesa_ahash_dma_last_cleanup(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+
+	mv_cesa_ahash_dma_free_padding(&creq->req.dma);
+}
+
+static inline void mv_cesa_ahash_dma_cleanup(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+
+	dma_unmap_sg(cesa_dev->dev, req->src, creq->src_nents, DMA_TO_DEVICE);
+	mv_cesa_dma_cleanup(&creq->req.dma.base);
+}
+
+static inline void mv_cesa_ahash_cleanup(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		mv_cesa_ahash_dma_cleanup(req);
+}
+
+static void mv_cesa_ahash_last_cleanup(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+
+	mv_cesa_ahash_free_cache(creq);
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		mv_cesa_ahash_dma_last_cleanup(req);
+}
+
+static int mv_cesa_ahash_pad_len(struct mv_cesa_ahash_req *creq)
+{
+	unsigned int index, padlen;
+
+	index = creq->len & CESA_HASH_BLOCK_SIZE_MSK;
+	padlen = (index < 56) ? (56 - index) : (64 + 56 - index);
+
+	return padlen;
+}
+
+static int mv_cesa_ahash_pad_req(struct mv_cesa_ahash_req *creq, u8 *buf)
+{
+	__be64 bits = cpu_to_be64(creq->len << 3);
+	unsigned int index, padlen;
+
+	buf[0] = 0x80;
+	/* Pad out to 56 mod 64 */
+	index = creq->len & CESA_HASH_BLOCK_SIZE_MSK;
+	padlen = mv_cesa_ahash_pad_len(creq);
+	memset(buf + 1, 0, padlen - 1);
+	memcpy(buf + padlen, &bits, sizeof(bits));
+
+	return padlen + 8;
+}
+
+static void mv_cesa_ahash_std_step(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	struct mv_cesa_ahash_std_req *sreq = &creq->req.std;
+	struct mv_cesa_engine *engine = sreq->base.engine;
+	struct mv_cesa_op_ctx *op;
+	unsigned int new_cache_ptr = 0;
+	u32 frag_mode;
+	size_t  len;
+
+	if (creq->cache_ptr)
+		memcpy(engine->sram + CESA_SA_DATA_SRAM_OFFSET, creq->cache,
+		       creq->cache_ptr);
+
+	len = min_t(size_t, req->nbytes + creq->cache_ptr - sreq->offset,
+		    CESA_SA_SRAM_PAYLOAD_SIZE);
+
+	if (!creq->last_req) {
+		new_cache_ptr = len & CESA_HASH_BLOCK_SIZE_MSK;
+		len &= ~CESA_HASH_BLOCK_SIZE_MSK;
+	}
+
+	if (len - creq->cache_ptr)
+		sreq->offset += sg_pcopy_to_buffer(req->src, creq->src_nents,
+						   engine->sram +
+						   CESA_SA_DATA_SRAM_OFFSET +
+						   creq->cache_ptr,
+						   len - creq->cache_ptr,
+						   sreq->offset);
+
+	op = &creq->op_tmpl;
+
+	frag_mode = mv_cesa_get_op_cfg(op) & CESA_SA_DESC_CFG_FRAG_MSK;
+
+	if (creq->last_req && sreq->offset == req->nbytes &&
+	    creq->len <= CESA_SA_DESC_MAC_SRC_TOTAL_LEN_MAX) {
+		if (frag_mode == CESA_SA_DESC_CFG_FIRST_FRAG)
+			frag_mode = CESA_SA_DESC_CFG_NOT_FRAG;
+		else if (frag_mode == CESA_SA_DESC_CFG_MID_FRAG)
+			frag_mode = CESA_SA_DESC_CFG_LAST_FRAG;
+	}
+
+	if (frag_mode == CESA_SA_DESC_CFG_NOT_FRAG ||
+	    frag_mode == CESA_SA_DESC_CFG_LAST_FRAG) {
+		if (len &&
+		    creq->len <= CESA_SA_DESC_MAC_SRC_TOTAL_LEN_MAX) {
+			mv_cesa_set_mac_op_total_len(op, creq->len);
+		} else {
+			int trailerlen = mv_cesa_ahash_pad_len(creq) + 8;
+
+			if (len + trailerlen > CESA_SA_SRAM_PAYLOAD_SIZE) {
+				len &= CESA_HASH_BLOCK_SIZE_MSK;
+				new_cache_ptr = 64 - trailerlen;
+				memcpy(creq->cache,
+				       engine->sram +
+				       CESA_SA_DATA_SRAM_OFFSET + len,
+				       new_cache_ptr);
+			} else {
+				len += mv_cesa_ahash_pad_req(creq,
+						engine->sram + len +
+						CESA_SA_DATA_SRAM_OFFSET);
+			}
+
+			if (frag_mode == CESA_SA_DESC_CFG_LAST_FRAG)
+				frag_mode = CESA_SA_DESC_CFG_MID_FRAG;
+			else
+				frag_mode = CESA_SA_DESC_CFG_FIRST_FRAG;
+		}
+	}
+
+	mv_cesa_set_mac_op_frag_len(op, len);
+	mv_cesa_update_op_cfg(op, frag_mode, CESA_SA_DESC_CFG_FRAG_MSK);
+
+	/* FIXME: only update enc_len field */
+	memcpy(engine->sram, op, sizeof(*op));
+
+	if (frag_mode == CESA_SA_DESC_CFG_FIRST_FRAG)
+		mv_cesa_update_op_cfg(op, CESA_SA_DESC_CFG_MID_FRAG,
+				      CESA_SA_DESC_CFG_FRAG_MSK);
+
+	creq->cache_ptr = new_cache_ptr;
+
+	mv_cesa_set_int_mask(engine, CESA_SA_INT_ACCEL0_DONE);
+	writel(CESA_SA_CFG_PARA_DIS, engine->regs + CESA_SA_CFG);
+	writel(CESA_SA_CMD_EN_CESA_SA_ACCL0, engine->regs + CESA_SA_CMD);
+}
+
+static int mv_cesa_ahash_std_process(struct ahash_request *req, u32 status)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	struct mv_cesa_ahash_std_req *sreq = &creq->req.std;
+
+	if (sreq->offset < (req->nbytes - creq->cache_ptr))
+		return -EINPROGRESS;
+
+	return 0;
+}
+
+static inline void mv_cesa_ahash_dma_prepare(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	struct mv_cesa_tdma_req *dreq = &creq->req.dma.base;
+
+	mv_cesa_dma_prepare(dreq, dreq->base.engine);
+}
+
+static void mv_cesa_ahash_std_prepare(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	struct mv_cesa_ahash_std_req *sreq = &creq->req.std;
+	struct mv_cesa_engine *engine = sreq->base.engine;
+
+	sreq->offset = 0;
+	mv_cesa_adjust_op(engine, &creq->op_tmpl);
+	memcpy(engine->sram, &creq->op_tmpl, sizeof(creq->op_tmpl));
+}
+
+static void mv_cesa_ahash_step(struct crypto_async_request *req)
+{
+	struct ahash_request *ahashreq = ahash_request_cast(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(ahashreq);
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		mv_cesa_dma_step(&creq->req.dma.base);
+	else
+		mv_cesa_ahash_std_step(ahashreq);
+}
+
+static int mv_cesa_ahash_process(struct crypto_async_request *req, u32 status)
+{
+	struct ahash_request *ahashreq = ahash_request_cast(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(ahashreq);
+	struct mv_cesa_engine *engine = creq->req.base.engine;
+	unsigned int digsize;
+	int ret, i;
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		ret = mv_cesa_dma_process(&creq->req.dma.base, status);
+	else
+		ret = mv_cesa_ahash_std_process(ahashreq, status);
+
+	if (ret == -EINPROGRESS)
+		return ret;
+
+	digsize = crypto_ahash_digestsize(crypto_ahash_reqtfm(ahashreq));
+	for (i = 0; i < digsize / 4; i++)
+		creq->state[i] = readl(engine->regs + CESA_IVDIG(i));
+
+	if (creq->cache_ptr)
+		sg_pcopy_to_buffer(ahashreq->src, creq->src_nents,
+				   creq->cache,
+				   creq->cache_ptr,
+				   ahashreq->nbytes - creq->cache_ptr);
+
+	if (creq->last_req) {
+		for (i = 0; i < digsize / 4; i++) {
+			/*
+			 * Hardware provides MD5 digest in a different
+			 * endianness than SHA-1 and SHA-256 ones.
+			 */
+			if (digsize == MD5_DIGEST_SIZE)
+				creq->state[i] = cpu_to_le32(creq->state[i]);
+			else
+				creq->state[i] = cpu_to_be32(creq->state[i]);
+		}
+
+		memcpy(ahashreq->result, creq->state, digsize);
+	}
+
+	return ret;
+}
+
+static void mv_cesa_ahash_prepare(struct crypto_async_request *req,
+				  struct mv_cesa_engine *engine)
+{
+	struct ahash_request *ahashreq = ahash_request_cast(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(ahashreq);
+	unsigned int digsize;
+	int i;
+
+	creq->req.base.engine = engine;
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		mv_cesa_ahash_dma_prepare(ahashreq);
+	else
+		mv_cesa_ahash_std_prepare(ahashreq);
+
+	digsize = crypto_ahash_digestsize(crypto_ahash_reqtfm(ahashreq));
+	for (i = 0; i < digsize / 4; i++)
+		writel(creq->state[i],
+		       engine->regs + CESA_IVDIG(i));
+}
+
+static void mv_cesa_ahash_req_cleanup(struct crypto_async_request *req)
+{
+	struct ahash_request *ahashreq = ahash_request_cast(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(ahashreq);
+
+	if (creq->last_req)
+		mv_cesa_ahash_last_cleanup(ahashreq);
+
+	mv_cesa_ahash_cleanup(ahashreq);
+}
+
+static const struct mv_cesa_req_ops mv_cesa_ahash_req_ops = {
+	.step = mv_cesa_ahash_step,
+	.process = mv_cesa_ahash_process,
+	.prepare = mv_cesa_ahash_prepare,
+	.cleanup = mv_cesa_ahash_req_cleanup,
+};
+
+static int mv_cesa_ahash_init(struct ahash_request *req,
+			      struct mv_cesa_op_ctx *tmpl)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+
+	memset(creq, 0, sizeof(*creq));
+	mv_cesa_update_op_cfg(tmpl,
+			      CESA_SA_DESC_CFG_OP_MAC_ONLY |
+			      CESA_SA_DESC_CFG_FIRST_FRAG,
+			      CESA_SA_DESC_CFG_OP_MSK |
+			      CESA_SA_DESC_CFG_FRAG_MSK);
+	mv_cesa_set_mac_op_total_len(tmpl, 0);
+	mv_cesa_set_mac_op_frag_len(tmpl, 0);
+	creq->op_tmpl = *tmpl;
+	creq->len = 0;
+
+	return 0;
+}
+
+static inline int mv_cesa_ahash_cra_init(struct crypto_tfm *tfm)
+{
+	struct mv_cesa_hash_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	ctx->base.ops = &mv_cesa_ahash_req_ops;
+
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct mv_cesa_ahash_req));
+	return 0;
+}
+
+static int mv_cesa_ahash_cache_req(struct ahash_request *req, bool *cached)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	int ret;
+
+	if (((creq->cache_ptr + req->nbytes) & CESA_HASH_BLOCK_SIZE_MSK) &&
+	    !creq->last_req) {
+		ret = mv_cesa_ahash_alloc_cache(req);
+		if (ret)
+			return ret;
+	}
+
+	if (creq->cache_ptr + req->nbytes < 64 && !creq->last_req) {
+		*cached = true;
+
+		if (!req->nbytes)
+			return 0;
+
+		sg_pcopy_to_buffer(req->src, creq->src_nents,
+				   creq->cache + creq->cache_ptr,
+				   req->nbytes, 0);
+
+		creq->cache_ptr += req->nbytes;
+	}
+
+	return 0;
+}
+
+static struct mv_cesa_op_ctx *
+mv_cesa_ahash_dma_add_cache(struct mv_cesa_tdma_chain *chain,
+			    struct mv_cesa_ahash_dma_iter *dma_iter,
+			    struct mv_cesa_ahash_req *creq,
+			    gfp_t flags)
+{
+	struct mv_cesa_ahash_dma_req *ahashdreq = &creq->req.dma;
+	struct mv_cesa_op_ctx *op = NULL;
+	int ret;
+
+	if (!creq->cache_ptr)
+		return NULL;
+
+	ret = mv_cesa_dma_add_data_transfer(chain,
+					    CESA_SA_DATA_SRAM_OFFSET,
+					    ahashdreq->cache_dma,
+					    creq->cache_ptr,
+					    CESA_TDMA_DST_IN_SRAM,
+					    flags);
+	if (ret)
+		return ERR_PTR(ret);
+
+	if (!dma_iter->base.op_len) {
+		op = mv_cesa_dma_add_op(chain, &creq->op_tmpl, false, flags);
+		if (IS_ERR(op))
+			return op;
+
+		mv_cesa_set_mac_op_frag_len(op, creq->cache_ptr);
+
+		/* Add dummy desc to launch crypto operation */
+		ret = mv_cesa_dma_add_dummy_launch(chain, flags);
+		if (ret)
+			return ERR_PTR(ret);
+	}
+
+	return op;
+}
+
+static struct mv_cesa_op_ctx *
+mv_cesa_ahash_dma_add_data(struct mv_cesa_tdma_chain *chain,
+			   struct mv_cesa_ahash_dma_iter *dma_iter,
+			   struct mv_cesa_ahash_req *creq,
+			   gfp_t flags)
+{
+	struct mv_cesa_op_ctx *op;
+	int ret;
+
+	op = mv_cesa_dma_add_op(chain, &creq->op_tmpl, false, flags);
+	if (IS_ERR(op))
+		return op;
+
+	mv_cesa_set_mac_op_frag_len(op, dma_iter->base.op_len);
+
+	if ((mv_cesa_get_op_cfg(&creq->op_tmpl) & CESA_SA_DESC_CFG_FRAG_MSK) ==
+	    CESA_SA_DESC_CFG_FIRST_FRAG)
+		mv_cesa_update_op_cfg(&creq->op_tmpl,
+				      CESA_SA_DESC_CFG_MID_FRAG,
+				      CESA_SA_DESC_CFG_FRAG_MSK);
+
+	/* Add input transfers */
+	ret = mv_cesa_dma_add_op_transfers(chain, &dma_iter->base,
+					   &dma_iter->src, flags);
+	if (ret)
+		return ERR_PTR(ret);
+
+	/* Add dummy desc to launch crypto operation */
+	ret = mv_cesa_dma_add_dummy_launch(chain, flags);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return op;
+}
+
+static struct mv_cesa_op_ctx *
+mv_cesa_ahash_dma_last_req(struct mv_cesa_tdma_chain *chain,
+			   struct mv_cesa_ahash_dma_iter *dma_iter,
+			   struct mv_cesa_ahash_req *creq,
+			   struct mv_cesa_op_ctx *op,
+			   gfp_t flags)
+{
+	struct mv_cesa_ahash_dma_req *ahashdreq = &creq->req.dma;
+	unsigned int len, trailerlen, padoff = 0;
+	int ret;
+
+	if (!creq->last_req)
+		return op;
+
+	if (op && creq->len <= CESA_SA_DESC_MAC_SRC_TOTAL_LEN_MAX) {
+		u32 frag = CESA_SA_DESC_CFG_NOT_FRAG;
+
+		if ((mv_cesa_get_op_cfg(op) & CESA_SA_DESC_CFG_FRAG_MSK) !=
+		    CESA_SA_DESC_CFG_FIRST_FRAG)
+			frag = CESA_SA_DESC_CFG_LAST_FRAG;
+
+		mv_cesa_update_op_cfg(op, frag, CESA_SA_DESC_CFG_FRAG_MSK);
+
+		return op;
+	}
+
+	ret = mv_cesa_ahash_dma_alloc_padding(ahashdreq, flags);
+	if (ret)
+		return ERR_PTR(ret);
+
+	trailerlen = mv_cesa_ahash_pad_req(creq, ahashdreq->padding);
+
+	if (op) {
+		len = min(CESA_SA_SRAM_PAYLOAD_SIZE - dma_iter->base.op_len,
+			  trailerlen);
+		if (len) {
+			ret = mv_cesa_dma_add_data_transfer(chain,
+						CESA_SA_DATA_SRAM_OFFSET +
+						dma_iter->base.op_len,
+						ahashdreq->padding_dma,
+						len, CESA_TDMA_DST_IN_SRAM,
+						flags);
+			if (ret)
+				return ERR_PTR(ret);
+
+			mv_cesa_update_op_cfg(op, CESA_SA_DESC_CFG_MID_FRAG,
+					      CESA_SA_DESC_CFG_FRAG_MSK);
+			mv_cesa_set_mac_op_frag_len(op,
+					dma_iter->base.op_len + len);
+			padoff += len;
+		}
+	}
+
+	if (padoff >= trailerlen)
+		return op;
+
+	if ((mv_cesa_get_op_cfg(&creq->op_tmpl) & CESA_SA_DESC_CFG_FRAG_MSK) !=
+	    CESA_SA_DESC_CFG_FIRST_FRAG)
+		mv_cesa_update_op_cfg(&creq->op_tmpl,
+				      CESA_SA_DESC_CFG_MID_FRAG,
+				      CESA_SA_DESC_CFG_FRAG_MSK);
+
+	op = mv_cesa_dma_add_op(chain, &creq->op_tmpl, false, flags);
+	if (IS_ERR(op))
+		return op;
+
+	mv_cesa_set_mac_op_frag_len(op, trailerlen - padoff);
+
+	ret = mv_cesa_dma_add_data_transfer(chain,
+					    CESA_SA_DATA_SRAM_OFFSET,
+					    ahashdreq->padding_dma +
+					    padoff,
+					    trailerlen - padoff,
+					    CESA_TDMA_DST_IN_SRAM,
+					    flags);
+	if (ret)
+		return ERR_PTR(ret);
+
+	/* Add dummy desc to launch crypto operation */
+	ret = mv_cesa_dma_add_dummy_launch(chain, flags);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return op;
+}
+
+static int mv_cesa_ahash_dma_req_init(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	gfp_t flags = (req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP) ?
+		      GFP_KERNEL : GFP_ATOMIC;
+	struct mv_cesa_ahash_dma_req *ahashdreq = &creq->req.dma;
+	struct mv_cesa_tdma_req *dreq = &ahashdreq->base;
+	struct mv_cesa_tdma_chain chain;
+	struct mv_cesa_ahash_dma_iter iter;
+	struct mv_cesa_op_ctx *op = NULL;
+	int ret;
+
+	dreq->chain.first = NULL;
+	dreq->chain.last = NULL;
+
+	if (creq->src_nents) {
+		ret = dma_map_sg(cesa_dev->dev, req->src, creq->src_nents,
+				 DMA_TO_DEVICE);
+		if (!ret) {
+			ret = -ENOMEM;
+			goto err;
+		}
+	}
+
+	mv_cesa_tdma_desc_iter_init(&chain);
+	mv_cesa_ahash_req_iter_init(&iter, req);
+
+	op = mv_cesa_ahash_dma_add_cache(&chain, &iter,
+					 creq, flags);
+	if (IS_ERR(op)) {
+		ret = PTR_ERR(op);
+		goto err_free_tdma;
+	}
+
+	do {
+		if (!iter.base.op_len)
+			break;
+
+		op = mv_cesa_ahash_dma_add_data(&chain, &iter,
+						creq, flags);
+		if (IS_ERR(op)) {
+			ret = PTR_ERR(op);
+			goto err_free_tdma;
+		}
+	} while (mv_cesa_ahash_req_iter_next_op(&iter));
+
+	op = mv_cesa_ahash_dma_last_req(&chain, &iter, creq, op, flags);
+	if (IS_ERR(op)) {
+		ret = PTR_ERR(op);
+		goto err_free_tdma;
+	}
+
+	if (op) {
+		/* Add dummy desc to wait for crypto operation end */
+		ret = mv_cesa_dma_add_dummy_end(&chain, flags);
+		if (ret)
+			goto err_free_tdma;
+	}
+
+	if (!creq->last_req)
+		creq->cache_ptr = req->nbytes + creq->cache_ptr -
+				  iter.base.len;
+	else
+		creq->cache_ptr = 0;
+
+	dreq->chain = chain;
+
+	return 0;
+
+err_free_tdma:
+	mv_cesa_dma_cleanup(dreq);
+	dma_unmap_sg(cesa_dev->dev, req->src, creq->src_nents, DMA_TO_DEVICE);
+
+err:
+	mv_cesa_ahash_last_cleanup(req);
+
+	return ret;
+}
+
+static int mv_cesa_ahash_req_init(struct ahash_request *req, bool *cached)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	int ret;
+
+	if (cesa_dev->caps->has_tdma)
+		creq->req.base.type = CESA_DMA_REQ;
+	else
+		creq->req.base.type = CESA_STD_REQ;
+
+	creq->src_nents = sg_nents_for_len(req->src, req->nbytes);
+
+	ret = mv_cesa_ahash_cache_req(req, cached);
+	if (ret)
+		return ret;
+
+	if (*cached)
+		return 0;
+
+	if (creq->req.base.type == CESA_DMA_REQ)
+		ret = mv_cesa_ahash_dma_req_init(req);
+
+	return ret;
+}
+
+static int mv_cesa_ahash_update(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	bool cached = false;
+	int ret;
+
+	creq->len += req->nbytes;
+	ret = mv_cesa_ahash_req_init(req, &cached);
+	if (ret)
+		return ret;
+
+	if (cached)
+		return 0;
+
+	ret = mv_cesa_queue_req(&req->base);
+	if (ret && ret != -EINPROGRESS) {
+		mv_cesa_ahash_cleanup(req);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int mv_cesa_ahash_final(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	struct mv_cesa_op_ctx *tmpl = &creq->op_tmpl;
+	bool cached = false;
+	int ret;
+
+	mv_cesa_set_mac_op_total_len(tmpl, creq->len);
+	creq->last_req = true;
+	req->nbytes = 0;
+
+	ret = mv_cesa_ahash_req_init(req, &cached);
+	if (ret)
+		return ret;
+
+	if (cached)
+		return 0;
+
+	ret = mv_cesa_queue_req(&req->base);
+	if (ret && ret != -EINPROGRESS)
+		mv_cesa_ahash_cleanup(req);
+
+	return ret;
+}
+
+static int mv_cesa_ahash_finup(struct ahash_request *req)
+{
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	struct mv_cesa_op_ctx *tmpl = &creq->op_tmpl;
+	bool cached = false;
+	int ret;
+
+	creq->len += req->nbytes;
+	mv_cesa_set_mac_op_total_len(tmpl, creq->len);
+	creq->last_req = true;
+
+	ret = mv_cesa_ahash_req_init(req, &cached);
+	if (ret)
+		return ret;
+
+	if (cached)
+		return 0;
+
+	ret = mv_cesa_queue_req(&req->base);
+	if (ret && ret != -EINPROGRESS)
+		mv_cesa_ahash_cleanup(req);
+
+	return ret;
+}
+
+static int mv_cesa_md5_init(struct ahash_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_MACM_MD5);
+
+	mv_cesa_ahash_init(req, &tmpl);
+
+	return 0;
+}
+
+static int mv_cesa_md5_export(struct ahash_request *req, void *out)
+{
+	struct md5_state *out_state = out;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	unsigned int digsize = crypto_ahash_digestsize(ahash);
+
+	out_state->byte_count = creq->len;
+	memcpy(out_state->hash, creq->state, digsize);
+	memset(out_state->block, 0, sizeof(out_state->block));
+	if (creq->cache)
+		memcpy(out_state->block, creq->cache, creq->cache_ptr);
+
+	return 0;
+}
+
+static int mv_cesa_md5_import(struct ahash_request *req, const void *in)
+{
+	const struct md5_state *in_state = in;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	unsigned int digsize = crypto_ahash_digestsize(ahash);
+	unsigned int cache_ptr;
+	int ret;
+
+	creq->len = in_state->byte_count;
+	memcpy(creq->state, in_state->hash, digsize);
+	creq->cache_ptr = 0;
+
+	cache_ptr = creq->len % sizeof(in_state->block);
+	if (!cache_ptr)
+		return 0;
+
+	ret = mv_cesa_ahash_alloc_cache(req);
+	if (ret)
+		return ret;
+
+	memcpy(creq->cache, in_state->block, cache_ptr);
+	creq->cache_ptr = cache_ptr;
+
+	return 0;
+}
+
+static int mv_cesa_md5_digest(struct ahash_request *req)
+{
+	int ret;
+
+	ret = mv_cesa_md5_init(req);
+	if (ret)
+		return ret;
+
+	return mv_cesa_ahash_finup(req);
+}
+
+struct ahash_alg mv_md5_alg = {
+	.init = mv_cesa_md5_init,
+	.update = mv_cesa_ahash_update,
+	.final = mv_cesa_ahash_final,
+	.finup = mv_cesa_ahash_finup,
+	.digest = mv_cesa_md5_digest,
+	.export = mv_cesa_md5_export,
+	.import = mv_cesa_md5_import,
+	.halg = {
+		.digestsize = MD5_DIGEST_SIZE,
+		.base = {
+			.cra_name = "md5",
+			.cra_driver_name = "mv-md5",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_blocksize = MD5_HMAC_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct mv_cesa_hash_ctx),
+			.cra_init = mv_cesa_ahash_cra_init,
+			.cra_module = THIS_MODULE,
+		 }
+	}
+};
+
+static int mv_cesa_sha1_init(struct ahash_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_MACM_SHA1);
+
+	mv_cesa_ahash_init(req, &tmpl);
+
+	return 0;
+}
+
+static int mv_cesa_sha1_export(struct ahash_request *req, void *out)
+{
+	struct sha1_state *out_state = out;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	unsigned int digsize = crypto_ahash_digestsize(ahash);
+
+	out_state->count = creq->len;
+	memcpy(out_state->state, creq->state, digsize);
+	memset(out_state->buffer, 0, sizeof(out_state->buffer));
+	if (creq->cache)
+		memcpy(out_state->buffer, creq->cache, creq->cache_ptr);
+
+	return 0;
+}
+
+static int mv_cesa_sha1_import(struct ahash_request *req, const void *in)
+{
+	const struct sha1_state *in_state = in;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	unsigned int digsize = crypto_ahash_digestsize(ahash);
+	unsigned int cache_ptr;
+	int ret;
+
+	creq->len = in_state->count;
+	memcpy(creq->state, in_state->state, digsize);
+	creq->cache_ptr = 0;
+
+	cache_ptr = creq->len % SHA1_BLOCK_SIZE;
+	if (!cache_ptr)
+		return 0;
+
+	ret = mv_cesa_ahash_alloc_cache(req);
+	if (ret)
+		return ret;
+
+	memcpy(creq->cache, in_state->buffer, cache_ptr);
+	creq->cache_ptr = cache_ptr;
+
+	return 0;
+}
+
+static int mv_cesa_sha1_digest(struct ahash_request *req)
+{
+	int ret;
+
+	ret = mv_cesa_sha1_init(req);
+	if (ret)
+		return ret;
+
+	return mv_cesa_ahash_finup(req);
+}
+
+struct ahash_alg mv_sha1_alg = {
+	.init = mv_cesa_sha1_init,
+	.update = mv_cesa_ahash_update,
+	.final = mv_cesa_ahash_final,
+	.finup = mv_cesa_ahash_finup,
+	.digest = mv_cesa_sha1_digest,
+	.export = mv_cesa_sha1_export,
+	.import = mv_cesa_sha1_import,
+	.halg = {
+		.digestsize = SHA1_DIGEST_SIZE,
+		.base = {
+			.cra_name = "sha1",
+			.cra_driver_name = "mv-sha1",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_blocksize = SHA1_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct mv_cesa_hash_ctx),
+			.cra_init = mv_cesa_ahash_cra_init,
+			.cra_module = THIS_MODULE,
+		 }
+	}
+};
+
+static int mv_cesa_sha256_init(struct ahash_request *req)
+{
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_MACM_SHA256);
+
+	mv_cesa_ahash_init(req, &tmpl);
+
+	return 0;
+}
+
+static int mv_cesa_sha256_digest(struct ahash_request *req)
+{
+	int ret;
+
+	ret = mv_cesa_sha256_init(req);
+	if (ret)
+		return ret;
+
+	return mv_cesa_ahash_finup(req);
+}
+
+static int mv_cesa_sha256_export(struct ahash_request *req, void *out)
+{
+	struct sha256_state *out_state = out;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	unsigned int ds = crypto_ahash_digestsize(ahash);
+
+	out_state->count = creq->len;
+	memcpy(out_state->state, creq->state, ds);
+	memset(out_state->buf, 0, sizeof(out_state->buf));
+	if (creq->cache)
+		memcpy(out_state->buf, creq->cache, creq->cache_ptr);
+
+	return 0;
+}
+
+static int mv_cesa_sha256_import(struct ahash_request *req, const void *in)
+{
+	const struct sha256_state *in_state = in;
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct mv_cesa_ahash_req *creq = ahash_request_ctx(req);
+	unsigned int digsize = crypto_ahash_digestsize(ahash);
+	unsigned int cache_ptr;
+	int ret;
+
+	creq->len = in_state->count;
+	memcpy(creq->state, in_state->state, digsize);
+	creq->cache_ptr = 0;
+
+	cache_ptr = creq->len % SHA256_BLOCK_SIZE;
+	if (!cache_ptr)
+		return 0;
+
+	ret = mv_cesa_ahash_alloc_cache(req);
+	if (ret)
+		return ret;
+
+	memcpy(creq->cache, in_state->buf, cache_ptr);
+	creq->cache_ptr = cache_ptr;
+
+	return 0;
+}
+
+struct ahash_alg mv_sha256_alg = {
+	.init = mv_cesa_sha256_init,
+	.update = mv_cesa_ahash_update,
+	.final = mv_cesa_ahash_final,
+	.finup = mv_cesa_ahash_finup,
+	.digest = mv_cesa_sha256_digest,
+	.export = mv_cesa_sha256_export,
+	.import = mv_cesa_sha256_import,
+	.halg = {
+		.digestsize = SHA256_DIGEST_SIZE,
+		.base = {
+			.cra_name = "sha256",
+			.cra_driver_name = "mv-sha256",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_blocksize = SHA256_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct mv_cesa_hash_ctx),
+			.cra_init = mv_cesa_ahash_cra_init,
+			.cra_module = THIS_MODULE,
+		 }
+	}
+};
+
+struct mv_cesa_ahash_result {
+	struct completion completion;
+	int error;
+};
+
+static void mv_cesa_hmac_ahash_complete(struct crypto_async_request *req,
+					int error)
+{
+	struct mv_cesa_ahash_result *result = req->data;
+
+	if (error == -EINPROGRESS)
+		return;
+
+	result->error = error;
+	complete(&result->completion);
+}
+
+static int mv_cesa_ahmac_iv_state_init(struct ahash_request *req, u8 *pad,
+				       void *state, unsigned int blocksize)
+{
+	struct mv_cesa_ahash_result result;
+	struct scatterlist sg;
+	int ret;
+
+	ahash_request_set_callback(req, CRYPTO_TFM_REQ_MAY_BACKLOG,
+				   mv_cesa_hmac_ahash_complete, &result);
+	sg_init_one(&sg, pad, blocksize);
+	ahash_request_set_crypt(req, &sg, pad, blocksize);
+	init_completion(&result.completion);
+
+	ret = crypto_ahash_init(req);
+	if (ret)
+		return ret;
+
+	ret = crypto_ahash_update(req);
+	if (ret && ret != -EINPROGRESS)
+		return ret;
+
+	wait_for_completion_interruptible(&result.completion);
+	if (result.error)
+		return result.error;
+
+	ret = crypto_ahash_export(req, state);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mv_cesa_ahmac_pad_init(struct ahash_request *req,
+				  const u8 *key, unsigned int keylen,
+				  u8 *ipad, u8 *opad,
+				  unsigned int blocksize)
+{
+	struct mv_cesa_ahash_result result;
+	struct scatterlist sg;
+	int ret;
+	int i;
+
+	if (keylen <= blocksize) {
+		memcpy(ipad, key, keylen);
+	} else {
+		u8 *keydup = kmemdup(key, keylen, GFP_KERNEL);
+
+		if (!keydup)
+			return -ENOMEM;
+
+		ahash_request_set_callback(req, CRYPTO_TFM_REQ_MAY_BACKLOG,
+					   mv_cesa_hmac_ahash_complete,
+					   &result);
+		sg_init_one(&sg, keydup, keylen);
+		ahash_request_set_crypt(req, &sg, ipad, keylen);
+		init_completion(&result.completion);
+
+		ret = crypto_ahash_digest(req);
+		if (ret == -EINPROGRESS) {
+			wait_for_completion_interruptible(&result.completion);
+			ret = result.error;
+		}
+
+		/* Set the memory region to 0 to avoid any leak. */
+		memset(keydup, 0, keylen);
+		kfree(keydup);
+
+		if (ret)
+			return ret;
+
+		keylen = crypto_ahash_digestsize(crypto_ahash_reqtfm(req));
+	}
+
+	memset(ipad + keylen, 0, blocksize - keylen);
+	memcpy(opad, ipad, blocksize);
+
+	for (i = 0; i < blocksize; i++) {
+		ipad[i] ^= 0x36;
+		opad[i] ^= 0x5c;
+	}
+
+	return 0;
+}
+
+static int mv_cesa_ahmac_setkey(const char *hash_alg_name,
+				const u8 *key, unsigned int keylen,
+				void *istate, void *ostate)
+{
+	struct ahash_request *req;
+	struct crypto_ahash *tfm;
+	unsigned int blocksize;
+	u8 *ipad = NULL;
+	u8 *opad;
+	int ret;
+
+	tfm = crypto_alloc_ahash(hash_alg_name, CRYPTO_ALG_TYPE_AHASH,
+				 CRYPTO_ALG_TYPE_AHASH_MASK);
+	if (IS_ERR(tfm))
+		return PTR_ERR(tfm);
+
+	req = ahash_request_alloc(tfm, GFP_KERNEL);
+	if (!req) {
+		ret = -ENOMEM;
+		goto free_ahash;
+	}
+
+	crypto_ahash_clear_flags(tfm, ~0);
+
+	blocksize = crypto_tfm_alg_blocksize(crypto_ahash_tfm(tfm));
+
+	ipad = kzalloc(2 * blocksize, GFP_KERNEL);
+	if (!ipad) {
+		ret = -ENOMEM;
+		goto free_req;
+	}
+
+	opad = ipad + blocksize;
+
+	ret = mv_cesa_ahmac_pad_init(req, key, keylen, ipad, opad, blocksize);
+	if (ret)
+		goto free_ipad;
+
+	ret = mv_cesa_ahmac_iv_state_init(req, ipad, istate, blocksize);
+	if (ret)
+		goto free_ipad;
+
+	ret = mv_cesa_ahmac_iv_state_init(req, opad, ostate, blocksize);
+
+free_ipad:
+	kfree(ipad);
+free_req:
+	ahash_request_free(req);
+free_ahash:
+	crypto_free_ahash(tfm);
+
+	return ret;
+}
+
+static int mv_cesa_ahmac_cra_init(struct crypto_tfm *tfm)
+{
+	struct mv_cesa_hmac_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	ctx->base.ops = &mv_cesa_ahash_req_ops;
+
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct mv_cesa_ahash_req));
+	return 0;
+}
+
+static int mv_cesa_ahmac_md5_init(struct ahash_request *req)
+{
+	struct mv_cesa_hmac_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_MACM_HMAC_MD5);
+	memcpy(tmpl.ctx.hash.iv, ctx->iv, sizeof(ctx->iv));
+
+	mv_cesa_ahash_init(req, &tmpl);
+
+	return 0;
+}
+
+static int mv_cesa_ahmac_md5_setkey(struct crypto_ahash *tfm, const u8 *key,
+				    unsigned int keylen)
+{
+	struct mv_cesa_hmac_ctx *ctx = crypto_tfm_ctx(crypto_ahash_tfm(tfm));
+	struct md5_state istate, ostate;
+	int ret, i;
+
+	ret = mv_cesa_ahmac_setkey("mv-md5", key, keylen, &istate, &ostate);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(istate.hash); i++)
+		ctx->iv[i] = be32_to_cpu(istate.hash[i]);
+
+	for (i = 0; i < ARRAY_SIZE(ostate.hash); i++)
+		ctx->iv[i + 8] = be32_to_cpu(ostate.hash[i]);
+
+	return 0;
+}
+
+static int mv_cesa_ahmac_md5_digest(struct ahash_request *req)
+{
+	int ret;
+
+	ret = mv_cesa_ahmac_md5_init(req);
+	if (ret)
+		return ret;
+
+	return mv_cesa_ahash_finup(req);
+}
+
+struct ahash_alg mv_ahmac_md5_alg = {
+	.init = mv_cesa_ahmac_md5_init,
+	.update = mv_cesa_ahash_update,
+	.final = mv_cesa_ahash_final,
+	.finup = mv_cesa_ahash_finup,
+	.digest = mv_cesa_ahmac_md5_digest,
+	.setkey = mv_cesa_ahmac_md5_setkey,
+	.export = mv_cesa_md5_export,
+	.import = mv_cesa_md5_import,
+	.halg = {
+		.digestsize = MD5_DIGEST_SIZE,
+		.statesize = sizeof(struct md5_state),
+		.base = {
+			.cra_name = "hmac(md5)",
+			.cra_driver_name = "mv-hmac-md5",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_blocksize = MD5_HMAC_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct mv_cesa_hmac_ctx),
+			.cra_init = mv_cesa_ahmac_cra_init,
+			.cra_module = THIS_MODULE,
+		 }
+	}
+};
+
+static int mv_cesa_ahmac_sha1_init(struct ahash_request *req)
+{
+	struct mv_cesa_hmac_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_MACM_HMAC_SHA1);
+	memcpy(tmpl.ctx.hash.iv, ctx->iv, sizeof(ctx->iv));
+
+	mv_cesa_ahash_init(req, &tmpl);
+
+	return 0;
+}
+
+static int mv_cesa_ahmac_sha1_setkey(struct crypto_ahash *tfm, const u8 *key,
+				     unsigned int keylen)
+{
+	struct mv_cesa_hmac_ctx *ctx = crypto_tfm_ctx(crypto_ahash_tfm(tfm));
+	struct sha1_state istate, ostate;
+	int ret, i;
+
+	ret = mv_cesa_ahmac_setkey("mv-sha1", key, keylen, &istate, &ostate);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(istate.state); i++)
+		ctx->iv[i] = be32_to_cpu(istate.state[i]);
+
+	for (i = 0; i < ARRAY_SIZE(ostate.state); i++)
+		ctx->iv[i + 8] = be32_to_cpu(ostate.state[i]);
+
+	return 0;
+}
+
+static int mv_cesa_ahmac_sha1_digest(struct ahash_request *req)
+{
+	int ret;
+
+	ret = mv_cesa_ahmac_sha1_init(req);
+	if (ret)
+		return ret;
+
+	return mv_cesa_ahash_finup(req);
+}
+
+struct ahash_alg mv_ahmac_sha1_alg = {
+	.init = mv_cesa_ahmac_sha1_init,
+	.update = mv_cesa_ahash_update,
+	.final = mv_cesa_ahash_final,
+	.finup = mv_cesa_ahash_finup,
+	.digest = mv_cesa_ahmac_sha1_digest,
+	.setkey = mv_cesa_ahmac_sha1_setkey,
+	.export = mv_cesa_sha1_export,
+	.import = mv_cesa_sha1_import,
+	.halg = {
+		.digestsize = SHA1_DIGEST_SIZE,
+		.statesize = sizeof(struct sha1_state),
+		.base = {
+			.cra_name = "hmac(sha1)",
+			.cra_driver_name = "mv-hmac-sha1",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_blocksize = SHA1_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct mv_cesa_hmac_ctx),
+			.cra_init = mv_cesa_ahmac_cra_init,
+			.cra_module = THIS_MODULE,
+		 }
+	}
+};
+
+static int mv_cesa_ahmac_sha256_setkey(struct crypto_ahash *tfm, const u8 *key,
+				       unsigned int keylen)
+{
+	struct mv_cesa_hmac_ctx *ctx = crypto_tfm_ctx(crypto_ahash_tfm(tfm));
+	struct sha256_state istate, ostate;
+	int ret, i;
+
+	ret = mv_cesa_ahmac_setkey("mv-sha256", key, keylen, &istate, &ostate);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(istate.state); i++)
+		ctx->iv[i] = be32_to_cpu(istate.state[i]);
+
+	for (i = 0; i < ARRAY_SIZE(ostate.state); i++)
+		ctx->iv[i + 8] = be32_to_cpu(ostate.state[i]);
+
+	return 0;
+}
+
+static int mv_cesa_ahmac_sha256_init(struct ahash_request *req)
+{
+	struct mv_cesa_hmac_ctx *ctx = crypto_tfm_ctx(req->base.tfm);
+	struct mv_cesa_op_ctx tmpl;
+
+	mv_cesa_set_op_cfg(&tmpl, CESA_SA_DESC_CFG_MACM_HMAC_SHA256);
+	memcpy(tmpl.ctx.hash.iv, ctx->iv, sizeof(ctx->iv));
+
+	mv_cesa_ahash_init(req, &tmpl);
+
+	return 0;
+}
+
+static int mv_cesa_ahmac_sha256_digest(struct ahash_request *req)
+{
+	int ret;
+
+	ret = mv_cesa_ahmac_sha256_init(req);
+	if (ret)
+		return ret;
+
+	return mv_cesa_ahash_finup(req);
+}
+
+struct ahash_alg mv_ahmac_sha256_alg = {
+	.init = mv_cesa_ahmac_sha256_init,
+	.update = mv_cesa_ahash_update,
+	.final = mv_cesa_ahash_final,
+	.finup = mv_cesa_ahash_finup,
+	.digest = mv_cesa_ahmac_sha256_digest,
+	.setkey = mv_cesa_ahmac_sha256_setkey,
+	.export = mv_cesa_sha256_export,
+	.import = mv_cesa_sha256_import,
+	.halg = {
+		.digestsize = SHA256_DIGEST_SIZE,
+		.statesize = sizeof(struct sha256_state),
+		.base = {
+			.cra_name = "hmac(sha256)",
+			.cra_driver_name = "mv-hmac-sha256",
+			.cra_priority = 300,
+			.cra_flags = CRYPTO_ALG_ASYNC |
+				     CRYPTO_ALG_KERN_DRIVER_ONLY,
+			.cra_blocksize = SHA256_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct mv_cesa_hmac_ctx),
+			.cra_init = mv_cesa_ahmac_cra_init,
+			.cra_module = THIS_MODULE,
+		 }
+	}
+};
diff --git a/drivers/crypto/marvell/tdma.c b/drivers/crypto/marvell/tdma.c
new file mode 100644
index 000000000000..64a366c50174
--- /dev/null
+++ b/drivers/crypto/marvell/tdma.c
@@ -0,0 +1,224 @@
+/*
+ * Provide TDMA helper functions used by cipher and hash algorithm
+ * implementations.
+ *
+ * Author: Boris Brezillon <boris.brezillon@free-electrons.com>
+ * Author: Arnaud Ebalard <arno@natisbad.org>
+ *
+ * This work is based on an initial version written by
+ * Sebastian Andrzej Siewior < sebastian at breakpoint dot cc >
+ *
+ * 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 "cesa.h"
+
+bool mv_cesa_req_dma_iter_next_transfer(struct mv_cesa_dma_iter *iter,
+					struct mv_cesa_sg_dma_iter *sgiter,
+					unsigned int len)
+{
+	if (!sgiter->sg)
+		return false;
+
+	sgiter->op_offset += len;
+	sgiter->offset += len;
+	if (sgiter->offset == sg_dma_len(sgiter->sg)) {
+		if (sg_is_last(sgiter->sg))
+			return false;
+		sgiter->offset = 0;
+		sgiter->sg = sg_next(sgiter->sg);
+	}
+
+	if (sgiter->op_offset == iter->op_len)
+		return false;
+
+	return true;
+}
+
+void mv_cesa_dma_step(struct mv_cesa_tdma_req *dreq)
+{
+	struct mv_cesa_engine *engine = dreq->base.engine;
+
+	writel(0, engine->regs + CESA_SA_CFG);
+
+	mv_cesa_set_int_mask(engine, CESA_SA_INT_ACC0_IDMA_DONE);
+	writel(CESA_TDMA_DST_BURST_128B | CESA_TDMA_SRC_BURST_128B |
+	       CESA_TDMA_NO_BYTE_SWAP | CESA_TDMA_EN,
+	       engine->regs + CESA_TDMA_CONTROL);
+
+	writel(CESA_SA_CFG_ACT_CH0_IDMA | CESA_SA_CFG_MULTI_PKT |
+	       CESA_SA_CFG_CH0_W_IDMA | CESA_SA_CFG_PARA_DIS,
+	       engine->regs + CESA_SA_CFG);
+	writel(dreq->chain.first->cur_dma,
+	       engine->regs + CESA_TDMA_NEXT_ADDR);
+	writel(CESA_SA_CMD_EN_CESA_SA_ACCL0, engine->regs + CESA_SA_CMD);
+}
+
+void mv_cesa_dma_cleanup(struct mv_cesa_tdma_req *dreq)
+{
+	struct mv_cesa_tdma_desc *tdma;
+
+	for (tdma = dreq->chain.first; tdma;) {
+		struct mv_cesa_tdma_desc *old_tdma = tdma;
+
+		if (tdma->flags & CESA_TDMA_OP)
+			dma_pool_free(cesa_dev->dma->op_pool, tdma->op,
+				      le32_to_cpu(tdma->src));
+
+		tdma = tdma->next;
+		dma_pool_free(cesa_dev->dma->tdma_desc_pool, old_tdma,
+			      le32_to_cpu(old_tdma->cur_dma));
+	}
+
+	dreq->chain.first = NULL;
+	dreq->chain.last = NULL;
+}
+
+void mv_cesa_dma_prepare(struct mv_cesa_tdma_req *dreq,
+			 struct mv_cesa_engine *engine)
+{
+	struct mv_cesa_tdma_desc *tdma;
+
+	for (tdma = dreq->chain.first; tdma; tdma = tdma->next) {
+		if (tdma->flags & CESA_TDMA_DST_IN_SRAM)
+			tdma->dst = cpu_to_le32(tdma->dst + engine->sram_dma);
+
+		if (tdma->flags & CESA_TDMA_SRC_IN_SRAM)
+			tdma->src = cpu_to_le32(tdma->src + engine->sram_dma);
+
+		if (tdma->flags & CESA_TDMA_OP)
+			mv_cesa_adjust_op(engine, tdma->op);
+	}
+}
+
+static struct mv_cesa_tdma_desc *
+mv_cesa_dma_add_desc(struct mv_cesa_tdma_chain *chain, gfp_t flags)
+{
+	struct mv_cesa_tdma_desc *new_tdma = NULL;
+	dma_addr_t dma_handle;
+
+	new_tdma = dma_pool_alloc(cesa_dev->dma->tdma_desc_pool, flags,
+				  &dma_handle);
+	if (!new_tdma)
+		return ERR_PTR(-ENOMEM);
+
+	memset(new_tdma, 0, sizeof(*new_tdma));
+	new_tdma->cur_dma = cpu_to_le32(dma_handle);
+	if (chain->last) {
+		chain->last->next_dma = new_tdma->cur_dma;
+		chain->last->next = new_tdma;
+	} else {
+		chain->first = new_tdma;
+	}
+
+	chain->last = new_tdma;
+
+	return new_tdma;
+}
+
+struct mv_cesa_op_ctx *mv_cesa_dma_add_op(struct mv_cesa_tdma_chain *chain,
+					const struct mv_cesa_op_ctx *op_templ,
+					bool skip_ctx,
+					gfp_t flags)
+{
+	struct mv_cesa_tdma_desc *tdma;
+	struct mv_cesa_op_ctx *op;
+	dma_addr_t dma_handle;
+
+	tdma = mv_cesa_dma_add_desc(chain, flags);
+	if (IS_ERR(tdma))
+		return ERR_CAST(tdma);
+
+	op = dma_pool_alloc(cesa_dev->dma->op_pool, flags, &dma_handle);
+	if (!op)
+		return ERR_PTR(-ENOMEM);
+
+	*op = *op_templ;
+
+	tdma = chain->last;
+	tdma->op = op;
+	tdma->byte_cnt = (skip_ctx ? sizeof(op->desc) : sizeof(*op)) | BIT(31);
+	tdma->src = dma_handle;
+	tdma->flags = CESA_TDMA_DST_IN_SRAM | CESA_TDMA_OP;
+
+	return op;
+}
+
+int mv_cesa_dma_add_data_transfer(struct mv_cesa_tdma_chain *chain,
+				  dma_addr_t dst, dma_addr_t src, u32 size,
+				  u32 flags, gfp_t gfp_flags)
+{
+	struct mv_cesa_tdma_desc *tdma;
+
+	tdma = mv_cesa_dma_add_desc(chain, gfp_flags);
+	if (IS_ERR(tdma))
+		return PTR_ERR(tdma);
+
+	tdma->byte_cnt = size | BIT(31);
+	tdma->src = src;
+	tdma->dst = dst;
+
+	flags &= (CESA_TDMA_DST_IN_SRAM | CESA_TDMA_SRC_IN_SRAM);
+	tdma->flags = flags | CESA_TDMA_DATA;
+
+	return 0;
+}
+
+int mv_cesa_dma_add_dummy_launch(struct mv_cesa_tdma_chain *chain,
+				 u32 flags)
+{
+	struct mv_cesa_tdma_desc *tdma;
+
+	tdma = mv_cesa_dma_add_desc(chain, flags);
+	if (IS_ERR(tdma))
+		return PTR_ERR(tdma);
+
+	return 0;
+}
+
+int mv_cesa_dma_add_dummy_end(struct mv_cesa_tdma_chain *chain, u32 flags)
+{
+	struct mv_cesa_tdma_desc *tdma;
+
+	tdma = mv_cesa_dma_add_desc(chain, flags);
+	if (IS_ERR(tdma))
+		return PTR_ERR(tdma);
+
+	tdma->byte_cnt = BIT(31);
+
+	return 0;
+}
+
+int mv_cesa_dma_add_op_transfers(struct mv_cesa_tdma_chain *chain,
+				 struct mv_cesa_dma_iter *dma_iter,
+				 struct mv_cesa_sg_dma_iter *sgiter,
+				 gfp_t gfp_flags)
+{
+	u32 flags = sgiter->dir == DMA_TO_DEVICE ?
+		    CESA_TDMA_DST_IN_SRAM : CESA_TDMA_SRC_IN_SRAM;
+	unsigned int len;
+
+	do {
+		dma_addr_t dst, src;
+		int ret;
+
+		len = mv_cesa_req_dma_iter_transfer_len(dma_iter, sgiter);
+		if (sgiter->dir == DMA_TO_DEVICE) {
+			dst = CESA_SA_DATA_SRAM_OFFSET + sgiter->op_offset;
+			src = sg_dma_address(sgiter->sg) + sgiter->offset;
+		} else {
+			dst = sg_dma_address(sgiter->sg) + sgiter->offset;
+			src = CESA_SA_DATA_SRAM_OFFSET + sgiter->op_offset;
+		}
+
+		ret = mv_cesa_dma_add_data_transfer(chain, dst, src, len,
+						    flags, gfp_flags);
+		if (ret)
+			return ret;
+
+	} while (mv_cesa_req_dma_iter_next_transfer(dma_iter, sgiter, len));
+
+	return 0;
+}
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index f91f15ddee92..e6b658faef63 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -9,6 +9,7 @@
 #include <crypto/aes.h>
 #include <crypto/algapi.h>
 #include <linux/crypto.h>
+#include <linux/genalloc.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/kthread.h>
@@ -29,6 +30,8 @@
 #define MAX_HW_HASH_SIZE	0xFFFF
 #define MV_CESA_EXPIRE		500 /* msec */
 
+#define MV_CESA_DEFAULT_SRAM_SIZE	2048
+
 /*
  * STM:
  *   /---------------------------------------\
@@ -83,6 +86,8 @@ struct req_progress {
 struct crypto_priv {
 	void __iomem *reg;
 	void __iomem *sram;
+	struct gen_pool *sram_pool;
+	dma_addr_t sram_dma;
 	int irq;
 	struct clk *clk;
 	struct task_struct *queue_th;
@@ -595,7 +600,7 @@ static int queue_manag(void *data)
 	cpg->eng_st = ENGINE_IDLE;
 	do {
 		struct crypto_async_request *async_req = NULL;
-		struct crypto_async_request *backlog;
+		struct crypto_async_request *backlog = NULL;
 
 		__set_current_state(TASK_INTERRUPTIBLE);
 
@@ -1019,6 +1024,39 @@ static struct ahash_alg mv_hmac_sha1_alg = {
 		 }
 };
 
+static int mv_cesa_get_sram(struct platform_device *pdev,
+			    struct crypto_priv *cp)
+{
+	struct resource *res;
+	u32 sram_size = MV_CESA_DEFAULT_SRAM_SIZE;
+
+	of_property_read_u32(pdev->dev.of_node, "marvell,crypto-sram-size",
+			     &sram_size);
+
+	cp->sram_size = sram_size;
+	cp->sram_pool = of_gen_pool_get(pdev->dev.of_node,
+					"marvell,crypto-srams", 0);
+	if (cp->sram_pool) {
+		cp->sram = gen_pool_dma_alloc(cp->sram_pool, sram_size,
+					      &cp->sram_dma);
+		if (cp->sram)
+			return 0;
+
+		return -ENOMEM;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "sram");
+	if (!res || resource_size(res) < cp->sram_size)
+		return -EINVAL;
+
+	cp->sram = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(cp->sram))
+		return PTR_ERR(cp->sram);
+
+	return 0;
+}
+
 static int mv_probe(struct platform_device *pdev)
 {
 	struct crypto_priv *cp;
@@ -1041,24 +1079,17 @@ static int mv_probe(struct platform_device *pdev)
 
 	spin_lock_init(&cp->lock);
 	crypto_init_queue(&cp->queue, 50);
-	cp->reg = ioremap(res->start, resource_size(res));
-	if (!cp->reg) {
-		ret = -ENOMEM;
+	cp->reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(cp->reg)) {
+		ret = PTR_ERR(cp->reg);
 		goto err;
 	}
 
-	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sram");
-	if (!res) {
-		ret = -ENXIO;
-		goto err_unmap_reg;
-	}
-	cp->sram_size = resource_size(res);
+	ret = mv_cesa_get_sram(pdev, cp);
+	if (ret)
+		goto err;
+
 	cp->max_req_size = cp->sram_size - SRAM_CFG_SPACE;
-	cp->sram = ioremap(res->start, cp->sram_size);
-	if (!cp->sram) {
-		ret = -ENOMEM;
-		goto err_unmap_reg;
-	}
 
 	if (pdev->dev.of_node)
 		irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
@@ -1066,7 +1097,7 @@ static int mv_probe(struct platform_device *pdev)
 		irq = platform_get_irq(pdev, 0);
 	if (irq < 0 || irq == NO_IRQ) {
 		ret = irq;
-		goto err_unmap_sram;
+		goto err;
 	}
 	cp->irq = irq;
 
@@ -1076,7 +1107,7 @@ static int mv_probe(struct platform_device *pdev)
 	cp->queue_th = kthread_run(queue_manag, cp, "mv_crypto");
 	if (IS_ERR(cp->queue_th)) {
 		ret = PTR_ERR(cp->queue_th);
-		goto err_unmap_sram;
+		goto err;
 	}
 
 	ret = request_irq(irq, crypto_int, 0, dev_name(&pdev->dev),
@@ -1134,10 +1165,6 @@ err_irq:
 	}
 err_thread:
 	kthread_stop(cp->queue_th);
-err_unmap_sram:
-	iounmap(cp->sram);
-err_unmap_reg:
-	iounmap(cp->reg);
 err:
 	kfree(cp);
 	cpg = NULL;
@@ -1157,8 +1184,6 @@ static int mv_remove(struct platform_device *pdev)
 	kthread_stop(cp->queue_th);
 	free_irq(cp->irq, cp);
 	memset(cp->sram, 0, cp->sram_size);
-	iounmap(cp->sram);
-	iounmap(cp->reg);
 
 	if (!IS_ERR(cp->clk)) {
 		clk_disable_unprepare(cp->clk);
@@ -1172,6 +1197,8 @@ static int mv_remove(struct platform_device *pdev)
 
 static const struct of_device_id mv_cesa_of_match_table[] = {
 	{ .compatible = "marvell,orion-crypto", },
+	{ .compatible = "marvell,kirkwood-crypto", },
+	{ .compatible = "marvell,dove-crypto", },
 	{}
 };
 MODULE_DEVICE_TABLE(of, mv_cesa_of_match_table);
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index 10a9aeff1666..2e8dab9d4263 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -1281,10 +1281,10 @@ static const char md5_zero[MD5_DIGEST_SIZE] = {
 	0xe9, 0x80, 0x09, 0x98, 0xec, 0xf8, 0x42, 0x7e,
 };
 static const u32 md5_init[MD5_HASH_WORDS] = {
-	cpu_to_le32(0x67452301),
-	cpu_to_le32(0xefcdab89),
-	cpu_to_le32(0x98badcfe),
-	cpu_to_le32(0x10325476),
+	cpu_to_le32(MD5_H0),
+	cpu_to_le32(MD5_H1),
+	cpu_to_le32(MD5_H2),
+	cpu_to_le32(MD5_H3),
 };
 static const char sha1_zero[SHA1_DIGEST_SIZE] = {
 	0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b, 0x0d, 0x32,
diff --git a/drivers/crypto/nx/Kconfig b/drivers/crypto/nx/Kconfig
index f82616621ae1..e421c96c763a 100644
--- a/drivers/crypto/nx/Kconfig
+++ b/drivers/crypto/nx/Kconfig
@@ -1,26 +1,55 @@
+
 config CRYPTO_DEV_NX_ENCRYPT
-	tristate "Encryption acceleration support"
-	depends on PPC64 && IBMVIO
+	tristate "Encryption acceleration support on pSeries platform"
+	depends on PPC_PSERIES && IBMVIO && !CPU_LITTLE_ENDIAN
 	default y
 	select CRYPTO_AES
-	select CRYPTO_CBC
-	select CRYPTO_ECB
 	select CRYPTO_CCM
-	select CRYPTO_GCM
-	select CRYPTO_AUTHENC
-	select CRYPTO_XCBC
-	select CRYPTO_SHA256
-	select CRYPTO_SHA512
 	help
-	  Support for Power7+ in-Nest encryption acceleration. This
-	  module supports acceleration for AES and SHA2 algorithms. If you
-	  choose 'M' here, this module will be called nx_crypto.
+	  Support for PowerPC Nest (NX) encryption acceleration. This
+	  module supports acceleration for AES and SHA2 algorithms on
+	  the pSeries platform.  If you choose 'M' here, this module
+	  will be called nx_crypto.
 
 config CRYPTO_DEV_NX_COMPRESS
 	tristate "Compression acceleration support"
-	depends on PPC64 && IBMVIO
 	default y
 	help
-	  Support for Power7+ in-Nest compression acceleration. This
-	  module supports acceleration for AES and SHA2 algorithms. If you
-	  choose 'M' here, this module will be called nx_compress.
+	  Support for PowerPC Nest (NX) compression acceleration. This
+	  module supports acceleration for compressing memory with the 842
+	  algorithm.  One of the platform drivers must be selected also.
+	  If you choose 'M' here, this module will be called nx_compress.
+
+if CRYPTO_DEV_NX_COMPRESS
+
+config CRYPTO_DEV_NX_COMPRESS_PSERIES
+	tristate "Compression acceleration support on pSeries platform"
+	depends on PPC_PSERIES && IBMVIO
+	default y
+	help
+	  Support for PowerPC Nest (NX) compression acceleration. This
+	  module supports acceleration for compressing memory with the 842
+	  algorithm.  This supports NX hardware on the pSeries platform.
+	  If you choose 'M' here, this module will be called nx_compress_pseries.
+
+config CRYPTO_DEV_NX_COMPRESS_POWERNV
+	tristate "Compression acceleration support on PowerNV platform"
+	depends on PPC_POWERNV
+	default y
+	help
+	  Support for PowerPC Nest (NX) compression acceleration. This
+	  module supports acceleration for compressing memory with the 842
+	  algorithm.  This supports NX hardware on the PowerNV platform.
+	  If you choose 'M' here, this module will be called nx_compress_powernv.
+
+config CRYPTO_DEV_NX_COMPRESS_CRYPTO
+	tristate "Compression acceleration cryptographic interface"
+	select CRYPTO_ALGAPI
+	select 842_DECOMPRESS
+	default y
+	help
+	  Support for PowerPC Nest (NX) accelerators using the cryptographic
+	  API.  If you choose 'M' here, this module will be called
+	  nx_compress_crypto.
+
+endif
diff --git a/drivers/crypto/nx/Makefile b/drivers/crypto/nx/Makefile
index bb770ea45ce9..e1684f5adb11 100644
--- a/drivers/crypto/nx/Makefile
+++ b/drivers/crypto/nx/Makefile
@@ -10,5 +10,12 @@ nx-crypto-objs := nx.o \
 		  nx-sha256.o \
 		  nx-sha512.o
 
-obj-$(CONFIG_CRYPTO_DEV_NX_COMPRESS) += nx-compress.o
+obj-$(CONFIG_CRYPTO_DEV_NX_COMPRESS) += nx-compress.o nx-compress-platform.o
+obj-$(CONFIG_CRYPTO_DEV_NX_COMPRESS_PSERIES) += nx-compress-pseries.o
+obj-$(CONFIG_CRYPTO_DEV_NX_COMPRESS_POWERNV) += nx-compress-powernv.o
+obj-$(CONFIG_CRYPTO_DEV_NX_COMPRESS_CRYPTO) += nx-compress-crypto.o
 nx-compress-objs := nx-842.o
+nx-compress-platform-objs := nx-842-platform.o
+nx-compress-pseries-objs := nx-842-pseries.o
+nx-compress-powernv-objs := nx-842-powernv.o
+nx-compress-crypto-objs := nx-842-crypto.o
diff --git a/drivers/crypto/nx/nx-842-crypto.c b/drivers/crypto/nx/nx-842-crypto.c
new file mode 100644
index 000000000000..d53a1dcd7b4e
--- /dev/null
+++ b/drivers/crypto/nx/nx-842-crypto.c
@@ -0,0 +1,580 @@
+/*
+ * Cryptographic API for the NX-842 hardware compression.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under 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.
+ *
+ * Copyright (C) IBM Corporation, 2011-2015
+ *
+ * Original Authors: Robert Jennings <rcj@linux.vnet.ibm.com>
+ *                   Seth Jennings <sjenning@linux.vnet.ibm.com>
+ *
+ * Rewrite: Dan Streetman <ddstreet@ieee.org>
+ *
+ * This is an interface to the NX-842 compression hardware in PowerPC
+ * processors.  Most of the complexity of this drvier is due to the fact that
+ * the NX-842 compression hardware requires the input and output data buffers
+ * to be specifically aligned, to be a specific multiple in length, and within
+ * specific minimum and maximum lengths.  Those restrictions, provided by the
+ * nx-842 driver via nx842_constraints, mean this driver must use bounce
+ * buffers and headers to correct misaligned in or out buffers, and to split
+ * input buffers that are too large.
+ *
+ * This driver will fall back to software decompression if the hardware
+ * decompression fails, so this driver's decompression should never fail as
+ * long as the provided compressed buffer is valid.  Any compressed buffer
+ * created by this driver will have a header (except ones where the input
+ * perfectly matches the constraints); so users of this driver cannot simply
+ * pass a compressed buffer created by this driver over to the 842 software
+ * decompression library.  Instead, users must use this driver to decompress;
+ * if the hardware fails or is unavailable, the compressed buffer will be
+ * parsed and the header removed, and the raw 842 buffer(s) passed to the 842
+ * software decompression library.
+ *
+ * This does not fall back to software compression, however, since the caller
+ * of this function is specifically requesting hardware compression; if the
+ * hardware compression fails, the caller can fall back to software
+ * compression, and the raw 842 compressed buffer that the software compressor
+ * creates can be passed to this driver for hardware decompression; any
+ * buffer without our specific header magic is assumed to be a raw 842 buffer
+ * and passed directly to the hardware.  Note that the software compression
+ * library will produce a compressed buffer that is incompatible with the
+ * hardware decompressor if the original input buffer length is not a multiple
+ * of 8; if such a compressed buffer is passed to this driver for
+ * decompression, the hardware will reject it and this driver will then pass
+ * it over to the software library for decompression.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/crypto.h>
+#include <linux/vmalloc.h>
+#include <linux/sw842.h>
+#include <linux/ratelimit.h>
+
+#include "nx-842.h"
+
+/* The first 5 bits of this magic are 0x1f, which is an invalid 842 5-bit
+ * template (see lib/842/842.h), so this magic number will never appear at
+ * the start of a raw 842 compressed buffer.  That is important, as any buffer
+ * passed to us without this magic is assumed to be a raw 842 compressed
+ * buffer, and passed directly to the hardware to decompress.
+ */
+#define NX842_CRYPTO_MAGIC	(0xf842)
+#define NX842_CRYPTO_GROUP_MAX	(0x20)
+#define NX842_CRYPTO_HEADER_SIZE(g)				\
+	(sizeof(struct nx842_crypto_header) +			\
+	 sizeof(struct nx842_crypto_header_group) * (g))
+#define NX842_CRYPTO_HEADER_MAX_SIZE				\
+	NX842_CRYPTO_HEADER_SIZE(NX842_CRYPTO_GROUP_MAX)
+
+/* bounce buffer size */
+#define BOUNCE_BUFFER_ORDER	(2)
+#define BOUNCE_BUFFER_SIZE					\
+	((unsigned int)(PAGE_SIZE << BOUNCE_BUFFER_ORDER))
+
+/* try longer on comp because we can fallback to sw decomp if hw is busy */
+#define COMP_BUSY_TIMEOUT	(250) /* ms */
+#define DECOMP_BUSY_TIMEOUT	(50) /* ms */
+
+struct nx842_crypto_header_group {
+	__be16 padding;			/* unused bytes at start of group */
+	__be32 compressed_length;	/* compressed bytes in group */
+	__be32 uncompressed_length;	/* bytes after decompression */
+} __packed;
+
+struct nx842_crypto_header {
+	__be16 magic;		/* NX842_CRYPTO_MAGIC */
+	__be16 ignore;		/* decompressed end bytes to ignore */
+	u8 groups;		/* total groups in this header */
+	struct nx842_crypto_header_group group[];
+} __packed;
+
+struct nx842_crypto_param {
+	u8 *in;
+	unsigned int iremain;
+	u8 *out;
+	unsigned int oremain;
+	unsigned int ototal;
+};
+
+static int update_param(struct nx842_crypto_param *p,
+			unsigned int slen, unsigned int dlen)
+{
+	if (p->iremain < slen)
+		return -EOVERFLOW;
+	if (p->oremain < dlen)
+		return -ENOSPC;
+
+	p->in += slen;
+	p->iremain -= slen;
+	p->out += dlen;
+	p->oremain -= dlen;
+	p->ototal += dlen;
+
+	return 0;
+}
+
+struct nx842_crypto_ctx {
+	u8 *wmem;
+	u8 *sbounce, *dbounce;
+
+	struct nx842_crypto_header header;
+	struct nx842_crypto_header_group group[NX842_CRYPTO_GROUP_MAX];
+};
+
+static int nx842_crypto_init(struct crypto_tfm *tfm)
+{
+	struct nx842_crypto_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	ctx->wmem = kmalloc(nx842_workmem_size(), GFP_KERNEL);
+	ctx->sbounce = (u8 *)__get_free_pages(GFP_KERNEL, BOUNCE_BUFFER_ORDER);
+	ctx->dbounce = (u8 *)__get_free_pages(GFP_KERNEL, BOUNCE_BUFFER_ORDER);
+	if (!ctx->wmem || !ctx->sbounce || !ctx->dbounce) {
+		kfree(ctx->wmem);
+		free_page((unsigned long)ctx->sbounce);
+		free_page((unsigned long)ctx->dbounce);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void nx842_crypto_exit(struct crypto_tfm *tfm)
+{
+	struct nx842_crypto_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	kfree(ctx->wmem);
+	free_page((unsigned long)ctx->sbounce);
+	free_page((unsigned long)ctx->dbounce);
+}
+
+static int read_constraints(struct nx842_constraints *c)
+{
+	int ret;
+
+	ret = nx842_constraints(c);
+	if (ret) {
+		pr_err_ratelimited("could not get nx842 constraints : %d\n",
+				   ret);
+		return ret;
+	}
+
+	/* limit maximum, to always have enough bounce buffer to decompress */
+	if (c->maximum > BOUNCE_BUFFER_SIZE) {
+		c->maximum = BOUNCE_BUFFER_SIZE;
+		pr_info_once("limiting nx842 maximum to %x\n", c->maximum);
+	}
+
+	return 0;
+}
+
+static int nx842_crypto_add_header(struct nx842_crypto_header *hdr, u8 *buf)
+{
+	int s = NX842_CRYPTO_HEADER_SIZE(hdr->groups);
+
+	/* compress should have added space for header */
+	if (s > be16_to_cpu(hdr->group[0].padding)) {
+		pr_err("Internal error: no space for header\n");
+		return -EINVAL;
+	}
+
+	memcpy(buf, hdr, s);
+
+	print_hex_dump_debug("header ", DUMP_PREFIX_OFFSET, 16, 1, buf, s, 0);
+
+	return 0;
+}
+
+static int compress(struct nx842_crypto_ctx *ctx,
+		    struct nx842_crypto_param *p,
+		    struct nx842_crypto_header_group *g,
+		    struct nx842_constraints *c,
+		    u16 *ignore,
+		    unsigned int hdrsize)
+{
+	unsigned int slen = p->iremain, dlen = p->oremain, tmplen;
+	unsigned int adj_slen = slen;
+	u8 *src = p->in, *dst = p->out;
+	int ret, dskip = 0;
+	ktime_t timeout;
+
+	if (p->iremain == 0)
+		return -EOVERFLOW;
+
+	if (p->oremain == 0 || hdrsize + c->minimum > dlen)
+		return -ENOSPC;
+
+	if (slen % c->multiple)
+		adj_slen = round_up(slen, c->multiple);
+	if (slen < c->minimum)
+		adj_slen = c->minimum;
+	if (slen > c->maximum)
+		adj_slen = slen = c->maximum;
+	if (adj_slen > slen || (u64)src % c->alignment) {
+		adj_slen = min(adj_slen, BOUNCE_BUFFER_SIZE);
+		slen = min(slen, BOUNCE_BUFFER_SIZE);
+		if (adj_slen > slen)
+			memset(ctx->sbounce + slen, 0, adj_slen - slen);
+		memcpy(ctx->sbounce, src, slen);
+		src = ctx->sbounce;
+		slen = adj_slen;
+		pr_debug("using comp sbounce buffer, len %x\n", slen);
+	}
+
+	dst += hdrsize;
+	dlen -= hdrsize;
+
+	if ((u64)dst % c->alignment) {
+		dskip = (int)(PTR_ALIGN(dst, c->alignment) - dst);
+		dst += dskip;
+		dlen -= dskip;
+	}
+	if (dlen % c->multiple)
+		dlen = round_down(dlen, c->multiple);
+	if (dlen < c->minimum) {
+nospc:
+		dst = ctx->dbounce;
+		dlen = min(p->oremain, BOUNCE_BUFFER_SIZE);
+		dlen = round_down(dlen, c->multiple);
+		dskip = 0;
+		pr_debug("using comp dbounce buffer, len %x\n", dlen);
+	}
+	if (dlen > c->maximum)
+		dlen = c->maximum;
+
+	tmplen = dlen;
+	timeout = ktime_add_ms(ktime_get(), COMP_BUSY_TIMEOUT);
+	do {
+		dlen = tmplen; /* reset dlen, if we're retrying */
+		ret = nx842_compress(src, slen, dst, &dlen, ctx->wmem);
+		/* possibly we should reduce the slen here, instead of
+		 * retrying with the dbounce buffer?
+		 */
+		if (ret == -ENOSPC && dst != ctx->dbounce)
+			goto nospc;
+	} while (ret == -EBUSY && ktime_before(ktime_get(), timeout));
+	if (ret)
+		return ret;
+
+	dskip += hdrsize;
+
+	if (dst == ctx->dbounce)
+		memcpy(p->out + dskip, dst, dlen);
+
+	g->padding = cpu_to_be16(dskip);
+	g->compressed_length = cpu_to_be32(dlen);
+	g->uncompressed_length = cpu_to_be32(slen);
+
+	if (p->iremain < slen) {
+		*ignore = slen - p->iremain;
+		slen = p->iremain;
+	}
+
+	pr_debug("compress slen %x ignore %x dlen %x padding %x\n",
+		 slen, *ignore, dlen, dskip);
+
+	return update_param(p, slen, dskip + dlen);
+}
+
+static int nx842_crypto_compress(struct crypto_tfm *tfm,
+				 const u8 *src, unsigned int slen,
+				 u8 *dst, unsigned int *dlen)
+{
+	struct nx842_crypto_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct nx842_crypto_header *hdr = &ctx->header;
+	struct nx842_crypto_param p;
+	struct nx842_constraints c;
+	unsigned int groups, hdrsize, h;
+	int ret, n;
+	bool add_header;
+	u16 ignore = 0;
+
+	p.in = (u8 *)src;
+	p.iremain = slen;
+	p.out = dst;
+	p.oremain = *dlen;
+	p.ototal = 0;
+
+	*dlen = 0;
+
+	ret = read_constraints(&c);
+	if (ret)
+		return ret;
+
+	groups = min_t(unsigned int, NX842_CRYPTO_GROUP_MAX,
+		       DIV_ROUND_UP(p.iremain, c.maximum));
+	hdrsize = NX842_CRYPTO_HEADER_SIZE(groups);
+
+	/* skip adding header if the buffers meet all constraints */
+	add_header = (p.iremain % c.multiple	||
+		      p.iremain < c.minimum	||
+		      p.iremain > c.maximum	||
+		      (u64)p.in % c.alignment	||
+		      p.oremain % c.multiple	||
+		      p.oremain < c.minimum	||
+		      p.oremain > c.maximum	||
+		      (u64)p.out % c.alignment);
+
+	hdr->magic = cpu_to_be16(NX842_CRYPTO_MAGIC);
+	hdr->groups = 0;
+	hdr->ignore = 0;
+
+	while (p.iremain > 0) {
+		n = hdr->groups++;
+		if (hdr->groups > NX842_CRYPTO_GROUP_MAX)
+			return -ENOSPC;
+
+		/* header goes before first group */
+		h = !n && add_header ? hdrsize : 0;
+
+		if (ignore)
+			pr_warn("interal error, ignore is set %x\n", ignore);
+
+		ret = compress(ctx, &p, &hdr->group[n], &c, &ignore, h);
+		if (ret)
+			return ret;
+	}
+
+	if (!add_header && hdr->groups > 1) {
+		pr_err("Internal error: No header but multiple groups\n");
+		return -EINVAL;
+	}
+
+	/* ignore indicates the input stream needed to be padded */
+	hdr->ignore = cpu_to_be16(ignore);
+	if (ignore)
+		pr_debug("marked %d bytes as ignore\n", ignore);
+
+	if (add_header)
+		ret = nx842_crypto_add_header(hdr, dst);
+	if (ret)
+		return ret;
+
+	*dlen = p.ototal;
+
+	pr_debug("compress total slen %x dlen %x\n", slen, *dlen);
+
+	return 0;
+}
+
+static int decompress(struct nx842_crypto_ctx *ctx,
+		      struct nx842_crypto_param *p,
+		      struct nx842_crypto_header_group *g,
+		      struct nx842_constraints *c,
+		      u16 ignore,
+		      bool usehw)
+{
+	unsigned int slen = be32_to_cpu(g->compressed_length);
+	unsigned int required_len = be32_to_cpu(g->uncompressed_length);
+	unsigned int dlen = p->oremain, tmplen;
+	unsigned int adj_slen = slen;
+	u8 *src = p->in, *dst = p->out;
+	u16 padding = be16_to_cpu(g->padding);
+	int ret, spadding = 0, dpadding = 0;
+	ktime_t timeout;
+
+	if (!slen || !required_len)
+		return -EINVAL;
+
+	if (p->iremain <= 0 || padding + slen > p->iremain)
+		return -EOVERFLOW;
+
+	if (p->oremain <= 0 || required_len - ignore > p->oremain)
+		return -ENOSPC;
+
+	src += padding;
+
+	if (!usehw)
+		goto usesw;
+
+	if (slen % c->multiple)
+		adj_slen = round_up(slen, c->multiple);
+	if (slen < c->minimum)
+		adj_slen = c->minimum;
+	if (slen > c->maximum)
+		goto usesw;
+	if (slen < adj_slen || (u64)src % c->alignment) {
+		/* we can append padding bytes because the 842 format defines
+		 * an "end" template (see lib/842/842_decompress.c) and will
+		 * ignore any bytes following it.
+		 */
+		if (slen < adj_slen)
+			memset(ctx->sbounce + slen, 0, adj_slen - slen);
+		memcpy(ctx->sbounce, src, slen);
+		src = ctx->sbounce;
+		spadding = adj_slen - slen;
+		slen = adj_slen;
+		pr_debug("using decomp sbounce buffer, len %x\n", slen);
+	}
+
+	if (dlen % c->multiple)
+		dlen = round_down(dlen, c->multiple);
+	if (dlen < required_len || (u64)dst % c->alignment) {
+		dst = ctx->dbounce;
+		dlen = min(required_len, BOUNCE_BUFFER_SIZE);
+		pr_debug("using decomp dbounce buffer, len %x\n", dlen);
+	}
+	if (dlen < c->minimum)
+		goto usesw;
+	if (dlen > c->maximum)
+		dlen = c->maximum;
+
+	tmplen = dlen;
+	timeout = ktime_add_ms(ktime_get(), DECOMP_BUSY_TIMEOUT);
+	do {
+		dlen = tmplen; /* reset dlen, if we're retrying */
+		ret = nx842_decompress(src, slen, dst, &dlen, ctx->wmem);
+	} while (ret == -EBUSY && ktime_before(ktime_get(), timeout));
+	if (ret) {
+usesw:
+		/* reset everything, sw doesn't have constraints */
+		src = p->in + padding;
+		slen = be32_to_cpu(g->compressed_length);
+		spadding = 0;
+		dst = p->out;
+		dlen = p->oremain;
+		dpadding = 0;
+		if (dlen < required_len) { /* have ignore bytes */
+			dst = ctx->dbounce;
+			dlen = BOUNCE_BUFFER_SIZE;
+		}
+		pr_info_ratelimited("using software 842 decompression\n");
+		ret = sw842_decompress(src, slen, dst, &dlen);
+	}
+	if (ret)
+		return ret;
+
+	slen -= spadding;
+
+	dlen -= ignore;
+	if (ignore)
+		pr_debug("ignoring last %x bytes\n", ignore);
+
+	if (dst == ctx->dbounce)
+		memcpy(p->out, dst, dlen);
+
+	pr_debug("decompress slen %x padding %x dlen %x ignore %x\n",
+		 slen, padding, dlen, ignore);
+
+	return update_param(p, slen + padding, dlen);
+}
+
+static int nx842_crypto_decompress(struct crypto_tfm *tfm,
+				   const u8 *src, unsigned int slen,
+				   u8 *dst, unsigned int *dlen)
+{
+	struct nx842_crypto_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct nx842_crypto_header *hdr;
+	struct nx842_crypto_param p;
+	struct nx842_constraints c;
+	int n, ret, hdr_len;
+	u16 ignore = 0;
+	bool usehw = true;
+
+	p.in = (u8 *)src;
+	p.iremain = slen;
+	p.out = dst;
+	p.oremain = *dlen;
+	p.ototal = 0;
+
+	*dlen = 0;
+
+	if (read_constraints(&c))
+		usehw = false;
+
+	hdr = (struct nx842_crypto_header *)src;
+
+	/* If it doesn't start with our header magic number, assume it's a raw
+	 * 842 compressed buffer and pass it directly to the hardware driver
+	 */
+	if (be16_to_cpu(hdr->magic) != NX842_CRYPTO_MAGIC) {
+		struct nx842_crypto_header_group g = {
+			.padding =		0,
+			.compressed_length =	cpu_to_be32(p.iremain),
+			.uncompressed_length =	cpu_to_be32(p.oremain),
+		};
+
+		ret = decompress(ctx, &p, &g, &c, 0, usehw);
+		if (ret)
+			return ret;
+
+		*dlen = p.ototal;
+
+		return 0;
+	}
+
+	if (!hdr->groups) {
+		pr_err("header has no groups\n");
+		return -EINVAL;
+	}
+	if (hdr->groups > NX842_CRYPTO_GROUP_MAX) {
+		pr_err("header has too many groups %x, max %x\n",
+		       hdr->groups, NX842_CRYPTO_GROUP_MAX);
+		return -EINVAL;
+	}
+
+	hdr_len = NX842_CRYPTO_HEADER_SIZE(hdr->groups);
+	if (hdr_len > slen)
+		return -EOVERFLOW;
+
+	memcpy(&ctx->header, src, hdr_len);
+	hdr = &ctx->header;
+
+	for (n = 0; n < hdr->groups; n++) {
+		/* ignore applies to last group */
+		if (n + 1 == hdr->groups)
+			ignore = be16_to_cpu(hdr->ignore);
+
+		ret = decompress(ctx, &p, &hdr->group[n], &c, ignore, usehw);
+		if (ret)
+			return ret;
+	}
+
+	*dlen = p.ototal;
+
+	pr_debug("decompress total slen %x dlen %x\n", slen, *dlen);
+
+	return 0;
+}
+
+static struct crypto_alg alg = {
+	.cra_name		= "842",
+	.cra_driver_name	= "842-nx",
+	.cra_priority		= 300,
+	.cra_flags		= CRYPTO_ALG_TYPE_COMPRESS,
+	.cra_ctxsize		= sizeof(struct nx842_crypto_ctx),
+	.cra_module		= THIS_MODULE,
+	.cra_init		= nx842_crypto_init,
+	.cra_exit		= nx842_crypto_exit,
+	.cra_u			= { .compress = {
+	.coa_compress		= nx842_crypto_compress,
+	.coa_decompress		= nx842_crypto_decompress } }
+};
+
+static int __init nx842_crypto_mod_init(void)
+{
+	return crypto_register_alg(&alg);
+}
+module_init(nx842_crypto_mod_init);
+
+static void __exit nx842_crypto_mod_exit(void)
+{
+	crypto_unregister_alg(&alg);
+}
+module_exit(nx842_crypto_mod_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("IBM PowerPC Nest (NX) 842 Hardware Compression Interface");
+MODULE_ALIAS_CRYPTO("842");
+MODULE_ALIAS_CRYPTO("842-nx");
+MODULE_AUTHOR("Dan Streetman <ddstreet@ieee.org>");
diff --git a/drivers/crypto/nx/nx-842-platform.c b/drivers/crypto/nx/nx-842-platform.c
new file mode 100644
index 000000000000..664f13dd06ed
--- /dev/null
+++ b/drivers/crypto/nx/nx-842-platform.c
@@ -0,0 +1,84 @@
+
+#include "nx-842.h"
+
+/* this is needed, separate from the main nx-842.c driver, because that main
+ * driver loads the platform drivers during its init(), and it expects one
+ * (or none) of the platform drivers to set this pointer to its driver.
+ * That means this pointer can't be in the main nx-842 driver, because it
+ * wouldn't be accessible until after the main driver loaded, which wouldn't
+ * be possible as it's waiting for the platform driver to load.  So place it
+ * here.
+ */
+static struct nx842_driver *driver;
+static DEFINE_SPINLOCK(driver_lock);
+
+struct nx842_driver *nx842_platform_driver(void)
+{
+	return driver;
+}
+EXPORT_SYMBOL_GPL(nx842_platform_driver);
+
+bool nx842_platform_driver_set(struct nx842_driver *_driver)
+{
+	bool ret = false;
+
+	spin_lock(&driver_lock);
+
+	if (!driver) {
+		driver = _driver;
+		ret = true;
+	} else
+		WARN(1, "can't set platform driver, already set to %s\n",
+		     driver->name);
+
+	spin_unlock(&driver_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(nx842_platform_driver_set);
+
+/* only call this from the platform driver exit function */
+void nx842_platform_driver_unset(struct nx842_driver *_driver)
+{
+	spin_lock(&driver_lock);
+
+	if (driver == _driver)
+		driver = NULL;
+	else if (driver)
+		WARN(1, "can't unset platform driver %s, currently set to %s\n",
+		     _driver->name, driver->name);
+	else
+		WARN(1, "can't unset platform driver, already unset\n");
+
+	spin_unlock(&driver_lock);
+}
+EXPORT_SYMBOL_GPL(nx842_platform_driver_unset);
+
+bool nx842_platform_driver_get(void)
+{
+	bool ret = false;
+
+	spin_lock(&driver_lock);
+
+	if (driver)
+		ret = try_module_get(driver->owner);
+
+	spin_unlock(&driver_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(nx842_platform_driver_get);
+
+void nx842_platform_driver_put(void)
+{
+	spin_lock(&driver_lock);
+
+	if (driver)
+		module_put(driver->owner);
+
+	spin_unlock(&driver_lock);
+}
+EXPORT_SYMBOL_GPL(nx842_platform_driver_put);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dan Streetman <ddstreet@ieee.org>");
+MODULE_DESCRIPTION("842 H/W Compression platform driver");
diff --git a/drivers/crypto/nx/nx-842-powernv.c b/drivers/crypto/nx/nx-842-powernv.c
new file mode 100644
index 000000000000..33b3b0abf4ae
--- /dev/null
+++ b/drivers/crypto/nx/nx-842-powernv.c
@@ -0,0 +1,637 @@
+/*
+ * Driver for IBM PowerNV 842 compression accelerator
+ *
+ * Copyright (C) 2015 Dan Streetman, 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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 "nx-842.h"
+
+#include <linux/timer.h>
+
+#include <asm/prom.h>
+#include <asm/icswx.h>
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dan Streetman <ddstreet@ieee.org>");
+MODULE_DESCRIPTION("842 H/W Compression driver for IBM PowerNV processors");
+
+#define WORKMEM_ALIGN	(CRB_ALIGN)
+#define CSB_WAIT_MAX	(5000) /* ms */
+
+struct nx842_workmem {
+	/* Below fields must be properly aligned */
+	struct coprocessor_request_block crb; /* CRB_ALIGN align */
+	struct data_descriptor_entry ddl_in[DDL_LEN_MAX]; /* DDE_ALIGN align */
+	struct data_descriptor_entry ddl_out[DDL_LEN_MAX]; /* DDE_ALIGN align */
+	/* Above fields must be properly aligned */
+
+	ktime_t start;
+
+	char padding[WORKMEM_ALIGN]; /* unused, to allow alignment */
+} __packed __aligned(WORKMEM_ALIGN);
+
+struct nx842_coproc {
+	unsigned int chip_id;
+	unsigned int ct;
+	unsigned int ci;
+	struct list_head list;
+};
+
+/* no cpu hotplug on powernv, so this list never changes after init */
+static LIST_HEAD(nx842_coprocs);
+static unsigned int nx842_ct;
+
+/**
+ * setup_indirect_dde - Setup an indirect DDE
+ *
+ * The DDE is setup with the the DDE count, byte count, and address of
+ * first direct DDE in the list.
+ */
+static void setup_indirect_dde(struct data_descriptor_entry *dde,
+			       struct data_descriptor_entry *ddl,
+			       unsigned int dde_count, unsigned int byte_count)
+{
+	dde->flags = 0;
+	dde->count = dde_count;
+	dde->index = 0;
+	dde->length = cpu_to_be32(byte_count);
+	dde->address = cpu_to_be64(nx842_get_pa(ddl));
+}
+
+/**
+ * setup_direct_dde - Setup single DDE from buffer
+ *
+ * The DDE is setup with the buffer and length.  The buffer must be properly
+ * aligned.  The used length is returned.
+ * Returns:
+ *   N    Successfully set up DDE with N bytes
+ */
+static unsigned int setup_direct_dde(struct data_descriptor_entry *dde,
+				     unsigned long pa, unsigned int len)
+{
+	unsigned int l = min_t(unsigned int, len, LEN_ON_PAGE(pa));
+
+	dde->flags = 0;
+	dde->count = 0;
+	dde->index = 0;
+	dde->length = cpu_to_be32(l);
+	dde->address = cpu_to_be64(pa);
+
+	return l;
+}
+
+/**
+ * setup_ddl - Setup DDL from buffer
+ *
+ * Returns:
+ *   0		Successfully set up DDL
+ */
+static int setup_ddl(struct data_descriptor_entry *dde,
+		     struct data_descriptor_entry *ddl,
+		     unsigned char *buf, unsigned int len,
+		     bool in)
+{
+	unsigned long pa = nx842_get_pa(buf);
+	int i, ret, total_len = len;
+
+	if (!IS_ALIGNED(pa, DDE_BUFFER_ALIGN)) {
+		pr_debug("%s buffer pa 0x%lx not 0x%x-byte aligned\n",
+			 in ? "input" : "output", pa, DDE_BUFFER_ALIGN);
+		return -EINVAL;
+	}
+
+	/* only need to check last mult; since buffer must be
+	 * DDE_BUFFER_ALIGN aligned, and that is a multiple of
+	 * DDE_BUFFER_SIZE_MULT, and pre-last page DDE buffers
+	 * are guaranteed a multiple of DDE_BUFFER_SIZE_MULT.
+	 */
+	if (len % DDE_BUFFER_LAST_MULT) {
+		pr_debug("%s buffer len 0x%x not a multiple of 0x%x\n",
+			 in ? "input" : "output", len, DDE_BUFFER_LAST_MULT);
+		if (in)
+			return -EINVAL;
+		len = round_down(len, DDE_BUFFER_LAST_MULT);
+	}
+
+	/* use a single direct DDE */
+	if (len <= LEN_ON_PAGE(pa)) {
+		ret = setup_direct_dde(dde, pa, len);
+		WARN_ON(ret < len);
+		return 0;
+	}
+
+	/* use the DDL */
+	for (i = 0; i < DDL_LEN_MAX && len > 0; i++) {
+		ret = setup_direct_dde(&ddl[i], pa, len);
+		buf += ret;
+		len -= ret;
+		pa = nx842_get_pa(buf);
+	}
+
+	if (len > 0) {
+		pr_debug("0x%x total %s bytes 0x%x too many for DDL.\n",
+			 total_len, in ? "input" : "output", len);
+		if (in)
+			return -EMSGSIZE;
+		total_len -= len;
+	}
+	setup_indirect_dde(dde, ddl, i, total_len);
+
+	return 0;
+}
+
+#define CSB_ERR(csb, msg, ...)					\
+	pr_err("ERROR: " msg " : %02x %02x %02x %02x %08x\n",	\
+	       ##__VA_ARGS__, (csb)->flags,			\
+	       (csb)->cs, (csb)->cc, (csb)->ce,			\
+	       be32_to_cpu((csb)->count))
+
+#define CSB_ERR_ADDR(csb, msg, ...)				\
+	CSB_ERR(csb, msg " at %lx", ##__VA_ARGS__,		\
+		(unsigned long)be64_to_cpu((csb)->address))
+
+/**
+ * wait_for_csb
+ */
+static int wait_for_csb(struct nx842_workmem *wmem,
+			struct coprocessor_status_block *csb)
+{
+	ktime_t start = wmem->start, now = ktime_get();
+	ktime_t timeout = ktime_add_ms(start, CSB_WAIT_MAX);
+
+	while (!(ACCESS_ONCE(csb->flags) & CSB_V)) {
+		cpu_relax();
+		now = ktime_get();
+		if (ktime_after(now, timeout))
+			break;
+	}
+
+	/* hw has updated csb and output buffer */
+	barrier();
+
+	/* check CSB flags */
+	if (!(csb->flags & CSB_V)) {
+		CSB_ERR(csb, "CSB still not valid after %ld us, giving up",
+			(long)ktime_us_delta(now, start));
+		return -ETIMEDOUT;
+	}
+	if (csb->flags & CSB_F) {
+		CSB_ERR(csb, "Invalid CSB format");
+		return -EPROTO;
+	}
+	if (csb->flags & CSB_CH) {
+		CSB_ERR(csb, "Invalid CSB chaining state");
+		return -EPROTO;
+	}
+
+	/* verify CSB completion sequence is 0 */
+	if (csb->cs) {
+		CSB_ERR(csb, "Invalid CSB completion sequence");
+		return -EPROTO;
+	}
+
+	/* check CSB Completion Code */
+	switch (csb->cc) {
+	/* no error */
+	case CSB_CC_SUCCESS:
+		break;
+	case CSB_CC_TPBC_GT_SPBC:
+		/* not an error, but the compressed data is
+		 * larger than the uncompressed data :(
+		 */
+		break;
+
+	/* input data errors */
+	case CSB_CC_OPERAND_OVERLAP:
+		/* input and output buffers overlap */
+		CSB_ERR(csb, "Operand Overlap error");
+		return -EINVAL;
+	case CSB_CC_INVALID_OPERAND:
+		CSB_ERR(csb, "Invalid operand");
+		return -EINVAL;
+	case CSB_CC_NOSPC:
+		/* output buffer too small */
+		return -ENOSPC;
+	case CSB_CC_ABORT:
+		CSB_ERR(csb, "Function aborted");
+		return -EINTR;
+	case CSB_CC_CRC_MISMATCH:
+		CSB_ERR(csb, "CRC mismatch");
+		return -EINVAL;
+	case CSB_CC_TEMPL_INVALID:
+		CSB_ERR(csb, "Compressed data template invalid");
+		return -EINVAL;
+	case CSB_CC_TEMPL_OVERFLOW:
+		CSB_ERR(csb, "Compressed data template shows data past end");
+		return -EINVAL;
+
+	/* these should not happen */
+	case CSB_CC_INVALID_ALIGN:
+		/* setup_ddl should have detected this */
+		CSB_ERR_ADDR(csb, "Invalid alignment");
+		return -EINVAL;
+	case CSB_CC_DATA_LENGTH:
+		/* setup_ddl should have detected this */
+		CSB_ERR(csb, "Invalid data length");
+		return -EINVAL;
+	case CSB_CC_WR_TRANSLATION:
+	case CSB_CC_TRANSLATION:
+	case CSB_CC_TRANSLATION_DUP1:
+	case CSB_CC_TRANSLATION_DUP2:
+	case CSB_CC_TRANSLATION_DUP3:
+	case CSB_CC_TRANSLATION_DUP4:
+	case CSB_CC_TRANSLATION_DUP5:
+	case CSB_CC_TRANSLATION_DUP6:
+		/* should not happen, we use physical addrs */
+		CSB_ERR_ADDR(csb, "Translation error");
+		return -EPROTO;
+	case CSB_CC_WR_PROTECTION:
+	case CSB_CC_PROTECTION:
+	case CSB_CC_PROTECTION_DUP1:
+	case CSB_CC_PROTECTION_DUP2:
+	case CSB_CC_PROTECTION_DUP3:
+	case CSB_CC_PROTECTION_DUP4:
+	case CSB_CC_PROTECTION_DUP5:
+	case CSB_CC_PROTECTION_DUP6:
+		/* should not happen, we use physical addrs */
+		CSB_ERR_ADDR(csb, "Protection error");
+		return -EPROTO;
+	case CSB_CC_PRIVILEGE:
+		/* shouldn't happen, we're in HYP mode */
+		CSB_ERR(csb, "Insufficient Privilege error");
+		return -EPROTO;
+	case CSB_CC_EXCESSIVE_DDE:
+		/* shouldn't happen, setup_ddl doesn't use many dde's */
+		CSB_ERR(csb, "Too many DDEs in DDL");
+		return -EINVAL;
+	case CSB_CC_TRANSPORT:
+		/* shouldn't happen, we setup CRB correctly */
+		CSB_ERR(csb, "Invalid CRB");
+		return -EINVAL;
+	case CSB_CC_SEGMENTED_DDL:
+		/* shouldn't happen, setup_ddl creates DDL right */
+		CSB_ERR(csb, "Segmented DDL error");
+		return -EINVAL;
+	case CSB_CC_DDE_OVERFLOW:
+		/* shouldn't happen, setup_ddl creates DDL right */
+		CSB_ERR(csb, "DDE overflow error");
+		return -EINVAL;
+	case CSB_CC_SESSION:
+		/* should not happen with ICSWX */
+		CSB_ERR(csb, "Session violation error");
+		return -EPROTO;
+	case CSB_CC_CHAIN:
+		/* should not happen, we don't use chained CRBs */
+		CSB_ERR(csb, "Chained CRB error");
+		return -EPROTO;
+	case CSB_CC_SEQUENCE:
+		/* should not happen, we don't use chained CRBs */
+		CSB_ERR(csb, "CRB seqeunce number error");
+		return -EPROTO;
+	case CSB_CC_UNKNOWN_CODE:
+		CSB_ERR(csb, "Unknown subfunction code");
+		return -EPROTO;
+
+	/* hardware errors */
+	case CSB_CC_RD_EXTERNAL:
+	case CSB_CC_RD_EXTERNAL_DUP1:
+	case CSB_CC_RD_EXTERNAL_DUP2:
+	case CSB_CC_RD_EXTERNAL_DUP3:
+		CSB_ERR_ADDR(csb, "Read error outside coprocessor");
+		return -EPROTO;
+	case CSB_CC_WR_EXTERNAL:
+		CSB_ERR_ADDR(csb, "Write error outside coprocessor");
+		return -EPROTO;
+	case CSB_CC_INTERNAL:
+		CSB_ERR(csb, "Internal error in coprocessor");
+		return -EPROTO;
+	case CSB_CC_PROVISION:
+		CSB_ERR(csb, "Storage provision error");
+		return -EPROTO;
+	case CSB_CC_HW:
+		CSB_ERR(csb, "Correctable hardware error");
+		return -EPROTO;
+
+	default:
+		CSB_ERR(csb, "Invalid CC %d", csb->cc);
+		return -EPROTO;
+	}
+
+	/* check Completion Extension state */
+	if (csb->ce & CSB_CE_TERMINATION) {
+		CSB_ERR(csb, "CSB request was terminated");
+		return -EPROTO;
+	}
+	if (csb->ce & CSB_CE_INCOMPLETE) {
+		CSB_ERR(csb, "CSB request not complete");
+		return -EPROTO;
+	}
+	if (!(csb->ce & CSB_CE_TPBC)) {
+		CSB_ERR(csb, "TPBC not provided, unknown target length");
+		return -EPROTO;
+	}
+
+	/* successful completion */
+	pr_debug_ratelimited("Processed %u bytes in %lu us\n", csb->count,
+			     (unsigned long)ktime_us_delta(now, start));
+
+	return 0;
+}
+
+/**
+ * nx842_powernv_function - compress/decompress data using the 842 algorithm
+ *
+ * (De)compression provided by the NX842 coprocessor on IBM PowerNV systems.
+ * This compresses or decompresses the provided input buffer into the provided
+ * output buffer.
+ *
+ * Upon return from this function @outlen contains the length of the
+ * output data.  If there is an error then @outlen will be 0 and an
+ * error will be specified by the return code from this function.
+ *
+ * The @workmem buffer should only be used by one function call at a time.
+ *
+ * @in: input buffer pointer
+ * @inlen: input buffer size
+ * @out: output buffer pointer
+ * @outlenp: output buffer size pointer
+ * @workmem: working memory buffer pointer, size determined by
+ *           nx842_powernv_driver.workmem_size
+ * @fc: function code, see CCW Function Codes in nx-842.h
+ *
+ * Returns:
+ *   0		Success, output of length @outlenp stored in the buffer at @out
+ *   -ENODEV	Hardware unavailable
+ *   -ENOSPC	Output buffer is to small
+ *   -EMSGSIZE	Input buffer too large
+ *   -EINVAL	buffer constraints do not fix nx842_constraints
+ *   -EPROTO	hardware error during operation
+ *   -ETIMEDOUT	hardware did not complete operation in reasonable time
+ *   -EINTR	operation was aborted
+ */
+static int nx842_powernv_function(const unsigned char *in, unsigned int inlen,
+				  unsigned char *out, unsigned int *outlenp,
+				  void *workmem, int fc)
+{
+	struct coprocessor_request_block *crb;
+	struct coprocessor_status_block *csb;
+	struct nx842_workmem *wmem;
+	int ret;
+	u64 csb_addr;
+	u32 ccw;
+	unsigned int outlen = *outlenp;
+
+	wmem = PTR_ALIGN(workmem, WORKMEM_ALIGN);
+
+	*outlenp = 0;
+
+	/* shoudn't happen, we don't load without a coproc */
+	if (!nx842_ct) {
+		pr_err_ratelimited("coprocessor CT is 0");
+		return -ENODEV;
+	}
+
+	crb = &wmem->crb;
+	csb = &crb->csb;
+
+	/* Clear any previous values */
+	memset(crb, 0, sizeof(*crb));
+
+	/* set up DDLs */
+	ret = setup_ddl(&crb->source, wmem->ddl_in,
+			(unsigned char *)in, inlen, true);
+	if (ret)
+		return ret;
+	ret = setup_ddl(&crb->target, wmem->ddl_out,
+			out, outlen, false);
+	if (ret)
+		return ret;
+
+	/* set up CCW */
+	ccw = 0;
+	ccw = SET_FIELD(ccw, CCW_CT, nx842_ct);
+	ccw = SET_FIELD(ccw, CCW_CI_842, 0); /* use 0 for hw auto-selection */
+	ccw = SET_FIELD(ccw, CCW_FC_842, fc);
+
+	/* set up CRB's CSB addr */
+	csb_addr = nx842_get_pa(csb) & CRB_CSB_ADDRESS;
+	csb_addr |= CRB_CSB_AT; /* Addrs are phys */
+	crb->csb_addr = cpu_to_be64(csb_addr);
+
+	wmem->start = ktime_get();
+
+	/* do ICSWX */
+	ret = icswx(cpu_to_be32(ccw), crb);
+
+	pr_debug_ratelimited("icswx CR %x ccw %x crb->ccw %x\n", ret,
+			     (unsigned int)ccw,
+			     (unsigned int)be32_to_cpu(crb->ccw));
+
+	switch (ret) {
+	case ICSWX_INITIATED:
+		ret = wait_for_csb(wmem, csb);
+		break;
+	case ICSWX_BUSY:
+		pr_debug_ratelimited("842 Coprocessor busy\n");
+		ret = -EBUSY;
+		break;
+	case ICSWX_REJECTED:
+		pr_err_ratelimited("ICSWX rejected\n");
+		ret = -EPROTO;
+		break;
+	default:
+		pr_err_ratelimited("Invalid ICSWX return code %x\n", ret);
+		ret = -EPROTO;
+		break;
+	}
+
+	if (!ret)
+		*outlenp = be32_to_cpu(csb->count);
+
+	return ret;
+}
+
+/**
+ * nx842_powernv_compress - Compress data using the 842 algorithm
+ *
+ * Compression provided by the NX842 coprocessor on IBM PowerNV systems.
+ * The input buffer is compressed and the result is stored in the
+ * provided output buffer.
+ *
+ * Upon return from this function @outlen contains the length of the
+ * compressed data.  If there is an error then @outlen will be 0 and an
+ * error will be specified by the return code from this function.
+ *
+ * @in: input buffer pointer
+ * @inlen: input buffer size
+ * @out: output buffer pointer
+ * @outlenp: output buffer size pointer
+ * @workmem: working memory buffer pointer, size determined by
+ *           nx842_powernv_driver.workmem_size
+ *
+ * Returns: see @nx842_powernv_function()
+ */
+static int nx842_powernv_compress(const unsigned char *in, unsigned int inlen,
+				  unsigned char *out, unsigned int *outlenp,
+				  void *wmem)
+{
+	return nx842_powernv_function(in, inlen, out, outlenp,
+				      wmem, CCW_FC_842_COMP_NOCRC);
+}
+
+/**
+ * nx842_powernv_decompress - Decompress data using the 842 algorithm
+ *
+ * Decompression provided by the NX842 coprocessor on IBM PowerNV systems.
+ * The input buffer is decompressed and the result is stored in the
+ * provided output buffer.
+ *
+ * Upon return from this function @outlen contains the length of the
+ * decompressed data.  If there is an error then @outlen will be 0 and an
+ * error will be specified by the return code from this function.
+ *
+ * @in: input buffer pointer
+ * @inlen: input buffer size
+ * @out: output buffer pointer
+ * @outlenp: output buffer size pointer
+ * @workmem: working memory buffer pointer, size determined by
+ *           nx842_powernv_driver.workmem_size
+ *
+ * Returns: see @nx842_powernv_function()
+ */
+static int nx842_powernv_decompress(const unsigned char *in, unsigned int inlen,
+				    unsigned char *out, unsigned int *outlenp,
+				    void *wmem)
+{
+	return nx842_powernv_function(in, inlen, out, outlenp,
+				      wmem, CCW_FC_842_DECOMP_NOCRC);
+}
+
+static int __init nx842_powernv_probe(struct device_node *dn)
+{
+	struct nx842_coproc *coproc;
+	struct property *ct_prop, *ci_prop;
+	unsigned int ct, ci;
+	int chip_id;
+
+	chip_id = of_get_ibm_chip_id(dn);
+	if (chip_id < 0) {
+		pr_err("ibm,chip-id missing\n");
+		return -EINVAL;
+	}
+	ct_prop = of_find_property(dn, "ibm,842-coprocessor-type", NULL);
+	if (!ct_prop) {
+		pr_err("ibm,842-coprocessor-type missing\n");
+		return -EINVAL;
+	}
+	ct = be32_to_cpu(*(unsigned int *)ct_prop->value);
+	ci_prop = of_find_property(dn, "ibm,842-coprocessor-instance", NULL);
+	if (!ci_prop) {
+		pr_err("ibm,842-coprocessor-instance missing\n");
+		return -EINVAL;
+	}
+	ci = be32_to_cpu(*(unsigned int *)ci_prop->value);
+
+	coproc = kmalloc(sizeof(*coproc), GFP_KERNEL);
+	if (!coproc)
+		return -ENOMEM;
+
+	coproc->chip_id = chip_id;
+	coproc->ct = ct;
+	coproc->ci = ci;
+	INIT_LIST_HEAD(&coproc->list);
+	list_add(&coproc->list, &nx842_coprocs);
+
+	pr_info("coprocessor found on chip %d, CT %d CI %d\n", chip_id, ct, ci);
+
+	if (!nx842_ct)
+		nx842_ct = ct;
+	else if (nx842_ct != ct)
+		pr_err("NX842 chip %d, CT %d != first found CT %d\n",
+		       chip_id, ct, nx842_ct);
+
+	return 0;
+}
+
+static struct nx842_constraints nx842_powernv_constraints = {
+	.alignment =	DDE_BUFFER_ALIGN,
+	.multiple =	DDE_BUFFER_LAST_MULT,
+	.minimum =	DDE_BUFFER_LAST_MULT,
+	.maximum =	(DDL_LEN_MAX - 1) * PAGE_SIZE,
+};
+
+static struct nx842_driver nx842_powernv_driver = {
+	.name =		KBUILD_MODNAME,
+	.owner =	THIS_MODULE,
+	.workmem_size =	sizeof(struct nx842_workmem),
+	.constraints =	&nx842_powernv_constraints,
+	.compress =	nx842_powernv_compress,
+	.decompress =	nx842_powernv_decompress,
+};
+
+static __init int nx842_powernv_init(void)
+{
+	struct device_node *dn;
+
+	/* verify workmem size/align restrictions */
+	BUILD_BUG_ON(WORKMEM_ALIGN % CRB_ALIGN);
+	BUILD_BUG_ON(CRB_ALIGN % DDE_ALIGN);
+	BUILD_BUG_ON(CRB_SIZE % DDE_ALIGN);
+	/* verify buffer size/align restrictions */
+	BUILD_BUG_ON(PAGE_SIZE % DDE_BUFFER_ALIGN);
+	BUILD_BUG_ON(DDE_BUFFER_ALIGN % DDE_BUFFER_SIZE_MULT);
+	BUILD_BUG_ON(DDE_BUFFER_SIZE_MULT % DDE_BUFFER_LAST_MULT);
+
+	pr_info("loading\n");
+
+	for_each_compatible_node(dn, NULL, "ibm,power-nx")
+		nx842_powernv_probe(dn);
+
+	if (!nx842_ct) {
+		pr_err("no coprocessors found\n");
+		return -ENODEV;
+	}
+
+	if (!nx842_platform_driver_set(&nx842_powernv_driver)) {
+		struct nx842_coproc *coproc, *n;
+
+		list_for_each_entry_safe(coproc, n, &nx842_coprocs, list) {
+			list_del(&coproc->list);
+			kfree(coproc);
+		}
+
+		return -EEXIST;
+	}
+
+	pr_info("loaded\n");
+
+	return 0;
+}
+module_init(nx842_powernv_init);
+
+static void __exit nx842_powernv_exit(void)
+{
+	struct nx842_coproc *coproc, *n;
+
+	nx842_platform_driver_unset(&nx842_powernv_driver);
+
+	list_for_each_entry_safe(coproc, n, &nx842_coprocs, list) {
+		list_del(&coproc->list);
+		kfree(coproc);
+	}
+
+	pr_info("unloaded\n");
+}
+module_exit(nx842_powernv_exit);
diff --git a/drivers/crypto/nx/nx-842-pseries.c b/drivers/crypto/nx/nx-842-pseries.c
new file mode 100644
index 000000000000..3040a6091bf2
--- /dev/null
+++ b/drivers/crypto/nx/nx-842-pseries.c
@@ -0,0 +1,1140 @@
+/*
+ * Driver for IBM Power 842 compression accelerator
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under 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, 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ * Copyright (C) IBM Corporation, 2012
+ *
+ * Authors: Robert Jennings <rcj@linux.vnet.ibm.com>
+ *          Seth Jennings <sjenning@linux.vnet.ibm.com>
+ */
+
+#include <asm/vio.h>
+
+#include "nx-842.h"
+#include "nx_csbcpb.h" /* struct nx_csbcpb */
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Robert Jennings <rcj@linux.vnet.ibm.com>");
+MODULE_DESCRIPTION("842 H/W Compression driver for IBM Power processors");
+
+static struct nx842_constraints nx842_pseries_constraints = {
+	.alignment =	DDE_BUFFER_ALIGN,
+	.multiple =	DDE_BUFFER_LAST_MULT,
+	.minimum =	DDE_BUFFER_LAST_MULT,
+	.maximum =	PAGE_SIZE, /* dynamic, max_sync_size */
+};
+
+static int check_constraints(unsigned long buf, unsigned int *len, bool in)
+{
+	if (!IS_ALIGNED(buf, nx842_pseries_constraints.alignment)) {
+		pr_debug("%s buffer 0x%lx not aligned to 0x%x\n",
+			 in ? "input" : "output", buf,
+			 nx842_pseries_constraints.alignment);
+		return -EINVAL;
+	}
+	if (*len % nx842_pseries_constraints.multiple) {
+		pr_debug("%s buffer len 0x%x not multiple of 0x%x\n",
+			 in ? "input" : "output", *len,
+			 nx842_pseries_constraints.multiple);
+		if (in)
+			return -EINVAL;
+		*len = round_down(*len, nx842_pseries_constraints.multiple);
+	}
+	if (*len < nx842_pseries_constraints.minimum) {
+		pr_debug("%s buffer len 0x%x under minimum 0x%x\n",
+			 in ? "input" : "output", *len,
+			 nx842_pseries_constraints.minimum);
+		return -EINVAL;
+	}
+	if (*len > nx842_pseries_constraints.maximum) {
+		pr_debug("%s buffer len 0x%x over maximum 0x%x\n",
+			 in ? "input" : "output", *len,
+			 nx842_pseries_constraints.maximum);
+		if (in)
+			return -EINVAL;
+		*len = nx842_pseries_constraints.maximum;
+	}
+	return 0;
+}
+
+/* I assume we need to align the CSB? */
+#define WORKMEM_ALIGN	(256)
+
+struct nx842_workmem {
+	/* scatterlist */
+	char slin[4096];
+	char slout[4096];
+	/* coprocessor status/parameter block */
+	struct nx_csbcpb csbcpb;
+
+	char padding[WORKMEM_ALIGN];
+} __aligned(WORKMEM_ALIGN);
+
+/* Macros for fields within nx_csbcpb */
+/* Check the valid bit within the csbcpb valid field */
+#define NX842_CSBCBP_VALID_CHK(x) (x & BIT_MASK(7))
+
+/* CE macros operate on the completion_extension field bits in the csbcpb.
+ * CE0 0=full completion, 1=partial completion
+ * CE1 0=CE0 indicates completion, 1=termination (output may be modified)
+ * CE2 0=processed_bytes is source bytes, 1=processed_bytes is target bytes */
+#define NX842_CSBCPB_CE0(x)	(x & BIT_MASK(7))
+#define NX842_CSBCPB_CE1(x)	(x & BIT_MASK(6))
+#define NX842_CSBCPB_CE2(x)	(x & BIT_MASK(5))
+
+/* The NX unit accepts data only on 4K page boundaries */
+#define NX842_HW_PAGE_SIZE	(4096)
+#define NX842_HW_PAGE_MASK	(~(NX842_HW_PAGE_SIZE-1))
+
+enum nx842_status {
+	UNAVAILABLE,
+	AVAILABLE
+};
+
+struct ibm_nx842_counters {
+	atomic64_t comp_complete;
+	atomic64_t comp_failed;
+	atomic64_t decomp_complete;
+	atomic64_t decomp_failed;
+	atomic64_t swdecomp;
+	atomic64_t comp_times[32];
+	atomic64_t decomp_times[32];
+};
+
+static struct nx842_devdata {
+	struct vio_dev *vdev;
+	struct device *dev;
+	struct ibm_nx842_counters *counters;
+	unsigned int max_sg_len;
+	unsigned int max_sync_size;
+	unsigned int max_sync_sg;
+	enum nx842_status status;
+} __rcu *devdata;
+static DEFINE_SPINLOCK(devdata_mutex);
+
+#define NX842_COUNTER_INC(_x) \
+static inline void nx842_inc_##_x( \
+	const struct nx842_devdata *dev) { \
+	if (dev) \
+		atomic64_inc(&dev->counters->_x); \
+}
+NX842_COUNTER_INC(comp_complete);
+NX842_COUNTER_INC(comp_failed);
+NX842_COUNTER_INC(decomp_complete);
+NX842_COUNTER_INC(decomp_failed);
+NX842_COUNTER_INC(swdecomp);
+
+#define NX842_HIST_SLOTS 16
+
+static void ibm_nx842_incr_hist(atomic64_t *times, unsigned int time)
+{
+	int bucket = fls(time);
+
+	if (bucket)
+		bucket = min((NX842_HIST_SLOTS - 1), bucket - 1);
+
+	atomic64_inc(&times[bucket]);
+}
+
+/* NX unit operation flags */
+#define NX842_OP_COMPRESS	0x0
+#define NX842_OP_CRC		0x1
+#define NX842_OP_DECOMPRESS	0x2
+#define NX842_OP_COMPRESS_CRC   (NX842_OP_COMPRESS | NX842_OP_CRC)
+#define NX842_OP_DECOMPRESS_CRC (NX842_OP_DECOMPRESS | NX842_OP_CRC)
+#define NX842_OP_ASYNC		(1<<23)
+#define NX842_OP_NOTIFY		(1<<22)
+#define NX842_OP_NOTIFY_INT(x)	((x & 0xff)<<8)
+
+static unsigned long nx842_get_desired_dma(struct vio_dev *viodev)
+{
+	/* No use of DMA mappings within the driver. */
+	return 0;
+}
+
+struct nx842_slentry {
+	__be64 ptr; /* Real address (use __pa()) */
+	__be64 len;
+};
+
+/* pHyp scatterlist entry */
+struct nx842_scatterlist {
+	int entry_nr; /* number of slentries */
+	struct nx842_slentry *entries; /* ptr to array of slentries */
+};
+
+/* Does not include sizeof(entry_nr) in the size */
+static inline unsigned long nx842_get_scatterlist_size(
+				struct nx842_scatterlist *sl)
+{
+	return sl->entry_nr * sizeof(struct nx842_slentry);
+}
+
+static int nx842_build_scatterlist(unsigned long buf, int len,
+			struct nx842_scatterlist *sl)
+{
+	unsigned long entrylen;
+	struct nx842_slentry *entry;
+
+	sl->entry_nr = 0;
+
+	entry = sl->entries;
+	while (len) {
+		entry->ptr = cpu_to_be64(nx842_get_pa((void *)buf));
+		entrylen = min_t(int, len,
+				 LEN_ON_SIZE(buf, NX842_HW_PAGE_SIZE));
+		entry->len = cpu_to_be64(entrylen);
+
+		len -= entrylen;
+		buf += entrylen;
+
+		sl->entry_nr++;
+		entry++;
+	}
+
+	return 0;
+}
+
+static int nx842_validate_result(struct device *dev,
+	struct cop_status_block *csb)
+{
+	/* The csb must be valid after returning from vio_h_cop_sync */
+	if (!NX842_CSBCBP_VALID_CHK(csb->valid)) {
+		dev_err(dev, "%s: cspcbp not valid upon completion.\n",
+				__func__);
+		dev_dbg(dev, "valid:0x%02x cs:0x%02x cc:0x%02x ce:0x%02x\n",
+				csb->valid,
+				csb->crb_seq_number,
+				csb->completion_code,
+				csb->completion_extension);
+		dev_dbg(dev, "processed_bytes:%d address:0x%016lx\n",
+				be32_to_cpu(csb->processed_byte_count),
+				(unsigned long)be64_to_cpu(csb->address));
+		return -EIO;
+	}
+
+	/* Check return values from the hardware in the CSB */
+	switch (csb->completion_code) {
+	case 0:	/* Completed without error */
+		break;
+	case 64: /* Target bytes > Source bytes during compression */
+	case 13: /* Output buffer too small */
+		dev_dbg(dev, "%s: Compression output larger than input\n",
+					__func__);
+		return -ENOSPC;
+	case 66: /* Input data contains an illegal template field */
+	case 67: /* Template indicates data past the end of the input stream */
+		dev_dbg(dev, "%s: Bad data for decompression (code:%d)\n",
+					__func__, csb->completion_code);
+		return -EINVAL;
+	default:
+		dev_dbg(dev, "%s: Unspecified error (code:%d)\n",
+					__func__, csb->completion_code);
+		return -EIO;
+	}
+
+	/* Hardware sanity check */
+	if (!NX842_CSBCPB_CE2(csb->completion_extension)) {
+		dev_err(dev, "%s: No error returned by hardware, but "
+				"data returned is unusable, contact support.\n"
+				"(Additional info: csbcbp->processed bytes "
+				"does not specify processed bytes for the "
+				"target buffer.)\n", __func__);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/**
+ * nx842_pseries_compress - Compress data using the 842 algorithm
+ *
+ * Compression provide by the NX842 coprocessor on IBM Power systems.
+ * The input buffer is compressed and the result is stored in the
+ * provided output buffer.
+ *
+ * Upon return from this function @outlen contains the length of the
+ * compressed data.  If there is an error then @outlen will be 0 and an
+ * error will be specified by the return code from this function.
+ *
+ * @in: Pointer to input buffer
+ * @inlen: Length of input buffer
+ * @out: Pointer to output buffer
+ * @outlen: Length of output buffer
+ * @wrkmem: ptr to buffer for working memory, size determined by
+ *          nx842_pseries_driver.workmem_size
+ *
+ * Returns:
+ *   0		Success, output of length @outlen stored in the buffer at @out
+ *   -ENOMEM	Unable to allocate internal buffers
+ *   -ENOSPC	Output buffer is to small
+ *   -EIO	Internal error
+ *   -ENODEV	Hardware unavailable
+ */
+static int nx842_pseries_compress(const unsigned char *in, unsigned int inlen,
+				  unsigned char *out, unsigned int *outlen,
+				  void *wmem)
+{
+	struct nx842_devdata *local_devdata;
+	struct device *dev = NULL;
+	struct nx842_workmem *workmem;
+	struct nx842_scatterlist slin, slout;
+	struct nx_csbcpb *csbcpb;
+	int ret = 0, max_sync_size;
+	unsigned long inbuf, outbuf;
+	struct vio_pfo_op op = {
+		.done = NULL,
+		.handle = 0,
+		.timeout = 0,
+	};
+	unsigned long start = get_tb();
+
+	inbuf = (unsigned long)in;
+	if (check_constraints(inbuf, &inlen, true))
+		return -EINVAL;
+
+	outbuf = (unsigned long)out;
+	if (check_constraints(outbuf, outlen, false))
+		return -EINVAL;
+
+	rcu_read_lock();
+	local_devdata = rcu_dereference(devdata);
+	if (!local_devdata || !local_devdata->dev) {
+		rcu_read_unlock();
+		return -ENODEV;
+	}
+	max_sync_size = local_devdata->max_sync_size;
+	dev = local_devdata->dev;
+
+	/* Init scatterlist */
+	workmem = PTR_ALIGN(wmem, WORKMEM_ALIGN);
+	slin.entries = (struct nx842_slentry *)workmem->slin;
+	slout.entries = (struct nx842_slentry *)workmem->slout;
+
+	/* Init operation */
+	op.flags = NX842_OP_COMPRESS;
+	csbcpb = &workmem->csbcpb;
+	memset(csbcpb, 0, sizeof(*csbcpb));
+	op.csbcpb = nx842_get_pa(csbcpb);
+
+	if ((inbuf & NX842_HW_PAGE_MASK) ==
+	    ((inbuf + inlen - 1) & NX842_HW_PAGE_MASK)) {
+		/* Create direct DDE */
+		op.in = nx842_get_pa((void *)inbuf);
+		op.inlen = inlen;
+	} else {
+		/* Create indirect DDE (scatterlist) */
+		nx842_build_scatterlist(inbuf, inlen, &slin);
+		op.in = nx842_get_pa(slin.entries);
+		op.inlen = -nx842_get_scatterlist_size(&slin);
+	}
+
+	if ((outbuf & NX842_HW_PAGE_MASK) ==
+	    ((outbuf + *outlen - 1) & NX842_HW_PAGE_MASK)) {
+		/* Create direct DDE */
+		op.out = nx842_get_pa((void *)outbuf);
+		op.outlen = *outlen;
+	} else {
+		/* Create indirect DDE (scatterlist) */
+		nx842_build_scatterlist(outbuf, *outlen, &slout);
+		op.out = nx842_get_pa(slout.entries);
+		op.outlen = -nx842_get_scatterlist_size(&slout);
+	}
+
+	dev_dbg(dev, "%s: op.in %lx op.inlen %ld op.out %lx op.outlen %ld\n",
+		__func__, (unsigned long)op.in, (long)op.inlen,
+		(unsigned long)op.out, (long)op.outlen);
+
+	/* Send request to pHyp */
+	ret = vio_h_cop_sync(local_devdata->vdev, &op);
+
+	/* Check for pHyp error */
+	if (ret) {
+		dev_dbg(dev, "%s: vio_h_cop_sync error (ret=%d, hret=%ld)\n",
+			__func__, ret, op.hcall_err);
+		ret = -EIO;
+		goto unlock;
+	}
+
+	/* Check for hardware error */
+	ret = nx842_validate_result(dev, &csbcpb->csb);
+	if (ret)
+		goto unlock;
+
+	*outlen = be32_to_cpu(csbcpb->csb.processed_byte_count);
+	dev_dbg(dev, "%s: processed_bytes=%d\n", __func__, *outlen);
+
+unlock:
+	if (ret)
+		nx842_inc_comp_failed(local_devdata);
+	else {
+		nx842_inc_comp_complete(local_devdata);
+		ibm_nx842_incr_hist(local_devdata->counters->comp_times,
+			(get_tb() - start) / tb_ticks_per_usec);
+	}
+	rcu_read_unlock();
+	return ret;
+}
+
+/**
+ * nx842_pseries_decompress - Decompress data using the 842 algorithm
+ *
+ * Decompression provide by the NX842 coprocessor on IBM Power systems.
+ * The input buffer is decompressed and the result is stored in the
+ * provided output buffer.  The size allocated to the output buffer is
+ * provided by the caller of this function in @outlen.  Upon return from
+ * this function @outlen contains the length of the decompressed data.
+ * If there is an error then @outlen will be 0 and an error will be
+ * specified by the return code from this function.
+ *
+ * @in: Pointer to input buffer
+ * @inlen: Length of input buffer
+ * @out: Pointer to output buffer
+ * @outlen: Length of output buffer
+ * @wrkmem: ptr to buffer for working memory, size determined by
+ *          nx842_pseries_driver.workmem_size
+ *
+ * Returns:
+ *   0		Success, output of length @outlen stored in the buffer at @out
+ *   -ENODEV	Hardware decompression device is unavailable
+ *   -ENOMEM	Unable to allocate internal buffers
+ *   -ENOSPC	Output buffer is to small
+ *   -EINVAL	Bad input data encountered when attempting decompress
+ *   -EIO	Internal error
+ */
+static int nx842_pseries_decompress(const unsigned char *in, unsigned int inlen,
+				    unsigned char *out, unsigned int *outlen,
+				    void *wmem)
+{
+	struct nx842_devdata *local_devdata;
+	struct device *dev = NULL;
+	struct nx842_workmem *workmem;
+	struct nx842_scatterlist slin, slout;
+	struct nx_csbcpb *csbcpb;
+	int ret = 0, max_sync_size;
+	unsigned long inbuf, outbuf;
+	struct vio_pfo_op op = {
+		.done = NULL,
+		.handle = 0,
+		.timeout = 0,
+	};
+	unsigned long start = get_tb();
+
+	/* Ensure page alignment and size */
+	inbuf = (unsigned long)in;
+	if (check_constraints(inbuf, &inlen, true))
+		return -EINVAL;
+
+	outbuf = (unsigned long)out;
+	if (check_constraints(outbuf, outlen, false))
+		return -EINVAL;
+
+	rcu_read_lock();
+	local_devdata = rcu_dereference(devdata);
+	if (!local_devdata || !local_devdata->dev) {
+		rcu_read_unlock();
+		return -ENODEV;
+	}
+	max_sync_size = local_devdata->max_sync_size;
+	dev = local_devdata->dev;
+
+	workmem = PTR_ALIGN(wmem, WORKMEM_ALIGN);
+
+	/* Init scatterlist */
+	slin.entries = (struct nx842_slentry *)workmem->slin;
+	slout.entries = (struct nx842_slentry *)workmem->slout;
+
+	/* Init operation */
+	op.flags = NX842_OP_DECOMPRESS;
+	csbcpb = &workmem->csbcpb;
+	memset(csbcpb, 0, sizeof(*csbcpb));
+	op.csbcpb = nx842_get_pa(csbcpb);
+
+	if ((inbuf & NX842_HW_PAGE_MASK) ==
+	    ((inbuf + inlen - 1) & NX842_HW_PAGE_MASK)) {
+		/* Create direct DDE */
+		op.in = nx842_get_pa((void *)inbuf);
+		op.inlen = inlen;
+	} else {
+		/* Create indirect DDE (scatterlist) */
+		nx842_build_scatterlist(inbuf, inlen, &slin);
+		op.in = nx842_get_pa(slin.entries);
+		op.inlen = -nx842_get_scatterlist_size(&slin);
+	}
+
+	if ((outbuf & NX842_HW_PAGE_MASK) ==
+	    ((outbuf + *outlen - 1) & NX842_HW_PAGE_MASK)) {
+		/* Create direct DDE */
+		op.out = nx842_get_pa((void *)outbuf);
+		op.outlen = *outlen;
+	} else {
+		/* Create indirect DDE (scatterlist) */
+		nx842_build_scatterlist(outbuf, *outlen, &slout);
+		op.out = nx842_get_pa(slout.entries);
+		op.outlen = -nx842_get_scatterlist_size(&slout);
+	}
+
+	dev_dbg(dev, "%s: op.in %lx op.inlen %ld op.out %lx op.outlen %ld\n",
+		__func__, (unsigned long)op.in, (long)op.inlen,
+		(unsigned long)op.out, (long)op.outlen);
+
+	/* Send request to pHyp */
+	ret = vio_h_cop_sync(local_devdata->vdev, &op);
+
+	/* Check for pHyp error */
+	if (ret) {
+		dev_dbg(dev, "%s: vio_h_cop_sync error (ret=%d, hret=%ld)\n",
+			__func__, ret, op.hcall_err);
+		goto unlock;
+	}
+
+	/* Check for hardware error */
+	ret = nx842_validate_result(dev, &csbcpb->csb);
+	if (ret)
+		goto unlock;
+
+	*outlen = be32_to_cpu(csbcpb->csb.processed_byte_count);
+
+unlock:
+	if (ret)
+		/* decompress fail */
+		nx842_inc_decomp_failed(local_devdata);
+	else {
+		nx842_inc_decomp_complete(local_devdata);
+		ibm_nx842_incr_hist(local_devdata->counters->decomp_times,
+			(get_tb() - start) / tb_ticks_per_usec);
+	}
+
+	rcu_read_unlock();
+	return ret;
+}
+
+/**
+ * nx842_OF_set_defaults -- Set default (disabled) values for devdata
+ *
+ * @devdata - struct nx842_devdata to update
+ *
+ * Returns:
+ *  0 on success
+ *  -ENOENT if @devdata ptr is NULL
+ */
+static int nx842_OF_set_defaults(struct nx842_devdata *devdata)
+{
+	if (devdata) {
+		devdata->max_sync_size = 0;
+		devdata->max_sync_sg = 0;
+		devdata->max_sg_len = 0;
+		devdata->status = UNAVAILABLE;
+		return 0;
+	} else
+		return -ENOENT;
+}
+
+/**
+ * nx842_OF_upd_status -- Update the device info from OF status prop
+ *
+ * The status property indicates if the accelerator is enabled.  If the
+ * device is in the OF tree it indicates that the hardware is present.
+ * The status field indicates if the device is enabled when the status
+ * is 'okay'.  Otherwise the device driver will be disabled.
+ *
+ * @devdata - struct nx842_devdata to update
+ * @prop - struct property point containing the maxsyncop for the update
+ *
+ * Returns:
+ *  0 - Device is available
+ *  -EINVAL - Device is not available
+ */
+static int nx842_OF_upd_status(struct nx842_devdata *devdata,
+					struct property *prop) {
+	int ret = 0;
+	const char *status = (const char *)prop->value;
+
+	if (!strncmp(status, "okay", (size_t)prop->length)) {
+		devdata->status = AVAILABLE;
+	} else {
+		dev_info(devdata->dev, "%s: status '%s' is not 'okay'\n",
+				__func__, status);
+		devdata->status = UNAVAILABLE;
+	}
+
+	return ret;
+}
+
+/**
+ * nx842_OF_upd_maxsglen -- Update the device info from OF maxsglen prop
+ *
+ * Definition of the 'ibm,max-sg-len' OF property:
+ *  This field indicates the maximum byte length of a scatter list
+ *  for the platform facility. It is a single cell encoded as with encode-int.
+ *
+ * Example:
+ *  # od -x ibm,max-sg-len
+ *  0000000 0000 0ff0
+ *
+ *  In this example, the maximum byte length of a scatter list is
+ *  0x0ff0 (4,080).
+ *
+ * @devdata - struct nx842_devdata to update
+ * @prop - struct property point containing the maxsyncop for the update
+ *
+ * Returns:
+ *  0 on success
+ *  -EINVAL on failure
+ */
+static int nx842_OF_upd_maxsglen(struct nx842_devdata *devdata,
+					struct property *prop) {
+	int ret = 0;
+	const unsigned int maxsglen = of_read_number(prop->value, 1);
+
+	if (prop->length != sizeof(maxsglen)) {
+		dev_err(devdata->dev, "%s: unexpected format for ibm,max-sg-len property\n", __func__);
+		dev_dbg(devdata->dev, "%s: ibm,max-sg-len is %d bytes long, expected %lu bytes\n", __func__,
+				prop->length, sizeof(maxsglen));
+		ret = -EINVAL;
+	} else {
+		devdata->max_sg_len = min_t(unsigned int,
+					    maxsglen, NX842_HW_PAGE_SIZE);
+	}
+
+	return ret;
+}
+
+/**
+ * nx842_OF_upd_maxsyncop -- Update the device info from OF maxsyncop prop
+ *
+ * Definition of the 'ibm,max-sync-cop' OF property:
+ *  Two series of cells.  The first series of cells represents the maximums
+ *  that can be synchronously compressed. The second series of cells
+ *  represents the maximums that can be synchronously decompressed.
+ *  1. The first cell in each series contains the count of the number of
+ *     data length, scatter list elements pairs that follow – each being
+ *     of the form
+ *    a. One cell data byte length
+ *    b. One cell total number of scatter list elements
+ *
+ * Example:
+ *  # od -x ibm,max-sync-cop
+ *  0000000 0000 0001 0000 1000 0000 01fe 0000 0001
+ *  0000020 0000 1000 0000 01fe
+ *
+ *  In this example, compression supports 0x1000 (4,096) data byte length
+ *  and 0x1fe (510) total scatter list elements.  Decompression supports
+ *  0x1000 (4,096) data byte length and 0x1f3 (510) total scatter list
+ *  elements.
+ *
+ * @devdata - struct nx842_devdata to update
+ * @prop - struct property point containing the maxsyncop for the update
+ *
+ * Returns:
+ *  0 on success
+ *  -EINVAL on failure
+ */
+static int nx842_OF_upd_maxsyncop(struct nx842_devdata *devdata,
+					struct property *prop) {
+	int ret = 0;
+	unsigned int comp_data_limit, decomp_data_limit;
+	unsigned int comp_sg_limit, decomp_sg_limit;
+	const struct maxsynccop_t {
+		__be32 comp_elements;
+		__be32 comp_data_limit;
+		__be32 comp_sg_limit;
+		__be32 decomp_elements;
+		__be32 decomp_data_limit;
+		__be32 decomp_sg_limit;
+	} *maxsynccop;
+
+	if (prop->length != sizeof(*maxsynccop)) {
+		dev_err(devdata->dev, "%s: unexpected format for ibm,max-sync-cop property\n", __func__);
+		dev_dbg(devdata->dev, "%s: ibm,max-sync-cop is %d bytes long, expected %lu bytes\n", __func__, prop->length,
+				sizeof(*maxsynccop));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	maxsynccop = (const struct maxsynccop_t *)prop->value;
+	comp_data_limit = be32_to_cpu(maxsynccop->comp_data_limit);
+	comp_sg_limit = be32_to_cpu(maxsynccop->comp_sg_limit);
+	decomp_data_limit = be32_to_cpu(maxsynccop->decomp_data_limit);
+	decomp_sg_limit = be32_to_cpu(maxsynccop->decomp_sg_limit);
+
+	/* Use one limit rather than separate limits for compression and
+	 * decompression. Set a maximum for this so as not to exceed the
+	 * size that the header can support and round the value down to
+	 * the hardware page size (4K) */
+	devdata->max_sync_size = min(comp_data_limit, decomp_data_limit);
+
+	devdata->max_sync_size = min_t(unsigned int, devdata->max_sync_size,
+					65536);
+
+	if (devdata->max_sync_size < 4096) {
+		dev_err(devdata->dev, "%s: hardware max data size (%u) is "
+				"less than the driver minimum, unable to use "
+				"the hardware device\n",
+				__func__, devdata->max_sync_size);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	nx842_pseries_constraints.maximum = devdata->max_sync_size;
+
+	devdata->max_sync_sg = min(comp_sg_limit, decomp_sg_limit);
+	if (devdata->max_sync_sg < 1) {
+		dev_err(devdata->dev, "%s: hardware max sg size (%u) is "
+				"less than the driver minimum, unable to use "
+				"the hardware device\n",
+				__func__, devdata->max_sync_sg);
+		ret = -EINVAL;
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+/**
+ *
+ * nx842_OF_upd -- Handle OF properties updates for the device.
+ *
+ * Set all properties from the OF tree.  Optionally, a new property
+ * can be provided by the @new_prop pointer to overwrite an existing value.
+ * The device will remain disabled until all values are valid, this function
+ * will return an error for updates unless all values are valid.
+ *
+ * @new_prop: If not NULL, this property is being updated.  If NULL, update
+ *  all properties from the current values in the OF tree.
+ *
+ * Returns:
+ *  0 - Success
+ *  -ENOMEM - Could not allocate memory for new devdata structure
+ *  -EINVAL - property value not found, new_prop is not a recognized
+ *	property for the device or property value is not valid.
+ *  -ENODEV - Device is not available
+ */
+static int nx842_OF_upd(struct property *new_prop)
+{
+	struct nx842_devdata *old_devdata = NULL;
+	struct nx842_devdata *new_devdata = NULL;
+	struct device_node *of_node = NULL;
+	struct property *status = NULL;
+	struct property *maxsglen = NULL;
+	struct property *maxsyncop = NULL;
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&devdata_mutex, flags);
+	old_devdata = rcu_dereference_check(devdata,
+			lockdep_is_held(&devdata_mutex));
+	if (old_devdata)
+		of_node = old_devdata->dev->of_node;
+
+	if (!old_devdata || !of_node) {
+		pr_err("%s: device is not available\n", __func__);
+		spin_unlock_irqrestore(&devdata_mutex, flags);
+		return -ENODEV;
+	}
+
+	new_devdata = kzalloc(sizeof(*new_devdata), GFP_NOFS);
+	if (!new_devdata) {
+		dev_err(old_devdata->dev, "%s: Could not allocate memory for device data\n", __func__);
+		ret = -ENOMEM;
+		goto error_out;
+	}
+
+	memcpy(new_devdata, old_devdata, sizeof(*old_devdata));
+	new_devdata->counters = old_devdata->counters;
+
+	/* Set ptrs for existing properties */
+	status = of_find_property(of_node, "status", NULL);
+	maxsglen = of_find_property(of_node, "ibm,max-sg-len", NULL);
+	maxsyncop = of_find_property(of_node, "ibm,max-sync-cop", NULL);
+	if (!status || !maxsglen || !maxsyncop) {
+		dev_err(old_devdata->dev, "%s: Could not locate device properties\n", __func__);
+		ret = -EINVAL;
+		goto error_out;
+	}
+
+	/*
+	 * If this is a property update, there are only certain properties that
+	 * we care about. Bail if it isn't in the below list
+	 */
+	if (new_prop && (strncmp(new_prop->name, "status", new_prop->length) ||
+		         strncmp(new_prop->name, "ibm,max-sg-len", new_prop->length) ||
+		         strncmp(new_prop->name, "ibm,max-sync-cop", new_prop->length)))
+		goto out;
+
+	/* Perform property updates */
+	ret = nx842_OF_upd_status(new_devdata, status);
+	if (ret)
+		goto error_out;
+
+	ret = nx842_OF_upd_maxsglen(new_devdata, maxsglen);
+	if (ret)
+		goto error_out;
+
+	ret = nx842_OF_upd_maxsyncop(new_devdata, maxsyncop);
+	if (ret)
+		goto error_out;
+
+out:
+	dev_info(old_devdata->dev, "%s: max_sync_size new:%u old:%u\n",
+			__func__, new_devdata->max_sync_size,
+			old_devdata->max_sync_size);
+	dev_info(old_devdata->dev, "%s: max_sync_sg new:%u old:%u\n",
+			__func__, new_devdata->max_sync_sg,
+			old_devdata->max_sync_sg);
+	dev_info(old_devdata->dev, "%s: max_sg_len new:%u old:%u\n",
+			__func__, new_devdata->max_sg_len,
+			old_devdata->max_sg_len);
+
+	rcu_assign_pointer(devdata, new_devdata);
+	spin_unlock_irqrestore(&devdata_mutex, flags);
+	synchronize_rcu();
+	dev_set_drvdata(new_devdata->dev, new_devdata);
+	kfree(old_devdata);
+	return 0;
+
+error_out:
+	if (new_devdata) {
+		dev_info(old_devdata->dev, "%s: device disabled\n", __func__);
+		nx842_OF_set_defaults(new_devdata);
+		rcu_assign_pointer(devdata, new_devdata);
+		spin_unlock_irqrestore(&devdata_mutex, flags);
+		synchronize_rcu();
+		dev_set_drvdata(new_devdata->dev, new_devdata);
+		kfree(old_devdata);
+	} else {
+		dev_err(old_devdata->dev, "%s: could not update driver from hardware\n", __func__);
+		spin_unlock_irqrestore(&devdata_mutex, flags);
+	}
+
+	if (!ret)
+		ret = -EINVAL;
+	return ret;
+}
+
+/**
+ * nx842_OF_notifier - Process updates to OF properties for the device
+ *
+ * @np: notifier block
+ * @action: notifier action
+ * @update: struct pSeries_reconfig_prop_update pointer if action is
+ *	PSERIES_UPDATE_PROPERTY
+ *
+ * Returns:
+ *	NOTIFY_OK on success
+ *	NOTIFY_BAD encoded with error number on failure, use
+ *		notifier_to_errno() to decode this value
+ */
+static int nx842_OF_notifier(struct notifier_block *np, unsigned long action,
+			     void *data)
+{
+	struct of_reconfig_data *upd = data;
+	struct nx842_devdata *local_devdata;
+	struct device_node *node = NULL;
+
+	rcu_read_lock();
+	local_devdata = rcu_dereference(devdata);
+	if (local_devdata)
+		node = local_devdata->dev->of_node;
+
+	if (local_devdata &&
+			action == OF_RECONFIG_UPDATE_PROPERTY &&
+			!strcmp(upd->dn->name, node->name)) {
+		rcu_read_unlock();
+		nx842_OF_upd(upd->prop);
+	} else
+		rcu_read_unlock();
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block nx842_of_nb = {
+	.notifier_call = nx842_OF_notifier,
+};
+
+#define nx842_counter_read(_name)					\
+static ssize_t nx842_##_name##_show(struct device *dev,		\
+		struct device_attribute *attr,				\
+		char *buf) {						\
+	struct nx842_devdata *local_devdata;			\
+	int p = 0;							\
+	rcu_read_lock();						\
+	local_devdata = rcu_dereference(devdata);			\
+	if (local_devdata)						\
+		p = snprintf(buf, PAGE_SIZE, "%ld\n",			\
+		       atomic64_read(&local_devdata->counters->_name));	\
+	rcu_read_unlock();						\
+	return p;							\
+}
+
+#define NX842DEV_COUNTER_ATTR_RO(_name)					\
+	nx842_counter_read(_name);					\
+	static struct device_attribute dev_attr_##_name = __ATTR(_name,	\
+						0444,			\
+						nx842_##_name##_show,\
+						NULL);
+
+NX842DEV_COUNTER_ATTR_RO(comp_complete);
+NX842DEV_COUNTER_ATTR_RO(comp_failed);
+NX842DEV_COUNTER_ATTR_RO(decomp_complete);
+NX842DEV_COUNTER_ATTR_RO(decomp_failed);
+NX842DEV_COUNTER_ATTR_RO(swdecomp);
+
+static ssize_t nx842_timehist_show(struct device *,
+		struct device_attribute *, char *);
+
+static struct device_attribute dev_attr_comp_times = __ATTR(comp_times, 0444,
+		nx842_timehist_show, NULL);
+static struct device_attribute dev_attr_decomp_times = __ATTR(decomp_times,
+		0444, nx842_timehist_show, NULL);
+
+static ssize_t nx842_timehist_show(struct device *dev,
+		struct device_attribute *attr, char *buf) {
+	char *p = buf;
+	struct nx842_devdata *local_devdata;
+	atomic64_t *times;
+	int bytes_remain = PAGE_SIZE;
+	int bytes;
+	int i;
+
+	rcu_read_lock();
+	local_devdata = rcu_dereference(devdata);
+	if (!local_devdata) {
+		rcu_read_unlock();
+		return 0;
+	}
+
+	if (attr == &dev_attr_comp_times)
+		times = local_devdata->counters->comp_times;
+	else if (attr == &dev_attr_decomp_times)
+		times = local_devdata->counters->decomp_times;
+	else {
+		rcu_read_unlock();
+		return 0;
+	}
+
+	for (i = 0; i < (NX842_HIST_SLOTS - 2); i++) {
+		bytes = snprintf(p, bytes_remain, "%u-%uus:\t%ld\n",
+			       i ? (2<<(i-1)) : 0, (2<<i)-1,
+			       atomic64_read(&times[i]));
+		bytes_remain -= bytes;
+		p += bytes;
+	}
+	/* The last bucket holds everything over
+	 * 2<<(NX842_HIST_SLOTS - 2) us */
+	bytes = snprintf(p, bytes_remain, "%uus - :\t%ld\n",
+			2<<(NX842_HIST_SLOTS - 2),
+			atomic64_read(&times[(NX842_HIST_SLOTS - 1)]));
+	p += bytes;
+
+	rcu_read_unlock();
+	return p - buf;
+}
+
+static struct attribute *nx842_sysfs_entries[] = {
+	&dev_attr_comp_complete.attr,
+	&dev_attr_comp_failed.attr,
+	&dev_attr_decomp_complete.attr,
+	&dev_attr_decomp_failed.attr,
+	&dev_attr_swdecomp.attr,
+	&dev_attr_comp_times.attr,
+	&dev_attr_decomp_times.attr,
+	NULL,
+};
+
+static struct attribute_group nx842_attribute_group = {
+	.name = NULL,		/* put in device directory */
+	.attrs = nx842_sysfs_entries,
+};
+
+static struct nx842_driver nx842_pseries_driver = {
+	.name =		KBUILD_MODNAME,
+	.owner =	THIS_MODULE,
+	.workmem_size =	sizeof(struct nx842_workmem),
+	.constraints =	&nx842_pseries_constraints,
+	.compress =	nx842_pseries_compress,
+	.decompress =	nx842_pseries_decompress,
+};
+
+static int __init nx842_probe(struct vio_dev *viodev,
+				  const struct vio_device_id *id)
+{
+	struct nx842_devdata *old_devdata, *new_devdata = NULL;
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&devdata_mutex, flags);
+	old_devdata = rcu_dereference_check(devdata,
+			lockdep_is_held(&devdata_mutex));
+
+	if (old_devdata && old_devdata->vdev != NULL) {
+		dev_err(&viodev->dev, "%s: Attempt to register more than one instance of the hardware\n", __func__);
+		ret = -1;
+		goto error_unlock;
+	}
+
+	dev_set_drvdata(&viodev->dev, NULL);
+
+	new_devdata = kzalloc(sizeof(*new_devdata), GFP_NOFS);
+	if (!new_devdata) {
+		dev_err(&viodev->dev, "%s: Could not allocate memory for device data\n", __func__);
+		ret = -ENOMEM;
+		goto error_unlock;
+	}
+
+	new_devdata->counters = kzalloc(sizeof(*new_devdata->counters),
+			GFP_NOFS);
+	if (!new_devdata->counters) {
+		dev_err(&viodev->dev, "%s: Could not allocate memory for performance counters\n", __func__);
+		ret = -ENOMEM;
+		goto error_unlock;
+	}
+
+	new_devdata->vdev = viodev;
+	new_devdata->dev = &viodev->dev;
+	nx842_OF_set_defaults(new_devdata);
+
+	rcu_assign_pointer(devdata, new_devdata);
+	spin_unlock_irqrestore(&devdata_mutex, flags);
+	synchronize_rcu();
+	kfree(old_devdata);
+
+	of_reconfig_notifier_register(&nx842_of_nb);
+
+	ret = nx842_OF_upd(NULL);
+	if (ret && ret != -ENODEV) {
+		dev_err(&viodev->dev, "could not parse device tree. %d\n", ret);
+		ret = -1;
+		goto error;
+	}
+
+	rcu_read_lock();
+	dev_set_drvdata(&viodev->dev, rcu_dereference(devdata));
+	rcu_read_unlock();
+
+	if (sysfs_create_group(&viodev->dev.kobj, &nx842_attribute_group)) {
+		dev_err(&viodev->dev, "could not create sysfs device attributes\n");
+		ret = -1;
+		goto error;
+	}
+
+	return 0;
+
+error_unlock:
+	spin_unlock_irqrestore(&devdata_mutex, flags);
+	if (new_devdata)
+		kfree(new_devdata->counters);
+	kfree(new_devdata);
+error:
+	return ret;
+}
+
+static int __exit nx842_remove(struct vio_dev *viodev)
+{
+	struct nx842_devdata *old_devdata;
+	unsigned long flags;
+
+	pr_info("Removing IBM Power 842 compression device\n");
+	sysfs_remove_group(&viodev->dev.kobj, &nx842_attribute_group);
+
+	spin_lock_irqsave(&devdata_mutex, flags);
+	old_devdata = rcu_dereference_check(devdata,
+			lockdep_is_held(&devdata_mutex));
+	of_reconfig_notifier_unregister(&nx842_of_nb);
+	RCU_INIT_POINTER(devdata, NULL);
+	spin_unlock_irqrestore(&devdata_mutex, flags);
+	synchronize_rcu();
+	dev_set_drvdata(&viodev->dev, NULL);
+	if (old_devdata)
+		kfree(old_devdata->counters);
+	kfree(old_devdata);
+
+	return 0;
+}
+
+static struct vio_device_id nx842_vio_driver_ids[] = {
+	{"ibm,compression-v1", "ibm,compression"},
+	{"", ""},
+};
+
+static struct vio_driver nx842_vio_driver = {
+	.name = KBUILD_MODNAME,
+	.probe = nx842_probe,
+	.remove = __exit_p(nx842_remove),
+	.get_desired_dma = nx842_get_desired_dma,
+	.id_table = nx842_vio_driver_ids,
+};
+
+static int __init nx842_init(void)
+{
+	struct nx842_devdata *new_devdata;
+	int ret;
+
+	pr_info("Registering IBM Power 842 compression driver\n");
+
+	if (!of_find_compatible_node(NULL, NULL, "ibm,compression"))
+		return -ENODEV;
+
+	RCU_INIT_POINTER(devdata, NULL);
+	new_devdata = kzalloc(sizeof(*new_devdata), GFP_KERNEL);
+	if (!new_devdata) {
+		pr_err("Could not allocate memory for device data\n");
+		return -ENOMEM;
+	}
+	new_devdata->status = UNAVAILABLE;
+	RCU_INIT_POINTER(devdata, new_devdata);
+
+	ret = vio_register_driver(&nx842_vio_driver);
+	if (ret) {
+		pr_err("Could not register VIO driver %d\n", ret);
+
+		kfree(new_devdata);
+		return ret;
+	}
+
+	if (!nx842_platform_driver_set(&nx842_pseries_driver)) {
+		vio_unregister_driver(&nx842_vio_driver);
+		kfree(new_devdata);
+		return -EEXIST;
+	}
+
+	return 0;
+}
+
+module_init(nx842_init);
+
+static void __exit nx842_exit(void)
+{
+	struct nx842_devdata *old_devdata;
+	unsigned long flags;
+
+	pr_info("Exiting IBM Power 842 compression driver\n");
+	nx842_platform_driver_unset(&nx842_pseries_driver);
+	spin_lock_irqsave(&devdata_mutex, flags);
+	old_devdata = rcu_dereference_check(devdata,
+			lockdep_is_held(&devdata_mutex));
+	RCU_INIT_POINTER(devdata, NULL);
+	spin_unlock_irqrestore(&devdata_mutex, flags);
+	synchronize_rcu();
+	if (old_devdata && old_devdata->dev)
+		dev_set_drvdata(old_devdata->dev, NULL);
+	kfree(old_devdata);
+	vio_unregister_driver(&nx842_vio_driver);
+}
+
+module_exit(nx842_exit);
+
diff --git a/drivers/crypto/nx/nx-842.c b/drivers/crypto/nx/nx-842.c
index 887196e9b50c..6e5e0d60d0c8 100644
--- a/drivers/crypto/nx/nx-842.c
+++ b/drivers/crypto/nx/nx-842.c
@@ -1,5 +1,10 @@
 /*
- * Driver for IBM Power 842 compression accelerator
+ * Driver frontend for IBM Power 842 compression accelerator
+ *
+ * Copyright (C) 2015 Dan Streetman, IBM Corp
+ *
+ * Designer of the Power data compression engine:
+ *   Bulent Abali <abali@us.ibm.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -10,1594 +15,89 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
- *
- * Copyright (C) IBM Corporation, 2012
- *
- * Authors: Robert Jennings <rcj@linux.vnet.ibm.com>
- *          Seth Jennings <sjenning@linux.vnet.ibm.com>
  */
 
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/nx842.h>
-#include <linux/of.h>
-#include <linux/slab.h>
-
-#include <asm/page.h>
-#include <asm/vio.h>
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include "nx_csbcpb.h" /* struct nx_csbcpb */
+#include "nx-842.h"
 
-#define MODULE_NAME "nx-compress"
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Robert Jennings <rcj@linux.vnet.ibm.com>");
+MODULE_AUTHOR("Dan Streetman <ddstreet@ieee.org>");
 MODULE_DESCRIPTION("842 H/W Compression driver for IBM Power processors");
 
-#define SHIFT_4K 12
-#define SHIFT_64K 16
-#define SIZE_4K (1UL << SHIFT_4K)
-#define SIZE_64K (1UL << SHIFT_64K)
-
-/* IO buffer must be 128 byte aligned */
-#define IO_BUFFER_ALIGN 128
-
-struct nx842_header {
-	int blocks_nr; /* number of compressed blocks */
-	int offset; /* offset of the first block (from beginning of header) */
-	int sizes[0]; /* size of compressed blocks */
-};
-
-static inline int nx842_header_size(const struct nx842_header *hdr)
-{
-	return sizeof(struct nx842_header) +
-			hdr->blocks_nr * sizeof(hdr->sizes[0]);
-}
-
-/* Macros for fields within nx_csbcpb */
-/* Check the valid bit within the csbcpb valid field */
-#define NX842_CSBCBP_VALID_CHK(x) (x & BIT_MASK(7))
-
-/* CE macros operate on the completion_extension field bits in the csbcpb.
- * CE0 0=full completion, 1=partial completion
- * CE1 0=CE0 indicates completion, 1=termination (output may be modified)
- * CE2 0=processed_bytes is source bytes, 1=processed_bytes is target bytes */
-#define NX842_CSBCPB_CE0(x)	(x & BIT_MASK(7))
-#define NX842_CSBCPB_CE1(x)	(x & BIT_MASK(6))
-#define NX842_CSBCPB_CE2(x)	(x & BIT_MASK(5))
-
-/* The NX unit accepts data only on 4K page boundaries */
-#define NX842_HW_PAGE_SHIFT	SHIFT_4K
-#define NX842_HW_PAGE_SIZE	(ASM_CONST(1) << NX842_HW_PAGE_SHIFT)
-#define NX842_HW_PAGE_MASK	(~(NX842_HW_PAGE_SIZE-1))
-
-enum nx842_status {
-	UNAVAILABLE,
-	AVAILABLE
-};
-
-struct ibm_nx842_counters {
-	atomic64_t comp_complete;
-	atomic64_t comp_failed;
-	atomic64_t decomp_complete;
-	atomic64_t decomp_failed;
-	atomic64_t swdecomp;
-	atomic64_t comp_times[32];
-	atomic64_t decomp_times[32];
-};
-
-static struct nx842_devdata {
-	struct vio_dev *vdev;
-	struct device *dev;
-	struct ibm_nx842_counters *counters;
-	unsigned int max_sg_len;
-	unsigned int max_sync_size;
-	unsigned int max_sync_sg;
-	enum nx842_status status;
-} __rcu *devdata;
-static DEFINE_SPINLOCK(devdata_mutex);
-
-#define NX842_COUNTER_INC(_x) \
-static inline void nx842_inc_##_x( \
-	const struct nx842_devdata *dev) { \
-	if (dev) \
-		atomic64_inc(&dev->counters->_x); \
-}
-NX842_COUNTER_INC(comp_complete);
-NX842_COUNTER_INC(comp_failed);
-NX842_COUNTER_INC(decomp_complete);
-NX842_COUNTER_INC(decomp_failed);
-NX842_COUNTER_INC(swdecomp);
-
-#define NX842_HIST_SLOTS 16
-
-static void ibm_nx842_incr_hist(atomic64_t *times, unsigned int time)
-{
-	int bucket = fls(time);
-
-	if (bucket)
-		bucket = min((NX842_HIST_SLOTS - 1), bucket - 1);
-
-	atomic64_inc(&times[bucket]);
-}
-
-/* NX unit operation flags */
-#define NX842_OP_COMPRESS	0x0
-#define NX842_OP_CRC		0x1
-#define NX842_OP_DECOMPRESS	0x2
-#define NX842_OP_COMPRESS_CRC   (NX842_OP_COMPRESS | NX842_OP_CRC)
-#define NX842_OP_DECOMPRESS_CRC (NX842_OP_DECOMPRESS | NX842_OP_CRC)
-#define NX842_OP_ASYNC		(1<<23)
-#define NX842_OP_NOTIFY		(1<<22)
-#define NX842_OP_NOTIFY_INT(x)	((x & 0xff)<<8)
-
-static unsigned long nx842_get_desired_dma(struct vio_dev *viodev)
-{
-	/* No use of DMA mappings within the driver. */
-	return 0;
-}
-
-struct nx842_slentry {
-	unsigned long ptr; /* Real address (use __pa()) */
-	unsigned long len;
-};
-
-/* pHyp scatterlist entry */
-struct nx842_scatterlist {
-	int entry_nr; /* number of slentries */
-	struct nx842_slentry *entries; /* ptr to array of slentries */
-};
-
-/* Does not include sizeof(entry_nr) in the size */
-static inline unsigned long nx842_get_scatterlist_size(
-				struct nx842_scatterlist *sl)
-{
-	return sl->entry_nr * sizeof(struct nx842_slentry);
-}
-
-static inline unsigned long nx842_get_pa(void *addr)
-{
-	if (is_vmalloc_addr(addr))
-		return page_to_phys(vmalloc_to_page(addr))
-		       + offset_in_page(addr);
-	else
-		return __pa(addr);
-}
-
-static int nx842_build_scatterlist(unsigned long buf, int len,
-			struct nx842_scatterlist *sl)
-{
-	unsigned long nextpage;
-	struct nx842_slentry *entry;
-
-	sl->entry_nr = 0;
-
-	entry = sl->entries;
-	while (len) {
-		entry->ptr = nx842_get_pa((void *)buf);
-		nextpage = ALIGN(buf + 1, NX842_HW_PAGE_SIZE);
-		if (nextpage < buf + len) {
-			/* we aren't at the end yet */
-			if (IS_ALIGNED(buf, NX842_HW_PAGE_SIZE))
-				/* we are in the middle (or beginning) */
-				entry->len = NX842_HW_PAGE_SIZE;
-			else
-				/* we are at the beginning */
-				entry->len = nextpage - buf;
-		} else {
-			/* at the end */
-			entry->len = len;
-		}
-
-		len -= entry->len;
-		buf += entry->len;
-		sl->entry_nr++;
-		entry++;
-	}
-
-	return 0;
-}
-
-/*
- * Working memory for software decompression
- */
-struct sw842_fifo {
-	union {
-		char f8[256][8];
-		char f4[512][4];
-	};
-	char f2[256][2];
-	unsigned char f84_full;
-	unsigned char f2_full;
-	unsigned char f8_count;
-	unsigned char f2_count;
-	unsigned int f4_count;
-};
-
-/*
- * Working memory for crypto API
+/**
+ * nx842_constraints
+ *
+ * This provides the driver's constraints.  Different nx842 implementations
+ * may have varying requirements.  The constraints are:
+ *   @alignment:	All buffers should be aligned to this
+ *   @multiple:		All buffer lengths should be a multiple of this
+ *   @minimum:		Buffer lengths must not be less than this amount
+ *   @maximum:		Buffer lengths must not be more than this amount
+ *
+ * The constraints apply to all buffers and lengths, both input and output,
+ * for both compression and decompression, except for the minimum which
+ * only applies to compression input and decompression output; the
+ * compressed data can be less than the minimum constraint.  It can be
+ * assumed that compressed data will always adhere to the multiple
+ * constraint.
+ *
+ * The driver may succeed even if these constraints are violated;
+ * however the driver can return failure or suffer reduced performance
+ * if any constraint is not met.
  */
-struct nx842_workmem {
-	char bounce[PAGE_SIZE]; /* bounce buffer for decompression input */
-	union {
-		/* hardware working memory */
-		struct {
-			/* scatterlist */
-			char slin[SIZE_4K];
-			char slout[SIZE_4K];
-			/* coprocessor status/parameter block */
-			struct nx_csbcpb csbcpb;
-		};
-		/* software working memory */
-		struct sw842_fifo swfifo; /* software decompression fifo */
-	};
-};
-
-int nx842_get_workmem_size(void)
-{
-	return sizeof(struct nx842_workmem) + NX842_HW_PAGE_SIZE;
-}
-EXPORT_SYMBOL_GPL(nx842_get_workmem_size);
-
-int nx842_get_workmem_size_aligned(void)
-{
-	return sizeof(struct nx842_workmem);
-}
-EXPORT_SYMBOL_GPL(nx842_get_workmem_size_aligned);
-
-static int nx842_validate_result(struct device *dev,
-	struct cop_status_block *csb)
+int nx842_constraints(struct nx842_constraints *c)
 {
-	/* The csb must be valid after returning from vio_h_cop_sync */
-	if (!NX842_CSBCBP_VALID_CHK(csb->valid)) {
-		dev_err(dev, "%s: cspcbp not valid upon completion.\n",
-				__func__);
-		dev_dbg(dev, "valid:0x%02x cs:0x%02x cc:0x%02x ce:0x%02x\n",
-				csb->valid,
-				csb->crb_seq_number,
-				csb->completion_code,
-				csb->completion_extension);
-		dev_dbg(dev, "processed_bytes:%d address:0x%016lx\n",
-				csb->processed_byte_count,
-				(unsigned long)csb->address);
-		return -EIO;
-	}
-
-	/* Check return values from the hardware in the CSB */
-	switch (csb->completion_code) {
-	case 0:	/* Completed without error */
-		break;
-	case 64: /* Target bytes > Source bytes during compression */
-	case 13: /* Output buffer too small */
-		dev_dbg(dev, "%s: Compression output larger than input\n",
-					__func__);
-		return -ENOSPC;
-	case 66: /* Input data contains an illegal template field */
-	case 67: /* Template indicates data past the end of the input stream */
-		dev_dbg(dev, "%s: Bad data for decompression (code:%d)\n",
-					__func__, csb->completion_code);
-		return -EINVAL;
-	default:
-		dev_dbg(dev, "%s: Unspecified error (code:%d)\n",
-					__func__, csb->completion_code);
-		return -EIO;
-	}
-
-	/* Hardware sanity check */
-	if (!NX842_CSBCPB_CE2(csb->completion_extension)) {
-		dev_err(dev, "%s: No error returned by hardware, but "
-				"data returned is unusable, contact support.\n"
-				"(Additional info: csbcbp->processed bytes "
-				"does not specify processed bytes for the "
-				"target buffer.)\n", __func__);
-		return -EIO;
-	}
-
+	memcpy(c, nx842_platform_driver()->constraints, sizeof(*c));
 	return 0;
 }
+EXPORT_SYMBOL_GPL(nx842_constraints);
 
 /**
- * nx842_compress - Compress data using the 842 algorithm
- *
- * Compression provide by the NX842 coprocessor on IBM Power systems.
- * The input buffer is compressed and the result is stored in the
- * provided output buffer.
- *
- * Upon return from this function @outlen contains the length of the
- * compressed data.  If there is an error then @outlen will be 0 and an
- * error will be specified by the return code from this function.
- *
- * @in: Pointer to input buffer, must be page aligned
- * @inlen: Length of input buffer, must be PAGE_SIZE
- * @out: Pointer to output buffer
- * @outlen: Length of output buffer
- * @wrkmem: ptr to buffer for working memory, size determined by
- *          nx842_get_workmem_size()
+ * nx842_workmem_size
  *
- * Returns:
- *   0		Success, output of length @outlen stored in the buffer at @out
- *   -ENOMEM	Unable to allocate internal buffers
- *   -ENOSPC	Output buffer is to small
- *   -EMSGSIZE	XXX Difficult to describe this limitation
- *   -EIO	Internal error
- *   -ENODEV	Hardware unavailable
+ * Get the amount of working memory the driver requires.
  */
-int nx842_compress(const unsigned char *in, unsigned int inlen,
-		       unsigned char *out, unsigned int *outlen, void *wmem)
+size_t nx842_workmem_size(void)
 {
-	struct nx842_header *hdr;
-	struct nx842_devdata *local_devdata;
-	struct device *dev = NULL;
-	struct nx842_workmem *workmem;
-	struct nx842_scatterlist slin, slout;
-	struct nx_csbcpb *csbcpb;
-	int ret = 0, max_sync_size, i, bytesleft, size, hdrsize;
-	unsigned long inbuf, outbuf, padding;
-	struct vio_pfo_op op = {
-		.done = NULL,
-		.handle = 0,
-		.timeout = 0,
-	};
-	unsigned long start_time = get_tb();
-
-	/*
-	 * Make sure input buffer is 64k page aligned.  This is assumed since
-	 * this driver is designed for page compression only (for now).  This
-	 * is very nice since we can now use direct DDE(s) for the input and
-	 * the alignment is guaranteed.
-	*/
-	inbuf = (unsigned long)in;
-	if (!IS_ALIGNED(inbuf, PAGE_SIZE) || inlen != PAGE_SIZE)
-		return -EINVAL;
-
-	rcu_read_lock();
-	local_devdata = rcu_dereference(devdata);
-	if (!local_devdata || !local_devdata->dev) {
-		rcu_read_unlock();
-		return -ENODEV;
-	}
-	max_sync_size = local_devdata->max_sync_size;
-	dev = local_devdata->dev;
-
-	/* Create the header */
-	hdr = (struct nx842_header *)out;
-	hdr->blocks_nr = PAGE_SIZE / max_sync_size;
-	hdrsize = nx842_header_size(hdr);
-	outbuf = (unsigned long)out + hdrsize;
-	bytesleft = *outlen - hdrsize;
-
-	/* Init scatterlist */
-	workmem = (struct nx842_workmem *)ALIGN((unsigned long)wmem,
-		NX842_HW_PAGE_SIZE);
-	slin.entries = (struct nx842_slentry *)workmem->slin;
-	slout.entries = (struct nx842_slentry *)workmem->slout;
-
-	/* Init operation */
-	op.flags = NX842_OP_COMPRESS;
-	csbcpb = &workmem->csbcpb;
-	memset(csbcpb, 0, sizeof(*csbcpb));
-	op.csbcpb = nx842_get_pa(csbcpb);
-	op.out = nx842_get_pa(slout.entries);
-
-	for (i = 0; i < hdr->blocks_nr; i++) {
-		/*
-		 * Aligning the output blocks to 128 bytes does waste space,
-		 * but it prevents the need for bounce buffers and memory
-		 * copies.  It also simplifies the code a lot.  In the worst
-		 * case (64k page, 4k max_sync_size), you lose up to
-		 * (128*16)/64k = ~3% the compression factor. For 64k
-		 * max_sync_size, the loss would be at most 128/64k = ~0.2%.
-		 */
-		padding = ALIGN(outbuf, IO_BUFFER_ALIGN) - outbuf;
-		outbuf += padding;
-		bytesleft -= padding;
-		if (i == 0)
-			/* save offset into first block in header */
-			hdr->offset = padding + hdrsize;
-
-		if (bytesleft <= 0) {
-			ret = -ENOSPC;
-			goto unlock;
-		}
-
-		/*
-		 * NOTE: If the default max_sync_size is changed from 4k
-		 * to 64k, remove the "likely" case below, since a
-		 * scatterlist will always be needed.
-		 */
-		if (likely(max_sync_size == NX842_HW_PAGE_SIZE)) {
-			/* Create direct DDE */
-			op.in = nx842_get_pa((void *)inbuf);
-			op.inlen = max_sync_size;
-
-		} else {
-			/* Create indirect DDE (scatterlist) */
-			nx842_build_scatterlist(inbuf, max_sync_size, &slin);
-			op.in = nx842_get_pa(slin.entries);
-			op.inlen = -nx842_get_scatterlist_size(&slin);
-		}
-
-		/*
-		 * If max_sync_size != NX842_HW_PAGE_SIZE, an indirect
-		 * DDE is required for the outbuf.
-		 * If max_sync_size == NX842_HW_PAGE_SIZE, outbuf must
-		 * also be page aligned (1 in 128/4k=32 chance) in order
-		 * to use a direct DDE.
-		 * This is unlikely, just use an indirect DDE always.
-		 */
-		nx842_build_scatterlist(outbuf,
-			min(bytesleft, max_sync_size), &slout);
-		/* op.out set before loop */
-		op.outlen = -nx842_get_scatterlist_size(&slout);
-
-		/* Send request to pHyp */
-		ret = vio_h_cop_sync(local_devdata->vdev, &op);
-
-		/* Check for pHyp error */
-		if (ret) {
-			dev_dbg(dev, "%s: vio_h_cop_sync error (ret=%d, hret=%ld)\n",
-				__func__, ret, op.hcall_err);
-			ret = -EIO;
-			goto unlock;
-		}
-
-		/* Check for hardware error */
-		ret = nx842_validate_result(dev, &csbcpb->csb);
-		if (ret && ret != -ENOSPC)
-			goto unlock;
-
-		/* Handle incompressible data */
-		if (unlikely(ret == -ENOSPC)) {
-			if (bytesleft < max_sync_size) {
-				/*
-				 * Not enough space left in the output buffer
-				 * to store uncompressed block
-				 */
-				goto unlock;
-			} else {
-				/* Store incompressible block */
-				memcpy((void *)outbuf, (void *)inbuf,
-					max_sync_size);
-				hdr->sizes[i] = -max_sync_size;
-				outbuf += max_sync_size;
-				bytesleft -= max_sync_size;
-				/* Reset ret, incompressible data handled */
-				ret = 0;
-			}
-		} else {
-			/* Normal case, compression was successful */
-			size = csbcpb->csb.processed_byte_count;
-			dev_dbg(dev, "%s: processed_bytes=%d\n",
-				__func__, size);
-			hdr->sizes[i] = size;
-			outbuf += size;
-			bytesleft -= size;
-		}
-
-		inbuf += max_sync_size;
-	}
-
-	*outlen = (unsigned int)(outbuf - (unsigned long)out);
-
-unlock:
-	if (ret)
-		nx842_inc_comp_failed(local_devdata);
-	else {
-		nx842_inc_comp_complete(local_devdata);
-		ibm_nx842_incr_hist(local_devdata->counters->comp_times,
-			(get_tb() - start_time) / tb_ticks_per_usec);
-	}
-	rcu_read_unlock();
-	return ret;
+	return nx842_platform_driver()->workmem_size;
 }
-EXPORT_SYMBOL_GPL(nx842_compress);
-
-static int sw842_decompress(const unsigned char *, int, unsigned char *, int *,
-			const void *);
+EXPORT_SYMBOL_GPL(nx842_workmem_size);
 
-/**
- * nx842_decompress - Decompress data using the 842 algorithm
- *
- * Decompression provide by the NX842 coprocessor on IBM Power systems.
- * The input buffer is decompressed and the result is stored in the
- * provided output buffer.  The size allocated to the output buffer is
- * provided by the caller of this function in @outlen.  Upon return from
- * this function @outlen contains the length of the decompressed data.
- * If there is an error then @outlen will be 0 and an error will be
- * specified by the return code from this function.
- *
- * @in: Pointer to input buffer, will use bounce buffer if not 128 byte
- *      aligned
- * @inlen: Length of input buffer
- * @out: Pointer to output buffer, must be page aligned
- * @outlen: Length of output buffer, must be PAGE_SIZE
- * @wrkmem: ptr to buffer for working memory, size determined by
- *          nx842_get_workmem_size()
- *
- * Returns:
- *   0		Success, output of length @outlen stored in the buffer at @out
- *   -ENODEV	Hardware decompression device is unavailable
- *   -ENOMEM	Unable to allocate internal buffers
- *   -ENOSPC	Output buffer is to small
- *   -EINVAL	Bad input data encountered when attempting decompress
- *   -EIO	Internal error
- */
-int nx842_decompress(const unsigned char *in, unsigned int inlen,
-			 unsigned char *out, unsigned int *outlen, void *wmem)
+int nx842_compress(const unsigned char *in, unsigned int ilen,
+		   unsigned char *out, unsigned int *olen, void *wmem)
 {
-	struct nx842_header *hdr;
-	struct nx842_devdata *local_devdata;
-	struct device *dev = NULL;
-	struct nx842_workmem *workmem;
-	struct nx842_scatterlist slin, slout;
-	struct nx_csbcpb *csbcpb;
-	int ret = 0, i, size, max_sync_size;
-	unsigned long inbuf, outbuf;
-	struct vio_pfo_op op = {
-		.done = NULL,
-		.handle = 0,
-		.timeout = 0,
-	};
-	unsigned long start_time = get_tb();
-
-	/* Ensure page alignment and size */
-	outbuf = (unsigned long)out;
-	if (!IS_ALIGNED(outbuf, PAGE_SIZE) || *outlen != PAGE_SIZE)
-		return -EINVAL;
-
-	rcu_read_lock();
-	local_devdata = rcu_dereference(devdata);
-	if (local_devdata)
-		dev = local_devdata->dev;
-
-	/* Get header */
-	hdr = (struct nx842_header *)in;
-
-	workmem = (struct nx842_workmem *)ALIGN((unsigned long)wmem,
-		NX842_HW_PAGE_SIZE);
-
-	inbuf = (unsigned long)in + hdr->offset;
-	if (likely(!IS_ALIGNED(inbuf, IO_BUFFER_ALIGN))) {
-		/* Copy block(s) into bounce buffer for alignment */
-		memcpy(workmem->bounce, in + hdr->offset, inlen - hdr->offset);
-		inbuf = (unsigned long)workmem->bounce;
-	}
-
-	/* Init scatterlist */
-	slin.entries = (struct nx842_slentry *)workmem->slin;
-	slout.entries = (struct nx842_slentry *)workmem->slout;
-
-	/* Init operation */
-	op.flags = NX842_OP_DECOMPRESS;
-	csbcpb = &workmem->csbcpb;
-	memset(csbcpb, 0, sizeof(*csbcpb));
-	op.csbcpb = nx842_get_pa(csbcpb);
-
-	/*
-	 * max_sync_size may have changed since compression,
-	 * so we can't read it from the device info. We need
-	 * to derive it from hdr->blocks_nr.
-	 */
-	max_sync_size = PAGE_SIZE / hdr->blocks_nr;
-
-	for (i = 0; i < hdr->blocks_nr; i++) {
-		/* Skip padding */
-		inbuf = ALIGN(inbuf, IO_BUFFER_ALIGN);
-
-		if (hdr->sizes[i] < 0) {
-			/* Negative sizes indicate uncompressed data blocks */
-			size = abs(hdr->sizes[i]);
-			memcpy((void *)outbuf, (void *)inbuf, size);
-			outbuf += size;
-			inbuf += size;
-			continue;
-		}
-
-		if (!dev)
-			goto sw;
-
-		/*
-		 * The better the compression, the more likely the "likely"
-		 * case becomes.
-		 */
-		if (likely((inbuf & NX842_HW_PAGE_MASK) ==
-			((inbuf + hdr->sizes[i] - 1) & NX842_HW_PAGE_MASK))) {
-			/* Create direct DDE */
-			op.in = nx842_get_pa((void *)inbuf);
-			op.inlen = hdr->sizes[i];
-		} else {
-			/* Create indirect DDE (scatterlist) */
-			nx842_build_scatterlist(inbuf, hdr->sizes[i] , &slin);
-			op.in = nx842_get_pa(slin.entries);
-			op.inlen = -nx842_get_scatterlist_size(&slin);
-		}
-
-		/*
-		 * NOTE: If the default max_sync_size is changed from 4k
-		 * to 64k, remove the "likely" case below, since a
-		 * scatterlist will always be needed.
-		 */
-		if (likely(max_sync_size == NX842_HW_PAGE_SIZE)) {
-			/* Create direct DDE */
-			op.out = nx842_get_pa((void *)outbuf);
-			op.outlen = max_sync_size;
-		} else {
-			/* Create indirect DDE (scatterlist) */
-			nx842_build_scatterlist(outbuf, max_sync_size, &slout);
-			op.out = nx842_get_pa(slout.entries);
-			op.outlen = -nx842_get_scatterlist_size(&slout);
-		}
-
-		/* Send request to pHyp */
-		ret = vio_h_cop_sync(local_devdata->vdev, &op);
-
-		/* Check for pHyp error */
-		if (ret) {
-			dev_dbg(dev, "%s: vio_h_cop_sync error (ret=%d, hret=%ld)\n",
-				__func__, ret, op.hcall_err);
-			dev = NULL;
-			goto sw;
-		}
-
-		/* Check for hardware error */
-		ret = nx842_validate_result(dev, &csbcpb->csb);
-		if (ret) {
-			dev = NULL;
-			goto sw;
-		}
-
-		/* HW decompression success */
-		inbuf += hdr->sizes[i];
-		outbuf += csbcpb->csb.processed_byte_count;
-		continue;
-
-sw:
-		/* software decompression */
-		size = max_sync_size;
-		ret = sw842_decompress(
-			(unsigned char *)inbuf, hdr->sizes[i],
-			(unsigned char *)outbuf, &size, wmem);
-		if (ret)
-			pr_debug("%s: sw842_decompress failed with %d\n",
-				__func__, ret);
-
-		if (ret) {
-			if (ret != -ENOSPC && ret != -EINVAL &&
-					ret != -EMSGSIZE)
-				ret = -EIO;
-			goto unlock;
-		}
-
-		/* SW decompression success */
-		inbuf += hdr->sizes[i];
-		outbuf += size;
-	}
-
-	*outlen = (unsigned int)(outbuf - (unsigned long)out);
-
-unlock:
-	if (ret)
-		/* decompress fail */
-		nx842_inc_decomp_failed(local_devdata);
-	else {
-		if (!dev)
-			/* software decompress */
-			nx842_inc_swdecomp(local_devdata);
-		nx842_inc_decomp_complete(local_devdata);
-		ibm_nx842_incr_hist(local_devdata->counters->decomp_times,
-			(get_tb() - start_time) / tb_ticks_per_usec);
-	}
-
-	rcu_read_unlock();
-	return ret;
+	return nx842_platform_driver()->compress(in, ilen, out, olen, wmem);
 }
-EXPORT_SYMBOL_GPL(nx842_decompress);
+EXPORT_SYMBOL_GPL(nx842_compress);
 
-/**
- * nx842_OF_set_defaults -- Set default (disabled) values for devdata
- *
- * @devdata - struct nx842_devdata to update
- *
- * Returns:
- *  0 on success
- *  -ENOENT if @devdata ptr is NULL
- */
-static int nx842_OF_set_defaults(struct nx842_devdata *devdata)
+int nx842_decompress(const unsigned char *in, unsigned int ilen,
+		     unsigned char *out, unsigned int *olen, void *wmem)
 {
-	if (devdata) {
-		devdata->max_sync_size = 0;
-		devdata->max_sync_sg = 0;
-		devdata->max_sg_len = 0;
-		devdata->status = UNAVAILABLE;
-		return 0;
-	} else
-		return -ENOENT;
-}
-
-/**
- * nx842_OF_upd_status -- Update the device info from OF status prop
- *
- * The status property indicates if the accelerator is enabled.  If the
- * device is in the OF tree it indicates that the hardware is present.
- * The status field indicates if the device is enabled when the status
- * is 'okay'.  Otherwise the device driver will be disabled.
- *
- * @devdata - struct nx842_devdata to update
- * @prop - struct property point containing the maxsyncop for the update
- *
- * Returns:
- *  0 - Device is available
- *  -EINVAL - Device is not available
- */
-static int nx842_OF_upd_status(struct nx842_devdata *devdata,
-					struct property *prop) {
-	int ret = 0;
-	const char *status = (const char *)prop->value;
-
-	if (!strncmp(status, "okay", (size_t)prop->length)) {
-		devdata->status = AVAILABLE;
-	} else {
-		dev_info(devdata->dev, "%s: status '%s' is not 'okay'\n",
-				__func__, status);
-		devdata->status = UNAVAILABLE;
-	}
-
-	return ret;
-}
-
-/**
- * nx842_OF_upd_maxsglen -- Update the device info from OF maxsglen prop
- *
- * Definition of the 'ibm,max-sg-len' OF property:
- *  This field indicates the maximum byte length of a scatter list
- *  for the platform facility. It is a single cell encoded as with encode-int.
- *
- * Example:
- *  # od -x ibm,max-sg-len
- *  0000000 0000 0ff0
- *
- *  In this example, the maximum byte length of a scatter list is
- *  0x0ff0 (4,080).
- *
- * @devdata - struct nx842_devdata to update
- * @prop - struct property point containing the maxsyncop for the update
- *
- * Returns:
- *  0 on success
- *  -EINVAL on failure
- */
-static int nx842_OF_upd_maxsglen(struct nx842_devdata *devdata,
-					struct property *prop) {
-	int ret = 0;
-	const int *maxsglen = prop->value;
-
-	if (prop->length != sizeof(*maxsglen)) {
-		dev_err(devdata->dev, "%s: unexpected format for ibm,max-sg-len property\n", __func__);
-		dev_dbg(devdata->dev, "%s: ibm,max-sg-len is %d bytes long, expected %lu bytes\n", __func__,
-				prop->length, sizeof(*maxsglen));
-		ret = -EINVAL;
-	} else {
-		devdata->max_sg_len = (unsigned int)min(*maxsglen,
-				(int)NX842_HW_PAGE_SIZE);
-	}
-
-	return ret;
-}
-
-/**
- * nx842_OF_upd_maxsyncop -- Update the device info from OF maxsyncop prop
- *
- * Definition of the 'ibm,max-sync-cop' OF property:
- *  Two series of cells.  The first series of cells represents the maximums
- *  that can be synchronously compressed. The second series of cells
- *  represents the maximums that can be synchronously decompressed.
- *  1. The first cell in each series contains the count of the number of
- *     data length, scatter list elements pairs that follow – each being
- *     of the form
- *    a. One cell data byte length
- *    b. One cell total number of scatter list elements
- *
- * Example:
- *  # od -x ibm,max-sync-cop
- *  0000000 0000 0001 0000 1000 0000 01fe 0000 0001
- *  0000020 0000 1000 0000 01fe
- *
- *  In this example, compression supports 0x1000 (4,096) data byte length
- *  and 0x1fe (510) total scatter list elements.  Decompression supports
- *  0x1000 (4,096) data byte length and 0x1f3 (510) total scatter list
- *  elements.
- *
- * @devdata - struct nx842_devdata to update
- * @prop - struct property point containing the maxsyncop for the update
- *
- * Returns:
- *  0 on success
- *  -EINVAL on failure
- */
-static int nx842_OF_upd_maxsyncop(struct nx842_devdata *devdata,
-					struct property *prop) {
-	int ret = 0;
-	const struct maxsynccop_t {
-		int comp_elements;
-		int comp_data_limit;
-		int comp_sg_limit;
-		int decomp_elements;
-		int decomp_data_limit;
-		int decomp_sg_limit;
-	} *maxsynccop;
-
-	if (prop->length != sizeof(*maxsynccop)) {
-		dev_err(devdata->dev, "%s: unexpected format for ibm,max-sync-cop property\n", __func__);
-		dev_dbg(devdata->dev, "%s: ibm,max-sync-cop is %d bytes long, expected %lu bytes\n", __func__, prop->length,
-				sizeof(*maxsynccop));
-		ret = -EINVAL;
-		goto out;
-	}
-
-	maxsynccop = (const struct maxsynccop_t *)prop->value;
-
-	/* Use one limit rather than separate limits for compression and
-	 * decompression. Set a maximum for this so as not to exceed the
-	 * size that the header can support and round the value down to
-	 * the hardware page size (4K) */
-	devdata->max_sync_size =
-			(unsigned int)min(maxsynccop->comp_data_limit,
-					maxsynccop->decomp_data_limit);
-
-	devdata->max_sync_size = min_t(unsigned int, devdata->max_sync_size,
-					SIZE_64K);
-
-	if (devdata->max_sync_size < SIZE_4K) {
-		dev_err(devdata->dev, "%s: hardware max data size (%u) is "
-				"less than the driver minimum, unable to use "
-				"the hardware device\n",
-				__func__, devdata->max_sync_size);
-		ret = -EINVAL;
-		goto out;
-	}
-
-	devdata->max_sync_sg = (unsigned int)min(maxsynccop->comp_sg_limit,
-						maxsynccop->decomp_sg_limit);
-	if (devdata->max_sync_sg < 1) {
-		dev_err(devdata->dev, "%s: hardware max sg size (%u) is "
-				"less than the driver minimum, unable to use "
-				"the hardware device\n",
-				__func__, devdata->max_sync_sg);
-		ret = -EINVAL;
-		goto out;
-	}
-
-out:
-	return ret;
+	return nx842_platform_driver()->decompress(in, ilen, out, olen, wmem);
 }
+EXPORT_SYMBOL_GPL(nx842_decompress);
 
-/**
- *
- * nx842_OF_upd -- Handle OF properties updates for the device.
- *
- * Set all properties from the OF tree.  Optionally, a new property
- * can be provided by the @new_prop pointer to overwrite an existing value.
- * The device will remain disabled until all values are valid, this function
- * will return an error for updates unless all values are valid.
- *
- * @new_prop: If not NULL, this property is being updated.  If NULL, update
- *  all properties from the current values in the OF tree.
- *
- * Returns:
- *  0 - Success
- *  -ENOMEM - Could not allocate memory for new devdata structure
- *  -EINVAL - property value not found, new_prop is not a recognized
- *	property for the device or property value is not valid.
- *  -ENODEV - Device is not available
- */
-static int nx842_OF_upd(struct property *new_prop)
+static __init int nx842_init(void)
 {
-	struct nx842_devdata *old_devdata = NULL;
-	struct nx842_devdata *new_devdata = NULL;
-	struct device_node *of_node = NULL;
-	struct property *status = NULL;
-	struct property *maxsglen = NULL;
-	struct property *maxsyncop = NULL;
-	int ret = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&devdata_mutex, flags);
-	old_devdata = rcu_dereference_check(devdata,
-			lockdep_is_held(&devdata_mutex));
-	if (old_devdata)
-		of_node = old_devdata->dev->of_node;
+	request_module("nx-compress-powernv");
+	request_module("nx-compress-pseries");
 
-	if (!old_devdata || !of_node) {
-		pr_err("%s: device is not available\n", __func__);
-		spin_unlock_irqrestore(&devdata_mutex, flags);
-		return -ENODEV;
-	}
-
-	new_devdata = kzalloc(sizeof(*new_devdata), GFP_NOFS);
-	if (!new_devdata) {
-		dev_err(old_devdata->dev, "%s: Could not allocate memory for device data\n", __func__);
-		ret = -ENOMEM;
-		goto error_out;
-	}
-
-	memcpy(new_devdata, old_devdata, sizeof(*old_devdata));
-	new_devdata->counters = old_devdata->counters;
-
-	/* Set ptrs for existing properties */
-	status = of_find_property(of_node, "status", NULL);
-	maxsglen = of_find_property(of_node, "ibm,max-sg-len", NULL);
-	maxsyncop = of_find_property(of_node, "ibm,max-sync-cop", NULL);
-	if (!status || !maxsglen || !maxsyncop) {
-		dev_err(old_devdata->dev, "%s: Could not locate device properties\n", __func__);
-		ret = -EINVAL;
-		goto error_out;
-	}
-
-	/*
-	 * If this is a property update, there are only certain properties that
-	 * we care about. Bail if it isn't in the below list
+	/* we prevent loading if there's no platform driver, and we get the
+	 * module that set it so it won't unload, so we don't need to check
+	 * if it's set in any of the above functions
 	 */
-	if (new_prop && (strncmp(new_prop->name, "status", new_prop->length) ||
-		         strncmp(new_prop->name, "ibm,max-sg-len", new_prop->length) ||
-		         strncmp(new_prop->name, "ibm,max-sync-cop", new_prop->length)))
-		goto out;
-
-	/* Perform property updates */
-	ret = nx842_OF_upd_status(new_devdata, status);
-	if (ret)
-		goto error_out;
-
-	ret = nx842_OF_upd_maxsglen(new_devdata, maxsglen);
-	if (ret)
-		goto error_out;
-
-	ret = nx842_OF_upd_maxsyncop(new_devdata, maxsyncop);
-	if (ret)
-		goto error_out;
-
-out:
-	dev_info(old_devdata->dev, "%s: max_sync_size new:%u old:%u\n",
-			__func__, new_devdata->max_sync_size,
-			old_devdata->max_sync_size);
-	dev_info(old_devdata->dev, "%s: max_sync_sg new:%u old:%u\n",
-			__func__, new_devdata->max_sync_sg,
-			old_devdata->max_sync_sg);
-	dev_info(old_devdata->dev, "%s: max_sg_len new:%u old:%u\n",
-			__func__, new_devdata->max_sg_len,
-			old_devdata->max_sg_len);
-
-	rcu_assign_pointer(devdata, new_devdata);
-	spin_unlock_irqrestore(&devdata_mutex, flags);
-	synchronize_rcu();
-	dev_set_drvdata(new_devdata->dev, new_devdata);
-	kfree(old_devdata);
-	return 0;
-
-error_out:
-	if (new_devdata) {
-		dev_info(old_devdata->dev, "%s: device disabled\n", __func__);
-		nx842_OF_set_defaults(new_devdata);
-		rcu_assign_pointer(devdata, new_devdata);
-		spin_unlock_irqrestore(&devdata_mutex, flags);
-		synchronize_rcu();
-		dev_set_drvdata(new_devdata->dev, new_devdata);
-		kfree(old_devdata);
-	} else {
-		dev_err(old_devdata->dev, "%s: could not update driver from hardware\n", __func__);
-		spin_unlock_irqrestore(&devdata_mutex, flags);
-	}
-
-	if (!ret)
-		ret = -EINVAL;
-	return ret;
-}
-
-/**
- * nx842_OF_notifier - Process updates to OF properties for the device
- *
- * @np: notifier block
- * @action: notifier action
- * @update: struct pSeries_reconfig_prop_update pointer if action is
- *	PSERIES_UPDATE_PROPERTY
- *
- * Returns:
- *	NOTIFY_OK on success
- *	NOTIFY_BAD encoded with error number on failure, use
- *		notifier_to_errno() to decode this value
- */
-static int nx842_OF_notifier(struct notifier_block *np, unsigned long action,
-			     void *data)
-{
-	struct of_reconfig_data *upd = data;
-	struct nx842_devdata *local_devdata;
-	struct device_node *node = NULL;
-
-	rcu_read_lock();
-	local_devdata = rcu_dereference(devdata);
-	if (local_devdata)
-		node = local_devdata->dev->of_node;
-
-	if (local_devdata &&
-			action == OF_RECONFIG_UPDATE_PROPERTY &&
-			!strcmp(upd->dn->name, node->name)) {
-		rcu_read_unlock();
-		nx842_OF_upd(upd->prop);
-	} else
-		rcu_read_unlock();
-
-	return NOTIFY_OK;
-}
-
-static struct notifier_block nx842_of_nb = {
-	.notifier_call = nx842_OF_notifier,
-};
-
-#define nx842_counter_read(_name)					\
-static ssize_t nx842_##_name##_show(struct device *dev,		\
-		struct device_attribute *attr,				\
-		char *buf) {						\
-	struct nx842_devdata *local_devdata;			\
-	int p = 0;							\
-	rcu_read_lock();						\
-	local_devdata = rcu_dereference(devdata);			\
-	if (local_devdata)						\
-		p = snprintf(buf, PAGE_SIZE, "%ld\n",			\
-		       atomic64_read(&local_devdata->counters->_name));	\
-	rcu_read_unlock();						\
-	return p;							\
-}
-
-#define NX842DEV_COUNTER_ATTR_RO(_name)					\
-	nx842_counter_read(_name);					\
-	static struct device_attribute dev_attr_##_name = __ATTR(_name,	\
-						0444,			\
-						nx842_##_name##_show,\
-						NULL);
-
-NX842DEV_COUNTER_ATTR_RO(comp_complete);
-NX842DEV_COUNTER_ATTR_RO(comp_failed);
-NX842DEV_COUNTER_ATTR_RO(decomp_complete);
-NX842DEV_COUNTER_ATTR_RO(decomp_failed);
-NX842DEV_COUNTER_ATTR_RO(swdecomp);
-
-static ssize_t nx842_timehist_show(struct device *,
-		struct device_attribute *, char *);
-
-static struct device_attribute dev_attr_comp_times = __ATTR(comp_times, 0444,
-		nx842_timehist_show, NULL);
-static struct device_attribute dev_attr_decomp_times = __ATTR(decomp_times,
-		0444, nx842_timehist_show, NULL);
-
-static ssize_t nx842_timehist_show(struct device *dev,
-		struct device_attribute *attr, char *buf) {
-	char *p = buf;
-	struct nx842_devdata *local_devdata;
-	atomic64_t *times;
-	int bytes_remain = PAGE_SIZE;
-	int bytes;
-	int i;
-
-	rcu_read_lock();
-	local_devdata = rcu_dereference(devdata);
-	if (!local_devdata) {
-		rcu_read_unlock();
-		return 0;
-	}
-
-	if (attr == &dev_attr_comp_times)
-		times = local_devdata->counters->comp_times;
-	else if (attr == &dev_attr_decomp_times)
-		times = local_devdata->counters->decomp_times;
-	else {
-		rcu_read_unlock();
-		return 0;
-	}
-
-	for (i = 0; i < (NX842_HIST_SLOTS - 2); i++) {
-		bytes = snprintf(p, bytes_remain, "%u-%uus:\t%ld\n",
-			       i ? (2<<(i-1)) : 0, (2<<i)-1,
-			       atomic64_read(&times[i]));
-		bytes_remain -= bytes;
-		p += bytes;
-	}
-	/* The last bucket holds everything over
-	 * 2<<(NX842_HIST_SLOTS - 2) us */
-	bytes = snprintf(p, bytes_remain, "%uus - :\t%ld\n",
-			2<<(NX842_HIST_SLOTS - 2),
-			atomic64_read(&times[(NX842_HIST_SLOTS - 1)]));
-	p += bytes;
-
-	rcu_read_unlock();
-	return p - buf;
-}
-
-static struct attribute *nx842_sysfs_entries[] = {
-	&dev_attr_comp_complete.attr,
-	&dev_attr_comp_failed.attr,
-	&dev_attr_decomp_complete.attr,
-	&dev_attr_decomp_failed.attr,
-	&dev_attr_swdecomp.attr,
-	&dev_attr_comp_times.attr,
-	&dev_attr_decomp_times.attr,
-	NULL,
-};
-
-static struct attribute_group nx842_attribute_group = {
-	.name = NULL,		/* put in device directory */
-	.attrs = nx842_sysfs_entries,
-};
-
-static int __init nx842_probe(struct vio_dev *viodev,
-				  const struct vio_device_id *id)
-{
-	struct nx842_devdata *old_devdata, *new_devdata = NULL;
-	unsigned long flags;
-	int ret = 0;
-
-	spin_lock_irqsave(&devdata_mutex, flags);
-	old_devdata = rcu_dereference_check(devdata,
-			lockdep_is_held(&devdata_mutex));
-
-	if (old_devdata && old_devdata->vdev != NULL) {
-		dev_err(&viodev->dev, "%s: Attempt to register more than one instance of the hardware\n", __func__);
-		ret = -1;
-		goto error_unlock;
-	}
-
-	dev_set_drvdata(&viodev->dev, NULL);
-
-	new_devdata = kzalloc(sizeof(*new_devdata), GFP_NOFS);
-	if (!new_devdata) {
-		dev_err(&viodev->dev, "%s: Could not allocate memory for device data\n", __func__);
-		ret = -ENOMEM;
-		goto error_unlock;
-	}
-
-	new_devdata->counters = kzalloc(sizeof(*new_devdata->counters),
-			GFP_NOFS);
-	if (!new_devdata->counters) {
-		dev_err(&viodev->dev, "%s: Could not allocate memory for performance counters\n", __func__);
-		ret = -ENOMEM;
-		goto error_unlock;
-	}
-
-	new_devdata->vdev = viodev;
-	new_devdata->dev = &viodev->dev;
-	nx842_OF_set_defaults(new_devdata);
-
-	rcu_assign_pointer(devdata, new_devdata);
-	spin_unlock_irqrestore(&devdata_mutex, flags);
-	synchronize_rcu();
-	kfree(old_devdata);
-
-	of_reconfig_notifier_register(&nx842_of_nb);
-
-	ret = nx842_OF_upd(NULL);
-	if (ret && ret != -ENODEV) {
-		dev_err(&viodev->dev, "could not parse device tree. %d\n", ret);
-		ret = -1;
-		goto error;
-	}
-
-	rcu_read_lock();
-	dev_set_drvdata(&viodev->dev, rcu_dereference(devdata));
-	rcu_read_unlock();
-
-	if (sysfs_create_group(&viodev->dev.kobj, &nx842_attribute_group)) {
-		dev_err(&viodev->dev, "could not create sysfs device attributes\n");
-		ret = -1;
-		goto error;
+	if (!nx842_platform_driver_get()) {
+		pr_err("no nx842 driver found.\n");
+		return -ENODEV;
 	}
 
 	return 0;
-
-error_unlock:
-	spin_unlock_irqrestore(&devdata_mutex, flags);
-	if (new_devdata)
-		kfree(new_devdata->counters);
-	kfree(new_devdata);
-error:
-	return ret;
-}
-
-static int __exit nx842_remove(struct vio_dev *viodev)
-{
-	struct nx842_devdata *old_devdata;
-	unsigned long flags;
-
-	pr_info("Removing IBM Power 842 compression device\n");
-	sysfs_remove_group(&viodev->dev.kobj, &nx842_attribute_group);
-
-	spin_lock_irqsave(&devdata_mutex, flags);
-	old_devdata = rcu_dereference_check(devdata,
-			lockdep_is_held(&devdata_mutex));
-	of_reconfig_notifier_unregister(&nx842_of_nb);
-	RCU_INIT_POINTER(devdata, NULL);
-	spin_unlock_irqrestore(&devdata_mutex, flags);
-	synchronize_rcu();
-	dev_set_drvdata(&viodev->dev, NULL);
-	if (old_devdata)
-		kfree(old_devdata->counters);
-	kfree(old_devdata);
-	return 0;
-}
-
-static struct vio_device_id nx842_driver_ids[] = {
-	{"ibm,compression-v1", "ibm,compression"},
-	{"", ""},
-};
-
-static struct vio_driver nx842_driver = {
-	.name = MODULE_NAME,
-	.probe = nx842_probe,
-	.remove = __exit_p(nx842_remove),
-	.get_desired_dma = nx842_get_desired_dma,
-	.id_table = nx842_driver_ids,
-};
-
-static int __init nx842_init(void)
-{
-	struct nx842_devdata *new_devdata;
-	pr_info("Registering IBM Power 842 compression driver\n");
-
-	RCU_INIT_POINTER(devdata, NULL);
-	new_devdata = kzalloc(sizeof(*new_devdata), GFP_KERNEL);
-	if (!new_devdata) {
-		pr_err("Could not allocate memory for device data\n");
-		return -ENOMEM;
-	}
-	new_devdata->status = UNAVAILABLE;
-	RCU_INIT_POINTER(devdata, new_devdata);
-
-	return vio_register_driver(&nx842_driver);
 }
-
 module_init(nx842_init);
 
 static void __exit nx842_exit(void)
 {
-	struct nx842_devdata *old_devdata;
-	unsigned long flags;
-
-	pr_info("Exiting IBM Power 842 compression driver\n");
-	spin_lock_irqsave(&devdata_mutex, flags);
-	old_devdata = rcu_dereference_check(devdata,
-			lockdep_is_held(&devdata_mutex));
-	RCU_INIT_POINTER(devdata, NULL);
-	spin_unlock_irqrestore(&devdata_mutex, flags);
-	synchronize_rcu();
-	if (old_devdata)
-		dev_set_drvdata(old_devdata->dev, NULL);
-	kfree(old_devdata);
-	vio_unregister_driver(&nx842_driver);
+	nx842_platform_driver_put();
 }
-
 module_exit(nx842_exit);
-
-/*********************************
- * 842 software decompressor
-*********************************/
-typedef int (*sw842_template_op)(const char **, int *, unsigned char **,
-						struct sw842_fifo *);
-
-static int sw842_data8(const char **, int *, unsigned char **,
-						struct sw842_fifo *);
-static int sw842_data4(const char **, int *, unsigned char **,
-						struct sw842_fifo *);
-static int sw842_data2(const char **, int *, unsigned char **,
-						struct sw842_fifo *);
-static int sw842_ptr8(const char **, int *, unsigned char **,
-						struct sw842_fifo *);
-static int sw842_ptr4(const char **, int *, unsigned char **,
-						struct sw842_fifo *);
-static int sw842_ptr2(const char **, int *, unsigned char **,
-						struct sw842_fifo *);
-
-/* special templates */
-#define SW842_TMPL_REPEAT 0x1B
-#define SW842_TMPL_ZEROS 0x1C
-#define SW842_TMPL_EOF 0x1E
-
-static sw842_template_op sw842_tmpl_ops[26][4] = {
-	{ sw842_data8, NULL}, /* 0 (00000) */
-	{ sw842_data4, sw842_data2, sw842_ptr2,  NULL},
-	{ sw842_data4, sw842_ptr2,  sw842_data2, NULL},
-	{ sw842_data4, sw842_ptr2,  sw842_ptr2,  NULL},
-	{ sw842_data4, sw842_ptr4,  NULL},
-	{ sw842_data2, sw842_ptr2,  sw842_data4, NULL},
-	{ sw842_data2, sw842_ptr2,  sw842_data2, sw842_ptr2},
-	{ sw842_data2, sw842_ptr2,  sw842_ptr2,  sw842_data2},
-	{ sw842_data2, sw842_ptr2,  sw842_ptr2,  sw842_ptr2,},
-	{ sw842_data2, sw842_ptr2,  sw842_ptr4,  NULL},
-	{ sw842_ptr2,  sw842_data2, sw842_data4, NULL}, /* 10 (01010) */
-	{ sw842_ptr2,  sw842_data4, sw842_ptr2,  NULL},
-	{ sw842_ptr2,  sw842_data2, sw842_ptr2,  sw842_data2},
-	{ sw842_ptr2,  sw842_data2, sw842_ptr2,  sw842_ptr2},
-	{ sw842_ptr2,  sw842_data2, sw842_ptr4,  NULL},
-	{ sw842_ptr2,  sw842_ptr2,  sw842_data4, NULL},
-	{ sw842_ptr2,  sw842_ptr2,  sw842_data2, sw842_ptr2},
-	{ sw842_ptr2,  sw842_ptr2,  sw842_ptr2,  sw842_data2},
-	{ sw842_ptr2,  sw842_ptr2,  sw842_ptr2,  sw842_ptr2},
-	{ sw842_ptr2,  sw842_ptr2,  sw842_ptr4,  NULL},
-	{ sw842_ptr4,  sw842_data4, NULL}, /* 20 (10100) */
-	{ sw842_ptr4,  sw842_data2, sw842_ptr2,  NULL},
-	{ sw842_ptr4,  sw842_ptr2,  sw842_data2, NULL},
-	{ sw842_ptr4,  sw842_ptr2,  sw842_ptr2,  NULL},
-	{ sw842_ptr4,  sw842_ptr4,  NULL},
-	{ sw842_ptr8,  NULL}
-};
-
-/* Software decompress helpers */
-
-static uint8_t sw842_get_byte(const char *buf, int bit)
-{
-	uint8_t tmpl;
-	uint16_t tmp;
-	tmp = htons(*(uint16_t *)(buf));
-	tmp = (uint16_t)(tmp << bit);
-	tmp = ntohs(tmp);
-	memcpy(&tmpl, &tmp, 1);
-	return tmpl;
-}
-
-static uint8_t sw842_get_template(const char **buf, int *bit)
-{
-	uint8_t byte;
-	byte = sw842_get_byte(*buf, *bit);
-	byte = byte >> 3;
-	byte &= 0x1F;
-	*buf += (*bit + 5) / 8;
-	*bit = (*bit + 5) % 8;
-	return byte;
-}
-
-/* repeat_count happens to be 5-bit too (like the template) */
-static uint8_t sw842_get_repeat_count(const char **buf, int *bit)
-{
-	uint8_t byte;
-	byte = sw842_get_byte(*buf, *bit);
-	byte = byte >> 2;
-	byte &= 0x3F;
-	*buf += (*bit + 6) / 8;
-	*bit = (*bit + 6) % 8;
-	return byte;
-}
-
-static uint8_t sw842_get_ptr2(const char **buf, int *bit)
-{
-	uint8_t ptr;
-	ptr = sw842_get_byte(*buf, *bit);
-	(*buf)++;
-	return ptr;
-}
-
-static uint16_t sw842_get_ptr4(const char **buf, int *bit,
-		struct sw842_fifo *fifo)
-{
-	uint16_t ptr;
-	ptr = htons(*(uint16_t *)(*buf));
-	ptr = (uint16_t)(ptr << *bit);
-	ptr = ptr >> 7;
-	ptr &= 0x01FF;
-	*buf += (*bit + 9) / 8;
-	*bit = (*bit + 9) % 8;
-	return ptr;
-}
-
-static uint8_t sw842_get_ptr8(const char **buf, int *bit,
-		struct sw842_fifo *fifo)
-{
-	return sw842_get_ptr2(buf, bit);
-}
-
-/* Software decompress template ops */
-
-static int sw842_data8(const char **inbuf, int *inbit,
-		unsigned char **outbuf, struct sw842_fifo *fifo)
-{
-	int ret;
-
-	ret = sw842_data4(inbuf, inbit, outbuf, fifo);
-	if (ret)
-		return ret;
-	ret = sw842_data4(inbuf, inbit, outbuf, fifo);
-	return ret;
-}
-
-static int sw842_data4(const char **inbuf, int *inbit,
-		unsigned char **outbuf, struct sw842_fifo *fifo)
-{
-	int ret;
-
-	ret = sw842_data2(inbuf, inbit, outbuf, fifo);
-	if (ret)
-		return ret;
-	ret = sw842_data2(inbuf, inbit, outbuf, fifo);
-	return ret;
-}
-
-static int sw842_data2(const char **inbuf, int *inbit,
-		unsigned char **outbuf, struct sw842_fifo *fifo)
-{
-	**outbuf = sw842_get_byte(*inbuf, *inbit);
-	(*inbuf)++;
-	(*outbuf)++;
-	**outbuf = sw842_get_byte(*inbuf, *inbit);
-	(*inbuf)++;
-	(*outbuf)++;
-	return 0;
-}
-
-static int sw842_ptr8(const char **inbuf, int *inbit,
-		unsigned char **outbuf, struct sw842_fifo *fifo)
-{
-	uint8_t ptr;
-	ptr = sw842_get_ptr8(inbuf, inbit, fifo);
-	if (!fifo->f84_full && (ptr >= fifo->f8_count))
-		return 1;
-	memcpy(*outbuf, fifo->f8[ptr], 8);
-	*outbuf += 8;
-	return 0;
-}
-
-static int sw842_ptr4(const char **inbuf, int *inbit,
-		unsigned char **outbuf, struct sw842_fifo *fifo)
-{
-	uint16_t ptr;
-	ptr = sw842_get_ptr4(inbuf, inbit, fifo);
-	if (!fifo->f84_full && (ptr >= fifo->f4_count))
-		return 1;
-	memcpy(*outbuf, fifo->f4[ptr], 4);
-	*outbuf += 4;
-	return 0;
-}
-
-static int sw842_ptr2(const char **inbuf, int *inbit,
-		unsigned char **outbuf, struct sw842_fifo *fifo)
-{
-	uint8_t ptr;
-	ptr = sw842_get_ptr2(inbuf, inbit);
-	if (!fifo->f2_full && (ptr >= fifo->f2_count))
-		return 1;
-	memcpy(*outbuf, fifo->f2[ptr], 2);
-	*outbuf += 2;
-	return 0;
-}
-
-static void sw842_copy_to_fifo(const char *buf, struct sw842_fifo *fifo)
-{
-	unsigned char initial_f2count = fifo->f2_count;
-
-	memcpy(fifo->f8[fifo->f8_count], buf, 8);
-	fifo->f4_count += 2;
-	fifo->f8_count += 1;
-
-	if (!fifo->f84_full && fifo->f4_count >= 512) {
-		fifo->f84_full = 1;
-		fifo->f4_count /= 512;
-	}
-
-	memcpy(fifo->f2[fifo->f2_count++], buf, 2);
-	memcpy(fifo->f2[fifo->f2_count++], buf + 2, 2);
-	memcpy(fifo->f2[fifo->f2_count++], buf + 4, 2);
-	memcpy(fifo->f2[fifo->f2_count++], buf + 6, 2);
-	if (fifo->f2_count < initial_f2count)
-		fifo->f2_full = 1;
-}
-
-static int sw842_decompress(const unsigned char *src, int srclen,
-			unsigned char *dst, int *destlen,
-			const void *wrkmem)
-{
-	uint8_t tmpl;
-	const char *inbuf;
-	int inbit = 0;
-	unsigned char *outbuf, *outbuf_end, *origbuf, *prevbuf;
-	const char *inbuf_end;
-	sw842_template_op op;
-	int opindex;
-	int i, repeat_count;
-	struct sw842_fifo *fifo;
-	int ret = 0;
-
-	fifo = &((struct nx842_workmem *)(wrkmem))->swfifo;
-	memset(fifo, 0, sizeof(*fifo));
-
-	origbuf = NULL;
-	inbuf = src;
-	inbuf_end = src + srclen;
-	outbuf = dst;
-	outbuf_end = dst + *destlen;
-
-	while ((tmpl = sw842_get_template(&inbuf, &inbit)) != SW842_TMPL_EOF) {
-		if (inbuf >= inbuf_end) {
-			ret = -EINVAL;
-			goto out;
-		}
-
-		opindex = 0;
-		prevbuf = origbuf;
-		origbuf = outbuf;
-		switch (tmpl) {
-		case SW842_TMPL_REPEAT:
-			if (prevbuf == NULL) {
-				ret = -EINVAL;
-				goto out;
-			}
-
-			repeat_count = sw842_get_repeat_count(&inbuf,
-								&inbit) + 1;
-
-			/* Did the repeat count advance past the end of input */
-			if (inbuf > inbuf_end) {
-				ret = -EINVAL;
-				goto out;
-			}
-
-			for (i = 0; i < repeat_count; i++) {
-				/* Would this overflow the output buffer */
-				if ((outbuf + 8) > outbuf_end) {
-					ret = -ENOSPC;
-					goto out;
-				}
-
-				memcpy(outbuf, prevbuf, 8);
-				sw842_copy_to_fifo(outbuf, fifo);
-				outbuf += 8;
-			}
-			break;
-
-		case SW842_TMPL_ZEROS:
-			/* Would this overflow the output buffer */
-			if ((outbuf + 8) > outbuf_end) {
-				ret = -ENOSPC;
-				goto out;
-			}
-
-			memset(outbuf, 0, 8);
-			sw842_copy_to_fifo(outbuf, fifo);
-			outbuf += 8;
-			break;
-
-		default:
-			if (tmpl > 25) {
-				ret = -EINVAL;
-				goto out;
-			}
-
-			/* Does this go past the end of the input buffer */
-			if ((inbuf + 2) > inbuf_end) {
-				ret = -EINVAL;
-				goto out;
-			}
-
-			/* Would this overflow the output buffer */
-			if ((outbuf + 8) > outbuf_end) {
-				ret = -ENOSPC;
-				goto out;
-			}
-
-			while (opindex < 4 &&
-				(op = sw842_tmpl_ops[tmpl][opindex++])
-					!= NULL) {
-				ret = (*op)(&inbuf, &inbit, &outbuf, fifo);
-				if (ret) {
-					ret = -EINVAL;
-					goto out;
-				}
-				sw842_copy_to_fifo(origbuf, fifo);
-			}
-		}
-	}
-
-out:
-	if (!ret)
-		*destlen = (unsigned int)(outbuf - dst);
-	else
-		*destlen = 0;
-
-	return ret;
-}
diff --git a/drivers/crypto/nx/nx-842.h b/drivers/crypto/nx/nx-842.h
new file mode 100644
index 000000000000..ac0ea79d0f8b
--- /dev/null
+++ b/drivers/crypto/nx/nx-842.h
@@ -0,0 +1,144 @@
+
+#ifndef __NX_842_H__
+#define __NX_842_H__
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sw842.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/ratelimit.h>
+
+/* Restrictions on Data Descriptor List (DDL) and Entry (DDE) buffers
+ *
+ * From NX P8 workbook, sec 4.9.1 "842 details"
+ *   Each DDE buffer is 128 byte aligned
+ *   Each DDE buffer size is a multiple of 32 bytes (except the last)
+ *   The last DDE buffer size is a multiple of 8 bytes
+ */
+#define DDE_BUFFER_ALIGN	(128)
+#define DDE_BUFFER_SIZE_MULT	(32)
+#define DDE_BUFFER_LAST_MULT	(8)
+
+/* Arbitrary DDL length limit
+ * Allows max buffer size of MAX-1 to MAX pages
+ * (depending on alignment)
+ */
+#define DDL_LEN_MAX		(17)
+
+/* CCW 842 CI/FC masks
+ * NX P8 workbook, section 4.3.1, figure 4-6
+ * "CI/FC Boundary by NX CT type"
+ */
+#define CCW_CI_842		(0x00003ff8)
+#define CCW_FC_842		(0x00000007)
+
+/* CCW Function Codes (FC) for 842
+ * NX P8 workbook, section 4.9, table 4-28
+ * "Function Code Definitions for 842 Memory Compression"
+ */
+#define CCW_FC_842_COMP_NOCRC	(0)
+#define CCW_FC_842_COMP_CRC	(1)
+#define CCW_FC_842_DECOMP_NOCRC	(2)
+#define CCW_FC_842_DECOMP_CRC	(3)
+#define CCW_FC_842_MOVE		(4)
+
+/* CSB CC Error Types for 842
+ * NX P8 workbook, section 4.10.3, table 4-30
+ * "Reported Error Types Summary Table"
+ */
+/* These are all duplicates of existing codes defined in icswx.h. */
+#define CSB_CC_TRANSLATION_DUP1	(80)
+#define CSB_CC_TRANSLATION_DUP2	(82)
+#define CSB_CC_TRANSLATION_DUP3	(84)
+#define CSB_CC_TRANSLATION_DUP4	(86)
+#define CSB_CC_TRANSLATION_DUP5	(92)
+#define CSB_CC_TRANSLATION_DUP6	(94)
+#define CSB_CC_PROTECTION_DUP1	(81)
+#define CSB_CC_PROTECTION_DUP2	(83)
+#define CSB_CC_PROTECTION_DUP3	(85)
+#define CSB_CC_PROTECTION_DUP4	(87)
+#define CSB_CC_PROTECTION_DUP5	(93)
+#define CSB_CC_PROTECTION_DUP6	(95)
+#define CSB_CC_RD_EXTERNAL_DUP1	(89)
+#define CSB_CC_RD_EXTERNAL_DUP2	(90)
+#define CSB_CC_RD_EXTERNAL_DUP3	(91)
+/* These are specific to NX */
+/* 842 codes */
+#define CSB_CC_TPBC_GT_SPBC	(64) /* no error, but >1 comp ratio */
+#define CSB_CC_CRC_MISMATCH	(65) /* decomp crc mismatch */
+#define CSB_CC_TEMPL_INVALID	(66) /* decomp invalid template value */
+#define CSB_CC_TEMPL_OVERFLOW	(67) /* decomp template shows data after end */
+/* sym crypt codes */
+#define CSB_CC_DECRYPT_OVERFLOW	(64)
+/* asym crypt codes */
+#define CSB_CC_MINV_OVERFLOW	(128)
+/* These are reserved for hypervisor use */
+#define CSB_CC_HYP_RESERVE_START	(240)
+#define CSB_CC_HYP_RESERVE_END		(253)
+#define CSB_CC_HYP_NO_HW		(254)
+#define CSB_CC_HYP_HANG_ABORTED		(255)
+
+/* CCB Completion Modes (CM) for 842
+ * NX P8 workbook, section 4.3, figure 4-5
+ * "CRB Details - Normal Cop_Req (CL=00, C=1)"
+ */
+#define CCB_CM_EXTRA_WRITE	(CCB_CM0_ALL_COMPLETIONS & CCB_CM12_STORE)
+#define CCB_CM_INTERRUPT	(CCB_CM0_ALL_COMPLETIONS & CCB_CM12_INTERRUPT)
+
+#define LEN_ON_SIZE(pa, size)	((size) - ((pa) & ((size) - 1)))
+#define LEN_ON_PAGE(pa)		LEN_ON_SIZE(pa, PAGE_SIZE)
+
+static inline unsigned long nx842_get_pa(void *addr)
+{
+	if (!is_vmalloc_addr(addr))
+		return __pa(addr);
+
+	return page_to_phys(vmalloc_to_page(addr)) + offset_in_page(addr);
+}
+
+/* Get/Set bit fields */
+#define MASK_LSH(m)		(__builtin_ffsl(m) - 1)
+#define GET_FIELD(v, m)		(((v) & (m)) >> MASK_LSH(m))
+#define SET_FIELD(v, m, val)	(((v) & ~(m)) | (((val) << MASK_LSH(m)) & (m)))
+
+struct nx842_constraints {
+	int alignment;
+	int multiple;
+	int minimum;
+	int maximum;
+};
+
+struct nx842_driver {
+	char *name;
+	struct module *owner;
+	size_t workmem_size;
+
+	struct nx842_constraints *constraints;
+
+	int (*compress)(const unsigned char *in, unsigned int in_len,
+			unsigned char *out, unsigned int *out_len,
+			void *wrkmem);
+	int (*decompress)(const unsigned char *in, unsigned int in_len,
+			  unsigned char *out, unsigned int *out_len,
+			  void *wrkmem);
+};
+
+struct nx842_driver *nx842_platform_driver(void);
+bool nx842_platform_driver_set(struct nx842_driver *driver);
+void nx842_platform_driver_unset(struct nx842_driver *driver);
+bool nx842_platform_driver_get(void);
+void nx842_platform_driver_put(void);
+
+size_t nx842_workmem_size(void);
+
+int nx842_constraints(struct nx842_constraints *constraints);
+
+int nx842_compress(const unsigned char *in, unsigned int in_len,
+		   unsigned char *out, unsigned int *out_len, void *wrkmem);
+int nx842_decompress(const unsigned char *in, unsigned int in_len,
+		     unsigned char *out, unsigned int *out_len, void *wrkmem);
+
+#endif /* __NX_842_H__ */
diff --git a/drivers/crypto/nx/nx-aes-gcm.c b/drivers/crypto/nx/nx-aes-gcm.c
index 88c562434bc0..08ac6d48688c 100644
--- a/drivers/crypto/nx/nx-aes-gcm.c
+++ b/drivers/crypto/nx/nx-aes-gcm.c
@@ -93,17 +93,6 @@ out:
 	return rc;
 }
 
-static int gcm_aes_nx_setauthsize(struct crypto_aead *tfm,
-				  unsigned int authsize)
-{
-	if (authsize > crypto_aead_alg(tfm)->maxauthsize)
-		return -EINVAL;
-
-	crypto_aead_crt(tfm)->authsize = authsize;
-
-	return 0;
-}
-
 static int gcm4106_aes_nx_setauthsize(struct crypto_aead *tfm,
 				      unsigned int authsize)
 {
@@ -116,8 +105,6 @@ static int gcm4106_aes_nx_setauthsize(struct crypto_aead *tfm,
 		return -EINVAL;
 	}
 
-	crypto_aead_crt(tfm)->authsize = authsize;
-
 	return 0;
 }
 
@@ -134,7 +121,7 @@ static int nx_gca(struct nx_crypto_ctx  *nx_ctx,
 	unsigned int max_sg_len;
 
 	if (nbytes <= AES_BLOCK_SIZE) {
-		scatterwalk_start(&walk, req->assoc);
+		scatterwalk_start(&walk, req->src);
 		scatterwalk_copychunks(out, &walk, nbytes, SCATTERWALK_FROM_SG);
 		scatterwalk_done(&walk, SCATTERWALK_FROM_SG, 0);
 		return 0;
@@ -159,7 +146,7 @@ static int nx_gca(struct nx_crypto_ctx  *nx_ctx,
 				   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);
+					  req->src, processed, &to_process);
 
 		if ((to_process + processed) < nbytes)
 			NX_CPB_FDM(csbcpb_aead) |= NX_FDM_INTERMEDIATE;
@@ -225,7 +212,7 @@ static int gmac(struct aead_request *req, struct blkcipher_desc *desc)
 				   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);
+					  req->src, processed, &to_process);
 
 		if ((to_process + processed) < nbytes)
 			NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
@@ -377,7 +364,8 @@ static int gcm_aes_nx_crypt(struct aead_request *req, int enc)
 		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 + req->assoclen,
 				       csbcpb->cpb.aes_gcm.iv_or_cnt);
 
 		if (rc)
@@ -412,17 +400,19 @@ static int gcm_aes_nx_crypt(struct aead_request *req, int enc)
 mac:
 	if (enc) {
 		/* copy out the auth tag */
-		scatterwalk_map_and_copy(csbcpb->cpb.aes_gcm.out_pat_or_mac,
-				 req->dst, nbytes,
-				 crypto_aead_authsize(crypto_aead_reqtfm(req)),
-				 SCATTERWALK_TO_SG);
+		scatterwalk_map_and_copy(
+			csbcpb->cpb.aes_gcm.out_pat_or_mac,
+			req->dst, req->assoclen + nbytes,
+			crypto_aead_authsize(crypto_aead_reqtfm(req)),
+			SCATTERWALK_TO_SG);
 	} else {
 		u8 *itag = nx_ctx->priv.gcm.iauth_tag;
 		u8 *otag = csbcpb->cpb.aes_gcm.out_pat_or_mac;
 
-		scatterwalk_map_and_copy(itag, req->src, nbytes,
-				 crypto_aead_authsize(crypto_aead_reqtfm(req)),
-				 SCATTERWALK_FROM_SG);
+		scatterwalk_map_and_copy(
+			itag, req->src, req->assoclen + nbytes,
+			crypto_aead_authsize(crypto_aead_reqtfm(req)),
+			SCATTERWALK_FROM_SG);
 		rc = memcmp(itag, otag,
 			    crypto_aead_authsize(crypto_aead_reqtfm(req))) ?
 		     -EBADMSG : 0;
@@ -481,45 +471,39 @@ static int gcm4106_aes_nx_decrypt(struct aead_request *req)
  * during encrypt/decrypt doesn't solve this problem, because it calls
  * blkcipher_walk_done under the covers, which doesn't use walk->blocksize,
  * but instead uses this tfm->blocksize. */
-struct crypto_alg nx_gcm_aes_alg = {
-	.cra_name        = "gcm(aes)",
-	.cra_driver_name = "gcm-aes-nx",
-	.cra_priority    = 300,
-	.cra_flags       = CRYPTO_ALG_TYPE_AEAD,
-	.cra_blocksize   = 1,
-	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
-	.cra_type        = &crypto_aead_type,
-	.cra_module      = THIS_MODULE,
-	.cra_init        = nx_crypto_ctx_aes_gcm_init,
-	.cra_exit        = nx_crypto_ctx_exit,
-	.cra_aead = {
-		.ivsize      = AES_BLOCK_SIZE,
-		.maxauthsize = AES_BLOCK_SIZE,
-		.setkey      = gcm_aes_nx_set_key,
-		.setauthsize = gcm_aes_nx_setauthsize,
-		.encrypt     = gcm_aes_nx_encrypt,
-		.decrypt     = gcm_aes_nx_decrypt,
-	}
+struct aead_alg nx_gcm_aes_alg = {
+	.base = {
+		.cra_name        = "gcm(aes)",
+		.cra_driver_name = "gcm-aes-nx",
+		.cra_priority    = 300,
+		.cra_blocksize   = 1,
+		.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+		.cra_module      = THIS_MODULE,
+	},
+	.init        = nx_crypto_ctx_aes_gcm_init,
+	.exit        = nx_crypto_ctx_aead_exit,
+	.ivsize      = 12,
+	.maxauthsize = AES_BLOCK_SIZE,
+	.setkey      = gcm_aes_nx_set_key,
+	.encrypt     = gcm_aes_nx_encrypt,
+	.decrypt     = gcm_aes_nx_decrypt,
 };
 
-struct crypto_alg nx_gcm4106_aes_alg = {
-	.cra_name        = "rfc4106(gcm(aes))",
-	.cra_driver_name = "rfc4106-gcm-aes-nx",
-	.cra_priority    = 300,
-	.cra_flags       = CRYPTO_ALG_TYPE_AEAD,
-	.cra_blocksize   = 1,
-	.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
-	.cra_type        = &crypto_nivaead_type,
-	.cra_module      = THIS_MODULE,
-	.cra_init        = nx_crypto_ctx_aes_gcm_init,
-	.cra_exit        = nx_crypto_ctx_exit,
-	.cra_aead = {
-		.ivsize      = 8,
-		.maxauthsize = AES_BLOCK_SIZE,
-		.geniv       = "seqiv",
-		.setkey      = gcm4106_aes_nx_set_key,
-		.setauthsize = gcm4106_aes_nx_setauthsize,
-		.encrypt     = gcm4106_aes_nx_encrypt,
-		.decrypt     = gcm4106_aes_nx_decrypt,
-	}
+struct aead_alg nx_gcm4106_aes_alg = {
+	.base = {
+		.cra_name        = "rfc4106(gcm(aes))",
+		.cra_driver_name = "rfc4106-gcm-aes-nx",
+		.cra_priority    = 300,
+		.cra_blocksize   = 1,
+		.cra_ctxsize     = sizeof(struct nx_crypto_ctx),
+		.cra_module      = THIS_MODULE,
+	},
+	.init        = nx_crypto_ctx_aes_gcm_init,
+	.exit        = nx_crypto_ctx_aead_exit,
+	.ivsize      = 8,
+	.maxauthsize = AES_BLOCK_SIZE,
+	.setkey      = gcm4106_aes_nx_set_key,
+	.setauthsize = gcm4106_aes_nx_setauthsize,
+	.encrypt     = gcm4106_aes_nx_encrypt,
+	.decrypt     = gcm4106_aes_nx_decrypt,
 };
diff --git a/drivers/crypto/nx/nx-sha256.c b/drivers/crypto/nx/nx-sha256.c
index 23621da624c3..4e91bdb83c59 100644
--- a/drivers/crypto/nx/nx-sha256.c
+++ b/drivers/crypto/nx/nx-sha256.c
@@ -33,8 +33,9 @@ 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;
+	u32 max_sg_len;
 
 	nx_ctx_init(nx_ctx, HCOP_FC_SHA);
 
@@ -44,15 +45,18 @@ static int nx_sha256_init(struct shash_desc *desc)
 
 	NX_CPB_SET_DIGEST_SIZE(nx_ctx->csbcpb, NX_DS_SHA256);
 
+	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);
+
 	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);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *)sctx->state,
+				  &len, max_sg_len);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
 
-	if (rc)
-		goto out;
+	if (len != SHA256_DIGEST_SIZE)
+		return -EINVAL;
 
 	sctx->state[0] = __cpu_to_be32(SHA256_H0);
 	sctx->state[1] = __cpu_to_be32(SHA256_H1);
@@ -64,7 +68,6 @@ static int nx_sha256_init(struct shash_desc *desc)
 	sctx->state[7] = __cpu_to_be32(SHA256_H7);
 	sctx->count = 0;
 
-out:
 	return 0;
 }
 
@@ -74,10 +77,12 @@ 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 = 0, leftover, total;
 	unsigned long irq_flags;
 	int rc = 0;
 	int data_len;
+	u32 max_sg_len;
 	u64 buf_len = (sctx->count % SHA256_BLOCK_SIZE);
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
@@ -97,6 +102,12 @@ static int nx_sha256_update(struct shash_desc *desc, const u8 *data,
 	NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
 	NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
 
+	in_sg = nx_ctx->in_sg;
+	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: the SHA256_BLOCK_SIZE data chunk to process in
@@ -108,25 +119,22 @@ static int nx_sha256_update(struct shash_desc *desc, const u8 *data,
 
 		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);
+			in_sg = nx_build_sg_list(nx_ctx->in_sg,
+						 (u8 *) sctx->buf,
+						 &data_len,
+						 max_sg_len);
 
-			if (rc || data_len != buf_len)
+			if (data_len != buf_len) {
+				rc = -EINVAL;
 				goto out;
+			}
 		}
 
 		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);
+		in_sg = nx_build_sg_list(in_sg, (u8 *) data,
+					 &data_len, max_sg_len);
 
-		if (rc)
-			goto out;
+		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) * sizeof(struct nx_sg);
 
 		to_process = (data_len + buf_len);
 		leftover = total - to_process;
@@ -173,12 +181,19 @@ 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;
 	unsigned long irq_flags;
-	int rc;
+	u32 max_sg_len;
+	int rc = 0;
 	int len;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
+	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);
+
 	/* final is represented by continuing the operation and indicating that
 	 * this is not an intermediate operation */
 	if (sctx->count >= SHA256_BLOCK_SIZE) {
@@ -195,25 +210,24 @@ static int nx_sha256_final(struct shash_desc *desc, u8 *out)
 	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,
+				 &len, max_sg_len);
 
-	if (rc || len != (sctx->count & (SHA256_BLOCK_SIZE - 1)))
+	if (len != (sctx->count & (SHA256_BLOCK_SIZE - 1))) {
+		rc = -EINVAL;
 		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);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, &len, max_sg_len);
 
-	if (rc || len != SHA256_DIGEST_SIZE)
+	if (len != SHA256_DIGEST_SIZE) {
+		rc = -EINVAL;
 		goto out;
+	}
 
+	nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) * sizeof(struct nx_sg);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
 	if (!nx_ctx->op.outlen) {
 		rc = -EINVAL;
 		goto out;
diff --git a/drivers/crypto/nx/nx-sha512.c b/drivers/crypto/nx/nx-sha512.c
index b3adf1022673..e6a58d2ee628 100644
--- a/drivers/crypto/nx/nx-sha512.c
+++ b/drivers/crypto/nx/nx-sha512.c
@@ -32,8 +32,9 @@ 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;
+	u32 max_sg_len;
 
 	nx_ctx_init(nx_ctx, HCOP_FC_SHA);
 
@@ -43,15 +44,18 @@ static int nx_sha512_init(struct shash_desc *desc)
 
 	NX_CPB_SET_DIGEST_SIZE(nx_ctx->csbcpb, NX_DS_SHA512);
 
+	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);
+
 	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);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, (u8 *)sctx->state,
+				  &len, max_sg_len);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
 
-	if (rc || len != SHA512_DIGEST_SIZE)
-		goto out;
+	if (len != SHA512_DIGEST_SIZE)
+		return -EINVAL;
 
 	sctx->state[0] = __cpu_to_be64(SHA512_H0);
 	sctx->state[1] = __cpu_to_be64(SHA512_H1);
@@ -63,7 +67,6 @@ static int nx_sha512_init(struct shash_desc *desc)
 	sctx->state[7] = __cpu_to_be64(SHA512_H7);
 	sctx->count[0] = 0;
 
-out:
 	return 0;
 }
 
@@ -73,10 +76,12 @@ 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 = 0, total;
 	unsigned long irq_flags;
 	int rc = 0;
 	int data_len;
+	u32 max_sg_len;
 	u64 buf_len = (sctx->count[0] % SHA512_BLOCK_SIZE);
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
@@ -96,6 +101,12 @@ static int nx_sha512_update(struct shash_desc *desc, const u8 *data,
 	NX_CPB_FDM(csbcpb) |= NX_FDM_INTERMEDIATE;
 	NX_CPB_FDM(csbcpb) |= NX_FDM_CONTINUATION;
 
+	in_sg = nx_ctx->in_sg;
+	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: the SHA512_BLOCK_SIZE data chunk to process in
@@ -108,25 +119,26 @@ static int nx_sha512_update(struct shash_desc *desc, const u8 *data,
 
 		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);
+			in_sg = nx_build_sg_list(nx_ctx->in_sg,
+						 (u8 *) sctx->buf,
+						 &data_len, max_sg_len);
 
-			if (rc || data_len != buf_len)
+			if (data_len != buf_len) {
+				rc = -EINVAL;
 				goto out;
+			}
 		}
 
 		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);
+		in_sg = nx_build_sg_list(in_sg, (u8 *) data,
+					 &data_len, max_sg_len);
 
-		if (rc || data_len != (to_process - buf_len))
+		nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) * sizeof(struct nx_sg);
+
+		if (data_len != (to_process - buf_len)) {
+			rc = -EINVAL;
 			goto out;
+		}
 
 		to_process = (data_len + buf_len);
 		leftover = total - to_process;
@@ -172,13 +184,20 @@ 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 rc = 0;
 	int len;
 
 	spin_lock_irqsave(&nx_ctx->lock, irq_flags);
 
+	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);
+
 	/* final is represented by continuing the operation and indicating that
 	 * this is not an intermediate operation */
 	if (sctx->count[0] >= SHA512_BLOCK_SIZE) {
@@ -200,24 +219,20 @@ static int nx_sha512_final(struct shash_desc *desc, u8 *out)
 	csbcpb->cpb.sha512.message_bit_length_lo = count0;
 
 	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);
+	in_sg = nx_build_sg_list(nx_ctx->in_sg, sctx->buf, &len,
+				 max_sg_len);
 
-	if (rc || len != (sctx->count[0] & (SHA512_BLOCK_SIZE - 1)))
+	if (len != (sctx->count[0] & (SHA512_BLOCK_SIZE - 1))) {
+		rc = -EINVAL;
 		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);
+	out_sg = nx_build_sg_list(nx_ctx->out_sg, out, &len,
+				 max_sg_len);
 
-	if (rc)
-		goto out;
+	nx_ctx->op.inlen = (nx_ctx->in_sg - in_sg) * sizeof(struct nx_sg);
+	nx_ctx->op.outlen = (nx_ctx->out_sg - out_sg) * sizeof(struct nx_sg);
 
 	if (!nx_ctx->op.outlen) {
 		rc = -EINVAL;
diff --git a/drivers/crypto/nx/nx.c b/drivers/crypto/nx/nx.c
index 1da6dc59d0dd..f6198f29a4a8 100644
--- a/drivers/crypto/nx/nx.c
+++ b/drivers/crypto/nx/nx.c
@@ -19,8 +19,8 @@
  * Author: Kent Yoder <yoder1@us.ibm.com>
  */
 
+#include <crypto/internal/aead.h>
 #include <crypto/internal/hash.h>
-#include <crypto/hash.h>
 #include <crypto/aes.h>
 #include <crypto/sha.h>
 #include <crypto/algapi.h>
@@ -29,10 +29,10 @@
 #include <linux/moduleparam.h>
 #include <linux/types.h>
 #include <linux/mm.h>
-#include <linux/crypto.h>
 #include <linux/scatterlist.h>
 #include <linux/device.h>
 #include <linux/of.h>
+#include <linux/types.h>
 #include <asm/hvcall.h>
 #include <asm/vio.h>
 
@@ -215,8 +215,15 @@ struct nx_sg *nx_walk_and_build(struct nx_sg       *nx_dst,
  * @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)
+static long int trim_sg_list(struct nx_sg *sg,
+			     struct nx_sg *end,
+			     unsigned int delta,
+			     unsigned int *nbytes)
 {
+	long int oplen;
+	long int data_back;
+	unsigned int is_delta = delta;
+
 	while (delta && end > sg) {
 		struct nx_sg *last = end - 1;
 
@@ -228,54 +235,20 @@ static long int trim_sg_list(struct nx_sg *sg, struct nx_sg *end, unsigned int d
 			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;
+	/* There are cases where we need to crop list in order to make it
+	 * a block size multiple, but we also need to align data. In order to
+	 * that we need to calculate how much we need to put back to be
+	 * processed
+	 */
+	oplen = (sg - end) * sizeof(struct nx_sg);
+	if (is_delta) {
+		data_back = (abs(oplen) / AES_BLOCK_SIZE) *  sg->len;
+		data_back = *nbytes - (data_back & ~(AES_BLOCK_SIZE - 1));
+		*nbytes -= data_back;
 	}
-	*op_len = trim_sg_list(nx_in_outsg, nx_insg, delta);
 
-	return 0;
+	return oplen;
 }
 
 /**
@@ -330,8 +303,8 @@ int nx_build_sg_lists(struct nx_crypto_ctx  *nx_ctx,
 	/* 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 = trim_sg_list(nx_ctx->in_sg, nx_insg, delta);
-	nx_ctx->op.outlen = trim_sg_list(nx_ctx->out_sg, nx_outsg, delta);
+	nx_ctx->op.inlen = trim_sg_list(nx_ctx->in_sg, nx_insg, delta, nbytes);
+	nx_ctx->op.outlen = trim_sg_list(nx_ctx->out_sg, nx_outsg, delta, nbytes);
 
 	return 0;
 }
@@ -426,6 +399,13 @@ static void nx_of_update_msc(struct device   *dev,
 				goto next_loop;
 			}
 
+			if (!trip->sglen || trip->databytelen < NX_PAGE_SIZE) {
+				dev_warn(dev, "bogus sglen/databytelen: "
+					 "%u/%u (ignored)\n", trip->sglen,
+					 trip->databytelen);
+				goto next_loop;
+			}
+
 			switch (trip->keybitlen) {
 			case 128:
 			case 160:
@@ -518,6 +498,72 @@ static void nx_of_init(struct device *dev, struct nx_of *props)
 		nx_of_update_msc(dev, p, props);
 }
 
+static bool nx_check_prop(struct device *dev, u32 fc, u32 mode, int slot)
+{
+	struct alg_props *props = &nx_driver.of.ap[fc][mode][slot];
+
+	if (!props->sglen || props->databytelen < NX_PAGE_SIZE) {
+		if (dev)
+			dev_warn(dev, "bogus sglen/databytelen for %u/%u/%u: "
+				 "%u/%u (ignored)\n", fc, mode, slot,
+				 props->sglen, props->databytelen);
+		return false;
+	}
+
+	return true;
+}
+
+static bool nx_check_props(struct device *dev, u32 fc, u32 mode)
+{
+	int i;
+
+	for (i = 0; i < 3; i++)
+		if (!nx_check_prop(dev, fc, mode, i))
+			return false;
+
+	return true;
+}
+
+static int nx_register_alg(struct crypto_alg *alg, u32 fc, u32 mode)
+{
+	return nx_check_props(&nx_driver.viodev->dev, fc, mode) ?
+	       crypto_register_alg(alg) : 0;
+}
+
+static int nx_register_aead(struct aead_alg *alg, u32 fc, u32 mode)
+{
+	return nx_check_props(&nx_driver.viodev->dev, fc, mode) ?
+	       crypto_register_aead(alg) : 0;
+}
+
+static int nx_register_shash(struct shash_alg *alg, u32 fc, u32 mode, int slot)
+{
+	return (slot >= 0 ? nx_check_prop(&nx_driver.viodev->dev,
+					  fc, mode, slot) :
+			    nx_check_props(&nx_driver.viodev->dev, fc, mode)) ?
+	       crypto_register_shash(alg) : 0;
+}
+
+static void nx_unregister_alg(struct crypto_alg *alg, u32 fc, u32 mode)
+{
+	if (nx_check_props(NULL, fc, mode))
+		crypto_unregister_alg(alg);
+}
+
+static void nx_unregister_aead(struct aead_alg *alg, u32 fc, u32 mode)
+{
+	if (nx_check_props(NULL, fc, mode))
+		crypto_unregister_aead(alg);
+}
+
+static void nx_unregister_shash(struct shash_alg *alg, u32 fc, u32 mode,
+				int slot)
+{
+	if (slot >= 0 ? nx_check_prop(NULL, fc, mode, slot) :
+			nx_check_props(NULL, fc, mode))
+		crypto_unregister_shash(alg);
+}
+
 /**
  * nx_register_algs - register algorithms with the crypto API
  *
@@ -542,72 +588,77 @@ static int nx_register_algs(void)
 
 	nx_driver.of.status = NX_OKAY;
 
-	rc = crypto_register_alg(&nx_ecb_aes_alg);
+	rc = nx_register_alg(&nx_ecb_aes_alg, NX_FC_AES, NX_MODE_AES_ECB);
 	if (rc)
 		goto out;
 
-	rc = crypto_register_alg(&nx_cbc_aes_alg);
+	rc = nx_register_alg(&nx_cbc_aes_alg, NX_FC_AES, NX_MODE_AES_CBC);
 	if (rc)
 		goto out_unreg_ecb;
 
-	rc = crypto_register_alg(&nx_ctr_aes_alg);
+	rc = nx_register_alg(&nx_ctr_aes_alg, NX_FC_AES, NX_MODE_AES_CTR);
 	if (rc)
 		goto out_unreg_cbc;
 
-	rc = crypto_register_alg(&nx_ctr3686_aes_alg);
+	rc = nx_register_alg(&nx_ctr3686_aes_alg, NX_FC_AES, NX_MODE_AES_CTR);
 	if (rc)
 		goto out_unreg_ctr;
 
-	rc = crypto_register_alg(&nx_gcm_aes_alg);
+	rc = nx_register_aead(&nx_gcm_aes_alg, NX_FC_AES, NX_MODE_AES_GCM);
 	if (rc)
 		goto out_unreg_ctr3686;
 
-	rc = crypto_register_alg(&nx_gcm4106_aes_alg);
+	rc = nx_register_aead(&nx_gcm4106_aes_alg, NX_FC_AES, NX_MODE_AES_GCM);
 	if (rc)
 		goto out_unreg_gcm;
 
-	rc = crypto_register_alg(&nx_ccm_aes_alg);
+	rc = nx_register_alg(&nx_ccm_aes_alg, NX_FC_AES, NX_MODE_AES_CCM);
 	if (rc)
 		goto out_unreg_gcm4106;
 
-	rc = crypto_register_alg(&nx_ccm4309_aes_alg);
+	rc = nx_register_alg(&nx_ccm4309_aes_alg, NX_FC_AES, NX_MODE_AES_CCM);
 	if (rc)
 		goto out_unreg_ccm;
 
-	rc = crypto_register_shash(&nx_shash_sha256_alg);
+	rc = nx_register_shash(&nx_shash_sha256_alg, NX_FC_SHA, NX_MODE_SHA,
+			       NX_PROPS_SHA256);
 	if (rc)
 		goto out_unreg_ccm4309;
 
-	rc = crypto_register_shash(&nx_shash_sha512_alg);
+	rc = nx_register_shash(&nx_shash_sha512_alg, NX_FC_SHA, NX_MODE_SHA,
+			       NX_PROPS_SHA512);
 	if (rc)
 		goto out_unreg_s256;
 
-	rc = crypto_register_shash(&nx_shash_aes_xcbc_alg);
+	rc = nx_register_shash(&nx_shash_aes_xcbc_alg,
+			       NX_FC_AES, NX_MODE_AES_XCBC_MAC, -1);
 	if (rc)
 		goto out_unreg_s512;
 
 	goto out;
 
 out_unreg_s512:
-	crypto_unregister_shash(&nx_shash_sha512_alg);
+	nx_unregister_shash(&nx_shash_sha512_alg, NX_FC_SHA, NX_MODE_SHA,
+			    NX_PROPS_SHA512);
 out_unreg_s256:
-	crypto_unregister_shash(&nx_shash_sha256_alg);
+	nx_unregister_shash(&nx_shash_sha256_alg, NX_FC_SHA, NX_MODE_SHA,
+			    NX_PROPS_SHA256);
 out_unreg_ccm4309:
-	crypto_unregister_alg(&nx_ccm4309_aes_alg);
+	nx_unregister_alg(&nx_ccm4309_aes_alg, NX_FC_AES, NX_MODE_AES_CCM);
 out_unreg_ccm:
-	crypto_unregister_alg(&nx_ccm_aes_alg);
+	nx_unregister_alg(&nx_ccm_aes_alg, NX_FC_AES, NX_MODE_AES_CCM);
 out_unreg_gcm4106:
-	crypto_unregister_alg(&nx_gcm4106_aes_alg);
+	nx_unregister_aead(&nx_gcm4106_aes_alg, NX_FC_AES, NX_MODE_AES_GCM);
 out_unreg_gcm:
-	crypto_unregister_alg(&nx_gcm_aes_alg);
+	nx_unregister_aead(&nx_gcm_aes_alg, NX_FC_AES, NX_MODE_AES_GCM);
 out_unreg_ctr3686:
-	crypto_unregister_alg(&nx_ctr3686_aes_alg);
+	nx_unregister_alg(&nx_ctr3686_aes_alg, NX_FC_AES, NX_MODE_AES_CTR);
 out_unreg_ctr:
-	crypto_unregister_alg(&nx_ctr_aes_alg);
+	nx_unregister_alg(&nx_ctr_aes_alg, NX_FC_AES, NX_MODE_AES_CTR);
 out_unreg_cbc:
-	crypto_unregister_alg(&nx_cbc_aes_alg);
+	nx_unregister_alg(&nx_cbc_aes_alg, NX_FC_AES, NX_MODE_AES_CBC);
 out_unreg_ecb:
-	crypto_unregister_alg(&nx_ecb_aes_alg);
+	nx_unregister_alg(&nx_ecb_aes_alg, NX_FC_AES, NX_MODE_AES_ECB);
 out:
 	return rc;
 }
@@ -666,9 +717,9 @@ int nx_crypto_ctx_aes_ccm_init(struct crypto_tfm *tfm)
 				  NX_MODE_AES_CCM);
 }
 
-int nx_crypto_ctx_aes_gcm_init(struct crypto_tfm *tfm)
+int nx_crypto_ctx_aes_gcm_init(struct crypto_aead *tfm)
 {
-	return nx_crypto_ctx_init(crypto_tfm_ctx(tfm), NX_FC_AES,
+	return nx_crypto_ctx_init(crypto_aead_ctx(tfm), NX_FC_AES,
 				  NX_MODE_AES_GCM);
 }
 
@@ -720,6 +771,13 @@ void nx_crypto_ctx_exit(struct crypto_tfm *tfm)
 	nx_ctx->out_sg = NULL;
 }
 
+void nx_crypto_ctx_aead_exit(struct crypto_aead *tfm)
+{
+	struct nx_crypto_ctx *nx_ctx = crypto_aead_ctx(tfm);
+
+	kzfree(nx_ctx->kmem);
+}
+
 static int nx_probe(struct vio_dev *viodev, const struct vio_device_id *id)
 {
 	dev_dbg(&viodev->dev, "driver probed: %s resource id: 0x%x\n",
@@ -746,17 +804,24 @@ static int nx_remove(struct vio_dev *viodev)
 	if (nx_driver.of.status == NX_OKAY) {
 		NX_DEBUGFS_FINI(&nx_driver);
 
-		crypto_unregister_alg(&nx_ccm_aes_alg);
-		crypto_unregister_alg(&nx_ccm4309_aes_alg);
-		crypto_unregister_alg(&nx_gcm_aes_alg);
-		crypto_unregister_alg(&nx_gcm4106_aes_alg);
-		crypto_unregister_alg(&nx_ctr_aes_alg);
-		crypto_unregister_alg(&nx_ctr3686_aes_alg);
-		crypto_unregister_alg(&nx_cbc_aes_alg);
-		crypto_unregister_alg(&nx_ecb_aes_alg);
-		crypto_unregister_shash(&nx_shash_sha256_alg);
-		crypto_unregister_shash(&nx_shash_sha512_alg);
-		crypto_unregister_shash(&nx_shash_aes_xcbc_alg);
+		nx_unregister_shash(&nx_shash_aes_xcbc_alg,
+				    NX_FC_AES, NX_MODE_AES_XCBC_MAC, -1);
+		nx_unregister_shash(&nx_shash_sha512_alg,
+				    NX_FC_SHA, NX_MODE_SHA, NX_PROPS_SHA256);
+		nx_unregister_shash(&nx_shash_sha256_alg,
+				    NX_FC_SHA, NX_MODE_SHA, NX_PROPS_SHA512);
+		nx_unregister_alg(&nx_ccm4309_aes_alg,
+				  NX_FC_AES, NX_MODE_AES_CCM);
+		nx_unregister_alg(&nx_ccm_aes_alg, NX_FC_AES, NX_MODE_AES_CCM);
+		nx_unregister_aead(&nx_gcm4106_aes_alg,
+				   NX_FC_AES, NX_MODE_AES_GCM);
+		nx_unregister_aead(&nx_gcm_aes_alg,
+				   NX_FC_AES, NX_MODE_AES_GCM);
+		nx_unregister_alg(&nx_ctr3686_aes_alg,
+				  NX_FC_AES, NX_MODE_AES_CTR);
+		nx_unregister_alg(&nx_ctr_aes_alg, NX_FC_AES, NX_MODE_AES_CTR);
+		nx_unregister_alg(&nx_cbc_aes_alg, NX_FC_AES, NX_MODE_AES_CBC);
+		nx_unregister_alg(&nx_ecb_aes_alg, NX_FC_AES, NX_MODE_AES_ECB);
 	}
 
 	return 0;
diff --git a/drivers/crypto/nx/nx.h b/drivers/crypto/nx/nx.h
index 6c9ecaaead52..de3ea8738146 100644
--- a/drivers/crypto/nx/nx.h
+++ b/drivers/crypto/nx/nx.h
@@ -143,18 +143,17 @@ struct nx_crypto_ctx {
 
 /* prototypes */
 int nx_crypto_ctx_aes_ccm_init(struct crypto_tfm *tfm);
-int nx_crypto_ctx_aes_gcm_init(struct crypto_tfm *tfm);
+int nx_crypto_ctx_aes_gcm_init(struct crypto_aead *tfm);
 int nx_crypto_ctx_aes_xcbc_init(struct crypto_tfm *tfm);
 int nx_crypto_ctx_aes_ctr_init(struct crypto_tfm *tfm);
 int nx_crypto_ctx_aes_cbc_init(struct crypto_tfm *tfm);
 int nx_crypto_ctx_aes_ecb_init(struct crypto_tfm *tfm);
 int nx_crypto_ctx_sha_init(struct crypto_tfm *tfm);
 void nx_crypto_ctx_exit(struct crypto_tfm *tfm);
+void nx_crypto_ctx_aead_exit(struct crypto_aead *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);
-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 *,
@@ -178,8 +177,8 @@ void nx_debugfs_fini(struct nx_crypto_driver *);
 
 extern struct crypto_alg nx_cbc_aes_alg;
 extern struct crypto_alg nx_ecb_aes_alg;
-extern struct crypto_alg nx_gcm_aes_alg;
-extern struct crypto_alg nx_gcm4106_aes_alg;
+extern struct aead_alg nx_gcm_aes_alg;
+extern struct aead_alg nx_gcm4106_aes_alg;
 extern struct crypto_alg nx_ctr_aes_alg;
 extern struct crypto_alg nx_ctr3686_aes_alg;
 extern struct crypto_alg nx_ccm_aes_alg;
diff --git a/drivers/crypto/omap-sham.c b/drivers/crypto/omap-sham.c
index 4d63e0d4da9a..b2024c95a3cf 100644
--- a/drivers/crypto/omap-sham.c
+++ b/drivers/crypto/omap-sham.c
@@ -362,7 +362,13 @@ static void omap_sham_copy_ready_hash(struct ahash_request *req)
 
 static int omap_sham_hw_init(struct omap_sham_dev *dd)
 {
-	pm_runtime_get_sync(dd->dev);
+	int err;
+
+	err = pm_runtime_get_sync(dd->dev);
+	if (err < 0) {
+		dev_err(dd->dev, "failed to get sync: %d\n", err);
+		return err;
+	}
 
 	if (!test_bit(FLAGS_INIT, &dd->flags)) {
 		set_bit(FLAGS_INIT, &dd->flags);
@@ -1793,6 +1799,10 @@ static const struct of_device_id omap_sham_of_match[] = {
 		.data		= &omap_sham_pdata_omap2,
 	},
 	{
+		.compatible	= "ti,omap3-sham",
+		.data		= &omap_sham_pdata_omap2,
+	},
+	{
 		.compatible	= "ti,omap4-sham",
 		.data		= &omap_sham_pdata_omap4,
 	},
@@ -1947,7 +1957,13 @@ static int omap_sham_probe(struct platform_device *pdev)
 
 	pm_runtime_enable(dev);
 	pm_runtime_irq_safe(dev);
-	pm_runtime_get_sync(dev);
+
+	err = pm_runtime_get_sync(dev);
+	if (err < 0) {
+		dev_err(dev, "failed to get sync: %d\n", err);
+		goto err_pm;
+	}
+
 	rev = omap_sham_read(dd, SHA_REG_REV(dd));
 	pm_runtime_put_sync(&pdev->dev);
 
@@ -1977,6 +1993,7 @@ err_algs:
 		for (j = dd->pdata->algs_info[i].registered - 1; j >= 0; j--)
 			crypto_unregister_ahash(
 					&dd->pdata->algs_info[i].algs_list[j]);
+err_pm:
 	pm_runtime_disable(dev);
 	if (dd->dma_lch)
 		dma_release_channel(dd->dma_lch);
@@ -2019,7 +2036,11 @@ static int omap_sham_suspend(struct device *dev)
 
 static int omap_sham_resume(struct device *dev)
 {
-	pm_runtime_get_sync(dev);
+	int err = pm_runtime_get_sync(dev);
+	if (err < 0) {
+		dev_err(dev, "failed to get sync: %d\n", err);
+		return err;
+	}
 	return 0;
 }
 #endif
diff --git a/drivers/crypto/padlock-aes.c b/drivers/crypto/padlock-aes.c
index c178ed8c3908..da2d6777bd09 100644
--- a/drivers/crypto/padlock-aes.c
+++ b/drivers/crypto/padlock-aes.c
@@ -22,7 +22,7 @@
 #include <asm/cpu_device_id.h>
 #include <asm/byteorder.h>
 #include <asm/processor.h>
-#include <asm/i387.h>
+#include <asm/fpu/api.h>
 
 /*
  * Number of data blocks actually fetched for each xcrypt insn.
diff --git a/drivers/crypto/padlock-sha.c b/drivers/crypto/padlock-sha.c
index 95f7d27ce491..4e154c9b9206 100644
--- a/drivers/crypto/padlock-sha.c
+++ b/drivers/crypto/padlock-sha.c
@@ -23,7 +23,7 @@
 #include <linux/kernel.h>
 #include <linux/scatterlist.h>
 #include <asm/cpu_device_id.h>
-#include <asm/i387.h>
+#include <asm/fpu/api.h>
 
 struct padlock_sha_desc {
 	struct shash_desc fallback;
diff --git a/drivers/crypto/picoxcell_crypto.c b/drivers/crypto/picoxcell_crypto.c
index 5da5b98b8f29..4f56f3681abd 100644
--- a/drivers/crypto/picoxcell_crypto.c
+++ b/drivers/crypto/picoxcell_crypto.c
@@ -15,7 +15,7 @@
  * along with this program; if not, write to the Free Software
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
-#include <crypto/aead.h>
+#include <crypto/internal/aead.h>
 #include <crypto/aes.h>
 #include <crypto/algapi.h>
 #include <crypto/authenc.h>
@@ -40,6 +40,7 @@
 #include <linux/rtnetlink.h>
 #include <linux/scatterlist.h>
 #include <linux/sched.h>
+#include <linux/sizes.h>
 #include <linux/slab.h>
 #include <linux/timer.h>
 
@@ -261,18 +262,9 @@ static unsigned spacc_load_ctx(struct spacc_generic_ctx *ctx,
 }
 
 /* Count the number of scatterlist entries in a scatterlist. */
-static int sg_count(struct scatterlist *sg_list, int nbytes)
+static inline int sg_count(struct scatterlist *sg_list, int nbytes)
 {
-	struct scatterlist *sg = sg_list;
-	int sg_nents = 0;
-
-	while (nbytes > 0) {
-		++sg_nents;
-		nbytes -= sg->length;
-		sg = sg_next(sg);
-	}
-
-	return sg_nents;
+	return sg_nents_for_len(sg_list, nbytes);
 }
 
 static inline void ddt_set(struct spacc_ddt *ddt, dma_addr_t phys, size_t len)
@@ -326,6 +318,7 @@ static int spacc_aead_make_ddts(struct spacc_req *req, u8 *giv)
 	struct spacc_ddt *src_ddt, *dst_ddt;
 	unsigned ivsize = crypto_aead_ivsize(crypto_aead_reqtfm(areq));
 	unsigned nents = sg_count(areq->src, areq->cryptlen);
+	unsigned total;
 	dma_addr_t iv_addr;
 	struct scatterlist *cur;
 	int i, dst_ents, src_ents, assoc_ents;
@@ -369,11 +362,18 @@ static int spacc_aead_make_ddts(struct spacc_req *req, u8 *giv)
 	 * Map the associated data. For decryption we don't copy the
 	 * associated data.
 	 */
+	total = areq->assoclen;
 	for_each_sg(areq->assoc, cur, assoc_ents, i) {
-		ddt_set(src_ddt++, sg_dma_address(cur), sg_dma_len(cur));
+		unsigned len = sg_dma_len(cur);
+
+		if (len > total)
+			len = total;
+
+		total -= len;
+
+		ddt_set(src_ddt++, sg_dma_address(cur), len);
 		if (req->is_encrypt)
-			ddt_set(dst_ddt++, sg_dma_address(cur),
-				sg_dma_len(cur));
+			ddt_set(dst_ddt++, sg_dma_address(cur), len);
 	}
 	ddt_set(src_ddt++, iv_addr, ivsize);
 
@@ -790,7 +790,8 @@ static int spacc_aead_cra_init(struct crypto_tfm *tfm)
 
 	get_random_bytes(ctx->salt, sizeof(ctx->salt));
 
-	tfm->crt_aead.reqsize = sizeof(struct spacc_req);
+	crypto_aead_set_reqsize(__crypto_aead_cast(tfm),
+				sizeof(struct spacc_req));
 
 	return 0;
 }
@@ -1754,15 +1755,15 @@ static int spacc_probe(struct platform_device *pdev)
 		return PTR_ERR(engine->clk);
 	}
 
-	if (clk_enable(engine->clk)) {
-		dev_info(&pdev->dev, "unable to enable clk\n");
+	if (clk_prepare_enable(engine->clk)) {
+		dev_info(&pdev->dev, "unable to prepare/enable clk\n");
 		clk_put(engine->clk);
 		return -EIO;
 	}
 
 	err = device_create_file(&pdev->dev, &dev_attr_stat_irq_thresh);
 	if (err) {
-		clk_disable(engine->clk);
+		clk_disable_unprepare(engine->clk);
 		clk_put(engine->clk);
 		return err;
 	}
@@ -1830,7 +1831,7 @@ static int spacc_remove(struct platform_device *pdev)
 		crypto_unregister_alg(&alg->alg);
 	}
 
-	clk_disable(engine->clk);
+	clk_disable_unprepare(engine->clk);
 	clk_put(engine->clk);
 
 	return 0;
diff --git a/drivers/crypto/qat/Kconfig b/drivers/crypto/qat/Kconfig
index 49bede2a9f77..6fdb9e8b22a7 100644
--- a/drivers/crypto/qat/Kconfig
+++ b/drivers/crypto/qat/Kconfig
@@ -2,9 +2,8 @@ config CRYPTO_DEV_QAT
 	tristate
 	select CRYPTO_AEAD
 	select CRYPTO_AUTHENC
-	select CRYPTO_ALGAPI
-	select CRYPTO_AES
-	select CRYPTO_CBC
+	select CRYPTO_BLKCIPHER
+	select CRYPTO_HMAC
 	select CRYPTO_SHA1
 	select CRYPTO_SHA256
 	select CRYPTO_SHA512
@@ -13,7 +12,6 @@ config CRYPTO_DEV_QAT
 config CRYPTO_DEV_QAT_DH895xCC
 	tristate "Support for Intel(R) DH895xCC"
 	depends on X86 && PCI
-	default n
 	select CRYPTO_DEV_QAT
 	help
 	  Support for Intel(R) DH895xcc with Intel(R) QuickAssist Technology
diff --git a/drivers/crypto/qat/qat_common/adf_accel_devices.h b/drivers/crypto/qat/qat_common/adf_accel_devices.h
index f22ce7169fa5..5fe902967620 100644
--- a/drivers/crypto/qat/qat_common/adf_accel_devices.h
+++ b/drivers/crypto/qat/qat_common/adf_accel_devices.h
@@ -48,7 +48,6 @@
 #define ADF_ACCEL_DEVICES_H_
 #include <linux/module.h>
 #include <linux/list.h>
-#include <linux/proc_fs.h>
 #include <linux/io.h>
 #include "adf_cfg_common.h"
 
diff --git a/drivers/crypto/qat/qat_common/adf_accel_engine.c b/drivers/crypto/qat/qat_common/adf_accel_engine.c
index 7f8b66c915ed..fdda8e7ae302 100644
--- a/drivers/crypto/qat/qat_common/adf_accel_engine.c
+++ b/drivers/crypto/qat/qat_common/adf_accel_engine.c
@@ -88,10 +88,7 @@ void adf_ae_fw_release(struct adf_accel_dev *accel_dev)
 
 	qat_uclo_del_uof_obj(loader_data->fw_loader);
 	qat_hal_deinit(loader_data->fw_loader);
-
-	if (loader_data->uof_fw)
-		release_firmware(loader_data->uof_fw);
-
+	release_firmware(loader_data->uof_fw);
 	loader_data->uof_fw = NULL;
 	loader_data->fw_loader = NULL;
 }
diff --git a/drivers/crypto/qat/qat_common/adf_cfg_user.h b/drivers/crypto/qat/qat_common/adf_cfg_user.h
index 0c38a155a865..ef5988afd4c6 100644
--- a/drivers/crypto/qat/qat_common/adf_cfg_user.h
+++ b/drivers/crypto/qat/qat_common/adf_cfg_user.h
@@ -54,14 +54,6 @@ struct adf_user_cfg_key_val {
 	char key[ADF_CFG_MAX_KEY_LEN_IN_BYTES];
 	char val[ADF_CFG_MAX_VAL_LEN_IN_BYTES];
 	union {
-		char *user_val_ptr;
-		uint64_t padding1;
-	};
-	union {
-		struct adf_user_cfg_key_val *prev;
-		uint64_t padding2;
-	};
-	union {
 		struct adf_user_cfg_key_val *next;
 		uint64_t padding3;
 	};
@@ -75,10 +67,6 @@ struct adf_user_cfg_section {
 		uint64_t padding1;
 	};
 	union {
-		struct adf_user_cfg_section *prev;
-		uint64_t padding2;
-	};
-	union {
 		struct adf_user_cfg_section *next;
 		uint64_t padding3;
 	};
diff --git a/drivers/crypto/qat/qat_common/adf_common_drv.h b/drivers/crypto/qat/qat_common/adf_common_drv.h
index 0666ee6a3360..27e16c09230b 100644
--- a/drivers/crypto/qat/qat_common/adf_common_drv.h
+++ b/drivers/crypto/qat/qat_common/adf_common_drv.h
@@ -53,6 +53,13 @@
 #include "icp_qat_fw_loader_handle.h"
 #include "icp_qat_hal.h"
 
+#define ADF_MAJOR_VERSION	0
+#define ADF_MINOR_VERSION	1
+#define ADF_BUILD_VERSION	3
+#define ADF_DRV_VERSION		__stringify(ADF_MAJOR_VERSION) "." \
+				__stringify(ADF_MINOR_VERSION) "." \
+				__stringify(ADF_BUILD_VERSION)
+
 #define ADF_STATUS_RESTARTING 0
 #define ADF_STATUS_STARTING 1
 #define ADF_STATUS_CONFIGURED 2
diff --git a/drivers/crypto/qat/qat_common/adf_ctl_drv.c b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
index cb5f066e93a6..e056b9e9bf8a 100644
--- a/drivers/crypto/qat/qat_common/adf_ctl_drv.c
+++ b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
@@ -504,3 +504,4 @@ MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Intel");
 MODULE_DESCRIPTION("Intel(R) QuickAssist Technology");
 MODULE_ALIAS_CRYPTO("intel_qat");
+MODULE_VERSION(ADF_DRV_VERSION);
diff --git a/drivers/crypto/qat/qat_common/adf_transport.c b/drivers/crypto/qat/qat_common/adf_transport.c
index ccec327489da..db2926bff8a5 100644
--- a/drivers/crypto/qat/qat_common/adf_transport.c
+++ b/drivers/crypto/qat/qat_common/adf_transport.c
@@ -449,7 +449,7 @@ static int adf_init_bank(struct adf_accel_dev *accel_dev,
 err:
 	for (i = 0; i < ADF_ETR_MAX_RINGS_PER_BANK; i++) {
 		ring = &bank->rings[i];
-		if (hw_data->tx_rings_mask & (1 << i) && ring->inflights)
+		if (hw_data->tx_rings_mask & (1 << i))
 			kfree(ring->inflights);
 	}
 	return -ENOMEM;
diff --git a/drivers/crypto/qat/qat_common/qat_algs.c b/drivers/crypto/qat/qat_common/qat_algs.c
index 1dc5b0a17cf7..067402c7c2a9 100644
--- a/drivers/crypto/qat/qat_common/qat_algs.c
+++ b/drivers/crypto/qat/qat_common/qat_algs.c
@@ -47,7 +47,7 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/crypto.h>
-#include <crypto/aead.h>
+#include <crypto/internal/aead.h>
 #include <crypto/aes.h>
 #include <crypto/sha.h>
 #include <crypto/hash.h>
@@ -653,7 +653,7 @@ static void qat_alg_free_bufl(struct qat_crypto_instance *inst,
 }
 
 static int qat_alg_sgl_to_bufl(struct qat_crypto_instance *inst,
-			       struct scatterlist *assoc,
+			       struct scatterlist *assoc, int assoclen,
 			       struct scatterlist *sgl,
 			       struct scatterlist *sglout, uint8_t *iv,
 			       uint8_t ivlen,
@@ -685,15 +685,21 @@ static int qat_alg_sgl_to_bufl(struct qat_crypto_instance *inst,
 	for_each_sg(assoc, sg, assoc_n, i) {
 		if (!sg->length)
 			continue;
-		bufl->bufers[bufs].addr = dma_map_single(dev,
-							 sg_virt(sg),
-							 sg->length,
-							 DMA_BIDIRECTIONAL);
-		bufl->bufers[bufs].len = sg->length;
+
+		if (!(assoclen > 0))
+			break;
+
+		bufl->bufers[bufs].addr =
+			dma_map_single(dev, sg_virt(sg),
+				       min_t(int, assoclen, sg->length),
+				       DMA_BIDIRECTIONAL);
+		bufl->bufers[bufs].len = min_t(int, assoclen, sg->length);
 		if (unlikely(dma_mapping_error(dev, bufl->bufers[bufs].addr)))
 			goto err;
 		bufs++;
+		assoclen -= sg->length;
 	}
+
 	if (ivlen) {
 		bufl->bufers[bufs].addr = dma_map_single(dev, iv, ivlen,
 							 DMA_BIDIRECTIONAL);
@@ -845,8 +851,9 @@ static int qat_alg_aead_dec(struct aead_request *areq)
 	int digst_size = crypto_aead_crt(aead_tfm)->authsize;
 	int ret, ctr = 0;
 
-	ret = qat_alg_sgl_to_bufl(ctx->inst, areq->assoc, areq->src, areq->dst,
-				  areq->iv, AES_BLOCK_SIZE, qat_req);
+	ret = qat_alg_sgl_to_bufl(ctx->inst, areq->assoc, areq->assoclen,
+				  areq->src, areq->dst, areq->iv,
+				  AES_BLOCK_SIZE, qat_req);
 	if (unlikely(ret))
 		return ret;
 
@@ -889,8 +896,9 @@ static int qat_alg_aead_enc_internal(struct aead_request *areq, uint8_t *iv,
 	struct icp_qat_fw_la_bulk_req *msg;
 	int ret, ctr = 0;
 
-	ret = qat_alg_sgl_to_bufl(ctx->inst, areq->assoc, areq->src, areq->dst,
-				  iv, AES_BLOCK_SIZE, qat_req);
+	ret = qat_alg_sgl_to_bufl(ctx->inst, areq->assoc, areq->assoclen,
+				  areq->src, areq->dst, iv, AES_BLOCK_SIZE,
+				  qat_req);
 	if (unlikely(ret))
 		return ret;
 
@@ -1017,7 +1025,7 @@ static int qat_alg_ablkcipher_encrypt(struct ablkcipher_request *req)
 	struct icp_qat_fw_la_bulk_req *msg;
 	int ret, ctr = 0;
 
-	ret = qat_alg_sgl_to_bufl(ctx->inst, NULL, req->src, req->dst,
+	ret = qat_alg_sgl_to_bufl(ctx->inst, NULL, 0, req->src, req->dst,
 				  NULL, 0, qat_req);
 	if (unlikely(ret))
 		return ret;
@@ -1055,7 +1063,7 @@ static int qat_alg_ablkcipher_decrypt(struct ablkcipher_request *req)
 	struct icp_qat_fw_la_bulk_req *msg;
 	int ret, ctr = 0;
 
-	ret = qat_alg_sgl_to_bufl(ctx->inst, NULL, req->src, req->dst,
+	ret = qat_alg_sgl_to_bufl(ctx->inst, NULL, 0, req->src, req->dst,
 				  NULL, 0, qat_req);
 	if (unlikely(ret))
 		return ret;
@@ -1094,8 +1102,9 @@ static int qat_alg_aead_init(struct crypto_tfm *tfm,
 		return -EFAULT;
 	spin_lock_init(&ctx->lock);
 	ctx->qat_hash_alg = hash;
-	tfm->crt_aead.reqsize = sizeof(struct aead_request) +
-				sizeof(struct qat_crypto_request);
+	crypto_aead_set_reqsize(__crypto_aead_cast(tfm),
+		sizeof(struct aead_request) +
+		sizeof(struct qat_crypto_request));
 	ctx->tfm = tfm;
 	return 0;
 }
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_drv.c b/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
index 9decea2779c6..1bde45b7a3c5 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
@@ -300,6 +300,8 @@ static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (ret)
 		goto out_err;
 
+	pcie_set_readrq(pdev, 1024);
+
 	/* enable PCI device */
 	if (pci_enable_device(pdev)) {
 		ret = -EFAULT;
@@ -417,5 +419,6 @@ module_exit(adfdrv_release);
 
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Intel");
-MODULE_FIRMWARE("qat_895xcc.bin");
+MODULE_FIRMWARE(ADF_DH895XCC_FW);
 MODULE_DESCRIPTION("Intel(R) QuickAssist Technology");
+MODULE_VERSION(ADF_DRV_VERSION);
diff --git a/drivers/crypto/sahara.c b/drivers/crypto/sahara.c
index 6be377f6b9e7..397a500b3d8a 100644
--- a/drivers/crypto/sahara.c
+++ b/drivers/crypto/sahara.c
@@ -1578,8 +1578,12 @@ static int sahara_probe(struct platform_device *pdev)
 
 	init_completion(&dev->dma_completion);
 
-	clk_prepare_enable(dev->clk_ipg);
-	clk_prepare_enable(dev->clk_ahb);
+	err = clk_prepare_enable(dev->clk_ipg);
+	if (err)
+		goto err_link;
+	err = clk_prepare_enable(dev->clk_ahb);
+	if (err)
+		goto clk_ipg_disable;
 
 	version = sahara_read(dev, SAHARA_REG_VERSION);
 	if (of_device_is_compatible(pdev->dev.of_node, "fsl,imx27-sahara")) {
@@ -1619,10 +1623,11 @@ err_algs:
 	dma_free_coherent(&pdev->dev,
 			  SAHARA_MAX_HW_LINK * sizeof(struct sahara_hw_link),
 			  dev->hw_link[0], dev->hw_phys_link[0]);
-	clk_disable_unprepare(dev->clk_ipg);
-	clk_disable_unprepare(dev->clk_ahb);
 	kthread_stop(dev->kthread);
 	dev_ptr = NULL;
+	clk_disable_unprepare(dev->clk_ahb);
+clk_ipg_disable:
+	clk_disable_unprepare(dev->clk_ipg);
 err_link:
 	dma_free_coherent(&pdev->dev,
 			  2 * AES_KEYSIZE_128,
diff --git a/drivers/crypto/talitos.c b/drivers/crypto/talitos.c
index 857414afa29a..83aca95a95bc 100644
--- a/drivers/crypto/talitos.c
+++ b/drivers/crypto/talitos.c
@@ -46,7 +46,7 @@
 #include <crypto/des.h>
 #include <crypto/sha.h>
 #include <crypto/md5.h>
-#include <crypto/aead.h>
+#include <crypto/internal/aead.h>
 #include <crypto/authenc.h>
 #include <crypto/skcipher.h>
 #include <crypto/hash.h>
@@ -55,49 +55,92 @@
 
 #include "talitos.h"
 
-static void to_talitos_ptr(struct talitos_ptr *talitos_ptr, dma_addr_t dma_addr)
+static void to_talitos_ptr(struct talitos_ptr *ptr, dma_addr_t dma_addr,
+			   bool is_sec1)
 {
-	talitos_ptr->ptr = cpu_to_be32(lower_32_bits(dma_addr));
-	talitos_ptr->eptr = upper_32_bits(dma_addr);
+	ptr->ptr = cpu_to_be32(lower_32_bits(dma_addr));
+	if (!is_sec1)
+		ptr->eptr = upper_32_bits(dma_addr);
+}
+
+static void to_talitos_ptr_len(struct talitos_ptr *ptr, unsigned int len,
+			       bool is_sec1)
+{
+	if (is_sec1) {
+		ptr->res = 0;
+		ptr->len1 = cpu_to_be16(len);
+	} else {
+		ptr->len = cpu_to_be16(len);
+	}
+}
+
+static unsigned short from_talitos_ptr_len(struct talitos_ptr *ptr,
+					   bool is_sec1)
+{
+	if (is_sec1)
+		return be16_to_cpu(ptr->len1);
+	else
+		return be16_to_cpu(ptr->len);
+}
+
+static void to_talitos_ptr_extent_clear(struct talitos_ptr *ptr, bool is_sec1)
+{
+	if (!is_sec1)
+		ptr->j_extent = 0;
 }
 
 /*
  * map virtual single (contiguous) pointer to h/w descriptor pointer
  */
 static void map_single_talitos_ptr(struct device *dev,
-				   struct talitos_ptr *talitos_ptr,
-				   unsigned short len, void *data,
-				   unsigned char extent,
+				   struct talitos_ptr *ptr,
+				   unsigned int len, void *data,
 				   enum dma_data_direction dir)
 {
 	dma_addr_t dma_addr = dma_map_single(dev, data, len, dir);
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
 
-	talitos_ptr->len = cpu_to_be16(len);
-	to_talitos_ptr(talitos_ptr, dma_addr);
-	talitos_ptr->j_extent = extent;
+	to_talitos_ptr_len(ptr, len, is_sec1);
+	to_talitos_ptr(ptr, dma_addr, is_sec1);
+	to_talitos_ptr_extent_clear(ptr, is_sec1);
 }
 
 /*
  * unmap bus single (contiguous) h/w descriptor pointer
  */
 static void unmap_single_talitos_ptr(struct device *dev,
-				     struct talitos_ptr *talitos_ptr,
+				     struct talitos_ptr *ptr,
 				     enum dma_data_direction dir)
 {
-	dma_unmap_single(dev, be32_to_cpu(talitos_ptr->ptr),
-			 be16_to_cpu(talitos_ptr->len), dir);
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
+
+	dma_unmap_single(dev, be32_to_cpu(ptr->ptr),
+			 from_talitos_ptr_len(ptr, is_sec1), dir);
 }
 
 static int reset_channel(struct device *dev, int ch)
 {
 	struct talitos_private *priv = dev_get_drvdata(dev);
 	unsigned int timeout = TALITOS_TIMEOUT;
+	bool is_sec1 = has_ftr_sec1(priv);
 
-	setbits32(priv->chan[ch].reg + TALITOS_CCCR, TALITOS_CCCR_RESET);
+	if (is_sec1) {
+		setbits32(priv->chan[ch].reg + TALITOS_CCCR_LO,
+			  TALITOS1_CCCR_LO_RESET);
 
-	while ((in_be32(priv->chan[ch].reg + TALITOS_CCCR) & TALITOS_CCCR_RESET)
-	       && --timeout)
-		cpu_relax();
+		while ((in_be32(priv->chan[ch].reg + TALITOS_CCCR_LO) &
+			TALITOS1_CCCR_LO_RESET) && --timeout)
+			cpu_relax();
+	} else {
+		setbits32(priv->chan[ch].reg + TALITOS_CCCR,
+			  TALITOS2_CCCR_RESET);
+
+		while ((in_be32(priv->chan[ch].reg + TALITOS_CCCR) &
+			TALITOS2_CCCR_RESET) && --timeout)
+			cpu_relax();
+	}
 
 	if (timeout == 0) {
 		dev_err(dev, "failed to reset channel %d\n", ch);
@@ -120,11 +163,12 @@ static int reset_device(struct device *dev)
 {
 	struct talitos_private *priv = dev_get_drvdata(dev);
 	unsigned int timeout = TALITOS_TIMEOUT;
-	u32 mcr = TALITOS_MCR_SWR;
+	bool is_sec1 = has_ftr_sec1(priv);
+	u32 mcr = is_sec1 ? TALITOS1_MCR_SWR : TALITOS2_MCR_SWR;
 
 	setbits32(priv->reg + TALITOS_MCR, mcr);
 
-	while ((in_be32(priv->reg + TALITOS_MCR) & TALITOS_MCR_SWR)
+	while ((in_be32(priv->reg + TALITOS_MCR) & mcr)
 	       && --timeout)
 		cpu_relax();
 
@@ -148,6 +192,7 @@ static int init_device(struct device *dev)
 {
 	struct talitos_private *priv = dev_get_drvdata(dev);
 	int ch, err;
+	bool is_sec1 = has_ftr_sec1(priv);
 
 	/*
 	 * Master reset
@@ -171,12 +216,19 @@ static int init_device(struct device *dev)
 	}
 
 	/* enable channel done and error interrupts */
-	setbits32(priv->reg + TALITOS_IMR, TALITOS_IMR_INIT);
-	setbits32(priv->reg + TALITOS_IMR_LO, TALITOS_IMR_LO_INIT);
+	if (is_sec1) {
+		clrbits32(priv->reg + TALITOS_IMR, TALITOS1_IMR_INIT);
+		clrbits32(priv->reg + TALITOS_IMR_LO, TALITOS1_IMR_LO_INIT);
+		/* disable parity error check in DEU (erroneous? test vect.) */
+		setbits32(priv->reg_deu + TALITOS_EUICR, TALITOS1_DEUICR_KPE);
+	} else {
+		setbits32(priv->reg + TALITOS_IMR, TALITOS2_IMR_INIT);
+		setbits32(priv->reg + TALITOS_IMR_LO, TALITOS2_IMR_LO_INIT);
+	}
 
 	/* disable integrity check error interrupts (use writeback instead) */
 	if (priv->features & TALITOS_FTR_HW_AUTH_CHECK)
-		setbits32(priv->reg + TALITOS_MDEUICR_LO,
+		setbits32(priv->reg_mdeu + TALITOS_EUICR_LO,
 		          TALITOS_MDEUICR_LO_ICE);
 
 	return 0;
@@ -204,6 +256,7 @@ int talitos_submit(struct device *dev, int ch, struct talitos_desc *desc,
 	struct talitos_request *request;
 	unsigned long flags;
 	int head;
+	bool is_sec1 = has_ftr_sec1(priv);
 
 	spin_lock_irqsave(&priv->chan[ch].head_lock, flags);
 
@@ -217,8 +270,17 @@ int talitos_submit(struct device *dev, int ch, struct talitos_desc *desc,
 	request = &priv->chan[ch].fifo[head];
 
 	/* map descriptor and save caller data */
-	request->dma_desc = dma_map_single(dev, desc, sizeof(*desc),
-					   DMA_BIDIRECTIONAL);
+	if (is_sec1) {
+		desc->hdr1 = desc->hdr;
+		desc->next_desc = 0;
+		request->dma_desc = dma_map_single(dev, &desc->hdr1,
+						   TALITOS_DESC_SIZE,
+						   DMA_BIDIRECTIONAL);
+	} else {
+		request->dma_desc = dma_map_single(dev, desc,
+						   TALITOS_DESC_SIZE,
+						   DMA_BIDIRECTIONAL);
+	}
 	request->callback = callback;
 	request->context = context;
 
@@ -250,16 +312,21 @@ static void flush_channel(struct device *dev, int ch, int error, int reset_ch)
 	struct talitos_request *request, saved_req;
 	unsigned long flags;
 	int tail, status;
+	bool is_sec1 = has_ftr_sec1(priv);
 
 	spin_lock_irqsave(&priv->chan[ch].tail_lock, flags);
 
 	tail = priv->chan[ch].tail;
 	while (priv->chan[ch].fifo[tail].desc) {
+		__be32 hdr;
+
 		request = &priv->chan[ch].fifo[tail];
 
 		/* descriptors with their done bits set don't get the error */
 		rmb();
-		if ((request->desc->hdr & DESC_HDR_DONE) == DESC_HDR_DONE)
+		hdr = is_sec1 ? request->desc->hdr1 : request->desc->hdr;
+
+		if ((hdr & DESC_HDR_DONE) == DESC_HDR_DONE)
 			status = 0;
 		else
 			if (!error)
@@ -268,7 +335,7 @@ static void flush_channel(struct device *dev, int ch, int error, int reset_ch)
 				status = error;
 
 		dma_unmap_single(dev, request->dma_desc,
-				 sizeof(struct talitos_desc),
+				 TALITOS_DESC_SIZE,
 				 DMA_BIDIRECTIONAL);
 
 		/* copy entries so we can call callback outside lock */
@@ -302,8 +369,37 @@ static void flush_channel(struct device *dev, int ch, int error, int reset_ch)
 /*
  * process completed requests for channels that have done status
  */
-#define DEF_TALITOS_DONE(name, ch_done_mask)				\
-static void talitos_done_##name(unsigned long data)			\
+#define DEF_TALITOS1_DONE(name, ch_done_mask)				\
+static void talitos1_done_##name(unsigned long data)			\
+{									\
+	struct device *dev = (struct device *)data;			\
+	struct talitos_private *priv = dev_get_drvdata(dev);		\
+	unsigned long flags;						\
+									\
+	if (ch_done_mask & 0x10000000)					\
+		flush_channel(dev, 0, 0, 0);			\
+	if (priv->num_channels == 1)					\
+		goto out;						\
+	if (ch_done_mask & 0x40000000)					\
+		flush_channel(dev, 1, 0, 0);			\
+	if (ch_done_mask & 0x00010000)					\
+		flush_channel(dev, 2, 0, 0);			\
+	if (ch_done_mask & 0x00040000)					\
+		flush_channel(dev, 3, 0, 0);			\
+									\
+out:									\
+	/* At this point, all completed channels have been processed */	\
+	/* Unmask done interrupts for channels completed later on. */	\
+	spin_lock_irqsave(&priv->reg_lock, flags);			\
+	clrbits32(priv->reg + TALITOS_IMR, ch_done_mask);		\
+	clrbits32(priv->reg + TALITOS_IMR_LO, TALITOS1_IMR_LO_INIT);	\
+	spin_unlock_irqrestore(&priv->reg_lock, flags);			\
+}
+
+DEF_TALITOS1_DONE(4ch, TALITOS1_ISR_4CHDONE)
+
+#define DEF_TALITOS2_DONE(name, ch_done_mask)				\
+static void talitos2_done_##name(unsigned long data)			\
 {									\
 	struct device *dev = (struct device *)data;			\
 	struct talitos_private *priv = dev_get_drvdata(dev);		\
@@ -325,12 +421,13 @@ out:									\
 	/* Unmask done interrupts for channels completed later on. */	\
 	spin_lock_irqsave(&priv->reg_lock, flags);			\
 	setbits32(priv->reg + TALITOS_IMR, ch_done_mask);		\
-	setbits32(priv->reg + TALITOS_IMR_LO, TALITOS_IMR_LO_INIT);	\
+	setbits32(priv->reg + TALITOS_IMR_LO, TALITOS2_IMR_LO_INIT);	\
 	spin_unlock_irqrestore(&priv->reg_lock, flags);			\
 }
-DEF_TALITOS_DONE(4ch, TALITOS_ISR_4CHDONE)
-DEF_TALITOS_DONE(ch0_2, TALITOS_ISR_CH_0_2_DONE)
-DEF_TALITOS_DONE(ch1_3, TALITOS_ISR_CH_1_3_DONE)
+
+DEF_TALITOS2_DONE(4ch, TALITOS2_ISR_4CHDONE)
+DEF_TALITOS2_DONE(ch0_2, TALITOS2_ISR_CH_0_2_DONE)
+DEF_TALITOS2_DONE(ch1_3, TALITOS2_ISR_CH_1_3_DONE)
 
 /*
  * locate current (offending) descriptor
@@ -377,44 +474,44 @@ static void report_eu_error(struct device *dev, int ch, u32 desc_hdr)
 	switch (desc_hdr & DESC_HDR_SEL0_MASK) {
 	case DESC_HDR_SEL0_AFEU:
 		dev_err(dev, "AFEUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_AFEUISR),
-			in_be32(priv->reg + TALITOS_AFEUISR_LO));
+			in_be32(priv->reg_afeu + TALITOS_EUISR),
+			in_be32(priv->reg_afeu + TALITOS_EUISR_LO));
 		break;
 	case DESC_HDR_SEL0_DEU:
 		dev_err(dev, "DEUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_DEUISR),
-			in_be32(priv->reg + TALITOS_DEUISR_LO));
+			in_be32(priv->reg_deu + TALITOS_EUISR),
+			in_be32(priv->reg_deu + TALITOS_EUISR_LO));
 		break;
 	case DESC_HDR_SEL0_MDEUA:
 	case DESC_HDR_SEL0_MDEUB:
 		dev_err(dev, "MDEUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_MDEUISR),
-			in_be32(priv->reg + TALITOS_MDEUISR_LO));
+			in_be32(priv->reg_mdeu + TALITOS_EUISR),
+			in_be32(priv->reg_mdeu + TALITOS_EUISR_LO));
 		break;
 	case DESC_HDR_SEL0_RNG:
 		dev_err(dev, "RNGUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_RNGUISR),
-			in_be32(priv->reg + TALITOS_RNGUISR_LO));
+			in_be32(priv->reg_rngu + TALITOS_ISR),
+			in_be32(priv->reg_rngu + TALITOS_ISR_LO));
 		break;
 	case DESC_HDR_SEL0_PKEU:
 		dev_err(dev, "PKEUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_PKEUISR),
-			in_be32(priv->reg + TALITOS_PKEUISR_LO));
+			in_be32(priv->reg_pkeu + TALITOS_EUISR),
+			in_be32(priv->reg_pkeu + TALITOS_EUISR_LO));
 		break;
 	case DESC_HDR_SEL0_AESU:
 		dev_err(dev, "AESUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_AESUISR),
-			in_be32(priv->reg + TALITOS_AESUISR_LO));
+			in_be32(priv->reg_aesu + TALITOS_EUISR),
+			in_be32(priv->reg_aesu + TALITOS_EUISR_LO));
 		break;
 	case DESC_HDR_SEL0_CRCU:
 		dev_err(dev, "CRCUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_CRCUISR),
-			in_be32(priv->reg + TALITOS_CRCUISR_LO));
+			in_be32(priv->reg_crcu + TALITOS_EUISR),
+			in_be32(priv->reg_crcu + TALITOS_EUISR_LO));
 		break;
 	case DESC_HDR_SEL0_KEU:
 		dev_err(dev, "KEUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_KEUISR),
-			in_be32(priv->reg + TALITOS_KEUISR_LO));
+			in_be32(priv->reg_pkeu + TALITOS_EUISR),
+			in_be32(priv->reg_pkeu + TALITOS_EUISR_LO));
 		break;
 	}
 
@@ -422,13 +519,13 @@ static void report_eu_error(struct device *dev, int ch, u32 desc_hdr)
 	case DESC_HDR_SEL1_MDEUA:
 	case DESC_HDR_SEL1_MDEUB:
 		dev_err(dev, "MDEUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_MDEUISR),
-			in_be32(priv->reg + TALITOS_MDEUISR_LO));
+			in_be32(priv->reg_mdeu + TALITOS_EUISR),
+			in_be32(priv->reg_mdeu + TALITOS_EUISR_LO));
 		break;
 	case DESC_HDR_SEL1_CRCU:
 		dev_err(dev, "CRCUISR 0x%08x_%08x\n",
-			in_be32(priv->reg + TALITOS_CRCUISR),
-			in_be32(priv->reg + TALITOS_CRCUISR_LO));
+			in_be32(priv->reg_crcu + TALITOS_EUISR),
+			in_be32(priv->reg_crcu + TALITOS_EUISR_LO));
 		break;
 	}
 
@@ -445,17 +542,24 @@ static void talitos_error(struct device *dev, u32 isr, u32 isr_lo)
 {
 	struct talitos_private *priv = dev_get_drvdata(dev);
 	unsigned int timeout = TALITOS_TIMEOUT;
-	int ch, error, reset_dev = 0, reset_ch = 0;
-	u32 v, v_lo;
+	int ch, error, reset_dev = 0;
+	u32 v_lo;
+	bool is_sec1 = has_ftr_sec1(priv);
+	int reset_ch = is_sec1 ? 1 : 0; /* only SEC2 supports continuation */
 
 	for (ch = 0; ch < priv->num_channels; ch++) {
 		/* skip channels without errors */
-		if (!(isr & (1 << (ch * 2 + 1))))
-			continue;
+		if (is_sec1) {
+			/* bits 29, 31, 17, 19 */
+			if (!(isr & (1 << (29 + (ch & 1) * 2 - (ch & 2) * 6))))
+				continue;
+		} else {
+			if (!(isr & (1 << (ch * 2 + 1))))
+				continue;
+		}
 
 		error = -EINVAL;
 
-		v = in_be32(priv->chan[ch].reg + TALITOS_CCPSR);
 		v_lo = in_be32(priv->chan[ch].reg + TALITOS_CCPSR_LO);
 
 		if (v_lo & TALITOS_CCPSR_LO_DOF) {
@@ -471,23 +575,28 @@ static void talitos_error(struct device *dev, u32 isr, u32 isr_lo)
 		if (v_lo & TALITOS_CCPSR_LO_MDTE)
 			dev_err(dev, "master data transfer error\n");
 		if (v_lo & TALITOS_CCPSR_LO_SGDLZ)
-			dev_err(dev, "s/g data length zero error\n");
+			dev_err(dev, is_sec1 ? "pointeur not complete error\n"
+					     : "s/g data length zero error\n");
 		if (v_lo & TALITOS_CCPSR_LO_FPZ)
-			dev_err(dev, "fetch pointer zero error\n");
+			dev_err(dev, is_sec1 ? "parity error\n"
+					     : "fetch pointer zero error\n");
 		if (v_lo & TALITOS_CCPSR_LO_IDH)
 			dev_err(dev, "illegal descriptor header error\n");
 		if (v_lo & TALITOS_CCPSR_LO_IEU)
-			dev_err(dev, "invalid execution unit error\n");
+			dev_err(dev, is_sec1 ? "static assignment error\n"
+					     : "invalid exec unit error\n");
 		if (v_lo & TALITOS_CCPSR_LO_EU)
 			report_eu_error(dev, ch, current_desc_hdr(dev, ch));
-		if (v_lo & TALITOS_CCPSR_LO_GB)
-			dev_err(dev, "gather boundary error\n");
-		if (v_lo & TALITOS_CCPSR_LO_GRL)
-			dev_err(dev, "gather return/length error\n");
-		if (v_lo & TALITOS_CCPSR_LO_SB)
-			dev_err(dev, "scatter boundary error\n");
-		if (v_lo & TALITOS_CCPSR_LO_SRL)
-			dev_err(dev, "scatter return/length error\n");
+		if (!is_sec1) {
+			if (v_lo & TALITOS_CCPSR_LO_GB)
+				dev_err(dev, "gather boundary error\n");
+			if (v_lo & TALITOS_CCPSR_LO_GRL)
+				dev_err(dev, "gather return/length error\n");
+			if (v_lo & TALITOS_CCPSR_LO_SB)
+				dev_err(dev, "scatter boundary error\n");
+			if (v_lo & TALITOS_CCPSR_LO_SRL)
+				dev_err(dev, "scatter return/length error\n");
+		}
 
 		flush_channel(dev, ch, error, reset_ch);
 
@@ -495,10 +604,10 @@ static void talitos_error(struct device *dev, u32 isr, u32 isr_lo)
 			reset_channel(dev, ch);
 		} else {
 			setbits32(priv->chan[ch].reg + TALITOS_CCCR,
-				  TALITOS_CCCR_CONT);
+				  TALITOS2_CCCR_CONT);
 			setbits32(priv->chan[ch].reg + TALITOS_CCCR_LO, 0);
 			while ((in_be32(priv->chan[ch].reg + TALITOS_CCCR) &
-			       TALITOS_CCCR_CONT) && --timeout)
+			       TALITOS2_CCCR_CONT) && --timeout)
 				cpu_relax();
 			if (timeout == 0) {
 				dev_err(dev, "failed to restart channel %d\n",
@@ -507,9 +616,14 @@ static void talitos_error(struct device *dev, u32 isr, u32 isr_lo)
 			}
 		}
 	}
-	if (reset_dev || isr & ~TALITOS_ISR_4CHERR || isr_lo) {
-		dev_err(dev, "done overflow, internal time out, or rngu error: "
-		        "ISR 0x%08x_%08x\n", isr, isr_lo);
+	if (reset_dev || (is_sec1 && isr & ~TALITOS1_ISR_4CHERR) ||
+	    (!is_sec1 && isr & ~TALITOS2_ISR_4CHERR) || isr_lo) {
+		if (is_sec1 && (isr_lo & TALITOS1_ISR_TEA_ERR))
+			dev_err(dev, "TEA error: ISR 0x%08x_%08x\n",
+				isr, isr_lo);
+		else
+			dev_err(dev, "done overflow, internal time out, or "
+				"rngu error: ISR 0x%08x_%08x\n", isr, isr_lo);
 
 		/* purge request queues */
 		for (ch = 0; ch < priv->num_channels; ch++)
@@ -520,8 +634,43 @@ static void talitos_error(struct device *dev, u32 isr, u32 isr_lo)
 	}
 }
 
-#define DEF_TALITOS_INTERRUPT(name, ch_done_mask, ch_err_mask, tlet)	       \
-static irqreturn_t talitos_interrupt_##name(int irq, void *data)	       \
+#define DEF_TALITOS1_INTERRUPT(name, ch_done_mask, ch_err_mask, tlet)	       \
+static irqreturn_t talitos1_interrupt_##name(int irq, void *data)	       \
+{									       \
+	struct device *dev = data;					       \
+	struct talitos_private *priv = dev_get_drvdata(dev);		       \
+	u32 isr, isr_lo;						       \
+	unsigned long flags;						       \
+									       \
+	spin_lock_irqsave(&priv->reg_lock, flags);			       \
+	isr = in_be32(priv->reg + TALITOS_ISR);				       \
+	isr_lo = in_be32(priv->reg + TALITOS_ISR_LO);			       \
+	/* Acknowledge interrupt */					       \
+	out_be32(priv->reg + TALITOS_ICR, isr & (ch_done_mask | ch_err_mask)); \
+	out_be32(priv->reg + TALITOS_ICR_LO, isr_lo);			       \
+									       \
+	if (unlikely(isr & ch_err_mask || isr_lo & TALITOS1_IMR_LO_INIT)) {    \
+		spin_unlock_irqrestore(&priv->reg_lock, flags);		       \
+		talitos_error(dev, isr & ch_err_mask, isr_lo);		       \
+	}								       \
+	else {								       \
+		if (likely(isr & ch_done_mask)) {			       \
+			/* mask further done interrupts. */		       \
+			setbits32(priv->reg + TALITOS_IMR, ch_done_mask);      \
+			/* done_task will unmask done interrupts at exit */    \
+			tasklet_schedule(&priv->done_task[tlet]);	       \
+		}							       \
+		spin_unlock_irqrestore(&priv->reg_lock, flags);		       \
+	}								       \
+									       \
+	return (isr & (ch_done_mask | ch_err_mask) || isr_lo) ? IRQ_HANDLED :  \
+								IRQ_NONE;      \
+}
+
+DEF_TALITOS1_INTERRUPT(4ch, TALITOS1_ISR_4CHDONE, TALITOS1_ISR_4CHERR, 0)
+
+#define DEF_TALITOS2_INTERRUPT(name, ch_done_mask, ch_err_mask, tlet)	       \
+static irqreturn_t talitos2_interrupt_##name(int irq, void *data)	       \
 {									       \
 	struct device *dev = data;					       \
 	struct talitos_private *priv = dev_get_drvdata(dev);		       \
@@ -552,9 +701,12 @@ static irqreturn_t talitos_interrupt_##name(int irq, void *data)	       \
 	return (isr & (ch_done_mask | ch_err_mask) || isr_lo) ? IRQ_HANDLED :  \
 								IRQ_NONE;      \
 }
-DEF_TALITOS_INTERRUPT(4ch, TALITOS_ISR_4CHDONE, TALITOS_ISR_4CHERR, 0)
-DEF_TALITOS_INTERRUPT(ch0_2, TALITOS_ISR_CH_0_2_DONE, TALITOS_ISR_CH_0_2_ERR, 0)
-DEF_TALITOS_INTERRUPT(ch1_3, TALITOS_ISR_CH_1_3_DONE, TALITOS_ISR_CH_1_3_ERR, 1)
+
+DEF_TALITOS2_INTERRUPT(4ch, TALITOS2_ISR_4CHDONE, TALITOS2_ISR_4CHERR, 0)
+DEF_TALITOS2_INTERRUPT(ch0_2, TALITOS2_ISR_CH_0_2_DONE, TALITOS2_ISR_CH_0_2_ERR,
+		       0)
+DEF_TALITOS2_INTERRUPT(ch1_3, TALITOS2_ISR_CH_1_3_DONE, TALITOS2_ISR_CH_1_3_ERR,
+		       1)
 
 /*
  * hwrng
@@ -567,7 +719,7 @@ static int talitos_rng_data_present(struct hwrng *rng, int wait)
 	int i;
 
 	for (i = 0; i < 20; i++) {
-		ofl = in_be32(priv->reg + TALITOS_RNGUSR_LO) &
+		ofl = in_be32(priv->reg_rngu + TALITOS_EUSR_LO) &
 		      TALITOS_RNGUSR_LO_OFL;
 		if (ofl || !wait)
 			break;
@@ -583,8 +735,8 @@ static int talitos_rng_data_read(struct hwrng *rng, u32 *data)
 	struct talitos_private *priv = dev_get_drvdata(dev);
 
 	/* rng fifo requires 64-bit accesses */
-	*data = in_be32(priv->reg + TALITOS_RNGU_FIFO);
-	*data = in_be32(priv->reg + TALITOS_RNGU_FIFO_LO);
+	*data = in_be32(priv->reg_rngu + TALITOS_EU_FIFO);
+	*data = in_be32(priv->reg_rngu + TALITOS_EU_FIFO_LO);
 
 	return sizeof(u32);
 }
@@ -595,8 +747,9 @@ static int talitos_rng_init(struct hwrng *rng)
 	struct talitos_private *priv = dev_get_drvdata(dev);
 	unsigned int timeout = TALITOS_TIMEOUT;
 
-	setbits32(priv->reg + TALITOS_RNGURCR_LO, TALITOS_RNGURCR_LO_SR);
-	while (!(in_be32(priv->reg + TALITOS_RNGUSR_LO) & TALITOS_RNGUSR_LO_RD)
+	setbits32(priv->reg_rngu + TALITOS_EURCR_LO, TALITOS_RNGURCR_LO_SR);
+	while (!(in_be32(priv->reg_rngu + TALITOS_EUSR_LO)
+		 & TALITOS_RNGUSR_LO_RD)
 	       && --timeout)
 		cpu_relax();
 	if (timeout == 0) {
@@ -605,7 +758,7 @@ static int talitos_rng_init(struct hwrng *rng)
 	}
 
 	/* start generating */
-	setbits32(priv->reg + TALITOS_RNGUDSR_LO, 0);
+	setbits32(priv->reg_rngu + TALITOS_EUDSR_LO, 0);
 
 	return 0;
 }
@@ -661,7 +814,7 @@ struct talitos_ahash_req_ctx {
 	unsigned int first;
 	unsigned int last;
 	unsigned int to_hash_later;
-	u64 nbuf;
+	unsigned int nbuf;
 	struct scatterlist bufsl[2];
 	struct scatterlist *psrc;
 };
@@ -712,9 +865,10 @@ badkey:
  * @dst_chained: whether dst is chained or not
  * @iv_dma: dma address of iv for checking continuity and link table
  * @dma_len: length of dma mapped link_tbl space
- * @dma_link_tbl: bus physical address of link_tbl
+ * @dma_link_tbl: bus physical address of link_tbl/buf
  * @desc: h/w descriptor
- * @link_tbl: input and output h/w link tables (if {src,dst}_nents > 1)
+ * @link_tbl: input and output h/w link tables (if {src,dst}_nents > 1) (SEC2)
+ * @buf: input and output buffeur (if {src,dst}_nents > 1) (SEC1)
  *
  * if decrypting (with authcheck), or either one of src_nents or dst_nents
  * is greater than 1, an integrity check value is concatenated to the end
@@ -731,7 +885,10 @@ struct talitos_edesc {
 	int dma_len;
 	dma_addr_t dma_link_tbl;
 	struct talitos_desc desc;
-	struct talitos_ptr link_tbl[0];
+	union {
+		struct talitos_ptr link_tbl[0];
+		u8 buf[0];
+	};
 };
 
 static int talitos_map_sg(struct device *dev, struct scatterlist *sg,
@@ -907,8 +1064,8 @@ static int sg_to_link_tbl(struct scatterlist *sg, int sg_count,
 {
 	int n_sg = sg_count;
 
-	while (n_sg--) {
-		to_talitos_ptr(link_tbl_ptr, sg_dma_address(sg));
+	while (sg && n_sg--) {
+		to_talitos_ptr(link_tbl_ptr, sg_dma_address(sg), 0);
 		link_tbl_ptr->len = cpu_to_be16(sg_dma_len(sg));
 		link_tbl_ptr->j_extent = 0;
 		link_tbl_ptr++;
@@ -925,7 +1082,8 @@ static int sg_to_link_tbl(struct scatterlist *sg, int sg_count,
 		sg_count--;
 		link_tbl_ptr--;
 	}
-	be16_add_cpu(&link_tbl_ptr->len, cryptlen);
+	link_tbl_ptr->len = cpu_to_be16(be16_to_cpu(link_tbl_ptr->len)
+					+ cryptlen);
 
 	/* tag end of link table */
 	link_tbl_ptr->j_extent = DESC_PTR_LNKTBL_RETURN;
@@ -953,7 +1111,7 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 
 	/* hmac key */
 	map_single_talitos_ptr(dev, &desc->ptr[0], ctx->authkeylen, &ctx->key,
-			       0, DMA_TO_DEVICE);
+			       DMA_TO_DEVICE);
 
 	/* hmac data */
 	desc->ptr[1].len = cpu_to_be16(areq->assoclen + ivsize);
@@ -962,7 +1120,7 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 		struct talitos_ptr *tbl_ptr = &edesc->link_tbl[tbl_off];
 
 		to_talitos_ptr(&desc->ptr[1], edesc->dma_link_tbl + tbl_off *
-			       sizeof(struct talitos_ptr));
+			       sizeof(struct talitos_ptr), 0);
 		desc->ptr[1].j_extent = DESC_PTR_LNKTBL_JUMP;
 
 		/* assoc_nents - 1 entries for assoc, 1 for IV */
@@ -973,7 +1131,7 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 		tbl_ptr += sg_count - 1;
 		tbl_ptr->j_extent = 0;
 		tbl_ptr++;
-		to_talitos_ptr(tbl_ptr, edesc->iv_dma);
+		to_talitos_ptr(tbl_ptr, edesc->iv_dma, 0);
 		tbl_ptr->len = cpu_to_be16(ivsize);
 		tbl_ptr->j_extent = DESC_PTR_LNKTBL_RETURN;
 
@@ -982,14 +1140,14 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 	} else {
 		if (areq->assoclen)
 			to_talitos_ptr(&desc->ptr[1],
-				       sg_dma_address(areq->assoc));
+				       sg_dma_address(areq->assoc), 0);
 		else
-			to_talitos_ptr(&desc->ptr[1], edesc->iv_dma);
+			to_talitos_ptr(&desc->ptr[1], edesc->iv_dma, 0);
 		desc->ptr[1].j_extent = 0;
 	}
 
 	/* cipher iv */
-	to_talitos_ptr(&desc->ptr[2], edesc->iv_dma);
+	to_talitos_ptr(&desc->ptr[2], edesc->iv_dma, 0);
 	desc->ptr[2].len = cpu_to_be16(ivsize);
 	desc->ptr[2].j_extent = 0;
 	/* Sync needed for the aead_givencrypt case */
@@ -997,7 +1155,7 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 
 	/* cipher key */
 	map_single_talitos_ptr(dev, &desc->ptr[3], ctx->enckeylen,
-			       (char *)&ctx->key + ctx->authkeylen, 0,
+			       (char *)&ctx->key + ctx->authkeylen,
 			       DMA_TO_DEVICE);
 
 	/*
@@ -1015,7 +1173,7 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 				  edesc->src_chained);
 
 	if (sg_count == 1) {
-		to_talitos_ptr(&desc->ptr[4], sg_dma_address(areq->src));
+		to_talitos_ptr(&desc->ptr[4], sg_dma_address(areq->src), 0);
 	} else {
 		sg_link_tbl_len = cryptlen;
 
@@ -1026,14 +1184,14 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 					  &edesc->link_tbl[0]);
 		if (sg_count > 1) {
 			desc->ptr[4].j_extent |= DESC_PTR_LNKTBL_JUMP;
-			to_talitos_ptr(&desc->ptr[4], edesc->dma_link_tbl);
+			to_talitos_ptr(&desc->ptr[4], edesc->dma_link_tbl, 0);
 			dma_sync_single_for_device(dev, edesc->dma_link_tbl,
 						   edesc->dma_len,
 						   DMA_BIDIRECTIONAL);
 		} else {
 			/* Only one segment now, so no link tbl needed */
 			to_talitos_ptr(&desc->ptr[4],
-				       sg_dma_address(areq->src));
+				       sg_dma_address(areq->src), 0);
 		}
 	}
 
@@ -1047,13 +1205,13 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 					  DMA_FROM_DEVICE, edesc->dst_chained);
 
 	if (sg_count == 1) {
-		to_talitos_ptr(&desc->ptr[5], sg_dma_address(areq->dst));
+		to_talitos_ptr(&desc->ptr[5], sg_dma_address(areq->dst), 0);
 	} else {
 		int tbl_off = edesc->src_nents + 1;
 		struct talitos_ptr *tbl_ptr = &edesc->link_tbl[tbl_off];
 
 		to_talitos_ptr(&desc->ptr[5], edesc->dma_link_tbl +
-			       tbl_off * sizeof(struct talitos_ptr));
+			       tbl_off * sizeof(struct talitos_ptr), 0);
 		sg_count = sg_to_link_tbl(areq->dst, sg_count, cryptlen,
 					  tbl_ptr);
 
@@ -1068,14 +1226,14 @@ static int ipsec_esp(struct talitos_edesc *edesc, struct aead_request *areq,
 		to_talitos_ptr(tbl_ptr, edesc->dma_link_tbl +
 			       (tbl_off + edesc->dst_nents + 1 +
 				edesc->assoc_nents) *
-			       sizeof(struct talitos_ptr));
+			       sizeof(struct talitos_ptr), 0);
 		desc->ptr[5].j_extent |= DESC_PTR_LNKTBL_JUMP;
 		dma_sync_single_for_device(ctx->dev, edesc->dma_link_tbl,
 					   edesc->dma_len, DMA_BIDIRECTIONAL);
 	}
 
 	/* iv out */
-	map_single_talitos_ptr(dev, &desc->ptr[6], ivsize, ctx->iv, 0,
+	map_single_talitos_ptr(dev, &desc->ptr[6], ivsize, ctx->iv,
 			       DMA_FROM_DEVICE);
 
 	ret = talitos_submit(dev, ctx->ch, desc, callback, areq);
@@ -1095,7 +1253,7 @@ static int sg_count(struct scatterlist *sg_list, int nbytes, bool *chained)
 	int sg_nents = 0;
 
 	*chained = false;
-	while (nbytes > 0) {
+	while (nbytes > 0 && sg) {
 		sg_nents++;
 		nbytes -= sg->length;
 		if (!sg_is_last(sg) && (sg + 1)->length == 0)
@@ -1128,8 +1286,11 @@ static struct talitos_edesc *talitos_edesc_alloc(struct device *dev,
 	dma_addr_t iv_dma = 0;
 	gfp_t flags = cryptoflags & CRYPTO_TFM_REQ_MAY_SLEEP ? GFP_KERNEL :
 		      GFP_ATOMIC;
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
+	int max_len = is_sec1 ? TALITOS1_MAX_DATA_LEN : TALITOS2_MAX_DATA_LEN;
 
-	if (cryptlen + authsize > TALITOS_MAX_DATA_LEN) {
+	if (cryptlen + authsize > max_len) {
 		dev_err(dev, "length exceeds h/w max limit\n");
 		return ERR_PTR(-EINVAL);
 	}
@@ -1173,8 +1334,12 @@ static struct talitos_edesc *talitos_edesc_alloc(struct device *dev,
 	 */
 	alloc_len = sizeof(struct talitos_edesc);
 	if (assoc_nents || src_nents || dst_nents) {
-		dma_len = (src_nents + dst_nents + 2 + assoc_nents) *
-			  sizeof(struct talitos_ptr) + authsize;
+		if (is_sec1)
+			dma_len = (src_nents ? cryptlen : 0) +
+				  (dst_nents ? cryptlen : 0);
+		else
+			dma_len = (src_nents + dst_nents + 2 + assoc_nents) *
+				  sizeof(struct talitos_ptr) + authsize;
 		alloc_len += dma_len;
 	} else {
 		dma_len = 0;
@@ -1327,16 +1492,43 @@ static int ablkcipher_setkey(struct crypto_ablkcipher *cipher,
 	return 0;
 }
 
+static void unmap_sg_talitos_ptr(struct device *dev, struct scatterlist *src,
+				 struct scatterlist *dst, unsigned int len,
+				 struct talitos_edesc *edesc)
+{
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
+
+	if (is_sec1) {
+		if (!edesc->src_nents) {
+			dma_unmap_sg(dev, src, 1,
+				     dst != src ? DMA_TO_DEVICE
+						: DMA_BIDIRECTIONAL);
+		}
+		if (dst && edesc->dst_nents) {
+			dma_sync_single_for_device(dev,
+						   edesc->dma_link_tbl + len,
+						   len, DMA_FROM_DEVICE);
+			sg_copy_from_buffer(dst, edesc->dst_nents ? : 1,
+					    edesc->buf + len, len);
+		} else if (dst && dst != src) {
+			dma_unmap_sg(dev, dst, 1, DMA_FROM_DEVICE);
+		}
+	} else {
+		talitos_sg_unmap(dev, edesc, src, dst);
+	}
+}
+
 static void common_nonsnoop_unmap(struct device *dev,
 				  struct talitos_edesc *edesc,
 				  struct ablkcipher_request *areq)
 {
 	unmap_single_talitos_ptr(dev, &edesc->desc.ptr[5], DMA_FROM_DEVICE);
+
+	unmap_sg_talitos_ptr(dev, areq->src, areq->dst, areq->nbytes, edesc);
 	unmap_single_talitos_ptr(dev, &edesc->desc.ptr[2], DMA_TO_DEVICE);
 	unmap_single_talitos_ptr(dev, &edesc->desc.ptr[1], DMA_TO_DEVICE);
 
-	talitos_sg_unmap(dev, edesc, areq->src, areq->dst);
-
 	if (edesc->dma_len)
 		dma_unmap_single(dev, edesc->dma_link_tbl, edesc->dma_len,
 				 DMA_BIDIRECTIONAL);
@@ -1358,6 +1550,102 @@ static void ablkcipher_done(struct device *dev,
 	areq->base.complete(&areq->base, err);
 }
 
+int map_sg_in_talitos_ptr(struct device *dev, struct scatterlist *src,
+			  unsigned int len, struct talitos_edesc *edesc,
+			  enum dma_data_direction dir, struct talitos_ptr *ptr)
+{
+	int sg_count;
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
+
+	to_talitos_ptr_len(ptr, len, is_sec1);
+
+	if (is_sec1) {
+		sg_count = edesc->src_nents ? : 1;
+
+		if (sg_count == 1) {
+			dma_map_sg(dev, src, 1, dir);
+			to_talitos_ptr(ptr, sg_dma_address(src), is_sec1);
+		} else {
+			sg_copy_to_buffer(src, sg_count, edesc->buf, len);
+			to_talitos_ptr(ptr, edesc->dma_link_tbl, is_sec1);
+			dma_sync_single_for_device(dev, edesc->dma_link_tbl,
+						   len, DMA_TO_DEVICE);
+		}
+	} else {
+		to_talitos_ptr_extent_clear(ptr, is_sec1);
+
+		sg_count = talitos_map_sg(dev, src, edesc->src_nents ? : 1, dir,
+					  edesc->src_chained);
+
+		if (sg_count == 1) {
+			to_talitos_ptr(ptr, sg_dma_address(src), is_sec1);
+		} else {
+			sg_count = sg_to_link_tbl(src, sg_count, len,
+						  &edesc->link_tbl[0]);
+			if (sg_count > 1) {
+				to_talitos_ptr(ptr, edesc->dma_link_tbl, 0);
+				ptr->j_extent |= DESC_PTR_LNKTBL_JUMP;
+				dma_sync_single_for_device(dev,
+							   edesc->dma_link_tbl,
+							   edesc->dma_len,
+							   DMA_BIDIRECTIONAL);
+			} else {
+				/* Only one segment now, so no link tbl needed*/
+				to_talitos_ptr(ptr, sg_dma_address(src),
+					       is_sec1);
+			}
+		}
+	}
+	return sg_count;
+}
+
+void map_sg_out_talitos_ptr(struct device *dev, struct scatterlist *dst,
+			    unsigned int len, struct talitos_edesc *edesc,
+			    enum dma_data_direction dir,
+			    struct talitos_ptr *ptr, int sg_count)
+{
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
+
+	if (dir != DMA_NONE)
+		sg_count = talitos_map_sg(dev, dst, edesc->dst_nents ? : 1,
+					  dir, edesc->dst_chained);
+
+	to_talitos_ptr_len(ptr, len, is_sec1);
+
+	if (is_sec1) {
+		if (sg_count == 1) {
+			if (dir != DMA_NONE)
+				dma_map_sg(dev, dst, 1, dir);
+			to_talitos_ptr(ptr, sg_dma_address(dst), is_sec1);
+		} else {
+			to_talitos_ptr(ptr, edesc->dma_link_tbl + len, is_sec1);
+			dma_sync_single_for_device(dev,
+						   edesc->dma_link_tbl + len,
+						   len, DMA_FROM_DEVICE);
+		}
+	} else {
+		to_talitos_ptr_extent_clear(ptr, is_sec1);
+
+		if (sg_count == 1) {
+			to_talitos_ptr(ptr, sg_dma_address(dst), is_sec1);
+		} else {
+			struct talitos_ptr *link_tbl_ptr =
+				&edesc->link_tbl[edesc->src_nents + 1];
+
+			to_talitos_ptr(ptr, edesc->dma_link_tbl +
+					    (edesc->src_nents + 1) *
+					     sizeof(struct talitos_ptr), 0);
+			ptr->j_extent |= DESC_PTR_LNKTBL_JUMP;
+			sg_to_link_tbl(dst, sg_count, len, link_tbl_ptr);
+			dma_sync_single_for_device(dev, edesc->dma_link_tbl,
+						   edesc->dma_len,
+						   DMA_BIDIRECTIONAL);
+		}
+	}
+}
+
 static int common_nonsnoop(struct talitos_edesc *edesc,
 			   struct ablkcipher_request *areq,
 			   void (*callback) (struct device *dev,
@@ -1371,83 +1659,41 @@ static int common_nonsnoop(struct talitos_edesc *edesc,
 	unsigned int cryptlen = areq->nbytes;
 	unsigned int ivsize = crypto_ablkcipher_ivsize(cipher);
 	int sg_count, ret;
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
 
 	/* first DWORD empty */
-	desc->ptr[0].len = 0;
-	to_talitos_ptr(&desc->ptr[0], 0);
-	desc->ptr[0].j_extent = 0;
+	desc->ptr[0] = zero_entry;
 
 	/* cipher iv */
-	to_talitos_ptr(&desc->ptr[1], edesc->iv_dma);
-	desc->ptr[1].len = cpu_to_be16(ivsize);
-	desc->ptr[1].j_extent = 0;
+	to_talitos_ptr(&desc->ptr[1], edesc->iv_dma, is_sec1);
+	to_talitos_ptr_len(&desc->ptr[1], ivsize, is_sec1);
+	to_talitos_ptr_extent_clear(&desc->ptr[1], is_sec1);
 
 	/* cipher key */
 	map_single_talitos_ptr(dev, &desc->ptr[2], ctx->keylen,
-			       (char *)&ctx->key, 0, DMA_TO_DEVICE);
+			       (char *)&ctx->key, DMA_TO_DEVICE);
 
 	/*
 	 * cipher in
 	 */
-	desc->ptr[3].len = cpu_to_be16(cryptlen);
-	desc->ptr[3].j_extent = 0;
-
-	sg_count = talitos_map_sg(dev, areq->src, edesc->src_nents ? : 1,
-				  (areq->src == areq->dst) ? DMA_BIDIRECTIONAL
-							   : DMA_TO_DEVICE,
-				  edesc->src_chained);
-
-	if (sg_count == 1) {
-		to_talitos_ptr(&desc->ptr[3], sg_dma_address(areq->src));
-	} else {
-		sg_count = sg_to_link_tbl(areq->src, sg_count, cryptlen,
-					  &edesc->link_tbl[0]);
-		if (sg_count > 1) {
-			to_talitos_ptr(&desc->ptr[3], edesc->dma_link_tbl);
-			desc->ptr[3].j_extent |= DESC_PTR_LNKTBL_JUMP;
-			dma_sync_single_for_device(dev, edesc->dma_link_tbl,
-						   edesc->dma_len,
-						   DMA_BIDIRECTIONAL);
-		} else {
-			/* Only one segment now, so no link tbl needed */
-			to_talitos_ptr(&desc->ptr[3],
-				       sg_dma_address(areq->src));
-		}
-	}
+	sg_count = map_sg_in_talitos_ptr(dev, areq->src, cryptlen, edesc,
+					 (areq->src == areq->dst) ?
+					  DMA_BIDIRECTIONAL : DMA_TO_DEVICE,
+					  &desc->ptr[3]);
 
 	/* cipher out */
-	desc->ptr[4].len = cpu_to_be16(cryptlen);
-	desc->ptr[4].j_extent = 0;
-
-	if (areq->src != areq->dst)
-		sg_count = talitos_map_sg(dev, areq->dst,
-					  edesc->dst_nents ? : 1,
-					  DMA_FROM_DEVICE, edesc->dst_chained);
-
-	if (sg_count == 1) {
-		to_talitos_ptr(&desc->ptr[4], sg_dma_address(areq->dst));
-	} else {
-		struct talitos_ptr *link_tbl_ptr =
-			&edesc->link_tbl[edesc->src_nents + 1];
-
-		to_talitos_ptr(&desc->ptr[4], edesc->dma_link_tbl +
-					      (edesc->src_nents + 1) *
-					      sizeof(struct talitos_ptr));
-		desc->ptr[4].j_extent |= DESC_PTR_LNKTBL_JUMP;
-		sg_count = sg_to_link_tbl(areq->dst, sg_count, cryptlen,
-					  link_tbl_ptr);
-		dma_sync_single_for_device(ctx->dev, edesc->dma_link_tbl,
-					   edesc->dma_len, DMA_BIDIRECTIONAL);
-	}
+	map_sg_out_talitos_ptr(dev, areq->dst, cryptlen, edesc,
+			       (areq->src == areq->dst) ? DMA_NONE
+							: DMA_FROM_DEVICE,
+			       &desc->ptr[4], sg_count);
 
 	/* iv out */
-	map_single_talitos_ptr(dev, &desc->ptr[5], ivsize, ctx->iv, 0,
+	map_single_talitos_ptr(dev, &desc->ptr[5], ivsize, ctx->iv,
 			       DMA_FROM_DEVICE);
 
 	/* last DWORD empty */
-	desc->ptr[6].len = 0;
-	to_talitos_ptr(&desc->ptr[6], 0);
-	desc->ptr[6].j_extent = 0;
+	desc->ptr[6] = zero_entry;
 
 	ret = talitos_submit(dev, ctx->ch, desc, callback, areq);
 	if (ret != -EINPROGRESS) {
@@ -1507,20 +1753,22 @@ static void common_nonsnoop_hash_unmap(struct device *dev,
 				       struct ahash_request *areq)
 {
 	struct talitos_ahash_req_ctx *req_ctx = ahash_request_ctx(areq);
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
 
 	unmap_single_talitos_ptr(dev, &edesc->desc.ptr[5], DMA_FROM_DEVICE);
 
+	unmap_sg_talitos_ptr(dev, req_ctx->psrc, NULL, 0, edesc);
+
 	/* When using hashctx-in, must unmap it. */
-	if (edesc->desc.ptr[1].len)
+	if (from_talitos_ptr_len(&edesc->desc.ptr[1], is_sec1))
 		unmap_single_talitos_ptr(dev, &edesc->desc.ptr[1],
 					 DMA_TO_DEVICE);
 
-	if (edesc->desc.ptr[2].len)
+	if (from_talitos_ptr_len(&edesc->desc.ptr[2], is_sec1))
 		unmap_single_talitos_ptr(dev, &edesc->desc.ptr[2],
 					 DMA_TO_DEVICE);
 
-	talitos_sg_unmap(dev, edesc, req_ctx->psrc, NULL);
-
 	if (edesc->dma_len)
 		dma_unmap_single(dev, edesc->dma_link_tbl, edesc->dma_len,
 				 DMA_BIDIRECTIONAL);
@@ -1548,6 +1796,27 @@ static void ahash_done(struct device *dev,
 	areq->base.complete(&areq->base, err);
 }
 
+/*
+ * SEC1 doesn't like hashing of 0 sized message, so we do the padding
+ * ourself and submit a padded block
+ */
+void talitos_handle_buggy_hash(struct talitos_ctx *ctx,
+			       struct talitos_edesc *edesc,
+			       struct talitos_ptr *ptr)
+{
+	static u8 padded_hash[64] = {
+		0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	};
+
+	pr_err_once("Bug in SEC1, padding ourself\n");
+	edesc->desc.hdr &= ~DESC_HDR_MODE0_MDEU_PAD;
+	map_single_talitos_ptr(ctx->dev, ptr, sizeof(padded_hash),
+			       (char *)padded_hash, DMA_TO_DEVICE);
+}
+
 static int common_nonsnoop_hash(struct talitos_edesc *edesc,
 				struct ahash_request *areq, unsigned int length,
 				void (*callback) (struct device *dev,
@@ -1559,7 +1828,9 @@ static int common_nonsnoop_hash(struct talitos_edesc *edesc,
 	struct talitos_ahash_req_ctx *req_ctx = ahash_request_ctx(areq);
 	struct device *dev = ctx->dev;
 	struct talitos_desc *desc = &edesc->desc;
-	int sg_count, ret;
+	int ret;
+	struct talitos_private *priv = dev_get_drvdata(dev);
+	bool is_sec1 = has_ftr_sec1(priv);
 
 	/* first DWORD empty */
 	desc->ptr[0] = zero_entry;
@@ -1568,7 +1839,7 @@ static int common_nonsnoop_hash(struct talitos_edesc *edesc,
 	if (!req_ctx->first || req_ctx->swinit) {
 		map_single_talitos_ptr(dev, &desc->ptr[1],
 				       req_ctx->hw_context_size,
-				       (char *)req_ctx->hw_context, 0,
+				       (char *)req_ctx->hw_context,
 				       DMA_TO_DEVICE);
 		req_ctx->swinit = 0;
 	} else {
@@ -1580,38 +1851,15 @@ static int common_nonsnoop_hash(struct talitos_edesc *edesc,
 	/* HMAC key */
 	if (ctx->keylen)
 		map_single_talitos_ptr(dev, &desc->ptr[2], ctx->keylen,
-				       (char *)&ctx->key, 0, DMA_TO_DEVICE);
+				       (char *)&ctx->key, DMA_TO_DEVICE);
 	else
 		desc->ptr[2] = zero_entry;
 
 	/*
 	 * data in
 	 */
-	desc->ptr[3].len = cpu_to_be16(length);
-	desc->ptr[3].j_extent = 0;
-
-	sg_count = talitos_map_sg(dev, req_ctx->psrc,
-				  edesc->src_nents ? : 1,
-				  DMA_TO_DEVICE, edesc->src_chained);
-
-	if (sg_count == 1) {
-		to_talitos_ptr(&desc->ptr[3], sg_dma_address(req_ctx->psrc));
-	} else {
-		sg_count = sg_to_link_tbl(req_ctx->psrc, sg_count, length,
-					  &edesc->link_tbl[0]);
-		if (sg_count > 1) {
-			desc->ptr[3].j_extent |= DESC_PTR_LNKTBL_JUMP;
-			to_talitos_ptr(&desc->ptr[3], edesc->dma_link_tbl);
-			dma_sync_single_for_device(ctx->dev,
-						   edesc->dma_link_tbl,
-						   edesc->dma_len,
-						   DMA_BIDIRECTIONAL);
-		} else {
-			/* Only one segment now, so no link tbl needed */
-			to_talitos_ptr(&desc->ptr[3],
-				       sg_dma_address(req_ctx->psrc));
-		}
-	}
+	map_sg_in_talitos_ptr(dev, req_ctx->psrc, length, edesc,
+			      DMA_TO_DEVICE, &desc->ptr[3]);
 
 	/* fifth DWORD empty */
 	desc->ptr[4] = zero_entry;
@@ -1620,15 +1868,18 @@ static int common_nonsnoop_hash(struct talitos_edesc *edesc,
 	if (req_ctx->last)
 		map_single_talitos_ptr(dev, &desc->ptr[5],
 				       crypto_ahash_digestsize(tfm),
-				       areq->result, 0, DMA_FROM_DEVICE);
+				       areq->result, DMA_FROM_DEVICE);
 	else
 		map_single_talitos_ptr(dev, &desc->ptr[5],
 				       req_ctx->hw_context_size,
-				       req_ctx->hw_context, 0, DMA_FROM_DEVICE);
+				       req_ctx->hw_context, DMA_FROM_DEVICE);
 
 	/* last DWORD empty */
 	desc->ptr[6] = zero_entry;
 
+	if (is_sec1 && from_talitos_ptr_len(&desc->ptr[3], true) == 0)
+		talitos_handle_buggy_hash(ctx, edesc, &desc->ptr[3]);
+
 	ret = talitos_submit(dev, ctx->ch, desc, callback, areq);
 	if (ret != -EINPROGRESS) {
 		common_nonsnoop_hash_unmap(dev, edesc, areq);
@@ -2561,6 +2812,7 @@ static struct talitos_crypto_alg *talitos_alg_alloc(struct device *dev,
 		break;
 	default:
 		dev_err(dev, "unknown algorithm type %d\n", t_alg->algt.type);
+		kfree(t_alg);
 		return ERR_PTR(-EINVAL);
 	}
 
@@ -2581,29 +2833,35 @@ static int talitos_probe_irq(struct platform_device *ofdev)
 	struct device_node *np = ofdev->dev.of_node;
 	struct talitos_private *priv = dev_get_drvdata(dev);
 	int err;
+	bool is_sec1 = has_ftr_sec1(priv);
 
 	priv->irq[0] = irq_of_parse_and_map(np, 0);
 	if (!priv->irq[0]) {
 		dev_err(dev, "failed to map irq\n");
 		return -EINVAL;
 	}
+	if (is_sec1) {
+		err = request_irq(priv->irq[0], talitos1_interrupt_4ch, 0,
+				  dev_driver_string(dev), dev);
+		goto primary_out;
+	}
 
 	priv->irq[1] = irq_of_parse_and_map(np, 1);
 
 	/* get the primary irq line */
 	if (!priv->irq[1]) {
-		err = request_irq(priv->irq[0], talitos_interrupt_4ch, 0,
+		err = request_irq(priv->irq[0], talitos2_interrupt_4ch, 0,
 				  dev_driver_string(dev), dev);
 		goto primary_out;
 	}
 
-	err = request_irq(priv->irq[0], talitos_interrupt_ch0_2, 0,
+	err = request_irq(priv->irq[0], talitos2_interrupt_ch0_2, 0,
 			  dev_driver_string(dev), dev);
 	if (err)
 		goto primary_out;
 
 	/* get the secondary irq line */
-	err = request_irq(priv->irq[1], talitos_interrupt_ch1_3, 0,
+	err = request_irq(priv->irq[1], talitos2_interrupt_ch1_3, 0,
 			  dev_driver_string(dev), dev);
 	if (err) {
 		dev_err(dev, "failed to request secondary irq\n");
@@ -2630,6 +2888,7 @@ static int talitos_probe(struct platform_device *ofdev)
 	struct talitos_private *priv;
 	const unsigned int *prop;
 	int i, err;
+	int stride;
 
 	priv = kzalloc(sizeof(struct talitos_private), GFP_KERNEL);
 	if (!priv)
@@ -2643,20 +2902,6 @@ static int talitos_probe(struct platform_device *ofdev)
 
 	spin_lock_init(&priv->reg_lock);
 
-	err = talitos_probe_irq(ofdev);
-	if (err)
-		goto err_out;
-
-	if (!priv->irq[1]) {
-		tasklet_init(&priv->done_task[0], talitos_done_4ch,
-			     (unsigned long)dev);
-	} else {
-		tasklet_init(&priv->done_task[0], talitos_done_ch0_2,
-			     (unsigned long)dev);
-		tasklet_init(&priv->done_task[1], talitos_done_ch1_3,
-			     (unsigned long)dev);
-	}
-
 	priv->reg = of_iomap(np, 0);
 	if (!priv->reg) {
 		dev_err(dev, "failed to of_iomap\n");
@@ -2696,6 +2941,53 @@ static int talitos_probe(struct platform_device *ofdev)
 				  TALITOS_FTR_SHA224_HWINIT |
 				  TALITOS_FTR_HMAC_OK;
 
+	if (of_device_is_compatible(np, "fsl,sec1.0"))
+		priv->features |= TALITOS_FTR_SEC1;
+
+	if (of_device_is_compatible(np, "fsl,sec1.2")) {
+		priv->reg_deu = priv->reg + TALITOS12_DEU;
+		priv->reg_aesu = priv->reg + TALITOS12_AESU;
+		priv->reg_mdeu = priv->reg + TALITOS12_MDEU;
+		stride = TALITOS1_CH_STRIDE;
+	} else if (of_device_is_compatible(np, "fsl,sec1.0")) {
+		priv->reg_deu = priv->reg + TALITOS10_DEU;
+		priv->reg_aesu = priv->reg + TALITOS10_AESU;
+		priv->reg_mdeu = priv->reg + TALITOS10_MDEU;
+		priv->reg_afeu = priv->reg + TALITOS10_AFEU;
+		priv->reg_rngu = priv->reg + TALITOS10_RNGU;
+		priv->reg_pkeu = priv->reg + TALITOS10_PKEU;
+		stride = TALITOS1_CH_STRIDE;
+	} else {
+		priv->reg_deu = priv->reg + TALITOS2_DEU;
+		priv->reg_aesu = priv->reg + TALITOS2_AESU;
+		priv->reg_mdeu = priv->reg + TALITOS2_MDEU;
+		priv->reg_afeu = priv->reg + TALITOS2_AFEU;
+		priv->reg_rngu = priv->reg + TALITOS2_RNGU;
+		priv->reg_pkeu = priv->reg + TALITOS2_PKEU;
+		priv->reg_keu = priv->reg + TALITOS2_KEU;
+		priv->reg_crcu = priv->reg + TALITOS2_CRCU;
+		stride = TALITOS2_CH_STRIDE;
+	}
+
+	err = talitos_probe_irq(ofdev);
+	if (err)
+		goto err_out;
+
+	if (of_device_is_compatible(np, "fsl,sec1.0")) {
+		tasklet_init(&priv->done_task[0], talitos1_done_4ch,
+			     (unsigned long)dev);
+	} else {
+		if (!priv->irq[1]) {
+			tasklet_init(&priv->done_task[0], talitos2_done_4ch,
+				     (unsigned long)dev);
+		} else {
+			tasklet_init(&priv->done_task[0], talitos2_done_ch0_2,
+				     (unsigned long)dev);
+			tasklet_init(&priv->done_task[1], talitos2_done_ch1_3,
+				     (unsigned long)dev);
+		}
+	}
+
 	priv->chan = kzalloc(sizeof(struct talitos_channel) *
 			     priv->num_channels, GFP_KERNEL);
 	if (!priv->chan) {
@@ -2707,7 +2999,7 @@ static int talitos_probe(struct platform_device *ofdev)
 	priv->fifo_len = roundup_pow_of_two(priv->chfifo_len);
 
 	for (i = 0; i < priv->num_channels; i++) {
-		priv->chan[i].reg = priv->reg + TALITOS_CH_STRIDE * (i + 1);
+		priv->chan[i].reg = priv->reg + stride * (i + 1);
 		if (!priv->irq[1] || !(i & 1))
 			priv->chan[i].reg += TALITOS_CH_BASE_OFFSET;
 
@@ -2794,9 +3086,16 @@ err_out:
 }
 
 static const struct of_device_id talitos_match[] = {
+#ifdef CONFIG_CRYPTO_DEV_TALITOS1
+	{
+		.compatible = "fsl,sec1.0",
+	},
+#endif
+#ifdef CONFIG_CRYPTO_DEV_TALITOS2
 	{
 		.compatible = "fsl,sec2.0",
 	},
+#endif
 	{},
 };
 MODULE_DEVICE_TABLE(of, talitos_match);
diff --git a/drivers/crypto/talitos.h b/drivers/crypto/talitos.h
index 61a14054aa39..314daf55e7f7 100644
--- a/drivers/crypto/talitos.h
+++ b/drivers/crypto/talitos.h
@@ -29,7 +29,8 @@
  */
 
 #define TALITOS_TIMEOUT 100000
-#define TALITOS_MAX_DATA_LEN 65535
+#define TALITOS1_MAX_DATA_LEN 32768
+#define TALITOS2_MAX_DATA_LEN 65535
 
 #define DESC_TYPE(desc_hdr) ((be32_to_cpu(desc_hdr) >> 3) & 0x1f)
 #define PRIMARY_EU(desc_hdr) ((be32_to_cpu(desc_hdr) >> 28) & 0xf)
@@ -37,9 +38,17 @@
 
 /* descriptor pointer entry */
 struct talitos_ptr {
-	__be16 len;     /* length */
-	u8 j_extent;    /* jump to sg link table and/or extent */
-	u8 eptr;        /* extended address */
+	union {
+		struct {		/* SEC2 format */
+			__be16 len;     /* length */
+			u8 j_extent;    /* jump to sg link table and/or extent*/
+			u8 eptr;        /* extended address */
+		};
+		struct {			/* SEC1 format */
+			__be16 res;
+			__be16 len1;	/* length */
+		};
+	};
 	__be32 ptr;     /* address */
 };
 
@@ -53,10 +62,16 @@ static const struct talitos_ptr zero_entry = {
 /* descriptor */
 struct talitos_desc {
 	__be32 hdr;                     /* header high bits */
-	__be32 hdr_lo;                  /* header low bits */
+	union {
+		__be32 hdr_lo;		/* header low bits */
+		__be32 hdr1;		/* header for SEC1 */
+	};
 	struct talitos_ptr ptr[7];      /* ptr/len pair array */
+	__be32 next_desc;		/* next descriptor (SEC1) */
 };
 
+#define TALITOS_DESC_SIZE	(sizeof(struct talitos_desc) - sizeof(__be32))
+
 /**
  * talitos_request - descriptor submission request
  * @desc: descriptor pointer (kernel virtual)
@@ -97,6 +112,14 @@ struct talitos_private {
 	struct device *dev;
 	struct platform_device *ofdev;
 	void __iomem *reg;
+	void __iomem *reg_deu;
+	void __iomem *reg_aesu;
+	void __iomem *reg_mdeu;
+	void __iomem *reg_afeu;
+	void __iomem *reg_rngu;
+	void __iomem *reg_pkeu;
+	void __iomem *reg_keu;
+	void __iomem *reg_crcu;
 	int irq[2];
 
 	/* SEC global registers lock  */
@@ -144,49 +167,80 @@ extern int talitos_submit(struct device *dev, int ch, struct talitos_desc *desc,
 #define TALITOS_FTR_HW_AUTH_CHECK 0x00000002
 #define TALITOS_FTR_SHA224_HWINIT 0x00000004
 #define TALITOS_FTR_HMAC_OK 0x00000008
+#define TALITOS_FTR_SEC1 0x00000010
+
+/*
+ * If both CONFIG_CRYPTO_DEV_TALITOS1 and CONFIG_CRYPTO_DEV_TALITOS2 are
+ * defined, we check the features which are set according to the device tree.
+ * Otherwise, we answer true or false directly
+ */
+static inline bool has_ftr_sec1(struct talitos_private *priv)
+{
+#if defined(CONFIG_CRYPTO_DEV_TALITOS1) && defined(CONFIG_CRYPTO_DEV_TALITOS2)
+	return priv->features & TALITOS_FTR_SEC1 ? true : false;
+#elif defined(CONFIG_CRYPTO_DEV_TALITOS1)
+	return true;
+#else
+	return false;
+#endif
+}
 
 /*
  * TALITOS_xxx_LO addresses point to the low data bits (32-63) of the register
  */
 
+#define ISR1_FORMAT(x)			(((x) << 28) | ((x) << 16))
+#define ISR2_FORMAT(x)			(((x) << 4) | (x))
+
 /* global register offset addresses */
 #define TALITOS_MCR			0x1030  /* master control register */
 #define   TALITOS_MCR_RCA0		(1 << 15) /* remap channel 0 */
 #define   TALITOS_MCR_RCA1		(1 << 14) /* remap channel 1 */
 #define   TALITOS_MCR_RCA2		(1 << 13) /* remap channel 2 */
 #define   TALITOS_MCR_RCA3		(1 << 12) /* remap channel 3 */
-#define   TALITOS_MCR_SWR		0x1     /* s/w reset */
+#define   TALITOS1_MCR_SWR		0x1000000     /* s/w reset */
+#define   TALITOS2_MCR_SWR		0x1     /* s/w reset */
 #define TALITOS_MCR_LO			0x1034
 #define TALITOS_IMR			0x1008  /* interrupt mask register */
-#define   TALITOS_IMR_INIT		0x100ff /* enable channel IRQs */
-#define   TALITOS_IMR_DONE		0x00055 /* done IRQs */
+/* enable channel IRQs */
+#define   TALITOS1_IMR_INIT		ISR1_FORMAT(0xf)
+#define   TALITOS1_IMR_DONE		ISR1_FORMAT(0x5) /* done IRQs */
+/* enable channel IRQs */
+#define   TALITOS2_IMR_INIT		(ISR2_FORMAT(0xf) | 0x10000)
+#define   TALITOS2_IMR_DONE		ISR1_FORMAT(0x5) /* done IRQs */
 #define TALITOS_IMR_LO			0x100C
-#define   TALITOS_IMR_LO_INIT		0x20000 /* allow RNGU error IRQs */
+#define   TALITOS1_IMR_LO_INIT		0x2000000 /* allow RNGU error IRQs */
+#define   TALITOS2_IMR_LO_INIT		0x20000 /* allow RNGU error IRQs */
 #define TALITOS_ISR			0x1010  /* interrupt status register */
-#define   TALITOS_ISR_4CHERR		0xaa    /* 4 channel errors mask */
-#define   TALITOS_ISR_4CHDONE		0x55    /* 4 channel done mask */
-#define   TALITOS_ISR_CH_0_2_ERR	0x22    /* channels 0, 2 errors mask */
-#define   TALITOS_ISR_CH_0_2_DONE	0x11    /* channels 0, 2 done mask */
-#define   TALITOS_ISR_CH_1_3_ERR	0x88    /* channels 1, 3 errors mask */
-#define   TALITOS_ISR_CH_1_3_DONE	0x44    /* channels 1, 3 done mask */
+#define   TALITOS1_ISR_4CHERR		ISR1_FORMAT(0xa) /* 4 ch errors mask */
+#define   TALITOS1_ISR_4CHDONE		ISR1_FORMAT(0x5) /* 4 ch done mask */
+#define   TALITOS1_ISR_TEA_ERR		0x00000040
+#define   TALITOS2_ISR_4CHERR		ISR2_FORMAT(0xa) /* 4 ch errors mask */
+#define   TALITOS2_ISR_4CHDONE		ISR2_FORMAT(0x5) /* 4 ch done mask */
+#define   TALITOS2_ISR_CH_0_2_ERR	ISR2_FORMAT(0x2) /* ch 0, 2 err mask */
+#define   TALITOS2_ISR_CH_0_2_DONE	ISR2_FORMAT(0x1) /* ch 0, 2 done mask */
+#define   TALITOS2_ISR_CH_1_3_ERR	ISR2_FORMAT(0x8) /* ch 1, 3 err mask */
+#define   TALITOS2_ISR_CH_1_3_DONE	ISR2_FORMAT(0x4) /* ch 1, 3 done mask */
 #define TALITOS_ISR_LO			0x1014
 #define TALITOS_ICR			0x1018  /* interrupt clear register */
 #define TALITOS_ICR_LO			0x101C
 
 /* channel register address stride */
 #define TALITOS_CH_BASE_OFFSET		0x1000	/* default channel map base */
-#define TALITOS_CH_STRIDE		0x100
+#define TALITOS1_CH_STRIDE		0x1000
+#define TALITOS2_CH_STRIDE		0x100
 
 /* channel configuration register  */
 #define TALITOS_CCCR			0x8
-#define   TALITOS_CCCR_CONT		0x2    /* channel continue */
-#define   TALITOS_CCCR_RESET		0x1    /* channel reset */
+#define   TALITOS2_CCCR_CONT		0x2    /* channel continue on SEC2 */
+#define   TALITOS2_CCCR_RESET		0x1    /* channel reset on SEC2 */
 #define TALITOS_CCCR_LO			0xc
 #define   TALITOS_CCCR_LO_IWSE		0x80   /* chan. ICCR writeback enab. */
 #define   TALITOS_CCCR_LO_EAE		0x20   /* extended address enable */
 #define   TALITOS_CCCR_LO_CDWE		0x10   /* chan. done writeback enab. */
 #define   TALITOS_CCCR_LO_NT		0x4    /* notification type */
 #define   TALITOS_CCCR_LO_CDIE		0x2    /* channel done IRQ enable */
+#define   TALITOS1_CCCR_LO_RESET	0x1    /* channel reset on SEC1 */
 
 /* CCPSR: channel pointer status register */
 #define TALITOS_CCPSR			0x10
@@ -224,37 +278,48 @@ extern int talitos_submit(struct device *dev, int ch, struct talitos_desc *desc,
 #define TALITOS_SCATTER			0xe0
 #define TALITOS_SCATTER_LO		0xe4
 
+/* execution unit registers base */
+#define TALITOS2_DEU			0x2000
+#define TALITOS2_AESU			0x4000
+#define TALITOS2_MDEU			0x6000
+#define TALITOS2_AFEU			0x8000
+#define TALITOS2_RNGU			0xa000
+#define TALITOS2_PKEU			0xc000
+#define TALITOS2_KEU			0xe000
+#define TALITOS2_CRCU			0xf000
+
+#define TALITOS12_AESU			0x4000
+#define TALITOS12_DEU			0x5000
+#define TALITOS12_MDEU			0x6000
+
+#define TALITOS10_AFEU			0x8000
+#define TALITOS10_DEU			0xa000
+#define TALITOS10_MDEU			0xc000
+#define TALITOS10_RNGU			0xe000
+#define TALITOS10_PKEU			0x10000
+#define TALITOS10_AESU			0x12000
+
 /* execution unit interrupt status registers */
-#define TALITOS_DEUISR			0x2030 /* DES unit */
-#define TALITOS_DEUISR_LO		0x2034
-#define TALITOS_AESUISR			0x4030 /* AES unit */
-#define TALITOS_AESUISR_LO		0x4034
-#define TALITOS_MDEUISR			0x6030 /* message digest unit */
-#define TALITOS_MDEUISR_LO		0x6034
-#define TALITOS_MDEUICR			0x6038 /* interrupt control */
-#define TALITOS_MDEUICR_LO		0x603c
+#define TALITOS_EUDSR			0x10	/* data size */
+#define TALITOS_EUDSR_LO		0x14
+#define TALITOS_EURCR			0x18 /* reset control*/
+#define TALITOS_EURCR_LO		0x1c
+#define TALITOS_EUSR			0x28 /* rng status */
+#define TALITOS_EUSR_LO			0x2c
+#define TALITOS_EUISR			0x30
+#define TALITOS_EUISR_LO		0x34
+#define TALITOS_EUICR			0x38 /* int. control */
+#define TALITOS_EUICR_LO		0x3c
+#define TALITOS_EU_FIFO			0x800 /* output FIFO */
+#define TALITOS_EU_FIFO_LO		0x804 /* output FIFO */
+/* DES unit */
+#define   TALITOS1_DEUICR_KPE		0x00200000 /* Key Parity Error */
+/* message digest unit */
 #define   TALITOS_MDEUICR_LO_ICE	0x4000 /* integrity check IRQ enable */
-#define TALITOS_AFEUISR			0x8030 /* arc4 unit */
-#define TALITOS_AFEUISR_LO		0x8034
-#define TALITOS_RNGUISR			0xa030 /* random number unit */
-#define TALITOS_RNGUISR_LO		0xa034
-#define TALITOS_RNGUSR			0xa028 /* rng status */
-#define TALITOS_RNGUSR_LO		0xa02c
+/* random number unit */
 #define   TALITOS_RNGUSR_LO_RD		0x1	/* reset done */
 #define   TALITOS_RNGUSR_LO_OFL		0xff0000/* output FIFO length */
-#define TALITOS_RNGUDSR			0xa010	/* data size */
-#define TALITOS_RNGUDSR_LO		0xa014
-#define TALITOS_RNGU_FIFO		0xa800	/* output FIFO */
-#define TALITOS_RNGU_FIFO_LO		0xa804	/* output FIFO */
-#define TALITOS_RNGURCR			0xa018	/* reset control */
-#define TALITOS_RNGURCR_LO		0xa01c
 #define   TALITOS_RNGURCR_LO_SR		0x1	/* software reset */
-#define TALITOS_PKEUISR			0xc030 /* public key unit */
-#define TALITOS_PKEUISR_LO		0xc034
-#define TALITOS_KEUISR			0xe030 /* kasumi unit */
-#define TALITOS_KEUISR_LO		0xe034
-#define TALITOS_CRCUISR			0xf030 /* cyclic redundancy check unit*/
-#define TALITOS_CRCUISR_LO		0xf034
 
 #define TALITOS_MDEU_CONTEXT_SIZE_MD5_SHA1_SHA256	0x28
 #define TALITOS_MDEU_CONTEXT_SIZE_SHA384_SHA512		0x48
diff --git a/drivers/crypto/ux500/Kconfig b/drivers/crypto/ux500/Kconfig
index b35e5c4b025a..30796441b0a6 100644
--- a/drivers/crypto/ux500/Kconfig
+++ b/drivers/crypto/ux500/Kconfig
@@ -7,6 +7,8 @@
 config CRYPTO_DEV_UX500_CRYP
 	tristate "UX500 crypto driver for CRYP block"
 	depends on CRYPTO_DEV_UX500
+	select CRYPTO_ALGAPI
+	select CRYPTO_BLKCIPHER
 	select CRYPTO_DES
 	help
         This selects the crypto driver for the UX500_CRYP hardware. It supports
@@ -16,7 +18,6 @@ config CRYPTO_DEV_UX500_HASH
         tristate "UX500 crypto driver for HASH block"
         depends on CRYPTO_DEV_UX500
         select CRYPTO_HASH
-        select CRYPTO_HMAC
         help
           This selects the hash driver for the UX500_HASH hardware.
           Depends on UX500/STM DMA if running in DMA mode.
@@ -24,7 +25,6 @@ config CRYPTO_DEV_UX500_HASH
 config CRYPTO_DEV_UX500_DEBUG
 	bool "Activate ux500 platform debug-mode for crypto and hash block"
 	depends on CRYPTO_DEV_UX500_CRYP || CRYPTO_DEV_UX500_HASH
-	default n
 	help
 	  Say Y if you want to add debug prints to ux500_hash and
 	  ux500_cryp devices.
diff --git a/drivers/crypto/vmx/Kconfig b/drivers/crypto/vmx/Kconfig
index 771babf16aa0..89d8208d9851 100644
--- a/drivers/crypto/vmx/Kconfig
+++ b/drivers/crypto/vmx/Kconfig
@@ -1,6 +1,6 @@
 config CRYPTO_DEV_VMX_ENCRYPT
 	tristate "Encryption acceleration support on P8 CPU"
-	depends on PPC64 && CRYPTO_DEV_VMX
+	depends on CRYPTO_DEV_VMX
 	default y
 	help
 	  Support for VMX cryptographic acceleration instructions on Power8 CPU.
diff --git a/drivers/crypto/vmx/Makefile b/drivers/crypto/vmx/Makefile
index c699c6e6c82e..d28ab96a2475 100644
--- a/drivers/crypto/vmx/Makefile
+++ b/drivers/crypto/vmx/Makefile
@@ -4,7 +4,7 @@ vmx-crypto-objs := vmx.o aesp8-ppc.o ghashp8-ppc.o aes.o aes_cbc.o aes_ctr.o gha
 ifeq ($(CONFIG_CPU_LITTLE_ENDIAN),y)
 TARGET := linux-ppc64le
 else
-TARGET := linux-pcc64
+TARGET := linux-ppc64
 endif
 
 quiet_cmd_perl = PERL $@
diff --git a/drivers/crypto/vmx/aes.c b/drivers/crypto/vmx/aes.c
index ab300ea19434..e79e567e43aa 100644
--- a/drivers/crypto/vmx/aes.c
+++ b/drivers/crypto/vmx/aes.c
@@ -30,110 +30,118 @@
 #include "aesp8-ppc.h"
 
 struct p8_aes_ctx {
-    struct crypto_cipher *fallback;
-    struct aes_key enc_key;
-    struct aes_key dec_key;
+	struct crypto_cipher *fallback;
+	struct aes_key enc_key;
+	struct aes_key dec_key;
 };
 
 static int p8_aes_init(struct crypto_tfm *tfm)
 {
-    const char *alg;
-    struct crypto_cipher *fallback;
-    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
-
-    if (!(alg = crypto_tfm_alg_name(tfm))) {
-        printk(KERN_ERR "Failed to get algorithm name.\n");
-        return -ENOENT;
-    }
-
-    fallback = crypto_alloc_cipher(alg, 0 ,CRYPTO_ALG_NEED_FALLBACK);
-    if (IS_ERR(fallback)) {
-        printk(KERN_ERR "Failed to allocate transformation for '%s': %ld\n",
-                alg, PTR_ERR(fallback));
-        return PTR_ERR(fallback);
-    }
-    printk(KERN_INFO "Using '%s' as fallback implementation.\n",
-            crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
-
-    crypto_cipher_set_flags(fallback,
-            crypto_cipher_get_flags((struct crypto_cipher *) tfm));
-    ctx->fallback = fallback;
-
-    return 0;
+	const char *alg;
+	struct crypto_cipher *fallback;
+	struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (!(alg = crypto_tfm_alg_name(tfm))) {
+		printk(KERN_ERR "Failed to get algorithm name.\n");
+		return -ENOENT;
+	}
+
+	fallback = crypto_alloc_cipher(alg, 0, CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(fallback)) {
+		printk(KERN_ERR
+		       "Failed to allocate transformation for '%s': %ld\n",
+		       alg, PTR_ERR(fallback));
+		return PTR_ERR(fallback);
+	}
+	printk(KERN_INFO "Using '%s' as fallback implementation.\n",
+	       crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
+
+	crypto_cipher_set_flags(fallback,
+				crypto_cipher_get_flags((struct
+							 crypto_cipher *)
+							tfm));
+	ctx->fallback = fallback;
+
+	return 0;
 }
 
 static void p8_aes_exit(struct crypto_tfm *tfm)
 {
-    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
 
-    if (ctx->fallback) {
-        crypto_free_cipher(ctx->fallback);
-        ctx->fallback = NULL;
-    }
+	if (ctx->fallback) {
+		crypto_free_cipher(ctx->fallback);
+		ctx->fallback = NULL;
+	}
 }
 
 static int p8_aes_setkey(struct crypto_tfm *tfm, const u8 *key,
-    unsigned int keylen)
+			 unsigned int keylen)
 {
-    int ret;
-    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
-
-    pagefault_disable();
-    enable_kernel_altivec();
-    ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
-    ret += aes_p8_set_decrypt_key(key, keylen * 8, &ctx->dec_key);
-    pagefault_enable();
-    
-    ret += crypto_cipher_setkey(ctx->fallback, key, keylen);
-    return ret;
+	int ret;
+	struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	preempt_disable();
+	pagefault_disable();
+	enable_kernel_altivec();
+	ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
+	ret += aes_p8_set_decrypt_key(key, keylen * 8, &ctx->dec_key);
+	pagefault_enable();
+	preempt_enable();
+
+	ret += crypto_cipher_setkey(ctx->fallback, key, keylen);
+	return ret;
 }
 
 static void p8_aes_encrypt(struct crypto_tfm *tfm, u8 *dst, const u8 *src)
 {
-    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
-
-    if (in_interrupt()) {
-        crypto_cipher_encrypt_one(ctx->fallback, dst, src);
-    } else {
-        pagefault_disable();
-        enable_kernel_altivec();
-        aes_p8_encrypt(src, dst, &ctx->enc_key);
-        pagefault_enable();
-    }
+	struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (in_interrupt()) {
+		crypto_cipher_encrypt_one(ctx->fallback, dst, src);
+	} else {
+		preempt_disable();
+		pagefault_disable();
+		enable_kernel_altivec();
+		aes_p8_encrypt(src, dst, &ctx->enc_key);
+		pagefault_enable();
+		preempt_enable();
+	}
 }
 
 static void p8_aes_decrypt(struct crypto_tfm *tfm, u8 *dst, const u8 *src)
 {
-    struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
-
-    if (in_interrupt()) {
-        crypto_cipher_decrypt_one(ctx->fallback, dst, src);
-    } else {
-        pagefault_disable();
-        enable_kernel_altivec();
-        aes_p8_decrypt(src, dst, &ctx->dec_key);
-        pagefault_enable();
-    }
+	struct p8_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (in_interrupt()) {
+		crypto_cipher_decrypt_one(ctx->fallback, dst, src);
+	} else {
+		preempt_disable();
+		pagefault_disable();
+		enable_kernel_altivec();
+		aes_p8_decrypt(src, dst, &ctx->dec_key);
+		pagefault_enable();
+		preempt_enable();
+	}
 }
 
 struct crypto_alg p8_aes_alg = {
-    .cra_name = "aes",
-    .cra_driver_name = "p8_aes",
-    .cra_module = THIS_MODULE,
-    .cra_priority = 1000,
-    .cra_type = NULL,
-    .cra_flags = CRYPTO_ALG_TYPE_CIPHER | CRYPTO_ALG_NEED_FALLBACK,
-    .cra_alignmask = 0,
-    .cra_blocksize = AES_BLOCK_SIZE,
-    .cra_ctxsize = sizeof(struct p8_aes_ctx),
-    .cra_init = p8_aes_init,
-    .cra_exit = p8_aes_exit,
-    .cra_cipher = {
-        .cia_min_keysize = AES_MIN_KEY_SIZE,
-        .cia_max_keysize = AES_MAX_KEY_SIZE,
-        .cia_setkey = p8_aes_setkey,
-        .cia_encrypt = p8_aes_encrypt,
-        .cia_decrypt = p8_aes_decrypt,
-    },
+	.cra_name = "aes",
+	.cra_driver_name = "p8_aes",
+	.cra_module = THIS_MODULE,
+	.cra_priority = 1000,
+	.cra_type = NULL,
+	.cra_flags = CRYPTO_ALG_TYPE_CIPHER | CRYPTO_ALG_NEED_FALLBACK,
+	.cra_alignmask = 0,
+	.cra_blocksize = AES_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct p8_aes_ctx),
+	.cra_init = p8_aes_init,
+	.cra_exit = p8_aes_exit,
+	.cra_cipher = {
+		       .cia_min_keysize = AES_MIN_KEY_SIZE,
+		       .cia_max_keysize = AES_MAX_KEY_SIZE,
+		       .cia_setkey = p8_aes_setkey,
+		       .cia_encrypt = p8_aes_encrypt,
+		       .cia_decrypt = p8_aes_decrypt,
+	},
 };
-
diff --git a/drivers/crypto/vmx/aes_cbc.c b/drivers/crypto/vmx/aes_cbc.c
index 1a559b7dddb5..7299995c78ec 100644
--- a/drivers/crypto/vmx/aes_cbc.c
+++ b/drivers/crypto/vmx/aes_cbc.c
@@ -31,154 +31,168 @@
 #include "aesp8-ppc.h"
 
 struct p8_aes_cbc_ctx {
-    struct crypto_blkcipher *fallback;
-    struct aes_key enc_key;
-    struct aes_key dec_key;
+	struct crypto_blkcipher *fallback;
+	struct aes_key enc_key;
+	struct aes_key dec_key;
 };
 
 static int p8_aes_cbc_init(struct crypto_tfm *tfm)
 {
-    const char *alg;
-    struct crypto_blkcipher *fallback;
-    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
-
-    if (!(alg = crypto_tfm_alg_name(tfm))) {
-        printk(KERN_ERR "Failed to get algorithm name.\n");
-        return -ENOENT;
-    }
-
-    fallback = crypto_alloc_blkcipher(alg, 0 ,CRYPTO_ALG_NEED_FALLBACK);
-    if (IS_ERR(fallback)) {
-        printk(KERN_ERR "Failed to allocate transformation for '%s': %ld\n",
-                alg, PTR_ERR(fallback));
-        return PTR_ERR(fallback);
-    }
-    printk(KERN_INFO "Using '%s' as fallback implementation.\n",
-            crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
-
-    crypto_blkcipher_set_flags(fallback,
-            crypto_blkcipher_get_flags((struct crypto_blkcipher *) tfm));
-    ctx->fallback = fallback;
-
-    return 0;
+	const char *alg;
+	struct crypto_blkcipher *fallback;
+	struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (!(alg = crypto_tfm_alg_name(tfm))) {
+		printk(KERN_ERR "Failed to get algorithm name.\n");
+		return -ENOENT;
+	}
+
+	fallback =
+	    crypto_alloc_blkcipher(alg, 0, CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(fallback)) {
+		printk(KERN_ERR
+		       "Failed to allocate transformation for '%s': %ld\n",
+		       alg, PTR_ERR(fallback));
+		return PTR_ERR(fallback);
+	}
+	printk(KERN_INFO "Using '%s' as fallback implementation.\n",
+	       crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
+
+	crypto_blkcipher_set_flags(
+		fallback,
+		crypto_blkcipher_get_flags((struct crypto_blkcipher *)tfm));
+	ctx->fallback = fallback;
+
+	return 0;
 }
 
 static void p8_aes_cbc_exit(struct crypto_tfm *tfm)
 {
-    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
 
-    if (ctx->fallback) {
-        crypto_free_blkcipher(ctx->fallback);
-        ctx->fallback = NULL;
-    }
+	if (ctx->fallback) {
+		crypto_free_blkcipher(ctx->fallback);
+		ctx->fallback = NULL;
+	}
 }
 
 static int p8_aes_cbc_setkey(struct crypto_tfm *tfm, const u8 *key,
-    unsigned int keylen)
+			     unsigned int keylen)
 {
-    int ret;
-    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
-
-    pagefault_disable();
-    enable_kernel_altivec();
-    ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
-    ret += aes_p8_set_decrypt_key(key, keylen * 8, &ctx->dec_key);
-    pagefault_enable();
-
-    ret += crypto_blkcipher_setkey(ctx->fallback, key, keylen);
-    return ret;
+	int ret;
+	struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	preempt_disable();
+	pagefault_disable();
+	enable_kernel_altivec();
+	ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
+	ret += aes_p8_set_decrypt_key(key, keylen * 8, &ctx->dec_key);
+	pagefault_enable();
+	preempt_enable();
+
+	ret += crypto_blkcipher_setkey(ctx->fallback, key, keylen);
+	return ret;
 }
 
 static int p8_aes_cbc_encrypt(struct blkcipher_desc *desc,
-    struct scatterlist *dst, struct scatterlist *src,
-    unsigned int nbytes)
+			      struct scatterlist *dst,
+			      struct scatterlist *src, unsigned int nbytes)
 {
-    int ret;
-    struct blkcipher_walk walk;
-    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(
-            crypto_blkcipher_tfm(desc->tfm));
-    struct blkcipher_desc fallback_desc = {
-        .tfm = ctx->fallback,
-        .info = desc->info,
-        .flags = desc->flags
-    };
-
-    if (in_interrupt()) {
-        ret = crypto_blkcipher_encrypt(&fallback_desc, dst, src, nbytes);
-    } else {
-        pagefault_disable();
-        enable_kernel_altivec();
-
-	blkcipher_walk_init(&walk, dst, src, nbytes);
-        ret = blkcipher_walk_virt(desc, &walk);
-        while ((nbytes = walk.nbytes)) {
-			aes_p8_cbc_encrypt(walk.src.virt.addr, walk.dst.virt.addr,
-				nbytes & AES_BLOCK_MASK, &ctx->enc_key, walk.iv, 1);
+	int ret;
+	struct blkcipher_walk walk;
+	struct p8_aes_cbc_ctx *ctx =
+		crypto_tfm_ctx(crypto_blkcipher_tfm(desc->tfm));
+	struct blkcipher_desc fallback_desc = {
+		.tfm = ctx->fallback,
+		.info = desc->info,
+		.flags = desc->flags
+	};
+
+	if (in_interrupt()) {
+		ret = crypto_blkcipher_encrypt(&fallback_desc, dst, src,
+					       nbytes);
+	} else {
+		preempt_disable();
+		pagefault_disable();
+		enable_kernel_altivec();
+
+		blkcipher_walk_init(&walk, dst, src, nbytes);
+		ret = blkcipher_walk_virt(desc, &walk);
+		while ((nbytes = walk.nbytes)) {
+			aes_p8_cbc_encrypt(walk.src.virt.addr,
+					   walk.dst.virt.addr,
+					   nbytes & AES_BLOCK_MASK,
+					   &ctx->enc_key, walk.iv, 1);
 			nbytes &= AES_BLOCK_SIZE - 1;
 			ret = blkcipher_walk_done(desc, &walk, nbytes);
-	}
+		}
 
-        pagefault_enable();
-    }
+		pagefault_enable();
+		preempt_enable();
+	}
 
-    return ret;
+	return ret;
 }
 
 static int p8_aes_cbc_decrypt(struct blkcipher_desc *desc,
-    struct scatterlist *dst, struct scatterlist *src,
-    unsigned int nbytes)
+			      struct scatterlist *dst,
+			      struct scatterlist *src, unsigned int nbytes)
 {
-    int ret;
-    struct blkcipher_walk walk;
-    struct p8_aes_cbc_ctx *ctx = crypto_tfm_ctx(
-            crypto_blkcipher_tfm(desc->tfm));
-    struct blkcipher_desc fallback_desc = {
-        .tfm = ctx->fallback,
-        .info = desc->info,
-        .flags = desc->flags
-    };
-
-    if (in_interrupt()) {
-        ret = crypto_blkcipher_decrypt(&fallback_desc, dst, src, nbytes);
-    } else {
-        pagefault_disable();
-        enable_kernel_altivec();
-
-	blkcipher_walk_init(&walk, dst, src, nbytes);
-        ret = blkcipher_walk_virt(desc, &walk);
-        while ((nbytes = walk.nbytes)) {
-			aes_p8_cbc_encrypt(walk.src.virt.addr, walk.dst.virt.addr,
-				nbytes & AES_BLOCK_MASK, &ctx->dec_key, walk.iv, 0);
+	int ret;
+	struct blkcipher_walk walk;
+	struct p8_aes_cbc_ctx *ctx =
+		crypto_tfm_ctx(crypto_blkcipher_tfm(desc->tfm));
+	struct blkcipher_desc fallback_desc = {
+		.tfm = ctx->fallback,
+		.info = desc->info,
+		.flags = desc->flags
+	};
+
+	if (in_interrupt()) {
+		ret = crypto_blkcipher_decrypt(&fallback_desc, dst, src,
+					       nbytes);
+	} else {
+		preempt_disable();
+		pagefault_disable();
+		enable_kernel_altivec();
+
+		blkcipher_walk_init(&walk, dst, src, nbytes);
+		ret = blkcipher_walk_virt(desc, &walk);
+		while ((nbytes = walk.nbytes)) {
+			aes_p8_cbc_encrypt(walk.src.virt.addr,
+					   walk.dst.virt.addr,
+					   nbytes & AES_BLOCK_MASK,
+					   &ctx->dec_key, walk.iv, 0);
 			nbytes &= AES_BLOCK_SIZE - 1;
 			ret = blkcipher_walk_done(desc, &walk, nbytes);
 		}
 
-        pagefault_enable();
-    }
+		pagefault_enable();
+		preempt_enable();
+	}
 
-    return ret;
+	return ret;
 }
 
 
 struct crypto_alg p8_aes_cbc_alg = {
-    .cra_name = "cbc(aes)",
-    .cra_driver_name = "p8_aes_cbc",
-    .cra_module = THIS_MODULE,
-    .cra_priority = 1000,
-    .cra_type = &crypto_blkcipher_type,
-    .cra_flags = CRYPTO_ALG_TYPE_BLKCIPHER | CRYPTO_ALG_NEED_FALLBACK,
-    .cra_alignmask = 0,
-    .cra_blocksize = AES_BLOCK_SIZE,
-    .cra_ctxsize = sizeof(struct p8_aes_cbc_ctx),
-    .cra_init = p8_aes_cbc_init,
-    .cra_exit = p8_aes_cbc_exit,
-    .cra_blkcipher = {
-        .ivsize = 0,
-        .min_keysize = AES_MIN_KEY_SIZE,
-        .max_keysize = AES_MAX_KEY_SIZE,
-        .setkey = p8_aes_cbc_setkey,
-        .encrypt = p8_aes_cbc_encrypt,
-        .decrypt = p8_aes_cbc_decrypt,
-    },
+	.cra_name = "cbc(aes)",
+	.cra_driver_name = "p8_aes_cbc",
+	.cra_module = THIS_MODULE,
+	.cra_priority = 1000,
+	.cra_type = &crypto_blkcipher_type,
+	.cra_flags = CRYPTO_ALG_TYPE_BLKCIPHER | CRYPTO_ALG_NEED_FALLBACK,
+	.cra_alignmask = 0,
+	.cra_blocksize = AES_BLOCK_SIZE,
+	.cra_ctxsize = sizeof(struct p8_aes_cbc_ctx),
+	.cra_init = p8_aes_cbc_init,
+	.cra_exit = p8_aes_cbc_exit,
+	.cra_blkcipher = {
+			  .ivsize = 0,
+			  .min_keysize = AES_MIN_KEY_SIZE,
+			  .max_keysize = AES_MAX_KEY_SIZE,
+			  .setkey = p8_aes_cbc_setkey,
+			  .encrypt = p8_aes_cbc_encrypt,
+			  .decrypt = p8_aes_cbc_decrypt,
+	},
 };
-
diff --git a/drivers/crypto/vmx/aes_ctr.c b/drivers/crypto/vmx/aes_ctr.c
index 96dbee4bf4a6..7adae42a7b79 100644
--- a/drivers/crypto/vmx/aes_ctr.c
+++ b/drivers/crypto/vmx/aes_ctr.c
@@ -30,138 +30,147 @@
 #include "aesp8-ppc.h"
 
 struct p8_aes_ctr_ctx {
-    struct crypto_blkcipher *fallback;
-    struct aes_key enc_key;
+	struct crypto_blkcipher *fallback;
+	struct aes_key enc_key;
 };
 
 static int p8_aes_ctr_init(struct crypto_tfm *tfm)
 {
-    const char *alg;
-    struct crypto_blkcipher *fallback;
-    struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
-
-    if (!(alg = crypto_tfm_alg_name(tfm))) {
-        printk(KERN_ERR "Failed to get algorithm name.\n");
-        return -ENOENT;
-    }
-
-    fallback = crypto_alloc_blkcipher(alg, 0 ,CRYPTO_ALG_NEED_FALLBACK);
-    if (IS_ERR(fallback)) {
-        printk(KERN_ERR "Failed to allocate transformation for '%s': %ld\n",
-                alg, PTR_ERR(fallback));
-        return PTR_ERR(fallback);
-    }
-    printk(KERN_INFO "Using '%s' as fallback implementation.\n",
-            crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
-
-    crypto_blkcipher_set_flags(fallback,
-            crypto_blkcipher_get_flags((struct crypto_blkcipher *) tfm));
-    ctx->fallback = fallback;
-
-    return 0;
+	const char *alg;
+	struct crypto_blkcipher *fallback;
+	struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (!(alg = crypto_tfm_alg_name(tfm))) {
+		printk(KERN_ERR "Failed to get algorithm name.\n");
+		return -ENOENT;
+	}
+
+	fallback =
+	    crypto_alloc_blkcipher(alg, 0, CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(fallback)) {
+		printk(KERN_ERR
+		       "Failed to allocate transformation for '%s': %ld\n",
+		       alg, PTR_ERR(fallback));
+		return PTR_ERR(fallback);
+	}
+	printk(KERN_INFO "Using '%s' as fallback implementation.\n",
+	       crypto_tfm_alg_driver_name((struct crypto_tfm *) fallback));
+
+	crypto_blkcipher_set_flags(
+		fallback,
+		crypto_blkcipher_get_flags((struct crypto_blkcipher *)tfm));
+	ctx->fallback = fallback;
+
+	return 0;
 }
 
 static void p8_aes_ctr_exit(struct crypto_tfm *tfm)
 {
-    struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
 
-    if (ctx->fallback) {
-        crypto_free_blkcipher(ctx->fallback);
-        ctx->fallback = NULL;
-    }
+	if (ctx->fallback) {
+		crypto_free_blkcipher(ctx->fallback);
+		ctx->fallback = NULL;
+	}
 }
 
 static int p8_aes_ctr_setkey(struct crypto_tfm *tfm, const u8 *key,
-    unsigned int keylen)
+			     unsigned int keylen)
 {
-    int ret;
-    struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
+	int ret;
+	struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
 
-    pagefault_disable();
-    enable_kernel_altivec();
-    ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
-    pagefault_enable();
+	pagefault_disable();
+	enable_kernel_altivec();
+	ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
+	pagefault_enable();
 
-    ret += crypto_blkcipher_setkey(ctx->fallback, key, keylen);
-    return ret;
+	ret += crypto_blkcipher_setkey(ctx->fallback, key, keylen);
+	return ret;
 }
 
 static void p8_aes_ctr_final(struct p8_aes_ctr_ctx *ctx,
-                struct blkcipher_walk *walk)
+			     struct blkcipher_walk *walk)
 {
-    u8 *ctrblk = walk->iv;
-    u8 keystream[AES_BLOCK_SIZE];
-    u8 *src = walk->src.virt.addr;
-    u8 *dst = walk->dst.virt.addr;
-    unsigned int nbytes = walk->nbytes;
-
-    pagefault_disable();
-    enable_kernel_altivec();
-    aes_p8_encrypt(ctrblk, keystream, &ctx->enc_key);
-    pagefault_enable();
-
-    crypto_xor(keystream, src, nbytes);
-    memcpy(dst, keystream, nbytes);
-    crypto_inc(ctrblk, AES_BLOCK_SIZE);
+	u8 *ctrblk = walk->iv;
+	u8 keystream[AES_BLOCK_SIZE];
+	u8 *src = walk->src.virt.addr;
+	u8 *dst = walk->dst.virt.addr;
+	unsigned int nbytes = walk->nbytes;
+
+	pagefault_disable();
+	enable_kernel_altivec();
+	aes_p8_encrypt(ctrblk, keystream, &ctx->enc_key);
+	pagefault_enable();
+
+	crypto_xor(keystream, src, nbytes);
+	memcpy(dst, keystream, nbytes);
+	crypto_inc(ctrblk, AES_BLOCK_SIZE);
 }
 
 static int p8_aes_ctr_crypt(struct blkcipher_desc *desc,
-    struct scatterlist *dst, struct scatterlist *src,
-    unsigned int nbytes)
+			    struct scatterlist *dst,
+			    struct scatterlist *src, unsigned int nbytes)
 {
-    int ret;
-    struct blkcipher_walk walk;
-    struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(
-            crypto_blkcipher_tfm(desc->tfm));
-    struct blkcipher_desc fallback_desc = {
-        .tfm = ctx->fallback,
-        .info = desc->info,
-        .flags = desc->flags
-    };
-
-    if (in_interrupt()) {
-        ret = crypto_blkcipher_encrypt(&fallback_desc, dst, src, nbytes);
-    } else {
-        blkcipher_walk_init(&walk, dst, src, nbytes);
-        ret = blkcipher_walk_virt_block(desc, &walk, AES_BLOCK_SIZE);
-        while ((nbytes = walk.nbytes) >= AES_BLOCK_SIZE) {
-            pagefault_disable();
-            enable_kernel_altivec();
-            aes_p8_ctr32_encrypt_blocks(walk.src.virt.addr, walk.dst.virt.addr,
-                (nbytes & AES_BLOCK_MASK)/AES_BLOCK_SIZE, &ctx->enc_key, walk.iv);
-            pagefault_enable();
-
-            crypto_inc(walk.iv, AES_BLOCK_SIZE);
-            nbytes &= AES_BLOCK_SIZE - 1;
-            ret = blkcipher_walk_done(desc, &walk, nbytes);
-        }
-        if (walk.nbytes) {
-            p8_aes_ctr_final(ctx, &walk);
-            ret = blkcipher_walk_done(desc, &walk, 0);
-        }
-    }
-
-    return ret;
+	int ret;
+	struct blkcipher_walk walk;
+	struct p8_aes_ctr_ctx *ctx =
+		crypto_tfm_ctx(crypto_blkcipher_tfm(desc->tfm));
+	struct blkcipher_desc fallback_desc = {
+		.tfm = ctx->fallback,
+		.info = desc->info,
+		.flags = desc->flags
+	};
+
+	if (in_interrupt()) {
+		ret = crypto_blkcipher_encrypt(&fallback_desc, dst, src,
+					       nbytes);
+	} else {
+		blkcipher_walk_init(&walk, dst, src, nbytes);
+		ret = blkcipher_walk_virt_block(desc, &walk, AES_BLOCK_SIZE);
+		while ((nbytes = walk.nbytes) >= AES_BLOCK_SIZE) {
+			pagefault_disable();
+			enable_kernel_altivec();
+			aes_p8_ctr32_encrypt_blocks(walk.src.virt.addr,
+						    walk.dst.virt.addr,
+						    (nbytes &
+						     AES_BLOCK_MASK) /
+						    AES_BLOCK_SIZE,
+						    &ctx->enc_key,
+						    walk.iv);
+			pagefault_enable();
+
+			crypto_inc(walk.iv, AES_BLOCK_SIZE);
+			nbytes &= AES_BLOCK_SIZE - 1;
+			ret = blkcipher_walk_done(desc, &walk, nbytes);
+		}
+		if (walk.nbytes) {
+			p8_aes_ctr_final(ctx, &walk);
+			ret = blkcipher_walk_done(desc, &walk, 0);
+		}
+	}
+
+	return ret;
 }
 
 struct crypto_alg p8_aes_ctr_alg = {
-    .cra_name = "ctr(aes)",
-    .cra_driver_name = "p8_aes_ctr",
-    .cra_module = THIS_MODULE,
-    .cra_priority = 1000,
-    .cra_type = &crypto_blkcipher_type,
-    .cra_flags = CRYPTO_ALG_TYPE_BLKCIPHER | CRYPTO_ALG_NEED_FALLBACK,
-    .cra_alignmask = 0,
-    .cra_blocksize = 1,
-    .cra_ctxsize = sizeof(struct p8_aes_ctr_ctx),
-    .cra_init = p8_aes_ctr_init,
-    .cra_exit = p8_aes_ctr_exit,
-    .cra_blkcipher = {
-        .ivsize = 0,
-        .min_keysize = AES_MIN_KEY_SIZE,
-        .max_keysize = AES_MAX_KEY_SIZE,
-        .setkey = p8_aes_ctr_setkey,
-        .encrypt = p8_aes_ctr_crypt,
-        .decrypt = p8_aes_ctr_crypt,
-    },
+	.cra_name = "ctr(aes)",
+	.cra_driver_name = "p8_aes_ctr",
+	.cra_module = THIS_MODULE,
+	.cra_priority = 1000,
+	.cra_type = &crypto_blkcipher_type,
+	.cra_flags = CRYPTO_ALG_TYPE_BLKCIPHER | CRYPTO_ALG_NEED_FALLBACK,
+	.cra_alignmask = 0,
+	.cra_blocksize = 1,
+	.cra_ctxsize = sizeof(struct p8_aes_ctr_ctx),
+	.cra_init = p8_aes_ctr_init,
+	.cra_exit = p8_aes_ctr_exit,
+	.cra_blkcipher = {
+			  .ivsize = 0,
+			  .min_keysize = AES_MIN_KEY_SIZE,
+			  .max_keysize = AES_MAX_KEY_SIZE,
+			  .setkey = p8_aes_ctr_setkey,
+			  .encrypt = p8_aes_ctr_crypt,
+			  .decrypt = p8_aes_ctr_crypt,
+	},
 };
diff --git a/drivers/crypto/vmx/aesp8-ppc.h b/drivers/crypto/vmx/aesp8-ppc.h
index e963945a83e1..4cd34ee54a94 100644
--- a/drivers/crypto/vmx/aesp8-ppc.h
+++ b/drivers/crypto/vmx/aesp8-ppc.h
@@ -4,17 +4,18 @@
 #define AES_BLOCK_MASK  (~(AES_BLOCK_SIZE-1))
 
 struct aes_key {
-    u8 key[AES_MAX_KEYLENGTH];
-    int rounds;
+	u8 key[AES_MAX_KEYLENGTH];
+	int rounds;
 };
 
 int aes_p8_set_encrypt_key(const u8 *userKey, const int bits,
-        struct aes_key *key);
+			   struct aes_key *key);
 int aes_p8_set_decrypt_key(const u8 *userKey, const int bits,
-        struct aes_key *key);
+			   struct aes_key *key);
 void aes_p8_encrypt(const u8 *in, u8 *out, const struct aes_key *key);
-void aes_p8_decrypt(const u8 *in, u8 *out,const struct aes_key *key);
+void aes_p8_decrypt(const u8 *in, u8 *out, const struct aes_key *key);
 void aes_p8_cbc_encrypt(const u8 *in, u8 *out, size_t len,
-		const struct aes_key *key, u8 *iv, const int enc);
+			const struct aes_key *key, u8 *iv, const int enc);
 void aes_p8_ctr32_encrypt_blocks(const u8 *in, u8 *out,
-        size_t len, const struct aes_key *key, const u8 *iv);
+				 size_t len, const struct aes_key *key,
+				 const u8 *iv);
diff --git a/drivers/crypto/vmx/ghash.c b/drivers/crypto/vmx/ghash.c
index d0ffe277af5c..b5e29002b666 100644
--- a/drivers/crypto/vmx/ghash.c
+++ b/drivers/crypto/vmx/ghash.c
@@ -39,176 +39,188 @@
 void gcm_init_p8(u128 htable[16], const u64 Xi[2]);
 void gcm_gmult_p8(u64 Xi[2], const u128 htable[16]);
 void gcm_ghash_p8(u64 Xi[2], const u128 htable[16],
-        const u8 *in,size_t len);
+		  const u8 *in, size_t len);
 
 struct p8_ghash_ctx {
-    u128 htable[16];
-    struct crypto_shash *fallback;
+	u128 htable[16];
+	struct crypto_shash *fallback;
 };
 
 struct p8_ghash_desc_ctx {
-    u64 shash[2];
-    u8 buffer[GHASH_DIGEST_SIZE];
-    int bytes;
-    struct shash_desc fallback_desc;
+	u64 shash[2];
+	u8 buffer[GHASH_DIGEST_SIZE];
+	int bytes;
+	struct shash_desc fallback_desc;
 };
 
 static int p8_ghash_init_tfm(struct crypto_tfm *tfm)
 {
-    const char *alg;
-    struct crypto_shash *fallback;
-    struct crypto_shash *shash_tfm = __crypto_shash_cast(tfm);
-    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(tfm);
-
-    if (!(alg = crypto_tfm_alg_name(tfm))) {
-        printk(KERN_ERR "Failed to get algorithm name.\n");
-        return -ENOENT;
-    }
-
-    fallback = crypto_alloc_shash(alg, 0 ,CRYPTO_ALG_NEED_FALLBACK);
-    if (IS_ERR(fallback)) {
-        printk(KERN_ERR "Failed to allocate transformation for '%s': %ld\n",
-                alg, PTR_ERR(fallback));
-        return PTR_ERR(fallback);
-    }
-    printk(KERN_INFO "Using '%s' as fallback implementation.\n",
-            crypto_tfm_alg_driver_name(crypto_shash_tfm(fallback)));
-
-    crypto_shash_set_flags(fallback,
-            crypto_shash_get_flags((struct crypto_shash *) tfm));
-    ctx->fallback = fallback;
-
-    shash_tfm->descsize = sizeof(struct p8_ghash_desc_ctx)
-        + crypto_shash_descsize(fallback);
-
-    return 0;
+	const char *alg;
+	struct crypto_shash *fallback;
+	struct crypto_shash *shash_tfm = __crypto_shash_cast(tfm);
+	struct p8_ghash_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (!(alg = crypto_tfm_alg_name(tfm))) {
+		printk(KERN_ERR "Failed to get algorithm name.\n");
+		return -ENOENT;
+	}
+
+	fallback = crypto_alloc_shash(alg, 0, CRYPTO_ALG_NEED_FALLBACK);
+	if (IS_ERR(fallback)) {
+		printk(KERN_ERR
+		       "Failed to allocate transformation for '%s': %ld\n",
+		       alg, PTR_ERR(fallback));
+		return PTR_ERR(fallback);
+	}
+	printk(KERN_INFO "Using '%s' as fallback implementation.\n",
+	       crypto_tfm_alg_driver_name(crypto_shash_tfm(fallback)));
+
+	crypto_shash_set_flags(fallback,
+			       crypto_shash_get_flags((struct crypto_shash
+						       *) tfm));
+	ctx->fallback = fallback;
+
+	shash_tfm->descsize = sizeof(struct p8_ghash_desc_ctx)
+	    + crypto_shash_descsize(fallback);
+
+	return 0;
 }
 
 static void p8_ghash_exit_tfm(struct crypto_tfm *tfm)
 {
-    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct p8_ghash_ctx *ctx = crypto_tfm_ctx(tfm);
 
-    if (ctx->fallback) {
-        crypto_free_shash(ctx->fallback);
-        ctx->fallback = NULL;
-    }
+	if (ctx->fallback) {
+		crypto_free_shash(ctx->fallback);
+		ctx->fallback = NULL;
+	}
 }
 
 static int p8_ghash_init(struct shash_desc *desc)
 {
-    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
-    struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
-
-    dctx->bytes = 0;
-    memset(dctx->shash, 0, GHASH_DIGEST_SIZE);
-    dctx->fallback_desc.tfm = ctx->fallback;
-    dctx->fallback_desc.flags = desc->flags;
-    return crypto_shash_init(&dctx->fallback_desc);
+	struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
+	struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
+
+	dctx->bytes = 0;
+	memset(dctx->shash, 0, GHASH_DIGEST_SIZE);
+	dctx->fallback_desc.tfm = ctx->fallback;
+	dctx->fallback_desc.flags = desc->flags;
+	return crypto_shash_init(&dctx->fallback_desc);
 }
 
 static int p8_ghash_setkey(struct crypto_shash *tfm, const u8 *key,
-    unsigned int keylen)
+			   unsigned int keylen)
 {
-    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(tfm));
-
-    if (keylen != GHASH_KEY_LEN)
-        return -EINVAL;
-
-    pagefault_disable();
-    enable_kernel_altivec();
-    enable_kernel_fp();
-    gcm_init_p8(ctx->htable, (const u64 *) key);
-    pagefault_enable();
-    return crypto_shash_setkey(ctx->fallback, key, keylen);
+	struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(tfm));
+
+	if (keylen != GHASH_KEY_LEN)
+		return -EINVAL;
+
+	preempt_disable();
+	pagefault_disable();
+	enable_kernel_altivec();
+	enable_kernel_fp();
+	gcm_init_p8(ctx->htable, (const u64 *) key);
+	pagefault_enable();
+	preempt_enable();
+	return crypto_shash_setkey(ctx->fallback, key, keylen);
 }
 
 static int p8_ghash_update(struct shash_desc *desc,
-        const u8 *src, unsigned int srclen)
+			   const u8 *src, unsigned int srclen)
 {
-    unsigned int len;
-    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
-    struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
-
-    if (IN_INTERRUPT) {
-        return crypto_shash_update(&dctx->fallback_desc, src, srclen);
-    } else {
-        if (dctx->bytes) {
-            if (dctx->bytes + srclen < GHASH_DIGEST_SIZE) {
-                memcpy(dctx->buffer + dctx->bytes, src, srclen);
-                dctx->bytes += srclen;
-                return 0;
-            }
-            memcpy(dctx->buffer + dctx->bytes, src,
-                    GHASH_DIGEST_SIZE - dctx->bytes);
-            pagefault_disable();
-            enable_kernel_altivec();
-            enable_kernel_fp();
-            gcm_ghash_p8(dctx->shash, ctx->htable, dctx->buffer,
-                    GHASH_DIGEST_SIZE);
-            pagefault_enable();
-            src += GHASH_DIGEST_SIZE - dctx->bytes;
-            srclen -= GHASH_DIGEST_SIZE - dctx->bytes;
-            dctx->bytes = 0;
-        }
-        len = srclen & ~(GHASH_DIGEST_SIZE - 1);
-        if (len) {
-            pagefault_disable();
-            enable_kernel_altivec();
-            enable_kernel_fp();
-            gcm_ghash_p8(dctx->shash, ctx->htable, src, len);
-            pagefault_enable();
-            src += len;
-            srclen -= len;
-        }
-        if (srclen) {
-            memcpy(dctx->buffer, src, srclen);
-            dctx->bytes = srclen;
-        }
-        return 0;
-    }
+	unsigned int len;
+	struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
+	struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
+
+	if (IN_INTERRUPT) {
+		return crypto_shash_update(&dctx->fallback_desc, src,
+					   srclen);
+	} else {
+		if (dctx->bytes) {
+			if (dctx->bytes + srclen < GHASH_DIGEST_SIZE) {
+				memcpy(dctx->buffer + dctx->bytes, src,
+				       srclen);
+				dctx->bytes += srclen;
+				return 0;
+			}
+			memcpy(dctx->buffer + dctx->bytes, src,
+			       GHASH_DIGEST_SIZE - dctx->bytes);
+			preempt_disable();
+			pagefault_disable();
+			enable_kernel_altivec();
+			enable_kernel_fp();
+			gcm_ghash_p8(dctx->shash, ctx->htable,
+				     dctx->buffer, GHASH_DIGEST_SIZE);
+			pagefault_enable();
+			preempt_enable();
+			src += GHASH_DIGEST_SIZE - dctx->bytes;
+			srclen -= GHASH_DIGEST_SIZE - dctx->bytes;
+			dctx->bytes = 0;
+		}
+		len = srclen & ~(GHASH_DIGEST_SIZE - 1);
+		if (len) {
+			preempt_disable();
+			pagefault_disable();
+			enable_kernel_altivec();
+			enable_kernel_fp();
+			gcm_ghash_p8(dctx->shash, ctx->htable, src, len);
+			pagefault_enable();
+			preempt_enable();
+			src += len;
+			srclen -= len;
+		}
+		if (srclen) {
+			memcpy(dctx->buffer, src, srclen);
+			dctx->bytes = srclen;
+		}
+		return 0;
+	}
 }
 
 static int p8_ghash_final(struct shash_desc *desc, u8 *out)
 {
-    int i;
-    struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
-    struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
-
-    if (IN_INTERRUPT) {
-        return crypto_shash_final(&dctx->fallback_desc, out);
-    } else {
-        if (dctx->bytes) {
-            for (i = dctx->bytes; i < GHASH_DIGEST_SIZE; i++)
-                dctx->buffer[i] = 0;
-            pagefault_disable();
-            enable_kernel_altivec();
-            enable_kernel_fp();
-            gcm_ghash_p8(dctx->shash, ctx->htable, dctx->buffer,
-                    GHASH_DIGEST_SIZE);
-            pagefault_enable();
-            dctx->bytes = 0;
-        }
-        memcpy(out, dctx->shash, GHASH_DIGEST_SIZE);
-        return 0;
-    }
+	int i;
+	struct p8_ghash_ctx *ctx = crypto_tfm_ctx(crypto_shash_tfm(desc->tfm));
+	struct p8_ghash_desc_ctx *dctx = shash_desc_ctx(desc);
+
+	if (IN_INTERRUPT) {
+		return crypto_shash_final(&dctx->fallback_desc, out);
+	} else {
+		if (dctx->bytes) {
+			for (i = dctx->bytes; i < GHASH_DIGEST_SIZE; i++)
+				dctx->buffer[i] = 0;
+			preempt_disable();
+			pagefault_disable();
+			enable_kernel_altivec();
+			enable_kernel_fp();
+			gcm_ghash_p8(dctx->shash, ctx->htable,
+				     dctx->buffer, GHASH_DIGEST_SIZE);
+			pagefault_enable();
+			preempt_enable();
+			dctx->bytes = 0;
+		}
+		memcpy(out, dctx->shash, GHASH_DIGEST_SIZE);
+		return 0;
+	}
 }
 
 struct shash_alg p8_ghash_alg = {
-    .digestsize = GHASH_DIGEST_SIZE,
-    .init       = p8_ghash_init,
-    .update     = p8_ghash_update,
-    .final      = p8_ghash_final,
-    .setkey     = p8_ghash_setkey,
-    .descsize   = sizeof(struct p8_ghash_desc_ctx),
-    .base       = {
-        .cra_name = "ghash",
-        .cra_driver_name = "p8_ghash",
-        .cra_priority = 1000,
-        .cra_flags = CRYPTO_ALG_TYPE_SHASH | CRYPTO_ALG_NEED_FALLBACK,
-        .cra_blocksize = GHASH_BLOCK_SIZE,
-        .cra_ctxsize = sizeof(struct p8_ghash_ctx),
-        .cra_module = THIS_MODULE,
-        .cra_init = p8_ghash_init_tfm,
-        .cra_exit = p8_ghash_exit_tfm,
-    },
+	.digestsize = GHASH_DIGEST_SIZE,
+	.init = p8_ghash_init,
+	.update = p8_ghash_update,
+	.final = p8_ghash_final,
+	.setkey = p8_ghash_setkey,
+	.descsize = sizeof(struct p8_ghash_desc_ctx),
+	.base = {
+		 .cra_name = "ghash",
+		 .cra_driver_name = "p8_ghash",
+		 .cra_priority = 1000,
+		 .cra_flags = CRYPTO_ALG_TYPE_SHASH | CRYPTO_ALG_NEED_FALLBACK,
+		 .cra_blocksize = GHASH_BLOCK_SIZE,
+		 .cra_ctxsize = sizeof(struct p8_ghash_ctx),
+		 .cra_module = THIS_MODULE,
+		 .cra_init = p8_ghash_init_tfm,
+		 .cra_exit = p8_ghash_exit_tfm,
+	},
 };
diff --git a/drivers/crypto/vmx/vmx.c b/drivers/crypto/vmx/vmx.c
index 44d8d5cfe40d..e163d5770438 100644
--- a/drivers/crypto/vmx/vmx.c
+++ b/drivers/crypto/vmx/vmx.c
@@ -32,57 +32,57 @@ extern struct crypto_alg p8_aes_alg;
 extern struct crypto_alg p8_aes_cbc_alg;
 extern struct crypto_alg p8_aes_ctr_alg;
 static struct crypto_alg *algs[] = {
-    &p8_aes_alg,
-    &p8_aes_cbc_alg,
-    &p8_aes_ctr_alg,
-    NULL,
+	&p8_aes_alg,
+	&p8_aes_cbc_alg,
+	&p8_aes_ctr_alg,
+	NULL,
 };
 
 int __init p8_init(void)
 {
-    int ret = 0;
-    struct crypto_alg **alg_it;
+	int ret = 0;
+	struct crypto_alg **alg_it;
 
-    if (!(cur_cpu_spec->cpu_user_features2 & PPC_FEATURE2_VEC_CRYPTO))
-        return -ENODEV;
+	if (!(cur_cpu_spec->cpu_user_features2 & PPC_FEATURE2_VEC_CRYPTO))
+		return -ENODEV;
 
-    for (alg_it = algs; *alg_it; alg_it++) {
-        ret = crypto_register_alg(*alg_it);
-        printk(KERN_INFO "crypto_register_alg '%s' = %d\n",
-                (*alg_it)->cra_name, ret);
-        if (ret) {
-            for (alg_it--; alg_it >= algs; alg_it--)
-                crypto_unregister_alg(*alg_it);
-            break;
-        }
-    }
-    if (ret)
-        return ret;
+	for (alg_it = algs; *alg_it; alg_it++) {
+		ret = crypto_register_alg(*alg_it);
+		printk(KERN_INFO "crypto_register_alg '%s' = %d\n",
+		       (*alg_it)->cra_name, ret);
+		if (ret) {
+			for (alg_it--; alg_it >= algs; alg_it--)
+				crypto_unregister_alg(*alg_it);
+			break;
+		}
+	}
+	if (ret)
+		return ret;
 
-    ret = crypto_register_shash(&p8_ghash_alg);
-    if (ret) {
-        for (alg_it = algs; *alg_it; alg_it++)
-            crypto_unregister_alg(*alg_it);
-    }
-    return ret;
+	ret = crypto_register_shash(&p8_ghash_alg);
+	if (ret) {
+		for (alg_it = algs; *alg_it; alg_it++)
+			crypto_unregister_alg(*alg_it);
+	}
+	return ret;
 }
 
 void __exit p8_exit(void)
 {
-    struct crypto_alg **alg_it;
+	struct crypto_alg **alg_it;
 
-    for (alg_it = algs; *alg_it; alg_it++) {
-        printk(KERN_INFO "Removing '%s'\n", (*alg_it)->cra_name);
-        crypto_unregister_alg(*alg_it);
-    }
-    crypto_unregister_shash(&p8_ghash_alg);
+	for (alg_it = algs; *alg_it; alg_it++) {
+		printk(KERN_INFO "Removing '%s'\n", (*alg_it)->cra_name);
+		crypto_unregister_alg(*alg_it);
+	}
+	crypto_unregister_shash(&p8_ghash_alg);
 }
 
 module_init(p8_init);
 module_exit(p8_exit);
 
 MODULE_AUTHOR("Marcelo Cerri<mhcerri@br.ibm.com>");
-MODULE_DESCRIPTION("IBM VMX cryptogaphic acceleration instructions support on Power 8");
+MODULE_DESCRIPTION("IBM VMX cryptographic acceleration instructions "
+		   "support on Power 8");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("1.0.0");
-
diff --git a/drivers/dma-buf/dma-buf.c b/drivers/dma-buf/dma-buf.c
index c5a9138a6a8d..155c1464948e 100644
--- a/drivers/dma-buf/dma-buf.c
+++ b/drivers/dma-buf/dma-buf.c
@@ -29,6 +29,7 @@
 #include <linux/anon_inodes.h>
 #include <linux/export.h>
 #include <linux/debugfs.h>
+#include <linux/module.h>
 #include <linux/seq_file.h>
 #include <linux/poll.h>
 #include <linux/reservation.h>
@@ -72,6 +73,7 @@ static int dma_buf_release(struct inode *inode, struct file *file)
 	if (dmabuf->resv == (struct reservation_object *)&dmabuf[1])
 		reservation_object_fini(dmabuf->resv);
 
+	module_put(dmabuf->owner);
 	kfree(dmabuf);
 	return 0;
 }
@@ -285,6 +287,7 @@ struct dma_buf *dma_buf_export(const struct dma_buf_export_info *exp_info)
 	struct reservation_object *resv = exp_info->resv;
 	struct file *file;
 	size_t alloc_size = sizeof(struct dma_buf);
+
 	if (!exp_info->resv)
 		alloc_size += sizeof(struct reservation_object);
 	else
@@ -302,14 +305,20 @@ struct dma_buf *dma_buf_export(const struct dma_buf_export_info *exp_info)
 		return ERR_PTR(-EINVAL);
 	}
 
+	if (!try_module_get(exp_info->owner))
+		return ERR_PTR(-ENOENT);
+
 	dmabuf = kzalloc(alloc_size, GFP_KERNEL);
-	if (dmabuf == NULL)
+	if (!dmabuf) {
+		module_put(exp_info->owner);
 		return ERR_PTR(-ENOMEM);
+	}
 
 	dmabuf->priv = exp_info->priv;
 	dmabuf->ops = exp_info->ops;
 	dmabuf->size = exp_info->size;
 	dmabuf->exp_name = exp_info->exp_name;
+	dmabuf->owner = exp_info->owner;
 	init_waitqueue_head(&dmabuf->poll);
 	dmabuf->cb_excl.poll = dmabuf->cb_shared.poll = &dmabuf->poll;
 	dmabuf->cb_excl.active = dmabuf->cb_shared.active = 0;
@@ -545,7 +554,8 @@ int dma_buf_begin_cpu_access(struct dma_buf *dmabuf, size_t start, size_t len,
 		return -EINVAL;
 
 	if (dmabuf->ops->begin_cpu_access)
-		ret = dmabuf->ops->begin_cpu_access(dmabuf, start, len, direction);
+		ret = dmabuf->ops->begin_cpu_access(dmabuf, start,
+							len, direction);
 
 	return ret;
 }
@@ -649,7 +659,7 @@ EXPORT_SYMBOL_GPL(dma_buf_kunmap);
  * @dmabuf:	[in]	buffer that should back the vma
  * @vma:	[in]	vma for the mmap
  * @pgoff:	[in]	offset in pages where this mmap should start within the
- * 			dma-buf buffer.
+ *			dma-buf buffer.
  *
  * This function adjusts the passed in vma so that it points at the file of the
  * dma_buf operation. It also adjusts the starting pgoff and does bounds
@@ -826,6 +836,7 @@ static int dma_buf_describe(struct seq_file *s)
 static int dma_buf_show(struct seq_file *s, void *unused)
 {
 	void (*func)(struct seq_file *) = s->private;
+
 	func(s);
 	return 0;
 }
@@ -847,7 +858,9 @@ static struct dentry *dma_buf_debugfs_dir;
 static int dma_buf_init_debugfs(void)
 {
 	int err = 0;
+
 	dma_buf_debugfs_dir = debugfs_create_dir("dma_buf", NULL);
+
 	if (IS_ERR(dma_buf_debugfs_dir)) {
 		err = PTR_ERR(dma_buf_debugfs_dir);
 		dma_buf_debugfs_dir = NULL;
diff --git a/drivers/dma-buf/reservation.c b/drivers/dma-buf/reservation.c
index 39920d77f288..c0bd5722c997 100644
--- a/drivers/dma-buf/reservation.c
+++ b/drivers/dma-buf/reservation.c
@@ -337,7 +337,8 @@ retry:
 	rcu_read_lock();
 
 	if (wait_all) {
-		struct reservation_object_list *fobj = rcu_dereference(obj->fence);
+		struct reservation_object_list *fobj =
+						rcu_dereference(obj->fence);
 
 		if (fobj)
 			shared_count = fobj->shared_count;
@@ -429,7 +430,8 @@ retry:
 	if (test_all) {
 		unsigned i;
 
-		struct reservation_object_list *fobj = rcu_dereference(obj->fence);
+		struct reservation_object_list *fobj =
+						rcu_dereference(obj->fence);
 
 		if (fobj)
 			shared_count = fobj->shared_count;
@@ -462,7 +464,8 @@ retry:
 			goto unlock_retry;
 
 		if (fence_excl) {
-			ret = reservation_object_test_signaled_single(fence_excl);
+			ret = reservation_object_test_signaled_single(
+								fence_excl);
 			if (ret < 0)
 				goto unlock_retry;
 		}
diff --git a/drivers/dma-buf/seqno-fence.c b/drivers/dma-buf/seqno-fence.c
index 7d12a39a4b57..71127f8f1626 100644
--- a/drivers/dma-buf/seqno-fence.c
+++ b/drivers/dma-buf/seqno-fence.c
@@ -24,24 +24,28 @@
 static const char *seqno_fence_get_driver_name(struct fence *fence)
 {
 	struct seqno_fence *seqno_fence = to_seqno_fence(fence);
+
 	return seqno_fence->ops->get_driver_name(fence);
 }
 
 static const char *seqno_fence_get_timeline_name(struct fence *fence)
 {
 	struct seqno_fence *seqno_fence = to_seqno_fence(fence);
+
 	return seqno_fence->ops->get_timeline_name(fence);
 }
 
 static bool seqno_enable_signaling(struct fence *fence)
 {
 	struct seqno_fence *seqno_fence = to_seqno_fence(fence);
+
 	return seqno_fence->ops->enable_signaling(fence);
 }
 
 static bool seqno_signaled(struct fence *fence)
 {
 	struct seqno_fence *seqno_fence = to_seqno_fence(fence);
+
 	return seqno_fence->ops->signaled && seqno_fence->ops->signaled(fence);
 }
 
@@ -56,9 +60,11 @@ static void seqno_release(struct fence *fence)
 		fence_free(&f->base);
 }
 
-static signed long seqno_wait(struct fence *fence, bool intr, signed long timeout)
+static signed long seqno_wait(struct fence *fence, bool intr,
+				signed long timeout)
 {
 	struct seqno_fence *f = to_seqno_fence(fence);
+
 	return f->ops->wait(fence, intr, timeout);
 }
 
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index bda2cb06dc7a..88d474b78076 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -162,6 +162,17 @@ config MX3_IPU_IRQS
 	  To avoid bloating the irq_desc[] array we allocate a sufficient
 	  number of IRQ slots and map them dynamically to specific sources.
 
+config PXA_DMA
+	bool "PXA DMA support"
+	depends on (ARCH_MMP || ARCH_PXA)
+	select DMA_ENGINE
+	select DMA_VIRTUAL_CHANNELS
+	help
+	  Support the DMA engine for PXA. It is also compatible with MMP PDMA
+	  platform. The internal DMA IP of all PXA variants is supported, with
+	  16 to 32 channels for peripheral to memory or memory to memory
+	  transfers.
+
 config TXX9_DMAC
 	tristate "Toshiba TXx9 SoC DMA support"
 	depends on MACH_TX49XX || MACH_TX39XX
@@ -245,6 +256,9 @@ config TI_EDMA
 	  Enable support for the TI EDMA controller. This DMA
 	  engine is found on TI DaVinci and AM33xx parts.
 
+config TI_DMA_CROSSBAR
+	bool
+
 config ARCH_HAS_ASYNC_TX_FIND_CHANNEL
 	bool
 
@@ -330,6 +344,7 @@ config DMA_OMAP
 	depends on ARCH_OMAP
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
+	select TI_DMA_CROSSBAR if SOC_DRA7XX
 
 config DMA_BCM2835
 	tristate "BCM2835 DMA engine support"
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index 69f77d5ba53b..6a4d6f2827da 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -25,6 +25,7 @@ obj-$(CONFIG_AMCC_PPC440SPE_ADMA) += ppc4xx/
 obj-$(CONFIG_IMX_SDMA) += imx-sdma.o
 obj-$(CONFIG_IMX_DMA) += imx-dma.o
 obj-$(CONFIG_MXS_DMA) += mxs-dma.o
+obj-$(CONFIG_PXA_DMA) += pxa_dma.o
 obj-$(CONFIG_TIMB_DMA) += timb_dma.o
 obj-$(CONFIG_SIRF_DMA) += sirf-dma.o
 obj-$(CONFIG_TI_EDMA) += edma.o
@@ -38,6 +39,7 @@ obj-$(CONFIG_EP93XX_DMA) += ep93xx_dma.o
 obj-$(CONFIG_DMA_SA11X0) += sa11x0-dma.o
 obj-$(CONFIG_MMP_TDMA) += mmp_tdma.o
 obj-$(CONFIG_DMA_OMAP) += omap-dma.o
+obj-$(CONFIG_TI_DMA_CROSSBAR) += ti-dma-crossbar.o
 obj-$(CONFIG_DMA_BCM2835) += bcm2835-dma.o
 obj-$(CONFIG_MMP_PDMA) += mmp_pdma.o
 obj-$(CONFIG_DMA_JZ4740) += dma-jz4740.o
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index 49d396ec06e5..5de3cf453f35 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -474,7 +474,7 @@ static void pl08x_terminate_phy_chan(struct pl08x_driver_data *pl08x,
 	u32 val = readl(ch->reg_config);
 
 	val &= ~(PL080_CONFIG_ENABLE | PL080_CONFIG_ERR_IRQ_MASK |
-	         PL080_CONFIG_TC_IRQ_MASK);
+		 PL080_CONFIG_TC_IRQ_MASK);
 
 	writel(val, ch->reg_config);
 
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 57b2141ddddc..59892126d175 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -247,6 +247,10 @@ static void atc_dostart(struct at_dma_chan *atchan, struct at_desc *first)
 	channel_writel(atchan, CTRLA, 0);
 	channel_writel(atchan, CTRLB, 0);
 	channel_writel(atchan, DSCR, first->txd.phys);
+	channel_writel(atchan, SPIP, ATC_SPIP_HOLE(first->src_hole) |
+		       ATC_SPIP_BOUNDARY(first->boundary));
+	channel_writel(atchan, DPIP, ATC_DPIP_HOLE(first->dst_hole) |
+		       ATC_DPIP_BOUNDARY(first->boundary));
 	dma_writel(atdma, CHER, atchan->mask);
 
 	vdbg_dump_regs(atchan);
@@ -635,6 +639,104 @@ static dma_cookie_t atc_tx_submit(struct dma_async_tx_descriptor *tx)
 }
 
 /**
+ * atc_prep_dma_interleaved - prepare memory to memory interleaved operation
+ * @chan: the channel to prepare operation on
+ * @xt: Interleaved transfer template
+ * @flags: tx descriptor status flags
+ */
+static struct dma_async_tx_descriptor *
+atc_prep_dma_interleaved(struct dma_chan *chan,
+			 struct dma_interleaved_template *xt,
+			 unsigned long flags)
+{
+	struct at_dma_chan	*atchan = to_at_dma_chan(chan);
+	struct data_chunk	*first = xt->sgl;
+	struct at_desc		*desc = NULL;
+	size_t			xfer_count;
+	unsigned int		dwidth;
+	u32			ctrla;
+	u32			ctrlb;
+	size_t			len = 0;
+	int			i;
+
+	dev_info(chan2dev(chan),
+		 "%s: src=0x%08x, dest=0x%08x, numf=%d, frame_size=%d, flags=0x%lx\n",
+		__func__, xt->src_start, xt->dst_start, xt->numf,
+		xt->frame_size, flags);
+
+	if (unlikely(!xt || xt->numf != 1 || !xt->frame_size))
+		return NULL;
+
+	/*
+	 * The controller can only "skip" X bytes every Y bytes, so we
+	 * need to make sure we are given a template that fit that
+	 * description, ie a template with chunks that always have the
+	 * same size, with the same ICGs.
+	 */
+	for (i = 0; i < xt->frame_size; i++) {
+		struct data_chunk *chunk = xt->sgl + i;
+
+		if ((chunk->size != xt->sgl->size) ||
+		    (dmaengine_get_dst_icg(xt, chunk) != dmaengine_get_dst_icg(xt, first)) ||
+		    (dmaengine_get_src_icg(xt, chunk) != dmaengine_get_src_icg(xt, first))) {
+			dev_err(chan2dev(chan),
+				"%s: the controller can transfer only identical chunks\n",
+				__func__);
+			return NULL;
+		}
+
+		len += chunk->size;
+	}
+
+	dwidth = atc_get_xfer_width(xt->src_start,
+				    xt->dst_start, len);
+
+	xfer_count = len >> dwidth;
+	if (xfer_count > ATC_BTSIZE_MAX) {
+		dev_err(chan2dev(chan), "%s: buffer is too big\n", __func__);
+		return NULL;
+	}
+
+	ctrla = ATC_SRC_WIDTH(dwidth) |
+		ATC_DST_WIDTH(dwidth);
+
+	ctrlb =   ATC_DEFAULT_CTRLB | ATC_IEN
+		| ATC_SRC_ADDR_MODE_INCR
+		| ATC_DST_ADDR_MODE_INCR
+		| ATC_SRC_PIP
+		| ATC_DST_PIP
+		| ATC_FC_MEM2MEM;
+
+	/* create the transfer */
+	desc = atc_desc_get(atchan);
+	if (!desc) {
+		dev_err(chan2dev(chan),
+			"%s: couldn't allocate our descriptor\n", __func__);
+		return NULL;
+	}
+
+	desc->lli.saddr = xt->src_start;
+	desc->lli.daddr = xt->dst_start;
+	desc->lli.ctrla = ctrla | xfer_count;
+	desc->lli.ctrlb = ctrlb;
+
+	desc->boundary = first->size >> dwidth;
+	desc->dst_hole = (dmaengine_get_dst_icg(xt, first) >> dwidth) + 1;
+	desc->src_hole = (dmaengine_get_src_icg(xt, first) >> dwidth) + 1;
+
+	desc->txd.cookie = -EBUSY;
+	desc->total_len = desc->len = len;
+	desc->tx_width = dwidth;
+
+	/* set end-of-link to the last link descriptor of list*/
+	set_desc_eol(desc);
+
+	desc->txd.flags = flags; /* client is in control of this ack */
+
+	return &desc->txd;
+}
+
+/**
  * atc_prep_dma_memcpy - prepare a memcpy operation
  * @chan: the channel to prepare operation on
  * @dest: operation virtual destination address
@@ -1609,6 +1711,7 @@ static int __init at_dma_probe(struct platform_device *pdev)
 	/* setup platform data for each SoC */
 	dma_cap_set(DMA_MEMCPY, at91sam9rl_config.cap_mask);
 	dma_cap_set(DMA_SG, at91sam9rl_config.cap_mask);
+	dma_cap_set(DMA_INTERLEAVE, at91sam9g45_config.cap_mask);
 	dma_cap_set(DMA_MEMCPY, at91sam9g45_config.cap_mask);
 	dma_cap_set(DMA_SLAVE, at91sam9g45_config.cap_mask);
 	dma_cap_set(DMA_SG, at91sam9g45_config.cap_mask);
@@ -1713,6 +1816,9 @@ static int __init at_dma_probe(struct platform_device *pdev)
 	atdma->dma_common.dev = &pdev->dev;
 
 	/* set prep routines based on capability */
+	if (dma_has_cap(DMA_INTERLEAVE, atdma->dma_common.cap_mask))
+		atdma->dma_common.device_prep_interleaved_dma = atc_prep_dma_interleaved;
+
 	if (dma_has_cap(DMA_MEMCPY, atdma->dma_common.cap_mask))
 		atdma->dma_common.device_prep_dma_memcpy = atc_prep_dma_memcpy;
 
diff --git a/drivers/dma/at_hdmac_regs.h b/drivers/dma/at_hdmac_regs.h
index 2727ca560572..bc8d5ebedd19 100644
--- a/drivers/dma/at_hdmac_regs.h
+++ b/drivers/dma/at_hdmac_regs.h
@@ -196,6 +196,11 @@ struct at_desc {
 	size_t				len;
 	u32				tx_width;
 	size_t				total_len;
+
+	/* Interleaved data */
+	size_t				boundary;
+	size_t				dst_hole;
+	size_t				src_hole;
 };
 
 static inline struct at_desc *
diff --git a/drivers/dma/at_xdmac.c b/drivers/dma/at_xdmac.c
index 933e4b338459..cf1213de7865 100644
--- a/drivers/dma/at_xdmac.c
+++ b/drivers/dma/at_xdmac.c
@@ -174,6 +174,8 @@
 #define AT_XDMAC_MBR_UBC_NDV3		(0x3 << 27)	/* Next Descriptor View 3 */
 
 #define AT_XDMAC_MAX_CHAN	0x20
+#define AT_XDMAC_MAX_CSIZE	16	/* 16 data */
+#define AT_XDMAC_MAX_DWIDTH	8	/* 64 bits */
 
 #define AT_XDMAC_DMA_BUSWIDTHS\
 	(BIT(DMA_SLAVE_BUSWIDTH_UNDEFINED) |\
@@ -192,20 +194,17 @@ struct at_xdmac_chan {
 	struct dma_chan			chan;
 	void __iomem			*ch_regs;
 	u32				mask;		/* Channel Mask */
-	u32				cfg[2];		/* Channel Configuration Register */
-	#define	AT_XDMAC_DEV_TO_MEM_CFG	0		/* Predifined dev to mem channel conf */
-	#define	AT_XDMAC_MEM_TO_DEV_CFG	1		/* Predifined mem to dev channel conf */
+	u32				cfg;		/* Channel Configuration Register */
 	u8				perid;		/* Peripheral ID */
 	u8				perif;		/* Peripheral Interface */
 	u8				memif;		/* Memory Interface */
-	u32				per_src_addr;
-	u32				per_dst_addr;
 	u32				save_cc;
 	u32				save_cim;
 	u32				save_cnda;
 	u32				save_cndc;
 	unsigned long			status;
 	struct tasklet_struct		tasklet;
+	struct dma_slave_config		sconfig;
 
 	spinlock_t			lock;
 
@@ -236,6 +235,10 @@ struct at_xdmac_lld {
 	dma_addr_t	mbr_sa;		/* Source Address Member */
 	dma_addr_t	mbr_da;		/* Destination Address Member */
 	u32		mbr_cfg;	/* Configuration Register */
+	u32		mbr_bc;		/* Block Control Register */
+	u32		mbr_ds;		/* Data Stride Register */
+	u32		mbr_sus;	/* Source Microblock Stride Register */
+	u32		mbr_dus;	/* Destination Microblock Stride Register */
 };
 
 
@@ -359,6 +362,8 @@ static void at_xdmac_start_xfer(struct at_xdmac_chan *atchan,
 	if (at_xdmac_chan_is_cyclic(atchan)) {
 		reg = AT_XDMAC_CNDC_NDVIEW_NDV1;
 		at_xdmac_chan_write(atchan, AT_XDMAC_CC, first->lld.mbr_cfg);
+	} else if (first->lld.mbr_ubc & AT_XDMAC_MBR_UBC_NDV3) {
+		reg = AT_XDMAC_CNDC_NDVIEW_NDV3;
 	} else {
 		/*
 		 * No need to write AT_XDMAC_CC reg, it will be done when the
@@ -415,8 +420,9 @@ 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;
+	unsigned long		irqflags;
 
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, irqflags);
 	cookie = dma_cookie_assign(tx);
 
 	dev_vdbg(chan2dev(tx->chan), "%s: atchan 0x%p, add desc 0x%p to xfers_list\n",
@@ -425,7 +431,7 @@ static dma_cookie_t at_xdmac_tx_submit(struct dma_async_tx_descriptor *tx)
 	if (list_is_singular(&atchan->xfers_list))
 		at_xdmac_start_xfer(atchan, desc);
 
-	spin_unlock_bh(&atchan->lock);
+	spin_unlock_irqrestore(&atchan->lock, irqflags);
 	return cookie;
 }
 
@@ -465,6 +471,33 @@ static struct at_xdmac_desc *at_xdmac_get_desc(struct at_xdmac_chan *atchan)
 	return desc;
 }
 
+static void at_xdmac_queue_desc(struct dma_chan *chan,
+				struct at_xdmac_desc *prev,
+				struct at_xdmac_desc *desc)
+{
+	if (!prev || !desc)
+		return;
+
+	prev->lld.mbr_nda = desc->tx_dma_desc.phys;
+	prev->lld.mbr_ubc |= AT_XDMAC_MBR_UBC_NDE;
+
+	dev_dbg(chan2dev(chan),	"%s: chain lld: prev=0x%p, mbr_nda=%pad\n",
+		__func__, prev, &prev->lld.mbr_nda);
+}
+
+static inline void at_xdmac_increment_block_count(struct dma_chan *chan,
+						  struct at_xdmac_desc *desc)
+{
+	if (!desc)
+		return;
+
+	desc->lld.mbr_bc++;
+
+	dev_dbg(chan2dev(chan),
+		"%s: incrementing the block count of the desc 0x%p\n",
+		__func__, desc);
+}
+
 static struct dma_chan *at_xdmac_xlate(struct of_phandle_args *dma_spec,
 				       struct of_dma *of_dma)
 {
@@ -494,61 +527,94 @@ static struct dma_chan *at_xdmac_xlate(struct of_phandle_args *dma_spec,
 	return chan;
 }
 
+static int at_xdmac_compute_chan_conf(struct dma_chan *chan,
+				      enum dma_transfer_direction direction)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	int			csize, dwidth;
+
+	if (direction == DMA_DEV_TO_MEM) {
+		atchan->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 = ffs(atchan->sconfig.src_maxburst) - 1;
+		if (csize < 0) {
+			dev_err(chan2dev(chan), "invalid src maxburst value\n");
+			return -EINVAL;
+		}
+		atchan->cfg |= AT_XDMAC_CC_CSIZE(csize);
+		dwidth = ffs(atchan->sconfig.src_addr_width) - 1;
+		if (dwidth < 0) {
+			dev_err(chan2dev(chan), "invalid src addr width value\n");
+			return -EINVAL;
+		}
+		atchan->cfg |= AT_XDMAC_CC_DWIDTH(dwidth);
+	} else if (direction == DMA_MEM_TO_DEV) {
+		atchan->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 = ffs(atchan->sconfig.dst_maxburst) - 1;
+		if (csize < 0) {
+			dev_err(chan2dev(chan), "invalid src maxburst value\n");
+			return -EINVAL;
+		}
+		atchan->cfg |= AT_XDMAC_CC_CSIZE(csize);
+		dwidth = ffs(atchan->sconfig.dst_addr_width) - 1;
+		if (dwidth < 0) {
+			dev_err(chan2dev(chan), "invalid dst addr width value\n");
+			return -EINVAL;
+		}
+		atchan->cfg |= AT_XDMAC_CC_DWIDTH(dwidth);
+	}
+
+	dev_dbg(chan2dev(chan),	"%s: cfg=0x%08x\n", __func__, atchan->cfg);
+
+	return 0;
+}
+
+/*
+ * Only check that maxburst and addr width values are supported by the
+ * the controller but not that the configuration is good to perform the
+ * transfer since we don't know the direction at this stage.
+ */
+static int at_xdmac_check_slave_config(struct dma_slave_config *sconfig)
+{
+	if ((sconfig->src_maxburst > AT_XDMAC_MAX_CSIZE)
+	    || (sconfig->dst_maxburst > AT_XDMAC_MAX_CSIZE))
+		return -EINVAL;
+
+	if ((sconfig->src_addr_width > AT_XDMAC_MAX_DWIDTH)
+	    || (sconfig->dst_addr_width > AT_XDMAC_MAX_DWIDTH))
+		return -EINVAL;
+
+	return 0;
+}
+
 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");
+	if (at_xdmac_check_slave_config(sconfig)) {
+		dev_err(chan2dev(chan), "invalid slave configuration\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);
+	memcpy(&atchan->sconfig, sconfig, sizeof(atchan->sconfig));
 
 	return 0;
 }
@@ -563,6 +629,8 @@ at_xdmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 	struct scatterlist	*sg;
 	int			i;
 	unsigned int		xfer_size = 0;
+	unsigned long		irqflags;
+	struct dma_async_tx_descriptor	*ret = NULL;
 
 	if (!sgl)
 		return NULL;
@@ -578,7 +646,10 @@ at_xdmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		 flags);
 
 	/* Protect dma_sconfig field that can be modified by set_slave_conf. */
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, irqflags);
+
+	if (at_xdmac_compute_chan_conf(chan, direction))
+		goto spin_unlock;
 
 	/* Prepare descriptors. */
 	for_each_sg(sgl, sg, sg_len, i) {
@@ -589,8 +660,7 @@ at_xdmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		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;
+			goto spin_unlock;
 		}
 		dev_dbg(chan2dev(chan), "%s: * sg%d len=%u, mem=0x%08x\n",
 			 __func__, i, len, mem);
@@ -600,20 +670,18 @@ at_xdmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 			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;
+			goto spin_unlock;
 		}
 
 		/* Linked list descriptor setup. */
 		if (direction == DMA_DEV_TO_MEM) {
-			desc->lld.mbr_sa = atchan->per_src_addr;
+			desc->lld.mbr_sa = atchan->sconfig.src_addr;
 			desc->lld.mbr_da = mem;
-			desc->lld.mbr_cfg = atchan->cfg[AT_XDMAC_DEV_TO_MEM_CFG];
 		} else {
 			desc->lld.mbr_sa = mem;
-			desc->lld.mbr_da = atchan->per_dst_addr;
-			desc->lld.mbr_cfg = atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG];
+			desc->lld.mbr_da = atchan->sconfig.dst_addr;
 		}
+		desc->lld.mbr_cfg = atchan->cfg;
 		dwidth = at_xdmac_get_dwidth(desc->lld.mbr_cfg);
 		fixed_dwidth = IS_ALIGNED(len, 1 << dwidth)
 			       ? at_xdmac_get_dwidth(desc->lld.mbr_cfg)
@@ -621,19 +689,14 @@ at_xdmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		desc->lld.mbr_ubc = AT_XDMAC_MBR_UBC_NDV2			/* 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 >> fixed_dwidth);				/* 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);
-		}
+		if (prev)
+			at_xdmac_queue_desc(chan, prev, desc);
 
 		prev = desc;
 		if (!first)
@@ -645,13 +708,15 @@ at_xdmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 		xfer_size += len;
 	}
 
-	spin_unlock_bh(&atchan->lock);
 
 	first->tx_dma_desc.flags = flags;
 	first->xfer_size = xfer_size;
 	first->direction = direction;
+	ret = &first->tx_dma_desc;
 
-	return &first->tx_dma_desc;
+spin_unlock:
+	spin_unlock_irqrestore(&atchan->lock, irqflags);
+	return ret;
 }
 
 static struct dma_async_tx_descriptor *
@@ -664,6 +729,7 @@ at_xdmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 	struct at_xdmac_desc	*first = NULL, *prev = NULL;
 	unsigned int		periods = buf_len / period_len;
 	int			i;
+	unsigned long		irqflags;
 
 	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,
@@ -679,36 +745,37 @@ at_xdmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 		return NULL;
 	}
 
+	if (at_xdmac_compute_chan_conf(chan, direction))
+		return NULL;
+
 	for (i = 0; i < periods; i++) {
 		struct at_xdmac_desc	*desc = NULL;
 
-		spin_lock_bh(&atchan->lock);
+		spin_lock_irqsave(&atchan->lock, irqflags);
 		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);
+			spin_unlock_irqrestore(&atchan->lock, irqflags);
 			return NULL;
 		}
-		spin_unlock_bh(&atchan->lock);
+		spin_unlock_irqrestore(&atchan->lock, irqflags);
 		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_sa = atchan->sconfig.src_addr;
 			desc->lld.mbr_da = buf_addr + i * period_len;
-			desc->lld.mbr_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;
-			desc->lld.mbr_cfg = atchan->cfg[AT_XDMAC_MEM_TO_DEV_CFG];
+			desc->lld.mbr_da = atchan->sconfig.dst_addr;
 		}
+		desc->lld.mbr_cfg = atchan->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(desc->lld.mbr_cfg);
 
 		dev_dbg(chan2dev(chan),
@@ -716,12 +783,8 @@ at_xdmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 			 __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);
-		}
+		if (prev)
+			at_xdmac_queue_desc(chan, prev, desc);
 
 		prev = desc;
 		if (!first)
@@ -743,6 +806,215 @@ at_xdmac_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr,
 	return &first->tx_dma_desc;
 }
 
+static inline u32 at_xdmac_align_width(struct dma_chan *chan, dma_addr_t addr)
+{
+	u32 width;
+
+	/*
+	 * 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 (!(addr & 7)) {
+		width = AT_XDMAC_CC_DWIDTH_DWORD;
+		dev_dbg(chan2dev(chan), "%s: dwidth: double word\n", __func__);
+	} else if (!(addr & 3)) {
+		width = AT_XDMAC_CC_DWIDTH_WORD;
+		dev_dbg(chan2dev(chan), "%s: dwidth: word\n", __func__);
+	} else if (!(addr & 1)) {
+		width = AT_XDMAC_CC_DWIDTH_HALFWORD;
+		dev_dbg(chan2dev(chan), "%s: dwidth: half word\n", __func__);
+	} else {
+		width = AT_XDMAC_CC_DWIDTH_BYTE;
+		dev_dbg(chan2dev(chan), "%s: dwidth: byte\n", __func__);
+	}
+
+	return width;
+}
+
+static struct at_xdmac_desc *
+at_xdmac_interleaved_queue_desc(struct dma_chan *chan,
+				struct at_xdmac_chan *atchan,
+				struct at_xdmac_desc *prev,
+				dma_addr_t src, dma_addr_t dst,
+				struct dma_interleaved_template *xt,
+				struct data_chunk *chunk)
+{
+	struct at_xdmac_desc	*desc;
+	u32			dwidth;
+	unsigned long		flags;
+	size_t			ublen;
+	/*
+	 * WARNING: The channel configuration is set here since there is no
+	 * dmaengine_slave_config call in this case. Moreover 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_DIF(0)
+					| AT_XDMAC_CC_SIF(0)
+					| AT_XDMAC_CC_MBSIZE_SIXTEEN
+					| AT_XDMAC_CC_TYPE_MEM_TRAN;
+
+	dwidth = at_xdmac_align_width(chan, src | dst | chunk->size);
+	if (chunk->size >= (AT_XDMAC_MBR_UBC_UBLEN_MAX << dwidth)) {
+		dev_dbg(chan2dev(chan),
+			"%s: chunk too big (%d, max size %lu)...\n",
+			__func__, chunk->size,
+			AT_XDMAC_MBR_UBC_UBLEN_MAX << dwidth);
+		return NULL;
+	}
+
+	if (prev)
+		dev_dbg(chan2dev(chan),
+			"Adding items at the end of desc 0x%p\n", prev);
+
+	if (xt->src_inc) {
+		if (xt->src_sgl)
+			chan_cc |=  AT_XDMAC_CC_SAM_UBS_DS_AM;
+		else
+			chan_cc |=  AT_XDMAC_CC_SAM_INCREMENTED_AM;
+	}
+
+	if (xt->dst_inc) {
+		if (xt->dst_sgl)
+			chan_cc |=  AT_XDMAC_CC_DAM_UBS_DS_AM;
+		else
+			chan_cc |=  AT_XDMAC_CC_DAM_INCREMENTED_AM;
+	}
+
+	spin_lock_irqsave(&atchan->lock, flags);
+	desc = at_xdmac_get_desc(atchan);
+	spin_unlock_irqrestore(&atchan->lock, flags);
+	if (!desc) {
+		dev_err(chan2dev(chan), "can't get descriptor\n");
+		return NULL;
+	}
+
+	chan_cc |= AT_XDMAC_CC_DWIDTH(dwidth);
+
+	ublen = chunk->size >> dwidth;
+
+	desc->lld.mbr_sa = src;
+	desc->lld.mbr_da = dst;
+	desc->lld.mbr_sus = dmaengine_get_src_icg(xt, chunk);
+	desc->lld.mbr_dus = dmaengine_get_dst_icg(xt, chunk);
+
+	desc->lld.mbr_ubc = AT_XDMAC_MBR_UBC_NDV3
+		| AT_XDMAC_MBR_UBC_NDEN
+		| AT_XDMAC_MBR_UBC_NSEN
+		| ublen;
+	desc->lld.mbr_cfg = chan_cc;
+
+	dev_dbg(chan2dev(chan),
+		"%s: lld: mbr_sa=0x%08x, mbr_da=0x%08x, 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)
+		at_xdmac_queue_desc(chan, prev, desc);
+
+	return desc;
+}
+
+static struct dma_async_tx_descriptor *
+at_xdmac_prep_interleaved(struct dma_chan *chan,
+			  struct dma_interleaved_template *xt,
+			  unsigned long flags)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac_desc	*prev = NULL, *first = NULL;
+	struct data_chunk	*chunk, *prev_chunk = NULL;
+	dma_addr_t		dst_addr, src_addr;
+	size_t			dst_skip, src_skip, len = 0;
+	size_t			prev_dst_icg = 0, prev_src_icg = 0;
+	int			i;
+
+	if (!xt || (xt->numf != 1) || (xt->dir != DMA_MEM_TO_MEM))
+		return NULL;
+
+	dev_dbg(chan2dev(chan), "%s: src=0x%08x, dest=0x%08x, numf=%d, frame_size=%d, flags=0x%lx\n",
+		__func__, xt->src_start, xt->dst_start,	xt->numf,
+		xt->frame_size, flags);
+
+	src_addr = xt->src_start;
+	dst_addr = xt->dst_start;
+
+	for (i = 0; i < xt->frame_size; i++) {
+		struct at_xdmac_desc *desc;
+		size_t src_icg, dst_icg;
+
+		chunk = xt->sgl + i;
+
+		dst_icg = dmaengine_get_dst_icg(xt, chunk);
+		src_icg = dmaengine_get_src_icg(xt, chunk);
+
+		src_skip = chunk->size + src_icg;
+		dst_skip = chunk->size + dst_icg;
+
+		dev_dbg(chan2dev(chan),
+			"%s: chunk size=%d, src icg=%d, dst icg=%d\n",
+			__func__, chunk->size, src_icg, dst_icg);
+
+		/*
+		 * Handle the case where we just have the same
+		 * transfer to setup, we can just increase the
+		 * block number and reuse the same descriptor.
+		 */
+		if (prev_chunk && prev &&
+		    (prev_chunk->size == chunk->size) &&
+		    (prev_src_icg == src_icg) &&
+		    (prev_dst_icg == dst_icg)) {
+			dev_dbg(chan2dev(chan),
+				"%s: same configuration that the previous chunk, merging the descriptors...\n",
+				__func__);
+			at_xdmac_increment_block_count(chan, prev);
+			continue;
+		}
+
+		desc = at_xdmac_interleaved_queue_desc(chan, atchan,
+						       prev,
+						       src_addr, dst_addr,
+						       xt, chunk);
+		if (!desc) {
+			list_splice_init(&first->descs_list,
+					 &atchan->free_descs_list);
+			return NULL;
+		}
+
+		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);
+
+		if (xt->src_sgl)
+			src_addr += src_skip;
+
+		if (xt->dst_sgl)
+			dst_addr += dst_skip;
+
+		len += chunk->size;
+		prev_chunk = chunk;
+		prev_dst_icg = dst_icg;
+		prev_src_icg = src_icg;
+		prev = desc;
+	}
+
+	first->tx_dma_desc.cookie = -EBUSY;
+	first->tx_dma_desc.flags = flags;
+	first->xfer_size = len;
+
+	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)
@@ -766,6 +1038,7 @@ at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 					| AT_XDMAC_CC_SIF(0)
 					| AT_XDMAC_CC_MBSIZE_SIXTEEN
 					| AT_XDMAC_CC_TYPE_MEM_TRAN;
+	unsigned long		irqflags;
 
 	dev_dbg(chan2dev(chan), "%s: src=%pad, dest=%pad, len=%zd, flags=0x%lx\n",
 		__func__, &src, &dest, len, flags);
@@ -773,24 +1046,7 @@ at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	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__);
-	}
+	dwidth = at_xdmac_align_width(chan, src_addr | dst_addr);
 
 	/* Prepare descriptors. */
 	while (remaining_size) {
@@ -798,9 +1054,9 @@ at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 
 		dev_dbg(chan2dev(chan), "%s: remaining_size=%zu\n", __func__, remaining_size);
 
-		spin_lock_bh(&atchan->lock);
+		spin_lock_irqsave(&atchan->lock, irqflags);
 		desc = at_xdmac_get_desc(atchan);
-		spin_unlock_bh(&atchan->lock);
+		spin_unlock_irqrestore(&atchan->lock, irqflags);
 		if (!desc) {
 			dev_err(chan2dev(chan), "can't get descriptor\n");
 			if (first)
@@ -820,19 +1076,8 @@ at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 		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__);
-		}
+		dwidth = at_xdmac_align_width(chan,
+					      src_addr | dst_addr | xfer_size);
 		chan_cc |= AT_XDMAC_CC_DWIDTH(dwidth);
 
 		ublen = xfer_size >> dwidth;
@@ -843,7 +1088,6 @@ at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 		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;
 
@@ -852,12 +1096,8 @@ at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 			 __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);
-		}
+		if (prev)
+			at_xdmac_queue_desc(chan, prev, desc);
 
 		prev = desc;
 		if (!first)
@@ -874,6 +1114,93 @@ at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	return &first->tx_dma_desc;
 }
 
+static struct at_xdmac_desc *at_xdmac_memset_create_desc(struct dma_chan *chan,
+							 struct at_xdmac_chan *atchan,
+							 dma_addr_t dst_addr,
+							 size_t len,
+							 int value)
+{
+	struct at_xdmac_desc	*desc;
+	unsigned long		flags;
+	size_t			ublen;
+	u32			dwidth;
+	/*
+	 * WARNING: The channel configuration is set here since there is no
+	 * dmaengine_slave_config call in this case. Moreover 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_MEMSET_HW_MODE
+					| AT_XDMAC_CC_TYPE_MEM_TRAN;
+
+	dwidth = at_xdmac_align_width(chan, dst_addr);
+
+	if (len >= (AT_XDMAC_MBR_UBC_UBLEN_MAX << dwidth)) {
+		dev_err(chan2dev(chan),
+			"%s: Transfer too large, aborting...\n",
+			__func__);
+		return NULL;
+	}
+
+	spin_lock_irqsave(&atchan->lock, flags);
+	desc = at_xdmac_get_desc(atchan);
+	spin_unlock_irqrestore(&atchan->lock, flags);
+	if (!desc) {
+		dev_err(chan2dev(chan), "can't get descriptor\n");
+		return NULL;
+	}
+
+	chan_cc |= AT_XDMAC_CC_DWIDTH(dwidth);
+
+	ublen = len >> dwidth;
+
+	desc->lld.mbr_da = dst_addr;
+	desc->lld.mbr_ds = value;
+	desc->lld.mbr_ubc = AT_XDMAC_MBR_UBC_NDV3
+		| AT_XDMAC_MBR_UBC_NDEN
+		| AT_XDMAC_MBR_UBC_NSEN
+		| ublen;
+	desc->lld.mbr_cfg = chan_cc;
+
+	dev_dbg(chan2dev(chan),
+		"%s: lld: mbr_da=0x%08x, mbr_ds=0x%08x, mbr_ubc=0x%08x, mbr_cfg=0x%08x\n",
+		__func__, desc->lld.mbr_da, desc->lld.mbr_ds, desc->lld.mbr_ubc,
+		desc->lld.mbr_cfg);
+
+	return desc;
+}
+
+struct dma_async_tx_descriptor *
+at_xdmac_prep_dma_memset(struct dma_chan *chan, dma_addr_t dest, int value,
+			 size_t len, unsigned long flags)
+{
+	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
+	struct at_xdmac_desc	*desc;
+
+	dev_dbg(chan2dev(chan), "%s: dest=0x%08x, len=%d, pattern=0x%x, flags=0x%lx\n",
+		__func__, dest, len, value, flags);
+
+	if (unlikely(!len))
+		return NULL;
+
+	desc = at_xdmac_memset_create_desc(chan, atchan, dest, len, value);
+	list_add_tail(&desc->desc_node, &desc->descs_list);
+
+	desc->tx_dma_desc.cookie = -EBUSY;
+	desc->tx_dma_desc.flags = flags;
+	desc->xfer_size = len;
+
+	return &desc->tx_dma_desc;
+}
+
 static enum dma_status
 at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 		struct dma_tx_state *txstate)
@@ -886,6 +1213,7 @@ at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 	int			residue;
 	u32			cur_nda, mask, value;
 	u8			dwidth = 0;
+	unsigned long		flags;
 
 	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret == DMA_COMPLETE)
@@ -894,7 +1222,7 @@ at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 	if (!txstate)
 		return ret;
 
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, flags);
 
 	desc = list_first_entry(&atchan->xfers_list, struct at_xdmac_desc, xfer_node);
 
@@ -904,8 +1232,7 @@ at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 	 */
 	if (!desc->active_xfer) {
 		dma_set_residue(txstate, desc->xfer_size);
-		spin_unlock_bh(&atchan->lock);
-		return ret;
+		goto spin_unlock;
 	}
 
 	residue = desc->xfer_size;
@@ -936,14 +1263,14 @@ at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 	}
 	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);
 
+spin_unlock:
+	spin_unlock_irqrestore(&atchan->lock, flags);
 	return ret;
 }
 
@@ -964,8 +1291,9 @@ static void at_xdmac_remove_xfer(struct at_xdmac_chan *atchan,
 static void at_xdmac_advance_work(struct at_xdmac_chan *atchan)
 {
 	struct at_xdmac_desc	*desc;
+	unsigned long		flags;
 
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, flags);
 
 	/*
 	 * If channel is enabled, do nothing, advance_work will be triggered
@@ -980,7 +1308,7 @@ static void at_xdmac_advance_work(struct at_xdmac_chan *atchan)
 			at_xdmac_start_xfer(atchan, desc);
 	}
 
-	spin_unlock_bh(&atchan->lock);
+	spin_unlock_irqrestore(&atchan->lock, flags);
 }
 
 static void at_xdmac_handle_cyclic(struct at_xdmac_chan *atchan)
@@ -1116,12 +1444,13 @@ static int at_xdmac_device_config(struct dma_chan *chan,
 {
 	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
 	int ret;
+	unsigned long		flags;
 
 	dev_dbg(chan2dev(chan), "%s\n", __func__);
 
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, flags);
 	ret = at_xdmac_set_slave_config(chan, config);
-	spin_unlock_bh(&atchan->lock);
+	spin_unlock_irqrestore(&atchan->lock, flags);
 
 	return ret;
 }
@@ -1130,18 +1459,19 @@ static int at_xdmac_device_pause(struct dma_chan *chan)
 {
 	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
 	struct at_xdmac		*atxdmac = to_at_xdmac(atchan->chan.device);
+	unsigned long		flags;
 
 	dev_dbg(chan2dev(chan), "%s\n", __func__);
 
 	if (test_and_set_bit(AT_XDMAC_CHAN_IS_PAUSED, &atchan->status))
 		return 0;
 
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, flags);
 	at_xdmac_write(atxdmac, AT_XDMAC_GRWS, atchan->mask);
 	while (at_xdmac_chan_read(atchan, AT_XDMAC_CC)
 	       & (AT_XDMAC_CC_WRIP | AT_XDMAC_CC_RDIP))
 		cpu_relax();
-	spin_unlock_bh(&atchan->lock);
+	spin_unlock_irqrestore(&atchan->lock, flags);
 
 	return 0;
 }
@@ -1150,18 +1480,19 @@ static int at_xdmac_device_resume(struct dma_chan *chan)
 {
 	struct at_xdmac_chan	*atchan = to_at_xdmac_chan(chan);
 	struct at_xdmac		*atxdmac = to_at_xdmac(atchan->chan.device);
+	unsigned long		flags;
 
 	dev_dbg(chan2dev(chan), "%s\n", __func__);
 
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, flags);
 	if (!at_xdmac_chan_is_paused(atchan)) {
-		spin_unlock_bh(&atchan->lock);
+		spin_unlock_irqrestore(&atchan->lock, flags);
 		return 0;
 	}
 
 	at_xdmac_write(atxdmac, AT_XDMAC_GRWR, atchan->mask);
 	clear_bit(AT_XDMAC_CHAN_IS_PAUSED, &atchan->status);
-	spin_unlock_bh(&atchan->lock);
+	spin_unlock_irqrestore(&atchan->lock, flags);
 
 	return 0;
 }
@@ -1171,10 +1502,11 @@ static int at_xdmac_device_terminate_all(struct dma_chan *chan)
 	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);
+	unsigned long		flags;
 
 	dev_dbg(chan2dev(chan), "%s\n", __func__);
 
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, flags);
 	at_xdmac_write(atxdmac, AT_XDMAC_GD, atchan->mask);
 	while (at_xdmac_read(atxdmac, AT_XDMAC_GS) & atchan->mask)
 		cpu_relax();
@@ -1184,7 +1516,7 @@ static int at_xdmac_device_terminate_all(struct dma_chan *chan)
 		at_xdmac_remove_xfer(atchan, desc);
 
 	clear_bit(AT_XDMAC_CHAN_IS_CYCLIC, &atchan->status);
-	spin_unlock_bh(&atchan->lock);
+	spin_unlock_irqrestore(&atchan->lock, flags);
 
 	return 0;
 }
@@ -1194,8 +1526,9 @@ 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;
+	unsigned long		flags;
 
-	spin_lock_bh(&atchan->lock);
+	spin_lock_irqsave(&atchan->lock, flags);
 
 	if (at_xdmac_chan_is_enabled(atchan)) {
 		dev_err(chan2dev(chan),
@@ -1226,7 +1559,7 @@ static int at_xdmac_alloc_chan_resources(struct dma_chan *chan)
 	dev_dbg(chan2dev(chan), "%s: allocated %d descriptors\n", __func__, i);
 
 spin_unlock:
-	spin_unlock_bh(&atchan->lock);
+	spin_unlock_irqrestore(&atchan->lock, flags);
 	return i;
 }
 
@@ -1398,7 +1731,9 @@ static int at_xdmac_probe(struct platform_device *pdev)
 	}
 
 	dma_cap_set(DMA_CYCLIC, atxdmac->dma.cap_mask);
+	dma_cap_set(DMA_INTERLEAVE, atxdmac->dma.cap_mask);
 	dma_cap_set(DMA_MEMCPY, atxdmac->dma.cap_mask);
+	dma_cap_set(DMA_MEMSET, 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
@@ -1411,7 +1746,9 @@ static int at_xdmac_probe(struct platform_device *pdev)
 	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_interleaved_dma	= at_xdmac_prep_interleaved;
 	atxdmac->dma.device_prep_dma_memcpy		= at_xdmac_prep_dma_memcpy;
+	atxdmac->dma.device_prep_dma_memset		= at_xdmac_prep_dma_memset;
 	atxdmac->dma.device_prep_slave_sg		= at_xdmac_prep_slave_sg;
 	atxdmac->dma.device_config			= at_xdmac_device_config;
 	atxdmac->dma.device_pause			= at_xdmac_device_pause;
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index 2890d744bb1b..4a4cce15f25d 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -267,6 +267,13 @@ static void dma_chan_put(struct dma_chan *chan)
 	/* This channel is not in use anymore, free it */
 	if (!chan->client_count && chan->device->device_free_chan_resources)
 		chan->device->device_free_chan_resources(chan);
+
+	/* If the channel is used via a DMA request router, free the mapping */
+	if (chan->router && chan->router->route_free) {
+		chan->router->route_free(chan->router->dev, chan->route_data);
+		chan->router = NULL;
+		chan->route_data = NULL;
+	}
 }
 
 enum dma_status dma_sync_wait(struct dma_chan *chan, dma_cookie_t cookie)
@@ -487,7 +494,11 @@ int dma_get_slave_caps(struct dma_chan *chan, struct dma_slave_caps *caps)
 	caps->directions = device->directions;
 	caps->residue_granularity = device->residue_granularity;
 
-	caps->cmd_pause = !!device->device_pause;
+	/*
+	 * Some devices implement only pause (e.g. to get residuum) but no
+	 * resume. However cmd_pause is advertised as pause AND resume.
+	 */
+	caps->cmd_pause = !!(device->device_pause && device->device_resume);
 	caps->cmd_terminate = !!device->device_terminate_all;
 
 	return 0;
@@ -532,7 +543,7 @@ static struct dma_chan *private_candidate(const dma_cap_mask_t *mask,
 }
 
 /**
- * dma_request_slave_channel - try to get specific channel exclusively
+ * dma_get_slave_channel - try to get specific channel exclusively
  * @chan: target channel
  */
 struct dma_chan *dma_get_slave_channel(struct dma_chan *chan)
@@ -644,7 +655,7 @@ struct dma_chan *__dma_request_channel(const dma_cap_mask_t *mask,
 EXPORT_SYMBOL_GPL(__dma_request_channel);
 
 /**
- * dma_request_slave_channel - try to allocate an exclusive slave channel
+ * dma_request_slave_channel_reason - try to allocate an exclusive slave channel
  * @dev:	pointer to client device structure
  * @name:	slave channel name
  *
@@ -832,6 +843,8 @@ int dma_async_device_register(struct dma_device *device)
 		!device->device_prep_dma_pq);
 	BUG_ON(dma_has_cap(DMA_PQ_VAL, device->cap_mask) &&
 		!device->device_prep_dma_pq_val);
+	BUG_ON(dma_has_cap(DMA_MEMSET, device->cap_mask) &&
+		!device->device_prep_dma_memset);
 	BUG_ON(dma_has_cap(DMA_INTERRUPT, device->cap_mask) &&
 		!device->device_prep_dma_interrupt);
 	BUG_ON(dma_has_cap(DMA_SG, device->cap_mask) &&
diff --git a/drivers/dma/dmatest.c b/drivers/dma/dmatest.c
index 220ee49633e4..b8576fd6bd0e 100644
--- a/drivers/dma/dmatest.c
+++ b/drivers/dma/dmatest.c
@@ -120,7 +120,7 @@ static struct dmatest_info {
 
 static int dmatest_run_set(const char *val, const struct kernel_param *kp);
 static int dmatest_run_get(char *val, const struct kernel_param *kp);
-static struct kernel_param_ops run_ops = {
+static const struct kernel_param_ops run_ops = {
 	.set = dmatest_run_set,
 	.get = dmatest_run_get,
 };
@@ -195,7 +195,7 @@ static int dmatest_wait_get(char *val, const struct kernel_param *kp)
 	return param_get_bool(val, kp);
 }
 
-static struct kernel_param_ops wait_ops = {
+static const struct kernel_param_ops wait_ops = {
 	.get = dmatest_wait_get,
 	.set = param_set_bool,
 };
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
index bf09db7ca9ee..88853af69489 100644
--- a/drivers/dma/edma.c
+++ b/drivers/dma/edma.c
@@ -300,8 +300,7 @@ static int edma_dma_pause(struct dma_chan *chan)
 {
 	struct edma_chan *echan = to_edma_chan(chan);
 
-	/* Pause/Resume only allowed with cyclic mode */
-	if (!echan->edesc || !echan->edesc->cyclic)
+	if (!echan->edesc)
 		return -EINVAL;
 
 	edma_pause(echan->ch_num);
@@ -312,10 +311,6 @@ static int edma_dma_resume(struct dma_chan *chan)
 {
 	struct edma_chan *echan = to_edma_chan(chan);
 
-	/* Pause/Resume only allowed with cyclic mode */
-	if (!echan->edesc->cyclic)
-		return -EINVAL;
-
 	edma_resume(echan->ch_num);
 	return 0;
 }
diff --git a/drivers/dma/ep93xx_dma.c b/drivers/dma/ep93xx_dma.c
index 24e5290faa32..57ff46284f15 100644
--- a/drivers/dma/ep93xx_dma.c
+++ b/drivers/dma/ep93xx_dma.c
@@ -1364,7 +1364,7 @@ static int __init ep93xx_dma_probe(struct platform_device *pdev)
 	return ret;
 }
 
-static struct platform_device_id ep93xx_dma_driver_ids[] = {
+static const struct platform_device_id ep93xx_dma_driver_ids[] = {
 	{ "ep93xx-dma-m2p", 0 },
 	{ "ep93xx-dma-m2m", 1 },
 	{ },
diff --git a/drivers/dma/fsl-edma.c b/drivers/dma/fsl-edma.c
index 09e2842d15ec..915eec3cc279 100644
--- a/drivers/dma/fsl-edma.c
+++ b/drivers/dma/fsl-edma.c
@@ -881,10 +881,6 @@ static int fsl_edma_probe(struct platform_device *pdev)
 
 	}
 
-	ret = fsl_edma_irq_init(pdev, fsl_edma);
-	if (ret)
-		return ret;
-
 	fsl_edma->big_endian = of_property_read_bool(np, "big-endian");
 
 	INIT_LIST_HEAD(&fsl_edma->dma_dev.channels);
@@ -900,6 +896,11 @@ static int fsl_edma_probe(struct platform_device *pdev)
 		fsl_edma_chan_mux(fsl_chan, 0, false);
 	}
 
+	edma_writel(fsl_edma, ~0, fsl_edma->membase + EDMA_INTR);
+	ret = fsl_edma_irq_init(pdev, fsl_edma);
+	if (ret)
+		return ret;
+
 	dma_cap_set(DMA_PRIVATE, fsl_edma->dma_dev.cap_mask);
 	dma_cap_set(DMA_SLAVE, fsl_edma->dma_dev.cap_mask);
 	dma_cap_set(DMA_CYCLIC, fsl_edma->dma_dev.cap_mask);
diff --git a/drivers/dma/hsu/hsu.c b/drivers/dma/hsu/hsu.c
index 9b84def7a353..f42f71e37e73 100644
--- a/drivers/dma/hsu/hsu.c
+++ b/drivers/dma/hsu/hsu.c
@@ -384,7 +384,10 @@ static int hsu_dma_terminate_all(struct dma_chan *chan)
 	spin_lock_irqsave(&hsuc->vchan.lock, flags);
 
 	hsu_dma_stop_channel(hsuc);
-	hsuc->desc = NULL;
+	if (hsuc->desc) {
+		hsu_dma_desc_free(&hsuc->desc->vdesc);
+		hsuc->desc = NULL;
+	}
 
 	vchan_get_all_descriptors(&hsuc->vchan, &head);
 	spin_unlock_irqrestore(&hsuc->vchan.lock, flags);
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index eed405976ea9..865501fcc67d 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -193,7 +193,7 @@ struct imxdma_filter_data {
 	int			 request;
 };
 
-static struct platform_device_id imx_dma_devtype[] = {
+static const struct platform_device_id imx_dma_devtype[] = {
 	{
 		.name = "imx1-dma",
 		.driver_data = IMX1_DMA,
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index 62bbd79338e0..77b6aab04f47 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -420,7 +420,7 @@ static struct sdma_driver_data sdma_imx6q = {
 	.script_addrs = &sdma_script_imx6q,
 };
 
-static struct platform_device_id sdma_devtypes[] = {
+static const struct platform_device_id sdma_devtypes[] = {
 	{
 		.name = "imx25-sdma",
 		.driver_data = (unsigned long)&sdma_imx25,
diff --git a/drivers/dma/mic_x100_dma.c b/drivers/dma/mic_x100_dma.c
index 6de2e677be04..74d9db05a5ad 100644
--- a/drivers/dma/mic_x100_dma.c
+++ b/drivers/dma/mic_x100_dma.c
@@ -22,6 +22,7 @@
 #include <linux/module.h>
 #include <linux/io.h>
 #include <linux/seq_file.h>
+#include <linux/vmalloc.h>
 
 #include "mic_x100_dma.h"
 
diff --git a/drivers/dma/mmp_tdma.c b/drivers/dma/mmp_tdma.c
index 449e785def17..e683761e0f8f 100644
--- a/drivers/dma/mmp_tdma.c
+++ b/drivers/dma/mmp_tdma.c
@@ -657,7 +657,7 @@ static int mmp_tdma_probe(struct platform_device *pdev)
 	INIT_LIST_HEAD(&tdev->device.channels);
 
 	if (pdev->dev.of_node)
-		pool = of_get_named_gen_pool(pdev->dev.of_node, "asram", 0);
+		pool = of_gen_pool_get(pdev->dev.of_node, "asram", 0);
 	else
 		pool = sram_get_gpool("asram");
 	if (!pool) {
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index 1c56001df676..fbaf1ead2597 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -19,6 +19,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/spinlock.h>
 #include <linux/interrupt.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/memory.h>
 #include <linux/clk.h>
@@ -30,6 +31,11 @@
 #include "dmaengine.h"
 #include "mv_xor.h"
 
+enum mv_xor_mode {
+	XOR_MODE_IN_REG,
+	XOR_MODE_IN_DESC,
+};
+
 static void mv_xor_issue_pending(struct dma_chan *chan);
 
 #define to_mv_xor_chan(chan)		\
@@ -56,18 +62,30 @@ static void mv_desc_init(struct mv_xor_desc_slot *desc,
 	hw_desc->byte_count = byte_count;
 }
 
-static void mv_desc_set_next_desc(struct mv_xor_desc_slot *desc,
-				  u32 next_desc_addr)
+static void mv_desc_set_mode(struct mv_xor_desc_slot *desc)
 {
 	struct mv_xor_desc *hw_desc = desc->hw_desc;
-	BUG_ON(hw_desc->phy_next_desc);
-	hw_desc->phy_next_desc = next_desc_addr;
+
+	switch (desc->type) {
+	case DMA_XOR:
+	case DMA_INTERRUPT:
+		hw_desc->desc_command |= XOR_DESC_OPERATION_XOR;
+		break;
+	case DMA_MEMCPY:
+		hw_desc->desc_command |= XOR_DESC_OPERATION_MEMCPY;
+		break;
+	default:
+		BUG();
+		return;
+	}
 }
 
-static void mv_desc_clear_next_desc(struct mv_xor_desc_slot *desc)
+static void mv_desc_set_next_desc(struct mv_xor_desc_slot *desc,
+				  u32 next_desc_addr)
 {
 	struct mv_xor_desc *hw_desc = desc->hw_desc;
-	hw_desc->phy_next_desc = 0;
+	BUG_ON(hw_desc->phy_next_desc);
+	hw_desc->phy_next_desc = next_desc_addr;
 }
 
 static void mv_desc_set_src_addr(struct mv_xor_desc_slot *desc,
@@ -104,7 +122,7 @@ static u32 mv_chan_get_intr_cause(struct mv_xor_chan *chan)
 	return intr_cause;
 }
 
-static void mv_xor_device_clear_eoc_cause(struct mv_xor_chan *chan)
+static void mv_chan_clear_eoc_cause(struct mv_xor_chan *chan)
 {
 	u32 val;
 
@@ -114,14 +132,14 @@ static void mv_xor_device_clear_eoc_cause(struct mv_xor_chan *chan)
 	writel_relaxed(val, XOR_INTR_CAUSE(chan));
 }
 
-static void mv_xor_device_clear_err_status(struct mv_xor_chan *chan)
+static void mv_chan_clear_err_status(struct mv_xor_chan *chan)
 {
 	u32 val = 0xFFFF0000 >> (chan->idx * 16);
 	writel_relaxed(val, XOR_INTR_CAUSE(chan));
 }
 
-static void mv_set_mode(struct mv_xor_chan *chan,
-			       enum dma_transaction_type type)
+static void mv_chan_set_mode(struct mv_xor_chan *chan,
+			     enum dma_transaction_type type)
 {
 	u32 op_mode;
 	u32 config = readl_relaxed(XOR_CONFIG(chan));
@@ -144,6 +162,25 @@ static void mv_set_mode(struct mv_xor_chan *chan,
 	config &= ~0x7;
 	config |= op_mode;
 
+	if (IS_ENABLED(__BIG_ENDIAN))
+		config |= XOR_DESCRIPTOR_SWAP;
+	else
+		config &= ~XOR_DESCRIPTOR_SWAP;
+
+	writel_relaxed(config, XOR_CONFIG(chan));
+	chan->current_type = type;
+}
+
+static void mv_chan_set_mode_to_desc(struct mv_xor_chan *chan)
+{
+	u32 op_mode;
+	u32 config = readl_relaxed(XOR_CONFIG(chan));
+
+	op_mode = XOR_OPERATION_MODE_IN_DESC;
+
+	config &= ~0x7;
+	config |= op_mode;
+
 #if defined(__BIG_ENDIAN)
 	config |= XOR_DESCRIPTOR_SWAP;
 #else
@@ -151,7 +188,6 @@ static void mv_set_mode(struct mv_xor_chan *chan,
 #endif
 
 	writel_relaxed(config, XOR_CONFIG(chan));
-	chan->current_type = type;
 }
 
 static void mv_chan_activate(struct mv_xor_chan *chan)
@@ -171,28 +207,13 @@ static char mv_chan_is_busy(struct mv_xor_chan *chan)
 	return (state == 1) ? 1 : 0;
 }
 
-/**
- * mv_xor_free_slots - flags descriptor slots for reuse
- * @slot: Slot to free
- * Caller must hold &mv_chan->lock while calling this function
- */
-static void mv_xor_free_slots(struct mv_xor_chan *mv_chan,
-			      struct mv_xor_desc_slot *slot)
-{
-	dev_dbg(mv_chan_to_devp(mv_chan), "%s %d slot %p\n",
-		__func__, __LINE__, slot);
-
-	slot->slot_used = 0;
-
-}
-
 /*
- * mv_xor_start_new_chain - program the engine to operate on new chain headed by
- * sw_desc
+ * mv_chan_start_new_chain - program the engine to operate on new
+ * chain headed by sw_desc
  * Caller must hold &mv_chan->lock while calling this function
  */
-static void mv_xor_start_new_chain(struct mv_xor_chan *mv_chan,
-				   struct mv_xor_desc_slot *sw_desc)
+static void mv_chan_start_new_chain(struct mv_xor_chan *mv_chan,
+				    struct mv_xor_desc_slot *sw_desc)
 {
 	dev_dbg(mv_chan_to_devp(mv_chan), "%s %d: sw_desc %p\n",
 		__func__, __LINE__, sw_desc);
@@ -205,8 +226,9 @@ static void mv_xor_start_new_chain(struct mv_xor_chan *mv_chan,
 }
 
 static dma_cookie_t
-mv_xor_run_tx_complete_actions(struct mv_xor_desc_slot *desc,
-	struct mv_xor_chan *mv_chan, dma_cookie_t cookie)
+mv_desc_run_tx_complete_actions(struct mv_xor_desc_slot *desc,
+				struct mv_xor_chan *mv_chan,
+				dma_cookie_t cookie)
 {
 	BUG_ON(desc->async_tx.cookie < 0);
 
@@ -230,93 +252,110 @@ mv_xor_run_tx_complete_actions(struct mv_xor_desc_slot *desc,
 }
 
 static int
-mv_xor_clean_completed_slots(struct mv_xor_chan *mv_chan)
+mv_chan_clean_completed_slots(struct mv_xor_chan *mv_chan)
 {
 	struct mv_xor_desc_slot *iter, *_iter;
 
 	dev_dbg(mv_chan_to_devp(mv_chan), "%s %d\n", __func__, __LINE__);
 	list_for_each_entry_safe(iter, _iter, &mv_chan->completed_slots,
-				 completed_node) {
+				 node) {
 
-		if (async_tx_test_ack(&iter->async_tx)) {
-			list_del(&iter->completed_node);
-			mv_xor_free_slots(mv_chan, iter);
-		}
+		if (async_tx_test_ack(&iter->async_tx))
+			list_move_tail(&iter->node, &mv_chan->free_slots);
 	}
 	return 0;
 }
 
 static int
-mv_xor_clean_slot(struct mv_xor_desc_slot *desc,
-	struct mv_xor_chan *mv_chan)
+mv_desc_clean_slot(struct mv_xor_desc_slot *desc,
+		   struct mv_xor_chan *mv_chan)
 {
 	dev_dbg(mv_chan_to_devp(mv_chan), "%s %d: desc %p flags %d\n",
 		__func__, __LINE__, desc, desc->async_tx.flags);
-	list_del(&desc->chain_node);
+
 	/* the client is allowed to attach dependent operations
 	 * until 'ack' is set
 	 */
-	if (!async_tx_test_ack(&desc->async_tx)) {
+	if (!async_tx_test_ack(&desc->async_tx))
 		/* move this slot to the completed_slots */
-		list_add_tail(&desc->completed_node, &mv_chan->completed_slots);
-		return 0;
-	}
+		list_move_tail(&desc->node, &mv_chan->completed_slots);
+	else
+		list_move_tail(&desc->node, &mv_chan->free_slots);
 
-	mv_xor_free_slots(mv_chan, desc);
 	return 0;
 }
 
 /* This function must be called with the mv_xor_chan spinlock held */
-static void mv_xor_slot_cleanup(struct mv_xor_chan *mv_chan)
+static void mv_chan_slot_cleanup(struct mv_xor_chan *mv_chan)
 {
 	struct mv_xor_desc_slot *iter, *_iter;
 	dma_cookie_t cookie = 0;
 	int busy = mv_chan_is_busy(mv_chan);
 	u32 current_desc = mv_chan_get_current_desc(mv_chan);
-	int seen_current = 0;
+	int current_cleaned = 0;
+	struct mv_xor_desc *hw_desc;
 
 	dev_dbg(mv_chan_to_devp(mv_chan), "%s %d\n", __func__, __LINE__);
 	dev_dbg(mv_chan_to_devp(mv_chan), "current_desc %x\n", current_desc);
-	mv_xor_clean_completed_slots(mv_chan);
+	mv_chan_clean_completed_slots(mv_chan);
 
 	/* free completed slots from the chain starting with
 	 * the oldest descriptor
 	 */
 
 	list_for_each_entry_safe(iter, _iter, &mv_chan->chain,
-					chain_node) {
-		prefetch(_iter);
-		prefetch(&_iter->async_tx);
+				 node) {
 
-		/* do not advance past the current descriptor loaded into the
-		 * hardware channel, subsequent descriptors are either in
-		 * process or have not been submitted
-		 */
-		if (seen_current)
-			break;
+		/* clean finished descriptors */
+		hw_desc = iter->hw_desc;
+		if (hw_desc->status & XOR_DESC_SUCCESS) {
+			cookie = mv_desc_run_tx_complete_actions(iter, mv_chan,
+								 cookie);
 
-		/* stop the search if we reach the current descriptor and the
-		 * channel is busy
-		 */
-		if (iter->async_tx.phys == current_desc) {
-			seen_current = 1;
-			if (busy)
+			/* done processing desc, clean slot */
+			mv_desc_clean_slot(iter, mv_chan);
+
+			/* break if we did cleaned the current */
+			if (iter->async_tx.phys == current_desc) {
+				current_cleaned = 1;
 				break;
+			}
+		} else {
+			if (iter->async_tx.phys == current_desc) {
+				current_cleaned = 0;
+				break;
+			}
 		}
-
-		cookie = mv_xor_run_tx_complete_actions(iter, mv_chan, cookie);
-
-		if (mv_xor_clean_slot(iter, mv_chan))
-			break;
 	}
 
 	if ((busy == 0) && !list_empty(&mv_chan->chain)) {
-		struct mv_xor_desc_slot *chain_head;
-		chain_head = list_entry(mv_chan->chain.next,
-					struct mv_xor_desc_slot,
-					chain_node);
-
-		mv_xor_start_new_chain(mv_chan, chain_head);
+		if (current_cleaned) {
+			/*
+			 * current descriptor cleaned and removed, run
+			 * from list head
+			 */
+			iter = list_entry(mv_chan->chain.next,
+					  struct mv_xor_desc_slot,
+					  node);
+			mv_chan_start_new_chain(mv_chan, iter);
+		} else {
+			if (!list_is_last(&iter->node, &mv_chan->chain)) {
+				/*
+				 * descriptors are still waiting after
+				 * current, trigger them
+				 */
+				iter = list_entry(iter->node.next,
+						  struct mv_xor_desc_slot,
+						  node);
+				mv_chan_start_new_chain(mv_chan, iter);
+			} else {
+				/*
+				 * some descriptors are still waiting
+				 * to be cleaned
+				 */
+				tasklet_schedule(&mv_chan->irq_tasklet);
+			}
+		}
 	}
 
 	if (cookie > 0)
@@ -328,56 +367,35 @@ static void mv_xor_tasklet(unsigned long data)
 	struct mv_xor_chan *chan = (struct mv_xor_chan *) data;
 
 	spin_lock_bh(&chan->lock);
-	mv_xor_slot_cleanup(chan);
+	mv_chan_slot_cleanup(chan);
 	spin_unlock_bh(&chan->lock);
 }
 
 static struct mv_xor_desc_slot *
-mv_xor_alloc_slot(struct mv_xor_chan *mv_chan)
+mv_chan_alloc_slot(struct mv_xor_chan *mv_chan)
 {
-	struct mv_xor_desc_slot *iter, *_iter;
-	int retry = 0;
+	struct mv_xor_desc_slot *iter;
 
-	/* start search from the last allocated descrtiptor
-	 * if a contiguous allocation can not be found start searching
-	 * from the beginning of the list
-	 */
-retry:
-	if (retry == 0)
-		iter = mv_chan->last_used;
-	else
-		iter = list_entry(&mv_chan->all_slots,
-			struct mv_xor_desc_slot,
-			slot_node);
-
-	list_for_each_entry_safe_continue(
-		iter, _iter, &mv_chan->all_slots, slot_node) {
-
-		prefetch(_iter);
-		prefetch(&_iter->async_tx);
-		if (iter->slot_used) {
-			/* give up after finding the first busy slot
-			 * on the second pass through the list
-			 */
-			if (retry)
-				break;
-			continue;
-		}
+	spin_lock_bh(&mv_chan->lock);
+
+	if (!list_empty(&mv_chan->free_slots)) {
+		iter = list_first_entry(&mv_chan->free_slots,
+					struct mv_xor_desc_slot,
+					node);
+
+		list_move_tail(&iter->node, &mv_chan->allocated_slots);
+
+		spin_unlock_bh(&mv_chan->lock);
 
 		/* pre-ack descriptor */
 		async_tx_ack(&iter->async_tx);
-
-		iter->slot_used = 1;
-		INIT_LIST_HEAD(&iter->chain_node);
 		iter->async_tx.cookie = -EBUSY;
-		mv_chan->last_used = iter;
-		mv_desc_clear_next_desc(iter);
 
 		return iter;
 
 	}
-	if (!retry++)
-		goto retry;
+
+	spin_unlock_bh(&mv_chan->lock);
 
 	/* try to free some slots if the allocation fails */
 	tasklet_schedule(&mv_chan->irq_tasklet);
@@ -403,14 +421,14 @@ mv_xor_tx_submit(struct dma_async_tx_descriptor *tx)
 	cookie = dma_cookie_assign(tx);
 
 	if (list_empty(&mv_chan->chain))
-		list_add_tail(&sw_desc->chain_node, &mv_chan->chain);
+		list_move_tail(&sw_desc->node, &mv_chan->chain);
 	else {
 		new_hw_chain = 0;
 
 		old_chain_tail = list_entry(mv_chan->chain.prev,
 					    struct mv_xor_desc_slot,
-					    chain_node);
-		list_add_tail(&sw_desc->chain_node, &mv_chan->chain);
+					    node);
+		list_move_tail(&sw_desc->node, &mv_chan->chain);
 
 		dev_dbg(mv_chan_to_devp(mv_chan), "Append to last desc %pa\n",
 			&old_chain_tail->async_tx.phys);
@@ -431,7 +449,7 @@ mv_xor_tx_submit(struct dma_async_tx_descriptor *tx)
 	}
 
 	if (new_hw_chain)
-		mv_xor_start_new_chain(mv_chan, sw_desc);
+		mv_chan_start_new_chain(mv_chan, sw_desc);
 
 	spin_unlock_bh(&mv_chan->lock);
 
@@ -463,26 +481,20 @@ static int mv_xor_alloc_chan_resources(struct dma_chan *chan)
 
 		dma_async_tx_descriptor_init(&slot->async_tx, chan);
 		slot->async_tx.tx_submit = mv_xor_tx_submit;
-		INIT_LIST_HEAD(&slot->chain_node);
-		INIT_LIST_HEAD(&slot->slot_node);
+		INIT_LIST_HEAD(&slot->node);
 		dma_desc = mv_chan->dma_desc_pool;
 		slot->async_tx.phys = dma_desc + idx * MV_XOR_SLOT_SIZE;
 		slot->idx = idx++;
 
 		spin_lock_bh(&mv_chan->lock);
 		mv_chan->slots_allocated = idx;
-		list_add_tail(&slot->slot_node, &mv_chan->all_slots);
+		list_add_tail(&slot->node, &mv_chan->free_slots);
 		spin_unlock_bh(&mv_chan->lock);
 	}
 
-	if (mv_chan->slots_allocated && !mv_chan->last_used)
-		mv_chan->last_used = list_entry(mv_chan->all_slots.next,
-					struct mv_xor_desc_slot,
-					slot_node);
-
 	dev_dbg(mv_chan_to_devp(mv_chan),
-		"allocated %d descriptor slots last_used: %p\n",
-		mv_chan->slots_allocated, mv_chan->last_used);
+		"allocated %d descriptor slots\n",
+		mv_chan->slots_allocated);
 
 	return mv_chan->slots_allocated ? : -ENOMEM;
 }
@@ -503,16 +515,17 @@ mv_xor_prep_dma_xor(struct dma_chan *chan, dma_addr_t dest, dma_addr_t *src,
 		"%s src_cnt: %d len: %u dest %pad flags: %ld\n",
 		__func__, src_cnt, len, &dest, flags);
 
-	spin_lock_bh(&mv_chan->lock);
-	sw_desc = mv_xor_alloc_slot(mv_chan);
+	sw_desc = mv_chan_alloc_slot(mv_chan);
 	if (sw_desc) {
 		sw_desc->type = DMA_XOR;
 		sw_desc->async_tx.flags = flags;
 		mv_desc_init(sw_desc, dest, len, flags);
+		if (mv_chan->op_in_desc == XOR_MODE_IN_DESC)
+			mv_desc_set_mode(sw_desc);
 		while (src_cnt--)
 			mv_desc_set_src_addr(sw_desc, src_cnt, src[src_cnt]);
 	}
-	spin_unlock_bh(&mv_chan->lock);
+
 	dev_dbg(mv_chan_to_devp(mv_chan),
 		"%s sw_desc %p async_tx %p \n",
 		__func__, sw_desc, &sw_desc->async_tx);
@@ -556,25 +569,29 @@ static void mv_xor_free_chan_resources(struct dma_chan *chan)
 
 	spin_lock_bh(&mv_chan->lock);
 
-	mv_xor_slot_cleanup(mv_chan);
+	mv_chan_slot_cleanup(mv_chan);
 
 	list_for_each_entry_safe(iter, _iter, &mv_chan->chain,
-					chain_node) {
+					node) {
 		in_use_descs++;
-		list_del(&iter->chain_node);
+		list_move_tail(&iter->node, &mv_chan->free_slots);
 	}
 	list_for_each_entry_safe(iter, _iter, &mv_chan->completed_slots,
-				 completed_node) {
+				 node) {
 		in_use_descs++;
-		list_del(&iter->completed_node);
+		list_move_tail(&iter->node, &mv_chan->free_slots);
+	}
+	list_for_each_entry_safe(iter, _iter, &mv_chan->allocated_slots,
+				 node) {
+		in_use_descs++;
+		list_move_tail(&iter->node, &mv_chan->free_slots);
 	}
 	list_for_each_entry_safe_reverse(
-		iter, _iter, &mv_chan->all_slots, slot_node) {
-		list_del(&iter->slot_node);
+		iter, _iter, &mv_chan->free_slots, node) {
+		list_del(&iter->node);
 		kfree(iter);
 		mv_chan->slots_allocated--;
 	}
-	mv_chan->last_used = NULL;
 
 	dev_dbg(mv_chan_to_devp(mv_chan), "%s slots_allocated %d\n",
 		__func__, mv_chan->slots_allocated);
@@ -603,13 +620,13 @@ static enum dma_status mv_xor_status(struct dma_chan *chan,
 		return ret;
 
 	spin_lock_bh(&mv_chan->lock);
-	mv_xor_slot_cleanup(mv_chan);
+	mv_chan_slot_cleanup(mv_chan);
 	spin_unlock_bh(&mv_chan->lock);
 
 	return dma_cookie_status(chan, cookie, txstate);
 }
 
-static void mv_dump_xor_regs(struct mv_xor_chan *chan)
+static void mv_chan_dump_regs(struct mv_xor_chan *chan)
 {
 	u32 val;
 
@@ -632,8 +649,8 @@ static void mv_dump_xor_regs(struct mv_xor_chan *chan)
 	dev_err(mv_chan_to_devp(chan), "error addr   0x%08x\n", val);
 }
 
-static void mv_xor_err_interrupt_handler(struct mv_xor_chan *chan,
-					 u32 intr_cause)
+static void mv_chan_err_interrupt_handler(struct mv_xor_chan *chan,
+					  u32 intr_cause)
 {
 	if (intr_cause & XOR_INT_ERR_DECODE) {
 		dev_dbg(mv_chan_to_devp(chan), "ignoring address decode error\n");
@@ -643,7 +660,7 @@ static void mv_xor_err_interrupt_handler(struct mv_xor_chan *chan,
 	dev_err(mv_chan_to_devp(chan), "error on chan %d. intr cause 0x%08x\n",
 		chan->idx, intr_cause);
 
-	mv_dump_xor_regs(chan);
+	mv_chan_dump_regs(chan);
 	WARN_ON(1);
 }
 
@@ -655,11 +672,11 @@ static irqreturn_t mv_xor_interrupt_handler(int irq, void *data)
 	dev_dbg(mv_chan_to_devp(chan), "intr cause %x\n", intr_cause);
 
 	if (intr_cause & XOR_INTR_ERRORS)
-		mv_xor_err_interrupt_handler(chan, intr_cause);
+		mv_chan_err_interrupt_handler(chan, intr_cause);
 
 	tasklet_schedule(&chan->irq_tasklet);
 
-	mv_xor_device_clear_eoc_cause(chan);
+	mv_chan_clear_eoc_cause(chan);
 
 	return IRQ_HANDLED;
 }
@@ -678,7 +695,7 @@ static void mv_xor_issue_pending(struct dma_chan *chan)
  * Perform a transaction to verify the HW works.
  */
 
-static int mv_xor_memcpy_self_test(struct mv_xor_chan *mv_chan)
+static int mv_chan_memcpy_self_test(struct mv_xor_chan *mv_chan)
 {
 	int i, ret;
 	void *src, *dest;
@@ -787,7 +804,7 @@ out:
 
 #define MV_XOR_NUM_SRC_TEST 4 /* must be <= 15 */
 static int
-mv_xor_xor_self_test(struct mv_xor_chan *mv_chan)
+mv_chan_xor_self_test(struct mv_xor_chan *mv_chan)
 {
 	int i, src_idx, ret;
 	struct page *dest;
@@ -951,7 +968,7 @@ static int mv_xor_channel_remove(struct mv_xor_chan *mv_chan)
 static struct mv_xor_chan *
 mv_xor_channel_add(struct mv_xor_device *xordev,
 		   struct platform_device *pdev,
-		   int idx, dma_cap_mask_t cap_mask, int irq)
+		   int idx, dma_cap_mask_t cap_mask, int irq, int op_in_desc)
 {
 	int ret = 0;
 	struct mv_xor_chan *mv_chan;
@@ -963,6 +980,7 @@ mv_xor_channel_add(struct mv_xor_device *xordev,
 
 	mv_chan->idx = idx;
 	mv_chan->irq = irq;
+	mv_chan->op_in_desc = op_in_desc;
 
 	dma_dev = &mv_chan->dmadev;
 
@@ -1014,7 +1032,7 @@ mv_xor_channel_add(struct mv_xor_device *xordev,
 		     mv_chan);
 
 	/* clear errors before enabling interrupts */
-	mv_xor_device_clear_err_status(mv_chan);
+	mv_chan_clear_err_status(mv_chan);
 
 	ret = request_irq(mv_chan->irq, mv_xor_interrupt_handler,
 			  0, dev_name(&pdev->dev), mv_chan);
@@ -1023,32 +1041,37 @@ mv_xor_channel_add(struct mv_xor_device *xordev,
 
 	mv_chan_unmask_interrupts(mv_chan);
 
-	mv_set_mode(mv_chan, DMA_XOR);
+	if (mv_chan->op_in_desc == XOR_MODE_IN_DESC)
+		mv_chan_set_mode_to_desc(mv_chan);
+	else
+		mv_chan_set_mode(mv_chan, DMA_XOR);
 
 	spin_lock_init(&mv_chan->lock);
 	INIT_LIST_HEAD(&mv_chan->chain);
 	INIT_LIST_HEAD(&mv_chan->completed_slots);
-	INIT_LIST_HEAD(&mv_chan->all_slots);
+	INIT_LIST_HEAD(&mv_chan->free_slots);
+	INIT_LIST_HEAD(&mv_chan->allocated_slots);
 	mv_chan->dmachan.device = dma_dev;
 	dma_cookie_init(&mv_chan->dmachan);
 
 	list_add_tail(&mv_chan->dmachan.device_node, &dma_dev->channels);
 
 	if (dma_has_cap(DMA_MEMCPY, dma_dev->cap_mask)) {
-		ret = mv_xor_memcpy_self_test(mv_chan);
+		ret = mv_chan_memcpy_self_test(mv_chan);
 		dev_dbg(&pdev->dev, "memcpy self test returned %d\n", ret);
 		if (ret)
 			goto err_free_irq;
 	}
 
 	if (dma_has_cap(DMA_XOR, dma_dev->cap_mask)) {
-		ret = mv_xor_xor_self_test(mv_chan);
+		ret = mv_chan_xor_self_test(mv_chan);
 		dev_dbg(&pdev->dev, "xor self test returned %d\n", ret);
 		if (ret)
 			goto err_free_irq;
 	}
 
-	dev_info(&pdev->dev, "Marvell XOR: ( %s%s%s)\n",
+	dev_info(&pdev->dev, "Marvell XOR (%s): ( %s%s%s)\n",
+		 mv_chan->op_in_desc ? "Descriptor Mode" : "Registers Mode",
 		 dma_has_cap(DMA_XOR, dma_dev->cap_mask) ? "xor " : "",
 		 dma_has_cap(DMA_MEMCPY, dma_dev->cap_mask) ? "cpy " : "",
 		 dma_has_cap(DMA_INTERRUPT, dma_dev->cap_mask) ? "intr " : "");
@@ -1097,6 +1120,13 @@ mv_xor_conf_mbus_windows(struct mv_xor_device *xordev,
 	writel(0, base + WINDOW_OVERRIDE_CTRL(1));
 }
 
+static const struct of_device_id mv_xor_dt_ids[] = {
+	{ .compatible = "marvell,orion-xor", .data = (void *)XOR_MODE_IN_REG },
+	{ .compatible = "marvell,armada-380-xor", .data = (void *)XOR_MODE_IN_DESC },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mv_xor_dt_ids);
+
 static int mv_xor_probe(struct platform_device *pdev)
 {
 	const struct mbus_dram_target_info *dram;
@@ -1104,6 +1134,7 @@ static int mv_xor_probe(struct platform_device *pdev)
 	struct mv_xor_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct resource *res;
 	int i, ret;
+	int op_in_desc;
 
 	dev_notice(&pdev->dev, "Marvell shared XOR driver\n");
 
@@ -1148,11 +1179,15 @@ static int mv_xor_probe(struct platform_device *pdev)
 	if (pdev->dev.of_node) {
 		struct device_node *np;
 		int i = 0;
+		const struct of_device_id *of_id =
+			of_match_device(mv_xor_dt_ids,
+					&pdev->dev);
 
 		for_each_child_of_node(pdev->dev.of_node, np) {
 			struct mv_xor_chan *chan;
 			dma_cap_mask_t cap_mask;
 			int irq;
+			op_in_desc = (int)of_id->data;
 
 			dma_cap_zero(cap_mask);
 			if (of_property_read_bool(np, "dmacap,memcpy"))
@@ -1169,7 +1204,7 @@ static int mv_xor_probe(struct platform_device *pdev)
 			}
 
 			chan = mv_xor_channel_add(xordev, pdev, i,
-						  cap_mask, irq);
+						  cap_mask, irq, op_in_desc);
 			if (IS_ERR(chan)) {
 				ret = PTR_ERR(chan);
 				irq_dispose_mapping(irq);
@@ -1198,7 +1233,8 @@ static int mv_xor_probe(struct platform_device *pdev)
 			}
 
 			chan = mv_xor_channel_add(xordev, pdev, i,
-						  cd->cap_mask, irq);
+						  cd->cap_mask, irq,
+						  XOR_MODE_IN_REG);
 			if (IS_ERR(chan)) {
 				ret = PTR_ERR(chan);
 				goto err_channel_add;
@@ -1244,14 +1280,6 @@ static int mv_xor_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_OF
-static const struct of_device_id mv_xor_dt_ids[] = {
-       { .compatible = "marvell,orion-xor", },
-       {},
-};
-MODULE_DEVICE_TABLE(of, mv_xor_dt_ids);
-#endif
-
 static struct platform_driver mv_xor_driver = {
 	.probe		= mv_xor_probe,
 	.remove		= mv_xor_remove,
diff --git a/drivers/dma/mv_xor.h b/drivers/dma/mv_xor.h
index 91958dba39a2..b7455b42137b 100644
--- a/drivers/dma/mv_xor.h
+++ b/drivers/dma/mv_xor.h
@@ -19,7 +19,7 @@
 #include <linux/dmaengine.h>
 #include <linux/interrupt.h>
 
-#define MV_XOR_POOL_SIZE		PAGE_SIZE
+#define MV_XOR_POOL_SIZE		(MV_XOR_SLOT_SIZE * 3072)
 #define MV_XOR_SLOT_SIZE		64
 #define MV_XOR_THRESHOLD		1
 #define MV_XOR_MAX_CHANNELS             2
@@ -30,7 +30,13 @@
 /* Values for the XOR_CONFIG register */
 #define XOR_OPERATION_MODE_XOR		0
 #define XOR_OPERATION_MODE_MEMCPY	2
+#define XOR_OPERATION_MODE_IN_DESC      7
 #define XOR_DESCRIPTOR_SWAP		BIT(14)
+#define XOR_DESC_SUCCESS		0x40000000
+
+#define XOR_DESC_OPERATION_XOR          (0 << 24)
+#define XOR_DESC_OPERATION_CRC32C       (1 << 24)
+#define XOR_DESC_OPERATION_MEMCPY       (2 << 24)
 
 #define XOR_DESC_DMA_OWNED		BIT(31)
 #define XOR_DESC_EOD_INT_EN		BIT(31)
@@ -88,13 +94,14 @@ struct mv_xor_device {
  * @mmr_base: memory mapped register base
  * @idx: the index of the xor channel
  * @chain: device chain view of the descriptors
+ * @free_slots: free slots usable by the channel
+ * @allocated_slots: slots allocated by the driver
  * @completed_slots: slots completed by HW but still need to be acked
  * @device: parent device
  * @common: common dmaengine channel object members
- * @last_used: place holder for allocation to continue from where it left off
- * @all_slots: complete domain of slots usable by the channel
  * @slots_allocated: records the actual size of the descriptor slot pool
  * @irq_tasklet: bottom half where mv_xor_slot_cleanup runs
+ * @op_in_desc: new mode of driver, each op is writen to descriptor.
  */
 struct mv_xor_chan {
 	int			pending;
@@ -105,16 +112,17 @@ struct mv_xor_chan {
 	int                     irq;
 	enum dma_transaction_type	current_type;
 	struct list_head	chain;
+	struct list_head	free_slots;
+	struct list_head	allocated_slots;
 	struct list_head	completed_slots;
 	dma_addr_t		dma_desc_pool;
 	void			*dma_desc_pool_virt;
 	size_t                  pool_size;
 	struct dma_device	dmadev;
 	struct dma_chan		dmachan;
-	struct mv_xor_desc_slot	*last_used;
-	struct list_head	all_slots;
 	int			slots_allocated;
 	struct tasklet_struct	irq_tasklet;
+	int                     op_in_desc;
 	char			dummy_src[MV_XOR_MIN_BYTE_COUNT];
 	char			dummy_dst[MV_XOR_MIN_BYTE_COUNT];
 	dma_addr_t		dummy_src_addr, dummy_dst_addr;
@@ -122,9 +130,7 @@ struct mv_xor_chan {
 
 /**
  * struct mv_xor_desc_slot - software descriptor
- * @slot_node: node on the mv_xor_chan.all_slots list
- * @chain_node: node on the mv_xor_chan.chain list
- * @completed_node: node on the mv_xor_chan.completed_slots list
+ * @node: node on the mv_xor_chan lists
  * @hw_desc: virtual address of the hardware descriptor chain
  * @phys: hardware address of the hardware descriptor chain
  * @slot_used: slot in use or not
@@ -133,12 +139,9 @@ struct mv_xor_chan {
  * @async_tx: support for the async_tx api
  */
 struct mv_xor_desc_slot {
-	struct list_head	slot_node;
-	struct list_head	chain_node;
-	struct list_head	completed_node;
+	struct list_head	node;
 	enum dma_transaction_type	type;
 	void			*hw_desc;
-	u16			slot_used;
 	u16			idx;
 	struct dma_async_tx_descriptor	async_tx;
 };
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index 829ec686dac3..60de35251da5 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -170,7 +170,7 @@ static struct mxs_dma_type mxs_dma_types[] = {
 	}
 };
 
-static struct platform_device_id mxs_dma_ids[] = {
+static const struct platform_device_id mxs_dma_ids[] = {
 	{
 		.name = "imx23-dma-apbh",
 		.driver_data = (kernel_ulong_t) &mxs_dma_types[0],
diff --git a/drivers/dma/nbpfaxi.c b/drivers/dma/nbpfaxi.c
index 88b77c98365d..2b5a198ac77e 100644
--- a/drivers/dma/nbpfaxi.c
+++ b/drivers/dma/nbpfaxi.c
@@ -1455,7 +1455,7 @@ static int nbpf_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id nbpf_ids[] = {
+static const struct platform_device_id nbpf_ids[] = {
 	{"nbpfaxi64dmac1b4",	(kernel_ulong_t)&nbpf_cfg[NBPF1B4]},
 	{"nbpfaxi64dmac1b8",	(kernel_ulong_t)&nbpf_cfg[NBPF1B8]},
 	{"nbpfaxi64dmac1b16",	(kernel_ulong_t)&nbpf_cfg[NBPF1B16]},
diff --git a/drivers/dma/of-dma.c b/drivers/dma/of-dma.c
index cbd4a8aff120..1e1f2986eba8 100644
--- a/drivers/dma/of-dma.c
+++ b/drivers/dma/of-dma.c
@@ -45,6 +45,50 @@ static struct of_dma *of_dma_find_controller(struct of_phandle_args *dma_spec)
 }
 
 /**
+ * of_dma_router_xlate - translation function for router devices
+ * @dma_spec:	pointer to DMA specifier as found in the device tree
+ * @of_dma:	pointer to DMA controller data (router information)
+ *
+ * The function creates new dma_spec to be passed to the router driver's
+ * of_dma_route_allocate() function to prepare a dma_spec which will be used
+ * to request channel from the real DMA controller.
+ */
+static struct dma_chan *of_dma_router_xlate(struct of_phandle_args *dma_spec,
+					    struct of_dma *ofdma)
+{
+	struct dma_chan		*chan;
+	struct of_dma		*ofdma_target;
+	struct of_phandle_args	dma_spec_target;
+	void			*route_data;
+
+	/* translate the request for the real DMA controller */
+	memcpy(&dma_spec_target, dma_spec, sizeof(dma_spec_target));
+	route_data = ofdma->of_dma_route_allocate(&dma_spec_target, ofdma);
+	if (IS_ERR(route_data))
+		return NULL;
+
+	ofdma_target = of_dma_find_controller(&dma_spec_target);
+	if (!ofdma_target)
+		return NULL;
+
+	chan = ofdma_target->of_dma_xlate(&dma_spec_target, ofdma_target);
+	if (chan) {
+		chan->router = ofdma->dma_router;
+		chan->route_data = route_data;
+	} else {
+		ofdma->dma_router->route_free(ofdma->dma_router->dev,
+					      route_data);
+	}
+
+	/*
+	 * Need to put the node back since the ofdma->of_dma_route_allocate
+	 * has taken it for generating the new, translated dma_spec
+	 */
+	of_node_put(dma_spec_target.np);
+	return chan;
+}
+
+/**
  * of_dma_controller_register - Register a DMA controller to DT DMA helpers
  * @np:			device node of DMA controller
  * @of_dma_xlate:	translation function which converts a phandle
@@ -110,6 +154,51 @@ void of_dma_controller_free(struct device_node *np)
 EXPORT_SYMBOL_GPL(of_dma_controller_free);
 
 /**
+ * of_dma_router_register - Register a DMA router to DT DMA helpers as a
+ *			    controller
+ * @np:				device node of DMA router
+ * @of_dma_route_allocate:	setup function for the router which need to
+ *				modify the dma_spec for the DMA controller to
+ *				use and to set up the requested route.
+ * @dma_router:			pointer to dma_router structure to be used when
+ *				the route need to be free up.
+ *
+ * Returns 0 on success or appropriate errno value on error.
+ *
+ * Allocated memory should be freed with appropriate of_dma_controller_free()
+ * call.
+ */
+int of_dma_router_register(struct device_node *np,
+			   void *(*of_dma_route_allocate)
+			   (struct of_phandle_args *, struct of_dma *),
+			   struct dma_router *dma_router)
+{
+	struct of_dma	*ofdma;
+
+	if (!np || !of_dma_route_allocate || !dma_router) {
+		pr_err("%s: not enough information provided\n", __func__);
+		return -EINVAL;
+	}
+
+	ofdma = kzalloc(sizeof(*ofdma), GFP_KERNEL);
+	if (!ofdma)
+		return -ENOMEM;
+
+	ofdma->of_node = np;
+	ofdma->of_dma_xlate = of_dma_router_xlate;
+	ofdma->of_dma_route_allocate = of_dma_route_allocate;
+	ofdma->dma_router = dma_router;
+
+	/* Now queue of_dma controller structure in list */
+	mutex_lock(&of_dma_lock);
+	list_add_tail(&ofdma->of_dma_controllers, &of_dma_list);
+	mutex_unlock(&of_dma_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(of_dma_router_register);
+
+/**
  * of_dma_match_channel - Check if a DMA specifier matches name
  * @np:		device node to look for DMA channels
  * @name:	channel name to be matched
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index 167dbaf65742..249445c8a4c6 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -22,6 +22,9 @@
 
 #include "virt-dma.h"
 
+#define OMAP_SDMA_REQUESTS	127
+#define OMAP_SDMA_CHANNELS	32
+
 struct omap_dmadev {
 	struct dma_device ddev;
 	spinlock_t lock;
@@ -31,9 +34,10 @@ struct omap_dmadev {
 	const struct omap_dma_reg *reg_map;
 	struct omap_system_dma_plat_info *plat;
 	bool legacy;
+	unsigned dma_requests;
 	spinlock_t irq_lock;
 	uint32_t irq_enable_mask;
-	struct omap_chan *lch_map[32];
+	struct omap_chan *lch_map[OMAP_SDMA_CHANNELS];
 };
 
 struct omap_chan {
@@ -362,7 +366,7 @@ static void omap_dma_start_sg(struct omap_chan *c, struct omap_desc *d,
 	struct omap_sg *sg = d->sg + idx;
 	unsigned cxsa, cxei, cxfi;
 
-	if (d->dir == DMA_DEV_TO_MEM) {
+	if (d->dir == DMA_DEV_TO_MEM || d->dir == DMA_MEM_TO_MEM) {
 		cxsa = CDSA;
 		cxei = CDEI;
 		cxfi = CDFI;
@@ -408,7 +412,7 @@ static void omap_dma_start_desc(struct omap_chan *c)
 	if (dma_omap1())
 		omap_dma_chan_write(c, CCR2, d->ccr >> 16);
 
-	if (d->dir == DMA_DEV_TO_MEM) {
+	if (d->dir == DMA_DEV_TO_MEM || d->dir == DMA_MEM_TO_MEM) {
 		cxsa = CSSA;
 		cxei = CSEI;
 		cxfi = CSFI;
@@ -589,6 +593,7 @@ static void omap_dma_free_chan_resources(struct dma_chan *chan)
 	omap_free_dma(c->dma_ch);
 
 	dev_dbg(od->ddev.dev, "freeing channel for %u\n", c->dma_sig);
+	c->dma_sig = 0;
 }
 
 static size_t omap_dma_sg_size(struct omap_sg *sg)
@@ -948,6 +953,51 @@ static struct dma_async_tx_descriptor *omap_dma_prep_dma_cyclic(
 	return vchan_tx_prep(&c->vc, &d->vd, flags);
 }
 
+static struct dma_async_tx_descriptor *omap_dma_prep_dma_memcpy(
+	struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
+	size_t len, unsigned long tx_flags)
+{
+	struct omap_chan *c = to_omap_dma_chan(chan);
+	struct omap_desc *d;
+	uint8_t data_type;
+
+	d = kzalloc(sizeof(*d) + sizeof(d->sg[0]), GFP_ATOMIC);
+	if (!d)
+		return NULL;
+
+	data_type = __ffs((src | dest | len));
+	if (data_type > CSDP_DATA_TYPE_32)
+		data_type = CSDP_DATA_TYPE_32;
+
+	d->dir = DMA_MEM_TO_MEM;
+	d->dev_addr = src;
+	d->fi = 0;
+	d->es = data_type;
+	d->sg[0].en = len / BIT(data_type);
+	d->sg[0].fn = 1;
+	d->sg[0].addr = dest;
+	d->sglen = 1;
+	d->ccr = c->ccr;
+	d->ccr |= CCR_DST_AMODE_POSTINC | CCR_SRC_AMODE_POSTINC;
+
+	d->cicr = CICR_DROP_IE;
+	if (tx_flags & DMA_PREP_INTERRUPT)
+		d->cicr |= CICR_FRAME_IE;
+
+	d->csdp = data_type;
+
+	if (dma_omap1()) {
+		d->cicr |= CICR_TOUT_IE;
+		d->csdp |= CSDP_DST_PORT_EMIFF | CSDP_SRC_PORT_EMIFF;
+	} else {
+		d->csdp |= CSDP_DST_PACKED | CSDP_SRC_PACKED;
+		d->cicr |= CICR_MISALIGNED_ERR_IE | CICR_TRANS_ERR_IE;
+		d->csdp |= CSDP_DST_BURST_64 | CSDP_SRC_BURST_64;
+	}
+
+	return vchan_tx_prep(&c->vc, &d->vd, tx_flags);
+}
+
 static int omap_dma_slave_config(struct dma_chan *chan, struct dma_slave_config *cfg)
 {
 	struct omap_chan *c = to_omap_dma_chan(chan);
@@ -1037,7 +1087,7 @@ static int omap_dma_resume(struct dma_chan *chan)
 	return 0;
 }
 
-static int omap_dma_chan_init(struct omap_dmadev *od, int dma_sig)
+static int omap_dma_chan_init(struct omap_dmadev *od)
 {
 	struct omap_chan *c;
 
@@ -1046,7 +1096,6 @@ static int omap_dma_chan_init(struct omap_dmadev *od, int dma_sig)
 		return -ENOMEM;
 
 	c->reg_map = od->reg_map;
-	c->dma_sig = dma_sig;
 	c->vc.desc_free = omap_dma_desc_free;
 	vchan_init(&c->vc, &od->ddev);
 	INIT_LIST_HEAD(&c->node);
@@ -1094,12 +1143,14 @@ static int omap_dma_probe(struct platform_device *pdev)
 
 	dma_cap_set(DMA_SLAVE, od->ddev.cap_mask);
 	dma_cap_set(DMA_CYCLIC, od->ddev.cap_mask);
+	dma_cap_set(DMA_MEMCPY, od->ddev.cap_mask);
 	od->ddev.device_alloc_chan_resources = omap_dma_alloc_chan_resources;
 	od->ddev.device_free_chan_resources = omap_dma_free_chan_resources;
 	od->ddev.device_tx_status = omap_dma_tx_status;
 	od->ddev.device_issue_pending = omap_dma_issue_pending;
 	od->ddev.device_prep_slave_sg = omap_dma_prep_slave_sg;
 	od->ddev.device_prep_dma_cyclic = omap_dma_prep_dma_cyclic;
+	od->ddev.device_prep_dma_memcpy = omap_dma_prep_dma_memcpy;
 	od->ddev.device_config = omap_dma_slave_config;
 	od->ddev.device_pause = omap_dma_pause;
 	od->ddev.device_resume = omap_dma_resume;
@@ -1116,8 +1167,17 @@ static int omap_dma_probe(struct platform_device *pdev)
 
 	tasklet_init(&od->task, omap_dma_sched, (unsigned long)od);
 
-	for (i = 0; i < 127; i++) {
-		rc = omap_dma_chan_init(od, i);
+	od->dma_requests = OMAP_SDMA_REQUESTS;
+	if (pdev->dev.of_node && of_property_read_u32(pdev->dev.of_node,
+						      "dma-requests",
+						      &od->dma_requests)) {
+		dev_info(&pdev->dev,
+			 "Missing dma-requests property, using %u.\n",
+			 OMAP_SDMA_REQUESTS);
+	}
+
+	for (i = 0; i < OMAP_SDMA_CHANNELS; i++) {
+		rc = omap_dma_chan_init(od);
 		if (rc) {
 			omap_dma_free(od);
 			return rc;
@@ -1208,10 +1268,14 @@ static struct platform_driver omap_dma_driver = {
 bool omap_dma_filter_fn(struct dma_chan *chan, void *param)
 {
 	if (chan->device->dev->driver == &omap_dma_driver.driver) {
+		struct omap_dmadev *od = to_omap_dma_dev(chan->device);
 		struct omap_chan *c = to_omap_dma_chan(chan);
 		unsigned req = *(unsigned *)param;
 
-		return req == c->dma_sig;
+		if (req <= od->dma_requests) {
+			c->dma_sig = req;
+			return true;
+		}
 	}
 	return false;
 }
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index a7d9d3029b14..f513f77b1d85 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -1424,8 +1424,8 @@ static int pl330_submit_req(struct pl330_thread *thrd,
 		goto xfer_exit;
 
 	if (ret > pl330->mcbufsz / 2) {
-		dev_info(pl330->ddma.dev, "%s:%d Trying increasing mcbufsz\n",
-				__func__, __LINE__);
+		dev_info(pl330->ddma.dev, "%s:%d Try increasing mcbufsz (%i/%i)\n",
+				__func__, __LINE__, ret, pl330->mcbufsz / 2);
 		ret = -ENOMEM;
 		goto xfer_exit;
 	}
@@ -2127,6 +2127,7 @@ static int pl330_terminate_all(struct dma_chan *chan)
 	struct pl330_dmac *pl330 = pch->dmac;
 	LIST_HEAD(list);
 
+	pm_runtime_get_sync(pl330->ddma.dev);
 	spin_lock_irqsave(&pch->lock, flags);
 	spin_lock(&pl330->lock);
 	_stop(pch->thread);
@@ -2151,6 +2152,8 @@ static int pl330_terminate_all(struct dma_chan *chan)
 	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);
 
 	return 0;
 }
@@ -2581,12 +2584,14 @@ pl330_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dst,
 {
 	struct dma_pl330_desc *desc;
 	struct dma_pl330_chan *pch = to_pchan(chan);
-	struct pl330_dmac *pl330 = pch->dmac;
+	struct pl330_dmac *pl330;
 	int burst;
 
 	if (unlikely(!pch || !len))
 		return NULL;
 
+	pl330 = pch->dmac;
+
 	desc = __pl330_prep_dma_memcpy(pch, dst, src, len);
 	if (!desc)
 		return NULL;
diff --git a/drivers/dma/pxa_dma.c b/drivers/dma/pxa_dma.c
new file mode 100644
index 000000000000..ddcbbf5cd9e9
--- /dev/null
+++ b/drivers/dma/pxa_dma.c
@@ -0,0 +1,1467 @@
+/*
+ * Copyright 2015 Robert Jarzmik <robert.jarzmik@free.fr>
+ *
+ * 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/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+#include <linux/dmaengine.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/platform_data/mmp_dma.h>
+#include <linux/dmapool.h>
+#include <linux/of_device.h>
+#include <linux/of_dma.h>
+#include <linux/of.h>
+#include <linux/dma/pxa-dma.h>
+
+#include "dmaengine.h"
+#include "virt-dma.h"
+
+#define DCSR(n)		(0x0000 + ((n) << 2))
+#define DALGN(n)	0x00a0
+#define DINT		0x00f0
+#define DDADR(n)	(0x0200 + ((n) << 4))
+#define DSADR(n)	(0x0204 + ((n) << 4))
+#define DTADR(n)	(0x0208 + ((n) << 4))
+#define DCMD(n)		(0x020c + ((n) << 4))
+
+#define PXA_DCSR_RUN		BIT(31)	/* Run Bit (read / write) */
+#define PXA_DCSR_NODESC		BIT(30)	/* No-Descriptor Fetch (read / write) */
+#define PXA_DCSR_STOPIRQEN	BIT(29)	/* Stop Interrupt Enable (R/W) */
+#define PXA_DCSR_REQPEND	BIT(8)	/* Request Pending (read-only) */
+#define PXA_DCSR_STOPSTATE	BIT(3)	/* Stop State (read-only) */
+#define PXA_DCSR_ENDINTR	BIT(2)	/* End Interrupt (read / write) */
+#define PXA_DCSR_STARTINTR	BIT(1)	/* Start Interrupt (read / write) */
+#define PXA_DCSR_BUSERR		BIT(0)	/* Bus Error Interrupt (read / write) */
+
+#define PXA_DCSR_EORIRQEN	BIT(28)	/* End of Receive IRQ Enable (R/W) */
+#define PXA_DCSR_EORJMPEN	BIT(27)	/* Jump to next descriptor on EOR */
+#define PXA_DCSR_EORSTOPEN	BIT(26)	/* STOP on an EOR */
+#define PXA_DCSR_SETCMPST	BIT(25)	/* Set Descriptor Compare Status */
+#define PXA_DCSR_CLRCMPST	BIT(24)	/* Clear Descriptor Compare Status */
+#define PXA_DCSR_CMPST		BIT(10)	/* The Descriptor Compare Status */
+#define PXA_DCSR_EORINTR	BIT(9)	/* The end of Receive */
+
+#define DRCMR_MAPVLD	BIT(7)	/* Map Valid (read / write) */
+#define DRCMR_CHLNUM	0x1f	/* mask for Channel Number (read / write) */
+
+#define DDADR_DESCADDR	0xfffffff0	/* Address of next descriptor (mask) */
+#define DDADR_STOP	BIT(0)	/* Stop (read / write) */
+
+#define PXA_DCMD_INCSRCADDR	BIT(31)	/* Source Address Increment Setting. */
+#define PXA_DCMD_INCTRGADDR	BIT(30)	/* Target Address Increment Setting. */
+#define PXA_DCMD_FLOWSRC	BIT(29)	/* Flow Control by the source. */
+#define PXA_DCMD_FLOWTRG	BIT(28)	/* Flow Control by the target. */
+#define PXA_DCMD_STARTIRQEN	BIT(22)	/* Start Interrupt Enable */
+#define PXA_DCMD_ENDIRQEN	BIT(21)	/* End Interrupt Enable */
+#define PXA_DCMD_ENDIAN		BIT(18)	/* Device Endian-ness. */
+#define PXA_DCMD_BURST8		(1 << 16)	/* 8 byte burst */
+#define PXA_DCMD_BURST16	(2 << 16)	/* 16 byte burst */
+#define PXA_DCMD_BURST32	(3 << 16)	/* 32 byte burst */
+#define PXA_DCMD_WIDTH1		(1 << 14)	/* 1 byte width */
+#define PXA_DCMD_WIDTH2		(2 << 14)	/* 2 byte width (HalfWord) */
+#define PXA_DCMD_WIDTH4		(3 << 14)	/* 4 byte width (Word) */
+#define PXA_DCMD_LENGTH		0x01fff		/* length mask (max = 8K - 1) */
+
+#define PDMA_ALIGNMENT		3
+#define PDMA_MAX_DESC_BYTES	(PXA_DCMD_LENGTH & ~((1 << PDMA_ALIGNMENT) - 1))
+
+struct pxad_desc_hw {
+	u32 ddadr;	/* Points to the next descriptor + flags */
+	u32 dsadr;	/* DSADR value for the current transfer */
+	u32 dtadr;	/* DTADR value for the current transfer */
+	u32 dcmd;	/* DCMD value for the current transfer */
+} __aligned(16);
+
+struct pxad_desc_sw {
+	struct virt_dma_desc	vd;		/* Virtual descriptor */
+	int			nb_desc;	/* Number of hw. descriptors */
+	size_t			len;		/* Number of bytes xfered */
+	dma_addr_t		first;		/* First descriptor's addr */
+
+	/* At least one descriptor has an src/dst address not multiple of 8 */
+	bool			misaligned;
+	bool			cyclic;
+	struct dma_pool		*desc_pool;	/* Channel's used allocator */
+
+	struct pxad_desc_hw	*hw_desc[];	/* DMA coherent descriptors */
+};
+
+struct pxad_phy {
+	int			idx;
+	void __iomem		*base;
+	struct pxad_chan	*vchan;
+};
+
+struct pxad_chan {
+	struct virt_dma_chan	vc;		/* Virtual channel */
+	u32			drcmr;		/* Requestor of the channel */
+	enum pxad_chan_prio	prio;		/* Required priority of phy */
+	/*
+	 * At least one desc_sw in submitted or issued transfers on this channel
+	 * has one address such as: addr % 8 != 0. This implies the DALGN
+	 * setting on the phy.
+	 */
+	bool			misaligned;
+	struct dma_slave_config	cfg;		/* Runtime config */
+
+	/* protected by vc->lock */
+	struct pxad_phy		*phy;
+	struct dma_pool		*desc_pool;	/* Descriptors pool */
+};
+
+struct pxad_device {
+	struct dma_device		slave;
+	int				nr_chans;
+	void __iomem			*base;
+	struct pxad_phy			*phys;
+	spinlock_t			phy_lock;	/* Phy association */
+#ifdef CONFIG_DEBUG_FS
+	struct dentry			*dbgfs_root;
+	struct dentry			*dbgfs_state;
+	struct dentry			**dbgfs_chan;
+#endif
+};
+
+#define tx_to_pxad_desc(tx)					\
+	container_of(tx, struct pxad_desc_sw, async_tx)
+#define to_pxad_chan(dchan)					\
+	container_of(dchan, struct pxad_chan, vc.chan)
+#define to_pxad_dev(dmadev)					\
+	container_of(dmadev, struct pxad_device, slave)
+#define to_pxad_sw_desc(_vd)				\
+	container_of((_vd), struct pxad_desc_sw, vd)
+
+#define _phy_readl_relaxed(phy, _reg)					\
+	readl_relaxed((phy)->base + _reg((phy)->idx))
+#define phy_readl_relaxed(phy, _reg)					\
+	({								\
+		u32 _v;							\
+		_v = readl_relaxed((phy)->base + _reg((phy)->idx));	\
+		dev_vdbg(&phy->vchan->vc.chan.dev->device,		\
+			 "%s(): readl(%s): 0x%08x\n", __func__, #_reg,	\
+			  _v);						\
+		_v;							\
+	})
+#define phy_writel(phy, val, _reg)					\
+	do {								\
+		writel((val), (phy)->base + _reg((phy)->idx));		\
+		dev_vdbg(&phy->vchan->vc.chan.dev->device,		\
+			 "%s(): writel(0x%08x, %s)\n",			\
+			 __func__, (u32)(val), #_reg);			\
+	} while (0)
+#define phy_writel_relaxed(phy, val, _reg)				\
+	do {								\
+		writel_relaxed((val), (phy)->base + _reg((phy)->idx));	\
+		dev_vdbg(&phy->vchan->vc.chan.dev->device,		\
+			 "%s(): writel_relaxed(0x%08x, %s)\n",		\
+			 __func__, (u32)(val), #_reg);			\
+	} while (0)
+
+static unsigned int pxad_drcmr(unsigned int line)
+{
+	if (line < 64)
+		return 0x100 + line * 4;
+	return 0x1000 + line * 4;
+}
+
+/*
+ * Debug fs
+ */
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/seq_file.h>
+
+static int dbg_show_requester_chan(struct seq_file *s, void *p)
+{
+	int pos = 0;
+	struct pxad_phy *phy = s->private;
+	int i;
+	u32 drcmr;
+
+	pos += seq_printf(s, "DMA channel %d requester :\n", phy->idx);
+	for (i = 0; i < 70; i++) {
+		drcmr = readl_relaxed(phy->base + pxad_drcmr(i));
+		if ((drcmr & DRCMR_CHLNUM) == phy->idx)
+			pos += seq_printf(s, "\tRequester %d (MAPVLD=%d)\n", i,
+					  !!(drcmr & DRCMR_MAPVLD));
+	}
+	return pos;
+}
+
+static inline int dbg_burst_from_dcmd(u32 dcmd)
+{
+	int burst = (dcmd >> 16) & 0x3;
+
+	return burst ? 4 << burst : 0;
+}
+
+static int is_phys_valid(unsigned long addr)
+{
+	return pfn_valid(__phys_to_pfn(addr));
+}
+
+#define PXA_DCSR_STR(flag) (dcsr & PXA_DCSR_##flag ? #flag" " : "")
+#define PXA_DCMD_STR(flag) (dcmd & PXA_DCMD_##flag ? #flag" " : "")
+
+static int dbg_show_descriptors(struct seq_file *s, void *p)
+{
+	struct pxad_phy *phy = s->private;
+	int i, max_show = 20, burst, width;
+	u32 dcmd;
+	unsigned long phys_desc, ddadr;
+	struct pxad_desc_hw *desc;
+
+	phys_desc = ddadr = _phy_readl_relaxed(phy, DDADR);
+
+	seq_printf(s, "DMA channel %d descriptors :\n", phy->idx);
+	seq_printf(s, "[%03d] First descriptor unknown\n", 0);
+	for (i = 1; i < max_show && is_phys_valid(phys_desc); i++) {
+		desc = phys_to_virt(phys_desc);
+		dcmd = desc->dcmd;
+		burst = dbg_burst_from_dcmd(dcmd);
+		width = (1 << ((dcmd >> 14) & 0x3)) >> 1;
+
+		seq_printf(s, "[%03d] Desc at %08lx(virt %p)\n",
+			   i, phys_desc, desc);
+		seq_printf(s, "\tDDADR = %08x\n", desc->ddadr);
+		seq_printf(s, "\tDSADR = %08x\n", desc->dsadr);
+		seq_printf(s, "\tDTADR = %08x\n", desc->dtadr);
+		seq_printf(s, "\tDCMD  = %08x (%s%s%s%s%s%s%sburst=%d width=%d len=%d)\n",
+			   dcmd,
+			   PXA_DCMD_STR(INCSRCADDR), PXA_DCMD_STR(INCTRGADDR),
+			   PXA_DCMD_STR(FLOWSRC), PXA_DCMD_STR(FLOWTRG),
+			   PXA_DCMD_STR(STARTIRQEN), PXA_DCMD_STR(ENDIRQEN),
+			   PXA_DCMD_STR(ENDIAN), burst, width,
+			   dcmd & PXA_DCMD_LENGTH);
+		phys_desc = desc->ddadr;
+	}
+	if (i == max_show)
+		seq_printf(s, "[%03d] Desc at %08lx ... max display reached\n",
+			   i, phys_desc);
+	else
+		seq_printf(s, "[%03d] Desc at %08lx is %s\n",
+			   i, phys_desc, phys_desc == DDADR_STOP ?
+			   "DDADR_STOP" : "invalid");
+
+	return 0;
+}
+
+static int dbg_show_chan_state(struct seq_file *s, void *p)
+{
+	struct pxad_phy *phy = s->private;
+	u32 dcsr, dcmd;
+	int burst, width;
+	static const char * const str_prio[] = {
+		"high", "normal", "low", "invalid"
+	};
+
+	dcsr = _phy_readl_relaxed(phy, DCSR);
+	dcmd = _phy_readl_relaxed(phy, DCMD);
+	burst = dbg_burst_from_dcmd(dcmd);
+	width = (1 << ((dcmd >> 14) & 0x3)) >> 1;
+
+	seq_printf(s, "DMA channel %d\n", phy->idx);
+	seq_printf(s, "\tPriority : %s\n",
+			  str_prio[(phy->idx & 0xf) / 4]);
+	seq_printf(s, "\tUnaligned transfer bit: %s\n",
+			  _phy_readl_relaxed(phy, DALGN) & BIT(phy->idx) ?
+			  "yes" : "no");
+	seq_printf(s, "\tDCSR  = %08x (%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s)\n",
+		   dcsr, PXA_DCSR_STR(RUN), PXA_DCSR_STR(NODESC),
+		   PXA_DCSR_STR(STOPIRQEN), PXA_DCSR_STR(EORIRQEN),
+		   PXA_DCSR_STR(EORJMPEN), PXA_DCSR_STR(EORSTOPEN),
+		   PXA_DCSR_STR(SETCMPST), PXA_DCSR_STR(CLRCMPST),
+		   PXA_DCSR_STR(CMPST), PXA_DCSR_STR(EORINTR),
+		   PXA_DCSR_STR(REQPEND), PXA_DCSR_STR(STOPSTATE),
+		   PXA_DCSR_STR(ENDINTR), PXA_DCSR_STR(STARTINTR),
+		   PXA_DCSR_STR(BUSERR));
+
+	seq_printf(s, "\tDCMD  = %08x (%s%s%s%s%s%s%sburst=%d width=%d len=%d)\n",
+		   dcmd,
+		   PXA_DCMD_STR(INCSRCADDR), PXA_DCMD_STR(INCTRGADDR),
+		   PXA_DCMD_STR(FLOWSRC), PXA_DCMD_STR(FLOWTRG),
+		   PXA_DCMD_STR(STARTIRQEN), PXA_DCMD_STR(ENDIRQEN),
+		   PXA_DCMD_STR(ENDIAN), burst, width, dcmd & PXA_DCMD_LENGTH);
+	seq_printf(s, "\tDSADR = %08x\n", _phy_readl_relaxed(phy, DSADR));
+	seq_printf(s, "\tDTADR = %08x\n", _phy_readl_relaxed(phy, DTADR));
+	seq_printf(s, "\tDDADR = %08x\n", _phy_readl_relaxed(phy, DDADR));
+
+	return 0;
+}
+
+static int dbg_show_state(struct seq_file *s, void *p)
+{
+	struct pxad_device *pdev = s->private;
+
+	/* basic device status */
+	seq_puts(s, "DMA engine status\n");
+	seq_printf(s, "\tChannel number: %d\n", pdev->nr_chans);
+
+	return 0;
+}
+
+#define DBGFS_FUNC_DECL(name) \
+static int dbg_open_##name(struct inode *inode, struct file *file) \
+{ \
+	return single_open(file, dbg_show_##name, inode->i_private); \
+} \
+static const struct file_operations dbg_fops_##name = { \
+	.owner		= THIS_MODULE, \
+	.open		= dbg_open_##name, \
+	.llseek		= seq_lseek, \
+	.read		= seq_read, \
+	.release	= single_release, \
+}
+
+DBGFS_FUNC_DECL(state);
+DBGFS_FUNC_DECL(chan_state);
+DBGFS_FUNC_DECL(descriptors);
+DBGFS_FUNC_DECL(requester_chan);
+
+static struct dentry *pxad_dbg_alloc_chan(struct pxad_device *pdev,
+					     int ch, struct dentry *chandir)
+{
+	char chan_name[11];
+	struct dentry *chan, *chan_state = NULL, *chan_descr = NULL;
+	struct dentry *chan_reqs = NULL;
+	void *dt;
+
+	scnprintf(chan_name, sizeof(chan_name), "%d", ch);
+	chan = debugfs_create_dir(chan_name, chandir);
+	dt = (void *)&pdev->phys[ch];
+
+	if (chan)
+		chan_state = debugfs_create_file("state", 0400, chan, dt,
+						 &dbg_fops_chan_state);
+	if (chan_state)
+		chan_descr = debugfs_create_file("descriptors", 0400, chan, dt,
+						 &dbg_fops_descriptors);
+	if (chan_descr)
+		chan_reqs = debugfs_create_file("requesters", 0400, chan, dt,
+						&dbg_fops_requester_chan);
+	if (!chan_reqs)
+		goto err_state;
+
+	return chan;
+
+err_state:
+	debugfs_remove_recursive(chan);
+	return NULL;
+}
+
+static void pxad_init_debugfs(struct pxad_device *pdev)
+{
+	int i;
+	struct dentry *chandir;
+
+	pdev->dbgfs_root = debugfs_create_dir(dev_name(pdev->slave.dev), NULL);
+	if (IS_ERR(pdev->dbgfs_root) || !pdev->dbgfs_root)
+		goto err_root;
+
+	pdev->dbgfs_state = debugfs_create_file("state", 0400, pdev->dbgfs_root,
+						pdev, &dbg_fops_state);
+	if (!pdev->dbgfs_state)
+		goto err_state;
+
+	pdev->dbgfs_chan =
+		kmalloc_array(pdev->nr_chans, sizeof(*pdev->dbgfs_state),
+			      GFP_KERNEL);
+	if (!pdev->dbgfs_chan)
+		goto err_alloc;
+
+	chandir = debugfs_create_dir("channels", pdev->dbgfs_root);
+	if (!chandir)
+		goto err_chandir;
+
+	for (i = 0; i < pdev->nr_chans; i++) {
+		pdev->dbgfs_chan[i] = pxad_dbg_alloc_chan(pdev, i, chandir);
+		if (!pdev->dbgfs_chan[i])
+			goto err_chans;
+	}
+
+	return;
+err_chans:
+err_chandir:
+	kfree(pdev->dbgfs_chan);
+err_alloc:
+err_state:
+	debugfs_remove_recursive(pdev->dbgfs_root);
+err_root:
+	pr_err("pxad: debugfs is not available\n");
+}
+
+static void pxad_cleanup_debugfs(struct pxad_device *pdev)
+{
+	debugfs_remove_recursive(pdev->dbgfs_root);
+}
+#else
+static inline void pxad_init_debugfs(struct pxad_device *pdev) {}
+static inline void pxad_cleanup_debugfs(struct pxad_device *pdev) {}
+#endif
+
+/*
+ * In the transition phase where legacy pxa handling is done at the same time as
+ * mmp_dma, the DMA physical channel split between the 2 DMA providers is done
+ * through legacy_reserved. Legacy code reserves DMA channels by settings
+ * corresponding bits in legacy_reserved.
+ */
+static u32 legacy_reserved;
+static u32 legacy_unavailable;
+
+static struct pxad_phy *lookup_phy(struct pxad_chan *pchan)
+{
+	int prio, i;
+	struct pxad_device *pdev = to_pxad_dev(pchan->vc.chan.device);
+	struct pxad_phy *phy, *found = NULL;
+	unsigned long flags;
+
+	/*
+	 * dma channel priorities
+	 * ch 0 - 3,  16 - 19  <--> (0)
+	 * ch 4 - 7,  20 - 23  <--> (1)
+	 * ch 8 - 11, 24 - 27  <--> (2)
+	 * ch 12 - 15, 28 - 31  <--> (3)
+	 */
+
+	spin_lock_irqsave(&pdev->phy_lock, flags);
+	for (prio = pchan->prio; prio >= PXAD_PRIO_HIGHEST; prio--) {
+		for (i = 0; i < pdev->nr_chans; i++) {
+			if (prio != (i & 0xf) >> 2)
+				continue;
+			if ((i < 32) && (legacy_reserved & BIT(i)))
+				continue;
+			phy = &pdev->phys[i];
+			if (!phy->vchan) {
+				phy->vchan = pchan;
+				found = phy;
+				if (i < 32)
+					legacy_unavailable |= BIT(i);
+				goto out_unlock;
+			}
+		}
+	}
+
+out_unlock:
+	spin_unlock_irqrestore(&pdev->phy_lock, flags);
+	dev_dbg(&pchan->vc.chan.dev->device,
+		"%s(): phy=%p(%d)\n", __func__, found,
+		found ? found->idx : -1);
+
+	return found;
+}
+
+static void pxad_free_phy(struct pxad_chan *chan)
+{
+	struct pxad_device *pdev = to_pxad_dev(chan->vc.chan.device);
+	unsigned long flags;
+	u32 reg;
+	int i;
+
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): freeing\n", __func__);
+	if (!chan->phy)
+		return;
+
+	/* clear the channel mapping in DRCMR */
+	reg = pxad_drcmr(chan->drcmr);
+	writel_relaxed(0, chan->phy->base + reg);
+
+	spin_lock_irqsave(&pdev->phy_lock, flags);
+	for (i = 0; i < 32; i++)
+		if (chan->phy == &pdev->phys[i])
+			legacy_unavailable &= ~BIT(i);
+	chan->phy->vchan = NULL;
+	chan->phy = NULL;
+	spin_unlock_irqrestore(&pdev->phy_lock, flags);
+}
+
+static bool is_chan_running(struct pxad_chan *chan)
+{
+	u32 dcsr;
+	struct pxad_phy *phy = chan->phy;
+
+	if (!phy)
+		return false;
+	dcsr = phy_readl_relaxed(phy, DCSR);
+	return dcsr & PXA_DCSR_RUN;
+}
+
+static bool is_running_chan_misaligned(struct pxad_chan *chan)
+{
+	u32 dalgn;
+
+	BUG_ON(!chan->phy);
+	dalgn = phy_readl_relaxed(chan->phy, DALGN);
+	return dalgn & (BIT(chan->phy->idx));
+}
+
+static void phy_enable(struct pxad_phy *phy, bool misaligned)
+{
+	u32 reg, dalgn;
+
+	if (!phy->vchan)
+		return;
+
+	dev_dbg(&phy->vchan->vc.chan.dev->device,
+		"%s(); phy=%p(%d) misaligned=%d\n", __func__,
+		phy, phy->idx, misaligned);
+
+	reg = pxad_drcmr(phy->vchan->drcmr);
+	writel_relaxed(DRCMR_MAPVLD | phy->idx, phy->base + reg);
+
+	dalgn = phy_readl_relaxed(phy, DALGN);
+	if (misaligned)
+		dalgn |= BIT(phy->idx);
+	else
+		dalgn &= ~BIT(phy->idx);
+	phy_writel_relaxed(phy, dalgn, DALGN);
+
+	phy_writel(phy, PXA_DCSR_STOPIRQEN | PXA_DCSR_ENDINTR |
+		   PXA_DCSR_BUSERR | PXA_DCSR_RUN, DCSR);
+}
+
+static void phy_disable(struct pxad_phy *phy)
+{
+	u32 dcsr;
+
+	if (!phy)
+		return;
+
+	dcsr = phy_readl_relaxed(phy, DCSR);
+	dev_dbg(&phy->vchan->vc.chan.dev->device,
+		"%s(): phy=%p(%d)\n", __func__, phy, phy->idx);
+	phy_writel(phy, dcsr & ~PXA_DCSR_RUN & ~PXA_DCSR_STOPIRQEN, DCSR);
+}
+
+static void pxad_launch_chan(struct pxad_chan *chan,
+				 struct pxad_desc_sw *desc)
+{
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): desc=%p\n", __func__, desc);
+	if (!chan->phy) {
+		chan->phy = lookup_phy(chan);
+		if (!chan->phy) {
+			dev_dbg(&chan->vc.chan.dev->device,
+				"%s(): no free dma channel\n", __func__);
+			return;
+		}
+	}
+
+	/*
+	 * Program the descriptor's address into the DMA controller,
+	 * then start the DMA transaction
+	 */
+	phy_writel(chan->phy, desc->first, DDADR);
+	phy_enable(chan->phy, chan->misaligned);
+}
+
+static void set_updater_desc(struct pxad_desc_sw *sw_desc,
+			     unsigned long flags)
+{
+	struct pxad_desc_hw *updater =
+		sw_desc->hw_desc[sw_desc->nb_desc - 1];
+	dma_addr_t dma = sw_desc->hw_desc[sw_desc->nb_desc - 2]->ddadr;
+
+	updater->ddadr = DDADR_STOP;
+	updater->dsadr = dma;
+	updater->dtadr = dma + 8;
+	updater->dcmd = PXA_DCMD_WIDTH4 | PXA_DCMD_BURST32 |
+		(PXA_DCMD_LENGTH & sizeof(u32));
+	if (flags & DMA_PREP_INTERRUPT)
+		updater->dcmd |= PXA_DCMD_ENDIRQEN;
+}
+
+static bool is_desc_completed(struct virt_dma_desc *vd)
+{
+	struct pxad_desc_sw *sw_desc = to_pxad_sw_desc(vd);
+	struct pxad_desc_hw *updater =
+		sw_desc->hw_desc[sw_desc->nb_desc - 1];
+
+	return updater->dtadr != (updater->dsadr + 8);
+}
+
+static void pxad_desc_chain(struct virt_dma_desc *vd1,
+				struct virt_dma_desc *vd2)
+{
+	struct pxad_desc_sw *desc1 = to_pxad_sw_desc(vd1);
+	struct pxad_desc_sw *desc2 = to_pxad_sw_desc(vd2);
+	dma_addr_t dma_to_chain;
+
+	dma_to_chain = desc2->first;
+	desc1->hw_desc[desc1->nb_desc - 1]->ddadr = dma_to_chain;
+}
+
+static bool pxad_try_hotchain(struct virt_dma_chan *vc,
+				  struct virt_dma_desc *vd)
+{
+	struct virt_dma_desc *vd_last_issued = NULL;
+	struct pxad_chan *chan = to_pxad_chan(&vc->chan);
+
+	/*
+	 * Attempt to hot chain the tx if the phy is still running. This is
+	 * considered successful only if either the channel is still running
+	 * after the chaining, or if the chained transfer is completed after
+	 * having been hot chained.
+	 * A change of alignment is not allowed, and forbids hotchaining.
+	 */
+	if (is_chan_running(chan)) {
+		BUG_ON(list_empty(&vc->desc_issued));
+
+		if (!is_running_chan_misaligned(chan) &&
+		    to_pxad_sw_desc(vd)->misaligned)
+			return false;
+
+		vd_last_issued = list_entry(vc->desc_issued.prev,
+					    struct virt_dma_desc, node);
+		pxad_desc_chain(vd_last_issued, vd);
+		if (is_chan_running(chan) || is_desc_completed(vd_last_issued))
+			return true;
+	}
+
+	return false;
+}
+
+static unsigned int clear_chan_irq(struct pxad_phy *phy)
+{
+	u32 dcsr;
+	u32 dint = readl(phy->base + DINT);
+
+	if (!(dint & BIT(phy->idx)))
+		return PXA_DCSR_RUN;
+
+	/* clear irq */
+	dcsr = phy_readl_relaxed(phy, DCSR);
+	phy_writel(phy, dcsr, DCSR);
+	if ((dcsr & PXA_DCSR_BUSERR) && (phy->vchan))
+		dev_warn(&phy->vchan->vc.chan.dev->device,
+			 "%s(chan=%p): PXA_DCSR_BUSERR\n",
+			 __func__, &phy->vchan);
+
+	return dcsr & ~PXA_DCSR_RUN;
+}
+
+static irqreturn_t pxad_chan_handler(int irq, void *dev_id)
+{
+	struct pxad_phy *phy = dev_id;
+	struct pxad_chan *chan = phy->vchan;
+	struct virt_dma_desc *vd, *tmp;
+	unsigned int dcsr;
+	unsigned long flags;
+
+	BUG_ON(!chan);
+
+	dcsr = clear_chan_irq(phy);
+	if (dcsr & PXA_DCSR_RUN)
+		return IRQ_NONE;
+
+	spin_lock_irqsave(&chan->vc.lock, flags);
+	list_for_each_entry_safe(vd, tmp, &chan->vc.desc_issued, node) {
+		dev_dbg(&chan->vc.chan.dev->device,
+			"%s(): checking txd %p[%x]: completed=%d\n",
+			__func__, vd, vd->tx.cookie, is_desc_completed(vd));
+		if (is_desc_completed(vd)) {
+			list_del(&vd->node);
+			vchan_cookie_complete(vd);
+		} else {
+			break;
+		}
+	}
+
+	if (dcsr & PXA_DCSR_STOPSTATE) {
+		dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): channel stopped, submitted_empty=%d issued_empty=%d",
+			__func__,
+			list_empty(&chan->vc.desc_submitted),
+			list_empty(&chan->vc.desc_issued));
+		phy_writel_relaxed(phy, dcsr & ~PXA_DCSR_STOPIRQEN, DCSR);
+
+		if (list_empty(&chan->vc.desc_issued)) {
+			chan->misaligned =
+				!list_empty(&chan->vc.desc_submitted);
+		} else {
+			vd = list_first_entry(&chan->vc.desc_issued,
+					      struct virt_dma_desc, node);
+			pxad_launch_chan(chan, to_pxad_sw_desc(vd));
+		}
+	}
+	spin_unlock_irqrestore(&chan->vc.lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t pxad_int_handler(int irq, void *dev_id)
+{
+	struct pxad_device *pdev = dev_id;
+	struct pxad_phy *phy;
+	u32 dint = readl(pdev->base + DINT);
+	int i, ret = IRQ_NONE;
+
+	while (dint) {
+		i = __ffs(dint);
+		dint &= (dint - 1);
+		phy = &pdev->phys[i];
+		if ((i < 32) && (legacy_reserved & BIT(i)))
+			continue;
+		if (pxad_chan_handler(irq, phy) == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static int pxad_alloc_chan_resources(struct dma_chan *dchan)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+	struct pxad_device *pdev = to_pxad_dev(chan->vc.chan.device);
+
+	if (chan->desc_pool)
+		return 1;
+
+	chan->desc_pool = dma_pool_create(dma_chan_name(dchan),
+					  pdev->slave.dev,
+					  sizeof(struct pxad_desc_hw),
+					  __alignof__(struct pxad_desc_hw),
+					  0);
+	if (!chan->desc_pool) {
+		dev_err(&chan->vc.chan.dev->device,
+			"%s(): unable to allocate descriptor pool\n",
+			__func__);
+		return -ENOMEM;
+	}
+
+	return 1;
+}
+
+static void pxad_free_chan_resources(struct dma_chan *dchan)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+
+	vchan_free_chan_resources(&chan->vc);
+	dma_pool_destroy(chan->desc_pool);
+	chan->desc_pool = NULL;
+
+}
+
+static void pxad_free_desc(struct virt_dma_desc *vd)
+{
+	int i;
+	dma_addr_t dma;
+	struct pxad_desc_sw *sw_desc = to_pxad_sw_desc(vd);
+
+	BUG_ON(sw_desc->nb_desc == 0);
+	for (i = sw_desc->nb_desc - 1; i >= 0; i--) {
+		if (i > 0)
+			dma = sw_desc->hw_desc[i - 1]->ddadr;
+		else
+			dma = sw_desc->first;
+		dma_pool_free(sw_desc->desc_pool,
+			      sw_desc->hw_desc[i], dma);
+	}
+	sw_desc->nb_desc = 0;
+	kfree(sw_desc);
+}
+
+static struct pxad_desc_sw *
+pxad_alloc_desc(struct pxad_chan *chan, unsigned int nb_hw_desc)
+{
+	struct pxad_desc_sw *sw_desc;
+	dma_addr_t dma;
+	int i;
+
+	sw_desc = kzalloc(sizeof(*sw_desc) +
+			  nb_hw_desc * sizeof(struct pxad_desc_hw *),
+			  GFP_NOWAIT);
+	if (!sw_desc)
+		return NULL;
+	sw_desc->desc_pool = chan->desc_pool;
+
+	for (i = 0; i < nb_hw_desc; i++) {
+		sw_desc->hw_desc[i] = dma_pool_alloc(sw_desc->desc_pool,
+						     GFP_NOWAIT, &dma);
+		if (!sw_desc->hw_desc[i]) {
+			dev_err(&chan->vc.chan.dev->device,
+				"%s(): Couldn't allocate the %dth hw_desc from dma_pool %p\n",
+				__func__, i, sw_desc->desc_pool);
+			goto err;
+		}
+
+		if (i == 0)
+			sw_desc->first = dma;
+		else
+			sw_desc->hw_desc[i - 1]->ddadr = dma;
+		sw_desc->nb_desc++;
+	}
+
+	return sw_desc;
+err:
+	pxad_free_desc(&sw_desc->vd);
+	return NULL;
+}
+
+static dma_cookie_t pxad_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+	struct virt_dma_chan *vc = to_virt_chan(tx->chan);
+	struct pxad_chan *chan = to_pxad_chan(&vc->chan);
+	struct virt_dma_desc *vd_chained = NULL,
+		*vd = container_of(tx, struct virt_dma_desc, tx);
+	dma_cookie_t cookie;
+	unsigned long flags;
+
+	set_updater_desc(to_pxad_sw_desc(vd), tx->flags);
+
+	spin_lock_irqsave(&vc->lock, flags);
+	cookie = dma_cookie_assign(tx);
+
+	if (list_empty(&vc->desc_submitted) && pxad_try_hotchain(vc, vd)) {
+		list_move_tail(&vd->node, &vc->desc_issued);
+		dev_dbg(&chan->vc.chan.dev->device,
+			"%s(): txd %p[%x]: submitted (hot linked)\n",
+			__func__, vd, cookie);
+		goto out;
+	}
+
+	/*
+	 * Fallback to placing the tx in the submitted queue
+	 */
+	if (!list_empty(&vc->desc_submitted)) {
+		vd_chained = list_entry(vc->desc_submitted.prev,
+					struct virt_dma_desc, node);
+		/*
+		 * Only chain the descriptors if no new misalignment is
+		 * introduced. If a new misalignment is chained, let the channel
+		 * stop, and be relaunched in misalign mode from the irq
+		 * handler.
+		 */
+		if (chan->misaligned || !to_pxad_sw_desc(vd)->misaligned)
+			pxad_desc_chain(vd_chained, vd);
+		else
+			vd_chained = NULL;
+	}
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): txd %p[%x]: submitted (%s linked)\n",
+		__func__, vd, cookie, vd_chained ? "cold" : "not");
+	list_move_tail(&vd->node, &vc->desc_submitted);
+	chan->misaligned |= to_pxad_sw_desc(vd)->misaligned;
+
+out:
+	spin_unlock_irqrestore(&vc->lock, flags);
+	return cookie;
+}
+
+static void pxad_issue_pending(struct dma_chan *dchan)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+	struct virt_dma_desc *vd_first;
+	unsigned long flags;
+
+	spin_lock_irqsave(&chan->vc.lock, flags);
+	if (list_empty(&chan->vc.desc_submitted))
+		goto out;
+
+	vd_first = list_first_entry(&chan->vc.desc_submitted,
+				    struct virt_dma_desc, node);
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): txd %p[%x]", __func__, vd_first, vd_first->tx.cookie);
+
+	vchan_issue_pending(&chan->vc);
+	if (!pxad_try_hotchain(&chan->vc, vd_first))
+		pxad_launch_chan(chan, to_pxad_sw_desc(vd_first));
+out:
+	spin_unlock_irqrestore(&chan->vc.lock, flags);
+}
+
+static inline struct dma_async_tx_descriptor *
+pxad_tx_prep(struct virt_dma_chan *vc, struct virt_dma_desc *vd,
+		 unsigned long tx_flags)
+{
+	struct dma_async_tx_descriptor *tx;
+	struct pxad_chan *chan = container_of(vc, struct pxad_chan, vc);
+
+	tx = vchan_tx_prep(vc, vd, tx_flags);
+	tx->tx_submit = pxad_tx_submit;
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): vc=%p txd=%p[%x] flags=0x%lx\n", __func__,
+		vc, vd, vd->tx.cookie,
+		tx_flags);
+
+	return tx;
+}
+
+static void pxad_get_config(struct pxad_chan *chan,
+			    enum dma_transfer_direction dir,
+			    u32 *dcmd, u32 *dev_src, u32 *dev_dst)
+{
+	u32 maxburst = 0, dev_addr = 0;
+	enum dma_slave_buswidth width = DMA_SLAVE_BUSWIDTH_UNDEFINED;
+
+	*dcmd = 0;
+	if (chan->cfg.direction == DMA_DEV_TO_MEM) {
+		maxburst = chan->cfg.src_maxburst;
+		width = chan->cfg.src_addr_width;
+		dev_addr = chan->cfg.src_addr;
+		*dev_src = dev_addr;
+		*dcmd |= PXA_DCMD_INCTRGADDR | PXA_DCMD_FLOWSRC;
+	}
+	if (chan->cfg.direction == DMA_MEM_TO_DEV) {
+		maxburst = chan->cfg.dst_maxburst;
+		width = chan->cfg.dst_addr_width;
+		dev_addr = chan->cfg.dst_addr;
+		*dev_dst = dev_addr;
+		*dcmd |= PXA_DCMD_INCSRCADDR | PXA_DCMD_FLOWTRG;
+	}
+	if (chan->cfg.direction == DMA_MEM_TO_MEM)
+		*dcmd |= PXA_DCMD_BURST32 | PXA_DCMD_INCTRGADDR |
+			PXA_DCMD_INCSRCADDR;
+
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): dev_addr=0x%x maxburst=%d width=%d  dir=%d\n",
+		__func__, dev_addr, maxburst, width, dir);
+
+	if (width == DMA_SLAVE_BUSWIDTH_1_BYTE)
+		*dcmd |= PXA_DCMD_WIDTH1;
+	else if (width == DMA_SLAVE_BUSWIDTH_2_BYTES)
+		*dcmd |= PXA_DCMD_WIDTH2;
+	else if (width == DMA_SLAVE_BUSWIDTH_4_BYTES)
+		*dcmd |= PXA_DCMD_WIDTH4;
+
+	if (maxburst == 8)
+		*dcmd |= PXA_DCMD_BURST8;
+	else if (maxburst == 16)
+		*dcmd |= PXA_DCMD_BURST16;
+	else if (maxburst == 32)
+		*dcmd |= PXA_DCMD_BURST32;
+
+	/* FIXME: drivers should be ported over to use the filter
+	 * function. Once that's done, the following two lines can
+	 * be removed.
+	 */
+	if (chan->cfg.slave_id)
+		chan->drcmr = chan->cfg.slave_id;
+}
+
+static struct dma_async_tx_descriptor *
+pxad_prep_memcpy(struct dma_chan *dchan,
+		 dma_addr_t dma_dst, dma_addr_t dma_src,
+		 size_t len, unsigned long flags)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+	struct pxad_desc_sw *sw_desc;
+	struct pxad_desc_hw *hw_desc;
+	u32 dcmd;
+	unsigned int i, nb_desc = 0;
+	size_t copy;
+
+	if (!dchan || !len)
+		return NULL;
+
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): dma_dst=0x%lx dma_src=0x%lx len=%zu flags=%lx\n",
+		__func__, (unsigned long)dma_dst, (unsigned long)dma_src,
+		len, flags);
+	pxad_get_config(chan, DMA_MEM_TO_MEM, &dcmd, NULL, NULL);
+
+	nb_desc = DIV_ROUND_UP(len, PDMA_MAX_DESC_BYTES);
+	sw_desc = pxad_alloc_desc(chan, nb_desc + 1);
+	if (!sw_desc)
+		return NULL;
+	sw_desc->len = len;
+
+	if (!IS_ALIGNED(dma_src, 1 << PDMA_ALIGNMENT) ||
+	    !IS_ALIGNED(dma_dst, 1 << PDMA_ALIGNMENT))
+		sw_desc->misaligned = true;
+
+	i = 0;
+	do {
+		hw_desc = sw_desc->hw_desc[i++];
+		copy = min_t(size_t, len, PDMA_MAX_DESC_BYTES);
+		hw_desc->dcmd = dcmd | (PXA_DCMD_LENGTH & copy);
+		hw_desc->dsadr = dma_src;
+		hw_desc->dtadr = dma_dst;
+		len -= copy;
+		dma_src += copy;
+		dma_dst += copy;
+	} while (len);
+	set_updater_desc(sw_desc, flags);
+
+	return pxad_tx_prep(&chan->vc, &sw_desc->vd, flags);
+}
+
+static struct dma_async_tx_descriptor *
+pxad_prep_slave_sg(struct dma_chan *dchan, struct scatterlist *sgl,
+		   unsigned int sg_len, enum dma_transfer_direction dir,
+		   unsigned long flags, void *context)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+	struct pxad_desc_sw *sw_desc;
+	size_t len, avail;
+	struct scatterlist *sg;
+	dma_addr_t dma;
+	u32 dcmd, dsadr = 0, dtadr = 0;
+	unsigned int nb_desc = 0, i, j = 0;
+
+	if ((sgl == NULL) || (sg_len == 0))
+		return NULL;
+
+	pxad_get_config(chan, dir, &dcmd, &dsadr, &dtadr);
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): dir=%d flags=%lx\n", __func__, dir, flags);
+
+	for_each_sg(sgl, sg, sg_len, i)
+		nb_desc += DIV_ROUND_UP(sg_dma_len(sg), PDMA_MAX_DESC_BYTES);
+	sw_desc = pxad_alloc_desc(chan, nb_desc + 1);
+	if (!sw_desc)
+		return NULL;
+
+	for_each_sg(sgl, sg, sg_len, i) {
+		dma = sg_dma_address(sg);
+		avail = sg_dma_len(sg);
+		sw_desc->len += avail;
+
+		do {
+			len = min_t(size_t, avail, PDMA_MAX_DESC_BYTES);
+			if (dma & 0x7)
+				sw_desc->misaligned = true;
+
+			sw_desc->hw_desc[j]->dcmd =
+				dcmd | (PXA_DCMD_LENGTH & len);
+			sw_desc->hw_desc[j]->dsadr = dsadr ? dsadr : dma;
+			sw_desc->hw_desc[j++]->dtadr = dtadr ? dtadr : dma;
+
+			dma += len;
+			avail -= len;
+		} while (avail);
+	}
+	set_updater_desc(sw_desc, flags);
+
+	return pxad_tx_prep(&chan->vc, &sw_desc->vd, flags);
+}
+
+static struct dma_async_tx_descriptor *
+pxad_prep_dma_cyclic(struct dma_chan *dchan,
+		     dma_addr_t buf_addr, size_t len, size_t period_len,
+		     enum dma_transfer_direction dir, unsigned long flags)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+	struct pxad_desc_sw *sw_desc;
+	struct pxad_desc_hw **phw_desc;
+	dma_addr_t dma;
+	u32 dcmd, dsadr = 0, dtadr = 0;
+	unsigned int nb_desc = 0;
+
+	if (!dchan || !len || !period_len)
+		return NULL;
+	if ((dir != DMA_DEV_TO_MEM) && (dir != DMA_MEM_TO_DEV)) {
+		dev_err(&chan->vc.chan.dev->device,
+			"Unsupported direction for cyclic DMA\n");
+		return NULL;
+	}
+	/* the buffer length must be a multiple of period_len */
+	if (len % period_len != 0 || period_len > PDMA_MAX_DESC_BYTES ||
+	    !IS_ALIGNED(period_len, 1 << PDMA_ALIGNMENT))
+		return NULL;
+
+	pxad_get_config(chan, dir, &dcmd, &dsadr, &dtadr);
+	dcmd |= PXA_DCMD_ENDIRQEN | (PXA_DCMD_LENGTH | period_len);
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): buf_addr=0x%lx len=%zu period=%zu dir=%d flags=%lx\n",
+		__func__, (unsigned long)buf_addr, len, period_len, dir, flags);
+
+	nb_desc = DIV_ROUND_UP(period_len, PDMA_MAX_DESC_BYTES);
+	nb_desc *= DIV_ROUND_UP(len, period_len);
+	sw_desc = pxad_alloc_desc(chan, nb_desc + 1);
+	if (!sw_desc)
+		return NULL;
+	sw_desc->cyclic = true;
+	sw_desc->len = len;
+
+	phw_desc = sw_desc->hw_desc;
+	dma = buf_addr;
+	do {
+		phw_desc[0]->dsadr = dsadr ? dsadr : dma;
+		phw_desc[0]->dtadr = dtadr ? dtadr : dma;
+		phw_desc[0]->dcmd = dcmd;
+		phw_desc++;
+		dma += period_len;
+		len -= period_len;
+	} while (len);
+	set_updater_desc(sw_desc, flags);
+
+	return pxad_tx_prep(&chan->vc, &sw_desc->vd, flags);
+}
+
+static int pxad_config(struct dma_chan *dchan,
+		       struct dma_slave_config *cfg)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+
+	if (!dchan)
+		return -EINVAL;
+
+	chan->cfg = *cfg;
+	return 0;
+}
+
+static int pxad_terminate_all(struct dma_chan *dchan)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+	struct pxad_device *pdev = to_pxad_dev(chan->vc.chan.device);
+	struct virt_dma_desc *vd = NULL;
+	unsigned long flags;
+	struct pxad_phy *phy;
+	LIST_HEAD(head);
+
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): vchan %p: terminate all\n", __func__, &chan->vc);
+
+	spin_lock_irqsave(&chan->vc.lock, flags);
+	vchan_get_all_descriptors(&chan->vc, &head);
+
+	list_for_each_entry(vd, &head, node) {
+		dev_dbg(&chan->vc.chan.dev->device,
+			"%s(): cancelling txd %p[%x] (completed=%d)", __func__,
+			vd, vd->tx.cookie, is_desc_completed(vd));
+	}
+
+	phy = chan->phy;
+	if (phy) {
+		phy_disable(chan->phy);
+		pxad_free_phy(chan);
+		chan->phy = NULL;
+		spin_lock(&pdev->phy_lock);
+		phy->vchan = NULL;
+		spin_unlock(&pdev->phy_lock);
+	}
+	spin_unlock_irqrestore(&chan->vc.lock, flags);
+	vchan_dma_desc_free_list(&chan->vc, &head);
+
+	return 0;
+}
+
+static unsigned int pxad_residue(struct pxad_chan *chan,
+				 dma_cookie_t cookie)
+{
+	struct virt_dma_desc *vd = NULL;
+	struct pxad_desc_sw *sw_desc = NULL;
+	struct pxad_desc_hw *hw_desc = NULL;
+	u32 curr, start, len, end, residue = 0;
+	unsigned long flags;
+	bool passed = false;
+	int i;
+
+	/*
+	 * If the channel does not have a phy pointer anymore, it has already
+	 * been completed. Therefore, its residue is 0.
+	 */
+	if (!chan->phy)
+		return 0;
+
+	spin_lock_irqsave(&chan->vc.lock, flags);
+
+	vd = vchan_find_desc(&chan->vc, cookie);
+	if (!vd)
+		goto out;
+
+	sw_desc = to_pxad_sw_desc(vd);
+	if (sw_desc->hw_desc[0]->dcmd & PXA_DCMD_INCSRCADDR)
+		curr = phy_readl_relaxed(chan->phy, DSADR);
+	else
+		curr = phy_readl_relaxed(chan->phy, DTADR);
+
+	for (i = 0; i < sw_desc->nb_desc - 1; i++) {
+		hw_desc = sw_desc->hw_desc[i];
+		if (sw_desc->hw_desc[0]->dcmd & PXA_DCMD_INCSRCADDR)
+			start = hw_desc->dsadr;
+		else
+			start = hw_desc->dtadr;
+		len = hw_desc->dcmd & PXA_DCMD_LENGTH;
+		end = start + len;
+
+		/*
+		 * 'passed' will be latched once we found the descriptor
+		 * which lies inside the boundaries of the curr
+		 * pointer. All descriptors that occur in the list
+		 * _after_ we found that partially handled descriptor
+		 * are still to be processed and are hence added to the
+		 * residual bytes counter.
+		 */
+
+		if (passed) {
+			residue += len;
+		} else if (curr >= start && curr <= end) {
+			residue += end - curr;
+			passed = true;
+		}
+	}
+	if (!passed)
+		residue = sw_desc->len;
+
+out:
+	spin_unlock_irqrestore(&chan->vc.lock, flags);
+	dev_dbg(&chan->vc.chan.dev->device,
+		"%s(): txd %p[%x] sw_desc=%p: %d\n",
+		__func__, vd, cookie, sw_desc, residue);
+	return residue;
+}
+
+static enum dma_status pxad_tx_status(struct dma_chan *dchan,
+				      dma_cookie_t cookie,
+				      struct dma_tx_state *txstate)
+{
+	struct pxad_chan *chan = to_pxad_chan(dchan);
+	enum dma_status ret;
+
+	ret = dma_cookie_status(dchan, cookie, txstate);
+	if (likely(txstate && (ret != DMA_ERROR)))
+		dma_set_residue(txstate, pxad_residue(chan, cookie));
+
+	return ret;
+}
+
+static void pxad_free_channels(struct dma_device *dmadev)
+{
+	struct pxad_chan *c, *cn;
+
+	list_for_each_entry_safe(c, cn, &dmadev->channels,
+				 vc.chan.device_node) {
+		list_del(&c->vc.chan.device_node);
+		tasklet_kill(&c->vc.task);
+	}
+}
+
+static int pxad_remove(struct platform_device *op)
+{
+	struct pxad_device *pdev = platform_get_drvdata(op);
+
+	pxad_cleanup_debugfs(pdev);
+	pxad_free_channels(&pdev->slave);
+	dma_async_device_unregister(&pdev->slave);
+	return 0;
+}
+
+static int pxad_init_phys(struct platform_device *op,
+			  struct pxad_device *pdev,
+			  unsigned int nb_phy_chans)
+{
+	int irq0, irq, nr_irq = 0, i, ret;
+	struct pxad_phy *phy;
+
+	irq0 = platform_get_irq(op, 0);
+	if (irq0 < 0)
+		return irq0;
+
+	pdev->phys = devm_kcalloc(&op->dev, nb_phy_chans,
+				  sizeof(pdev->phys[0]), GFP_KERNEL);
+	if (!pdev->phys)
+		return -ENOMEM;
+
+	for (i = 0; i < nb_phy_chans; i++)
+		if (platform_get_irq(op, i) > 0)
+			nr_irq++;
+
+	for (i = 0; i < nb_phy_chans; i++) {
+		phy = &pdev->phys[i];
+		phy->base = pdev->base;
+		phy->idx = i;
+		irq = platform_get_irq(op, i);
+		if ((nr_irq > 1) && (irq > 0))
+			ret = devm_request_irq(&op->dev, irq,
+					       pxad_chan_handler,
+					       IRQF_SHARED, "pxa-dma", phy);
+		if ((nr_irq == 1) && (i == 0))
+			ret = devm_request_irq(&op->dev, irq0,
+					       pxad_int_handler,
+					       IRQF_SHARED, "pxa-dma", pdev);
+		if (ret) {
+			dev_err(pdev->slave.dev,
+				"%s(): can't request irq %d:%d\n", __func__,
+				irq, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static const struct of_device_id const pxad_dt_ids[] = {
+	{ .compatible = "marvell,pdma-1.0", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, pxad_dt_ids);
+
+static struct dma_chan *pxad_dma_xlate(struct of_phandle_args *dma_spec,
+					   struct of_dma *ofdma)
+{
+	struct pxad_device *d = ofdma->of_dma_data;
+	struct dma_chan *chan;
+
+	chan = dma_get_any_slave_channel(&d->slave);
+	if (!chan)
+		return NULL;
+
+	to_pxad_chan(chan)->drcmr = dma_spec->args[0];
+	to_pxad_chan(chan)->prio = dma_spec->args[1];
+
+	return chan;
+}
+
+static int pxad_init_dmadev(struct platform_device *op,
+			    struct pxad_device *pdev,
+			    unsigned int nr_phy_chans)
+{
+	int ret;
+	unsigned int i;
+	struct pxad_chan *c;
+
+	pdev->nr_chans = nr_phy_chans;
+	INIT_LIST_HEAD(&pdev->slave.channels);
+	pdev->slave.device_alloc_chan_resources = pxad_alloc_chan_resources;
+	pdev->slave.device_free_chan_resources = pxad_free_chan_resources;
+	pdev->slave.device_tx_status = pxad_tx_status;
+	pdev->slave.device_issue_pending = pxad_issue_pending;
+	pdev->slave.device_config = pxad_config;
+	pdev->slave.device_terminate_all = pxad_terminate_all;
+
+	if (op->dev.coherent_dma_mask)
+		dma_set_mask(&op->dev, op->dev.coherent_dma_mask);
+	else
+		dma_set_mask(&op->dev, DMA_BIT_MASK(32));
+
+	ret = pxad_init_phys(op, pdev, nr_phy_chans);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < nr_phy_chans; i++) {
+		c = devm_kzalloc(&op->dev, sizeof(*c), GFP_KERNEL);
+		if (!c)
+			return -ENOMEM;
+		c->vc.desc_free = pxad_free_desc;
+		vchan_init(&c->vc, &pdev->slave);
+	}
+
+	return dma_async_device_register(&pdev->slave);
+}
+
+static int pxad_probe(struct platform_device *op)
+{
+	struct pxad_device *pdev;
+	const struct of_device_id *of_id;
+	struct mmp_dma_platdata *pdata = dev_get_platdata(&op->dev);
+	struct resource *iores;
+	int ret, dma_channels = 0;
+	const enum dma_slave_buswidth widths =
+		DMA_SLAVE_BUSWIDTH_1_BYTE   | DMA_SLAVE_BUSWIDTH_2_BYTES |
+		DMA_SLAVE_BUSWIDTH_4_BYTES;
+
+	pdev = devm_kzalloc(&op->dev, sizeof(*pdev), GFP_KERNEL);
+	if (!pdev)
+		return -ENOMEM;
+
+	spin_lock_init(&pdev->phy_lock);
+
+	iores = platform_get_resource(op, IORESOURCE_MEM, 0);
+	pdev->base = devm_ioremap_resource(&op->dev, iores);
+	if (IS_ERR(pdev->base))
+		return PTR_ERR(pdev->base);
+
+	of_id = of_match_device(pxad_dt_ids, &op->dev);
+	if (of_id)
+		of_property_read_u32(op->dev.of_node, "#dma-channels",
+				     &dma_channels);
+	else if (pdata && pdata->dma_channels)
+		dma_channels = pdata->dma_channels;
+	else
+		dma_channels = 32;	/* default 32 channel */
+
+	dma_cap_set(DMA_SLAVE, pdev->slave.cap_mask);
+	dma_cap_set(DMA_MEMCPY, pdev->slave.cap_mask);
+	dma_cap_set(DMA_CYCLIC, pdev->slave.cap_mask);
+	dma_cap_set(DMA_PRIVATE, pdev->slave.cap_mask);
+	pdev->slave.device_prep_dma_memcpy = pxad_prep_memcpy;
+	pdev->slave.device_prep_slave_sg = pxad_prep_slave_sg;
+	pdev->slave.device_prep_dma_cyclic = pxad_prep_dma_cyclic;
+
+	pdev->slave.copy_align = PDMA_ALIGNMENT;
+	pdev->slave.src_addr_widths = widths;
+	pdev->slave.dst_addr_widths = widths;
+	pdev->slave.directions = BIT(DMA_MEM_TO_DEV) | BIT(DMA_DEV_TO_MEM);
+	pdev->slave.residue_granularity = DMA_RESIDUE_GRANULARITY_DESCRIPTOR;
+
+	pdev->slave.dev = &op->dev;
+	ret = pxad_init_dmadev(op, pdev, dma_channels);
+	if (ret) {
+		dev_err(pdev->slave.dev, "unable to register\n");
+		return ret;
+	}
+
+	if (op->dev.of_node) {
+		/* Device-tree DMA controller registration */
+		ret = of_dma_controller_register(op->dev.of_node,
+						 pxad_dma_xlate, pdev);
+		if (ret < 0) {
+			dev_err(pdev->slave.dev,
+				"of_dma_controller_register failed\n");
+			return ret;
+		}
+	}
+
+	platform_set_drvdata(op, pdev);
+	pxad_init_debugfs(pdev);
+	dev_info(pdev->slave.dev, "initialized %d channels\n", dma_channels);
+	return 0;
+}
+
+static const struct platform_device_id pxad_id_table[] = {
+	{ "pxa-dma", },
+	{ },
+};
+
+static struct platform_driver pxad_driver = {
+	.driver		= {
+		.name	= "pxa-dma",
+		.of_match_table = pxad_dt_ids,
+	},
+	.id_table	= pxad_id_table,
+	.probe		= pxad_probe,
+	.remove		= pxad_remove,
+};
+
+bool pxad_filter_fn(struct dma_chan *chan, void *param)
+{
+	struct pxad_chan *c = to_pxad_chan(chan);
+	struct pxad_param *p = param;
+
+	if (chan->device->dev->driver != &pxad_driver.driver)
+		return false;
+
+	c->drcmr = p->drcmr;
+	c->prio = p->prio;
+
+	return true;
+}
+EXPORT_SYMBOL_GPL(pxad_filter_fn);
+
+int pxad_toggle_reserved_channel(int legacy_channel)
+{
+	if (legacy_unavailable & (BIT(legacy_channel)))
+		return -EBUSY;
+	legacy_reserved ^= BIT(legacy_channel);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(pxad_toggle_reserved_channel);
+
+module_platform_driver(pxad_driver);
+
+MODULE_DESCRIPTION("Marvell PXA Peripheral DMA Driver");
+MODULE_AUTHOR("Robert Jarzmik <robert.jarzmik@free.fr>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/s3c24xx-dma.c b/drivers/dma/s3c24xx-dma.c
index 01dcaf21b988..17ccdfd28f37 100644
--- a/drivers/dma/s3c24xx-dma.c
+++ b/drivers/dma/s3c24xx-dma.c
@@ -1168,7 +1168,7 @@ static struct soc_data soc_s3c2443 = {
 	.has_clocks = true,
 };
 
-static struct platform_device_id s3c24xx_dma_driver_ids[] = {
+static const struct platform_device_id s3c24xx_dma_driver_ids[] = {
 	{
 		.name		= "s3c2410-dma",
 		.driver_data	= (kernel_ulong_t)&soc_s3c2410,
diff --git a/drivers/dma/sh/rcar-dmac.c b/drivers/dma/sh/rcar-dmac.c
index a18d16cc4795..7820d07e7bee 100644
--- a/drivers/dma/sh/rcar-dmac.c
+++ b/drivers/dma/sh/rcar-dmac.c
@@ -183,7 +183,7 @@ struct rcar_dmac {
 	unsigned int n_channels;
 	struct rcar_dmac_chan *channels;
 
-	unsigned long modules[256 / BITS_PER_LONG];
+	DECLARE_BITMAP(modules, 256);
 };
 
 #define to_rcar_dmac(d)		container_of(d, struct rcar_dmac, engine)
@@ -465,6 +465,7 @@ static dma_cookie_t rcar_dmac_tx_submit(struct dma_async_tx_descriptor *tx)
 static int rcar_dmac_desc_alloc(struct rcar_dmac_chan *chan, gfp_t gfp)
 {
 	struct rcar_dmac_desc_page *page;
+	unsigned long flags;
 	LIST_HEAD(list);
 	unsigned int i;
 
@@ -482,10 +483,10 @@ static int rcar_dmac_desc_alloc(struct rcar_dmac_chan *chan, gfp_t gfp)
 		list_add_tail(&desc->node, &list);
 	}
 
-	spin_lock_irq(&chan->lock);
+	spin_lock_irqsave(&chan->lock, flags);
 	list_splice_tail(&list, &chan->desc.free);
 	list_add_tail(&page->node, &chan->desc.pages);
-	spin_unlock_irq(&chan->lock);
+	spin_unlock_irqrestore(&chan->lock, flags);
 
 	return 0;
 }
@@ -516,6 +517,7 @@ static void rcar_dmac_desc_put(struct rcar_dmac_chan *chan,
 static void rcar_dmac_desc_recycle_acked(struct rcar_dmac_chan *chan)
 {
 	struct rcar_dmac_desc *desc, *_desc;
+	unsigned long flags;
 	LIST_HEAD(list);
 
 	/*
@@ -524,9 +526,9 @@ static void rcar_dmac_desc_recycle_acked(struct rcar_dmac_chan *chan)
 	 * list_for_each_entry_safe, isn't safe if we release the channel lock
 	 * around the rcar_dmac_desc_put() call.
 	 */
-	spin_lock_irq(&chan->lock);
+	spin_lock_irqsave(&chan->lock, flags);
 	list_splice_init(&chan->desc.wait, &list);
-	spin_unlock_irq(&chan->lock);
+	spin_unlock_irqrestore(&chan->lock, flags);
 
 	list_for_each_entry_safe(desc, _desc, &list, node) {
 		if (async_tx_test_ack(&desc->async_tx)) {
@@ -539,9 +541,9 @@ static void rcar_dmac_desc_recycle_acked(struct rcar_dmac_chan *chan)
 		return;
 
 	/* Put the remaining descriptors back in the wait list. */
-	spin_lock_irq(&chan->lock);
+	spin_lock_irqsave(&chan->lock, flags);
 	list_splice(&list, &chan->desc.wait);
-	spin_unlock_irq(&chan->lock);
+	spin_unlock_irqrestore(&chan->lock, flags);
 }
 
 /*
@@ -556,12 +558,13 @@ static void rcar_dmac_desc_recycle_acked(struct rcar_dmac_chan *chan)
 static struct rcar_dmac_desc *rcar_dmac_desc_get(struct rcar_dmac_chan *chan)
 {
 	struct rcar_dmac_desc *desc;
+	unsigned long flags;
 	int ret;
 
 	/* Recycle acked descriptors before attempting allocation. */
 	rcar_dmac_desc_recycle_acked(chan);
 
-	spin_lock_irq(&chan->lock);
+	spin_lock_irqsave(&chan->lock, flags);
 
 	while (list_empty(&chan->desc.free)) {
 		/*
@@ -570,17 +573,17 @@ static struct rcar_dmac_desc *rcar_dmac_desc_get(struct rcar_dmac_chan *chan)
 		 * allocated descriptors. If the allocation fails return an
 		 * error.
 		 */
-		spin_unlock_irq(&chan->lock);
+		spin_unlock_irqrestore(&chan->lock, flags);
 		ret = rcar_dmac_desc_alloc(chan, GFP_NOWAIT);
 		if (ret < 0)
 			return NULL;
-		spin_lock_irq(&chan->lock);
+		spin_lock_irqsave(&chan->lock, flags);
 	}
 
 	desc = list_first_entry(&chan->desc.free, struct rcar_dmac_desc, node);
 	list_del(&desc->node);
 
-	spin_unlock_irq(&chan->lock);
+	spin_unlock_irqrestore(&chan->lock, flags);
 
 	return desc;
 }
@@ -593,6 +596,7 @@ static struct rcar_dmac_desc *rcar_dmac_desc_get(struct rcar_dmac_chan *chan)
 static int rcar_dmac_xfer_chunk_alloc(struct rcar_dmac_chan *chan, gfp_t gfp)
 {
 	struct rcar_dmac_desc_page *page;
+	unsigned long flags;
 	LIST_HEAD(list);
 	unsigned int i;
 
@@ -606,10 +610,10 @@ static int rcar_dmac_xfer_chunk_alloc(struct rcar_dmac_chan *chan, gfp_t gfp)
 		list_add_tail(&chunk->node, &list);
 	}
 
-	spin_lock_irq(&chan->lock);
+	spin_lock_irqsave(&chan->lock, flags);
 	list_splice_tail(&list, &chan->desc.chunks_free);
 	list_add_tail(&page->node, &chan->desc.pages);
-	spin_unlock_irq(&chan->lock);
+	spin_unlock_irqrestore(&chan->lock, flags);
 
 	return 0;
 }
@@ -627,9 +631,10 @@ static struct rcar_dmac_xfer_chunk *
 rcar_dmac_xfer_chunk_get(struct rcar_dmac_chan *chan)
 {
 	struct rcar_dmac_xfer_chunk *chunk;
+	unsigned long flags;
 	int ret;
 
-	spin_lock_irq(&chan->lock);
+	spin_lock_irqsave(&chan->lock, flags);
 
 	while (list_empty(&chan->desc.chunks_free)) {
 		/*
@@ -638,18 +643,18 @@ rcar_dmac_xfer_chunk_get(struct rcar_dmac_chan *chan)
 		 * allocated descriptors. If the allocation fails return an
 		 * error.
 		 */
-		spin_unlock_irq(&chan->lock);
+		spin_unlock_irqrestore(&chan->lock, flags);
 		ret = rcar_dmac_xfer_chunk_alloc(chan, GFP_NOWAIT);
 		if (ret < 0)
 			return NULL;
-		spin_lock_irq(&chan->lock);
+		spin_lock_irqsave(&chan->lock, flags);
 	}
 
 	chunk = list_first_entry(&chan->desc.chunks_free,
 				 struct rcar_dmac_xfer_chunk, node);
 	list_del(&chunk->node);
 
-	spin_unlock_irq(&chan->lock);
+	spin_unlock_irqrestore(&chan->lock, flags);
 
 	return chunk;
 }
diff --git a/drivers/dma/sh/shdma-r8a73a4.c b/drivers/dma/sh/shdma-r8a73a4.c
index 4fb99970a3ea..96ea3828c3eb 100644
--- a/drivers/dma/sh/shdma-r8a73a4.c
+++ b/drivers/dma/sh/shdma-r8a73a4.c
@@ -11,7 +11,7 @@
 
 #include "shdma-arm.h"
 
-const unsigned int dma_ts_shift[] = SH_DMAE_TS_SHIFT;
+static const unsigned int dma_ts_shift[] = SH_DMAE_TS_SHIFT;
 
 static const struct sh_dmae_slave_config dma_slaves[] = {
 	{
diff --git a/drivers/dma/sirf-dma.c b/drivers/dma/sirf-dma.c
index a1afda43b8ef..8c5186cc9f63 100644
--- a/drivers/dma/sirf-dma.c
+++ b/drivers/dma/sirf-dma.c
@@ -23,8 +23,13 @@
 
 #include "dmaengine.h"
 
+#define SIRFSOC_DMA_VER_A7V1                    1
+#define SIRFSOC_DMA_VER_A7V2                    2
+#define SIRFSOC_DMA_VER_A6                      4
+
 #define SIRFSOC_DMA_DESCRIPTORS                 16
 #define SIRFSOC_DMA_CHANNELS                    16
+#define SIRFSOC_DMA_TABLE_NUM                   256
 
 #define SIRFSOC_DMA_CH_ADDR                     0x00
 #define SIRFSOC_DMA_CH_XLEN                     0x04
@@ -35,15 +40,44 @@
 #define SIRFSOC_DMA_CH_VALID                    0x140
 #define SIRFSOC_DMA_CH_INT                      0x144
 #define SIRFSOC_DMA_INT_EN                      0x148
-#define SIRFSOC_DMA_INT_EN_CLR			0x14C
+#define SIRFSOC_DMA_INT_EN_CLR                  0x14C
 #define SIRFSOC_DMA_CH_LOOP_CTRL                0x150
-#define SIRFSOC_DMA_CH_LOOP_CTRL_CLR            0x15C
+#define SIRFSOC_DMA_CH_LOOP_CTRL_CLR            0x154
+#define SIRFSOC_DMA_WIDTH_ATLAS7                0x10
+#define SIRFSOC_DMA_VALID_ATLAS7                0x14
+#define SIRFSOC_DMA_INT_ATLAS7                  0x18
+#define SIRFSOC_DMA_INT_EN_ATLAS7               0x1c
+#define SIRFSOC_DMA_LOOP_CTRL_ATLAS7            0x20
+#define SIRFSOC_DMA_CUR_DATA_ADDR               0x34
+#define SIRFSOC_DMA_MUL_ATLAS7                  0x38
+#define SIRFSOC_DMA_CH_LOOP_CTRL_ATLAS7         0x158
+#define SIRFSOC_DMA_CH_LOOP_CTRL_CLR_ATLAS7     0x15C
+#define SIRFSOC_DMA_IOBG_SCMD_EN		0x800
+#define SIRFSOC_DMA_EARLY_RESP_SET		0x818
+#define SIRFSOC_DMA_EARLY_RESP_CLR		0x81C
 
 #define SIRFSOC_DMA_MODE_CTRL_BIT               4
 #define SIRFSOC_DMA_DIR_CTRL_BIT                5
+#define SIRFSOC_DMA_MODE_CTRL_BIT_ATLAS7        2
+#define SIRFSOC_DMA_CHAIN_CTRL_BIT_ATLAS7       3
+#define SIRFSOC_DMA_DIR_CTRL_BIT_ATLAS7         4
+#define SIRFSOC_DMA_TAB_NUM_ATLAS7              7
+#define SIRFSOC_DMA_CHAIN_INT_BIT_ATLAS7        5
+#define SIRFSOC_DMA_CHAIN_FLAG_SHIFT_ATLAS7     25
+#define SIRFSOC_DMA_CHAIN_ADDR_SHIFT            32
+
+#define SIRFSOC_DMA_INT_FINI_INT_ATLAS7         BIT(0)
+#define SIRFSOC_DMA_INT_CNT_INT_ATLAS7          BIT(1)
+#define SIRFSOC_DMA_INT_PAU_INT_ATLAS7          BIT(2)
+#define SIRFSOC_DMA_INT_LOOP_INT_ATLAS7         BIT(3)
+#define SIRFSOC_DMA_INT_INV_INT_ATLAS7          BIT(4)
+#define SIRFSOC_DMA_INT_END_INT_ATLAS7          BIT(5)
+#define SIRFSOC_DMA_INT_ALL_ATLAS7              0x3F
 
 /* xlen and dma_width register is in 4 bytes boundary */
 #define SIRFSOC_DMA_WORD_LEN			4
+#define SIRFSOC_DMA_XLEN_MAX_V1         0x800
+#define SIRFSOC_DMA_XLEN_MAX_V2         0x1000
 
 struct sirfsoc_dma_desc {
 	struct dma_async_tx_descriptor	desc;
@@ -56,7 +90,9 @@ struct sirfsoc_dma_desc {
 	int             width;          /* DMA width */
 	int             dir;
 	bool            cyclic;         /* is loop DMA? */
+	bool            chain;          /* is chain DMA? */
 	u32             addr;		/* DMA buffer address */
+	u64 chain_table[SIRFSOC_DMA_TABLE_NUM]; /* chain tbl */
 };
 
 struct sirfsoc_dma_chan {
@@ -87,10 +123,25 @@ struct sirfsoc_dma {
 	void __iomem			*base;
 	int				irq;
 	struct clk			*clk;
-	bool				is_marco;
+	int				type;
+	void (*exec_desc)(struct sirfsoc_dma_desc *sdesc,
+		int cid, int burst_mode, void __iomem *base);
 	struct sirfsoc_dma_regs		regs_save;
 };
 
+struct sirfsoc_dmadata {
+	void (*exec)(struct sirfsoc_dma_desc *sdesc,
+		int cid, int burst_mode, void __iomem *base);
+	int type;
+};
+
+enum sirfsoc_dma_chain_flag {
+	SIRFSOC_DMA_CHAIN_NORMAL = 0x01,
+	SIRFSOC_DMA_CHAIN_PAUSE = 0x02,
+	SIRFSOC_DMA_CHAIN_LOOP = 0x03,
+	SIRFSOC_DMA_CHAIN_END = 0x04
+};
+
 #define DRV_NAME	"sirfsoc_dma"
 
 static int sirfsoc_dma_runtime_suspend(struct device *dev);
@@ -109,48 +160,105 @@ static inline struct sirfsoc_dma *dma_chan_to_sirfsoc_dma(struct dma_chan *c)
 	return container_of(schan, struct sirfsoc_dma, channels[c->chan_id]);
 }
 
+static void sirfsoc_dma_execute_hw_a7v2(struct sirfsoc_dma_desc *sdesc,
+		int cid, int burst_mode, void __iomem *base)
+{
+	if (sdesc->chain) {
+		/* DMA v2 HW chain mode */
+		writel_relaxed((sdesc->dir << SIRFSOC_DMA_DIR_CTRL_BIT_ATLAS7) |
+			       (sdesc->chain <<
+				SIRFSOC_DMA_CHAIN_CTRL_BIT_ATLAS7) |
+			       (0x8 << SIRFSOC_DMA_TAB_NUM_ATLAS7) | 0x3,
+			       base + SIRFSOC_DMA_CH_CTRL);
+	} else {
+		/* DMA v2 legacy mode */
+		writel_relaxed(sdesc->xlen, base + SIRFSOC_DMA_CH_XLEN);
+		writel_relaxed(sdesc->ylen, base + SIRFSOC_DMA_CH_YLEN);
+		writel_relaxed(sdesc->width, base + SIRFSOC_DMA_WIDTH_ATLAS7);
+		writel_relaxed((sdesc->width*((sdesc->ylen+1)>>1)),
+				base + SIRFSOC_DMA_MUL_ATLAS7);
+		writel_relaxed((sdesc->dir << SIRFSOC_DMA_DIR_CTRL_BIT_ATLAS7) |
+			       (sdesc->chain <<
+				SIRFSOC_DMA_CHAIN_CTRL_BIT_ATLAS7) |
+			       0x3, base + SIRFSOC_DMA_CH_CTRL);
+	}
+	writel_relaxed(sdesc->chain ? SIRFSOC_DMA_INT_END_INT_ATLAS7 :
+		       (SIRFSOC_DMA_INT_FINI_INT_ATLAS7 |
+			SIRFSOC_DMA_INT_LOOP_INT_ATLAS7),
+		       base + SIRFSOC_DMA_INT_EN_ATLAS7);
+	writel(sdesc->addr, base + SIRFSOC_DMA_CH_ADDR);
+	if (sdesc->cyclic)
+		writel(0x10001, base + SIRFSOC_DMA_LOOP_CTRL_ATLAS7);
+}
+
+static void sirfsoc_dma_execute_hw_a7v1(struct sirfsoc_dma_desc *sdesc,
+		int cid, int burst_mode, void __iomem *base)
+{
+	writel_relaxed(1, base + SIRFSOC_DMA_IOBG_SCMD_EN);
+	writel_relaxed((1 << cid), base + SIRFSOC_DMA_EARLY_RESP_SET);
+	writel_relaxed(sdesc->width, base + SIRFSOC_DMA_WIDTH_0 + cid * 4);
+	writel_relaxed(cid | (burst_mode << SIRFSOC_DMA_MODE_CTRL_BIT) |
+		       (sdesc->dir << SIRFSOC_DMA_DIR_CTRL_BIT),
+		       base + cid * 0x10 + SIRFSOC_DMA_CH_CTRL);
+	writel_relaxed(sdesc->xlen, base + cid * 0x10 + SIRFSOC_DMA_CH_XLEN);
+	writel_relaxed(sdesc->ylen, base + cid * 0x10 + SIRFSOC_DMA_CH_YLEN);
+	writel_relaxed(readl_relaxed(base + SIRFSOC_DMA_INT_EN) |
+		       (1 << cid), base + SIRFSOC_DMA_INT_EN);
+	writel(sdesc->addr >> 2, base + cid * 0x10 + SIRFSOC_DMA_CH_ADDR);
+	if (sdesc->cyclic) {
+		writel((1 << cid) | 1 << (cid + 16) |
+		       readl_relaxed(base + SIRFSOC_DMA_CH_LOOP_CTRL_ATLAS7),
+		       base + SIRFSOC_DMA_CH_LOOP_CTRL_ATLAS7);
+	}
+
+}
+
+static void sirfsoc_dma_execute_hw_a6(struct sirfsoc_dma_desc *sdesc,
+		int cid, int burst_mode, void __iomem *base)
+{
+	writel_relaxed(sdesc->width, base + SIRFSOC_DMA_WIDTH_0 + cid * 4);
+	writel_relaxed(cid | (burst_mode << SIRFSOC_DMA_MODE_CTRL_BIT) |
+		       (sdesc->dir << SIRFSOC_DMA_DIR_CTRL_BIT),
+		       base + cid * 0x10 + SIRFSOC_DMA_CH_CTRL);
+	writel_relaxed(sdesc->xlen, base + cid * 0x10 + SIRFSOC_DMA_CH_XLEN);
+	writel_relaxed(sdesc->ylen, base + cid * 0x10 + SIRFSOC_DMA_CH_YLEN);
+	writel_relaxed(readl_relaxed(base + SIRFSOC_DMA_INT_EN) |
+		       (1 << cid), base + SIRFSOC_DMA_INT_EN);
+	writel(sdesc->addr >> 2, base + cid * 0x10 + SIRFSOC_DMA_CH_ADDR);
+	if (sdesc->cyclic) {
+		writel((1 << cid) | 1 << (cid + 16) |
+		       readl_relaxed(base + SIRFSOC_DMA_CH_LOOP_CTRL),
+		       base + SIRFSOC_DMA_CH_LOOP_CTRL);
+	}
+
+}
+
 /* Execute all queued DMA descriptors */
 static void sirfsoc_dma_execute(struct sirfsoc_dma_chan *schan)
 {
 	struct sirfsoc_dma *sdma = dma_chan_to_sirfsoc_dma(&schan->chan);
 	int cid = schan->chan.chan_id;
 	struct sirfsoc_dma_desc *sdesc = NULL;
+	void __iomem *base;
 
 	/*
 	 * lock has been held by functions calling this, so we don't hold
 	 * lock again
 	 */
-
+	base = sdma->base;
 	sdesc = list_first_entry(&schan->queued, struct sirfsoc_dma_desc,
-		node);
+				 node);
 	/* Move the first queued descriptor to active list */
 	list_move_tail(&sdesc->node, &schan->active);
 
-	/* Start the DMA transfer */
-	writel_relaxed(sdesc->width, sdma->base + SIRFSOC_DMA_WIDTH_0 +
-		cid * 4);
-	writel_relaxed(cid | (schan->mode << SIRFSOC_DMA_MODE_CTRL_BIT) |
-		(sdesc->dir << SIRFSOC_DMA_DIR_CTRL_BIT),
-		sdma->base + cid * 0x10 + SIRFSOC_DMA_CH_CTRL);
-	writel_relaxed(sdesc->xlen, sdma->base + cid * 0x10 +
-		SIRFSOC_DMA_CH_XLEN);
-	writel_relaxed(sdesc->ylen, sdma->base + cid * 0x10 +
-		SIRFSOC_DMA_CH_YLEN);
-	writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_INT_EN) |
-		(1 << cid), sdma->base + SIRFSOC_DMA_INT_EN);
+	if (sdma->type == SIRFSOC_DMA_VER_A7V2)
+		cid = 0;
 
-	/*
-	 * writel has an implict memory write barrier to make sure data is
-	 * flushed into memory before starting DMA
-	 */
-	writel(sdesc->addr >> 2, sdma->base + cid * 0x10 + SIRFSOC_DMA_CH_ADDR);
+	/* Start the DMA transfer */
+	sdma->exec_desc(sdesc, cid, schan->mode, base);
 
-	if (sdesc->cyclic) {
-		writel((1 << cid) | 1 << (cid + 16) |
-			readl_relaxed(sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL),
-			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
+	if (sdesc->cyclic)
 		schan->happened_cyclic = schan->completed_cyclic = 0;
-	}
 }
 
 /* Interrupt handler */
@@ -160,27 +268,65 @@ static irqreturn_t sirfsoc_dma_irq(int irq, void *data)
 	struct sirfsoc_dma_chan *schan;
 	struct sirfsoc_dma_desc *sdesc = NULL;
 	u32 is;
+	bool chain;
 	int ch;
+	void __iomem *reg;
+
+	switch (sdma->type) {
+	case SIRFSOC_DMA_VER_A6:
+	case SIRFSOC_DMA_VER_A7V1:
+		is = readl(sdma->base + SIRFSOC_DMA_CH_INT);
+		reg = sdma->base + SIRFSOC_DMA_CH_INT;
+		while ((ch = fls(is) - 1) >= 0) {
+			is &= ~(1 << ch);
+			writel_relaxed(1 << ch, reg);
+			schan = &sdma->channels[ch];
+			spin_lock(&schan->lock);
+			sdesc = list_first_entry(&schan->active,
+						 struct sirfsoc_dma_desc, node);
+			if (!sdesc->cyclic) {
+				/* Execute queued descriptors */
+				list_splice_tail_init(&schan->active,
+						      &schan->completed);
+				dma_cookie_complete(&sdesc->desc);
+				if (!list_empty(&schan->queued))
+					sirfsoc_dma_execute(schan);
+			} else
+				schan->happened_cyclic++;
+			spin_unlock(&schan->lock);
+		}
+		break;
 
-	is = readl(sdma->base + SIRFSOC_DMA_CH_INT);
-	while ((ch = fls(is) - 1) >= 0) {
-		is &= ~(1 << ch);
-		writel_relaxed(1 << ch, sdma->base + SIRFSOC_DMA_CH_INT);
-		schan = &sdma->channels[ch];
+	case SIRFSOC_DMA_VER_A7V2:
+		is = readl(sdma->base + SIRFSOC_DMA_INT_ATLAS7);
 
+		reg = sdma->base + SIRFSOC_DMA_INT_ATLAS7;
+		writel_relaxed(SIRFSOC_DMA_INT_ALL_ATLAS7, reg);
+		schan = &sdma->channels[0];
 		spin_lock(&schan->lock);
-
-		sdesc = list_first_entry(&schan->active, struct sirfsoc_dma_desc,
-			node);
+		sdesc = list_first_entry(&schan->active,
+					 struct sirfsoc_dma_desc, node);
 		if (!sdesc->cyclic) {
-			/* Execute queued descriptors */
-			list_splice_tail_init(&schan->active, &schan->completed);
-			if (!list_empty(&schan->queued))
-				sirfsoc_dma_execute(schan);
-		} else
+			chain = sdesc->chain;
+			if ((chain && (is & SIRFSOC_DMA_INT_END_INT_ATLAS7)) ||
+				(!chain &&
+				(is & SIRFSOC_DMA_INT_FINI_INT_ATLAS7))) {
+				/* Execute queued descriptors */
+				list_splice_tail_init(&schan->active,
+						      &schan->completed);
+				dma_cookie_complete(&sdesc->desc);
+				if (!list_empty(&schan->queued))
+					sirfsoc_dma_execute(schan);
+			}
+		} else if (sdesc->cyclic && (is &
+					SIRFSOC_DMA_INT_LOOP_INT_ATLAS7))
 			schan->happened_cyclic++;
 
 		spin_unlock(&schan->lock);
+		break;
+
+	default:
+		break;
 	}
 
 	/* Schedule tasklet */
@@ -227,16 +373,15 @@ static void sirfsoc_dma_process_completed(struct sirfsoc_dma *sdma)
 			schan->chan.completed_cookie = last_cookie;
 			spin_unlock_irqrestore(&schan->lock, flags);
 		} else {
-			/* for cyclic channel, desc is always in active list */
-			sdesc = list_first_entry(&schan->active, struct sirfsoc_dma_desc,
-				node);
-
-			if (!sdesc || (sdesc && !sdesc->cyclic)) {
-				/* without active cyclic DMA */
+			if (list_empty(&schan->active)) {
 				spin_unlock_irqrestore(&schan->lock, flags);
 				continue;
 			}
 
+			/* for cyclic channel, desc is always in active list */
+			sdesc = list_first_entry(&schan->active,
+				struct sirfsoc_dma_desc, node);
+
 			/* cyclic DMA */
 			happened_cyclic = schan->happened_cyclic;
 			spin_unlock_irqrestore(&schan->lock, flags);
@@ -307,20 +452,32 @@ static int sirfsoc_dma_terminate_all(struct dma_chan *chan)
 
 	spin_lock_irqsave(&schan->lock, flags);
 
-	if (!sdma->is_marco) {
-		writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_INT_EN) &
-			~(1 << cid), sdma->base + SIRFSOC_DMA_INT_EN);
-		writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL)
-			& ~((1 << cid) | 1 << (cid + 16)),
-			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
-	} else {
+	switch (sdma->type) {
+	case SIRFSOC_DMA_VER_A7V1:
 		writel_relaxed(1 << cid, sdma->base + SIRFSOC_DMA_INT_EN_CLR);
 		writel_relaxed((1 << cid) | 1 << (cid + 16),
-			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL_CLR);
+			       sdma->base +
+			       SIRFSOC_DMA_CH_LOOP_CTRL_CLR_ATLAS7);
+		writel_relaxed(1 << cid, sdma->base + SIRFSOC_DMA_CH_VALID);
+		break;
+	case SIRFSOC_DMA_VER_A7V2:
+		writel_relaxed(0, sdma->base + SIRFSOC_DMA_INT_EN_ATLAS7);
+		writel_relaxed(0, sdma->base + SIRFSOC_DMA_LOOP_CTRL_ATLAS7);
+		writel_relaxed(0, sdma->base + SIRFSOC_DMA_VALID_ATLAS7);
+		break;
+	case SIRFSOC_DMA_VER_A6:
+		writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_INT_EN) &
+			       ~(1 << cid), sdma->base + SIRFSOC_DMA_INT_EN);
+		writel_relaxed(readl_relaxed(sdma->base +
+					     SIRFSOC_DMA_CH_LOOP_CTRL) &
+			       ~((1 << cid) | 1 << (cid + 16)),
+			       sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
+		writel_relaxed(1 << cid, sdma->base + SIRFSOC_DMA_CH_VALID);
+		break;
+	default:
+		break;
 	}
 
-	writel_relaxed(1 << cid, sdma->base + SIRFSOC_DMA_CH_VALID);
-
 	list_splice_tail_init(&schan->active, &schan->free);
 	list_splice_tail_init(&schan->queued, &schan->free);
 
@@ -338,13 +495,25 @@ static int sirfsoc_dma_pause_chan(struct dma_chan *chan)
 
 	spin_lock_irqsave(&schan->lock, flags);
 
-	if (!sdma->is_marco)
-		writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL)
-			& ~((1 << cid) | 1 << (cid + 16)),
-			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
-	else
+	switch (sdma->type) {
+	case SIRFSOC_DMA_VER_A7V1:
 		writel_relaxed((1 << cid) | 1 << (cid + 16),
-			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL_CLR);
+			       sdma->base +
+			       SIRFSOC_DMA_CH_LOOP_CTRL_CLR_ATLAS7);
+		break;
+	case SIRFSOC_DMA_VER_A7V2:
+		writel_relaxed(0, sdma->base + SIRFSOC_DMA_LOOP_CTRL_ATLAS7);
+		break;
+	case SIRFSOC_DMA_VER_A6:
+		writel_relaxed(readl_relaxed(sdma->base +
+					     SIRFSOC_DMA_CH_LOOP_CTRL) &
+			       ~((1 << cid) | 1 << (cid + 16)),
+			       sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
+		break;
+
+	default:
+		break;
+	}
 
 	spin_unlock_irqrestore(&schan->lock, flags);
 
@@ -359,14 +528,25 @@ static int sirfsoc_dma_resume_chan(struct dma_chan *chan)
 	unsigned long flags;
 
 	spin_lock_irqsave(&schan->lock, flags);
-
-	if (!sdma->is_marco)
-		writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL)
-			| ((1 << cid) | 1 << (cid + 16)),
-			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
-	else
+	switch (sdma->type) {
+	case SIRFSOC_DMA_VER_A7V1:
 		writel_relaxed((1 << cid) | 1 << (cid + 16),
-			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
+			       sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL_ATLAS7);
+		break;
+	case SIRFSOC_DMA_VER_A7V2:
+		writel_relaxed(0x10001,
+			       sdma->base + SIRFSOC_DMA_LOOP_CTRL_ATLAS7);
+		break;
+	case SIRFSOC_DMA_VER_A6:
+		writel_relaxed(readl_relaxed(sdma->base +
+					     SIRFSOC_DMA_CH_LOOP_CTRL) |
+			       ((1 << cid) | 1 << (cid + 16)),
+			       sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
+		break;
+
+	default:
+		break;
+	}
 
 	spin_unlock_irqrestore(&schan->lock, flags);
 
@@ -473,14 +653,31 @@ sirfsoc_dma_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 
 	spin_lock_irqsave(&schan->lock, flags);
 
-	sdesc = list_first_entry(&schan->active, struct sirfsoc_dma_desc,
-			node);
-	dma_request_bytes = (sdesc->xlen + 1) * (sdesc->ylen + 1) *
-		(sdesc->width * SIRFSOC_DMA_WORD_LEN);
+	if (list_empty(&schan->active)) {
+		ret = dma_cookie_status(chan, cookie, txstate);
+		dma_set_residue(txstate, 0);
+		spin_unlock_irqrestore(&schan->lock, flags);
+		return ret;
+	}
+	sdesc = list_first_entry(&schan->active, struct sirfsoc_dma_desc, node);
+	if (sdesc->cyclic)
+		dma_request_bytes = (sdesc->xlen + 1) * (sdesc->ylen + 1) *
+			(sdesc->width * SIRFSOC_DMA_WORD_LEN);
+	else
+		dma_request_bytes = sdesc->xlen * SIRFSOC_DMA_WORD_LEN;
 
 	ret = dma_cookie_status(chan, cookie, txstate);
-	dma_pos = readl_relaxed(sdma->base + cid * 0x10 + SIRFSOC_DMA_CH_ADDR)
-		<< 2;
+
+	if (sdma->type == SIRFSOC_DMA_VER_A7V2)
+		cid = 0;
+
+	if (sdma->type == SIRFSOC_DMA_VER_A7V2) {
+		dma_pos = readl_relaxed(sdma->base + SIRFSOC_DMA_CUR_DATA_ADDR);
+	} else {
+		dma_pos = readl_relaxed(
+			sdma->base + cid * 0x10 + SIRFSOC_DMA_CH_ADDR) << 2;
+	}
+
 	residue = dma_request_bytes - (dma_pos - sdesc->addr);
 	dma_set_residue(txstate, residue);
 
@@ -647,6 +844,7 @@ static int sirfsoc_dma_probe(struct platform_device *op)
 	struct dma_device *dma;
 	struct sirfsoc_dma *sdma;
 	struct sirfsoc_dma_chan *schan;
+	struct sirfsoc_dmadata *data;
 	struct resource res;
 	ulong regs_start, regs_size;
 	u32 id;
@@ -657,9 +855,11 @@ static int sirfsoc_dma_probe(struct platform_device *op)
 		dev_err(dev, "Memory exhausted!\n");
 		return -ENOMEM;
 	}
-
-	if (of_device_is_compatible(dn, "sirf,marco-dmac"))
-		sdma->is_marco = true;
+	data = (struct sirfsoc_dmadata *)
+		(of_match_device(op->dev.driver->of_match_table,
+				 &op->dev)->data);
+	sdma->exec_desc = data->exec;
+	sdma->type = data->type;
 
 	if (of_property_read_u32(dn, "cell-index", &id)) {
 		dev_err(dev, "Fail to get DMAC index\n");
@@ -816,6 +1016,8 @@ static int sirfsoc_dma_pm_suspend(struct device *dev)
 	struct sirfsoc_dma_chan *schan;
 	int ch;
 	int ret;
+	int count;
+	u32 int_offset;
 
 	/*
 	 * if we were runtime-suspended before, resume to enable clock
@@ -827,11 +1029,19 @@ static int sirfsoc_dma_pm_suspend(struct device *dev)
 			return ret;
 	}
 
+	if (sdma->type == SIRFSOC_DMA_VER_A7V2) {
+		count = 1;
+		int_offset = SIRFSOC_DMA_INT_EN_ATLAS7;
+	} else {
+		count = SIRFSOC_DMA_CHANNELS;
+		int_offset = SIRFSOC_DMA_INT_EN;
+	}
+
 	/*
 	 * DMA controller will lose all registers while suspending
 	 * so we need to save registers for active channels
 	 */
-	for (ch = 0; ch < SIRFSOC_DMA_CHANNELS; ch++) {
+	for (ch = 0; ch < count; ch++) {
 		schan = &sdma->channels[ch];
 		if (list_empty(&schan->active))
 			continue;
@@ -841,7 +1051,7 @@ static int sirfsoc_dma_pm_suspend(struct device *dev)
 		save->ctrl[ch] = readl_relaxed(sdma->base +
 			ch * 0x10 + SIRFSOC_DMA_CH_CTRL);
 	}
-	save->interrupt_en = readl_relaxed(sdma->base + SIRFSOC_DMA_INT_EN);
+	save->interrupt_en = readl_relaxed(sdma->base + int_offset);
 
 	/* Disable clock */
 	sirfsoc_dma_runtime_suspend(dev);
@@ -857,14 +1067,27 @@ static int sirfsoc_dma_pm_resume(struct device *dev)
 	struct sirfsoc_dma_chan *schan;
 	int ch;
 	int ret;
+	int count;
+	u32 int_offset;
+	u32 width_offset;
 
 	/* Enable clock before accessing register */
 	ret = sirfsoc_dma_runtime_resume(dev);
 	if (ret < 0)
 		return ret;
 
-	writel_relaxed(save->interrupt_en, sdma->base + SIRFSOC_DMA_INT_EN);
-	for (ch = 0; ch < SIRFSOC_DMA_CHANNELS; ch++) {
+	if (sdma->type == SIRFSOC_DMA_VER_A7V2) {
+		count = 1;
+		int_offset = SIRFSOC_DMA_INT_EN_ATLAS7;
+		width_offset = SIRFSOC_DMA_WIDTH_ATLAS7;
+	} else {
+		count = SIRFSOC_DMA_CHANNELS;
+		int_offset = SIRFSOC_DMA_INT_EN;
+		width_offset = SIRFSOC_DMA_WIDTH_0;
+	}
+
+	writel_relaxed(save->interrupt_en, sdma->base + int_offset);
+	for (ch = 0; ch < count; ch++) {
 		schan = &sdma->channels[ch];
 		if (list_empty(&schan->active))
 			continue;
@@ -872,15 +1095,21 @@ static int sirfsoc_dma_pm_resume(struct device *dev)
 			struct sirfsoc_dma_desc,
 			node);
 		writel_relaxed(sdesc->width,
-			sdma->base + SIRFSOC_DMA_WIDTH_0 + ch * 4);
+			sdma->base + width_offset + ch * 4);
 		writel_relaxed(sdesc->xlen,
 			sdma->base + ch * 0x10 + SIRFSOC_DMA_CH_XLEN);
 		writel_relaxed(sdesc->ylen,
 			sdma->base + ch * 0x10 + SIRFSOC_DMA_CH_YLEN);
 		writel_relaxed(save->ctrl[ch],
 			sdma->base + ch * 0x10 + SIRFSOC_DMA_CH_CTRL);
-		writel_relaxed(sdesc->addr >> 2,
-			sdma->base + ch * 0x10 + SIRFSOC_DMA_CH_ADDR);
+		if (sdma->type == SIRFSOC_DMA_VER_A7V2) {
+			writel_relaxed(sdesc->addr,
+				sdma->base + SIRFSOC_DMA_CH_ADDR);
+		} else {
+			writel_relaxed(sdesc->addr >> 2,
+				sdma->base + ch * 0x10 + SIRFSOC_DMA_CH_ADDR);
+
+		}
 	}
 
 	/* if we were runtime-suspended before, suspend again */
@@ -896,9 +1125,25 @@ static const struct dev_pm_ops sirfsoc_dma_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(sirfsoc_dma_pm_suspend, sirfsoc_dma_pm_resume)
 };
 
+struct sirfsoc_dmadata sirfsoc_dmadata_a6 = {
+	.exec = sirfsoc_dma_execute_hw_a6,
+	.type = SIRFSOC_DMA_VER_A6,
+};
+
+struct sirfsoc_dmadata sirfsoc_dmadata_a7v1 = {
+	.exec = sirfsoc_dma_execute_hw_a7v1,
+	.type = SIRFSOC_DMA_VER_A7V1,
+};
+
+struct sirfsoc_dmadata sirfsoc_dmadata_a7v2 = {
+	.exec = sirfsoc_dma_execute_hw_a7v2,
+	.type = SIRFSOC_DMA_VER_A7V2,
+};
+
 static const struct of_device_id sirfsoc_dma_match[] = {
-	{ .compatible = "sirf,prima2-dmac", },
-	{ .compatible = "sirf,marco-dmac", },
+	{ .compatible = "sirf,prima2-dmac", .data = &sirfsoc_dmadata_a6,},
+	{ .compatible = "sirf,atlas7-dmac", .data = &sirfsoc_dmadata_a7v1,},
+	{ .compatible = "sirf,atlas7-dmac-v2", .data = &sirfsoc_dmadata_a7v2,},
 	{},
 };
 
@@ -925,7 +1170,7 @@ static void __exit sirfsoc_dma_exit(void)
 subsys_initcall(sirfsoc_dma_init);
 module_exit(sirfsoc_dma_exit);
 
-MODULE_AUTHOR("Rongjun Ying <rongjun.ying@csr.com>, "
-	"Barry Song <baohua.song@csr.com>");
+MODULE_AUTHOR("Rongjun Ying <rongjun.ying@csr.com>");
+MODULE_AUTHOR("Barry Song <baohua.song@csr.com>");
 MODULE_DESCRIPTION("SIRFSOC DMA control driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/sun6i-dma.c b/drivers/dma/sun6i-dma.c
index 11e536586812..842ff97c2cfb 100644
--- a/drivers/dma/sun6i-dma.c
+++ b/drivers/dma/sun6i-dma.c
@@ -891,9 +891,21 @@ static struct sun6i_dma_config sun8i_a23_dma_cfg = {
 	.nr_max_vchans   = 37,
 };
 
+/*
+ * The H3 has 12 physical channels, a maximum DRQ port id of 27,
+ * and a total of 34 usable source and destination endpoints.
+ */
+
+static struct sun6i_dma_config sun8i_h3_dma_cfg = {
+	.nr_max_channels = 12,
+	.nr_max_requests = 27,
+	.nr_max_vchans   = 34,
+};
+
 static const 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 },
+	{ .compatible = "allwinner,sun8i-h3-dma", .data = &sun8i_h3_dma_cfg },
 	{ /* sentinel */ }
 };
 
diff --git a/drivers/dma/ti-dma-crossbar.c b/drivers/dma/ti-dma-crossbar.c
new file mode 100644
index 000000000000..24f5ca2356bf
--- /dev/null
+++ b/drivers/dma/ti-dma-crossbar.c
@@ -0,0 +1,188 @@
+/*
+ *  Copyright (C) 2015 Texas Instruments Incorporated - http://www.ti.com
+ *  Author: Peter Ujfalusi <peter.ujfalusi@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/slab.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/idr.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_dma.h>
+
+#define TI_XBAR_OUTPUTS	127
+#define TI_XBAR_INPUTS	256
+
+static DEFINE_IDR(map_idr);
+
+struct ti_dma_xbar_data {
+	void __iomem *iomem;
+
+	struct dma_router dmarouter;
+
+	u16 safe_val; /* Value to rest the crossbar lines */
+	u32 xbar_requests; /* number of DMA requests connected to XBAR */
+	u32 dma_requests; /* number of DMA requests forwarded to DMA */
+};
+
+struct ti_dma_xbar_map {
+	u16 xbar_in;
+	int xbar_out;
+};
+
+static inline void ti_dma_xbar_write(void __iomem *iomem, int xbar, u16 val)
+{
+	writew_relaxed(val, iomem + (xbar * 2));
+}
+
+static void ti_dma_xbar_free(struct device *dev, void *route_data)
+{
+	struct ti_dma_xbar_data *xbar = dev_get_drvdata(dev);
+	struct ti_dma_xbar_map *map = route_data;
+
+	dev_dbg(dev, "Unmapping XBAR%u (was routed to %d)\n",
+		map->xbar_in, map->xbar_out);
+
+	ti_dma_xbar_write(xbar->iomem, map->xbar_out, xbar->safe_val);
+	idr_remove(&map_idr, map->xbar_out);
+	kfree(map);
+}
+
+static void *ti_dma_xbar_route_allocate(struct of_phandle_args *dma_spec,
+					struct of_dma *ofdma)
+{
+	struct platform_device *pdev = of_find_device_by_node(ofdma->of_node);
+	struct ti_dma_xbar_data *xbar = platform_get_drvdata(pdev);
+	struct ti_dma_xbar_map *map;
+
+	if (dma_spec->args[0] >= xbar->xbar_requests) {
+		dev_err(&pdev->dev, "Invalid XBAR request number: %d\n",
+			dma_spec->args[0]);
+		return ERR_PTR(-EINVAL);
+	}
+
+	/* The of_node_put() will be done in the core for the node */
+	dma_spec->np = of_parse_phandle(ofdma->of_node, "dma-masters", 0);
+	if (!dma_spec->np) {
+		dev_err(&pdev->dev, "Can't get DMA master\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	map = kzalloc(sizeof(*map), GFP_KERNEL);
+	if (!map) {
+		of_node_put(dma_spec->np);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	map->xbar_out = idr_alloc(&map_idr, NULL, 0, xbar->dma_requests,
+				  GFP_KERNEL);
+	map->xbar_in = (u16)dma_spec->args[0];
+
+	/* The DMA request is 1 based in sDMA */
+	dma_spec->args[0] = map->xbar_out + 1;
+
+	dev_dbg(&pdev->dev, "Mapping XBAR%u to DMA%d\n",
+		map->xbar_in, map->xbar_out);
+
+	ti_dma_xbar_write(xbar->iomem, map->xbar_out, map->xbar_in);
+
+	return map;
+}
+
+static int ti_dma_xbar_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *dma_node;
+	struct ti_dma_xbar_data *xbar;
+	struct resource *res;
+	u32 safe_val;
+	void __iomem *iomem;
+	int i, ret;
+
+	if (!node)
+		return -ENODEV;
+
+	xbar = devm_kzalloc(&pdev->dev, sizeof(*xbar), GFP_KERNEL);
+	if (!xbar)
+		return -ENOMEM;
+
+	dma_node = of_parse_phandle(node, "dma-masters", 0);
+	if (!dma_node) {
+		dev_err(&pdev->dev, "Can't get DMA master node\n");
+		return -ENODEV;
+	}
+
+	if (of_property_read_u32(dma_node, "dma-requests",
+				 &xbar->dma_requests)) {
+		dev_info(&pdev->dev,
+			 "Missing XBAR output information, using %u.\n",
+			 TI_XBAR_OUTPUTS);
+		xbar->dma_requests = TI_XBAR_OUTPUTS;
+	}
+	of_node_put(dma_node);
+
+	if (of_property_read_u32(node, "dma-requests", &xbar->xbar_requests)) {
+		dev_info(&pdev->dev,
+			 "Missing XBAR input information, using %u.\n",
+			 TI_XBAR_INPUTS);
+		xbar->xbar_requests = TI_XBAR_INPUTS;
+	}
+
+	if (!of_property_read_u32(node, "ti,dma-safe-map", &safe_val))
+		xbar->safe_val = (u16)safe_val;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	iomem = devm_ioremap_resource(&pdev->dev, res);
+	if (!iomem)
+		return -ENOMEM;
+
+	xbar->iomem = iomem;
+
+	xbar->dmarouter.dev = &pdev->dev;
+	xbar->dmarouter.route_free = ti_dma_xbar_free;
+
+	platform_set_drvdata(pdev, xbar);
+
+	/* Reset the crossbar */
+	for (i = 0; i < xbar->dma_requests; i++)
+		ti_dma_xbar_write(xbar->iomem, i, xbar->safe_val);
+
+	ret = of_dma_router_register(node, ti_dma_xbar_route_allocate,
+				     &xbar->dmarouter);
+	if (ret) {
+		/* Restore the defaults for the crossbar */
+		for (i = 0; i < xbar->dma_requests; i++)
+			ti_dma_xbar_write(xbar->iomem, i, i);
+	}
+
+	return ret;
+}
+
+static const struct of_device_id ti_dma_xbar_match[] = {
+	{ .compatible = "ti,dra7-dma-crossbar" },
+	{},
+};
+
+static struct platform_driver ti_dma_xbar_driver = {
+	.driver = {
+		.name = "ti-dma-crossbar",
+		.of_match_table = of_match_ptr(ti_dma_xbar_match),
+	},
+	.probe	= ti_dma_xbar_probe,
+};
+
+int omap_dmaxbar_init(void)
+{
+	return platform_driver_register(&ti_dma_xbar_driver);
+}
+arch_initcall(omap_dmaxbar_init);
diff --git a/drivers/dma/virt-dma.c b/drivers/dma/virt-dma.c
index 6f80432a3f0a..7d2c17d8d30f 100644
--- a/drivers/dma/virt-dma.c
+++ b/drivers/dma/virt-dma.c
@@ -29,7 +29,7 @@ dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *tx)
 	spin_lock_irqsave(&vc->lock, flags);
 	cookie = dma_cookie_assign(tx);
 
-	list_add_tail(&vd->node, &vc->desc_submitted);
+	list_move_tail(&vd->node, &vc->desc_submitted);
 	spin_unlock_irqrestore(&vc->lock, flags);
 
 	dev_dbg(vc->chan.device->dev, "vchan %p: txd %p[%x]: submitted\n",
@@ -83,8 +83,10 @@ static void vchan_complete(unsigned long arg)
 		cb_data = vd->tx.callback_param;
 
 		list_del(&vd->node);
-
-		vc->desc_free(vd);
+		if (async_tx_test_ack(&vd->tx))
+			list_add(&vd->node, &vc->desc_allocated);
+		else
+			vc->desc_free(vd);
 
 		if (cb)
 			cb(cb_data);
@@ -96,9 +98,13 @@ void vchan_dma_desc_free_list(struct virt_dma_chan *vc, struct list_head *head)
 	while (!list_empty(head)) {
 		struct virt_dma_desc *vd = list_first_entry(head,
 			struct virt_dma_desc, node);
-		list_del(&vd->node);
-		dev_dbg(vc->chan.device->dev, "txd %p: freeing\n", vd);
-		vc->desc_free(vd);
+		if (async_tx_test_ack(&vd->tx)) {
+			list_move_tail(&vd->node, &vc->desc_allocated);
+		} else {
+			dev_dbg(vc->chan.device->dev, "txd %p: freeing\n", vd);
+			list_del(&vd->node);
+			vc->desc_free(vd);
+		}
 	}
 }
 EXPORT_SYMBOL_GPL(vchan_dma_desc_free_list);
@@ -108,6 +114,7 @@ void vchan_init(struct virt_dma_chan *vc, struct dma_device *dmadev)
 	dma_cookie_init(&vc->chan);
 
 	spin_lock_init(&vc->lock);
+	INIT_LIST_HEAD(&vc->desc_allocated);
 	INIT_LIST_HEAD(&vc->desc_submitted);
 	INIT_LIST_HEAD(&vc->desc_issued);
 	INIT_LIST_HEAD(&vc->desc_completed);
diff --git a/drivers/dma/virt-dma.h b/drivers/dma/virt-dma.h
index 181b95267866..189e75dbcb15 100644
--- a/drivers/dma/virt-dma.h
+++ b/drivers/dma/virt-dma.h
@@ -29,6 +29,7 @@ struct virt_dma_chan {
 	spinlock_t lock;
 
 	/* protected by vc.lock */
+	struct list_head desc_allocated;
 	struct list_head desc_submitted;
 	struct list_head desc_issued;
 	struct list_head desc_completed;
@@ -55,11 +56,16 @@ static inline struct dma_async_tx_descriptor *vchan_tx_prep(struct virt_dma_chan
 	struct virt_dma_desc *vd, unsigned long tx_flags)
 {
 	extern dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *);
+	unsigned long flags;
 
 	dma_async_tx_descriptor_init(&vd->tx, &vc->chan);
 	vd->tx.flags = tx_flags;
 	vd->tx.tx_submit = vchan_tx_submit;
 
+	spin_lock_irqsave(&vc->lock, flags);
+	list_add_tail(&vd->node, &vc->desc_allocated);
+	spin_unlock_irqrestore(&vc->lock, flags);
+
 	return &vd->tx;
 }
 
@@ -122,7 +128,8 @@ static inline struct virt_dma_desc *vchan_next_desc(struct virt_dma_chan *vc)
 }
 
 /**
- * vchan_get_all_descriptors - obtain all submitted and issued descriptors
+ * vchan_get_all_descriptors - obtain all allocated, submitted and issued
+ *                             descriptors
  * vc: virtual channel to get descriptors from
  * head: list of descriptors found
  *
@@ -134,6 +141,7 @@ static inline struct virt_dma_desc *vchan_next_desc(struct virt_dma_chan *vc)
 static inline void vchan_get_all_descriptors(struct virt_dma_chan *vc,
 	struct list_head *head)
 {
+	list_splice_tail_init(&vc->desc_allocated, head);
 	list_splice_tail_init(&vc->desc_submitted, head);
 	list_splice_tail_init(&vc->desc_issued, head);
 	list_splice_tail_init(&vc->desc_completed, head);
@@ -141,11 +149,14 @@ static inline void vchan_get_all_descriptors(struct virt_dma_chan *vc,
 
 static inline void vchan_free_chan_resources(struct virt_dma_chan *vc)
 {
+	struct virt_dma_desc *vd;
 	unsigned long flags;
 	LIST_HEAD(head);
 
 	spin_lock_irqsave(&vc->lock, flags);
 	vchan_get_all_descriptors(vc, &head);
+	list_for_each_entry(vd, &head, node)
+		async_tx_clear_ack(&vd->tx);
 	spin_unlock_irqrestore(&vc->lock, flags);
 
 	vchan_dma_desc_free_list(vc, &head);
diff --git a/drivers/dma/xgene-dma.c b/drivers/dma/xgene-dma.c
index f52e37502254..620fd55ec766 100755..100644
--- a/drivers/dma/xgene-dma.c
+++ b/drivers/dma/xgene-dma.c
@@ -124,32 +124,8 @@
 #define XGENE_DMA_DESC_ELERR_POS		46
 #define XGENE_DMA_DESC_RTYPE_POS		56
 #define XGENE_DMA_DESC_LERR_POS			60
-#define XGENE_DMA_DESC_FLYBY_POS		4
 #define XGENE_DMA_DESC_BUFLEN_POS		48
 #define XGENE_DMA_DESC_HOENQ_NUM_POS		48
-
-#define XGENE_DMA_DESC_NV_SET(m)		\
-	(((u64 *)(m))[0] |= XGENE_DMA_DESC_NV_BIT)
-#define XGENE_DMA_DESC_IN_SET(m)		\
-	(((u64 *)(m))[0] |= XGENE_DMA_DESC_IN_BIT)
-#define XGENE_DMA_DESC_RTYPE_SET(m, v)		\
-	(((u64 *)(m))[0] |= ((u64)(v) << XGENE_DMA_DESC_RTYPE_POS))
-#define XGENE_DMA_DESC_BUFADDR_SET(m, v)	\
-	(((u64 *)(m))[0] |= (v))
-#define XGENE_DMA_DESC_BUFLEN_SET(m, v)		\
-	(((u64 *)(m))[0] |= ((u64)(v) << XGENE_DMA_DESC_BUFLEN_POS))
-#define XGENE_DMA_DESC_C_SET(m)			\
-	(((u64 *)(m))[1] |= XGENE_DMA_DESC_C_BIT)
-#define XGENE_DMA_DESC_FLYBY_SET(m, v)		\
-	(((u64 *)(m))[2] |= ((v) << XGENE_DMA_DESC_FLYBY_POS))
-#define XGENE_DMA_DESC_MULTI_SET(m, v, i)	\
-	(((u64 *)(m))[2] |= ((u64)(v) << (((i) + 1) * 8)))
-#define XGENE_DMA_DESC_DR_SET(m)		\
-	(((u64 *)(m))[2] |= XGENE_DMA_DESC_DR_BIT)
-#define XGENE_DMA_DESC_DST_ADDR_SET(m, v)	\
-	(((u64 *)(m))[3] |= (v))
-#define XGENE_DMA_DESC_H0ENQ_NUM_SET(m, v)	\
-	(((u64 *)(m))[3] |= ((u64)(v) << XGENE_DMA_DESC_HOENQ_NUM_POS))
 #define XGENE_DMA_DESC_ELERR_RD(m)		\
 	(((m) >> XGENE_DMA_DESC_ELERR_POS) & 0x3)
 #define XGENE_DMA_DESC_LERR_RD(m)		\
@@ -158,14 +134,7 @@
 	(((elerr) << 4) | (lerr))
 
 /* X-Gene DMA descriptor empty s/w signature */
-#define XGENE_DMA_DESC_EMPTY_INDEX		0
 #define XGENE_DMA_DESC_EMPTY_SIGNATURE		~0ULL
-#define XGENE_DMA_DESC_SET_EMPTY(m)		\
-	(((u64 *)(m))[XGENE_DMA_DESC_EMPTY_INDEX] =	\
-	 XGENE_DMA_DESC_EMPTY_SIGNATURE)
-#define XGENE_DMA_DESC_IS_EMPTY(m)		\
-	(((u64 *)(m))[XGENE_DMA_DESC_EMPTY_INDEX] ==	\
-	 XGENE_DMA_DESC_EMPTY_SIGNATURE)
 
 /* X-Gene DMA configurable parameters defines */
 #define XGENE_DMA_RING_NUM		512
@@ -184,7 +153,7 @@
 #define XGENE_DMA_XOR_ALIGNMENT		6	/* 64 Bytes */
 #define XGENE_DMA_MAX_XOR_SRC		5
 #define XGENE_DMA_16K_BUFFER_LEN_CODE	0x0
-#define XGENE_DMA_INVALID_LEN_CODE	0x7800
+#define XGENE_DMA_INVALID_LEN_CODE	0x7800000000000000ULL
 
 /* X-Gene DMA descriptor error codes */
 #define ERR_DESC_AXI			0x01
@@ -214,10 +183,10 @@
 #define ERR_DESC_SRC_INT		0xB
 
 /* X-Gene DMA flyby operation code */
-#define FLYBY_2SRC_XOR			0x8
-#define FLYBY_3SRC_XOR			0x9
-#define FLYBY_4SRC_XOR			0xA
-#define FLYBY_5SRC_XOR			0xB
+#define FLYBY_2SRC_XOR			0x80
+#define FLYBY_3SRC_XOR			0x90
+#define FLYBY_4SRC_XOR			0xA0
+#define FLYBY_5SRC_XOR			0xB0
 
 /* X-Gene DMA SW descriptor flags */
 #define XGENE_DMA_FLAG_64B_DESC		BIT(0)
@@ -238,10 +207,10 @@
 	dev_err(chan->dev, "%s: " fmt, chan->name, ##arg)
 
 struct xgene_dma_desc_hw {
-	u64 m0;
-	u64 m1;
-	u64 m2;
-	u64 m3;
+	__le64 m0;
+	__le64 m1;
+	__le64 m2;
+	__le64 m3;
 };
 
 enum xgene_dma_ring_cfgsize {
@@ -388,18 +357,11 @@ static bool is_pq_enabled(struct xgene_dma *pdma)
 	return !(val & XGENE_DMA_PQ_DISABLE_MASK);
 }
 
-static void xgene_dma_cpu_to_le64(u64 *desc, int count)
-{
-	int i;
-
-	for (i = 0; i < count; i++)
-		desc[i] = cpu_to_le64(desc[i]);
-}
-
-static u16 xgene_dma_encode_len(u32 len)
+static u64 xgene_dma_encode_len(size_t len)
 {
 	return (len < XGENE_DMA_MAX_BYTE_CNT) ?
-		len : XGENE_DMA_16K_BUFFER_LEN_CODE;
+		((u64)len << XGENE_DMA_DESC_BUFLEN_POS) :
+		XGENE_DMA_16K_BUFFER_LEN_CODE;
 }
 
 static u8 xgene_dma_encode_xor_flyby(u32 src_cnt)
@@ -424,34 +386,50 @@ static u32 xgene_dma_ring_desc_cnt(struct xgene_dma_ring *ring)
 	return XGENE_DMA_RING_DESC_CNT(ring_state);
 }
 
-static void xgene_dma_set_src_buffer(void *ext8, size_t *len,
+static void xgene_dma_set_src_buffer(__le64 *ext8, size_t *len,
 				     dma_addr_t *paddr)
 {
 	size_t nbytes = (*len < XGENE_DMA_MAX_BYTE_CNT) ?
 			*len : XGENE_DMA_MAX_BYTE_CNT;
 
-	XGENE_DMA_DESC_BUFADDR_SET(ext8, *paddr);
-	XGENE_DMA_DESC_BUFLEN_SET(ext8, xgene_dma_encode_len(nbytes));
+	*ext8 |= cpu_to_le64(*paddr);
+	*ext8 |= cpu_to_le64(xgene_dma_encode_len(nbytes));
 	*len -= nbytes;
 	*paddr += nbytes;
 }
 
-static void xgene_dma_invalidate_buffer(void *ext8)
+static void xgene_dma_invalidate_buffer(__le64 *ext8)
 {
-	XGENE_DMA_DESC_BUFLEN_SET(ext8, XGENE_DMA_INVALID_LEN_CODE);
+	*ext8 |= cpu_to_le64(XGENE_DMA_INVALID_LEN_CODE);
 }
 
-static void *xgene_dma_lookup_ext8(u64 *desc, int idx)
+static __le64 *xgene_dma_lookup_ext8(struct xgene_dma_desc_hw *desc, int idx)
 {
-	return (idx % 2) ? (desc + idx - 1) : (desc + idx + 1);
+	switch (idx) {
+	case 0:
+		return &desc->m1;
+	case 1:
+		return &desc->m0;
+	case 2:
+		return &desc->m3;
+	case 3:
+		return &desc->m2;
+	default:
+		pr_err("Invalid dma descriptor index\n");
+	}
+
+	return NULL;
 }
 
-static void xgene_dma_init_desc(void *desc, u16 dst_ring_num)
+static void xgene_dma_init_desc(struct xgene_dma_desc_hw *desc,
+				u16 dst_ring_num)
 {
-	XGENE_DMA_DESC_C_SET(desc); /* Coherent IO */
-	XGENE_DMA_DESC_IN_SET(desc);
-	XGENE_DMA_DESC_H0ENQ_NUM_SET(desc, dst_ring_num);
-	XGENE_DMA_DESC_RTYPE_SET(desc, XGENE_DMA_RING_OWNER_DMA);
+	desc->m0 |= cpu_to_le64(XGENE_DMA_DESC_IN_BIT);
+	desc->m0 |= cpu_to_le64((u64)XGENE_DMA_RING_OWNER_DMA <<
+				XGENE_DMA_DESC_RTYPE_POS);
+	desc->m1 |= cpu_to_le64(XGENE_DMA_DESC_C_BIT);
+	desc->m3 |= cpu_to_le64((u64)dst_ring_num <<
+				XGENE_DMA_DESC_HOENQ_NUM_POS);
 }
 
 static void xgene_dma_prep_cpy_desc(struct xgene_dma_chan *chan,
@@ -459,7 +437,7 @@ static void xgene_dma_prep_cpy_desc(struct xgene_dma_chan *chan,
 				    dma_addr_t dst, dma_addr_t src,
 				    size_t len)
 {
-	void *desc1, *desc2;
+	struct xgene_dma_desc_hw *desc1, *desc2;
 	int i;
 
 	/* Get 1st descriptor */
@@ -467,23 +445,21 @@ static void xgene_dma_prep_cpy_desc(struct xgene_dma_chan *chan,
 	xgene_dma_init_desc(desc1, chan->tx_ring.dst_ring_num);
 
 	/* Set destination address */
-	XGENE_DMA_DESC_DR_SET(desc1);
-	XGENE_DMA_DESC_DST_ADDR_SET(desc1, dst);
+	desc1->m2 |= cpu_to_le64(XGENE_DMA_DESC_DR_BIT);
+	desc1->m3 |= cpu_to_le64(dst);
 
 	/* Set 1st source address */
-	xgene_dma_set_src_buffer(desc1 + 8, &len, &src);
+	xgene_dma_set_src_buffer(&desc1->m1, &len, &src);
 
-	if (len <= 0) {
-		desc2 = NULL;
-		goto skip_additional_src;
-	}
+	if (!len)
+		return;
 
 	/*
 	 * We need to split this source buffer,
 	 * and need to use 2nd descriptor
 	 */
 	desc2 = &desc_sw->desc2;
-	XGENE_DMA_DESC_NV_SET(desc1);
+	desc1->m0 |= cpu_to_le64(XGENE_DMA_DESC_NV_BIT);
 
 	/* Set 2nd to 5th source address */
 	for (i = 0; i < 4 && len; i++)
@@ -496,12 +472,6 @@ static void xgene_dma_prep_cpy_desc(struct xgene_dma_chan *chan,
 
 	/* Updated flag that we have prepared 64B descriptor */
 	desc_sw->flags |= XGENE_DMA_FLAG_64B_DESC;
-
-skip_additional_src:
-	/* Hardware stores descriptor in little endian format */
-	xgene_dma_cpu_to_le64(desc1, 4);
-	if (desc2)
-		xgene_dma_cpu_to_le64(desc2, 4);
 }
 
 static void xgene_dma_prep_xor_desc(struct xgene_dma_chan *chan,
@@ -510,7 +480,7 @@ static void xgene_dma_prep_xor_desc(struct xgene_dma_chan *chan,
 				    u32 src_cnt, size_t *nbytes,
 				    const u8 *scf)
 {
-	void *desc1, *desc2;
+	struct xgene_dma_desc_hw *desc1, *desc2;
 	size_t len = *nbytes;
 	int i;
 
@@ -521,28 +491,24 @@ static void xgene_dma_prep_xor_desc(struct xgene_dma_chan *chan,
 	xgene_dma_init_desc(desc1, chan->tx_ring.dst_ring_num);
 
 	/* Set destination address */
-	XGENE_DMA_DESC_DR_SET(desc1);
-	XGENE_DMA_DESC_DST_ADDR_SET(desc1, *dst);
+	desc1->m2 |= cpu_to_le64(XGENE_DMA_DESC_DR_BIT);
+	desc1->m3 |= cpu_to_le64(*dst);
 
 	/* We have multiple source addresses, so need to set NV bit*/
-	XGENE_DMA_DESC_NV_SET(desc1);
+	desc1->m0 |= cpu_to_le64(XGENE_DMA_DESC_NV_BIT);
 
 	/* Set flyby opcode */
-	XGENE_DMA_DESC_FLYBY_SET(desc1, xgene_dma_encode_xor_flyby(src_cnt));
+	desc1->m2 |= cpu_to_le64(xgene_dma_encode_xor_flyby(src_cnt));
 
 	/* Set 1st to 5th source addresses */
 	for (i = 0; i < src_cnt; i++) {
 		len = *nbytes;
-		xgene_dma_set_src_buffer((i == 0) ? (desc1 + 8) :
+		xgene_dma_set_src_buffer((i == 0) ? &desc1->m1 :
 					 xgene_dma_lookup_ext8(desc2, i - 1),
 					 &len, &src[i]);
-		XGENE_DMA_DESC_MULTI_SET(desc1, scf[i], i);
+		desc1->m2 |= cpu_to_le64((scf[i] << ((i + 1) * 8)));
 	}
 
-	/* Hardware stores descriptor in little endian format */
-	xgene_dma_cpu_to_le64(desc1, 4);
-	xgene_dma_cpu_to_le64(desc2, 4);
-
 	/* Update meta data */
 	*nbytes = len;
 	*dst += XGENE_DMA_MAX_BYTE_CNT;
@@ -738,7 +704,7 @@ static int xgene_chan_xfer_request(struct xgene_dma_ring *ring,
  * xgene_chan_xfer_ld_pending - push any pending transactions to hw
  * @chan : X-Gene DMA channel
  *
- * LOCKING: must hold chan->desc_lock
+ * LOCKING: must hold chan->lock
  */
 static void xgene_chan_xfer_ld_pending(struct xgene_dma_chan *chan)
 {
@@ -808,7 +774,8 @@ static void xgene_dma_cleanup_descriptors(struct xgene_dma_chan *chan)
 		desc_hw = &ring->desc_hw[ring->head];
 
 		/* Check if this descriptor has been completed */
-		if (unlikely(XGENE_DMA_DESC_IS_EMPTY(desc_hw)))
+		if (unlikely(le64_to_cpu(desc_hw->m0) ==
+			     XGENE_DMA_DESC_EMPTY_SIGNATURE))
 			break;
 
 		if (++ring->head == ring->slots)
@@ -842,7 +809,7 @@ static void xgene_dma_cleanup_descriptors(struct xgene_dma_chan *chan)
 		iowrite32(-1, ring->cmd);
 
 		/* Mark this hw descriptor as processed */
-		XGENE_DMA_DESC_SET_EMPTY(desc_hw);
+		desc_hw->m0 = cpu_to_le64(XGENE_DMA_DESC_EMPTY_SIGNATURE);
 
 		xgene_dma_run_tx_complete_actions(chan, desc_sw);
 
@@ -889,7 +856,7 @@ static int xgene_dma_alloc_chan_resources(struct dma_chan *dchan)
  * @chan: X-Gene DMA channel
  * @list: the list to free
  *
- * LOCKING: must hold chan->desc_lock
+ * LOCKING: must hold chan->lock
  */
 static void xgene_dma_free_desc_list(struct xgene_dma_chan *chan,
 				     struct list_head *list)
@@ -900,15 +867,6 @@ static void xgene_dma_free_desc_list(struct xgene_dma_chan *chan,
 		xgene_dma_clean_descriptor(chan, desc);
 }
 
-static void xgene_dma_free_tx_desc_list(struct xgene_dma_chan *chan,
-					struct list_head *list)
-{
-	struct xgene_dma_desc_sw *desc, *_desc;
-
-	list_for_each_entry_safe(desc, _desc, list, node)
-		xgene_dma_clean_descriptor(chan, desc);
-}
-
 static void xgene_dma_free_chan_resources(struct dma_chan *dchan)
 {
 	struct xgene_dma_chan *chan = to_dma_chan(dchan);
@@ -985,7 +943,7 @@ fail:
 	if (!first)
 		return NULL;
 
-	xgene_dma_free_tx_desc_list(chan, &first->tx_list);
+	xgene_dma_free_desc_list(chan, &first->tx_list);
 	return NULL;
 }
 
@@ -1093,7 +1051,7 @@ fail:
 	if (!first)
 		return NULL;
 
-	xgene_dma_free_tx_desc_list(chan, &first->tx_list);
+	xgene_dma_free_desc_list(chan, &first->tx_list);
 	return NULL;
 }
 
@@ -1141,7 +1099,7 @@ fail:
 	if (!first)
 		return NULL;
 
-	xgene_dma_free_tx_desc_list(chan, &first->tx_list);
+	xgene_dma_free_desc_list(chan, &first->tx_list);
 	return NULL;
 }
 
@@ -1218,7 +1176,7 @@ fail:
 	if (!first)
 		return NULL;
 
-	xgene_dma_free_tx_desc_list(chan, &first->tx_list);
+	xgene_dma_free_desc_list(chan, &first->tx_list);
 	return NULL;
 }
 
@@ -1316,7 +1274,6 @@ static void xgene_dma_setup_ring(struct xgene_dma_ring *ring)
 {
 	void *ring_cfg = ring->state;
 	u64 addr = ring->desc_paddr;
-	void *desc;
 	u32 i, val;
 
 	ring->slots = ring->size / XGENE_DMA_RING_WQ_DESC_SIZE;
@@ -1358,8 +1315,10 @@ static void xgene_dma_setup_ring(struct xgene_dma_ring *ring)
 
 	/* Set empty signature to DMA Rx ring descriptors */
 	for (i = 0; i < ring->slots; i++) {
+		struct xgene_dma_desc_hw *desc;
+
 		desc = &ring->desc_hw[i];
-		XGENE_DMA_DESC_SET_EMPTY(desc);
+		desc->m0 = cpu_to_le64(XGENE_DMA_DESC_EMPTY_SIGNATURE);
 	}
 
 	/* Enable DMA Rx ring interrupt */
diff --git a/drivers/edac/Kconfig b/drivers/edac/Kconfig
index cb59619df23f..8677ead2a8e1 100644
--- a/drivers/edac/Kconfig
+++ b/drivers/edac/Kconfig
@@ -2,15 +2,16 @@
 #	EDAC Kconfig
 #	Copyright (c) 2008 Doug Thompson www.softwarebitmaker.com
 #	Licensed and distributed under the GPL
-#
+
+config EDAC_ATOMIC_SCRUB
+	bool
 
 config EDAC_SUPPORT
 	bool
 
 menuconfig EDAC
 	bool "EDAC (Error Detection And Correction) reporting"
-	depends on HAS_IOMEM
-	depends on X86 || PPC || TILE || ARM || EDAC_SUPPORT
+	depends on HAS_IOMEM && EDAC_SUPPORT
 	help
 	  EDAC is designed to report errors in the core system.
 	  These are low-level errors that are reported in the CPU or
@@ -262,10 +263,10 @@ config EDAC_SBRIDGE
 
 config EDAC_MPC85XX
 	tristate "Freescale MPC83xx / MPC85xx"
-	depends on EDAC_MM_EDAC && FSL_SOC && (PPC_83xx || PPC_85xx)
+	depends on EDAC_MM_EDAC && FSL_SOC
 	help
 	  Support for error detection and correction on the Freescale
-	  MPC8349, MPC8560, MPC8540, MPC8548
+	  MPC8349, MPC8560, MPC8540, MPC8548, T4240
 
 config EDAC_MV64X60
 	tristate "Marvell MV64x60"
@@ -377,8 +378,8 @@ config EDAC_OCTEON_PCI
 	  Cavium Octeon family of SOCs.
 
 config EDAC_ALTERA_MC
-	tristate "Altera SDRAM Memory Controller EDAC"
-	depends on EDAC_MM_EDAC && ARCH_SOCFPGA
+	bool "Altera SDRAM Memory Controller EDAC"
+	depends on EDAC_MM_EDAC=y && ARCH_SOCFPGA
 	help
 	  Support for error detection and correction on the
 	  Altera SDRAM memory controller. Note that the
@@ -392,4 +393,11 @@ config EDAC_SYNOPSYS
 	  Support for error detection and correction on the Synopsys DDR
 	  memory controller.
 
+config EDAC_XGENE
+	tristate "APM X-Gene SoC"
+	depends on EDAC_MM_EDAC && (ARM64 || COMPILE_TEST)
+	help
+	  Support for error detection and correction on the
+	  APM X-Gene family of SOCs.
+
 endif # EDAC
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index b255f362b1db..28ef2a519f65 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -68,3 +68,4 @@ obj-$(CONFIG_EDAC_OCTEON_PCI)		+= octeon_edac-pci.o
 
 obj-$(CONFIG_EDAC_ALTERA_MC)		+= altera_edac.o
 obj-$(CONFIG_EDAC_SYNOPSYS)		+= synopsys_edac.o
+obj-$(CONFIG_EDAC_XGENE)		+= xgene_edac.o
diff --git a/drivers/edac/altera_edac.c b/drivers/edac/altera_edac.c
index 3c4929fda9d5..23ef0917483c 100644
--- a/drivers/edac/altera_edac.c
+++ b/drivers/edac/altera_edac.c
@@ -1,5 +1,5 @@
 /*
- *  Copyright Altera Corporation (C) 2014. All rights reserved.
+ *  Copyright Altera Corporation (C) 2014-2015. All rights reserved.
  *  Copyright 2011-2012 Calxeda, Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
@@ -28,113 +28,92 @@
 #include <linux/types.h>
 #include <linux/uaccess.h>
 
+#include "altera_edac.h"
 #include "edac_core.h"
 #include "edac_module.h"
 
 #define EDAC_MOD_STR		"altera_edac"
 #define EDAC_VERSION		"1"
 
-/* SDRAM Controller CtrlCfg Register */
-#define CTLCFG_OFST             0x00
-
-/* SDRAM Controller CtrlCfg Register Bit Masks */
-#define CTLCFG_ECC_EN           0x400
-#define CTLCFG_ECC_CORR_EN      0x800
-#define CTLCFG_GEN_SB_ERR       0x2000
-#define CTLCFG_GEN_DB_ERR       0x4000
-
-#define CTLCFG_ECC_AUTO_EN	(CTLCFG_ECC_EN | \
-				 CTLCFG_ECC_CORR_EN)
-
-/* SDRAM Controller Address Width Register */
-#define DRAMADDRW_OFST          0x2C
-
-/* SDRAM Controller Address Widths Field Register */
-#define DRAMADDRW_COLBIT_MASK   0x001F
-#define DRAMADDRW_COLBIT_SHIFT  0
-#define DRAMADDRW_ROWBIT_MASK   0x03E0
-#define DRAMADDRW_ROWBIT_SHIFT  5
-#define DRAMADDRW_BANKBIT_MASK	0x1C00
-#define DRAMADDRW_BANKBIT_SHIFT 10
-#define DRAMADDRW_CSBIT_MASK	0xE000
-#define DRAMADDRW_CSBIT_SHIFT   13
-
-/* SDRAM Controller Interface Data Width Register */
-#define DRAMIFWIDTH_OFST        0x30
-
-/* SDRAM Controller Interface Data Width Defines */
-#define DRAMIFWIDTH_16B_ECC     24
-#define DRAMIFWIDTH_32B_ECC     40
-
-/* SDRAM Controller DRAM Status Register */
-#define DRAMSTS_OFST            0x38
-
-/* SDRAM Controller DRAM Status Register Bit Masks */
-#define DRAMSTS_SBEERR          0x04
-#define DRAMSTS_DBEERR          0x08
-#define DRAMSTS_CORR_DROP       0x10
-
-/* SDRAM Controller DRAM IRQ Register */
-#define DRAMINTR_OFST           0x3C
-
-/* SDRAM Controller DRAM IRQ Register Bit Masks */
-#define DRAMINTR_INTREN         0x01
-#define DRAMINTR_SBEMASK        0x02
-#define DRAMINTR_DBEMASK        0x04
-#define DRAMINTR_CORRDROPMASK   0x08
-#define DRAMINTR_INTRCLR        0x10
-
-/* SDRAM Controller Single Bit Error Count Register */
-#define SBECOUNT_OFST           0x40
-
-/* SDRAM Controller Single Bit Error Count Register Bit Masks */
-#define SBECOUNT_MASK           0x0F
-
-/* SDRAM Controller Double Bit Error Count Register */
-#define DBECOUNT_OFST           0x44
-
-/* SDRAM Controller Double Bit Error Count Register Bit Masks */
-#define DBECOUNT_MASK           0x0F
-
-/* SDRAM Controller ECC Error Address Register */
-#define ERRADDR_OFST            0x48
-
-/* SDRAM Controller ECC Error Address Register Bit Masks */
-#define ERRADDR_MASK            0xFFFFFFFF
+static const struct altr_sdram_prv_data c5_data = {
+	.ecc_ctrl_offset    = CV_CTLCFG_OFST,
+	.ecc_ctl_en_mask    = CV_CTLCFG_ECC_AUTO_EN,
+	.ecc_stat_offset    = CV_DRAMSTS_OFST,
+	.ecc_stat_ce_mask   = CV_DRAMSTS_SBEERR,
+	.ecc_stat_ue_mask   = CV_DRAMSTS_DBEERR,
+	.ecc_saddr_offset   = CV_ERRADDR_OFST,
+	.ecc_daddr_offset   = CV_ERRADDR_OFST,
+	.ecc_cecnt_offset   = CV_SBECOUNT_OFST,
+	.ecc_uecnt_offset   = CV_DBECOUNT_OFST,
+	.ecc_irq_en_offset  = CV_DRAMINTR_OFST,
+	.ecc_irq_en_mask    = CV_DRAMINTR_INTREN,
+	.ecc_irq_clr_offset = CV_DRAMINTR_OFST,
+	.ecc_irq_clr_mask   = (CV_DRAMINTR_INTRCLR | CV_DRAMINTR_INTREN),
+	.ecc_cnt_rst_offset = CV_DRAMINTR_OFST,
+	.ecc_cnt_rst_mask   = CV_DRAMINTR_INTRCLR,
+#ifdef CONFIG_EDAC_DEBUG
+	.ce_ue_trgr_offset  = CV_CTLCFG_OFST,
+	.ce_set_mask        = CV_CTLCFG_GEN_SB_ERR,
+	.ue_set_mask        = CV_CTLCFG_GEN_DB_ERR,
+#endif
+};
 
-/* Altera SDRAM Memory Controller data */
-struct altr_sdram_mc_data {
-	struct regmap *mc_vbase;
+static const struct altr_sdram_prv_data a10_data = {
+	.ecc_ctrl_offset    = A10_ECCCTRL1_OFST,
+	.ecc_ctl_en_mask    = A10_ECCCTRL1_ECC_EN,
+	.ecc_stat_offset    = A10_INTSTAT_OFST,
+	.ecc_stat_ce_mask   = A10_INTSTAT_SBEERR,
+	.ecc_stat_ue_mask   = A10_INTSTAT_DBEERR,
+	.ecc_saddr_offset   = A10_SERRADDR_OFST,
+	.ecc_daddr_offset   = A10_DERRADDR_OFST,
+	.ecc_irq_en_offset  = A10_ERRINTEN_OFST,
+	.ecc_irq_en_mask    = A10_ECC_IRQ_EN_MASK,
+	.ecc_irq_clr_offset = A10_INTSTAT_OFST,
+	.ecc_irq_clr_mask   = (A10_INTSTAT_SBEERR | A10_INTSTAT_DBEERR),
+	.ecc_cnt_rst_offset = A10_ECCCTRL1_OFST,
+	.ecc_cnt_rst_mask   = A10_ECC_CNT_RESET_MASK,
+#ifdef CONFIG_EDAC_DEBUG
+	.ce_ue_trgr_offset  = A10_DIAGINTTEST_OFST,
+	.ce_set_mask        = A10_DIAGINT_TSERRA_MASK,
+	.ue_set_mask        = A10_DIAGINT_TDERRA_MASK,
+#endif
 };
 
 static irqreturn_t altr_sdram_mc_err_handler(int irq, void *dev_id)
 {
 	struct mem_ctl_info *mci = dev_id;
 	struct altr_sdram_mc_data *drvdata = mci->pvt_info;
-	u32 status, err_count, err_addr;
+	const struct altr_sdram_prv_data *priv = drvdata->data;
+	u32 status, err_count = 1, err_addr;
 
-	/* Error Address is shared by both SBE & DBE */
-	regmap_read(drvdata->mc_vbase, ERRADDR_OFST, &err_addr);
+	regmap_read(drvdata->mc_vbase, priv->ecc_stat_offset, &status);
 
-	regmap_read(drvdata->mc_vbase, DRAMSTS_OFST, &status);
-
-	if (status & DRAMSTS_DBEERR) {
-		regmap_read(drvdata->mc_vbase, DBECOUNT_OFST, &err_count);
+	if (status & priv->ecc_stat_ue_mask) {
+		regmap_read(drvdata->mc_vbase, priv->ecc_daddr_offset,
+			    &err_addr);
+		if (priv->ecc_uecnt_offset)
+			regmap_read(drvdata->mc_vbase, priv->ecc_uecnt_offset,
+				    &err_count);
 		panic("\nEDAC: [%d Uncorrectable errors @ 0x%08X]\n",
 		      err_count, err_addr);
 	}
-	if (status & DRAMSTS_SBEERR) {
-		regmap_read(drvdata->mc_vbase, SBECOUNT_OFST, &err_count);
+	if (status & priv->ecc_stat_ce_mask) {
+		regmap_read(drvdata->mc_vbase, priv->ecc_saddr_offset,
+			    &err_addr);
+		if (priv->ecc_uecnt_offset)
+			regmap_read(drvdata->mc_vbase,  priv->ecc_cecnt_offset,
+				    &err_count);
 		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, err_count,
 				     err_addr >> PAGE_SHIFT,
 				     err_addr & ~PAGE_MASK, 0,
 				     0, 0, -1, mci->ctl_name, "");
-	}
-
-	regmap_write(drvdata->mc_vbase,	DRAMINTR_OFST,
-		     (DRAMINTR_INTRCLR | DRAMINTR_INTREN));
+		/* Clear IRQ to resume */
+		regmap_write(drvdata->mc_vbase,	priv->ecc_irq_clr_offset,
+			     priv->ecc_irq_clr_mask);
 
-	return IRQ_HANDLED;
+		return IRQ_HANDLED;
+	}
+	return IRQ_NONE;
 }
 
 #ifdef CONFIG_EDAC_DEBUG
@@ -144,6 +123,7 @@ static ssize_t altr_sdr_mc_err_inject_write(struct file *file,
 {
 	struct mem_ctl_info *mci = file->private_data;
 	struct altr_sdram_mc_data *drvdata = mci->pvt_info;
+	const struct altr_sdram_prv_data *priv = drvdata->data;
 	u32 *ptemp;
 	dma_addr_t dma_handle;
 	u32 reg, read_reg;
@@ -156,8 +136,9 @@ static ssize_t altr_sdr_mc_err_inject_write(struct file *file,
 		return -ENOMEM;
 	}
 
-	regmap_read(drvdata->mc_vbase, CTLCFG_OFST, &read_reg);
-	read_reg &= ~(CTLCFG_GEN_SB_ERR | CTLCFG_GEN_DB_ERR);
+	regmap_read(drvdata->mc_vbase, priv->ce_ue_trgr_offset,
+		    &read_reg);
+	read_reg &= ~(priv->ce_set_mask | priv->ue_set_mask);
 
 	/* Error are injected by writing a word while the SBE or DBE
 	 * bit in the CTLCFG register is set. Reading the word will
@@ -166,20 +147,20 @@ static ssize_t altr_sdr_mc_err_inject_write(struct file *file,
 	if (count == 3) {
 		edac_printk(KERN_ALERT, EDAC_MC,
 			    "Inject Double bit error\n");
-		regmap_write(drvdata->mc_vbase, CTLCFG_OFST,
-			     (read_reg | CTLCFG_GEN_DB_ERR));
+		regmap_write(drvdata->mc_vbase, priv->ce_ue_trgr_offset,
+			     (read_reg | priv->ue_set_mask));
 	} else {
 		edac_printk(KERN_ALERT, EDAC_MC,
 			    "Inject Single bit error\n");
-		regmap_write(drvdata->mc_vbase,	CTLCFG_OFST,
-			     (read_reg | CTLCFG_GEN_SB_ERR));
+		regmap_write(drvdata->mc_vbase,	priv->ce_ue_trgr_offset,
+			     (read_reg | priv->ce_set_mask));
 	}
 
 	ptemp[0] = 0x5A5A5A5A;
 	ptemp[1] = 0xA5A5A5A5;
 
 	/* Clear the error injection bits */
-	regmap_write(drvdata->mc_vbase,	CTLCFG_OFST, read_reg);
+	regmap_write(drvdata->mc_vbase,	priv->ce_ue_trgr_offset, read_reg);
 	/* Ensure it has been written out */
 	wmb();
 
@@ -219,50 +200,106 @@ static void altr_sdr_mc_create_debugfs_nodes(struct mem_ctl_info *mci)
 {}
 #endif
 
-/* Get total memory size in bytes */
-static u32 altr_sdram_get_total_mem_size(struct regmap *mc_vbase)
+/* Get total memory size from Open Firmware DTB */
+static unsigned long get_total_mem(void)
 {
-	u32 size, read_reg, row, bank, col, cs, width;
-
-	if (regmap_read(mc_vbase, DRAMADDRW_OFST, &read_reg) < 0)
-		return 0;
-
-	if (regmap_read(mc_vbase, DRAMIFWIDTH_OFST, &width) < 0)
-		return 0;
-
-	col = (read_reg & DRAMADDRW_COLBIT_MASK) >>
-		DRAMADDRW_COLBIT_SHIFT;
-	row = (read_reg & DRAMADDRW_ROWBIT_MASK) >>
-		DRAMADDRW_ROWBIT_SHIFT;
-	bank = (read_reg & DRAMADDRW_BANKBIT_MASK) >>
-		DRAMADDRW_BANKBIT_SHIFT;
-	cs = (read_reg & DRAMADDRW_CSBIT_MASK) >>
-		DRAMADDRW_CSBIT_SHIFT;
-
-	/* Correct for ECC as its not addressible */
-	if (width == DRAMIFWIDTH_32B_ECC)
-		width = 32;
-	if (width == DRAMIFWIDTH_16B_ECC)
-		width = 16;
-
-	/* calculate the SDRAM size base on this info */
-	size = 1 << (row + bank + col);
-	size = size * cs * (width / 8);
-	return size;
+	struct device_node *np = NULL;
+	const unsigned int *reg, *reg_end;
+	int len, sw, aw;
+	unsigned long start, size, total_mem = 0;
+
+	for_each_node_by_type(np, "memory") {
+		aw = of_n_addr_cells(np);
+		sw = of_n_size_cells(np);
+		reg = (const unsigned int *)of_get_property(np, "reg", &len);
+		reg_end = reg + (len / sizeof(u32));
+
+		total_mem = 0;
+		do {
+			start = of_read_number(reg, aw);
+			reg += aw;
+			size = of_read_number(reg, sw);
+			reg += sw;
+			total_mem += size;
+		} while (reg < reg_end);
+	}
+	edac_dbg(0, "total_mem 0x%lx\n", total_mem);
+	return total_mem;
+}
+
+static const struct of_device_id altr_sdram_ctrl_of_match[] = {
+	{ .compatible = "altr,sdram-edac", .data = (void *)&c5_data},
+	{ .compatible = "altr,sdram-edac-a10", .data = (void *)&a10_data},
+	{},
+};
+MODULE_DEVICE_TABLE(of, altr_sdram_ctrl_of_match);
+
+static int a10_init(struct regmap *mc_vbase)
+{
+	if (regmap_update_bits(mc_vbase, A10_INTMODE_OFST,
+			       A10_INTMODE_SB_INT, A10_INTMODE_SB_INT)) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Error setting SB IRQ mode\n");
+		return -ENODEV;
+	}
+
+	if (regmap_write(mc_vbase, A10_SERRCNTREG_OFST, 1)) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Error setting trigger count\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int a10_unmask_irq(struct platform_device *pdev, u32 mask)
+{
+	void __iomem  *sm_base;
+	int  ret = 0;
+
+	if (!request_mem_region(A10_SYMAN_INTMASK_CLR, sizeof(u32),
+				dev_name(&pdev->dev))) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Unable to request mem region\n");
+		return -EBUSY;
+	}
+
+	sm_base = ioremap(A10_SYMAN_INTMASK_CLR, sizeof(u32));
+	if (!sm_base) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Unable to ioremap device\n");
+
+		ret = -ENOMEM;
+		goto release;
+	}
+
+	iowrite32(mask, sm_base);
+
+	iounmap(sm_base);
+
+release:
+	release_mem_region(A10_SYMAN_INTMASK_CLR, sizeof(u32));
+
+	return ret;
 }
 
 static int altr_sdram_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *id;
 	struct edac_mc_layer layers[2];
 	struct mem_ctl_info *mci;
 	struct altr_sdram_mc_data *drvdata;
+	const struct altr_sdram_prv_data *priv;
 	struct regmap *mc_vbase;
 	struct dimm_info *dimm;
-	u32 read_reg, mem_size;
-	int irq;
-	int res = 0;
+	u32 read_reg;
+	int irq, irq2, res = 0;
+	unsigned long mem_size, irqflags = 0;
+
+	id = of_match_device(altr_sdram_ctrl_of_match, &pdev->dev);
+	if (!id)
+		return -ENODEV;
 
-	/* Validate the SDRAM controller has ECC enabled */
 	/* Grab the register range from the sdr controller in device tree */
 	mc_vbase = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
 						   "altr,sdr-syscon");
@@ -272,25 +309,46 @@ static int altr_sdram_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	if (regmap_read(mc_vbase, CTLCFG_OFST, &read_reg) ||
-	    ((read_reg & CTLCFG_ECC_AUTO_EN) !=	CTLCFG_ECC_AUTO_EN)) {
+	/* Check specific dependencies for the module */
+	priv = of_match_node(altr_sdram_ctrl_of_match,
+			     pdev->dev.of_node)->data;
+
+	/* Validate the SDRAM controller has ECC enabled */
+	if (regmap_read(mc_vbase, priv->ecc_ctrl_offset, &read_reg) ||
+	    ((read_reg & priv->ecc_ctl_en_mask) != priv->ecc_ctl_en_mask)) {
 		edac_printk(KERN_ERR, EDAC_MC,
 			    "No ECC/ECC disabled [0x%08X]\n", read_reg);
 		return -ENODEV;
 	}
 
 	/* Grab memory size from device tree. */
-	mem_size = altr_sdram_get_total_mem_size(mc_vbase);
+	mem_size = get_total_mem();
 	if (!mem_size) {
+		edac_printk(KERN_ERR, EDAC_MC, "Unable to calculate memory size\n");
+		return -ENODEV;
+	}
+
+	/* Ensure the SDRAM Interrupt is disabled */
+	if (regmap_update_bits(mc_vbase, priv->ecc_irq_en_offset,
+			       priv->ecc_irq_en_mask, 0)) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Error disabling SDRAM ECC IRQ\n");
+		return -ENODEV;
+	}
+
+	/* Toggle to clear the SDRAM Error count */
+	if (regmap_update_bits(mc_vbase, priv->ecc_cnt_rst_offset,
+			       priv->ecc_cnt_rst_mask,
+			       priv->ecc_cnt_rst_mask)) {
 		edac_printk(KERN_ERR, EDAC_MC,
-			    "Unable to calculate memory size\n");
+			    "Error clearing SDRAM ECC count\n");
 		return -ENODEV;
 	}
 
-	/* Ensure the SDRAM Interrupt is disabled and cleared */
-	if (regmap_write(mc_vbase, DRAMINTR_OFST, DRAMINTR_INTRCLR)) {
+	if (regmap_update_bits(mc_vbase, priv->ecc_cnt_rst_offset,
+			       priv->ecc_cnt_rst_mask, 0)) {
 		edac_printk(KERN_ERR, EDAC_MC,
-			    "Error clearing SDRAM ECC IRQ\n");
+			    "Error clearing SDRAM ECC count\n");
 		return -ENODEV;
 	}
 
@@ -301,6 +359,9 @@ static int altr_sdram_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
+	/* Arria10 has a 2nd IRQ */
+	irq2 = platform_get_irq(pdev, 1);
+
 	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
 	layers[0].size = 1;
 	layers[0].is_virt_csrow = true;
@@ -315,9 +376,12 @@ static int altr_sdram_probe(struct platform_device *pdev)
 	mci->pdev = &pdev->dev;
 	drvdata = mci->pvt_info;
 	drvdata->mc_vbase = mc_vbase;
+	drvdata->data = priv;
 	platform_set_drvdata(pdev, mci);
 
 	if (!devres_open_group(&pdev->dev, NULL, GFP_KERNEL)) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Unable to get managed device resource\n");
 		res = -ENOMEM;
 		goto free;
 	}
@@ -342,8 +406,32 @@ static int altr_sdram_probe(struct platform_device *pdev)
 	if (res < 0)
 		goto err;
 
+	/* Only the Arria10 has separate IRQs */
+	if (irq2 > 0) {
+		/* Arria10 specific initialization */
+		res = a10_init(mc_vbase);
+		if (res < 0)
+			goto err2;
+
+		res = devm_request_irq(&pdev->dev, irq2,
+				       altr_sdram_mc_err_handler,
+				       IRQF_SHARED, dev_name(&pdev->dev), mci);
+		if (res < 0) {
+			edac_mc_printk(mci, KERN_ERR,
+				       "Unable to request irq %d\n", irq2);
+			res = -ENODEV;
+			goto err2;
+		}
+
+		res = a10_unmask_irq(pdev, A10_DDR0_IRQ_MASK);
+		if (res < 0)
+			goto err2;
+
+		irqflags = IRQF_SHARED;
+	}
+
 	res = devm_request_irq(&pdev->dev, irq, altr_sdram_mc_err_handler,
-			       0, dev_name(&pdev->dev), mci);
+			       irqflags, dev_name(&pdev->dev), mci);
 	if (res < 0) {
 		edac_mc_printk(mci, KERN_ERR,
 			       "Unable to request irq %d\n", irq);
@@ -351,8 +439,9 @@ static int altr_sdram_probe(struct platform_device *pdev)
 		goto err2;
 	}
 
-	if (regmap_write(drvdata->mc_vbase, DRAMINTR_OFST,
-			 (DRAMINTR_INTRCLR | DRAMINTR_INTREN))) {
+	/* Infrastructure ready - enable the IRQ */
+	if (regmap_update_bits(drvdata->mc_vbase, priv->ecc_irq_en_offset,
+			       priv->ecc_irq_en_mask, priv->ecc_irq_en_mask)) {
 		edac_mc_printk(mci, KERN_ERR,
 			       "Error enabling SDRAM ECC IRQ\n");
 		res = -ENODEV;
@@ -388,17 +477,31 @@ static int altr_sdram_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id altr_sdram_ctrl_of_match[] = {
-	{ .compatible = "altr,sdram-edac", },
-	{},
+/*
+ * If you want to suspend, need to disable EDAC by removing it
+ * from the device tree or defconfig.
+ */
+#ifdef CONFIG_PM
+static int altr_sdram_prepare(struct device *dev)
+{
+	pr_err("Suspend not allowed when EDAC is enabled.\n");
+
+	return -EPERM;
+}
+
+static const struct dev_pm_ops altr_sdram_pm_ops = {
+	.prepare = altr_sdram_prepare,
 };
-MODULE_DEVICE_TABLE(of, altr_sdram_ctrl_of_match);
+#endif
 
 static struct platform_driver altr_sdram_edac_driver = {
 	.probe = altr_sdram_probe,
 	.remove = altr_sdram_remove,
 	.driver = {
 		.name = "altr_sdram_edac",
+#ifdef CONFIG_PM
+		.pm = &altr_sdram_pm_ops,
+#endif
 		.of_match_table = altr_sdram_ctrl_of_match,
 	},
 };
diff --git a/drivers/edac/altera_edac.h b/drivers/edac/altera_edac.h
new file mode 100644
index 000000000000..7b64dc7c4eb7
--- /dev/null
+++ b/drivers/edac/altera_edac.h
@@ -0,0 +1,201 @@
+/*
+ *
+ * Copyright (C) 2015 Altera Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _ALTERA_EDAC_H
+#define _ALTERA_EDAC_H
+
+#include <linux/edac.h>
+#include <linux/types.h>
+
+/* SDRAM Controller CtrlCfg Register */
+#define CV_CTLCFG_OFST             0x00
+
+/* SDRAM Controller CtrlCfg Register Bit Masks */
+#define CV_CTLCFG_ECC_EN           0x400
+#define CV_CTLCFG_ECC_CORR_EN      0x800
+#define CV_CTLCFG_GEN_SB_ERR       0x2000
+#define CV_CTLCFG_GEN_DB_ERR       0x4000
+
+#define CV_CTLCFG_ECC_AUTO_EN     (CV_CTLCFG_ECC_EN | \
+				   CV_CTLCFG_ECC_CORR_EN)
+
+/* SDRAM Controller Address Width Register */
+#define CV_DRAMADDRW_OFST          0x2C
+
+/* SDRAM Controller Address Widths Field Register */
+#define DRAMADDRW_COLBIT_MASK      0x001F
+#define DRAMADDRW_COLBIT_SHIFT     0
+#define DRAMADDRW_ROWBIT_MASK      0x03E0
+#define DRAMADDRW_ROWBIT_SHIFT     5
+#define CV_DRAMADDRW_BANKBIT_MASK  0x1C00
+#define CV_DRAMADDRW_BANKBIT_SHIFT 10
+#define CV_DRAMADDRW_CSBIT_MASK    0xE000
+#define CV_DRAMADDRW_CSBIT_SHIFT   13
+
+/* SDRAM Controller Interface Data Width Register */
+#define CV_DRAMIFWIDTH_OFST        0x30
+
+/* SDRAM Controller Interface Data Width Defines */
+#define CV_DRAMIFWIDTH_16B_ECC     24
+#define CV_DRAMIFWIDTH_32B_ECC     40
+
+/* SDRAM Controller DRAM Status Register */
+#define CV_DRAMSTS_OFST            0x38
+
+/* SDRAM Controller DRAM Status Register Bit Masks */
+#define CV_DRAMSTS_SBEERR          0x04
+#define CV_DRAMSTS_DBEERR          0x08
+#define CV_DRAMSTS_CORR_DROP       0x10
+
+/* SDRAM Controller DRAM IRQ Register */
+#define CV_DRAMINTR_OFST           0x3C
+
+/* SDRAM Controller DRAM IRQ Register Bit Masks */
+#define CV_DRAMINTR_INTREN         0x01
+#define CV_DRAMINTR_SBEMASK        0x02
+#define CV_DRAMINTR_DBEMASK        0x04
+#define CV_DRAMINTR_CORRDROPMASK   0x08
+#define CV_DRAMINTR_INTRCLR        0x10
+
+/* SDRAM Controller Single Bit Error Count Register */
+#define CV_SBECOUNT_OFST           0x40
+
+/* SDRAM Controller Double Bit Error Count Register */
+#define CV_DBECOUNT_OFST           0x44
+
+/* SDRAM Controller ECC Error Address Register */
+#define CV_ERRADDR_OFST            0x48
+
+/*-----------------------------------------*/
+
+/* SDRAM Controller EccCtrl Register */
+#define A10_ECCCTRL1_OFST          0x00
+
+/* SDRAM Controller EccCtrl Register Bit Masks */
+#define A10_ECCCTRL1_ECC_EN        0x001
+#define A10_ECCCTRL1_CNT_RST       0x010
+#define A10_ECCCTRL1_AWB_CNT_RST   0x100
+#define A10_ECC_CNT_RESET_MASK     (A10_ECCCTRL1_CNT_RST | \
+				    A10_ECCCTRL1_AWB_CNT_RST)
+
+/* SDRAM Controller Address Width Register */
+#define CV_DRAMADDRW               0xFFC2502C
+#define A10_DRAMADDRW              0xFFCFA0A8
+
+/* SDRAM Controller Address Widths Field Register */
+#define DRAMADDRW_COLBIT_MASK      0x001F
+#define DRAMADDRW_COLBIT_SHIFT     0
+#define DRAMADDRW_ROWBIT_MASK      0x03E0
+#define DRAMADDRW_ROWBIT_SHIFT     5
+#define CV_DRAMADDRW_BANKBIT_MASK  0x1C00
+#define CV_DRAMADDRW_BANKBIT_SHIFT 10
+#define CV_DRAMADDRW_CSBIT_MASK    0xE000
+#define CV_DRAMADDRW_CSBIT_SHIFT   13
+
+#define A10_DRAMADDRW_BANKBIT_MASK  0x3C00
+#define A10_DRAMADDRW_BANKBIT_SHIFT 10
+#define A10_DRAMADDRW_GRPBIT_MASK   0xC000
+#define A10_DRAMADDRW_GRPBIT_SHIFT  14
+#define A10_DRAMADDRW_CSBIT_MASK    0x70000
+#define A10_DRAMADDRW_CSBIT_SHIFT   16
+
+/* SDRAM Controller Interface Data Width Register */
+#define CV_DRAMIFWIDTH             0xFFC25030
+#define A10_DRAMIFWIDTH            0xFFCFB008
+
+/* SDRAM Controller Interface Data Width Defines */
+#define CV_DRAMIFWIDTH_16B_ECC     24
+#define CV_DRAMIFWIDTH_32B_ECC     40
+
+#define A10_DRAMIFWIDTH_16B        0x0
+#define A10_DRAMIFWIDTH_32B        0x1
+#define A10_DRAMIFWIDTH_64B        0x2
+
+/* SDRAM Controller DRAM IRQ Register */
+#define A10_ERRINTEN_OFST          0x10
+
+/* SDRAM Controller DRAM IRQ Register Bit Masks */
+#define A10_ERRINTEN_SERRINTEN     0x01
+#define A10_ERRINTEN_DERRINTEN     0x02
+#define A10_ECC_IRQ_EN_MASK        (A10_ERRINTEN_SERRINTEN | \
+				    A10_ERRINTEN_DERRINTEN)
+
+/* SDRAM Interrupt Mode Register */
+#define A10_INTMODE_OFST           0x1C
+#define A10_INTMODE_SB_INT         1
+
+/* SDRAM Controller Error Status Register */
+#define A10_INTSTAT_OFST           0x20
+
+/* SDRAM Controller Error Status Register Bit Masks */
+#define A10_INTSTAT_SBEERR         0x01
+#define A10_INTSTAT_DBEERR         0x02
+
+/* SDRAM Controller ECC Error Address Register */
+#define A10_DERRADDR_OFST          0x2C
+#define A10_SERRADDR_OFST          0x30
+
+/* SDRAM Controller ECC Diagnostic Register */
+#define A10_DIAGINTTEST_OFST       0x24
+
+#define A10_DIAGINT_TSERRA_MASK    0x0001
+#define A10_DIAGINT_TDERRA_MASK    0x0100
+
+#define A10_SBERR_IRQ              34
+#define A10_DBERR_IRQ              32
+
+/* SDRAM Single Bit Error Count Compare Set Register */
+#define A10_SERRCNTREG_OFST        0x3C
+
+#define A10_SYMAN_INTMASK_CLR      0xFFD06098
+#define A10_INTMASK_CLR_OFST       0x10
+#define A10_DDR0_IRQ_MASK          BIT(17)
+
+struct altr_sdram_prv_data {
+	int ecc_ctrl_offset;
+	int ecc_ctl_en_mask;
+	int ecc_cecnt_offset;
+	int ecc_uecnt_offset;
+	int ecc_stat_offset;
+	int ecc_stat_ce_mask;
+	int ecc_stat_ue_mask;
+	int ecc_saddr_offset;
+	int ecc_daddr_offset;
+	int ecc_irq_en_offset;
+	int ecc_irq_en_mask;
+	int ecc_irq_clr_offset;
+	int ecc_irq_clr_mask;
+	int ecc_cnt_rst_offset;
+	int ecc_cnt_rst_mask;
+#ifdef CONFIG_EDAC_DEBUG
+	struct edac_dev_sysfs_attribute *eccmgr_sysfs_attr;
+	int ecc_enable_mask;
+	int ce_set_mask;
+	int ue_set_mask;
+	int ce_ue_trgr_offset;
+#endif
+};
+
+/* Altera SDRAM Memory Controller data */
+struct altr_sdram_mc_data {
+	struct regmap *mc_vbase;
+	int sb_irq;
+	int db_irq;
+	const struct altr_sdram_prv_data *data;
+};
+
+#endif	/* #ifndef _ALTERA_EDAC_H */
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index 92772fffc52f..73aea40a9c89 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -2964,6 +2964,7 @@ static struct pci_driver amd64_pci_driver = {
 	.probe		= probe_one_instance,
 	.remove		= remove_one_instance,
 	.id_table	= amd64_pci_table,
+	.driver.probe_type = PROBE_FORCE_SYNCHRONOUS,
 };
 
 static void setup_pci_device(void)
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index af3be1914dbb..943ed8cf71b9 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -30,11 +30,16 @@
 #include <linux/bitops.h>
 #include <asm/uaccess.h>
 #include <asm/page.h>
-#include <asm/edac.h>
 #include "edac_core.h"
 #include "edac_module.h"
 #include <ras/ras_event.h>
 
+#ifdef CONFIG_EDAC_ATOMIC_SCRUB
+#include <asm/edac.h>
+#else
+#define edac_atomic_scrub(va, size) do { } while (0)
+#endif
+
 /* lock to memory controller's control array */
 static DEFINE_MUTEX(mem_ctls_mutex);
 static LIST_HEAD(mc_devices);
@@ -874,7 +879,7 @@ static void edac_mc_scrub_block(unsigned long page, unsigned long offset,
 	virt_addr = kmap_atomic(pg);
 
 	/* Perform architecture specific atomic scrub operation */
-	atomic_scrub(virt_addr + offset, size);
+	edac_atomic_scrub(virt_addr + offset, size);
 
 	/* Unmap and complete */
 	kunmap_atomic(virt_addr);
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index 112d63ad1154..33df7d93c857 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -307,8 +307,6 @@ static struct device_type csrow_attr_type = {
  *
  */
 
-#define EDAC_NR_CHANNELS	6
-
 DEVICE_CHANNEL(ch0_dimm_label, S_IRUGO | S_IWUSR,
 	channel_dimm_label_show, channel_dimm_label_store, 0);
 DEVICE_CHANNEL(ch1_dimm_label, S_IRUGO | S_IWUSR,
@@ -403,9 +401,6 @@ static inline int nr_pages_per_csrow(struct csrow_info *csrow)
 static int edac_create_csrow_object(struct mem_ctl_info *mci,
 				    struct csrow_info *csrow, int index)
 {
-	if (csrow->nr_channels > EDAC_NR_CHANNELS)
-		return -ENODEV;
-
 	csrow->dev.type = &csrow_attr_type;
 	csrow->dev.bus = mci->bus;
 	csrow->dev.groups = csrow_dev_groups;
diff --git a/drivers/edac/edac_stub.c b/drivers/edac/edac_stub.c
index 9d9e18aefaaa..ff07aae5b7fb 100644
--- a/drivers/edac/edac_stub.c
+++ b/drivers/edac/edac_stub.c
@@ -16,7 +16,6 @@
 #include <linux/edac.h>
 #include <linux/atomic.h>
 #include <linux/device.h>
-#include <asm/edac.h>
 
 int edac_op_state = EDAC_OPSTATE_INVAL;
 EXPORT_SYMBOL_GPL(edac_op_state);
diff --git a/drivers/edac/mce_amd_inj.c b/drivers/edac/mce_amd_inj.c
index f7681b553fd5..4c73e4d03d46 100644
--- a/drivers/edac/mce_amd_inj.c
+++ b/drivers/edac/mce_amd_inj.c
@@ -15,6 +15,8 @@
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/cpu.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
 #include <asm/mce.h>
 
 #include "mce_amd.h"
@@ -25,6 +27,25 @@
 static struct mce i_mce;
 static struct dentry *dfs_inj;
 
+static u8 n_banks;
+
+#define MAX_FLAG_OPT_SIZE	3
+
+enum injection_type {
+	SW_INJ = 0,	/* SW injection, simply decode the error */
+	HW_INJ,		/* Trigger a #MC */
+	N_INJ_TYPES,
+};
+
+static const char * const flags_options[] = {
+	[SW_INJ] = "sw",
+	[HW_INJ] = "hw",
+	NULL
+};
+
+/* Set default injection to SW_INJ */
+static enum injection_type inj_type = SW_INJ;
+
 #define MCE_INJECT_SET(reg)						\
 static int inj_##reg##_set(void *data, u64 val)				\
 {									\
@@ -79,24 +100,66 @@ static int toggle_hw_mce_inject(unsigned int cpu, bool enable)
 	return err;
 }
 
-static int flags_get(void *data, u64 *val)
+static int __set_inj(const char *buf)
 {
-	struct mce *m = (struct mce *)data;
+	int i;
 
-	*val = m->inject_flags;
+	for (i = 0; i < N_INJ_TYPES; i++) {
+		if (!strncmp(flags_options[i], buf, strlen(flags_options[i]))) {
+			inj_type = i;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
 
-	return 0;
+static ssize_t flags_read(struct file *filp, char __user *ubuf,
+			  size_t cnt, loff_t *ppos)
+{
+	char buf[MAX_FLAG_OPT_SIZE];
+	int n;
+
+	n = sprintf(buf, "%s\n", flags_options[inj_type]);
+
+	return simple_read_from_buffer(ubuf, cnt, ppos, buf, n);
 }
 
-static int flags_set(void *data, u64 val)
+static ssize_t flags_write(struct file *filp, const char __user *ubuf,
+			   size_t cnt, loff_t *ppos)
 {
-	struct mce *m = (struct mce *)data;
+	char buf[MAX_FLAG_OPT_SIZE], *__buf;
+	int err;
+	size_t ret;
 
-	m->inject_flags = (u8)val;
-	return 0;
+	if (cnt > MAX_FLAG_OPT_SIZE)
+		cnt = MAX_FLAG_OPT_SIZE;
+
+	ret = cnt;
+
+	if (copy_from_user(&buf, ubuf, cnt))
+		return -EFAULT;
+
+	buf[cnt - 1] = 0;
+
+	/* strip whitespace */
+	__buf = strstrip(buf);
+
+	err = __set_inj(__buf);
+	if (err) {
+		pr_err("%s: Invalid flags value: %s\n", __func__, __buf);
+		return err;
+	}
+
+	*ppos += ret;
+
+	return ret;
 }
 
-DEFINE_SIMPLE_ATTRIBUTE(flags_fops, flags_get, flags_set, "%llu\n");
+static const struct file_operations flags_fops = {
+	.read           = flags_read,
+	.write          = flags_write,
+	.llseek         = generic_file_llseek,
+};
 
 /*
  * On which CPU to inject?
@@ -128,21 +191,24 @@ static void do_inject(void)
 	unsigned int cpu = i_mce.extcpu;
 	u8 b = i_mce.bank;
 
-	if (!(i_mce.inject_flags & MCJ_EXCEPTION)) {
+	if (i_mce.misc)
+		i_mce.status |= MCI_STATUS_MISCV;
+
+	if (inj_type == SW_INJ) {
 		amd_decode_mce(NULL, 0, &i_mce);
 		return;
 	}
 
-	get_online_cpus();
-	if (!cpu_online(cpu))
-		goto err;
-
 	/* prep MCE global settings for the injection */
 	mcg_status = MCG_STATUS_MCIP | MCG_STATUS_EIPV;
 
 	if (!(i_mce.status & MCI_STATUS_PCC))
 		mcg_status |= MCG_STATUS_RIPV;
 
+	get_online_cpus();
+	if (!cpu_online(cpu))
+		goto err;
+
 	toggle_hw_mce_inject(cpu, true);
 
 	wrmsr_on_cpu(cpu, MSR_IA32_MCG_STATUS,
@@ -174,11 +240,9 @@ static int inj_bank_set(void *data, u64 val)
 {
 	struct mce *m = (struct mce *)data;
 
-	if (val > 5) {
-		if (boot_cpu_data.x86 != 0x15 || val > 6) {
-			pr_err("Non-existent MCE bank: %llu\n", val);
-			return -EINVAL;
-		}
+	if (val >= n_banks) {
+		pr_err("Non-existent MCE bank: %llu\n", val);
+		return -EINVAL;
 	}
 
 	m->bank = val;
@@ -187,32 +251,81 @@ static int inj_bank_set(void *data, u64 val)
 	return 0;
 }
 
-static int inj_bank_get(void *data, u64 *val)
-{
-	struct mce *m = (struct mce *)data;
+MCE_INJECT_GET(bank);
 
-	*val = m->bank;
-	return 0;
+DEFINE_SIMPLE_ATTRIBUTE(bank_fops, inj_bank_get, inj_bank_set, "%llu\n");
+
+static const char readme_msg[] =
+"Description of the files and their usages:\n"
+"\n"
+"Note1: i refers to the bank number below.\n"
+"Note2: See respective BKDGs for the exact bit definitions of the files below\n"
+"as they mirror the hardware registers.\n"
+"\n"
+"status:\t Set MCi_STATUS: the bits in that MSR control the error type and\n"
+"\t attributes of the error which caused the MCE.\n"
+"\n"
+"misc:\t Set MCi_MISC: provide auxiliary info about the error. It is mostly\n"
+"\t used for error thresholding purposes and its validity is indicated by\n"
+"\t MCi_STATUS[MiscV].\n"
+"\n"
+"addr:\t Error address value to be written to MCi_ADDR. Log address information\n"
+"\t associated with the error.\n"
+"\n"
+"cpu:\t The CPU to inject the error on.\n"
+"\n"
+"bank:\t Specify the bank you want to inject the error into: the number of\n"
+"\t banks in a processor varies and is family/model-specific, therefore, the\n"
+"\t supplied value is sanity-checked. Setting the bank value also triggers the\n"
+"\t injection.\n"
+"\n"
+"flags:\t Injection type to be performed. Writing to this file will trigger a\n"
+"\t real machine check, an APIC interrupt or invoke the error decoder routines\n"
+"\t for AMD processors.\n"
+"\n"
+"\t Allowed error injection types:\n"
+"\t  - \"sw\": Software error injection. Decode error to a human-readable \n"
+"\t    format only. Safe to use.\n"
+"\t  - \"hw\": Hardware error injection. Causes the #MC exception handler to \n"
+"\t    handle the error. Be warned: might cause system panic if MCi_STATUS[PCC] \n"
+"\t    is set. Therefore, consider setting (debugfs_mountpoint)/mce/fake_panic \n"
+"\t    before injecting.\n"
+"\n";
+
+static ssize_t
+inj_readme_read(struct file *filp, char __user *ubuf,
+		       size_t cnt, loff_t *ppos)
+{
+	return simple_read_from_buffer(ubuf, cnt, ppos,
+					readme_msg, strlen(readme_msg));
 }
 
-DEFINE_SIMPLE_ATTRIBUTE(bank_fops, inj_bank_get, inj_bank_set, "%llu\n");
+static const struct file_operations readme_fops = {
+	.read		= inj_readme_read,
+};
 
 static struct dfs_node {
 	char *name;
 	struct dentry *d;
 	const struct file_operations *fops;
+	umode_t perm;
 } dfs_fls[] = {
-	{ .name = "status",	.fops = &status_fops },
-	{ .name = "misc",	.fops = &misc_fops },
-	{ .name = "addr",	.fops = &addr_fops },
-	{ .name = "bank",	.fops = &bank_fops },
-	{ .name = "flags",	.fops = &flags_fops },
-	{ .name = "cpu",	.fops = &extcpu_fops },
+	{ .name = "status",	.fops = &status_fops, .perm = S_IRUSR | S_IWUSR },
+	{ .name = "misc",	.fops = &misc_fops,   .perm = S_IRUSR | S_IWUSR },
+	{ .name = "addr",	.fops = &addr_fops,   .perm = S_IRUSR | S_IWUSR },
+	{ .name = "bank",	.fops = &bank_fops,   .perm = S_IRUSR | S_IWUSR },
+	{ .name = "flags",	.fops = &flags_fops,  .perm = S_IRUSR | S_IWUSR },
+	{ .name = "cpu",	.fops = &extcpu_fops, .perm = S_IRUSR | S_IWUSR },
+	{ .name = "README",	.fops = &readme_fops, .perm = S_IRUSR | S_IRGRP | S_IROTH },
 };
 
 static int __init init_mce_inject(void)
 {
 	int i;
+	u64 cap;
+
+	rdmsrl(MSR_IA32_MCG_CAP, cap);
+	n_banks = cap & MCG_BANKCNT_MASK;
 
 	dfs_inj = debugfs_create_dir("mce-inject", NULL);
 	if (!dfs_inj)
@@ -220,7 +333,7 @@ static int __init init_mce_inject(void)
 
 	for (i = 0; i < ARRAY_SIZE(dfs_fls); i++) {
 		dfs_fls[i].d = debugfs_create_file(dfs_fls[i].name,
-						    S_IRUSR | S_IWUSR,
+						    dfs_fls[i].perm,
 						    dfs_inj,
 						    &i_mce,
 						    dfs_fls[i].fops);
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 68bf234bdfe6..23ef8e9f2c9a 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -811,6 +811,8 @@ static void sbe_ecc_decode(u32 cap_high, u32 cap_low, u32 cap_ecc,
 	}
 }
 
+#define make64(high, low) (((u64)(high) << 32) | (low))
+
 static void mpc85xx_mc_check(struct mem_ctl_info *mci)
 {
 	struct mpc85xx_mc_pdata *pdata = mci->pvt_info;
@@ -818,7 +820,7 @@ static void mpc85xx_mc_check(struct mem_ctl_info *mci)
 	u32 bus_width;
 	u32 err_detect;
 	u32 syndrome;
-	u32 err_addr;
+	u64 err_addr;
 	u32 pfn;
 	int row_index;
 	u32 cap_high;
@@ -849,7 +851,9 @@ static void mpc85xx_mc_check(struct mem_ctl_info *mci)
 	else
 		syndrome &= 0xffff;
 
-	err_addr = in_be32(pdata->mc_vbase + MPC85XX_MC_CAPTURE_ADDRESS);
+	err_addr = make64(
+		in_be32(pdata->mc_vbase + MPC85XX_MC_CAPTURE_EXT_ADDRESS),
+		in_be32(pdata->mc_vbase + MPC85XX_MC_CAPTURE_ADDRESS));
 	pfn = err_addr >> PAGE_SHIFT;
 
 	for (row_index = 0; row_index < mci->nr_csrows; row_index++) {
@@ -886,7 +890,7 @@ static void mpc85xx_mc_check(struct mem_ctl_info *mci)
 	mpc85xx_mc_printk(mci, KERN_ERR,
 			"Captured Data / ECC:\t%#8.8x_%08x / %#2.2x\n",
 			cap_high, cap_low, syndrome);
-	mpc85xx_mc_printk(mci, KERN_ERR, "Err addr: %#8.8x\n", err_addr);
+	mpc85xx_mc_printk(mci, KERN_ERR, "Err addr: %#8.8llx\n", err_addr);
 	mpc85xx_mc_printk(mci, KERN_ERR, "PFN: %#8.8x\n", pfn);
 
 	/* we are out of range */
diff --git a/drivers/edac/mpc85xx_edac.h b/drivers/edac/mpc85xx_edac.h
index 4498baf9ce05..9352e88d53e5 100644
--- a/drivers/edac/mpc85xx_edac.h
+++ b/drivers/edac/mpc85xx_edac.h
@@ -43,6 +43,7 @@
 #define MPC85XX_MC_ERR_INT_EN		0x0e48
 #define MPC85XX_MC_CAPTURE_ATRIBUTES	0x0e4c
 #define MPC85XX_MC_CAPTURE_ADDRESS	0x0e50
+#define MPC85XX_MC_CAPTURE_EXT_ADDRESS	0x0e54
 #define MPC85XX_MC_ERR_SBE		0x0e58
 
 #define DSC_MEM_EN	0x80000000
diff --git a/drivers/edac/octeon_edac-l2c.c b/drivers/edac/octeon_edac-l2c.c
index 7e98084d3645..afea7fc625cc 100644
--- a/drivers/edac/octeon_edac-l2c.c
+++ b/drivers/edac/octeon_edac-l2c.c
@@ -151,7 +151,7 @@ static int octeon_l2c_probe(struct platform_device *pdev)
 	l2c->ctl_name = "octeon_l2c_err";
 
 
-	if (OCTEON_IS_MODEL(OCTEON_FAM_1_PLUS)) {
+	if (OCTEON_IS_OCTEON1PLUS()) {
 		union cvmx_l2t_err l2t_err;
 		union cvmx_l2d_err l2d_err;
 
diff --git a/drivers/edac/octeon_edac-lmc.c b/drivers/edac/octeon_edac-lmc.c
index bb19e0732681..cda6dab5067a 100644
--- a/drivers/edac/octeon_edac-lmc.c
+++ b/drivers/edac/octeon_edac-lmc.c
@@ -234,7 +234,7 @@ static int octeon_lmc_edac_probe(struct platform_device *pdev)
 	layers[0].size = 1;
 	layers[0].is_virt_csrow = false;
 
-	if (OCTEON_IS_MODEL(OCTEON_FAM_1_PLUS)) {
+	if (OCTEON_IS_OCTEON1PLUS()) {
 		union cvmx_lmcx_mem_cfg0 cfg0;
 
 		cfg0.u64 = cvmx_read_csr(CVMX_LMCX_MEM_CFG0(0));
diff --git a/drivers/edac/octeon_edac-pc.c b/drivers/edac/octeon_edac-pc.c
index 0f83c33a7d1f..2ab6cf24c959 100644
--- a/drivers/edac/octeon_edac-pc.c
+++ b/drivers/edac/octeon_edac-pc.c
@@ -73,7 +73,7 @@ static int  co_cache_error_event(struct notifier_block *this,
 			edac_device_handle_ce(p->ed, cpu, 0, "dcache");
 
 		/* Clear the error indication */
-		if (OCTEON_IS_MODEL(OCTEON_FAM_2))
+		if (OCTEON_IS_OCTEON2())
 			write_octeon_c0_dcacheerr(1);
 		else
 			write_octeon_c0_dcacheerr(0);
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index 1acf57ba4c86..ca7831168298 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -34,7 +34,7 @@ static int probed;
 /*
  * Alter this version for the module when modifications are made
  */
-#define SBRIDGE_REVISION    " Ver: 1.1.0 "
+#define SBRIDGE_REVISION    " Ver: 1.1.1 "
 #define EDAC_MOD_STR      "sbridge_edac"
 
 /*
@@ -254,7 +254,7 @@ static const u32 correrrthrsld[] = {
  * sbridge structs
  */
 
-#define NUM_CHANNELS		4
+#define NUM_CHANNELS		8	/* 2MC per socket, four chan per MC */
 #define MAX_DIMMS		3	/* Max DIMMS per channel */
 #define CHANNEL_UNSPECIFIED	0xf	/* Intel IA32 SDM 15-14 */
 
@@ -393,6 +393,8 @@ static const struct pci_id_table pci_dev_descr_sbridge_table[] = {
 #define PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_RAS		0x0e79
 #define PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0	0x0e6a
 #define PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD1	0x0e6b
+#define PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD2	0x0e6c
+#define PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD3	0x0e6d
 
 static const struct pci_id_descr pci_dev_descr_ibridge[] = {
 		/* Processor Home Agent */
@@ -421,6 +423,8 @@ static const struct pci_id_descr pci_dev_descr_ibridge[] = {
 #endif
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0, 1)	},
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD1, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD2, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD3, 1)	},
 
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_1HA_DDRIO0, 1)	},
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_2HA_DDRIO0, 1)	},
@@ -504,17 +508,35 @@ static const struct pci_id_table pci_dev_descr_haswell_table[] = {
  * DE processor:
  *	- 1 IMC
  *	- 2 DDR3 channels, 2 DPC per channel
+ * EP processor:
+ *	- 1 or 2 IMC
+ *	- 4 DDR4 channels, 3 DPC per channel
+ * EP 4S processor:
+ *	- 2 IMC
+ *	- 4 DDR4 channels, 3 DPC per channel
+ * EX processor:
+ *	- 2 IMC
+ *	- each IMC interfaces with a SMI 2 channel
+ *	- each SMI channel interfaces with a scalable memory buffer
+ *	- each scalable memory buffer supports 4 DDR3/DDR4 channels, 3 DPC
  */
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_VTD_MISC 0x6f28
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0	0x6fa0
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1	0x6f60
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TA	0x6fa8
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_THERMAL 0x6f71
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TA	0x6f68
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_THERMAL 0x6f79
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD0 0x6ffc
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD1 0x6ffd
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0 0x6faa
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD1 0x6fab
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD2 0x6fac
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD3 0x6fad
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD0 0x6f6a
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD1 0x6f6b
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD2 0x6f6c
+#define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD3 0x6f6d
 #define PCI_DEVICE_ID_INTEL_BROADWELL_IMC_DDRIO0 0x6faf
 
 static const struct pci_id_descr pci_dev_descr_broadwell[] = {
@@ -524,13 +546,23 @@ static const struct pci_id_descr pci_dev_descr_broadwell[] = {
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD0, 0)	},
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_CBO_SAD1, 0)	},
 
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1, 1)		},
+
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TA, 0)	},
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_THERMAL, 0)	},
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0, 0)	},
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD1, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD2, 0)	},
-	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD3, 0)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD2, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD3, 1)	},
+
 	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_DDRIO0, 1)	},
+
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TA, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_THERMAL, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD0, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD1, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD2, 1)	},
+	{ PCI_DESCR(PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD3, 1)	},
 };
 
 static const struct pci_id_table pci_dev_descr_broadwell_table[] = {
@@ -559,7 +591,7 @@ static inline int numrank(enum type type, u32 mtr)
 	int ranks = (1 << RANK_CNT_BITS(mtr));
 	int max = 4;
 
-	if (type == HASWELL)
+	if (type == HASWELL || type == BROADWELL)
 		max = 8;
 
 	if (ranks > max) {
@@ -909,6 +941,8 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 	for (i = 0; i < NUM_CHANNELS; i++) {
 		u32 mtr;
 
+		if (!pvt->pci_tad[i])
+			continue;
 		for (j = 0; j < ARRAY_SIZE(mtr_regs); j++) {
 			dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers,
 				       i, j, 0);
@@ -925,8 +959,8 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 				size = ((u64)rows * cols * banks * ranks) >> (20 - 3);
 				npages = MiB_TO_PAGES(size);
 
-				edac_dbg(0, "mc#%d: channel %d, dimm %d, %Ld Mb (%d pages) bank: %d, rank: %d, row: %#x, col: %#x\n",
-					 pvt->sbridge_dev->mc, i, j,
+				edac_dbg(0, "mc#%d: ha %d channel %d, dimm %d, %lld Mb (%d pages) bank: %d, rank: %d, row: %#x, col: %#x\n",
+					 pvt->sbridge_dev->mc, i/4, i%4, j,
 					 size, npages,
 					 banks, ranks, rows, cols);
 
@@ -946,8 +980,8 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 				dimm->mtype = mtype;
 				dimm->edac_mode = mode;
 				snprintf(dimm->label, sizeof(dimm->label),
-					 "CPU_SrcID#%u_Channel#%u_DIMM#%u",
-					 pvt->sbridge_dev->source_id, i, j);
+					 "CPU_SrcID#%u_Ha#%u_Chan#%u_DIMM#%u",
+					 pvt->sbridge_dev->source_id, i/4, i%4, j);
 			}
 		}
 	}
@@ -1128,7 +1162,7 @@ static struct mem_ctl_info *get_mci_for_node_id(u8 node_id)
 
 static int get_memory_error_data(struct mem_ctl_info *mci,
 				 u64 addr,
-				 u8 *socket,
+				 u8 *socket, u8 *ha,
 				 long *channel_mask,
 				 u8 *rank,
 				 char **area_type, char *msg)
@@ -1141,7 +1175,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	int			interleave_mode, shiftup = 0;
 	unsigned		sad_interleave[pvt->info.max_interleave];
 	u32			reg, dram_rule;
-	u8			ch_way, sck_way, pkg, sad_ha = 0;
+	u8			ch_way, sck_way, pkg, sad_ha = 0, ch_add = 0;
 	u32			tad_offset;
 	u32			rir_way;
 	u32			mb, gb;
@@ -1242,9 +1276,9 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 			bits = GET_BITFIELD(addr, 7, 8) << 1;
 			bits |= GET_BITFIELD(addr, 9, 9);
 		} else
-			bits = GET_BITFIELD(addr, 7, 9);
+			bits = GET_BITFIELD(addr, 6, 8);
 
-		if (interleave_mode) {
+		if (interleave_mode == 0) {
 			/* interleave mode will XOR {8,7,6} with {18,17,16} */
 			idx = GET_BITFIELD(addr, 16, 18);
 			idx ^= bits;
@@ -1254,6 +1288,8 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		pkg = sad_pkg(pvt->info.interleave_pkg, reg, idx);
 		*socket = sad_pkg_socket(pkg);
 		sad_ha = sad_pkg_ha(pkg);
+		if (sad_ha)
+			ch_add = 4;
 
 		if (a7mode) {
 			/* MCChanShiftUpEnable */
@@ -1270,10 +1306,14 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		pkg = sad_pkg(pvt->info.interleave_pkg, reg, idx);
 		*socket = sad_pkg_socket(pkg);
 		sad_ha = sad_pkg_ha(pkg);
+		if (sad_ha)
+			ch_add = 4;
 		edac_dbg(0, "SAD interleave package: %d = CPU socket %d, HA %d\n",
 			 idx, *socket, sad_ha);
 	}
 
+	*ha = sad_ha;
+
 	/*
 	 * Move to the proper node structure, in order to access the
 	 * right PCI registers
@@ -1346,7 +1386,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	}
 	*channel_mask = 1 << base_ch;
 
-	pci_read_config_dword(pvt->pci_tad[base_ch],
+	pci_read_config_dword(pvt->pci_tad[ch_add + base_ch],
 				tad_ch_nilv_offset[n_tads],
 				&tad_offset);
 
@@ -1405,7 +1445,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 	 * Step 3) Decode rank
 	 */
 	for (n_rir = 0; n_rir < MAX_RIR_RANGES; n_rir++) {
-		pci_read_config_dword(pvt->pci_tad[base_ch],
+		pci_read_config_dword(pvt->pci_tad[ch_add + base_ch],
 				      rir_way_limit[n_rir],
 				      &reg);
 
@@ -1435,7 +1475,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		idx = (ch_addr >> 13);	/* FIXME: Datasheet says to shift by 15 */
 	idx %= 1 << rir_way;
 
-	pci_read_config_dword(pvt->pci_tad[base_ch],
+	pci_read_config_dword(pvt->pci_tad[ch_add + base_ch],
 			      rir_offset[n_rir][idx],
 			      &reg);
 	*rank = RIR_RNK_TGT(reg);
@@ -1681,16 +1721,9 @@ static int ibridge_mci_bind_devs(struct mem_ctl_info *mci,
 				 struct sbridge_dev *sbridge_dev)
 {
 	struct sbridge_pvt *pvt = mci->pvt_info;
-	struct pci_dev *pdev, *tmp;
+	struct pci_dev *pdev;
+	u8 saw_chan_mask = 0;
 	int i;
-	bool mode_2ha = false;
-
-	tmp = pci_get_device(PCI_VENDOR_ID_INTEL,
-			     PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1, NULL);
-	if (tmp) {
-		mode_2ha = true;
-		pci_dev_put(tmp);
-	}
 
 	for (i = 0; i < sbridge_dev->n_devs; i++) {
 		pdev = sbridge_dev->pdev[i];
@@ -1706,26 +1739,21 @@ static int ibridge_mci_bind_devs(struct mem_ctl_info *mci,
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_RAS:
 			pvt->pci_ras = pdev;
 			break;
-		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD2:
-		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD3:
-			/* if we have 2 HAs active, channels 2 and 3
-			 * are in other device */
-			if (mode_2ha)
-				break;
-			/* fall through */
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD0:
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD1:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD2:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD3:
 		{
 			int id = pdev->device - PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TAD0;
 			pvt->pci_tad[id] = pdev;
+			saw_chan_mask |= 1 << id;
 		}
 			break;
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_2HA_DDRIO0:
 			pvt->pci_ddrio = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_1HA_DDRIO0:
-			if (!mode_2ha)
-				pvt->pci_ddrio = pdev;
+			pvt->pci_ddrio = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_SAD:
 			pvt->pci_sad0 = pdev;
@@ -1741,13 +1769,12 @@ static int ibridge_mci_bind_devs(struct mem_ctl_info *mci,
 			break;
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0:
 		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD1:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD2:
+		case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD3:
 		{
-			int id = pdev->device - PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0 + 2;
-
-			/* we shouldn't have this device if we have just one
-			 * HA present */
-			WARN_ON(!mode_2ha);
+			int id = pdev->device - PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA1_TAD0 + 4;
 			pvt->pci_tad[id] = pdev;
+			saw_chan_mask |= 1 << id;
 		}
 			break;
 		default:
@@ -1766,10 +1793,10 @@ static int ibridge_mci_bind_devs(struct mem_ctl_info *mci,
 	    !pvt->pci_ta)
 		goto enodev;
 
-	for (i = 0; i < NUM_CHANNELS; i++) {
-		if (!pvt->pci_tad[i])
-			goto enodev;
-	}
+	if (saw_chan_mask != 0x0f && /* -EN */
+	    saw_chan_mask != 0x33 && /* -EP */
+	    saw_chan_mask != 0xff)   /* -EX */
+		goto enodev;
 	return 0;
 
 enodev:
@@ -1787,16 +1814,9 @@ static int haswell_mci_bind_devs(struct mem_ctl_info *mci,
 				 struct sbridge_dev *sbridge_dev)
 {
 	struct sbridge_pvt *pvt = mci->pvt_info;
-	struct pci_dev *pdev, *tmp;
+	struct pci_dev *pdev;
+	u8 saw_chan_mask = 0;
 	int i;
-	bool mode_2ha = false;
-
-	tmp = pci_get_device(PCI_VENDOR_ID_INTEL,
-			     PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1, NULL);
-	if (tmp) {
-		mode_2ha = true;
-		pci_dev_put(tmp);
-	}
 
 	/* there's only one device per system; not tied to any bus */
 	if (pvt->info.pci_vtd == NULL)
@@ -1827,18 +1847,26 @@ static int haswell_mci_bind_devs(struct mem_ctl_info *mci,
 			pvt->pci_ras = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD0:
-			pvt->pci_tad[0] = pdev;
-			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD1:
-			pvt->pci_tad[1] = pdev;
-			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD2:
-			if (!mode_2ha)
-				pvt->pci_tad[2] = pdev;
-			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD3:
-			if (!mode_2ha)
-				pvt->pci_tad[3] = pdev;
+		{
+			int id = pdev->device - PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0_TAD0;
+
+			pvt->pci_tad[id] = pdev;
+			saw_chan_mask |= 1 << id;
+		}
+			break;
+		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD0:
+		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD1:
+		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD2:
+		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD3:
+		{
+			int id = pdev->device - PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD0 + 4;
+
+			pvt->pci_tad[id] = pdev;
+			saw_chan_mask |= 1 << id;
+		}
 			break;
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_DDRIO0:
 			pvt->pci_ddrio = pdev;
@@ -1849,14 +1877,6 @@ static int haswell_mci_bind_devs(struct mem_ctl_info *mci,
 		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TA:
 			pvt->pci_ha1_ta = pdev;
 			break;
-		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD0:
-			if (mode_2ha)
-				pvt->pci_tad[2] = pdev;
-			break;
-		case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA1_TAD1:
-			if (mode_2ha)
-				pvt->pci_tad[3] = pdev;
-			break;
 		default:
 			break;
 		}
@@ -1872,10 +1892,10 @@ static int haswell_mci_bind_devs(struct mem_ctl_info *mci,
 	    !pvt->pci_ras  || !pvt->pci_ta || !pvt->info.pci_vtd)
 		goto enodev;
 
-	for (i = 0; i < NUM_CHANNELS; i++) {
-		if (!pvt->pci_tad[i])
-			goto enodev;
-	}
+	if (saw_chan_mask != 0x0f && /* -EN */
+	    saw_chan_mask != 0x33 && /* -EP */
+	    saw_chan_mask != 0xff)   /* -EX */
+		goto enodev;
 	return 0;
 
 enodev:
@@ -1888,6 +1908,7 @@ static int broadwell_mci_bind_devs(struct mem_ctl_info *mci,
 {
 	struct sbridge_pvt *pvt = mci->pvt_info;
 	struct pci_dev *pdev;
+	u8 saw_chan_mask = 0;
 	int i;
 
 	/* there's only one device per system; not tied to any bus */
@@ -1919,20 +1940,34 @@ static int broadwell_mci_bind_devs(struct mem_ctl_info *mci,
 			pvt->pci_ras = pdev;
 			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0:
-			pvt->pci_tad[0] = pdev;
-			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD1:
-			pvt->pci_tad[1] = pdev;
-			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD2:
-			pvt->pci_tad[2] = pdev;
-			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD3:
-			pvt->pci_tad[3] = pdev;
+		{
+			int id = pdev->device - PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TAD0;
+			pvt->pci_tad[id] = pdev;
+			saw_chan_mask |= 1 << id;
+		}
+			break;
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD0:
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD1:
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD2:
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD3:
+		{
+			int id = pdev->device - PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TAD0 + 4;
+			pvt->pci_tad[id] = pdev;
+			saw_chan_mask |= 1 << id;
+		}
 			break;
 		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_DDRIO0:
 			pvt->pci_ddrio = pdev;
 			break;
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1:
+			pvt->pci_ha1 = pdev;
+			break;
+		case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA1_TA:
+			pvt->pci_ha1_ta = pdev;
+			break;
 		default:
 			break;
 		}
@@ -1948,10 +1983,10 @@ static int broadwell_mci_bind_devs(struct mem_ctl_info *mci,
 	    !pvt->pci_ras  || !pvt->pci_ta || !pvt->info.pci_vtd)
 		goto enodev;
 
-	for (i = 0; i < NUM_CHANNELS; i++) {
-		if (!pvt->pci_tad[i])
-			goto enodev;
-	}
+	if (saw_chan_mask != 0x0f && /* -EN */
+	    saw_chan_mask != 0x33 && /* -EP */
+	    saw_chan_mask != 0xff)   /* -EX */
+		goto enodev;
 	return 0;
 
 enodev:
@@ -1986,11 +2021,11 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	u32 channel = GET_BITFIELD(m->status, 0, 3);
 	u32 optypenum = GET_BITFIELD(m->status, 4, 6);
 	long channel_mask, first_channel;
-	u8  rank, socket;
+	u8  rank, socket, ha;
 	int rc, dimm;
 	char *area_type = NULL;
 
-	if (pvt->info.type == IVY_BRIDGE)
+	if (pvt->info.type != SANDY_BRIDGE)
 		recoverable = true;
 	else
 		recoverable = GET_BITFIELD(m->status, 56, 56);
@@ -2048,7 +2083,7 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	if (!GET_BITFIELD(m->status, 58, 58))
 		return;
 
-	rc = get_memory_error_data(mci, m->addr, &socket,
+	rc = get_memory_error_data(mci, m->addr, &socket, &ha,
 				   &channel_mask, &rank, &area_type, msg);
 	if (rc < 0)
 		goto err_parsing;
@@ -2080,12 +2115,12 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 		channel = first_channel;
 
 	snprintf(msg, sizeof(msg),
-		 "%s%s area:%s err_code:%04x:%04x socket:%d channel_mask:%ld rank:%d",
+		 "%s%s area:%s err_code:%04x:%04x socket:%d ha:%d channel_mask:%ld rank:%d",
 		 overflow ? " OVERFLOW" : "",
 		 (uncorrected_error && recoverable) ? " recoverable" : "",
 		 area_type,
 		 mscod, errcode,
-		 socket,
+		 socket, ha,
 		 channel_mask,
 		 rank);
 
@@ -2099,7 +2134,7 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	/* Call the helper to output message */
 	edac_mc_handle_error(tp_event, mci, core_err_cnt,
 			     m->addr >> PAGE_SHIFT, m->addr & ~PAGE_MASK, 0,
-			     channel, dimm, -1,
+			     4*ha+channel, dimm, -1,
 			     optype, msg);
 	return;
 err_parsing:
diff --git a/drivers/edac/xgene_edac.c b/drivers/edac/xgene_edac.c
new file mode 100644
index 000000000000..14636e4b6a08
--- /dev/null
+++ b/drivers/edac/xgene_edac.c
@@ -0,0 +1,1215 @@
+/*
+ * APM X-Gene SoC EDAC (error detection and correction)
+ *
+ * Copyright (c) 2015, Applied Micro Circuits Corporation
+ * Author: Feng Kan <fkan@apm.com>
+ *         Loc Ho <lho@apm.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/ctype.h>
+#include <linux/edac.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/regmap.h>
+
+#include "edac_core.h"
+
+#define EDAC_MOD_STR			"xgene_edac"
+
+/* Global error configuration status registers (CSR) */
+#define PCPHPERRINTSTS			0x0000
+#define PCPHPERRINTMSK			0x0004
+#define  MCU_CTL_ERR_MASK		BIT(12)
+#define  IOB_PA_ERR_MASK		BIT(11)
+#define  IOB_BA_ERR_MASK		BIT(10)
+#define  IOB_XGIC_ERR_MASK		BIT(9)
+#define  IOB_RB_ERR_MASK		BIT(8)
+#define  L3C_UNCORR_ERR_MASK		BIT(5)
+#define  MCU_UNCORR_ERR_MASK		BIT(4)
+#define  PMD3_MERR_MASK			BIT(3)
+#define  PMD2_MERR_MASK			BIT(2)
+#define  PMD1_MERR_MASK			BIT(1)
+#define  PMD0_MERR_MASK			BIT(0)
+#define PCPLPERRINTSTS			0x0008
+#define PCPLPERRINTMSK			0x000C
+#define  CSW_SWITCH_TRACE_ERR_MASK	BIT(2)
+#define  L3C_CORR_ERR_MASK		BIT(1)
+#define  MCU_CORR_ERR_MASK		BIT(0)
+#define MEMERRINTSTS			0x0010
+#define MEMERRINTMSK			0x0014
+
+struct xgene_edac {
+	struct device		*dev;
+	struct regmap		*csw_map;
+	struct regmap		*mcba_map;
+	struct regmap		*mcbb_map;
+	struct regmap		*efuse_map;
+	void __iomem		*pcp_csr;
+	spinlock_t		lock;
+	struct dentry		*dfs;
+
+	struct list_head	mcus;
+	struct list_head	pmds;
+
+	struct mutex		mc_lock;
+	int			mc_active_mask;
+	int			mc_registered_mask;
+};
+
+static void xgene_edac_pcp_rd(struct xgene_edac *edac, u32 reg, u32 *val)
+{
+	*val = readl(edac->pcp_csr + reg);
+}
+
+static void xgene_edac_pcp_clrbits(struct xgene_edac *edac, u32 reg,
+				   u32 bits_mask)
+{
+	u32 val;
+
+	spin_lock(&edac->lock);
+	val = readl(edac->pcp_csr + reg);
+	val &= ~bits_mask;
+	writel(val, edac->pcp_csr + reg);
+	spin_unlock(&edac->lock);
+}
+
+static void xgene_edac_pcp_setbits(struct xgene_edac *edac, u32 reg,
+				   u32 bits_mask)
+{
+	u32 val;
+
+	spin_lock(&edac->lock);
+	val = readl(edac->pcp_csr + reg);
+	val |= bits_mask;
+	writel(val, edac->pcp_csr + reg);
+	spin_unlock(&edac->lock);
+}
+
+/* Memory controller error CSR */
+#define MCU_MAX_RANK			8
+#define MCU_RANK_STRIDE			0x40
+
+#define MCUGECR				0x0110
+#define  MCU_GECR_DEMANDUCINTREN_MASK	BIT(0)
+#define  MCU_GECR_BACKUCINTREN_MASK	BIT(1)
+#define  MCU_GECR_CINTREN_MASK		BIT(2)
+#define  MUC_GECR_MCUADDRERREN_MASK	BIT(9)
+#define MCUGESR				0x0114
+#define  MCU_GESR_ADDRNOMATCH_ERR_MASK	BIT(7)
+#define  MCU_GESR_ADDRMULTIMATCH_ERR_MASK	BIT(6)
+#define  MCU_GESR_PHYP_ERR_MASK		BIT(3)
+#define MCUESRR0			0x0314
+#define  MCU_ESRR_MULTUCERR_MASK	BIT(3)
+#define  MCU_ESRR_BACKUCERR_MASK	BIT(2)
+#define  MCU_ESRR_DEMANDUCERR_MASK	BIT(1)
+#define  MCU_ESRR_CERR_MASK		BIT(0)
+#define MCUESRRA0			0x0318
+#define MCUEBLRR0			0x031c
+#define  MCU_EBLRR_ERRBANK_RD(src)	(((src) & 0x00000007) >> 0)
+#define MCUERCRR0			0x0320
+#define  MCU_ERCRR_ERRROW_RD(src)	(((src) & 0xFFFF0000) >> 16)
+#define  MCU_ERCRR_ERRCOL_RD(src)	((src) & 0x00000FFF)
+#define MCUSBECNT0			0x0324
+#define MCU_SBECNT_COUNT(src)		((src) & 0xFFFF)
+
+#define CSW_CSWCR			0x0000
+#define  CSW_CSWCR_DUALMCB_MASK		BIT(0)
+
+#define MCBADDRMR			0x0000
+#define  MCBADDRMR_MCU_INTLV_MODE_MASK	BIT(3)
+#define  MCBADDRMR_DUALMCU_MODE_MASK	BIT(2)
+#define  MCBADDRMR_MCB_INTLV_MODE_MASK	BIT(1)
+#define  MCBADDRMR_ADDRESS_MODE_MASK	BIT(0)
+
+struct xgene_edac_mc_ctx {
+	struct list_head	next;
+	char			*name;
+	struct mem_ctl_info	*mci;
+	struct xgene_edac	*edac;
+	void __iomem		*mcu_csr;
+	u32			mcu_id;
+};
+
+static ssize_t xgene_edac_mc_err_inject_write(struct file *file,
+					      const char __user *data,
+					      size_t count, loff_t *ppos)
+{
+	struct mem_ctl_info *mci = file->private_data;
+	struct xgene_edac_mc_ctx *ctx = mci->pvt_info;
+	int i;
+
+	for (i = 0; i < MCU_MAX_RANK; i++) {
+		writel(MCU_ESRR_MULTUCERR_MASK | MCU_ESRR_BACKUCERR_MASK |
+		       MCU_ESRR_DEMANDUCERR_MASK | MCU_ESRR_CERR_MASK,
+		       ctx->mcu_csr + MCUESRRA0 + i * MCU_RANK_STRIDE);
+	}
+	return count;
+}
+
+static const struct file_operations xgene_edac_mc_debug_inject_fops = {
+	.open = simple_open,
+	.write = xgene_edac_mc_err_inject_write,
+	.llseek = generic_file_llseek,
+};
+
+static void xgene_edac_mc_create_debugfs_node(struct mem_ctl_info *mci)
+{
+	if (!IS_ENABLED(CONFIG_EDAC_DEBUG))
+		return;
+#ifdef CONFIG_EDAC_DEBUG
+	if (!mci->debugfs)
+		return;
+	debugfs_create_file("inject_ctrl", S_IWUSR, mci->debugfs, mci,
+			    &xgene_edac_mc_debug_inject_fops);
+#endif
+}
+
+static void xgene_edac_mc_check(struct mem_ctl_info *mci)
+{
+	struct xgene_edac_mc_ctx *ctx = mci->pvt_info;
+	unsigned int pcp_hp_stat;
+	unsigned int pcp_lp_stat;
+	u32 reg;
+	u32 rank;
+	u32 bank;
+	u32 count;
+	u32 col_row;
+
+	xgene_edac_pcp_rd(ctx->edac, PCPHPERRINTSTS, &pcp_hp_stat);
+	xgene_edac_pcp_rd(ctx->edac, PCPLPERRINTSTS, &pcp_lp_stat);
+	if (!((MCU_UNCORR_ERR_MASK & pcp_hp_stat) ||
+	      (MCU_CTL_ERR_MASK & pcp_hp_stat) ||
+	      (MCU_CORR_ERR_MASK & pcp_lp_stat)))
+		return;
+
+	for (rank = 0; rank < MCU_MAX_RANK; rank++) {
+		reg = readl(ctx->mcu_csr + MCUESRR0 + rank * MCU_RANK_STRIDE);
+
+		/* Detect uncorrectable memory error */
+		if (reg & (MCU_ESRR_DEMANDUCERR_MASK |
+			   MCU_ESRR_BACKUCERR_MASK)) {
+			/* Detected uncorrectable memory error */
+			edac_mc_chipset_printk(mci, KERN_ERR, "X-Gene",
+				"MCU uncorrectable error at rank %d\n", rank);
+
+			edac_mc_handle_error(HW_EVENT_ERR_UNCORRECTED, mci,
+				1, 0, 0, 0, 0, 0, -1, mci->ctl_name, "");
+		}
+
+		/* Detect correctable memory error */
+		if (reg & MCU_ESRR_CERR_MASK) {
+			bank = readl(ctx->mcu_csr + MCUEBLRR0 +
+				     rank * MCU_RANK_STRIDE);
+			col_row = readl(ctx->mcu_csr + MCUERCRR0 +
+					rank * MCU_RANK_STRIDE);
+			count = readl(ctx->mcu_csr + MCUSBECNT0 +
+				      rank * MCU_RANK_STRIDE);
+			edac_mc_chipset_printk(mci, KERN_WARNING, "X-Gene",
+				"MCU correctable error at rank %d bank %d column %d row %d count %d\n",
+				rank, MCU_EBLRR_ERRBANK_RD(bank),
+				MCU_ERCRR_ERRCOL_RD(col_row),
+				MCU_ERCRR_ERRROW_RD(col_row),
+				MCU_SBECNT_COUNT(count));
+
+			edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci,
+				1, 0, 0, 0, 0, 0, -1, mci->ctl_name, "");
+		}
+
+		/* Clear all error registers */
+		writel(0x0, ctx->mcu_csr + MCUEBLRR0 + rank * MCU_RANK_STRIDE);
+		writel(0x0, ctx->mcu_csr + MCUERCRR0 + rank * MCU_RANK_STRIDE);
+		writel(0x0, ctx->mcu_csr + MCUSBECNT0 +
+		       rank * MCU_RANK_STRIDE);
+		writel(reg, ctx->mcu_csr + MCUESRR0 + rank * MCU_RANK_STRIDE);
+	}
+
+	/* Detect memory controller error */
+	reg = readl(ctx->mcu_csr + MCUGESR);
+	if (reg) {
+		if (reg & MCU_GESR_ADDRNOMATCH_ERR_MASK)
+			edac_mc_chipset_printk(mci, KERN_WARNING, "X-Gene",
+				"MCU address miss-match error\n");
+		if (reg & MCU_GESR_ADDRMULTIMATCH_ERR_MASK)
+			edac_mc_chipset_printk(mci, KERN_WARNING, "X-Gene",
+				"MCU address multi-match error\n");
+
+		writel(reg, ctx->mcu_csr + MCUGESR);
+	}
+}
+
+static void xgene_edac_mc_irq_ctl(struct mem_ctl_info *mci, bool enable)
+{
+	struct xgene_edac_mc_ctx *ctx = mci->pvt_info;
+	unsigned int val;
+
+	if (edac_op_state != EDAC_OPSTATE_INT)
+		return;
+
+	mutex_lock(&ctx->edac->mc_lock);
+
+	/*
+	 * As there is only single bit for enable error and interrupt mask,
+	 * we must only enable top level interrupt after all MCUs are
+	 * registered. Otherwise, if there is an error and the corresponding
+	 * MCU has not registered, the interrupt will never get cleared. To
+	 * determine all MCU have registered, we will keep track of active
+	 * MCUs and registered MCUs.
+	 */
+	if (enable) {
+		/* Set registered MCU bit */
+		ctx->edac->mc_registered_mask |= 1 << ctx->mcu_id;
+
+		/* Enable interrupt after all active MCU registered */
+		if (ctx->edac->mc_registered_mask ==
+		    ctx->edac->mc_active_mask) {
+			/* Enable memory controller top level interrupt */
+			xgene_edac_pcp_clrbits(ctx->edac, PCPHPERRINTMSK,
+					       MCU_UNCORR_ERR_MASK |
+					       MCU_CTL_ERR_MASK);
+			xgene_edac_pcp_clrbits(ctx->edac, PCPLPERRINTMSK,
+					       MCU_CORR_ERR_MASK);
+		}
+
+		/* Enable MCU interrupt and error reporting */
+		val = readl(ctx->mcu_csr + MCUGECR);
+		val |= MCU_GECR_DEMANDUCINTREN_MASK |
+		       MCU_GECR_BACKUCINTREN_MASK |
+		       MCU_GECR_CINTREN_MASK |
+		       MUC_GECR_MCUADDRERREN_MASK;
+		writel(val, ctx->mcu_csr + MCUGECR);
+	} else {
+		/* Disable MCU interrupt */
+		val = readl(ctx->mcu_csr + MCUGECR);
+		val &= ~(MCU_GECR_DEMANDUCINTREN_MASK |
+			 MCU_GECR_BACKUCINTREN_MASK |
+			 MCU_GECR_CINTREN_MASK |
+			 MUC_GECR_MCUADDRERREN_MASK);
+		writel(val, ctx->mcu_csr + MCUGECR);
+
+		/* Disable memory controller top level interrupt */
+		xgene_edac_pcp_setbits(ctx->edac, PCPHPERRINTMSK,
+				       MCU_UNCORR_ERR_MASK | MCU_CTL_ERR_MASK);
+		xgene_edac_pcp_setbits(ctx->edac, PCPLPERRINTMSK,
+				       MCU_CORR_ERR_MASK);
+
+		/* Clear registered MCU bit */
+		ctx->edac->mc_registered_mask &= ~(1 << ctx->mcu_id);
+	}
+
+	mutex_unlock(&ctx->edac->mc_lock);
+}
+
+static int xgene_edac_mc_is_active(struct xgene_edac_mc_ctx *ctx, int mc_idx)
+{
+	unsigned int reg;
+	u32 mcu_mask;
+
+	if (regmap_read(ctx->edac->csw_map, CSW_CSWCR, &reg))
+		return 0;
+
+	if (reg & CSW_CSWCR_DUALMCB_MASK) {
+		/*
+		 * Dual MCB active - Determine if all 4 active or just MCU0
+		 * and MCU2 active
+		 */
+		if (regmap_read(ctx->edac->mcbb_map, MCBADDRMR, &reg))
+			return 0;
+		mcu_mask = (reg & MCBADDRMR_DUALMCU_MODE_MASK) ? 0xF : 0x5;
+	} else {
+		/*
+		 * Single MCB active - Determine if MCU0/MCU1 or just MCU0
+		 * active
+		 */
+		if (regmap_read(ctx->edac->mcba_map, MCBADDRMR, &reg))
+			return 0;
+		mcu_mask = (reg & MCBADDRMR_DUALMCU_MODE_MASK) ? 0x3 : 0x1;
+	}
+
+	/* Save active MC mask if hasn't set already */
+	if (!ctx->edac->mc_active_mask)
+		ctx->edac->mc_active_mask = mcu_mask;
+
+	return (mcu_mask & (1 << mc_idx)) ? 1 : 0;
+}
+
+static int xgene_edac_mc_add(struct xgene_edac *edac, struct device_node *np)
+{
+	struct mem_ctl_info *mci;
+	struct edac_mc_layer layers[2];
+	struct xgene_edac_mc_ctx tmp_ctx;
+	struct xgene_edac_mc_ctx *ctx;
+	struct resource res;
+	int rc;
+
+	memset(&tmp_ctx, 0, sizeof(tmp_ctx));
+	tmp_ctx.edac = edac;
+
+	if (!devres_open_group(edac->dev, xgene_edac_mc_add, GFP_KERNEL))
+		return -ENOMEM;
+
+	rc = of_address_to_resource(np, 0, &res);
+	if (rc < 0) {
+		dev_err(edac->dev, "no MCU resource address\n");
+		goto err_group;
+	}
+	tmp_ctx.mcu_csr = devm_ioremap_resource(edac->dev, &res);
+	if (IS_ERR(tmp_ctx.mcu_csr)) {
+		dev_err(edac->dev, "unable to map MCU resource\n");
+		rc = PTR_ERR(tmp_ctx.mcu_csr);
+		goto err_group;
+	}
+
+	/* Ignore non-active MCU */
+	if (of_property_read_u32(np, "memory-controller", &tmp_ctx.mcu_id)) {
+		dev_err(edac->dev, "no memory-controller property\n");
+		rc = -ENODEV;
+		goto err_group;
+	}
+	if (!xgene_edac_mc_is_active(&tmp_ctx, tmp_ctx.mcu_id)) {
+		rc = -ENODEV;
+		goto err_group;
+	}
+
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = 4;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = 2;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(tmp_ctx.mcu_id, ARRAY_SIZE(layers), layers,
+			    sizeof(*ctx));
+	if (!mci) {
+		rc = -ENOMEM;
+		goto err_group;
+	}
+
+	ctx = mci->pvt_info;
+	*ctx = tmp_ctx;		/* Copy over resource value */
+	ctx->name = "xgene_edac_mc_err";
+	ctx->mci = mci;
+	mci->pdev = &mci->dev;
+	mci->ctl_name = ctx->name;
+	mci->dev_name = ctx->name;
+
+	mci->mtype_cap = MEM_FLAG_RDDR | MEM_FLAG_RDDR2 | MEM_FLAG_RDDR3 |
+			 MEM_FLAG_DDR | MEM_FLAG_DDR2 | MEM_FLAG_DDR3;
+	mci->edac_ctl_cap = EDAC_FLAG_SECDED;
+	mci->edac_cap = EDAC_FLAG_SECDED;
+	mci->mod_name = EDAC_MOD_STR;
+	mci->mod_ver = "0.1";
+	mci->ctl_page_to_phys = NULL;
+	mci->scrub_cap = SCRUB_FLAG_HW_SRC;
+	mci->scrub_mode = SCRUB_HW_SRC;
+
+	if (edac_op_state == EDAC_OPSTATE_POLL)
+		mci->edac_check = xgene_edac_mc_check;
+
+	if (edac_mc_add_mc(mci)) {
+		dev_err(edac->dev, "edac_mc_add_mc failed\n");
+		rc = -EINVAL;
+		goto err_free;
+	}
+
+	xgene_edac_mc_create_debugfs_node(mci);
+
+	list_add(&ctx->next, &edac->mcus);
+
+	xgene_edac_mc_irq_ctl(mci, true);
+
+	devres_remove_group(edac->dev, xgene_edac_mc_add);
+
+	dev_info(edac->dev, "X-Gene EDAC MC registered\n");
+	return 0;
+
+err_free:
+	edac_mc_free(mci);
+err_group:
+	devres_release_group(edac->dev, xgene_edac_mc_add);
+	return rc;
+}
+
+static int xgene_edac_mc_remove(struct xgene_edac_mc_ctx *mcu)
+{
+	xgene_edac_mc_irq_ctl(mcu->mci, false);
+	edac_mc_del_mc(&mcu->mci->dev);
+	edac_mc_free(mcu->mci);
+	return 0;
+}
+
+/* CPU L1/L2 error CSR */
+#define MAX_CPU_PER_PMD				2
+#define CPU_CSR_STRIDE				0x00100000
+#define CPU_L2C_PAGE				0x000D0000
+#define CPU_MEMERR_L2C_PAGE			0x000E0000
+#define CPU_MEMERR_CPU_PAGE			0x000F0000
+
+#define MEMERR_CPU_ICFECR_PAGE_OFFSET		0x0000
+#define MEMERR_CPU_ICFESR_PAGE_OFFSET		0x0004
+#define  MEMERR_CPU_ICFESR_ERRWAY_RD(src)	(((src) & 0xFF000000) >> 24)
+#define  MEMERR_CPU_ICFESR_ERRINDEX_RD(src)	(((src) & 0x003F0000) >> 16)
+#define  MEMERR_CPU_ICFESR_ERRINFO_RD(src)	(((src) & 0x0000FF00) >> 8)
+#define  MEMERR_CPU_ICFESR_ERRTYPE_RD(src)	(((src) & 0x00000070) >> 4)
+#define  MEMERR_CPU_ICFESR_MULTCERR_MASK	BIT(2)
+#define  MEMERR_CPU_ICFESR_CERR_MASK		BIT(0)
+#define MEMERR_CPU_LSUESR_PAGE_OFFSET		0x000c
+#define  MEMERR_CPU_LSUESR_ERRWAY_RD(src)	(((src) & 0xFF000000) >> 24)
+#define  MEMERR_CPU_LSUESR_ERRINDEX_RD(src)	(((src) & 0x003F0000) >> 16)
+#define  MEMERR_CPU_LSUESR_ERRINFO_RD(src)	(((src) & 0x0000FF00) >> 8)
+#define  MEMERR_CPU_LSUESR_ERRTYPE_RD(src)	(((src) & 0x00000070) >> 4)
+#define  MEMERR_CPU_LSUESR_MULTCERR_MASK	BIT(2)
+#define  MEMERR_CPU_LSUESR_CERR_MASK		BIT(0)
+#define MEMERR_CPU_LSUECR_PAGE_OFFSET		0x0008
+#define MEMERR_CPU_MMUECR_PAGE_OFFSET		0x0010
+#define MEMERR_CPU_MMUESR_PAGE_OFFSET		0x0014
+#define  MEMERR_CPU_MMUESR_ERRWAY_RD(src)	(((src) & 0xFF000000) >> 24)
+#define  MEMERR_CPU_MMUESR_ERRINDEX_RD(src)	(((src) & 0x007F0000) >> 16)
+#define  MEMERR_CPU_MMUESR_ERRINFO_RD(src)	(((src) & 0x0000FF00) >> 8)
+#define  MEMERR_CPU_MMUESR_ERRREQSTR_LSU_MASK	BIT(7)
+#define  MEMERR_CPU_MMUESR_ERRTYPE_RD(src)	(((src) & 0x00000070) >> 4)
+#define  MEMERR_CPU_MMUESR_MULTCERR_MASK	BIT(2)
+#define  MEMERR_CPU_MMUESR_CERR_MASK		BIT(0)
+#define MEMERR_CPU_ICFESRA_PAGE_OFFSET		0x0804
+#define MEMERR_CPU_LSUESRA_PAGE_OFFSET		0x080c
+#define MEMERR_CPU_MMUESRA_PAGE_OFFSET		0x0814
+
+#define MEMERR_L2C_L2ECR_PAGE_OFFSET		0x0000
+#define MEMERR_L2C_L2ESR_PAGE_OFFSET		0x0004
+#define  MEMERR_L2C_L2ESR_ERRSYN_RD(src)	(((src) & 0xFF000000) >> 24)
+#define  MEMERR_L2C_L2ESR_ERRWAY_RD(src)	(((src) & 0x00FC0000) >> 18)
+#define  MEMERR_L2C_L2ESR_ERRCPU_RD(src)	(((src) & 0x00020000) >> 17)
+#define  MEMERR_L2C_L2ESR_ERRGROUP_RD(src)	(((src) & 0x0000E000) >> 13)
+#define  MEMERR_L2C_L2ESR_ERRACTION_RD(src)	(((src) & 0x00001C00) >> 10)
+#define  MEMERR_L2C_L2ESR_ERRTYPE_RD(src)	(((src) & 0x00000300) >> 8)
+#define  MEMERR_L2C_L2ESR_MULTUCERR_MASK	BIT(3)
+#define  MEMERR_L2C_L2ESR_MULTICERR_MASK	BIT(2)
+#define  MEMERR_L2C_L2ESR_UCERR_MASK		BIT(1)
+#define  MEMERR_L2C_L2ESR_ERR_MASK		BIT(0)
+#define MEMERR_L2C_L2EALR_PAGE_OFFSET		0x0008
+#define CPUX_L2C_L2RTOCR_PAGE_OFFSET		0x0010
+#define MEMERR_L2C_L2EAHR_PAGE_OFFSET		0x000c
+#define CPUX_L2C_L2RTOSR_PAGE_OFFSET		0x0014
+#define  MEMERR_L2C_L2RTOSR_MULTERR_MASK	BIT(1)
+#define  MEMERR_L2C_L2RTOSR_ERR_MASK		BIT(0)
+#define CPUX_L2C_L2RTOALR_PAGE_OFFSET		0x0018
+#define CPUX_L2C_L2RTOAHR_PAGE_OFFSET		0x001c
+#define MEMERR_L2C_L2ESRA_PAGE_OFFSET		0x0804
+
+/*
+ * Processor Module Domain (PMD) context - Context for a pair of processsors.
+ * Each PMD consists of 2 CPUs and a shared L2 cache. Each CPU consists of
+ * its own L1 cache.
+ */
+struct xgene_edac_pmd_ctx {
+	struct list_head	next;
+	struct device		ddev;
+	char			*name;
+	struct xgene_edac	*edac;
+	struct edac_device_ctl_info *edac_dev;
+	void __iomem		*pmd_csr;
+	u32			pmd;
+	int			version;
+};
+
+static void xgene_edac_pmd_l1_check(struct edac_device_ctl_info *edac_dev,
+				    int cpu_idx)
+{
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	void __iomem *pg_f;
+	u32 val;
+
+	pg_f = ctx->pmd_csr + cpu_idx * CPU_CSR_STRIDE + CPU_MEMERR_CPU_PAGE;
+
+	val = readl(pg_f + MEMERR_CPU_ICFESR_PAGE_OFFSET);
+	if (val) {
+		dev_err(edac_dev->dev,
+			"CPU%d L1 memory error ICF 0x%08X Way 0x%02X Index 0x%02X Info 0x%02X\n",
+			ctx->pmd * MAX_CPU_PER_PMD + cpu_idx, val,
+			MEMERR_CPU_ICFESR_ERRWAY_RD(val),
+			MEMERR_CPU_ICFESR_ERRINDEX_RD(val),
+			MEMERR_CPU_ICFESR_ERRINFO_RD(val));
+		if (val & MEMERR_CPU_ICFESR_CERR_MASK)
+			dev_err(edac_dev->dev,
+				"One or more correctable error\n");
+		if (val & MEMERR_CPU_ICFESR_MULTCERR_MASK)
+			dev_err(edac_dev->dev, "Multiple correctable error\n");
+		switch (MEMERR_CPU_ICFESR_ERRTYPE_RD(val)) {
+		case 1:
+			dev_err(edac_dev->dev, "L1 TLB multiple hit\n");
+			break;
+		case 2:
+			dev_err(edac_dev->dev, "Way select multiple hit\n");
+			break;
+		case 3:
+			dev_err(edac_dev->dev, "Physical tag parity error\n");
+			break;
+		case 4:
+		case 5:
+			dev_err(edac_dev->dev, "L1 data parity error\n");
+			break;
+		case 6:
+			dev_err(edac_dev->dev, "L1 pre-decode parity error\n");
+			break;
+		}
+
+		/* Clear any HW errors */
+		writel(val, pg_f + MEMERR_CPU_ICFESR_PAGE_OFFSET);
+
+		if (val & (MEMERR_CPU_ICFESR_CERR_MASK |
+			   MEMERR_CPU_ICFESR_MULTCERR_MASK))
+			edac_device_handle_ce(edac_dev, 0, 0,
+					      edac_dev->ctl_name);
+	}
+
+	val = readl(pg_f + MEMERR_CPU_LSUESR_PAGE_OFFSET);
+	if (val) {
+		dev_err(edac_dev->dev,
+			"CPU%d memory error LSU 0x%08X Way 0x%02X Index 0x%02X Info 0x%02X\n",
+			ctx->pmd * MAX_CPU_PER_PMD + cpu_idx, val,
+			MEMERR_CPU_LSUESR_ERRWAY_RD(val),
+			MEMERR_CPU_LSUESR_ERRINDEX_RD(val),
+			MEMERR_CPU_LSUESR_ERRINFO_RD(val));
+		if (val & MEMERR_CPU_LSUESR_CERR_MASK)
+			dev_err(edac_dev->dev,
+				"One or more correctable error\n");
+		if (val & MEMERR_CPU_LSUESR_MULTCERR_MASK)
+			dev_err(edac_dev->dev, "Multiple correctable error\n");
+		switch (MEMERR_CPU_LSUESR_ERRTYPE_RD(val)) {
+		case 0:
+			dev_err(edac_dev->dev, "Load tag error\n");
+			break;
+		case 1:
+			dev_err(edac_dev->dev, "Load data error\n");
+			break;
+		case 2:
+			dev_err(edac_dev->dev, "WSL multihit error\n");
+			break;
+		case 3:
+			dev_err(edac_dev->dev, "Store tag error\n");
+			break;
+		case 4:
+			dev_err(edac_dev->dev,
+				"DTB multihit from load pipeline error\n");
+			break;
+		case 5:
+			dev_err(edac_dev->dev,
+				"DTB multihit from store pipeline error\n");
+			break;
+		}
+
+		/* Clear any HW errors */
+		writel(val, pg_f + MEMERR_CPU_LSUESR_PAGE_OFFSET);
+
+		if (val & (MEMERR_CPU_LSUESR_CERR_MASK |
+			   MEMERR_CPU_LSUESR_MULTCERR_MASK))
+			edac_device_handle_ce(edac_dev, 0, 0,
+					      edac_dev->ctl_name);
+	}
+
+	val = readl(pg_f + MEMERR_CPU_MMUESR_PAGE_OFFSET);
+	if (val) {
+		dev_err(edac_dev->dev,
+			"CPU%d memory error MMU 0x%08X Way 0x%02X Index 0x%02X Info 0x%02X %s\n",
+			ctx->pmd * MAX_CPU_PER_PMD + cpu_idx, val,
+			MEMERR_CPU_MMUESR_ERRWAY_RD(val),
+			MEMERR_CPU_MMUESR_ERRINDEX_RD(val),
+			MEMERR_CPU_MMUESR_ERRINFO_RD(val),
+			val & MEMERR_CPU_MMUESR_ERRREQSTR_LSU_MASK ? "LSU" :
+								     "ICF");
+		if (val & MEMERR_CPU_MMUESR_CERR_MASK)
+			dev_err(edac_dev->dev,
+				"One or more correctable error\n");
+		if (val & MEMERR_CPU_MMUESR_MULTCERR_MASK)
+			dev_err(edac_dev->dev, "Multiple correctable error\n");
+		switch (MEMERR_CPU_MMUESR_ERRTYPE_RD(val)) {
+		case 0:
+			dev_err(edac_dev->dev, "Stage 1 UTB hit error\n");
+			break;
+		case 1:
+			dev_err(edac_dev->dev, "Stage 1 UTB miss error\n");
+			break;
+		case 2:
+			dev_err(edac_dev->dev, "Stage 1 UTB allocate error\n");
+			break;
+		case 3:
+			dev_err(edac_dev->dev,
+				"TMO operation single bank error\n");
+			break;
+		case 4:
+			dev_err(edac_dev->dev, "Stage 2 UTB error\n");
+			break;
+		case 5:
+			dev_err(edac_dev->dev, "Stage 2 UTB miss error\n");
+			break;
+		case 6:
+			dev_err(edac_dev->dev, "Stage 2 UTB allocate error\n");
+			break;
+		case 7:
+			dev_err(edac_dev->dev,
+				"TMO operation multiple bank error\n");
+			break;
+		}
+
+		/* Clear any HW errors */
+		writel(val, pg_f + MEMERR_CPU_MMUESR_PAGE_OFFSET);
+
+		edac_device_handle_ce(edac_dev, 0, 0, edac_dev->ctl_name);
+	}
+}
+
+static void xgene_edac_pmd_l2_check(struct edac_device_ctl_info *edac_dev)
+{
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	void __iomem *pg_d;
+	void __iomem *pg_e;
+	u32 val_hi;
+	u32 val_lo;
+	u32 val;
+
+	/* Check L2 */
+	pg_e = ctx->pmd_csr + CPU_MEMERR_L2C_PAGE;
+	val = readl(pg_e + MEMERR_L2C_L2ESR_PAGE_OFFSET);
+	if (val) {
+		val_lo = readl(pg_e + MEMERR_L2C_L2EALR_PAGE_OFFSET);
+		val_hi = readl(pg_e + MEMERR_L2C_L2EAHR_PAGE_OFFSET);
+		dev_err(edac_dev->dev,
+			"PMD%d memory error L2C L2ESR 0x%08X @ 0x%08X.%08X\n",
+			ctx->pmd, val, val_hi, val_lo);
+		dev_err(edac_dev->dev,
+			"ErrSyndrome 0x%02X ErrWay 0x%02X ErrCpu %d ErrGroup 0x%02X ErrAction 0x%02X\n",
+			MEMERR_L2C_L2ESR_ERRSYN_RD(val),
+			MEMERR_L2C_L2ESR_ERRWAY_RD(val),
+			MEMERR_L2C_L2ESR_ERRCPU_RD(val),
+			MEMERR_L2C_L2ESR_ERRGROUP_RD(val),
+			MEMERR_L2C_L2ESR_ERRACTION_RD(val));
+
+		if (val & MEMERR_L2C_L2ESR_ERR_MASK)
+			dev_err(edac_dev->dev,
+				"One or more correctable error\n");
+		if (val & MEMERR_L2C_L2ESR_MULTICERR_MASK)
+			dev_err(edac_dev->dev, "Multiple correctable error\n");
+		if (val & MEMERR_L2C_L2ESR_UCERR_MASK)
+			dev_err(edac_dev->dev,
+				"One or more uncorrectable error\n");
+		if (val & MEMERR_L2C_L2ESR_MULTUCERR_MASK)
+			dev_err(edac_dev->dev,
+				"Multiple uncorrectable error\n");
+
+		switch (MEMERR_L2C_L2ESR_ERRTYPE_RD(val)) {
+		case 0:
+			dev_err(edac_dev->dev, "Outbound SDB parity error\n");
+			break;
+		case 1:
+			dev_err(edac_dev->dev, "Inbound SDB parity error\n");
+			break;
+		case 2:
+			dev_err(edac_dev->dev, "Tag ECC error\n");
+			break;
+		case 3:
+			dev_err(edac_dev->dev, "Data ECC error\n");
+			break;
+		}
+
+		/* Clear any HW errors */
+		writel(val, pg_e + MEMERR_L2C_L2ESR_PAGE_OFFSET);
+
+		if (val & (MEMERR_L2C_L2ESR_ERR_MASK |
+			   MEMERR_L2C_L2ESR_MULTICERR_MASK))
+			edac_device_handle_ce(edac_dev, 0, 0,
+					      edac_dev->ctl_name);
+		if (val & (MEMERR_L2C_L2ESR_UCERR_MASK |
+			   MEMERR_L2C_L2ESR_MULTUCERR_MASK))
+			edac_device_handle_ue(edac_dev, 0, 0,
+					      edac_dev->ctl_name);
+	}
+
+	/* Check if any memory request timed out on L2 cache */
+	pg_d = ctx->pmd_csr + CPU_L2C_PAGE;
+	val = readl(pg_d + CPUX_L2C_L2RTOSR_PAGE_OFFSET);
+	if (val) {
+		val_lo = readl(pg_d + CPUX_L2C_L2RTOALR_PAGE_OFFSET);
+		val_hi = readl(pg_d + CPUX_L2C_L2RTOAHR_PAGE_OFFSET);
+		dev_err(edac_dev->dev,
+			"PMD%d L2C error L2C RTOSR 0x%08X @ 0x%08X.%08X\n",
+			ctx->pmd, val, val_hi, val_lo);
+		writel(val, pg_d + CPUX_L2C_L2RTOSR_PAGE_OFFSET);
+	}
+}
+
+static void xgene_edac_pmd_check(struct edac_device_ctl_info *edac_dev)
+{
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	unsigned int pcp_hp_stat;
+	int i;
+
+	xgene_edac_pcp_rd(ctx->edac, PCPHPERRINTSTS, &pcp_hp_stat);
+	if (!((PMD0_MERR_MASK << ctx->pmd) & pcp_hp_stat))
+		return;
+
+	/* Check CPU L1 error */
+	for (i = 0; i < MAX_CPU_PER_PMD; i++)
+		xgene_edac_pmd_l1_check(edac_dev, i);
+
+	/* Check CPU L2 error */
+	xgene_edac_pmd_l2_check(edac_dev);
+}
+
+static void xgene_edac_pmd_cpu_hw_cfg(struct edac_device_ctl_info *edac_dev,
+				      int cpu)
+{
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	void __iomem *pg_f = ctx->pmd_csr + cpu * CPU_CSR_STRIDE +
+			     CPU_MEMERR_CPU_PAGE;
+
+	/*
+	 * Enable CPU memory error:
+	 *  MEMERR_CPU_ICFESRA, MEMERR_CPU_LSUESRA, and MEMERR_CPU_MMUESRA
+	 */
+	writel(0x00000301, pg_f + MEMERR_CPU_ICFECR_PAGE_OFFSET);
+	writel(0x00000301, pg_f + MEMERR_CPU_LSUECR_PAGE_OFFSET);
+	writel(0x00000101, pg_f + MEMERR_CPU_MMUECR_PAGE_OFFSET);
+}
+
+static void xgene_edac_pmd_hw_cfg(struct edac_device_ctl_info *edac_dev)
+{
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	void __iomem *pg_d = ctx->pmd_csr + CPU_L2C_PAGE;
+	void __iomem *pg_e = ctx->pmd_csr + CPU_MEMERR_L2C_PAGE;
+
+	/* Enable PMD memory error - MEMERR_L2C_L2ECR and L2C_L2RTOCR */
+	writel(0x00000703, pg_e + MEMERR_L2C_L2ECR_PAGE_OFFSET);
+	/* Configure L2C HW request time out feature if supported */
+	if (ctx->version > 1)
+		writel(0x00000119, pg_d + CPUX_L2C_L2RTOCR_PAGE_OFFSET);
+}
+
+static void xgene_edac_pmd_hw_ctl(struct edac_device_ctl_info *edac_dev,
+				  bool enable)
+{
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	int i;
+
+	/* Enable PMD error interrupt */
+	if (edac_dev->op_state == OP_RUNNING_INTERRUPT) {
+		if (enable)
+			xgene_edac_pcp_clrbits(ctx->edac, PCPHPERRINTMSK,
+					       PMD0_MERR_MASK << ctx->pmd);
+		else
+			xgene_edac_pcp_setbits(ctx->edac, PCPHPERRINTMSK,
+					       PMD0_MERR_MASK << ctx->pmd);
+	}
+
+	if (enable) {
+		xgene_edac_pmd_hw_cfg(edac_dev);
+
+		/* Two CPUs per a PMD */
+		for (i = 0; i < MAX_CPU_PER_PMD; i++)
+			xgene_edac_pmd_cpu_hw_cfg(edac_dev, i);
+	}
+}
+
+static ssize_t xgene_edac_pmd_l1_inject_ctrl_write(struct file *file,
+						   const char __user *data,
+						   size_t count, loff_t *ppos)
+{
+	struct edac_device_ctl_info *edac_dev = file->private_data;
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	void __iomem *cpux_pg_f;
+	int i;
+
+	for (i = 0; i < MAX_CPU_PER_PMD; i++) {
+		cpux_pg_f = ctx->pmd_csr + i * CPU_CSR_STRIDE +
+			    CPU_MEMERR_CPU_PAGE;
+
+		writel(MEMERR_CPU_ICFESR_MULTCERR_MASK |
+		       MEMERR_CPU_ICFESR_CERR_MASK,
+		       cpux_pg_f + MEMERR_CPU_ICFESRA_PAGE_OFFSET);
+		writel(MEMERR_CPU_LSUESR_MULTCERR_MASK |
+		       MEMERR_CPU_LSUESR_CERR_MASK,
+		       cpux_pg_f + MEMERR_CPU_LSUESRA_PAGE_OFFSET);
+		writel(MEMERR_CPU_MMUESR_MULTCERR_MASK |
+		       MEMERR_CPU_MMUESR_CERR_MASK,
+		       cpux_pg_f + MEMERR_CPU_MMUESRA_PAGE_OFFSET);
+	}
+	return count;
+}
+
+static ssize_t xgene_edac_pmd_l2_inject_ctrl_write(struct file *file,
+						   const char __user *data,
+						   size_t count, loff_t *ppos)
+{
+	struct edac_device_ctl_info *edac_dev = file->private_data;
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	void __iomem *pg_e = ctx->pmd_csr + CPU_MEMERR_L2C_PAGE;
+
+	writel(MEMERR_L2C_L2ESR_MULTUCERR_MASK |
+	       MEMERR_L2C_L2ESR_MULTICERR_MASK |
+	       MEMERR_L2C_L2ESR_UCERR_MASK |
+	       MEMERR_L2C_L2ESR_ERR_MASK,
+	       pg_e + MEMERR_L2C_L2ESRA_PAGE_OFFSET);
+	return count;
+}
+
+static const struct file_operations xgene_edac_pmd_debug_inject_fops[] = {
+	{
+	.open = simple_open,
+	.write = xgene_edac_pmd_l1_inject_ctrl_write,
+	.llseek = generic_file_llseek, },
+	{
+	.open = simple_open,
+	.write = xgene_edac_pmd_l2_inject_ctrl_write,
+	.llseek = generic_file_llseek, },
+	{ }
+};
+
+static void xgene_edac_pmd_create_debugfs_nodes(
+	struct edac_device_ctl_info *edac_dev)
+{
+	struct xgene_edac_pmd_ctx *ctx = edac_dev->pvt_info;
+	struct dentry *edac_debugfs;
+	char name[30];
+
+	if (!IS_ENABLED(CONFIG_EDAC_DEBUG))
+		return;
+
+	/*
+	 * Todo: Switch to common EDAC debug file system for edac device
+	 *       when available.
+	 */
+	if (!ctx->edac->dfs) {
+		ctx->edac->dfs = debugfs_create_dir(edac_dev->dev->kobj.name,
+						    NULL);
+		if (!ctx->edac->dfs)
+			return;
+	}
+	sprintf(name, "PMD%d", ctx->pmd);
+	edac_debugfs = debugfs_create_dir(name, ctx->edac->dfs);
+	if (!edac_debugfs)
+		return;
+
+	debugfs_create_file("l1_inject_ctrl", S_IWUSR, edac_debugfs, edac_dev,
+			    &xgene_edac_pmd_debug_inject_fops[0]);
+	debugfs_create_file("l2_inject_ctrl", S_IWUSR, edac_debugfs, edac_dev,
+			    &xgene_edac_pmd_debug_inject_fops[1]);
+}
+
+static int xgene_edac_pmd_available(u32 efuse, int pmd)
+{
+	return (efuse & (1 << pmd)) ? 0 : 1;
+}
+
+static int xgene_edac_pmd_add(struct xgene_edac *edac, struct device_node *np,
+			      int version)
+{
+	struct edac_device_ctl_info *edac_dev;
+	struct xgene_edac_pmd_ctx *ctx;
+	struct resource res;
+	char edac_name[10];
+	u32 pmd;
+	int rc;
+	u32 val;
+
+	if (!devres_open_group(edac->dev, xgene_edac_pmd_add, GFP_KERNEL))
+		return -ENOMEM;
+
+	/* Determine if this PMD is disabled */
+	if (of_property_read_u32(np, "pmd-controller", &pmd)) {
+		dev_err(edac->dev, "no pmd-controller property\n");
+		rc = -ENODEV;
+		goto err_group;
+	}
+	rc = regmap_read(edac->efuse_map, 0, &val);
+	if (rc)
+		goto err_group;
+	if (!xgene_edac_pmd_available(val, pmd)) {
+		rc = -ENODEV;
+		goto err_group;
+	}
+
+	sprintf(edac_name, "l2c%d", pmd);
+	edac_dev = edac_device_alloc_ctl_info(sizeof(*ctx),
+					      edac_name, 1, "l2c", 1, 2, NULL,
+					      0, edac_device_alloc_index());
+	if (!edac_dev) {
+		rc = -ENOMEM;
+		goto err_group;
+	}
+
+	ctx = edac_dev->pvt_info;
+	ctx->name = "xgene_pmd_err";
+	ctx->pmd = pmd;
+	ctx->edac = edac;
+	ctx->edac_dev = edac_dev;
+	ctx->ddev = *edac->dev;
+	ctx->version = version;
+	edac_dev->dev = &ctx->ddev;
+	edac_dev->ctl_name = ctx->name;
+	edac_dev->dev_name = ctx->name;
+	edac_dev->mod_name = EDAC_MOD_STR;
+
+	rc = of_address_to_resource(np, 0, &res);
+	if (rc < 0) {
+		dev_err(edac->dev, "no PMD resource address\n");
+		goto err_free;
+	}
+	ctx->pmd_csr = devm_ioremap_resource(edac->dev, &res);
+	if (IS_ERR(ctx->pmd_csr)) {
+		dev_err(edac->dev,
+			"devm_ioremap_resource failed for PMD resource address\n");
+		rc = PTR_ERR(ctx->pmd_csr);
+		goto err_free;
+	}
+
+	if (edac_op_state == EDAC_OPSTATE_POLL)
+		edac_dev->edac_check = xgene_edac_pmd_check;
+
+	xgene_edac_pmd_create_debugfs_nodes(edac_dev);
+
+	rc = edac_device_add_device(edac_dev);
+	if (rc > 0) {
+		dev_err(edac->dev, "edac_device_add_device failed\n");
+		rc = -ENOMEM;
+		goto err_free;
+	}
+
+	if (edac_op_state == EDAC_OPSTATE_INT)
+		edac_dev->op_state = OP_RUNNING_INTERRUPT;
+
+	list_add(&ctx->next, &edac->pmds);
+
+	xgene_edac_pmd_hw_ctl(edac_dev, 1);
+
+	devres_remove_group(edac->dev, xgene_edac_pmd_add);
+
+	dev_info(edac->dev, "X-Gene EDAC PMD%d registered\n", ctx->pmd);
+	return 0;
+
+err_free:
+	edac_device_free_ctl_info(edac_dev);
+err_group:
+	devres_release_group(edac->dev, xgene_edac_pmd_add);
+	return rc;
+}
+
+static int xgene_edac_pmd_remove(struct xgene_edac_pmd_ctx *pmd)
+{
+	struct edac_device_ctl_info *edac_dev = pmd->edac_dev;
+
+	xgene_edac_pmd_hw_ctl(edac_dev, 0);
+	edac_device_del_device(edac_dev->dev);
+	edac_device_free_ctl_info(edac_dev);
+	return 0;
+}
+
+static irqreturn_t xgene_edac_isr(int irq, void *dev_id)
+{
+	struct xgene_edac *ctx = dev_id;
+	struct xgene_edac_pmd_ctx *pmd;
+	unsigned int pcp_hp_stat;
+	unsigned int pcp_lp_stat;
+
+	xgene_edac_pcp_rd(ctx, PCPHPERRINTSTS, &pcp_hp_stat);
+	xgene_edac_pcp_rd(ctx, PCPLPERRINTSTS, &pcp_lp_stat);
+	if ((MCU_UNCORR_ERR_MASK & pcp_hp_stat) ||
+	    (MCU_CTL_ERR_MASK & pcp_hp_stat) ||
+	    (MCU_CORR_ERR_MASK & pcp_lp_stat)) {
+		struct xgene_edac_mc_ctx *mcu;
+
+		list_for_each_entry(mcu, &ctx->mcus, next) {
+			xgene_edac_mc_check(mcu->mci);
+		}
+	}
+
+	list_for_each_entry(pmd, &ctx->pmds, next) {
+		if ((PMD0_MERR_MASK << pmd->pmd) & pcp_hp_stat)
+			xgene_edac_pmd_check(pmd->edac_dev);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int xgene_edac_probe(struct platform_device *pdev)
+{
+	struct xgene_edac *edac;
+	struct device_node *child;
+	struct resource *res;
+	int rc;
+
+	edac = devm_kzalloc(&pdev->dev, sizeof(*edac), GFP_KERNEL);
+	if (!edac)
+		return -ENOMEM;
+
+	edac->dev = &pdev->dev;
+	platform_set_drvdata(pdev, edac);
+	INIT_LIST_HEAD(&edac->mcus);
+	INIT_LIST_HEAD(&edac->pmds);
+	spin_lock_init(&edac->lock);
+	mutex_init(&edac->mc_lock);
+
+	edac->csw_map = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
+							"regmap-csw");
+	if (IS_ERR(edac->csw_map)) {
+		dev_err(edac->dev, "unable to get syscon regmap csw\n");
+		rc = PTR_ERR(edac->csw_map);
+		goto out_err;
+	}
+
+	edac->mcba_map = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
+							 "regmap-mcba");
+	if (IS_ERR(edac->mcba_map)) {
+		dev_err(edac->dev, "unable to get syscon regmap mcba\n");
+		rc = PTR_ERR(edac->mcba_map);
+		goto out_err;
+	}
+
+	edac->mcbb_map = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
+							 "regmap-mcbb");
+	if (IS_ERR(edac->mcbb_map)) {
+		dev_err(edac->dev, "unable to get syscon regmap mcbb\n");
+		rc = PTR_ERR(edac->mcbb_map);
+		goto out_err;
+	}
+	edac->efuse_map = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
+							  "regmap-efuse");
+	if (IS_ERR(edac->efuse_map)) {
+		dev_err(edac->dev, "unable to get syscon regmap efuse\n");
+		rc = PTR_ERR(edac->efuse_map);
+		goto out_err;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	edac->pcp_csr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(edac->pcp_csr)) {
+		dev_err(&pdev->dev, "no PCP resource address\n");
+		rc = PTR_ERR(edac->pcp_csr);
+		goto out_err;
+	}
+
+	if (edac_op_state == EDAC_OPSTATE_INT) {
+		int irq;
+		int i;
+
+		for (i = 0; i < 3; i++) {
+			irq = platform_get_irq(pdev, i);
+			if (irq < 0) {
+				dev_err(&pdev->dev, "No IRQ resource\n");
+				rc = -EINVAL;
+				goto out_err;
+			}
+			rc = devm_request_irq(&pdev->dev, irq,
+					      xgene_edac_isr, IRQF_SHARED,
+					      dev_name(&pdev->dev), edac);
+			if (rc) {
+				dev_err(&pdev->dev,
+					"Could not request IRQ %d\n", irq);
+				goto out_err;
+			}
+		}
+	}
+
+	for_each_child_of_node(pdev->dev.of_node, child) {
+		if (!of_device_is_available(child))
+			continue;
+		if (of_device_is_compatible(child, "apm,xgene-edac-mc"))
+			xgene_edac_mc_add(edac, child);
+		if (of_device_is_compatible(child, "apm,xgene-edac-pmd"))
+			xgene_edac_pmd_add(edac, child, 1);
+		if (of_device_is_compatible(child, "apm,xgene-edac-pmd-v2"))
+			xgene_edac_pmd_add(edac, child, 2);
+	}
+
+	return 0;
+
+out_err:
+	return rc;
+}
+
+static int xgene_edac_remove(struct platform_device *pdev)
+{
+	struct xgene_edac *edac = dev_get_drvdata(&pdev->dev);
+	struct xgene_edac_mc_ctx *mcu;
+	struct xgene_edac_mc_ctx *temp_mcu;
+	struct xgene_edac_pmd_ctx *pmd;
+	struct xgene_edac_pmd_ctx *temp_pmd;
+
+	list_for_each_entry_safe(mcu, temp_mcu, &edac->mcus, next) {
+		xgene_edac_mc_remove(mcu);
+	}
+
+	list_for_each_entry_safe(pmd, temp_pmd, &edac->pmds, next) {
+		xgene_edac_pmd_remove(pmd);
+	}
+	return 0;
+}
+
+static const struct of_device_id xgene_edac_of_match[] = {
+	{ .compatible = "apm,xgene-edac" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, xgene_edac_of_match);
+
+static struct platform_driver xgene_edac_driver = {
+	.probe = xgene_edac_probe,
+	.remove = xgene_edac_remove,
+	.driver = {
+		.name = "xgene-edac",
+		.owner = THIS_MODULE,
+		.of_match_table = xgene_edac_of_match,
+	},
+};
+
+static int __init xgene_edac_init(void)
+{
+	int rc;
+
+	/* Make sure error reporting method is sane */
+	switch (edac_op_state) {
+	case EDAC_OPSTATE_POLL:
+	case EDAC_OPSTATE_INT:
+		break;
+	default:
+		edac_op_state = EDAC_OPSTATE_INT;
+		break;
+	}
+
+	rc = platform_driver_register(&xgene_edac_driver);
+	if (rc) {
+		edac_printk(KERN_ERR, EDAC_MOD_STR,
+			    "EDAC fails to register\n");
+		goto reg_failed;
+	}
+
+	return 0;
+
+reg_failed:
+	return rc;
+}
+module_init(xgene_edac_init);
+
+static void __exit xgene_edac_exit(void)
+{
+	platform_driver_unregister(&xgene_edac_driver);
+}
+module_exit(xgene_edac_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Feng Kan <fkan@apm.com>");
+MODULE_DESCRIPTION("APM X-Gene EDAC driver");
+module_param(edac_op_state, int, 0444);
+MODULE_PARM_DESC(edac_op_state,
+		 "EDAC error reporting state: 0=Poll, 2=Interrupt");
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index fdc0bf0543ce..0cebbf668886 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -28,15 +28,22 @@ config EXTCON_ARIZONA
 	  with Wolfson Arizona devices. These are audio CODECs with
 	  advanced audio accessory detection support.
 
+config EXTCON_AXP288
+	tristate "X-Power AXP288 EXTCON support"
+	depends on MFD_AXP20X && USB_PHY
+	help
+	  Say Y here to enable support for USB peripheral detection
+	  and USB MUX switching by X-Power AXP288 PMIC.
+
 config EXTCON_GPIO
 	tristate "GPIO extcon support"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Say Y here to enable GPIO based extcon support. Note that GPIO
 	  extcon supports single state per extcon instance.
 
 config EXTCON_MAX14577
-	tristate "MAX14577/77836 EXTCON Support"
+	tristate "Maxim MAX14577/77836 EXTCON Support"
 	depends on MFD_MAX14577
 	select IRQ_DOMAIN
 	select REGMAP_I2C
@@ -46,7 +53,7 @@ config EXTCON_MAX14577
 	  detector and switch.
 
 config EXTCON_MAX77693
-	tristate "MAX77693 EXTCON Support"
+	tristate "Maxim MAX77693 EXTCON Support"
 	depends on MFD_MAX77693 && INPUT
 	select IRQ_DOMAIN
 	select REGMAP_I2C
@@ -56,7 +63,7 @@ config EXTCON_MAX77693
 	  detector and switch.
 
 config EXTCON_MAX77843
-	tristate "MAX77843 EXTCON Support"
+	tristate "Maxim MAX77843 EXTCON Support"
 	depends on MFD_MAX77843
 	select IRQ_DOMAIN
 	select REGMAP_I2C
@@ -66,7 +73,7 @@ config EXTCON_MAX77843
 	  detector add switch.
 
 config EXTCON_MAX8997
-	tristate "MAX8997 EXTCON Support"
+	tristate "Maxim MAX8997 EXTCON Support"
 	depends on MFD_MAX8997 && IRQ_DOMAIN
 	help
 	  If you say yes here you get support for the MUIC device of
@@ -81,7 +88,7 @@ config EXTCON_PALMAS
 	  detection by palmas usb.
 
 config EXTCON_RT8973A
-	tristate "RT8973A EXTCON support"
+	tristate "Richtek RT8973A EXTCON support"
 	depends on I2C
 	select IRQ_DOMAIN
 	select REGMAP_I2C
@@ -93,7 +100,7 @@ config EXTCON_RT8973A
 	  from abnormal high input voltage (up to 28V).
 
 config EXTCON_SM5502
-	tristate "SM5502 EXTCON support"
+	tristate "Silicon Mitus SM5502 EXTCON support"
 	depends on I2C
 	select IRQ_DOMAIN
 	select REGMAP_I2C
@@ -105,9 +112,9 @@ config EXTCON_SM5502
 
 config EXTCON_USB_GPIO
 	tristate "USB GPIO extcon support"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Say Y here to enable GPIO based USB cable detection extcon support.
 	  Used typically if GPIO is used for USB ID pin detection.
 
-endif # MULTISTATE_SWITCH
+endif
diff --git a/drivers/extcon/Makefile b/drivers/extcon/Makefile
index 9204114791a3..ba787d04295b 100644
--- a/drivers/extcon/Makefile
+++ b/drivers/extcon/Makefile
@@ -5,6 +5,7 @@
 obj-$(CONFIG_EXTCON)		+= extcon.o
 obj-$(CONFIG_EXTCON_ADC_JACK)	+= extcon-adc-jack.o
 obj-$(CONFIG_EXTCON_ARIZONA)	+= extcon-arizona.o
+obj-$(CONFIG_EXTCON_AXP288)	+= extcon-axp288.o
 obj-$(CONFIG_EXTCON_GPIO)	+= extcon-gpio.o
 obj-$(CONFIG_EXTCON_MAX14577)	+= extcon-max14577.o
 obj-$(CONFIG_EXTCON_MAX77693)	+= extcon-max77693.o
diff --git a/drivers/extcon/extcon-adc-jack.c b/drivers/extcon/extcon-adc-jack.c
index 2bb82e55065a..7fc0ae1912f8 100644
--- a/drivers/extcon/extcon-adc-jack.c
+++ b/drivers/extcon/extcon-adc-jack.c
@@ -29,7 +29,6 @@
  * struct adc_jack_data - internal data for adc_jack device driver
  * @edev:		extcon device.
  * @cable_names:	list of supported cables.
- * @num_cables:		size of cable_names.
  * @adc_conditions:	list of adc value conditions.
  * @num_conditions:	size of adc_conditions.
  * @irq:		irq number of attach/detach event (0 if not exist).
@@ -41,8 +40,7 @@
 struct adc_jack_data {
 	struct extcon_dev *edev;
 
-	const char **cable_names;
-	int num_cables;
+	const unsigned int **cable_names;
 	struct adc_jack_cond *adc_conditions;
 	int num_conditions;
 
@@ -112,17 +110,6 @@ static int adc_jack_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to allocate extcon device\n");
 		return -ENOMEM;
 	}
-	data->edev->name = pdata->name;
-
-	/* Check the length of array and set num_cables */
-	for (i = 0; data->edev->supported_cable[i]; i++)
-		;
-	if (i == 0 || i > SUPPORTED_CABLE_MAX) {
-		dev_err(&pdev->dev, "error: pdata->cable_names size = %d\n",
-				i - 1);
-		return -EINVAL;
-	}
-	data->num_cables = i;
 
 	if (!pdata->adc_conditions ||
 			!pdata->adc_conditions[0].state) {
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index a0ed35b336e4..ad87f263056f 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -32,13 +32,10 @@
 #include <linux/mfd/arizona/core.h>
 #include <linux/mfd/arizona/pdata.h>
 #include <linux/mfd/arizona/registers.h>
+#include <dt-bindings/mfd/arizona.h>
 
 #define ARIZONA_MAX_MICD_RANGE 8
 
-#define ARIZONA_ACCDET_MODE_MIC 0
-#define ARIZONA_ACCDET_MODE_HPL 1
-#define ARIZONA_ACCDET_MODE_HPR 2
-
 #define ARIZONA_MICD_CLAMP_MODE_JDL      0x4
 #define ARIZONA_MICD_CLAMP_MODE_JDH      0x5
 #define ARIZONA_MICD_CLAMP_MODE_JDL_GP5H 0x9
@@ -94,7 +91,7 @@ struct arizona_extcon_info {
 	bool detecting;
 	int jack_flips;
 
-	int hpdet_ip;
+	int hpdet_ip_version;
 
 	struct extcon_dev *edev;
 };
@@ -121,17 +118,12 @@ static const int arizona_micd_levels[] = {
 	1257,
 };
 
-#define ARIZONA_CABLE_MECHANICAL 0
-#define ARIZONA_CABLE_MICROPHONE 1
-#define ARIZONA_CABLE_HEADPHONE  2
-#define ARIZONA_CABLE_LINEOUT    3
-
-static const char *arizona_cable[] = {
-	"Mechanical",
-	"Microphone",
-	"Headphone",
-	"Line-out",
-	NULL,
+static const unsigned int arizona_cable[] = {
+	EXTCON_MECHANICAL,
+	EXTCON_MICROPHONE,
+	EXTCON_HEADPHONE,
+	EXTCON_LINE_OUT,
+	EXTCON_NONE,
 };
 
 static void arizona_start_hpdet_acc_id(struct arizona_extcon_info *info);
@@ -145,6 +137,7 @@ static void arizona_extcon_hp_clamp(struct arizona_extcon_info *info,
 
 	switch (arizona->type) {
 	case WM5110:
+	case WM8280:
 		mask = ARIZONA_HP1L_SHRTO | ARIZONA_HP1L_FLWR |
 		       ARIZONA_HP1L_SHRTI;
 		if (clamp)
@@ -380,7 +373,7 @@ static int arizona_hpdet_read(struct arizona_extcon_info *info)
 		return ret;
 	}
 
-	switch (info->hpdet_ip) {
+	switch (info->hpdet_ip_version) {
 	case 0:
 		if (!(val & ARIZONA_HP_DONE)) {
 			dev_err(arizona->dev, "HPDET did not complete: %x\n",
@@ -441,7 +434,7 @@ static int arizona_hpdet_read(struct arizona_extcon_info *info)
 
 	default:
 		dev_warn(arizona->dev, "Unknown HPDET IP revision %d\n",
-			 info->hpdet_ip);
+			 info->hpdet_ip_version);
 	case 2:
 		if (!(val & ARIZONA_HP_DONE_B)) {
 			dev_err(arizona->dev, "HPDET did not complete: %x\n",
@@ -559,7 +552,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 	struct arizona_extcon_info *info = data;
 	struct arizona *arizona = info->arizona;
 	int id_gpio = arizona->pdata.hpdet_id_gpio;
-	int report = ARIZONA_CABLE_HEADPHONE;
+	unsigned int report = EXTCON_HEADPHONE;
 	int ret, reading;
 	bool mic = false;
 
@@ -573,7 +566,7 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 	}
 
 	/* If the cable was removed while measuring ignore the result */
-	ret = extcon_get_cable_state_(info->edev, ARIZONA_CABLE_MECHANICAL);
+	ret = extcon_get_cable_state_(info->edev, EXTCON_MECHANICAL);
 	if (ret < 0) {
 		dev_err(arizona->dev, "Failed to check cable state: %d\n",
 			ret);
@@ -604,9 +597,9 @@ static irqreturn_t arizona_hpdet_irq(int irq, void *data)
 
 	/* Report high impedence cables as line outputs */
 	if (reading >= 5000)
-		report = ARIZONA_CABLE_LINEOUT;
+		report = EXTCON_LINE_OUT;
 	else
-		report = ARIZONA_CABLE_HEADPHONE;
+		report = EXTCON_HEADPHONE;
 
 	ret = extcon_set_cable_state_(info->edev, report, true);
 	if (ret != 0)
@@ -670,9 +663,9 @@ static void arizona_identify_headphone(struct arizona_extcon_info *info)
 	ret = regmap_update_bits(arizona->regmap,
 				 ARIZONA_ACCESSORY_DETECT_MODE_1,
 				 ARIZONA_ACCDET_MODE_MASK,
-				 ARIZONA_ACCDET_MODE_HPL);
+				 arizona->pdata.hpdet_channel);
 	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to set HPDETL mode: %d\n", ret);
+		dev_err(arizona->dev, "Failed to set HPDET mode: %d\n", ret);
 		goto err;
 	}
 
@@ -691,8 +684,7 @@ err:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* Just report headphone */
-	ret = extcon_set_cable_state_(info->edev,
-				      ARIZONA_CABLE_HEADPHONE, true);
+	ret = extcon_set_cable_state_(info->edev, EXTCON_HEADPHONE, true);
 	if (ret != 0)
 		dev_err(arizona->dev, "Failed to report headphone: %d\n", ret);
 
@@ -722,9 +714,9 @@ static void arizona_start_hpdet_acc_id(struct arizona_extcon_info *info)
 				 ARIZONA_ACCESSORY_DETECT_MODE_1,
 				 ARIZONA_ACCDET_SRC | ARIZONA_ACCDET_MODE_MASK,
 				 info->micd_modes[0].src |
-				 ARIZONA_ACCDET_MODE_HPL);
+				 arizona->pdata.hpdet_channel);
 	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to set HPDETL mode: %d\n", ret);
+		dev_err(arizona->dev, "Failed to set HPDET mode: %d\n", ret);
 		goto err;
 	}
 
@@ -749,8 +741,7 @@ err:
 			   ARIZONA_ACCDET_MODE_MASK, ARIZONA_ACCDET_MODE_MIC);
 
 	/* Just report headphone */
-	ret = extcon_set_cable_state_(info->edev,
-				      ARIZONA_CABLE_HEADPHONE, true);
+	ret = extcon_set_cable_state_(info->edev, EXTCON_HEADPHONE, true);
 	if (ret != 0)
 		dev_err(arizona->dev, "Failed to report headphone: %d\n", ret);
 
@@ -789,7 +780,7 @@ static void arizona_micd_detect(struct work_struct *work)
 	mutex_lock(&info->lock);
 
 	/* If the cable was removed while measuring ignore the result */
-	ret = extcon_get_cable_state_(info->edev, ARIZONA_CABLE_MECHANICAL);
+	ret = extcon_get_cable_state_(info->edev, EXTCON_MECHANICAL);
 	if (ret < 0) {
 		dev_err(arizona->dev, "Failed to check cable state: %d\n",
 				ret);
@@ -838,8 +829,7 @@ static void arizona_micd_detect(struct work_struct *work)
 		arizona_identify_headphone(info);
 
 		ret = extcon_set_cable_state_(info->edev,
-					      ARIZONA_CABLE_MICROPHONE, true);
-
+					      EXTCON_MICROPHONE, true);
 		if (ret != 0)
 			dev_err(arizona->dev, "Headset report failed: %d\n",
 				ret);
@@ -1030,7 +1020,7 @@ static irqreturn_t arizona_jackdet(int irq, void *data)
 	if (info->last_jackdet == present) {
 		dev_dbg(arizona->dev, "Detected jack\n");
 		ret = extcon_set_cable_state_(info->edev,
-					      ARIZONA_CABLE_MECHANICAL, true);
+					      EXTCON_MECHANICAL, true);
 
 		if (ret != 0)
 			dev_err(arizona->dev, "Mechanical report failed: %d\n",
@@ -1120,6 +1110,26 @@ static void arizona_micd_set_level(struct arizona *arizona, int index,
 	regmap_update_bits(arizona->regmap, reg, mask, level);
 }
 
+static int arizona_extcon_of_get_pdata(struct arizona *arizona)
+{
+	struct arizona_pdata *pdata = &arizona->pdata;
+	unsigned int val = ARIZONA_ACCDET_MODE_HPL;
+
+	of_property_read_u32(arizona->dev->of_node, "wlf,hpdet-channel", &val);
+	switch (val) {
+	case ARIZONA_ACCDET_MODE_HPL:
+	case ARIZONA_ACCDET_MODE_HPR:
+		pdata->hpdet_channel = val;
+		break;
+	default:
+		dev_err(arizona->dev,
+			"Wrong wlf,hpdet-channel DT value %d\n", val);
+		pdata->hpdet_channel = ARIZONA_ACCDET_MODE_HPL;
+	}
+
+	return 0;
+}
+
 static int arizona_extcon_probe(struct platform_device *pdev)
 {
 	struct arizona *arizona = dev_get_drvdata(pdev->dev.parent);
@@ -1137,6 +1147,11 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 	if (!info)
 		return -ENOMEM;
 
+	if (IS_ENABLED(CONFIG_OF)) {
+		if (!dev_get_platdata(arizona->dev))
+			arizona_extcon_of_get_pdata(arizona);
+	}
+
 	info->micvdd = devm_regulator_get(&pdev->dev, "MICVDD");
 	if (IS_ERR(info->micvdd)) {
 		ret = PTR_ERR(info->micvdd);
@@ -1161,7 +1176,7 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 			break;
 		default:
 			info->micd_clamp = true;
-			info->hpdet_ip = 1;
+			info->hpdet_ip_version = 1;
 			break;
 		}
 		break;
@@ -1172,7 +1187,7 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 			break;
 		default:
 			info->micd_clamp = true;
-			info->hpdet_ip = 2;
+			info->hpdet_ip_version = 2;
 			break;
 		}
 		break;
@@ -1185,7 +1200,6 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to allocate extcon device\n");
 		return -ENOMEM;
 	}
-	info->edev->name = "Headset Jack";
 
 	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret < 0) {
diff --git a/drivers/extcon/extcon-axp288.c b/drivers/extcon/extcon-axp288.c
new file mode 100644
index 000000000000..ea962bc547b8
--- /dev/null
+++ b/drivers/extcon/extcon-axp288.c
@@ -0,0 +1,381 @@
+/*
+ * extcon-axp288.c - X-Power AXP288 PMIC extcon cable detection driver
+ *
+ * Copyright (C) 2015 Intel Corporation
+ * Author: Ramakrishna Pallala <ramakrishna.pallala@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+#include <linux/usb/phy.h>
+#include <linux/notifier.h>
+#include <linux/extcon.h>
+#include <linux/regmap.h>
+#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/mfd/axp20x.h>
+
+/* Power source status register */
+#define PS_STAT_VBUS_TRIGGER		BIT(0)
+#define PS_STAT_BAT_CHRG_DIR		BIT(2)
+#define PS_STAT_VBUS_ABOVE_VHOLD	BIT(3)
+#define PS_STAT_VBUS_VALID		BIT(4)
+#define PS_STAT_VBUS_PRESENT		BIT(5)
+
+/* BC module global register */
+#define BC_GLOBAL_RUN			BIT(0)
+#define BC_GLOBAL_DET_STAT		BIT(2)
+#define BC_GLOBAL_DBP_TOUT		BIT(3)
+#define BC_GLOBAL_VLGC_COM_SEL		BIT(4)
+#define BC_GLOBAL_DCD_TOUT_MASK		(BIT(6)|BIT(5))
+#define BC_GLOBAL_DCD_TOUT_300MS	0
+#define BC_GLOBAL_DCD_TOUT_100MS	1
+#define BC_GLOBAL_DCD_TOUT_500MS	2
+#define BC_GLOBAL_DCD_TOUT_900MS	3
+#define BC_GLOBAL_DCD_DET_SEL		BIT(7)
+
+/* BC module vbus control and status register */
+#define VBUS_CNTL_DPDM_PD_EN		BIT(4)
+#define VBUS_CNTL_DPDM_FD_EN		BIT(5)
+#define VBUS_CNTL_FIRST_PO_STAT		BIT(6)
+
+/* BC USB status register */
+#define USB_STAT_BUS_STAT_MASK		(BIT(3)|BIT(2)|BIT(1)|BIT(0))
+#define USB_STAT_BUS_STAT_SHIFT		0
+#define USB_STAT_BUS_STAT_ATHD		0
+#define USB_STAT_BUS_STAT_CONN		1
+#define USB_STAT_BUS_STAT_SUSP		2
+#define USB_STAT_BUS_STAT_CONF		3
+#define USB_STAT_USB_SS_MODE		BIT(4)
+#define USB_STAT_DEAD_BAT_DET		BIT(6)
+#define USB_STAT_DBP_UNCFG		BIT(7)
+
+/* BC detect status register */
+#define DET_STAT_MASK			(BIT(7)|BIT(6)|BIT(5))
+#define DET_STAT_SHIFT			5
+#define DET_STAT_SDP			1
+#define DET_STAT_CDP			2
+#define DET_STAT_DCP			3
+
+/* IRQ enable-1 register */
+#define PWRSRC_IRQ_CFG_MASK		(BIT(4)|BIT(3)|BIT(2))
+
+/* IRQ enable-6 register */
+#define BC12_IRQ_CFG_MASK		BIT(1)
+
+enum axp288_extcon_reg {
+	AXP288_PS_STAT_REG		= 0x00,
+	AXP288_PS_BOOT_REASON_REG	= 0x02,
+	AXP288_BC_GLOBAL_REG		= 0x2c,
+	AXP288_BC_VBUS_CNTL_REG		= 0x2d,
+	AXP288_BC_USB_STAT_REG		= 0x2e,
+	AXP288_BC_DET_STAT_REG		= 0x2f,
+	AXP288_PWRSRC_IRQ_CFG_REG	= 0x40,
+	AXP288_BC12_IRQ_CFG_REG		= 0x45,
+};
+
+enum axp288_mux_select {
+	EXTCON_GPIO_MUX_SEL_PMIC = 0,
+	EXTCON_GPIO_MUX_SEL_SOC,
+};
+
+enum axp288_extcon_irq {
+	VBUS_FALLING_IRQ = 0,
+	VBUS_RISING_IRQ,
+	MV_CHNG_IRQ,
+	BC_USB_CHNG_IRQ,
+	EXTCON_IRQ_END,
+};
+
+static const unsigned int axp288_extcon_cables[] = {
+	EXTCON_SLOW_CHARGER,
+	EXTCON_CHARGE_DOWNSTREAM,
+	EXTCON_FAST_CHARGER,
+	EXTCON_NONE,
+};
+
+struct axp288_extcon_info {
+	struct device *dev;
+	struct regmap *regmap;
+	struct regmap_irq_chip_data *regmap_irqc;
+	struct axp288_extcon_pdata *pdata;
+	int irq[EXTCON_IRQ_END];
+	struct extcon_dev *edev;
+	struct notifier_block extcon_nb;
+	struct usb_phy *otg;
+};
+
+/* Power up/down reason string array */
+static char *axp288_pwr_up_down_info[] = {
+	"Last wake caused by user pressing the power button",
+	"Last wake caused by a charger insertion",
+	"Last wake caused by a battery insertion",
+	"Last wake caused by SOC initiated global reset",
+	"Last wake caused by cold reset",
+	"Last shutdown caused by PMIC UVLO threshold",
+	"Last shutdown caused by SOC initiated cold off",
+	"Last shutdown caused by user pressing the power button",
+	NULL,
+};
+
+/*
+ * Decode and log the given "reset source indicator" (rsi)
+ * register and then clear it.
+ */
+static void axp288_extcon_log_rsi(struct axp288_extcon_info *info)
+{
+	char **rsi;
+	unsigned int val, i, clear_mask = 0;
+	int ret;
+
+	ret = regmap_read(info->regmap, AXP288_PS_BOOT_REASON_REG, &val);
+	for (i = 0, rsi = axp288_pwr_up_down_info; *rsi; rsi++, i++) {
+		if (val & BIT(i)) {
+			dev_dbg(info->dev, "%s\n", *rsi);
+			clear_mask |= BIT(i);
+		}
+	}
+
+	/* Clear the register value for next reboot (write 1 to clear bit) */
+	regmap_write(info->regmap, AXP288_PS_BOOT_REASON_REG, clear_mask);
+}
+
+static int axp288_handle_chrg_det_event(struct axp288_extcon_info *info)
+{
+	static bool notify_otg, notify_charger;
+	static unsigned int cable;
+	int ret, stat, cfg, pwr_stat;
+	u8 chrg_type;
+	bool vbus_attach = false;
+
+	ret = regmap_read(info->regmap, AXP288_PS_STAT_REG, &pwr_stat);
+	if (ret < 0) {
+		dev_err(info->dev, "failed to read vbus status\n");
+		return ret;
+	}
+
+	vbus_attach = (pwr_stat & PS_STAT_VBUS_PRESENT);
+	if (!vbus_attach)
+		goto notify_otg;
+
+	/* Check charger detection completion status */
+	ret = regmap_read(info->regmap, AXP288_BC_GLOBAL_REG, &cfg);
+	if (ret < 0)
+		goto dev_det_ret;
+	if (cfg & BC_GLOBAL_DET_STAT) {
+		dev_dbg(info->dev, "can't complete the charger detection\n");
+		goto dev_det_ret;
+	}
+
+	ret = regmap_read(info->regmap, AXP288_BC_DET_STAT_REG, &stat);
+	if (ret < 0)
+		goto dev_det_ret;
+
+	chrg_type = (stat & DET_STAT_MASK) >> DET_STAT_SHIFT;
+
+	switch (chrg_type) {
+	case DET_STAT_SDP:
+		dev_dbg(info->dev, "sdp cable is connecetd\n");
+		notify_otg = true;
+		notify_charger = true;
+		cable = EXTCON_SLOW_CHARGER;
+		break;
+	case DET_STAT_CDP:
+		dev_dbg(info->dev, "cdp cable is connecetd\n");
+		notify_otg = true;
+		notify_charger = true;
+		cable = EXTCON_CHARGE_DOWNSTREAM;
+		break;
+	case DET_STAT_DCP:
+		dev_dbg(info->dev, "dcp cable is connecetd\n");
+		notify_charger = true;
+		cable = EXTCON_FAST_CHARGER;
+		break;
+	default:
+		dev_warn(info->dev,
+			"disconnect or unknown or ID event\n");
+	}
+
+notify_otg:
+	if (notify_otg) {
+		/*
+		 * If VBUS is absent Connect D+/D- lines to PMIC for BC
+		 * detection. Else connect them to SOC for USB communication.
+		 */
+		if (info->pdata->gpio_mux_cntl)
+			gpiod_set_value(info->pdata->gpio_mux_cntl,
+				vbus_attach ? EXTCON_GPIO_MUX_SEL_SOC
+						: EXTCON_GPIO_MUX_SEL_PMIC);
+
+		atomic_notifier_call_chain(&info->otg->notifier,
+			vbus_attach ? USB_EVENT_VBUS : USB_EVENT_NONE, NULL);
+	}
+
+	if (notify_charger)
+		extcon_set_cable_state_(info->edev, cable, vbus_attach);
+
+	/* Clear the flags on disconnect event */
+	if (!vbus_attach)
+		notify_otg = notify_charger = false;
+
+	return 0;
+
+dev_det_ret:
+	if (ret < 0)
+		dev_err(info->dev, "failed to detect BC Mod\n");
+
+	return ret;
+}
+
+static irqreturn_t axp288_extcon_isr(int irq, void *data)
+{
+	struct axp288_extcon_info *info = data;
+	int ret;
+
+	ret = axp288_handle_chrg_det_event(info);
+	if (ret < 0)
+		dev_err(info->dev, "failed to handle the interrupt\n");
+
+	return IRQ_HANDLED;
+}
+
+static void axp288_extcon_enable_irq(struct axp288_extcon_info *info)
+{
+	/* Unmask VBUS interrupt */
+	regmap_write(info->regmap, AXP288_PWRSRC_IRQ_CFG_REG,
+						PWRSRC_IRQ_CFG_MASK);
+	regmap_update_bits(info->regmap, AXP288_BC_GLOBAL_REG,
+						BC_GLOBAL_RUN, 0);
+	/* Unmask the BC1.2 complete interrupts */
+	regmap_write(info->regmap, AXP288_BC12_IRQ_CFG_REG, BC12_IRQ_CFG_MASK);
+	/* Enable the charger detection logic */
+	regmap_update_bits(info->regmap, AXP288_BC_GLOBAL_REG,
+					BC_GLOBAL_RUN, BC_GLOBAL_RUN);
+}
+
+static int axp288_extcon_probe(struct platform_device *pdev)
+{
+	struct axp288_extcon_info *info;
+	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+	int ret, i, pirq, gpio;
+
+	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->dev = &pdev->dev;
+	info->regmap = axp20x->regmap;
+	info->regmap_irqc = axp20x->regmap_irqc;
+	info->pdata = pdev->dev.platform_data;
+
+	if (!info->pdata) {
+		/* Try ACPI provided pdata via device properties */
+		if (!device_property_present(&pdev->dev,
+					"axp288_extcon_data\n"))
+			dev_err(&pdev->dev, "failed to get platform data\n");
+		return -ENODEV;
+	}
+	platform_set_drvdata(pdev, info);
+
+	axp288_extcon_log_rsi(info);
+
+	/* Initialize extcon device */
+	info->edev = devm_extcon_dev_allocate(&pdev->dev,
+					      axp288_extcon_cables);
+	if (IS_ERR(info->edev)) {
+		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
+		return PTR_ERR(info->edev);
+	}
+
+	/* Register extcon device */
+	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register extcon device\n");
+		return ret;
+	}
+
+	/* Get otg transceiver phy */
+	info->otg = usb_get_phy(USB_PHY_TYPE_USB2);
+	if (IS_ERR(info->otg)) {
+		dev_err(&pdev->dev, "failed to get otg transceiver\n");
+		return PTR_ERR(info->otg);
+	}
+
+	/* Set up gpio control for USB Mux */
+	if (info->pdata->gpio_mux_cntl) {
+		gpio = desc_to_gpio(info->pdata->gpio_mux_cntl);
+		ret = gpio_request(gpio, "USB_MUX");
+		if (ret < 0) {
+			dev_err(&pdev->dev,
+				"failed to request the gpio=%d\n", gpio);
+			goto gpio_req_failed;
+		}
+		gpiod_direction_output(info->pdata->gpio_mux_cntl,
+						EXTCON_GPIO_MUX_SEL_PMIC);
+	}
+
+	for (i = 0; i < EXTCON_IRQ_END; i++) {
+		pirq = platform_get_irq(pdev, i);
+		info->irq[i] = regmap_irq_get_virq(info->regmap_irqc, pirq);
+		if (info->irq[i] < 0) {
+			dev_err(&pdev->dev,
+				"failed to get virtual interrupt=%d\n", pirq);
+			ret = info->irq[i];
+			goto gpio_req_failed;
+		}
+
+		ret = devm_request_threaded_irq(&pdev->dev, info->irq[i],
+				NULL, axp288_extcon_isr,
+				IRQF_ONESHOT | IRQF_NO_SUSPEND,
+				pdev->name, info);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to request interrupt=%d\n",
+							info->irq[i]);
+			goto gpio_req_failed;
+		}
+	}
+
+	/* Enable interrupts */
+	axp288_extcon_enable_irq(info);
+
+	return 0;
+
+gpio_req_failed:
+	usb_put_phy(info->otg);
+	return ret;
+}
+
+static int axp288_extcon_remove(struct platform_device *pdev)
+{
+	struct axp288_extcon_info *info = platform_get_drvdata(pdev);
+
+	usb_put_phy(info->otg);
+	return 0;
+}
+
+static struct platform_driver axp288_extcon_driver = {
+	.probe = axp288_extcon_probe,
+	.remove = axp288_extcon_remove,
+	.driver = {
+		.name = "axp288_extcon",
+	},
+};
+module_platform_driver(axp288_extcon_driver);
+
+MODULE_AUTHOR("Ramakrishna Pallala <ramakrishna.pallala@intel.com>");
+MODULE_DESCRIPTION("X-Powers AXP288 extcon driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/extcon/extcon-gpio.c b/drivers/extcon/extcon-gpio.c
index 7af33fc433cd..355459a54e8b 100644
--- a/drivers/extcon/extcon-gpio.c
+++ b/drivers/extcon/extcon-gpio.c
@@ -104,7 +104,6 @@ static int gpio_extcon_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to allocate extcon device\n");
 		return -ENOMEM;
 	}
-	extcon_data->edev->name = pdata->name;
 
 	extcon_data->gpio = pdata->gpio;
 	extcon_data->gpio_active_low = pdata->gpio_active_low;
diff --git a/drivers/extcon/extcon-max14577.c b/drivers/extcon/extcon-max14577.c
index 3823aa4a3a80..df0659d98e5a 100644
--- a/drivers/extcon/extcon-max14577.c
+++ b/drivers/extcon/extcon-max14577.c
@@ -148,33 +148,14 @@ enum max14577_muic_acc_type {
 	MAX14577_MUIC_ADC_OPEN,
 };
 
-/* max14577 MUIC device support below list of accessories(external connector) */
-enum {
-	EXTCON_CABLE_USB = 0,
-	EXTCON_CABLE_TA,
-	EXTCON_CABLE_FAST_CHARGER,
-	EXTCON_CABLE_SLOW_CHARGER,
-	EXTCON_CABLE_CHARGE_DOWNSTREAM,
-	EXTCON_CABLE_JIG_USB_ON,
-	EXTCON_CABLE_JIG_USB_OFF,
-	EXTCON_CABLE_JIG_UART_OFF,
-	EXTCON_CABLE_JIG_UART_ON,
-
-	_EXTCON_CABLE_NUM,
-};
-
-static const char *max14577_extcon_cable[] = {
-	[EXTCON_CABLE_USB]			= "USB",
-	[EXTCON_CABLE_TA]			= "TA",
-	[EXTCON_CABLE_FAST_CHARGER]		= "Fast-charger",
-	[EXTCON_CABLE_SLOW_CHARGER]		= "Slow-charger",
-	[EXTCON_CABLE_CHARGE_DOWNSTREAM]	= "Charge-downstream",
-	[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]		= "JIG-UART-ON",
-
-	NULL,
+static const unsigned int max14577_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_TA,
+	EXTCON_FAST_CHARGER,
+	EXTCON_SLOW_CHARGER,
+	EXTCON_CHARGE_DOWNSTREAM,
+	EXTCON_JIG,
+	EXTCON_NONE,
 };
 
 /*
@@ -348,7 +329,6 @@ static int max14577_muic_get_cable_type(struct max14577_muic_info *info,
 static int max14577_muic_jig_handler(struct max14577_muic_info *info,
 		int cable_type, bool attached)
 {
-	char cable_name[32];
 	int ret = 0;
 	u8 path = CTRL1_SW_OPEN;
 
@@ -358,18 +338,12 @@ static int max14577_muic_jig_handler(struct max14577_muic_info *info,
 
 	switch (cable_type) {
 	case MAX14577_MUIC_ADC_FACTORY_MODE_USB_OFF:	/* ADC_JIG_USB_OFF */
-		/* PATH:AP_USB */
-		strcpy(cable_name, "JIG-USB-OFF");
-		path = CTRL1_SW_USB;
-		break;
 	case MAX14577_MUIC_ADC_FACTORY_MODE_USB_ON:	/* ADC_JIG_USB_ON */
 		/* PATH:AP_USB */
-		strcpy(cable_name, "JIG-USB-ON");
 		path = CTRL1_SW_USB;
 		break;
 	case MAX14577_MUIC_ADC_FACTORY_MODE_UART_OFF:	/* ADC_JIG_UART_OFF */
 		/* PATH:AP_UART */
-		strcpy(cable_name, "JIG-UART-OFF");
 		path = CTRL1_SW_UART;
 		break;
 	default:
@@ -382,7 +356,7 @@ static int max14577_muic_jig_handler(struct max14577_muic_info *info,
 	if (ret < 0)
 		return ret;
 
-	extcon_set_cable_state(info->edev, cable_name, attached);
+	extcon_set_cable_state_(info->edev, EXTCON_JIG, attached);
 
 	return 0;
 }
@@ -479,20 +453,22 @@ static int max14577_muic_chg_handler(struct max14577_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev, "USB", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_USB, attached);
 		break;
 	case MAX14577_CHARGER_TYPE_DEDICATED_CHG:
-		extcon_set_cable_state(info->edev, "TA", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_TA, attached);
 		break;
 	case MAX14577_CHARGER_TYPE_DOWNSTREAM_PORT:
-		extcon_set_cable_state(info->edev,
-				"Charge-downstream", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_CHARGE_DOWNSTREAM,
+					attached);
 		break;
 	case MAX14577_CHARGER_TYPE_SPECIAL_500MA:
-		extcon_set_cable_state(info->edev, "Slow-charger", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_SLOW_CHARGER,
+					attached);
 		break;
 	case MAX14577_CHARGER_TYPE_SPECIAL_1A:
-		extcon_set_cable_state(info->edev, "Fast-charger", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_FAST_CHARGER,
+					attached);
 		break;
 	case MAX14577_CHARGER_TYPE_NONE:
 	case MAX14577_CHARGER_TYPE_DEAD_BATTERY:
@@ -742,8 +718,6 @@ static int max14577_muic_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	info->edev->name = dev_name(&pdev->dev);
-
 	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index a66bec8f6252..f4f3b3d53928 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -200,44 +200,17 @@ enum max77693_muic_acc_type {
 /*
  * MAX77693 MUIC device support below list of accessories(external connector)
  */
-enum {
-	EXTCON_CABLE_USB = 0,
-	EXTCON_CABLE_USB_HOST,
-	EXTCON_CABLE_TA,
-	EXTCON_CABLE_FAST_CHARGER,
-	EXTCON_CABLE_SLOW_CHARGER,
-	EXTCON_CABLE_CHARGE_DOWNSTREAM,
-	EXTCON_CABLE_MHL,
-	EXTCON_CABLE_MHL_TA,
-	EXTCON_CABLE_JIG_USB_ON,
-	EXTCON_CABLE_JIG_USB_OFF,
-	EXTCON_CABLE_JIG_UART_OFF,
-	EXTCON_CABLE_JIG_UART_ON,
-	EXTCON_CABLE_DOCK_SMART,
-	EXTCON_CABLE_DOCK_DESK,
-	EXTCON_CABLE_DOCK_AUDIO,
-
-	_EXTCON_CABLE_NUM,
-};
-
-static const char *max77693_extcon_cable[] = {
-	[EXTCON_CABLE_USB]			= "USB",
-	[EXTCON_CABLE_USB_HOST]			= "USB-Host",
-	[EXTCON_CABLE_TA]			= "TA",
-	[EXTCON_CABLE_FAST_CHARGER]		= "Fast-charger",
-	[EXTCON_CABLE_SLOW_CHARGER]		= "Slow-charger",
-	[EXTCON_CABLE_CHARGE_DOWNSTREAM]	= "Charge-downstream",
-	[EXTCON_CABLE_MHL]			= "MHL",
-	[EXTCON_CABLE_MHL_TA]			= "MHL-TA",
-	[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]		= "JIG-UART-ON",
-	[EXTCON_CABLE_DOCK_SMART]		= "Dock-Smart",
-	[EXTCON_CABLE_DOCK_DESK]		= "Dock-Desk",
-	[EXTCON_CABLE_DOCK_AUDIO]		= "Dock-Audio",
-
-	NULL,
+static const unsigned int max77693_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_TA,
+	EXTCON_FAST_CHARGER,
+	EXTCON_SLOW_CHARGER,
+	EXTCON_CHARGE_DOWNSTREAM,
+	EXTCON_MHL,
+	EXTCON_JIG,
+	EXTCON_DOCK,
+	EXTCON_NONE,
 };
 
 /*
@@ -484,7 +457,7 @@ static int max77693_muic_dock_handler(struct max77693_muic_info *info,
 	int ret = 0;
 	int vbvolt;
 	bool cable_attached;
-	char dock_name[CABLE_NAME_MAX];
+	unsigned int dock_id;
 
 	dev_info(info->dev,
 		"external connector is %s (adc:0x%02x)\n",
@@ -507,15 +480,15 @@ static int max77693_muic_dock_handler(struct max77693_muic_info *info,
 		}
 
 		/*
-		 * Notify Dock-Smart/MHL state.
-		 * - Dock-Smart device include three type of cable which
+		 * Notify Dock/MHL state.
+		 * - Dock device include three type of cable which
 		 * are HDMI, USB for mouse/keyboard and micro-usb port
-		 * for USB/TA cable. Dock-Smart device need always exteranl
-		 * power supply(USB/TA cable through micro-usb cable). Dock-
-		 * Smart device support screen output of target to separate
+		 * for USB/TA cable. Dock device need always exteranl
+		 * power supply(USB/TA cable through micro-usb cable). Dock
+		 * device support screen output of target to separate
 		 * monitor and mouse/keyboard for desktop mode.
 		 *
-		 * Features of 'USB/TA cable with Dock-Smart device'
+		 * Features of 'USB/TA cable with Dock device'
 		 * - Support MHL
 		 * - Support external output feature of audio
 		 * - Support charging through micro-usb port without data
@@ -529,16 +502,16 @@ static int max77693_muic_dock_handler(struct max77693_muic_info *info,
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev, "Dock-Smart", attached);
-		extcon_set_cable_state(info->edev, "MHL", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_DOCK, attached);
+		extcon_set_cable_state_(info->edev, EXTCON_MHL, attached);
 		goto out;
 	case MAX77693_MUIC_ADC_AUDIO_MODE_REMOTE:	/* Dock-Desk */
-		strcpy(dock_name, "Dock-Desk");
+		dock_id = EXTCON_DOCK;
 		break;
 	case MAX77693_MUIC_ADC_AV_CABLE_NOLOAD:		/* Dock-Audio */
-		strcpy(dock_name, "Dock-Audio");
+		dock_id = EXTCON_DOCK;
 		if (!attached)
-			extcon_set_cable_state(info->edev, "USB", false);
+			extcon_set_cable_state_(info->edev, EXTCON_USB, false);
 		break;
 	default:
 		dev_err(info->dev, "failed to detect %s dock device\n",
@@ -550,7 +523,7 @@ static int max77693_muic_dock_handler(struct max77693_muic_info *info,
 	ret = max77693_muic_set_path(info, CONTROL1_SW_AUDIO, attached);
 	if (ret < 0)
 		return ret;
-	extcon_set_cable_state(info->edev, dock_name, attached);
+	extcon_set_cable_state_(info->edev, dock_id, attached);
 
 out:
 	return 0;
@@ -615,20 +588,19 @@ static int max77693_muic_adc_ground_handler(struct max77693_muic_info *info)
 		ret = max77693_muic_set_path(info, CONTROL1_SW_USB, attached);
 		if (ret < 0)
 			return ret;
-		extcon_set_cable_state(info->edev, "USB-Host", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, attached);
 		break;
 	case MAX77693_MUIC_GND_AV_CABLE_LOAD:
 		/* Audio Video Cable with load, PATH:AUDIO */
 		ret = max77693_muic_set_path(info, CONTROL1_SW_AUDIO, attached);
 		if (ret < 0)
 			return ret;
-		extcon_set_cable_state(info->edev,
-				"Audio-video-load", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_USB, attached);
 		break;
 	case MAX77693_MUIC_GND_MHL:
 	case MAX77693_MUIC_GND_MHL_VB:
 		/* MHL or MHL with USB/TA cable */
-		extcon_set_cable_state(info->edev, "MHL", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_MHL, attached);
 		break;
 	default:
 		dev_err(info->dev, "failed to detect %s cable of gnd type\n",
@@ -642,7 +614,6 @@ static int max77693_muic_adc_ground_handler(struct max77693_muic_info *info)
 static int max77693_muic_jig_handler(struct max77693_muic_info *info,
 		int cable_type, bool attached)
 {
-	char cable_name[32];
 	int ret = 0;
 	u8 path = CONTROL1_SW_OPEN;
 
@@ -652,23 +623,13 @@ static int max77693_muic_jig_handler(struct max77693_muic_info *info,
 
 	switch (cable_type) {
 	case MAX77693_MUIC_ADC_FACTORY_MODE_USB_OFF:	/* ADC_JIG_USB_OFF */
-		/* PATH:AP_USB */
-		strcpy(cable_name, "JIG-USB-OFF");
-		path = CONTROL1_SW_USB;
-		break;
 	case MAX77693_MUIC_ADC_FACTORY_MODE_USB_ON:	/* ADC_JIG_USB_ON */
 		/* PATH:AP_USB */
-		strcpy(cable_name, "JIG-USB-ON");
 		path = CONTROL1_SW_USB;
 		break;
 	case MAX77693_MUIC_ADC_FACTORY_MODE_UART_OFF:	/* ADC_JIG_UART_OFF */
-		/* PATH:AP_UART */
-		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:
@@ -681,7 +642,7 @@ static int max77693_muic_jig_handler(struct max77693_muic_info *info,
 	if (ret < 0)
 		return ret;
 
-	extcon_set_cable_state(info->edev, cable_name, attached);
+	extcon_set_cable_state_(info->edev, EXTCON_JIG, attached);
 
 	return 0;
 }
@@ -823,22 +784,22 @@ static int max77693_muic_chg_handler(struct max77693_muic_info *info)
 		case MAX77693_MUIC_GND_MHL:
 		case MAX77693_MUIC_GND_MHL_VB:
 			/*
-			 * MHL cable with MHL-TA(USB/TA) cable
+			 * MHL cable with USB/TA cable
 			 * - MHL cable include two port(HDMI line and separate
 			 * micro-usb port. When the target connect MHL cable,
-			 * extcon driver check whether MHL-TA(USB/TA) cable is
-			 * connected. If MHL-TA cable is connected, extcon
+			 * extcon driver check whether USB/TA cable is
+			 * connected. If USB/TA cable is connected, extcon
 			 * driver notify state to notifiee for charging battery.
 			 *
-			 * Features of 'MHL-TA(USB/TA) with MHL cable'
+			 * Features of 'USB/TA with MHL cable'
 			 * - Support MHL
 			 * - Support charging through micro-usb port without
 			 *   data connection
 			 */
-			extcon_set_cable_state(info->edev, "MHL-TA", attached);
+			extcon_set_cable_state_(info->edev, EXTCON_TA, attached);
 			if (!cable_attached)
-				extcon_set_cable_state(info->edev,
-						      "MHL", cable_attached);
+				extcon_set_cable_state_(info->edev, EXTCON_MHL,
+							cable_attached);
 			break;
 		}
 
@@ -861,11 +822,12 @@ static int max77693_muic_chg_handler(struct max77693_muic_info *info)
 			 * - Support charging through micro-usb port without
 			 *   data connection.
 			 */
-			extcon_set_cable_state(info->edev, "USB", attached);
+			extcon_set_cable_state_(info->edev, EXTCON_USB,
+						attached);
 
 			if (!cable_attached)
-				extcon_set_cable_state(info->edev, "Dock-Audio",
-						      cable_attached);
+				extcon_set_cable_state_(info->edev, EXTCON_DOCK,
+							cable_attached);
 			break;
 		case MAX77693_MUIC_ADC_RESERVED_ACC_3:		/* Dock-Smart */
 			/*
@@ -893,10 +855,10 @@ static int max77693_muic_chg_handler(struct max77693_muic_info *info)
 			if (ret < 0)
 				return ret;
 
-			extcon_set_cable_state(info->edev, "Dock-Smart",
-					      attached);
-			extcon_set_cable_state(info->edev, "MHL", attached);
-
+			extcon_set_cable_state_(info->edev, EXTCON_DOCK,
+						attached);
+			extcon_set_cable_state_(info->edev, EXTCON_MHL,
+						attached);
 			break;
 		}
 
@@ -929,23 +891,26 @@ static int max77693_muic_chg_handler(struct max77693_muic_info *info)
 			if (ret < 0)
 				return ret;
 
-			extcon_set_cable_state(info->edev, "USB", attached);
+			extcon_set_cable_state_(info->edev, EXTCON_USB,
+						attached);
 			break;
 		case MAX77693_CHARGER_TYPE_DEDICATED_CHG:
 			/* Only TA cable */
-			extcon_set_cable_state(info->edev, "TA", attached);
+			extcon_set_cable_state_(info->edev, EXTCON_TA, attached);
 			break;
 		}
 		break;
 	case MAX77693_CHARGER_TYPE_DOWNSTREAM_PORT:
-		extcon_set_cable_state(info->edev,
-				"Charge-downstream", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_CHARGE_DOWNSTREAM,
+					attached);
 		break;
 	case MAX77693_CHARGER_TYPE_APPLE_500MA:
-		extcon_set_cable_state(info->edev, "Slow-charger", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_SLOW_CHARGER,
+					attached);
 		break;
 	case MAX77693_CHARGER_TYPE_APPLE_1A_2A:
-		extcon_set_cable_state(info->edev, "Fast-charger", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_FAST_CHARGER,
+					attached);
 		break;
 	case MAX77693_CHARGER_TYPE_DEAD_BATTERY:
 		break;
@@ -1182,7 +1147,6 @@ static int max77693_muic_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
 		return -ENOMEM;
 	}
-	info->edev->name = DEV_NAME;
 
 	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
diff --git a/drivers/extcon/extcon-max77843.c b/drivers/extcon/extcon-max77843.c
index 8db6a926ea07..fac2f1417a79 100644
--- a/drivers/extcon/extcon-max77843.c
+++ b/drivers/extcon/extcon-max77843.c
@@ -118,36 +118,16 @@ enum max77843_muic_charger_type {
 	MAX77843_MUIC_CHG_GND,
 };
 
-enum {
-	MAX77843_CABLE_USB = 0,
-	MAX77843_CABLE_USB_HOST,
-	MAX77843_CABLE_TA,
-	MAX77843_CABLE_CHARGE_DOWNSTREAM,
-	MAX77843_CABLE_FAST_CHARGER,
-	MAX77843_CABLE_SLOW_CHARGER,
-	MAX77843_CABLE_MHL,
-	MAX77843_CABLE_MHL_TA,
-	MAX77843_CABLE_JIG_USB_ON,
-	MAX77843_CABLE_JIG_USB_OFF,
-	MAX77843_CABLE_JIG_UART_ON,
-	MAX77843_CABLE_JIG_UART_OFF,
-
-	MAX77843_CABLE_NUM,
-};
-
-static const char *max77843_extcon_cable[] = {
-	[MAX77843_CABLE_USB]			= "USB",
-	[MAX77843_CABLE_USB_HOST]		= "USB-HOST",
-	[MAX77843_CABLE_TA]			= "TA",
-	[MAX77843_CABLE_CHARGE_DOWNSTREAM]	= "CHARGER-DOWNSTREAM",
-	[MAX77843_CABLE_FAST_CHARGER]		= "FAST-CHARGER",
-	[MAX77843_CABLE_SLOW_CHARGER]		= "SLOW-CHARGER",
-	[MAX77843_CABLE_MHL]			= "MHL",
-	[MAX77843_CABLE_MHL_TA]			= "MHL-TA",
-	[MAX77843_CABLE_JIG_USB_ON]		= "JIG-USB-ON",
-	[MAX77843_CABLE_JIG_USB_OFF]		= "JIG-USB-OFF",
-	[MAX77843_CABLE_JIG_UART_ON]		= "JIG-UART-ON",
-	[MAX77843_CABLE_JIG_UART_OFF]		= "JIG-UART-OFF",
+static const unsigned int max77843_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_TA,
+	EXTCON_CHARGE_DOWNSTREAM,
+	EXTCON_FAST_CHARGER,
+	EXTCON_SLOW_CHARGER,
+	EXTCON_MHL,
+	EXTCON_JIG,
+	EXTCON_NONE,
 };
 
 struct max77843_muic_irq {
@@ -362,7 +342,7 @@ static int max77843_muic_adc_gnd_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev, "USB-HOST", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, attached);
 		break;
 	case MAX77843_MUIC_GND_MHL_VB:
 	case MAX77843_MUIC_GND_MHL:
@@ -370,7 +350,7 @@ static int max77843_muic_adc_gnd_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev, "MHL", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_MHL, attached);
 		break;
 	default:
 		dev_err(info->dev, "failed to detect %s accessory(gnd:0x%x)\n",
@@ -385,36 +365,29 @@ static int max77843_muic_jig_handler(struct max77843_muic_info *info,
 		int cable_type, bool attached)
 {
 	int ret;
+	u8 path = CONTROL1_SW_OPEN;
 
 	dev_dbg(info->dev, "external connector is %s (adc:0x%02x)\n",
 			attached ? "attached" : "detached", cable_type);
 
 	switch (cable_type) {
 	case MAX77843_MUIC_ADC_FACTORY_MODE_USB_OFF:
-		ret = max77843_muic_set_path(info, CONTROL1_SW_USB, attached);
-		if (ret < 0)
-			return ret;
-		extcon_set_cable_state(info->edev, "JIG-USB-OFF", attached);
-		break;
 	case MAX77843_MUIC_ADC_FACTORY_MODE_USB_ON:
-		ret = max77843_muic_set_path(info, CONTROL1_SW_USB, attached);
-		if (ret < 0)
-			return ret;
-		extcon_set_cable_state(info->edev, "JIG-USB-ON", attached);
+		path = CONTROL1_SW_USB;
 		break;
 	case MAX77843_MUIC_ADC_FACTORY_MODE_UART_OFF:
-		ret = max77843_muic_set_path(info, CONTROL1_SW_UART, attached);
-		if (ret < 0)
-			return ret;
-		extcon_set_cable_state(info->edev, "JIG-UART-OFF", attached);
+		path = CONTROL1_SW_UART;
 		break;
 	default:
-		ret = max77843_muic_set_path(info, CONTROL1_SW_OPEN, attached);
-		if (ret < 0)
-			return ret;
-		break;
+		return -EINVAL;
 	}
 
+	ret = max77843_muic_set_path(info, path, attached);
+	if (ret < 0)
+		return ret;
+
+	extcon_set_cable_state_(info->edev, EXTCON_JIG, attached);
+
 	return 0;
 }
 
@@ -505,36 +478,38 @@ static int max77843_muic_chg_handler(struct max77843_muic_info *info)
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev, "USB", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_USB, attached);
 		break;
 	case MAX77843_MUIC_CHG_DOWNSTREAM:
 		ret = max77843_muic_set_path(info, CONTROL1_SW_OPEN, attached);
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev,
-				"CHARGER-DOWNSTREAM", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_CHARGE_DOWNSTREAM,
+					attached);
 		break;
 	case MAX77843_MUIC_CHG_DEDICATED:
 		ret = max77843_muic_set_path(info, CONTROL1_SW_OPEN, attached);
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev, "TA", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_TA, attached);
 		break;
 	case MAX77843_MUIC_CHG_SPECIAL_500MA:
 		ret = max77843_muic_set_path(info, CONTROL1_SW_OPEN, attached);
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev, "SLOW-CHAREGER", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_SLOW_CHARGER,
+					attached);
 		break;
 	case MAX77843_MUIC_CHG_SPECIAL_1A:
 		ret = max77843_muic_set_path(info, CONTROL1_SW_OPEN, attached);
 		if (ret < 0)
 			return ret;
 
-		extcon_set_cable_state(info->edev, "FAST-CHARGER", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_FAST_CHARGER,
+					attached);
 		break;
 	case MAX77843_MUIC_CHG_GND:
 		gnd_type = max77843_muic_get_cable_type(info,
@@ -542,9 +517,9 @@ static int max77843_muic_chg_handler(struct max77843_muic_info *info)
 
 		/* Charger cable on MHL accessory is attach or detach */
 		if (gnd_type == MAX77843_MUIC_GND_MHL_VB)
-			extcon_set_cable_state(info->edev, "MHL-TA", true);
+			extcon_set_cable_state_(info->edev, EXTCON_TA, true);
 		else if (gnd_type == MAX77843_MUIC_GND_MHL)
-			extcon_set_cable_state(info->edev, "MHL-TA", false);
+			extcon_set_cable_state_(info->edev, EXTCON_TA, false);
 		break;
 	case MAX77843_MUIC_CHG_NONE:
 		break;
diff --git a/drivers/extcon/extcon-max8997.c b/drivers/extcon/extcon-max8997.c
index 5774e56c6422..7b1ef200b121 100644
--- a/drivers/extcon/extcon-max8997.c
+++ b/drivers/extcon/extcon-max8997.c
@@ -145,34 +145,17 @@ struct max8997_muic_info {
 	int path_uart;
 };
 
-enum {
-	EXTCON_CABLE_USB = 0,
-	EXTCON_CABLE_USB_HOST,
-	EXTCON_CABLE_TA,
-	EXTCON_CABLE_FAST_CHARGER,
-	EXTCON_CABLE_SLOW_CHARGER,
-	EXTCON_CABLE_CHARGE_DOWNSTREAM,
-	EXTCON_CABLE_MHL,
-	EXTCON_CABLE_DOCK_DESK,
-	EXTCON_CABLE_DOCK_CARD,
-	EXTCON_CABLE_JIG,
-
-	_EXTCON_CABLE_NUM,
-};
-
-static const char *max8997_extcon_cable[] = {
-	[EXTCON_CABLE_USB]			= "USB",
-	[EXTCON_CABLE_USB_HOST]			= "USB-Host",
-	[EXTCON_CABLE_TA]			= "TA",
-	[EXTCON_CABLE_FAST_CHARGER]		= "Fast-charger",
-	[EXTCON_CABLE_SLOW_CHARGER]		= "Slow-charger",
-	[EXTCON_CABLE_CHARGE_DOWNSTREAM]	= "Charge-downstream",
-	[EXTCON_CABLE_MHL]			= "MHL",
-	[EXTCON_CABLE_DOCK_DESK]		= "Dock-Desk",
-	[EXTCON_CABLE_DOCK_CARD]		= "Dock-Card",
-	[EXTCON_CABLE_JIG]			= "JIG",
-
-	NULL,
+static const unsigned int max8997_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_TA,
+	EXTCON_FAST_CHARGER,
+	EXTCON_SLOW_CHARGER,
+	EXTCON_CHARGE_DOWNSTREAM,
+	EXTCON_MHL,
+	EXTCON_DOCK,
+	EXTCON_JIG,
+	EXTCON_NONE,
 };
 
 /*
@@ -347,10 +330,10 @@ static int max8997_muic_handle_usb(struct max8997_muic_info *info,
 
 	switch (usb_type) {
 	case MAX8997_USB_HOST:
-		extcon_set_cable_state(info->edev, "USB-Host", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, attached);
 		break;
 	case MAX8997_USB_DEVICE:
-		extcon_set_cable_state(info->edev, "USB", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_USB, attached);
 		break;
 	default:
 		dev_err(info->dev, "failed to detect %s usb cable\n",
@@ -374,10 +357,8 @@ static int max8997_muic_handle_dock(struct max8997_muic_info *info,
 
 	switch (cable_type) {
 	case MAX8997_MUIC_ADC_AV_CABLE_NOLOAD:
-		extcon_set_cable_state(info->edev, "Dock-desk", attached);
-		break;
 	case MAX8997_MUIC_ADC_FACTORY_MODE_UART_ON:
-		extcon_set_cable_state(info->edev, "Dock-card", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_DOCK, attached);
 		break;
 	default:
 		dev_err(info->dev, "failed to detect %s dock device\n",
@@ -400,7 +381,7 @@ static int max8997_muic_handle_jig_uart(struct max8997_muic_info *info,
 		return ret;
 	}
 
-	extcon_set_cable_state(info->edev, "JIG", attached);
+	extcon_set_cable_state_(info->edev, EXTCON_JIG, attached);
 
 	return 0;
 }
@@ -422,7 +403,7 @@ static int max8997_muic_adc_handler(struct max8997_muic_info *info)
 			return ret;
 		break;
 	case MAX8997_MUIC_ADC_MHL:
-		extcon_set_cable_state(info->edev, "MHL", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_MHL, attached);
 		break;
 	case MAX8997_MUIC_ADC_FACTORY_MODE_USB_OFF:
 	case MAX8997_MUIC_ADC_FACTORY_MODE_USB_ON:
@@ -505,17 +486,19 @@ static int max8997_muic_chg_handler(struct max8997_muic_info *info)
 		}
 		break;
 	case MAX8997_CHARGER_TYPE_DOWNSTREAM_PORT:
-		extcon_set_cable_state(info->edev,
-				      "Charge-downstream", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_CHARGE_DOWNSTREAM,
+					attached);
 		break;
 	case MAX8997_CHARGER_TYPE_DEDICATED_CHG:
-		extcon_set_cable_state(info->edev, "TA", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_TA, attached);
 		break;
 	case MAX8997_CHARGER_TYPE_500MA:
-		extcon_set_cable_state(info->edev, "Slow-charger", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_SLOW_CHARGER,
+					attached);
 		break;
 	case MAX8997_CHARGER_TYPE_1A:
-		extcon_set_cable_state(info->edev, "Fast-charger", attached);
+		extcon_set_cable_state_(info->edev, EXTCON_FAST_CHARGER,
+					attached);
 		break;
 	default:
 		dev_err(info->dev,
@@ -700,7 +683,6 @@ static int max8997_muic_probe(struct platform_device *pdev)
 		ret = -ENOMEM;
 		goto err_irq;
 	}
-	info->edev->name = DEV_NAME;
 
 	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
diff --git a/drivers/extcon/extcon-palmas.c b/drivers/extcon/extcon-palmas.c
index 11c6757b6c40..080d5cc27055 100644
--- a/drivers/extcon/extcon-palmas.c
+++ b/drivers/extcon/extcon-palmas.c
@@ -29,10 +29,10 @@
 #include <linux/of.h>
 #include <linux/of_platform.h>
 
-static const char *palmas_extcon_cable[] = {
-	[0] = "USB",
-	[1] = "USB-HOST",
-	NULL,
+static const unsigned int palmas_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_NONE,
 };
 
 static const int mutually_exclusive[] = {0x3, 0x0};
@@ -49,6 +49,7 @@ static void palmas_usb_wakeup(struct palmas *palmas, int enable)
 static irqreturn_t palmas_vbus_irq_handler(int irq, void *_palmas_usb)
 {
 	struct palmas_usb *palmas_usb = _palmas_usb;
+	struct extcon_dev *edev = palmas_usb->edev;
 	unsigned int vbus_line_state;
 
 	palmas_read(palmas_usb->palmas, PALMAS_INTERRUPT_BASE,
@@ -57,7 +58,7 @@ static irqreturn_t palmas_vbus_irq_handler(int irq, void *_palmas_usb)
 	if (vbus_line_state & PALMAS_INT3_LINE_STATE_VBUS) {
 		if (palmas_usb->linkstat != PALMAS_USB_STATE_VBUS) {
 			palmas_usb->linkstat = PALMAS_USB_STATE_VBUS;
-			extcon_set_cable_state(palmas_usb->edev, "USB", true);
+			extcon_set_cable_state_(edev, EXTCON_USB, true);
 			dev_info(palmas_usb->dev, "USB cable is attached\n");
 		} else {
 			dev_dbg(palmas_usb->dev,
@@ -66,7 +67,7 @@ static irqreturn_t palmas_vbus_irq_handler(int irq, void *_palmas_usb)
 	} else if (!(vbus_line_state & PALMAS_INT3_LINE_STATE_VBUS)) {
 		if (palmas_usb->linkstat == PALMAS_USB_STATE_VBUS) {
 			palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-			extcon_set_cable_state(palmas_usb->edev, "USB", false);
+			extcon_set_cable_state_(edev, EXTCON_USB, false);
 			dev_info(palmas_usb->dev, "USB cable is detached\n");
 		} else {
 			dev_dbg(palmas_usb->dev,
@@ -81,6 +82,7 @@ static irqreturn_t palmas_id_irq_handler(int irq, void *_palmas_usb)
 {
 	unsigned int set, id_src;
 	struct palmas_usb *palmas_usb = _palmas_usb;
+	struct extcon_dev *edev = palmas_usb->edev;
 
 	palmas_read(palmas_usb->palmas, PALMAS_USB_OTG_BASE,
 		PALMAS_USB_ID_INT_LATCH_SET, &set);
@@ -93,7 +95,7 @@ static irqreturn_t palmas_id_irq_handler(int irq, void *_palmas_usb)
 			PALMAS_USB_ID_INT_LATCH_CLR,
 			PALMAS_USB_ID_INT_EN_HI_CLR_ID_GND);
 		palmas_usb->linkstat = PALMAS_USB_STATE_ID;
-		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", true);
+		extcon_set_cable_state_(edev, EXTCON_USB_HOST, true);
 		dev_info(palmas_usb->dev, "USB-HOST cable is attached\n");
 	} else if ((set & PALMAS_USB_ID_INT_SRC_ID_FLOAT) &&
 				(id_src & PALMAS_USB_ID_INT_SRC_ID_FLOAT)) {
@@ -101,17 +103,17 @@ static irqreturn_t palmas_id_irq_handler(int irq, void *_palmas_usb)
 			PALMAS_USB_ID_INT_LATCH_CLR,
 			PALMAS_USB_ID_INT_EN_HI_CLR_ID_FLOAT);
 		palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", false);
+		extcon_set_cable_state_(edev, EXTCON_USB_HOST, false);
 		dev_info(palmas_usb->dev, "USB-HOST cable is detached\n");
 	} else if ((palmas_usb->linkstat == PALMAS_USB_STATE_ID) &&
 				(!(set & PALMAS_USB_ID_INT_SRC_ID_GND))) {
 		palmas_usb->linkstat = PALMAS_USB_STATE_DISCONNECT;
-		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", false);
+		extcon_set_cable_state_(edev, EXTCON_USB_HOST, false);
 		dev_info(palmas_usb->dev, "USB-HOST cable is detached\n");
 	} else if ((palmas_usb->linkstat == PALMAS_USB_STATE_DISCONNECT) &&
 				(id_src & PALMAS_USB_ID_INT_SRC_ID_GND)) {
 		palmas_usb->linkstat = PALMAS_USB_STATE_ID;
-		extcon_set_cable_state(palmas_usb->edev, "USB-HOST", true);
+		extcon_set_cable_state_(edev, EXTCON_USB_HOST, true);
 		dev_info(palmas_usb->dev, " USB-HOST cable is attached\n");
 	}
 
@@ -193,7 +195,6 @@ static int palmas_usb_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "failed to allocate extcon device\n");
 		return -ENOMEM;
 	}
-	palmas_usb->edev->name = kstrdup(node->name, GFP_KERNEL);
 	palmas_usb->edev->mutually_exclusive = mutually_exclusive;
 
 	status = devm_extcon_dev_register(&pdev->dev, palmas_usb->edev);
diff --git a/drivers/extcon/extcon-rt8973a.c b/drivers/extcon/extcon-rt8973a.c
index 9ccd5af89d1c..92c939221a41 100644
--- a/drivers/extcon/extcon-rt8973a.c
+++ b/drivers/extcon/extcon-rt8973a.c
@@ -90,27 +90,12 @@ static struct reg_data rt8973a_reg_data[] = {
 };
 
 /* List of detectable cables */
-enum {
-	EXTCON_CABLE_USB = 0,
-	EXTCON_CABLE_USB_HOST,
-	EXTCON_CABLE_TA,
-	EXTCON_CABLE_JIG_OFF_USB,
-	EXTCON_CABLE_JIG_ON_USB,
-	EXTCON_CABLE_JIG_OFF_UART,
-	EXTCON_CABLE_JIG_ON_UART,
-
-	EXTCON_CABLE_END,
-};
-
-static const char *rt8973a_extcon_cable[] = {
-	[EXTCON_CABLE_USB]		= "USB",
-	[EXTCON_CABLE_USB_HOST]		= "USB-Host",
-	[EXTCON_CABLE_TA]		= "TA",
-	[EXTCON_CABLE_JIG_OFF_USB]	= "JIG-USB-OFF",
-	[EXTCON_CABLE_JIG_ON_USB]	= "JIG-USB-ON",
-	[EXTCON_CABLE_JIG_OFF_UART]	= "JIG-UART-OFF",
-	[EXTCON_CABLE_JIG_ON_UART]	= "JIG-UART-ON",
-	NULL,
+static const unsigned int rt8973a_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_TA,
+	EXTCON_JIG,
+	EXTCON_NONE,
 };
 
 /* Define OVP (Over Voltage Protection), OTP (Over Temperature Protection) */
@@ -313,14 +298,11 @@ static int rt8973a_muic_cable_handler(struct rt8973a_muic_info *info,
 					enum rt8973a_event_type event)
 {
 	static unsigned int prev_cable_type;
-	const char **cable_names = info->edev->supported_cable;
 	unsigned int con_sw = DM_DP_SWITCH_UART;
-	int ret, idx = 0, cable_type;
+	int ret, cable_type;
+	unsigned int id;
 	bool attached = false;
 
-	if (!cable_names)
-		return 0;
-
 	switch (event) {
 	case RT8973A_EVENT_ATTACH:
 		cable_type = rt8973a_muic_get_cable_type(info);
@@ -347,31 +329,25 @@ static int rt8973a_muic_cable_handler(struct rt8973a_muic_info *info,
 
 	switch (cable_type) {
 	case RT8973A_MUIC_ADC_OTG:
-		idx = EXTCON_CABLE_USB_HOST;
+		id = EXTCON_USB_HOST;
 		con_sw = DM_DP_SWITCH_USB;
 		break;
 	case RT8973A_MUIC_ADC_TA:
-		idx = EXTCON_CABLE_TA;
+		id = EXTCON_TA;
 		con_sw = DM_DP_SWITCH_OPEN;
 		break;
 	case RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_OFF_USB:
-		idx = EXTCON_CABLE_JIG_OFF_USB;
-		con_sw = DM_DP_SWITCH_UART;
-		break;
 	case RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_ON_USB:
-		idx = EXTCON_CABLE_JIG_ON_USB;
-		con_sw = DM_DP_SWITCH_UART;
+		id = EXTCON_JIG;
+		con_sw = DM_DP_SWITCH_USB;
 		break;
 	case RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_OFF_UART:
-		idx = EXTCON_CABLE_JIG_OFF_UART;
-		con_sw = DM_DP_SWITCH_UART;
-		break;
 	case RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_ON_UART:
-		idx = EXTCON_CABLE_JIG_ON_UART;
+		id = EXTCON_JIG;
 		con_sw = DM_DP_SWITCH_UART;
 		break;
 	case RT8973A_MUIC_ADC_USB:
-		idx = EXTCON_CABLE_USB;
+		id = EXTCON_USB;
 		con_sw = DM_DP_SWITCH_USB;
 		break;
 	case RT8973A_MUIC_ADC_OPEN:
@@ -421,7 +397,7 @@ static int rt8973a_muic_cable_handler(struct rt8973a_muic_info *info,
 		return ret;
 
 	/* Change the state of external accessory */
-	extcon_set_cable_state(info->edev, cable_names[idx], attached);
+	extcon_set_cable_state_(info->edev, id, attached);
 
 	return 0;
 }
@@ -643,7 +619,6 @@ static int rt8973a_muic_i2c_probe(struct i2c_client *i2c,
 		dev_err(info->dev, "failed to allocate memory for extcon\n");
 		return -ENOMEM;
 	}
-	info->edev->name = np->name;
 
 	/* Register extcon device */
 	ret = devm_extcon_dev_register(info->dev, info->edev);
diff --git a/drivers/extcon/extcon-sm5502.c b/drivers/extcon/extcon-sm5502.c
index 2f93cf307852..817dece23b4c 100644
--- a/drivers/extcon/extcon-sm5502.c
+++ b/drivers/extcon/extcon-sm5502.c
@@ -92,19 +92,11 @@ static struct reg_data sm5502_reg_data[] = {
 };
 
 /* List of detectable cables */
-enum {
-	EXTCON_CABLE_USB = 0,
-	EXTCON_CABLE_USB_HOST,
-	EXTCON_CABLE_TA,
-
-	EXTCON_CABLE_END,
-};
-
-static const char *sm5502_extcon_cable[] = {
-	[EXTCON_CABLE_USB]	= "USB",
-	[EXTCON_CABLE_USB_HOST]	= "USB-Host",
-	[EXTCON_CABLE_TA]	= "TA",
-	NULL,
+static const unsigned int sm5502_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_TA,
+	EXTCON_NONE,
 };
 
 /* Define supported accessory type */
@@ -377,16 +369,12 @@ static int sm5502_muic_cable_handler(struct sm5502_muic_info *info,
 				     bool attached)
 {
 	static unsigned int prev_cable_type = SM5502_MUIC_ADC_GROUND;
-	const char **cable_names = info->edev->supported_cable;
 	unsigned int cable_type = SM5502_MUIC_ADC_GROUND;
 	unsigned int con_sw = DM_DP_SWITCH_OPEN;
 	unsigned int vbus_sw = VBUSIN_SWITCH_OPEN;
-	unsigned int idx = 0;
+	unsigned int id;
 	int ret;
 
-	if (!cable_names)
-		return 0;
-
 	/* Get the type of attached or detached cable */
 	if (attached)
 		cable_type = sm5502_muic_get_cable_type(info);
@@ -396,17 +384,17 @@ static int sm5502_muic_cable_handler(struct sm5502_muic_info *info,
 
 	switch (cable_type) {
 	case SM5502_MUIC_ADC_OPEN_USB:
-		idx	= EXTCON_CABLE_USB;
+		id	= EXTCON_USB;
 		con_sw	= DM_DP_SWITCH_USB;
 		vbus_sw	= VBUSIN_SWITCH_VBUSOUT_WITH_USB;
 		break;
 	case SM5502_MUIC_ADC_OPEN_TA:
-		idx	= EXTCON_CABLE_TA;
+		id	= EXTCON_TA;
 		con_sw	= DM_DP_SWITCH_OPEN;
 		vbus_sw	= VBUSIN_SWITCH_VBUSOUT;
 		break;
 	case SM5502_MUIC_ADC_OPEN_USB_OTG:
-		idx	= EXTCON_CABLE_USB_HOST;
+		id	= EXTCON_USB_HOST;
 		con_sw	= DM_DP_SWITCH_USB;
 		vbus_sw	= VBUSIN_SWITCH_OPEN;
 		break;
@@ -422,7 +410,7 @@ static int sm5502_muic_cable_handler(struct sm5502_muic_info *info,
 		return ret;
 
 	/* Change the state of external accessory */
-	extcon_set_cable_state(info->edev, cable_names[idx], attached);
+	extcon_set_cable_state_(info->edev, id, attached);
 
 	return 0;
 }
@@ -623,7 +611,6 @@ static int sm5022_muic_i2c_probe(struct i2c_client *i2c,
 		dev_err(info->dev, "failed to allocate memory for extcon\n");
 		return -ENOMEM;
 	}
-	info->edev->name = np->name;
 
 	/* Register extcon device */
 	ret = devm_extcon_dev_register(info->dev, info->edev);
diff --git a/drivers/extcon/extcon-usb-gpio.c b/drivers/extcon/extcon-usb-gpio.c
index e45d1f13f445..a2a44536a608 100644
--- a/drivers/extcon/extcon-usb-gpio.c
+++ b/drivers/extcon/extcon-usb-gpio.c
@@ -15,6 +15,7 @@
  */
 
 #include <linux/extcon.h>
+#include <linux/gpio/consumer.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
@@ -38,18 +39,10 @@ struct usb_extcon_info {
 	struct delayed_work wq_detcable;
 };
 
-/* List of detectable cables */
-enum {
-	EXTCON_CABLE_USB = 0,
-	EXTCON_CABLE_USB_HOST,
-
-	EXTCON_CABLE_END,
-};
-
-static const char *usb_extcon_cable[] = {
-	[EXTCON_CABLE_USB] = "USB",
-	[EXTCON_CABLE_USB_HOST] = "USB-HOST",
-	NULL,
+static const unsigned int usb_extcon_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_NONE,
 };
 
 static void usb_extcon_detect_cable(struct work_struct *work)
@@ -67,24 +60,16 @@ static void usb_extcon_detect_cable(struct work_struct *work)
 		 * As we don't have event for USB peripheral cable attached,
 		 * we simulate USB peripheral attach here.
 		 */
-		extcon_set_cable_state(info->edev,
-				       usb_extcon_cable[EXTCON_CABLE_USB_HOST],
-				       false);
-		extcon_set_cable_state(info->edev,
-				       usb_extcon_cable[EXTCON_CABLE_USB],
-				       true);
+		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, false);
+		extcon_set_cable_state_(info->edev, EXTCON_USB, true);
 	} else {
 		/*
 		 * ID = 0 means USB HOST cable attached.
 		 * As we don't have event for USB peripheral cable detached,
 		 * we simulate USB peripheral detach here.
 		 */
-		extcon_set_cable_state(info->edev,
-				       usb_extcon_cable[EXTCON_CABLE_USB],
-				       false);
-		extcon_set_cable_state(info->edev,
-				       usb_extcon_cable[EXTCON_CABLE_USB_HOST],
-				       true);
+		extcon_set_cable_state_(info->edev, EXTCON_USB, false);
+		extcon_set_cable_state_(info->edev, EXTCON_USB_HOST, true);
 	}
 }
 
@@ -113,7 +98,7 @@ static int usb_extcon_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	info->dev = dev;
-	info->id_gpiod = devm_gpiod_get(&pdev->dev, "id");
+	info->id_gpiod = devm_gpiod_get(&pdev->dev, "id", GPIOD_IN);
 	if (IS_ERR(info->id_gpiod)) {
 		dev_err(dev, "failed to get ID GPIO\n");
 		return PTR_ERR(info->id_gpiod);
diff --git a/drivers/extcon/extcon.c b/drivers/extcon/extcon.c
index 4c9f165e4a04..76157ab9faf3 100644
--- a/drivers/extcon/extcon.c
+++ b/drivers/extcon/extcon.c
@@ -1,8 +1,11 @@
 /*
- *  drivers/extcon/extcon_class.c
+ *  drivers/extcon/extcon.c - External Connector (extcon) framework.
  *
  *  External connector (extcon) class driver
  *
+ * Copyright (C) 2015 Samsung Electronics
+ * Author: Chanwoo Choi <cw00.choi@samsung.com>
+ *
  * Copyright (C) 2012 Samsung Electronics
  * Author: Donggeun Kim <dg77.kim@samsung.com>
  * Author: MyungJoo Ham <myungjoo.ham@samsung.com>
@@ -19,8 +22,7 @@
  * but WITHOUT 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>
@@ -33,36 +35,43 @@
 #include <linux/slab.h>
 #include <linux/sysfs.h>
 
-/*
- * extcon_cable_name suggests the standard cable names for commonly used
- * cable types.
- *
- * However, please do not use extcon_cable_name directly for extcon_dev
- * struct's supported_cable pointer unless your device really supports
- * every single port-type of the following cable names. Please choose cable
- * names that are actually used in your extcon device.
- */
-const char extcon_cable_name[][CABLE_NAME_MAX + 1] = {
+#define SUPPORTED_CABLE_MAX	32
+#define CABLE_NAME_MAX		30
+
+static const char *extcon_name[] =  {
+	[EXTCON_NONE]		= "NONE",
+
+	/* USB external connector */
 	[EXTCON_USB]		= "USB",
-	[EXTCON_USB_HOST]	= "USB-Host",
+	[EXTCON_USB_HOST]	= "USB-HOST",
+
+	/* Charger external connector */
 	[EXTCON_TA]		= "TA",
-	[EXTCON_FAST_CHARGER]	= "Fast-charger",
-	[EXTCON_SLOW_CHARGER]	= "Slow-charger",
-	[EXTCON_CHARGE_DOWNSTREAM]	= "Charge-downstream",
+	[EXTCON_FAST_CHARGER]	= "FAST-CHARGER",
+	[EXTCON_SLOW_CHARGER]	= "SLOW-CHARGER",
+	[EXTCON_CHARGE_DOWNSTREAM] = "CHARGE-DOWNSTREAM",
+
+	/* Audio/Video external connector */
+	[EXTCON_LINE_IN]	= "LINE-IN",
+	[EXTCON_LINE_OUT]	= "LINE-OUT",
+	[EXTCON_MICROPHONE]	= "MICROPHONE",
+	[EXTCON_HEADPHONE]	= "HEADPHONE",
+
 	[EXTCON_HDMI]		= "HDMI",
 	[EXTCON_MHL]		= "MHL",
 	[EXTCON_DVI]		= "DVI",
 	[EXTCON_VGA]		= "VGA",
-	[EXTCON_DOCK]		= "Dock",
-	[EXTCON_LINE_IN]	= "Line-in",
-	[EXTCON_LINE_OUT]	= "Line-out",
-	[EXTCON_MIC_IN]		= "Microphone",
-	[EXTCON_HEADPHONE_OUT]	= "Headphone",
-	[EXTCON_SPDIF_IN]	= "SPDIF-in",
-	[EXTCON_SPDIF_OUT]	= "SPDIF-out",
-	[EXTCON_VIDEO_IN]	= "Video-in",
-	[EXTCON_VIDEO_OUT]	= "Video-out",
-	[EXTCON_MECHANICAL]	= "Mechanical",
+	[EXTCON_SPDIF_IN]	= "SPDIF-IN",
+	[EXTCON_SPDIF_OUT]	= "SPDIF-OUT",
+	[EXTCON_VIDEO_IN]	= "VIDEO-IN",
+	[EXTCON_VIDEO_OUT]	= "VIDEO-OUT",
+
+	/* Etc external connector */
+	[EXTCON_DOCK]		= "DOCK",
+	[EXTCON_JIG]		= "JIG",
+	[EXTCON_MECHANICAL]	= "MECHANICAL",
+
+	NULL,
 };
 
 static struct class *extcon_class;
@@ -102,6 +111,51 @@ static int check_mutually_exclusive(struct extcon_dev *edev, u32 new_state)
 	return 0;
 }
 
+static int find_cable_index_by_id(struct extcon_dev *edev, const unsigned int id)
+{
+	int i;
+
+	/* Find the the index of extcon cable in edev->supported_cable */
+	for (i = 0; i < edev->max_supported; i++) {
+		if (edev->supported_cable[i] == id)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int find_cable_index_by_name(struct extcon_dev *edev, const char *name)
+{
+	unsigned int id = EXTCON_NONE;
+	int i = 0;
+
+	if (edev->max_supported == 0)
+		return -EINVAL;
+
+	/* Find the the number of extcon cable */
+	while (extcon_name[i]) {
+		if (!strncmp(extcon_name[i], name, CABLE_NAME_MAX)) {
+			id = i;
+			break;
+		}
+	}
+
+	if (id == EXTCON_NONE)
+		return -EINVAL;
+
+	return find_cable_index_by_id(edev, id);
+}
+
+static bool is_extcon_changed(u32 prev, u32 new, int idx, bool *attached)
+{
+	if (((prev >> idx) & 0x1) != ((new >> idx) & 0x1)) {
+		*attached = new ? true : false;
+		return true;
+	}
+
+	return false;
+}
+
 static ssize_t state_show(struct device *dev, struct device_attribute *attr,
 			  char *buf)
 {
@@ -119,11 +173,9 @@ static ssize_t state_show(struct device *dev, struct device_attribute *attr,
 	if (edev->max_supported == 0)
 		return sprintf(buf, "%u\n", edev->state);
 
-	for (i = 0; i < SUPPORTED_CABLE_MAX; i++) {
-		if (!edev->supported_cable[i])
-			break;
+	for (i = 0; i < edev->max_supported; i++) {
 		count += sprintf(buf + count, "%s=%d\n",
-				 edev->supported_cable[i],
+				extcon_name[edev->supported_cable[i]],
 				 !!(edev->state & (1 << i)));
 	}
 
@@ -155,15 +207,7 @@ static ssize_t name_show(struct device *dev, struct device_attribute *attr,
 {
 	struct extcon_dev *edev = dev_get_drvdata(dev);
 
-	/* Optional callback given by the user */
-	if (edev->print_name) {
-		int ret = edev->print_name(edev, buf);
-
-		if (ret >= 0)
-			return ret;
-	}
-
-	return sprintf(buf, "%s\n", dev_name(&edev->dev));
+	return sprintf(buf, "%s\n", edev->name);
 }
 static DEVICE_ATTR_RO(name);
 
@@ -172,9 +216,10 @@ static ssize_t cable_name_show(struct device *dev,
 {
 	struct extcon_cable *cable = container_of(attr, struct extcon_cable,
 						  attr_name);
+	int i = cable->cable_index;
 
 	return sprintf(buf, "%s\n",
-		       cable->edev->supported_cable[cable->cable_index]);
+			extcon_name[cable->edev->supported_cable[i]]);
 }
 
 static ssize_t cable_state_show(struct device *dev,
@@ -211,23 +256,27 @@ int extcon_update_state(struct extcon_dev *edev, u32 mask, u32 state)
 	char *envp[3];
 	int env_offset = 0;
 	int length;
+	int index;
 	unsigned long flags;
+	bool attached;
 
 	spin_lock_irqsave(&edev->lock, flags);
 
 	if (edev->state != ((edev->state & ~mask) | (state & mask))) {
-		u32 old_state = edev->state;
-
 		if (check_mutually_exclusive(edev, (edev->state & ~mask) |
 						   (state & mask))) {
 			spin_unlock_irqrestore(&edev->lock, flags);
 			return -EPERM;
 		}
 
+		for (index = 0; index < edev->max_supported; index++) {
+			if (is_extcon_changed(edev->state, state, index, &attached))
+				raw_notifier_call_chain(&edev->nh[index], attached, edev);
+		}
+
 		edev->state &= ~mask;
 		edev->state |= state & mask;
 
-		raw_notifier_call_chain(&edev->nh, old_state, edev);
 		/* This could be in interrupt handler */
 		prop_buf = (char *)get_zeroed_page(GFP_ATOMIC);
 		if (prop_buf) {
@@ -284,39 +333,19 @@ int extcon_set_state(struct extcon_dev *edev, u32 state)
 EXPORT_SYMBOL_GPL(extcon_set_state);
 
 /**
- * extcon_find_cable_index() - Get the cable index based on the cable name.
+ * extcon_get_cable_state_() - Get the status of a specific cable.
  * @edev:	the extcon device that has the cable.
- * @cable_name:	cable name to be searched.
- *
- * Note that accessing a cable state based on cable_index is faster than
- * cable_name because using cable_name induces a loop with strncmp().
- * Thus, when get/set_cable_state is repeatedly used, using cable_index
- * is recommended.
+ * @id:		the unique id of each external connector in extcon enumeration.
  */
-int extcon_find_cable_index(struct extcon_dev *edev, const char *cable_name)
+int extcon_get_cable_state_(struct extcon_dev *edev, const unsigned int id)
 {
-	int i;
+	int index;
 
-	if (edev->supported_cable) {
-		for (i = 0; edev->supported_cable[i]; i++) {
-			if (!strncmp(edev->supported_cable[i],
-				cable_name, CABLE_NAME_MAX))
-				return i;
-		}
-	}
+	index = find_cable_index_by_id(edev, id);
+	if (index < 0)
+		return index;
 
-	return -EINVAL;
-}
-EXPORT_SYMBOL_GPL(extcon_find_cable_index);
-
-/**
- * extcon_get_cable_state_() - Get the status of a specific cable.
- * @edev:	the extcon device that has the cable.
- * @index:	cable index that can be retrieved by extcon_find_cable_index().
- */
-int extcon_get_cable_state_(struct extcon_dev *edev, int index)
-{
-	if (index < 0 || (edev->max_supported && edev->max_supported <= index))
+	if (edev->max_supported && edev->max_supported <= index)
 		return -EINVAL;
 
 	return !!(edev->state & (1 << index));
@@ -332,7 +361,7 @@ EXPORT_SYMBOL_GPL(extcon_get_cable_state_);
  */
 int extcon_get_cable_state(struct extcon_dev *edev, const char *cable_name)
 {
-	return extcon_get_cable_state_(edev, extcon_find_cable_index
+	return extcon_get_cable_state_(edev, find_cable_index_by_name
 						(edev, cable_name));
 }
 EXPORT_SYMBOL_GPL(extcon_get_cable_state);
@@ -340,17 +369,22 @@ EXPORT_SYMBOL_GPL(extcon_get_cable_state);
 /**
  * extcon_set_cable_state_() - Set the status of a specific cable.
  * @edev:		the extcon device that has the cable.
- * @index:		cable index that can be retrieved by
- *			extcon_find_cable_index().
- * @cable_state:	the new cable status. The default semantics is
+ * @id:			the unique id of each external connector
+ *			in extcon enumeration.
+ * @state:		the new cable status. The default semantics is
  *			true: attached / false: detached.
  */
-int extcon_set_cable_state_(struct extcon_dev *edev,
-			int index, bool cable_state)
+int extcon_set_cable_state_(struct extcon_dev *edev, unsigned int id,
+				bool cable_state)
 {
 	u32 state;
+	int index;
+
+	index = find_cable_index_by_id(edev, id);
+	if (index < 0)
+		return index;
 
-	if (index < 0 || (edev->max_supported && edev->max_supported <= index))
+	if (edev->max_supported && edev->max_supported <= index)
 		return -EINVAL;
 
 	state = cable_state ? (1 << index) : 0;
@@ -370,7 +404,7 @@ EXPORT_SYMBOL_GPL(extcon_set_cable_state_);
 int extcon_set_cable_state(struct extcon_dev *edev,
 			const char *cable_name, bool cable_state)
 {
-	return extcon_set_cable_state_(edev, extcon_find_cable_index
+	return extcon_set_cable_state_(edev, find_cable_index_by_name
 					(edev, cable_name), cable_state);
 }
 EXPORT_SYMBOL_GPL(extcon_set_cable_state);
@@ -395,29 +429,6 @@ out:
 }
 EXPORT_SYMBOL_GPL(extcon_get_extcon_dev);
 
-static int _call_per_cable(struct notifier_block *nb, unsigned long val,
-			   void *ptr)
-{
-	struct extcon_specific_cable_nb *obj = container_of(nb,
-			struct extcon_specific_cable_nb, internal_nb);
-	struct extcon_dev *edev = ptr;
-
-	if ((val & (1 << obj->cable_index)) !=
-	    (edev->state & (1 << obj->cable_index))) {
-		bool cable_state = true;
-
-		obj->previous_value = val;
-
-		if (val & (1 << obj->cable_index))
-			cable_state = false;
-
-		return obj->user_nb->notifier_call(obj->user_nb,
-				cable_state, ptr);
-	}
-
-	return NOTIFY_OK;
-}
-
 /**
  * extcon_register_interest() - Register a notifier for a state change of a
  *				specific cable, not an entier set of cables of a
@@ -456,20 +467,18 @@ int extcon_register_interest(struct extcon_specific_cable_nb *obj,
 		if (!obj->edev)
 			return -ENODEV;
 
-		obj->cable_index = extcon_find_cable_index(obj->edev,
-							  cable_name);
+		obj->cable_index = find_cable_index_by_name(obj->edev,
+							cable_name);
 		if (obj->cable_index < 0)
 			return obj->cable_index;
 
 		obj->user_nb = nb;
 
-		obj->internal_nb.notifier_call = _call_per_cable;
-
 		spin_lock_irqsave(&obj->edev->lock, flags);
-		ret = raw_notifier_chain_register(&obj->edev->nh,
-						  &obj->internal_nb);
+		ret = raw_notifier_chain_register(
+					&obj->edev->nh[obj->cable_index],
+					obj->user_nb);
 		spin_unlock_irqrestore(&obj->edev->lock, flags);
-		return ret;
 	} else {
 		struct class_dev_iter iter;
 		struct extcon_dev *extd;
@@ -481,7 +490,7 @@ int extcon_register_interest(struct extcon_specific_cable_nb *obj,
 		while ((dev = class_dev_iter_next(&iter))) {
 			extd = dev_get_drvdata(dev);
 
-			if (extcon_find_cable_index(extd, cable_name) < 0)
+			if (find_cable_index_by_name(extd, cable_name) < 0)
 				continue;
 
 			class_dev_iter_exit(&iter);
@@ -489,8 +498,10 @@ int extcon_register_interest(struct extcon_specific_cable_nb *obj,
 						cable_name, nb);
 		}
 
-		return -ENODEV;
+		ret = -ENODEV;
 	}
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(extcon_register_interest);
 
@@ -509,7 +520,8 @@ int extcon_unregister_interest(struct extcon_specific_cable_nb *obj)
 		return -EINVAL;
 
 	spin_lock_irqsave(&obj->edev->lock, flags);
-	ret = raw_notifier_chain_unregister(&obj->edev->nh, &obj->internal_nb);
+	ret = raw_notifier_chain_unregister(
+			&obj->edev->nh[obj->cable_index], obj->user_nb);
 	spin_unlock_irqrestore(&obj->edev->lock, flags);
 
 	return ret;
@@ -519,21 +531,24 @@ EXPORT_SYMBOL_GPL(extcon_unregister_interest);
 /**
  * extcon_register_notifier() - Register a notifiee to get notified by
  *				any attach status changes from the extcon.
- * @edev:	the extcon device.
+ * @edev:	the extcon device that has the external connecotr.
+ * @id:		the unique id of each external connector in extcon enumeration.
  * @nb:		a notifier block to be registered.
  *
  * Note that the second parameter given to the callback of nb (val) is
  * "old_state", not the current state. The current state can be retrieved
  * by looking at the third pameter (edev pointer)'s state value.
  */
-int extcon_register_notifier(struct extcon_dev *edev,
-			struct notifier_block *nb)
+int extcon_register_notifier(struct extcon_dev *edev, unsigned int id,
+			     struct notifier_block *nb)
 {
 	unsigned long flags;
-	int ret;
+	int ret, idx;
+
+	idx = find_cable_index_by_id(edev, id);
 
 	spin_lock_irqsave(&edev->lock, flags);
-	ret = raw_notifier_chain_register(&edev->nh, nb);
+	ret = raw_notifier_chain_register(&edev->nh[idx], nb);
 	spin_unlock_irqrestore(&edev->lock, flags);
 
 	return ret;
@@ -542,17 +557,20 @@ EXPORT_SYMBOL_GPL(extcon_register_notifier);
 
 /**
  * extcon_unregister_notifier() - Unregister a notifiee from the extcon device.
- * @edev:	the extcon device.
- * @nb:		a registered notifier block to be unregistered.
+ * @edev:	the extcon device that has the external connecotr.
+ * @id:		the unique id of each external connector in extcon enumeration.
+ * @nb:		a notifier block to be registered.
  */
-int extcon_unregister_notifier(struct extcon_dev *edev,
-			struct notifier_block *nb)
+int extcon_unregister_notifier(struct extcon_dev *edev, unsigned int id,
+				struct notifier_block *nb)
 {
 	unsigned long flags;
-	int ret;
+	int ret, idx;
+
+	idx = find_cable_index_by_id(edev, id);
 
 	spin_lock_irqsave(&edev->lock, flags);
-	ret = raw_notifier_chain_unregister(&edev->nh, nb);
+	ret = raw_notifier_chain_unregister(&edev->nh[idx], nb);
 	spin_unlock_irqrestore(&edev->lock, flags);
 
 	return ret;
@@ -595,7 +613,7 @@ static void dummy_sysfs_dev_release(struct device *dev)
 
 /*
  * extcon_dev_allocate() - Allocate the memory of extcon device.
- * @supported_cable:	Array of supported cable names ending with NULL.
+ * @supported_cable:	Array of supported extcon ending with EXTCON_NONE.
  *			If supported_cable is NULL, cable name related APIs
  *			are disabled.
  *
@@ -605,7 +623,7 @@ static void dummy_sysfs_dev_release(struct device *dev)
  *
  * Return the pointer of extcon device if success or ERR_PTR(err) if fail
  */
-struct extcon_dev *extcon_dev_allocate(const char **supported_cable)
+struct extcon_dev *extcon_dev_allocate(const unsigned int *supported_cable)
 {
 	struct extcon_dev *edev;
 
@@ -647,7 +665,7 @@ static void devm_extcon_dev_release(struct device *dev, void *res)
 /**
  * devm_extcon_dev_allocate - Allocate managed extcon device
  * @dev:		device owning the extcon device being created
- * @supported_cable:	Array of supported cable names ending with NULL.
+ * @supported_cable:	Array of supported extcon ending with EXTCON_NONE.
  *			If supported_cable is NULL, cable name related APIs
  *			are disabled.
  *
@@ -659,7 +677,7 @@ static void devm_extcon_dev_release(struct device *dev, void *res)
  * or ERR_PTR(err) if fail
  */
 struct extcon_dev *devm_extcon_dev_allocate(struct device *dev,
-					    const char **supported_cable)
+					const unsigned int *supported_cable)
 {
 	struct extcon_dev **ptr, *edev;
 
@@ -701,6 +719,7 @@ EXPORT_SYMBOL_GPL(devm_extcon_dev_free);
 int extcon_dev_register(struct extcon_dev *edev)
 {
 	int ret, index = 0;
+	static atomic_t edev_no = ATOMIC_INIT(-1);
 
 	if (!extcon_class) {
 		ret = create_extcon_class();
@@ -708,30 +727,29 @@ int extcon_dev_register(struct extcon_dev *edev)
 			return ret;
 	}
 
-	if (edev->supported_cable) {
-		/* Get size of array */
-		for (index = 0; edev->supported_cable[index]; index++)
-			;
-		edev->max_supported = index;
-	} else {
-		edev->max_supported = 0;
-	}
+	if (!edev->supported_cable)
+		return -EINVAL;
+
+	for (; edev->supported_cable[index] != EXTCON_NONE; index++);
 
+	edev->max_supported = index;
 	if (index > SUPPORTED_CABLE_MAX) {
-		dev_err(&edev->dev, "extcon: maximum number of supported cables exceeded.\n");
+		dev_err(&edev->dev,
+			"exceed the maximum number of supported cables\n");
 		return -EINVAL;
 	}
 
 	edev->dev.class = extcon_class;
 	edev->dev.release = extcon_dev_release;
 
-	edev->name = edev->name ? edev->name : dev_name(edev->dev.parent);
+	edev->name = dev_name(edev->dev.parent);
 	if (IS_ERR_OR_NULL(edev->name)) {
 		dev_err(&edev->dev,
 			"extcon device name is null\n");
 		return -EINVAL;
 	}
-	dev_set_name(&edev->dev, "%s", edev->name);
+	dev_set_name(&edev->dev, "extcon%lu",
+			(unsigned long)atomic_inc_return(&edev_no));
 
 	if (edev->max_supported) {
 		char buf[10];
@@ -864,7 +882,15 @@ int extcon_dev_register(struct extcon_dev *edev)
 
 	spin_lock_init(&edev->lock);
 
-	RAW_INIT_NOTIFIER_HEAD(&edev->nh);
+	edev->nh = devm_kzalloc(&edev->dev,
+			sizeof(*edev->nh) * edev->max_supported, GFP_KERNEL);
+	if (!edev->nh) {
+		ret = -ENOMEM;
+		goto err_dev;
+	}
+
+	for (index = 0; index < edev->max_supported; index++)
+		RAW_INIT_NOTIFIER_HEAD(&edev->nh[index]);
 
 	dev_set_drvdata(&edev->dev, edev);
 	edev->state = 0;
@@ -1044,6 +1070,15 @@ struct extcon_dev *extcon_get_edev_by_phandle(struct device *dev, int index)
 #endif /* CONFIG_OF */
 EXPORT_SYMBOL_GPL(extcon_get_edev_by_phandle);
 
+/**
+ * extcon_get_edev_name() - Get the name of the extcon device.
+ * @edev:	the extcon device
+ */
+const char *extcon_get_edev_name(struct extcon_dev *edev)
+{
+	return !edev ? NULL : edev->name;
+}
+
 static int __init extcon_class_init(void)
 {
 	return create_extcon_class();
@@ -1059,6 +1094,7 @@ static void __exit extcon_class_exit(void)
 }
 module_exit(extcon_class_exit);
 
+MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>");
 MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
 MODULE_AUTHOR("Donggeun Kim <dg77.kim@samsung.com>");
 MODULE_AUTHOR("MyungJoo Ham <myungjoo.ham@samsung.com>");
diff --git a/drivers/firewire/sbp2.c b/drivers/firewire/sbp2.c
index c22606fe3d44..6bac03999fd4 100644
--- a/drivers/firewire/sbp2.c
+++ b/drivers/firewire/sbp2.c
@@ -1611,7 +1611,6 @@ static struct scsi_host_template scsi_driver_template = {
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
 	.use_clustering		= ENABLE_CLUSTERING,
-	.cmd_per_lun		= 1,
 	.can_queue		= 1,
 	.sdev_attrs		= sbp2_scsi_sysfs_attrs,
 };
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index 6517132e5d8b..99c69a3205c4 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -136,6 +136,7 @@ config QCOM_SCM
 	bool
 	depends on ARM || ARM64
 
+source "drivers/firmware/broadcom/Kconfig"
 source "drivers/firmware/google/Kconfig"
 source "drivers/firmware/efi/Kconfig"
 
diff --git a/drivers/firmware/Makefile b/drivers/firmware/Makefile
index 3fdd3912709a..4a4b897f9314 100644
--- a/drivers/firmware/Makefile
+++ b/drivers/firmware/Makefile
@@ -12,8 +12,10 @@ obj-$(CONFIG_ISCSI_IBFT_FIND)	+= iscsi_ibft_find.o
 obj-$(CONFIG_ISCSI_IBFT)	+= iscsi_ibft.o
 obj-$(CONFIG_FIRMWARE_MEMMAP)	+= memmap.o
 obj-$(CONFIG_QCOM_SCM)		+= qcom_scm.o
-CFLAGS_qcom_scm.o :=$(call as-instr,.arch_extension sec,-DREQUIRES_SEC=1)
+obj-$(CONFIG_QCOM_SCM)		+= qcom_scm-32.o
+CFLAGS_qcom_scm-32.o :=$(call as-instr,.arch_extension sec,-DREQUIRES_SEC=1)
 
+obj-y				+= broadcom/
 obj-$(CONFIG_GOOGLE_FIRMWARE)	+= google/
 obj-$(CONFIG_EFI)		+= efi/
 obj-$(CONFIG_UEFI_CPER)		+= efi/
diff --git a/drivers/firmware/broadcom/Kconfig b/drivers/firmware/broadcom/Kconfig
new file mode 100644
index 000000000000..6bed119930dd
--- /dev/null
+++ b/drivers/firmware/broadcom/Kconfig
@@ -0,0 +1,11 @@
+config BCM47XX_NVRAM
+	bool "Broadcom NVRAM driver"
+	depends on BCM47XX || ARCH_BCM_5301X
+	help
+	  Broadcom home routers contain flash partition called "nvram" with all
+	  important hardware configuration as well as some minor user setup.
+	  NVRAM partition contains a text-like data representing name=value
+	  pairs.
+	  This driver provides an easy way to get value of requested parameter.
+	  It simply reads content of NVRAM and parses it. It doesn't control any
+	  hardware part itself.
diff --git a/drivers/firmware/broadcom/Makefile b/drivers/firmware/broadcom/Makefile
new file mode 100644
index 000000000000..d0e683583cd6
--- /dev/null
+++ b/drivers/firmware/broadcom/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_BCM47XX_NVRAM)		+= bcm47xx_nvram.o
diff --git a/drivers/firmware/broadcom/bcm47xx_nvram.c b/drivers/firmware/broadcom/bcm47xx_nvram.c
new file mode 100644
index 000000000000..87add3fdce52
--- /dev/null
+++ b/drivers/firmware/broadcom/bcm47xx_nvram.c
@@ -0,0 +1,248 @@
+/*
+ * BCM947xx nvram variable access
+ *
+ * Copyright (C) 2005 Broadcom Corporation
+ * Copyright (C) 2006 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2010-2012 Hauke Mehrtens <hauke@hauke-m.de>
+ *
+ * This program is free software; you can redistribute	it and/or modify it
+ * under  the terms of	the GNU General	 Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/io.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/mtd/mtd.h>
+#include <linux/bcm47xx_nvram.h>
+
+#define NVRAM_MAGIC			0x48534C46	/* 'FLSH' */
+#define NVRAM_SPACE			0x10000
+#define NVRAM_MAX_GPIO_ENTRIES		32
+#define NVRAM_MAX_GPIO_VALUE_LEN	30
+
+#define FLASH_MIN		0x00020000	/* Minimum flash size */
+
+struct nvram_header {
+	u32 magic;
+	u32 len;
+	u32 crc_ver_init;	/* 0:7 crc, 8:15 ver, 16:31 sdram_init */
+	u32 config_refresh;	/* 0:15 sdram_config, 16:31 sdram_refresh */
+	u32 config_ncdl;	/* ncdl values for memc */
+};
+
+static char nvram_buf[NVRAM_SPACE];
+static size_t nvram_len;
+static const u32 nvram_sizes[] = {0x8000, 0xF000, 0x10000};
+
+static u32 find_nvram_size(void __iomem *end)
+{
+	struct nvram_header __iomem *header;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(nvram_sizes); i++) {
+		header = (struct nvram_header *)(end - nvram_sizes[i]);
+		if (header->magic == NVRAM_MAGIC)
+			return nvram_sizes[i];
+	}
+
+	return 0;
+}
+
+/* Probe for NVRAM header */
+static int nvram_find_and_copy(void __iomem *iobase, u32 lim)
+{
+	struct nvram_header __iomem *header;
+	int i;
+	u32 off;
+	u32 *src, *dst;
+	u32 size;
+
+	if (nvram_len) {
+		pr_warn("nvram already initialized\n");
+		return -EEXIST;
+	}
+
+	/* TODO: when nvram is on nand flash check for bad blocks first. */
+	off = FLASH_MIN;
+	while (off <= lim) {
+		/* Windowed flash access */
+		size = find_nvram_size(iobase + off);
+		if (size) {
+			header = (struct nvram_header *)(iobase + off - size);
+			goto found;
+		}
+		off <<= 1;
+	}
+
+	/* Try embedded NVRAM at 4 KB and 1 KB as last resorts */
+	header = (struct nvram_header *)(iobase + 4096);
+	if (header->magic == NVRAM_MAGIC) {
+		size = NVRAM_SPACE;
+		goto found;
+	}
+
+	header = (struct nvram_header *)(iobase + 1024);
+	if (header->magic == NVRAM_MAGIC) {
+		size = NVRAM_SPACE;
+		goto found;
+	}
+
+	pr_err("no nvram found\n");
+	return -ENXIO;
+
+found:
+	src = (u32 *)header;
+	dst = (u32 *)nvram_buf;
+	for (i = 0; i < sizeof(struct nvram_header); i += 4)
+		*dst++ = __raw_readl(src++);
+	header = (struct nvram_header *)nvram_buf;
+	nvram_len = header->len;
+	if (nvram_len > size) {
+		pr_err("The nvram size according to the header seems to be bigger than the partition on flash\n");
+		nvram_len = size;
+	}
+	if (nvram_len >= NVRAM_SPACE) {
+		pr_err("nvram on flash (%i bytes) is bigger than the reserved space in memory, will just copy the first %i bytes\n",
+		       header->len, NVRAM_SPACE - 1);
+		nvram_len = NVRAM_SPACE - 1;
+	}
+	/* proceed reading data after header */
+	for (; i < nvram_len; i += 4)
+		*dst++ = readl(src++);
+	nvram_buf[NVRAM_SPACE - 1] = '\0';
+
+	return 0;
+}
+
+/*
+ * On bcm47xx we need access to the NVRAM very early, so we can't use mtd
+ * subsystem to access flash. We can't even use platform device / driver to
+ * store memory offset.
+ * To handle this we provide following symbol. It's supposed to be called as
+ * soon as we get info about flash device, before any NVRAM entry is needed.
+ */
+int bcm47xx_nvram_init_from_mem(u32 base, u32 lim)
+{
+	void __iomem *iobase;
+	int err;
+
+	iobase = ioremap_nocache(base, lim);
+	if (!iobase)
+		return -ENOMEM;
+
+	err = nvram_find_and_copy(iobase, lim);
+
+	iounmap(iobase);
+
+	return err;
+}
+
+static int nvram_init(void)
+{
+#ifdef CONFIG_MTD
+	struct mtd_info *mtd;
+	struct nvram_header header;
+	size_t bytes_read;
+	int err;
+
+	mtd = get_mtd_device_nm("nvram");
+	if (IS_ERR(mtd))
+		return -ENODEV;
+
+	err = mtd_read(mtd, 0, sizeof(header), &bytes_read, (uint8_t *)&header);
+	if (!err && header.magic == NVRAM_MAGIC &&
+	    header.len > sizeof(header)) {
+		nvram_len = header.len;
+		if (nvram_len >= NVRAM_SPACE) {
+			pr_err("nvram on flash (%i bytes) is bigger than the reserved space in memory, will just copy the first %i bytes\n",
+				header.len, NVRAM_SPACE);
+			nvram_len = NVRAM_SPACE - 1;
+		}
+
+		err = mtd_read(mtd, 0, nvram_len, &nvram_len,
+			       (u8 *)nvram_buf);
+		return err;
+	}
+#endif
+
+	return -ENXIO;
+}
+
+int bcm47xx_nvram_getenv(const char *name, char *val, size_t val_len)
+{
+	char *var, *value, *end, *eq;
+	int err;
+
+	if (!name)
+		return -EINVAL;
+
+	if (!nvram_len) {
+		err = nvram_init();
+		if (err)
+			return err;
+	}
+
+	/* Look for name=value and return value */
+	var = &nvram_buf[sizeof(struct nvram_header)];
+	end = nvram_buf + sizeof(nvram_buf);
+	while (var < end && *var) {
+		eq = strchr(var, '=');
+		if (!eq)
+			break;
+		value = eq + 1;
+		if (eq - var == strlen(name) &&
+		    strncmp(var, name, eq - var) == 0)
+			return snprintf(val, val_len, "%s", value);
+		var = value + strlen(value) + 1;
+	}
+	return -ENOENT;
+}
+EXPORT_SYMBOL(bcm47xx_nvram_getenv);
+
+int bcm47xx_nvram_gpio_pin(const char *name)
+{
+	int i, err;
+	char nvram_var[] = "gpioXX";
+	char buf[NVRAM_MAX_GPIO_VALUE_LEN];
+
+	/* TODO: Optimize it to don't call getenv so many times */
+	for (i = 0; i < NVRAM_MAX_GPIO_ENTRIES; i++) {
+		err = snprintf(nvram_var, sizeof(nvram_var), "gpio%i", i);
+		if (err <= 0)
+			continue;
+		err = bcm47xx_nvram_getenv(nvram_var, buf, sizeof(buf));
+		if (err <= 0)
+			continue;
+		if (!strcmp(name, buf))
+			return i;
+	}
+	return -ENOENT;
+}
+EXPORT_SYMBOL(bcm47xx_nvram_gpio_pin);
+
+char *bcm47xx_nvram_get_contents(size_t *nvram_size)
+{
+	int err;
+	char *nvram;
+
+	if (!nvram_len) {
+		err = nvram_init();
+		if (err)
+			return NULL;
+	}
+
+	*nvram_size = nvram_len - sizeof(struct nvram_header);
+	nvram = vmalloc(*nvram_size);
+	if (!nvram)
+		return NULL;
+	memcpy(nvram, &nvram_buf[sizeof(struct nvram_header)], *nvram_size);
+
+	return nvram;
+}
+EXPORT_SYMBOL(bcm47xx_nvram_get_contents);
+
+MODULE_LICENSE("GPLv2");
diff --git a/drivers/firmware/dmi-sysfs.c b/drivers/firmware/dmi-sysfs.c
index e0f1cb3d3598..ef76e5eecf0b 100644
--- a/drivers/firmware/dmi-sysfs.c
+++ b/drivers/firmware/dmi-sysfs.c
@@ -566,7 +566,6 @@ static struct kobj_type dmi_sysfs_entry_ktype = {
 	.default_attrs = dmi_sysfs_entry_attrs,
 };
 
-static struct kobject *dmi_kobj;
 static struct kset *dmi_kset;
 
 /* Global count of all instances seen.  Only for setup */
@@ -648,17 +647,20 @@ static void cleanup_entry_list(void)
 
 static int __init dmi_sysfs_init(void)
 {
-	int error = -ENOMEM;
+	int error;
 	int val;
 
-	/* Set up our directory */
-	dmi_kobj = kobject_create_and_add("dmi", firmware_kobj);
-	if (!dmi_kobj)
+	if (!dmi_kobj) {
+		pr_err("dmi-sysfs: dmi entry is absent.\n");
+		error = -ENODATA;
 		goto err;
+	}
 
 	dmi_kset = kset_create_and_add("entries", NULL, dmi_kobj);
-	if (!dmi_kset)
+	if (!dmi_kset) {
+		error = -ENOMEM;
 		goto err;
+	}
 
 	val = 0;
 	error = dmi_walk(dmi_sysfs_register_handle, &val);
@@ -675,7 +677,6 @@ static int __init dmi_sysfs_init(void)
 err:
 	cleanup_entry_list();
 	kset_unregister(dmi_kset);
-	kobject_put(dmi_kobj);
 	return error;
 }
 
@@ -685,8 +686,6 @@ static void __exit dmi_sysfs_exit(void)
 	pr_debug("dmi-sysfs: unloading.\n");
 	cleanup_entry_list();
 	kset_unregister(dmi_kset);
-	kobject_del(dmi_kobj);
-	kobject_put(dmi_kobj);
 }
 
 module_init(dmi_sysfs_init);
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index 97b1616aa391..ac1ce4a73edf 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -10,6 +10,9 @@
 #include <asm/dmi.h>
 #include <asm/unaligned.h>
 
+struct kobject *dmi_kobj;
+EXPORT_SYMBOL_GPL(dmi_kobj);
+
 /*
  * DMI stands for "Desktop Management Interface".  It is part
  * of and an antecedent to, SMBIOS, which stands for System
@@ -20,6 +23,9 @@ static const char dmi_empty_string[] = "        ";
 static u32 dmi_ver __initdata;
 static u32 dmi_len;
 static u16 dmi_num;
+static u8 smbios_entry_point[32];
+static int smbios_entry_point_size;
+
 /*
  * Catch too early calls to dmi_check_system():
  */
@@ -80,18 +86,18 @@ static const char * __init dmi_string(const struct dmi_header *dm, u8 s)
  *	We have to be cautious here. We have seen BIOSes with DMI pointers
  *	pointing to completely the wrong place for example
  */
-static void dmi_table(u8 *buf,
-		      void (*decode)(const struct dmi_header *, void *),
-		      void *private_data)
+static void dmi_decode_table(u8 *buf,
+			     void (*decode)(const struct dmi_header *, void *),
+			     void *private_data)
 {
 	u8 *data = buf;
 	int i = 0;
 
 	/*
 	 * Stop when we have seen all the items the table claimed to have
-	 * (SMBIOS < 3.0 only) OR we reach an end-of-table marker OR we run
-	 * off the end of the table (should never happen but sometimes does
-	 * on bogus implementations.)
+	 * (SMBIOS < 3.0 only) OR we reach an end-of-table marker (SMBIOS
+	 * >= 3.0 only) OR we run off the end of the table (should never
+	 * happen but sometimes does on bogus implementations.)
 	 */
 	while ((!dmi_num || i < dmi_num) &&
 	       (data - buf + sizeof(struct dmi_header)) <= dmi_len) {
@@ -108,15 +114,24 @@ static void dmi_table(u8 *buf,
 		if (data - buf < dmi_len - 1)
 			decode(dm, private_data);
 
+		data += 2;
+		i++;
+
 		/*
 		 * 7.45 End-of-Table (Type 127) [SMBIOS reference spec v3.0.0]
+		 * For tables behind a 64-bit entry point, we have no item
+		 * count and no exact table length, so stop on end-of-table
+		 * marker. For tables behind a 32-bit entry point, we have
+		 * seen OEM structures behind the end-of-table marker on
+		 * some systems, so don't trust it.
 		 */
-		if (dm->type == DMI_ENTRY_END_OF_TABLE)
+		if (!dmi_num && dm->type == DMI_ENTRY_END_OF_TABLE)
 			break;
-
-		data += 2;
-		i++;
 	}
+
+	/* Trim DMI table length if needed */
+	if (dmi_len > data - buf)
+		dmi_len = data - buf;
 }
 
 static phys_addr_t dmi_base;
@@ -125,16 +140,17 @@ static int __init dmi_walk_early(void (*decode)(const struct dmi_header *,
 		void *))
 {
 	u8 *buf;
+	u32 orig_dmi_len = dmi_len;
 
-	buf = dmi_early_remap(dmi_base, dmi_len);
+	buf = dmi_early_remap(dmi_base, orig_dmi_len);
 	if (buf == NULL)
 		return -1;
 
-	dmi_table(buf, decode, NULL);
+	dmi_decode_table(buf, decode, NULL);
 
 	add_device_randomness(buf, dmi_len);
 
-	dmi_early_unmap(buf, dmi_len);
+	dmi_early_unmap(buf, orig_dmi_len);
 	return 0;
 }
 
@@ -478,17 +494,19 @@ static int __init dmi_present(const u8 *buf)
 	if (memcmp(buf, "_SM_", 4) == 0 &&
 	    buf[5] < 32 && dmi_checksum(buf, buf[5])) {
 		smbios_ver = get_unaligned_be16(buf + 6);
+		smbios_entry_point_size = buf[5];
+		memcpy(smbios_entry_point, buf, smbios_entry_point_size);
 
 		/* Some BIOS report weird SMBIOS version, fix that up */
 		switch (smbios_ver) {
 		case 0x021F:
 		case 0x0221:
-			pr_debug("SMBIOS version fixup(2.%d->2.%d)\n",
+			pr_debug("SMBIOS version fixup (2.%d->2.%d)\n",
 				 smbios_ver & 0xFF, 3);
 			smbios_ver = 0x0203;
 			break;
 		case 0x0233:
-			pr_debug("SMBIOS version fixup(2.%d->2.%d)\n", 51, 6);
+			pr_debug("SMBIOS version fixup (2.%d->2.%d)\n", 51, 6);
 			smbios_ver = 0x0206;
 			break;
 		}
@@ -512,6 +530,9 @@ static int __init dmi_present(const u8 *buf)
 				pr_info("SMBIOS %d.%d present.\n",
 				       dmi_ver >> 8, dmi_ver & 0xFF);
 			} else {
+				smbios_entry_point_size = 15;
+				memcpy(smbios_entry_point, buf,
+				       smbios_entry_point_size);
 				pr_info("Legacy DMI %d.%d present.\n",
 				       dmi_ver >> 8, dmi_ver & 0xFF);
 			}
@@ -533,11 +554,12 @@ static int __init dmi_smbios3_present(const u8 *buf)
 {
 	if (memcmp(buf, "_SM3_", 5) == 0 &&
 	    buf[6] < 32 && dmi_checksum(buf, buf[6])) {
-		dmi_ver = get_unaligned_be32(buf + 6);
-		dmi_ver &= 0xFFFFFF;
+		dmi_ver = get_unaligned_be32(buf + 6) & 0xFFFFFF;
 		dmi_num = 0;			/* No longer specified */
 		dmi_len = get_unaligned_le32(buf + 12);
 		dmi_base = get_unaligned_le64(buf + 16);
+		smbios_entry_point_size = buf[6];
+		memcpy(smbios_entry_point, buf, smbios_entry_point_size);
 
 		if (dmi_walk_early(dmi_decode) == 0) {
 			pr_info("SMBIOS %d.%d.%d present.\n",
@@ -629,6 +651,71 @@ void __init dmi_scan_machine(void)
 	dmi_initialized = 1;
 }
 
+static ssize_t raw_table_read(struct file *file, struct kobject *kobj,
+			      struct bin_attribute *attr, char *buf,
+			      loff_t pos, size_t count)
+{
+	memcpy(buf, attr->private + pos, count);
+	return count;
+}
+
+static BIN_ATTR(smbios_entry_point, S_IRUSR, raw_table_read, NULL, 0);
+static BIN_ATTR(DMI, S_IRUSR, raw_table_read, NULL, 0);
+
+static int __init dmi_init(void)
+{
+	struct kobject *tables_kobj;
+	u8 *dmi_table;
+	int ret = -ENOMEM;
+
+	if (!dmi_available) {
+		ret = -ENODATA;
+		goto err;
+	}
+
+	/*
+	 * Set up dmi directory at /sys/firmware/dmi. This entry should stay
+	 * even after farther error, as it can be used by other modules like
+	 * dmi-sysfs.
+	 */
+	dmi_kobj = kobject_create_and_add("dmi", firmware_kobj);
+	if (!dmi_kobj)
+		goto err;
+
+	tables_kobj = kobject_create_and_add("tables", dmi_kobj);
+	if (!tables_kobj)
+		goto err;
+
+	dmi_table = dmi_remap(dmi_base, dmi_len);
+	if (!dmi_table)
+		goto err_tables;
+
+	bin_attr_smbios_entry_point.size = smbios_entry_point_size;
+	bin_attr_smbios_entry_point.private = smbios_entry_point;
+	ret = sysfs_create_bin_file(tables_kobj, &bin_attr_smbios_entry_point);
+	if (ret)
+		goto err_unmap;
+
+	bin_attr_DMI.size = dmi_len;
+	bin_attr_DMI.private = dmi_table;
+	ret = sysfs_create_bin_file(tables_kobj, &bin_attr_DMI);
+	if (!ret)
+		return 0;
+
+	sysfs_remove_bin_file(tables_kobj,
+			      &bin_attr_smbios_entry_point);
+ err_unmap:
+	dmi_unmap(dmi_table);
+ err_tables:
+	kobject_del(tables_kobj);
+	kobject_put(tables_kobj);
+ err:
+	pr_err("dmi: Firmware registration failed.\n");
+
+	return ret;
+}
+subsys_initcall(dmi_init);
+
 /**
  * dmi_set_dump_stack_arch_desc - set arch description for dump_stack()
  *
@@ -897,7 +984,7 @@ int dmi_walk(void (*decode)(const struct dmi_header *, void *),
 	if (buf == NULL)
 		return -1;
 
-	dmi_table(buf, decode, private_data);
+	dmi_decode_table(buf, decode, private_data);
 
 	dmi_unmap(buf);
 	return 0;
diff --git a/drivers/firmware/efi/Kconfig b/drivers/firmware/efi/Kconfig
index 8de4da5c9ab6..54071c148340 100644
--- a/drivers/firmware/efi/Kconfig
+++ b/drivers/firmware/efi/Kconfig
@@ -18,6 +18,11 @@ config EFI_VARS
 	  Subsequent efibootmgr releases may be found at:
 	  <http://github.com/vathpela/efibootmgr>
 
+config EFI_ESRT
+	bool
+	depends on EFI && !IA64
+	default y
+
 config EFI_VARS_PSTORE
 	tristate "Register efivars backend for pstore"
 	depends on EFI_VARS && PSTORE
diff --git a/drivers/firmware/efi/Makefile b/drivers/firmware/efi/Makefile
index d8be608a9f3b..6fd3da938717 100644
--- a/drivers/firmware/efi/Makefile
+++ b/drivers/firmware/efi/Makefile
@@ -3,6 +3,7 @@
 #
 obj-$(CONFIG_EFI)			+= efi.o vars.o reboot.o
 obj-$(CONFIG_EFI_VARS)			+= efivars.o
+obj-$(CONFIG_EFI_ESRT)			+= esrt.o
 obj-$(CONFIG_EFI_VARS_PSTORE)		+= efi-pstore.o
 obj-$(CONFIG_UEFI_CPER)			+= cper.o
 obj-$(CONFIG_EFI_RUNTIME_MAP)		+= runtime-map.o
diff --git a/drivers/firmware/efi/efi.c b/drivers/firmware/efi/efi.c
index 3061bb8629dc..9fa8084a7c8d 100644
--- a/drivers/firmware/efi/efi.c
+++ b/drivers/firmware/efi/efi.c
@@ -39,6 +39,7 @@ struct efi __read_mostly efi = {
 	.fw_vendor  = EFI_INVALID_TABLE_ADDR,
 	.runtime    = EFI_INVALID_TABLE_ADDR,
 	.config_table  = EFI_INVALID_TABLE_ADDR,
+	.esrt       = EFI_INVALID_TABLE_ADDR,
 };
 EXPORT_SYMBOL(efi);
 
@@ -64,8 +65,7 @@ static int __init parse_efi_cmdline(char *str)
 }
 early_param("efi", parse_efi_cmdline);
 
-static struct kobject *efi_kobj;
-static struct kobject *efivars_kobj;
+struct kobject *efi_kobj;
 
 /*
  * Let's not leave out systab information that snuck into
@@ -85,10 +85,15 @@ static ssize_t systab_show(struct kobject *kobj,
 		str += sprintf(str, "ACPI20=0x%lx\n", efi.acpi20);
 	if (efi.acpi != EFI_INVALID_TABLE_ADDR)
 		str += sprintf(str, "ACPI=0x%lx\n", efi.acpi);
-	if (efi.smbios != EFI_INVALID_TABLE_ADDR)
-		str += sprintf(str, "SMBIOS=0x%lx\n", efi.smbios);
+	/*
+	 * If both SMBIOS and SMBIOS3 entry points are implemented, the
+	 * SMBIOS3 entry point shall be preferred, so we list it first to
+	 * let applications stop parsing after the first match.
+	 */
 	if (efi.smbios3 != EFI_INVALID_TABLE_ADDR)
 		str += sprintf(str, "SMBIOS3=0x%lx\n", efi.smbios3);
+	if (efi.smbios != EFI_INVALID_TABLE_ADDR)
+		str += sprintf(str, "SMBIOS=0x%lx\n", efi.smbios);
 	if (efi.hcdp != EFI_INVALID_TABLE_ADDR)
 		str += sprintf(str, "HCDP=0x%lx\n", efi.hcdp);
 	if (efi.boot_info != EFI_INVALID_TABLE_ADDR)
@@ -212,10 +217,9 @@ static int __init efisubsys_init(void)
 		goto err_remove_group;
 
 	/* and the standard mountpoint for efivarfs */
-	efivars_kobj = kobject_create_and_add("efivars", efi_kobj);
-	if (!efivars_kobj) {
+	error = sysfs_create_mount_point(efi_kobj, "efivars");
+	if (error) {
 		pr_err("efivars: Subsystem registration failed.\n");
-		error = -ENOMEM;
 		goto err_remove_group;
 	}
 
@@ -232,6 +236,84 @@ err_put:
 
 subsys_initcall(efisubsys_init);
 
+/*
+ * Find the efi memory descriptor for a given physical address.  Given a
+ * physicall address, determine if it exists within an EFI Memory Map entry,
+ * and if so, populate the supplied memory descriptor with the appropriate
+ * data.
+ */
+int __init efi_mem_desc_lookup(u64 phys_addr, efi_memory_desc_t *out_md)
+{
+	struct efi_memory_map *map = efi.memmap;
+	void *p, *e;
+
+	if (!efi_enabled(EFI_MEMMAP)) {
+		pr_err_once("EFI_MEMMAP is not enabled.\n");
+		return -EINVAL;
+	}
+
+	if (!map) {
+		pr_err_once("efi.memmap is not set.\n");
+		return -EINVAL;
+	}
+	if (!out_md) {
+		pr_err_once("out_md is null.\n");
+		return -EINVAL;
+        }
+	if (WARN_ON_ONCE(!map->phys_map))
+		return -EINVAL;
+	if (WARN_ON_ONCE(map->nr_map == 0) || WARN_ON_ONCE(map->desc_size == 0))
+		return -EINVAL;
+
+	e = map->phys_map + map->nr_map * map->desc_size;
+	for (p = map->phys_map; p < e; p += map->desc_size) {
+		efi_memory_desc_t *md;
+		u64 size;
+		u64 end;
+
+		/*
+		 * If a driver calls this after efi_free_boot_services,
+		 * ->map will be NULL, and the target may also not be mapped.
+		 * So just always get our own virtual map on the CPU.
+		 *
+		 */
+		md = early_memremap((phys_addr_t)p, sizeof (*md));
+		if (!md) {
+			pr_err_once("early_memremap(%p, %zu) failed.\n",
+				    p, sizeof (*md));
+			return -ENOMEM;
+		}
+
+		if (!(md->attribute & EFI_MEMORY_RUNTIME) &&
+		    md->type != EFI_BOOT_SERVICES_DATA &&
+		    md->type != EFI_RUNTIME_SERVICES_DATA) {
+			early_memunmap(md, sizeof (*md));
+			continue;
+		}
+
+		size = md->num_pages << EFI_PAGE_SHIFT;
+		end = md->phys_addr + size;
+		if (phys_addr >= md->phys_addr && phys_addr < end) {
+			memcpy(out_md, md, sizeof(*out_md));
+			early_memunmap(md, sizeof (*md));
+			return 0;
+		}
+
+		early_memunmap(md, sizeof (*md));
+	}
+	pr_err_once("requested map not found.\n");
+	return -ENOENT;
+}
+
+/*
+ * Calculate the highest address of an efi memory descriptor.
+ */
+u64 __init efi_mem_desc_end(efi_memory_desc_t *md)
+{
+	u64 size = md->num_pages << EFI_PAGE_SHIFT;
+	u64 end = md->phys_addr + size;
+	return end;
+}
 
 /*
  * We can't ioremap data in EFI boot services RAM, because we've already mapped
@@ -274,6 +356,7 @@ static __initdata efi_config_table_type_t common_tables[] = {
 	{SMBIOS_TABLE_GUID, "SMBIOS", &efi.smbios},
 	{SMBIOS3_TABLE_GUID, "SMBIOS 3.0", &efi.smbios3},
 	{UGA_IO_PROTOCOL_GUID, "UGA", &efi.uga},
+	{EFI_SYSTEM_RESOURCE_TABLE_GUID, "ESRT", &efi.esrt},
 	{NULL_GUID, NULL, NULL},
 };
 
diff --git a/drivers/firmware/efi/efivars.c b/drivers/firmware/efi/efivars.c
index 7b2e0496e0c0..756eca8c4cf8 100644
--- a/drivers/firmware/efi/efivars.c
+++ b/drivers/firmware/efi/efivars.c
@@ -535,7 +535,7 @@ static ssize_t efivar_delete(struct file *filp, struct kobject *kobj,
  * efivar_create_sysfs_entry - create a new entry in sysfs
  * @new_var: efivar entry to create
  *
- * Returns 1 on failure, 0 on success
+ * Returns 0 on success, negative error code on failure
  */
 static int
 efivar_create_sysfs_entry(struct efivar_entry *new_var)
@@ -544,6 +544,7 @@ efivar_create_sysfs_entry(struct efivar_entry *new_var)
 	char *short_name;
 	unsigned long variable_name_size;
 	efi_char16_t *variable_name;
+	int ret;
 
 	variable_name = new_var->var.VariableName;
 	variable_name_size = ucs2_strlen(variable_name) * sizeof(efi_char16_t);
@@ -558,7 +559,7 @@ efivar_create_sysfs_entry(struct efivar_entry *new_var)
 	short_name = kzalloc(short_name_size, GFP_KERNEL);
 
 	if (!short_name)
-		return 1;
+		return -ENOMEM;
 
 	/* Convert Unicode to normal chars (assume top bits are 0),
 	   ala UTF-8 */
@@ -574,11 +575,11 @@ efivar_create_sysfs_entry(struct efivar_entry *new_var)
 
 	new_var->kobj.kset = efivars_kset;
 
-	i = kobject_init_and_add(&new_var->kobj, &efivar_ktype,
+	ret = kobject_init_and_add(&new_var->kobj, &efivar_ktype,
 				   NULL, "%s", short_name);
 	kfree(short_name);
-	if (i)
-		return 1;
+	if (ret)
+		return ret;
 
 	kobject_uevent(&new_var->kobj, KOBJ_ADD);
 	efivar_entry_add(new_var, &efivar_sysfs_list);
diff --git a/drivers/firmware/efi/esrt.c b/drivers/firmware/efi/esrt.c
new file mode 100644
index 000000000000..a5b95d61ae71
--- /dev/null
+++ b/drivers/firmware/efi/esrt.c
@@ -0,0 +1,471 @@
+/*
+ * esrt.c
+ *
+ * This module exports EFI System Resource Table (ESRT) entries into userspace
+ * through the sysfs file system. The ESRT provides a read-only catalog of
+ * system components for which the system accepts firmware upgrades via UEFI's
+ * "Capsule Update" feature. This module allows userland utilities to evaluate
+ * what firmware updates can be applied to this system, and potentially arrange
+ * for those updates to occur.
+ *
+ * Data is currently found below /sys/firmware/efi/esrt/...
+ */
+#define pr_fmt(fmt) "esrt: " fmt
+
+#include <linux/capability.h>
+#include <linux/device.h>
+#include <linux/efi.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/kobject.h>
+#include <linux/list.h>
+#include <linux/memblock.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include <asm/io.h>
+#include <asm/early_ioremap.h>
+
+struct efi_system_resource_entry_v1 {
+	efi_guid_t	fw_class;
+	u32		fw_type;
+	u32		fw_version;
+	u32		lowest_supported_fw_version;
+	u32		capsule_flags;
+	u32		last_attempt_version;
+	u32		last_attempt_status;
+};
+
+/*
+ * _count and _version are what they seem like.  _max is actually just
+ * accounting info for the firmware when creating the table; it should never
+ * have been exposed to us.  To wit, the spec says:
+ * The maximum number of resource array entries that can be within the
+ * table without reallocating the table, must not be zero.
+ * Since there's no guidance about what that means in terms of memory layout,
+ * it means nothing to us.
+ */
+struct efi_system_resource_table {
+	u32	fw_resource_count;
+	u32	fw_resource_count_max;
+	u64	fw_resource_version;
+	u8	entries[];
+};
+
+static phys_addr_t esrt_data;
+static size_t esrt_data_size;
+
+static struct efi_system_resource_table *esrt;
+
+struct esre_entry {
+	union {
+		struct efi_system_resource_entry_v1 *esre1;
+	} esre;
+
+	struct kobject kobj;
+	struct list_head list;
+};
+
+/* global list of esre_entry. */
+static LIST_HEAD(entry_list);
+
+/* entry attribute */
+struct esre_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct esre_entry *entry, char *buf);
+	ssize_t (*store)(struct esre_entry *entry,
+			 const char *buf, size_t count);
+};
+
+static struct esre_entry *to_entry(struct kobject *kobj)
+{
+	return container_of(kobj, struct esre_entry, kobj);
+}
+
+static struct esre_attribute *to_attr(struct attribute *attr)
+{
+	return container_of(attr, struct esre_attribute, attr);
+}
+
+static ssize_t esre_attr_show(struct kobject *kobj,
+			      struct attribute *_attr, char *buf)
+{
+	struct esre_entry *entry = to_entry(kobj);
+	struct esre_attribute *attr = to_attr(_attr);
+
+	/* Don't tell normal users what firmware versions we've got... */
+	if (!capable(CAP_SYS_ADMIN))
+		return -EACCES;
+
+	return attr->show(entry, buf);
+}
+
+static const struct sysfs_ops esre_attr_ops = {
+	.show = esre_attr_show,
+};
+
+/* Generic ESRT Entry ("ESRE") support. */
+static ssize_t esre_fw_class_show(struct esre_entry *entry, char *buf)
+{
+	char *str = buf;
+
+	efi_guid_to_str(&entry->esre.esre1->fw_class, str);
+	str += strlen(str);
+	str += sprintf(str, "\n");
+
+	return str - buf;
+}
+
+static struct esre_attribute esre_fw_class = __ATTR(fw_class, 0400,
+	esre_fw_class_show, NULL);
+
+#define esre_attr_decl(name, size, fmt) \
+static ssize_t esre_##name##_show(struct esre_entry *entry, char *buf) \
+{ \
+	return sprintf(buf, fmt "\n", \
+		       le##size##_to_cpu(entry->esre.esre1->name)); \
+} \
+\
+static struct esre_attribute esre_##name = __ATTR(name, 0400, \
+	esre_##name##_show, NULL)
+
+esre_attr_decl(fw_type, 32, "%u");
+esre_attr_decl(fw_version, 32, "%u");
+esre_attr_decl(lowest_supported_fw_version, 32, "%u");
+esre_attr_decl(capsule_flags, 32, "0x%x");
+esre_attr_decl(last_attempt_version, 32, "%u");
+esre_attr_decl(last_attempt_status, 32, "%u");
+
+static struct attribute *esre1_attrs[] = {
+	&esre_fw_class.attr,
+	&esre_fw_type.attr,
+	&esre_fw_version.attr,
+	&esre_lowest_supported_fw_version.attr,
+	&esre_capsule_flags.attr,
+	&esre_last_attempt_version.attr,
+	&esre_last_attempt_status.attr,
+	NULL
+};
+static void esre_release(struct kobject *kobj)
+{
+	struct esre_entry *entry = to_entry(kobj);
+
+	list_del(&entry->list);
+	kfree(entry);
+}
+
+static struct kobj_type esre1_ktype = {
+	.release = esre_release,
+	.sysfs_ops = &esre_attr_ops,
+	.default_attrs = esre1_attrs,
+};
+
+
+static struct kobject *esrt_kobj;
+static struct kset *esrt_kset;
+
+static int esre_create_sysfs_entry(void *esre, int entry_num)
+{
+	struct esre_entry *entry;
+	char name[20];
+
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	sprintf(name, "entry%d", entry_num);
+
+	entry->kobj.kset = esrt_kset;
+
+	if (esrt->fw_resource_version == 1) {
+		int rc = 0;
+
+		entry->esre.esre1 = esre;
+		rc = kobject_init_and_add(&entry->kobj, &esre1_ktype, NULL,
+					  "%s", name);
+		if (rc) {
+			kfree(entry);
+			return rc;
+		}
+	}
+
+	list_add_tail(&entry->list, &entry_list);
+	return 0;
+}
+
+/* support for displaying ESRT fields at the top level */
+#define esrt_attr_decl(name, size, fmt) \
+static ssize_t esrt_##name##_show(struct kobject *kobj, \
+				  struct kobj_attribute *attr, char *buf)\
+{ \
+	return sprintf(buf, fmt "\n", le##size##_to_cpu(esrt->name)); \
+} \
+\
+static struct kobj_attribute esrt_##name = __ATTR(name, 0400, \
+	esrt_##name##_show, NULL)
+
+esrt_attr_decl(fw_resource_count, 32, "%u");
+esrt_attr_decl(fw_resource_count_max, 32, "%u");
+esrt_attr_decl(fw_resource_version, 64, "%llu");
+
+static struct attribute *esrt_attrs[] = {
+	&esrt_fw_resource_count.attr,
+	&esrt_fw_resource_count_max.attr,
+	&esrt_fw_resource_version.attr,
+	NULL,
+};
+
+static inline int esrt_table_exists(void)
+{
+	if (!efi_enabled(EFI_CONFIG_TABLES))
+		return 0;
+	if (efi.esrt == EFI_INVALID_TABLE_ADDR)
+		return 0;
+	return 1;
+}
+
+static umode_t esrt_attr_is_visible(struct kobject *kobj,
+				    struct attribute *attr, int n)
+{
+	if (!esrt_table_exists())
+		return 0;
+	return attr->mode;
+}
+
+static struct attribute_group esrt_attr_group = {
+	.attrs = esrt_attrs,
+	.is_visible = esrt_attr_is_visible,
+};
+
+/*
+ * remap the table, copy it to kmalloced pages, and unmap it.
+ */
+void __init efi_esrt_init(void)
+{
+	void *va;
+	struct efi_system_resource_table tmpesrt;
+	struct efi_system_resource_entry_v1 *v1_entries;
+	size_t size, max, entry_size, entries_size;
+	efi_memory_desc_t md;
+	int rc;
+	phys_addr_t end;
+
+	pr_debug("esrt-init: loading.\n");
+	if (!esrt_table_exists())
+		return;
+
+	rc = efi_mem_desc_lookup(efi.esrt, &md);
+	if (rc < 0) {
+		pr_err("ESRT header is not in the memory map.\n");
+		return;
+	}
+
+	max = efi_mem_desc_end(&md);
+	if (max < efi.esrt) {
+		pr_err("EFI memory descriptor is invalid. (esrt: %p max: %p)\n",
+		       (void *)efi.esrt, (void *)max);
+		return;
+	}
+
+	size = sizeof(*esrt);
+	max -= efi.esrt;
+
+	if (max < size) {
+		pr_err("ESRT header doen't fit on single memory map entry. (size: %zu max: %zu)\n",
+		       size, max);
+		return;
+	}
+
+	va = early_memremap(efi.esrt, size);
+	if (!va) {
+		pr_err("early_memremap(%p, %zu) failed.\n", (void *)efi.esrt,
+		       size);
+		return;
+	}
+
+	memcpy(&tmpesrt, va, sizeof(tmpesrt));
+
+	if (tmpesrt.fw_resource_version == 1) {
+		entry_size = sizeof (*v1_entries);
+	} else {
+		pr_err("Unsupported ESRT version %lld.\n",
+		       tmpesrt.fw_resource_version);
+		return;
+	}
+
+	if (tmpesrt.fw_resource_count > 0 && max - size < entry_size) {
+		pr_err("ESRT memory map entry can only hold the header. (max: %zu size: %zu)\n",
+		       max - size, entry_size);
+		goto err_memunmap;
+	}
+
+	/*
+	 * The format doesn't really give us any boundary to test here,
+	 * so I'm making up 128 as the max number of individually updatable
+	 * components we support.
+	 * 128 should be pretty excessive, but there's still some chance
+	 * somebody will do that someday and we'll need to raise this.
+	 */
+	if (tmpesrt.fw_resource_count > 128) {
+		pr_err("ESRT says fw_resource_count has very large value %d.\n",
+		       tmpesrt.fw_resource_count);
+		goto err_memunmap;
+	}
+
+	/*
+	 * We know it can't be larger than N * sizeof() here, and N is limited
+	 * by the previous test to a small number, so there's no overflow.
+	 */
+	entries_size = tmpesrt.fw_resource_count * entry_size;
+	if (max < size + entries_size) {
+		pr_err("ESRT does not fit on single memory map entry (size: %zu max: %zu)\n",
+		       size, max);
+		goto err_memunmap;
+	}
+
+	/* remap it with our (plausible) new pages */
+	early_memunmap(va, size);
+	size += entries_size;
+	va = early_memremap(efi.esrt, size);
+	if (!va) {
+		pr_err("early_memremap(%p, %zu) failed.\n", (void *)efi.esrt,
+		       size);
+		return;
+	}
+
+	esrt_data = (phys_addr_t)efi.esrt;
+	esrt_data_size = size;
+
+	end = esrt_data + size;
+	pr_info("Reserving ESRT space from %pa to %pa.\n", &esrt_data, &end);
+	memblock_reserve(esrt_data, esrt_data_size);
+
+	pr_debug("esrt-init: loaded.\n");
+err_memunmap:
+	early_memunmap(va, size);
+}
+
+static int __init register_entries(void)
+{
+	struct efi_system_resource_entry_v1 *v1_entries = (void *)esrt->entries;
+	int i, rc;
+
+	if (!esrt_table_exists())
+		return 0;
+
+	for (i = 0; i < le32_to_cpu(esrt->fw_resource_count); i++) {
+		void *esre = NULL;
+		if (esrt->fw_resource_version == 1) {
+			esre = &v1_entries[i];
+		} else {
+			pr_err("Unsupported ESRT version %lld.\n",
+			       esrt->fw_resource_version);
+			return -EINVAL;
+		}
+
+		rc = esre_create_sysfs_entry(esre, i);
+		if (rc < 0) {
+			pr_err("ESRT entry creation failed with error %d.\n",
+			       rc);
+			return rc;
+		}
+	}
+	return 0;
+}
+
+static void cleanup_entry_list(void)
+{
+	struct esre_entry *entry, *next;
+
+	list_for_each_entry_safe(entry, next, &entry_list, list) {
+		kobject_put(&entry->kobj);
+	}
+}
+
+static int __init esrt_sysfs_init(void)
+{
+	int error;
+	struct efi_system_resource_table __iomem *ioesrt;
+
+	pr_debug("esrt-sysfs: loading.\n");
+	if (!esrt_data || !esrt_data_size)
+		return -ENOSYS;
+
+	ioesrt = ioremap(esrt_data, esrt_data_size);
+	if (!ioesrt) {
+		pr_err("ioremap(%pa, %zu) failed.\n", &esrt_data,
+		       esrt_data_size);
+		return -ENOMEM;
+	}
+
+	esrt = kmalloc(esrt_data_size, GFP_KERNEL);
+	if (!esrt) {
+		pr_err("kmalloc failed. (wanted %zu bytes)\n", esrt_data_size);
+		iounmap(ioesrt);
+		return -ENOMEM;
+	}
+
+	memcpy_fromio(esrt, ioesrt, esrt_data_size);
+
+	esrt_kobj = kobject_create_and_add("esrt", efi_kobj);
+	if (!esrt_kobj) {
+		pr_err("Firmware table registration failed.\n");
+		error = -ENOMEM;
+		goto err;
+	}
+
+	error = sysfs_create_group(esrt_kobj, &esrt_attr_group);
+	if (error) {
+		pr_err("Sysfs attribute export failed with error %d.\n",
+		       error);
+		goto err_remove_esrt;
+	}
+
+	esrt_kset = kset_create_and_add("entries", NULL, esrt_kobj);
+	if (!esrt_kset) {
+		pr_err("kset creation failed.\n");
+		error = -ENOMEM;
+		goto err_remove_group;
+	}
+
+	error = register_entries();
+	if (error)
+		goto err_cleanup_list;
+
+	memblock_remove(esrt_data, esrt_data_size);
+
+	pr_debug("esrt-sysfs: loaded.\n");
+
+	return 0;
+err_cleanup_list:
+	cleanup_entry_list();
+	kset_unregister(esrt_kset);
+err_remove_group:
+	sysfs_remove_group(esrt_kobj, &esrt_attr_group);
+err_remove_esrt:
+	kobject_put(esrt_kobj);
+err:
+	kfree(esrt);
+	esrt = NULL;
+	return error;
+}
+
+static void __exit esrt_sysfs_exit(void)
+{
+	pr_debug("esrt-sysfs: unloading.\n");
+	cleanup_entry_list();
+	kset_unregister(esrt_kset);
+	sysfs_remove_group(esrt_kobj, &esrt_attr_group);
+	kfree(esrt);
+	esrt = NULL;
+	kobject_del(esrt_kobj);
+	kobject_put(esrt_kobj);
+}
+
+module_init(esrt_sysfs_init);
+module_exit(esrt_sysfs_exit);
+
+MODULE_AUTHOR("Peter Jones <pjones@redhat.com>");
+MODULE_DESCRIPTION("EFI System Resource Table support");
+MODULE_LICENSE("GPL");
diff --git a/drivers/firmware/efi/libstub/Makefile b/drivers/firmware/efi/libstub/Makefile
index 280bc0a63365..816dbe9f4b82 100644
--- a/drivers/firmware/efi/libstub/Makefile
+++ b/drivers/firmware/efi/libstub/Makefile
@@ -24,8 +24,6 @@ KASAN_SANITIZE			:= n
 lib-y				:= efi-stub-helper.o
 lib-$(CONFIG_EFI_ARMSTUB)	+= arm-stub.o fdt.o
 
-CFLAGS_fdt.o			+= -I$(srctree)/scripts/dtc/libfdt/
-
 #
 # arm64 puts the stub in the kernel proper, which will unnecessarily retain all
 # code indefinitely unless it is annotated as __init/__initdata/__initconst etc.
diff --git a/drivers/firmware/iscsi_ibft.c b/drivers/firmware/iscsi_ibft.c
index 071c2c969eec..72791232e46b 100644
--- a/drivers/firmware/iscsi_ibft.c
+++ b/drivers/firmware/iscsi_ibft.c
@@ -186,8 +186,20 @@ struct ibft_kobject {
 
 static struct iscsi_boot_kset *boot_kset;
 
+/* fully null address */
 static const char nulls[16];
 
+/* IPv4-mapped IPv6 ::ffff:0.0.0.0 */
+static const char mapped_nulls[16] = { 0x00, 0x00, 0x00, 0x00,
+                                       0x00, 0x00, 0x00, 0x00,
+                                       0x00, 0x00, 0xff, 0xff,
+                                       0x00, 0x00, 0x00, 0x00 };
+
+static int address_not_null(u8 *ip)
+{
+	return (memcmp(ip, nulls, 16) && memcmp(ip, mapped_nulls, 16));
+}
+
 /*
  * Helper functions to parse data properly.
  */
@@ -445,7 +457,7 @@ static umode_t ibft_check_nic_for(void *data, int type)
 		rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_ETH_IP_ADDR:
-		if (memcmp(nic->ip_addr, nulls, sizeof(nic->ip_addr)))
+		if (address_not_null(nic->ip_addr))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_ETH_SUBNET_MASK:
@@ -456,21 +468,19 @@ static umode_t ibft_check_nic_for(void *data, int type)
 		rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_ETH_GATEWAY:
-		if (memcmp(nic->gateway, nulls, sizeof(nic->gateway)))
+		if (address_not_null(nic->gateway))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_ETH_PRIMARY_DNS:
-		if (memcmp(nic->primary_dns, nulls,
-			   sizeof(nic->primary_dns)))
+		if (address_not_null(nic->primary_dns))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_ETH_SECONDARY_DNS:
-		if (memcmp(nic->secondary_dns, nulls,
-			   sizeof(nic->secondary_dns)))
+		if (address_not_null(nic->secondary_dns))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_ETH_DHCP:
-		if (memcmp(nic->dhcp, nulls, sizeof(nic->dhcp)))
+		if (address_not_null(nic->dhcp))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_ETH_VLAN:
@@ -536,23 +546,19 @@ static umode_t __init ibft_check_initiator_for(void *data, int type)
 		rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_INI_ISNS_SERVER:
-		if (memcmp(init->isns_server, nulls,
-			   sizeof(init->isns_server)))
+		if (address_not_null(init->isns_server))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_INI_SLP_SERVER:
-		if (memcmp(init->slp_server, nulls,
-			   sizeof(init->slp_server)))
+		if (address_not_null(init->slp_server))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_INI_PRI_RADIUS_SERVER:
-		if (memcmp(init->pri_radius_server, nulls,
-			   sizeof(init->pri_radius_server)))
+		if (address_not_null(init->pri_radius_server))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_INI_SEC_RADIUS_SERVER:
-		if (memcmp(init->sec_radius_server, nulls,
-			   sizeof(init->sec_radius_server)))
+		if (address_not_null(init->sec_radius_server))
 			rc = S_IRUGO;
 		break;
 	case ISCSI_BOOT_INI_INITIATOR_NAME:
diff --git a/drivers/firmware/memmap.c b/drivers/firmware/memmap.c
index cc016c615c19..5de3ed29282c 100644
--- a/drivers/firmware/memmap.c
+++ b/drivers/firmware/memmap.c
@@ -144,7 +144,9 @@ static struct kobj_type __refdata memmap_ktype = {
  *
  * Common implementation of firmware_map_add() and firmware_map_add_early()
  * which expects a pre-allocated struct firmware_map_entry.
- **/
+ *
+ * Return: 0 always
+ */
 static int firmware_map_add_entry(u64 start, u64 end,
 				  const char *type,
 				  struct firmware_map_entry *entry)
@@ -170,7 +172,7 @@ static int firmware_map_add_entry(u64 start, u64 end,
  * @entry: removed entry.
  *
  * The caller must hold map_entries_lock, and release it properly.
- **/
+ */
 static inline void firmware_map_remove_entry(struct firmware_map_entry *entry)
 {
 	list_del(&entry->list);
@@ -208,7 +210,7 @@ static inline void remove_sysfs_fw_map_entry(struct firmware_map_entry *entry)
 	kobject_put(&entry->kobj);
 }
 
-/*
+/**
  * firmware_map_find_entry_in_list() - Search memmap entry in a given list.
  * @start: Start of the memory range.
  * @end:   End of the memory range (exclusive).
@@ -236,7 +238,7 @@ firmware_map_find_entry_in_list(u64 start, u64 end, const char *type,
 	return NULL;
 }
 
-/*
+/**
  * firmware_map_find_entry() - Search memmap entry in map_entries.
  * @start: Start of the memory range.
  * @end:   End of the memory range (exclusive).
@@ -254,7 +256,7 @@ firmware_map_find_entry(u64 start, u64 end, const char *type)
 	return firmware_map_find_entry_in_list(start, end, type, &map_entries);
 }
 
-/*
+/**
  * firmware_map_find_entry_bootmem() - Search memmap entry in map_entries_bootmem.
  * @start: Start of the memory range.
  * @end:   End of the memory range (exclusive).
@@ -283,8 +285,8 @@ firmware_map_find_entry_bootmem(u64 start, u64 end, const char *type)
  * similar to function firmware_map_add_early(). The only difference is that
  * it will create the syfs entry dynamically.
  *
- * Returns 0 on success, or -ENOMEM if no memory could be allocated.
- **/
+ * Return: 0 on success, or -ENOMEM if no memory could be allocated.
+ */
 int __meminit firmware_map_add_hotplug(u64 start, u64 end, const char *type)
 {
 	struct firmware_map_entry *entry;
@@ -325,8 +327,8 @@ int __meminit firmware_map_add_hotplug(u64 start, u64 end, const char *type)
  *
  * That function must be called before late_initcall.
  *
- * Returns 0 on success, or -ENOMEM if no memory could be allocated.
- **/
+ * Return: 0 on success, or -ENOMEM if no memory could be allocated.
+ */
 int __init firmware_map_add_early(u64 start, u64 end, const char *type)
 {
 	struct firmware_map_entry *entry;
@@ -346,8 +348,8 @@ int __init firmware_map_add_early(u64 start, u64 end, const char *type)
  *
  * removes a firmware mapping entry.
  *
- * Returns 0 on success, or -EINVAL if no entry.
- **/
+ * Return: 0 on success, or -EINVAL if no entry.
+ */
 int __meminit firmware_map_remove(u64 start, u64 end, const char *type)
 {
 	struct firmware_map_entry *entry;
diff --git a/drivers/firmware/qcom_scm-32.c b/drivers/firmware/qcom_scm-32.c
new file mode 100644
index 000000000000..1bd6f9c34331
--- /dev/null
+++ b/drivers/firmware/qcom_scm-32.c
@@ -0,0 +1,503 @@
+/* Copyright (c) 2010,2015, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2015 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/qcom_scm.h>
+
+#include <asm/outercache.h>
+#include <asm/cacheflush.h>
+
+#include "qcom_scm.h"
+
+#define QCOM_SCM_FLAG_COLDBOOT_CPU0	0x00
+#define QCOM_SCM_FLAG_COLDBOOT_CPU1	0x01
+#define QCOM_SCM_FLAG_COLDBOOT_CPU2	0x08
+#define QCOM_SCM_FLAG_COLDBOOT_CPU3	0x20
+
+#define QCOM_SCM_FLAG_WARMBOOT_CPU0	0x04
+#define QCOM_SCM_FLAG_WARMBOOT_CPU1	0x02
+#define QCOM_SCM_FLAG_WARMBOOT_CPU2	0x10
+#define QCOM_SCM_FLAG_WARMBOOT_CPU3	0x40
+
+struct qcom_scm_entry {
+	int flag;
+	void *entry;
+};
+
+static struct qcom_scm_entry qcom_scm_wb[] = {
+	{ .flag = QCOM_SCM_FLAG_WARMBOOT_CPU0 },
+	{ .flag = QCOM_SCM_FLAG_WARMBOOT_CPU1 },
+	{ .flag = QCOM_SCM_FLAG_WARMBOOT_CPU2 },
+	{ .flag = QCOM_SCM_FLAG_WARMBOOT_CPU3 },
+};
+
+static DEFINE_MUTEX(qcom_scm_lock);
+
+/**
+ * struct qcom_scm_command - one SCM command buffer
+ * @len: total available memory for command and response
+ * @buf_offset: start of command buffer
+ * @resp_hdr_offset: start of response buffer
+ * @id: command to be executed
+ * @buf: buffer returned from qcom_scm_get_command_buffer()
+ *
+ * An SCM command is laid out in memory as follows:
+ *
+ *	------------------- <--- struct qcom_scm_command
+ *	| command header  |
+ *	------------------- <--- qcom_scm_get_command_buffer()
+ *	| command buffer  |
+ *	------------------- <--- struct qcom_scm_response and
+ *	| response header |      qcom_scm_command_to_response()
+ *	------------------- <--- qcom_scm_get_response_buffer()
+ *	| response buffer |
+ *	-------------------
+ *
+ * There can be arbitrary padding between the headers and buffers so
+ * you should always use the appropriate qcom_scm_get_*_buffer() routines
+ * to access the buffers in a safe manner.
+ */
+struct qcom_scm_command {
+	__le32 len;
+	__le32 buf_offset;
+	__le32 resp_hdr_offset;
+	__le32 id;
+	__le32 buf[0];
+};
+
+/**
+ * struct qcom_scm_response - one SCM response buffer
+ * @len: total available memory for response
+ * @buf_offset: start of response data relative to start of qcom_scm_response
+ * @is_complete: indicates if the command has finished processing
+ */
+struct qcom_scm_response {
+	__le32 len;
+	__le32 buf_offset;
+	__le32 is_complete;
+};
+
+/**
+ * alloc_qcom_scm_command() - Allocate an SCM command
+ * @cmd_size: size of the command buffer
+ * @resp_size: size of the response buffer
+ *
+ * Allocate an SCM command, including enough room for the command
+ * and response headers as well as the command and response buffers.
+ *
+ * Returns a valid &qcom_scm_command on success or %NULL if the allocation fails.
+ */
+static struct qcom_scm_command *alloc_qcom_scm_command(size_t cmd_size, size_t resp_size)
+{
+	struct qcom_scm_command *cmd;
+	size_t len = sizeof(*cmd) + sizeof(struct qcom_scm_response) + cmd_size +
+		resp_size;
+	u32 offset;
+
+	cmd = kzalloc(PAGE_ALIGN(len), GFP_KERNEL);
+	if (cmd) {
+		cmd->len = cpu_to_le32(len);
+		offset = offsetof(struct qcom_scm_command, buf);
+		cmd->buf_offset = cpu_to_le32(offset);
+		cmd->resp_hdr_offset = cpu_to_le32(offset + cmd_size);
+	}
+	return cmd;
+}
+
+/**
+ * free_qcom_scm_command() - Free an SCM command
+ * @cmd: command to free
+ *
+ * Free an SCM command.
+ */
+static inline void free_qcom_scm_command(struct qcom_scm_command *cmd)
+{
+	kfree(cmd);
+}
+
+/**
+ * qcom_scm_command_to_response() - Get a pointer to a qcom_scm_response
+ * @cmd: command
+ *
+ * Returns a pointer to a response for a command.
+ */
+static inline struct qcom_scm_response *qcom_scm_command_to_response(
+		const struct qcom_scm_command *cmd)
+{
+	return (void *)cmd + le32_to_cpu(cmd->resp_hdr_offset);
+}
+
+/**
+ * qcom_scm_get_command_buffer() - Get a pointer to a command buffer
+ * @cmd: command
+ *
+ * Returns a pointer to the command buffer of a command.
+ */
+static inline void *qcom_scm_get_command_buffer(const struct qcom_scm_command *cmd)
+{
+	return (void *)cmd->buf;
+}
+
+/**
+ * qcom_scm_get_response_buffer() - Get a pointer to a response buffer
+ * @rsp: response
+ *
+ * Returns a pointer to a response buffer of a response.
+ */
+static inline void *qcom_scm_get_response_buffer(const struct qcom_scm_response *rsp)
+{
+	return (void *)rsp + le32_to_cpu(rsp->buf_offset);
+}
+
+static int qcom_scm_remap_error(int err)
+{
+	pr_err("qcom_scm_call failed with error code %d\n", err);
+	switch (err) {
+	case QCOM_SCM_ERROR:
+		return -EIO;
+	case QCOM_SCM_EINVAL_ADDR:
+	case QCOM_SCM_EINVAL_ARG:
+		return -EINVAL;
+	case QCOM_SCM_EOPNOTSUPP:
+		return -EOPNOTSUPP;
+	case QCOM_SCM_ENOMEM:
+		return -ENOMEM;
+	}
+	return -EINVAL;
+}
+
+static u32 smc(u32 cmd_addr)
+{
+	int context_id;
+	register u32 r0 asm("r0") = 1;
+	register u32 r1 asm("r1") = (u32)&context_id;
+	register u32 r2 asm("r2") = cmd_addr;
+	do {
+		asm volatile(
+			__asmeq("%0", "r0")
+			__asmeq("%1", "r0")
+			__asmeq("%2", "r1")
+			__asmeq("%3", "r2")
+#ifdef REQUIRES_SEC
+			".arch_extension sec\n"
+#endif
+			"smc	#0	@ switch to secure world\n"
+			: "=r" (r0)
+			: "r" (r0), "r" (r1), "r" (r2)
+			: "r3");
+	} while (r0 == QCOM_SCM_INTERRUPTED);
+
+	return r0;
+}
+
+static int __qcom_scm_call(const struct qcom_scm_command *cmd)
+{
+	int ret;
+	u32 cmd_addr = virt_to_phys(cmd);
+
+	/*
+	 * Flush the command buffer so that the secure world sees
+	 * the correct data.
+	 */
+	__cpuc_flush_dcache_area((void *)cmd, cmd->len);
+	outer_flush_range(cmd_addr, cmd_addr + cmd->len);
+
+	ret = smc(cmd_addr);
+	if (ret < 0)
+		ret = qcom_scm_remap_error(ret);
+
+	return ret;
+}
+
+static void qcom_scm_inv_range(unsigned long start, unsigned long end)
+{
+	u32 cacheline_size, ctr;
+
+	asm volatile("mrc p15, 0, %0, c0, c0, 1" : "=r" (ctr));
+	cacheline_size = 4 << ((ctr >> 16) & 0xf);
+
+	start = round_down(start, cacheline_size);
+	end = round_up(end, cacheline_size);
+	outer_inv_range(start, end);
+	while (start < end) {
+		asm ("mcr p15, 0, %0, c7, c6, 1" : : "r" (start)
+		     : "memory");
+		start += cacheline_size;
+	}
+	dsb();
+	isb();
+}
+
+/**
+ * qcom_scm_call() - Send an SCM command
+ * @svc_id: service identifier
+ * @cmd_id: command identifier
+ * @cmd_buf: command buffer
+ * @cmd_len: length of the command buffer
+ * @resp_buf: response buffer
+ * @resp_len: length of the response buffer
+ *
+ * Sends a command to the SCM and waits for the command to finish processing.
+ *
+ * A note on cache maintenance:
+ * Note that any buffers that are expected to be accessed by the secure world
+ * must be flushed before invoking qcom_scm_call and invalidated in the cache
+ * immediately after qcom_scm_call returns. Cache maintenance on the command
+ * and response buffers is taken care of by qcom_scm_call; however, callers are
+ * responsible for any other cached buffers passed over to the secure world.
+ */
+static int qcom_scm_call(u32 svc_id, u32 cmd_id, const void *cmd_buf,
+			size_t cmd_len, void *resp_buf, size_t resp_len)
+{
+	int ret;
+	struct qcom_scm_command *cmd;
+	struct qcom_scm_response *rsp;
+	unsigned long start, end;
+
+	cmd = alloc_qcom_scm_command(cmd_len, resp_len);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->id = cpu_to_le32((svc_id << 10) | cmd_id);
+	if (cmd_buf)
+		memcpy(qcom_scm_get_command_buffer(cmd), cmd_buf, cmd_len);
+
+	mutex_lock(&qcom_scm_lock);
+	ret = __qcom_scm_call(cmd);
+	mutex_unlock(&qcom_scm_lock);
+	if (ret)
+		goto out;
+
+	rsp = qcom_scm_command_to_response(cmd);
+	start = (unsigned long)rsp;
+
+	do {
+		qcom_scm_inv_range(start, start + sizeof(*rsp));
+	} while (!rsp->is_complete);
+
+	end = (unsigned long)qcom_scm_get_response_buffer(rsp) + resp_len;
+	qcom_scm_inv_range(start, end);
+
+	if (resp_buf)
+		memcpy(resp_buf, qcom_scm_get_response_buffer(rsp), resp_len);
+out:
+	free_qcom_scm_command(cmd);
+	return ret;
+}
+
+#define SCM_CLASS_REGISTER	(0x2 << 8)
+#define SCM_MASK_IRQS		BIT(5)
+#define SCM_ATOMIC(svc, cmd, n) (((((svc) << 10)|((cmd) & 0x3ff)) << 12) | \
+				SCM_CLASS_REGISTER | \
+				SCM_MASK_IRQS | \
+				(n & 0xf))
+
+/**
+ * qcom_scm_call_atomic1() - Send an atomic SCM command with one argument
+ * @svc_id: service identifier
+ * @cmd_id: command identifier
+ * @arg1: first argument
+ *
+ * This shall only be used with commands that are guaranteed to be
+ * uninterruptable, atomic and SMP safe.
+ */
+static s32 qcom_scm_call_atomic1(u32 svc, u32 cmd, u32 arg1)
+{
+	int context_id;
+
+	register u32 r0 asm("r0") = SCM_ATOMIC(svc, cmd, 1);
+	register u32 r1 asm("r1") = (u32)&context_id;
+	register u32 r2 asm("r2") = arg1;
+
+	asm volatile(
+			__asmeq("%0", "r0")
+			__asmeq("%1", "r0")
+			__asmeq("%2", "r1")
+			__asmeq("%3", "r2")
+#ifdef REQUIRES_SEC
+			".arch_extension sec\n"
+#endif
+			"smc    #0      @ switch to secure world\n"
+			: "=r" (r0)
+			: "r" (r0), "r" (r1), "r" (r2)
+			: "r3");
+	return r0;
+}
+
+u32 qcom_scm_get_version(void)
+{
+	int context_id;
+	static u32 version = -1;
+	register u32 r0 asm("r0");
+	register u32 r1 asm("r1");
+
+	if (version != -1)
+		return version;
+
+	mutex_lock(&qcom_scm_lock);
+
+	r0 = 0x1 << 8;
+	r1 = (u32)&context_id;
+	do {
+		asm volatile(
+			__asmeq("%0", "r0")
+			__asmeq("%1", "r1")
+			__asmeq("%2", "r0")
+			__asmeq("%3", "r1")
+#ifdef REQUIRES_SEC
+			".arch_extension sec\n"
+#endif
+			"smc	#0	@ switch to secure world\n"
+			: "=r" (r0), "=r" (r1)
+			: "r" (r0), "r" (r1)
+			: "r2", "r3");
+	} while (r0 == QCOM_SCM_INTERRUPTED);
+
+	version = r1;
+	mutex_unlock(&qcom_scm_lock);
+
+	return version;
+}
+EXPORT_SYMBOL(qcom_scm_get_version);
+
+/*
+ * Set the cold/warm boot address for one of the CPU cores.
+ */
+static int qcom_scm_set_boot_addr(u32 addr, int flags)
+{
+	struct {
+		__le32 flags;
+		__le32 addr;
+	} cmd;
+
+	cmd.addr = cpu_to_le32(addr);
+	cmd.flags = cpu_to_le32(flags);
+	return qcom_scm_call(QCOM_SCM_SVC_BOOT, QCOM_SCM_BOOT_ADDR,
+			&cmd, sizeof(cmd), NULL, 0);
+}
+
+/**
+ * qcom_scm_set_cold_boot_addr() - Set the cold boot address for cpus
+ * @entry: Entry point function for the cpus
+ * @cpus: The cpumask of cpus that will use the entry point
+ *
+ * Set the cold boot address of the cpus. Any cpu outside the supported
+ * range would be removed from the cpu present mask.
+ */
+int __qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus)
+{
+	int flags = 0;
+	int cpu;
+	int scm_cb_flags[] = {
+		QCOM_SCM_FLAG_COLDBOOT_CPU0,
+		QCOM_SCM_FLAG_COLDBOOT_CPU1,
+		QCOM_SCM_FLAG_COLDBOOT_CPU2,
+		QCOM_SCM_FLAG_COLDBOOT_CPU3,
+	};
+
+	if (!cpus || (cpus && cpumask_empty(cpus)))
+		return -EINVAL;
+
+	for_each_cpu(cpu, cpus) {
+		if (cpu < ARRAY_SIZE(scm_cb_flags))
+			flags |= scm_cb_flags[cpu];
+		else
+			set_cpu_present(cpu, false);
+	}
+
+	return qcom_scm_set_boot_addr(virt_to_phys(entry), flags);
+}
+
+/**
+ * qcom_scm_set_warm_boot_addr() - Set the warm boot address for cpus
+ * @entry: Entry point function for the cpus
+ * @cpus: The cpumask of cpus that will use the entry point
+ *
+ * Set the Linux entry point for the SCM to transfer control to when coming
+ * out of a power down. CPU power down may be executed on cpuidle or hotplug.
+ */
+int __qcom_scm_set_warm_boot_addr(void *entry, const cpumask_t *cpus)
+{
+	int ret;
+	int flags = 0;
+	int cpu;
+
+	/*
+	 * Reassign only if we are switching from hotplug entry point
+	 * to cpuidle entry point or vice versa.
+	 */
+	for_each_cpu(cpu, cpus) {
+		if (entry == qcom_scm_wb[cpu].entry)
+			continue;
+		flags |= qcom_scm_wb[cpu].flag;
+	}
+
+	/* No change in entry function */
+	if (!flags)
+		return 0;
+
+	ret = qcom_scm_set_boot_addr(virt_to_phys(entry), flags);
+	if (!ret) {
+		for_each_cpu(cpu, cpus)
+			qcom_scm_wb[cpu].entry = entry;
+	}
+
+	return ret;
+}
+
+/**
+ * qcom_scm_cpu_power_down() - Power down the cpu
+ * @flags - Flags to flush cache
+ *
+ * This is an end point to power down cpu. If there was a pending interrupt,
+ * the control would return from this function, otherwise, the cpu jumps to the
+ * warm boot entry point set for this cpu upon reset.
+ */
+void __qcom_scm_cpu_power_down(u32 flags)
+{
+	qcom_scm_call_atomic1(QCOM_SCM_SVC_BOOT, QCOM_SCM_CMD_TERMINATE_PC,
+			flags & QCOM_SCM_FLUSH_FLAG_MASK);
+}
+
+int __qcom_scm_is_call_available(u32 svc_id, u32 cmd_id)
+{
+	int ret;
+	u32 svc_cmd = (svc_id << 10) | cmd_id;
+	u32 ret_val = 0;
+
+	ret = qcom_scm_call(QCOM_SCM_SVC_INFO, QCOM_IS_CALL_AVAIL_CMD, &svc_cmd,
+			sizeof(svc_cmd), &ret_val, sizeof(ret_val));
+	if (ret)
+		return ret;
+
+	return ret_val;
+}
+
+int __qcom_scm_hdcp_req(struct qcom_scm_hdcp_req *req, u32 req_cnt, u32 *resp)
+{
+	if (req_cnt > QCOM_SCM_HDCP_MAX_REQ_CNT)
+		return -ERANGE;
+
+	return qcom_scm_call(QCOM_SCM_SVC_HDCP, QCOM_SCM_CMD_HDCP,
+		req, req_cnt * sizeof(*req), resp, sizeof(*resp));
+}
diff --git a/drivers/firmware/qcom_scm.c b/drivers/firmware/qcom_scm.c
index 994b50fd997c..45c008d68891 100644
--- a/drivers/firmware/qcom_scm.c
+++ b/drivers/firmware/qcom_scm.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2010, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2010,2015, The Linux Foundation. All rights reserved.
  * Copyright (C) 2015 Linaro Ltd.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -16,393 +16,12 @@
  * 02110-1301, USA.
  */
 
-#include <linux/slab.h>
-#include <linux/io.h>
-#include <linux/module.h>
-#include <linux/mutex.h>
-#include <linux/errno.h>
-#include <linux/err.h>
+#include <linux/cpumask.h>
+#include <linux/export.h>
+#include <linux/types.h>
 #include <linux/qcom_scm.h>
 
-#include <asm/outercache.h>
-#include <asm/cacheflush.h>
-
-
-#define QCOM_SCM_ENOMEM		-5
-#define QCOM_SCM_EOPNOTSUPP	-4
-#define QCOM_SCM_EINVAL_ADDR	-3
-#define QCOM_SCM_EINVAL_ARG	-2
-#define QCOM_SCM_ERROR		-1
-#define QCOM_SCM_INTERRUPTED	1
-
-#define QCOM_SCM_FLAG_COLDBOOT_CPU0	0x00
-#define QCOM_SCM_FLAG_COLDBOOT_CPU1	0x01
-#define QCOM_SCM_FLAG_COLDBOOT_CPU2	0x08
-#define QCOM_SCM_FLAG_COLDBOOT_CPU3	0x20
-
-#define QCOM_SCM_FLAG_WARMBOOT_CPU0	0x04
-#define QCOM_SCM_FLAG_WARMBOOT_CPU1	0x02
-#define QCOM_SCM_FLAG_WARMBOOT_CPU2	0x10
-#define QCOM_SCM_FLAG_WARMBOOT_CPU3	0x40
-
-struct qcom_scm_entry {
-	int flag;
-	void *entry;
-};
-
-static struct qcom_scm_entry qcom_scm_wb[] = {
-	{ .flag = QCOM_SCM_FLAG_WARMBOOT_CPU0 },
-	{ .flag = QCOM_SCM_FLAG_WARMBOOT_CPU1 },
-	{ .flag = QCOM_SCM_FLAG_WARMBOOT_CPU2 },
-	{ .flag = QCOM_SCM_FLAG_WARMBOOT_CPU3 },
-};
-
-static DEFINE_MUTEX(qcom_scm_lock);
-
-/**
- * struct qcom_scm_command - one SCM command buffer
- * @len: total available memory for command and response
- * @buf_offset: start of command buffer
- * @resp_hdr_offset: start of response buffer
- * @id: command to be executed
- * @buf: buffer returned from qcom_scm_get_command_buffer()
- *
- * An SCM command is laid out in memory as follows:
- *
- *	------------------- <--- struct qcom_scm_command
- *	| command header  |
- *	------------------- <--- qcom_scm_get_command_buffer()
- *	| command buffer  |
- *	------------------- <--- struct qcom_scm_response and
- *	| response header |      qcom_scm_command_to_response()
- *	------------------- <--- qcom_scm_get_response_buffer()
- *	| response buffer |
- *	-------------------
- *
- * There can be arbitrary padding between the headers and buffers so
- * you should always use the appropriate qcom_scm_get_*_buffer() routines
- * to access the buffers in a safe manner.
- */
-struct qcom_scm_command {
-	__le32 len;
-	__le32 buf_offset;
-	__le32 resp_hdr_offset;
-	__le32 id;
-	__le32 buf[0];
-};
-
-/**
- * struct qcom_scm_response - one SCM response buffer
- * @len: total available memory for response
- * @buf_offset: start of response data relative to start of qcom_scm_response
- * @is_complete: indicates if the command has finished processing
- */
-struct qcom_scm_response {
-	__le32 len;
-	__le32 buf_offset;
-	__le32 is_complete;
-};
-
-/**
- * alloc_qcom_scm_command() - Allocate an SCM command
- * @cmd_size: size of the command buffer
- * @resp_size: size of the response buffer
- *
- * Allocate an SCM command, including enough room for the command
- * and response headers as well as the command and response buffers.
- *
- * Returns a valid &qcom_scm_command on success or %NULL if the allocation fails.
- */
-static struct qcom_scm_command *alloc_qcom_scm_command(size_t cmd_size, size_t resp_size)
-{
-	struct qcom_scm_command *cmd;
-	size_t len = sizeof(*cmd) + sizeof(struct qcom_scm_response) + cmd_size +
-		resp_size;
-	u32 offset;
-
-	cmd = kzalloc(PAGE_ALIGN(len), GFP_KERNEL);
-	if (cmd) {
-		cmd->len = cpu_to_le32(len);
-		offset = offsetof(struct qcom_scm_command, buf);
-		cmd->buf_offset = cpu_to_le32(offset);
-		cmd->resp_hdr_offset = cpu_to_le32(offset + cmd_size);
-	}
-	return cmd;
-}
-
-/**
- * free_qcom_scm_command() - Free an SCM command
- * @cmd: command to free
- *
- * Free an SCM command.
- */
-static inline void free_qcom_scm_command(struct qcom_scm_command *cmd)
-{
-	kfree(cmd);
-}
-
-/**
- * qcom_scm_command_to_response() - Get a pointer to a qcom_scm_response
- * @cmd: command
- *
- * Returns a pointer to a response for a command.
- */
-static inline struct qcom_scm_response *qcom_scm_command_to_response(
-		const struct qcom_scm_command *cmd)
-{
-	return (void *)cmd + le32_to_cpu(cmd->resp_hdr_offset);
-}
-
-/**
- * qcom_scm_get_command_buffer() - Get a pointer to a command buffer
- * @cmd: command
- *
- * Returns a pointer to the command buffer of a command.
- */
-static inline void *qcom_scm_get_command_buffer(const struct qcom_scm_command *cmd)
-{
-	return (void *)cmd->buf;
-}
-
-/**
- * qcom_scm_get_response_buffer() - Get a pointer to a response buffer
- * @rsp: response
- *
- * Returns a pointer to a response buffer of a response.
- */
-static inline void *qcom_scm_get_response_buffer(const struct qcom_scm_response *rsp)
-{
-	return (void *)rsp + le32_to_cpu(rsp->buf_offset);
-}
-
-static int qcom_scm_remap_error(int err)
-{
-	pr_err("qcom_scm_call failed with error code %d\n", err);
-	switch (err) {
-	case QCOM_SCM_ERROR:
-		return -EIO;
-	case QCOM_SCM_EINVAL_ADDR:
-	case QCOM_SCM_EINVAL_ARG:
-		return -EINVAL;
-	case QCOM_SCM_EOPNOTSUPP:
-		return -EOPNOTSUPP;
-	case QCOM_SCM_ENOMEM:
-		return -ENOMEM;
-	}
-	return -EINVAL;
-}
-
-static u32 smc(u32 cmd_addr)
-{
-	int context_id;
-	register u32 r0 asm("r0") = 1;
-	register u32 r1 asm("r1") = (u32)&context_id;
-	register u32 r2 asm("r2") = cmd_addr;
-	do {
-		asm volatile(
-			__asmeq("%0", "r0")
-			__asmeq("%1", "r0")
-			__asmeq("%2", "r1")
-			__asmeq("%3", "r2")
-#ifdef REQUIRES_SEC
-			".arch_extension sec\n"
-#endif
-			"smc	#0	@ switch to secure world\n"
-			: "=r" (r0)
-			: "r" (r0), "r" (r1), "r" (r2)
-			: "r3");
-	} while (r0 == QCOM_SCM_INTERRUPTED);
-
-	return r0;
-}
-
-static int __qcom_scm_call(const struct qcom_scm_command *cmd)
-{
-	int ret;
-	u32 cmd_addr = virt_to_phys(cmd);
-
-	/*
-	 * Flush the command buffer so that the secure world sees
-	 * the correct data.
-	 */
-	__cpuc_flush_dcache_area((void *)cmd, cmd->len);
-	outer_flush_range(cmd_addr, cmd_addr + cmd->len);
-
-	ret = smc(cmd_addr);
-	if (ret < 0)
-		ret = qcom_scm_remap_error(ret);
-
-	return ret;
-}
-
-static void qcom_scm_inv_range(unsigned long start, unsigned long end)
-{
-	u32 cacheline_size, ctr;
-
-	asm volatile("mrc p15, 0, %0, c0, c0, 1" : "=r" (ctr));
-	cacheline_size = 4 << ((ctr >> 16) & 0xf);
-
-	start = round_down(start, cacheline_size);
-	end = round_up(end, cacheline_size);
-	outer_inv_range(start, end);
-	while (start < end) {
-		asm ("mcr p15, 0, %0, c7, c6, 1" : : "r" (start)
-		     : "memory");
-		start += cacheline_size;
-	}
-	dsb();
-	isb();
-}
-
-/**
- * qcom_scm_call() - Send an SCM command
- * @svc_id: service identifier
- * @cmd_id: command identifier
- * @cmd_buf: command buffer
- * @cmd_len: length of the command buffer
- * @resp_buf: response buffer
- * @resp_len: length of the response buffer
- *
- * Sends a command to the SCM and waits for the command to finish processing.
- *
- * A note on cache maintenance:
- * Note that any buffers that are expected to be accessed by the secure world
- * must be flushed before invoking qcom_scm_call and invalidated in the cache
- * immediately after qcom_scm_call returns. Cache maintenance on the command
- * and response buffers is taken care of by qcom_scm_call; however, callers are
- * responsible for any other cached buffers passed over to the secure world.
- */
-static int qcom_scm_call(u32 svc_id, u32 cmd_id, const void *cmd_buf,
-			size_t cmd_len, void *resp_buf, size_t resp_len)
-{
-	int ret;
-	struct qcom_scm_command *cmd;
-	struct qcom_scm_response *rsp;
-	unsigned long start, end;
-
-	cmd = alloc_qcom_scm_command(cmd_len, resp_len);
-	if (!cmd)
-		return -ENOMEM;
-
-	cmd->id = cpu_to_le32((svc_id << 10) | cmd_id);
-	if (cmd_buf)
-		memcpy(qcom_scm_get_command_buffer(cmd), cmd_buf, cmd_len);
-
-	mutex_lock(&qcom_scm_lock);
-	ret = __qcom_scm_call(cmd);
-	mutex_unlock(&qcom_scm_lock);
-	if (ret)
-		goto out;
-
-	rsp = qcom_scm_command_to_response(cmd);
-	start = (unsigned long)rsp;
-
-	do {
-		qcom_scm_inv_range(start, start + sizeof(*rsp));
-	} while (!rsp->is_complete);
-
-	end = (unsigned long)qcom_scm_get_response_buffer(rsp) + resp_len;
-	qcom_scm_inv_range(start, end);
-
-	if (resp_buf)
-		memcpy(resp_buf, qcom_scm_get_response_buffer(rsp), resp_len);
-out:
-	free_qcom_scm_command(cmd);
-	return ret;
-}
-
-#define SCM_CLASS_REGISTER	(0x2 << 8)
-#define SCM_MASK_IRQS		BIT(5)
-#define SCM_ATOMIC(svc, cmd, n) (((((svc) << 10)|((cmd) & 0x3ff)) << 12) | \
-				SCM_CLASS_REGISTER | \
-				SCM_MASK_IRQS | \
-				(n & 0xf))
-
-/**
- * qcom_scm_call_atomic1() - Send an atomic SCM command with one argument
- * @svc_id: service identifier
- * @cmd_id: command identifier
- * @arg1: first argument
- *
- * This shall only be used with commands that are guaranteed to be
- * uninterruptable, atomic and SMP safe.
- */
-static s32 qcom_scm_call_atomic1(u32 svc, u32 cmd, u32 arg1)
-{
-	int context_id;
-
-	register u32 r0 asm("r0") = SCM_ATOMIC(svc, cmd, 1);
-	register u32 r1 asm("r1") = (u32)&context_id;
-	register u32 r2 asm("r2") = arg1;
-
-	asm volatile(
-			__asmeq("%0", "r0")
-			__asmeq("%1", "r0")
-			__asmeq("%2", "r1")
-			__asmeq("%3", "r2")
-#ifdef REQUIRES_SEC
-			".arch_extension sec\n"
-#endif
-			"smc    #0      @ switch to secure world\n"
-			: "=r" (r0)
-			: "r" (r0), "r" (r1), "r" (r2)
-			: "r3");
-	return r0;
-}
-
-u32 qcom_scm_get_version(void)
-{
-	int context_id;
-	static u32 version = -1;
-	register u32 r0 asm("r0");
-	register u32 r1 asm("r1");
-
-	if (version != -1)
-		return version;
-
-	mutex_lock(&qcom_scm_lock);
-
-	r0 = 0x1 << 8;
-	r1 = (u32)&context_id;
-	do {
-		asm volatile(
-			__asmeq("%0", "r0")
-			__asmeq("%1", "r1")
-			__asmeq("%2", "r0")
-			__asmeq("%3", "r1")
-#ifdef REQUIRES_SEC
-			".arch_extension sec\n"
-#endif
-			"smc	#0	@ switch to secure world\n"
-			: "=r" (r0), "=r" (r1)
-			: "r" (r0), "r" (r1)
-			: "r2", "r3");
-	} while (r0 == QCOM_SCM_INTERRUPTED);
-
-	version = r1;
-	mutex_unlock(&qcom_scm_lock);
-
-	return version;
-}
-EXPORT_SYMBOL(qcom_scm_get_version);
-
-#define QCOM_SCM_SVC_BOOT			0x1
-#define QCOM_SCM_BOOT_ADDR			0x1
-/*
- * Set the cold/warm boot address for one of the CPU cores.
- */
-static int qcom_scm_set_boot_addr(u32 addr, int flags)
-{
-	struct {
-		__le32 flags;
-		__le32 addr;
-	} cmd;
-
-	cmd.addr = cpu_to_le32(addr);
-	cmd.flags = cpu_to_le32(flags);
-	return qcom_scm_call(QCOM_SCM_SVC_BOOT, QCOM_SCM_BOOT_ADDR,
-			&cmd, sizeof(cmd), NULL, 0);
-}
+#include "qcom_scm.h"
 
 /**
  * qcom_scm_set_cold_boot_addr() - Set the cold boot address for cpus
@@ -414,26 +33,7 @@ static int qcom_scm_set_boot_addr(u32 addr, int flags)
  */
 int qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus)
 {
-	int flags = 0;
-	int cpu;
-	int scm_cb_flags[] = {
-		QCOM_SCM_FLAG_COLDBOOT_CPU0,
-		QCOM_SCM_FLAG_COLDBOOT_CPU1,
-		QCOM_SCM_FLAG_COLDBOOT_CPU2,
-		QCOM_SCM_FLAG_COLDBOOT_CPU3,
-	};
-
-	if (!cpus || (cpus && cpumask_empty(cpus)))
-		return -EINVAL;
-
-	for_each_cpu(cpu, cpus) {
-		if (cpu < ARRAY_SIZE(scm_cb_flags))
-			flags |= scm_cb_flags[cpu];
-		else
-			set_cpu_present(cpu, false);
-	}
-
-	return qcom_scm_set_boot_addr(virt_to_phys(entry), flags);
+	return __qcom_scm_set_cold_boot_addr(entry, cpus);
 }
 EXPORT_SYMBOL(qcom_scm_set_cold_boot_addr);
 
@@ -447,37 +47,10 @@ EXPORT_SYMBOL(qcom_scm_set_cold_boot_addr);
  */
 int qcom_scm_set_warm_boot_addr(void *entry, const cpumask_t *cpus)
 {
-	int ret;
-	int flags = 0;
-	int cpu;
-
-	/*
-	 * Reassign only if we are switching from hotplug entry point
-	 * to cpuidle entry point or vice versa.
-	 */
-	for_each_cpu(cpu, cpus) {
-		if (entry == qcom_scm_wb[cpu].entry)
-			continue;
-		flags |= qcom_scm_wb[cpu].flag;
-	}
-
-	/* No change in entry function */
-	if (!flags)
-		return 0;
-
-	ret = qcom_scm_set_boot_addr(virt_to_phys(entry), flags);
-	if (!ret) {
-		for_each_cpu(cpu, cpus)
-			qcom_scm_wb[cpu].entry = entry;
-	}
-
-	return ret;
+	return __qcom_scm_set_warm_boot_addr(entry, cpus);
 }
 EXPORT_SYMBOL(qcom_scm_set_warm_boot_addr);
 
-#define QCOM_SCM_CMD_TERMINATE_PC	0x2
-#define QCOM_SCM_FLUSH_FLAG_MASK	0x3
-
 /**
  * qcom_scm_cpu_power_down() - Power down the cpu
  * @flags - Flags to flush cache
@@ -488,7 +61,36 @@ EXPORT_SYMBOL(qcom_scm_set_warm_boot_addr);
  */
 void qcom_scm_cpu_power_down(u32 flags)
 {
-	qcom_scm_call_atomic1(QCOM_SCM_SVC_BOOT, QCOM_SCM_CMD_TERMINATE_PC,
-			flags & QCOM_SCM_FLUSH_FLAG_MASK);
+	__qcom_scm_cpu_power_down(flags);
 }
 EXPORT_SYMBOL(qcom_scm_cpu_power_down);
+
+/**
+ * qcom_scm_hdcp_available() - Check if secure environment supports HDCP.
+ *
+ * Return true if HDCP is supported, false if not.
+ */
+bool qcom_scm_hdcp_available(void)
+{
+	int ret;
+
+	ret = __qcom_scm_is_call_available(QCOM_SCM_SVC_HDCP,
+		QCOM_SCM_CMD_HDCP);
+
+	return (ret > 0) ? true : false;
+}
+EXPORT_SYMBOL(qcom_scm_hdcp_available);
+
+/**
+ * qcom_scm_hdcp_req() - Send HDCP request.
+ * @req: HDCP request array
+ * @req_cnt: HDCP request array count
+ * @resp: response buffer passed to SCM
+ *
+ * Write HDCP register(s) through SCM.
+ */
+int qcom_scm_hdcp_req(struct qcom_scm_hdcp_req *req, u32 req_cnt, u32 *resp)
+{
+	return __qcom_scm_hdcp_req(req, req_cnt, resp);
+}
+EXPORT_SYMBOL(qcom_scm_hdcp_req);
diff --git a/drivers/firmware/qcom_scm.h b/drivers/firmware/qcom_scm.h
new file mode 100644
index 000000000000..2cce75c08b99
--- /dev/null
+++ b/drivers/firmware/qcom_scm.h
@@ -0,0 +1,47 @@
+/* Copyright (c) 2010-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 __QCOM_SCM_INT_H
+#define __QCOM_SCM_INT_H
+
+#define QCOM_SCM_SVC_BOOT		0x1
+#define QCOM_SCM_BOOT_ADDR		0x1
+#define QCOM_SCM_BOOT_ADDR_MC		0x11
+
+#define QCOM_SCM_FLAG_HLOS		0x01
+#define QCOM_SCM_FLAG_COLDBOOT_MC	0x02
+#define QCOM_SCM_FLAG_WARMBOOT_MC	0x04
+extern int __qcom_scm_set_warm_boot_addr(void *entry, const cpumask_t *cpus);
+extern int __qcom_scm_set_cold_boot_addr(void *entry, const cpumask_t *cpus);
+
+#define QCOM_SCM_CMD_TERMINATE_PC	0x2
+#define QCOM_SCM_FLUSH_FLAG_MASK	0x3
+#define QCOM_SCM_CMD_CORE_HOTPLUGGED	0x10
+extern void __qcom_scm_cpu_power_down(u32 flags);
+
+#define QCOM_SCM_SVC_INFO		0x6
+#define QCOM_IS_CALL_AVAIL_CMD		0x1
+extern int __qcom_scm_is_call_available(u32 svc_id, u32 cmd_id);
+
+#define QCOM_SCM_SVC_HDCP		0x11
+#define QCOM_SCM_CMD_HDCP		0x01
+extern int __qcom_scm_hdcp_req(struct qcom_scm_hdcp_req *req, u32 req_cnt,
+		u32 *resp);
+
+/* common error codes */
+#define QCOM_SCM_ENOMEM		-5
+#define QCOM_SCM_EOPNOTSUPP	-4
+#define QCOM_SCM_EINVAL_ADDR	-3
+#define QCOM_SCM_EINVAL_ARG	-2
+#define QCOM_SCM_ERROR		-1
+#define QCOM_SCM_INTERRUPTED	1
+
+#endif
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index caefe806db5e..8f1fe739c985 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -126,6 +126,14 @@ config GPIO_BCM_KONA
 	help
 	  Turn on GPIO support for Broadcom "Kona" chips.
 
+config GPIO_BRCMSTB
+	tristate "BRCMSTB GPIO support"
+	default y if ARCH_BRCMSTB
+	depends on OF_GPIO && (ARCH_BRCMSTB || COMPILE_TEST)
+	select GPIO_GENERIC
+	help
+	  Say yes here to enable GPIO support for Broadcom STB (BCM7XXX) SoCs.
+
 config GPIO_CLPS711X
 	tristate "CLPS711X GPIO support"
 	depends on ARCH_CLPS711X || COMPILE_TEST
@@ -159,6 +167,14 @@ config GPIO_EP93XX
 	depends on ARCH_EP93XX
 	select GPIO_GENERIC
 
+config GPIO_ETRAXFS
+	bool "Axis ETRAX FS General I/O"
+	depends on CRIS || COMPILE_TEST
+	depends on OF
+	select GPIO_GENERIC
+	help
+	  Say yes here to support the GPIO controller on Axis ETRAX FS SoCs.
+
 config GPIO_F7188X
 	tristate "F71869, F71869A, F71882FG and F71889F GPIO support"
 	depends on X86
@@ -230,6 +246,14 @@ config GPIO_LOONGSON
 	help
 	  driver for GPIO functionality on Loongson-2F/3A/3B processors.
 
+config GPIO_LPC18XX
+	bool "NXP LPC18XX/43XX GPIO support"
+	default y if ARCH_LPC18XX
+	depends on OF_GPIO && (ARCH_LPC18XX || COMPILE_TEST)
+	help
+	  Select this option to enable GPIO driver for
+	  NXP LPC18XX/43XX devices.
+
 config GPIO_LYNXPOINT
 	tristate "Intel Lynxpoint GPIO support"
 	depends on ACPI && X86
@@ -308,7 +332,7 @@ config GPIO_OCTEON
 	  family of SOCs.
 
 config GPIO_OMAP
-	bool "TI OMAP GPIO support" if COMPILE_TEST && !ARCH_OMAP2PLUS
+	tristate "TI OMAP GPIO support" if ARCH_OMAP2PLUS || COMPILE_TEST
 	default y if ARCH_OMAP
 	depends on ARM
 	select GENERIC_IRQ_CHIP
@@ -488,6 +512,17 @@ config GPIO_XILINX
 	help
 	  Say yes here to support the Xilinx FPGA GPIO device
 
+config GPIO_XLP
+	tristate "Netlogic XLP GPIO support"
+	depends on CPU_XLP
+	select GPIOLIB_IRQCHIP
+	help
+	  This driver provides support for GPIO interface on Netlogic XLP MIPS64
+	  SoCs. Currently supported XLP variants are XLP8XX, XLP3XX, XLP2XX,
+	  XLP9XX and XLP5XX.
+
+	  If unsure, say N.
+
 config GPIO_XTENSA
 	bool "Xtensa GPIO32 support"
 	depends on XTENSA
@@ -505,7 +540,7 @@ config GPIO_ZEVIO
 
 config GPIO_ZYNQ
 	tristate "Xilinx Zynq GPIO support"
-	depends on ARCH_ZYNQ
+	depends on ARCH_ZYNQ || ARCH_ZYNQMP
 	select GPIOLIB_IRQCHIP
 	help
 	  Say yes here to support Xilinx Zynq GPIO controller.
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index f71bb971329c..f82cd678ce08 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_GPIO_ALTERA)  	+= gpio-altera.o
 obj-$(CONFIG_GPIO_AMD8111)	+= gpio-amd8111.o
 obj-$(CONFIG_GPIO_ARIZONA)	+= gpio-arizona.o
 obj-$(CONFIG_GPIO_BCM_KONA)	+= gpio-bcm-kona.o
+obj-$(CONFIG_GPIO_BRCMSTB)	+= gpio-brcmstb.o
 obj-$(CONFIG_GPIO_BT8XX)	+= gpio-bt8xx.o
 obj-$(CONFIG_GPIO_CLPS711X)	+= gpio-clps711x.o
 obj-$(CONFIG_GPIO_CS5535)	+= gpio-cs5535.o
@@ -32,6 +33,7 @@ obj-$(CONFIG_GPIO_DLN2)		+= gpio-dln2.o
 obj-$(CONFIG_GPIO_DWAPB)	+= gpio-dwapb.o
 obj-$(CONFIG_GPIO_EM)		+= gpio-em.o
 obj-$(CONFIG_GPIO_EP93XX)	+= gpio-ep93xx.o
+obj-$(CONFIG_GPIO_ETRAXFS)	+= gpio-etraxfs.o
 obj-$(CONFIG_GPIO_F7188X)	+= gpio-f7188x.o
 obj-$(CONFIG_GPIO_GE_FPGA)	+= gpio-ge.o
 obj-$(CONFIG_GPIO_GRGPIO)	+= gpio-grgpio.o
@@ -44,6 +46,7 @@ obj-$(CONFIG_ARCH_KS8695)	+= gpio-ks8695.o
 obj-$(CONFIG_GPIO_INTEL_MID)	+= gpio-intel-mid.o
 obj-$(CONFIG_GPIO_LOONGSON)	+= gpio-loongson.o
 obj-$(CONFIG_GPIO_LP3943)	+= gpio-lp3943.o
+obj-$(CONFIG_GPIO_LPC18XX)	+= gpio-lpc18xx.o
 obj-$(CONFIG_ARCH_LPC32XX)	+= gpio-lpc32xx.o
 obj-$(CONFIG_GPIO_LYNXPOINT)	+= gpio-lynxpoint.o
 obj-$(CONFIG_GPIO_MAX730X)	+= gpio-max730x.o
@@ -109,6 +112,7 @@ obj-$(CONFIG_GPIO_WM8994)	+= gpio-wm8994.o
 obj-$(CONFIG_GPIO_XGENE)	+= gpio-xgene.o
 obj-$(CONFIG_GPIO_XGENE_SB)	+= gpio-xgene-sb.o
 obj-$(CONFIG_GPIO_XILINX)	+= gpio-xilinx.o
+obj-$(CONFIG_GPIO_XLP)		+= gpio-xlp.o
 obj-$(CONFIG_GPIO_XTENSA)	+= gpio-xtensa.o
 obj-$(CONFIG_GPIO_ZEVIO)	+= gpio-zevio.o
 obj-$(CONFIG_GPIO_ZYNQ)		+= gpio-zynq.o
diff --git a/drivers/gpio/gpio-altera.c b/drivers/gpio/gpio-altera.c
index 449fb46cb8a0..0f3d336d6303 100644
--- a/drivers/gpio/gpio-altera.c
+++ b/drivers/gpio/gpio-altera.c
@@ -107,7 +107,8 @@ static int altera_gpio_irq_set_type(struct irq_data *d,
 	return -EINVAL;
 }
 
-static unsigned int altera_gpio_irq_startup(struct irq_data *d) {
+static unsigned int altera_gpio_irq_startup(struct irq_data *d)
+{
 	altera_gpio_irq_unmask(d);
 
 	return 0;
diff --git a/drivers/gpio/gpio-bcm-kona.c b/drivers/gpio/gpio-bcm-kona.c
index b164ce837b43..40343fa92c7b 100644
--- a/drivers/gpio/gpio-bcm-kona.c
+++ b/drivers/gpio/gpio-bcm-kona.c
@@ -122,6 +122,16 @@ static void bcm_kona_gpio_unlock_gpio(struct bcm_kona_gpio *kona_gpio,
 	spin_unlock_irqrestore(&kona_gpio->lock, flags);
 }
 
+static int bcm_kona_gpio_get_dir(struct gpio_chip *chip, unsigned gpio)
+{
+	struct bcm_kona_gpio *kona_gpio = to_kona_gpio(chip);
+	void __iomem *reg_base = kona_gpio->reg_base;
+	u32 val;
+
+	val = readl(reg_base + GPIO_CONTROL(gpio)) & GPIO_GPCTR0_IOTR_MASK;
+	return val ? GPIOF_DIR_IN : GPIOF_DIR_OUT;
+}
+
 static void bcm_kona_gpio_set(struct gpio_chip *chip, unsigned gpio, int value)
 {
 	struct bcm_kona_gpio *kona_gpio;
@@ -135,12 +145,8 @@ static void bcm_kona_gpio_set(struct gpio_chip *chip, unsigned gpio, int value)
 	reg_base = kona_gpio->reg_base;
 	spin_lock_irqsave(&kona_gpio->lock, flags);
 
-	/* determine the GPIO pin direction */
-	val = readl(reg_base + GPIO_CONTROL(gpio));
-	val &= GPIO_GPCTR0_IOTR_MASK;
-
 	/* this function only applies to output pin */
-	if (GPIO_GPCTR0_IOTR_CMD_INPUT == val)
+	if (bcm_kona_gpio_get_dir(chip, gpio) == GPIOF_DIR_IN)
 		goto out;
 
 	reg_offset = value ? GPIO_OUT_SET(bank_id) : GPIO_OUT_CLEAR(bank_id);
@@ -166,13 +172,12 @@ static int bcm_kona_gpio_get(struct gpio_chip *chip, unsigned gpio)
 	reg_base = kona_gpio->reg_base;
 	spin_lock_irqsave(&kona_gpio->lock, flags);
 
-	/* determine the GPIO pin direction */
-	val = readl(reg_base + GPIO_CONTROL(gpio));
-	val &= GPIO_GPCTR0_IOTR_MASK;
+	if (bcm_kona_gpio_get_dir(chip, gpio) == GPIOF_DIR_IN)
+		reg_offset = GPIO_IN_STATUS(bank_id);
+	else
+		reg_offset = GPIO_OUT_STATUS(bank_id);
 
 	/* read the GPIO bank status */
-	reg_offset = (GPIO_GPCTR0_IOTR_CMD_INPUT == val) ?
-	    GPIO_IN_STATUS(bank_id) : GPIO_OUT_STATUS(bank_id);
 	val = readl(reg_base + reg_offset);
 
 	spin_unlock_irqrestore(&kona_gpio->lock, flags);
@@ -310,6 +315,7 @@ static struct gpio_chip template_chip = {
 	.owner = THIS_MODULE,
 	.request = bcm_kona_gpio_request,
 	.free = bcm_kona_gpio_free,
+	.get_direction = bcm_kona_gpio_get_dir,
 	.direction_input = bcm_kona_gpio_direction_input,
 	.get = bcm_kona_gpio_get,
 	.direction_output = bcm_kona_gpio_direction_output,
@@ -534,7 +540,7 @@ static void bcm_kona_gpio_irq_unmap(struct irq_domain *d, unsigned int irq)
 	irq_set_chip_data(irq, NULL);
 }
 
-static struct irq_domain_ops bcm_kona_irq_ops = {
+static const struct irq_domain_ops bcm_kona_irq_ops = {
 	.map = bcm_kona_gpio_irq_map,
 	.unmap = bcm_kona_gpio_irq_unmap,
 	.xlate = irq_domain_xlate_twocell,
@@ -651,8 +657,9 @@ static int bcm_kona_gpio_probe(struct platform_device *pdev)
 	}
 	for (i = 0; i < kona_gpio->num_bank; i++) {
 		bank = &kona_gpio->banks[i];
-		irq_set_chained_handler(bank->irq, bcm_kona_gpio_irq_handler);
-		irq_set_handler_data(bank->irq, bank);
+		irq_set_chained_handler_and_data(bank->irq,
+						 bcm_kona_gpio_irq_handler,
+						 bank);
 	}
 
 	spin_lock_init(&kona_gpio->lock);
diff --git a/drivers/gpio/gpio-brcmstb.c b/drivers/gpio/gpio-brcmstb.c
new file mode 100644
index 000000000000..7a3cb1fa0a76
--- /dev/null
+++ b/drivers/gpio/gpio-brcmstb.c
@@ -0,0 +1,252 @@
+/*
+ * Copyright (C) 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/bitops.h>
+#include <linux/gpio/driver.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+#include <linux/module.h>
+#include <linux/basic_mmio_gpio.h>
+
+#define GIO_BANK_SIZE           0x20
+#define GIO_ODEN(bank)          (((bank) * GIO_BANK_SIZE) + 0x00)
+#define GIO_DATA(bank)          (((bank) * GIO_BANK_SIZE) + 0x04)
+#define GIO_IODIR(bank)         (((bank) * GIO_BANK_SIZE) + 0x08)
+#define GIO_EC(bank)            (((bank) * GIO_BANK_SIZE) + 0x0c)
+#define GIO_EI(bank)            (((bank) * GIO_BANK_SIZE) + 0x10)
+#define GIO_MASK(bank)          (((bank) * GIO_BANK_SIZE) + 0x14)
+#define GIO_LEVEL(bank)         (((bank) * GIO_BANK_SIZE) + 0x18)
+#define GIO_STAT(bank)          (((bank) * GIO_BANK_SIZE) + 0x1c)
+
+struct brcmstb_gpio_bank {
+	struct list_head node;
+	int id;
+	struct bgpio_chip bgc;
+	struct brcmstb_gpio_priv *parent_priv;
+	u32 width;
+};
+
+struct brcmstb_gpio_priv {
+	struct list_head bank_list;
+	void __iomem *reg_base;
+	int num_banks;
+	struct platform_device *pdev;
+	int gpio_base;
+};
+
+#define MAX_GPIO_PER_BANK           32
+#define GPIO_BANK(gpio)         ((gpio) >> 5)
+/* assumes MAX_GPIO_PER_BANK is a multiple of 2 */
+#define GPIO_BIT(gpio)          ((gpio) & (MAX_GPIO_PER_BANK - 1))
+
+static inline struct brcmstb_gpio_bank *
+brcmstb_gpio_gc_to_bank(struct gpio_chip *gc)
+{
+	struct bgpio_chip *bgc = to_bgpio_chip(gc);
+	return container_of(bgc, struct brcmstb_gpio_bank, bgc);
+}
+
+static inline struct brcmstb_gpio_priv *
+brcmstb_gpio_gc_to_priv(struct gpio_chip *gc)
+{
+	struct brcmstb_gpio_bank *bank = brcmstb_gpio_gc_to_bank(gc);
+	return bank->parent_priv;
+}
+
+/* Make sure that the number of banks matches up between properties */
+static int brcmstb_gpio_sanity_check_banks(struct device *dev,
+		struct device_node *np, struct resource *res)
+{
+	int res_num_banks = resource_size(res) / GIO_BANK_SIZE;
+	int num_banks =
+		of_property_count_u32_elems(np, "brcm,gpio-bank-widths");
+
+	if (res_num_banks != num_banks) {
+		dev_err(dev, "Mismatch in banks: res had %d, bank-widths had %d\n",
+				res_num_banks, num_banks);
+		return -EINVAL;
+	} else {
+		return 0;
+	}
+}
+
+static int brcmstb_gpio_remove(struct platform_device *pdev)
+{
+	struct brcmstb_gpio_priv *priv = platform_get_drvdata(pdev);
+	struct list_head *pos;
+	struct brcmstb_gpio_bank *bank;
+	int ret = 0;
+
+	list_for_each(pos, &priv->bank_list) {
+		bank = list_entry(pos, struct brcmstb_gpio_bank, node);
+		ret = bgpio_remove(&bank->bgc);
+		if (ret)
+			dev_err(&pdev->dev, "gpiochip_remove fail in cleanup");
+	}
+	return ret;
+}
+
+static int brcmstb_gpio_of_xlate(struct gpio_chip *gc,
+		const struct of_phandle_args *gpiospec, u32 *flags)
+{
+	struct brcmstb_gpio_priv *priv = brcmstb_gpio_gc_to_priv(gc);
+	struct brcmstb_gpio_bank *bank = brcmstb_gpio_gc_to_bank(gc);
+	int offset;
+
+	if (gc->of_gpio_n_cells != 2) {
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	if (WARN_ON(gpiospec->args_count < gc->of_gpio_n_cells))
+		return -EINVAL;
+
+	offset = gpiospec->args[0] - (gc->base - priv->gpio_base);
+	if (offset >= gc->ngpio)
+		return -EINVAL;
+
+	if (unlikely(offset >= bank->width)) {
+		dev_warn_ratelimited(&priv->pdev->dev,
+			"Received request for invalid GPIO offset %d\n",
+			gpiospec->args[0]);
+	}
+
+	if (flags)
+		*flags = gpiospec->args[1];
+
+	return offset;
+}
+
+static int brcmstb_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	void __iomem *reg_base;
+	struct brcmstb_gpio_priv *priv;
+	struct resource *res;
+	struct property *prop;
+	const __be32 *p;
+	u32 bank_width;
+	int err;
+	static int gpio_base;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	reg_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(reg_base))
+		return PTR_ERR(reg_base);
+
+	priv->gpio_base = gpio_base;
+	priv->reg_base = reg_base;
+	priv->pdev = pdev;
+
+	INIT_LIST_HEAD(&priv->bank_list);
+	if (brcmstb_gpio_sanity_check_banks(dev, np, res))
+		return -EINVAL;
+
+	of_property_for_each_u32(np, "brcm,gpio-bank-widths", prop, p,
+			bank_width) {
+		struct brcmstb_gpio_bank *bank;
+		struct bgpio_chip *bgc;
+		struct gpio_chip *gc;
+
+		bank = devm_kzalloc(dev, sizeof(*bank), GFP_KERNEL);
+		if (!bank) {
+			err = -ENOMEM;
+			goto fail;
+		}
+
+		bank->parent_priv = priv;
+		bank->id = priv->num_banks;
+		if (bank_width <= 0 || bank_width > MAX_GPIO_PER_BANK) {
+			dev_err(dev, "Invalid bank width %d\n", bank_width);
+			goto fail;
+		} else {
+			bank->width = bank_width;
+		}
+
+		/*
+		 * Regs are 4 bytes wide, have data reg, no set/clear regs,
+		 * and direction bits have 0 = output and 1 = input
+		 */
+		bgc = &bank->bgc;
+		err = bgpio_init(bgc, dev, 4,
+				reg_base + GIO_DATA(bank->id),
+				NULL, NULL, NULL,
+				reg_base + GIO_IODIR(bank->id), 0);
+		if (err) {
+			dev_err(dev, "bgpio_init() failed\n");
+			goto fail;
+		}
+
+		gc = &bgc->gc;
+		gc->of_node = np;
+		gc->owner = THIS_MODULE;
+		gc->label = np->full_name;
+		gc->base = gpio_base;
+		gc->of_gpio_n_cells = 2;
+		gc->of_xlate = brcmstb_gpio_of_xlate;
+		/* not all ngpio lines are valid, will use bank width later */
+		gc->ngpio = MAX_GPIO_PER_BANK;
+
+		err = gpiochip_add(gc);
+		if (err) {
+			dev_err(dev, "Could not add gpiochip for bank %d\n",
+					bank->id);
+			goto fail;
+		}
+		gpio_base += gc->ngpio;
+		dev_dbg(dev, "bank=%d, base=%d, ngpio=%d, width=%d\n", bank->id,
+			gc->base, gc->ngpio, bank->width);
+
+		/* Everything looks good, so add bank to list */
+		list_add(&bank->node, &priv->bank_list);
+
+		priv->num_banks++;
+	}
+
+	dev_info(dev, "Registered %d banks (GPIO(s): %d-%d)\n",
+			priv->num_banks, priv->gpio_base, gpio_base - 1);
+
+	platform_set_drvdata(pdev, priv);
+
+	return 0;
+
+fail:
+	(void) brcmstb_gpio_remove(pdev);
+	return err;
+}
+
+static const struct of_device_id brcmstb_gpio_of_match[] = {
+	{ .compatible = "brcm,brcmstb-gpio" },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, brcmstb_gpio_of_match);
+
+static struct platform_driver brcmstb_gpio_driver = {
+	.driver = {
+		.name = "brcmstb-gpio",
+		.of_match_table = brcmstb_gpio_of_match,
+	},
+	.probe = brcmstb_gpio_probe,
+	.remove = brcmstb_gpio_remove,
+};
+module_platform_driver(brcmstb_gpio_driver);
+
+MODULE_AUTHOR("Gregory Fong");
+MODULE_DESCRIPTION("Driver for Broadcom BRCMSTB SoC UPG GPIO");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-crystalcove.c b/drivers/gpio/gpio-crystalcove.c
index 91a7ffe83135..fddd204dc9b6 100644
--- a/drivers/gpio/gpio-crystalcove.c
+++ b/drivers/gpio/gpio-crystalcove.c
@@ -16,6 +16,7 @@
  */
 
 #include <linux/interrupt.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/gpio.h>
 #include <linux/seq_file.h>
@@ -94,9 +95,8 @@ static inline int to_reg(int gpio, enum ctrl_register reg_type)
 {
 	int reg;
 
-	if (gpio == 94) {
+	if (gpio == 94)
 		return GPIOPANELCTL;
-	}
 
 	if (reg_type == CTRL_IN) {
 		if (gpio < 8)
@@ -255,6 +255,7 @@ static struct irq_chip crystalcove_irqchip = {
 	.irq_set_type		= crystalcove_irq_type,
 	.irq_bus_lock		= crystalcove_bus_lock,
 	.irq_bus_sync_unlock	= crystalcove_bus_sync_unlock,
+	.flags			= IRQCHIP_SKIP_SET_WAKE,
 };
 
 static irqreturn_t crystalcove_gpio_irq_handler(int irq, void *data)
diff --git a/drivers/gpio/gpio-dln2.c b/drivers/gpio/gpio-dln2.c
index dbdb4de82c6d..6685712c15cf 100644
--- a/drivers/gpio/gpio-dln2.c
+++ b/drivers/gpio/gpio-dln2.c
@@ -466,7 +466,6 @@ static int dln2_gpio_probe(struct platform_device *pdev)
 	dln2->gpio.owner = THIS_MODULE;
 	dln2->gpio.base = -1;
 	dln2->gpio.ngpio = pins;
-	dln2->gpio.exported = true;
 	dln2->gpio.can_sleep = true;
 	dln2->gpio.irq_not_threaded = true;
 	dln2->gpio.set = dln2_gpio_set;
diff --git a/drivers/gpio/gpio-dwapb.c b/drivers/gpio/gpio-dwapb.c
index 58faf04fce5d..55fa9853a7f2 100644
--- a/drivers/gpio/gpio-dwapb.c
+++ b/drivers/gpio/gpio-dwapb.c
@@ -348,8 +348,8 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	irq_gc->chip_types[1].handler = handle_edge_irq;
 
 	if (!pp->irq_shared) {
-		irq_set_chained_handler(pp->irq, dwapb_irq_handler);
-		irq_set_handler_data(pp->irq, gpio);
+		irq_set_chained_handler_and_data(pp->irq, dwapb_irq_handler,
+						 gpio);
 	} else {
 		/*
 		 * Request a shared IRQ since where MFD would have devices
diff --git a/drivers/gpio/gpio-em.c b/drivers/gpio/gpio-em.c
index 3cfcfc620c8e..fbf287307c4c 100644
--- a/drivers/gpio/gpio-em.c
+++ b/drivers/gpio/gpio-em.c
@@ -266,7 +266,7 @@ static int em_gio_irq_domain_map(struct irq_domain *h, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops em_gio_irq_domain_ops = {
+static const struct irq_domain_ops em_gio_irq_domain_ops = {
 	.map	= em_gio_irq_domain_map,
 	.xlate	= irq_domain_xlate_twocell,
 };
diff --git a/drivers/gpio/gpio-etraxfs.c b/drivers/gpio/gpio-etraxfs.c
new file mode 100644
index 000000000000..28071f4a5672
--- /dev/null
+++ b/drivers/gpio/gpio-etraxfs.c
@@ -0,0 +1,176 @@
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/basic_mmio_gpio.h>
+
+#define ETRAX_FS_rw_pa_dout	0
+#define ETRAX_FS_r_pa_din	4
+#define ETRAX_FS_rw_pa_oe	8
+#define ETRAX_FS_rw_intr_cfg	12
+#define ETRAX_FS_rw_intr_mask	16
+#define ETRAX_FS_rw_ack_intr	20
+#define ETRAX_FS_r_intr		24
+#define ETRAX_FS_rw_pb_dout	32
+#define ETRAX_FS_r_pb_din	36
+#define ETRAX_FS_rw_pb_oe	40
+#define ETRAX_FS_rw_pc_dout	48
+#define ETRAX_FS_r_pc_din	52
+#define ETRAX_FS_rw_pc_oe	56
+#define ETRAX_FS_rw_pd_dout	64
+#define ETRAX_FS_r_pd_din	68
+#define ETRAX_FS_rw_pd_oe	72
+#define ETRAX_FS_rw_pe_dout	80
+#define ETRAX_FS_r_pe_din	84
+#define ETRAX_FS_rw_pe_oe	88
+
+struct etraxfs_gpio_port {
+	const char *label;
+	unsigned int oe;
+	unsigned int dout;
+	unsigned int din;
+	unsigned int ngpio;
+};
+
+struct etraxfs_gpio_info {
+	unsigned int num_ports;
+	const struct etraxfs_gpio_port *ports;
+};
+
+static const struct etraxfs_gpio_port etraxfs_gpio_etraxfs_ports[] = {
+	{
+		.label	= "A",
+		.ngpio	= 8,
+		.oe	= ETRAX_FS_rw_pa_oe,
+		.dout	= ETRAX_FS_rw_pa_dout,
+		.din	= ETRAX_FS_r_pa_din,
+	},
+	{
+		.label	= "B",
+		.ngpio	= 18,
+		.oe	= ETRAX_FS_rw_pb_oe,
+		.dout	= ETRAX_FS_rw_pb_dout,
+		.din	= ETRAX_FS_r_pb_din,
+	},
+	{
+		.label	= "C",
+		.ngpio	= 18,
+		.oe	= ETRAX_FS_rw_pc_oe,
+		.dout	= ETRAX_FS_rw_pc_dout,
+		.din	= ETRAX_FS_r_pc_din,
+	},
+	{
+		.label	= "D",
+		.ngpio	= 18,
+		.oe	= ETRAX_FS_rw_pd_oe,
+		.dout	= ETRAX_FS_rw_pd_dout,
+		.din	= ETRAX_FS_r_pd_din,
+	},
+	{
+		.label	= "E",
+		.ngpio	= 18,
+		.oe	= ETRAX_FS_rw_pe_oe,
+		.dout	= ETRAX_FS_rw_pe_dout,
+		.din	= ETRAX_FS_r_pe_din,
+	},
+};
+
+static const struct etraxfs_gpio_info etraxfs_gpio_etraxfs = {
+	.num_ports = ARRAY_SIZE(etraxfs_gpio_etraxfs_ports),
+	.ports = etraxfs_gpio_etraxfs_ports,
+};
+
+static int etraxfs_gpio_of_xlate(struct gpio_chip *gc,
+			       const struct of_phandle_args *gpiospec,
+			       u32 *flags)
+{
+	/*
+	 * Port numbers are A to E, and the properties are integers, so we
+	 * specify them as 0xA - 0xE.
+	 */
+	if (gc->label[0] - 'A' + 0xA != gpiospec->args[2])
+		return -EINVAL;
+
+	return of_gpio_simple_xlate(gc, gpiospec, flags);
+}
+
+static const struct of_device_id etraxfs_gpio_of_table[] = {
+	{
+		.compatible = "axis,etraxfs-gio",
+		.data = &etraxfs_gpio_etraxfs,
+	},
+	{},
+};
+
+static int etraxfs_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	const struct etraxfs_gpio_info *info;
+	const struct of_device_id *match;
+	struct bgpio_chip *chips;
+	struct resource *res;
+	void __iomem *regs;
+	int ret;
+	int i;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	regs = devm_ioremap_resource(dev, res);
+	if (!regs)
+		return -ENOMEM;
+
+	match = of_match_node(etraxfs_gpio_of_table, dev->of_node);
+	if (!match)
+		return -EINVAL;
+
+	info = match->data;
+
+	chips = devm_kzalloc(dev, sizeof(*chips) * info->num_ports, GFP_KERNEL);
+	if (!chips)
+		return -ENOMEM;
+
+	for (i = 0; i < info->num_ports; i++) {
+		struct bgpio_chip *bgc = &chips[i];
+		const struct etraxfs_gpio_port *port = &info->ports[i];
+
+		ret = bgpio_init(bgc, dev, 4,
+				 regs + port->din,	/* dat */
+				 regs + port->dout,	/* set */
+				 NULL,			/* clr */
+				 regs + port->oe,	/* dirout */
+				 NULL,			/* dirin */
+				 BGPIOF_UNREADABLE_REG_SET);
+		if (ret)
+			return ret;
+
+		bgc->gc.ngpio = port->ngpio;
+		bgc->gc.label = port->label;
+
+		bgc->gc.of_node = dev->of_node;
+		bgc->gc.of_gpio_n_cells = 3;
+		bgc->gc.of_xlate = etraxfs_gpio_of_xlate;
+
+		ret = gpiochip_add(&bgc->gc);
+		if (ret)
+			dev_err(dev, "Unable to register port %s\n",
+				bgc->gc.label);
+	}
+
+	return 0;
+}
+
+static struct platform_driver etraxfs_gpio_driver = {
+	.driver = {
+		.name		= "etraxfs-gpio",
+		.of_match_table = of_match_ptr(etraxfs_gpio_of_table),
+	},
+	.probe	= etraxfs_gpio_probe,
+};
+
+static int __init etraxfs_gpio_init(void)
+{
+	return platform_driver_register(&etraxfs_gpio_driver);
+}
+
+device_initcall(etraxfs_gpio_init);
diff --git a/drivers/gpio/gpio-f7188x.c b/drivers/gpio/gpio-f7188x.c
index dbda8433c4f7..5e3c4fa67d82 100644
--- a/drivers/gpio/gpio-f7188x.c
+++ b/drivers/gpio/gpio-f7188x.c
@@ -172,7 +172,7 @@ static struct f7188x_gpio_bank f71869a_gpio_bank[] = {
 };
 
 static struct f7188x_gpio_bank f71882_gpio_bank[] = {
-	F7188X_GPIO_BANK(0 , 8, 0xF0),
+	F7188X_GPIO_BANK(0, 8, 0xF0),
 	F7188X_GPIO_BANK(10, 8, 0xE0),
 	F7188X_GPIO_BANK(20, 8, 0xD0),
 	F7188X_GPIO_BANK(30, 4, 0xC0),
@@ -180,7 +180,7 @@ static struct f7188x_gpio_bank f71882_gpio_bank[] = {
 };
 
 static struct f7188x_gpio_bank f71889_gpio_bank[] = {
-	F7188X_GPIO_BANK(0 , 7, 0xF0),
+	F7188X_GPIO_BANK(0, 7, 0xF0),
 	F7188X_GPIO_BANK(10, 7, 0xE0),
 	F7188X_GPIO_BANK(20, 8, 0xD0),
 	F7188X_GPIO_BANK(30, 8, 0xC0),
diff --git a/drivers/gpio/gpio-generic.c b/drivers/gpio/gpio-generic.c
index b92a690f5765..9bda3727fac1 100644
--- a/drivers/gpio/gpio-generic.c
+++ b/drivers/gpio/gpio-generic.c
@@ -135,6 +135,17 @@ static unsigned long bgpio_pin2mask_be(struct bgpio_chip *bgc,
 	return 1 << (bgc->bits - 1 - pin);
 }
 
+static int bgpio_get_set(struct gpio_chip *gc, unsigned int gpio)
+{
+	struct bgpio_chip *bgc = to_bgpio_chip(gc);
+	unsigned long pinmask = bgc->pin2mask(bgc, gpio);
+
+	if (bgc->dir & pinmask)
+		return bgc->read_reg(bgc->reg_set) & pinmask;
+	else
+		return bgc->read_reg(bgc->reg_dat) & pinmask;
+}
+
 static int bgpio_get(struct gpio_chip *gc, unsigned int gpio)
 {
 	struct bgpio_chip *bgc = to_bgpio_chip(gc);
@@ -416,7 +427,8 @@ static int bgpio_setup_accessors(struct device *dev,
 static int bgpio_setup_io(struct bgpio_chip *bgc,
 			  void __iomem *dat,
 			  void __iomem *set,
-			  void __iomem *clr)
+			  void __iomem *clr,
+			  unsigned long flags)
 {
 
 	bgc->reg_dat = dat;
@@ -437,7 +449,11 @@ static int bgpio_setup_io(struct bgpio_chip *bgc,
 		bgc->gc.set_multiple = bgpio_set_multiple;
 	}
 
-	bgc->gc.get = bgpio_get;
+	if (!(flags & BGPIOF_UNREADABLE_REG_SET) &&
+	    (flags & BGPIOF_READ_OUTPUT_REG_SET))
+		bgc->gc.get = bgpio_get_set;
+	else
+		bgc->gc.get = bgpio_get;
 
 	return 0;
 }
@@ -500,7 +516,7 @@ int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
 	bgc->gc.ngpio = bgc->bits;
 	bgc->gc.request = bgpio_request;
 
-	ret = bgpio_setup_io(bgc, dat, set, clr);
+	ret = bgpio_setup_io(bgc, dat, set, clr, flags);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpio/gpio-grgpio.c b/drivers/gpio/gpio-grgpio.c
index 35a02770c8b0..0a8f7617e72e 100644
--- a/drivers/gpio/gpio-grgpio.c
+++ b/drivers/gpio/gpio-grgpio.c
@@ -332,7 +332,7 @@ static void grgpio_irq_unmap(struct irq_domain *d, unsigned int irq)
 	spin_unlock_irqrestore(&priv->bgc.lock, flags);
 }
 
-static struct irq_domain_ops grgpio_irq_domain_ops = {
+static const struct irq_domain_ops grgpio_irq_domain_ops = {
 	.map	= grgpio_irq_map,
 	.unmap	= grgpio_irq_unmap,
 };
diff --git a/drivers/gpio/gpio-it8761e.c b/drivers/gpio/gpio-it8761e.c
index dadfc245cf09..30a8f24c92c5 100644
--- a/drivers/gpio/gpio-it8761e.c
+++ b/drivers/gpio/gpio-it8761e.c
@@ -123,7 +123,7 @@ static void it8761e_gpio_set(struct gpio_chip *gc,
 
 	curr_vals = inb(reg);
 	if (val)
-		outb(curr_vals | (1 << bit) , reg);
+		outb(curr_vals | (1 << bit), reg);
 	else
 		outb(curr_vals & ~(1 << bit), reg);
 
diff --git a/drivers/gpio/gpio-lpc18xx.c b/drivers/gpio/gpio-lpc18xx.c
new file mode 100644
index 000000000000..eb68603136b0
--- /dev/null
+++ b/drivers/gpio/gpio-lpc18xx.c
@@ -0,0 +1,180 @@
+/*
+ * GPIO driver for NXP LPC18xx/43xx.
+ *
+ * Copyright (C) 2015 Joachim Eastwood <manabian@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/gpio/driver.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+
+/* LPC18xx GPIO register offsets */
+#define LPC18XX_REG_DIR(n)	(0x2000 + n * sizeof(u32))
+
+#define LPC18XX_MAX_PORTS	8
+#define LPC18XX_PINS_PER_PORT	32
+
+struct lpc18xx_gpio_chip {
+	struct gpio_chip gpio;
+	void __iomem *base;
+	struct clk *clk;
+	spinlock_t lock;
+};
+
+static inline struct lpc18xx_gpio_chip *to_lpc18xx_gpio(struct gpio_chip *chip)
+{
+	return container_of(chip, struct lpc18xx_gpio_chip, gpio);
+}
+
+static int lpc18xx_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_request_gpio(offset);
+}
+
+static void lpc18xx_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	pinctrl_free_gpio(offset);
+}
+
+static void lpc18xx_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+{
+	struct lpc18xx_gpio_chip *gc = to_lpc18xx_gpio(chip);
+	writeb(value ? 1 : 0, gc->base + offset);
+}
+
+static int lpc18xx_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct lpc18xx_gpio_chip *gc = to_lpc18xx_gpio(chip);
+	return !!readb(gc->base + offset);
+}
+
+static int lpc18xx_gpio_direction(struct gpio_chip *chip, unsigned offset,
+				  bool out)
+{
+	struct lpc18xx_gpio_chip *gc = to_lpc18xx_gpio(chip);
+	unsigned long flags;
+	u32 port, pin, dir;
+
+	port = offset / LPC18XX_PINS_PER_PORT;
+	pin  = offset % LPC18XX_PINS_PER_PORT;
+
+	spin_lock_irqsave(&gc->lock, flags);
+	dir = readl(gc->base + LPC18XX_REG_DIR(port));
+	if (out)
+		dir |= BIT(pin);
+	else
+		dir &= ~BIT(pin);
+	writel(dir, gc->base + LPC18XX_REG_DIR(port));
+	spin_unlock_irqrestore(&gc->lock, flags);
+
+	return 0;
+}
+
+static int lpc18xx_gpio_direction_input(struct gpio_chip *chip,
+					unsigned offset)
+{
+	return lpc18xx_gpio_direction(chip, offset, false);
+}
+
+static int lpc18xx_gpio_direction_output(struct gpio_chip *chip,
+					 unsigned offset, int value)
+{
+	lpc18xx_gpio_set(chip, offset, value);
+	return lpc18xx_gpio_direction(chip, offset, true);
+}
+
+static struct gpio_chip lpc18xx_chip = {
+	.label			= "lpc18xx/43xx-gpio",
+	.request		= lpc18xx_gpio_request,
+	.free			= lpc18xx_gpio_free,
+	.direction_input	= lpc18xx_gpio_direction_input,
+	.direction_output	= lpc18xx_gpio_direction_output,
+	.set			= lpc18xx_gpio_set,
+	.get			= lpc18xx_gpio_get,
+	.ngpio			= LPC18XX_MAX_PORTS * LPC18XX_PINS_PER_PORT,
+	.owner			= THIS_MODULE,
+};
+
+static int lpc18xx_gpio_probe(struct platform_device *pdev)
+{
+	struct lpc18xx_gpio_chip *gc;
+	struct resource *res;
+	int ret;
+
+	gc = devm_kzalloc(&pdev->dev, sizeof(*gc), GFP_KERNEL);
+	if (!gc)
+		return -ENOMEM;
+
+	gc->gpio = lpc18xx_chip;
+	platform_set_drvdata(pdev, gc);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	gc->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(gc->base))
+		return PTR_ERR(gc->base);
+
+	gc->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(gc->clk)) {
+		dev_err(&pdev->dev, "input clock not found\n");
+		return PTR_ERR(gc->clk);
+	}
+
+	ret = clk_prepare_enable(gc->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to enable clock\n");
+		return ret;
+	}
+
+	spin_lock_init(&gc->lock);
+
+	gc->gpio.dev = &pdev->dev;
+
+	ret = gpiochip_add(&gc->gpio);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to add gpio chip\n");
+		clk_disable_unprepare(gc->clk);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int lpc18xx_gpio_remove(struct platform_device *pdev)
+{
+	struct lpc18xx_gpio_chip *gc = platform_get_drvdata(pdev);
+
+	gpiochip_remove(&gc->gpio);
+	clk_disable_unprepare(gc->clk);
+
+	return 0;
+}
+
+static const struct of_device_id lpc18xx_gpio_match[] = {
+	{ .compatible = "nxp,lpc1850-gpio" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, lpc18xx_gpio_match);
+
+static struct platform_driver lpc18xx_gpio_driver = {
+	.probe	= lpc18xx_gpio_probe,
+	.remove	= lpc18xx_gpio_remove,
+	.driver	= {
+		.name		= "lpc18xx-gpio",
+		.of_match_table	= lpc18xx_gpio_match,
+	},
+};
+module_platform_driver(lpc18xx_gpio_driver);
+
+MODULE_AUTHOR("Joachim Eastwood <manabian@gmail.com>");
+MODULE_DESCRIPTION("GPIO driver for LPC18xx/43xx");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-lynxpoint.c b/drivers/gpio/gpio-lynxpoint.c
index 127c755b38dc..153af464c7a7 100644
--- a/drivers/gpio/gpio-lynxpoint.c
+++ b/drivers/gpio/gpio-lynxpoint.c
@@ -72,7 +72,7 @@ struct lp_gpio {
  *
  * per gpio specific registers consist of two 32bit registers per gpio
  * (LP_CONFIG1 and LP_CONFIG2), with 94 gpios there's a total of
- * 188 config registes.
+ * 188 config registers.
  *
  * A simplified view of the register layout look like this:
  *
diff --git a/drivers/gpio/gpio-max732x.c b/drivers/gpio/gpio-max732x.c
index 0fa4543c5e02..aed4ca9338bc 100644
--- a/drivers/gpio/gpio-max732x.c
+++ b/drivers/gpio/gpio-max732x.c
@@ -429,6 +429,14 @@ static int max732x_irq_set_type(struct irq_data *d, unsigned int type)
 	return 0;
 }
 
+static int max732x_irq_set_wake(struct irq_data *data, unsigned int on)
+{
+	struct max732x_chip *chip = irq_data_get_irq_chip_data(data);
+
+	irq_set_irq_wake(chip->client->irq, on);
+	return 0;
+}
+
 static struct irq_chip max732x_irq_chip = {
 	.name			= "max732x",
 	.irq_mask		= max732x_irq_mask,
@@ -436,6 +444,7 @@ static struct irq_chip max732x_irq_chip = {
 	.irq_bus_lock		= max732x_irq_bus_lock,
 	.irq_bus_sync_unlock	= max732x_irq_bus_sync_unlock,
 	.irq_set_type		= max732x_irq_set_type,
+	.irq_set_wake		= max732x_irq_set_wake,
 };
 
 static uint8_t max732x_irq_pending(struct max732x_chip *chip)
@@ -507,12 +516,10 @@ static int max732x_irq_setup(struct max732x_chip *chip,
 		chip->irq_features = has_irq;
 		mutex_init(&chip->irq_lock);
 
-		ret = devm_request_threaded_irq(&client->dev,
-					client->irq,
-					NULL,
-					max732x_irq_handler,
-					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
-					dev_name(&client->dev), chip);
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+				NULL, max732x_irq_handler, IRQF_ONESHOT |
+				IRQF_TRIGGER_FALLING | IRQF_SHARED,
+				dev_name(&client->dev), chip);
 		if (ret) {
 			dev_err(&client->dev, "failed to request irq %d\n",
 				client->irq);
@@ -521,7 +528,7 @@ static int max732x_irq_setup(struct max732x_chip *chip,
 		ret =  gpiochip_irqchip_add(&chip->gpio_chip,
 					    &max732x_irq_chip,
 					    irq_base,
-					    handle_edge_irq,
+					    handle_simple_irq,
 					    IRQ_TYPE_NONE);
 		if (ret) {
 			dev_err(&client->dev,
diff --git a/drivers/gpio/gpio-moxart.c b/drivers/gpio/gpio-moxart.c
index c3ab46e595da..abd8676ce2b6 100644
--- a/drivers/gpio/gpio-moxart.c
+++ b/drivers/gpio/gpio-moxart.c
@@ -39,17 +39,6 @@ static void moxart_gpio_free(struct gpio_chip *chip, unsigned offset)
 	pinctrl_free_gpio(offset);
 }
 
-static int moxart_gpio_get(struct gpio_chip *chip, unsigned offset)
-{
-	struct bgpio_chip *bgc = to_bgpio_chip(chip);
-	u32 ret = bgc->read_reg(bgc->reg_dir);
-
-	if (ret & BIT(offset))
-		return !!(bgc->read_reg(bgc->reg_set) & BIT(offset));
-	else
-		return !!(bgc->read_reg(bgc->reg_dat) & BIT(offset));
-}
-
 static int moxart_gpio_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -68,8 +57,9 @@ static int moxart_gpio_probe(struct platform_device *pdev)
 		return PTR_ERR(base);
 
 	ret = bgpio_init(bgc, dev, 4, base + GPIO_DATA_IN,
-		    base + GPIO_DATA_OUT, NULL,
-		    base + GPIO_PIN_DIRECTION, NULL, 0);
+			 base + GPIO_DATA_OUT, NULL,
+			 base + GPIO_PIN_DIRECTION, NULL,
+			 BGPIOF_READ_OUTPUT_REG_SET);
 	if (ret) {
 		dev_err(&pdev->dev, "bgpio_init failed\n");
 		return ret;
@@ -78,7 +68,6 @@ static int moxart_gpio_probe(struct platform_device *pdev)
 	bgc->gc.label = "moxart-gpio";
 	bgc->gc.request = moxart_gpio_request;
 	bgc->gc.free = moxart_gpio_free;
-	bgc->gc.get = moxart_gpio_get;
 	bgc->data = bgc->read_reg(bgc->reg_set);
 	bgc->gc.base = 0;
 	bgc->gc.ngpio = 32;
diff --git a/drivers/gpio/gpio-mpc8xxx.c b/drivers/gpio/gpio-mpc8xxx.c
index a65b75161aa4..20aa66f34f6e 100644
--- a/drivers/gpio/gpio-mpc8xxx.c
+++ b/drivers/gpio/gpio-mpc8xxx.c
@@ -329,7 +329,7 @@ static int mpc8xxx_gpio_irq_map(struct irq_domain *h, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops mpc8xxx_gpio_irq_ops = {
+static const struct irq_domain_ops mpc8xxx_gpio_irq_ops = {
 	.map	= mpc8xxx_gpio_irq_map,
 	.xlate	= irq_domain_xlate_twocell,
 };
diff --git a/drivers/gpio/gpio-msic.c b/drivers/gpio/gpio-msic.c
index 01acf0a8cdb1..7bcfb87a5fa6 100644
--- a/drivers/gpio/gpio-msic.c
+++ b/drivers/gpio/gpio-msic.c
@@ -309,8 +309,7 @@ static int platform_msic_gpio_probe(struct platform_device *pdev)
 					 &msic_irqchip,
 					 handle_simple_irq);
 	}
-	irq_set_chained_handler(mg->irq, msic_gpio_irq_handler);
-	irq_set_handler_data(mg->irq, mg);
+	irq_set_chained_handler_and_data(mg->irq, msic_gpio_irq_handler, mg);
 
 	return 0;
 err:
diff --git a/drivers/gpio/gpio-mxc.c b/drivers/gpio/gpio-mxc.c
index 9f7446a7ac64..ec1eb1b7250f 100644
--- a/drivers/gpio/gpio-mxc.c
+++ b/drivers/gpio/gpio-mxc.c
@@ -131,7 +131,7 @@ static struct mxc_gpio_hwdata *mxc_gpio_hwdata;
 #define GPIO_INT_FALL_EDGE	(mxc_gpio_hwdata->fall_edge)
 #define GPIO_INT_BOTH_EDGES	0x4
 
-static struct platform_device_id mxc_gpio_devtype[] = {
+static const struct platform_device_id mxc_gpio_devtype[] = {
 	{
 		.name = "imx1-gpio",
 		.driver_data = IMX1_GPIO,
@@ -437,20 +437,20 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 		irq_set_chained_handler(port->irq, mx2_gpio_irq_handler);
 	} else {
 		/* setup one handler for each entry */
-		irq_set_chained_handler(port->irq, mx3_gpio_irq_handler);
-		irq_set_handler_data(port->irq, port);
-		if (port->irq_high > 0) {
+		irq_set_chained_handler_and_data(port->irq,
+						 mx3_gpio_irq_handler, port);
+		if (port->irq_high > 0)
 			/* setup handler for GPIO 16 to 31 */
-			irq_set_chained_handler(port->irq_high,
-						mx3_gpio_irq_handler);
-			irq_set_handler_data(port->irq_high, port);
-		}
+			irq_set_chained_handler_and_data(port->irq_high,
+							 mx3_gpio_irq_handler,
+							 port);
 	}
 
 	err = bgpio_init(&port->bgc, &pdev->dev, 4,
 			 port->base + GPIO_PSR,
 			 port->base + GPIO_DR, NULL,
-			 port->base + GPIO_GDIR, NULL, 0);
+			 port->base + GPIO_GDIR, NULL,
+			 BGPIOF_READ_OUTPUT_REG_SET);
 	if (err)
 		goto out_bgio;
 
diff --git a/drivers/gpio/gpio-mxs.c b/drivers/gpio/gpio-mxs.c
index 84cbda6acdda..551d15d7c369 100644
--- a/drivers/gpio/gpio-mxs.c
+++ b/drivers/gpio/gpio-mxs.c
@@ -239,7 +239,7 @@ static int mxs_gpio_get_direction(struct gpio_chip *gc, unsigned offset)
 	return !(dir & mask);
 }
 
-static struct platform_device_id mxs_gpio_ids[] = {
+static const struct platform_device_id mxs_gpio_ids[] = {
 	{
 		.name = "imx23-gpio",
 		.driver_data = IMX23_GPIO,
@@ -320,8 +320,8 @@ static int mxs_gpio_probe(struct platform_device *pdev)
 	mxs_gpio_init_gc(port, irq_base);
 
 	/* setup one handler for each entry */
-	irq_set_chained_handler(port->irq, mxs_gpio_irq_handler);
-	irq_set_handler_data(port->irq, port);
+	irq_set_chained_handler_and_data(port->irq, mxs_gpio_irq_handler,
+					 port);
 
 	err = bgpio_init(&port->bgc, &pdev->dev, 4,
 			 port->base + PINCTRL_DIN(port),
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index b232397ad7ec..b0c57d505be7 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -488,9 +488,6 @@ static int omap_gpio_irq_type(struct irq_data *d, unsigned type)
 	unsigned long flags;
 	unsigned offset = d->hwirq;
 
-	if (!BANK_USED(bank))
-		pm_runtime_get_sync(bank->dev);
-
 	if (type & ~IRQ_TYPE_SENSE_MASK)
 		return -EINVAL;
 
@@ -498,12 +495,18 @@ static int omap_gpio_irq_type(struct irq_data *d, unsigned type)
 		(type & (IRQ_TYPE_LEVEL_LOW|IRQ_TYPE_LEVEL_HIGH)))
 		return -EINVAL;
 
+	if (!BANK_USED(bank))
+		pm_runtime_get_sync(bank->dev);
+
 	spin_lock_irqsave(&bank->lock, flags);
 	retval = omap_set_gpio_triggering(bank, offset, type);
+	if (retval)
+		goto error;
 	omap_gpio_init_irq(bank, offset);
 	if (!omap_gpio_is_input(bank, offset)) {
 		spin_unlock_irqrestore(&bank->lock, flags);
-		return -EINVAL;
+		retval = -EINVAL;
+		goto error;
 	}
 	spin_unlock_irqrestore(&bank->lock, flags);
 
@@ -512,6 +515,11 @@ static int omap_gpio_irq_type(struct irq_data *d, unsigned type)
 	else if (type & (IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING))
 		__irq_set_handler_locked(d->irq, handle_edge_irq);
 
+	return 0;
+
+error:
+	if (!BANK_USED(bank))
+		pm_runtime_put(bank->dev);
 	return retval;
 }
 
@@ -638,15 +646,6 @@ static int omap_set_gpio_wakeup(struct gpio_bank *bank, unsigned offset,
 	return 0;
 }
 
-static void omap_reset_gpio(struct gpio_bank *bank, unsigned offset)
-{
-	omap_set_gpio_direction(bank, offset, 1);
-	omap_set_gpio_irqenable(bank, offset, 0);
-	omap_clear_gpio_irqstatus(bank, offset);
-	omap_set_gpio_triggering(bank, offset, IRQ_TYPE_NONE);
-	omap_clear_gpio_debounce(bank, offset);
-}
-
 /* Use disable_irq_wake() and enable_irq_wake() functions from drivers */
 static int omap_gpio_wake_enable(struct irq_data *d, unsigned int enable)
 {
@@ -669,14 +668,7 @@ static int omap_gpio_request(struct gpio_chip *chip, unsigned offset)
 		pm_runtime_get_sync(bank->dev);
 
 	spin_lock_irqsave(&bank->lock, flags);
-	/* Set trigger to none. You need to enable the desired trigger with
-	 * request_irq() or set_irq_type(). Only do this if the IRQ line has
-	 * not already been requested.
-	 */
-	if (!LINE_USED(bank->irq_usage, offset)) {
-		omap_set_gpio_triggering(bank, offset, IRQ_TYPE_NONE);
-		omap_enable_gpio_module(bank, offset);
-	}
+	omap_enable_gpio_module(bank, offset);
 	bank->mod_usage |= BIT(offset);
 	spin_unlock_irqrestore(&bank->lock, flags);
 
@@ -690,8 +682,11 @@ static void omap_gpio_free(struct gpio_chip *chip, unsigned offset)
 
 	spin_lock_irqsave(&bank->lock, flags);
 	bank->mod_usage &= ~(BIT(offset));
+	if (!LINE_USED(bank->irq_usage, offset)) {
+		omap_set_gpio_direction(bank, offset, 1);
+		omap_clear_gpio_debounce(bank, offset);
+	}
 	omap_disable_gpio_module(bank, offset);
-	omap_reset_gpio(bank, offset);
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	/*
@@ -795,11 +790,23 @@ static unsigned int omap_gpio_irq_startup(struct irq_data *d)
 		pm_runtime_get_sync(bank->dev);
 
 	spin_lock_irqsave(&bank->lock, flags);
-	omap_gpio_init_irq(bank, offset);
+
+	if (!LINE_USED(bank->mod_usage, offset))
+		omap_set_gpio_direction(bank, offset, 1);
+	else if (!omap_gpio_is_input(bank, offset))
+		goto err;
+	omap_enable_gpio_module(bank, offset);
+	bank->irq_usage |= BIT(offset);
+
 	spin_unlock_irqrestore(&bank->lock, flags);
 	omap_gpio_unmask_irq(d);
 
 	return 0;
+err:
+	spin_unlock_irqrestore(&bank->lock, flags);
+	if (!BANK_USED(bank))
+		pm_runtime_put(bank->dev);
+	return -EINVAL;
 }
 
 static void omap_gpio_irq_shutdown(struct irq_data *d)
@@ -810,8 +817,12 @@ static void omap_gpio_irq_shutdown(struct irq_data *d)
 
 	spin_lock_irqsave(&bank->lock, flags);
 	bank->irq_usage &= ~(BIT(offset));
+	omap_set_gpio_irqenable(bank, offset, 0);
+	omap_clear_gpio_irqstatus(bank, offset);
+	omap_set_gpio_triggering(bank, offset, IRQ_TYPE_NONE);
+	if (!LINE_USED(bank->mod_usage, offset))
+		omap_clear_gpio_debounce(bank, offset);
 	omap_disable_gpio_module(bank, offset);
-	omap_reset_gpio(bank, offset);
 	spin_unlock_irqrestore(&bank->lock, flags);
 
 	/*
@@ -1233,6 +1244,17 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static int omap_gpio_remove(struct platform_device *pdev)
+{
+	struct gpio_bank *bank = platform_get_drvdata(pdev);
+
+	list_del(&bank->node);
+	gpiochip_remove(&bank->chip);
+	pm_runtime_disable(bank->dev);
+
+	return 0;
+}
+
 #ifdef CONFIG_ARCH_OMAP2PLUS
 
 #if defined(CONFIG_PM)
@@ -1418,6 +1440,7 @@ static int omap_gpio_runtime_resume(struct device *dev)
 }
 #endif /* CONFIG_PM */
 
+#if IS_BUILTIN(CONFIG_GPIO_OMAP)
 void omap2_gpio_prepare_for_idle(int pwr_mode)
 {
 	struct gpio_bank *bank;
@@ -1443,6 +1466,7 @@ void omap2_gpio_resume_after_idle(void)
 		pm_runtime_get_sync(bank->dev);
 	}
 }
+#endif
 
 #if defined(CONFIG_PM)
 static void omap_gpio_init_context(struct gpio_bank *p)
@@ -1598,6 +1622,7 @@ MODULE_DEVICE_TABLE(of, omap_gpio_match);
 
 static struct platform_driver omap_gpio_driver = {
 	.probe		= omap_gpio_probe,
+	.remove		= omap_gpio_remove,
 	.driver		= {
 		.name	= "omap_gpio",
 		.pm	= &gpio_pm_ops,
@@ -1615,3 +1640,13 @@ static int __init omap_gpio_drv_reg(void)
 	return platform_driver_register(&omap_gpio_driver);
 }
 postcore_initcall(omap_gpio_drv_reg);
+
+static void __exit omap_gpio_exit(void)
+{
+	platform_driver_unregister(&omap_gpio_driver);
+}
+module_exit(omap_gpio_exit);
+
+MODULE_DESCRIPTION("omap gpio driver");
+MODULE_ALIAS("platform:gpio-omap");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index e2da64abbccd..d233eb3b8132 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -443,12 +443,13 @@ static struct irq_chip pca953x_irq_chip = {
 	.irq_set_type		= pca953x_irq_set_type,
 };
 
-static u8 pca953x_irq_pending(struct pca953x_chip *chip, u8 *pending)
+static bool pca953x_irq_pending(struct pca953x_chip *chip, u8 *pending)
 {
 	u8 cur_stat[MAX_BANK];
 	u8 old_stat[MAX_BANK];
-	u8 pendings = 0;
-	u8 trigger[MAX_BANK], triggers = 0;
+	bool pending_seen = false;
+	bool trigger_seen = false;
+	u8 trigger[MAX_BANK];
 	int ret, i, offset = 0;
 
 	switch (chip->chip_type) {
@@ -461,7 +462,7 @@ static u8 pca953x_irq_pending(struct pca953x_chip *chip, u8 *pending)
 	}
 	ret = pca953x_read_regs(chip, offset, cur_stat);
 	if (ret)
-		return 0;
+		return false;
 
 	/* Remove output pins from the equation */
 	for (i = 0; i < NBANK(chip); i++)
@@ -471,11 +472,12 @@ static u8 pca953x_irq_pending(struct pca953x_chip *chip, u8 *pending)
 
 	for (i = 0; i < NBANK(chip); i++) {
 		trigger[i] = (cur_stat[i] ^ old_stat[i]) & chip->irq_mask[i];
-		triggers += trigger[i];
+		if (trigger[i])
+			trigger_seen = true;
 	}
 
-	if (!triggers)
-		return 0;
+	if (!trigger_seen)
+		return false;
 
 	memcpy(chip->irq_stat, cur_stat, NBANK(chip));
 
@@ -483,10 +485,11 @@ static u8 pca953x_irq_pending(struct pca953x_chip *chip, u8 *pending)
 		pending[i] = (old_stat[i] & chip->irq_trig_fall[i]) |
 			(cur_stat[i] & chip->irq_trig_raise[i]);
 		pending[i] &= trigger[i];
-		pendings += pending[i];
+		if (pending[i])
+			pending_seen = true;
 	}
 
-	return pendings;
+	return pending_seen;
 }
 
 static irqreturn_t pca953x_irq_handler(int irq, void *devid)
@@ -630,7 +633,7 @@ static int device_pca957x_init(struct pca953x_chip *chip, u32 invert)
 		memset(val, 0, NBANK(chip));
 	pca953x_write_regs(chip, PCA957X_INVRT, val);
 
-	/* To enable register 6, 7 to controll pull up and pull down */
+	/* To enable register 6, 7 to control pull up and pull down */
 	memset(val, 0x02, NBANK(chip));
 	pca953x_write_regs(chip, PCA957X_BKEN, val);
 
diff --git a/drivers/gpio/gpio-pcf857x.c b/drivers/gpio/gpio-pcf857x.c
index 945f0cda8529..404f3c61ef9b 100644
--- a/drivers/gpio/gpio-pcf857x.c
+++ b/drivers/gpio/gpio-pcf857x.c
@@ -91,6 +91,8 @@ struct pcf857x {
 	spinlock_t		slock;		/* protect irq demux */
 	unsigned		out;		/* software latch */
 	unsigned		status;		/* current status */
+	unsigned int		irq_parent;
+	unsigned		irq_enabled;	/* enabled irqs */
 
 	int (*write)(struct i2c_client *client, unsigned data);
 	int (*read)(struct i2c_client *client);
@@ -194,7 +196,7 @@ static irqreturn_t pcf857x_irq(int irq, void *data)
 	 * interrupt source, just to avoid bad irqs
 	 */
 
-	change = (gpio->status ^ status);
+	change = (gpio->status ^ status) & gpio->irq_enabled;
 	for_each_set_bit(i, &change, gpio->chip.ngpio)
 		handle_nested_irq(irq_find_mapping(gpio->chip.irqdomain, i));
 	gpio->status = status;
@@ -209,29 +211,62 @@ static irqreturn_t pcf857x_irq(int irq, void *data)
  */
 static void noop(struct irq_data *data) { }
 
-static unsigned int noop_ret(struct irq_data *data)
+static int pcf857x_irq_set_wake(struct irq_data *data, unsigned int on)
 {
-	return 0;
+	struct pcf857x *gpio = irq_data_get_irq_chip_data(data);
+
+	int error = 0;
+
+	if (gpio->irq_parent) {
+		error = irq_set_irq_wake(gpio->irq_parent, on);
+		if (error) {
+			dev_dbg(&gpio->client->dev,
+				"irq %u doesn't support irq_set_wake\n",
+				gpio->irq_parent);
+			gpio->irq_parent = 0;
+		}
+	}
+	return error;
 }
 
-static int pcf857x_irq_set_wake(struct irq_data *data, unsigned int on)
+static void pcf857x_irq_enable(struct irq_data *data)
 {
 	struct pcf857x *gpio = irq_data_get_irq_chip_data(data);
 
-	irq_set_irq_wake(gpio->client->irq, on);
-	return 0;
+	gpio->irq_enabled |= (1 << data->hwirq);
+}
+
+static void pcf857x_irq_disable(struct irq_data *data)
+{
+	struct pcf857x *gpio = irq_data_get_irq_chip_data(data);
+
+	gpio->irq_enabled &= ~(1 << data->hwirq);
+}
+
+static void pcf857x_irq_bus_lock(struct irq_data *data)
+{
+	struct pcf857x *gpio = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&gpio->lock);
+}
+
+static void pcf857x_irq_bus_sync_unlock(struct irq_data *data)
+{
+	struct pcf857x *gpio = irq_data_get_irq_chip_data(data);
+
+	mutex_unlock(&gpio->lock);
 }
 
 static struct irq_chip pcf857x_irq_chip = {
 	.name		= "pcf857x",
-	.irq_startup	= noop_ret,
-	.irq_shutdown	= noop,
-	.irq_enable	= noop,
-	.irq_disable	= noop,
+	.irq_enable	= pcf857x_irq_enable,
+	.irq_disable	= pcf857x_irq_disable,
 	.irq_ack	= noop,
 	.irq_mask	= noop,
 	.irq_unmask	= noop,
 	.irq_set_wake	= pcf857x_irq_set_wake,
+	.irq_bus_lock		= pcf857x_irq_bus_lock,
+	.irq_bus_sync_unlock	= pcf857x_irq_bus_sync_unlock,
 };
 
 /*-------------------------------------------------------------------------*/
@@ -364,6 +399,7 @@ static int pcf857x_probe(struct i2c_client *client,
 
 		gpiochip_set_chained_irqchip(&gpio->chip, &pcf857x_irq_chip,
 					     client->irq, NULL);
+		gpio->irq_parent = client->irq;
 	}
 
 	/* Let platform code set up the GPIOs and their users.
diff --git a/drivers/gpio/gpio-rcar.c b/drivers/gpio/gpio-rcar.c
index fd3977465948..1e14a6c74ed1 100644
--- a/drivers/gpio/gpio-rcar.c
+++ b/drivers/gpio/gpio-rcar.c
@@ -177,8 +177,17 @@ static int gpio_rcar_irq_set_wake(struct irq_data *d, unsigned int on)
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
 	struct gpio_rcar_priv *p = container_of(gc, struct gpio_rcar_priv,
 						gpio_chip);
-
-	irq_set_irq_wake(p->irq_parent, on);
+	int error;
+
+	if (p->irq_parent) {
+		error = irq_set_irq_wake(p->irq_parent, on);
+		if (error) {
+			dev_dbg(&p->pdev->dev,
+				"irq %u doesn't support irq_set_wake\n",
+				p->irq_parent);
+			p->irq_parent = 0;
+		}
+	}
 
 	if (!p->clk)
 		return 0;
diff --git a/drivers/gpio/gpio-sa1100.c b/drivers/gpio/gpio-sa1100.c
index bec397a60204..3fa22dade243 100644
--- a/drivers/gpio/gpio-sa1100.c
+++ b/drivers/gpio/gpio-sa1100.c
@@ -160,7 +160,7 @@ static int sa1100_gpio_irqdomain_map(struct irq_domain *d,
 	return 0;
 }
 
-static struct irq_domain_ops sa1100_gpio_irqdomain_ops = {
+static const struct irq_domain_ops sa1100_gpio_irqdomain_ops = {
 	.map = sa1100_gpio_irqdomain_map,
 	.xlate = irq_domain_xlate_onetwocell,
 };
diff --git a/drivers/gpio/gpio-sodaville.c b/drivers/gpio/gpio-sodaville.c
index d8da36cd8123..65bc9f47a68e 100644
--- a/drivers/gpio/gpio-sodaville.c
+++ b/drivers/gpio/gpio-sodaville.c
@@ -125,7 +125,7 @@ static int sdv_xlate(struct irq_domain *h, struct device_node *node,
 	return 0;
 }
 
-static struct irq_domain_ops irq_domain_sdv_ops = {
+static const struct irq_domain_ops irq_domain_sdv_ops = {
 	.xlate = sdv_xlate,
 };
 
diff --git a/drivers/gpio/gpio-stp-xway.c b/drivers/gpio/gpio-stp-xway.c
index 202361eb7279..81bdbe7ba2a4 100644
--- a/drivers/gpio/gpio-stp-xway.c
+++ b/drivers/gpio/gpio-stp-xway.c
@@ -58,7 +58,7 @@
 #define XWAY_STP_ADSL_MASK	0x3
 
 /* 2 groups of 3 bits can be driven by the phys */
-#define XWAY_STP_PHY_MASK	0x3
+#define XWAY_STP_PHY_MASK	0x7
 #define XWAY_STP_PHY1_SHIFT	27
 #define XWAY_STP_PHY2_SHIFT	15
 
@@ -200,7 +200,7 @@ static int xway_stp_hw_init(struct xway_stp *chip)
 static int xway_stp_probe(struct platform_device *pdev)
 {
 	struct resource *res;
-	const __be32 *shadow, *groups, *dsl, *phy;
+	u32 shadow, groups, dsl, phy;
 	struct xway_stp *chip;
 	struct clk *clk;
 	int ret = 0;
@@ -223,33 +223,28 @@ static int xway_stp_probe(struct platform_device *pdev)
 	chip->gc.owner = THIS_MODULE;
 
 	/* store the shadow value if one was passed by the devicetree */
-	shadow = of_get_property(pdev->dev.of_node, "lantiq,shadow", NULL);
-	if (shadow)
-		chip->shadow = be32_to_cpu(*shadow);
+	if (!of_property_read_u32(pdev->dev.of_node, "lantiq,shadow", &shadow))
+		chip->shadow = shadow;
 
 	/* find out which gpio groups should be enabled */
-	groups = of_get_property(pdev->dev.of_node, "lantiq,groups", NULL);
-	if (groups)
-		chip->groups = be32_to_cpu(*groups) & XWAY_STP_GROUP_MASK;
+	if (!of_property_read_u32(pdev->dev.of_node, "lantiq,groups", &groups))
+		chip->groups = groups & XWAY_STP_GROUP_MASK;
 	else
 		chip->groups = XWAY_STP_GROUP0;
 	chip->gc.ngpio = fls(chip->groups) * 8;
 
 	/* find out which gpios are controlled by the dsl core */
-	dsl = of_get_property(pdev->dev.of_node, "lantiq,dsl", NULL);
-	if (dsl)
-		chip->dsl = be32_to_cpu(*dsl) & XWAY_STP_ADSL_MASK;
+	if (!of_property_read_u32(pdev->dev.of_node, "lantiq,dsl", &dsl))
+		chip->dsl = dsl & XWAY_STP_ADSL_MASK;
 
 	/* find out which gpios are controlled by the phys */
 	if (of_machine_is_compatible("lantiq,ar9") ||
 			of_machine_is_compatible("lantiq,gr9") ||
 			of_machine_is_compatible("lantiq,vr9")) {
-		phy = of_get_property(pdev->dev.of_node, "lantiq,phy1", NULL);
-		if (phy)
-			chip->phy1 = be32_to_cpu(*phy) & XWAY_STP_PHY_MASK;
-		phy = of_get_property(pdev->dev.of_node, "lantiq,phy2", NULL);
-		if (phy)
-			chip->phy2 = be32_to_cpu(*phy) & XWAY_STP_PHY_MASK;
+		if (!of_property_read_u32(pdev->dev.of_node, "lantiq,phy1", &phy))
+			chip->phy1 = phy & XWAY_STP_PHY_MASK;
+		if (!of_property_read_u32(pdev->dev.of_node, "lantiq,phy2", &phy))
+			chip->phy2 = phy & XWAY_STP_PHY_MASK;
 	}
 
 	/* check which edge trigger we should use, default to a falling edge */
diff --git a/drivers/gpio/gpio-tb10x.c b/drivers/gpio/gpio-tb10x.c
index 46b89614aa91..12c99d969b98 100644
--- a/drivers/gpio/gpio-tb10x.c
+++ b/drivers/gpio/gpio-tb10x.c
@@ -292,7 +292,6 @@ static int tb10x_gpio_remove(struct platform_device *pdev)
 					BIT(tb10x_gpio->gc.ngpio) - 1, 0, 0);
 		kfree(tb10x_gpio->domain->gc);
 		irq_domain_remove(tb10x_gpio->domain);
-		free_irq(tb10x_gpio->irq, tb10x_gpio);
 	}
 	gpiochip_remove(&tb10x_gpio->gc);
 
diff --git a/drivers/gpio/gpio-tegra.c b/drivers/gpio/gpio-tegra.c
index 1741981d53c8..9b25c90f725c 100644
--- a/drivers/gpio/gpio-tegra.c
+++ b/drivers/gpio/gpio-tegra.c
@@ -288,7 +288,7 @@ static void tegra_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 			tegra_gpio_writel(1 << pin, GPIO_INT_CLR(gpio));
 
 			/* if gpio is edge triggered, clear condition
-			 * before executing the hander so that we don't
+			 * before executing the handler so that we don't
 			 * miss edges
 			 */
 			if (lvl & (0x100 << pin)) {
@@ -515,8 +515,8 @@ static int tegra_gpio_probe(struct platform_device *pdev)
 	for (i = 0; i < tegra_gpio_bank_count; i++) {
 		bank = &tegra_gpio_banks[i];
 
-		irq_set_chained_handler(bank->irq, tegra_gpio_irq_handler);
-		irq_set_handler_data(bank->irq, bank);
+		irq_set_chained_handler_and_data(bank->irq,
+						 tegra_gpio_irq_handler, bank);
 
 		for (j = 0; j < 4; j++)
 			spin_lock_init(&bank->lvl_lock[j]);
diff --git a/drivers/gpio/gpio-ts5500.c b/drivers/gpio/gpio-ts5500.c
index 92fbabd82879..b29a102d136b 100644
--- a/drivers/gpio/gpio-ts5500.c
+++ b/drivers/gpio/gpio-ts5500.c
@@ -440,7 +440,7 @@ static int ts5500_dio_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id ts5500_dio_ids[] = {
+static const struct platform_device_id ts5500_dio_ids[] = {
 	{ "ts5500-dio1", TS5500_DIO1 },
 	{ "ts5500-dio2", TS5500_DIO2 },
 	{ "ts5500-dio-lcd", TS5500_LCD },
diff --git a/drivers/gpio/gpio-xgene-sb.c b/drivers/gpio/gpio-xgene-sb.c
index fb9d29a5d584..d57068b9083e 100644
--- a/drivers/gpio/gpio-xgene-sb.c
+++ b/drivers/gpio/gpio-xgene-sb.c
@@ -25,8 +25,11 @@
 #include <linux/of_gpio.h>
 #include <linux/gpio.h>
 #include <linux/gpio/driver.h>
+#include <linux/acpi.h>
 #include <linux/basic_mmio_gpio.h>
 
+#include "gpiolib.h"
+
 #define XGENE_MAX_GPIO_DS		22
 #define XGENE_MAX_GPIO_DS_IRQ		6
 
@@ -112,7 +115,6 @@ static int xgene_gpio_sb_probe(struct platform_device *pdev)
 				   GFP_KERNEL);
 	if (!priv->irq)
 		return -ENOMEM;
-	memset(priv->irq, 0, sizeof(u32) * XGENE_MAX_GPIO_DS);
 
 	for (i = 0; i < priv->nirq; i++) {
 		priv->irq[default_lines[i]] = platform_get_irq(pdev, i);
@@ -129,6 +131,11 @@ static int xgene_gpio_sb_probe(struct platform_device *pdev)
 	else
 		dev_info(&pdev->dev, "X-Gene GPIO Standby driver registered\n");
 
+	if (priv->nirq > 0) {
+		/* Register interrupt handlers for gpio signaled acpi events */
+		acpi_gpiochip_request_interrupts(&priv->bgc.gc);
+	}
+
 	return ret;
 }
 
@@ -136,6 +143,10 @@ static int xgene_gpio_sb_remove(struct platform_device *pdev)
 {
 	struct xgene_gpio_sb *priv = platform_get_drvdata(pdev);
 
+	if (priv->nirq > 0) {
+		acpi_gpiochip_free_interrupts(&priv->bgc.gc);
+	}
+
 	return bgpio_remove(&priv->bgc);
 }
 
@@ -145,10 +156,19 @@ static const struct of_device_id xgene_gpio_sb_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, xgene_gpio_sb_of_match);
 
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id xgene_gpio_sb_acpi_match[] = {
+	{"APMC0D15", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, xgene_gpio_sb_acpi_match);
+#endif
+
 static struct platform_driver xgene_gpio_sb_driver = {
 	.driver = {
 		   .name = "xgene-gpio-sb",
 		   .of_match_table = xgene_gpio_sb_of_match,
+		   .acpi_match_table = ACPI_PTR(xgene_gpio_sb_acpi_match),
 		   },
 	.probe = xgene_gpio_sb_probe,
 	.remove = xgene_gpio_sb_remove,
diff --git a/drivers/gpio/gpio-xilinx.c b/drivers/gpio/gpio-xilinx.c
index 61243d177740..77fe5d3cb105 100644
--- a/drivers/gpio/gpio-xilinx.c
+++ b/drivers/gpio/gpio-xilinx.c
@@ -41,10 +41,10 @@
 /**
  * struct xgpio_instance - Stores information about GPIO device
  * @mmchip: OF GPIO chip for memory mapped banks
+ * @gpio_width: GPIO width for every channel
  * @gpio_state: GPIO state shadow register
  * @gpio_dir: GPIO direction shadow register
  * @gpio_lock: Lock used for synchronization
- * @inited: True if the port has been inited
  */
 struct xgpio_instance {
 	struct of_mm_gpio_chip mmchip;
@@ -231,6 +231,8 @@ static void xgpio_save_regs(struct of_mm_gpio_chip *mm_gc)
  * @pdev: pointer to the platform device
  *
  * This function remove gpiochips and frees all the allocated resources.
+ *
+ * Return: 0 always
  */
 static int xgpio_remove(struct platform_device *pdev)
 {
diff --git a/drivers/gpio/gpio-xlp.c b/drivers/gpio/gpio-xlp.c
new file mode 100644
index 000000000000..9bdab7203d65
--- /dev/null
+++ b/drivers/gpio/gpio-xlp.c
@@ -0,0 +1,427 @@
+/*
+ * Copyright (C) 2003-2015 Broadcom Corporation
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/of_device.h>
+#include <linux/module.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+
+/*
+ * XLP GPIO has multiple 32 bit registers for each feature where each register
+ * controls 32 pins. So, pins up to 64 require 2 32-bit registers and up to 96
+ * require 3 32-bit registers for each feature.
+ * Here we only define offset of the first register for each feature. Offset of
+ * the registers for pins greater than 32 can be calculated as following(Use
+ * GPIO_INT_STAT as example):
+ *
+ * offset = (gpio / XLP_GPIO_REGSZ) * 4;
+ * reg_addr = addr + offset;
+ *
+ * where addr is base address of the that feature register and gpio is the pin.
+ */
+#define GPIO_OUTPUT_EN		0x00
+#define GPIO_PADDRV		0x08
+#define GPIO_INT_EN00		0x18
+#define GPIO_INT_EN10		0x20
+#define GPIO_INT_EN20		0x28
+#define GPIO_INT_EN30		0x30
+#define GPIO_INT_POL		0x38
+#define GPIO_INT_TYPE		0x40
+#define GPIO_INT_STAT		0x48
+
+#define GPIO_9XX_BYTESWAP	0X00
+#define GPIO_9XX_CTRL		0X04
+#define GPIO_9XX_OUTPUT_EN	0x14
+#define GPIO_9XX_PADDRV		0x24
+/*
+ * Only for 4 interrupt enable reg are defined for now,
+ * total reg available are 12.
+ */
+#define GPIO_9XX_INT_EN00	0x44
+#define GPIO_9XX_INT_EN10	0x54
+#define GPIO_9XX_INT_EN20	0x64
+#define GPIO_9XX_INT_EN30	0x74
+#define GPIO_9XX_INT_POL	0x104
+#define GPIO_9XX_INT_TYPE	0x114
+#define GPIO_9XX_INT_STAT	0x124
+
+#define GPIO_3XX_INT_EN00	0x18
+#define GPIO_3XX_INT_EN10	0x20
+#define GPIO_3XX_INT_EN20	0x28
+#define GPIO_3XX_INT_EN30	0x30
+#define GPIO_3XX_INT_POL	0x78
+#define GPIO_3XX_INT_TYPE	0x80
+#define GPIO_3XX_INT_STAT	0x88
+
+/* Interrupt type register mask */
+#define XLP_GPIO_IRQ_TYPE_LVL	0x0
+#define XLP_GPIO_IRQ_TYPE_EDGE	0x1
+
+/* Interrupt polarity register mask */
+#define XLP_GPIO_IRQ_POL_HIGH	0x0
+#define XLP_GPIO_IRQ_POL_LOW	0x1
+
+#define XLP_GPIO_REGSZ		32
+#define XLP_GPIO_IRQ_BASE	768
+#define XLP_MAX_NR_GPIO		96
+
+/* XLP variants supported by this driver */
+enum {
+	XLP_GPIO_VARIANT_XLP832 = 1,
+	XLP_GPIO_VARIANT_XLP316,
+	XLP_GPIO_VARIANT_XLP208,
+	XLP_GPIO_VARIANT_XLP980,
+	XLP_GPIO_VARIANT_XLP532
+};
+
+struct xlp_gpio_priv {
+	struct gpio_chip chip;
+	DECLARE_BITMAP(gpio_enabled_mask, XLP_MAX_NR_GPIO);
+	void __iomem *gpio_intr_en;	/* pointer to first intr enable reg */
+	void __iomem *gpio_intr_stat;	/* pointer to first intr status reg */
+	void __iomem *gpio_intr_type;	/* pointer to first intr type reg */
+	void __iomem *gpio_intr_pol;	/* pointer to first intr polarity reg */
+	void __iomem *gpio_out_en;	/* pointer to first output enable reg */
+	void __iomem *gpio_paddrv;	/* pointer to first pad drive reg */
+	spinlock_t lock;
+};
+
+static struct xlp_gpio_priv *gpio_chip_to_xlp_priv(struct gpio_chip *gc)
+{
+	return container_of(gc, struct xlp_gpio_priv, chip);
+}
+
+static int xlp_gpio_get_reg(void __iomem *addr, unsigned gpio)
+{
+	u32 pos, regset;
+
+	pos = gpio % XLP_GPIO_REGSZ;
+	regset = (gpio / XLP_GPIO_REGSZ) * 4;
+	return !!(readl(addr + regset) & BIT(pos));
+}
+
+static void xlp_gpio_set_reg(void __iomem *addr, unsigned gpio, int state)
+{
+	u32 value, pos, regset;
+
+	pos = gpio % XLP_GPIO_REGSZ;
+	regset = (gpio / XLP_GPIO_REGSZ) * 4;
+	value = readl(addr + regset);
+
+	if (state)
+		value |= BIT(pos);
+	else
+		value &= ~BIT(pos);
+
+	writel(value, addr + regset);
+}
+
+static void xlp_gpio_irq_disable(struct irq_data *d)
+{
+	struct gpio_chip *gc  = irq_data_get_irq_chip_data(d);
+	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	xlp_gpio_set_reg(priv->gpio_intr_en, d->hwirq, 0x0);
+	__clear_bit(d->hwirq, priv->gpio_enabled_mask);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static void xlp_gpio_irq_mask_ack(struct irq_data *d)
+{
+	struct gpio_chip *gc  = irq_data_get_irq_chip_data(d);
+	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	xlp_gpio_set_reg(priv->gpio_intr_en, d->hwirq, 0x0);
+	xlp_gpio_set_reg(priv->gpio_intr_stat, d->hwirq, 0x1);
+	__clear_bit(d->hwirq, priv->gpio_enabled_mask);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static void xlp_gpio_irq_unmask(struct irq_data *d)
+{
+	struct gpio_chip *gc  = irq_data_get_irq_chip_data(d);
+	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	xlp_gpio_set_reg(priv->gpio_intr_en, d->hwirq, 0x1);
+	__set_bit(d->hwirq, priv->gpio_enabled_mask);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int xlp_gpio_set_irq_type(struct irq_data *d, unsigned int type)
+{
+	struct gpio_chip *gc  = irq_data_get_irq_chip_data(d);
+	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	int pol, irq_type;
+
+	switch (type) {
+	case IRQ_TYPE_EDGE_RISING:
+		irq_type = XLP_GPIO_IRQ_TYPE_EDGE;
+		pol = XLP_GPIO_IRQ_POL_HIGH;
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		irq_type = XLP_GPIO_IRQ_TYPE_EDGE;
+		pol = XLP_GPIO_IRQ_POL_LOW;
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		irq_type = XLP_GPIO_IRQ_TYPE_LVL;
+		pol = XLP_GPIO_IRQ_POL_HIGH;
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		irq_type = XLP_GPIO_IRQ_TYPE_LVL;
+		pol = XLP_GPIO_IRQ_POL_LOW;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	xlp_gpio_set_reg(priv->gpio_intr_type, d->hwirq, irq_type);
+	xlp_gpio_set_reg(priv->gpio_intr_pol, d->hwirq, pol);
+
+	return 0;
+}
+
+static struct irq_chip xlp_gpio_irq_chip = {
+	.name		= "XLP-GPIO",
+	.irq_mask_ack	= xlp_gpio_irq_mask_ack,
+	.irq_disable	= xlp_gpio_irq_disable,
+	.irq_set_type	= xlp_gpio_set_irq_type,
+	.irq_unmask	= xlp_gpio_irq_unmask,
+	.flags		= IRQCHIP_ONESHOT_SAFE,
+};
+
+static irqreturn_t xlp_gpio_generic_handler(int irq, void *data)
+{
+	struct xlp_gpio_priv *priv = data;
+	int gpio, regoff;
+	u32 gpio_stat;
+
+	regoff = -1;
+	gpio_stat = 0;
+	for_each_set_bit(gpio, priv->gpio_enabled_mask, XLP_MAX_NR_GPIO) {
+		if (regoff != gpio / XLP_GPIO_REGSZ) {
+			regoff = gpio / XLP_GPIO_REGSZ;
+			gpio_stat = readl(priv->gpio_intr_stat + regoff * 4);
+		}
+		if (gpio_stat & BIT(gpio % XLP_GPIO_REGSZ))
+			generic_handle_irq(irq_find_mapping(
+						priv->chip.irqdomain, gpio));
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int xlp_gpio_dir_output(struct gpio_chip *gc, unsigned gpio, int state)
+{
+	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+
+	BUG_ON(gpio >= gc->ngpio);
+	xlp_gpio_set_reg(priv->gpio_out_en, gpio, 0x1);
+
+	return 0;
+}
+
+static int xlp_gpio_dir_input(struct gpio_chip *gc, unsigned gpio)
+{
+	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+
+	BUG_ON(gpio >= gc->ngpio);
+	xlp_gpio_set_reg(priv->gpio_out_en, gpio, 0x0);
+
+	return 0;
+}
+
+static int xlp_gpio_get(struct gpio_chip *gc, unsigned gpio)
+{
+	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+
+	BUG_ON(gpio >= gc->ngpio);
+	return xlp_gpio_get_reg(priv->gpio_paddrv, gpio);
+}
+
+static void xlp_gpio_set(struct gpio_chip *gc, unsigned gpio, int state)
+{
+	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+
+	BUG_ON(gpio >= gc->ngpio);
+	xlp_gpio_set_reg(priv->gpio_paddrv, gpio, state);
+}
+
+static const struct of_device_id xlp_gpio_of_ids[] = {
+	{
+		.compatible = "netlogic,xlp832-gpio",
+		.data	    = (void *)XLP_GPIO_VARIANT_XLP832,
+	},
+	{
+		.compatible = "netlogic,xlp316-gpio",
+		.data	    = (void *)XLP_GPIO_VARIANT_XLP316,
+	},
+	{
+		.compatible = "netlogic,xlp208-gpio",
+		.data	    = (void *)XLP_GPIO_VARIANT_XLP208,
+	},
+	{
+		.compatible = "netlogic,xlp980-gpio",
+		.data	    = (void *)XLP_GPIO_VARIANT_XLP980,
+	},
+	{
+		.compatible = "netlogic,xlp532-gpio",
+		.data	    = (void *)XLP_GPIO_VARIANT_XLP532,
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, xlp_gpio_of_ids);
+
+static int xlp_gpio_probe(struct platform_device *pdev)
+{
+	struct gpio_chip *gc;
+	struct resource *iores;
+	struct xlp_gpio_priv *priv;
+	const struct of_device_id *of_id;
+	void __iomem *gpio_base;
+	int irq_base, irq, err;
+	int ngpio;
+	u32 soc_type;
+
+	iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!iores)
+		return -ENODEV;
+
+	priv = devm_kzalloc(&pdev->dev,	sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	gpio_base = devm_ioremap_resource(&pdev->dev, iores);
+	if (IS_ERR(gpio_base))
+		return PTR_ERR(gpio_base);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	of_id = of_match_device(xlp_gpio_of_ids, &pdev->dev);
+	if (!of_id) {
+		dev_err(&pdev->dev, "Failed to get soc type!\n");
+		return -ENODEV;
+	}
+
+	soc_type = (uintptr_t) of_id->data;
+
+	switch (soc_type) {
+	case XLP_GPIO_VARIANT_XLP832:
+		priv->gpio_out_en = gpio_base + GPIO_OUTPUT_EN;
+		priv->gpio_paddrv = gpio_base + GPIO_PADDRV;
+		priv->gpio_intr_stat = gpio_base + GPIO_INT_STAT;
+		priv->gpio_intr_type = gpio_base + GPIO_INT_TYPE;
+		priv->gpio_intr_pol = gpio_base + GPIO_INT_POL;
+		priv->gpio_intr_en = gpio_base + GPIO_INT_EN00;
+		ngpio = 41;
+		break;
+	case XLP_GPIO_VARIANT_XLP208:
+	case XLP_GPIO_VARIANT_XLP316:
+		priv->gpio_out_en = gpio_base + GPIO_OUTPUT_EN;
+		priv->gpio_paddrv = gpio_base + GPIO_PADDRV;
+		priv->gpio_intr_stat = gpio_base + GPIO_3XX_INT_STAT;
+		priv->gpio_intr_type = gpio_base + GPIO_3XX_INT_TYPE;
+		priv->gpio_intr_pol = gpio_base + GPIO_3XX_INT_POL;
+		priv->gpio_intr_en = gpio_base + GPIO_3XX_INT_EN00;
+
+		ngpio = (soc_type == XLP_GPIO_VARIANT_XLP208) ? 42 : 57;
+		break;
+	case XLP_GPIO_VARIANT_XLP980:
+	case XLP_GPIO_VARIANT_XLP532:
+		priv->gpio_out_en = gpio_base + GPIO_9XX_OUTPUT_EN;
+		priv->gpio_paddrv = gpio_base + GPIO_9XX_PADDRV;
+		priv->gpio_intr_stat = gpio_base + GPIO_9XX_INT_STAT;
+		priv->gpio_intr_type = gpio_base + GPIO_9XX_INT_TYPE;
+		priv->gpio_intr_pol = gpio_base + GPIO_9XX_INT_POL;
+		priv->gpio_intr_en = gpio_base + GPIO_9XX_INT_EN00;
+
+		ngpio = (soc_type == XLP_GPIO_VARIANT_XLP980) ? 66 : 67;
+		break;
+	default:
+		dev_err(&pdev->dev, "Unknown Processor type!\n");
+		return -ENODEV;
+	}
+
+	bitmap_zero(priv->gpio_enabled_mask, XLP_MAX_NR_GPIO);
+
+	gc = &priv->chip;
+
+	gc->owner = THIS_MODULE;
+	gc->label = dev_name(&pdev->dev);
+	gc->base = 0;
+	gc->dev = &pdev->dev;
+	gc->ngpio = ngpio;
+	gc->of_node = pdev->dev.of_node;
+	gc->direction_output = xlp_gpio_dir_output;
+	gc->direction_input = xlp_gpio_dir_input;
+	gc->set = xlp_gpio_set;
+	gc->get = xlp_gpio_get;
+
+	spin_lock_init(&priv->lock);
+
+	err = devm_request_irq(&pdev->dev, irq, xlp_gpio_generic_handler,
+			IRQ_TYPE_NONE, pdev->name, priv);
+	if (err)
+		return err;
+
+	irq_base = irq_alloc_descs(-1, XLP_GPIO_IRQ_BASE, gc->ngpio, 0);
+	if (irq_base < 0) {
+		dev_err(&pdev->dev, "Failed to allocate IRQ numbers\n");
+		return err;
+	}
+
+	err = gpiochip_add(gc);
+	if (err < 0)
+		goto out_free_desc;
+
+	err = gpiochip_irqchip_add(gc, &xlp_gpio_irq_chip, irq_base,
+				handle_level_irq, IRQ_TYPE_NONE);
+	if (err) {
+		dev_err(&pdev->dev, "Could not connect irqchip to gpiochip!\n");
+		goto out_gpio_remove;
+	}
+
+	dev_info(&pdev->dev, "registered %d GPIOs\n", gc->ngpio);
+
+	return 0;
+
+out_gpio_remove:
+	gpiochip_remove(gc);
+out_free_desc:
+	irq_free_descs(irq_base, gc->ngpio);
+	return err;
+}
+
+static struct platform_driver xlp_gpio_driver = {
+	.driver		= {
+		.name	= "xlp-gpio",
+		.of_match_table = xlp_gpio_of_ids,
+	},
+	.probe		= xlp_gpio_probe,
+};
+module_platform_driver(xlp_gpio_driver);
+
+MODULE_AUTHOR("Kamlakant Patel <kamlakant.patel@broadcom.com>");
+MODULE_AUTHOR("Ganesan Ramalingam <ganesanr@broadcom.com>");
+MODULE_DESCRIPTION("Netlogic XLP GPIO Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpio/gpio-zynq.c b/drivers/gpio/gpio-zynq.c
index 184c4b1b2558..2e87c4b8da26 100644
--- a/drivers/gpio/gpio-zynq.c
+++ b/drivers/gpio/gpio-zynq.c
@@ -18,34 +18,47 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/of.h>
 
 #define DRIVER_NAME "zynq-gpio"
 
 /* Maximum banks */
 #define ZYNQ_GPIO_MAX_BANK	4
+#define ZYNQMP_GPIO_MAX_BANK	6
 
 #define ZYNQ_GPIO_BANK0_NGPIO	32
 #define ZYNQ_GPIO_BANK1_NGPIO	22
 #define ZYNQ_GPIO_BANK2_NGPIO	32
 #define ZYNQ_GPIO_BANK3_NGPIO	32
 
-#define ZYNQ_GPIO_NR_GPIOS	(ZYNQ_GPIO_BANK0_NGPIO + \
-				 ZYNQ_GPIO_BANK1_NGPIO + \
-				 ZYNQ_GPIO_BANK2_NGPIO + \
-				 ZYNQ_GPIO_BANK3_NGPIO)
-
-#define ZYNQ_GPIO_BANK0_PIN_MIN	0
-#define ZYNQ_GPIO_BANK0_PIN_MAX	(ZYNQ_GPIO_BANK0_PIN_MIN + \
-					ZYNQ_GPIO_BANK0_NGPIO - 1)
-#define ZYNQ_GPIO_BANK1_PIN_MIN	(ZYNQ_GPIO_BANK0_PIN_MAX + 1)
-#define ZYNQ_GPIO_BANK1_PIN_MAX	(ZYNQ_GPIO_BANK1_PIN_MIN + \
-					ZYNQ_GPIO_BANK1_NGPIO - 1)
-#define ZYNQ_GPIO_BANK2_PIN_MIN	(ZYNQ_GPIO_BANK1_PIN_MAX + 1)
-#define ZYNQ_GPIO_BANK2_PIN_MAX	(ZYNQ_GPIO_BANK2_PIN_MIN + \
-					ZYNQ_GPIO_BANK2_NGPIO - 1)
-#define ZYNQ_GPIO_BANK3_PIN_MIN	(ZYNQ_GPIO_BANK2_PIN_MAX + 1)
-#define ZYNQ_GPIO_BANK3_PIN_MAX	(ZYNQ_GPIO_BANK3_PIN_MIN + \
-					ZYNQ_GPIO_BANK3_NGPIO - 1)
+#define ZYNQMP_GPIO_BANK0_NGPIO 26
+#define ZYNQMP_GPIO_BANK1_NGPIO 26
+#define ZYNQMP_GPIO_BANK2_NGPIO 26
+#define ZYNQMP_GPIO_BANK3_NGPIO 32
+#define ZYNQMP_GPIO_BANK4_NGPIO 32
+#define ZYNQMP_GPIO_BANK5_NGPIO 32
+
+#define	ZYNQ_GPIO_NR_GPIOS	118
+#define	ZYNQMP_GPIO_NR_GPIOS	174
+
+#define ZYNQ_GPIO_BANK0_PIN_MIN(str)	0
+#define ZYNQ_GPIO_BANK0_PIN_MAX(str)	(ZYNQ_GPIO_BANK0_PIN_MIN(str) + \
+					ZYNQ##str##_GPIO_BANK0_NGPIO - 1)
+#define ZYNQ_GPIO_BANK1_PIN_MIN(str)	(ZYNQ_GPIO_BANK0_PIN_MAX(str) + 1)
+#define ZYNQ_GPIO_BANK1_PIN_MAX(str)	(ZYNQ_GPIO_BANK1_PIN_MIN(str) + \
+					ZYNQ##str##_GPIO_BANK1_NGPIO - 1)
+#define ZYNQ_GPIO_BANK2_PIN_MIN(str)	(ZYNQ_GPIO_BANK1_PIN_MAX(str) + 1)
+#define ZYNQ_GPIO_BANK2_PIN_MAX(str)	(ZYNQ_GPIO_BANK2_PIN_MIN(str) + \
+					ZYNQ##str##_GPIO_BANK2_NGPIO - 1)
+#define ZYNQ_GPIO_BANK3_PIN_MIN(str)	(ZYNQ_GPIO_BANK2_PIN_MAX(str) + 1)
+#define ZYNQ_GPIO_BANK3_PIN_MAX(str)	(ZYNQ_GPIO_BANK3_PIN_MIN(str) + \
+					ZYNQ##str##_GPIO_BANK3_NGPIO - 1)
+#define ZYNQ_GPIO_BANK4_PIN_MIN(str)	(ZYNQ_GPIO_BANK3_PIN_MAX(str) + 1)
+#define ZYNQ_GPIO_BANK4_PIN_MAX(str)	(ZYNQ_GPIO_BANK4_PIN_MIN(str) + \
+					ZYNQ##str##_GPIO_BANK4_NGPIO - 1)
+#define ZYNQ_GPIO_BANK5_PIN_MIN(str)	(ZYNQ_GPIO_BANK4_PIN_MAX(str) + 1)
+#define ZYNQ_GPIO_BANK5_PIN_MAX(str)	(ZYNQ_GPIO_BANK5_PIN_MIN(str) + \
+					ZYNQ##str##_GPIO_BANK5_NGPIO - 1)
 
 
 /* Register offsets for the GPIO device */
@@ -89,12 +102,30 @@
  * @base_addr:	base address of the GPIO device
  * @clk:	clock resource for this controller
  * @irq:	interrupt for the GPIO device
+ * @p_data:	pointer to platform data
  */
 struct zynq_gpio {
 	struct gpio_chip chip;
 	void __iomem *base_addr;
 	struct clk *clk;
 	int irq;
+	const struct zynq_platform_data *p_data;
+};
+
+/**
+ * struct zynq_platform_data -  zynq gpio platform data structure
+ * @label:	string to store in gpio->label
+ * @ngpio:	max number of gpio pins
+ * @max_bank:	maximum number of gpio banks
+ * @bank_min:	this array represents bank's min pin
+ * @bank_max:	this array represents bank's max pin
+*/
+struct zynq_platform_data {
+	const char *label;
+	u16 ngpio;
+	int max_bank;
+	int bank_min[ZYNQMP_GPIO_MAX_BANK];
+	int bank_max[ZYNQMP_GPIO_MAX_BANK];
 };
 
 static struct irq_chip zynq_gpio_level_irqchip;
@@ -112,39 +143,26 @@ static struct irq_chip zynq_gpio_edge_irqchip;
  */
 static inline void zynq_gpio_get_bank_pin(unsigned int pin_num,
 					  unsigned int *bank_num,
-					  unsigned int *bank_pin_num)
+					  unsigned int *bank_pin_num,
+					  struct zynq_gpio *gpio)
 {
-	switch (pin_num) {
-	case ZYNQ_GPIO_BANK0_PIN_MIN ... ZYNQ_GPIO_BANK0_PIN_MAX:
-		*bank_num = 0;
-		*bank_pin_num = pin_num;
-		break;
-	case ZYNQ_GPIO_BANK1_PIN_MIN ... ZYNQ_GPIO_BANK1_PIN_MAX:
-		*bank_num = 1;
-		*bank_pin_num = pin_num - ZYNQ_GPIO_BANK1_PIN_MIN;
-		break;
-	case ZYNQ_GPIO_BANK2_PIN_MIN ... ZYNQ_GPIO_BANK2_PIN_MAX:
-		*bank_num = 2;
-		*bank_pin_num = pin_num - ZYNQ_GPIO_BANK2_PIN_MIN;
-		break;
-	case ZYNQ_GPIO_BANK3_PIN_MIN ... ZYNQ_GPIO_BANK3_PIN_MAX:
-		*bank_num = 3;
-		*bank_pin_num = pin_num - ZYNQ_GPIO_BANK3_PIN_MIN;
-		break;
-	default:
-		WARN(true, "invalid GPIO pin number: %u", pin_num);
-		*bank_num = 0;
-		*bank_pin_num = 0;
-		break;
+	int bank;
+
+	for (bank = 0; bank < gpio->p_data->max_bank; bank++) {
+		if ((pin_num >= gpio->p_data->bank_min[bank]) &&
+			(pin_num <= gpio->p_data->bank_max[bank])) {
+				*bank_num = bank;
+				*bank_pin_num = pin_num -
+						gpio->p_data->bank_min[bank];
+				return;
+		}
 	}
-}
 
-static const unsigned int zynq_gpio_bank_offset[] = {
-	ZYNQ_GPIO_BANK0_PIN_MIN,
-	ZYNQ_GPIO_BANK1_PIN_MIN,
-	ZYNQ_GPIO_BANK2_PIN_MIN,
-	ZYNQ_GPIO_BANK3_PIN_MIN,
-};
+	/* default */
+	WARN(true, "invalid GPIO pin number: %u", pin_num);
+	*bank_num = 0;
+	*bank_pin_num = 0;
+}
 
 /**
  * zynq_gpio_get_value - Get the state of the specified pin of GPIO device
@@ -161,7 +179,7 @@ static int zynq_gpio_get_value(struct gpio_chip *chip, unsigned int pin)
 	unsigned int bank_num, bank_pin_num;
 	struct zynq_gpio *gpio = container_of(chip, struct zynq_gpio, chip);
 
-	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num);
+	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num, gpio);
 
 	data = readl_relaxed(gpio->base_addr +
 			     ZYNQ_GPIO_DATA_RO_OFFSET(bank_num));
@@ -185,7 +203,7 @@ static void zynq_gpio_set_value(struct gpio_chip *chip, unsigned int pin,
 	unsigned int reg_offset, bank_num, bank_pin_num;
 	struct zynq_gpio *gpio = container_of(chip, struct zynq_gpio, chip);
 
-	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num);
+	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num, gpio);
 
 	if (bank_pin_num >= ZYNQ_GPIO_MID_PIN_NUM) {
 		/* only 16 data bits in bit maskable reg */
@@ -222,7 +240,7 @@ static int zynq_gpio_dir_in(struct gpio_chip *chip, unsigned int pin)
 	unsigned int bank_num, bank_pin_num;
 	struct zynq_gpio *gpio = container_of(chip, struct zynq_gpio, chip);
 
-	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num);
+	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num, gpio);
 
 	/* bank 0 pins 7 and 8 are special and cannot be used as inputs */
 	if (bank_num == 0 && (bank_pin_num == 7 || bank_pin_num == 8))
@@ -255,7 +273,7 @@ static int zynq_gpio_dir_out(struct gpio_chip *chip, unsigned int pin,
 	unsigned int bank_num, bank_pin_num;
 	struct zynq_gpio *gpio = container_of(chip, struct zynq_gpio, chip);
 
-	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num);
+	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num, gpio);
 
 	/* set the GPIO pin as output */
 	reg = readl_relaxed(gpio->base_addr + ZYNQ_GPIO_DIRM_OFFSET(bank_num));
@@ -286,7 +304,7 @@ static void zynq_gpio_irq_mask(struct irq_data *irq_data)
 	struct zynq_gpio *gpio = irq_data_get_irq_chip_data(irq_data);
 
 	device_pin_num = irq_data->hwirq;
-	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num);
+	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num, gpio);
 	writel_relaxed(BIT(bank_pin_num),
 		       gpio->base_addr + ZYNQ_GPIO_INTDIS_OFFSET(bank_num));
 }
@@ -306,7 +324,7 @@ static void zynq_gpio_irq_unmask(struct irq_data *irq_data)
 	struct zynq_gpio *gpio = irq_data_get_irq_chip_data(irq_data);
 
 	device_pin_num = irq_data->hwirq;
-	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num);
+	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num, gpio);
 	writel_relaxed(BIT(bank_pin_num),
 		       gpio->base_addr + ZYNQ_GPIO_INTEN_OFFSET(bank_num));
 }
@@ -325,7 +343,7 @@ static void zynq_gpio_irq_ack(struct irq_data *irq_data)
 	struct zynq_gpio *gpio = irq_data_get_irq_chip_data(irq_data);
 
 	device_pin_num = irq_data->hwirq;
-	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num);
+	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num, gpio);
 	writel_relaxed(BIT(bank_pin_num),
 		       gpio->base_addr + ZYNQ_GPIO_INTSTS_OFFSET(bank_num));
 }
@@ -335,7 +353,7 @@ static void zynq_gpio_irq_ack(struct irq_data *irq_data)
  * @irq_data:	irq data containing irq number of gpio pin for the interrupt
  *		to enable
  *
- * Clears the INTSTS bit and unmasks the given interrrupt.
+ * Clears the INTSTS bit and unmasks the given interrupt.
  */
 static void zynq_gpio_irq_enable(struct irq_data *irq_data)
 {
@@ -375,7 +393,7 @@ static int zynq_gpio_set_irq_type(struct irq_data *irq_data, unsigned int type)
 	struct zynq_gpio *gpio = irq_data_get_irq_chip_data(irq_data);
 
 	device_pin_num = irq_data->hwirq;
-	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num);
+	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num, gpio);
 
 	int_type = readl_relaxed(gpio->base_addr +
 				 ZYNQ_GPIO_INTTYPE_OFFSET(bank_num));
@@ -470,7 +488,7 @@ static void zynq_gpio_handle_bank_irq(struct zynq_gpio *gpio,
 				      unsigned int bank_num,
 				      unsigned long pending)
 {
-	unsigned int bank_offset = zynq_gpio_bank_offset[bank_num];
+	unsigned int bank_offset = gpio->p_data->bank_min[bank_num];
 	struct irq_domain *irqdomain = gpio->chip.irqdomain;
 	int offset;
 
@@ -505,7 +523,7 @@ static void zynq_gpio_irqhandler(unsigned int irq, struct irq_desc *desc)
 
 	chained_irq_enter(irqchip, desc);
 
-	for (bank_num = 0; bank_num < ZYNQ_GPIO_MAX_BANK; bank_num++) {
+	for (bank_num = 0; bank_num < gpio->p_data->max_bank; bank_num++) {
 		int_sts = readl_relaxed(gpio->base_addr +
 					ZYNQ_GPIO_INTSTS_OFFSET(bank_num));
 		int_enb = readl_relaxed(gpio->base_addr +
@@ -582,6 +600,46 @@ static const struct dev_pm_ops zynq_gpio_dev_pm_ops = {
 			zynq_gpio_runtime_resume, NULL)
 };
 
+static const struct zynq_platform_data zynqmp_gpio_def = {
+	.label = "zynqmp_gpio",
+	.ngpio = ZYNQMP_GPIO_NR_GPIOS,
+	.max_bank = ZYNQMP_GPIO_MAX_BANK,
+	.bank_min[0] = ZYNQ_GPIO_BANK0_PIN_MIN(MP),
+	.bank_max[0] = ZYNQ_GPIO_BANK0_PIN_MAX(MP),
+	.bank_min[1] = ZYNQ_GPIO_BANK1_PIN_MIN(MP),
+	.bank_max[1] = ZYNQ_GPIO_BANK1_PIN_MAX(MP),
+	.bank_min[2] = ZYNQ_GPIO_BANK2_PIN_MIN(MP),
+	.bank_max[2] = ZYNQ_GPIO_BANK2_PIN_MAX(MP),
+	.bank_min[3] = ZYNQ_GPIO_BANK3_PIN_MIN(MP),
+	.bank_max[3] = ZYNQ_GPIO_BANK3_PIN_MAX(MP),
+	.bank_min[4] = ZYNQ_GPIO_BANK4_PIN_MIN(MP),
+	.bank_max[4] = ZYNQ_GPIO_BANK4_PIN_MAX(MP),
+	.bank_min[5] = ZYNQ_GPIO_BANK5_PIN_MIN(MP),
+	.bank_max[5] = ZYNQ_GPIO_BANK5_PIN_MAX(MP),
+};
+
+static const struct zynq_platform_data zynq_gpio_def = {
+	.label = "zynq_gpio",
+	.ngpio = ZYNQ_GPIO_NR_GPIOS,
+	.max_bank = ZYNQ_GPIO_MAX_BANK,
+	.bank_min[0] = ZYNQ_GPIO_BANK0_PIN_MIN(),
+	.bank_max[0] = ZYNQ_GPIO_BANK0_PIN_MAX(),
+	.bank_min[1] = ZYNQ_GPIO_BANK1_PIN_MIN(),
+	.bank_max[1] = ZYNQ_GPIO_BANK1_PIN_MAX(),
+	.bank_min[2] = ZYNQ_GPIO_BANK2_PIN_MIN(),
+	.bank_max[2] = ZYNQ_GPIO_BANK2_PIN_MAX(),
+	.bank_min[3] = ZYNQ_GPIO_BANK3_PIN_MIN(),
+	.bank_max[3] = ZYNQ_GPIO_BANK3_PIN_MAX(),
+};
+
+static const struct of_device_id zynq_gpio_of_match[] = {
+	{ .compatible = "xlnx,zynq-gpio-1.0", .data = (void *)&zynq_gpio_def },
+	{ .compatible = "xlnx,zynqmp-gpio-1.0",
+					.data = (void *)&zynqmp_gpio_def },
+	{ /* end of table */ }
+};
+MODULE_DEVICE_TABLE(of, zynq_gpio_of_match);
+
 /**
  * zynq_gpio_probe - Initialization method for a zynq_gpio device
  * @pdev:	platform device instance
@@ -599,11 +657,18 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 	struct zynq_gpio *gpio;
 	struct gpio_chip *chip;
 	struct resource *res;
+	const struct of_device_id *match;
 
 	gpio = devm_kzalloc(&pdev->dev, sizeof(*gpio), GFP_KERNEL);
 	if (!gpio)
 		return -ENOMEM;
 
+	match = of_match_node(zynq_gpio_of_match, pdev->dev.of_node);
+	if (!match) {
+		dev_err(&pdev->dev, "of_match_node() failed\n");
+		return -EINVAL;
+	}
+	gpio->p_data = match->data;
 	platform_set_drvdata(pdev, gpio);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -619,7 +684,7 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 
 	/* configure the gpio chip */
 	chip = &gpio->chip;
-	chip->label = "zynq_gpio";
+	chip->label = gpio->p_data->label;
 	chip->owner = THIS_MODULE;
 	chip->dev = &pdev->dev;
 	chip->get = zynq_gpio_get_value;
@@ -629,7 +694,7 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 	chip->direction_input = zynq_gpio_dir_in;
 	chip->direction_output = zynq_gpio_dir_out;
 	chip->base = -1;
-	chip->ngpio = ZYNQ_GPIO_NR_GPIOS;
+	chip->ngpio = gpio->p_data->ngpio;
 
 	/* Enable GPIO clock */
 	gpio->clk = devm_clk_get(&pdev->dev, NULL);
@@ -651,7 +716,7 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 	}
 
 	/* disable interrupts for all banks */
-	for (bank_num = 0; bank_num < ZYNQ_GPIO_MAX_BANK; bank_num++)
+	for (bank_num = 0; bank_num < gpio->p_data->max_bank; bank_num++)
 		writel_relaxed(ZYNQ_GPIO_IXR_DISABLE_ALL, gpio->base_addr +
 			       ZYNQ_GPIO_INTDIS_OFFSET(bank_num));
 
@@ -695,12 +760,6 @@ static int zynq_gpio_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id zynq_gpio_of_match[] = {
-	{ .compatible = "xlnx,zynq-gpio-1.0", },
-	{ /* end of table */ }
-};
-MODULE_DEVICE_TABLE(of, zynq_gpio_of_match);
-
 static struct platform_driver zynq_gpio_driver = {
 	.driver	= {
 		.name = DRIVER_NAME,
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index 725d16138b74..533fe5dbe6f8 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -114,10 +114,11 @@ static inline int acpi_gpiochip_pin_to_gpio_offset(struct gpio_chip *chip,
  * @path:	ACPI GPIO controller full path name, (e.g. "\\_SB.GPO1")
  * @pin:	ACPI GPIO pin number (0-based, controller-relative)
  *
- * Returns GPIO descriptor to use with Linux generic GPIO API, or ERR_PTR
- * error value
+ * Return: GPIO descriptor to use with Linux generic GPIO API, or ERR_PTR
+ * error value. Specifically returns %-EPROBE_DEFER if the referenced GPIO
+ * controller does not have gpiochip registered at the moment. This is to
+ * support probe deferral.
  */
-
 static struct gpio_desc *acpi_get_gpiod(char *path, int pin)
 {
 	struct gpio_chip *chip;
@@ -131,7 +132,7 @@ static struct gpio_desc *acpi_get_gpiod(char *path, int pin)
 
 	chip = gpiochip_find(handle, acpi_gpiochip_find);
 	if (!chip)
-		return ERR_PTR(-ENODEV);
+		return ERR_PTR(-EPROBE_DEFER);
 
 	offset = acpi_gpiochip_pin_to_gpio_offset(chip, pin);
 	if (offset < 0)
@@ -307,6 +308,7 @@ void acpi_gpiochip_request_interrupts(struct gpio_chip *chip)
 	acpi_walk_resources(handle, "_AEI",
 			    acpi_gpiochip_request_interrupt, acpi_gpio);
 }
+EXPORT_SYMBOL_GPL(acpi_gpiochip_request_interrupts);
 
 /**
  * acpi_gpiochip_free_interrupts() - Free GPIO ACPI event interrupts.
@@ -346,6 +348,7 @@ void acpi_gpiochip_free_interrupts(struct gpio_chip *chip)
 		kfree(event);
 	}
 }
+EXPORT_SYMBOL_GPL(acpi_gpiochip_free_interrupts);
 
 int acpi_dev_add_driver_gpios(struct acpi_device *adev,
 			      const struct acpi_gpio_mapping *gpios)
@@ -514,6 +517,35 @@ struct gpio_desc *acpi_get_gpiod_by_index(struct acpi_device *adev,
 	return lookup.desc ? lookup.desc : ERR_PTR(-ENOENT);
 }
 
+/**
+ * acpi_dev_gpio_irq_get() - Find GpioInt and translate it to Linux IRQ number
+ * @adev: pointer to a ACPI device to get IRQ from
+ * @index: index of GpioInt resource (starting from %0)
+ *
+ * If the device has one or more GpioInt resources, this function can be
+ * used to translate from the GPIO offset in the resource to the Linux IRQ
+ * number.
+ *
+ * Return: Linux IRQ number (>%0) on success, negative errno on failure.
+ */
+int acpi_dev_gpio_irq_get(struct acpi_device *adev, int index)
+{
+	int idx, i;
+
+	for (i = 0, idx = 0; idx <= index; i++) {
+		struct acpi_gpio_info info;
+		struct gpio_desc *desc;
+
+		desc = acpi_get_gpiod_by_index(adev, NULL, i, &info);
+		if (IS_ERR(desc))
+			break;
+		if (info.gpioint && idx++ == index)
+			return gpiod_to_irq(desc);
+	}
+	return -ENOENT;
+}
+EXPORT_SYMBOL_GPL(acpi_dev_gpio_irq_get);
+
 static acpi_status
 acpi_gpio_adr_space_handler(u32 function, acpi_physical_address address,
 			    u32 bits, u64 *value, void *handler_context,
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index a6c67c6b4680..9a0ec48a4737 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -242,7 +242,7 @@ int of_gpio_simple_xlate(struct gpio_chip *gc,
 {
 	/*
 	 * We're discouraging gpio_cells < 2, since that way you'll have to
-	 * write your own xlate function (that will have to retrive the GPIO
+	 * write your own xlate function (that will have to retrieve the GPIO
 	 * number and the flags from a single gpio cell -- this is possible,
 	 * but not recommended).
 	 */
diff --git a/drivers/gpio/gpiolib-sysfs.c b/drivers/gpio/gpiolib-sysfs.c
index af3bc7a8033b..b57ed8e55ab5 100644
--- a/drivers/gpio/gpiolib-sysfs.c
+++ b/drivers/gpio/gpiolib-sysfs.c
@@ -6,14 +6,29 @@
 #include <linux/gpio/driver.h>
 #include <linux/interrupt.h>
 #include <linux/kdev_t.h>
+#include <linux/slab.h>
 
 #include "gpiolib.h"
 
-static DEFINE_IDR(dirent_idr);
+#define GPIO_IRQF_TRIGGER_FALLING	BIT(0)
+#define GPIO_IRQF_TRIGGER_RISING	BIT(1)
+#define GPIO_IRQF_TRIGGER_BOTH		(GPIO_IRQF_TRIGGER_FALLING | \
+					 GPIO_IRQF_TRIGGER_RISING)
 
+struct gpiod_data {
+	struct gpio_desc *desc;
+
+	struct mutex mutex;
+	struct kernfs_node *value_kn;
+	int irq;
+	unsigned char irq_flags;
 
-/* lock protects against unexport_gpio() being called while
- * sysfs files are active.
+	bool direction_can_change;
+};
+
+/*
+ * Lock to serialise gpiod export and unexport, and prevent re-export of
+ * gpiod whose chip is being unregistered.
  */
 static DEFINE_MUTEX(sysfs_lock);
 
@@ -38,38 +53,35 @@ static DEFINE_MUTEX(sysfs_lock);
  *        /edge configuration
  */
 
-static ssize_t gpio_direction_show(struct device *dev,
+static ssize_t direction_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct gpio_desc	*desc = dev_get_drvdata(dev);
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	struct gpio_desc *desc = data->desc;
 	ssize_t			status;
 
-	mutex_lock(&sysfs_lock);
+	mutex_lock(&data->mutex);
 
-	if (!test_bit(FLAG_EXPORT, &desc->flags)) {
-		status = -EIO;
-	} else {
-		gpiod_get_direction(desc);
-		status = sprintf(buf, "%s\n",
+	gpiod_get_direction(desc);
+	status = sprintf(buf, "%s\n",
 			test_bit(FLAG_IS_OUT, &desc->flags)
 				? "out" : "in");
-	}
 
-	mutex_unlock(&sysfs_lock);
+	mutex_unlock(&data->mutex);
+
 	return status;
 }
 
-static ssize_t gpio_direction_store(struct device *dev,
+static ssize_t direction_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
-	struct gpio_desc	*desc = dev_get_drvdata(dev);
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	struct gpio_desc *desc = data->desc;
 	ssize_t			status;
 
-	mutex_lock(&sysfs_lock);
+	mutex_lock(&data->mutex);
 
-	if (!test_bit(FLAG_EXPORT, &desc->flags))
-		status = -EIO;
-	else if (sysfs_streq(buf, "high"))
+	if (sysfs_streq(buf, "high"))
 		status = gpiod_direction_output_raw(desc, 1);
 	else if (sysfs_streq(buf, "out") || sysfs_streq(buf, "low"))
 		status = gpiod_direction_output_raw(desc, 0);
@@ -78,43 +90,40 @@ static ssize_t gpio_direction_store(struct device *dev,
 	else
 		status = -EINVAL;
 
-	mutex_unlock(&sysfs_lock);
+	mutex_unlock(&data->mutex);
+
 	return status ? : size;
 }
+static DEVICE_ATTR_RW(direction);
 
-static /* const */ DEVICE_ATTR(direction, 0644,
-		gpio_direction_show, gpio_direction_store);
-
-static ssize_t gpio_value_show(struct device *dev,
+static ssize_t value_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	struct gpio_desc	*desc = dev_get_drvdata(dev);
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	struct gpio_desc *desc = data->desc;
 	ssize_t			status;
 
-	mutex_lock(&sysfs_lock);
+	mutex_lock(&data->mutex);
 
-	if (!test_bit(FLAG_EXPORT, &desc->flags))
-		status = -EIO;
-	else
-		status = sprintf(buf, "%d\n", gpiod_get_value_cansleep(desc));
+	status = sprintf(buf, "%d\n", gpiod_get_value_cansleep(desc));
+
+	mutex_unlock(&data->mutex);
 
-	mutex_unlock(&sysfs_lock);
 	return status;
 }
 
-static ssize_t gpio_value_store(struct device *dev,
+static ssize_t value_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
-	struct gpio_desc	*desc = dev_get_drvdata(dev);
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	struct gpio_desc *desc = data->desc;
 	ssize_t			status;
 
-	mutex_lock(&sysfs_lock);
+	mutex_lock(&data->mutex);
 
-	if (!test_bit(FLAG_EXPORT, &desc->flags))
-		status = -EIO;
-	else if (!test_bit(FLAG_IS_OUT, &desc->flags))
+	if (!test_bit(FLAG_IS_OUT, &desc->flags)) {
 		status = -EPERM;
-	else {
+	} else {
 		long		value;
 
 		status = kstrtol(buf, 0, &value);
@@ -124,172 +133,168 @@ static ssize_t gpio_value_store(struct device *dev,
 		}
 	}
 
-	mutex_unlock(&sysfs_lock);
+	mutex_unlock(&data->mutex);
+
 	return status;
 }
-
-static DEVICE_ATTR(value, 0644,
-		gpio_value_show, gpio_value_store);
+static DEVICE_ATTR_RW(value);
 
 static irqreturn_t gpio_sysfs_irq(int irq, void *priv)
 {
-	struct kernfs_node	*value_sd = priv;
+	struct gpiod_data *data = priv;
+
+	sysfs_notify_dirent(data->value_kn);
 
-	sysfs_notify_dirent(value_sd);
 	return IRQ_HANDLED;
 }
 
-static int gpio_setup_irq(struct gpio_desc *desc, struct device *dev,
-		unsigned long gpio_flags)
+/* Caller holds gpiod-data mutex. */
+static int gpio_sysfs_request_irq(struct device *dev, unsigned char flags)
 {
-	struct kernfs_node	*value_sd;
+	struct gpiod_data	*data = dev_get_drvdata(dev);
+	struct gpio_desc	*desc = data->desc;
 	unsigned long		irq_flags;
-	int			ret, irq, id;
+	int			ret;
 
-	if ((desc->flags & GPIO_TRIGGER_MASK) == gpio_flags)
-		return 0;
-
-	irq = gpiod_to_irq(desc);
-	if (irq < 0)
+	data->irq = gpiod_to_irq(desc);
+	if (data->irq < 0)
 		return -EIO;
 
-	id = desc->flags >> ID_SHIFT;
-	value_sd = idr_find(&dirent_idr, id);
-	if (value_sd)
-		free_irq(irq, value_sd);
-
-	desc->flags &= ~GPIO_TRIGGER_MASK;
-
-	if (!gpio_flags) {
-		gpiochip_unlock_as_irq(desc->chip, gpio_chip_hwgpio(desc));
-		ret = 0;
-		goto free_id;
-	}
+	data->value_kn = sysfs_get_dirent(dev->kobj.sd, "value");
+	if (!data->value_kn)
+		return -ENODEV;
 
 	irq_flags = IRQF_SHARED;
-	if (test_bit(FLAG_TRIG_FALL, &gpio_flags))
+	if (flags & GPIO_IRQF_TRIGGER_FALLING)
 		irq_flags |= test_bit(FLAG_ACTIVE_LOW, &desc->flags) ?
 			IRQF_TRIGGER_RISING : IRQF_TRIGGER_FALLING;
-	if (test_bit(FLAG_TRIG_RISE, &gpio_flags))
+	if (flags & GPIO_IRQF_TRIGGER_RISING)
 		irq_flags |= test_bit(FLAG_ACTIVE_LOW, &desc->flags) ?
 			IRQF_TRIGGER_FALLING : IRQF_TRIGGER_RISING;
 
-	if (!value_sd) {
-		value_sd = sysfs_get_dirent(dev->kobj.sd, "value");
-		if (!value_sd) {
-			ret = -ENODEV;
-			goto err_out;
-		}
-
-		ret = idr_alloc(&dirent_idr, value_sd, 1, 0, GFP_KERNEL);
-		if (ret < 0)
-			goto free_sd;
-		id = ret;
-
-		desc->flags &= GPIO_FLAGS_MASK;
-		desc->flags |= (unsigned long)id << ID_SHIFT;
-
-		if (desc->flags >> ID_SHIFT != id) {
-			ret = -ERANGE;
-			goto free_id;
-		}
-	}
+	/*
+	 * FIXME: This should be done in the irq_request_resources callback
+	 *        when the irq is requested, but a few drivers currently fail
+	 *        to do so.
+	 *
+	 *        Remove this redundant call (along with the corresponding
+	 *        unlock) when those drivers have been fixed.
+	 */
+	ret = gpiochip_lock_as_irq(desc->chip, gpio_chip_hwgpio(desc));
+	if (ret < 0)
+		goto err_put_kn;
 
-	ret = request_any_context_irq(irq, gpio_sysfs_irq, irq_flags,
-				"gpiolib", value_sd);
+	ret = request_any_context_irq(data->irq, gpio_sysfs_irq, irq_flags,
+				"gpiolib", data);
 	if (ret < 0)
-		goto free_id;
+		goto err_unlock;
 
-	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;
-	}
+	data->irq_flags = flags;
 
-	desc->flags |= gpio_flags;
 	return 0;
 
-free_id:
-	idr_remove(&dirent_idr, id);
-	desc->flags &= GPIO_FLAGS_MASK;
-free_sd:
-	if (value_sd)
-		sysfs_put(value_sd);
-err_out:
+err_unlock:
+	gpiochip_unlock_as_irq(desc->chip, gpio_chip_hwgpio(desc));
+err_put_kn:
+	sysfs_put(data->value_kn);
+
 	return ret;
 }
 
+/*
+ * Caller holds gpiod-data mutex (unless called after class-device
+ * deregistration).
+ */
+static void gpio_sysfs_free_irq(struct device *dev)
+{
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	struct gpio_desc *desc = data->desc;
+
+	data->irq_flags = 0;
+	free_irq(data->irq, data);
+	gpiochip_unlock_as_irq(desc->chip, gpio_chip_hwgpio(desc));
+	sysfs_put(data->value_kn);
+}
+
 static const struct {
 	const char *name;
-	unsigned long flags;
+	unsigned char flags;
 } trigger_types[] = {
 	{ "none",    0 },
-	{ "falling", BIT(FLAG_TRIG_FALL) },
-	{ "rising",  BIT(FLAG_TRIG_RISE) },
-	{ "both",    BIT(FLAG_TRIG_FALL) | BIT(FLAG_TRIG_RISE) },
+	{ "falling", GPIO_IRQF_TRIGGER_FALLING },
+	{ "rising",  GPIO_IRQF_TRIGGER_RISING },
+	{ "both",    GPIO_IRQF_TRIGGER_BOTH },
 };
 
-static ssize_t gpio_edge_show(struct device *dev,
+static ssize_t edge_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	const struct gpio_desc	*desc = dev_get_drvdata(dev);
-	ssize_t			status;
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	ssize_t	status = 0;
+	int i;
 
-	mutex_lock(&sysfs_lock);
-
-	if (!test_bit(FLAG_EXPORT, &desc->flags))
-		status = -EIO;
-	else {
-		int i;
+	mutex_lock(&data->mutex);
 
-		status = 0;
-		for (i = 0; i < ARRAY_SIZE(trigger_types); i++)
-			if ((desc->flags & GPIO_TRIGGER_MASK)
-					== trigger_types[i].flags) {
-				status = sprintf(buf, "%s\n",
-						 trigger_types[i].name);
-				break;
-			}
+	for (i = 0; i < ARRAY_SIZE(trigger_types); i++) {
+		if (data->irq_flags == trigger_types[i].flags) {
+			status = sprintf(buf, "%s\n", trigger_types[i].name);
+			break;
+		}
 	}
 
-	mutex_unlock(&sysfs_lock);
+	mutex_unlock(&data->mutex);
+
 	return status;
 }
 
-static ssize_t gpio_edge_store(struct device *dev,
+static ssize_t edge_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
-	struct gpio_desc	*desc = dev_get_drvdata(dev);
-	ssize_t			status;
-	int			i;
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	unsigned char flags;
+	ssize_t	status = size;
+	int i;
 
-	for (i = 0; i < ARRAY_SIZE(trigger_types); i++)
+	for (i = 0; i < ARRAY_SIZE(trigger_types); i++) {
 		if (sysfs_streq(trigger_types[i].name, buf))
-			goto found;
-	return -EINVAL;
+			break;
+	}
 
-found:
-	mutex_lock(&sysfs_lock);
+	if (i == ARRAY_SIZE(trigger_types))
+		return -EINVAL;
 
-	if (!test_bit(FLAG_EXPORT, &desc->flags))
-		status = -EIO;
-	else {
-		status = gpio_setup_irq(desc, dev, trigger_types[i].flags);
+	flags = trigger_types[i].flags;
+
+	mutex_lock(&data->mutex);
+
+	if (flags == data->irq_flags) {
+		status = size;
+		goto out_unlock;
+	}
+
+	if (data->irq_flags)
+		gpio_sysfs_free_irq(dev);
+
+	if (flags) {
+		status = gpio_sysfs_request_irq(dev, flags);
 		if (!status)
 			status = size;
 	}
 
-	mutex_unlock(&sysfs_lock);
+out_unlock:
+	mutex_unlock(&data->mutex);
 
 	return status;
 }
+static DEVICE_ATTR_RW(edge);
 
-static DEVICE_ATTR(edge, 0644, gpio_edge_show, gpio_edge_store);
-
-static int sysfs_set_active_low(struct gpio_desc *desc, struct device *dev,
-				int value)
+/* Caller holds gpiod-data mutex. */
+static int gpio_sysfs_set_active_low(struct device *dev, int value)
 {
+	struct gpiod_data	*data = dev_get_drvdata(dev);
+	struct gpio_desc	*desc = data->desc;
 	int			status = 0;
+	unsigned int		flags = data->irq_flags;
 
 	if (!!test_bit(FLAG_ACTIVE_LOW, &desc->flags) == !!value)
 		return 0;
@@ -300,69 +305,59 @@ static int sysfs_set_active_low(struct gpio_desc *desc, struct device *dev,
 		clear_bit(FLAG_ACTIVE_LOW, &desc->flags);
 
 	/* reconfigure poll(2) support if enabled on one edge only */
-	if (dev != NULL && (!!test_bit(FLAG_TRIG_RISE, &desc->flags) ^
-				!!test_bit(FLAG_TRIG_FALL, &desc->flags))) {
-		unsigned long trigger_flags = desc->flags & GPIO_TRIGGER_MASK;
-
-		gpio_setup_irq(desc, dev, 0);
-		status = gpio_setup_irq(desc, dev, trigger_flags);
+	if (flags == GPIO_IRQF_TRIGGER_FALLING ||
+					flags == GPIO_IRQF_TRIGGER_RISING) {
+		gpio_sysfs_free_irq(dev);
+		status = gpio_sysfs_request_irq(dev, flags);
 	}
 
 	return status;
 }
 
-static ssize_t gpio_active_low_show(struct device *dev,
+static ssize_t active_low_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
-	const struct gpio_desc	*desc = dev_get_drvdata(dev);
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	struct gpio_desc *desc = data->desc;
 	ssize_t			status;
 
-	mutex_lock(&sysfs_lock);
+	mutex_lock(&data->mutex);
 
-	if (!test_bit(FLAG_EXPORT, &desc->flags))
-		status = -EIO;
-	else
-		status = sprintf(buf, "%d\n",
+	status = sprintf(buf, "%d\n",
 				!!test_bit(FLAG_ACTIVE_LOW, &desc->flags));
 
-	mutex_unlock(&sysfs_lock);
+	mutex_unlock(&data->mutex);
 
 	return status;
 }
 
-static ssize_t gpio_active_low_store(struct device *dev,
+static ssize_t active_low_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
-	struct gpio_desc	*desc = dev_get_drvdata(dev);
+	struct gpiod_data	*data = dev_get_drvdata(dev);
 	ssize_t			status;
+	long			value;
 
-	mutex_lock(&sysfs_lock);
+	mutex_lock(&data->mutex);
 
-	if (!test_bit(FLAG_EXPORT, &desc->flags)) {
-		status = -EIO;
-	} else {
-		long		value;
+	status = kstrtol(buf, 0, &value);
+	if (status == 0)
+		status = gpio_sysfs_set_active_low(dev, value);
 
-		status = kstrtol(buf, 0, &value);
-		if (status == 0)
-			status = sysfs_set_active_low(desc, dev, value != 0);
-	}
-
-	mutex_unlock(&sysfs_lock);
+	mutex_unlock(&data->mutex);
 
 	return status ? : size;
 }
-
-static DEVICE_ATTR(active_low, 0644,
-		gpio_active_low_show, gpio_active_low_store);
+static DEVICE_ATTR_RW(active_low);
 
 static umode_t gpio_is_visible(struct kobject *kobj, struct attribute *attr,
 			       int n)
 {
 	struct device *dev = container_of(kobj, struct device, kobj);
-	struct gpio_desc *desc = dev_get_drvdata(dev);
+	struct gpiod_data *data = dev_get_drvdata(dev);
+	struct gpio_desc *desc = data->desc;
 	umode_t mode = attr->mode;
-	bool show_direction = test_bit(FLAG_SYSFS_DIR, &desc->flags);
+	bool show_direction = data->direction_can_change;
 
 	if (attr == &dev_attr_direction.attr) {
 		if (!show_direction)
@@ -402,32 +397,32 @@ static const struct attribute_group *gpio_groups[] = {
  *   /ngpio ... matching gpio_chip.ngpio
  */
 
-static ssize_t chip_base_show(struct device *dev,
+static ssize_t base_show(struct device *dev,
 			       struct device_attribute *attr, char *buf)
 {
 	const struct gpio_chip	*chip = dev_get_drvdata(dev);
 
 	return sprintf(buf, "%d\n", chip->base);
 }
-static DEVICE_ATTR(base, 0444, chip_base_show, NULL);
+static DEVICE_ATTR_RO(base);
 
-static ssize_t chip_label_show(struct device *dev,
+static ssize_t label_show(struct device *dev,
 			       struct device_attribute *attr, char *buf)
 {
 	const struct gpio_chip	*chip = dev_get_drvdata(dev);
 
 	return sprintf(buf, "%s\n", chip->label ? : "");
 }
-static DEVICE_ATTR(label, 0444, chip_label_show, NULL);
+static DEVICE_ATTR_RO(label);
 
-static ssize_t chip_ngpio_show(struct device *dev,
+static ssize_t ngpio_show(struct device *dev,
 			       struct device_attribute *attr, char *buf)
 {
 	const struct gpio_chip	*chip = dev_get_drvdata(dev);
 
 	return sprintf(buf, "%u\n", chip->ngpio);
 }
-static DEVICE_ATTR(ngpio, 0444, chip_ngpio_show, NULL);
+static DEVICE_ATTR_RO(ngpio);
 
 static struct attribute *gpiochip_attrs[] = {
 	&dev_attr_base.attr,
@@ -552,6 +547,7 @@ static struct class gpio_class = {
 int gpiod_export(struct gpio_desc *desc, bool direction_may_change)
 {
 	struct gpio_chip	*chip;
+	struct gpiod_data	*data;
 	unsigned long		flags;
 	int			status;
 	const char		*ioname = NULL;
@@ -574,9 +570,9 @@ int gpiod_export(struct gpio_desc *desc, bool direction_may_change)
 	mutex_lock(&sysfs_lock);
 
 	/* check if chip is being removed */
-	if (!chip || !chip->exported) {
+	if (!chip || !chip->cdev) {
 		status = -ENODEV;
-		goto fail_unlock;
+		goto err_unlock;
 	}
 
 	spin_lock_irqsave(&gpio_lock, flags);
@@ -588,43 +584,54 @@ int gpiod_export(struct gpio_desc *desc, bool direction_may_change)
 				test_bit(FLAG_REQUESTED, &desc->flags),
 				test_bit(FLAG_EXPORT, &desc->flags));
 		status = -EPERM;
-		goto fail_unlock;
+		goto err_unlock;
 	}
+	spin_unlock_irqrestore(&gpio_lock, flags);
 
-	if (desc->chip->direction_input && desc->chip->direction_output &&
-			direction_may_change) {
-		set_bit(FLAG_SYSFS_DIR, &desc->flags);
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		status = -ENOMEM;
+		goto err_unlock;
 	}
 
-	spin_unlock_irqrestore(&gpio_lock, flags);
+	data->desc = desc;
+	mutex_init(&data->mutex);
+	if (chip->direction_input && chip->direction_output)
+		data->direction_can_change = direction_may_change;
+	else
+		data->direction_can_change = false;
 
 	offset = gpio_chip_hwgpio(desc);
-	if (desc->chip->names && desc->chip->names[offset])
-		ioname = desc->chip->names[offset];
+	if (chip->names && chip->names[offset])
+		ioname = chip->names[offset];
 
-	dev = device_create_with_groups(&gpio_class, desc->chip->dev,
-					MKDEV(0, 0), desc, gpio_groups,
+	dev = device_create_with_groups(&gpio_class, chip->dev,
+					MKDEV(0, 0), data, gpio_groups,
 					ioname ? ioname : "gpio%u",
 					desc_to_gpio(desc));
 	if (IS_ERR(dev)) {
 		status = PTR_ERR(dev);
-		goto fail_unlock;
+		goto err_free_data;
 	}
 
 	set_bit(FLAG_EXPORT, &desc->flags);
 	mutex_unlock(&sysfs_lock);
 	return 0;
 
-fail_unlock:
+err_free_data:
+	kfree(data);
+err_unlock:
 	mutex_unlock(&sysfs_lock);
 	gpiod_dbg(desc, "%s: status %d\n", __func__, status);
 	return status;
 }
 EXPORT_SYMBOL_GPL(gpiod_export);
 
-static int match_export(struct device *dev, const void *data)
+static int match_export(struct device *dev, const void *desc)
 {
-	return dev_get_drvdata(dev) == data;
+	struct gpiod_data *data = dev_get_drvdata(dev);
+
+	return data->desc == desc;
 }
 
 /**
@@ -641,82 +648,26 @@ static int match_export(struct device *dev, const void *data)
 int gpiod_export_link(struct device *dev, const char *name,
 		      struct gpio_desc *desc)
 {
-	int			status = -EINVAL;
+	struct device *cdev;
+	int ret;
 
 	if (!desc) {
 		pr_warn("%s: invalid GPIO\n", __func__);
 		return -EINVAL;
 	}
 
-	mutex_lock(&sysfs_lock);
-
-	if (test_bit(FLAG_EXPORT, &desc->flags)) {
-		struct device *tdev;
-
-		tdev = class_find_device(&gpio_class, NULL, desc, match_export);
-		if (tdev != NULL) {
-			status = sysfs_create_link(&dev->kobj, &tdev->kobj,
-						name);
-			put_device(tdev);
-		} else {
-			status = -ENODEV;
-		}
-	}
-
-	mutex_unlock(&sysfs_lock);
+	cdev = class_find_device(&gpio_class, NULL, desc, match_export);
+	if (!cdev)
+		return -ENODEV;
 
-	if (status)
-		gpiod_dbg(desc, "%s: status %d\n", __func__, status);
+	ret = sysfs_create_link(&dev->kobj, &cdev->kobj, name);
+	put_device(cdev);
 
-	return status;
+	return ret;
 }
 EXPORT_SYMBOL_GPL(gpiod_export_link);
 
 /**
- * gpiod_sysfs_set_active_low - set the polarity of gpio sysfs value
- * @gpio: gpio to change
- * @value: non-zero to use active low, i.e. inverted values
- *
- * Set the polarity of /sys/class/gpio/gpioN/value sysfs attribute.
- * The GPIO does not have to be exported yet.  If poll(2) support has
- * been enabled for either rising or falling edge, it will be
- * reconfigured to follow the new polarity.
- *
- * Returns zero on success, else an error.
- */
-int gpiod_sysfs_set_active_low(struct gpio_desc *desc, int value)
-{
-	struct device		*dev = NULL;
-	int			status = -EINVAL;
-
-	if (!desc) {
-		pr_warn("%s: invalid GPIO\n", __func__);
-		return -EINVAL;
-	}
-
-	mutex_lock(&sysfs_lock);
-
-	if (test_bit(FLAG_EXPORT, &desc->flags)) {
-		dev = class_find_device(&gpio_class, NULL, desc, match_export);
-		if (dev == NULL) {
-			status = -ENODEV;
-			goto unlock;
-		}
-	}
-
-	status = sysfs_set_active_low(desc, dev, value);
-	put_device(dev);
-unlock:
-	mutex_unlock(&sysfs_lock);
-
-	if (status)
-		gpiod_dbg(desc, "%s: status %d\n", __func__, status);
-
-	return status;
-}
-EXPORT_SYMBOL_GPL(gpiod_sysfs_set_active_low);
-
-/**
  * gpiod_unexport - reverse effect of gpio_export()
  * @gpio: gpio to make unavailable
  *
@@ -724,8 +675,8 @@ EXPORT_SYMBOL_GPL(gpiod_sysfs_set_active_low);
  */
 void gpiod_unexport(struct gpio_desc *desc)
 {
-	int			status = 0;
-	struct device		*dev = NULL;
+	struct gpiod_data *data;
+	struct device *dev;
 
 	if (!desc) {
 		pr_warn("%s: invalid GPIO\n", __func__);
@@ -734,82 +685,79 @@ void gpiod_unexport(struct gpio_desc *desc)
 
 	mutex_lock(&sysfs_lock);
 
-	if (test_bit(FLAG_EXPORT, &desc->flags)) {
+	if (!test_bit(FLAG_EXPORT, &desc->flags))
+		goto err_unlock;
 
-		dev = class_find_device(&gpio_class, NULL, desc, match_export);
-		if (dev) {
-			gpio_setup_irq(desc, dev, 0);
-			clear_bit(FLAG_SYSFS_DIR, &desc->flags);
-			clear_bit(FLAG_EXPORT, &desc->flags);
-		} else
-			status = -ENODEV;
-	}
+	dev = class_find_device(&gpio_class, NULL, desc, match_export);
+	if (!dev)
+		goto err_unlock;
+
+	data = dev_get_drvdata(dev);
+
+	clear_bit(FLAG_EXPORT, &desc->flags);
+
+	device_unregister(dev);
+
+	/*
+	 * Release irq after deregistration to prevent race with edge_store.
+	 */
+	if (data->irq_flags)
+		gpio_sysfs_free_irq(dev);
 
 	mutex_unlock(&sysfs_lock);
 
-	if (dev) {
-		device_unregister(dev);
-		put_device(dev);
-	}
+	put_device(dev);
+	kfree(data);
 
-	if (status)
-		gpiod_dbg(desc, "%s: status %d\n", __func__, status);
+	return;
+
+err_unlock:
+	mutex_unlock(&sysfs_lock);
 }
 EXPORT_SYMBOL_GPL(gpiod_unexport);
 
-int gpiochip_export(struct gpio_chip *chip)
+int gpiochip_sysfs_register(struct gpio_chip *chip)
 {
-	int		status;
 	struct device	*dev;
 
-	/* Many systems register gpio chips for SOC support very early,
+	/*
+	 * Many systems add gpio chips for SOC support very early,
 	 * before driver model support is available.  In those cases we
-	 * export this later, in gpiolib_sysfs_init() ... here we just
+	 * register later, in gpiolib_sysfs_init() ... here we just
 	 * verify that _some_ field of gpio_class got initialized.
 	 */
 	if (!gpio_class.p)
 		return 0;
 
 	/* use chip->base for the ID; it's already known to be unique */
-	mutex_lock(&sysfs_lock);
 	dev = device_create_with_groups(&gpio_class, chip->dev, MKDEV(0, 0),
 					chip, gpiochip_groups,
 					"gpiochip%d", chip->base);
 	if (IS_ERR(dev))
-		status = PTR_ERR(dev);
-	else
-		status = 0;
-	chip->exported = (status == 0);
-	mutex_unlock(&sysfs_lock);
+		return PTR_ERR(dev);
 
-	if (status)
-		chip_dbg(chip, "%s: status %d\n", __func__, status);
+	mutex_lock(&sysfs_lock);
+	chip->cdev = dev;
+	mutex_unlock(&sysfs_lock);
 
-	return status;
+	return 0;
 }
 
-void gpiochip_unexport(struct gpio_chip *chip)
+void gpiochip_sysfs_unregister(struct gpio_chip *chip)
 {
-	int			status;
-	struct device		*dev;
 	struct gpio_desc *desc;
 	unsigned int i;
 
+	if (!chip->cdev)
+		return;
+
+	device_unregister(chip->cdev);
+
+	/* prevent further gpiod exports */
 	mutex_lock(&sysfs_lock);
-	dev = class_find_device(&gpio_class, NULL, chip, match_export);
-	if (dev) {
-		put_device(dev);
-		device_unregister(dev);
-		/* prevent further gpiod exports */
-		chip->exported = false;
-		status = 0;
-	} else
-		status = -ENODEV;
+	chip->cdev = NULL;
 	mutex_unlock(&sysfs_lock);
 
-	if (status)
-		chip_dbg(chip, "%s: status %d\n", __func__, status);
-
 	/* unregister gpiod class devices owned by sysfs */
 	for (i = 0; i < chip->ngpio; i++) {
 		desc = &chip->desc[i];
@@ -836,19 +784,20 @@ static int __init gpiolib_sysfs_init(void)
 	 */
 	spin_lock_irqsave(&gpio_lock, flags);
 	list_for_each_entry(chip, &gpio_chips, list) {
-		if (chip->exported)
+		if (chip->cdev)
 			continue;
 
 		/*
-		 * TODO we yield gpio_lock here because gpiochip_export()
-		 * acquires a mutex. This is unsafe and needs to be fixed.
+		 * TODO we yield gpio_lock here because
+		 * gpiochip_sysfs_register() acquires a mutex. This is unsafe
+		 * and needs to be fixed.
 		 *
 		 * Also it would be nice to use gpiochip_find() here so we
 		 * can keep gpio_chips local to gpiolib.c, but the yield of
 		 * gpio_lock prevents us from doing this.
 		 */
 		spin_unlock_irqrestore(&gpio_lock, flags);
-		status = gpiochip_export(chip);
+		status = gpiochip_sysfs_register(chip);
 		spin_lock_irqsave(&gpio_lock, flags);
 	}
 	spin_unlock_irqrestore(&gpio_lock, flags);
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 6bc612b8a49f..bf4bd1d120c3 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -290,7 +290,7 @@ int gpiochip_add(struct gpio_chip *chip)
 	of_gpiochip_add(chip);
 	acpi_gpiochip_add(chip);
 
-	status = gpiochip_export(chip);
+	status = gpiochip_sysfs_register(chip);
 	if (status)
 		goto err_remove_chip;
 
@@ -327,10 +327,12 @@ EXPORT_SYMBOL_GPL(gpiochip_add);
  */
 void gpiochip_remove(struct gpio_chip *chip)
 {
+	struct gpio_desc *desc;
 	unsigned long	flags;
 	unsigned	id;
+	bool		requested = false;
 
-	gpiochip_unexport(chip);
+	gpiochip_sysfs_unregister(chip);
 
 	gpiochip_irqchip_remove(chip);
 
@@ -341,15 +343,17 @@ void gpiochip_remove(struct gpio_chip *chip)
 
 	spin_lock_irqsave(&gpio_lock, flags);
 	for (id = 0; id < chip->ngpio; id++) {
-		if (test_bit(FLAG_REQUESTED, &chip->desc[id].flags))
-			dev_crit(chip->dev, "REMOVING GPIOCHIP WITH GPIOS STILL REQUESTED\n");
+		desc = &chip->desc[id];
+		desc->chip = NULL;
+		if (test_bit(FLAG_REQUESTED, &desc->flags))
+			requested = true;
 	}
-	for (id = 0; id < chip->ngpio; id++)
-		chip->desc[id].chip = NULL;
-
 	list_del(&chip->list);
 	spin_unlock_irqrestore(&gpio_lock, flags);
 
+	if (requested)
+		dev_crit(chip->dev, "REMOVING GPIOCHIP WITH GPIOS STILL REQUESTED\n");
+
 	kfree(chip->desc);
 	chip->desc = NULL;
 }
@@ -441,6 +445,8 @@ void gpiochip_set_chained_irqchip(struct gpio_chip *gpiochip,
 		 */
 		irq_set_handler_data(parent_irq, gpiochip);
 		irq_set_chained_handler(parent_irq, parent_handler);
+
+		gpiochip->irq_parent = parent_irq;
 	}
 
 	/* Set the parent IRQ for all affected IRQs */
@@ -549,6 +555,11 @@ static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip)
 
 	acpi_gpiochip_free_interrupts(gpiochip);
 
+	if (gpiochip->irq_parent) {
+		irq_set_chained_handler(gpiochip->irq_parent, NULL);
+		irq_set_handler_data(gpiochip->irq_parent, NULL);
+	}
+
 	/* Remove all IRQ mappings and delete the domain */
 	if (gpiochip->irqdomain) {
 		for (offset = 0; offset < gpiochip->ngpio; offset++)
@@ -608,7 +619,7 @@ int gpiochip_irqchip_add(struct gpio_chip *gpiochip,
 	of_node = gpiochip->dev->of_node;
 #ifdef CONFIG_OF_GPIO
 	/*
-	 * If the gpiochip has an assigned OF node this takes precendence
+	 * If the gpiochip has an assigned OF node this takes precedence
 	 * FIXME: get rid of this and use gpiochip->dev->of_node everywhere
 	 */
 	if (gpiochip->of_node)
@@ -1211,7 +1222,7 @@ EXPORT_SYMBOL_GPL(gpiod_get_value);
 /*
  *  _gpio_set_open_drain_value() - Set the open drain gpio's value.
  * @desc: gpio descriptor whose state need to be set.
- * @value: Non-zero for setting it HIGH otherise it will set to LOW.
+ * @value: Non-zero for setting it HIGH otherwise it will set to LOW.
  */
 static void _gpio_set_open_drain_value(struct gpio_desc *desc, bool value)
 {
@@ -1238,7 +1249,7 @@ static void _gpio_set_open_drain_value(struct gpio_desc *desc, bool value)
 /*
  *  _gpio_set_open_source_value() - Set the open source gpio's value.
  * @desc: gpio descriptor whose state need to be set.
- * @value: Non-zero for setting it HIGH otherise it will set to LOW.
+ * @value: Non-zero for setting it HIGH otherwise it will set to LOW.
  */
 static void _gpio_set_open_source_value(struct gpio_desc *desc, bool value)
 {
@@ -1300,17 +1311,16 @@ static void gpio_chip_set_multiple(struct gpio_chip *chip,
 				continue;
 			}
 			/* set outputs if the corresponding mask bit is set */
-			if (__test_and_clear_bit(i, mask)) {
+			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)
+static void gpiod_set_array_value_priv(bool raw, bool can_sleep,
+				       unsigned int array_size,
+				       struct gpio_desc **desc_array,
+				       int *value_array)
 {
 	int i = 0;
 
@@ -1320,9 +1330,9 @@ static void gpiod_set_array_priv(bool raw, bool can_sleep,
 		unsigned long bits[BITS_TO_LONGS(chip->ngpio)];
 		int count = 0;
 
-		if (!can_sleep) {
+		if (!can_sleep)
 			WARN_ON(chip->can_sleep);
-		}
+
 		memset(mask, 0, sizeof(mask));
 		do {
 			struct gpio_desc *desc = desc_array[i];
@@ -1337,24 +1347,22 @@ static void gpiod_set_array_priv(bool raw, bool can_sleep,
 			 * open drain and open source outputs are set individually
 			 */
 			if (test_bit(FLAG_OPEN_DRAIN, &desc->flags)) {
-				_gpio_set_open_drain_value(desc,value);
+				_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) {
+				if (value)
 					__set_bit(hwgpio, bits);
-				} else {
+				else
 					__clear_bit(hwgpio, bits);
-				}
 				count++;
 			}
 			i++;
 		} while ((i < array_size) && (desc_array[i]->chip == chip));
 		/* push collected bits to outputs */
-		if (count != 0) {
+		if (count != 0)
 			gpio_chip_set_multiple(chip, mask, bits);
-		}
 	}
 }
 
@@ -1403,7 +1411,7 @@ 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
+ * gpiod_set_raw_array_value() - 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
@@ -1414,17 +1422,18 @@ EXPORT_SYMBOL_GPL(gpiod_set_value);
  * 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,
+void gpiod_set_raw_array_value(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);
+	gpiod_set_array_value_priv(true, false, array_size, desc_array,
+				   value_array);
 }
-EXPORT_SYMBOL_GPL(gpiod_set_raw_array);
+EXPORT_SYMBOL_GPL(gpiod_set_raw_array_value);
 
 /**
- * gpiod_set_array() - assign values to an array of GPIOs
+ * gpiod_set_array_value() - 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
@@ -1435,14 +1444,15 @@ EXPORT_SYMBOL_GPL(gpiod_set_raw_array);
  * 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)
+void gpiod_set_array_value(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);
+	gpiod_set_array_value_priv(false, false, array_size, desc_array,
+				   value_array);
 }
-EXPORT_SYMBOL_GPL(gpiod_set_array);
+EXPORT_SYMBOL_GPL(gpiod_set_array_value);
 
 /**
  * gpiod_cansleep() - report whether gpio value access may sleep
@@ -1604,7 +1614,7 @@ 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
+ * gpiod_set_raw_array_value_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
@@ -1614,19 +1624,20 @@ EXPORT_SYMBOL_GPL(gpiod_set_value_cansleep);
  *
  * 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)
+void gpiod_set_raw_array_value_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);
+	gpiod_set_array_value_priv(true, true, array_size, desc_array,
+				   value_array);
 }
-EXPORT_SYMBOL_GPL(gpiod_set_raw_array_cansleep);
+EXPORT_SYMBOL_GPL(gpiod_set_raw_array_value_cansleep);
 
 /**
- * gpiod_set_array_cansleep() - assign values to an array of GPIOs
+ * gpiod_set_array_value_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
@@ -1636,16 +1647,17 @@ EXPORT_SYMBOL_GPL(gpiod_set_raw_array_cansleep);
  *
  * 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)
+void gpiod_set_array_value_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);
+	gpiod_set_array_value_priv(false, true, array_size, desc_array,
+				   value_array);
 }
-EXPORT_SYMBOL_GPL(gpiod_set_array_cansleep);
+EXPORT_SYMBOL_GPL(gpiod_set_array_value_cansleep);
 
 /**
  * gpiod_add_lookup_table() - register GPIO device consumers
@@ -1880,7 +1892,7 @@ EXPORT_SYMBOL_GPL(gpiod_count);
  *
  * Return the GPIO descriptor corresponding to the function con_id of device
  * dev, -ENOENT if no GPIO has been assigned to the requested function, or
- * another IS_ERR() code if an error occured while trying to acquire the GPIO.
+ * another IS_ERR() code if an error occurred while trying to acquire the GPIO.
  */
 struct gpio_desc *__must_check __gpiod_get(struct device *dev, const char *con_id,
 					 enum gpiod_flags flags)
@@ -1960,7 +1972,7 @@ static int gpiod_configure_flags(struct gpio_desc *desc, const char *con_id,
  *
  * Return a valid GPIO descriptor, -ENOENT if no GPIO has been assigned to the
  * requested function and/or index, or another IS_ERR() code if an error
- * occured while trying to acquire the GPIO.
+ * occurred while trying to acquire the GPIO.
  */
 struct gpio_desc *__must_check __gpiod_get_index(struct device *dev,
 					       const char *con_id,
@@ -2040,14 +2052,14 @@ struct gpio_desc *fwnode_get_named_gpiod(struct fwnode_handle *fwnode,
 	if (is_of_node(fwnode)) {
 		enum of_gpio_flags flags;
 
-		desc = of_get_named_gpiod_flags(of_node(fwnode), propname, 0,
+		desc = of_get_named_gpiod_flags(to_of_node(fwnode), propname, 0,
 						&flags);
 		if (!IS_ERR(desc))
 			active_low = flags & OF_GPIO_ACTIVE_LOW;
 	} else if (is_acpi_node(fwnode)) {
 		struct acpi_gpio_info info;
 
-		desc = acpi_get_gpiod_by_index(acpi_node(fwnode), propname, 0,
+		desc = acpi_get_gpiod_by_index(to_acpi_node(fwnode), propname, 0,
 					       &info);
 		if (!IS_ERR(desc))
 			active_low = info.active_low;
@@ -2118,13 +2130,15 @@ int gpiod_hog(struct gpio_desc *desc, const char *name,
 
 	local_desc = gpiochip_request_own_desc(chip, hwnum, name);
 	if (IS_ERR(local_desc)) {
-		pr_debug("requesting own GPIO %s failed\n", name);
+		pr_err("requesting hog GPIO %s (chip %s, offset %d) failed\n",
+		       name, chip->label, hwnum);
 		return PTR_ERR(local_desc);
 	}
 
 	status = gpiod_configure_flags(desc, name, lflags, dflags);
 	if (status < 0) {
-		pr_debug("setup of GPIO %s failed\n", name);
+		pr_err("setup of hog GPIO %s (chip %s, offset %d) failed\n",
+		       name, chip->label, hwnum);
 		gpiochip_free_own_desc(desc);
 		return status;
 	}
diff --git a/drivers/gpio/gpiolib.h b/drivers/gpio/gpiolib.h
index 594b1798c0e7..bf343004b008 100644
--- a/drivers/gpio/gpiolib.h
+++ b/drivers/gpio/gpiolib.h
@@ -83,20 +83,12 @@ struct gpio_desc {
 #define FLAG_IS_OUT	1
 #define FLAG_EXPORT	2	/* protected by sysfs_lock */
 #define FLAG_SYSFS	3	/* exported via /sys/class/gpio/control */
-#define FLAG_TRIG_FALL	4	/* trigger on falling edge */
-#define FLAG_TRIG_RISE	5	/* trigger on rising edge */
 #define FLAG_ACTIVE_LOW	6	/* value has active low */
 #define FLAG_OPEN_DRAIN	7	/* Gpio is open drain type */
 #define FLAG_OPEN_SOURCE 8	/* Gpio is open source type */
 #define FLAG_USED_AS_IRQ 9	/* GPIO is connected to an IRQ */
-#define FLAG_SYSFS_DIR	10	/* show sysfs direction attribute */
 #define FLAG_IS_HOGGED	11	/* GPIO is hogged */
 
-#define ID_SHIFT	16	/* add new flags before this one */
-
-#define GPIO_FLAGS_MASK		((1 << ID_SHIFT) - 1)
-#define GPIO_TRIGGER_MASK	(BIT(FLAG_TRIG_FALL) | BIT(FLAG_TRIG_RISE))
-
 	const char		*label;
 };
 
@@ -151,17 +143,17 @@ static int __maybe_unused gpio_chip_hwgpio(const struct gpio_desc *desc)
 
 #ifdef CONFIG_GPIO_SYSFS
 
-int gpiochip_export(struct gpio_chip *chip);
-void gpiochip_unexport(struct gpio_chip *chip);
+int gpiochip_sysfs_register(struct gpio_chip *chip);
+void gpiochip_sysfs_unregister(struct gpio_chip *chip);
 
 #else
 
-static inline int gpiochip_export(struct gpio_chip *chip)
+static inline int gpiochip_sysfs_register(struct gpio_chip *chip)
 {
 	return 0;
 }
 
-static inline void gpiochip_unexport(struct gpio_chip *chip)
+static inline void gpiochip_sysfs_unregister(struct gpio_chip *chip)
 {
 }
 
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu.h b/drivers/gpu/drm/amd/amdgpu/amdgpu.h
index 22866d1c3d69..01657830b470 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu.h
@@ -425,6 +425,8 @@ int amdgpu_fence_driver_start_ring(struct amdgpu_ring *ring,
 				   unsigned irq_type);
 int amdgpu_fence_emit(struct amdgpu_ring *ring, void *owner,
 		      struct amdgpu_fence **fence);
+int amdgpu_fence_recreate(struct amdgpu_ring *ring, void *owner,
+			  uint64_t seq, struct amdgpu_fence **fence);
 void amdgpu_fence_process(struct amdgpu_ring *ring);
 int amdgpu_fence_wait_next(struct amdgpu_ring *ring);
 int amdgpu_fence_wait_empty(struct amdgpu_ring *ring);
@@ -435,9 +437,6 @@ int amdgpu_fence_wait(struct amdgpu_fence *fence, bool interruptible);
 int amdgpu_fence_wait_any(struct amdgpu_device *adev,
 			  struct amdgpu_fence **fences,
 			  bool intr);
-long amdgpu_fence_wait_seq_timeout(struct amdgpu_device *adev,
-				   u64 *target_seq, bool intr,
-				   long timeout);
 struct amdgpu_fence *amdgpu_fence_ref(struct amdgpu_fence *fence);
 void amdgpu_fence_unref(struct amdgpu_fence **fence);
 
@@ -1622,6 +1621,7 @@ struct amdgpu_vce {
 	unsigned		fb_version;
 	atomic_t		handles[AMDGPU_MAX_VCE_HANDLES];
 	struct drm_file		*filp[AMDGPU_MAX_VCE_HANDLES];
+	uint32_t		img_size[AMDGPU_MAX_VCE_HANDLES];
 	struct delayed_work	idle_work;
 	const struct firmware	*fw;	/* VCE firmware */
 	struct amdgpu_ring	ring[AMDGPU_MAX_VCE_RINGS];
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_bo_list.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_bo_list.c
index 36d34e0afbc3..f82a2dd83874 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_bo_list.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_bo_list.c
@@ -30,6 +30,7 @@
 
 #include <drm/drmP.h>
 #include "amdgpu.h"
+#include "amdgpu_trace.h"
 
 static int amdgpu_bo_list_create(struct amdgpu_fpriv *fpriv,
 				 struct amdgpu_bo_list **result,
@@ -124,6 +125,8 @@ static int amdgpu_bo_list_set(struct amdgpu_device *adev,
 			gws_obj = entry->robj;
 		if (entry->prefered_domains == AMDGPU_GEM_DOMAIN_OA)
 			oa_obj = entry->robj;
+
+		trace_amdgpu_bo_list_set(list, entry->robj);
 	}
 
 	for (i = 0; i < list->num_entries; ++i)
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_cs.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_cs.c
index f09b2cba40ca..d63135bf29c0 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_cs.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_cs.c
@@ -181,8 +181,6 @@ int amdgpu_cs_parser_init(struct amdgpu_cs_parser *p, void *data)
 		}
 		p->chunks[i].chunk_id = user_chunk.chunk_id;
 		p->chunks[i].length_dw = user_chunk.length_dw;
-		if (p->chunks[i].chunk_id == AMDGPU_CHUNK_ID_IB)
-			p->num_ibs++;
 
 		size = p->chunks[i].length_dw;
 		cdata = (void __user *)(unsigned long)user_chunk.chunk_data;
@@ -199,7 +197,12 @@ int amdgpu_cs_parser_init(struct amdgpu_cs_parser *p, void *data)
 			goto out;
 		}
 
-		if (p->chunks[i].chunk_id == AMDGPU_CHUNK_ID_FENCE) {
+		switch (p->chunks[i].chunk_id) {
+		case AMDGPU_CHUNK_ID_IB:
+			p->num_ibs++;
+			break;
+
+		case AMDGPU_CHUNK_ID_FENCE:
 			size = sizeof(struct drm_amdgpu_cs_chunk_fence);
 			if (p->chunks[i].length_dw * sizeof(uint32_t) >= size) {
 				uint32_t handle;
@@ -221,6 +224,14 @@ int amdgpu_cs_parser_init(struct amdgpu_cs_parser *p, void *data)
 				r = -EINVAL;
 				goto out;
 			}
+			break;
+
+		case AMDGPU_CHUNK_ID_DEPENDENCIES:
+			break;
+
+		default:
+			r = -EINVAL;
+			goto out;
 		}
 	}
 
@@ -445,8 +456,9 @@ static void amdgpu_cs_parser_fini(struct amdgpu_cs_parser *parser, int error, bo
 	for (i = 0; i < parser->nchunks; i++)
 		drm_free_large(parser->chunks[i].kdata);
 	kfree(parser->chunks);
-	for (i = 0; i < parser->num_ibs; i++)
-		amdgpu_ib_free(parser->adev, &parser->ibs[i]);
+	if (parser->ibs)
+		for (i = 0; i < parser->num_ibs; i++)
+			amdgpu_ib_free(parser->adev, &parser->ibs[i]);
 	kfree(parser->ibs);
 	if (parser->uf.bo)
 		drm_gem_object_unreference_unlocked(&parser->uf.bo->gem_base);
@@ -654,6 +666,55 @@ static int amdgpu_cs_ib_fill(struct amdgpu_device *adev,
 	return 0;
 }
 
+static int amdgpu_cs_dependencies(struct amdgpu_device *adev,
+				  struct amdgpu_cs_parser *p)
+{
+	struct amdgpu_ib *ib;
+	int i, j, r;
+
+	if (!p->num_ibs)
+		return 0;
+
+	/* Add dependencies to first IB */
+	ib = &p->ibs[0];
+	for (i = 0; i < p->nchunks; ++i) {
+		struct drm_amdgpu_cs_chunk_dep *deps;
+		struct amdgpu_cs_chunk *chunk;
+		unsigned num_deps;
+
+		chunk = &p->chunks[i];
+
+		if (chunk->chunk_id != AMDGPU_CHUNK_ID_DEPENDENCIES)
+			continue;
+
+		deps = (struct drm_amdgpu_cs_chunk_dep *)chunk->kdata;
+		num_deps = chunk->length_dw * 4 /
+			sizeof(struct drm_amdgpu_cs_chunk_dep);
+
+		for (j = 0; j < num_deps; ++j) {
+			struct amdgpu_fence *fence;
+			struct amdgpu_ring *ring;
+
+			r = amdgpu_cs_get_ring(adev, deps[j].ip_type,
+					       deps[j].ip_instance,
+					       deps[j].ring, &ring);
+			if (r)
+				return r;
+
+			r = amdgpu_fence_recreate(ring, p->filp,
+						  deps[j].handle,
+						  &fence);
+			if (r)
+				return r;
+
+			amdgpu_sync_fence(&ib->sync, fence);
+			amdgpu_fence_unref(&fence);
+		}
+	}
+
+	return 0;
+}
+
 int amdgpu_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 {
 	struct amdgpu_device *adev = dev->dev_private;
@@ -688,11 +749,16 @@ int amdgpu_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 			else
 				DRM_ERROR("Failed to process the buffer list %d!\n", r);
 		}
-	} else {
+	}
+
+	if (!r) {
 		reserved_buffers = true;
 		r = amdgpu_cs_ib_fill(adev, &parser);
 	}
 
+	if (!r)
+		r = amdgpu_cs_dependencies(adev, &parser);
+
 	if (r) {
 		amdgpu_cs_parser_fini(&parser, r, reserved_buffers);
 		up_read(&adev->exclusive_lock);
@@ -730,9 +796,9 @@ int amdgpu_cs_wait_ioctl(struct drm_device *dev, void *data,
 {
 	union drm_amdgpu_wait_cs *wait = data;
 	struct amdgpu_device *adev = dev->dev_private;
-	uint64_t seq[AMDGPU_MAX_RINGS] = {0};
-	struct amdgpu_ring *ring = NULL;
 	unsigned long timeout = amdgpu_gem_timeout(wait->in.timeout);
+	struct amdgpu_fence *fence = NULL;
+	struct amdgpu_ring *ring = NULL;
 	struct amdgpu_ctx *ctx;
 	long r;
 
@@ -745,9 +811,12 @@ int amdgpu_cs_wait_ioctl(struct drm_device *dev, void *data,
 	if (r)
 		return r;
 
-	seq[ring->idx] = wait->in.handle;
+	r = amdgpu_fence_recreate(ring, filp, wait->in.handle, &fence);
+	if (r)
+		return r;
 
-	r = amdgpu_fence_wait_seq_timeout(adev, seq, true, timeout);
+	r = fence_wait_timeout(&fence->base, true, timeout);
+	amdgpu_fence_unref(&fence);
 	amdgpu_ctx_put(ctx);
 	if (r < 0)
 		return r;
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
index fec487d1c870..ba46be361c9b 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
@@ -1191,7 +1191,9 @@ static int amdgpu_early_init(struct amdgpu_device *adev)
 		return -EINVAL;
 	}
 
-
+	adev->ip_block_enabled = kcalloc(adev->num_ip_blocks, sizeof(bool), GFP_KERNEL);
+	if (adev->ip_block_enabled == NULL)
+		return -ENOMEM;
 
 	if (adev->ip_blocks == NULL) {
 		DRM_ERROR("No IP blocks found!\n");
@@ -1575,8 +1577,7 @@ void amdgpu_device_fini(struct amdgpu_device *adev)
 	amdgpu_fence_driver_fini(adev);
 	amdgpu_fbdev_fini(adev);
 	r = amdgpu_fini(adev);
-	if (adev->ip_block_enabled)
-		kfree(adev->ip_block_enabled);
+	kfree(adev->ip_block_enabled);
 	adev->ip_block_enabled = NULL;
 	adev->accel_working = false;
 	/* free i2c buses */
@@ -2000,4 +2001,10 @@ int amdgpu_debugfs_init(struct drm_minor *minor)
 void amdgpu_debugfs_cleanup(struct drm_minor *minor)
 {
 }
+#else
+static int amdgpu_debugfs_regs_init(struct amdgpu_device *adev)
+{
+	return 0;
+}
+static void amdgpu_debugfs_regs_cleanup(struct amdgpu_device *adev) { }
 #endif
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_fence.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_fence.c
index 5c9918d01bf9..a7189a1fa6a1 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_fence.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_fence.c
@@ -136,6 +136,38 @@ int amdgpu_fence_emit(struct amdgpu_ring *ring, void *owner,
 }
 
 /**
+ * amdgpu_fence_recreate - recreate a fence from an user fence
+ *
+ * @ring: ring the fence is associated with
+ * @owner: creator of the fence
+ * @seq: user fence sequence number
+ * @fence: resulting amdgpu fence object
+ *
+ * Recreates a fence command from the user fence sequence number (all asics).
+ * Returns 0 on success, -ENOMEM on failure.
+ */
+int amdgpu_fence_recreate(struct amdgpu_ring *ring, void *owner,
+			  uint64_t seq, struct amdgpu_fence **fence)
+{
+	struct amdgpu_device *adev = ring->adev;
+
+	if (seq > ring->fence_drv.sync_seq[ring->idx])
+		return -EINVAL;
+
+	*fence = kmalloc(sizeof(struct amdgpu_fence), GFP_KERNEL);
+	if ((*fence) == NULL)
+		return -ENOMEM;
+
+	(*fence)->seq = seq;
+	(*fence)->ring = ring;
+	(*fence)->owner = owner;
+	fence_init(&(*fence)->base, &amdgpu_fence_ops,
+		&adev->fence_queue.lock, adev->fence_context + ring->idx,
+		(*fence)->seq);
+	return 0;
+}
+
+/**
  * amdgpu_fence_check_signaled - callback from fence_queue
  *
  * this function is called with fence_queue lock held, which is also used
@@ -517,12 +549,14 @@ static bool amdgpu_fence_any_seq_signaled(struct amdgpu_device *adev, u64 *seq)
  * the wait timeout, or an error for all other cases.
  * -EDEADLK is returned when a GPU lockup has been detected.
  */
-long amdgpu_fence_wait_seq_timeout(struct amdgpu_device *adev, u64 *target_seq,
-				   bool intr, long timeout)
+static long amdgpu_fence_wait_seq_timeout(struct amdgpu_device *adev,
+					  u64 *target_seq, bool intr,
+					  long timeout)
 {
 	uint64_t last_seq[AMDGPU_MAX_RINGS];
 	bool signaled;
-	int i, r;
+	int i;
+	long r;
 
 	if (timeout == 0) {
 		return amdgpu_fence_any_seq_signaled(adev, target_seq);
@@ -1023,7 +1057,7 @@ static int amdgpu_debugfs_fence_info(struct seq_file *m, void *data)
 
 		amdgpu_fence_process(ring);
 
-		seq_printf(m, "--- ring %d ---\n", i);
+		seq_printf(m, "--- ring %d (%s) ---\n", i, ring->name);
 		seq_printf(m, "Last signaled fence 0x%016llx\n",
 			   (unsigned long long)atomic64_read(&ring->fence_drv.last_seq));
 		seq_printf(m, "Last emitted        0x%016llx\n",
@@ -1031,7 +1065,8 @@ static int amdgpu_debugfs_fence_info(struct seq_file *m, void *data)
 
 		for (j = 0; j < AMDGPU_MAX_RINGS; ++j) {
 			struct amdgpu_ring *other = adev->rings[j];
-			if (i != j && other && other->fence_drv.initialized)
+			if (i != j && other && other->fence_drv.initialized &&
+			    ring->fence_drv.sync_seq[j])
 				seq_printf(m, "Last sync to ring %d 0x%016llx\n",
 					   j, ring->fence_drv.sync_seq[j]);
 		}
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_gem.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_gem.c
index 0ec222295fee..ae43b58c9733 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_gem.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_gem.c
@@ -352,7 +352,7 @@ unsigned long amdgpu_gem_timeout(uint64_t timeout_ns)
 	if (((int64_t)timeout_ns) < 0)
 		return MAX_SCHEDULE_TIMEOUT;
 
-	timeout = ktime_sub_ns(ktime_get(), timeout_ns);
+	timeout = ktime_sub(ns_to_ktime(timeout_ns), ktime_get());
 	if (ktime_to_ns(timeout) < 0)
 		return 0;
 
@@ -496,7 +496,7 @@ error_unreserve:
 error_free:
 	drm_free_large(vm_bos);
 
-	if (r)
+	if (r && r != -ERESTARTSYS)
 		DRM_ERROR("Couldn't update BO_VA (%d)\n", r);
 }
 
@@ -525,8 +525,8 @@ int amdgpu_gem_va_ioctl(struct drm_device *dev, void *data,
 		return -EINVAL;
 	}
 
-	invalid_flags = ~(AMDGPU_VM_PAGE_READABLE | AMDGPU_VM_PAGE_WRITEABLE |
-			AMDGPU_VM_PAGE_EXECUTABLE);
+	invalid_flags = ~(AMDGPU_VM_DELAY_UPDATE | AMDGPU_VM_PAGE_READABLE |
+			AMDGPU_VM_PAGE_WRITEABLE | AMDGPU_VM_PAGE_EXECUTABLE);
 	if ((args->flags & invalid_flags)) {
 		dev_err(&dev->pdev->dev, "invalid flags 0x%08X vs 0x%08X\n",
 			args->flags, invalid_flags);
@@ -579,7 +579,7 @@ int amdgpu_gem_va_ioctl(struct drm_device *dev, void *data,
 		break;
 	}
 
-	if (!r)
+	if (!r && !(args->flags & AMDGPU_VM_DELAY_UPDATE))
 		amdgpu_gem_va_update_vm(adev, bo_va);
 
 	drm_gem_object_unreference_unlocked(gobj);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_trace.h b/drivers/gpu/drm/amd/amdgpu/amdgpu_trace.h
index b56dd64bd4ea..961d7265c286 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_trace.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_trace.h
@@ -30,19 +30,21 @@ TRACE_EVENT(amdgpu_cs,
 	    TP_PROTO(struct amdgpu_cs_parser *p, int i),
 	    TP_ARGS(p, i),
 	    TP_STRUCT__entry(
+			     __field(struct amdgpu_bo_list *, bo_list)
 			     __field(u32, ring)
 			     __field(u32, dw)
 			     __field(u32, fences)
 			     ),
 
 	    TP_fast_assign(
+			   __entry->bo_list = p->bo_list;
 			   __entry->ring = p->ibs[i].ring->idx;
 			   __entry->dw = p->ibs[i].length_dw;
 			   __entry->fences = amdgpu_fence_count_emitted(
 				p->ibs[i].ring);
 			   ),
-	    TP_printk("ring=%u, dw=%u, fences=%u",
-		      __entry->ring, __entry->dw,
+	    TP_printk("bo_list=%p, ring=%u, dw=%u, fences=%u",
+		      __entry->bo_list, __entry->ring, __entry->dw,
 		      __entry->fences)
 );
 
@@ -61,6 +63,54 @@ TRACE_EVENT(amdgpu_vm_grab_id,
 	    TP_printk("vmid=%u, ring=%u", __entry->vmid, __entry->ring)
 );
 
+TRACE_EVENT(amdgpu_vm_bo_map,
+	    TP_PROTO(struct amdgpu_bo_va *bo_va,
+		     struct amdgpu_bo_va_mapping *mapping),
+	    TP_ARGS(bo_va, mapping),
+	    TP_STRUCT__entry(
+			     __field(struct amdgpu_bo *, bo)
+			     __field(long, start)
+			     __field(long, last)
+			     __field(u64, offset)
+			     __field(u32, flags)
+			     ),
+
+	    TP_fast_assign(
+			   __entry->bo = bo_va->bo;
+			   __entry->start = mapping->it.start;
+			   __entry->last = mapping->it.last;
+			   __entry->offset = mapping->offset;
+			   __entry->flags = mapping->flags;
+			   ),
+	    TP_printk("bo=%p, start=%lx, last=%lx, offset=%010llx, flags=%08x",
+		      __entry->bo, __entry->start, __entry->last,
+		      __entry->offset, __entry->flags)
+);
+
+TRACE_EVENT(amdgpu_vm_bo_unmap,
+	    TP_PROTO(struct amdgpu_bo_va *bo_va,
+		     struct amdgpu_bo_va_mapping *mapping),
+	    TP_ARGS(bo_va, mapping),
+	    TP_STRUCT__entry(
+			     __field(struct amdgpu_bo *, bo)
+			     __field(long, start)
+			     __field(long, last)
+			     __field(u64, offset)
+			     __field(u32, flags)
+			     ),
+
+	    TP_fast_assign(
+			   __entry->bo = bo_va->bo;
+			   __entry->start = mapping->it.start;
+			   __entry->last = mapping->it.last;
+			   __entry->offset = mapping->offset;
+			   __entry->flags = mapping->flags;
+			   ),
+	    TP_printk("bo=%p, start=%lx, last=%lx, offset=%010llx, flags=%08x",
+		      __entry->bo, __entry->start, __entry->last,
+		      __entry->offset, __entry->flags)
+);
+
 TRACE_EVENT(amdgpu_vm_bo_update,
 	    TP_PROTO(struct amdgpu_bo_va_mapping *mapping),
 	    TP_ARGS(mapping),
@@ -121,6 +171,21 @@ TRACE_EVENT(amdgpu_vm_flush,
 		      __entry->pd_addr, __entry->ring, __entry->id)
 );
 
+TRACE_EVENT(amdgpu_bo_list_set,
+	    TP_PROTO(struct amdgpu_bo_list *list, struct amdgpu_bo *bo),
+	    TP_ARGS(list, bo),
+	    TP_STRUCT__entry(
+			     __field(struct amdgpu_bo_list *, list)
+			     __field(struct amdgpu_bo *, bo)
+			     ),
+
+	    TP_fast_assign(
+			   __entry->list = list;
+			   __entry->bo = bo;
+			   ),
+	    TP_printk("list=%p, bo=%p", __entry->list, __entry->bo)
+);
+
 DECLARE_EVENT_CLASS(amdgpu_fence_request,
 
 	    TP_PROTO(struct drm_device *dev, int ring, u32 seqno),
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
index d3706a498293..dd3415d2e45d 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
@@ -674,7 +674,7 @@ static int amdgpu_ttm_tt_populate(struct ttm_tt *ttm)
 		return 0;
 
 	if (gtt && gtt->userptr) {
-		ttm->sg = kcalloc(1, sizeof(struct sg_table), GFP_KERNEL);
+		ttm->sg = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
 		if (!ttm->sg)
 			return -ENOMEM;
 
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.c
index 1127a504f118..d3ca73090e39 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.c
@@ -464,28 +464,42 @@ int amdgpu_vce_get_destroy_msg(struct amdgpu_ring *ring, uint32_t handle,
  * @p: parser context
  * @lo: address of lower dword
  * @hi: address of higher dword
+ * @size: minimum size
  *
  * Patch relocation inside command stream with real buffer address
  */
-int amdgpu_vce_cs_reloc(struct amdgpu_cs_parser *p, uint32_t ib_idx, int lo, int hi)
+static int amdgpu_vce_cs_reloc(struct amdgpu_cs_parser *p, uint32_t ib_idx,
+			       int lo, int hi, unsigned size, uint32_t index)
 {
 	struct amdgpu_bo_va_mapping *mapping;
 	struct amdgpu_ib *ib = &p->ibs[ib_idx];
 	struct amdgpu_bo *bo;
 	uint64_t addr;
 
+	if (index == 0xffffffff)
+		index = 0;
+
 	addr = ((uint64_t)amdgpu_get_ib_value(p, ib_idx, lo)) |
 	       ((uint64_t)amdgpu_get_ib_value(p, ib_idx, hi)) << 32;
+	addr += ((uint64_t)size) * ((uint64_t)index);
 
 	mapping = amdgpu_cs_find_mapping(p, addr, &bo);
 	if (mapping == NULL) {
-		DRM_ERROR("Can't find BO for addr 0x%010Lx %d %d\n",
+		DRM_ERROR("Can't find BO for addr 0x%010Lx %d %d %d %d\n",
+			  addr, lo, hi, size, index);
+		return -EINVAL;
+	}
+
+	if ((addr + (uint64_t)size) >
+	    ((uint64_t)mapping->it.last + 1) * AMDGPU_GPU_PAGE_SIZE) {
+		DRM_ERROR("BO to small for addr 0x%010Lx %d %d\n",
 			  addr, lo, hi);
 		return -EINVAL;
 	}
 
 	addr -= ((uint64_t)mapping->it.start) * AMDGPU_GPU_PAGE_SIZE;
 	addr += amdgpu_bo_gpu_offset(bo);
+	addr -= ((uint64_t)size) * ((uint64_t)index);
 
 	ib->ptr[lo] = addr & 0xFFFFFFFF;
 	ib->ptr[hi] = addr >> 32;
@@ -494,6 +508,48 @@ int amdgpu_vce_cs_reloc(struct amdgpu_cs_parser *p, uint32_t ib_idx, int lo, int
 }
 
 /**
+ * amdgpu_vce_validate_handle - validate stream handle
+ *
+ * @p: parser context
+ * @handle: handle to validate
+ * @allocated: allocated a new handle?
+ *
+ * Validates the handle and return the found session index or -EINVAL
+ * we we don't have another free session index.
+ */
+static int amdgpu_vce_validate_handle(struct amdgpu_cs_parser *p,
+				      uint32_t handle, bool *allocated)
+{
+	unsigned i;
+
+	*allocated = false;
+
+	/* validate the handle */
+	for (i = 0; i < AMDGPU_MAX_VCE_HANDLES; ++i) {
+		if (atomic_read(&p->adev->vce.handles[i]) == handle) {
+			if (p->adev->vce.filp[i] != p->filp) {
+				DRM_ERROR("VCE handle collision detected!\n");
+				return -EINVAL;
+			}
+			return i;
+		}
+	}
+
+	/* handle not found try to alloc a new one */
+	for (i = 0; i < AMDGPU_MAX_VCE_HANDLES; ++i) {
+		if (!atomic_cmpxchg(&p->adev->vce.handles[i], 0, handle)) {
+			p->adev->vce.filp[i] = p->filp;
+			p->adev->vce.img_size[i] = 0;
+			*allocated = true;
+			return i;
+		}
+	}
+
+	DRM_ERROR("No more free VCE handles!\n");
+	return -EINVAL;
+}
+
+/**
  * amdgpu_vce_cs_parse - parse and validate the command stream
  *
  * @p: parser context
@@ -501,10 +557,15 @@ int amdgpu_vce_cs_reloc(struct amdgpu_cs_parser *p, uint32_t ib_idx, int lo, int
  */
 int amdgpu_vce_ring_parse_cs(struct amdgpu_cs_parser *p, uint32_t ib_idx)
 {
-	uint32_t handle = 0;
-	bool destroy = false;
-	int i, r, idx = 0;
 	struct amdgpu_ib *ib = &p->ibs[ib_idx];
+	unsigned fb_idx = 0, bs_idx = 0;
+	int session_idx = -1;
+	bool destroyed = false;
+	bool created = false;
+	bool allocated = false;
+	uint32_t tmp, handle = 0;
+	uint32_t *size = &tmp;
+	int i, r = 0, idx = 0;
 
 	amdgpu_vce_note_usage(p->adev);
 
@@ -514,16 +575,44 @@ int amdgpu_vce_ring_parse_cs(struct amdgpu_cs_parser *p, uint32_t ib_idx)
 
 		if ((len < 8) || (len & 3)) {
 			DRM_ERROR("invalid VCE command length (%d)!\n", len);
-			return -EINVAL;
+			r = -EINVAL;
+			goto out;
+		}
+
+		if (destroyed) {
+			DRM_ERROR("No other command allowed after destroy!\n");
+			r = -EINVAL;
+			goto out;
 		}
 
 		switch (cmd) {
 		case 0x00000001: // session
 			handle = amdgpu_get_ib_value(p, ib_idx, idx + 2);
+			session_idx = amdgpu_vce_validate_handle(p, handle,
+								 &allocated);
+			if (session_idx < 0)
+				return session_idx;
+			size = &p->adev->vce.img_size[session_idx];
 			break;
 
 		case 0x00000002: // task info
+			fb_idx = amdgpu_get_ib_value(p, ib_idx, idx + 6);
+			bs_idx = amdgpu_get_ib_value(p, ib_idx, idx + 7);
+			break;
+
 		case 0x01000001: // create
+			created = true;
+			if (!allocated) {
+				DRM_ERROR("Handle already in use!\n");
+				r = -EINVAL;
+				goto out;
+			}
+
+			*size = amdgpu_get_ib_value(p, ib_idx, idx + 8) *
+				amdgpu_get_ib_value(p, ib_idx, idx + 10) *
+				8 * 3 / 2;
+			break;
+
 		case 0x04000001: // config extension
 		case 0x04000002: // pic control
 		case 0x04000005: // rate control
@@ -534,60 +623,74 @@ int amdgpu_vce_ring_parse_cs(struct amdgpu_cs_parser *p, uint32_t ib_idx)
 			break;
 
 		case 0x03000001: // encode
-			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 10, idx + 9);
+			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 10, idx + 9,
+						*size, 0);
 			if (r)
-				return r;
+				goto out;
 
-			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 12, idx + 11);
+			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 12, idx + 11,
+						*size / 3, 0);
 			if (r)
-				return r;
+				goto out;
 			break;
 
 		case 0x02000001: // destroy
-			destroy = true;
+			destroyed = true;
 			break;
 
 		case 0x05000001: // context buffer
+			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 3, idx + 2,
+						*size * 2, 0);
+			if (r)
+				goto out;
+			break;
+
 		case 0x05000004: // video bitstream buffer
+			tmp = amdgpu_get_ib_value(p, ib_idx, idx + 4);
+			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 3, idx + 2,
+						tmp, bs_idx);
+			if (r)
+				goto out;
+			break;
+
 		case 0x05000005: // feedback buffer
-			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 3, idx + 2);
+			r = amdgpu_vce_cs_reloc(p, ib_idx, idx + 3, idx + 2,
+						4096, fb_idx);
 			if (r)
-				return r;
+				goto out;
 			break;
 
 		default:
 			DRM_ERROR("invalid VCE command (0x%x)!\n", cmd);
-			return -EINVAL;
+			r = -EINVAL;
+			goto out;
 		}
 
-		idx += len / 4;
-	}
-
-	if (destroy) {
-		/* IB contains a destroy msg, free the handle */
-		for (i = 0; i < AMDGPU_MAX_VCE_HANDLES; ++i)
-			atomic_cmpxchg(&p->adev->vce.handles[i], handle, 0);
+		if (session_idx == -1) {
+			DRM_ERROR("no session command at start of IB\n");
+			r = -EINVAL;
+			goto out;
+		}
 
-		return 0;
+		idx += len / 4;
 	}
 
-	/* create or encode, validate the handle */
-	for (i = 0; i < AMDGPU_MAX_VCE_HANDLES; ++i) {
-		if (atomic_read(&p->adev->vce.handles[i]) == handle)
-			return 0;
+	if (allocated && !created) {
+		DRM_ERROR("New session without create command!\n");
+		r = -ENOENT;
 	}
 
-	/* handle not found try to alloc a new one */
-	for (i = 0; i < AMDGPU_MAX_VCE_HANDLES; ++i) {
-		if (!atomic_cmpxchg(&p->adev->vce.handles[i], 0, handle)) {
-			p->adev->vce.filp[i] = p->filp;
-			return 0;
-		}
+out:
+	if ((!r && destroyed) || (r && allocated)) {
+		/*
+		 * IB contains a destroy msg or we have allocated an
+		 * handle and got an error, anyway free the handle
+		 */
+		for (i = 0; i < AMDGPU_MAX_VCE_HANDLES; ++i)
+			atomic_cmpxchg(&p->adev->vce.handles[i], handle, 0);
 	}
 
-	DRM_ERROR("No more free VCE handles!\n");
-
-	return -EINVAL;
+	return r;
 }
 
 /**
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.h b/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.h
index b6a9d0956c60..7ccdb5927da5 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_vce.h
@@ -33,7 +33,6 @@ int amdgpu_vce_get_create_msg(struct amdgpu_ring *ring, uint32_t handle,
 int amdgpu_vce_get_destroy_msg(struct amdgpu_ring *ring, uint32_t handle,
 			       struct amdgpu_fence **fence);
 void amdgpu_vce_free_handles(struct amdgpu_device *adev, struct drm_file *filp);
-int amdgpu_vce_cs_reloc(struct amdgpu_cs_parser *p, uint32_t ib_idx, int lo, int hi);
 int amdgpu_vce_ring_parse_cs(struct amdgpu_cs_parser *p, uint32_t ib_idx);
 bool amdgpu_vce_ring_emit_semaphore(struct amdgpu_ring *ring,
 				    struct amdgpu_semaphore *semaphore,
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
index 407882b233c7..9a4e3b63f1cb 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
@@ -1001,6 +1001,7 @@ int amdgpu_vm_bo_map(struct amdgpu_device *adev,
 
 	list_add(&mapping->list, &bo_va->mappings);
 	interval_tree_insert(&mapping->it, &vm->va);
+	trace_amdgpu_vm_bo_map(bo_va, mapping);
 
 	bo_va->addr = 0;
 
@@ -1058,6 +1059,7 @@ error_free:
 	mutex_lock(&vm->mutex);
 	list_del(&mapping->list);
 	interval_tree_remove(&mapping->it, &vm->va);
+	trace_amdgpu_vm_bo_unmap(bo_va, mapping);
 	kfree(mapping);
 
 error_unlock:
@@ -1099,6 +1101,7 @@ int amdgpu_vm_bo_unmap(struct amdgpu_device *adev,
 	mutex_lock(&vm->mutex);
 	list_del(&mapping->list);
 	interval_tree_remove(&mapping->it, &vm->va);
+	trace_amdgpu_vm_bo_unmap(bo_va, mapping);
 
 	if (bo_va->addr) {
 		/* clear the old address */
@@ -1139,6 +1142,7 @@ void amdgpu_vm_bo_rmv(struct amdgpu_device *adev,
 	list_for_each_entry_safe(mapping, next, &bo_va->mappings, list) {
 		list_del(&mapping->list);
 		interval_tree_remove(&mapping->it, &vm->va);
+		trace_amdgpu_vm_bo_unmap(bo_va, mapping);
 		if (bo_va->addr)
 			list_add(&mapping->list, &vm->freed);
 		else
diff --git a/drivers/gpu/drm/amd/amdgpu/cik.c b/drivers/gpu/drm/amd/amdgpu/cik.c
index 5dab578d6462..341c56681841 100644
--- a/drivers/gpu/drm/amd/amdgpu/cik.c
+++ b/drivers/gpu/drm/amd/amdgpu/cik.c
@@ -2256,10 +2256,6 @@ int cik_set_ip_blocks(struct amdgpu_device *adev)
 		return -EINVAL;
 	}
 
-	adev->ip_block_enabled = kcalloc(adev->num_ip_blocks, sizeof(bool), GFP_KERNEL);
-	if (adev->ip_block_enabled == NULL)
-		return -ENOMEM;
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/amd/amdgpu/cikd.h b/drivers/gpu/drm/amd/amdgpu/cikd.h
index 220865a44814..d19085a97064 100644
--- a/drivers/gpu/drm/amd/amdgpu/cikd.h
+++ b/drivers/gpu/drm/amd/amdgpu/cikd.h
@@ -552,4 +552,10 @@
 #define VCE_CMD_IB_AUTO		0x00000005
 #define VCE_CMD_SEMAPHORE	0x00000006
 
+/* valid for both DEFAULT_MTYPE and APE1_MTYPE */
+enum {
+	MTYPE_CACHED = 0,
+	MTYPE_NONCACHED = 3
+};
+
 #endif
diff --git a/drivers/gpu/drm/amd/amdgpu/cz_dpm.c b/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
index e4936a452bc6..f75a31df30bd 100644
--- a/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
@@ -425,7 +425,7 @@ static int cz_dpm_init(struct amdgpu_device *adev)
 	pi->mgcg_cgtt_local1 = 0x0;
 	pi->clock_slow_down_step = 25000;
 	pi->skip_clock_slow_down = 1;
-	pi->enable_nb_ps_policy = 1;
+	pi->enable_nb_ps_policy = 0;
 	pi->caps_power_containment = true;
 	pi->caps_cac = true;
 	pi->didt_enabled = false;
diff --git a/drivers/gpu/drm/amd/amdgpu/cz_dpm.h b/drivers/gpu/drm/amd/amdgpu/cz_dpm.h
index 782a74107664..99e1afc89629 100644
--- a/drivers/gpu/drm/amd/amdgpu/cz_dpm.h
+++ b/drivers/gpu/drm/amd/amdgpu/cz_dpm.h
@@ -46,7 +46,7 @@
 
 /* Do not change the following, it is also defined in SMU8.h */
 #define SMU_EnabledFeatureScoreboard_AcpDpmOn		0x00000001
-#define SMU_EnabledFeatureScoreboard_SclkDpmOn		0x00100000
+#define SMU_EnabledFeatureScoreboard_SclkDpmOn		0x00200000
 #define SMU_EnabledFeatureScoreboard_UvdDpmOn		0x00800000
 #define SMU_EnabledFeatureScoreboard_VceDpmOn		0x01000000
 
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
index 5cde635978f9..6e77964f1b64 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
@@ -3403,19 +3403,25 @@ static int dce_v10_0_crtc_irq(struct amdgpu_device *adev,
 
 	switch (entry->src_data) {
 	case 0: /* vblank */
-		if (disp_int & interrupt_status_offsets[crtc].vblank) {
+		if (disp_int & interrupt_status_offsets[crtc].vblank)
 			dce_v10_0_crtc_vblank_int_ack(adev, crtc);
-			if (amdgpu_irq_enabled(adev, source, irq_type)) {
-				drm_handle_vblank(adev->ddev, crtc);
-			}
-			DRM_DEBUG("IH: D%d vblank\n", crtc + 1);
+		else
+			DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+		if (amdgpu_irq_enabled(adev, source, irq_type)) {
+			drm_handle_vblank(adev->ddev, crtc);
 		}
+		DRM_DEBUG("IH: D%d vblank\n", crtc + 1);
+
 		break;
 	case 1: /* vline */
-		if (disp_int & interrupt_status_offsets[crtc].vline) {
+		if (disp_int & interrupt_status_offsets[crtc].vline)
 			dce_v10_0_crtc_vline_int_ack(adev, crtc);
-			DRM_DEBUG("IH: D%d vline\n", crtc + 1);
-		}
+		else
+			DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+		DRM_DEBUG("IH: D%d vline\n", crtc + 1);
+
 		break;
 	default:
 		DRM_DEBUG("Unhandled interrupt: %d %d\n", entry->src_id, entry->src_data);
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
index 95efd98b202d..7f7abb0e0be5 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
@@ -3402,19 +3402,25 @@ static int dce_v11_0_crtc_irq(struct amdgpu_device *adev,
 
 	switch (entry->src_data) {
 	case 0: /* vblank */
-		if (disp_int & interrupt_status_offsets[crtc].vblank) {
+		if (disp_int & interrupt_status_offsets[crtc].vblank)
 			dce_v11_0_crtc_vblank_int_ack(adev, crtc);
-			if (amdgpu_irq_enabled(adev, source, irq_type)) {
-				drm_handle_vblank(adev->ddev, crtc);
-			}
-			DRM_DEBUG("IH: D%d vblank\n", crtc + 1);
+		else
+			DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+		if (amdgpu_irq_enabled(adev, source, irq_type)) {
+			drm_handle_vblank(adev->ddev, crtc);
 		}
+		DRM_DEBUG("IH: D%d vblank\n", crtc + 1);
+
 		break;
 	case 1: /* vline */
-		if (disp_int & interrupt_status_offsets[crtc].vline) {
+		if (disp_int & interrupt_status_offsets[crtc].vline)
 			dce_v11_0_crtc_vline_int_ack(adev, crtc);
-			DRM_DEBUG("IH: D%d vline\n", crtc + 1);
-		}
+		else
+			DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+		DRM_DEBUG("IH: D%d vline\n", crtc + 1);
+
 		break;
 	default:
 		DRM_DEBUG("Unhandled interrupt: %d %d\n", entry->src_id, entry->src_data);
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
index 72c27ac915f2..08387dfd98a7 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
@@ -3237,19 +3237,25 @@ static int dce_v8_0_crtc_irq(struct amdgpu_device *adev,
 
 	switch (entry->src_data) {
 	case 0: /* vblank */
-		if (disp_int & interrupt_status_offsets[crtc].vblank) {
+		if (disp_int & interrupt_status_offsets[crtc].vblank)
 			WREG32(mmLB_VBLANK_STATUS + crtc_offsets[crtc], LB_VBLANK_STATUS__VBLANK_ACK_MASK);
-			if (amdgpu_irq_enabled(adev, source, irq_type)) {
-				drm_handle_vblank(adev->ddev, crtc);
-			}
-			DRM_DEBUG("IH: D%d vblank\n", crtc + 1);
+		else
+			DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+		if (amdgpu_irq_enabled(adev, source, irq_type)) {
+			drm_handle_vblank(adev->ddev, crtc);
 		}
+		DRM_DEBUG("IH: D%d vblank\n", crtc + 1);
+
 		break;
 	case 1: /* vline */
-		if (disp_int & interrupt_status_offsets[crtc].vline) {
+		if (disp_int & interrupt_status_offsets[crtc].vline)
 			WREG32(mmLB_VLINE_STATUS + crtc_offsets[crtc], LB_VLINE_STATUS__VLINE_ACK_MASK);
-			DRM_DEBUG("IH: D%d vline\n", crtc + 1);
-		}
+		else
+			DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+		DRM_DEBUG("IH: D%d vline\n", crtc + 1);
+
 		break;
 	default:
 		DRM_DEBUG("Unhandled interrupt: %d %d\n", entry->src_id, entry->src_data);
@@ -3379,7 +3385,7 @@ static int dce_v8_0_hpd_irq(struct amdgpu_device *adev,
 	uint32_t disp_int, mask, int_control, tmp;
 	unsigned hpd;
 
-	if (entry->src_data > 6) {
+	if (entry->src_data >= adev->mode_info.num_hpd) {
 		DRM_DEBUG("Unhandled interrupt: %d %d\n", entry->src_id, entry->src_data);
 		return 0;
 	}
diff --git a/drivers/gpu/drm/amd/amdgpu/gfx_v7_0.c b/drivers/gpu/drm/amd/amdgpu/gfx_v7_0.c
index cb7907447b81..2c188fb9fd22 100644
--- a/drivers/gpu/drm/amd/amdgpu/gfx_v7_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gfx_v7_0.c
@@ -2010,6 +2010,46 @@ static void gfx_v7_0_setup_rb(struct amdgpu_device *adev,
 }
 
 /**
+ * gmc_v7_0_init_compute_vmid - gart enable
+ *
+ * @rdev: amdgpu_device pointer
+ *
+ * Initialize compute vmid sh_mem registers
+ *
+ */
+#define DEFAULT_SH_MEM_BASES	(0x6000)
+#define FIRST_COMPUTE_VMID	(8)
+#define LAST_COMPUTE_VMID	(16)
+static void gmc_v7_0_init_compute_vmid(struct amdgpu_device *adev)
+{
+	int i;
+	uint32_t sh_mem_config;
+	uint32_t sh_mem_bases;
+
+	/*
+	 * Configure apertures:
+	 * LDS:         0x60000000'00000000 - 0x60000001'00000000 (4GB)
+	 * Scratch:     0x60000001'00000000 - 0x60000002'00000000 (4GB)
+	 * GPUVM:       0x60010000'00000000 - 0x60020000'00000000 (1TB)
+	*/
+	sh_mem_bases = DEFAULT_SH_MEM_BASES | (DEFAULT_SH_MEM_BASES << 16);
+	sh_mem_config = SH_MEM_ALIGNMENT_MODE_UNALIGNED <<
+			SH_MEM_CONFIG__ALIGNMENT_MODE__SHIFT;
+	sh_mem_config |= MTYPE_NONCACHED << SH_MEM_CONFIG__DEFAULT_MTYPE__SHIFT;
+	mutex_lock(&adev->srbm_mutex);
+	for (i = FIRST_COMPUTE_VMID; i < LAST_COMPUTE_VMID; i++) {
+		cik_srbm_select(adev, 0, 0, 0, i);
+		/* CP and shaders */
+		WREG32(mmSH_MEM_CONFIG, sh_mem_config);
+		WREG32(mmSH_MEM_APE1_BASE, 1);
+		WREG32(mmSH_MEM_APE1_LIMIT, 0);
+		WREG32(mmSH_MEM_BASES, sh_mem_bases);
+	}
+	cik_srbm_select(adev, 0, 0, 0, 0);
+	mutex_unlock(&adev->srbm_mutex);
+}
+
+/**
  * gfx_v7_0_gpu_init - setup the 3D engine
  *
  * @adev: amdgpu_device pointer
@@ -2230,6 +2270,8 @@ static void gfx_v7_0_gpu_init(struct amdgpu_device *adev)
 	cik_srbm_select(adev, 0, 0, 0, 0);
 	mutex_unlock(&adev->srbm_mutex);
 
+	gmc_v7_0_init_compute_vmid(adev);
+
 	WREG32(mmSX_DEBUG_1, 0x20);
 
 	WREG32(mmTA_CNTL_AUX, 0x00010000);
diff --git a/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c b/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
index 14242bd33363..7b683fb2173c 100644
--- a/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
@@ -1894,6 +1894,51 @@ static void gfx_v8_0_setup_rb(struct amdgpu_device *adev,
 	mutex_unlock(&adev->grbm_idx_mutex);
 }
 
+/**
+ * gmc_v8_0_init_compute_vmid - gart enable
+ *
+ * @rdev: amdgpu_device pointer
+ *
+ * Initialize compute vmid sh_mem registers
+ *
+ */
+#define DEFAULT_SH_MEM_BASES	(0x6000)
+#define FIRST_COMPUTE_VMID	(8)
+#define LAST_COMPUTE_VMID	(16)
+static void gmc_v8_0_init_compute_vmid(struct amdgpu_device *adev)
+{
+	int i;
+	uint32_t sh_mem_config;
+	uint32_t sh_mem_bases;
+
+	/*
+	 * Configure apertures:
+	 * LDS:         0x60000000'00000000 - 0x60000001'00000000 (4GB)
+	 * Scratch:     0x60000001'00000000 - 0x60000002'00000000 (4GB)
+	 * GPUVM:       0x60010000'00000000 - 0x60020000'00000000 (1TB)
+	 */
+	sh_mem_bases = DEFAULT_SH_MEM_BASES | (DEFAULT_SH_MEM_BASES << 16);
+
+	sh_mem_config = SH_MEM_ADDRESS_MODE_HSA64 <<
+			SH_MEM_CONFIG__ADDRESS_MODE__SHIFT |
+			SH_MEM_ALIGNMENT_MODE_UNALIGNED <<
+			SH_MEM_CONFIG__ALIGNMENT_MODE__SHIFT |
+			MTYPE_CC << SH_MEM_CONFIG__DEFAULT_MTYPE__SHIFT |
+			SH_MEM_CONFIG__PRIVATE_ATC_MASK;
+
+	mutex_lock(&adev->srbm_mutex);
+	for (i = FIRST_COMPUTE_VMID; i < LAST_COMPUTE_VMID; i++) {
+		vi_srbm_select(adev, 0, 0, 0, i);
+		/* CP and shaders */
+		WREG32(mmSH_MEM_CONFIG, sh_mem_config);
+		WREG32(mmSH_MEM_APE1_BASE, 1);
+		WREG32(mmSH_MEM_APE1_LIMIT, 0);
+		WREG32(mmSH_MEM_BASES, sh_mem_bases);
+	}
+	vi_srbm_select(adev, 0, 0, 0, 0);
+	mutex_unlock(&adev->srbm_mutex);
+}
+
 static void gfx_v8_0_gpu_init(struct amdgpu_device *adev)
 {
 	u32 gb_addr_config;
@@ -2113,6 +2158,8 @@ static void gfx_v8_0_gpu_init(struct amdgpu_device *adev)
 	vi_srbm_select(adev, 0, 0, 0, 0);
 	mutex_unlock(&adev->srbm_mutex);
 
+	gmc_v8_0_init_compute_vmid(adev);
+
 	mutex_lock(&adev->grbm_idx_mutex);
 	/*
 	 * making sure that the following register writes will be broadcasted
@@ -3081,7 +3128,7 @@ static int gfx_v8_0_cp_compute_resume(struct amdgpu_device *adev)
 				WREG32(mmCP_MEC_DOORBELL_RANGE_LOWER,
 				       AMDGPU_DOORBELL_KIQ << 2);
 				WREG32(mmCP_MEC_DOORBELL_RANGE_UPPER,
-				       AMDGPU_DOORBELL_MEC_RING7 << 2);
+						0x7FFFF << 2);
 			}
 			tmp = RREG32(mmCP_HQD_PQ_DOORBELL_CONTROL);
 			tmp = REG_SET_FIELD(tmp, CP_HQD_PQ_DOORBELL_CONTROL,
@@ -3097,6 +3144,12 @@ static int gfx_v8_0_cp_compute_resume(struct amdgpu_device *adev)
 		WREG32(mmCP_HQD_PQ_DOORBELL_CONTROL,
 		       mqd->cp_hqd_pq_doorbell_control);
 
+		/* reset read and write pointers, similar to CP_RB0_WPTR/_RPTR */
+		ring->wptr = 0;
+		mqd->cp_hqd_pq_wptr = ring->wptr;
+		WREG32(mmCP_HQD_PQ_WPTR, mqd->cp_hqd_pq_wptr);
+		mqd->cp_hqd_pq_rptr = RREG32(mmCP_HQD_PQ_RPTR);
+
 		/* set the vmid for the queue */
 		mqd->cp_hqd_vmid = 0;
 		WREG32(mmCP_HQD_VMID, mqd->cp_hqd_vmid);
diff --git a/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c b/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
index e3c1fde75363..7bb37b93993f 100644
--- a/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
@@ -439,6 +439,31 @@ static void sdma_v3_0_rlc_stop(struct amdgpu_device *adev)
 }
 
 /**
+ * sdma_v3_0_ctx_switch_enable - stop the async dma engines context switch
+ *
+ * @adev: amdgpu_device pointer
+ * @enable: enable/disable the DMA MEs context switch.
+ *
+ * Halt or unhalt the async dma engines context switch (VI).
+ */
+static void sdma_v3_0_ctx_switch_enable(struct amdgpu_device *adev, bool enable)
+{
+	u32 f32_cntl;
+	int i;
+
+	for (i = 0; i < SDMA_MAX_INSTANCE; i++) {
+		f32_cntl = RREG32(mmSDMA0_CNTL + sdma_offsets[i]);
+		if (enable)
+			f32_cntl = REG_SET_FIELD(f32_cntl, SDMA0_CNTL,
+					AUTO_CTXSW_ENABLE, 1);
+		else
+			f32_cntl = REG_SET_FIELD(f32_cntl, SDMA0_CNTL,
+					AUTO_CTXSW_ENABLE, 0);
+		WREG32(mmSDMA0_CNTL + sdma_offsets[i], f32_cntl);
+	}
+}
+
+/**
  * sdma_v3_0_enable - stop the async dma engines
  *
  * @adev: amdgpu_device pointer
@@ -648,6 +673,8 @@ static int sdma_v3_0_start(struct amdgpu_device *adev)
 
 	/* unhalt the MEs */
 	sdma_v3_0_enable(adev, true);
+	/* enable sdma ring preemption */
+	sdma_v3_0_ctx_switch_enable(adev, true);
 
 	/* start the gfx rings and rlc compute queues */
 	r = sdma_v3_0_gfx_resume(adev);
@@ -1079,6 +1106,7 @@ static int sdma_v3_0_hw_fini(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	sdma_v3_0_ctx_switch_enable(adev, false);
 	sdma_v3_0_enable(adev, false);
 
 	return 0;
diff --git a/drivers/gpu/drm/amd/amdgpu/vi.c b/drivers/gpu/drm/amd/amdgpu/vi.c
index 90fc93c2c1d0..fa5a4448531d 100644
--- a/drivers/gpu/drm/amd/amdgpu/vi.c
+++ b/drivers/gpu/drm/amd/amdgpu/vi.c
@@ -1189,10 +1189,6 @@ int vi_set_ip_blocks(struct amdgpu_device *adev)
 		return -EINVAL;
 	}
 
-	adev->ip_block_enabled = kcalloc(adev->num_ip_blocks, sizeof(bool), GFP_KERNEL);
-	if (adev->ip_block_enabled == NULL)
-		return -ENOMEM;
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_process.c b/drivers/gpu/drm/amd/amdkfd/kfd_process.c
index 8a1f999daa24..9be007081b72 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_process.c
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_process.c
@@ -420,6 +420,12 @@ void kfd_unbind_process_from_device(struct kfd_dev *dev, unsigned int pasid)
 	pqm_uninit(&p->pqm);
 
 	pdd = kfd_get_process_device_data(dev, p);
+
+	if (!pdd) {
+		mutex_unlock(&p->mutex);
+		return;
+	}
+
 	if (pdd->reset_wavefronts) {
 		dbgdev_wave_reset_wavefronts(pdd->dev, p);
 		pdd->reset_wavefronts = false;
@@ -431,8 +437,7 @@ void kfd_unbind_process_from_device(struct kfd_dev *dev, unsigned int pasid)
 	 * We don't call amd_iommu_unbind_pasid() here
 	 * because the IOMMU called us.
 	 */
-	if (pdd)
-		pdd->bound = false;
+	pdd->bound = false;
 
 	mutex_unlock(&p->mutex);
 }
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_topology.c b/drivers/gpu/drm/amd/amdkfd/kfd_topology.c
index e469c4b2e8cc..c25728bc388a 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_topology.c
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_topology.c
@@ -684,8 +684,6 @@ static ssize_t node_show(struct kobject *kobj, struct attribute *attr,
 			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",
@@ -736,6 +734,8 @@ static ssize_t node_show(struct kobject *kobj, struct attribute *attr,
 			dev->gpu->kfd2kgd->get_fw_version(
 						dev->gpu->kgd,
 						KGD_ENGINE_MEC1));
+		sysfs_show_32bit_prop(buffer, "capability",
+				dev->node_props.capability);
 	}
 
 	return sysfs_show_32bit_prop(buffer, "max_engine_clk_ccompute",
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index 79fe31e5851e..9c978d9de3b8 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -4730,7 +4730,7 @@ int drm_mode_connector_update_edid_property(struct drm_connector *connector,
 		return 0;
 
 	if (edid)
-		size = EDID_LENGTH + (1 + edid->extensions);
+		size = EDID_LENGTH * (1 + edid->extensions);
 
 	ret = drm_property_replace_global_blob(dev,
 					       &connector->edid_blob_ptr,
diff --git a/drivers/gpu/drm/drm_dp_mst_topology.c b/drivers/gpu/drm/drm_dp_mst_topology.c
index f7213497ad6c..778bbb6425b8 100644
--- a/drivers/gpu/drm/drm_dp_mst_topology.c
+++ b/drivers/gpu/drm/drm_dp_mst_topology.c
@@ -867,8 +867,16 @@ static void drm_dp_destroy_port(struct kref *kref)
 		port->vcpi.num_slots = 0;
 
 		kfree(port->cached_edid);
-		if (port->connector)
-			(*port->mgr->cbs->destroy_connector)(mgr, port->connector);
+
+		/* we can't destroy the connector here, as
+		   we might be holding the mode_config.mutex
+		   from an EDID retrieval */
+		if (port->connector) {
+			mutex_lock(&mgr->destroy_connector_lock);
+			list_add(&port->connector->destroy_list, &mgr->destroy_connector_list);
+			mutex_unlock(&mgr->destroy_connector_lock);
+			schedule_work(&mgr->destroy_connector_work);
+		}
 		drm_dp_port_teardown_pdt(port, port->pdt);
 
 		if (!port->input && port->vcpi.vcpi > 0)
@@ -2649,6 +2657,30 @@ static void drm_dp_tx_work(struct work_struct *work)
 	mutex_unlock(&mgr->qlock);
 }
 
+static void drm_dp_destroy_connector_work(struct work_struct *work)
+{
+	struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, destroy_connector_work);
+	struct drm_connector *connector;
+
+	/*
+	 * Not a regular list traverse as we have to drop the destroy
+	 * connector lock before destroying the connector, to avoid AB->BA
+	 * ordering between this lock and the config mutex.
+	 */
+	for (;;) {
+		mutex_lock(&mgr->destroy_connector_lock);
+		connector = list_first_entry_or_null(&mgr->destroy_connector_list, struct drm_connector, destroy_list);
+		if (!connector) {
+			mutex_unlock(&mgr->destroy_connector_lock);
+			break;
+		}
+		list_del(&connector->destroy_list);
+		mutex_unlock(&mgr->destroy_connector_lock);
+
+		mgr->cbs->destroy_connector(mgr, connector);
+	}
+}
+
 /**
  * drm_dp_mst_topology_mgr_init - initialise a topology manager
  * @mgr: manager struct to initialise
@@ -2668,10 +2700,13 @@ int drm_dp_mst_topology_mgr_init(struct drm_dp_mst_topology_mgr *mgr,
 	mutex_init(&mgr->lock);
 	mutex_init(&mgr->qlock);
 	mutex_init(&mgr->payload_lock);
+	mutex_init(&mgr->destroy_connector_lock);
 	INIT_LIST_HEAD(&mgr->tx_msg_upq);
 	INIT_LIST_HEAD(&mgr->tx_msg_downq);
+	INIT_LIST_HEAD(&mgr->destroy_connector_list);
 	INIT_WORK(&mgr->work, drm_dp_mst_link_probe_work);
 	INIT_WORK(&mgr->tx_work, drm_dp_tx_work);
+	INIT_WORK(&mgr->destroy_connector_work, drm_dp_destroy_connector_work);
 	init_waitqueue_head(&mgr->tx_waitq);
 	mgr->dev = dev;
 	mgr->aux = aux;
@@ -2696,6 +2731,7 @@ EXPORT_SYMBOL(drm_dp_mst_topology_mgr_init);
  */
 void drm_dp_mst_topology_mgr_destroy(struct drm_dp_mst_topology_mgr *mgr)
 {
+	flush_work(&mgr->destroy_connector_work);
 	mutex_lock(&mgr->payload_lock);
 	kfree(mgr->payloads);
 	mgr->payloads = NULL;
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 3b3c4f537e95..b7bf4ce8c012 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -46,13 +46,11 @@ MODULE_AUTHOR(CORE_AUTHOR);
 MODULE_DESCRIPTION(CORE_DESC);
 MODULE_LICENSE("GPL and additional rights");
 MODULE_PARM_DESC(debug, "Enable debug output");
-MODULE_PARM_DESC(atomic, "Enable experimental atomic KMS API");
 MODULE_PARM_DESC(vblankoffdelay, "Delay until vblank irq auto-disable [msecs] (0: never disable, <0: disable immediately)");
 MODULE_PARM_DESC(timestamp_precision_usec, "Max. error on timestamps [usecs]");
 MODULE_PARM_DESC(timestamp_monotonic, "Use monotonic timestamps");
 
 module_param_named(debug, drm_debug, int, 0600);
-module_param_named_unsafe(atomic, drm_atomic, bool, 0600);
 
 static DEFINE_SPINLOCK(drm_minor_lock);
 static struct idr drm_minors_idr;
diff --git a/drivers/gpu/drm/drm_ioctl.c b/drivers/gpu/drm/drm_ioctl.c
index 9bac1b7479af..b1d303fa2327 100644
--- a/drivers/gpu/drm/drm_ioctl.c
+++ b/drivers/gpu/drm/drm_ioctl.c
@@ -36,9 +36,6 @@
 
 #include <linux/pci.h>
 #include <linux/export.h>
-#ifdef CONFIG_X86
-#include <asm/mtrr.h>
-#endif
 
 static int drm_version(struct drm_device *dev, void *data,
 		       struct drm_file *file_priv);
@@ -197,16 +194,7 @@ static int drm_getmap(struct drm_device *dev, void *data,
 	map->type = r_list->map->type;
 	map->flags = r_list->map->flags;
 	map->handle = (void *)(unsigned long) r_list->user_token;
-
-#ifdef CONFIG_X86
-	/*
-	 * There appears to be exactly one user of the mtrr index: dritest.
-	 * It's easy enough to keep it working on non-PAT systems.
-	 */
-	map->mtrr = phys_wc_to_mtrr_index(r_list->map->mtrr);
-#else
-	map->mtrr = -1;
-#endif
+	map->mtrr = arch_phys_wc_index(r_list->map->mtrr);
 
 	mutex_unlock(&dev->struct_mutex);
 
@@ -350,9 +338,6 @@ drm_setclientcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
 		file_priv->universal_planes = req->value;
 		break;
 	case DRM_CLIENT_CAP_ATOMIC:
-		/* for now, hide behind experimental drm.atomic moduleparam */
-		if (!drm_atomic)
-			return -EINVAL;
 		if (!drm_core_check_feature(dev, DRIVER_ATOMIC))
 			return -EINVAL;
 		if (req->value > 1)
diff --git a/drivers/gpu/drm/drm_sysfs.c b/drivers/gpu/drm/drm_sysfs.c
index 487ddf5ffe51..0f6cd33b531f 100644
--- a/drivers/gpu/drm/drm_sysfs.c
+++ b/drivers/gpu/drm/drm_sysfs.c
@@ -217,7 +217,7 @@ static ssize_t status_store(struct device *device,
 
 	mutex_unlock(&dev->mode_config.mutex);
 
-	return ret;
+	return ret ? ret : count;
 }
 
 static ssize_t status_show(struct device *device,
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index f3f014df61a3..bc817da9fef7 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -1732,12 +1732,15 @@ static int i915_sr_status(struct seq_file *m, void *unused)
 
 	if (HAS_PCH_SPLIT(dev))
 		sr_enabled = I915_READ(WM1_LP_ILK) & WM1_LP_SR_EN;
-	else if (IS_CRESTLINE(dev) || IS_I945G(dev) || IS_I945GM(dev))
+	else if (IS_CRESTLINE(dev) || IS_G4X(dev) ||
+		 IS_I945G(dev) || IS_I945GM(dev))
 		sr_enabled = I915_READ(FW_BLC_SELF) & FW_BLC_SELF_EN;
 	else if (IS_I915GM(dev))
 		sr_enabled = I915_READ(INSTPM) & INSTPM_SELF_EN;
 	else if (IS_PINEVIEW(dev))
 		sr_enabled = I915_READ(DSPFW3) & PINEVIEW_SELF_REFRESH_EN;
+	else if (IS_VALLEYVIEW(dev))
+		sr_enabled = I915_READ(FW_BLC_SELF_VLV) & FW_CSPWRDWNEN;
 
 	intel_runtime_pm_put(dev_priv);
 
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 4af33d59d507..01fbdc57462a 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -866,6 +866,7 @@ struct intel_context {
 	struct kref ref;
 	int user_handle;
 	uint8_t remap_slice;
+	struct drm_i915_private *i915;
 	int flags;
 	struct drm_i915_file_private *file_priv;
 	struct i915_ctx_hang_stats hang_stats;
@@ -2073,8 +2074,6 @@ struct drm_i915_gem_object {
 	unsigned int cache_level:3;
 	unsigned int cache_dirty:1;
 
-	unsigned int has_dma_mapping:1;
-
 	unsigned int frontbuffer_bits:INTEL_FRONTBUFFER_BITS;
 
 	unsigned int pin_display;
@@ -3173,7 +3172,8 @@ void i915_debugfs_cleanup(struct drm_minor *minor);
 int i915_debugfs_connector_add(struct drm_connector *connector);
 void intel_display_crc_init(struct drm_device *dev);
 #else
-static inline int i915_debugfs_connector_add(struct drm_connector *connector) {}
+static inline int i915_debugfs_connector_add(struct drm_connector *connector)
+{ return 0; }
 static inline void intel_display_crc_init(struct drm_device *dev) {}
 #endif
 
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index aaabf3c259d5..d9f2701b4593 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -217,7 +217,6 @@ i915_gem_object_get_pages_phys(struct drm_i915_gem_object *obj)
 	sg_dma_len(sg) = obj->base.size;
 
 	obj->pages = st;
-	obj->has_dma_mapping = true;
 	return 0;
 }
 
@@ -269,8 +268,6 @@ i915_gem_object_put_pages_phys(struct drm_i915_gem_object *obj)
 
 	sg_free_table(obj->pages);
 	kfree(obj->pages);
-
-	obj->has_dma_mapping = false;
 }
 
 static void
@@ -2146,6 +2143,8 @@ i915_gem_object_put_pages_gtt(struct drm_i915_gem_object *obj)
 		obj->base.read_domains = obj->base.write_domain = I915_GEM_DOMAIN_CPU;
 	}
 
+	i915_gem_gtt_finish_object(obj);
+
 	if (i915_gem_object_needs_bit17_swizzle(obj))
 		i915_gem_object_save_bit_17_swizzle(obj);
 
@@ -2206,6 +2205,7 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	struct sg_page_iter sg_iter;
 	struct page *page;
 	unsigned long last_pfn = 0;	/* suppress gcc warning */
+	int ret;
 	gfp_t gfp;
 
 	/* Assert that the object is not currently in any GPU domain. As it
@@ -2253,8 +2253,10 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 			 */
 			i915_gem_shrink_all(dev_priv);
 			page = shmem_read_mapping_page(mapping, i);
-			if (IS_ERR(page))
+			if (IS_ERR(page)) {
+				ret = PTR_ERR(page);
 				goto err_pages;
+			}
 		}
 #ifdef CONFIG_SWIOTLB
 		if (swiotlb_nr_tbl()) {
@@ -2283,6 +2285,10 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 		sg_mark_end(sg);
 	obj->pages = st;
 
+	ret = i915_gem_gtt_prepare_object(obj);
+	if (ret)
+		goto err_pages;
+
 	if (i915_gem_object_needs_bit17_swizzle(obj))
 		i915_gem_object_do_bit_17_swizzle(obj);
 
@@ -2307,10 +2313,10 @@ err_pages:
 	 * space and so want to translate the error from shmemfs back to our
 	 * usual understanding of ENOMEM.
 	 */
-	if (PTR_ERR(page) == -ENOSPC)
-		return -ENOMEM;
-	else
-		return PTR_ERR(page);
+	if (ret == -ENOSPC)
+		ret = -ENOMEM;
+
+	return ret;
 }
 
 /* Ensure that the associated pages are gathered from the backing storage
@@ -2549,6 +2555,7 @@ void __i915_add_request(struct drm_i915_gem_request *request,
 	request->batch_obj = obj;
 
 	request->emitted_jiffies = jiffies;
+	ring->last_submitted_seqno = request->seqno;
 	list_add_tail(&request->list, &ring->request_list);
 
 	trace_i915_gem_request_add(request);
@@ -3279,8 +3286,8 @@ int i915_vma_unbind(struct i915_vma *vma)
 		} else if (vma->ggtt_view.pages) {
 			sg_free_table(vma->ggtt_view.pages);
 			kfree(vma->ggtt_view.pages);
-			vma->ggtt_view.pages = NULL;
 		}
+		vma->ggtt_view.pages = NULL;
 	}
 
 	drm_mm_remove_node(&vma->node);
@@ -3288,10 +3295,8 @@ int i915_vma_unbind(struct i915_vma *vma)
 
 	/* Since the unbound list is global, only move to that list if
 	 * no more VMAs exist. */
-	if (list_empty(&obj->vma_list)) {
-		i915_gem_gtt_finish_object(obj);
+	if (list_empty(&obj->vma_list))
 		list_move_tail(&obj->global_list, &dev_priv->mm.unbound_list);
-	}
 
 	/* And finally now the object is completely decoupled from this vma,
 	 * we can drop its hold on the backing storage and allow it to be
@@ -3819,22 +3824,16 @@ search_free:
 		goto err_remove_node;
 	}
 
-	ret = i915_gem_gtt_prepare_object(obj);
-	if (ret)
-		goto err_remove_node;
-
 	trace_i915_vma_bind(vma, flags);
 	ret = i915_vma_bind(vma, obj->cache_level, flags);
 	if (ret)
-		goto err_finish_gtt;
+		goto err_remove_node;
 
 	list_move_tail(&obj->global_list, &dev_priv->mm.bound_list);
 	list_add_tail(&vma->mm_list, &vm->inactive_list);
 
 	return vma;
 
-err_finish_gtt:
-	i915_gem_gtt_finish_object(obj);
 err_remove_node:
 	drm_mm_remove_node(&vma->node);
 err_free_vma:
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index 4256b8e97e40..b77a8f78c35a 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -135,8 +135,7 @@ static int get_context_size(struct drm_device *dev)
 
 void i915_gem_context_free(struct kref *ctx_ref)
 {
-	struct intel_context *ctx = container_of(ctx_ref,
-						 typeof(*ctx), ref);
+	struct intel_context *ctx = container_of(ctx_ref, typeof(*ctx), ref);
 
 	trace_i915_context_free(ctx);
 
@@ -157,9 +156,7 @@ i915_gem_alloc_context_obj(struct drm_device *dev, size_t size)
 	struct drm_i915_gem_object *obj;
 	int ret;
 
-	obj = i915_gem_object_create_stolen(dev, size);
-	if (obj == NULL)
-		obj = i915_gem_alloc_object(dev, size);
+	obj = i915_gem_alloc_object(dev, size);
 	if (obj == NULL)
 		return ERR_PTR(-ENOMEM);
 
@@ -197,6 +194,7 @@ __create_hw_context(struct drm_device *dev,
 
 	kref_init(&ctx->ref);
 	list_add_tail(&ctx->link, &dev_priv->context_list);
+	ctx->i915 = dev_priv;
 
 	if (dev_priv->hw_context_size) {
 		struct drm_i915_gem_object *obj =
diff --git a/drivers/gpu/drm/i915/i915_gem_dmabuf.c b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
index 7998da27c500..e9c2bfd85b52 100644
--- a/drivers/gpu/drm/i915/i915_gem_dmabuf.c
+++ b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
@@ -256,7 +256,6 @@ static int i915_gem_object_get_pages_dmabuf(struct drm_i915_gem_object *obj)
 		return PTR_ERR(sg);
 
 	obj->pages = sg;
-	obj->has_dma_mapping = true;
 	return 0;
 }
 
@@ -264,7 +263,6 @@ static void i915_gem_object_put_pages_dmabuf(struct drm_i915_gem_object *obj)
 {
 	dma_buf_unmap_attachment(obj->base.import_attach,
 				 obj->pages, DMA_BIDIRECTIONAL);
-	obj->has_dma_mapping = false;
 }
 
 static const struct drm_i915_gem_object_ops i915_gem_object_dmabuf_ops = {
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 83577c615962..923a3c4bf0b7 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -32,6 +32,7 @@
 #include "i915_trace.h"
 #include "intel_drv.h"
 #include <linux/dma_remapping.h>
+#include <linux/uaccess.h>
 
 #define  __EXEC_OBJECT_HAS_PIN (1<<31)
 #define  __EXEC_OBJECT_HAS_FENCE (1<<30)
@@ -458,7 +459,7 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 	}
 
 	/* We can't wait for rendering with pagefaults disabled */
-	if (obj->active && in_atomic())
+	if (obj->active && pagefault_disabled())
 		return -EFAULT;
 
 	if (use_cpu_reloc(obj))
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index ed65f24867b4..c2a291e09bd9 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -585,17 +585,17 @@ static void gen8_ppgtt_clear_range(struct i915_address_space *vm,
 		struct i915_page_table *pt;
 
 		if (WARN_ON(!ppgtt->pdp.page_directory[pdpe]))
-			continue;
+			break;
 
 		pd = ppgtt->pdp.page_directory[pdpe];
 
 		if (WARN_ON(!pd->page_table[pde]))
-			continue;
+			break;
 
 		pt = pd->page_table[pde];
 
 		if (WARN_ON(!px_page(pt)))
-			continue;
+			break;
 
 		last_pte = pte + num_entries;
 		if (last_pte > GEN8_PTES)
@@ -1802,9 +1802,6 @@ void i915_gem_suspend_gtt_mappings(struct drm_device *dev)
 
 int i915_gem_gtt_prepare_object(struct drm_i915_gem_object *obj)
 {
-	if (obj->has_dma_mapping)
-		return 0;
-
 	if (!dma_map_sg(&obj->base.dev->pdev->dev,
 			obj->pages->sgl, obj->pages->nents,
 			PCI_DMA_BIDIRECTIONAL))
@@ -2052,10 +2049,8 @@ void i915_gem_gtt_finish_object(struct drm_i915_gem_object *obj)
 
 	interruptible = do_idling(dev_priv);
 
-	if (!obj->has_dma_mapping)
-		dma_unmap_sg(&dev->pdev->dev,
-			     obj->pages->sgl, obj->pages->nents,
-			     PCI_DMA_BIDIRECTIONAL);
+	dma_unmap_sg(&dev->pdev->dev, obj->pages->sgl, obj->pages->nents,
+		     PCI_DMA_BIDIRECTIONAL);
 
 	undo_idling(dev_priv, interruptible);
 }
@@ -2593,6 +2588,8 @@ void i915_gem_restore_gtt_mappings(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_i915_gem_object *obj;
 	struct i915_address_space *vm;
+	struct i915_vma *vma;
+	bool flush;
 
 	i915_check_and_clear_faults(dev);
 
@@ -2602,16 +2599,23 @@ void i915_gem_restore_gtt_mappings(struct drm_device *dev)
 				       dev_priv->gtt.base.total,
 				       true);
 
+	/* Cache flush objects bound into GGTT and rebind them. */
+	vm = &dev_priv->gtt.base;
 	list_for_each_entry(obj, &dev_priv->mm.bound_list, global_list) {
-		struct i915_vma *vma = i915_gem_obj_to_vma(obj,
-							   &dev_priv->gtt.base);
-		if (!vma)
-			continue;
+		flush = false;
+		list_for_each_entry(vma, &obj->vma_list, vma_link) {
+			if (vma->vm != vm)
+				continue;
 
-		i915_gem_clflush_object(obj, obj->pin_display);
-		WARN_ON(i915_vma_bind(vma, obj->cache_level, PIN_UPDATE));
-	}
+			WARN_ON(i915_vma_bind(vma, obj->cache_level,
+					      PIN_UPDATE));
 
+			flush = true;
+		}
+
+		if (flush)
+			i915_gem_clflush_object(obj, obj->pin_display);
+	}
 
 	if (INTEL_INFO(dev)->gen >= 8) {
 		if (IS_CHERRYVIEW(dev) || IS_BROXTON(dev))
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
index de76d886cd1d..ed682a9a9cbb 100644
--- a/drivers/gpu/drm/i915/i915_gem_stolen.c
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -316,7 +316,6 @@ _i915_gem_object_create_stolen(struct drm_device *dev,
 	if (obj->pages == NULL)
 		goto cleanup;
 
-	obj->has_dma_mapping = true;
 	i915_gem_object_pin_pages(obj);
 	obj->stolen = stolen;
 
diff --git a/drivers/gpu/drm/i915/i915_gem_userptr.c b/drivers/gpu/drm/i915/i915_gem_userptr.c
index 1f4e5a32a16e..8fd431bcdfd3 100644
--- a/drivers/gpu/drm/i915/i915_gem_userptr.c
+++ b/drivers/gpu/drm/i915/i915_gem_userptr.c
@@ -545,6 +545,26 @@ err:
 	return ret;
 }
 
+static int
+__i915_gem_userptr_set_pages(struct drm_i915_gem_object *obj,
+			     struct page **pvec, int num_pages)
+{
+	int ret;
+
+	ret = st_set_pages(&obj->pages, pvec, num_pages);
+	if (ret)
+		return ret;
+
+	ret = i915_gem_gtt_prepare_object(obj);
+	if (ret) {
+		sg_free_table(obj->pages);
+		kfree(obj->pages);
+		obj->pages = NULL;
+	}
+
+	return ret;
+}
+
 static void
 __i915_gem_userptr_get_pages_worker(struct work_struct *_work)
 {
@@ -584,9 +604,12 @@ __i915_gem_userptr_get_pages_worker(struct work_struct *_work)
 	if (obj->userptr.work != &work->work) {
 		ret = 0;
 	} else if (pinned == num_pages) {
-		ret = st_set_pages(&obj->pages, pvec, num_pages);
+		ret = __i915_gem_userptr_set_pages(obj, pvec, num_pages);
 		if (ret == 0) {
 			list_add_tail(&obj->global_list, &to_i915(dev)->mm.unbound_list);
+			obj->get_page.sg = obj->pages->sgl;
+			obj->get_page.last = 0;
+
 			pinned = 0;
 		}
 	}
@@ -693,7 +716,7 @@ i915_gem_userptr_get_pages(struct drm_i915_gem_object *obj)
 			}
 		}
 	} else {
-		ret = st_set_pages(&obj->pages, pvec, num_pages);
+		ret = __i915_gem_userptr_set_pages(obj, pvec, num_pages);
 		if (ret == 0) {
 			obj->userptr.work = NULL;
 			pinned = 0;
@@ -715,6 +738,8 @@ i915_gem_userptr_put_pages(struct drm_i915_gem_object *obj)
 	if (obj->madv != I915_MADV_WILLNEED)
 		obj->dirty = 0;
 
+	i915_gem_gtt_finish_object(obj);
+
 	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents, 0) {
 		struct page *page = sg_page_iter_page(&sg_iter);
 
diff --git a/drivers/gpu/drm/i915/i915_ioc32.c b/drivers/gpu/drm/i915/i915_ioc32.c
index a5a2d5b3f44b..97f3a5640289 100644
--- a/drivers/gpu/drm/i915/i915_ioc32.c
+++ b/drivers/gpu/drm/i915/i915_ioc32.c
@@ -84,7 +84,7 @@ long i915_compat_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	drm_ioctl_compat_t *fn = NULL;
 	int ret;
 
-	if (nr < DRM_COMMAND_BASE)
+	if (nr < DRM_COMMAND_BASE || nr >= DRM_COMMAND_END)
 		return drm_compat_ioctl(filp, cmd, arg);
 
 	if (nr < DRM_COMMAND_BASE + ARRAY_SIZE(i915_compat_ioctls))
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index a897f68485c2..d87f173a0179 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -2486,18 +2486,11 @@ static void gen8_disable_vblank(struct drm_device *dev, int pipe)
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
-static struct drm_i915_gem_request *
-ring_last_request(struct intel_engine_cs *ring)
-{
-	return list_entry(ring->request_list.prev,
-			  struct drm_i915_gem_request, list);
-}
-
 static bool
-ring_idle(struct intel_engine_cs *ring)
+ring_idle(struct intel_engine_cs *ring, u32 seqno)
 {
 	return (list_empty(&ring->request_list) ||
-		i915_gem_request_completed(ring_last_request(ring), false));
+		i915_seqno_passed(seqno, ring->last_submitted_seqno));
 }
 
 static bool
@@ -2719,7 +2712,7 @@ static void i915_hangcheck_elapsed(struct work_struct *work)
 		acthd = intel_ring_get_active_head(ring);
 
 		if (ring->hangcheck.seqno == seqno) {
-			if (ring_idle(ring)) {
+			if (ring_idle(ring, seqno)) {
 				ring->hangcheck.action = HANGCHECK_IDLE;
 
 				if (waitqueue_active(&ring->irq_queue)) {
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index ef5f69a1607f..e9a95df639f0 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -3532,6 +3532,7 @@ enum skl_disp_power_wells {
 #define   BLM_POLARITY_PNV			(1 << 0) /* pnv only */
 
 #define BLC_HIST_CTL	(dev_priv->info.display_mmio_offset + 0x61260)
+#define  BLM_HISTOGRAM_ENABLE			(1 << 31)
 
 /* New registers for PCH-split platforms. Safe where new bits show up, the
  * register layout machtes with gen4 BLC_PWM_CTL[12]. */
@@ -7031,6 +7032,9 @@ enum skl_disp_power_wells {
 #define   AUDIO_CP_READY(trans)		((1 << 1) << ((trans) * 4))
 #define   AUDIO_ELD_VALID(trans)	((1 << 0) << ((trans) * 4))
 
+#define HSW_AUD_CHICKENBIT			0x65f10
+#define   SKL_AUD_CODEC_WAKE_SIGNAL		(1 << 15)
+
 /* HSW Power Wells */
 #define HSW_PWR_WELL_BIOS			0x45400 /* CTL1 */
 #define HSW_PWR_WELL_DRIVER			0x45404 /* CTL2 */
diff --git a/drivers/gpu/drm/i915/i915_trace.h b/drivers/gpu/drm/i915/i915_trace.h
index 63328b6e8ea5..2f34c47bd4bf 100644
--- a/drivers/gpu/drm/i915/i915_trace.h
+++ b/drivers/gpu/drm/i915/i915_trace.h
@@ -727,7 +727,7 @@ DECLARE_EVENT_CLASS(i915_context,
 	TP_fast_assign(
 			__entry->ctx = ctx;
 			__entry->vm = ctx->ppgtt ? &ctx->ppgtt->base : NULL;
-			__entry->dev = ctx->file_priv->dev_priv->dev->primary->index;
+			__entry->dev = ctx->i915->dev->primary->index;
 	),
 
 	TP_printk("dev=%u, ctx=%p, ctx_vm=%p",
diff --git a/drivers/gpu/drm/i915/intel_audio.c b/drivers/gpu/drm/i915/intel_audio.c
index c4397c1e7e4f..dc32cf4585f8 100644
--- a/drivers/gpu/drm/i915/intel_audio.c
+++ b/drivers/gpu/drm/i915/intel_audio.c
@@ -471,6 +471,32 @@ static void i915_audio_component_put_power(struct device *dev)
 	intel_display_power_put(dev_to_i915(dev), POWER_DOMAIN_AUDIO);
 }
 
+static void i915_audio_component_codec_wake_override(struct device *dev,
+						     bool enable)
+{
+	struct drm_i915_private *dev_priv = dev_to_i915(dev);
+	u32 tmp;
+
+	if (!IS_SKYLAKE(dev_priv))
+		return;
+
+	/*
+	 * Enable/disable generating the codec wake signal, overriding the
+	 * internal logic to generate the codec wake to controller.
+	 */
+	tmp = I915_READ(HSW_AUD_CHICKENBIT);
+	tmp &= ~SKL_AUD_CODEC_WAKE_SIGNAL;
+	I915_WRITE(HSW_AUD_CHICKENBIT, tmp);
+	usleep_range(1000, 1500);
+
+	if (enable) {
+		tmp = I915_READ(HSW_AUD_CHICKENBIT);
+		tmp |= SKL_AUD_CODEC_WAKE_SIGNAL;
+		I915_WRITE(HSW_AUD_CHICKENBIT, tmp);
+		usleep_range(1000, 1500);
+	}
+}
+
 /* Get CDCLK in kHz  */
 static int i915_audio_component_get_cdclk_freq(struct device *dev)
 {
@@ -492,6 +518,7 @@ static const struct i915_audio_component_ops i915_audio_component_ops = {
 	.owner		= THIS_MODULE,
 	.get_power	= i915_audio_component_get_power,
 	.put_power	= i915_audio_component_put_power,
+	.codec_wake_override = i915_audio_component_codec_wake_override,
 	.get_cdclk_freq	= i915_audio_component_get_cdclk_freq,
 };
 
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index bd92e9ce4127..ede652867596 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -8096,7 +8096,7 @@ static void chv_crtc_clock_get(struct intel_crtc *crtc,
 	int pipe = pipe_config->cpu_transcoder;
 	enum dpio_channel port = vlv_pipe_to_channel(pipe);
 	intel_clock_t clock;
-	u32 cmn_dw13, pll_dw0, pll_dw1, pll_dw2;
+	u32 cmn_dw13, pll_dw0, pll_dw1, pll_dw2, pll_dw3;
 	int refclk = 100000;
 
 	mutex_lock(&dev_priv->sb_lock);
@@ -8104,10 +8104,13 @@ static void chv_crtc_clock_get(struct intel_crtc *crtc,
 	pll_dw0 = vlv_dpio_read(dev_priv, pipe, CHV_PLL_DW0(port));
 	pll_dw1 = vlv_dpio_read(dev_priv, pipe, CHV_PLL_DW1(port));
 	pll_dw2 = vlv_dpio_read(dev_priv, pipe, CHV_PLL_DW2(port));
+	pll_dw3 = vlv_dpio_read(dev_priv, pipe, CHV_PLL_DW3(port));
 	mutex_unlock(&dev_priv->sb_lock);
 
 	clock.m1 = (pll_dw1 & 0x7) == DPIO_CHV_M1_DIV_BY_2 ? 2 : 0;
-	clock.m2 = ((pll_dw0 & 0xff) << 22) | (pll_dw2 & 0x3fffff);
+	clock.m2 = (pll_dw0 & 0xff) << 22;
+	if (pll_dw3 & DPIO_CHV_FRAC_DIV_EN)
+		clock.m2 |= pll_dw2 & 0x3fffff;
 	clock.n = (pll_dw1 >> DPIO_CHV_N_DIV_SHIFT) & 0xf;
 	clock.p1 = (cmn_dw13 >> DPIO_CHV_P1_DIV_SHIFT) & 0x7;
 	clock.p2 = (cmn_dw13 >> DPIO_CHV_P2_DIV_SHIFT) & 0x1f;
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 085d84156008..f1b9f939b435 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -900,10 +900,8 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 				continue;
 			}
 			if (status & DP_AUX_CH_CTL_DONE)
-				break;
+				goto done;
 		}
-		if (status & DP_AUX_CH_CTL_DONE)
-			break;
 	}
 
 	if ((status & DP_AUX_CH_CTL_DONE) == 0) {
@@ -912,6 +910,7 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 		goto out;
 	}
 
+done:
 	/* Check for timeout or receive error.
 	 * Timeouts occur when the sink is not connected
 	 */
@@ -1148,6 +1147,9 @@ skl_edp_set_pll_config(struct intel_crtc_state *pipe_config, int link_clock)
 static void
 hsw_dp_set_ddi_pll_sel(struct intel_crtc_state *pipe_config, int link_bw)
 {
+	memset(&pipe_config->dpll_hw_state, 0,
+	       sizeof(pipe_config->dpll_hw_state));
+
 	switch (link_bw) {
 	case DP_LINK_BW_1_62:
 		pipe_config->ddi_pll_sel = PORT_CLK_SEL_LCPLL_810;
diff --git a/drivers/gpu/drm/i915/intel_i2c.c b/drivers/gpu/drm/i915/intel_i2c.c
index 92072f56e418..a64f26c670af 100644
--- a/drivers/gpu/drm/i915/intel_i2c.c
+++ b/drivers/gpu/drm/i915/intel_i2c.c
@@ -486,7 +486,7 @@ gmbus_xfer(struct i2c_adapter *adapter,
 					       struct intel_gmbus,
 					       adapter);
 	struct drm_i915_private *dev_priv = bus->dev_priv;
-	int i, reg_offset;
+	int i = 0, inc, try = 0, reg_offset;
 	int ret = 0;
 
 	intel_aux_display_runtime_get(dev_priv);
@@ -499,12 +499,14 @@ gmbus_xfer(struct i2c_adapter *adapter,
 
 	reg_offset = dev_priv->gpio_mmio_base;
 
+retry:
 	I915_WRITE(GMBUS0 + reg_offset, bus->reg0);
 
-	for (i = 0; i < num; i++) {
+	for (; i < num; i += inc) {
+		inc = 1;
 		if (gmbus_is_index_read(msgs, i, num)) {
 			ret = gmbus_xfer_index_read(dev_priv, &msgs[i]);
-			i += 1;  /* set i to the index of the read xfer */
+			inc = 2; /* an index read is two msgs */
 		} else if (msgs[i].flags & I2C_M_RD) {
 			ret = gmbus_xfer_read(dev_priv, &msgs[i], 0);
 		} else {
@@ -576,6 +578,18 @@ clear_err:
 			 adapter->name, msgs[i].addr,
 			 (msgs[i].flags & I2C_M_RD) ? 'r' : 'w', msgs[i].len);
 
+	/*
+	 * Passive adapters sometimes NAK the first probe. Retry the first
+	 * message once on -ENXIO for GMBUS transfers; the bit banging algorithm
+	 * has retries internally. See also the retry loop in
+	 * drm_do_probe_ddc_edid, which bails out on the first -ENXIO.
+	 */
+	if (ret == -ENXIO && i == 0 && try++ == 0) {
+		DRM_DEBUG_KMS("GMBUS [%s] NAK on first message, retry\n",
+			      adapter->name);
+		goto retry;
+	}
+
 	goto out;
 
 timeout:
diff --git a/drivers/gpu/drm/i915/intel_lrc.c b/drivers/gpu/drm/i915/intel_lrc.c
index 83ed0f7fcbe2..9faad82c42ec 100644
--- a/drivers/gpu/drm/i915/intel_lrc.c
+++ b/drivers/gpu/drm/i915/intel_lrc.c
@@ -1421,6 +1421,12 @@ static int gen8_init_common_ring(struct intel_engine_cs *ring)
 	I915_WRITE_IMR(ring, ~(ring->irq_enable_mask | ring->irq_keep_mask));
 	I915_WRITE(RING_HWSTAM(ring->mmio_base), 0xffffffff);
 
+	if (ring->status_page.obj) {
+		I915_WRITE(RING_HWS_PGA(ring->mmio_base),
+			   (u32)ring->status_page.gfx_addr);
+		POSTING_READ(RING_HWS_PGA(ring->mmio_base));
+	}
+
 	I915_WRITE(RING_MODE_GEN7(ring),
 		   _MASKED_BIT_DISABLE(GFX_REPLAY_MODE) |
 		   _MASKED_BIT_ENABLE(GFX_RUN_LIST_ENABLE));
diff --git a/drivers/gpu/drm/i915/intel_opregion.c b/drivers/gpu/drm/i915/intel_opregion.c
index 7df916e914a4..cb1c65739425 100644
--- a/drivers/gpu/drm/i915/intel_opregion.c
+++ b/drivers/gpu/drm/i915/intel_opregion.c
@@ -403,16 +403,6 @@ int intel_opregion_notify_adapter(struct drm_device *dev, pci_power_t state)
 	return -EINVAL;
 }
 
-/*
- * If the vendor backlight interface is not in use and ACPI backlight interface
- * is broken, do not bother processing backlight change requests from firmware.
- */
-static bool should_ignore_backlight_request(void)
-{
-	return acpi_video_backlight_support() &&
-	       !acpi_video_verify_backlight_support();
-}
-
 static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -421,7 +411,7 @@ static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 
 	DRM_DEBUG_DRIVER("bclp = 0x%08x\n", bclp);
 
-	if (should_ignore_backlight_request()) {
+	if (acpi_video_get_backlight_type() == acpi_backlight_native) {
 		DRM_DEBUG_KMS("opregion backlight request ignored\n");
 		return 0;
 	}
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index 7d83527f95f7..55aad2322e10 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -907,6 +907,14 @@ static void i9xx_enable_backlight(struct intel_connector *connector)
 
 	/* XXX: combine this into above write? */
 	intel_panel_actually_set_backlight(connector, panel->backlight.level);
+
+	/*
+	 * Needed to enable backlight on some 855gm models. BLC_HIST_CTL is
+	 * 855gm only, but checking for gen2 is safe, as 855gm is the only gen2
+	 * that has backlight.
+	 */
+	if (IS_GEN2(dev))
+		I915_WRITE(BLC_HIST_CTL, BLM_HISTOGRAM_ENABLE);
 }
 
 static void i965_enable_backlight(struct intel_connector *connector)
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 1c14233d179f..177f7ed16cf0 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -1041,6 +1041,13 @@ static int skl_init_workarounds(struct intel_engine_cs *ring)
 		WA_SET_BIT_MASKED(HIZ_CHICKEN,
 				  BDW_HIZ_POWER_COMPILER_CLOCK_GATING_DISABLE);
 
+	if (INTEL_REVID(dev) == SKL_REVID_C0 ||
+	    INTEL_REVID(dev) == SKL_REVID_D0)
+		/* WaBarrierPerformanceFixDisable:skl */
+		WA_SET_BIT_MASKED(HDC_CHICKEN0,
+				  HDC_FENCE_DEST_SLM_DISABLE |
+				  HDC_BARRIER_PERFORMANCE_DISABLE);
+
 	if (INTEL_REVID(dev) <= SKL_REVID_D0) {
 		/*
 		 *Use Force Non-Coherent whenever executing a 3D context. This
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index 0ea89ea30182..2e85fda94963 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -292,6 +292,13 @@ struct  intel_engine_cs {
 	 */
 	struct list_head request_list;
 
+	/**
+	 * Seqno of request most recently submitted to request_list.
+	 * Used exclusively by hang checker to avoid grabbing lock while
+	 * inspecting request list.
+	 */
+	u32 last_submitted_seqno;
+
 	bool gpu_caches_dirty;
 
 	wait_queue_head_t irq_queue;
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index d24ef75596a1..aa2fd751609c 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -2554,7 +2554,7 @@ intel_sdvo_analog_init(struct intel_sdvo *intel_sdvo, int device)
 
 	DRM_DEBUG_KMS("initialising analog device %d\n", device);
 
-	intel_sdvo_connector = kzalloc(sizeof(*intel_sdvo_connector), GFP_KERNEL);
+	intel_sdvo_connector = intel_sdvo_connector_alloc();
 	if (!intel_sdvo_connector)
 		return false;
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index 6e84df9369a6..ad4b9010dfb0 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -1526,6 +1526,11 @@ static int mga_vga_mode_valid(struct drm_connector *connector,
 		return MODE_BANDWIDTH;
 	}
 
+	if ((mode->hdisplay % 8) != 0 || (mode->hsync_start % 8) != 0 ||
+	    (mode->hsync_end % 8) != 0 || (mode->htotal % 8) != 0) {
+		return MODE_H_ILLEGAL;
+	}
+
 	if (mode->crtc_hdisplay > 2048 || mode->crtc_hsync_start > 4096 ||
 	    mode->crtc_hsync_end > 4096 || mode->crtc_htotal > 4096 ||
 	    mode->crtc_vdisplay > 2048 || mode->crtc_vsync_start > 4096 ||
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.c b/drivers/gpu/drm/nouveau/nouveau_drm.c
index 89049335b738..649024d4daf1 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.c
@@ -23,6 +23,7 @@
  */
 
 #include <linux/console.h>
+#include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/pm_runtime.h>
@@ -666,6 +667,7 @@ nouveau_pmops_suspend(struct device *dev)
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
 	pci_set_power_state(pdev, PCI_D3hot);
+	udelay(200);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_gem.c b/drivers/gpu/drm/nouveau/nouveau_gem.c
index 0e690bf19fc9..af1ee517f372 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gem.c
@@ -555,10 +555,7 @@ nouveau_gem_pushbuf_validate(struct nouveau_channel *chan,
 static inline void
 u_free(void *addr)
 {
-	if (!is_vmalloc_addr(addr))
-		kfree(addr);
-	else
-		vfree(addr);
+	kvfree(addr);
 }
 
 static inline void *
diff --git a/drivers/gpu/drm/omapdrm/omap_dmm_tiler.c b/drivers/gpu/drm/omapdrm/omap_dmm_tiler.c
index f2daad8c3d96..7841970de48d 100644
--- a/drivers/gpu/drm/omapdrm/omap_dmm_tiler.c
+++ b/drivers/gpu/drm/omapdrm/omap_dmm_tiler.c
@@ -285,7 +285,7 @@ static int dmm_txn_commit(struct dmm_txn *txn, bool wait)
 
 	if (wait) {
 		if (!wait_for_completion_timeout(&engine->compl,
-				msecs_to_jiffies(1))) {
+				msecs_to_jiffies(100))) {
 			dev_err(dmm->dev, "timed out waiting for done\n");
 			ret = -ETIMEDOUT;
 		}
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.h b/drivers/gpu/drm/omapdrm/omap_drv.h
index ae2df41f216f..12081e61d45a 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.h
+++ b/drivers/gpu/drm/omapdrm/omap_drv.h
@@ -177,7 +177,7 @@ struct drm_framebuffer *omap_framebuffer_init(struct drm_device *dev,
 		struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos);
 struct drm_gem_object *omap_framebuffer_bo(struct drm_framebuffer *fb, int p);
 int omap_framebuffer_pin(struct drm_framebuffer *fb);
-int omap_framebuffer_unpin(struct drm_framebuffer *fb);
+void omap_framebuffer_unpin(struct drm_framebuffer *fb);
 void omap_framebuffer_update_scanout(struct drm_framebuffer *fb,
 		struct omap_drm_window *win, struct omap_overlay_info *info);
 struct drm_connector *omap_framebuffer_get_next_connector(
@@ -211,7 +211,7 @@ void omap_gem_dma_sync(struct drm_gem_object *obj,
 		enum dma_data_direction dir);
 int omap_gem_get_paddr(struct drm_gem_object *obj,
 		dma_addr_t *paddr, bool remap);
-int omap_gem_put_paddr(struct drm_gem_object *obj);
+void omap_gem_put_paddr(struct drm_gem_object *obj);
 int omap_gem_get_pages(struct drm_gem_object *obj, struct page ***pages,
 		bool remap);
 int omap_gem_put_pages(struct drm_gem_object *obj);
@@ -236,7 +236,7 @@ static inline int align_pitch(int pitch, int width, int bpp)
 	/* PVR needs alignment to 8 pixels.. right now that is the most
 	 * restrictive stride requirement..
 	 */
-	return ALIGN(pitch, 8 * bytespp);
+	return roundup(pitch, 8 * bytespp);
 }
 
 /* map crtc to vblank mask */
diff --git a/drivers/gpu/drm/omapdrm/omap_fb.c b/drivers/gpu/drm/omapdrm/omap_fb.c
index 0b967e76df1a..51b1219af87f 100644
--- a/drivers/gpu/drm/omapdrm/omap_fb.c
+++ b/drivers/gpu/drm/omapdrm/omap_fb.c
@@ -287,10 +287,10 @@ fail:
 }
 
 /* unpin, no longer being scanned out: */
-int omap_framebuffer_unpin(struct drm_framebuffer *fb)
+void omap_framebuffer_unpin(struct drm_framebuffer *fb)
 {
 	struct omap_framebuffer *omap_fb = to_omap_framebuffer(fb);
-	int ret, i, n = drm_format_num_planes(fb->pixel_format);
+	int i, n = drm_format_num_planes(fb->pixel_format);
 
 	mutex_lock(&omap_fb->lock);
 
@@ -298,24 +298,16 @@ int omap_framebuffer_unpin(struct drm_framebuffer *fb)
 
 	if (omap_fb->pin_count > 0) {
 		mutex_unlock(&omap_fb->lock);
-		return 0;
+		return;
 	}
 
 	for (i = 0; i < n; i++) {
 		struct plane *plane = &omap_fb->planes[i];
-		ret = omap_gem_put_paddr(plane->bo);
-		if (ret)
-			goto fail;
+		omap_gem_put_paddr(plane->bo);
 		plane->paddr = 0;
 	}
 
 	mutex_unlock(&omap_fb->lock);
-
-	return 0;
-
-fail:
-	mutex_unlock(&omap_fb->lock);
-	return ret;
 }
 
 struct drm_gem_object *omap_framebuffer_bo(struct drm_framebuffer *fb, int p)
diff --git a/drivers/gpu/drm/omapdrm/omap_fbdev.c b/drivers/gpu/drm/omapdrm/omap_fbdev.c
index 23b5a84389e3..720d16bce7e8 100644
--- a/drivers/gpu/drm/omapdrm/omap_fbdev.c
+++ b/drivers/gpu/drm/omapdrm/omap_fbdev.c
@@ -135,7 +135,7 @@ static int omap_fbdev_create(struct drm_fb_helper *helper,
 	fbdev->ywrap_enabled = priv->has_dmm && ywrap_enabled;
 	if (fbdev->ywrap_enabled) {
 		/* need to align pitch to page size if using DMM scrolling */
-		mode_cmd.pitches[0] = ALIGN(mode_cmd.pitches[0], PAGE_SIZE);
+		mode_cmd.pitches[0] = PAGE_ALIGN(mode_cmd.pitches[0]);
 	}
 
 	/* allocate backing bo */
diff --git a/drivers/gpu/drm/omapdrm/omap_gem.c b/drivers/gpu/drm/omapdrm/omap_gem.c
index 2ab77801cf5f..7ed08fdc4c42 100644
--- a/drivers/gpu/drm/omapdrm/omap_gem.c
+++ b/drivers/gpu/drm/omapdrm/omap_gem.c
@@ -808,10 +808,10 @@ fail:
 /* Release physical address, when DMA is no longer being performed.. this
  * could potentially unpin and unmap buffers from TILER
  */
-int omap_gem_put_paddr(struct drm_gem_object *obj)
+void omap_gem_put_paddr(struct drm_gem_object *obj)
 {
 	struct omap_gem_object *omap_obj = to_omap_bo(obj);
-	int ret = 0;
+	int ret;
 
 	mutex_lock(&obj->dev->struct_mutex);
 	if (omap_obj->paddr_cnt > 0) {
@@ -821,7 +821,6 @@ int omap_gem_put_paddr(struct drm_gem_object *obj)
 			if (ret) {
 				dev_err(obj->dev->dev,
 					"could not unpin pages: %d\n", ret);
-				goto fail;
 			}
 			ret = tiler_release(omap_obj->block);
 			if (ret) {
@@ -832,9 +831,8 @@ int omap_gem_put_paddr(struct drm_gem_object *obj)
 			omap_obj->block = NULL;
 		}
 	}
-fail:
+
 	mutex_unlock(&obj->dev->struct_mutex);
-	return ret;
 }
 
 /* Get rotated scanout address (only valid if already pinned), at the
@@ -1378,11 +1376,7 @@ struct drm_gem_object *omap_gem_new(struct drm_device *dev,
 
 	omap_obj = kzalloc(sizeof(*omap_obj), GFP_KERNEL);
 	if (!omap_obj)
-		goto fail;
-
-	spin_lock(&priv->list_lock);
-	list_add(&omap_obj->mm_list, &priv->obj_list);
-	spin_unlock(&priv->list_lock);
+		return NULL;
 
 	obj = &omap_obj->base;
 
@@ -1392,11 +1386,19 @@ struct drm_gem_object *omap_gem_new(struct drm_device *dev,
 		 */
 		omap_obj->vaddr =  dma_alloc_writecombine(dev->dev, size,
 				&omap_obj->paddr, GFP_KERNEL);
-		if (omap_obj->vaddr)
-			flags |= OMAP_BO_DMA;
+		if (!omap_obj->vaddr) {
+			kfree(omap_obj);
+
+			return NULL;
+		}
 
+		flags |= OMAP_BO_DMA;
 	}
 
+	spin_lock(&priv->list_lock);
+	list_add(&omap_obj->mm_list, &priv->obj_list);
+	spin_unlock(&priv->list_lock);
+
 	omap_obj->flags = flags;
 
 	if (flags & OMAP_BO_TILED) {
diff --git a/drivers/gpu/drm/omapdrm/omap_plane.c b/drivers/gpu/drm/omapdrm/omap_plane.c
index cfa8276c4deb..098904696a5c 100644
--- a/drivers/gpu/drm/omapdrm/omap_plane.c
+++ b/drivers/gpu/drm/omapdrm/omap_plane.c
@@ -17,6 +17,7 @@
  * this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <drm/drm_atomic.h>
 #include <drm/drm_atomic_helper.h>
 #include <drm/drm_plane_helper.h>
 
@@ -153,9 +154,34 @@ static void omap_plane_atomic_disable(struct drm_plane *plane,
 	dispc_ovl_enable(omap_plane->id, false);
 }
 
+static int omap_plane_atomic_check(struct drm_plane *plane,
+				   struct drm_plane_state *state)
+{
+	struct drm_crtc_state *crtc_state;
+
+	if (!state->crtc)
+		return 0;
+
+	crtc_state = drm_atomic_get_crtc_state(state->state, state->crtc);
+	if (IS_ERR(crtc_state))
+		return PTR_ERR(crtc_state);
+
+	if (state->crtc_x < 0 || state->crtc_y < 0)
+		return -EINVAL;
+
+	if (state->crtc_x + state->crtc_w > crtc_state->adjusted_mode.hdisplay)
+		return -EINVAL;
+
+	if (state->crtc_y + state->crtc_h > crtc_state->adjusted_mode.vdisplay)
+		return -EINVAL;
+
+	return 0;
+}
+
 static const struct drm_plane_helper_funcs omap_plane_helper_funcs = {
 	.prepare_fb = omap_plane_prepare_fb,
 	.cleanup_fb = omap_plane_cleanup_fb,
+	.atomic_check = omap_plane_atomic_check,
 	.atomic_update = omap_plane_atomic_update,
 	.atomic_disable = omap_plane_atomic_disable,
 };
diff --git a/drivers/gpu/drm/radeon/atombios.h b/drivers/gpu/drm/radeon/atombios.h
index 92be50c39ffd..ab89eed9ddd9 100644
--- a/drivers/gpu/drm/radeon/atombios.h
+++ b/drivers/gpu/drm/radeon/atombios.h
@@ -7944,8 +7944,8 @@ typedef struct {
 typedef struct {
   AMD_ACPI_DESCRIPTION_HEADER SHeader;
   UCHAR TableUUID[16];    //0x24
-  ULONG VBIOSImageOffset; //0x34. Offset to the first GOP_VBIOS_CONTENT block from the beginning of the stucture.
-  ULONG Lib1ImageOffset;  //0x38. Offset to the first GOP_LIB1_CONTENT block from the beginning of the stucture.
+  ULONG VBIOSImageOffset; //0x34. Offset to the first GOP_VBIOS_CONTENT block from the beginning of the structure.
+  ULONG Lib1ImageOffset;  //0x38. Offset to the first GOP_LIB1_CONTENT block from the beginning of the structure.
   ULONG Reserved[4];      //0x3C
 }UEFI_ACPI_VFCT;
 
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index e597ffc26563..dac78ad24b31 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -580,9 +580,6 @@ static u32 atombios_adjust_pll(struct drm_crtc *crtc,
 		else
 			radeon_crtc->pll_flags |= RADEON_PLL_PREFER_LOW_REF_DIV;
 
-		/* if there is no audio, set MINM_OVER_MAXP  */
-		if (!drm_detect_monitor_audio(radeon_connector_edid(connector)))
-			radeon_crtc->pll_flags |= RADEON_PLL_PREFER_MINM_OVER_MAXP;
 		if (rdev->family < CHIP_RV770)
 			radeon_crtc->pll_flags |= RADEON_PLL_PREFER_MINM_OVER_MAXP;
 		/* use frac fb div on APUs */
@@ -1798,9 +1795,7 @@ static int radeon_get_shared_nondp_ppll(struct drm_crtc *crtc)
 			if ((crtc->mode.clock == test_crtc->mode.clock) &&
 			    (adjusted_clock == test_adjusted_clock) &&
 			    (radeon_crtc->ss_enabled == test_radeon_crtc->ss_enabled) &&
-			    (test_radeon_crtc->pll_id != ATOM_PPLL_INVALID) &&
-			    (drm_detect_monitor_audio(radeon_connector_edid(test_radeon_crtc->connector)) ==
-			     drm_detect_monitor_audio(radeon_connector_edid(radeon_crtc->connector))))
+			    (test_radeon_crtc->pll_id != ATOM_PPLL_INVALID))
 				return test_radeon_crtc->pll_id;
 		}
 	}
diff --git a/drivers/gpu/drm/radeon/cik.c b/drivers/gpu/drm/radeon/cik.c
index b0688b0c8908..248953d2fdb7 100644
--- a/drivers/gpu/drm/radeon/cik.c
+++ b/drivers/gpu/drm/radeon/cik.c
@@ -4604,6 +4604,31 @@ void cik_compute_set_wptr(struct radeon_device *rdev,
 	WDOORBELL32(ring->doorbell_index, ring->wptr);
 }
 
+static void cik_compute_stop(struct radeon_device *rdev,
+			     struct radeon_ring *ring)
+{
+	u32 j, tmp;
+
+	cik_srbm_select(rdev, ring->me, ring->pipe, ring->queue, 0);
+	/* Disable wptr polling. */
+	tmp = RREG32(CP_PQ_WPTR_POLL_CNTL);
+	tmp &= ~WPTR_POLL_EN;
+	WREG32(CP_PQ_WPTR_POLL_CNTL, tmp);
+	/* Disable HQD. */
+	if (RREG32(CP_HQD_ACTIVE) & 1) {
+		WREG32(CP_HQD_DEQUEUE_REQUEST, 1);
+		for (j = 0; j < rdev->usec_timeout; j++) {
+			if (!(RREG32(CP_HQD_ACTIVE) & 1))
+				break;
+			udelay(1);
+		}
+		WREG32(CP_HQD_DEQUEUE_REQUEST, 0);
+		WREG32(CP_HQD_PQ_RPTR, 0);
+		WREG32(CP_HQD_PQ_WPTR, 0);
+	}
+	cik_srbm_select(rdev, 0, 0, 0, 0);
+}
+
 /**
  * cik_cp_compute_enable - enable/disable the compute CP MEs
  *
@@ -4617,6 +4642,15 @@ static void cik_cp_compute_enable(struct radeon_device *rdev, bool enable)
 	if (enable)
 		WREG32(CP_MEC_CNTL, 0);
 	else {
+		/*
+		 * To make hibernation reliable we need to clear compute ring
+		 * configuration before halting the compute ring.
+		 */
+		mutex_lock(&rdev->srbm_mutex);
+		cik_compute_stop(rdev,&rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX]);
+		cik_compute_stop(rdev,&rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX]);
+		mutex_unlock(&rdev->srbm_mutex);
+
 		WREG32(CP_MEC_CNTL, (MEC_ME1_HALT | MEC_ME2_HALT));
 		rdev->ring[CAYMAN_RING_TYPE_CP1_INDEX].ready = false;
 		rdev->ring[CAYMAN_RING_TYPE_CP2_INDEX].ready = false;
@@ -7930,23 +7964,27 @@ restart_ih:
 		case 1: /* D1 vblank/vline */
 			switch (src_data) {
 			case 0: /* D1 vblank */
-				if (rdev->irq.stat_regs.cik.disp_int & LB_D1_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[0]) {
-						drm_handle_vblank(rdev->ddev, 0);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[0]))
-						radeon_crtc_handle_vblank(rdev, 0);
-					rdev->irq.stat_regs.cik.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D1 vblank\n");
+				if (!(rdev->irq.stat_regs.cik.disp_int & LB_D1_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[0]) {
+					drm_handle_vblank(rdev->ddev, 0);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[0]))
+					radeon_crtc_handle_vblank(rdev, 0);
+				rdev->irq.stat_regs.cik.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D1 vblank\n");
+
 				break;
 			case 1: /* D1 vline */
-				if (rdev->irq.stat_regs.cik.disp_int & LB_D1_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int &= ~LB_D1_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D1 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int & LB_D1_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int &= ~LB_D1_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D1 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -7956,23 +7994,27 @@ restart_ih:
 		case 2: /* D2 vblank/vline */
 			switch (src_data) {
 			case 0: /* D2 vblank */
-				if (rdev->irq.stat_regs.cik.disp_int_cont & LB_D2_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[1]) {
-						drm_handle_vblank(rdev->ddev, 1);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[1]))
-						radeon_crtc_handle_vblank(rdev, 1);
-					rdev->irq.stat_regs.cik.disp_int_cont &= ~LB_D2_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D2 vblank\n");
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont & LB_D2_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[1]) {
+					drm_handle_vblank(rdev->ddev, 1);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[1]))
+					radeon_crtc_handle_vblank(rdev, 1);
+				rdev->irq.stat_regs.cik.disp_int_cont &= ~LB_D2_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D2 vblank\n");
+
 				break;
 			case 1: /* D2 vline */
-				if (rdev->irq.stat_regs.cik.disp_int_cont & LB_D2_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont &= ~LB_D2_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D2 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont & LB_D2_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont &= ~LB_D2_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D2 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -7982,23 +8024,27 @@ restart_ih:
 		case 3: /* D3 vblank/vline */
 			switch (src_data) {
 			case 0: /* D3 vblank */
-				if (rdev->irq.stat_regs.cik.disp_int_cont2 & LB_D3_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[2]) {
-						drm_handle_vblank(rdev->ddev, 2);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[2]))
-						radeon_crtc_handle_vblank(rdev, 2);
-					rdev->irq.stat_regs.cik.disp_int_cont2 &= ~LB_D3_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D3 vblank\n");
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont2 & LB_D3_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[2]) {
+					drm_handle_vblank(rdev->ddev, 2);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[2]))
+					radeon_crtc_handle_vblank(rdev, 2);
+				rdev->irq.stat_regs.cik.disp_int_cont2 &= ~LB_D3_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D3 vblank\n");
+
 				break;
 			case 1: /* D3 vline */
-				if (rdev->irq.stat_regs.cik.disp_int_cont2 & LB_D3_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont2 &= ~LB_D3_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D3 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont2 & LB_D3_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont2 &= ~LB_D3_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D3 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -8008,23 +8054,27 @@ restart_ih:
 		case 4: /* D4 vblank/vline */
 			switch (src_data) {
 			case 0: /* D4 vblank */
-				if (rdev->irq.stat_regs.cik.disp_int_cont3 & LB_D4_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[3]) {
-						drm_handle_vblank(rdev->ddev, 3);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[3]))
-						radeon_crtc_handle_vblank(rdev, 3);
-					rdev->irq.stat_regs.cik.disp_int_cont3 &= ~LB_D4_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D4 vblank\n");
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont3 & LB_D4_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[3]) {
+					drm_handle_vblank(rdev->ddev, 3);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[3]))
+					radeon_crtc_handle_vblank(rdev, 3);
+				rdev->irq.stat_regs.cik.disp_int_cont3 &= ~LB_D4_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D4 vblank\n");
+
 				break;
 			case 1: /* D4 vline */
-				if (rdev->irq.stat_regs.cik.disp_int_cont3 & LB_D4_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont3 &= ~LB_D4_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D4 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont3 & LB_D4_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont3 &= ~LB_D4_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D4 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -8034,23 +8084,27 @@ restart_ih:
 		case 5: /* D5 vblank/vline */
 			switch (src_data) {
 			case 0: /* D5 vblank */
-				if (rdev->irq.stat_regs.cik.disp_int_cont4 & LB_D5_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[4]) {
-						drm_handle_vblank(rdev->ddev, 4);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[4]))
-						radeon_crtc_handle_vblank(rdev, 4);
-					rdev->irq.stat_regs.cik.disp_int_cont4 &= ~LB_D5_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D5 vblank\n");
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont4 & LB_D5_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[4]) {
+					drm_handle_vblank(rdev->ddev, 4);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[4]))
+					radeon_crtc_handle_vblank(rdev, 4);
+				rdev->irq.stat_regs.cik.disp_int_cont4 &= ~LB_D5_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D5 vblank\n");
+
 				break;
 			case 1: /* D5 vline */
-				if (rdev->irq.stat_regs.cik.disp_int_cont4 & LB_D5_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont4 &= ~LB_D5_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D5 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont4 & LB_D5_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont4 &= ~LB_D5_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D5 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -8060,23 +8114,27 @@ restart_ih:
 		case 6: /* D6 vblank/vline */
 			switch (src_data) {
 			case 0: /* D6 vblank */
-				if (rdev->irq.stat_regs.cik.disp_int_cont5 & LB_D6_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[5]) {
-						drm_handle_vblank(rdev->ddev, 5);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[5]))
-						radeon_crtc_handle_vblank(rdev, 5);
-					rdev->irq.stat_regs.cik.disp_int_cont5 &= ~LB_D6_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D6 vblank\n");
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont5 & LB_D6_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[5]) {
+					drm_handle_vblank(rdev->ddev, 5);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[5]))
+					radeon_crtc_handle_vblank(rdev, 5);
+				rdev->irq.stat_regs.cik.disp_int_cont5 &= ~LB_D6_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D6 vblank\n");
+
 				break;
 			case 1: /* D6 vline */
-				if (rdev->irq.stat_regs.cik.disp_int_cont5 & LB_D6_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont5 &= ~LB_D6_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D6 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont5 & LB_D6_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont5 &= ~LB_D6_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D6 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -8096,88 +8154,112 @@ restart_ih:
 		case 42: /* HPD hotplug */
 			switch (src_data) {
 			case 0:
-				if (rdev->irq.stat_regs.cik.disp_int & DC_HPD1_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int &= ~DC_HPD1_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD1\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int & DC_HPD1_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int &= ~DC_HPD1_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD1\n");
+
 				break;
 			case 1:
-				if (rdev->irq.stat_regs.cik.disp_int_cont & DC_HPD2_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont &= ~DC_HPD2_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD2\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont & DC_HPD2_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont &= ~DC_HPD2_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD2\n");
+
 				break;
 			case 2:
-				if (rdev->irq.stat_regs.cik.disp_int_cont2 & DC_HPD3_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont2 &= ~DC_HPD3_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD3\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont2 & DC_HPD3_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont2 &= ~DC_HPD3_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD3\n");
+
 				break;
 			case 3:
-				if (rdev->irq.stat_regs.cik.disp_int_cont3 & DC_HPD4_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont3 &= ~DC_HPD4_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD4\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont3 & DC_HPD4_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont3 &= ~DC_HPD4_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD4\n");
+
 				break;
 			case 4:
-				if (rdev->irq.stat_regs.cik.disp_int_cont4 & DC_HPD5_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont4 &= ~DC_HPD5_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD5\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont4 & DC_HPD5_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont4 &= ~DC_HPD5_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD5\n");
+
 				break;
 			case 5:
-				if (rdev->irq.stat_regs.cik.disp_int_cont5 & DC_HPD6_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont5 &= ~DC_HPD6_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD6\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont5 & DC_HPD6_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont5 &= ~DC_HPD6_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD6\n");
+
 				break;
 			case 6:
-				if (rdev->irq.stat_regs.cik.disp_int & DC_HPD1_RX_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int &= ~DC_HPD1_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 1\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int & DC_HPD1_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int &= ~DC_HPD1_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 1\n");
+
 				break;
 			case 7:
-				if (rdev->irq.stat_regs.cik.disp_int_cont & DC_HPD2_RX_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont &= ~DC_HPD2_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 2\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont & DC_HPD2_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont &= ~DC_HPD2_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 2\n");
+
 				break;
 			case 8:
-				if (rdev->irq.stat_regs.cik.disp_int_cont2 & DC_HPD3_RX_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont2 &= ~DC_HPD3_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 3\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont2 & DC_HPD3_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont2 &= ~DC_HPD3_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 3\n");
+
 				break;
 			case 9:
-				if (rdev->irq.stat_regs.cik.disp_int_cont3 & DC_HPD4_RX_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont3 &= ~DC_HPD4_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 4\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont3 & DC_HPD4_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont3 &= ~DC_HPD4_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 4\n");
+
 				break;
 			case 10:
-				if (rdev->irq.stat_regs.cik.disp_int_cont4 & DC_HPD5_RX_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont4 &= ~DC_HPD5_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 5\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont4 & DC_HPD5_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont4 &= ~DC_HPD5_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 5\n");
+
 				break;
 			case 11:
-				if (rdev->irq.stat_regs.cik.disp_int_cont5 & DC_HPD6_RX_INTERRUPT) {
-					rdev->irq.stat_regs.cik.disp_int_cont5 &= ~DC_HPD6_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 6\n");
-				}
+				if (!(rdev->irq.stat_regs.cik.disp_int_cont5 & DC_HPD6_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.cik.disp_int_cont5 &= ~DC_HPD6_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 6\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
diff --git a/drivers/gpu/drm/radeon/cik_sdma.c b/drivers/gpu/drm/radeon/cik_sdma.c
index f86eb54e7763..d16f2eebd95e 100644
--- a/drivers/gpu/drm/radeon/cik_sdma.c
+++ b/drivers/gpu/drm/radeon/cik_sdma.c
@@ -268,6 +268,17 @@ static void cik_sdma_gfx_stop(struct radeon_device *rdev)
 	}
 	rdev->ring[R600_RING_TYPE_DMA_INDEX].ready = false;
 	rdev->ring[CAYMAN_RING_TYPE_DMA1_INDEX].ready = false;
+
+	/* FIXME use something else than big hammer but after few days can not
+	 * seem to find good combination so reset SDMA blocks as it seems we
+	 * do not shut them down properly. This fix hibernation and does not
+	 * affect suspend to ram.
+	 */
+	WREG32(SRBM_SOFT_RESET, SOFT_RESET_SDMA | SOFT_RESET_SDMA1);
+	(void)RREG32(SRBM_SOFT_RESET);
+	udelay(50);
+	WREG32(SRBM_SOFT_RESET, 0);
+	(void)RREG32(SRBM_SOFT_RESET);
 }
 
 /**
diff --git a/drivers/gpu/drm/radeon/dce3_1_afmt.c b/drivers/gpu/drm/radeon/dce3_1_afmt.c
index f04205170b8a..cfa3a84a2af0 100644
--- a/drivers/gpu/drm/radeon/dce3_1_afmt.c
+++ b/drivers/gpu/drm/radeon/dce3_1_afmt.c
@@ -173,7 +173,7 @@ void dce3_2_hdmi_update_acr(struct drm_encoder *encoder, long offset,
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
 
-	WREG32(HDMI0_ACR_PACKET_CONTROL + offset,
+	WREG32(DCE3_HDMI0_ACR_PACKET_CONTROL + offset,
 		HDMI0_ACR_SOURCE |		/* select SW CTS value */
 		HDMI0_ACR_AUTO_SEND);	/* allow hw to sent ACR packets when required */
 
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index 3a6d483a2c36..0acde1949c18 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -4924,7 +4924,7 @@ restart_ih:
 		return IRQ_NONE;
 
 	rptr = rdev->ih.rptr;
-	DRM_DEBUG("r600_irq_process start: rptr %d, wptr %d\n", rptr, wptr);
+	DRM_DEBUG("evergreen_irq_process start: rptr %d, wptr %d\n", rptr, wptr);
 
 	/* Order reading of wptr vs. reading of IH ring data */
 	rmb();
@@ -4942,23 +4942,27 @@ restart_ih:
 		case 1: /* D1 vblank/vline */
 			switch (src_data) {
 			case 0: /* D1 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int & LB_D1_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[0]) {
-						drm_handle_vblank(rdev->ddev, 0);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[0]))
-						radeon_crtc_handle_vblank(rdev, 0);
-					rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D1 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int & LB_D1_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: D1 vblank - IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[0]) {
+					drm_handle_vblank(rdev->ddev, 0);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[0]))
+					radeon_crtc_handle_vblank(rdev, 0);
+				rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D1 vblank\n");
+
 				break;
 			case 1: /* D1 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int & LB_D1_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D1 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int & LB_D1_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: D1 vline - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D1 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -4968,23 +4972,27 @@ restart_ih:
 		case 2: /* D2 vblank/vline */
 			switch (src_data) {
 			case 0: /* D2 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont & LB_D2_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[1]) {
-						drm_handle_vblank(rdev->ddev, 1);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[1]))
-						radeon_crtc_handle_vblank(rdev, 1);
-					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D2 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont & LB_D2_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: D2 vblank - IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[1]) {
+					drm_handle_vblank(rdev->ddev, 1);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[1]))
+					radeon_crtc_handle_vblank(rdev, 1);
+				rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D2 vblank\n");
+
 				break;
 			case 1: /* D2 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont & LB_D2_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D2 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont & LB_D2_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: D2 vline - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D2 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -4994,23 +5002,27 @@ restart_ih:
 		case 3: /* D3 vblank/vline */
 			switch (src_data) {
 			case 0: /* D3 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont2 & LB_D3_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[2]) {
-						drm_handle_vblank(rdev->ddev, 2);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[2]))
-						radeon_crtc_handle_vblank(rdev, 2);
-					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D3 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont2 & LB_D3_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: D3 vblank - IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[2]) {
+					drm_handle_vblank(rdev->ddev, 2);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[2]))
+					radeon_crtc_handle_vblank(rdev, 2);
+				rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D3 vblank\n");
+
 				break;
 			case 1: /* D3 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont2 & LB_D3_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D3 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont2 & LB_D3_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: D3 vline - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D3 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -5020,23 +5032,27 @@ restart_ih:
 		case 4: /* D4 vblank/vline */
 			switch (src_data) {
 			case 0: /* D4 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont3 & LB_D4_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[3]) {
-						drm_handle_vblank(rdev->ddev, 3);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[3]))
-						radeon_crtc_handle_vblank(rdev, 3);
-					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D4 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont3 & LB_D4_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: D4 vblank - IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[3]) {
+					drm_handle_vblank(rdev->ddev, 3);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[3]))
+					radeon_crtc_handle_vblank(rdev, 3);
+				rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D4 vblank\n");
+
 				break;
 			case 1: /* D4 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont3 & LB_D4_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D4 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont3 & LB_D4_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: D4 vline - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D4 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -5046,23 +5062,27 @@ restart_ih:
 		case 5: /* D5 vblank/vline */
 			switch (src_data) {
 			case 0: /* D5 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont4 & LB_D5_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[4]) {
-						drm_handle_vblank(rdev->ddev, 4);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[4]))
-						radeon_crtc_handle_vblank(rdev, 4);
-					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D5 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont4 & LB_D5_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: D5 vblank - IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[4]) {
+					drm_handle_vblank(rdev->ddev, 4);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[4]))
+					radeon_crtc_handle_vblank(rdev, 4);
+				rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D5 vblank\n");
+
 				break;
 			case 1: /* D5 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont4 & LB_D5_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D5 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont4 & LB_D5_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: D5 vline - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D5 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -5072,23 +5092,27 @@ restart_ih:
 		case 6: /* D6 vblank/vline */
 			switch (src_data) {
 			case 0: /* D6 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & LB_D6_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[5]) {
-						drm_handle_vblank(rdev->ddev, 5);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[5]))
-						radeon_crtc_handle_vblank(rdev, 5);
-					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D6 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont5 & LB_D6_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: D6 vblank - IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[5]) {
+					drm_handle_vblank(rdev->ddev, 5);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[5]))
+					radeon_crtc_handle_vblank(rdev, 5);
+				rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D6 vblank\n");
+
 				break;
 			case 1: /* D6 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & LB_D6_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D6 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont5 & LB_D6_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: D6 vline - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D6 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -5108,88 +5132,100 @@ restart_ih:
 		case 42: /* HPD hotplug */
 			switch (src_data) {
 			case 0:
-				if (rdev->irq.stat_regs.evergreen.disp_int & DC_HPD1_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int &= ~DC_HPD1_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD1\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int & DC_HPD1_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int &= ~DC_HPD1_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD1\n");
 				break;
 			case 1:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont & DC_HPD2_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~DC_HPD2_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD2\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont & DC_HPD2_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont &= ~DC_HPD2_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD2\n");
 				break;
 			case 2:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont2 & DC_HPD3_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~DC_HPD3_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD3\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont2 & DC_HPD3_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~DC_HPD3_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD3\n");
 				break;
 			case 3:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont3 & DC_HPD4_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~DC_HPD4_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD4\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont3 & DC_HPD4_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~DC_HPD4_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD4\n");
 				break;
 			case 4:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont4 & DC_HPD5_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~DC_HPD5_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD5\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont4 & DC_HPD5_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~DC_HPD5_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD5\n");
 				break;
 			case 5:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~DC_HPD6_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD6\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~DC_HPD6_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD6\n");
 				break;
 			case 6:
-				if (rdev->irq.stat_regs.evergreen.disp_int & DC_HPD1_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int &= ~DC_HPD1_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 1\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int & DC_HPD1_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int &= ~DC_HPD1_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 1\n");
 				break;
 			case 7:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont & DC_HPD2_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~DC_HPD2_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 2\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont & DC_HPD2_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont &= ~DC_HPD2_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 2\n");
 				break;
 			case 8:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont2 & DC_HPD3_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~DC_HPD3_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 3\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont2 & DC_HPD3_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~DC_HPD3_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 3\n");
 				break;
 			case 9:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont3 & DC_HPD4_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~DC_HPD4_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 4\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont3 & DC_HPD4_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~DC_HPD4_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 4\n");
 				break;
 			case 10:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont4 & DC_HPD5_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~DC_HPD5_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 5\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont4 & DC_HPD5_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~DC_HPD5_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 5\n");
 				break;
 			case 11:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~DC_HPD6_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 6\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~DC_HPD6_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 6\n");
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -5199,46 +5235,52 @@ restart_ih:
 		case 44: /* hdmi */
 			switch (src_data) {
 			case 0:
-				if (rdev->irq.stat_regs.evergreen.afmt_status1 & AFMT_AZ_FORMAT_WTRIG) {
-					rdev->irq.stat_regs.evergreen.afmt_status1 &= ~AFMT_AZ_FORMAT_WTRIG;
-					queue_hdmi = true;
-					DRM_DEBUG("IH: HDMI0\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.afmt_status1 & AFMT_AZ_FORMAT_WTRIG))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.afmt_status1 &= ~AFMT_AZ_FORMAT_WTRIG;
+				queue_hdmi = true;
+				DRM_DEBUG("IH: HDMI0\n");
 				break;
 			case 1:
-				if (rdev->irq.stat_regs.evergreen.afmt_status2 & AFMT_AZ_FORMAT_WTRIG) {
-					rdev->irq.stat_regs.evergreen.afmt_status2 &= ~AFMT_AZ_FORMAT_WTRIG;
-					queue_hdmi = true;
-					DRM_DEBUG("IH: HDMI1\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.afmt_status2 & AFMT_AZ_FORMAT_WTRIG))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.afmt_status2 &= ~AFMT_AZ_FORMAT_WTRIG;
+				queue_hdmi = true;
+				DRM_DEBUG("IH: HDMI1\n");
 				break;
 			case 2:
-				if (rdev->irq.stat_regs.evergreen.afmt_status3 & AFMT_AZ_FORMAT_WTRIG) {
-					rdev->irq.stat_regs.evergreen.afmt_status3 &= ~AFMT_AZ_FORMAT_WTRIG;
-					queue_hdmi = true;
-					DRM_DEBUG("IH: HDMI2\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.afmt_status3 & AFMT_AZ_FORMAT_WTRIG))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.afmt_status3 &= ~AFMT_AZ_FORMAT_WTRIG;
+				queue_hdmi = true;
+				DRM_DEBUG("IH: HDMI2\n");
 				break;
 			case 3:
-				if (rdev->irq.stat_regs.evergreen.afmt_status4 & AFMT_AZ_FORMAT_WTRIG) {
-					rdev->irq.stat_regs.evergreen.afmt_status4 &= ~AFMT_AZ_FORMAT_WTRIG;
-					queue_hdmi = true;
-					DRM_DEBUG("IH: HDMI3\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.afmt_status4 & AFMT_AZ_FORMAT_WTRIG))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.afmt_status4 &= ~AFMT_AZ_FORMAT_WTRIG;
+				queue_hdmi = true;
+				DRM_DEBUG("IH: HDMI3\n");
 				break;
 			case 4:
-				if (rdev->irq.stat_regs.evergreen.afmt_status5 & AFMT_AZ_FORMAT_WTRIG) {
-					rdev->irq.stat_regs.evergreen.afmt_status5 &= ~AFMT_AZ_FORMAT_WTRIG;
-					queue_hdmi = true;
-					DRM_DEBUG("IH: HDMI4\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.afmt_status5 & AFMT_AZ_FORMAT_WTRIG))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.afmt_status5 &= ~AFMT_AZ_FORMAT_WTRIG;
+				queue_hdmi = true;
+				DRM_DEBUG("IH: HDMI4\n");
 				break;
 			case 5:
-				if (rdev->irq.stat_regs.evergreen.afmt_status6 & AFMT_AZ_FORMAT_WTRIG) {
-					rdev->irq.stat_regs.evergreen.afmt_status6 &= ~AFMT_AZ_FORMAT_WTRIG;
-					queue_hdmi = true;
-					DRM_DEBUG("IH: HDMI5\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.afmt_status6 & AFMT_AZ_FORMAT_WTRIG))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.afmt_status6 &= ~AFMT_AZ_FORMAT_WTRIG;
+				queue_hdmi = true;
+				DRM_DEBUG("IH: HDMI5\n");
 				break;
 			default:
 				DRM_ERROR("Unhandled interrupt: %d %d\n", src_id, src_data);
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index 8e5aeeb058a5..158872eb78e4 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -2162,18 +2162,20 @@ static int cayman_startup(struct radeon_device *rdev)
 			DRM_ERROR("radeon: failed initializing UVD (%d).\n", r);
 	}
 
-	ring = &rdev->ring[TN_RING_TYPE_VCE1_INDEX];
-	if (ring->ring_size)
-		r = radeon_ring_init(rdev, ring, ring->ring_size, 0, 0x0);
+	if (rdev->family == CHIP_ARUBA) {
+		ring = &rdev->ring[TN_RING_TYPE_VCE1_INDEX];
+		if (ring->ring_size)
+			r = radeon_ring_init(rdev, ring, ring->ring_size, 0, 0x0);
 
-	ring = &rdev->ring[TN_RING_TYPE_VCE2_INDEX];
-	if (ring->ring_size)
-		r = radeon_ring_init(rdev, ring, ring->ring_size, 0, 0x0);
+		ring = &rdev->ring[TN_RING_TYPE_VCE2_INDEX];
+		if (ring->ring_size)
+			r = radeon_ring_init(rdev, ring, ring->ring_size, 0, 0x0);
 
-	if (!r)
-		r = vce_v1_0_init(rdev);
-	else if (r != -ENOENT)
-		DRM_ERROR("radeon: failed initializing VCE (%d).\n", r);
+		if (!r)
+			r = vce_v1_0_init(rdev);
+		if (r)
+			DRM_ERROR("radeon: failed initializing VCE (%d).\n", r);
+	}
 
 	r = radeon_ib_pool_init(rdev);
 	if (r) {
@@ -2396,7 +2398,8 @@ void cayman_fini(struct radeon_device *rdev)
 	radeon_irq_kms_fini(rdev);
 	uvd_v1_0_fini(rdev);
 	radeon_uvd_fini(rdev);
-	radeon_vce_fini(rdev);
+	if (rdev->family == CHIP_ARUBA)
+		radeon_vce_fini(rdev);
 	cayman_pcie_gart_fini(rdev);
 	r600_vram_scratch_fini(rdev);
 	radeon_gem_fini(rdev);
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 35dafd77a639..4ea5b10ff5f4 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -4086,23 +4086,27 @@ restart_ih:
 		case 1: /* D1 vblank/vline */
 			switch (src_data) {
 			case 0: /* D1 vblank */
-				if (rdev->irq.stat_regs.r600.disp_int & LB_D1_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[0]) {
-						drm_handle_vblank(rdev->ddev, 0);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[0]))
-						radeon_crtc_handle_vblank(rdev, 0);
-					rdev->irq.stat_regs.r600.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D1 vblank\n");
+				if (!(rdev->irq.stat_regs.r600.disp_int & LB_D1_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: D1 vblank - IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[0]) {
+					drm_handle_vblank(rdev->ddev, 0);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[0]))
+					radeon_crtc_handle_vblank(rdev, 0);
+				rdev->irq.stat_regs.r600.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D1 vblank\n");
+
 				break;
 			case 1: /* D1 vline */
-				if (rdev->irq.stat_regs.r600.disp_int & LB_D1_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.r600.disp_int &= ~LB_D1_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D1 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.disp_int & LB_D1_VLINE_INTERRUPT))
+				    DRM_DEBUG("IH: D1 vline - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.disp_int &= ~LB_D1_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D1 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -4112,23 +4116,27 @@ restart_ih:
 		case 5: /* D2 vblank/vline */
 			switch (src_data) {
 			case 0: /* D2 vblank */
-				if (rdev->irq.stat_regs.r600.disp_int & LB_D2_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[1]) {
-						drm_handle_vblank(rdev->ddev, 1);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[1]))
-						radeon_crtc_handle_vblank(rdev, 1);
-					rdev->irq.stat_regs.r600.disp_int &= ~LB_D2_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D2 vblank\n");
+				if (!(rdev->irq.stat_regs.r600.disp_int & LB_D2_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: D2 vblank - IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[1]) {
+					drm_handle_vblank(rdev->ddev, 1);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[1]))
+					radeon_crtc_handle_vblank(rdev, 1);
+				rdev->irq.stat_regs.r600.disp_int &= ~LB_D2_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D2 vblank\n");
+
 				break;
 			case 1: /* D1 vline */
-				if (rdev->irq.stat_regs.r600.disp_int & LB_D2_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.r600.disp_int &= ~LB_D2_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D2 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.disp_int & LB_D2_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: D2 vline - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.disp_int &= ~LB_D2_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D2 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -4148,46 +4156,53 @@ restart_ih:
 		case 19: /* HPD/DAC hotplug */
 			switch (src_data) {
 			case 0:
-				if (rdev->irq.stat_regs.r600.disp_int & DC_HPD1_INTERRUPT) {
-					rdev->irq.stat_regs.r600.disp_int &= ~DC_HPD1_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD1\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.disp_int & DC_HPD1_INTERRUPT))
+					DRM_DEBUG("IH: HPD1 - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.disp_int &= ~DC_HPD1_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD1\n");
 				break;
 			case 1:
-				if (rdev->irq.stat_regs.r600.disp_int & DC_HPD2_INTERRUPT) {
-					rdev->irq.stat_regs.r600.disp_int &= ~DC_HPD2_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD2\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.disp_int & DC_HPD2_INTERRUPT))
+					DRM_DEBUG("IH: HPD2 - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.disp_int &= ~DC_HPD2_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD2\n");
 				break;
 			case 4:
-				if (rdev->irq.stat_regs.r600.disp_int_cont & DC_HPD3_INTERRUPT) {
-					rdev->irq.stat_regs.r600.disp_int_cont &= ~DC_HPD3_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD3\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.disp_int_cont & DC_HPD3_INTERRUPT))
+					DRM_DEBUG("IH: HPD3 - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.disp_int_cont &= ~DC_HPD3_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD3\n");
 				break;
 			case 5:
-				if (rdev->irq.stat_regs.r600.disp_int_cont & DC_HPD4_INTERRUPT) {
-					rdev->irq.stat_regs.r600.disp_int_cont &= ~DC_HPD4_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD4\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.disp_int_cont & DC_HPD4_INTERRUPT))
+					DRM_DEBUG("IH: HPD4 - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.disp_int_cont &= ~DC_HPD4_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD4\n");
 				break;
 			case 10:
-				if (rdev->irq.stat_regs.r600.disp_int_cont2 & DC_HPD5_INTERRUPT) {
-					rdev->irq.stat_regs.r600.disp_int_cont2 &= ~DC_HPD5_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD5\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.disp_int_cont2 & DC_HPD5_INTERRUPT))
+					DRM_DEBUG("IH: HPD5 - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.disp_int_cont2 &= ~DC_HPD5_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD5\n");
 				break;
 			case 12:
-				if (rdev->irq.stat_regs.r600.disp_int_cont2 & DC_HPD6_INTERRUPT) {
-					rdev->irq.stat_regs.r600.disp_int_cont2 &= ~DC_HPD6_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD6\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.disp_int_cont2 & DC_HPD6_INTERRUPT))
+					DRM_DEBUG("IH: HPD6 - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.disp_int_cont2 &= ~DC_HPD6_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD6\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -4197,18 +4212,22 @@ restart_ih:
 		case 21: /* hdmi */
 			switch (src_data) {
 			case 4:
-				if (rdev->irq.stat_regs.r600.hdmi0_status & HDMI0_AZ_FORMAT_WTRIG) {
-					rdev->irq.stat_regs.r600.hdmi0_status &= ~HDMI0_AZ_FORMAT_WTRIG;
-					queue_hdmi = true;
-					DRM_DEBUG("IH: HDMI0\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.hdmi0_status & HDMI0_AZ_FORMAT_WTRIG))
+					DRM_DEBUG("IH: HDMI0 - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.hdmi0_status &= ~HDMI0_AZ_FORMAT_WTRIG;
+				queue_hdmi = true;
+				DRM_DEBUG("IH: HDMI0\n");
+
 				break;
 			case 5:
-				if (rdev->irq.stat_regs.r600.hdmi1_status & HDMI0_AZ_FORMAT_WTRIG) {
-					rdev->irq.stat_regs.r600.hdmi1_status &= ~HDMI0_AZ_FORMAT_WTRIG;
-					queue_hdmi = true;
-					DRM_DEBUG("IH: HDMI1\n");
-				}
+				if (!(rdev->irq.stat_regs.r600.hdmi1_status & HDMI0_AZ_FORMAT_WTRIG))
+					DRM_DEBUG("IH: HDMI1 - IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.r600.hdmi1_status &= ~HDMI0_AZ_FORMAT_WTRIG;
+				queue_hdmi = true;
+				DRM_DEBUG("IH: HDMI1\n");
+
 				break;
 			default:
 				DRM_ERROR("Unhandled interrupt: %d %d\n", src_id, src_data);
diff --git a/drivers/gpu/drm/radeon/r600_cp.c b/drivers/gpu/drm/radeon/r600_cp.c
index 09e3f39925fa..98f9adaccc3d 100644
--- a/drivers/gpu/drm/radeon/r600_cp.c
+++ b/drivers/gpu/drm/radeon/r600_cp.c
@@ -2483,7 +2483,7 @@ int r600_cp_dispatch_texture(struct drm_device *dev,
 	struct drm_buf *buf;
 	u32 *buffer;
 	const u8 __user *data;
-	int size, pass_size;
+	unsigned int size, pass_size;
 	u64 src_offset, dst_offset;
 
 	if (!radeon_check_offset(dev_priv, tex->offset)) {
diff --git a/drivers/gpu/drm/radeon/radeon_audio.c b/drivers/gpu/drm/radeon/radeon_audio.c
index c89215275053..fa719c53449b 100644
--- a/drivers/gpu/drm/radeon/radeon_audio.c
+++ b/drivers/gpu/drm/radeon/radeon_audio.c
@@ -469,22 +469,22 @@ void radeon_audio_detect(struct drm_connector *connector,
 	dig = radeon_encoder->enc_priv;
 
 	if (status == connector_status_connected) {
-		struct radeon_connector *radeon_connector;
-		int sink_type;
-
 		if (!drm_detect_monitor_audio(radeon_connector_edid(connector))) {
 			radeon_encoder->audio = NULL;
 			return;
 		}
 
-		radeon_connector = to_radeon_connector(connector);
-		sink_type = radeon_dp_getsinktype(radeon_connector);
+		if (connector->connector_type == DRM_MODE_CONNECTOR_DisplayPort) {
+			struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 
-		if (connector->connector_type == DRM_MODE_CONNECTOR_DisplayPort &&
-			sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT)
-			radeon_encoder->audio = rdev->audio.dp_funcs;
-		else
+			if (radeon_dp_getsinktype(radeon_connector) ==
+			    CONNECTOR_OBJECT_ID_DISPLAYPORT)
+				radeon_encoder->audio = rdev->audio.dp_funcs;
+			else
+				radeon_encoder->audio = rdev->audio.hdmi_funcs;
+		} else {
 			radeon_encoder->audio = rdev->audio.hdmi_funcs;
+		}
 
 		dig->afmt->pin = radeon_audio_get_pin(connector->encoder);
 		radeon_audio_enable(rdev, dig->afmt->pin, 0xf);
diff --git a/drivers/gpu/drm/radeon/radeon_cursor.c b/drivers/gpu/drm/radeon/radeon_cursor.c
index 45e54060ee97..afaf346bd50e 100644
--- a/drivers/gpu/drm/radeon/radeon_cursor.c
+++ b/drivers/gpu/drm/radeon/radeon_cursor.c
@@ -91,15 +91,34 @@ static void radeon_show_cursor(struct drm_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(radeon_crtc->cursor_addr));
+		WREG32(EVERGREEN_CUR_SURFACE_ADDRESS + radeon_crtc->crtc_offset,
+		       lower_32_bits(radeon_crtc->cursor_addr));
 		WREG32(RADEON_MM_INDEX, EVERGREEN_CUR_CONTROL + radeon_crtc->crtc_offset);
 		WREG32(RADEON_MM_DATA, EVERGREEN_CURSOR_EN |
 		       EVERGREEN_CURSOR_MODE(EVERGREEN_CURSOR_24_8_PRE_MULT) |
 		       EVERGREEN_CURSOR_URGENT_CONTROL(EVERGREEN_CURSOR_URGENT_1_2));
 	} 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(radeon_crtc->cursor_addr));
+			else
+				WREG32(R700_D1CUR_SURFACE_ADDRESS_HIGH,
+				       upper_32_bits(radeon_crtc->cursor_addr));
+		}
+
+		WREG32(AVIVO_D1CUR_SURFACE_ADDRESS + radeon_crtc->crtc_offset,
+		       lower_32_bits(radeon_crtc->cursor_addr));
 		WREG32(RADEON_MM_INDEX, AVIVO_D1CUR_CONTROL + radeon_crtc->crtc_offset);
 		WREG32(RADEON_MM_DATA, AVIVO_D1CURSOR_EN |
 		       (AVIVO_D1CURSOR_MODE_24BPP << AVIVO_D1CURSOR_MODE_SHIFT));
 	} else {
+		/* offset is from DISP(2)_BASE_ADDRESS */
+		WREG32(RADEON_CUR_OFFSET + radeon_crtc->crtc_offset,
+		       radeon_crtc->cursor_addr - radeon_crtc->legacy_display_base_addr);
+
 		switch (radeon_crtc->crtc_id) {
 		case 0:
 			WREG32(RADEON_MM_INDEX, RADEON_CRTC_GEN_CNTL);
@@ -205,8 +224,9 @@ static int radeon_cursor_move_locked(struct drm_crtc *crtc, int x, int y)
 			| (x << 16)
 			| y));
 		/* offset is from DISP(2)_BASE_ADDRESS */
-		WREG32(RADEON_CUR_OFFSET + radeon_crtc->crtc_offset, (radeon_crtc->legacy_cursor_offset +
-								      (yorigin * 256)));
+		WREG32(RADEON_CUR_OFFSET + radeon_crtc->crtc_offset,
+		       radeon_crtc->cursor_addr - radeon_crtc->legacy_display_base_addr +
+		       yorigin * 256);
 	}
 
 	radeon_crtc->cursor_x = x;
@@ -227,53 +247,6 @@ int radeon_crtc_cursor_move(struct drm_crtc *crtc,
 	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,
@@ -283,7 +256,9 @@ int radeon_crtc_cursor_set2(struct drm_crtc *crtc,
 			    int32_t hot_y)
 {
 	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;
 	int ret;
 
 	if (!handle) {
@@ -305,6 +280,23 @@ int radeon_crtc_cursor_set2(struct drm_crtc *crtc,
 		return -ENOENT;
 	}
 
+	robj = gem_to_radeon_bo(obj);
+	ret = radeon_bo_reserve(robj, false);
+	if (ret != 0) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ret;
+	}
+	/* Only 27 bit offset for legacy cursor */
+	ret = radeon_bo_pin_restricted(robj, RADEON_GEM_DOMAIN_VRAM,
+				       ASIC_IS_AVIVO(rdev) ? 0 : 1 << 27,
+				       &radeon_crtc->cursor_addr);
+	radeon_bo_unreserve(robj);
+	if (ret) {
+		DRM_ERROR("Failed to pin new cursor BO (%d)\n", ret);
+		drm_gem_object_unreference_unlocked(obj);
+		return ret;
+	}
+
 	radeon_crtc->cursor_width = width;
 	radeon_crtc->cursor_height = height;
 
@@ -323,13 +315,7 @@ int radeon_crtc_cursor_set2(struct drm_crtc *crtc,
 		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_show_cursor(crtc);
 
 	radeon_lock_cursor(crtc, false);
 
@@ -341,8 +327,7 @@ unpin:
 			radeon_bo_unpin(robj);
 			radeon_bo_unreserve(robj);
 		}
-		if (radeon_crtc->cursor_bo != obj)
-			drm_gem_object_unreference_unlocked(radeon_crtc->cursor_bo);
+		drm_gem_object_unreference_unlocked(radeon_crtc->cursor_bo);
 	}
 
 	radeon_crtc->cursor_bo = obj;
@@ -360,7 +345,6 @@ unpin:
 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);
@@ -368,12 +352,7 @@ void radeon_cursor_reset(struct drm_crtc *crtc)
 		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_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 13e207e0dff0..d8319dae8358 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -1080,6 +1080,22 @@ static bool radeon_check_pot_argument(int arg)
 }
 
 /**
+ * Determine a sensible default GART size according to ASIC family.
+ *
+ * @family ASIC family name
+ */
+static int radeon_gart_size_auto(enum radeon_family family)
+{
+	/* default to a larger gart size on newer asics */
+	if (family >= CHIP_TAHITI)
+		return 2048;
+	else if (family >= CHIP_RV770)
+		return 1024;
+	else
+		return 512;
+}
+
+/**
  * radeon_check_arguments - validate module params
  *
  * @rdev: radeon_device pointer
@@ -1097,27 +1113,17 @@ static void radeon_check_arguments(struct radeon_device *rdev)
 	}
 
 	if (radeon_gart_size == -1) {
-		/* default to a larger gart size on newer asics */
-		if (rdev->family >= CHIP_RV770)
-			radeon_gart_size = 1024;
-		else
-			radeon_gart_size = 512;
+		radeon_gart_size = radeon_gart_size_auto(rdev->family);
 	}
 	/* gtt size must be power of two and greater or equal to 32M */
 	if (radeon_gart_size < 32) {
 		dev_warn(rdev->dev, "gart size (%d) too small\n",
 				radeon_gart_size);
-		if (rdev->family >= CHIP_RV770)
-			radeon_gart_size = 1024;
-		else
-			radeon_gart_size = 512;
+		radeon_gart_size = radeon_gart_size_auto(rdev->family);
 	} else if (!radeon_check_pot_argument(radeon_gart_size)) {
 		dev_warn(rdev->dev, "gart size (%d) must be a power of 2\n",
 				radeon_gart_size);
-		if (rdev->family >= CHIP_RV770)
-			radeon_gart_size = 1024;
-		else
-			radeon_gart_size = 512;
+		radeon_gart_size = radeon_gart_size_auto(rdev->family);
 	}
 	rdev->mc.gtt_size = (uint64_t)radeon_gart_size << 20;
 
@@ -1463,6 +1469,21 @@ int radeon_device_init(struct radeon_device *rdev,
 	if (r)
 		DRM_ERROR("ib ring test failed (%d).\n", r);
 
+	/*
+	 * Turks/Thames GPU will freeze whole laptop if DPM is not restarted
+	 * after the CP ring have chew one packet at least. Hence here we stop
+	 * and restart DPM after the radeon_ib_ring_tests().
+	 */
+	if (rdev->pm.dpm_enabled &&
+	    (rdev->pm.pm_method == PM_METHOD_DPM) &&
+	    (rdev->family == CHIP_TURKS) &&
+	    (rdev->flags & RADEON_IS_MOBILITY)) {
+		mutex_lock(&rdev->pm.mutex);
+		radeon_dpm_disable(rdev);
+		radeon_dpm_enable(rdev);
+		mutex_unlock(&rdev->pm.mutex);
+	}
+
 	if ((radeon_testing & 1)) {
 		if (rdev->accel_working)
 			radeon_test_moves(rdev);
@@ -1557,11 +1578,21 @@ int radeon_suspend_kms(struct drm_device *dev, bool suspend, bool fbcon)
 		drm_helper_connector_dpms(connector, DRM_MODE_DPMS_OFF);
 	}
 
-	/* unpin the front buffers */
+	/* unpin the front buffers and cursors */
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
 		struct radeon_framebuffer *rfb = to_radeon_framebuffer(crtc->primary->fb);
 		struct radeon_bo *robj;
 
+		if (radeon_crtc->cursor_bo) {
+			struct radeon_bo *robj = gem_to_radeon_bo(radeon_crtc->cursor_bo);
+			r = radeon_bo_reserve(robj, false);
+			if (r == 0) {
+				radeon_bo_unpin(robj);
+				radeon_bo_unreserve(robj);
+			}
+		}
+
 		if (rfb == NULL || rfb->obj == NULL) {
 			continue;
 		}
@@ -1624,6 +1655,7 @@ int radeon_resume_kms(struct drm_device *dev, bool resume, bool fbcon)
 {
 	struct drm_connector *connector;
 	struct radeon_device *rdev = dev->dev_private;
+	struct drm_crtc *crtc;
 	int r;
 
 	if (dev->switch_power_state == DRM_SWITCH_POWER_OFF)
@@ -1663,6 +1695,27 @@ int radeon_resume_kms(struct drm_device *dev, bool resume, bool fbcon)
 
 	radeon_restore_bios_scratch_regs(rdev);
 
+	/* pin cursors */
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+		struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
+
+		if (radeon_crtc->cursor_bo) {
+			struct radeon_bo *robj = gem_to_radeon_bo(radeon_crtc->cursor_bo);
+			r = radeon_bo_reserve(robj, false);
+			if (r == 0) {
+				/* Only 27 bit offset for legacy cursor */
+				r = radeon_bo_pin_restricted(robj,
+							     RADEON_GEM_DOMAIN_VRAM,
+							     ASIC_IS_AVIVO(rdev) ?
+							     0 : 1 << 27,
+							     &radeon_crtc->cursor_addr);
+				if (r != 0)
+					DRM_ERROR("Failed to pin cursor BO (%d)\n", r);
+				radeon_bo_unreserve(robj);
+			}
+		}
+	}
+
 	/* init dig PHYs, disp eng pll */
 	if (rdev->is_atom_bios) {
 		radeon_atom_encoder_init(rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_dp_mst.c b/drivers/gpu/drm/radeon/radeon_dp_mst.c
index 2b98ed3e684d..257b10be5cda 100644
--- a/drivers/gpu/drm/radeon/radeon_dp_mst.c
+++ b/drivers/gpu/drm/radeon/radeon_dp_mst.c
@@ -663,12 +663,17 @@ int
 radeon_dp_mst_probe(struct radeon_connector *radeon_connector)
 {
 	struct radeon_connector_atom_dig *dig_connector = radeon_connector->con_priv;
+	struct drm_device *dev = radeon_connector->base.dev;
+	struct radeon_device *rdev = dev->dev_private;
 	int ret;
 	u8 msg[1];
 
 	if (!radeon_mst)
 		return 0;
 
+	if (!ASIC_IS_DCE5(rdev))
+		return 0;
+
 	if (dig_connector->dpcd[DP_DPCD_REV] < 0x12)
 		return 0;
 
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index ac3c1310b953..013ec7106e55 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -428,7 +428,6 @@ int radeon_gem_mmap_ioctl(struct drm_device *dev, void *data,
 int radeon_gem_busy_ioctl(struct drm_device *dev, void *data,
 			  struct drm_file *filp)
 {
-	struct radeon_device *rdev = dev->dev_private;
 	struct drm_radeon_gem_busy *args = data;
 	struct drm_gem_object *gobj;
 	struct radeon_bo *robj;
@@ -440,10 +439,16 @@ int radeon_gem_busy_ioctl(struct drm_device *dev, void *data,
 		return -ENOENT;
 	}
 	robj = gem_to_radeon_bo(gobj);
-	r = radeon_bo_wait(robj, &cur_placement, true);
+
+	r = reservation_object_test_signaled_rcu(robj->tbo.resv, true);
+	if (r == 0)
+		r = -EBUSY;
+	else
+		r = 0;
+
+	cur_placement = ACCESS_ONCE(robj->tbo.mem.mem_type);
 	args->domain = radeon_mem_type_to_domain(cur_placement);
 	drm_gem_object_unreference_unlocked(gobj);
-	r = radeon_gem_handle_lockup(rdev, r);
 	return r;
 }
 
@@ -471,6 +476,7 @@ int radeon_gem_wait_idle_ioctl(struct drm_device *dev, void *data,
 		r = ret;
 
 	/* Flush HDP cache via MMIO if necessary */
+	cur_placement = ACCESS_ONCE(robj->tbo.mem.mem_type);
 	if (rdev->asic->mmio_hdp_flush &&
 	    radeon_mem_type_to_domain(cur_placement) == RADEON_GEM_DOMAIN_VRAM)
 		robj->rdev->asic->mmio_hdp_flush(rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 9632e886ddc3..4a119c255ba9 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -576,6 +576,9 @@ static int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file
 		if (radeon_get_allowed_info_register(rdev, *value, value))
 			return -EINVAL;
 		break;
+	case RADEON_INFO_VA_UNMAP_WORKING:
+		*value = true;
+		break;
 	case RADEON_INFO_GPU_RESET_COUNTER:
 		*value = atomic_read(&rdev->gpu_reset_counter);
 		break;
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index 6de5459316b5..07909d817381 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -343,7 +343,6 @@ struct radeon_crtc {
 	int max_cursor_width;
 	int max_cursor_height;
 	uint32_t legacy_display_base_addr;
-	uint32_t legacy_cursor_offset;
 	enum radeon_rmx_type rmx_type;
 	u8 h_border;
 	u8 v_border;
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index edafd3c2b170..06ac59fe332a 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -719,7 +719,7 @@ static int radeon_ttm_tt_populate(struct ttm_tt *ttm)
 		return 0;
 
 	if (gtt && gtt->userptr) {
-		ttm->sg = kcalloc(1, sizeof(struct sg_table), GFP_KERNEL);
+		ttm->sg = kzalloc(sizeof(struct sg_table), GFP_KERNEL);
 		if (!ttm->sg)
 			return -ENOMEM;
 
diff --git a/drivers/gpu/drm/radeon/radeon_vm.c b/drivers/gpu/drm/radeon/radeon_vm.c
index 9739ded91b7a..48d97c040f49 100644
--- a/drivers/gpu/drm/radeon/radeon_vm.c
+++ b/drivers/gpu/drm/radeon/radeon_vm.c
@@ -457,14 +457,16 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 		/* make sure object fit at this offset */
 		eoffset = soffset + size;
 		if (soffset >= eoffset) {
-			return -EINVAL;
+			r = -EINVAL;
+			goto error_unreserve;
 		}
 
 		last_pfn = eoffset / RADEON_GPU_PAGE_SIZE;
 		if (last_pfn > rdev->vm_manager.max_pfn) {
 			dev_err(rdev->dev, "va above limit (0x%08X > 0x%08X)\n",
 				last_pfn, rdev->vm_manager.max_pfn);
-			return -EINVAL;
+			r = -EINVAL;
+			goto error_unreserve;
 		}
 
 	} else {
@@ -485,42 +487,41 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 				"(bo %p 0x%010lx 0x%010lx)\n", bo_va->bo,
 				soffset, tmp->bo, tmp->it.start, tmp->it.last);
 			mutex_unlock(&vm->mutex);
-			return -EINVAL;
+			r = -EINVAL;
+			goto error_unreserve;
 		}
 	}
 
 	if (bo_va->it.start || bo_va->it.last) {
-		spin_lock(&vm->status_lock);
-		if (list_empty(&bo_va->vm_status)) {
-			/* add a clone of the bo_va to clear the old address */
-			struct radeon_bo_va *tmp;
-			spin_unlock(&vm->status_lock);
-			tmp = kzalloc(sizeof(struct radeon_bo_va), GFP_KERNEL);
-			if (!tmp) {
-				mutex_unlock(&vm->mutex);
-				return -ENOMEM;
-			}
-			tmp->it.start = bo_va->it.start;
-			tmp->it.last = bo_va->it.last;
-			tmp->vm = vm;
-			tmp->bo = radeon_bo_ref(bo_va->bo);
-			spin_lock(&vm->status_lock);
-			list_add(&tmp->vm_status, &vm->freed);
+		/* add a clone of the bo_va to clear the old address */
+		struct radeon_bo_va *tmp;
+		tmp = kzalloc(sizeof(struct radeon_bo_va), GFP_KERNEL);
+		if (!tmp) {
+			mutex_unlock(&vm->mutex);
+			r = -ENOMEM;
+			goto error_unreserve;
 		}
-		spin_unlock(&vm->status_lock);
+		tmp->it.start = bo_va->it.start;
+		tmp->it.last = bo_va->it.last;
+		tmp->vm = vm;
+		tmp->bo = radeon_bo_ref(bo_va->bo);
 
 		interval_tree_remove(&bo_va->it, &vm->va);
+		spin_lock(&vm->status_lock);
 		bo_va->it.start = 0;
 		bo_va->it.last = 0;
+		list_del_init(&bo_va->vm_status);
+		list_add(&tmp->vm_status, &vm->freed);
+		spin_unlock(&vm->status_lock);
 	}
 
 	if (soffset || eoffset) {
+		spin_lock(&vm->status_lock);
 		bo_va->it.start = soffset;
 		bo_va->it.last = eoffset - 1;
-		interval_tree_insert(&bo_va->it, &vm->va);
-		spin_lock(&vm->status_lock);
 		list_add(&bo_va->vm_status, &vm->cleared);
 		spin_unlock(&vm->status_lock);
+		interval_tree_insert(&bo_va->it, &vm->va);
 	}
 
 	bo_va->flags = flags;
@@ -555,7 +556,6 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 		r = radeon_vm_clear_bo(rdev, pt);
 		if (r) {
 			radeon_bo_unref(&pt);
-			radeon_bo_reserve(bo_va->bo, false);
 			return r;
 		}
 
@@ -575,6 +575,10 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 
 	mutex_unlock(&vm->mutex);
 	return 0;
+
+error_unreserve:
+	radeon_bo_unreserve(bo_va->bo);
+	return r;
 }
 
 /**
@@ -1122,12 +1126,12 @@ void radeon_vm_bo_rmv(struct radeon_device *rdev,
 		interval_tree_remove(&bo_va->it, &vm->va);
 
 	spin_lock(&vm->status_lock);
-	if (list_empty(&bo_va->vm_status)) {
+	list_del(&bo_va->vm_status);
+	if (bo_va->it.start || bo_va->it.last) {
 		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);
-		list_del(&bo_va->vm_status);
 		kfree(bo_va);
 	}
 	spin_unlock(&vm->status_lock);
@@ -1151,7 +1155,8 @@ void radeon_vm_bo_invalidate(struct radeon_device *rdev,
 
 	list_for_each_entry(bo_va, &bo->va, bo_list) {
 		spin_lock(&bo_va->vm->status_lock);
-		if (list_empty(&bo_va->vm_status))
+		if (list_empty(&bo_va->vm_status) &&
+		    (bo_va->it.start || bo_va->it.last))
 			list_add(&bo_va->vm_status, &bo_va->vm->invalidated);
 		spin_unlock(&bo_va->vm->status_lock);
 	}
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 26388b5dd6ed..07037e32dea3 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -6466,23 +6466,27 @@ restart_ih:
 		case 1: /* D1 vblank/vline */
 			switch (src_data) {
 			case 0: /* D1 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int & LB_D1_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[0]) {
-						drm_handle_vblank(rdev->ddev, 0);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[0]))
-						radeon_crtc_handle_vblank(rdev, 0);
-					rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D1 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int & LB_D1_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[0]) {
+					drm_handle_vblank(rdev->ddev, 0);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[0]))
+					radeon_crtc_handle_vblank(rdev, 0);
+				rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D1 vblank\n");
+
 				break;
 			case 1: /* D1 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int & LB_D1_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D1 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int & LB_D1_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int &= ~LB_D1_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D1 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -6492,23 +6496,27 @@ restart_ih:
 		case 2: /* D2 vblank/vline */
 			switch (src_data) {
 			case 0: /* D2 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont & LB_D2_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[1]) {
-						drm_handle_vblank(rdev->ddev, 1);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[1]))
-						radeon_crtc_handle_vblank(rdev, 1);
-					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D2 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont & LB_D2_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[1]) {
+					drm_handle_vblank(rdev->ddev, 1);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[1]))
+					radeon_crtc_handle_vblank(rdev, 1);
+				rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D2 vblank\n");
+
 				break;
 			case 1: /* D2 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont & LB_D2_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D2 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont & LB_D2_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont &= ~LB_D2_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D2 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -6518,23 +6526,27 @@ restart_ih:
 		case 3: /* D3 vblank/vline */
 			switch (src_data) {
 			case 0: /* D3 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont2 & LB_D3_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[2]) {
-						drm_handle_vblank(rdev->ddev, 2);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[2]))
-						radeon_crtc_handle_vblank(rdev, 2);
-					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D3 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont2 & LB_D3_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[2]) {
+					drm_handle_vblank(rdev->ddev, 2);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[2]))
+					radeon_crtc_handle_vblank(rdev, 2);
+				rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D3 vblank\n");
+
 				break;
 			case 1: /* D3 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont2 & LB_D3_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D3 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont2 & LB_D3_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~LB_D3_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D3 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -6544,23 +6556,27 @@ restart_ih:
 		case 4: /* D4 vblank/vline */
 			switch (src_data) {
 			case 0: /* D4 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont3 & LB_D4_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[3]) {
-						drm_handle_vblank(rdev->ddev, 3);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[3]))
-						radeon_crtc_handle_vblank(rdev, 3);
-					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D4 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont3 & LB_D4_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[3]) {
+					drm_handle_vblank(rdev->ddev, 3);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[3]))
+					radeon_crtc_handle_vblank(rdev, 3);
+				rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D4 vblank\n");
+
 				break;
 			case 1: /* D4 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont3 & LB_D4_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D4 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont3 & LB_D4_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~LB_D4_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D4 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -6570,23 +6586,27 @@ restart_ih:
 		case 5: /* D5 vblank/vline */
 			switch (src_data) {
 			case 0: /* D5 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont4 & LB_D5_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[4]) {
-						drm_handle_vblank(rdev->ddev, 4);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[4]))
-						radeon_crtc_handle_vblank(rdev, 4);
-					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D5 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont4 & LB_D5_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[4]) {
+					drm_handle_vblank(rdev->ddev, 4);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[4]))
+					radeon_crtc_handle_vblank(rdev, 4);
+				rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D5 vblank\n");
+
 				break;
 			case 1: /* D5 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont4 & LB_D5_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D5 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont4 & LB_D5_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~LB_D5_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D5 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -6596,23 +6616,27 @@ restart_ih:
 		case 6: /* D6 vblank/vline */
 			switch (src_data) {
 			case 0: /* D6 vblank */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & LB_D6_VBLANK_INTERRUPT) {
-					if (rdev->irq.crtc_vblank_int[5]) {
-						drm_handle_vblank(rdev->ddev, 5);
-						rdev->pm.vblank_sync = true;
-						wake_up(&rdev->irq.vblank_queue);
-					}
-					if (atomic_read(&rdev->irq.pflip[5]))
-						radeon_crtc_handle_vblank(rdev, 5);
-					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VBLANK_INTERRUPT;
-					DRM_DEBUG("IH: D6 vblank\n");
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont5 & LB_D6_VBLANK_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				if (rdev->irq.crtc_vblank_int[5]) {
+					drm_handle_vblank(rdev->ddev, 5);
+					rdev->pm.vblank_sync = true;
+					wake_up(&rdev->irq.vblank_queue);
 				}
+				if (atomic_read(&rdev->irq.pflip[5]))
+					radeon_crtc_handle_vblank(rdev, 5);
+				rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VBLANK_INTERRUPT;
+				DRM_DEBUG("IH: D6 vblank\n");
+
 				break;
 			case 1: /* D6 vline */
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & LB_D6_VLINE_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VLINE_INTERRUPT;
-					DRM_DEBUG("IH: D6 vline\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont5 & LB_D6_VLINE_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~LB_D6_VLINE_INTERRUPT;
+				DRM_DEBUG("IH: D6 vline\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
@@ -6632,88 +6656,112 @@ restart_ih:
 		case 42: /* HPD hotplug */
 			switch (src_data) {
 			case 0:
-				if (rdev->irq.stat_regs.evergreen.disp_int & DC_HPD1_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int &= ~DC_HPD1_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD1\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int & DC_HPD1_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int &= ~DC_HPD1_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD1\n");
+
 				break;
 			case 1:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont & DC_HPD2_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~DC_HPD2_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD2\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont & DC_HPD2_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont &= ~DC_HPD2_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD2\n");
+
 				break;
 			case 2:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont2 & DC_HPD3_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~DC_HPD3_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD3\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont2 & DC_HPD3_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~DC_HPD3_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD3\n");
+
 				break;
 			case 3:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont3 & DC_HPD4_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~DC_HPD4_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD4\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont3 & DC_HPD4_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~DC_HPD4_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD4\n");
+
 				break;
 			case 4:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont4 & DC_HPD5_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~DC_HPD5_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD5\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont4 & DC_HPD5_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~DC_HPD5_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD5\n");
+
 				break;
 			case 5:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~DC_HPD6_INTERRUPT;
-					queue_hotplug = true;
-					DRM_DEBUG("IH: HPD6\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~DC_HPD6_INTERRUPT;
+				queue_hotplug = true;
+				DRM_DEBUG("IH: HPD6\n");
+
 				break;
 			case 6:
-				if (rdev->irq.stat_regs.evergreen.disp_int & DC_HPD1_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int &= ~DC_HPD1_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 1\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int & DC_HPD1_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int &= ~DC_HPD1_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 1\n");
+
 				break;
 			case 7:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont & DC_HPD2_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont &= ~DC_HPD2_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 2\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont & DC_HPD2_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont &= ~DC_HPD2_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 2\n");
+
 				break;
 			case 8:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont2 & DC_HPD3_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~DC_HPD3_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 3\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont2 & DC_HPD3_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont2 &= ~DC_HPD3_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 3\n");
+
 				break;
 			case 9:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont3 & DC_HPD4_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~DC_HPD4_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 4\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont3 & DC_HPD4_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont3 &= ~DC_HPD4_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 4\n");
+
 				break;
 			case 10:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont4 & DC_HPD5_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~DC_HPD5_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 5\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont4 & DC_HPD5_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont4 &= ~DC_HPD5_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 5\n");
+
 				break;
 			case 11:
-				if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_RX_INTERRUPT) {
-					rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~DC_HPD6_RX_INTERRUPT;
-					queue_dp = true;
-					DRM_DEBUG("IH: HPD_RX 6\n");
-				}
+				if (!(rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_RX_INTERRUPT))
+					DRM_DEBUG("IH: IH event w/o asserted irq bit?\n");
+
+				rdev->irq.stat_regs.evergreen.disp_int_cont5 &= ~DC_HPD6_RX_INTERRUPT;
+				queue_dp = true;
+				DRM_DEBUG("IH: HPD_RX 6\n");
+
 				break;
 			default:
 				DRM_DEBUG("Unhandled interrupt: %d %d\n", src_id, src_data);
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_drv.c b/drivers/gpu/drm/rockchip/rockchip_drm_drv.c
index 3962176ee713..01b558fe3695 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_drv.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_drv.c
@@ -21,6 +21,7 @@
 #include <drm/drm_fb_helper.h>
 #include <linux/dma-mapping.h>
 #include <linux/pm_runtime.h>
+#include <linux/module.h>
 #include <linux/of_graph.h>
 #include <linux/component.h>
 
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
index 4557f335a8a5..dc65161d7cad 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
@@ -19,6 +19,7 @@
 #include <drm/drm_plane_helper.h>
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/of.h>
diff --git a/drivers/gpu/drm/vgem/vgem_drv.c b/drivers/gpu/drm/vgem/vgem_drv.c
index 7a207ca547be..6394547cf67a 100644
--- a/drivers/gpu/drm/vgem/vgem_drv.c
+++ b/drivers/gpu/drm/vgem/vgem_drv.c
@@ -328,6 +328,8 @@ static int __init vgem_init(void)
 		goto out;
 	}
 
+	drm_dev_set_unique(vgem_device, "vgem");
+
 	ret  = drm_dev_register(vgem_device, 0);
 
 	if (ret)
diff --git a/drivers/gpu/ipu-v3/ipu-common.c b/drivers/gpu/ipu-v3/ipu-common.c
index 67bab5c36056..6d2f39d36e44 100644
--- a/drivers/gpu/ipu-v3/ipu-common.c
+++ b/drivers/gpu/ipu-v3/ipu-common.c
@@ -1119,10 +1119,9 @@ static int ipu_irq_init(struct ipu_soc *ipu)
 		ct->regs.mask = IPU_INT_CTRL(i / 32);
 	}
 
-	irq_set_chained_handler(ipu->irq_sync, ipu_irq_handler);
-	irq_set_handler_data(ipu->irq_sync, ipu);
-	irq_set_chained_handler(ipu->irq_err, ipu_err_irq_handler);
-	irq_set_handler_data(ipu->irq_err, ipu);
+	irq_set_chained_handler_and_data(ipu->irq_sync, ipu_irq_handler, ipu);
+	irq_set_chained_handler_and_data(ipu->irq_err, ipu_err_irq_handler,
+					 ipu);
 
 	return 0;
 }
@@ -1131,10 +1130,8 @@ static void ipu_irq_exit(struct ipu_soc *ipu)
 {
 	int i, irq;
 
-	irq_set_chained_handler(ipu->irq_err, NULL);
-	irq_set_handler_data(ipu->irq_err, NULL);
-	irq_set_chained_handler(ipu->irq_sync, NULL);
-	irq_set_handler_data(ipu->irq_sync, NULL);
+	irq_set_chained_handler_and_data(ipu->irq_err, NULL, NULL);
+	irq_set_chained_handler_and_data(ipu->irq_sync, NULL, NULL);
 
 	/* TODO: remove irq_domain_generic_chips */
 
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 15338afdf7f9..cc4c6649d195 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -634,7 +634,12 @@ config HID_PLANTRONICS
 	tristate "Plantronics USB HID Driver"
 	depends on HID
 	---help---
-	Provides HID support for Plantronics telephony devices.
+	  Provides HID support for Plantronics USB audio devices.
+	  Correctly maps vendor unique volume up/down HID usages to
+	  KEY_VOLUMEUP and KEY_VOLUMEDOWN events and prevents core mapping
+	  of other vendor unique HID usages to random mouse events.
+
+	  Say M here if you may ever plug in a Plantronics USB audio device.
 
 config HID_PRIMAX
 	tristate "Primax non-fully HID-compliant devices"
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index e4a21dfd7ef3..2f8a41dc3cc8 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -24,7 +24,7 @@ obj-$(CONFIG_HID_A4TECH)	+= hid-a4tech.o
 obj-$(CONFIG_HID_ACRUX)		+= hid-axff.o
 obj-$(CONFIG_HID_APPLE)		+= hid-apple.o
 obj-$(CONFIG_HID_APPLEIR)	+= hid-appleir.o
-obj-$(CONFIG_HID_AUREAL)        += hid-aureal.o
+obj-$(CONFIG_HID_AUREAL)	+= hid-aureal.o
 obj-$(CONFIG_HID_BELKIN)	+= hid-belkin.o
 obj-$(CONFIG_HID_BETOP_FF)	+= hid-betopff.o
 obj-$(CONFIG_HID_CHERRY)	+= hid-cherry.o
@@ -46,12 +46,12 @@ obj-$(CONFIG_HID_ICADE)		+= hid-icade.o
 obj-$(CONFIG_HID_KENSINGTON)	+= hid-kensington.o
 obj-$(CONFIG_HID_KEYTOUCH)	+= hid-keytouch.o
 obj-$(CONFIG_HID_KYE)		+= hid-kye.o
-obj-$(CONFIG_HID_LCPOWER)       += hid-lcpower.o
+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_MAGICMOUSE)	+= hid-magicmouse.o
 obj-$(CONFIG_HID_MICROSOFT)	+= hid-microsoft.o
 obj-$(CONFIG_HID_MONTEREY)	+= hid-monterey.o
 obj-$(CONFIG_HID_MULTITOUCH)	+= hid-multitouch.o
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 722a925795a2..157c62775053 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -706,7 +706,8 @@ static void hid_scan_collection(struct hid_parser *parser, unsigned type)
 
 	if (hid->vendor == USB_VENDOR_ID_MICROSOFT &&
 	    (hid->product == USB_DEVICE_ID_MS_TYPE_COVER_3 ||
-	     hid->product == USB_DEVICE_ID_MS_TYPE_COVER_3_JP) &&
+	     hid->product == USB_DEVICE_ID_MS_TYPE_COVER_3_JP ||
+	     hid->product == USB_DEVICE_ID_MS_POWER_COVER) &&
 	    hid->group == HID_GROUP_MULTITOUCH)
 		hid->group = HID_GROUP_GENERIC;
 
@@ -1061,13 +1062,13 @@ static u32 s32ton(__s32 value, unsigned n)
  * Search linux-kernel and linux-usb-devel archives for "hid-core extract".
  */
 
-static __u32 extract(const struct hid_device *hid, __u8 *report,
+__u32 hid_field_extract(const struct hid_device *hid, __u8 *report,
 		     unsigned offset, unsigned n)
 {
 	u64 x;
 
 	if (n > 32)
-		hid_warn(hid, "extract() called with n (%d) > 32! (%s)\n",
+		hid_warn(hid, "hid_field_extract() called with n (%d) > 32! (%s)\n",
 			 n, current->comm);
 
 	report += offset >> 3;  /* adjust byte index */
@@ -1076,6 +1077,7 @@ static __u32 extract(const struct hid_device *hid, __u8 *report,
 	x = (x >> offset) & ((1ULL << n) - 1);  /* extract bit field */
 	return (u32) x;
 }
+EXPORT_SYMBOL_GPL(hid_field_extract);
 
 /*
  * "implement" : set bits in a little endian bit stream.
@@ -1221,9 +1223,9 @@ static void hid_input_field(struct hid_device *hid, struct hid_field *field,
 	for (n = 0; n < count; n++) {
 
 		value[n] = min < 0 ?
-			snto32(extract(hid, data, offset + n * size, size),
-			       size) :
-			extract(hid, data, offset + n * size, size);
+			snto32(hid_field_extract(hid, data, offset + n * size,
+			       size), size) :
+			hid_field_extract(hid, data, offset + n * size, size);
 
 		/* Ignore report if ErrorRollOver */
 		if (!(field->flags & HID_MAIN_ITEM_VARIABLE) &&
@@ -1851,6 +1853,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_TPKBD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_CUSBKBD) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_CBTKBD) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_TPPRODOCK) },
 #endif
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_MX3000_RECEIVER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER) },
@@ -1901,6 +1904,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ 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_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3_JP) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_POWER_COVER) },
 	{ 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) },
@@ -1959,9 +1963,12 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ 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_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_MOTION_CONTROLLER) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_MOTION_CONTROLLER) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_BDREMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_CONTROLLER) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_CONTROLLER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS4_CONTROLLER) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS4_CONTROLLER) },
@@ -1997,6 +2004,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_JOY_BOX_3_PRO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_DUAL_BOX_PRO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP_LTD, USB_DEVICE_ID_SUPER_JOY_BOX_5_PRO) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PLAYDOTCOM, USB_DEVICE_ID_PLAYDOTCOM_EMS_USBII) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SLIM_TABLET_5_8_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_SLIM_TABLET_12_1_INCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WALTOP, USB_DEVICE_ID_WALTOP_Q_PAD) },
@@ -2265,14 +2273,6 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH, 0x0001) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH, 0x0002) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH, 0x0004) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_4_PHIDGETSERVO_30) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_1_PHIDGETSERVO_30) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_0_0_4_IF_KIT) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_0_16_16_IF_KIT) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_8_8_8_IF_KIT) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_0_8_7_IF_KIT) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_0_8_8_IF_KIT) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_PHIDGET_MOTORCONTROL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GOTOP, USB_DEVICE_ID_SUPER_Q2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GOTOP, USB_DEVICE_ID_GOGOPEN) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GOTOP, USB_DEVICE_ID_PENPOWER) },
@@ -2399,14 +2399,6 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_WTP) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_DPAD) },
 #endif
-	{ HID_USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_LABPRO) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_GOTEMP) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_SKIP) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_CYCLOPS) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_LCSPEC) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_4_PHIDGETSERVO_20) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_1_PHIDGETSERVO_20) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_8_8_4_IF_KIT) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_YEALINK, USB_DEVICE_ID_YEALINK_P1K_P4K_B2K) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_RISO_KAGAKU, USB_DEVICE_ID_RI_KA_WEBMAIL) },
 	{ }
diff --git a/drivers/hid/hid-cypress.c b/drivers/hid/hid-cypress.c
index c4ef3bc726e3..1b764d1745f3 100644
--- a/drivers/hid/hid-cypress.c
+++ b/drivers/hid/hid-cypress.c
@@ -41,13 +41,9 @@ static __u8 *cp_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 
 	for (i = 0; i < *rsize - 4; i++)
 		if (rdesc[i] == 0x29 && rdesc[i + 2] == 0x19) {
-			__u8 tmp;
-
 			rdesc[i] = 0x19;
 			rdesc[i + 2] = 0x29;
-			tmp = rdesc[i + 3];
-			rdesc[i + 3] = rdesc[i + 1];
-			rdesc[i + 1] = tmp;
+			swap(rdesc[i + 3], rdesc[i + 1]);
 		}
 	return rdesc;
 }
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 7ce93d927f62..b04b0820d816 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -227,6 +227,7 @@
 #define USB_DEVICE_ID_CHICONY_TACTICAL_PAD	0x0418
 #define USB_DEVICE_ID_CHICONY_MULTI_TOUCH	0xb19d
 #define USB_DEVICE_ID_CHICONY_WIRELESS	0x0618
+#define USB_DEVICE_ID_CHICONY_PIXART_USB_OPTICAL_MOUSE	0x1053
 #define USB_DEVICE_ID_CHICONY_WIRELESS2	0x1123
 #define USB_DEVICE_ID_CHICONY_AK1D	0x1125
 
@@ -363,16 +364,6 @@
 #define USB_DEVICE_ID_GENERAL_TOUCH_WIN8_PIT_010A 0x010a
 #define USB_DEVICE_ID_GENERAL_TOUCH_WIN8_PIT_E100 0xe100
 
-#define USB_VENDOR_ID_GLAB		0x06c2
-#define USB_DEVICE_ID_4_PHIDGETSERVO_30	0x0038
-#define USB_DEVICE_ID_1_PHIDGETSERVO_30	0x0039
-#define USB_DEVICE_ID_0_0_4_IF_KIT	0x0040
-#define USB_DEVICE_ID_0_16_16_IF_KIT	0x0044
-#define USB_DEVICE_ID_8_8_8_IF_KIT	0x0045
-#define USB_DEVICE_ID_0_8_7_IF_KIT	0x0051
-#define USB_DEVICE_ID_0_8_8_IF_KIT	0x0053
-#define USB_DEVICE_ID_PHIDGET_MOTORCONTROL	0x0058
-
 #define USB_VENDOR_ID_GOODTOUCH		0x1aad
 #define USB_DEVICE_ID_GOODTOUCH_000f	0x000f
 
@@ -586,6 +577,7 @@
 #define USB_DEVICE_ID_LENOVO_TPKBD	0x6009
 #define USB_DEVICE_ID_LENOVO_CUSBKBD	0x6047
 #define USB_DEVICE_ID_LENOVO_CBTKBD	0x6048
+#define USB_DEVICE_ID_LENOVO_TPPRODOCK	0x6067
 
 #define USB_VENDOR_ID_LG		0x1fd2
 #define USB_DEVICE_ID_LG_MULTITOUCH	0x0064
@@ -673,6 +665,7 @@
 #define USB_DEVICE_ID_MS_TYPE_COVER_2    0x07a9
 #define USB_DEVICE_ID_MS_TYPE_COVER_3    0x07dc
 #define USB_DEVICE_ID_MS_TYPE_COVER_3_JP 0x07dd
+#define USB_DEVICE_ID_MS_POWER_COVER     0x07da
 
 #define USB_VENDOR_ID_MOJO		0x8282
 #define USB_DEVICE_ID_RETRO_ADAPTER	0x3201
@@ -852,6 +845,7 @@
 #define USB_DEVICE_ID_SONY_PS3_BDREMOTE		0x0306
 #define USB_DEVICE_ID_SONY_PS3_CONTROLLER	0x0268
 #define USB_DEVICE_ID_SONY_PS4_CONTROLLER	0x05c4
+#define USB_DEVICE_ID_SONY_MOTION_CONTROLLER	0x03d5
 #define USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER	0x042f
 #define USB_DEVICE_ID_SONY_BUZZ_CONTROLLER		0x0002
 #define USB_DEVICE_ID_SONY_WIRELESS_BUZZ_CONTROLLER	0x1000
@@ -958,13 +952,6 @@
 #define USB_DEVICE_ID_VELLEMAN_K8061_FIRST	0x8061
 #define USB_DEVICE_ID_VELLEMAN_K8061_LAST	0x8068
 
-#define USB_VENDOR_ID_VERNIER		0x08f7
-#define USB_DEVICE_ID_VERNIER_LABPRO	0x0001
-#define USB_DEVICE_ID_VERNIER_GOTEMP	0x0002
-#define USB_DEVICE_ID_VERNIER_SKIP	0x0003
-#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
 
@@ -983,9 +970,6 @@
 
 #define USB_VENDOR_ID_WISEGROUP		0x0925
 #define USB_DEVICE_ID_SMARTJOY_PLUS	0x0005
-#define USB_DEVICE_ID_1_PHIDGETSERVO_20	0x8101
-#define USB_DEVICE_ID_4_PHIDGETSERVO_20	0x8104
-#define USB_DEVICE_ID_8_8_4_IF_KIT	0x8201
 #define USB_DEVICE_ID_SUPER_JOY_BOX_3	0x8888
 #define USB_DEVICE_ID_QUAD_USB_JOYPAD	0x8800
 #define USB_DEVICE_ID_DUAL_USB_JOYPAD	0x8866
@@ -1039,4 +1023,11 @@
 #define USB_VENDOR_ID_RISO_KAGAKU	0x1294	/* Riso Kagaku Corp. */
 #define USB_DEVICE_ID_RI_KA_WEBMAIL	0x1320	/* Webmail Notifier */
 
+#define USB_VENDOR_ID_MULTIPLE_1781	0x1781
+#define USB_DEVICE_ID_RAPHNET_4NES4SNES_OLD	0x0a8d
+
+#define USB_VENDOR_ID_DRACAL_RAPHNET	0x289b
+#define USB_DEVICE_ID_RAPHNET_2NES2SNES	0x0002
+#define USB_DEVICE_ID_RAPHNET_4NES4SNES	0x0003
+
 #endif
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 008e89bf6f3c..3511bbaba505 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -1157,7 +1157,8 @@ void hidinput_hid_event(struct hid_device *hid, struct hid_field *field, struct
 		return;
 
 	/* report the usage code as scancode if the key status has changed */
-	if (usage->type == EV_KEY && !!test_bit(usage->code, input->key) != value)
+	if (usage->type == EV_KEY &&
+	    (!test_bit(usage->code, input->key)) == value)
 		input_event(input, EV_MSC, MSC_SCAN, usage->hid);
 
 	input_event(input, usage->type, usage->code, value);
diff --git a/drivers/hid/hid-lenovo.c b/drivers/hid/hid-lenovo.c
index c4c3f0952521..4f59bffd0205 100644
--- a/drivers/hid/hid-lenovo.c
+++ b/drivers/hid/hid-lenovo.c
@@ -43,6 +43,35 @@ struct lenovo_drvdata_cptkbd {
 
 #define map_key_clear(c) hid_map_usage_clear(hi, usage, bit, max, EV_KEY, (c))
 
+static const __u8 lenovo_pro_dock_need_fixup_collection[] = {
+	0x05, 0x88,		/* Usage Page (Vendor Usage Page 0x88)	*/
+	0x09, 0x01,		/* Usage (Vendor Usage 0x01)		*/
+	0xa1, 0x01,		/* Collection (Application)		*/
+	0x85, 0x04,		/*  Report ID (4)			*/
+	0x19, 0x00,		/*  Usage Minimum (0)			*/
+	0x2a, 0xff, 0xff,	/*  Usage Maximum (65535)		*/
+};
+
+static __u8 *lenovo_report_fixup(struct hid_device *hdev, __u8 *rdesc,
+		unsigned int *rsize)
+{
+	switch (hdev->product) {
+	case USB_DEVICE_ID_LENOVO_TPPRODOCK:
+		/* the fixups that need to be done:
+		 *   - get a reasonable usage max for the vendor collection
+		 *     0x8801 from the report ID 4
+		 */
+		if (*rsize >= 153 &&
+		    memcmp(&rdesc[140], lenovo_pro_dock_need_fixup_collection,
+			  sizeof(lenovo_pro_dock_need_fixup_collection)) == 0) {
+			rdesc[151] = 0x01;
+			rdesc[152] = 0x00;
+		}
+		break;
+	}
+	return rdesc;
+}
+
 static int lenovo_input_mapping_tpkbd(struct hid_device *hdev,
 		struct hid_input *hi, struct hid_field *field,
 		struct hid_usage *usage, unsigned long **bit, int *max)
@@ -599,7 +628,8 @@ static int lenovo_probe_tpkbd(struct hid_device *hdev)
 				    GFP_KERNEL);
 	if (data_pointer == NULL) {
 		hid_err(hdev, "Could not allocate memory for driver data\n");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto err;
 	}
 
 	// set same default values as windows driver
@@ -610,7 +640,8 @@ static int lenovo_probe_tpkbd(struct hid_device *hdev)
 	name_micmute = devm_kzalloc(&hdev->dev, name_sz, GFP_KERNEL);
 	if (name_mute == NULL || name_micmute == NULL) {
 		hid_err(hdev, "Could not allocate memory for led data\n");
-		return -ENOMEM;
+		ret = -ENOMEM;
+		goto err;
 	}
 	snprintf(name_mute, name_sz, "%s:amber:mute", dev_name(dev));
 	snprintf(name_micmute, name_sz, "%s:amber:micmute", dev_name(dev));
@@ -634,6 +665,9 @@ static int lenovo_probe_tpkbd(struct hid_device *hdev)
 	lenovo_features_set_tpkbd(hdev);
 
 	return 0;
+err:
+	sysfs_remove_group(&hdev->dev.kobj, &lenovo_attr_group_tpkbd);
+	return ret;
 }
 
 static int lenovo_probe_cptkbd(struct hid_device *hdev)
@@ -762,10 +796,29 @@ static void lenovo_remove(struct hid_device *hdev)
 	hid_hw_stop(hdev);
 }
 
+static void lenovo_input_configured(struct hid_device *hdev,
+		struct hid_input *hi)
+{
+	switch (hdev->product) {
+		case USB_DEVICE_ID_LENOVO_TPKBD:
+		case USB_DEVICE_ID_LENOVO_CUSBKBD:
+		case USB_DEVICE_ID_LENOVO_CBTKBD:
+			if (test_bit(EV_REL, hi->input->evbit)) {
+				/* set only for trackpoint device */
+				__set_bit(INPUT_PROP_POINTER, hi->input->propbit);
+				__set_bit(INPUT_PROP_POINTING_STICK,
+						hi->input->propbit);
+			}
+			break;
+	}
+}
+
+
 static const struct hid_device_id lenovo_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_TPKBD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_CUSBKBD) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_CBTKBD) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LENOVO, USB_DEVICE_ID_LENOVO_TPPRODOCK) },
 	{ }
 };
 
@@ -774,10 +827,12 @@ MODULE_DEVICE_TABLE(hid, lenovo_devices);
 static struct hid_driver lenovo_driver = {
 	.name = "lenovo",
 	.id_table = lenovo_devices,
+	.input_configured = lenovo_input_configured,
 	.input_mapping = lenovo_input_mapping,
 	.probe = lenovo_probe,
 	.remove = lenovo_remove,
 	.raw_event = lenovo_raw_event,
+	.report_fixup = lenovo_report_fixup,
 };
 module_hid_driver(lenovo_driver);
 
diff --git a/drivers/hid/hid-lg.c b/drivers/hid/hid-lg.c
index b86c18e651ed..429340d809b5 100644
--- a/drivers/hid/hid-lg.c
+++ b/drivers/hid/hid-lg.c
@@ -700,7 +700,8 @@ static int lg_probe(struct hid_device *hdev, const struct hid_device_id *id)
 			/* insert a little delay of 10 jiffies ~ 40ms */
 			wait_queue_head_t wait;
 			init_waitqueue_head (&wait);
-			wait_event_interruptible_timeout(wait, 0, 10);
+			wait_event_interruptible_timeout(wait, 0,
+							 msecs_to_jiffies(40));
 
 			/* Select random Address */
 			buf[1] = 0xB2;
@@ -712,13 +713,16 @@ static int lg_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	}
 
 	if (drv_data->quirks & LG_FF)
-		lgff_init(hdev);
-	if (drv_data->quirks & LG_FF2)
-		lg2ff_init(hdev);
-	if (drv_data->quirks & LG_FF3)
-		lg3ff_init(hdev);
-	if (drv_data->quirks & LG_FF4)
-		lg4ff_init(hdev);
+		ret = lgff_init(hdev);
+	else if (drv_data->quirks & LG_FF2)
+		ret = lg2ff_init(hdev);
+	else if (drv_data->quirks & LG_FF3)
+		ret = lg3ff_init(hdev);
+	else if (drv_data->quirks & LG_FF4)
+		ret = lg4ff_init(hdev);
+
+	if (ret)
+		goto err_free;
 
 	return 0;
 err_free:
@@ -731,8 +735,8 @@ static void lg_remove(struct hid_device *hdev)
 	struct lg_drv_data *drv_data = hid_get_drvdata(hdev);
 	if (drv_data->quirks & LG_FF4)
 		lg4ff_deinit(hdev);
-
-	hid_hw_stop(hdev);
+	else
+		hid_hw_stop(hdev);
 	kfree(drv_data);
 }
 
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index 1232210b1cc5..02cec83caac3 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -68,26 +68,32 @@
 #define LG4FF_FFEX_REV_MAJ 0x21
 #define LG4FF_FFEX_REV_MIN 0x00
 
-static void hid_lg4ff_set_range_dfp(struct hid_device *hid, u16 range);
-static void hid_lg4ff_set_range_g25(struct hid_device *hid, u16 range);
-
-struct lg4ff_device_entry {
-	__u32 product_id;
-	__u16 range;
-	__u16 min_range;
-	__u16 max_range;
+static void lg4ff_set_range_dfp(struct hid_device *hid, u16 range);
+static void lg4ff_set_range_g25(struct hid_device *hid, u16 range);
+
+struct lg4ff_wheel_data {
+	const u32 product_id;
+	u16 range;
+	const u16 min_range;
+	const u16 max_range;
 #ifdef CONFIG_LEDS_CLASS
-	__u8  led_state;
+	u8  led_state;
 	struct led_classdev *led[5];
 #endif
-	u32 alternate_modes;
-	const char *real_tag;
-	const char *real_name;
-	u16 real_product_id;
-	struct list_head list;
+	const u32 alternate_modes;
+	const char * const real_tag;
+	const char * const real_name;
+	const u16 real_product_id;
+
 	void (*set_range)(struct hid_device *hid, u16 range);
 };
 
+struct lg4ff_device_entry {
+	spinlock_t report_lock; /* Protect output HID report */
+	struct hid_report *report;
+	struct lg4ff_wheel_data wdata;
+};
+
 static const signed short lg4ff_wheel_effects[] = {
 	FF_CONSTANT,
 	FF_AUTOCENTER,
@@ -95,16 +101,16 @@ static const signed short lg4ff_wheel_effects[] = {
 };
 
 struct lg4ff_wheel {
-	const __u32 product_id;
+	const u32 product_id;
 	const signed short *ff_effects;
-	const __u16 min_range;
-	const __u16 max_range;
+	const u16 min_range;
+	const u16 max_range;
 	void (*set_range)(struct hid_device *hid, u16 range);
 };
 
 struct lg4ff_compat_mode_switch {
-	const __u8 cmd_count;	/* Number of commands to send */
-	const __u8 cmd[];
+	const u8 cmd_count;	/* Number of commands to send */
+	const u8 cmd[];
 };
 
 struct lg4ff_wheel_ident_info {
@@ -134,10 +140,10 @@ struct lg4ff_alternate_mode {
 static const struct lg4ff_wheel lg4ff_devices[] = {
 	{USB_DEVICE_ID_LOGITECH_WHEEL,       lg4ff_wheel_effects, 40, 270, NULL},
 	{USB_DEVICE_ID_LOGITECH_MOMO_WHEEL,  lg4ff_wheel_effects, 40, 270, NULL},
-	{USB_DEVICE_ID_LOGITECH_DFP_WHEEL,   lg4ff_wheel_effects, 40, 900, hid_lg4ff_set_range_dfp},
-	{USB_DEVICE_ID_LOGITECH_G25_WHEEL,   lg4ff_wheel_effects, 40, 900, hid_lg4ff_set_range_g25},
-	{USB_DEVICE_ID_LOGITECH_DFGT_WHEEL,  lg4ff_wheel_effects, 40, 900, hid_lg4ff_set_range_g25},
-	{USB_DEVICE_ID_LOGITECH_G27_WHEEL,   lg4ff_wheel_effects, 40, 900, hid_lg4ff_set_range_g25},
+	{USB_DEVICE_ID_LOGITECH_DFP_WHEEL,   lg4ff_wheel_effects, 40, 900, lg4ff_set_range_dfp},
+	{USB_DEVICE_ID_LOGITECH_G25_WHEEL,   lg4ff_wheel_effects, 40, 900, lg4ff_set_range_g25},
+	{USB_DEVICE_ID_LOGITECH_DFGT_WHEEL,  lg4ff_wheel_effects, 40, 900, lg4ff_set_range_g25},
+	{USB_DEVICE_ID_LOGITECH_G27_WHEEL,   lg4ff_wheel_effects, 40, 900, lg4ff_set_range_g25},
 	{USB_DEVICE_ID_LOGITECH_MOMO_WHEEL2, lg4ff_wheel_effects, 40, 270, NULL},
 	{USB_DEVICE_ID_LOGITECH_WII_WHEEL,   lg4ff_wheel_effects, 40, 270, NULL}
 };
@@ -245,10 +251,10 @@ static const struct lg4ff_compat_mode_switch lg4ff_mode_switch_ext16_g25 = {
 };
 
 /* Recalculates X axis value accordingly to currently selected range */
-static __s32 lg4ff_adjust_dfp_x_axis(__s32 value, __u16 range)
+static s32 lg4ff_adjust_dfp_x_axis(s32 value, u16 range)
 {
-	__u16 max_range;
-	__s32 new_value;
+	u16 max_range;
+	s32 new_value;
 
 	if (range == 900)
 		return value;
@@ -269,21 +275,21 @@ static __s32 lg4ff_adjust_dfp_x_axis(__s32 value, __u16 range)
 }
 
 int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
-			     struct hid_usage *usage, __s32 value, struct lg_drv_data *drv_data)
+			     struct hid_usage *usage, s32 value, struct lg_drv_data *drv_data)
 {
 	struct lg4ff_device_entry *entry = drv_data->device_props;
-	__s32 new_value = 0;
+	s32 new_value = 0;
 
 	if (!entry) {
 		hid_err(hid, "Device properties not found");
 		return 0;
 	}
 
-	switch (entry->product_id) {
+	switch (entry->wdata.product_id) {
 	case USB_DEVICE_ID_LOGITECH_DFP_WHEEL:
 		switch (usage->code) {
 		case ABS_X:
-			new_value = lg4ff_adjust_dfp_x_axis(value, entry->range);
+			new_value = lg4ff_adjust_dfp_x_axis(value, entry->wdata.range);
 			input_event(field->hidinput->input, usage->type, usage->code, new_value);
 			return 1;
 		default:
@@ -294,14 +300,56 @@ int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
 	}
 }
 
-static int hid_lg4ff_play(struct input_dev *dev, void *data, struct ff_effect *effect)
+static void lg4ff_init_wheel_data(struct lg4ff_wheel_data * const wdata, const struct lg4ff_wheel *wheel,
+				  const struct lg4ff_multimode_wheel *mmode_wheel,
+				  const u16 real_product_id)
+{
+	u32 alternate_modes = 0;
+	const char *real_tag = NULL;
+	const char *real_name = NULL;
+
+	if (mmode_wheel) {
+		alternate_modes = mmode_wheel->alternate_modes;
+		real_tag = mmode_wheel->real_tag;
+		real_name = mmode_wheel->real_name;
+	}
+
+	{
+		struct lg4ff_wheel_data t_wdata =  { .product_id = wheel->product_id,
+						     .real_product_id = real_product_id,
+						     .min_range = wheel->min_range,
+						     .max_range = wheel->max_range,
+						     .set_range = wheel->set_range,
+						     .alternate_modes = alternate_modes,
+						     .real_tag = real_tag,
+						     .real_name = real_name };
+
+		memcpy(wdata, &t_wdata, sizeof(t_wdata));
+	}
+}
+
+static int lg4ff_play(struct input_dev *dev, void *data, struct ff_effect *effect)
 {
 	struct hid_device *hid = input_get_drvdata(dev);
-	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
-	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
-	__s32 *value = report->field[0]->value;
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	unsigned long flags;
+	s32 *value;
 	int x;
 
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return -EINVAL;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return -EINVAL;
+	}
+	value = entry->report->field[0]->value;
+
 #define CLAMP(x) do { if (x < 0) x = 0; else if (x > 0xff) x = 0xff; } while (0)
 
 	switch (effect->type) {
@@ -309,6 +357,7 @@ static int hid_lg4ff_play(struct input_dev *dev, void *data, struct ff_effect *e
 		x = effect->u.ramp.start_level + 0x80;	/* 0x80 is no force */
 		CLAMP(x);
 
+		spin_lock_irqsave(&entry->report_lock, flags);
 		if (x == 0x80) {
 			/* De-activate force in slot-1*/
 			value[0] = 0x13;
@@ -319,7 +368,8 @@ static int hid_lg4ff_play(struct input_dev *dev, void *data, struct ff_effect *e
 			value[5] = 0x00;
 			value[6] = 0x00;
 
-			hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+			hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+			spin_unlock_irqrestore(&entry->report_lock, flags);
 			return 0;
 		}
 
@@ -331,7 +381,8 @@ static int hid_lg4ff_play(struct input_dev *dev, void *data, struct ff_effect *e
 		value[5] = 0x00;
 		value[6] = 0x00;
 
-		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+		hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+		spin_unlock_irqrestore(&entry->report_lock, flags);
 		break;
 	}
 	return 0;
@@ -339,15 +390,16 @@ static int hid_lg4ff_play(struct input_dev *dev, void *data, struct ff_effect *e
 
 /* Sends default autocentering command compatible with
  * all wheels except Formula Force EX */
-static void hid_lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitude)
+static void lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitude)
 {
 	struct hid_device *hid = input_get_drvdata(dev);
 	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
 	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
-	__s32 *value = report->field[0]->value;
-	__u32 expand_a, expand_b;
+	s32 *value = report->field[0]->value;
+	u32 expand_a, expand_b;
 	struct lg4ff_device_entry *entry;
 	struct lg_drv_data *drv_data;
+	unsigned long flags;
 
 	drv_data = hid_get_drvdata(hid);
 	if (!drv_data) {
@@ -360,8 +412,10 @@ static void hid_lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitud
 		hid_err(hid, "Device properties not found!\n");
 		return;
 	}
+	value = entry->report->field[0]->value;
 
 	/* De-activate Auto-Center */
+	spin_lock_irqsave(&entry->report_lock, flags);
 	if (magnitude == 0) {
 		value[0] = 0xf5;
 		value[1] = 0x00;
@@ -371,7 +425,8 @@ static void hid_lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitud
 		value[5] = 0x00;
 		value[6] = 0x00;
 
-		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+		hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+		spin_unlock_irqrestore(&entry->report_lock, flags);
 		return;
 	}
 
@@ -384,7 +439,7 @@ static void hid_lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitud
 	}
 
 	/* Adjust for non-MOMO wheels */
-	switch (entry->product_id) {
+	switch (entry->wdata.product_id) {
 	case USB_DEVICE_ID_LOGITECH_MOMO_WHEEL:
 	case USB_DEVICE_ID_LOGITECH_MOMO_WHEEL2:
 		break;
@@ -401,7 +456,7 @@ static void hid_lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitud
 	value[5] = 0x00;
 	value[6] = 0x00;
 
-	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+	hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
 
 	/* Activate Auto-Center */
 	value[0] = 0x14;
@@ -412,18 +467,34 @@ static void hid_lg4ff_set_autocenter_default(struct input_dev *dev, u16 magnitud
 	value[5] = 0x00;
 	value[6] = 0x00;
 
-	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+	hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+	spin_unlock_irqrestore(&entry->report_lock, flags);
 }
 
 /* Sends autocentering command compatible with Formula Force EX */
-static void hid_lg4ff_set_autocenter_ffex(struct input_dev *dev, u16 magnitude)
+static void lg4ff_set_autocenter_ffex(struct input_dev *dev, u16 magnitude)
 {
 	struct hid_device *hid = input_get_drvdata(dev);
-	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
-	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
-	__s32 *value = report->field[0]->value;
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	unsigned long flags;
+	s32 *value;
 	magnitude = magnitude * 90 / 65535;
 
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return;
+	}
+	value = entry->report->field[0]->value;
+
+	spin_lock_irqsave(&entry->report_lock, flags);
 	value[0] = 0xfe;
 	value[1] = 0x03;
 	value[2] = magnitude >> 14;
@@ -432,18 +503,33 @@ static void hid_lg4ff_set_autocenter_ffex(struct input_dev *dev, u16 magnitude)
 	value[5] = 0x00;
 	value[6] = 0x00;
 
-	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+	hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+	spin_unlock_irqrestore(&entry->report_lock, flags);
 }
 
 /* Sends command to set range compatible with G25/G27/Driving Force GT */
-static void hid_lg4ff_set_range_g25(struct hid_device *hid, u16 range)
+static void lg4ff_set_range_g25(struct hid_device *hid, u16 range)
 {
-	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
-	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
-	__s32 *value = report->field[0]->value;
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	unsigned long flags;
+	s32 *value;
 
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return;
+	}
+	value = entry->report->field[0]->value;
 	dbg_hid("G25/G27/DFGT: setting range to %u\n", range);
 
+	spin_lock_irqsave(&entry->report_lock, flags);
 	value[0] = 0xf8;
 	value[1] = 0x81;
 	value[2] = range & 0x00ff;
@@ -452,20 +538,35 @@ static void hid_lg4ff_set_range_g25(struct hid_device *hid, u16 range)
 	value[5] = 0x00;
 	value[6] = 0x00;
 
-	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+	hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+	spin_unlock_irqrestore(&entry->report_lock, flags);
 }
 
 /* Sends commands to set range compatible with Driving Force Pro wheel */
-static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
+static void lg4ff_set_range_dfp(struct hid_device *hid, u16 range)
 {
-	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
-	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	unsigned long flags;
 	int start_left, start_right, full_range;
-	__s32 *value = report->field[0]->value;
+	s32 *value;
+
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return;
+	}
 
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return;
+	}
+	value = entry->report->field[0]->value;
 	dbg_hid("Driving Force Pro: setting range to %u\n", range);
 
 	/* Prepare "coarse" limit command */
+	spin_lock_irqsave(&entry->report_lock, flags);
 	value[0] = 0xf8;
 	value[1] = 0x00;	/* Set later */
 	value[2] = 0x00;
@@ -475,13 +576,13 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	value[6] = 0x00;
 
 	if (range > 200) {
-		report->field[0]->value[1] = 0x03;
+		value[1] = 0x03;
 		full_range = 900;
 	} else {
-		report->field[0]->value[1] = 0x02;
+		value[1] = 0x02;
 		full_range = 200;
 	}
-	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+	hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
 
 	/* Prepare "fine" limit command */
 	value[0] = 0x81;
@@ -493,7 +594,8 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	value[6] = 0x00;
 
 	if (range == 200 || range == 900) {	/* Do not apply any fine limit */
-		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+		hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+		spin_unlock_irqrestore(&entry->report_lock, flags);
 		return;
 	}
 
@@ -507,7 +609,8 @@ static void hid_lg4ff_set_range_dfp(struct hid_device *hid, __u16 range)
 	value[5] = (start_right & 0xe) << 4 | (start_left & 0xe);
 	value[6] = 0xff;
 
-	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+	hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+	spin_unlock_irqrestore(&entry->report_lock, flags);
 }
 
 static const struct lg4ff_compat_mode_switch *lg4ff_get_mode_switch_command(const u16 real_product_id, const u16 target_product_id)
@@ -569,19 +672,35 @@ static const struct lg4ff_compat_mode_switch *lg4ff_get_mode_switch_command(cons
 
 static int lg4ff_switch_compatibility_mode(struct hid_device *hid, const struct lg4ff_compat_mode_switch *s)
 {
-	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
-	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
-	__s32 *value = report->field[0]->value;
+	struct lg4ff_device_entry *entry;
+	struct lg_drv_data *drv_data;
+	unsigned long flags;
+	s32 *value;
 	u8 i;
 
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return -EINVAL;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return -EINVAL;
+	}
+	value = entry->report->field[0]->value;
+
+	spin_lock_irqsave(&entry->report_lock, flags);
 	for (i = 0; i < s->cmd_count; i++) {
 		u8 j;
 
 		for (j = 0; j < 7; j++)
 			value[j] = s->cmd[j + (7*i)];
 
-		hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+		hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
 	}
+	spin_unlock_irqrestore(&entry->report_lock, flags);
 	hid_hw_wait(hid);
 	return 0;
 }
@@ -606,23 +725,23 @@ static ssize_t lg4ff_alternate_modes_show(struct device *dev, struct device_attr
 		return 0;
 	}
 
-	if (!entry->real_name) {
+	if (!entry->wdata.real_name) {
 		hid_err(hid, "NULL pointer to string\n");
 		return 0;
 	}
 
 	for (i = 0; i < LG4FF_MODE_MAX_IDX; i++) {
-		if (entry->alternate_modes & BIT(i)) {
+		if (entry->wdata.alternate_modes & BIT(i)) {
 			/* Print tag and full name */
 			count += scnprintf(buf + count, PAGE_SIZE - count, "%s: %s",
 					   lg4ff_alternate_modes[i].tag,
-					   !lg4ff_alternate_modes[i].product_id ? entry->real_name : lg4ff_alternate_modes[i].name);
+					   !lg4ff_alternate_modes[i].product_id ? entry->wdata.real_name : lg4ff_alternate_modes[i].name);
 			if (count >= PAGE_SIZE - 1)
 				return count;
 
 			/* Mark the currently active mode with an asterisk */
-			if (lg4ff_alternate_modes[i].product_id == entry->product_id ||
-			    (lg4ff_alternate_modes[i].product_id == 0 && entry->product_id == entry->real_product_id))
+			if (lg4ff_alternate_modes[i].product_id == entry->wdata.product_id ||
+			    (lg4ff_alternate_modes[i].product_id == 0 && entry->wdata.product_id == entry->wdata.real_product_id))
 				count += scnprintf(buf + count, PAGE_SIZE - count, " *\n");
 			else
 				count += scnprintf(buf + count, PAGE_SIZE - count, "\n");
@@ -675,10 +794,10 @@ static ssize_t lg4ff_alternate_modes_store(struct device *dev, struct device_att
 		const u16 mode_product_id = lg4ff_alternate_modes[i].product_id;
 		const char *tag = lg4ff_alternate_modes[i].tag;
 
-		if (entry->alternate_modes & BIT(i)) {
+		if (entry->wdata.alternate_modes & BIT(i)) {
 			if (!strcmp(tag, lbuf)) {
 				if (!mode_product_id)
-					target_product_id = entry->real_product_id;
+					target_product_id = entry->wdata.real_product_id;
 				else
 					target_product_id = mode_product_id;
 				break;
@@ -693,24 +812,24 @@ static ssize_t lg4ff_alternate_modes_store(struct device *dev, struct device_att
 	}
 	kfree(lbuf); /* Not needed anymore */
 
-	if (target_product_id == entry->product_id) /* Nothing to do */
+	if (target_product_id == entry->wdata.product_id) /* Nothing to do */
 		return count;
 
 	/* Automatic switching has to be disabled for the switch to DF-EX mode to work correctly */
 	if (target_product_id == USB_DEVICE_ID_LOGITECH_WHEEL && !lg4ff_no_autoswitch) {
 		hid_info(hid, "\"%s\" cannot be switched to \"DF-EX\" mode. Load the \"hid_logitech\" module with \"lg4ff_no_autoswitch=1\" parameter set and try again\n",
-			 entry->real_name);
+			 entry->wdata.real_name);
 		return -EINVAL;
 	}
 
 	/* Take care of hardware limitations */
-	if ((entry->real_product_id == USB_DEVICE_ID_LOGITECH_DFP_WHEEL || entry->real_product_id == USB_DEVICE_ID_LOGITECH_G25_WHEEL) &&
-	    entry->product_id > target_product_id) {
-		hid_info(hid, "\"%s\" cannot be switched back into \"%s\" mode\n", entry->real_name, lg4ff_alternate_modes[i].name);
+	if ((entry->wdata.real_product_id == USB_DEVICE_ID_LOGITECH_DFP_WHEEL || entry->wdata.real_product_id == USB_DEVICE_ID_LOGITECH_G25_WHEEL) &&
+	    entry->wdata.product_id > target_product_id) {
+		hid_info(hid, "\"%s\" cannot be switched back into \"%s\" mode\n", entry->wdata.real_name, lg4ff_alternate_modes[i].name);
 		return -EINVAL;
 	}
 
-	s = lg4ff_get_mode_switch_command(entry->real_product_id, target_product_id);
+	s = lg4ff_get_mode_switch_command(entry->wdata.real_product_id, target_product_id);
 	if (!s) {
 		hid_err(hid, "Invalid target product ID %X\n", target_product_id);
 		return -EINVAL;
@@ -721,9 +840,9 @@ static ssize_t lg4ff_alternate_modes_store(struct device *dev, struct device_att
 }
 static DEVICE_ATTR(alternate_modes, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH, lg4ff_alternate_modes_show, lg4ff_alternate_modes_store);
 
-/* Read current range and display it in terminal */
-static ssize_t range_show(struct device *dev, struct device_attribute *attr,
-			  char *buf)
+/* Export the currently set range of the wheel */
+static ssize_t lg4ff_range_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
 {
 	struct hid_device *hid = to_hid_device(dev);
 	struct lg4ff_device_entry *entry;
@@ -742,19 +861,19 @@ static ssize_t range_show(struct device *dev, struct device_attribute *attr,
 		return 0;
 	}
 
-	count = scnprintf(buf, PAGE_SIZE, "%u\n", entry->range);
+	count = scnprintf(buf, PAGE_SIZE, "%u\n", entry->wdata.range);
 	return count;
 }
 
 /* Set range to user specified value, call appropriate function
  * according to the type of the wheel */
-static ssize_t range_store(struct device *dev, struct device_attribute *attr,
-			   const char *buf, size_t count)
+static ssize_t lg4ff_range_store(struct device *dev, struct device_attribute *attr,
+				 const char *buf, size_t count)
 {
 	struct hid_device *hid = to_hid_device(dev);
 	struct lg4ff_device_entry *entry;
 	struct lg_drv_data *drv_data;
-	__u16 range = simple_strtoul(buf, NULL, 10);
+	u16 range = simple_strtoul(buf, NULL, 10);
 
 	drv_data = hid_get_drvdata(hid);
 	if (!drv_data) {
@@ -769,18 +888,18 @@ static ssize_t range_store(struct device *dev, struct device_attribute *attr,
 	}
 
 	if (range == 0)
-		range = entry->max_range;
+		range = entry->wdata.max_range;
 
 	/* Check if the wheel supports range setting
 	 * and that the range is within limits for the wheel */
-	if (entry->set_range != NULL && range >= entry->min_range && range <= entry->max_range) {
-		entry->set_range(hid, range);
-		entry->range = range;
+	if (entry->wdata.set_range && range >= entry->wdata.min_range && range <= entry->wdata.max_range) {
+		entry->wdata.set_range(hid, range);
+		entry->wdata.range = range;
 	}
 
 	return count;
 }
-static DEVICE_ATTR_RW(range);
+static DEVICE_ATTR(range, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH, lg4ff_range_show, lg4ff_range_store);
 
 static ssize_t lg4ff_real_id_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
@@ -801,12 +920,12 @@ static ssize_t lg4ff_real_id_show(struct device *dev, struct device_attribute *a
 		return 0;
 	}
 
-	if (!entry->real_tag || !entry->real_name) {
+	if (!entry->wdata.real_tag || !entry->wdata.real_name) {
 		hid_err(hid, "NULL pointer to string\n");
 		return 0;
 	}
 
-	count = scnprintf(buf, PAGE_SIZE, "%s: %s\n", entry->real_tag, entry->real_name);
+	count = scnprintf(buf, PAGE_SIZE, "%s: %s\n", entry->wdata.real_tag, entry->wdata.real_name);
 	return count;
 }
 
@@ -818,12 +937,27 @@ static ssize_t lg4ff_real_id_store(struct device *dev, struct device_attribute *
 static DEVICE_ATTR(real_id, S_IRUGO, lg4ff_real_id_show, lg4ff_real_id_store);
 
 #ifdef CONFIG_LEDS_CLASS
-static void lg4ff_set_leds(struct hid_device *hid, __u8 leds)
+static void lg4ff_set_leds(struct hid_device *hid, u8 leds)
 {
-	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
-	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
-	__s32 *value = report->field[0]->value;
+	struct lg_drv_data *drv_data;
+	struct lg4ff_device_entry *entry;
+	unsigned long flags;
+	s32 *value;
+
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Private driver data not found!\n");
+		return;
+	}
+
+	entry = drv_data->device_props;
+	if (!entry) {
+		hid_err(hid, "Device properties not found!\n");
+		return;
+	}
+	value = entry->report->field[0]->value;
 
+	spin_lock_irqsave(&entry->report_lock, flags);
 	value[0] = 0xf8;
 	value[1] = 0x12;
 	value[2] = leds;
@@ -831,7 +965,8 @@ static void lg4ff_set_leds(struct hid_device *hid, __u8 leds)
 	value[4] = 0x00;
 	value[5] = 0x00;
 	value[6] = 0x00;
-	hid_hw_request(hid, report, HID_REQ_SET_REPORT);
+	hid_hw_request(hid, entry->report, HID_REQ_SET_REPORT);
+	spin_unlock_irqrestore(&entry->report_lock, flags);
 }
 
 static void lg4ff_led_set_brightness(struct led_classdev *led_cdev,
@@ -848,7 +983,7 @@ static void lg4ff_led_set_brightness(struct led_classdev *led_cdev,
 		return;
 	}
 
-	entry = (struct lg4ff_device_entry *)drv_data->device_props;
+	entry = drv_data->device_props;
 
 	if (!entry) {
 		hid_err(hid, "Device properties not found.");
@@ -856,15 +991,15 @@ static void lg4ff_led_set_brightness(struct led_classdev *led_cdev,
 	}
 
 	for (i = 0; i < 5; i++) {
-		if (led_cdev != entry->led[i])
+		if (led_cdev != entry->wdata.led[i])
 			continue;
-		state = (entry->led_state >> i) & 1;
+		state = (entry->wdata.led_state >> i) & 1;
 		if (value == LED_OFF && state) {
-			entry->led_state &= ~(1 << i);
-			lg4ff_set_leds(hid, entry->led_state);
+			entry->wdata.led_state &= ~(1 << i);
+			lg4ff_set_leds(hid, entry->wdata.led_state);
 		} else if (value != LED_OFF && !state) {
-			entry->led_state |= 1 << i;
-			lg4ff_set_leds(hid, entry->led_state);
+			entry->wdata.led_state |= 1 << i;
+			lg4ff_set_leds(hid, entry->wdata.led_state);
 		}
 		break;
 	}
@@ -883,7 +1018,7 @@ static enum led_brightness lg4ff_led_get_brightness(struct led_classdev *led_cde
 		return LED_OFF;
 	}
 
-	entry = (struct lg4ff_device_entry *)drv_data->device_props;
+	entry = drv_data->device_props;
 
 	if (!entry) {
 		hid_err(hid, "Device properties not found.");
@@ -891,8 +1026,8 @@ static enum led_brightness lg4ff_led_get_brightness(struct led_classdev *led_cde
 	}
 
 	for (i = 0; i < 5; i++)
-		if (led_cdev == entry->led[i]) {
-			value = (entry->led_state >> i) & 1;
+		if (led_cdev == entry->wdata.led[i]) {
+			value = (entry->wdata.led_state >> i) & 1;
 			break;
 		}
 
@@ -991,8 +1126,11 @@ int lg4ff_init(struct hid_device *hid)
 {
 	struct hid_input *hidinput = list_entry(hid->inputs.next, struct hid_input, list);
 	struct input_dev *dev = hidinput->input;
+	struct list_head *report_list = &hid->report_enum[HID_OUTPUT_REPORT].report_list;
+	struct hid_report *report = list_entry(report_list->next, struct hid_report, list);
 	const struct usb_device_descriptor *udesc = &(hid_to_usb_dev(hid)->descriptor);
 	const u16 bcdDevice = le16_to_cpu(udesc->bcdDevice);
+	const struct lg4ff_multimode_wheel *mmode_wheel = NULL;
 	struct lg4ff_device_entry *entry;
 	struct lg_drv_data *drv_data;
 	int error, i, j;
@@ -1003,6 +1141,18 @@ int lg4ff_init(struct hid_device *hid)
 	if (!hid_validate_values(hid, HID_OUTPUT_REPORT, 0, 0, 7))
 		return -1;
 
+	drv_data = hid_get_drvdata(hid);
+	if (!drv_data) {
+		hid_err(hid, "Cannot add device, private driver data not allocated\n");
+		return -1;
+	}
+	entry = kzalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+	spin_lock_init(&entry->report_lock);
+	entry->report = report;
+	drv_data->device_props = entry;
+
 	/* Check if a multimode wheel has been connected and
 	 * handle it appropriately */
 	mmode_ret = lg4ff_handle_multimode_wheel(hid, &real_product_id, bcdDevice);
@@ -1012,6 +1162,11 @@ int lg4ff_init(struct hid_device *hid)
 	 */
 	if (mmode_ret == LG4FF_MMODE_SWITCHED)
 		return 0;
+	else if (mmode_ret < 0) {
+		hid_err(hid, "Unable to switch device mode during initialization, errno %d\n", mmode_ret);
+		error = mmode_ret;
+		goto err_init;
+	}
 
 	/* Check what wheel has been connected */
 	for (i = 0; i < ARRAY_SIZE(lg4ff_devices); i++) {
@@ -1022,9 +1177,11 @@ int lg4ff_init(struct hid_device *hid)
 	}
 
 	if (i == ARRAY_SIZE(lg4ff_devices)) {
-		hid_err(hid, "Device is not supported by lg4ff driver. If you think it should be, consider reporting a bug to"
-			     "LKML, Simon Wood <simon@mungewell.org> or Michal Maly <madcatxster@gmail.com>\n");
-		return -1;
+		hid_err(hid, "This device is flagged to be handled by the lg4ff module but this module does not know how to handle it. "
+			     "Please report this as a bug to LKML, Simon Wood <simon@mungewell.org> or "
+			     "Michal Maly <madcatxster@devoid-pointer.net>\n");
+		error = -1;
+		goto err_init;
 	}
 
 	if (mmode_ret == LG4FF_MMODE_IS_MULTIMODE) {
@@ -1035,7 +1192,8 @@ int lg4ff_init(struct hid_device *hid)
 
 		if (mmode_idx == ARRAY_SIZE(lg4ff_multimode_wheels)) {
 			hid_err(hid, "Device product ID %X is not listed as a multimode wheel", real_product_id);
-			return -1;
+			error = -1;
+			goto err_init;
 		}
 	}
 
@@ -1043,37 +1201,17 @@ int lg4ff_init(struct hid_device *hid)
 	for (j = 0; lg4ff_devices[i].ff_effects[j] >= 0; j++)
 		set_bit(lg4ff_devices[i].ff_effects[j], dev->ffbit);
 
-	error = input_ff_create_memless(dev, NULL, hid_lg4ff_play);
+	error = input_ff_create_memless(dev, NULL, lg4ff_play);
 
 	if (error)
-		return error;
-
-	/* Get private driver data */
-	drv_data = hid_get_drvdata(hid);
-	if (!drv_data) {
-		hid_err(hid, "Cannot add device, private driver data not allocated\n");
-		return -1;
-	}
+		goto err_init;
 
 	/* Initialize device properties */
-	entry = kzalloc(sizeof(struct lg4ff_device_entry), GFP_KERNEL);
-	if (!entry) {
-		hid_err(hid, "Cannot add device, insufficient memory to allocate device properties.\n");
-		return -ENOMEM;
-	}
-	drv_data->device_props = entry;
-
-	entry->product_id = lg4ff_devices[i].product_id;
-	entry->real_product_id = real_product_id;
-	entry->min_range = lg4ff_devices[i].min_range;
-	entry->max_range = lg4ff_devices[i].max_range;
-	entry->set_range = lg4ff_devices[i].set_range;
 	if (mmode_ret == LG4FF_MMODE_IS_MULTIMODE) {
 		BUG_ON(mmode_idx == -1);
-		entry->alternate_modes = lg4ff_multimode_wheels[mmode_idx].alternate_modes;
-		entry->real_tag = lg4ff_multimode_wheels[mmode_idx].real_tag;
-		entry->real_name = lg4ff_multimode_wheels[mmode_idx].real_name;
+		mmode_wheel = &lg4ff_multimode_wheels[mmode_idx];
 	}
+	lg4ff_init_wheel_data(&entry->wdata, &lg4ff_devices[i], mmode_wheel, real_product_id);
 
 	/* Check if autocentering is available and
 	 * set the centering force to zero by default */
@@ -1081,9 +1219,9 @@ int lg4ff_init(struct hid_device *hid)
 		/* Formula Force EX expects different autocentering command */
 		if ((bcdDevice >> 8) == LG4FF_FFEX_REV_MAJ &&
 		    (bcdDevice & 0xff) == LG4FF_FFEX_REV_MIN)
-			dev->ff->set_autocenter = hid_lg4ff_set_autocenter_ffex;
+			dev->ff->set_autocenter = lg4ff_set_autocenter_ffex;
 		else
-			dev->ff->set_autocenter = hid_lg4ff_set_autocenter_default;
+			dev->ff->set_autocenter = lg4ff_set_autocenter_default;
 
 		dev->ff->set_autocenter(dev, 0);
 	}
@@ -1091,27 +1229,27 @@ int lg4ff_init(struct hid_device *hid)
 	/* Create sysfs interface */
 	error = device_create_file(&hid->dev, &dev_attr_range);
 	if (error)
-		return error;
+		hid_warn(hid, "Unable to create sysfs interface for \"range\", errno %d\n", error);
 	if (mmode_ret == LG4FF_MMODE_IS_MULTIMODE) {
 		error = device_create_file(&hid->dev, &dev_attr_real_id);
 		if (error)
-			return error;
+			hid_warn(hid, "Unable to create sysfs interface for \"real_id\", errno %d\n", error);
 		error = device_create_file(&hid->dev, &dev_attr_alternate_modes);
 		if (error)
-			return error;
+			hid_warn(hid, "Unable to create sysfs interface for \"alternate_modes\", errno %d\n", error);
 	}
 	dbg_hid("sysfs interface created\n");
 
 	/* Set the maximum range to start with */
-	entry->range = entry->max_range;
-	if (entry->set_range != NULL)
-		entry->set_range(hid, entry->range);
+	entry->wdata.range = entry->wdata.max_range;
+	if (entry->wdata.set_range)
+		entry->wdata.set_range(hid, entry->wdata.range);
 
 #ifdef CONFIG_LEDS_CLASS
 	/* register led subsystem - G27 only */
-	entry->led_state = 0;
+	entry->wdata.led_state = 0;
 	for (j = 0; j < 5; j++)
-		entry->led[j] = NULL;
+		entry->wdata.led[j] = NULL;
 
 	if (lg4ff_devices[i].product_id == USB_DEVICE_ID_LOGITECH_G27_WHEEL) {
 		struct led_classdev *led;
@@ -1126,7 +1264,7 @@ int lg4ff_init(struct hid_device *hid)
 			led = kzalloc(sizeof(struct led_classdev)+name_sz, GFP_KERNEL);
 			if (!led) {
 				hid_err(hid, "can't allocate memory for LED %d\n", j);
-				goto err;
+				goto err_leds;
 			}
 
 			name = (void *)(&led[1]);
@@ -1137,16 +1275,16 @@ int lg4ff_init(struct hid_device *hid)
 			led->brightness_get = lg4ff_led_get_brightness;
 			led->brightness_set = lg4ff_led_set_brightness;
 
-			entry->led[j] = led;
+			entry->wdata.led[j] = led;
 			error = led_classdev_register(&hid->dev, led);
 
 			if (error) {
 				hid_err(hid, "failed to register LED %d. Aborting.\n", j);
-err:
+err_leds:
 				/* Deregister LEDs (if any) */
 				for (j = 0; j < 5; j++) {
-					led = entry->led[j];
-					entry->led[j] = NULL;
+					led = entry->wdata.led[j];
+					entry->wdata.led[j] = NULL;
 					if (!led)
 						continue;
 					led_classdev_unregister(led);
@@ -1160,6 +1298,11 @@ out:
 #endif
 	hid_info(hid, "Force feedback support for Logitech Gaming Wheels\n");
 	return 0;
+
+err_init:
+	drv_data->device_props = NULL;
+	kfree(entry);
+	return error;
 }
 
 int lg4ff_deinit(struct hid_device *hid)
@@ -1176,14 +1319,13 @@ int lg4ff_deinit(struct hid_device *hid)
 	if (!entry)
 		goto out; /* Nothing more to do */
 
-	device_remove_file(&hid->dev, &dev_attr_range);
-
 	/* Multimode devices will have at least the "MODE_NATIVE" bit set */
-	if (entry->alternate_modes) {
+	if (entry->wdata.alternate_modes) {
 		device_remove_file(&hid->dev, &dev_attr_real_id);
 		device_remove_file(&hid->dev, &dev_attr_alternate_modes);
 	}
 
+	device_remove_file(&hid->dev, &dev_attr_range);
 #ifdef CONFIG_LEDS_CLASS
 	{
 		int j;
@@ -1192,8 +1334,8 @@ int lg4ff_deinit(struct hid_device *hid)
 		/* Deregister LEDs (if any) */
 		for (j = 0; j < 5; j++) {
 
-			led = entry->led[j];
-			entry->led[j] = NULL;
+			led = entry->wdata.led[j];
+			entry->wdata.led[j] = NULL;
 			if (!led)
 				continue;
 			led_classdev_unregister(led);
@@ -1201,10 +1343,10 @@ int lg4ff_deinit(struct hid_device *hid)
 		}
 	}
 #endif
+	hid_hw_stop(hid);
+	drv_data->device_props = NULL;
 
-	/* Deallocate memory */
 	kfree(entry);
-
 out:
 	dbg_hid("Device successfully unregistered\n");
 	return 0;
diff --git a/drivers/hid/hid-lg4ff.h b/drivers/hid/hid-lg4ff.h
index 5b6a5086c47f..66201af44da3 100644
--- a/drivers/hid/hid-lg4ff.h
+++ b/drivers/hid/hid-lg4ff.h
@@ -5,12 +5,12 @@
 extern int lg4ff_no_autoswitch; /* From hid-lg.c */
 
 int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
-			     struct hid_usage *usage, __s32 value, struct lg_drv_data *drv_data);
+			     struct hid_usage *usage, s32 value, struct lg_drv_data *drv_data);
 int lg4ff_init(struct hid_device *hdev);
 int lg4ff_deinit(struct hid_device *hdev);
 #else
 static inline int lg4ff_adjust_input_event(struct hid_device *hid, struct hid_field *field,
-					   struct hid_usage *usage, __s32 value, struct lg_drv_data *drv_data) { return 0; }
+					   struct hid_usage *usage, s32 value, struct lg_drv_data *drv_data) { return 0; }
 static inline int lg4ff_init(struct hid_device *hdev) { return -1; }
 static inline int lg4ff_deinit(struct hid_device *hdev) { return -1; }
 #endif
diff --git a/drivers/hid/hid-logitech-hidpp.c b/drivers/hid/hid-logitech-hidpp.c
index 5fd530acf747..484196459305 100644
--- a/drivers/hid/hid-logitech-hidpp.c
+++ b/drivers/hid/hid-logitech-hidpp.c
@@ -40,8 +40,9 @@ MODULE_PARM_DESC(disable_raw_mode,
 #define HIDPP_REPORT_LONG_LENGTH		20
 
 #define HIDPP_QUIRK_CLASS_WTP			BIT(0)
+#define HIDPP_QUIRK_CLASS_M560			BIT(1)
 
-/* bits 1..20 are reserved for classes */
+/* bits 2..20 are reserved for classes */
 #define HIDPP_QUIRK_DELAYED_INIT		BIT(21)
 #define HIDPP_QUIRK_WTP_PHYSICAL_BUTTONS	BIT(22)
 
@@ -930,6 +931,207 @@ static int wtp_connect(struct hid_device *hdev, bool connected)
 			true, true);
 }
 
+/* ------------------------------------------------------------------------- */
+/* Logitech M560 devices                                                     */
+/* ------------------------------------------------------------------------- */
+
+/*
+ * Logitech M560 protocol overview
+ *
+ * The Logitech M560 mouse, is designed for windows 8. When the middle and/or
+ * the sides buttons are pressed, it sends some keyboard keys events
+ * instead of buttons ones.
+ * To complicate things further, the middle button keys sequence
+ * is different from the odd press and the even press.
+ *
+ * forward button -> Super_R
+ * backward button -> Super_L+'d' (press only)
+ * middle button -> 1st time: Alt_L+SuperL+XF86TouchpadOff (press only)
+ *                  2nd time: left-click (press only)
+ * NB: press-only means that when the button is pressed, the
+ * KeyPress/ButtonPress and KeyRelease/ButtonRelease events are generated
+ * together sequentially; instead when the button is released, no event is
+ * generated !
+ *
+ * With the command
+ *	10<xx>0a 3500af03 (where <xx> is the mouse id),
+ * the mouse reacts differently:
+ * - it never sends a keyboard key event
+ * - for the three mouse button it sends:
+ *	middle button               press   11<xx>0a 3500af00...
+ *	side 1 button (forward)     press   11<xx>0a 3500b000...
+ *	side 2 button (backward)    press   11<xx>0a 3500ae00...
+ *	middle/side1/side2 button   release 11<xx>0a 35000000...
+ */
+
+static const u8 m560_config_parameter[] = {0x00, 0xaf, 0x03};
+
+struct m560_private_data {
+	struct input_dev *input;
+};
+
+/* how buttons are mapped in the report */
+#define M560_MOUSE_BTN_LEFT		0x01
+#define M560_MOUSE_BTN_RIGHT		0x02
+#define M560_MOUSE_BTN_WHEEL_LEFT	0x08
+#define M560_MOUSE_BTN_WHEEL_RIGHT	0x10
+
+#define M560_SUB_ID			0x0a
+#define M560_BUTTON_MODE_REGISTER	0x35
+
+static int m560_send_config_command(struct hid_device *hdev, bool connected)
+{
+	struct hidpp_report response;
+	struct hidpp_device *hidpp_dev;
+
+	hidpp_dev = hid_get_drvdata(hdev);
+
+	if (!connected)
+		return -ENODEV;
+
+	return hidpp_send_rap_command_sync(
+		hidpp_dev,
+		REPORT_ID_HIDPP_SHORT,
+		M560_SUB_ID,
+		M560_BUTTON_MODE_REGISTER,
+		(u8 *)m560_config_parameter,
+		sizeof(m560_config_parameter),
+		&response
+	);
+}
+
+static int m560_allocate(struct hid_device *hdev)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+	struct m560_private_data *d;
+
+	d = devm_kzalloc(&hdev->dev, sizeof(struct m560_private_data),
+			GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	hidpp->private_data = d;
+
+	return 0;
+};
+
+static int m560_raw_event(struct hid_device *hdev, u8 *data, int size)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+	struct m560_private_data *mydata = hidpp->private_data;
+
+	/* sanity check */
+	if (!mydata || !mydata->input) {
+		hid_err(hdev, "error in parameter\n");
+		return -EINVAL;
+	}
+
+	if (size < 7) {
+		hid_err(hdev, "error in report\n");
+		return 0;
+	}
+
+	if (data[0] == REPORT_ID_HIDPP_LONG &&
+	    data[2] == M560_SUB_ID && data[6] == 0x00) {
+		/*
+		 * m560 mouse report for middle, forward and backward button
+		 *
+		 * data[0] = 0x11
+		 * data[1] = device-id
+		 * data[2] = 0x0a
+		 * data[5] = 0xaf -> middle
+		 *	     0xb0 -> forward
+		 *	     0xae -> backward
+		 *	     0x00 -> release all
+		 * data[6] = 0x00
+		 */
+
+		switch (data[5]) {
+		case 0xaf:
+			input_report_key(mydata->input, BTN_MIDDLE, 1);
+			break;
+		case 0xb0:
+			input_report_key(mydata->input, BTN_FORWARD, 1);
+			break;
+		case 0xae:
+			input_report_key(mydata->input, BTN_BACK, 1);
+			break;
+		case 0x00:
+			input_report_key(mydata->input, BTN_BACK, 0);
+			input_report_key(mydata->input, BTN_FORWARD, 0);
+			input_report_key(mydata->input, BTN_MIDDLE, 0);
+			break;
+		default:
+			hid_err(hdev, "error in report\n");
+			return 0;
+		}
+		input_sync(mydata->input);
+
+	} else if (data[0] == 0x02) {
+		/*
+		 * Logitech M560 mouse report
+		 *
+		 * data[0] = type (0x02)
+		 * data[1..2] = buttons
+		 * data[3..5] = xy
+		 * data[6] = wheel
+		 */
+
+		int v;
+
+		input_report_key(mydata->input, BTN_LEFT,
+			!!(data[1] & M560_MOUSE_BTN_LEFT));
+		input_report_key(mydata->input, BTN_RIGHT,
+			!!(data[1] & M560_MOUSE_BTN_RIGHT));
+
+		if (data[1] & M560_MOUSE_BTN_WHEEL_LEFT)
+			input_report_rel(mydata->input, REL_HWHEEL, -1);
+		else if (data[1] & M560_MOUSE_BTN_WHEEL_RIGHT)
+			input_report_rel(mydata->input, REL_HWHEEL, 1);
+
+		v = hid_snto32(hid_field_extract(hdev, data+3, 0, 12), 12);
+		input_report_rel(mydata->input, REL_X, v);
+
+		v = hid_snto32(hid_field_extract(hdev, data+3, 12, 12), 12);
+		input_report_rel(mydata->input, REL_Y, v);
+
+		v = hid_snto32(data[6], 8);
+		input_report_rel(mydata->input, REL_WHEEL, v);
+
+		input_sync(mydata->input);
+	}
+
+	return 1;
+}
+
+static void m560_populate_input(struct hidpp_device *hidpp,
+		struct input_dev *input_dev, bool origin_is_hid_core)
+{
+	struct m560_private_data *mydata = hidpp->private_data;
+
+	mydata->input = input_dev;
+
+	__set_bit(EV_KEY, mydata->input->evbit);
+	__set_bit(BTN_MIDDLE, mydata->input->keybit);
+	__set_bit(BTN_RIGHT, mydata->input->keybit);
+	__set_bit(BTN_LEFT, mydata->input->keybit);
+	__set_bit(BTN_BACK, mydata->input->keybit);
+	__set_bit(BTN_FORWARD, mydata->input->keybit);
+
+	__set_bit(EV_REL, mydata->input->evbit);
+	__set_bit(REL_X, mydata->input->relbit);
+	__set_bit(REL_Y, mydata->input->relbit);
+	__set_bit(REL_WHEEL, mydata->input->relbit);
+	__set_bit(REL_HWHEEL, mydata->input->relbit);
+}
+
+static int m560_input_mapping(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	return -1;
+}
+
 /* -------------------------------------------------------------------------- */
 /* Generic HID++ devices                                                      */
 /* -------------------------------------------------------------------------- */
@@ -942,6 +1144,9 @@ static int hidpp_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 
 	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)
 		return wtp_input_mapping(hdev, hi, field, usage, bit, max);
+	else if (hidpp->quirks & HIDPP_QUIRK_CLASS_M560 &&
+			field->application != HID_GD_MOUSE)
+		return m560_input_mapping(hdev, hi, field, usage, bit, max);
 
 	return 0;
 }
@@ -951,6 +1156,8 @@ static void hidpp_populate_input(struct hidpp_device *hidpp,
 {
 	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)
 		wtp_populate_input(hidpp, input, origin_is_hid_core);
+	else if (hidpp->quirks & HIDPP_QUIRK_CLASS_M560)
+		m560_populate_input(hidpp, input, origin_is_hid_core);
 }
 
 static void hidpp_input_configured(struct hid_device *hdev,
@@ -1038,6 +1245,8 @@ static int hidpp_raw_event(struct hid_device *hdev, struct hid_report *report,
 
 	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)
 		return wtp_raw_event(hdev, data, size);
+	else if (hidpp->quirks & HIDPP_QUIRK_CLASS_M560)
+		return m560_raw_event(hdev, data, size);
 
 	return 0;
 }
@@ -1115,6 +1324,10 @@ static void hidpp_connect_event(struct hidpp_device *hidpp)
 		ret = wtp_connect(hdev, connected);
 		if (ret)
 			return;
+	} else if (hidpp->quirks & HIDPP_QUIRK_CLASS_M560) {
+		ret = m560_send_config_command(hdev, connected);
+		if (ret)
+			return;
 	}
 
 	if (!connected || hidpp->delayed_input)
@@ -1190,7 +1403,11 @@ static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	if (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP) {
 		ret = wtp_allocate(hdev, id);
 		if (ret)
-			goto wtp_allocate_fail;
+			goto allocate_fail;
+	} else if (hidpp->quirks & HIDPP_QUIRK_CLASS_M560) {
+		ret = m560_allocate(hdev);
+		if (ret)
+			goto allocate_fail;
 	}
 
 	INIT_WORK(&hidpp->work, delayed_work_cb);
@@ -1253,7 +1470,7 @@ hid_hw_start_fail:
 hid_parse_fail:
 	cancel_work_sync(&hidpp->work);
 	mutex_destroy(&hidpp->send_mutex);
-wtp_allocate_fail:
+allocate_fail:
 	hid_set_drvdata(hdev, NULL);
 	return ret;
 }
@@ -1281,6 +1498,10 @@ static const struct hid_device_id hidpp_devices[] = {
 	  HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_LOGITECH,
 		USB_DEVICE_ID_LOGITECH_T651),
 	  .driver_data = HIDPP_QUIRK_CLASS_WTP },
+	{ /* Mouse logitech M560 */
+	  HID_DEVICE(BUS_USB, HID_GROUP_LOGITECH_DJ_DEVICE,
+		USB_VENDOR_ID_LOGITECH, 0x402d),
+	  .driver_data = HIDPP_QUIRK_DELAYED_INIT | HIDPP_QUIRK_CLASS_M560 },
 
 	{ HID_DEVICE(BUS_USB, HID_GROUP_LOGITECH_DJ_DEVICE,
 		USB_VENDOR_ID_LOGITECH, HID_ANY_ID)},
diff --git a/drivers/hid/hid-microsoft.c b/drivers/hid/hid-microsoft.c
index af935eb198c9..32a596f554af 100644
--- a/drivers/hid/hid-microsoft.c
+++ b/drivers/hid/hid-microsoft.c
@@ -280,6 +280,8 @@ static const struct hid_device_id ms_devices[] = {
 		.driver_data = MS_HIDINPUT },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3_JP),
 		.driver_data = MS_HIDINPUT },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_POWER_COVER),
+		.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-plantronics.c b/drivers/hid/hid-plantronics.c
index 2180e0789b76..febb21ee190e 100644
--- a/drivers/hid/hid-plantronics.c
+++ b/drivers/hid/hid-plantronics.c
@@ -2,7 +2,7 @@
  *  Plantronics USB HID Driver
  *
  *  Copyright (c) 2014 JD Cole <jd.cole@plantronics.com>
- *  Copyright (c) 2014 Terry Junge <terry.junge@plantronics.com>
+ *  Copyright (c) 2015 Terry Junge <terry.junge@plantronics.com>
  */
 
 /*
@@ -17,23 +17,138 @@
 #include <linux/hid.h>
 #include <linux/module.h>
 
+#define PLT_HID_1_0_PAGE	0xffa00000
+#define PLT_HID_2_0_PAGE	0xffa20000
+
+#define PLT_BASIC_TELEPHONY	0x0003
+#define PLT_BASIC_EXCEPTION	0x0005
+
+#define PLT_VOL_UP		0x00b1
+#define PLT_VOL_DOWN		0x00b2
+
+#define PLT1_VOL_UP		(PLT_HID_1_0_PAGE | PLT_VOL_UP)
+#define PLT1_VOL_DOWN		(PLT_HID_1_0_PAGE | PLT_VOL_DOWN)
+#define PLT2_VOL_UP		(PLT_HID_2_0_PAGE | PLT_VOL_UP)
+#define PLT2_VOL_DOWN		(PLT_HID_2_0_PAGE | PLT_VOL_DOWN)
+
+#define PLT_DA60		0xda60
+#define PLT_BT300_MIN		0x0413
+#define PLT_BT300_MAX		0x0418
+
+
+#define PLT_ALLOW_CONSUMER (field->application == HID_CP_CONSUMERCONTROL && \
+			    (usage->hid & HID_USAGE_PAGE) == HID_UP_CONSUMER)
+
 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;
+	unsigned short mapped_key;
+	unsigned long plt_type = (unsigned long)hid_get_drvdata(hdev);
+
+	/* handle volume up/down mapping */
+	/* non-standard types or multi-HID interfaces - plt_type is PID */
+	if (!(plt_type & HID_USAGE_PAGE)) {
+		switch (plt_type) {
+		case PLT_DA60:
+			if (PLT_ALLOW_CONSUMER)
+				goto defaulted;
+			goto ignored;
+		default:
+			if (PLT_ALLOW_CONSUMER)
+				goto defaulted;
+		}
+	}
+	/* handle standard types - plt_type is 0xffa0uuuu or 0xffa2uuuu */
+	/* 'basic telephony compliant' - allow default consumer page map */
+	else if ((plt_type & HID_USAGE) >= PLT_BASIC_TELEPHONY &&
+		 (plt_type & HID_USAGE) != PLT_BASIC_EXCEPTION) {
+		if (PLT_ALLOW_CONSUMER)
+			goto defaulted;
+	}
+	/* not 'basic telephony' - apply legacy mapping */
+	/* only map if the field is in the device's primary vendor page */
+	else if (!((field->application ^ plt_type) & HID_USAGE_PAGE)) {
+		switch (usage->hid) {
+		case PLT1_VOL_UP:
+		case PLT2_VOL_UP:
+			mapped_key = KEY_VOLUMEUP;
+			goto mapped;
+		case PLT1_VOL_DOWN:
+		case PLT2_VOL_DOWN:
+			mapped_key = KEY_VOLUMEDOWN;
+			goto mapped;
+		}
 	}
 
-	hid_dbg(hdev, "usage: %08x (appl: %08x) - ignored\n",
-		usage->hid, field->application);
+/*
+ * Future mapping of call control or other usages,
+ * if and when keys are defined would go here
+ * otherwise, ignore everything else that was not mapped
+ */
 
+ignored:
 	return -1;
+
+defaulted:
+	hid_dbg(hdev, "usage: %08x (appl: %08x) - defaulted\n",
+		usage->hid, field->application);
+	return 0;
+
+mapped:
+	hid_map_usage_clear(hi, usage, bit, max, EV_KEY, mapped_key);
+	hid_dbg(hdev, "usage: %08x (appl: %08x) - mapped to key %d\n",
+		usage->hid, field->application, mapped_key);
+	return 1;
+}
+
+static unsigned long plantronics_device_type(struct hid_device *hdev)
+{
+	unsigned i, col_page;
+	unsigned long plt_type = hdev->product;
+
+	/* multi-HID interfaces? - plt_type is PID */
+	if (plt_type >= PLT_BT300_MIN && plt_type <= PLT_BT300_MAX)
+		goto exit;
+
+	/* determine primary vendor page */
+	for (i = 0; i < hdev->maxcollection; i++) {
+		col_page = hdev->collection[i].usage & HID_USAGE_PAGE;
+		if (col_page == PLT_HID_2_0_PAGE) {
+			plt_type = hdev->collection[i].usage;
+			break;
+		}
+		if (col_page == PLT_HID_1_0_PAGE)
+			plt_type = hdev->collection[i].usage;
+	}
+
+exit:
+	hid_dbg(hdev, "plt_type decoded as: %08lx\n", plt_type);
+	return plt_type;
+}
+
+static int plantronics_probe(struct hid_device *hdev,
+			     const struct hid_device_id *id)
+{
+	int ret;
+
+	ret = hid_parse(hdev);
+	if (ret) {
+		hid_err(hdev, "parse failed\n");
+		goto err;
+	}
+
+	hid_set_drvdata(hdev, (void *)plantronics_device_type(hdev));
+
+	ret = hid_hw_start(hdev, HID_CONNECT_DEFAULT |
+		HID_CONNECT_HIDINPUT_FORCE | HID_CONNECT_HIDDEV_FORCE);
+	if (ret)
+		hid_err(hdev, "hw start failed\n");
+
+err:
+	return ret;
 }
 
 static const struct hid_device_id plantronics_devices[] = {
@@ -46,6 +161,7 @@ static struct hid_driver plantronics_driver = {
 	.name = "plantronics",
 	.id_table = plantronics_devices,
 	.input_mapping = plantronics_input_mapping,
+	.probe = plantronics_probe,
 };
 module_hid_driver(plantronics_driver);
 
diff --git a/drivers/hid/hid-prodikeys.c b/drivers/hid/hid-prodikeys.c
index 91fab975063c..e3e98ccf137b 100644
--- a/drivers/hid/hid-prodikeys.c
+++ b/drivers/hid/hid-prodikeys.c
@@ -395,11 +395,10 @@ static int pcmidi_handle_report4(struct pcmidi_snd *pm, u8 *data)
 
 	/* break keys */
 	for (bit_index = 0; bit_index < 24; bit_index++) {
-		key = pm->last_key[bit_index];
 		if (!((0x01 << bit_index) & bit_mask)) {
 			input_event(pm->input_ep82, EV_KEY,
 				pm->last_key[bit_index], 0);
-				pm->last_key[bit_index] = 0;
+			pm->last_key[bit_index] = 0;
 		}
 	}
 
diff --git a/drivers/hid/hid-rmi.c b/drivers/hid/hid-rmi.c
index 368ffdf2c0a3..4cf80bb276dc 100644
--- a/drivers/hid/hid-rmi.c
+++ b/drivers/hid/hid-rmi.c
@@ -29,9 +29,9 @@
 #define RMI_SET_RMI_MODE_REPORT_ID	0x0f /* Feature Report */
 
 /* flags */
-#define RMI_READ_REQUEST_PENDING	BIT(0)
-#define RMI_READ_DATA_PENDING		BIT(1)
-#define RMI_STARTED			BIT(2)
+#define RMI_READ_REQUEST_PENDING	0
+#define RMI_READ_DATA_PENDING		1
+#define RMI_STARTED			2
 
 /* device flags */
 #define RMI_DEVICE			BIT(0)
@@ -1013,6 +1013,7 @@ static int rmi_populate_f30(struct hid_device *hdev)
 
 static int rmi_populate(struct hid_device *hdev)
 {
+	struct rmi_data *data = hid_get_drvdata(hdev);
 	int ret;
 
 	ret = rmi_scan_pdt(hdev);
@@ -1033,9 +1034,11 @@ static int rmi_populate(struct hid_device *hdev)
 		return ret;
 	}
 
-	ret = rmi_populate_f30(hdev);
-	if (ret)
-		hid_warn(hdev, "Error while initializing F30 (%d).\n", ret);
+	if (!(data->device_flags & RMI_DEVICE_HAS_PHYS_BUTTONS)) {
+		ret = rmi_populate_f30(hdev);
+		if (ret)
+			hid_warn(hdev, "Error while initializing F30 (%d).\n", ret);
+	}
 
 	return 0;
 }
diff --git a/drivers/hid/hid-sjoy.c b/drivers/hid/hid-sjoy.c
index 37845eccddb5..36b6470af947 100644
--- a/drivers/hid/hid-sjoy.c
+++ b/drivers/hid/hid-sjoy.c
@@ -166,6 +166,9 @@ static const struct hid_device_id sjoy_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_WISEGROUP, USB_DEVICE_ID_DUAL_USB_JOYPAD),
 		.driver_data = HID_QUIRK_MULTI_INPUT |
 			       HID_QUIRK_SKIP_OUTPUT_REPORTS },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PLAYDOTCOM, USB_DEVICE_ID_PLAYDOTCOM_EMS_USBII),
+		.driver_data = HID_QUIRK_MULTI_INPUT |
+			       HID_QUIRK_SKIP_OUTPUT_REPORTS },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, sjoy_devices);
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 6ca96cebb44c..ed2f008f8403 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -46,20 +46,37 @@
 #define PS3REMOTE                 BIT(4)
 #define DUALSHOCK4_CONTROLLER_USB BIT(5)
 #define DUALSHOCK4_CONTROLLER_BT  BIT(6)
+#define MOTION_CONTROLLER_USB     BIT(7)
+#define MOTION_CONTROLLER_BT      BIT(8)
+#define NAVIGATION_CONTROLLER_USB BIT(9)
+#define NAVIGATION_CONTROLLER_BT  BIT(10)
 
 #define SIXAXIS_CONTROLLER (SIXAXIS_CONTROLLER_USB | SIXAXIS_CONTROLLER_BT)
+#define MOTION_CONTROLLER (MOTION_CONTROLLER_USB | MOTION_CONTROLLER_BT)
+#define NAVIGATION_CONTROLLER (NAVIGATION_CONTROLLER_USB |\
+				NAVIGATION_CONTROLLER_BT)
 #define DUALSHOCK4_CONTROLLER (DUALSHOCK4_CONTROLLER_USB |\
 				DUALSHOCK4_CONTROLLER_BT)
 #define SONY_LED_SUPPORT (SIXAXIS_CONTROLLER | BUZZ_CONTROLLER |\
-				DUALSHOCK4_CONTROLLER)
-#define SONY_BATTERY_SUPPORT (SIXAXIS_CONTROLLER | DUALSHOCK4_CONTROLLER)
-#define SONY_FF_SUPPORT (SIXAXIS_CONTROLLER | DUALSHOCK4_CONTROLLER)
+				DUALSHOCK4_CONTROLLER | MOTION_CONTROLLER |\
+				NAVIGATION_CONTROLLER)
+#define SONY_BATTERY_SUPPORT (SIXAXIS_CONTROLLER | DUALSHOCK4_CONTROLLER |\
+				MOTION_CONTROLLER_BT | NAVIGATION_CONTROLLER)
+#define SONY_FF_SUPPORT (SIXAXIS_CONTROLLER | DUALSHOCK4_CONTROLLER |\
+				MOTION_CONTROLLER)
 
 #define MAX_LEDS 4
 
+/*
+ * The Sixaxis reports both digital and analog values for each button on the
+ * controller except for Start, Select and the PS button.  The controller ends
+ * up reporting 27 axes which causes them to spill over into the multi-touch
+ * axis values.  Additionally, the controller only has 20 actual, physical axes
+ * so there are several unused axes in between the used ones.
+ */
 static __u8 sixaxis_rdesc[] = {
 	0x05, 0x01,         /*  Usage Page (Desktop),               */
-	0x09, 0x04,         /*  Usage (Joystik),                    */
+	0x09, 0x04,         /*  Usage (Joystick),                   */
 	0xA1, 0x01,         /*  Collection (Application),           */
 	0xA1, 0x02,         /*      Collection (Logical),           */
 	0x85, 0x01,         /*          Report ID (1),              */
@@ -134,6 +151,186 @@ static __u8 sixaxis_rdesc[] = {
 	0xC0                /*  End Collection                      */
 };
 
+/* PS/3 Motion controller */
+static __u8 motion_rdesc[] = {
+	0x05, 0x01,         /*  Usage Page (Desktop),               */
+	0x09, 0x04,         /*  Usage (Joystick),                   */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0xA1, 0x02,         /*      Collection (Logical),           */
+	0x85, 0x01,         /*          Report ID (1),              */
+	0x75, 0x01,         /*          Report Size (1),            */
+	0x95, 0x15,         /*          Report Count (21),          */
+	0x15, 0x00,         /*          Logical Minimum (0),        */
+	0x25, 0x01,         /*          Logical Maximum (1),        */
+	0x35, 0x00,         /*          Physical Minimum (0),       */
+	0x45, 0x01,         /*          Physical Maximum (1),       */
+	0x05, 0x09,         /*          Usage Page (Button),        */
+	0x19, 0x01,         /*          Usage Minimum (01h),        */
+	0x29, 0x15,         /*          Usage Maximum (15h),        */
+	0x81, 0x02,         /*          Input (Variable),           * Buttons */
+	0x95, 0x0B,         /*          Report Count (11),          */
+	0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+	0x81, 0x03,         /*          Input (Constant, Variable), * Padding */
+	0x15, 0x00,         /*          Logical Minimum (0),        */
+	0x26, 0xFF, 0x00,   /*          Logical Maximum (255),      */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0xA1, 0x00,         /*          Collection (Physical),      */
+	0x75, 0x08,         /*              Report Size (8),        */
+	0x95, 0x01,         /*              Report Count (1),       */
+	0x35, 0x00,         /*              Physical Minimum (0),   */
+	0x46, 0xFF, 0x00,   /*              Physical Maximum (255), */
+	0x09, 0x30,         /*              Usage (X),              */
+	0x81, 0x02,         /*              Input (Variable),       * Trigger */
+	0xC0,               /*          End Collection,             */
+	0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x07,         /*          Report Count (7),           * skip 7 bytes */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0x75, 0x10,         /*          Report Size (16),           */
+	0x46, 0xFF, 0xFF,   /*          Physical Maximum (65535),   */
+	0x27, 0xFF, 0xFF, 0x00, 0x00, /*      Logical Maximum (65535),    */
+	0x95, 0x03,         /*          Report Count (3),           * 3x Accels */
+	0x09, 0x33,         /*              Usage (rX),             */
+	0x09, 0x34,         /*              Usage (rY),             */
+	0x09, 0x35,         /*              Usage (rZ),             */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+	0x95, 0x03,         /*          Report Count (3),           * Skip Accels 2nd frame */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0x95, 0x03,         /*          Report Count (3),           * 3x Gyros */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+	0x95, 0x03,         /*          Report Count (3),           * Skip Gyros 2nd frame */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x75, 0x0C,         /*          Report Size (12),           */
+	0x46, 0xFF, 0x0F,   /*          Physical Maximum (4095),    */
+	0x26, 0xFF, 0x0F,   /*          Logical Maximum (4095),     */
+	0x95, 0x04,         /*          Report Count (4),           * Skip Temp and Magnetometers */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x46, 0xFF, 0x00,   /*          Physical Maximum (255),     */
+	0x26, 0xFF, 0x00,   /*          Logical Maximum (255),      */
+	0x95, 0x06,         /*          Report Count (6),           * Skip Timestamp and Extension Bytes */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0x91, 0x02,         /*          Output (Variable),          */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xB1, 0x02,         /*          Feature (Variable),         */
+	0xC0,               /*      End Collection,                 */
+	0xA1, 0x02,         /*      Collection (Logical),           */
+	0x85, 0x02,         /*          Report ID (2),              */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xB1, 0x02,         /*          Feature (Variable),         */
+	0xC0,               /*      End Collection,                 */
+	0xA1, 0x02,         /*      Collection (Logical),           */
+	0x85, 0xEE,         /*          Report ID (238),            */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xB1, 0x02,         /*          Feature (Variable),         */
+	0xC0,               /*      End Collection,                 */
+	0xA1, 0x02,         /*      Collection (Logical),           */
+	0x85, 0xEF,         /*          Report ID (239),            */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xB1, 0x02,         /*          Feature (Variable),         */
+	0xC0,               /*      End Collection,                 */
+	0xC0                /*  End Collection                      */
+};
+
+/* PS/3 Navigation controller */
+static __u8 navigation_rdesc[] = {
+	0x05, 0x01,         /*  Usage Page (Desktop),               */
+	0x09, 0x04,         /*  Usage (Joystik),                    */
+	0xA1, 0x01,         /*  Collection (Application),           */
+	0xA1, 0x02,         /*      Collection (Logical),           */
+	0x85, 0x01,         /*          Report ID (1),              */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x01,         /*          Report Count (1),           */
+	0x15, 0x00,         /*          Logical Minimum (0),        */
+	0x26, 0xFF, 0x00,   /*          Logical Maximum (255),      */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0x75, 0x01,         /*          Report Size (1),            */
+	0x95, 0x13,         /*          Report Count (19),          */
+	0x15, 0x00,         /*          Logical Minimum (0),        */
+	0x25, 0x01,         /*          Logical Maximum (1),        */
+	0x35, 0x00,         /*          Physical Minimum (0),       */
+	0x45, 0x01,         /*          Physical Maximum (1),       */
+	0x05, 0x09,         /*          Usage Page (Button),        */
+	0x19, 0x01,         /*          Usage Minimum (01h),        */
+	0x29, 0x13,         /*          Usage Maximum (13h),        */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x75, 0x01,         /*          Report Size (1),            */
+	0x95, 0x0D,         /*          Report Count (13),          */
+	0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0x15, 0x00,         /*          Logical Minimum (0),        */
+	0x26, 0xFF, 0x00,   /*          Logical Maximum (255),      */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xA1, 0x00,         /*          Collection (Physical),      */
+	0x75, 0x08,         /*              Report Size (8),        */
+	0x95, 0x02,         /*              Report Count (2),       */
+	0x35, 0x00,         /*              Physical Minimum (0),   */
+	0x46, 0xFF, 0x00,   /*              Physical Maximum (255), */
+	0x09, 0x30,         /*              Usage (X),              */
+	0x09, 0x31,         /*              Usage (Y),              */
+	0x81, 0x02,         /*              Input (Variable),       */
+	0xC0,               /*          End Collection,             */
+	0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+	0x95, 0x06,         /*          Report Count (6),           */
+	0x81, 0x03,         /*          Input (Constant, Variable), */
+	0x05, 0x01,         /*          Usage Page (Desktop),       */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x05,         /*          Report Count (5),           */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x06, 0x00, 0xFF,   /*          Usage Page (FF00h),         */
+	0x95, 0x20,         /*          Report Count (26),          */
+	0x81, 0x02,         /*          Input (Variable),           */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0x91, 0x02,         /*          Output (Variable),          */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xB1, 0x02,         /*          Feature (Variable),         */
+	0xC0,               /*      End Collection,                 */
+	0xA1, 0x02,         /*      Collection (Logical),           */
+	0x85, 0x02,         /*          Report ID (2),              */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xB1, 0x02,         /*          Feature (Variable),         */
+	0xC0,               /*      End Collection,                 */
+	0xA1, 0x02,         /*      Collection (Logical),           */
+	0x85, 0xEE,         /*          Report ID (238),            */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xB1, 0x02,         /*          Feature (Variable),         */
+	0xC0,               /*      End Collection,                 */
+	0xA1, 0x02,         /*      Collection (Logical),           */
+	0x85, 0xEF,         /*          Report ID (239),            */
+	0x75, 0x08,         /*          Report Size (8),            */
+	0x95, 0x30,         /*          Report Count (48),          */
+	0x09, 0x01,         /*          Usage (Pointer),            */
+	0xB1, 0x02,         /*          Feature (Variable),         */
+	0xC0,               /*      End Collection,                 */
+	0xC0                /*  End Collection                      */
+};
+
 /*
  * The default descriptor doesn't provide mapping for the accelerometers
  * or orientation sensors.  This fixed descriptor maps the accelerometers
@@ -798,12 +995,20 @@ union sixaxis_output_report_01 {
 	__u8 buf[36];
 };
 
+struct motion_output_report_02 {
+	u8 type, zero;
+	u8 r, g, b;
+	u8 zero2;
+	u8 rumble;
+};
+
 #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 17
 #define SIXAXIS_REPORT_0xF5_SIZE 8
+#define MOTION_REPORT_0x02_SIZE 49
 
 static DEFINE_SPINLOCK(sony_dev_list_lock);
 static LIST_HEAD(sony_device_list);
@@ -844,6 +1049,20 @@ static __u8 *sixaxis_fixup(struct hid_device *hdev, __u8 *rdesc,
 	return sixaxis_rdesc;
 }
 
+static u8 *motion_fixup(struct hid_device *hdev, u8 *rdesc,
+			     unsigned int *rsize)
+{
+	*rsize = sizeof(motion_rdesc);
+	return motion_rdesc;
+}
+
+static u8 *navigation_fixup(struct hid_device *hdev, u8 *rdesc,
+			     unsigned int *rsize)
+{
+	*rsize = sizeof(navigation_rdesc);
+	return navigation_rdesc;
+}
+
 static __u8 *ps3remote_fixup(struct hid_device *hdev, __u8 *rdesc,
 			     unsigned int *rsize)
 {
@@ -924,6 +1143,12 @@ static __u8 *sony_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 	if (sc->quirks & SIXAXIS_CONTROLLER)
 		return sixaxis_fixup(hdev, rdesc, rsize);
 
+	if (sc->quirks & MOTION_CONTROLLER)
+		return motion_fixup(hdev, rdesc, rsize);
+
+	if (sc->quirks & NAVIGATION_CONTROLLER)
+		return navigation_fixup(hdev, rdesc, rsize);
+
 	if (sc->quirks & PS3REMOTE)
 		return ps3remote_fixup(hdev, rdesc, rsize);
 
@@ -934,6 +1159,7 @@ static void sixaxis_parse_report(struct sony_sc *sc, __u8 *rd, int size)
 {
 	static const __u8 sixaxis_battery_capacity[] = { 0, 1, 25, 50, 75, 100 };
 	unsigned long flags;
+	int offset;
 	__u8 cable_state, battery_capacity, battery_charging;
 
 	/*
@@ -942,12 +1168,14 @@ static void sixaxis_parse_report(struct sony_sc *sc, __u8 *rd, int size)
 	 * It does not report the actual level while charging so it
 	 * is set to 100% while charging is in progress.
 	 */
-	if (rd[30] >= 0xee) {
+	offset = (sc->quirks & MOTION_CONTROLLER) ? 12 : 30;
+
+	if (rd[offset] >= 0xee) {
 		battery_capacity = 100;
-		battery_charging = !(rd[30] & 0x01);
+		battery_charging = !(rd[offset] & 0x01);
 		cable_state = 1;
 	} else {
-		__u8 index = rd[30] <= 5 ? rd[30] : 5;
+		__u8 index = rd[offset] <= 5 ? rd[offset] : 5;
 		battery_capacity = sixaxis_battery_capacity[index];
 		battery_charging = 0;
 		cable_state = 0;
@@ -1048,6 +1276,11 @@ static int sony_raw_event(struct hid_device *hdev, struct hid_report *report,
 		swap(rd[47], rd[48]);
 
 		sixaxis_parse_report(sc, rd, size);
+	} else if ((sc->quirks & MOTION_CONTROLLER_BT) && rd[0] == 0x01 && size == 49) {
+		sixaxis_parse_report(sc, rd, size);
+	} else if ((sc->quirks & NAVIGATION_CONTROLLER) && rd[0] == 0x01 &&
+			size == 49) {
+		sixaxis_parse_report(sc, rd, size);
 	} else if (((sc->quirks & DUALSHOCK4_CONTROLLER_USB) && rd[0] == 0x01 &&
 			size == 64) || ((sc->quirks & DUALSHOCK4_CONTROLLER_BT)
 			&& rd[0] == 0x11 && size == 78)) {
@@ -1208,7 +1441,7 @@ static int dualshock4_set_operational_bt(struct hid_device *hdev)
 	return ret;
 }
 
-static void sixaxis_set_leds_from_id(int id, __u8 values[MAX_LEDS])
+static void sixaxis_set_leds_from_id(struct sony_sc *sc)
 {
 	static const __u8 sixaxis_leds[10][4] = {
 				{ 0x01, 0x00, 0x00, 0x00 },
@@ -1223,16 +1456,18 @@ static void sixaxis_set_leds_from_id(int id, __u8 values[MAX_LEDS])
 				{ 0x01, 0x01, 0x01, 0x01 }
 	};
 
-	BUG_ON(MAX_LEDS < ARRAY_SIZE(sixaxis_leds[0]));
+	int id = sc->device_id;
+
+	BUILD_BUG_ON(MAX_LEDS < ARRAY_SIZE(sixaxis_leds[0]));
 
 	if (id < 0)
 		return;
 
 	id %= 10;
-	memcpy(values, sixaxis_leds[id], sizeof(sixaxis_leds[id]));
+	memcpy(sc->led_state, sixaxis_leds[id], sizeof(sixaxis_leds[id]));
 }
 
-static void dualshock4_set_leds_from_id(int id, __u8 values[MAX_LEDS])
+static void dualshock4_set_leds_from_id(struct sony_sc *sc)
 {
 	/* The first 4 color/index entries match what the PS4 assigns */
 	static const __u8 color_code[7][3] = {
@@ -1245,46 +1480,44 @@ static void dualshock4_set_leds_from_id(int id, __u8 values[MAX_LEDS])
 			/* White  */	{ 0x01, 0x01, 0x01 }
 	};
 
-	BUG_ON(MAX_LEDS < ARRAY_SIZE(color_code[0]));
+	int id = sc->device_id;
+
+	BUILD_BUG_ON(MAX_LEDS < ARRAY_SIZE(color_code[0]));
 
 	if (id < 0)
 		return;
 
 	id %= 7;
-	memcpy(values, color_code[id], sizeof(color_code[id]));
+	memcpy(sc->led_state, color_code[id], sizeof(color_code[id]));
 }
 
-static void buzz_set_leds(struct hid_device *hdev, const __u8 *leds)
+static void buzz_set_leds(struct sony_sc *sc)
 {
+	struct hid_device *hdev = sc->hdev;
 	struct list_head *report_list =
 		&hdev->report_enum[HID_OUTPUT_REPORT].report_list;
 	struct hid_report *report = list_entry(report_list->next,
 		struct hid_report, list);
 	__s32 *value = report->field[0]->value;
 
+	BUILD_BUG_ON(MAX_LEDS < 4);
+
 	value[0] = 0x00;
-	value[1] = leds[0] ? 0xff : 0x00;
-	value[2] = leds[1] ? 0xff : 0x00;
-	value[3] = leds[2] ? 0xff : 0x00;
-	value[4] = leds[3] ? 0xff : 0x00;
+	value[1] = sc->led_state[0] ? 0xff : 0x00;
+	value[2] = sc->led_state[1] ? 0xff : 0x00;
+	value[3] = sc->led_state[2] ? 0xff : 0x00;
+	value[4] = sc->led_state[3] ? 0xff : 0x00;
 	value[5] = 0x00;
 	value[6] = 0x00;
 	hid_hw_request(hdev, report, HID_REQ_SET_REPORT);
 }
 
-static void sony_set_leds(struct sony_sc *sc, const __u8 *leds, int count)
+static void sony_set_leds(struct sony_sc *sc)
 {
-	int n;
-
-	BUG_ON(count > MAX_LEDS);
-
-	if (sc->quirks & BUZZ_CONTROLLER && count == 4) {
-		buzz_set_leds(sc->hdev, leds);
-	} else {
-		for (n = 0; n < count; n++)
-			sc->led_state[n] = leds[n];
+	if (!(sc->quirks & BUZZ_CONTROLLER))
 		schedule_work(&sc->state_worker);
-	}
+	else
+		buzz_set_leds(sc);
 }
 
 static void sony_led_set_brightness(struct led_classdev *led,
@@ -1324,8 +1557,7 @@ static void sony_led_set_brightness(struct led_classdev *led,
 			drv_data->led_delay_on[n] = 0;
 			drv_data->led_delay_off[n] = 0;
 
-			sony_set_leds(drv_data, drv_data->led_state,
-					drv_data->led_count);
+			sony_set_leds(drv_data);
 			break;
 		}
 	}
@@ -1431,7 +1663,6 @@ static int sony_leds_init(struct sony_sc *sc)
 	const char *name_fmt;
 	static const char * const ds4_name_str[] = { "red", "green", "blue",
 						  "global" };
-	__u8 initial_values[MAX_LEDS] = { 0 };
 	__u8 max_brightness[MAX_LEDS] = { [0 ... (MAX_LEDS - 1)] = 1 };
 	__u8 use_hw_blink[MAX_LEDS] = { 0 };
 
@@ -1446,16 +1677,31 @@ static int sony_leds_init(struct sony_sc *sc)
 		if (!hid_validate_values(hdev, HID_OUTPUT_REPORT, 0, 0, 7))
 			return -ENODEV;
 	} else if (sc->quirks & DUALSHOCK4_CONTROLLER) {
-		dualshock4_set_leds_from_id(sc->device_id, initial_values);
-		initial_values[3] = 1;
+		dualshock4_set_leds_from_id(sc);
+		sc->led_state[3] = 1;
 		sc->led_count = 4;
 		memset(max_brightness, 255, 3);
 		use_hw_blink[3] = 1;
 		use_ds4_names = 1;
 		name_len = 0;
 		name_fmt = "%s:%s";
+	} else if (sc->quirks & MOTION_CONTROLLER) {
+		sc->led_count = 3;
+		memset(max_brightness, 255, 3);
+		use_ds4_names = 1;
+		name_len = 0;
+		name_fmt = "%s:%s";
+	} else if (sc->quirks & NAVIGATION_CONTROLLER) {
+		static const __u8 navigation_leds[4] = {0x01, 0x00, 0x00, 0x00};
+
+		memcpy(sc->led_state, navigation_leds, sizeof(navigation_leds));
+		sc->led_count = 1;
+		memset(use_hw_blink, 1, 4);
+		use_ds4_names = 0;
+		name_len = strlen("::sony#");
+		name_fmt = "%s::sony%d";
 	} else {
-		sixaxis_set_leds_from_id(sc->device_id, initial_values);
+		sixaxis_set_leds_from_id(sc);
 		sc->led_count = 4;
 		memset(use_hw_blink, 1, 4);
 		use_ds4_names = 0;
@@ -1468,7 +1714,7 @@ static int sony_leds_init(struct sony_sc *sc)
 	 * only relevant if the driver is loaded after somebody actively set the
 	 * LEDs to on
 	 */
-	sony_set_leds(sc, initial_values, sc->led_count);
+	sony_set_leds(sc);
 
 	name_sz = strlen(dev_name(&hdev->dev)) + name_len + 1;
 
@@ -1491,7 +1737,7 @@ static int sony_leds_init(struct sony_sc *sc)
 		else
 			snprintf(name, name_sz, name_fmt, dev_name(&hdev->dev), n + 1);
 		led->name = name;
-		led->brightness = initial_values[n];
+		led->brightness = sc->led_state[n];
 		led->max_brightness = max_brightness[n];
 		led->brightness_get = sony_led_get_brightness;
 		led->brightness_set = sony_led_set_brightness;
@@ -1622,9 +1868,31 @@ static void dualshock4_state_worker(struct work_struct *work)
 				HID_OUTPUT_REPORT, HID_REQ_SET_REPORT);
 }
 
+static void motion_state_worker(struct work_struct *work)
+{
+	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
+	struct hid_device *hdev = sc->hdev;
+	struct motion_output_report_02 *report =
+		(struct motion_output_report_02 *)sc->output_report_dmabuf;
+
+	memset(report, 0, MOTION_REPORT_0x02_SIZE);
+
+	report->type = 0x02; /* set leds */
+	report->r = sc->led_state[0];
+	report->g = sc->led_state[1];
+	report->b = sc->led_state[2];
+
+#ifdef CONFIG_SONY_FF
+	report->rumble = max(sc->right, sc->left);
+#endif
+
+	hid_hw_output_report(hdev, (__u8 *)report, MOTION_REPORT_0x02_SIZE);
+}
+
 static int sony_allocate_output_report(struct sony_sc *sc)
 {
-	if (sc->quirks & SIXAXIS_CONTROLLER)
+	if ((sc->quirks & SIXAXIS_CONTROLLER) ||
+			(sc->quirks & NAVIGATION_CONTROLLER))
 		sc->output_report_dmabuf =
 			kmalloc(sizeof(union sixaxis_output_report_01),
 				GFP_KERNEL);
@@ -1634,6 +1902,9 @@ static int sony_allocate_output_report(struct sony_sc *sc)
 	else if (sc->quirks & DUALSHOCK4_CONTROLLER_USB)
 		sc->output_report_dmabuf = kmalloc(DS4_REPORT_0x05_SIZE,
 						GFP_KERNEL);
+	else if (sc->quirks & MOTION_CONTROLLER)
+		sc->output_report_dmabuf = kmalloc(MOTION_REPORT_0x02_SIZE,
+						GFP_KERNEL);
 	else
 		return 0;
 
@@ -1839,6 +2110,8 @@ static int sony_check_add(struct sony_sc *sc)
 	int n, ret;
 
 	if ((sc->quirks & DUALSHOCK4_CONTROLLER_BT) ||
+	    (sc->quirks & MOTION_CONTROLLER_BT) ||
+	    (sc->quirks & NAVIGATION_CONTROLLER_BT) ||
 	    (sc->quirks & SIXAXIS_CONTROLLER_BT)) {
 		/*
 		 * sony_get_bt_devaddr() attempts to parse the Bluetooth MAC
@@ -1871,7 +2144,8 @@ static int sony_check_add(struct sony_sc *sc)
 		}
 
 		memcpy(sc->mac_address, &buf[1], sizeof(sc->mac_address));
-	} else if (sc->quirks & SIXAXIS_CONTROLLER_USB) {
+	} else if ((sc->quirks & SIXAXIS_CONTROLLER_USB) ||
+			(sc->quirks & NAVIGATION_CONTROLLER_USB)) {
 		buf = kmalloc(SIXAXIS_REPORT_0xF2_SIZE, GFP_KERNEL);
 		if (!buf)
 			return -ENOMEM;
@@ -1993,19 +2267,20 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		return ret;
 	}
 
-	ret = sony_allocate_output_report(sc);
+	ret = sony_set_device_id(sc);
 	if (ret < 0) {
-		hid_err(hdev, "failed to allocate the output report buffer\n");
+		hid_err(hdev, "failed to allocate the device id\n");
 		goto err_stop;
 	}
 
-	ret = sony_set_device_id(sc);
+	ret = sony_allocate_output_report(sc);
 	if (ret < 0) {
-		hid_err(hdev, "failed to allocate the device id\n");
+		hid_err(hdev, "failed to allocate the output report buffer\n");
 		goto err_stop;
 	}
 
-	if (sc->quirks & SIXAXIS_CONTROLLER_USB) {
+	if ((sc->quirks & SIXAXIS_CONTROLLER_USB) ||
+			(sc->quirks & NAVIGATION_CONTROLLER_USB)) {
 		/*
 		 * The Sony Sixaxis does not handle HID Output Reports on the
 		 * Interrupt EP like it could, so we need to force HID Output
@@ -2020,7 +2295,8 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		hdev->quirks |= HID_QUIRK_SKIP_OUTPUT_REPORT_ID;
 		ret = sixaxis_set_operational_usb(hdev);
 		sony_init_work(sc, sixaxis_state_worker);
-	} else if (sc->quirks & SIXAXIS_CONTROLLER_BT) {
+	} else if ((sc->quirks & SIXAXIS_CONTROLLER_BT) ||
+			(sc->quirks & NAVIGATION_CONTROLLER_BT)) {
 		/*
 		 * The Sixaxis wants output reports sent on the ctrl endpoint
 		 * when connected via Bluetooth.
@@ -2043,6 +2319,8 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		}
 
 		sony_init_work(sc, dualshock4_state_worker);
+	} else if (sc->quirks & MOTION_CONTROLLER) {
+		sony_init_work(sc, motion_state_worker);
 	} else {
 		ret = 0;
 	}
@@ -2122,7 +2400,13 @@ static const struct hid_device_id sony_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_CONTROLLER),
 		.driver_data = SIXAXIS_CONTROLLER_USB },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER),
-		.driver_data = SIXAXIS_CONTROLLER_USB },
+		.driver_data = NAVIGATION_CONTROLLER_USB },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_NAVIGATION_CONTROLLER),
+		.driver_data = NAVIGATION_CONTROLLER_BT },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_MOTION_CONTROLLER),
+		.driver_data = MOTION_CONTROLLER_USB },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_MOTION_CONTROLLER),
+		.driver_data = MOTION_CONTROLLER_BT },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_CONTROLLER),
 		.driver_data = SIXAXIS_CONTROLLER_BT },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_VAIO_VGX_MOUSE),
diff --git a/drivers/hid/i2c-hid/i2c-hid.c b/drivers/hid/i2c-hid/i2c-hid.c
index 92d6cdf02460..f77469d4edfb 100644
--- a/drivers/hid/i2c-hid/i2c-hid.c
+++ b/drivers/hid/i2c-hid/i2c-hid.c
@@ -42,9 +42,9 @@
 #include <linux/i2c/i2c-hid.h>
 
 /* flags */
-#define I2C_HID_STARTED		(1 << 0)
-#define I2C_HID_RESET_PENDING	(1 << 1)
-#define I2C_HID_READ_PENDING	(1 << 2)
+#define I2C_HID_STARTED		0
+#define I2C_HID_RESET_PENDING	1
+#define I2C_HID_READ_PENDING	2
 
 #define I2C_HID_PWR_ON		0x00
 #define I2C_HID_PWR_SLEEP	0x01
@@ -1019,7 +1019,6 @@ static int i2c_hid_probe(struct i2c_client *client,
 	hid->driver_data = client;
 	hid->ll_driver = &i2c_hid_ll_driver;
 	hid->dev.parent = &client->dev;
-	ACPI_COMPANION_SET(&hid->dev, ACPI_COMPANION(&client->dev));
 	hid->bus = BUS_I2C;
 	hid->version = le16_to_cpu(ihid->hdesc.bcdVersion);
 	hid->vendor = le16_to_cpu(ihid->hdesc.wVendorID);
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 4696895eb708..53e7de7cb9e2 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -52,7 +52,6 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_ETURBOTOUCH, USB_DEVICE_ID_ETURBOTOUCH_2968, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_GREENASIA, USB_DEVICE_ID_GREENASIA_DUAL_USB_JOYPAD, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_PANTHERLORD, USB_DEVICE_ID_PANTHERLORD_TWIN_USB_JOYSTICK, HID_QUIRK_MULTI_INPUT | HID_QUIRK_SKIP_OUTPUT_REPORTS },
-	{ USB_VENDOR_ID_PLAYDOTCOM, USB_DEVICE_ID_PLAYDOTCOM_EMS_USBII, HID_QUIRK_MULTI_INPUT },
 	{ USB_VENDOR_ID_TOUCHPACK, USB_DEVICE_ID_TOUCHPACK_RTS, HID_QUIRK_MULTI_INPUT },
 
 	{ USB_VENDOR_ID_AIREN, USB_DEVICE_ID_AIREN_SLIMPLUS, HID_QUIRK_NOGET },
@@ -70,6 +69,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_CH, USB_DEVICE_ID_CH_PRO_PEDALS, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_CH, USB_DEVICE_ID_CH_3AXIS_5BUTTON_STICK, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_CH, USB_DEVICE_ID_CH_AXIS_295, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_CHICONY, USB_DEVICE_ID_CHICONY_PIXART_USB_OPTICAL_MOUSE, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_DMI, USB_DEVICE_ID_DMI_ENC, HID_QUIRK_NOGET },
 	{ 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 },
@@ -89,6 +89,7 @@ static const struct hid_blacklist {
 	{ 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_MICROSOFT, USB_DEVICE_ID_MS_TYPE_COVER_3_JP, HID_QUIRK_NO_INIT_REPORTS },
+	{ USB_VENDOR_ID_MICROSOFT, USB_DEVICE_ID_MS_POWER_COVER, 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 },
@@ -141,6 +142,9 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_QUAD_HD, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_SYNAPTICS, USB_DEVICE_ID_SYNAPTICS_TP_V103, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_KEYBOARD_A096, HID_QUIRK_NO_INIT_INPUT_REPORTS },
+	{ USB_VENDOR_ID_MULTIPLE_1781, USB_DEVICE_ID_RAPHNET_4NES4SNES_OLD, HID_QUIRK_MULTI_INPUT },
+	{ USB_VENDOR_ID_DRACAL_RAPHNET, USB_DEVICE_ID_RAPHNET_2NES2SNES, HID_QUIRK_MULTI_INPUT },
+	{ USB_VENDOR_ID_DRACAL_RAPHNET, USB_DEVICE_ID_RAPHNET_4NES4SNES, HID_QUIRK_MULTI_INPUT },
 
 	{ 0, 0 }
 };
diff --git a/drivers/hid/wacom.h b/drivers/hid/wacom.h
index 024f4d89d579..a533787a6d85 100644
--- a/drivers/hid/wacom.h
+++ b/drivers/hid/wacom.h
@@ -134,8 +134,10 @@ static inline void wacom_schedule_work(struct wacom_wac *wacom_wac)
 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_pentouch_input_capabilities(struct input_dev *input_dev,
+void wacom_setup_device_quirks(struct wacom *wacom);
+int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
+				   struct wacom_wac *wacom_wac);
+int wacom_setup_touch_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 e8607d096138..4c0ffca97bef 100644
--- a/drivers/hid/wacom_sys.c
+++ b/drivers/hid/wacom_sys.c
@@ -35,7 +35,11 @@ static int wacom_get_report(struct hid_device *hdev, u8 type, u8 *buf,
 	do {
 		retval = hid_hw_raw_request(hdev, buf[0], buf, size, type,
 				HID_REQ_GET_REPORT);
-	} while ((retval == -ETIMEDOUT || retval == -EPIPE) && --retries);
+	} while ((retval == -ETIMEDOUT || retval == -EAGAIN) && --retries);
+
+	if (retval < 0)
+		hid_err(hdev, "wacom_get_report: ran out of retries "
+			"(last error = %d)\n", retval);
 
 	return retval;
 }
@@ -48,7 +52,11 @@ static int wacom_set_report(struct hid_device *hdev, u8 type, u8 *buf,
 	do {
 		retval = hid_hw_raw_request(hdev, buf[0], buf, size, type,
 				HID_REQ_SET_REPORT);
-	} while ((retval == -ETIMEDOUT || retval == -EPIPE) && --retries);
+	} while ((retval == -ETIMEDOUT || retval == -EAGAIN) && --retries);
+
+	if (retval < 0)
+		hid_err(hdev, "wacom_set_report: ran out of retries "
+			"(last error = %d)\n", retval);
 
 	return retval;
 }
@@ -117,9 +125,16 @@ static void wacom_feature_mapping(struct hid_device *hdev,
 				break;
 			data[0] = field->report->id;
 			ret = wacom_get_report(hdev, HID_FEATURE_REPORT,
-						data, 2, 0);
-			if (ret == 2)
+						data, 2, WAC_CMD_RETRIES);
+			if (ret == 2) {
 				features->touch_max = data[1];
+			} else {
+				features->touch_max = 16;
+				hid_warn(hdev, "wacom_feature_mapping: "
+					 "could not get HID_DG_CONTACTMAX, "
+					 "defaulting to %d\n",
+					  features->touch_max);
+			}
 			kfree(data);
 		}
 		break;
@@ -181,7 +196,11 @@ static void wacom_usage_mapping(struct hid_device *hdev,
 	* X/Y values and some cases of invalid Digitizer X/Y
 	* values commonly reported.
 	*/
-	if (!pen && !finger)
+	if (pen)
+		features->device_type |= WACOM_DEVICETYPE_PEN;
+	else if (finger)
+		features->device_type |= WACOM_DEVICETYPE_TOUCH;
+	else
 		return;
 
 	/*
@@ -198,14 +217,11 @@ static void wacom_usage_mapping(struct hid_device *hdev,
 	case HID_GD_X:
 		features->x_max = field->logical_maximum;
 		if (finger) {
-			features->device_type = BTN_TOOL_FINGER;
 			features->x_phy = field->physical_maximum;
 			if (features->type != BAMBOO_PT) {
 				features->unit = field->unit;
 				features->unitExpo = field->unit_exponent;
 			}
-		} else {
-			features->device_type = BTN_TOOL_PEN;
 		}
 		break;
 	case HID_GD_Y:
@@ -237,7 +253,7 @@ static void wacom_post_parse_hid(struct hid_device *hdev,
 	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_init_slots(wacom_wac->touch_input, wacom_wac->features.touch_max,
 				    INPUT_MT_DIRECT);
 		}
 	}
@@ -395,7 +411,7 @@ static int wacom_query_tablet_data(struct hid_device *hdev,
 	if (features->type == HID_GENERIC)
 		return wacom_hid_set_device_mode(hdev);
 
-	if (features->device_type == BTN_TOOL_FINGER) {
+	if (features->device_type & WACOM_DEVICETYPE_TOUCH) {
 		if (features->type > TABLETPC) {
 			/* MT Tablet PC touch */
 			return wacom_set_device_mode(hdev, 3, 4, 4);
@@ -409,7 +425,7 @@ static int wacom_query_tablet_data(struct hid_device *hdev,
 		else if (features->type == BAMBOO_PAD) {
 			return wacom_set_device_mode(hdev, 2, 2, 2);
 		}
-	} else if (features->device_type == BTN_TOOL_PEN) {
+	} else if (features->device_type & WACOM_DEVICETYPE_PEN) {
 		if (features->type <= BAMBOO_PT && features->type != WIRELESS) {
 			return wacom_set_device_mode(hdev, 2, 2, 2);
 		}
@@ -425,7 +441,6 @@ static void wacom_retrieve_hid_descriptor(struct hid_device *hdev,
 	struct usb_interface *intf = wacom->intf;
 
 	/* default features */
-	features->device_type = BTN_TOOL_PEN;
 	features->x_fuzz = 4;
 	features->y_fuzz = 4;
 	features->pressure_fuzz = 0;
@@ -439,17 +454,13 @@ static void wacom_retrieve_hid_descriptor(struct hid_device *hdev,
 	 */
 	if (features->type == WIRELESS) {
 		if (intf->cur_altsetting->desc.bInterfaceNumber == 0) {
-			features->device_type = 0;
+			features->device_type = WACOM_DEVICETYPE_NONE;
 		} else if (intf->cur_altsetting->desc.bInterfaceNumber == 2) {
-			features->device_type = BTN_TOOL_FINGER;
+			features->device_type |= WACOM_DEVICETYPE_TOUCH;
 			features->pktlen = WACOM_PKGLEN_BBTOUCH3;
 		}
 	}
 
-	/* only devices that support touch need to retrieve the info */
-	if (features->type < BAMBOO_PT)
-		return;
-
 	wacom_parse_hid(hdev, features);
 }
 
@@ -527,9 +538,9 @@ static int wacom_add_shared_data(struct hid_device *hdev)
 
 	wacom_wac->shared = &data->shared;
 
-	if (wacom_wac->features.device_type == BTN_TOOL_FINGER)
+	if (wacom_wac->features.device_type & WACOM_DEVICETYPE_TOUCH)
 		wacom_wac->shared->touch = hdev;
-	else if (wacom_wac->features.device_type == BTN_TOOL_PEN)
+	else if (wacom_wac->features.device_type & WACOM_DEVICETYPE_PEN)
 		wacom_wac->shared->pen = hdev;
 
 out:
@@ -848,6 +859,9 @@ static int wacom_initialize_leds(struct wacom *wacom)
 {
 	int error;
 
+	if (!(wacom->wacom_wac.features.device_type & WACOM_DEVICETYPE_PAD))
+		return 0;
+
 	/* Initialize default values */
 	switch (wacom->wacom_wac.features.type) {
 	case INTUOS4S:
@@ -881,17 +895,14 @@ static int wacom_initialize_leds(struct wacom *wacom)
 	case INTUOSPS:
 	case INTUOSPM:
 	case INTUOSPL:
-		if (wacom->wacom_wac.features.device_type == BTN_TOOL_PEN) {
-			wacom->led.select[0] = 0;
-			wacom->led.select[1] = 0;
-			wacom->led.llv = 32;
-			wacom->led.hlv = 0;
-			wacom->led.img_lum = 0;
-
-			error = sysfs_create_group(&wacom->hdev->dev.kobj,
-						  &intuos5_led_attr_group);
-		} else
-			return 0;
+		wacom->led.select[0] = 0;
+		wacom->led.select[1] = 0;
+		wacom->led.llv = 32;
+		wacom->led.hlv = 0;
+		wacom->led.img_lum = 0;
+
+		error = sysfs_create_group(&wacom->hdev->dev.kobj,
+					  &intuos5_led_attr_group);
 		break;
 
 	default:
@@ -914,6 +925,9 @@ static void wacom_destroy_leds(struct wacom *wacom)
 	if (!wacom->led_initialized)
 		return;
 
+	if (!(wacom->wacom_wac.features.device_type & WACOM_DEVICETYPE_PAD))
+		return;
+
 	wacom->led_initialized = false;
 
 	switch (wacom->wacom_wac.features.type) {
@@ -937,9 +951,8 @@ static void wacom_destroy_leds(struct wacom *wacom)
 	case INTUOSPS:
 	case INTUOSPM:
 	case INTUOSPL:
-		if (wacom->wacom_wac.features.device_type == BTN_TOOL_PEN)
-			sysfs_remove_group(&wacom->hdev->dev.kobj,
-					   &intuos5_led_attr_group);
+		sysfs_remove_group(&wacom->hdev->dev.kobj,
+				   &intuos5_led_attr_group);
 		break;
 	}
 }
@@ -1117,7 +1130,7 @@ static struct input_dev *wacom_allocate_input(struct wacom *wacom)
 	if (!input_dev)
 		return NULL;
 
-	input_dev->name = wacom_wac->name;
+	input_dev->name = wacom_wac->pen_name;
 	input_dev->phys = hdev->phys;
 	input_dev->dev.parent = &hdev->dev;
 	input_dev->open = wacom_open;
@@ -1136,27 +1149,33 @@ static void wacom_free_inputs(struct wacom *wacom)
 {
 	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
 
-	if (wacom_wac->input)
-		input_free_device(wacom_wac->input);
+	if (wacom_wac->pen_input)
+		input_free_device(wacom_wac->pen_input);
+	if (wacom_wac->touch_input)
+		input_free_device(wacom_wac->touch_input);
 	if (wacom_wac->pad_input)
 		input_free_device(wacom_wac->pad_input);
-	wacom_wac->input = NULL;
+	wacom_wac->pen_input = NULL;
+	wacom_wac->touch_input = NULL;
 	wacom_wac->pad_input = NULL;
 }
 
 static int wacom_allocate_inputs(struct wacom *wacom)
 {
-	struct input_dev *input_dev, *pad_input_dev;
+	struct input_dev *pen_input_dev, *touch_input_dev, *pad_input_dev;
 	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
 
-	input_dev = wacom_allocate_input(wacom);
+	pen_input_dev = wacom_allocate_input(wacom);
+	touch_input_dev = wacom_allocate_input(wacom);
 	pad_input_dev = wacom_allocate_input(wacom);
-	if (!input_dev || !pad_input_dev) {
+	if (!pen_input_dev || !touch_input_dev || !pad_input_dev) {
 		wacom_free_inputs(wacom);
 		return -ENOMEM;
 	}
 
-	wacom_wac->input = input_dev;
+	wacom_wac->pen_input = pen_input_dev;
+	wacom_wac->touch_input = touch_input_dev;
+	wacom_wac->touch_input->name = wacom_wac->touch_name;
 	wacom_wac->pad_input = pad_input_dev;
 	wacom_wac->pad_input->name = wacom_wac->pad_name;
 
@@ -1165,11 +1184,17 @@ static int wacom_allocate_inputs(struct wacom *wacom)
 
 static void wacom_clean_inputs(struct wacom *wacom)
 {
-	if (wacom->wacom_wac.input) {
-		if (wacom->wacom_wac.input_registered)
-			input_unregister_device(wacom->wacom_wac.input);
+	if (wacom->wacom_wac.pen_input) {
+		if (wacom->wacom_wac.pen_registered)
+			input_unregister_device(wacom->wacom_wac.pen_input);
 		else
-			input_free_device(wacom->wacom_wac.input);
+			input_free_device(wacom->wacom_wac.pen_input);
+	}
+	if (wacom->wacom_wac.touch_input) {
+		if (wacom->wacom_wac.touch_registered)
+			input_unregister_device(wacom->wacom_wac.touch_input);
+		else
+			input_free_device(wacom->wacom_wac.touch_input);
 	}
 	if (wacom->wacom_wac.pad_input) {
 		if (wacom->wacom_wac.pad_registered)
@@ -1177,29 +1202,49 @@ static void wacom_clean_inputs(struct wacom *wacom)
 		else
 			input_free_device(wacom->wacom_wac.pad_input);
 	}
-	wacom->wacom_wac.input = NULL;
+	wacom->wacom_wac.pen_input = NULL;
+	wacom->wacom_wac.touch_input = NULL;
 	wacom->wacom_wac.pad_input = NULL;
 	wacom_destroy_leds(wacom);
 }
 
 static int wacom_register_inputs(struct wacom *wacom)
 {
-	struct input_dev *input_dev, *pad_input_dev;
+	struct input_dev *pen_input_dev, *touch_input_dev, *pad_input_dev;
 	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
-	int error;
+	int error = 0;
 
-	input_dev = wacom_wac->input;
+	pen_input_dev = wacom_wac->pen_input;
+	touch_input_dev = wacom_wac->touch_input;
 	pad_input_dev = wacom_wac->pad_input;
 
-	if (!input_dev || !pad_input_dev)
+	if (!pen_input_dev || !touch_input_dev || !pad_input_dev)
 		return -EINVAL;
 
-	error = wacom_setup_pentouch_input_capabilities(input_dev, wacom_wac);
-	if (!error) {
-		error = input_register_device(input_dev);
+	error = wacom_setup_pen_input_capabilities(pen_input_dev, wacom_wac);
+	if (error) {
+		/* no pen in use on this interface */
+		input_free_device(pen_input_dev);
+		wacom_wac->pen_input = NULL;
+		pen_input_dev = NULL;
+	} else {
+		error = input_register_device(pen_input_dev);
+		if (error)
+			goto fail_register_pen_input;
+		wacom_wac->pen_registered = true;
+	}
+
+	error = wacom_setup_touch_input_capabilities(touch_input_dev, wacom_wac);
+	if (error) {
+		/* no touch in use on this interface */
+		input_free_device(touch_input_dev);
+		wacom_wac->touch_input = NULL;
+		touch_input_dev = NULL;
+	} else {
+		error = input_register_device(touch_input_dev);
 		if (error)
-			return error;
-		wacom_wac->input_registered = true;
+			goto fail_register_touch_input;
+		wacom_wac->touch_registered = true;
 	}
 
 	error = wacom_setup_pad_input_capabilities(pad_input_dev, wacom_wac);
@@ -1226,9 +1271,14 @@ fail_leds:
 	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;
+	input_unregister_device(touch_input_dev);
+	wacom_wac->touch_input = NULL;
+	wacom_wac->touch_registered = false;
+fail_register_touch_input:
+	input_unregister_device(pen_input_dev);
+	wacom_wac->pen_input = NULL;
+	wacom_wac->pen_registered = false;
+fail_register_pen_input:
 	return error;
 }
 
@@ -1285,8 +1335,11 @@ static void wacom_wireless_work(struct work_struct *work)
 		/* Stylus interface */
 		wacom_wac1->features =
 			*((struct wacom_features *)id->driver_data);
-		wacom_wac1->features.device_type = BTN_TOOL_PEN;
-		snprintf(wacom_wac1->name, WACOM_NAME_MAX, "%s (WL) Pen",
+		wacom_wac1->features.device_type |= WACOM_DEVICETYPE_PEN;
+		if (wacom_wac1->features.type != INTUOSHT &&
+		    wacom_wac1->features.type != BAMBOO_PT)
+			wacom_wac1->features.device_type |= WACOM_DEVICETYPE_PAD;
+		snprintf(wacom_wac1->pen_name, WACOM_NAME_MAX, "%s (WL) Pen",
 			 wacom_wac1->features.name);
 		snprintf(wacom_wac1->pad_name, WACOM_NAME_MAX, "%s (WL) Pad",
 			 wacom_wac1->features.name);
@@ -1304,16 +1357,16 @@ static void wacom_wireless_work(struct work_struct *work)
 			wacom_wac2->features =
 				*((struct wacom_features *)id->driver_data);
 			wacom_wac2->features.pktlen = WACOM_PKGLEN_BBTOUCH3;
-			wacom_wac2->features.device_type = BTN_TOOL_FINGER;
 			wacom_wac2->features.x_max = wacom_wac2->features.y_max = 4096;
-			if (wacom_wac2->features.touch_max)
-				snprintf(wacom_wac2->name, WACOM_NAME_MAX,
-					 "%s (WL) Finger",wacom_wac2->features.name);
-			else
-				snprintf(wacom_wac2->name, WACOM_NAME_MAX,
-					 "%s (WL) Pad",wacom_wac2->features.name);
+			snprintf(wacom_wac2->touch_name, WACOM_NAME_MAX,
+				 "%s (WL) Finger",wacom_wac2->features.name);
 			snprintf(wacom_wac2->pad_name, WACOM_NAME_MAX,
-				 "%s (WL) Pad", wacom_wac2->features.name);
+				 "%s (WL) Pad",wacom_wac2->features.name);
+			if (wacom_wac1->features.touch_max)
+				wacom_wac2->features.device_type |= WACOM_DEVICETYPE_TOUCH;
+			if (wacom_wac1->features.type == INTUOSHT ||
+			    wacom_wac1->features.type == BAMBOO_PT)
+				wacom_wac2->features.device_type |= WACOM_DEVICETYPE_PAD;
 			wacom_wac2->pid = wacom_wac->pid;
 			error = wacom_allocate_inputs(wacom2) ||
 				wacom_register_inputs(wacom2);
@@ -1322,7 +1375,7 @@ static void wacom_wireless_work(struct work_struct *work)
 
 			if (wacom_wac1->features.type == INTUOSHT &&
 			    wacom_wac1->features.touch_max)
-				wacom_wac->shared->touch_input = wacom_wac2->input;
+				wacom_wac->shared->touch_input = wacom_wac2->touch_input;
 		}
 
 		error = wacom_initialize_battery(wacom);
@@ -1369,6 +1422,12 @@ static void wacom_set_default_phy(struct wacom_features *features)
 
 static void wacom_calculate_res(struct wacom_features *features)
 {
+	/* set unit to "100th of a mm" for devices not reported by HID */
+	if (!features->unit) {
+		features->unit = 0x11;
+		features->unitExpo = -3;
+	}
+
 	features->x_resolution = wacom_calc_hid_res(features->x_max,
 						    features->x_phy,
 						    features->unit,
@@ -1396,6 +1455,49 @@ static size_t wacom_compute_pktlen(struct hid_device *hdev)
 	return size;
 }
 
+static void wacom_update_name(struct wacom *wacom)
+{
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct wacom_features *features = &wacom_wac->features;
+	char name[WACOM_NAME_MAX];
+
+	/* Generic devices name unspecified */
+	if ((features->type == HID_GENERIC) && !strcmp("Wacom HID", features->name)) {
+		if (strstr(wacom->hdev->name, "Wacom") ||
+		    strstr(wacom->hdev->name, "wacom") ||
+		    strstr(wacom->hdev->name, "WACOM")) {
+			/* name is in HID descriptor, use it */
+			strlcpy(name, wacom->hdev->name, sizeof(name));
+
+			/* strip out excess whitespaces */
+			while (1) {
+				char *gap = strstr(name, "  ");
+				if (gap == NULL)
+					break;
+				/* shift everything including the terminator */
+				memmove(gap, gap+1, strlen(gap));
+			}
+			/* get rid of trailing whitespace */
+			if (name[strlen(name)-1] == ' ')
+				name[strlen(name)-1] = '\0';
+		} else {
+			/* no meaningful name retrieved. use product ID */
+			snprintf(name, sizeof(name),
+				 "%s %X", features->name, wacom->hdev->product);
+		}
+	} else {
+		strlcpy(name, features->name, sizeof(name));
+	}
+
+	/* Append the device type to the name */
+	snprintf(wacom_wac->pen_name, sizeof(wacom_wac->pen_name),
+		"%s Pen", name);
+	snprintf(wacom_wac->touch_name, sizeof(wacom_wac->touch_name),
+		"%s Finger", name);
+	snprintf(wacom_wac->pad_name, sizeof(wacom_wac->pad_name),
+		"%s Pad", name);
+}
+
 static int wacom_probe(struct hid_device *hdev,
 		const struct hid_device_id *id)
 {
@@ -1474,64 +1576,25 @@ static int wacom_probe(struct hid_device *hdev,
 
 	/* Retrieve the physical and logical size for touch devices */
 	wacom_retrieve_hid_descriptor(hdev, features);
+	wacom_setup_device_quirks(wacom);
 
-	/*
-	 * Intuos5 has no useful data about its touch interface in its
-	 * HID descriptor. If this is the touch interface (PacketSize
-	 * of WACOM_PKGLEN_BBTOUCH3), override the table values.
-	 */
-	if (features->type >= INTUOS5S && features->type <= INTUOSHT) {
-		if (features->pktlen == WACOM_PKGLEN_BBTOUCH3) {
-			features->device_type = BTN_TOOL_FINGER;
+	if (features->device_type == WACOM_DEVICETYPE_NONE &&
+	    features->type != WIRELESS) {
+		error = features->type == HID_GENERIC ? -ENODEV : 0;
 
-			features->x_max = 4096;
-			features->y_max = 4096;
-		} else {
-			features->device_type = BTN_TOOL_PEN;
-		}
-	}
+		dev_warn(&hdev->dev, "Unknown device_type for '%s'. %s.",
+			 hdev->name,
+			 error ? "Ignoring" : "Assuming pen");
 
-	/*
-	 * Same thing for Bamboo 3rd gen.
-	 */
-	if ((features->type == BAMBOO_PT) &&
-	    (features->pktlen == WACOM_PKGLEN_BBTOUCH3) &&
-	    (features->device_type == BTN_TOOL_PEN)) {
-		features->device_type = BTN_TOOL_FINGER;
+		if (error)
+			goto fail_shared_data;
 
-		features->x_max = 4096;
-		features->y_max = 4096;
+		features->device_type |= WACOM_DEVICETYPE_PEN;
 	}
 
-	/*
-	 * Same thing for Bamboo PAD
-	 */
-	if (features->type == BAMBOO_PAD)
-		features->device_type = BTN_TOOL_FINGER;
-
-	if (hdev->bus == BUS_BLUETOOTH)
-		features->quirks |= WACOM_QUIRK_BATTERY;
-
-	wacom_setup_device_quirks(features);
-
-	/* set unit to "100th of a mm" for devices not reported by HID */
-	if (!features->unit) {
-		features->unit = 0x11;
-		features->unitExpo = -3;
-	}
 	wacom_calculate_res(features);
 
-	strlcpy(wacom_wac->name, features->name, sizeof(wacom_wac->name));
-	snprintf(wacom_wac->pad_name, sizeof(wacom_wac->pad_name),
-		"%s Pad", features->name);
-
-	/* Append the device type to the name */
-	if (features->device_type != BTN_TOOL_FINGER)
-		strlcat(wacom_wac->name, " Pen", WACOM_NAME_MAX);
-	else if (features->touch_max)
-		strlcat(wacom_wac->name, " Finger", WACOM_NAME_MAX);
-	else
-		strlcat(wacom_wac->name, " Pad", WACOM_NAME_MAX);
+	wacom_update_name(wacom);
 
 	error = wacom_add_shared_data(hdev);
 	if (error)
@@ -1574,9 +1637,9 @@ static int wacom_probe(struct hid_device *hdev,
 	if (features->quirks & WACOM_QUIRK_MONITOR)
 		error = hid_hw_open(hdev);
 
-	if (wacom_wac->features.type == INTUOSHT && wacom_wac->features.touch_max) {
-		if (wacom_wac->features.device_type == BTN_TOOL_FINGER)
-			wacom_wac->shared->touch_input = wacom_wac->input;
+	if (wacom_wac->features.type == INTUOSHT && 
+	    wacom_wac->features.device_type & WACOM_DEVICETYPE_TOUCH) {
+			wacom_wac->shared->touch_input = wacom_wac->touch_input;
 	}
 
 	return 0;
diff --git a/drivers/hid/wacom_wac.c b/drivers/hid/wacom_wac.c
index adf959dcfa5d..232da89f4e88 100644
--- a/drivers/hid/wacom_wac.c
+++ b/drivers/hid/wacom_wac.c
@@ -69,7 +69,7 @@ static void wacom_notify_battery(struct wacom_wac *wacom_wac,
 static int wacom_penpartner_irq(struct wacom_wac *wacom)
 {
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 
 	switch (data[0]) {
 	case 1:
@@ -114,7 +114,7 @@ static int wacom_pl_irq(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	int prox, pressure;
 
 	if (data[0] != WACOM_REPORT_PENABLED) {
@@ -186,7 +186,7 @@ static int wacom_pl_irq(struct wacom_wac *wacom)
 static int wacom_ptu_irq(struct wacom_wac *wacom)
 {
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 
 	if (data[0] != WACOM_REPORT_PENABLED) {
 		dev_dbg(input->dev.parent,
@@ -215,7 +215,7 @@ static int wacom_ptu_irq(struct wacom_wac *wacom)
 static int wacom_dtu_irq(struct wacom_wac *wacom)
 {
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	int prox = data[1] & 0x20;
 
 	dev_dbg(input->dev.parent,
@@ -245,7 +245,7 @@ static int wacom_dtu_irq(struct wacom_wac *wacom)
 static int wacom_dtus_irq(struct wacom_wac *wacom)
 {
 	char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	unsigned short prox, pressure = 0;
 
 	if (data[0] != WACOM_REPORT_DTUS && data[0] != WACOM_REPORT_DTUSPAD) {
@@ -297,7 +297,7 @@ static int wacom_graphire_irq(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	struct input_dev *pad_input = wacom->pad_input;
 	int battery_capacity, ps_connected;
 	int prox;
@@ -464,7 +464,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	int idx = 0;
 
 	/* tool number */
@@ -649,7 +649,7 @@ static void wacom_intuos_general(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	unsigned int t;
 
 	/* general pen packet */
@@ -681,7 +681,7 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	unsigned int t;
 	int idx = 0, result;
 
@@ -1025,7 +1025,7 @@ static void wacom_intuos_bt_process_data(struct wacom_wac *wacom,
 	memcpy(wacom->data, data, 10);
 	wacom_intuos_irq(wacom);
 
-	input_sync(wacom->input);
+	input_sync(wacom->pen_input);
 	if (wacom->pad_input)
 		input_sync(wacom->pad_input);
 }
@@ -1057,7 +1057,7 @@ static int wacom_intuos_bt_irq(struct wacom_wac *wacom, size_t len)
 				     ps_connected);
 		break;
 	default:
-		dev_dbg(wacom->input->dev.parent,
+		dev_dbg(wacom->pen_input->dev.parent,
 				"Unknown report: %d,%d size:%zu\n",
 				data[0], data[1], len);
 		return 0;
@@ -1067,7 +1067,7 @@ static int wacom_intuos_bt_irq(struct wacom_wac *wacom, size_t len)
 
 static int wacom_wac_finger_count_touches(struct wacom_wac *wacom)
 {
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->touch_input;
 	unsigned touch_max = wacom->features.touch_max;
 	int count = 0;
 	int i;
@@ -1075,9 +1075,8 @@ static int wacom_wac_finger_count_touches(struct wacom_wac *wacom)
 	if (!touch_max)
 		return 0;
 
-	/* non-HID_GENERIC single touch input doesn't call this routine */
-	if ((touch_max == 1) && (wacom->features.type == HID_GENERIC))
-		return wacom->hid_data.tipswitch &&
+	if (touch_max == 1)
+		return test_bit(BTN_TOUCH, input->key) &&
 		       !wacom->shared->stylus_in_proximity;
 
 	for (i = 0; i < input->mt->num_slots; i++) {
@@ -1092,7 +1091,7 @@ static int wacom_wac_finger_count_touches(struct wacom_wac *wacom)
 
 static int wacom_24hdt_irq(struct wacom_wac *wacom)
 {
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->touch_input;
 	unsigned char *data = wacom->data;
 	int i;
 	int current_num_contacts = data[61];
@@ -1160,7 +1159,7 @@ static int wacom_24hdt_irq(struct wacom_wac *wacom)
 
 static int wacom_mt_touch(struct wacom_wac *wacom)
 {
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->touch_input;
 	unsigned char *data = wacom->data;
 	int i;
 	int current_num_contacts = data[2];
@@ -1211,7 +1210,7 @@ static int wacom_mt_touch(struct wacom_wac *wacom)
 
 static int wacom_tpc_mt_touch(struct wacom_wac *wacom)
 {
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->touch_input;
 	unsigned char *data = wacom->data;
 	int i;
 
@@ -1240,7 +1239,7 @@ static int wacom_tpc_mt_touch(struct wacom_wac *wacom)
 static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 {
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->touch_input;
 	bool prox = !wacom->shared->stylus_in_proximity;
 	int x = 0, y = 0;
 
@@ -1276,7 +1275,7 @@ static int wacom_tpc_single_touch(struct wacom_wac *wacom, size_t len)
 static int wacom_tpc_pen(struct wacom_wac *wacom)
 {
 	unsigned char *data = wacom->data;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	bool prox = data[1] & 0x20;
 
 	if (!wacom->shared->stylus_in_proximity) /* first in prox */
@@ -1305,8 +1304,12 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 {
 	unsigned char *data = wacom->data;
 
-	dev_dbg(wacom->input->dev.parent,
-		"%s: received report #%d\n", __func__, data[0]);
+	if (wacom->pen_input)
+		dev_dbg(wacom->pen_input->dev.parent,
+			"%s: received report #%d\n", __func__, data[0]);
+	else if (wacom->touch_input)
+		dev_dbg(wacom->touch_input->dev.parent,
+			"%s: received report #%d\n", __func__, data[0]);
 
 	switch (len) {
 	case WACOM_PKGLEN_TPC1FG:
@@ -1338,11 +1341,9 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 	return 0;
 }
 
-static void wacom_map_usage(struct wacom *wacom, struct hid_usage *usage,
+static void wacom_map_usage(struct input_dev *input, struct hid_usage *usage,
 		struct hid_field *field, __u8 type, __u16 code, int fuzz)
 {
-	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
-	struct input_dev *input = wacom_wac->input;
 	int fmin = field->logical_minimum;
 	int fmax = field->logical_maximum;
 
@@ -1370,36 +1371,38 @@ static void wacom_wac_pen_usage_mapping(struct hid_device *hdev,
 		struct hid_field *field, struct hid_usage *usage)
 {
 	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct input_dev *input = wacom_wac->pen_input;
 
 	switch (usage->hid) {
 	case HID_GD_X:
-		wacom_map_usage(wacom, usage, field, EV_ABS, ABS_X, 4);
+		wacom_map_usage(input, usage, field, EV_ABS, ABS_X, 4);
 		break;
 	case HID_GD_Y:
-		wacom_map_usage(wacom, usage, field, EV_ABS, ABS_Y, 4);
+		wacom_map_usage(input, usage, field, EV_ABS, ABS_Y, 4);
 		break;
 	case HID_DG_TIPPRESSURE:
-		wacom_map_usage(wacom, usage, field, EV_ABS, ABS_PRESSURE, 0);
+		wacom_map_usage(input, usage, field, EV_ABS, ABS_PRESSURE, 0);
 		break;
 	case HID_DG_INRANGE:
-		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_TOOL_PEN, 0);
+		wacom_map_usage(input, usage, field, EV_KEY, BTN_TOOL_PEN, 0);
 		break;
 	case HID_DG_INVERT:
-		wacom_map_usage(wacom, usage, field, EV_KEY,
+		wacom_map_usage(input, usage, field, EV_KEY,
 				BTN_TOOL_RUBBER, 0);
 		break;
 	case HID_DG_ERASER:
 	case HID_DG_TIPSWITCH:
-		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_TOUCH, 0);
+		wacom_map_usage(input, usage, field, EV_KEY, BTN_TOUCH, 0);
 		break;
 	case HID_DG_BARRELSWITCH:
-		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_STYLUS, 0);
+		wacom_map_usage(input, usage, field, EV_KEY, BTN_STYLUS, 0);
 		break;
 	case HID_DG_BARRELSWITCH2:
-		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_STYLUS2, 0);
+		wacom_map_usage(input, usage, field, EV_KEY, BTN_STYLUS2, 0);
 		break;
 	case HID_DG_TOOLSERIALNUMBER:
-		wacom_map_usage(wacom, usage, field, EV_MSC, MSC_SERIAL, 0);
+		wacom_map_usage(input, usage, field, EV_MSC, MSC_SERIAL, 0);
 		break;
 	}
 }
@@ -1409,7 +1412,7 @@ static int wacom_wac_pen_event(struct hid_device *hdev, struct hid_field *field,
 {
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
-	struct input_dev *input = wacom_wac->input;
+	struct input_dev *input = wacom_wac->pen_input;
 
 	/* checking which Tool / tip switch to send */
 	switch (usage->hid) {
@@ -1439,7 +1442,7 @@ static void wacom_wac_pen_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;
+	struct input_dev *input = wacom_wac->pen_input;
 	bool prox = wacom_wac->hid_data.inrange_state;
 
 	if (!wacom_wac->shared->stylus_in_proximity) /* first in prox */
@@ -1468,23 +1471,24 @@ 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 wacom_features *features = &wacom_wac->features;
+	struct input_dev *input = wacom_wac->touch_input;
 	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);
+			wacom_map_usage(input, usage, field, EV_ABS, ABS_X, 4);
 		else
-			wacom_map_usage(wacom, usage, field, EV_ABS,
+			wacom_map_usage(input, usage, field, EV_ABS,
 					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);
+			wacom_map_usage(input, usage, field, EV_ABS, ABS_Y, 4);
 		else
-			wacom_map_usage(wacom, usage, field, EV_ABS,
+			wacom_map_usage(input, usage, field, EV_ABS,
 					ABS_MT_POSITION_Y, 4);
 		break;
 	case HID_DG_CONTACTID:
@@ -1498,7 +1502,7 @@ static void wacom_wac_finger_usage_mapping(struct hid_device *hdev,
 		break;
 	case HID_DG_TIPSWITCH:
 		features->last_slot_field = usage->hid;
-		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_TOUCH, 0);
+		wacom_map_usage(input, usage, field, EV_KEY, BTN_TOUCH, 0);
 		break;
 	}
 }
@@ -1554,7 +1558,7 @@ 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);
+			wacom_wac_finger_slot(wacom_wac, wacom_wac->touch_input);
 	}
 
 	return 0;
@@ -1565,7 +1569,7 @@ 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;
+	struct input_dev *input = wacom_wac->touch_input;
 	unsigned touch_max = wacom_wac->features.touch_max;
 
 	if (touch_max > 1)
@@ -1582,10 +1586,10 @@ void wacom_wac_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;
 
 	/* currently, only direct devices have proper hid report descriptors */
-	__set_bit(INPUT_PROP_DIRECT, input->propbit);
+	__set_bit(INPUT_PROP_DIRECT, wacom_wac->pen_input->propbit);
+	__set_bit(INPUT_PROP_DIRECT, wacom_wac->touch_input->propbit);
 
 	if (WACOM_PEN_FIELD(field))
 		return wacom_wac_pen_usage_mapping(hdev, field, usage);
@@ -1630,7 +1634,7 @@ void wacom_wac_report(struct hid_device *hdev, struct hid_report *report)
 static int wacom_bpt_touch(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->touch_input;
 	struct input_dev *pad_input = wacom->pad_input;
 	unsigned char *data = wacom->data;
 	int i;
@@ -1678,7 +1682,7 @@ static int wacom_bpt_touch(struct wacom_wac *wacom)
 static void wacom_bpt3_touch_msg(struct wacom_wac *wacom, unsigned char *data)
 {
 	struct wacom_features *features = &wacom->features;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->touch_input;
 	bool touch = data[1] & 0x80;
 	int slot = input_mt_get_slot_by_key(input, data[0]);
 
@@ -1736,7 +1740,6 @@ static void wacom_bpt3_button_msg(struct wacom_wac *wacom, unsigned char *data)
 
 static int wacom_bpt3_touch(struct wacom_wac *wacom)
 {
-	struct input_dev *input = wacom->input;
 	unsigned char *data = wacom->data;
 	int count = data[1] & 0x07;
 	int i;
@@ -1755,8 +1758,12 @@ static int wacom_bpt3_touch(struct wacom_wac *wacom)
 			wacom_bpt3_button_msg(wacom, data + offset);
 
 	}
-	input_mt_sync_frame(input);
-	wacom->shared->touch_down = wacom_wac_finger_count_touches(wacom);
+
+	/* only update the touch if we actually have a touchpad */
+	if (wacom->touch_registered) {
+		input_mt_sync_frame(wacom->touch_input);
+		wacom->shared->touch_down = wacom_wac_finger_count_touches(wacom);
+	}
 
 	return 1;
 }
@@ -1764,7 +1771,7 @@ static int wacom_bpt3_touch(struct wacom_wac *wacom)
 static int wacom_bpt_pen(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->pen_input;
 	unsigned char *data = wacom->data;
 	int prox = 0, x = 0, y = 0, p = 0, d = 0, pen = 0, btn1 = 0, btn2 = 0;
 
@@ -1873,7 +1880,7 @@ static void wacom_bamboo_pad_pen_event(struct wacom_wac *wacom,
 static int wacom_bamboo_pad_touch_event(struct wacom_wac *wacom,
 		unsigned char *data)
 {
-	struct input_dev *input = wacom->input;
+	struct input_dev *input = wacom->touch_input;
 	unsigned char *finger_data, prefix;
 	unsigned id;
 	int x, y;
@@ -2117,7 +2124,10 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 	}
 
 	if (sync) {
-		input_sync(wacom_wac->input);
+		if (wacom_wac->pen_input)
+			input_sync(wacom_wac->pen_input);
+		if (wacom_wac->touch_input)
+			input_sync(wacom_wac->touch_input);
 		if (wacom_wac->pad_input)
 			input_sync(wacom_wac->pad_input);
 	}
@@ -2125,7 +2135,7 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
 
 static void wacom_setup_cintiq(struct wacom_wac *wacom_wac)
 {
-	struct input_dev *input_dev = wacom_wac->input;
+	struct input_dev *input_dev = wacom_wac->pen_input;
 
 	input_set_capability(input_dev, EV_MSC, MSC_SERIAL);
 
@@ -2148,7 +2158,7 @@ static void wacom_setup_cintiq(struct wacom_wac *wacom_wac)
 
 static void wacom_setup_intuos(struct wacom_wac *wacom_wac)
 {
-	struct input_dev *input_dev = wacom_wac->input;
+	struct input_dev *input_dev = wacom_wac->pen_input;
 
 	input_set_capability(input_dev, EV_REL, REL_WHEEL);
 
@@ -2167,15 +2177,57 @@ static void wacom_setup_intuos(struct wacom_wac *wacom_wac)
 	input_set_abs_params(input_dev, ABS_THROTTLE, -1023, 1023, 0, 0);
 }
 
-void wacom_setup_device_quirks(struct wacom_features *features)
+void wacom_setup_device_quirks(struct wacom *wacom)
 {
+	struct wacom_features *features = &wacom->wacom_wac.features;
+
+	/* The pen and pad share the same interface on most devices */
+	if (features->type == GRAPHIRE_BT || features->type == WACOM_G4 ||
+	    features->type == DTUS || features->type == WACOM_MO ||
+	    (features->type >= INTUOS3S && features->type <= WACOM_13HD && 
+	     features->type != INTUOSHT)) {
+		if (features->device_type & WACOM_DEVICETYPE_PEN)
+			features->device_type |= WACOM_DEVICETYPE_PAD;
+	}
 
 	/* touch device found but size is not defined. use default */
-	if (features->device_type == BTN_TOOL_FINGER && !features->x_max) {
+	if (features->device_type & WACOM_DEVICETYPE_TOUCH && !features->x_max) {
 		features->x_max = 1023;
 		features->y_max = 1023;
 	}
 
+	/*
+	 * Intuos5/Pro and Bamboo 3rd gen have no useful data about its
+	 * touch interface in its HID descriptor. If this is the touch
+	 * interface (PacketSize of WACOM_PKGLEN_BBTOUCH3), override the
+	 * tablet values.
+	 */
+	if ((features->type >= INTUOS5S && features->type <= INTUOSHT) ||
+		(features->type == BAMBOO_PT)) {
+		if (features->pktlen == WACOM_PKGLEN_BBTOUCH3) {
+			if (features->touch_max)
+				features->device_type |= WACOM_DEVICETYPE_TOUCH;
+			if (features->type == BAMBOO_PT || features->type == INTUOSHT)
+				features->device_type |= WACOM_DEVICETYPE_PAD;
+
+			features->x_max = 4096;
+			features->y_max = 4096;
+		}
+	}
+
+	/*
+	 * Raw Wacom-mode pen and touch events both come from interface
+	 * 0, whose HID descriptor has an application usage of 0xFF0D
+	 * (i.e., WACOM_VENDORDEFINED_PEN). We route pen packets back
+	 * out through the HID_GENERIC device created for interface 1,
+	 * so rewrite this one to be of type BTN_TOOL_FINGER.
+	 */
+	if (features->type == BAMBOO_PAD)
+		features->device_type |= WACOM_DEVICETYPE_TOUCH;
+
+	if (wacom->hdev->bus == BUS_BLUETOOTH)
+		features->quirks |= WACOM_QUIRK_BATTERY;
+
 	/* quirk for bamboo touch with 2 low res touches */
 	if (features->type == BAMBOO_PT &&
 	    features->pktlen == WACOM_PKGLEN_BBTOUCH) {
@@ -2192,61 +2244,23 @@ void wacom_setup_device_quirks(struct wacom_features *features)
 		features->quirks |= WACOM_QUIRK_NO_INPUT;
 
 		/* must be monitor interface if no device_type set */
-		if (!features->device_type) {
+		if (features->device_type == WACOM_DEVICETYPE_NONE) {
 			features->quirks |= WACOM_QUIRK_MONITOR;
 			features->quirks |= WACOM_QUIRK_BATTERY;
 		}
 	}
 }
 
-static void wacom_abs_set_axis(struct input_dev *input_dev,
-			       struct wacom_wac *wacom_wac)
-{
-	struct wacom_features *features = &wacom_wac->features;
-
-	if (features->device_type == BTN_TOOL_PEN) {
-		input_set_abs_params(input_dev, ABS_X, features->x_min,
-				     features->x_max, features->x_fuzz, 0);
-		input_set_abs_params(input_dev, ABS_Y, features->y_min,
-				     features->y_max, features->y_fuzz, 0);
-		input_set_abs_params(input_dev, ABS_PRESSURE, 0,
-			features->pressure_max, features->pressure_fuzz, 0);
-
-		/* penabled devices have fixed resolution for each model */
-		input_abs_set_res(input_dev, ABS_X, features->x_resolution);
-		input_abs_set_res(input_dev, ABS_Y, features->y_resolution);
-	} else {
-		if (features->touch_max == 1) {
-			input_set_abs_params(input_dev, ABS_X, 0,
-				features->x_max, features->x_fuzz, 0);
-			input_set_abs_params(input_dev, ABS_Y, 0,
-				features->y_max, features->y_fuzz, 0);
-			input_abs_set_res(input_dev, ABS_X,
-					  features->x_resolution);
-			input_abs_set_res(input_dev, ABS_Y,
-					  features->y_resolution);
-		}
-
-		if (features->touch_max > 1) {
-			input_set_abs_params(input_dev, ABS_MT_POSITION_X, 0,
-				features->x_max, features->x_fuzz, 0);
-			input_set_abs_params(input_dev, ABS_MT_POSITION_Y, 0,
-				features->y_max, features->y_fuzz, 0);
-			input_abs_set_res(input_dev, ABS_MT_POSITION_X,
-					  features->x_resolution);
-			input_abs_set_res(input_dev, ABS_MT_POSITION_Y,
-					  features->y_resolution);
-		}
-	}
-}
-
-int wacom_setup_pentouch_input_capabilities(struct input_dev *input_dev,
+int wacom_setup_pen_input_capabilities(struct input_dev *input_dev,
 				   struct wacom_wac *wacom_wac)
 {
 	struct wacom_features *features = &wacom_wac->features;
 
 	input_dev->evbit[0] |= BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
 
+	if (!(features->device_type & WACOM_DEVICETYPE_PEN))
+		return -ENODEV;
+
 	if (features->type == HID_GENERIC)
 		/* setup has already been done */
 		return 0;
@@ -2254,7 +2268,17 @@ int wacom_setup_pentouch_input_capabilities(struct input_dev *input_dev,
 	__set_bit(BTN_TOUCH, input_dev->keybit);
 	__set_bit(ABS_MISC, input_dev->absbit);
 
-	wacom_abs_set_axis(input_dev, wacom_wac);
+	input_set_abs_params(input_dev, ABS_X, features->x_min,
+			     features->x_max, features->x_fuzz, 0);
+	input_set_abs_params(input_dev, ABS_Y, features->y_min,
+			     features->y_max, features->y_fuzz, 0);
+	input_set_abs_params(input_dev, ABS_PRESSURE, 0,
+		features->pressure_max, features->pressure_fuzz, 0);
+
+	/* penabled devices have fixed resolution for each model */
+	input_abs_set_res(input_dev, ABS_X, features->x_resolution);
+	input_abs_set_res(input_dev, ABS_Y, features->y_resolution);
+
 
 	switch (features->type) {
 	case GRAPHIRE_BT:
@@ -2323,53 +2347,25 @@ int wacom_setup_pentouch_input_capabilities(struct input_dev *input_dev,
 	case INTUOSPS:
 		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
 
-		if (features->device_type == BTN_TOOL_PEN) {
-			input_set_abs_params(input_dev, ABS_DISTANCE, 0,
-					      features->distance_max,
-					      0, 0);
-
-			input_set_abs_params(input_dev, ABS_Z, -900, 899, 0, 0);
-			input_abs_set_res(input_dev, ABS_Z, 287);
+		input_set_abs_params(input_dev, ABS_DISTANCE, 0,
+				      features->distance_max,
+				      0, 0);
 
-			wacom_setup_intuos(wacom_wac);
-		} else if (features->device_type == BTN_TOOL_FINGER) {
-			__clear_bit(ABS_MISC, input_dev->absbit);
+		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_MT_TOUCH_MAJOR,
-			                     0, features->x_max, 0, 0);
-			input_set_abs_params(input_dev, ABS_MT_TOUCH_MINOR,
-			                     0, features->y_max, 0, 0);
-			input_mt_init_slots(input_dev, features->touch_max, INPUT_MT_POINTER);
-		}
+		wacom_setup_intuos(wacom_wac);
 		break;
 
 	case WACOM_24HDT:
-		if (features->device_type == BTN_TOOL_FINGER) {
-			input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR, 0, features->x_max, 0, 0);
-			input_set_abs_params(input_dev, ABS_MT_WIDTH_MAJOR, 0, features->x_max, 0, 0);
-			input_set_abs_params(input_dev, ABS_MT_WIDTH_MINOR, 0, features->y_max, 0, 0);
-			input_set_abs_params(input_dev, ABS_MT_ORIENTATION, 0, 1, 0, 0);
-		}
-		/* fall through */
-
 	case WACOM_27QHDT:
 	case MTSCREEN:
 	case MTTPC:
 	case MTTPC_B:
 	case TABLETPC2FG:
-		if (features->device_type == BTN_TOOL_FINGER && features->touch_max > 1)
-			input_mt_init_slots(input_dev, features->touch_max, INPUT_MT_DIRECT);
-		/* fall through */
-
 	case TABLETPC:
 	case TABLETPCE:
 		__clear_bit(ABS_MISC, input_dev->absbit);
-
-		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
-
-		if (features->device_type != BTN_TOOL_PEN)
-			break;  /* no need to process stylus stuff */
-
 		/* fall through */
 
 	case DTUS:
@@ -2397,50 +2393,114 @@ int wacom_setup_pentouch_input_capabilities(struct input_dev *input_dev,
 		break;
 
 	case INTUOSHT:
-		if (features->touch_max &&
-		    features->device_type == BTN_TOOL_FINGER) {
-			input_dev->evbit[0] |= BIT_MASK(EV_SW);
-			__set_bit(SW_MUTE_DEVICE, input_dev->swbit);
-		}
-		/* fall through */
-
 	case BAMBOO_PT:
 		__clear_bit(ABS_MISC, input_dev->absbit);
 
-		if (features->device_type == BTN_TOOL_FINGER) {
+		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+		__set_bit(BTN_TOOL_RUBBER, input_dev->keybit);
+		__set_bit(BTN_TOOL_PEN, input_dev->keybit);
+		__set_bit(BTN_STYLUS, input_dev->keybit);
+		__set_bit(BTN_STYLUS2, input_dev->keybit);
+		input_set_abs_params(input_dev, ABS_DISTANCE, 0,
+				      features->distance_max,
+				      0, 0);
+		break;
+	case BAMBOO_PAD:
+		__clear_bit(ABS_MISC, input_dev->absbit);
+		break;
+	}
+	return 0;
+}
 
-			if (features->touch_max) {
-				if (features->pktlen == WACOM_PKGLEN_BBTOUCH3) {
-					input_set_abs_params(input_dev,
-						     ABS_MT_TOUCH_MAJOR,
-						     0, features->x_max, 0, 0);
-					input_set_abs_params(input_dev,
-						     ABS_MT_TOUCH_MINOR,
-						     0, features->y_max, 0, 0);
-				}
-				input_mt_init_slots(input_dev, features->touch_max, INPUT_MT_POINTER);
-			} else {
-				/* buttons/keys only interface */
-				__clear_bit(ABS_X, input_dev->absbit);
-				__clear_bit(ABS_Y, input_dev->absbit);
-				__clear_bit(BTN_TOUCH, input_dev->keybit);
+int wacom_setup_touch_input_capabilities(struct input_dev *input_dev,
+					 struct wacom_wac *wacom_wac)
+{
+	struct wacom_features *features = &wacom_wac->features;
 
-				/* 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);
-			__set_bit(BTN_TOOL_RUBBER, input_dev->keybit);
-			__set_bit(BTN_TOOL_PEN, input_dev->keybit);
-			__set_bit(BTN_STYLUS, input_dev->keybit);
-			__set_bit(BTN_STYLUS2, input_dev->keybit);
-			input_set_abs_params(input_dev, ABS_DISTANCE, 0,
-					      features->distance_max,
-					      0, 0);
+	input_dev->evbit[0] |= BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+
+	if (!(features->device_type & WACOM_DEVICETYPE_TOUCH))
+		return -ENODEV;
+
+	if (features->type == HID_GENERIC)
+		/* setup has already been done */
+		return 0;
+
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	if (features->touch_max == 1) {
+		input_set_abs_params(input_dev, ABS_X, 0,
+			features->x_max, features->x_fuzz, 0);
+		input_set_abs_params(input_dev, ABS_Y, 0,
+			features->y_max, features->y_fuzz, 0);
+		input_abs_set_res(input_dev, ABS_X,
+				  features->x_resolution);
+		input_abs_set_res(input_dev, ABS_Y,
+				  features->y_resolution);
+	}
+	else if (features->touch_max > 1) {
+		input_set_abs_params(input_dev, ABS_MT_POSITION_X, 0,
+			features->x_max, features->x_fuzz, 0);
+		input_set_abs_params(input_dev, ABS_MT_POSITION_Y, 0,
+			features->y_max, features->y_fuzz, 0);
+		input_abs_set_res(input_dev, ABS_MT_POSITION_X,
+				  features->x_resolution);
+		input_abs_set_res(input_dev, ABS_MT_POSITION_Y,
+				  features->y_resolution);
+	}
+
+	switch (features->type) {
+	case INTUOS5:
+	case INTUOS5L:
+	case INTUOSPM:
+	case INTUOSPL:
+	case INTUOS5S:
+	case INTUOSPS:
+		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+
+		input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR, 0, features->x_max, 0, 0);
+		input_set_abs_params(input_dev, ABS_MT_TOUCH_MINOR, 0, features->y_max, 0, 0);
+		input_mt_init_slots(input_dev, features->touch_max, INPUT_MT_POINTER);
+		break;
+
+	case WACOM_24HDT:
+		input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR, 0, features->x_max, 0, 0);
+		input_set_abs_params(input_dev, ABS_MT_WIDTH_MAJOR, 0, features->x_max, 0, 0);
+		input_set_abs_params(input_dev, ABS_MT_WIDTH_MINOR, 0, features->y_max, 0, 0);
+		input_set_abs_params(input_dev, ABS_MT_ORIENTATION, 0, 1, 0, 0);
+		/* fall through */
+
+	case WACOM_27QHDT:
+	case MTSCREEN:
+	case MTTPC:
+	case MTTPC_B:
+	case TABLETPC2FG:
+		input_mt_init_slots(input_dev, features->touch_max, INPUT_MT_DIRECT);
+		/*fall through */
+
+	case TABLETPC:
+	case TABLETPCE:
+		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
+		break;
+
+	case INTUOSHT:
+		input_dev->evbit[0] |= BIT_MASK(EV_SW);
+		__set_bit(SW_MUTE_DEVICE, input_dev->swbit);
+		/* fall through */
+
+	case BAMBOO_PT:
+		if (features->pktlen == WACOM_PKGLEN_BBTOUCH3) {
+			input_set_abs_params(input_dev,
+				     ABS_MT_TOUCH_MAJOR,
+				     0, features->x_max, 0, 0);
+			input_set_abs_params(input_dev,
+				     ABS_MT_TOUCH_MINOR,
+				     0, features->y_max, 0, 0);
 		}
+		input_mt_init_slots(input_dev, features->touch_max, INPUT_MT_POINTER);
 		break;
+
 	case BAMBOO_PAD:
-		__clear_bit(ABS_MISC, input_dev->absbit);
 		input_mt_init_slots(input_dev, features->touch_max,
 				    INPUT_MT_POINTER);
 		__set_bit(BTN_LEFT, input_dev->keybit);
@@ -2456,6 +2516,9 @@ int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 	struct wacom_features *features = &wacom_wac->features;
 	int i;
 
+	if (!(features->device_type & WACOM_DEVICETYPE_PAD))
+		return -ENODEV;
+
 	input_dev->evbit[0] |= BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
 
 	/* kept for making legacy xf86-input-wacom working with the wheels */
@@ -2592,10 +2655,6 @@ int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 
 	case INTUOS5S:
 	case INTUOSPS:
-		/* touch interface does not have the pad device */
-		if (features->device_type != BTN_TOOL_PEN)
-			return -ENODEV;
-
 		for (i = 0; i < 7; i++)
 			__set_bit(BTN_0 + i, input_dev->keybit);
 
@@ -2637,12 +2696,6 @@ 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) ||
-		    /* Bamboo Pen only tablet does not have pad */
-		    ((features->type == BAMBOO_PT) && !features->touch_max))
-			return -ENODEV;
-
 		__clear_bit(ABS_MISC, input_dev->absbit);
 
 		__set_bit(BTN_LEFT, input_dev->keybit);
@@ -2922,6 +2975,9 @@ 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_0x336 =
+	{ "Wacom DTU1141", 23472, 13203, 1023, 0,
+	  DTUS, WACOM_INTUOS_RES, WACOM_INTUOS_RES };
 static const struct wacom_features wacom_features_0x57 =
 	{ "Wacom DTK2241", 95640, 54060, 2047, 63,
 	  DTK, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES,
@@ -3275,6 +3331,7 @@ const struct hid_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x32F) },
 	{ USB_DEVICE_WACOM(0x333) },
 	{ USB_DEVICE_WACOM(0x335) },
+	{ USB_DEVICE_WACOM(0x336) },
 	{ USB_DEVICE_WACOM(0x4001) },
 	{ USB_DEVICE_WACOM(0x4004) },
 	{ USB_DEVICE_WACOM(0x5000) },
diff --git a/drivers/hid/wacom_wac.h b/drivers/hid/wacom_wac.h
index 4700ac994a3b..2978c303909d 100644
--- a/drivers/hid/wacom_wac.h
+++ b/drivers/hid/wacom_wac.h
@@ -18,10 +18,7 @@
 #define WACOM_NAME_MAX		64
 
 /* packet length for individual models */
-#define WACOM_PKGLEN_PENPRTN	 7
-#define WACOM_PKGLEN_GRAPHIRE	 8
 #define WACOM_PKGLEN_BBFUN	 9
-#define WACOM_PKGLEN_INTUOS	10
 #define WACOM_PKGLEN_TPC1FG	 5
 #define WACOM_PKGLEN_TPC1FG_B	10
 #define WACOM_PKGLEN_TPC2FG	14
@@ -29,9 +26,6 @@
 #define WACOM_PKGLEN_BBTOUCH3	64
 #define WACOM_PKGLEN_BBPEN	10
 #define WACOM_PKGLEN_WIRELESS	32
-#define WACOM_PKGLEN_MTOUCH	62
-#define WACOM_PKGLEN_MTTPC	40
-#define WACOM_PKGLEN_DTUS	68
 #define WACOM_PKGLEN_PENABLED	 8
 #define WACOM_PKGLEN_BPAD_TOUCH	32
 #define WACOM_PKGLEN_BPAD_TOUCH_USB	64
@@ -78,10 +72,20 @@
 #define WACOM_QUIRK_MONITOR		0x0004
 #define WACOM_QUIRK_BATTERY		0x0008
 
+/* device types */
+#define WACOM_DEVICETYPE_NONE           0x0000
+#define WACOM_DEVICETYPE_PEN            0x0001
+#define WACOM_DEVICETYPE_TOUCH          0x0002
+#define WACOM_DEVICETYPE_PAD            0x0004
+
+#define WACOM_VENDORDEFINED_PEN		0xff0d0001
+
 #define WACOM_PEN_FIELD(f)	(((f)->logical == HID_DG_STYLUS) || \
 				 ((f)->physical == HID_DG_STYLUS) || \
 				 ((f)->physical == HID_DG_PEN) || \
-				 ((f)->application == HID_DG_PEN))
+				 ((f)->application == HID_DG_PEN) || \
+				 ((f)->application == HID_DG_DIGITIZER) || \
+				 ((f)->application == WACOM_VENDORDEFINED_PEN))
 #define WACOM_FINGER_FIELD(f)	(((f)->logical == HID_DG_FINGER) || \
 				 ((f)->physical == HID_DG_FINGER) || \
 				 ((f)->application == HID_DG_TOUCHSCREEN))
@@ -192,7 +196,8 @@ struct hid_data {
 };
 
 struct wacom_wac {
-	char name[WACOM_NAME_MAX];
+	char pen_name[WACOM_NAME_MAX];
+	char touch_name[WACOM_NAME_MAX];
 	char pad_name[WACOM_NAME_MAX];
 	char bat_name[WACOM_NAME_MAX];
 	char ac_name[WACOM_NAME_MAX];
@@ -203,9 +208,11 @@ struct wacom_wac {
 	bool reporting_data;
 	struct wacom_features features;
 	struct wacom_shared *shared;
-	struct input_dev *input;
+	struct input_dev *pen_input;
+	struct input_dev *touch_input;
 	struct input_dev *pad_input;
-	bool input_registered;
+	bool pen_registered;
+	bool touch_registered;
 	bool pad_registered;
 	int pid;
 	int battery_capacity;
diff --git a/drivers/hsi/clients/cmt_speech.c b/drivers/hsi/clients/cmt_speech.c
index 4983529a9c6c..d04643f9548b 100644
--- a/drivers/hsi/clients/cmt_speech.c
+++ b/drivers/hsi/clients/cmt_speech.c
@@ -451,9 +451,14 @@ static void cs_hsi_read_on_control_complete(struct hsi_msg *msg)
 	dev_dbg(&hi->cl->device, "Read on control: %08X\n", cmd);
 	cs_release_cmd(msg);
 	if (hi->flags & CS_FEAT_TSTAMP_RX_CTRL) {
-		struct timespec *tstamp =
+		struct timespec tspec;
+		struct cs_timestamp *tstamp =
 			&hi->mmap_cfg->tstamp_rx_ctrl;
-		do_posix_clock_monotonic_gettime(tstamp);
+
+		ktime_get_ts(&tspec);
+
+		tstamp->tv_sec = (__u32) tspec.tv_sec;
+		tstamp->tv_nsec = (__u32) tspec.tv_nsec;
 	}
 	spin_unlock(&hi->lock);
 
diff --git a/drivers/hsi/clients/nokia-modem.c b/drivers/hsi/clients/nokia-modem.c
index bbb19231fa82..7f82c911ad74 100644
--- a/drivers/hsi/clients/nokia-modem.c
+++ b/drivers/hsi/clients/nokia-modem.c
@@ -112,7 +112,8 @@ static int nokia_modem_gpio_probe(struct device *dev)
 	modem->gpio_amount = gpio_count;
 
 	for (i = 0; i < gpio_count; i++) {
-		modem->gpios[i].gpio = devm_gpiod_get_index(dev, NULL, i);
+		modem->gpios[i].gpio = devm_gpiod_get_index(dev, NULL, i,
+							    GPIOD_OUT_LOW);
 		if (IS_ERR(modem->gpios[i].gpio)) {
 			dev_err(dev, "Could not get gpio %d\n", i);
 			return PTR_ERR(modem->gpios[i].gpio);
@@ -125,10 +126,6 @@ static int nokia_modem_gpio_probe(struct device *dev)
 			return err;
 		}
 
-		err = gpiod_direction_output(modem->gpios[i].gpio, 0);
-		if (err)
-			return err;
-
 		err = gpiod_export(modem->gpios[i].gpio, 0);
 		if (err)
 			return err;
@@ -208,7 +205,7 @@ static int nokia_modem_probe(struct device *dev)
 
 	err = device_attach(&modem->ssi_protocol->device);
 	if (err == 0) {
-		dev_err(dev, "Missing ssi-protocol driver\n");
+		dev_dbg(dev, "Missing ssi-protocol driver\n");
 		err = -EPROBE_DEFER;
 		goto error3;
 	} else if (err < 0) {
@@ -231,7 +228,7 @@ static int nokia_modem_probe(struct device *dev)
 
 	err = device_attach(&modem->cmt_speech->device);
 	if (err == 0) {
-		dev_err(dev, "Missing cmt-speech driver\n");
+		dev_dbg(dev, "Missing cmt-speech driver\n");
 		err = -EPROBE_DEFER;
 		goto error4;
 	} else if (err < 0) {
diff --git a/drivers/hsi/controllers/omap_ssi.h b/drivers/hsi/controllers/omap_ssi.h
index 9d056417d88c..f9aaf37262be 100644
--- a/drivers/hsi/controllers/omap_ssi.h
+++ b/drivers/hsi/controllers/omap_ssi.h
@@ -24,6 +24,7 @@
 #define __LINUX_HSI_OMAP_SSI_H__
 
 #include <linux/device.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/hsi/hsi.h>
 #include <linux/gpio.h>
diff --git a/drivers/hv/Makefile b/drivers/hv/Makefile
index 5e4dfa4cfe22..39c9b2c08d33 100644
--- a/drivers/hv/Makefile
+++ b/drivers/hv/Makefile
@@ -5,4 +5,4 @@ obj-$(CONFIG_HYPERV_BALLOON)	+= hv_balloon.o
 hv_vmbus-y := vmbus_drv.o \
 		 hv.o connection.o channel.o \
 		 channel_mgmt.o ring_buffer.o
-hv_utils-y := hv_util.o hv_kvp.o hv_snapshot.o hv_fcopy.o
+hv_utils-y := hv_util.o hv_kvp.o hv_snapshot.o hv_fcopy.o hv_utils_transport.o
diff --git a/drivers/hv/channel.c b/drivers/hv/channel.c
index 54da66dc7d16..603ce97e9027 100644
--- a/drivers/hv/channel.c
+++ b/drivers/hv/channel.c
@@ -73,6 +73,7 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 	unsigned long flags;
 	int ret, err = 0;
 	unsigned long t;
+	struct page *page;
 
 	spin_lock_irqsave(&newchannel->lock, flags);
 	if (newchannel->state == CHANNEL_OPEN_STATE) {
@@ -87,8 +88,17 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 	newchannel->channel_callback_context = context;
 
 	/* Allocate the ring buffer */
-	out = (void *)__get_free_pages(GFP_KERNEL|__GFP_ZERO,
-		get_order(send_ringbuffer_size + recv_ringbuffer_size));
+	page = alloc_pages_node(cpu_to_node(newchannel->target_cpu),
+				GFP_KERNEL|__GFP_ZERO,
+				get_order(send_ringbuffer_size +
+				recv_ringbuffer_size));
+
+	if (!page)
+		out = (void *)__get_free_pages(GFP_KERNEL|__GFP_ZERO,
+					       get_order(send_ringbuffer_size +
+					       recv_ringbuffer_size));
+	else
+		out = (void *)page_address(page);
 
 	if (!out) {
 		err = -ENOMEM;
@@ -178,19 +188,18 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 		goto error1;
 	}
 
-
-	if (open_info->response.open_result.status)
-		err = open_info->response.open_result.status;
-
 	spin_lock_irqsave(&vmbus_connection.channelmsg_lock, flags);
 	list_del(&open_info->msglistentry);
 	spin_unlock_irqrestore(&vmbus_connection.channelmsg_lock, flags);
 
-	if (err == 0)
-		newchannel->state = CHANNEL_OPENED_STATE;
+	if (open_info->response.open_result.status) {
+		err = -EAGAIN;
+		goto error_gpadl;
+	}
 
+	newchannel->state = CHANNEL_OPENED_STATE;
 	kfree(open_info);
-	return err;
+	return 0;
 
 error1:
 	spin_lock_irqsave(&vmbus_connection.channelmsg_lock, flags);
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index 0eeb1b3bc048..4506a6623618 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -32,6 +32,9 @@
 
 #include "hyperv_vmbus.h"
 
+static void init_vp_index(struct vmbus_channel *channel,
+			  const uuid_le *type_guid);
+
 /**
  * vmbus_prep_negotiate_resp() - Create default response for Hyper-V Negotiate message
  * @icmsghdrp: Pointer to msg header structure
@@ -205,6 +208,7 @@ void hv_process_channel_removal(struct vmbus_channel *channel, u32 relid)
 		primary_channel = channel->primary_channel;
 		spin_lock_irqsave(&primary_channel->lock, flags);
 		list_del(&channel->sc_list);
+		primary_channel->num_sc--;
 		spin_unlock_irqrestore(&primary_channel->lock, flags);
 	}
 	free_channel(channel);
@@ -212,11 +216,16 @@ void hv_process_channel_removal(struct vmbus_channel *channel, u32 relid)
 
 void vmbus_free_channels(void)
 {
-	struct vmbus_channel *channel;
+	struct vmbus_channel *channel, *tmp;
+
+	list_for_each_entry_safe(channel, tmp, &vmbus_connection.chn_list,
+		listentry) {
+		/* if we don't set rescind to true, vmbus_close_internal()
+		 * won't invoke hv_process_channel_removal().
+		 */
+		channel->rescind = true;
 
-	list_for_each_entry(channel, &vmbus_connection.chn_list, listentry) {
 		vmbus_device_unregister(channel->device_obj);
-		free_channel(channel);
 	}
 }
 
@@ -228,7 +237,6 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 {
 	struct vmbus_channel *channel;
 	bool fnew = true;
-	bool enq = false;
 	unsigned long flags;
 
 	/* Make sure this is a new offer */
@@ -244,25 +252,12 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 		}
 	}
 
-	if (fnew) {
+	if (fnew)
 		list_add_tail(&newchannel->listentry,
 			      &vmbus_connection.chn_list);
-		enq = true;
-	}
 
 	spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
 
-	if (enq) {
-		if (newchannel->target_cpu != get_cpu()) {
-			put_cpu();
-			smp_call_function_single(newchannel->target_cpu,
-						 percpu_channel_enq,
-						 newchannel, true);
-		} else {
-			percpu_channel_enq(newchannel);
-			put_cpu();
-		}
-	}
 	if (!fnew) {
 		/*
 		 * Check to see if this is a sub-channel.
@@ -274,27 +269,22 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 			newchannel->primary_channel = channel;
 			spin_lock_irqsave(&channel->lock, flags);
 			list_add_tail(&newchannel->sc_list, &channel->sc_list);
-			spin_unlock_irqrestore(&channel->lock, flags);
-
-			if (newchannel->target_cpu != get_cpu()) {
-				put_cpu();
-				smp_call_function_single(newchannel->target_cpu,
-							 percpu_channel_enq,
-							 newchannel, true);
-			} else {
-				percpu_channel_enq(newchannel);
-				put_cpu();
-			}
-
-			newchannel->state = CHANNEL_OPEN_STATE;
 			channel->num_sc++;
-			if (channel->sc_creation_callback != NULL)
-				channel->sc_creation_callback(newchannel);
+			spin_unlock_irqrestore(&channel->lock, flags);
+		} else
+			goto err_free_chan;
+	}
 
-			return;
-		}
+	init_vp_index(newchannel, &newchannel->offermsg.offer.if_type);
 
-		goto err_free_chan;
+	if (newchannel->target_cpu != get_cpu()) {
+		put_cpu();
+		smp_call_function_single(newchannel->target_cpu,
+					 percpu_channel_enq,
+					 newchannel, true);
+	} else {
+		percpu_channel_enq(newchannel);
+		put_cpu();
 	}
 
 	/*
@@ -304,6 +294,12 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 	 */
 	newchannel->state = CHANNEL_OPEN_STATE;
 
+	if (!fnew) {
+		if (channel->sc_creation_callback != NULL)
+			channel->sc_creation_callback(newchannel);
+		return;
+	}
+
 	/*
 	 * Start the process of binding this offer to the driver
 	 * We need to set the DeviceObject field before calling
@@ -374,23 +370,27 @@ static const struct hv_vmbus_device_id hp_devs[] = {
 /*
  * We use this state to statically distribute the channel interrupt load.
  */
-static u32  next_vp;
+static int next_numa_node_id;
 
 /*
  * Starting with Win8, we can statically distribute the incoming
- * channel interrupt load by binding a channel to VCPU. We
- * implement here a simple round robin scheme for distributing
- * the interrupt load.
- * We will bind channels that are not performance critical to cpu 0 and
- * performance critical channels (IDE, SCSI and Network) will be uniformly
- * distributed across all available CPUs.
+ * channel interrupt load by binding a channel to VCPU.
+ * We do this in a hierarchical fashion:
+ * First distribute the primary channels across available NUMA nodes
+ * and then distribute the subchannels amongst the CPUs in the NUMA
+ * node assigned to the primary channel.
+ *
+ * For pre-win8 hosts or non-performance critical channels we assign the
+ * first CPU in the first NUMA node.
  */
 static void init_vp_index(struct vmbus_channel *channel, const uuid_le *type_guid)
 {
 	u32 cur_cpu;
 	int i;
 	bool perf_chn = false;
-	u32 max_cpus = num_online_cpus();
+	struct vmbus_channel *primary = channel->primary_channel;
+	int next_node;
+	struct cpumask available_mask;
 
 	for (i = IDE; i < MAX_PERF_CHN; i++) {
 		if (!memcmp(type_guid->b, hp_devs[i].guid,
@@ -407,16 +407,77 @@ static void init_vp_index(struct vmbus_channel *channel, const uuid_le *type_gui
 		 * Also if the channel is not a performance critical
 		 * channel, bind it to cpu 0.
 		 */
+		channel->numa_node = 0;
+		cpumask_set_cpu(0, &channel->alloced_cpus_in_node);
 		channel->target_cpu = 0;
-		channel->target_vp = 0;
+		channel->target_vp = hv_context.vp_index[0];
 		return;
 	}
-	cur_cpu = (++next_vp % max_cpus);
+
+	/*
+	 * We distribute primary channels evenly across all the available
+	 * NUMA nodes and within the assigned NUMA node we will assign the
+	 * first available CPU to the primary channel.
+	 * The sub-channels will be assigned to the CPUs available in the
+	 * NUMA node evenly.
+	 */
+	if (!primary) {
+		while (true) {
+			next_node = next_numa_node_id++;
+			if (next_node == nr_node_ids)
+				next_node = next_numa_node_id = 0;
+			if (cpumask_empty(cpumask_of_node(next_node)))
+				continue;
+			break;
+		}
+		channel->numa_node = next_node;
+		primary = channel;
+	}
+
+	if (cpumask_weight(&primary->alloced_cpus_in_node) ==
+	    cpumask_weight(cpumask_of_node(primary->numa_node))) {
+		/*
+		 * We have cycled through all the CPUs in the node;
+		 * reset the alloced map.
+		 */
+		cpumask_clear(&primary->alloced_cpus_in_node);
+	}
+
+	cpumask_xor(&available_mask, &primary->alloced_cpus_in_node,
+		    cpumask_of_node(primary->numa_node));
+
+	cur_cpu = cpumask_next(-1, &available_mask);
+	cpumask_set_cpu(cur_cpu, &primary->alloced_cpus_in_node);
+
 	channel->target_cpu = cur_cpu;
 	channel->target_vp = hv_context.vp_index[cur_cpu];
 }
 
 /*
+ * vmbus_unload_response - Handler for the unload response.
+ */
+static void vmbus_unload_response(struct vmbus_channel_message_header *hdr)
+{
+	/*
+	 * This is a global event; just wakeup the waiting thread.
+	 * Once we successfully unload, we can cleanup the monitor state.
+	 */
+	complete(&vmbus_connection.unload_event);
+}
+
+void vmbus_initiate_unload(void)
+{
+	struct vmbus_channel_message_header hdr;
+
+	init_completion(&vmbus_connection.unload_event);
+	memset(&hdr, 0, sizeof(struct vmbus_channel_message_header));
+	hdr.msgtype = CHANNELMSG_UNLOAD;
+	vmbus_post_msg(&hdr, sizeof(struct vmbus_channel_message_header));
+
+	wait_for_completion(&vmbus_connection.unload_event);
+}
+
+/*
  * vmbus_onoffer - Handler for channel offers from vmbus in parent partition.
  *
  */
@@ -461,8 +522,6 @@ static void vmbus_onoffer(struct vmbus_channel_message_header *hdr)
 				offer->connection_id;
 	}
 
-	init_vp_index(newchannel, &offer->offer.if_type);
-
 	memcpy(&newchannel->offermsg, offer,
 	       sizeof(struct vmbus_channel_offer_channel));
 	newchannel->monitor_grp = (u8)offer->monitorid / 32;
@@ -712,6 +771,7 @@ struct vmbus_channel_message_table_entry
 	{CHANNELMSG_INITIATE_CONTACT,		0, NULL},
 	{CHANNELMSG_VERSION_RESPONSE,		1, vmbus_onversion_response},
 	{CHANNELMSG_UNLOAD,			0, NULL},
+	{CHANNELMSG_UNLOAD_RESPONSE,		1, vmbus_unload_response},
 };
 
 /*
diff --git a/drivers/hv/connection.c b/drivers/hv/connection.c
index b27220a425f4..4fc2e8836e60 100644
--- a/drivers/hv/connection.c
+++ b/drivers/hv/connection.c
@@ -58,6 +58,9 @@ static __u32 vmbus_get_next_version(__u32 current_version)
 	case (VERSION_WIN8_1):
 		return VERSION_WIN8;
 
+	case (VERSION_WIN10):
+		return VERSION_WIN8_1;
+
 	case (VERSION_WS2008):
 	default:
 		return VERSION_INVAL;
@@ -80,7 +83,7 @@ static int vmbus_negotiate_version(struct vmbus_channel_msginfo *msginfo,
 	msg->interrupt_page = virt_to_phys(vmbus_connection.int_page);
 	msg->monitor_page1 = virt_to_phys(vmbus_connection.monitor_pages[0]);
 	msg->monitor_page2 = virt_to_phys(vmbus_connection.monitor_pages[1]);
-	if (version == VERSION_WIN8_1) {
+	if (version >= VERSION_WIN8_1) {
 		msg->target_vcpu = hv_context.vp_index[get_cpu()];
 		put_cpu();
 	}
@@ -227,6 +230,11 @@ cleanup:
 
 void vmbus_disconnect(void)
 {
+	/*
+	 * First send the unload request to the host.
+	 */
+	vmbus_initiate_unload();
+
 	if (vmbus_connection.work_queue) {
 		drain_workqueue(vmbus_connection.work_queue);
 		destroy_workqueue(vmbus_connection.work_queue);
@@ -371,8 +379,7 @@ void vmbus_on_event(unsigned long data)
 	int cpu = smp_processor_id();
 	union hv_synic_event_flags *event;
 
-	if ((vmbus_proto_version == VERSION_WS2008) ||
-		(vmbus_proto_version == VERSION_WIN7)) {
+	if (vmbus_proto_version < VERSION_WIN8) {
 		maxdword = MAX_NUM_CHANNELS_SUPPORTED >> 5;
 		recv_int_page = vmbus_connection.recv_int_page;
 	} else {
diff --git a/drivers/hv/hv_balloon.c b/drivers/hv/hv_balloon.c
index cb5b7dc9797f..8a725cd69ad7 100644
--- a/drivers/hv/hv_balloon.c
+++ b/drivers/hv/hv_balloon.c
@@ -567,7 +567,9 @@ static int hv_memory_notifier(struct notifier_block *nb, unsigned long val,
 	case MEM_ONLINE:
 		dm_device.num_pages_onlined += mem->nr_pages;
 	case MEM_CANCEL_ONLINE:
-		mutex_unlock(&dm_device.ha_region_mutex);
+		if (val == MEM_ONLINE ||
+		    mutex_is_locked(&dm_device.ha_region_mutex))
+			mutex_unlock(&dm_device.ha_region_mutex);
 		if (dm_device.ha_waiting) {
 			dm_device.ha_waiting = false;
 			complete(&dm_device.ol_waitevent);
diff --git a/drivers/hv/hv_fcopy.c b/drivers/hv/hv_fcopy.c
index cd453e4b2a07..b50dd330cf31 100644
--- a/drivers/hv/hv_fcopy.c
+++ b/drivers/hv/hv_fcopy.c
@@ -19,17 +19,13 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <linux/semaphore.h>
-#include <linux/fs.h>
 #include <linux/nls.h>
 #include <linux/workqueue.h>
-#include <linux/cdev.h>
 #include <linux/hyperv.h>
 #include <linux/sched.h>
-#include <linux/uaccess.h>
-#include <linux/miscdevice.h>
 
 #include "hyperv_vmbus.h"
+#include "hv_utils_transport.h"
 
 #define WIN8_SRV_MAJOR		1
 #define WIN8_SRV_MINOR		1
@@ -47,39 +43,31 @@
  * ensure this by serializing packet processing in this driver - we do not
  * read additional packets from the VMBUs until the current packet is fully
  * handled.
- *
- * The transaction "active" state is set when we receive a request from the
- * host and we cleanup this state when the transaction is completed - when we
- * respond to the host with our response. When the transaction active state is
- * set, we defer handling incoming packets.
  */
 
 static struct {
-	bool active; /* transaction status - active or not */
+	int state;   /* hvutil_device_state */
 	int recv_len; /* number of bytes received. */
 	struct hv_fcopy_hdr  *fcopy_msg; /* current message */
-	struct hv_start_fcopy  message; /*  sent to daemon */
 	struct vmbus_channel *recv_channel; /* chn we got the request */
 	u64 recv_req_id; /* request ID. */
 	void *fcopy_context; /* for the channel callback */
-	struct semaphore read_sema;
 } fcopy_transaction;
 
-static bool opened; /* currently device opened */
-
-/*
- * Before we can accept copy messages from the host, we need
- * to handshake with the user level daemon. This state tracks
- * if we are in the handshake phase.
- */
-static bool in_hand_shake = true;
-static void fcopy_send_data(void);
 static void fcopy_respond_to_host(int error);
-static void fcopy_work_func(struct work_struct *dummy);
-static DECLARE_DELAYED_WORK(fcopy_work, fcopy_work_func);
+static void fcopy_send_data(struct work_struct *dummy);
+static void fcopy_timeout_func(struct work_struct *dummy);
+static DECLARE_DELAYED_WORK(fcopy_timeout_work, fcopy_timeout_func);
+static DECLARE_WORK(fcopy_send_work, fcopy_send_data);
+static const char fcopy_devname[] = "vmbus/hv_fcopy";
 static u8 *recv_buffer;
+static struct hvutil_transport *hvt;
+/*
+ * This state maintains the version number registered by the daemon.
+ */
+static int dm_reg_value;
 
-static void fcopy_work_func(struct work_struct *dummy)
+static void fcopy_timeout_func(struct work_struct *dummy)
 {
 	/*
 	 * If the timer fires, the user-mode component has not responded;
@@ -87,23 +75,28 @@ static void fcopy_work_func(struct work_struct *dummy)
 	 */
 	fcopy_respond_to_host(HV_E_FAIL);
 
-	/* In the case the user-space daemon crashes, hangs or is killed, we
-	 * need to down the semaphore, otherwise, after the daemon starts next
-	 * time, the obsolete data in fcopy_transaction.message or
-	 * fcopy_transaction.fcopy_msg will be used immediately.
-	 *
-	 * NOTE: fcopy_read() happens to get the semaphore (very rare)? We're
-	 * still OK, because we've reported the failure to the host.
-	 */
-	if (down_trylock(&fcopy_transaction.read_sema))
-		;
+	/* Transaction is finished, reset the state. */
+	if (fcopy_transaction.state > HVUTIL_READY)
+		fcopy_transaction.state = HVUTIL_READY;
 
+	hv_poll_channel(fcopy_transaction.fcopy_context,
+			hv_fcopy_onchannelcallback);
 }
 
 static int fcopy_handle_handshake(u32 version)
 {
+	u32 our_ver = FCOPY_CURRENT_VERSION;
+
 	switch (version) {
-	case FCOPY_CURRENT_VERSION:
+	case FCOPY_VERSION_0:
+		/* Daemon doesn't expect us to reply */
+		dm_reg_value = version;
+		break;
+	case FCOPY_VERSION_1:
+		/* Daemon expects us to reply with our own version */
+		if (hvutil_transport_send(hvt, &our_ver, sizeof(our_ver)))
+			return -EFAULT;
+		dm_reg_value = version;
 		break;
 	default:
 		/*
@@ -114,20 +107,20 @@ static int fcopy_handle_handshake(u32 version)
 		 */
 		return -EINVAL;
 	}
-	pr_info("FCP: user-mode registering done. Daemon version: %d\n",
-		version);
-	fcopy_transaction.active = false;
-	if (fcopy_transaction.fcopy_context)
-		hv_fcopy_onchannelcallback(fcopy_transaction.fcopy_context);
-	in_hand_shake = false;
+	pr_debug("FCP: userspace daemon ver. %d registered\n", version);
+	fcopy_transaction.state = HVUTIL_READY;
+	hv_poll_channel(fcopy_transaction.fcopy_context,
+			hv_fcopy_onchannelcallback);
 	return 0;
 }
 
-static void fcopy_send_data(void)
+static void fcopy_send_data(struct work_struct *dummy)
 {
-	struct hv_start_fcopy *smsg_out = &fcopy_transaction.message;
+	struct hv_start_fcopy smsg_out;
 	int operation = fcopy_transaction.fcopy_msg->operation;
 	struct hv_start_fcopy *smsg_in;
+	void *out_src;
+	int rc, out_len;
 
 	/*
 	 * The  strings sent from the host are encoded in
@@ -142,26 +135,39 @@ static void fcopy_send_data(void)
 
 	switch (operation) {
 	case START_FILE_COPY:
-		memset(smsg_out, 0, sizeof(struct hv_start_fcopy));
-		smsg_out->hdr.operation = operation;
+		out_len = sizeof(struct hv_start_fcopy);
+		memset(&smsg_out, 0, out_len);
+		smsg_out.hdr.operation = operation;
 		smsg_in = (struct hv_start_fcopy *)fcopy_transaction.fcopy_msg;
 
 		utf16s_to_utf8s((wchar_t *)smsg_in->file_name, W_MAX_PATH,
 				UTF16_LITTLE_ENDIAN,
-				(__u8 *)smsg_out->file_name, W_MAX_PATH - 1);
+				(__u8 *)&smsg_out.file_name, W_MAX_PATH - 1);
 
 		utf16s_to_utf8s((wchar_t *)smsg_in->path_name, W_MAX_PATH,
 				UTF16_LITTLE_ENDIAN,
-				(__u8 *)smsg_out->path_name, W_MAX_PATH - 1);
+				(__u8 *)&smsg_out.path_name, W_MAX_PATH - 1);
 
-		smsg_out->copy_flags = smsg_in->copy_flags;
-		smsg_out->file_size = smsg_in->file_size;
+		smsg_out.copy_flags = smsg_in->copy_flags;
+		smsg_out.file_size = smsg_in->file_size;
+		out_src = &smsg_out;
 		break;
 
 	default:
+		out_src = fcopy_transaction.fcopy_msg;
+		out_len = fcopy_transaction.recv_len;
 		break;
 	}
-	up(&fcopy_transaction.read_sema);
+
+	fcopy_transaction.state = HVUTIL_USERSPACE_REQ;
+	rc = hvutil_transport_send(hvt, out_src, out_len);
+	if (rc) {
+		pr_debug("FCP: failed to communicate to the daemon: %d\n", rc);
+		if (cancel_delayed_work_sync(&fcopy_timeout_work)) {
+			fcopy_respond_to_host(HV_E_FAIL);
+			fcopy_transaction.state = HVUTIL_READY;
+		}
+	}
 	return;
 }
 
@@ -189,8 +195,6 @@ fcopy_respond_to_host(int error)
 	channel = fcopy_transaction.recv_channel;
 	req_id = fcopy_transaction.recv_req_id;
 
-	fcopy_transaction.active = false;
-
 	icmsghdr = (struct icmsg_hdr *)
 			&recv_buffer[sizeof(struct vmbuspipe_hdr)];
 
@@ -218,7 +222,7 @@ void hv_fcopy_onchannelcallback(void *context)
 	int util_fw_version;
 	int fcopy_srv_version;
 
-	if (fcopy_transaction.active) {
+	if (fcopy_transaction.state > HVUTIL_READY) {
 		/*
 		 * We will defer processing this callback once
 		 * the current transaction is complete.
@@ -226,6 +230,7 @@ void hv_fcopy_onchannelcallback(void *context)
 		fcopy_transaction.fcopy_context = context;
 		return;
 	}
+	fcopy_transaction.fcopy_context = NULL;
 
 	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE * 2, &recvlen,
 			 &requestid);
@@ -249,17 +254,23 @@ void hv_fcopy_onchannelcallback(void *context)
 		 * transaction; note transactions are serialized.
 		 */
 
-		fcopy_transaction.active = true;
 		fcopy_transaction.recv_len = recvlen;
 		fcopy_transaction.recv_channel = channel;
 		fcopy_transaction.recv_req_id = requestid;
 		fcopy_transaction.fcopy_msg = fcopy_msg;
 
+		if (fcopy_transaction.state < HVUTIL_READY) {
+			/* Userspace is not registered yet */
+			fcopy_respond_to_host(HV_E_FAIL);
+			return;
+		}
+		fcopy_transaction.state = HVUTIL_HOSTMSG_RECEIVED;
+
 		/*
 		 * Send the information to the user-level daemon.
 		 */
-		schedule_delayed_work(&fcopy_work, 5*HZ);
-		fcopy_send_data();
+		schedule_work(&fcopy_send_work);
+		schedule_delayed_work(&fcopy_timeout_work, 5*HZ);
 		return;
 	}
 	icmsghdr->icflags = ICMSGHDRFLAG_TRANSACTION | ICMSGHDRFLAG_RESPONSE;
@@ -267,155 +278,44 @@ void hv_fcopy_onchannelcallback(void *context)
 			VM_PKT_DATA_INBAND, 0);
 }
 
-/*
- * Create a char device that can support read/write for passing
- * the payload.
- */
-
-static ssize_t fcopy_read(struct file *file, char __user *buf,
-		size_t count, loff_t *ppos)
-{
-	void *src;
-	size_t copy_size;
-	int operation;
-
-	/*
-	 * Wait until there is something to be read.
-	 */
-	if (down_interruptible(&fcopy_transaction.read_sema))
-		return -EINTR;
-
-	/*
-	 * The channel may be rescinded and in this case, we will wakeup the
-	 * the thread blocked on the semaphore and we will use the opened
-	 * state to correctly handle this case.
-	 */
-	if (!opened)
-		return -ENODEV;
-
-	operation = fcopy_transaction.fcopy_msg->operation;
-
-	if (operation == START_FILE_COPY) {
-		src = &fcopy_transaction.message;
-		copy_size = sizeof(struct hv_start_fcopy);
-		if (count < copy_size)
-			return 0;
-	} else {
-		src = fcopy_transaction.fcopy_msg;
-		copy_size = sizeof(struct hv_do_fcopy);
-		if (count < copy_size)
-			return 0;
-	}
-	if (copy_to_user(buf, src, copy_size))
-		return -EFAULT;
-
-	return copy_size;
-}
-
-static ssize_t fcopy_write(struct file *file, const char __user *buf,
-			size_t count, loff_t *ppos)
+/* Callback when data is received from userspace */
+static int fcopy_on_msg(void *msg, int len)
 {
-	int response = 0;
+	int *val = (int *)msg;
 
-	if (count != sizeof(int))
+	if (len != sizeof(int))
 		return -EINVAL;
 
-	if (copy_from_user(&response, buf, sizeof(int)))
-		return -EFAULT;
+	if (fcopy_transaction.state == HVUTIL_DEVICE_INIT)
+		return fcopy_handle_handshake(*val);
 
-	if (in_hand_shake) {
-		if (fcopy_handle_handshake(response))
-			return -EINVAL;
-		return sizeof(int);
-	}
+	if (fcopy_transaction.state != HVUTIL_USERSPACE_REQ)
+		return -EINVAL;
 
 	/*
 	 * Complete the transaction by forwarding the result
 	 * to the host. But first, cancel the timeout.
 	 */
-	if (cancel_delayed_work_sync(&fcopy_work))
-		fcopy_respond_to_host(response);
-
-	return sizeof(int);
-}
-
-static int fcopy_open(struct inode *inode, struct file *f)
-{
-	/*
-	 * The user level daemon that will open this device is
-	 * really an extension of this driver. We can have only
-	 * active open at a time.
-	 */
-	if (opened)
-		return -EBUSY;
+	if (cancel_delayed_work_sync(&fcopy_timeout_work)) {
+		fcopy_transaction.state = HVUTIL_USERSPACE_RECV;
+		fcopy_respond_to_host(*val);
+		fcopy_transaction.state = HVUTIL_READY;
+		hv_poll_channel(fcopy_transaction.fcopy_context,
+				hv_fcopy_onchannelcallback);
+	}
 
-	/*
-	 * The daemon is alive; setup the state.
-	 */
-	opened = true;
 	return 0;
 }
 
-/* XXX: there are still some tricky corner cases, e.g.,
- * 1) In a SMP guest, when fcopy_release() runs between
- * schedule_delayed_work() and fcopy_send_data(), there is
- * still a chance an obsolete message will be queued.
- *
- * 2) When the fcopy daemon is running, if we unload the driver,
- * we'll notice a kernel oops when we kill the daemon later.
- */
-static int fcopy_release(struct inode *inode, struct file *f)
+static void fcopy_on_reset(void)
 {
 	/*
 	 * The daemon has exited; reset the state.
 	 */
-	in_hand_shake = true;
-	opened = false;
+	fcopy_transaction.state = HVUTIL_DEVICE_INIT;
 
-	if (cancel_delayed_work_sync(&fcopy_work)) {
-		/* We haven't up()-ed the semaphore(very rare)? */
-		if (down_trylock(&fcopy_transaction.read_sema))
-			;
+	if (cancel_delayed_work_sync(&fcopy_timeout_work))
 		fcopy_respond_to_host(HV_E_FAIL);
-	}
-	return 0;
-}
-
-
-static const struct file_operations fcopy_fops = {
-	.read           = fcopy_read,
-	.write          = fcopy_write,
-	.release	= fcopy_release,
-	.open		= fcopy_open,
-};
-
-static struct miscdevice fcopy_misc = {
-	.minor          = MISC_DYNAMIC_MINOR,
-	.name           = "vmbus/hv_fcopy",
-	.fops           = &fcopy_fops,
-};
-
-static int fcopy_dev_init(void)
-{
-	return misc_register(&fcopy_misc);
-}
-
-static void fcopy_dev_deinit(void)
-{
-
-	/*
-	 * The device is going away - perhaps because the
-	 * host has rescinded the channel. Setup state so that
-	 * user level daemon can gracefully exit if it is blocked
-	 * on the read semaphore.
-	 */
-	opened = false;
-	/*
-	 * Signal the semaphore as the device is
-	 * going away.
-	 */
-	up(&fcopy_transaction.read_sema);
-	misc_deregister(&fcopy_misc);
 }
 
 int hv_fcopy_init(struct hv_util_service *srv)
@@ -428,14 +328,19 @@ int hv_fcopy_init(struct hv_util_service *srv)
 	 * Defer processing channel callbacks until the daemon
 	 * has registered.
 	 */
-	fcopy_transaction.active = true;
-	sema_init(&fcopy_transaction.read_sema, 0);
+	fcopy_transaction.state = HVUTIL_DEVICE_INIT;
+
+	hvt = hvutil_transport_init(fcopy_devname, 0, 0,
+				    fcopy_on_msg, fcopy_on_reset);
+	if (!hvt)
+		return -EFAULT;
 
-	return fcopy_dev_init();
+	return 0;
 }
 
 void hv_fcopy_deinit(void)
 {
-	cancel_delayed_work_sync(&fcopy_work);
-	fcopy_dev_deinit();
+	fcopy_transaction.state = HVUTIL_DEVICE_DYING;
+	cancel_delayed_work_sync(&fcopy_timeout_work);
+	hvutil_transport_destroy(hvt);
 }
diff --git a/drivers/hv/hv_kvp.c b/drivers/hv/hv_kvp.c
index beb8105c0e7b..d85798d5992c 100644
--- a/drivers/hv/hv_kvp.c
+++ b/drivers/hv/hv_kvp.c
@@ -28,6 +28,8 @@
 #include <linux/workqueue.h>
 #include <linux/hyperv.h>
 
+#include "hyperv_vmbus.h"
+#include "hv_utils_transport.h"
 
 /*
  * Pre win8 version numbers used in ws2008 and ws 2008 r2 (win7)
@@ -45,16 +47,21 @@
 #define WIN8_SRV_VERSION     (WIN8_SRV_MAJOR << 16 | WIN8_SRV_MINOR)
 
 /*
- * Global state maintained for transaction that is being processed.
- * Note that only one transaction can be active at any point in time.
+ * Global state maintained for transaction that is being processed. For a class
+ * of integration services, including the "KVP service", the specified protocol
+ * is a "request/response" protocol which means that there can only be single
+ * outstanding transaction from the host at any given point in time. We use
+ * this to simplify memory management in this driver - we cache and process
+ * only one message at a time.
  *
- * This state is set when we receive a request from the host; we
- * cleanup this state when the transaction is completed - when we respond
- * to the host with the key value.
+ * While the request/response protocol is guaranteed by the host, we further
+ * ensure this by serializing packet processing in this driver - we do not
+ * read additional packets from the VMBUs until the current packet is fully
+ * handled.
  */
 
 static struct {
-	bool active; /* transaction status - active or not */
+	int state;   /* hvutil_device_state */
 	int recv_len; /* number of bytes received. */
 	struct hv_kvp_msg  *kvp_msg; /* current message */
 	struct vmbus_channel *recv_channel; /* chn we got the request */
@@ -63,13 +70,6 @@ static struct {
 } kvp_transaction;
 
 /*
- * Before we can accept KVP messages from the host, we need
- * to handshake with the user level daemon. This state tracks
- * if we are in the handshake phase.
- */
-static bool in_hand_shake = true;
-
-/*
  * This state maintains the version number registered by the daemon.
  */
 static int dm_reg_value;
@@ -78,15 +78,15 @@ static void kvp_send_key(struct work_struct *dummy);
 
 
 static void kvp_respond_to_host(struct hv_kvp_msg *msg, int error);
-static void kvp_work_func(struct work_struct *dummy);
+static void kvp_timeout_func(struct work_struct *dummy);
 static void kvp_register(int);
 
-static DECLARE_DELAYED_WORK(kvp_work, kvp_work_func);
+static DECLARE_DELAYED_WORK(kvp_timeout_work, kvp_timeout_func);
 static DECLARE_WORK(kvp_sendkey_work, kvp_send_key);
 
-static struct cb_id kvp_id = { CN_KVP_IDX, CN_KVP_VAL };
-static const char kvp_name[] = "kvp_kernel_module";
+static const char kvp_devname[] = "vmbus/hv_kvp";
 static u8 *recv_buffer;
+static struct hvutil_transport *hvt;
 /*
  * Register the kernel component with the user-level daemon.
  * As part of this registration, pass the LIC version number.
@@ -98,50 +98,39 @@ static void
 kvp_register(int reg_value)
 {
 
-	struct cn_msg *msg;
 	struct hv_kvp_msg *kvp_msg;
 	char *version;
 
-	msg = kzalloc(sizeof(*msg) + sizeof(struct hv_kvp_msg), GFP_ATOMIC);
+	kvp_msg = kzalloc(sizeof(*kvp_msg), GFP_KERNEL);
 
-	if (msg) {
-		kvp_msg = (struct hv_kvp_msg *)msg->data;
+	if (kvp_msg) {
 		version = kvp_msg->body.kvp_register.version;
-		msg->id.idx =  CN_KVP_IDX;
-		msg->id.val = CN_KVP_VAL;
-
 		kvp_msg->kvp_hdr.operation = reg_value;
 		strcpy(version, HV_DRV_VERSION);
-		msg->len = sizeof(struct hv_kvp_msg);
-		cn_netlink_send(msg, 0, 0, GFP_ATOMIC);
-		kfree(msg);
+
+		hvutil_transport_send(hvt, kvp_msg, sizeof(*kvp_msg));
+		kfree(kvp_msg);
 	}
 }
-static void
-kvp_work_func(struct work_struct *dummy)
+
+static void kvp_timeout_func(struct work_struct *dummy)
 {
 	/*
 	 * If the timer fires, the user-mode component has not responded;
 	 * process the pending transaction.
 	 */
 	kvp_respond_to_host(NULL, HV_E_FAIL);
-}
 
-static void poll_channel(struct vmbus_channel *channel)
-{
-	if (channel->target_cpu != smp_processor_id())
-		smp_call_function_single(channel->target_cpu,
-					 hv_kvp_onchannelcallback,
-					 channel, true);
-	else
-		hv_kvp_onchannelcallback(channel);
-}
+	/* Transaction is finished, reset the state. */
+	if (kvp_transaction.state > HVUTIL_READY)
+		kvp_transaction.state = HVUTIL_READY;
 
+	hv_poll_channel(kvp_transaction.kvp_context,
+			hv_kvp_onchannelcallback);
+}
 
 static int kvp_handle_handshake(struct hv_kvp_msg *msg)
 {
-	int ret = 1;
-
 	switch (msg->kvp_hdr.operation) {
 	case KVP_OP_REGISTER:
 		dm_reg_value = KVP_OP_REGISTER;
@@ -155,20 +144,18 @@ static int kvp_handle_handshake(struct hv_kvp_msg *msg)
 		pr_info("KVP: incompatible daemon\n");
 		pr_info("KVP: KVP version: %d, Daemon version: %d\n",
 			KVP_OP_REGISTER1, msg->kvp_hdr.operation);
-		ret = 0;
+		return -EINVAL;
 	}
 
-	if (ret) {
-		/*
-		 * We have a compatible daemon; complete the handshake.
-		 */
-		pr_info("KVP: user-mode registering done.\n");
-		kvp_register(dm_reg_value);
-		kvp_transaction.active = false;
-		if (kvp_transaction.kvp_context)
-			poll_channel(kvp_transaction.kvp_context);
-	}
-	return ret;
+	/*
+	 * We have a compatible daemon; complete the handshake.
+	 */
+	pr_debug("KVP: userspace daemon ver. %d registered\n",
+		 KVP_OP_REGISTER);
+	kvp_register(dm_reg_value);
+	kvp_transaction.state = HVUTIL_READY;
+
+	return 0;
 }
 
 
@@ -176,26 +163,30 @@ static int kvp_handle_handshake(struct hv_kvp_msg *msg)
  * Callback when data is received from user mode.
  */
 
-static void
-kvp_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
+static int kvp_on_msg(void *msg, int len)
 {
-	struct hv_kvp_msg *message;
+	struct hv_kvp_msg *message = (struct hv_kvp_msg *)msg;
 	struct hv_kvp_msg_enumerate *data;
 	int	error = 0;
 
-	message = (struct hv_kvp_msg *)msg->data;
+	if (len < sizeof(*message))
+		return -EINVAL;
 
 	/*
 	 * If we are negotiating the version information
 	 * with the daemon; handle that first.
 	 */
 
-	if (in_hand_shake) {
-		if (kvp_handle_handshake(message))
-			in_hand_shake = false;
-		return;
+	if (kvp_transaction.state < HVUTIL_READY) {
+		return kvp_handle_handshake(message);
 	}
 
+	/* We didn't send anything to userspace so the reply is spurious */
+	if (kvp_transaction.state < HVUTIL_USERSPACE_REQ)
+		return -EINVAL;
+
+	kvp_transaction.state = HVUTIL_USERSPACE_RECV;
+
 	/*
 	 * Based on the version of the daemon, we propagate errors from the
 	 * daemon differently.
@@ -225,8 +216,14 @@ kvp_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 	 * Complete the transaction by forwarding the key value
 	 * to the host. But first, cancel the timeout.
 	 */
-	if (cancel_delayed_work_sync(&kvp_work))
+	if (cancel_delayed_work_sync(&kvp_timeout_work)) {
 		kvp_respond_to_host(message, error);
+		kvp_transaction.state = HVUTIL_READY;
+		hv_poll_channel(kvp_transaction.kvp_context,
+				hv_kvp_onchannelcallback);
+	}
+
+	return 0;
 }
 
 
@@ -343,7 +340,6 @@ static void process_ib_ipinfo(void *in_msg, void *out_msg, int op)
 static void
 kvp_send_key(struct work_struct *dummy)
 {
-	struct cn_msg *msg;
 	struct hv_kvp_msg *message;
 	struct hv_kvp_msg *in_msg;
 	__u8 operation = kvp_transaction.kvp_msg->kvp_hdr.operation;
@@ -352,14 +348,11 @@ kvp_send_key(struct work_struct *dummy)
 	__u64 val64;
 	int rc;
 
-	msg = kzalloc(sizeof(*msg) + sizeof(struct hv_kvp_msg) , GFP_ATOMIC);
-	if (!msg)
+	/* The transaction state is wrong. */
+	if (kvp_transaction.state != HVUTIL_HOSTMSG_RECEIVED)
 		return;
 
-	msg->id.idx =  CN_KVP_IDX;
-	msg->id.val = CN_KVP_VAL;
-
-	message = (struct hv_kvp_msg *)msg->data;
+	message = kzalloc(sizeof(*message), GFP_KERNEL);
 	message->kvp_hdr.operation = operation;
 	message->kvp_hdr.pool = pool;
 	in_msg = kvp_transaction.kvp_msg;
@@ -446,15 +439,17 @@ kvp_send_key(struct work_struct *dummy)
 			break;
 	}
 
-	msg->len = sizeof(struct hv_kvp_msg);
-	rc = cn_netlink_send(msg, 0, 0, GFP_ATOMIC);
+	kvp_transaction.state = HVUTIL_USERSPACE_REQ;
+	rc = hvutil_transport_send(hvt, message, sizeof(*message));
 	if (rc) {
 		pr_debug("KVP: failed to communicate to the daemon: %d\n", rc);
-		if (cancel_delayed_work_sync(&kvp_work))
+		if (cancel_delayed_work_sync(&kvp_timeout_work)) {
 			kvp_respond_to_host(message, HV_E_FAIL);
+			kvp_transaction.state = HVUTIL_READY;
+		}
 	}
 
-	kfree(msg);
+	kfree(message);
 
 	return;
 }
@@ -479,17 +474,6 @@ kvp_respond_to_host(struct hv_kvp_msg *msg_to_host, int error)
 	int ret;
 
 	/*
-	 * If a transaction is not active; log and return.
-	 */
-
-	if (!kvp_transaction.active) {
-		/*
-		 * This is a spurious call!
-		 */
-		pr_warn("KVP: Transaction not active\n");
-		return;
-	}
-	/*
 	 * Copy the global state for completing the transaction. Note that
 	 * only one transaction can be active at a time.
 	 */
@@ -498,8 +482,6 @@ kvp_respond_to_host(struct hv_kvp_msg *msg_to_host, int error)
 	channel = kvp_transaction.recv_channel;
 	req_id = kvp_transaction.recv_req_id;
 
-	kvp_transaction.active = false;
-
 	icmsghdrp = (struct icmsg_hdr *)
 			&recv_buffer[sizeof(struct vmbuspipe_hdr)];
 
@@ -586,7 +568,6 @@ response_done:
 
 	vmbus_sendpacket(channel, recv_buffer, buf_len, req_id,
 				VM_PKT_DATA_INBAND, 0);
-	poll_channel(channel);
 }
 
 /*
@@ -612,7 +593,7 @@ void hv_kvp_onchannelcallback(void *context)
 	int util_fw_version;
 	int kvp_srv_version;
 
-	if (kvp_transaction.active) {
+	if (kvp_transaction.state > HVUTIL_READY) {
 		/*
 		 * We will defer processing this callback once
 		 * the current transaction is complete.
@@ -620,6 +601,7 @@ void hv_kvp_onchannelcallback(void *context)
 		kvp_transaction.kvp_context = context;
 		return;
 	}
+	kvp_transaction.kvp_context = NULL;
 
 	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE * 4, &recvlen,
 			 &requestid);
@@ -664,9 +646,15 @@ void hv_kvp_onchannelcallback(void *context)
 			kvp_transaction.recv_len = recvlen;
 			kvp_transaction.recv_channel = channel;
 			kvp_transaction.recv_req_id = requestid;
-			kvp_transaction.active = true;
 			kvp_transaction.kvp_msg = kvp_msg;
 
+			if (kvp_transaction.state < HVUTIL_READY) {
+				/* Userspace is not registered yet */
+				kvp_respond_to_host(NULL, HV_E_FAIL);
+				return;
+			}
+			kvp_transaction.state = HVUTIL_HOSTMSG_RECEIVED;
+
 			/*
 			 * Get the information from the
 			 * user-mode component.
@@ -677,7 +665,7 @@ void hv_kvp_onchannelcallback(void *context)
 			 * user-mode not responding.
 			 */
 			schedule_work(&kvp_sendkey_work);
-			schedule_delayed_work(&kvp_work, 5*HZ);
+			schedule_delayed_work(&kvp_timeout_work, 5*HZ);
 
 			return;
 
@@ -693,14 +681,16 @@ void hv_kvp_onchannelcallback(void *context)
 
 }
 
+static void kvp_on_reset(void)
+{
+	if (cancel_delayed_work_sync(&kvp_timeout_work))
+		kvp_respond_to_host(NULL, HV_E_FAIL);
+	kvp_transaction.state = HVUTIL_DEVICE_INIT;
+}
+
 int
 hv_kvp_init(struct hv_util_service *srv)
 {
-	int err;
-
-	err = cn_add_callback(&kvp_id, kvp_name, kvp_cn_callback);
-	if (err)
-		return err;
 	recv_buffer = srv->recv_buffer;
 
 	/*
@@ -709,14 +699,20 @@ hv_kvp_init(struct hv_util_service *srv)
 	 * Defer processing channel callbacks until the daemon
 	 * has registered.
 	 */
-	kvp_transaction.active = true;
+	kvp_transaction.state = HVUTIL_DEVICE_INIT;
+
+	hvt = hvutil_transport_init(kvp_devname, CN_KVP_IDX, CN_KVP_VAL,
+				    kvp_on_msg, kvp_on_reset);
+	if (!hvt)
+		return -EFAULT;
 
 	return 0;
 }
 
 void hv_kvp_deinit(void)
 {
-	cn_del_callback(&kvp_id);
-	cancel_delayed_work_sync(&kvp_work);
+	kvp_transaction.state = HVUTIL_DEVICE_DYING;
+	cancel_delayed_work_sync(&kvp_timeout_work);
 	cancel_work_sync(&kvp_sendkey_work);
+	hvutil_transport_destroy(hvt);
 }
diff --git a/drivers/hv/hv_snapshot.c b/drivers/hv/hv_snapshot.c
index 9d5e0d1efdb5..815405f2e777 100644
--- a/drivers/hv/hv_snapshot.c
+++ b/drivers/hv/hv_snapshot.c
@@ -24,6 +24,9 @@
 #include <linux/workqueue.h>
 #include <linux/hyperv.h>
 
+#include "hyperv_vmbus.h"
+#include "hv_utils_transport.h"
+
 #define VSS_MAJOR  5
 #define VSS_MINOR  0
 #define VSS_VERSION    (VSS_MAJOR << 16 | VSS_MINOR)
@@ -31,28 +34,39 @@
 #define VSS_USERSPACE_TIMEOUT (msecs_to_jiffies(10 * 1000))
 
 /*
- * Global state maintained for transaction that is being processed.
- * Note that only one transaction can be active at any point in time.
+ * Global state maintained for transaction that is being processed. For a class
+ * of integration services, including the "VSS service", the specified protocol
+ * is a "request/response" protocol which means that there can only be single
+ * outstanding transaction from the host at any given point in time. We use
+ * this to simplify memory management in this driver - we cache and process
+ * only one message at a time.
  *
- * This state is set when we receive a request from the host; we
- * cleanup this state when the transaction is completed - when we respond
- * to the host with the key value.
+ * While the request/response protocol is guaranteed by the host, we further
+ * ensure this by serializing packet processing in this driver - we do not
+ * read additional packets from the VMBUs until the current packet is fully
+ * handled.
  */
 
 static struct {
-	bool active; /* transaction status - active or not */
+	int state;   /* hvutil_device_state */
 	int recv_len; /* number of bytes received. */
 	struct vmbus_channel *recv_channel; /* chn we got the request */
 	u64 recv_req_id; /* request ID. */
 	struct hv_vss_msg  *msg; /* current message */
+	void *vss_context; /* for the channel callback */
 } vss_transaction;
 
 
 static void vss_respond_to_host(int error);
 
-static struct cb_id vss_id = { CN_VSS_IDX, CN_VSS_VAL };
-static const char vss_name[] = "vss_kernel_module";
+/*
+ * This state maintains the version number registered by the daemon.
+ */
+static int dm_reg_value;
+
+static const char vss_devname[] = "vmbus/hv_vss";
 static __u8 *recv_buffer;
+static struct hvutil_transport *hvt;
 
 static void vss_send_op(struct work_struct *dummy);
 static void vss_timeout_func(struct work_struct *dummy);
@@ -71,25 +85,69 @@ static void vss_timeout_func(struct work_struct *dummy)
 	 */
 	pr_warn("VSS: timeout waiting for daemon to reply\n");
 	vss_respond_to_host(HV_E_FAIL);
+
+	/* Transaction is finished, reset the state. */
+	if (vss_transaction.state > HVUTIL_READY)
+		vss_transaction.state = HVUTIL_READY;
+
+	hv_poll_channel(vss_transaction.vss_context,
+			hv_vss_onchannelcallback);
 }
 
-static void
-vss_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
+static int vss_handle_handshake(struct hv_vss_msg *vss_msg)
 {
-	struct hv_vss_msg *vss_msg;
+	u32 our_ver = VSS_OP_REGISTER1;
+
+	switch (vss_msg->vss_hdr.operation) {
+	case VSS_OP_REGISTER:
+		/* Daemon doesn't expect us to reply */
+		dm_reg_value = VSS_OP_REGISTER;
+		break;
+	case VSS_OP_REGISTER1:
+		/* Daemon expects us to reply with our own version*/
+		if (hvutil_transport_send(hvt, &our_ver, sizeof(our_ver)))
+			return -EFAULT;
+		dm_reg_value = VSS_OP_REGISTER1;
+		break;
+	default:
+		return -EINVAL;
+	}
+	vss_transaction.state = HVUTIL_READY;
+	pr_debug("VSS: userspace daemon ver. %d registered\n", dm_reg_value);
+	return 0;
+}
 
-	vss_msg = (struct hv_vss_msg *)msg->data;
+static int vss_on_msg(void *msg, int len)
+{
+	struct hv_vss_msg *vss_msg = (struct hv_vss_msg *)msg;
 
-	if (vss_msg->vss_hdr.operation == VSS_OP_REGISTER) {
-		pr_info("VSS daemon registered\n");
-		vss_transaction.active = false;
-		if (vss_transaction.recv_channel != NULL)
-			hv_vss_onchannelcallback(vss_transaction.recv_channel);
-		return;
+	if (len != sizeof(*vss_msg))
+		return -EINVAL;
 
+	if (vss_msg->vss_hdr.operation == VSS_OP_REGISTER ||
+	    vss_msg->vss_hdr.operation == VSS_OP_REGISTER1) {
+		/*
+		 * Don't process registration messages if we're in the middle
+		 * of a transaction processing.
+		 */
+		if (vss_transaction.state > HVUTIL_READY)
+			return -EINVAL;
+		return vss_handle_handshake(vss_msg);
+	} else if (vss_transaction.state == HVUTIL_USERSPACE_REQ) {
+		vss_transaction.state = HVUTIL_USERSPACE_RECV;
+		if (cancel_delayed_work_sync(&vss_timeout_work)) {
+			vss_respond_to_host(vss_msg->error);
+			/* Transaction is finished, reset the state. */
+			vss_transaction.state = HVUTIL_READY;
+			hv_poll_channel(vss_transaction.vss_context,
+					hv_vss_onchannelcallback);
+		}
+	} else {
+		/* This is a spurious call! */
+		pr_warn("VSS: Transaction not active\n");
+		return -EINVAL;
 	}
-	if (cancel_delayed_work_sync(&vss_timeout_work))
-		vss_respond_to_host(vss_msg->error);
+	return 0;
 }
 
 
@@ -97,28 +155,29 @@ 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;
 
-	msg = kzalloc(sizeof(*msg) + sizeof(*vss_msg), GFP_ATOMIC);
-	if (!msg)
+	/* The transaction state is wrong. */
+	if (vss_transaction.state != HVUTIL_HOSTMSG_RECEIVED)
 		return;
 
-	vss_msg = (struct hv_vss_msg *)msg->data;
-
-	msg->id.idx =  CN_VSS_IDX;
-	msg->id.val = CN_VSS_VAL;
+	vss_msg = kzalloc(sizeof(*vss_msg), GFP_KERNEL);
+	if (!vss_msg)
+		return;
 
 	vss_msg->vss_hdr.operation = op;
-	msg->len = sizeof(struct hv_vss_msg);
 
-	rc = cn_netlink_send(msg, 0, 0, GFP_ATOMIC);
+	vss_transaction.state = HVUTIL_USERSPACE_REQ;
+	rc = hvutil_transport_send(hvt, vss_msg, sizeof(*vss_msg));
 	if (rc) {
 		pr_warn("VSS: failed to communicate to the daemon: %d\n", rc);
-		if (cancel_delayed_work_sync(&vss_timeout_work))
+		if (cancel_delayed_work_sync(&vss_timeout_work)) {
 			vss_respond_to_host(HV_E_FAIL);
+			vss_transaction.state = HVUTIL_READY;
+		}
 	}
-	kfree(msg);
+
+	kfree(vss_msg);
 
 	return;
 }
@@ -136,17 +195,6 @@ vss_respond_to_host(int error)
 	u64	req_id;
 
 	/*
-	 * If a transaction is not active; log and return.
-	 */
-
-	if (!vss_transaction.active) {
-		/*
-		 * This is a spurious call!
-		 */
-		pr_warn("VSS: Transaction not active\n");
-		return;
-	}
-	/*
 	 * Copy the global state for completing the transaction. Note that
 	 * only one transaction can be active at a time.
 	 */
@@ -154,7 +202,6 @@ vss_respond_to_host(int error)
 	buf_len = vss_transaction.recv_len;
 	channel = vss_transaction.recv_channel;
 	req_id = vss_transaction.recv_req_id;
-	vss_transaction.active = false;
 
 	icmsghdrp = (struct icmsg_hdr *)
 			&recv_buffer[sizeof(struct vmbuspipe_hdr)];
@@ -191,14 +238,15 @@ void hv_vss_onchannelcallback(void *context)
 	struct icmsg_hdr *icmsghdrp;
 	struct icmsg_negotiate *negop = NULL;
 
-	if (vss_transaction.active) {
+	if (vss_transaction.state > HVUTIL_READY) {
 		/*
 		 * We will defer processing this callback once
 		 * the current transaction is complete.
 		 */
-		vss_transaction.recv_channel = channel;
+		vss_transaction.vss_context = context;
 		return;
 	}
+	vss_transaction.vss_context = NULL;
 
 	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE * 2, &recvlen,
 			 &requestid);
@@ -224,7 +272,6 @@ void hv_vss_onchannelcallback(void *context)
 			vss_transaction.recv_len = recvlen;
 			vss_transaction.recv_channel = channel;
 			vss_transaction.recv_req_id = requestid;
-			vss_transaction.active = true;
 			vss_transaction.msg = (struct hv_vss_msg *)vss_msg;
 
 			switch (vss_msg->vss_hdr.operation) {
@@ -241,6 +288,12 @@ void hv_vss_onchannelcallback(void *context)
 				 */
 			case VSS_OP_FREEZE:
 			case VSS_OP_THAW:
+				if (vss_transaction.state < HVUTIL_READY) {
+					/* Userspace is not registered yet */
+					vss_respond_to_host(HV_E_FAIL);
+					return;
+				}
+				vss_transaction.state = HVUTIL_HOSTMSG_RECEIVED;
 				schedule_work(&vss_send_op_work);
 				schedule_delayed_work(&vss_timeout_work,
 						      VSS_USERSPACE_TIMEOUT);
@@ -275,14 +328,16 @@ void hv_vss_onchannelcallback(void *context)
 
 }
 
+static void vss_on_reset(void)
+{
+	if (cancel_delayed_work_sync(&vss_timeout_work))
+		vss_respond_to_host(HV_E_FAIL);
+	vss_transaction.state = HVUTIL_DEVICE_INIT;
+}
+
 int
 hv_vss_init(struct hv_util_service *srv)
 {
-	int err;
-
-	err = cn_add_callback(&vss_id, vss_name, vss_cn_callback);
-	if (err)
-		return err;
 	recv_buffer = srv->recv_buffer;
 
 	/*
@@ -291,13 +346,20 @@ hv_vss_init(struct hv_util_service *srv)
 	 * Defer processing channel callbacks until the daemon
 	 * has registered.
 	 */
-	vss_transaction.active = true;
+	vss_transaction.state = HVUTIL_DEVICE_INIT;
+
+	hvt = hvutil_transport_init(vss_devname, CN_VSS_IDX, CN_VSS_VAL,
+				    vss_on_msg, vss_on_reset);
+	if (!hvt)
+		return -EFAULT;
+
 	return 0;
 }
 
 void hv_vss_deinit(void)
 {
-	cn_del_callback(&vss_id);
+	vss_transaction.state = HVUTIL_DEVICE_DYING;
 	cancel_delayed_work_sync(&vss_timeout_work);
 	cancel_work_sync(&vss_send_op_work);
+	hvutil_transport_destroy(hvt);
 }
diff --git a/drivers/hv/hv_utils_transport.c b/drivers/hv/hv_utils_transport.c
new file mode 100644
index 000000000000..ea7ba5ef16a9
--- /dev/null
+++ b/drivers/hv/hv_utils_transport.c
@@ -0,0 +1,276 @@
+/*
+ * Kernel/userspace transport abstraction for Hyper-V util driver.
+ *
+ * Copyright (C) 2015, Vitaly Kuznetsov <vkuznets@redhat.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, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/poll.h>
+
+#include "hyperv_vmbus.h"
+#include "hv_utils_transport.h"
+
+static DEFINE_SPINLOCK(hvt_list_lock);
+static struct list_head hvt_list = LIST_HEAD_INIT(hvt_list);
+
+static void hvt_reset(struct hvutil_transport *hvt)
+{
+	mutex_lock(&hvt->outmsg_lock);
+	kfree(hvt->outmsg);
+	hvt->outmsg = NULL;
+	hvt->outmsg_len = 0;
+	mutex_unlock(&hvt->outmsg_lock);
+	if (hvt->on_reset)
+		hvt->on_reset();
+}
+
+static ssize_t hvt_op_read(struct file *file, char __user *buf,
+			   size_t count, loff_t *ppos)
+{
+	struct hvutil_transport *hvt;
+	int ret;
+
+	hvt = container_of(file->f_op, struct hvutil_transport, fops);
+
+	if (wait_event_interruptible(hvt->outmsg_q, hvt->outmsg_len > 0))
+		return -EINTR;
+
+	mutex_lock(&hvt->outmsg_lock);
+	if (!hvt->outmsg) {
+		ret = -EAGAIN;
+		goto out_unlock;
+	}
+
+	if (count < hvt->outmsg_len) {
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	if (!copy_to_user(buf, hvt->outmsg, hvt->outmsg_len))
+		ret = hvt->outmsg_len;
+	else
+		ret = -EFAULT;
+
+	kfree(hvt->outmsg);
+	hvt->outmsg = NULL;
+	hvt->outmsg_len = 0;
+
+out_unlock:
+	mutex_unlock(&hvt->outmsg_lock);
+	return ret;
+}
+
+static ssize_t hvt_op_write(struct file *file, const char __user *buf,
+			    size_t count, loff_t *ppos)
+{
+	struct hvutil_transport *hvt;
+	u8 *inmsg;
+
+	hvt = container_of(file->f_op, struct hvutil_transport, fops);
+
+	inmsg = kzalloc(count, GFP_KERNEL);
+	if (copy_from_user(inmsg, buf, count)) {
+		kfree(inmsg);
+		return -EFAULT;
+	}
+	if (hvt->on_msg(inmsg, count))
+		return -EFAULT;
+	kfree(inmsg);
+
+	return count;
+}
+
+static unsigned int hvt_op_poll(struct file *file, poll_table *wait)
+{
+	struct hvutil_transport *hvt;
+
+	hvt = container_of(file->f_op, struct hvutil_transport, fops);
+
+	poll_wait(file, &hvt->outmsg_q, wait);
+	if (hvt->outmsg_len > 0)
+		return POLLIN | POLLRDNORM;
+
+	return 0;
+}
+
+static int hvt_op_open(struct inode *inode, struct file *file)
+{
+	struct hvutil_transport *hvt;
+
+	hvt = container_of(file->f_op, struct hvutil_transport, fops);
+
+	/*
+	 * Switching to CHARDEV mode. We switch bach to INIT when device
+	 * gets released.
+	 */
+	if (hvt->mode == HVUTIL_TRANSPORT_INIT)
+		hvt->mode = HVUTIL_TRANSPORT_CHARDEV;
+	else if (hvt->mode == HVUTIL_TRANSPORT_NETLINK) {
+		/*
+		 * We're switching from netlink communication to using char
+		 * device. Issue the reset first.
+		 */
+		hvt_reset(hvt);
+		hvt->mode = HVUTIL_TRANSPORT_CHARDEV;
+	} else
+		return -EBUSY;
+
+	return 0;
+}
+
+static int hvt_op_release(struct inode *inode, struct file *file)
+{
+	struct hvutil_transport *hvt;
+
+	hvt = container_of(file->f_op, struct hvutil_transport, fops);
+
+	hvt->mode = HVUTIL_TRANSPORT_INIT;
+	/*
+	 * Cleanup message buffers to avoid spurious messages when the daemon
+	 * connects back.
+	 */
+	hvt_reset(hvt);
+
+	return 0;
+}
+
+static void hvt_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
+{
+	struct hvutil_transport *hvt, *hvt_found = NULL;
+
+	spin_lock(&hvt_list_lock);
+	list_for_each_entry(hvt, &hvt_list, list) {
+		if (hvt->cn_id.idx == msg->id.idx &&
+		    hvt->cn_id.val == msg->id.val) {
+			hvt_found = hvt;
+			break;
+		}
+	}
+	spin_unlock(&hvt_list_lock);
+	if (!hvt_found) {
+		pr_warn("hvt_cn_callback: spurious message received!\n");
+		return;
+	}
+
+	/*
+	 * Switching to NETLINK mode. Switching to CHARDEV happens when someone
+	 * opens the device.
+	 */
+	if (hvt->mode == HVUTIL_TRANSPORT_INIT)
+		hvt->mode = HVUTIL_TRANSPORT_NETLINK;
+
+	if (hvt->mode == HVUTIL_TRANSPORT_NETLINK)
+		hvt_found->on_msg(msg->data, msg->len);
+	else
+		pr_warn("hvt_cn_callback: unexpected netlink message!\n");
+}
+
+int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len)
+{
+	struct cn_msg *cn_msg;
+	int ret = 0;
+
+	if (hvt->mode == HVUTIL_TRANSPORT_INIT) {
+		return -EINVAL;
+	} else if (hvt->mode == HVUTIL_TRANSPORT_NETLINK) {
+		cn_msg = kzalloc(sizeof(*cn_msg) + len, GFP_ATOMIC);
+		if (!msg)
+			return -ENOMEM;
+		cn_msg->id.idx = hvt->cn_id.idx;
+		cn_msg->id.val = hvt->cn_id.val;
+		cn_msg->len = len;
+		memcpy(cn_msg->data, msg, len);
+		ret = cn_netlink_send(cn_msg, 0, 0, GFP_ATOMIC);
+		kfree(cn_msg);
+		return ret;
+	}
+	/* HVUTIL_TRANSPORT_CHARDEV */
+	mutex_lock(&hvt->outmsg_lock);
+	if (hvt->outmsg) {
+		/* Previous message wasn't received */
+		ret = -EFAULT;
+		goto out_unlock;
+	}
+	hvt->outmsg = kzalloc(len, GFP_KERNEL);
+	memcpy(hvt->outmsg, msg, len);
+	hvt->outmsg_len = len;
+	wake_up_interruptible(&hvt->outmsg_q);
+out_unlock:
+	mutex_unlock(&hvt->outmsg_lock);
+	return ret;
+}
+
+struct hvutil_transport *hvutil_transport_init(const char *name,
+					       u32 cn_idx, u32 cn_val,
+					       int (*on_msg)(void *, int),
+					       void (*on_reset)(void))
+{
+	struct hvutil_transport *hvt;
+
+	hvt = kzalloc(sizeof(*hvt), GFP_KERNEL);
+	if (!hvt)
+		return NULL;
+
+	hvt->cn_id.idx = cn_idx;
+	hvt->cn_id.val = cn_val;
+
+	hvt->mdev.minor = MISC_DYNAMIC_MINOR;
+	hvt->mdev.name = name;
+
+	hvt->fops.owner = THIS_MODULE;
+	hvt->fops.read = hvt_op_read;
+	hvt->fops.write = hvt_op_write;
+	hvt->fops.poll = hvt_op_poll;
+	hvt->fops.open = hvt_op_open;
+	hvt->fops.release = hvt_op_release;
+
+	hvt->mdev.fops = &hvt->fops;
+
+	init_waitqueue_head(&hvt->outmsg_q);
+	mutex_init(&hvt->outmsg_lock);
+
+	spin_lock(&hvt_list_lock);
+	list_add(&hvt->list, &hvt_list);
+	spin_unlock(&hvt_list_lock);
+
+	hvt->on_msg = on_msg;
+	hvt->on_reset = on_reset;
+
+	if (misc_register(&hvt->mdev))
+		goto err_free_hvt;
+
+	/* Use cn_id.idx/cn_id.val to determine if we need to setup netlink */
+	if (hvt->cn_id.idx > 0 && hvt->cn_id.val > 0 &&
+	    cn_add_callback(&hvt->cn_id, name, hvt_cn_callback))
+		goto err_free_hvt;
+
+	return hvt;
+
+err_free_hvt:
+	kfree(hvt);
+	return NULL;
+}
+
+void hvutil_transport_destroy(struct hvutil_transport *hvt)
+{
+	spin_lock(&hvt_list_lock);
+	list_del(&hvt->list);
+	spin_unlock(&hvt_list_lock);
+	if (hvt->cn_id.idx > 0 && hvt->cn_id.val > 0)
+		cn_del_callback(&hvt->cn_id);
+	misc_deregister(&hvt->mdev);
+	kfree(hvt->outmsg);
+	kfree(hvt);
+}
diff --git a/drivers/hv/hv_utils_transport.h b/drivers/hv/hv_utils_transport.h
new file mode 100644
index 000000000000..314c76ce1b07
--- /dev/null
+++ b/drivers/hv/hv_utils_transport.h
@@ -0,0 +1,51 @@
+/*
+ * Kernel/userspace transport abstraction for Hyper-V util driver.
+ *
+ * Copyright (C) 2015, Vitaly Kuznetsov <vkuznets@redhat.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, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ */
+
+#ifndef _HV_UTILS_TRANSPORT_H
+#define _HV_UTILS_TRANSPORT_H
+
+#include <linux/connector.h>
+#include <linux/miscdevice.h>
+
+enum hvutil_transport_mode {
+	HVUTIL_TRANSPORT_INIT = 0,
+	HVUTIL_TRANSPORT_NETLINK,
+	HVUTIL_TRANSPORT_CHARDEV,
+};
+
+struct hvutil_transport {
+	int mode;                           /* hvutil_transport_mode */
+	struct file_operations fops;        /* file operations */
+	struct miscdevice mdev;             /* misc device */
+	struct cb_id cn_id;                 /* CN_*_IDX/CN_*_VAL */
+	struct list_head list;              /* hvt_list */
+	int (*on_msg)(void *, int);         /* callback on new user message */
+	void (*on_reset)(void);             /* callback when userspace drops */
+	u8 *outmsg;                         /* message to the userspace */
+	int outmsg_len;                     /* its length */
+	wait_queue_head_t outmsg_q;         /* poll/read wait queue */
+	struct mutex outmsg_lock;           /* protects outmsg */
+};
+
+struct hvutil_transport *hvutil_transport_init(const char *name,
+					       u32 cn_idx, u32 cn_val,
+					       int (*on_msg)(void *, int),
+					       void (*on_reset)(void));
+int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len);
+void hvutil_transport_destroy(struct hvutil_transport *hvt);
+
+#endif /* _HV_UTILS_TRANSPORT_H */
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index 887287ad411f..cddc0c9f6bf9 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -647,6 +647,7 @@ struct vmbus_connection {
 
 	atomic_t next_gpadl_handle;
 
+	struct completion  unload_event;
 	/*
 	 * Represents channel interrupts. Each bit position represents a
 	 * channel.  When a channel sends an interrupt via VMBUS, it finds its
@@ -730,9 +731,39 @@ int vmbus_set_event(struct vmbus_channel *channel);
 
 void vmbus_on_event(unsigned long data);
 
+int hv_kvp_init(struct hv_util_service *);
+void hv_kvp_deinit(void);
+void hv_kvp_onchannelcallback(void *);
+
+int hv_vss_init(struct hv_util_service *);
+void hv_vss_deinit(void);
+void hv_vss_onchannelcallback(void *);
+
 int hv_fcopy_init(struct hv_util_service *);
 void hv_fcopy_deinit(void);
 void hv_fcopy_onchannelcallback(void *);
+void vmbus_initiate_unload(void);
+
+static inline void hv_poll_channel(struct vmbus_channel *channel,
+				   void (*cb)(void *))
+{
+	if (!channel)
+		return;
+
+	if (channel->target_cpu != smp_processor_id())
+		smp_call_function_single(channel->target_cpu,
+					 cb, channel, true);
+	else
+		cb(channel);
+}
 
+enum hvutil_device_state {
+	HVUTIL_DEVICE_INIT = 0,  /* driver is loaded, waiting for userspace */
+	HVUTIL_READY,            /* userspace is registered */
+	HVUTIL_HOSTMSG_RECEIVED, /* message from the host was received */
+	HVUTIL_USERSPACE_REQ,    /* request to userspace was sent */
+	HVUTIL_USERSPACE_RECV,   /* reply from userspace was received */
+	HVUTIL_DEVICE_DYING,     /* driver unload is in progress */
+};
 
 #endif /* _HYPERV_VMBUS_H */
diff --git a/drivers/hv/vmbus_drv.c b/drivers/hv/vmbus_drv.c
index c85235e9f245..cf204005ee78 100644
--- a/drivers/hv/vmbus_drv.c
+++ b/drivers/hv/vmbus_drv.c
@@ -1035,6 +1035,15 @@ acpi_walk_err:
 	return ret_val;
 }
 
+static int vmbus_acpi_remove(struct acpi_device *device)
+{
+	int ret = 0;
+
+	if (hyperv_mmio.start && hyperv_mmio.end)
+		ret = release_resource(&hyperv_mmio);
+	return ret;
+}
+
 static const struct acpi_device_id vmbus_acpi_device_ids[] = {
 	{"VMBUS", 0},
 	{"VMBus", 0},
@@ -1047,6 +1056,7 @@ static struct acpi_driver vmbus_acpi_driver = {
 	.ids = vmbus_acpi_device_ids,
 	.ops = {
 		.add = vmbus_acpi_add,
+		.remove = vmbus_acpi_remove,
 	},
 };
 
@@ -1096,15 +1106,22 @@ static void __exit vmbus_exit(void)
 
 	vmbus_connection.conn_state = DISCONNECTED;
 	hv_synic_clockevents_cleanup();
+	vmbus_disconnect();
 	hv_remove_vmbus_irq();
+	tasklet_kill(&msg_dpc);
 	vmbus_free_channels();
+	if (ms_hyperv.features & HV_FEATURE_GUEST_CRASH_MSR_AVAILABLE) {
+		atomic_notifier_chain_unregister(&panic_notifier_list,
+						 &hyperv_panic_block);
+	}
 	bus_unregister(&hv_bus);
 	hv_cleanup();
-	for_each_online_cpu(cpu)
+	for_each_online_cpu(cpu) {
+		tasklet_kill(hv_context.event_dpc[cpu]);
 		smp_call_function_single(cpu, hv_synic_cleanup, NULL, 1);
+	}
 	acpi_bus_unregister_driver(&vmbus_acpi_driver);
 	hv_cpu_hotplug_quirk(false);
-	vmbus_disconnect();
 }
 
 
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 25d9e72627e9..7c65b7334738 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -371,6 +371,17 @@ config SENSORS_DS1621
 	  This driver can also be built as a module.  If so, the module
 	  will be called ds1621.
 
+config SENSORS_DELL_SMM
+	tristate "Dell laptop SMM BIOS hwmon driver"
+	depends on X86
+	help
+	  This hwmon driver adds support for reporting temperature of different
+	  sensors and controls the fans on Dell laptops via System Management
+	  Mode provided by Dell BIOS.
+
+	  When option I8K is also enabled this driver provides legacy /proc/i8k
+	  userspace interface for i8kutils package.
+
 config SENSORS_DA9052_ADC
 	tristate "Dialog DA9052/DA9053 ADC"
 	depends on PMIC_DA9052
@@ -509,7 +520,7 @@ config SENSORS_G762
 
 config SENSORS_GPIO_FAN
 	tristate "GPIO fan"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	depends on THERMAL || THERMAL=n
 	help
 	  If you say yes here you get support for fans connected to GPIO lines.
@@ -1106,8 +1117,8 @@ config SENSORS_NTC_THERMISTOR
 	  send notifications about the temperature.
 
 	  Currently, this driver supports
-	  NCP15WB473, NCP18WB473, NCP21WB473, NCP03WB473, and NCP15WL333
-	  from Murata and B57330V2103 from EPCOS.
+	  NCP15WB473, NCP18WB473, NCP21WB473, NCP03WB473, NCP15WL333,
+	  and NCP03WF104 from Murata and B57330V2103 from EPCOS.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called ntc-thermistor.
@@ -1186,7 +1197,7 @@ config SENSORS_PWM_FAN
 
 config SENSORS_SHT15
 	tristate "Sensiron humidity and temperature sensors. SHT15 and compat."
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  If you say yes here you get support for the Sensiron SHT10, SHT11,
 	  SHT15, SHT71, SHT75 humidity and temperature sensors.
@@ -1452,6 +1463,16 @@ config SENSORS_INA2XX
 	  This driver can also be built as a module.  If so, the module
 	  will be called ina2xx.
 
+config SENSORS_TC74
+	tristate "Microchip TC74"
+	depends on I2C
+	help
+	  If you say yes here you get support for Microchip TC74 single
+	  input temperature sensor chips.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called tc74.
+
 config SENSORS_THMC50
 	tristate "Texas Instruments THMC50 / Analog Devices ADM1022"
 	depends on I2C
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index b4a40f17e2aa..9e0f3dd2841d 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -49,6 +49,7 @@ obj-$(CONFIG_SENSORS_ATXP1)	+= atxp1.o
 obj-$(CONFIG_SENSORS_CORETEMP)	+= coretemp.o
 obj-$(CONFIG_SENSORS_DA9052_ADC)+= da9052-hwmon.o
 obj-$(CONFIG_SENSORS_DA9055)+= da9055-hwmon.o
+obj-$(CONFIG_SENSORS_DELL_SMM)	+= dell-smm-hwmon.o
 obj-$(CONFIG_SENSORS_DME1737)	+= dme1737.o
 obj-$(CONFIG_SENSORS_DS620)	+= ds620.o
 obj-$(CONFIG_SENSORS_DS1621)	+= ds1621.o
@@ -140,6 +141,7 @@ obj-$(CONFIG_SENSORS_SMSC47B397)+= smsc47b397.o
 obj-$(CONFIG_SENSORS_SMSC47M1)	+= smsc47m1.o
 obj-$(CONFIG_SENSORS_SMSC47M192)+= smsc47m192.o
 obj-$(CONFIG_SENSORS_AMC6821)	+= amc6821.o
+obj-$(CONFIG_SENSORS_TC74)	+= tc74.o
 obj-$(CONFIG_SENSORS_THMC50)	+= thmc50.o
 obj-$(CONFIG_SENSORS_TMP102)	+= tmp102.o
 obj-$(CONFIG_SENSORS_TMP103)	+= tmp103.o
diff --git a/drivers/hwmon/atxp1.c b/drivers/hwmon/atxp1.c
index 4c829bb2f9db..f2f2f2fc755a 100644
--- a/drivers/hwmon/atxp1.c
+++ b/drivers/hwmon/atxp1.c
@@ -12,10 +12,9 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
+ * The ATXP1 can reside on I2C addresses 0x37 or 0x4e. The chip is
+ * not auto-detected by the driver and must be instantiated explicitly.
+ * See Documentation/i2c/instantiating-devices for more information.
  */
 
 #include <linux/kernel.h>
@@ -43,8 +42,6 @@ MODULE_AUTHOR("Sebastian Witt <se.witt@gmx.net>");
 #define ATXP1_VIDMASK	0x1f
 #define ATXP1_GPIO1MASK	0x0f
 
-static const unsigned short normal_i2c[] = { 0x37, 0x4e, I2C_CLIENT_END };
-
 struct atxp1_data {
 	struct i2c_client *client;
 	struct mutex update_lock;
@@ -259,48 +256,6 @@ static struct attribute *atxp1_attrs[] = {
 };
 ATTRIBUTE_GROUPS(atxp1);
 
-/* Return 0 if detection is successful, -ENODEV otherwise */
-static int atxp1_detect(struct i2c_client *new_client,
-			struct i2c_board_info *info)
-{
-	struct i2c_adapter *adapter = new_client->adapter;
-
-	u8 temp;
-
-	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -ENODEV;
-
-	/* Detect ATXP1, checking if vendor ID registers are all zero */
-	if (!((i2c_smbus_read_byte_data(new_client, 0x3e) == 0) &&
-	     (i2c_smbus_read_byte_data(new_client, 0x3f) == 0) &&
-	     (i2c_smbus_read_byte_data(new_client, 0xfe) == 0) &&
-	     (i2c_smbus_read_byte_data(new_client, 0xff) == 0)))
-		return -ENODEV;
-
-	/*
-	 * No vendor ID, now checking if registers 0x10,0x11 (non-existent)
-	 * showing the same as register 0x00
-	 */
-	temp = i2c_smbus_read_byte_data(new_client, 0x00);
-
-	if (!((i2c_smbus_read_byte_data(new_client, 0x10) == temp) &&
-	      (i2c_smbus_read_byte_data(new_client, 0x11) == temp)))
-		return -ENODEV;
-
-	/* Get VRM */
-	temp = vid_which_vrm();
-
-	if ((temp != 90) && (temp != 91)) {
-		dev_err(&adapter->dev, "atxp1: Not supporting VRM %d.%d\n",
-				temp / 10, temp % 10);
-		return -ENODEV;
-	}
-
-	strlcpy(info->type, "atxp1", I2C_NAME_SIZE);
-
-	return 0;
-}
-
 static int atxp1_probe(struct i2c_client *client,
 		       const struct i2c_device_id *id)
 {
@@ -314,6 +269,11 @@ static int atxp1_probe(struct i2c_client *client,
 
 	/* Get VRM */
 	data->vrm = vid_which_vrm();
+	if (data->vrm != 90 && data->vrm != 91) {
+		dev_err(dev, "atxp1: Not supporting VRM %d.%d\n",
+			data->vrm / 10, data->vrm % 10);
+		return -ENODEV;
+	}
 
 	data->client = client;
 	mutex_init(&data->update_lock);
@@ -342,8 +302,6 @@ static struct i2c_driver atxp1_driver = {
 	},
 	.probe		= atxp1_probe,
 	.id_table	= atxp1_id,
-	.detect		= atxp1_detect,
-	.address_list	= normal_i2c,
 };
 
 module_i2c_driver(atxp1_driver);
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index ed303ba3a593..3e03379e7c5d 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -63,7 +63,8 @@ MODULE_PARM_DESC(tjmax, "TjMax value in degrees Celsius");
 #define TO_ATTR_NO(cpu)		(TO_CORE_ID(cpu) + BASE_SYSFS_ATTR_NO)
 
 #ifdef CONFIG_SMP
-#define for_each_sibling(i, cpu)	for_each_cpu(i, cpu_sibling_mask(cpu))
+#define for_each_sibling(i, cpu) \
+	for_each_cpu(i, topology_sibling_cpumask(cpu))
 #else
 #define for_each_sibling(i, cpu)	for (i = 0; false; )
 #endif
diff --git a/drivers/char/i8k.c b/drivers/hwmon/dell-smm-hwmon.c
index a43048b5b05f..37c16afe007a 100644
--- a/drivers/char/i8k.c
+++ b/drivers/hwmon/dell-smm-hwmon.c
@@ -1,12 +1,12 @@
 /*
- * i8k.c -- Linux driver for accessing the SMM BIOS on Dell laptops.
+ * dell-smm-hwmon.c -- Linux driver for accessing the SMM BIOS on Dell laptops.
  *
  * Copyright (C) 2001  Massimo Dal Zotto <dz@debian.org>
  *
  * Hwmon integration:
  * Copyright (C) 2011  Jean Delvare <jdelvare@suse.de>
  * Copyright (C) 2013, 2014  Guenter Roeck <linux@roeck-us.net>
- * Copyright (C) 2014  Pali Rohár <pali.rohar@gmail.com>
+ * Copyright (C) 2014, 2015  Pali Rohár <pali.rohar@gmail.com>
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License as published by the
@@ -80,8 +80,10 @@ static uint i8k_fan_max = I8K_FAN_HIGH;
 #define I8K_HWMON_HAVE_FAN2	(1 << 5)
 
 MODULE_AUTHOR("Massimo Dal Zotto (dz@debian.org)");
-MODULE_DESCRIPTION("Driver for accessing SMM BIOS on Dell laptops");
+MODULE_AUTHOR("Pali Rohár <pali.rohar@gmail.com>");
+MODULE_DESCRIPTION("Dell laptop SMM BIOS hwmon driver");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("i8k");
 
 static bool force;
 module_param(force, bool, 0);
@@ -91,6 +93,7 @@ static bool ignore_dmi;
 module_param(ignore_dmi, bool, 0);
 MODULE_PARM_DESC(ignore_dmi, "Continue probing hardware even if DMI data does not match");
 
+#if IS_ENABLED(CONFIG_I8K)
 static bool restricted;
 module_param(restricted, bool, 0);
 MODULE_PARM_DESC(restricted, "Allow fan control if SYS_ADMIN capability set");
@@ -98,6 +101,7 @@ MODULE_PARM_DESC(restricted, "Allow fan control if SYS_ADMIN capability set");
 static bool power_status;
 module_param(power_status, bool, 0600);
 MODULE_PARM_DESC(power_status, "Report power status in /proc/i8k");
+#endif
 
 static uint fan_mult;
 module_param(fan_mult, uint, 0);
@@ -107,18 +111,6 @@ static uint fan_max;
 module_param(fan_max, uint, 0);
 MODULE_PARM_DESC(fan_max, "Maximum configurable fan speed (default: autodetect)");
 
-static int i8k_open_fs(struct inode *inode, struct file *file);
-static long i8k_ioctl(struct file *, unsigned int, unsigned long);
-
-static const struct file_operations i8k_fops = {
-	.owner		= THIS_MODULE,
-	.open		= i8k_open_fs,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-	.unlocked_ioctl	= i8k_ioctl,
-};
-
 struct smm_regs {
 	unsigned int eax;
 	unsigned int ebx __packed;
@@ -219,45 +211,6 @@ out:
 }
 
 /*
- * Read the Fn key status.
- */
-static int i8k_get_fn_status(void)
-{
-	struct smm_regs regs = { .eax = I8K_SMM_FN_STATUS, };
-	int rc;
-
-	rc = i8k_smm(&regs);
-	if (rc < 0)
-		return rc;
-
-	switch ((regs.eax >> I8K_FN_SHIFT) & I8K_FN_MASK) {
-	case I8K_FN_UP:
-		return I8K_VOL_UP;
-	case I8K_FN_DOWN:
-		return I8K_VOL_DOWN;
-	case I8K_FN_MUTE:
-		return I8K_VOL_MUTE;
-	default:
-		return 0;
-	}
-}
-
-/*
- * Read the power status.
- */
-static int i8k_get_power_status(void)
-{
-	struct smm_regs regs = { .eax = I8K_SMM_POWER_STATUS, };
-	int rc;
-
-	rc = i8k_smm(&regs);
-	if (rc < 0)
-		return rc;
-
-	return (regs.eax & 0xff) == I8K_POWER_AC ? I8K_AC : I8K_BATTERY;
-}
-
-/*
  * Read the fan status.
  */
 static int i8k_get_fan_status(int fan)
@@ -376,6 +329,51 @@ static int i8k_get_dell_signature(int req_fn)
 	return regs.eax == 1145651527 && regs.edx == 1145392204 ? 0 : -1;
 }
 
+#if IS_ENABLED(CONFIG_I8K)
+
+/*
+ * Read the Fn key status.
+ */
+static int i8k_get_fn_status(void)
+{
+	struct smm_regs regs = { .eax = I8K_SMM_FN_STATUS, };
+	int rc;
+
+	rc = i8k_smm(&regs);
+	if (rc < 0)
+		return rc;
+
+	switch ((regs.eax >> I8K_FN_SHIFT) & I8K_FN_MASK) {
+	case I8K_FN_UP:
+		return I8K_VOL_UP;
+	case I8K_FN_DOWN:
+		return I8K_VOL_DOWN;
+	case I8K_FN_MUTE:
+		return I8K_VOL_MUTE;
+	default:
+		return 0;
+	}
+}
+
+/*
+ * Read the power status.
+ */
+static int i8k_get_power_status(void)
+{
+	struct smm_regs regs = { .eax = I8K_SMM_POWER_STATUS, };
+	int rc;
+
+	rc = i8k_smm(&regs);
+	if (rc < 0)
+		return rc;
+
+	return (regs.eax & 0xff) == I8K_POWER_AC ? I8K_AC : I8K_BATTERY;
+}
+
+/*
+ * Procfs interface
+ */
+
 static int
 i8k_ioctl_unlocked(struct file *fp, unsigned int cmd, unsigned long arg)
 {
@@ -526,6 +524,37 @@ static int i8k_open_fs(struct inode *inode, struct file *file)
 	return single_open(file, i8k_proc_show, NULL);
 }
 
+static const struct file_operations i8k_fops = {
+	.owner		= THIS_MODULE,
+	.open		= i8k_open_fs,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+	.unlocked_ioctl	= i8k_ioctl,
+};
+
+static void __init i8k_init_procfs(void)
+{
+	/* Register the proc entry */
+	proc_create("i8k", 0, NULL, &i8k_fops);
+}
+
+static void __exit i8k_exit_procfs(void)
+{
+	remove_proc_entry("i8k", NULL);
+}
+
+#else
+
+static inline void __init i8k_init_procfs(void)
+{
+}
+
+static inline void __exit i8k_exit_procfs(void)
+{
+}
+
+#endif
 
 /*
  * Hwmon interface
@@ -748,8 +777,8 @@ static int __init i8k_init_hwmon(void)
 	if (err >= 0)
 		i8k_hwmon_flags |= I8K_HWMON_HAVE_FAN2;
 
-	i8k_hwmon_dev = hwmon_device_register_with_groups(NULL, "i8k", NULL,
-							  i8k_groups);
+	i8k_hwmon_dev = hwmon_device_register_with_groups(NULL, "dell_smm",
+							  NULL, i8k_groups);
 	if (IS_ERR(i8k_hwmon_dev)) {
 		err = PTR_ERR(i8k_hwmon_dev);
 		i8k_hwmon_dev = NULL;
@@ -974,33 +1003,24 @@ static int __init i8k_probe(void)
 
 static int __init i8k_init(void)
 {
-	struct proc_dir_entry *proc_i8k;
 	int err;
 
 	/* Are we running on an supported laptop? */
 	if (i8k_probe())
 		return -ENODEV;
 
-	/* Register the proc entry */
-	proc_i8k = proc_create("i8k", 0, NULL, &i8k_fops);
-	if (!proc_i8k)
-		return -ENOENT;
-
 	err = i8k_init_hwmon();
 	if (err)
-		goto exit_remove_proc;
+		return err;
 
+	i8k_init_procfs();
 	return 0;
-
- exit_remove_proc:
-	remove_proc_entry("i8k", NULL);
-	return err;
 }
 
 static void __exit i8k_exit(void)
 {
 	hwmon_device_unregister(i8k_hwmon_dev);
-	remove_proc_entry("i8k", NULL);
+	i8k_exit_procfs();
 }
 
 module_init(i8k_init);
diff --git a/drivers/hwmon/max197.c b/drivers/hwmon/max197.c
index cb0dcfda958c..07628569547a 100644
--- a/drivers/hwmon/max197.c
+++ b/drivers/hwmon/max197.c
@@ -324,7 +324,7 @@ static int max197_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id max197_device_ids[] = {
+static const struct platform_device_id max197_device_ids[] = {
 	{ "max197", max197 },
 	{ "max199", max199 },
 	{ }
diff --git a/drivers/hwmon/mcp3021.c b/drivers/hwmon/mcp3021.c
index d219c06a857b..972444a14cca 100644
--- a/drivers/hwmon/mcp3021.c
+++ b/drivers/hwmon/mcp3021.c
@@ -31,14 +31,11 @@
 /* output format */
 #define MCP3021_SAR_SHIFT	2
 #define MCP3021_SAR_MASK	0x3ff
-
 #define MCP3021_OUTPUT_RES	10	/* 10-bit resolution */
-#define MCP3021_OUTPUT_SCALE	4
 
 #define MCP3221_SAR_SHIFT	0
 #define MCP3221_SAR_MASK	0xfff
 #define MCP3221_OUTPUT_RES	12	/* 12-bit resolution */
-#define MCP3221_OUTPUT_SCALE	1
 
 enum chips {
 	mcp3021,
@@ -54,7 +51,6 @@ struct mcp3021_data {
 	u16 sar_shift;
 	u16 sar_mask;
 	u8 output_res;
-	u8 output_scale;
 };
 
 static int mcp3021_read16(struct i2c_client *client)
@@ -84,13 +80,7 @@ static int mcp3021_read16(struct i2c_client *client)
 
 static inline u16 volts_from_reg(struct mcp3021_data *data, u16 val)
 {
-	if (val == 0)
-		return 0;
-
-	val = val * data->output_scale - data->output_scale / 2;
-
-	return val * DIV_ROUND_CLOSEST(data->vdd,
-			(1 << data->output_res) * data->output_scale);
+	return DIV_ROUND_CLOSEST(data->vdd * val, 1 << data->output_res);
 }
 
 static ssize_t show_in_input(struct device *dev, struct device_attribute *attr,
@@ -132,14 +122,12 @@ static int mcp3021_probe(struct i2c_client *client,
 		data->sar_shift = MCP3021_SAR_SHIFT;
 		data->sar_mask = MCP3021_SAR_MASK;
 		data->output_res = MCP3021_OUTPUT_RES;
-		data->output_scale = MCP3021_OUTPUT_SCALE;
 		break;
 
 	case mcp3221:
 		data->sar_shift = MCP3221_SAR_SHIFT;
 		data->sar_mask = MCP3221_SAR_MASK;
 		data->output_res = MCP3221_OUTPUT_RES;
-		data->output_scale = MCP3221_OUTPUT_SCALE;
 		break;
 	}
 
diff --git a/drivers/hwmon/nct7802.c b/drivers/hwmon/nct7802.c
index 55765790907b..28fcb2e246d5 100644
--- a/drivers/hwmon/nct7802.c
+++ b/drivers/hwmon/nct7802.c
@@ -547,7 +547,7 @@ static umode_t nct7802_temp_is_visible(struct kobject *kobj,
 	if (index >= 9 && index < 18 &&
 	    (reg & 0x0c) != 0x04 && (reg & 0x0c) != 0x08)	/* RD2 */
 		return 0;
-	if (index >= 18 && index < 27 && (reg & 0x30) != 0x10)	/* RD3 */
+	if (index >= 18 && index < 27 && (reg & 0x30) != 0x20)	/* RD3 */
 		return 0;
 	if (index >= 27 && index < 35)				/* local */
 		return attr->mode;
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index 68800115876b..dc0b76c5e302 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -53,6 +53,7 @@ static const struct platform_device_id ntc_thermistor_id[] = {
 	{ "ncp03wb473", TYPE_NCPXXWB473 },
 	{ "ncp15wl333", TYPE_NCPXXWL333 },
 	{ "b57330v2103", TYPE_B57330V2103},
+	{ "ncp03wf104", TYPE_NCPXXWF104 },
 	{ },
 };
 
@@ -135,6 +136,43 @@ static const struct ntc_compensation ncpXXwl333[] = {
 	{ .temp_c	= 125, .ohm	= 707 },
 };
 
+static const struct ntc_compensation ncpXXwf104[] = {
+	{ .temp_c	= -40, .ohm	= 4397119 },
+	{ .temp_c	= -35, .ohm	= 3088599 },
+	{ .temp_c	= -30, .ohm	= 2197225 },
+	{ .temp_c	= -25, .ohm	= 1581881 },
+	{ .temp_c	= -20, .ohm	= 1151037 },
+	{ .temp_c	= -15, .ohm	= 846579 },
+	{ .temp_c	= -10, .ohm	= 628988 },
+	{ .temp_c	= -5, .ohm	= 471632 },
+	{ .temp_c	= 0, .ohm	= 357012 },
+	{ .temp_c	= 5, .ohm	= 272500 },
+	{ .temp_c	= 10, .ohm	= 209710 },
+	{ .temp_c	= 15, .ohm	= 162651 },
+	{ .temp_c	= 20, .ohm	= 127080 },
+	{ .temp_c	= 25, .ohm	= 100000 },
+	{ .temp_c	= 30, .ohm	= 79222 },
+	{ .temp_c	= 35, .ohm	= 63167 },
+	{ .temp_c	= 40, .ohm	= 50677 },
+	{ .temp_c	= 45, .ohm	= 40904 },
+	{ .temp_c	= 50, .ohm	= 33195 },
+	{ .temp_c	= 55, .ohm	= 27091 },
+	{ .temp_c	= 60, .ohm	= 22224 },
+	{ .temp_c	= 65, .ohm	= 18323 },
+	{ .temp_c	= 70, .ohm	= 15184 },
+	{ .temp_c	= 75, .ohm	= 12635 },
+	{ .temp_c	= 80, .ohm	= 10566 },
+	{ .temp_c	= 85, .ohm	= 8873 },
+	{ .temp_c	= 90, .ohm	= 7481 },
+	{ .temp_c	= 95, .ohm	= 6337 },
+	{ .temp_c	= 100, .ohm	= 5384 },
+	{ .temp_c	= 105, .ohm	= 4594 },
+	{ .temp_c	= 110, .ohm	= 3934 },
+	{ .temp_c	= 115, .ohm	= 3380 },
+	{ .temp_c	= 120, .ohm	= 2916 },
+	{ .temp_c	= 125, .ohm	= 2522 },
+};
+
 /*
  * The following compensation table is from the specification of EPCOS NTC
  * Thermistors Datasheet
@@ -190,20 +228,21 @@ struct ntc_data {
 static int ntc_adc_iio_read(struct ntc_thermistor_platform_data *pdata)
 {
 	struct iio_channel *channel = pdata->chan;
-	s64 result;
-	int val, ret;
+	int raw, uv, ret;
 
-	ret = iio_read_channel_raw(channel, &val);
+	ret = iio_read_channel_raw(channel, &raw);
 	if (ret < 0) {
 		pr_err("read channel() error: %d\n", ret);
 		return ret;
 	}
 
-	/* unit: mV */
-	result = pdata->pullup_uv * (s64) val;
-	result >>= 12;
+	ret = iio_convert_raw_to_processed(channel, raw, &uv, 1000);
+	if (ret < 0) {
+		/* Assume 12 bit ADC with vref at pullup_uv */
+		uv = (pdata->pullup_uv * (s64)raw) >> 12;
+	}
 
-	return (int)result;
+	return uv;
 }
 
 static const struct of_device_id ntc_match[] = {
@@ -219,6 +258,8 @@ static const struct of_device_id ntc_match[] = {
 		.data = &ntc_thermistor_id[4] },
 	{ .compatible = "epcos,b57330v2103",
 		.data = &ntc_thermistor_id[5]},
+	{ .compatible = "murata,ncp03wf104",
+		.data = &ntc_thermistor_id[6] },
 
 	/* Usage of vendor name "ntc" is deprecated */
 	{ .compatible = "ntc,ncp15wb473",
@@ -309,30 +350,27 @@ static inline u64 div64_u64_safe(u64 dividend, u64 divisor)
 static int get_ohm_of_thermistor(struct ntc_data *data, unsigned int uv)
 {
 	struct ntc_thermistor_platform_data *pdata = data->pdata;
-	u64 mv = uv / 1000;
-	u64 pmv = pdata->pullup_uv / 1000;
+	u32 puv = pdata->pullup_uv;
 	u64 n, puo, pdo;
 	puo = pdata->pullup_ohm;
 	pdo = pdata->pulldown_ohm;
 
-	if (mv == 0) {
-		if (pdata->connect == NTC_CONNECTED_POSITIVE)
-			return INT_MAX;
-		return 0;
-	}
-	if (mv >= pmv)
+	if (uv == 0)
+		return (pdata->connect == NTC_CONNECTED_POSITIVE) ?
+			INT_MAX : 0;
+	if (uv >= puv)
 		return (pdata->connect == NTC_CONNECTED_POSITIVE) ?
 			0 : INT_MAX;
 
 	if (pdata->connect == NTC_CONNECTED_POSITIVE && puo == 0)
-		n = div64_u64_safe(pdo * (pmv - mv), mv);
+		n = div_u64(pdo * (puv - uv), uv);
 	else if (pdata->connect == NTC_CONNECTED_GROUND && pdo == 0)
-		n = div64_u64_safe(puo * mv, pmv - mv);
+		n = div_u64(puo * uv, puv - uv);
 	else if (pdata->connect == NTC_CONNECTED_POSITIVE)
-		n = div64_u64_safe(pdo * puo * (pmv - mv),
-				puo * mv - pdo * (pmv - mv));
+		n = div64_u64_safe(pdo * puo * (puv - uv),
+				puo * uv - pdo * (puv - uv));
 	else
-		n = div64_u64_safe(pdo * puo * mv, pdo * (pmv - mv) - puo * mv);
+		n = div64_u64_safe(pdo * puo * uv, pdo * (puv - uv) - puo * uv);
 
 	if (n > INT_MAX)
 		n = INT_MAX;
@@ -567,6 +605,10 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 		data->comp = b57330v2103;
 		data->n_comp = ARRAY_SIZE(b57330v2103);
 		break;
+	case TYPE_NCPXXWF104:
+		data->comp = ncpXXwf104;
+		data->n_comp = ARRAY_SIZE(ncpXXwf104);
+		break;
 	default:
 		dev_err(&pdev->dev, "Unknown device type: %lu(%s)\n",
 				pdev_id->driver_data, pdev_id->name);
diff --git a/drivers/hwmon/sht15.c b/drivers/hwmon/sht15.c
index d4f0935daaa1..497a7f822a12 100644
--- a/drivers/hwmon/sht15.c
+++ b/drivers/hwmon/sht15.c
@@ -1074,7 +1074,7 @@ static int sht15_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id sht15_device_ids[] = {
+static const struct platform_device_id sht15_device_ids[] = {
 	{ "sht10", sht10 },
 	{ "sht11", sht11 },
 	{ "sht15", sht15 },
diff --git a/drivers/hwmon/tc74.c b/drivers/hwmon/tc74.c
new file mode 100644
index 000000000000..d95165158800
--- /dev/null
+++ b/drivers/hwmon/tc74.c
@@ -0,0 +1,177 @@
+/*
+ * An hwmon driver for the Microchip TC74
+ *
+ * Copyright 2015 Maciej Szmigiero <mail@maciej.szmigiero.name>
+ *
+ * Based on ad7414.c:
+ *	Copyright 2006 Stefan Roese, DENX Software Engineering
+ *	Copyright 2008 Sean MacLennan, PIKA Technologies
+ *	Copyright 2008 Frank Edelhaeuser, Spansion Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/bitops.h>
+#include <linux/err.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/i2c.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+
+/* TC74 registers */
+#define TC74_REG_TEMP		0x00
+#define TC74_REG_CONFIG		0x01
+
+struct tc74_data {
+	struct i2c_client	*client;
+	struct mutex		lock;	/* atomic read data updates */
+	bool			valid;	/* validity of fields below */
+	unsigned long		next_update;	/* In jiffies */
+	s8			temp_input;	/* Temp value in dC */
+};
+
+static int tc74_update_device(struct device *dev)
+{
+	struct tc74_data *data = dev_get_drvdata(dev);
+	struct i2c_client *client = data->client;
+	int ret;
+
+	ret = mutex_lock_interruptible(&data->lock);
+	if (ret)
+		return ret;
+
+	if (time_after(jiffies, data->next_update) || !data->valid) {
+		s32 value;
+
+		value = i2c_smbus_read_byte_data(client, TC74_REG_CONFIG);
+		if (value < 0) {
+			dev_dbg(&client->dev, "TC74_REG_CONFIG read err %d\n",
+				(int)value);
+
+			ret = value;
+			goto ret_unlock;
+		}
+
+		if (!(value & BIT(6))) {
+			/* not ready yet */
+
+			ret = -EAGAIN;
+			goto ret_unlock;
+		}
+
+		value = i2c_smbus_read_byte_data(client, TC74_REG_TEMP);
+		if (value < 0) {
+			dev_dbg(&client->dev, "TC74_REG_TEMP read err %d\n",
+				(int)value);
+
+			ret = value;
+			goto ret_unlock;
+		}
+
+		data->temp_input = value;
+		data->next_update = jiffies + HZ / 4;
+		data->valid = true;
+	}
+
+ret_unlock:
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static ssize_t show_temp_input(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	struct tc74_data *data = dev_get_drvdata(dev);
+	int ret;
+
+	ret = tc74_update_device(dev);
+	if (ret)
+		return ret;
+
+	return sprintf(buf, "%d\n", data->temp_input * 1000);
+}
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp_input, NULL, 0);
+
+static struct attribute *tc74_attrs[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	NULL
+};
+
+ATTRIBUTE_GROUPS(tc74);
+
+static int tc74_probe(struct i2c_client *client,
+		      const struct i2c_device_id *dev_id)
+{
+	struct device *dev = &client->dev;
+	struct tc74_data *data;
+	struct device *hwmon_dev;
+	s32 conf;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EOPNOTSUPP;
+
+	data = devm_kzalloc(dev, sizeof(struct tc74_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->client = client;
+	mutex_init(&data->lock);
+
+	/* Make sure the chip is powered up. */
+	conf = i2c_smbus_read_byte_data(client, TC74_REG_CONFIG);
+	if (conf < 0) {
+		dev_err(dev, "unable to read config register\n");
+
+		return conf;
+	}
+
+	if (conf & 0x3f) {
+		dev_err(dev, "invalid config register value\n");
+
+		return -ENODEV;
+	}
+
+	if (conf & BIT(7)) {
+		s32 ret;
+
+		conf &= ~BIT(7);
+
+		ret = i2c_smbus_write_byte_data(client, TC74_REG_CONFIG, conf);
+		if (ret)
+			dev_warn(dev, "unable to disable STANDBY\n");
+	}
+
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev,
+							   client->name,
+							   data, tc74_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
+}
+
+static const struct i2c_device_id tc74_id[] = {
+	{ "tc74", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, tc74_id);
+
+static struct i2c_driver tc74_driver = {
+	.driver = {
+		.name	= "tc74",
+	},
+	.probe	= tc74_probe,
+	.id_table = tc74_id,
+};
+
+module_i2c_driver(tc74_driver);
+
+MODULE_AUTHOR("Maciej Szmigiero <mail@maciej.szmigiero.name>");
+
+MODULE_DESCRIPTION("TC74 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/w83627ehf.c b/drivers/hwmon/w83627ehf.c
index b10353b31806..697007afb99c 100644
--- a/drivers/hwmon/w83627ehf.c
+++ b/drivers/hwmon/w83627ehf.c
@@ -1937,27 +1937,11 @@ static inline void w83627ehf_init_device(struct w83627ehf_data *data,
 static void w82627ehf_swap_tempreg(struct w83627ehf_data *data,
 				   int r1, int r2)
 {
-	u16 tmp;
-
-	tmp = data->temp_src[r1];
-	data->temp_src[r1] = data->temp_src[r2];
-	data->temp_src[r2] = tmp;
-
-	tmp = data->reg_temp[r1];
-	data->reg_temp[r1] = data->reg_temp[r2];
-	data->reg_temp[r2] = tmp;
-
-	tmp = data->reg_temp_over[r1];
-	data->reg_temp_over[r1] = data->reg_temp_over[r2];
-	data->reg_temp_over[r2] = tmp;
-
-	tmp = data->reg_temp_hyst[r1];
-	data->reg_temp_hyst[r1] = data->reg_temp_hyst[r2];
-	data->reg_temp_hyst[r2] = tmp;
-
-	tmp = data->reg_temp_config[r1];
-	data->reg_temp_config[r1] = data->reg_temp_config[r2];
-	data->reg_temp_config[r2] = tmp;
+	swap(data->temp_src[r1], data->temp_src[r2]);
+	swap(data->reg_temp[r1], data->reg_temp[r2]);
+	swap(data->reg_temp_over[r1], data->reg_temp_over[r2]);
+	swap(data->reg_temp_hyst[r1], data->reg_temp_hyst[r2]);
+	swap(data->reg_temp_config[r1], data->reg_temp_config[r2]);
 }
 
 static void
diff --git a/drivers/hwmon/w83792d.c b/drivers/hwmon/w83792d.c
index 4068db4d9580..0a8bce726b4b 100644
--- a/drivers/hwmon/w83792d.c
+++ b/drivers/hwmon/w83792d.c
@@ -289,10 +289,7 @@ struct w83792d_data {
 	u8 temp1[3];		/* current, over, thyst */
 	u8 temp_add[2][6];	/* Register value */
 	u8 fan_div[7];		/* Register encoding, shifted right */
-	u8 pwm[7];		/*
-				 * We only consider the first 3 set of pwm,
-				 * although 792 chip has 7 set of pwm.
-				 */
+	u8 pwm[7];		/* The 7 PWM outputs */
 	u8 pwmenable[3];
 	u32 alarms;		/* realtime status register encoding,combined */
 	u8 chassis;		/* Chassis status */
@@ -1075,6 +1072,10 @@ static DEVICE_ATTR(intrusion0_alarm, S_IRUGO | S_IWUSR,
 static SENSOR_DEVICE_ATTR(pwm1, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 0);
 static SENSOR_DEVICE_ATTR(pwm2, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 1);
 static SENSOR_DEVICE_ATTR(pwm3, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 2);
+static SENSOR_DEVICE_ATTR(pwm4, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 3);
+static SENSOR_DEVICE_ATTR(pwm5, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 4);
+static SENSOR_DEVICE_ATTR(pwm6, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 5);
+static SENSOR_DEVICE_ATTR(pwm7, S_IWUSR | S_IRUGO, show_pwm, store_pwm, 6);
 static SENSOR_DEVICE_ATTR(pwm1_enable, S_IWUSR | S_IRUGO,
 			show_pwmenable, store_pwmenable, 1);
 static SENSOR_DEVICE_ATTR(pwm2_enable, S_IWUSR | S_IRUGO,
@@ -1087,6 +1088,14 @@ static SENSOR_DEVICE_ATTR(pwm2_mode, S_IWUSR | S_IRUGO,
 			show_pwm_mode, store_pwm_mode, 1);
 static SENSOR_DEVICE_ATTR(pwm3_mode, S_IWUSR | S_IRUGO,
 			show_pwm_mode, store_pwm_mode, 2);
+static SENSOR_DEVICE_ATTR(pwm4_mode, S_IWUSR | S_IRUGO,
+			show_pwm_mode, store_pwm_mode, 3);
+static SENSOR_DEVICE_ATTR(pwm5_mode, S_IWUSR | S_IRUGO,
+			show_pwm_mode, store_pwm_mode, 4);
+static SENSOR_DEVICE_ATTR(pwm6_mode, S_IWUSR | S_IRUGO,
+			show_pwm_mode, store_pwm_mode, 5);
+static SENSOR_DEVICE_ATTR(pwm7_mode, S_IWUSR | S_IRUGO,
+			show_pwm_mode, store_pwm_mode, 6);
 static SENSOR_DEVICE_ATTR(tolerance1, S_IWUSR | S_IRUGO,
 			show_tolerance, store_tolerance, 1);
 static SENSOR_DEVICE_ATTR(tolerance2, S_IWUSR | S_IRUGO,
@@ -1177,30 +1186,38 @@ static SENSOR_DEVICE_ATTR(fan6_div, S_IWUSR | S_IRUGO,
 static SENSOR_DEVICE_ATTR(fan7_div, S_IWUSR | S_IRUGO,
 			show_fan_div, store_fan_div, 7);
 
-static struct attribute *w83792d_attributes_fan[4][5] = {
+static struct attribute *w83792d_attributes_fan[4][7] = {
 	{
 		&sensor_dev_attr_fan4_input.dev_attr.attr,
 		&sensor_dev_attr_fan4_min.dev_attr.attr,
 		&sensor_dev_attr_fan4_div.dev_attr.attr,
 		&sensor_dev_attr_fan4_alarm.dev_attr.attr,
+		&sensor_dev_attr_pwm4.dev_attr.attr,
+		&sensor_dev_attr_pwm4_mode.dev_attr.attr,
 		NULL
 	}, {
 		&sensor_dev_attr_fan5_input.dev_attr.attr,
 		&sensor_dev_attr_fan5_min.dev_attr.attr,
 		&sensor_dev_attr_fan5_div.dev_attr.attr,
 		&sensor_dev_attr_fan5_alarm.dev_attr.attr,
+		&sensor_dev_attr_pwm5.dev_attr.attr,
+		&sensor_dev_attr_pwm5_mode.dev_attr.attr,
 		NULL
 	}, {
 		&sensor_dev_attr_fan6_input.dev_attr.attr,
 		&sensor_dev_attr_fan6_min.dev_attr.attr,
 		&sensor_dev_attr_fan6_div.dev_attr.attr,
 		&sensor_dev_attr_fan6_alarm.dev_attr.attr,
+		&sensor_dev_attr_pwm6.dev_attr.attr,
+		&sensor_dev_attr_pwm6_mode.dev_attr.attr,
 		NULL
 	}, {
 		&sensor_dev_attr_fan7_input.dev_attr.attr,
 		&sensor_dev_attr_fan7_min.dev_attr.attr,
 		&sensor_dev_attr_fan7_div.dev_attr.attr,
 		&sensor_dev_attr_fan7_alarm.dev_attr.attr,
+		&sensor_dev_attr_pwm7.dev_attr.attr,
+		&sensor_dev_attr_pwm7_mode.dev_attr.attr,
 		NULL
 	}
 };
diff --git a/drivers/hwspinlock/Kconfig b/drivers/hwspinlock/Kconfig
index 3612cb5b30b2..73a401662853 100644
--- a/drivers/hwspinlock/Kconfig
+++ b/drivers/hwspinlock/Kconfig
@@ -18,6 +18,30 @@ config HWSPINLOCK_OMAP
 
 	  If unsure, say N.
 
+config HWSPINLOCK_QCOM
+	tristate "Qualcomm Hardware Spinlock device"
+	depends on ARCH_QCOM
+	select HWSPINLOCK
+	select MFD_SYSCON
+	help
+	  Say y here to support the Qualcomm Hardware Mutex functionality, which
+	  provides a synchronisation mechanism for the various processors on
+	  the SoC.
+
+	  If unsure, say N.
+
+config HWSPINLOCK_SIRF
+	tristate "SIRF Hardware Spinlock device"
+	depends on ARCH_SIRF
+	select HWSPINLOCK
+	help
+	  Say y here to support the SIRF Hardware Spinlock device, which
+	  provides a synchronisation mechanism for the various processors
+	  on the SoC.
+
+	  It's safe to say n here if you're not interested in SIRF hardware
+	  spinlock or just want a bare minimum kernel.
+
 config HSEM_U8500
 	tristate "STE Hardware Semaphore functionality"
 	depends on ARCH_U8500
diff --git a/drivers/hwspinlock/Makefile b/drivers/hwspinlock/Makefile
index 93eb64b66486..6b59cb5a4f3a 100644
--- a/drivers/hwspinlock/Makefile
+++ b/drivers/hwspinlock/Makefile
@@ -4,4 +4,6 @@
 
 obj-$(CONFIG_HWSPINLOCK)		+= hwspinlock_core.o
 obj-$(CONFIG_HWSPINLOCK_OMAP)		+= omap_hwspinlock.o
+obj-$(CONFIG_HWSPINLOCK_QCOM)		+= qcom_hwspinlock.o
+obj-$(CONFIG_HWSPINLOCK_SIRF)		+= sirf_hwspinlock.o
 obj-$(CONFIG_HSEM_U8500)		+= u8500_hsem.o
diff --git a/drivers/hwspinlock/hwspinlock_core.c b/drivers/hwspinlock/hwspinlock_core.c
index 461a0d739d75..52f708bcf77f 100644
--- a/drivers/hwspinlock/hwspinlock_core.c
+++ b/drivers/hwspinlock/hwspinlock_core.c
@@ -27,6 +27,7 @@
 #include <linux/hwspinlock.h>
 #include <linux/pm_runtime.h>
 #include <linux/mutex.h>
+#include <linux/of.h>
 
 #include "hwspinlock_internal.h"
 
@@ -257,6 +258,84 @@ void __hwspin_unlock(struct hwspinlock *hwlock, int mode, unsigned long *flags)
 }
 EXPORT_SYMBOL_GPL(__hwspin_unlock);
 
+/**
+ * of_hwspin_lock_simple_xlate - translate hwlock_spec to return a lock id
+ * @bank: the hwspinlock device bank
+ * @hwlock_spec: hwlock specifier as found in the device tree
+ *
+ * This is a simple translation function, suitable for hwspinlock platform
+ * drivers that only has a lock specifier length of 1.
+ *
+ * Returns a relative index of the lock within a specified bank on success,
+ * or -EINVAL on invalid specifier cell count.
+ */
+static inline int
+of_hwspin_lock_simple_xlate(const struct of_phandle_args *hwlock_spec)
+{
+	if (WARN_ON(hwlock_spec->args_count != 1))
+		return -EINVAL;
+
+	return hwlock_spec->args[0];
+}
+
+/**
+ * of_hwspin_lock_get_id() - get lock id for an OF phandle-based specific lock
+ * @np: device node from which to request the specific hwlock
+ * @index: index of the hwlock in the list of values
+ *
+ * This function provides a means for DT users of the hwspinlock module to
+ * get the global lock id of a specific hwspinlock using the phandle of the
+ * hwspinlock device, so that it can be requested using the normal
+ * hwspin_lock_request_specific() API.
+ *
+ * Returns the global lock id number on success, -EPROBE_DEFER if the hwspinlock
+ * device is not yet registered, -EINVAL on invalid args specifier value or an
+ * appropriate error as returned from the OF parsing of the DT client node.
+ */
+int of_hwspin_lock_get_id(struct device_node *np, int index)
+{
+	struct of_phandle_args args;
+	struct hwspinlock *hwlock;
+	struct radix_tree_iter iter;
+	void **slot;
+	int id;
+	int ret;
+
+	ret = of_parse_phandle_with_args(np, "hwlocks", "#hwlock-cells", index,
+					 &args);
+	if (ret)
+		return ret;
+
+	/* Find the hwspinlock device: we need its base_id */
+	ret = -EPROBE_DEFER;
+	rcu_read_lock();
+	radix_tree_for_each_slot(slot, &hwspinlock_tree, &iter, 0) {
+		hwlock = radix_tree_deref_slot(slot);
+		if (unlikely(!hwlock))
+			continue;
+
+		if (hwlock->bank->dev->of_node == args.np) {
+			ret = 0;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (ret < 0)
+		goto out;
+
+	id = of_hwspin_lock_simple_xlate(&args);
+	if (id < 0 || id >= hwlock->bank->num_locks) {
+		ret = -EINVAL;
+		goto out;
+	}
+	id += hwlock->bank->base_id;
+
+out:
+	of_node_put(args.np);
+	return ret ? ret : id;
+}
+EXPORT_SYMBOL_GPL(of_hwspin_lock_get_id);
+
 static int hwspin_lock_register_single(struct hwspinlock *hwlock, int id)
 {
 	struct hwspinlock *tmp;
diff --git a/drivers/hwspinlock/omap_hwspinlock.c b/drivers/hwspinlock/omap_hwspinlock.c
index 47a275c6ece1..ad2f8cac8487 100644
--- a/drivers/hwspinlock/omap_hwspinlock.c
+++ b/drivers/hwspinlock/omap_hwspinlock.c
@@ -1,7 +1,7 @@
 /*
  * OMAP hardware spinlock driver
  *
- * Copyright (C) 2010 Texas Instruments Incorporated - http://www.ti.com
+ * Copyright (C) 2010-2015 Texas Instruments Incorporated - http://www.ti.com
  *
  * Contact: Simon Que <sque@ti.com>
  *          Hari Kanigeri <h-kanigeri2@ti.com>
@@ -27,6 +27,7 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/hwspinlock.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 
 #include "hwspinlock_internal.h"
@@ -80,14 +81,16 @@ static const struct hwspinlock_ops omap_hwspinlock_ops = {
 
 static int omap_hwspinlock_probe(struct platform_device *pdev)
 {
-	struct hwspinlock_pdata *pdata = pdev->dev.platform_data;
+	struct device_node *node = pdev->dev.of_node;
 	struct hwspinlock_device *bank;
 	struct hwspinlock *hwlock;
 	struct resource *res;
 	void __iomem *io_base;
 	int num_locks, i, ret;
+	/* Only a single hwspinlock block device is supported */
+	int base_id = 0;
 
-	if (!pdata)
+	if (!node)
 		return -ENODEV;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -141,7 +144,7 @@ static int omap_hwspinlock_probe(struct platform_device *pdev)
 		hwlock->priv = io_base + LOCK_BASE_OFFSET + sizeof(u32) * i;
 
 	ret = hwspin_lock_register(bank, &pdev->dev, &omap_hwspinlock_ops,
-						pdata->base_id, num_locks);
+						base_id, num_locks);
 	if (ret)
 		goto reg_fail;
 
@@ -174,11 +177,18 @@ static int omap_hwspinlock_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id omap_hwspinlock_of_match[] = {
+	{ .compatible = "ti,omap4-hwspinlock", },
+	{ /* end */ },
+};
+MODULE_DEVICE_TABLE(of, omap_hwspinlock_of_match);
+
 static struct platform_driver omap_hwspinlock_driver = {
 	.probe		= omap_hwspinlock_probe,
 	.remove		= omap_hwspinlock_remove,
 	.driver		= {
 		.name	= "omap_hwspinlock",
+		.of_match_table = of_match_ptr(omap_hwspinlock_of_match),
 	},
 };
 
diff --git a/drivers/hwspinlock/qcom_hwspinlock.c b/drivers/hwspinlock/qcom_hwspinlock.c
new file mode 100644
index 000000000000..c752447fbac7
--- /dev/null
+++ b/drivers/hwspinlock/qcom_hwspinlock.c
@@ -0,0 +1,181 @@
+/*
+ * Copyright (c) 2013, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2015, Sony Mobile Communications AB
+ *
+ * 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/hwspinlock.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+
+#include "hwspinlock_internal.h"
+
+#define QCOM_MUTEX_APPS_PROC_ID	1
+#define QCOM_MUTEX_NUM_LOCKS	32
+
+static int qcom_hwspinlock_trylock(struct hwspinlock *lock)
+{
+	struct regmap_field *field = lock->priv;
+	u32 lock_owner;
+	int ret;
+
+	ret = regmap_field_write(field, QCOM_MUTEX_APPS_PROC_ID);
+	if (ret)
+		return ret;
+
+	ret = regmap_field_read(field, &lock_owner);
+	if (ret)
+		return ret;
+
+	return lock_owner == QCOM_MUTEX_APPS_PROC_ID;
+}
+
+static void qcom_hwspinlock_unlock(struct hwspinlock *lock)
+{
+	struct regmap_field *field = lock->priv;
+	u32 lock_owner;
+	int ret;
+
+	ret = regmap_field_read(field, &lock_owner);
+	if (ret) {
+		pr_err("%s: unable to query spinlock owner\n", __func__);
+		return;
+	}
+
+	if (lock_owner != QCOM_MUTEX_APPS_PROC_ID) {
+		pr_err("%s: spinlock not owned by us (actual owner is %d)\n",
+				__func__, lock_owner);
+	}
+
+	ret = regmap_field_write(field, 0);
+	if (ret)
+		pr_err("%s: failed to unlock spinlock\n", __func__);
+}
+
+static const struct hwspinlock_ops qcom_hwspinlock_ops = {
+	.trylock	= qcom_hwspinlock_trylock,
+	.unlock		= qcom_hwspinlock_unlock,
+};
+
+static const struct of_device_id qcom_hwspinlock_of_match[] = {
+	{ .compatible = "qcom,sfpb-mutex" },
+	{ .compatible = "qcom,tcsr-mutex" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, qcom_hwspinlock_of_match);
+
+static int qcom_hwspinlock_probe(struct platform_device *pdev)
+{
+	struct hwspinlock_device *bank;
+	struct device_node *syscon;
+	struct reg_field field;
+	struct regmap *regmap;
+	size_t array_size;
+	u32 stride;
+	u32 base;
+	int ret;
+	int i;
+
+	syscon = of_parse_phandle(pdev->dev.of_node, "syscon", 0);
+	if (!syscon) {
+		dev_err(&pdev->dev, "no syscon property\n");
+		return -ENODEV;
+	}
+
+	regmap = syscon_node_to_regmap(syscon);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	ret = of_property_read_u32_index(pdev->dev.of_node, "syscon", 1, &base);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "no offset in syscon\n");
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32_index(pdev->dev.of_node, "syscon", 2, &stride);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "no stride syscon\n");
+		return -EINVAL;
+	}
+
+	array_size = QCOM_MUTEX_NUM_LOCKS * sizeof(struct hwspinlock);
+	bank = devm_kzalloc(&pdev->dev, sizeof(*bank) + array_size, GFP_KERNEL);
+	if (!bank)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, bank);
+
+	for (i = 0; i < QCOM_MUTEX_NUM_LOCKS; i++) {
+		field.reg = base + i * stride;
+		field.lsb = 0;
+		field.msb = 31;
+
+		bank->lock[i].priv = devm_regmap_field_alloc(&pdev->dev,
+							     regmap, field);
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	ret = hwspin_lock_register(bank, &pdev->dev, &qcom_hwspinlock_ops,
+				   0, QCOM_MUTEX_NUM_LOCKS);
+	if (ret)
+		pm_runtime_disable(&pdev->dev);
+
+	return ret;
+}
+
+static int qcom_hwspinlock_remove(struct platform_device *pdev)
+{
+	struct hwspinlock_device *bank = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = hwspin_lock_unregister(bank);
+	if (ret) {
+		dev_err(&pdev->dev, "%s failed: %d\n", __func__, ret);
+		return ret;
+	}
+
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static struct platform_driver qcom_hwspinlock_driver = {
+	.probe		= qcom_hwspinlock_probe,
+	.remove		= qcom_hwspinlock_remove,
+	.driver		= {
+		.name	= "qcom_hwspinlock",
+		.of_match_table = qcom_hwspinlock_of_match,
+	},
+};
+
+static int __init qcom_hwspinlock_init(void)
+{
+	return platform_driver_register(&qcom_hwspinlock_driver);
+}
+/* board init code might need to reserve hwspinlocks for predefined purposes */
+postcore_initcall(qcom_hwspinlock_init);
+
+static void __exit qcom_hwspinlock_exit(void)
+{
+	platform_driver_unregister(&qcom_hwspinlock_driver);
+}
+module_exit(qcom_hwspinlock_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Hardware spinlock driver for Qualcomm SoCs");
diff --git a/drivers/hwspinlock/sirf_hwspinlock.c b/drivers/hwspinlock/sirf_hwspinlock.c
new file mode 100644
index 000000000000..16018544d431
--- /dev/null
+++ b/drivers/hwspinlock/sirf_hwspinlock.c
@@ -0,0 +1,136 @@
+/*
+ * SIRF hardware spinlock driver
+ *
+ * Copyright (c) 2015 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/hwspinlock.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "hwspinlock_internal.h"
+
+struct sirf_hwspinlock {
+	void __iomem *io_base;
+	struct hwspinlock_device bank;
+};
+
+/* Number of Hardware Spinlocks*/
+#define	HW_SPINLOCK_NUMBER	30
+
+/* Hardware spinlock register offsets */
+#define HW_SPINLOCK_BASE	0x404
+#define HW_SPINLOCK_OFFSET(x)	(HW_SPINLOCK_BASE + 0x4 * (x))
+
+static int sirf_hwspinlock_trylock(struct hwspinlock *lock)
+{
+	void __iomem *lock_addr = lock->priv;
+
+	/* attempt to acquire the lock by reading value == 1 from it */
+	return !!readl(lock_addr);
+}
+
+static void sirf_hwspinlock_unlock(struct hwspinlock *lock)
+{
+	void __iomem *lock_addr = lock->priv;
+
+	/* release the lock by writing 0 to it */
+	writel(0, lock_addr);
+}
+
+static const struct hwspinlock_ops sirf_hwspinlock_ops = {
+	.trylock = sirf_hwspinlock_trylock,
+	.unlock = sirf_hwspinlock_unlock,
+};
+
+static int sirf_hwspinlock_probe(struct platform_device *pdev)
+{
+	struct sirf_hwspinlock *hwspin;
+	struct hwspinlock *hwlock;
+	int idx, ret;
+
+	if (!pdev->dev.of_node)
+		return -ENODEV;
+
+	hwspin = devm_kzalloc(&pdev->dev, sizeof(*hwspin) +
+			sizeof(*hwlock) * HW_SPINLOCK_NUMBER, GFP_KERNEL);
+	if (!hwspin)
+		return -ENOMEM;
+
+	/* retrieve io base */
+	hwspin->io_base = of_iomap(pdev->dev.of_node, 0);
+	if (!hwspin->io_base)
+		return -ENOMEM;
+
+	for (idx = 0; idx < HW_SPINLOCK_NUMBER; idx++) {
+		hwlock = &hwspin->bank.lock[idx];
+		hwlock->priv = hwspin->io_base + HW_SPINLOCK_OFFSET(idx);
+	}
+
+	platform_set_drvdata(pdev, hwspin);
+
+	pm_runtime_enable(&pdev->dev);
+
+	ret = hwspin_lock_register(&hwspin->bank, &pdev->dev,
+				   &sirf_hwspinlock_ops, 0,
+				   HW_SPINLOCK_NUMBER);
+	if (ret)
+		goto reg_failed;
+
+	return 0;
+
+reg_failed:
+	pm_runtime_disable(&pdev->dev);
+	iounmap(hwspin->io_base);
+
+	return ret;
+}
+
+static int sirf_hwspinlock_remove(struct platform_device *pdev)
+{
+	struct sirf_hwspinlock *hwspin = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = hwspin_lock_unregister(&hwspin->bank);
+	if (ret) {
+		dev_err(&pdev->dev, "%s failed: %d\n", __func__, ret);
+		return ret;
+	}
+
+	pm_runtime_disable(&pdev->dev);
+
+	iounmap(hwspin->io_base);
+
+	return 0;
+}
+
+static const struct of_device_id sirf_hwpinlock_ids[] = {
+	{ .compatible = "sirf,hwspinlock", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, sirf_hwpinlock_ids);
+
+static struct platform_driver sirf_hwspinlock_driver = {
+	.probe = sirf_hwspinlock_probe,
+	.remove = sirf_hwspinlock_remove,
+	.driver = {
+		.name = "atlas7_hwspinlock",
+		.of_match_table = of_match_ptr(sirf_hwpinlock_ids),
+	},
+};
+
+module_platform_driver(sirf_hwspinlock_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("SIRF Hardware spinlock driver");
+MODULE_AUTHOR("Wei Chen <wei.chen@csr.com>");
diff --git a/drivers/hwtracing/coresight/Kconfig b/drivers/hwtracing/coresight/Kconfig
index fc1f1ae7a49d..6c8921140f02 100644
--- a/drivers/hwtracing/coresight/Kconfig
+++ b/drivers/hwtracing/coresight/Kconfig
@@ -58,4 +58,23 @@ config CORESIGHT_SOURCE_ETM3X
 	  which allows tracing the instructions that a processor is executing
 	  This is primarily useful for instruction level tracing.  Depending
 	  the ETM version data tracing may also be available.
+
+config CORESIGHT_SOURCE_ETM4X
+	bool "CoreSight Embedded Trace Macrocell 4.x driver"
+	depends on ARM64
+	select CORESIGHT_LINKS_AND_SINKS
+	help
+	  This driver provides support for the ETM4.x tracer module, tracing the
+	  instructions that a processor is executing. This is primarily useful
+	  for instruction level tracing. Depending on the implemented version
+	  data tracing may also be available.
+
+config CORESIGHT_QCOM_REPLICATOR
+	bool "Qualcomm CoreSight Replicator driver"
+	depends on CORESIGHT_LINKS_AND_SINKS
+	help
+	  This enables support for Qualcomm CoreSight link driver. The
+	  programmable ATB replicator sends the ATB trace stream from the
+	  ETB/ETF to the TPIUi and ETR.
+
 endif
diff --git a/drivers/hwtracing/coresight/Makefile b/drivers/hwtracing/coresight/Makefile
index 4b4bec890ef5..99f8e5f6256e 100644
--- a/drivers/hwtracing/coresight/Makefile
+++ b/drivers/hwtracing/coresight/Makefile
@@ -9,3 +9,5 @@ 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
+obj-$(CONFIG_CORESIGHT_SOURCE_ETM4X) += coresight-etm4x.o
+obj-$(CONFIG_CORESIGHT_QCOM_REPLICATOR) += coresight-replicator-qcom.o
diff --git a/drivers/hwtracing/coresight/coresight-etb10.c b/drivers/hwtracing/coresight/coresight-etb10.c
index 40049869aecd..77d0f9c1118d 100644
--- a/drivers/hwtracing/coresight/coresight-etb10.c
+++ b/drivers/hwtracing/coresight/coresight-etb10.c
@@ -22,10 +22,11 @@
 #include <linux/uaccess.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
-#include <linux/clk.h>
+#include <linux/pm_runtime.h>
 #include <linux/seq_file.h>
 #include <linux/coresight.h>
 #include <linux/amba/bus.h>
+#include <linux/clk.h>
 
 #include "coresight-priv.h"
 
@@ -66,9 +67,9 @@
  * 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.
+ * @atclk:	optional clock for the core parts of the ETB.
  * @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.
@@ -79,9 +80,9 @@
 struct etb_drvdata {
 	void __iomem		*base;
 	struct device		*dev;
+	struct clk		*atclk;
 	struct coresight_device	*csdev;
 	struct miscdevice	miscdev;
-	struct clk		*clk;
 	spinlock_t		spinlock;
 	atomic_t		in_use;
 	u8			*buf;
@@ -92,17 +93,14 @@ struct etb_drvdata {
 
 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;
+	pm_runtime_get_sync(drvdata->dev);
 
 	/* RO registers don't need locking */
 	depth = readl_relaxed(drvdata->base + ETB_RAM_DEPTH_REG);
 
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 	return depth;
 }
 
@@ -137,12 +135,9 @@ static void etb_enable_hw(struct etb_drvdata *drvdata)
 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;
+	pm_runtime_get_sync(drvdata->dev);
 
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 	etb_enable_hw(drvdata);
@@ -252,7 +247,7 @@ static void etb_disable(struct coresight_device *csdev)
 	drvdata->enable = false;
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
 
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 
 	dev_info(drvdata->dev, "ETB disabled\n");
 }
@@ -339,16 +334,12 @@ static const struct file_operations etb_fops = {
 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;
-
+	pm_runtime_get_sync(drvdata->dev);
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 	CS_UNLOCK(drvdata->base);
 
@@ -364,7 +355,7 @@ static ssize_t status_show(struct device *dev,
 	CS_LOCK(drvdata->base);
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
 
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 
 	return sprintf(buf,
 		       "Depth:\t\t0x%x\n"
@@ -377,7 +368,7 @@ static ssize_t status_show(struct device *dev,
 		       "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);
@@ -438,6 +429,12 @@ static int etb_probe(struct amba_device *adev, const struct amba_id *id)
 		return -ENOMEM;
 
 	drvdata->dev = &adev->dev;
+	drvdata->atclk = devm_clk_get(&adev->dev, "atclk"); /* optional */
+	if (!IS_ERR(drvdata->atclk)) {
+		ret = clk_prepare_enable(drvdata->atclk);
+		if (ret)
+			return ret;
+	}
 	dev_set_drvdata(dev, drvdata);
 
 	/* validity for the resource is already checked by the AMBA core */
@@ -449,21 +446,19 @@ static int etb_probe(struct amba_device *adev, const struct amba_id *id)
 
 	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);
+	pm_runtime_put(&adev->dev);
 
-	if (drvdata->buffer_depth < 0)
+	if (drvdata->buffer_depth & 0x80000000)
 		return -EINVAL;
 
 	drvdata->buf = devm_kzalloc(dev,
 				    drvdata->buffer_depth * 4, GFP_KERNEL);
-	if (!drvdata->buf)
+	if (!drvdata->buf) {
+		dev_err(dev, "Failed to allocate %u bytes for buffer data\n",
+			drvdata->buffer_depth * 4);
 		return -ENOMEM;
+	}
 
 	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
 	if (!desc)
@@ -503,6 +498,32 @@ static int etb_remove(struct amba_device *adev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int etb_runtime_suspend(struct device *dev)
+{
+	struct etb_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_disable_unprepare(drvdata->atclk);
+
+	return 0;
+}
+
+static int etb_runtime_resume(struct device *dev)
+{
+	struct etb_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_prepare_enable(drvdata->atclk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops etb_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(etb_runtime_suspend, etb_runtime_resume, NULL)
+};
+
 static struct amba_id etb_ids[] = {
 	{
 		.id	= 0x0003b907,
@@ -515,6 +536,8 @@ static struct amba_driver etb_driver = {
 	.drv = {
 		.name	= "coresight-etb10",
 		.owner	= THIS_MODULE,
+		.pm	= &etb_dev_pm_ops,
+
 	},
 	.probe		= etb_probe,
 	.remove		= etb_remove,
diff --git a/drivers/hwtracing/coresight/coresight-etm.h b/drivers/hwtracing/coresight/coresight-etm.h
index 501c5fac8a45..098ffbec0a44 100644
--- a/drivers/hwtracing/coresight/coresight-etm.h
+++ b/drivers/hwtracing/coresight/coresight-etm.h
@@ -140,8 +140,8 @@
  * 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.
+ * @atclk:	optional clock for the core parts of the ETM.
  * @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.
@@ -192,8 +192,8 @@
 struct etm_drvdata {
 	void __iomem			*base;
 	struct device			*dev;
+	struct clk			*atclk;
 	struct coresight_device		*csdev;
-	struct clk			*clk;
 	spinlock_t			spinlock;
 	int				cpu;
 	int				port_size;
diff --git a/drivers/hwtracing/coresight/coresight-etm3x.c b/drivers/hwtracing/coresight/coresight-etm3x.c
index c965f5724abd..018a00fda611 100644
--- a/drivers/hwtracing/coresight/coresight-etm3x.c
+++ b/drivers/hwtracing/coresight/coresight-etm3x.c
@@ -23,13 +23,14 @@
 #include <linux/smp.h>
 #include <linux/sysfs.h>
 #include <linux/stat.h>
-#include <linux/clk.h>
+#include <linux/pm_runtime.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 <linux/clk.h>
 #include <asm/sections.h>
 
 #include "coresight-etm.h"
@@ -325,9 +326,7 @@ static int etm_trace_id(struct coresight_device *csdev)
 
 	if (!drvdata->enable)
 		return drvdata->traceid;
-
-	if (clk_prepare_enable(drvdata->clk))
-		goto out;
+	pm_runtime_get_sync(csdev->dev.parent);
 
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 
@@ -336,8 +335,8 @@ static int etm_trace_id(struct coresight_device *csdev)
 	CS_LOCK(drvdata->base);
 
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
-	clk_disable_unprepare(drvdata->clk);
-out:
+	pm_runtime_put(csdev->dev.parent);
+
 	return trace_id;
 }
 
@@ -346,10 +345,7 @@ 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;
-
+	pm_runtime_get_sync(csdev->dev.parent);
 	spin_lock(&drvdata->spinlock);
 
 	/*
@@ -373,8 +369,7 @@ static int etm_enable(struct coresight_device *csdev)
 	return 0;
 err:
 	spin_unlock(&drvdata->spinlock);
-	clk_disable_unprepare(drvdata->clk);
-err_clk:
+	pm_runtime_put(csdev->dev.parent);
 	return ret;
 }
 
@@ -423,8 +418,7 @@ static void etm_disable(struct coresight_device *csdev)
 
 	spin_unlock(&drvdata->spinlock);
 	put_online_cpus();
-
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(csdev->dev.parent);
 
 	dev_info(drvdata->dev, "ETM tracing disabled\n");
 }
@@ -474,14 +468,10 @@ 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;
-
+	pm_runtime_get_sync(drvdata->dev);
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 	CS_UNLOCK(drvdata->base);
 
@@ -489,7 +479,7 @@ static ssize_t etmsr_show(struct device *dev,
 
 	CS_LOCK(drvdata->base);
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 
 	return sprintf(buf, "%#lx\n", val);
 }
@@ -1317,7 +1307,6 @@ 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);
 
@@ -1326,10 +1315,7 @@ static ssize_t seq_curr_state_show(struct device *dev,
 		goto out;
 	}
 
-	ret = clk_prepare_enable(drvdata->clk);
-	if (ret)
-		return ret;
-
+	pm_runtime_get_sync(drvdata->dev);
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 
 	CS_UNLOCK(drvdata->base);
@@ -1337,7 +1323,7 @@ static ssize_t seq_curr_state_show(struct device *dev,
 	CS_LOCK(drvdata->base);
 
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 out:
 	return sprintf(buf, "%#lx\n", val);
 }
@@ -1521,10 +1507,7 @@ static ssize_t status_show(struct device *dev,
 	unsigned long flags;
 	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
 
-	ret = clk_prepare_enable(drvdata->clk);
-	if (ret)
-		return ret;
-
+	pm_runtime_get_sync(drvdata->dev);
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 
 	CS_UNLOCK(drvdata->base);
@@ -1550,7 +1533,7 @@ static ssize_t status_show(struct device *dev,
 	CS_LOCK(drvdata->base);
 
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 
 	return ret;
 }
@@ -1559,7 +1542,6 @@ 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);
 
@@ -1568,10 +1550,7 @@ static ssize_t traceid_show(struct device *dev,
 		goto out;
 	}
 
-	ret = clk_prepare_enable(drvdata->clk);
-	if (ret)
-		return ret;
-
+	pm_runtime_get_sync(drvdata->dev);
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 	CS_UNLOCK(drvdata->base);
 
@@ -1579,7 +1558,7 @@ static ssize_t traceid_show(struct device *dev,
 
 	CS_LOCK(drvdata->base);
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 out:
 	return sprintf(buf, "%#lx\n", val);
 }
@@ -1817,10 +1796,12 @@ static int etm_probe(struct amba_device *adev, const struct amba_id *id)
 
 	spin_lock_init(&drvdata->spinlock);
 
-	drvdata->clk = adev->pclk;
-	ret = clk_prepare_enable(drvdata->clk);
-	if (ret)
-		return ret;
+	drvdata->atclk = devm_clk_get(&adev->dev, "atclk"); /* optional */
+	if (!IS_ERR(drvdata->atclk)) {
+		ret = clk_prepare_enable(drvdata->atclk);
+		if (ret)
+			return ret;
+	}
 
 	drvdata->cpu = pdata ? pdata->cpu : 0;
 
@@ -1845,8 +1826,6 @@ static int etm_probe(struct amba_device *adev, const struct amba_id *id)
 	}
 	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;
@@ -1859,7 +1838,8 @@ static int etm_probe(struct amba_device *adev, const struct amba_id *id)
 		goto err_arch_supported;
 	}
 
-	dev_info(dev, "ETM initialized\n");
+	pm_runtime_put(&adev->dev);
+	dev_info(dev, "%s initialized\n", (char *)id->data);
 
 	if (boot_enable) {
 		coresight_enable(drvdata->csdev);
@@ -1869,7 +1849,6 @@ static int etm_probe(struct amba_device *adev, const struct amba_id *id)
 	return 0;
 
 err_arch_supported:
-	clk_disable_unprepare(drvdata->clk);
 	if (--etm_count == 0)
 		unregister_hotcpu_notifier(&etm_cpu_notifier);
 	return ret;
@@ -1886,22 +1865,52 @@ static int etm_remove(struct amba_device *adev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int etm_runtime_suspend(struct device *dev)
+{
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_disable_unprepare(drvdata->atclk);
+
+	return 0;
+}
+
+static int etm_runtime_resume(struct device *dev)
+{
+	struct etm_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_prepare_enable(drvdata->atclk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops etm_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(etm_runtime_suspend, etm_runtime_resume, NULL)
+};
+
 static struct amba_id etm_ids[] = {
 	{	/* ETM 3.3 */
 		.id	= 0x0003b921,
 		.mask	= 0x0003ffff,
+		.data	= "ETM 3.3",
 	},
 	{	/* ETM 3.5 */
 		.id	= 0x0003b956,
 		.mask	= 0x0003ffff,
+		.data	= "ETM 3.5",
 	},
 	{	/* PTM 1.0 */
 		.id	= 0x0003b950,
 		.mask	= 0x0003ffff,
+		.data	= "PTM 1.0",
 	},
 	{	/* PTM 1.1 */
 		.id	= 0x0003b95f,
 		.mask	= 0x0003ffff,
+		.data	= "PTM 1.1",
 	},
 	{ 0, 0},
 };
@@ -1910,23 +1919,14 @@ static struct amba_driver etm_driver = {
 	.drv = {
 		.name	= "coresight-etm3x",
 		.owner	= THIS_MODULE,
+		.pm	= &etm_dev_pm_ops,
 	},
 	.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_amba_driver(etm_driver);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("CoreSight Program Flow Trace driver");
diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
new file mode 100644
index 000000000000..1312e993c501
--- /dev/null
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c
@@ -0,0 +1,2702 @@
+/* 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 <linux/kernel.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/module.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/coresight.h>
+#include <linux/pm_wakeup.h>
+#include <linux/amba/bus.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/pm_runtime.h>
+#include <asm/sections.h>
+
+#include "coresight-etm4x.h"
+
+static int boot_enable;
+module_param_named(boot_enable, boot_enable, int, S_IRUGO);
+
+/* The number of ETMv4 currently registered */
+static int etm4_count;
+static struct etmv4_drvdata *etmdrvdata[NR_CPUS];
+
+static void etm4_os_unlock(void *info)
+{
+	struct etmv4_drvdata *drvdata = (struct etmv4_drvdata *)info;
+
+	/* Writing any value to ETMOSLAR unlocks the trace registers */
+	writel_relaxed(0x0, drvdata->base + TRCOSLAR);
+	isb();
+}
+
+static bool etm4_arch_supported(u8 arch)
+{
+	switch (arch) {
+	case ETM_ARCH_V4:
+		break;
+	default:
+		return false;
+	}
+	return true;
+}
+
+static int etm4_trace_id(struct coresight_device *csdev)
+{
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	unsigned long flags;
+	int trace_id = -1;
+
+	if (!drvdata->enable)
+		return drvdata->trcid;
+
+	pm_runtime_get_sync(drvdata->dev);
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+
+	CS_UNLOCK(drvdata->base);
+	trace_id = readl_relaxed(drvdata->base + TRCTRACEIDR);
+	trace_id &= ETM_TRACEID_MASK;
+	CS_LOCK(drvdata->base);
+
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+	pm_runtime_put(drvdata->dev);
+
+	return trace_id;
+}
+
+static void etm4_enable_hw(void *info)
+{
+	int i;
+	struct etmv4_drvdata *drvdata = info;
+
+	CS_UNLOCK(drvdata->base);
+
+	etm4_os_unlock(drvdata);
+
+	/* Disable the trace unit before programming trace registers */
+	writel_relaxed(0, drvdata->base + TRCPRGCTLR);
+
+	/* wait for TRCSTATR.IDLE to go up */
+	if (coresight_timeout(drvdata->base, TRCSTATR, TRCSTATR_IDLE_BIT, 1))
+		dev_err(drvdata->dev,
+			"timeout observed when probing at offset %#x\n",
+			TRCSTATR);
+
+	writel_relaxed(drvdata->pe_sel, drvdata->base + TRCPROCSELR);
+	writel_relaxed(drvdata->cfg, drvdata->base + TRCCONFIGR);
+	/* nothing specific implemented */
+	writel_relaxed(0x0, drvdata->base + TRCAUXCTLR);
+	writel_relaxed(drvdata->eventctrl0, drvdata->base + TRCEVENTCTL0R);
+	writel_relaxed(drvdata->eventctrl1, drvdata->base + TRCEVENTCTL1R);
+	writel_relaxed(drvdata->stall_ctrl, drvdata->base + TRCSTALLCTLR);
+	writel_relaxed(drvdata->ts_ctrl, drvdata->base + TRCTSCTLR);
+	writel_relaxed(drvdata->syncfreq, drvdata->base + TRCSYNCPR);
+	writel_relaxed(drvdata->ccctlr, drvdata->base + TRCCCCTLR);
+	writel_relaxed(drvdata->bb_ctrl, drvdata->base + TRCBBCTLR);
+	writel_relaxed(drvdata->trcid, drvdata->base + TRCTRACEIDR);
+	writel_relaxed(drvdata->vinst_ctrl, drvdata->base + TRCVICTLR);
+	writel_relaxed(drvdata->viiectlr, drvdata->base + TRCVIIECTLR);
+	writel_relaxed(drvdata->vissctlr,
+		       drvdata->base + TRCVISSCTLR);
+	writel_relaxed(drvdata->vipcssctlr,
+		       drvdata->base + TRCVIPCSSCTLR);
+	for (i = 0; i < drvdata->nrseqstate - 1; i++)
+		writel_relaxed(drvdata->seq_ctrl[i],
+			       drvdata->base + TRCSEQEVRn(i));
+	writel_relaxed(drvdata->seq_rst, drvdata->base + TRCSEQRSTEVR);
+	writel_relaxed(drvdata->seq_state, drvdata->base + TRCSEQSTR);
+	writel_relaxed(drvdata->ext_inp, drvdata->base + TRCEXTINSELR);
+	for (i = 0; i < drvdata->nr_cntr; i++) {
+		writel_relaxed(drvdata->cntrldvr[i],
+			       drvdata->base + TRCCNTRLDVRn(i));
+		writel_relaxed(drvdata->cntr_ctrl[i],
+			       drvdata->base + TRCCNTCTLRn(i));
+		writel_relaxed(drvdata->cntr_val[i],
+			       drvdata->base + TRCCNTVRn(i));
+	}
+	for (i = 0; i < drvdata->nr_resource; i++)
+		writel_relaxed(drvdata->res_ctrl[i],
+			       drvdata->base + TRCRSCTLRn(i));
+
+	for (i = 0; i < drvdata->nr_ss_cmp; i++) {
+		writel_relaxed(drvdata->ss_ctrl[i],
+			       drvdata->base + TRCSSCCRn(i));
+		writel_relaxed(drvdata->ss_status[i],
+			       drvdata->base + TRCSSCSRn(i));
+		writel_relaxed(drvdata->ss_pe_cmp[i],
+			       drvdata->base + TRCSSPCICRn(i));
+	}
+	for (i = 0; i < drvdata->nr_addr_cmp; i++) {
+		writeq_relaxed(drvdata->addr_val[i],
+			       drvdata->base + TRCACVRn(i));
+		writeq_relaxed(drvdata->addr_acc[i],
+			       drvdata->base + TRCACATRn(i));
+	}
+	for (i = 0; i < drvdata->numcidc; i++)
+		writeq_relaxed(drvdata->ctxid_val[i],
+			       drvdata->base + TRCCIDCVRn(i));
+	writel_relaxed(drvdata->ctxid_mask0, drvdata->base + TRCCIDCCTLR0);
+	writel_relaxed(drvdata->ctxid_mask1, drvdata->base + TRCCIDCCTLR1);
+
+	for (i = 0; i < drvdata->numvmidc; i++)
+		writeq_relaxed(drvdata->vmid_val[i],
+			       drvdata->base + TRCVMIDCVRn(i));
+	writel_relaxed(drvdata->vmid_mask0, drvdata->base + TRCVMIDCCTLR0);
+	writel_relaxed(drvdata->vmid_mask1, drvdata->base + TRCVMIDCCTLR1);
+
+	/* Enable the trace unit */
+	writel_relaxed(1, drvdata->base + TRCPRGCTLR);
+
+	/* wait for TRCSTATR.IDLE to go back down to '0' */
+	if (coresight_timeout(drvdata->base, TRCSTATR, TRCSTATR_IDLE_BIT, 0))
+		dev_err(drvdata->dev,
+			"timeout observed when probing at offset %#x\n",
+			TRCSTATR);
+
+	CS_LOCK(drvdata->base);
+
+	dev_dbg(drvdata->dev, "cpu: %d enable smp call done\n", drvdata->cpu);
+}
+
+static int etm4_enable(struct coresight_device *csdev)
+{
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	int ret;
+
+	pm_runtime_get_sync(drvdata->dev);
+	spin_lock(&drvdata->spinlock);
+
+	/*
+	 * Executing etm4_enable_hw on the cpu whose ETM is being enabled
+	 * ensures that register writes occur when cpu is powered.
+	 */
+	ret = smp_call_function_single(drvdata->cpu,
+				       etm4_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);
+	pm_runtime_put(drvdata->dev);
+	return ret;
+}
+
+static void etm4_disable_hw(void *info)
+{
+	u32 control;
+	struct etmv4_drvdata *drvdata = info;
+
+	CS_UNLOCK(drvdata->base);
+
+	control = readl_relaxed(drvdata->base + TRCPRGCTLR);
+
+	/* EN, bit[0] Trace unit enable bit */
+	control &= ~0x1;
+
+	/* make sure everything completes before disabling */
+	mb();
+	isb();
+	writel_relaxed(control, drvdata->base + TRCPRGCTLR);
+
+	CS_LOCK(drvdata->base);
+
+	dev_dbg(drvdata->dev, "cpu: %d disable smp call done\n", drvdata->cpu);
+}
+
+static void etm4_disable(struct coresight_device *csdev)
+{
+	struct etmv4_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 etm4_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, etm4_disable_hw, drvdata, 1);
+	drvdata->enable = false;
+
+	spin_unlock(&drvdata->spinlock);
+	put_online_cpus();
+
+	pm_runtime_put(drvdata->dev);
+
+	dev_info(drvdata->dev, "ETM tracing disabled\n");
+}
+
+static const struct coresight_ops_source etm4_source_ops = {
+	.trace_id	= etm4_trace_id,
+	.enable		= etm4_enable,
+	.disable	= etm4_disable,
+};
+
+static const struct coresight_ops etm4_cs_ops = {
+	.source_ops	= &etm4_source_ops,
+};
+
+static int etm4_set_mode_exclude(struct etmv4_drvdata *drvdata, bool exclude)
+{
+	u8 idx = drvdata->addr_idx;
+
+	/*
+	 * TRCACATRn.TYPE bit[1:0]: type of comparison
+	 * the trace unit performs
+	 */
+	if (BMVAL(drvdata->addr_acc[idx], 0, 1) == ETM_INSTR_ADDR) {
+		if (idx % 2 != 0)
+			return -EINVAL;
+
+		/*
+		 * We are performing instruction address comparison. Set the
+		 * relevant bit of ViewInst Include/Exclude Control register
+		 * for corresponding address comparator pair.
+		 */
+		if (drvdata->addr_type[idx] != ETM_ADDR_TYPE_RANGE ||
+		    drvdata->addr_type[idx + 1] != ETM_ADDR_TYPE_RANGE)
+			return -EINVAL;
+
+		if (exclude == true) {
+			/*
+			 * Set exclude bit and unset the include bit
+			 * corresponding to comparator pair
+			 */
+			drvdata->viiectlr |= BIT(idx / 2 + 16);
+			drvdata->viiectlr &= ~BIT(idx / 2);
+		} else {
+			/*
+			 * Set include bit and unset exclude bit
+			 * corresponding to comparator pair
+			 */
+			drvdata->viiectlr |= BIT(idx / 2);
+			drvdata->viiectlr &= ~BIT(idx / 2 + 16);
+		}
+	}
+	return 0;
+}
+
+static ssize_t nr_pe_cmp_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_pe_cmp;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nr_pe_cmp);
+
+static ssize_t nr_addr_cmp_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_addr_cmp;
+	return scnprintf(buf, PAGE_SIZE, "%#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 etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_cntr;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nr_cntr);
+
+static ssize_t nr_ext_inp_show(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_ext_inp;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nr_ext_inp);
+
+static ssize_t numcidc_show(struct device *dev,
+			    struct device_attribute *attr,
+			    char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->numcidc;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(numcidc);
+
+static ssize_t numvmidc_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->numvmidc;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(numvmidc);
+
+static ssize_t nrseqstate_show(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nrseqstate;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nrseqstate);
+
+static ssize_t nr_resource_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_resource;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nr_resource);
+
+static ssize_t nr_ss_cmp_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->nr_ss_cmp;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+static DEVICE_ATTR_RO(nr_ss_cmp);
+
+static ssize_t reset_store(struct device *dev,
+			   struct device_attribute *attr,
+			   const char *buf, size_t size)
+{
+	int i;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	if (val)
+		drvdata->mode = 0x0;
+
+	/* Disable data tracing: do not trace load and store data transfers */
+	drvdata->mode &= ~(ETM_MODE_LOAD | ETM_MODE_STORE);
+	drvdata->cfg &= ~(BIT(1) | BIT(2));
+
+	/* Disable data value and data address tracing */
+	drvdata->mode &= ~(ETM_MODE_DATA_TRACE_ADDR |
+			   ETM_MODE_DATA_TRACE_VAL);
+	drvdata->cfg &= ~(BIT(16) | BIT(17));
+
+	/* Disable all events tracing */
+	drvdata->eventctrl0 = 0x0;
+	drvdata->eventctrl1 = 0x0;
+
+	/* Disable timestamp event */
+	drvdata->ts_ctrl = 0x0;
+
+	/* Disable stalling */
+	drvdata->stall_ctrl = 0x0;
+
+	/* Reset trace synchronization period  to 2^8 = 256 bytes*/
+	if (drvdata->syncpr == false)
+		drvdata->syncfreq = 0x8;
+
+	/*
+	 * Enable ViewInst to trace everything with start-stop logic in
+	 * started state. ARM recommends start-stop logic is set before
+	 * each trace run.
+	 */
+	drvdata->vinst_ctrl |= BIT(0);
+	if (drvdata->nr_addr_cmp == true) {
+		drvdata->mode |= ETM_MODE_VIEWINST_STARTSTOP;
+		/* SSSTATUS, bit[9] */
+		drvdata->vinst_ctrl |= BIT(9);
+	}
+
+	/* No address range filtering for ViewInst */
+	drvdata->viiectlr = 0x0;
+
+	/* No start-stop filtering for ViewInst */
+	drvdata->vissctlr = 0x0;
+
+	/* Disable seq events */
+	for (i = 0; i < drvdata->nrseqstate-1; i++)
+		drvdata->seq_ctrl[i] = 0x0;
+	drvdata->seq_rst = 0x0;
+	drvdata->seq_state = 0x0;
+
+	/* Disable external input events */
+	drvdata->ext_inp = 0x0;
+
+	drvdata->cntr_idx = 0x0;
+	for (i = 0; i < drvdata->nr_cntr; i++) {
+		drvdata->cntrldvr[i] = 0x0;
+		drvdata->cntr_ctrl[i] = 0x0;
+		drvdata->cntr_val[i] = 0x0;
+	}
+
+	drvdata->res_idx = 0x0;
+	for (i = 0; i < drvdata->nr_resource; i++)
+		drvdata->res_ctrl[i] = 0x0;
+
+	for (i = 0; i < drvdata->nr_ss_cmp; i++) {
+		drvdata->ss_ctrl[i] = 0x0;
+		drvdata->ss_pe_cmp[i] = 0x0;
+	}
+
+	drvdata->addr_idx = 0x0;
+	for (i = 0; i < drvdata->nr_addr_cmp * 2; i++) {
+		drvdata->addr_val[i] = 0x0;
+		drvdata->addr_acc[i] = 0x0;
+		drvdata->addr_type[i] = ETM_ADDR_TYPE_NONE;
+	}
+
+	drvdata->ctxid_idx = 0x0;
+	for (i = 0; i < drvdata->numcidc; i++)
+		drvdata->ctxid_val[i] = 0x0;
+	drvdata->ctxid_mask0 = 0x0;
+	drvdata->ctxid_mask1 = 0x0;
+
+	drvdata->vmid_idx = 0x0;
+	for (i = 0; i < drvdata->numvmidc; i++)
+		drvdata->vmid_val[i] = 0x0;
+	drvdata->vmid_mask0 = 0x0;
+	drvdata->vmid_mask1 = 0x0;
+
+	drvdata->trcid = drvdata->cpu + 1;
+	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 etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->mode;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t mode_store(struct device *dev,
+			  struct device_attribute *attr,
+			  const char *buf, size_t size)
+{
+	unsigned long val, mode;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->mode = val & ETMv4_MODE_ALL;
+
+	if (drvdata->mode & ETM_MODE_EXCLUDE)
+		etm4_set_mode_exclude(drvdata, true);
+	else
+		etm4_set_mode_exclude(drvdata, false);
+
+	if (drvdata->instrp0 == true) {
+		/* start by clearing instruction P0 field */
+		drvdata->cfg  &= ~(BIT(1) | BIT(2));
+		if (drvdata->mode & ETM_MODE_LOAD)
+			/* 0b01 Trace load instructions as P0 instructions */
+			drvdata->cfg  |= BIT(1);
+		if (drvdata->mode & ETM_MODE_STORE)
+			/* 0b10 Trace store instructions as P0 instructions */
+			drvdata->cfg  |= BIT(2);
+		if (drvdata->mode & ETM_MODE_LOAD_STORE)
+			/*
+			 * 0b11 Trace load and store instructions
+			 * as P0 instructions
+			 */
+			drvdata->cfg  |= BIT(1) | BIT(2);
+	}
+
+	/* bit[3], Branch broadcast mode */
+	if ((drvdata->mode & ETM_MODE_BB) && (drvdata->trcbb == true))
+		drvdata->cfg |= BIT(3);
+	else
+		drvdata->cfg &= ~BIT(3);
+
+	/* bit[4], Cycle counting instruction trace bit */
+	if ((drvdata->mode & ETMv4_MODE_CYCACC) &&
+		(drvdata->trccci == true))
+		drvdata->cfg |= BIT(4);
+	else
+		drvdata->cfg &= ~BIT(4);
+
+	/* bit[6], Context ID tracing bit */
+	if ((drvdata->mode & ETMv4_MODE_CTXID) && (drvdata->ctxid_size))
+		drvdata->cfg |= BIT(6);
+	else
+		drvdata->cfg &= ~BIT(6);
+
+	if ((drvdata->mode & ETM_MODE_VMID) && (drvdata->vmid_size))
+		drvdata->cfg |= BIT(7);
+	else
+		drvdata->cfg &= ~BIT(7);
+
+	/* bits[10:8], Conditional instruction tracing bit */
+	mode = ETM_MODE_COND(drvdata->mode);
+	if (drvdata->trccond == true) {
+		drvdata->cfg &= ~(BIT(8) | BIT(9) | BIT(10));
+		drvdata->cfg |= mode << 8;
+	}
+
+	/* bit[11], Global timestamp tracing bit */
+	if ((drvdata->mode & ETMv4_MODE_TIMESTAMP) && (drvdata->ts_size))
+		drvdata->cfg |= BIT(11);
+	else
+		drvdata->cfg &= ~BIT(11);
+
+	/* bit[12], Return stack enable bit */
+	if ((drvdata->mode & ETM_MODE_RETURNSTACK) &&
+		(drvdata->retstack == true))
+		drvdata->cfg |= BIT(12);
+	else
+		drvdata->cfg &= ~BIT(12);
+
+	/* bits[14:13], Q element enable field */
+	mode = ETM_MODE_QELEM(drvdata->mode);
+	/* start by clearing QE bits */
+	drvdata->cfg &= ~(BIT(13) | BIT(14));
+	/* if supported, Q elements with instruction counts are enabled */
+	if ((mode & BIT(0)) && (drvdata->q_support & BIT(0)))
+		drvdata->cfg |= BIT(13);
+	/*
+	 * if supported, Q elements with and without instruction
+	 * counts are enabled
+	 */
+	if ((mode & BIT(1)) && (drvdata->q_support & BIT(1)))
+		drvdata->cfg |= BIT(14);
+
+	/* bit[11], AMBA Trace Bus (ATB) trigger enable bit */
+	if ((drvdata->mode & ETM_MODE_ATB_TRIGGER) &&
+	    (drvdata->atbtrig == true))
+		drvdata->eventctrl1 |= BIT(11);
+	else
+		drvdata->eventctrl1 &= ~BIT(11);
+
+	/* bit[12], Low-power state behavior override bit */
+	if ((drvdata->mode & ETM_MODE_LPOVERRIDE) &&
+	    (drvdata->lpoverride == true))
+		drvdata->eventctrl1 |= BIT(12);
+	else
+		drvdata->eventctrl1 &= ~BIT(12);
+
+	/* bit[8], Instruction stall bit */
+	if (drvdata->mode & ETM_MODE_ISTALL_EN)
+		drvdata->stall_ctrl |= BIT(8);
+	else
+		drvdata->stall_ctrl &= ~BIT(8);
+
+	/* bit[10], Prioritize instruction trace bit */
+	if (drvdata->mode & ETM_MODE_INSTPRIO)
+		drvdata->stall_ctrl |= BIT(10);
+	else
+		drvdata->stall_ctrl &= ~BIT(10);
+
+	/* bit[13], Trace overflow prevention bit */
+	if ((drvdata->mode & ETM_MODE_NOOVERFLOW) &&
+		(drvdata->nooverflow == true))
+		drvdata->stall_ctrl |= BIT(13);
+	else
+		drvdata->stall_ctrl &= ~BIT(13);
+
+	/* bit[9] Start/stop logic control bit */
+	if (drvdata->mode & ETM_MODE_VIEWINST_STARTSTOP)
+		drvdata->vinst_ctrl |= BIT(9);
+	else
+		drvdata->vinst_ctrl &= ~BIT(9);
+
+	/* bit[10], Whether a trace unit must trace a Reset exception */
+	if (drvdata->mode & ETM_MODE_TRACE_RESET)
+		drvdata->vinst_ctrl |= BIT(10);
+	else
+		drvdata->vinst_ctrl &= ~BIT(10);
+
+	/* bit[11], Whether a trace unit must trace a system error exception */
+	if ((drvdata->mode & ETM_MODE_TRACE_ERR) &&
+		(drvdata->trc_error == true))
+		drvdata->vinst_ctrl |= BIT(11);
+	else
+		drvdata->vinst_ctrl &= ~BIT(11);
+
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(mode);
+
+static ssize_t pe_show(struct device *dev,
+		       struct device_attribute *attr,
+		       char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->pe_sel;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t pe_store(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	if (val > drvdata->nr_pe) {
+		spin_unlock(&drvdata->spinlock);
+		return -EINVAL;
+	}
+
+	drvdata->pe_sel = val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(pe);
+
+static ssize_t event_show(struct device *dev,
+			  struct device_attribute *attr,
+			  char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->eventctrl0;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t event_store(struct device *dev,
+			   struct device_attribute *attr,
+			   const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	switch (drvdata->nr_event) {
+	case 0x0:
+		/* EVENT0, bits[7:0] */
+		drvdata->eventctrl0 = val & 0xFF;
+		break;
+	case 0x1:
+		 /* EVENT1, bits[15:8] */
+		drvdata->eventctrl0 = val & 0xFFFF;
+		break;
+	case 0x2:
+		/* EVENT2, bits[23:16] */
+		drvdata->eventctrl0 = val & 0xFFFFFF;
+		break;
+	case 0x3:
+		/* EVENT3, bits[31:24] */
+		drvdata->eventctrl0 = val;
+		break;
+	default:
+		break;
+	}
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(event);
+
+static ssize_t event_instren_show(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = BMVAL(drvdata->eventctrl1, 0, 3);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t event_instren_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	/* start by clearing all instruction event enable bits */
+	drvdata->eventctrl1 &= ~(BIT(0) | BIT(1) | BIT(2) | BIT(3));
+	switch (drvdata->nr_event) {
+	case 0x0:
+		/* generate Event element for event 1 */
+		drvdata->eventctrl1 |= val & BIT(1);
+		break;
+	case 0x1:
+		/* generate Event element for event 1 and 2 */
+		drvdata->eventctrl1 |= val & (BIT(0) | BIT(1));
+		break;
+	case 0x2:
+		/* generate Event element for event 1, 2 and 3 */
+		drvdata->eventctrl1 |= val & (BIT(0) | BIT(1) | BIT(2));
+		break;
+	case 0x3:
+		/* generate Event element for all 4 events */
+		drvdata->eventctrl1 |= val & 0xF;
+		break;
+	default:
+		break;
+	}
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(event_instren);
+
+static ssize_t event_ts_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->ts_ctrl;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t event_ts_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (!drvdata->ts_size)
+		return -EINVAL;
+
+	drvdata->ts_ctrl = val & ETMv4_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(event_ts);
+
+static ssize_t syncfreq_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->syncfreq;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t syncfreq_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (drvdata->syncpr == true)
+		return -EINVAL;
+
+	drvdata->syncfreq = val & ETMv4_SYNC_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(syncfreq);
+
+static ssize_t cyc_threshold_show(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->ccctlr;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t cyc_threshold_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (val < drvdata->ccitmin)
+		return -EINVAL;
+
+	drvdata->ccctlr = val & ETM_CYC_THRESHOLD_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(cyc_threshold);
+
+static ssize_t bb_ctrl_show(struct device *dev,
+			    struct device_attribute *attr,
+			    char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->bb_ctrl;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t bb_ctrl_store(struct device *dev,
+			     struct device_attribute *attr,
+			     const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (drvdata->trcbb == false)
+		return -EINVAL;
+	if (!drvdata->nr_addr_cmp)
+		return -EINVAL;
+	/*
+	 * Bit[7:0] selects which address range comparator is used for
+	 * branch broadcast control.
+	 */
+	if (BMVAL(val, 0, 7) > drvdata->nr_addr_cmp)
+		return -EINVAL;
+
+	drvdata->bb_ctrl = val;
+	return size;
+}
+static DEVICE_ATTR_RW(bb_ctrl);
+
+static ssize_t event_vinst_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->vinst_ctrl & ETMv4_EVENT_MASK;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t event_vinst_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	val &= ETMv4_EVENT_MASK;
+	drvdata->vinst_ctrl &= ~ETMv4_EVENT_MASK;
+	drvdata->vinst_ctrl |= val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(event_vinst);
+
+static ssize_t s_exlevel_vinst_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = BMVAL(drvdata->vinst_ctrl, 16, 19);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t s_exlevel_vinst_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	/* clear all EXLEVEL_S bits (bit[18] is never implemented) */
+	drvdata->vinst_ctrl &= ~(BIT(16) | BIT(17) | BIT(19));
+	/* enable instruction tracing for corresponding exception level */
+	val &= drvdata->s_ex_level;
+	drvdata->vinst_ctrl |= (val << 16);
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(s_exlevel_vinst);
+
+static ssize_t ns_exlevel_vinst_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	/* EXLEVEL_NS, bits[23:20] */
+	val = BMVAL(drvdata->vinst_ctrl, 20, 23);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t ns_exlevel_vinst_store(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	/* clear EXLEVEL_NS bits (bit[23] is never implemented */
+	drvdata->vinst_ctrl &= ~(BIT(20) | BIT(21) | BIT(22));
+	/* enable instruction tracing for corresponding exception level */
+	val &= drvdata->ns_ex_level;
+	drvdata->vinst_ctrl |= (val << 20);
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(ns_exlevel_vinst);
+
+static ssize_t addr_idx_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->addr_idx;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t addr_idx_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (val >= drvdata->nr_addr_cmp * 2)
+		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_instdatatype_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	ssize_t len;
+	u8 val, idx;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	val = BMVAL(drvdata->addr_acc[idx], 0, 1);
+	len = scnprintf(buf, PAGE_SIZE, "%s\n",
+			val == ETM_INSTR_ADDR ? "instr" :
+			(val == ETM_DATA_LOAD_ADDR ? "data_load" :
+			(val == ETM_DATA_STORE_ADDR ? "data_store" :
+			"data_load_store")));
+	spin_unlock(&drvdata->spinlock);
+	return len;
+}
+
+static ssize_t addr_instdatatype_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf, size_t size)
+{
+	u8 idx;
+	char str[20] = "";
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (strlen(buf) >= 20)
+		return -EINVAL;
+	if (sscanf(buf, "%s", str) != 1)
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!strcmp(str, "instr"))
+		/* TYPE, bits[1:0] */
+		drvdata->addr_acc[idx] &= ~(BIT(0) | BIT(1));
+
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(addr_instdatatype);
+
+static ssize_t addr_single_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	idx = drvdata->addr_idx;
+	spin_lock(&drvdata->spinlock);
+	if (!(drvdata->addr_type[idx] == ETM_ADDR_TYPE_NONE ||
+	      drvdata->addr_type[idx] == ETM_ADDR_TYPE_SINGLE)) {
+		spin_unlock(&drvdata->spinlock);
+		return -EPERM;
+	}
+	val = (unsigned long)drvdata->addr_val[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t addr_single_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	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 -EPERM;
+	}
+
+	drvdata->addr_val[idx] = (u64)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 etmv4_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 = (unsigned long)drvdata->addr_val[idx];
+	val2 = (unsigned long)drvdata->addr_val[idx + 1];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#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 etmv4_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] = (u64)val1;
+	drvdata->addr_type[idx] = ETM_ADDR_TYPE_RANGE;
+	drvdata->addr_val[idx + 1] = (u64)val2;
+	drvdata->addr_type[idx + 1] = ETM_ADDR_TYPE_RANGE;
+	/*
+	 * Program include or exclude control bits for vinst or vdata
+	 * whenever we change addr comparators to ETM_ADDR_TYPE_RANGE
+	 */
+	if (drvdata->mode & ETM_MODE_EXCLUDE)
+		etm4_set_mode_exclude(drvdata, true);
+	else
+		etm4_set_mode_exclude(drvdata, false);
+
+	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 etmv4_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 = (unsigned long)drvdata->addr_val[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t addr_start_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!drvdata->nr_addr_cmp) {
+		spin_unlock(&drvdata->spinlock);
+		return -EINVAL;
+	}
+	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] = (u64)val;
+	drvdata->addr_type[idx] = ETM_ADDR_TYPE_START;
+	drvdata->vissctlr |= BIT(idx);
+	/* SSSTATUS, bit[9] - turn on start/stop logic */
+	drvdata->vinst_ctrl |= BIT(9);
+	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 etmv4_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 = (unsigned long)drvdata->addr_val[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t addr_stop_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!drvdata->nr_addr_cmp) {
+		spin_unlock(&drvdata->spinlock);
+		return -EINVAL;
+	}
+	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] = (u64)val;
+	drvdata->addr_type[idx] = ETM_ADDR_TYPE_STOP;
+	drvdata->vissctlr |= BIT(idx + 16);
+	/* SSSTATUS, bit[9] - turn on start/stop logic */
+	drvdata->vinst_ctrl |= BIT(9);
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(addr_stop);
+
+static ssize_t addr_ctxtype_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	ssize_t len;
+	u8 idx, val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	/* CONTEXTTYPE, bits[3:2] */
+	val = BMVAL(drvdata->addr_acc[idx], 2, 3);
+	len = scnprintf(buf, PAGE_SIZE, "%s\n", val == ETM_CTX_NONE ? "none" :
+			(val == ETM_CTX_CTXID ? "ctxid" :
+			(val == ETM_CTX_VMID ? "vmid" : "all")));
+	spin_unlock(&drvdata->spinlock);
+	return len;
+}
+
+static ssize_t addr_ctxtype_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	u8 idx;
+	char str[10] = "";
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (strlen(buf) >= 10)
+		return -EINVAL;
+	if (sscanf(buf, "%s", str) != 1)
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	if (!strcmp(str, "none"))
+		/* start by clearing context type bits */
+		drvdata->addr_acc[idx] &= ~(BIT(2) | BIT(3));
+	else if (!strcmp(str, "ctxid")) {
+		/* 0b01 The trace unit performs a Context ID */
+		if (drvdata->numcidc) {
+			drvdata->addr_acc[idx] |= BIT(2);
+			drvdata->addr_acc[idx] &= ~BIT(3);
+		}
+	} else if (!strcmp(str, "vmid")) {
+		/* 0b10 The trace unit performs a VMID */
+		if (drvdata->numvmidc) {
+			drvdata->addr_acc[idx] &= ~BIT(2);
+			drvdata->addr_acc[idx] |= BIT(3);
+		}
+	} else if (!strcmp(str, "all")) {
+		/*
+		 * 0b11 The trace unit performs a Context ID
+		 * comparison and a VMID
+		 */
+		if (drvdata->numcidc)
+			drvdata->addr_acc[idx] |= BIT(2);
+		if (drvdata->numvmidc)
+			drvdata->addr_acc[idx] |= BIT(3);
+	}
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(addr_ctxtype);
+
+static ssize_t addr_context_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	/* context ID comparator bits[6:4] */
+	val = BMVAL(drvdata->addr_acc[idx], 4, 6);
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t addr_context_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if ((drvdata->numcidc <= 1) && (drvdata->numvmidc <= 1))
+		return -EINVAL;
+	if (val >=  (drvdata->numcidc >= drvdata->numvmidc ?
+		     drvdata->numcidc : drvdata->numvmidc))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->addr_idx;
+	/* clear context ID comparator bits[6:4] */
+	drvdata->addr_acc[idx] &= ~(BIT(4) | BIT(5) | BIT(6));
+	drvdata->addr_acc[idx] |= (val << 4);
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(addr_context);
+
+static ssize_t seq_idx_show(struct device *dev,
+			    struct device_attribute *attr,
+			    char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_idx;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t seq_idx_store(struct device *dev,
+			     struct device_attribute *attr,
+			     const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (val >= drvdata->nrseqstate - 1)
+		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->seq_idx = val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(seq_idx);
+
+static ssize_t seq_state_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_state;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t seq_state_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (val >= drvdata->nrseqstate)
+		return -EINVAL;
+
+	drvdata->seq_state = val;
+	return size;
+}
+static DEVICE_ATTR_RW(seq_state);
+
+static ssize_t seq_event_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->seq_idx;
+	val = drvdata->seq_ctrl[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t seq_event_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->seq_idx;
+	/* RST, bits[7:0] */
+	drvdata->seq_ctrl[idx] = val & 0xFF;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(seq_event);
+
+static ssize_t seq_reset_event_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->seq_rst;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t seq_reset_event_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (!(drvdata->nrseqstate))
+		return -EINVAL;
+
+	drvdata->seq_rst = val & ETMv4_EVENT_MASK;
+	return size;
+}
+static DEVICE_ATTR_RW(seq_reset_event);
+
+static ssize_t cntr_idx_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->cntr_idx;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t cntr_idx_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	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 cntrldvr_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->cntr_idx;
+	val = drvdata->cntrldvr[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t cntrldvr_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (val > ETM_CNTR_MAX_VAL)
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->cntr_idx;
+	drvdata->cntrldvr[idx] = val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(cntrldvr);
+
+static ssize_t cntr_val_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->cntr_idx;
+	val = drvdata->cntr_val[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t cntr_val_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (val > ETM_CNTR_MAX_VAL)
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->cntr_idx;
+	drvdata->cntr_val[idx] = val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(cntr_val);
+
+static ssize_t cntr_ctrl_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->cntr_idx;
+	val = drvdata->cntr_ctrl[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t cntr_ctrl_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->cntr_idx;
+	drvdata->cntr_ctrl[idx] = val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(cntr_ctrl);
+
+static ssize_t res_idx_show(struct device *dev,
+			    struct device_attribute *attr,
+			    char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->res_idx;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t res_idx_store(struct device *dev,
+			     struct device_attribute *attr,
+			     const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	/* Resource selector pair 0 is always implemented and reserved */
+	if ((val == 0) || (val >= drvdata->nr_resource))
+		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->res_idx = val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(res_idx);
+
+static ssize_t res_ctrl_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->res_idx;
+	val = drvdata->res_ctrl[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t res_ctrl_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->res_idx;
+	/* For odd idx pair inversal bit is RES0 */
+	if (idx % 2 != 0)
+		/* PAIRINV, bit[21] */
+		val &= ~BIT(21);
+	drvdata->res_ctrl[idx] = val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(res_ctrl);
+
+static ssize_t ctxid_idx_show(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->ctxid_idx;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t ctxid_idx_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (val >= drvdata->numcidc)
+		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)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->ctxid_idx;
+	val = (unsigned long)drvdata->ctxid_val[idx];
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t ctxid_val_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t size)
+{
+	u8 idx;
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	/*
+	 * only implemented when ctxid tracing is enabled, i.e. at least one
+	 * ctxid comparator is implemented and ctxid is greater than 0 bits
+	 * in length
+	 */
+	if (!drvdata->ctxid_size || !drvdata->numcidc)
+		return -EINVAL;
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	idx = drvdata->ctxid_idx;
+	drvdata->ctxid_val[idx] = (u64)val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(ctxid_val);
+
+static ssize_t ctxid_masks_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	unsigned long val1, val2;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	val1 = drvdata->ctxid_mask0;
+	val2 = drvdata->ctxid_mask1;
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx %#lx\n", val1, val2);
+}
+
+static ssize_t ctxid_masks_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	u8 i, j, maskbyte;
+	unsigned long val1, val2, mask;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	/*
+	 * only implemented when ctxid tracing is enabled, i.e. at least one
+	 * ctxid comparator is implemented and ctxid is greater than 0 bits
+	 * in length
+	 */
+	if (!drvdata->ctxid_size || !drvdata->numcidc)
+		return -EINVAL;
+	if (sscanf(buf, "%lx %lx", &val1, &val2) != 2)
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	/*
+	 * each byte[0..3] controls mask value applied to ctxid
+	 * comparator[0..3]
+	 */
+	switch (drvdata->numcidc) {
+	case 0x1:
+		/* COMP0, bits[7:0] */
+		drvdata->ctxid_mask0 = val1 & 0xFF;
+		break;
+	case 0x2:
+		/* COMP1, bits[15:8] */
+		drvdata->ctxid_mask0 = val1 & 0xFFFF;
+		break;
+	case 0x3:
+		/* COMP2, bits[23:16] */
+		drvdata->ctxid_mask0 = val1 & 0xFFFFFF;
+		break;
+	case 0x4:
+		 /* COMP3, bits[31:24] */
+		drvdata->ctxid_mask0 = val1;
+		break;
+	case 0x5:
+		/* COMP4, bits[7:0] */
+		drvdata->ctxid_mask0 = val1;
+		drvdata->ctxid_mask1 = val2 & 0xFF;
+		break;
+	case 0x6:
+		/* COMP5, bits[15:8] */
+		drvdata->ctxid_mask0 = val1;
+		drvdata->ctxid_mask1 = val2 & 0xFFFF;
+		break;
+	case 0x7:
+		/* COMP6, bits[23:16] */
+		drvdata->ctxid_mask0 = val1;
+		drvdata->ctxid_mask1 = val2 & 0xFFFFFF;
+		break;
+	case 0x8:
+		/* COMP7, bits[31:24] */
+		drvdata->ctxid_mask0 = val1;
+		drvdata->ctxid_mask1 = val2;
+		break;
+	default:
+		break;
+	}
+	/*
+	 * If software sets a mask bit to 1, it must program relevant byte
+	 * of ctxid comparator value 0x0, otherwise behavior is unpredictable.
+	 * For example, if bit[3] of ctxid_mask0 is 1, we must clear bits[31:24]
+	 * of ctxid comparator0 value (corresponding to byte 0) register.
+	 */
+	mask = drvdata->ctxid_mask0;
+	for (i = 0; i < drvdata->numcidc; i++) {
+		/* mask value of corresponding ctxid comparator */
+		maskbyte = mask & ETMv4_EVENT_MASK;
+		/*
+		 * each bit corresponds to a byte of respective ctxid comparator
+		 * value register
+		 */
+		for (j = 0; j < 8; j++) {
+			if (maskbyte & 1)
+				drvdata->ctxid_val[i] &= ~(0xFF << (j * 8));
+			maskbyte >>= 1;
+		}
+		/* Select the next ctxid comparator mask value */
+		if (i == 3)
+			/* ctxid comparators[4-7] */
+			mask = drvdata->ctxid_mask1;
+		else
+			mask >>= 0x8;
+	}
+
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(ctxid_masks);
+
+static ssize_t vmid_idx_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->vmid_idx;
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t vmid_idx_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+	if (val >= drvdata->numvmidc)
+		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->vmid_idx = val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(vmid_idx);
+
+static ssize_t vmid_val_show(struct device *dev,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = (unsigned long)drvdata->vmid_val[drvdata->vmid_idx];
+	return scnprintf(buf, PAGE_SIZE, "%#lx\n", val);
+}
+
+static ssize_t vmid_val_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t size)
+{
+	unsigned long val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	/*
+	 * only implemented when vmid tracing is enabled, i.e. at least one
+	 * vmid comparator is implemented and at least 8 bit vmid size
+	 */
+	if (!drvdata->vmid_size || !drvdata->numvmidc)
+		return -EINVAL;
+	if (kstrtoul(buf, 16, &val))
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+	drvdata->vmid_val[drvdata->vmid_idx] = (u64)val;
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(vmid_val);
+
+static ssize_t vmid_masks_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	unsigned long val1, val2;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	spin_lock(&drvdata->spinlock);
+	val1 = drvdata->vmid_mask0;
+	val2 = drvdata->vmid_mask1;
+	spin_unlock(&drvdata->spinlock);
+	return scnprintf(buf, PAGE_SIZE, "%#lx %#lx\n", val1, val2);
+}
+
+static ssize_t vmid_masks_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	u8 i, j, maskbyte;
+	unsigned long val1, val2, mask;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+	/*
+	 * only implemented when vmid tracing is enabled, i.e. at least one
+	 * vmid comparator is implemented and at least 8 bit vmid size
+	 */
+	if (!drvdata->vmid_size || !drvdata->numvmidc)
+		return -EINVAL;
+	if (sscanf(buf, "%lx %lx", &val1, &val2) != 2)
+		return -EINVAL;
+
+	spin_lock(&drvdata->spinlock);
+
+	/*
+	 * each byte[0..3] controls mask value applied to vmid
+	 * comparator[0..3]
+	 */
+	switch (drvdata->numvmidc) {
+	case 0x1:
+		/* COMP0, bits[7:0] */
+		drvdata->vmid_mask0 = val1 & 0xFF;
+		break;
+	case 0x2:
+		/* COMP1, bits[15:8] */
+		drvdata->vmid_mask0 = val1 & 0xFFFF;
+		break;
+	case 0x3:
+		/* COMP2, bits[23:16] */
+		drvdata->vmid_mask0 = val1 & 0xFFFFFF;
+		break;
+	case 0x4:
+		/* COMP3, bits[31:24] */
+		drvdata->vmid_mask0 = val1;
+		break;
+	case 0x5:
+		/* COMP4, bits[7:0] */
+		drvdata->vmid_mask0 = val1;
+		drvdata->vmid_mask1 = val2 & 0xFF;
+		break;
+	case 0x6:
+		/* COMP5, bits[15:8] */
+		drvdata->vmid_mask0 = val1;
+		drvdata->vmid_mask1 = val2 & 0xFFFF;
+		break;
+	case 0x7:
+		/* COMP6, bits[23:16] */
+		drvdata->vmid_mask0 = val1;
+		drvdata->vmid_mask1 = val2 & 0xFFFFFF;
+		break;
+	case 0x8:
+		/* COMP7, bits[31:24] */
+		drvdata->vmid_mask0 = val1;
+		drvdata->vmid_mask1 = val2;
+		break;
+	default:
+		break;
+	}
+
+	/*
+	 * If software sets a mask bit to 1, it must program relevant byte
+	 * of vmid comparator value 0x0, otherwise behavior is unpredictable.
+	 * For example, if bit[3] of vmid_mask0 is 1, we must clear bits[31:24]
+	 * of vmid comparator0 value (corresponding to byte 0) register.
+	 */
+	mask = drvdata->vmid_mask0;
+	for (i = 0; i < drvdata->numvmidc; i++) {
+		/* mask value of corresponding vmid comparator */
+		maskbyte = mask & ETMv4_EVENT_MASK;
+		/*
+		 * each bit corresponds to a byte of respective vmid comparator
+		 * value register
+		 */
+		for (j = 0; j < 8; j++) {
+			if (maskbyte & 1)
+				drvdata->vmid_val[i] &= ~(0xFF << (j * 8));
+			maskbyte >>= 1;
+		}
+		/* Select the next vmid comparator mask value */
+		if (i == 3)
+			/* vmid comparators[4-7] */
+			mask = drvdata->vmid_mask1;
+		else
+			mask >>= 0x8;
+	}
+	spin_unlock(&drvdata->spinlock);
+	return size;
+}
+static DEVICE_ATTR_RW(vmid_masks);
+
+static ssize_t cpu_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	int val;
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(dev->parent);
+
+	val = drvdata->cpu;
+	return scnprintf(buf, PAGE_SIZE, "%d\n", val);
+
+}
+static DEVICE_ATTR_RO(cpu);
+
+static struct attribute *coresight_etmv4_attrs[] = {
+	&dev_attr_nr_pe_cmp.attr,
+	&dev_attr_nr_addr_cmp.attr,
+	&dev_attr_nr_cntr.attr,
+	&dev_attr_nr_ext_inp.attr,
+	&dev_attr_numcidc.attr,
+	&dev_attr_numvmidc.attr,
+	&dev_attr_nrseqstate.attr,
+	&dev_attr_nr_resource.attr,
+	&dev_attr_nr_ss_cmp.attr,
+	&dev_attr_reset.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_pe.attr,
+	&dev_attr_event.attr,
+	&dev_attr_event_instren.attr,
+	&dev_attr_event_ts.attr,
+	&dev_attr_syncfreq.attr,
+	&dev_attr_cyc_threshold.attr,
+	&dev_attr_bb_ctrl.attr,
+	&dev_attr_event_vinst.attr,
+	&dev_attr_s_exlevel_vinst.attr,
+	&dev_attr_ns_exlevel_vinst.attr,
+	&dev_attr_addr_idx.attr,
+	&dev_attr_addr_instdatatype.attr,
+	&dev_attr_addr_single.attr,
+	&dev_attr_addr_range.attr,
+	&dev_attr_addr_start.attr,
+	&dev_attr_addr_stop.attr,
+	&dev_attr_addr_ctxtype.attr,
+	&dev_attr_addr_context.attr,
+	&dev_attr_seq_idx.attr,
+	&dev_attr_seq_state.attr,
+	&dev_attr_seq_event.attr,
+	&dev_attr_seq_reset_event.attr,
+	&dev_attr_cntr_idx.attr,
+	&dev_attr_cntrldvr.attr,
+	&dev_attr_cntr_val.attr,
+	&dev_attr_cntr_ctrl.attr,
+	&dev_attr_res_idx.attr,
+	&dev_attr_res_ctrl.attr,
+	&dev_attr_ctxid_idx.attr,
+	&dev_attr_ctxid_val.attr,
+	&dev_attr_ctxid_masks.attr,
+	&dev_attr_vmid_idx.attr,
+	&dev_attr_vmid_val.attr,
+	&dev_attr_vmid_masks.attr,
+	&dev_attr_cpu.attr,
+	NULL,
+};
+
+#define coresight_simple_func(name, offset)				\
+static ssize_t name##_show(struct device *_dev,				\
+			   struct device_attribute *attr, char *buf)	\
+{									\
+	struct etmv4_drvdata *drvdata = dev_get_drvdata(_dev->parent);	\
+	return scnprintf(buf, PAGE_SIZE, "0x%x\n",			\
+			 readl_relaxed(drvdata->base + offset));	\
+}									\
+DEVICE_ATTR_RO(name)
+
+coresight_simple_func(trcoslsr, TRCOSLSR);
+coresight_simple_func(trcpdcr, TRCPDCR);
+coresight_simple_func(trcpdsr, TRCPDSR);
+coresight_simple_func(trclsr, TRCLSR);
+coresight_simple_func(trcauthstatus, TRCAUTHSTATUS);
+coresight_simple_func(trcdevid, TRCDEVID);
+coresight_simple_func(trcdevtype, TRCDEVTYPE);
+coresight_simple_func(trcpidr0, TRCPIDR0);
+coresight_simple_func(trcpidr1, TRCPIDR1);
+coresight_simple_func(trcpidr2, TRCPIDR2);
+coresight_simple_func(trcpidr3, TRCPIDR3);
+
+static struct attribute *coresight_etmv4_mgmt_attrs[] = {
+	&dev_attr_trcoslsr.attr,
+	&dev_attr_trcpdcr.attr,
+	&dev_attr_trcpdsr.attr,
+	&dev_attr_trclsr.attr,
+	&dev_attr_trcauthstatus.attr,
+	&dev_attr_trcdevid.attr,
+	&dev_attr_trcdevtype.attr,
+	&dev_attr_trcpidr0.attr,
+	&dev_attr_trcpidr1.attr,
+	&dev_attr_trcpidr2.attr,
+	&dev_attr_trcpidr3.attr,
+	NULL,
+};
+
+coresight_simple_func(trcidr0, TRCIDR0);
+coresight_simple_func(trcidr1, TRCIDR1);
+coresight_simple_func(trcidr2, TRCIDR2);
+coresight_simple_func(trcidr3, TRCIDR3);
+coresight_simple_func(trcidr4, TRCIDR4);
+coresight_simple_func(trcidr5, TRCIDR5);
+/* trcidr[6,7] are reserved */
+coresight_simple_func(trcidr8, TRCIDR8);
+coresight_simple_func(trcidr9, TRCIDR9);
+coresight_simple_func(trcidr10, TRCIDR10);
+coresight_simple_func(trcidr11, TRCIDR11);
+coresight_simple_func(trcidr12, TRCIDR12);
+coresight_simple_func(trcidr13, TRCIDR13);
+
+static struct attribute *coresight_etmv4_trcidr_attrs[] = {
+	&dev_attr_trcidr0.attr,
+	&dev_attr_trcidr1.attr,
+	&dev_attr_trcidr2.attr,
+	&dev_attr_trcidr3.attr,
+	&dev_attr_trcidr4.attr,
+	&dev_attr_trcidr5.attr,
+	/* trcidr[6,7] are reserved */
+	&dev_attr_trcidr8.attr,
+	&dev_attr_trcidr9.attr,
+	&dev_attr_trcidr10.attr,
+	&dev_attr_trcidr11.attr,
+	&dev_attr_trcidr12.attr,
+	&dev_attr_trcidr13.attr,
+	NULL,
+};
+
+static const struct attribute_group coresight_etmv4_group = {
+	.attrs = coresight_etmv4_attrs,
+};
+
+static const struct attribute_group coresight_etmv4_mgmt_group = {
+	.attrs = coresight_etmv4_mgmt_attrs,
+	.name = "mgmt",
+};
+
+static const struct attribute_group coresight_etmv4_trcidr_group = {
+	.attrs = coresight_etmv4_trcidr_attrs,
+	.name = "trcidr",
+};
+
+static const struct attribute_group *coresight_etmv4_groups[] = {
+	&coresight_etmv4_group,
+	&coresight_etmv4_mgmt_group,
+	&coresight_etmv4_trcidr_group,
+	NULL,
+};
+
+static void etm4_init_arch_data(void *info)
+{
+	u32 etmidr0;
+	u32 etmidr1;
+	u32 etmidr2;
+	u32 etmidr3;
+	u32 etmidr4;
+	u32 etmidr5;
+	struct etmv4_drvdata *drvdata = info;
+
+	CS_UNLOCK(drvdata->base);
+
+	/* find all capabilities of the tracing unit */
+	etmidr0 = readl_relaxed(drvdata->base + TRCIDR0);
+
+	/* INSTP0, bits[2:1] P0 tracing support field */
+	if (BMVAL(etmidr0, 1, 1) && BMVAL(etmidr0, 2, 2))
+		drvdata->instrp0 = true;
+	else
+		drvdata->instrp0 = false;
+
+	/* TRCBB, bit[5] Branch broadcast tracing support bit */
+	if (BMVAL(etmidr0, 5, 5))
+		drvdata->trcbb = true;
+	else
+		drvdata->trcbb = false;
+
+	/* TRCCOND, bit[6] Conditional instruction tracing support bit */
+	if (BMVAL(etmidr0, 6, 6))
+		drvdata->trccond = true;
+	else
+		drvdata->trccond = false;
+
+	/* TRCCCI, bit[7] Cycle counting instruction bit */
+	if (BMVAL(etmidr0, 7, 7))
+		drvdata->trccci = true;
+	else
+		drvdata->trccci = false;
+
+	/* RETSTACK, bit[9] Return stack bit */
+	if (BMVAL(etmidr0, 9, 9))
+		drvdata->retstack = true;
+	else
+		drvdata->retstack = false;
+
+	/* NUMEVENT, bits[11:10] Number of events field */
+	drvdata->nr_event = BMVAL(etmidr0, 10, 11);
+	/* QSUPP, bits[16:15] Q element support field */
+	drvdata->q_support = BMVAL(etmidr0, 15, 16);
+	/* TSSIZE, bits[28:24] Global timestamp size field */
+	drvdata->ts_size = BMVAL(etmidr0, 24, 28);
+
+	/* base architecture of trace unit */
+	etmidr1 = readl_relaxed(drvdata->base + TRCIDR1);
+	/*
+	 * TRCARCHMIN, bits[7:4] architecture the minor version number
+	 * TRCARCHMAJ, bits[11:8] architecture major versin number
+	 */
+	drvdata->arch = BMVAL(etmidr1, 4, 11);
+
+	/* maximum size of resources */
+	etmidr2 = readl_relaxed(drvdata->base + TRCIDR2);
+	/* CIDSIZE, bits[9:5] Indicates the Context ID size */
+	drvdata->ctxid_size = BMVAL(etmidr2, 5, 9);
+	/* VMIDSIZE, bits[14:10] Indicates the VMID size */
+	drvdata->vmid_size = BMVAL(etmidr2, 10, 14);
+	/* CCSIZE, bits[28:25] size of the cycle counter in bits minus 12 */
+	drvdata->ccsize = BMVAL(etmidr2, 25, 28);
+
+	etmidr3 = readl_relaxed(drvdata->base + TRCIDR3);
+	/* CCITMIN, bits[11:0] minimum threshold value that can be programmed */
+	drvdata->ccitmin = BMVAL(etmidr3, 0, 11);
+	/* EXLEVEL_S, bits[19:16] Secure state instruction tracing */
+	drvdata->s_ex_level = BMVAL(etmidr3, 16, 19);
+	/* EXLEVEL_NS, bits[23:20] Non-secure state instruction tracing */
+	drvdata->ns_ex_level = BMVAL(etmidr3, 20, 23);
+
+	/*
+	 * TRCERR, bit[24] whether a trace unit can trace a
+	 * system error exception.
+	 */
+	if (BMVAL(etmidr3, 24, 24))
+		drvdata->trc_error = true;
+	else
+		drvdata->trc_error = false;
+
+	/* SYNCPR, bit[25] implementation has a fixed synchronization period? */
+	if (BMVAL(etmidr3, 25, 25))
+		drvdata->syncpr = true;
+	else
+		drvdata->syncpr = false;
+
+	/* STALLCTL, bit[26] is stall control implemented? */
+	if (BMVAL(etmidr3, 26, 26))
+		drvdata->stallctl = true;
+	else
+		drvdata->stallctl = false;
+
+	/* SYSSTALL, bit[27] implementation can support stall control? */
+	if (BMVAL(etmidr3, 27, 27))
+		drvdata->sysstall = true;
+	else
+		drvdata->sysstall = false;
+
+	/* NUMPROC, bits[30:28] the number of PEs available for tracing */
+	drvdata->nr_pe = BMVAL(etmidr3, 28, 30);
+
+	/* NOOVERFLOW, bit[31] is trace overflow prevention supported */
+	if (BMVAL(etmidr3, 31, 31))
+		drvdata->nooverflow = true;
+	else
+		drvdata->nooverflow = false;
+
+	/* number of resources trace unit supports */
+	etmidr4 = readl_relaxed(drvdata->base + TRCIDR4);
+	/* NUMACPAIRS, bits[0:3] number of addr comparator pairs for tracing */
+	drvdata->nr_addr_cmp = BMVAL(etmidr4, 0, 3);
+	/* NUMPC, bits[15:12] number of PE comparator inputs for tracing */
+	drvdata->nr_pe_cmp = BMVAL(etmidr4, 12, 15);
+	/* NUMRSPAIR, bits[19:16] the number of resource pairs for tracing */
+	drvdata->nr_resource = BMVAL(etmidr4, 16, 19);
+	/*
+	 * NUMSSCC, bits[23:20] the number of single-shot
+	 * comparator control for tracing
+	 */
+	drvdata->nr_ss_cmp = BMVAL(etmidr4, 20, 23);
+	/* NUMCIDC, bits[27:24] number of Context ID comparators for tracing */
+	drvdata->numcidc = BMVAL(etmidr4, 24, 27);
+	/* NUMVMIDC, bits[31:28] number of VMID comparators for tracing */
+	drvdata->numvmidc = BMVAL(etmidr4, 28, 31);
+
+	etmidr5 = readl_relaxed(drvdata->base + TRCIDR5);
+	/* NUMEXTIN, bits[8:0] number of external inputs implemented */
+	drvdata->nr_ext_inp = BMVAL(etmidr5, 0, 8);
+	/* TRACEIDSIZE, bits[21:16] indicates the trace ID width */
+	drvdata->trcid_size = BMVAL(etmidr5, 16, 21);
+	/* ATBTRIG, bit[22] implementation can support ATB triggers? */
+	if (BMVAL(etmidr5, 22, 22))
+		drvdata->atbtrig = true;
+	else
+		drvdata->atbtrig = false;
+	/*
+	 * LPOVERRIDE, bit[23] implementation supports
+	 * low-power state override
+	 */
+	if (BMVAL(etmidr5, 23, 23))
+		drvdata->lpoverride = true;
+	else
+		drvdata->lpoverride = false;
+	/* NUMSEQSTATE, bits[27:25] number of sequencer states implemented */
+	drvdata->nrseqstate = BMVAL(etmidr5, 25, 27);
+	/* NUMCNTR, bits[30:28] number of counters available for tracing */
+	drvdata->nr_cntr = BMVAL(etmidr5, 28, 30);
+	CS_LOCK(drvdata->base);
+}
+
+static void etm4_init_default_data(struct etmv4_drvdata *drvdata)
+{
+	int i;
+
+	drvdata->pe_sel = 0x0;
+	drvdata->cfg = (ETMv4_MODE_CTXID | ETM_MODE_VMID |
+			ETMv4_MODE_TIMESTAMP | ETM_MODE_RETURNSTACK);
+
+	/* disable all events tracing */
+	drvdata->eventctrl0 = 0x0;
+	drvdata->eventctrl1 = 0x0;
+
+	/* disable stalling */
+	drvdata->stall_ctrl = 0x0;
+
+	/* disable timestamp event */
+	drvdata->ts_ctrl = 0x0;
+
+	/* enable trace synchronization every 4096 bytes for trace */
+	if (drvdata->syncpr == false)
+		drvdata->syncfreq = 0xC;
+
+	/*
+	 *  enable viewInst to trace everything with start-stop logic in
+	 *  started state
+	 */
+	drvdata->vinst_ctrl |= BIT(0);
+	/* set initial state of start-stop logic */
+	if (drvdata->nr_addr_cmp)
+		drvdata->vinst_ctrl |= BIT(9);
+
+	/* no address range filtering for ViewInst */
+	drvdata->viiectlr = 0x0;
+	/* no start-stop filtering for ViewInst */
+	drvdata->vissctlr = 0x0;
+
+	/* disable seq events */
+	for (i = 0; i < drvdata->nrseqstate-1; i++)
+		drvdata->seq_ctrl[i] = 0x0;
+	drvdata->seq_rst = 0x0;
+	drvdata->seq_state = 0x0;
+
+	/* disable external input events */
+	drvdata->ext_inp = 0x0;
+
+	for (i = 0; i < drvdata->nr_cntr; i++) {
+		drvdata->cntrldvr[i] = 0x0;
+		drvdata->cntr_ctrl[i] = 0x0;
+		drvdata->cntr_val[i] = 0x0;
+	}
+
+	for (i = 2; i < drvdata->nr_resource * 2; i++)
+		drvdata->res_ctrl[i] = 0x0;
+
+	for (i = 0; i < drvdata->nr_ss_cmp; i++) {
+		drvdata->ss_ctrl[i] = 0x0;
+		drvdata->ss_pe_cmp[i] = 0x0;
+	}
+
+	if (drvdata->nr_addr_cmp >= 1) {
+		drvdata->addr_val[0] = (unsigned long)_stext;
+		drvdata->addr_val[1] = (unsigned long)_etext;
+		drvdata->addr_type[0] = ETM_ADDR_TYPE_RANGE;
+		drvdata->addr_type[1] = ETM_ADDR_TYPE_RANGE;
+	}
+
+	for (i = 0; i < drvdata->numcidc; i++)
+		drvdata->ctxid_val[i] = 0x0;
+	drvdata->ctxid_mask0 = 0x0;
+	drvdata->ctxid_mask1 = 0x0;
+
+	for (i = 0; i < drvdata->numvmidc; i++)
+		drvdata->vmid_val[i] = 0x0;
+	drvdata->vmid_mask0 = 0x0;
+	drvdata->vmid_mask1 = 0x0;
+
+	/*
+	 * A trace ID value of 0 is invalid, so let's start at some
+	 * random value that fits in 7 bits.  ETMv3.x has 0x10 so let's
+	 * start at 0x20.
+	 */
+	drvdata->trcid = 0x20 + drvdata->cpu;
+}
+
+static int etm4_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) {
+			etm4_os_unlock(etmdrvdata[cpu]);
+			etmdrvdata[cpu]->os_unlock = true;
+		}
+
+		if (etmdrvdata[cpu]->enable)
+			etm4_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)
+			etm4_disable_hw(etmdrvdata[cpu]);
+		spin_unlock(&etmdrvdata[cpu]->spinlock);
+		break;
+	}
+out:
+	return NOTIFY_OK;
+}
+
+static struct notifier_block etm4_cpu_notifier = {
+	.notifier_call = etm4_cpu_callback,
+};
+
+static int etm4_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 etmv4_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->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->cpu = pdata ? pdata->cpu : 0;
+
+	get_online_cpus();
+	etmdrvdata[drvdata->cpu] = drvdata;
+
+	if (!smp_call_function_single(drvdata->cpu, etm4_os_unlock, drvdata, 1))
+		drvdata->os_unlock = true;
+
+	if (smp_call_function_single(drvdata->cpu,
+				etm4_init_arch_data,  drvdata, 1))
+		dev_err(dev, "ETM arch init failed\n");
+
+	if (!etm4_count++)
+		register_hotcpu_notifier(&etm4_cpu_notifier);
+
+	put_online_cpus();
+
+	if (etm4_arch_supported(drvdata->arch) == false) {
+		ret = -EINVAL;
+		goto err_arch_supported;
+	}
+	etm4_init_default_data(drvdata);
+
+	pm_runtime_put(&adev->dev);
+
+	desc->type = CORESIGHT_DEV_TYPE_SOURCE;
+	desc->subtype.source_subtype = CORESIGHT_DEV_SUBTYPE_SOURCE_PROC;
+	desc->ops = &etm4_cs_ops;
+	desc->pdata = pdata;
+	desc->dev = dev;
+	desc->groups = coresight_etmv4_groups;
+	drvdata->csdev = coresight_register(desc);
+	if (IS_ERR(drvdata->csdev)) {
+		ret = PTR_ERR(drvdata->csdev);
+		goto err_coresight_register;
+	}
+
+	dev_info(dev, "%s initialized\n", (char *)id->data);
+
+	if (boot_enable) {
+		coresight_enable(drvdata->csdev);
+		drvdata->boot_enable = true;
+	}
+
+	return 0;
+
+err_arch_supported:
+	pm_runtime_put(&adev->dev);
+err_coresight_register:
+	if (--etm4_count == 0)
+		unregister_hotcpu_notifier(&etm4_cpu_notifier);
+	return ret;
+}
+
+static int etm4_remove(struct amba_device *adev)
+{
+	struct etmv4_drvdata *drvdata = amba_get_drvdata(adev);
+
+	coresight_unregister(drvdata->csdev);
+	if (--etm4_count == 0)
+		unregister_hotcpu_notifier(&etm4_cpu_notifier);
+
+	return 0;
+}
+
+static struct amba_id etm4_ids[] = {
+	{       /* ETM 4.0 - Qualcomm */
+		.id	= 0x0003b95d,
+		.mask	= 0x0003ffff,
+		.data	= "ETM 4.0",
+	},
+	{       /* ETM 4.0 - Juno board */
+		.id	= 0x000bb95e,
+		.mask	= 0x000fffff,
+		.data	= "ETM 4.0",
+	},
+	{ 0, 0},
+};
+
+static struct amba_driver etm4x_driver = {
+	.drv = {
+		.name   = "coresight-etm4x",
+	},
+	.probe		= etm4_probe,
+	.remove		= etm4_remove,
+	.id_table	= etm4_ids,
+};
+
+module_amba_driver(etm4x_driver);
diff --git a/drivers/hwtracing/coresight/coresight-etm4x.h b/drivers/hwtracing/coresight/coresight-etm4x.h
new file mode 100644
index 000000000000..e08e983dd2d9
--- /dev/null
+++ b/drivers/hwtracing/coresight/coresight-etm4x.h
@@ -0,0 +1,391 @@
+/* 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
+ * 0xF00: Management		registers
+ * 0xFA0 - 0xFA4: Trace		registers
+ * 0xFA8 - 0xFFC: Management	registers
+ */
+/* Trace registers (0x000-0x2FC) */
+/* Main control and configuration registers */
+#define TRCPRGCTLR			0x004
+#define TRCPROCSELR			0x008
+#define TRCSTATR			0x00C
+#define TRCCONFIGR			0x010
+#define TRCAUXCTLR			0x018
+#define TRCEVENTCTL0R			0x020
+#define TRCEVENTCTL1R			0x024
+#define TRCSTALLCTLR			0x02C
+#define TRCTSCTLR			0x030
+#define TRCSYNCPR			0x034
+#define TRCCCCTLR			0x038
+#define TRCBBCTLR			0x03C
+#define TRCTRACEIDR			0x040
+#define TRCQCTLR			0x044
+/* Filtering control registers */
+#define TRCVICTLR			0x080
+#define TRCVIIECTLR			0x084
+#define TRCVISSCTLR			0x088
+#define TRCVIPCSSCTLR			0x08C
+#define TRCVDCTLR			0x0A0
+#define TRCVDSACCTLR			0x0A4
+#define TRCVDARCCTLR			0x0A8
+/* Derived resources registers */
+#define TRCSEQEVRn(n)			(0x100 + (n * 4))
+#define TRCSEQRSTEVR			0x118
+#define TRCSEQSTR			0x11C
+#define TRCEXTINSELR			0x120
+#define TRCCNTRLDVRn(n)			(0x140 + (n * 4))
+#define TRCCNTCTLRn(n)			(0x150 + (n * 4))
+#define TRCCNTVRn(n)			(0x160 + (n * 4))
+/* ID registers */
+#define TRCIDR8				0x180
+#define TRCIDR9				0x184
+#define TRCIDR10			0x188
+#define TRCIDR11			0x18C
+#define TRCIDR12			0x190
+#define TRCIDR13			0x194
+#define TRCIMSPEC0			0x1C0
+#define TRCIMSPECn(n)			(0x1C0 + (n * 4))
+#define TRCIDR0				0x1E0
+#define TRCIDR1				0x1E4
+#define TRCIDR2				0x1E8
+#define TRCIDR3				0x1EC
+#define TRCIDR4				0x1F0
+#define TRCIDR5				0x1F4
+#define TRCIDR6				0x1F8
+#define TRCIDR7				0x1FC
+/* Resource selection registers */
+#define TRCRSCTLRn(n)			(0x200 + (n * 4))
+/* Single-shot comparator registers */
+#define TRCSSCCRn(n)			(0x280 + (n * 4))
+#define TRCSSCSRn(n)			(0x2A0 + (n * 4))
+#define TRCSSPCICRn(n)			(0x2C0 + (n * 4))
+/* Management registers (0x300-0x314) */
+#define TRCOSLAR			0x300
+#define TRCOSLSR			0x304
+#define TRCPDCR				0x310
+#define TRCPDSR				0x314
+/* Trace registers (0x318-0xEFC) */
+/* Comparator registers */
+#define TRCACVRn(n)			(0x400 + (n * 8))
+#define TRCACATRn(n)			(0x480 + (n * 8))
+#define TRCDVCVRn(n)			(0x500 + (n * 16))
+#define TRCDVCMRn(n)			(0x580 + (n * 16))
+#define TRCCIDCVRn(n)			(0x600 + (n * 8))
+#define TRCVMIDCVRn(n)			(0x640 + (n * 8))
+#define TRCCIDCCTLR0			0x680
+#define TRCCIDCCTLR1			0x684
+#define TRCVMIDCCTLR0			0x688
+#define TRCVMIDCCTLR1			0x68C
+/* Management register (0xF00) */
+/* Integration control registers */
+#define TRCITCTRL			0xF00
+/* Trace registers (0xFA0-0xFA4) */
+/* Claim tag registers */
+#define TRCCLAIMSET			0xFA0
+#define TRCCLAIMCLR			0xFA4
+/* Management registers (0xFA8-0xFFC) */
+#define TRCDEVAFF0			0xFA8
+#define TRCDEVAFF1			0xFAC
+#define TRCLAR				0xFB0
+#define TRCLSR				0xFB4
+#define TRCAUTHSTATUS			0xFB8
+#define TRCDEVARCH			0xFBC
+#define TRCDEVID			0xFC8
+#define TRCDEVTYPE			0xFCC
+#define TRCPIDR4			0xFD0
+#define TRCPIDR5			0xFD4
+#define TRCPIDR6			0xFD8
+#define TRCPIDR7			0xFDC
+#define TRCPIDR0			0xFE0
+#define TRCPIDR1			0xFE4
+#define TRCPIDR2			0xFE8
+#define TRCPIDR3			0xFEC
+#define TRCCIDR0			0xFF0
+#define TRCCIDR1			0xFF4
+#define TRCCIDR2			0xFF8
+#define TRCCIDR3			0xFFC
+
+/* ETMv4 resources */
+#define ETM_MAX_NR_PE			8
+#define ETMv4_MAX_CNTR			4
+#define ETM_MAX_SEQ_STATES		4
+#define ETM_MAX_EXT_INP_SEL		4
+#define ETM_MAX_EXT_INP			256
+#define ETM_MAX_EXT_OUT			4
+#define ETM_MAX_SINGLE_ADDR_CMP		16
+#define ETM_MAX_ADDR_RANGE_CMP		(ETM_MAX_SINGLE_ADDR_CMP / 2)
+#define ETM_MAX_DATA_VAL_CMP		8
+#define ETMv4_MAX_CTXID_CMP		8
+#define ETM_MAX_VMID_CMP		8
+#define ETM_MAX_PE_CMP			8
+#define ETM_MAX_RES_SEL			16
+#define ETM_MAX_SS_CMP			8
+
+#define ETM_ARCH_V4			0x40
+#define ETMv4_SYNC_MASK			0x1F
+#define ETM_CYC_THRESHOLD_MASK		0xFFF
+#define ETMv4_EVENT_MASK		0xFF
+#define ETM_CNTR_MAX_VAL		0xFFFF
+#define ETM_TRACEID_MASK		0x3f
+
+/* ETMv4 programming modes */
+#define ETM_MODE_EXCLUDE		BIT(0)
+#define ETM_MODE_LOAD			BIT(1)
+#define ETM_MODE_STORE			BIT(2)
+#define ETM_MODE_LOAD_STORE		BIT(3)
+#define ETM_MODE_BB			BIT(4)
+#define ETMv4_MODE_CYCACC		BIT(5)
+#define ETMv4_MODE_CTXID		BIT(6)
+#define ETM_MODE_VMID			BIT(7)
+#define ETM_MODE_COND(val)		BMVAL(val, 8, 10)
+#define ETMv4_MODE_TIMESTAMP		BIT(11)
+#define ETM_MODE_RETURNSTACK		BIT(12)
+#define ETM_MODE_QELEM(val)		BMVAL(val, 13, 14)
+#define ETM_MODE_DATA_TRACE_ADDR	BIT(15)
+#define ETM_MODE_DATA_TRACE_VAL		BIT(16)
+#define ETM_MODE_ISTALL			BIT(17)
+#define ETM_MODE_DSTALL			BIT(18)
+#define ETM_MODE_ATB_TRIGGER		BIT(19)
+#define ETM_MODE_LPOVERRIDE		BIT(20)
+#define ETM_MODE_ISTALL_EN		BIT(21)
+#define ETM_MODE_DSTALL_EN		BIT(22)
+#define ETM_MODE_INSTPRIO		BIT(23)
+#define ETM_MODE_NOOVERFLOW		BIT(24)
+#define ETM_MODE_TRACE_RESET		BIT(25)
+#define ETM_MODE_TRACE_ERR		BIT(26)
+#define ETM_MODE_VIEWINST_STARTSTOP	BIT(27)
+#define ETMv4_MODE_ALL			0xFFFFFFF
+
+#define TRCSTATR_IDLE_BIT		0
+
+/**
+ * struct etm4_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.
+ * @spinlock:   Only one at a time pls.
+ * @cpu:        The cpu this component is affined to.
+ * @arch:       ETM version number.
+ * @enable:	Is this ETM 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_pe:	The number of processing entity available for tracing.
+ * @nr_pe_cmp:	The number of processing entity comparator inputs that are
+ *		available for tracing.
+ * @nr_addr_cmp:Number of pairs of address comparators available
+ *		as found in ETMIDR4 0-3.
+ * @nr_cntr:    Number of counters as found in ETMIDR5 bit 28-30.
+ * @nr_ext_inp: Number of external input.
+ * @numcidc:	Number of contextID comparators.
+ * @numvmidc:	Number of VMID comparators.
+ * @nrseqstate: The number of sequencer states that are implemented.
+ * @nr_event:	Indicates how many events the trace unit support.
+ * @nr_resource:The number of resource selection pairs available for tracing.
+ * @nr_ss_cmp:	Number of single-shot comparator controls that are available.
+ * @mode:	Controls various modes supported by this ETM.
+ * @trcid:	value of the current ID for this component.
+ * @trcid_size: Indicates the trace ID width.
+ * @instrp0:	Tracing of load and store instructions
+ *		as P0 elements is supported.
+ * @trccond:	If the trace unit supports conditional
+ *		instruction tracing.
+ * @retstack:	Indicates if the implementation supports a return stack.
+ * @trc_error:	Whether a trace unit can trace a system
+ *		error exception.
+ * @atbtrig:	If the implementation can support ATB triggers
+ * @lpoverride:	If the implementation can support low-power state over.
+ * @pe_sel:	Controls which PE to trace.
+ * @cfg:	Controls the tracing options.
+ * @eventctrl0: Controls the tracing of arbitrary events.
+ * @eventctrl1: Controls the behavior of the events that @event_ctrl0 selects.
+ * @stallctl:	If functionality that prevents trace unit buffer overflows
+ *		is available.
+ * @sysstall:	Does the system support stall control of the PE?
+ * @nooverflow:	Indicate if overflow prevention is supported.
+ * @stall_ctrl:	Enables trace unit functionality that prevents trace
+ *		unit buffer overflows.
+ * @ts_size:	Global timestamp size field.
+ * @ts_ctrl:	Controls the insertion of global timestamps in the
+ *		trace streams.
+ * @syncpr:	Indicates if an implementation has a fixed
+ *		synchronization period.
+ * @syncfreq:	Controls how often trace synchronization requests occur.
+ * @trccci:	Indicates if the trace unit supports cycle counting
+ *		for instruction.
+ * @ccsize:	Indicates the size of the cycle counter in bits.
+ * @ccitmin:	minimum value that can be programmed in
+ *		the TRCCCCTLR register.
+ * @ccctlr:	Sets the threshold value for cycle counting.
+ * @trcbb:	Indicates if the trace unit supports branch broadcast tracing.
+ * @q_support:	Q element support characteristics.
+ * @vinst_ctrl:	Controls instruction trace filtering.
+ * @viiectlr:	Set or read, the address range comparators.
+ * @vissctlr:	Set, or read, the single address comparators that control the
+ *		ViewInst start-stop logic.
+ * @vipcssctlr:	Set, or read, which PE comparator inputs can control the
+ *		ViewInst start-stop logic.
+ * @seq_idx:	Sequencor index selector.
+ * @seq_ctrl:	Control for the sequencer state transition control register.
+ * @seq_rst:	Moves the sequencer to state 0 when a programmed event occurs.
+ * @seq_state:	Set, or read the sequencer state.
+ * @cntr_idx:	Counter index seletor.
+ * @cntrldvr:	Sets or returns the reload count value for a counter.
+ * @cntr_ctrl:	Controls the operation of a counter.
+ * @cntr_val:	Sets or returns the value for a counter.
+ * @res_idx:	Resource index selector.
+ * @res_ctrl:	Controls the selection of the resources in the trace unit.
+ * @ss_ctrl:	Controls the corresponding single-shot comparator resource.
+ * @ss_status:	The status of the corresponding single-shot comparator.
+ * @ss_pe_cmp:	Selects the PE comparator inputs for Single-shot control.
+ * @addr_idx:	Address comparator index selector.
+ * @addr_val:	Value for address comparator.
+ * @addr_acc:	Address comparator access type.
+ * @addr_type:	Current status of the comparator register.
+ * @ctxid_idx:	Context ID index selector.
+ * @ctxid_size:	Size of the context ID field to consider.
+ * @ctxid_val:	Value of the context ID comparator.
+ * @ctxid_mask0:Context ID comparator mask for comparator 0-3.
+ * @ctxid_mask1:Context ID comparator mask for comparator 4-7.
+ * @vmid_idx:	VM ID index selector.
+ * @vmid_size:	Size of the VM ID comparator to consider.
+ * @vmid_val:	Value of the VM ID comparator.
+ * @vmid_mask0:	VM ID comparator mask for comparator 0-3.
+ * @vmid_mask1:	VM ID comparator mask for comparator 4-7.
+ * @s_ex_level:	In secure state, indicates whether instruction tracing is
+ *		supported for the corresponding Exception level.
+ * @ns_ex_level:In non-secure state, indicates whether instruction tracing is
+ *		supported for the corresponding Exception level.
+ * @ext_inp:	External input selection.
+ */
+struct etmv4_drvdata {
+	void __iomem			*base;
+	struct device			*dev;
+	struct coresight_device		*csdev;
+	spinlock_t			spinlock;
+	int				cpu;
+	u8				arch;
+	bool				enable;
+	bool				sticky_enable;
+	bool				boot_enable;
+	bool				os_unlock;
+	u8				nr_pe;
+	u8				nr_pe_cmp;
+	u8				nr_addr_cmp;
+	u8				nr_cntr;
+	u8				nr_ext_inp;
+	u8				numcidc;
+	u8				numvmidc;
+	u8				nrseqstate;
+	u8				nr_event;
+	u8				nr_resource;
+	u8				nr_ss_cmp;
+	u32				mode;
+	u8				trcid;
+	u8				trcid_size;
+	bool				instrp0;
+	bool				trccond;
+	bool				retstack;
+	bool				trc_error;
+	bool				atbtrig;
+	bool				lpoverride;
+	u32				pe_sel;
+	u32				cfg;
+	u32				eventctrl0;
+	u32				eventctrl1;
+	bool				stallctl;
+	bool				sysstall;
+	bool				nooverflow;
+	u32				stall_ctrl;
+	u8				ts_size;
+	u32				ts_ctrl;
+	bool				syncpr;
+	u32				syncfreq;
+	bool				trccci;
+	u8				ccsize;
+	u8				ccitmin;
+	u32				ccctlr;
+	bool				trcbb;
+	u32				bb_ctrl;
+	bool				q_support;
+	u32				vinst_ctrl;
+	u32				viiectlr;
+	u32				vissctlr;
+	u32				vipcssctlr;
+	u8				seq_idx;
+	u32				seq_ctrl[ETM_MAX_SEQ_STATES];
+	u32				seq_rst;
+	u32				seq_state;
+	u8				cntr_idx;
+	u32				cntrldvr[ETMv4_MAX_CNTR];
+	u32				cntr_ctrl[ETMv4_MAX_CNTR];
+	u32				cntr_val[ETMv4_MAX_CNTR];
+	u8				res_idx;
+	u32				res_ctrl[ETM_MAX_RES_SEL];
+	u32				ss_ctrl[ETM_MAX_SS_CMP];
+	u32				ss_status[ETM_MAX_SS_CMP];
+	u32				ss_pe_cmp[ETM_MAX_SS_CMP];
+	u8				addr_idx;
+	u64				addr_val[ETM_MAX_SINGLE_ADDR_CMP];
+	u64				addr_acc[ETM_MAX_SINGLE_ADDR_CMP];
+	u8				addr_type[ETM_MAX_SINGLE_ADDR_CMP];
+	u8				ctxid_idx;
+	u8				ctxid_size;
+	u64				ctxid_val[ETMv4_MAX_CTXID_CMP];
+	u32				ctxid_mask0;
+	u32				ctxid_mask1;
+	u8				vmid_idx;
+	u8				vmid_size;
+	u64				vmid_val[ETM_MAX_VMID_CMP];
+	u32				vmid_mask0;
+	u32				vmid_mask1;
+	u8				s_ex_level;
+	u8				ns_ex_level;
+	u32				ext_inp;
+};
+
+/* Address comparator access types */
+enum etm_addr_acctype {
+	ETM_INSTR_ADDR,
+	ETM_DATA_LOAD_ADDR,
+	ETM_DATA_STORE_ADDR,
+	ETM_DATA_LOAD_STORE_ADDR,
+};
+
+/* Address comparator context types */
+enum etm_addr_ctxtype {
+	ETM_CTX_NONE,
+	ETM_CTX_CTXID,
+	ETM_CTX_VMID,
+	ETM_CTX_CTXID_VMID,
+};
+
+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/hwtracing/coresight/coresight-funnel.c b/drivers/hwtracing/coresight/coresight-funnel.c
index 3db36f70b666..2e36bde7fcb4 100644
--- a/drivers/hwtracing/coresight/coresight-funnel.c
+++ b/drivers/hwtracing/coresight/coresight-funnel.c
@@ -18,9 +18,10 @@
 #include <linux/err.h>
 #include <linux/fs.h>
 #include <linux/slab.h>
-#include <linux/clk.h>
+#include <linux/pm_runtime.h>
 #include <linux/coresight.h>
 #include <linux/amba/bus.h>
+#include <linux/clk.h>
 
 #include "coresight-priv.h"
 
@@ -35,15 +36,15 @@
  * 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.
+ * @atclk:	optional clock for the core parts of the funnel.
  * @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 clk		*atclk;
 	struct coresight_device	*csdev;
-	struct clk		*clk;
 	unsigned long		priority;
 };
 
@@ -67,12 +68,8 @@ 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;
 
+	pm_runtime_get_sync(drvdata->dev);
 	funnel_enable_hw(drvdata, inport);
 
 	dev_info(drvdata->dev, "FUNNEL inport %d enabled\n", inport);
@@ -98,8 +95,7 @@ static void funnel_disable(struct coresight_device *csdev, int inport,
 	struct funnel_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
 
 	funnel_disable_hw(drvdata, inport);
-
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 
 	dev_info(drvdata->dev, "FUNNEL inport %d disabled\n", inport);
 }
@@ -153,16 +149,14 @@ static u32 get_funnel_ctrl_hw(struct funnel_drvdata *drvdata)
 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;
+	pm_runtime_get_sync(drvdata->dev);
 
 	val = get_funnel_ctrl_hw(drvdata);
-	clk_disable_unprepare(drvdata->clk);
+
+	pm_runtime_put(drvdata->dev);
 
 	return sprintf(buf, "%#x\n", val);
 }
@@ -177,6 +171,7 @@ ATTRIBUTE_GROUPS(coresight_funnel);
 
 static int funnel_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;
@@ -197,6 +192,12 @@ static int funnel_probe(struct amba_device *adev, const struct amba_id *id)
 		return -ENOMEM;
 
 	drvdata->dev = &adev->dev;
+	drvdata->atclk = devm_clk_get(&adev->dev, "atclk"); /* optional */
+	if (!IS_ERR(drvdata->atclk)) {
+		ret = clk_prepare_enable(drvdata->atclk);
+		if (ret)
+			return ret;
+	}
 	dev_set_drvdata(dev, drvdata);
 
 	/* Validity for the resource is already checked by the AMBA core */
@@ -205,8 +206,7 @@ static int funnel_probe(struct amba_device *adev, const struct amba_id *id)
 		return PTR_ERR(base);
 
 	drvdata->base = base;
-
-	drvdata->clk = adev->pclk;
+	pm_runtime_put(&adev->dev);
 
 	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
 	if (!desc)
@@ -234,6 +234,32 @@ static int funnel_remove(struct amba_device *adev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int funnel_runtime_suspend(struct device *dev)
+{
+	struct funnel_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_disable_unprepare(drvdata->atclk);
+
+	return 0;
+}
+
+static int funnel_runtime_resume(struct device *dev)
+{
+	struct funnel_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_prepare_enable(drvdata->atclk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops funnel_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(funnel_runtime_suspend, funnel_runtime_resume, NULL)
+};
+
 static struct amba_id funnel_ids[] = {
 	{
 		.id     = 0x0003b908,
@@ -246,6 +272,7 @@ static struct amba_driver funnel_driver = {
 	.drv = {
 		.name	= "coresight-funnel",
 		.owner	= THIS_MODULE,
+		.pm	= &funnel_dev_pm_ops,
 	},
 	.probe		= funnel_probe,
 	.remove		= funnel_remove,
diff --git a/drivers/hwtracing/coresight/coresight-replicator-qcom.c b/drivers/hwtracing/coresight/coresight-replicator-qcom.c
new file mode 100644
index 000000000000..584059e9e866
--- /dev/null
+++ b/drivers/hwtracing/coresight/coresight-replicator-qcom.c
@@ -0,0 +1,215 @@
+/*
+ * Copyright (c) 2011-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.
+ */
+
+#include <linux/amba/bus.h>
+#include <linux/clk.h>
+#include <linux/coresight.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include "coresight-priv.h"
+
+#define REPLICATOR_IDFILTER0		0x000
+#define REPLICATOR_IDFILTER1		0x004
+
+/**
+ * struct replicator_state - specifics associated to a replicator component
+ * @base:	memory mapped base address for this component.
+ * @dev:	the device entity associated with this component
+ * @atclk:	optional clock for the core parts of the replicator.
+ * @csdev:	component vitals needed by the framework
+ */
+struct replicator_state {
+	void __iomem		*base;
+	struct device		*dev;
+	struct clk		*atclk;
+	struct coresight_device	*csdev;
+};
+
+static int replicator_enable(struct coresight_device *csdev, int inport,
+			      int outport)
+{
+	struct replicator_state *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	pm_runtime_get_sync(drvdata->dev);
+
+	CS_UNLOCK(drvdata->base);
+
+	/*
+	 * Ensure that the other port is disabled
+	 * 0x00 - passing through the replicator unimpeded
+	 * 0xff - disable (or impede) the flow of ATB data
+	 */
+	if (outport == 0) {
+		writel_relaxed(0x00, drvdata->base + REPLICATOR_IDFILTER0);
+		writel_relaxed(0xff, drvdata->base + REPLICATOR_IDFILTER1);
+	} else {
+		writel_relaxed(0x00, drvdata->base + REPLICATOR_IDFILTER1);
+		writel_relaxed(0xff, drvdata->base + REPLICATOR_IDFILTER0);
+	}
+
+	CS_LOCK(drvdata->base);
+
+	dev_info(drvdata->dev, "REPLICATOR enabled\n");
+	return 0;
+}
+
+static void replicator_disable(struct coresight_device *csdev, int inport,
+				int outport)
+{
+	struct replicator_state *drvdata = dev_get_drvdata(csdev->dev.parent);
+
+	CS_UNLOCK(drvdata->base);
+
+	/* disable the flow of ATB data through port */
+	if (outport == 0)
+		writel_relaxed(0xff, drvdata->base + REPLICATOR_IDFILTER0);
+	else
+		writel_relaxed(0xff, drvdata->base + REPLICATOR_IDFILTER1);
+
+	CS_LOCK(drvdata->base);
+
+	pm_runtime_put(drvdata->dev);
+
+	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 amba_device *adev, const struct amba_id *id)
+{
+	int ret;
+	struct device *dev = &adev->dev;
+	struct resource *res = &adev->res;
+	struct coresight_platform_data *pdata = NULL;
+	struct replicator_state *drvdata;
+	struct coresight_desc *desc;
+	struct device_node *np = adev->dev.of_node;
+	void __iomem *base;
+
+	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;
+	drvdata->atclk = devm_clk_get(&adev->dev, "atclk"); /* optional */
+	if (!IS_ERR(drvdata->atclk)) {
+		ret = clk_prepare_enable(drvdata->atclk);
+		if (ret)
+			return ret;
+	}
+
+	/* 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;
+	dev_set_drvdata(dev, drvdata);
+	pm_runtime_put(&adev->dev);
+
+	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_SPLIT;
+	desc->ops = &replicator_cs_ops;
+	desc->pdata = adev->dev.platform_data;
+	desc->dev = &adev->dev;
+	drvdata->csdev = coresight_register(desc);
+	if (IS_ERR(drvdata->csdev))
+		return PTR_ERR(drvdata->csdev);
+
+	dev_info(dev, "%s initialized\n", (char *)id->data);
+	return 0;
+}
+
+static int replicator_remove(struct amba_device *adev)
+{
+	struct replicator_state *drvdata = amba_get_drvdata(adev);
+
+	pm_runtime_disable(&adev->dev);
+	coresight_unregister(drvdata->csdev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int replicator_runtime_suspend(struct device *dev)
+{
+	struct replicator_state *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_disable_unprepare(drvdata->atclk);
+
+	return 0;
+}
+
+static int replicator_runtime_resume(struct device *dev)
+{
+	struct replicator_state *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_prepare_enable(drvdata->atclk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops replicator_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(replicator_runtime_suspend,
+			   replicator_runtime_resume,
+			   NULL)
+};
+
+static struct amba_id replicator_ids[] = {
+	{
+		.id     = 0x0003b909,
+		.mask   = 0x0003ffff,
+		.data	= "REPLICATOR 1.0",
+	},
+	{ 0, 0 },
+};
+
+static struct amba_driver replicator_driver = {
+	.drv = {
+		.name	= "coresight-replicator-qcom",
+		.pm	= &replicator_dev_pm_ops,
+	},
+	.probe		= replicator_probe,
+	.remove		= replicator_remove,
+	.id_table	= replicator_ids,
+};
+
+module_amba_driver(replicator_driver);
diff --git a/drivers/hwtracing/coresight/coresight-replicator.c b/drivers/hwtracing/coresight/coresight-replicator.c
index 75b9abd804e6..7974b7c3da6b 100644
--- a/drivers/hwtracing/coresight/coresight-replicator.c
+++ b/drivers/hwtracing/coresight/coresight-replicator.c
@@ -18,6 +18,7 @@
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/slab.h>
+#include <linux/pm_runtime.h>
 #include <linux/clk.h>
 #include <linux/of.h>
 #include <linux/coresight.h>
@@ -27,10 +28,12 @@
 /**
  * struct replicator_drvdata - specifics associated to a replicator component
  * @dev:	the device entity associated with this component
+ * @atclk:	optional clock for the core parts of the replicator.
  * @csdev:	component vitals needed by the framework
  */
 struct replicator_drvdata {
 	struct device		*dev;
+	struct clk		*atclk;
 	struct coresight_device	*csdev;
 };
 
@@ -39,6 +42,7 @@ static int replicator_enable(struct coresight_device *csdev, int inport,
 {
 	struct replicator_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
 
+	pm_runtime_get_sync(drvdata->dev);
 	dev_info(drvdata->dev, "REPLICATOR enabled\n");
 	return 0;
 }
@@ -48,6 +52,7 @@ static void replicator_disable(struct coresight_device *csdev, int inport,
 {
 	struct replicator_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
 
+	pm_runtime_put(drvdata->dev);
 	dev_info(drvdata->dev, "REPLICATOR disabled\n");
 }
 
@@ -62,6 +67,7 @@ static const struct coresight_ops replicator_cs_ops = {
 
 static int replicator_probe(struct platform_device *pdev)
 {
+	int ret;
 	struct device *dev = &pdev->dev;
 	struct coresight_platform_data *pdata = NULL;
 	struct replicator_drvdata *drvdata;
@@ -80,11 +86,22 @@ static int replicator_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	drvdata->dev = &pdev->dev;
+	drvdata->atclk = devm_clk_get(&pdev->dev, "atclk"); /* optional */
+	if (!IS_ERR(drvdata->atclk)) {
+		ret = clk_prepare_enable(drvdata->atclk);
+		if (ret)
+			return ret;
+	}
+	pm_runtime_get_noresume(&pdev->dev);
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
 	platform_set_drvdata(pdev, drvdata);
 
 	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
-	if (!desc)
-		return -ENOMEM;
+	if (!desc) {
+		ret = -ENOMEM;
+		goto out_disable_pm;
+	}
 
 	desc->type = CORESIGHT_DEV_TYPE_LINK;
 	desc->subtype.link_subtype = CORESIGHT_DEV_SUBTYPE_LINK_SPLIT;
@@ -92,11 +109,23 @@ static int replicator_probe(struct platform_device *pdev)
 	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);
+	if (IS_ERR(drvdata->csdev)) {
+		ret = PTR_ERR(drvdata->csdev);
+		goto out_disable_pm;
+	}
+
+	pm_runtime_put(&pdev->dev);
 
 	dev_info(dev, "REPLICATOR initialized\n");
 	return 0;
+
+out_disable_pm:
+	if (!IS_ERR(drvdata->atclk))
+		clk_disable_unprepare(drvdata->atclk);
+	pm_runtime_put_noidle(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return ret;
 }
 
 static int replicator_remove(struct platform_device *pdev)
@@ -104,9 +133,42 @@ static int replicator_remove(struct platform_device *pdev)
 	struct replicator_drvdata *drvdata = platform_get_drvdata(pdev);
 
 	coresight_unregister(drvdata->csdev);
+	pm_runtime_get_sync(&pdev->dev);
+	if (!IS_ERR(drvdata->atclk))
+		clk_disable_unprepare(drvdata->atclk);
+	pm_runtime_put_noidle(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int replicator_runtime_suspend(struct device *dev)
+{
+	struct replicator_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_disable_unprepare(drvdata->atclk);
+
 	return 0;
 }
 
+static int replicator_runtime_resume(struct device *dev)
+{
+	struct replicator_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_prepare_enable(drvdata->atclk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops replicator_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(replicator_runtime_suspend,
+			   replicator_runtime_resume, NULL)
+};
+
 static const struct of_device_id replicator_match[] = {
 	{.compatible = "arm,coresight-replicator"},
 	{}
@@ -118,6 +180,7 @@ static struct platform_driver replicator_driver = {
 	.driver         = {
 		.name   = "coresight-replicator",
 		.of_match_table = replicator_match,
+		.pm	= &replicator_dev_pm_ops,
 	},
 };
 
diff --git a/drivers/hwtracing/coresight/coresight-tmc.c b/drivers/hwtracing/coresight/coresight-tmc.c
index 7147f3dd363c..a57c7ec1661f 100644
--- a/drivers/hwtracing/coresight/coresight-tmc.c
+++ b/drivers/hwtracing/coresight/coresight-tmc.c
@@ -23,7 +23,7 @@
 #include <linux/slab.h>
 #include <linux/dma-mapping.h>
 #include <linux/spinlock.h>
-#include <linux/clk.h>
+#include <linux/pm_runtime.h>
 #include <linux/of.h>
 #include <linux/coresight.h>
 #include <linux/amba/bus.h>
@@ -104,7 +104,6 @@ enum tmc_mem_intf_width {
  * @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.
@@ -120,7 +119,6 @@ struct tmc_drvdata {
 	struct device		*dev;
 	struct coresight_device	*csdev;
 	struct miscdevice	miscdev;
-	struct clk		*clk;
 	spinlock_t		spinlock;
 	int			read_count;
 	bool			reading;
@@ -242,17 +240,14 @@ static void tmc_etf_enable_hw(struct tmc_drvdata *drvdata)
 
 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;
+	pm_runtime_get_sync(drvdata->dev);
 
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 	if (drvdata->reading) {
 		spin_unlock_irqrestore(&drvdata->spinlock, flags);
-		clk_disable_unprepare(drvdata->clk);
+		pm_runtime_put(drvdata->dev);
 		return -EBUSY;
 	}
 
@@ -386,7 +381,7 @@ out:
 	drvdata->enable = false;
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
 
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 
 	dev_info(drvdata->dev, "TMC disabled\n");
 }
@@ -568,17 +563,13 @@ static const struct file_operations tmc_fops = {
 static ssize_t status_show(struct device *dev,
 			   struct device_attribute *attr, char *buf)
 {
-	int ret;
 	unsigned long flags;
 	u32 tmc_rsz, tmc_sts, tmc_rrp, tmc_rwp, tmc_trg;
 	u32 tmc_ctl, tmc_ffsr, tmc_ffcr, tmc_mode, tmc_pscr;
 	u32 devid;
 	struct tmc_drvdata *drvdata = dev_get_drvdata(dev->parent);
 
-	ret = clk_prepare_enable(drvdata->clk);
-	if (ret)
-		goto out;
-
+	pm_runtime_get_sync(drvdata->dev);
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 	CS_UNLOCK(drvdata->base);
 
@@ -596,8 +587,7 @@ static ssize_t status_show(struct device *dev,
 
 	CS_LOCK(drvdata->base);
 	spin_unlock_irqrestore(&drvdata->spinlock, flags);
-
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(drvdata->dev);
 
 	return sprintf(buf,
 		       "Depth:\t\t0x%x\n"
@@ -613,7 +603,7 @@ static ssize_t status_show(struct device *dev,
 		       "DEVID:\t\t0x%x\n",
 			tmc_rsz, tmc_sts, tmc_rrp, tmc_rwp, tmc_trg,
 			tmc_ctl, tmc_ffsr, tmc_ffcr, tmc_mode, tmc_pscr, devid);
-out:
+
 	return -EINVAL;
 }
 static DEVICE_ATTR_RO(status);
@@ -700,11 +690,6 @@ static int tmc_probe(struct amba_device *adev, const struct amba_id *id)
 
 	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);
 
@@ -719,7 +704,7 @@ static int tmc_probe(struct amba_device *adev, const struct amba_id *id)
 		drvdata->size = readl_relaxed(drvdata->base + TMC_RSZ) * 4;
 	}
 
-	clk_disable_unprepare(drvdata->clk);
+	pm_runtime_put(&adev->dev);
 
 	if (drvdata->config_type == TMC_CONFIG_TYPE_ETR) {
 		drvdata->vaddr = dma_alloc_coherent(dev, drvdata->size,
diff --git a/drivers/hwtracing/coresight/coresight-tpiu.c b/drivers/hwtracing/coresight/coresight-tpiu.c
index 3b33af2416bb..7214efd10db5 100644
--- a/drivers/hwtracing/coresight/coresight-tpiu.c
+++ b/drivers/hwtracing/coresight/coresight-tpiu.c
@@ -17,9 +17,10 @@
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/slab.h>
-#include <linux/clk.h>
+#include <linux/pm_runtime.h>
 #include <linux/coresight.h>
 #include <linux/amba/bus.h>
+#include <linux/clk.h>
 
 #include "coresight-priv.h"
 
@@ -50,14 +51,14 @@
 /**
  * @base:	memory mapped base address for this component.
  * @dev:	the device entity associated to this component.
+ * @atclk:	optional clock for the core parts of the TPIU.
  * @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 clk		*atclk;
 	struct coresight_device	*csdev;
-	struct clk		*clk;
 };
 
 static void tpiu_enable_hw(struct tpiu_drvdata *drvdata)
@@ -72,12 +73,8 @@ static void tpiu_enable_hw(struct tpiu_drvdata *drvdata)
 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;
 
+	pm_runtime_get_sync(csdev->dev.parent);
 	tpiu_enable_hw(drvdata);
 
 	dev_info(drvdata->dev, "TPIU enabled\n");
@@ -101,8 +98,7 @@ 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);
+	pm_runtime_put(csdev->dev.parent);
 
 	dev_info(drvdata->dev, "TPIU disabled\n");
 }
@@ -139,6 +135,12 @@ static int tpiu_probe(struct amba_device *adev, const struct amba_id *id)
 		return -ENOMEM;
 
 	drvdata->dev = &adev->dev;
+	drvdata->atclk = devm_clk_get(&adev->dev, "atclk"); /* optional */
+	if (!IS_ERR(drvdata->atclk)) {
+		ret = clk_prepare_enable(drvdata->atclk);
+		if (ret)
+			return ret;
+	}
 	dev_set_drvdata(dev, drvdata);
 
 	/* Validity for the resource is already checked by the AMBA core */
@@ -148,15 +150,10 @@ static int tpiu_probe(struct amba_device *adev, const struct amba_id *id)
 
 	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);
+	pm_runtime_put(&adev->dev);
 
 	desc = devm_kzalloc(dev, sizeof(*desc), GFP_KERNEL);
 	if (!desc)
@@ -183,11 +180,41 @@ static int tpiu_remove(struct amba_device *adev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int tpiu_runtime_suspend(struct device *dev)
+{
+	struct tpiu_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_disable_unprepare(drvdata->atclk);
+
+	return 0;
+}
+
+static int tpiu_runtime_resume(struct device *dev)
+{
+	struct tpiu_drvdata *drvdata = dev_get_drvdata(dev);
+
+	if (drvdata && !IS_ERR(drvdata->atclk))
+		clk_prepare_enable(drvdata->atclk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops tpiu_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(tpiu_runtime_suspend, tpiu_runtime_resume, NULL)
+};
+
 static struct amba_id tpiu_ids[] = {
 	{
 		.id	= 0x0003b912,
 		.mask	= 0x0003ffff,
 	},
+	{
+		.id	= 0x0004b912,
+		.mask	= 0x0007ffff,
+	},
 	{ 0, 0},
 };
 
@@ -195,6 +222,7 @@ static struct amba_driver tpiu_driver = {
 	.drv = {
 		.name	= "coresight-tpiu",
 		.owner	= THIS_MODULE,
+		.pm	= &tpiu_dev_pm_ops,
 	},
 	.probe		= tpiu_probe,
 	.remove		= tpiu_remove,
diff --git a/drivers/hwtracing/coresight/of_coresight.c b/drivers/hwtracing/coresight/of_coresight.c
index 35e51ce93a5c..b0973617826f 100644
--- a/drivers/hwtracing/coresight/of_coresight.c
+++ b/drivers/hwtracing/coresight/of_coresight.c
@@ -37,7 +37,7 @@ 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
+	 * If we have a non-configurable replicator, it will be found on the
 	 * platform bus.
 	 */
 	dev = bus_find_device(&platform_bus_type, NULL,
diff --git a/drivers/i2c/algos/i2c-algo-pca.c b/drivers/i2c/algos/i2c-algo-pca.c
index 580dbf05c148..e370804ec8bc 100644
--- a/drivers/i2c/algos/i2c-algo-pca.c
+++ b/drivers/i2c/algos/i2c-algo-pca.c
@@ -521,7 +521,7 @@ static int pca_init(struct i2c_adapter *adap)
 
 		pca_set_con(pca_data, I2C_PCA_CON_ENSIO);
 	}
-	udelay(500); /* 500 us for oscilator to stabilise */
+	udelay(500); /* 500 us for oscillator to stabilise */
 
 	return 0;
 }
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 2255af23b9c7..577d58d1f1a1 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -392,6 +392,16 @@ config I2C_BCM_KONA
 
 	  If you do not need KONA I2C interface, say N.
 
+config I2C_BRCMSTB
+	tristate "BRCM Settop I2C controller"
+	depends on ARCH_BRCMSTB || COMPILE_TEST
+	default y
+	help
+	  If you say yes to this option, support will be included for the
+	  I2C interface on the Broadcom Settop SoCs.
+
+	  If you do not need I2C interface, say N.
+
 config I2C_BLACKFIN_TWI
 	tristate "Blackfin TWI I2C support"
 	depends on BLACKFIN
@@ -419,7 +429,7 @@ config I2C_CADENCE
 
 config I2C_CBUS_GPIO
 	tristate "CBUS I2C driver"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Support for CBUS access using I2C API. Mostly relevant for Nokia
 	  Internet Tablets (770, N800 and N810).
@@ -525,7 +535,7 @@ config I2C_EXYNOS5
 
 config I2C_GPIO
 	tristate "GPIO-based bitbanging I2C"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	select I2C_ALGOBIT
 	help
 	  This is a very simple bitbanging I2C driver utilizing the
@@ -620,6 +630,16 @@ config I2C_MPC
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-mpc.
 
+config I2C_MT65XX
+	tristate "MediaTek I2C adapter"
+	depends on ARCH_MEDIATEK || COMPILE_TEST
+	depends on HAS_DMA
+	help
+	  This selects the MediaTek(R) Integrated Inter Circuit bus driver
+	  for MT65xx and MT81xx.
+	  If you want to use MediaTek(R) I2C interface, say Y or M here.
+	  If unsure, say N.
+
 config I2C_MV64XXX
 	tristate "Marvell mv64xxx I2C Controller"
 	depends on MV64X60 || PLAT_ORION || ARCH_SUNXI
@@ -1103,13 +1123,22 @@ config I2C_SIBYTE
 
 config I2C_CROS_EC_TUNNEL
 	tristate "ChromeOS EC tunnel I2C bus"
-	depends on MFD_CROS_EC
+	depends on CROS_EC_PROTO
 	help
 	  If you say yes here you get an I2C bus that will tunnel i2c commands
 	  through to the other side of the ChromeOS EC to the i2c bus
 	  connected there. This will work whatever the interface used to
 	  talk to the EC (SPI, I2C or LPC).
 
+config I2C_XGENE_SLIMPRO
+	tristate "APM X-Gene SoC I2C SLIMpro devices support"
+	depends on ARCH_XGENE && MAILBOX
+	help
+	  Enable I2C bus access using the APM X-Gene SoC SLIMpro
+	  co-processor. The I2C device access the I2C bus via the X-Gene
+	  to SLIMpro (On chip coprocessor) mailbox mechanism.
+	  If unsure, say N.
+
 config SCx200_ACB
 	tristate "Geode ACCESS.bus support"
 	depends on X86_32 && PCI
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index cdf941da91c6..e5f537c80da0 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -60,6 +60,7 @@ obj-$(CONFIG_I2C_JZ4780)	+= i2c-jz4780.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_MT65XX)	+= i2c-mt65xx.o
 obj-$(CONFIG_I2C_MV64XXX)	+= i2c-mv64xxx.o
 obj-$(CONFIG_I2C_MXS)		+= i2c-mxs.o
 obj-$(CONFIG_I2C_NOMADIK)	+= i2c-nomadik.o
@@ -105,11 +106,13 @@ obj-$(CONFIG_I2C_VIPERBOARD)	+= i2c-viperboard.o
 # Other I2C/SMBus bus drivers
 obj-$(CONFIG_I2C_ACORN)		+= i2c-acorn.o
 obj-$(CONFIG_I2C_BCM_KONA)	+= i2c-bcm-kona.o
+obj-$(CONFIG_I2C_BRCMSTB)	+= i2c-brcmstb.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_I2C_XGENE_SLIMPRO) += i2c-xgene-slimpro.o
 obj-$(CONFIG_SCx200_ACB)	+= scx200_acb.o
 
 ccflags-$(CONFIG_I2C_DEBUG_BUS) := -DDEBUG
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index ff23d1bdd230..1c758cd1e1ba 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -41,29 +41,41 @@
 
 /* AT91 TWI register definitions */
 #define	AT91_TWI_CR		0x0000	/* Control Register */
-#define	AT91_TWI_START		0x0001	/* Send a Start Condition */
-#define	AT91_TWI_STOP		0x0002	/* Send a Stop Condition */
-#define	AT91_TWI_MSEN		0x0004	/* Master Transfer Enable */
-#define	AT91_TWI_SVDIS		0x0020	/* Slave Transfer Disable */
-#define	AT91_TWI_QUICK		0x0040	/* SMBus quick command */
-#define	AT91_TWI_SWRST		0x0080	/* Software Reset */
+#define	AT91_TWI_START		BIT(0)	/* Send a Start Condition */
+#define	AT91_TWI_STOP		BIT(1)	/* Send a Stop Condition */
+#define	AT91_TWI_MSEN		BIT(2)	/* Master Transfer Enable */
+#define	AT91_TWI_MSDIS		BIT(3)	/* Master Transfer Disable */
+#define	AT91_TWI_SVEN		BIT(4)	/* Slave Transfer Enable */
+#define	AT91_TWI_SVDIS		BIT(5)	/* Slave Transfer Disable */
+#define	AT91_TWI_QUICK		BIT(6)	/* SMBus quick command */
+#define	AT91_TWI_SWRST		BIT(7)	/* Software Reset */
+#define	AT91_TWI_ACMEN		BIT(16) /* Alternative Command Mode Enable */
+#define	AT91_TWI_ACMDIS		BIT(17) /* Alternative Command Mode Disable */
+#define	AT91_TWI_THRCLR		BIT(24) /* Transmit Holding Register Clear */
+#define	AT91_TWI_RHRCLR		BIT(25) /* Receive Holding Register Clear */
+#define	AT91_TWI_LOCKCLR	BIT(26) /* Lock Clear */
+#define	AT91_TWI_FIFOEN		BIT(28) /* FIFO Enable */
+#define	AT91_TWI_FIFODIS	BIT(29) /* FIFO Disable */
 
 #define	AT91_TWI_MMR		0x0004	/* Master Mode Register */
 #define	AT91_TWI_IADRSZ_1	0x0100	/* Internal Device Address Size */
-#define	AT91_TWI_MREAD		0x1000	/* Master Read Direction */
+#define	AT91_TWI_MREAD		BIT(12)	/* Master Read Direction */
 
 #define	AT91_TWI_IADR		0x000c	/* Internal Address Register */
 
 #define	AT91_TWI_CWGR		0x0010	/* Clock Waveform Generator Reg */
 
 #define	AT91_TWI_SR		0x0020	/* Status Register */
-#define	AT91_TWI_TXCOMP		0x0001	/* Transmission Complete */
-#define	AT91_TWI_RXRDY		0x0002	/* Receive Holding Register Ready */
-#define	AT91_TWI_TXRDY		0x0004	/* Transmit Holding Register Ready */
+#define	AT91_TWI_TXCOMP		BIT(0)	/* Transmission Complete */
+#define	AT91_TWI_RXRDY		BIT(1)	/* Receive Holding Register Ready */
+#define	AT91_TWI_TXRDY		BIT(2)	/* Transmit Holding Register Ready */
+#define	AT91_TWI_OVRE		BIT(6)	/* Overrun Error */
+#define	AT91_TWI_UNRE		BIT(7)	/* Underrun Error */
+#define	AT91_TWI_NACK		BIT(8)	/* Not Acknowledged */
+#define	AT91_TWI_LOCK		BIT(23) /* TWI Lock due to Frame Errors */
 
-#define	AT91_TWI_OVRE		0x0040	/* Overrun Error */
-#define	AT91_TWI_UNRE		0x0080	/* Underrun Error */
-#define	AT91_TWI_NACK		0x0100	/* Not Acknowledged */
+#define	AT91_TWI_INT_MASK \
+	(AT91_TWI_TXCOMP | AT91_TWI_RXRDY | AT91_TWI_TXRDY | AT91_TWI_NACK)
 
 #define	AT91_TWI_IER		0x0024	/* Interrupt Enable Register */
 #define	AT91_TWI_IDR		0x0028	/* Interrupt Disable Register */
@@ -71,17 +83,40 @@
 #define	AT91_TWI_RHR		0x0030	/* Receive Holding Register */
 #define	AT91_TWI_THR		0x0034	/* Transmit Holding Register */
 
+#define	AT91_TWI_ACR		0x0040	/* Alternative Command Register */
+#define	AT91_TWI_ACR_DATAL(len)	((len) & 0xff)
+#define	AT91_TWI_ACR_DIR	BIT(8)
+
+#define	AT91_TWI_FMR		0x0050	/* FIFO Mode Register */
+#define	AT91_TWI_FMR_TXRDYM(mode)	(((mode) & 0x3) << 0)
+#define	AT91_TWI_FMR_TXRDYM_MASK	(0x3 << 0)
+#define	AT91_TWI_FMR_RXRDYM(mode)	(((mode) & 0x3) << 4)
+#define	AT91_TWI_FMR_RXRDYM_MASK	(0x3 << 4)
+#define	AT91_TWI_ONE_DATA	0x0
+#define	AT91_TWI_TWO_DATA	0x1
+#define	AT91_TWI_FOUR_DATA	0x2
+
+#define	AT91_TWI_FLR		0x0054	/* FIFO Level Register */
+
+#define	AT91_TWI_FSR		0x0060	/* FIFO Status Register */
+#define	AT91_TWI_FIER		0x0064	/* FIFO Interrupt Enable Register */
+#define	AT91_TWI_FIDR		0x0068	/* FIFO Interrupt Disable Register */
+#define	AT91_TWI_FIMR		0x006c	/* FIFO Interrupt Mask Register */
+
+#define	AT91_TWI_VER		0x00fc	/* Version Register */
+
 struct at91_twi_pdata {
 	unsigned clk_max_div;
 	unsigned clk_offset;
 	bool has_unre_flag;
+	bool has_alt_cmd;
 	struct at_dma_slave dma_slave;
 };
 
 struct at91_twi_dma {
 	struct dma_chan *chan_rx;
 	struct dma_chan *chan_tx;
-	struct scatterlist sg;
+	struct scatterlist sg[2];
 	struct dma_async_tx_descriptor *data_desc;
 	enum dma_data_direction direction;
 	bool buf_mapped;
@@ -104,6 +139,7 @@ struct at91_twi_dev {
 	struct at91_twi_pdata *pdata;
 	bool use_dma;
 	bool recv_len_abort;
+	u32 fifo_size;
 	struct at91_twi_dma dma;
 };
 
@@ -119,13 +155,12 @@ static void at91_twi_write(struct at91_twi_dev *dev, unsigned reg, unsigned val)
 
 static void at91_disable_twi_interrupts(struct at91_twi_dev *dev)
 {
-	at91_twi_write(dev, AT91_TWI_IDR,
-		       AT91_TWI_TXCOMP | AT91_TWI_RXRDY | AT91_TWI_TXRDY);
+	at91_twi_write(dev, AT91_TWI_IDR, AT91_TWI_INT_MASK);
 }
 
 static void at91_twi_irq_save(struct at91_twi_dev *dev)
 {
-	dev->imr = at91_twi_read(dev, AT91_TWI_IMR) & 0x7;
+	dev->imr = at91_twi_read(dev, AT91_TWI_IMR) & AT91_TWI_INT_MASK;
 	at91_disable_twi_interrupts(dev);
 }
 
@@ -138,6 +173,9 @@ static void at91_init_twi_bus(struct at91_twi_dev *dev)
 {
 	at91_disable_twi_interrupts(dev);
 	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_SWRST);
+	/* FIFO should be enabled immediately after the software reset */
+	if (dev->fifo_size)
+		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_FIFOEN);
 	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_MSEN);
 	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_SVDIS);
 	at91_twi_write(dev, AT91_TWI_CWGR, dev->twi_cwgr_reg);
@@ -184,7 +222,7 @@ static void at91_twi_dma_cleanup(struct at91_twi_dev *dev)
 		dma->xfer_in_progress = false;
 	}
 	if (dma->buf_mapped) {
-		dma_unmap_single(dev->dev, sg_dma_address(&dma->sg),
+		dma_unmap_single(dev->dev, sg_dma_address(&dma->sg[0]),
 				 dev->buf_len, dma->direction);
 		dma->buf_mapped = false;
 	}
@@ -194,14 +232,16 @@ static void at91_twi_dma_cleanup(struct at91_twi_dev *dev)
 
 static void at91_twi_write_next_byte(struct at91_twi_dev *dev)
 {
-	if (dev->buf_len <= 0)
+	if (!dev->buf_len)
 		return;
 
-	at91_twi_write(dev, AT91_TWI_THR, *dev->buf);
+	/* 8bit write works with and without FIFO */
+	writeb_relaxed(*dev->buf, dev->base + AT91_TWI_THR);
 
 	/* send stop when last byte has been written */
 	if (--dev->buf_len == 0)
-		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
+		if (!dev->pdata->has_alt_cmd)
+			at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
 
 	dev_dbg(dev->dev, "wrote 0x%x, to go %d\n", *dev->buf, dev->buf_len);
 
@@ -212,10 +252,19 @@ static void at91_twi_write_data_dma_callback(void *data)
 {
 	struct at91_twi_dev *dev = (struct at91_twi_dev *)data;
 
-	dma_unmap_single(dev->dev, sg_dma_address(&dev->dma.sg),
+	dma_unmap_single(dev->dev, sg_dma_address(&dev->dma.sg[0]),
 			 dev->buf_len, DMA_TO_DEVICE);
 
-	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
+	/*
+	 * When this callback is called, THR/TX FIFO is likely not to be empty
+	 * yet. So we have to wait for TXCOMP or NACK bits to be set into the
+	 * Status Register to be sure that the STOP bit has been sent and the
+	 * transfer is completed. The NACK interrupt has already been enabled,
+	 * we just have to enable TXCOMP one.
+	 */
+	at91_twi_write(dev, AT91_TWI_IER, AT91_TWI_TXCOMP);
+	if (!dev->pdata->has_alt_cmd)
+		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
 }
 
 static void at91_twi_write_data_dma(struct at91_twi_dev *dev)
@@ -224,8 +273,9 @@ static void at91_twi_write_data_dma(struct at91_twi_dev *dev)
 	struct dma_async_tx_descriptor *txdesc;
 	struct at91_twi_dma *dma = &dev->dma;
 	struct dma_chan *chan_tx = dma->chan_tx;
+	unsigned int sg_len = 1;
 
-	if (dev->buf_len <= 0)
+	if (!dev->buf_len)
 		return;
 
 	dma->direction = DMA_TO_DEVICE;
@@ -239,10 +289,43 @@ static void at91_twi_write_data_dma(struct at91_twi_dev *dev)
 	}
 	dma->buf_mapped = true;
 	at91_twi_irq_restore(dev);
-	sg_dma_len(&dma->sg) = dev->buf_len;
-	sg_dma_address(&dma->sg) = dma_addr;
 
-	txdesc = dmaengine_prep_slave_sg(chan_tx, &dma->sg, 1, DMA_MEM_TO_DEV,
+	if (dev->fifo_size) {
+		size_t part1_len, part2_len;
+		struct scatterlist *sg;
+		unsigned fifo_mr;
+
+		sg_len = 0;
+
+		part1_len = dev->buf_len & ~0x3;
+		if (part1_len) {
+			sg = &dma->sg[sg_len++];
+			sg_dma_len(sg) = part1_len;
+			sg_dma_address(sg) = dma_addr;
+		}
+
+		part2_len = dev->buf_len & 0x3;
+		if (part2_len) {
+			sg = &dma->sg[sg_len++];
+			sg_dma_len(sg) = part2_len;
+			sg_dma_address(sg) = dma_addr + part1_len;
+		}
+
+		/*
+		 * DMA controller is triggered when at least 4 data can be
+		 * written into the TX FIFO
+		 */
+		fifo_mr = at91_twi_read(dev, AT91_TWI_FMR);
+		fifo_mr &= ~AT91_TWI_FMR_TXRDYM_MASK;
+		fifo_mr |= AT91_TWI_FMR_TXRDYM(AT91_TWI_FOUR_DATA);
+		at91_twi_write(dev, AT91_TWI_FMR, fifo_mr);
+	} else {
+		sg_dma_len(&dma->sg[0]) = dev->buf_len;
+		sg_dma_address(&dma->sg[0]) = dma_addr;
+	}
+
+	txdesc = dmaengine_prep_slave_sg(chan_tx, dma->sg, sg_len,
+					 DMA_MEM_TO_DEV,
 					 DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!txdesc) {
 		dev_err(dev->dev, "dma prep slave sg failed\n");
@@ -264,10 +347,11 @@ error:
 
 static void at91_twi_read_next_byte(struct at91_twi_dev *dev)
 {
-	if (dev->buf_len <= 0)
+	if (!dev->buf_len)
 		return;
 
-	*dev->buf = at91_twi_read(dev, AT91_TWI_RHR) & 0xff;
+	/* 8bit read works with and without FIFO */
+	*dev->buf = readb_relaxed(dev->base + AT91_TWI_RHR);
 	--dev->buf_len;
 
 	/* return if aborting, we only needed to read RHR to clear RXRDY*/
@@ -291,7 +375,7 @@ static void at91_twi_read_next_byte(struct at91_twi_dev *dev)
 	}
 
 	/* send stop if second but last byte has been read */
-	if (dev->buf_len == 1)
+	if (!dev->pdata->has_alt_cmd && dev->buf_len == 1)
 		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
 
 	dev_dbg(dev->dev, "read 0x%x, to go %d\n", *dev->buf, dev->buf_len);
@@ -302,14 +386,18 @@ static void at91_twi_read_next_byte(struct at91_twi_dev *dev)
 static void at91_twi_read_data_dma_callback(void *data)
 {
 	struct at91_twi_dev *dev = (struct at91_twi_dev *)data;
+	unsigned ier = AT91_TWI_TXCOMP;
 
-	dma_unmap_single(dev->dev, sg_dma_address(&dev->dma.sg),
+	dma_unmap_single(dev->dev, sg_dma_address(&dev->dma.sg[0]),
 			 dev->buf_len, DMA_FROM_DEVICE);
 
-	/* The last two bytes have to be read without using dma */
-	dev->buf += dev->buf_len - 2;
-	dev->buf_len = 2;
-	at91_twi_write(dev, AT91_TWI_IER, AT91_TWI_RXRDY);
+	if (!dev->pdata->has_alt_cmd) {
+		/* The last two bytes have to be read without using dma */
+		dev->buf += dev->buf_len - 2;
+		dev->buf_len = 2;
+		ier |= AT91_TWI_RXRDY;
+	}
+	at91_twi_write(dev, AT91_TWI_IER, ier);
 }
 
 static void at91_twi_read_data_dma(struct at91_twi_dev *dev)
@@ -318,23 +406,38 @@ static void at91_twi_read_data_dma(struct at91_twi_dev *dev)
 	struct dma_async_tx_descriptor *rxdesc;
 	struct at91_twi_dma *dma = &dev->dma;
 	struct dma_chan *chan_rx = dma->chan_rx;
+	size_t buf_len;
 
+	buf_len = (dev->pdata->has_alt_cmd) ? dev->buf_len : dev->buf_len - 2;
 	dma->direction = DMA_FROM_DEVICE;
 
 	/* Keep in mind that we won't use dma to read the last two bytes */
 	at91_twi_irq_save(dev);
-	dma_addr = dma_map_single(dev->dev, dev->buf, dev->buf_len - 2,
-				  DMA_FROM_DEVICE);
+	dma_addr = dma_map_single(dev->dev, dev->buf, buf_len, DMA_FROM_DEVICE);
 	if (dma_mapping_error(dev->dev, dma_addr)) {
 		dev_err(dev->dev, "dma map failed\n");
 		return;
 	}
 	dma->buf_mapped = true;
 	at91_twi_irq_restore(dev);
-	dma->sg.dma_address = dma_addr;
-	sg_dma_len(&dma->sg) = dev->buf_len - 2;
 
-	rxdesc = dmaengine_prep_slave_sg(chan_rx, &dma->sg, 1, DMA_DEV_TO_MEM,
+	if (dev->fifo_size && IS_ALIGNED(buf_len, 4)) {
+		unsigned fifo_mr;
+
+		/*
+		 * DMA controller is triggered when at least 4 data can be
+		 * read from the RX FIFO
+		 */
+		fifo_mr = at91_twi_read(dev, AT91_TWI_FMR);
+		fifo_mr &= ~AT91_TWI_FMR_RXRDYM_MASK;
+		fifo_mr |= AT91_TWI_FMR_RXRDYM(AT91_TWI_FOUR_DATA);
+		at91_twi_write(dev, AT91_TWI_FMR, fifo_mr);
+	}
+
+	sg_dma_len(&dma->sg[0]) = buf_len;
+	sg_dma_address(&dma->sg[0]) = dma_addr;
+
+	rxdesc = dmaengine_prep_slave_sg(chan_rx, dma->sg, 1, DMA_DEV_TO_MEM,
 					 DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!rxdesc) {
 		dev_err(dev->dev, "dma prep slave sg failed\n");
@@ -370,7 +473,7 @@ static irqreturn_t atmel_twi_interrupt(int irq, void *dev_id)
 	/* catch error flags */
 	dev->transfer_status |= status;
 
-	if (irqstatus & AT91_TWI_TXCOMP) {
+	if (irqstatus & (AT91_TWI_TXCOMP | AT91_TWI_NACK)) {
 		at91_disable_twi_interrupts(dev);
 		complete(&dev->cmd_complete);
 	}
@@ -383,6 +486,50 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 	int ret;
 	unsigned long time_left;
 	bool has_unre_flag = dev->pdata->has_unre_flag;
+	bool has_alt_cmd = dev->pdata->has_alt_cmd;
+
+	/*
+	 * WARNING: the TXCOMP bit in the Status Register is NOT a clear on
+	 * read flag but shows the state of the transmission at the time the
+	 * Status Register is read. According to the programmer datasheet,
+	 * TXCOMP is set when both holding register and internal shifter are
+	 * empty and STOP condition has been sent.
+	 * Consequently, we should enable NACK interrupt rather than TXCOMP to
+	 * detect transmission failure.
+	 * Indeed let's take the case of an i2c write command using DMA.
+	 * Whenever the slave doesn't acknowledge a byte, the LOCK, NACK and
+	 * TXCOMP bits are set together into the Status Register.
+	 * LOCK is a clear on write bit, which is set to prevent the DMA
+	 * controller from sending new data on the i2c bus after a NACK
+	 * condition has happened. Once locked, this i2c peripheral stops
+	 * triggering the DMA controller for new data but it is more than
+	 * likely that a new DMA transaction is already in progress, writing
+	 * into the Transmit Holding Register. Since the peripheral is locked,
+	 * these new data won't be sent to the i2c bus but they will remain
+	 * into the Transmit Holding Register, so TXCOMP bit is cleared.
+	 * Then when the interrupt handler is called, the Status Register is
+	 * read: the TXCOMP bit is clear but NACK bit is still set. The driver
+	 * manage the error properly, without waiting for timeout.
+	 * This case can be reproduced easyly when writing into an at24 eeprom.
+	 *
+	 * Besides, the TXCOMP bit is already set before the i2c transaction
+	 * has been started. For read transactions, this bit is cleared when
+	 * writing the START bit into the Control Register. So the
+	 * corresponding interrupt can safely be enabled just after.
+	 * However for write transactions managed by the CPU, we first write
+	 * into THR, so TXCOMP is cleared. Then we can safely enable TXCOMP
+	 * interrupt. If TXCOMP interrupt were enabled before writing into THR,
+	 * the interrupt handler would be called immediately and the i2c command
+	 * would be reported as completed.
+	 * Also when a write transaction is managed by the DMA controller,
+	 * enabling the TXCOMP interrupt in this function may lead to a race
+	 * condition since we don't know whether the TXCOMP interrupt is enabled
+	 * before or after the DMA has started to write into THR. So the TXCOMP
+	 * interrupt is enabled later by at91_twi_write_data_dma_callback().
+	 * Immediately after in that DMA callback, if the alternative command
+	 * mode is not used, we still need to send the STOP condition manually
+	 * writing the corresponding bit into the Control Register.
+	 */
 
 	dev_dbg(dev->dev, "transfer: %s %d bytes.\n",
 		(dev->msg->flags & I2C_M_RD) ? "read" : "write", dev->buf_len);
@@ -390,6 +537,21 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 	reinit_completion(&dev->cmd_complete);
 	dev->transfer_status = 0;
 
+	if (dev->fifo_size) {
+		unsigned fifo_mr = at91_twi_read(dev, AT91_TWI_FMR);
+
+		/* Reset FIFO mode register */
+		fifo_mr &= ~(AT91_TWI_FMR_TXRDYM_MASK |
+			     AT91_TWI_FMR_RXRDYM_MASK);
+		fifo_mr |= AT91_TWI_FMR_TXRDYM(AT91_TWI_ONE_DATA);
+		fifo_mr |= AT91_TWI_FMR_RXRDYM(AT91_TWI_ONE_DATA);
+		at91_twi_write(dev, AT91_TWI_FMR, fifo_mr);
+
+		/* Flush FIFOs */
+		at91_twi_write(dev, AT91_TWI_CR,
+			       AT91_TWI_THRCLR | AT91_TWI_RHRCLR);
+	}
+
 	if (!dev->buf_len) {
 		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_QUICK);
 		at91_twi_write(dev, AT91_TWI_IER, AT91_TWI_TXCOMP);
@@ -402,44 +564,45 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 		}
 
 		/* if only one byte is to be read, immediately stop transfer */
-		if (dev->buf_len <= 1 && !(dev->msg->flags & I2C_M_RECV_LEN))
+		if (!has_alt_cmd && dev->buf_len <= 1 &&
+		    !(dev->msg->flags & I2C_M_RECV_LEN))
 			start_flags |= AT91_TWI_STOP;
 		at91_twi_write(dev, AT91_TWI_CR, start_flags);
 		/*
-		 * When using dma, the last byte has to be read manually in
-		 * order to not send the stop command too late and then
-		 * to receive extra data. In practice, there are some issues
-		 * if you use the dma to read n-1 bytes because of latency.
+		 * When using dma without alternative command mode, the last
+		 * byte has to be read manually in order to not send the stop
+		 * command too late and then to receive extra data.
+		 * In practice, there are some issues if you use the dma to
+		 * read n-1 bytes because of latency.
 		 * Reading n-2 bytes with dma and the two last ones manually
 		 * seems to be the best solution.
 		 */
 		if (dev->use_dma && (dev->buf_len > AT91_I2C_DMA_THRESHOLD)) {
+			at91_twi_write(dev, AT91_TWI_IER, AT91_TWI_NACK);
 			at91_twi_read_data_dma(dev);
-			/*
-			 * It is important to enable TXCOMP irq here because
-			 * doing it only when transferring the last two bytes
-			 * will mask NACK errors since TXCOMP is set when a
-			 * NACK occurs.
-			 */
-			at91_twi_write(dev, AT91_TWI_IER,
-			       AT91_TWI_TXCOMP);
-		} else
+		} else {
 			at91_twi_write(dev, AT91_TWI_IER,
-			       AT91_TWI_TXCOMP | AT91_TWI_RXRDY);
+				       AT91_TWI_TXCOMP |
+				       AT91_TWI_NACK |
+				       AT91_TWI_RXRDY);
+		}
 	} else {
 		if (dev->use_dma && (dev->buf_len > AT91_I2C_DMA_THRESHOLD)) {
+			at91_twi_write(dev, AT91_TWI_IER, AT91_TWI_NACK);
 			at91_twi_write_data_dma(dev);
-			at91_twi_write(dev, AT91_TWI_IER, AT91_TWI_TXCOMP);
 		} else {
 			at91_twi_write_next_byte(dev);
 			at91_twi_write(dev, AT91_TWI_IER,
-				AT91_TWI_TXCOMP | AT91_TWI_TXRDY);
+				       AT91_TWI_TXCOMP |
+				       AT91_TWI_NACK |
+				       AT91_TWI_TXRDY);
 		}
 	}
 
 	time_left = wait_for_completion_timeout(&dev->cmd_complete,
 					      dev->adapter.timeout);
 	if (time_left == 0) {
+		dev->transfer_status |= at91_twi_read(dev, AT91_TWI_SR);
 		dev_err(dev->dev, "controller timed out\n");
 		at91_init_twi_bus(dev);
 		ret = -ETIMEDOUT;
@@ -460,6 +623,12 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 		ret = -EIO;
 		goto error;
 	}
+	if ((has_alt_cmd || dev->fifo_size) &&
+	    (dev->transfer_status & AT91_TWI_LOCK)) {
+		dev_err(dev->dev, "tx locked\n");
+		ret = -EIO;
+		goto error;
+	}
 	if (dev->recv_len_abort) {
 		dev_err(dev->dev, "invalid smbus block length recvd\n");
 		ret = -EPROTO;
@@ -471,7 +640,15 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 	return 0;
 
 error:
+	/* first stop DMA transfer if still in progress */
 	at91_twi_dma_cleanup(dev);
+	/* then flush THR/FIFO and unlock TX if locked */
+	if ((has_alt_cmd || dev->fifo_size) &&
+	    (dev->transfer_status & AT91_TWI_LOCK)) {
+		dev_dbg(dev->dev, "unlock tx\n");
+		at91_twi_write(dev, AT91_TWI_CR,
+			       AT91_TWI_THRCLR | AT91_TWI_LOCKCLR);
+	}
 	return ret;
 }
 
@@ -481,6 +658,7 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 	int ret;
 	unsigned int_addr_flag = 0;
 	struct i2c_msg *m_start = msg;
+	bool is_read, use_alt_cmd = false;
 
 	dev_dbg(&adap->dev, "at91_xfer: processing %d messages:\n", num);
 
@@ -503,8 +681,23 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 		at91_twi_write(dev, AT91_TWI_IADR, internal_address);
 	}
 
-	at91_twi_write(dev, AT91_TWI_MMR, (m_start->addr << 16) | int_addr_flag
-		       | ((m_start->flags & I2C_M_RD) ? AT91_TWI_MREAD : 0));
+	is_read = (m_start->flags & I2C_M_RD);
+	if (dev->pdata->has_alt_cmd) {
+		if (m_start->len > 0) {
+			at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_ACMEN);
+			at91_twi_write(dev, AT91_TWI_ACR,
+				       AT91_TWI_ACR_DATAL(m_start->len) |
+				       ((is_read) ? AT91_TWI_ACR_DIR : 0));
+			use_alt_cmd = true;
+		} else {
+			at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_ACMDIS);
+		}
+	}
+
+	at91_twi_write(dev, AT91_TWI_MMR,
+		       (m_start->addr << 16) |
+		       int_addr_flag |
+		       ((!use_alt_cmd && is_read) ? AT91_TWI_MREAD : 0));
 
 	dev->buf_len = m_start->len;
 	dev->buf = m_start->buf;
@@ -545,30 +738,35 @@ static struct at91_twi_pdata at91rm9200_config = {
 	.clk_max_div = 5,
 	.clk_offset = 3,
 	.has_unre_flag = true,
+	.has_alt_cmd = false,
 };
 
 static struct at91_twi_pdata at91sam9261_config = {
 	.clk_max_div = 5,
 	.clk_offset = 4,
 	.has_unre_flag = false,
+	.has_alt_cmd = false,
 };
 
 static struct at91_twi_pdata at91sam9260_config = {
 	.clk_max_div = 7,
 	.clk_offset = 4,
 	.has_unre_flag = false,
+	.has_alt_cmd = false,
 };
 
 static struct at91_twi_pdata at91sam9g20_config = {
 	.clk_max_div = 7,
 	.clk_offset = 4,
 	.has_unre_flag = false,
+	.has_alt_cmd = false,
 };
 
 static struct at91_twi_pdata at91sam9g10_config = {
 	.clk_max_div = 7,
 	.clk_offset = 4,
 	.has_unre_flag = false,
+	.has_alt_cmd = false,
 };
 
 static const struct platform_device_id at91_twi_devtypes[] = {
@@ -597,6 +795,14 @@ static struct at91_twi_pdata at91sam9x5_config = {
 	.clk_max_div = 7,
 	.clk_offset = 4,
 	.has_unre_flag = false,
+	.has_alt_cmd = false,
+};
+
+static struct at91_twi_pdata sama5d2_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+	.has_unre_flag = true,
+	.has_alt_cmd = true,
 };
 
 static const struct of_device_id atmel_twi_dt_ids[] = {
@@ -619,6 +825,9 @@ static const struct of_device_id atmel_twi_dt_ids[] = {
 		.compatible = "atmel,at91sam9x5-i2c",
 		.data = &at91sam9x5_config,
 	}, {
+		.compatible = "atmel,sama5d2-i2c",
+		.data = &sama5d2_config,
+	}, {
 		/* sentinel */
 	}
 };
@@ -630,13 +839,32 @@ static int at91_twi_configure_dma(struct at91_twi_dev *dev, u32 phy_addr)
 	int ret = 0;
 	struct dma_slave_config slave_config;
 	struct at91_twi_dma *dma = &dev->dma;
+	enum dma_slave_buswidth addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+
+	/*
+	 * The actual width of the access will be chosen in
+	 * dmaengine_prep_slave_sg():
+	 * for each buffer in the scatter-gather list, if its size is aligned
+	 * to addr_width then addr_width accesses will be performed to transfer
+	 * the buffer. On the other hand, if the buffer size is not aligned to
+	 * addr_width then the buffer is transferred using single byte accesses.
+	 * Please refer to the Atmel eXtended DMA controller driver.
+	 * When FIFOs are used, the TXRDYM threshold can always be set to
+	 * trigger the XDMAC when at least 4 data can be written into the TX
+	 * FIFO, even if single byte accesses are performed.
+	 * However the RXRDYM threshold must be set to fit the access width,
+	 * deduced from buffer length, so the XDMAC is triggered properly to
+	 * read data from the RX FIFO.
+	 */
+	if (dev->fifo_size)
+		addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
 
 	memset(&slave_config, 0, sizeof(slave_config));
 	slave_config.src_addr = (dma_addr_t)phy_addr + AT91_TWI_RHR;
-	slave_config.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	slave_config.src_addr_width = addr_width;
 	slave_config.src_maxburst = 1;
 	slave_config.dst_addr = (dma_addr_t)phy_addr + AT91_TWI_THR;
-	slave_config.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	slave_config.dst_addr_width = addr_width;
 	slave_config.dst_maxburst = 1;
 	slave_config.device_fc = false;
 
@@ -668,7 +896,7 @@ static int at91_twi_configure_dma(struct at91_twi_dev *dev, u32 phy_addr)
 		goto error;
 	}
 
-	sg_init_table(&dma->sg, 1);
+	sg_init_table(dma->sg, 2);
 	dma->buf_mapped = false;
 	dma->xfer_in_progress = false;
 	dev->use_dma = true;
@@ -754,6 +982,11 @@ static int at91_twi_probe(struct platform_device *pdev)
 			return rc;
 	}
 
+	if (!of_property_read_u32(pdev->dev.of_node, "atmel,fifo-size",
+				  &dev->fifo_size)) {
+		dev_info(dev->dev, "Using FIFO (%u data)\n", dev->fifo_size);
+	}
+
 	rc = of_property_read_u32(dev->dev->of_node, "clock-frequency",
 			&bus_clk_rate);
 	if (rc)
@@ -790,7 +1023,8 @@ static int at91_twi_probe(struct platform_device *pdev)
 		return rc;
 	}
 
-	dev_info(dev->dev, "AT91 i2c bus driver.\n");
+	dev_info(dev->dev, "AT91 i2c bus driver (hw version: %#x).\n",
+		 at91_twi_read(dev, AT91_TWI_VER));
 	return 0;
 }
 
diff --git a/drivers/i2c/busses/i2c-axxia.c b/drivers/i2c/busses/i2c-axxia.c
index 32d883490863..c335cc7852f9 100644
--- a/drivers/i2c/busses/i2c-axxia.c
+++ b/drivers/i2c/busses/i2c-axxia.c
@@ -42,6 +42,10 @@
 #define IBML_LOW_SEXT		0x18
 #define TIMER_CLOCK_DIV		0x1c
 #define I2C_BUS_MONITOR		0x20
+#define   BM_SDAC		BIT(3)
+#define   BM_SCLC		BIT(2)
+#define   BM_SDAS		BIT(1)
+#define   BM_SCLS		BIT(0)
 #define SOFT_RESET		0x24
 #define MST_COMMAND		0x28
 #define   CMD_BUSY		(1<<3)
@@ -394,6 +398,9 @@ static int axxia_i2c_xfer_msg(struct axxia_i2c_dev *idev, struct i2c_msg *msg)
 	if (time_left == 0)
 		idev->msg_err = -ETIMEDOUT;
 
+	if (idev->msg_err == -ETIMEDOUT)
+		i2c_recover_bus(&idev->adapter);
+
 	if (unlikely(idev->msg_err) && idev->msg_err != -ENXIO)
 		axxia_i2c_init(idev);
 
@@ -437,6 +444,39 @@ axxia_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 	return ret ? : i;
 }
 
+static int axxia_i2c_get_scl(struct i2c_adapter *adap)
+{
+	struct axxia_i2c_dev *idev = i2c_get_adapdata(adap);
+
+	return !!(readl(idev->base + I2C_BUS_MONITOR) & BM_SCLS);
+}
+
+static void axxia_i2c_set_scl(struct i2c_adapter *adap, int val)
+{
+	struct axxia_i2c_dev *idev = i2c_get_adapdata(adap);
+	u32 tmp;
+
+	/* Preserve SDA Control */
+	tmp = readl(idev->base + I2C_BUS_MONITOR) & BM_SDAC;
+	if (!val)
+		tmp |= BM_SCLC;
+	writel(tmp, idev->base + I2C_BUS_MONITOR);
+}
+
+static int axxia_i2c_get_sda(struct i2c_adapter *adap)
+{
+	struct axxia_i2c_dev *idev = i2c_get_adapdata(adap);
+
+	return !!(readl(idev->base + I2C_BUS_MONITOR) & BM_SDAS);
+}
+
+static struct i2c_bus_recovery_info axxia_i2c_recovery_info = {
+	.recover_bus = i2c_generic_scl_recovery,
+	.get_scl = axxia_i2c_get_scl,
+	.set_scl = axxia_i2c_set_scl,
+	.get_sda = axxia_i2c_get_sda,
+};
+
 static u32 axxia_i2c_func(struct i2c_adapter *adap)
 {
 	u32 caps = (I2C_FUNC_I2C | I2C_FUNC_10BIT_ADDR |
@@ -511,6 +551,7 @@ static int axxia_i2c_probe(struct platform_device *pdev)
 	strlcpy(idev->adapter.name, pdev->name, sizeof(idev->adapter.name));
 	idev->adapter.owner = THIS_MODULE;
 	idev->adapter.algo = &axxia_i2c_algo;
+	idev->adapter.bus_recovery_info = &axxia_i2c_recovery_info;
 	idev->adapter.quirks = &axxia_i2c_quirks;
 	idev->adapter.dev.parent = &pdev->dev;
 	idev->adapter.dev.of_node = pdev->dev.of_node;
diff --git a/drivers/i2c/busses/i2c-bcm-iproc.c b/drivers/i2c/busses/i2c-bcm-iproc.c
index f9f2c2082151..0419f5284609 100644
--- a/drivers/i2c/busses/i2c-bcm-iproc.c
+++ b/drivers/i2c/busses/i2c-bcm-iproc.c
@@ -91,6 +91,7 @@ struct bcm_iproc_i2c_dev {
 	void __iomem *base;
 
 	struct i2c_adapter adapter;
+	unsigned int bus_speed;
 
 	struct completion done;
 	int xfer_is_done;
@@ -309,6 +310,7 @@ static int bcm_iproc_i2c_cfg_speed(struct bcm_iproc_i2c_dev *iproc_i2c)
 		bus_speed = 400000;
 	}
 
+	iproc_i2c->bus_speed = bus_speed;
 	val = readl(iproc_i2c->base + TIM_CFG_OFFSET);
 	val &= ~(1 << TIM_CFG_MODE_400_SHIFT);
 	val |= (bus_speed == 400000) << TIM_CFG_MODE_400_SHIFT;
@@ -439,6 +441,60 @@ static int bcm_iproc_i2c_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+
+static int bcm_iproc_i2c_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct bcm_iproc_i2c_dev *iproc_i2c = platform_get_drvdata(pdev);
+
+	/* make sure there's no pending interrupt when we go into suspend */
+	writel(0, iproc_i2c->base + IE_OFFSET);
+	readl(iproc_i2c->base + IE_OFFSET);
+	synchronize_irq(iproc_i2c->irq);
+
+	/* now disable the controller */
+	bcm_iproc_i2c_enable_disable(iproc_i2c, false);
+
+	return 0;
+}
+
+static int bcm_iproc_i2c_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct bcm_iproc_i2c_dev *iproc_i2c = platform_get_drvdata(pdev);
+	int ret;
+	u32 val;
+
+	/*
+	 * Power domain could have been shut off completely in system deep
+	 * sleep, so re-initialize the block here
+	 */
+	ret = bcm_iproc_i2c_init(iproc_i2c);
+	if (ret)
+		return ret;
+
+	/* configure to the desired bus speed */
+	val = readl(iproc_i2c->base + TIM_CFG_OFFSET);
+	val &= ~(1 << TIM_CFG_MODE_400_SHIFT);
+	val |= (iproc_i2c->bus_speed == 400000) << TIM_CFG_MODE_400_SHIFT;
+	writel(val, iproc_i2c->base + TIM_CFG_OFFSET);
+
+	bcm_iproc_i2c_enable_disable(iproc_i2c, true);
+
+	return 0;
+}
+
+static const struct dev_pm_ops bcm_iproc_i2c_pm_ops = {
+	.suspend_late = &bcm_iproc_i2c_suspend,
+	.resume_early = &bcm_iproc_i2c_resume
+};
+
+#define BCM_IPROC_I2C_PM_OPS (&bcm_iproc_i2c_pm_ops)
+#else
+#define BCM_IPROC_I2C_PM_OPS NULL
+#endif /* CONFIG_PM_SLEEP */
+
 static const struct of_device_id bcm_iproc_i2c_of_match[] = {
 	{ .compatible = "brcm,iproc-i2c" },
 	{ /* sentinel */ }
@@ -449,6 +505,7 @@ static struct platform_driver bcm_iproc_i2c_driver = {
 	.driver = {
 		.name = "bcm-iproc-i2c",
 		.of_match_table = bcm_iproc_i2c_of_match,
+		.pm = BCM_IPROC_I2C_PM_OPS,
 	},
 	.probe = bcm_iproc_i2c_probe,
 	.remove = bcm_iproc_i2c_remove,
diff --git a/drivers/i2c/busses/i2c-bcm2835.c b/drivers/i2c/busses/i2c-bcm2835.c
index c9336a3202d5..3032b89ac60b 100644
--- a/drivers/i2c/busses/i2c-bcm2835.c
+++ b/drivers/i2c/busses/i2c-bcm2835.c
@@ -50,6 +50,11 @@
 #define BCM2835_I2C_S_CLKT	BIT(9)
 #define BCM2835_I2C_S_LEN	BIT(10) /* Fake bit for SW error reporting */
 
+#define BCM2835_I2C_BITMSK_S	0x03FF
+
+#define BCM2835_I2C_CDIV_MIN	0x0002
+#define BCM2835_I2C_CDIV_MAX	0xFFFE
+
 #define BCM2835_I2C_TIMEOUT (msecs_to_jiffies(1000))
 
 struct bcm2835_i2c_dev {
@@ -111,6 +116,7 @@ static irqreturn_t bcm2835_i2c_isr(int this_irq, void *data)
 	u32 val, err;
 
 	val = bcm2835_i2c_readl(i2c_dev, BCM2835_I2C_S);
+	val &= BCM2835_I2C_BITMSK_S;
 	bcm2835_i2c_writel(i2c_dev, BCM2835_I2C_S, val);
 
 	err = val & (BCM2835_I2C_S_CLKT | BCM2835_I2C_S_ERR);
@@ -258,6 +264,11 @@ static int bcm2835_i2c_probe(struct platform_device *pdev)
 	 */
 	if (divider & 1)
 		divider++;
+	if ((divider < BCM2835_I2C_CDIV_MIN) ||
+	    (divider > BCM2835_I2C_CDIV_MAX)) {
+		dev_err(&pdev->dev, "Invalid clock-frequency\n");
+		return -ENODEV;
+	}
 	bcm2835_i2c_writel(i2c_dev, BCM2835_I2C_DIV, divider);
 
 	irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
diff --git a/drivers/i2c/busses/i2c-brcmstb.c b/drivers/i2c/busses/i2c-brcmstb.c
new file mode 100644
index 000000000000..8e9637eea512
--- /dev/null
+++ b/drivers/i2c/busses/i2c-brcmstb.c
@@ -0,0 +1,694 @@
+/*
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+
+#define N_DATA_REGS					8
+#define N_DATA_BYTES					(N_DATA_REGS * 4)
+
+/* BSC count register field definitions */
+#define BSC_CNT_REG1_MASK				0x0000003f
+#define BSC_CNT_REG1_SHIFT				0
+#define BSC_CNT_REG2_MASK				0x00000fc0
+#define BSC_CNT_REG2_SHIFT				6
+
+/* BSC CTL register field definitions */
+#define BSC_CTL_REG_DTF_MASK				0x00000003
+#define BSC_CTL_REG_SCL_SEL_MASK			0x00000030
+#define BSC_CTL_REG_SCL_SEL_SHIFT			4
+#define BSC_CTL_REG_INT_EN_MASK				0x00000040
+#define BSC_CTL_REG_INT_EN_SHIFT			6
+#define BSC_CTL_REG_DIV_CLK_MASK			0x00000080
+
+/* BSC_IIC_ENABLE r/w enable and interrupt field defintions */
+#define BSC_IIC_EN_RESTART_MASK				0x00000040
+#define BSC_IIC_EN_NOSTART_MASK				0x00000020
+#define BSC_IIC_EN_NOSTOP_MASK				0x00000010
+#define BSC_IIC_EN_NOACK_MASK				0x00000004
+#define BSC_IIC_EN_INTRP_MASK				0x00000002
+#define BSC_IIC_EN_ENABLE_MASK				0x00000001
+
+/* BSC_CTLHI control register field definitions */
+#define BSC_CTLHI_REG_INPUT_SWITCHING_LEVEL_MASK	0x00000080
+#define BSC_CTLHI_REG_DATAREG_SIZE_MASK			0x00000040
+#define BSC_CTLHI_REG_IGNORE_ACK_MASK			0x00000002
+#define BSC_CTLHI_REG_WAIT_DIS_MASK			0x00000001
+
+#define I2C_TIMEOUT					100 /* msecs */
+
+/* Condition mask used for non combined transfer */
+#define COND_RESTART		BSC_IIC_EN_RESTART_MASK
+#define COND_NOSTART		BSC_IIC_EN_NOSTART_MASK
+#define COND_NOSTOP		BSC_IIC_EN_NOSTOP_MASK
+#define COND_START_STOP		(COND_RESTART | COND_NOSTART | COND_NOSTOP)
+
+/* BSC data transfer direction */
+#define DTF_WR_MASK		0x00000000
+#define DTF_RD_MASK		0x00000001
+/* BSC data transfer direction combined format */
+#define DTF_RD_WR_MASK		0x00000002
+#define DTF_WR_RD_MASK		0x00000003
+
+#define INT_ENABLE		true
+#define INT_DISABLE		false
+
+/* BSC block register map structure to cache fields to be written */
+struct bsc_regs {
+	u32	chip_address;           /* slave address */
+	u32	data_in[N_DATA_REGS];   /* tx data buffer*/
+	u32	cnt_reg;		/* rx/tx data length */
+	u32	ctl_reg;		/* control register */
+	u32	iic_enable;		/* xfer enable and status */
+	u32	data_out[N_DATA_REGS];  /* rx data buffer */
+	u32	ctlhi_reg;		/* more control fields */
+	u32	scl_param;		/* reserved */
+};
+
+struct bsc_clk_param {
+	u32 hz;
+	u32 scl_mask;
+	u32 div_mask;
+};
+
+enum bsc_xfer_cmd {
+	CMD_WR,
+	CMD_RD,
+	CMD_WR_NOACK,
+	CMD_RD_NOACK,
+};
+
+static char const *cmd_string[] = {
+	[CMD_WR] = "WR",
+	[CMD_RD] = "RD",
+	[CMD_WR_NOACK] = "WR NOACK",
+	[CMD_RD_NOACK] = "RD NOACK",
+};
+
+enum bus_speeds {
+	SPD_375K,
+	SPD_390K,
+	SPD_187K,
+	SPD_200K,
+	SPD_93K,
+	SPD_97K,
+	SPD_46K,
+	SPD_50K
+};
+
+static const struct bsc_clk_param bsc_clk[] = {
+	[SPD_375K] = {
+		.hz = 375000,
+		.scl_mask = SPD_375K << BSC_CTL_REG_SCL_SEL_SHIFT,
+		.div_mask = 0
+	},
+	[SPD_390K] = {
+		.hz = 390000,
+		.scl_mask = SPD_390K << BSC_CTL_REG_SCL_SEL_SHIFT,
+		.div_mask = 0
+	},
+	[SPD_187K] = {
+		.hz = 187500,
+		.scl_mask = SPD_187K << BSC_CTL_REG_SCL_SEL_SHIFT,
+		.div_mask = 0
+	},
+	[SPD_200K] = {
+		.hz = 200000,
+		.scl_mask = SPD_200K << BSC_CTL_REG_SCL_SEL_SHIFT,
+		.div_mask = 0
+	},
+	[SPD_93K]  = {
+		.hz = 93750,
+		.scl_mask = SPD_375K << BSC_CTL_REG_SCL_SEL_SHIFT,
+		.div_mask = BSC_CTL_REG_DIV_CLK_MASK
+	},
+	[SPD_97K]  = {
+		.hz = 97500,
+		.scl_mask = SPD_390K << BSC_CTL_REG_SCL_SEL_SHIFT,
+		.div_mask = BSC_CTL_REG_DIV_CLK_MASK
+	},
+	[SPD_46K]  = {
+		.hz = 46875,
+		.scl_mask = SPD_187K << BSC_CTL_REG_SCL_SEL_SHIFT,
+		.div_mask = BSC_CTL_REG_DIV_CLK_MASK
+	},
+	[SPD_50K]  = {
+		.hz = 50000,
+		.scl_mask = SPD_200K << BSC_CTL_REG_SCL_SEL_SHIFT,
+		.div_mask = BSC_CTL_REG_DIV_CLK_MASK
+	}
+};
+
+struct brcmstb_i2c_dev {
+	struct device *device;
+	void __iomem *base;
+	void __iomem *irq_base;
+	int irq;
+	struct bsc_regs *bsc_regmap;
+	struct i2c_adapter adapter;
+	struct completion done;
+	bool is_suspended;
+	u32 clk_freq_hz;
+};
+
+/* register accessors for both be and le cpu arch */
+#ifdef CONFIG_CPU_BIG_ENDIAN
+#define __bsc_readl(_reg) ioread32be(_reg)
+#define __bsc_writel(_val, _reg) iowrite32be(_val, _reg)
+#else
+#define __bsc_readl(_reg) ioread32(_reg)
+#define __bsc_writel(_val, _reg) iowrite32(_val, _reg)
+#endif
+
+#define bsc_readl(_dev, _reg)						\
+	__bsc_readl(_dev->base + offsetof(struct bsc_regs, _reg))
+
+#define bsc_writel(_dev, _val, _reg)					\
+	__bsc_writel(_val, _dev->base + offsetof(struct bsc_regs, _reg))
+
+static void brcmstb_i2c_enable_disable_irq(struct brcmstb_i2c_dev *dev,
+					   bool int_en)
+{
+
+	if (int_en)
+		/* Enable BSC  CTL interrupt line */
+		dev->bsc_regmap->ctl_reg |= BSC_CTL_REG_INT_EN_MASK;
+	else
+		/* Disable BSC CTL interrupt line */
+		dev->bsc_regmap->ctl_reg &= ~BSC_CTL_REG_INT_EN_MASK;
+
+	barrier();
+	bsc_writel(dev, dev->bsc_regmap->ctl_reg, ctl_reg);
+}
+
+static irqreturn_t brcmstb_i2c_isr(int irq, void *devid)
+{
+	struct brcmstb_i2c_dev *dev = devid;
+	u32 status_bsc_ctl = bsc_readl(dev, ctl_reg);
+	u32 status_iic_intrp = bsc_readl(dev, iic_enable);
+
+	dev_dbg(dev->device, "isr CTL_REG %x IIC_EN %x\n",
+		status_bsc_ctl, status_iic_intrp);
+
+	if (!(status_bsc_ctl & BSC_CTL_REG_INT_EN_MASK))
+		return IRQ_NONE;
+
+	brcmstb_i2c_enable_disable_irq(dev, INT_DISABLE);
+	complete_all(&dev->done);
+
+	dev_dbg(dev->device, "isr handled");
+	return IRQ_HANDLED;
+}
+
+/* Wait for device to be ready */
+static int brcmstb_i2c_wait_if_busy(struct brcmstb_i2c_dev *dev)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(I2C_TIMEOUT);
+
+	while ((bsc_readl(dev, iic_enable) & BSC_IIC_EN_INTRP_MASK)) {
+		if (time_after(jiffies, timeout))
+			return -ETIMEDOUT;
+		cpu_relax();
+	}
+	return 0;
+}
+
+/* i2c xfer completion function, handles both irq and polling mode */
+static int brcmstb_i2c_wait_for_completion(struct brcmstb_i2c_dev *dev)
+{
+	int ret = 0;
+	unsigned long timeout = msecs_to_jiffies(I2C_TIMEOUT);
+
+	if (dev->irq >= 0) {
+		if (!wait_for_completion_timeout(&dev->done, timeout))
+			ret = -ETIMEDOUT;
+	} else {
+		/* we are in polling mode */
+		u32 bsc_intrp;
+		unsigned long time_left = jiffies + timeout;
+
+		do {
+			bsc_intrp = bsc_readl(dev, iic_enable) &
+				BSC_IIC_EN_INTRP_MASK;
+			if (time_after(jiffies, time_left)) {
+				ret = -ETIMEDOUT;
+				break;
+			}
+			cpu_relax();
+		} while (!bsc_intrp);
+	}
+
+	if (dev->irq < 0 || ret == -ETIMEDOUT)
+		brcmstb_i2c_enable_disable_irq(dev, INT_DISABLE);
+
+	return ret;
+}
+
+/* Set xfer START/STOP conditions for subsequent transfer */
+static void brcmstb_set_i2c_start_stop(struct brcmstb_i2c_dev *dev,
+				       u32 cond_flag)
+{
+	u32 regval = dev->bsc_regmap->iic_enable;
+
+	dev->bsc_regmap->iic_enable = (regval & ~COND_START_STOP) | cond_flag;
+}
+
+/* Send I2C request check completion */
+static int brcmstb_send_i2c_cmd(struct brcmstb_i2c_dev *dev,
+				enum bsc_xfer_cmd cmd)
+{
+	int rc = 0;
+	struct bsc_regs *pi2creg = dev->bsc_regmap;
+
+	/* Make sure the hardware is ready */
+	rc = brcmstb_i2c_wait_if_busy(dev);
+	if (rc < 0)
+		return rc;
+
+	/* only if we are in interrupt mode */
+	if (dev->irq >= 0)
+		reinit_completion(&dev->done);
+
+	/* enable BSC CTL interrupt line */
+	brcmstb_i2c_enable_disable_irq(dev, INT_ENABLE);
+
+	/* initiate transfer by setting iic_enable */
+	pi2creg->iic_enable |= BSC_IIC_EN_ENABLE_MASK;
+	bsc_writel(dev, pi2creg->iic_enable, iic_enable);
+
+	/* Wait for transaction to finish or timeout */
+	rc = brcmstb_i2c_wait_for_completion(dev);
+	if (rc) {
+		dev_dbg(dev->device, "intr timeout for cmd %s\n",
+			cmd_string[cmd]);
+		goto cmd_out;
+	}
+
+	if ((CMD_RD || CMD_WR) &&
+	    bsc_readl(dev, iic_enable) & BSC_IIC_EN_NOACK_MASK) {
+		rc = -EREMOTEIO;
+		dev_dbg(dev->device, "controller received NOACK intr for %s\n",
+			cmd_string[cmd]);
+	}
+
+cmd_out:
+	bsc_writel(dev, 0, cnt_reg);
+	bsc_writel(dev, 0, iic_enable);
+
+	return rc;
+}
+
+/* Actual data transfer through the BSC master */
+static int brcmstb_i2c_xfer_bsc_data(struct brcmstb_i2c_dev *dev,
+				     u8 *buf, unsigned int len,
+				     struct i2c_msg *pmsg)
+{
+	int cnt, byte, rc;
+	enum bsc_xfer_cmd cmd;
+	u32 ctl_reg;
+	struct bsc_regs *pi2creg = dev->bsc_regmap;
+	int no_ack = pmsg->flags & I2C_M_IGNORE_NAK;
+
+	/* see if the transaction needs to check NACK conditions */
+	if (no_ack || len <= N_DATA_BYTES) {
+		cmd = (pmsg->flags & I2C_M_RD) ? CMD_RD_NOACK
+			: CMD_WR_NOACK;
+		pi2creg->ctlhi_reg |= BSC_CTLHI_REG_IGNORE_ACK_MASK;
+	} else {
+		cmd = (pmsg->flags & I2C_M_RD) ? CMD_RD : CMD_WR;
+		pi2creg->ctlhi_reg &= ~BSC_CTLHI_REG_IGNORE_ACK_MASK;
+	}
+	bsc_writel(dev, pi2creg->ctlhi_reg, ctlhi_reg);
+
+	/* set data transfer direction */
+	ctl_reg = pi2creg->ctl_reg & ~BSC_CTL_REG_DTF_MASK;
+	if (cmd == CMD_WR || cmd == CMD_WR_NOACK)
+		pi2creg->ctl_reg = ctl_reg | DTF_WR_MASK;
+	else
+		pi2creg->ctl_reg = ctl_reg | DTF_RD_MASK;
+
+	/* set the read/write length */
+	bsc_writel(dev, BSC_CNT_REG1_MASK & (len << BSC_CNT_REG1_SHIFT),
+		   cnt_reg);
+
+	/* Write data into data_in register */
+	if (cmd == CMD_WR || cmd == CMD_WR_NOACK) {
+		for (cnt = 0; cnt < len; cnt += 4) {
+			u32 word = 0;
+
+			for (byte = 0; byte < 4; byte++) {
+				word >>= 8;
+				if ((cnt + byte) < len)
+					word |= buf[cnt + byte] << 24;
+			}
+			bsc_writel(dev, word, data_in[cnt >> 2]);
+		}
+	}
+
+	/* Initiate xfer, the function will return on completion */
+	rc = brcmstb_send_i2c_cmd(dev, cmd);
+
+	if (rc != 0) {
+		dev_dbg(dev->device, "%s failure", cmd_string[cmd]);
+		return rc;
+	}
+
+	if (cmd == CMD_RD || cmd == CMD_RD_NOACK) {
+		for (cnt = 0; cnt < len; cnt += 4) {
+			u32 data = bsc_readl(dev, data_out[cnt >> 2]);
+
+			for (byte = 0; byte < 4 &&
+				     (byte + cnt) < len; byte++) {
+				buf[cnt + byte] = data & 0xff;
+				data >>= 8;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/* Write a single byte of data to the i2c bus */
+static int brcmstb_i2c_write_data_byte(struct brcmstb_i2c_dev *dev,
+				       u8 *buf, unsigned int nak_expected)
+{
+	enum bsc_xfer_cmd cmd = nak_expected ? CMD_WR : CMD_WR_NOACK;
+
+	bsc_writel(dev, 1, cnt_reg);
+	bsc_writel(dev, *buf, data_in);
+
+	return brcmstb_send_i2c_cmd(dev, cmd);
+}
+
+/* Send i2c address */
+static int brcmstb_i2c_do_addr(struct brcmstb_i2c_dev *dev,
+			       struct i2c_msg *msg)
+{
+	unsigned char addr;
+
+	if (msg->flags & I2C_M_TEN) {
+		/* First byte is 11110XX0 where XX is upper 2 bits */
+		addr = 0xF0 | ((msg->addr & 0x300) >> 7);
+		bsc_writel(dev, addr, chip_address);
+
+		/* Second byte is the remaining 8 bits */
+		addr = msg->addr & 0xFF;
+		if (brcmstb_i2c_write_data_byte(dev, &addr, 0) < 0)
+			return -EREMOTEIO;
+
+		if (msg->flags & I2C_M_RD) {
+			/* For read, send restart without stop condition */
+			brcmstb_set_i2c_start_stop(dev, COND_RESTART
+						   | COND_NOSTOP);
+			/* Then re-send the first byte with the read bit set */
+			addr = 0xF0 | ((msg->addr & 0x300) >> 7) | 0x01;
+			if (brcmstb_i2c_write_data_byte(dev, &addr, 0) < 0)
+				return -EREMOTEIO;
+
+		}
+	} else {
+		addr = msg->addr << 1;
+		if (msg->flags & I2C_M_RD)
+			addr |= 1;
+
+		bsc_writel(dev, addr, chip_address);
+	}
+
+	return 0;
+}
+
+/* Master transfer function */
+static int brcmstb_i2c_xfer(struct i2c_adapter *adapter,
+			    struct i2c_msg msgs[], int num)
+{
+	struct brcmstb_i2c_dev *dev = i2c_get_adapdata(adapter);
+	struct i2c_msg *pmsg;
+	int rc = 0;
+	int i;
+	int bytes_to_xfer;
+	u8 *tmp_buf;
+	int len = 0;
+
+	if (dev->is_suspended)
+		return -EBUSY;
+
+	/* Loop through all messages */
+	for (i = 0; i < num; i++) {
+		pmsg = &msgs[i];
+		len = pmsg->len;
+		tmp_buf = pmsg->buf;
+
+		dev_dbg(dev->device,
+			"msg# %d/%d flg %x buf %x len %d\n", i,
+			num - 1, pmsg->flags,
+			pmsg->buf ? pmsg->buf[0] : '0', pmsg->len);
+
+		if (i < (num - 1) && (msgs[i + 1].flags & I2C_M_NOSTART))
+			brcmstb_set_i2c_start_stop(dev, ~(COND_START_STOP));
+		else
+			brcmstb_set_i2c_start_stop(dev,
+						   COND_RESTART | COND_NOSTOP);
+
+		/* Send slave address */
+		if (!(pmsg->flags & I2C_M_NOSTART)) {
+			rc = brcmstb_i2c_do_addr(dev, pmsg);
+			if (rc < 0) {
+				dev_dbg(dev->device,
+					"NACK for addr %2.2x msg#%d rc = %d\n",
+					pmsg->addr, i, rc);
+				goto out;
+			}
+		}
+
+		/* Perform data transfer */
+		while (len) {
+			bytes_to_xfer = min(len, N_DATA_BYTES);
+
+			if (len <= N_DATA_BYTES && i == (num - 1))
+				brcmstb_set_i2c_start_stop(dev,
+							   ~(COND_START_STOP));
+
+			rc = brcmstb_i2c_xfer_bsc_data(dev, tmp_buf,
+						       bytes_to_xfer, pmsg);
+			if (rc < 0)
+				goto out;
+
+			len -=  bytes_to_xfer;
+			tmp_buf += bytes_to_xfer;
+		}
+	}
+
+	rc = num;
+out:
+	return rc;
+
+}
+
+static u32 brcmstb_i2c_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_10BIT_ADDR
+		| I2C_FUNC_NOSTART | I2C_FUNC_PROTOCOL_MANGLING;
+}
+
+static const struct i2c_algorithm brcmstb_i2c_algo = {
+	.master_xfer = brcmstb_i2c_xfer,
+	.functionality = brcmstb_i2c_functionality,
+};
+
+static void brcmstb_i2c_set_bus_speed(struct brcmstb_i2c_dev *dev)
+{
+	int i = 0, num_speeds = ARRAY_SIZE(bsc_clk);
+	u32 clk_freq_hz = dev->clk_freq_hz;
+
+	for (i = 0; i < num_speeds; i++) {
+		if (bsc_clk[i].hz == clk_freq_hz) {
+			dev->bsc_regmap->ctl_reg &= ~(BSC_CTL_REG_SCL_SEL_MASK
+						| BSC_CTL_REG_DIV_CLK_MASK);
+			dev->bsc_regmap->ctl_reg |= (bsc_clk[i].scl_mask |
+						     bsc_clk[i].div_mask);
+			bsc_writel(dev, dev->bsc_regmap->ctl_reg, ctl_reg);
+			break;
+		}
+	}
+
+	/* in case we did not get find a valid speed */
+	if (i == num_speeds) {
+		i = (bsc_readl(dev, ctl_reg) & BSC_CTL_REG_SCL_SEL_MASK) >>
+			BSC_CTL_REG_SCL_SEL_SHIFT;
+		dev_warn(dev->device, "leaving current clock-frequency @ %dHz\n",
+			bsc_clk[i].hz);
+	}
+}
+
+static void brcmstb_i2c_set_bsc_reg_defaults(struct brcmstb_i2c_dev *dev)
+{
+	/* 4 byte data register */
+	dev->bsc_regmap->ctlhi_reg = BSC_CTLHI_REG_DATAREG_SIZE_MASK;
+	bsc_writel(dev, dev->bsc_regmap->ctlhi_reg, ctlhi_reg);
+	/* set bus speed */
+	brcmstb_i2c_set_bus_speed(dev);
+}
+
+static int brcmstb_i2c_probe(struct platform_device *pdev)
+{
+	int rc = 0;
+	struct brcmstb_i2c_dev *dev;
+	struct i2c_adapter *adap;
+	struct resource *iomem;
+	const char *int_name;
+
+	/* Allocate memory for private data structure */
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->bsc_regmap = devm_kzalloc(&pdev->dev, sizeof(struct bsc_regs *),
+				       GFP_KERNEL);
+	if (!dev->bsc_regmap)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, dev);
+	dev->device = &pdev->dev;
+	init_completion(&dev->done);
+
+	/* Map hardware registers */
+	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dev->base = devm_ioremap_resource(dev->device, iomem);
+	if (IS_ERR(dev->base)) {
+		rc = -ENOMEM;
+		goto probe_errorout;
+	}
+
+	rc = of_property_read_string(dev->device->of_node, "interrupt-names",
+				     &int_name);
+	if (rc < 0)
+		int_name = NULL;
+
+	/* Get the interrupt number */
+	dev->irq = platform_get_irq(pdev, 0);
+
+	/* disable the bsc interrupt line */
+	brcmstb_i2c_enable_disable_irq(dev, INT_DISABLE);
+
+	/* register the ISR handler */
+	rc = devm_request_irq(&pdev->dev, dev->irq, brcmstb_i2c_isr,
+			      IRQF_SHARED,
+			      int_name ? int_name : pdev->name,
+			      dev);
+
+	if (rc) {
+		dev_dbg(dev->device, "falling back to polling mode");
+		dev->irq = -1;
+	}
+
+	if (of_property_read_u32(dev->device->of_node,
+				 "clock-frequency", &dev->clk_freq_hz)) {
+		dev_warn(dev->device, "setting clock-frequency@%dHz\n",
+			 bsc_clk[0].hz);
+		dev->clk_freq_hz = bsc_clk[0].hz;
+	}
+
+	brcmstb_i2c_set_bsc_reg_defaults(dev);
+
+	/* Add the i2c adapter */
+	adap = &dev->adapter;
+	i2c_set_adapdata(adap, dev);
+	adap->owner = THIS_MODULE;
+	strlcpy(adap->name, "Broadcom STB : ", sizeof(adap->name));
+	if (int_name)
+		strlcat(adap->name, int_name, sizeof(adap->name));
+	adap->algo = &brcmstb_i2c_algo;
+	adap->dev.parent = &pdev->dev;
+	adap->dev.of_node = pdev->dev.of_node;
+	rc = i2c_add_adapter(adap);
+	if (rc) {
+		dev_err(dev->device, "failed to add adapter\n");
+		goto probe_errorout;
+	}
+
+	dev_info(dev->device, "%s@%dhz registered in %s mode\n",
+		 int_name ? int_name : " ", dev->clk_freq_hz,
+		 (dev->irq >= 0) ? "interrupt" : "polling");
+
+	return 0;
+
+probe_errorout:
+	return rc;
+}
+
+static int brcmstb_i2c_remove(struct platform_device *pdev)
+{
+	struct brcmstb_i2c_dev *dev = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(&dev->adapter);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int brcmstb_i2c_suspend(struct device *dev)
+{
+	struct brcmstb_i2c_dev *i2c_dev = dev_get_drvdata(dev);
+
+	i2c_lock_adapter(&i2c_dev->adapter);
+	i2c_dev->is_suspended = true;
+	i2c_unlock_adapter(&i2c_dev->adapter);
+
+	return 0;
+}
+
+static int brcmstb_i2c_resume(struct device *dev)
+{
+	struct brcmstb_i2c_dev *i2c_dev = dev_get_drvdata(dev);
+
+	i2c_lock_adapter(&i2c_dev->adapter);
+	brcmstb_i2c_set_bsc_reg_defaults(i2c_dev);
+	i2c_dev->is_suspended = false;
+	i2c_unlock_adapter(&i2c_dev->adapter);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(brcmstb_i2c_pm, brcmstb_i2c_suspend,
+			 brcmstb_i2c_resume);
+
+static const struct of_device_id brcmstb_i2c_of_match[] = {
+	{.compatible = "brcm,brcmstb-i2c"},
+	{},
+};
+MODULE_DEVICE_TABLE(of, brcmstb_i2c_of_match);
+
+static struct platform_driver brcmstb_i2c_driver = {
+	.driver = {
+		   .name = "brcmstb-i2c",
+		   .of_match_table = brcmstb_i2c_of_match,
+		   .pm = &brcmstb_i2c_pm,
+		   },
+	.probe = brcmstb_i2c_probe,
+	.remove = brcmstb_i2c_remove,
+};
+module_platform_driver(brcmstb_i2c_driver);
+
+MODULE_AUTHOR("Kamal Dasu <kdasu@broadcom.com>");
+MODULE_DESCRIPTION("Broadcom Settop I2C Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/busses/i2c-cros-ec-tunnel.c b/drivers/i2c/busses/i2c-cros-ec-tunnel.c
index fa8dedd8c3a2..a0d95ff682ae 100644
--- a/drivers/i2c/busses/i2c-cros-ec-tunnel.c
+++ b/drivers/i2c/busses/i2c-cros-ec-tunnel.c
@@ -182,8 +182,9 @@ static int ec_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg i2c_msgs[],
 	const u16 bus_num = bus->remote_bus;
 	int request_len;
 	int response_len;
+	int alloc_size;
 	int result;
-	struct cros_ec_command msg = { };
+	struct cros_ec_command *msg;
 
 	request_len = ec_i2c_count_message(i2c_msgs, num);
 	if (request_len < 0) {
@@ -198,25 +199,39 @@ static int ec_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg i2c_msgs[],
 		return response_len;
 	}
 
-	result = ec_i2c_construct_message(msg.outdata, i2c_msgs, num, bus_num);
-	if (result)
-		return result;
+	alloc_size = max(request_len, response_len);
+	msg = kmalloc(sizeof(*msg) + alloc_size, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
 
-	msg.version = 0;
-	msg.command = EC_CMD_I2C_PASSTHRU;
-	msg.outsize = request_len;
-	msg.insize = response_len;
+	result = ec_i2c_construct_message(msg->data, i2c_msgs, num, bus_num);
+	if (result) {
+		dev_err(dev, "Error constructing EC i2c message %d\n", result);
+		goto exit;
+	}
 
-	result = cros_ec_cmd_xfer(bus->ec, &msg);
-	if (result < 0)
-		return result;
+	msg->version = 0;
+	msg->command = EC_CMD_I2C_PASSTHRU;
+	msg->outsize = request_len;
+	msg->insize = response_len;
 
-	result = ec_i2c_parse_response(msg.indata, i2c_msgs, &num);
-	if (result < 0)
-		return result;
+	result = cros_ec_cmd_xfer(bus->ec, msg);
+	if (result < 0) {
+		dev_err(dev, "Error transferring EC i2c message %d\n", result);
+		goto exit;
+	}
+
+	result = ec_i2c_parse_response(msg->data, i2c_msgs, &num);
+	if (result < 0) {
+		dev_err(dev, "Error parsing EC i2c message %d\n", result);
+		goto exit;
+	}
 
 	/* Indicate success by saying how many messages were sent */
-	return num;
+	result = num;
+exit:
+	kfree(msg);
+	return result;
 }
 
 static u32 ec_i2c_functionality(struct i2c_adapter *adap)
diff --git a/drivers/i2c/busses/i2c-davinci.c b/drivers/i2c/busses/i2c-davinci.c
index 4788a32afb86..3fbb9a035532 100644
--- a/drivers/i2c/busses/i2c-davinci.c
+++ b/drivers/i2c/busses/i2c-davinci.c
@@ -41,8 +41,8 @@
 
 #define DAVINCI_I2C_TIMEOUT	(1*HZ)
 #define DAVINCI_I2C_MAX_TRIES	2
-#define I2C_DAVINCI_INTR_ALL    (DAVINCI_I2C_IMR_AAS | \
-				 DAVINCI_I2C_IMR_SCD | \
+#define DAVINCI_I2C_OWN_ADDRESS	0x08
+#define I2C_DAVINCI_INTR_ALL    (DAVINCI_I2C_IMR_SCD | \
 				 DAVINCI_I2C_IMR_ARDY | \
 				 DAVINCI_I2C_IMR_NACK | \
 				 DAVINCI_I2C_IMR_AL)
@@ -204,9 +204,30 @@ static void i2c_davinci_calc_clk_dividers(struct davinci_i2c_dev *dev)
 		psc++;	/* better to run under spec than over */
 	d = (psc >= 2) ? 5 : 7 - psc;
 
-	clk = ((input_clock / (psc + 1)) / (pdata->bus_freq * 1000)) - (d << 1);
-	clkh = clk >> 1;
-	clkl = clk - clkh;
+	clk = ((input_clock / (psc + 1)) / (pdata->bus_freq * 1000));
+	/* Avoid driving the bus too fast because of rounding errors above */
+	if (input_clock / (psc + 1) / clk > pdata->bus_freq * 1000)
+		clk++;
+	/*
+	 * According to I2C-BUS Spec 2.1, in FAST-MODE LOW period should be at
+	 * least 1.3uS, which is not the case with 50% duty cycle. Driving HIGH
+	 * to LOW ratio as 1 to 2 is more safe.
+	 */
+	if (pdata->bus_freq > 100)
+		clkl = (clk << 1) / 3;
+	else
+		clkl = (clk >> 1);
+	/*
+	 * It's not always possible to have 1 to 2 ratio when d=7, so fall back
+	 * to minimal possible clkh in this case.
+	 */
+	if (clk >= clkl + d) {
+		clkh = clk - clkl - d;
+		clkl -= d;
+	} else {
+		clkh = 0;
+		clkl = clk - (d << 1);
+	}
 
 	davinci_i2c_write_reg(dev, DAVINCI_I2C_PSC_REG, psc);
 	davinci_i2c_write_reg(dev, DAVINCI_I2C_CLKH_REG, clkh);
@@ -233,7 +254,7 @@ static int i2c_davinci_init(struct davinci_i2c_dev *dev)
 	/* Respond at reserved "SMBus Host" slave address" (and zero);
 	 * we seem to have no option to not respond...
 	 */
-	davinci_i2c_write_reg(dev, DAVINCI_I2C_OAR_REG, 0x08);
+	davinci_i2c_write_reg(dev, DAVINCI_I2C_OAR_REG, DAVINCI_I2C_OWN_ADDRESS);
 
 	dev_dbg(dev->dev, "PSC  = %d\n",
 		davinci_i2c_read_reg(dev, DAVINCI_I2C_PSC_REG));
@@ -350,29 +371,25 @@ static struct i2c_bus_recovery_info davinci_i2c_scl_recovery_info = {
 /*
  * Waiting for bus not busy
  */
-static int i2c_davinci_wait_bus_not_busy(struct davinci_i2c_dev *dev,
-					 char allow_sleep)
+static int i2c_davinci_wait_bus_not_busy(struct davinci_i2c_dev *dev)
 {
-	unsigned long timeout;
-	static u16 to_cnt;
-
-	timeout = jiffies + dev->adapter.timeout;
-	while (davinci_i2c_read_reg(dev, DAVINCI_I2C_STR_REG)
-	       & DAVINCI_I2C_STR_BB) {
-		if (to_cnt <= DAVINCI_I2C_MAX_TRIES) {
-			if (time_after(jiffies, timeout)) {
-				dev_warn(dev->dev,
-				"timeout waiting for bus ready\n");
-				to_cnt++;
-				return -ETIMEDOUT;
-			} else {
-				to_cnt = 0;
-				i2c_recover_bus(&dev->adapter);
-			}
-		}
-		if (allow_sleep)
-			schedule_timeout(1);
-	}
+	unsigned long timeout = jiffies + dev->adapter.timeout;
+
+	do {
+		if (!(davinci_i2c_read_reg(dev, DAVINCI_I2C_STR_REG) & DAVINCI_I2C_STR_BB))
+			return 0;
+		schedule_timeout_uninterruptible(1);
+	} while (time_before_eq(jiffies, timeout));
+
+	dev_warn(dev->dev, "timeout waiting for bus ready\n");
+	i2c_recover_bus(&dev->adapter);
+
+	/*
+	 * if bus is still "busy" here, it's most probably a HW problem like
+	 * short-circuit
+	 */
+	if (davinci_i2c_read_reg(dev, DAVINCI_I2C_STR_REG) & DAVINCI_I2C_STR_BB)
+		return -EIO;
 
 	return 0;
 }
@@ -390,6 +407,11 @@ i2c_davinci_xfer_msg(struct i2c_adapter *adap, struct i2c_msg *msg, int stop)
 	u16 w;
 	unsigned long time_left;
 
+	if (msg->addr == DAVINCI_I2C_OWN_ADDRESS) {
+		dev_warn(dev->dev, "transfer to own address aborted\n");
+		return -EADDRNOTAVAIL;
+	}
+
 	/* Introduce a delay, required for some boards (e.g Davinci EVM) */
 	if (pdata->bus_delay)
 		udelay(pdata->bus_delay);
@@ -505,7 +527,7 @@ i2c_davinci_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
 
 	dev_dbg(dev->dev, "%s: msgs: %d\n", __func__, num);
 
-	ret = i2c_davinci_wait_bus_not_busy(dev, 1);
+	ret = i2c_davinci_wait_bus_not_busy(dev);
 	if (ret < 0) {
 		dev_warn(dev->dev, "timeout waiting for bus ready\n");
 		return ret;
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index 0a80e4aabaed..3dd2de31a2f8 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -281,7 +281,8 @@ static int dw_i2c_remove(struct platform_device *pdev)
 
 	i2c_dw_disable(dev);
 
-	pm_runtime_put(&pdev->dev);
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
+	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 
 	if (has_acpi_companion(&pdev->dev))
@@ -298,6 +299,22 @@ static const struct of_device_id dw_i2c_of_match[] = {
 MODULE_DEVICE_TABLE(of, dw_i2c_of_match);
 #endif
 
+#ifdef CONFIG_PM_SLEEP
+static int dw_i2c_prepare(struct device *dev)
+{
+	return pm_runtime_suspended(dev);
+}
+
+static void dw_i2c_complete(struct device *dev)
+{
+	if (dev->power.direct_complete)
+		pm_request_resume(dev);
+}
+#else
+#define dw_i2c_prepare	NULL
+#define dw_i2c_complete	NULL
+#endif
+
 #ifdef CONFIG_PM
 static int dw_i2c_suspend(struct device *dev)
 {
@@ -322,10 +339,18 @@ static int dw_i2c_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
-static UNIVERSAL_DEV_PM_OPS(dw_i2c_dev_pm_ops, dw_i2c_suspend,
-			    dw_i2c_resume, NULL);
+static const struct dev_pm_ops dw_i2c_dev_pm_ops = {
+	.prepare = dw_i2c_prepare,
+	.complete = dw_i2c_complete,
+	SET_SYSTEM_SLEEP_PM_OPS(dw_i2c_suspend, dw_i2c_resume)
+	SET_RUNTIME_PM_OPS(dw_i2c_suspend, dw_i2c_resume, NULL)
+};
+
+#define DW_I2C_DEV_PMOPS (&dw_i2c_dev_pm_ops)
+#else
+#define DW_I2C_DEV_PMOPS NULL
+#endif
 
 /* work with hotplug and coldplug */
 MODULE_ALIAS("platform:i2c_designware");
@@ -337,7 +362,7 @@ static struct platform_driver dw_i2c_driver = {
 		.name	= "i2c_designware",
 		.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,
+		.pm	= DW_I2C_DEV_PMOPS,
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-hix5hd2.c b/drivers/i2c/busses/i2c-hix5hd2.c
index 8fe78d08e01c..7c6966434ee7 100644
--- a/drivers/i2c/busses/i2c-hix5hd2.c
+++ b/drivers/i2c/busses/i2c-hix5hd2.c
@@ -554,4 +554,4 @@ module_platform_driver(hix5hd2_i2c_driver);
 MODULE_DESCRIPTION("Hix5hd2 I2C Bus driver");
 MODULE_AUTHOR("Wei Yan <sledge.yanwei@huawei.com>");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:i2c-hix5hd2");
+MODULE_ALIAS("platform:hix5hd2-i2c");
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index a53a7dd66945..785aa674a4da 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -241,7 +241,7 @@ static struct imx_i2c_hwdata vf610_i2c_hwdata = {
 
 };
 
-static struct platform_device_id imx_i2c_devtype[] = {
+static const struct platform_device_id imx_i2c_devtype[] = {
 	{
 		.name = "imx1-i2c",
 		.driver_data = (kernel_ulong_t)&imx1_i2c_hwdata,
diff --git a/drivers/i2c/busses/i2c-jz4780.c b/drivers/i2c/busses/i2c-jz4780.c
index 19b2d689a5ef..f325663c27c5 100644
--- a/drivers/i2c/busses/i2c-jz4780.c
+++ b/drivers/i2c/busses/i2c-jz4780.c
@@ -764,12 +764,15 @@ static int jz4780_i2c_probe(struct platform_device *pdev)
 	if (IS_ERR(i2c->clk))
 		return PTR_ERR(i2c->clk);
 
-	clk_prepare_enable(i2c->clk);
+	ret = clk_prepare_enable(i2c->clk);
+	if (ret)
+		return ret;
 
-	if (of_property_read_u32(pdev->dev.of_node, "clock-frequency",
-				 &clk_freq)) {
+	ret = of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+				   &clk_freq);
+	if (ret) {
 		dev_err(&pdev->dev, "clock-frequency not specified in DT");
-		return clk_freq;
+		goto err;
 	}
 
 	i2c->speed = clk_freq / 1000;
@@ -790,10 +793,8 @@ static int jz4780_i2c_probe(struct platform_device *pdev)
 	i2c->irq = platform_get_irq(pdev, 0);
 	ret = devm_request_irq(&pdev->dev, i2c->irq, jz4780_i2c_irq, 0,
 			       dev_name(&pdev->dev), i2c);
-	if (ret) {
-		ret = -ENODEV;
+	if (ret)
 		goto err;
-	}
 
 	ret = i2c_add_adapter(&i2c->adap);
 	if (ret < 0) {
diff --git a/drivers/i2c/busses/i2c-mt65xx.c b/drivers/i2c/busses/i2c-mt65xx.c
new file mode 100644
index 000000000000..9920eef74672
--- /dev/null
+++ b/drivers/i2c/busses/i2c-mt65xx.c
@@ -0,0 +1,731 @@
+/*
+ * Copyright (c) 2014 MediaTek Inc.
+ * Author: Xudong Chen <xudong.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/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.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/kernel.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#define I2C_RS_TRANSFER			(1 << 4)
+#define I2C_HS_NACKERR			(1 << 2)
+#define I2C_ACKERR			(1 << 1)
+#define I2C_TRANSAC_COMP		(1 << 0)
+#define I2C_TRANSAC_START		(1 << 0)
+#define I2C_RS_MUL_CNFG			(1 << 15)
+#define I2C_RS_MUL_TRIG			(1 << 14)
+#define I2C_DCM_DISABLE			0x0000
+#define I2C_IO_CONFIG_OPEN_DRAIN	0x0003
+#define I2C_IO_CONFIG_PUSH_PULL		0x0000
+#define I2C_SOFT_RST			0x0001
+#define I2C_FIFO_ADDR_CLR		0x0001
+#define I2C_DELAY_LEN			0x0002
+#define I2C_ST_START_CON		0x8001
+#define I2C_FS_START_CON		0x1800
+#define I2C_TIME_CLR_VALUE		0x0000
+#define I2C_TIME_DEFAULT_VALUE		0x0003
+#define I2C_FS_TIME_INIT_VALUE		0x1303
+#define I2C_WRRD_TRANAC_VALUE		0x0002
+#define I2C_RD_TRANAC_VALUE		0x0001
+
+#define I2C_DMA_CON_TX			0x0000
+#define I2C_DMA_CON_RX			0x0001
+#define I2C_DMA_START_EN		0x0001
+#define I2C_DMA_INT_FLAG_NONE		0x0000
+#define I2C_DMA_CLR_FLAG		0x0000
+
+#define I2C_DEFAULT_SPEED		100000	/* hz */
+#define MAX_FS_MODE_SPEED		400000
+#define MAX_HS_MODE_SPEED		3400000
+#define MAX_SAMPLE_CNT_DIV		8
+#define MAX_STEP_CNT_DIV		64
+#define MAX_HS_STEP_CNT_DIV		8
+
+#define I2C_CONTROL_RS                  (0x1 << 1)
+#define I2C_CONTROL_DMA_EN              (0x1 << 2)
+#define I2C_CONTROL_CLK_EXT_EN          (0x1 << 3)
+#define I2C_CONTROL_DIR_CHANGE          (0x1 << 4)
+#define I2C_CONTROL_ACKERR_DET_EN       (0x1 << 5)
+#define I2C_CONTROL_TRANSFER_LEN_CHANGE (0x1 << 6)
+#define I2C_CONTROL_WRAPPER             (0x1 << 0)
+
+#define I2C_DRV_NAME		"i2c-mt65xx"
+
+enum DMA_REGS_OFFSET {
+	OFFSET_INT_FLAG = 0x0,
+	OFFSET_INT_EN = 0x04,
+	OFFSET_EN = 0x08,
+	OFFSET_CON = 0x18,
+	OFFSET_TX_MEM_ADDR = 0x1c,
+	OFFSET_RX_MEM_ADDR = 0x20,
+	OFFSET_TX_LEN = 0x24,
+	OFFSET_RX_LEN = 0x28,
+};
+
+enum i2c_trans_st_rs {
+	I2C_TRANS_STOP = 0,
+	I2C_TRANS_REPEATED_START,
+};
+
+enum mtk_trans_op {
+	I2C_MASTER_WR = 1,
+	I2C_MASTER_RD,
+	I2C_MASTER_WRRD,
+};
+
+enum I2C_REGS_OFFSET {
+	OFFSET_DATA_PORT = 0x0,
+	OFFSET_SLAVE_ADDR = 0x04,
+	OFFSET_INTR_MASK = 0x08,
+	OFFSET_INTR_STAT = 0x0c,
+	OFFSET_CONTROL = 0x10,
+	OFFSET_TRANSFER_LEN = 0x14,
+	OFFSET_TRANSAC_LEN = 0x18,
+	OFFSET_DELAY_LEN = 0x1c,
+	OFFSET_TIMING = 0x20,
+	OFFSET_START = 0x24,
+	OFFSET_EXT_CONF = 0x28,
+	OFFSET_FIFO_STAT = 0x30,
+	OFFSET_FIFO_THRESH = 0x34,
+	OFFSET_FIFO_ADDR_CLR = 0x38,
+	OFFSET_IO_CONFIG = 0x40,
+	OFFSET_RSV_DEBUG = 0x44,
+	OFFSET_HS = 0x48,
+	OFFSET_SOFTRESET = 0x50,
+	OFFSET_DCM_EN = 0x54,
+	OFFSET_PATH_DIR = 0x60,
+	OFFSET_DEBUGSTAT = 0x64,
+	OFFSET_DEBUGCTRL = 0x68,
+	OFFSET_TRANSFER_LEN_AUX = 0x6c,
+};
+
+struct mtk_i2c_compatible {
+	const struct i2c_adapter_quirks *quirks;
+	unsigned char pmic_i2c: 1;
+	unsigned char dcm: 1;
+	unsigned char auto_restart: 1;
+};
+
+struct mtk_i2c {
+	struct i2c_adapter adap;	/* i2c host adapter */
+	struct device *dev;
+	struct completion msg_complete;
+
+	/* set in i2c probe */
+	void __iomem *base;		/* i2c base addr */
+	void __iomem *pdmabase;		/* dma base address*/
+	struct clk *clk_main;		/* main clock for i2c bus */
+	struct clk *clk_dma;		/* DMA clock for i2c via DMA */
+	struct clk *clk_pmic;		/* PMIC clock for i2c from PMIC */
+	bool have_pmic;			/* can use i2c pins from PMIC */
+	bool use_push_pull;		/* IO config push-pull mode */
+
+	u16 irq_stat;			/* interrupt status */
+	unsigned int speed_hz;		/* The speed in transfer */
+	enum mtk_trans_op op;
+	u16 timing_reg;
+	u16 high_speed_reg;
+	const struct mtk_i2c_compatible *dev_comp;
+};
+
+static const struct i2c_adapter_quirks mt6577_i2c_quirks = {
+	.flags = I2C_AQ_COMB_WRITE_THEN_READ,
+	.max_num_msgs = 1,
+	.max_write_len = 255,
+	.max_read_len = 255,
+	.max_comb_1st_msg_len = 255,
+	.max_comb_2nd_msg_len = 31,
+};
+
+static const struct i2c_adapter_quirks mt8173_i2c_quirks = {
+	.max_num_msgs = 65535,
+	.max_write_len = 65535,
+	.max_read_len = 65535,
+	.max_comb_1st_msg_len = 65535,
+	.max_comb_2nd_msg_len = 65535,
+};
+
+static const struct mtk_i2c_compatible mt6577_compat = {
+	.quirks = &mt6577_i2c_quirks,
+	.pmic_i2c = 0,
+	.dcm = 1,
+	.auto_restart = 0,
+};
+
+static const struct mtk_i2c_compatible mt6589_compat = {
+	.quirks = &mt6577_i2c_quirks,
+	.pmic_i2c = 1,
+	.dcm = 0,
+	.auto_restart = 0,
+};
+
+static const struct mtk_i2c_compatible mt8173_compat = {
+	.quirks = &mt8173_i2c_quirks,
+	.pmic_i2c = 0,
+	.dcm = 1,
+	.auto_restart = 1,
+};
+
+static const struct of_device_id mtk_i2c_of_match[] = {
+	{ .compatible = "mediatek,mt6577-i2c", .data = &mt6577_compat },
+	{ .compatible = "mediatek,mt6589-i2c", .data = &mt6589_compat },
+	{ .compatible = "mediatek,mt8173-i2c", .data = &mt8173_compat },
+	{}
+};
+MODULE_DEVICE_TABLE(of, mtk_i2c_of_match);
+
+static int mtk_i2c_clock_enable(struct mtk_i2c *i2c)
+{
+	int ret;
+
+	ret = clk_prepare_enable(i2c->clk_dma);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(i2c->clk_main);
+	if (ret)
+		goto err_main;
+
+	if (i2c->have_pmic) {
+		ret = clk_prepare_enable(i2c->clk_pmic);
+		if (ret)
+			goto err_pmic;
+	}
+	return 0;
+
+err_pmic:
+	clk_disable_unprepare(i2c->clk_main);
+err_main:
+	clk_disable_unprepare(i2c->clk_dma);
+
+	return ret;
+}
+
+static void mtk_i2c_clock_disable(struct mtk_i2c *i2c)
+{
+	if (i2c->have_pmic)
+		clk_disable_unprepare(i2c->clk_pmic);
+
+	clk_disable_unprepare(i2c->clk_main);
+	clk_disable_unprepare(i2c->clk_dma);
+}
+
+static void mtk_i2c_init_hw(struct mtk_i2c *i2c)
+{
+	u16 control_reg;
+
+	writew(I2C_SOFT_RST, i2c->base + OFFSET_SOFTRESET);
+
+	/* Set ioconfig */
+	if (i2c->use_push_pull)
+		writew(I2C_IO_CONFIG_PUSH_PULL, i2c->base + OFFSET_IO_CONFIG);
+	else
+		writew(I2C_IO_CONFIG_OPEN_DRAIN, i2c->base + OFFSET_IO_CONFIG);
+
+	if (i2c->dev_comp->dcm)
+		writew(I2C_DCM_DISABLE, i2c->base + OFFSET_DCM_EN);
+
+	writew(i2c->timing_reg, i2c->base + OFFSET_TIMING);
+	writew(i2c->high_speed_reg, i2c->base + OFFSET_HS);
+
+	/* If use i2c pin from PMIC mt6397 side, need set PATH_DIR first */
+	if (i2c->have_pmic)
+		writew(I2C_CONTROL_WRAPPER, i2c->base + OFFSET_PATH_DIR);
+
+	control_reg = I2C_CONTROL_ACKERR_DET_EN |
+		      I2C_CONTROL_CLK_EXT_EN | I2C_CONTROL_DMA_EN;
+	writew(control_reg, i2c->base + OFFSET_CONTROL);
+	writew(I2C_DELAY_LEN, i2c->base + OFFSET_DELAY_LEN);
+}
+
+/*
+ * Calculate i2c port speed
+ *
+ * Hardware design:
+ * i2c_bus_freq = parent_clk / (clock_div * 2 * sample_cnt * step_cnt)
+ * clock_div: fixed in hardware, but may be various in different SoCs
+ *
+ * The calculation want to pick the highest bus frequency that is still
+ * less than or equal to i2c->speed_hz. The calculation try to get
+ * sample_cnt and step_cn
+ */
+static int mtk_i2c_set_speed(struct mtk_i2c *i2c, unsigned int parent_clk,
+			     unsigned int clock_div)
+{
+	unsigned int clk_src;
+	unsigned int step_cnt;
+	unsigned int sample_cnt;
+	unsigned int max_step_cnt;
+	unsigned int target_speed;
+	unsigned int base_sample_cnt = MAX_SAMPLE_CNT_DIV;
+	unsigned int base_step_cnt;
+	unsigned int opt_div;
+	unsigned int best_mul;
+	unsigned int cnt_mul;
+
+	clk_src = parent_clk / clock_div;
+	target_speed = i2c->speed_hz;
+
+	if (target_speed > MAX_HS_MODE_SPEED)
+		target_speed = MAX_HS_MODE_SPEED;
+
+	if (target_speed > MAX_FS_MODE_SPEED)
+		max_step_cnt = MAX_HS_STEP_CNT_DIV;
+	else
+		max_step_cnt = MAX_STEP_CNT_DIV;
+
+	base_step_cnt = max_step_cnt;
+	/* Find the best combination */
+	opt_div = DIV_ROUND_UP(clk_src >> 1, target_speed);
+	best_mul = MAX_SAMPLE_CNT_DIV * max_step_cnt;
+
+	/* Search for the best pair (sample_cnt, step_cnt) with
+	 * 0 < sample_cnt < MAX_SAMPLE_CNT_DIV
+	 * 0 < step_cnt < max_step_cnt
+	 * sample_cnt * step_cnt >= opt_div
+	 * optimizing for sample_cnt * step_cnt being minimal
+	 */
+	for (sample_cnt = 1; sample_cnt <= MAX_SAMPLE_CNT_DIV; sample_cnt++) {
+		step_cnt = DIV_ROUND_UP(opt_div, sample_cnt);
+		cnt_mul = step_cnt * sample_cnt;
+		if (step_cnt > max_step_cnt)
+			continue;
+
+		if (cnt_mul < best_mul) {
+			best_mul = cnt_mul;
+			base_sample_cnt = sample_cnt;
+			base_step_cnt = step_cnt;
+			if (best_mul == opt_div)
+				break;
+		}
+	}
+
+	sample_cnt = base_sample_cnt;
+	step_cnt = base_step_cnt;
+
+	if ((clk_src / (2 * sample_cnt * step_cnt)) > target_speed) {
+		/* In this case, hardware can't support such
+		 * low i2c_bus_freq
+		 */
+		dev_dbg(i2c->dev, "Unsupported speed (%uhz)\n",	target_speed);
+		return -EINVAL;
+	}
+
+	step_cnt--;
+	sample_cnt--;
+
+	if (target_speed > MAX_FS_MODE_SPEED) {
+		/* Set the high speed mode register */
+		i2c->timing_reg = I2C_FS_TIME_INIT_VALUE;
+		i2c->high_speed_reg = I2C_TIME_DEFAULT_VALUE |
+			(sample_cnt << 12) | (step_cnt << 8);
+	} else {
+		i2c->timing_reg = (sample_cnt << 8) | (step_cnt << 0);
+		/* Disable the high speed transaction */
+		i2c->high_speed_reg = I2C_TIME_CLR_VALUE;
+	}
+
+	return 0;
+}
+
+static int mtk_i2c_do_transfer(struct mtk_i2c *i2c, struct i2c_msg *msgs,
+			       int num, int left_num)
+{
+	u16 addr_reg;
+	u16 start_reg;
+	u16 control_reg;
+	u16 restart_flag = 0;
+	dma_addr_t rpaddr = 0;
+	dma_addr_t wpaddr = 0;
+	int ret;
+
+	i2c->irq_stat = 0;
+
+	if (i2c->dev_comp->auto_restart)
+		restart_flag = I2C_RS_TRANSFER;
+
+	reinit_completion(&i2c->msg_complete);
+
+	control_reg = readw(i2c->base + OFFSET_CONTROL) &
+			~(I2C_CONTROL_DIR_CHANGE | I2C_CONTROL_RS);
+	if ((i2c->speed_hz > 400000) || (left_num >= 1))
+		control_reg |= I2C_CONTROL_RS;
+
+	if (i2c->op == I2C_MASTER_WRRD)
+		control_reg |= I2C_CONTROL_DIR_CHANGE | I2C_CONTROL_RS;
+
+	writew(control_reg, i2c->base + OFFSET_CONTROL);
+
+	/* set start condition */
+	if (i2c->speed_hz <= 100000)
+		writew(I2C_ST_START_CON, i2c->base + OFFSET_EXT_CONF);
+	else
+		writew(I2C_FS_START_CON, i2c->base + OFFSET_EXT_CONF);
+
+	addr_reg = msgs->addr << 1;
+	if (i2c->op == I2C_MASTER_RD)
+		addr_reg |= 0x1;
+
+	writew(addr_reg, i2c->base + OFFSET_SLAVE_ADDR);
+
+	/* Clear interrupt status */
+	writew(restart_flag | I2C_HS_NACKERR | I2C_ACKERR |
+	       I2C_TRANSAC_COMP, i2c->base + OFFSET_INTR_STAT);
+	writew(I2C_FIFO_ADDR_CLR, i2c->base + OFFSET_FIFO_ADDR_CLR);
+
+	/* Enable interrupt */
+	writew(restart_flag | I2C_HS_NACKERR | I2C_ACKERR |
+	       I2C_TRANSAC_COMP, i2c->base + OFFSET_INTR_MASK);
+
+	/* Set transfer and transaction len */
+	if (i2c->op == I2C_MASTER_WRRD) {
+		writew(msgs->len | ((msgs + 1)->len) << 8,
+		       i2c->base + OFFSET_TRANSFER_LEN);
+		writew(I2C_WRRD_TRANAC_VALUE, i2c->base + OFFSET_TRANSAC_LEN);
+	} else {
+		writew(msgs->len, i2c->base + OFFSET_TRANSFER_LEN);
+		writew(num, i2c->base + OFFSET_TRANSAC_LEN);
+	}
+
+	/* Prepare buffer data to start transfer */
+	if (i2c->op == I2C_MASTER_RD) {
+		writel(I2C_DMA_INT_FLAG_NONE, i2c->pdmabase + OFFSET_INT_FLAG);
+		writel(I2C_DMA_CON_RX, i2c->pdmabase + OFFSET_CON);
+		rpaddr = dma_map_single(i2c->dev, msgs->buf,
+					msgs->len, DMA_FROM_DEVICE);
+		if (dma_mapping_error(i2c->dev, rpaddr))
+			return -ENOMEM;
+		writel((u32)rpaddr, i2c->pdmabase + OFFSET_RX_MEM_ADDR);
+		writel(msgs->len, i2c->pdmabase + OFFSET_RX_LEN);
+	} else if (i2c->op == I2C_MASTER_WR) {
+		writel(I2C_DMA_INT_FLAG_NONE, i2c->pdmabase + OFFSET_INT_FLAG);
+		writel(I2C_DMA_CON_TX, i2c->pdmabase + OFFSET_CON);
+		wpaddr = dma_map_single(i2c->dev, msgs->buf,
+					msgs->len, DMA_TO_DEVICE);
+		if (dma_mapping_error(i2c->dev, wpaddr))
+			return -ENOMEM;
+		writel((u32)wpaddr, i2c->pdmabase + OFFSET_TX_MEM_ADDR);
+		writel(msgs->len, i2c->pdmabase + OFFSET_TX_LEN);
+	} else {
+		writel(I2C_DMA_CLR_FLAG, i2c->pdmabase + OFFSET_INT_FLAG);
+		writel(I2C_DMA_CLR_FLAG, i2c->pdmabase + OFFSET_CON);
+		wpaddr = dma_map_single(i2c->dev, msgs->buf,
+					msgs->len, DMA_TO_DEVICE);
+		if (dma_mapping_error(i2c->dev, wpaddr))
+			return -ENOMEM;
+		rpaddr = dma_map_single(i2c->dev, (msgs + 1)->buf,
+					(msgs + 1)->len,
+					DMA_FROM_DEVICE);
+		if (dma_mapping_error(i2c->dev, rpaddr)) {
+			dma_unmap_single(i2c->dev, wpaddr,
+					 msgs->len, DMA_TO_DEVICE);
+			return -ENOMEM;
+		}
+		writel((u32)wpaddr, i2c->pdmabase + OFFSET_TX_MEM_ADDR);
+		writel((u32)rpaddr, i2c->pdmabase + OFFSET_RX_MEM_ADDR);
+		writel(msgs->len, i2c->pdmabase + OFFSET_TX_LEN);
+		writel((msgs + 1)->len, i2c->pdmabase + OFFSET_RX_LEN);
+	}
+
+	writel(I2C_DMA_START_EN, i2c->pdmabase + OFFSET_EN);
+
+	if (!i2c->dev_comp->auto_restart) {
+		start_reg = I2C_TRANSAC_START;
+	} else {
+		start_reg = I2C_TRANSAC_START | I2C_RS_MUL_TRIG;
+		if (left_num >= 1)
+			start_reg |= I2C_RS_MUL_CNFG;
+	}
+	writew(start_reg, i2c->base + OFFSET_START);
+
+	ret = wait_for_completion_timeout(&i2c->msg_complete,
+					  i2c->adap.timeout);
+
+	/* Clear interrupt mask */
+	writew(~(restart_flag | I2C_HS_NACKERR | I2C_ACKERR |
+	       I2C_TRANSAC_COMP), i2c->base + OFFSET_INTR_MASK);
+
+	if (i2c->op == I2C_MASTER_WR) {
+		dma_unmap_single(i2c->dev, wpaddr,
+				 msgs->len, DMA_TO_DEVICE);
+	} else if (i2c->op == I2C_MASTER_RD) {
+		dma_unmap_single(i2c->dev, rpaddr,
+				 msgs->len, DMA_FROM_DEVICE);
+	} else {
+		dma_unmap_single(i2c->dev, wpaddr, msgs->len,
+				 DMA_TO_DEVICE);
+		dma_unmap_single(i2c->dev, rpaddr, (msgs + 1)->len,
+				 DMA_FROM_DEVICE);
+	}
+
+	if (ret == 0) {
+		dev_dbg(i2c->dev, "addr: %x, transfer timeout\n", msgs->addr);
+		mtk_i2c_init_hw(i2c);
+		return -ETIMEDOUT;
+	}
+
+	completion_done(&i2c->msg_complete);
+
+	if (i2c->irq_stat & (I2C_HS_NACKERR | I2C_ACKERR)) {
+		dev_dbg(i2c->dev, "addr: %x, transfer ACK error\n", msgs->addr);
+		mtk_i2c_init_hw(i2c);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int mtk_i2c_transfer(struct i2c_adapter *adap,
+			    struct i2c_msg msgs[], int num)
+{
+	int ret;
+	int left_num = num;
+	struct mtk_i2c *i2c = i2c_get_adapdata(adap);
+
+	ret = mtk_i2c_clock_enable(i2c);
+	if (ret)
+		return ret;
+
+	while (left_num--) {
+		if (!msgs->buf) {
+			dev_dbg(i2c->dev, "data buffer is NULL.\n");
+			ret = -EINVAL;
+			goto err_exit;
+		}
+
+		if (msgs->flags & I2C_M_RD)
+			i2c->op = I2C_MASTER_RD;
+		else
+			i2c->op = I2C_MASTER_WR;
+
+		if (!i2c->dev_comp->auto_restart) {
+			if (num > 1) {
+				/* combined two messages into one transaction */
+				i2c->op = I2C_MASTER_WRRD;
+				left_num--;
+			}
+		}
+
+		/* always use DMA mode. */
+		ret = mtk_i2c_do_transfer(i2c, msgs, num, left_num);
+		if (ret < 0)
+			goto err_exit;
+
+		msgs++;
+	}
+	/* the return value is number of executed messages */
+	ret = num;
+
+err_exit:
+	mtk_i2c_clock_disable(i2c);
+	return ret;
+}
+
+static irqreturn_t mtk_i2c_irq(int irqno, void *dev_id)
+{
+	struct mtk_i2c *i2c = dev_id;
+	u16 restart_flag = 0;
+
+	if (i2c->dev_comp->auto_restart)
+		restart_flag = I2C_RS_TRANSFER;
+
+	i2c->irq_stat = readw(i2c->base + OFFSET_INTR_STAT);
+	writew(restart_flag | I2C_HS_NACKERR | I2C_ACKERR
+		| I2C_TRANSAC_COMP, i2c->base + OFFSET_INTR_STAT);
+
+	complete(&i2c->msg_complete);
+
+	return IRQ_HANDLED;
+}
+
+static u32 mtk_i2c_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+static const struct i2c_algorithm mtk_i2c_algorithm = {
+	.master_xfer = mtk_i2c_transfer,
+	.functionality = mtk_i2c_functionality,
+};
+
+static int mtk_i2c_parse_dt(struct device_node *np, struct mtk_i2c *i2c,
+			    unsigned int *clk_src_div)
+{
+	int ret;
+
+	ret = of_property_read_u32(np, "clock-frequency", &i2c->speed_hz);
+	if (ret < 0)
+		i2c->speed_hz = I2C_DEFAULT_SPEED;
+
+	ret = of_property_read_u32(np, "clock-div", clk_src_div);
+	if (ret < 0)
+		return ret;
+
+	if (*clk_src_div == 0)
+		return -EINVAL;
+
+	i2c->have_pmic = of_property_read_bool(np, "mediatek,have-pmic");
+	i2c->use_push_pull =
+		of_property_read_bool(np, "mediatek,use-push-pull");
+
+	return 0;
+}
+
+static int mtk_i2c_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *of_id;
+	int ret = 0;
+	struct mtk_i2c *i2c;
+	struct clk *clk;
+	unsigned int clk_src_div;
+	struct resource *res;
+	int irq;
+
+	i2c = devm_kzalloc(&pdev->dev, sizeof(*i2c), GFP_KERNEL);
+	if (!i2c)
+		return -ENOMEM;
+
+	ret = mtk_i2c_parse_dt(pdev->dev.of_node, i2c, &clk_src_div);
+	if (ret)
+		return -EINVAL;
+
+	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);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	i2c->pdmabase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(i2c->pdmabase))
+		return PTR_ERR(i2c->pdmabase);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0)
+		return irq;
+
+	init_completion(&i2c->msg_complete);
+
+	of_id = of_match_node(mtk_i2c_of_match, pdev->dev.of_node);
+	if (!of_id)
+		return -EINVAL;
+
+	i2c->dev_comp = of_id->data;
+	i2c->adap.dev.of_node = pdev->dev.of_node;
+	i2c->dev = &pdev->dev;
+	i2c->adap.dev.parent = &pdev->dev;
+	i2c->adap.owner = THIS_MODULE;
+	i2c->adap.algo = &mtk_i2c_algorithm;
+	i2c->adap.quirks = i2c->dev_comp->quirks;
+	i2c->adap.timeout = 2 * HZ;
+	i2c->adap.retries = 1;
+
+	if (i2c->have_pmic && !i2c->dev_comp->pmic_i2c)
+		return -EINVAL;
+
+	i2c->clk_main = devm_clk_get(&pdev->dev, "main");
+	if (IS_ERR(i2c->clk_main)) {
+		dev_err(&pdev->dev, "cannot get main clock\n");
+		return PTR_ERR(i2c->clk_main);
+	}
+
+	i2c->clk_dma = devm_clk_get(&pdev->dev, "dma");
+	if (IS_ERR(i2c->clk_dma)) {
+		dev_err(&pdev->dev, "cannot get dma clock\n");
+		return PTR_ERR(i2c->clk_dma);
+	}
+
+	clk = i2c->clk_main;
+	if (i2c->have_pmic) {
+		i2c->clk_pmic = devm_clk_get(&pdev->dev, "pmic");
+		if (IS_ERR(i2c->clk_pmic)) {
+			dev_err(&pdev->dev, "cannot get pmic clock\n");
+			return PTR_ERR(i2c->clk_pmic);
+		}
+		clk = i2c->clk_pmic;
+	}
+
+	strlcpy(i2c->adap.name, I2C_DRV_NAME, sizeof(i2c->adap.name));
+
+	ret = mtk_i2c_set_speed(i2c, clk_get_rate(clk), clk_src_div);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to set the speed.\n");
+		return -EINVAL;
+	}
+
+	ret = mtk_i2c_clock_enable(i2c);
+	if (ret) {
+		dev_err(&pdev->dev, "clock enable failed!\n");
+		return ret;
+	}
+	mtk_i2c_init_hw(i2c);
+	mtk_i2c_clock_disable(i2c);
+
+	ret = devm_request_irq(&pdev->dev, irq, mtk_i2c_irq,
+			       IRQF_TRIGGER_NONE, I2C_DRV_NAME, i2c);
+	if (ret < 0) {
+		dev_err(&pdev->dev,
+			"Request I2C IRQ %d fail\n", irq);
+		return ret;
+	}
+
+	i2c_set_adapdata(&i2c->adap, i2c);
+	ret = i2c_add_adapter(&i2c->adap);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to add i2c bus to i2c core\n");
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, i2c);
+
+	return 0;
+}
+
+static int mtk_i2c_remove(struct platform_device *pdev)
+{
+	struct mtk_i2c *i2c = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(&i2c->adap);
+
+	return 0;
+}
+
+static struct platform_driver mtk_i2c_driver = {
+	.probe = mtk_i2c_probe,
+	.remove = mtk_i2c_remove,
+	.driver = {
+		.name = I2C_DRV_NAME,
+		.of_match_table = of_match_ptr(mtk_i2c_of_match),
+	},
+};
+
+module_platform_driver(mtk_i2c_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MediaTek I2C Bus Driver");
+MODULE_AUTHOR("Xudong Chen <xudong.chen@mediatek.com>");
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 3e84f6c090a5..033846cdf266 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -784,7 +784,7 @@ static int mxs_i2c_get_ofdata(struct mxs_i2c_dev *i2c)
 	return 0;
 }
 
-static struct platform_device_id mxs_i2c_devtype[] = {
+static const struct platform_device_id mxs_i2c_devtype[] = {
 	{
 		.name = "imx23-i2c",
 		.driver_data = MXS_I2C_V1,
diff --git a/drivers/i2c/busses/i2c-octeon.c b/drivers/i2c/busses/i2c-octeon.c
index 6e75e016bffc..32914ab42a19 100644
--- a/drivers/i2c/busses/i2c-octeon.c
+++ b/drivers/i2c/busses/i2c-octeon.c
@@ -200,7 +200,7 @@ static int octeon_i2c_test_iflg(struct octeon_i2c *i2c)
  */
 static int octeon_i2c_wait(struct octeon_i2c *i2c)
 {
-	int result;
+	long result;
 
 	octeon_i2c_int_enable(i2c);
 
@@ -210,10 +210,7 @@ static int octeon_i2c_wait(struct octeon_i2c *i2c)
 
 	octeon_i2c_int_disable(i2c);
 
-	if (result < 0) {
-		dev_dbg(i2c->dev, "%s: wait interrupted\n", __func__);
-		return result;
-	} else if (result == 0) {
+	if (result == 0) {
 		dev_dbg(i2c->dev, "%s: timeout\n", __func__);
 		return -ETIMEDOUT;
 	}
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index 0e894193accf..d1c22e3fdd14 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -38,6 +38,7 @@
 #include <linux/slab.h>
 #include <linux/i2c-omap.h>
 #include <linux/pm_runtime.h>
+#include <linux/pinctrl/consumer.h>
 
 /* I2C controller revisions */
 #define OMAP_I2C_OMAP1_REV_2		0x20
@@ -481,10 +482,8 @@ static int omap_i2c_wait_for_bb(struct omap_i2c_dev *dev)
 
 	timeout = jiffies + OMAP_I2C_TIMEOUT;
 	while (omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG) & OMAP_I2C_STAT_BB) {
-		if (time_after(jiffies, timeout)) {
-			dev_warn(dev->dev, "timeout waiting for bus ready\n");
-			return -ETIMEDOUT;
-		}
+		if (time_after(jiffies, timeout))
+			return i2c_recover_bus(&dev->adapter);
 		msleep(1);
 	}
 
@@ -1209,6 +1208,68 @@ MODULE_DEVICE_TABLE(of, omap_i2c_of_match);
 #define OMAP_I2C_SCHEME_0		0
 #define OMAP_I2C_SCHEME_1		1
 
+static int omap_i2c_get_scl(struct i2c_adapter *adap)
+{
+	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+	u32 reg;
+
+	reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+
+	return reg & OMAP_I2C_SYSTEST_SCL_I_FUNC;
+}
+
+static int omap_i2c_get_sda(struct i2c_adapter *adap)
+{
+	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+	u32 reg;
+
+	reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+
+	return reg & OMAP_I2C_SYSTEST_SDA_I_FUNC;
+}
+
+static void omap_i2c_set_scl(struct i2c_adapter *adap, int val)
+{
+	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+	u32 reg;
+
+	reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+	if (val)
+		reg |= OMAP_I2C_SYSTEST_SCL_O;
+	else
+		reg &= ~OMAP_I2C_SYSTEST_SCL_O;
+	omap_i2c_write_reg(dev, OMAP_I2C_SYSTEST_REG, reg);
+}
+
+static void omap_i2c_prepare_recovery(struct i2c_adapter *adap)
+{
+	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+	u32 reg;
+
+	reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+	reg |= OMAP_I2C_SYSTEST_ST_EN;
+	omap_i2c_write_reg(dev, OMAP_I2C_SYSTEST_REG, reg);
+}
+
+static void omap_i2c_unprepare_recovery(struct i2c_adapter *adap)
+{
+	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+	u32 reg;
+
+	reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+	reg &= ~OMAP_I2C_SYSTEST_ST_EN;
+	omap_i2c_write_reg(dev, OMAP_I2C_SYSTEST_REG, reg);
+}
+
+static struct i2c_bus_recovery_info omap_i2c_bus_recovery_info = {
+	.get_scl		= omap_i2c_get_scl,
+	.get_sda		= omap_i2c_get_sda,
+	.set_scl		= omap_i2c_set_scl,
+	.prepare_recovery	= omap_i2c_prepare_recovery,
+	.unprepare_recovery	= omap_i2c_unprepare_recovery,
+	.recover_bus		= i2c_generic_scl_recovery,
+};
+
 static int
 omap_i2c_probe(struct platform_device *pdev)
 {
@@ -1358,6 +1419,7 @@ omap_i2c_probe(struct platform_device *pdev)
 	adap->algo = &omap_i2c_algo;
 	adap->dev.parent = &pdev->dev;
 	adap->dev.of_node = pdev->dev.of_node;
+	adap->bus_recovery_info = &omap_i2c_bus_recovery_info;
 
 	/* i2c device drivers may be active on return from add_adapter() */
 	adap->nr = pdev->id;
@@ -1423,6 +1485,8 @@ static int omap_i2c_runtime_suspend(struct device *dev)
 		omap_i2c_read_reg(_dev, OMAP_I2C_STAT_REG);
 	}
 
+	pinctrl_pm_select_sleep_state(dev);
+
 	return 0;
 }
 
@@ -1431,6 +1495,8 @@ static int omap_i2c_runtime_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct omap_i2c_dev *_dev = platform_get_drvdata(pdev);
 
+	pinctrl_pm_select_default_state(dev);
+
 	if (!_dev->regs)
 		return 0;
 
diff --git a/drivers/i2c/busses/i2c-parport.c b/drivers/i2c/busses/i2c-parport.c
index a1fac5aa9bae..9b94c3db80ab 100644
--- a/drivers/i2c/busses/i2c-parport.c
+++ b/drivers/i2c/busses/i2c-parport.c
@@ -46,6 +46,9 @@ struct i2c_par {
 
 static LIST_HEAD(adapter_list);
 static DEFINE_MUTEX(adapter_list_lock);
+#define MAX_DEVICE 4
+static int parport[MAX_DEVICE] = {0, -1, -1, -1};
+
 
 /* ----- Low-level parallel port access ----------------------------------- */
 
@@ -163,17 +166,34 @@ static void i2c_parport_irq(void *data)
 static void i2c_parport_attach(struct parport *port)
 {
 	struct i2c_par *adapter;
+	int i;
+	struct pardev_cb i2c_parport_cb;
+
+	for (i = 0; i < MAX_DEVICE; i++) {
+		if (parport[i] == -1)
+			continue;
+		if (port->number == parport[i])
+			break;
+	}
+	if (i == MAX_DEVICE) {
+		pr_debug("i2c-parport: Not using parport%d.\n", port->number);
+		return;
+	}
 
 	adapter = kzalloc(sizeof(struct i2c_par), GFP_KERNEL);
 	if (adapter == NULL) {
 		printk(KERN_ERR "i2c-parport: Failed to kzalloc\n");
 		return;
 	}
+	memset(&i2c_parport_cb, 0, sizeof(i2c_parport_cb));
+	i2c_parport_cb.flags = PARPORT_FLAG_EXCL;
+	i2c_parport_cb.irq_func = i2c_parport_irq;
+	i2c_parport_cb.private = adapter;
 
 	pr_debug("i2c-parport: attaching to %s\n", port->name);
 	parport_disable_irq(port);
-	adapter->pdev = parport_register_device(port, "i2c-parport",
-		NULL, NULL, i2c_parport_irq, PARPORT_FLAG_EXCL, adapter);
+	adapter->pdev = parport_register_dev_model(port, "i2c-parport",
+						   &i2c_parport_cb, i);
 	if (!adapter->pdev) {
 		printk(KERN_ERR "i2c-parport: Unable to register with parport\n");
 		goto err_free;
@@ -267,9 +287,10 @@ static void i2c_parport_detach(struct parport *port)
 }
 
 static struct parport_driver i2c_parport_driver = {
-	.name	= "i2c-parport",
-	.attach	= i2c_parport_attach,
-	.detach	= i2c_parport_detach,
+	.name = "i2c-parport",
+	.match_port = i2c_parport_attach,
+	.detach = i2c_parport_detach,
+	.devmodel = true,
 };
 
 /* ----- Module loading, unloading and information ------------------------ */
@@ -298,5 +319,12 @@ MODULE_AUTHOR("Jean Delvare <jdelvare@suse.de>");
 MODULE_DESCRIPTION("I2C bus over parallel port");
 MODULE_LICENSE("GPL");
 
+module_param_array(parport, int, NULL, 0);
+MODULE_PARM_DESC(parport,
+		 "List of parallel ports to bind to, by index.\n"
+		 " Atmost " __stringify(MAX_DEVICE) " devices are supported.\n"
+		 " Default is one device connected to parport0.\n"
+);
+
 module_init(i2c_parport_init);
 module_exit(i2c_parport_exit);
diff --git a/drivers/i2c/busses/i2c-piix4.c b/drivers/i2c/busses/i2c-piix4.c
index 67cbec6796a0..630bce68bf38 100644
--- a/drivers/i2c/busses/i2c-piix4.c
+++ b/drivers/i2c/busses/i2c-piix4.c
@@ -245,7 +245,7 @@ static int piix4_setup_sb800(struct pci_dev *PIIX4_dev,
 	     PIIX4_dev->device == PCI_DEVICE_ID_AMD_HUDSON2_SMBUS &&
 	     PIIX4_dev->revision >= 0x41) ||
 	    (PIIX4_dev->vendor == PCI_VENDOR_ID_AMD &&
-	     PIIX4_dev->device == 0x790b &&
+	     PIIX4_dev->device == PCI_DEVICE_ID_AMD_KERNCZ_SMBUS &&
 	     PIIX4_dev->revision >= 0x49))
 		smb_en = 0x00;
 	else
@@ -545,7 +545,7 @@ static const struct pci_device_id piix4_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_DEVICE_ID_ATI_IXP400_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_ATI, PCI_DEVICE_ID_ATI_SBX00_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_HUDSON2_SMBUS) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, 0x790b) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_KERNCZ_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_SERVERWORKS,
 		     PCI_DEVICE_ID_SERVERWORKS_OSB4) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_SERVERWORKS,
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index 5a84bea5b845..d8361dada584 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -490,7 +490,8 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 	struct rcar_i2c_priv *priv = i2c_get_adapdata(adap);
 	struct device *dev = rcar_i2c_priv_to_dev(priv);
 	unsigned long flags;
-	int i, ret, timeout;
+	int i, ret;
+	long timeout;
 
 	pm_runtime_get_sync(dev);
 
@@ -532,7 +533,7 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 
 		timeout = wait_event_timeout(priv->wait,
 					     rcar_i2c_flags_has(priv, ID_DONE),
-					     5 * HZ);
+					     adap->timeout);
 		if (!timeout) {
 			ret = -ETIMEDOUT;
 			break;
@@ -604,7 +605,8 @@ static int rcar_unreg_slave(struct i2c_client *slave)
 static u32 rcar_i2c_func(struct i2c_adapter *adap)
 {
 	/* This HW can't do SMBUS_QUICK and NOSTART */
-	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
+	return I2C_FUNC_I2C | I2C_FUNC_SLAVE |
+		(I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
 }
 
 static const struct i2c_algorithm rcar_i2c_algo = {
@@ -713,7 +715,7 @@ static int rcar_i2c_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id rcar_i2c_id_table[] = {
+static const struct platform_device_id rcar_i2c_id_table[] = {
 	{ "i2c-rcar",		I2C_RCAR_GEN1 },
 	{ "i2c-rcar_gen1",	I2C_RCAR_GEN1 },
 	{ "i2c-rcar_gen2",	I2C_RCAR_GEN2 },
diff --git a/drivers/i2c/busses/i2c-rk3x.c b/drivers/i2c/busses/i2c-rk3x.c
index 019d5426fe52..72e97e306bd9 100644
--- a/drivers/i2c/busses/i2c-rk3x.c
+++ b/drivers/i2c/busses/i2c-rk3x.c
@@ -72,7 +72,7 @@ enum {
 #define REG_INT_ALL       0x7f
 
 /* Constants */
-#define WAIT_TIMEOUT      200 /* ms */
+#define WAIT_TIMEOUT      1000 /* ms */
 #define DEFAULT_SCL_RATE  (100 * 1000) /* Hz */
 
 enum rk3x_i2c_state {
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index 958c8db4ec30..50bfd8cef5f2 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -132,7 +132,7 @@ struct s3c24xx_i2c {
 	unsigned int		sys_i2c_cfg;
 };
 
-static struct platform_device_id s3c24xx_driver_ids[] = {
+static const struct platform_device_id s3c24xx_driver_ids[] = {
 	{
 		.name		= "s3c2410-i2c",
 		.driver_data	= 0,
@@ -1143,6 +1143,7 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	i2c->quirks = s3c24xx_get_device_quirks(pdev);
+	i2c->sysreg = ERR_PTR(-ENOENT);
 	if (pdata)
 		memcpy(i2c->pdata, pdata, sizeof(*pdata));
 	else
diff --git a/drivers/i2c/busses/i2c-sh_mobile.c b/drivers/i2c/busses/i2c-sh_mobile.c
index 007818b3e174..47659a925e09 100644
--- a/drivers/i2c/busses/i2c-sh_mobile.c
+++ b/drivers/i2c/busses/i2c-sh_mobile.c
@@ -150,6 +150,7 @@ struct sh_mobile_i2c_data {
 
 struct sh_mobile_dt_config {
 	int clks_per_count;
+	void (*setup)(struct sh_mobile_i2c_data *pd);
 };
 
 #define IIC_FLAG_HAS_ICIC67	(1 << 0)
@@ -164,6 +165,7 @@ struct sh_mobile_dt_config {
 #define ICIC			0x0c
 #define ICCL			0x10
 #define ICCH			0x14
+#define ICSTART			0x70
 
 /* Register bits */
 #define ICCR_ICE		0x80
@@ -190,6 +192,8 @@ struct sh_mobile_dt_config {
 #define ICIC_WAITE		0x02
 #define ICIC_DTEE		0x01
 
+#define ICSTART_ICSTART		0x10
+
 static void iic_wr(struct sh_mobile_i2c_data *pd, int offs, unsigned char data)
 {
 	if (offs == ICIC)
@@ -726,7 +730,8 @@ static int sh_mobile_i2c_xfer(struct i2c_adapter *adapter,
 	struct sh_mobile_i2c_data *pd = i2c_get_adapdata(adapter);
 	struct i2c_msg	*msg;
 	int err = 0;
-	int i, k;
+	int i;
+	long timeout;
 
 	activate_ch(pd);
 
@@ -745,10 +750,10 @@ static int sh_mobile_i2c_xfer(struct i2c_adapter *adapter,
 			i2c_op(pd, OP_START, 0);
 
 		/* The interrupt handler takes care of the rest... */
-		k = wait_event_timeout(pd->wait,
+		timeout = wait_event_timeout(pd->wait,
 				       pd->sr & (ICSR_TACK | SW_DONE),
-				       5 * HZ);
-		if (!k) {
+				       adapter->timeout);
+		if (!timeout) {
 			dev_err(pd->dev, "Transfer request timed out\n");
 			if (pd->dma_direction != DMA_NONE)
 				sh_mobile_i2c_cleanup_dma(pd);
@@ -782,6 +787,33 @@ static struct i2c_algorithm sh_mobile_i2c_algorithm = {
 	.master_xfer	= sh_mobile_i2c_xfer,
 };
 
+/*
+ * r8a7740 chip has lasting errata on I2C I/O pad reset.
+ * this is work-around for it.
+ */
+static void sh_mobile_i2c_r8a7740_workaround(struct sh_mobile_i2c_data *pd)
+{
+	iic_set_clr(pd, ICCR, ICCR_ICE, 0);
+	iic_rd(pd, ICCR); /* dummy read */
+
+	iic_set_clr(pd, ICSTART, ICSTART_ICSTART, 0);
+	iic_rd(pd, ICSTART); /* dummy read */
+
+	udelay(10);
+
+	iic_wr(pd, ICCR, ICCR_SCP);
+	iic_wr(pd, ICSTART, 0);
+
+	udelay(10);
+
+	iic_wr(pd, ICCR, ICCR_TRS);
+	udelay(10);
+	iic_wr(pd, ICCR, 0);
+	udelay(10);
+	iic_wr(pd, ICCR, ICCR_TRS);
+	udelay(10);
+}
+
 static const struct sh_mobile_dt_config default_dt_config = {
 	.clks_per_count = 1,
 };
@@ -790,9 +822,15 @@ static const struct sh_mobile_dt_config fast_clock_dt_config = {
 	.clks_per_count = 2,
 };
 
+static const struct sh_mobile_dt_config r8a7740_dt_config = {
+	.clks_per_count = 1,
+	.setup = sh_mobile_i2c_r8a7740_workaround,
+};
+
 static const struct of_device_id sh_mobile_i2c_dt_ids[] = {
 	{ .compatible = "renesas,rmobile-iic", .data = &default_dt_config },
 	{ .compatible = "renesas,iic-r8a73a4", .data = &fast_clock_dt_config },
+	{ .compatible = "renesas,iic-r8a7740", .data = &r8a7740_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 },
@@ -885,6 +923,9 @@ static int sh_mobile_i2c_probe(struct platform_device *dev)
 
 			config = match->data;
 			pd->clks_per_count = config->clks_per_count;
+
+			if (config->setup)
+				config->setup(pd);
 		}
 	} else {
 		if (pdata && pdata->bus_speed)
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index 1bcd75ea0b4c..78a366814696 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -656,8 +656,8 @@ static int tegra_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
 static u32 tegra_i2c_func(struct i2c_adapter *adap)
 {
 	struct tegra_i2c_dev *i2c_dev = i2c_get_adapdata(adap);
-	u32 ret = I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_10BIT_ADDR |
-				I2C_FUNC_PROTOCOL_MANGLING;
+	u32 ret = I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK) |
+		  I2C_FUNC_10BIT_ADDR |	I2C_FUNC_PROTOCOL_MANGLING;
 
 	if (i2c_dev->hw->has_continue_xfer_support)
 		ret |= I2C_FUNC_NOSTART;
@@ -669,6 +669,12 @@ static const struct i2c_algorithm tegra_i2c_algo = {
 	.functionality	= tegra_i2c_func,
 };
 
+/* payload size is only 12 bit */
+static struct i2c_adapter_quirks tegra_i2c_quirks = {
+	.max_read_len = 4096,
+	.max_write_len = 4096,
+};
+
 static const struct tegra_i2c_hw_feature tegra20_i2c_hw = {
 	.has_continue_xfer_support = false,
 	.has_per_pkt_xfer_complete_irq = false,
@@ -739,6 +745,7 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 	i2c_dev->base = base;
 	i2c_dev->div_clk = div_clk;
 	i2c_dev->adapter.algo = &tegra_i2c_algo;
+	i2c_dev->adapter.quirks = &tegra_i2c_quirks;
 	i2c_dev->irq = irq;
 	i2c_dev->cont_id = pdev->id;
 	i2c_dev->dev = &pdev->dev;
diff --git a/drivers/i2c/busses/i2c-xgene-slimpro.c b/drivers/i2c/busses/i2c-xgene-slimpro.c
new file mode 100644
index 000000000000..1c9cb65ac4cf
--- /dev/null
+++ b/drivers/i2c/busses/i2c-xgene-slimpro.c
@@ -0,0 +1,470 @@
+/*
+ * X-Gene SLIMpro I2C Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Author: Feng Kan <fkan@apm.com>
+ * Author: Hieu Le <hnle@apm.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * This driver provides support for X-Gene SLIMpro I2C device access
+ * using the APM X-Gene SLIMpro mailbox driver.
+ *
+ */
+#include <linux/acpi.h>
+#include <linux/dma-mapping.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/mailbox_client.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/version.h>
+
+#define MAILBOX_OP_TIMEOUT		1000	/* Operation time out in ms */
+#define MAILBOX_I2C_INDEX		0
+#define SLIMPRO_IIC_BUS			1	/* Use I2C bus 1 only */
+
+#define SMBUS_CMD_LEN			1
+#define BYTE_DATA			1
+#define WORD_DATA			2
+#define BLOCK_DATA			3
+
+#define SLIMPRO_IIC_I2C_PROTOCOL	0
+#define SLIMPRO_IIC_SMB_PROTOCOL	1
+
+#define SLIMPRO_IIC_READ		0
+#define SLIMPRO_IIC_WRITE		1
+
+#define IIC_SMB_WITHOUT_DATA_LEN	0
+#define IIC_SMB_WITH_DATA_LEN		1
+
+#define SLIMPRO_DEBUG_MSG		0
+#define SLIMPRO_MSG_TYPE_SHIFT		28
+#define SLIMPRO_DBG_SUBTYPE_I2C1READ	4
+#define SLIMPRO_DBGMSG_TYPE_SHIFT	24
+#define SLIMPRO_DBGMSG_TYPE_MASK	0x0F000000U
+#define SLIMPRO_IIC_DEV_SHIFT		23
+#define SLIMPRO_IIC_DEV_MASK		0x00800000U
+#define SLIMPRO_IIC_DEVID_SHIFT		13
+#define SLIMPRO_IIC_DEVID_MASK		0x007FE000U
+#define SLIMPRO_IIC_RW_SHIFT		12
+#define SLIMPRO_IIC_RW_MASK		0x00001000U
+#define SLIMPRO_IIC_PROTO_SHIFT		11
+#define SLIMPRO_IIC_PROTO_MASK		0x00000800U
+#define SLIMPRO_IIC_ADDRLEN_SHIFT	8
+#define SLIMPRO_IIC_ADDRLEN_MASK	0x00000700U
+#define SLIMPRO_IIC_DATALEN_SHIFT	0
+#define SLIMPRO_IIC_DATALEN_MASK	0x000000FFU
+
+/*
+ * SLIMpro I2C message encode
+ *
+ * dev		- Controller number (0-based)
+ * chip		- I2C chip address
+ * op		- SLIMPRO_IIC_READ or SLIMPRO_IIC_WRITE
+ * proto	- SLIMPRO_IIC_SMB_PROTOCOL or SLIMPRO_IIC_I2C_PROTOCOL
+ * addrlen	- Length of the address field
+ * datalen	- Length of the data field
+ */
+#define SLIMPRO_IIC_ENCODE_MSG(dev, chip, op, proto, addrlen, datalen) \
+	((SLIMPRO_DEBUG_MSG << SLIMPRO_MSG_TYPE_SHIFT) | \
+	((SLIMPRO_DBG_SUBTYPE_I2C1READ << SLIMPRO_DBGMSG_TYPE_SHIFT) & \
+	SLIMPRO_DBGMSG_TYPE_MASK) | \
+	((dev << SLIMPRO_IIC_DEV_SHIFT) & SLIMPRO_IIC_DEV_MASK) | \
+	((chip << SLIMPRO_IIC_DEVID_SHIFT) & SLIMPRO_IIC_DEVID_MASK) | \
+	((op << SLIMPRO_IIC_RW_SHIFT) & SLIMPRO_IIC_RW_MASK) | \
+	((proto << SLIMPRO_IIC_PROTO_SHIFT) & SLIMPRO_IIC_PROTO_MASK) | \
+	((addrlen << SLIMPRO_IIC_ADDRLEN_SHIFT) & SLIMPRO_IIC_ADDRLEN_MASK) | \
+	((datalen << SLIMPRO_IIC_DATALEN_SHIFT) & SLIMPRO_IIC_DATALEN_MASK))
+
+/*
+ * Encode for upper address for block data
+ */
+#define SLIMPRO_IIC_ENCODE_FLAG_BUFADDR			0x80000000
+#define SLIMPRO_IIC_ENCODE_FLAG_WITH_DATA_LEN(a)	((u32) (((a) << 30) \
+								& 0x40000000))
+#define SLIMPRO_IIC_ENCODE_UPPER_BUFADDR(a)		((u32) (((a) >> 12) \
+								& 0x3FF00000))
+#define SLIMPRO_IIC_ENCODE_ADDR(a)			((a) & 0x000FFFFF)
+
+struct slimpro_i2c_dev {
+	struct i2c_adapter adapter;
+	struct device *dev;
+	struct mbox_chan *mbox_chan;
+	struct mbox_client mbox_client;
+	struct completion rd_complete;
+	u8 dma_buffer[I2C_SMBUS_BLOCK_MAX];
+	u32 *resp_msg;
+};
+
+#define to_slimpro_i2c_dev(cl)	\
+		container_of(cl, struct slimpro_i2c_dev, mbox_client)
+
+static void slimpro_i2c_rx_cb(struct mbox_client *cl, void *mssg)
+{
+	struct slimpro_i2c_dev *ctx = to_slimpro_i2c_dev(cl);
+
+	/*
+	 * Response message format:
+	 * mssg[0] is the return code of the operation
+	 * mssg[1] is the first data word
+	 * mssg[2] is NOT used
+	 */
+	if (ctx->resp_msg)
+		*ctx->resp_msg = ((u32 *)mssg)[1];
+
+	if (ctx->mbox_client.tx_block)
+		complete(&ctx->rd_complete);
+}
+
+static int start_i2c_msg_xfer(struct slimpro_i2c_dev *ctx)
+{
+	if (ctx->mbox_client.tx_block) {
+		if (!wait_for_completion_timeout(&ctx->rd_complete,
+						 msecs_to_jiffies(MAILBOX_OP_TIMEOUT)))
+			return -ETIMEDOUT;
+	}
+
+	/* Check of invalid data or no device */
+	if (*ctx->resp_msg == 0xffffffff)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int slimpro_i2c_rd(struct slimpro_i2c_dev *ctx, u32 chip,
+			  u32 addr, u32 addrlen, u32 protocol,
+			  u32 readlen, u32 *data)
+{
+	u32 msg[3];
+	int rc;
+
+	msg[0] = SLIMPRO_IIC_ENCODE_MSG(SLIMPRO_IIC_BUS, chip,
+					SLIMPRO_IIC_READ, protocol, addrlen, readlen);
+	msg[1] = SLIMPRO_IIC_ENCODE_ADDR(addr);
+	msg[2] = 0;
+	ctx->resp_msg = data;
+	rc = mbox_send_message(ctx->mbox_chan, &msg);
+	if (rc < 0)
+		goto err;
+
+	rc = start_i2c_msg_xfer(ctx);
+err:
+	ctx->resp_msg = NULL;
+	return rc;
+}
+
+static int slimpro_i2c_wr(struct slimpro_i2c_dev *ctx, u32 chip,
+			  u32 addr, u32 addrlen, u32 protocol, u32 writelen,
+			  u32 data)
+{
+	u32 msg[3];
+	int rc;
+
+	msg[0] = SLIMPRO_IIC_ENCODE_MSG(SLIMPRO_IIC_BUS, chip,
+					SLIMPRO_IIC_WRITE, protocol, addrlen, writelen);
+	msg[1] = SLIMPRO_IIC_ENCODE_ADDR(addr);
+	msg[2] = data;
+	ctx->resp_msg = msg;
+
+	rc = mbox_send_message(ctx->mbox_chan, &msg);
+	if (rc < 0)
+		goto err;
+
+	rc = start_i2c_msg_xfer(ctx);
+err:
+	ctx->resp_msg = NULL;
+	return rc;
+}
+
+static int slimpro_i2c_blkrd(struct slimpro_i2c_dev *ctx, u32 chip, u32 addr,
+			     u32 addrlen, u32 protocol, u32 readlen,
+			     u32 with_data_len, void *data)
+{
+	dma_addr_t paddr;
+	u32 msg[3];
+	int rc;
+
+	paddr = dma_map_single(ctx->dev, ctx->dma_buffer, readlen, DMA_FROM_DEVICE);
+	rc = dma_mapping_error(ctx->dev, paddr);
+	if (rc) {
+		dev_err(&ctx->adapter.dev, "Error in mapping dma buffer %p\n",
+			ctx->dma_buffer);
+		goto err;
+	}
+
+	msg[0] = SLIMPRO_IIC_ENCODE_MSG(SLIMPRO_IIC_BUS, chip, SLIMPRO_IIC_READ,
+					protocol, addrlen, readlen);
+	msg[1] = SLIMPRO_IIC_ENCODE_FLAG_BUFADDR |
+		 SLIMPRO_IIC_ENCODE_FLAG_WITH_DATA_LEN(with_data_len) |
+		 SLIMPRO_IIC_ENCODE_UPPER_BUFADDR(paddr) |
+		 SLIMPRO_IIC_ENCODE_ADDR(addr);
+	msg[2] = (u32)paddr;
+	ctx->resp_msg = msg;
+
+	rc = mbox_send_message(ctx->mbox_chan, &msg);
+	if (rc < 0)
+		goto err_unmap;
+
+	rc = start_i2c_msg_xfer(ctx);
+
+	/* Copy to destination */
+	memcpy(data, ctx->dma_buffer, readlen);
+
+err_unmap:
+	dma_unmap_single(ctx->dev, paddr, readlen, DMA_FROM_DEVICE);
+err:
+	ctx->resp_msg = NULL;
+	return rc;
+}
+
+static int slimpro_i2c_blkwr(struct slimpro_i2c_dev *ctx, u32 chip,
+			     u32 addr, u32 addrlen, u32 protocol, u32 writelen,
+			     void *data)
+{
+	dma_addr_t paddr;
+	u32 msg[3];
+	int rc;
+
+	memcpy(ctx->dma_buffer, data, writelen);
+	paddr = dma_map_single(ctx->dev, ctx->dma_buffer, writelen,
+			       DMA_TO_DEVICE);
+	rc = dma_mapping_error(ctx->dev, paddr);
+	if (rc) {
+		dev_err(&ctx->adapter.dev, "Error in mapping dma buffer %p\n",
+			ctx->dma_buffer);
+		goto err;
+	}
+
+	msg[0] = SLIMPRO_IIC_ENCODE_MSG(SLIMPRO_IIC_BUS, chip, SLIMPRO_IIC_WRITE,
+					protocol, addrlen, writelen);
+	msg[1] = SLIMPRO_IIC_ENCODE_FLAG_BUFADDR |
+		 SLIMPRO_IIC_ENCODE_UPPER_BUFADDR(paddr) |
+		 SLIMPRO_IIC_ENCODE_ADDR(addr);
+	msg[2] = (u32)paddr;
+	ctx->resp_msg = msg;
+
+	if (ctx->mbox_client.tx_block)
+		reinit_completion(&ctx->rd_complete);
+
+	rc = mbox_send_message(ctx->mbox_chan, &msg);
+	if (rc < 0)
+		goto err_unmap;
+
+	rc = start_i2c_msg_xfer(ctx);
+
+err_unmap:
+	dma_unmap_single(ctx->dev, paddr, writelen, DMA_TO_DEVICE);
+err:
+	ctx->resp_msg = NULL;
+	return rc;
+}
+
+static int xgene_slimpro_i2c_xfer(struct i2c_adapter *adap, u16 addr,
+				  unsigned short flags, char read_write,
+				  u8 command, int size,
+				  union i2c_smbus_data *data)
+{
+	struct slimpro_i2c_dev *ctx = i2c_get_adapdata(adap);
+	int ret = -EOPNOTSUPP;
+	u32 val;
+
+	switch (size) {
+	case I2C_SMBUS_BYTE:
+		if (read_write == I2C_SMBUS_READ) {
+			ret = slimpro_i2c_rd(ctx, addr, 0, 0,
+					     SLIMPRO_IIC_SMB_PROTOCOL,
+					     BYTE_DATA, &val);
+			data->byte = val;
+		} else {
+			ret = slimpro_i2c_wr(ctx, addr, command, SMBUS_CMD_LEN,
+					     SLIMPRO_IIC_SMB_PROTOCOL,
+					     0, 0);
+		}
+		break;
+	case I2C_SMBUS_BYTE_DATA:
+		if (read_write == I2C_SMBUS_READ) {
+			ret = slimpro_i2c_rd(ctx, addr, command, SMBUS_CMD_LEN,
+					     SLIMPRO_IIC_SMB_PROTOCOL,
+					     BYTE_DATA, &val);
+			data->byte = val;
+		} else {
+			val = data->byte;
+			ret = slimpro_i2c_wr(ctx, addr, command, SMBUS_CMD_LEN,
+					     SLIMPRO_IIC_SMB_PROTOCOL,
+					     BYTE_DATA, val);
+		}
+		break;
+	case I2C_SMBUS_WORD_DATA:
+		if (read_write == I2C_SMBUS_READ) {
+			ret = slimpro_i2c_rd(ctx, addr, command, SMBUS_CMD_LEN,
+					     SLIMPRO_IIC_SMB_PROTOCOL,
+					     WORD_DATA, &val);
+			data->word = val;
+		} else {
+			val = data->word;
+			ret = slimpro_i2c_wr(ctx, addr, command, SMBUS_CMD_LEN,
+					     SLIMPRO_IIC_SMB_PROTOCOL,
+					     WORD_DATA, val);
+		}
+		break;
+	case I2C_SMBUS_BLOCK_DATA:
+		if (read_write == I2C_SMBUS_READ) {
+			ret = slimpro_i2c_blkrd(ctx, addr, command,
+						SMBUS_CMD_LEN,
+						SLIMPRO_IIC_SMB_PROTOCOL,
+						I2C_SMBUS_BLOCK_MAX + 1,
+						IIC_SMB_WITH_DATA_LEN,
+						&data->block[0]);
+
+		} else {
+			ret = slimpro_i2c_blkwr(ctx, addr, command,
+						SMBUS_CMD_LEN,
+						SLIMPRO_IIC_SMB_PROTOCOL,
+						data->block[0] + 1,
+						&data->block[0]);
+		}
+		break;
+	case I2C_SMBUS_I2C_BLOCK_DATA:
+		if (read_write == I2C_SMBUS_READ) {
+			ret = slimpro_i2c_blkrd(ctx, addr,
+						command,
+						SMBUS_CMD_LEN,
+						SLIMPRO_IIC_I2C_PROTOCOL,
+						I2C_SMBUS_BLOCK_MAX,
+						IIC_SMB_WITHOUT_DATA_LEN,
+						&data->block[1]);
+		} else {
+			ret = slimpro_i2c_blkwr(ctx, addr, command,
+						SMBUS_CMD_LEN,
+						SLIMPRO_IIC_I2C_PROTOCOL,
+						data->block[0],
+						&data->block[1]);
+		}
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+/*
+* Return list of supported functionality.
+*/
+static u32 xgene_slimpro_i2c_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_SMBUS_BYTE |
+		I2C_FUNC_SMBUS_BYTE_DATA |
+		I2C_FUNC_SMBUS_WORD_DATA |
+		I2C_FUNC_SMBUS_BLOCK_DATA |
+		I2C_FUNC_SMBUS_I2C_BLOCK;
+}
+
+static struct i2c_algorithm xgene_slimpro_i2c_algorithm = {
+	.smbus_xfer = xgene_slimpro_i2c_xfer,
+	.functionality = xgene_slimpro_i2c_func,
+};
+
+static int xgene_slimpro_i2c_probe(struct platform_device *pdev)
+{
+	struct slimpro_i2c_dev *ctx;
+	struct i2c_adapter *adapter;
+	struct mbox_client *cl;
+	int rc;
+
+	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->dev = &pdev->dev;
+	platform_set_drvdata(pdev, ctx);
+	cl = &ctx->mbox_client;
+
+	/* Request mailbox channel */
+	cl->dev = &pdev->dev;
+	cl->rx_callback = slimpro_i2c_rx_cb;
+	cl->tx_block = true;
+	init_completion(&ctx->rd_complete);
+	cl->tx_tout = MAILBOX_OP_TIMEOUT;
+	cl->knows_txdone = false;
+	ctx->mbox_chan = mbox_request_channel(cl, MAILBOX_I2C_INDEX);
+	if (IS_ERR(ctx->mbox_chan)) {
+		dev_err(&pdev->dev, "i2c mailbox channel request failed\n");
+		return PTR_ERR(ctx->mbox_chan);
+	}
+
+	rc = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64));
+	if (rc)
+		dev_warn(&pdev->dev, "Unable to set dma mask\n");
+
+	/* Setup I2C adapter */
+	adapter = &ctx->adapter;
+	snprintf(adapter->name, sizeof(adapter->name), "MAILBOX I2C");
+	adapter->algo = &xgene_slimpro_i2c_algorithm;
+	adapter->class = I2C_CLASS_HWMON;
+	adapter->dev.parent = &pdev->dev;
+	i2c_set_adapdata(adapter, ctx);
+	rc = i2c_add_adapter(adapter);
+	if (rc) {
+		dev_err(&pdev->dev, "Adapter registeration failed\n");
+		mbox_free_channel(ctx->mbox_chan);
+		return rc;
+	}
+
+	dev_info(&pdev->dev, "Mailbox I2C Adapter registered\n");
+	return 0;
+}
+
+static int xgene_slimpro_i2c_remove(struct platform_device *pdev)
+{
+	struct slimpro_i2c_dev *ctx = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(&ctx->adapter);
+
+	mbox_free_channel(ctx->mbox_chan);
+
+	return 0;
+}
+
+static const struct of_device_id xgene_slimpro_i2c_dt_ids[] = {
+	{.compatible = "apm,xgene-slimpro-i2c" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, xgene_slimpro_i2c_dt_ids);
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id xgene_slimpro_i2c_acpi_ids[] = {
+	{"APMC0D40", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(acpi, xgene_slimpro_i2c_acpi_ids);
+#endif
+
+static struct platform_driver xgene_slimpro_i2c_driver = {
+	.probe	= xgene_slimpro_i2c_probe,
+	.remove	= xgene_slimpro_i2c_remove,
+	.driver	= {
+		.name	= "xgene-slimpro-i2c",
+		.of_match_table = of_match_ptr(xgene_slimpro_i2c_dt_ids),
+		.acpi_match_table = ACPI_PTR(xgene_slimpro_i2c_acpi_ids)
+	},
+};
+
+module_platform_driver(xgene_slimpro_i2c_driver);
+
+MODULE_DESCRIPTION("APM X-Gene SLIMpro I2C driver");
+MODULE_AUTHOR("Feng Kan <fkan@apm.com>");
+MODULE_AUTHOR("Hieu Le <hnle@apm.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/busses/i2c-xiic.c b/drivers/i2c/busses/i2c-xiic.c
index e8400042b358..4dda23f22a67 100644
--- a/drivers/i2c/busses/i2c-xiic.c
+++ b/drivers/i2c/busses/i2c-xiic.c
@@ -63,6 +63,7 @@ enum xiic_endian {
  * @state:	See STATE_
  * @rx_msg:	Current RX message
  * @rx_pos:	Position within current RX message
+ * @endianness: big/little-endian byte order
  */
 struct xiic_i2c {
 	void __iomem		*base;
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index 987c124432c5..e6d4935161e4 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -107,7 +107,7 @@ static int acpi_i2c_add_resource(struct acpi_resource *ares, void *data)
 			if (sb->access_mode == ACPI_I2C_10BIT_MODE)
 				info->flags |= I2C_CLIENT_TEN;
 		}
-	} else if (info->irq < 0) {
+	} else if (!info->irq) {
 		struct resource r;
 
 		if (acpi_dev_resource_interrupt(ares, 0, &r))
@@ -134,7 +134,6 @@ static acpi_status acpi_i2c_add_device(acpi_handle handle, u32 level,
 
 	memset(&info, 0, sizeof(info));
 	info.fwnode = acpi_fwnode_handle(adev);
-	info.irq = -1;
 
 	INIT_LIST_HEAD(&resource_list);
 	ret = acpi_dev_get_resources(adev, &resource_list,
@@ -258,7 +257,7 @@ acpi_i2c_space_handler(u32 function, acpi_physical_address command,
 	struct acpi_connection_info *info = &data->info;
 	struct acpi_resource_i2c_serialbus *sb;
 	struct i2c_adapter *adapter = data->adapter;
-	struct i2c_client client;
+	struct i2c_client *client;
 	struct acpi_resource *ares;
 	u32 accessor_type = function >> 16;
 	u8 action = function & ACPI_IO_MASK;
@@ -269,6 +268,12 @@ acpi_i2c_space_handler(u32 function, acpi_physical_address command,
 	if (ACPI_FAILURE(ret))
 		return ret;
 
+	client = kzalloc(sizeof(*client), GFP_KERNEL);
+	if (!client) {
+		ret = AE_NO_MEMORY;
+		goto err;
+	}
+
 	if (!value64 || ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS) {
 		ret = AE_BAD_PARAMETER;
 		goto err;
@@ -280,75 +285,73 @@ acpi_i2c_space_handler(u32 function, acpi_physical_address command,
 		goto err;
 	}
 
-	memset(&client, 0, sizeof(client));
-	client.adapter = adapter;
-	client.addr = sb->slave_address;
-	client.flags = 0;
+	client->adapter = adapter;
+	client->addr = sb->slave_address;
 
 	if (sb->access_mode == ACPI_I2C_10BIT_MODE)
-		client.flags |= I2C_CLIENT_TEN;
+		client->flags |= I2C_CLIENT_TEN;
 
 	switch (accessor_type) {
 	case ACPI_GSB_ACCESS_ATTRIB_SEND_RCV:
 		if (action == ACPI_READ) {
-			status = i2c_smbus_read_byte(&client);
+			status = i2c_smbus_read_byte(client);
 			if (status >= 0) {
 				gsb->bdata = status;
 				status = 0;
 			}
 		} else {
-			status = i2c_smbus_write_byte(&client, gsb->bdata);
+			status = i2c_smbus_write_byte(client, gsb->bdata);
 		}
 		break;
 
 	case ACPI_GSB_ACCESS_ATTRIB_BYTE:
 		if (action == ACPI_READ) {
-			status = i2c_smbus_read_byte_data(&client, command);
+			status = i2c_smbus_read_byte_data(client, command);
 			if (status >= 0) {
 				gsb->bdata = status;
 				status = 0;
 			}
 		} else {
-			status = i2c_smbus_write_byte_data(&client, command,
+			status = i2c_smbus_write_byte_data(client, command,
 					gsb->bdata);
 		}
 		break;
 
 	case ACPI_GSB_ACCESS_ATTRIB_WORD:
 		if (action == ACPI_READ) {
-			status = i2c_smbus_read_word_data(&client, command);
+			status = i2c_smbus_read_word_data(client, command);
 			if (status >= 0) {
 				gsb->wdata = status;
 				status = 0;
 			}
 		} else {
-			status = i2c_smbus_write_word_data(&client, command,
+			status = i2c_smbus_write_word_data(client, command,
 					gsb->wdata);
 		}
 		break;
 
 	case ACPI_GSB_ACCESS_ATTRIB_BLOCK:
 		if (action == ACPI_READ) {
-			status = i2c_smbus_read_block_data(&client, command,
+			status = i2c_smbus_read_block_data(client, command,
 					gsb->data);
 			if (status >= 0) {
 				gsb->len = status;
 				status = 0;
 			}
 		} else {
-			status = i2c_smbus_write_block_data(&client, command,
+			status = i2c_smbus_write_block_data(client, command,
 					gsb->len, gsb->data);
 		}
 		break;
 
 	case ACPI_GSB_ACCESS_ATTRIB_MULTIBYTE:
 		if (action == ACPI_READ) {
-			status = acpi_gsb_i2c_read_bytes(&client, command,
+			status = acpi_gsb_i2c_read_bytes(client, command,
 					gsb->data, info->access_length);
 			if (status > 0)
 				status = 0;
 		} else {
-			status = acpi_gsb_i2c_write_bytes(&client, command,
+			status = acpi_gsb_i2c_write_bytes(client, command,
 					gsb->data, info->access_length);
 		}
 		break;
@@ -362,6 +365,7 @@ acpi_i2c_space_handler(u32 function, acpi_physical_address command,
 	gsb->status = status;
 
  err:
+	kfree(client);
 	ACPI_FREE(ares);
 	return ret;
 }
@@ -632,8 +636,13 @@ 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 (!client->irq) {
+		int irq = -ENOENT;
+
+		if (dev->of_node)
+			irq = of_irq_get(dev->of_node, 0);
+		else if (ACPI_COMPANION(dev))
+			irq = acpi_dev_gpio_irq_get(ACPI_COMPANION(dev), 0);
 
 		if (irq == -EPROBE_DEFER)
 			return irq;
@@ -1003,6 +1012,8 @@ EXPORT_SYMBOL_GPL(i2c_new_device);
  */
 void i2c_unregister_device(struct i2c_client *client)
 {
+	if (client->dev.of_node)
+		of_node_clear_flag(client->dev.of_node, OF_POPULATED);
 	device_unregister(&client->dev);
 }
 EXPORT_SYMBOL_GPL(i2c_unregister_device);
@@ -1272,7 +1283,7 @@ static struct i2c_client *of_i2c_register_device(struct i2c_adapter *adap,
 	}
 
 	addr = of_get_property(node, "reg", &len);
-	if (!addr || (len < sizeof(int))) {
+	if (!addr || (len < sizeof(*addr))) {
 		dev_err(&adap->dev, "of_i2c: invalid reg on %s\n",
 			node->full_name);
 		return ERR_PTR(-EINVAL);
@@ -1311,8 +1322,11 @@ static void of_i2c_register_devices(struct i2c_adapter *adap)
 
 	dev_dbg(&adap->dev, "of_i2c: walking child nodes\n");
 
-	for_each_available_child_of_node(adap->dev.of_node, node)
+	for_each_available_child_of_node(adap->dev.of_node, node) {
+		if (of_node_test_and_set_flag(node, OF_POPULATED))
+			continue;
 		of_i2c_register_device(adap, node);
+	}
 }
 
 static int of_dev_node_match(struct device *dev, void *data)
@@ -1673,7 +1687,7 @@ void i2c_del_adapter(struct i2c_adapter *adap)
 	 * FIXME: This is old code and should ideally be replaced by an
 	 * alternative which results in decoupling the lifetime of the struct
 	 * device from the i2c_adapter, like spi or netdev do. Any solution
-	 * should be throughly tested with DEBUG_KOBJECT_RELEASE enabled!
+	 * should be thoroughly tested with DEBUG_KOBJECT_RELEASE enabled!
 	 */
 	init_completion(&adap->dev_released);
 	device_unregister(&adap->dev);
@@ -1844,6 +1858,11 @@ static int of_i2c_notify(struct notifier_block *nb, unsigned long action,
 		if (adap == NULL)
 			return NOTIFY_OK;	/* not for us */
 
+		if (of_node_test_and_set_flag(rd->dn, OF_POPULATED)) {
+			put_device(&adap->dev);
+			return NOTIFY_OK;
+		}
+
 		client = of_i2c_register_device(adap, rd->dn);
 		put_device(&adap->dev);
 
@@ -1854,6 +1873,10 @@ static int of_i2c_notify(struct notifier_block *nb, unsigned long action,
 		}
 		break;
 	case OF_RECONFIG_CHANGE_REMOVE:
+		/* already depopulated? */
+		if (!of_node_check_flag(rd->dn, OF_POPULATED))
+			return NOTIFY_OK;
+
 		/* find our device by node */
 		client = of_find_i2c_device_by_node(rd->dn);
 		if (client == NULL)
@@ -2914,18 +2937,24 @@ int i2c_slave_register(struct i2c_client *client, i2c_slave_cb_t slave_cb)
 {
 	int ret;
 
-	if (!client || !slave_cb)
+	if (!client || !slave_cb) {
+		WARN(1, "insufficent data\n");
 		return -EINVAL;
+	}
 
 	if (!(client->flags & I2C_CLIENT_TEN)) {
 		/* Enforce stricter address checking */
 		ret = i2c_check_addr_validity(client->addr);
-		if (ret)
+		if (ret) {
+			dev_err(&client->dev, "%s: invalid address\n", __func__);
 			return ret;
+		}
 	}
 
-	if (!client->adapter->algo->reg_slave)
+	if (!client->adapter->algo->reg_slave) {
+		dev_err(&client->dev, "%s: not supported by adapter\n", __func__);
 		return -EOPNOTSUPP;
+	}
 
 	client->slave_cb = slave_cb;
 
@@ -2933,8 +2962,10 @@ int i2c_slave_register(struct i2c_client *client, i2c_slave_cb_t slave_cb)
 	ret = client->adapter->algo->reg_slave(client);
 	i2c_unlock_adapter(client->adapter);
 
-	if (ret)
+	if (ret) {
 		client->slave_cb = NULL;
+		dev_err(&client->dev, "%s: adapter returned error %d\n", __func__, ret);
+	}
 
 	return ret;
 }
@@ -2944,8 +2975,10 @@ int i2c_slave_unregister(struct i2c_client *client)
 {
 	int ret;
 
-	if (!client->adapter->algo->unreg_slave)
+	if (!client->adapter->algo->unreg_slave) {
+		dev_err(&client->dev, "%s: not supported by adapter\n", __func__);
 		return -EOPNOTSUPP;
+	}
 
 	i2c_lock_adapter(client->adapter);
 	ret = client->adapter->algo->unreg_slave(client);
@@ -2953,6 +2986,8 @@ int i2c_slave_unregister(struct i2c_client *client)
 
 	if (ret == 0)
 		client->slave_cb = NULL;
+	else
+		dev_err(&client->dev, "%s: adapter returned error %d\n", __func__, ret);
 
 	return ret;
 }
diff --git a/drivers/i2c/i2c-mux.c b/drivers/i2c/i2c-mux.c
index 06cc1ff088f1..2ba7c0fbc615 100644
--- a/drivers/i2c/i2c-mux.c
+++ b/drivers/i2c/i2c-mux.c
@@ -51,7 +51,7 @@ static int i2c_mux_master_xfer(struct i2c_adapter *adap,
 
 	ret = priv->select(parent, priv->mux_priv, priv->chan_id);
 	if (ret >= 0)
-		ret = parent->algo->master_xfer(parent, msgs, num);
+		ret = __i2c_transfer(parent, msgs, num);
 	if (priv->deselect)
 		priv->deselect(parent, priv->mux_priv, priv->chan_id);
 
@@ -144,6 +144,7 @@ struct i2c_adapter *i2c_add_mux_adapter(struct i2c_adapter *parent,
 	priv->adap.dev.parent = &parent->dev;
 	priv->adap.retries = parent->retries;
 	priv->adap.timeout = parent->timeout;
+	priv->adap.quirks = parent->quirks;
 
 	/* Sanity check on class */
 	if (i2c_mux_parent_classes(parent) & class)
diff --git a/drivers/i2c/i2c-smbus.c b/drivers/i2c/i2c-smbus.c
index 9ebf9cb4ad7a..94765a81970d 100644
--- a/drivers/i2c/i2c-smbus.c
+++ b/drivers/i2c/i2c-smbus.c
@@ -89,7 +89,7 @@ static void smbus_alert(struct work_struct *work)
 		 * to high, because of slave transmit arbitration.  After
 		 * responding, an SMBus device stops asserting SMBALERT#.
 		 *
-		 * Note that SMBus 2.0 reserves 10-bit addresess for future
+		 * Note that SMBus 2.0 reserves 10-bit addresses for future
 		 * use.  We neither handle them, nor try to use PEC here.
 		 */
 		status = i2c_smbus_read_byte(ara);
diff --git a/drivers/i2c/muxes/Kconfig b/drivers/i2c/muxes/Kconfig
index f6d313e528de..fdd0769c84a3 100644
--- a/drivers/i2c/muxes/Kconfig
+++ b/drivers/i2c/muxes/Kconfig
@@ -7,7 +7,8 @@ menu "Multiplexer I2C Chip support"
 
 config I2C_ARB_GPIO_CHALLENGE
 	tristate "GPIO-based I2C arbitration"
-	depends on GPIOLIB && OF
+	depends on GPIOLIB || COMPILE_TEST
+	depends on OF
 	help
 	  If you say yes to this option, support will be included for an
 	  I2C multimaster arbitration scheme using GPIOs and a challenge &
@@ -40,7 +41,7 @@ config I2C_MUX_PCA9541
 
 config I2C_MUX_PCA954x
 	tristate "Philips PCA954x I2C Mux/switches"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  If you say yes here you get support for the Philips PCA954x
 	  I2C mux/switch devices.
diff --git a/drivers/i2c/muxes/i2c-mux-pca9541.c b/drivers/i2c/muxes/i2c-mux-pca9541.c
index cb772775da43..0c8d4d2cbdaf 100644
--- a/drivers/i2c/muxes/i2c-mux-pca9541.c
+++ b/drivers/i2c/muxes/i2c-mux-pca9541.c
@@ -104,7 +104,7 @@ static int pca9541_reg_write(struct i2c_client *client, u8 command, u8 val)
 		buf[0] = command;
 		buf[1] = val;
 		msg.buf = buf;
-		ret = adap->algo->master_xfer(adap, &msg, 1);
+		ret = __i2c_transfer(adap, &msg, 1);
 	} else {
 		union i2c_smbus_data data;
 
@@ -144,7 +144,7 @@ static int pca9541_reg_read(struct i2c_client *client, u8 command)
 				.buf = &val
 			}
 		};
-		ret = adap->algo->master_xfer(adap, msg, 2);
+		ret = __i2c_transfer(adap, msg, 2);
 		if (ret == 2)
 			ret = val;
 		else if (ret >= 0)
diff --git a/drivers/i2c/muxes/i2c-mux-pca954x.c b/drivers/i2c/muxes/i2c-mux-pca954x.c
index bea0d2de2993..ea4aa9dfcea9 100644
--- a/drivers/i2c/muxes/i2c-mux-pca954x.c
+++ b/drivers/i2c/muxes/i2c-mux-pca954x.c
@@ -134,7 +134,7 @@ static int pca954x_reg_write(struct i2c_adapter *adap,
 		msg.len = 1;
 		buf[0] = val;
 		msg.buf = buf;
-		ret = adap->algo->master_xfer(adap, &msg, 1);
+		ret = __i2c_transfer(adap, &msg, 1);
 	} else {
 		union i2c_smbus_data data;
 		ret = adap->algo->smbus_xfer(adap, client->addr,
diff --git a/drivers/ide/ide-atapi.c b/drivers/ide/ide-atapi.c
index fac3d9da2e07..1362ad80a76c 100644
--- a/drivers/ide/ide-atapi.c
+++ b/drivers/ide/ide-atapi.c
@@ -93,7 +93,7 @@ int ide_queue_pc_tail(ide_drive_t *drive, struct gendisk *disk,
 	int error;
 
 	rq = blk_get_request(drive->queue, READ, __GFP_WAIT);
-	rq->cmd_type = REQ_TYPE_SPECIAL;
+	rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	rq->special = (char *)pc;
 
 	if (buf && bufflen) {
@@ -191,7 +191,7 @@ void ide_prep_sense(ide_drive_t *drive, struct request *rq)
 
 	BUG_ON(sense_len > sizeof(*sense));
 
-	if (rq->cmd_type == REQ_TYPE_SENSE || drive->sense_rq_armed)
+	if (rq->cmd_type == REQ_TYPE_ATA_SENSE || drive->sense_rq_armed)
 		return;
 
 	memset(sense, 0, sizeof(*sense));
@@ -210,7 +210,7 @@ void ide_prep_sense(ide_drive_t *drive, struct request *rq)
 	sense_rq->rq_disk = rq->rq_disk;
 	sense_rq->cmd[0] = GPCMD_REQUEST_SENSE;
 	sense_rq->cmd[4] = cmd_len;
-	sense_rq->cmd_type = REQ_TYPE_SENSE;
+	sense_rq->cmd_type = REQ_TYPE_ATA_SENSE;
 	sense_rq->cmd_flags |= REQ_PREEMPT;
 
 	if (drive->media == ide_tape)
@@ -310,7 +310,7 @@ int ide_cd_get_xferlen(struct request *rq)
 	switch (rq->cmd_type) {
 	case REQ_TYPE_FS:
 		return 32768;
-	case REQ_TYPE_SENSE:
+	case REQ_TYPE_ATA_SENSE:
 	case REQ_TYPE_BLOCK_PC:
 	case REQ_TYPE_ATA_PC:
 		return blk_rq_bytes(rq);
@@ -477,7 +477,7 @@ static ide_startstop_t ide_pc_intr(ide_drive_t *drive)
 		if (uptodate == 0)
 			drive->failed_pc = NULL;
 
-		if (rq->cmd_type == REQ_TYPE_SPECIAL) {
+		if (rq->cmd_type == REQ_TYPE_DRV_PRIV) {
 			rq->errors = 0;
 			error = 0;
 		} else {
diff --git a/drivers/ide/ide-cd.c b/drivers/ide/ide-cd.c
index 0b510bafd90e..64a6b827b3dd 100644
--- a/drivers/ide/ide-cd.c
+++ b/drivers/ide/ide-cd.c
@@ -210,7 +210,7 @@ static void cdrom_analyze_sense_data(ide_drive_t *drive,
 static void ide_cd_complete_failed_rq(ide_drive_t *drive, struct request *rq)
 {
 	/*
-	 * For REQ_TYPE_SENSE, "rq->special" points to the original
+	 * For REQ_TYPE_ATA_SENSE, "rq->special" points to the original
 	 * failed request.  Also, the sense data should be read
 	 * directly from rq which might be different from the original
 	 * sense buffer if it got copied during mapping.
@@ -285,7 +285,7 @@ static int cdrom_decode_status(ide_drive_t *drive, u8 stat)
 				  "stat 0x%x",
 				  rq->cmd[0], rq->cmd_type, err, stat);
 
-	if (rq->cmd_type == REQ_TYPE_SENSE) {
+	if (rq->cmd_type == REQ_TYPE_ATA_SENSE) {
 		/*
 		 * We got an error trying to get sense info from the drive
 		 * (probably while trying to recover from a former error).
@@ -526,7 +526,7 @@ static ide_startstop_t cdrom_newpc_intr(ide_drive_t *drive)
 	ide_expiry_t *expiry = NULL;
 	int dma_error = 0, dma, thislen, uptodate = 0;
 	int write = (rq_data_dir(rq) == WRITE) ? 1 : 0, rc = 0;
-	int sense = (rq->cmd_type == REQ_TYPE_SENSE);
+	int sense = (rq->cmd_type == REQ_TYPE_ATA_SENSE);
 	unsigned int timeout;
 	u16 len;
 	u8 ireason, stat;
@@ -791,7 +791,7 @@ static ide_startstop_t ide_cd_do_request(ide_drive_t *drive, struct request *rq,
 		if (cdrom_start_rw(drive, rq) == ide_stopped)
 			goto out_end;
 		break;
-	case REQ_TYPE_SENSE:
+	case REQ_TYPE_ATA_SENSE:
 	case REQ_TYPE_BLOCK_PC:
 	case REQ_TYPE_ATA_PC:
 		if (!rq->timeout)
@@ -799,7 +799,7 @@ static ide_startstop_t ide_cd_do_request(ide_drive_t *drive, struct request *rq,
 
 		cdrom_do_block_pc(drive, rq);
 		break;
-	case REQ_TYPE_SPECIAL:
+	case REQ_TYPE_DRV_PRIV:
 		/* right now this can only be a reset... */
 		uptodate = 1;
 		goto out_end;
diff --git a/drivers/ide/ide-cd_ioctl.c b/drivers/ide/ide-cd_ioctl.c
index 02caa7dd51c8..066e39036518 100644
--- a/drivers/ide/ide-cd_ioctl.c
+++ b/drivers/ide/ide-cd_ioctl.c
@@ -304,7 +304,7 @@ int ide_cdrom_reset(struct cdrom_device_info *cdi)
 	int ret;
 
 	rq = blk_get_request(drive->queue, READ, __GFP_WAIT);
-	rq->cmd_type = REQ_TYPE_SPECIAL;
+	rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	rq->cmd_flags = REQ_QUIET;
 	ret = blk_execute_rq(drive->queue, cd->disk, rq, 0);
 	blk_put_request(rq);
diff --git a/drivers/ide/ide-devsets.c b/drivers/ide/ide-devsets.c
index 9e98122f646e..b05a74d78ef5 100644
--- a/drivers/ide/ide-devsets.c
+++ b/drivers/ide/ide-devsets.c
@@ -166,7 +166,7 @@ int ide_devset_execute(ide_drive_t *drive, const struct ide_devset *setting,
 		return setting->set(drive, arg);
 
 	rq = blk_get_request(q, READ, __GFP_WAIT);
-	rq->cmd_type = REQ_TYPE_SPECIAL;
+	rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	rq->cmd_len = 5;
 	rq->cmd[0] = REQ_DEVSET_EXEC;
 	*(int *)&rq->cmd[1] = arg;
diff --git a/drivers/ide/ide-eh.c b/drivers/ide/ide-eh.c
index 32970664c275..d6da011299f5 100644
--- a/drivers/ide/ide-eh.c
+++ b/drivers/ide/ide-eh.c
@@ -129,7 +129,7 @@ ide_startstop_t ide_error(ide_drive_t *drive, const char *msg, u8 stat)
 
 			if (cmd)
 				ide_complete_cmd(drive, cmd, stat, err);
-		} else if (blk_pm_request(rq)) {
+		} else if (ata_pm_request(rq)) {
 			rq->errors = 1;
 			ide_complete_pm_rq(drive, rq);
 			return ide_stopped;
@@ -147,7 +147,7 @@ static inline void ide_complete_drive_reset(ide_drive_t *drive, int err)
 {
 	struct request *rq = drive->hwif->rq;
 
-	if (rq && rq->cmd_type == REQ_TYPE_SPECIAL &&
+	if (rq && rq->cmd_type == REQ_TYPE_DRV_PRIV &&
 	    rq->cmd[0] == REQ_DRIVE_RESET) {
 		if (err <= 0 && rq->errors == 0)
 			rq->errors = -EIO;
diff --git a/drivers/ide/ide-floppy.c b/drivers/ide/ide-floppy.c
index 8c6363cdd208..2fb5350c5410 100644
--- a/drivers/ide/ide-floppy.c
+++ b/drivers/ide/ide-floppy.c
@@ -97,7 +97,7 @@ static int ide_floppy_callback(ide_drive_t *drive, int dsc)
 			       "Aborting request!\n");
 	}
 
-	if (rq->cmd_type == REQ_TYPE_SPECIAL)
+	if (rq->cmd_type == REQ_TYPE_DRV_PRIV)
 		rq->errors = uptodate ? 0 : IDE_DRV_ERROR_GENERAL;
 
 	return uptodate;
@@ -246,7 +246,7 @@ static ide_startstop_t ide_floppy_do_request(ide_drive_t *drive,
 		} else
 			printk(KERN_ERR PFX "%s: I/O error\n", drive->name);
 
-		if (rq->cmd_type == REQ_TYPE_SPECIAL) {
+		if (rq->cmd_type == REQ_TYPE_DRV_PRIV) {
 			rq->errors = 0;
 			ide_complete_rq(drive, 0, blk_rq_bytes(rq));
 			return ide_stopped;
@@ -265,8 +265,8 @@ static ide_startstop_t ide_floppy_do_request(ide_drive_t *drive,
 		pc = &floppy->queued_pc;
 		idefloppy_create_rw_cmd(drive, pc, rq, (unsigned long)block);
 		break;
-	case REQ_TYPE_SPECIAL:
-	case REQ_TYPE_SENSE:
+	case REQ_TYPE_DRV_PRIV:
+	case REQ_TYPE_ATA_SENSE:
 		pc = (struct ide_atapi_pc *)rq->special;
 		break;
 	case REQ_TYPE_BLOCK_PC:
diff --git a/drivers/ide/ide-io.c b/drivers/ide/ide-io.c
index 177db6d5b2f5..669ea1e45795 100644
--- a/drivers/ide/ide-io.c
+++ b/drivers/ide/ide-io.c
@@ -135,7 +135,7 @@ EXPORT_SYMBOL(ide_complete_rq);
 
 void ide_kill_rq(ide_drive_t *drive, struct request *rq)
 {
-	u8 drv_req = (rq->cmd_type == REQ_TYPE_SPECIAL) && rq->rq_disk;
+	u8 drv_req = (rq->cmd_type == REQ_TYPE_DRV_PRIV) && rq->rq_disk;
 	u8 media = drive->media;
 
 	drive->failed_pc = NULL;
@@ -320,7 +320,7 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 		goto kill_rq;
 	}
 
-	if (blk_pm_request(rq))
+	if (ata_pm_request(rq))
 		ide_check_pm_state(drive, rq);
 
 	drive->hwif->tp_ops->dev_select(drive);
@@ -342,8 +342,8 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 
 		if (rq->cmd_type == REQ_TYPE_ATA_TASKFILE)
 			return execute_drive_cmd(drive, rq);
-		else if (blk_pm_request(rq)) {
-			struct request_pm_state *pm = rq->special;
+		else if (ata_pm_request(rq)) {
+			struct ide_pm_state *pm = rq->special;
 #ifdef DEBUG_PM
 			printk("%s: start_power_step(step: %d)\n",
 				drive->name, pm->pm_step);
@@ -353,7 +353,7 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 			    pm->pm_step == IDE_PM_COMPLETED)
 				ide_complete_pm_rq(drive, rq);
 			return startstop;
-		} else if (!rq->rq_disk && rq->cmd_type == REQ_TYPE_SPECIAL)
+		} else if (!rq->rq_disk && rq->cmd_type == REQ_TYPE_DRV_PRIV)
 			/*
 			 * TODO: Once all ULDs have been modified to
 			 * check for specific op codes rather than
@@ -538,7 +538,7 @@ repeat:
 		 * state machine.
 		 */
 		if ((drive->dev_flags & IDE_DFLAG_BLOCKED) &&
-		    blk_pm_request(rq) == 0 &&
+		    ata_pm_request(rq) == 0 &&
 		    (rq->cmd_flags & REQ_PREEMPT) == 0) {
 			/* there should be no pending command at this point */
 			ide_unlock_port(hwif);
diff --git a/drivers/ide/ide-ioctls.c b/drivers/ide/ide-ioctls.c
index 6233fa2cb8a9..aa2e9b77b20d 100644
--- a/drivers/ide/ide-ioctls.c
+++ b/drivers/ide/ide-ioctls.c
@@ -222,7 +222,7 @@ static int generic_drive_reset(ide_drive_t *drive)
 	int ret = 0;
 
 	rq = blk_get_request(drive->queue, READ, __GFP_WAIT);
-	rq->cmd_type = REQ_TYPE_SPECIAL;
+	rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	rq->cmd_len = 1;
 	rq->cmd[0] = REQ_DRIVE_RESET;
 	if (blk_execute_rq(drive->queue, NULL, rq, 1))
diff --git a/drivers/ide/ide-park.c b/drivers/ide/ide-park.c
index ca958604cda2..c80868520488 100644
--- a/drivers/ide/ide-park.c
+++ b/drivers/ide/ide-park.c
@@ -34,7 +34,7 @@ static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 	rq = blk_get_request(q, READ, __GFP_WAIT);
 	rq->cmd[0] = REQ_PARK_HEADS;
 	rq->cmd_len = 1;
-	rq->cmd_type = REQ_TYPE_SPECIAL;
+	rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	rq->special = &timeout;
 	rc = blk_execute_rq(q, NULL, rq, 1);
 	blk_put_request(rq);
@@ -51,7 +51,7 @@ static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 
 	rq->cmd[0] = REQ_UNPARK_HEADS;
 	rq->cmd_len = 1;
-	rq->cmd_type = REQ_TYPE_SPECIAL;
+	rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	elv_add_request(q, rq, ELEVATOR_INSERT_FRONT);
 
 out:
diff --git a/drivers/ide/ide-pm.c b/drivers/ide/ide-pm.c
index 8d1e32d7cd97..081e43458d50 100644
--- a/drivers/ide/ide-pm.c
+++ b/drivers/ide/ide-pm.c
@@ -8,7 +8,7 @@ int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 	ide_drive_t *pair = ide_get_pair_dev(drive);
 	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq;
-	struct request_pm_state rqpm;
+	struct ide_pm_state rqpm;
 	int ret;
 
 	if (ide_port_acpi(hwif)) {
@@ -19,7 +19,7 @@ int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 
 	memset(&rqpm, 0, sizeof(rqpm));
 	rq = blk_get_request(drive->queue, READ, __GFP_WAIT);
-	rq->cmd_type = REQ_TYPE_PM_SUSPEND;
+	rq->cmd_type = REQ_TYPE_ATA_PM_SUSPEND;
 	rq->special = &rqpm;
 	rqpm.pm_step = IDE_PM_START_SUSPEND;
 	if (mesg.event == PM_EVENT_PRETHAW)
@@ -38,13 +38,43 @@ int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 	return ret;
 }
 
+static void ide_end_sync_rq(struct request *rq, int error)
+{
+	complete(rq->end_io_data);
+}
+
+static int ide_pm_execute_rq(struct request *rq)
+{
+	struct request_queue *q = rq->q;
+	DECLARE_COMPLETION_ONSTACK(wait);
+
+	rq->end_io_data = &wait;
+	rq->end_io = ide_end_sync_rq;
+
+	spin_lock_irq(q->queue_lock);
+	if (unlikely(blk_queue_dying(q))) {
+		rq->cmd_flags |= REQ_QUIET;
+		rq->errors = -ENXIO;
+		__blk_end_request_all(rq, rq->errors);
+		spin_unlock_irq(q->queue_lock);
+		return -ENXIO;
+	}
+	__elv_add_request(q, rq, ELEVATOR_INSERT_FRONT);
+	__blk_run_queue_uncond(q);
+	spin_unlock_irq(q->queue_lock);
+
+	wait_for_completion_io(&wait);
+
+	return rq->errors ? -EIO : 0;
+}
+
 int generic_ide_resume(struct device *dev)
 {
 	ide_drive_t *drive = to_ide_device(dev);
 	ide_drive_t *pair = ide_get_pair_dev(drive);
 	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq;
-	struct request_pm_state rqpm;
+	struct ide_pm_state rqpm;
 	int err;
 
 	if (ide_port_acpi(hwif)) {
@@ -59,13 +89,13 @@ int generic_ide_resume(struct device *dev)
 
 	memset(&rqpm, 0, sizeof(rqpm));
 	rq = blk_get_request(drive->queue, READ, __GFP_WAIT);
-	rq->cmd_type = REQ_TYPE_PM_RESUME;
+	rq->cmd_type = REQ_TYPE_ATA_PM_RESUME;
 	rq->cmd_flags |= REQ_PREEMPT;
 	rq->special = &rqpm;
 	rqpm.pm_step = IDE_PM_START_RESUME;
 	rqpm.pm_state = PM_EVENT_ON;
 
-	err = blk_execute_rq(drive->queue, NULL, rq, 1);
+	err = ide_pm_execute_rq(rq);
 	blk_put_request(rq);
 
 	if (err == 0 && dev->driver) {
@@ -80,7 +110,7 @@ int generic_ide_resume(struct device *dev)
 
 void ide_complete_power_step(ide_drive_t *drive, struct request *rq)
 {
-	struct request_pm_state *pm = rq->special;
+	struct ide_pm_state *pm = rq->special;
 
 #ifdef DEBUG_PM
 	printk(KERN_INFO "%s: complete_power_step(step: %d)\n",
@@ -110,7 +140,7 @@ void ide_complete_power_step(ide_drive_t *drive, struct request *rq)
 
 ide_startstop_t ide_start_power_step(ide_drive_t *drive, struct request *rq)
 {
-	struct request_pm_state *pm = rq->special;
+	struct ide_pm_state *pm = rq->special;
 	struct ide_cmd cmd = { };
 
 	switch (pm->pm_step) {
@@ -182,7 +212,7 @@ out_do_tf:
 void ide_complete_pm_rq(ide_drive_t *drive, struct request *rq)
 {
 	struct request_queue *q = drive->queue;
-	struct request_pm_state *pm = rq->special;
+	struct ide_pm_state *pm = rq->special;
 	unsigned long flags;
 
 	ide_complete_power_step(drive, rq);
@@ -191,10 +221,10 @@ void ide_complete_pm_rq(ide_drive_t *drive, struct request *rq)
 
 #ifdef DEBUG_PM
 	printk("%s: completing PM request, %s\n", drive->name,
-	       (rq->cmd_type == REQ_TYPE_PM_SUSPEND) ? "suspend" : "resume");
+	       (rq->cmd_type == REQ_TYPE_ATA_PM_SUSPEND) ? "suspend" : "resume");
 #endif
 	spin_lock_irqsave(q->queue_lock, flags);
-	if (rq->cmd_type == REQ_TYPE_PM_SUSPEND)
+	if (rq->cmd_type == REQ_TYPE_ATA_PM_SUSPEND)
 		blk_stop_queue(q);
 	else
 		drive->dev_flags &= ~IDE_DFLAG_BLOCKED;
@@ -208,13 +238,13 @@ void ide_complete_pm_rq(ide_drive_t *drive, struct request *rq)
 
 void ide_check_pm_state(ide_drive_t *drive, struct request *rq)
 {
-	struct request_pm_state *pm = rq->special;
+	struct ide_pm_state *pm = rq->special;
 
-	if (rq->cmd_type == REQ_TYPE_PM_SUSPEND &&
+	if (rq->cmd_type == REQ_TYPE_ATA_PM_SUSPEND &&
 	    pm->pm_step == IDE_PM_START_SUSPEND)
 		/* Mark drive blocked when starting the suspend sequence. */
 		drive->dev_flags |= IDE_DFLAG_BLOCKED;
-	else if (rq->cmd_type == REQ_TYPE_PM_RESUME &&
+	else if (rq->cmd_type == REQ_TYPE_ATA_PM_RESUME &&
 		 pm->pm_step == IDE_PM_START_RESUME) {
 		/*
 		 * The first thing we do on wakeup is to wait for BSY bit to
diff --git a/drivers/ide/ide-tape.c b/drivers/ide/ide-tape.c
index 6eb738ca6d2f..f5d51d1d09ee 100644
--- a/drivers/ide/ide-tape.c
+++ b/drivers/ide/ide-tape.c
@@ -576,8 +576,8 @@ static ide_startstop_t idetape_do_request(ide_drive_t *drive,
 		      rq->cmd[0], (unsigned long long)blk_rq_pos(rq),
 		      blk_rq_sectors(rq));
 
-	BUG_ON(!(rq->cmd_type == REQ_TYPE_SPECIAL ||
-		 rq->cmd_type == REQ_TYPE_SENSE));
+	BUG_ON(!(rq->cmd_type == REQ_TYPE_DRV_PRIV ||
+		 rq->cmd_type == REQ_TYPE_ATA_SENSE));
 
 	/* Retry a failed packet command */
 	if (drive->failed_pc && drive->pc->c[0] == REQUEST_SENSE) {
@@ -853,7 +853,7 @@ static int idetape_queue_rw_tail(ide_drive_t *drive, int cmd, int size)
 	BUG_ON(size < 0 || size % tape->blk_size);
 
 	rq = blk_get_request(drive->queue, READ, __GFP_WAIT);
-	rq->cmd_type = REQ_TYPE_SPECIAL;
+	rq->cmd_type = REQ_TYPE_DRV_PRIV;
 	rq->cmd[13] = cmd;
 	rq->rq_disk = tape->disk;
 	rq->__sector = tape->first_frame;
diff --git a/drivers/ide/ide-taskfile.c b/drivers/ide/ide-taskfile.c
index dabb88b1cbec..0979e126fff1 100644
--- a/drivers/ide/ide-taskfile.c
+++ b/drivers/ide/ide-taskfile.c
@@ -186,7 +186,7 @@ static ide_startstop_t task_no_data_intr(ide_drive_t *drive)
 	    tf->command == ATA_CMD_CHK_POWER) {
 		struct request *rq = hwif->rq;
 
-		if (blk_pm_request(rq))
+		if (ata_pm_request(rq))
 			ide_complete_pm_rq(drive, rq);
 		else
 			ide_finish_cmd(drive, cmd, stat);
diff --git a/drivers/ide/ide.c b/drivers/ide/ide.c
index e29b02ca9e91..f086ef387475 100644
--- a/drivers/ide/ide.c
+++ b/drivers/ide/ide.c
@@ -199,7 +199,7 @@ static int ide_set_dev_param_mask(const char *s, const struct kernel_param *kp)
 	return 0;
 }
 
-static struct kernel_param_ops param_ops_ide_dev_mask = {
+static const struct kernel_param_ops param_ops_ide_dev_mask = {
 	.set = ide_set_dev_param_mask
 };
 
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index 7c9a9a94a8ce..00e7bcbdbe24 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -136,4 +136,25 @@ config MMA9553
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called mma9553.
+
+config STK8312
+	tristate "Sensortek STK8312 3-Axis Accelerometer Driver"
+	depends on I2C
+	help
+	  Say yes here to get support for the Sensortek STK8312 3-axis
+	  accelerometer.
+
+	  Choosing M will build the driver as a module. If so, the module
+	  will be called stk8312.
+
+config STK8BA50
+	tristate "Sensortek STK8BA50 3-Axis Accelerometer Driver"
+	depends on I2C
+	help
+	  Say yes here to get support for the Sensortek STK8BA50 3-axis
+	  accelerometer.
+
+	  Choosing M will build the driver as a module. If so, the module
+	  will be called stk8ba50.
+
 endmenu
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index 99d89e46cad1..ebd2675b2a02 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -14,6 +14,9 @@ obj-$(CONFIG_MMA9551_CORE)	+= mma9551_core.o
 obj-$(CONFIG_MMA9551)		+= mma9551.o
 obj-$(CONFIG_MMA9553)		+= mma9553.o
 
+obj-$(CONFIG_STK8312)		+= stk8312.o
+obj-$(CONFIG_STK8BA50)		+= stk8ba50.o
+
 obj-$(CONFIG_IIO_SSP_SENSORS_COMMONS) += ssp_accel_sensor.o
 
 obj-$(CONFIG_IIO_ST_ACCEL_3AXIS) += st_accel.o
diff --git a/drivers/iio/accel/bmc150-accel.c b/drivers/iio/accel/bmc150-accel.c
index 73e87739d219..4e70f51c2370 100644
--- a/drivers/iio/accel/bmc150-accel.c
+++ b/drivers/iio/accel/bmc150-accel.c
@@ -196,7 +196,7 @@ struct bmc150_accel_data {
 	u32 slope_thres;
 	u32 range;
 	int ev_enable_state;
-	int64_t timestamp, old_timestamp;
+	int64_t timestamp, old_timestamp; /* Only used in hw fifo mode. */
 	const struct bmc150_accel_chip_info *chip_info;
 };
 
@@ -1183,7 +1183,6 @@ static const struct iio_info bmc150_accel_info = {
 	.write_event_value	= bmc150_accel_write_event,
 	.write_event_config	= bmc150_accel_write_event_config,
 	.read_event_config	= bmc150_accel_read_event_config,
-	.validate_trigger	= bmc150_accel_validate_trigger,
 	.driver_module		= THIS_MODULE,
 };
 
@@ -1222,7 +1221,7 @@ static irqreturn_t bmc150_accel_trigger_handler(int irq, void *p)
 	mutex_unlock(&data->mutex);
 
 	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
-					   data->timestamp);
+					   pf->timestamp);
 err_read:
 	iio_trigger_notify_done(indio_dev->trig);
 
@@ -1535,6 +1534,13 @@ static int bmc150_accel_fifo_set_mode(struct bmc150_accel_data *data)
 	return ret;
 }
 
+static int bmc150_accel_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	return bmc150_accel_set_power_state(data, true);
+}
+
 static int bmc150_accel_buffer_postenable(struct iio_dev *indio_dev)
 {
 	struct bmc150_accel_data *data = iio_priv(indio_dev);
@@ -1591,9 +1597,18 @@ out:
 	return 0;
 }
 
+static int bmc150_accel_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	return bmc150_accel_set_power_state(data, false);
+}
+
 static const struct iio_buffer_setup_ops bmc150_accel_buffer_ops = {
+	.preenable = bmc150_accel_buffer_preenable,
 	.postenable = bmc150_accel_buffer_postenable,
 	.predisable = bmc150_accel_buffer_predisable,
+	.postdisable = bmc150_accel_buffer_postdisable,
 };
 
 static int bmc150_accel_probe(struct i2c_client *client,
@@ -1636,6 +1651,15 @@ static int bmc150_accel_probe(struct i2c_client *client,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &bmc150_accel_info;
 
+	ret = iio_triggered_buffer_setup(indio_dev,
+					 &iio_pollfunc_store_time,
+					 bmc150_accel_trigger_handler,
+					 &bmc150_accel_buffer_ops);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed: iio triggered buffer setup\n");
+		return ret;
+	}
+
 	if (client->irq < 0)
 		client->irq = bmc150_accel_gpio_probe(client, data);
 
@@ -1648,7 +1672,7 @@ static int bmc150_accel_probe(struct i2c_client *client,
 						BMC150_ACCEL_IRQ_NAME,
 						indio_dev);
 		if (ret)
-			return ret;
+			goto err_buffer_cleanup;
 
 		/*
 		 * Set latched mode interrupt. While certain interrupts are
@@ -1661,24 +1685,14 @@ static int bmc150_accel_probe(struct i2c_client *client,
 					     BMC150_ACCEL_INT_MODE_LATCH_RESET);
 		if (ret < 0) {
 			dev_err(&data->client->dev, "Error writing reg_int_rst_latch\n");
-			return ret;
+			goto err_buffer_cleanup;
 		}
 
 		bmc150_accel_interrupts_setup(indio_dev, data);
 
 		ret = bmc150_accel_triggers_setup(indio_dev, data);
 		if (ret)
-			return ret;
-
-		ret = iio_triggered_buffer_setup(indio_dev,
-						 &iio_pollfunc_store_time,
-						 bmc150_accel_trigger_handler,
-						 &bmc150_accel_buffer_ops);
-		if (ret < 0) {
-			dev_err(&client->dev,
-				"Failed: iio triggered buffer setup\n");
-			goto err_trigger_unregister;
-		}
+			goto err_buffer_cleanup;
 
 		if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C) ||
 		    i2c_check_functionality(client->adapter,
@@ -1692,7 +1706,7 @@ static int bmc150_accel_probe(struct i2c_client *client,
 	ret = iio_device_register(indio_dev);
 	if (ret < 0) {
 		dev_err(&client->dev, "Unable to register iio device\n");
-		goto err_buffer_cleanup;
+		goto err_trigger_unregister;
 	}
 
 	ret = pm_runtime_set_active(&client->dev);
@@ -1708,11 +1722,10 @@ static int bmc150_accel_probe(struct i2c_client *client,
 
 err_iio_unregister:
 	iio_device_unregister(indio_dev);
-err_buffer_cleanup:
-	if (indio_dev->pollfunc)
-		iio_triggered_buffer_cleanup(indio_dev);
 err_trigger_unregister:
 	bmc150_accel_unregister_triggers(data, BMC150_ACCEL_TRIGGERS - 1);
+err_buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
 
 	return ret;
 }
@@ -1730,6 +1743,8 @@ static int bmc150_accel_remove(struct i2c_client *client)
 
 	bmc150_accel_unregister_triggers(data, BMC150_ACCEL_TRIGGERS - 1);
 
+	iio_triggered_buffer_cleanup(indio_dev);
+
 	mutex_lock(&data->mutex);
 	bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_DEEP_SUSPEND, 0);
 	mutex_unlock(&data->mutex);
diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
index 2b4fad6998c1..ab1e238d5c75 100644
--- a/drivers/iio/accel/hid-sensor-accel-3d.c
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -299,7 +299,6 @@ static int hid_accel_3d_probe(struct platform_device *pdev)
 	struct iio_dev *indio_dev;
 	struct accel_3d_state *accel_state;
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
-	struct iio_chan_spec *channels;
 
 	indio_dev = devm_iio_device_alloc(&pdev->dev,
 					  sizeof(struct accel_3d_state));
@@ -320,21 +319,21 @@ static int hid_accel_3d_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	channels = kmemdup(accel_3d_channels, sizeof(accel_3d_channels),
-			   GFP_KERNEL);
-	if (!channels) {
+	indio_dev->channels = kmemdup(accel_3d_channels,
+				      sizeof(accel_3d_channels), GFP_KERNEL);
+	if (!indio_dev->channels) {
 		dev_err(&pdev->dev, "failed to duplicate channels\n");
 		return -ENOMEM;
 	}
 
-	ret = accel_3d_parse_report(pdev, hsdev, channels,
-					HID_USAGE_SENSOR_ACCEL_3D, accel_state);
+	ret = accel_3d_parse_report(pdev, hsdev,
+				    (struct iio_chan_spec *)indio_dev->channels,
+				    HID_USAGE_SENSOR_ACCEL_3D, accel_state);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to setup attributes\n");
 		goto error_free_dev_mem;
 	}
 
-	indio_dev->channels = channels;
 	indio_dev->num_channels = ARRAY_SIZE(accel_3d_channels);
 	indio_dev->dev.parent = &pdev->dev;
 	indio_dev->info = &accel_3d_info;
@@ -400,7 +399,7 @@ static int hid_accel_3d_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_accel_3d_ids[] = {
+static const struct platform_device_id hid_accel_3d_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-200073",
diff --git a/drivers/iio/accel/kxcjk-1013.c b/drivers/iio/accel/kxcjk-1013.c
index 51da3692d561..0d9bd35ff258 100644
--- a/drivers/iio/accel/kxcjk-1013.c
+++ b/drivers/iio/accel/kxcjk-1013.c
@@ -875,15 +875,18 @@ static int kxcjk1013_write_event_config(struct iio_dev *indio_dev,
 	return 0;
 }
 
-static int kxcjk1013_validate_trigger(struct iio_dev *indio_dev,
-				      struct iio_trigger *trig)
+static int kxcjk1013_buffer_preenable(struct iio_dev *indio_dev)
 {
 	struct kxcjk1013_data *data = iio_priv(indio_dev);
 
-	if (data->dready_trig != trig && data->motion_trig != trig)
-		return -EINVAL;
+	return kxcjk1013_set_power_state(data, true);
+}
 
-	return 0;
+static int kxcjk1013_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
+
+	return kxcjk1013_set_power_state(data, false);
 }
 
 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL(
@@ -935,6 +938,13 @@ static const struct iio_chan_spec kxcjk1013_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(3),
 };
 
+static const struct iio_buffer_setup_ops kxcjk1013_buffer_setup_ops = {
+	.preenable		= kxcjk1013_buffer_preenable,
+	.postenable		= iio_triggered_buffer_postenable,
+	.postdisable		= kxcjk1013_buffer_postdisable,
+	.predisable		= iio_triggered_buffer_predisable,
+};
+
 static const struct iio_info kxcjk1013_info = {
 	.attrs			= &kxcjk1013_attrs_group,
 	.read_raw		= kxcjk1013_read_raw,
@@ -943,7 +953,6 @@ static const struct iio_info kxcjk1013_info = {
 	.write_event_value	= kxcjk1013_write_event,
 	.write_event_config	= kxcjk1013_write_event_config,
 	.read_event_config	= kxcjk1013_read_event_config,
-	.validate_trigger	= kxcjk1013_validate_trigger,
 	.driver_module		= THIS_MODULE,
 };
 
@@ -1147,8 +1156,10 @@ static const char *kxcjk1013_match_acpi_device(struct device *dev,
 	id = acpi_match_device(dev->driver->acpi_match_table, dev);
 	if (!id)
 		return NULL;
+
 	if (strcmp(id->id, "SMO8500") == 0)
 		*is_smo8500_device = true;
+
 	*chipset = (enum kx_chipset)id->driver_data;
 
 	return dev_name(dev);
@@ -1163,6 +1174,7 @@ static int kxcjk1013_gpio_probe(struct i2c_client *client,
 
 	if (!client)
 		return -EINVAL;
+
 	if (data->is_smo8500_device)
 		return -ENOTSUPP;
 
@@ -1276,16 +1288,15 @@ static int kxcjk1013_probe(struct i2c_client *client,
 			data->motion_trig = NULL;
 			goto err_trigger_unregister;
 		}
+	}
 
-		ret = iio_triggered_buffer_setup(indio_dev,
-						&iio_pollfunc_store_time,
-						kxcjk1013_trigger_handler,
-						NULL);
-		if (ret < 0) {
-			dev_err(&client->dev,
-					"iio triggered buffer setup failed\n");
-			goto err_trigger_unregister;
-		}
+	ret = iio_triggered_buffer_setup(indio_dev,
+					 &iio_pollfunc_store_time,
+					 kxcjk1013_trigger_handler,
+					 &kxcjk1013_buffer_setup_ops);
+	if (ret < 0) {
+		dev_err(&client->dev, "iio triggered buffer setup failed\n");
+		goto err_trigger_unregister;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -1418,6 +1429,7 @@ static const struct dev_pm_ops kxcjk1013_pm_ops = {
 static const struct acpi_device_id kx_acpi_match[] = {
 	{"KXCJ1013", KXCJK1013},
 	{"KXCJ1008", KXCJ91008},
+	{"KXCJ9000", KXCJ91008},
 	{"KXTJ1009", KXTJ21009},
 	{"SMO8500",  KXCJ91008},
 	{ },
diff --git a/drivers/iio/accel/mma8452.c b/drivers/iio/accel/mma8452.c
index 5b80657883bb..e8e2077c7244 100644
--- a/drivers/iio/accel/mma8452.c
+++ b/drivers/iio/accel/mma8452.c
@@ -9,7 +9,7 @@
  *
  * 7-bit I2C slave address 0x1c/0x1d (pin selectable)
  *
- * TODO: interrupt, thresholding, orientation / freefall events, autosleep
+ * TODO: orientation / freefall events, autosleep
  */
 
 #include <linux/module.h>
@@ -18,20 +18,42 @@
 #include <linux/iio/sysfs.h>
 #include <linux/iio/trigger_consumer.h>
 #include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
 #include <linux/iio/triggered_buffer.h>
+#include <linux/iio/events.h>
 #include <linux/delay.h>
 
 #define MMA8452_STATUS 0x00
 #define MMA8452_OUT_X 0x01 /* MSB first, 12-bit  */
 #define MMA8452_OUT_Y 0x03
 #define MMA8452_OUT_Z 0x05
+#define MMA8452_INT_SRC 0x0c
 #define MMA8452_WHO_AM_I 0x0d
 #define MMA8452_DATA_CFG 0x0e
+#define MMA8452_HP_FILTER_CUTOFF 0x0f
+#define MMA8452_HP_FILTER_CUTOFF_SEL_MASK	(BIT(0) | BIT(1))
+#define MMA8452_TRANSIENT_CFG 0x1d
+#define MMA8452_TRANSIENT_CFG_ELE		BIT(4)
+#define MMA8452_TRANSIENT_CFG_CHAN(chan)	BIT(chan + 1)
+#define MMA8452_TRANSIENT_CFG_HPF_BYP		BIT(0)
+#define MMA8452_TRANSIENT_SRC 0x1e
+#define MMA8452_TRANSIENT_SRC_XTRANSE		BIT(1)
+#define MMA8452_TRANSIENT_SRC_YTRANSE		BIT(3)
+#define MMA8452_TRANSIENT_SRC_ZTRANSE		BIT(5)
+#define MMA8452_TRANSIENT_THS 0x1f
+#define MMA8452_TRANSIENT_THS_MASK	0x7f
+#define MMA8452_TRANSIENT_COUNT 0x20
 #define MMA8452_OFF_X 0x2f
 #define MMA8452_OFF_Y 0x30
 #define MMA8452_OFF_Z 0x31
 #define MMA8452_CTRL_REG1 0x2a
 #define MMA8452_CTRL_REG2 0x2b
+#define MMA8452_CTRL_REG2_RST		BIT(6)
+#define MMA8452_CTRL_REG4 0x2d
+#define MMA8452_CTRL_REG5 0x2e
+
+#define MMA8452_MAX_REG 0x31
 
 #define MMA8452_STATUS_DRDY (BIT(2) | BIT(1) | BIT(0))
 
@@ -44,6 +66,10 @@
 #define MMA8452_DATA_CFG_FS_2G 0
 #define MMA8452_DATA_CFG_FS_4G 1
 #define MMA8452_DATA_CFG_FS_8G 2
+#define MMA8452_DATA_CFG_HPF_MASK BIT(4)
+
+#define MMA8452_INT_DRDY	BIT(0)
+#define MMA8452_INT_TRANS	BIT(5)
 
 #define MMA8452_DEVICE_ID 0x2a
 
@@ -106,6 +132,12 @@ static int mma8452_get_int_plus_micros_index(const int (*vals)[2], int n,
 	return -EINVAL;
 }
 
+static int mma8452_get_odr_index(struct mma8452_data *data)
+{
+	return (data->ctrl_reg1 & MMA8452_CTRL_DR_MASK) >>
+			MMA8452_CTRL_DR_SHIFT;
+}
+
 static const int mma8452_samp_freq[8][2] = {
 	{800, 0}, {400, 0}, {200, 0}, {100, 0}, {50, 0}, {12, 500000},
 	{6, 250000}, {1, 560000}
@@ -121,6 +153,30 @@ static const int mma8452_scales[3][2] = {
 	{0, 9577}, {0, 19154}, {0, 38307}
 };
 
+/* Datasheet table 35  (step time vs sample frequency) */
+static const int mma8452_transient_time_step_us[8] = {
+	1250,
+	2500,
+	5000,
+	10000,
+	20000,
+	20000,
+	20000,
+	20000
+};
+
+/* Datasheet table 18 (normal mode) */
+static const int mma8452_hp_filter_cutoff[8][4][2] = {
+	{ {16, 0}, {8, 0}, {4, 0}, {2, 0} },		/* 800 Hz sample */
+	{ {16, 0}, {8, 0}, {4, 0}, {2, 0} },		/* 400 Hz sample */
+	{ {8, 0}, {4, 0}, {2, 0}, {1, 0} },		/* 200 Hz sample */
+	{ {4, 0}, {2, 0}, {1, 0}, {0, 500000} },	/* 100 Hz sample */
+	{ {2, 0}, {1, 0}, {0, 500000}, {0, 250000} },	/* 50 Hz sample */
+	{ {2, 0}, {1, 0}, {0, 500000}, {0, 250000} },	/* 12.5 Hz sample */
+	{ {2, 0}, {1, 0}, {0, 500000}, {0, 250000} },	/* 6.25 Hz sample */
+	{ {2, 0}, {1, 0}, {0, 500000}, {0, 250000} }	/* 1.56 Hz sample */
+};
+
 static ssize_t mma8452_show_samp_freq_avail(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
@@ -135,9 +191,23 @@ static ssize_t mma8452_show_scale_avail(struct device *dev,
 		ARRAY_SIZE(mma8452_scales));
 }
 
+static ssize_t mma8452_show_hp_cutoff_avail(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int i = mma8452_get_odr_index(data);
+
+	return mma8452_show_int_plus_micros(buf, mma8452_hp_filter_cutoff[i],
+		ARRAY_SIZE(mma8452_hp_filter_cutoff[0]));
+}
+
 static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(mma8452_show_samp_freq_avail);
 static IIO_DEVICE_ATTR(in_accel_scale_available, S_IRUGO,
 	mma8452_show_scale_avail, NULL, 0);
+static IIO_DEVICE_ATTR(in_accel_filter_high_pass_3db_frequency_available,
+			S_IRUGO, mma8452_show_hp_cutoff_avail, NULL, 0);
 
 static int mma8452_get_samp_freq_index(struct mma8452_data *data,
 	int val, int val2)
@@ -153,6 +223,31 @@ static int mma8452_get_scale_index(struct mma8452_data *data,
 		ARRAY_SIZE(mma8452_scales), val, val2);
 }
 
+static int mma8452_get_hp_filter_index(struct mma8452_data *data,
+				       int val, int val2)
+{
+	int i = mma8452_get_odr_index(data);
+
+	return mma8452_get_int_plus_micros_index(mma8452_hp_filter_cutoff[i],
+		ARRAY_SIZE(mma8452_scales[0]), val, val2);
+}
+
+static int mma8452_read_hp_filter(struct mma8452_data *data, int *hz, int *uHz)
+{
+	int i, ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, MMA8452_HP_FILTER_CUTOFF);
+	if (ret < 0)
+		return ret;
+
+	i = mma8452_get_odr_index(data);
+	ret &= MMA8452_HP_FILTER_CUTOFF_SEL_MASK;
+	*hz = mma8452_hp_filter_cutoff[i][ret][0];
+	*uHz = mma8452_hp_filter_cutoff[i][ret][1];
+
+	return 0;
+}
+
 static int mma8452_read_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *chan,
 			    int *val, int *val2, long mask)
@@ -180,8 +275,7 @@ static int mma8452_read_raw(struct iio_dev *indio_dev,
 		*val2 = mma8452_scales[i][1];
 		return IIO_VAL_INT_PLUS_MICRO;
 	case IIO_CHAN_INFO_SAMP_FREQ:
-		i = (data->ctrl_reg1 & MMA8452_CTRL_DR_MASK) >>
-			MMA8452_CTRL_DR_SHIFT;
+		i = mma8452_get_odr_index(data);
 		*val = mma8452_samp_freq[i][0];
 		*val2 = mma8452_samp_freq[i][1];
 		return IIO_VAL_INT_PLUS_MICRO;
@@ -192,6 +286,16 @@ static int mma8452_read_raw(struct iio_dev *indio_dev,
 			return ret;
 		*val = sign_extend32(ret, 7);
 		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_HIGH_PASS_FILTER_3DB_FREQUENCY:
+		if (data->data_cfg & MMA8452_DATA_CFG_HPF_MASK) {
+			ret = mma8452_read_hp_filter(data, val, val2);
+			if (ret < 0)
+				return ret;
+		} else {
+			*val = 0;
+			*val2 = 0;
+		}
+		return IIO_VAL_INT_PLUS_MICRO;
 	}
 	return -EINVAL;
 }
@@ -233,12 +337,31 @@ fail:
 	return ret;
 }
 
+static int mma8452_set_hp_filter_frequency(struct mma8452_data *data,
+					   int val, int val2)
+{
+	int i, reg;
+
+	i = mma8452_get_hp_filter_index(data, val, val2);
+	if (i < 0)
+		return -EINVAL;
+
+	reg = i2c_smbus_read_byte_data(data->client,
+				       MMA8452_HP_FILTER_CUTOFF);
+	if (reg < 0)
+		return reg;
+	reg &= ~MMA8452_HP_FILTER_CUTOFF_SEL_MASK;
+	reg |= i;
+
+	return mma8452_change_config(data, MMA8452_HP_FILTER_CUTOFF, reg);
+}
+
 static int mma8452_write_raw(struct iio_dev *indio_dev,
 			     struct iio_chan_spec const *chan,
 			     int val, int val2, long mask)
 {
 	struct mma8452_data *data = iio_priv(indio_dev);
-	int i;
+	int i, ret;
 
 	if (iio_buffer_enabled(indio_dev))
 		return -EBUSY;
@@ -266,11 +389,217 @@ static int mma8452_write_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 		return mma8452_change_config(data, MMA8452_OFF_X +
 			chan->scan_index, val);
+
+	case IIO_CHAN_INFO_HIGH_PASS_FILTER_3DB_FREQUENCY:
+		if (val == 0 && val2 == 0) {
+			data->data_cfg &= ~MMA8452_DATA_CFG_HPF_MASK;
+		} else {
+			data->data_cfg |= MMA8452_DATA_CFG_HPF_MASK;
+			ret = mma8452_set_hp_filter_frequency(data, val, val2);
+			if (ret < 0)
+				return ret;
+		}
+		return mma8452_change_config(data, MMA8452_DATA_CFG,
+						data->data_cfg);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mma8452_read_thresh(struct iio_dev *indio_dev,
+			       const struct iio_chan_spec *chan,
+			       enum iio_event_type type,
+			       enum iio_event_direction dir,
+			       enum iio_event_info info,
+			       int *val, int *val2)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int ret, us;
+
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		ret = i2c_smbus_read_byte_data(data->client,
+					       MMA8452_TRANSIENT_THS);
+		if (ret < 0)
+			return ret;
+
+		*val = ret & MMA8452_TRANSIENT_THS_MASK;
+		return IIO_VAL_INT;
+
+	case IIO_EV_INFO_PERIOD:
+		ret = i2c_smbus_read_byte_data(data->client,
+					       MMA8452_TRANSIENT_COUNT);
+		if (ret < 0)
+			return ret;
+
+		us = ret * mma8452_transient_time_step_us[
+				mma8452_get_odr_index(data)];
+		*val = us / USEC_PER_SEC;
+		*val2 = us % USEC_PER_SEC;
+		return IIO_VAL_INT_PLUS_MICRO;
+
+	case IIO_EV_INFO_HIGH_PASS_FILTER_3DB:
+		ret = i2c_smbus_read_byte_data(data->client,
+					       MMA8452_TRANSIENT_CFG);
+		if (ret < 0)
+			return ret;
+
+		if (ret & MMA8452_TRANSIENT_CFG_HPF_BYP) {
+			*val = 0;
+			*val2 = 0;
+		} else {
+			ret = mma8452_read_hp_filter(data, val, val2);
+			if (ret < 0)
+				return ret;
+		}
+		return IIO_VAL_INT_PLUS_MICRO;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mma8452_write_thresh(struct iio_dev *indio_dev,
+				const struct iio_chan_spec *chan,
+				enum iio_event_type type,
+				enum iio_event_direction dir,
+				enum iio_event_info info,
+				int val, int val2)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int ret, reg, steps;
+
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		return mma8452_change_config(data, MMA8452_TRANSIENT_THS,
+					     val & MMA8452_TRANSIENT_THS_MASK);
+
+	case IIO_EV_INFO_PERIOD:
+		steps = (val * USEC_PER_SEC + val2) /
+				mma8452_transient_time_step_us[
+					mma8452_get_odr_index(data)];
+
+		if (steps > 0xff)
+			return -EINVAL;
+
+		return mma8452_change_config(data, MMA8452_TRANSIENT_COUNT,
+					     steps);
+	case IIO_EV_INFO_HIGH_PASS_FILTER_3DB:
+		reg = i2c_smbus_read_byte_data(data->client,
+					       MMA8452_TRANSIENT_CFG);
+		if (reg < 0)
+			return reg;
+
+		if (val == 0 && val2 == 0) {
+			reg |= MMA8452_TRANSIENT_CFG_HPF_BYP;
+		} else {
+			reg &= ~MMA8452_TRANSIENT_CFG_HPF_BYP;
+			ret = mma8452_set_hp_filter_frequency(data, val, val2);
+			if (ret < 0)
+				return ret;
+		}
+		return mma8452_change_config(data, MMA8452_TRANSIENT_CFG, reg);
+
 	default:
 		return -EINVAL;
 	}
 }
 
+static int mma8452_read_event_config(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan,
+				     enum iio_event_type type,
+				     enum iio_event_direction dir)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, MMA8452_TRANSIENT_CFG);
+	if (ret < 0)
+		return ret;
+
+	return ret & MMA8452_TRANSIENT_CFG_CHAN(chan->scan_index) ? 1 : 0;
+}
+
+static int mma8452_write_event_config(struct iio_dev *indio_dev,
+				      const struct iio_chan_spec *chan,
+				      enum iio_event_type type,
+				      enum iio_event_direction dir,
+				      int state)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int val;
+
+	val = i2c_smbus_read_byte_data(data->client, MMA8452_TRANSIENT_CFG);
+	if (val < 0)
+		return val;
+
+	if (state)
+		val |= MMA8452_TRANSIENT_CFG_CHAN(chan->scan_index);
+	else
+		val &= ~MMA8452_TRANSIENT_CFG_CHAN(chan->scan_index);
+
+	val |= MMA8452_TRANSIENT_CFG_ELE;
+
+	return mma8452_change_config(data, MMA8452_TRANSIENT_CFG, val);
+}
+
+static void mma8452_transient_interrupt(struct iio_dev *indio_dev)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	s64 ts = iio_get_time_ns();
+	int src;
+
+	src = i2c_smbus_read_byte_data(data->client, MMA8452_TRANSIENT_SRC);
+	if (src < 0)
+		return;
+
+	if (src & MMA8452_TRANSIENT_SRC_XTRANSE)
+		iio_push_event(indio_dev,
+			       IIO_MOD_EVENT_CODE(IIO_ACCEL, 0, IIO_MOD_X,
+						  IIO_EV_TYPE_THRESH,
+						  IIO_EV_DIR_RISING),
+			       ts);
+
+	if (src & MMA8452_TRANSIENT_SRC_YTRANSE)
+		iio_push_event(indio_dev,
+			       IIO_MOD_EVENT_CODE(IIO_ACCEL, 0, IIO_MOD_Y,
+						  IIO_EV_TYPE_THRESH,
+						  IIO_EV_DIR_RISING),
+			       ts);
+
+	if (src & MMA8452_TRANSIENT_SRC_ZTRANSE)
+		iio_push_event(indio_dev,
+			       IIO_MOD_EVENT_CODE(IIO_ACCEL, 0, IIO_MOD_Z,
+						  IIO_EV_TYPE_THRESH,
+						  IIO_EV_DIR_RISING),
+			       ts);
+}
+
+static irqreturn_t mma8452_interrupt(int irq, void *p)
+{
+	struct iio_dev *indio_dev = p;
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int ret = IRQ_NONE;
+	int src;
+
+	src = i2c_smbus_read_byte_data(data->client, MMA8452_INT_SRC);
+	if (src < 0)
+		return IRQ_NONE;
+
+	if (src & MMA8452_INT_DRDY) {
+		iio_trigger_poll_chained(indio_dev->trig);
+		ret = IRQ_HANDLED;
+	}
+
+	if (src & MMA8452_INT_TRANS) {
+		mma8452_transient_interrupt(indio_dev);
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
 static irqreturn_t mma8452_trigger_handler(int irq, void *p)
 {
 	struct iio_poll_func *pf = p;
@@ -291,6 +620,55 @@ done:
 	return IRQ_HANDLED;
 }
 
+static int mma8452_reg_access_dbg(struct iio_dev *indio_dev,
+				  unsigned reg, unsigned writeval,
+				  unsigned *readval)
+{
+	int ret;
+	struct mma8452_data *data = iio_priv(indio_dev);
+
+	if (reg > MMA8452_MAX_REG)
+		return -EINVAL;
+
+	if (!readval)
+		return mma8452_change_config(data, reg, writeval);
+
+	ret = i2c_smbus_read_byte_data(data->client, reg);
+	if (ret < 0)
+		return ret;
+
+	*readval = ret;
+
+	return 0;
+}
+
+static const struct iio_event_spec mma8452_transient_event[] = {
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING,
+		.mask_separate = BIT(IIO_EV_INFO_ENABLE),
+		.mask_shared_by_type = BIT(IIO_EV_INFO_VALUE) |
+					BIT(IIO_EV_INFO_PERIOD) |
+					BIT(IIO_EV_INFO_HIGH_PASS_FILTER_3DB)
+	},
+};
+
+/*
+ * Threshold is configured in fixed 8G/127 steps regardless of
+ * currently selected scale for measurement.
+ */
+static IIO_CONST_ATTR_NAMED(accel_transient_scale, in_accel_scale, "0.617742");
+
+static struct attribute *mma8452_event_attributes[] = {
+	&iio_const_attr_accel_transient_scale.dev_attr.attr,
+	NULL,
+};
+
+static struct attribute_group mma8452_event_attribute_group = {
+	.attrs = mma8452_event_attributes,
+	.name = "events",
+};
+
 #define MMA8452_CHANNEL(axis, idx) { \
 	.type = IIO_ACCEL, \
 	.modified = 1, \
@@ -298,7 +676,8 @@ done:
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
 		BIT(IIO_CHAN_INFO_CALIBBIAS), \
 	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
-		BIT(IIO_CHAN_INFO_SCALE), \
+		BIT(IIO_CHAN_INFO_SCALE) | \
+		BIT(IIO_CHAN_INFO_HIGH_PASS_FILTER_3DB_FREQUENCY), \
 	.scan_index = idx, \
 	.scan_type = { \
 		.sign = 's', \
@@ -307,6 +686,8 @@ done:
 		.shift = 4, \
 		.endianness = IIO_BE, \
 	}, \
+	.event_spec = mma8452_transient_event, \
+	.num_event_specs = ARRAY_SIZE(mma8452_transient_event), \
 }
 
 static const struct iio_chan_spec mma8452_channels[] = {
@@ -319,6 +700,7 @@ static const struct iio_chan_spec mma8452_channels[] = {
 static struct attribute *mma8452_attributes[] = {
 	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
 	&iio_dev_attr_in_accel_scale_available.dev_attr.attr,
+	&iio_dev_attr_in_accel_filter_high_pass_3db_frequency_available.dev_attr.attr,
 	NULL
 };
 
@@ -330,11 +712,107 @@ static const struct iio_info mma8452_info = {
 	.attrs = &mma8452_group,
 	.read_raw = &mma8452_read_raw,
 	.write_raw = &mma8452_write_raw,
+	.event_attrs = &mma8452_event_attribute_group,
+	.read_event_value = &mma8452_read_thresh,
+	.write_event_value = &mma8452_write_thresh,
+	.read_event_config = &mma8452_read_event_config,
+	.write_event_config = &mma8452_write_event_config,
+	.debugfs_reg_access = &mma8452_reg_access_dbg,
 	.driver_module = THIS_MODULE,
 };
 
 static const unsigned long mma8452_scan_masks[] = {0x7, 0};
 
+static int mma8452_data_rdy_trigger_set_state(struct iio_trigger *trig,
+					      bool state)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct mma8452_data *data = iio_priv(indio_dev);
+	int reg;
+
+	reg = i2c_smbus_read_byte_data(data->client, MMA8452_CTRL_REG4);
+	if (reg < 0)
+		return reg;
+
+	if (state)
+		reg |= MMA8452_INT_DRDY;
+	else
+		reg &= ~MMA8452_INT_DRDY;
+
+	return mma8452_change_config(data, MMA8452_CTRL_REG4, reg);
+}
+
+static int mma8452_validate_device(struct iio_trigger *trig,
+				   struct iio_dev *indio_dev)
+{
+	struct iio_dev *indio = iio_trigger_get_drvdata(trig);
+
+	if (indio != indio_dev)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct iio_trigger_ops mma8452_trigger_ops = {
+	.set_trigger_state = mma8452_data_rdy_trigger_set_state,
+	.validate_device = mma8452_validate_device,
+	.owner = THIS_MODULE,
+};
+
+static int mma8452_trigger_setup(struct iio_dev *indio_dev)
+{
+	struct mma8452_data *data = iio_priv(indio_dev);
+	struct iio_trigger *trig;
+	int ret;
+
+	trig = devm_iio_trigger_alloc(&data->client->dev, "%s-dev%d",
+				      indio_dev->name,
+				      indio_dev->id);
+	if (!trig)
+		return -ENOMEM;
+
+	trig->dev.parent = &data->client->dev;
+	trig->ops = &mma8452_trigger_ops;
+	iio_trigger_set_drvdata(trig, indio_dev);
+
+	ret = iio_trigger_register(trig);
+	if (ret)
+		return ret;
+
+	indio_dev->trig = trig;
+	return 0;
+}
+
+static void mma8452_trigger_cleanup(struct iio_dev *indio_dev)
+{
+	if (indio_dev->trig)
+		iio_trigger_unregister(indio_dev->trig);
+}
+
+static int mma8452_reset(struct i2c_client *client)
+{
+	int i;
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client,	MMA8452_CTRL_REG2,
+					MMA8452_CTRL_REG2_RST);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < 10; i++) {
+		usleep_range(100, 200);
+		ret = i2c_smbus_read_byte_data(client, MMA8452_CTRL_REG2);
+		if (ret == -EIO)
+			continue; /* I2C comm reset */
+		if (ret < 0)
+			return ret;
+		if (!(ret & MMA8452_CTRL_REG2_RST))
+			return 0;
+	}
+
+	return -ETIMEDOUT;
+}
+
 static int mma8452_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
@@ -365,10 +843,7 @@ static int mma8452_probe(struct i2c_client *client,
 	indio_dev->num_channels = ARRAY_SIZE(mma8452_channels);
 	indio_dev->available_scan_masks = mma8452_scan_masks;
 
-	data->ctrl_reg1 = MMA8452_CTRL_ACTIVE |
-		(MMA8452_CTRL_DR_DEFAULT << MMA8452_CTRL_DR_SHIFT);
-	ret = i2c_smbus_write_byte_data(client, MMA8452_CTRL_REG1,
-		data->ctrl_reg1);
+	ret = mma8452_reset(client);
 	if (ret < 0)
 		return ret;
 
@@ -378,18 +853,77 @@ static int mma8452_probe(struct i2c_client *client,
 	if (ret < 0)
 		return ret;
 
+	/*
+	 * By default set transient threshold to max to avoid events if
+	 * enabling without configuring threshold.
+	 */
+	ret = i2c_smbus_write_byte_data(client, MMA8452_TRANSIENT_THS,
+					MMA8452_TRANSIENT_THS_MASK);
+	if (ret < 0)
+		return ret;
+
+	if (client->irq) {
+		/*
+		 * Although we enable the transient interrupt source once and
+		 * for all here the transient event detection itself is not
+		 * enabled until userspace asks for it by
+		 * mma8452_write_event_config()
+		 */
+		int supported_interrupts = MMA8452_INT_DRDY | MMA8452_INT_TRANS;
+		int enabled_interrupts = MMA8452_INT_TRANS;
+
+		/* Assume wired to INT1 pin */
+		ret = i2c_smbus_write_byte_data(client,
+						MMA8452_CTRL_REG5,
+						supported_interrupts);
+		if (ret < 0)
+			return ret;
+
+		ret = i2c_smbus_write_byte_data(client,
+						MMA8452_CTRL_REG4,
+						enabled_interrupts);
+		if (ret < 0)
+			return ret;
+
+		ret = mma8452_trigger_setup(indio_dev);
+		if (ret < 0)
+			return ret;
+	}
+
+	data->ctrl_reg1 = MMA8452_CTRL_ACTIVE |
+		(MMA8452_CTRL_DR_DEFAULT << MMA8452_CTRL_DR_SHIFT);
+	ret = i2c_smbus_write_byte_data(client, MMA8452_CTRL_REG1,
+					data->ctrl_reg1);
+	if (ret < 0)
+		goto trigger_cleanup;
+
 	ret = iio_triggered_buffer_setup(indio_dev, NULL,
 		mma8452_trigger_handler, NULL);
 	if (ret < 0)
-		return ret;
+		goto trigger_cleanup;
+
+	if (client->irq) {
+		ret = devm_request_threaded_irq(&client->dev,
+						client->irq,
+						NULL, mma8452_interrupt,
+						IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+						client->name, indio_dev);
+		if (ret)
+			goto buffer_cleanup;
+	}
 
 	ret = iio_device_register(indio_dev);
 	if (ret < 0)
 		goto buffer_cleanup;
+
 	return 0;
 
 buffer_cleanup:
 	iio_triggered_buffer_cleanup(indio_dev);
+
+trigger_cleanup:
+	mma8452_trigger_cleanup(indio_dev);
+
 	return ret;
 }
 
@@ -399,6 +933,7 @@ static int mma8452_remove(struct i2c_client *client)
 
 	iio_device_unregister(indio_dev);
 	iio_triggered_buffer_cleanup(indio_dev);
+	mma8452_trigger_cleanup(indio_dev);
 	mma8452_standby(iio_priv(indio_dev));
 
 	return 0;
diff --git a/drivers/iio/accel/mma9551_core.c b/drivers/iio/accel/mma9551_core.c
index c6d5a3a40b60..2fd2a995686b 100644
--- a/drivers/iio/accel/mma9551_core.c
+++ b/drivers/iio/accel/mma9551_core.c
@@ -374,7 +374,7 @@ EXPORT_SYMBOL(mma9551_read_status_word);
  * @app_id:	Application ID
  * @reg:	Application register
  * @len:	Length of array to read in bytes
- * @val:	Array of words to read
+ * @buf:	Array of words to read
  *
  * Read multiple configuration registers (word-sized registers).
  *
@@ -414,7 +414,7 @@ EXPORT_SYMBOL(mma9551_read_config_words);
  * @app_id:	Application ID
  * @reg:	Application register
  * @len:	Length of array to read in bytes
- * @val:	Array of words to read
+ * @buf:	Array of words to read
  *
  * Read multiple status registers (word-sized registers).
  *
@@ -454,7 +454,7 @@ EXPORT_SYMBOL(mma9551_read_status_words);
  * @app_id:	Application ID
  * @reg:	Application register
  * @len:	Length of array to write in bytes
- * @val:	Array of words to write
+ * @buf:	Array of words to write
  *
  * Write multiple configuration registers (word-sized registers).
  *
@@ -800,7 +800,7 @@ EXPORT_SYMBOL(mma9551_read_accel_scale);
  */
 int mma9551_app_reset(struct i2c_client *client, u32 app_mask)
 {
-	return mma9551_write_config_byte(client, MMA9551_APPID_RCS,
+	return mma9551_write_config_byte(client, MMA9551_APPID_RSC,
 					 MMA9551_RSC_RESET +
 					 MMA9551_RSC_OFFSET(app_mask),
 					 MMA9551_RSC_VAL(app_mask));
diff --git a/drivers/iio/accel/mma9551_core.h b/drivers/iio/accel/mma9551_core.h
index edaa56b1078e..79939e40805a 100644
--- a/drivers/iio/accel/mma9551_core.h
+++ b/drivers/iio/accel/mma9551_core.h
@@ -22,7 +22,7 @@
 #define MMA9551_APPID_TILT		0x0B
 #define MMA9551_APPID_SLEEP_WAKE	0x12
 #define MMA9551_APPID_PEDOMETER	        0x15
-#define MMA9551_APPID_RCS		0x17
+#define MMA9551_APPID_RSC		0x17
 #define MMA9551_APPID_NONE		0xff
 
 /* Reset/Suspend/Clear application app masks */
diff --git a/drivers/iio/accel/mma9553.c b/drivers/iio/accel/mma9553.c
index 365a109aaaef..8bfc61824fb2 100644
--- a/drivers/iio/accel/mma9553.c
+++ b/drivers/iio/accel/mma9553.c
@@ -63,8 +63,8 @@
 #define MMA9553_MASK_STATUS_STEPCHG		BIT(13)
 #define MMA9553_MASK_STATUS_ACTCHG		BIT(12)
 #define MMA9553_MASK_STATUS_SUSP		BIT(11)
-#define MMA9553_MASK_STATUS_ACTIVITY		(BIT(10) | BIT(9) | BIT(8))
-#define MMA9553_MASK_STATUS_VERSION		0x00FF
+#define MMA9553_MASK_STATUS_ACTIVITY		GENMASK(10, 8)
+#define MMA9553_MASK_STATUS_VERSION		GENMASK(7, 0)
 
 #define MMA9553_REG_STEPCNT			0x02
 #define MMA9553_REG_DISTANCE			0x04
@@ -76,14 +76,15 @@
 #define MMA9553_DEFAULT_GPIO_PIN	mma9551_gpio6
 #define MMA9553_DEFAULT_GPIO_POLARITY	0
 
-/* Bitnum used for gpio configuration = bit number in high status byte */
-#define STATUS_TO_BITNUM(bit)		(ffs(bit) - 9)
+/* Bitnum used for GPIO configuration = bit number in high status byte */
+#define MMA9553_STATUS_TO_BITNUM(bit)	(ffs(bit) - 9)
+#define MMA9553_MAX_BITNUM		MMA9553_STATUS_TO_BITNUM(BIT(16))
 
 #define MMA9553_DEFAULT_SAMPLE_RATE	30	/* Hz */
 
 /*
  * The internal activity level must be stable for ACTTHD samples before
- * ACTIVITY is updated.The ACTIVITY variable contains the current activity
+ * ACTIVITY is updated. The ACTIVITY variable contains the current activity
  * level and is updated every time a step is detected or once a second
  * if there are no steps.
  */
@@ -351,11 +352,11 @@ static int mma9553_conf_gpio(struct mma9553_data *data)
 	 * This bit is the logical OR of the SUSPCHG, STEPCHG, and ACTCHG flags.
 	 */
 	if (activity_enabled && ev_step_detect->enabled)
-		bitnum = STATUS_TO_BITNUM(MMA9553_MASK_STATUS_MRGFL);
+		bitnum = MMA9553_STATUS_TO_BITNUM(MMA9553_MASK_STATUS_MRGFL);
 	else if (ev_step_detect->enabled)
-		bitnum = STATUS_TO_BITNUM(MMA9553_MASK_STATUS_STEPCHG);
+		bitnum = MMA9553_STATUS_TO_BITNUM(MMA9553_MASK_STATUS_STEPCHG);
 	else if (activity_enabled)
-		bitnum = STATUS_TO_BITNUM(MMA9553_MASK_STATUS_ACTCHG);
+		bitnum = MMA9553_STATUS_TO_BITNUM(MMA9553_MASK_STATUS_ACTCHG);
 	else			/* Reset */
 		appid = MMA9551_APPID_NONE;
 
@@ -363,9 +364,12 @@ static int mma9553_conf_gpio(struct mma9553_data *data)
 		return 0;
 
 	/* Save initial values for activity and stepcnt */
-	if (activity_enabled || ev_step_detect->enabled)
-		mma9553_read_activity_stepcnt(data, &data->activity,
-					      &data->stepcnt);
+	if (activity_enabled || ev_step_detect->enabled) {
+		ret = mma9553_read_activity_stepcnt(data, &data->activity,
+						    &data->stepcnt);
+		if (ret < 0)
+			return ret;
+	}
 
 	ret = mma9551_gpio_config(data->client,
 				  MMA9553_DEFAULT_GPIO_PIN,
@@ -396,13 +400,13 @@ static int mma9553_init(struct mma9553_data *data)
 				      sizeof(data->conf), (u16 *) &data->conf);
 	if (ret < 0) {
 		dev_err(&data->client->dev,
-			"device is not MMA9553L: failed to read cfg regs\n");
+			"failed to read configuration registers\n");
 		return ret;
 	}
 
 
-	/* Reset gpio */
-	data->gpio_bitnum = -1;
+	/* Reset GPIO */
+	data->gpio_bitnum = MMA9553_MAX_BITNUM;
 	ret = mma9553_conf_gpio(data);
 	if (ret < 0)
 		return ret;
@@ -436,6 +440,32 @@ static int mma9553_init(struct mma9553_data *data)
 	return mma9551_set_device_state(data->client, true);
 }
 
+static int mma9553_read_status_word(struct mma9553_data *data, u16 reg,
+				    u16 *tmp)
+{
+	bool powered_on;
+	int ret;
+
+	/*
+	 * The HW only counts steps and other dependent
+	 * parameters (speed, distance, calories, activity)
+	 * if power is on (from enabling an event or the
+	 * step counter).
+	 */
+	powered_on = mma9553_is_any_event_enabled(data, false, 0) ||
+		     data->stepcnt_enabled;
+	if (!powered_on) {
+		dev_err(&data->client->dev, "No channels enabled\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&data->mutex);
+	ret = mma9551_read_status_word(data->client, MMA9551_APPID_PEDOMETER,
+				       reg, tmp);
+	mutex_unlock(&data->mutex);
+	return ret;
+}
+
 static int mma9553_read_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *chan,
 			    int *val, int *val2, long mask)
@@ -444,69 +474,30 @@ static int mma9553_read_raw(struct iio_dev *indio_dev,
 	int ret;
 	u16 tmp;
 	u8 activity;
-	bool powered_on;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_PROCESSED:
 		switch (chan->type) {
 		case IIO_STEPS:
-			/*
-			 * The HW only counts steps and other dependent
-			 * parameters (speed, distance, calories, activity)
-			 * if power is on (from enabling an event or the
-			 * step counter */
-			powered_on =
-			    mma9553_is_any_event_enabled(data, false, 0) ||
-			    data->stepcnt_enabled;
-			if (!powered_on) {
-				dev_err(&data->client->dev,
-					"No channels enabled\n");
-				return -EINVAL;
-			}
-			mutex_lock(&data->mutex);
-			ret = mma9551_read_status_word(data->client,
-						       MMA9551_APPID_PEDOMETER,
+			ret = mma9553_read_status_word(data,
 						       MMA9553_REG_STEPCNT,
 						       &tmp);
-			mutex_unlock(&data->mutex);
 			if (ret < 0)
 				return ret;
 			*val = tmp;
 			return IIO_VAL_INT;
 		case IIO_DISTANCE:
-			powered_on =
-			    mma9553_is_any_event_enabled(data, false, 0) ||
-			    data->stepcnt_enabled;
-			if (!powered_on) {
-				dev_err(&data->client->dev,
-					"No channels enabled\n");
-				return -EINVAL;
-			}
-			mutex_lock(&data->mutex);
-			ret = mma9551_read_status_word(data->client,
-						       MMA9551_APPID_PEDOMETER,
+			ret = mma9553_read_status_word(data,
 						       MMA9553_REG_DISTANCE,
 						       &tmp);
-			mutex_unlock(&data->mutex);
 			if (ret < 0)
 				return ret;
 			*val = tmp;
 			return IIO_VAL_INT;
 		case IIO_ACTIVITY:
-			powered_on =
-			    mma9553_is_any_event_enabled(data, false, 0) ||
-			    data->stepcnt_enabled;
-			if (!powered_on) {
-				dev_err(&data->client->dev,
-					"No channels enabled\n");
-				return -EINVAL;
-			}
-			mutex_lock(&data->mutex);
-			ret = mma9551_read_status_word(data->client,
-						       MMA9551_APPID_PEDOMETER,
+			ret = mma9553_read_status_word(data,
 						       MMA9553_REG_STATUS,
 						       &tmp);
-			mutex_unlock(&data->mutex);
 			if (ret < 0)
 				return ret;
 
@@ -531,38 +522,17 @@ static int mma9553_read_raw(struct iio_dev *indio_dev,
 		case IIO_VELOCITY:	/* m/h */
 			if (chan->channel2 != IIO_MOD_ROOT_SUM_SQUARED_X_Y_Z)
 				return -EINVAL;
-			powered_on =
-			    mma9553_is_any_event_enabled(data, false, 0) ||
-			    data->stepcnt_enabled;
-			if (!powered_on) {
-				dev_err(&data->client->dev,
-					"No channels enabled\n");
-				return -EINVAL;
-			}
-			mutex_lock(&data->mutex);
-			ret = mma9551_read_status_word(data->client,
-						       MMA9551_APPID_PEDOMETER,
-						       MMA9553_REG_SPEED, &tmp);
-			mutex_unlock(&data->mutex);
+			ret = mma9553_read_status_word(data,
+						       MMA9553_REG_SPEED,
+						       &tmp);
 			if (ret < 0)
 				return ret;
 			*val = tmp;
 			return IIO_VAL_INT;
 		case IIO_ENERGY:	/* Cal or kcal */
-			powered_on =
-			    mma9553_is_any_event_enabled(data, false, 0) ||
-			    data->stepcnt_enabled;
-			if (!powered_on) {
-				dev_err(&data->client->dev,
-					"No channels enabled\n");
-				return -EINVAL;
-			}
-			mutex_lock(&data->mutex);
-			ret = mma9551_read_status_word(data->client,
-						       MMA9551_APPID_PEDOMETER,
+			ret = mma9553_read_status_word(data,
 						       MMA9553_REG_CALORIES,
 						       &tmp);
-			mutex_unlock(&data->mutex);
 			if (ret < 0)
 				return ret;
 			*val = tmp;
@@ -789,7 +759,7 @@ static int mma9553_write_event_config(struct iio_dev *indio_dev,
 
 	mutex_unlock(&data->mutex);
 
-	return ret;
+	return 0;
 
 err_conf_gpio:
 	if (state) {
@@ -897,7 +867,7 @@ static int mma9553_get_calibgender_mode(struct iio_dev *indio_dev,
 	gender = mma9553_get_bits(data->conf.filter, MMA9553_MASK_CONF_MALE);
 	/*
 	 * HW expects 0 for female and 1 for male,
-	 * while iio index is 0 for male and 1 for female
+	 * while iio index is 0 for male and 1 for female.
 	 */
 	return !gender;
 }
@@ -944,11 +914,11 @@ static const struct iio_event_spec mma9553_activity_events[] = {
 	},
 };
 
-static const char * const calibgender_modes[] = { "male", "female" };
+static const char * const mma9553_calibgender_modes[] = { "male", "female" };
 
 static const struct iio_enum mma9553_calibgender_enum = {
-	.items = calibgender_modes,
-	.num_items = ARRAY_SIZE(calibgender_modes),
+	.items = mma9553_calibgender_modes,
+	.num_items = ARRAY_SIZE(mma9553_calibgender_modes),
 	.get = mma9553_get_calibgender_mode,
 	.set = mma9553_set_calibgender_mode,
 };
@@ -1110,16 +1080,16 @@ static int mma9553_gpio_probe(struct i2c_client *client)
 
 	dev = &client->dev;
 
-	/* data ready gpio interrupt pin */
+	/* data ready GPIO interrupt pin */
 	gpio = devm_gpiod_get_index(dev, MMA9553_GPIO_NAME, 0, GPIOD_IN);
 	if (IS_ERR(gpio)) {
-		dev_err(dev, "acpi gpio get index failed\n");
+		dev_err(dev, "ACPI GPIO get index failed\n");
 		return PTR_ERR(gpio);
 	}
 
 	ret = gpiod_to_irq(gpio);
 
-	dev_dbg(dev, "gpio resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
+	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
 
 	return ret;
 }
diff --git a/drivers/iio/accel/st_accel.h b/drivers/iio/accel/st_accel.h
index 7ee9724b1428..aa1001931d0c 100644
--- a/drivers/iio/accel/st_accel.h
+++ b/drivers/iio/accel/st_accel.h
@@ -20,6 +20,7 @@
 #define LSM330D_ACCEL_DEV_NAME		"lsm330d_accel"
 #define LSM330DL_ACCEL_DEV_NAME		"lsm330dl_accel"
 #define LSM330DLC_ACCEL_DEV_NAME	"lsm330dlc_accel"
+#define LIS331DL_ACCEL_DEV_NAME		"lis331dl_accel"
 #define LIS331DLH_ACCEL_DEV_NAME	"lis331dlh"
 #define LSM303DL_ACCEL_DEV_NAME		"lsm303dl_accel"
 #define LSM303DLH_ACCEL_DEV_NAME	"lsm303dlh_accel"
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 211b13271c61..4002e6410444 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -153,6 +153,44 @@
 #define ST_ACCEL_4_IG1_EN_MASK			0x08
 #define ST_ACCEL_4_MULTIREAD_BIT		true
 
+/* CUSTOM VALUES FOR SENSOR 5 */
+#define ST_ACCEL_5_WAI_EXP			0x3b
+#define ST_ACCEL_5_ODR_ADDR			0x20
+#define ST_ACCEL_5_ODR_MASK			0x80
+#define ST_ACCEL_5_ODR_AVL_100HZ_VAL		0x00
+#define ST_ACCEL_5_ODR_AVL_400HZ_VAL		0x01
+#define ST_ACCEL_5_PW_ADDR			0x20
+#define ST_ACCEL_5_PW_MASK			0x40
+#define ST_ACCEL_5_FS_ADDR			0x20
+#define ST_ACCEL_5_FS_MASK			0x20
+#define ST_ACCEL_5_FS_AVL_2_VAL			0X00
+#define ST_ACCEL_5_FS_AVL_8_VAL			0X01
+/* TODO: check these resulting gain settings, these are not in the datsheet */
+#define ST_ACCEL_5_FS_AVL_2_GAIN		IIO_G_TO_M_S_2(18000)
+#define ST_ACCEL_5_FS_AVL_8_GAIN		IIO_G_TO_M_S_2(72000)
+#define ST_ACCEL_5_DRDY_IRQ_ADDR		0x22
+#define ST_ACCEL_5_DRDY_IRQ_INT1_MASK		0x04
+#define ST_ACCEL_5_DRDY_IRQ_INT2_MASK		0x20
+#define ST_ACCEL_5_IG1_EN_ADDR			0x21
+#define ST_ACCEL_5_IG1_EN_MASK			0x08
+#define ST_ACCEL_5_MULTIREAD_BIT		false
+
+static const struct iio_chan_spec st_accel_8bit_channels[] = {
+	ST_SENSORS_LSM_CHANNELS(IIO_ACCEL,
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+			ST_SENSORS_SCAN_X, 1, IIO_MOD_X, 's', IIO_LE, 8, 8,
+			ST_ACCEL_DEFAULT_OUT_X_L_ADDR+1),
+	ST_SENSORS_LSM_CHANNELS(IIO_ACCEL,
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+			ST_SENSORS_SCAN_Y, 1, IIO_MOD_Y, 's', IIO_LE, 8, 8,
+			ST_ACCEL_DEFAULT_OUT_Y_L_ADDR+1),
+	ST_SENSORS_LSM_CHANNELS(IIO_ACCEL,
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+			ST_SENSORS_SCAN_Z, 1, IIO_MOD_Z, 's', IIO_LE, 8, 8,
+			ST_ACCEL_DEFAULT_OUT_Z_L_ADDR+1),
+	IIO_CHAN_SOFT_TIMESTAMP(3)
+};
+
 static const struct iio_chan_spec st_accel_12bit_channels[] = {
 	ST_SENSORS_LSM_CHANNELS(IIO_ACCEL,
 			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
@@ -454,6 +492,54 @@ static const struct st_sensor_settings st_accel_sensors_settings[] = {
 		.multi_read_bit = ST_ACCEL_4_MULTIREAD_BIT,
 		.bootime = 2, /* guess */
 	},
+	{
+		.wai = ST_ACCEL_5_WAI_EXP,
+		.sensors_supported = {
+			[0] = LIS331DL_ACCEL_DEV_NAME,
+		},
+		.ch = (struct iio_chan_spec *)st_accel_8bit_channels,
+		.odr = {
+			.addr = ST_ACCEL_5_ODR_ADDR,
+			.mask = ST_ACCEL_5_ODR_MASK,
+			.odr_avl = {
+				{ 100, ST_ACCEL_5_ODR_AVL_100HZ_VAL },
+				{ 400, ST_ACCEL_5_ODR_AVL_400HZ_VAL, },
+			},
+		},
+		.pw = {
+			.addr = ST_ACCEL_5_PW_ADDR,
+			.mask = ST_ACCEL_5_PW_MASK,
+			.value_on = ST_SENSORS_DEFAULT_POWER_ON_VALUE,
+			.value_off = ST_SENSORS_DEFAULT_POWER_OFF_VALUE,
+		},
+		.enable_axis = {
+			.addr = ST_SENSORS_DEFAULT_AXIS_ADDR,
+			.mask = ST_SENSORS_DEFAULT_AXIS_MASK,
+		},
+		.fs = {
+			.addr = ST_ACCEL_5_FS_ADDR,
+			.mask = ST_ACCEL_5_FS_MASK,
+			.fs_avl = {
+				[0] = {
+					.num = ST_ACCEL_FS_AVL_2G,
+					.value = ST_ACCEL_5_FS_AVL_2_VAL,
+					.gain = ST_ACCEL_5_FS_AVL_2_GAIN,
+				},
+				[1] = {
+					.num = ST_ACCEL_FS_AVL_8G,
+					.value = ST_ACCEL_5_FS_AVL_8_VAL,
+					.gain = ST_ACCEL_5_FS_AVL_8_GAIN,
+				},
+			},
+		},
+		.drdy_irq = {
+			.addr = ST_ACCEL_5_DRDY_IRQ_ADDR,
+			.mask_int1 = ST_ACCEL_5_DRDY_IRQ_INT1_MASK,
+			.mask_int2 = ST_ACCEL_5_DRDY_IRQ_INT2_MASK,
+		},
+		.multi_read_bit = ST_ACCEL_5_MULTIREAD_BIT,
+		.bootime = 2, /* guess */
+	},
 };
 
 static int st_accel_read_raw(struct iio_dev *indio_dev,
diff --git a/drivers/iio/accel/st_accel_i2c.c b/drivers/iio/accel/st_accel_i2c.c
index 6b720c190b2d..d4ad72ca4a3d 100644
--- a/drivers/iio/accel/st_accel_i2c.c
+++ b/drivers/iio/accel/st_accel_i2c.c
@@ -49,6 +49,10 @@ static const struct of_device_id st_accel_of_match[] = {
 		.data = LSM330DLC_ACCEL_DEV_NAME,
 	},
 	{
+		.compatible = "st,lis331dl-accel",
+		.data = LIS331DL_ACCEL_DEV_NAME,
+	},
+	{
 		.compatible = "st,lis331dlh-accel",
 		.data = LIS331DLH_ACCEL_DEV_NAME,
 	},
diff --git a/drivers/iio/accel/stk8312.c b/drivers/iio/accel/stk8312.c
new file mode 100644
index 000000000000..d211d9f3975b
--- /dev/null
+++ b/drivers/iio/accel/stk8312.c
@@ -0,0 +1,390 @@
+/**
+ * Sensortek STK8312 3-Axis Accelerometer
+ *
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * IIO driver for STK8312; 7-bit I2C address: 0x3D.
+ */
+
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define STK8312_REG_XOUT		0x00
+#define STK8312_REG_YOUT		0x01
+#define STK8312_REG_ZOUT		0x02
+#define STK8312_REG_MODE		0x07
+#define STK8312_REG_STH			0x13
+#define STK8312_REG_RESET		0x20
+#define STK8312_REG_AFECTRL		0x24
+#define STK8312_REG_OTPADDR		0x3D
+#define STK8312_REG_OTPDATA		0x3E
+#define STK8312_REG_OTPCTRL		0x3F
+
+#define STK8312_MODE_ACTIVE		1
+#define STK8312_MODE_STANDBY		0
+#define STK8312_MODE_MASK		0x01
+#define STK8312_RNG_MASK		0xC0
+#define STK8312_RNG_SHIFT		6
+#define STK8312_READ_RETRIES		16
+
+#define STK8312_DRIVER_NAME		"stk8312"
+
+/*
+ * The accelerometer has two measurement ranges:
+ *
+ * -6g - +6g (8-bit, signed)
+ * -16g - +16g (8-bit, signed)
+ *
+ * scale1 = (6 + 6) * 9.81 / (2^8 - 1)     = 0.4616
+ * scale2 = (16 + 16) * 9.81 / (2^8 - 1)   = 1.2311
+ */
+#define STK8312_SCALE_AVAIL		"0.4616 1.2311"
+
+static const int stk8312_scale_table[][2] = {
+	{0, 461600}, {1, 231100}
+};
+
+#define STK8312_ACCEL_CHANNEL(reg, axis) {			\
+	.type = IIO_ACCEL,					\
+	.address = reg,						\
+	.modified = 1,						\
+	.channel2 = IIO_MOD_##axis,				\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+}
+
+static const struct iio_chan_spec stk8312_channels[] = {
+	STK8312_ACCEL_CHANNEL(STK8312_REG_XOUT, X),
+	STK8312_ACCEL_CHANNEL(STK8312_REG_YOUT, Y),
+	STK8312_ACCEL_CHANNEL(STK8312_REG_ZOUT, Z),
+};
+
+struct stk8312_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	int range;
+	u8 mode;
+};
+
+static IIO_CONST_ATTR(in_accel_scale_available, STK8312_SCALE_AVAIL);
+
+static struct attribute *stk8312_attributes[] = {
+	&iio_const_attr_in_accel_scale_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group stk8312_attribute_group = {
+	.attrs = stk8312_attributes
+};
+
+static int stk8312_otp_init(struct stk8312_data *data)
+{
+	int ret;
+	int count = 10;
+	struct i2c_client *client = data->client;
+
+	ret = i2c_smbus_write_byte_data(client, STK8312_REG_OTPADDR, 0x70);
+	if (ret < 0)
+		goto exit_err;
+	ret = i2c_smbus_write_byte_data(client, STK8312_REG_OTPCTRL, 0x02);
+	if (ret < 0)
+		goto exit_err;
+
+	do {
+		usleep_range(1000, 5000);
+		ret = i2c_smbus_read_byte_data(client, STK8312_REG_OTPCTRL);
+		if (ret < 0)
+			goto exit_err;
+		count--;
+	} while (!(ret & 0x80) && count > 0);
+
+	if (count == 0)
+		goto exit_err;
+
+	ret = i2c_smbus_read_byte_data(client, STK8312_REG_OTPDATA);
+	if (ret < 0)
+		goto exit_err;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+			STK8312_REG_AFECTRL, ret);
+	if (ret < 0)
+		goto exit_err;
+	msleep(150);
+
+	return ret;
+
+exit_err:
+	dev_err(&client->dev, "failed to initialize sensor\n");
+	return ret;
+}
+
+static int stk8312_set_mode(struct stk8312_data *data, u8 mode)
+{
+	int ret;
+	u8 masked_reg;
+	struct i2c_client *client = data->client;
+
+	if (mode > 1)
+		return -EINVAL;
+	else if (mode == data->mode)
+		return 0;
+
+	ret = i2c_smbus_read_byte_data(client, STK8312_REG_MODE);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to change sensor mode\n");
+		return ret;
+	}
+	masked_reg = ret & (~STK8312_MODE_MASK);
+	masked_reg |= mode;
+
+	ret = i2c_smbus_write_byte_data(client,
+			STK8312_REG_MODE, masked_reg);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to change sensor mode\n");
+		return ret;
+	}
+
+	data->mode = mode;
+	if (mode == STK8312_MODE_ACTIVE) {
+		/* Need to run OTP sequence before entering active mode */
+		usleep_range(1000, 5000);
+		ret = stk8312_otp_init(data);
+	}
+
+	return ret;
+}
+
+static int stk8312_set_range(struct stk8312_data *data, u8 range)
+{
+	int ret;
+	u8 masked_reg;
+	u8 mode;
+	struct i2c_client *client = data->client;
+
+	if (range != 1 && range != 2)
+		return -EINVAL;
+	else if (range == data->range)
+		return 0;
+
+	mode = data->mode;
+	/* We need to go in standby mode to modify registers */
+	ret = stk8312_set_mode(data, STK8312_MODE_STANDBY);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_read_byte_data(client, STK8312_REG_STH);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to change sensor range\n");
+		return ret;
+	}
+
+	masked_reg = ret & (~STK8312_RNG_MASK);
+	masked_reg |= range << STK8312_RNG_SHIFT;
+
+	ret = i2c_smbus_write_byte_data(client, STK8312_REG_STH, masked_reg);
+	if (ret < 0)
+		dev_err(&client->dev, "failed to change sensor range\n");
+	else
+		data->range = range;
+
+	return stk8312_set_mode(data, mode);
+}
+
+static int stk8312_read_accel(struct stk8312_data *data, u8 address)
+{
+	int ret;
+	struct i2c_client *client = data->client;
+
+	if (address > 2)
+		return -EINVAL;
+
+	ret = i2c_smbus_read_byte_data(client, address);
+	if (ret < 0) {
+		dev_err(&client->dev, "register read failed\n");
+		return ret;
+	}
+
+	return sign_extend32(ret, 7);
+}
+
+static int stk8312_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct stk8312_data *data = iio_priv(indio_dev);
+
+	if (chan->type != IIO_ACCEL)
+		return -EINVAL;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&data->lock);
+		*val = stk8312_read_accel(data, chan->address);
+		mutex_unlock(&data->lock);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = stk8312_scale_table[data->range - 1][0];
+		*val2 = stk8312_scale_table[data->range - 1][1];
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+
+	return -EINVAL;
+}
+
+static int stk8312_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	int i;
+	int index = -1;
+	int ret;
+	struct stk8312_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		for (i = 0; i < ARRAY_SIZE(stk8312_scale_table); i++)
+			if (val == stk8312_scale_table[i][0] &&
+			    val2 == stk8312_scale_table[i][1]) {
+				index = i + 1;
+				break;
+			}
+		if (index < 0)
+			return -EINVAL;
+
+		mutex_lock(&data->lock);
+		ret = stk8312_set_range(data, index);
+		mutex_unlock(&data->lock);
+
+		return ret;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info stk8312_info = {
+	.driver_module		= THIS_MODULE,
+	.read_raw		= stk8312_read_raw,
+	.write_raw		= stk8312_write_raw,
+	.attrs			= &stk8312_attribute_group,
+};
+
+static int stk8312_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct stk8312_data *data;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev) {
+		dev_err(&client->dev, "iio allocation failed!\n");
+		return -ENOMEM;
+	}
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	i2c_set_clientdata(client, indio_dev);
+	mutex_init(&data->lock);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &stk8312_info;
+	indio_dev->name = STK8312_DRIVER_NAME;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = stk8312_channels;
+	indio_dev->num_channels = ARRAY_SIZE(stk8312_channels);
+
+	/* A software reset is recommended at power-on */
+	ret = i2c_smbus_write_byte_data(data->client, STK8312_REG_RESET, 0x00);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to reset sensor\n");
+		return ret;
+	}
+	ret = stk8312_set_range(data, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = stk8312_set_mode(data, STK8312_MODE_ACTIVE);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "device_register failed\n");
+		stk8312_set_mode(data, STK8312_MODE_STANDBY);
+	}
+
+	return ret;
+}
+
+static int stk8312_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+
+	return stk8312_set_mode(iio_priv(indio_dev), STK8312_MODE_STANDBY);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int stk8312_suspend(struct device *dev)
+{
+	struct stk8312_data *data;
+
+	data = iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	return stk8312_set_mode(data, STK8312_MODE_STANDBY);
+}
+
+static int stk8312_resume(struct device *dev)
+{
+	struct stk8312_data *data;
+
+	data = iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	return stk8312_set_mode(data, STK8312_MODE_ACTIVE);
+}
+
+static SIMPLE_DEV_PM_OPS(stk8312_pm_ops, stk8312_suspend, stk8312_resume);
+
+#define STK8312_PM_OPS (&stk8312_pm_ops)
+#else
+#define STK8312_PM_OPS NULL
+#endif
+
+static const struct i2c_device_id stk8312_i2c_id[] = {
+	{"STK8312", 0},
+	{}
+};
+
+static const struct acpi_device_id stk8312_acpi_id[] = {
+	{"STK8312", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, stk8312_acpi_id);
+
+static struct i2c_driver stk8312_driver = {
+	.driver = {
+		.name = "stk8312",
+		.pm = STK8312_PM_OPS,
+		.acpi_match_table = ACPI_PTR(stk8312_acpi_id),
+	},
+	.probe =            stk8312_probe,
+	.remove =           stk8312_remove,
+	.id_table =         stk8312_i2c_id,
+};
+
+module_i2c_driver(stk8312_driver);
+
+MODULE_AUTHOR("Tiberiu Breana <tiberiu.a.breana@intel.com>");
+MODULE_DESCRIPTION("STK8312 3-Axis Accelerometer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/stk8ba50.c b/drivers/iio/accel/stk8ba50.c
new file mode 100644
index 000000000000..30950c6b36de
--- /dev/null
+++ b/drivers/iio/accel/stk8ba50.c
@@ -0,0 +1,302 @@
+/**
+ * Sensortek STK8BA50 3-Axis Accelerometer
+ *
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * STK8BA50 7-bit I2C address: 0x18.
+ */
+
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define STK8BA50_REG_XOUT			0x02
+#define STK8BA50_REG_YOUT			0x04
+#define STK8BA50_REG_ZOUT			0x06
+#define STK8BA50_REG_RANGE			0x0F
+#define STK8BA50_REG_POWMODE			0x11
+#define STK8BA50_REG_SWRST			0x14
+
+#define STK8BA50_MODE_NORMAL			0
+#define STK8BA50_MODE_SUSPEND			1
+#define STK8BA50_MODE_POWERBIT			BIT(7)
+#define STK8BA50_DATA_SHIFT			6
+#define STK8BA50_RESET_CMD			0xB6
+
+#define STK8BA50_DRIVER_NAME			"stk8ba50"
+
+#define STK8BA50_SCALE_AVAIL			"0.0384 0.0767 0.1534 0.3069"
+
+/*
+ * The accelerometer has four measurement ranges:
+ * +/-2g; +/-4g; +/-8g; +/-16g
+ *
+ * Acceleration values are 10-bit, 2's complement.
+ * Scales are calculated as following:
+ *
+ * scale1 = (2 + 2) * 9.81 / (2^10 - 1)   = 0.0384
+ * scale2 = (4 + 4) * 9.81 / (2^10 - 1)   = 0.0767
+ * etc.
+ *
+ * Scales are stored in this format:
+ * { <register value>, <scale value> }
+ *
+ * Locally, the range is stored as a table index.
+ */
+static const int stk8ba50_scale_table[][2] = {
+	{3, 38400}, {5, 76700}, {8, 153400}, {12, 306900}
+};
+
+struct stk8ba50_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	int range;
+};
+
+#define STK8BA50_ACCEL_CHANNEL(reg, axis) {			\
+	.type = IIO_ACCEL,					\
+	.address = reg,						\
+	.modified = 1,						\
+	.channel2 = IIO_MOD_##axis,				\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+}
+
+static const struct iio_chan_spec stk8ba50_channels[] = {
+	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_XOUT, X),
+	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_YOUT, Y),
+	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_ZOUT, Z),
+};
+
+static IIO_CONST_ATTR(in_accel_scale_available, STK8BA50_SCALE_AVAIL);
+
+static struct attribute *stk8ba50_attributes[] = {
+	&iio_const_attr_in_accel_scale_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group stk8ba50_attribute_group = {
+	.attrs = stk8ba50_attributes
+};
+
+static int stk8ba50_read_accel(struct stk8ba50_data *data, u8 reg)
+{
+	int ret;
+	struct i2c_client *client = data->client;
+
+	ret = i2c_smbus_read_word_data(client, reg);
+	if (ret < 0) {
+		dev_err(&client->dev, "register read failed\n");
+		return ret;
+	}
+
+	return sign_extend32(ret >> STK8BA50_DATA_SHIFT, 9);
+}
+
+static int stk8ba50_read_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int *val, int *val2, long mask)
+{
+	struct stk8ba50_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&data->lock);
+		*val = stk8ba50_read_accel(data, chan->address);
+		mutex_unlock(&data->lock);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = stk8ba50_scale_table[data->range][1];
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+
+	return -EINVAL;
+}
+
+static int stk8ba50_write_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int val, int val2, long mask)
+{
+	int ret;
+	int i;
+	int index = -1;
+	struct stk8ba50_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		if (val != 0)
+			return -EINVAL;
+
+		for (i = 0; i < ARRAY_SIZE(stk8ba50_scale_table); i++)
+			if (val2 == stk8ba50_scale_table[i][1]) {
+				index = i;
+				break;
+			}
+		if (index < 0)
+			return -EINVAL;
+
+		ret = i2c_smbus_write_byte_data(data->client,
+				STK8BA50_REG_RANGE,
+				stk8ba50_scale_table[index][0]);
+		if (ret < 0)
+			dev_err(&data->client->dev,
+					"failed to set measurement range\n");
+		else
+			data->range = index;
+
+		return ret;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info stk8ba50_info = {
+	.driver_module		= THIS_MODULE,
+	.read_raw		= stk8ba50_read_raw,
+	.write_raw		= stk8ba50_write_raw,
+	.attrs			= &stk8ba50_attribute_group,
+};
+
+static int stk8ba50_set_power(struct stk8ba50_data *data, bool mode)
+{
+	int ret;
+	u8 masked_reg;
+	struct i2c_client *client = data->client;
+
+	ret = i2c_smbus_read_byte_data(client, STK8BA50_REG_POWMODE);
+	if (ret < 0)
+		goto exit_err;
+
+	if (mode)
+		masked_reg = ret | STK8BA50_MODE_POWERBIT;
+	else
+		masked_reg = ret & (~STK8BA50_MODE_POWERBIT);
+
+	ret = i2c_smbus_write_byte_data(client, STK8BA50_REG_POWMODE,
+					masked_reg);
+	if (ret < 0)
+		goto exit_err;
+
+	return ret;
+
+exit_err:
+	dev_err(&client->dev, "failed to change sensor mode\n");
+	return ret;
+}
+
+static int stk8ba50_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct stk8ba50_data *data;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev) {
+		dev_err(&client->dev, "iio allocation failed!\n");
+		return -ENOMEM;
+	}
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	i2c_set_clientdata(client, indio_dev);
+	mutex_init(&data->lock);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &stk8ba50_info;
+	indio_dev->name = STK8BA50_DRIVER_NAME;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = stk8ba50_channels;
+	indio_dev->num_channels = ARRAY_SIZE(stk8ba50_channels);
+
+	/* Reset all registers on startup */
+	ret = i2c_smbus_write_byte_data(client,
+			STK8BA50_REG_SWRST, STK8BA50_RESET_CMD);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to reset sensor\n");
+		return ret;
+	}
+
+	/* The default range is +/-2g */
+	data->range = 0;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "device_register failed\n");
+		stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
+	}
+
+	return ret;
+}
+
+static int stk8ba50_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+
+	return stk8ba50_set_power(iio_priv(indio_dev), STK8BA50_MODE_SUSPEND);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int stk8ba50_suspend(struct device *dev)
+{
+	struct stk8ba50_data *data;
+
+	data = iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	return stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
+}
+
+static int stk8ba50_resume(struct device *dev)
+{
+	struct stk8ba50_data *data;
+
+	data = iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	return stk8ba50_set_power(data, STK8BA50_MODE_NORMAL);
+}
+
+static SIMPLE_DEV_PM_OPS(stk8ba50_pm_ops, stk8ba50_suspend, stk8ba50_resume);
+
+#define STK8BA50_PM_OPS (&stk8ba50_pm_ops)
+#else
+#define STK8BA50_PM_OPS NULL
+#endif
+
+static const struct i2c_device_id stk8ba50_i2c_id[] = {
+	{"stk8ba50", 0},
+	{}
+};
+
+static const struct acpi_device_id stk8ba50_acpi_id[] = {
+	{"STK8BA50", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, stk8ba50_acpi_id);
+
+static struct i2c_driver stk8ba50_driver = {
+	.driver = {
+		.name = "stk8ba50",
+		.pm = STK8BA50_PM_OPS,
+		.acpi_match_table = ACPI_PTR(stk8ba50_acpi_id),
+	},
+	.probe =            stk8ba50_probe,
+	.remove =           stk8ba50_remove,
+	.id_table =         stk8ba50_i2c_id,
+};
+
+module_i2c_driver(stk8ba50_driver);
+
+MODULE_AUTHOR("Tiberiu Breana <tiberiu.a.breana@intel.com>");
+MODULE_DESCRIPTION("STK8BA50 3-Axis Accelerometer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index e36a73e7c3a8..7c5565891cb8 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -135,6 +135,13 @@ config AXP288_ADC
 	  device. Depending on platform configuration, this general purpose ADC can
 	  be used for sampling sensors such as thermal resistors.
 
+config BERLIN2_ADC
+	tristate "Marvell Berlin2 ADC driver"
+	depends on ARCH_BERLIN
+	help
+	  Marvell Berlin2 ADC driver. This ADC has 8 channels, with one used for
+	  temperature measurement.
+
 config DA9150_GPADC
 	tristate "Dialog DA9150 GPADC driver support"
 	depends on MFD_DA9150
@@ -285,11 +292,11 @@ config TI_ADC081C
 	  called ti-adc081c.
 
 config TI_ADC128S052
-	tristate "Texas Instruments ADC128S052"
+	tristate "Texas Instruments ADC128S052/ADC122S021"
 	depends on SPI
 	help
 	  If you say yes here you get support for Texas Instruments ADC128S052
-	  chip.
+	  and ADC122S021 chips.
 
 	  This driver can also be built as a module. If so, the module will be
 	  called ti-adc128s052.
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 3930e63e84bc..a0962103e866 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_AD7887) += ad7887.o
 obj-$(CONFIG_AD799X) += ad799x.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
 obj-$(CONFIG_AXP288_ADC) += axp288_adc.o
+obj-$(CONFIG_BERLIN2_ADC) += berlin2-adc.o
 obj-$(CONFIG_DA9150_GPADC) += da9150-gpadc.o
 obj-$(CONFIG_CC10001_ADC) += cc10001_adc.o
 obj-$(CONFIG_EXYNOS_ADC) += exynos_adc.o
diff --git a/drivers/iio/adc/axp288_adc.c b/drivers/iio/adc/axp288_adc.c
index 56008a86b78f..0c904edd6c00 100644
--- a/drivers/iio/adc/axp288_adc.c
+++ b/drivers/iio/adc/axp288_adc.c
@@ -238,7 +238,7 @@ static int axp288_adc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id axp288_adc_id_table[] = {
+static const struct platform_device_id axp288_adc_id_table[] = {
 	{ .name = "axp288_adc" },
 	{},
 };
diff --git a/drivers/iio/adc/berlin2-adc.c b/drivers/iio/adc/berlin2-adc.c
new file mode 100644
index 000000000000..aecc9ad995ad
--- /dev/null
+++ b/drivers/iio/adc/berlin2-adc.c
@@ -0,0 +1,378 @@
+/*
+ * Marvell Berlin2 ADC driver
+ *
+ * Copyright (C) 2015 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/iio/iio.h>
+#include <linux/iio/driver.h>
+#include <linux/iio/machine.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+
+#define BERLIN2_SM_CTRL				0x14
+#define  BERLIN2_SM_CTRL_SM_SOC_INT		BIT(1)
+#define  BERLIN2_SM_CTRL_SOC_SM_INT		BIT(2)
+#define  BERLIN2_SM_CTRL_ADC_SEL(x)		(BIT(x) << 5)	/* 0-15 */
+#define  BERLIN2_SM_CTRL_ADC_SEL_MASK		(0xf << 5)
+#define  BERLIN2_SM_CTRL_ADC_POWER		BIT(9)
+#define  BERLIN2_SM_CTRL_ADC_CLKSEL_DIV2	(0x0 << 10)
+#define  BERLIN2_SM_CTRL_ADC_CLKSEL_DIV3	(0x1 << 10)
+#define  BERLIN2_SM_CTRL_ADC_CLKSEL_DIV4	(0x2 << 10)
+#define  BERLIN2_SM_CTRL_ADC_CLKSEL_DIV8	(0x3 << 10)
+#define  BERLIN2_SM_CTRL_ADC_CLKSEL_MASK	(0x3 << 10)
+#define  BERLIN2_SM_CTRL_ADC_START		BIT(12)
+#define  BERLIN2_SM_CTRL_ADC_RESET		BIT(13)
+#define  BERLIN2_SM_CTRL_ADC_BANDGAP_RDY	BIT(14)
+#define  BERLIN2_SM_CTRL_ADC_CONT_SINGLE	(0x0 << 15)
+#define  BERLIN2_SM_CTRL_ADC_CONT_CONTINUOUS	(0x1 << 15)
+#define  BERLIN2_SM_CTRL_ADC_BUFFER_EN		BIT(16)
+#define  BERLIN2_SM_CTRL_ADC_VREF_EXT		(0x0 << 17)
+#define  BERLIN2_SM_CTRL_ADC_VREF_INT		(0x1 << 17)
+#define  BERLIN2_SM_CTRL_ADC_ROTATE		BIT(19)
+#define  BERLIN2_SM_CTRL_TSEN_EN		BIT(20)
+#define  BERLIN2_SM_CTRL_TSEN_CLK_SEL_125	(0x0 << 21)	/* 1.25 MHz */
+#define  BERLIN2_SM_CTRL_TSEN_CLK_SEL_250	(0x1 << 21)	/* 2.5 MHz */
+#define  BERLIN2_SM_CTRL_TSEN_MODE_0_125	(0x0 << 22)	/* 0-125 C */
+#define  BERLIN2_SM_CTRL_TSEN_MODE_10_50	(0x1 << 22)	/* 10-50 C */
+#define  BERLIN2_SM_CTRL_TSEN_RESET		BIT(29)
+#define BERLIN2_SM_ADC_DATA			0x20
+#define  BERLIN2_SM_ADC_MASK			0x3ff
+#define BERLIN2_SM_ADC_STATUS			0x1c
+#define  BERLIN2_SM_ADC_STATUS_DATA_RDY(x)	BIT(x)		/* 0-15 */
+#define  BERLIN2_SM_ADC_STATUS_DATA_RDY_MASK	0xf
+#define  BERLIN2_SM_ADC_STATUS_INT_EN(x)	(BIT(x) << 16)	/* 0-15 */
+#define  BERLIN2_SM_ADC_STATUS_INT_EN_MASK	(0xf << 16)
+#define BERLIN2_SM_TSEN_STATUS			0x24
+#define  BERLIN2_SM_TSEN_STATUS_DATA_RDY	BIT(0)
+#define  BERLIN2_SM_TSEN_STATUS_INT_EN		BIT(1)
+#define BERLIN2_SM_TSEN_DATA			0x28
+#define  BERLIN2_SM_TSEN_MASK			0xfff
+#define BERLIN2_SM_TSEN_CTRL			0x74
+#define  BERLIN2_SM_TSEN_CTRL_START		BIT(8)
+#define  BERLIN2_SM_TSEN_CTRL_SETTLING_4	(0x0 << 21)	/* 4 us */
+#define  BERLIN2_SM_TSEN_CTRL_SETTLING_12	(0x1 << 21)	/* 12 us */
+#define  BERLIN2_SM_TSEN_CTRL_SETTLING_MASK	(0x1 << 21)
+#define  BERLIN2_SM_TSEN_CTRL_TRIM(x)		((x) << 22)
+#define  BERLIN2_SM_TSEN_CTRL_TRIM_MASK		(0xf << 22)
+
+struct berlin2_adc_priv {
+	struct regmap		*regmap;
+	struct mutex		lock;
+	wait_queue_head_t	wq;
+	bool			data_available;
+	int			data;
+};
+
+#define BERLIN2_ADC_CHANNEL(n, t)					\
+		{							\
+			.channel	= n,				\
+			.datasheet_name	= "channel"#n,			\
+			.type		= t,				\
+			.indexed	= 1,				\
+			.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+		}
+
+static struct iio_chan_spec berlin2_adc_channels[] = {
+	BERLIN2_ADC_CHANNEL(0, IIO_VOLTAGE),	/* external input */
+	BERLIN2_ADC_CHANNEL(1, IIO_VOLTAGE),	/* external input */
+	BERLIN2_ADC_CHANNEL(2, IIO_VOLTAGE),	/* external input */
+	BERLIN2_ADC_CHANNEL(3, IIO_VOLTAGE),	/* external input */
+	BERLIN2_ADC_CHANNEL(4, IIO_VOLTAGE),	/* reserved */
+	BERLIN2_ADC_CHANNEL(5, IIO_VOLTAGE),	/* reserved */
+	{					/* temperature sensor */
+		.channel		= 6,
+		.datasheet_name		= "channel6",
+		.type			= IIO_TEMP,
+		.indexed		= 0,
+		.info_mask_separate	= BIT(IIO_CHAN_INFO_PROCESSED),
+	},
+	BERLIN2_ADC_CHANNEL(7, IIO_VOLTAGE),	/* reserved */
+	IIO_CHAN_SOFT_TIMESTAMP(8),		/* timestamp */
+};
+#define BERLIN2_N_CHANNELS	ARRAY_SIZE(berlin2_adc_channels)
+
+static int berlin2_adc_read(struct iio_dev *indio_dev, int channel)
+{
+	struct berlin2_adc_priv *priv = iio_priv(indio_dev);
+	int data, ret;
+
+	mutex_lock(&priv->lock);
+
+	/* Configure the ADC */
+	regmap_update_bits(priv->regmap, BERLIN2_SM_CTRL,
+			BERLIN2_SM_CTRL_ADC_RESET | BERLIN2_SM_CTRL_ADC_SEL_MASK
+			| BERLIN2_SM_CTRL_ADC_START,
+			BERLIN2_SM_CTRL_ADC_SEL(channel) | BERLIN2_SM_CTRL_ADC_START);
+
+	ret = wait_event_interruptible_timeout(priv->wq, priv->data_available,
+			msecs_to_jiffies(1000));
+
+	/* Disable the interrupts */
+	regmap_update_bits(priv->regmap, BERLIN2_SM_ADC_STATUS,
+			BERLIN2_SM_ADC_STATUS_INT_EN(channel), 0);
+
+	if (ret == 0)
+		ret = -ETIMEDOUT;
+	if (ret < 0) {
+		mutex_unlock(&priv->lock);
+		return ret;
+	}
+
+	regmap_update_bits(priv->regmap, BERLIN2_SM_CTRL,
+			BERLIN2_SM_CTRL_ADC_START, 0);
+
+	data = priv->data;
+	priv->data_available = false;
+
+	mutex_unlock(&priv->lock);
+
+	return data;
+}
+
+static int berlin2_adc_tsen_read(struct iio_dev *indio_dev)
+{
+	struct berlin2_adc_priv *priv = iio_priv(indio_dev);
+	int data, ret;
+
+	mutex_lock(&priv->lock);
+
+	/* Configure the ADC */
+	regmap_update_bits(priv->regmap, BERLIN2_SM_CTRL,
+			BERLIN2_SM_CTRL_TSEN_RESET | BERLIN2_SM_CTRL_ADC_ROTATE,
+			BERLIN2_SM_CTRL_ADC_ROTATE);
+
+	/* Configure the temperature sensor */
+	regmap_update_bits(priv->regmap, BERLIN2_SM_TSEN_CTRL,
+			BERLIN2_SM_TSEN_CTRL_TRIM_MASK | BERLIN2_SM_TSEN_CTRL_SETTLING_MASK
+			| BERLIN2_SM_TSEN_CTRL_START,
+			BERLIN2_SM_TSEN_CTRL_TRIM(3) | BERLIN2_SM_TSEN_CTRL_SETTLING_12
+			| BERLIN2_SM_TSEN_CTRL_START);
+
+	ret = wait_event_interruptible_timeout(priv->wq, priv->data_available,
+			msecs_to_jiffies(1000));
+
+	/* Disable interrupts */
+	regmap_update_bits(priv->regmap, BERLIN2_SM_TSEN_STATUS,
+			BERLIN2_SM_TSEN_STATUS_INT_EN, 0);
+
+	if (ret == 0)
+		ret = -ETIMEDOUT;
+	if (ret < 0) {
+		mutex_unlock(&priv->lock);
+		return ret;
+	}
+
+	regmap_update_bits(priv->regmap, BERLIN2_SM_TSEN_CTRL,
+			BERLIN2_SM_TSEN_CTRL_START, 0);
+
+	data = priv->data;
+	priv->data_available = false;
+
+	mutex_unlock(&priv->lock);
+
+	return data;
+}
+
+static int berlin2_adc_read_raw(struct iio_dev *indio_dev,
+		struct iio_chan_spec const *chan, int *val, int *val2,
+		long mask)
+{
+	struct berlin2_adc_priv *priv = iio_priv(indio_dev);
+	int temp;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (chan->type != IIO_VOLTAGE)
+			return -EINVAL;
+
+		/* Enable the interrupts */
+		regmap_write(priv->regmap, BERLIN2_SM_ADC_STATUS,
+				BERLIN2_SM_ADC_STATUS_INT_EN(chan->channel));
+
+		*val = berlin2_adc_read(indio_dev, chan->channel);
+		if (*val < 0)
+			return *val;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_PROCESSED:
+		if (chan->type != IIO_TEMP)
+			return -EINVAL;
+
+		/* Enable interrupts */
+		regmap_write(priv->regmap, BERLIN2_SM_TSEN_STATUS,
+				BERLIN2_SM_TSEN_STATUS_INT_EN);
+
+		temp = berlin2_adc_tsen_read(indio_dev);
+		if (temp < 0)
+			return temp;
+
+		if (temp > 2047)
+			temp = -(4096 - temp);
+
+		/* Convert to milli Celsius */
+		*val = ((temp * 100000) / 264 - 270000);
+		return IIO_VAL_INT;
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static irqreturn_t berlin2_adc_irq(int irq, void *private)
+{
+	struct berlin2_adc_priv *priv = iio_priv(private);
+	unsigned val;
+
+	regmap_read(priv->regmap, BERLIN2_SM_ADC_STATUS, &val);
+	if (val & BERLIN2_SM_ADC_STATUS_DATA_RDY_MASK) {
+		regmap_read(priv->regmap, BERLIN2_SM_ADC_DATA, &priv->data);
+		priv->data &= BERLIN2_SM_ADC_MASK;
+
+		val &= ~BERLIN2_SM_ADC_STATUS_DATA_RDY_MASK;
+		regmap_write(priv->regmap, BERLIN2_SM_ADC_STATUS, val);
+
+		priv->data_available = true;
+		wake_up_interruptible(&priv->wq);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t berlin2_adc_tsen_irq(int irq, void *private)
+{
+	struct berlin2_adc_priv *priv = iio_priv(private);
+	unsigned val;
+
+	regmap_read(priv->regmap, BERLIN2_SM_TSEN_STATUS, &val);
+	if (val & BERLIN2_SM_TSEN_STATUS_DATA_RDY) {
+		regmap_read(priv->regmap, BERLIN2_SM_TSEN_DATA, &priv->data);
+		priv->data &= BERLIN2_SM_TSEN_MASK;
+
+		val &= ~BERLIN2_SM_TSEN_STATUS_DATA_RDY;
+		regmap_write(priv->regmap, BERLIN2_SM_TSEN_STATUS, val);
+
+		priv->data_available = true;
+		wake_up_interruptible(&priv->wq);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static const struct iio_info berlin2_adc_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= berlin2_adc_read_raw,
+};
+
+static int berlin2_adc_probe(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev;
+	struct berlin2_adc_priv *priv;
+	struct device_node *parent_np = of_get_parent(pdev->dev.of_node);
+	int irq, tsen_irq;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev,
+					  sizeof(struct berlin2_adc_priv));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	priv = iio_priv(indio_dev);
+	platform_set_drvdata(pdev, indio_dev);
+
+	priv->regmap = syscon_node_to_regmap(parent_np);
+	of_node_put(parent_np);
+	if (IS_ERR(priv->regmap))
+		return PTR_ERR(priv->regmap);
+
+	irq = platform_get_irq_byname(pdev, "adc");
+	if (irq < 0)
+		return -ENODEV;
+
+	tsen_irq = platform_get_irq_byname(pdev, "tsen");
+	if (tsen_irq < 0)
+		return -ENODEV;
+
+	ret = devm_request_irq(&pdev->dev, irq, berlin2_adc_irq, 0,
+			pdev->dev.driver->name, indio_dev);
+	if (ret)
+		return ret;
+
+	ret = devm_request_irq(&pdev->dev, tsen_irq, berlin2_adc_tsen_irq,
+			0, pdev->dev.driver->name, indio_dev);
+	if (ret)
+		return ret;
+
+	init_waitqueue_head(&priv->wq);
+	mutex_init(&priv->lock);
+
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &berlin2_adc_info;
+
+	indio_dev->num_channels = BERLIN2_N_CHANNELS;
+	indio_dev->channels = berlin2_adc_channels;
+
+	/* Power up the ADC */
+	regmap_update_bits(priv->regmap, BERLIN2_SM_CTRL,
+			BERLIN2_SM_CTRL_ADC_POWER, BERLIN2_SM_CTRL_ADC_POWER);
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		/* Power down the ADC */
+		regmap_update_bits(priv->regmap, BERLIN2_SM_CTRL,
+				BERLIN2_SM_CTRL_ADC_POWER, 0);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int berlin2_adc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct berlin2_adc_priv *priv = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	/* Power down the ADC */
+	regmap_update_bits(priv->regmap, BERLIN2_SM_CTRL,
+			BERLIN2_SM_CTRL_ADC_POWER, 0);
+
+	return 0;
+}
+
+static const struct of_device_id berlin2_adc_match[] = {
+	{ .compatible = "marvell,berlin2-adc", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, berlin2_adc_match);
+
+static struct platform_driver berlin2_adc_driver = {
+	.driver	= {
+		.name		= "berlin2-adc",
+		.of_match_table	= berlin2_adc_match,
+	},
+	.probe	= berlin2_adc_probe,
+	.remove	= berlin2_adc_remove,
+};
+module_platform_driver(berlin2_adc_driver);
+
+MODULE_AUTHOR("Antoine Tenart <antoine.tenart@free-electrons.com>");
+MODULE_DESCRIPTION("Marvell Berlin2 ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ti-adc128s052.c b/drivers/iio/adc/ti-adc128s052.c
index 655cb564ec54..915be6b60097 100644
--- a/drivers/iio/adc/ti-adc128s052.c
+++ b/drivers/iio/adc/ti-adc128s052.c
@@ -1,9 +1,10 @@
 /*
  * Copyright (C) 2014 Angelo Compagnucci <angelo.compagnucci@gmail.com>
  *
- * Driver for Texas Instruments' ADC128S052 ADC chip.
- * Datasheet can be found here:
+ * Driver for Texas Instruments' ADC128S052 and ADC122S021 ADC chip.
+ * Datasheets can be found here:
  * http://www.ti.com/lit/ds/symlink/adc128s052.pdf
+ * http://www.ti.com/lit/ds/symlink/adc122s021.pdf
  *
  * 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
@@ -16,6 +17,11 @@
 #include <linux/iio/iio.h>
 #include <linux/regulator/consumer.h>
 
+struct adc128_configuration {
+	const struct iio_chan_spec	*channels;
+	u8				num_channels;
+};
+
 struct adc128 {
 	struct spi_device *spi;
 
@@ -92,7 +98,7 @@ static int adc128_read_raw(struct iio_dev *indio_dev,
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) \
 	}
 
-static const struct iio_chan_spec adc128_channels[] = {
+static const struct iio_chan_spec adc128s052_channels[] = {
 	ADC128_VOLTAGE_CHANNEL(0),
 	ADC128_VOLTAGE_CHANNEL(1),
 	ADC128_VOLTAGE_CHANNEL(2),
@@ -103,6 +109,16 @@ static const struct iio_chan_spec adc128_channels[] = {
 	ADC128_VOLTAGE_CHANNEL(7),
 };
 
+static const struct iio_chan_spec adc122s021_channels[] = {
+	ADC128_VOLTAGE_CHANNEL(0),
+	ADC128_VOLTAGE_CHANNEL(1),
+};
+
+static const struct adc128_configuration adc128_config[] = {
+	{ adc128s052_channels, ARRAY_SIZE(adc128s052_channels) },
+	{ adc122s021_channels, ARRAY_SIZE(adc122s021_channels) },
+};
+
 static const struct iio_info adc128_info = {
 	.read_raw = adc128_read_raw,
 	.driver_module = THIS_MODULE,
@@ -112,6 +128,7 @@ static int adc128_probe(struct spi_device *spi)
 {
 	struct iio_dev *indio_dev;
 	struct adc128 *adc;
+	int config = spi_get_device_id(spi)->driver_data;
 	int ret;
 
 	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*adc));
@@ -128,8 +145,8 @@ static int adc128_probe(struct spi_device *spi)
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &adc128_info;
 
-	indio_dev->channels = adc128_channels;
-	indio_dev->num_channels = ARRAY_SIZE(adc128_channels);
+	indio_dev->channels = adc128_config[config].channels;
+	indio_dev->num_channels = adc128_config[config].num_channels;
 
 	adc->reg = devm_regulator_get(&spi->dev, "vref");
 	if (IS_ERR(adc->reg))
@@ -158,7 +175,8 @@ static int adc128_remove(struct spi_device *spi)
 }
 
 static const struct spi_device_id adc128_id[] = {
-	{ "adc128s052", 0},
+	{ "adc128s052", 0},	/* index into adc128_config */
+	{ "adc122s021",	1},
 	{ }
 };
 MODULE_DEVICE_TABLE(spi, adc128_id);
diff --git a/drivers/iio/adc/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c
index a0e7161f040c..942320e32753 100644
--- a/drivers/iio/adc/ti_am335x_adc.c
+++ b/drivers/iio/adc/ti_am335x_adc.c
@@ -37,6 +37,7 @@ struct tiadc_device {
 	u8 channel_step[8];
 	int buffer_en_ch_steps;
 	u16 data[8];
+	u32 open_delay[8], sample_delay[8], step_avg[8];
 };
 
 static unsigned int tiadc_readl(struct tiadc_device *adc, unsigned int reg)
@@ -85,6 +86,7 @@ static u32 get_adc_step_bit(struct tiadc_device *adc_dev, int chan)
 static void tiadc_step_config(struct iio_dev *indio_dev)
 {
 	struct tiadc_device *adc_dev = iio_priv(indio_dev);
+	struct device *dev = adc_dev->mfd_tscadc->dev;
 	unsigned int stepconfig;
 	int i, steps = 0;
 
@@ -98,20 +100,47 @@ static void tiadc_step_config(struct iio_dev *indio_dev)
 	 * needs to be given to ADC to digitalize data.
 	 */
 
-	if (iio_buffer_enabled(indio_dev))
-		stepconfig = STEPCONFIG_AVG_16 | STEPCONFIG_FIFO1
-					| STEPCONFIG_MODE_SWCNT;
-	else
-		stepconfig = STEPCONFIG_AVG_16 | STEPCONFIG_FIFO1;
 
 	for (i = 0; i < adc_dev->channels; i++) {
 		int chan;
 
 		chan = adc_dev->channel_line[i];
+
+		if (adc_dev->step_avg[i] > STEPCONFIG_AVG_16) {
+			dev_warn(dev, "chan %d step_avg truncating to %d\n",
+				 chan, STEPCONFIG_AVG_16);
+			adc_dev->step_avg[i] = STEPCONFIG_AVG_16;
+		}
+
+		if (adc_dev->step_avg[i])
+			stepconfig =
+			STEPCONFIG_AVG(ffs(adc_dev->step_avg[i]) - 1) |
+			STEPCONFIG_FIFO1;
+		else
+			stepconfig = STEPCONFIG_FIFO1;
+
+		if (iio_buffer_enabled(indio_dev))
+			stepconfig |= STEPCONFIG_MODE_SWCNT;
+
 		tiadc_writel(adc_dev, REG_STEPCONFIG(steps),
 				stepconfig | STEPCONFIG_INP(chan));
+
+		if (adc_dev->open_delay[i] > STEPDELAY_OPEN_MASK) {
+			dev_warn(dev, "chan %d open delay truncating to 0x3FFFF\n",
+				 chan);
+			adc_dev->open_delay[i] = STEPDELAY_OPEN_MASK;
+		}
+
+		if (adc_dev->sample_delay[i] > 0xFF) {
+			dev_warn(dev, "chan %d sample delay truncating to 0xFF\n",
+				 chan);
+			adc_dev->sample_delay[i] = 0xFF;
+		}
+
 		tiadc_writel(adc_dev, REG_STEPDELAY(steps),
-				STEPCONFIG_OPENDLY);
+				STEPDELAY_OPEN(adc_dev->open_delay[i]) |
+				STEPDELAY_SAMPLE(adc_dev->sample_delay[i]));
+
 		adc_dev->channel_step[i] = steps;
 		steps++;
 	}
@@ -395,16 +424,43 @@ static const struct iio_info tiadc_info = {
 	.driver_module = THIS_MODULE,
 };
 
+static int tiadc_parse_dt(struct platform_device *pdev,
+			  struct tiadc_device *adc_dev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct property *prop;
+	const __be32 *cur;
+	int channels = 0;
+	u32 val;
+
+	of_property_for_each_u32(node, "ti,adc-channels", prop, cur, val) {
+		adc_dev->channel_line[channels] = val;
+
+		/* Set Default values for optional DT parameters */
+		adc_dev->open_delay[channels] = STEPCONFIG_OPENDLY;
+		adc_dev->sample_delay[channels] = STEPCONFIG_SAMPLEDLY;
+		adc_dev->step_avg[channels] = 16;
+
+		channels++;
+	}
+
+	of_property_read_u32_array(node, "ti,chan-step-avg",
+				   adc_dev->step_avg, channels);
+	of_property_read_u32_array(node, "ti,chan-step-opendelay",
+				   adc_dev->open_delay, channels);
+	of_property_read_u32_array(node, "ti,chan-step-sampledelay",
+				   adc_dev->sample_delay, channels);
+
+	adc_dev->channels = channels;
+	return 0;
+}
+
 static int tiadc_probe(struct platform_device *pdev)
 {
 	struct iio_dev		*indio_dev;
 	struct tiadc_device	*adc_dev;
 	struct device_node	*node = pdev->dev.of_node;
-	struct property		*prop;
-	const __be32		*cur;
 	int			err;
-	u32			val;
-	int			channels = 0;
 
 	if (!node) {
 		dev_err(&pdev->dev, "Could not find valid DT data.\n");
@@ -420,12 +476,7 @@ static int tiadc_probe(struct platform_device *pdev)
 	adc_dev = iio_priv(indio_dev);
 
 	adc_dev->mfd_tscadc = ti_tscadc_dev_get(pdev);
-
-	of_property_for_each_u32(node, "ti,adc-channels", prop, cur, val) {
-		adc_dev->channel_line[channels] = val;
-		channels++;
-	}
-	adc_dev->channels = channels;
+	tiadc_parse_dt(pdev, adc_dev);
 
 	indio_dev->dev.parent = &pdev->dev;
 	indio_dev->name = dev_name(&pdev->dev);
diff --git a/drivers/iio/adc/twl4030-madc.c b/drivers/iio/adc/twl4030-madc.c
index 94c5f05b4bc1..06f4792240f0 100644
--- a/drivers/iio/adc/twl4030-madc.c
+++ b/drivers/iio/adc/twl4030-madc.c
@@ -235,7 +235,7 @@ static int twl4030battery_temperature(int raw_volt)
 	if (ret < 0)
 		return ret;
 
-	curr = ((val & TWL4030_BCI_ITHEN) + 1) * 10;
+	curr = ((val & TWL4030_BCI_ITHSENS) + 1) * 10;
 	/* Getting and calculating the thermistor resistance in ohms */
 	res = volt * 1000 / curr;
 	/* calculating temperature */
@@ -662,10 +662,8 @@ EXPORT_SYMBOL_GPL(twl4030_get_madc_conversion);
  *
  * @madc:	pointer to twl4030_madc_data struct
  * @chan:	can be one of the two values:
- *		TWL4030_BCI_ITHEN
- *		Enables bias current for main battery type reading
- *		TWL4030_BCI_TYPEN
- *		Enables bias current for main battery temperature sensing
+ *		0 - Enables bias current for main battery type reading
+ *		1 - Enables bias current for main battery temperature sensing
  * @on:		enable or disable chan.
  *
  * Function to enable or disable bias current for
diff --git a/drivers/iio/adc/twl6030-gpadc.c b/drivers/iio/adc/twl6030-gpadc.c
index 89d8aa1d2818..df12c57e6ce0 100644
--- a/drivers/iio/adc/twl6030-gpadc.c
+++ b/drivers/iio/adc/twl6030-gpadc.c
@@ -1001,7 +1001,7 @@ static struct platform_driver twl6030_gpadc_driver = {
 
 module_platform_driver(twl6030_gpadc_driver);
 
-MODULE_ALIAS("platform: " DRIVER_NAME);
+MODULE_ALIAS("platform:" DRIVER_NAME);
 MODULE_AUTHOR("Balaji T K <balajitk@ti.com>");
 MODULE_AUTHOR("Graeme Gregory <gg@slimlogic.co.uk>");
 MODULE_AUTHOR("Oleksandr Kozaruk <oleksandr.kozaruk@ti.com");
diff --git a/drivers/iio/adc/vf610_adc.c b/drivers/iio/adc/vf610_adc.c
index 56292ae4538d..480f335a0f9f 100644
--- a/drivers/iio/adc/vf610_adc.c
+++ b/drivers/iio/adc/vf610_adc.c
@@ -118,15 +118,21 @@ enum average_sel {
 	VF610_ADC_SAMPLE_32,
 };
 
+enum conversion_mode_sel {
+	VF610_ADC_CONV_NORMAL,
+	VF610_ADC_CONV_HIGH_SPEED,
+	VF610_ADC_CONV_LOW_POWER,
+};
+
 struct vf610_adc_feature {
 	enum clk_sel	clk_sel;
 	enum vol_ref	vol_ref;
+	enum conversion_mode_sel conv_mode;
 
 	int	clk_div;
 	int     sample_rate;
 	int	res_mode;
 
-	bool	lpm;
 	bool	calibration;
 	bool	ovwren;
 };
@@ -139,6 +145,8 @@ struct vf610_adc {
 	u32 vref_uv;
 	u32 value;
 	struct regulator *vref;
+
+	u32 max_adck_rate[3];
 	struct vf610_adc_feature adc_feature;
 
 	u32 sample_freq_avail[5];
@@ -148,46 +156,22 @@ struct vf610_adc {
 
 static const u32 vf610_hw_avgs[] = { 1, 4, 8, 16, 32 };
 
-#define VF610_ADC_CHAN(_idx, _chan_type) {			\
-	.type = (_chan_type),					\
-	.indexed = 1,						\
-	.channel = (_idx),					\
-	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
-	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |	\
-				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),
-	VF610_ADC_CHAN(2, IIO_VOLTAGE),
-	VF610_ADC_CHAN(3, IIO_VOLTAGE),
-	VF610_ADC_CHAN(4, IIO_VOLTAGE),
-	VF610_ADC_CHAN(5, IIO_VOLTAGE),
-	VF610_ADC_CHAN(6, IIO_VOLTAGE),
-	VF610_ADC_CHAN(7, IIO_VOLTAGE),
-	VF610_ADC_CHAN(8, IIO_VOLTAGE),
-	VF610_ADC_CHAN(9, IIO_VOLTAGE),
-	VF610_ADC_CHAN(10, IIO_VOLTAGE),
-	VF610_ADC_CHAN(11, IIO_VOLTAGE),
-	VF610_ADC_CHAN(12, IIO_VOLTAGE),
-	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 */
-};
-
 static inline void vf610_adc_calculate_rates(struct vf610_adc *info)
 {
+	struct vf610_adc_feature *adc_feature = &info->adc_feature;
 	unsigned long adck_rate, ipg_rate = clk_get_rate(info->clk);
-	int i;
+	int divisor, i;
+
+	adck_rate = info->max_adck_rate[adc_feature->conv_mode];
+
+	if (adck_rate) {
+		/* calculate clk divider which is within specification */
+		divisor = ipg_rate / adck_rate;
+		adc_feature->clk_div = 1 << fls(divisor + 1);
+	} else {
+		/* fall-back value using a safe divisor */
+		adc_feature->clk_div = 8;
+	}
 
 	/*
 	 * Calculate ADC sample frequencies
@@ -219,10 +203,8 @@ static inline void vf610_adc_cfg_init(struct vf610_adc *info)
 
 	adc_feature->res_mode = 12;
 	adc_feature->sample_rate = 1;
-	adc_feature->lpm = true;
 
-	/* Use a save ADCK which is below 20MHz on all devices */
-	adc_feature->clk_div = 8;
+	adc_feature->conv_mode = VF610_ADC_CONV_LOW_POWER;
 
 	vf610_adc_calculate_rates(info);
 }
@@ -304,10 +286,12 @@ static void vf610_adc_cfg_set(struct vf610_adc *info)
 	cfg_data = readl(info->regs + VF610_REG_ADC_CFG);
 
 	cfg_data &= ~VF610_ADC_ADLPC_EN;
-	if (adc_feature->lpm)
+	if (adc_feature->conv_mode == VF610_ADC_CONV_LOW_POWER)
 		cfg_data |= VF610_ADC_ADLPC_EN;
 
 	cfg_data &= ~VF610_ADC_ADHSC_EN;
+	if (adc_feature->conv_mode == VF610_ADC_CONV_HIGH_SPEED)
+		cfg_data |= VF610_ADC_ADHSC_EN;
 
 	writel(cfg_data, info->regs + VF610_REG_ADC_CFG);
 }
@@ -409,6 +393,81 @@ static void vf610_adc_hw_init(struct vf610_adc *info)
 	vf610_adc_cfg_set(info);
 }
 
+static int vf610_set_conversion_mode(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan,
+				     unsigned int mode)
+{
+	struct vf610_adc *info = iio_priv(indio_dev);
+
+	mutex_lock(&indio_dev->mlock);
+	info->adc_feature.conv_mode = mode;
+	vf610_adc_calculate_rates(info);
+	vf610_adc_hw_init(info);
+	mutex_unlock(&indio_dev->mlock);
+
+	return 0;
+}
+
+static int vf610_get_conversion_mode(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan)
+{
+	struct vf610_adc *info = iio_priv(indio_dev);
+
+	return info->adc_feature.conv_mode;
+}
+
+static const char * const vf610_conv_modes[] = { "normal", "high-speed",
+						 "low-power" };
+
+static const struct iio_enum vf610_conversion_mode = {
+	.items = vf610_conv_modes,
+	.num_items = ARRAY_SIZE(vf610_conv_modes),
+	.get = vf610_get_conversion_mode,
+	.set = vf610_set_conversion_mode,
+};
+
+static const struct iio_chan_spec_ext_info vf610_ext_info[] = {
+	IIO_ENUM("conversion_mode", IIO_SHARED_BY_DIR, &vf610_conversion_mode),
+	{},
+};
+
+#define VF610_ADC_CHAN(_idx, _chan_type) {			\
+	.type = (_chan_type),					\
+	.indexed = 1,						\
+	.channel = (_idx),					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |	\
+				BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
+	.ext_info = vf610_ext_info,				\
+}
+
+#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),
+	VF610_ADC_CHAN(2, IIO_VOLTAGE),
+	VF610_ADC_CHAN(3, IIO_VOLTAGE),
+	VF610_ADC_CHAN(4, IIO_VOLTAGE),
+	VF610_ADC_CHAN(5, IIO_VOLTAGE),
+	VF610_ADC_CHAN(6, IIO_VOLTAGE),
+	VF610_ADC_CHAN(7, IIO_VOLTAGE),
+	VF610_ADC_CHAN(8, IIO_VOLTAGE),
+	VF610_ADC_CHAN(9, IIO_VOLTAGE),
+	VF610_ADC_CHAN(10, IIO_VOLTAGE),
+	VF610_ADC_CHAN(11, IIO_VOLTAGE),
+	VF610_ADC_CHAN(12, IIO_VOLTAGE),
+	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 */
+};
+
 static int vf610_adc_read_data(struct vf610_adc *info)
 {
 	int result;
@@ -651,6 +710,9 @@ static int vf610_adc_probe(struct platform_device *pdev)
 
 	info->vref_uv = regulator_get_voltage(info->vref);
 
+	of_property_read_u32_array(pdev->dev.of_node, "fsl,adck-max-frequency",
+			info->max_adck_rate, 3);
+
 	platform_set_drvdata(pdev, indio_dev);
 
 	init_completion(&info->completion);
diff --git a/drivers/iio/buffer_cb.c b/drivers/iio/buffer_cb.c
index eb46e728aa2e..1648e6e5a848 100644
--- a/drivers/iio/buffer_cb.c
+++ b/drivers/iio/buffer_cb.c
@@ -33,6 +33,8 @@ static void iio_buffer_cb_release(struct iio_buffer *buffer)
 static const struct iio_buffer_access_funcs iio_cb_access = {
 	.store_to = &iio_buffer_cb_store_to,
 	.release = &iio_buffer_cb_release,
+
+	.modes = INDIO_BUFFER_SOFTWARE | INDIO_BUFFER_TRIGGERED,
 };
 
 struct iio_cb_buffer *iio_channel_get_all_cb(struct device *dev,
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 8dd0477e201c..8086cbcff87d 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -245,6 +245,16 @@ static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
 {
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
+	/* Sensor does not support interrupts */
+	if (sdata->sensor_settings->drdy_irq.addr == 0) {
+		if (pdata->drdy_int_pin)
+			dev_info(&indio_dev->dev,
+				 "DRDY on pin INT%d specified, but sensor "
+				 "does not support interrupts\n",
+				 pdata->drdy_int_pin);
+		return 0;
+	}
+
 	switch (pdata->drdy_int_pin) {
 	case 1:
 		if (sdata->sensor_settings->drdy_irq.mask_int1 == 0) {
@@ -285,7 +295,7 @@ static struct st_sensors_platform_data *st_sensors_of_probe(struct device *dev,
 	if (!of_property_read_u32(np, "st,drdy-int-pin", &val) && (val <= 2))
 		pdata->drdy_int_pin = (u8) val;
 	else
-		pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 1;
+		pdata->drdy_int_pin = defdata ? defdata->drdy_int_pin : 0;
 
 	return pdata;
 }
@@ -332,11 +342,13 @@ int st_sensors_init_sensor(struct iio_dev *indio_dev,
 		return err;
 
 	/* set BDU */
-	err = st_sensors_write_data_with_mask(indio_dev,
+	if (sdata->sensor_settings->bdu.addr) {
+		err = st_sensors_write_data_with_mask(indio_dev,
 					sdata->sensor_settings->bdu.addr,
 					sdata->sensor_settings->bdu.mask, true);
-	if (err < 0)
-		return err;
+		if (err < 0)
+			return err;
+	}
 
 	err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
 
@@ -419,7 +431,9 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
 	if (err < 0)
 		goto st_sensors_free_memory;
 
-	if (byte_for_channel == 2)
+	if (byte_for_channel == 1)
+		*data = (s8)*outdata;
+	else if (byte_for_channel == 2)
 		*data = (s16)get_unaligned_le16(outdata);
 	else if (byte_for_channel == 3)
 		*data = (s32)st_sensors_get_unaligned_le24(outdata);
@@ -489,7 +503,8 @@ int st_sensors_check_device_support(struct iio_dev *indio_dev,
 			break;
 	}
 	if (n == ARRAY_SIZE(sensor_settings[i].sensors_supported)) {
-		dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
+		dev_err(&indio_dev->dev, "device name \"%s\" and WhoAmI (0x%02x) mismatch",
+			indio_dev->name, wai);
 		goto sensor_name_mismatch;
 	}
 
diff --git a/drivers/iio/common/st_sensors/st_sensors_trigger.c b/drivers/iio/common/st_sensors/st_sensors_trigger.c
index 8d8ca6f1e16a..3e907040c2c7 100644
--- a/drivers/iio/common/st_sensors/st_sensors_trigger.c
+++ b/drivers/iio/common/st_sensors/st_sensors_trigger.c
@@ -37,8 +37,10 @@ int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
 			IRQF_TRIGGER_RISING,
 			sdata->trig->name,
 			sdata->trig);
-	if (err)
+	if (err) {
+		dev_err(&indio_dev->dev, "failed to request trigger IRQ.\n");
 		goto request_irq_error;
+	}
 
 	iio_trigger_set_drvdata(sdata->trig, indio_dev);
 	sdata->trig->ops = trigger_ops;
diff --git a/drivers/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index 13471a76e5bf..e701e28fb1cd 100644
--- a/drivers/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -142,6 +142,16 @@ config AD7303
 	  To compile this driver as module choose M here: the module will be called
 	  ad7303.
 
+config M62332
+	tristate "Mitsubishi M62332 DAC driver"
+	depends on I2C
+	help
+	  If you say yes here you get support for the Mitsubishi M62332
+	  (I2C 8-Bit DACs with rail-to-rail outputs).
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called m62332.
+
 config MAX517
 	tristate "Maxim MAX517/518/519/520/521 DAC driver"
 	depends on I2C
diff --git a/drivers/iio/dac/Makefile b/drivers/iio/dac/Makefile
index 52be7e1acf16..63ae05633e0c 100644
--- a/drivers/iio/dac/Makefile
+++ b/drivers/iio/dac/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_AD5764) += ad5764.o
 obj-$(CONFIG_AD5791) += ad5791.o
 obj-$(CONFIG_AD5686) += ad5686.o
 obj-$(CONFIG_AD7303) += ad7303.o
+obj-$(CONFIG_M62332) += m62332.o
 obj-$(CONFIG_MAX517) += max517.o
 obj-$(CONFIG_MAX5821) += max5821.o
 obj-$(CONFIG_MCP4725) += mcp4725.o
diff --git a/drivers/iio/dac/m62332.c b/drivers/iio/dac/m62332.c
new file mode 100644
index 000000000000..c23d7fa889ee
--- /dev/null
+++ b/drivers/iio/dac/m62332.c
@@ -0,0 +1,269 @@
+/*
+ *  m62332.c - Support for Mitsubishi m62332 DAC
+ *
+ *  Copyright (c) 2014 Dmitry Eremin-Solenikov
+ *
+ *  Based on max517 driver:
+ *  Copyright (C) 2010, 2011 Roland Stigge <stigge@antcom.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License 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/slab.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/driver.h>
+
+#include <linux/regulator/consumer.h>
+
+#define M62332_CHANNELS 2
+
+struct m62332_data {
+	struct i2c_client	*client;
+	u16			vref_mv;
+	struct regulator	*vcc;
+	struct mutex		mutex;
+	u8			raw[M62332_CHANNELS];
+#ifdef CONFIG_PM_SLEEP
+	u8			save[M62332_CHANNELS];
+#endif
+};
+
+static int m62332_set_value(struct iio_dev *indio_dev,
+	u8 val, int channel)
+{
+	struct m62332_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+	u8 outbuf[2];
+	int res;
+
+	if (val == data->raw[channel])
+		return 0;
+
+	outbuf[0] = channel;
+	outbuf[1] = val;
+
+	mutex_lock(&data->mutex);
+
+	if (val) {
+		res = regulator_enable(data->vcc);
+		if (res)
+			goto out;
+	}
+
+	res = i2c_master_send(client, outbuf, 2);
+	if (res >= 0 && res != 2)
+		res = -EIO;
+	if (res < 0)
+		goto out;
+
+	data->raw[channel] = val;
+
+	if (!val)
+		regulator_disable(data->vcc);
+
+	mutex_unlock(&data->mutex);
+
+	return 0;
+
+out:
+	mutex_unlock(&data->mutex);
+
+	return res;
+}
+
+static int m62332_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val,
+			   int *val2,
+			   long m)
+{
+	struct m62332_data *data = iio_priv(indio_dev);
+
+	switch (m) {
+	case IIO_CHAN_INFO_SCALE:
+		/* Corresponds to Vref / 2^(bits) */
+		*val = data->vref_mv;
+		*val2 = 8;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	case IIO_CHAN_INFO_RAW:
+		*val = data->raw[chan->channel];
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_OFFSET:
+		*val = 1;
+		return IIO_VAL_INT;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
+static int m62332_write_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int val, int val2, long mask)
+{
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (val < 0 || val > 255)
+			return -EINVAL;
+
+		ret = m62332_set_value(indio_dev, val, chan->channel);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int m62332_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct m62332_data *data = iio_priv(indio_dev);
+	int ret;
+
+	data->save[0] = data->raw[0];
+	data->save[1] = data->raw[1];
+
+	ret = m62332_set_value(indio_dev, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	return m62332_set_value(indio_dev, 0, 1);
+}
+
+static int m62332_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct m62332_data *data = iio_priv(indio_dev);
+	int ret;
+
+	ret = m62332_set_value(indio_dev, data->save[0], 0);
+	if (ret < 0)
+		return ret;
+
+	return m62332_set_value(indio_dev, data->save[1], 1);
+}
+
+static SIMPLE_DEV_PM_OPS(m62332_pm_ops, m62332_suspend, m62332_resume);
+#define M62332_PM_OPS (&m62332_pm_ops)
+#else
+#define M62332_PM_OPS NULL
+#endif
+
+static const struct iio_info m62332_info = {
+	.read_raw = m62332_read_raw,
+	.write_raw = m62332_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define M62332_CHANNEL(chan) {				\
+	.type = IIO_VOLTAGE,				\
+	.indexed = 1,					\
+	.output = 1,					\
+	.channel = (chan),				\
+	.datasheet_name = "CH" #chan,			\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |	\
+		BIT(IIO_CHAN_INFO_SCALE) |		\
+		BIT(IIO_CHAN_INFO_OFFSET),		\
+}
+
+static const struct iio_chan_spec m62332_channels[M62332_CHANNELS] = {
+	M62332_CHANNEL(0),
+	M62332_CHANNEL(1)
+};
+
+static int m62332_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct m62332_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	mutex_init(&data->mutex);
+
+	data->vcc = devm_regulator_get(&client->dev, "VCC");
+	if (IS_ERR(data->vcc))
+		return PTR_ERR(data->vcc);
+
+	/* establish that the iio_dev is a child of the i2c device */
+	indio_dev->dev.parent = &client->dev;
+
+	indio_dev->num_channels = M62332_CHANNELS;
+	indio_dev->channels = m62332_channels;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &m62332_info;
+
+	ret = regulator_get_voltage(data->vcc);
+	if (ret < 0)
+		return ret;
+	data->vref_mv = ret / 1000; /* mV */
+
+	ret = iio_map_array_register(indio_dev, client->dev.platform_data);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto err;
+
+	return 0;
+
+err:
+	iio_map_array_unregister(indio_dev);
+	return ret;
+}
+
+static int m62332_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	iio_map_array_unregister(indio_dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id m62332_id[] = {
+	{ "m62332", },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, m62332_id);
+
+static struct i2c_driver m62332_driver = {
+	.driver = {
+		.name	= "m62332",
+		.pm	= M62332_PM_OPS,
+	},
+	.probe		= m62332_probe,
+	.remove		= m62332_remove,
+	.id_table	= m62332_id,
+};
+module_i2c_driver(m62332_driver);
+
+MODULE_AUTHOR("Dmitry Eremin-Solenikov");
+MODULE_DESCRIPTION("M62332 8-bit DAC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/gyro/bmg160.c b/drivers/iio/gyro/bmg160.c
index 4415f55d26b6..460bf715d541 100644
--- a/drivers/iio/gyro/bmg160.c
+++ b/drivers/iio/gyro/bmg160.c
@@ -108,7 +108,6 @@ struct bmg160_data {
 	int slope_thres;
 	bool dready_trigger_on;
 	bool motion_trigger_on;
-	int64_t timestamp;
 };
 
 enum bmg160_axis {
@@ -738,17 +737,6 @@ static int bmg160_write_event_config(struct iio_dev *indio_dev,
 	return 0;
 }
 
-static int bmg160_validate_trigger(struct iio_dev *indio_dev,
-				   struct iio_trigger *trig)
-{
-	struct bmg160_data *data = iio_priv(indio_dev);
-
-	if (data->dready_trig != trig && data->motion_trig != trig)
-		return -EINVAL;
-
-	return 0;
-}
-
 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("100 200 400 1000 2000");
 
 static IIO_CONST_ATTR(in_anglvel_scale_available,
@@ -810,7 +798,6 @@ static const struct iio_info bmg160_info = {
 	.write_event_value	= bmg160_write_event,
 	.write_event_config	= bmg160_write_event_config,
 	.read_event_config	= bmg160_read_event_config,
-	.validate_trigger	= bmg160_validate_trigger,
 	.driver_module		= THIS_MODULE,
 };
 
@@ -835,7 +822,7 @@ static irqreturn_t bmg160_trigger_handler(int irq, void *p)
 	mutex_unlock(&data->mutex);
 
 	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
-					   data->timestamp);
+					   pf->timestamp);
 err:
 	iio_trigger_notify_done(indio_dev->trig);
 
@@ -938,21 +925,21 @@ static irqreturn_t bmg160_event_handler(int irq, void *private)
 							IIO_MOD_X,
 							IIO_EV_TYPE_ROC,
 							dir),
-							data->timestamp);
+							iio_get_time_ns());
 	if (ret & BMG160_ANY_MOTION_BIT_Y)
 		iio_push_event(indio_dev, IIO_MOD_EVENT_CODE(IIO_ANGL_VEL,
 							0,
 							IIO_MOD_Y,
 							IIO_EV_TYPE_ROC,
 							dir),
-							data->timestamp);
+							iio_get_time_ns());
 	if (ret & BMG160_ANY_MOTION_BIT_Z)
 		iio_push_event(indio_dev, IIO_MOD_EVENT_CODE(IIO_ANGL_VEL,
 							0,
 							IIO_MOD_Z,
 							IIO_EV_TYPE_ROC,
 							dir),
-							data->timestamp);
+							iio_get_time_ns());
 
 ack_intr_status:
 	if (!data->dready_trigger_on) {
@@ -973,8 +960,6 @@ static irqreturn_t bmg160_data_rdy_trig_poll(int irq, void *private)
 	struct iio_dev *indio_dev = private;
 	struct bmg160_data *data = iio_priv(indio_dev);
 
-	data->timestamp = iio_get_time_ns();
-
 	if (data->dready_trigger_on)
 		iio_trigger_poll(data->dready_trig);
 	else if (data->motion_trigger_on)
@@ -987,6 +972,27 @@ static irqreturn_t bmg160_data_rdy_trig_poll(int irq, void *private)
 
 }
 
+static int bmg160_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	return bmg160_set_power_state(data, true);
+}
+
+static int bmg160_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	return bmg160_set_power_state(data, false);
+}
+
+static const struct iio_buffer_setup_ops bmg160_buffer_setup_ops = {
+	.preenable = bmg160_buffer_preenable,
+	.postenable = iio_triggered_buffer_postenable,
+	.predisable = iio_triggered_buffer_predisable,
+	.postdisable = bmg160_buffer_postdisable,
+};
+
 static int bmg160_gpio_probe(struct i2c_client *client,
 			     struct bmg160_data *data)
 
@@ -1103,16 +1109,16 @@ static int bmg160_probe(struct i2c_client *client,
 			data->motion_trig = NULL;
 			goto err_trigger_unregister;
 		}
+	}
 
-		ret = iio_triggered_buffer_setup(indio_dev,
-						 NULL,
-						 bmg160_trigger_handler,
-						 NULL);
-		if (ret < 0) {
-			dev_err(&client->dev,
-				"iio triggered buffer setup failed\n");
-			goto err_trigger_unregister;
-		}
+	ret = iio_triggered_buffer_setup(indio_dev,
+					 iio_pollfunc_store_time,
+					 bmg160_trigger_handler,
+					 &bmg160_buffer_setup_ops);
+	if (ret < 0) {
+		dev_err(&client->dev,
+			"iio triggered buffer setup failed\n");
+		goto err_trigger_unregister;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -1135,8 +1141,7 @@ static int bmg160_probe(struct i2c_client *client,
 err_iio_unregister:
 	iio_device_unregister(indio_dev);
 err_buffer_cleanup:
-	if (data->dready_trig)
-		iio_triggered_buffer_cleanup(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
 err_trigger_unregister:
 	if (data->dready_trig)
 		iio_trigger_unregister(data->dready_trig);
@@ -1156,9 +1161,9 @@ static int bmg160_remove(struct i2c_client *client)
 	pm_runtime_put_noidle(&client->dev);
 
 	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
 
 	if (data->dready_trig) {
-		iio_triggered_buffer_cleanup(indio_dev);
 		iio_trigger_unregister(data->dready_trig);
 		iio_trigger_unregister(data->motion_trig);
 	}
diff --git a/drivers/iio/gyro/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c
index b5883b6f4e50..c67ce2ac4715 100644
--- a/drivers/iio/gyro/hid-sensor-gyro-3d.c
+++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c
@@ -298,7 +298,6 @@ static int hid_gyro_3d_probe(struct platform_device *pdev)
 	struct iio_dev *indio_dev;
 	struct gyro_3d_state *gyro_state;
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
-	struct iio_chan_spec *channels;
 
 	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*gyro_state));
 	if (!indio_dev)
@@ -317,21 +316,21 @@ static int hid_gyro_3d_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	channels = kmemdup(gyro_3d_channels, sizeof(gyro_3d_channels),
-			   GFP_KERNEL);
-	if (!channels) {
+	indio_dev->channels = kmemdup(gyro_3d_channels,
+				      sizeof(gyro_3d_channels), GFP_KERNEL);
+	if (!indio_dev->channels) {
 		dev_err(&pdev->dev, "failed to duplicate channels\n");
 		return -ENOMEM;
 	}
 
-	ret = gyro_3d_parse_report(pdev, hsdev, channels,
-					HID_USAGE_SENSOR_GYRO_3D, gyro_state);
+	ret = gyro_3d_parse_report(pdev, hsdev,
+				   (struct iio_chan_spec *)indio_dev->channels,
+				   HID_USAGE_SENSOR_GYRO_3D, gyro_state);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to setup attributes\n");
 		goto error_free_dev_mem;
 	}
 
-	indio_dev->channels = channels;
 	indio_dev->num_channels = ARRAY_SIZE(gyro_3d_channels);
 	indio_dev->dev.parent = &pdev->dev;
 	indio_dev->info = &gyro_3d_info;
@@ -397,7 +396,7 @@ static int hid_gyro_3d_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_gyro_3d_ids[] = {
+static const struct platform_device_id hid_gyro_3d_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-200076",
diff --git a/drivers/iio/humidity/Kconfig b/drivers/iio/humidity/Kconfig
index 4813b793b9f7..688c0d1cb47d 100644
--- a/drivers/iio/humidity/Kconfig
+++ b/drivers/iio/humidity/Kconfig
@@ -5,7 +5,7 @@ menu "Humidity sensors"
 
 config DHT11
 	tristate "DHT11 (and compatible sensors) driver"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  This driver supports reading data via a single interrupt
 	  generating GPIO line. Currently tested are DHT11 and DHT22.
diff --git a/drivers/iio/imu/adis16400.h b/drivers/iio/imu/adis16400.h
index 0916bf6b6c31..73b189c1c0fb 100644
--- a/drivers/iio/imu/adis16400.h
+++ b/drivers/iio/imu/adis16400.h
@@ -139,6 +139,7 @@
 #define ADIS16400_NO_BURST		BIT(1)
 #define ADIS16400_HAS_SLOW_MODE		BIT(2)
 #define ADIS16400_HAS_SERIAL_NUMBER	BIT(3)
+#define ADIS16400_BURST_DIAG_STAT	BIT(4)
 
 struct adis16400_state;
 
@@ -165,6 +166,7 @@ struct adis16400_state {
 	int				filt_int;
 
 	struct adis adis;
+	unsigned long avail_scan_mask[2];
 };
 
 /* At the moment triggers are only used for ring buffer
diff --git a/drivers/iio/imu/adis16400_buffer.c b/drivers/iio/imu/adis16400_buffer.c
index 6e727ffe5262..90c24a23c679 100644
--- a/drivers/iio/imu/adis16400_buffer.c
+++ b/drivers/iio/imu/adis16400_buffer.c
@@ -18,7 +18,8 @@ int adis16400_update_scan_mode(struct iio_dev *indio_dev,
 {
 	struct adis16400_state *st = iio_priv(indio_dev);
 	struct adis *adis = &st->adis;
-	uint16_t *tx;
+	unsigned int burst_length;
+	u8 *tx;
 
 	if (st->variant->flags & ADIS16400_NO_BURST)
 		return adis_update_scan_mode(indio_dev, scan_mask);
@@ -26,26 +27,29 @@ int adis16400_update_scan_mode(struct iio_dev *indio_dev,
 	kfree(adis->xfer);
 	kfree(adis->buffer);
 
+	/* All but the timestamp channel */
+	burst_length = (indio_dev->num_channels - 1) * sizeof(u16);
+	if (st->variant->flags & ADIS16400_BURST_DIAG_STAT)
+		burst_length += sizeof(u16);
+
 	adis->xfer = kcalloc(2, sizeof(*adis->xfer), GFP_KERNEL);
 	if (!adis->xfer)
 		return -ENOMEM;
 
-	adis->buffer = kzalloc(indio_dev->scan_bytes + sizeof(u16),
-		GFP_KERNEL);
+	adis->buffer = kzalloc(burst_length + sizeof(u16), GFP_KERNEL);
 	if (!adis->buffer)
 		return -ENOMEM;
 
-	tx = adis->buffer + indio_dev->scan_bytes;
-
+	tx = adis->buffer + burst_length;
 	tx[0] = ADIS_READ_REG(ADIS16400_GLOB_CMD);
 	tx[1] = 0;
 
 	adis->xfer[0].tx_buf = tx;
 	adis->xfer[0].bits_per_word = 8;
 	adis->xfer[0].len = 2;
-	adis->xfer[1].tx_buf = tx;
+	adis->xfer[1].rx_buf = adis->buffer;
 	adis->xfer[1].bits_per_word = 8;
-	adis->xfer[1].len = indio_dev->scan_bytes;
+	adis->xfer[1].len = burst_length;
 
 	spi_message_init(&adis->msg);
 	spi_message_add_tail(&adis->xfer[0], &adis->msg);
@@ -61,6 +65,7 @@ irqreturn_t adis16400_trigger_handler(int irq, void *p)
 	struct adis16400_state *st = iio_priv(indio_dev);
 	struct adis *adis = &st->adis;
 	u32 old_speed_hz = st->adis.spi->max_speed_hz;
+	void *buffer;
 	int ret;
 
 	if (!adis->buffer)
@@ -81,7 +86,12 @@ irqreturn_t adis16400_trigger_handler(int irq, void *p)
 		spi_setup(st->adis.spi);
 	}
 
-	iio_push_to_buffers_with_timestamp(indio_dev, adis->buffer,
+	if (st->variant->flags & ADIS16400_BURST_DIAG_STAT)
+		buffer = adis->buffer + sizeof(u16);
+	else
+		buffer = adis->buffer;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, buffer,
 		pf->timestamp);
 
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/iio/imu/adis16400_core.c b/drivers/iio/imu/adis16400_core.c
index fa795dcd5f75..2fd68f2219a7 100644
--- a/drivers/iio/imu/adis16400_core.c
+++ b/drivers/iio/imu/adis16400_core.c
@@ -405,6 +405,11 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 			*val = st->variant->temp_scale_nano / 1000000;
 			*val2 = (st->variant->temp_scale_nano % 1000000);
 			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_PRESSURE:
+			/* 20 uBar = 0.002kPascal */
+			*val = 0;
+			*val2 = 2000;
+			return IIO_VAL_INT_PLUS_MICRO;
 		default:
 			return -EINVAL;
 		}
@@ -454,10 +459,10 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 	}
 }
 
-#define ADIS16400_VOLTAGE_CHAN(addr, bits, name, si) { \
+#define ADIS16400_VOLTAGE_CHAN(addr, bits, name, si, chn) { \
 	.type = IIO_VOLTAGE, \
 	.indexed = 1, \
-	.channel = 0, \
+	.channel = chn, \
 	.extend_name = name, \
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
 		BIT(IIO_CHAN_INFO_SCALE), \
@@ -474,10 +479,10 @@ static int adis16400_read_raw(struct iio_dev *indio_dev,
 }
 
 #define ADIS16400_SUPPLY_CHAN(addr, bits) \
-	ADIS16400_VOLTAGE_CHAN(addr, bits, "supply", ADIS16400_SCAN_SUPPLY)
+	ADIS16400_VOLTAGE_CHAN(addr, bits, "supply", ADIS16400_SCAN_SUPPLY, 0)
 
 #define ADIS16400_AUX_ADC_CHAN(addr, bits) \
-	ADIS16400_VOLTAGE_CHAN(addr, bits, NULL, ADIS16400_SCAN_ADC)
+	ADIS16400_VOLTAGE_CHAN(addr, bits, NULL, ADIS16400_SCAN_ADC, 1)
 
 #define ADIS16400_GYRO_CHAN(mod, addr, bits) { \
 	.type = IIO_ANGL_VEL, \
@@ -773,7 +778,8 @@ static struct adis16400_chip_info adis16400_chips[] = {
 		.channels = adis16448_channels,
 		.num_channels = ARRAY_SIZE(adis16448_channels),
 		.flags = ADIS16400_HAS_PROD_ID |
-				ADIS16400_HAS_SERIAL_NUMBER,
+				ADIS16400_HAS_SERIAL_NUMBER |
+				ADIS16400_BURST_DIAG_STAT,
 		.gyro_scale_micro = IIO_DEGREE_TO_RAD(10000), /* 0.01 deg/s */
 		.accel_scale_micro = IIO_G_TO_M_S_2(833), /* 1/1200 g */
 		.temp_scale_nano = 73860000, /* 0.07386 C */
@@ -791,11 +797,6 @@ static const struct iio_info adis16400_info = {
 	.debugfs_reg_access = adis_debugfs_reg_access,
 };
 
-static const unsigned long adis16400_burst_scan_mask[] = {
-	~0UL,
-	0,
-};
-
 static const char * const adis16400_status_error_msgs[] = {
 	[ADIS16400_DIAG_STAT_ZACCL_FAIL] = "Z-axis accelerometer self-test failure",
 	[ADIS16400_DIAG_STAT_YACCL_FAIL] = "Y-axis accelerometer self-test failure",
@@ -843,6 +844,20 @@ static const struct adis_data adis16400_data = {
 		BIT(ADIS16400_DIAG_STAT_POWER_LOW),
 };
 
+static void adis16400_setup_chan_mask(struct adis16400_state *st)
+{
+	const struct adis16400_chip_info *chip_info = st->variant;
+	unsigned i;
+
+	for (i = 0; i < chip_info->num_channels; i++) {
+		const struct iio_chan_spec *ch = &chip_info->channels[i];
+
+		if (ch->scan_index >= 0 &&
+		    ch->scan_index != ADIS16400_SCAN_TIMESTAMP)
+			st->avail_scan_mask[0] |= BIT(ch->scan_index);
+	}
+}
+
 static int adis16400_probe(struct spi_device *spi)
 {
 	struct adis16400_state *st;
@@ -866,8 +881,10 @@ static int adis16400_probe(struct spi_device *spi)
 	indio_dev->info = &adis16400_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	if (!(st->variant->flags & ADIS16400_NO_BURST))
-		indio_dev->available_scan_masks = adis16400_burst_scan_mask;
+	if (!(st->variant->flags & ADIS16400_NO_BURST)) {
+		adis16400_setup_chan_mask(st);
+		indio_dev->available_scan_masks = st->avail_scan_mask;
+	}
 
 	ret = adis_init(&st->adis, indio_dev, spi, &adis16400_data);
 	if (ret)
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index df919f44d513..6eee1b044c60 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -239,13 +239,19 @@ static ssize_t iio_scan_el_show(struct device *dev,
 /* Note NULL used as error indicator as it doesn't make sense. */
 static const unsigned long *iio_scan_mask_match(const unsigned long *av_masks,
 					  unsigned int masklength,
-					  const unsigned long *mask)
+					  const unsigned long *mask,
+					  bool strict)
 {
 	if (bitmap_empty(mask, masklength))
 		return NULL;
 	while (*av_masks) {
-		if (bitmap_subset(mask, av_masks, masklength))
-			return av_masks;
+		if (strict) {
+			if (bitmap_equal(mask, av_masks, masklength))
+				return av_masks;
+		} else {
+			if (bitmap_subset(mask, av_masks, masklength))
+				return av_masks;
+		}
 		av_masks += BITS_TO_LONGS(masklength);
 	}
 	return NULL;
@@ -295,7 +301,7 @@ static int iio_scan_mask_set(struct iio_dev *indio_dev,
 	if (indio_dev->available_scan_masks) {
 		mask = iio_scan_mask_match(indio_dev->available_scan_masks,
 					   indio_dev->masklength,
-					   trialmask);
+					   trialmask, false);
 		if (!mask)
 			goto err_invalid_mask;
 	}
@@ -539,26 +545,13 @@ static void iio_buffer_deactivate(struct iio_buffer *buffer)
 	iio_buffer_put(buffer);
 }
 
-void iio_disable_all_buffers(struct iio_dev *indio_dev)
+static void iio_buffer_deactivate_all(struct iio_dev *indio_dev)
 {
 	struct iio_buffer *buffer, *_buffer;
 
-	if (list_empty(&indio_dev->buffer_list))
-		return;
-
-	if (indio_dev->setup_ops->predisable)
-		indio_dev->setup_ops->predisable(indio_dev);
-
 	list_for_each_entry_safe(buffer, _buffer,
 			&indio_dev->buffer_list, buffer_list)
 		iio_buffer_deactivate(buffer);
-
-	indio_dev->currentmode = INDIO_DIRECT_MODE;
-	if (indio_dev->setup_ops->postdisable)
-		indio_dev->setup_ops->postdisable(indio_dev);
-
-	if (indio_dev->available_scan_masks == NULL)
-		kfree(indio_dev->active_scan_mask);
 }
 
 static void iio_buffer_update_bytes_per_datum(struct iio_dev *indio_dev,
@@ -575,167 +568,277 @@ static void iio_buffer_update_bytes_per_datum(struct iio_dev *indio_dev,
 	buffer->access->set_bytes_per_datum(buffer, bytes);
 }
 
-static int __iio_update_buffers(struct iio_dev *indio_dev,
-		       struct iio_buffer *insert_buffer,
-		       struct iio_buffer *remove_buffer)
+static int iio_buffer_request_update(struct iio_dev *indio_dev,
+	struct iio_buffer *buffer)
 {
 	int ret;
-	int success = 0;
-	struct iio_buffer *buffer;
-	unsigned long *compound_mask;
-	const unsigned long *old_mask;
 
-	/* Wind down existing buffers - iff there are any */
-	if (!list_empty(&indio_dev->buffer_list)) {
-		if (indio_dev->setup_ops->predisable) {
-			ret = indio_dev->setup_ops->predisable(indio_dev);
-			if (ret)
-				return ret;
-		}
-		indio_dev->currentmode = INDIO_DIRECT_MODE;
-		if (indio_dev->setup_ops->postdisable) {
-			ret = indio_dev->setup_ops->postdisable(indio_dev);
-			if (ret)
-				return ret;
+	iio_buffer_update_bytes_per_datum(indio_dev, buffer);
+	if (buffer->access->request_update) {
+		ret = buffer->access->request_update(buffer);
+		if (ret) {
+			dev_dbg(&indio_dev->dev,
+			       "Buffer not started: buffer parameter update failed (%d)\n",
+				ret);
+			return ret;
 		}
 	}
-	/* Keep a copy of current setup to allow roll back */
-	old_mask = indio_dev->active_scan_mask;
+
+	return 0;
+}
+
+static void iio_free_scan_mask(struct iio_dev *indio_dev,
+	const unsigned long *mask)
+{
+	/* If the mask is dynamically allocated free it, otherwise do nothing */
 	if (!indio_dev->available_scan_masks)
-		indio_dev->active_scan_mask = NULL;
+		kfree(mask);
+}
+
+struct iio_device_config {
+	unsigned int mode;
+	const unsigned long *scan_mask;
+	unsigned int scan_bytes;
+	bool scan_timestamp;
+};
+
+static int iio_verify_update(struct iio_dev *indio_dev,
+	struct iio_buffer *insert_buffer, struct iio_buffer *remove_buffer,
+	struct iio_device_config *config)
+{
+	unsigned long *compound_mask;
+	const unsigned long *scan_mask;
+	bool strict_scanmask = false;
+	struct iio_buffer *buffer;
+	bool scan_timestamp;
+	unsigned int modes;
+
+	memset(config, 0, sizeof(*config));
+
+	/*
+	 * If there is just one buffer and we are removing it there is nothing
+	 * to verify.
+	 */
+	if (remove_buffer && !insert_buffer &&
+		list_is_singular(&indio_dev->buffer_list))
+			return 0;
+
+	modes = indio_dev->modes;
+
+	list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
+		if (buffer == remove_buffer)
+			continue;
+		modes &= buffer->access->modes;
+	}
 
-	if (remove_buffer)
-		iio_buffer_deactivate(remove_buffer);
 	if (insert_buffer)
-		iio_buffer_activate(indio_dev, insert_buffer);
+		modes &= insert_buffer->access->modes;
 
-	/* If no buffers in list, we are done */
-	if (list_empty(&indio_dev->buffer_list)) {
-		indio_dev->currentmode = INDIO_DIRECT_MODE;
-		if (indio_dev->available_scan_masks == NULL)
-			kfree(old_mask);
-		return 0;
+	/* Definitely possible for devices to support both of these. */
+	if ((modes & INDIO_BUFFER_TRIGGERED) && indio_dev->trig) {
+		config->mode = INDIO_BUFFER_TRIGGERED;
+	} else if (modes & INDIO_BUFFER_HARDWARE) {
+		/*
+		 * Keep things simple for now and only allow a single buffer to
+		 * be connected in hardware mode.
+		 */
+		if (insert_buffer && !list_empty(&indio_dev->buffer_list))
+			return -EINVAL;
+		config->mode = INDIO_BUFFER_HARDWARE;
+		strict_scanmask = true;
+	} else if (modes & INDIO_BUFFER_SOFTWARE) {
+		config->mode = INDIO_BUFFER_SOFTWARE;
+	} else {
+		/* Can only occur on first buffer */
+		if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
+			dev_dbg(&indio_dev->dev, "Buffer not started: no trigger\n");
+		return -EINVAL;
 	}
 
 	/* What scan mask do we actually have? */
 	compound_mask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
 				sizeof(long), GFP_KERNEL);
-	if (compound_mask == NULL) {
-		if (indio_dev->available_scan_masks == NULL)
-			kfree(old_mask);
+	if (compound_mask == NULL)
 		return -ENOMEM;
-	}
-	indio_dev->scan_timestamp = 0;
+
+	scan_timestamp = false;
 
 	list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
+		if (buffer == remove_buffer)
+			continue;
 		bitmap_or(compound_mask, compound_mask, buffer->scan_mask,
 			  indio_dev->masklength);
-		indio_dev->scan_timestamp |= buffer->scan_timestamp;
+		scan_timestamp |= buffer->scan_timestamp;
+	}
+
+	if (insert_buffer) {
+		bitmap_or(compound_mask, compound_mask,
+			  insert_buffer->scan_mask, indio_dev->masklength);
+		scan_timestamp |= insert_buffer->scan_timestamp;
 	}
+
 	if (indio_dev->available_scan_masks) {
-		indio_dev->active_scan_mask =
-			iio_scan_mask_match(indio_dev->available_scan_masks,
-					    indio_dev->masklength,
-					    compound_mask);
-		if (indio_dev->active_scan_mask == NULL) {
-			/*
-			 * Roll back.
-			 * Note can only occur when adding a buffer.
-			 */
-			iio_buffer_deactivate(insert_buffer);
-			if (old_mask) {
-				indio_dev->active_scan_mask = old_mask;
-				success = -EINVAL;
-			}
-			else {
-				kfree(compound_mask);
-				ret = -EINVAL;
-				return ret;
-			}
-		}
+		scan_mask = iio_scan_mask_match(indio_dev->available_scan_masks,
+				    indio_dev->masklength,
+				    compound_mask,
+				    strict_scanmask);
+		kfree(compound_mask);
+		if (scan_mask == NULL)
+			return -EINVAL;
 	} else {
-		indio_dev->active_scan_mask = compound_mask;
+	    scan_mask = compound_mask;
 	}
 
+	config->scan_bytes = iio_compute_scan_bytes(indio_dev,
+				    scan_mask, scan_timestamp);
+	config->scan_mask = scan_mask;
+	config->scan_timestamp = scan_timestamp;
+
+	return 0;
+}
+
+static int iio_enable_buffers(struct iio_dev *indio_dev,
+	struct iio_device_config *config)
+{
+	int ret;
+
+	indio_dev->active_scan_mask = config->scan_mask;
+	indio_dev->scan_timestamp = config->scan_timestamp;
+	indio_dev->scan_bytes = config->scan_bytes;
+
 	iio_update_demux(indio_dev);
 
 	/* Wind up again */
 	if (indio_dev->setup_ops->preenable) {
 		ret = indio_dev->setup_ops->preenable(indio_dev);
 		if (ret) {
-			printk(KERN_ERR
+			dev_dbg(&indio_dev->dev,
 			       "Buffer not started: buffer preenable failed (%d)\n", ret);
-			goto error_remove_inserted;
-		}
-	}
-	indio_dev->scan_bytes =
-		iio_compute_scan_bytes(indio_dev,
-				       indio_dev->active_scan_mask,
-				       indio_dev->scan_timestamp);
-	list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
-		iio_buffer_update_bytes_per_datum(indio_dev, buffer);
-		if (buffer->access->request_update) {
-			ret = buffer->access->request_update(buffer);
-			if (ret) {
-				printk(KERN_INFO
-				       "Buffer not started: buffer parameter update failed (%d)\n", ret);
-				goto error_run_postdisable;
-			}
+			goto err_undo_config;
 		}
 	}
+
 	if (indio_dev->info->update_scan_mode) {
 		ret = indio_dev->info
 			->update_scan_mode(indio_dev,
 					   indio_dev->active_scan_mask);
 		if (ret < 0) {
-			printk(KERN_INFO "Buffer not started: update scan mode failed (%d)\n", ret);
-			goto error_run_postdisable;
+			dev_dbg(&indio_dev->dev,
+				"Buffer not started: update scan mode failed (%d)\n",
+				ret);
+			goto err_run_postdisable;
 		}
 	}
-	/* Definitely possible for devices to support both of these. */
-	if ((indio_dev->modes & INDIO_BUFFER_TRIGGERED) && indio_dev->trig) {
-		indio_dev->currentmode = INDIO_BUFFER_TRIGGERED;
-	} else if (indio_dev->modes & INDIO_BUFFER_HARDWARE) {
-		indio_dev->currentmode = INDIO_BUFFER_HARDWARE;
-	} else if (indio_dev->modes & INDIO_BUFFER_SOFTWARE) {
-		indio_dev->currentmode = INDIO_BUFFER_SOFTWARE;
-	} else { /* Should never be reached */
-		/* Can only occur on first buffer */
-		if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
-			pr_info("Buffer not started: no trigger\n");
-		ret = -EINVAL;
-		goto error_run_postdisable;
-	}
+
+	indio_dev->currentmode = config->mode;
 
 	if (indio_dev->setup_ops->postenable) {
 		ret = indio_dev->setup_ops->postenable(indio_dev);
 		if (ret) {
-			printk(KERN_INFO
+			dev_dbg(&indio_dev->dev,
 			       "Buffer not started: postenable failed (%d)\n", ret);
-			indio_dev->currentmode = INDIO_DIRECT_MODE;
-			if (indio_dev->setup_ops->postdisable)
-				indio_dev->setup_ops->postdisable(indio_dev);
-			goto error_disable_all_buffers;
+			goto err_run_postdisable;
 		}
 	}
 
-	if (indio_dev->available_scan_masks)
-		kfree(compound_mask);
-	else
-		kfree(old_mask);
-
-	return success;
+	return 0;
 
-error_disable_all_buffers:
+err_run_postdisable:
 	indio_dev->currentmode = INDIO_DIRECT_MODE;
-error_run_postdisable:
 	if (indio_dev->setup_ops->postdisable)
 		indio_dev->setup_ops->postdisable(indio_dev);
-error_remove_inserted:
+err_undo_config:
+	indio_dev->active_scan_mask = NULL;
+
+	return ret;
+}
+
+static int iio_disable_buffers(struct iio_dev *indio_dev)
+{
+	int ret = 0;
+	int ret2;
+
+	/* Wind down existing buffers - iff there are any */
+	if (list_empty(&indio_dev->buffer_list))
+		return 0;
+
+	/*
+	 * If things go wrong at some step in disable we still need to continue
+	 * to perform the other steps, otherwise we leave the device in a
+	 * inconsistent state. We return the error code for the first error we
+	 * encountered.
+	 */
+
+	if (indio_dev->setup_ops->predisable) {
+		ret2 = indio_dev->setup_ops->predisable(indio_dev);
+		if (ret2 && !ret)
+			ret = ret2;
+	}
+
+	indio_dev->currentmode = INDIO_DIRECT_MODE;
+
+	if (indio_dev->setup_ops->postdisable) {
+		ret2 = indio_dev->setup_ops->postdisable(indio_dev);
+		if (ret2 && !ret)
+			ret = ret2;
+	}
+
+	iio_free_scan_mask(indio_dev, indio_dev->active_scan_mask);
+	indio_dev->active_scan_mask = NULL;
+
+	return ret;
+}
+
+static int __iio_update_buffers(struct iio_dev *indio_dev,
+		       struct iio_buffer *insert_buffer,
+		       struct iio_buffer *remove_buffer)
+{
+	struct iio_device_config new_config;
+	int ret;
+
+	ret = iio_verify_update(indio_dev, insert_buffer, remove_buffer,
+		&new_config);
+	if (ret)
+		return ret;
+
+	if (insert_buffer) {
+		ret = iio_buffer_request_update(indio_dev, insert_buffer);
+		if (ret)
+			goto err_free_config;
+	}
+
+	ret = iio_disable_buffers(indio_dev);
+	if (ret)
+		goto err_deactivate_all;
+
+	if (remove_buffer)
+		iio_buffer_deactivate(remove_buffer);
 	if (insert_buffer)
-		iio_buffer_deactivate(insert_buffer);
-	indio_dev->active_scan_mask = old_mask;
-	kfree(compound_mask);
+		iio_buffer_activate(indio_dev, insert_buffer);
+
+	/* If no buffers in list, we are done */
+	if (list_empty(&indio_dev->buffer_list))
+		return 0;
+
+	ret = iio_enable_buffers(indio_dev, &new_config);
+	if (ret)
+		goto err_deactivate_all;
+
+	return 0;
+
+err_deactivate_all:
+	/*
+	 * We've already verified that the config is valid earlier. If things go
+	 * wrong in either enable or disable the most likely reason is an IO
+	 * error from the device. In this case there is no good recovery
+	 * strategy. Just make sure to disable everything and leave the device
+	 * in a sane state.  With a bit of luck the device might come back to
+	 * life again later and userspace can try again.
+	 */
+	iio_buffer_deactivate_all(indio_dev);
+
+err_free_config:
+	iio_free_scan_mask(indio_dev, new_config.scan_mask);
 	return ret;
 }
 
@@ -777,6 +880,12 @@ out_unlock:
 }
 EXPORT_SYMBOL_GPL(iio_update_buffers);
 
+void iio_disable_all_buffers(struct iio_dev *indio_dev)
+{
+	iio_disable_buffers(indio_dev);
+	iio_buffer_deactivate_all(indio_dev);
+}
+
 static ssize_t iio_buffer_store_enable(struct device *dev,
 				       struct device_attribute *attr,
 				       const char *buf,
@@ -806,8 +915,6 @@ static ssize_t iio_buffer_store_enable(struct device *dev,
 		ret = __iio_update_buffers(indio_dev,
 					 NULL, indio_dev->buffer);
 
-	if (ret < 0)
-		goto done;
 done:
 	mutex_unlock(&indio_dev->mlock);
 	return (ret < 0) ? ret : len;
@@ -886,6 +993,15 @@ int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 	int ret, i, attrn, attrcount, attrcount_orig = 0;
 	const struct iio_chan_spec *channels;
 
+	channels = indio_dev->channels;
+	if (channels) {
+		int ml = indio_dev->masklength;
+
+		for (i = 0; i < indio_dev->num_channels; i++)
+			ml = max(ml, channels[i].scan_index + 1);
+		indio_dev->masklength = ml;
+	}
+
 	if (!buffer)
 		return 0;
 
@@ -929,12 +1045,6 @@ int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 			if (channels[i].scan_index < 0)
 				continue;
 
-			/* Establish necessary mask length */
-			if (channels[i].scan_index >
-			    (int)indio_dev->masklength - 1)
-				indio_dev->masklength
-					= channels[i].scan_index + 1;
-
 			ret = iio_buffer_add_channel_sysfs(indio_dev,
 							 &channels[i]);
 			if (ret < 0)
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index 4df97f650e44..3524b0de8721 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -101,6 +101,8 @@ static const char * const iio_modifier_names[] = {
 	[IIO_MOD_WALKING] = "walking",
 	[IIO_MOD_STILL] = "still",
 	[IIO_MOD_ROOT_SUM_SQUARED_X_Y_Z] = "sqrt(x^2+y^2+z^2)",
+	[IIO_MOD_I] = "i",
+	[IIO_MOD_Q] = "q",
 };
 
 /* relies on pairs of these shared then separate */
@@ -117,6 +119,8 @@ static const char * const iio_chan_info_postfix[] = {
 	[IIO_CHAN_INFO_AVERAGE_RAW] = "mean_raw",
 	[IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY]
 	= "filter_low_pass_3db_frequency",
+	[IIO_CHAN_INFO_HIGH_PASS_FILTER_3DB_FREQUENCY]
+	= "filter_high_pass_3db_frequency",
 	[IIO_CHAN_INFO_SAMP_FREQ] = "sampling_frequency",
 	[IIO_CHAN_INFO_FREQUENCY] = "frequency",
 	[IIO_CHAN_INFO_PHASE] = "phase",
@@ -128,6 +132,8 @@ static const char * const iio_chan_info_postfix[] = {
 	[IIO_CHAN_INFO_CALIBWEIGHT] = "calibweight",
 	[IIO_CHAN_INFO_DEBOUNCE_COUNT] = "debounce_count",
 	[IIO_CHAN_INFO_DEBOUNCE_TIME] = "debounce_time",
+	[IIO_CHAN_INFO_CALIBEMISSIVITY] = "calibemissivity",
+	[IIO_CHAN_INFO_OVERSAMPLING_RATIO] = "oversampling_ratio",
 };
 
 /**
diff --git a/drivers/iio/industrialio-event.c b/drivers/iio/industrialio-event.c
index a99692ba91bc..894d8137c4cf 100644
--- a/drivers/iio/industrialio-event.c
+++ b/drivers/iio/industrialio-event.c
@@ -211,6 +211,8 @@ static const char * const iio_ev_info_text[] = {
 	[IIO_EV_INFO_VALUE] = "value",
 	[IIO_EV_INFO_HYSTERESIS] = "hysteresis",
 	[IIO_EV_INFO_PERIOD] = "period",
+	[IIO_EV_INFO_HIGH_PASS_FILTER_3DB] = "high_pass_filter_3db",
+	[IIO_EV_INFO_LOW_PASS_FILTER_3DB] = "low_pass_filter_3db",
 };
 
 static enum iio_event_direction iio_ev_attr_dir(struct iio_dev_attr *attr)
diff --git a/drivers/iio/kfifo_buf.c b/drivers/iio/kfifo_buf.c
index 55c267bbfd2f..c5b999f0c519 100644
--- a/drivers/iio/kfifo_buf.c
+++ b/drivers/iio/kfifo_buf.c
@@ -136,6 +136,8 @@ static const struct iio_buffer_access_funcs kfifo_access_funcs = {
 	.set_bytes_per_datum = &iio_set_bytes_per_datum_kfifo,
 	.set_length = &iio_set_length_kfifo,
 	.release = &iio_kfifo_buffer_release,
+
+	.modes = INDIO_BUFFER_SOFTWARE | INDIO_BUFFER_TRIGGERED,
 };
 
 struct iio_buffer *iio_kfifo_allocate(void)
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
index 01a1a16ab7be..e6198b7c9cbf 100644
--- a/drivers/iio/light/Kconfig
+++ b/drivers/iio/light/Kconfig
@@ -5,6 +5,19 @@
 
 menu "Light sensors"
 
+config ACPI_ALS
+	tristate "ACPI Ambient Light Sensor"
+	depends on ACPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select IIO_KFIFO_BUF
+	help
+	 Say Y here if you want to build a driver for the ACPI0008
+	 Ambient Light Sensor.
+
+	 To compile this driver as a module, choose M here: the module will
+	 be called acpi-als.
+
 config ADJD_S311
 	tristate "ADJD-S311-CR999 digital color sensor"
 	select IIO_BUFFER
@@ -37,6 +50,16 @@ config APDS9300
 	 To compile this driver as a module, choose M here: the
 	 module will be called apds9300.
 
+config BH1750
+	tristate "ROHM BH1750 ambient light sensor"
+	depends on I2C
+	help
+	 Say Y here to build support for the ROHM BH1710, BH1715, BH1721,
+	 BH1750, BH1751 ambient light sensors.
+
+	 To compile this driver as a module, choose M here: the module will
+	 be called bh1750.
+
 config CM32181
 	depends on I2C
 	tristate "CM32181 driver"
@@ -169,11 +192,23 @@ config LTR501
 	select IIO_TRIGGERED_BUFFER
 	help
 	 If you say yes here you get support for the Lite-On LTR-501ALS-01
-	 ambient light and proximity sensor.
+	 ambient light and proximity sensor. This driver also supports LTR-559
+	 ALS/PS or LTR-301 ALS sensors.
 
 	 This driver can also be built as a module.  If so, the module
          will be called ltr501.
 
+config STK3310
+	tristate "STK3310 ALS and proximity sensor"
+	depends on I2C
+	help
+	 Say yes here to get support for the Sensortek STK3310 ambient light
+	 and proximity sensor. The STK3311 model is also supported by this
+	 driver.
+
+	 Choosing M will build the driver as a module. If so, the module
+	 will be called stk3310.
+
 config TCS3414
 	tristate "TAOS TCS3414 digital color sensor"
 	depends on I2C
diff --git a/drivers/iio/light/Makefile b/drivers/iio/light/Makefile
index ad7c30fe443b..e2d50fd59c66 100644
--- a/drivers/iio/light/Makefile
+++ b/drivers/iio/light/Makefile
@@ -3,9 +3,11 @@
 #
 
 # When adding new entries keep the list in alphabetical order
+obj-$(CONFIG_ACPI_ALS)		+= acpi-als.o
 obj-$(CONFIG_ADJD_S311)		+= adjd_s311.o
 obj-$(CONFIG_AL3320A)		+= al3320a.o
 obj-$(CONFIG_APDS9300)		+= apds9300.o
+obj-$(CONFIG_BH1750)		+= bh1750.o
 obj-$(CONFIG_CM32181)		+= cm32181.o
 obj-$(CONFIG_CM3232)		+= cm3232.o
 obj-$(CONFIG_CM3323)		+= cm3323.o
@@ -18,6 +20,7 @@ obj-$(CONFIG_JSA1212)		+= jsa1212.o
 obj-$(CONFIG_SENSORS_LM3533)	+= lm3533-als.o
 obj-$(CONFIG_LTR501)		+= ltr501.o
 obj-$(CONFIG_SENSORS_TSL2563)	+= tsl2563.o
+obj-$(CONFIG_STK3310)          += stk3310.o
 obj-$(CONFIG_TCS3414)		+= tcs3414.o
 obj-$(CONFIG_TCS3472)		+= tcs3472.o
 obj-$(CONFIG_TSL4531)		+= tsl4531.o
diff --git a/drivers/iio/light/acpi-als.c b/drivers/iio/light/acpi-als.c
new file mode 100644
index 000000000000..1dafa0756bfa
--- /dev/null
+++ b/drivers/iio/light/acpi-als.c
@@ -0,0 +1,231 @@
+/*
+ * ACPI Ambient Light Sensor Driver
+ *
+ * Based on ALS driver:
+ * Copyright (C) 2009 Zhang Rui <rui.zhang@intel.com>
+ *
+ * Rework for IIO subsystem:
+ * Copyright (C) 2012-2013 Martin Liska <marxin.liska@gmail.com>
+ *
+ * Final cleanup and debugging:
+ * Copyright (C) 2013-2014 Marek Vasut <marex@denx.de>
+ * Copyright (C) 2015 Gabriele Mazzotta <gabriele.mzt@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/acpi.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+
+#define ACPI_ALS_CLASS			"als"
+#define ACPI_ALS_DEVICE_NAME		"acpi-als"
+#define ACPI_ALS_NOTIFY_ILLUMINANCE	0x80
+
+ACPI_MODULE_NAME("acpi-als");
+
+/*
+ * So far, there's only one channel in here, but the specification for
+ * ACPI0008 says there can be more to what the block can report. Like
+ * chromaticity and such. We are ready for incoming additions!
+ */
+static const struct iio_chan_spec acpi_als_channels[] = {
+	{
+		.type		= IIO_LIGHT,
+		.scan_type	= {
+			.sign		= 's',
+			.realbits	= 32,
+			.storagebits	= 32,
+		},
+		.info_mask_separate	= BIT(IIO_CHAN_INFO_RAW),
+	},
+};
+
+/*
+ * The event buffer contains timestamp and all the data from
+ * the ACPI0008 block. There are multiple, but so far we only
+ * support _ALI (illuminance). Once someone adds new channels
+ * to acpi_als_channels[], the evt_buffer below will grow
+ * automatically.
+ */
+#define EVT_NR_SOURCES		ARRAY_SIZE(acpi_als_channels)
+#define EVT_BUFFER_SIZE		\
+	(sizeof(s64) + (EVT_NR_SOURCES * sizeof(s32)))
+
+struct acpi_als {
+	struct acpi_device	*device;
+	struct mutex		lock;
+
+	s32			evt_buffer[EVT_BUFFER_SIZE];
+};
+
+/*
+ * All types of properties the ACPI0008 block can report. The ALI, ALC, ALT
+ * and ALP can all be handled by als_read_value() below, while the ALR is
+ * special.
+ *
+ * The _ALR property returns tables that can be used to fine-tune the values
+ * reported by the other props based on the particular hardware type and it's
+ * location (it contains tables for "rainy", "bright inhouse lighting" etc.).
+ *
+ * So far, we support only ALI (illuminance).
+ */
+#define ACPI_ALS_ILLUMINANCE	"_ALI"
+#define ACPI_ALS_CHROMATICITY	"_ALC"
+#define ACPI_ALS_COLOR_TEMP	"_ALT"
+#define ACPI_ALS_POLLING	"_ALP"
+#define ACPI_ALS_TABLES		"_ALR"
+
+static int als_read_value(struct acpi_als *als, char *prop, s32 *val)
+{
+	unsigned long long temp_val;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(als->device->handle, prop, NULL,
+				       &temp_val);
+
+	if (ACPI_FAILURE(status)) {
+		ACPI_EXCEPTION((AE_INFO, status, "Error reading ALS %s", prop));
+		return -EIO;
+	}
+
+	*val = temp_val;
+
+	return 0;
+}
+
+static void acpi_als_notify(struct acpi_device *device, u32 event)
+{
+	struct iio_dev *indio_dev = acpi_driver_data(device);
+	struct acpi_als *als = iio_priv(indio_dev);
+	s32 *buffer = als->evt_buffer;
+	s64 time_ns = iio_get_time_ns();
+	s32 val;
+	int ret;
+
+	mutex_lock(&als->lock);
+
+	memset(buffer, 0, EVT_BUFFER_SIZE);
+
+	switch (event) {
+	case ACPI_ALS_NOTIFY_ILLUMINANCE:
+		ret = als_read_value(als, ACPI_ALS_ILLUMINANCE, &val);
+		if (ret < 0)
+			goto out;
+		*buffer++ = val;
+		break;
+	default:
+		/* Unhandled event */
+		dev_dbg(&device->dev, "Unhandled ACPI ALS event (%08x)!\n",
+			event);
+		goto out;
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev, als->evt_buffer, time_ns);
+
+out:
+	mutex_unlock(&als->lock);
+}
+
+static int acpi_als_read_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan, int *val,
+			     int *val2, long mask)
+{
+	struct acpi_als *als = iio_priv(indio_dev);
+	s32 temp_val;
+	int ret;
+
+	if (mask != IIO_CHAN_INFO_RAW)
+		return -EINVAL;
+
+	/* we support only illumination (_ALI) so far. */
+	if (chan->type != IIO_LIGHT)
+		return -EINVAL;
+
+	ret = als_read_value(als, ACPI_ALS_ILLUMINANCE, &temp_val);
+	if (ret < 0)
+		return ret;
+
+	*val = temp_val;
+
+	return IIO_VAL_INT;
+}
+
+static const struct iio_info acpi_als_info = {
+	.driver_module		= THIS_MODULE,
+	.read_raw		= acpi_als_read_raw,
+};
+
+static int acpi_als_add(struct acpi_device *device)
+{
+	struct acpi_als *als;
+	struct iio_dev *indio_dev;
+	struct iio_buffer *buffer;
+
+	indio_dev = devm_iio_device_alloc(&device->dev, sizeof(*als));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	als = iio_priv(indio_dev);
+
+	device->driver_data = indio_dev;
+	als->device = device;
+	mutex_init(&als->lock);
+
+	indio_dev->name = ACPI_ALS_DEVICE_NAME;
+	indio_dev->dev.parent = &device->dev;
+	indio_dev->info = &acpi_als_info;
+	indio_dev->modes = INDIO_BUFFER_SOFTWARE;
+	indio_dev->channels = acpi_als_channels;
+	indio_dev->num_channels = ARRAY_SIZE(acpi_als_channels);
+
+	buffer = devm_iio_kfifo_allocate(&device->dev);
+	if (!buffer)
+		return -ENOMEM;
+
+	iio_device_attach_buffer(indio_dev, buffer);
+
+	return devm_iio_device_register(&device->dev, indio_dev);
+}
+
+static const struct acpi_device_id acpi_als_device_ids[] = {
+	{"ACPI0008", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(acpi, acpi_als_device_ids);
+
+static struct acpi_driver acpi_als_driver = {
+	.name	= "acpi_als",
+	.class	= ACPI_ALS_CLASS,
+	.ids	= acpi_als_device_ids,
+	.ops = {
+		.add	= acpi_als_add,
+		.notify	= acpi_als_notify,
+	},
+};
+
+module_acpi_driver(acpi_als_driver);
+
+MODULE_AUTHOR("Zhang Rui <rui.zhang@intel.com>");
+MODULE_AUTHOR("Martin Liska <marxin.liska@gmail.com>");
+MODULE_AUTHOR("Marek Vasut <marex@denx.de>");
+MODULE_DESCRIPTION("ACPI Ambient Light Sensor Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/light/bh1750.c b/drivers/iio/light/bh1750.c
new file mode 100644
index 000000000000..564c2b3c1a83
--- /dev/null
+++ b/drivers/iio/light/bh1750.c
@@ -0,0 +1,334 @@
+/*
+ * ROHM BH1710/BH1715/BH1721/BH1750/BH1751 ambient light sensor driver
+ *
+ * Copyright (c) Tomasz Duszynski <tduszyns@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.
+ *
+ * Data sheets:
+ *  http://rohmfs.rohm.com/en/products/databook/datasheet/ic/sensor/light/bh1710fvc-e.pdf
+ *  http://rohmfs.rohm.com/en/products/databook/datasheet/ic/sensor/light/bh1715fvc-e.pdf
+ *  http://rohmfs.rohm.com/en/products/databook/datasheet/ic/sensor/light/bh1721fvc-e.pdf
+ *  http://rohmfs.rohm.com/en/products/databook/datasheet/ic/sensor/light/bh1750fvi-e.pdf
+ *  http://rohmfs.rohm.com/en/products/databook/datasheet/ic/sensor/light/bh1751fvi-e.pdf
+ *
+ * 7-bit I2C slave addresses:
+ *  0x23 (ADDR pin low)
+ *  0x5C (ADDR pin high)
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/module.h>
+
+#define BH1750_POWER_DOWN		0x00
+#define BH1750_ONE_TIME_H_RES_MODE	0x20 /* auto-mode for BH1721 */
+#define BH1750_CHANGE_INT_TIME_H_BIT	0x40
+#define BH1750_CHANGE_INT_TIME_L_BIT	0x60
+
+enum {
+	BH1710,
+	BH1721,
+	BH1750,
+};
+
+struct bh1750_chip_info;
+struct bh1750_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	const struct bh1750_chip_info *chip_info;
+	u16 mtreg;
+};
+
+struct bh1750_chip_info {
+	u16 mtreg_min;
+	u16 mtreg_max;
+	u16 mtreg_default;
+	int mtreg_to_usec;
+	int mtreg_to_scale;
+
+	/*
+	 * For BH1710/BH1721 all possible integration time values won't fit
+	 * into one page so displaying is limited to every second one.
+	 * Note, that user can still write proper values which were not
+	 * listed.
+	 */
+	int inc;
+
+	u16 int_time_low_mask;
+	u16 int_time_high_mask;
+}
+
+static const bh1750_chip_info_tbl[] = {
+	[BH1710] = { 140, 1022, 300, 400,  250000000, 2, 0x001F, 0x03E0 },
+	[BH1721] = { 140, 1020, 300, 400,  250000000, 2, 0x0010, 0x03E0 },
+	[BH1750] = { 31,  254,  69,  1740, 57500000,  1, 0x001F, 0x00E0 },
+};
+
+static int bh1750_change_int_time(struct bh1750_data *data, int usec)
+{
+	int ret;
+	u16 val;
+	u8 regval;
+	const struct bh1750_chip_info *chip_info = data->chip_info;
+
+	if ((usec % chip_info->mtreg_to_usec) != 0)
+		return -EINVAL;
+
+	val = usec / chip_info->mtreg_to_usec;
+	if (val < chip_info->mtreg_min || val > chip_info->mtreg_max)
+		return -EINVAL;
+
+	ret = i2c_smbus_write_byte(data->client, BH1750_POWER_DOWN);
+	if (ret < 0)
+		return ret;
+
+	regval = (val & chip_info->int_time_high_mask) >> 5;
+	ret = i2c_smbus_write_byte(data->client,
+				   BH1750_CHANGE_INT_TIME_H_BIT | regval);
+	if (ret < 0)
+		return ret;
+
+	regval = val & chip_info->int_time_low_mask;
+	ret = i2c_smbus_write_byte(data->client,
+				   BH1750_CHANGE_INT_TIME_L_BIT | regval);
+	if (ret < 0)
+		return ret;
+
+	data->mtreg = val;
+
+	return 0;
+}
+
+static int bh1750_read(struct bh1750_data *data, int *val)
+{
+	int ret;
+	__be16 result;
+	const struct bh1750_chip_info *chip_info = data->chip_info;
+	unsigned long delay = chip_info->mtreg_to_usec * data->mtreg;
+
+	/*
+	 * BH1721 will enter continuous mode on receiving this command.
+	 * Note, that this eliminates need for bh1750_resume().
+	 */
+	ret = i2c_smbus_write_byte(data->client, BH1750_ONE_TIME_H_RES_MODE);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(delay + 15000, delay + 40000);
+
+	ret = i2c_master_recv(data->client, (char *)&result, 2);
+	if (ret < 0)
+		return ret;
+
+	*val = be16_to_cpu(result);
+
+	return 0;
+}
+
+static int bh1750_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	int ret, tmp;
+	struct bh1750_data *data = iio_priv(indio_dev);
+	const struct bh1750_chip_info *chip_info = data->chip_info;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		switch (chan->type) {
+		case IIO_LIGHT:
+			mutex_lock(&data->lock);
+			ret = bh1750_read(data, val);
+			mutex_unlock(&data->lock);
+			if (ret < 0)
+				return ret;
+
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_SCALE:
+		tmp = chip_info->mtreg_to_scale / data->mtreg;
+		*val = tmp / 1000000;
+		*val2 = tmp % 1000000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_INT_TIME:
+		*val = 0;
+		*val2 = chip_info->mtreg_to_usec * data->mtreg;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int bh1750_write_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int val, int val2, long mask)
+{
+	int ret;
+	struct bh1750_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_INT_TIME:
+		if (val != 0)
+			return -EINVAL;
+
+		mutex_lock(&data->lock);
+		ret = bh1750_change_int_time(data, val2);
+		mutex_unlock(&data->lock);
+		return ret;
+	default:
+		return -EINVAL;
+	}
+}
+
+static ssize_t bh1750_show_int_time_available(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int i;
+	size_t len = 0;
+	struct bh1750_data *data = iio_priv(dev_to_iio_dev(dev));
+	const struct bh1750_chip_info *chip_info = data->chip_info;
+
+	for (i = chip_info->mtreg_min; i <= chip_info->mtreg_max; i += chip_info->inc)
+		len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06d ",
+				 chip_info->mtreg_to_usec * i);
+
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static IIO_DEV_ATTR_INT_TIME_AVAIL(bh1750_show_int_time_available);
+
+static struct attribute *bh1750_attributes[] = {
+	&iio_dev_attr_integration_time_available.dev_attr.attr,
+	NULL,
+};
+
+static struct attribute_group bh1750_attribute_group = {
+	.attrs = bh1750_attributes,
+};
+
+static const struct iio_info bh1750_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &bh1750_attribute_group,
+	.read_raw = bh1750_read_raw,
+	.write_raw = bh1750_write_raw,
+};
+
+static const struct iio_chan_spec bh1750_channels[] = {
+	{
+		.type = IIO_LIGHT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				      BIT(IIO_CHAN_INFO_SCALE) |
+				      BIT(IIO_CHAN_INFO_INT_TIME)
+	}
+};
+
+static int bh1750_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret, usec;
+	struct bh1750_data *data;
+	struct iio_dev *indio_dev;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C |
+				I2C_FUNC_SMBUS_WRITE_BYTE))
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+	data->chip_info = &bh1750_chip_info_tbl[id->driver_data];
+
+	usec = data->chip_info->mtreg_to_usec * data->chip_info->mtreg_default;
+	ret = bh1750_change_int_time(data, usec);
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&data->lock);
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &bh1750_info;
+	indio_dev->name = id->name;
+	indio_dev->channels = bh1750_channels;
+	indio_dev->num_channels = ARRAY_SIZE(bh1750_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	return iio_device_register(indio_dev);
+}
+
+static int bh1750_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct bh1750_data *data = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	mutex_lock(&data->lock);
+	i2c_smbus_write_byte(client, BH1750_POWER_DOWN);
+	mutex_unlock(&data->lock);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int bh1750_suspend(struct device *dev)
+{
+	int ret;
+	struct bh1750_data *data =
+		iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	/*
+	 * This is mainly for BH1721 which doesn't enter power down
+	 * mode automatically.
+	 */
+	mutex_lock(&data->lock);
+	ret = i2c_smbus_write_byte(data->client, BH1750_POWER_DOWN);
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static SIMPLE_DEV_PM_OPS(bh1750_pm_ops, bh1750_suspend, NULL);
+#define BH1750_PM_OPS (&bh1750_pm_ops)
+#else
+#define BH1750_PM_OPS NULL
+#endif
+
+static const struct i2c_device_id bh1750_id[] = {
+	{ "bh1710", BH1710 },
+	{ "bh1715", BH1750 },
+	{ "bh1721", BH1721 },
+	{ "bh1750", BH1750 },
+	{ "bh1751", BH1750 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, bh1750_id);
+
+static struct i2c_driver bh1750_driver = {
+	.driver = {
+		.name = "bh1750",
+		.owner = THIS_MODULE,
+		.pm = BH1750_PM_OPS,
+	},
+	.probe = bh1750_probe,
+	.remove = bh1750_remove,
+	.id_table = bh1750_id,
+
+};
+module_i2c_driver(bh1750_driver);
+
+MODULE_AUTHOR("Tomasz Duszynski <tduszyns@gmail.com>");
+MODULE_DESCRIPTION("ROHM BH1710/BH1715/BH1721/BH1750/BH1751 als driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/light/hid-sensor-als.c b/drivers/iio/light/hid-sensor-als.c
index 1609ecdd01b0..8bb1f90ecd51 100644
--- a/drivers/iio/light/hid-sensor-als.c
+++ b/drivers/iio/light/hid-sensor-als.c
@@ -263,7 +263,6 @@ static int hid_als_probe(struct platform_device *pdev)
 	struct iio_dev *indio_dev;
 	struct als_state *als_state;
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
-	struct iio_chan_spec *channels;
 
 	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(struct als_state));
 	if (!indio_dev)
@@ -281,20 +280,21 @@ static int hid_als_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	channels = kmemdup(als_channels, sizeof(als_channels), GFP_KERNEL);
-	if (!channels) {
+	indio_dev->channels = kmemdup(als_channels,
+				      sizeof(als_channels), GFP_KERNEL);
+	if (!indio_dev->channels) {
 		dev_err(&pdev->dev, "failed to duplicate channels\n");
 		return -ENOMEM;
 	}
 
-	ret = als_parse_report(pdev, hsdev, channels,
-				HID_USAGE_SENSOR_ALS, als_state);
+	ret = als_parse_report(pdev, hsdev,
+			       (struct iio_chan_spec *)indio_dev->channels,
+			       HID_USAGE_SENSOR_ALS, als_state);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to setup attributes\n");
 		goto error_free_dev_mem;
 	}
 
-	indio_dev->channels = channels;
 	indio_dev->num_channels =
 				ARRAY_SIZE(als_channels);
 	indio_dev->dev.parent = &pdev->dev;
@@ -361,7 +361,7 @@ static int hid_als_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_als_ids[] = {
+static const struct platform_device_id hid_als_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-200041",
diff --git a/drivers/iio/light/hid-sensor-prox.c b/drivers/iio/light/hid-sensor-prox.c
index ef60bae738e3..0d248476f4c9 100644
--- a/drivers/iio/light/hid-sensor-prox.c
+++ b/drivers/iio/light/hid-sensor-prox.c
@@ -350,7 +350,7 @@ static int hid_prox_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_prox_ids[] = {
+static const struct platform_device_id hid_prox_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-200011",
diff --git a/drivers/iio/light/ltr501.c b/drivers/iio/light/ltr501.c
index 78b87839c4b9..1ef7d3773ab9 100644
--- a/drivers/iio/light/ltr501.c
+++ b/drivers/iio/light/ltr501.c
@@ -9,15 +9,18 @@
  *
  * 7-bit I2C slave address 0x23
  *
- * TODO: interrupt, threshold, measurement rate, IR LED characteristics
+ * TODO: IR LED characteristics
  */
 
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/err.h>
 #include <linux/delay.h>
+#include <linux/regmap.h>
+#include <linux/acpi.h>
 
 #include <linux/iio/iio.h>
+#include <linux/iio/events.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/trigger_consumer.h>
 #include <linux/iio/buffer.h>
@@ -27,12 +30,21 @@
 
 #define LTR501_ALS_CONTR 0x80 /* ALS operation mode, SW reset */
 #define LTR501_PS_CONTR 0x81 /* PS operation mode */
+#define LTR501_PS_MEAS_RATE 0x84 /* measurement rate*/
+#define LTR501_ALS_MEAS_RATE 0x85 /* ALS integ time, measurement rate*/
 #define LTR501_PART_ID 0x86
 #define LTR501_MANUFAC_ID 0x87
 #define LTR501_ALS_DATA1 0x88 /* 16-bit, little endian */
 #define LTR501_ALS_DATA0 0x8a /* 16-bit, little endian */
 #define LTR501_ALS_PS_STATUS 0x8c
 #define LTR501_PS_DATA 0x8d /* 16-bit, little endian */
+#define LTR501_INTR 0x8f /* output mode, polarity, mode */
+#define LTR501_PS_THRESH_UP 0x90 /* 11 bit, ps upper threshold */
+#define LTR501_PS_THRESH_LOW 0x92 /* 11 bit, ps lower threshold */
+#define LTR501_ALS_THRESH_UP 0x97 /* 16 bit, ALS upper threshold */
+#define LTR501_ALS_THRESH_LOW 0x99 /* 16 bit, ALS lower threshold */
+#define LTR501_INTR_PRST 0x9e /* ps thresh, als thresh */
+#define LTR501_MAX_REG 0x9f
 
 #define LTR501_ALS_CONTR_SW_RESET BIT(2)
 #define LTR501_CONTR_PS_GAIN_MASK (BIT(3) | BIT(2))
@@ -40,28 +52,288 @@
 #define LTR501_CONTR_ALS_GAIN_MASK BIT(3)
 #define LTR501_CONTR_ACTIVE BIT(1)
 
+#define LTR501_STATUS_ALS_INTR BIT(3)
 #define LTR501_STATUS_ALS_RDY BIT(2)
+#define LTR501_STATUS_PS_INTR BIT(1)
 #define LTR501_STATUS_PS_RDY BIT(0)
 
 #define LTR501_PS_DATA_MASK 0x7ff
+#define LTR501_PS_THRESH_MASK 0x7ff
+#define LTR501_ALS_THRESH_MASK 0xffff
+
+#define LTR501_ALS_DEF_PERIOD 500000
+#define LTR501_PS_DEF_PERIOD 100000
+
+#define LTR501_REGMAP_NAME "ltr501_regmap"
+
+#define LTR501_LUX_CONV(vis_coeff, vis_data, ir_coeff, ir_data) \
+			((vis_coeff * vis_data) - (ir_coeff * ir_data))
+
+static const int int_time_mapping[] = {100000, 50000, 200000, 400000};
+
+static const struct reg_field reg_field_it =
+				REG_FIELD(LTR501_ALS_MEAS_RATE, 3, 4);
+static const struct reg_field reg_field_als_intr =
+				REG_FIELD(LTR501_INTR, 0, 0);
+static const struct reg_field reg_field_ps_intr =
+				REG_FIELD(LTR501_INTR, 1, 1);
+static const struct reg_field reg_field_als_rate =
+				REG_FIELD(LTR501_ALS_MEAS_RATE, 0, 2);
+static const struct reg_field reg_field_ps_rate =
+				REG_FIELD(LTR501_PS_MEAS_RATE, 0, 3);
+static const struct reg_field reg_field_als_prst =
+				REG_FIELD(LTR501_INTR_PRST, 0, 3);
+static const struct reg_field reg_field_ps_prst =
+				REG_FIELD(LTR501_INTR_PRST, 4, 7);
+
+struct ltr501_samp_table {
+	int freq_val;  /* repetition frequency in micro HZ*/
+	int time_val; /* repetition rate in micro seconds */
+};
+
+#define LTR501_RESERVED_GAIN -1
+
+enum {
+	ltr501 = 0,
+	ltr559,
+	ltr301,
+};
+
+struct ltr501_gain {
+	int scale;
+	int uscale;
+};
+
+static struct ltr501_gain ltr501_als_gain_tbl[] = {
+	{1, 0},
+	{0, 5000},
+};
+
+static struct ltr501_gain ltr559_als_gain_tbl[] = {
+	{1, 0},
+	{0, 500000},
+	{0, 250000},
+	{0, 125000},
+	{LTR501_RESERVED_GAIN, LTR501_RESERVED_GAIN},
+	{LTR501_RESERVED_GAIN, LTR501_RESERVED_GAIN},
+	{0, 20000},
+	{0, 10000},
+};
+
+static struct ltr501_gain ltr501_ps_gain_tbl[] = {
+	{1, 0},
+	{0, 250000},
+	{0, 125000},
+	{0, 62500},
+};
+
+static struct ltr501_gain ltr559_ps_gain_tbl[] = {
+	{0, 62500}, /* x16 gain */
+	{0, 31250}, /* x32 gain */
+	{0, 15625}, /* bits X1 are for x64 gain */
+	{0, 15624},
+};
+
+struct ltr501_chip_info {
+	u8 partid;
+	struct ltr501_gain *als_gain;
+	int als_gain_tbl_size;
+	struct ltr501_gain *ps_gain;
+	int ps_gain_tbl_size;
+	u8 als_mode_active;
+	u8 als_gain_mask;
+	u8 als_gain_shift;
+	struct iio_chan_spec const *channels;
+	const int no_channels;
+	const struct iio_info *info;
+	const struct iio_info *info_no_irq;
+};
 
 struct ltr501_data {
 	struct i2c_client *client;
 	struct mutex lock_als, lock_ps;
+	struct ltr501_chip_info *chip_info;
 	u8 als_contr, ps_contr;
+	int als_period, ps_period; /* period in micro seconds */
+	struct regmap *regmap;
+	struct regmap_field *reg_it;
+	struct regmap_field *reg_als_intr;
+	struct regmap_field *reg_ps_intr;
+	struct regmap_field *reg_als_rate;
+	struct regmap_field *reg_ps_rate;
+	struct regmap_field *reg_als_prst;
+	struct regmap_field *reg_ps_prst;
+};
+
+static const struct ltr501_samp_table ltr501_als_samp_table[] = {
+			{20000000, 50000}, {10000000, 100000},
+			{5000000, 200000}, {2000000, 500000},
+			{1000000, 1000000}, {500000, 2000000},
+			{500000, 2000000}, {500000, 2000000}
 };
 
+static const struct ltr501_samp_table ltr501_ps_samp_table[] = {
+			{20000000, 50000}, {14285714, 70000},
+			{10000000, 100000}, {5000000, 200000},
+			{2000000, 500000}, {1000000, 1000000},
+			{500000, 2000000}, {500000, 2000000},
+			{500000, 2000000}
+};
+
+static unsigned int ltr501_match_samp_freq(const struct ltr501_samp_table *tab,
+					   int len, int val, int val2)
+{
+	int i, freq;
+
+	freq = val * 1000000 + val2;
+
+	for (i = 0; i < len; i++) {
+		if (tab[i].freq_val == freq)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int ltr501_als_read_samp_freq(struct ltr501_data *data,
+				     int *val, int *val2)
+{
+	int ret, i;
+
+	ret = regmap_field_read(data->reg_als_rate, &i);
+	if (ret < 0)
+		return ret;
+
+	if (i < 0 || i >= ARRAY_SIZE(ltr501_als_samp_table))
+		return -EINVAL;
+
+	*val = ltr501_als_samp_table[i].freq_val / 1000000;
+	*val2 = ltr501_als_samp_table[i].freq_val % 1000000;
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int ltr501_ps_read_samp_freq(struct ltr501_data *data,
+				    int *val, int *val2)
+{
+	int ret, i;
+
+	ret = regmap_field_read(data->reg_ps_rate, &i);
+	if (ret < 0)
+		return ret;
+
+	if (i < 0 || i >= ARRAY_SIZE(ltr501_ps_samp_table))
+		return -EINVAL;
+
+	*val = ltr501_ps_samp_table[i].freq_val / 1000000;
+	*val2 = ltr501_ps_samp_table[i].freq_val % 1000000;
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int ltr501_als_write_samp_freq(struct ltr501_data *data,
+				      int val, int val2)
+{
+	int i, ret;
+
+	i = ltr501_match_samp_freq(ltr501_als_samp_table,
+				   ARRAY_SIZE(ltr501_als_samp_table),
+				   val, val2);
+
+	if (i < 0)
+		return i;
+
+	mutex_lock(&data->lock_als);
+	ret = regmap_field_write(data->reg_als_rate, i);
+	mutex_unlock(&data->lock_als);
+
+	return ret;
+}
+
+static int ltr501_ps_write_samp_freq(struct ltr501_data *data,
+				     int val, int val2)
+{
+	int i, ret;
+
+	i = ltr501_match_samp_freq(ltr501_ps_samp_table,
+				   ARRAY_SIZE(ltr501_ps_samp_table),
+				   val, val2);
+
+	if (i < 0)
+		return i;
+
+	mutex_lock(&data->lock_ps);
+	ret = regmap_field_write(data->reg_ps_rate, i);
+	mutex_unlock(&data->lock_ps);
+
+	return ret;
+}
+
+static int ltr501_als_read_samp_period(struct ltr501_data *data, int *val)
+{
+	int ret, i;
+
+	ret = regmap_field_read(data->reg_als_rate, &i);
+	if (ret < 0)
+		return ret;
+
+	if (i < 0 || i >= ARRAY_SIZE(ltr501_als_samp_table))
+		return -EINVAL;
+
+	*val = ltr501_als_samp_table[i].time_val;
+
+	return IIO_VAL_INT;
+}
+
+static int ltr501_ps_read_samp_period(struct ltr501_data *data, int *val)
+{
+	int ret, i;
+
+	ret = regmap_field_read(data->reg_ps_rate, &i);
+	if (ret < 0)
+		return ret;
+
+	if (i < 0 || i >= ARRAY_SIZE(ltr501_ps_samp_table))
+		return -EINVAL;
+
+	*val = ltr501_ps_samp_table[i].time_val;
+
+	return IIO_VAL_INT;
+}
+
+/* IR and visible spectrum coeff's are given in data sheet */
+static unsigned long ltr501_calculate_lux(u16 vis_data, u16 ir_data)
+{
+	unsigned long ratio, lux;
+
+	if (vis_data == 0)
+		return 0;
+
+	/* multiply numerator by 100 to avoid handling ratio < 1 */
+	ratio = DIV_ROUND_UP(ir_data * 100, ir_data + vis_data);
+
+	if (ratio < 45)
+		lux = LTR501_LUX_CONV(1774, vis_data, -1105, ir_data);
+	else if (ratio >= 45 && ratio < 64)
+		lux = LTR501_LUX_CONV(3772, vis_data, 1336, ir_data);
+	else if (ratio >= 64 && ratio < 85)
+		lux = LTR501_LUX_CONV(1690, vis_data, 169, ir_data);
+	else
+		lux = 0;
+
+	return lux / 1000;
+}
+
 static int ltr501_drdy(struct ltr501_data *data, u8 drdy_mask)
 {
 	int tries = 100;
-	int ret;
+	int ret, status;
 
 	while (tries--) {
-		ret = i2c_smbus_read_byte_data(data->client,
-			LTR501_ALS_PS_STATUS);
+		ret = regmap_read(data->regmap, LTR501_ALS_PS_STATUS, &status);
 		if (ret < 0)
 			return ret;
-		if ((ret & drdy_mask) == drdy_mask)
+		if ((status & drdy_mask) == drdy_mask)
 			return 0;
 		msleep(25);
 	}
@@ -70,25 +342,221 @@ static int ltr501_drdy(struct ltr501_data *data, u8 drdy_mask)
 	return -EIO;
 }
 
+static int ltr501_set_it_time(struct ltr501_data *data, int it)
+{
+	int ret, i, index = -1, status;
+
+	for (i = 0; i < ARRAY_SIZE(int_time_mapping); i++) {
+		if (int_time_mapping[i] == it) {
+			index = i;
+			break;
+		}
+	}
+	/* Make sure integ time index is valid */
+	if (index < 0)
+		return -EINVAL;
+
+	ret = regmap_read(data->regmap, LTR501_ALS_CONTR, &status);
+	if (ret < 0)
+		return ret;
+
+	if (status & LTR501_CONTR_ALS_GAIN_MASK) {
+		/*
+		 * 200 ms and 400 ms integ time can only be
+		 * used in dynamic range 1
+		 */
+		if (index > 1)
+			return -EINVAL;
+	} else
+		/* 50 ms integ time can only be used in dynamic range 2 */
+		if (index == 1)
+			return -EINVAL;
+
+	return regmap_field_write(data->reg_it, index);
+}
+
+/* read int time in micro seconds */
+static int ltr501_read_it_time(struct ltr501_data *data, int *val, int *val2)
+{
+	int ret, index;
+
+	ret = regmap_field_read(data->reg_it, &index);
+	if (ret < 0)
+		return ret;
+
+	/* Make sure integ time index is valid */
+	if (index < 0 || index >= ARRAY_SIZE(int_time_mapping))
+		return -EINVAL;
+
+	*val2 = int_time_mapping[index];
+	*val = 0;
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
 static int ltr501_read_als(struct ltr501_data *data, __le16 buf[2])
 {
-	int ret = ltr501_drdy(data, LTR501_STATUS_ALS_RDY);
+	int ret;
+
+	ret = ltr501_drdy(data, LTR501_STATUS_ALS_RDY);
 	if (ret < 0)
 		return ret;
 	/* always read both ALS channels in given order */
-	return i2c_smbus_read_i2c_block_data(data->client,
-		LTR501_ALS_DATA1, 2 * sizeof(__le16), (u8 *) buf);
+	return regmap_bulk_read(data->regmap, LTR501_ALS_DATA1,
+				buf, 2 * sizeof(__le16));
 }
 
 static int ltr501_read_ps(struct ltr501_data *data)
 {
-	int ret = ltr501_drdy(data, LTR501_STATUS_PS_RDY);
+	int ret, status;
+
+	ret = ltr501_drdy(data, LTR501_STATUS_PS_RDY);
 	if (ret < 0)
 		return ret;
-	return i2c_smbus_read_word_data(data->client, LTR501_PS_DATA);
+
+	ret = regmap_bulk_read(data->regmap, LTR501_PS_DATA,
+			       &status, 2);
+	if (ret < 0)
+		return ret;
+
+	return status;
+}
+
+static int ltr501_read_intr_prst(struct ltr501_data *data,
+				 enum iio_chan_type type,
+				 int *val2)
+{
+	int ret, samp_period, prst;
+
+	switch (type) {
+	case IIO_INTENSITY:
+		ret = regmap_field_read(data->reg_als_prst, &prst);
+		if (ret < 0)
+			return ret;
+
+		ret = ltr501_als_read_samp_period(data, &samp_period);
+
+		if (ret < 0)
+			return ret;
+		*val2 = samp_period * prst;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_PROXIMITY:
+		ret = regmap_field_read(data->reg_ps_prst, &prst);
+		if (ret < 0)
+			return ret;
+
+		ret = ltr501_ps_read_samp_period(data, &samp_period);
+
+		if (ret < 0)
+			return ret;
+
+		*val2 = samp_period * prst;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
 }
 
-#define LTR501_INTENSITY_CHANNEL(_idx, _addr, _mod, _shared) { \
+static int ltr501_write_intr_prst(struct ltr501_data *data,
+				  enum iio_chan_type type,
+				  int val, int val2)
+{
+	int ret, samp_period, new_val;
+	unsigned long period;
+
+	if (val < 0 || val2 < 0)
+		return -EINVAL;
+
+	/* period in microseconds */
+	period = ((val * 1000000) + val2);
+
+	switch (type) {
+	case IIO_INTENSITY:
+		ret = ltr501_als_read_samp_period(data, &samp_period);
+		if (ret < 0)
+			return ret;
+
+		/* period should be atleast equal to sampling period */
+		if (period < samp_period)
+			return -EINVAL;
+
+		new_val = DIV_ROUND_UP(period, samp_period);
+		if (new_val < 0 || new_val > 0x0f)
+			return -EINVAL;
+
+		mutex_lock(&data->lock_als);
+		ret = regmap_field_write(data->reg_als_prst, new_val);
+		mutex_unlock(&data->lock_als);
+		if (ret >= 0)
+			data->als_period = period;
+
+		return ret;
+	case IIO_PROXIMITY:
+		ret = ltr501_ps_read_samp_period(data, &samp_period);
+		if (ret < 0)
+			return ret;
+
+		/* period should be atleast equal to rate */
+		if (period < samp_period)
+			return -EINVAL;
+
+		new_val = DIV_ROUND_UP(period, samp_period);
+		if (new_val < 0 || new_val > 0x0f)
+			return -EINVAL;
+
+		mutex_lock(&data->lock_ps);
+		ret = regmap_field_write(data->reg_ps_prst, new_val);
+		mutex_unlock(&data->lock_ps);
+		if (ret >= 0)
+			data->ps_period = period;
+
+		return ret;
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_event_spec ltr501_als_event_spec[] = {
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE),
+	}, {
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE),
+	}, {
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_EITHER,
+		.mask_separate = BIT(IIO_EV_INFO_ENABLE) |
+				 BIT(IIO_EV_INFO_PERIOD),
+	},
+
+};
+
+static const struct iio_event_spec ltr501_pxs_event_spec[] = {
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE),
+	}, {
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE),
+	}, {
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_EITHER,
+		.mask_separate = BIT(IIO_EV_INFO_ENABLE) |
+				 BIT(IIO_EV_INFO_PERIOD),
+	},
+};
+
+#define LTR501_INTENSITY_CHANNEL(_idx, _addr, _mod, _shared, \
+				 _evspec, _evsize) { \
 	.type = IIO_INTENSITY, \
 	.modified = 1, \
 	.address = (_addr), \
@@ -101,13 +569,27 @@ static int ltr501_read_ps(struct ltr501_data *data)
 		.realbits = 16, \
 		.storagebits = 16, \
 		.endianness = IIO_CPU, \
-	} \
+	}, \
+	.event_spec = _evspec,\
+	.num_event_specs = _evsize,\
+}
+
+#define LTR501_LIGHT_CHANNEL() { \
+	.type = IIO_LIGHT, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), \
+	.scan_index = -1, \
 }
 
 static const struct iio_chan_spec ltr501_channels[] = {
-	LTR501_INTENSITY_CHANNEL(0, LTR501_ALS_DATA0, IIO_MOD_LIGHT_BOTH, 0),
+	LTR501_LIGHT_CHANNEL(),
+	LTR501_INTENSITY_CHANNEL(0, LTR501_ALS_DATA0, IIO_MOD_LIGHT_BOTH, 0,
+				 ltr501_als_event_spec,
+				 ARRAY_SIZE(ltr501_als_event_spec)),
 	LTR501_INTENSITY_CHANNEL(1, LTR501_ALS_DATA1, IIO_MOD_LIGHT_IR,
-		BIT(IIO_CHAN_INFO_SCALE)),
+				 BIT(IIO_CHAN_INFO_SCALE) |
+				 BIT(IIO_CHAN_INFO_INT_TIME) |
+				 BIT(IIO_CHAN_INFO_SAMP_FREQ),
+				 NULL, 0),
 	{
 		.type = IIO_PROXIMITY,
 		.address = LTR501_PS_DATA,
@@ -120,23 +602,51 @@ static const struct iio_chan_spec ltr501_channels[] = {
 			.storagebits = 16,
 			.endianness = IIO_CPU,
 		},
+		.event_spec = ltr501_pxs_event_spec,
+		.num_event_specs = ARRAY_SIZE(ltr501_pxs_event_spec),
 	},
 	IIO_CHAN_SOFT_TIMESTAMP(3),
 };
 
-static const int ltr501_ps_gain[4][2] = {
-	{1, 0}, {0, 250000}, {0, 125000}, {0, 62500}
+static const struct iio_chan_spec ltr301_channels[] = {
+	LTR501_LIGHT_CHANNEL(),
+	LTR501_INTENSITY_CHANNEL(0, LTR501_ALS_DATA0, IIO_MOD_LIGHT_BOTH, 0,
+				 ltr501_als_event_spec,
+				 ARRAY_SIZE(ltr501_als_event_spec)),
+	LTR501_INTENSITY_CHANNEL(1, LTR501_ALS_DATA1, IIO_MOD_LIGHT_IR,
+				 BIT(IIO_CHAN_INFO_SCALE) |
+				 BIT(IIO_CHAN_INFO_INT_TIME) |
+				 BIT(IIO_CHAN_INFO_SAMP_FREQ),
+				 NULL, 0),
+	IIO_CHAN_SOFT_TIMESTAMP(2),
 };
 
 static int ltr501_read_raw(struct iio_dev *indio_dev,
-				struct iio_chan_spec const *chan,
-				int *val, int *val2, long mask)
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
 {
 	struct ltr501_data *data = iio_priv(indio_dev);
 	__le16 buf[2];
 	int ret, i;
 
 	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
+		if (iio_buffer_enabled(indio_dev))
+			return -EBUSY;
+
+		switch (chan->type) {
+		case IIO_LIGHT:
+			mutex_lock(&data->lock_als);
+			ret = ltr501_read_als(data, buf);
+			mutex_unlock(&data->lock_als);
+			if (ret < 0)
+				return ret;
+			*val = ltr501_calculate_lux(le16_to_cpu(buf[1]),
+						    le16_to_cpu(buf[0]));
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
 	case IIO_CHAN_INFO_RAW:
 		if (iio_buffer_enabled(indio_dev))
 			return -EBUSY;
@@ -149,7 +659,7 @@ static int ltr501_read_raw(struct iio_dev *indio_dev,
 			if (ret < 0)
 				return ret;
 			*val = le16_to_cpu(chan->address == LTR501_ALS_DATA1 ?
-				buf[0] : buf[1]);
+					   buf[0] : buf[1]);
 			return IIO_VAL_INT;
 		case IIO_PROXIMITY:
 			mutex_lock(&data->lock_ps);
@@ -165,45 +675,59 @@ static int ltr501_read_raw(struct iio_dev *indio_dev,
 	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_INTENSITY:
-			if (data->als_contr & LTR501_CONTR_ALS_GAIN_MASK) {
-				*val = 0;
-				*val2 = 5000;
-				return IIO_VAL_INT_PLUS_MICRO;
-			} else {
-				*val = 1;
-				*val2 = 0;
-				return IIO_VAL_INT;
-			}
+			i = (data->als_contr & data->chip_info->als_gain_mask)
+			     >> data->chip_info->als_gain_shift;
+			*val = data->chip_info->als_gain[i].scale;
+			*val2 = data->chip_info->als_gain[i].uscale;
+			return IIO_VAL_INT_PLUS_MICRO;
 		case IIO_PROXIMITY:
 			i = (data->ps_contr & LTR501_CONTR_PS_GAIN_MASK) >>
 				LTR501_CONTR_PS_GAIN_SHIFT;
-			*val = ltr501_ps_gain[i][0];
-			*val2 = ltr501_ps_gain[i][1];
+			*val = data->chip_info->ps_gain[i].scale;
+			*val2 = data->chip_info->ps_gain[i].uscale;
 			return IIO_VAL_INT_PLUS_MICRO;
 		default:
 			return -EINVAL;
 		}
+	case IIO_CHAN_INFO_INT_TIME:
+		switch (chan->type) {
+		case IIO_INTENSITY:
+			return ltr501_read_it_time(data, val, val2);
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		switch (chan->type) {
+		case IIO_INTENSITY:
+			return ltr501_als_read_samp_freq(data, val, val2);
+		case IIO_PROXIMITY:
+			return ltr501_ps_read_samp_freq(data, val, val2);
+		default:
+			return -EINVAL;
+		}
 	}
 	return -EINVAL;
 }
 
-static int ltr501_get_ps_gain_index(int val, int val2)
+static int ltr501_get_gain_index(struct ltr501_gain *gain, int size,
+				 int val, int val2)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(ltr501_ps_gain); i++)
-		if (val == ltr501_ps_gain[i][0] && val2 == ltr501_ps_gain[i][1])
+	for (i = 0; i < size; i++)
+		if (val == gain[i].scale && val2 == gain[i].uscale)
 			return i;
 
 	return -1;
 }
 
 static int ltr501_write_raw(struct iio_dev *indio_dev,
-			       struct iio_chan_spec const *chan,
-			       int val, int val2, long mask)
+			    struct iio_chan_spec const *chan,
+			    int val, int val2, long mask)
 {
 	struct ltr501_data *data = iio_priv(indio_dev);
-	int i;
+	int i, ret, freq_val, freq_val2;
+	struct ltr501_chip_info *info = data->chip_info;
 
 	if (iio_buffer_enabled(indio_dev))
 		return -EBUSY;
@@ -212,35 +736,382 @@ static int ltr501_write_raw(struct iio_dev *indio_dev,
 	case IIO_CHAN_INFO_SCALE:
 		switch (chan->type) {
 		case IIO_INTENSITY:
-			if (val == 0 && val2 == 5000)
-				data->als_contr |= LTR501_CONTR_ALS_GAIN_MASK;
-			else if (val == 1 && val2 == 0)
-				data->als_contr &= ~LTR501_CONTR_ALS_GAIN_MASK;
-			else
+			i = ltr501_get_gain_index(info->als_gain,
+						  info->als_gain_tbl_size,
+						  val, val2);
+			if (i < 0)
 				return -EINVAL;
-			return i2c_smbus_write_byte_data(data->client,
-				LTR501_ALS_CONTR, data->als_contr);
+
+			data->als_contr &= ~info->als_gain_mask;
+			data->als_contr |= i << info->als_gain_shift;
+
+			return regmap_write(data->regmap, LTR501_ALS_CONTR,
+					    data->als_contr);
 		case IIO_PROXIMITY:
-			i = ltr501_get_ps_gain_index(val, val2);
+			i = ltr501_get_gain_index(info->ps_gain,
+						  info->ps_gain_tbl_size,
+						  val, val2);
 			if (i < 0)
 				return -EINVAL;
 			data->ps_contr &= ~LTR501_CONTR_PS_GAIN_MASK;
 			data->ps_contr |= i << LTR501_CONTR_PS_GAIN_SHIFT;
-			return i2c_smbus_write_byte_data(data->client,
-				LTR501_PS_CONTR, data->ps_contr);
+
+			return regmap_write(data->regmap, LTR501_PS_CONTR,
+					    data->ps_contr);
 		default:
 			return -EINVAL;
 		}
+	case IIO_CHAN_INFO_INT_TIME:
+		switch (chan->type) {
+		case IIO_INTENSITY:
+			if (val != 0)
+				return -EINVAL;
+			mutex_lock(&data->lock_als);
+			i = ltr501_set_it_time(data, val2);
+			mutex_unlock(&data->lock_als);
+			return i;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		switch (chan->type) {
+		case IIO_INTENSITY:
+			ret = ltr501_als_read_samp_freq(data, &freq_val,
+							&freq_val2);
+			if (ret < 0)
+				return ret;
+
+			ret = ltr501_als_write_samp_freq(data, val, val2);
+			if (ret < 0)
+				return ret;
+
+			/* update persistence count when changing frequency */
+			ret = ltr501_write_intr_prst(data, chan->type,
+						     0, data->als_period);
+
+			if (ret < 0)
+				return ltr501_als_write_samp_freq(data,
+								  freq_val,
+								  freq_val2);
+			return ret;
+		case IIO_PROXIMITY:
+			ret = ltr501_ps_read_samp_freq(data, &freq_val,
+						       &freq_val2);
+			if (ret < 0)
+				return ret;
+
+			ret = ltr501_ps_write_samp_freq(data, val, val2);
+			if (ret < 0)
+				return ret;
+
+			/* update persistence count when changing frequency */
+			ret = ltr501_write_intr_prst(data, chan->type,
+						     0, data->ps_period);
+
+			if (ret < 0)
+				return ltr501_ps_write_samp_freq(data,
+								 freq_val,
+								 freq_val2);
+			return ret;
+		default:
+			return -EINVAL;
+		}
+	}
+	return -EINVAL;
+}
+
+static int ltr501_read_thresh(struct iio_dev *indio_dev,
+			      const struct iio_chan_spec *chan,
+			      enum iio_event_type type,
+			      enum iio_event_direction dir,
+			      enum iio_event_info info,
+			      int *val, int *val2)
+{
+	struct ltr501_data *data = iio_priv(indio_dev);
+	int ret, thresh_data;
+
+	switch (chan->type) {
+	case IIO_INTENSITY:
+		switch (dir) {
+		case IIO_EV_DIR_RISING:
+			ret = regmap_bulk_read(data->regmap,
+					       LTR501_ALS_THRESH_UP,
+					       &thresh_data, 2);
+			if (ret < 0)
+				return ret;
+			*val = thresh_data & LTR501_ALS_THRESH_MASK;
+			return IIO_VAL_INT;
+		case IIO_EV_DIR_FALLING:
+			ret = regmap_bulk_read(data->regmap,
+					       LTR501_ALS_THRESH_LOW,
+					       &thresh_data, 2);
+			if (ret < 0)
+				return ret;
+			*val = thresh_data & LTR501_ALS_THRESH_MASK;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_PROXIMITY:
+		switch (dir) {
+		case IIO_EV_DIR_RISING:
+			ret = regmap_bulk_read(data->regmap,
+					       LTR501_PS_THRESH_UP,
+					       &thresh_data, 2);
+			if (ret < 0)
+				return ret;
+			*val = thresh_data & LTR501_PS_THRESH_MASK;
+			return IIO_VAL_INT;
+		case IIO_EV_DIR_FALLING:
+			ret = regmap_bulk_read(data->regmap,
+					       LTR501_PS_THRESH_LOW,
+					       &thresh_data, 2);
+			if (ret < 0)
+				return ret;
+			*val = thresh_data & LTR501_PS_THRESH_MASK;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
 	}
+
 	return -EINVAL;
 }
 
-static IIO_CONST_ATTR(in_proximity_scale_available, "1 0.25 0.125 0.0625");
-static IIO_CONST_ATTR(in_intensity_scale_available, "1 0.005");
+static int ltr501_write_thresh(struct iio_dev *indio_dev,
+			       const struct iio_chan_spec *chan,
+			       enum iio_event_type type,
+			       enum iio_event_direction dir,
+			       enum iio_event_info info,
+			       int val, int val2)
+{
+	struct ltr501_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (val < 0)
+		return -EINVAL;
+
+	switch (chan->type) {
+	case IIO_INTENSITY:
+		if (val > LTR501_ALS_THRESH_MASK)
+			return -EINVAL;
+		switch (dir) {
+		case IIO_EV_DIR_RISING:
+			mutex_lock(&data->lock_als);
+			ret = regmap_bulk_write(data->regmap,
+						LTR501_ALS_THRESH_UP,
+						&val, 2);
+			mutex_unlock(&data->lock_als);
+			return ret;
+		case IIO_EV_DIR_FALLING:
+			mutex_lock(&data->lock_als);
+			ret = regmap_bulk_write(data->regmap,
+						LTR501_ALS_THRESH_LOW,
+						&val, 2);
+			mutex_unlock(&data->lock_als);
+			return ret;
+		default:
+			return -EINVAL;
+		}
+	case IIO_PROXIMITY:
+		if (val > LTR501_PS_THRESH_MASK)
+			return -EINVAL;
+		switch (dir) {
+		case IIO_EV_DIR_RISING:
+			mutex_lock(&data->lock_ps);
+			ret = regmap_bulk_write(data->regmap,
+						LTR501_PS_THRESH_UP,
+						&val, 2);
+			mutex_unlock(&data->lock_ps);
+			return ret;
+		case IIO_EV_DIR_FALLING:
+			mutex_lock(&data->lock_ps);
+			ret = regmap_bulk_write(data->regmap,
+						LTR501_PS_THRESH_LOW,
+						&val, 2);
+			mutex_unlock(&data->lock_ps);
+			return ret;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static int ltr501_read_event(struct iio_dev *indio_dev,
+			     const struct iio_chan_spec *chan,
+			     enum iio_event_type type,
+			     enum iio_event_direction dir,
+			     enum iio_event_info info,
+			     int *val, int *val2)
+{
+	int ret;
+
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		return ltr501_read_thresh(indio_dev, chan, type, dir,
+					  info, val, val2);
+	case IIO_EV_INFO_PERIOD:
+		ret = ltr501_read_intr_prst(iio_priv(indio_dev),
+					    chan->type, val2);
+		*val = *val2 / 1000000;
+		*val2 = *val2 % 1000000;
+		return ret;
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static int ltr501_write_event(struct iio_dev *indio_dev,
+			      const struct iio_chan_spec *chan,
+			      enum iio_event_type type,
+			      enum iio_event_direction dir,
+			      enum iio_event_info info,
+			      int val, int val2)
+{
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		if (val2 != 0)
+			return -EINVAL;
+		return ltr501_write_thresh(indio_dev, chan, type, dir,
+					   info, val, val2);
+	case IIO_EV_INFO_PERIOD:
+		return ltr501_write_intr_prst(iio_priv(indio_dev), chan->type,
+					      val, val2);
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static int ltr501_read_event_config(struct iio_dev *indio_dev,
+				    const struct iio_chan_spec *chan,
+				    enum iio_event_type type,
+				    enum iio_event_direction dir)
+{
+	struct ltr501_data *data = iio_priv(indio_dev);
+	int ret, status;
+
+	switch (chan->type) {
+	case IIO_INTENSITY:
+		ret = regmap_field_read(data->reg_als_intr, &status);
+		if (ret < 0)
+			return ret;
+		return status;
+	case IIO_PROXIMITY:
+		ret = regmap_field_read(data->reg_ps_intr, &status);
+		if (ret < 0)
+			return ret;
+		return status;
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static int ltr501_write_event_config(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan,
+				     enum iio_event_type type,
+				     enum iio_event_direction dir, int state)
+{
+	struct ltr501_data *data = iio_priv(indio_dev);
+	int ret;
+
+	/* only 1 and 0 are valid inputs */
+	if (state != 1  && state != 0)
+		return -EINVAL;
+
+	switch (chan->type) {
+	case IIO_INTENSITY:
+		mutex_lock(&data->lock_als);
+		ret = regmap_field_write(data->reg_als_intr, state);
+		mutex_unlock(&data->lock_als);
+		return ret;
+	case IIO_PROXIMITY:
+		mutex_lock(&data->lock_ps);
+		ret = regmap_field_write(data->reg_ps_intr, state);
+		mutex_unlock(&data->lock_ps);
+		return ret;
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static ssize_t ltr501_show_proximity_scale_avail(struct device *dev,
+						 struct device_attribute *attr,
+						 char *buf)
+{
+	struct ltr501_data *data = iio_priv(dev_to_iio_dev(dev));
+	struct ltr501_chip_info *info = data->chip_info;
+	ssize_t len = 0;
+	int i;
+
+	for (i = 0; i < info->ps_gain_tbl_size; i++) {
+		if (info->ps_gain[i].scale == LTR501_RESERVED_GAIN)
+			continue;
+		len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ",
+				 info->ps_gain[i].scale,
+				 info->ps_gain[i].uscale);
+	}
+
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static ssize_t ltr501_show_intensity_scale_avail(struct device *dev,
+						 struct device_attribute *attr,
+						 char *buf)
+{
+	struct ltr501_data *data = iio_priv(dev_to_iio_dev(dev));
+	struct ltr501_chip_info *info = data->chip_info;
+	ssize_t len = 0;
+	int i;
+
+	for (i = 0; i < info->als_gain_tbl_size; i++) {
+		if (info->als_gain[i].scale == LTR501_RESERVED_GAIN)
+			continue;
+		len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%06d ",
+				 info->als_gain[i].scale,
+				 info->als_gain[i].uscale);
+	}
+
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static IIO_CONST_ATTR_INT_TIME_AVAIL("0.05 0.1 0.2 0.4");
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("20 10 5 2 1 0.5");
+
+static IIO_DEVICE_ATTR(in_proximity_scale_available, S_IRUGO,
+		       ltr501_show_proximity_scale_avail, NULL, 0);
+static IIO_DEVICE_ATTR(in_intensity_scale_available, S_IRUGO,
+		       ltr501_show_intensity_scale_avail, NULL, 0);
 
 static struct attribute *ltr501_attributes[] = {
-	&iio_const_attr_in_proximity_scale_available.dev_attr.attr,
-	&iio_const_attr_in_intensity_scale_available.dev_attr.attr,
+	&iio_dev_attr_in_proximity_scale_available.dev_attr.attr,
+	&iio_dev_attr_in_intensity_scale_available.dev_attr.attr,
+	&iio_const_attr_integration_time_available.dev_attr.attr,
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	NULL
+};
+
+static struct attribute *ltr301_attributes[] = {
+	&iio_dev_attr_in_intensity_scale_available.dev_attr.attr,
+	&iio_const_attr_integration_time_available.dev_attr.attr,
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
 	NULL
 };
 
@@ -248,20 +1119,98 @@ static const struct attribute_group ltr501_attribute_group = {
 	.attrs = ltr501_attributes,
 };
 
+static const struct attribute_group ltr301_attribute_group = {
+	.attrs = ltr301_attributes,
+};
+
+static const struct iio_info ltr501_info_no_irq = {
+	.read_raw = ltr501_read_raw,
+	.write_raw = ltr501_write_raw,
+	.attrs = &ltr501_attribute_group,
+	.driver_module = THIS_MODULE,
+};
+
 static const struct iio_info ltr501_info = {
 	.read_raw = ltr501_read_raw,
 	.write_raw = ltr501_write_raw,
 	.attrs = &ltr501_attribute_group,
+	.read_event_value	= &ltr501_read_event,
+	.write_event_value	= &ltr501_write_event,
+	.read_event_config	= &ltr501_read_event_config,
+	.write_event_config	= &ltr501_write_event_config,
+	.driver_module = THIS_MODULE,
+};
+
+static const struct iio_info ltr301_info_no_irq = {
+	.read_raw = ltr501_read_raw,
+	.write_raw = ltr501_write_raw,
+	.attrs = &ltr301_attribute_group,
 	.driver_module = THIS_MODULE,
 };
 
-static int ltr501_write_contr(struct i2c_client *client, u8 als_val, u8 ps_val)
+static const struct iio_info ltr301_info = {
+	.read_raw = ltr501_read_raw,
+	.write_raw = ltr501_write_raw,
+	.attrs = &ltr301_attribute_group,
+	.read_event_value	= &ltr501_read_event,
+	.write_event_value	= &ltr501_write_event,
+	.read_event_config	= &ltr501_read_event_config,
+	.write_event_config	= &ltr501_write_event_config,
+	.driver_module = THIS_MODULE,
+};
+
+static struct ltr501_chip_info ltr501_chip_info_tbl[] = {
+	[ltr501] = {
+		.partid = 0x08,
+		.als_gain = ltr501_als_gain_tbl,
+		.als_gain_tbl_size = ARRAY_SIZE(ltr501_als_gain_tbl),
+		.ps_gain = ltr501_ps_gain_tbl,
+		.ps_gain_tbl_size = ARRAY_SIZE(ltr501_ps_gain_tbl),
+		.als_mode_active = BIT(0) | BIT(1),
+		.als_gain_mask = BIT(3),
+		.als_gain_shift = 3,
+		.info = &ltr501_info,
+		.info_no_irq = &ltr501_info_no_irq,
+		.channels = ltr501_channels,
+		.no_channels = ARRAY_SIZE(ltr501_channels),
+	},
+	[ltr559] = {
+		.partid = 0x09,
+		.als_gain = ltr559_als_gain_tbl,
+		.als_gain_tbl_size = ARRAY_SIZE(ltr559_als_gain_tbl),
+		.ps_gain = ltr559_ps_gain_tbl,
+		.ps_gain_tbl_size = ARRAY_SIZE(ltr559_ps_gain_tbl),
+		.als_mode_active = BIT(1),
+		.als_gain_mask = BIT(2) | BIT(3) | BIT(4),
+		.als_gain_shift = 2,
+		.info = &ltr501_info,
+		.info_no_irq = &ltr501_info_no_irq,
+		.channels = ltr501_channels,
+		.no_channels = ARRAY_SIZE(ltr501_channels),
+	},
+	[ltr301] = {
+		.partid = 0x08,
+		.als_gain = ltr501_als_gain_tbl,
+		.als_gain_tbl_size = ARRAY_SIZE(ltr501_als_gain_tbl),
+		.als_mode_active = BIT(0) | BIT(1),
+		.als_gain_mask = BIT(3),
+		.als_gain_shift = 3,
+		.info = &ltr301_info,
+		.info_no_irq = &ltr301_info_no_irq,
+		.channels = ltr301_channels,
+		.no_channels = ARRAY_SIZE(ltr301_channels),
+	},
+};
+
+static int ltr501_write_contr(struct ltr501_data *data, u8 als_val, u8 ps_val)
 {
-	int ret = i2c_smbus_write_byte_data(client, LTR501_ALS_CONTR, als_val);
+	int ret;
+
+	ret = regmap_write(data->regmap, LTR501_ALS_CONTR, als_val);
 	if (ret < 0)
 		return ret;
 
-	return i2c_smbus_write_byte_data(client, LTR501_PS_CONTR, ps_val);
+	return regmap_write(data->regmap, LTR501_PS_CONTR, ps_val);
 }
 
 static irqreturn_t ltr501_trigger_handler(int irq, void *p)
@@ -273,13 +1222,13 @@ static irqreturn_t ltr501_trigger_handler(int irq, void *p)
 	__le16 als_buf[2];
 	u8 mask = 0;
 	int j = 0;
-	int ret;
+	int ret, psdata;
 
 	memset(buf, 0, sizeof(buf));
 
 	/* figure out which data needs to be ready */
 	if (test_bit(0, indio_dev->active_scan_mask) ||
-		test_bit(1, indio_dev->active_scan_mask))
+	    test_bit(1, indio_dev->active_scan_mask))
 		mask |= LTR501_STATUS_ALS_RDY;
 	if (test_bit(2, indio_dev->active_scan_mask))
 		mask |= LTR501_STATUS_PS_RDY;
@@ -289,8 +1238,8 @@ static irqreturn_t ltr501_trigger_handler(int irq, void *p)
 		goto done;
 
 	if (mask & LTR501_STATUS_ALS_RDY) {
-		ret = i2c_smbus_read_i2c_block_data(data->client,
-			LTR501_ALS_DATA1, sizeof(als_buf), (u8 *) als_buf);
+		ret = regmap_bulk_read(data->regmap, LTR501_ALS_DATA1,
+				       (u8 *)als_buf, sizeof(als_buf));
 		if (ret < 0)
 			return ret;
 		if (test_bit(0, indio_dev->active_scan_mask))
@@ -300,14 +1249,14 @@ static irqreturn_t ltr501_trigger_handler(int irq, void *p)
 	}
 
 	if (mask & LTR501_STATUS_PS_RDY) {
-		ret = i2c_smbus_read_word_data(data->client, LTR501_PS_DATA);
+		ret = regmap_bulk_read(data->regmap, LTR501_PS_DATA,
+				       &psdata, 2);
 		if (ret < 0)
 			goto done;
-		buf[j++] = ret & LTR501_PS_DATA_MASK;
+		buf[j++] = psdata & LTR501_PS_DATA_MASK;
 	}
 
-	iio_push_to_buffers_with_timestamp(indio_dev, buf,
-		iio_get_time_ns());
+	iio_push_to_buffers_with_timestamp(indio_dev, buf, iio_get_time_ns());
 
 done:
 	iio_trigger_notify_done(indio_dev->trig);
@@ -315,67 +1264,225 @@ done:
 	return IRQ_HANDLED;
 }
 
+static irqreturn_t ltr501_interrupt_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct ltr501_data *data = iio_priv(indio_dev);
+	int ret, status;
+
+	ret = regmap_read(data->regmap, LTR501_ALS_PS_STATUS, &status);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"irq read int reg failed\n");
+		return IRQ_HANDLED;
+	}
+
+	if (status & LTR501_STATUS_ALS_INTR)
+		iio_push_event(indio_dev,
+			       IIO_UNMOD_EVENT_CODE(IIO_INTENSITY, 0,
+						    IIO_EV_TYPE_THRESH,
+						    IIO_EV_DIR_EITHER),
+			       iio_get_time_ns());
+
+	if (status & LTR501_STATUS_PS_INTR)
+		iio_push_event(indio_dev,
+			       IIO_UNMOD_EVENT_CODE(IIO_PROXIMITY, 0,
+						    IIO_EV_TYPE_THRESH,
+						    IIO_EV_DIR_EITHER),
+			       iio_get_time_ns());
+
+	return IRQ_HANDLED;
+}
+
 static int ltr501_init(struct ltr501_data *data)
 {
-	int ret;
+	int ret, status;
 
-	ret = i2c_smbus_read_byte_data(data->client, LTR501_ALS_CONTR);
+	ret = regmap_read(data->regmap, LTR501_ALS_CONTR, &status);
 	if (ret < 0)
 		return ret;
-	data->als_contr = ret | LTR501_CONTR_ACTIVE;
 
-	ret = i2c_smbus_read_byte_data(data->client, LTR501_PS_CONTR);
+	data->als_contr = ret | data->chip_info->als_mode_active;
+
+	ret = regmap_read(data->regmap, LTR501_PS_CONTR, &status);
 	if (ret < 0)
 		return ret;
-	data->ps_contr = ret | LTR501_CONTR_ACTIVE;
 
-	return ltr501_write_contr(data->client, data->als_contr,
-		data->ps_contr);
+	data->ps_contr = status | LTR501_CONTR_ACTIVE;
+
+	ret = ltr501_read_intr_prst(data, IIO_INTENSITY, &data->als_period);
+	if (ret < 0)
+		return ret;
+
+	ret = ltr501_read_intr_prst(data, IIO_PROXIMITY, &data->ps_period);
+	if (ret < 0)
+		return ret;
+
+	return ltr501_write_contr(data, data->als_contr, data->ps_contr);
+}
+
+static bool ltr501_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case LTR501_ALS_DATA1:
+	case LTR501_ALS_DATA0:
+	case LTR501_ALS_PS_STATUS:
+	case LTR501_PS_DATA:
+		return true;
+	default:
+		return false;
+	}
 }
 
+static struct regmap_config ltr501_regmap_config = {
+	.name =  LTR501_REGMAP_NAME,
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = LTR501_MAX_REG,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = ltr501_is_volatile_reg,
+};
+
 static int ltr501_powerdown(struct ltr501_data *data)
 {
-	return ltr501_write_contr(data->client,
-				  data->als_contr & ~LTR501_CONTR_ACTIVE,
+	return ltr501_write_contr(data, data->als_contr &
+				  ~data->chip_info->als_mode_active,
 				  data->ps_contr & ~LTR501_CONTR_ACTIVE);
 }
 
+static const char *ltr501_match_acpi_device(struct device *dev, int *chip_idx)
+{
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return NULL;
+	*chip_idx = id->driver_data;
+	return dev_name(dev);
+}
+
 static int ltr501_probe(struct i2c_client *client,
-			  const struct i2c_device_id *id)
+			const struct i2c_device_id *id)
 {
 	struct ltr501_data *data;
 	struct iio_dev *indio_dev;
-	int ret;
+	struct regmap *regmap;
+	int ret, partid, chip_idx = 0;
+	const char *name = NULL;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
 	if (!indio_dev)
 		return -ENOMEM;
 
+	regmap = devm_regmap_init_i2c(client, &ltr501_regmap_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&client->dev, "Regmap initialization failed.\n");
+		return PTR_ERR(regmap);
+	}
+
 	data = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
+	data->regmap = regmap;
 	mutex_init(&data->lock_als);
 	mutex_init(&data->lock_ps);
 
-	ret = i2c_smbus_read_byte_data(data->client, LTR501_PART_ID);
+	data->reg_it = devm_regmap_field_alloc(&client->dev, regmap,
+					       reg_field_it);
+	if (IS_ERR(data->reg_it)) {
+		dev_err(&client->dev, "Integ time reg field init failed.\n");
+		return PTR_ERR(data->reg_it);
+	}
+
+	data->reg_als_intr = devm_regmap_field_alloc(&client->dev, regmap,
+						     reg_field_als_intr);
+	if (IS_ERR(data->reg_als_intr)) {
+		dev_err(&client->dev, "ALS intr mode reg field init failed\n");
+		return PTR_ERR(data->reg_als_intr);
+	}
+
+	data->reg_ps_intr = devm_regmap_field_alloc(&client->dev, regmap,
+						    reg_field_ps_intr);
+	if (IS_ERR(data->reg_ps_intr)) {
+		dev_err(&client->dev, "PS intr mode reg field init failed.\n");
+		return PTR_ERR(data->reg_ps_intr);
+	}
+
+	data->reg_als_rate = devm_regmap_field_alloc(&client->dev, regmap,
+						     reg_field_als_rate);
+	if (IS_ERR(data->reg_als_rate)) {
+		dev_err(&client->dev, "ALS samp rate field init failed.\n");
+		return PTR_ERR(data->reg_als_rate);
+	}
+
+	data->reg_ps_rate = devm_regmap_field_alloc(&client->dev, regmap,
+						    reg_field_ps_rate);
+	if (IS_ERR(data->reg_ps_rate)) {
+		dev_err(&client->dev, "PS samp rate field init failed.\n");
+		return PTR_ERR(data->reg_ps_rate);
+	}
+
+	data->reg_als_prst = devm_regmap_field_alloc(&client->dev, regmap,
+						     reg_field_als_prst);
+	if (IS_ERR(data->reg_als_prst)) {
+		dev_err(&client->dev, "ALS prst reg field init failed\n");
+		return PTR_ERR(data->reg_als_prst);
+	}
+
+	data->reg_ps_prst = devm_regmap_field_alloc(&client->dev, regmap,
+						    reg_field_ps_prst);
+	if (IS_ERR(data->reg_ps_prst)) {
+		dev_err(&client->dev, "PS prst reg field init failed.\n");
+		return PTR_ERR(data->reg_ps_prst);
+	}
+
+	ret = regmap_read(data->regmap, LTR501_PART_ID, &partid);
 	if (ret < 0)
 		return ret;
-	if ((ret >> 4) != 0x8)
+
+	if (id) {
+		name = id->name;
+		chip_idx = id->driver_data;
+	} else  if (ACPI_HANDLE(&client->dev)) {
+		name = ltr501_match_acpi_device(&client->dev, &chip_idx);
+	} else {
+		return -ENODEV;
+	}
+
+	data->chip_info = &ltr501_chip_info_tbl[chip_idx];
+
+	if ((partid >> 4) != data->chip_info->partid)
 		return -ENODEV;
 
 	indio_dev->dev.parent = &client->dev;
-	indio_dev->info = &ltr501_info;
-	indio_dev->channels = ltr501_channels;
-	indio_dev->num_channels = ARRAY_SIZE(ltr501_channels);
-	indio_dev->name = LTR501_DRV_NAME;
+	indio_dev->info = data->chip_info->info;
+	indio_dev->channels = data->chip_info->channels;
+	indio_dev->num_channels = data->chip_info->no_channels;
+	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
 	ret = ltr501_init(data);
 	if (ret < 0)
 		return ret;
 
+	if (client->irq > 0) {
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+						NULL, ltr501_interrupt_handler,
+						IRQF_TRIGGER_FALLING |
+						IRQF_ONESHOT,
+						"ltr501_thresh_event",
+						indio_dev);
+		if (ret) {
+			dev_err(&client->dev, "request irq (%d) failed\n",
+				client->irq);
+			return ret;
+		}
+	} else {
+		indio_dev->info = data->chip_info->info_no_irq;
+	}
+
 	ret = iio_triggered_buffer_setup(indio_dev, NULL,
-		ltr501_trigger_handler, NULL);
+					 ltr501_trigger_handler, NULL);
 	if (ret)
 		goto powerdown_on_error;
 
@@ -407,24 +1514,34 @@ static int ltr501_remove(struct i2c_client *client)
 static int ltr501_suspend(struct device *dev)
 {
 	struct ltr501_data *data = iio_priv(i2c_get_clientdata(
-		to_i2c_client(dev)));
+					    to_i2c_client(dev)));
 	return ltr501_powerdown(data);
 }
 
 static int ltr501_resume(struct device *dev)
 {
 	struct ltr501_data *data = iio_priv(i2c_get_clientdata(
-		to_i2c_client(dev)));
+					    to_i2c_client(dev)));
 
-	return ltr501_write_contr(data->client, data->als_contr,
+	return ltr501_write_contr(data, data->als_contr,
 		data->ps_contr);
 }
 #endif
 
 static SIMPLE_DEV_PM_OPS(ltr501_pm_ops, ltr501_suspend, ltr501_resume);
 
+static const struct acpi_device_id ltr_acpi_match[] = {
+	{"LTER0501", ltr501},
+	{"LTER0559", ltr559},
+	{"LTER0301", ltr301},
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, ltr_acpi_match);
+
 static const struct i2c_device_id ltr501_id[] = {
-	{ "ltr501", 0 },
+	{ "ltr501", ltr501},
+	{ "ltr559", ltr559},
+	{ "ltr301", ltr301},
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ltr501_id);
@@ -433,6 +1550,7 @@ static struct i2c_driver ltr501_driver = {
 	.driver = {
 		.name   = LTR501_DRV_NAME,
 		.pm	= &ltr501_pm_ops,
+		.acpi_match_table = ACPI_PTR(ltr_acpi_match),
 		.owner  = THIS_MODULE,
 	},
 	.probe  = ltr501_probe,
diff --git a/drivers/iio/light/stk3310.c b/drivers/iio/light/stk3310.c
new file mode 100644
index 000000000000..fee4297d7c8f
--- /dev/null
+++ b/drivers/iio/light/stk3310.c
@@ -0,0 +1,722 @@
+/**
+ * Sensortek STK3310/STK3311 Ambient Light and Proximity Sensor
+ *
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * IIO driver for STK3310/STK3311. 7-bit I2C address: 0x48.
+ */
+
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/gpio/consumer.h>
+#include <linux/iio/events.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define STK3310_REG_STATE			0x00
+#define STK3310_REG_PSCTRL			0x01
+#define STK3310_REG_ALSCTRL			0x02
+#define STK3310_REG_INT				0x04
+#define STK3310_REG_THDH_PS			0x06
+#define STK3310_REG_THDL_PS			0x08
+#define STK3310_REG_FLAG			0x10
+#define STK3310_REG_PS_DATA_MSB			0x11
+#define STK3310_REG_PS_DATA_LSB			0x12
+#define STK3310_REG_ALS_DATA_MSB		0x13
+#define STK3310_REG_ALS_DATA_LSB		0x14
+#define STK3310_REG_ID				0x3E
+#define STK3310_MAX_REG				0x80
+
+#define STK3310_STATE_EN_PS			0x01
+#define STK3310_STATE_EN_ALS			0x02
+#define STK3310_STATE_STANDBY			0x00
+
+#define STK3310_CHIP_ID_VAL			0x13
+#define STK3311_CHIP_ID_VAL			0x1D
+#define STK3310_PSINT_EN			0x01
+#define STK3310_PS_MAX_VAL			0xFFFF
+#define STK3310_THRESH_MAX			0xFFFF
+
+#define STK3310_DRIVER_NAME			"stk3310"
+#define STK3310_REGMAP_NAME			"stk3310_regmap"
+#define STK3310_EVENT				"stk3310_event"
+#define STK3310_GPIO				"stk3310_gpio"
+
+#define STK3310_SCALE_AVAILABLE			"6.4 1.6 0.4 0.1"
+
+#define STK3310_IT_AVAILABLE \
+	"0.000185 0.000370 0.000741 0.001480 0.002960 0.005920 0.011840 " \
+	"0.023680 0.047360 0.094720 0.189440 0.378880 0.757760 1.515520 " \
+	"3.031040 6.062080"
+
+#define STK3310_REGFIELD(name)						    \
+	do {								    \
+		data->reg_##name =					    \
+			devm_regmap_field_alloc(&client->dev, regmap,	    \
+				stk3310_reg_field_##name);		    \
+		if (IS_ERR(data->reg_##name)) {				    \
+			dev_err(&client->dev, "reg field alloc failed.\n"); \
+			return PTR_ERR(data->reg_##name);		    \
+		}							    \
+	} while (0)
+
+static const struct reg_field stk3310_reg_field_state =
+				REG_FIELD(STK3310_REG_STATE, 0, 2);
+static const struct reg_field stk3310_reg_field_als_gain =
+				REG_FIELD(STK3310_REG_ALSCTRL, 4, 5);
+static const struct reg_field stk3310_reg_field_ps_gain =
+				REG_FIELD(STK3310_REG_PSCTRL, 4, 5);
+static const struct reg_field stk3310_reg_field_als_it =
+				REG_FIELD(STK3310_REG_ALSCTRL, 0, 3);
+static const struct reg_field stk3310_reg_field_ps_it =
+				REG_FIELD(STK3310_REG_PSCTRL, 0, 3);
+static const struct reg_field stk3310_reg_field_int_ps =
+				REG_FIELD(STK3310_REG_INT, 0, 2);
+static const struct reg_field stk3310_reg_field_flag_psint =
+				REG_FIELD(STK3310_REG_FLAG, 4, 4);
+static const struct reg_field stk3310_reg_field_flag_nf =
+				REG_FIELD(STK3310_REG_FLAG, 0, 0);
+/*
+ * Maximum PS values with regard to scale. Used to export the 'inverse'
+ * PS value (high values for far objects, low values for near objects).
+ */
+static const int stk3310_ps_max[4] = {
+	STK3310_PS_MAX_VAL / 64,
+	STK3310_PS_MAX_VAL / 16,
+	STK3310_PS_MAX_VAL /  4,
+	STK3310_PS_MAX_VAL,
+};
+
+static const int stk3310_scale_table[][2] = {
+	{6, 400000}, {1, 600000}, {0, 400000}, {0, 100000}
+};
+
+/* Integration time in seconds, microseconds */
+static const int stk3310_it_table[][2] = {
+	{0, 185},	{0, 370},	{0, 741},	{0, 1480},
+	{0, 2960},	{0, 5920},	{0, 11840},	{0, 23680},
+	{0, 47360},	{0, 94720},	{0, 189440},	{0, 378880},
+	{0, 757760},	{1, 515520},	{3, 31040},	{6, 62080},
+};
+
+struct stk3310_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	bool als_enabled;
+	bool ps_enabled;
+	u64 timestamp;
+	struct regmap *regmap;
+	struct regmap_field *reg_state;
+	struct regmap_field *reg_als_gain;
+	struct regmap_field *reg_ps_gain;
+	struct regmap_field *reg_als_it;
+	struct regmap_field *reg_ps_it;
+	struct regmap_field *reg_int_ps;
+	struct regmap_field *reg_flag_psint;
+	struct regmap_field *reg_flag_nf;
+};
+
+static const struct iio_event_spec stk3310_events[] = {
+	/* Proximity event */
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+				 BIT(IIO_EV_INFO_ENABLE),
+	},
+	/* Out-of-proximity event */
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+				 BIT(IIO_EV_INFO_ENABLE),
+	},
+};
+
+static const struct iio_chan_spec stk3310_channels[] = {
+	{
+		.type = IIO_LIGHT,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_INT_TIME),
+	},
+	{
+		.type = IIO_PROXIMITY,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_INT_TIME),
+		.event_spec = stk3310_events,
+		.num_event_specs = ARRAY_SIZE(stk3310_events),
+	}
+};
+
+static IIO_CONST_ATTR(in_illuminance_scale_available, STK3310_SCALE_AVAILABLE);
+
+static IIO_CONST_ATTR(in_proximity_scale_available, STK3310_SCALE_AVAILABLE);
+
+static IIO_CONST_ATTR(in_illuminance_integration_time_available,
+		      STK3310_IT_AVAILABLE);
+
+static IIO_CONST_ATTR(in_proximity_integration_time_available,
+		      STK3310_IT_AVAILABLE);
+
+static struct attribute *stk3310_attributes[] = {
+	&iio_const_attr_in_illuminance_scale_available.dev_attr.attr,
+	&iio_const_attr_in_proximity_scale_available.dev_attr.attr,
+	&iio_const_attr_in_illuminance_integration_time_available.dev_attr.attr,
+	&iio_const_attr_in_proximity_integration_time_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group stk3310_attribute_group = {
+	.attrs = stk3310_attributes
+};
+
+static int stk3310_get_index(const int table[][2], int table_size,
+			     int val, int val2)
+{
+	int i;
+
+	for (i = 0; i < table_size; i++) {
+		if (val == table[i][0] && val2 == table[i][1])
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int stk3310_read_event(struct iio_dev *indio_dev,
+			      const struct iio_chan_spec *chan,
+			      enum iio_event_type type,
+			      enum iio_event_direction dir,
+			      enum iio_event_info info,
+			      int *val, int *val2)
+{
+	u8 reg;
+	u16 buf;
+	int ret;
+	unsigned int index;
+	struct stk3310_data *data = iio_priv(indio_dev);
+
+	if (info != IIO_EV_INFO_VALUE)
+		return -EINVAL;
+
+	/*
+	 * Only proximity interrupts are implemented at the moment.
+	 * Since we're inverting proximity values, the sensor's 'high'
+	 * threshold will become our 'low' threshold, associated with
+	 * 'near' events. Similarly, the sensor's 'low' threshold will
+	 * be our 'high' threshold, associated with 'far' events.
+	 */
+	if (dir == IIO_EV_DIR_RISING)
+		reg = STK3310_REG_THDL_PS;
+	else if (dir == IIO_EV_DIR_FALLING)
+		reg = STK3310_REG_THDH_PS;
+	else
+		return -EINVAL;
+
+	mutex_lock(&data->lock);
+	ret = regmap_bulk_read(data->regmap, reg, &buf, 2);
+	mutex_unlock(&data->lock);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "register read failed\n");
+		return ret;
+	}
+	regmap_field_read(data->reg_ps_gain, &index);
+	*val = swab16(stk3310_ps_max[index] - buf);
+
+	return IIO_VAL_INT;
+}
+
+static int stk3310_write_event(struct iio_dev *indio_dev,
+			       const struct iio_chan_spec *chan,
+			       enum iio_event_type type,
+			       enum iio_event_direction dir,
+			       enum iio_event_info info,
+			       int val, int val2)
+{
+	u8 reg;
+	u16 buf;
+	int ret;
+	unsigned int index;
+	struct stk3310_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+
+	regmap_field_read(data->reg_ps_gain, &index);
+	if (val > stk3310_ps_max[index])
+		return -EINVAL;
+
+	if (dir == IIO_EV_DIR_RISING)
+		reg = STK3310_REG_THDL_PS;
+	else if (dir == IIO_EV_DIR_FALLING)
+		reg = STK3310_REG_THDH_PS;
+	else
+		return -EINVAL;
+
+	buf = swab16(stk3310_ps_max[index] - val);
+	ret = regmap_bulk_write(data->regmap, reg, &buf, 2);
+	if (ret < 0)
+		dev_err(&client->dev, "failed to set PS threshold!\n");
+
+	return ret;
+}
+
+static int stk3310_read_event_config(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan,
+				     enum iio_event_type type,
+				     enum iio_event_direction dir)
+{
+	unsigned int event_val;
+	struct stk3310_data *data = iio_priv(indio_dev);
+
+	regmap_field_read(data->reg_int_ps, &event_val);
+
+	return event_val;
+}
+
+static int stk3310_write_event_config(struct iio_dev *indio_dev,
+				      const struct iio_chan_spec *chan,
+				      enum iio_event_type type,
+				      enum iio_event_direction dir,
+				      int state)
+{
+	int ret;
+	struct stk3310_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+
+	if (state < 0 || state > 7)
+		return -EINVAL;
+
+	/* Set INT_PS value */
+	mutex_lock(&data->lock);
+	ret = regmap_field_write(data->reg_int_ps, state);
+	if (ret < 0)
+		dev_err(&client->dev, "failed to set interrupt mode\n");
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static int stk3310_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	u8 reg;
+	u16 buf;
+	int ret;
+	unsigned int index;
+	struct stk3310_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (chan->type == IIO_LIGHT)
+			reg = STK3310_REG_ALS_DATA_MSB;
+		else if (chan->type == IIO_PROXIMITY)
+			reg = STK3310_REG_PS_DATA_MSB;
+		else
+			return -EINVAL;
+		mutex_lock(&data->lock);
+		ret = regmap_bulk_read(data->regmap, reg, &buf, 2);
+		if (ret < 0) {
+			dev_err(&client->dev, "register read failed\n");
+			mutex_unlock(&data->lock);
+			return ret;
+		}
+		*val = swab16(buf);
+		if (chan->type == IIO_PROXIMITY) {
+			/*
+			 * Invert the proximity data so we return low values
+			 * for close objects and high values for far ones.
+			 */
+			regmap_field_read(data->reg_ps_gain, &index);
+			*val = stk3310_ps_max[index] - *val;
+		}
+		mutex_unlock(&data->lock);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_INT_TIME:
+		if (chan->type == IIO_LIGHT)
+			regmap_field_read(data->reg_als_it, &index);
+		else
+			regmap_field_read(data->reg_ps_it, &index);
+		*val = stk3310_it_table[index][0];
+		*val2 = stk3310_it_table[index][1];
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SCALE:
+		if (chan->type == IIO_LIGHT)
+			regmap_field_read(data->reg_als_gain, &index);
+		else
+			regmap_field_read(data->reg_ps_gain, &index);
+		*val = stk3310_scale_table[index][0];
+		*val2 = stk3310_scale_table[index][1];
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+
+	return -EINVAL;
+}
+
+static int stk3310_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	int ret;
+	int index;
+	struct stk3310_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_INT_TIME:
+		index = stk3310_get_index(stk3310_it_table,
+					  ARRAY_SIZE(stk3310_it_table),
+					  val, val2);
+		if (index < 0)
+			return -EINVAL;
+		mutex_lock(&data->lock);
+		if (chan->type == IIO_LIGHT)
+			ret = regmap_field_write(data->reg_als_it, index);
+		else
+			ret = regmap_field_write(data->reg_ps_it, index);
+		if (ret < 0)
+			dev_err(&data->client->dev,
+					"sensor configuration failed\n");
+		mutex_unlock(&data->lock);
+		return ret;
+
+	case IIO_CHAN_INFO_SCALE:
+		index = stk3310_get_index(stk3310_scale_table,
+					  ARRAY_SIZE(stk3310_scale_table),
+					  val, val2);
+		if (index < 0)
+			return -EINVAL;
+		mutex_lock(&data->lock);
+		if (chan->type == IIO_LIGHT)
+			ret = regmap_field_write(data->reg_als_gain, index);
+		else
+			ret = regmap_field_write(data->reg_ps_gain, index);
+		if (ret < 0)
+			dev_err(&data->client->dev,
+					"sensor configuration failed\n");
+		mutex_unlock(&data->lock);
+		return ret;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info stk3310_info = {
+	.driver_module		= THIS_MODULE,
+	.read_raw		= stk3310_read_raw,
+	.write_raw		= stk3310_write_raw,
+	.attrs			= &stk3310_attribute_group,
+	.read_event_value	= stk3310_read_event,
+	.write_event_value	= stk3310_write_event,
+	.read_event_config	= stk3310_read_event_config,
+	.write_event_config	= stk3310_write_event_config,
+};
+
+static int stk3310_set_state(struct stk3310_data *data, u8 state)
+{
+	int ret;
+	struct i2c_client *client = data->client;
+
+	/* 3-bit state; 0b100 is not supported. */
+	if (state > 7 || state == 4)
+		return -EINVAL;
+
+	mutex_lock(&data->lock);
+	ret = regmap_field_write(data->reg_state, state);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to change sensor state\n");
+	} else if (state != STK3310_STATE_STANDBY) {
+		/* Don't reset the 'enabled' flags if we're going in standby */
+		data->ps_enabled  = !!(state & 0x01);
+		data->als_enabled = !!(state & 0x02);
+	}
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static int stk3310_init(struct iio_dev *indio_dev)
+{
+	int ret;
+	int chipid;
+	u8 state;
+	struct stk3310_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+
+	regmap_read(data->regmap, STK3310_REG_ID, &chipid);
+	if (chipid != STK3310_CHIP_ID_VAL &&
+	    chipid != STK3311_CHIP_ID_VAL) {
+		dev_err(&client->dev, "invalid chip id: 0x%x\n", chipid);
+		return -ENODEV;
+	}
+
+	state = STK3310_STATE_EN_ALS | STK3310_STATE_EN_PS;
+	ret = stk3310_set_state(data, state);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to enable sensor");
+		return ret;
+	}
+
+	/* Enable PS interrupts */
+	ret = regmap_field_write(data->reg_int_ps, STK3310_PSINT_EN);
+	if (ret < 0)
+		dev_err(&client->dev, "failed to enable interrupts!\n");
+
+	return ret;
+}
+
+static int stk3310_gpio_probe(struct i2c_client *client)
+{
+	struct device *dev;
+	struct gpio_desc *gpio;
+	int ret;
+
+	if (!client)
+		return -EINVAL;
+
+	dev = &client->dev;
+
+	/* gpio interrupt pin */
+	gpio = devm_gpiod_get_index(dev, STK3310_GPIO, 0);
+	if (IS_ERR(gpio)) {
+		dev_err(dev, "acpi gpio get index failed\n");
+		return PTR_ERR(gpio);
+	}
+
+	ret = gpiod_direction_input(gpio);
+	if (ret)
+		return ret;
+
+	ret = gpiod_to_irq(gpio);
+	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
+
+	return ret;
+}
+
+static bool stk3310_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case STK3310_REG_ALS_DATA_MSB:
+	case STK3310_REG_ALS_DATA_LSB:
+	case STK3310_REG_PS_DATA_LSB:
+	case STK3310_REG_PS_DATA_MSB:
+	case STK3310_REG_FLAG:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static struct regmap_config stk3310_regmap_config = {
+	.name = STK3310_REGMAP_NAME,
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = STK3310_MAX_REG,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = stk3310_is_volatile_reg,
+};
+
+static int stk3310_regmap_init(struct stk3310_data *data)
+{
+	struct regmap *regmap;
+	struct i2c_client *client;
+
+	client = data->client;
+	regmap = devm_regmap_init_i2c(client, &stk3310_regmap_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&client->dev, "regmap initialization failed.\n");
+		return PTR_ERR(regmap);
+	}
+	data->regmap = regmap;
+
+	STK3310_REGFIELD(state);
+	STK3310_REGFIELD(als_gain);
+	STK3310_REGFIELD(ps_gain);
+	STK3310_REGFIELD(als_it);
+	STK3310_REGFIELD(ps_it);
+	STK3310_REGFIELD(int_ps);
+	STK3310_REGFIELD(flag_psint);
+	STK3310_REGFIELD(flag_nf);
+
+	return 0;
+}
+
+static irqreturn_t stk3310_irq_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct stk3310_data *data = iio_priv(indio_dev);
+
+	data->timestamp = iio_get_time_ns();
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t stk3310_irq_event_handler(int irq, void *private)
+{
+	int ret;
+	unsigned int dir;
+	u64 event;
+
+	struct iio_dev *indio_dev = private;
+	struct stk3310_data *data = iio_priv(indio_dev);
+
+	/* Read FLAG_NF to figure out what threshold has been met. */
+	mutex_lock(&data->lock);
+	ret = regmap_field_read(data->reg_flag_nf, &dir);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "register read failed\n");
+		mutex_unlock(&data->lock);
+		return ret;
+	}
+	event = IIO_UNMOD_EVENT_CODE(IIO_PROXIMITY, 1,
+				     IIO_EV_TYPE_THRESH,
+				     (dir ? IIO_EV_DIR_RISING :
+					    IIO_EV_DIR_FALLING));
+	iio_push_event(indio_dev, event, data->timestamp);
+
+	/* Reset the interrupt flag */
+	ret = regmap_field_write(data->reg_flag_psint, 0);
+	if (ret < 0)
+		dev_err(&data->client->dev, "failed to reset interrupts\n");
+	mutex_unlock(&data->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int stk3310_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct stk3310_data *data;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev) {
+		dev_err(&client->dev, "iio allocation failed!\n");
+		return -ENOMEM;
+	}
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	i2c_set_clientdata(client, indio_dev);
+	mutex_init(&data->lock);
+
+	ret = stk3310_regmap_init(data);
+	if (ret < 0)
+		return ret;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &stk3310_info;
+	indio_dev->name = STK3310_DRIVER_NAME;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = stk3310_channels;
+	indio_dev->num_channels = ARRAY_SIZE(stk3310_channels);
+
+	ret = stk3310_init(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "device_register failed\n");
+		stk3310_set_state(data, STK3310_STATE_STANDBY);
+	}
+
+	if (client->irq <= 0)
+		client->irq = stk3310_gpio_probe(client);
+
+	if (client->irq >= 0) {
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+						stk3310_irq_handler,
+						stk3310_irq_event_handler,
+						IRQF_TRIGGER_FALLING |
+						IRQF_ONESHOT,
+						STK3310_EVENT, indio_dev);
+		if (ret < 0)
+			dev_err(&client->dev, "request irq %d failed\n",
+					client->irq);
+	}
+
+	return ret;
+}
+
+static int stk3310_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+
+	iio_device_unregister(indio_dev);
+	return stk3310_set_state(iio_priv(indio_dev), STK3310_STATE_STANDBY);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int stk3310_suspend(struct device *dev)
+{
+	struct stk3310_data *data;
+
+	data = iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+
+	return stk3310_set_state(data, STK3310_STATE_STANDBY);
+}
+
+static int stk3310_resume(struct device *dev)
+{
+	int state = 0;
+	struct stk3310_data *data;
+
+	data = iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+	if (data->ps_enabled)
+		state |= STK3310_STATE_EN_PS;
+	if (data->als_enabled)
+		state |= STK3310_STATE_EN_ALS;
+
+	return stk3310_set_state(data, state);
+}
+
+static SIMPLE_DEV_PM_OPS(stk3310_pm_ops, stk3310_suspend, stk3310_resume);
+
+#define STK3310_PM_OPS (&stk3310_pm_ops)
+#else
+#define STK3310_PM_OPS NULL
+#endif
+
+static const struct i2c_device_id stk3310_i2c_id[] = {
+	{"STK3310", 0},
+	{"STK3311", 0},
+	{}
+};
+
+static const struct acpi_device_id stk3310_acpi_id[] = {
+	{"STK3310", 0},
+	{"STK3311", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, stk3310_acpi_id);
+
+static struct i2c_driver stk3310_driver = {
+	.driver = {
+		.name = "stk3310",
+		.pm = STK3310_PM_OPS,
+		.acpi_match_table = ACPI_PTR(stk3310_acpi_id),
+	},
+	.probe =            stk3310_probe,
+	.remove =           stk3310_remove,
+	.id_table =         stk3310_i2c_id,
+};
+
+module_i2c_driver(stk3310_driver);
+
+MODULE_AUTHOR("Tiberiu Breana <tiberiu.a.breana@intel.com>");
+MODULE_DESCRIPTION("STK3310 Ambient Light and Proximity Sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/light/tsl2563.c b/drivers/iio/light/tsl2563.c
index 94daa9fc1247..12731d6b89ec 100644
--- a/drivers/iio/light/tsl2563.c
+++ b/drivers/iio/light/tsl2563.c
@@ -240,7 +240,7 @@ static int tsl2563_read_id(struct tsl2563_chip *chip, u8 *id)
  * convert between normalized values and HW values obtained using given
  * timing and gain settings.
  */
-static int adc_shiftbits(u8 timing)
+static int tsl2563_adc_shiftbits(u8 timing)
 {
 	int shift = 0;
 
@@ -263,9 +263,9 @@ static int adc_shiftbits(u8 timing)
 }
 
 /* Convert a HW ADC value to normalized scale. */
-static u32 normalize_adc(u16 adc, u8 timing)
+static u32 tsl2563_normalize_adc(u16 adc, u8 timing)
 {
-	return adc << adc_shiftbits(timing);
+	return adc << tsl2563_adc_shiftbits(timing);
 }
 
 static void tsl2563_wait_adc(struct tsl2563_chip *chip)
@@ -350,8 +350,8 @@ static int tsl2563_get_adc(struct tsl2563_chip *chip)
 		retry = tsl2563_adjust_gainlevel(chip, adc0);
 	}
 
-	chip->data0 = normalize_adc(adc0, chip->gainlevel->gaintime);
-	chip->data1 = normalize_adc(adc1, chip->gainlevel->gaintime);
+	chip->data0 = tsl2563_normalize_adc(adc0, chip->gainlevel->gaintime);
+	chip->data1 = tsl2563_normalize_adc(adc1, chip->gainlevel->gaintime);
 
 	if (!chip->int_enabled)
 		schedule_delayed_work(&chip->poweroff_work, 5 * HZ);
@@ -361,13 +361,13 @@ out:
 	return ret;
 }
 
-static inline int calib_to_sysfs(u32 calib)
+static inline int tsl2563_calib_to_sysfs(u32 calib)
 {
 	return (int) (((calib * CALIB_BASE_SYSFS) +
 		       CALIB_FRAC_HALF) >> CALIB_FRAC_BITS);
 }
 
-static inline u32 calib_from_sysfs(int value)
+static inline u32 tsl2563_calib_from_sysfs(int value)
 {
 	return (((u32) value) << CALIB_FRAC_BITS) / CALIB_BASE_SYSFS;
 }
@@ -426,7 +426,7 @@ static const struct tsl2563_lux_coeff lux_table[] = {
 };
 
 /* Convert normalized, scaled ADC values to lux. */
-static unsigned int adc_to_lux(u32 adc0, u32 adc1)
+static unsigned int tsl2563_adc_to_lux(u32 adc0, u32 adc1)
 {
 	const struct tsl2563_lux_coeff *lp = lux_table;
 	unsigned long ratio, lux, ch0 = adc0, ch1 = adc1;
@@ -442,7 +442,7 @@ static unsigned int adc_to_lux(u32 adc0, u32 adc1)
 }
 
 /* Apply calibration coefficient to ADC count. */
-static u32 calib_adc(u32 adc, u32 calib)
+static u32 tsl2563_calib_adc(u32 adc, u32 calib)
 {
 	unsigned long scaled = adc;
 
@@ -463,9 +463,9 @@ static int tsl2563_write_raw(struct iio_dev *indio_dev,
 	if (mask != IIO_CHAN_INFO_CALIBSCALE)
 		return -EINVAL;
 	if (chan->channel2 == IIO_MOD_LIGHT_BOTH)
-		chip->calib0 = calib_from_sysfs(val);
+		chip->calib0 = tsl2563_calib_from_sysfs(val);
 	else if (chan->channel2 == IIO_MOD_LIGHT_IR)
-		chip->calib1 = calib_from_sysfs(val);
+		chip->calib1 = tsl2563_calib_from_sysfs(val);
 	else
 		return -EINVAL;
 
@@ -491,11 +491,11 @@ static int tsl2563_read_raw(struct iio_dev *indio_dev,
 			ret = tsl2563_get_adc(chip);
 			if (ret)
 				goto error_ret;
-			calib0 = calib_adc(chip->data0, chip->calib0) *
+			calib0 = tsl2563_calib_adc(chip->data0, chip->calib0) *
 				chip->cover_comp_gain;
-			calib1 = calib_adc(chip->data1, chip->calib1) *
+			calib1 = tsl2563_calib_adc(chip->data1, chip->calib1) *
 				chip->cover_comp_gain;
-			*val = adc_to_lux(calib0, calib1);
+			*val = tsl2563_adc_to_lux(calib0, calib1);
 			ret = IIO_VAL_INT;
 			break;
 		case IIO_INTENSITY:
@@ -515,9 +515,9 @@ static int tsl2563_read_raw(struct iio_dev *indio_dev,
 
 	case IIO_CHAN_INFO_CALIBSCALE:
 		if (chan->channel2 == IIO_MOD_LIGHT_BOTH)
-			*val = calib_to_sysfs(chip->calib0);
+			*val = tsl2563_calib_to_sysfs(chip->calib0);
 		else
-			*val = calib_to_sysfs(chip->calib1);
+			*val = tsl2563_calib_to_sysfs(chip->calib1);
 		ret = IIO_VAL_INT;
 		break;
 	default:
@@ -750,8 +750,8 @@ static int tsl2563_probe(struct i2c_client *client,
 	chip->high_thres = 0xffff;
 	chip->gainlevel = tsl2563_gainlevel_table;
 	chip->intr = TSL2563_INT_PERSIST(4);
-	chip->calib0 = calib_from_sysfs(CALIB_BASE_SYSFS);
-	chip->calib1 = calib_from_sysfs(CALIB_BASE_SYSFS);
+	chip->calib0 = tsl2563_calib_from_sysfs(CALIB_BASE_SYSFS);
+	chip->calib1 = tsl2563_calib_from_sysfs(CALIB_BASE_SYSFS);
 
 	if (pdata)
 		chip->cover_comp_gain = pdata->cover_comp_gain;
diff --git a/drivers/iio/light/tsl4531.c b/drivers/iio/light/tsl4531.c
index 0763b8632573..63c26e2d5d97 100644
--- a/drivers/iio/light/tsl4531.c
+++ b/drivers/iio/light/tsl4531.c
@@ -24,12 +24,12 @@
 
 #define TSL4531_DRV_NAME "tsl4531"
 
-#define TCS3472_COMMAND BIT(7)
+#define TSL4531_COMMAND BIT(7)
 
-#define TSL4531_CONTROL (TCS3472_COMMAND | 0x00)
-#define TSL4531_CONFIG (TCS3472_COMMAND | 0x01)
-#define TSL4531_DATA (TCS3472_COMMAND | 0x04)
-#define TSL4531_ID (TCS3472_COMMAND | 0x0a)
+#define TSL4531_CONTROL (TSL4531_COMMAND | 0x00)
+#define TSL4531_CONFIG (TSL4531_COMMAND | 0x01)
+#define TSL4531_DATA (TSL4531_COMMAND | 0x04)
+#define TSL4531_ID (TSL4531_COMMAND | 0x0a)
 
 /* operating modes in control register */
 #define TSL4531_MODE_POWERDOWN 0x00
diff --git a/drivers/iio/magnetometer/Kconfig b/drivers/iio/magnetometer/Kconfig
index a5d6de72c523..dcadfc4f0661 100644
--- a/drivers/iio/magnetometer/Kconfig
+++ b/drivers/iio/magnetometer/Kconfig
@@ -8,7 +8,7 @@ menu "Magnetometer sensors"
 config AK8975
 	tristate "Asahi Kasei AK 3-Axis Magnetometer"
 	depends on I2C
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Say yes here to build support for Asahi Kasei AK8975, AK8963,
 	  AK09911 or AK09912 3-Axis Magnetometer.
@@ -19,7 +19,7 @@ config AK8975
 config AK09911
 	tristate "Asahi Kasei AK09911 3-axis Compass"
 	depends on I2C
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	select AK8975
 	help
 	  Deprecated: AK09911 is now supported by AK8975 driver.
@@ -47,6 +47,17 @@ config HID_SENSOR_MAGNETOMETER_3D
 	  Say yes here to build support for the HID SENSOR
 	  Magnetometer 3D.
 
+config MMC35240
+	tristate "MEMSIC MMC35240 3-axis magnetic sensor"
+	select REGMAP_I2C
+	depends on I2C
+	help
+	  Say yes here to build support for the MEMSIC MMC35240 3-axis
+	  magnetic sensor.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called mmc35240.
+
 config IIO_ST_MAGN_3AXIS
 	tristate "STMicroelectronics magnetometers 3-Axis Driver"
 	depends on (I2C || SPI_MASTER) && SYSFS
@@ -76,4 +87,18 @@ config IIO_ST_MAGN_SPI_3AXIS
 	depends on IIO_ST_MAGN_3AXIS
 	depends on IIO_ST_SENSORS_SPI
 
+config BMC150_MAGN
+	tristate "Bosch BMC150 Magnetometer Driver"
+	depends on I2C
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for the BMC150 magnetometer.
+
+	  Currently this only supports the device via an i2c interface.
+
+	  This is a combo module with both accelerometer and magnetometer.
+	  This driver is only implementing magnetometer part, which has
+	  its own address and register map.
+
 endmenu
diff --git a/drivers/iio/magnetometer/Makefile b/drivers/iio/magnetometer/Makefile
index 0f5d3c985799..33b1d4d54ee7 100644
--- a/drivers/iio/magnetometer/Makefile
+++ b/drivers/iio/magnetometer/Makefile
@@ -6,6 +6,7 @@
 obj-$(CONFIG_AK8975)	+= ak8975.o
 obj-$(CONFIG_MAG3110)	+= mag3110.o
 obj-$(CONFIG_HID_SENSOR_MAGNETOMETER_3D) += hid-sensor-magn-3d.o
+obj-$(CONFIG_MMC35240)	+= mmc35240.o
 
 obj-$(CONFIG_IIO_ST_MAGN_3AXIS) += st_magn.o
 st_magn-y := st_magn_core.o
@@ -13,3 +14,5 @@ st_magn-$(CONFIG_IIO_BUFFER) += st_magn_buffer.o
 
 obj-$(CONFIG_IIO_ST_MAGN_I2C_3AXIS) += st_magn_i2c.o
 obj-$(CONFIG_IIO_ST_MAGN_SPI_3AXIS) += st_magn_spi.o
+
+obj-$(CONFIG_BMC150_MAGN) += bmc150_magn.o
diff --git a/drivers/iio/magnetometer/bmc150_magn.c b/drivers/iio/magnetometer/bmc150_magn.c
new file mode 100644
index 000000000000..d4c178869991
--- /dev/null
+++ b/drivers/iio/magnetometer/bmc150_magn.c
@@ -0,0 +1,1109 @@
+/*
+ * Bosch BMC150 three-axis magnetic field sensor driver
+ *
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This code is based on bmm050_api.c authored by contact@bosch.sensortec.com:
+ *
+ * (C) Copyright 2011~2014 Bosch Sensortec GmbH All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/acpi.h>
+#include <linux/gpio/consumer.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/events.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/regmap.h>
+
+#define BMC150_MAGN_DRV_NAME			"bmc150_magn"
+#define BMC150_MAGN_IRQ_NAME			"bmc150_magn_event"
+#define BMC150_MAGN_GPIO_INT			"interrupt"
+
+#define BMC150_MAGN_REG_CHIP_ID			0x40
+#define BMC150_MAGN_CHIP_ID_VAL			0x32
+
+#define BMC150_MAGN_REG_X_L			0x42
+#define BMC150_MAGN_REG_X_M			0x43
+#define BMC150_MAGN_REG_Y_L			0x44
+#define BMC150_MAGN_REG_Y_M			0x45
+#define BMC150_MAGN_SHIFT_XY_L			3
+#define BMC150_MAGN_REG_Z_L			0x46
+#define BMC150_MAGN_REG_Z_M			0x47
+#define BMC150_MAGN_SHIFT_Z_L			1
+#define BMC150_MAGN_REG_RHALL_L			0x48
+#define BMC150_MAGN_REG_RHALL_M			0x49
+#define BMC150_MAGN_SHIFT_RHALL_L		2
+
+#define BMC150_MAGN_REG_INT_STATUS		0x4A
+
+#define BMC150_MAGN_REG_POWER			0x4B
+#define BMC150_MAGN_MASK_POWER_CTL		BIT(0)
+
+#define BMC150_MAGN_REG_OPMODE_ODR		0x4C
+#define BMC150_MAGN_MASK_OPMODE			GENMASK(2, 1)
+#define BMC150_MAGN_SHIFT_OPMODE		1
+#define BMC150_MAGN_MODE_NORMAL			0x00
+#define BMC150_MAGN_MODE_FORCED			0x01
+#define BMC150_MAGN_MODE_SLEEP			0x03
+#define BMC150_MAGN_MASK_ODR			GENMASK(5, 3)
+#define BMC150_MAGN_SHIFT_ODR			3
+
+#define BMC150_MAGN_REG_INT			0x4D
+
+#define BMC150_MAGN_REG_INT_DRDY		0x4E
+#define BMC150_MAGN_MASK_DRDY_EN		BIT(7)
+#define BMC150_MAGN_SHIFT_DRDY_EN		7
+#define BMC150_MAGN_MASK_DRDY_INT3		BIT(6)
+#define BMC150_MAGN_MASK_DRDY_Z_EN		BIT(5)
+#define BMC150_MAGN_MASK_DRDY_Y_EN		BIT(4)
+#define BMC150_MAGN_MASK_DRDY_X_EN		BIT(3)
+#define BMC150_MAGN_MASK_DRDY_DR_POLARITY	BIT(2)
+#define BMC150_MAGN_MASK_DRDY_LATCHING		BIT(1)
+#define BMC150_MAGN_MASK_DRDY_INT3_POLARITY	BIT(0)
+
+#define BMC150_MAGN_REG_LOW_THRESH		0x4F
+#define BMC150_MAGN_REG_HIGH_THRESH		0x50
+#define BMC150_MAGN_REG_REP_XY			0x51
+#define BMC150_MAGN_REG_REP_Z			0x52
+
+#define BMC150_MAGN_REG_TRIM_START		0x5D
+#define BMC150_MAGN_REG_TRIM_END		0x71
+
+#define BMC150_MAGN_XY_OVERFLOW_VAL		-4096
+#define BMC150_MAGN_Z_OVERFLOW_VAL		-16384
+
+/* Time from SUSPEND to SLEEP */
+#define BMC150_MAGN_START_UP_TIME_MS		3
+
+#define BMC150_MAGN_AUTO_SUSPEND_DELAY_MS	2000
+
+#define BMC150_MAGN_REGVAL_TO_REPXY(regval) (((regval) * 2) + 1)
+#define BMC150_MAGN_REGVAL_TO_REPZ(regval) ((regval) + 1)
+#define BMC150_MAGN_REPXY_TO_REGVAL(rep) (((rep) - 1) / 2)
+#define BMC150_MAGN_REPZ_TO_REGVAL(rep) ((rep) - 1)
+
+enum bmc150_magn_axis {
+	AXIS_X,
+	AXIS_Y,
+	AXIS_Z,
+	RHALL,
+	AXIS_XYZ_MAX = RHALL,
+	AXIS_XYZR_MAX,
+};
+
+enum bmc150_magn_power_modes {
+	BMC150_MAGN_POWER_MODE_SUSPEND,
+	BMC150_MAGN_POWER_MODE_SLEEP,
+	BMC150_MAGN_POWER_MODE_NORMAL,
+};
+
+struct bmc150_magn_trim_regs {
+	s8 x1;
+	s8 y1;
+	__le16 reserved1;
+	u8 reserved2;
+	__le16 z4;
+	s8 x2;
+	s8 y2;
+	__le16 reserved3;
+	__le16 z2;
+	__le16 z1;
+	__le16 xyz1;
+	__le16 z3;
+	s8 xy2;
+	u8 xy1;
+} __packed;
+
+struct bmc150_magn_data {
+	struct i2c_client *client;
+	/*
+	 * 1. Protect this structure.
+	 * 2. Serialize sequences that power on/off the device and access HW.
+	 */
+	struct mutex mutex;
+	struct regmap *regmap;
+	/* 4 x 32 bits for x, y z, 4 bytes align, 64 bits timestamp */
+	s32 buffer[6];
+	struct iio_trigger *dready_trig;
+	bool dready_trigger_on;
+	int max_odr;
+};
+
+static const struct {
+	int freq;
+	u8 reg_val;
+} bmc150_magn_samp_freq_table[] = { {2, 0x01},
+				    {6, 0x02},
+				    {8, 0x03},
+				    {10, 0x00},
+				    {15, 0x04},
+				    {20, 0x05},
+				    {25, 0x06},
+				    {30, 0x07} };
+
+enum bmc150_magn_presets {
+	LOW_POWER_PRESET,
+	REGULAR_PRESET,
+	ENHANCED_REGULAR_PRESET,
+	HIGH_ACCURACY_PRESET
+};
+
+static const struct bmc150_magn_preset {
+	u8 rep_xy;
+	u8 rep_z;
+	u8 odr;
+} bmc150_magn_presets_table[] = {
+	[LOW_POWER_PRESET] = {3, 3, 10},
+	[REGULAR_PRESET] =  {9, 15, 10},
+	[ENHANCED_REGULAR_PRESET] =  {15, 27, 10},
+	[HIGH_ACCURACY_PRESET] =  {47, 83, 20},
+};
+
+#define BMC150_MAGN_DEFAULT_PRESET REGULAR_PRESET
+
+static bool bmc150_magn_is_writeable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case BMC150_MAGN_REG_POWER:
+	case BMC150_MAGN_REG_OPMODE_ODR:
+	case BMC150_MAGN_REG_INT:
+	case BMC150_MAGN_REG_INT_DRDY:
+	case BMC150_MAGN_REG_LOW_THRESH:
+	case BMC150_MAGN_REG_HIGH_THRESH:
+	case BMC150_MAGN_REG_REP_XY:
+	case BMC150_MAGN_REG_REP_Z:
+		return true;
+	default:
+		return false;
+	};
+}
+
+static bool bmc150_magn_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case BMC150_MAGN_REG_X_L:
+	case BMC150_MAGN_REG_X_M:
+	case BMC150_MAGN_REG_Y_L:
+	case BMC150_MAGN_REG_Y_M:
+	case BMC150_MAGN_REG_Z_L:
+	case BMC150_MAGN_REG_Z_M:
+	case BMC150_MAGN_REG_RHALL_L:
+	case BMC150_MAGN_REG_RHALL_M:
+	case BMC150_MAGN_REG_INT_STATUS:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_config bmc150_magn_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = BMC150_MAGN_REG_TRIM_END,
+	.cache_type = REGCACHE_RBTREE,
+
+	.writeable_reg = bmc150_magn_is_writeable_reg,
+	.volatile_reg = bmc150_magn_is_volatile_reg,
+};
+
+static int bmc150_magn_set_power_mode(struct bmc150_magn_data *data,
+				      enum bmc150_magn_power_modes mode,
+				      bool state)
+{
+	int ret;
+
+	switch (mode) {
+	case BMC150_MAGN_POWER_MODE_SUSPEND:
+		ret = regmap_update_bits(data->regmap, BMC150_MAGN_REG_POWER,
+					 BMC150_MAGN_MASK_POWER_CTL, !state);
+		if (ret < 0)
+			return ret;
+		usleep_range(BMC150_MAGN_START_UP_TIME_MS * 1000, 20000);
+		return 0;
+	case BMC150_MAGN_POWER_MODE_SLEEP:
+		return regmap_update_bits(data->regmap,
+					  BMC150_MAGN_REG_OPMODE_ODR,
+					  BMC150_MAGN_MASK_OPMODE,
+					  BMC150_MAGN_MODE_SLEEP <<
+					  BMC150_MAGN_SHIFT_OPMODE);
+	case BMC150_MAGN_POWER_MODE_NORMAL:
+		return regmap_update_bits(data->regmap,
+					  BMC150_MAGN_REG_OPMODE_ODR,
+					  BMC150_MAGN_MASK_OPMODE,
+					  BMC150_MAGN_MODE_NORMAL <<
+					  BMC150_MAGN_SHIFT_OPMODE);
+	}
+
+	return -EINVAL;
+}
+
+static int bmc150_magn_set_power_state(struct bmc150_magn_data *data, bool on)
+{
+#ifdef CONFIG_PM
+	int ret;
+
+	if (on) {
+		ret = pm_runtime_get_sync(&data->client->dev);
+	} else {
+		pm_runtime_mark_last_busy(&data->client->dev);
+		ret = pm_runtime_put_autosuspend(&data->client->dev);
+	}
+
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"failed to change power state to %d\n", on);
+		if (on)
+			pm_runtime_put_noidle(&data->client->dev);
+
+		return ret;
+	}
+#endif
+
+	return 0;
+}
+
+static int bmc150_magn_get_odr(struct bmc150_magn_data *data, int *val)
+{
+	int ret, reg_val;
+	u8 i, odr_val;
+
+	ret = regmap_read(data->regmap, BMC150_MAGN_REG_OPMODE_ODR, &reg_val);
+	if (ret < 0)
+		return ret;
+	odr_val = (reg_val & BMC150_MAGN_MASK_ODR) >> BMC150_MAGN_SHIFT_ODR;
+
+	for (i = 0; i < ARRAY_SIZE(bmc150_magn_samp_freq_table); i++)
+		if (bmc150_magn_samp_freq_table[i].reg_val == odr_val) {
+			*val = bmc150_magn_samp_freq_table[i].freq;
+			return 0;
+		}
+
+	return -EINVAL;
+}
+
+static int bmc150_magn_set_odr(struct bmc150_magn_data *data, int val)
+{
+	int ret;
+	u8 i;
+
+	for (i = 0; i < ARRAY_SIZE(bmc150_magn_samp_freq_table); i++) {
+		if (bmc150_magn_samp_freq_table[i].freq == val) {
+			ret = regmap_update_bits(data->regmap,
+						 BMC150_MAGN_REG_OPMODE_ODR,
+						 BMC150_MAGN_MASK_ODR,
+						 bmc150_magn_samp_freq_table[i].
+						 reg_val <<
+						 BMC150_MAGN_SHIFT_ODR);
+			if (ret < 0)
+				return ret;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int bmc150_magn_set_max_odr(struct bmc150_magn_data *data, int rep_xy,
+				   int rep_z, int odr)
+{
+	int ret, reg_val, max_odr;
+
+	if (rep_xy <= 0) {
+		ret = regmap_read(data->regmap, BMC150_MAGN_REG_REP_XY,
+				  &reg_val);
+		if (ret < 0)
+			return ret;
+		rep_xy = BMC150_MAGN_REGVAL_TO_REPXY(reg_val);
+	}
+	if (rep_z <= 0) {
+		ret = regmap_read(data->regmap, BMC150_MAGN_REG_REP_Z,
+				  &reg_val);
+		if (ret < 0)
+			return ret;
+		rep_z = BMC150_MAGN_REGVAL_TO_REPZ(reg_val);
+	}
+	if (odr <= 0) {
+		ret = bmc150_magn_get_odr(data, &odr);
+		if (ret < 0)
+			return ret;
+	}
+	/* the maximum selectable read-out frequency from datasheet */
+	max_odr = 1000000 / (145 * rep_xy + 500 * rep_z + 980);
+	if (odr > max_odr) {
+		dev_err(&data->client->dev,
+			"Can't set oversampling with sampling freq %d\n",
+			odr);
+		return -EINVAL;
+	}
+	data->max_odr = max_odr;
+
+	return 0;
+}
+
+static s32 bmc150_magn_compensate_x(struct bmc150_magn_trim_regs *tregs, s16 x,
+				    u16 rhall)
+{
+	s16 val;
+	u16 xyz1 = le16_to_cpu(tregs->xyz1);
+
+	if (x == BMC150_MAGN_XY_OVERFLOW_VAL)
+		return S32_MIN;
+
+	if (!rhall)
+		rhall = xyz1;
+
+	val = ((s16)(((u16)((((s32)xyz1) << 14) / rhall)) - ((u16)0x4000)));
+	val = ((s16)((((s32)x) * ((((((((s32)tregs->xy2) * ((((s32)val) *
+	      ((s32)val)) >> 7)) + (((s32)val) *
+	      ((s32)(((s16)tregs->xy1) << 7)))) >> 9) + ((s32)0x100000)) *
+	      ((s32)(((s16)tregs->x2) + ((s16)0xA0)))) >> 12)) >> 13)) +
+	      (((s16)tregs->x1) << 3);
+
+	return (s32)val;
+}
+
+static s32 bmc150_magn_compensate_y(struct bmc150_magn_trim_regs *tregs, s16 y,
+				    u16 rhall)
+{
+	s16 val;
+	u16 xyz1 = le16_to_cpu(tregs->xyz1);
+
+	if (y == BMC150_MAGN_XY_OVERFLOW_VAL)
+		return S32_MIN;
+
+	if (!rhall)
+		rhall = xyz1;
+
+	val = ((s16)(((u16)((((s32)xyz1) << 14) / rhall)) - ((u16)0x4000)));
+	val = ((s16)((((s32)y) * ((((((((s32)tregs->xy2) * ((((s32)val) *
+	      ((s32)val)) >> 7)) + (((s32)val) *
+	      ((s32)(((s16)tregs->xy1) << 7)))) >> 9) + ((s32)0x100000)) *
+	      ((s32)(((s16)tregs->y2) + ((s16)0xA0)))) >> 12)) >> 13)) +
+	      (((s16)tregs->y1) << 3);
+
+	return (s32)val;
+}
+
+static s32 bmc150_magn_compensate_z(struct bmc150_magn_trim_regs *tregs, s16 z,
+				    u16 rhall)
+{
+	s32 val;
+	u16 xyz1 = le16_to_cpu(tregs->xyz1);
+	u16 z1 = le16_to_cpu(tregs->z1);
+	s16 z2 = le16_to_cpu(tregs->z2);
+	s16 z3 = le16_to_cpu(tregs->z3);
+	s16 z4 = le16_to_cpu(tregs->z4);
+
+	if (z == BMC150_MAGN_Z_OVERFLOW_VAL)
+		return S32_MIN;
+
+	val = (((((s32)(z - z4)) << 15) - ((((s32)z3) * ((s32)(((s16)rhall) -
+	      ((s16)xyz1)))) >> 2)) / (z2 + ((s16)(((((s32)z1) *
+	      ((((s16)rhall) << 1))) + (1 << 15)) >> 16))));
+
+	return val;
+}
+
+static int bmc150_magn_read_xyz(struct bmc150_magn_data *data, s32 *buffer)
+{
+	int ret;
+	__le16 values[AXIS_XYZR_MAX];
+	s16 raw_x, raw_y, raw_z;
+	u16 rhall;
+	struct bmc150_magn_trim_regs tregs;
+
+	ret = regmap_bulk_read(data->regmap, BMC150_MAGN_REG_X_L,
+			       values, sizeof(values));
+	if (ret < 0)
+		return ret;
+
+	raw_x = (s16)le16_to_cpu(values[AXIS_X]) >> BMC150_MAGN_SHIFT_XY_L;
+	raw_y = (s16)le16_to_cpu(values[AXIS_Y]) >> BMC150_MAGN_SHIFT_XY_L;
+	raw_z = (s16)le16_to_cpu(values[AXIS_Z]) >> BMC150_MAGN_SHIFT_Z_L;
+	rhall = le16_to_cpu(values[RHALL]) >> BMC150_MAGN_SHIFT_RHALL_L;
+
+	ret = regmap_bulk_read(data->regmap, BMC150_MAGN_REG_TRIM_START,
+			       &tregs, sizeof(tregs));
+	if (ret < 0)
+		return ret;
+
+	buffer[AXIS_X] = bmc150_magn_compensate_x(&tregs, raw_x, rhall);
+	buffer[AXIS_Y] = bmc150_magn_compensate_y(&tregs, raw_y, rhall);
+	buffer[AXIS_Z] = bmc150_magn_compensate_z(&tregs, raw_z, rhall);
+
+	return 0;
+}
+
+static int bmc150_magn_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	int ret, tmp;
+	s32 values[AXIS_XYZ_MAX];
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (iio_buffer_enabled(indio_dev))
+			return -EBUSY;
+		mutex_lock(&data->mutex);
+
+		ret = bmc150_magn_set_power_state(data, true);
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+
+		ret = bmc150_magn_read_xyz(data, values);
+		if (ret < 0) {
+			bmc150_magn_set_power_state(data, false);
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+		*val = values[chan->scan_index];
+
+		ret = bmc150_magn_set_power_state(data, false);
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+
+		mutex_unlock(&data->mutex);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		/*
+		 * The API/driver performs an off-chip temperature
+		 * compensation and outputs x/y/z magnetic field data in
+		 * 16 LSB/uT to the upper application layer.
+		 */
+		*val = 0;
+		*val2 = 625;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = bmc150_magn_get_odr(data, val);
+		if (ret < 0)
+			return ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_OVERSAMPLING_RATIO:
+		switch (chan->channel2) {
+		case IIO_MOD_X:
+		case IIO_MOD_Y:
+			ret = regmap_read(data->regmap, BMC150_MAGN_REG_REP_XY,
+					  &tmp);
+			if (ret < 0)
+				return ret;
+			*val = BMC150_MAGN_REGVAL_TO_REPXY(tmp);
+			return IIO_VAL_INT;
+		case IIO_MOD_Z:
+			ret = regmap_read(data->regmap, BMC150_MAGN_REG_REP_Z,
+					  &tmp);
+			if (ret < 0)
+				return ret;
+			*val = BMC150_MAGN_REGVAL_TO_REPZ(tmp);
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static int bmc150_magn_write_raw(struct iio_dev *indio_dev,
+				 struct iio_chan_spec const *chan,
+				 int val, int val2, long mask)
+{
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		if (val > data->max_odr)
+			return -EINVAL;
+		mutex_lock(&data->mutex);
+		ret = bmc150_magn_set_odr(data, val);
+		mutex_unlock(&data->mutex);
+		return ret;
+	case IIO_CHAN_INFO_OVERSAMPLING_RATIO:
+		switch (chan->channel2) {
+		case IIO_MOD_X:
+		case IIO_MOD_Y:
+			if (val < 1 || val > 511)
+				return -EINVAL;
+			mutex_lock(&data->mutex);
+			ret = bmc150_magn_set_max_odr(data, val, 0, 0);
+			if (ret < 0) {
+				mutex_unlock(&data->mutex);
+				return ret;
+			}
+			ret = regmap_update_bits(data->regmap,
+						 BMC150_MAGN_REG_REP_XY,
+						 0xFF,
+						 BMC150_MAGN_REPXY_TO_REGVAL
+						 (val));
+			mutex_unlock(&data->mutex);
+			return ret;
+		case IIO_MOD_Z:
+			if (val < 1 || val > 256)
+				return -EINVAL;
+			mutex_lock(&data->mutex);
+			ret = bmc150_magn_set_max_odr(data, 0, val, 0);
+			if (ret < 0) {
+				mutex_unlock(&data->mutex);
+				return ret;
+			}
+			ret = regmap_update_bits(data->regmap,
+						 BMC150_MAGN_REG_REP_Z,
+						 0xFF,
+						 BMC150_MAGN_REPZ_TO_REGVAL
+						 (val));
+			mutex_unlock(&data->mutex);
+			return ret;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static int bmc150_magn_validate_trigger(struct iio_dev *indio_dev,
+					struct iio_trigger *trig)
+{
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+
+	if (data->dready_trig != trig)
+		return -EINVAL;
+
+	return 0;
+}
+
+static ssize_t bmc150_magn_show_samp_freq_avail(struct device *dev,
+						struct device_attribute *attr,
+						char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	size_t len = 0;
+	u8 i;
+
+	for (i = 0; i < ARRAY_SIZE(bmc150_magn_samp_freq_table); i++) {
+		if (bmc150_magn_samp_freq_table[i].freq > data->max_odr)
+			break;
+		len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
+				 bmc150_magn_samp_freq_table[i].freq);
+	}
+	/* replace last space with a newline */
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(bmc150_magn_show_samp_freq_avail);
+
+static struct attribute *bmc150_magn_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group bmc150_magn_attrs_group = {
+	.attrs = bmc150_magn_attributes,
+};
+
+#define BMC150_MAGN_CHANNEL(_axis) {					\
+	.type = IIO_MAGN,						\
+	.modified = 1,							\
+	.channel2 = IIO_MOD_##_axis,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |			\
+			      BIT(IIO_CHAN_INFO_OVERSAMPLING_RATIO),	\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) |	\
+				    BIT(IIO_CHAN_INFO_SCALE),		\
+	.scan_index = AXIS_##_axis,					\
+	.scan_type = {							\
+		.sign = 's',						\
+		.realbits = 32,						\
+		.storagebits = 32,					\
+		.endianness = IIO_LE					\
+	},								\
+}
+
+static const struct iio_chan_spec bmc150_magn_channels[] = {
+	BMC150_MAGN_CHANNEL(X),
+	BMC150_MAGN_CHANNEL(Y),
+	BMC150_MAGN_CHANNEL(Z),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+static const struct iio_info bmc150_magn_info = {
+	.attrs = &bmc150_magn_attrs_group,
+	.read_raw = bmc150_magn_read_raw,
+	.write_raw = bmc150_magn_write_raw,
+	.validate_trigger = bmc150_magn_validate_trigger,
+	.driver_module = THIS_MODULE,
+};
+
+static const unsigned long bmc150_magn_scan_masks[] = {0x07, 0};
+
+static irqreturn_t bmc150_magn_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = bmc150_magn_read_xyz(data, data->buffer);
+	mutex_unlock(&data->mutex);
+	if (ret < 0)
+		goto err;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
+					   pf->timestamp);
+
+err:
+	iio_trigger_notify_done(data->dready_trig);
+
+	return IRQ_HANDLED;
+}
+
+static int bmc150_magn_init(struct bmc150_magn_data *data)
+{
+	int ret, chip_id;
+	struct bmc150_magn_preset preset;
+
+	ret = bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_SUSPEND,
+					 false);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Failed to bring up device from suspend mode\n");
+		return ret;
+	}
+
+	ret = regmap_read(data->regmap, BMC150_MAGN_REG_CHIP_ID, &chip_id);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Failed reading chip id\n");
+		goto err_poweroff;
+	}
+	if (chip_id != BMC150_MAGN_CHIP_ID_VAL) {
+		dev_err(&data->client->dev, "Invalid chip id 0x%x\n", ret);
+		ret = -ENODEV;
+		goto err_poweroff;
+	}
+	dev_dbg(&data->client->dev, "Chip id %x\n", ret);
+
+	preset = bmc150_magn_presets_table[BMC150_MAGN_DEFAULT_PRESET];
+	ret = bmc150_magn_set_odr(data, preset.odr);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Failed to set ODR to %d\n",
+			preset.odr);
+		goto err_poweroff;
+	}
+
+	ret = regmap_write(data->regmap, BMC150_MAGN_REG_REP_XY,
+			   BMC150_MAGN_REPXY_TO_REGVAL(preset.rep_xy));
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Failed to set REP XY to %d\n",
+			preset.rep_xy);
+		goto err_poweroff;
+	}
+
+	ret = regmap_write(data->regmap, BMC150_MAGN_REG_REP_Z,
+			   BMC150_MAGN_REPZ_TO_REGVAL(preset.rep_z));
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Failed to set REP Z to %d\n",
+			preset.rep_z);
+		goto err_poweroff;
+	}
+
+	ret = bmc150_magn_set_max_odr(data, preset.rep_xy, preset.rep_z,
+				      preset.odr);
+	if (ret < 0)
+		goto err_poweroff;
+
+	ret = bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_NORMAL,
+					 true);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Failed to power on device\n");
+		goto err_poweroff;
+	}
+
+	return 0;
+
+err_poweroff:
+	bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_SUSPEND, true);
+	return ret;
+}
+
+static int bmc150_magn_reset_intr(struct bmc150_magn_data *data)
+{
+	int tmp;
+
+	/*
+	 * Data Ready (DRDY) is always cleared after
+	 * readout of data registers ends.
+	 */
+	return regmap_read(data->regmap, BMC150_MAGN_REG_X_L, &tmp);
+}
+
+static int bmc150_magn_trig_try_reen(struct iio_trigger *trig)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (!data->dready_trigger_on)
+		return 0;
+
+	mutex_lock(&data->mutex);
+	ret = bmc150_magn_reset_intr(data);
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static int bmc150_magn_data_rdy_trigger_set_state(struct iio_trigger *trig,
+						  bool state)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	int ret = 0;
+
+	mutex_lock(&data->mutex);
+	if (state == data->dready_trigger_on)
+		goto err_unlock;
+
+	ret = bmc150_magn_set_power_state(data, state);
+	if (ret < 0)
+		goto err_unlock;
+
+	ret = regmap_update_bits(data->regmap, BMC150_MAGN_REG_INT_DRDY,
+				 BMC150_MAGN_MASK_DRDY_EN,
+				 state << BMC150_MAGN_SHIFT_DRDY_EN);
+	if (ret < 0)
+		goto err_poweroff;
+
+	data->dready_trigger_on = state;
+
+	if (state) {
+		ret = bmc150_magn_reset_intr(data);
+		if (ret < 0)
+			goto err_poweroff;
+	}
+	mutex_unlock(&data->mutex);
+
+	return 0;
+
+err_poweroff:
+	bmc150_magn_set_power_state(data, false);
+err_unlock:
+	mutex_unlock(&data->mutex);
+	return ret;
+}
+
+static const struct iio_trigger_ops bmc150_magn_trigger_ops = {
+	.set_trigger_state = bmc150_magn_data_rdy_trigger_set_state,
+	.try_reenable = bmc150_magn_trig_try_reen,
+	.owner = THIS_MODULE,
+};
+
+static int bmc150_magn_gpio_probe(struct i2c_client *client)
+{
+	struct device *dev;
+	struct gpio_desc *gpio;
+	int ret;
+
+	if (!client)
+		return -EINVAL;
+
+	dev = &client->dev;
+
+	/* data ready GPIO interrupt pin */
+	gpio = devm_gpiod_get_index(dev, BMC150_MAGN_GPIO_INT, 0);
+	if (IS_ERR(gpio)) {
+		dev_err(dev, "ACPI GPIO get index failed\n");
+		return PTR_ERR(gpio);
+	}
+
+	ret = gpiod_direction_input(gpio);
+	if (ret)
+		return ret;
+
+	ret = gpiod_to_irq(gpio);
+
+	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
+
+	return ret;
+}
+
+static const char *bmc150_magn_match_acpi_device(struct device *dev)
+{
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return NULL;
+
+	return dev_name(dev);
+}
+
+static int bmc150_magn_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct bmc150_magn_data *data;
+	struct iio_dev *indio_dev;
+	const char *name = NULL;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	if (id)
+		name = id->name;
+	else if (ACPI_HANDLE(&client->dev))
+		name = bmc150_magn_match_acpi_device(&client->dev);
+	else
+		return -ENOSYS;
+
+	mutex_init(&data->mutex);
+	data->regmap = devm_regmap_init_i2c(client, &bmc150_magn_regmap_config);
+	if (IS_ERR(data->regmap)) {
+		dev_err(&client->dev, "Failed to allocate register map\n");
+		return PTR_ERR(data->regmap);
+	}
+
+	ret = bmc150_magn_init(data);
+	if (ret < 0)
+		return ret;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->channels = bmc150_magn_channels;
+	indio_dev->num_channels = ARRAY_SIZE(bmc150_magn_channels);
+	indio_dev->available_scan_masks = bmc150_magn_scan_masks;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &bmc150_magn_info;
+
+	if (client->irq <= 0)
+		client->irq = bmc150_magn_gpio_probe(client);
+
+	if (client->irq > 0) {
+		data->dready_trig = devm_iio_trigger_alloc(&client->dev,
+							   "%s-dev%d",
+							   indio_dev->name,
+							   indio_dev->id);
+		if (!data->dready_trig) {
+			ret = -ENOMEM;
+			dev_err(&client->dev, "iio trigger alloc failed\n");
+			goto err_poweroff;
+		}
+
+		data->dready_trig->dev.parent = &client->dev;
+		data->dready_trig->ops = &bmc150_magn_trigger_ops;
+		iio_trigger_set_drvdata(data->dready_trig, indio_dev);
+		ret = iio_trigger_register(data->dready_trig);
+		if (ret) {
+			dev_err(&client->dev, "iio trigger register failed\n");
+			goto err_poweroff;
+		}
+
+		ret = iio_triggered_buffer_setup(indio_dev,
+						 &iio_pollfunc_store_time,
+						 bmc150_magn_trigger_handler,
+						 NULL);
+		if (ret < 0) {
+			dev_err(&client->dev,
+				"iio triggered buffer setup failed\n");
+			goto err_trigger_unregister;
+		}
+
+		ret = request_threaded_irq(client->irq,
+					   iio_trigger_generic_data_rdy_poll,
+					   NULL,
+					   IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					   BMC150_MAGN_IRQ_NAME,
+					   data->dready_trig);
+		if (ret < 0) {
+			dev_err(&client->dev, "request irq %d failed\n",
+				client->irq);
+			goto err_buffer_cleanup;
+		}
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to register iio device\n");
+		goto err_free_irq;
+	}
+
+	ret = pm_runtime_set_active(&client->dev);
+	if (ret)
+		goto err_iio_unregister;
+
+	pm_runtime_enable(&client->dev);
+	pm_runtime_set_autosuspend_delay(&client->dev,
+					 BMC150_MAGN_AUTO_SUSPEND_DELAY_MS);
+	pm_runtime_use_autosuspend(&client->dev);
+
+	dev_dbg(&indio_dev->dev, "Registered device %s\n", name);
+
+	return 0;
+
+err_iio_unregister:
+	iio_device_unregister(indio_dev);
+err_free_irq:
+	if (client->irq > 0)
+		free_irq(client->irq, data->dready_trig);
+err_buffer_cleanup:
+	if (data->dready_trig)
+		iio_triggered_buffer_cleanup(indio_dev);
+err_trigger_unregister:
+	if (data->dready_trig)
+		iio_trigger_unregister(data->dready_trig);
+err_poweroff:
+	bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_SUSPEND, true);
+	return ret;
+}
+
+static int bmc150_magn_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	iio_device_unregister(indio_dev);
+
+	if (client->irq > 0)
+		free_irq(data->client->irq, data->dready_trig);
+
+	if (data->dready_trig) {
+		iio_triggered_buffer_cleanup(indio_dev);
+		iio_trigger_unregister(data->dready_trig);
+	}
+
+	mutex_lock(&data->mutex);
+	bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_SUSPEND, true);
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int bmc150_magn_runtime_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_SLEEP,
+					 true);
+	mutex_unlock(&data->mutex);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "powering off device failed\n");
+		return ret;
+	}
+	return 0;
+}
+
+static int bmc150_magn_runtime_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+
+	return bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_NORMAL,
+					  true);
+}
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static int bmc150_magn_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_SLEEP,
+					 true);
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static int bmc150_magn_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmc150_magn_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = bmc150_magn_set_power_mode(data, BMC150_MAGN_POWER_MODE_NORMAL,
+					 true);
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops bmc150_magn_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(bmc150_magn_suspend, bmc150_magn_resume)
+	SET_RUNTIME_PM_OPS(bmc150_magn_runtime_suspend,
+			   bmc150_magn_runtime_resume, NULL)
+};
+
+static const struct acpi_device_id bmc150_magn_acpi_match[] = {
+	{"BMC150B", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, bmc150_magn_acpi_match);
+
+static const struct i2c_device_id bmc150_magn_id[] = {
+	{"bmc150_magn", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, bmc150_magn_id);
+
+static struct i2c_driver bmc150_magn_driver = {
+	.driver = {
+		   .name = BMC150_MAGN_DRV_NAME,
+		   .acpi_match_table = ACPI_PTR(bmc150_magn_acpi_match),
+		   .pm = &bmc150_magn_pm_ops,
+		   },
+	.probe = bmc150_magn_probe,
+	.remove = bmc150_magn_remove,
+	.id_table = bmc150_magn_id,
+};
+module_i2c_driver(bmc150_magn_driver);
+
+MODULE_AUTHOR("Irina Tirdea <irina.tirdea@intel.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("BMC150 magnetometer driver");
diff --git a/drivers/iio/magnetometer/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
index 4f9c0be24451..d8a0c8da8db0 100644
--- a/drivers/iio/magnetometer/hid-sensor-magn-3d.c
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -510,7 +510,7 @@ static int hid_magn_3d_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_magn_3d_ids[] = {
+static const struct platform_device_id hid_magn_3d_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-200083",
diff --git a/drivers/iio/magnetometer/mmc35240.c b/drivers/iio/magnetometer/mmc35240.c
new file mode 100644
index 000000000000..7a2ea71c659a
--- /dev/null
+++ b/drivers/iio/magnetometer/mmc35240.c
@@ -0,0 +1,579 @@
+/*
+ * MMC35240 - MEMSIC 3-axis Magnetic Sensor
+ *
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * IIO driver for MMC35240 (7-bit I2C slave address 0x30).
+ *
+ * TODO: offset, ACPI, continuous measurement mode, PM
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/regmap.h>
+#include <linux/acpi.h>
+#include <linux/pm.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define MMC35240_DRV_NAME "mmc35240"
+#define MMC35240_REGMAP_NAME "mmc35240_regmap"
+
+#define MMC35240_REG_XOUT_L	0x00
+#define MMC35240_REG_XOUT_H	0x01
+#define MMC35240_REG_YOUT_L	0x02
+#define MMC35240_REG_YOUT_H	0x03
+#define MMC35240_REG_ZOUT_L	0x04
+#define MMC35240_REG_ZOUT_H	0x05
+
+#define MMC35240_REG_STATUS	0x06
+#define MMC35240_REG_CTRL0	0x07
+#define MMC35240_REG_CTRL1	0x08
+
+#define MMC35240_REG_ID		0x20
+
+#define MMC35240_STATUS_MEAS_DONE_BIT	BIT(0)
+
+#define MMC35240_CTRL0_REFILL_BIT	BIT(7)
+#define MMC35240_CTRL0_RESET_BIT	BIT(6)
+#define MMC35240_CTRL0_SET_BIT		BIT(5)
+#define MMC35240_CTRL0_CMM_BIT		BIT(1)
+#define MMC35240_CTRL0_TM_BIT		BIT(0)
+
+/* output resolution bits */
+#define MMC35240_CTRL1_BW0_BIT		BIT(0)
+#define MMC35240_CTRL1_BW1_BIT		BIT(1)
+
+#define MMC35240_CTRL1_BW_MASK	 (MMC35240_CTRL1_BW0_BIT | \
+		 MMC35240_CTRL1_BW1_BIT)
+#define MMC35240_CTRL1_BW_SHIFT		0
+
+#define MMC35240_WAIT_CHARGE_PUMP	50000	/* us */
+#define MMC53240_WAIT_SET_RESET		1000	/* us */
+
+/*
+ * Memsic OTP process code piece is put here for reference:
+ *
+ * #define OTP_CONVERT(REG)  ((float)((REG) >=32 ? (32 - (REG)) : (REG)) * 0.006
+ * 1) For X axis, the COEFFICIENT is always 1.
+ * 2) For Y axis, the COEFFICIENT is as below:
+ *    f_OTP_matrix[4] = OTP_CONVERT(((reg_data[1] & 0x03) << 4) |
+ *                                   (reg_data[2] >> 4)) + 1.0;
+ * 3) For Z axis, the COEFFICIENT is as below:
+ *    f_OTP_matrix[8] = (OTP_CONVERT(reg_data[3] & 0x3f) + 1) * 1.35;
+ * We implemented the OTP logic into driver.
+ */
+
+/* scale = 1000 here for Y otp */
+#define MMC35240_OTP_CONVERT_Y(REG) (((REG) >= 32 ? (32 - (REG)) : (REG)) * 6)
+
+/* 0.6 * 1.35 = 0.81, scale 10000 for Z otp */
+#define MMC35240_OTP_CONVERT_Z(REG) (((REG) >= 32 ? (32 - (REG)) : (REG)) * 81)
+
+#define MMC35240_X_COEFF(x)	(x)
+#define MMC35240_Y_COEFF(y)	(y + 1000)
+#define MMC35240_Z_COEFF(z)	(z + 13500)
+
+#define MMC35240_OTP_START_ADDR		0x1B
+
+enum mmc35240_resolution {
+	MMC35240_16_BITS_SLOW = 0, /* 100 Hz */
+	MMC35240_16_BITS_FAST,     /* 200 Hz */
+	MMC35240_14_BITS,          /* 333 Hz */
+	MMC35240_12_BITS,          /* 666 Hz */
+};
+
+enum mmc35240_axis {
+	AXIS_X = 0,
+	AXIS_Y,
+	AXIS_Z,
+};
+
+static const struct {
+	int sens[3]; /* sensitivity per X, Y, Z axis */
+	int nfo; /* null field output */
+} mmc35240_props_table[] = {
+	/* 16 bits, 100Hz ODR */
+	{
+		{1024, 1024, 1024},
+		32768,
+	},
+	/* 16 bits, 200Hz ODR */
+	{
+		{1024, 1024, 770},
+		32768,
+	},
+	/* 14 bits, 333Hz ODR */
+	{
+		{256, 256, 193},
+		8192,
+	},
+	/* 12 bits, 666Hz ODR */
+	{
+		{64, 64, 48},
+		2048,
+	},
+};
+
+struct mmc35240_data {
+	struct i2c_client *client;
+	struct mutex mutex;
+	struct regmap *regmap;
+	enum mmc35240_resolution res;
+
+	/* OTP compensation */
+	int axis_coef[3];
+	int axis_scale[3];
+};
+
+static const int mmc35240_samp_freq[] = {100, 200, 333, 666};
+
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("100 200 333 666");
+
+#define MMC35240_CHANNEL(_axis) { \
+	.type = IIO_MAGN, \
+	.modified = 1, \
+	.channel2 = IIO_MOD_ ## _axis, \
+	.address = AXIS_ ## _axis, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
+			BIT(IIO_CHAN_INFO_SCALE), \
+}
+
+static const struct iio_chan_spec mmc35240_channels[] = {
+	MMC35240_CHANNEL(X),
+	MMC35240_CHANNEL(Y),
+	MMC35240_CHANNEL(Z),
+};
+
+static struct attribute *mmc35240_attributes[] = {
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group mmc35240_attribute_group = {
+	.attrs = mmc35240_attributes,
+};
+
+static int mmc35240_get_samp_freq_index(struct mmc35240_data *data,
+					int val, int val2)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mmc35240_samp_freq); i++)
+		if (mmc35240_samp_freq[i] == val)
+			return i;
+	return -EINVAL;
+}
+
+static int mmc35240_hw_set(struct mmc35240_data *data, bool set)
+{
+	int ret;
+	u8 coil_bit;
+
+	/*
+	 * Recharge the capacitor at VCAP pin, requested to be issued
+	 * before a SET/RESET command.
+	 */
+	ret = regmap_update_bits(data->regmap, MMC35240_REG_CTRL0,
+				 MMC35240_CTRL0_REFILL_BIT,
+				 MMC35240_CTRL0_REFILL_BIT);
+	if (ret < 0)
+		return ret;
+	usleep_range(MMC35240_WAIT_CHARGE_PUMP, MMC35240_WAIT_CHARGE_PUMP + 1);
+
+	if (set)
+		coil_bit = MMC35240_CTRL0_SET_BIT;
+	else
+		coil_bit = MMC35240_CTRL0_RESET_BIT;
+
+	return regmap_update_bits(data->regmap, MMC35240_REG_CTRL0,
+				  MMC35240_CTRL0_REFILL_BIT,
+				  coil_bit);
+}
+
+static int mmc35240_init(struct mmc35240_data *data)
+{
+	int ret, y_convert, z_convert;
+	unsigned int reg_id;
+	u8 otp_data[6];
+
+	ret = regmap_read(data->regmap, MMC35240_REG_ID, &reg_id);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading product id\n");
+		return ret;
+	}
+
+	dev_dbg(&data->client->dev, "MMC35240 chip id %x\n", reg_id);
+
+	/*
+	 * make sure we restore sensor characteristics, by doing
+	 * a RESET/SET sequence
+	 */
+	ret = mmc35240_hw_set(data, false);
+	if (ret < 0)
+		return ret;
+	usleep_range(MMC53240_WAIT_SET_RESET, MMC53240_WAIT_SET_RESET + 1);
+
+	ret = mmc35240_hw_set(data, true);
+	if (ret < 0)
+		return ret;
+
+	/* set default sampling frequency */
+	ret = regmap_update_bits(data->regmap, MMC35240_REG_CTRL1,
+				 MMC35240_CTRL1_BW_MASK,
+				 data->res << MMC35240_CTRL1_BW_SHIFT);
+	if (ret < 0)
+		return ret;
+
+	ret = regmap_bulk_read(data->regmap, MMC35240_OTP_START_ADDR,
+			       (u8 *)otp_data, sizeof(otp_data));
+	if (ret < 0)
+		return ret;
+
+	y_convert = MMC35240_OTP_CONVERT_Y(((otp_data[1] & 0x03) << 4) |
+					   (otp_data[2] >> 4));
+	z_convert = MMC35240_OTP_CONVERT_Z(otp_data[3] & 0x3f);
+
+	data->axis_coef[0] = MMC35240_X_COEFF(1);
+	data->axis_coef[1] = MMC35240_Y_COEFF(y_convert);
+	data->axis_coef[2] = MMC35240_Z_COEFF(z_convert);
+
+	data->axis_scale[0] = 1;
+	data->axis_scale[1] = 1000;
+	data->axis_scale[2] = 10000;
+
+	return 0;
+}
+
+static int mmc35240_take_measurement(struct mmc35240_data *data)
+{
+	int ret, tries = 100;
+	unsigned int reg_status;
+
+	ret = regmap_write(data->regmap, MMC35240_REG_CTRL0,
+			   MMC35240_CTRL0_TM_BIT);
+	if (ret < 0)
+		return ret;
+
+	while (tries-- > 0) {
+		ret = regmap_read(data->regmap, MMC35240_REG_STATUS,
+				  &reg_status);
+		if (ret < 0)
+			return ret;
+		if (reg_status & MMC35240_STATUS_MEAS_DONE_BIT)
+			break;
+		/* minimum wait time to complete measurement is 10 ms */
+		usleep_range(10000, 11000);
+	}
+
+	if (tries < 0) {
+		dev_err(&data->client->dev, "data not ready\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int mmc35240_read_measurement(struct mmc35240_data *data, __le16 buf[3])
+{
+	int ret;
+
+	ret = mmc35240_take_measurement(data);
+	if (ret < 0)
+		return ret;
+
+	return regmap_bulk_read(data->regmap, MMC35240_REG_XOUT_L, (u8 *)buf,
+				3 * sizeof(__le16));
+}
+
+/**
+ * mmc35240_raw_to_mgauss - convert raw readings to milli gauss. Also apply
+			    compensation for output value.
+ *
+ * @data: device private data
+ * @index: axis index for which we want the conversion
+ * @buf: raw data to be converted, 2 bytes in little endian format
+ * @val: compensated output reading (unit is milli gauss)
+ *
+ * Returns: 0 in case of success, -EINVAL when @index is not valid
+ */
+static int mmc35240_raw_to_mgauss(struct mmc35240_data *data, int index,
+				  __le16 buf[], int *val)
+{
+	int raw_x, raw_y, raw_z;
+	int sens_x, sens_y, sens_z;
+	int nfo;
+
+	raw_x = le16_to_cpu(buf[AXIS_X]);
+	raw_y = le16_to_cpu(buf[AXIS_Y]);
+	raw_z = le16_to_cpu(buf[AXIS_Z]);
+
+	sens_x = mmc35240_props_table[data->res].sens[AXIS_X];
+	sens_y = mmc35240_props_table[data->res].sens[AXIS_Y];
+	sens_z = mmc35240_props_table[data->res].sens[AXIS_Z];
+
+	nfo = mmc35240_props_table[data->res].nfo;
+
+	switch (index) {
+	case AXIS_X:
+		*val = (raw_x - nfo) * 1000 / sens_x;
+		break;
+	case AXIS_Y:
+		*val = (raw_y - nfo) * 1000 / sens_y -
+			(raw_z - nfo)  * 1000 / sens_z;
+		break;
+	case AXIS_Z:
+		*val = (raw_y - nfo) * 1000 / sens_y +
+			(raw_z - nfo) * 1000 / sens_z;
+		break;
+	default:
+		return -EINVAL;
+	}
+	/* apply OTP compensation */
+	*val = (*val) * data->axis_coef[index] / data->axis_scale[index];
+
+	return 0;
+}
+
+static int mmc35240_read_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan, int *val,
+			     int *val2, long mask)
+{
+	struct mmc35240_data *data = iio_priv(indio_dev);
+	int ret, i;
+	unsigned int reg;
+	__le16 buf[3];
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&data->mutex);
+		ret = mmc35240_read_measurement(data, buf);
+		mutex_unlock(&data->mutex);
+		if (ret < 0)
+			return ret;
+		ret = mmc35240_raw_to_mgauss(data, chan->address, buf, val);
+		if (ret < 0)
+			return ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = 1000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		mutex_lock(&data->mutex);
+		ret = regmap_read(data->regmap, MMC35240_REG_CTRL1, &reg);
+		mutex_unlock(&data->mutex);
+		if (ret < 0)
+			return ret;
+
+		i = (reg & MMC35240_CTRL1_BW_MASK) >> MMC35240_CTRL1_BW_SHIFT;
+		if (i < 0 || i >= ARRAY_SIZE(mmc35240_samp_freq))
+			return -EINVAL;
+
+		*val = mmc35240_samp_freq[i];
+		*val2 = 0;
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mmc35240_write_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan, int val,
+			      int val2, long mask)
+{
+	struct mmc35240_data *data = iio_priv(indio_dev);
+	int i, ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		i = mmc35240_get_samp_freq_index(data, val, val2);
+		if (i < 0)
+			return -EINVAL;
+		mutex_lock(&data->mutex);
+		ret = regmap_update_bits(data->regmap, MMC35240_REG_CTRL1,
+					 MMC35240_CTRL1_BW_MASK,
+					 i << MMC35240_CTRL1_BW_SHIFT);
+		mutex_unlock(&data->mutex);
+		return ret;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info mmc35240_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= mmc35240_read_raw,
+	.write_raw	= mmc35240_write_raw,
+	.attrs		= &mmc35240_attribute_group,
+};
+
+static bool mmc35240_is_writeable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MMC35240_REG_CTRL0:
+	case MMC35240_REG_CTRL1:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool mmc35240_is_readable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MMC35240_REG_XOUT_L:
+	case MMC35240_REG_XOUT_H:
+	case MMC35240_REG_YOUT_L:
+	case MMC35240_REG_YOUT_H:
+	case MMC35240_REG_ZOUT_L:
+	case MMC35240_REG_ZOUT_H:
+	case MMC35240_REG_STATUS:
+	case MMC35240_REG_ID:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool mmc35240_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MMC35240_REG_CTRL0:
+	case MMC35240_REG_CTRL1:
+		return false;
+	default:
+		return true;
+	}
+}
+
+static struct reg_default mmc35240_reg_defaults[] = {
+	{ MMC35240_REG_CTRL0,  0x00 },
+	{ MMC35240_REG_CTRL1,  0x00 },
+};
+
+static const struct regmap_config mmc35240_regmap_config = {
+	.name = MMC35240_REGMAP_NAME,
+
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = MMC35240_REG_ID,
+	.cache_type = REGCACHE_FLAT,
+
+	.writeable_reg = mmc35240_is_writeable_reg,
+	.readable_reg = mmc35240_is_readable_reg,
+	.volatile_reg = mmc35240_is_volatile_reg,
+
+	.reg_defaults = mmc35240_reg_defaults,
+	.num_reg_defaults = ARRAY_SIZE(mmc35240_reg_defaults),
+};
+
+static int mmc35240_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	struct mmc35240_data *data;
+	struct iio_dev *indio_dev;
+	struct regmap *regmap;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	regmap = devm_regmap_init_i2c(client, &mmc35240_regmap_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&client->dev, "regmap initialization failed\n");
+		return PTR_ERR(regmap);
+	}
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	data->regmap = regmap;
+	data->res = MMC35240_16_BITS_SLOW;
+
+	mutex_init(&data->mutex);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &mmc35240_info;
+	indio_dev->name = MMC35240_DRV_NAME;
+	indio_dev->channels = mmc35240_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mmc35240_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = mmc35240_init(data);
+	if (ret < 0) {
+		dev_err(&client->dev, "mmc35240 chip init failed\n");
+		return ret;
+	}
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int mmc35240_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct mmc35240_data *data = iio_priv(indio_dev);
+
+	regcache_cache_only(data->regmap, true);
+
+	return 0;
+}
+
+static int mmc35240_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct mmc35240_data *data = iio_priv(indio_dev);
+	int ret;
+
+	regcache_mark_dirty(data->regmap);
+	ret = regcache_sync_region(data->regmap, MMC35240_REG_CTRL0,
+				   MMC35240_REG_CTRL1);
+	if (ret < 0)
+		dev_err(dev, "Failed to restore control registers\n");
+
+	regcache_cache_only(data->regmap, false);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops mmc35240_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mmc35240_suspend, mmc35240_resume)
+};
+
+static const struct acpi_device_id mmc35240_acpi_match[] = {
+	{"MMC35240", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, mmc35240_acpi_match);
+
+static const struct i2c_device_id mmc35240_id[] = {
+	{"mmc35240", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, mmc35240_id);
+
+static struct i2c_driver mmc35240_driver = {
+	.driver = {
+		.name = MMC35240_DRV_NAME,
+		.pm = &mmc35240_pm_ops,
+		.acpi_match_table = ACPI_PTR(mmc35240_acpi_match),
+	},
+	.probe		= mmc35240_probe,
+	.id_table	= mmc35240_id,
+};
+
+module_i2c_driver(mmc35240_driver);
+
+MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>");
+MODULE_DESCRIPTION("MEMSIC MMC35240 magnetic sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/magnetometer/st_magn.h b/drivers/iio/magnetometer/st_magn.h
index 7e81d00ef0c3..287691ca56c1 100644
--- a/drivers/iio/magnetometer/st_magn.h
+++ b/drivers/iio/magnetometer/st_magn.h
@@ -14,6 +14,7 @@
 #include <linux/types.h>
 #include <linux/iio/common/st_sensors.h>
 
+#define LSM303DLH_MAGN_DEV_NAME		"lsm303dlh_magn"
 #define LSM303DLHC_MAGN_DEV_NAME	"lsm303dlhc_magn"
 #define LSM303DLM_MAGN_DEV_NAME		"lsm303dlm_magn"
 #define LIS3MDL_MAGN_DEV_NAME		"lis3mdl"
diff --git a/drivers/iio/magnetometer/st_magn_core.c b/drivers/iio/magnetometer/st_magn_core.c
index 2e56f812a644..b4bcfb790f49 100644
--- a/drivers/iio/magnetometer/st_magn_core.c
+++ b/drivers/iio/magnetometer/st_magn_core.c
@@ -45,6 +45,46 @@
 #define ST_MAGN_FS_AVL_12000MG			12000
 #define ST_MAGN_FS_AVL_16000MG			16000
 
+/* CUSTOM VALUES FOR SENSOR 0 */
+#define ST_MAGN_0_ODR_ADDR			0x00
+#define ST_MAGN_0_ODR_MASK			0x1c
+#define ST_MAGN_0_ODR_AVL_1HZ_VAL		0x00
+#define ST_MAGN_0_ODR_AVL_2HZ_VAL		0x01
+#define ST_MAGN_0_ODR_AVL_3HZ_VAL		0x02
+#define ST_MAGN_0_ODR_AVL_8HZ_VAL		0x03
+#define ST_MAGN_0_ODR_AVL_15HZ_VAL		0x04
+#define ST_MAGN_0_ODR_AVL_30HZ_VAL		0x05
+#define ST_MAGN_0_ODR_AVL_75HZ_VAL		0x06
+#define ST_MAGN_0_ODR_AVL_220HZ_VAL		0x07
+#define ST_MAGN_0_PW_ADDR			0x02
+#define ST_MAGN_0_PW_MASK			0x03
+#define ST_MAGN_0_PW_ON				0x00
+#define ST_MAGN_0_PW_OFF			0x03
+#define ST_MAGN_0_FS_ADDR			0x01
+#define ST_MAGN_0_FS_MASK			0xe0
+#define ST_MAGN_0_FS_AVL_1300_VAL		0x01
+#define ST_MAGN_0_FS_AVL_1900_VAL		0x02
+#define ST_MAGN_0_FS_AVL_2500_VAL		0x03
+#define ST_MAGN_0_FS_AVL_4000_VAL		0x04
+#define ST_MAGN_0_FS_AVL_4700_VAL		0x05
+#define ST_MAGN_0_FS_AVL_5600_VAL		0x06
+#define ST_MAGN_0_FS_AVL_8100_VAL		0x07
+#define ST_MAGN_0_FS_AVL_1300_GAIN_XY		1100
+#define ST_MAGN_0_FS_AVL_1900_GAIN_XY		855
+#define ST_MAGN_0_FS_AVL_2500_GAIN_XY		670
+#define ST_MAGN_0_FS_AVL_4000_GAIN_XY		450
+#define ST_MAGN_0_FS_AVL_4700_GAIN_XY		400
+#define ST_MAGN_0_FS_AVL_5600_GAIN_XY		330
+#define ST_MAGN_0_FS_AVL_8100_GAIN_XY		230
+#define ST_MAGN_0_FS_AVL_1300_GAIN_Z		980
+#define ST_MAGN_0_FS_AVL_1900_GAIN_Z		760
+#define ST_MAGN_0_FS_AVL_2500_GAIN_Z		600
+#define ST_MAGN_0_FS_AVL_4000_GAIN_Z		400
+#define ST_MAGN_0_FS_AVL_4700_GAIN_Z		355
+#define ST_MAGN_0_FS_AVL_5600_GAIN_Z		295
+#define ST_MAGN_0_FS_AVL_8100_GAIN_Z		205
+#define ST_MAGN_0_MULTIREAD_BIT			false
+
 /* CUSTOM VALUES FOR SENSOR 1 */
 #define ST_MAGN_1_WAI_EXP			0x3c
 #define ST_MAGN_1_ODR_ADDR			0x00
@@ -151,6 +191,82 @@ static const struct iio_chan_spec st_magn_2_16bit_channels[] = {
 
 static const struct st_sensor_settings st_magn_sensors_settings[] = {
 	{
+		.wai = 0, /* This sensor has no valid WhoAmI report 0 */
+		.sensors_supported = {
+			[0] = LSM303DLH_MAGN_DEV_NAME,
+		},
+		.ch = (struct iio_chan_spec *)st_magn_16bit_channels,
+		.odr = {
+			.addr = ST_MAGN_0_ODR_ADDR,
+			.mask = ST_MAGN_0_ODR_MASK,
+			.odr_avl = {
+				{ 1, ST_MAGN_0_ODR_AVL_1HZ_VAL, },
+				{ 2, ST_MAGN_0_ODR_AVL_2HZ_VAL, },
+				{ 3, ST_MAGN_0_ODR_AVL_3HZ_VAL, },
+				{ 8, ST_MAGN_0_ODR_AVL_8HZ_VAL, },
+				{ 15, ST_MAGN_0_ODR_AVL_15HZ_VAL, },
+				{ 30, ST_MAGN_0_ODR_AVL_30HZ_VAL, },
+				{ 75, ST_MAGN_0_ODR_AVL_75HZ_VAL, },
+			},
+		},
+		.pw = {
+			.addr = ST_MAGN_0_PW_ADDR,
+			.mask = ST_MAGN_0_PW_MASK,
+			.value_on = ST_MAGN_0_PW_ON,
+			.value_off = ST_MAGN_0_PW_OFF,
+		},
+		.fs = {
+			.addr = ST_MAGN_0_FS_ADDR,
+			.mask = ST_MAGN_0_FS_MASK,
+			.fs_avl = {
+				[0] = {
+					.num = ST_MAGN_FS_AVL_1300MG,
+					.value = ST_MAGN_0_FS_AVL_1300_VAL,
+					.gain = ST_MAGN_0_FS_AVL_1300_GAIN_XY,
+					.gain2 = ST_MAGN_0_FS_AVL_1300_GAIN_Z,
+				},
+				[1] = {
+					.num = ST_MAGN_FS_AVL_1900MG,
+					.value = ST_MAGN_0_FS_AVL_1900_VAL,
+					.gain = ST_MAGN_0_FS_AVL_1900_GAIN_XY,
+					.gain2 = ST_MAGN_0_FS_AVL_1900_GAIN_Z,
+				},
+				[2] = {
+					.num = ST_MAGN_FS_AVL_2500MG,
+					.value = ST_MAGN_0_FS_AVL_2500_VAL,
+					.gain = ST_MAGN_0_FS_AVL_2500_GAIN_XY,
+					.gain2 = ST_MAGN_0_FS_AVL_2500_GAIN_Z,
+				},
+				[3] = {
+					.num = ST_MAGN_FS_AVL_4000MG,
+					.value = ST_MAGN_0_FS_AVL_4000_VAL,
+					.gain = ST_MAGN_0_FS_AVL_4000_GAIN_XY,
+					.gain2 = ST_MAGN_0_FS_AVL_4000_GAIN_Z,
+				},
+				[4] = {
+					.num = ST_MAGN_FS_AVL_4700MG,
+					.value = ST_MAGN_0_FS_AVL_4700_VAL,
+					.gain = ST_MAGN_0_FS_AVL_4700_GAIN_XY,
+					.gain2 = ST_MAGN_0_FS_AVL_4700_GAIN_Z,
+				},
+				[5] = {
+					.num = ST_MAGN_FS_AVL_5600MG,
+					.value = ST_MAGN_0_FS_AVL_5600_VAL,
+					.gain = ST_MAGN_0_FS_AVL_5600_GAIN_XY,
+					.gain2 = ST_MAGN_0_FS_AVL_5600_GAIN_Z,
+				},
+				[6] = {
+					.num = ST_MAGN_FS_AVL_8100MG,
+					.value = ST_MAGN_0_FS_AVL_8100_VAL,
+					.gain = ST_MAGN_0_FS_AVL_8100_GAIN_XY,
+					.gain2 = ST_MAGN_0_FS_AVL_8100_GAIN_Z,
+				},
+			},
+		},
+		.multi_read_bit = ST_MAGN_0_MULTIREAD_BIT,
+		.bootime = 2,
+	},
+	{
 		.wai = ST_MAGN_1_WAI_EXP,
 		.sensors_supported = {
 			[0] = LSM303DLHC_MAGN_DEV_NAME,
diff --git a/drivers/iio/magnetometer/st_magn_i2c.c b/drivers/iio/magnetometer/st_magn_i2c.c
index 92e5c15452a3..5311d8aea8cc 100644
--- a/drivers/iio/magnetometer/st_magn_i2c.c
+++ b/drivers/iio/magnetometer/st_magn_i2c.c
@@ -21,6 +21,10 @@
 #ifdef CONFIG_OF
 static const struct of_device_id st_magn_of_match[] = {
 	{
+		.compatible = "st,lsm303dlh-magn",
+		.data = LSM303DLH_MAGN_DEV_NAME,
+	},
+	{
 		.compatible = "st,lsm303dlhc-magn",
 		.data = LSM303DLHC_MAGN_DEV_NAME,
 	},
@@ -71,6 +75,7 @@ static int st_magn_i2c_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id st_magn_id_table[] = {
+	{ LSM303DLH_MAGN_DEV_NAME },
 	{ LSM303DLHC_MAGN_DEV_NAME },
 	{ LSM303DLM_MAGN_DEV_NAME },
 	{ LIS3MDL_MAGN_DEV_NAME },
diff --git a/drivers/iio/orientation/hid-sensor-incl-3d.c b/drivers/iio/orientation/hid-sensor-incl-3d.c
index 5930fa32a2ab..fd1b3696ee42 100644
--- a/drivers/iio/orientation/hid-sensor-incl-3d.c
+++ b/drivers/iio/orientation/hid-sensor-incl-3d.c
@@ -315,7 +315,6 @@ static int hid_incl_3d_probe(struct platform_device *pdev)
 	struct iio_dev *indio_dev;
 	struct incl_3d_state *incl_state;
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
-	struct iio_chan_spec *channels;
 
 	indio_dev = devm_iio_device_alloc(&pdev->dev,
 					  sizeof(struct incl_3d_state));
@@ -336,21 +335,22 @@ static int hid_incl_3d_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	channels = kmemdup(incl_3d_channels, sizeof(incl_3d_channels),
-			   GFP_KERNEL);
-	if (!channels) {
+	indio_dev->channels = kmemdup(incl_3d_channels,
+				      sizeof(incl_3d_channels), GFP_KERNEL);
+	if (!indio_dev->channels) {
 		dev_err(&pdev->dev, "failed to duplicate channels\n");
 		return -ENOMEM;
 	}
 
-	ret = incl_3d_parse_report(pdev, hsdev, channels,
-				HID_USAGE_SENSOR_INCLINOMETER_3D, incl_state);
+	ret = incl_3d_parse_report(pdev, hsdev,
+				   (struct iio_chan_spec *)indio_dev->channels,
+				   HID_USAGE_SENSOR_INCLINOMETER_3D,
+				   incl_state);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to setup attributes\n");
 		goto error_free_dev_mem;
 	}
 
-	indio_dev->channels = channels;
 	indio_dev->num_channels = ARRAY_SIZE(incl_3d_channels);
 	indio_dev->dev.parent = &pdev->dev;
 	indio_dev->info = &incl_3d_info;
@@ -417,7 +417,7 @@ static int hid_incl_3d_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_incl_3d_ids[] = {
+static const struct platform_device_id hid_incl_3d_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-200086",
diff --git a/drivers/iio/orientation/hid-sensor-rotation.c b/drivers/iio/orientation/hid-sensor-rotation.c
index 4afb6c79ccbc..b98b9d94d184 100644
--- a/drivers/iio/orientation/hid-sensor-rotation.c
+++ b/drivers/iio/orientation/hid-sensor-rotation.c
@@ -222,7 +222,6 @@ static int hid_dev_rot_probe(struct platform_device *pdev)
 	struct iio_dev *indio_dev;
 	struct dev_rot_state *rot_state;
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
-	struct iio_chan_spec *channels;
 
 	indio_dev = devm_iio_device_alloc(&pdev->dev,
 					  sizeof(struct dev_rot_state));
@@ -243,21 +242,23 @@ static int hid_dev_rot_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	channels = devm_kmemdup(&pdev->dev, dev_rot_channels,
-					sizeof(dev_rot_channels), GFP_KERNEL);
-	if (!channels) {
+	indio_dev->channels = devm_kmemdup(&pdev->dev, dev_rot_channels,
+					   sizeof(dev_rot_channels),
+					   GFP_KERNEL);
+	if (!indio_dev->channels) {
 		dev_err(&pdev->dev, "failed to duplicate channels\n");
 		return -ENOMEM;
 	}
 
-	ret = dev_rot_parse_report(pdev, hsdev, channels,
-			HID_USAGE_SENSOR_DEVICE_ORIENTATION, rot_state);
+	ret = dev_rot_parse_report(pdev, hsdev,
+				   (struct iio_chan_spec *)indio_dev->channels,
+				   HID_USAGE_SENSOR_DEVICE_ORIENTATION,
+				   rot_state);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to setup attributes\n");
 		return ret;
 	}
 
-	indio_dev->channels = channels;
 	indio_dev->num_channels = ARRAY_SIZE(dev_rot_channels);
 	indio_dev->dev.parent = &pdev->dev;
 	indio_dev->info = &dev_rot_info;
@@ -321,7 +322,7 @@ static int hid_dev_rot_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_dev_rot_ids[] = {
+static const struct platform_device_id hid_dev_rot_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-20008a",
diff --git a/drivers/iio/pressure/hid-sensor-press.c b/drivers/iio/pressure/hid-sensor-press.c
index 3cf0bd67d24c..6848d8c80eff 100644
--- a/drivers/iio/pressure/hid-sensor-press.c
+++ b/drivers/iio/pressure/hid-sensor-press.c
@@ -260,7 +260,6 @@ static int hid_press_probe(struct platform_device *pdev)
 	struct iio_dev *indio_dev;
 	struct press_state *press_state;
 	struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
-	struct iio_chan_spec *channels;
 
 	indio_dev = devm_iio_device_alloc(&pdev->dev,
 				sizeof(struct press_state));
@@ -280,20 +279,21 @@ static int hid_press_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	channels = kmemdup(press_channels, sizeof(press_channels), GFP_KERNEL);
-	if (!channels) {
+	indio_dev->channels = kmemdup(press_channels, sizeof(press_channels),
+				      GFP_KERNEL);
+	if (!indio_dev->channels) {
 		dev_err(&pdev->dev, "failed to duplicate channels\n");
 		return -ENOMEM;
 	}
 
-	ret = press_parse_report(pdev, hsdev, channels,
-				HID_USAGE_SENSOR_PRESSURE, press_state);
+	ret = press_parse_report(pdev, hsdev,
+				 (struct iio_chan_spec *)indio_dev->channels,
+				 HID_USAGE_SENSOR_PRESSURE, press_state);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to setup attributes\n");
 		goto error_free_dev_mem;
 	}
 
-	indio_dev->channels = channels;
 	indio_dev->num_channels =
 				ARRAY_SIZE(press_channels);
 	indio_dev->dev.parent = &pdev->dev;
@@ -360,7 +360,7 @@ static int hid_press_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_press_ids[] = {
+static const struct platform_device_id hid_press_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-200031",
diff --git a/drivers/iio/proximity/sx9500.c b/drivers/iio/proximity/sx9500.c
index fa40f6d0ca39..2042e375f835 100644
--- a/drivers/iio/proximity/sx9500.c
+++ b/drivers/iio/proximity/sx9500.c
@@ -18,6 +18,8 @@
 #include <linux/acpi.h>
 #include <linux/gpio/consumer.h>
 #include <linux/regmap.h>
+#include <linux/pm.h>
+#include <linux/delay.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
@@ -29,7 +31,9 @@
 
 #define SX9500_DRIVER_NAME		"sx9500"
 #define SX9500_IRQ_NAME			"sx9500_event"
-#define SX9500_GPIO_NAME		"sx9500_gpio"
+
+#define SX9500_GPIO_INT			"interrupt"
+#define SX9500_GPIO_RESET		"reset"
 
 /* Register definitions. */
 #define SX9500_REG_IRQ_SRC		0x00
@@ -73,6 +77,7 @@
 #define SX9500_CONVDONE_IRQ		BIT(3)
 
 #define SX9500_PROXSTAT_SHIFT		4
+#define SX9500_COMPSTAT_MASK		GENMASK(3, 0)
 
 #define SX9500_NUM_CHANNELS		4
 
@@ -81,6 +86,7 @@ struct sx9500_data {
 	struct i2c_client *client;
 	struct iio_trigger *trig;
 	struct regmap *regmap;
+	struct gpio_desc *gpiod_rst;
 	/*
 	 * Last reading of the proximity status for each channel.  We
 	 * only send an event to user space when this changes.
@@ -89,6 +95,11 @@ struct sx9500_data {
 	bool event_enabled[SX9500_NUM_CHANNELS];
 	bool trigger_enabled;
 	u16 *buffer;
+	/* Remember enabled channels and sample rate during suspend. */
+	unsigned int suspend_ctrl0;
+	struct completion completion;
+	int data_rdy_users, close_far_users;
+	int channel_users[SX9500_NUM_CHANNELS];
 };
 
 static const struct iio_event_spec sx9500_events[] = {
@@ -139,6 +150,10 @@ static const struct {
 	{2, 500000},
 };
 
+static const unsigned int sx9500_scan_period_table[] = {
+	30, 60, 90, 120, 150, 200, 300, 400,
+};
+
 static const struct regmap_range sx9500_writable_reg_ranges[] = {
 	regmap_reg_range(SX9500_REG_IRQ_MSK, SX9500_REG_IRQ_MSK),
 	regmap_reg_range(SX9500_REG_PROX_CTRL0, SX9500_REG_PROX_CTRL8),
@@ -191,7 +206,67 @@ static const struct regmap_config sx9500_regmap_config = {
 	.volatile_table = &sx9500_volatile_regs,
 };
 
-static int sx9500_read_proximity(struct sx9500_data *data,
+static int sx9500_inc_users(struct sx9500_data *data, int *counter,
+			    unsigned int reg, unsigned int bitmask)
+{
+	(*counter)++;
+	if (*counter != 1)
+		/* Bit is already active, nothing to do. */
+		return 0;
+
+	return regmap_update_bits(data->regmap, reg, bitmask, bitmask);
+}
+
+static int sx9500_dec_users(struct sx9500_data *data, int *counter,
+			    unsigned int reg, unsigned int bitmask)
+{
+	(*counter)--;
+	if (*counter != 0)
+		/* There are more users, do not deactivate. */
+		return 0;
+
+	return regmap_update_bits(data->regmap, reg, bitmask, 0);
+}
+
+static int sx9500_inc_chan_users(struct sx9500_data *data, int chan)
+{
+	return sx9500_inc_users(data, &data->channel_users[chan],
+				SX9500_REG_PROX_CTRL0, BIT(chan));
+}
+
+static int sx9500_dec_chan_users(struct sx9500_data *data, int chan)
+{
+	return sx9500_dec_users(data, &data->channel_users[chan],
+				SX9500_REG_PROX_CTRL0, BIT(chan));
+}
+
+static int sx9500_inc_data_rdy_users(struct sx9500_data *data)
+{
+	return sx9500_inc_users(data, &data->data_rdy_users,
+				SX9500_REG_IRQ_MSK, SX9500_CONVDONE_IRQ);
+}
+
+static int sx9500_dec_data_rdy_users(struct sx9500_data *data)
+{
+	return sx9500_dec_users(data, &data->data_rdy_users,
+				SX9500_REG_IRQ_MSK, SX9500_CONVDONE_IRQ);
+}
+
+static int sx9500_inc_close_far_users(struct sx9500_data *data)
+{
+	return sx9500_inc_users(data, &data->close_far_users,
+				SX9500_REG_IRQ_MSK,
+				SX9500_CLOSE_IRQ | SX9500_FAR_IRQ);
+}
+
+static int sx9500_dec_close_far_users(struct sx9500_data *data)
+{
+	return sx9500_dec_users(data, &data->close_far_users,
+				SX9500_REG_IRQ_MSK,
+				SX9500_CLOSE_IRQ | SX9500_FAR_IRQ);
+}
+
+static int sx9500_read_prox_data(struct sx9500_data *data,
 				 const struct iio_chan_spec *chan,
 				 int *val)
 {
@@ -211,6 +286,79 @@ static int sx9500_read_proximity(struct sx9500_data *data,
 	return IIO_VAL_INT;
 }
 
+/*
+ * If we have no interrupt support, we have to wait for a scan period
+ * after enabling a channel to get a result.
+ */
+static int sx9500_wait_for_sample(struct sx9500_data *data)
+{
+	int ret;
+	unsigned int val;
+
+	ret = regmap_read(data->regmap, SX9500_REG_PROX_CTRL0, &val);
+	if (ret < 0)
+		return ret;
+
+	val = (val & SX9500_SCAN_PERIOD_MASK) >> SX9500_SCAN_PERIOD_SHIFT;
+
+	msleep(sx9500_scan_period_table[val]);
+
+	return 0;
+}
+
+static int sx9500_read_proximity(struct sx9500_data *data,
+				 const struct iio_chan_spec *chan,
+				 int *val)
+{
+	int ret;
+
+	mutex_lock(&data->mutex);
+
+	ret = sx9500_inc_chan_users(data, chan->channel);
+	if (ret < 0)
+		goto out;
+
+	ret = sx9500_inc_data_rdy_users(data);
+	if (ret < 0)
+		goto out_dec_chan;
+
+	mutex_unlock(&data->mutex);
+
+	if (data->client->irq > 0)
+		ret = wait_for_completion_interruptible(&data->completion);
+	else
+		ret = sx9500_wait_for_sample(data);
+
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&data->mutex);
+
+	ret = sx9500_read_prox_data(data, chan, val);
+	if (ret < 0)
+		goto out;
+
+	ret = sx9500_dec_chan_users(data, chan->channel);
+	if (ret < 0)
+		goto out;
+
+	ret = sx9500_dec_data_rdy_users(data);
+	if (ret < 0)
+		goto out;
+
+	ret = IIO_VAL_INT;
+
+	goto out;
+
+out_dec_chan:
+	sx9500_dec_chan_users(data, chan->channel);
+out:
+	mutex_unlock(&data->mutex);
+	reinit_completion(&data->completion);
+
+	return ret;
+}
+
 static int sx9500_read_samp_freq(struct sx9500_data *data,
 				 int *val, int *val2)
 {
@@ -236,7 +384,6 @@ static int sx9500_read_raw(struct iio_dev *indio_dev,
 			   int *val, int *val2, long mask)
 {
 	struct sx9500_data *data = iio_priv(indio_dev);
-	int ret;
 
 	switch (chan->type) {
 	case IIO_PROXIMITY:
@@ -244,10 +391,7 @@ static int sx9500_read_raw(struct iio_dev *indio_dev,
 		case IIO_CHAN_INFO_RAW:
 			if (iio_buffer_enabled(indio_dev))
 				return -EBUSY;
-			mutex_lock(&data->mutex);
-			ret = sx9500_read_proximity(data, chan, val);
-			mutex_unlock(&data->mutex);
-			return ret;
+			return sx9500_read_proximity(data, chan, val);
 		case IIO_CHAN_INFO_SAMP_FREQ:
 			return sx9500_read_samp_freq(data, val, val2);
 		default:
@@ -318,28 +462,16 @@ static irqreturn_t sx9500_irq_handler(int irq, void *private)
 	return IRQ_WAKE_THREAD;
 }
 
-static irqreturn_t sx9500_irq_thread_handler(int irq, void *private)
+static void sx9500_push_events(struct iio_dev *indio_dev)
 {
-	struct iio_dev *indio_dev = private;
-	struct sx9500_data *data = iio_priv(indio_dev);
 	int ret;
 	unsigned int val, chan;
-
-	mutex_lock(&data->mutex);
-
-	ret = regmap_read(data->regmap, SX9500_REG_IRQ_SRC, &val);
-	if (ret < 0) {
-		dev_err(&data->client->dev, "i2c transfer error in irq\n");
-		goto out;
-	}
-
-	if (!(val & (SX9500_CLOSE_IRQ | SX9500_FAR_IRQ)))
-		goto out;
+	struct sx9500_data *data = iio_priv(indio_dev);
 
 	ret = regmap_read(data->regmap, SX9500_REG_STAT, &val);
 	if (ret < 0) {
 		dev_err(&data->client->dev, "i2c transfer error in irq\n");
-		goto out;
+		return;
 	}
 
 	val >>= SX9500_PROXSTAT_SHIFT;
@@ -354,15 +486,34 @@ static irqreturn_t sx9500_irq_thread_handler(int irq, void *private)
 			/* No change on this channel. */
 			continue;
 
-		dir = new_prox ? IIO_EV_DIR_FALLING :
-			IIO_EV_DIR_RISING;
-		ev = IIO_UNMOD_EVENT_CODE(IIO_PROXIMITY,
-					  chan,
-					  IIO_EV_TYPE_THRESH,
-					  dir);
+		dir = new_prox ? IIO_EV_DIR_FALLING : IIO_EV_DIR_RISING;
+		ev = IIO_UNMOD_EVENT_CODE(IIO_PROXIMITY, chan,
+					  IIO_EV_TYPE_THRESH, dir);
 		iio_push_event(indio_dev, ev, iio_get_time_ns());
 		data->prox_stat[chan] = new_prox;
 	}
+}
+
+static irqreturn_t sx9500_irq_thread_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct sx9500_data *data = iio_priv(indio_dev);
+	int ret;
+	unsigned int val;
+
+	mutex_lock(&data->mutex);
+
+	ret = regmap_read(data->regmap, SX9500_REG_IRQ_SRC, &val);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "i2c transfer error in irq\n");
+		goto out;
+	}
+
+	if (val & (SX9500_CLOSE_IRQ | SX9500_FAR_IRQ))
+		sx9500_push_events(indio_dev);
+
+	if (val & SX9500_CONVDONE_IRQ)
+		complete_all(&data->completion);
 
 out:
 	mutex_unlock(&data->mutex);
@@ -391,9 +542,7 @@ static int sx9500_write_event_config(struct iio_dev *indio_dev,
 				     int state)
 {
 	struct sx9500_data *data = iio_priv(indio_dev);
-	int ret, i;
-	bool any_active = false;
-	unsigned int irqmask;
+	int ret;
 
 	if (chan->type != IIO_PROXIMITY || type != IIO_EV_TYPE_THRESH ||
 	    dir != IIO_EV_DIR_EITHER)
@@ -401,24 +550,32 @@ static int sx9500_write_event_config(struct iio_dev *indio_dev,
 
 	mutex_lock(&data->mutex);
 
-	data->event_enabled[chan->channel] = state;
+	if (state == 1) {
+		ret = sx9500_inc_chan_users(data, chan->channel);
+		if (ret < 0)
+			goto out_unlock;
+		ret = sx9500_inc_close_far_users(data);
+		if (ret < 0)
+			goto out_undo_chan;
+	} else {
+		ret = sx9500_dec_chan_users(data, chan->channel);
+		if (ret < 0)
+			goto out_unlock;
+		ret = sx9500_dec_close_far_users(data);
+		if (ret < 0)
+			goto out_undo_chan;
+	}
 
-	for (i = 0; i < SX9500_NUM_CHANNELS; i++)
-		if (data->event_enabled[i]) {
-			any_active = true;
-			break;
-		}
+	data->event_enabled[chan->channel] = state;
+	goto out_unlock;
 
-	irqmask = SX9500_CLOSE_IRQ | SX9500_FAR_IRQ;
-	if (any_active)
-		ret = regmap_update_bits(data->regmap, SX9500_REG_IRQ_MSK,
-					 irqmask, irqmask);
+out_undo_chan:
+	if (state == 1)
+		sx9500_dec_chan_users(data, chan->channel);
 	else
-		ret = regmap_update_bits(data->regmap, SX9500_REG_IRQ_MSK,
-					 irqmask, 0);
-
+		sx9500_inc_chan_users(data, chan->channel);
+out_unlock:
 	mutex_unlock(&data->mutex);
-
 	return ret;
 }
 
@@ -469,12 +626,16 @@ static int sx9500_set_trigger_state(struct iio_trigger *trig,
 
 	mutex_lock(&data->mutex);
 
-	ret = regmap_update_bits(data->regmap, SX9500_REG_IRQ_MSK,
-				 SX9500_CONVDONE_IRQ,
-				 state ? SX9500_CONVDONE_IRQ : 0);
-	if (ret == 0)
-		data->trigger_enabled = state;
+	if (state)
+		ret = sx9500_inc_data_rdy_users(data);
+	else
+		ret = sx9500_dec_data_rdy_users(data);
+	if (ret < 0)
+		goto out;
+
+	data->trigger_enabled = state;
 
+out:
 	mutex_unlock(&data->mutex);
 
 	return ret;
@@ -496,7 +657,7 @@ static irqreturn_t sx9500_trigger_handler(int irq, void *private)
 
 	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->masklength) {
-		ret = sx9500_read_proximity(data, &indio_dev->channels[bit],
+		ret = sx9500_read_prox_data(data, &indio_dev->channels[bit],
 					    &val);
 		if (ret < 0)
 			goto out;
@@ -515,6 +676,62 @@ out:
 	return IRQ_HANDLED;
 }
 
+static int sx9500_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct sx9500_data *data = iio_priv(indio_dev);
+	int ret, i;
+
+	mutex_lock(&data->mutex);
+
+	for (i = 0; i < SX9500_NUM_CHANNELS; i++)
+		if (test_bit(i, indio_dev->active_scan_mask)) {
+			ret = sx9500_inc_chan_users(data, i);
+			if (ret)
+				break;
+		}
+
+	if (ret)
+		for (i = i - 1; i >= 0; i--)
+			if (test_bit(i, indio_dev->active_scan_mask))
+				sx9500_dec_chan_users(data, i);
+
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static int sx9500_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct sx9500_data *data = iio_priv(indio_dev);
+	int ret, i;
+
+	iio_triggered_buffer_predisable(indio_dev);
+
+	mutex_lock(&data->mutex);
+
+	for (i = 0; i < SX9500_NUM_CHANNELS; i++)
+		if (test_bit(i, indio_dev->active_scan_mask)) {
+			ret = sx9500_dec_chan_users(data, i);
+			if (ret)
+				break;
+		}
+
+	if (ret)
+		for (i = i - 1; i >= 0; i--)
+			if (test_bit(i, indio_dev->active_scan_mask))
+				sx9500_inc_chan_users(data, i);
+
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static const struct iio_buffer_setup_ops sx9500_buffer_setup_ops = {
+	.preenable = sx9500_buffer_preenable,
+	.postenable = iio_triggered_buffer_postenable,
+	.predisable = sx9500_buffer_predisable,
+};
+
 struct sx9500_reg_default {
 	u8 reg;
 	u8 def;
@@ -570,17 +787,57 @@ static const struct sx9500_reg_default sx9500_default_regs[] = {
 	},
 	{
 		.reg = SX9500_REG_PROX_CTRL0,
-		/* Scan period: 30ms, all sensors enabled. */
-		.def = 0x0f,
+		/* Scan period: 30ms, all sensors disabled. */
+		.def = 0x00,
 	},
 };
 
+/* Activate all channels and perform an initial compensation. */
+static int sx9500_init_compensation(struct iio_dev *indio_dev)
+{
+	struct sx9500_data *data = iio_priv(indio_dev);
+	int i, ret;
+	unsigned int val;
+
+	ret = regmap_update_bits(data->regmap, SX9500_REG_PROX_CTRL0,
+				 GENMASK(SX9500_NUM_CHANNELS, 0),
+				 GENMASK(SX9500_NUM_CHANNELS, 0));
+	if (ret < 0)
+		return ret;
+
+	for (i = 10; i >= 0; i--) {
+		usleep_range(10000, 20000);
+		ret = regmap_read(data->regmap, SX9500_REG_STAT, &val);
+		if (ret < 0)
+			goto out;
+		if (!(val & SX9500_COMPSTAT_MASK))
+			break;
+	}
+
+	if (i < 0) {
+		dev_err(&data->client->dev, "initial compensation timed out");
+		ret = -ETIMEDOUT;
+	}
+
+out:
+	regmap_update_bits(data->regmap, SX9500_REG_PROX_CTRL0,
+			   GENMASK(SX9500_NUM_CHANNELS, 0), 0);
+	return ret;
+}
+
 static int sx9500_init_device(struct iio_dev *indio_dev)
 {
 	struct sx9500_data *data = iio_priv(indio_dev);
 	int ret, i;
 	unsigned int val;
 
+	if (data->gpiod_rst) {
+		gpiod_set_value_cansleep(data->gpiod_rst, 0);
+		usleep_range(1000, 2000);
+		gpiod_set_value_cansleep(data->gpiod_rst, 1);
+		usleep_range(1000, 2000);
+	}
+
 	ret = regmap_write(data->regmap, SX9500_REG_IRQ_MSK, 0);
 	if (ret < 0)
 		return ret;
@@ -602,33 +859,34 @@ static int sx9500_init_device(struct iio_dev *indio_dev)
 			return ret;
 	}
 
-	return 0;
+	return sx9500_init_compensation(indio_dev);
 }
 
-static int sx9500_gpio_probe(struct i2c_client *client,
-			     struct sx9500_data *data)
+static void sx9500_gpio_probe(struct i2c_client *client,
+			      struct sx9500_data *data)
 {
 	struct device *dev;
 	struct gpio_desc *gpio;
-	int ret;
 
 	if (!client)
-		return -EINVAL;
+		return;
 
 	dev = &client->dev;
 
-	/* data ready gpio interrupt pin */
-	gpio = devm_gpiod_get_index(dev, SX9500_GPIO_NAME, 0, GPIOD_IN);
-	if (IS_ERR(gpio)) {
-		dev_err(dev, "acpi gpio get index failed\n");
-		return PTR_ERR(gpio);
+	if (client->irq <= 0) {
+		gpio = devm_gpiod_get_index(dev, SX9500_GPIO_INT, 0, GPIOD_IN);
+		if (IS_ERR(gpio))
+			dev_err(dev, "gpio get irq failed\n");
+		else
+			client->irq = gpiod_to_irq(gpio);
 	}
 
-	ret = gpiod_to_irq(gpio);
-
-	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
-
-	return ret;
+	data->gpiod_rst = devm_gpiod_get_index(dev, SX9500_GPIO_RESET,
+					       0, GPIOD_OUT_HIGH);
+	if (IS_ERR(data->gpiod_rst)) {
+		dev_warn(dev, "gpio get reset pin failed\n");
+		data->gpiod_rst = NULL;
+	}
 }
 
 static int sx9500_probe(struct i2c_client *client,
@@ -645,14 +903,13 @@ static int sx9500_probe(struct i2c_client *client,
 	data = iio_priv(indio_dev);
 	data->client = client;
 	mutex_init(&data->mutex);
+	init_completion(&data->completion);
 	data->trigger_enabled = false;
 
 	data->regmap = devm_regmap_init_i2c(client, &sx9500_regmap_config);
 	if (IS_ERR(data->regmap))
 		return PTR_ERR(data->regmap);
 
-	sx9500_init_device(indio_dev);
-
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->name = SX9500_DRIVER_NAME;
 	indio_dev->channels = sx9500_channels;
@@ -661,10 +918,15 @@ static int sx9500_probe(struct i2c_client *client,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	i2c_set_clientdata(client, indio_dev);
 
-	if (client->irq <= 0)
-		client->irq = sx9500_gpio_probe(client, data);
+	sx9500_gpio_probe(client, data);
 
-	if (client->irq > 0) {
+	ret = sx9500_init_device(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	if (client->irq <= 0)
+		dev_warn(&client->dev, "no valid irq found\n");
+	else {
 		ret = devm_request_threaded_irq(&client->dev, client->irq,
 				sx9500_irq_handler, sx9500_irq_thread_handler,
 				IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
@@ -687,7 +949,8 @@ static int sx9500_probe(struct i2c_client *client,
 	}
 
 	ret = iio_triggered_buffer_setup(indio_dev, NULL,
-					 sx9500_trigger_handler, NULL);
+					 sx9500_trigger_handler,
+					 &sx9500_buffer_setup_ops);
 	if (ret < 0)
 		goto out_trigger_unregister;
 
@@ -720,6 +983,49 @@ static int sx9500_remove(struct i2c_client *client)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int sx9500_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct sx9500_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = regmap_read(data->regmap, SX9500_REG_PROX_CTRL0,
+			  &data->suspend_ctrl0);
+	if (ret < 0)
+		goto out;
+
+	/*
+	 * Scan period doesn't matter because when all the sensors are
+	 * deactivated the device is in sleep mode.
+	 */
+	ret = regmap_write(data->regmap, SX9500_REG_PROX_CTRL0, 0);
+
+out:
+	mutex_unlock(&data->mutex);
+	return ret;
+}
+
+static int sx9500_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct sx9500_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = regmap_write(data->regmap, SX9500_REG_PROX_CTRL0,
+			   data->suspend_ctrl0);
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static const struct dev_pm_ops sx9500_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(sx9500_suspend, sx9500_resume)
+};
+
 static const struct acpi_device_id sx9500_acpi_match[] = {
 	{"SSX9500", 0},
 	{ },
@@ -728,7 +1034,7 @@ MODULE_DEVICE_TABLE(acpi, sx9500_acpi_match);
 
 static const struct i2c_device_id sx9500_id[] = {
 	{"sx9500", 0},
-	{}
+	{ },
 };
 MODULE_DEVICE_TABLE(i2c, sx9500_id);
 
@@ -736,6 +1042,7 @@ static struct i2c_driver sx9500_driver = {
 	.driver = {
 		.name	= SX9500_DRIVER_NAME,
 		.acpi_match_table = ACPI_PTR(sx9500_acpi_match),
+		.pm = &sx9500_pm_ops,
 	},
 	.probe		= sx9500_probe,
 	.remove		= sx9500_remove,
diff --git a/drivers/iio/temperature/mlx90614.c b/drivers/iio/temperature/mlx90614.c
index a112fc9abf43..cb2e8ad8bfdc 100644
--- a/drivers/iio/temperature/mlx90614.c
+++ b/drivers/iio/temperature/mlx90614.c
@@ -12,12 +12,24 @@
  *
  * (7-bit I2C slave address 0x5a, 100KHz bus speed only!)
  *
- * TODO: sleep mode, configuration EEPROM
+ * To wake up from sleep mode, the SDA line must be held low while SCL is high
+ * for at least 33ms.  This is achieved with an extra GPIO that can be connected
+ * directly to the SDA line.  In normal operation, the GPIO is set as input and
+ * will not interfere in I2C communication.  While the GPIO is driven low, the
+ * i2c adapter is locked since it cannot be used by other clients.  The SCL line
+ * always has a pull-up so we do not need an extra GPIO to drive it high.  If
+ * the "wakeup" GPIO is not given, power management will be disabled.
+ *
+ * TODO: filter configuration
  */
 
 #include <linux/err.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/jiffies.h>
+#include <linux/gpio/consumer.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/iio/iio.h>
 
@@ -51,10 +63,101 @@
 #define MLX90614_TIMING_WAKEUP 34 /* time to hold SDA low for wake-up */
 #define MLX90614_TIMING_STARTUP 250 /* time before first data after wake-up */
 
+#define MLX90614_AUTOSLEEP_DELAY 5000 /* default autosleep delay */
+
 struct mlx90614_data {
 	struct i2c_client *client;
+	struct mutex lock; /* for EEPROM access only */
+	struct gpio_desc *wakeup_gpio; /* NULL to disable sleep/wake-up */
+	unsigned long ready_timestamp; /* in jiffies */
 };
 
+/*
+ * Erase an address and write word.
+ * The mutex must be locked before calling.
+ */
+static s32 mlx90614_write_word(const struct i2c_client *client, u8 command,
+			       u16 value)
+{
+	/*
+	 * Note: The mlx90614 requires a PEC on writing but does not send us a
+	 * valid PEC on reading.  Hence, we cannot set I2C_CLIENT_PEC in
+	 * i2c_client.flags.  As a workaround, we use i2c_smbus_xfer here.
+	 */
+	union i2c_smbus_data data;
+	s32 ret;
+
+	dev_dbg(&client->dev, "Writing 0x%x to address 0x%x", value, command);
+
+	data.word = 0x0000; /* erase command */
+	ret = i2c_smbus_xfer(client->adapter, client->addr,
+			     client->flags | I2C_CLIENT_PEC,
+			     I2C_SMBUS_WRITE, command,
+			     I2C_SMBUS_WORD_DATA, &data);
+	if (ret < 0)
+		return ret;
+
+	msleep(MLX90614_TIMING_EEPROM);
+
+	data.word = value; /* actual write */
+	ret = i2c_smbus_xfer(client->adapter, client->addr,
+			     client->flags | I2C_CLIENT_PEC,
+			     I2C_SMBUS_WRITE, command,
+			     I2C_SMBUS_WORD_DATA, &data);
+
+	msleep(MLX90614_TIMING_EEPROM);
+
+	return ret;
+}
+
+#ifdef CONFIG_PM
+/*
+ * If @startup is true, make sure MLX90614_TIMING_STARTUP ms have elapsed since
+ * the last wake-up.  This is normally only needed to get a valid temperature
+ * reading.  EEPROM access does not need such delay.
+ * Return 0 on success, <0 on error.
+ */
+static int mlx90614_power_get(struct mlx90614_data *data, bool startup)
+{
+	unsigned long now;
+
+	if (!data->wakeup_gpio)
+		return 0;
+
+	pm_runtime_get_sync(&data->client->dev);
+
+	if (startup) {
+		now = jiffies;
+		if (time_before(now, data->ready_timestamp) &&
+		    msleep_interruptible(jiffies_to_msecs(
+				data->ready_timestamp - now)) != 0) {
+			pm_runtime_put_autosuspend(&data->client->dev);
+			return -EINTR;
+		}
+	}
+
+	return 0;
+}
+
+static void mlx90614_power_put(struct mlx90614_data *data)
+{
+	if (!data->wakeup_gpio)
+		return;
+
+	pm_runtime_mark_last_busy(&data->client->dev);
+	pm_runtime_put_autosuspend(&data->client->dev);
+}
+#else
+static inline int mlx90614_power_get(struct mlx90614_data *data, bool startup)
+{
+	return 0;
+}
+
+static inline void mlx90614_power_put(struct mlx90614_data *data)
+{
+}
+#endif
+
 static int mlx90614_read_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *channel, int *val,
 			    int *val2, long mask)
@@ -85,9 +188,19 @@ static int mlx90614_read_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 		}
 
+		ret = mlx90614_power_get(data, true);
+		if (ret < 0)
+			return ret;
 		ret = i2c_smbus_read_word_data(data->client, cmd);
+		mlx90614_power_put(data);
+
 		if (ret < 0)
 			return ret;
+
+		/* MSB is an error flag */
+		if (ret & 0x8000)
+			return -EIO;
+
 		*val = ret;
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_OFFSET:
@@ -97,6 +210,63 @@ static int mlx90614_read_raw(struct iio_dev *indio_dev,
 	case IIO_CHAN_INFO_SCALE:
 		*val = 20;
 		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_CALIBEMISSIVITY: /* 1/65535 / LSB */
+		mlx90614_power_get(data, false);
+		mutex_lock(&data->lock);
+		ret = i2c_smbus_read_word_data(data->client,
+					       MLX90614_EMISSIVITY);
+		mutex_unlock(&data->lock);
+		mlx90614_power_put(data);
+
+		if (ret < 0)
+			return ret;
+
+		if (ret == 65535) {
+			*val = 1;
+			*val2 = 0;
+		} else {
+			*val = 0;
+			*val2 = ret * 15259; /* 1/65535 ~ 0.000015259 */
+		}
+		return IIO_VAL_INT_PLUS_NANO;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mlx90614_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *channel, int val,
+			     int val2, long mask)
+{
+	struct mlx90614_data *data = iio_priv(indio_dev);
+	s32 ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_CALIBEMISSIVITY: /* 1/65535 / LSB */
+		if (val < 0 || val2 < 0 || val > 1 || (val == 1 && val2 != 0))
+			return -EINVAL;
+		val = val * 65535 + val2 / 15259; /* 1/65535 ~ 0.000015259 */
+
+		mlx90614_power_get(data, false);
+		mutex_lock(&data->lock);
+		ret = mlx90614_write_word(data->client, MLX90614_EMISSIVITY,
+					  val);
+		mutex_unlock(&data->lock);
+		mlx90614_power_put(data);
+
+		return ret;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mlx90614_write_raw_get_fmt(struct iio_dev *indio_dev,
+				     struct iio_chan_spec const *channel,
+				     long mask)
+{
+	switch (mask) {
+	case IIO_CHAN_INFO_CALIBEMISSIVITY:
+		return IIO_VAL_INT_PLUS_NANO;
 	default:
 		return -EINVAL;
 	}
@@ -115,7 +285,8 @@ static const struct iio_chan_spec mlx90614_channels[] = {
 		.type = IIO_TEMP,
 		.modified = 1,
 		.channel2 = IIO_MOD_TEMP_OBJECT,
-		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		    BIT(IIO_CHAN_INFO_CALIBEMISSIVITY),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		    BIT(IIO_CHAN_INFO_SCALE),
 	},
@@ -125,7 +296,8 @@ static const struct iio_chan_spec mlx90614_channels[] = {
 		.modified = 1,
 		.channel = 1,
 		.channel2 = IIO_MOD_TEMP_OBJECT,
-		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		    BIT(IIO_CHAN_INFO_CALIBEMISSIVITY),
 		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
 		    BIT(IIO_CHAN_INFO_SCALE),
 	},
@@ -133,9 +305,103 @@ static const struct iio_chan_spec mlx90614_channels[] = {
 
 static const struct iio_info mlx90614_info = {
 	.read_raw = mlx90614_read_raw,
+	.write_raw = mlx90614_write_raw,
+	.write_raw_get_fmt = mlx90614_write_raw_get_fmt,
 	.driver_module = THIS_MODULE,
 };
 
+#ifdef CONFIG_PM
+static int mlx90614_sleep(struct mlx90614_data *data)
+{
+	s32 ret;
+
+	if (!data->wakeup_gpio) {
+		dev_dbg(&data->client->dev, "Sleep disabled");
+		return -ENOSYS;
+	}
+
+	dev_dbg(&data->client->dev, "Requesting sleep");
+
+	mutex_lock(&data->lock);
+	ret = i2c_smbus_xfer(data->client->adapter, data->client->addr,
+			     data->client->flags | I2C_CLIENT_PEC,
+			     I2C_SMBUS_WRITE, MLX90614_OP_SLEEP,
+			     I2C_SMBUS_BYTE, NULL);
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static int mlx90614_wakeup(struct mlx90614_data *data)
+{
+	if (!data->wakeup_gpio) {
+		dev_dbg(&data->client->dev, "Wake-up disabled");
+		return -ENOSYS;
+	}
+
+	dev_dbg(&data->client->dev, "Requesting wake-up");
+
+	i2c_lock_adapter(data->client->adapter);
+	gpiod_direction_output(data->wakeup_gpio, 0);
+	msleep(MLX90614_TIMING_WAKEUP);
+	gpiod_direction_input(data->wakeup_gpio);
+	i2c_unlock_adapter(data->client->adapter);
+
+	data->ready_timestamp = jiffies +
+			msecs_to_jiffies(MLX90614_TIMING_STARTUP);
+
+	/*
+	 * Quirk: the i2c controller may get confused right after the
+	 * wake-up signal has been sent.  As a workaround, do a dummy read.
+	 * If the read fails, the controller will probably be reset so that
+	 * further reads will work.
+	 */
+	i2c_smbus_read_word_data(data->client, MLX90614_CONFIG);
+
+	return 0;
+}
+
+/* Return wake-up GPIO or NULL if sleep functionality should be disabled. */
+static struct gpio_desc *mlx90614_probe_wakeup(struct i2c_client *client)
+{
+	struct gpio_desc *gpio;
+
+	if (!i2c_check_functionality(client->adapter,
+						I2C_FUNC_SMBUS_WRITE_BYTE)) {
+		dev_info(&client->dev,
+			 "i2c adapter does not support SMBUS_WRITE_BYTE, sleep disabled");
+		return NULL;
+	}
+
+	gpio = devm_gpiod_get_optional(&client->dev, "wakeup", GPIOD_IN);
+
+	if (IS_ERR(gpio)) {
+		dev_warn(&client->dev,
+			 "gpio acquisition failed with error %ld, sleep disabled",
+			 PTR_ERR(gpio));
+		return NULL;
+	} else if (!gpio) {
+		dev_info(&client->dev,
+			 "wakeup-gpio not found, sleep disabled");
+	}
+
+	return gpio;
+}
+#else
+static inline int mlx90614_sleep(struct mlx90614_data *data)
+{
+	return -ENOSYS;
+}
+static inline int mlx90614_wakeup(struct mlx90614_data *data)
+{
+	return -ENOSYS;
+}
+static inline struct gpio_desc *mlx90614_probe_wakeup(struct i2c_client *client)
+{
+	return NULL;
+}
+#endif
+
 /* Return 0 for single sensor, 1 for dual sensor, <0 on error. */
 static int mlx90614_probe_num_ir_sensors(struct i2c_client *client)
 {
@@ -166,6 +432,10 @@ static int mlx90614_probe(struct i2c_client *client,
 	data = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
+	mutex_init(&data->lock);
+	data->wakeup_gpio = mlx90614_probe_wakeup(client);
+
+	mlx90614_wakeup(data);
 
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->name = id->name;
@@ -188,12 +458,30 @@ static int mlx90614_probe(struct i2c_client *client,
 		return ret;
 	}
 
+	if (data->wakeup_gpio) {
+		pm_runtime_set_autosuspend_delay(&client->dev,
+						 MLX90614_AUTOSLEEP_DELAY);
+		pm_runtime_use_autosuspend(&client->dev);
+		pm_runtime_set_active(&client->dev);
+		pm_runtime_enable(&client->dev);
+	}
+
 	return iio_device_register(indio_dev);
 }
 
 static int mlx90614_remove(struct i2c_client *client)
 {
-	iio_device_unregister(i2c_get_clientdata(client));
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct mlx90614_data *data = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	if (data->wakeup_gpio) {
+		pm_runtime_disable(&client->dev);
+		if (!pm_runtime_status_suspended(&client->dev))
+			mlx90614_sleep(data);
+		pm_runtime_set_suspended(&client->dev);
+	}
 
 	return 0;
 }
@@ -204,10 +492,67 @@ static const struct i2c_device_id mlx90614_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, mlx90614_id);
 
+#ifdef CONFIG_PM_SLEEP
+static int mlx90614_pm_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct mlx90614_data *data = iio_priv(indio_dev);
+
+	if (data->wakeup_gpio && pm_runtime_active(dev))
+		return mlx90614_sleep(data);
+
+	return 0;
+}
+
+static int mlx90614_pm_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct mlx90614_data *data = iio_priv(indio_dev);
+	int err;
+
+	if (data->wakeup_gpio) {
+		err = mlx90614_wakeup(data);
+		if (err < 0)
+			return err;
+
+		pm_runtime_disable(dev);
+		pm_runtime_set_active(dev);
+		pm_runtime_enable(dev);
+	}
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_PM
+static int mlx90614_pm_runtime_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct mlx90614_data *data = iio_priv(indio_dev);
+
+	return mlx90614_sleep(data);
+}
+
+static int mlx90614_pm_runtime_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct mlx90614_data *data = iio_priv(indio_dev);
+
+	return mlx90614_wakeup(data);
+}
+#endif
+
+static const struct dev_pm_ops mlx90614_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mlx90614_pm_suspend, mlx90614_pm_resume)
+	SET_RUNTIME_PM_OPS(mlx90614_pm_runtime_suspend,
+			   mlx90614_pm_runtime_resume, NULL)
+};
+
 static struct i2c_driver mlx90614_driver = {
 	.driver = {
 		.name	= "mlx90614",
 		.owner	= THIS_MODULE,
+		.pm	= &mlx90614_pm_ops,
 	},
 	.probe = mlx90614_probe,
 	.remove = mlx90614_remove,
diff --git a/drivers/iio/temperature/tmp006.c b/drivers/iio/temperature/tmp006.c
index 84a0789c3d96..fcc49f89b946 100644
--- a/drivers/iio/temperature/tmp006.c
+++ b/drivers/iio/temperature/tmp006.c
@@ -41,8 +41,8 @@
 #define TMP006_CONFIG_CR_MASK 0x0e00
 #define TMP006_CONFIG_CR_SHIFT 9
 
-#define MANUFACTURER_MAGIC 0x5449
-#define DEVICE_MAGIC 0x0067
+#define TMP006_MANUFACTURER_MAGIC 0x5449
+#define TMP006_DEVICE_MAGIC 0x0067
 
 struct tmp006_data {
 	struct i2c_client *client;
@@ -191,7 +191,7 @@ static bool tmp006_check_identification(struct i2c_client *client)
 	if (did < 0)
 		return false;
 
-	return mid == MANUFACTURER_MAGIC && did == DEVICE_MAGIC;
+	return mid == TMP006_MANUFACTURER_MAGIC && did == TMP006_DEVICE_MAGIC;
 }
 
 static int tmp006_probe(struct i2c_client *client,
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index 38339d220d7f..746cdf56bc76 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -457,8 +457,8 @@ static void resolve_cb(int status, struct sockaddr *src_addr,
 	complete(&((struct resolve_cb_context *)context)->comp);
 }
 
-int rdma_addr_find_dmac_by_grh(union ib_gid *sgid, union ib_gid *dgid, u8 *dmac,
-			       u16 *vlan_id)
+int rdma_addr_find_dmac_by_grh(const union ib_gid *sgid, const union ib_gid *dgid,
+			       u8 *dmac, u16 *vlan_id)
 {
 	int ret = 0;
 	struct rdma_dev_addr dev_addr;
diff --git a/drivers/infiniband/core/agent.c b/drivers/infiniband/core/agent.c
index f6d29614cb01..c7dcfe4ca5f1 100644
--- a/drivers/infiniband/core/agent.c
+++ b/drivers/infiniband/core/agent.c
@@ -54,7 +54,7 @@ static DEFINE_SPINLOCK(ib_agent_port_list_lock);
 static LIST_HEAD(ib_agent_port_list);
 
 static struct ib_agent_port_private *
-__ib_get_agent_port(struct ib_device *device, int port_num)
+__ib_get_agent_port(const struct ib_device *device, int port_num)
 {
 	struct ib_agent_port_private *entry;
 
@@ -67,7 +67,7 @@ __ib_get_agent_port(struct ib_device *device, int port_num)
 }
 
 static struct ib_agent_port_private *
-ib_get_agent_port(struct ib_device *device, int port_num)
+ib_get_agent_port(const struct ib_device *device, int port_num)
 {
 	struct ib_agent_port_private *entry;
 	unsigned long flags;
@@ -78,9 +78,9 @@ ib_get_agent_port(struct ib_device *device, int port_num)
 	return entry;
 }
 
-void agent_send_response(struct ib_mad *mad, struct ib_grh *grh,
-			 struct ib_wc *wc, struct ib_device *device,
-			 int port_num, int qpn)
+void agent_send_response(const struct ib_mad_hdr *mad_hdr, const struct ib_grh *grh,
+			 const struct ib_wc *wc, const struct ib_device *device,
+			 int port_num, int qpn, size_t resp_mad_len, bool opa)
 {
 	struct ib_agent_port_private *port_priv;
 	struct ib_mad_agent *agent;
@@ -106,15 +106,20 @@ void agent_send_response(struct ib_mad *mad, struct ib_grh *grh,
 		return;
 	}
 
+	if (opa && mad_hdr->base_version != OPA_MGMT_BASE_VERSION)
+		resp_mad_len = IB_MGMT_MAD_SIZE;
+
 	send_buf = ib_create_send_mad(agent, wc->src_qp, wc->pkey_index, 0,
-				      IB_MGMT_MAD_HDR, IB_MGMT_MAD_DATA,
-				      GFP_KERNEL);
+				      IB_MGMT_MAD_HDR,
+				      resp_mad_len - IB_MGMT_MAD_HDR,
+				      GFP_KERNEL,
+				      mad_hdr->base_version);
 	if (IS_ERR(send_buf)) {
 		dev_err(&device->dev, "ib_create_send_mad error\n");
 		goto err1;
 	}
 
-	memcpy(send_buf->mad, mad, sizeof *mad);
+	memcpy(send_buf->mad, mad_hdr, resp_mad_len);
 	send_buf->ah = ah;
 
 	if (device->node_type == RDMA_NODE_IB_SWITCH) {
@@ -156,7 +161,7 @@ int ib_agent_port_open(struct ib_device *device, int port_num)
 		goto error1;
 	}
 
-	if (rdma_port_get_link_layer(device, port_num) == IB_LINK_LAYER_INFINIBAND) {
+	if (rdma_cap_ib_smi(device, port_num)) {
 		/* Obtain send only MAD agent for SMI QP */
 		port_priv->agent[0] = ib_register_mad_agent(device, port_num,
 							    IB_QPT_SMI, NULL, 0,
diff --git a/drivers/infiniband/core/agent.h b/drivers/infiniband/core/agent.h
index 6669287009c2..65f92bedae44 100644
--- a/drivers/infiniband/core/agent.h
+++ b/drivers/infiniband/core/agent.h
@@ -44,8 +44,8 @@ extern int ib_agent_port_open(struct ib_device *device, int port_num);
 
 extern int ib_agent_port_close(struct ib_device *device, int port_num);
 
-extern void agent_send_response(struct ib_mad *mad, struct ib_grh *grh,
-				struct ib_wc *wc, struct ib_device *device,
-				int port_num, int qpn);
+extern void agent_send_response(const struct ib_mad_hdr *mad_hdr, const struct ib_grh *grh,
+				const struct ib_wc *wc, const struct ib_device *device,
+				int port_num, int qpn, size_t resp_mad_len, bool opa);
 
 #endif	/* __AGENT_H_ */
diff --git a/drivers/infiniband/core/cache.c b/drivers/infiniband/core/cache.c
index 80f6cf2449fb..871da832d016 100644
--- a/drivers/infiniband/core/cache.c
+++ b/drivers/infiniband/core/cache.c
@@ -58,17 +58,6 @@ struct ib_update_work {
 	u8                 port_num;
 };
 
-static inline int start_port(struct ib_device *device)
-{
-	return (device->node_type == RDMA_NODE_IB_SWITCH) ? 0 : 1;
-}
-
-static inline int end_port(struct ib_device *device)
-{
-	return (device->node_type == RDMA_NODE_IB_SWITCH) ?
-		0 : device->phys_port_cnt;
-}
-
 int ib_get_cached_gid(struct ib_device *device,
 		      u8                port_num,
 		      int               index,
@@ -78,12 +67,12 @@ int ib_get_cached_gid(struct ib_device *device,
 	unsigned long flags;
 	int ret = 0;
 
-	if (port_num < start_port(device) || port_num > end_port(device))
+	if (port_num < rdma_start_port(device) || port_num > rdma_end_port(device))
 		return -EINVAL;
 
 	read_lock_irqsave(&device->cache.lock, flags);
 
-	cache = device->cache.gid_cache[port_num - start_port(device)];
+	cache = device->cache.gid_cache[port_num - rdma_start_port(device)];
 
 	if (index < 0 || index >= cache->table_len)
 		ret = -EINVAL;
@@ -96,10 +85,10 @@ int ib_get_cached_gid(struct ib_device *device,
 }
 EXPORT_SYMBOL(ib_get_cached_gid);
 
-int ib_find_cached_gid(struct ib_device *device,
-		       union ib_gid	*gid,
-		       u8               *port_num,
-		       u16              *index)
+int ib_find_cached_gid(struct ib_device   *device,
+		       const union ib_gid *gid,
+		       u8                 *port_num,
+		       u16                *index)
 {
 	struct ib_gid_cache *cache;
 	unsigned long flags;
@@ -112,11 +101,11 @@ int ib_find_cached_gid(struct ib_device *device,
 
 	read_lock_irqsave(&device->cache.lock, flags);
 
-	for (p = 0; p <= end_port(device) - start_port(device); ++p) {
+	for (p = 0; p <= rdma_end_port(device) - rdma_start_port(device); ++p) {
 		cache = device->cache.gid_cache[p];
 		for (i = 0; i < cache->table_len; ++i) {
 			if (!memcmp(gid, &cache->table[i], sizeof *gid)) {
-				*port_num = p + start_port(device);
+				*port_num = p + rdma_start_port(device);
 				if (index)
 					*index = i;
 				ret = 0;
@@ -140,12 +129,12 @@ int ib_get_cached_pkey(struct ib_device *device,
 	unsigned long flags;
 	int ret = 0;
 
-	if (port_num < start_port(device) || port_num > end_port(device))
+	if (port_num < rdma_start_port(device) || port_num > rdma_end_port(device))
 		return -EINVAL;
 
 	read_lock_irqsave(&device->cache.lock, flags);
 
-	cache = device->cache.pkey_cache[port_num - start_port(device)];
+	cache = device->cache.pkey_cache[port_num - rdma_start_port(device)];
 
 	if (index < 0 || index >= cache->table_len)
 		ret = -EINVAL;
@@ -169,12 +158,12 @@ int ib_find_cached_pkey(struct ib_device *device,
 	int ret = -ENOENT;
 	int partial_ix = -1;
 
-	if (port_num < start_port(device) || port_num > end_port(device))
+	if (port_num < rdma_start_port(device) || port_num > rdma_end_port(device))
 		return -EINVAL;
 
 	read_lock_irqsave(&device->cache.lock, flags);
 
-	cache = device->cache.pkey_cache[port_num - start_port(device)];
+	cache = device->cache.pkey_cache[port_num - rdma_start_port(device)];
 
 	*index = -1;
 
@@ -209,12 +198,12 @@ int ib_find_exact_cached_pkey(struct ib_device *device,
 	int i;
 	int ret = -ENOENT;
 
-	if (port_num < start_port(device) || port_num > end_port(device))
+	if (port_num < rdma_start_port(device) || port_num > rdma_end_port(device))
 		return -EINVAL;
 
 	read_lock_irqsave(&device->cache.lock, flags);
 
-	cache = device->cache.pkey_cache[port_num - start_port(device)];
+	cache = device->cache.pkey_cache[port_num - rdma_start_port(device)];
 
 	*index = -1;
 
@@ -238,11 +227,11 @@ int ib_get_cached_lmc(struct ib_device *device,
 	unsigned long flags;
 	int ret = 0;
 
-	if (port_num < start_port(device) || port_num > end_port(device))
+	if (port_num < rdma_start_port(device) || port_num > rdma_end_port(device))
 		return -EINVAL;
 
 	read_lock_irqsave(&device->cache.lock, flags);
-	*lmc = device->cache.lmc_cache[port_num - start_port(device)];
+	*lmc = device->cache.lmc_cache[port_num - rdma_start_port(device)];
 	read_unlock_irqrestore(&device->cache.lock, flags);
 
 	return ret;
@@ -303,13 +292,13 @@ static void ib_cache_update(struct ib_device *device,
 
 	write_lock_irq(&device->cache.lock);
 
-	old_pkey_cache = device->cache.pkey_cache[port - start_port(device)];
-	old_gid_cache  = device->cache.gid_cache [port - start_port(device)];
+	old_pkey_cache = device->cache.pkey_cache[port - rdma_start_port(device)];
+	old_gid_cache  = device->cache.gid_cache [port - rdma_start_port(device)];
 
-	device->cache.pkey_cache[port - start_port(device)] = pkey_cache;
-	device->cache.gid_cache [port - start_port(device)] = gid_cache;
+	device->cache.pkey_cache[port - rdma_start_port(device)] = pkey_cache;
+	device->cache.gid_cache [port - rdma_start_port(device)] = gid_cache;
 
-	device->cache.lmc_cache[port - start_port(device)] = tprops->lmc;
+	device->cache.lmc_cache[port - rdma_start_port(device)] = tprops->lmc;
 
 	write_unlock_irq(&device->cache.lock);
 
@@ -363,14 +352,14 @@ static void ib_cache_setup_one(struct ib_device *device)
 
 	device->cache.pkey_cache =
 		kmalloc(sizeof *device->cache.pkey_cache *
-			(end_port(device) - start_port(device) + 1), GFP_KERNEL);
+			(rdma_end_port(device) - rdma_start_port(device) + 1), GFP_KERNEL);
 	device->cache.gid_cache =
 		kmalloc(sizeof *device->cache.gid_cache *
-			(end_port(device) - start_port(device) + 1), GFP_KERNEL);
+			(rdma_end_port(device) - rdma_start_port(device) + 1), GFP_KERNEL);
 
 	device->cache.lmc_cache = kmalloc(sizeof *device->cache.lmc_cache *
-					  (end_port(device) -
-					   start_port(device) + 1),
+					  (rdma_end_port(device) -
+					   rdma_start_port(device) + 1),
 					  GFP_KERNEL);
 
 	if (!device->cache.pkey_cache || !device->cache.gid_cache ||
@@ -380,10 +369,10 @@ static void ib_cache_setup_one(struct ib_device *device)
 		goto err;
 	}
 
-	for (p = 0; p <= end_port(device) - start_port(device); ++p) {
+	for (p = 0; p <= rdma_end_port(device) - rdma_start_port(device); ++p) {
 		device->cache.pkey_cache[p] = NULL;
 		device->cache.gid_cache [p] = NULL;
-		ib_cache_update(device, p + start_port(device));
+		ib_cache_update(device, p + rdma_start_port(device));
 	}
 
 	INIT_IB_EVENT_HANDLER(&device->cache.event_handler,
@@ -394,7 +383,7 @@ static void ib_cache_setup_one(struct ib_device *device)
 	return;
 
 err_cache:
-	for (p = 0; p <= end_port(device) - start_port(device); ++p) {
+	for (p = 0; p <= rdma_end_port(device) - rdma_start_port(device); ++p) {
 		kfree(device->cache.pkey_cache[p]);
 		kfree(device->cache.gid_cache[p]);
 	}
@@ -412,7 +401,7 @@ static void ib_cache_cleanup_one(struct ib_device *device)
 	ib_unregister_event_handler(&device->cache.event_handler);
 	flush_workqueue(ib_wq);
 
-	for (p = 0; p <= end_port(device) - start_port(device); ++p) {
+	for (p = 0; p <= rdma_end_port(device) - rdma_start_port(device); ++p) {
 		kfree(device->cache.pkey_cache[p]);
 		kfree(device->cache.gid_cache[p]);
 	}
diff --git a/drivers/infiniband/core/cm.c b/drivers/infiniband/core/cm.c
index 0271608a51c4..dbddddd6fb5d 100644
--- a/drivers/infiniband/core/cm.c
+++ b/drivers/infiniband/core/cm.c
@@ -267,7 +267,8 @@ static int cm_alloc_msg(struct cm_id_private *cm_id_priv,
 	m = ib_create_send_mad(mad_agent, cm_id_priv->id.remote_cm_qpn,
 			       cm_id_priv->av.pkey_index,
 			       0, IB_MGMT_MAD_HDR, IB_MGMT_MAD_DATA,
-			       GFP_ATOMIC);
+			       GFP_ATOMIC,
+			       IB_MGMT_BASE_VERSION);
 	if (IS_ERR(m)) {
 		ib_destroy_ah(ah);
 		return PTR_ERR(m);
@@ -297,7 +298,8 @@ static int cm_alloc_response_msg(struct cm_port *port,
 
 	m = ib_create_send_mad(port->mad_agent, 1, mad_recv_wc->wc->pkey_index,
 			       0, IB_MGMT_MAD_HDR, IB_MGMT_MAD_DATA,
-			       GFP_ATOMIC);
+			       GFP_ATOMIC,
+			       IB_MGMT_BASE_VERSION);
 	if (IS_ERR(m)) {
 		ib_destroy_ah(ah);
 		return PTR_ERR(m);
@@ -3759,11 +3761,9 @@ static void cm_add_one(struct ib_device *ib_device)
 	};
 	unsigned long flags;
 	int ret;
+	int count = 0;
 	u8 i;
 
-	if (rdma_node_get_transport(ib_device->node_type) != RDMA_TRANSPORT_IB)
-		return;
-
 	cm_dev = kzalloc(sizeof(*cm_dev) + sizeof(*port) *
 			 ib_device->phys_port_cnt, GFP_KERNEL);
 	if (!cm_dev)
@@ -3782,6 +3782,9 @@ static void cm_add_one(struct ib_device *ib_device)
 
 	set_bit(IB_MGMT_METHOD_SEND, reg_req.method_mask);
 	for (i = 1; i <= ib_device->phys_port_cnt; i++) {
+		if (!rdma_cap_ib_cm(ib_device, i))
+			continue;
+
 		port = kzalloc(sizeof *port, GFP_KERNEL);
 		if (!port)
 			goto error1;
@@ -3808,7 +3811,13 @@ static void cm_add_one(struct ib_device *ib_device)
 		ret = ib_modify_port(ib_device, i, 0, &port_modify);
 		if (ret)
 			goto error3;
+
+		count++;
 	}
+
+	if (!count)
+		goto free;
+
 	ib_set_client_data(ib_device, &cm_client, cm_dev);
 
 	write_lock_irqsave(&cm.device_lock, flags);
@@ -3824,11 +3833,15 @@ error1:
 	port_modify.set_port_cap_mask = 0;
 	port_modify.clr_port_cap_mask = IB_PORT_CM_SUP;
 	while (--i) {
+		if (!rdma_cap_ib_cm(ib_device, i))
+			continue;
+
 		port = cm_dev->port[i-1];
 		ib_modify_port(ib_device, port->port_num, 0, &port_modify);
 		ib_unregister_mad_agent(port->mad_agent);
 		cm_remove_port_fs(port);
 	}
+free:
 	device_unregister(cm_dev->device);
 	kfree(cm_dev);
 }
@@ -3852,6 +3865,9 @@ static void cm_remove_one(struct ib_device *ib_device)
 	write_unlock_irqrestore(&cm.device_lock, flags);
 
 	for (i = 1; i <= ib_device->phys_port_cnt; i++) {
+		if (!rdma_cap_ib_cm(ib_device, i))
+			continue;
+
 		port = cm_dev->port[i-1];
 		ib_modify_port(ib_device, port->port_num, 0, &port_modify);
 		ib_unregister_mad_agent(port->mad_agent);
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index 38ffe0981503..143ded2bbe7c 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -65,6 +65,34 @@ MODULE_LICENSE("Dual BSD/GPL");
 #define CMA_CM_MRA_SETTING (IB_CM_MRA_FLAG_DELAY | 24)
 #define CMA_IBOE_PACKET_LIFETIME 18
 
+static const char * const cma_events[] = {
+	[RDMA_CM_EVENT_ADDR_RESOLVED]	 = "address resolved",
+	[RDMA_CM_EVENT_ADDR_ERROR]	 = "address error",
+	[RDMA_CM_EVENT_ROUTE_RESOLVED]	 = "route resolved ",
+	[RDMA_CM_EVENT_ROUTE_ERROR]	 = "route error",
+	[RDMA_CM_EVENT_CONNECT_REQUEST]	 = "connect request",
+	[RDMA_CM_EVENT_CONNECT_RESPONSE] = "connect response",
+	[RDMA_CM_EVENT_CONNECT_ERROR]	 = "connect error",
+	[RDMA_CM_EVENT_UNREACHABLE]	 = "unreachable",
+	[RDMA_CM_EVENT_REJECTED]	 = "rejected",
+	[RDMA_CM_EVENT_ESTABLISHED]	 = "established",
+	[RDMA_CM_EVENT_DISCONNECTED]	 = "disconnected",
+	[RDMA_CM_EVENT_DEVICE_REMOVAL]	 = "device removal",
+	[RDMA_CM_EVENT_MULTICAST_JOIN]	 = "multicast join",
+	[RDMA_CM_EVENT_MULTICAST_ERROR]	 = "multicast error",
+	[RDMA_CM_EVENT_ADDR_CHANGE]	 = "address change",
+	[RDMA_CM_EVENT_TIMEWAIT_EXIT]	 = "timewait exit",
+};
+
+const char *rdma_event_msg(enum rdma_cm_event_type event)
+{
+	size_t index = event;
+
+	return (index < ARRAY_SIZE(cma_events) && cma_events[index]) ?
+			cma_events[index] : "unrecognized event";
+}
+EXPORT_SYMBOL(rdma_event_msg);
+
 static void cma_add_one(struct ib_device *device);
 static void cma_remove_one(struct ib_device *device);
 
@@ -349,18 +377,35 @@ static int cma_translate_addr(struct sockaddr *addr, struct rdma_dev_addr *dev_a
 	return ret;
 }
 
+static inline int cma_validate_port(struct ib_device *device, u8 port,
+				      union ib_gid *gid, int dev_type)
+{
+	u8 found_port;
+	int ret = -ENODEV;
+
+	if ((dev_type == ARPHRD_INFINIBAND) && !rdma_protocol_ib(device, port))
+		return ret;
+
+	if ((dev_type != ARPHRD_INFINIBAND) && rdma_protocol_ib(device, port))
+		return ret;
+
+	ret = ib_find_cached_gid(device, gid, &found_port, NULL);
+	if (port != found_port)
+		return -ENODEV;
+
+	return ret;
+}
+
 static int cma_acquire_dev(struct rdma_id_private *id_priv,
 			   struct rdma_id_private *listen_id_priv)
 {
 	struct rdma_dev_addr *dev_addr = &id_priv->id.route.addr.dev_addr;
 	struct cma_device *cma_dev;
-	union ib_gid gid, iboe_gid;
+	union ib_gid gid, iboe_gid, *gidp;
 	int ret = -ENODEV;
-	u8 port, found_port;
-	enum rdma_link_layer dev_ll = dev_addr->dev_type == ARPHRD_INFINIBAND ?
-		IB_LINK_LAYER_INFINIBAND : IB_LINK_LAYER_ETHERNET;
+	u8 port;
 
-	if (dev_ll != IB_LINK_LAYER_INFINIBAND &&
+	if (dev_addr->dev_type != ARPHRD_INFINIBAND &&
 	    id_priv->id.ps == RDMA_PS_IPOIB)
 		return -EINVAL;
 
@@ -370,41 +415,36 @@ static int cma_acquire_dev(struct rdma_id_private *id_priv,
 
 	memcpy(&gid, dev_addr->src_dev_addr +
 	       rdma_addr_gid_offset(dev_addr), sizeof gid);
-	if (listen_id_priv &&
-	    rdma_port_get_link_layer(listen_id_priv->id.device,
-				     listen_id_priv->id.port_num) == dev_ll) {
+
+	if (listen_id_priv) {
 		cma_dev = listen_id_priv->cma_dev;
 		port = listen_id_priv->id.port_num;
-		if (rdma_node_get_transport(cma_dev->device->node_type) == RDMA_TRANSPORT_IB &&
-		    rdma_port_get_link_layer(cma_dev->device, port) == IB_LINK_LAYER_ETHERNET)
-			ret = ib_find_cached_gid(cma_dev->device, &iboe_gid,
-						 &found_port, NULL);
-		else
-			ret = ib_find_cached_gid(cma_dev->device, &gid,
-						 &found_port, NULL);
+		gidp = rdma_protocol_roce(cma_dev->device, port) ?
+		       &iboe_gid : &gid;
 
-		if (!ret && (port  == found_port)) {
-			id_priv->id.port_num = found_port;
+		ret = cma_validate_port(cma_dev->device, port, gidp,
+					dev_addr->dev_type);
+		if (!ret) {
+			id_priv->id.port_num = port;
 			goto out;
 		}
 	}
+
 	list_for_each_entry(cma_dev, &dev_list, list) {
 		for (port = 1; port <= cma_dev->device->phys_port_cnt; ++port) {
 			if (listen_id_priv &&
 			    listen_id_priv->cma_dev == cma_dev &&
 			    listen_id_priv->id.port_num == port)
 				continue;
-			if (rdma_port_get_link_layer(cma_dev->device, port) == dev_ll) {
-				if (rdma_node_get_transport(cma_dev->device->node_type) == RDMA_TRANSPORT_IB &&
-				    rdma_port_get_link_layer(cma_dev->device, port) == IB_LINK_LAYER_ETHERNET)
-					ret = ib_find_cached_gid(cma_dev->device, &iboe_gid, &found_port, NULL);
-				else
-					ret = ib_find_cached_gid(cma_dev->device, &gid, &found_port, NULL);
-
-				if (!ret && (port == found_port)) {
-					id_priv->id.port_num = found_port;
-					goto out;
-				}
+
+			gidp = rdma_protocol_roce(cma_dev->device, port) ?
+			       &iboe_gid : &gid;
+
+			ret = cma_validate_port(cma_dev->device, port, gidp,
+						dev_addr->dev_type);
+			if (!ret) {
+				id_priv->id.port_num = port;
+				goto out;
 			}
 		}
 	}
@@ -435,10 +475,10 @@ static int cma_resolve_ib_dev(struct rdma_id_private *id_priv)
 	pkey = ntohs(addr->sib_pkey);
 
 	list_for_each_entry(cur_dev, &dev_list, list) {
-		if (rdma_node_get_transport(cur_dev->device->node_type) != RDMA_TRANSPORT_IB)
-			continue;
-
 		for (p = 1; p <= cur_dev->device->phys_port_cnt; ++p) {
+			if (!rdma_cap_af_ib(cur_dev->device, p))
+				continue;
+
 			if (ib_find_cached_pkey(cur_dev->device, p, pkey, &index))
 				continue;
 
@@ -633,10 +673,9 @@ static int cma_modify_qp_rtr(struct rdma_id_private *id_priv,
 	if (ret)
 		goto out;
 
-	if (rdma_node_get_transport(id_priv->cma_dev->device->node_type)
-	    == RDMA_TRANSPORT_IB &&
-	    rdma_port_get_link_layer(id_priv->id.device, id_priv->id.port_num)
-	    == IB_LINK_LAYER_ETHERNET) {
+	BUG_ON(id_priv->cma_dev->device != id_priv->id.device);
+
+	if (rdma_protocol_roce(id_priv->id.device, id_priv->id.port_num)) {
 		ret = rdma_addr_find_smac_by_sgid(&sgid, qp_attr.smac, NULL);
 
 		if (ret)
@@ -700,11 +739,10 @@ static int cma_ib_init_qp_attr(struct rdma_id_private *id_priv,
 	int ret;
 	u16 pkey;
 
-	if (rdma_port_get_link_layer(id_priv->id.device, id_priv->id.port_num) ==
-	    IB_LINK_LAYER_INFINIBAND)
-		pkey = ib_addr_get_pkey(dev_addr);
-	else
+	if (rdma_cap_eth_ah(id_priv->id.device, id_priv->id.port_num))
 		pkey = 0xffff;
+	else
+		pkey = ib_addr_get_pkey(dev_addr);
 
 	ret = ib_find_cached_pkey(id_priv->id.device, id_priv->id.port_num,
 				  pkey, &qp_attr->pkey_index);
@@ -735,8 +773,7 @@ int rdma_init_qp_attr(struct rdma_cm_id *id, struct ib_qp_attr *qp_attr,
 	int ret = 0;
 
 	id_priv = container_of(id, struct rdma_id_private, id);
-	switch (rdma_node_get_transport(id_priv->id.device->node_type)) {
-	case RDMA_TRANSPORT_IB:
+	if (rdma_cap_ib_cm(id->device, id->port_num)) {
 		if (!id_priv->cm_id.ib || (id_priv->id.qp_type == IB_QPT_UD))
 			ret = cma_ib_init_qp_attr(id_priv, qp_attr, qp_attr_mask);
 		else
@@ -745,19 +782,15 @@ int rdma_init_qp_attr(struct rdma_cm_id *id, struct ib_qp_attr *qp_attr,
 
 		if (qp_attr->qp_state == IB_QPS_RTR)
 			qp_attr->rq_psn = id_priv->seq_num;
-		break;
-	case RDMA_TRANSPORT_IWARP:
+	} else if (rdma_cap_iw_cm(id->device, id->port_num)) {
 		if (!id_priv->cm_id.iw) {
 			qp_attr->qp_access_flags = 0;
 			*qp_attr_mask = IB_QP_STATE | IB_QP_ACCESS_FLAGS;
 		} else
 			ret = iw_cm_init_qp_attr(id_priv->cm_id.iw, qp_attr,
 						 qp_attr_mask);
-		break;
-	default:
+	} else
 		ret = -ENOSYS;
-		break;
-	}
 
 	return ret;
 }
@@ -945,13 +978,9 @@ static inline int cma_user_data_offset(struct rdma_id_private *id_priv)
 
 static void cma_cancel_route(struct rdma_id_private *id_priv)
 {
-	switch (rdma_port_get_link_layer(id_priv->id.device, id_priv->id.port_num)) {
-	case IB_LINK_LAYER_INFINIBAND:
+	if (rdma_cap_ib_sa(id_priv->id.device, id_priv->id.port_num)) {
 		if (id_priv->query)
 			ib_sa_cancel_query(id_priv->query_id, id_priv->query);
-		break;
-	default:
-		break;
 	}
 }
 
@@ -1023,17 +1052,12 @@ static void cma_leave_mc_groups(struct rdma_id_private *id_priv)
 		mc = container_of(id_priv->mc_list.next,
 				  struct cma_multicast, list);
 		list_del(&mc->list);
-		switch (rdma_port_get_link_layer(id_priv->cma_dev->device, id_priv->id.port_num)) {
-		case IB_LINK_LAYER_INFINIBAND:
+		if (rdma_cap_ib_mcast(id_priv->cma_dev->device,
+				      id_priv->id.port_num)) {
 			ib_sa_free_multicast(mc->multicast.ib);
 			kfree(mc);
-			break;
-		case IB_LINK_LAYER_ETHERNET:
+		} else
 			kref_put(&mc->mcref, release_mc);
-			break;
-		default:
-			break;
-		}
 	}
 }
 
@@ -1054,17 +1078,12 @@ void rdma_destroy_id(struct rdma_cm_id *id)
 	mutex_unlock(&id_priv->handler_mutex);
 
 	if (id_priv->cma_dev) {
-		switch (rdma_node_get_transport(id_priv->id.device->node_type)) {
-		case RDMA_TRANSPORT_IB:
+		if (rdma_cap_ib_cm(id_priv->id.device, 1)) {
 			if (id_priv->cm_id.ib)
 				ib_destroy_cm_id(id_priv->cm_id.ib);
-			break;
-		case RDMA_TRANSPORT_IWARP:
+		} else if (rdma_cap_iw_cm(id_priv->id.device, 1)) {
 			if (id_priv->cm_id.iw)
 				iw_destroy_cm_id(id_priv->cm_id.iw);
-			break;
-		default:
-			break;
 		}
 		cma_leave_mc_groups(id_priv);
 		cma_release_dev(id_priv);
@@ -1610,6 +1629,7 @@ static int cma_iw_listen(struct rdma_id_private *id_priv, int backlog)
 	if (IS_ERR(id))
 		return PTR_ERR(id);
 
+	id->tos = id_priv->tos;
 	id_priv->cm_id.iw = id;
 
 	memcpy(&id_priv->cm_id.iw->local_addr, cma_src_addr(id_priv),
@@ -1642,8 +1662,7 @@ static void cma_listen_on_dev(struct rdma_id_private *id_priv,
 	struct rdma_cm_id *id;
 	int ret;
 
-	if (cma_family(id_priv) == AF_IB &&
-	    rdma_node_get_transport(cma_dev->device->node_type) != RDMA_TRANSPORT_IB)
+	if (cma_family(id_priv) == AF_IB && !rdma_cap_ib_cm(cma_dev->device, 1))
 		return;
 
 	id = rdma_create_id(cma_listen_handler, id_priv, id_priv->id.ps,
@@ -1984,26 +2003,15 @@ int rdma_resolve_route(struct rdma_cm_id *id, int timeout_ms)
 		return -EINVAL;
 
 	atomic_inc(&id_priv->refcount);
-	switch (rdma_node_get_transport(id->device->node_type)) {
-	case RDMA_TRANSPORT_IB:
-		switch (rdma_port_get_link_layer(id->device, id->port_num)) {
-		case IB_LINK_LAYER_INFINIBAND:
-			ret = cma_resolve_ib_route(id_priv, timeout_ms);
-			break;
-		case IB_LINK_LAYER_ETHERNET:
-			ret = cma_resolve_iboe_route(id_priv);
-			break;
-		default:
-			ret = -ENOSYS;
-		}
-		break;
-	case RDMA_TRANSPORT_IWARP:
+	if (rdma_cap_ib_sa(id->device, id->port_num))
+		ret = cma_resolve_ib_route(id_priv, timeout_ms);
+	else if (rdma_protocol_roce(id->device, id->port_num))
+		ret = cma_resolve_iboe_route(id_priv);
+	else if (rdma_protocol_iwarp(id->device, id->port_num))
 		ret = cma_resolve_iw_route(id_priv, timeout_ms);
-		break;
-	default:
+	else
 		ret = -ENOSYS;
-		break;
-	}
+
 	if (ret)
 		goto err;
 
@@ -2045,7 +2053,7 @@ static int cma_bind_loopback(struct rdma_id_private *id_priv)
 	mutex_lock(&lock);
 	list_for_each_entry(cur_dev, &dev_list, list) {
 		if (cma_family(id_priv) == AF_IB &&
-		    rdma_node_get_transport(cur_dev->device->node_type) != RDMA_TRANSPORT_IB)
+		    !rdma_cap_ib_cm(cur_dev->device, 1))
 			continue;
 
 		if (!cma_dev)
@@ -2077,7 +2085,7 @@ port_found:
 		goto out;
 
 	id_priv->id.route.addr.dev_addr.dev_type =
-		(rdma_port_get_link_layer(cma_dev->device, p) == IB_LINK_LAYER_INFINIBAND) ?
+		(rdma_protocol_ib(cma_dev->device, p)) ?
 		ARPHRD_INFINIBAND : ARPHRD_ETHER;
 
 	rdma_addr_set_sgid(&id_priv->id.route.addr.dev_addr, &gid);
@@ -2554,18 +2562,15 @@ int rdma_listen(struct rdma_cm_id *id, int backlog)
 
 	id_priv->backlog = backlog;
 	if (id->device) {
-		switch (rdma_node_get_transport(id->device->node_type)) {
-		case RDMA_TRANSPORT_IB:
+		if (rdma_cap_ib_cm(id->device, 1)) {
 			ret = cma_ib_listen(id_priv);
 			if (ret)
 				goto err;
-			break;
-		case RDMA_TRANSPORT_IWARP:
+		} else if (rdma_cap_iw_cm(id->device, 1)) {
 			ret = cma_iw_listen(id_priv, backlog);
 			if (ret)
 				goto err;
-			break;
-		default:
+		} else {
 			ret = -ENOSYS;
 			goto err;
 		}
@@ -2857,6 +2862,7 @@ static int cma_connect_iw(struct rdma_id_private *id_priv,
 	if (IS_ERR(cm_id))
 		return PTR_ERR(cm_id);
 
+	cm_id->tos = id_priv->tos;
 	id_priv->cm_id.iw = cm_id;
 
 	memcpy(&cm_id->local_addr, cma_src_addr(id_priv),
@@ -2901,20 +2907,15 @@ int rdma_connect(struct rdma_cm_id *id, struct rdma_conn_param *conn_param)
 		id_priv->srq = conn_param->srq;
 	}
 
-	switch (rdma_node_get_transport(id->device->node_type)) {
-	case RDMA_TRANSPORT_IB:
+	if (rdma_cap_ib_cm(id->device, id->port_num)) {
 		if (id->qp_type == IB_QPT_UD)
 			ret = cma_resolve_ib_udp(id_priv, conn_param);
 		else
 			ret = cma_connect_ib(id_priv, conn_param);
-		break;
-	case RDMA_TRANSPORT_IWARP:
+	} else if (rdma_cap_iw_cm(id->device, id->port_num))
 		ret = cma_connect_iw(id_priv, conn_param);
-		break;
-	default:
+	else
 		ret = -ENOSYS;
-		break;
-	}
 	if (ret)
 		goto err;
 
@@ -3017,8 +3018,7 @@ int rdma_accept(struct rdma_cm_id *id, struct rdma_conn_param *conn_param)
 		id_priv->srq = conn_param->srq;
 	}
 
-	switch (rdma_node_get_transport(id->device->node_type)) {
-	case RDMA_TRANSPORT_IB:
+	if (rdma_cap_ib_cm(id->device, id->port_num)) {
 		if (id->qp_type == IB_QPT_UD) {
 			if (conn_param)
 				ret = cma_send_sidr_rep(id_priv, IB_SIDR_SUCCESS,
@@ -3034,14 +3034,10 @@ int rdma_accept(struct rdma_cm_id *id, struct rdma_conn_param *conn_param)
 			else
 				ret = cma_rep_recv(id_priv);
 		}
-		break;
-	case RDMA_TRANSPORT_IWARP:
+	} else if (rdma_cap_iw_cm(id->device, id->port_num))
 		ret = cma_accept_iw(id_priv, conn_param);
-		break;
-	default:
+	else
 		ret = -ENOSYS;
-		break;
-	}
 
 	if (ret)
 		goto reject;
@@ -3085,8 +3081,7 @@ int rdma_reject(struct rdma_cm_id *id, const void *private_data,
 	if (!id_priv->cm_id.ib)
 		return -EINVAL;
 
-	switch (rdma_node_get_transport(id->device->node_type)) {
-	case RDMA_TRANSPORT_IB:
+	if (rdma_cap_ib_cm(id->device, id->port_num)) {
 		if (id->qp_type == IB_QPT_UD)
 			ret = cma_send_sidr_rep(id_priv, IB_SIDR_REJECT, 0,
 						private_data, private_data_len);
@@ -3094,15 +3089,12 @@ int rdma_reject(struct rdma_cm_id *id, const void *private_data,
 			ret = ib_send_cm_rej(id_priv->cm_id.ib,
 					     IB_CM_REJ_CONSUMER_DEFINED, NULL,
 					     0, private_data, private_data_len);
-		break;
-	case RDMA_TRANSPORT_IWARP:
+	} else if (rdma_cap_iw_cm(id->device, id->port_num)) {
 		ret = iw_cm_reject(id_priv->cm_id.iw,
 				   private_data, private_data_len);
-		break;
-	default:
+	} else
 		ret = -ENOSYS;
-		break;
-	}
+
 	return ret;
 }
 EXPORT_SYMBOL(rdma_reject);
@@ -3116,22 +3108,18 @@ int rdma_disconnect(struct rdma_cm_id *id)
 	if (!id_priv->cm_id.ib)
 		return -EINVAL;
 
-	switch (rdma_node_get_transport(id->device->node_type)) {
-	case RDMA_TRANSPORT_IB:
+	if (rdma_cap_ib_cm(id->device, id->port_num)) {
 		ret = cma_modify_qp_err(id_priv);
 		if (ret)
 			goto out;
 		/* Initiate or respond to a disconnect. */
 		if (ib_send_cm_dreq(id_priv->cm_id.ib, NULL, 0))
 			ib_send_cm_drep(id_priv->cm_id.ib, NULL, 0);
-		break;
-	case RDMA_TRANSPORT_IWARP:
+	} else if (rdma_cap_iw_cm(id->device, id->port_num)) {
 		ret = iw_cm_disconnect(id_priv->cm_id.iw, 0);
-		break;
-	default:
+	} else
 		ret = -EINVAL;
-		break;
-	}
+
 out:
 	return ret;
 }
@@ -3377,24 +3365,13 @@ int rdma_join_multicast(struct rdma_cm_id *id, struct sockaddr *addr,
 	list_add(&mc->list, &id_priv->mc_list);
 	spin_unlock(&id_priv->lock);
 
-	switch (rdma_node_get_transport(id->device->node_type)) {
-	case RDMA_TRANSPORT_IB:
-		switch (rdma_port_get_link_layer(id->device, id->port_num)) {
-		case IB_LINK_LAYER_INFINIBAND:
-			ret = cma_join_ib_multicast(id_priv, mc);
-			break;
-		case IB_LINK_LAYER_ETHERNET:
-			kref_init(&mc->mcref);
-			ret = cma_iboe_join_multicast(id_priv, mc);
-			break;
-		default:
-			ret = -EINVAL;
-		}
-		break;
-	default:
+	if (rdma_protocol_roce(id->device, id->port_num)) {
+		kref_init(&mc->mcref);
+		ret = cma_iboe_join_multicast(id_priv, mc);
+	} else if (rdma_cap_ib_mcast(id->device, id->port_num))
+		ret = cma_join_ib_multicast(id_priv, mc);
+	else
 		ret = -ENOSYS;
-		break;
-	}
 
 	if (ret) {
 		spin_lock_irq(&id_priv->lock);
@@ -3422,19 +3399,15 @@ void rdma_leave_multicast(struct rdma_cm_id *id, struct sockaddr *addr)
 				ib_detach_mcast(id->qp,
 						&mc->multicast.ib->rec.mgid,
 						be16_to_cpu(mc->multicast.ib->rec.mlid));
-			if (rdma_node_get_transport(id_priv->cma_dev->device->node_type) == RDMA_TRANSPORT_IB) {
-				switch (rdma_port_get_link_layer(id->device, id->port_num)) {
-				case IB_LINK_LAYER_INFINIBAND:
-					ib_sa_free_multicast(mc->multicast.ib);
-					kfree(mc);
-					break;
-				case IB_LINK_LAYER_ETHERNET:
-					kref_put(&mc->mcref, release_mc);
-					break;
-				default:
-					break;
-				}
-			}
+
+			BUG_ON(id_priv->cma_dev->device != id->device);
+
+			if (rdma_cap_ib_mcast(id->device, id->port_num)) {
+				ib_sa_free_multicast(mc->multicast.ib);
+				kfree(mc);
+			} else if (rdma_protocol_roce(id->device, id->port_num))
+				kref_put(&mc->mcref, release_mc);
+
 			return;
 		}
 	}
diff --git a/drivers/infiniband/core/device.c b/drivers/infiniband/core/device.c
index 18c1ece765f2..9567756ca4f9 100644
--- a/drivers/infiniband/core/device.c
+++ b/drivers/infiniband/core/device.c
@@ -92,7 +92,8 @@ static int ib_device_check_mandatory(struct ib_device *device)
 		IB_MANDATORY_FUNC(poll_cq),
 		IB_MANDATORY_FUNC(req_notify_cq),
 		IB_MANDATORY_FUNC(get_dma_mr),
-		IB_MANDATORY_FUNC(dereg_mr)
+		IB_MANDATORY_FUNC(dereg_mr),
+		IB_MANDATORY_FUNC(get_port_immutable)
 	};
 	int i;
 
@@ -151,18 +152,6 @@ static int alloc_name(char *name)
 	return 0;
 }
 
-static int start_port(struct ib_device *device)
-{
-	return (device->node_type == RDMA_NODE_IB_SWITCH) ? 0 : 1;
-}
-
-
-static int end_port(struct ib_device *device)
-{
-	return (device->node_type == RDMA_NODE_IB_SWITCH) ?
-		0 : device->phys_port_cnt;
-}
-
 /**
  * ib_alloc_device - allocate an IB device struct
  * @size:size of structure to allocate
@@ -222,42 +211,49 @@ static int add_client_context(struct ib_device *device, struct ib_client *client
 	return 0;
 }
 
-static int read_port_table_lengths(struct ib_device *device)
+static int verify_immutable(const struct ib_device *dev, u8 port)
 {
-	struct ib_port_attr *tprops = NULL;
-	int num_ports, ret = -ENOMEM;
-	u8 port_index;
-
-	tprops = kmalloc(sizeof *tprops, GFP_KERNEL);
-	if (!tprops)
-		goto out;
-
-	num_ports = end_port(device) - start_port(device) + 1;
+	return WARN_ON(!rdma_cap_ib_mad(dev, port) &&
+			    rdma_max_mad_size(dev, port) != 0);
+}
 
-	device->pkey_tbl_len = kmalloc(sizeof *device->pkey_tbl_len * num_ports,
-				       GFP_KERNEL);
-	device->gid_tbl_len = kmalloc(sizeof *device->gid_tbl_len * num_ports,
-				      GFP_KERNEL);
-	if (!device->pkey_tbl_len || !device->gid_tbl_len)
+static int read_port_immutable(struct ib_device *device)
+{
+	int ret = -ENOMEM;
+	u8 start_port = rdma_start_port(device);
+	u8 end_port = rdma_end_port(device);
+	u8 port;
+
+	/**
+	 * device->port_immutable is indexed directly by the port number to make
+	 * access to this data as efficient as possible.
+	 *
+	 * Therefore port_immutable is declared as a 1 based array with
+	 * potential empty slots at the beginning.
+	 */
+	device->port_immutable = kzalloc(sizeof(*device->port_immutable)
+					 * (end_port + 1),
+					 GFP_KERNEL);
+	if (!device->port_immutable)
 		goto err;
 
-	for (port_index = 0; port_index < num_ports; ++port_index) {
-		ret = ib_query_port(device, port_index + start_port(device),
-					tprops);
+	for (port = start_port; port <= end_port; ++port) {
+		ret = device->get_port_immutable(device, port,
+						 &device->port_immutable[port]);
 		if (ret)
 			goto err;
-		device->pkey_tbl_len[port_index] = tprops->pkey_tbl_len;
-		device->gid_tbl_len[port_index]  = tprops->gid_tbl_len;
+
+		if (verify_immutable(device, port)) {
+			ret = -EINVAL;
+			goto err;
+		}
 	}
 
 	ret = 0;
 	goto out;
-
 err:
-	kfree(device->gid_tbl_len);
-	kfree(device->pkey_tbl_len);
+	kfree(device->port_immutable);
 out:
-	kfree(tprops);
 	return ret;
 }
 
@@ -294,9 +290,9 @@ int ib_register_device(struct ib_device *device,
 	spin_lock_init(&device->event_handler_lock);
 	spin_lock_init(&device->client_data_lock);
 
-	ret = read_port_table_lengths(device);
+	ret = read_port_immutable(device);
 	if (ret) {
-		printk(KERN_WARNING "Couldn't create table lengths cache for device %s\n",
+		printk(KERN_WARNING "Couldn't create per port immutable data %s\n",
 		       device->name);
 		goto out;
 	}
@@ -305,8 +301,7 @@ int ib_register_device(struct ib_device *device,
 	if (ret) {
 		printk(KERN_WARNING "Couldn't register device %s with driver model\n",
 		       device->name);
-		kfree(device->gid_tbl_len);
-		kfree(device->pkey_tbl_len);
+		kfree(device->port_immutable);
 		goto out;
 	}
 
@@ -348,9 +343,6 @@ void ib_unregister_device(struct ib_device *device)
 
 	list_del(&device->core_list);
 
-	kfree(device->gid_tbl_len);
-	kfree(device->pkey_tbl_len);
-
 	mutex_unlock(&device_mutex);
 
 	ib_device_unregister_sysfs(device);
@@ -558,7 +550,11 @@ EXPORT_SYMBOL(ib_dispatch_event);
 int ib_query_device(struct ib_device *device,
 		    struct ib_device_attr *device_attr)
 {
-	return device->query_device(device, device_attr);
+	struct ib_udata uhw = {.outlen = 0, .inlen = 0};
+
+	memset(device_attr, 0, sizeof(*device_attr));
+
+	return device->query_device(device, device_attr, &uhw);
 }
 EXPORT_SYMBOL(ib_query_device);
 
@@ -575,7 +571,7 @@ int ib_query_port(struct ib_device *device,
 		  u8 port_num,
 		  struct ib_port_attr *port_attr)
 {
-	if (port_num < start_port(device) || port_num > end_port(device))
+	if (port_num < rdma_start_port(device) || port_num > rdma_end_port(device))
 		return -EINVAL;
 
 	return device->query_port(device, port_num, port_attr);
@@ -653,7 +649,7 @@ int ib_modify_port(struct ib_device *device,
 	if (!device->modify_port)
 		return -ENOSYS;
 
-	if (port_num < start_port(device) || port_num > end_port(device))
+	if (port_num < rdma_start_port(device) || port_num > rdma_end_port(device))
 		return -EINVAL;
 
 	return device->modify_port(device, port_num, port_modify_mask,
@@ -676,8 +672,8 @@ int ib_find_gid(struct ib_device *device, union ib_gid *gid,
 	union ib_gid tmp_gid;
 	int ret, port, i;
 
-	for (port = start_port(device); port <= end_port(device); ++port) {
-		for (i = 0; i < device->gid_tbl_len[port - start_port(device)]; ++i) {
+	for (port = rdma_start_port(device); port <= rdma_end_port(device); ++port) {
+		for (i = 0; i < device->port_immutable[port].gid_tbl_len; ++i) {
 			ret = ib_query_gid(device, port, i, &tmp_gid);
 			if (ret)
 				return ret;
@@ -709,7 +705,7 @@ int ib_find_pkey(struct ib_device *device,
 	u16 tmp_pkey;
 	int partial_ix = -1;
 
-	for (i = 0; i < device->pkey_tbl_len[port_num - start_port(device)]; ++i) {
+	for (i = 0; i < device->port_immutable[port_num].pkey_tbl_len; ++i) {
 		ret = ib_query_pkey(device, port_num, i, &tmp_pkey);
 		if (ret)
 			return ret;
diff --git a/drivers/infiniband/core/mad.c b/drivers/infiniband/core/mad.c
index 74c30f4c557e..a4b1466c1bf6 100644
--- a/drivers/infiniband/core/mad.c
+++ b/drivers/infiniband/core/mad.c
@@ -3,6 +3,7 @@
  * Copyright (c) 2005 Intel Corporation.  All rights reserved.
  * Copyright (c) 2005 Mellanox Technologies Ltd.  All rights reserved.
  * Copyright (c) 2009 HNR Consulting. All rights reserved.
+ * Copyright (c) 2014 Intel Corporation.  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
@@ -44,6 +45,7 @@
 #include "mad_priv.h"
 #include "mad_rmpp.h"
 #include "smi.h"
+#include "opa_smi.h"
 #include "agent.h"
 
 MODULE_LICENSE("Dual BSD/GPL");
@@ -59,8 +61,6 @@ MODULE_PARM_DESC(send_queue_size, "Size of send queue in number of work requests
 module_param_named(recv_queue_size, mad_recvq_size, int, 0444);
 MODULE_PARM_DESC(recv_queue_size, "Size of receive queue in number of work requests");
 
-static struct kmem_cache *ib_mad_cache;
-
 static struct list_head ib_mad_port_list;
 static u32 ib_mad_client_id = 0;
 
@@ -73,7 +73,7 @@ static int method_in_use(struct ib_mad_mgmt_method_table **method,
 static void remove_mad_reg_req(struct ib_mad_agent_private *priv);
 static struct ib_mad_agent_private *find_mad_agent(
 					struct ib_mad_port_private *port_priv,
-					struct ib_mad *mad);
+					const struct ib_mad_hdr *mad);
 static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
 				    struct ib_mad_private *mad);
 static void cancel_mads(struct ib_mad_agent_private *mad_agent_priv);
@@ -179,12 +179,12 @@ static int is_vendor_method_in_use(
 	return 0;
 }
 
-int ib_response_mad(struct ib_mad *mad)
+int ib_response_mad(const struct ib_mad_hdr *hdr)
 {
-	return ((mad->mad_hdr.method & IB_MGMT_METHOD_RESP) ||
-		(mad->mad_hdr.method == IB_MGMT_METHOD_TRAP_REPRESS) ||
-		((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_BM) &&
-		 (mad->mad_hdr.attr_mod & IB_BM_ATTR_MOD_RESP)));
+	return ((hdr->method & IB_MGMT_METHOD_RESP) ||
+		(hdr->method == IB_MGMT_METHOD_TRAP_REPRESS) ||
+		((hdr->mgmt_class == IB_MGMT_CLASS_BM) &&
+		 (hdr->attr_mod & IB_BM_ATTR_MOD_RESP)));
 }
 EXPORT_SYMBOL(ib_response_mad);
 
@@ -717,6 +717,32 @@ static void build_smp_wc(struct ib_qp *qp,
 	wc->port_num = port_num;
 }
 
+static size_t mad_priv_size(const struct ib_mad_private *mp)
+{
+	return sizeof(struct ib_mad_private) + mp->mad_size;
+}
+
+static struct ib_mad_private *alloc_mad_private(size_t mad_size, gfp_t flags)
+{
+	size_t size = sizeof(struct ib_mad_private) + mad_size;
+	struct ib_mad_private *ret = kzalloc(size, flags);
+
+	if (ret)
+		ret->mad_size = mad_size;
+
+	return ret;
+}
+
+static size_t port_mad_size(const struct ib_mad_port_private *port_priv)
+{
+	return rdma_max_mad_size(port_priv->device, port_priv->port_num);
+}
+
+static size_t mad_priv_dma_size(const struct ib_mad_private *mp)
+{
+	return sizeof(struct ib_grh) + mp->mad_size;
+}
+
 /*
  * Return 0 if SMP is to be sent
  * Return 1 if SMP was consumed locally (whether or not solicited)
@@ -727,6 +753,7 @@ static int handle_outgoing_dr_smp(struct ib_mad_agent_private *mad_agent_priv,
 {
 	int ret = 0;
 	struct ib_smp *smp = mad_send_wr->send_buf.mad;
+	struct opa_smp *opa_smp = (struct opa_smp *)smp;
 	unsigned long flags;
 	struct ib_mad_local_private *local;
 	struct ib_mad_private *mad_priv;
@@ -736,6 +763,11 @@ static int handle_outgoing_dr_smp(struct ib_mad_agent_private *mad_agent_priv,
 	u8 port_num;
 	struct ib_wc mad_wc;
 	struct ib_send_wr *send_wr = &mad_send_wr->send_wr;
+	size_t mad_size = port_mad_size(mad_agent_priv->qp_info->port_priv);
+	u16 out_mad_pkey_index = 0;
+	u16 drslid;
+	bool opa = rdma_cap_opa_mad(mad_agent_priv->qp_info->port_priv->device,
+				    mad_agent_priv->qp_info->port_priv->port_num);
 
 	if (device->node_type == RDMA_NODE_IB_SWITCH &&
 	    smp->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
@@ -749,19 +781,48 @@ static int handle_outgoing_dr_smp(struct ib_mad_agent_private *mad_agent_priv,
 	 * If we are at the start of the LID routed part, don't update the
 	 * hop_ptr or hop_cnt.  See section 14.2.2, Vol 1 IB spec.
 	 */
-	if ((ib_get_smp_direction(smp) ? smp->dr_dlid : smp->dr_slid) ==
-	     IB_LID_PERMISSIVE &&
-	     smi_handle_dr_smp_send(smp, device->node_type, port_num) ==
-	     IB_SMI_DISCARD) {
-		ret = -EINVAL;
-		dev_err(&device->dev, "Invalid directed route\n");
-		goto out;
-	}
+	if (opa && smp->class_version == OPA_SMP_CLASS_VERSION) {
+		u32 opa_drslid;
+
+		if ((opa_get_smp_direction(opa_smp)
+		     ? opa_smp->route.dr.dr_dlid : opa_smp->route.dr.dr_slid) ==
+		     OPA_LID_PERMISSIVE &&
+		     opa_smi_handle_dr_smp_send(opa_smp, device->node_type,
+						port_num) == IB_SMI_DISCARD) {
+			ret = -EINVAL;
+			dev_err(&device->dev, "OPA Invalid directed route\n");
+			goto out;
+		}
+		opa_drslid = be32_to_cpu(opa_smp->route.dr.dr_slid);
+		if (opa_drslid != OPA_LID_PERMISSIVE &&
+		    opa_drslid & 0xffff0000) {
+			ret = -EINVAL;
+			dev_err(&device->dev, "OPA Invalid dr_slid 0x%x\n",
+			       opa_drslid);
+			goto out;
+		}
+		drslid = (u16)(opa_drslid & 0x0000ffff);
 
-	/* Check to post send on QP or process locally */
-	if (smi_check_local_smp(smp, device) == IB_SMI_DISCARD &&
-	    smi_check_local_returning_smp(smp, device) == IB_SMI_DISCARD)
-		goto out;
+		/* Check to post send on QP or process locally */
+		if (opa_smi_check_local_smp(opa_smp, device) == IB_SMI_DISCARD &&
+		    opa_smi_check_local_returning_smp(opa_smp, device) == IB_SMI_DISCARD)
+			goto out;
+	} else {
+		if ((ib_get_smp_direction(smp) ? smp->dr_dlid : smp->dr_slid) ==
+		     IB_LID_PERMISSIVE &&
+		     smi_handle_dr_smp_send(smp, device->node_type, port_num) ==
+		     IB_SMI_DISCARD) {
+			ret = -EINVAL;
+			dev_err(&device->dev, "Invalid directed route\n");
+			goto out;
+		}
+		drslid = be16_to_cpu(smp->dr_slid);
+
+		/* Check to post send on QP or process locally */
+		if (smi_check_local_smp(smp, device) == IB_SMI_DISCARD &&
+		    smi_check_local_returning_smp(smp, device) == IB_SMI_DISCARD)
+			goto out;
+	}
 
 	local = kmalloc(sizeof *local, GFP_ATOMIC);
 	if (!local) {
@@ -771,7 +832,7 @@ static int handle_outgoing_dr_smp(struct ib_mad_agent_private *mad_agent_priv,
 	}
 	local->mad_priv = NULL;
 	local->recv_mad_agent = NULL;
-	mad_priv = kmem_cache_alloc(ib_mad_cache, GFP_ATOMIC);
+	mad_priv = alloc_mad_private(mad_size, GFP_ATOMIC);
 	if (!mad_priv) {
 		ret = -ENOMEM;
 		dev_err(&device->dev, "No memory for local response MAD\n");
@@ -780,18 +841,25 @@ static int handle_outgoing_dr_smp(struct ib_mad_agent_private *mad_agent_priv,
 	}
 
 	build_smp_wc(mad_agent_priv->agent.qp,
-		     send_wr->wr_id, be16_to_cpu(smp->dr_slid),
+		     send_wr->wr_id, drslid,
 		     send_wr->wr.ud.pkey_index,
 		     send_wr->wr.ud.port_num, &mad_wc);
 
+	if (opa && smp->base_version == OPA_MGMT_BASE_VERSION) {
+		mad_wc.byte_len = mad_send_wr->send_buf.hdr_len
+					+ mad_send_wr->send_buf.data_len
+					+ sizeof(struct ib_grh);
+	}
+
 	/* No GRH for DR SMP */
 	ret = device->process_mad(device, 0, port_num, &mad_wc, NULL,
-				  (struct ib_mad *)smp,
-				  (struct ib_mad *)&mad_priv->mad);
+				  (const struct ib_mad_hdr *)smp, mad_size,
+				  (struct ib_mad_hdr *)mad_priv->mad,
+				  &mad_size, &out_mad_pkey_index);
 	switch (ret)
 	{
 	case IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY:
-		if (ib_response_mad(&mad_priv->mad.mad) &&
+		if (ib_response_mad((const struct ib_mad_hdr *)mad_priv->mad) &&
 		    mad_agent_priv->agent.recv_handler) {
 			local->mad_priv = mad_priv;
 			local->recv_mad_agent = mad_agent_priv;
@@ -801,39 +869,43 @@ static int handle_outgoing_dr_smp(struct ib_mad_agent_private *mad_agent_priv,
 			 */
 			atomic_inc(&mad_agent_priv->refcount);
 		} else
-			kmem_cache_free(ib_mad_cache, mad_priv);
+			kfree(mad_priv);
 		break;
 	case IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_CONSUMED:
-		kmem_cache_free(ib_mad_cache, mad_priv);
+		kfree(mad_priv);
 		break;
 	case IB_MAD_RESULT_SUCCESS:
 		/* Treat like an incoming receive MAD */
 		port_priv = ib_get_mad_port(mad_agent_priv->agent.device,
 					    mad_agent_priv->agent.port_num);
 		if (port_priv) {
-			memcpy(&mad_priv->mad.mad, smp, sizeof(struct ib_mad));
+			memcpy(mad_priv->mad, smp, mad_priv->mad_size);
 			recv_mad_agent = find_mad_agent(port_priv,
-						        &mad_priv->mad.mad);
+						        (const struct ib_mad_hdr *)mad_priv->mad);
 		}
 		if (!port_priv || !recv_mad_agent) {
 			/*
 			 * No receiving agent so drop packet and
 			 * generate send completion.
 			 */
-			kmem_cache_free(ib_mad_cache, mad_priv);
+			kfree(mad_priv);
 			break;
 		}
 		local->mad_priv = mad_priv;
 		local->recv_mad_agent = recv_mad_agent;
 		break;
 	default:
-		kmem_cache_free(ib_mad_cache, mad_priv);
+		kfree(mad_priv);
 		kfree(local);
 		ret = -EINVAL;
 		goto out;
 	}
 
 	local->mad_send_wr = mad_send_wr;
+	if (opa) {
+		local->mad_send_wr->send_wr.wr.ud.pkey_index = out_mad_pkey_index;
+		local->return_wc_byte_len = mad_size;
+	}
 	/* Reference MAD agent until send side of local completion handled */
 	atomic_inc(&mad_agent_priv->refcount);
 	/* Queue local completion to local list */
@@ -847,11 +919,11 @@ out:
 	return ret;
 }
 
-static int get_pad_size(int hdr_len, int data_len)
+static int get_pad_size(int hdr_len, int data_len, size_t mad_size)
 {
 	int seg_size, pad;
 
-	seg_size = sizeof(struct ib_mad) - hdr_len;
+	seg_size = mad_size - hdr_len;
 	if (data_len && seg_size) {
 		pad = seg_size - data_len % seg_size;
 		return pad == seg_size ? 0 : pad;
@@ -870,14 +942,15 @@ static void free_send_rmpp_list(struct ib_mad_send_wr_private *mad_send_wr)
 }
 
 static int alloc_send_rmpp_list(struct ib_mad_send_wr_private *send_wr,
-				gfp_t gfp_mask)
+				size_t mad_size, gfp_t gfp_mask)
 {
 	struct ib_mad_send_buf *send_buf = &send_wr->send_buf;
 	struct ib_rmpp_mad *rmpp_mad = send_buf->mad;
 	struct ib_rmpp_segment *seg = NULL;
 	int left, seg_size, pad;
 
-	send_buf->seg_size = sizeof (struct ib_mad) - send_buf->hdr_len;
+	send_buf->seg_size = mad_size - send_buf->hdr_len;
+	send_buf->seg_rmpp_size = mad_size - IB_MGMT_RMPP_HDR;
 	seg_size = send_buf->seg_size;
 	pad = send_wr->pad;
 
@@ -910,7 +983,7 @@ static int alloc_send_rmpp_list(struct ib_mad_send_wr_private *send_wr,
 	return 0;
 }
 
-int ib_mad_kernel_rmpp_agent(struct ib_mad_agent *agent)
+int ib_mad_kernel_rmpp_agent(const struct ib_mad_agent *agent)
 {
 	return agent->rmpp_version && !(agent->flags & IB_MAD_USER_RMPP);
 }
@@ -920,26 +993,37 @@ struct ib_mad_send_buf * ib_create_send_mad(struct ib_mad_agent *mad_agent,
 					    u32 remote_qpn, u16 pkey_index,
 					    int rmpp_active,
 					    int hdr_len, int data_len,
-					    gfp_t gfp_mask)
+					    gfp_t gfp_mask,
+					    u8 base_version)
 {
 	struct ib_mad_agent_private *mad_agent_priv;
 	struct ib_mad_send_wr_private *mad_send_wr;
 	int pad, message_size, ret, size;
 	void *buf;
+	size_t mad_size;
+	bool opa;
 
 	mad_agent_priv = container_of(mad_agent, struct ib_mad_agent_private,
 				      agent);
-	pad = get_pad_size(hdr_len, data_len);
+
+	opa = rdma_cap_opa_mad(mad_agent->device, mad_agent->port_num);
+
+	if (opa && base_version == OPA_MGMT_BASE_VERSION)
+		mad_size = sizeof(struct opa_mad);
+	else
+		mad_size = sizeof(struct ib_mad);
+
+	pad = get_pad_size(hdr_len, data_len, mad_size);
 	message_size = hdr_len + data_len + pad;
 
 	if (ib_mad_kernel_rmpp_agent(mad_agent)) {
-		if (!rmpp_active && message_size > sizeof(struct ib_mad))
+		if (!rmpp_active && message_size > mad_size)
 			return ERR_PTR(-EINVAL);
 	} else
-		if (rmpp_active || message_size > sizeof(struct ib_mad))
+		if (rmpp_active || message_size > mad_size)
 			return ERR_PTR(-EINVAL);
 
-	size = rmpp_active ? hdr_len : sizeof(struct ib_mad);
+	size = rmpp_active ? hdr_len : mad_size;
 	buf = kzalloc(sizeof *mad_send_wr + size, gfp_mask);
 	if (!buf)
 		return ERR_PTR(-ENOMEM);
@@ -954,7 +1038,14 @@ struct ib_mad_send_buf * ib_create_send_mad(struct ib_mad_agent *mad_agent,
 	mad_send_wr->mad_agent_priv = mad_agent_priv;
 	mad_send_wr->sg_list[0].length = hdr_len;
 	mad_send_wr->sg_list[0].lkey = mad_agent->mr->lkey;
-	mad_send_wr->sg_list[1].length = sizeof(struct ib_mad) - hdr_len;
+
+	/* OPA MADs don't have to be the full 2048 bytes */
+	if (opa && base_version == OPA_MGMT_BASE_VERSION &&
+	    data_len < mad_size - hdr_len)
+		mad_send_wr->sg_list[1].length = data_len;
+	else
+		mad_send_wr->sg_list[1].length = mad_size - hdr_len;
+
 	mad_send_wr->sg_list[1].lkey = mad_agent->mr->lkey;
 
 	mad_send_wr->send_wr.wr_id = (unsigned long) mad_send_wr;
@@ -967,7 +1058,7 @@ struct ib_mad_send_buf * ib_create_send_mad(struct ib_mad_agent *mad_agent,
 	mad_send_wr->send_wr.wr.ud.pkey_index = pkey_index;
 
 	if (rmpp_active) {
-		ret = alloc_send_rmpp_list(mad_send_wr, gfp_mask);
+		ret = alloc_send_rmpp_list(mad_send_wr, mad_size, gfp_mask);
 		if (ret) {
 			kfree(buf);
 			return ERR_PTR(ret);
@@ -1237,7 +1328,7 @@ void ib_free_recv_mad(struct ib_mad_recv_wc *mad_recv_wc)
 					    recv_wc);
 		priv = container_of(mad_priv_hdr, struct ib_mad_private,
 				    header);
-		kmem_cache_free(ib_mad_cache, priv);
+		kfree(priv);
 	}
 }
 EXPORT_SYMBOL(ib_free_recv_mad);
@@ -1324,7 +1415,7 @@ static int check_vendor_class(struct ib_mad_mgmt_vendor_class *vendor_class)
 }
 
 static int find_vendor_oui(struct ib_mad_mgmt_vendor_class *vendor_class,
-			   char *oui)
+			   const char *oui)
 {
 	int i;
 
@@ -1622,13 +1713,13 @@ out:
 
 static struct ib_mad_agent_private *
 find_mad_agent(struct ib_mad_port_private *port_priv,
-	       struct ib_mad *mad)
+	       const struct ib_mad_hdr *mad_hdr)
 {
 	struct ib_mad_agent_private *mad_agent = NULL;
 	unsigned long flags;
 
 	spin_lock_irqsave(&port_priv->reg_lock, flags);
-	if (ib_response_mad(mad)) {
+	if (ib_response_mad(mad_hdr)) {
 		u32 hi_tid;
 		struct ib_mad_agent_private *entry;
 
@@ -1636,7 +1727,7 @@ find_mad_agent(struct ib_mad_port_private *port_priv,
 		 * Routing is based on high 32 bits of transaction ID
 		 * of MAD.
 		 */
-		hi_tid = be64_to_cpu(mad->mad_hdr.tid) >> 32;
+		hi_tid = be64_to_cpu(mad_hdr->tid) >> 32;
 		list_for_each_entry(entry, &port_priv->agent_list, agent_list) {
 			if (entry->agent.hi_tid == hi_tid) {
 				mad_agent = entry;
@@ -1648,45 +1739,45 @@ find_mad_agent(struct ib_mad_port_private *port_priv,
 		struct ib_mad_mgmt_method_table *method;
 		struct ib_mad_mgmt_vendor_class_table *vendor;
 		struct ib_mad_mgmt_vendor_class *vendor_class;
-		struct ib_vendor_mad *vendor_mad;
+		const struct ib_vendor_mad *vendor_mad;
 		int index;
 
 		/*
 		 * Routing is based on version, class, and method
 		 * For "newer" vendor MADs, also based on OUI
 		 */
-		if (mad->mad_hdr.class_version >= MAX_MGMT_VERSION)
+		if (mad_hdr->class_version >= MAX_MGMT_VERSION)
 			goto out;
-		if (!is_vendor_class(mad->mad_hdr.mgmt_class)) {
+		if (!is_vendor_class(mad_hdr->mgmt_class)) {
 			class = port_priv->version[
-					mad->mad_hdr.class_version].class;
+					mad_hdr->class_version].class;
 			if (!class)
 				goto out;
-			if (convert_mgmt_class(mad->mad_hdr.mgmt_class) >=
+			if (convert_mgmt_class(mad_hdr->mgmt_class) >=
 			    IB_MGMT_MAX_METHODS)
 				goto out;
 			method = class->method_table[convert_mgmt_class(
-							mad->mad_hdr.mgmt_class)];
+							mad_hdr->mgmt_class)];
 			if (method)
-				mad_agent = method->agent[mad->mad_hdr.method &
+				mad_agent = method->agent[mad_hdr->method &
 							  ~IB_MGMT_METHOD_RESP];
 		} else {
 			vendor = port_priv->version[
-					mad->mad_hdr.class_version].vendor;
+					mad_hdr->class_version].vendor;
 			if (!vendor)
 				goto out;
 			vendor_class = vendor->vendor_class[vendor_class_index(
-						mad->mad_hdr.mgmt_class)];
+						mad_hdr->mgmt_class)];
 			if (!vendor_class)
 				goto out;
 			/* Find matching OUI */
-			vendor_mad = (struct ib_vendor_mad *)mad;
+			vendor_mad = (const struct ib_vendor_mad *)mad_hdr;
 			index = find_vendor_oui(vendor_class, vendor_mad->oui);
 			if (index == -1)
 				goto out;
 			method = vendor_class->method_table[index];
 			if (method) {
-				mad_agent = method->agent[mad->mad_hdr.method &
+				mad_agent = method->agent[mad_hdr->method &
 							  ~IB_MGMT_METHOD_RESP];
 			}
 		}
@@ -1708,20 +1799,24 @@ out:
 	return mad_agent;
 }
 
-static int validate_mad(struct ib_mad *mad, u32 qp_num)
+static int validate_mad(const struct ib_mad_hdr *mad_hdr,
+			const struct ib_mad_qp_info *qp_info,
+			bool opa)
 {
 	int valid = 0;
+	u32 qp_num = qp_info->qp->qp_num;
 
 	/* Make sure MAD base version is understood */
-	if (mad->mad_hdr.base_version != IB_MGMT_BASE_VERSION) {
-		pr_err("MAD received with unsupported base version %d\n",
-			mad->mad_hdr.base_version);
+	if (mad_hdr->base_version != IB_MGMT_BASE_VERSION &&
+	    (!opa || mad_hdr->base_version != OPA_MGMT_BASE_VERSION)) {
+		pr_err("MAD received with unsupported base version %d %s\n",
+		       mad_hdr->base_version, opa ? "(opa)" : "");
 		goto out;
 	}
 
 	/* Filter SMI packets sent to other than QP0 */
-	if ((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED) ||
-	    (mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)) {
+	if ((mad_hdr->mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED) ||
+	    (mad_hdr->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)) {
 		if (qp_num == 0)
 			valid = 1;
 	} else {
@@ -1734,8 +1829,8 @@ out:
 	return valid;
 }
 
-static int is_data_mad(struct ib_mad_agent_private *mad_agent_priv,
-		       struct ib_mad_hdr *mad_hdr)
+static int is_rmpp_data_mad(const struct ib_mad_agent_private *mad_agent_priv,
+			    const struct ib_mad_hdr *mad_hdr)
 {
 	struct ib_rmpp_mad *rmpp_mad;
 
@@ -1747,16 +1842,16 @@ static int is_data_mad(struct ib_mad_agent_private *mad_agent_priv,
 		(rmpp_mad->rmpp_hdr.rmpp_type == IB_MGMT_RMPP_TYPE_DATA);
 }
 
-static inline int rcv_has_same_class(struct ib_mad_send_wr_private *wr,
-				     struct ib_mad_recv_wc *rwc)
+static inline int rcv_has_same_class(const struct ib_mad_send_wr_private *wr,
+				     const struct ib_mad_recv_wc *rwc)
 {
-	return ((struct ib_mad *)(wr->send_buf.mad))->mad_hdr.mgmt_class ==
+	return ((struct ib_mad_hdr *)(wr->send_buf.mad))->mgmt_class ==
 		rwc->recv_buf.mad->mad_hdr.mgmt_class;
 }
 
-static inline int rcv_has_same_gid(struct ib_mad_agent_private *mad_agent_priv,
-				   struct ib_mad_send_wr_private *wr,
-				   struct ib_mad_recv_wc *rwc )
+static inline int rcv_has_same_gid(const struct ib_mad_agent_private *mad_agent_priv,
+				   const struct ib_mad_send_wr_private *wr,
+				   const struct ib_mad_recv_wc *rwc )
 {
 	struct ib_ah_attr attr;
 	u8 send_resp, rcv_resp;
@@ -1765,8 +1860,8 @@ static inline int rcv_has_same_gid(struct ib_mad_agent_private *mad_agent_priv,
 	u8 port_num = mad_agent_priv->agent.port_num;
 	u8 lmc;
 
-	send_resp = ib_response_mad((struct ib_mad *)wr->send_buf.mad);
-	rcv_resp = ib_response_mad(rwc->recv_buf.mad);
+	send_resp = ib_response_mad((struct ib_mad_hdr *)wr->send_buf.mad);
+	rcv_resp = ib_response_mad(&rwc->recv_buf.mad->mad_hdr);
 
 	if (send_resp == rcv_resp)
 		/* both requests, or both responses. GIDs different */
@@ -1811,22 +1906,22 @@ static inline int is_direct(u8 class)
 }
 
 struct ib_mad_send_wr_private*
-ib_find_send_mad(struct ib_mad_agent_private *mad_agent_priv,
-		 struct ib_mad_recv_wc *wc)
+ib_find_send_mad(const struct ib_mad_agent_private *mad_agent_priv,
+		 const struct ib_mad_recv_wc *wc)
 {
 	struct ib_mad_send_wr_private *wr;
-	struct ib_mad *mad;
+	const struct ib_mad_hdr *mad_hdr;
 
-	mad = (struct ib_mad *)wc->recv_buf.mad;
+	mad_hdr = &wc->recv_buf.mad->mad_hdr;
 
 	list_for_each_entry(wr, &mad_agent_priv->wait_list, agent_list) {
-		if ((wr->tid == mad->mad_hdr.tid) &&
+		if ((wr->tid == mad_hdr->tid) &&
 		    rcv_has_same_class(wr, wc) &&
 		    /*
 		     * Don't check GID for direct routed MADs.
 		     * These might have permissive LIDs.
 		     */
-		    (is_direct(wc->recv_buf.mad->mad_hdr.mgmt_class) ||
+		    (is_direct(mad_hdr->mgmt_class) ||
 		     rcv_has_same_gid(mad_agent_priv, wr, wc)))
 			return (wr->status == IB_WC_SUCCESS) ? wr : NULL;
 	}
@@ -1836,15 +1931,15 @@ ib_find_send_mad(struct ib_mad_agent_private *mad_agent_priv,
 	 * been notified that the send has completed
 	 */
 	list_for_each_entry(wr, &mad_agent_priv->send_list, agent_list) {
-		if (is_data_mad(mad_agent_priv, wr->send_buf.mad) &&
-		    wr->tid == mad->mad_hdr.tid &&
+		if (is_rmpp_data_mad(mad_agent_priv, wr->send_buf.mad) &&
+		    wr->tid == mad_hdr->tid &&
 		    wr->timeout &&
 		    rcv_has_same_class(wr, wc) &&
 		    /*
 		     * Don't check GID for direct routed MADs.
 		     * These might have permissive LIDs.
 		     */
-		    (is_direct(wc->recv_buf.mad->mad_hdr.mgmt_class) ||
+		    (is_direct(mad_hdr->mgmt_class) ||
 		     rcv_has_same_gid(mad_agent_priv, wr, wc)))
 			/* Verify request has not been canceled */
 			return (wr->status == IB_WC_SUCCESS) ? wr : NULL;
@@ -1879,7 +1974,7 @@ static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
 	}
 
 	/* Complete corresponding request */
-	if (ib_response_mad(mad_recv_wc->recv_buf.mad)) {
+	if (ib_response_mad(&mad_recv_wc->recv_buf.mad->mad_hdr)) {
 		spin_lock_irqsave(&mad_agent_priv->lock, flags);
 		mad_send_wr = ib_find_send_mad(mad_agent_priv, mad_recv_wc);
 		if (!mad_send_wr) {
@@ -1924,26 +2019,163 @@ static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
 	}
 }
 
-static bool generate_unmatched_resp(struct ib_mad_private *recv,
-				    struct ib_mad_private *response)
+static enum smi_action handle_ib_smi(const struct ib_mad_port_private *port_priv,
+				     const struct ib_mad_qp_info *qp_info,
+				     const struct ib_wc *wc,
+				     int port_num,
+				     struct ib_mad_private *recv,
+				     struct ib_mad_private *response)
+{
+	enum smi_forward_action retsmi;
+	struct ib_smp *smp = (struct ib_smp *)recv->mad;
+
+	if (smi_handle_dr_smp_recv(smp,
+				   port_priv->device->node_type,
+				   port_num,
+				   port_priv->device->phys_port_cnt) ==
+				   IB_SMI_DISCARD)
+		return IB_SMI_DISCARD;
+
+	retsmi = smi_check_forward_dr_smp(smp);
+	if (retsmi == IB_SMI_LOCAL)
+		return IB_SMI_HANDLE;
+
+	if (retsmi == IB_SMI_SEND) { /* don't forward */
+		if (smi_handle_dr_smp_send(smp,
+					   port_priv->device->node_type,
+					   port_num) == IB_SMI_DISCARD)
+			return IB_SMI_DISCARD;
+
+		if (smi_check_local_smp(smp, port_priv->device) == IB_SMI_DISCARD)
+			return IB_SMI_DISCARD;
+	} else if (port_priv->device->node_type == RDMA_NODE_IB_SWITCH) {
+		/* forward case for switches */
+		memcpy(response, recv, mad_priv_size(response));
+		response->header.recv_wc.wc = &response->header.wc;
+		response->header.recv_wc.recv_buf.mad = (struct ib_mad *)response->mad;
+		response->header.recv_wc.recv_buf.grh = &response->grh;
+
+		agent_send_response((const struct ib_mad_hdr *)response->mad,
+				    &response->grh, wc,
+				    port_priv->device,
+				    smi_get_fwd_port(smp),
+				    qp_info->qp->qp_num,
+				    response->mad_size,
+				    false);
+
+		return IB_SMI_DISCARD;
+	}
+	return IB_SMI_HANDLE;
+}
+
+static bool generate_unmatched_resp(const struct ib_mad_private *recv,
+				    struct ib_mad_private *response,
+				    size_t *resp_len, bool opa)
 {
-	if (recv->mad.mad.mad_hdr.method == IB_MGMT_METHOD_GET ||
-	    recv->mad.mad.mad_hdr.method == IB_MGMT_METHOD_SET) {
-		memcpy(response, recv, sizeof *response);
+	const struct ib_mad_hdr *recv_hdr = (const struct ib_mad_hdr *)recv->mad;
+	struct ib_mad_hdr *resp_hdr = (struct ib_mad_hdr *)response->mad;
+
+	if (recv_hdr->method == IB_MGMT_METHOD_GET ||
+	    recv_hdr->method == IB_MGMT_METHOD_SET) {
+		memcpy(response, recv, mad_priv_size(response));
 		response->header.recv_wc.wc = &response->header.wc;
-		response->header.recv_wc.recv_buf.mad = &response->mad.mad;
+		response->header.recv_wc.recv_buf.mad = (struct ib_mad *)response->mad;
 		response->header.recv_wc.recv_buf.grh = &response->grh;
-		response->mad.mad.mad_hdr.method = IB_MGMT_METHOD_GET_RESP;
-		response->mad.mad.mad_hdr.status =
-			cpu_to_be16(IB_MGMT_MAD_STATUS_UNSUPPORTED_METHOD_ATTRIB);
-		if (recv->mad.mad.mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
-			response->mad.mad.mad_hdr.status |= IB_SMP_DIRECTION;
+		resp_hdr->method = IB_MGMT_METHOD_GET_RESP;
+		resp_hdr->status = cpu_to_be16(IB_MGMT_MAD_STATUS_UNSUPPORTED_METHOD_ATTRIB);
+		if (recv_hdr->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
+			resp_hdr->status |= IB_SMP_DIRECTION;
+
+		if (opa && recv_hdr->base_version == OPA_MGMT_BASE_VERSION) {
+			if (recv_hdr->mgmt_class ==
+			    IB_MGMT_CLASS_SUBN_LID_ROUTED ||
+			    recv_hdr->mgmt_class ==
+			    IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
+				*resp_len = opa_get_smp_header_size(
+							(struct opa_smp *)recv->mad);
+			else
+				*resp_len = sizeof(struct ib_mad_hdr);
+		}
 
 		return true;
 	} else {
 		return false;
 	}
 }
+
+static enum smi_action
+handle_opa_smi(struct ib_mad_port_private *port_priv,
+	       struct ib_mad_qp_info *qp_info,
+	       struct ib_wc *wc,
+	       int port_num,
+	       struct ib_mad_private *recv,
+	       struct ib_mad_private *response)
+{
+	enum smi_forward_action retsmi;
+	struct opa_smp *smp = (struct opa_smp *)recv->mad;
+
+	if (opa_smi_handle_dr_smp_recv(smp,
+				   port_priv->device->node_type,
+				   port_num,
+				   port_priv->device->phys_port_cnt) ==
+				   IB_SMI_DISCARD)
+		return IB_SMI_DISCARD;
+
+	retsmi = opa_smi_check_forward_dr_smp(smp);
+	if (retsmi == IB_SMI_LOCAL)
+		return IB_SMI_HANDLE;
+
+	if (retsmi == IB_SMI_SEND) { /* don't forward */
+		if (opa_smi_handle_dr_smp_send(smp,
+					   port_priv->device->node_type,
+					   port_num) == IB_SMI_DISCARD)
+			return IB_SMI_DISCARD;
+
+		if (opa_smi_check_local_smp(smp, port_priv->device) ==
+		    IB_SMI_DISCARD)
+			return IB_SMI_DISCARD;
+
+	} else if (port_priv->device->node_type == RDMA_NODE_IB_SWITCH) {
+		/* forward case for switches */
+		memcpy(response, recv, mad_priv_size(response));
+		response->header.recv_wc.wc = &response->header.wc;
+		response->header.recv_wc.recv_buf.opa_mad =
+				(struct opa_mad *)response->mad;
+		response->header.recv_wc.recv_buf.grh = &response->grh;
+
+		agent_send_response((const struct ib_mad_hdr *)response->mad,
+				    &response->grh, wc,
+				    port_priv->device,
+				    opa_smi_get_fwd_port(smp),
+				    qp_info->qp->qp_num,
+				    recv->header.wc.byte_len,
+				    true);
+
+		return IB_SMI_DISCARD;
+	}
+
+	return IB_SMI_HANDLE;
+}
+
+static enum smi_action
+handle_smi(struct ib_mad_port_private *port_priv,
+	   struct ib_mad_qp_info *qp_info,
+	   struct ib_wc *wc,
+	   int port_num,
+	   struct ib_mad_private *recv,
+	   struct ib_mad_private *response,
+	   bool opa)
+{
+	struct ib_mad_hdr *mad_hdr = (struct ib_mad_hdr *)recv->mad;
+
+	if (opa && mad_hdr->base_version == OPA_MGMT_BASE_VERSION &&
+	    mad_hdr->class_version == OPA_SMI_CLASS_VERSION)
+		return handle_opa_smi(port_priv, qp_info, wc, port_num, recv,
+				      response);
+
+	return handle_ib_smi(port_priv, qp_info, wc, port_num, recv, response);
+}
+
 static void ib_mad_recv_done_handler(struct ib_mad_port_private *port_priv,
 				     struct ib_wc *wc)
 {
@@ -1954,35 +2186,49 @@ static void ib_mad_recv_done_handler(struct ib_mad_port_private *port_priv,
 	struct ib_mad_agent_private *mad_agent;
 	int port_num;
 	int ret = IB_MAD_RESULT_SUCCESS;
+	size_t mad_size;
+	u16 resp_mad_pkey_index = 0;
+	bool opa;
 
 	mad_list = (struct ib_mad_list_head *)(unsigned long)wc->wr_id;
 	qp_info = mad_list->mad_queue->qp_info;
 	dequeue_mad(mad_list);
 
+	opa = rdma_cap_opa_mad(qp_info->port_priv->device,
+			       qp_info->port_priv->port_num);
+
 	mad_priv_hdr = container_of(mad_list, struct ib_mad_private_header,
 				    mad_list);
 	recv = container_of(mad_priv_hdr, struct ib_mad_private, header);
 	ib_dma_unmap_single(port_priv->device,
 			    recv->header.mapping,
-			    sizeof(struct ib_mad_private) -
-			      sizeof(struct ib_mad_private_header),
+			    mad_priv_dma_size(recv),
 			    DMA_FROM_DEVICE);
 
 	/* Setup MAD receive work completion from "normal" work completion */
 	recv->header.wc = *wc;
 	recv->header.recv_wc.wc = &recv->header.wc;
-	recv->header.recv_wc.mad_len = sizeof(struct ib_mad);
-	recv->header.recv_wc.recv_buf.mad = &recv->mad.mad;
+
+	if (opa && ((struct ib_mad_hdr *)(recv->mad))->base_version == OPA_MGMT_BASE_VERSION) {
+		recv->header.recv_wc.mad_len = wc->byte_len - sizeof(struct ib_grh);
+		recv->header.recv_wc.mad_seg_size = sizeof(struct opa_mad);
+	} else {
+		recv->header.recv_wc.mad_len = sizeof(struct ib_mad);
+		recv->header.recv_wc.mad_seg_size = sizeof(struct ib_mad);
+	}
+
+	recv->header.recv_wc.recv_buf.mad = (struct ib_mad *)recv->mad;
 	recv->header.recv_wc.recv_buf.grh = &recv->grh;
 
 	if (atomic_read(&qp_info->snoop_count))
 		snoop_recv(qp_info, &recv->header.recv_wc, IB_MAD_SNOOP_RECVS);
 
 	/* Validate MAD */
-	if (!validate_mad(&recv->mad.mad, qp_info->qp->qp_num))
+	if (!validate_mad((const struct ib_mad_hdr *)recv->mad, qp_info, opa))
 		goto out;
 
-	response = kmem_cache_alloc(ib_mad_cache, GFP_KERNEL);
+	mad_size = recv->mad_size;
+	response = alloc_mad_private(mad_size, GFP_KERNEL);
 	if (!response) {
 		dev_err(&port_priv->device->dev,
 			"ib_mad_recv_done_handler no memory for response buffer\n");
@@ -1994,69 +2240,43 @@ static void ib_mad_recv_done_handler(struct ib_mad_port_private *port_priv,
 	else
 		port_num = port_priv->port_num;
 
-	if (recv->mad.mad.mad_hdr.mgmt_class ==
+	if (((struct ib_mad_hdr *)recv->mad)->mgmt_class ==
 	    IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) {
-		enum smi_forward_action retsmi;
-
-		if (smi_handle_dr_smp_recv(&recv->mad.smp,
-					   port_priv->device->node_type,
-					   port_num,
-					   port_priv->device->phys_port_cnt) ==
-					   IB_SMI_DISCARD)
+		if (handle_smi(port_priv, qp_info, wc, port_num, recv,
+			       response, opa)
+		    == IB_SMI_DISCARD)
 			goto out;
-
-		retsmi = smi_check_forward_dr_smp(&recv->mad.smp);
-		if (retsmi == IB_SMI_LOCAL)
-			goto local;
-
-		if (retsmi == IB_SMI_SEND) { /* don't forward */
-			if (smi_handle_dr_smp_send(&recv->mad.smp,
-						   port_priv->device->node_type,
-						   port_num) == IB_SMI_DISCARD)
-				goto out;
-
-			if (smi_check_local_smp(&recv->mad.smp, port_priv->device) == IB_SMI_DISCARD)
-				goto out;
-		} else if (port_priv->device->node_type == RDMA_NODE_IB_SWITCH) {
-			/* forward case for switches */
-			memcpy(response, recv, sizeof(*response));
-			response->header.recv_wc.wc = &response->header.wc;
-			response->header.recv_wc.recv_buf.mad = &response->mad.mad;
-			response->header.recv_wc.recv_buf.grh = &response->grh;
-
-			agent_send_response(&response->mad.mad,
-					    &response->grh, wc,
-					    port_priv->device,
-					    smi_get_fwd_port(&recv->mad.smp),
-					    qp_info->qp->qp_num);
-
-			goto out;
-		}
 	}
 
-local:
 	/* Give driver "right of first refusal" on incoming MAD */
 	if (port_priv->device->process_mad) {
 		ret = port_priv->device->process_mad(port_priv->device, 0,
 						     port_priv->port_num,
 						     wc, &recv->grh,
-						     &recv->mad.mad,
-						     &response->mad.mad);
+						     (const struct ib_mad_hdr *)recv->mad,
+						     recv->mad_size,
+						     (struct ib_mad_hdr *)response->mad,
+						     &mad_size, &resp_mad_pkey_index);
+
+		if (opa)
+			wc->pkey_index = resp_mad_pkey_index;
+
 		if (ret & IB_MAD_RESULT_SUCCESS) {
 			if (ret & IB_MAD_RESULT_CONSUMED)
 				goto out;
 			if (ret & IB_MAD_RESULT_REPLY) {
-				agent_send_response(&response->mad.mad,
+				agent_send_response((const struct ib_mad_hdr *)response->mad,
 						    &recv->grh, wc,
 						    port_priv->device,
 						    port_num,
-						    qp_info->qp->qp_num);
+						    qp_info->qp->qp_num,
+						    mad_size, opa);
 				goto out;
 			}
 		}
 	}
 
-	mad_agent = find_mad_agent(port_priv, &recv->mad.mad);
+	mad_agent = find_mad_agent(port_priv, (const struct ib_mad_hdr *)recv->mad);
 	if (mad_agent) {
 		ib_mad_complete_recv(mad_agent, &recv->header.recv_wc);
 		/*
@@ -2065,17 +2285,17 @@ local:
 		 */
 		recv = NULL;
 	} else if ((ret & IB_MAD_RESULT_SUCCESS) &&
-		   generate_unmatched_resp(recv, response)) {
-		agent_send_response(&response->mad.mad, &recv->grh, wc,
-				    port_priv->device, port_num, qp_info->qp->qp_num);
+		   generate_unmatched_resp(recv, response, &mad_size, opa)) {
+		agent_send_response((const struct ib_mad_hdr *)response->mad, &recv->grh, wc,
+				    port_priv->device, port_num,
+				    qp_info->qp->qp_num, mad_size, opa);
 	}
 
 out:
 	/* Post another receive request for this QP */
 	if (response) {
 		ib_mad_post_receive_mads(qp_info, response);
-		if (recv)
-			kmem_cache_free(ib_mad_cache, recv);
+		kfree(recv);
 	} else
 		ib_mad_post_receive_mads(qp_info, recv);
 }
@@ -2411,7 +2631,8 @@ find_send_wr(struct ib_mad_agent_private *mad_agent_priv,
 
 	list_for_each_entry(mad_send_wr, &mad_agent_priv->send_list,
 			    agent_list) {
-		if (is_data_mad(mad_agent_priv, mad_send_wr->send_buf.mad) &&
+		if (is_rmpp_data_mad(mad_agent_priv,
+				     mad_send_wr->send_buf.mad) &&
 		    &mad_send_wr->send_buf == send_buf)
 			return mad_send_wr;
 	}
@@ -2468,10 +2689,14 @@ static void local_completions(struct work_struct *work)
 	int free_mad;
 	struct ib_wc wc;
 	struct ib_mad_send_wc mad_send_wc;
+	bool opa;
 
 	mad_agent_priv =
 		container_of(work, struct ib_mad_agent_private, local_work);
 
+	opa = rdma_cap_opa_mad(mad_agent_priv->qp_info->port_priv->device,
+			       mad_agent_priv->qp_info->port_priv->port_num);
+
 	spin_lock_irqsave(&mad_agent_priv->lock, flags);
 	while (!list_empty(&mad_agent_priv->local_list)) {
 		local = list_entry(mad_agent_priv->local_list.next,
@@ -2481,6 +2706,7 @@ static void local_completions(struct work_struct *work)
 		spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
 		free_mad = 0;
 		if (local->mad_priv) {
+			u8 base_version;
 			recv_mad_agent = local->recv_mad_agent;
 			if (!recv_mad_agent) {
 				dev_err(&mad_agent_priv->agent.device->dev,
@@ -2496,17 +2722,26 @@ static void local_completions(struct work_struct *work)
 			build_smp_wc(recv_mad_agent->agent.qp,
 				     (unsigned long) local->mad_send_wr,
 				     be16_to_cpu(IB_LID_PERMISSIVE),
-				     0, recv_mad_agent->agent.port_num, &wc);
+				     local->mad_send_wr->send_wr.wr.ud.pkey_index,
+				     recv_mad_agent->agent.port_num, &wc);
 
 			local->mad_priv->header.recv_wc.wc = &wc;
-			local->mad_priv->header.recv_wc.mad_len =
-						sizeof(struct ib_mad);
+
+			base_version = ((struct ib_mad_hdr *)(local->mad_priv->mad))->base_version;
+			if (opa && base_version == OPA_MGMT_BASE_VERSION) {
+				local->mad_priv->header.recv_wc.mad_len = local->return_wc_byte_len;
+				local->mad_priv->header.recv_wc.mad_seg_size = sizeof(struct opa_mad);
+			} else {
+				local->mad_priv->header.recv_wc.mad_len = sizeof(struct ib_mad);
+				local->mad_priv->header.recv_wc.mad_seg_size = sizeof(struct ib_mad);
+			}
+
 			INIT_LIST_HEAD(&local->mad_priv->header.recv_wc.rmpp_list);
 			list_add(&local->mad_priv->header.recv_wc.recv_buf.list,
 				 &local->mad_priv->header.recv_wc.rmpp_list);
 			local->mad_priv->header.recv_wc.recv_buf.grh = NULL;
 			local->mad_priv->header.recv_wc.recv_buf.mad =
-						&local->mad_priv->mad.mad;
+						(struct ib_mad *)local->mad_priv->mad;
 			if (atomic_read(&recv_mad_agent->qp_info->snoop_count))
 				snoop_recv(recv_mad_agent->qp_info,
 					  &local->mad_priv->header.recv_wc,
@@ -2534,7 +2769,7 @@ local_send_completion:
 		spin_lock_irqsave(&mad_agent_priv->lock, flags);
 		atomic_dec(&mad_agent_priv->refcount);
 		if (free_mad)
-			kmem_cache_free(ib_mad_cache, local->mad_priv);
+			kfree(local->mad_priv);
 		kfree(local);
 	}
 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
@@ -2649,7 +2884,6 @@ static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
 	struct ib_mad_queue *recv_queue = &qp_info->recv_queue;
 
 	/* Initialize common scatter list fields */
-	sg_list.length = sizeof *mad_priv - sizeof mad_priv->header;
 	sg_list.lkey = (*qp_info->port_priv->mr).lkey;
 
 	/* Initialize common receive WR fields */
@@ -2663,7 +2897,8 @@ static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
 			mad_priv = mad;
 			mad = NULL;
 		} else {
-			mad_priv = kmem_cache_alloc(ib_mad_cache, GFP_KERNEL);
+			mad_priv = alloc_mad_private(port_mad_size(qp_info->port_priv),
+						     GFP_ATOMIC);
 			if (!mad_priv) {
 				dev_err(&qp_info->port_priv->device->dev,
 					"No memory for receive buffer\n");
@@ -2671,10 +2906,10 @@ static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
 				break;
 			}
 		}
+		sg_list.length = mad_priv_dma_size(mad_priv);
 		sg_list.addr = ib_dma_map_single(qp_info->port_priv->device,
 						 &mad_priv->grh,
-						 sizeof *mad_priv -
-						   sizeof mad_priv->header,
+						 mad_priv_dma_size(mad_priv),
 						 DMA_FROM_DEVICE);
 		if (unlikely(ib_dma_mapping_error(qp_info->port_priv->device,
 						  sg_list.addr))) {
@@ -2698,10 +2933,9 @@ static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
 			spin_unlock_irqrestore(&recv_queue->lock, flags);
 			ib_dma_unmap_single(qp_info->port_priv->device,
 					    mad_priv->header.mapping,
-					    sizeof *mad_priv -
-					      sizeof mad_priv->header,
+					    mad_priv_dma_size(mad_priv),
 					    DMA_FROM_DEVICE);
-			kmem_cache_free(ib_mad_cache, mad_priv);
+			kfree(mad_priv);
 			dev_err(&qp_info->port_priv->device->dev,
 				"ib_post_recv failed: %d\n", ret);
 			break;
@@ -2738,10 +2972,9 @@ static void cleanup_recv_queue(struct ib_mad_qp_info *qp_info)
 
 		ib_dma_unmap_single(qp_info->port_priv->device,
 				    recv->header.mapping,
-				    sizeof(struct ib_mad_private) -
-				      sizeof(struct ib_mad_private_header),
+				    mad_priv_dma_size(recv),
 				    DMA_FROM_DEVICE);
-		kmem_cache_free(ib_mad_cache, recv);
+		kfree(recv);
 	}
 
 	qp_info->recv_queue.count = 0;
@@ -2922,6 +3155,14 @@ static int ib_mad_port_open(struct ib_device *device,
 	unsigned long flags;
 	char name[sizeof "ib_mad123"];
 	int has_smi;
+	struct ib_cq_init_attr cq_attr = {};
+
+	if (WARN_ON(rdma_max_mad_size(device, port_num) < IB_MGMT_MAD_SIZE))
+		return -EFAULT;
+
+	if (WARN_ON(rdma_cap_opa_mad(device, port_num) &&
+		    rdma_max_mad_size(device, port_num) < OPA_MGMT_MAD_SIZE))
+		return -EFAULT;
 
 	/* Create new device info */
 	port_priv = kzalloc(sizeof *port_priv, GFP_KERNEL);
@@ -2938,13 +3179,14 @@ static int ib_mad_port_open(struct ib_device *device,
 	init_mad_qp(port_priv, &port_priv->qp_info[1]);
 
 	cq_size = mad_sendq_size + mad_recvq_size;
-	has_smi = rdma_port_get_link_layer(device, port_num) == IB_LINK_LAYER_INFINIBAND;
+	has_smi = rdma_cap_ib_smi(device, port_num);
 	if (has_smi)
 		cq_size *= 2;
 
+	cq_attr.cqe = cq_size;
 	port_priv->cq = ib_create_cq(port_priv->device,
 				     ib_mad_thread_completion_handler,
-				     NULL, port_priv, cq_size, 0);
+				     NULL, port_priv, &cq_attr);
 	if (IS_ERR(port_priv->cq)) {
 		dev_err(&device->dev, "Couldn't create ib_mad CQ\n");
 		ret = PTR_ERR(port_priv->cq);
@@ -3057,9 +3299,6 @@ static void ib_mad_init_device(struct ib_device *device)
 {
 	int start, end, i;
 
-	if (rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
-		return;
-
 	if (device->node_type == RDMA_NODE_IB_SWITCH) {
 		start = 0;
 		end   = 0;
@@ -3069,6 +3308,9 @@ static void ib_mad_init_device(struct ib_device *device)
 	}
 
 	for (i = start; i <= end; i++) {
+		if (!rdma_cap_ib_mad(device, i))
+			continue;
+
 		if (ib_mad_port_open(device, i)) {
 			dev_err(&device->dev, "Couldn't open port %d\n", i);
 			goto error;
@@ -3086,40 +3328,39 @@ error_agent:
 		dev_err(&device->dev, "Couldn't close port %d\n", i);
 
 error:
-	i--;
+	while (--i >= start) {
+		if (!rdma_cap_ib_mad(device, i))
+			continue;
 
-	while (i >= start) {
 		if (ib_agent_port_close(device, i))
 			dev_err(&device->dev,
 				"Couldn't close port %d for agents\n", i);
 		if (ib_mad_port_close(device, i))
 			dev_err(&device->dev, "Couldn't close port %d\n", i);
-		i--;
 	}
 }
 
 static void ib_mad_remove_device(struct ib_device *device)
 {
-	int i, num_ports, cur_port;
-
-	if (rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
-		return;
+	int start, end, i;
 
 	if (device->node_type == RDMA_NODE_IB_SWITCH) {
-		num_ports = 1;
-		cur_port = 0;
+		start = 0;
+		end   = 0;
 	} else {
-		num_ports = device->phys_port_cnt;
-		cur_port = 1;
+		start = 1;
+		end   = device->phys_port_cnt;
 	}
-	for (i = 0; i < num_ports; i++, cur_port++) {
-		if (ib_agent_port_close(device, cur_port))
+
+	for (i = start; i <= end; i++) {
+		if (!rdma_cap_ib_mad(device, i))
+			continue;
+
+		if (ib_agent_port_close(device, i))
 			dev_err(&device->dev,
-				"Couldn't close port %d for agents\n",
-				cur_port);
-		if (ib_mad_port_close(device, cur_port))
-			dev_err(&device->dev, "Couldn't close port %d\n",
-				cur_port);
+				"Couldn't close port %d for agents\n", i);
+		if (ib_mad_port_close(device, i))
+			dev_err(&device->dev, "Couldn't close port %d\n", i);
 	}
 }
 
@@ -3131,45 +3372,25 @@ static struct ib_client mad_client = {
 
 static int __init ib_mad_init_module(void)
 {
-	int ret;
-
 	mad_recvq_size = min(mad_recvq_size, IB_MAD_QP_MAX_SIZE);
 	mad_recvq_size = max(mad_recvq_size, IB_MAD_QP_MIN_SIZE);
 
 	mad_sendq_size = min(mad_sendq_size, IB_MAD_QP_MAX_SIZE);
 	mad_sendq_size = max(mad_sendq_size, IB_MAD_QP_MIN_SIZE);
 
-	ib_mad_cache = kmem_cache_create("ib_mad",
-					 sizeof(struct ib_mad_private),
-					 0,
-					 SLAB_HWCACHE_ALIGN,
-					 NULL);
-	if (!ib_mad_cache) {
-		pr_err("Couldn't create ib_mad cache\n");
-		ret = -ENOMEM;
-		goto error1;
-	}
-
 	INIT_LIST_HEAD(&ib_mad_port_list);
 
 	if (ib_register_client(&mad_client)) {
 		pr_err("Couldn't register ib_mad client\n");
-		ret = -EINVAL;
-		goto error2;
+		return -EINVAL;
 	}
 
 	return 0;
-
-error2:
-	kmem_cache_destroy(ib_mad_cache);
-error1:
-	return ret;
 }
 
 static void __exit ib_mad_cleanup_module(void)
 {
 	ib_unregister_client(&mad_client);
-	kmem_cache_destroy(ib_mad_cache);
 }
 
 module_init(ib_mad_init_module);
diff --git a/drivers/infiniband/core/mad_priv.h b/drivers/infiniband/core/mad_priv.h
index d1a0b0ee9444..5be89f98928f 100644
--- a/drivers/infiniband/core/mad_priv.h
+++ b/drivers/infiniband/core/mad_priv.h
@@ -41,6 +41,7 @@
 #include <linux/workqueue.h>
 #include <rdma/ib_mad.h>
 #include <rdma/ib_smi.h>
+#include <rdma/opa_smi.h>
 
 #define IB_MAD_QPS_CORE		2 /* Always QP0 and QP1 as a minimum */
 
@@ -56,7 +57,7 @@
 
 /* Registration table sizes */
 #define MAX_MGMT_CLASS		80
-#define MAX_MGMT_VERSION	8
+#define MAX_MGMT_VERSION	0x83
 #define MAX_MGMT_OUI		8
 #define MAX_MGMT_VENDOR_RANGE2	(IB_MGMT_CLASS_VENDOR_RANGE2_END - \
 				IB_MGMT_CLASS_VENDOR_RANGE2_START + 1)
@@ -75,12 +76,9 @@ struct ib_mad_private_header {
 
 struct ib_mad_private {
 	struct ib_mad_private_header header;
+	size_t mad_size;
 	struct ib_grh grh;
-	union {
-		struct ib_mad mad;
-		struct ib_rmpp_mad rmpp_mad;
-		struct ib_smp smp;
-	} mad;
+	u8 mad[0];
 } __attribute__ ((packed));
 
 struct ib_rmpp_segment {
@@ -150,6 +148,7 @@ struct ib_mad_local_private {
 	struct ib_mad_private *mad_priv;
 	struct ib_mad_agent_private *recv_mad_agent;
 	struct ib_mad_send_wr_private *mad_send_wr;
+	size_t return_wc_byte_len;
 };
 
 struct ib_mad_mgmt_method_table {
@@ -213,8 +212,8 @@ struct ib_mad_port_private {
 int ib_send_mad(struct ib_mad_send_wr_private *mad_send_wr);
 
 struct ib_mad_send_wr_private *
-ib_find_send_mad(struct ib_mad_agent_private *mad_agent_priv,
-		 struct ib_mad_recv_wc *mad_recv_wc);
+ib_find_send_mad(const struct ib_mad_agent_private *mad_agent_priv,
+		 const struct ib_mad_recv_wc *mad_recv_wc);
 
 void ib_mad_complete_send_wr(struct ib_mad_send_wr_private *mad_send_wr,
 			     struct ib_mad_send_wc *mad_send_wc);
diff --git a/drivers/infiniband/core/mad_rmpp.c b/drivers/infiniband/core/mad_rmpp.c
index f37878c9c06e..382941b46e43 100644
--- a/drivers/infiniband/core/mad_rmpp.c
+++ b/drivers/infiniband/core/mad_rmpp.c
@@ -1,6 +1,7 @@
 /*
  * Copyright (c) 2005 Intel Inc. All rights reserved.
  * Copyright (c) 2005-2006 Voltaire, Inc. All rights reserved.
+ * Copyright (c) 2014 Intel Corporation.  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
@@ -67,6 +68,7 @@ struct mad_rmpp_recv {
 	u8 mgmt_class;
 	u8 class_version;
 	u8 method;
+	u8 base_version;
 };
 
 static inline void deref_rmpp_recv(struct mad_rmpp_recv *rmpp_recv)
@@ -139,7 +141,8 @@ static void ack_recv(struct mad_rmpp_recv *rmpp_recv,
 	hdr_len = ib_get_mad_data_offset(recv_wc->recv_buf.mad->mad_hdr.mgmt_class);
 	msg = ib_create_send_mad(&rmpp_recv->agent->agent, recv_wc->wc->src_qp,
 				 recv_wc->wc->pkey_index, 1, hdr_len,
-				 0, GFP_KERNEL);
+				 0, GFP_KERNEL,
+				 IB_MGMT_BASE_VERSION);
 	if (IS_ERR(msg))
 		return;
 
@@ -165,7 +168,8 @@ static struct ib_mad_send_buf *alloc_response_msg(struct ib_mad_agent *agent,
 	hdr_len = ib_get_mad_data_offset(recv_wc->recv_buf.mad->mad_hdr.mgmt_class);
 	msg = ib_create_send_mad(agent, recv_wc->wc->src_qp,
 				 recv_wc->wc->pkey_index, 1,
-				 hdr_len, 0, GFP_KERNEL);
+				 hdr_len, 0, GFP_KERNEL,
+				 IB_MGMT_BASE_VERSION);
 	if (IS_ERR(msg))
 		ib_destroy_ah(ah);
 	else {
@@ -316,6 +320,7 @@ create_rmpp_recv(struct ib_mad_agent_private *agent,
 	rmpp_recv->mgmt_class = mad_hdr->mgmt_class;
 	rmpp_recv->class_version = mad_hdr->class_version;
 	rmpp_recv->method  = mad_hdr->method;
+	rmpp_recv->base_version  = mad_hdr->base_version;
 	return rmpp_recv;
 
 error:	kfree(rmpp_recv);
@@ -431,14 +436,23 @@ static inline int get_mad_len(struct mad_rmpp_recv *rmpp_recv)
 {
 	struct ib_rmpp_mad *rmpp_mad;
 	int hdr_size, data_size, pad;
+	bool opa = rdma_cap_opa_mad(rmpp_recv->agent->qp_info->port_priv->device,
+				    rmpp_recv->agent->qp_info->port_priv->port_num);
 
 	rmpp_mad = (struct ib_rmpp_mad *)rmpp_recv->cur_seg_buf->mad;
 
 	hdr_size = ib_get_mad_data_offset(rmpp_mad->mad_hdr.mgmt_class);
-	data_size = sizeof(struct ib_rmpp_mad) - hdr_size;
-	pad = IB_MGMT_RMPP_DATA - be32_to_cpu(rmpp_mad->rmpp_hdr.paylen_newwin);
-	if (pad > IB_MGMT_RMPP_DATA || pad < 0)
-		pad = 0;
+	if (opa && rmpp_recv->base_version == OPA_MGMT_BASE_VERSION) {
+		data_size = sizeof(struct opa_rmpp_mad) - hdr_size;
+		pad = OPA_MGMT_RMPP_DATA - be32_to_cpu(rmpp_mad->rmpp_hdr.paylen_newwin);
+		if (pad > OPA_MGMT_RMPP_DATA || pad < 0)
+			pad = 0;
+	} else {
+		data_size = sizeof(struct ib_rmpp_mad) - hdr_size;
+		pad = IB_MGMT_RMPP_DATA - be32_to_cpu(rmpp_mad->rmpp_hdr.paylen_newwin);
+		if (pad > IB_MGMT_RMPP_DATA || pad < 0)
+			pad = 0;
+	}
 
 	return hdr_size + rmpp_recv->seg_num * data_size - pad;
 }
@@ -570,13 +584,14 @@ static int send_next_seg(struct ib_mad_send_wr_private *mad_send_wr)
 
 	if (mad_send_wr->seg_num == 1) {
 		rmpp_mad->rmpp_hdr.rmpp_rtime_flags |= IB_MGMT_RMPP_FLAG_FIRST;
-		paylen = mad_send_wr->send_buf.seg_count * IB_MGMT_RMPP_DATA -
-			 mad_send_wr->pad;
+		paylen = (mad_send_wr->send_buf.seg_count *
+			  mad_send_wr->send_buf.seg_rmpp_size) -
+			  mad_send_wr->pad;
 	}
 
 	if (mad_send_wr->seg_num == mad_send_wr->send_buf.seg_count) {
 		rmpp_mad->rmpp_hdr.rmpp_rtime_flags |= IB_MGMT_RMPP_FLAG_LAST;
-		paylen = IB_MGMT_RMPP_DATA - mad_send_wr->pad;
+		paylen = mad_send_wr->send_buf.seg_rmpp_size - mad_send_wr->pad;
 	}
 	rmpp_mad->rmpp_hdr.paylen_newwin = cpu_to_be32(paylen);
 
diff --git a/drivers/infiniband/core/multicast.c b/drivers/infiniband/core/multicast.c
index fa17b552ff78..1244f02a5c6d 100644
--- a/drivers/infiniband/core/multicast.c
+++ b/drivers/infiniband/core/multicast.c
@@ -780,8 +780,7 @@ static void mcast_event_handler(struct ib_event_handler *handler,
 	int index;
 
 	dev = container_of(handler, struct mcast_device, event_handler);
-	if (rdma_port_get_link_layer(dev->device, event->element.port_num) !=
-	    IB_LINK_LAYER_INFINIBAND)
+	if (!rdma_cap_ib_mcast(dev->device, event->element.port_num))
 		return;
 
 	index = event->element.port_num - dev->start_port;
@@ -808,9 +807,6 @@ static void mcast_add_one(struct ib_device *device)
 	int i;
 	int count = 0;
 
-	if (rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
-		return;
-
 	dev = kmalloc(sizeof *dev + device->phys_port_cnt * sizeof *port,
 		      GFP_KERNEL);
 	if (!dev)
@@ -824,8 +820,7 @@ static void mcast_add_one(struct ib_device *device)
 	}
 
 	for (i = 0; i <= dev->end_port - dev->start_port; i++) {
-		if (rdma_port_get_link_layer(device, dev->start_port + i) !=
-		    IB_LINK_LAYER_INFINIBAND)
+		if (!rdma_cap_ib_mcast(device, dev->start_port + i))
 			continue;
 		port = &dev->port[i];
 		port->dev = dev;
@@ -863,8 +858,7 @@ static void mcast_remove_one(struct ib_device *device)
 	flush_workqueue(mcast_wq);
 
 	for (i = 0; i <= dev->end_port - dev->start_port; i++) {
-		if (rdma_port_get_link_layer(device, dev->start_port + i) ==
-		    IB_LINK_LAYER_INFINIBAND) {
+		if (rdma_cap_ib_mcast(device, dev->start_port + i)) {
 			port = &dev->port[i];
 			deref_port(port);
 			wait_for_completion(&port->comp);
diff --git a/drivers/infiniband/core/opa_smi.h b/drivers/infiniband/core/opa_smi.h
new file mode 100644
index 000000000000..62d91bfa4cb7
--- /dev/null
+++ b/drivers/infiniband/core/opa_smi.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2014 Intel Corporation.  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.
+ *
+ */
+
+#ifndef __OPA_SMI_H_
+#define __OPA_SMI_H_
+
+#include <rdma/ib_smi.h>
+#include <rdma/opa_smi.h>
+
+#include "smi.h"
+
+enum smi_action opa_smi_handle_dr_smp_recv(struct opa_smp *smp, u8 node_type,
+				       int port_num, int phys_port_cnt);
+int opa_smi_get_fwd_port(struct opa_smp *smp);
+extern enum smi_forward_action opa_smi_check_forward_dr_smp(struct opa_smp *smp);
+extern enum smi_action opa_smi_handle_dr_smp_send(struct opa_smp *smp,
+					      u8 node_type, int port_num);
+
+/*
+ * Return IB_SMI_HANDLE if the SMP should be handled by the local SMA/SM
+ * via process_mad
+ */
+static inline enum smi_action opa_smi_check_local_smp(struct opa_smp *smp,
+						      struct ib_device *device)
+{
+	/* C14-9:3 -- We're at the end of the DR segment of path */
+	/* C14-9:4 -- Hop Pointer = Hop Count + 1 -> give to SMA/SM */
+	return (device->process_mad &&
+		!opa_get_smp_direction(smp) &&
+		(smp->hop_ptr == smp->hop_cnt + 1)) ?
+		IB_SMI_HANDLE : IB_SMI_DISCARD;
+}
+
+/*
+ * Return IB_SMI_HANDLE if the SMP should be handled by the local SMA/SM
+ * via process_mad
+ */
+static inline enum smi_action opa_smi_check_local_returning_smp(struct opa_smp *smp,
+								struct ib_device *device)
+{
+	/* C14-13:3 -- We're at the end of the DR segment of path */
+	/* C14-13:4 -- Hop Pointer == 0 -> give to SM */
+	return (device->process_mad &&
+		opa_get_smp_direction(smp) &&
+		!smp->hop_ptr) ? IB_SMI_HANDLE : IB_SMI_DISCARD;
+}
+
+#endif	/* __OPA_SMI_H_ */
diff --git a/drivers/infiniband/core/sa_query.c b/drivers/infiniband/core/sa_query.c
index c38f030f0dc9..0fae85062a65 100644
--- a/drivers/infiniband/core/sa_query.c
+++ b/drivers/infiniband/core/sa_query.c
@@ -450,7 +450,7 @@ static void ib_sa_event(struct ib_event_handler *handler, struct ib_event *event
 		struct ib_sa_port *port =
 			&sa_dev->port[event->element.port_num - sa_dev->start_port];
 
-		if (rdma_port_get_link_layer(handler->device, port->port_num) != IB_LINK_LAYER_INFINIBAND)
+		if (!rdma_cap_ib_sa(handler->device, port->port_num))
 			return;
 
 		spin_lock_irqsave(&port->ah_lock, flags);
@@ -540,7 +540,7 @@ int ib_init_ah_from_path(struct ib_device *device, u8 port_num,
 	ah_attr->port_num = port_num;
 	ah_attr->static_rate = rec->rate;
 
-	force_grh = rdma_port_get_link_layer(device, port_num) == IB_LINK_LAYER_ETHERNET;
+	force_grh = rdma_cap_eth_ah(device, port_num);
 
 	if (rec->hop_limit > 1 || force_grh) {
 		ah_attr->ah_flags = IB_AH_GRH;
@@ -583,7 +583,8 @@ static int alloc_mad(struct ib_sa_query *query, gfp_t gfp_mask)
 	query->mad_buf = ib_create_send_mad(query->port->agent, 1,
 					    query->sm_ah->pkey_index,
 					    0, IB_MGMT_SA_HDR, IB_MGMT_SA_DATA,
-					    gfp_mask);
+					    gfp_mask,
+					    IB_MGMT_BASE_VERSION);
 	if (IS_ERR(query->mad_buf)) {
 		kref_put(&query->sm_ah->ref, free_sm_ah);
 		return -ENOMEM;
@@ -1153,9 +1154,7 @@ static void ib_sa_add_one(struct ib_device *device)
 {
 	struct ib_sa_device *sa_dev;
 	int s, e, i;
-
-	if (rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
-		return;
+	int count = 0;
 
 	if (device->node_type == RDMA_NODE_IB_SWITCH)
 		s = e = 0;
@@ -1175,7 +1174,7 @@ static void ib_sa_add_one(struct ib_device *device)
 
 	for (i = 0; i <= e - s; ++i) {
 		spin_lock_init(&sa_dev->port[i].ah_lock);
-		if (rdma_port_get_link_layer(device, i + 1) != IB_LINK_LAYER_INFINIBAND)
+		if (!rdma_cap_ib_sa(device, i + 1))
 			continue;
 
 		sa_dev->port[i].sm_ah    = NULL;
@@ -1189,8 +1188,13 @@ static void ib_sa_add_one(struct ib_device *device)
 			goto err;
 
 		INIT_WORK(&sa_dev->port[i].update_task, update_sm_ah);
+
+		count++;
 	}
 
+	if (!count)
+		goto free;
+
 	ib_set_client_data(device, &sa_client, sa_dev);
 
 	/*
@@ -1204,19 +1208,20 @@ static void ib_sa_add_one(struct ib_device *device)
 	if (ib_register_event_handler(&sa_dev->event_handler))
 		goto err;
 
-	for (i = 0; i <= e - s; ++i)
-		if (rdma_port_get_link_layer(device, i + 1) == IB_LINK_LAYER_INFINIBAND)
+	for (i = 0; i <= e - s; ++i) {
+		if (rdma_cap_ib_sa(device, i + 1))
 			update_sm_ah(&sa_dev->port[i].update_task);
+	}
 
 	return;
 
 err:
-	while (--i >= 0)
-		if (rdma_port_get_link_layer(device, i + 1) == IB_LINK_LAYER_INFINIBAND)
+	while (--i >= 0) {
+		if (rdma_cap_ib_sa(device, i + 1))
 			ib_unregister_mad_agent(sa_dev->port[i].agent);
-
+	}
+free:
 	kfree(sa_dev);
-
 	return;
 }
 
@@ -1233,7 +1238,7 @@ static void ib_sa_remove_one(struct ib_device *device)
 	flush_workqueue(ib_wq);
 
 	for (i = 0; i <= sa_dev->end_port - sa_dev->start_port; ++i) {
-		if (rdma_port_get_link_layer(device, i + 1) == IB_LINK_LAYER_INFINIBAND) {
+		if (rdma_cap_ib_sa(device, i + 1)) {
 			ib_unregister_mad_agent(sa_dev->port[i].agent);
 			if (sa_dev->port[i].sm_ah)
 				kref_put(&sa_dev->port[i].sm_ah->ref, free_sm_ah);
diff --git a/drivers/infiniband/core/smi.c b/drivers/infiniband/core/smi.c
index 5855e4405d9b..368a561d1a5d 100644
--- a/drivers/infiniband/core/smi.c
+++ b/drivers/infiniband/core/smi.c
@@ -5,6 +5,7 @@
  * Copyright (c) 2004, 2005 Topspin Corporation.  All rights reserved.
  * Copyright (c) 2004-2007 Voltaire Corporation.  All rights reserved.
  * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved.
+ * Copyright (c) 2014 Intel Corporation.  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
@@ -38,85 +39,82 @@
 
 #include <rdma/ib_smi.h>
 #include "smi.h"
-
-/*
- * Fixup a directed route SMP for sending
- * Return 0 if the SMP should be discarded
- */
-enum smi_action smi_handle_dr_smp_send(struct ib_smp *smp,
-				       u8 node_type, int port_num)
+#include "opa_smi.h"
+
+static enum smi_action __smi_handle_dr_smp_send(u8 node_type, int port_num,
+						u8 *hop_ptr, u8 hop_cnt,
+						const u8 *initial_path,
+						const u8 *return_path,
+						u8 direction,
+						bool dr_dlid_is_permissive,
+						bool dr_slid_is_permissive)
 {
-	u8 hop_ptr, hop_cnt;
-
-	hop_ptr = smp->hop_ptr;
-	hop_cnt = smp->hop_cnt;
-
 	/* See section 14.2.2.2, Vol 1 IB spec */
 	/* C14-6 -- valid hop_cnt values are from 0 to 63 */
 	if (hop_cnt >= IB_SMP_MAX_PATH_HOPS)
 		return IB_SMI_DISCARD;
 
-	if (!ib_get_smp_direction(smp)) {
+	if (!direction) {
 		/* C14-9:1 */
-		if (hop_cnt && hop_ptr == 0) {
-			smp->hop_ptr++;
-			return (smp->initial_path[smp->hop_ptr] ==
+		if (hop_cnt && *hop_ptr == 0) {
+			(*hop_ptr)++;
+			return (initial_path[*hop_ptr] ==
 				port_num ? IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-9:2 */
-		if (hop_ptr && hop_ptr < hop_cnt) {
+		if (*hop_ptr && *hop_ptr < hop_cnt) {
 			if (node_type != RDMA_NODE_IB_SWITCH)
 				return IB_SMI_DISCARD;
 
-			/* smp->return_path set when received */
-			smp->hop_ptr++;
-			return (smp->initial_path[smp->hop_ptr] ==
+			/* return_path set when received */
+			(*hop_ptr)++;
+			return (initial_path[*hop_ptr] ==
 				port_num ? IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-9:3 -- We're at the end of the DR segment of path */
-		if (hop_ptr == hop_cnt) {
-			/* smp->return_path set when received */
-			smp->hop_ptr++;
+		if (*hop_ptr == hop_cnt) {
+			/* return_path set when received */
+			(*hop_ptr)++;
 			return (node_type == RDMA_NODE_IB_SWITCH ||
-				smp->dr_dlid == IB_LID_PERMISSIVE ?
+				dr_dlid_is_permissive ?
 				IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-9:4 -- hop_ptr = hop_cnt + 1 -> give to SMA/SM */
 		/* C14-9:5 -- Fail unreasonable hop pointer */
-		return (hop_ptr == hop_cnt + 1 ? IB_SMI_HANDLE : IB_SMI_DISCARD);
+		return (*hop_ptr == hop_cnt + 1 ? IB_SMI_HANDLE : IB_SMI_DISCARD);
 
 	} else {
 		/* C14-13:1 */
-		if (hop_cnt && hop_ptr == hop_cnt + 1) {
-			smp->hop_ptr--;
-			return (smp->return_path[smp->hop_ptr] ==
+		if (hop_cnt && *hop_ptr == hop_cnt + 1) {
+			(*hop_ptr)--;
+			return (return_path[*hop_ptr] ==
 				port_num ? IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-13:2 */
-		if (2 <= hop_ptr && hop_ptr <= hop_cnt) {
+		if (2 <= *hop_ptr && *hop_ptr <= hop_cnt) {
 			if (node_type != RDMA_NODE_IB_SWITCH)
 				return IB_SMI_DISCARD;
 
-			smp->hop_ptr--;
-			return (smp->return_path[smp->hop_ptr] ==
+			(*hop_ptr)--;
+			return (return_path[*hop_ptr] ==
 				port_num ? IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-13:3 -- at the end of the DR segment of path */
-		if (hop_ptr == 1) {
-			smp->hop_ptr--;
+		if (*hop_ptr == 1) {
+			(*hop_ptr)--;
 			/* C14-13:3 -- SMPs destined for SM shouldn't be here */
 			return (node_type == RDMA_NODE_IB_SWITCH ||
-				smp->dr_slid == IB_LID_PERMISSIVE ?
+				dr_slid_is_permissive ?
 				IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-13:4 -- hop_ptr = 0 -> should have gone to SM */
-		if (hop_ptr == 0)
+		if (*hop_ptr == 0)
 			return IB_SMI_HANDLE;
 
 		/* C14-13:5 -- Check for unreasonable hop pointer */
@@ -125,105 +123,164 @@ enum smi_action smi_handle_dr_smp_send(struct ib_smp *smp,
 }
 
 /*
- * Adjust information for a received SMP
- * Return 0 if the SMP should be dropped
+ * Fixup a directed route SMP for sending
+ * Return IB_SMI_DISCARD if the SMP should be discarded
  */
-enum smi_action smi_handle_dr_smp_recv(struct ib_smp *smp, u8 node_type,
-				       int port_num, int phys_port_cnt)
+enum smi_action smi_handle_dr_smp_send(struct ib_smp *smp,
+				       u8 node_type, int port_num)
 {
-	u8 hop_ptr, hop_cnt;
+	return __smi_handle_dr_smp_send(node_type, port_num,
+					&smp->hop_ptr, smp->hop_cnt,
+					smp->initial_path,
+					smp->return_path,
+					ib_get_smp_direction(smp),
+					smp->dr_dlid == IB_LID_PERMISSIVE,
+					smp->dr_slid == IB_LID_PERMISSIVE);
+}
 
-	hop_ptr = smp->hop_ptr;
-	hop_cnt = smp->hop_cnt;
+enum smi_action opa_smi_handle_dr_smp_send(struct opa_smp *smp,
+				       u8 node_type, int port_num)
+{
+	return __smi_handle_dr_smp_send(node_type, port_num,
+					&smp->hop_ptr, smp->hop_cnt,
+					smp->route.dr.initial_path,
+					smp->route.dr.return_path,
+					opa_get_smp_direction(smp),
+					smp->route.dr.dr_dlid ==
+					OPA_LID_PERMISSIVE,
+					smp->route.dr.dr_slid ==
+					OPA_LID_PERMISSIVE);
+}
 
+static enum smi_action __smi_handle_dr_smp_recv(u8 node_type, int port_num,
+						int phys_port_cnt,
+						u8 *hop_ptr, u8 hop_cnt,
+						const u8 *initial_path,
+						u8 *return_path,
+						u8 direction,
+						bool dr_dlid_is_permissive,
+						bool dr_slid_is_permissive)
+{
 	/* See section 14.2.2.2, Vol 1 IB spec */
 	/* C14-6 -- valid hop_cnt values are from 0 to 63 */
 	if (hop_cnt >= IB_SMP_MAX_PATH_HOPS)
 		return IB_SMI_DISCARD;
 
-	if (!ib_get_smp_direction(smp)) {
+	if (!direction) {
 		/* C14-9:1 -- sender should have incremented hop_ptr */
-		if (hop_cnt && hop_ptr == 0)
+		if (hop_cnt && *hop_ptr == 0)
 			return IB_SMI_DISCARD;
 
 		/* C14-9:2 -- intermediate hop */
-		if (hop_ptr && hop_ptr < hop_cnt) {
+		if (*hop_ptr && *hop_ptr < hop_cnt) {
 			if (node_type != RDMA_NODE_IB_SWITCH)
 				return IB_SMI_DISCARD;
 
-			smp->return_path[hop_ptr] = port_num;
-			/* smp->hop_ptr updated when sending */
-			return (smp->initial_path[hop_ptr+1] <= phys_port_cnt ?
+			return_path[*hop_ptr] = port_num;
+			/* hop_ptr updated when sending */
+			return (initial_path[*hop_ptr+1] <= phys_port_cnt ?
 				IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-9:3 -- We're at the end of the DR segment of path */
-		if (hop_ptr == hop_cnt) {
+		if (*hop_ptr == hop_cnt) {
 			if (hop_cnt)
-				smp->return_path[hop_ptr] = port_num;
-			/* smp->hop_ptr updated when sending */
+				return_path[*hop_ptr] = port_num;
+			/* hop_ptr updated when sending */
 
 			return (node_type == RDMA_NODE_IB_SWITCH ||
-				smp->dr_dlid == IB_LID_PERMISSIVE ?
+				dr_dlid_is_permissive ?
 				IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-9:4 -- hop_ptr = hop_cnt + 1 -> give to SMA/SM */
 		/* C14-9:5 -- fail unreasonable hop pointer */
-		return (hop_ptr == hop_cnt + 1 ? IB_SMI_HANDLE : IB_SMI_DISCARD);
+		return (*hop_ptr == hop_cnt + 1 ? IB_SMI_HANDLE : IB_SMI_DISCARD);
 
 	} else {
 
 		/* C14-13:1 */
-		if (hop_cnt && hop_ptr == hop_cnt + 1) {
-			smp->hop_ptr--;
-			return (smp->return_path[smp->hop_ptr] ==
+		if (hop_cnt && *hop_ptr == hop_cnt + 1) {
+			(*hop_ptr)--;
+			return (return_path[*hop_ptr] ==
 				port_num ? IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-13:2 */
-		if (2 <= hop_ptr && hop_ptr <= hop_cnt) {
+		if (2 <= *hop_ptr && *hop_ptr <= hop_cnt) {
 			if (node_type != RDMA_NODE_IB_SWITCH)
 				return IB_SMI_DISCARD;
 
-			/* smp->hop_ptr updated when sending */
-			return (smp->return_path[hop_ptr-1] <= phys_port_cnt ?
+			/* hop_ptr updated when sending */
+			return (return_path[*hop_ptr-1] <= phys_port_cnt ?
 				IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-13:3 -- We're at the end of the DR segment of path */
-		if (hop_ptr == 1) {
-			if (smp->dr_slid == IB_LID_PERMISSIVE) {
+		if (*hop_ptr == 1) {
+			if (dr_slid_is_permissive) {
 				/* giving SMP to SM - update hop_ptr */
-				smp->hop_ptr--;
+				(*hop_ptr)--;
 				return IB_SMI_HANDLE;
 			}
-			/* smp->hop_ptr updated when sending */
+			/* hop_ptr updated when sending */
 			return (node_type == RDMA_NODE_IB_SWITCH ?
 				IB_SMI_HANDLE : IB_SMI_DISCARD);
 		}
 
 		/* C14-13:4 -- hop_ptr = 0 -> give to SM */
 		/* C14-13:5 -- Check for unreasonable hop pointer */
-		return (hop_ptr == 0 ? IB_SMI_HANDLE : IB_SMI_DISCARD);
+		return (*hop_ptr == 0 ? IB_SMI_HANDLE : IB_SMI_DISCARD);
 	}
 }
 
-enum smi_forward_action smi_check_forward_dr_smp(struct ib_smp *smp)
+/*
+ * Adjust information for a received SMP
+ * Return IB_SMI_DISCARD if the SMP should be dropped
+ */
+enum smi_action smi_handle_dr_smp_recv(struct ib_smp *smp, u8 node_type,
+				       int port_num, int phys_port_cnt)
 {
-	u8 hop_ptr, hop_cnt;
+	return __smi_handle_dr_smp_recv(node_type, port_num, phys_port_cnt,
+					&smp->hop_ptr, smp->hop_cnt,
+					smp->initial_path,
+					smp->return_path,
+					ib_get_smp_direction(smp),
+					smp->dr_dlid == IB_LID_PERMISSIVE,
+					smp->dr_slid == IB_LID_PERMISSIVE);
+}
 
-	hop_ptr = smp->hop_ptr;
-	hop_cnt = smp->hop_cnt;
+/*
+ * Adjust information for a received SMP
+ * Return IB_SMI_DISCARD if the SMP should be dropped
+ */
+enum smi_action opa_smi_handle_dr_smp_recv(struct opa_smp *smp, u8 node_type,
+					   int port_num, int phys_port_cnt)
+{
+	return __smi_handle_dr_smp_recv(node_type, port_num, phys_port_cnt,
+					&smp->hop_ptr, smp->hop_cnt,
+					smp->route.dr.initial_path,
+					smp->route.dr.return_path,
+					opa_get_smp_direction(smp),
+					smp->route.dr.dr_dlid ==
+					OPA_LID_PERMISSIVE,
+					smp->route.dr.dr_slid ==
+					OPA_LID_PERMISSIVE);
+}
 
-	if (!ib_get_smp_direction(smp)) {
+static enum smi_forward_action __smi_check_forward_dr_smp(u8 hop_ptr, u8 hop_cnt,
+							  u8 direction,
+							  bool dr_dlid_is_permissive,
+							  bool dr_slid_is_permissive)
+{
+	if (!direction) {
 		/* C14-9:2 -- intermediate hop */
 		if (hop_ptr && hop_ptr < hop_cnt)
 			return IB_SMI_FORWARD;
 
 		/* C14-9:3 -- at the end of the DR segment of path */
 		if (hop_ptr == hop_cnt)
-			return (smp->dr_dlid == IB_LID_PERMISSIVE ?
+			return (dr_dlid_is_permissive ?
 				IB_SMI_SEND : IB_SMI_LOCAL);
 
 		/* C14-9:4 -- hop_ptr = hop_cnt + 1 -> give to SMA/SM */
@@ -236,10 +293,29 @@ enum smi_forward_action smi_check_forward_dr_smp(struct ib_smp *smp)
 
 		/* C14-13:3 -- at the end of the DR segment of path */
 		if (hop_ptr == 1)
-			return (smp->dr_slid != IB_LID_PERMISSIVE ?
+			return (!dr_slid_is_permissive ?
 				IB_SMI_SEND : IB_SMI_LOCAL);
 	}
 	return IB_SMI_LOCAL;
+
+}
+
+enum smi_forward_action smi_check_forward_dr_smp(struct ib_smp *smp)
+{
+	return __smi_check_forward_dr_smp(smp->hop_ptr, smp->hop_cnt,
+					  ib_get_smp_direction(smp),
+					  smp->dr_dlid == IB_LID_PERMISSIVE,
+					  smp->dr_slid == IB_LID_PERMISSIVE);
+}
+
+enum smi_forward_action opa_smi_check_forward_dr_smp(struct opa_smp *smp)
+{
+	return __smi_check_forward_dr_smp(smp->hop_ptr, smp->hop_cnt,
+					  opa_get_smp_direction(smp),
+					  smp->route.dr.dr_dlid ==
+					  OPA_LID_PERMISSIVE,
+					  smp->route.dr.dr_slid ==
+					  OPA_LID_PERMISSIVE);
 }
 
 /*
@@ -251,3 +327,13 @@ int smi_get_fwd_port(struct ib_smp *smp)
 	return (!ib_get_smp_direction(smp) ? smp->initial_path[smp->hop_ptr+1] :
 		smp->return_path[smp->hop_ptr-1]);
 }
+
+/*
+ * Return the forwarding port number from initial_path for outgoing SMP and
+ * from return_path for returning SMP
+ */
+int opa_smi_get_fwd_port(struct opa_smp *smp)
+{
+	return !opa_get_smp_direction(smp) ? smp->route.dr.initial_path[smp->hop_ptr+1] :
+		smp->route.dr.return_path[smp->hop_ptr-1];
+}
diff --git a/drivers/infiniband/core/sysfs.c b/drivers/infiniband/core/sysfs.c
index cbd0383f622e..ed6b6c85c334 100644
--- a/drivers/infiniband/core/sysfs.c
+++ b/drivers/infiniband/core/sysfs.c
@@ -326,6 +326,8 @@ static ssize_t show_pma_counter(struct ib_port *p, struct port_attribute *attr,
 	int width  = (tab_attr->index >> 16) & 0xff;
 	struct ib_mad *in_mad  = NULL;
 	struct ib_mad *out_mad = NULL;
+	size_t mad_size = sizeof(*out_mad);
+	u16 out_mad_pkey_index = 0;
 	ssize_t ret;
 
 	if (!p->ibdev->process_mad)
@@ -347,7 +349,10 @@ static ssize_t show_pma_counter(struct ib_port *p, struct port_attribute *attr,
 	in_mad->data[41] = p->port_num;	/* PortSelect field */
 
 	if ((p->ibdev->process_mad(p->ibdev, IB_MAD_IGNORE_MKEY,
-		 p->port_num, NULL, NULL, in_mad, out_mad) &
+		 p->port_num, NULL, NULL,
+		 (const struct ib_mad_hdr *)in_mad, mad_size,
+		 (struct ib_mad_hdr *)out_mad, &mad_size,
+		 &out_mad_pkey_index) &
 	     (IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY)) !=
 	    (IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY)) {
 		ret = -EINVAL;
@@ -456,6 +461,7 @@ static void ib_device_release(struct device *device)
 {
 	struct ib_device *dev = container_of(device, struct ib_device, dev);
 
+	kfree(dev->port_immutable);
 	kfree(dev);
 }
 
diff --git a/drivers/infiniband/core/ucm.c b/drivers/infiniband/core/ucm.c
index f2f63933e8a9..62c24b1452b8 100644
--- a/drivers/infiniband/core/ucm.c
+++ b/drivers/infiniband/core/ucm.c
@@ -1253,8 +1253,7 @@ static void ib_ucm_add_one(struct ib_device *device)
 	dev_t base;
 	struct ib_ucm_device *ucm_dev;
 
-	if (!device->alloc_ucontext ||
-	    rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
+	if (!device->alloc_ucontext || !rdma_cap_ib_cm(device, 1))
 		return;
 
 	ucm_dev = kzalloc(sizeof *ucm_dev, GFP_KERNEL);
diff --git a/drivers/infiniband/core/ucma.c b/drivers/infiniband/core/ucma.c
index 45d67e9228d7..ad45469f7582 100644
--- a/drivers/infiniband/core/ucma.c
+++ b/drivers/infiniband/core/ucma.c
@@ -722,26 +722,13 @@ static ssize_t ucma_query_route(struct ucma_file *file,
 
 	resp.node_guid = (__force __u64) ctx->cm_id->device->node_guid;
 	resp.port_num = ctx->cm_id->port_num;
-	switch (rdma_node_get_transport(ctx->cm_id->device->node_type)) {
-	case RDMA_TRANSPORT_IB:
-		switch (rdma_port_get_link_layer(ctx->cm_id->device,
-			ctx->cm_id->port_num)) {
-		case IB_LINK_LAYER_INFINIBAND:
-			ucma_copy_ib_route(&resp, &ctx->cm_id->route);
-			break;
-		case IB_LINK_LAYER_ETHERNET:
-			ucma_copy_iboe_route(&resp, &ctx->cm_id->route);
-			break;
-		default:
-			break;
-		}
-		break;
-	case RDMA_TRANSPORT_IWARP:
+
+	if (rdma_cap_ib_sa(ctx->cm_id->device, ctx->cm_id->port_num))
+		ucma_copy_ib_route(&resp, &ctx->cm_id->route);
+	else if (rdma_protocol_roce(ctx->cm_id->device, ctx->cm_id->port_num))
+		ucma_copy_iboe_route(&resp, &ctx->cm_id->route);
+	else if (rdma_protocol_iwarp(ctx->cm_id->device, ctx->cm_id->port_num))
 		ucma_copy_iw_route(&resp, &ctx->cm_id->route);
-		break;
-	default:
-		break;
-	}
 
 out:
 	if (copy_to_user((void __user *)(unsigned long)cmd.response,
diff --git a/drivers/infiniband/core/user_mad.c b/drivers/infiniband/core/user_mad.c
index 928cdd20e2d1..35567fffaa4e 100644
--- a/drivers/infiniband/core/user_mad.c
+++ b/drivers/infiniband/core/user_mad.c
@@ -99,7 +99,6 @@ struct ib_umad_port {
 };
 
 struct ib_umad_device {
-	int                  start_port, end_port;
 	struct kobject       kobj;
 	struct ib_umad_port  port[0];
 };
@@ -263,20 +262,23 @@ static ssize_t copy_recv_mad(struct ib_umad_file *file, char __user *buf,
 {
 	struct ib_mad_recv_buf *recv_buf;
 	int left, seg_payload, offset, max_seg_payload;
+	size_t seg_size;
 
-	/* We need enough room to copy the first (or only) MAD segment. */
 	recv_buf = &packet->recv_wc->recv_buf;
-	if ((packet->length <= sizeof (*recv_buf->mad) &&
+	seg_size = packet->recv_wc->mad_seg_size;
+
+	/* We need enough room to copy the first (or only) MAD segment. */
+	if ((packet->length <= seg_size &&
 	     count < hdr_size(file) + packet->length) ||
-	    (packet->length > sizeof (*recv_buf->mad) &&
-	     count < hdr_size(file) + sizeof (*recv_buf->mad)))
+	    (packet->length > seg_size &&
+	     count < hdr_size(file) + seg_size))
 		return -EINVAL;
 
 	if (copy_to_user(buf, &packet->mad, hdr_size(file)))
 		return -EFAULT;
 
 	buf += hdr_size(file);
-	seg_payload = min_t(int, packet->length, sizeof (*recv_buf->mad));
+	seg_payload = min_t(int, packet->length, seg_size);
 	if (copy_to_user(buf, recv_buf->mad, seg_payload))
 		return -EFAULT;
 
@@ -293,7 +295,7 @@ static ssize_t copy_recv_mad(struct ib_umad_file *file, char __user *buf,
 			return -ENOSPC;
 		}
 		offset = ib_get_mad_data_offset(recv_buf->mad->mad_hdr.mgmt_class);
-		max_seg_payload = sizeof (struct ib_mad) - offset;
+		max_seg_payload = seg_size - offset;
 
 		for (left = packet->length - seg_payload, buf += seg_payload;
 		     left; left -= seg_payload, buf += seg_payload) {
@@ -426,11 +428,11 @@ static int is_duplicate(struct ib_umad_file *file,
 		 * the same TID, reject the second as a duplicate.  This is more
 		 * restrictive than required by the spec.
 		 */
-		if (!ib_response_mad((struct ib_mad *) hdr)) {
-			if (!ib_response_mad((struct ib_mad *) sent_hdr))
+		if (!ib_response_mad(hdr)) {
+			if (!ib_response_mad(sent_hdr))
 				return 1;
 			continue;
-		} else if (!ib_response_mad((struct ib_mad *) sent_hdr))
+		} else if (!ib_response_mad(sent_hdr))
 			continue;
 
 		if (same_destination(&packet->mad.hdr, &sent_packet->mad.hdr))
@@ -451,6 +453,7 @@ static ssize_t ib_umad_write(struct file *filp, const char __user *buf,
 	struct ib_rmpp_mad *rmpp_mad;
 	__be64 *tid;
 	int ret, data_len, hdr_len, copy_offset, rmpp_active;
+	u8 base_version;
 
 	if (count < hdr_size(file) + IB_MGMT_RMPP_HDR)
 		return -EINVAL;
@@ -517,11 +520,13 @@ static ssize_t ib_umad_write(struct file *filp, const char __user *buf,
 		rmpp_active = 0;
 	}
 
+	base_version = ((struct ib_mad_hdr *)&packet->mad.data)->base_version;
 	data_len = count - hdr_size(file) - hdr_len;
 	packet->msg = ib_create_send_mad(agent,
 					 be32_to_cpu(packet->mad.hdr.qpn),
 					 packet->mad.hdr.pkey_index, rmpp_active,
-					 hdr_len, data_len, GFP_KERNEL);
+					 hdr_len, data_len, GFP_KERNEL,
+					 base_version);
 	if (IS_ERR(packet->msg)) {
 		ret = PTR_ERR(packet->msg);
 		goto err_ah;
@@ -1273,16 +1278,10 @@ static void ib_umad_add_one(struct ib_device *device)
 {
 	struct ib_umad_device *umad_dev;
 	int s, e, i;
+	int count = 0;
 
-	if (rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
-		return;
-
-	if (device->node_type == RDMA_NODE_IB_SWITCH)
-		s = e = 0;
-	else {
-		s = 1;
-		e = device->phys_port_cnt;
-	}
+	s = rdma_start_port(device);
+	e = rdma_end_port(device);
 
 	umad_dev = kzalloc(sizeof *umad_dev +
 			   (e - s + 1) * sizeof (struct ib_umad_port),
@@ -1292,25 +1291,34 @@ static void ib_umad_add_one(struct ib_device *device)
 
 	kobject_init(&umad_dev->kobj, &ib_umad_dev_ktype);
 
-	umad_dev->start_port = s;
-	umad_dev->end_port   = e;
-
 	for (i = s; i <= e; ++i) {
+		if (!rdma_cap_ib_mad(device, i))
+			continue;
+
 		umad_dev->port[i - s].umad_dev = umad_dev;
 
 		if (ib_umad_init_port(device, i, umad_dev,
 				      &umad_dev->port[i - s]))
 			goto err;
+
+		count++;
 	}
 
+	if (!count)
+		goto free;
+
 	ib_set_client_data(device, &umad_client, umad_dev);
 
 	return;
 
 err:
-	while (--i >= s)
-		ib_umad_kill_port(&umad_dev->port[i - s]);
+	while (--i >= s) {
+		if (!rdma_cap_ib_mad(device, i))
+			continue;
 
+		ib_umad_kill_port(&umad_dev->port[i - s]);
+	}
+free:
 	kobject_put(&umad_dev->kobj);
 }
 
@@ -1322,8 +1330,10 @@ static void ib_umad_remove_one(struct ib_device *device)
 	if (!umad_dev)
 		return;
 
-	for (i = 0; i <= umad_dev->end_port - umad_dev->start_port; ++i)
-		ib_umad_kill_port(&umad_dev->port[i]);
+	for (i = 0; i <= rdma_end_port(device) - rdma_start_port(device); ++i) {
+		if (rdma_cap_ib_mad(device, i + rdma_start_port(device)))
+			ib_umad_kill_port(&umad_dev->port[i]);
+	}
 
 	kobject_put(&umad_dev->kobj);
 }
diff --git a/drivers/infiniband/core/uverbs.h b/drivers/infiniband/core/uverbs.h
index b716b0815644..ba365b6d1e8d 100644
--- a/drivers/infiniband/core/uverbs.h
+++ b/drivers/infiniband/core/uverbs.h
@@ -259,5 +259,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);
+IB_UVERBS_DECLARE_EX_CMD(create_cq);
 
 #endif /* UVERBS_H */
diff --git a/drivers/infiniband/core/uverbs_cmd.c b/drivers/infiniband/core/uverbs_cmd.c
index a9f048990dfc..bbb02ffe87df 100644
--- a/drivers/infiniband/core/uverbs_cmd.c
+++ b/drivers/infiniband/core/uverbs_cmd.c
@@ -1330,40 +1330,37 @@ ssize_t ib_uverbs_create_comp_channel(struct ib_uverbs_file *file,
 	return in_len;
 }
 
-ssize_t ib_uverbs_create_cq(struct ib_uverbs_file *file,
-			    const char __user *buf, int in_len,
-			    int out_len)
+static struct ib_ucq_object *create_cq(struct ib_uverbs_file *file,
+				       struct ib_udata *ucore,
+				       struct ib_udata *uhw,
+				       struct ib_uverbs_ex_create_cq *cmd,
+				       size_t cmd_sz,
+				       int (*cb)(struct ib_uverbs_file *file,
+						 struct ib_ucq_object *obj,
+						 struct ib_uverbs_ex_create_cq_resp *resp,
+						 struct ib_udata *udata,
+						 void *context),
+				       void *context)
 {
-	struct ib_uverbs_create_cq      cmd;
-	struct ib_uverbs_create_cq_resp resp;
-	struct ib_udata                 udata;
 	struct ib_ucq_object           *obj;
 	struct ib_uverbs_event_file    *ev_file = NULL;
 	struct ib_cq                   *cq;
 	int                             ret;
+	struct ib_uverbs_ex_create_cq_resp resp;
+	struct ib_cq_init_attr attr = {};
 
-	if (out_len < sizeof resp)
-		return -ENOSPC;
-
-	if (copy_from_user(&cmd, buf, sizeof cmd))
-		return -EFAULT;
-
-	INIT_UDATA(&udata, buf + sizeof cmd,
-		   (unsigned long) cmd.response + sizeof resp,
-		   in_len - sizeof cmd, out_len - sizeof resp);
-
-	if (cmd.comp_vector >= file->device->num_comp_vectors)
-		return -EINVAL;
+	if (cmd->comp_vector >= file->device->num_comp_vectors)
+		return ERR_PTR(-EINVAL);
 
 	obj = kmalloc(sizeof *obj, GFP_KERNEL);
 	if (!obj)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
-	init_uobj(&obj->uobject, cmd.user_handle, file->ucontext, &cq_lock_class);
+	init_uobj(&obj->uobject, cmd->user_handle, file->ucontext, &cq_lock_class);
 	down_write(&obj->uobject.mutex);
 
-	if (cmd.comp_channel >= 0) {
-		ev_file = ib_uverbs_lookup_comp_file(cmd.comp_channel);
+	if (cmd->comp_channel >= 0) {
+		ev_file = ib_uverbs_lookup_comp_file(cmd->comp_channel);
 		if (!ev_file) {
 			ret = -EINVAL;
 			goto err;
@@ -1376,9 +1373,14 @@ ssize_t ib_uverbs_create_cq(struct ib_uverbs_file *file,
 	INIT_LIST_HEAD(&obj->comp_list);
 	INIT_LIST_HEAD(&obj->async_list);
 
-	cq = file->device->ib_dev->create_cq(file->device->ib_dev, cmd.cqe,
-					     cmd.comp_vector,
-					     file->ucontext, &udata);
+	attr.cqe = cmd->cqe;
+	attr.comp_vector = cmd->comp_vector;
+
+	if (cmd_sz > offsetof(typeof(*cmd), flags) + sizeof(cmd->flags))
+		attr.flags = cmd->flags;
+
+	cq = file->device->ib_dev->create_cq(file->device->ib_dev, &attr,
+					     file->ucontext, uhw);
 	if (IS_ERR(cq)) {
 		ret = PTR_ERR(cq);
 		goto err_file;
@@ -1397,14 +1399,15 @@ ssize_t ib_uverbs_create_cq(struct ib_uverbs_file *file,
 		goto err_free;
 
 	memset(&resp, 0, sizeof resp);
-	resp.cq_handle = obj->uobject.id;
-	resp.cqe       = cq->cqe;
+	resp.base.cq_handle = obj->uobject.id;
+	resp.base.cqe       = cq->cqe;
 
-	if (copy_to_user((void __user *) (unsigned long) cmd.response,
-			 &resp, sizeof resp)) {
-		ret = -EFAULT;
-		goto err_copy;
-	}
+	resp.response_length = offsetof(typeof(resp), response_length) +
+		sizeof(resp.response_length);
+
+	ret = cb(file, obj, &resp, ucore, context);
+	if (ret)
+		goto err_cb;
 
 	mutex_lock(&file->mutex);
 	list_add_tail(&obj->uobject.list, &file->ucontext->cq_list);
@@ -1414,9 +1417,9 @@ ssize_t ib_uverbs_create_cq(struct ib_uverbs_file *file,
 
 	up_write(&obj->uobject.mutex);
 
-	return in_len;
+	return obj;
 
-err_copy:
+err_cb:
 	idr_remove_uobj(&ib_uverbs_cq_idr, &obj->uobject);
 
 err_free:
@@ -1428,7 +1431,106 @@ err_file:
 
 err:
 	put_uobj_write(&obj->uobject);
-	return ret;
+
+	return ERR_PTR(ret);
+}
+
+static int ib_uverbs_create_cq_cb(struct ib_uverbs_file *file,
+				  struct ib_ucq_object *obj,
+				  struct ib_uverbs_ex_create_cq_resp *resp,
+				  struct ib_udata *ucore, void *context)
+{
+	if (ib_copy_to_udata(ucore, &resp->base, sizeof(resp->base)))
+		return -EFAULT;
+
+	return 0;
+}
+
+ssize_t ib_uverbs_create_cq(struct ib_uverbs_file *file,
+			    const char __user *buf, int in_len,
+			    int out_len)
+{
+	struct ib_uverbs_create_cq      cmd;
+	struct ib_uverbs_ex_create_cq	cmd_ex;
+	struct ib_uverbs_create_cq_resp resp;
+	struct ib_udata                 ucore;
+	struct ib_udata                 uhw;
+	struct ib_ucq_object           *obj;
+
+	if (out_len < sizeof(resp))
+		return -ENOSPC;
+
+	if (copy_from_user(&cmd, buf, sizeof(cmd)))
+		return -EFAULT;
+
+	INIT_UDATA(&ucore, buf, cmd.response, sizeof(cmd), sizeof(resp));
+
+	INIT_UDATA(&uhw, buf + sizeof(cmd),
+		   (unsigned long)cmd.response + sizeof(resp),
+		   in_len - sizeof(cmd), out_len - sizeof(resp));
+
+	memset(&cmd_ex, 0, sizeof(cmd_ex));
+	cmd_ex.user_handle = cmd.user_handle;
+	cmd_ex.cqe = cmd.cqe;
+	cmd_ex.comp_vector = cmd.comp_vector;
+	cmd_ex.comp_channel = cmd.comp_channel;
+
+	obj = create_cq(file, &ucore, &uhw, &cmd_ex,
+			offsetof(typeof(cmd_ex), comp_channel) +
+			sizeof(cmd.comp_channel), ib_uverbs_create_cq_cb,
+			NULL);
+
+	if (IS_ERR(obj))
+		return PTR_ERR(obj);
+
+	return in_len;
+}
+
+static int ib_uverbs_ex_create_cq_cb(struct ib_uverbs_file *file,
+				     struct ib_ucq_object *obj,
+				     struct ib_uverbs_ex_create_cq_resp *resp,
+				     struct ib_udata *ucore, void *context)
+{
+	if (ib_copy_to_udata(ucore, resp, resp->response_length))
+		return -EFAULT;
+
+	return 0;
+}
+
+int ib_uverbs_ex_create_cq(struct ib_uverbs_file *file,
+			   struct ib_udata *ucore,
+			   struct ib_udata *uhw)
+{
+	struct ib_uverbs_ex_create_cq_resp resp;
+	struct ib_uverbs_ex_create_cq  cmd;
+	struct ib_ucq_object           *obj;
+	int err;
+
+	if (ucore->inlen < sizeof(cmd))
+		return -EINVAL;
+
+	err = ib_copy_from_udata(&cmd, ucore, sizeof(cmd));
+	if (err)
+		return err;
+
+	if (cmd.comp_mask)
+		return -EINVAL;
+
+	if (cmd.reserved)
+		return -EINVAL;
+
+	if (ucore->outlen < (offsetof(typeof(resp), response_length) +
+			     sizeof(resp.response_length)))
+		return -ENOSPC;
+
+	obj = create_cq(file, ucore, uhw, &cmd,
+			min(ucore->inlen, sizeof(cmd)),
+			ib_uverbs_ex_create_cq_cb, NULL);
+
+	if (IS_ERR(obj))
+		return PTR_ERR(obj);
+
+	return 0;
 }
 
 ssize_t ib_uverbs_resize_cq(struct ib_uverbs_file *file,
@@ -3324,7 +3426,9 @@ int ib_uverbs_ex_query_device(struct ib_uverbs_file *file,
 	if (ucore->outlen < resp.response_length)
 		return -ENOSPC;
 
-	err = device->query_device(device, &attr);
+	memset(&attr, 0, sizeof(attr));
+
+	err = device->query_device(device, &attr, uhw);
 	if (err)
 		return err;
 
@@ -3348,6 +3452,18 @@ int ib_uverbs_ex_query_device(struct ib_uverbs_file *file,
 #endif
 	resp.response_length += sizeof(resp.odp_caps);
 
+	if (ucore->outlen < resp.response_length + sizeof(resp.timestamp_mask))
+		goto end;
+
+	resp.timestamp_mask = attr.timestamp_mask;
+	resp.response_length += sizeof(resp.timestamp_mask);
+
+	if (ucore->outlen < resp.response_length + sizeof(resp.hca_core_clock))
+		goto end;
+
+	resp.hca_core_clock = attr.hca_core_clock;
+	resp.response_length += sizeof(resp.hca_core_clock);
+
 end:
 	err = ib_copy_to_udata(ucore, &resp, resp.response_length);
 	if (err)
diff --git a/drivers/infiniband/core/uverbs_main.c b/drivers/infiniband/core/uverbs_main.c
index 88cce9bb72fe..f6eef2da7097 100644
--- a/drivers/infiniband/core/uverbs_main.c
+++ b/drivers/infiniband/core/uverbs_main.c
@@ -124,6 +124,7 @@ static int (*uverbs_ex_cmd_table[])(struct ib_uverbs_file *file,
 	[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_QUERY_DEVICE]	= ib_uverbs_ex_query_device,
+	[IB_USER_VERBS_EX_CMD_CREATE_CQ]	= ib_uverbs_ex_create_cq,
 };
 
 static void ib_uverbs_add_one(struct ib_device *device);
diff --git a/drivers/infiniband/core/verbs.c b/drivers/infiniband/core/verbs.c
index f93eb8da7b5a..bac3fb406a74 100644
--- a/drivers/infiniband/core/verbs.c
+++ b/drivers/infiniband/core/verbs.c
@@ -48,6 +48,71 @@
 
 #include "core_priv.h"
 
+static const char * const ib_events[] = {
+	[IB_EVENT_CQ_ERR]		= "CQ error",
+	[IB_EVENT_QP_FATAL]		= "QP fatal error",
+	[IB_EVENT_QP_REQ_ERR]		= "QP request error",
+	[IB_EVENT_QP_ACCESS_ERR]	= "QP access error",
+	[IB_EVENT_COMM_EST]		= "communication established",
+	[IB_EVENT_SQ_DRAINED]		= "send queue drained",
+	[IB_EVENT_PATH_MIG]		= "path migration successful",
+	[IB_EVENT_PATH_MIG_ERR]		= "path migration error",
+	[IB_EVENT_DEVICE_FATAL]		= "device fatal error",
+	[IB_EVENT_PORT_ACTIVE]		= "port active",
+	[IB_EVENT_PORT_ERR]		= "port error",
+	[IB_EVENT_LID_CHANGE]		= "LID change",
+	[IB_EVENT_PKEY_CHANGE]		= "P_key change",
+	[IB_EVENT_SM_CHANGE]		= "SM change",
+	[IB_EVENT_SRQ_ERR]		= "SRQ error",
+	[IB_EVENT_SRQ_LIMIT_REACHED]	= "SRQ limit reached",
+	[IB_EVENT_QP_LAST_WQE_REACHED]	= "last WQE reached",
+	[IB_EVENT_CLIENT_REREGISTER]	= "client reregister",
+	[IB_EVENT_GID_CHANGE]		= "GID changed",
+};
+
+const char *ib_event_msg(enum ib_event_type event)
+{
+	size_t index = event;
+
+	return (index < ARRAY_SIZE(ib_events) && ib_events[index]) ?
+			ib_events[index] : "unrecognized event";
+}
+EXPORT_SYMBOL(ib_event_msg);
+
+static const char * const wc_statuses[] = {
+	[IB_WC_SUCCESS]			= "success",
+	[IB_WC_LOC_LEN_ERR]		= "local length error",
+	[IB_WC_LOC_QP_OP_ERR]		= "local QP operation error",
+	[IB_WC_LOC_EEC_OP_ERR]		= "local EE context operation error",
+	[IB_WC_LOC_PROT_ERR]		= "local protection error",
+	[IB_WC_WR_FLUSH_ERR]		= "WR flushed",
+	[IB_WC_MW_BIND_ERR]		= "memory management operation error",
+	[IB_WC_BAD_RESP_ERR]		= "bad response error",
+	[IB_WC_LOC_ACCESS_ERR]		= "local access error",
+	[IB_WC_REM_INV_REQ_ERR]		= "invalid request error",
+	[IB_WC_REM_ACCESS_ERR]		= "remote access error",
+	[IB_WC_REM_OP_ERR]		= "remote operation error",
+	[IB_WC_RETRY_EXC_ERR]		= "transport retry counter exceeded",
+	[IB_WC_RNR_RETRY_EXC_ERR]	= "RNR retry counter exceeded",
+	[IB_WC_LOC_RDD_VIOL_ERR]	= "local RDD violation error",
+	[IB_WC_REM_INV_RD_REQ_ERR]	= "remote invalid RD request",
+	[IB_WC_REM_ABORT_ERR]		= "operation aborted",
+	[IB_WC_INV_EECN_ERR]		= "invalid EE context number",
+	[IB_WC_INV_EEC_STATE_ERR]	= "invalid EE context state",
+	[IB_WC_FATAL_ERR]		= "fatal error",
+	[IB_WC_RESP_TIMEOUT_ERR]	= "response timeout error",
+	[IB_WC_GENERAL_ERR]		= "general error",
+};
+
+const char *ib_wc_status_msg(enum ib_wc_status status)
+{
+	size_t index = status;
+
+	return (index < ARRAY_SIZE(wc_statuses) && wc_statuses[index]) ?
+			wc_statuses[index] : "unrecognized status";
+}
+EXPORT_SYMBOL(ib_wc_status_msg);
+
 __attribute_const__ int ib_rate_to_mult(enum ib_rate rate)
 {
 	switch (rate) {
@@ -192,17 +257,16 @@ struct ib_ah *ib_create_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr)
 }
 EXPORT_SYMBOL(ib_create_ah);
 
-int ib_init_ah_from_wc(struct ib_device *device, u8 port_num, struct ib_wc *wc,
-		       struct ib_grh *grh, struct ib_ah_attr *ah_attr)
+int ib_init_ah_from_wc(struct ib_device *device, u8 port_num,
+		       const struct ib_wc *wc, const struct ib_grh *grh,
+		       struct ib_ah_attr *ah_attr)
 {
 	u32 flow_class;
 	u16 gid_index;
 	int ret;
-	int is_eth = (rdma_port_get_link_layer(device, port_num) ==
-			IB_LINK_LAYER_ETHERNET);
 
 	memset(ah_attr, 0, sizeof *ah_attr);
-	if (is_eth) {
+	if (rdma_cap_eth_ah(device, port_num)) {
 		if (!(wc->wc_flags & IB_WC_GRH))
 			return -EPROTOTYPE;
 
@@ -244,8 +308,8 @@ int ib_init_ah_from_wc(struct ib_device *device, u8 port_num, struct ib_wc *wc,
 }
 EXPORT_SYMBOL(ib_init_ah_from_wc);
 
-struct ib_ah *ib_create_ah_from_wc(struct ib_pd *pd, struct ib_wc *wc,
-				   struct ib_grh *grh, u8 port_num)
+struct ib_ah *ib_create_ah_from_wc(struct ib_pd *pd, const struct ib_wc *wc,
+				   const struct ib_grh *grh, u8 port_num)
 {
 	struct ib_ah_attr ah_attr;
 	int ret;
@@ -871,7 +935,7 @@ int ib_resolve_eth_l2_attrs(struct ib_qp *qp,
 	union ib_gid  sgid;
 
 	if ((*qp_attr_mask & IB_QP_AV)  &&
-	    (rdma_port_get_link_layer(qp->device, qp_attr->ah_attr.port_num) == IB_LINK_LAYER_ETHERNET)) {
+	    (rdma_cap_eth_ah(qp->device, qp_attr->ah_attr.port_num))) {
 		ret = ib_query_gid(qp->device, qp_attr->ah_attr.port_num,
 				   qp_attr->ah_attr.grh.sgid_index, &sgid);
 		if (ret)
@@ -1012,11 +1076,12 @@ EXPORT_SYMBOL(ib_destroy_qp);
 struct ib_cq *ib_create_cq(struct ib_device *device,
 			   ib_comp_handler comp_handler,
 			   void (*event_handler)(struct ib_event *, void *),
-			   void *cq_context, int cqe, int comp_vector)
+			   void *cq_context,
+			   const struct ib_cq_init_attr *cq_attr)
 {
 	struct ib_cq *cq;
 
-	cq = device->create_cq(device, cqe, comp_vector, NULL, NULL);
+	cq = device->create_cq(device, cq_attr, NULL, NULL);
 
 	if (!IS_ERR(cq)) {
 		cq->device        = device;
diff --git a/drivers/infiniband/hw/amso1100/c2_provider.c b/drivers/infiniband/hw/amso1100/c2_provider.c
index bdf3507810cb..25c3f0085563 100644
--- a/drivers/infiniband/hw/amso1100/c2_provider.c
+++ b/drivers/infiniband/hw/amso1100/c2_provider.c
@@ -63,13 +63,16 @@
 #include "c2_provider.h"
 #include "c2_user.h"
 
-static int c2_query_device(struct ib_device *ibdev,
-			   struct ib_device_attr *props)
+static int c2_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+			   struct ib_udata *uhw)
 {
 	struct c2_dev *c2dev = to_c2dev(ibdev);
 
 	pr_debug("%s:%u\n", __func__, __LINE__);
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	*props = c2dev->props;
 	return 0;
 }
@@ -286,13 +289,18 @@ static int c2_destroy_qp(struct ib_qp *ib_qp)
 	return 0;
 }
 
-static struct ib_cq *c2_create_cq(struct ib_device *ibdev, int entries, int vector,
+static struct ib_cq *c2_create_cq(struct ib_device *ibdev,
+				  const struct ib_cq_init_attr *attr,
 				  struct ib_ucontext *context,
 				  struct ib_udata *udata)
 {
+	int entries = attr->cqe;
 	struct c2_cq *cq;
 	int err;
 
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	cq = kmalloc(sizeof(*cq), GFP_KERNEL);
 	if (!cq) {
 		pr_debug("%s: Unable to allocate CQ\n", __func__);
@@ -582,9 +590,13 @@ static int c2_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 static int c2_process_mad(struct ib_device *ibdev,
 			  int mad_flags,
 			  u8 port_num,
-			  struct ib_wc *in_wc,
-			  struct ib_grh *in_grh,
-			  struct ib_mad *in_mad, struct ib_mad *out_mad)
+			  const struct ib_wc *in_wc,
+			  const struct ib_grh *in_grh,
+			  const struct ib_mad_hdr *in_mad,
+			  size_t in_mad_size,
+			  struct ib_mad_hdr *out_mad,
+			  size_t *out_mad_size,
+			  u16 *out_mad_pkey_index)
 {
 	pr_debug("%s:%u\n", __func__, __LINE__);
 	return -ENOSYS;
@@ -757,6 +769,23 @@ static struct net_device *c2_pseudo_netdev_init(struct c2_dev *c2dev)
 	return netdev;
 }
 
+static int c2_port_immutable(struct ib_device *ibdev, u8 port_num,
+			     struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = c2_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IWARP;
+
+	return 0;
+}
+
 int c2_register_device(struct c2_dev *dev)
 {
 	int ret = -ENOMEM;
@@ -820,6 +849,7 @@ int c2_register_device(struct c2_dev *dev)
 	dev->ibdev.reg_phys_mr = c2_reg_phys_mr;
 	dev->ibdev.reg_user_mr = c2_reg_user_mr;
 	dev->ibdev.dereg_mr = c2_dereg_mr;
+	dev->ibdev.get_port_immutable = c2_port_immutable;
 
 	dev->ibdev.alloc_fmr = NULL;
 	dev->ibdev.unmap_fmr = NULL;
diff --git a/drivers/infiniband/hw/cxgb3/iwch_provider.c b/drivers/infiniband/hw/cxgb3/iwch_provider.c
index 811b24a539c0..b1b73232f217 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_provider.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_provider.c
@@ -85,9 +85,13 @@ static int iwch_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 static int iwch_process_mad(struct ib_device *ibdev,
 			    int mad_flags,
 			    u8 port_num,
-			    struct ib_wc *in_wc,
-			    struct ib_grh *in_grh,
-			    struct ib_mad *in_mad, struct ib_mad *out_mad)
+			    const struct ib_wc *in_wc,
+			    const struct ib_grh *in_grh,
+			    const struct ib_mad_hdr *in_mad,
+			    size_t in_mad_size,
+			    struct ib_mad_hdr *out_mad,
+			    size_t *out_mad_size,
+			    u16 *out_mad_pkey_index)
 {
 	return -ENOSYS;
 }
@@ -138,10 +142,12 @@ static int iwch_destroy_cq(struct ib_cq *ib_cq)
 	return 0;
 }
 
-static struct ib_cq *iwch_create_cq(struct ib_device *ibdev, int entries, int vector,
-			     struct ib_ucontext *ib_context,
-			     struct ib_udata *udata)
+static struct ib_cq *iwch_create_cq(struct ib_device *ibdev,
+				    const struct ib_cq_init_attr *attr,
+				    struct ib_ucontext *ib_context,
+				    struct ib_udata *udata)
 {
+	int entries = attr->cqe;
 	struct iwch_dev *rhp;
 	struct iwch_cq *chp;
 	struct iwch_create_cq_resp uresp;
@@ -151,6 +157,9 @@ static struct ib_cq *iwch_create_cq(struct ib_device *ibdev, int entries, int ve
 	size_t resplen;
 
 	PDBG("%s ib_dev %p entries %d\n", __func__, ibdev, entries);
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	rhp = to_iwch_dev(ibdev);
 	chp = kzalloc(sizeof(*chp), GFP_KERNEL);
 	if (!chp)
@@ -1145,13 +1154,17 @@ static u64 fw_vers_string_to_u64(struct iwch_dev *iwch_dev)
 	       (fw_mic & 0xffff);
 }
 
-static int iwch_query_device(struct ib_device *ibdev,
-			     struct ib_device_attr *props)
+static int iwch_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+			     struct ib_udata *uhw)
 {
 
 	struct iwch_dev *dev;
+
 	PDBG("%s ibdev %p\n", __func__, ibdev);
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	dev = to_iwch_dev(ibdev);
 	memset(props, 0, sizeof *props);
 	memcpy(&props->sys_image_guid, dev->rdev.t3cdev_p->lldev->dev_addr, 6);
@@ -1343,6 +1356,23 @@ static struct device_attribute *iwch_class_attributes[] = {
 	&dev_attr_board_id,
 };
 
+static int iwch_port_immutable(struct ib_device *ibdev, u8 port_num,
+			       struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = iwch_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IWARP;
+
+	return 0;
+}
+
 int iwch_register_device(struct iwch_dev *dev)
 {
 	int ret;
@@ -1420,6 +1450,7 @@ int iwch_register_device(struct iwch_dev *dev)
 	dev->ibdev.post_recv = iwch_post_receive;
 	dev->ibdev.get_protocol_stats = iwch_get_mib;
 	dev->ibdev.uverbs_abi_ver = IWCH_UVERBS_ABI_VERSION;
+	dev->ibdev.get_port_immutable = iwch_port_immutable;
 
 	dev->ibdev.iwcm = kmalloc(sizeof(struct iw_cm_verbs), GFP_KERNEL);
 	if (!dev->ibdev.iwcm)
diff --git a/drivers/infiniband/hw/cxgb4/cq.c b/drivers/infiniband/hw/cxgb4/cq.c
index 68ddb3710215..c7aab48f07cd 100644
--- a/drivers/infiniband/hw/cxgb4/cq.c
+++ b/drivers/infiniband/hw/cxgb4/cq.c
@@ -156,19 +156,17 @@ static int create_cq(struct c4iw_rdev *rdev, struct t4_cq *cq,
 		goto err4;
 
 	cq->gen = 1;
+	cq->gts = rdev->lldi.gts_reg;
 	cq->rdev = rdev;
-	if (user) {
-		u32 off = (cq->cqid << rdev->cqshift) & PAGE_MASK;
 
-		cq->ugts = (u64)rdev->bar2_pa + off;
-	} else if (is_t4(rdev->lldi.adapter_type)) {
-		cq->gts = rdev->lldi.gts_reg;
-		cq->qid_mask = -1U;
-	} else {
-		u32 off = ((cq->cqid << rdev->cqshift) & PAGE_MASK) + 12;
-
-		cq->gts = rdev->bar2_kva + off;
-		cq->qid_mask = rdev->qpmask;
+	cq->bar2_va = c4iw_bar2_addrs(rdev, cq->cqid, T4_BAR2_QTYPE_INGRESS,
+				      &cq->bar2_qid,
+				      user ? &cq->bar2_pa : NULL);
+	if (user && !cq->bar2_va) {
+		pr_warn(MOD "%s: cqid %u not in BAR2 range.\n",
+			pci_name(rdev->lldi.pdev), cq->cqid);
+		ret = -EINVAL;
+		goto err4;
 	}
 	return 0;
 err4:
@@ -866,10 +864,13 @@ int c4iw_destroy_cq(struct ib_cq *ib_cq)
 	return 0;
 }
 
-struct ib_cq *c4iw_create_cq(struct ib_device *ibdev, int entries,
-			     int vector, struct ib_ucontext *ib_context,
+struct ib_cq *c4iw_create_cq(struct ib_device *ibdev,
+			     const struct ib_cq_init_attr *attr,
+			     struct ib_ucontext *ib_context,
 			     struct ib_udata *udata)
 {
+	int entries = attr->cqe;
+	int vector = attr->comp_vector;
 	struct c4iw_dev *rhp;
 	struct c4iw_cq *chp;
 	struct c4iw_create_cq_resp uresp;
@@ -879,6 +880,8 @@ struct ib_cq *c4iw_create_cq(struct ib_device *ibdev, int entries,
 	struct c4iw_mm_entry *mm, *mm2;
 
 	PDBG("%s ib_dev %p entries %d\n", __func__, ibdev, entries);
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
 
 	rhp = to_c4iw_dev(ibdev);
 
@@ -971,7 +974,7 @@ struct ib_cq *c4iw_create_cq(struct ib_device *ibdev, int entries,
 		insert_mmap(ucontext, mm);
 
 		mm2->key = uresp.gts_key;
-		mm2->addr = chp->cq.ugts;
+		mm2->addr = chp->cq.bar2_pa;
 		mm2->len = PAGE_SIZE;
 		insert_mmap(ucontext, mm2);
 	}
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index 7e895d714b19..1a297391b54c 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -795,13 +795,7 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 		goto err1;
 	}
 
-	/*
-	 * qpshift is the number of bits to shift the qpid left in order
-	 * to get the correct address of the doorbell for that qp.
-	 */
-	rdev->qpshift = PAGE_SHIFT - ilog2(rdev->lldi.udb_density);
 	rdev->qpmask = rdev->lldi.udb_density - 1;
-	rdev->cqshift = PAGE_SHIFT - ilog2(rdev->lldi.ucq_density);
 	rdev->cqmask = rdev->lldi.ucq_density - 1;
 	PDBG("%s dev %s stag start 0x%0x size 0x%0x num stags %d "
 	     "pbl start 0x%0x size 0x%0x rq start 0x%0x size 0x%0x "
@@ -815,14 +809,12 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 	     rdev->lldi.vr->qp.size,
 	     rdev->lldi.vr->cq.start,
 	     rdev->lldi.vr->cq.size);
-	PDBG("udb len 0x%x udb base %p db_reg %p gts_reg %p qpshift %lu "
-	     "qpmask 0x%x cqshift %lu cqmask 0x%x\n",
+	PDBG("udb len 0x%x udb base %p db_reg %p gts_reg %p "
+	     "qpmask 0x%x cqmask 0x%x\n",
 	     (unsigned)pci_resource_len(rdev->lldi.pdev, 2),
 	     (void *)pci_resource_start(rdev->lldi.pdev, 2),
-	     rdev->lldi.db_reg,
-	     rdev->lldi.gts_reg,
-	     rdev->qpshift, rdev->qpmask,
-	     rdev->cqshift, rdev->cqmask);
+	     rdev->lldi.db_reg, rdev->lldi.gts_reg,
+	     rdev->qpmask, rdev->cqmask);
 
 	if (c4iw_num_stags(rdev) == 0) {
 		err = -EINVAL;
diff --git a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
index 97bb5550a6cf..cc77844fada3 100644
--- a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
+++ b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
@@ -165,9 +165,7 @@ struct wr_log_entry {
 
 struct c4iw_rdev {
 	struct c4iw_resource resource;
-	unsigned long qpshift;
 	u32 qpmask;
-	unsigned long cqshift;
 	u32 cqmask;
 	struct c4iw_dev_ucontext uctx;
 	struct gen_pool *pbl_pool;
@@ -992,10 +990,10 @@ int c4iw_reregister_phys_mem(struct ib_mr *mr,
 				     int acc, u64 *iova_start);
 int c4iw_dereg_mr(struct ib_mr *ib_mr);
 int c4iw_destroy_cq(struct ib_cq *ib_cq);
-struct ib_cq *c4iw_create_cq(struct ib_device *ibdev, int entries,
-					int vector,
-					struct ib_ucontext *ib_context,
-					struct ib_udata *udata);
+struct ib_cq *c4iw_create_cq(struct ib_device *ibdev,
+			     const struct ib_cq_init_attr *attr,
+			     struct ib_ucontext *ib_context,
+			     struct ib_udata *udata);
 int c4iw_resize_cq(struct ib_cq *cq, int cqe, struct ib_udata *udata);
 int c4iw_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags flags);
 int c4iw_destroy_qp(struct ib_qp *ib_qp);
@@ -1032,6 +1030,9 @@ void c4iw_ev_dispatch(struct c4iw_dev *dev, struct t4_cqe *err_cqe);
 
 extern struct cxgb4_client t4c_client;
 extern c4iw_handler_func c4iw_handlers[NUM_CPL_CMDS];
+void __iomem *c4iw_bar2_addrs(struct c4iw_rdev *rdev, unsigned int qid,
+			      enum cxgb4_bar2_qtype qtype,
+			      unsigned int *pbar2_qid, u64 *pbar2_pa);
 extern void c4iw_log_wr_stats(struct t4_wq *wq, struct t4_cqe *cqe);
 extern int c4iw_wr_log;
 extern int db_fc_threshold;
diff --git a/drivers/infiniband/hw/cxgb4/provider.c b/drivers/infiniband/hw/cxgb4/provider.c
index 66bd6a2ad83b..6eee3d385541 100644
--- a/drivers/infiniband/hw/cxgb4/provider.c
+++ b/drivers/infiniband/hw/cxgb4/provider.c
@@ -80,9 +80,13 @@ static int c4iw_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 }
 
 static int c4iw_process_mad(struct ib_device *ibdev, int mad_flags,
-			    u8 port_num, struct ib_wc *in_wc,
-			    struct ib_grh *in_grh, struct ib_mad *in_mad,
-			    struct ib_mad *out_mad)
+			    u8 port_num, const struct ib_wc *in_wc,
+			    const struct ib_grh *in_grh,
+			    const struct ib_mad_hdr *in_mad,
+			    size_t in_mad_size,
+			    struct ib_mad_hdr *out_mad,
+			    size_t *out_mad_size,
+			    u16 *out_mad_pkey_index)
 {
 	return -ENOSYS;
 }
@@ -301,13 +305,17 @@ static int c4iw_query_gid(struct ib_device *ibdev, u8 port, int index,
 	return 0;
 }
 
-static int c4iw_query_device(struct ib_device *ibdev,
-			     struct ib_device_attr *props)
+static int c4iw_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+			     struct ib_udata *uhw)
 {
 
 	struct c4iw_dev *dev;
+
 	PDBG("%s ibdev %p\n", __func__, ibdev);
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	dev = to_c4iw_dev(ibdev);
 	memset(props, 0, sizeof *props);
 	memcpy(&props->sys_image_guid, dev->rdev.lldi.ports[0]->dev_addr, 6);
@@ -445,10 +453,10 @@ static int c4iw_get_mib(struct ib_device *ibdev,
 
 	cxgb4_get_tcp_stats(c4iw_dev->rdev.lldi.pdev, &v4, &v6);
 	memset(stats, 0, sizeof *stats);
-	stats->iw.tcpInSegs = v4.tcpInSegs + v6.tcpInSegs;
-	stats->iw.tcpOutSegs = v4.tcpOutSegs + v6.tcpOutSegs;
-	stats->iw.tcpRetransSegs = v4.tcpRetransSegs + v6.tcpRetransSegs;
-	stats->iw.tcpOutRsts = v4.tcpOutRsts + v6.tcpOutSegs;
+	stats->iw.tcpInSegs = v4.tcp_in_segs + v6.tcp_in_segs;
+	stats->iw.tcpOutSegs = v4.tcp_out_segs + v6.tcp_out_segs;
+	stats->iw.tcpRetransSegs = v4.tcp_retrans_segs + v6.tcp_retrans_segs;
+	stats->iw.tcpOutRsts = v4.tcp_out_rsts + v6.tcp_out_rsts;
 
 	return 0;
 }
@@ -465,6 +473,23 @@ static struct device_attribute *c4iw_class_attributes[] = {
 	&dev_attr_board_id,
 };
 
+static int c4iw_port_immutable(struct ib_device *ibdev, u8 port_num,
+			       struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = c4iw_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IWARP;
+
+	return 0;
+}
+
 int c4iw_register_device(struct c4iw_dev *dev)
 {
 	int ret;
@@ -542,6 +567,7 @@ int c4iw_register_device(struct c4iw_dev *dev)
 	dev->ibdev.post_recv = c4iw_post_receive;
 	dev->ibdev.get_protocol_stats = c4iw_get_mib;
 	dev->ibdev.uverbs_abi_ver = C4IW_UVERBS_ABI_VERSION;
+	dev->ibdev.get_port_immutable = c4iw_port_immutable;
 
 	dev->ibdev.iwcm = kmalloc(sizeof(struct iw_cm_verbs), GFP_KERNEL);
 	if (!dev->ibdev.iwcm)
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index 389ced335bc5..6517e1208ccb 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -165,6 +165,29 @@ static int destroy_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 	return 0;
 }
 
+/*
+ * Determine the BAR2 virtual address and qid. If pbar2_pa is not NULL,
+ * then this is a user mapping so compute the page-aligned physical address
+ * for mapping.
+ */
+void __iomem *c4iw_bar2_addrs(struct c4iw_rdev *rdev, unsigned int qid,
+			      enum cxgb4_bar2_qtype qtype,
+			      unsigned int *pbar2_qid, u64 *pbar2_pa)
+{
+	u64 bar2_qoffset;
+	int ret;
+
+	ret = cxgb4_bar2_sge_qregs(rdev->lldi.ports[0], qid, qtype,
+				   pbar2_pa ? 1 : 0,
+				   &bar2_qoffset, pbar2_qid);
+	if (ret)
+		return NULL;
+
+	if (pbar2_pa)
+		*pbar2_pa = (rdev->bar2_pa + bar2_qoffset) & PAGE_MASK;
+	return rdev->bar2_kva + bar2_qoffset;
+}
+
 static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 		     struct t4_cq *rcq, struct t4_cq *scq,
 		     struct c4iw_dev_ucontext *uctx)
@@ -236,25 +259,23 @@ static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 	dma_unmap_addr_set(&wq->rq, mapping, wq->rq.dma_addr);
 
 	wq->db = rdev->lldi.db_reg;
-	wq->gts = rdev->lldi.gts_reg;
-	if (user || is_t5(rdev->lldi.adapter_type)) {
-		u32 off;
 
-		off = (wq->sq.qid << rdev->qpshift) & PAGE_MASK;
-		if (user) {
-			wq->sq.udb = (u64 __iomem *)(rdev->bar2_pa + off);
-		} else {
-			off += 128 * (wq->sq.qid & rdev->qpmask) + 8;
-			wq->sq.udb = (u64 __iomem *)(rdev->bar2_kva + off);
-		}
-		off = (wq->rq.qid << rdev->qpshift) & PAGE_MASK;
-		if (user) {
-			wq->rq.udb = (u64 __iomem *)(rdev->bar2_pa + off);
-		} else {
-			off += 128 * (wq->rq.qid & rdev->qpmask) + 8;
-			wq->rq.udb = (u64 __iomem *)(rdev->bar2_kva + off);
-		}
+	wq->sq.bar2_va = c4iw_bar2_addrs(rdev, wq->sq.qid, T4_BAR2_QTYPE_EGRESS,
+					 &wq->sq.bar2_qid,
+					 user ? &wq->sq.bar2_pa : NULL);
+	wq->rq.bar2_va = c4iw_bar2_addrs(rdev, wq->rq.qid, T4_BAR2_QTYPE_EGRESS,
+					 &wq->rq.bar2_qid,
+					 user ? &wq->rq.bar2_pa : NULL);
+
+	/*
+	 * User mode must have bar2 access.
+	 */
+	if (user && (!wq->sq.bar2_va || !wq->rq.bar2_va)) {
+		pr_warn(MOD "%s: sqid %u or rqid %u not in BAR2 range.\n",
+			pci_name(rdev->lldi.pdev), wq->sq.qid, wq->rq.qid);
+		goto free_dma;
 	}
+
 	wq->rdev = rdev;
 	wq->rq.msn = 1;
 
@@ -336,10 +357,9 @@ static int create_qp(struct c4iw_rdev *rdev, struct t4_wq *wq,
 	if (ret)
 		goto free_dma;
 
-	PDBG("%s sqid 0x%x rqid 0x%x kdb 0x%p squdb 0x%lx rqudb 0x%lx\n",
+	PDBG("%s sqid 0x%x rqid 0x%x kdb 0x%p sq_bar2_addr %p rq_bar2_addr %p\n",
 	     __func__, wq->sq.qid, wq->rq.qid, wq->db,
-	     (__force unsigned long) wq->sq.udb,
-	     (__force unsigned long) wq->rq.udb);
+	     wq->sq.bar2_va, wq->rq.bar2_va);
 
 	return 0;
 free_dma:
@@ -1766,11 +1786,11 @@ struct ib_qp *c4iw_create_qp(struct ib_pd *pd, struct ib_qp_init_attr *attrs,
 		mm2->len = PAGE_ALIGN(qhp->wq.rq.memsize);
 		insert_mmap(ucontext, mm2);
 		mm3->key = uresp.sq_db_gts_key;
-		mm3->addr = (__force unsigned long)qhp->wq.sq.udb;
+		mm3->addr = (__force unsigned long)qhp->wq.sq.bar2_pa;
 		mm3->len = PAGE_SIZE;
 		insert_mmap(ucontext, mm3);
 		mm4->key = uresp.rq_db_gts_key;
-		mm4->addr = (__force unsigned long)qhp->wq.rq.udb;
+		mm4->addr = (__force unsigned long)qhp->wq.rq.bar2_pa;
 		mm4->len = PAGE_SIZE;
 		insert_mmap(ucontext, mm4);
 		if (mm5) {
diff --git a/drivers/infiniband/hw/cxgb4/t4.h b/drivers/infiniband/hw/cxgb4/t4.h
index 7f2a6c244d25..274a7ab13bef 100644
--- a/drivers/infiniband/hw/cxgb4/t4.h
+++ b/drivers/infiniband/hw/cxgb4/t4.h
@@ -33,6 +33,7 @@
 
 #include "t4_hw.h"
 #include "t4_regs.h"
+#include "t4_values.h"
 #include "t4_msg.h"
 #include "t4fw_ri_api.h"
 
@@ -290,8 +291,10 @@ struct t4_sq {
 	unsigned long phys_addr;
 	struct t4_swsqe *sw_sq;
 	struct t4_swsqe *oldest_read;
-	u64 __iomem *udb;
+	void __iomem *bar2_va;
+	u64 bar2_pa;
 	size_t memsize;
+	u32 bar2_qid;
 	u32 qid;
 	u16 in_use;
 	u16 size;
@@ -314,8 +317,10 @@ struct t4_rq {
 	dma_addr_t dma_addr;
 	DEFINE_DMA_UNMAP_ADDR(mapping);
 	struct t4_swrqe *sw_rq;
-	u64 __iomem *udb;
+	void __iomem *bar2_va;
+	u64 bar2_pa;
 	size_t memsize;
+	u32 bar2_qid;
 	u32 qid;
 	u32 msn;
 	u32 rqt_hwaddr;
@@ -332,7 +337,6 @@ struct t4_wq {
 	struct t4_sq sq;
 	struct t4_rq rq;
 	void __iomem *db;
-	void __iomem *gts;
 	struct c4iw_rdev *rdev;
 	int flushed;
 };
@@ -457,15 +461,18 @@ static inline void t4_ring_sq_db(struct t4_wq *wq, u16 inc, u8 t5,
 
 	/* Flush host queue memory writes. */
 	wmb();
-	if (t5) {
-		if (inc == 1 && wqe) {
+	if (wq->sq.bar2_va) {
+		if (inc == 1 && wq->sq.bar2_qid == 0 && wqe) {
 			PDBG("%s: WC wq->sq.pidx = %d\n",
 			     __func__, wq->sq.pidx);
-			pio_copy(wq->sq.udb + 7, (void *)wqe);
+			pio_copy((u64 __iomem *)
+				 (wq->sq.bar2_va + SGE_UDB_WCDOORBELL),
+				 (u64 *)wqe);
 		} else {
 			PDBG("%s: DB wq->sq.pidx = %d\n",
 			     __func__, wq->sq.pidx);
-			writel(PIDX_T5_V(inc), wq->sq.udb);
+			writel(PIDX_T5_V(inc) | QID_V(wq->sq.bar2_qid),
+			       wq->sq.bar2_va + SGE_UDB_KDOORBELL);
 		}
 
 		/* Flush user doorbell area writes. */
@@ -481,15 +488,18 @@ static inline void t4_ring_rq_db(struct t4_wq *wq, u16 inc, u8 t5,
 
 	/* Flush host queue memory writes. */
 	wmb();
-	if (t5) {
-		if (inc == 1 && wqe) {
+	if (wq->rq.bar2_va) {
+		if (inc == 1 && wq->rq.bar2_qid == 0 && wqe) {
 			PDBG("%s: WC wq->rq.pidx = %d\n",
 			     __func__, wq->rq.pidx);
-			pio_copy(wq->rq.udb + 7, (void *)wqe);
+			pio_copy((u64 __iomem *)
+				 (wq->rq.bar2_va + SGE_UDB_WCDOORBELL),
+				 (void *)wqe);
 		} else {
 			PDBG("%s: DB wq->rq.pidx = %d\n",
 			     __func__, wq->rq.pidx);
-			writel(PIDX_T5_V(inc), wq->rq.udb);
+			writel(PIDX_T5_V(inc) | QID_V(wq->rq.bar2_qid),
+			       wq->rq.bar2_va + SGE_UDB_KDOORBELL);
 		}
 
 		/* Flush user doorbell area writes. */
@@ -534,8 +544,10 @@ struct t4_cq {
 	DEFINE_DMA_UNMAP_ADDR(mapping);
 	struct t4_cqe *sw_queue;
 	void __iomem *gts;
+	void __iomem *bar2_va;
+	u64 bar2_pa;
+	u32 bar2_qid;
 	struct c4iw_rdev *rdev;
-	u64 ugts;
 	size_t memsize;
 	__be64 bits_type_ts;
 	u32 cqid;
@@ -552,6 +564,15 @@ struct t4_cq {
 	unsigned long flags;
 };
 
+static inline void write_gts(struct t4_cq *cq, u32 val)
+{
+	if (cq->bar2_va)
+		writel(val | INGRESSQID_V(cq->bar2_qid),
+		       cq->bar2_va + SGE_UDB_GTS);
+	else
+		writel(val | INGRESSQID_V(cq->cqid), cq->gts);
+}
+
 static inline int t4_clear_cq_armed(struct t4_cq *cq)
 {
 	return test_and_clear_bit(CQ_ARMED, &cq->flags);
@@ -563,14 +584,12 @@ static inline int t4_arm_cq(struct t4_cq *cq, int se)
 
 	set_bit(CQ_ARMED, &cq->flags);
 	while (cq->cidx_inc > CIDXINC_M) {
-		val = SEINTARM_V(0) | CIDXINC_V(CIDXINC_M) | TIMERREG_V(7) |
-		      INGRESSQID_V(cq->cqid & cq->qid_mask);
-		writel(val, cq->gts);
+		val = SEINTARM_V(0) | CIDXINC_V(CIDXINC_M) | TIMERREG_V(7);
+		write_gts(cq, val);
 		cq->cidx_inc -= CIDXINC_M;
 	}
-	val = SEINTARM_V(se) | CIDXINC_V(cq->cidx_inc) | TIMERREG_V(6) |
-	      INGRESSQID_V(cq->cqid & cq->qid_mask);
-	writel(val, cq->gts);
+	val = SEINTARM_V(se) | CIDXINC_V(cq->cidx_inc) | TIMERREG_V(6);
+	write_gts(cq, val);
 	cq->cidx_inc = 0;
 	return 0;
 }
@@ -601,9 +620,8 @@ static inline void t4_hwcq_consume(struct t4_cq *cq)
 	if (++cq->cidx_inc == (cq->size >> 4) || cq->cidx_inc == CIDXINC_M) {
 		u32 val;
 
-		val = SEINTARM_V(0) | CIDXINC_V(cq->cidx_inc) | TIMERREG_V(7) |
-		      INGRESSQID_V(cq->cqid & cq->qid_mask);
-		writel(val, cq->gts);
+		val = SEINTARM_V(0) | CIDXINC_V(cq->cidx_inc) | TIMERREG_V(7);
+		write_gts(cq, val);
 		cq->cidx_inc = 0;
 	}
 	if (++cq->cidx == cq->size) {
diff --git a/drivers/infiniband/hw/ehca/ehca_cq.c b/drivers/infiniband/hw/ehca/ehca_cq.c
index 8cc837537768..9b68b175069b 100644
--- a/drivers/infiniband/hw/ehca/ehca_cq.c
+++ b/drivers/infiniband/hw/ehca/ehca_cq.c
@@ -113,10 +113,12 @@ struct ehca_qp *ehca_cq_get_qp(struct ehca_cq *cq, int real_qp_num)
 	return ret;
 }
 
-struct ib_cq *ehca_create_cq(struct ib_device *device, int cqe, int comp_vector,
+struct ib_cq *ehca_create_cq(struct ib_device *device,
+			     const struct ib_cq_init_attr *attr,
 			     struct ib_ucontext *context,
 			     struct ib_udata *udata)
 {
+	int cqe = attr->cqe;
 	static const u32 additional_cqe = 20;
 	struct ib_cq *cq;
 	struct ehca_cq *my_cq;
@@ -131,6 +133,9 @@ struct ib_cq *ehca_create_cq(struct ib_device *device, int cqe, int comp_vector,
 	int ipz_rc, i;
 	unsigned long flags;
 
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	if (cqe >= 0xFFFFFFFF - 64 - additional_cqe)
 		return ERR_PTR(-EINVAL);
 
diff --git a/drivers/infiniband/hw/ehca/ehca_hca.c b/drivers/infiniband/hw/ehca/ehca_hca.c
index 9ed4d2588304..e8b1bb65797a 100644
--- a/drivers/infiniband/hw/ehca/ehca_hca.c
+++ b/drivers/infiniband/hw/ehca/ehca_hca.c
@@ -50,7 +50,8 @@ static unsigned int limit_uint(unsigned int value)
 	return min_t(unsigned int, value, INT_MAX);
 }
 
-int ehca_query_device(struct ib_device *ibdev, struct ib_device_attr *props)
+int ehca_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+		      struct ib_udata *uhw)
 {
 	int i, ret = 0;
 	struct ehca_shca *shca = container_of(ibdev, struct ehca_shca,
@@ -71,6 +72,9 @@ int ehca_query_device(struct ib_device *ibdev, struct ib_device_attr *props)
 		IB_DEVICE_PORT_ACTIVE_EVENT,  HCA_CAP_PORT_ACTIVE_EVENT,
 	};
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
 	if (!rblock) {
 		ehca_err(&shca->ib_device, "Can't allocate rblock memory.");
diff --git a/drivers/infiniband/hw/ehca/ehca_iverbs.h b/drivers/infiniband/hw/ehca/ehca_iverbs.h
index 22f79afa7fc1..80e6a3d5df3e 100644
--- a/drivers/infiniband/hw/ehca/ehca_iverbs.h
+++ b/drivers/infiniband/hw/ehca/ehca_iverbs.h
@@ -44,11 +44,15 @@
 
 #include "ehca_classes.h"
 
-int ehca_query_device(struct ib_device *ibdev, struct ib_device_attr *props);
+int ehca_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+		      struct ib_udata *uhw);
 
 int ehca_query_port(struct ib_device *ibdev, u8 port,
 		    struct ib_port_attr *props);
 
+enum rdma_protocol_type
+ehca_query_protocol(struct ib_device *device, u8 port_num);
+
 int ehca_query_sma_attr(struct ehca_shca *shca, u8 port,
 			struct ehca_sma_attr *attr);
 
@@ -126,7 +130,8 @@ int ehca_destroy_eq(struct ehca_shca *shca, struct ehca_eq *eq);
 void *ehca_poll_eq(struct ehca_shca *shca, struct ehca_eq *eq);
 
 
-struct ib_cq *ehca_create_cq(struct ib_device *device, int cqe, int comp_vector,
+struct ib_cq *ehca_create_cq(struct ib_device *device,
+			     const struct ib_cq_init_attr *attr,
 			     struct ib_ucontext *context,
 			     struct ib_udata *udata);
 
@@ -188,9 +193,10 @@ int ehca_dealloc_ucontext(struct ib_ucontext *context);
 int ehca_mmap(struct ib_ucontext *context, struct vm_area_struct *vma);
 
 int ehca_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-		     struct ib_wc *in_wc, struct ib_grh *in_grh,
-		     struct ib_mad *in_mad,
-		     struct ib_mad *out_mad);
+		     const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		     const struct ib_mad_hdr *in, size_t in_mad_size,
+		     struct ib_mad_hdr *out, size_t *out_mad_size,
+		     u16 *out_mad_pkey_index);
 
 void ehca_poll_eqs(unsigned long data);
 
diff --git a/drivers/infiniband/hw/ehca/ehca_main.c b/drivers/infiniband/hw/ehca/ehca_main.c
index cd8d290a09fc..8246418cd4e0 100644
--- a/drivers/infiniband/hw/ehca/ehca_main.c
+++ b/drivers/infiniband/hw/ehca/ehca_main.c
@@ -46,6 +46,7 @@
 
 #include <linux/notifier.h>
 #include <linux/memory.h>
+#include <rdma/ib_mad.h>
 #include "ehca_classes.h"
 #include "ehca_iverbs.h"
 #include "ehca_mrmw.h"
@@ -431,6 +432,24 @@ init_node_guid1:
 	return ret;
 }
 
+static int ehca_port_immutable(struct ib_device *ibdev, u8 port_num,
+			       struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = ehca_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
+	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
+
+	return 0;
+}
+
 static int ehca_init_device(struct ehca_shca *shca)
 {
 	int ret;
@@ -510,6 +529,7 @@ static int ehca_init_device(struct ehca_shca *shca)
 	shca->ib_device.process_mad	    = ehca_process_mad;
 	shca->ib_device.mmap		    = ehca_mmap;
 	shca->ib_device.dma_ops		    = &ehca_dma_mapping_ops;
+	shca->ib_device.get_port_immutable  = ehca_port_immutable;
 
 	if (EHCA_BMASK_GET(HCA_CAP_SRQ, shca->hca_cap)) {
 		shca->ib_device.uverbs_cmd_mask |=
@@ -534,6 +554,7 @@ static int ehca_create_aqp1(struct ehca_shca *shca, u32 port)
 	struct ib_cq *ibcq;
 	struct ib_qp *ibqp;
 	struct ib_qp_init_attr qp_init_attr;
+	struct ib_cq_init_attr cq_attr = {};
 	int ret;
 
 	if (sport->ibcq_aqp1) {
@@ -541,7 +562,9 @@ static int ehca_create_aqp1(struct ehca_shca *shca, u32 port)
 		return -EPERM;
 	}
 
-	ibcq = ib_create_cq(&shca->ib_device, NULL, NULL, (void *)(-1), 10, 0);
+	cq_attr.cqe = 10;
+	ibcq = ib_create_cq(&shca->ib_device, NULL, NULL, (void *)(-1),
+			    &cq_attr);
 	if (IS_ERR(ibcq)) {
 		ehca_err(&shca->ib_device, "Cannot create AQP1 CQ.");
 		return PTR_ERR(ibcq);
diff --git a/drivers/infiniband/hw/ehca/ehca_sqp.c b/drivers/infiniband/hw/ehca/ehca_sqp.c
index dba8f9f8b996..12b5bc23832b 100644
--- a/drivers/infiniband/hw/ehca/ehca_sqp.c
+++ b/drivers/infiniband/hw/ehca/ehca_sqp.c
@@ -140,10 +140,10 @@ struct vertcfl {
 } __attribute__ ((packed));
 
 static int ehca_process_perf(struct ib_device *ibdev, u8 port_num,
-			     struct ib_wc *in_wc, struct ib_grh *in_grh,
-			     struct ib_mad *in_mad, struct ib_mad *out_mad)
+			     const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+			     const struct ib_mad *in_mad, struct ib_mad *out_mad)
 {
-	struct ib_perf *in_perf = (struct ib_perf *)in_mad;
+	const struct ib_perf *in_perf = (const struct ib_perf *)in_mad;
 	struct ib_perf *out_perf = (struct ib_perf *)out_mad;
 	struct ib_class_port_info *poi =
 		(struct ib_class_port_info *)out_perf->data;
@@ -187,8 +187,8 @@ static int ehca_process_perf(struct ib_device *ibdev, u8 port_num,
 
 		/* if request was globally routed, copy route info */
 		if (in_grh) {
-			struct vertcfl *vertcfl =
-				(struct vertcfl *)&in_grh->version_tclass_flow;
+			const struct vertcfl *vertcfl =
+				(const struct vertcfl *)&in_grh->version_tclass_flow;
 			memcpy(poi->redirect_gid, in_grh->dgid.raw,
 			       sizeof(poi->redirect_gid));
 			tcslfl->tc        = vertcfl->tc;
@@ -217,10 +217,17 @@ perf_reply:
 }
 
 int ehca_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-		     struct ib_wc *in_wc, struct ib_grh *in_grh,
-		     struct ib_mad *in_mad, struct ib_mad *out_mad)
+		     const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		     const struct ib_mad_hdr *in, size_t in_mad_size,
+		     struct ib_mad_hdr *out, size_t *out_mad_size,
+		     u16 *out_mad_pkey_index)
 {
 	int ret;
+	const struct ib_mad *in_mad = (const struct ib_mad *)in;
+	struct ib_mad *out_mad = (struct ib_mad *)out;
+
+	BUG_ON(in_mad_size != sizeof(*in_mad) ||
+	       *out_mad_size != sizeof(*out_mad));
 
 	if (!port_num || port_num > ibdev->phys_port_cnt || !in_wc)
 		return IB_MAD_RESULT_FAILURE;
diff --git a/drivers/infiniband/hw/ehca/ipz_pt_fn.c b/drivers/infiniband/hw/ehca/ipz_pt_fn.c
index 8d594517cd29..7ffc748cb973 100644
--- a/drivers/infiniband/hw/ehca/ipz_pt_fn.c
+++ b/drivers/infiniband/hw/ehca/ipz_pt_fn.c
@@ -245,10 +245,7 @@ int ipz_queue_ctor(struct ehca_pd *pd, struct ipz_queue *queue,
 ipz_queue_ctor_exit0:
 	ehca_gen_err("Couldn't alloc pages queue=%p "
 		 "nr_of_pages=%x",  queue, nr_of_pages);
-	if (is_vmalloc_addr(queue->queue_pages))
-		vfree(queue->queue_pages);
-	else
-		kfree(queue->queue_pages);
+	kvfree(queue->queue_pages);
 
 	return 0;
 }
@@ -270,10 +267,7 @@ int ipz_queue_dtor(struct ehca_pd *pd, struct ipz_queue *queue)
 			free_page((unsigned long)queue->queue_pages[i]);
 	}
 
-	if (is_vmalloc_addr(queue->queue_pages))
-		vfree(queue->queue_pages);
-	else
-		kfree(queue->queue_pages);
+	kvfree(queue->queue_pages);
 
 	return 1;
 }
diff --git a/drivers/infiniband/hw/ipath/Kconfig b/drivers/infiniband/hw/ipath/Kconfig
index 1d9bb115cbf6..8fe54ff00580 100644
--- a/drivers/infiniband/hw/ipath/Kconfig
+++ b/drivers/infiniband/hw/ipath/Kconfig
@@ -9,3 +9,6 @@ config INFINIBAND_IPATH
 	as IP-over-InfiniBand as well as with userspace applications
 	(in conjunction with InfiniBand userspace access).
 	For QLogic PCIe QLE based cards, use the QIB driver instead.
+
+	If you have this hardware you will need to boot with PAT disabled
+	on your x86-64 systems, use the nopat kernel parameter.
diff --git a/drivers/infiniband/hw/ipath/ipath_cq.c b/drivers/infiniband/hw/ipath/ipath_cq.c
index 0416c6c0e126..e9dd9112e718 100644
--- a/drivers/infiniband/hw/ipath/ipath_cq.c
+++ b/drivers/infiniband/hw/ipath/ipath_cq.c
@@ -188,7 +188,7 @@ static void send_complete(unsigned long data)
 /**
  * ipath_create_cq - create a completion queue
  * @ibdev: the device this completion queue is attached to
- * @entries: the minimum size of the completion queue
+ * @attr: creation attributes
  * @context: unused by the InfiniPath driver
  * @udata: unused by the InfiniPath driver
  *
@@ -197,16 +197,21 @@ static void send_complete(unsigned long data)
  *
  * Called by ib_create_cq() in the generic verbs code.
  */
-struct ib_cq *ipath_create_cq(struct ib_device *ibdev, int entries, int comp_vector,
+struct ib_cq *ipath_create_cq(struct ib_device *ibdev,
+			      const struct ib_cq_init_attr *attr,
 			      struct ib_ucontext *context,
 			      struct ib_udata *udata)
 {
+	int entries = attr->cqe;
 	struct ipath_ibdev *dev = to_idev(ibdev);
 	struct ipath_cq *cq;
 	struct ipath_cq_wc *wc;
 	struct ib_cq *ret;
 	u32 sz;
 
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	if (entries < 1 || entries > ib_ipath_max_cqes) {
 		ret = ERR_PTR(-EINVAL);
 		goto done;
diff --git a/drivers/infiniband/hw/ipath/ipath_driver.c b/drivers/infiniband/hw/ipath/ipath_driver.c
index bd0caedafe99..2d7e503d13cb 100644
--- a/drivers/infiniband/hw/ipath/ipath_driver.c
+++ b/drivers/infiniband/hw/ipath/ipath_driver.c
@@ -42,6 +42,9 @@
 #include <linux/bitmap.h>
 #include <linux/slab.h>
 #include <linux/module.h>
+#ifdef CONFIG_X86_64
+#include <asm/pat.h>
+#endif
 
 #include "ipath_kernel.h"
 #include "ipath_verbs.h"
@@ -395,6 +398,14 @@ static int ipath_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	unsigned long long addr;
 	u32 bar0 = 0, bar1 = 0;
 
+#ifdef CONFIG_X86_64
+	if (WARN(pat_enabled(),
+		 "ipath needs PAT disabled, boot with nopat kernel parameter\n")) {
+		ret = -ENODEV;
+		goto bail;
+	}
+#endif
+
 	dd = ipath_alloc_devdata(pdev);
 	if (IS_ERR(dd)) {
 		ret = PTR_ERR(dd);
@@ -542,6 +553,7 @@ static int ipath_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	dd->ipath_kregbase = __ioremap(addr, len,
 		(_PAGE_NO_CACHE|_PAGE_WRITETHRU));
 #else
+	/* XXX: split this properly to enable on PAT */
 	dd->ipath_kregbase = ioremap_nocache(addr, len);
 #endif
 
@@ -587,12 +599,8 @@ static int ipath_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	ret = ipath_enable_wc(dd);
 
-	if (ret) {
-		ipath_dev_err(dd, "Write combining not enabled "
-			      "(err %d): performance may be poor\n",
-			      -ret);
+	if (ret)
 		ret = 0;
-	}
 
 	ipath_verify_pioperf(dd);
 
diff --git a/drivers/infiniband/hw/ipath/ipath_fs.c b/drivers/infiniband/hw/ipath/ipath_fs.c
index 1ca8e32a9592..25422a3a7238 100644
--- a/drivers/infiniband/hw/ipath/ipath_fs.c
+++ b/drivers/infiniband/hw/ipath/ipath_fs.c
@@ -277,7 +277,7 @@ static int remove_file(struct dentry *parent, char *name)
 	}
 
 	spin_lock(&tmp->d_lock);
-	if (!d_unhashed(tmp) && d_really_is_positive(tmp)) {
+	if (simple_positive(tmp)) {
 		dget_dlock(tmp);
 		__d_drop(tmp);
 		spin_unlock(&tmp->d_lock);
diff --git a/drivers/infiniband/hw/ipath/ipath_kernel.h b/drivers/infiniband/hw/ipath/ipath_kernel.h
index e08db7020cd4..f0f947122779 100644
--- a/drivers/infiniband/hw/ipath/ipath_kernel.h
+++ b/drivers/infiniband/hw/ipath/ipath_kernel.h
@@ -463,9 +463,7 @@ struct ipath_devdata {
 	/* offset in HT config space of slave/primary interface block */
 	u8 ipath_ht_slave_off;
 	/* for write combining settings */
-	unsigned long ipath_wc_cookie;
-	unsigned long ipath_wc_base;
-	unsigned long ipath_wc_len;
+	int wc_cookie;
 	/* ref count for each pkey */
 	atomic_t ipath_pkeyrefs[4];
 	/* shadow copy of struct page *'s for exp tid pages */
diff --git a/drivers/infiniband/hw/ipath/ipath_mad.c b/drivers/infiniband/hw/ipath/ipath_mad.c
index e890e5ba0e01..948188e37f95 100644
--- a/drivers/infiniband/hw/ipath/ipath_mad.c
+++ b/drivers/infiniband/hw/ipath/ipath_mad.c
@@ -1257,7 +1257,7 @@ static int recv_pma_set_portcounters_ext(struct ib_pma_mad *pmp,
 }
 
 static int process_subn(struct ib_device *ibdev, int mad_flags,
-			u8 port_num, struct ib_mad *in_mad,
+			u8 port_num, const struct ib_mad *in_mad,
 			struct ib_mad *out_mad)
 {
 	struct ib_smp *smp = (struct ib_smp *)out_mad;
@@ -1389,7 +1389,7 @@ bail:
 }
 
 static int process_perf(struct ib_device *ibdev, u8 port_num,
-			struct ib_mad *in_mad,
+			const struct ib_mad *in_mad,
 			struct ib_mad *out_mad)
 {
 	struct ib_pma_mad *pmp = (struct ib_pma_mad *)out_mad;
@@ -1490,10 +1490,17 @@ bail:
  * This is called by the ib_mad module.
  */
 int ipath_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-		      struct ib_wc *in_wc, struct ib_grh *in_grh,
-		      struct ib_mad *in_mad, struct ib_mad *out_mad)
+		      const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		      const struct ib_mad_hdr *in, size_t in_mad_size,
+		      struct ib_mad_hdr *out, size_t *out_mad_size,
+		      u16 *out_mad_pkey_index)
 {
 	int ret;
+	const struct ib_mad *in_mad = (const struct ib_mad *)in;
+	struct ib_mad *out_mad = (struct ib_mad *)out;
+
+	BUG_ON(in_mad_size != sizeof(*in_mad) ||
+	       *out_mad_size != sizeof(*out_mad));
 
 	switch (in_mad->mad_hdr.mgmt_class) {
 	case IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE:
diff --git a/drivers/infiniband/hw/ipath/ipath_verbs.c b/drivers/infiniband/hw/ipath/ipath_verbs.c
index 44ea9390417c..48253b839a6f 100644
--- a/drivers/infiniband/hw/ipath/ipath_verbs.c
+++ b/drivers/infiniband/hw/ipath/ipath_verbs.c
@@ -1495,11 +1495,14 @@ bail:
 	return 0;
 }
 
-static int ipath_query_device(struct ib_device *ibdev,
-			      struct ib_device_attr *props)
+static int ipath_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+			      struct ib_udata *uhw)
 {
 	struct ipath_ibdev *dev = to_idev(ibdev);
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	memset(props, 0, sizeof(*props));
 
 	props->device_cap_flags = IB_DEVICE_BAD_PKEY_CNTR |
@@ -1980,6 +1983,24 @@ static int disable_timer(struct ipath_devdata *dd)
 	return 0;
 }
 
+static int ipath_port_immutable(struct ib_device *ibdev, u8 port_num,
+			        struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = ipath_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
+	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
+
+	return 0;
+}
+
 /**
  * ipath_register_ib_device - register our device with the infiniband core
  * @dd: the device data structure
@@ -2179,6 +2200,7 @@ int ipath_register_ib_device(struct ipath_devdata *dd)
 	dev->process_mad = ipath_process_mad;
 	dev->mmap = ipath_mmap;
 	dev->dma_ops = &ipath_dma_mapping_ops;
+	dev->get_port_immutable = ipath_port_immutable;
 
 	snprintf(dev->node_desc, sizeof(dev->node_desc),
 		 IPATH_IDSTR " %s", init_utsname()->nodename);
diff --git a/drivers/infiniband/hw/ipath/ipath_verbs.h b/drivers/infiniband/hw/ipath/ipath_verbs.h
index ae6cff4abffc..ec167e545e15 100644
--- a/drivers/infiniband/hw/ipath/ipath_verbs.h
+++ b/drivers/infiniband/hw/ipath/ipath_verbs.h
@@ -701,9 +701,11 @@ static inline void ipath_schedule_send(struct ipath_qp *qp)
 int ipath_process_mad(struct ib_device *ibdev,
 		      int mad_flags,
 		      u8 port_num,
-		      struct ib_wc *in_wc,
-		      struct ib_grh *in_grh,
-		      struct ib_mad *in_mad, struct ib_mad *out_mad);
+		      const struct ib_wc *in_wc,
+		      const struct ib_grh *in_grh,
+		      const struct ib_mad_hdr *in, size_t in_mad_size,
+		      struct ib_mad_hdr *out, size_t *out_mad_size,
+		      u16 *out_mad_pkey_index);
 
 /*
  * Compare the lower 24 bits of the two values.
@@ -807,7 +809,8 @@ void ipath_cq_enter(struct ipath_cq *cq, struct ib_wc *entry, int sig);
 
 int ipath_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *entry);
 
-struct ib_cq *ipath_create_cq(struct ib_device *ibdev, int entries, int comp_vector,
+struct ib_cq *ipath_create_cq(struct ib_device *ibdev,
+			      const struct ib_cq_init_attr *attr,
 			      struct ib_ucontext *context,
 			      struct ib_udata *udata);
 
diff --git a/drivers/infiniband/hw/ipath/ipath_wc_x86_64.c b/drivers/infiniband/hw/ipath/ipath_wc_x86_64.c
index 4ad0b932df1f..7b6e4c843e19 100644
--- a/drivers/infiniband/hw/ipath/ipath_wc_x86_64.c
+++ b/drivers/infiniband/hw/ipath/ipath_wc_x86_64.c
@@ -37,7 +37,6 @@
  */
 
 #include <linux/pci.h>
-#include <asm/mtrr.h>
 #include <asm/processor.h>
 
 #include "ipath_kernel.h"
@@ -122,27 +121,14 @@ int ipath_enable_wc(struct ipath_devdata *dd)
 	}
 
 	if (!ret) {
-		int cookie;
-		ipath_cdbg(VERBOSE, "Setting mtrr for chip to WC "
-			   "(addr %llx, len=0x%llx)\n",
-			   (unsigned long long) pioaddr,
-			   (unsigned long long) piolen);
-		cookie = mtrr_add(pioaddr, piolen, MTRR_TYPE_WRCOMB, 0);
-		if (cookie < 0) {
-			{
-				dev_info(&dd->pcidev->dev,
-					 "mtrr_add()  WC for PIO bufs "
-					 "failed (%d)\n",
-					 cookie);
-				ret = -EINVAL;
-			}
-		} else {
-			ipath_cdbg(VERBOSE, "Set mtrr for chip to WC, "
-				   "cookie is %d\n", cookie);
-			dd->ipath_wc_cookie = cookie;
-			dd->ipath_wc_base = (unsigned long) pioaddr;
-			dd->ipath_wc_len = (unsigned long) piolen;
-		}
+		dd->wc_cookie = arch_phys_wc_add(pioaddr, piolen);
+		if (dd->wc_cookie < 0) {
+			ipath_dev_err(dd, "Seting mtrr failed on PIO buffers\n");
+			ret = -ENODEV;
+		} else if (dd->wc_cookie == 0)
+			ipath_cdbg(VERBOSE, "Set mtrr for chip to WC not needed\n");
+		else
+			ipath_cdbg(VERBOSE, "Set mtrr for chip to WC\n");
 	}
 
 	return ret;
@@ -154,16 +140,5 @@ int ipath_enable_wc(struct ipath_devdata *dd)
  */
 void ipath_disable_wc(struct ipath_devdata *dd)
 {
-	if (dd->ipath_wc_cookie) {
-		int r;
-		ipath_cdbg(VERBOSE, "undoing WCCOMB on pio buffers\n");
-		r = mtrr_del(dd->ipath_wc_cookie, dd->ipath_wc_base,
-			     dd->ipath_wc_len);
-		if (r < 0)
-			dev_info(&dd->pcidev->dev,
-				 "mtrr_del(%lx, %lx, %lx) failed: %d\n",
-				 dd->ipath_wc_cookie, dd->ipath_wc_base,
-				 dd->ipath_wc_len, r);
-		dd->ipath_wc_cookie = 0; /* even on failure */
-	}
+	arch_phys_wc_del(dd->wc_cookie);
 }
diff --git a/drivers/infiniband/hw/mlx4/alias_GUID.c b/drivers/infiniband/hw/mlx4/alias_GUID.c
index 0f00204d2ece..21cb41a60fe8 100644
--- a/drivers/infiniband/hw/mlx4/alias_GUID.c
+++ b/drivers/infiniband/hw/mlx4/alias_GUID.c
@@ -189,7 +189,7 @@ void mlx4_ib_notify_slaves_on_guid_change(struct mlx4_ib_dev *dev,
 {
 	int i;
 	u64 guid_indexes;
-	int slave_id;
+	int slave_id, slave_port;
 	enum slave_port_state new_state;
 	enum slave_port_state prev_state;
 	__be64 tmp_cur_ag, form_cache_ag;
@@ -217,6 +217,11 @@ void mlx4_ib_notify_slaves_on_guid_change(struct mlx4_ib_dev *dev,
 		slave_id = (block_num * NUM_ALIAS_GUID_IN_REC) + i ;
 		if (slave_id >= dev->dev->persist->num_vfs + 1)
 			return;
+
+		slave_port = mlx4_phys_to_slave_port(dev->dev, slave_id, port_num);
+		if (slave_port < 0) /* this port isn't available for the VF */
+			continue;
+
 		tmp_cur_ag = *(__be64 *)&p_data[i * GUID_REC_SIZE];
 		form_cache_ag = get_cached_alias_guid(dev, port_num,
 					(NUM_ALIAS_GUID_IN_REC * block_num) + i);
diff --git a/drivers/infiniband/hw/mlx4/cq.c b/drivers/infiniband/hw/mlx4/cq.c
index 0176caa5792c..36eb3d012b6d 100644
--- a/drivers/infiniband/hw/mlx4/cq.c
+++ b/drivers/infiniband/hw/mlx4/cq.c
@@ -166,10 +166,14 @@ err_buf:
 	return err;
 }
 
-struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev, int entries, int vector,
+#define CQ_CREATE_FLAGS_SUPPORTED IB_CQ_FLAGS_TIMESTAMP_COMPLETION
+struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev,
+				const struct ib_cq_init_attr *attr,
 				struct ib_ucontext *context,
 				struct ib_udata *udata)
 {
+	int entries = attr->cqe;
+	int vector = attr->comp_vector;
 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
 	struct mlx4_ib_cq *cq;
 	struct mlx4_uar *uar;
@@ -178,6 +182,9 @@ struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev, int entries, int vector
 	if (entries < 1 || entries > dev->dev->caps.max_cqes)
 		return ERR_PTR(-EINVAL);
 
+	if (attr->flags & ~CQ_CREATE_FLAGS_SUPPORTED)
+		return ERR_PTR(-EINVAL);
+
 	cq = kmalloc(sizeof *cq, GFP_KERNEL);
 	if (!cq)
 		return ERR_PTR(-ENOMEM);
@@ -188,6 +195,7 @@ struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev, int entries, int vector
 	spin_lock_init(&cq->lock);
 	cq->resize_buf = NULL;
 	cq->resize_umem = NULL;
+	cq->create_flags = attr->flags;
 	INIT_LIST_HEAD(&cq->send_qp_list);
 	INIT_LIST_HEAD(&cq->recv_qp_list);
 
@@ -231,7 +239,8 @@ struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev, int entries, int vector
 		vector = dev->eq_table[vector % ibdev->num_comp_vectors];
 
 	err = mlx4_cq_alloc(dev->dev, entries, &cq->buf.mtt, uar,
-			    cq->db.dma, &cq->mcq, vector, 0, 0);
+			    cq->db.dma, &cq->mcq, vector, 0,
+			    !!(cq->create_flags & IB_CQ_FLAGS_TIMESTAMP_COMPLETION));
 	if (err)
 		goto err_dbmap;
 
diff --git a/drivers/infiniband/hw/mlx4/mad.c b/drivers/infiniband/hw/mlx4/mad.c
index 9cd2b002d7ae..85a50df2f203 100644
--- a/drivers/infiniband/hw/mlx4/mad.c
+++ b/drivers/infiniband/hw/mlx4/mad.c
@@ -64,14 +64,6 @@ enum {
 #define GUID_TBL_BLK_NUM_ENTRIES 8
 #define GUID_TBL_BLK_SIZE (GUID_TBL_ENTRY_SIZE * GUID_TBL_BLK_NUM_ENTRIES)
 
-/* Counters should be saturate once they reach their maximum value */
-#define ASSIGN_32BIT_COUNTER(counter, value) do {\
-	if ((value) > U32_MAX)			 \
-		counter = cpu_to_be32(U32_MAX); \
-	else					 \
-		counter = cpu_to_be32(value);	 \
-} while (0)
-
 struct mlx4_mad_rcv_buf {
 	struct ib_grh grh;
 	u8 payload[256];
@@ -111,8 +103,9 @@ __be64 mlx4_ib_get_new_demux_tid(struct mlx4_ib_demux_ctx *ctx)
 }
 
 int mlx4_MAD_IFC(struct mlx4_ib_dev *dev, int mad_ifc_flags,
-		 int port, struct ib_wc *in_wc, struct ib_grh *in_grh,
-		 void *in_mad, void *response_mad)
+		 int port, const struct ib_wc *in_wc,
+		 const struct ib_grh *in_grh,
+		 const void *in_mad, void *response_mad)
 {
 	struct mlx4_cmd_mailbox *inmailbox, *outmailbox;
 	void *inbox;
@@ -220,7 +213,7 @@ static void update_sm_ah(struct mlx4_ib_dev *dev, u8 port_num, u16 lid, u8 sl)
  * Snoop SM MADs for port info, GUID info, and  P_Key table sets, so we can
  * synthesize LID change, Client-Rereg, GID change, and P_Key change events.
  */
-static void smp_snoop(struct ib_device *ibdev, u8 port_num, struct ib_mad *mad,
+static void smp_snoop(struct ib_device *ibdev, u8 port_num, const struct ib_mad *mad,
 		      u16 prev_lid)
 {
 	struct ib_port_info *pinfo;
@@ -356,7 +349,7 @@ static void node_desc_override(struct ib_device *dev,
 	}
 }
 
-static void forward_trap(struct mlx4_ib_dev *dev, u8 port_num, struct ib_mad *mad)
+static void forward_trap(struct mlx4_ib_dev *dev, u8 port_num, const struct ib_mad *mad)
 {
 	int qpn = mad->mad_hdr.mgmt_class != IB_MGMT_CLASS_SUBN_LID_ROUTED;
 	struct ib_mad_send_buf *send_buf;
@@ -366,7 +359,8 @@ static void forward_trap(struct mlx4_ib_dev *dev, u8 port_num, struct ib_mad *ma
 
 	if (agent) {
 		send_buf = ib_create_send_mad(agent, qpn, 0, 0, IB_MGMT_MAD_HDR,
-					      IB_MGMT_MAD_DATA, GFP_ATOMIC);
+					      IB_MGMT_MAD_DATA, GFP_ATOMIC,
+					      IB_MGMT_BASE_VERSION);
 		if (IS_ERR(send_buf))
 			return;
 		/*
@@ -722,8 +716,8 @@ static int mlx4_ib_demux_mad(struct ib_device *ibdev, u8 port,
 }
 
 static int ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-			struct ib_wc *in_wc, struct ib_grh *in_grh,
-			struct ib_mad *in_mad, struct ib_mad *out_mad)
+			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+			const struct ib_mad *in_mad, struct ib_mad *out_mad)
 {
 	u16 slid, prev_lid = 0;
 	int err;
@@ -825,34 +819,28 @@ static void edit_counter(struct mlx4_counter *cnt,
 }
 
 static int iboe_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-			struct ib_wc *in_wc, struct ib_grh *in_grh,
-			struct ib_mad *in_mad, struct ib_mad *out_mad)
+			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+			const struct ib_mad *in_mad, struct ib_mad *out_mad)
 {
-	struct mlx4_cmd_mailbox *mailbox;
+	struct mlx4_counter counter_stats;
 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
 	int err;
-	u32 inmod = dev->counters[port_num - 1] & 0xffff;
-	u8 mode;
 
 	if (in_mad->mad_hdr.mgmt_class != IB_MGMT_CLASS_PERF_MGMT)
 		return -EINVAL;
 
-	mailbox = mlx4_alloc_cmd_mailbox(dev->dev);
-	if (IS_ERR(mailbox))
-		return IB_MAD_RESULT_FAILURE;
-
-	err = mlx4_cmd_box(dev->dev, 0, mailbox->dma, inmod, 0,
-			   MLX4_CMD_QUERY_IF_STAT, MLX4_CMD_TIME_CLASS_C,
-			   MLX4_CMD_WRAPPED);
+	memset(&counter_stats, 0, sizeof(counter_stats));
+	err = mlx4_get_counter_stats(dev->dev,
+				     dev->counters[port_num - 1].index,
+				     &counter_stats, 0);
 	if (err)
 		err = IB_MAD_RESULT_FAILURE;
 	else {
 		memset(out_mad->data, 0, sizeof out_mad->data);
-		mode = ((struct mlx4_counter *)mailbox->buf)->counter_mode;
-		switch (mode & 0xf) {
+		switch (counter_stats.counter_mode & 0xf) {
 		case 0:
-			edit_counter(mailbox->buf,
-						(void *)(out_mad->data + 40));
+			edit_counter(&counter_stats,
+				     (void *)(out_mad->data + 40));
 			err = IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
 			break;
 		default:
@@ -860,19 +848,27 @@ static int iboe_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 		}
 	}
 
-	mlx4_free_cmd_mailbox(dev->dev, mailbox);
-
 	return err;
 }
 
 int mlx4_ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-			struct ib_wc *in_wc, struct ib_grh *in_grh,
-			struct ib_mad *in_mad, struct ib_mad *out_mad)
+			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+			const struct ib_mad_hdr *in, size_t in_mad_size,
+			struct ib_mad_hdr *out, size_t *out_mad_size,
+			u16 *out_mad_pkey_index)
 {
+	struct mlx4_ib_dev *dev = to_mdev(ibdev);
+	const struct ib_mad *in_mad = (const struct ib_mad *)in;
+	struct ib_mad *out_mad = (struct ib_mad *)out;
+
+	BUG_ON(in_mad_size != sizeof(*in_mad) ||
+	       *out_mad_size != sizeof(*out_mad));
+
 	switch (rdma_port_get_link_layer(ibdev, port_num)) {
 	case IB_LINK_LAYER_INFINIBAND:
-		return ib_process_mad(ibdev, mad_flags, port_num, in_wc,
-				      in_grh, in_mad, out_mad);
+		if (!mlx4_is_slave(dev->dev))
+			return ib_process_mad(ibdev, mad_flags, port_num, in_wc,
+					      in_grh, in_mad, out_mad);
 	case IB_LINK_LAYER_ETHERNET:
 		return iboe_process_mad(ibdev, mad_flags, port_num, in_wc,
 					  in_grh, in_mad, out_mad);
@@ -1365,14 +1361,17 @@ static void mlx4_ib_multiplex_mad(struct mlx4_ib_demux_pv_ctx *ctx, struct ib_wc
 	 * stadard address handle by decoding the tunnelled mlx4_ah fields */
 	memcpy(&ah.av, &tunnel->hdr.av, sizeof (struct mlx4_av));
 	ah.ibah.device = ctx->ib_dev;
+
+	port = be32_to_cpu(ah.av.ib.port_pd) >> 24;
+	port = mlx4_slave_convert_port(dev->dev, slave, port);
+	if (port < 0)
+		return;
+	ah.av.ib.port_pd = cpu_to_be32(port << 24 | (be32_to_cpu(ah.av.ib.port_pd) & 0xffffff));
+
 	mlx4_ib_query_ah(&ah.ibah, &ah_attr);
 	if (ah_attr.ah_flags & IB_AH_GRH)
 		fill_in_real_sgid_index(dev, slave, ctx->port, &ah_attr);
 
-	port = mlx4_slave_convert_port(dev->dev, slave, ah_attr.port_num);
-	if (port < 0)
-		return;
-	ah_attr.port_num = port;
 	memcpy(ah_attr.dmac, tunnel->hdr.mac, 6);
 	ah_attr.vlan_id = be16_to_cpu(tunnel->hdr.vlan);
 	/* if slave have default vlan use it */
@@ -1773,6 +1772,7 @@ static int create_pv_resources(struct ib_device *ibdev, int slave, int port,
 			       int create_tun, struct mlx4_ib_demux_pv_ctx *ctx)
 {
 	int ret, cq_size;
+	struct ib_cq_init_attr cq_attr = {};
 
 	if (ctx->state != DEMUX_PV_STATE_DOWN)
 		return -EEXIST;
@@ -1801,8 +1801,9 @@ static int create_pv_resources(struct ib_device *ibdev, int slave, int port,
 	if (ctx->has_smi)
 		cq_size *= 2;
 
+	cq_attr.cqe = cq_size;
 	ctx->cq = ib_create_cq(ctx->ib_dev, mlx4_ib_tunnel_comp_handler,
-			       NULL, ctx, cq_size, 0);
+			       NULL, ctx, &cq_attr);
 	if (IS_ERR(ctx->cq)) {
 		ret = PTR_ERR(ctx->cq);
 		pr_err("Couldn't create tunnel CQ (%d)\n", ret);
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index cc64400d41ac..067a691ecbed 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -132,14 +132,35 @@ static int num_ib_ports(struct mlx4_dev *dev)
 }
 
 static int mlx4_ib_query_device(struct ib_device *ibdev,
-				struct ib_device_attr *props)
+				struct ib_device_attr *props,
+				struct ib_udata *uhw)
 {
 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
 	struct ib_smp *in_mad  = NULL;
 	struct ib_smp *out_mad = NULL;
 	int err = -ENOMEM;
 	int have_ib_ports;
+	struct mlx4_uverbs_ex_query_device cmd;
+	struct mlx4_uverbs_ex_query_device_resp resp = {.comp_mask = 0};
+	struct mlx4_clock_params clock_params;
 
+	if (uhw->inlen) {
+		if (uhw->inlen < sizeof(cmd))
+			return -EINVAL;
+
+		err = ib_copy_from_udata(&cmd, uhw, sizeof(cmd));
+		if (err)
+			return err;
+
+		if (cmd.comp_mask)
+			return -EINVAL;
+
+		if (cmd.reserved)
+			return -EINVAL;
+	}
+
+	resp.response_length = offsetof(typeof(resp), response_length) +
+		sizeof(resp.response_length);
 	in_mad  = kzalloc(sizeof *in_mad, GFP_KERNEL);
 	out_mad = kmalloc(sizeof *out_mad, GFP_KERNEL);
 	if (!in_mad || !out_mad)
@@ -229,7 +250,24 @@ static int mlx4_ib_query_device(struct ib_device *ibdev,
 	props->max_total_mcast_qp_attach = props->max_mcast_qp_attach *
 					   props->max_mcast_grp;
 	props->max_map_per_fmr = dev->dev->caps.max_fmr_maps;
+	props->hca_core_clock = dev->dev->caps.hca_core_clock * 1000UL;
+	props->timestamp_mask = 0xFFFFFFFFFFFFULL;
+
+	err = mlx4_get_internal_clock_params(dev->dev, &clock_params);
+	if (err)
+		goto out;
+
+	if (uhw->outlen >= resp.response_length + sizeof(resp.hca_core_clock_offset)) {
+		resp.hca_core_clock_offset = clock_params.offset % PAGE_SIZE;
+		resp.response_length += sizeof(resp.hca_core_clock_offset);
+		resp.comp_mask |= QUERY_DEVICE_RESP_MASK_TIMESTAMP;
+	}
 
+	if (uhw->outlen) {
+		err = ib_copy_to_udata(uhw, &resp, resp.response_length);
+		if (err)
+			goto out;
+	}
 out:
 	kfree(in_mad);
 	kfree(out_mad);
@@ -712,8 +750,24 @@ static int mlx4_ib_mmap(struct ib_ucontext *context, struct vm_area_struct *vma)
 				       dev->dev->caps.num_uars,
 				       PAGE_SIZE, vma->vm_page_prot))
 			return -EAGAIN;
-	} else
+	} else if (vma->vm_pgoff == 3) {
+		struct mlx4_clock_params params;
+		int ret = mlx4_get_internal_clock_params(dev->dev, &params);
+
+		if (ret)
+			return ret;
+
+		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+		if (io_remap_pfn_range(vma, vma->vm_start,
+				       (pci_resource_start(dev->dev->persist->pdev,
+							   params.bar) +
+					params.offset)
+				       >> PAGE_SHIFT,
+				       PAGE_SIZE, vma->vm_page_prot))
+			return -EAGAIN;
+	} else {
 		return -EINVAL;
+	}
 
 	return 0;
 }
@@ -758,6 +812,7 @@ static struct ib_xrcd *mlx4_ib_alloc_xrcd(struct ib_device *ibdev,
 					  struct ib_udata *udata)
 {
 	struct mlx4_ib_xrcd *xrcd;
+	struct ib_cq_init_attr cq_attr = {};
 	int err;
 
 	if (!(to_mdev(ibdev)->dev->caps.flags & MLX4_DEV_CAP_FLAG_XRC))
@@ -777,7 +832,8 @@ static struct ib_xrcd *mlx4_ib_alloc_xrcd(struct ib_device *ibdev,
 		goto err2;
 	}
 
-	xrcd->cq = ib_create_cq(ibdev, NULL, NULL, xrcd, 1, 0);
+	cq_attr.cqe = 1;
+	xrcd->cq = ib_create_cq(ibdev, NULL, NULL, xrcd, &cq_attr);
 	if (IS_ERR(xrcd->cq)) {
 		err = PTR_ERR(xrcd->cq);
 		goto err3;
@@ -1090,7 +1146,7 @@ static int __mlx4_ib_create_flow(struct ib_qp *qp, struct ib_flow_attr *flow_att
 
 	ret = mlx4_cmd_imm(mdev->dev, mailbox->dma, reg_id, size >> 2, 0,
 			   MLX4_QP_FLOW_STEERING_ATTACH, MLX4_CMD_TIME_CLASS_A,
-			   MLX4_CMD_NATIVE);
+			   MLX4_CMD_WRAPPED);
 	if (ret == -ENOMEM)
 		pr_err("mcg table is full. Fail to register network rule.\n");
 	else if (ret == -ENXIO)
@@ -1107,7 +1163,7 @@ static int __mlx4_ib_destroy_flow(struct mlx4_dev *dev, u64 reg_id)
 	int err;
 	err = mlx4_cmd(dev, reg_id, 0, 0,
 		       MLX4_QP_FLOW_STEERING_DETACH, MLX4_CMD_TIME_CLASS_A,
-		       MLX4_CMD_NATIVE);
+		       MLX4_CMD_WRAPPED);
 	if (err)
 		pr_err("Fail to detach network rule. registration id = 0x%llx\n",
 		       reg_id);
@@ -1185,7 +1241,6 @@ static struct ib_flow *mlx4_ib_create_flow(struct ib_qp *qp,
 					    &mflow->reg_id[i].id);
 		if (err)
 			goto err_create_flow;
-		i++;
 		if (is_bonded) {
 			/* Application always sees one port so the mirror rule
 			 * must be on port #2
@@ -1200,6 +1255,7 @@ static struct ib_flow *mlx4_ib_create_flow(struct ib_qp *qp,
 			j++;
 		}
 
+		i++;
 	}
 
 	if (i < ARRAY_SIZE(type) && flow_attr->type == IB_FLOW_ATTR_NORMAL) {
@@ -1207,7 +1263,7 @@ static struct ib_flow *mlx4_ib_create_flow(struct ib_qp *qp,
 					       &mflow->reg_id[i].id);
 		if (err)
 			goto err_create_flow;
-		i++;
+
 		if (is_bonded) {
 			flow_attr->port = 2;
 			err = mlx4_ib_tunnel_steer_add(qp, flow_attr,
@@ -1218,6 +1274,7 @@ static struct ib_flow *mlx4_ib_create_flow(struct ib_qp *qp,
 			j++;
 		}
 		/* function to create mirror rule */
+		i++;
 	}
 
 	return &mflow->ibflow;
@@ -2041,77 +2098,75 @@ static void init_pkeys(struct mlx4_ib_dev *ibdev)
 
 static void mlx4_ib_alloc_eqs(struct mlx4_dev *dev, struct mlx4_ib_dev *ibdev)
 {
-	char name[80];
-	int eq_per_port = 0;
-	int added_eqs = 0;
-	int total_eqs = 0;
-	int i, j, eq;
-
-	/* Legacy mode or comp_pool is not large enough */
-	if (dev->caps.comp_pool == 0 ||
-	    dev->caps.num_ports > dev->caps.comp_pool)
-		return;
-
-	eq_per_port = dev->caps.comp_pool / dev->caps.num_ports;
-
-	/* Init eq table */
-	added_eqs = 0;
-	mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB)
-		added_eqs += eq_per_port;
-
-	total_eqs = dev->caps.num_comp_vectors + added_eqs;
+	int i, j, eq = 0, total_eqs = 0;
 
-	ibdev->eq_table = kzalloc(total_eqs * sizeof(int), GFP_KERNEL);
+	ibdev->eq_table = kcalloc(dev->caps.num_comp_vectors,
+				  sizeof(ibdev->eq_table[0]), GFP_KERNEL);
 	if (!ibdev->eq_table)
 		return;
 
-	ibdev->eq_added = added_eqs;
-
-	eq = 0;
-	mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB) {
-		for (j = 0; j < eq_per_port; j++) {
-			snprintf(name, sizeof(name), "mlx4-ib-%d-%d@%s",
-				 i, j, dev->persist->pdev->bus->name);
-			/* Set IRQ for specific name (per ring) */
-			if (mlx4_assign_eq(dev, name, NULL,
-					   &ibdev->eq_table[eq])) {
-				/* Use legacy (same as mlx4_en driver) */
-				pr_warn("Can't allocate EQ %d; reverting to legacy\n", eq);
-				ibdev->eq_table[eq] =
-					(eq % dev->caps.num_comp_vectors);
-			}
-			eq++;
+	for (i = 1; i <= dev->caps.num_ports; i++) {
+		for (j = 0; j < mlx4_get_eqs_per_port(dev, i);
+		     j++, total_eqs++) {
+			if (i > 1 &&  mlx4_is_eq_shared(dev, total_eqs))
+				continue;
+			ibdev->eq_table[eq] = total_eqs;
+			if (!mlx4_assign_eq(dev, i,
+					    &ibdev->eq_table[eq]))
+				eq++;
+			else
+				ibdev->eq_table[eq] = -1;
 		}
 	}
 
-	/* Fill the reset of the vector with legacy EQ */
-	for (i = 0, eq = added_eqs; i < dev->caps.num_comp_vectors; i++)
-		ibdev->eq_table[eq++] = i;
+	for (i = eq; i < dev->caps.num_comp_vectors;
+	     ibdev->eq_table[i++] = -1)
+		;
 
 	/* Advertise the new number of EQs to clients */
-	ibdev->ib_dev.num_comp_vectors = total_eqs;
+	ibdev->ib_dev.num_comp_vectors = eq;
 }
 
 static void mlx4_ib_free_eqs(struct mlx4_dev *dev, struct mlx4_ib_dev *ibdev)
 {
 	int i;
+	int total_eqs = ibdev->ib_dev.num_comp_vectors;
 
-	/* no additional eqs were added */
+	/* no eqs were allocated */
 	if (!ibdev->eq_table)
 		return;
 
 	/* Reset the advertised EQ number */
-	ibdev->ib_dev.num_comp_vectors = dev->caps.num_comp_vectors;
+	ibdev->ib_dev.num_comp_vectors = 0;
 
-	/* Free only the added eqs */
-	for (i = 0; i < ibdev->eq_added; i++) {
-		/* Don't free legacy eqs if used */
-		if (ibdev->eq_table[i] <= dev->caps.num_comp_vectors)
-			continue;
+	for (i = 0; i < total_eqs; i++)
 		mlx4_release_eq(dev, ibdev->eq_table[i]);
-	}
 
 	kfree(ibdev->eq_table);
+	ibdev->eq_table = NULL;
+}
+
+static int mlx4_port_immutable(struct ib_device *ibdev, u8 port_num,
+			       struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = mlx4_ib_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+
+	if (mlx4_ib_port_link_layer(ibdev, port_num) == IB_LINK_LAYER_INFINIBAND)
+		immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
+	else
+		immutable->core_cap_flags = RDMA_CORE_PORT_IBA_ROCE;
+
+	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
+
+	return 0;
 }
 
 static void *mlx4_ib_add(struct mlx4_dev *dev)
@@ -2123,6 +2178,8 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 	struct mlx4_ib_iboe *iboe;
 	int ib_num_ports = 0;
 	int num_req_counters;
+	int allocated;
+	u32 counter_index;
 
 	pr_info_once("%s", mlx4_ib_version);
 
@@ -2241,6 +2298,7 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 	ibdev->ib_dev.attach_mcast	= mlx4_ib_mcg_attach;
 	ibdev->ib_dev.detach_mcast	= mlx4_ib_mcg_detach;
 	ibdev->ib_dev.process_mad	= mlx4_ib_process_mad;
+	ibdev->ib_dev.get_port_immutable = mlx4_port_immutable;
 
 	if (!mlx4_is_slave(ibdev->dev)) {
 		ibdev->ib_dev.alloc_fmr		= mlx4_ib_fmr_alloc;
@@ -2278,6 +2336,10 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 			(1ull << IB_USER_VERBS_EX_CMD_DESTROY_FLOW);
 	}
 
+	ibdev->ib_dev.uverbs_ex_cmd_mask |=
+		(1ull << IB_USER_VERBS_EX_CMD_QUERY_DEVICE) |
+		(1ull << IB_USER_VERBS_EX_CMD_CREATE_CQ);
+
 	mlx4_ib_alloc_eqs(dev, ibdev);
 
 	spin_lock_init(&iboe->lock);
@@ -2288,19 +2350,31 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 	num_req_counters = mlx4_is_bonded(dev) ? 1 : ibdev->num_ports;
 	for (i = 0; i < num_req_counters; ++i) {
 		mutex_init(&ibdev->qp1_proxy_lock[i]);
+		allocated = 0;
 		if (mlx4_ib_port_link_layer(&ibdev->ib_dev, i + 1) ==
 						IB_LINK_LAYER_ETHERNET) {
-			err = mlx4_counter_alloc(ibdev->dev, &ibdev->counters[i]);
+			err = mlx4_counter_alloc(ibdev->dev, &counter_index);
+			/* if failed to allocate a new counter, use default */
 			if (err)
-				ibdev->counters[i] = -1;
-		} else {
-			ibdev->counters[i] = -1;
+				counter_index =
+					mlx4_get_default_counter_index(dev,
+								       i + 1);
+			else
+				allocated = 1;
+		} else { /* IB_LINK_LAYER_INFINIBAND use the default counter */
+			counter_index = mlx4_get_default_counter_index(dev,
+								       i + 1);
 		}
+		ibdev->counters[i].index = counter_index;
+		ibdev->counters[i].allocated = allocated;
+		pr_info("counter index %d for port %d allocated %d\n",
+			counter_index, i + 1, allocated);
 	}
 	if (mlx4_is_bonded(dev))
-		for (i = 1; i < ibdev->num_ports ; ++i)
-			ibdev->counters[i] = ibdev->counters[0];
-
+		for (i = 1; i < ibdev->num_ports ; ++i) {
+			ibdev->counters[i].index = ibdev->counters[0].index;
+			ibdev->counters[i].allocated = 0;
+		}
 
 	mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB)
 		ib_num_ports++;
@@ -2440,10 +2514,12 @@ err_steer_qp_release:
 		mlx4_qp_release_range(dev, ibdev->steer_qpn_base,
 				      ibdev->steer_qpn_count);
 err_counter:
-	for (; i; --i)
-		if (ibdev->counters[i - 1] != -1)
-			mlx4_counter_free(ibdev->dev, ibdev->counters[i - 1]);
-
+	for (i = 0; i < ibdev->num_ports; ++i) {
+		if (ibdev->counters[i].index != -1 &&
+		    ibdev->counters[i].allocated)
+			mlx4_counter_free(ibdev->dev,
+					  ibdev->counters[i].index);
+	}
 err_map:
 	iounmap(ibdev->uar_map);
 
@@ -2560,8 +2636,9 @@ static void mlx4_ib_remove(struct mlx4_dev *dev, void *ibdev_ptr)
 
 	iounmap(ibdev->uar_map);
 	for (p = 0; p < ibdev->num_ports; ++p)
-		if (ibdev->counters[p] != -1)
-			mlx4_counter_free(ibdev->dev, ibdev->counters[p]);
+		if (ibdev->counters[p].index != -1 &&
+		    ibdev->counters[p].allocated)
+			mlx4_counter_free(ibdev->dev, ibdev->counters[p].index);
 	mlx4_foreach_port(p, dev, MLX4_PORT_TYPE_IB)
 		mlx4_CLOSE_PORT(dev, p);
 
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index fce3934372a1..334387f63358 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -110,6 +110,7 @@ struct mlx4_ib_cq {
 	struct mutex		resize_mutex;
 	struct ib_umem	       *umem;
 	struct ib_umem	       *resize_umem;
+	int			create_flags;
 	/* List of qps that it serves.*/
 	struct list_head		send_qp_list;
 	struct list_head		recv_qp_list;
@@ -503,6 +504,11 @@ struct mlx4_ib_iov_port {
 	struct mlx4_ib_iov_sysfs_attr mcg_dentry;
 };
 
+struct counter_index {
+	u32		index;
+	u8		allocated;
+};
+
 struct mlx4_ib_dev {
 	struct ib_device	ib_dev;
 	struct mlx4_dev	       *dev;
@@ -521,9 +527,8 @@ struct mlx4_ib_dev {
 	struct mutex		cap_mask_mutex;
 	bool			ib_active;
 	struct mlx4_ib_iboe	iboe;
-	int			counters[MLX4_MAX_PORTS];
+	struct counter_index    counters[MLX4_MAX_PORTS];
 	int		       *eq_table;
-	int			eq_added;
 	struct kobject	       *iov_parent;
 	struct kobject	       *ports_parent;
 	struct kobject	       *dev_ports_parent[MLX4_MFUNC_MAX];
@@ -555,6 +560,21 @@ struct mlx4_ib_qp_tunnel_init_attr {
 	u8 port;
 };
 
+struct mlx4_uverbs_ex_query_device {
+	__u32 comp_mask;
+	__u32 reserved;
+};
+
+enum query_device_resp_mask {
+	QUERY_DEVICE_RESP_MASK_TIMESTAMP = 1UL << 0,
+};
+
+struct mlx4_uverbs_ex_query_device_resp {
+	__u32 comp_mask;
+	__u32 response_length;
+	__u64 hca_core_clock_offset;
+};
+
 static inline struct mlx4_ib_dev *to_mdev(struct ib_device *ibdev)
 {
 	return container_of(ibdev, struct mlx4_ib_dev, ib_dev);
@@ -668,7 +688,8 @@ void mlx4_ib_free_fast_reg_page_list(struct ib_fast_reg_page_list *page_list);
 
 int mlx4_ib_modify_cq(struct ib_cq *cq, u16 cq_count, u16 cq_period);
 int mlx4_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata);
-struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev, int entries, int vector,
+struct ib_cq *mlx4_ib_create_cq(struct ib_device *ibdev,
+				const struct ib_cq_init_attr *attr,
 				struct ib_ucontext *context,
 				struct ib_udata *udata);
 int mlx4_ib_destroy_cq(struct ib_cq *cq);
@@ -706,11 +727,13 @@ int mlx4_ib_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 		      struct ib_recv_wr **bad_wr);
 
 int mlx4_MAD_IFC(struct mlx4_ib_dev *dev, int mad_ifc_flags,
-		 int port, struct ib_wc *in_wc, struct ib_grh *in_grh,
-		 void *in_mad, void *response_mad);
+		 int port, const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		 const void *in_mad, void *response_mad);
 int mlx4_ib_process_mad(struct ib_device *ibdev, int mad_flags,	u8 port_num,
-			struct ib_wc *in_wc, struct ib_grh *in_grh,
-			struct ib_mad *in_mad, struct ib_mad *out_mad);
+			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+			const struct ib_mad_hdr *in, size_t in_mad_size,
+			struct ib_mad_hdr *out, size_t *out_mad_size,
+			u16 *out_mad_pkey_index);
 int mlx4_ib_mad_init(struct mlx4_ib_dev *dev);
 void mlx4_ib_mad_cleanup(struct mlx4_ib_dev *dev);
 
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 02fc91c68027..c5a3a5f0de41 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -1539,12 +1539,13 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 	}
 
 	if (cur_state == IB_QPS_INIT && new_state == IB_QPS_RTR) {
-		if (dev->counters[qp->port - 1] != -1) {
+		if (dev->counters[qp->port - 1].index != -1) {
 			context->pri_path.counter_index =
-						dev->counters[qp->port - 1];
+					dev->counters[qp->port - 1].index;
 			optpar |= MLX4_QP_OPTPAR_COUNTER_INDEX;
 		} else
-			context->pri_path.counter_index = 0xff;
+			context->pri_path.counter_index =
+				MLX4_SINK_COUNTER_INDEX(dev->dev);
 
 		if (qp->flags & MLX4_IB_QP_NETIF) {
 			mlx4_ib_steer_qp_reg(dev, qp, 1);
diff --git a/drivers/infiniband/hw/mlx5/Kconfig b/drivers/infiniband/hw/mlx5/Kconfig
index 10df386c6344..bce263b92821 100644
--- a/drivers/infiniband/hw/mlx5/Kconfig
+++ b/drivers/infiniband/hw/mlx5/Kconfig
@@ -1,8 +1,6 @@
 config MLX5_INFINIBAND
 	tristate "Mellanox Connect-IB HCA support"
-	depends on NETDEVICES && ETHERNET && PCI
-	select NET_VENDOR_MELLANOX
-	select MLX5_CORE
+	depends on NETDEVICES && ETHERNET && PCI && MLX5_CORE
 	---help---
 	  This driver provides low-level InfiniBand support for
 	  Mellanox Connect-IB PCI Express host channel adapters (HCAs).
diff --git a/drivers/infiniband/hw/mlx5/cq.c b/drivers/infiniband/hw/mlx5/cq.c
index 2ee6b1051975..5c9eeea62805 100644
--- a/drivers/infiniband/hw/mlx5/cq.c
+++ b/drivers/infiniband/hw/mlx5/cq.c
@@ -590,8 +590,7 @@ static int alloc_cq_buf(struct mlx5_ib_dev *dev, struct mlx5_ib_cq_buf *buf,
 {
 	int err;
 
-	err = mlx5_buf_alloc(dev->mdev, nent * cqe_size,
-			     PAGE_SIZE * 2, &buf->buf);
+	err = mlx5_buf_alloc(dev->mdev, nent * cqe_size, &buf->buf);
 	if (err)
 		return err;
 
@@ -736,10 +735,13 @@ static void destroy_cq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_cq *cq)
 	mlx5_db_free(dev->mdev, &cq->db);
 }
 
-struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev, int entries,
-				int vector, struct ib_ucontext *context,
+struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev,
+				const struct ib_cq_init_attr *attr,
+				struct ib_ucontext *context,
 				struct ib_udata *udata)
 {
+	int entries = attr->cqe;
+	int vector = attr->comp_vector;
 	struct mlx5_create_cq_mbox_in *cqb = NULL;
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
 	struct mlx5_ib_cq *cq;
@@ -750,11 +752,14 @@ struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev, int entries,
 	int eqn;
 	int err;
 
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	if (entries < 0)
 		return ERR_PTR(-EINVAL);
 
 	entries = roundup_pow_of_two(entries + 1);
-	if (entries > dev->mdev->caps.gen.max_cqes)
+	if (entries > (1 << MLX5_CAP_GEN(dev->mdev, log_max_cq_sz)))
 		return ERR_PTR(-EINVAL);
 
 	cq = kzalloc(sizeof(*cq), GFP_KERNEL);
@@ -921,7 +926,7 @@ int mlx5_ib_modify_cq(struct ib_cq *cq, u16 cq_count, u16 cq_period)
 	int err;
 	u32 fsel;
 
-	if (!(dev->mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_CQ_MODER))
+	if (!MLX5_CAP_GEN(dev->mdev, cq_moderation))
 		return -ENOSYS;
 
 	in = kzalloc(sizeof(*in), GFP_KERNEL);
@@ -1076,7 +1081,7 @@ int mlx5_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata)
 	int uninitialized_var(cqe_size);
 	unsigned long flags;
 
-	if (!(dev->mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_RESIZE_CQ)) {
+	if (!MLX5_CAP_GEN(dev->mdev, cq_resize)) {
 		pr_info("Firmware does not support resize CQ\n");
 		return -ENOSYS;
 	}
@@ -1085,7 +1090,7 @@ int mlx5_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata)
 		return -EINVAL;
 
 	entries = roundup_pow_of_two(entries + 1);
-	if (entries > dev->mdev->caps.gen.max_cqes + 1)
+	if (entries >  (1 << MLX5_CAP_GEN(dev->mdev, log_max_cq_sz)) + 1)
 		return -EINVAL;
 
 	if (entries == ibcq->cqe + 1)
diff --git a/drivers/infiniband/hw/mlx5/mad.c b/drivers/infiniband/hw/mlx5/mad.c
index 9cf9a37bb5ff..01fc97db45d6 100644
--- a/drivers/infiniband/hw/mlx5/mad.c
+++ b/drivers/infiniband/hw/mlx5/mad.c
@@ -41,8 +41,8 @@ enum {
 };
 
 int mlx5_MAD_IFC(struct mlx5_ib_dev *dev, int ignore_mkey, int ignore_bkey,
-		 u8 port, struct ib_wc *in_wc, struct ib_grh *in_grh,
-		 void *in_mad, void *response_mad)
+		 u8 port, const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		 const void *in_mad, void *response_mad)
 {
 	u8 op_modifier = 0;
 
@@ -58,11 +58,18 @@ int mlx5_MAD_IFC(struct mlx5_ib_dev *dev, int ignore_mkey, int ignore_bkey,
 }
 
 int mlx5_ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-			struct ib_wc *in_wc, struct ib_grh *in_grh,
-			struct ib_mad *in_mad, struct ib_mad *out_mad)
+			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+			const struct ib_mad_hdr *in, size_t in_mad_size,
+			struct ib_mad_hdr *out, size_t *out_mad_size,
+			u16 *out_mad_pkey_index)
 {
 	u16 slid;
 	int err;
+	const struct ib_mad *in_mad = (const struct ib_mad *)in;
+	struct ib_mad *out_mad = (struct ib_mad *)out;
+
+	BUG_ON(in_mad_size != sizeof(*in_mad) ||
+	       *out_mad_size != sizeof(*out_mad));
 
 	slid = in_wc ? in_wc->slid : be16_to_cpu(IB_LID_PERMISSIVE);
 
@@ -129,7 +136,7 @@ int mlx5_query_ext_port_caps(struct mlx5_ib_dev *dev, u8 port)
 
 	packet_error = be16_to_cpu(out_mad->status);
 
-	dev->mdev->caps.gen.ext_port_cap[port - 1] = (!err && !packet_error) ?
+	dev->mdev->port_caps[port - 1].ext_port_cap = (!err && !packet_error) ?
 		MLX_EXT_PORT_CAP_FLAG_EXTENDED_PORT_INFO : 0;
 
 out:
@@ -137,3 +144,300 @@ out:
 	kfree(out_mad);
 	return err;
 }
+
+int mlx5_query_mad_ifc_smp_attr_node_info(struct ib_device *ibdev,
+					  struct ib_smp *out_mad)
+{
+	struct ib_smp *in_mad = NULL;
+	int err = -ENOMEM;
+
+	in_mad = kzalloc(sizeof(*in_mad), GFP_KERNEL);
+	if (!in_mad)
+		return -ENOMEM;
+
+	init_query_mad(in_mad);
+	in_mad->attr_id = IB_SMP_ATTR_NODE_INFO;
+
+	err = mlx5_MAD_IFC(to_mdev(ibdev), 1, 1, 1, NULL, NULL, in_mad,
+			   out_mad);
+
+	kfree(in_mad);
+	return err;
+}
+
+int mlx5_query_mad_ifc_system_image_guid(struct ib_device *ibdev,
+					 __be64 *sys_image_guid)
+{
+	struct ib_smp *out_mad = NULL;
+	int err = -ENOMEM;
+
+	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
+	if (!out_mad)
+		return -ENOMEM;
+
+	err = mlx5_query_mad_ifc_smp_attr_node_info(ibdev, out_mad);
+	if (err)
+		goto out;
+
+	memcpy(sys_image_guid, out_mad->data + 4, 8);
+
+out:
+	kfree(out_mad);
+
+	return err;
+}
+
+int mlx5_query_mad_ifc_max_pkeys(struct ib_device *ibdev,
+				 u16 *max_pkeys)
+{
+	struct ib_smp *out_mad = NULL;
+	int err = -ENOMEM;
+
+	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
+	if (!out_mad)
+		return -ENOMEM;
+
+	err = mlx5_query_mad_ifc_smp_attr_node_info(ibdev, out_mad);
+	if (err)
+		goto out;
+
+	*max_pkeys = be16_to_cpup((__be16 *)(out_mad->data + 28));
+
+out:
+	kfree(out_mad);
+
+	return err;
+}
+
+int mlx5_query_mad_ifc_vendor_id(struct ib_device *ibdev,
+				 u32 *vendor_id)
+{
+	struct ib_smp *out_mad = NULL;
+	int err = -ENOMEM;
+
+	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
+	if (!out_mad)
+		return -ENOMEM;
+
+	err = mlx5_query_mad_ifc_smp_attr_node_info(ibdev, out_mad);
+	if (err)
+		goto out;
+
+	*vendor_id = be32_to_cpup((__be32 *)(out_mad->data + 36)) & 0xffff;
+
+out:
+	kfree(out_mad);
+
+	return err;
+}
+
+int mlx5_query_mad_ifc_node_desc(struct mlx5_ib_dev *dev, char *node_desc)
+{
+	struct ib_smp *in_mad  = NULL;
+	struct ib_smp *out_mad = NULL;
+	int err = -ENOMEM;
+
+	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
+	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
+	if (!in_mad || !out_mad)
+		goto out;
+
+	init_query_mad(in_mad);
+	in_mad->attr_id = IB_SMP_ATTR_NODE_DESC;
+
+	err = mlx5_MAD_IFC(dev, 1, 1, 1, NULL, NULL, in_mad, out_mad);
+	if (err)
+		goto out;
+
+	memcpy(node_desc, out_mad->data, 64);
+out:
+	kfree(in_mad);
+	kfree(out_mad);
+	return err;
+}
+
+int mlx5_query_mad_ifc_node_guid(struct mlx5_ib_dev *dev, __be64 *node_guid)
+{
+	struct ib_smp *in_mad  = NULL;
+	struct ib_smp *out_mad = NULL;
+	int err = -ENOMEM;
+
+	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
+	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
+	if (!in_mad || !out_mad)
+		goto out;
+
+	init_query_mad(in_mad);
+	in_mad->attr_id = IB_SMP_ATTR_NODE_INFO;
+
+	err = mlx5_MAD_IFC(dev, 1, 1, 1, NULL, NULL, in_mad, out_mad);
+	if (err)
+		goto out;
+
+	memcpy(node_guid, out_mad->data + 12, 8);
+out:
+	kfree(in_mad);
+	kfree(out_mad);
+	return err;
+}
+
+int mlx5_query_mad_ifc_pkey(struct ib_device *ibdev, u8 port, u16 index,
+			    u16 *pkey)
+{
+	struct ib_smp *in_mad  = NULL;
+	struct ib_smp *out_mad = NULL;
+	int err = -ENOMEM;
+
+	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
+	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
+	if (!in_mad || !out_mad)
+		goto out;
+
+	init_query_mad(in_mad);
+	in_mad->attr_id  = IB_SMP_ATTR_PKEY_TABLE;
+	in_mad->attr_mod = cpu_to_be32(index / 32);
+
+	err = mlx5_MAD_IFC(to_mdev(ibdev), 1, 1, port, NULL, NULL, in_mad,
+			   out_mad);
+	if (err)
+		goto out;
+
+	*pkey = be16_to_cpu(((__be16 *)out_mad->data)[index % 32]);
+
+out:
+	kfree(in_mad);
+	kfree(out_mad);
+	return err;
+}
+
+int mlx5_query_mad_ifc_gids(struct ib_device *ibdev, u8 port, int index,
+			    union ib_gid *gid)
+{
+	struct ib_smp *in_mad  = NULL;
+	struct ib_smp *out_mad = NULL;
+	int err = -ENOMEM;
+
+	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
+	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
+	if (!in_mad || !out_mad)
+		goto out;
+
+	init_query_mad(in_mad);
+	in_mad->attr_id  = IB_SMP_ATTR_PORT_INFO;
+	in_mad->attr_mod = cpu_to_be32(port);
+
+	err = mlx5_MAD_IFC(to_mdev(ibdev), 1, 1, port, NULL, NULL, in_mad,
+			   out_mad);
+	if (err)
+		goto out;
+
+	memcpy(gid->raw, out_mad->data + 8, 8);
+
+	init_query_mad(in_mad);
+	in_mad->attr_id  = IB_SMP_ATTR_GUID_INFO;
+	in_mad->attr_mod = cpu_to_be32(index / 8);
+
+	err = mlx5_MAD_IFC(to_mdev(ibdev), 1, 1, port, NULL, NULL, in_mad,
+			   out_mad);
+	if (err)
+		goto out;
+
+	memcpy(gid->raw + 8, out_mad->data + (index % 8) * 8, 8);
+
+out:
+	kfree(in_mad);
+	kfree(out_mad);
+	return err;
+}
+
+int mlx5_query_mad_ifc_port(struct ib_device *ibdev, u8 port,
+			    struct ib_port_attr *props)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	struct mlx5_core_dev *mdev = dev->mdev;
+	struct ib_smp *in_mad  = NULL;
+	struct ib_smp *out_mad = NULL;
+	int ext_active_speed;
+	int err = -ENOMEM;
+
+	if (port < 1 || port > MLX5_CAP_GEN(mdev, num_ports)) {
+		mlx5_ib_warn(dev, "invalid port number %d\n", port);
+		return -EINVAL;
+	}
+
+	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
+	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
+	if (!in_mad || !out_mad)
+		goto out;
+
+	memset(props, 0, sizeof(*props));
+
+	init_query_mad(in_mad);
+	in_mad->attr_id  = IB_SMP_ATTR_PORT_INFO;
+	in_mad->attr_mod = cpu_to_be32(port);
+
+	err = mlx5_MAD_IFC(dev, 1, 1, port, NULL, NULL, in_mad, out_mad);
+	if (err) {
+		mlx5_ib_warn(dev, "err %d\n", err);
+		goto out;
+	}
+
+	props->lid		= be16_to_cpup((__be16 *)(out_mad->data + 16));
+	props->lmc		= out_mad->data[34] & 0x7;
+	props->sm_lid		= be16_to_cpup((__be16 *)(out_mad->data + 18));
+	props->sm_sl		= out_mad->data[36] & 0xf;
+	props->state		= out_mad->data[32] & 0xf;
+	props->phys_state	= out_mad->data[33] >> 4;
+	props->port_cap_flags	= be32_to_cpup((__be32 *)(out_mad->data + 20));
+	props->gid_tbl_len	= out_mad->data[50];
+	props->max_msg_sz	= 1 << MLX5_CAP_GEN(mdev, log_max_msg);
+	props->pkey_tbl_len	= mdev->port_caps[port - 1].pkey_table_len;
+	props->bad_pkey_cntr	= be16_to_cpup((__be16 *)(out_mad->data + 46));
+	props->qkey_viol_cntr	= be16_to_cpup((__be16 *)(out_mad->data + 48));
+	props->active_width	= out_mad->data[31] & 0xf;
+	props->active_speed	= out_mad->data[35] >> 4;
+	props->max_mtu		= out_mad->data[41] & 0xf;
+	props->active_mtu	= out_mad->data[36] >> 4;
+	props->subnet_timeout	= out_mad->data[51] & 0x1f;
+	props->max_vl_num	= out_mad->data[37] >> 4;
+	props->init_type_reply	= out_mad->data[41] >> 4;
+
+	/* Check if extended speeds (EDR/FDR/...) are supported */
+	if (props->port_cap_flags & IB_PORT_EXTENDED_SPEEDS_SUP) {
+		ext_active_speed = out_mad->data[62] >> 4;
+
+		switch (ext_active_speed) {
+		case 1:
+			props->active_speed = 16; /* FDR */
+			break;
+		case 2:
+			props->active_speed = 32; /* EDR */
+			break;
+		}
+	}
+
+	/* If reported active speed is QDR, check if is FDR-10 */
+	if (props->active_speed == 4) {
+		if (mdev->port_caps[port - 1].ext_port_cap &
+		    MLX_EXT_PORT_CAP_FLAG_EXTENDED_PORT_INFO) {
+			init_query_mad(in_mad);
+			in_mad->attr_id = MLX5_ATTR_EXTENDED_PORT_INFO;
+			in_mad->attr_mod = cpu_to_be32(port);
+
+			err = mlx5_MAD_IFC(dev, 1, 1, port,
+					   NULL, NULL, in_mad, out_mad);
+			if (err)
+				goto out;
+
+			/* Checking LinkSpeedActive for FDR-10 */
+			if (out_mad->data[15] & 0x1)
+				props->active_speed = 8;
+		}
+	}
+
+out:
+	kfree(in_mad);
+	kfree(out_mad);
+
+	return err;
+}
diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index 57c9809e8b87..085c24b4b603 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -40,6 +40,7 @@
 #include <linux/io-mapping.h>
 #include <linux/sched.h>
 #include <rdma/ib_user_verbs.h>
+#include <linux/mlx5/vport.h>
 #include <rdma/ib_smi.h>
 #include <rdma/ib_umem.h>
 #include "user.h"
@@ -62,32 +63,172 @@ static char mlx5_version[] =
 	DRIVER_NAME ": Mellanox Connect-IB Infiniband driver v"
 	DRIVER_VERSION " (" DRIVER_RELDATE ")\n";
 
+static enum rdma_link_layer
+mlx5_ib_port_link_layer(struct ib_device *device)
+{
+	struct mlx5_ib_dev *dev = to_mdev(device);
+
+	switch (MLX5_CAP_GEN(dev->mdev, port_type)) {
+	case MLX5_CAP_PORT_TYPE_IB:
+		return IB_LINK_LAYER_INFINIBAND;
+	case MLX5_CAP_PORT_TYPE_ETH:
+		return IB_LINK_LAYER_ETHERNET;
+	default:
+		return IB_LINK_LAYER_UNSPECIFIED;
+	}
+}
+
+static int mlx5_use_mad_ifc(struct mlx5_ib_dev *dev)
+{
+	return !dev->mdev->issi;
+}
+
+enum {
+	MLX5_VPORT_ACCESS_METHOD_MAD,
+	MLX5_VPORT_ACCESS_METHOD_HCA,
+	MLX5_VPORT_ACCESS_METHOD_NIC,
+};
+
+static int mlx5_get_vport_access_method(struct ib_device *ibdev)
+{
+	if (mlx5_use_mad_ifc(to_mdev(ibdev)))
+		return MLX5_VPORT_ACCESS_METHOD_MAD;
+
+	if (mlx5_ib_port_link_layer(ibdev) ==
+	    IB_LINK_LAYER_ETHERNET)
+		return MLX5_VPORT_ACCESS_METHOD_NIC;
+
+	return MLX5_VPORT_ACCESS_METHOD_HCA;
+}
+
+static int mlx5_query_system_image_guid(struct ib_device *ibdev,
+					__be64 *sys_image_guid)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	struct mlx5_core_dev *mdev = dev->mdev;
+	u64 tmp;
+	int err;
+
+	switch (mlx5_get_vport_access_method(ibdev)) {
+	case MLX5_VPORT_ACCESS_METHOD_MAD:
+		return mlx5_query_mad_ifc_system_image_guid(ibdev,
+							    sys_image_guid);
+
+	case MLX5_VPORT_ACCESS_METHOD_HCA:
+		err = mlx5_query_hca_vport_system_image_guid(mdev, &tmp);
+		if (!err)
+			*sys_image_guid = cpu_to_be64(tmp);
+		return err;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mlx5_query_max_pkeys(struct ib_device *ibdev,
+				u16 *max_pkeys)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	struct mlx5_core_dev *mdev = dev->mdev;
+
+	switch (mlx5_get_vport_access_method(ibdev)) {
+	case MLX5_VPORT_ACCESS_METHOD_MAD:
+		return mlx5_query_mad_ifc_max_pkeys(ibdev, max_pkeys);
+
+	case MLX5_VPORT_ACCESS_METHOD_HCA:
+	case MLX5_VPORT_ACCESS_METHOD_NIC:
+		*max_pkeys = mlx5_to_sw_pkey_sz(MLX5_CAP_GEN(mdev,
+						pkey_table_size));
+		return 0;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mlx5_query_vendor_id(struct ib_device *ibdev,
+				u32 *vendor_id)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+
+	switch (mlx5_get_vport_access_method(ibdev)) {
+	case MLX5_VPORT_ACCESS_METHOD_MAD:
+		return mlx5_query_mad_ifc_vendor_id(ibdev, vendor_id);
+
+	case MLX5_VPORT_ACCESS_METHOD_HCA:
+	case MLX5_VPORT_ACCESS_METHOD_NIC:
+		return mlx5_core_query_vendor_id(dev->mdev, vendor_id);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int mlx5_query_node_guid(struct mlx5_ib_dev *dev,
+				__be64 *node_guid)
+{
+	u64 tmp;
+	int err;
+
+	switch (mlx5_get_vport_access_method(&dev->ib_dev)) {
+	case MLX5_VPORT_ACCESS_METHOD_MAD:
+		return mlx5_query_mad_ifc_node_guid(dev, node_guid);
+
+	case MLX5_VPORT_ACCESS_METHOD_HCA:
+		err = mlx5_query_hca_vport_node_guid(dev->mdev, &tmp);
+		if (!err)
+			*node_guid = cpu_to_be64(tmp);
+		return err;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+struct mlx5_reg_node_desc {
+	u8	desc[64];
+};
+
+static int mlx5_query_node_desc(struct mlx5_ib_dev *dev, char *node_desc)
+{
+	struct mlx5_reg_node_desc in;
+
+	if (mlx5_use_mad_ifc(dev))
+		return mlx5_query_mad_ifc_node_desc(dev, node_desc);
+
+	memset(&in, 0, sizeof(in));
+
+	return mlx5_core_access_reg(dev->mdev, &in, sizeof(in), node_desc,
+				    sizeof(struct mlx5_reg_node_desc),
+				    MLX5_REG_NODE_DESC, 0, 0);
+}
+
 static int mlx5_ib_query_device(struct ib_device *ibdev,
-				struct ib_device_attr *props)
+				struct ib_device_attr *props,
+				struct ib_udata *uhw)
 {
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
-	struct ib_smp *in_mad  = NULL;
-	struct ib_smp *out_mad = NULL;
-	struct mlx5_general_caps *gen;
+	struct mlx5_core_dev *mdev = dev->mdev;
 	int err = -ENOMEM;
 	int max_rq_sg;
 	int max_sq_sg;
-	u64 flags;
 
-	gen = &dev->mdev->caps.gen;
-	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
-	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
-	if (!in_mad || !out_mad)
-		goto out;
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
 
-	init_query_mad(in_mad);
-	in_mad->attr_id = IB_SMP_ATTR_NODE_INFO;
+	memset(props, 0, sizeof(*props));
+	err = mlx5_query_system_image_guid(ibdev,
+					   &props->sys_image_guid);
+	if (err)
+		return err;
 
-	err = mlx5_MAD_IFC(to_mdev(ibdev), 1, 1, 1, NULL, NULL, in_mad, out_mad);
+	err = mlx5_query_max_pkeys(ibdev, &props->max_pkeys);
 	if (err)
-		goto out;
+		return err;
 
-	memset(props, 0, sizeof(*props));
+	err = mlx5_query_vendor_id(ibdev, &props->vendor_id);
+	if (err)
+		return err;
 
 	props->fw_ver = ((u64)fw_rev_maj(dev->mdev) << 32) |
 		(fw_rev_min(dev->mdev) << 16) |
@@ -96,18 +237,18 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 		IB_DEVICE_PORT_ACTIVE_EVENT		|
 		IB_DEVICE_SYS_IMAGE_GUID		|
 		IB_DEVICE_RC_RNR_NAK_GEN;
-	flags = gen->flags;
-	if (flags & MLX5_DEV_CAP_FLAG_BAD_PKEY_CNTR)
+
+	if (MLX5_CAP_GEN(mdev, pkv))
 		props->device_cap_flags |= IB_DEVICE_BAD_PKEY_CNTR;
-	if (flags & MLX5_DEV_CAP_FLAG_BAD_QKEY_CNTR)
+	if (MLX5_CAP_GEN(mdev, qkv))
 		props->device_cap_flags |= IB_DEVICE_BAD_QKEY_CNTR;
-	if (flags & MLX5_DEV_CAP_FLAG_APM)
+	if (MLX5_CAP_GEN(mdev, apm))
 		props->device_cap_flags |= IB_DEVICE_AUTO_PATH_MIG;
 	props->device_cap_flags |= IB_DEVICE_LOCAL_DMA_LKEY;
-	if (flags & MLX5_DEV_CAP_FLAG_XRC)
+	if (MLX5_CAP_GEN(mdev, xrc))
 		props->device_cap_flags |= IB_DEVICE_XRC;
 	props->device_cap_flags |= IB_DEVICE_MEM_MGT_EXTENSIONS;
-	if (flags & MLX5_DEV_CAP_FLAG_SIG_HAND_OVER) {
+	if (MLX5_CAP_GEN(mdev, sho)) {
 		props->device_cap_flags |= IB_DEVICE_SIGNATURE_HANDOVER;
 		/* At this stage no support for signature handover */
 		props->sig_prot_cap = IB_PROT_T10DIF_TYPE_1 |
@@ -116,221 +257,270 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 		props->sig_guard_cap = IB_GUARD_T10DIF_CRC |
 				       IB_GUARD_T10DIF_CSUM;
 	}
-	if (flags & MLX5_DEV_CAP_FLAG_BLOCK_MCAST)
+	if (MLX5_CAP_GEN(mdev, block_lb_mc))
 		props->device_cap_flags |= IB_DEVICE_BLOCK_MULTICAST_LOOPBACK;
 
-	props->vendor_id	   = be32_to_cpup((__be32 *)(out_mad->data + 36)) &
-		0xffffff;
-	props->vendor_part_id	   = be16_to_cpup((__be16 *)(out_mad->data + 30));
-	props->hw_ver		   = be32_to_cpup((__be32 *)(out_mad->data + 32));
-	memcpy(&props->sys_image_guid, out_mad->data +	4, 8);
+	props->vendor_part_id	   = mdev->pdev->device;
+	props->hw_ver		   = mdev->pdev->revision;
 
 	props->max_mr_size	   = ~0ull;
-	props->page_size_cap	   = gen->min_page_sz;
-	props->max_qp		   = 1 << gen->log_max_qp;
-	props->max_qp_wr	   = gen->max_wqes;
-	max_rq_sg = gen->max_rq_desc_sz / sizeof(struct mlx5_wqe_data_seg);
-	max_sq_sg = (gen->max_sq_desc_sz - sizeof(struct mlx5_wqe_ctrl_seg)) /
-		sizeof(struct mlx5_wqe_data_seg);
+	props->page_size_cap	   = 1ull << MLX5_CAP_GEN(mdev, log_pg_sz);
+	props->max_qp		   = 1 << MLX5_CAP_GEN(mdev, log_max_qp);
+	props->max_qp_wr	   = 1 << MLX5_CAP_GEN(mdev, log_max_qp_sz);
+	max_rq_sg =  MLX5_CAP_GEN(mdev, max_wqe_sz_rq) /
+		     sizeof(struct mlx5_wqe_data_seg);
+	max_sq_sg = (MLX5_CAP_GEN(mdev, max_wqe_sz_sq) -
+		     sizeof(struct mlx5_wqe_ctrl_seg)) /
+		     sizeof(struct mlx5_wqe_data_seg);
 	props->max_sge = min(max_rq_sg, max_sq_sg);
-	props->max_cq		   = 1 << gen->log_max_cq;
-	props->max_cqe		   = gen->max_cqes - 1;
-	props->max_mr		   = 1 << gen->log_max_mkey;
-	props->max_pd		   = 1 << gen->log_max_pd;
-	props->max_qp_rd_atom	   = 1 << gen->log_max_ra_req_qp;
-	props->max_qp_init_rd_atom = 1 << gen->log_max_ra_res_qp;
-	props->max_srq		   = 1 << gen->log_max_srq;
-	props->max_srq_wr	   = gen->max_srq_wqes - 1;
-	props->local_ca_ack_delay  = gen->local_ca_ack_delay;
+	props->max_cq		   = 1 << MLX5_CAP_GEN(mdev, log_max_cq);
+	props->max_cqe = (1 << MLX5_CAP_GEN(mdev, log_max_eq_sz)) - 1;
+	props->max_mr		   = 1 << MLX5_CAP_GEN(mdev, log_max_mkey);
+	props->max_pd		   = 1 << MLX5_CAP_GEN(mdev, log_max_pd);
+	props->max_qp_rd_atom	   = 1 << MLX5_CAP_GEN(mdev, log_max_ra_req_qp);
+	props->max_qp_init_rd_atom = 1 << MLX5_CAP_GEN(mdev, log_max_ra_res_qp);
+	props->max_srq		   = 1 << MLX5_CAP_GEN(mdev, log_max_srq);
+	props->max_srq_wr = (1 << MLX5_CAP_GEN(mdev, log_max_srq_sz)) - 1;
+	props->local_ca_ack_delay  = MLX5_CAP_GEN(mdev, local_ca_ack_delay);
 	props->max_res_rd_atom	   = props->max_qp_rd_atom * props->max_qp;
 	props->max_srq_sge	   = max_rq_sg - 1;
 	props->max_fast_reg_page_list_len = (unsigned int)-1;
-	props->local_ca_ack_delay  = gen->local_ca_ack_delay;
 	props->atomic_cap	   = IB_ATOMIC_NONE;
 	props->masked_atomic_cap   = IB_ATOMIC_NONE;
-	props->max_pkeys	   = be16_to_cpup((__be16 *)(out_mad->data + 28));
-	props->max_mcast_grp	   = 1 << gen->log_max_mcg;
-	props->max_mcast_qp_attach = gen->max_qp_mcg;
+	props->max_mcast_grp	   = 1 << MLX5_CAP_GEN(mdev, log_max_mcg);
+	props->max_mcast_qp_attach = MLX5_CAP_GEN(mdev, max_qp_mcg);
 	props->max_total_mcast_qp_attach = props->max_mcast_qp_attach *
 					   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)
+	if (MLX5_CAP_GEN(mdev, pg))
 		props->device_cap_flags |= IB_DEVICE_ON_DEMAND_PAGING;
 	props->odp_caps = dev->odp_caps;
 #endif
 
-out:
-	kfree(in_mad);
-	kfree(out_mad);
-
-	return err;
+	return 0;
 }
 
-int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
-		       struct ib_port_attr *props)
+enum mlx5_ib_width {
+	MLX5_IB_WIDTH_1X	= 1 << 0,
+	MLX5_IB_WIDTH_2X	= 1 << 1,
+	MLX5_IB_WIDTH_4X	= 1 << 2,
+	MLX5_IB_WIDTH_8X	= 1 << 3,
+	MLX5_IB_WIDTH_12X	= 1 << 4
+};
+
+static int translate_active_width(struct ib_device *ibdev, u8 active_width,
+				  u8 *ib_width)
 {
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
-	struct ib_smp *in_mad  = NULL;
-	struct ib_smp *out_mad = NULL;
-	struct mlx5_general_caps *gen;
-	int ext_active_speed;
-	int err = -ENOMEM;
-
-	gen = &dev->mdev->caps.gen;
-	if (port < 1 || port > gen->num_ports) {
-		mlx5_ib_warn(dev, "invalid port number %d\n", port);
-		return -EINVAL;
+	int err = 0;
+
+	if (active_width & MLX5_IB_WIDTH_1X) {
+		*ib_width = IB_WIDTH_1X;
+	} else if (active_width & MLX5_IB_WIDTH_2X) {
+		mlx5_ib_dbg(dev, "active_width %d is not supported by IB spec\n",
+			    (int)active_width);
+		err = -EINVAL;
+	} else if (active_width & MLX5_IB_WIDTH_4X) {
+		*ib_width = IB_WIDTH_4X;
+	} else if (active_width & MLX5_IB_WIDTH_8X) {
+		*ib_width = IB_WIDTH_8X;
+	} else if (active_width & MLX5_IB_WIDTH_12X) {
+		*ib_width = IB_WIDTH_12X;
+	} else {
+		mlx5_ib_dbg(dev, "Invalid active_width %d\n",
+			    (int)active_width);
+		err = -EINVAL;
 	}
 
-	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
-	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
-	if (!in_mad || !out_mad)
-		goto out;
-
-	memset(props, 0, sizeof(*props));
-
-	init_query_mad(in_mad);
-	in_mad->attr_id  = IB_SMP_ATTR_PORT_INFO;
-	in_mad->attr_mod = cpu_to_be32(port);
+	return err;
+}
 
-	err = mlx5_MAD_IFC(dev, 1, 1, port, NULL, NULL, in_mad, out_mad);
-	if (err) {
-		mlx5_ib_warn(dev, "err %d\n", err);
-		goto out;
+static int mlx5_mtu_to_ib_mtu(int mtu)
+{
+	switch (mtu) {
+	case 256: return 1;
+	case 512: return 2;
+	case 1024: return 3;
+	case 2048: return 4;
+	case 4096: return 5;
+	default:
+		pr_warn("invalid mtu\n");
+		return -1;
 	}
+}
 
+enum ib_max_vl_num {
+	__IB_MAX_VL_0		= 1,
+	__IB_MAX_VL_0_1		= 2,
+	__IB_MAX_VL_0_3		= 3,
+	__IB_MAX_VL_0_7		= 4,
+	__IB_MAX_VL_0_14	= 5,
+};
 
-	props->lid		= be16_to_cpup((__be16 *)(out_mad->data + 16));
-	props->lmc		= out_mad->data[34] & 0x7;
-	props->sm_lid		= be16_to_cpup((__be16 *)(out_mad->data + 18));
-	props->sm_sl		= out_mad->data[36] & 0xf;
-	props->state		= out_mad->data[32] & 0xf;
-	props->phys_state	= out_mad->data[33] >> 4;
-	props->port_cap_flags	= be32_to_cpup((__be32 *)(out_mad->data + 20));
-	props->gid_tbl_len	= out_mad->data[50];
-	props->max_msg_sz	= 1 << gen->log_max_msg;
-	props->pkey_tbl_len	= gen->port[port - 1].pkey_table_len;
-	props->bad_pkey_cntr	= be16_to_cpup((__be16 *)(out_mad->data + 46));
-	props->qkey_viol_cntr	= be16_to_cpup((__be16 *)(out_mad->data + 48));
-	props->active_width	= out_mad->data[31] & 0xf;
-	props->active_speed	= out_mad->data[35] >> 4;
-	props->max_mtu		= out_mad->data[41] & 0xf;
-	props->active_mtu	= out_mad->data[36] >> 4;
-	props->subnet_timeout	= out_mad->data[51] & 0x1f;
-	props->max_vl_num	= out_mad->data[37] >> 4;
-	props->init_type_reply	= out_mad->data[41] >> 4;
-
-	/* Check if extended speeds (EDR/FDR/...) are supported */
-	if (props->port_cap_flags & IB_PORT_EXTENDED_SPEEDS_SUP) {
-		ext_active_speed = out_mad->data[62] >> 4;
-
-		switch (ext_active_speed) {
-		case 1:
-			props->active_speed = 16; /* FDR */
-			break;
-		case 2:
-			props->active_speed = 32; /* EDR */
-			break;
-		}
-	}
+enum mlx5_vl_hw_cap {
+	MLX5_VL_HW_0	= 1,
+	MLX5_VL_HW_0_1	= 2,
+	MLX5_VL_HW_0_2	= 3,
+	MLX5_VL_HW_0_3	= 4,
+	MLX5_VL_HW_0_4	= 5,
+	MLX5_VL_HW_0_5	= 6,
+	MLX5_VL_HW_0_6	= 7,
+	MLX5_VL_HW_0_7	= 8,
+	MLX5_VL_HW_0_14	= 15
+};
 
-	/* If reported active speed is QDR, check if is FDR-10 */
-	if (props->active_speed == 4) {
-		if (gen->ext_port_cap[port - 1] &
-		    MLX_EXT_PORT_CAP_FLAG_EXTENDED_PORT_INFO) {
-			init_query_mad(in_mad);
-			in_mad->attr_id = MLX5_ATTR_EXTENDED_PORT_INFO;
-			in_mad->attr_mod = cpu_to_be32(port);
-
-			err = mlx5_MAD_IFC(dev, 1, 1, port,
-					   NULL, NULL, in_mad, out_mad);
-			if (err)
-				goto out;
-
-			/* Checking LinkSpeedActive for FDR-10 */
-			if (out_mad->data[15] & 0x1)
-				props->active_speed = 8;
-		}
-	}
+static int translate_max_vl_num(struct ib_device *ibdev, u8 vl_hw_cap,
+				u8 *max_vl_num)
+{
+	switch (vl_hw_cap) {
+	case MLX5_VL_HW_0:
+		*max_vl_num = __IB_MAX_VL_0;
+		break;
+	case MLX5_VL_HW_0_1:
+		*max_vl_num = __IB_MAX_VL_0_1;
+		break;
+	case MLX5_VL_HW_0_3:
+		*max_vl_num = __IB_MAX_VL_0_3;
+		break;
+	case MLX5_VL_HW_0_7:
+		*max_vl_num = __IB_MAX_VL_0_7;
+		break;
+	case MLX5_VL_HW_0_14:
+		*max_vl_num = __IB_MAX_VL_0_14;
+		break;
 
-out:
-	kfree(in_mad);
-	kfree(out_mad);
+	default:
+		return -EINVAL;
+	}
 
-	return err;
+	return 0;
 }
 
-static int mlx5_ib_query_gid(struct ib_device *ibdev, u8 port, int index,
-			     union ib_gid *gid)
+static int mlx5_query_hca_port(struct ib_device *ibdev, u8 port,
+			       struct ib_port_attr *props)
 {
-	struct ib_smp *in_mad  = NULL;
-	struct ib_smp *out_mad = NULL;
-	int err = -ENOMEM;
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	struct mlx5_core_dev *mdev = dev->mdev;
+	struct mlx5_hca_vport_context *rep;
+	int max_mtu;
+	int oper_mtu;
+	int err;
+	u8 ib_link_width_oper;
+	u8 vl_hw_cap;
 
-	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
-	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
-	if (!in_mad || !out_mad)
+	rep = kzalloc(sizeof(*rep), GFP_KERNEL);
+	if (!rep) {
+		err = -ENOMEM;
 		goto out;
+	}
 
-	init_query_mad(in_mad);
-	in_mad->attr_id  = IB_SMP_ATTR_PORT_INFO;
-	in_mad->attr_mod = cpu_to_be32(port);
+	memset(props, 0, sizeof(*props));
 
-	err = mlx5_MAD_IFC(to_mdev(ibdev), 1, 1, port, NULL, NULL, in_mad, out_mad);
+	err = mlx5_query_hca_vport_context(mdev, 0, port, 0, rep);
 	if (err)
 		goto out;
 
-	memcpy(gid->raw, out_mad->data + 8, 8);
-
-	init_query_mad(in_mad);
-	in_mad->attr_id  = IB_SMP_ATTR_GUID_INFO;
-	in_mad->attr_mod = cpu_to_be32(index / 8);
+	props->lid		= rep->lid;
+	props->lmc		= rep->lmc;
+	props->sm_lid		= rep->sm_lid;
+	props->sm_sl		= rep->sm_sl;
+	props->state		= rep->vport_state;
+	props->phys_state	= rep->port_physical_state;
+	props->port_cap_flags	= rep->cap_mask1;
+	props->gid_tbl_len	= mlx5_get_gid_table_len(MLX5_CAP_GEN(mdev, gid_table_size));
+	props->max_msg_sz	= 1 << MLX5_CAP_GEN(mdev, log_max_msg);
+	props->pkey_tbl_len	= mlx5_to_sw_pkey_sz(MLX5_CAP_GEN(mdev, pkey_table_size));
+	props->bad_pkey_cntr	= rep->pkey_violation_counter;
+	props->qkey_viol_cntr	= rep->qkey_violation_counter;
+	props->subnet_timeout	= rep->subnet_timeout;
+	props->init_type_reply	= rep->init_type_reply;
+
+	err = mlx5_query_port_link_width_oper(mdev, &ib_link_width_oper, port);
+	if (err)
+		goto out;
 
-	err = mlx5_MAD_IFC(to_mdev(ibdev), 1, 1, port, NULL, NULL, in_mad, out_mad);
+	err = translate_active_width(ibdev, ib_link_width_oper,
+				     &props->active_width);
+	if (err)
+		goto out;
+	err = mlx5_query_port_proto_oper(mdev, &props->active_speed, MLX5_PTYS_IB,
+					 port);
 	if (err)
 		goto out;
 
-	memcpy(gid->raw + 8, out_mad->data + (index % 8) * 8, 8);
+	mlx5_query_port_max_mtu(mdev, &max_mtu, port);
+
+	props->max_mtu = mlx5_mtu_to_ib_mtu(max_mtu);
+
+	mlx5_query_port_oper_mtu(mdev, &oper_mtu, port);
+
+	props->active_mtu = mlx5_mtu_to_ib_mtu(oper_mtu);
 
+	err = mlx5_query_port_vl_hw_cap(mdev, &vl_hw_cap, port);
+	if (err)
+		goto out;
+
+	err = translate_max_vl_num(ibdev, vl_hw_cap,
+				   &props->max_vl_num);
 out:
-	kfree(in_mad);
-	kfree(out_mad);
+	kfree(rep);
 	return err;
 }
 
-static int mlx5_ib_query_pkey(struct ib_device *ibdev, u8 port, u16 index,
-			      u16 *pkey)
+int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
+		       struct ib_port_attr *props)
 {
-	struct ib_smp *in_mad  = NULL;
-	struct ib_smp *out_mad = NULL;
-	int err = -ENOMEM;
+	switch (mlx5_get_vport_access_method(ibdev)) {
+	case MLX5_VPORT_ACCESS_METHOD_MAD:
+		return mlx5_query_mad_ifc_port(ibdev, port, props);
 
-	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
-	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
-	if (!in_mad || !out_mad)
-		goto out;
+	case MLX5_VPORT_ACCESS_METHOD_HCA:
+		return mlx5_query_hca_port(ibdev, port, props);
 
-	init_query_mad(in_mad);
-	in_mad->attr_id  = IB_SMP_ATTR_PKEY_TABLE;
-	in_mad->attr_mod = cpu_to_be32(index / 32);
+	default:
+		return -EINVAL;
+	}
+}
 
-	err = mlx5_MAD_IFC(to_mdev(ibdev), 1, 1, port, NULL, NULL, in_mad, out_mad);
-	if (err)
-		goto out;
+static int mlx5_ib_query_gid(struct ib_device *ibdev, u8 port, int index,
+			     union ib_gid *gid)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	struct mlx5_core_dev *mdev = dev->mdev;
 
-	*pkey = be16_to_cpu(((__be16 *)out_mad->data)[index % 32]);
+	switch (mlx5_get_vport_access_method(ibdev)) {
+	case MLX5_VPORT_ACCESS_METHOD_MAD:
+		return mlx5_query_mad_ifc_gids(ibdev, port, index, gid);
+
+	case MLX5_VPORT_ACCESS_METHOD_HCA:
+		return mlx5_query_hca_vport_gid(mdev, 0, port, 0, index, gid);
+
+	default:
+		return -EINVAL;
+	}
 
-out:
-	kfree(in_mad);
-	kfree(out_mad);
-	return err;
 }
 
-struct mlx5_reg_node_desc {
-	u8	desc[64];
-};
+static int mlx5_ib_query_pkey(struct ib_device *ibdev, u8 port, u16 index,
+			      u16 *pkey)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	struct mlx5_core_dev *mdev = dev->mdev;
+
+	switch (mlx5_get_vport_access_method(ibdev)) {
+	case MLX5_VPORT_ACCESS_METHOD_MAD:
+		return mlx5_query_mad_ifc_pkey(ibdev, port, index, pkey);
+
+	case MLX5_VPORT_ACCESS_METHOD_HCA:
+	case MLX5_VPORT_ACCESS_METHOD_NIC:
+		return mlx5_query_hca_vport_pkey(mdev, 0, port,  0, index,
+						 pkey);
+	default:
+		return -EINVAL;
+	}
+}
 
 static int mlx5_ib_modify_device(struct ib_device *ibdev, int mask,
 				 struct ib_device_modify *props)
@@ -392,7 +582,6 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	struct mlx5_ib_alloc_ucontext_req_v2 req;
 	struct mlx5_ib_alloc_ucontext_resp resp;
 	struct mlx5_ib_ucontext *context;
-	struct mlx5_general_caps *gen;
 	struct mlx5_uuar_info *uuari;
 	struct mlx5_uar *uars;
 	int gross_uuars;
@@ -403,7 +592,6 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	int i;
 	size_t reqlen;
 
-	gen = &dev->mdev->caps.gen;
 	if (!dev->ib_active)
 		return ERR_PTR(-EAGAIN);
 
@@ -436,14 +624,14 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 
 	num_uars = req.total_num_uuars / MLX5_NON_FP_BF_REGS_PER_PAGE;
 	gross_uuars = num_uars * MLX5_BF_REGS_PER_PAGE;
-	resp.qp_tab_size      = 1 << gen->log_max_qp;
-	resp.bf_reg_size      = gen->bf_reg_size;
-	resp.cache_line_size  = L1_CACHE_BYTES;
-	resp.max_sq_desc_sz = gen->max_sq_desc_sz;
-	resp.max_rq_desc_sz = gen->max_rq_desc_sz;
-	resp.max_send_wqebb = gen->max_wqes;
-	resp.max_recv_wr = gen->max_wqes;
-	resp.max_srq_recv_wr = gen->max_srq_wqes;
+	resp.qp_tab_size = 1 << MLX5_CAP_GEN(dev->mdev, log_max_qp);
+	resp.bf_reg_size = 1 << MLX5_CAP_GEN(dev->mdev, log_bf_reg_size);
+	resp.cache_line_size = L1_CACHE_BYTES;
+	resp.max_sq_desc_sz = MLX5_CAP_GEN(dev->mdev, max_wqe_sz_sq);
+	resp.max_rq_desc_sz = MLX5_CAP_GEN(dev->mdev, max_wqe_sz_rq);
+	resp.max_send_wqebb = 1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz);
+	resp.max_recv_wr = 1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz);
+	resp.max_srq_recv_wr = 1 << MLX5_CAP_GEN(dev->mdev, log_max_srq_sz);
 
 	context = kzalloc(sizeof(*context), GFP_KERNEL);
 	if (!context)
@@ -493,7 +681,7 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	mutex_init(&context->db_page_mutex);
 
 	resp.tot_uuars = req.total_num_uuars;
-	resp.num_ports = gen->num_ports;
+	resp.num_ports = MLX5_CAP_GEN(dev->mdev, num_ports);
 	err = ib_copy_to_udata(udata, &resp,
 			       sizeof(resp) - sizeof(resp.reserved));
 	if (err)
@@ -731,37 +919,15 @@ static int mlx5_ib_mcg_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 
 static int init_node_data(struct mlx5_ib_dev *dev)
 {
-	struct ib_smp *in_mad  = NULL;
-	struct ib_smp *out_mad = NULL;
-	int err = -ENOMEM;
-
-	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
-	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
-	if (!in_mad || !out_mad)
-		goto out;
-
-	init_query_mad(in_mad);
-	in_mad->attr_id = IB_SMP_ATTR_NODE_DESC;
-
-	err = mlx5_MAD_IFC(dev, 1, 1, 1, NULL, NULL, in_mad, out_mad);
-	if (err)
-		goto out;
-
-	memcpy(dev->ib_dev.node_desc, out_mad->data, 64);
-
-	in_mad->attr_id = IB_SMP_ATTR_NODE_INFO;
+	int err;
 
-	err = mlx5_MAD_IFC(dev, 1, 1, 1, NULL, NULL, in_mad, out_mad);
+	err = mlx5_query_node_desc(dev, dev->ib_dev.node_desc);
 	if (err)
-		goto out;
+		return err;
 
-	dev->mdev->rev_id = be32_to_cpup((__be32 *)(out_mad->data + 32));
-	memcpy(&dev->ib_dev.node_guid, out_mad->data + 12, 8);
+	dev->mdev->rev_id = dev->mdev->pdev->revision;
 
-out:
-	kfree(in_mad);
-	kfree(out_mad);
-	return err;
+	return mlx5_query_node_guid(dev, &dev->ib_dev.node_guid);
 }
 
 static ssize_t show_fw_pages(struct device *device, struct device_attribute *attr,
@@ -895,11 +1061,9 @@ static void mlx5_ib_event(struct mlx5_core_dev *dev, void *context,
 
 static void get_ext_port_caps(struct mlx5_ib_dev *dev)
 {
-	struct mlx5_general_caps *gen;
 	int port;
 
-	gen = &dev->mdev->caps.gen;
-	for (port = 1; port <= gen->num_ports; port++)
+	for (port = 1; port <= MLX5_CAP_GEN(dev->mdev, num_ports); port++)
 		mlx5_query_ext_port_caps(dev, port);
 }
 
@@ -907,11 +1071,10 @@ static int get_port_caps(struct mlx5_ib_dev *dev)
 {
 	struct ib_device_attr *dprops = NULL;
 	struct ib_port_attr *pprops = NULL;
-	struct mlx5_general_caps *gen;
 	int err = -ENOMEM;
 	int port;
+	struct ib_udata uhw = {.inlen = 0, .outlen = 0};
 
-	gen = &dev->mdev->caps.gen;
 	pprops = kmalloc(sizeof(*pprops), GFP_KERNEL);
 	if (!pprops)
 		goto out;
@@ -920,20 +1083,23 @@ static int get_port_caps(struct mlx5_ib_dev *dev)
 	if (!dprops)
 		goto out;
 
-	err = mlx5_ib_query_device(&dev->ib_dev, dprops);
+	err = mlx5_ib_query_device(&dev->ib_dev, dprops, &uhw);
 	if (err) {
 		mlx5_ib_warn(dev, "query_device failed %d\n", err);
 		goto out;
 	}
 
-	for (port = 1; port <= gen->num_ports; port++) {
+	for (port = 1; port <= MLX5_CAP_GEN(dev->mdev, num_ports); port++) {
 		err = mlx5_ib_query_port(&dev->ib_dev, port, pprops);
 		if (err) {
-			mlx5_ib_warn(dev, "query_port %d failed %d\n", port, err);
+			mlx5_ib_warn(dev, "query_port %d failed %d\n",
+				     port, err);
 			break;
 		}
-		gen->port[port - 1].pkey_table_len = dprops->max_pkeys;
-		gen->port[port - 1].gid_table_len = pprops->gid_tbl_len;
+		dev->mdev->port_caps[port - 1].pkey_table_len =
+						dprops->max_pkeys;
+		dev->mdev->port_caps[port - 1].gid_table_len =
+						pprops->gid_tbl_len;
 		mlx5_ib_dbg(dev, "pkey_table_len %d, gid_table_len %d\n",
 			    dprops->max_pkeys, pprops->gid_tbl_len);
 	}
@@ -971,6 +1137,7 @@ static int create_umr_res(struct mlx5_ib_dev *dev)
 	struct ib_cq *cq;
 	struct ib_qp *qp;
 	struct ib_mr *mr;
+	struct ib_cq_init_attr cq_attr = {};
 	int ret;
 
 	attr = kzalloc(sizeof(*attr), GFP_KERNEL);
@@ -994,8 +1161,9 @@ static int create_umr_res(struct mlx5_ib_dev *dev)
 		goto error_1;
 	}
 
-	cq = ib_create_cq(&dev->ib_dev, mlx5_umr_cq_handler, NULL, NULL, 128,
-			  0);
+	cq_attr.cqe = 128;
+	cq = ib_create_cq(&dev->ib_dev, mlx5_umr_cq_handler, NULL, NULL,
+			  &cq_attr);
 	if (IS_ERR(cq)) {
 		mlx5_ib_dbg(dev, "Couldn't create CQ for sync UMR QP\n");
 		ret = PTR_ERR(cq);
@@ -1087,6 +1255,7 @@ static int create_dev_resources(struct mlx5_ib_resources *devr)
 {
 	struct ib_srq_init_attr attr;
 	struct mlx5_ib_dev *dev;
+	struct ib_cq_init_attr cq_attr = {.cqe = 1};
 	int ret = 0;
 
 	dev = container_of(devr, struct mlx5_ib_dev, devr);
@@ -1100,7 +1269,7 @@ static int create_dev_resources(struct mlx5_ib_resources *devr)
 	devr->p0->uobject = NULL;
 	atomic_set(&devr->p0->usecnt, 0);
 
-	devr->c0 = mlx5_ib_create_cq(&dev->ib_dev, 1, 0, NULL, NULL);
+	devr->c0 = mlx5_ib_create_cq(&dev->ib_dev, &cq_attr, NULL, NULL);
 	if (IS_ERR(devr->c0)) {
 		ret = PTR_ERR(devr->c0);
 		goto error1;
@@ -1159,8 +1328,29 @@ static int create_dev_resources(struct mlx5_ib_resources *devr)
 	atomic_inc(&devr->p0->usecnt);
 	atomic_set(&devr->s0->usecnt, 0);
 
+	memset(&attr, 0, sizeof(attr));
+	attr.attr.max_sge = 1;
+	attr.attr.max_wr = 1;
+	attr.srq_type = IB_SRQT_BASIC;
+	devr->s1 = mlx5_ib_create_srq(devr->p0, &attr, NULL);
+	if (IS_ERR(devr->s1)) {
+		ret = PTR_ERR(devr->s1);
+		goto error5;
+	}
+	devr->s1->device	= &dev->ib_dev;
+	devr->s1->pd		= devr->p0;
+	devr->s1->uobject       = NULL;
+	devr->s1->event_handler = NULL;
+	devr->s1->srq_context   = NULL;
+	devr->s1->srq_type      = IB_SRQT_BASIC;
+	devr->s1->ext.xrc.cq	= devr->c0;
+	atomic_inc(&devr->p0->usecnt);
+	atomic_set(&devr->s0->usecnt, 0);
+
 	return 0;
 
+error5:
+	mlx5_ib_destroy_srq(devr->s0);
 error4:
 	mlx5_ib_dealloc_xrcd(devr->x1);
 error3:
@@ -1175,6 +1365,7 @@ error0:
 
 static void destroy_dev_resources(struct mlx5_ib_resources *devr)
 {
+	mlx5_ib_destroy_srq(devr->s1);
 	mlx5_ib_destroy_srq(devr->s0);
 	mlx5_ib_dealloc_xrcd(devr->x0);
 	mlx5_ib_dealloc_xrcd(devr->x1);
@@ -1182,12 +1373,34 @@ static void destroy_dev_resources(struct mlx5_ib_resources *devr)
 	mlx5_ib_dealloc_pd(devr->p0);
 }
 
+static int mlx5_port_immutable(struct ib_device *ibdev, u8 port_num,
+			       struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = mlx5_ib_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
+	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
+
+	return 0;
+}
+
 static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 {
 	struct mlx5_ib_dev *dev;
 	int err;
 	int i;
 
+	/* don't create IB instance over Eth ports, no RoCE yet! */
+	if (MLX5_CAP_GEN(mdev, port_type) == MLX5_CAP_PORT_TYPE_ETH)
+		return NULL;
+
 	printk_once(KERN_INFO "%s", mlx5_version);
 
 	dev = (struct mlx5_ib_dev *)ib_alloc_device(sizeof(*dev));
@@ -1200,15 +1413,16 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 	if (err)
 		goto err_dealloc;
 
-	get_ext_port_caps(dev);
+	if (mlx5_use_mad_ifc(dev))
+		get_ext_port_caps(dev);
 
 	MLX5_INIT_DOORBELL_LOCK(&dev->uar_lock);
 
 	strlcpy(dev->ib_dev.name, "mlx5_%d", IB_DEVICE_NAME_MAX);
 	dev->ib_dev.owner		= THIS_MODULE;
 	dev->ib_dev.node_type		= RDMA_NODE_IB_CA;
-	dev->ib_dev.local_dma_lkey	= mdev->caps.gen.reserved_lkey;
-	dev->num_ports		= mdev->caps.gen.num_ports;
+	dev->ib_dev.local_dma_lkey	= 0 /* not supported for now */;
+	dev->num_ports		= MLX5_CAP_GEN(mdev, num_ports);
 	dev->ib_dev.phys_port_cnt     = dev->num_ports;
 	dev->ib_dev.num_comp_vectors    =
 		dev->mdev->priv.eq_table.num_comp_vectors;
@@ -1285,10 +1499,11 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 	dev->ib_dev.alloc_fast_reg_page_list = mlx5_ib_alloc_fast_reg_page_list;
 	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;
+	dev->ib_dev.get_port_immutable  = mlx5_port_immutable;
 
-	mlx5_ib_internal_query_odp_caps(dev);
+	mlx5_ib_internal_fill_odp_caps(dev);
 
-	if (mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_XRC) {
+	if (MLX5_CAP_GEN(mdev, xrc)) {
 		dev->ib_dev.alloc_xrcd = mlx5_ib_alloc_xrcd;
 		dev->ib_dev.dealloc_xrcd = mlx5_ib_dealloc_xrcd;
 		dev->ib_dev.uverbs_cmd_mask |=
diff --git a/drivers/infiniband/hw/mlx5/mlx5_ib.h b/drivers/infiniband/hw/mlx5/mlx5_ib.h
index dff1cfcdf476..7cae09836481 100644
--- a/drivers/infiniband/hw/mlx5/mlx5_ib.h
+++ b/drivers/infiniband/hw/mlx5/mlx5_ib.h
@@ -415,6 +415,7 @@ struct mlx5_ib_resources {
 	struct ib_xrcd	*x1;
 	struct ib_pd	*p0;
 	struct ib_srq	*s0;
+	struct ib_srq	*s1;
 };
 
 struct mlx5_ib_dev {
@@ -525,8 +526,8 @@ void __mlx5_ib_cq_clean(struct mlx5_ib_cq *cq, u32 qpn, struct mlx5_ib_srq *srq)
 void mlx5_ib_cq_clean(struct mlx5_ib_cq *cq, u32 qpn, struct mlx5_ib_srq *srq);
 void mlx5_ib_free_srq_wqe(struct mlx5_ib_srq *srq, int wqe_index);
 int mlx5_MAD_IFC(struct mlx5_ib_dev *dev, int ignore_mkey, int ignore_bkey,
-		 u8 port, struct ib_wc *in_wc, struct ib_grh *in_grh,
-		 void *in_mad, void *response_mad);
+		 u8 port, const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		 const void *in_mad, void *response_mad);
 struct ib_ah *create_ib_ah(struct ib_ah_attr *ah_attr,
 			   struct mlx5_ib_ah *ah);
 struct ib_ah *mlx5_ib_create_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr);
@@ -556,8 +557,9 @@ int mlx5_ib_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *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_cq *mlx5_ib_create_cq(struct ib_device *ibdev,
+				const struct ib_cq_init_attr *attr,
+				struct ib_ucontext *context,
 				struct ib_udata *udata);
 int mlx5_ib_destroy_cq(struct ib_cq *cq);
 int mlx5_ib_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc);
@@ -586,14 +588,32 @@ int mlx5_ib_map_phys_fmr(struct ib_fmr *ibfmr, u64 *page_list,
 int mlx5_ib_unmap_fmr(struct list_head *fmr_list);
 int mlx5_ib_fmr_dealloc(struct ib_fmr *ibfmr);
 int mlx5_ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-			struct ib_wc *in_wc, struct ib_grh *in_grh,
-			struct ib_mad *in_mad, struct ib_mad *out_mad);
+			const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+			const struct ib_mad_hdr *in, size_t in_mad_size,
+			struct ib_mad_hdr *out, size_t *out_mad_size,
+			u16 *out_mad_pkey_index);
 struct ib_xrcd *mlx5_ib_alloc_xrcd(struct ib_device *ibdev,
 					  struct ib_ucontext *context,
 					  struct ib_udata *udata);
 int mlx5_ib_dealloc_xrcd(struct ib_xrcd *xrcd);
 int mlx5_ib_get_buf_offset(u64 addr, int page_shift, u32 *offset);
 int mlx5_query_ext_port_caps(struct mlx5_ib_dev *dev, u8 port);
+int mlx5_query_mad_ifc_smp_attr_node_info(struct ib_device *ibdev,
+					  struct ib_smp *out_mad);
+int mlx5_query_mad_ifc_system_image_guid(struct ib_device *ibdev,
+					 __be64 *sys_image_guid);
+int mlx5_query_mad_ifc_max_pkeys(struct ib_device *ibdev,
+				 u16 *max_pkeys);
+int mlx5_query_mad_ifc_vendor_id(struct ib_device *ibdev,
+				 u32 *vendor_id);
+int mlx5_query_mad_ifc_node_desc(struct mlx5_ib_dev *dev, char *node_desc);
+int mlx5_query_mad_ifc_node_guid(struct mlx5_ib_dev *dev, __be64 *node_guid);
+int mlx5_query_mad_ifc_pkey(struct ib_device *ibdev, u8 port, u16 index,
+			    u16 *pkey);
+int mlx5_query_mad_ifc_gids(struct ib_device *ibdev, u8 port, int index,
+			    union ib_gid *gid);
+int mlx5_query_mad_ifc_port(struct ib_device *ibdev, u8 port,
+			    struct ib_port_attr *props);
 int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
 		       struct ib_port_attr *props);
 int mlx5_ib_init_fmr(struct mlx5_ib_dev *dev);
@@ -617,7 +637,7 @@ int mlx5_ib_check_mr_status(struct ib_mr *ibmr, u32 check_mask,
 #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_internal_fill_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);
@@ -631,9 +651,9 @@ 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)
+static inline void mlx5_ib_internal_fill_odp_caps(struct mlx5_ib_dev *dev)
 {
-	return 0;
+	return;
 }
 
 static inline void mlx5_ib_odp_create_qp(struct mlx5_ib_qp *qp)		{}
diff --git a/drivers/infiniband/hw/mlx5/mr.c b/drivers/infiniband/hw/mlx5/mr.c
index 71c593583864..bc9a0de897cb 100644
--- a/drivers/infiniband/hw/mlx5/mr.c
+++ b/drivers/infiniband/hw/mlx5/mr.c
@@ -975,8 +975,7 @@ 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);
+	bool pg_cap = !!(MLX5_CAP_GEN(dev->mdev, pg));
 
 	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
 	if (!mr)
diff --git a/drivers/infiniband/hw/mlx5/odp.c b/drivers/infiniband/hw/mlx5/odp.c
index 5099db08afd2..aa8391e75385 100644
--- a/drivers/infiniband/hw/mlx5/odp.c
+++ b/drivers/infiniband/hw/mlx5/odp.c
@@ -109,40 +109,33 @@ void mlx5_ib_invalidate_range(struct ib_umem *umem, unsigned long start,
 	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)
+void mlx5_ib_internal_fill_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;
+	if (!MLX5_CAP_GEN(dev->mdev, pg))
+		return;
 
 	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;
+
+	if (MLX5_CAP_ODP(dev->mdev, ud_odp_caps.send))
+		caps->per_transport_caps.ud_odp_caps |= IB_ODP_SUPPORT_SEND;
+
+	if (MLX5_CAP_ODP(dev->mdev, rc_odp_caps.send))
+		caps->per_transport_caps.rc_odp_caps |= IB_ODP_SUPPORT_SEND;
+
+	if (MLX5_CAP_ODP(dev->mdev, rc_odp_caps.receive))
+		caps->per_transport_caps.rc_odp_caps |= IB_ODP_SUPPORT_RECV;
+
+	if (MLX5_CAP_ODP(dev->mdev, rc_odp_caps.write))
+		caps->per_transport_caps.rc_odp_caps |= IB_ODP_SUPPORT_WRITE;
+
+	if (MLX5_CAP_ODP(dev->mdev, rc_odp_caps.read))
+		caps->per_transport_caps.rc_odp_caps |= IB_ODP_SUPPORT_READ;
+
+	return;
 }
 
 static struct mlx5_ib_mr *mlx5_ib_odp_find_mr_lkey(struct mlx5_ib_dev *dev,
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index d35f62d4f4c5..203c8a45e095 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -220,13 +220,11 @@ static void mlx5_ib_qp_event(struct mlx5_core_qp *qp, int type)
 static int set_rq_size(struct mlx5_ib_dev *dev, struct ib_qp_cap *cap,
 		       int has_rq, struct mlx5_ib_qp *qp, struct mlx5_ib_create_qp *ucmd)
 {
-	struct mlx5_general_caps *gen;
 	int wqe_size;
 	int wq_size;
 
-	gen = &dev->mdev->caps.gen;
 	/* Sanity check RQ size before proceeding */
-	if (cap->max_recv_wr  > gen->max_wqes)
+	if (cap->max_recv_wr > (1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz)))
 		return -EINVAL;
 
 	if (!has_rq) {
@@ -246,10 +244,11 @@ static int set_rq_size(struct mlx5_ib_dev *dev, struct ib_qp_cap *cap,
 			wq_size = roundup_pow_of_two(cap->max_recv_wr) * wqe_size;
 			wq_size = max_t(int, wq_size, MLX5_SEND_WQE_BB);
 			qp->rq.wqe_cnt = wq_size / wqe_size;
-			if (wqe_size > gen->max_rq_desc_sz) {
+			if (wqe_size > MLX5_CAP_GEN(dev->mdev, max_wqe_sz_rq)) {
 				mlx5_ib_dbg(dev, "wqe_size %d, max %d\n",
 					    wqe_size,
-					    gen->max_rq_desc_sz);
+					    MLX5_CAP_GEN(dev->mdev,
+							 max_wqe_sz_rq));
 				return -EINVAL;
 			}
 			qp->rq.wqe_shift = ilog2(wqe_size);
@@ -330,11 +329,9 @@ static int calc_send_wqe(struct ib_qp_init_attr *attr)
 static int calc_sq_size(struct mlx5_ib_dev *dev, struct ib_qp_init_attr *attr,
 			struct mlx5_ib_qp *qp)
 {
-	struct mlx5_general_caps *gen;
 	int wqe_size;
 	int wq_size;
 
-	gen = &dev->mdev->caps.gen;
 	if (!attr->cap.max_send_wr)
 		return 0;
 
@@ -343,9 +340,9 @@ static int calc_sq_size(struct mlx5_ib_dev *dev, struct ib_qp_init_attr *attr,
 	if (wqe_size < 0)
 		return wqe_size;
 
-	if (wqe_size > gen->max_sq_desc_sz) {
+	if (wqe_size > MLX5_CAP_GEN(dev->mdev, max_wqe_sz_sq)) {
 		mlx5_ib_dbg(dev, "wqe_size(%d) > max_sq_desc_sz(%d)\n",
-			    wqe_size, gen->max_sq_desc_sz);
+			    wqe_size, MLX5_CAP_GEN(dev->mdev, max_wqe_sz_sq));
 		return -EINVAL;
 	}
 
@@ -358,9 +355,10 @@ static int calc_sq_size(struct mlx5_ib_dev *dev, struct ib_qp_init_attr *attr,
 
 	wq_size = roundup_pow_of_two(attr->cap.max_send_wr * wqe_size);
 	qp->sq.wqe_cnt = wq_size / MLX5_SEND_WQE_BB;
-	if (qp->sq.wqe_cnt > gen->max_wqes) {
+	if (qp->sq.wqe_cnt > (1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz))) {
 		mlx5_ib_dbg(dev, "wqe count(%d) exceeds limits(%d)\n",
-			    qp->sq.wqe_cnt, gen->max_wqes);
+			    qp->sq.wqe_cnt,
+			    1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz));
 		return -ENOMEM;
 	}
 	qp->sq.wqe_shift = ilog2(MLX5_SEND_WQE_BB);
@@ -375,13 +373,11 @@ static int set_user_buf_size(struct mlx5_ib_dev *dev,
 			    struct mlx5_ib_qp *qp,
 			    struct mlx5_ib_create_qp *ucmd)
 {
-	struct mlx5_general_caps *gen;
 	int desc_sz = 1 << qp->sq.wqe_shift;
 
-	gen = &dev->mdev->caps.gen;
-	if (desc_sz > gen->max_sq_desc_sz) {
+	if (desc_sz > MLX5_CAP_GEN(dev->mdev, max_wqe_sz_sq)) {
 		mlx5_ib_warn(dev, "desc_sz %d, max_sq_desc_sz %d\n",
-			     desc_sz, gen->max_sq_desc_sz);
+			     desc_sz, MLX5_CAP_GEN(dev->mdev, max_wqe_sz_sq));
 		return -EINVAL;
 	}
 
@@ -393,9 +389,10 @@ static int set_user_buf_size(struct mlx5_ib_dev *dev,
 
 	qp->sq.wqe_cnt = ucmd->sq_wqe_count;
 
-	if (qp->sq.wqe_cnt > gen->max_wqes) {
+	if (qp->sq.wqe_cnt > (1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz))) {
 		mlx5_ib_warn(dev, "wqe_cnt %d, max_wqes %d\n",
-			     qp->sq.wqe_cnt, gen->max_wqes);
+			     qp->sq.wqe_cnt,
+			     1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz));
 		return -EINVAL;
 	}
 
@@ -768,7 +765,7 @@ static int create_kernel_qp(struct mlx5_ib_dev *dev,
 	qp->sq.offset = qp->rq.wqe_cnt << qp->rq.wqe_shift;
 	qp->buf_size = err + (qp->rq.wqe_cnt << qp->rq.wqe_shift);
 
-	err = mlx5_buf_alloc(dev->mdev, qp->buf_size, PAGE_SIZE * 2, &qp->buf);
+	err = mlx5_buf_alloc(dev->mdev, qp->buf_size, &qp->buf);
 	if (err) {
 		mlx5_ib_dbg(dev, "err %d\n", err);
 		goto err_uuar;
@@ -866,22 +863,21 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 			    struct ib_udata *udata, struct mlx5_ib_qp *qp)
 {
 	struct mlx5_ib_resources *devr = &dev->devr;
+	struct mlx5_core_dev *mdev = dev->mdev;
 	struct mlx5_ib_create_qp_resp resp;
 	struct mlx5_create_qp_mbox_in *in;
-	struct mlx5_general_caps *gen;
 	struct mlx5_ib_create_qp ucmd;
 	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);
 	spin_lock_init(&qp->rq.lock);
 
 	if (init_attr->create_flags & IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK) {
-		if (!(gen->flags & MLX5_DEV_CAP_FLAG_BLOCK_MCAST)) {
+		if (!MLX5_CAP_GEN(mdev, block_lb_mc)) {
 			mlx5_ib_dbg(dev, "block multicast loopback isn't supported\n");
 			return -EINVAL;
 		} else {
@@ -914,15 +910,17 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 
 	if (pd) {
 		if (pd->uobject) {
+			__u32 max_wqes =
+				1 << MLX5_CAP_GEN(mdev, log_max_qp_sz);
 			mlx5_ib_dbg(dev, "requested sq_wqe_count (%d)\n", ucmd.sq_wqe_count);
 			if (ucmd.rq_wqe_shift != qp->rq.wqe_shift ||
 			    ucmd.rq_wqe_count != qp->rq.wqe_cnt) {
 				mlx5_ib_dbg(dev, "invalid rq params\n");
 				return -EINVAL;
 			}
-			if (ucmd.sq_wqe_count > gen->max_wqes) {
+			if (ucmd.sq_wqe_count > max_wqes) {
 				mlx5_ib_dbg(dev, "requested sq_wqe_count (%d) > max allowed (%d)\n",
-					    ucmd.sq_wqe_count, gen->max_wqes);
+					    ucmd.sq_wqe_count, max_wqes);
 				return -EINVAL;
 			}
 			err = create_user_qp(dev, pd, qp, udata, &in, &resp, &inlen);
@@ -1014,7 +1012,8 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 			in->ctx.rq_type_srqn |= cpu_to_be32(to_msrq(init_attr->srq)->msrq.srqn);
 		} else {
 			in->ctx.xrcd = cpu_to_be32(to_mxrcd(devr->x1)->xrcdn);
-			in->ctx.rq_type_srqn |= cpu_to_be32(to_msrq(devr->s0)->msrq.srqn);
+			in->ctx.rq_type_srqn |=
+				cpu_to_be32(to_msrq(devr->s1)->msrq.srqn);
 		}
 	}
 
@@ -1226,7 +1225,6 @@ struct ib_qp *mlx5_ib_create_qp(struct ib_pd *pd,
 				struct ib_qp_init_attr *init_attr,
 				struct ib_udata *udata)
 {
-	struct mlx5_general_caps *gen;
 	struct mlx5_ib_dev *dev;
 	struct mlx5_ib_qp *qp;
 	u16 xrcdn = 0;
@@ -1244,12 +1242,11 @@ struct ib_qp *mlx5_ib_create_qp(struct ib_pd *pd,
 		}
 		dev = to_mdev(to_mxrcd(init_attr->xrcd)->ibxrcd.device);
 	}
-	gen = &dev->mdev->caps.gen;
 
 	switch (init_attr->qp_type) {
 	case IB_QPT_XRC_TGT:
 	case IB_QPT_XRC_INI:
-		if (!(gen->flags & MLX5_DEV_CAP_FLAG_XRC)) {
+		if (!MLX5_CAP_GEN(dev->mdev, xrc)) {
 			mlx5_ib_dbg(dev, "XRC not supported\n");
 			return ERR_PTR(-ENOSYS);
 		}
@@ -1356,9 +1353,6 @@ enum {
 
 static int ib_rate_to_mlx5(struct mlx5_ib_dev *dev, u8 rate)
 {
-	struct mlx5_general_caps *gen;
-
-	gen = &dev->mdev->caps.gen;
 	if (rate == IB_RATE_PORT_CURRENT) {
 		return 0;
 	} else if (rate < IB_RATE_2_5_GBPS || rate > IB_RATE_300_GBPS) {
@@ -1366,7 +1360,7 @@ static int ib_rate_to_mlx5(struct mlx5_ib_dev *dev, u8 rate)
 	} else {
 		while (rate != IB_RATE_2_5_GBPS &&
 		       !(1 << (rate + MLX5_STAT_RATE_OFFSET) &
-			 gen->stat_rate_support))
+			 MLX5_CAP_GEN(dev->mdev, stat_rate_support)))
 			--rate;
 	}
 
@@ -1377,10 +1371,8 @@ static int mlx5_set_path(struct mlx5_ib_dev *dev, const struct ib_ah_attr *ah,
 			 struct mlx5_qp_path *path, u8 port, int attr_mask,
 			 u32 path_flags, const struct ib_qp_attr *attr)
 {
-	struct mlx5_general_caps *gen;
 	int err;
 
-	gen = &dev->mdev->caps.gen;
 	path->fl = (path_flags & MLX5_PATH_FLAG_FL) ? 0x80 : 0;
 	path->free_ar = (path_flags & MLX5_PATH_FLAG_FREE_AR) ? 0x80 : 0;
 
@@ -1391,9 +1383,11 @@ static int mlx5_set_path(struct mlx5_ib_dev *dev, const struct ib_ah_attr *ah,
 	path->rlid	= cpu_to_be16(ah->dlid);
 
 	if (ah->ah_flags & IB_AH_GRH) {
-		if (ah->grh.sgid_index >= gen->port[port - 1].gid_table_len) {
+		if (ah->grh.sgid_index >=
+		    dev->mdev->port_caps[port - 1].gid_table_len) {
 			pr_err("sgid_index (%u) too large. max is %d\n",
-			       ah->grh.sgid_index, gen->port[port - 1].gid_table_len);
+			       ah->grh.sgid_index,
+			       dev->mdev->port_caps[port - 1].gid_table_len);
 			return -EINVAL;
 		}
 		path->grh_mlid |= 1 << 7;
@@ -1570,7 +1564,6 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	struct mlx5_ib_qp *qp = to_mqp(ibqp);
 	struct mlx5_ib_cq *send_cq, *recv_cq;
 	struct mlx5_qp_context *context;
-	struct mlx5_general_caps *gen;
 	struct mlx5_modify_qp_mbox_in *in;
 	struct mlx5_ib_pd *pd;
 	enum mlx5_qp_state mlx5_cur, mlx5_new;
@@ -1579,7 +1572,6 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	int mlx5_st;
 	int err;
 
-	gen = &dev->mdev->caps.gen;
 	in = kzalloc(sizeof(*in), GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
@@ -1619,7 +1611,8 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 			err = -EINVAL;
 			goto out;
 		}
-		context->mtu_msgmax = (attr->path_mtu << 5) | gen->log_max_msg;
+		context->mtu_msgmax = (attr->path_mtu << 5) |
+				      (u8)MLX5_CAP_GEN(dev->mdev, log_max_msg);
 	}
 
 	if (attr_mask & IB_QP_DEST_QPN)
@@ -1777,11 +1770,9 @@ int mlx5_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 	struct mlx5_ib_dev *dev = to_mdev(ibqp->device);
 	struct mlx5_ib_qp *qp = to_mqp(ibqp);
 	enum ib_qp_state cur_state, new_state;
-	struct mlx5_general_caps *gen;
 	int err = -EINVAL;
 	int port;
 
-	gen = &dev->mdev->caps.gen;
 	mutex_lock(&qp->mutex);
 
 	cur_state = attr_mask & IB_QP_CUR_STATE ? attr->cur_qp_state : qp->state;
@@ -1793,21 +1784,25 @@ int mlx5_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 		goto out;
 
 	if ((attr_mask & IB_QP_PORT) &&
-	    (attr->port_num == 0 || attr->port_num > gen->num_ports))
+	    (attr->port_num == 0 ||
+	     attr->port_num > MLX5_CAP_GEN(dev->mdev, num_ports)))
 		goto out;
 
 	if (attr_mask & IB_QP_PKEY_INDEX) {
 		port = attr_mask & IB_QP_PORT ? attr->port_num : qp->port;
-		if (attr->pkey_index >= gen->port[port - 1].pkey_table_len)
+		if (attr->pkey_index >=
+		    dev->mdev->port_caps[port - 1].pkey_table_len)
 			goto out;
 	}
 
 	if (attr_mask & IB_QP_MAX_QP_RD_ATOMIC &&
-	    attr->max_rd_atomic > (1 << gen->log_max_ra_res_qp))
+	    attr->max_rd_atomic >
+	    (1 << MLX5_CAP_GEN(dev->mdev, log_max_ra_res_qp)))
 		goto out;
 
 	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC &&
-	    attr->max_dest_rd_atomic > (1 << gen->log_max_ra_req_qp))
+	    attr->max_dest_rd_atomic >
+	    (1 << MLX5_CAP_GEN(dev->mdev, log_max_ra_req_qp)))
 		goto out;
 
 	if (cur_state == new_state && cur_state == IB_QPS_RESET) {
@@ -3009,7 +3004,7 @@ static void to_ib_ah_attr(struct mlx5_ib_dev *ibdev, struct ib_ah_attr *ib_ah_at
 	ib_ah_attr->port_num	  = path->port;
 
 	if (ib_ah_attr->port_num == 0 ||
-	    ib_ah_attr->port_num > dev->caps.gen.num_ports)
+	    ib_ah_attr->port_num > MLX5_CAP_GEN(dev, num_ports))
 		return;
 
 	ib_ah_attr->sl = path->sl & 0xf;
@@ -3135,12 +3130,10 @@ struct ib_xrcd *mlx5_ib_alloc_xrcd(struct ib_device *ibdev,
 					  struct ib_udata *udata)
 {
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
-	struct mlx5_general_caps *gen;
 	struct mlx5_ib_xrcd *xrcd;
 	int err;
 
-	gen = &dev->mdev->caps.gen;
-	if (!(gen->flags & MLX5_DEV_CAP_FLAG_XRC))
+	if (!MLX5_CAP_GEN(dev->mdev, xrc))
 		return ERR_PTR(-ENOSYS);
 
 	xrcd = kmalloc(sizeof(*xrcd), GFP_KERNEL);
diff --git a/drivers/infiniband/hw/mlx5/srq.c b/drivers/infiniband/hw/mlx5/srq.c
index 02d77a29764d..e008505e96e9 100644
--- a/drivers/infiniband/hw/mlx5/srq.c
+++ b/drivers/infiniband/hw/mlx5/srq.c
@@ -165,7 +165,7 @@ static int create_srq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_srq *srq,
 		return err;
 	}
 
-	if (mlx5_buf_alloc(dev->mdev, buf_size, PAGE_SIZE * 2, &srq->buf)) {
+	if (mlx5_buf_alloc(dev->mdev, buf_size, &srq->buf)) {
 		mlx5_ib_dbg(dev, "buf alloc failed\n");
 		err = -ENOMEM;
 		goto err_db;
@@ -236,7 +236,6 @@ struct ib_srq *mlx5_ib_create_srq(struct ib_pd *pd,
 				  struct ib_udata *udata)
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
-	struct mlx5_general_caps *gen;
 	struct mlx5_ib_srq *srq;
 	int desc_size;
 	int buf_size;
@@ -245,13 +244,13 @@ struct ib_srq *mlx5_ib_create_srq(struct ib_pd *pd,
 	int uninitialized_var(inlen);
 	int is_xrc;
 	u32 flgs, xrcdn;
+	__u32 max_srq_wqes = 1 << MLX5_CAP_GEN(dev->mdev, log_max_srq_sz);
 
-	gen = &dev->mdev->caps.gen;
 	/* Sanity check SRQ size before proceeding */
-	if (init_attr->attr.max_wr >= gen->max_srq_wqes) {
+	if (init_attr->attr.max_wr >= max_srq_wqes) {
 		mlx5_ib_dbg(dev, "max_wr %d, cap %d\n",
 			    init_attr->attr.max_wr,
-			    gen->max_srq_wqes);
+			    max_srq_wqes);
 		return ERR_PTR(-EINVAL);
 	}
 
@@ -303,7 +302,7 @@ struct ib_srq *mlx5_ib_create_srq(struct ib_pd *pd,
 
 	in->ctx.pd = cpu_to_be32(to_mpd(pd)->pdn);
 	in->ctx.db_record = cpu_to_be64(srq->db.dma);
-	err = mlx5_core_create_srq(dev->mdev, &srq->msrq, in, inlen);
+	err = mlx5_core_create_srq(dev->mdev, &srq->msrq, in, inlen, is_xrc);
 	kvfree(in);
 	if (err) {
 		mlx5_ib_dbg(dev, "create SRQ failed, err %d\n", err);
diff --git a/drivers/infiniband/hw/mthca/mthca_cmd.c b/drivers/infiniband/hw/mthca/mthca_cmd.c
index 9d3e5c1ac60e..c7f49bbb0c72 100644
--- a/drivers/infiniband/hw/mthca/mthca_cmd.c
+++ b/drivers/infiniband/hw/mthca/mthca_cmd.c
@@ -1858,8 +1858,8 @@ int mthca_CONF_SPECIAL_QP(struct mthca_dev *dev, int type, u32 qpn)
 }
 
 int mthca_MAD_IFC(struct mthca_dev *dev, int ignore_mkey, int ignore_bkey,
-		  int port, struct ib_wc *in_wc, struct ib_grh *in_grh,
-		  void *in_mad, void *response_mad)
+		  int port, const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		  const void *in_mad, void *response_mad)
 {
 	struct mthca_mailbox *inmailbox, *outmailbox;
 	void *inbox;
diff --git a/drivers/infiniband/hw/mthca/mthca_cmd.h b/drivers/infiniband/hw/mthca/mthca_cmd.h
index f952244c54de..d2e5b194b938 100644
--- a/drivers/infiniband/hw/mthca/mthca_cmd.h
+++ b/drivers/infiniband/hw/mthca/mthca_cmd.h
@@ -312,8 +312,8 @@ int mthca_QUERY_QP(struct mthca_dev *dev, u32 num, int is_ee,
 		   struct mthca_mailbox *mailbox);
 int mthca_CONF_SPECIAL_QP(struct mthca_dev *dev, int type, u32 qpn);
 int mthca_MAD_IFC(struct mthca_dev *dev, int ignore_mkey, int ignore_bkey,
-		  int port, struct ib_wc *in_wc, struct ib_grh *in_grh,
-		  void *in_mad, void *response_mad);
+		  int port, const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		  const void *in_mad, void *response_mad);
 int mthca_READ_MGM(struct mthca_dev *dev, int index,
 		   struct mthca_mailbox *mailbox);
 int mthca_WRITE_MGM(struct mthca_dev *dev, int index,
diff --git a/drivers/infiniband/hw/mthca/mthca_dev.h b/drivers/infiniband/hw/mthca/mthca_dev.h
index 7e6a6d64ad4e..4393a022867b 100644
--- a/drivers/infiniband/hw/mthca/mthca_dev.h
+++ b/drivers/infiniband/hw/mthca/mthca_dev.h
@@ -576,10 +576,11 @@ int mthca_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid);
 int mthca_process_mad(struct ib_device *ibdev,
 		      int mad_flags,
 		      u8 port_num,
-		      struct ib_wc *in_wc,
-		      struct ib_grh *in_grh,
-		      struct ib_mad *in_mad,
-		      struct ib_mad *out_mad);
+		      const struct ib_wc *in_wc,
+		      const struct ib_grh *in_grh,
+		      const struct ib_mad_hdr *in, size_t in_mad_size,
+		      struct ib_mad_hdr *out, size_t *out_mad_size,
+		      u16 *out_mad_pkey_index);
 int mthca_create_agents(struct mthca_dev *dev);
 void mthca_free_agents(struct mthca_dev *dev);
 
diff --git a/drivers/infiniband/hw/mthca/mthca_mad.c b/drivers/infiniband/hw/mthca/mthca_mad.c
index 8881fa376e06..6b2418b74c99 100644
--- a/drivers/infiniband/hw/mthca/mthca_mad.c
+++ b/drivers/infiniband/hw/mthca/mthca_mad.c
@@ -104,7 +104,7 @@ static void update_sm_ah(struct mthca_dev *dev,
  */
 static void smp_snoop(struct ib_device *ibdev,
 		      u8 port_num,
-		      struct ib_mad *mad,
+		      const struct ib_mad *mad,
 		      u16 prev_lid)
 {
 	struct ib_event event;
@@ -160,7 +160,7 @@ static void node_desc_override(struct ib_device *dev,
 
 static void forward_trap(struct mthca_dev *dev,
 			 u8 port_num,
-			 struct ib_mad *mad)
+			 const struct ib_mad *mad)
 {
 	int qpn = mad->mad_hdr.mgmt_class != IB_MGMT_CLASS_SUBN_LID_ROUTED;
 	struct ib_mad_send_buf *send_buf;
@@ -170,7 +170,8 @@ static void forward_trap(struct mthca_dev *dev,
 
 	if (agent) {
 		send_buf = ib_create_send_mad(agent, qpn, 0, 0, IB_MGMT_MAD_HDR,
-					      IB_MGMT_MAD_DATA, GFP_ATOMIC);
+					      IB_MGMT_MAD_DATA, GFP_ATOMIC,
+					      IB_MGMT_BASE_VERSION);
 		if (IS_ERR(send_buf))
 			return;
 		/*
@@ -195,15 +196,21 @@ static void forward_trap(struct mthca_dev *dev,
 int mthca_process_mad(struct ib_device *ibdev,
 		      int mad_flags,
 		      u8 port_num,
-		      struct ib_wc *in_wc,
-		      struct ib_grh *in_grh,
-		      struct ib_mad *in_mad,
-		      struct ib_mad *out_mad)
+		      const struct ib_wc *in_wc,
+		      const struct ib_grh *in_grh,
+		      const struct ib_mad_hdr *in, size_t in_mad_size,
+		      struct ib_mad_hdr *out, size_t *out_mad_size,
+		      u16 *out_mad_pkey_index)
 {
 	int err;
 	u16 slid = in_wc ? in_wc->slid : be16_to_cpu(IB_LID_PERMISSIVE);
 	u16 prev_lid = 0;
 	struct ib_port_attr pattr;
+	const struct ib_mad *in_mad = (const struct ib_mad *)in;
+	struct ib_mad *out_mad = (struct ib_mad *)out;
+
+	BUG_ON(in_mad_size != sizeof(*in_mad) ||
+	       *out_mad_size != sizeof(*out_mad));
 
 	/* Forward locally generated traps to the SM */
 	if (in_mad->mad_hdr.method == IB_MGMT_METHOD_TRAP &&
diff --git a/drivers/infiniband/hw/mthca/mthca_profile.c b/drivers/infiniband/hw/mthca/mthca_profile.c
index 8edb28a9a0e7..15d064479ef6 100644
--- a/drivers/infiniband/hw/mthca/mthca_profile.c
+++ b/drivers/infiniband/hw/mthca/mthca_profile.c
@@ -77,7 +77,6 @@ s64 mthca_make_profile(struct mthca_dev *dev,
 	u64 mem_base, mem_avail;
 	s64 total_size = 0;
 	struct mthca_resource *profile;
-	struct mthca_resource tmp;
 	int i, j;
 
 	profile = kzalloc(MTHCA_RES_NUM * sizeof *profile, GFP_KERNEL);
@@ -136,11 +135,8 @@ s64 mthca_make_profile(struct mthca_dev *dev,
 	 */
 	for (i = MTHCA_RES_NUM; i > 0; --i)
 		for (j = 1; j < i; ++j) {
-			if (profile[j].size > profile[j - 1].size) {
-				tmp            = profile[j];
-				profile[j]     = profile[j - 1];
-				profile[j - 1] = tmp;
-			}
+			if (profile[j].size > profile[j - 1].size)
+				swap(profile[j], profile[j - 1]);
 		}
 
 	for (i = 0; i < MTHCA_RES_NUM; ++i) {
diff --git a/drivers/infiniband/hw/mthca/mthca_provider.c b/drivers/infiniband/hw/mthca/mthca_provider.c
index 415f8e1a54db..93ae51dcf2ff 100644
--- a/drivers/infiniband/hw/mthca/mthca_provider.c
+++ b/drivers/infiniband/hw/mthca/mthca_provider.c
@@ -57,14 +57,17 @@ static void init_query_mad(struct ib_smp *mad)
 	mad->method    	   = IB_MGMT_METHOD_GET;
 }
 
-static int mthca_query_device(struct ib_device *ibdev,
-			      struct ib_device_attr *props)
+static int mthca_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+			      struct ib_udata *uhw)
 {
 	struct ib_smp *in_mad  = NULL;
 	struct ib_smp *out_mad = NULL;
 	int err = -ENOMEM;
 	struct mthca_dev *mdev = to_mdev(ibdev);
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	in_mad  = kzalloc(sizeof *in_mad, GFP_KERNEL);
 	out_mad = kmalloc(sizeof *out_mad, GFP_KERNEL);
 	if (!in_mad || !out_mad)
@@ -641,16 +644,20 @@ static int mthca_destroy_qp(struct ib_qp *qp)
 	return 0;
 }
 
-static struct ib_cq *mthca_create_cq(struct ib_device *ibdev, int entries,
-				     int comp_vector,
+static struct ib_cq *mthca_create_cq(struct ib_device *ibdev,
+				     const struct ib_cq_init_attr *attr,
 				     struct ib_ucontext *context,
 				     struct ib_udata *udata)
 {
+	int entries = attr->cqe;
 	struct mthca_create_cq ucmd;
 	struct mthca_cq *cq;
 	int nent;
 	int err;
 
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	if (entries < 1 || entries > to_mdev(ibdev)->limits.max_cqes)
 		return ERR_PTR(-EINVAL);
 
@@ -1244,6 +1251,24 @@ out:
 	return err;
 }
 
+static int mthca_port_immutable(struct ib_device *ibdev, u8 port_num,
+			        struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = mthca_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
+	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
+
+	return 0;
+}
+
 int mthca_register_device(struct mthca_dev *dev)
 {
 	int ret;
@@ -1323,6 +1348,7 @@ int mthca_register_device(struct mthca_dev *dev)
 	dev->ib_dev.reg_phys_mr          = mthca_reg_phys_mr;
 	dev->ib_dev.reg_user_mr          = mthca_reg_user_mr;
 	dev->ib_dev.dereg_mr             = mthca_dereg_mr;
+	dev->ib_dev.get_port_immutable   = mthca_port_immutable;
 
 	if (dev->mthca_flags & MTHCA_FLAG_FMR) {
 		dev->ib_dev.alloc_fmr            = mthca_alloc_fmr;
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index 72b43417cbe3..9047af429906 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -1616,6 +1616,8 @@ static struct nes_cm_node *make_cm_node(struct nes_cm_core *cm_core,
 		  &cm_node->loc_addr, cm_node->loc_port,
 		  &cm_node->rem_addr, cm_node->rem_port);
 	cm_node->listener = listener;
+	if (listener)
+		cm_node->tos = listener->tos;
 	cm_node->netdev = nesvnic->netdev;
 	cm_node->cm_id = cm_info->cm_id;
 	memcpy(cm_node->loc_mac, nesvnic->netdev->dev_addr, ETH_ALEN);
@@ -2938,6 +2940,9 @@ static int nes_cm_init_tsa_conn(struct nes_qp *nesqp, struct nes_cm_node *cm_nod
 
 	nesqp->nesqp_context->misc2 |= cpu_to_le32(64 << NES_QPCONTEXT_MISC2_TTL_SHIFT);
 
+	nesqp->nesqp_context->misc2 |= cpu_to_le32(
+		cm_node->tos << NES_QPCONTEXT_MISC2_TOS_SHIFT);
+
 	nesqp->nesqp_context->mss |= cpu_to_le32(((u32)cm_node->tcp_cntxt.mss) << 16);
 
 	nesqp->nesqp_context->tcp_state_flow_label |= cpu_to_le32(
@@ -3612,6 +3617,7 @@ int nes_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		cm_node->ord_size = 1;
 
 	cm_node->apbvt_set = apbvt_set;
+	cm_node->tos = cm_id->tos;
 	nesqp->cm_node = cm_node;
 	cm_node->nesqp = nesqp;
 	nes_add_ref(&nesqp->ibqp);
@@ -3666,6 +3672,7 @@ int nes_create_listen(struct iw_cm_id *cm_id, int backlog)
 	}
 
 	cm_id->provider_data = cm_node;
+	cm_node->tos = cm_id->tos;
 
 	if (!cm_node->reused_node) {
 		if (nes_create_mapinfo(&cm_info))
diff --git a/drivers/infiniband/hw/nes/nes_cm.h b/drivers/infiniband/hw/nes/nes_cm.h
index f522cf639789..32a6420c2940 100644
--- a/drivers/infiniband/hw/nes/nes_cm.h
+++ b/drivers/infiniband/hw/nes/nes_cm.h
@@ -303,6 +303,7 @@ struct nes_cm_listener {
 	int                        backlog;
 	enum nes_cm_listener_state listener_state;
 	u32                        reused_node;
+	u8			   tos;
 };
 
 /* per connection node and node state information */
@@ -352,6 +353,7 @@ struct nes_cm_node {
 	struct list_head	reset_entry;
 	struct nes_qp		*nesqp;
 	atomic_t 		passive_state;
+	u8			tos;
 };
 
 /* structure for client or CM to fill when making CM api calls. */
diff --git a/drivers/infiniband/hw/nes/nes_verbs.c b/drivers/infiniband/hw/nes/nes_verbs.c
index c0d0296e7a00..fbc43e5f717b 100644
--- a/drivers/infiniband/hw/nes/nes_verbs.c
+++ b/drivers/infiniband/hw/nes/nes_verbs.c
@@ -512,12 +512,16 @@ static void nes_free_fast_reg_page_list(struct ib_fast_reg_page_list *pifrpl)
 /**
  * nes_query_device
  */
-static int nes_query_device(struct ib_device *ibdev, struct ib_device_attr *props)
+static int nes_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+			    struct ib_udata *uhw)
 {
 	struct nes_vnic *nesvnic = to_nesvnic(ibdev);
 	struct nes_device *nesdev = nesvnic->nesdev;
 	struct nes_ib_device *nesibdev = nesvnic->nesibdev;
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	memset(props, 0, sizeof(*props));
 	memcpy(&props->sys_image_guid, nesvnic->netdev->dev_addr, 6);
 
@@ -606,7 +610,6 @@ static int nes_query_port(struct ib_device *ibdev, u8 port, struct ib_port_attr
 	return 0;
 }
 
-
 /**
  * nes_query_pkey
  */
@@ -1527,10 +1530,12 @@ static int nes_destroy_qp(struct ib_qp *ibqp)
 /**
  * nes_create_cq
  */
-static struct ib_cq *nes_create_cq(struct ib_device *ibdev, int entries,
-		int comp_vector,
-		struct ib_ucontext *context, struct ib_udata *udata)
+static struct ib_cq *nes_create_cq(struct ib_device *ibdev,
+				   const struct ib_cq_init_attr *attr,
+				   struct ib_ucontext *context,
+				   struct ib_udata *udata)
 {
+	int entries = attr->cqe;
 	u64 u64temp;
 	struct nes_vnic *nesvnic = to_nesvnic(ibdev);
 	struct nes_device *nesdev = nesvnic->nesdev;
@@ -1550,6 +1555,9 @@ static struct ib_cq *nes_create_cq(struct ib_device *ibdev, int entries,
 	unsigned long flags;
 	int ret;
 
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	if (entries > nesadapter->max_cqe)
 		return ERR_PTR(-EINVAL);
 
@@ -3222,8 +3230,10 @@ static int nes_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
  * nes_process_mad
  */
 static int nes_process_mad(struct ib_device *ibdev, int mad_flags,
-		u8 port_num, struct ib_wc *in_wc, struct ib_grh *in_grh,
-		struct ib_mad *in_mad, struct ib_mad *out_mad)
+		u8 port_num, const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		const struct ib_mad_hdr *in, size_t in_mad_size,
+		struct ib_mad_hdr *out, size_t *out_mad_size,
+		u16 *out_mad_pkey_index)
 {
 	nes_debug(NES_DBG_INIT, "\n");
 	return -ENOSYS;
@@ -3828,6 +3838,22 @@ static int nes_req_notify_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags notify_
 	return 0;
 }
 
+static int nes_port_immutable(struct ib_device *ibdev, u8 port_num,
+			      struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = nes_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IWARP;
+
+	return 0;
+}
 
 /**
  * nes_init_ofa_device
@@ -3928,6 +3954,7 @@ struct nes_ib_device *nes_init_ofa_device(struct net_device *netdev)
 	nesibdev->ibdev.iwcm->reject = nes_reject;
 	nesibdev->ibdev.iwcm->create_listen = nes_create_listen;
 	nesibdev->ibdev.iwcm->destroy_listen = nes_destroy_listen;
+	nesibdev->ibdev.get_port_immutable   = nes_port_immutable;
 
 	return nesibdev;
 }
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
index f5a5ea836dbd..4bafa15708d0 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
@@ -204,12 +204,19 @@ int ocrdma_modify_ah(struct ib_ah *ibah, struct ib_ah_attr *attr)
 int ocrdma_process_mad(struct ib_device *ibdev,
 		       int process_mad_flags,
 		       u8 port_num,
-		       struct ib_wc *in_wc,
-		       struct ib_grh *in_grh,
-		       struct ib_mad *in_mad, struct ib_mad *out_mad)
+		       const struct ib_wc *in_wc,
+		       const struct ib_grh *in_grh,
+		       const struct ib_mad_hdr *in, size_t in_mad_size,
+		       struct ib_mad_hdr *out, size_t *out_mad_size,
+		       u16 *out_mad_pkey_index)
 {
 	int status;
 	struct ocrdma_dev *dev;
+	const struct ib_mad *in_mad = (const struct ib_mad *)in;
+	struct ib_mad *out_mad = (struct ib_mad *)out;
+
+	BUG_ON(in_mad_size != sizeof(*in_mad) ||
+	       *out_mad_size != sizeof(*out_mad));
 
 	switch (in_mad->mad_hdr.mgmt_class) {
 	case IB_MGMT_CLASS_PERF_MGMT:
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_ah.h b/drivers/infiniband/hw/ocrdma/ocrdma_ah.h
index 726a87cf22dc..cf366fe03cb8 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_ah.h
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_ah.h
@@ -42,7 +42,9 @@ int ocrdma_modify_ah(struct ib_ah *, struct ib_ah_attr *);
 int ocrdma_process_mad(struct ib_device *,
 		       int process_mad_flags,
 		       u8 port_num,
-		       struct ib_wc *in_wc,
-		       struct ib_grh *in_grh,
-		       struct ib_mad *in_mad, struct ib_mad *out_mad);
+		       const struct ib_wc *in_wc,
+		       const struct ib_grh *in_grh,
+		       const struct ib_mad_hdr *in, size_t in_mad_size,
+		       struct ib_mad_hdr *out, size_t *out_mad_size,
+		       u16 *out_mad_pkey_index);
 #endif				/* __OCRDMA_AH_H__ */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_main.c b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
index 7a2b59aca004..8a1398b253a2 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_main.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
@@ -30,6 +30,7 @@
 #include <rdma/ib_verbs.h>
 #include <rdma/ib_user_verbs.h>
 #include <rdma/ib_addr.h>
+#include <rdma/ib_mad.h>
 
 #include <linux/netdevice.h>
 #include <net/addrconf.h>
@@ -202,6 +203,24 @@ static enum rdma_link_layer ocrdma_link_layer(struct ib_device *device,
 	return IB_LINK_LAYER_ETHERNET;
 }
 
+static int ocrdma_port_immutable(struct ib_device *ibdev, u8 port_num,
+			         struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = ocrdma_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_ROCE;
+	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
+
+	return 0;
+}
+
 static int ocrdma_register_device(struct ocrdma_dev *dev)
 {
 	strlcpy(dev->ibdev.name, "ocrdma%d", IB_DEVICE_NAME_MAX);
@@ -286,6 +305,7 @@ static int ocrdma_register_device(struct ocrdma_dev *dev)
 	dev->ibdev.dma_device = &dev->nic_info.pdev->dev;
 
 	dev->ibdev.process_mad = ocrdma_process_mad;
+	dev->ibdev.get_port_immutable = ocrdma_port_immutable;
 
 	if (ocrdma_get_asic_type(dev) == OCRDMA_ASIC_GEN_SKH_R) {
 		dev->ibdev.uverbs_cmd_mask |=
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
index 9dcb66077d6c..5bb61eb58f2c 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
@@ -61,10 +61,14 @@ int ocrdma_query_gid(struct ib_device *ibdev, u8 port,
 	return 0;
 }
 
-int ocrdma_query_device(struct ib_device *ibdev, struct ib_device_attr *attr)
+int ocrdma_query_device(struct ib_device *ibdev, struct ib_device_attr *attr,
+			struct ib_udata *uhw)
 {
 	struct ocrdma_dev *dev = get_ocrdma_dev(ibdev);
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	memset(attr, 0, sizeof *attr);
 	memcpy(&attr->fw_ver, &dev->attr.fw_ver[0],
 	       min(sizeof(dev->attr.fw_ver), sizeof(attr->fw_ver)));
@@ -375,7 +379,12 @@ static struct ocrdma_pd *_ocrdma_alloc_pd(struct ocrdma_dev *dev,
 
 	if (dev->pd_mgr->pd_prealloc_valid) {
 		status = ocrdma_get_pd_num(dev, pd);
-		return (status == 0) ? pd : ERR_PTR(status);
+		if (status == 0) {
+			return pd;
+		} else {
+			kfree(pd);
+			return ERR_PTR(status);
+		}
 	}
 
 retry:
@@ -679,7 +688,6 @@ err:
 		ocrdma_release_ucontext_pd(uctx);
 	} else {
 		status = _ocrdma_dealloc_pd(dev, pd);
-		kfree(pd);
 	}
 exit:
 	return ERR_PTR(status);
@@ -1000,10 +1008,12 @@ err:
 	return status;
 }
 
-struct ib_cq *ocrdma_create_cq(struct ib_device *ibdev, int entries, int vector,
+struct ib_cq *ocrdma_create_cq(struct ib_device *ibdev,
+			       const struct ib_cq_init_attr *attr,
 			       struct ib_ucontext *ib_ctx,
 			       struct ib_udata *udata)
 {
+	int entries = attr->cqe;
 	struct ocrdma_cq *cq;
 	struct ocrdma_dev *dev = get_ocrdma_dev(ibdev);
 	struct ocrdma_ucontext *uctx = NULL;
@@ -1011,6 +1021,9 @@ struct ib_cq *ocrdma_create_cq(struct ib_device *ibdev, int entries, int vector,
 	int status;
 	struct ocrdma_create_cq_ureq ureq;
 
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	if (udata) {
 		if (ib_copy_from_udata(&ureq, udata, sizeof(ureq)))
 			return ERR_PTR(-EFAULT);
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h
index b8f7853fd36c..b15c608efa7b 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h
@@ -36,11 +36,15 @@ int ocrdma_post_recv(struct ib_qp *, struct ib_recv_wr *,
 int ocrdma_poll_cq(struct ib_cq *, int num_entries, struct ib_wc *wc);
 int ocrdma_arm_cq(struct ib_cq *, enum ib_cq_notify_flags flags);
 
-int ocrdma_query_device(struct ib_device *, struct ib_device_attr *props);
+int ocrdma_query_device(struct ib_device *, struct ib_device_attr *props,
+			struct ib_udata *uhw);
 int ocrdma_query_port(struct ib_device *, u8 port, struct ib_port_attr *props);
 int ocrdma_modify_port(struct ib_device *, u8 port, int mask,
 		       struct ib_port_modify *props);
 
+enum rdma_protocol_type
+ocrdma_query_protocol(struct ib_device *device, u8 port_num);
+
 void ocrdma_get_guid(struct ocrdma_dev *, u8 *guid);
 int ocrdma_query_gid(struct ib_device *, u8 port,
 		     int index, union ib_gid *gid);
@@ -56,8 +60,10 @@ struct ib_pd *ocrdma_alloc_pd(struct ib_device *,
 			      struct ib_ucontext *, struct ib_udata *);
 int ocrdma_dealloc_pd(struct ib_pd *pd);
 
-struct ib_cq *ocrdma_create_cq(struct ib_device *, int entries, int vector,
-			       struct ib_ucontext *, struct ib_udata *);
+struct ib_cq *ocrdma_create_cq(struct ib_device *ibdev,
+			       const struct ib_cq_init_attr *attr,
+			       struct ib_ucontext *ib_ctx,
+			       struct ib_udata *udata);
 int ocrdma_resize_cq(struct ib_cq *, int cqe, struct ib_udata *);
 int ocrdma_destroy_cq(struct ib_cq *);
 
diff --git a/drivers/infiniband/hw/qib/qib_cq.c b/drivers/infiniband/hw/qib/qib_cq.c
index ab4e11cfab15..2b45d0b02300 100644
--- a/drivers/infiniband/hw/qib/qib_cq.c
+++ b/drivers/infiniband/hw/qib/qib_cq.c
@@ -203,7 +203,7 @@ static void send_complete(struct kthread_work *work)
 /**
  * qib_create_cq - create a completion queue
  * @ibdev: the device this completion queue is attached to
- * @entries: the minimum size of the completion queue
+ * @attr: creation attributes
  * @context: unused by the QLogic_IB driver
  * @udata: user data for libibverbs.so
  *
@@ -212,16 +212,21 @@ static void send_complete(struct kthread_work *work)
  *
  * Called by ib_create_cq() in the generic verbs code.
  */
-struct ib_cq *qib_create_cq(struct ib_device *ibdev, int entries,
-			    int comp_vector, struct ib_ucontext *context,
+struct ib_cq *qib_create_cq(struct ib_device *ibdev,
+			    const struct ib_cq_init_attr *attr,
+			    struct ib_ucontext *context,
 			    struct ib_udata *udata)
 {
+	int entries = attr->cqe;
 	struct qib_ibdev *dev = to_idev(ibdev);
 	struct qib_cq *cq;
 	struct qib_cq_wc *wc;
 	struct ib_cq *ret;
 	u32 sz;
 
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	if (entries < 1 || entries > ib_qib_max_cqes) {
 		ret = ERR_PTR(-EINVAL);
 		goto done;
diff --git a/drivers/infiniband/hw/qib/qib_fs.c b/drivers/infiniband/hw/qib/qib_fs.c
index bdd5d3857203..13ef22bd9459 100644
--- a/drivers/infiniband/hw/qib/qib_fs.c
+++ b/drivers/infiniband/hw/qib/qib_fs.c
@@ -455,7 +455,7 @@ static int remove_file(struct dentry *parent, char *name)
 	}
 
 	spin_lock(&tmp->d_lock);
-	if (!d_unhashed(tmp) && d_really_is_positive(tmp)) {
+	if (simple_positive(tmp)) {
 		__d_drop(tmp);
 		spin_unlock(&tmp->d_lock);
 		simple_unlink(d_inode(parent), tmp);
diff --git a/drivers/infiniband/hw/qib/qib_iba7322.c b/drivers/infiniband/hw/qib/qib_iba7322.c
index f32b4628e991..6c8ff10101c0 100644
--- a/drivers/infiniband/hw/qib/qib_iba7322.c
+++ b/drivers/infiniband/hw/qib/qib_iba7322.c
@@ -5502,7 +5502,8 @@ static void try_7322_ipg(struct qib_pportdata *ppd)
 		goto retry;
 
 	send_buf = ib_create_send_mad(agent, 0, 0, 0, IB_MGMT_MAD_HDR,
-				      IB_MGMT_MAD_DATA, GFP_ATOMIC);
+				      IB_MGMT_MAD_DATA, GFP_ATOMIC,
+				      IB_MGMT_BASE_VERSION);
 	if (IS_ERR(send_buf))
 		goto retry;
 
diff --git a/drivers/infiniband/hw/qib/qib_mad.c b/drivers/infiniband/hw/qib/qib_mad.c
index 395f4046dba2..05e3242d8442 100644
--- a/drivers/infiniband/hw/qib/qib_mad.c
+++ b/drivers/infiniband/hw/qib/qib_mad.c
@@ -83,7 +83,8 @@ static void qib_send_trap(struct qib_ibport *ibp, void *data, unsigned len)
 		return;
 
 	send_buf = ib_create_send_mad(agent, 0, 0, 0, IB_MGMT_MAD_HDR,
-				      IB_MGMT_MAD_DATA, GFP_ATOMIC);
+				      IB_MGMT_MAD_DATA, GFP_ATOMIC,
+				      IB_MGMT_BASE_VERSION);
 	if (IS_ERR(send_buf))
 		return;
 
@@ -1854,7 +1855,7 @@ static int pma_set_portcounters_ext(struct ib_pma_mad *pmp,
 }
 
 static int process_subn(struct ib_device *ibdev, int mad_flags,
-			u8 port, struct ib_mad *in_mad,
+			u8 port, const struct ib_mad *in_mad,
 			struct ib_mad *out_mad)
 {
 	struct ib_smp *smp = (struct ib_smp *)out_mad;
@@ -2006,7 +2007,7 @@ bail:
 }
 
 static int process_perf(struct ib_device *ibdev, u8 port,
-			struct ib_mad *in_mad,
+			const struct ib_mad *in_mad,
 			struct ib_mad *out_mad)
 {
 	struct ib_pma_mad *pmp = (struct ib_pma_mad *)out_mad;
@@ -2299,7 +2300,7 @@ static int check_cc_key(struct qib_ibport *ibp,
 }
 
 static int process_cc(struct ib_device *ibdev, int mad_flags,
-			u8 port, struct ib_mad *in_mad,
+			u8 port, const struct ib_mad *in_mad,
 			struct ib_mad *out_mad)
 {
 	struct ib_cc_mad *ccp = (struct ib_cc_mad *)out_mad;
@@ -2400,12 +2401,19 @@ bail:
  * This is called by the ib_mad module.
  */
 int qib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port,
-		    struct ib_wc *in_wc, struct ib_grh *in_grh,
-		    struct ib_mad *in_mad, struct ib_mad *out_mad)
+		    const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		    const struct ib_mad_hdr *in, size_t in_mad_size,
+		    struct ib_mad_hdr *out, size_t *out_mad_size,
+		    u16 *out_mad_pkey_index)
 {
 	int ret;
 	struct qib_ibport *ibp = to_iport(ibdev, port);
 	struct qib_pportdata *ppd = ppd_from_ibp(ibp);
+	const struct ib_mad *in_mad = (const struct ib_mad *)in;
+	struct ib_mad *out_mad = (struct ib_mad *)out;
+
+	BUG_ON(in_mad_size != sizeof(*in_mad) ||
+	       *out_mad_size != sizeof(*out_mad));
 
 	switch (in_mad->mad_hdr.mgmt_class) {
 	case IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE:
diff --git a/drivers/infiniband/hw/qib/qib_verbs.c b/drivers/infiniband/hw/qib/qib_verbs.c
index 4a3599890ea5..a05d1a372208 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.c
+++ b/drivers/infiniband/hw/qib/qib_verbs.c
@@ -1550,12 +1550,14 @@ full:
 	}
 }
 
-static int qib_query_device(struct ib_device *ibdev,
-			    struct ib_device_attr *props)
+static int qib_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
+			    struct ib_udata *uhw)
 {
 	struct qib_devdata *dd = dd_from_ibdev(ibdev);
 	struct qib_ibdev *dev = to_idev(ibdev);
 
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
 	memset(props, 0, sizeof(*props));
 
 	props->device_cap_flags = IB_DEVICE_BAD_PKEY_CNTR |
@@ -2040,6 +2042,24 @@ static void init_ibport(struct qib_pportdata *ppd)
 	RCU_INIT_POINTER(ibp->qp1, NULL);
 }
 
+static int qib_port_immutable(struct ib_device *ibdev, u8 port_num,
+			      struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = qib_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
+	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
+
+	return 0;
+}
+
 /**
  * qib_register_ib_device - register our device with the infiniband core
  * @dd: the device data structure
@@ -2227,6 +2247,7 @@ int qib_register_ib_device(struct qib_devdata *dd)
 	ibdev->process_mad = qib_process_mad;
 	ibdev->mmap = qib_mmap;
 	ibdev->dma_ops = &qib_dma_mapping_ops;
+	ibdev->get_port_immutable = qib_port_immutable;
 
 	snprintf(ibdev->node_desc, sizeof(ibdev->node_desc),
 		 "Intel Infiniband HCA %s", init_utsname()->nodename);
diff --git a/drivers/infiniband/hw/qib/qib_verbs.h b/drivers/infiniband/hw/qib/qib_verbs.h
index bfc8948fdd35..1635572752ce 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.h
+++ b/drivers/infiniband/hw/qib/qib_verbs.h
@@ -872,8 +872,10 @@ void qib_cap_mask_chg(struct qib_ibport *ibp);
 void qib_sys_guid_chg(struct qib_ibport *ibp);
 void qib_node_desc_chg(struct qib_ibport *ibp);
 int qib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-		    struct ib_wc *in_wc, struct ib_grh *in_grh,
-		    struct ib_mad *in_mad, struct ib_mad *out_mad);
+		    const struct ib_wc *in_wc, const struct ib_grh *in_grh,
+		    const struct ib_mad_hdr *in, size_t in_mad_size,
+		    struct ib_mad_hdr *out, size_t *out_mad_size,
+		    u16 *out_mad_pkey_index);
 int qib_create_agents(struct qib_ibdev *dev);
 void qib_free_agents(struct qib_ibdev *dev);
 
@@ -1007,8 +1009,9 @@ void qib_cq_enter(struct qib_cq *cq, struct ib_wc *entry, int sig);
 
 int qib_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *entry);
 
-struct ib_cq *qib_create_cq(struct ib_device *ibdev, int entries,
-			    int comp_vector, struct ib_ucontext *context,
+struct ib_cq *qib_create_cq(struct ib_device *ibdev,
+			    const struct ib_cq_init_attr *attr,
+			    struct ib_ucontext *context,
 			    struct ib_udata *udata);
 
 int qib_destroy_cq(struct ib_cq *ibcq);
diff --git a/drivers/infiniband/hw/usnic/usnic_ib_main.c b/drivers/infiniband/hw/usnic/usnic_ib_main.c
index 0d0f98695d53..34c49b8105fe 100644
--- a/drivers/infiniband/hw/usnic/usnic_ib_main.c
+++ b/drivers/infiniband/hw/usnic/usnic_ib_main.c
@@ -300,6 +300,22 @@ static struct notifier_block usnic_ib_inetaddr_notifier = {
 };
 /* End of inet section*/
 
+static int usnic_port_immutable(struct ib_device *ibdev, u8 port_num,
+			        struct ib_port_immutable *immutable)
+{
+	struct ib_port_attr attr;
+	int err;
+
+	err = usnic_ib_query_port(ibdev, port_num, &attr);
+	if (err)
+		return err;
+
+	immutable->pkey_tbl_len = attr.pkey_tbl_len;
+	immutable->gid_tbl_len = attr.gid_tbl_len;
+
+	return 0;
+}
+
 /* Start of PF discovery section */
 static void *usnic_ib_device_add(struct pci_dev *dev)
 {
@@ -383,6 +399,7 @@ static void *usnic_ib_device_add(struct pci_dev *dev)
 	us_ibdev->ib_dev.poll_cq = usnic_ib_poll_cq;
 	us_ibdev->ib_dev.req_notify_cq = usnic_ib_req_notify_cq;
 	us_ibdev->ib_dev.get_dma_mr = usnic_ib_get_dma_mr;
+	us_ibdev->ib_dev.get_port_immutable = usnic_port_immutable;
 
 
 	if (ib_register_device(&us_ibdev->ib_dev, NULL))
diff --git a/drivers/infiniband/hw/usnic/usnic_ib_verbs.c b/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
index 53bd6a2d9cdb..7df43827cb29 100644
--- a/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
+++ b/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
@@ -248,7 +248,8 @@ enum rdma_link_layer usnic_ib_port_link_layer(struct ib_device *device,
 }
 
 int usnic_ib_query_device(struct ib_device *ibdev,
-				struct ib_device_attr *props)
+			  struct ib_device_attr *props,
+			  struct ib_udata *uhw)
 {
 	struct usnic_ib_dev *us_ibdev = to_usdev(ibdev);
 	union ib_gid gid;
@@ -257,6 +258,9 @@ int usnic_ib_query_device(struct ib_device *ibdev,
 	int qp_per_vf;
 
 	usnic_dbg("\n");
+	if (uhw->inlen || uhw->outlen)
+		return -EINVAL;
+
 	mutex_lock(&us_ibdev->usdev_lock);
 	us_ibdev->netdev->ethtool_ops->get_drvinfo(us_ibdev->netdev, &info);
 	us_ibdev->netdev->ethtool_ops->get_settings(us_ibdev->netdev, &cmd);
@@ -570,13 +574,17 @@ int usnic_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 	return status;
 }
 
-struct ib_cq *usnic_ib_create_cq(struct ib_device *ibdev, int entries,
-					int vector, struct ib_ucontext *context,
-					struct ib_udata *udata)
+struct ib_cq *usnic_ib_create_cq(struct ib_device *ibdev,
+				 const struct ib_cq_init_attr *attr,
+				 struct ib_ucontext *context,
+				 struct ib_udata *udata)
 {
 	struct ib_cq *cq;
 
 	usnic_dbg("\n");
+	if (attr->flags)
+		return ERR_PTR(-EINVAL);
+
 	cq = kzalloc(sizeof(*cq), GFP_KERNEL);
 	if (!cq)
 		return ERR_PTR(-EBUSY);
diff --git a/drivers/infiniband/hw/usnic/usnic_ib_verbs.h b/drivers/infiniband/hw/usnic/usnic_ib_verbs.h
index bb864f5aed70..0bd04efa16f3 100644
--- a/drivers/infiniband/hw/usnic/usnic_ib_verbs.h
+++ b/drivers/infiniband/hw/usnic/usnic_ib_verbs.h
@@ -24,9 +24,12 @@
 enum rdma_link_layer usnic_ib_port_link_layer(struct ib_device *device,
 						u8 port_num);
 int usnic_ib_query_device(struct ib_device *ibdev,
-				struct ib_device_attr *props);
+				struct ib_device_attr *props,
+			  struct ib_udata *uhw);
 int usnic_ib_query_port(struct ib_device *ibdev, u8 port,
 				struct ib_port_attr *props);
+enum rdma_protocol_type
+usnic_ib_query_protocol(struct ib_device *device, u8 port_num);
 int usnic_ib_query_qp(struct ib_qp *qp, struct ib_qp_attr *qp_attr,
 				int qp_attr_mask,
 				struct ib_qp_init_attr *qp_init_attr);
@@ -44,9 +47,10 @@ struct ib_qp *usnic_ib_create_qp(struct ib_pd *pd,
 int usnic_ib_destroy_qp(struct ib_qp *qp);
 int usnic_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 				int attr_mask, struct ib_udata *udata);
-struct ib_cq *usnic_ib_create_cq(struct ib_device *ibdev, int entries,
-					int vector, struct ib_ucontext *context,
-					struct ib_udata *udata);
+struct ib_cq *usnic_ib_create_cq(struct ib_device *ibdev,
+				 const struct ib_cq_init_attr *attr,
+				 struct ib_ucontext *context,
+				 struct ib_udata *udata);
 int usnic_ib_destroy_cq(struct ib_cq *cq);
 struct ib_mr *usnic_ib_reg_mr(struct ib_pd *pd, u64 start, u64 length,
 				u64 virt_addr, int access_flags,
diff --git a/drivers/infiniband/hw/usnic/usnic_uiom.c b/drivers/infiniband/hw/usnic/usnic_uiom.c
index 417de1f32960..cb2337f0532b 100644
--- a/drivers/infiniband/hw/usnic/usnic_uiom.c
+++ b/drivers/infiniband/hw/usnic/usnic_uiom.c
@@ -472,11 +472,10 @@ struct usnic_uiom_pd *usnic_uiom_alloc_pd(void)
 		return ERR_PTR(-ENOMEM);
 
 	pd->domain = domain = iommu_domain_alloc(&pci_bus_type);
-	if (IS_ERR_OR_NULL(domain)) {
-		usnic_err("Failed to allocate IOMMU domain with err %ld\n",
-				PTR_ERR(pd->domain));
+	if (!domain) {
+		usnic_err("Failed to allocate IOMMU domain");
 		kfree(pd);
-		return ERR_PTR(domain ? PTR_ERR(domain) : -ENOMEM);
+		return ERR_PTR(-ENOMEM);
 	}
 
 	iommu_set_fault_handler(pd->domain, usnic_uiom_dma_fault, NULL);
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 9e1b203d756d..da149c278cb8 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -1128,7 +1128,7 @@ static int ipoib_neigh_hash_init(struct ipoib_dev_priv *priv)
 {
 	struct ipoib_neigh_table *ntbl = &priv->ntbl;
 	struct ipoib_neigh_hash *htbl;
-	struct ipoib_neigh **buckets;
+	struct ipoib_neigh __rcu **buckets;
 	u32 size;
 
 	clear_bit(IPOIB_NEIGH_TBL_FLUSH, &priv->flags);
@@ -1146,7 +1146,7 @@ static int ipoib_neigh_hash_init(struct ipoib_dev_priv *priv)
 	htbl->size = size;
 	htbl->mask = (size - 1);
 	htbl->buckets = buckets;
-	ntbl->htbl = htbl;
+	RCU_INIT_POINTER(ntbl->htbl, htbl);
 	htbl->ntbl = ntbl;
 	atomic_set(&ntbl->entries, 0);
 
@@ -1685,9 +1685,7 @@ static void ipoib_add_one(struct ib_device *device)
 	struct net_device *dev;
 	struct ipoib_dev_priv *priv;
 	int s, e, p;
-
-	if (rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
-		return;
+	int count = 0;
 
 	dev_list = kmalloc(sizeof *dev_list, GFP_KERNEL);
 	if (!dev_list)
@@ -1704,15 +1702,21 @@ static void ipoib_add_one(struct ib_device *device)
 	}
 
 	for (p = s; p <= e; ++p) {
-		if (rdma_port_get_link_layer(device, p) != IB_LINK_LAYER_INFINIBAND)
+		if (!rdma_protocol_ib(device, p))
 			continue;
 		dev = ipoib_add_port("ib%d", device, p);
 		if (!IS_ERR(dev)) {
 			priv = netdev_priv(dev);
 			list_add_tail(&priv->list, dev_list);
+			count++;
 		}
 	}
 
+	if (!count) {
+		kfree(dev_list);
+		return;
+	}
+
 	ib_set_client_data(device, &ipoib_client, dev_list);
 }
 
@@ -1721,9 +1725,6 @@ static void ipoib_remove_one(struct ib_device *device)
 	struct ipoib_dev_priv *priv, *tmp;
 	struct list_head *dev_list;
 
-	if (rdma_node_get_transport(device->node_type) != RDMA_TRANSPORT_IB)
-		return;
-
 	dev_list = ib_get_client_data(device, &ipoib_client);
 	if (!dev_list)
 		return;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_verbs.c b/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
index e5cc43074196..9e6ee82a8fd7 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_verbs.c
@@ -141,6 +141,7 @@ int ipoib_transport_dev_init(struct net_device *dev, struct ib_device *ca)
 		.sq_sig_type = IB_SIGNAL_ALL_WR,
 		.qp_type     = IB_QPT_UD
 	};
+	struct ib_cq_init_attr cq_attr = {};
 
 	int ret, size;
 	int i;
@@ -178,14 +179,17 @@ int ipoib_transport_dev_init(struct net_device *dev, struct ib_device *ca)
 	} else
 		goto out_free_wq;
 
-	priv->recv_cq = ib_create_cq(priv->ca, ipoib_ib_completion, NULL, dev, size, 0);
+	cq_attr.cqe = size;
+	priv->recv_cq = ib_create_cq(priv->ca, ipoib_ib_completion, NULL,
+				     dev, &cq_attr);
 	if (IS_ERR(priv->recv_cq)) {
 		printk(KERN_WARNING "%s: failed to create receive CQ\n", ca->name);
 		goto out_cm_dev_cleanup;
 	}
 
+	cq_attr.cqe = ipoib_sendq_size;
 	priv->send_cq = ib_create_cq(priv->ca, ipoib_send_comp_handler, NULL,
-				     dev, ipoib_sendq_size, 0);
+				     dev, &cq_attr);
 	if (IS_ERR(priv->send_cq)) {
 		printk(KERN_WARNING "%s: failed to create send CQ\n", ca->name);
 		goto out_free_recv_cq;
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index cc2dd35ffbc0..5c9f565ea0e8 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -51,19 +51,22 @@ static void iser_cq_callback(struct ib_cq *cq, void *cq_context);
 
 static void iser_cq_event_callback(struct ib_event *cause, void *context)
 {
-	iser_err("got cq event %d \n", cause->event);
+	iser_err("cq event %s (%d)\n",
+		 ib_event_msg(cause->event), cause->event);
 }
 
 static void iser_qp_event_callback(struct ib_event *cause, void *context)
 {
-	iser_err("got qp event %d\n",cause->event);
+	iser_err("qp event %s (%d)\n",
+		 ib_event_msg(cause->event), cause->event);
 }
 
 static void iser_event_handler(struct ib_event_handler *handler,
 				struct ib_event *event)
 {
-	iser_err("async event %d on device %s port %d\n", event->event,
-		event->device->name, event->element.port_num);
+	iser_err("async event %s (%d) on device %s port %d\n",
+		 ib_event_msg(event->event), event->event,
+		 event->device->name, event->element.port_num);
 }
 
 /**
@@ -123,14 +126,17 @@ static int iser_create_device_ib_res(struct iser_device *device)
 		goto pd_err;
 
 	for (i = 0; i < device->comps_used; i++) {
+		struct ib_cq_init_attr cq_attr = {};
 		struct iser_comp *comp = &device->comps[i];
 
 		comp->device = device;
+		cq_attr.cqe = max_cqe;
+		cq_attr.comp_vector = i;
 		comp->cq = ib_create_cq(device->ib_device,
 					iser_cq_callback,
 					iser_cq_event_callback,
 					(void *)comp,
-					max_cqe, i);
+					&cq_attr);
 		if (IS_ERR(comp->cq)) {
 			comp->cq = NULL;
 			goto cq_err;
@@ -873,8 +879,9 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 	int ret = 0;
 
 	iser_conn = (struct iser_conn *)cma_id->context;
-	iser_info("event %d status %d conn %p id %p\n",
-		  event->event, event->status, cma_id->context, cma_id);
+	iser_info("%s (%d): status %d conn %p id %p\n",
+		  rdma_event_msg(event->event), event->event,
+		  event->status, cma_id->context, cma_id);
 
 	mutex_lock(&iser_conn->state_mutex);
 	switch (event->event) {
@@ -913,7 +920,8 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 		}
 		break;
 	default:
-		iser_err("Unexpected RDMA CM event (%d)\n", event->event);
+		iser_err("Unexpected RDMA CM event: %s (%d)\n",
+			 rdma_event_msg(event->event), event->event);
 		break;
 	}
 	mutex_unlock(&iser_conn->state_mutex);
@@ -1173,10 +1181,13 @@ static void iser_handle_wc(struct ib_wc *wc)
 		}
 	} else {
 		if (wc->status != IB_WC_WR_FLUSH_ERR)
-			iser_err("wr id %llx status %d vend_err %x\n",
-				 wc->wr_id, wc->status, wc->vendor_err);
+			iser_err("%s (%d): wr id %llx vend_err %x\n",
+				 ib_wc_status_msg(wc->status), wc->status,
+				 wc->wr_id, wc->vendor_err);
 		else
-			iser_dbg("flush error: wr id %llx\n", wc->wr_id);
+			iser_dbg("%s (%d): wr id %llx\n",
+				 ib_wc_status_msg(wc->status), wc->status,
+				 wc->wr_id);
 
 		if (wc->wr_id == ISER_BEACON_WRID)
 			/* all flush errors were consumed */
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
index 3f40319a55da..771700963127 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.c
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -65,6 +65,8 @@ static int
 isert_rdma_accept(struct isert_conn *isert_conn);
 struct rdma_cm_id *isert_setup_id(struct isert_np *isert_np);
 
+static void isert_release_work(struct work_struct *work);
+
 static inline bool
 isert_prot_cmd(struct isert_conn *conn, struct se_cmd *cmd)
 {
@@ -78,7 +80,9 @@ isert_qp_event_callback(struct ib_event *e, void *context)
 {
 	struct isert_conn *isert_conn = context;
 
-	isert_err("conn %p event: %d\n", isert_conn, e->event);
+	isert_err("%s (%d): conn %p\n",
+		  ib_event_msg(e->event), e->event, isert_conn);
+
 	switch (e->event) {
 	case IB_EVENT_COMM_EST:
 		rdma_notify(isert_conn->cm_id, IB_EVENT_COMM_EST);
@@ -316,15 +320,18 @@ isert_alloc_comps(struct isert_device *device,
 	max_cqe = min(ISER_MAX_CQ_LEN, attr->max_cqe);
 
 	for (i = 0; i < device->comps_used; i++) {
+		struct ib_cq_init_attr cq_attr = {};
 		struct isert_comp *comp = &device->comps[i];
 
 		comp->device = device;
 		INIT_WORK(&comp->work, isert_cq_work);
+		cq_attr.cqe = max_cqe;
+		cq_attr.comp_vector = i;
 		comp->cq = ib_create_cq(device->ib_device,
 					isert_cq_callback,
 					isert_cq_event_callback,
 					(void *)comp,
-					max_cqe, i);
+					&cq_attr);
 		if (IS_ERR(comp->cq)) {
 			isert_err("Unable to allocate cq\n");
 			ret = PTR_ERR(comp->cq);
@@ -648,6 +655,7 @@ isert_init_conn(struct isert_conn *isert_conn)
 	mutex_init(&isert_conn->mutex);
 	spin_lock_init(&isert_conn->pool_lock);
 	INIT_LIST_HEAD(&isert_conn->fr_pool);
+	INIT_WORK(&isert_conn->release_work, isert_release_work);
 }
 
 static void
@@ -897,7 +905,8 @@ 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);
+	isert_dbg("%s (%d): isert np %p\n",
+		  rdma_event_msg(event), event, isert_np);
 
 	switch (event) {
 	case RDMA_CM_EVENT_DEVICE_REMOVAL:
@@ -925,6 +934,7 @@ isert_disconnected_handler(struct rdma_cm_id *cma_id,
 {
 	struct isert_np *isert_np = cma_id->context;
 	struct isert_conn *isert_conn;
+	bool terminating = false;
 
 	if (isert_np->np_cm_id == cma_id)
 		return isert_np_cma_handler(cma_id->context, event);
@@ -932,12 +942,25 @@ isert_disconnected_handler(struct rdma_cm_id *cma_id,
 	isert_conn = cma_id->qp->qp_context;
 
 	mutex_lock(&isert_conn->mutex);
+	terminating = (isert_conn->state == ISER_CONN_TERMINATING);
 	isert_conn_terminate(isert_conn);
 	mutex_unlock(&isert_conn->mutex);
 
 	isert_info("conn %p completing wait\n", isert_conn);
 	complete(&isert_conn->wait);
 
+	if (terminating)
+		goto out;
+
+	mutex_lock(&isert_np->np_accept_mutex);
+	if (!list_empty(&isert_conn->accept_node)) {
+		list_del_init(&isert_conn->accept_node);
+		isert_put_conn(isert_conn);
+		queue_work(isert_release_wq, &isert_conn->release_work);
+	}
+	mutex_unlock(&isert_np->np_accept_mutex);
+
+out:
 	return 0;
 }
 
@@ -957,7 +980,8 @@ isert_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 {
 	int ret = 0;
 
-	isert_info("event %d status %d id %p np %p\n", event->event,
+	isert_info("%s (%d): status %d id %p np %p\n",
+		   rdma_event_msg(event->event), event->event,
 		   event->status, cma_id, cma_id->context);
 
 	switch (event->event) {
@@ -1332,7 +1356,7 @@ sequence_cmd:
 	if (!rc && dump_payload == false && unsol_data)
 		iscsit_set_unsoliticed_dataout(cmd);
 	else if (dump_payload && imm_data)
-		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+		target_put_sess_cmd(&cmd->se_cmd);
 
 	return 0;
 }
@@ -1757,7 +1781,7 @@ isert_put_cmd(struct isert_cmd *isert_cmd, bool comp_err)
 			    cmd->se_cmd.t_state == TRANSPORT_WRITE_PENDING) {
 				struct se_cmd *se_cmd = &cmd->se_cmd;
 
-				target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+				target_put_sess_cmd(se_cmd);
 			}
 		}
 
@@ -1930,7 +1954,7 @@ isert_completion_rdma_read(struct iser_tx_desc *tx_desc,
 	spin_unlock_bh(&cmd->istate_lock);
 
 	if (ret) {
-		target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+		target_put_sess_cmd(se_cmd);
 		transport_send_check_condition_and_sense(se_cmd,
 							 se_cmd->pi_err, 0);
 	} else {
@@ -2091,10 +2115,13 @@ isert_handle_wc(struct ib_wc *wc)
 		}
 	} 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);
+			isert_err("%s (%d): wr id %llx vend_err %x\n",
+				  ib_wc_status_msg(wc->status), wc->status,
+				  wc->wr_id, wc->vendor_err);
 		else
-			isert_dbg("flush error: wr id %llx\n", wc->wr_id);
+			isert_dbg("%s (%d): wr id %llx\n",
+				  ib_wc_status_msg(wc->status), wc->status,
+				  wc->wr_id);
 
 		if (wc->wr_id != ISER_FASTREG_LI_WRID)
 			isert_cq_comp_err(isert_conn, wc);
@@ -2380,7 +2407,6 @@ isert_build_rdma_wr(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd,
 	page_off = offset % PAGE_SIZE;
 
 	send_wr->sg_list = ib_sge;
-	send_wr->num_sge = sg_nents;
 	send_wr->wr_id = (uintptr_t)&isert_cmd->tx_desc;
 	/*
 	 * Perform mapping of TCM scatterlist memory ib_sge dma_addr.
@@ -2400,14 +2426,17 @@ isert_build_rdma_wr(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd,
 			  ib_sge->addr, ib_sge->length, ib_sge->lkey);
 		page_off = 0;
 		data_left -= ib_sge->length;
+		if (!data_left)
+			break;
 		ib_sge++;
 		isert_dbg("Incrementing ib_sge pointer to %p\n", ib_sge);
 	}
 
+	send_wr->num_sge = ++i;
 	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;
+	return send_wr->num_sge;
 }
 
 static int
@@ -3366,7 +3395,6 @@ static void isert_wait_conn(struct iscsi_conn *conn)
 	isert_wait4flush(isert_conn);
 	isert_wait4logout(isert_conn);
 
-	INIT_WORK(&isert_conn->release_work, isert_release_work);
 	queue_work(isert_release_wq, &isert_conn->release_work);
 }
 
@@ -3374,6 +3402,7 @@ static void isert_free_conn(struct iscsi_conn *conn)
 {
 	struct isert_conn *isert_conn = conn->context;
 
+	isert_wait4flush(isert_conn);
 	isert_put_conn(isert_conn);
 }
 
diff --git a/drivers/infiniband/ulp/srp/ib_srp.c b/drivers/infiniband/ulp/srp/ib_srp.c
index 918814cd0f80..267dc4f75502 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.c
+++ b/drivers/infiniband/ulp/srp/ib_srp.c
@@ -59,9 +59,10 @@
 #define DRV_RELDATE	"July 1, 2013"
 
 MODULE_AUTHOR("Roland Dreier");
-MODULE_DESCRIPTION("InfiniBand SCSI RDMA Protocol initiator "
-		   "v" DRV_VERSION " (" DRV_RELDATE ")");
+MODULE_DESCRIPTION("InfiniBand SCSI RDMA Protocol initiator");
 MODULE_LICENSE("Dual BSD/GPL");
+MODULE_VERSION(DRV_VERSION);
+MODULE_INFO(release_date, DRV_RELDATE);
 
 static unsigned int srp_sg_tablesize;
 static unsigned int cmd_sg_entries;
@@ -98,7 +99,7 @@ module_param(register_always, bool, 0444);
 MODULE_PARM_DESC(register_always,
 		 "Use memory registration even for contiguous memory regions");
 
-static struct kernel_param_ops srp_tmo_ops;
+static const struct kernel_param_ops srp_tmo_ops;
 
 static int srp_reconnect_delay = 10;
 module_param_cb(reconnect_delay, &srp_tmo_ops, &srp_reconnect_delay,
@@ -183,7 +184,7 @@ out:
 	return res;
 }
 
-static struct kernel_param_ops srp_tmo_ops = {
+static const struct kernel_param_ops srp_tmo_ops = {
 	.get = srp_tmo_get,
 	.set = srp_tmo_set,
 };
@@ -253,7 +254,8 @@ static void srp_free_iu(struct srp_host *host, struct srp_iu *iu)
 
 static void srp_qp_event(struct ib_event *event, void *context)
 {
-	pr_debug("QP event %d\n", event->event);
+	pr_debug("QP event %s (%d)\n",
+		 ib_event_msg(event->event), event->event);
 }
 
 static int srp_init_qp(struct srp_target_port *target,
@@ -465,14 +467,13 @@ static struct srp_fr_pool *srp_alloc_fr_pool(struct srp_target_port *target)
  */
 static void srp_destroy_qp(struct srp_rdma_ch *ch)
 {
-	struct srp_target_port *target = ch->target;
 	static struct ib_qp_attr attr = { .qp_state = IB_QPS_ERR };
 	static struct ib_recv_wr wr = { .wr_id = SRP_LAST_WR_ID };
 	struct ib_recv_wr *bad_wr;
 	int ret;
 
 	/* Destroying a QP and reusing ch->done is only safe if not connected */
-	WARN_ON_ONCE(target->connected);
+	WARN_ON_ONCE(ch->connected);
 
 	ret = ib_modify_qp(ch->qp, &attr, IB_QP_STATE);
 	WARN_ONCE(ret, "ib_cm_init_qp_attr() returned %d\n", ret);
@@ -499,6 +500,7 @@ static int srp_create_ch_ib(struct srp_rdma_ch *ch)
 	struct ib_fmr_pool *fmr_pool = NULL;
 	struct srp_fr_pool *fr_pool = NULL;
 	const int m = 1 + dev->use_fast_reg;
+	struct ib_cq_init_attr cq_attr = {};
 	int ret;
 
 	init_attr = kzalloc(sizeof *init_attr, GFP_KERNEL);
@@ -506,15 +508,19 @@ static int srp_create_ch_ib(struct srp_rdma_ch *ch)
 		return -ENOMEM;
 
 	/* + 1 for SRP_LAST_WR_ID */
+	cq_attr.cqe = target->queue_size + 1;
+	cq_attr.comp_vector = ch->comp_vector;
 	recv_cq = ib_create_cq(dev->dev, srp_recv_completion, NULL, ch,
-			       target->queue_size + 1, ch->comp_vector);
+			       &cq_attr);
 	if (IS_ERR(recv_cq)) {
 		ret = PTR_ERR(recv_cq);
 		goto err;
 	}
 
+	cq_attr.cqe = m * target->queue_size;
+	cq_attr.comp_vector = ch->comp_vector;
 	send_cq = ib_create_cq(dev->dev, srp_send_completion, NULL, ch,
-			       m * target->queue_size, ch->comp_vector);
+			       &cq_attr);
 	if (IS_ERR(send_cq)) {
 		ret = PTR_ERR(send_cq);
 		goto err_recv_cq;
@@ -781,7 +787,7 @@ static int srp_send_req(struct srp_rdma_ch *ch, bool multich)
 		shost_printk(KERN_DEBUG, target->scsi_host,
 			     PFX "Topspin/Cisco initiator port ID workaround "
 			     "activated for target GUID %016llx\n",
-			     (unsigned long long) be64_to_cpu(target->ioc_guid));
+			     be64_to_cpu(target->ioc_guid));
 		memset(req->priv.initiator_port_id, 0, 8);
 		memcpy(req->priv.initiator_port_id + 8,
 		       &target->srp_host->srp_dev->dev->node_guid, 8);
@@ -811,35 +817,19 @@ static bool srp_queue_remove_work(struct srp_target_port *target)
 	return changed;
 }
 
-static bool srp_change_conn_state(struct srp_target_port *target,
-				  bool connected)
-{
-	bool changed = false;
-
-	spin_lock_irq(&target->lock);
-	if (target->connected != connected) {
-		target->connected = connected;
-		changed = true;
-	}
-	spin_unlock_irq(&target->lock);
-
-	return changed;
-}
-
 static void srp_disconnect_target(struct srp_target_port *target)
 {
 	struct srp_rdma_ch *ch;
 	int i;
 
-	if (srp_change_conn_state(target, false)) {
-		/* XXX should send SRP_I_LOGOUT request */
+	/* XXX should send SRP_I_LOGOUT request */
 
-		for (i = 0; i < target->ch_count; i++) {
-			ch = &target->ch[i];
-			if (ch->cm_id && ib_send_cm_dreq(ch->cm_id, NULL, 0)) {
-				shost_printk(KERN_DEBUG, target->scsi_host,
-					     PFX "Sending CM DREQ failed\n");
-			}
+	for (i = 0; i < target->ch_count; i++) {
+		ch = &target->ch[i];
+		ch->connected = false;
+		if (ch->cm_id && ib_send_cm_dreq(ch->cm_id, NULL, 0)) {
+			shost_printk(KERN_DEBUG, target->scsi_host,
+				     PFX "Sending CM DREQ failed\n");
 		}
 	}
 }
@@ -852,7 +842,7 @@ static void srp_free_req_data(struct srp_target_port *target,
 	struct srp_request *req;
 	int i;
 
-	if (!ch->target || !ch->req_ring)
+	if (!ch->req_ring)
 		return;
 
 	for (i = 0; i < target->req_ring_size; ++i) {
@@ -986,14 +976,26 @@ static void srp_rport_delete(struct srp_rport *rport)
 	srp_queue_remove_work(target);
 }
 
+/**
+ * srp_connected_ch() - number of connected channels
+ * @target: SRP target port.
+ */
+static int srp_connected_ch(struct srp_target_port *target)
+{
+	int i, c = 0;
+
+	for (i = 0; i < target->ch_count; i++)
+		c += target->ch[i].connected;
+
+	return c;
+}
+
 static int srp_connect_ch(struct srp_rdma_ch *ch, bool multich)
 {
 	struct srp_target_port *target = ch->target;
 	int ret;
 
-	WARN_ON_ONCE(!multich && target->connected);
-
-	target->qp_in_error = false;
+	WARN_ON_ONCE(!multich && srp_connected_ch(target) > 0);
 
 	ret = srp_lookup_path(ch);
 	if (ret)
@@ -1016,7 +1018,7 @@ static int srp_connect_ch(struct srp_rdma_ch *ch, bool multich)
 		 */
 		switch (ch->status) {
 		case 0:
-			srp_change_conn_state(target, true);
+			ch->connected = true;
 			return 0;
 
 		case SRP_PORT_REDIRECT:
@@ -1214,14 +1216,10 @@ static int srp_rport_reconnect(struct srp_rport *rport)
 	 */
 	for (i = 0; i < target->ch_count; i++) {
 		ch = &target->ch[i];
-		if (!ch->target)
-			break;
 		ret += srp_new_cm_id(ch);
 	}
 	for (i = 0; i < target->ch_count; i++) {
 		ch = &target->ch[i];
-		if (!ch->target)
-			break;
 		for (j = 0; j < target->req_ring_size; ++j) {
 			struct srp_request *req = &ch->req_ring[j];
 
@@ -1230,8 +1228,6 @@ static int srp_rport_reconnect(struct srp_rport *rport)
 	}
 	for (i = 0; i < target->ch_count; i++) {
 		ch = &target->ch[i];
-		if (!ch->target)
-			break;
 		/*
 		 * Whether or not creating a new CM ID succeeded, create a new
 		 * QP. This guarantees that all completion callback function
@@ -1243,13 +1239,13 @@ static int srp_rport_reconnect(struct srp_rport *rport)
 		for (j = 0; j < target->queue_size; ++j)
 			list_add(&ch->tx_ring[j]->list, &ch->free_tx);
 	}
+
+	target->qp_in_error = false;
+
 	for (i = 0; i < target->ch_count; i++) {
 		ch = &target->ch[i];
-		if (ret || !ch->target) {
-			if (i > 1)
-				ret = 0;
+		if (ret)
 			break;
-		}
 		ret = srp_connect_ch(ch, multich);
 		multich = true;
 	}
@@ -1842,7 +1838,7 @@ static void srp_process_aer_req(struct srp_rdma_ch *ch,
 	s32 delta = be32_to_cpu(req->req_lim_delta);
 
 	shost_printk(KERN_ERR, target->scsi_host, PFX
-		     "ignoring AER for LUN %llu\n", be64_to_cpu(req->lun));
+		     "ignoring AER for LUN %llu\n", scsilun_to_int(&req->lun));
 
 	if (srp_response_common(ch, delta, &rsp, sizeof(rsp)))
 		shost_printk(KERN_ERR, target->scsi_host, PFX
@@ -1929,20 +1925,21 @@ static void srp_handle_qp_err(u64 wr_id, enum ib_wc_status wc_status,
 		return;
 	}
 
-	if (target->connected && !target->qp_in_error) {
+	if (ch->connected && !target->qp_in_error) {
 		if (wr_id & LOCAL_INV_WR_ID_MASK) {
 			shost_printk(KERN_ERR, target->scsi_host, PFX
-				     "LOCAL_INV failed with status %d\n",
-				     wc_status);
+				     "LOCAL_INV failed with status %s (%d)\n",
+				     ib_wc_status_msg(wc_status), wc_status);
 		} else if (wr_id & FAST_REG_WR_ID_MASK) {
 			shost_printk(KERN_ERR, target->scsi_host, PFX
-				     "FAST_REG_MR failed status %d\n",
-				     wc_status);
+				     "FAST_REG_MR failed status %s (%d)\n",
+				     ib_wc_status_msg(wc_status), wc_status);
 		} else {
 			shost_printk(KERN_ERR, target->scsi_host,
-				     PFX "failed %s status %d for iu %p\n",
+				     PFX "failed %s status %s (%d) for iu %p\n",
 				     send_err ? "send" : "receive",
-				     wc_status, (void *)(uintptr_t)wr_id);
+				     ib_wc_status_msg(wc_status), wc_status,
+				     (void *)(uintptr_t)wr_id);
 		}
 		queue_work(system_long_wq, &target->tl_err_work);
 	}
@@ -2034,7 +2031,7 @@ static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
 	memset(cmd, 0, sizeof *cmd);
 
 	cmd->opcode = SRP_CMD;
-	cmd->lun    = cpu_to_be64((u64) scmnd->device->lun << 48);
+	int_to_scsilun(scmnd->device->lun, &cmd->lun);
 	cmd->tag    = tag;
 	memcpy(cmd->cdb, scmnd->cmnd, scmnd->cmd_len);
 
@@ -2367,7 +2364,7 @@ static int srp_cm_handler(struct ib_cm_id *cm_id, struct ib_cm_event *event)
 	case IB_CM_DREQ_RECEIVED:
 		shost_printk(KERN_WARNING, target->scsi_host,
 			     PFX "DREQ received - connection closed\n");
-		srp_change_conn_state(target, false);
+		ch->connected = false;
 		if (ib_send_cm_drep(cm_id, NULL, 0))
 			shost_printk(KERN_ERR, target->scsi_host,
 				     PFX "Sending CM DREP failed\n");
@@ -2414,8 +2411,8 @@ srp_change_queue_depth(struct scsi_device *sdev, int qdepth)
 	return scsi_change_queue_depth(sdev, qdepth);
 }
 
-static int srp_send_tsk_mgmt(struct srp_rdma_ch *ch, u64 req_tag,
-			     unsigned int lun, u8 func)
+static int srp_send_tsk_mgmt(struct srp_rdma_ch *ch, u64 req_tag, u64 lun,
+			     u8 func)
 {
 	struct srp_target_port *target = ch->target;
 	struct srp_rport *rport = target->rport;
@@ -2423,7 +2420,7 @@ static int srp_send_tsk_mgmt(struct srp_rdma_ch *ch, u64 req_tag,
 	struct srp_iu *iu;
 	struct srp_tsk_mgmt *tsk_mgmt;
 
-	if (!target->connected || target->qp_in_error)
+	if (!ch->connected || target->qp_in_error)
 		return -1;
 
 	init_completion(&ch->tsk_mgmt_done);
@@ -2449,7 +2446,7 @@ static int srp_send_tsk_mgmt(struct srp_rdma_ch *ch, u64 req_tag,
 	memset(tsk_mgmt, 0, sizeof *tsk_mgmt);
 
 	tsk_mgmt->opcode 	= SRP_TSK_MGMT;
-	tsk_mgmt->lun		= cpu_to_be64((u64) lun << 48);
+	int_to_scsilun(lun, &tsk_mgmt->lun);
 	tsk_mgmt->tag		= req_tag | SRP_TAG_TSK_MGMT;
 	tsk_mgmt->tsk_mgmt_func = func;
 	tsk_mgmt->task_tag	= req_tag;
@@ -2563,8 +2560,7 @@ static ssize_t show_id_ext(struct device *dev, struct device_attribute *attr,
 {
 	struct srp_target_port *target = host_to_target(class_to_shost(dev));
 
-	return sprintf(buf, "0x%016llx\n",
-		       (unsigned long long) be64_to_cpu(target->id_ext));
+	return sprintf(buf, "0x%016llx\n", be64_to_cpu(target->id_ext));
 }
 
 static ssize_t show_ioc_guid(struct device *dev, struct device_attribute *attr,
@@ -2572,8 +2568,7 @@ static ssize_t show_ioc_guid(struct device *dev, struct device_attribute *attr,
 {
 	struct srp_target_port *target = host_to_target(class_to_shost(dev));
 
-	return sprintf(buf, "0x%016llx\n",
-		       (unsigned long long) be64_to_cpu(target->ioc_guid));
+	return sprintf(buf, "0x%016llx\n", be64_to_cpu(target->ioc_guid));
 }
 
 static ssize_t show_service_id(struct device *dev,
@@ -2581,8 +2576,7 @@ static ssize_t show_service_id(struct device *dev,
 {
 	struct srp_target_port *target = host_to_target(class_to_shost(dev));
 
-	return sprintf(buf, "0x%016llx\n",
-		       (unsigned long long) be64_to_cpu(target->service_id));
+	return sprintf(buf, "0x%016llx\n", be64_to_cpu(target->service_id));
 }
 
 static ssize_t show_pkey(struct device *dev, struct device_attribute *attr,
@@ -2773,7 +2767,7 @@ static int srp_add_target(struct srp_host *host, struct srp_target_port *target)
 
 	target->state = SRP_TARGET_SCANNING;
 	sprintf(target->target_name, "SRP.T10:%016llX",
-		 (unsigned long long) be64_to_cpu(target->id_ext));
+		be64_to_cpu(target->id_ext));
 
 	if (scsi_add_host(target->scsi_host, host->srp_dev->dev->dma_device))
 		return -ENODEV;
@@ -2797,7 +2791,8 @@ static int srp_add_target(struct srp_host *host, struct srp_target_port *target)
 	scsi_scan_target(&target->scsi_host->shost_gendev,
 			 0, target->scsi_id, SCAN_WILD_CARD, 0);
 
-	if (!target->connected || target->qp_in_error) {
+	if (srp_connected_ch(target) < target->ch_count ||
+	    target->qp_in_error) {
 		shost_printk(KERN_INFO, target->scsi_host,
 			     PFX "SCSI scan failed - removing SCSI host\n");
 		srp_queue_remove_work(target);
@@ -3146,7 +3141,7 @@ static ssize_t srp_create_target(struct device *dev,
 	target_host->transportt  = ib_srp_transport_template;
 	target_host->max_channel = 0;
 	target_host->max_id      = 1;
-	target_host->max_lun     = SRP_MAX_LUN;
+	target_host->max_lun     = -1LL;
 	target_host->max_cmd_len = sizeof ((struct srp_cmd *) (void *) 0L)->cdb;
 
 	target = host_to_target(target_host);
@@ -3172,11 +3167,11 @@ static ssize_t srp_create_target(struct device *dev,
 
 	ret = srp_parse_options(buf, target);
 	if (ret)
-		goto err;
+		goto out;
 
 	ret = scsi_init_shared_tag_map(target_host, target_host->can_queue);
 	if (ret)
-		goto err;
+		goto out;
 
 	target->req_ring_size = target->queue_size - SRP_TSK_MGMT_SQ_SIZE;
 
@@ -3187,7 +3182,7 @@ static ssize_t srp_create_target(struct device *dev,
 			     be64_to_cpu(target->ioc_guid),
 			     be64_to_cpu(target->initiator_ext));
 		ret = -EEXIST;
-		goto err;
+		goto out;
 	}
 
 	if (!srp_dev->has_fmr && !srp_dev->has_fr && !target->allow_ext_sg &&
@@ -3208,7 +3203,7 @@ static ssize_t srp_create_target(struct device *dev,
 	spin_lock_init(&target->lock);
 	ret = ib_query_gid(ibdev, host->port, 0, &target->sgid);
 	if (ret)
-		goto err;
+		goto out;
 
 	ret = -ENOMEM;
 	target->ch_count = max_t(unsigned, num_online_nodes(),
@@ -3219,7 +3214,7 @@ static ssize_t srp_create_target(struct device *dev,
 	target->ch = kcalloc(target->ch_count, sizeof(*target->ch),
 			     GFP_KERNEL);
 	if (!target->ch)
-		goto err;
+		goto out;
 
 	node_idx = 0;
 	for_each_online_node(node) {
@@ -3315,9 +3310,6 @@ err_disconnect:
 	}
 
 	kfree(target->ch);
-
-err:
-	scsi_host_put(target_host);
 	goto out;
 }
 
diff --git a/drivers/infiniband/ulp/srp/ib_srp.h b/drivers/infiniband/ulp/srp/ib_srp.h
index a611556406ac..17ee3f80ba55 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.h
+++ b/drivers/infiniband/ulp/srp/ib_srp.h
@@ -54,7 +54,6 @@ enum {
 	SRP_DLID_REDIRECT	= 2,
 	SRP_STALE_CONN		= 3,
 
-	SRP_MAX_LUN		= 512,
 	SRP_DEF_SG_TABLESIZE	= 12,
 
 	SRP_DEFAULT_QUEUE_SIZE	= 1 << 6,
@@ -170,6 +169,7 @@ struct srp_rdma_ch {
 
 	struct completion	tsk_mgmt_done;
 	u8			tsk_mgmt_status;
+	bool			connected;
 };
 
 /**
@@ -214,7 +214,6 @@ struct srp_target_port {
 	__be16			pkey;
 
 	u32			rq_tmo_jiffies;
-	bool			connected;
 
 	int			zero_req_lim;
 
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index 9b84b4c0a000..82897ca17f32 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -41,12 +41,12 @@
 #include <linux/string.h>
 #include <linux/delay.h>
 #include <linux/atomic.h>
+#include <scsi/scsi_proto.h>
 #include <scsi/scsi_tcq.h>
 #include <target/configfs_macros.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric_configfs.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 #include "ib_srpt.h"
 
 /* Name of this kernel module. */
@@ -93,7 +93,6 @@ MODULE_PARM_DESC(srpt_service_guid,
 		 " instead of using the node_guid of the first HCA.");
 
 static struct ib_client srpt_client;
-static const struct target_core_fabric_ops srpt_template;
 static void srpt_release_channel(struct srpt_rdma_ch *ch);
 static int srpt_queue_status(struct se_cmd *cmd);
 
@@ -476,7 +475,8 @@ static void srpt_mad_recv_handler(struct ib_mad_agent *mad_agent,
 	rsp = ib_create_send_mad(mad_agent, mad_wc->wc->src_qp,
 				 mad_wc->wc->pkey_index, 0,
 				 IB_MGMT_DEVICE_HDR, IB_MGMT_DEVICE_DATA,
-				 GFP_KERNEL);
+				 GFP_KERNEL,
+				 IB_MGMT_BASE_VERSION);
 	if (IS_ERR(rsp))
 		goto err_rsp;
 
@@ -1334,12 +1334,12 @@ static int srpt_abort_cmd(struct srpt_send_ioctx *ioctx)
 
 		BUG_ON(ch->sess == NULL);
 
-		target_put_sess_cmd(ch->sess, &ioctx->cmd);
+		target_put_sess_cmd(&ioctx->cmd);
 		goto out;
 	}
 
 	pr_debug("Aborting cmd with state %d and tag %lld\n", state,
-		 ioctx->tag);
+		 ioctx->cmd.tag);
 
 	switch (state) {
 	case SRPT_STATE_NEW:
@@ -1365,11 +1365,11 @@ static int srpt_abort_cmd(struct srpt_send_ioctx *ioctx)
 		 * not been received in time.
 		 */
 		srpt_unmap_sg_to_ib_sge(ioctx->ch, ioctx);
-		target_put_sess_cmd(ioctx->ch->sess, &ioctx->cmd);
+		target_put_sess_cmd(&ioctx->cmd);
 		break;
 	case SRPT_STATE_MGMT_RSP_SENT:
 		srpt_set_cmd_state(ioctx, SRPT_STATE_DONE);
-		target_put_sess_cmd(ioctx->ch->sess, &ioctx->cmd);
+		target_put_sess_cmd(&ioctx->cmd);
 		break;
 	default:
 		WARN(1, "Unexpected command state (%d)", state);
@@ -1387,7 +1387,6 @@ static void srpt_handle_send_err_comp(struct srpt_rdma_ch *ch, u64 wr_id)
 {
 	struct srpt_send_ioctx *ioctx;
 	enum srpt_command_state state;
-	struct se_cmd *cmd;
 	u32 index;
 
 	atomic_inc(&ch->sq_wr_avail);
@@ -1395,7 +1394,6 @@ static void srpt_handle_send_err_comp(struct srpt_rdma_ch *ch, u64 wr_id)
 	index = idx_from_wr_id(wr_id);
 	ioctx = ch->ioctx_ring[index];
 	state = srpt_get_cmd_state(ioctx);
-	cmd = &ioctx->cmd;
 
 	WARN_ON(state != SRPT_STATE_CMD_RSP_SENT
 		&& state != SRPT_STATE_MGMT_RSP_SENT
@@ -1472,10 +1470,8 @@ static void srpt_handle_rdma_err_comp(struct srpt_rdma_ch *ch,
 				      struct srpt_send_ioctx *ioctx,
 				      enum srpt_opcode opcode)
 {
-	struct se_cmd *cmd;
 	enum srpt_command_state state;
 
-	cmd = &ioctx->cmd;
 	state = srpt_get_cmd_state(ioctx);
 	switch (opcode) {
 	case SRPT_RDMA_READ_LAST:
@@ -1679,7 +1675,7 @@ static int srpt_check_stop_free(struct se_cmd *cmd)
 	struct srpt_send_ioctx *ioctx = container_of(cmd,
 				struct srpt_send_ioctx, cmd);
 
-	return target_put_sess_cmd(ioctx->ch->sess, &ioctx->cmd);
+	return target_put_sess_cmd(&ioctx->cmd);
 }
 
 /**
@@ -1701,7 +1697,7 @@ static int srpt_handle_cmd(struct srpt_rdma_ch *ch,
 
 	srp_cmd = recv_ioctx->ioctx.buf;
 	cmd = &send_ioctx->cmd;
-	send_ioctx->tag = srp_cmd->tag;
+	cmd->tag = srp_cmd->tag;
 
 	switch (srp_cmd->task_attr) {
 	case SRP_CMD_SIMPLE_Q:
@@ -1772,7 +1768,7 @@ static int srpt_rx_mgmt_fn_tag(struct srpt_send_ioctx *ioctx, u64 tag)
 	for (i = 0; i < ch->rq_size; ++i) {
 		target = ch->ioctx_ring[i];
 		if (target->cmd.se_lun == ioctx->cmd.se_lun &&
-		    target->tag == tag &&
+		    target->cmd.tag == tag &&
 		    srpt_get_cmd_state(target) != SRPT_STATE_DONE) {
 			ret = 0;
 			/* now let the target core abort &target->cmd; */
@@ -1831,7 +1827,7 @@ static void srpt_handle_tsk_mgmt(struct srpt_rdma_ch *ch,
 		 srp_tsk->task_tag, srp_tsk->tag, ch->cm_id, ch->sess);
 
 	srpt_set_cmd_state(send_ioctx, SRPT_STATE_MGMT);
-	send_ioctx->tag = srp_tsk->tag;
+	send_ioctx->cmd.tag = srp_tsk->tag;
 	tcm_tmr = srp_tmr_to_tcm(srp_tsk->tsk_mgmt_func);
 	if (tcm_tmr < 0) {
 		send_ioctx->cmd.se_tmr_req->response =
@@ -2080,6 +2076,7 @@ static int srpt_create_ch_ib(struct srpt_rdma_ch *ch)
 	struct srpt_port *sport = ch->sport;
 	struct srpt_device *sdev = sport->sdev;
 	u32 srp_sq_size = sport->port_attrib.srp_sq_size;
+	struct ib_cq_init_attr cq_attr = {};
 	int ret;
 
 	WARN_ON(ch->rq_size < 1);
@@ -2090,8 +2087,9 @@ static int srpt_create_ch_ib(struct srpt_rdma_ch *ch)
 		goto out;
 
 retry:
+	cq_attr.cqe = ch->rq_size + srp_sq_size;
 	ch->cq = ib_create_cq(sdev->device, srpt_completion, NULL, ch,
-			      ch->rq_size + srp_sq_size, 0);
+			      &cq_attr);
 	if (IS_ERR(ch->cq)) {
 		ret = PTR_ERR(ch->cq);
 		pr_err("failed to create CQ cqe= %d ret= %d\n",
@@ -2176,12 +2174,9 @@ static void srpt_destroy_ch_ib(struct srpt_rdma_ch *ch)
  */
 static void __srpt_close_ch(struct srpt_rdma_ch *ch)
 {
-	struct srpt_device *sdev;
 	enum rdma_ch_state prev_state;
 	unsigned long flags;
 
-	sdev = ch->sport->sdev;
-
 	spin_lock_irqsave(&ch->spinlock, flags);
 	prev_state = ch->state;
 	switch (prev_state) {
@@ -2979,7 +2974,7 @@ static int srpt_write_pending(struct se_cmd *se_cmd)
 	case CH_DRAINING:
 	case CH_RELEASING:
 		pr_debug("cmd with tag %lld: channel disconnecting\n",
-			 ioctx->tag);
+			 ioctx->cmd.tag);
 		srpt_set_cmd_state(ioctx, SRPT_STATE_DATA_IN);
 		ret = -EINVAL;
 		goto out;
@@ -3054,27 +3049,27 @@ static void srpt_queue_response(struct se_cmd *cmd)
 		ret = srpt_xfer_data(ch, ioctx);
 		if (ret) {
 			pr_err("xfer_data failed for tag %llu\n",
-			       ioctx->tag);
+			       ioctx->cmd.tag);
 			return;
 		}
 	}
 
 	if (state != SRPT_STATE_MGMT)
-		resp_len = srpt_build_cmd_rsp(ch, ioctx, ioctx->tag,
+		resp_len = srpt_build_cmd_rsp(ch, ioctx, ioctx->cmd.tag,
 					      cmd->scsi_status);
 	else {
 		srp_tm_status
 			= tcm_to_srp_tsk_mgmt_status(cmd->se_tmr_req->response);
 		resp_len = srpt_build_tskmgmt_rsp(ch, ioctx, srp_tm_status,
-						 ioctx->tag);
+						 ioctx->cmd.tag);
 	}
 	ret = srpt_post_send(ch, ioctx, resp_len);
 	if (ret) {
 		pr_err("sending cmd response failed for tag %llu\n",
-		       ioctx->tag);
+		       ioctx->cmd.tag);
 		srpt_unmap_sg_to_ib_sge(ch, ioctx);
 		srpt_set_cmd_state(ioctx, SRPT_STATE_DONE);
-		target_put_sess_cmd(ioctx->ch->sess, &ioctx->cmd);
+		target_put_sess_cmd(&ioctx->cmd);
 	}
 }
 
@@ -3394,11 +3389,6 @@ static char *srpt_get_fabric_name(void)
 	return "srpt";
 }
 
-static u8 srpt_get_fabric_proto_ident(struct se_portal_group *se_tpg)
-{
-	return SCSI_TRANSPORTID_PROTOCOLID_SRP;
-}
-
 static char *srpt_get_fabric_wwn(struct se_portal_group *tpg)
 {
 	struct srpt_port *sport = container_of(tpg, struct srpt_port, port_tpg_1);
@@ -3411,69 +3401,6 @@ static u16 srpt_get_tag(struct se_portal_group *tpg)
 	return 1;
 }
 
-static u32 srpt_get_default_depth(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static u32 srpt_get_pr_transport_id(struct se_portal_group *se_tpg,
-				    struct se_node_acl *se_nacl,
-				    struct t10_pr_registration *pr_reg,
-				    int *format_code, unsigned char *buf)
-{
-	struct srpt_node_acl *nacl;
-	struct spc_rdma_transport_id *tr_id;
-
-	nacl = container_of(se_nacl, struct srpt_node_acl, nacl);
-	tr_id = (void *)buf;
-	tr_id->protocol_identifier = SCSI_TRANSPORTID_PROTOCOLID_SRP;
-	memcpy(tr_id->i_port_id, nacl->i_port_id, sizeof(tr_id->i_port_id));
-	return sizeof(*tr_id);
-}
-
-static u32 srpt_get_pr_transport_id_len(struct se_portal_group *se_tpg,
-					struct se_node_acl *se_nacl,
-					struct t10_pr_registration *pr_reg,
-					int *format_code)
-{
-	*format_code = 0;
-	return sizeof(struct spc_rdma_transport_id);
-}
-
-static char *srpt_parse_pr_out_transport_id(struct se_portal_group *se_tpg,
-					    const char *buf, u32 *out_tid_len,
-					    char **port_nexus_ptr)
-{
-	struct spc_rdma_transport_id *tr_id;
-
-	*port_nexus_ptr = NULL;
-	*out_tid_len = sizeof(struct spc_rdma_transport_id);
-	tr_id = (void *)buf;
-	return (char *)tr_id->i_port_id;
-}
-
-static struct se_node_acl *srpt_alloc_fabric_acl(struct se_portal_group *se_tpg)
-{
-	struct srpt_node_acl *nacl;
-
-	nacl = kzalloc(sizeof(struct srpt_node_acl), GFP_KERNEL);
-	if (!nacl) {
-		pr_err("Unable to allocate struct srpt_node_acl\n");
-		return NULL;
-	}
-
-	return &nacl->nacl;
-}
-
-static void srpt_release_fabric_acl(struct se_portal_group *se_tpg,
-				    struct se_node_acl *se_nacl)
-{
-	struct srpt_node_acl *nacl;
-
-	nacl = container_of(se_nacl, struct srpt_node_acl, nacl);
-	kfree(nacl);
-}
-
 static u32 srpt_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
 	return 1;
@@ -3547,14 +3474,6 @@ static void srpt_set_default_node_attrs(struct se_node_acl *nacl)
 {
 }
 
-static u32 srpt_get_task_tag(struct se_cmd *se_cmd)
-{
-	struct srpt_send_ioctx *ioctx;
-
-	ioctx = container_of(se_cmd, struct srpt_send_ioctx, cmd);
-	return ioctx->tag;
-}
-
 /* Note: only used from inside debug printk's by the TCM core. */
 static int srpt_get_tcm_cmd_state(struct se_cmd *se_cmd)
 {
@@ -3597,40 +3516,19 @@ out:
  * configfs callback function invoked for
  * mkdir /sys/kernel/config/target/$driver/$port/$tpg/acls/$i_port_id
  */
-static struct se_node_acl *srpt_make_nodeacl(struct se_portal_group *tpg,
-					     struct config_group *group,
-					     const char *name)
+static int srpt_init_nodeacl(struct se_node_acl *se_nacl, const char *name)
 {
-	struct srpt_port *sport = container_of(tpg, struct srpt_port, port_tpg_1);
-	struct se_node_acl *se_nacl, *se_nacl_new;
-	struct srpt_node_acl *nacl;
-	int ret = 0;
-	u32 nexus_depth = 1;
+	struct srpt_port *sport =
+		container_of(se_nacl->se_tpg, struct srpt_port, port_tpg_1);
+	struct srpt_node_acl *nacl =
+		container_of(se_nacl, struct srpt_node_acl, nacl);
 	u8 i_port_id[16];
 
 	if (srpt_parse_i_port_id(i_port_id, name) < 0) {
 		pr_err("invalid initiator port ID %s\n", name);
-		ret = -EINVAL;
-		goto err;
+		return -EINVAL;
 	}
 
-	se_nacl_new = srpt_alloc_fabric_acl(tpg);
-	if (!se_nacl_new) {
-		ret = -ENOMEM;
-		goto err;
-	}
-	/*
-	 * nacl_new may be released by core_tpg_add_initiator_node_acl()
-	 * when converting a node ACL from demo mode to explict
-	 */
-	se_nacl = core_tpg_add_initiator_node_acl(tpg, se_nacl_new, name,
-						  nexus_depth);
-	if (IS_ERR(se_nacl)) {
-		ret = PTR_ERR(se_nacl);
-		goto err;
-	}
-	/* Locate our struct srpt_node_acl and set sdev and i_port_id. */
-	nacl = container_of(se_nacl, struct srpt_node_acl, nacl);
 	memcpy(&nacl->i_port_id[0], &i_port_id[0], 16);
 	nacl->sport = sport;
 
@@ -3638,29 +3536,22 @@ static struct se_node_acl *srpt_make_nodeacl(struct se_portal_group *tpg,
 	list_add_tail(&nacl->list, &sport->port_acl_list);
 	spin_unlock_irq(&sport->port_acl_lock);
 
-	return se_nacl;
-err:
-	return ERR_PTR(ret);
+	return 0;
 }
 
 /*
  * configfs callback function invoked for
  * rmdir /sys/kernel/config/target/$driver/$port/$tpg/acls/$i_port_id
  */
-static void srpt_drop_nodeacl(struct se_node_acl *se_nacl)
+static void srpt_cleanup_nodeacl(struct se_node_acl *se_nacl)
 {
-	struct srpt_node_acl *nacl;
-	struct srpt_device *sdev;
-	struct srpt_port *sport;
+	struct srpt_node_acl *nacl =
+		container_of(se_nacl, struct srpt_node_acl, nacl);
+	struct srpt_port *sport = nacl->sport;
 
-	nacl = container_of(se_nacl, struct srpt_node_acl, nacl);
-	sport = nacl->sport;
-	sdev = sport->sdev;
 	spin_lock_irq(&sport->port_acl_lock);
 	list_del(&nacl->list);
 	spin_unlock_irq(&sport->port_acl_lock);
-	core_tpg_del_initiator_node_acl(&sport->port_tpg_1, se_nacl, 1);
-	srpt_release_fabric_acl(NULL, se_nacl);
 }
 
 static ssize_t srpt_tpg_attrib_show_srp_max_rdma_size(
@@ -3845,8 +3736,7 @@ static struct se_portal_group *srpt_make_tpg(struct se_wwn *wwn,
 	int res;
 
 	/* Initialize sport->port_wwn and sport->port_tpg_1 */
-	res = core_tpg_register(&srpt_template, &sport->port_wwn,
-			&sport->port_tpg_1, sport, TRANSPORT_TPG_TYPE_NORMAL);
+	res = core_tpg_register(&sport->port_wwn, &sport->port_tpg_1, SCSI_PROTOCOL_SRP);
 	if (res)
 		return ERR_PTR(res);
 
@@ -3916,20 +3806,14 @@ static struct configfs_attribute *srpt_wwn_attrs[] = {
 static const struct target_core_fabric_ops srpt_template = {
 	.module				= THIS_MODULE,
 	.name				= "srpt",
+	.node_acl_size			= sizeof(struct srpt_node_acl),
 	.get_fabric_name		= srpt_get_fabric_name,
-	.get_fabric_proto_ident		= srpt_get_fabric_proto_ident,
 	.tpg_get_wwn			= srpt_get_fabric_wwn,
 	.tpg_get_tag			= srpt_get_tag,
-	.tpg_get_default_depth		= srpt_get_default_depth,
-	.tpg_get_pr_transport_id	= srpt_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= srpt_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= srpt_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= srpt_check_false,
 	.tpg_check_demo_mode_cache	= srpt_check_true,
 	.tpg_check_demo_mode_write_protect = srpt_check_true,
 	.tpg_check_prod_mode_write_protect = srpt_check_false,
-	.tpg_alloc_fabric_acl		= srpt_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= srpt_release_fabric_acl,
 	.tpg_get_inst_index		= srpt_tpg_get_inst_index,
 	.release_cmd			= srpt_release_cmd,
 	.check_stop_free		= srpt_check_stop_free,
@@ -3940,7 +3824,6 @@ static const struct target_core_fabric_ops srpt_template = {
 	.write_pending			= srpt_write_pending,
 	.write_pending_status		= srpt_write_pending_status,
 	.set_default_node_attributes	= srpt_set_default_node_attrs,
-	.get_task_tag			= srpt_get_task_tag,
 	.get_cmd_state			= srpt_get_tcm_cmd_state,
 	.queue_data_in			= srpt_queue_data_in,
 	.queue_status			= srpt_queue_status,
@@ -3954,12 +3837,8 @@ static const struct target_core_fabric_ops srpt_template = {
 	.fabric_drop_wwn		= srpt_drop_tport,
 	.fabric_make_tpg		= srpt_make_tpg,
 	.fabric_drop_tpg		= srpt_drop_tpg,
-	.fabric_post_link		= NULL,
-	.fabric_pre_unlink		= NULL,
-	.fabric_make_np			= NULL,
-	.fabric_drop_np			= NULL,
-	.fabric_make_nodeacl		= srpt_make_nodeacl,
-	.fabric_drop_nodeacl		= srpt_drop_nodeacl,
+	.fabric_init_nodeacl		= srpt_init_nodeacl,
+	.fabric_cleanup_nodeacl		= srpt_cleanup_nodeacl,
 
 	.tfc_wwn_attrs			= srpt_wwn_attrs,
 	.tfc_tpg_base_attrs		= srpt_tpg_attrs,
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.h b/drivers/infiniband/ulp/srpt/ib_srpt.h
index 3dae156905de..21f8df67522a 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.h
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.h
@@ -238,14 +238,13 @@ struct srpt_send_ioctx {
 	bool			rdma_aborted;
 	struct se_cmd		cmd;
 	struct completion	tx_done;
-	u64			tag;
 	int			sg_cnt;
 	int			mapped_sg_count;
 	u16			n_rdma_ius;
 	u8			n_rdma;
 	u8			n_rbuf;
 	bool			queue_status_only;
-	u8			sense_data[SCSI_SENSE_BUFFERSIZE];
+	u8			sense_data[TRANSPORT_SENSE_BUFFER];
 };
 
 /**
@@ -410,34 +409,16 @@ struct srpt_device {
 
 /**
  * struct srpt_node_acl - Per-initiator ACL data (managed via configfs).
+ * @nacl:      Target core node ACL information.
  * @i_port_id: 128-bit SRP initiator port ID.
  * @sport:     port information.
- * @nacl:      Target core node ACL information.
  * @list:      Element of the per-HCA ACL list.
  */
 struct srpt_node_acl {
+	struct se_node_acl	nacl;
 	u8			i_port_id[16];
 	struct srpt_port	*sport;
-	struct se_node_acl	nacl;
 	struct list_head	list;
 };
 
-/*
- * SRP-releated SCSI persistent reservation definitions.
- *
- * See also SPC4r28, section 7.6.1 (Protocol specific parameters introduction).
- * See also SPC4r28, section 7.6.4.5 (TransportID for initiator ports using
- * SCSI over an RDMA interface).
- */
-
-enum {
-	SCSI_TRANSPORTID_PROTOCOLID_SRP	= 4,
-};
-
-struct spc_rdma_transport_id {
-	uint8_t protocol_identifier;
-	uint8_t reserved[7];
-	uint8_t i_port_id[16];
-};
-
 #endif				/* IB_SRPT_H */
diff --git a/drivers/input/Kconfig b/drivers/input/Kconfig
index a11ff74a5127..a35532ec00e4 100644
--- a/drivers/input/Kconfig
+++ b/drivers/input/Kconfig
@@ -25,6 +25,19 @@ config INPUT
 
 if INPUT
 
+config INPUT_LEDS
+	tristate "Export input device LEDs in sysfs"
+	depends on LEDS_CLASS
+	default INPUT
+	help
+	  Say Y here if you would like to export LEDs on input devices
+	  as standard LED class devices in sysfs.
+
+	  If unsure, say Y.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called input-leds.
+
 config INPUT_FF_MEMLESS
 	tristate "Support for memoryless force-feedback devices"
 	help
diff --git a/drivers/input/Makefile b/drivers/input/Makefile
index 5ca3f631497f..0c9302ca9954 100644
--- a/drivers/input/Makefile
+++ b/drivers/input/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_INPUT_POLLDEV)	+= input-polldev.o
 obj-$(CONFIG_INPUT_SPARSEKMAP)	+= sparse-keymap.o
 obj-$(CONFIG_INPUT_MATRIXKMAP)	+= matrix-keymap.o
 
+obj-$(CONFIG_INPUT_LEDS)	+= input-leds.o
 obj-$(CONFIG_INPUT_MOUSEDEV)	+= mousedev.o
 obj-$(CONFIG_INPUT_JOYDEV)	+= joydev.o
 obj-$(CONFIG_INPUT_EVDEV)	+= evdev.o
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index a18f41b89b6a..9d35499faca4 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -422,10 +422,7 @@ static int evdev_release(struct inode *inode, struct file *file)
 
 	evdev_detach_client(evdev, client);
 
-	if (is_vmalloc_addr(client))
-		vfree(client);
-	else
-		kfree(client);
+	kvfree(client);
 
 	evdev_close_device(evdev);
 
diff --git a/drivers/input/ff-core.c b/drivers/input/ff-core.c
index b81c88c43452..8f4a30fccbb6 100644
--- a/drivers/input/ff-core.c
+++ b/drivers/input/ff-core.c
@@ -70,7 +70,7 @@ static int compat_effect(struct ff_device *ff, struct ff_effect *effect)
 			return -EINVAL;
 
 		/*
-		 * calculate manginude of sine wave as average of rumble's
+		 * calculate magnitude of sine wave as average of rumble's
 		 * 2/3 of strong magnitude and 1/3 of weak magnitude
 		 */
 		magnitude = effect->u.rumble.strong_magnitude / 3 +
@@ -213,7 +213,7 @@ static int erase_effect(struct input_dev *dev, int effect_id,
 /**
  * input_ff_erase - erase a force-feedback effect from device
  * @dev: input device to erase effect from
- * @effect_id: id of the ffect to be erased
+ * @effect_id: id of the effect to be erased
  * @file: purported owner of the request
  *
  * This function erases a force-feedback effect from specified device.
diff --git a/drivers/input/input-leds.c b/drivers/input/input-leds.c
new file mode 100644
index 000000000000..074a65ed17bb
--- /dev/null
+++ b/drivers/input/input-leds.c
@@ -0,0 +1,212 @@
+/*
+ * LED support for the input layer
+ *
+ * Copyright 2010-2015 Samuel Thibault <samuel.thibault@ens-lyon.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/slab.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/leds.h>
+#include <linux/input.h>
+
+#if IS_ENABLED(CONFIG_VT)
+#define VT_TRIGGER(_name)	.trigger = _name
+#else
+#define VT_TRIGGER(_name)	.trigger = NULL
+#endif
+
+static const struct {
+	const char *name;
+	const char *trigger;
+} input_led_info[LED_CNT] = {
+	[LED_NUML]	= { "numlock", VT_TRIGGER("kbd-numlock") },
+	[LED_CAPSL]	= { "capslock", VT_TRIGGER("kbd-capslock") },
+	[LED_SCROLLL]	= { "scrolllock", VT_TRIGGER("kbd-scrolllock") },
+	[LED_COMPOSE]	= { "compose" },
+	[LED_KANA]	= { "kana", VT_TRIGGER("kbd-kanalock") },
+	[LED_SLEEP]	= { "sleep" } ,
+	[LED_SUSPEND]	= { "suspend" },
+	[LED_MUTE]	= { "mute" },
+	[LED_MISC]	= { "misc" },
+	[LED_MAIL]	= { "mail" },
+	[LED_CHARGING]	= { "charging" },
+};
+
+struct input_led {
+	struct led_classdev cdev;
+	struct input_handle *handle;
+	unsigned int code; /* One of LED_* constants */
+};
+
+struct input_leds {
+	struct input_handle handle;
+	unsigned int num_leds;
+	struct input_led leds[];
+};
+
+static enum led_brightness input_leds_brightness_get(struct led_classdev *cdev)
+{
+	struct input_led *led = container_of(cdev, struct input_led, cdev);
+	struct input_dev *input = led->handle->dev;
+
+	return test_bit(led->code, input->led) ? cdev->max_brightness : 0;
+}
+
+static void input_leds_brightness_set(struct led_classdev *cdev,
+				      enum led_brightness brightness)
+{
+	struct input_led *led = container_of(cdev, struct input_led, cdev);
+
+	input_inject_event(led->handle, EV_LED, led->code, !!brightness);
+}
+
+static void input_leds_event(struct input_handle *handle, unsigned int type,
+			     unsigned int code, int value)
+{
+}
+
+static int input_leds_connect(struct input_handler *handler,
+			      struct input_dev *dev,
+			      const struct input_device_id *id)
+{
+	struct input_leds *leds;
+	unsigned int num_leds;
+	unsigned int led_code;
+	int led_no;
+	int error;
+
+	num_leds = bitmap_weight(dev->ledbit, LED_CNT);
+	if (!num_leds)
+		return -ENXIO;
+
+	leds = kzalloc(sizeof(*leds) + num_leds * sizeof(*leds->leds),
+		       GFP_KERNEL);
+	if (!leds)
+		return -ENOMEM;
+
+	leds->num_leds = num_leds;
+
+	leds->handle.dev = dev;
+	leds->handle.handler = handler;
+	leds->handle.name = "leds";
+	leds->handle.private = leds;
+
+	error = input_register_handle(&leds->handle);
+	if (error)
+		goto err_free_mem;
+
+	error = input_open_device(&leds->handle);
+	if (error)
+		goto err_unregister_handle;
+
+	led_no = 0;
+	for_each_set_bit(led_code, dev->ledbit, LED_CNT) {
+		struct input_led *led = &leds->leds[led_no];
+
+		led->handle = &leds->handle;
+		led->code = led_code;
+
+		if (WARN_ON(!input_led_info[led_code].name))
+			continue;
+
+		led->cdev.name = kasprintf(GFP_KERNEL, "%s::%s",
+					   dev_name(&dev->dev),
+					   input_led_info[led_code].name);
+		if (!led->cdev.name) {
+			error = -ENOMEM;
+			goto err_unregister_leds;
+		}
+
+		led->cdev.max_brightness = 1;
+		led->cdev.brightness_get = input_leds_brightness_get;
+		led->cdev.brightness_set = input_leds_brightness_set;
+		led->cdev.default_trigger = input_led_info[led_code].trigger;
+
+		error = led_classdev_register(&dev->dev, &led->cdev);
+		if (error) {
+			dev_err(&dev->dev, "failed to register LED %s: %d\n",
+				led->cdev.name, error);
+			kfree(led->cdev.name);
+			goto err_unregister_leds;
+		}
+
+		led_no++;
+	}
+
+	return 0;
+
+err_unregister_leds:
+	while (--led_no >= 0) {
+		struct input_led *led = &leds->leds[led_no];
+
+		led_classdev_unregister(&led->cdev);
+		kfree(led->cdev.name);
+	}
+
+	input_close_device(&leds->handle);
+
+err_unregister_handle:
+	input_unregister_handle(&leds->handle);
+
+err_free_mem:
+	kfree(leds);
+	return error;
+}
+
+static void input_leds_disconnect(struct input_handle *handle)
+{
+	struct input_leds *leds = handle->private;
+	int i;
+
+	for (i = 0; i < leds->num_leds; i++) {
+		struct input_led *led = &leds->leds[i];
+
+		led_classdev_unregister(&led->cdev);
+		kfree(led->cdev.name);
+	}
+
+	input_close_device(handle);
+	input_unregister_handle(handle);
+
+	kfree(leds);
+}
+
+static const struct input_device_id input_leds_ids[] = {
+	{
+		.flags = INPUT_DEVICE_ID_MATCH_EVBIT,
+		.evbit = { BIT_MASK(EV_LED) },
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(input, input_leds_ids);
+
+static struct input_handler input_leds_handler = {
+	.event =	input_leds_event,
+	.connect =	input_leds_connect,
+	.disconnect =	input_leds_disconnect,
+	.name =		"leds",
+	.id_table =	input_leds_ids,
+};
+
+static int __init input_leds_init(void)
+{
+	return input_register_handler(&input_leds_handler);
+}
+module_init(input_leds_init);
+
+static void __exit input_leds_exit(void)
+{
+	input_unregister_handler(&input_leds_handler);
+}
+module_exit(input_leds_exit);
+
+MODULE_AUTHOR("Samuel Thibault <samuel.thibault@ens-lyon.org>");
+MODULE_AUTHOR("Dmitry Torokhov <dmitry.torokhov@gmail.com>");
+MODULE_DESCRIPTION("Input -> LEDs Bridge");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/input.c b/drivers/input/input.c
index cc357f1516a7..78d24990a816 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -677,12 +677,9 @@ static void input_dev_release_keys(struct input_dev *dev)
 	int code;
 
 	if (is_event_supported(EV_KEY, dev->evbit, EV_MAX)) {
-		for (code = 0; code <= KEY_MAX; code++) {
-			if (is_event_supported(code, dev->keybit, KEY_MAX) &&
-			    __test_and_clear_bit(code, dev->key)) {
-				input_pass_event(dev, EV_KEY, code, 0);
-			}
-		}
+		for_each_set_bit(code, dev->key, KEY_CNT)
+			input_pass_event(dev, EV_KEY, code, 0);
+		memset(dev->key, 0, sizeof(dev->key));
 		input_pass_event(dev, EV_SYN, SYN_REPORT, 1);
 	}
 }
@@ -1626,10 +1623,7 @@ static int input_dev_uevent(struct device *device, struct kobj_uevent_env *env)
 		if (!test_bit(EV_##type, dev->evbit))			\
 			break;						\
 									\
-		for (i = 0; i < type##_MAX; i++) {			\
-			if (!test_bit(i, dev->bits##bit))		\
-				continue;				\
-									\
+		for_each_set_bit(i, dev->bits##bit, type##_CNT) {	\
 			active = test_bit(i, dev->bits);		\
 			if (!active && !on)				\
 				continue;				\
@@ -1980,22 +1974,12 @@ static unsigned int input_estimate_events_per_packet(struct input_dev *dev)
 
 	events = mt_slots + 1; /* count SYN_MT_REPORT and SYN_REPORT */
 
-	if (test_bit(EV_ABS, dev->evbit)) {
-		for (i = 0; i < ABS_CNT; i++) {
-			if (test_bit(i, dev->absbit)) {
-				if (input_is_mt_axis(i))
-					events += mt_slots;
-				else
-					events++;
-			}
-		}
-	}
+	if (test_bit(EV_ABS, dev->evbit))
+		for_each_set_bit(i, dev->absbit, ABS_CNT)
+			events += input_is_mt_axis(i) ? mt_slots : 1;
 
-	if (test_bit(EV_REL, dev->evbit)) {
-		for (i = 0; i < REL_CNT; i++)
-			if (test_bit(i, dev->relbit))
-				events++;
-	}
+	if (test_bit(EV_REL, dev->evbit))
+		events += bitmap_weight(dev->relbit, REL_CNT);
 
 	/* Make room for KEY and MSC events */
 	events += 7;
@@ -2262,7 +2246,7 @@ EXPORT_SYMBOL(input_unregister_handler);
  *
  * Iterate over @bus's list of devices, and call @fn for each, passing
  * it @data and stop when @fn returns a non-zero value. The function is
- * using RCU to traverse the list and therefore may be usind in atonic
+ * using RCU to traverse the list and therefore may be using in atomic
  * contexts. The @fn callback is invoked from RCU critical section and
  * thus must not sleep.
  */
diff --git a/drivers/input/joystick/xpad.c b/drivers/input/joystick/xpad.c
index 61c761156371..f8850f9cb331 100644
--- a/drivers/input/joystick/xpad.c
+++ b/drivers/input/joystick/xpad.c
@@ -344,6 +344,7 @@ struct usb_xpad {
 
 	int mapping;			/* map d-pad to buttons or to axes */
 	int xtype;			/* type of xbox device */
+	unsigned long led_no;		/* led to lit on xbox360 controllers */
 };
 
 /*
@@ -488,6 +489,8 @@ static void xpad360_process_packet(struct usb_xpad *xpad,
 	input_sync(dev);
 }
 
+static void xpad_identify_controller(struct usb_xpad *xpad);
+
 /*
  * xpad360w_process_packet
  *
@@ -510,6 +513,11 @@ static void xpad360w_process_packet(struct usb_xpad *xpad, u16 cmd, unsigned cha
 		if (data[1] & 0x80) {
 			xpad->pad_present = 1;
 			usb_submit_urb(xpad->bulk_out, GFP_ATOMIC);
+			/*
+			 * Light up the segment corresponding to
+			 * controller number.
+			 */
+			xpad_identify_controller(xpad);
 		} else
 			xpad->pad_present = 0;
 	}
@@ -881,17 +889,63 @@ struct xpad_led {
 	struct usb_xpad *xpad;
 };
 
+/**
+ * @param command
+ *  0: off
+ *  1: all blink, then previous setting
+ *  2: 1/top-left blink, then on
+ *  3: 2/top-right blink, then on
+ *  4: 3/bottom-left blink, then on
+ *  5: 4/bottom-right blink, then on
+ *  6: 1/top-left on
+ *  7: 2/top-right on
+ *  8: 3/bottom-left on
+ *  9: 4/bottom-right on
+ * 10: rotate
+ * 11: blink, based on previous setting
+ * 12: slow blink, based on previous setting
+ * 13: rotate with two lights
+ * 14: persistent slow all blink
+ * 15: blink once, then previous setting
+ */
 static void xpad_send_led_command(struct usb_xpad *xpad, int command)
 {
-	if (command >= 0 && command < 14) {
-		mutex_lock(&xpad->odata_mutex);
+	command %= 16;
+
+	mutex_lock(&xpad->odata_mutex);
+
+	switch (xpad->xtype) {
+	case XTYPE_XBOX360:
 		xpad->odata[0] = 0x01;
 		xpad->odata[1] = 0x03;
 		xpad->odata[2] = command;
 		xpad->irq_out->transfer_buffer_length = 3;
-		usb_submit_urb(xpad->irq_out, GFP_KERNEL);
-		mutex_unlock(&xpad->odata_mutex);
+		break;
+	case XTYPE_XBOX360W:
+		xpad->odata[0] = 0x00;
+		xpad->odata[1] = 0x00;
+		xpad->odata[2] = 0x08;
+		xpad->odata[3] = 0x40 + command;
+		xpad->odata[4] = 0x00;
+		xpad->odata[5] = 0x00;
+		xpad->odata[6] = 0x00;
+		xpad->odata[7] = 0x00;
+		xpad->odata[8] = 0x00;
+		xpad->odata[9] = 0x00;
+		xpad->odata[10] = 0x00;
+		xpad->odata[11] = 0x00;
+		xpad->irq_out->transfer_buffer_length = 12;
+		break;
 	}
+
+	usb_submit_urb(xpad->irq_out, GFP_KERNEL);
+	mutex_unlock(&xpad->odata_mutex);
+}
+
+static void xpad_identify_controller(struct usb_xpad *xpad)
+{
+	/* Light up the segment corresponding to controller number */
+	xpad_send_led_command(xpad, (xpad->led_no % 4) + 2);
 }
 
 static void xpad_led_set(struct led_classdev *led_cdev,
@@ -905,22 +959,21 @@ 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(-1);
-	unsigned long led_no;
+	static atomic_t led_seq = ATOMIC_INIT(-1);
 	struct xpad_led *led;
 	struct led_classdev *led_cdev;
 	int error;
 
-	if (xpad->xtype != XTYPE_XBOX360)
+	if (xpad->xtype != XTYPE_XBOX360 && xpad->xtype != XTYPE_XBOX360W)
 		return 0;
 
 	xpad->led = led = kzalloc(sizeof(struct xpad_led), GFP_KERNEL);
 	if (!led)
 		return -ENOMEM;
 
-	led_no = atomic_inc_return(&led_seq);
+	xpad->led_no = atomic_inc_return(&led_seq);
 
-	snprintf(led->name, sizeof(led->name), "xpad%lu", led_no);
+	snprintf(led->name, sizeof(led->name), "xpad%lu", xpad->led_no);
 	led->xpad = xpad;
 
 	led_cdev = &led->led_cdev;
@@ -934,10 +987,8 @@ static int xpad_led_probe(struct usb_xpad *xpad)
 		return error;
 	}
 
-	/*
-	 * Light up the segment corresponding to controller number
-	 */
-	xpad_send_led_command(xpad, (led_no % 4) + 2);
+	/* Light up the segment corresponding to controller number */
+	xpad_identify_controller(xpad);
 
 	return 0;
 }
@@ -954,6 +1005,7 @@ static void xpad_led_disconnect(struct usb_xpad *xpad)
 #else
 static int xpad_led_probe(struct usb_xpad *xpad) { return 0; }
 static void xpad_led_disconnect(struct usb_xpad *xpad) { }
+static void xpad_identify_controller(struct usb_xpad *xpad) { }
 #endif
 
 
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index 106fbac7f8c5..4cd94fd6cbad 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -367,6 +367,7 @@ config KEYBOARD_MAPLE
 
 config KEYBOARD_MAX7359
 	tristate "Maxim MAX7359 Key Switch Controller"
+	select INPUT_MATRIXKMAP
 	depends on I2C
 	help
 	  If you say yes here you get support for the Maxim MAX7359 Key
@@ -677,7 +678,7 @@ config KEYBOARD_W90P910
 config KEYBOARD_CROS_EC
 	tristate "ChromeOS EC keyboard"
 	select INPUT_MATRIXKMAP
-	depends on MFD_CROS_EC
+	depends on CROS_EC_PROTO
 	help
 	  Say Y here to enable the matrix keyboard used by ChromeOS devices
 	  and implemented on the ChromeOS EC. You must enable one bus option
diff --git a/drivers/input/keyboard/adp5589-keys.c b/drivers/input/keyboard/adp5589-keys.c
index a45267729dfc..6ed83cf8b74e 100644
--- a/drivers/input/keyboard/adp5589-keys.c
+++ b/drivers/input/keyboard/adp5589-keys.c
@@ -180,7 +180,7 @@
 #define LOGIC2_STAT	(1 << 7)	/* ADP5589 only */
 #define LOGIC1_STAT	(1 << 6)
 #define LOCK_STAT	(1 << 5)	/* ADP5589 only */
-#define KEC		0xF
+#define KEC		0x1F
 
 /* PIN_CONFIG_D Register */
 #define C4_EXTEND_CFG	(1 << 6)	/* RESET2 */
@@ -726,7 +726,7 @@ static int adp5589_setup(struct adp5589_kpad *kpad)
 
 		pull_mask |= val << (2 * (i & 0x3));
 
-		if (i == 3 || i == kpad->var->max_row_num) {
+		if (i % 4 == 3 || i == kpad->var->max_row_num) {
 			ret |= adp5589_write(client, reg(ADP5585_RPULL_CONFIG_A)
 					     + (i >> 2), pull_mask);
 			pull_mask = 0;
@@ -746,7 +746,7 @@ static int adp5589_setup(struct adp5589_kpad *kpad)
 
 		pull_mask |= val << (2 * (i & 0x3));
 
-		if (i == 3 || i == kpad->var->max_col_num) {
+		if (i % 4 == 3 || i == kpad->var->max_col_num) {
 			ret |= adp5589_write(client,
 					     reg(ADP5585_RPULL_CONFIG_C) +
 					     (i >> 2), pull_mask);
diff --git a/drivers/input/keyboard/clps711x-keypad.c b/drivers/input/keyboard/clps711x-keypad.c
index 27ef29f8fe6a..b637f1af842e 100644
--- a/drivers/input/keyboard/clps711x-keypad.c
+++ b/drivers/input/keyboard/clps711x-keypad.c
@@ -120,14 +120,9 @@ static int clps711x_keypad_probe(struct platform_device *pdev)
 	for (i = 0; i < priv->row_count; i++) {
 		struct clps711x_gpio_data *data = &priv->gpio_data[i];
 
-		data->desc = devm_gpiod_get_index(dev, "row", i);
-		if (!data->desc)
-			return -EINVAL;
-
+		data->desc = devm_gpiod_get_index(dev, "row", i, GPIOD_IN);
 		if (IS_ERR(data->desc))
 			return PTR_ERR(data->desc);
-
-		gpiod_direction_input(data->desc);
 	}
 
 	err = of_property_read_u32(np, "poll-interval", &poll_interval);
diff --git a/drivers/input/keyboard/cros_ec_keyb.c b/drivers/input/keyboard/cros_ec_keyb.c
index b50c5b8b8a4d..b01966dc7eb3 100644
--- a/drivers/input/keyboard/cros_ec_keyb.c
+++ b/drivers/input/keyboard/cros_ec_keyb.c
@@ -148,19 +148,28 @@ static void cros_ec_keyb_process(struct cros_ec_keyb *ckdev,
 
 static int cros_ec_keyb_get_state(struct cros_ec_keyb *ckdev, uint8_t *kb_state)
 {
-	int ret;
-	struct cros_ec_command msg = {
-		.command = EC_CMD_MKBP_STATE,
-		.insize = ckdev->cols,
-	};
+	int ret = 0;
+	struct cros_ec_command *msg;
 
-	ret = cros_ec_cmd_xfer(ckdev->ec, &msg);
-	if (ret < 0)
-		return ret;
+	msg = kmalloc(sizeof(*msg) + ckdev->cols, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
 
-	memcpy(kb_state, msg.indata, ckdev->cols);
+	msg->version = 0;
+	msg->command = EC_CMD_MKBP_STATE;
+	msg->insize = ckdev->cols;
+	msg->outsize = 0;
 
-	return 0;
+	ret = cros_ec_cmd_xfer(ckdev->ec, msg);
+	if (ret < 0) {
+		dev_err(ckdev->dev, "Error transferring EC message %d\n", ret);
+		goto exit;
+	}
+
+	memcpy(kb_state, msg->data, ckdev->cols);
+exit:
+	kfree(msg);
+	return ret;
 }
 
 static irqreturn_t cros_ec_keyb_irq(int irq, void *data)
@@ -266,7 +275,7 @@ static int cros_ec_keyb_probe(struct platform_device *pdev)
 	ckdev->dev = dev;
 	dev_set_drvdata(&pdev->dev, ckdev);
 
-	idev->name = ec->ec_name;
+	idev->name = CROS_EC_DEV_NAME;
 	idev->phys = ec->phys_name;
 	__set_bit(EV_REP, idev->evbit);
 
diff --git a/drivers/input/keyboard/imx_keypad.c b/drivers/input/keyboard/imx_keypad.c
index 2e855e6f3565..d2ea863d6a45 100644
--- a/drivers/input/keyboard/imx_keypad.c
+++ b/drivers/input/keyboard/imx_keypad.c
@@ -506,7 +506,9 @@ static int imx_keypad_probe(struct platform_device *pdev)
 	input_set_drvdata(input_dev, keypad);
 
 	/* Ensure that the keypad will stay dormant until opened */
-	clk_prepare_enable(keypad->clk);
+	error = clk_prepare_enable(keypad->clk);
+	if (error)
+		return error;
 	imx_keypad_inhibit(keypad);
 	clk_disable_unprepare(keypad->clk);
 
diff --git a/drivers/input/keyboard/max7359_keypad.c b/drivers/input/keyboard/max7359_keypad.c
index faa6da53eba8..5091133b7b8e 100644
--- a/drivers/input/keyboard/max7359_keypad.c
+++ b/drivers/input/keyboard/max7359_keypad.c
@@ -84,26 +84,6 @@ static int max7359_read_reg(struct i2c_client *client, int reg)
 	return ret;
 }
 
-static void max7359_build_keycode(struct max7359_keypad *keypad,
-				const struct matrix_keymap_data *keymap_data)
-{
-	struct input_dev *input_dev = keypad->input_dev;
-	int i;
-
-	for (i = 0; i < keymap_data->keymap_size; i++) {
-		unsigned int key = keymap_data->keymap[i];
-		unsigned int row = KEY_ROW(key);
-		unsigned int col = KEY_COL(key);
-		unsigned int scancode = MATRIX_SCAN_CODE(row, col,
-						MAX7359_ROW_SHIFT);
-		unsigned short keycode = KEY_VAL(key);
-
-		keypad->keycodes[scancode] = keycode;
-		__set_bit(keycode, input_dev->keybit);
-	}
-	__clear_bit(KEY_RESERVED, input_dev->keybit);
-}
-
 /* runs in an IRQ thread -- can (and will!) sleep */
 static irqreturn_t max7359_interrupt(int irq, void *dev_id)
 {
@@ -166,7 +146,6 @@ static void max7359_close(struct input_dev *dev)
 static void max7359_initialize(struct i2c_client *client)
 {
 	max7359_write_reg(client, MAX7359_REG_CONFIG,
-		MAX7359_CFG_INTERRUPT | /* Irq clears after host read */
 		MAX7359_CFG_KEY_RELEASE | /* Key release enable */
 		MAX7359_CFG_WAKEUP); /* Key press wakeup enable */
 
@@ -233,7 +212,15 @@ static int max7359_probe(struct i2c_client *client,
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 	input_set_drvdata(input_dev, keypad);
 
-	max7359_build_keycode(keypad, keymap_data);
+	error = matrix_keypad_build_keymap(keymap_data, NULL,
+					   MAX7359_MAX_KEY_ROWS,
+					   MAX7359_MAX_KEY_COLS,
+					   keypad->keycodes,
+					   input_dev);
+	if (error) {
+		dev_err(&client->dev, "failed to build keymap\n");
+		return error;
+	}
 
 	error = devm_request_threaded_irq(&client->dev, client->irq, NULL,
 					  max7359_interrupt,
diff --git a/drivers/input/keyboard/samsung-keypad.c b/drivers/input/keyboard/samsung-keypad.c
index 6b9fdf6cf8e8..43e48dac7687 100644
--- a/drivers/input/keyboard/samsung-keypad.c
+++ b/drivers/input/keyboard/samsung-keypad.c
@@ -585,7 +585,7 @@ static const struct of_device_id samsung_keypad_dt_match[] = {
 MODULE_DEVICE_TABLE(of, samsung_keypad_dt_match);
 #endif
 
-static struct platform_device_id samsung_keypad_driver_ids[] = {
+static const struct platform_device_id samsung_keypad_driver_ids[] = {
 	{
 		.name		= "samsung-keypad",
 		.driver_data	= KEYPAD_TYPE_SAMSUNG,
diff --git a/drivers/input/keyboard/spear-keyboard.c b/drivers/input/keyboard/spear-keyboard.c
index f42a543db043..623d451767e3 100644
--- a/drivers/input/keyboard/spear-keyboard.c
+++ b/drivers/input/keyboard/spear-keyboard.c
@@ -3,7 +3,7 @@
  * Based on omap-keypad driver
  *
  * Copyright (C) 2010 ST Microelectronics
- * Rajeev Kumar<rajeev-dlh.kumar@st.com>
+ * Rajeev Kumar <rajeevkumar.linux@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
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 4436ab1b9735..d4f0a817e858 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -610,6 +610,16 @@ config INPUT_DA9055_ONKEY
 	  To compile this driver as a module, choose M here: the module
 	  will be called da9055_onkey.
 
+config INPUT_DA9063_ONKEY
+	tristate "Dialog DA9063 OnKey"
+	depends on MFD_DA9063
+	help
+	  Support the ONKEY of Dialog DA9063 Power Management IC as an
+	  input device reporting power button statue.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called da9063_onkey.
+
 config INPUT_DM355EVM
 	tristate "TI DaVinci DM355 EVM Keypad and IR Remote"
 	depends on MFD_DM355EVM_MSP
@@ -775,6 +785,17 @@ config INPUT_DRV260X_HAPTICS
 	  To compile this driver as a module, choose M here: the
 	  module will be called drv260x-haptics.
 
+config INPUT_DRV2665_HAPTICS
+	tristate "TI DRV2665 haptics support"
+	depends on INPUT && I2C
+	select INPUT_FF_MEMLESS
+	select REGMAP_I2C
+	help
+	  Say Y to enable support for the TI DRV2665 haptics driver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called drv2665-haptics.
+
 config INPUT_DRV2667_HAPTICS
 	tristate "TI DRV2667 haptics support"
 	depends on INPUT && I2C
@@ -784,6 +805,6 @@ config INPUT_DRV2667_HAPTICS
 	  Say Y to enable support for the TI DRV2667 haptics driver.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called drv260x-haptics.
+	  module will be called drv2667-haptics.
 
 endif
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index 78ba4c1b8532..53df07dcc23c 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -25,9 +25,11 @@ obj-$(CONFIG_INPUT_CMA3000_I2C)		+= cma3000_d0x_i2c.o
 obj-$(CONFIG_INPUT_COBALT_BTNS)		+= cobalt_btns.o
 obj-$(CONFIG_INPUT_DA9052_ONKEY)	+= da9052_onkey.o
 obj-$(CONFIG_INPUT_DA9055_ONKEY)	+= da9055_onkey.o
+obj-$(CONFIG_INPUT_DA9063_ONKEY)	+= da9063_onkey.o
 obj-$(CONFIG_INPUT_DM355EVM)		+= dm355evm_keys.o
 obj-$(CONFIG_INPUT_E3X0_BUTTON)		+= e3x0-button.o
 obj-$(CONFIG_INPUT_DRV260X_HAPTICS)	+= drv260x.o
+obj-$(CONFIG_INPUT_DRV2665_HAPTICS)	+= drv2665.o
 obj-$(CONFIG_INPUT_DRV2667_HAPTICS)	+= drv2667.o
 obj-$(CONFIG_INPUT_GP2A)		+= gp2ap002a00f.o
 obj-$(CONFIG_INPUT_GPIO_BEEPER)		+= gpio-beeper.o
diff --git a/drivers/input/misc/adxl34x-i2c.c b/drivers/input/misc/adxl34x-i2c.c
index 470bfd6f0830..bdb5d03b296e 100644
--- a/drivers/input/misc/adxl34x-i2c.c
+++ b/drivers/input/misc/adxl34x-i2c.c
@@ -10,6 +10,7 @@
 #include <linux/input.h>	/* BUS_I2C */
 #include <linux/i2c.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/types.h>
 #include <linux/pm.h>
 #include "adxl34x.h"
@@ -135,11 +136,31 @@ static const struct i2c_device_id adxl34x_id[] = {
 
 MODULE_DEVICE_TABLE(i2c, adxl34x_id);
 
+#ifdef CONFIG_OF
+static const struct of_device_id adxl34x_of_id[] = {
+	/*
+	 * The ADXL346 is backward-compatible with the ADXL345. Differences are
+	 * handled by runtime detection of the device model, there's thus no
+	 * need for listing the "adi,adxl346" compatible value explicitly.
+	 */
+	{ .compatible = "adi,adxl345", },
+	/*
+	 * Deprecated, DT nodes should use one or more of the device-specific
+	 * compatible values "adi,adxl345" and "adi,adxl346".
+	 */
+	{ .compatible = "adi,adxl34x", },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, adxl34x_of_id);
+#endif
+
 static struct i2c_driver adxl34x_driver = {
 	.driver = {
 		.name = "adxl34x",
 		.owner = THIS_MODULE,
 		.pm = &adxl34x_i2c_pm,
+		.of_match_table = of_match_ptr(adxl34x_of_id),
 	},
 	.probe    = adxl34x_i2c_probe,
 	.remove   = adxl34x_i2c_remove,
diff --git a/drivers/input/misc/ati_remote2.c b/drivers/input/misc/ati_remote2.c
index f63341f20b91..cfd58e87da26 100644
--- a/drivers/input/misc/ati_remote2.c
+++ b/drivers/input/misc/ati_remote2.c
@@ -94,7 +94,7 @@ static int ati_remote2_get_mode_mask(char *buffer,
 
 static unsigned int channel_mask = ATI_REMOTE2_MAX_CHANNEL_MASK;
 #define param_check_channel_mask(name, p) __param_check(name, p, unsigned int)
-static struct kernel_param_ops param_ops_channel_mask = {
+static const struct kernel_param_ops param_ops_channel_mask = {
 	.set = ati_remote2_set_channel_mask,
 	.get = ati_remote2_get_channel_mask,
 };
@@ -103,7 +103,7 @@ MODULE_PARM_DESC(channel_mask, "Bitmask of channels to accept <15:Channel16>...<
 
 static unsigned int mode_mask = ATI_REMOTE2_MAX_MODE_MASK;
 #define param_check_mode_mask(name, p) __param_check(name, p, unsigned int)
-static struct kernel_param_ops param_ops_mode_mask = {
+static const struct kernel_param_ops param_ops_mode_mask = {
 	.set = ati_remote2_set_mode_mask,
 	.get = ati_remote2_get_mode_mask,
 };
diff --git a/drivers/input/misc/axp20x-pek.c b/drivers/input/misc/axp20x-pek.c
index f1c844739cd7..10e140af5aac 100644
--- a/drivers/input/misc/axp20x-pek.c
+++ b/drivers/input/misc/axp20x-pek.c
@@ -167,9 +167,13 @@ static irqreturn_t axp20x_pek_irq(int irq, void *pwr)
 	struct input_dev *idev = pwr;
 	struct axp20x_pek *axp20x_pek = input_get_drvdata(idev);
 
-	if (irq == axp20x_pek->irq_dbr)
+	/*
+	 * The power-button is connected to ground so a falling edge (dbf)
+	 * means it is pressed.
+	 */
+	if (irq == axp20x_pek->irq_dbf)
 		input_report_key(idev, KEY_POWER, true);
-	else if (irq == axp20x_pek->irq_dbf)
+	else if (irq == axp20x_pek->irq_dbr)
 		input_report_key(idev, KEY_POWER, false);
 
 	input_sync(idev);
diff --git a/drivers/input/misc/da9063_onkey.c b/drivers/input/misc/da9063_onkey.c
new file mode 100644
index 000000000000..f577585ef999
--- /dev/null
+++ b/drivers/input/misc/da9063_onkey.c
@@ -0,0 +1,226 @@
+/*
+ * OnKey device driver for DA9063
+ * Copyright (C) 2015  Dialog Semiconductor Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+#include <linux/mfd/da9063/core.h>
+#include <linux/mfd/da9063/pdata.h>
+#include <linux/mfd/da9063/registers.h>
+
+struct da9063_onkey {
+	struct da9063 *hw;
+	struct delayed_work work;
+	struct input_dev *input;
+	struct device *dev;
+	bool key_power;
+};
+
+static void da9063_poll_on(struct work_struct *work)
+{
+	struct da9063_onkey *onkey = container_of(work, struct da9063_onkey,
+						  work.work);
+	unsigned int val;
+	int fault_log = 0;
+	bool poll = true;
+	int error;
+
+	/* Poll to see when the pin is released */
+	error = regmap_read(onkey->hw->regmap, DA9063_REG_STATUS_A, &val);
+	if (error) {
+		dev_err(onkey->dev,
+			"Failed to read ON status: %d\n", error);
+		goto err_poll;
+	}
+
+	if (!(val & DA9063_NONKEY)) {
+		error = regmap_update_bits(onkey->hw->regmap,
+					   DA9063_REG_CONTROL_B,
+					   DA9063_NONKEY_LOCK, 0);
+		if (error) {
+			dev_err(onkey->dev,
+				"Failed to reset the Key Delay %d\n", error);
+			goto err_poll;
+		}
+
+		input_report_key(onkey->input, KEY_POWER, 0);
+		input_sync(onkey->input);
+
+		poll = false;
+	}
+
+	/*
+	 * If the fault log KEY_RESET is detected, then clear it
+	 * and shut down the system.
+	 */
+	error = regmap_read(onkey->hw->regmap,
+			    DA9063_REG_FAULT_LOG, &fault_log);
+	if (error) {
+		dev_warn(&onkey->input->dev,
+			 "Cannot read FAULT_LOG: %d\n", error);
+	} else if (fault_log & DA9063_KEY_RESET) {
+		error = regmap_write(onkey->hw->regmap,
+				     DA9063_REG_FAULT_LOG,
+				     DA9063_KEY_RESET);
+		if (error) {
+			dev_warn(&onkey->input->dev,
+				 "Cannot reset KEY_RESET fault log: %d\n",
+				 error);
+		} else {
+			/* at this point we do any S/W housekeeping
+			 * and then send shutdown command
+			 */
+			dev_dbg(&onkey->input->dev,
+				 "Sending SHUTDOWN to DA9063 ...\n");
+			error = regmap_write(onkey->hw->regmap,
+					     DA9063_REG_CONTROL_F,
+					     DA9063_SHUTDOWN);
+			if (error)
+				dev_err(&onkey->input->dev,
+					"Cannot SHUTDOWN DA9063: %d\n",
+					error);
+		}
+	}
+
+err_poll:
+	if (poll)
+		schedule_delayed_work(&onkey->work, msecs_to_jiffies(50));
+}
+
+static irqreturn_t da9063_onkey_irq_handler(int irq, void *data)
+{
+	struct da9063_onkey *onkey = data;
+	unsigned int val;
+	int error;
+
+	error = regmap_read(onkey->hw->regmap, DA9063_REG_STATUS_A, &val);
+	if (onkey->key_power && !error && (val & DA9063_NONKEY)) {
+		input_report_key(onkey->input, KEY_POWER, 1);
+		input_sync(onkey->input);
+		schedule_delayed_work(&onkey->work, 0);
+		dev_dbg(onkey->dev, "KEY_POWER pressed.\n");
+	} else {
+		input_report_key(onkey->input, KEY_SLEEP, 1);
+		input_sync(onkey->input);
+		input_report_key(onkey->input, KEY_SLEEP, 0);
+		input_sync(onkey->input);
+		dev_dbg(onkey->dev, "KEY_SLEEP pressed.\n");
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void da9063_cancel_poll(void *data)
+{
+	struct da9063_onkey *onkey = data;
+
+	cancel_delayed_work_sync(&onkey->work);
+}
+
+static int da9063_onkey_probe(struct platform_device *pdev)
+{
+	struct da9063 *da9063 = dev_get_drvdata(pdev->dev.parent);
+	struct da9063_pdata *pdata = dev_get_platdata(da9063->dev);
+	struct da9063_onkey *onkey;
+	int irq;
+	int error;
+
+	onkey = devm_kzalloc(&pdev->dev, sizeof(struct da9063_onkey),
+			     GFP_KERNEL);
+	if (!onkey) {
+		dev_err(&pdev->dev, "Failed to allocate memory.\n");
+		return -ENOMEM;
+	}
+
+	onkey->dev = &pdev->dev;
+	onkey->hw = da9063;
+
+	if (pdata)
+		onkey->key_power = pdata->key_power;
+	else
+		onkey->key_power =
+			!of_property_read_bool(pdev->dev.of_node,
+					       "dlg,disable-key-power");
+
+	onkey->input = devm_input_allocate_device(&pdev->dev);
+	if (!onkey->input) {
+		dev_err(&pdev->dev, "Failed to allocated input device.\n");
+		return -ENOMEM;
+	}
+
+	onkey->input->name = DA9063_DRVNAME_ONKEY;
+	onkey->input->phys = DA9063_DRVNAME_ONKEY "/input0";
+	onkey->input->dev.parent = &pdev->dev;
+
+	if (onkey->key_power)
+		input_set_capability(onkey->input, EV_KEY, KEY_POWER);
+
+	input_set_capability(onkey->input, EV_KEY, KEY_SLEEP);
+
+	INIT_DELAYED_WORK(&onkey->work, da9063_poll_on);
+
+	error = devm_add_action(&pdev->dev, da9063_cancel_poll, onkey);
+	if (error) {
+		dev_err(&pdev->dev,
+			"Failed to add cancel poll action: %d\n",
+			error);
+		return error;
+	}
+
+	irq = platform_get_irq_byname(pdev, "ONKEY");
+	if (irq < 0) {
+		error = irq;
+		dev_err(&pdev->dev, "Failed to get platform IRQ: %d\n", error);
+		return error;
+	}
+
+	error = devm_request_threaded_irq(&pdev->dev, irq,
+					  NULL, da9063_onkey_irq_handler,
+					  IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					  "ONKEY", onkey);
+	if (error) {
+		dev_err(&pdev->dev,
+			"Failed to request IRQ %d: %d\n", irq, error);
+		return error;
+	}
+
+	error = input_register_device(onkey->input);
+	if (error) {
+		dev_err(&pdev->dev,
+			"Failed to register input device: %d\n", error);
+		return error;
+	}
+
+	platform_set_drvdata(pdev, onkey);
+	return 0;
+}
+
+static struct platform_driver da9063_onkey_driver = {
+	.probe	= da9063_onkey_probe,
+	.driver	= {
+		.name	= DA9063_DRVNAME_ONKEY,
+	},
+};
+module_platform_driver(da9063_onkey_driver);
+
+MODULE_AUTHOR("S Twiss <stwiss.opensource@diasemi.com>");
+MODULE_DESCRIPTION("Onkey device driver for Dialog DA9063");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DA9063_DRVNAME_ONKEY);
diff --git a/drivers/input/misc/drv260x.c b/drivers/input/misc/drv260x.c
index 599578042ea0..e5d60ecd29a4 100644
--- a/drivers/input/misc/drv260x.c
+++ b/drivers/input/misc/drv260x.c
@@ -580,15 +580,10 @@ static int drv260x_probe(struct i2c_client *client,
 		return error;
 	}
 
-	haptics->enable_gpio = devm_gpiod_get(&client->dev, "enable");
-	if (IS_ERR(haptics->enable_gpio)) {
-		error = PTR_ERR(haptics->enable_gpio);
-		if (error != -ENOENT && error != -ENOSYS)
-			return error;
-		haptics->enable_gpio = NULL;
-	} else {
-		gpiod_direction_output(haptics->enable_gpio, 1);
-	}
+	haptics->enable_gpio = devm_gpiod_get_optional(&client->dev, "enable",
+						       GPIOD_OUT_HIGH);
+	if (IS_ERR(haptics->enable_gpio))
+		return PTR_ERR(haptics->enable_gpio);
 
 	haptics->input_dev = devm_input_allocate_device(&client->dev);
 	if (!haptics->input_dev) {
diff --git a/drivers/input/misc/drv2665.c b/drivers/input/misc/drv2665.c
new file mode 100644
index 000000000000..0afaa33de07d
--- /dev/null
+++ b/drivers/input/misc/drv2665.c
@@ -0,0 +1,322 @@
+/*
+ * DRV2665 haptics driver family
+ *
+ * Author: Dan Murphy <dmurphy@ti.com>
+ *
+ * Copyright: (C) 2015 Texas Instruments, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+
+/* Contol registers */
+#define DRV2665_STATUS	0x00
+#define DRV2665_CTRL_1	0x01
+#define DRV2665_CTRL_2	0x02
+#define DRV2665_FIFO	0x0b
+
+/* Status Register */
+#define DRV2665_FIFO_FULL		BIT(0)
+#define DRV2665_FIFO_EMPTY		BIT(1)
+
+/* Control 1 Register */
+#define DRV2665_25_VPP_GAIN		0x00
+#define DRV2665_50_VPP_GAIN		0x01
+#define DRV2665_75_VPP_GAIN		0x02
+#define DRV2665_100_VPP_GAIN		0x03
+#define DRV2665_DIGITAL_IN		0xfc
+#define DRV2665_ANALOG_IN		BIT(2)
+
+/* Control 2 Register */
+#define DRV2665_BOOST_EN		BIT(1)
+#define DRV2665_STANDBY			BIT(6)
+#define DRV2665_DEV_RST			BIT(7)
+#define DRV2665_5_MS_IDLE_TOUT		0x00
+#define DRV2665_10_MS_IDLE_TOUT		0x04
+#define DRV2665_15_MS_IDLE_TOUT		0x08
+#define DRV2665_20_MS_IDLE_TOUT		0x0c
+
+/**
+ * struct drv2665_data -
+ * @input_dev - Pointer to the input device
+ * @client - Pointer to the I2C client
+ * @regmap - Register map of the device
+ * @work - Work item used to off load the enable/disable of the vibration
+ * @regulator - Pointer to the regulator for the IC
+ */
+struct drv2665_data {
+	struct input_dev *input_dev;
+	struct i2c_client *client;
+	struct regmap *regmap;
+	struct work_struct work;
+	struct regulator *regulator;
+};
+
+/* 8kHz Sine wave to stream to the FIFO */
+static const u8 drv2665_sine_wave_form[] = {
+	0x00, 0x10, 0x20, 0x2e, 0x3c, 0x48, 0x53, 0x5b, 0x61, 0x65, 0x66,
+	0x65, 0x61, 0x5b, 0x53, 0x48, 0x3c, 0x2e, 0x20, 0x10,
+	0x00, 0xf0, 0xe0, 0xd2, 0xc4, 0xb8, 0xad, 0xa5, 0x9f, 0x9b, 0x9a,
+	0x9b, 0x9f, 0xa5, 0xad, 0xb8, 0xc4, 0xd2, 0xe0, 0xf0, 0x00,
+};
+
+static struct reg_default drv2665_reg_defs[] = {
+	{ DRV2665_STATUS, 0x02 },
+	{ DRV2665_CTRL_1, 0x28 },
+	{ DRV2665_CTRL_2, 0x40 },
+	{ DRV2665_FIFO, 0x00 },
+};
+
+static void drv2665_worker(struct work_struct *work)
+{
+	struct drv2665_data *haptics =
+				container_of(work, struct drv2665_data, work);
+	unsigned int read_buf;
+	int error;
+
+	error = regmap_read(haptics->regmap, DRV2665_STATUS, &read_buf);
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to read status: %d\n", error);
+		return;
+	}
+
+	if (read_buf & DRV2665_FIFO_EMPTY) {
+		error = regmap_bulk_write(haptics->regmap,
+					  DRV2665_FIFO,
+					  drv2665_sine_wave_form,
+					  ARRAY_SIZE(drv2665_sine_wave_form));
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to write FIFO: %d\n", error);
+			return;
+		}
+	}
+}
+
+static int drv2665_haptics_play(struct input_dev *input, void *data,
+				struct ff_effect *effect)
+{
+	struct drv2665_data *haptics = input_get_drvdata(input);
+
+	schedule_work(&haptics->work);
+
+	return 0;
+}
+
+static void drv2665_close(struct input_dev *input)
+{
+	struct drv2665_data *haptics = input_get_drvdata(input);
+	int error;
+
+	cancel_work_sync(&haptics->work);
+
+	error = regmap_update_bits(haptics->regmap,
+				   DRV2665_CTRL_2, DRV2665_STANDBY, 1);
+	if (error)
+		dev_err(&haptics->client->dev,
+			"Failed to enter standby mode: %d\n", error);
+}
+
+static const struct reg_default drv2665_init_regs[] = {
+	{ DRV2665_CTRL_2, 0 | DRV2665_10_MS_IDLE_TOUT },
+	{ DRV2665_CTRL_1, DRV2665_25_VPP_GAIN },
+};
+
+static int drv2665_init(struct drv2665_data *haptics)
+{
+	int error;
+
+	error = regmap_register_patch(haptics->regmap,
+				      drv2665_init_regs,
+				      ARRAY_SIZE(drv2665_init_regs));
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to write init registers: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static const struct regmap_config drv2665_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = DRV2665_FIFO,
+	.reg_defaults = drv2665_reg_defs,
+	.num_reg_defaults = ARRAY_SIZE(drv2665_reg_defs),
+	.cache_type = REGCACHE_NONE,
+};
+
+static int drv2665_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct drv2665_data *haptics;
+	int error;
+
+	haptics = devm_kzalloc(&client->dev, sizeof(*haptics), GFP_KERNEL);
+	if (!haptics)
+		return -ENOMEM;
+
+	haptics->regulator = devm_regulator_get(&client->dev, "vbat");
+	if (IS_ERR(haptics->regulator)) {
+		error = PTR_ERR(haptics->regulator);
+		dev_err(&client->dev,
+			"unable to get regulator, error: %d\n", error);
+		return error;
+	}
+
+	haptics->input_dev = devm_input_allocate_device(&client->dev);
+	if (!haptics->input_dev) {
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	haptics->input_dev->name = "drv2665:haptics";
+	haptics->input_dev->dev.parent = client->dev.parent;
+	haptics->input_dev->close = drv2665_close;
+	input_set_drvdata(haptics->input_dev, haptics);
+	input_set_capability(haptics->input_dev, EV_FF, FF_RUMBLE);
+
+	error = input_ff_create_memless(haptics->input_dev, NULL,
+					drv2665_haptics_play);
+	if (error) {
+		dev_err(&client->dev, "input_ff_create() failed: %d\n",
+			error);
+		return error;
+	}
+
+	INIT_WORK(&haptics->work, drv2665_worker);
+
+	haptics->client = client;
+	i2c_set_clientdata(client, haptics);
+
+	haptics->regmap = devm_regmap_init_i2c(client, &drv2665_regmap_config);
+	if (IS_ERR(haptics->regmap)) {
+		error = PTR_ERR(haptics->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			error);
+		return error;
+	}
+
+	error = drv2665_init(haptics);
+	if (error) {
+		dev_err(&client->dev, "Device init failed: %d\n", error);
+		return error;
+	}
+
+	error = input_register_device(haptics->input_dev);
+	if (error) {
+		dev_err(&client->dev, "couldn't register input device: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int __maybe_unused drv2665_suspend(struct device *dev)
+{
+	struct drv2665_data *haptics = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&haptics->input_dev->mutex);
+
+	if (haptics->input_dev->users) {
+		ret = regmap_update_bits(haptics->regmap, DRV2665_CTRL_2,
+				DRV2665_STANDBY, 1);
+		if (ret) {
+			dev_err(dev, "Failed to set standby mode\n");
+			regulator_disable(haptics->regulator);
+			goto out;
+		}
+
+		ret = regulator_disable(haptics->regulator);
+		if (ret) {
+			dev_err(dev, "Failed to disable regulator\n");
+			regmap_update_bits(haptics->regmap,
+					   DRV2665_CTRL_2,
+					   DRV2665_STANDBY, 0);
+		}
+	}
+out:
+	mutex_unlock(&haptics->input_dev->mutex);
+	return ret;
+}
+
+static int __maybe_unused drv2665_resume(struct device *dev)
+{
+	struct drv2665_data *haptics = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&haptics->input_dev->mutex);
+
+	if (haptics->input_dev->users) {
+		ret = regulator_enable(haptics->regulator);
+		if (ret) {
+			dev_err(dev, "Failed to enable regulator\n");
+			goto out;
+		}
+
+		ret = regmap_update_bits(haptics->regmap, DRV2665_CTRL_2,
+					 DRV2665_STANDBY, 0);
+		if (ret) {
+			dev_err(dev, "Failed to unset standby mode\n");
+			regulator_disable(haptics->regulator);
+			goto out;
+		}
+
+	}
+
+out:
+	mutex_unlock(&haptics->input_dev->mutex);
+	return ret;
+}
+
+static SIMPLE_DEV_PM_OPS(drv2665_pm_ops, drv2665_suspend, drv2665_resume);
+
+static const struct i2c_device_id drv2665_id[] = {
+	{ "drv2665", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, drv2665_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id drv2665_of_match[] = {
+	{ .compatible = "ti,drv2665", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, drv2665_of_match);
+#endif
+
+static struct i2c_driver drv2665_driver = {
+	.probe		= drv2665_probe,
+	.driver		= {
+		.name	= "drv2665-haptics",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(drv2665_of_match),
+		.pm	= &drv2665_pm_ops,
+	},
+	.id_table = drv2665_id,
+};
+module_i2c_driver(drv2665_driver);
+
+MODULE_DESCRIPTION("TI DRV2665 haptics driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dan Murphy <dmurphy@ti.com>");
diff --git a/drivers/input/misc/gpio-beeper.c b/drivers/input/misc/gpio-beeper.c
index 4817c5f0c3e4..16272fffeb7e 100644
--- a/drivers/input/misc/gpio-beeper.c
+++ b/drivers/input/misc/gpio-beeper.c
@@ -66,13 +66,12 @@ static int gpio_beeper_probe(struct platform_device *pdev)
 {
 	struct gpio_beeper *beep;
 	struct input_dev *input;
-	int err;
 
 	beep = devm_kzalloc(&pdev->dev, sizeof(*beep), GFP_KERNEL);
 	if (!beep)
 		return -ENOMEM;
 
-	beep->desc = devm_gpiod_get(&pdev->dev, NULL);
+	beep->desc = devm_gpiod_get(&pdev->dev, NULL, GPIOD_OUT_LOW);
 	if (IS_ERR(beep->desc))
 		return PTR_ERR(beep->desc);
 
@@ -92,10 +91,6 @@ static int gpio_beeper_probe(struct platform_device *pdev)
 
 	input_set_capability(input, EV_SND, SND_BELL);
 
-	err = gpiod_direction_output(beep->desc, 0);
-	if (err)
-		return err;
-
 	input_set_drvdata(input, beep);
 
 	return input_register_device(input);
diff --git a/drivers/input/misc/retu-pwrbutton.c b/drivers/input/misc/retu-pwrbutton.c
index 0c8ac60e2639..30b459b6b344 100644
--- a/drivers/input/misc/retu-pwrbutton.c
+++ b/drivers/input/misc/retu-pwrbutton.c
@@ -63,7 +63,8 @@ static int retu_pwrbutton_probe(struct platform_device *pdev)
 	input_set_drvdata(idev, rdev);
 
 	error = devm_request_threaded_irq(&pdev->dev, irq,
-					  NULL, retu_pwrbutton_irq, 0,
+					  NULL, retu_pwrbutton_irq,
+					  IRQF_ONESHOT,
 					  "retu-pwrbutton", idev);
 	if (error)
 		return error;
diff --git a/drivers/input/misc/soc_button_array.c b/drivers/input/misc/soc_button_array.c
index e8e010a85484..c14b82709b0f 100644
--- a/drivers/input/misc/soc_button_array.c
+++ b/drivers/input/misc/soc_button_array.c
@@ -18,7 +18,6 @@
 #include <linux/gpio/consumer.h>
 #include <linux/gpio_keys.h>
 #include <linux/platform_device.h>
-#include <linux/acpi.h>
 
 /*
  * Definition of buttons on the tablet. The ACPI index of each button
diff --git a/drivers/input/misc/twl4030-pwrbutton.c b/drivers/input/misc/twl4030-pwrbutton.c
index e98cc81a84c6..603fc2fadf05 100644
--- a/drivers/input/misc/twl4030-pwrbutton.c
+++ b/drivers/input/misc/twl4030-pwrbutton.c
@@ -71,7 +71,8 @@ static int twl4030_pwrbutton_probe(struct platform_device *pdev)
 	pwr->dev.parent = &pdev->dev;
 
 	err = devm_request_threaded_irq(&pwr->dev, irq, NULL, powerbutton_irq,
-			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING |
+			IRQF_ONESHOT,
 			"twl4030_pwrbutton", pwr);
 	if (err < 0) {
 		dev_err(&pdev->dev, "Can't get IRQ for pwrbutton: %d\n", err);
diff --git a/drivers/input/misc/twl6040-vibra.c b/drivers/input/misc/twl6040-vibra.c
index 0e0d094df2e6..ea63fad48de6 100644
--- a/drivers/input/misc/twl6040-vibra.c
+++ b/drivers/input/misc/twl6040-vibra.c
@@ -308,7 +308,8 @@ static int twl6040_vibra_probe(struct platform_device *pdev)
 	mutex_init(&info->mutex);
 
 	error = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
-					  twl6040_vib_irq_handler, 0,
+					  twl6040_vib_irq_handler,
+					  IRQF_ONESHOT,
 					  "twl6040_irq_vib", info);
 	if (error) {
 		dev_err(info->dev, "VIB IRQ request failed: %d\n", error);
diff --git a/drivers/input/misc/wm831x-on.c b/drivers/input/misc/wm831x-on.c
index 59d4f7bcb4a3..1b44de265a0a 100644
--- a/drivers/input/misc/wm831x-on.c
+++ b/drivers/input/misc/wm831x-on.c
@@ -99,7 +99,8 @@ static int wm831x_on_probe(struct platform_device *pdev)
 	wm831x_on->dev->dev.parent = &pdev->dev;
 
 	ret = request_threaded_irq(irq, NULL, wm831x_on_irq,
-				   IRQF_TRIGGER_RISING, "wm831x_on",
+				   IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+				   "wm831x_on",
 				   wm831x_on);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Unable to request IRQ: %d\n", ret);
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index 7752bd59d4b7..113d6f1516a5 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -159,8 +159,8 @@ static const struct alps_protocol_info alps_v8_protocol_data = {
 
 static void alps_set_abs_params_st(struct alps_data *priv,
 				   struct input_dev *dev1);
-static void alps_set_abs_params_mt(struct alps_data *priv,
-				   struct input_dev *dev1);
+static void alps_set_abs_params_semi_mt(struct alps_data *priv,
+					struct input_dev *dev1);
 static void alps_set_abs_params_v7(struct alps_data *priv,
 				   struct input_dev *dev1);
 static void alps_set_abs_params_ss4_v2(struct alps_data *priv,
@@ -310,53 +310,6 @@ static void alps_process_packet_v1_v2(struct psmouse *psmouse)
 	input_sync(dev);
 }
 
-/*
- * Process bitmap data for V5 protocols. Return value is null.
- *
- * The bitmaps don't have enough data to track fingers, so this function
- * only generates points representing a bounding box of at most two contacts.
- * These two points are returned in fields->mt.
- */
-static void alps_process_bitmap_dolphin(struct alps_data *priv,
-					struct alps_fields *fields)
-{
-	int box_middle_x, box_middle_y;
-	unsigned int x_map, y_map;
-	unsigned char start_bit, end_bit;
-	unsigned char x_msb, x_lsb, y_msb, y_lsb;
-
-	x_map = fields->x_map;
-	y_map = fields->y_map;
-
-	if (!x_map || !y_map)
-		return;
-
-	/* Get Most-significant and Least-significant bit */
-	x_msb = fls(x_map);
-	x_lsb = ffs(x_map);
-	y_msb = fls(y_map);
-	y_lsb = ffs(y_map);
-
-	/* Most-significant bit should never exceed max sensor line number */
-	if (x_msb > priv->x_bits || y_msb > priv->y_bits)
-		return;
-
-	if (fields->fingers > 1) {
-		start_bit = priv->x_bits - x_msb;
-		end_bit = priv->x_bits - x_lsb;
-		box_middle_x = (priv->x_max * (start_bit + end_bit)) /
-				(2 * (priv->x_bits - 1));
-
-		start_bit = y_lsb - 1;
-		end_bit = y_msb - 1;
-		box_middle_y = (priv->y_max * (start_bit + end_bit)) /
-				(2 * (priv->y_bits - 1));
-		fields->mt[0] = fields->st;
-		fields->mt[1].x = 2 * box_middle_x - fields->mt[0].x;
-		fields->mt[1].y = 2 * box_middle_y - fields->mt[0].y;
-	}
-}
-
 static void alps_get_bitmap_points(unsigned int map,
 				   struct alps_bitmap_point *low,
 				   struct alps_bitmap_point *high,
@@ -384,7 +337,7 @@ static void alps_get_bitmap_points(unsigned int map,
 }
 
 /*
- * Process bitmap data from v3 and v4 protocols. Returns the number of
+ * Process bitmap data from semi-mt protocols. Returns the number of
  * fingers detected. A return value of 0 means at least one of the
  * bitmaps was empty.
  *
@@ -396,9 +349,10 @@ static void alps_get_bitmap_points(unsigned int map,
 static int alps_process_bitmap(struct alps_data *priv,
 			       struct alps_fields *fields)
 {
-	int i, fingers_x = 0, fingers_y = 0, fingers;
+	int i, fingers_x = 0, fingers_y = 0, fingers, closest;
 	struct alps_bitmap_point x_low = {0,}, x_high = {0,};
 	struct alps_bitmap_point y_low = {0,}, y_high = {0,};
+	struct input_mt_pos corner[4];
 
 	if (!fields->x_map || !fields->y_map)
 		return 0;
@@ -429,26 +383,76 @@ static int alps_process_bitmap(struct alps_data *priv,
 		y_high.num_bits = max(i, 1);
 	}
 
-	fields->mt[0].x =
+	/* top-left corner */
+	corner[0].x =
 		(priv->x_max * (2 * x_low.start_bit + x_low.num_bits - 1)) /
 		(2 * (priv->x_bits - 1));
-	fields->mt[0].y =
+	corner[0].y =
 		(priv->y_max * (2 * y_low.start_bit + y_low.num_bits - 1)) /
 		(2 * (priv->y_bits - 1));
 
-	fields->mt[1].x =
+	/* top-right corner */
+	corner[1].x =
 		(priv->x_max * (2 * x_high.start_bit + x_high.num_bits - 1)) /
 		(2 * (priv->x_bits - 1));
-	fields->mt[1].y =
+	corner[1].y =
+		(priv->y_max * (2 * y_low.start_bit + y_low.num_bits - 1)) /
+		(2 * (priv->y_bits - 1));
+
+	/* bottom-right corner */
+	corner[2].x =
+		(priv->x_max * (2 * x_high.start_bit + x_high.num_bits - 1)) /
+		(2 * (priv->x_bits - 1));
+	corner[2].y =
+		(priv->y_max * (2 * y_high.start_bit + y_high.num_bits - 1)) /
+		(2 * (priv->y_bits - 1));
+
+	/* bottom-left corner */
+	corner[3].x =
+		(priv->x_max * (2 * x_low.start_bit + x_low.num_bits - 1)) /
+		(2 * (priv->x_bits - 1));
+	corner[3].y =
 		(priv->y_max * (2 * y_high.start_bit + y_high.num_bits - 1)) /
 		(2 * (priv->y_bits - 1));
 
-	/* y-bitmap order is reversed, except on rushmore */
-	if (priv->proto_version != ALPS_PROTO_V3_RUSHMORE) {
-		fields->mt[0].y = priv->y_max - fields->mt[0].y;
-		fields->mt[1].y = priv->y_max - fields->mt[1].y;
+	/* x-bitmap order is reversed on v5 touchpads  */
+	if (priv->proto_version == ALPS_PROTO_V5) {
+		for (i = 0; i < 4; i++)
+			corner[i].x = priv->x_max - corner[i].x;
+	}
+
+	/* y-bitmap order is reversed on v3 and v4 touchpads  */
+	if (priv->proto_version == ALPS_PROTO_V3 ||
+	    priv->proto_version == ALPS_PROTO_V4) {
+		for (i = 0; i < 4; i++)
+			corner[i].y = priv->y_max - corner[i].y;
+	}
+
+	/*
+	 * We only select a corner for the second touch once per 2 finger
+	 * touch sequence to avoid the chosen corner (and thus the coordinates)
+	 * jumping around when the first touch is in the middle.
+	 */
+	if (priv->second_touch == -1) {
+		/* Find corner closest to our st coordinates */
+		closest = 0x7fffffff;
+		for (i = 0; i < 4; i++) {
+			int dx = fields->st.x - corner[i].x;
+			int dy = fields->st.y - corner[i].y;
+			int distance = dx * dx + dy * dy;
+
+			if (distance < closest) {
+				priv->second_touch = i;
+				closest = distance;
+			}
+		}
+		/* And select the opposite corner to use for the 2nd touch */
+		priv->second_touch = (priv->second_touch + 2) % 4;
 	}
 
+	fields->mt[0] = fields->st;
+	fields->mt[1] = corner[priv->second_touch];
+
 	return fingers;
 }
 
@@ -485,9 +489,14 @@ static void alps_report_semi_mt_data(struct psmouse *psmouse, int fingers)
 		f->mt[0].x = f->st.x;
 		f->mt[0].y = f->st.y;
 		fingers = f->pressure > 0 ? 1 : 0;
+		priv->second_touch = -1;
 	}
 
-	alps_report_mt_data(psmouse, (fingers <= 2) ? fingers : 2);
+	if (fingers >= 1)
+		alps_set_slot(dev, 0, f->mt[0].x, f->mt[0].y);
+	if (fingers >= 2)
+		alps_set_slot(dev, 1, f->mt[1].x, f->mt[1].y);
+	input_mt_sync_frame(dev);
 
 	input_mt_report_finger_count(dev, fingers);
 
@@ -584,20 +593,22 @@ static int alps_decode_pinnacle(struct alps_fields *f, unsigned char *p,
 	f->first_mp = !!(p[4] & 0x40);
 	f->is_mp = !!(p[0] & 0x40);
 
-	f->fingers = (p[5] & 0x3) + 1;
-	f->x_map = ((p[4] & 0x7e) << 8) |
-		   ((p[1] & 0x7f) << 2) |
-		   ((p[0] & 0x30) >> 4);
-	f->y_map = ((p[3] & 0x70) << 4) |
-		   ((p[2] & 0x7f) << 1) |
-		   (p[4] & 0x01);
-
-	f->st.x = ((p[1] & 0x7f) << 4) | ((p[4] & 0x30) >> 2) |
-	       ((p[0] & 0x30) >> 4);
-	f->st.y = ((p[2] & 0x7f) << 4) | (p[4] & 0x0f);
-	f->pressure = p[5] & 0x7f;
+	if (f->is_mp) {
+		f->fingers = (p[5] & 0x3) + 1;
+		f->x_map = ((p[4] & 0x7e) << 8) |
+			   ((p[1] & 0x7f) << 2) |
+			   ((p[0] & 0x30) >> 4);
+		f->y_map = ((p[3] & 0x70) << 4) |
+			   ((p[2] & 0x7f) << 1) |
+			   (p[4] & 0x01);
+	} else {
+		f->st.x = ((p[1] & 0x7f) << 4) | ((p[4] & 0x30) >> 2) |
+		       ((p[0] & 0x30) >> 4);
+		f->st.y = ((p[2] & 0x7f) << 4) | (p[4] & 0x0f);
+		f->pressure = p[5] & 0x7f;
 
-	alps_decode_buttons_v3(f, p);
+		alps_decode_buttons_v3(f, p);
+	}
 
 	return 0;
 }
@@ -605,13 +616,27 @@ static int alps_decode_pinnacle(struct alps_fields *f, unsigned char *p,
 static int alps_decode_rushmore(struct alps_fields *f, unsigned char *p,
 				 struct psmouse *psmouse)
 {
-	alps_decode_pinnacle(f, p, psmouse);
-
-	/* Rushmore's packet decode has a bit difference with Pinnacle's */
+	f->first_mp = !!(p[4] & 0x40);
 	f->is_mp = !!(p[5] & 0x40);
-	f->fingers = max((p[5] & 0x3), ((p[5] >> 2) & 0x3)) + 1;
-	f->x_map |= (p[5] & 0x10) << 11;
-	f->y_map |= (p[5] & 0x20) << 6;
+
+	if (f->is_mp) {
+		f->fingers = max((p[5] & 0x3), ((p[5] >> 2) & 0x3)) + 1;
+		f->x_map = ((p[5] & 0x10) << 11) |
+			   ((p[4] & 0x7e) << 8) |
+			   ((p[1] & 0x7f) << 2) |
+			   ((p[0] & 0x30) >> 4);
+		f->y_map = ((p[5] & 0x20) << 6) |
+			   ((p[3] & 0x70) << 4) |
+			   ((p[2] & 0x7f) << 1) |
+			   (p[4] & 0x01);
+	} else {
+		f->st.x = ((p[1] & 0x7f) << 4) | ((p[4] & 0x30) >> 2) |
+		       ((p[0] & 0x30) >> 4);
+		f->st.y = ((p[2] & 0x7f) << 4) | (p[4] & 0x0f);
+		f->pressure = p[5] & 0x7f;
+
+		alps_decode_buttons_v3(f, p);
+	}
 
 	return 0;
 }
@@ -680,30 +705,13 @@ static void alps_process_touchpad_packet_v3_v5(struct psmouse *psmouse)
 		 */
 		if (f->is_mp) {
 			fingers = f->fingers;
-			if (priv->proto_version == ALPS_PROTO_V3 ||
-			    priv->proto_version == ALPS_PROTO_V3_RUSHMORE) {
-				if (alps_process_bitmap(priv, f) == 0)
-					fingers = 0; /* Use st data */
-
-				/* Now process position packet */
-				priv->decode_fields(f, priv->multi_data,
-						    psmouse);
-			} else {
-				/*
-				 * Because Dolphin uses position packet's
-				 * coordinate data as Pt1 and uses it to
-				 * calculate Pt2, so we need to do position
-				 * packet decode first.
-				 */
-				priv->decode_fields(f, priv->multi_data,
-						    psmouse);
-
-				/*
-				 * Since Dolphin's finger number is reliable,
-				 * there is no need to compare with bmap_fn.
-				 */
-				alps_process_bitmap_dolphin(priv, f);
-			}
+			/*
+			 * Bitmap processing uses position packet's coordinate
+			 * data, so we need to do decode it first.
+			 */
+			priv->decode_fields(f, priv->multi_data, psmouse);
+			if (alps_process_bitmap(priv, f) == 0)
+				fingers = 0; /* Use st data */
 		} else {
 			priv->multi_packet = 0;
 		}
@@ -865,6 +873,14 @@ static void alps_process_packet_v4(struct psmouse *psmouse)
 	priv->multi_data[offset] = packet[6];
 	priv->multi_data[offset + 1] = packet[7];
 
+	f->left = !!(packet[4] & 0x01);
+	f->right = !!(packet[4] & 0x02);
+
+	f->st.x = ((packet[1] & 0x7f) << 4) | ((packet[3] & 0x30) >> 2) |
+		  ((packet[0] & 0x30) >> 4);
+	f->st.y = ((packet[2] & 0x7f) << 4) | (packet[3] & 0x0f);
+	f->pressure = packet[5] & 0x7f;
+
 	if (++priv->multi_packet > 2) {
 		priv->multi_packet = 0;
 
@@ -879,14 +895,6 @@ static void alps_process_packet_v4(struct psmouse *psmouse)
 		f->fingers = alps_process_bitmap(priv, f);
 	}
 
-	f->left = !!(packet[4] & 0x01);
-	f->right = !!(packet[4] & 0x02);
-
-	f->st.x = ((packet[1] & 0x7f) << 4) | ((packet[3] & 0x30) >> 2) |
-		  ((packet[0] & 0x30) >> 4);
-	f->st.y = ((packet[2] & 0x7f) << 4) | (packet[3] & 0x0f);
-	f->pressure = packet[5] & 0x7f;
-
 	alps_report_semi_mt_data(psmouse, f->fingers);
 }
 
@@ -1063,9 +1071,8 @@ static void alps_process_trackstick_packet_v7(struct psmouse *psmouse)
 	right = (packet[1] & 0x02) >> 1;
 	middle = (packet[1] & 0x04) >> 2;
 
-	/* Divide 2 since trackpoint's speed is too fast */
-	input_report_rel(dev2, REL_X, (char)x / 2);
-	input_report_rel(dev2, REL_Y, -((char)y / 2));
+	input_report_rel(dev2, REL_X, (char)x);
+	input_report_rel(dev2, REL_Y, -((char)y));
 
 	input_report_key(dev2, BTN_LEFT, left);
 	input_report_key(dev2, BTN_RIGHT, right);
@@ -2562,7 +2569,7 @@ static int alps_set_protocol(struct psmouse *psmouse,
 	case ALPS_PROTO_V3:
 		priv->hw_init = alps_hw_init_v3;
 		priv->process_packet = alps_process_packet_v3;
-		priv->set_abs_params = alps_set_abs_params_mt;
+		priv->set_abs_params = alps_set_abs_params_semi_mt;
 		priv->decode_fields = alps_decode_pinnacle;
 		priv->nibble_commands = alps_v3_nibble_commands;
 		priv->addr_command = PSMOUSE_CMD_RESET_WRAP;
@@ -2571,7 +2578,7 @@ static int alps_set_protocol(struct psmouse *psmouse,
 	case ALPS_PROTO_V3_RUSHMORE:
 		priv->hw_init = alps_hw_init_rushmore_v3;
 		priv->process_packet = alps_process_packet_v3;
-		priv->set_abs_params = alps_set_abs_params_mt;
+		priv->set_abs_params = alps_set_abs_params_semi_mt;
 		priv->decode_fields = alps_decode_rushmore;
 		priv->nibble_commands = alps_v3_nibble_commands;
 		priv->addr_command = PSMOUSE_CMD_RESET_WRAP;
@@ -2587,7 +2594,7 @@ static int alps_set_protocol(struct psmouse *psmouse,
 	case ALPS_PROTO_V4:
 		priv->hw_init = alps_hw_init_v4;
 		priv->process_packet = alps_process_packet_v4;
-		priv->set_abs_params = alps_set_abs_params_mt;
+		priv->set_abs_params = alps_set_abs_params_semi_mt;
 		priv->nibble_commands = alps_v4_nibble_commands;
 		priv->addr_command = PSMOUSE_CMD_DISABLE;
 		break;
@@ -2596,7 +2603,7 @@ static int alps_set_protocol(struct psmouse *psmouse,
 		priv->hw_init = alps_hw_init_dolphin_v1;
 		priv->process_packet = alps_process_touchpad_packet_v3_v5;
 		priv->decode_fields = alps_decode_dolphin;
-		priv->set_abs_params = alps_set_abs_params_mt;
+		priv->set_abs_params = alps_set_abs_params_semi_mt;
 		priv->nibble_commands = alps_v3_nibble_commands;
 		priv->addr_command = PSMOUSE_CMD_RESET_WRAP;
 		priv->x_bits = 23;
@@ -2778,15 +2785,15 @@ static void alps_set_abs_params_mt_common(struct alps_data *priv,
 	set_bit(BTN_TOOL_QUADTAP, dev1->keybit);
 }
 
-static void alps_set_abs_params_mt(struct alps_data *priv,
-				   struct input_dev *dev1)
+static void alps_set_abs_params_semi_mt(struct alps_data *priv,
+					struct input_dev *dev1)
 {
 	alps_set_abs_params_mt_common(priv, dev1);
 	input_set_abs_params(dev1, ABS_PRESSURE, 0, 127, 0, 0);
 
 	input_mt_init_slots(dev1, MAX_TOUCHES,
 			    INPUT_MT_POINTER | INPUT_MT_DROP_UNUSED |
-				INPUT_MT_TRACK | INPUT_MT_SEMI_MT);
+				INPUT_MT_SEMI_MT);
 }
 
 static void alps_set_abs_params_v7(struct alps_data *priv,
diff --git a/drivers/input/mouse/alps.h b/drivers/input/mouse/alps.h
index 6dfdccc3a7c6..d37f814dc447 100644
--- a/drivers/input/mouse/alps.h
+++ b/drivers/input/mouse/alps.h
@@ -278,6 +278,7 @@ struct alps_data {
 
 	int prev_fin;
 	int multi_packet;
+	int second_touch;
 	unsigned char multi_data[6];
 	struct alps_fields f;
 	u8 quirks;
diff --git a/drivers/input/mouse/cyapa_gen3.c b/drivers/input/mouse/cyapa_gen3.c
index 1e2291c378fe..3faf01c1b191 100644
--- a/drivers/input/mouse/cyapa_gen3.c
+++ b/drivers/input/mouse/cyapa_gen3.c
@@ -950,14 +950,13 @@ static u16 cyapa_get_wait_time_for_pwr_cmd(u8 pwr_mode)
  * Device power mode can only be set when device is in operational mode.
  */
 static int cyapa_gen3_set_power_mode(struct cyapa *cyapa, u8 power_mode,
-		u16 always_unused)
+				     u16 always_unused)
 {
 	int ret;
 	u8 power;
 	int tries;
 	u16 sleep_time;
 
-	always_unused = 0;
 	if (cyapa->state != CYAPA_STATE_OP)
 		return 0;
 
diff --git a/drivers/input/mouse/cyapa_gen5.c b/drivers/input/mouse/cyapa_gen5.c
index 5b611dd71e79..afc39e799da2 100644
--- a/drivers/input/mouse/cyapa_gen5.c
+++ b/drivers/input/mouse/cyapa_gen5.c
@@ -352,7 +352,7 @@ struct gen5_app_cmd_head {
 	u8 parameter_data[0];  /* Parameter data variable based on cmd_code */
 } __packed;
 
-/* Applicaton get/set parameter command data structure */
+/* Application get/set parameter command data structure */
 struct gen5_app_set_parameter_data {
 	u8 parameter_id;
 	u8 parameter_size;
@@ -832,7 +832,7 @@ static int gen5_hid_description_header_parse(struct cyapa *cyapa, u8 *reg_data)
 	int ret;
 
 	/* 0x20 0x00 0xF7 is Gen5 Application HID Description Header;
-	 * 0x20 0x00 0xFF is Gen5 Booloader HID Description Header.
+	 * 0x20 0x00 0xFF is Gen5 Bootloader HID Description Header.
 	 *
 	 * Must read HID Description content through out,
 	 * otherwise Gen5 trackpad cannot response next command
@@ -1654,8 +1654,8 @@ static int cyapa_gen5_set_power_mode(struct cyapa *cyapa,
 		 * that trackpad unable to report signal to wake system up
 		 * in the special situation that system is in suspending, and
 		 * at the same time, user touch trackpad to wake system up.
-		 * This function can avoid the data to be buffured when system
-		 * is suspending which may cause interrput line unable to be
+		 * This function can avoid the data to be buffered when system
+		 * is suspending which may cause interrupt line unable to be
 		 * asserted again.
 		 */
 		cyapa_empty_pip_output_data(cyapa, NULL, NULL, NULL);
@@ -2546,16 +2546,11 @@ static bool cyapa_gen5_irq_cmd_handler(struct cyapa *cyapa)
 			gen5_pip->resp_sort_func(cyapa,
 				gen5_pip->irq_cmd_buf, length))) {
 			/*
-			 * Cover the Gen5 V1 firmware issue.
-			 * The issue is there is no interrut will be
-			 * asserted to notityf host to read a command
-			 * data out when always has finger touch on
-			 * trackpad during the command is issued to
-			 * trackad device.
-			 * This issue has the scenario is that,
-			 * user always has his fingers touched on
-			 * trackpad device when booting/rebooting
-			 * their chrome book.
+			 * Work around the Gen5 V1 firmware
+			 * that does not assert interrupt signalling
+			 * that command response is ready if user
+			 * keeps touching the trackpad while command
+			 * is sent to the device.
 			 */
 			length = 0;
 			if (gen5_pip->resp_len)
diff --git a/drivers/input/mouse/elan_i2c.h b/drivers/input/mouse/elan_i2c.h
index 6d5f8a4c1748..73670f2aebfd 100644
--- a/drivers/input/mouse/elan_i2c.h
+++ b/drivers/input/mouse/elan_i2c.h
@@ -28,14 +28,13 @@
 #define ETP_PRESSURE_OFFSET	25
 
 /* IAP Firmware handling */
-#define ETP_FW_NAME		"elan_i2c.bin"
+#define ETP_PRODUCT_ID_FORMAT_STRING	"%d.0"
+#define ETP_FW_NAME		"elan_i2c_" ETP_PRODUCT_ID_FORMAT_STRING ".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_VAILDPAGE_COUNT	768
 #define ETP_FW_SIGNATURE_SIZE	6
-#define ETP_FW_SIGNATURE_ADDRESS	0xBFFA
 
 struct i2c_client;
 struct completion;
@@ -58,7 +57,8 @@ struct elan_transport_ops {
 				 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_sm_version)(struct i2c_client *client,
+			      u8* ic_type, u8 *version);
 	int (*get_checksum)(struct i2c_client *client, bool iap, u16 *csum);
 	int (*get_product_id)(struct i2c_client *client, u8 *id);
 
diff --git a/drivers/input/mouse/elan_i2c_core.c b/drivers/input/mouse/elan_i2c_core.c
index fd5068b2542d..5b5f403d8ce6 100644
--- a/drivers/input/mouse/elan_i2c_core.c
+++ b/drivers/input/mouse/elan_i2c_core.c
@@ -4,7 +4,7 @@
  * Copyright (c) 2013 ELAN Microelectronics Corp.
  *
  * Author: 林政維 (Duson Lin) <dusonlin@emc.com.tw>
- * Version: 1.5.7
+ * Version: 1.5.9
  *
  * Based on cyapa driver:
  * copyright (c) 2011-2012 Cypress Semiconductor, Inc.
@@ -40,7 +40,7 @@
 #include "elan_i2c.h"
 
 #define DRIVER_NAME		"elan_i2c"
-#define ELAN_DRIVER_VERSION	"1.5.7"
+#define ELAN_DRIVER_VERSION	"1.5.9"
 #define ETP_MAX_PRESSURE	255
 #define ETP_FWIDTH_REDUCE	90
 #define ETP_FINGER_WIDTH	15
@@ -83,6 +83,9 @@ struct elan_tp_data {
 	u16			fw_checksum;
 	int			pressure_adjustment;
 	u8			mode;
+	u8			ic_type;
+	u16			fw_vaildpage_count;
+	u16			fw_signature_address;
 
 	bool			irq_wake;
 
@@ -91,6 +94,29 @@ struct elan_tp_data {
 	bool			baseline_ready;
 };
 
+static int elan_get_fwinfo(u8 ic_type, u16 *vaildpage_count,
+			   u16 *signature_address)
+{
+	switch(ic_type) {
+	case 0x09:
+		*vaildpage_count = 768;
+		break;
+	case 0x0D:
+		*vaildpage_count = 896;
+		break;
+	default:
+		/* unknown ic type clear value */
+		*vaildpage_count = 0;
+		*signature_address = 0;
+		return -ENXIO;
+	}
+
+	*signature_address =
+		(*vaildpage_count * ETP_FW_PAGE_SIZE) - ETP_FW_SIGNATURE_SIZE;
+
+	return 0;
+}
+
 static int elan_enable_power(struct elan_tp_data *data)
 {
 	int repeat = ETP_RETRY_COUNT;
@@ -221,7 +247,8 @@ static int elan_query_device_info(struct elan_tp_data *data)
 	if (error)
 		return error;
 
-	error = data->ops->get_sm_version(data->client, &data->sm_version);
+	error = data->ops->get_sm_version(data->client, &data->ic_type,
+					  &data->sm_version);
 	if (error)
 		return error;
 
@@ -234,6 +261,14 @@ static int elan_query_device_info(struct elan_tp_data *data)
 	if (error)
 		return error;
 
+	error = elan_get_fwinfo(data->ic_type, &data->fw_vaildpage_count,
+				&data->fw_signature_address);
+	if (error) {
+		dev_err(&data->client->dev,
+			"unknown ic type %d\n", data->ic_type);
+		return error;
+	}
+
 	return 0;
 }
 
@@ -318,7 +353,7 @@ static int __elan_update_firmware(struct elan_tp_data *data,
 	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_VAILDPAGE_COUNT; i++) {
+	for (i = boot_page_count; i < data->fw_vaildpage_count; i++) {
 		u16 checksum = 0;
 		const u8 *page = &fw->data[i * ETP_FW_PAGE_SIZE];
 
@@ -403,7 +438,8 @@ static ssize_t elan_sysfs_read_product_id(struct device *dev,
 	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);
+	return sprintf(buf, ETP_PRODUCT_ID_FORMAT_STRING "\n",
+		       data->product_id);
 }
 
 static ssize_t elan_sysfs_read_fw_ver(struct device *dev,
@@ -442,19 +478,28 @@ static ssize_t elan_sysfs_update_fw(struct device *dev,
 {
 	struct elan_tp_data *data = dev_get_drvdata(dev);
 	const struct firmware *fw;
+	char *fw_name;
 	int error;
 	const u8 *fw_signature;
 	static const u8 signature[] = {0xAA, 0x55, 0xCC, 0x33, 0xFF, 0xFF};
 
-	error = request_firmware(&fw, ETP_FW_NAME, dev);
+	/* Look for a firmware with the product id appended. */
+	fw_name = kasprintf(GFP_KERNEL, ETP_FW_NAME, data->product_id);
+	if (!fw_name) {
+		dev_err(dev, "failed to allocate memory for firmware name\n");
+		return -ENOMEM;
+	}
+
+	dev_info(dev, "requesting fw '%s'\n", fw_name);
+	error = request_firmware(&fw, fw_name, dev);
+	kfree(fw_name);
 	if (error) {
-		dev_err(dev, "cannot load firmware %s: %d\n",
-			ETP_FW_NAME, error);
+		dev_err(dev, "failed to request firmware: %d\n", error);
 		return error;
 	}
 
 	/* Firmware file must match signature data */
-	fw_signature = &fw->data[ETP_FW_SIGNATURE_ADDRESS];
+	fw_signature = &fw->data[data->fw_signature_address];
 	if (memcmp(fw_signature, signature, sizeof(signature)) != 0) {
 		dev_err(dev, "signature mismatch (expected %*ph, got %*ph)\n",
 			(int)sizeof(signature), signature,
@@ -726,7 +771,7 @@ static const struct attribute_group *elan_sysfs_groups[] = {
  */
 static void elan_report_contact(struct elan_tp_data *data,
 				int contact_num, bool contact_valid,
-				bool hover_event, u8 *finger_data)
+				u8 *finger_data)
 {
 	struct input_dev *input = data->input;
 	unsigned int pos_x, pos_y;
@@ -770,9 +815,7 @@ static void elan_report_contact(struct elan_tp_data *data,
 		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_DISTANCE, hover_event);
-		input_report_abs(input, ABS_MT_PRESSURE,
-				 hover_event ? 0 : scaled_pressure);
+		input_report_abs(input, ABS_MT_PRESSURE, scaled_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);
@@ -794,14 +837,14 @@ static void elan_report_absolute(struct elan_tp_data *data, u8 *packet)
 	hover_event = hover_info & 0x40;
 	for (i = 0; i < ETP_MAX_FINGERS; i++) {
 		contact_valid = tp_info & (1U << (3 + i));
-		elan_report_contact(data, i, contact_valid, hover_event,
-				    finger_data);
+		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_report_abs(input, ABS_DISTANCE, hover_event != 0);
 	input_mt_report_pointer_emulation(input, true);
 	input_sync(input);
 }
@@ -877,6 +920,7 @@ static int elan_setup_input_device(struct elan_tp_data *data)
 	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);
+	input_set_abs_params(input, ABS_DISTANCE, 0, 1, 0, 0);
 
 	/* And MT parameters */
 	input_set_abs_params(input, ABS_MT_POSITION_X, 0, data->max_x, 0, 0);
@@ -889,7 +933,6 @@ static int elan_setup_input_device(struct elan_tp_data *data)
 			     ETP_FINGER_WIDTH * max_width, 0, 0);
 	input_set_abs_params(input, ABS_MT_TOUCH_MINOR, 0,
 			     ETP_FINGER_WIDTH * min_width, 0, 0);
-	input_set_abs_params(input, ABS_MT_DISTANCE, 0, 1, 0, 0);
 
 	data->input = input;
 
diff --git a/drivers/input/mouse/elan_i2c_i2c.c b/drivers/input/mouse/elan_i2c_i2c.c
index a0acbbf83bfd..683c840c9dd7 100644
--- a/drivers/input/mouse/elan_i2c_i2c.c
+++ b/drivers/input/mouse/elan_i2c_i2c.c
@@ -259,7 +259,8 @@ static int elan_i2c_get_version(struct i2c_client *client,
 	return 0;
 }
 
-static int elan_i2c_get_sm_version(struct i2c_client *client, u8 *version)
+static int elan_i2c_get_sm_version(struct i2c_client *client,
+				   u8 *ic_type, u8 *version)
 {
 	int error;
 	u8 val[3];
@@ -271,6 +272,7 @@ static int elan_i2c_get_sm_version(struct i2c_client *client, u8 *version)
 	}
 
 	*version = val[0];
+	*ic_type = val[1];
 	return 0;
 }
 
diff --git a/drivers/input/mouse/elan_i2c_smbus.c b/drivers/input/mouse/elan_i2c_smbus.c
index 30ab80dbcdd6..ff36a366b2aa 100644
--- a/drivers/input/mouse/elan_i2c_smbus.c
+++ b/drivers/input/mouse/elan_i2c_smbus.c
@@ -165,7 +165,8 @@ static int elan_smbus_get_version(struct i2c_client *client,
 	return 0;
 }
 
-static int elan_smbus_get_sm_version(struct i2c_client *client, u8 *version)
+static int elan_smbus_get_sm_version(struct i2c_client *client,
+				     u8 *ic_type, u8 *version)
 {
 	int error;
 	u8 val[3];
@@ -177,7 +178,8 @@ static int elan_smbus_get_sm_version(struct i2c_client *client, u8 *version)
 		return error;
 	}
 
-	*version = val[0]; /* XXX Why 0 and not 2 as in IAP/FW versions? */
+	*version = val[0];
+	*ic_type = val[1];
 	return 0;
 }
 
diff --git a/drivers/input/mouse/elantech.c b/drivers/input/mouse/elantech.c
index 79363b687195..ce3d40004458 100644
--- a/drivers/input/mouse/elantech.c
+++ b/drivers/input/mouse/elantech.c
@@ -1376,10 +1376,11 @@ static bool elantech_is_signature_valid(const unsigned char *param)
 		return true;
 
 	/*
-	 * Some models have a revision higher then 20. Meaning param[2] may
-	 * be 10 or 20, skip the rates check for these.
+	 * Some hw_version >= 4 models have a revision higher then 20. Meaning
+	 * that param[2] may be 10 or 20, skip the rates check for these.
 	 */
-	if (param[0] == 0x46 && (param[1] & 0xef) == 0x0f && param[2] < 40)
+	if ((param[0] & 0x0f) >= 0x06 && (param[1] & 0xaf) == 0x0f &&
+	    param[2] < 40)
 		return true;
 
 	for (i = 0; i < ARRAY_SIZE(rates); i++)
@@ -1555,6 +1556,7 @@ static int elantech_set_properties(struct elantech_data *etd)
 		case 9:
 		case 10:
 		case 13:
+		case 14:
 			etd->hw_version = 4;
 			break;
 		default:
diff --git a/drivers/input/mouse/focaltech.c b/drivers/input/mouse/focaltech.c
index 23d259416f2f..4d5576de81be 100644
--- a/drivers/input/mouse/focaltech.c
+++ b/drivers/input/mouse/focaltech.c
@@ -103,6 +103,16 @@ struct focaltech_hw_state {
 	 */
 	struct focaltech_finger_state fingers[FOC_MAX_FINGERS];
 
+	/*
+	 * Finger width 0-7 and 15 for a very big contact area.
+	 * 15 value stays until the finger is released.
+	 * Width is reported only in absolute packets.
+	 * Since hardware reports width only for last touching finger,
+	 * there is no need to store width for every specific finger,
+	 * so we keep only last value reported.
+	 */
+	unsigned int width;
+
 	/* True if the clickpad has been pressed. */
 	bool pressed;
 };
@@ -137,6 +147,7 @@ static void focaltech_report_state(struct psmouse *psmouse)
 			input_report_abs(dev, ABS_MT_POSITION_X, clamped_x);
 			input_report_abs(dev, ABS_MT_POSITION_Y,
 					 priv->y_max - clamped_y);
+			input_report_abs(dev, ABS_TOOL_WIDTH, state->width);
 		}
 	}
 	input_mt_report_pointer_emulation(dev, true);
@@ -187,6 +198,7 @@ static void focaltech_process_abs_packet(struct psmouse *psmouse,
 
 	state->fingers[finger].x = ((packet[1] & 0xf) << 8) | packet[2];
 	state->fingers[finger].y = (packet[3] << 8) | packet[4];
+	state->width = packet[5] >> 4;
 	state->fingers[finger].valid = true;
 }
 
@@ -331,6 +343,7 @@ static void focaltech_set_input_params(struct psmouse *psmouse)
 	__set_bit(EV_ABS, dev->evbit);
 	input_set_abs_params(dev, ABS_MT_POSITION_X, 0, priv->x_max, 0, 0);
 	input_set_abs_params(dev, ABS_MT_POSITION_Y, 0, priv->y_max, 0, 0);
+	input_set_abs_params(dev, ABS_TOOL_WIDTH, 0, 15, 0, 0);
 	input_mt_init_slots(dev, 5, INPUT_MT_POINTER);
 	__set_bit(INPUT_PROP_BUTTONPAD, dev->propbit);
 }
diff --git a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
index 5bb1658f60c7..ec3477036150 100644
--- a/drivers/input/mouse/psmouse-base.c
+++ b/drivers/input/mouse/psmouse-base.c
@@ -47,7 +47,7 @@ MODULE_LICENSE("GPL");
 static unsigned int psmouse_max_proto = PSMOUSE_AUTO;
 static int psmouse_set_maxproto(const char *val, const struct kernel_param *);
 static int psmouse_get_maxproto(char *buffer, const struct kernel_param *kp);
-static struct kernel_param_ops param_ops_proto_abbrev = {
+static const struct kernel_param_ops param_ops_proto_abbrev = {
 	.set = psmouse_set_maxproto,
 	.get = psmouse_get_maxproto,
 };
@@ -63,7 +63,7 @@ static unsigned int psmouse_rate = 100;
 module_param_named(rate, psmouse_rate, uint, 0644);
 MODULE_PARM_DESC(rate, "Report rate, in reports per second.");
 
-static bool psmouse_smartscroll = 1;
+static bool psmouse_smartscroll = true;
 module_param_named(smartscroll, psmouse_smartscroll, bool, 0644);
 MODULE_PARM_DESC(smartscroll, "Logitech Smartscroll autorepeat, 1 = enabled (default), 0 = disabled.");
 
diff --git a/drivers/input/mouse/sentelic.h b/drivers/input/mouse/sentelic.h
index aa697ece405b..42df9e3beae8 100644
--- a/drivers/input/mouse/sentelic.h
+++ b/drivers/input/mouse/sentelic.h
@@ -123,11 +123,11 @@ struct fsp_data {
 extern int fsp_detect(struct psmouse *psmouse, bool set_properties);
 extern int fsp_init(struct psmouse *psmouse);
 #else
-inline int fsp_detect(struct psmouse *psmouse, bool set_properties)
+static inline int fsp_detect(struct psmouse *psmouse, bool set_properties)
 {
 	return -ENOSYS;
 }
-inline int fsp_init(struct psmouse *psmouse)
+static inline int fsp_init(struct psmouse *psmouse)
 {
 	return -ENOSYS;
 }
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index 630af73e98c4..3a32caf06bf1 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -151,6 +151,11 @@ static const struct min_max_quirk min_max_pnpid_table[] = {
 		1024, 5112, 2024, 4832
 	},
 	{
+		(const char * const []){"LEN2000", NULL},
+		{ANY_BOARD_ID, ANY_BOARD_ID},
+		1024, 5113, 2021, 4832
+	},
+	{
 		(const char * const []){"LEN2001", NULL},
 		{ANY_BOARD_ID, ANY_BOARD_ID},
 		1024, 5022, 2508, 4832
@@ -191,7 +196,7 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	"LEN0045",
 	"LEN0047",
 	"LEN0049",
-	"LEN2000",
+	"LEN2000", /* S540 */
 	"LEN2001", /* Edge E431 */
 	"LEN2002", /* Edge E531 */
 	"LEN2003",
@@ -1194,7 +1199,7 @@ static void set_input_params(struct psmouse *psmouse,
 					ABS_MT_POSITION_Y);
 		/* Image sensors can report per-contact pressure */
 		input_set_abs_params(dev, ABS_MT_PRESSURE, 0, 255, 0, 0);
-		input_mt_init_slots(dev, 3, INPUT_MT_POINTER | INPUT_MT_TRACK);
+		input_mt_init_slots(dev, 2, INPUT_MT_POINTER | INPUT_MT_TRACK);
 
 		/* Image sensors can signal 4 and 5 finger clicks */
 		__set_bit(BTN_TOOL_QUADTAP, dev->keybit);
diff --git a/drivers/input/mouse/synaptics_i2c.c b/drivers/input/mouse/synaptics_i2c.c
index 878f18498f3b..ffceedcaf3c8 100644
--- a/drivers/input/mouse/synaptics_i2c.c
+++ b/drivers/input/mouse/synaptics_i2c.c
@@ -185,7 +185,7 @@
 #define NO_DATA_SLEEP_MSECS	(MSEC_PER_SEC / 4)
 
 /* Control touchpad's No Deceleration option */
-static bool no_decel = 1;
+static bool no_decel = true;
 module_param(no_decel, bool, 0644);
 MODULE_PARM_DESC(no_decel, "No Deceleration. Default = 1 (on)");
 
@@ -340,9 +340,9 @@ static bool synaptics_i2c_get_input(struct synaptics_i2c *touch)
 	s32 data;
 	s8 x_delta, y_delta;
 
-	/* Deal with spontanious resets and errors */
+	/* Deal with spontaneous resets and errors */
 	if (synaptics_i2c_check_error(touch->client))
-		return 0;
+		return false;
 
 	/* Get Gesture Bit */
 	data = synaptics_i2c_reg_get(touch->client, DATA_REG0);
diff --git a/drivers/input/serio/Kconfig b/drivers/input/serio/Kconfig
index 77833d7a004b..200841b77edb 100644
--- a/drivers/input/serio/Kconfig
+++ b/drivers/input/serio/Kconfig
@@ -244,6 +244,7 @@ config SERIO_PS2MULT
 
 config SERIO_ARC_PS2
 	tristate "ARC PS/2 support"
+	depends on HAS_IOMEM
 	help
 	  Say Y here if you have an ARC FPGA platform with a PS/2
 	  controller in it.
diff --git a/drivers/input/serio/serport.c b/drivers/input/serio/serport.c
index 69175b825346..9c927d35c1f5 100644
--- a/drivers/input/serio/serport.c
+++ b/drivers/input/serio/serport.c
@@ -167,7 +167,6 @@ static ssize_t serport_ldisc_read(struct tty_struct * tty, struct file * file, u
 {
 	struct serport *serport = (struct serport*) tty->disc_data;
 	struct serio *serio;
-	char name[64];
 
 	if (test_and_set_bit(SERPORT_BUSY, &serport->flags))
 		return -EBUSY;
@@ -177,7 +176,7 @@ static ssize_t serport_ldisc_read(struct tty_struct * tty, struct file * file, u
 		return -ENOMEM;
 
 	strlcpy(serio->name, "Serial port", sizeof(serio->name));
-	snprintf(serio->phys, sizeof(serio->phys), "%s/serio0", tty_name(tty, name));
+	snprintf(serio->phys, sizeof(serio->phys), "%s/serio0", tty_name(tty));
 	serio->id = serport->id;
 	serio->id.type = SERIO_RS232;
 	serio->write = serport_serio_write;
@@ -187,7 +186,7 @@ static ssize_t serport_ldisc_read(struct tty_struct * tty, struct file * file, u
 	serio->dev.parent = tty->dev;
 
 	serio_register_port(serport->serio);
-	printk(KERN_INFO "serio: Serial port %s\n", tty_name(tty, name));
+	printk(KERN_INFO "serio: Serial port %s\n", tty_name(tty));
 
 	wait_event_interruptible(serport->wait, test_bit(SERPORT_DEAD, &serport->flags));
 	serio_unregister_port(serport->serio);
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 80f6386709bf..a854c6e5f09e 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -658,6 +658,18 @@ config TOUCHSCREEN_PIXCIR
 	  To compile this driver as a module, choose M here: the
 	  module will be called pixcir_i2c_ts.
 
+config TOUCHSCREEN_WDT87XX_I2C
+	tristate "Weida HiTech I2C touchscreen"
+	depends on I2C
+	help
+	  Say Y here if you have a Weida WDT87XX 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 wdt87xx_i2c.
+
 config TOUCHSCREEN_WM831X
 	tristate "Support for WM831x touchscreen controllers"
 	depends on MFD_WM831X
@@ -958,6 +970,7 @@ config TOUCHSCREEN_ST1232
 config TOUCHSCREEN_STMPE
 	tristate "STMicroelectronics STMPE touchscreens"
 	depends on MFD_STMPE
+	depends on (OF || COMPILE_TEST)
 	help
 	  Say Y here if you want support for STMicroelectronics
 	  STMPE touchscreen controllers.
@@ -979,8 +992,7 @@ config TOUCHSCREEN_SUN4I
 
 config TOUCHSCREEN_SUR40
 	tristate "Samsung SUR40 (Surface 2.0/PixelSense) touchscreen"
-	depends on USB
-	depends on MEDIA_USB_SUPPORT
+	depends on USB && MEDIA_USB_SUPPORT && HAS_DMA
 	select INPUT_POLLDEV
 	select VIDEOBUF2_DMA_SG
 	help
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 44deea743d02..fa3d33bac7fc 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -72,6 +72,7 @@ obj-$(CONFIG_TOUCHSCREEN_TSC2007)	+= tsc2007.o
 obj-$(CONFIG_TOUCHSCREEN_UCB1400)	+= ucb1400_ts.o
 obj-$(CONFIG_TOUCHSCREEN_WACOM_W8001)	+= wacom_w8001.o
 obj-$(CONFIG_TOUCHSCREEN_WACOM_I2C)	+= wacom_i2c.o
+obj-$(CONFIG_TOUCHSCREEN_WDT87XX_I2C)	+= wdt87xx_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_WM831X)	+= wm831x-ts.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX)	+= wm97xx-ts.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9705)	+= wm9705.o
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index 40b98dda8f38..dfc7309e3d38 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -726,15 +726,15 @@ static void mxt_input_button(struct mxt_data *data, u8 *message)
 {
 	struct input_dev *input = data->input_dev;
 	const struct mxt_platform_data *pdata = data->pdata;
-	bool button;
 	int i;
 
-	/* Active-low switch */
 	for (i = 0; i < pdata->t19_num_keys; i++) {
 		if (pdata->t19_keymap[i] == KEY_RESERVED)
 			continue;
-		button = !(message[1] & (1 << i));
-		input_report_key(input, pdata->t19_keymap[i], button);
+
+		/* Active-low switch */
+		input_report_key(input, pdata->t19_keymap[i],
+				 !(message[1] & BIT(i)));
 	}
 }
 
diff --git a/drivers/input/touchscreen/cyttsp4_core.c b/drivers/input/touchscreen/cyttsp4_core.c
index 568a3d340c8a..5ed31057430c 100644
--- a/drivers/input/touchscreen/cyttsp4_core.c
+++ b/drivers/input/touchscreen/cyttsp4_core.c
@@ -775,7 +775,6 @@ static void cyttsp4_get_touch(struct cyttsp4_mt_data *md,
 	struct device *dev = &md->input->dev;
 	struct cyttsp4_sysinfo *si = md->si;
 	enum cyttsp4_tch_abs abs;
-	int tmp;
 	bool flipped;
 
 	for (abs = CY_TCH_X; abs < CY_TCH_NUM_ABS; abs++) {
@@ -790,9 +789,7 @@ static void cyttsp4_get_touch(struct cyttsp4_mt_data *md,
 	}
 
 	if (md->pdata->flags & CY_FLAG_FLIP) {
-		tmp = touch->abs[CY_TCH_X];
-		touch->abs[CY_TCH_X] = touch->abs[CY_TCH_Y];
-		touch->abs[CY_TCH_Y] = tmp;
+		swap(touch->abs[CY_TCH_X], touch->abs[CY_TCH_Y]);
 		flipped = true;
 	} else
 		flipped = false;
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index e6aef3e48bd9..394b1de9a2a3 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -1035,20 +1035,15 @@ static int edt_ft5x06_ts_probe(struct i2c_client *client,
 	input->id.bustype = BUS_I2C;
 	input->dev.parent = &client->dev;
 
-	__set_bit(EV_KEY, input->evbit);
-	__set_bit(EV_ABS, input->evbit);
-	__set_bit(BTN_TOUCH, input->keybit);
-	input_set_abs_params(input, ABS_X, 0, tsdata->num_x * 64 - 1, 0, 0);
-	input_set_abs_params(input, ABS_Y, 0, tsdata->num_y * 64 - 1, 0, 0);
 	input_set_abs_params(input, ABS_MT_POSITION_X,
 			     0, tsdata->num_x * 64 - 1, 0, 0);
 	input_set_abs_params(input, ABS_MT_POSITION_Y,
 			     0, tsdata->num_y * 64 - 1, 0, 0);
 
 	if (!pdata)
-		touchscreen_parse_of_params(input);
+		touchscreen_parse_of_params(input, true);
 
-	error = input_mt_init_slots(input, MAX_SUPPORT_POINTS, 0);
+	error = input_mt_init_slots(input, MAX_SUPPORT_POINTS, INPUT_MT_DIRECT);
 	if (error) {
 		dev_err(&client->dev, "Unable to init MT slots.\n");
 		return error;
diff --git a/drivers/input/touchscreen/goodix.c b/drivers/input/touchscreen/goodix.c
index 3af16984d57c..b4d12e29abff 100644
--- a/drivers/input/touchscreen/goodix.c
+++ b/drivers/input/touchscreen/goodix.c
@@ -47,7 +47,7 @@ struct goodix_ts_data {
 /* Register defines */
 #define GOODIX_READ_COOR_ADDR		0x814E
 #define GOODIX_REG_CONFIG_DATA		0x8047
-#define GOODIX_REG_VERSION		0x8140
+#define GOODIX_REG_ID			0x8140
 
 #define RESOLUTION_LOC		1
 #define MAX_CONTACTS_LOC	5
@@ -69,7 +69,7 @@ static const unsigned long goodix_irq_flags[] = {
  * @len: length of the buffer to write
  */
 static int goodix_i2c_read(struct i2c_client *client,
-				u16 reg, u8 *buf, int len)
+			   u16 reg, u8 *buf, int len)
 {
 	struct i2c_msg msgs[2];
 	u16 wbuf = cpu_to_be16(reg);
@@ -78,7 +78,7 @@ static int goodix_i2c_read(struct i2c_client *client,
 	msgs[0].flags = 0;
 	msgs[0].addr  = client->addr;
 	msgs[0].len   = 2;
-	msgs[0].buf   = (u8 *) &wbuf;
+	msgs[0].buf   = (u8 *)&wbuf;
 
 	msgs[1].flags = I2C_M_RD;
 	msgs[1].addr  = client->addr;
@@ -101,6 +101,9 @@ static int goodix_ts_read_input_report(struct goodix_ts_data *ts, u8 *data)
 		return error;
 	}
 
+	if (!(data[0] & 0x80))
+		return -EAGAIN;
+
 	touch_num = data[0] & 0x0f;
 	if (touch_num > ts->max_touch_num)
 		return -EPROTO;
@@ -144,7 +147,7 @@ static void goodix_ts_report_touch(struct goodix_ts_data *ts, u8 *coor_data)
  */
 static void goodix_process_events(struct goodix_ts_data *ts)
 {
-	u8  point_data[1 + GOODIX_CONTACT_SIZE * ts->max_touch_num];
+	u8  point_data[1 + GOODIX_CONTACT_SIZE * GOODIX_MAX_CONTACTS];
 	int touch_num;
 	int i;
 
@@ -196,8 +199,8 @@ static void goodix_read_config(struct goodix_ts_data *ts)
 	int error;
 
 	error = goodix_i2c_read(ts->client, GOODIX_REG_CONFIG_DATA,
-			      config,
-			   GOODIX_CONFIG_MAX_LENGTH);
+				config,
+				GOODIX_CONFIG_MAX_LENGTH);
 	if (error) {
 		dev_warn(&ts->client->dev,
 			 "Error reading config (%d), using defaults\n",
@@ -227,22 +230,28 @@ static void goodix_read_config(struct goodix_ts_data *ts)
  *
  * @client: the i2c client
  * @version: output buffer containing the version on success
+ * @id: output buffer containing the id on success
  */
-static int goodix_read_version(struct i2c_client *client, u16 *version)
+static int goodix_read_version(struct i2c_client *client, u16 *version, u16 *id)
 {
 	int error;
 	u8 buf[6];
+	char id_str[5];
 
-	error = goodix_i2c_read(client, GOODIX_REG_VERSION, buf, sizeof(buf));
+	error = goodix_i2c_read(client, GOODIX_REG_ID, 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]);
+	memcpy(id_str, buf, 4);
+	id_str[4] = 0;
+	if (kstrtou16(id_str, 10, id))
+		*id = 0x1001;
+
+	*version = get_unaligned_le16(&buf[4]);
 
-	dev_info(&client->dev, "IC VERSION: %6ph\n", buf);
+	dev_info(&client->dev, "ID %d, version: %04x\n", *id, *version);
 
 	return 0;
 }
@@ -276,10 +285,13 @@ static int goodix_i2c_test(struct i2c_client *client)
  * goodix_request_input_dev - Allocate, populate and register the input device
  *
  * @ts: our goodix_ts_data pointer
+ * @version: device firmware version
+ * @id: device ID
  *
  * Must be called during probe
  */
-static int goodix_request_input_dev(struct goodix_ts_data *ts)
+static int goodix_request_input_dev(struct goodix_ts_data *ts, u16 version,
+				    u16 id)
 {
 	int error;
 
@@ -289,14 +301,10 @@ static int goodix_request_input_dev(struct goodix_ts_data *ts)
 		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_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);
 
@@ -307,8 +315,8 @@ static int goodix_request_input_dev(struct goodix_ts_data *ts)
 	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;
+	ts->input_dev->id.product = id;
+	ts->input_dev->id.version = version;
 
 	error = input_register_device(ts->input_dev);
 	if (error) {
@@ -326,7 +334,7 @@ static int goodix_ts_probe(struct i2c_client *client,
 	struct goodix_ts_data *ts;
 	unsigned long irq_flags;
 	int error;
-	u16 version_info;
+	u16 version_info, id_info;
 
 	dev_dbg(&client->dev, "I2C Address: 0x%02x\n", client->addr);
 
@@ -348,7 +356,7 @@ static int goodix_ts_probe(struct i2c_client *client,
 		return error;
 	}
 
-	error = goodix_read_version(client, &version_info);
+	error = goodix_read_version(client, &version_info, &id_info);
 	if (error) {
 		dev_err(&client->dev, "Read version failed.\n");
 		return error;
@@ -356,7 +364,7 @@ static int goodix_ts_probe(struct i2c_client *client,
 
 	goodix_read_config(ts);
 
-	error = goodix_request_input_dev(ts);
+	error = goodix_request_input_dev(ts, version_info, id_info);
 	if (error)
 		return error;
 
diff --git a/drivers/input/touchscreen/of_touchscreen.c b/drivers/input/touchscreen/of_touchscreen.c
index b82b5207c78b..806cd0ad160f 100644
--- a/drivers/input/touchscreen/of_touchscreen.c
+++ b/drivers/input/touchscreen/of_touchscreen.c
@@ -14,14 +14,22 @@
 #include <linux/input/mt.h>
 #include <linux/input/touchscreen.h>
 
-static u32 of_get_optional_u32(struct device_node *np,
-			       const char *property)
+static bool touchscreen_get_prop_u32(struct device_node *np,
+				     const char *property,
+				     unsigned int default_value,
+				     unsigned int *value)
 {
-	u32 val = 0;
+	u32 val;
+	int error;
 
-	of_property_read_u32(np, property, &val);
+	error = of_property_read_u32(np, property, &val);
+	if (error) {
+		*value = default_value;
+		return false;
+	}
 
-	return val;
+	*value = val;
+	return true;
 }
 
 static void touchscreen_set_params(struct input_dev *dev,
@@ -54,34 +62,45 @@ static void touchscreen_set_params(struct input_dev *dev,
  * input device accordingly. The function keeps previously setuped default
  * values if no value is specified via DT.
  */
-void touchscreen_parse_of_params(struct input_dev *dev)
+void touchscreen_parse_of_params(struct input_dev *dev, bool multitouch)
 {
 	struct device_node *np = dev->dev.parent->of_node;
-	u32 maximum, fuzz;
+	unsigned int axis;
+	unsigned int maximum, fuzz;
+	bool data_present;
 
 	input_alloc_absinfo(dev);
 	if (!dev->absinfo)
 		return;
 
-	maximum = of_get_optional_u32(np, "touchscreen-size-x");
-	fuzz = of_get_optional_u32(np, "touchscreen-fuzz-x");
-	if (maximum || fuzz) {
-		touchscreen_set_params(dev, ABS_X, maximum, fuzz);
-		touchscreen_set_params(dev, ABS_MT_POSITION_X, maximum, fuzz);
-	}
+	axis = multitouch ? ABS_MT_POSITION_X : ABS_X;
+	data_present = touchscreen_get_prop_u32(np, "touchscreen-size-x",
+						input_abs_get_max(dev, axis),
+						&maximum) |
+		       touchscreen_get_prop_u32(np, "touchscreen-fuzz-x",
+						input_abs_get_fuzz(dev, axis),
+						&fuzz);
+	if (data_present)
+		touchscreen_set_params(dev, axis, maximum, fuzz);
 
-	maximum = of_get_optional_u32(np, "touchscreen-size-y");
-	fuzz = of_get_optional_u32(np, "touchscreen-fuzz-y");
-	if (maximum || fuzz) {
-		touchscreen_set_params(dev, ABS_Y, maximum, fuzz);
-		touchscreen_set_params(dev, ABS_MT_POSITION_Y, maximum, fuzz);
-	}
+	axis = multitouch ? ABS_MT_POSITION_Y : ABS_Y;
+	data_present = touchscreen_get_prop_u32(np, "touchscreen-size-y",
+						input_abs_get_max(dev, axis),
+						&maximum) |
+		       touchscreen_get_prop_u32(np, "touchscreen-fuzz-y",
+						input_abs_get_fuzz(dev, axis),
+						&fuzz);
+	if (data_present)
+		touchscreen_set_params(dev, axis, maximum, fuzz);
 
-	maximum = of_get_optional_u32(np, "touchscreen-max-pressure");
-	fuzz = of_get_optional_u32(np, "touchscreen-fuzz-pressure");
-	if (maximum || fuzz) {
-		touchscreen_set_params(dev, ABS_PRESSURE, maximum, fuzz);
-		touchscreen_set_params(dev, ABS_MT_PRESSURE, maximum, fuzz);
-	}
+	axis = multitouch ? ABS_MT_PRESSURE : ABS_PRESSURE;
+	data_present = touchscreen_get_prop_u32(np, "touchscreen-max-pressure",
+						input_abs_get_max(dev, axis),
+						&maximum) |
+		       touchscreen_get_prop_u32(np, "touchscreen-fuzz-pressure",
+						input_abs_get_fuzz(dev, axis),
+						&fuzz);
+	if (data_present)
+		touchscreen_set_params(dev, axis, maximum, fuzz);
 }
 EXPORT_SYMBOL(touchscreen_parse_of_params);
diff --git a/drivers/input/touchscreen/pixcir_i2c_ts.c b/drivers/input/touchscreen/pixcir_i2c_ts.c
index 2c2107147319..8f3e243a62bf 100644
--- a/drivers/input/touchscreen/pixcir_i2c_ts.c
+++ b/drivers/input/touchscreen/pixcir_i2c_ts.c
@@ -78,7 +78,7 @@ static void pixcir_ts_parse(struct pixcir_i2c_ts_data *tsdata,
 	}
 
 	ret = i2c_master_recv(tsdata->client, rdbuf, readsize);
-	if (ret != sizeof(rdbuf)) {
+	if (ret != readsize) {
 		dev_err(&tsdata->client->dev,
 			"%s: i2c_master_recv failed(), ret=%d\n",
 			__func__, ret);
diff --git a/drivers/input/touchscreen/s3c2410_ts.c b/drivers/input/touchscreen/s3c2410_ts.c
index bdfa27dc097b..a4a103e1d11b 100644
--- a/drivers/input/touchscreen/s3c2410_ts.c
+++ b/drivers/input/touchscreen/s3c2410_ts.c
@@ -411,7 +411,7 @@ static const struct dev_pm_ops s3c_ts_pmops = {
 };
 #endif
 
-static struct platform_device_id s3cts_driver_ids[] = {
+static const struct platform_device_id s3cts_driver_ids[] = {
 	{ "s3c2410-ts", 0 },
 	{ "s3c2440-ts", 0 },
 	{ "s3c64xx-ts", FEAT_PEN_IRQ },
diff --git a/drivers/input/touchscreen/stmpe-ts.c b/drivers/input/touchscreen/stmpe-ts.c
index e4c31256a74d..e414d43e5159 100644
--- a/drivers/input/touchscreen/stmpe-ts.c
+++ b/drivers/input/touchscreen/stmpe-ts.c
@@ -267,27 +267,10 @@ static void stmpe_ts_close(struct input_dev *dev)
 static void stmpe_ts_get_platform_info(struct platform_device *pdev,
 					struct stmpe_touch *ts)
 {
-	struct stmpe *stmpe = dev_get_drvdata(pdev->dev.parent);
 	struct device_node *np = pdev->dev.of_node;
-	struct stmpe_ts_platform_data *ts_pdata = NULL;
-
-	ts->stmpe = stmpe;
-
-	if (stmpe->pdata && stmpe->pdata->ts) {
-		ts_pdata = stmpe->pdata->ts;
-
-		ts->sample_time = ts_pdata->sample_time;
-		ts->mod_12b = ts_pdata->mod_12b;
-		ts->ref_sel = ts_pdata->ref_sel;
-		ts->adc_freq = ts_pdata->adc_freq;
-		ts->ave_ctrl = ts_pdata->ave_ctrl;
-		ts->touch_det_delay = ts_pdata->touch_det_delay;
-		ts->settling = ts_pdata->settling;
-		ts->fraction_z = ts_pdata->fraction_z;
-		ts->i_drive = ts_pdata->i_drive;
-	} else if (np) {
-		u32 val;
+	u32 val;
 
+	if (np) {
 		if (!of_property_read_u32(np, "st,sample-time", &val))
 			ts->sample_time = val;
 		if (!of_property_read_u32(np, "st,mod-12b", &val))
@@ -311,6 +294,7 @@ static void stmpe_ts_get_platform_info(struct platform_device *pdev,
 
 static int stmpe_input_probe(struct platform_device *pdev)
 {
+	struct stmpe *stmpe = dev_get_drvdata(pdev->dev.parent);
 	struct stmpe_touch *ts;
 	struct input_dev *idev;
 	int error;
@@ -329,6 +313,7 @@ static int stmpe_input_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	platform_set_drvdata(pdev, ts);
+	ts->stmpe = stmpe;
 	ts->idev = idev;
 	ts->dev = &pdev->dev;
 
@@ -351,14 +336,13 @@ static int stmpe_input_probe(struct platform_device *pdev)
 	idev->name = STMPE_TS_NAME;
 	idev->phys = STMPE_TS_NAME"/input0";
 	idev->id.bustype = BUS_I2C;
-	idev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
-	idev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
 
 	idev->open = stmpe_ts_open;
 	idev->close = stmpe_ts_close;
 
 	input_set_drvdata(idev, ts);
 
+	input_set_capability(idev, EV_KEY, BTN_TOUCH);
 	input_set_abs_params(idev, ABS_X, 0, XY_MASK, 0, 0);
 	input_set_abs_params(idev, ABS_Y, 0, XY_MASK, 0, 0);
 	input_set_abs_params(idev, ABS_PRESSURE, 0x0, 0xff, 0, 0);
@@ -383,14 +367,19 @@ static int stmpe_ts_remove(struct platform_device *pdev)
 
 static struct platform_driver stmpe_ts_driver = {
 	.driver = {
-		   .name = STMPE_TS_NAME,
-		   },
+		.name = STMPE_TS_NAME,
+	},
 	.probe = stmpe_input_probe,
 	.remove = stmpe_ts_remove,
 };
 module_platform_driver(stmpe_ts_driver);
 
+static const struct of_device_id stmpe_ts_ids[] = {
+	{ .compatible = "st,stmpe-ts", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, stmpe_ts_ids);
+
 MODULE_AUTHOR("Luotao Fu <l.fu@pengutronix.de>");
 MODULE_DESCRIPTION("STMPEXXX touchscreen driver");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:" STMPE_TS_NAME);
diff --git a/drivers/input/touchscreen/sur40.c b/drivers/input/touchscreen/sur40.c
index a24eba5ea843..8be7b9b79f20 100644
--- a/drivers/input/touchscreen/sur40.c
+++ b/drivers/input/touchscreen/sur40.c
@@ -125,7 +125,7 @@ struct sur40_image_header {
 #define VIDEO_PACKET_SIZE  16384
 
 /* polling interval (ms) */
-#define POLL_INTERVAL 10
+#define POLL_INTERVAL 4
 
 /* maximum number of contacts FIXME: this is a guess? */
 #define MAX_CONTACTS 64
@@ -342,7 +342,7 @@ static void sur40_poll(struct input_polled_dev *polldev)
 		 * instead of at the end.
 		 */
 		if (packet_id != header->packet_id)
-			dev_warn(sur40->dev, "packet ID mismatch\n");
+			dev_dbg(sur40->dev, "packet ID mismatch\n");
 
 		packet_blobs = result / sizeof(struct sur40_blob);
 		dev_dbg(sur40->dev, "received %d blobs\n", packet_blobs);
@@ -389,6 +389,8 @@ static void sur40_process_video(struct sur40_state *sur40)
 	list_del(&new_buf->list);
 	spin_unlock(&sur40->qlock);
 
+	dev_dbg(sur40->dev, "buffer acquired\n");
+
 	/* retrieve data via bulk read */
 	result = usb_bulk_msg(sur40->usbdev,
 			usb_rcvbulkpipe(sur40->usbdev, VIDEO_ENDPOINT),
@@ -416,6 +418,8 @@ static void sur40_process_video(struct sur40_state *sur40)
 		goto err_poll;
 	}
 
+	dev_dbg(sur40->dev, "header acquired\n");
+
 	sgt = vb2_dma_sg_plane_desc(&new_buf->vb, 0);
 
 	result = usb_sg_init(&sgr, sur40->usbdev,
@@ -432,11 +436,18 @@ static void sur40_process_video(struct sur40_state *sur40)
 		goto err_poll;
 	}
 
+	dev_dbg(sur40->dev, "image acquired\n");
+
+	/* return error if streaming was stopped in the meantime */
+	if (sur40->sequence == -1)
+		goto err_poll;
+
 	/* mark as finished */
 	v4l2_get_timestamp(&new_buf->vb.v4l2_buf.timestamp);
 	new_buf->vb.v4l2_buf.sequence = sur40->sequence++;
 	new_buf->vb.v4l2_buf.field = V4L2_FIELD_NONE;
 	vb2_buffer_done(&new_buf->vb, VB2_BUF_STATE_DONE);
+	dev_dbg(sur40->dev, "buffer marked done\n");
 	return;
 
 err_poll:
@@ -716,6 +727,7 @@ static int sur40_start_streaming(struct vb2_queue *vq, unsigned int count)
 static void sur40_stop_streaming(struct vb2_queue *vq)
 {
 	struct sur40_state *sur40 = vb2_get_drv_priv(vq);
+	sur40->sequence = -1;
 
 	/* Release all active buffers */
 	return_all_buffers(sur40, VB2_BUF_STATE_ERROR);
@@ -778,6 +790,33 @@ static int sur40_vidioc_enum_fmt(struct file *file, void *priv,
 	return 0;
 }
 
+static int sur40_vidioc_enum_framesizes(struct file *file, void *priv,
+					struct v4l2_frmsizeenum *f)
+{
+	if ((f->index != 0) || (f->pixel_format != V4L2_PIX_FMT_GREY))
+		return -EINVAL;
+
+	f->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	f->discrete.width  = sur40_video_format.width;
+	f->discrete.height = sur40_video_format.height;
+	return 0;
+}
+
+static int sur40_vidioc_enum_frameintervals(struct file *file, void *priv,
+					    struct v4l2_frmivalenum *f)
+{
+	if ((f->index > 1) || (f->pixel_format != V4L2_PIX_FMT_GREY)
+		|| (f->width  != sur40_video_format.width)
+		|| (f->height != sur40_video_format.height))
+			return -EINVAL;
+
+	f->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	f->discrete.denominator  = 60/(f->index+1);
+	f->discrete.numerator = 1;
+	return 0;
+}
+
+
 static const struct usb_device_id sur40_table[] = {
 	{ USB_DEVICE(ID_MICROSOFT, ID_SUR40) },  /* Samsung SUR40 */
 	{ }                                      /* terminating null entry */
@@ -829,6 +868,9 @@ static const struct v4l2_ioctl_ops sur40_video_ioctl_ops = {
 	.vidioc_s_fmt_vid_cap	= sur40_vidioc_fmt,
 	.vidioc_g_fmt_vid_cap	= sur40_vidioc_fmt,
 
+	.vidioc_enum_framesizes = sur40_vidioc_enum_framesizes,
+	.vidioc_enum_frameintervals = sur40_vidioc_enum_frameintervals,
+
 	.vidioc_enum_input	= sur40_vidioc_enum_input,
 	.vidioc_g_input		= sur40_vidioc_g_input,
 	.vidioc_s_input		= sur40_vidioc_s_input,
diff --git a/drivers/input/touchscreen/tsc2005.c b/drivers/input/touchscreen/tsc2005.c
index 72657c579430..d8c025b0f88c 100644
--- a/drivers/input/touchscreen/tsc2005.c
+++ b/drivers/input/touchscreen/tsc2005.c
@@ -709,7 +709,7 @@ static int tsc2005_probe(struct spi_device *spi)
 	input_set_abs_params(input_dev, ABS_PRESSURE, 0, max_p, fudge_p, 0);
 
 	if (np)
-		touchscreen_parse_of_params(input_dev);
+		touchscreen_parse_of_params(input_dev, false);
 
 	input_dev->open = tsc2005_open;
 	input_dev->close = tsc2005_close;
diff --git a/drivers/input/touchscreen/wdt87xx_i2c.c b/drivers/input/touchscreen/wdt87xx_i2c.c
new file mode 100644
index 000000000000..fb92ae1c5fae
--- /dev/null
+++ b/drivers/input/touchscreen/wdt87xx_i2c.c
@@ -0,0 +1,1149 @@
+/*
+ * Weida HiTech WDT87xx TouchScreen I2C driver
+ *
+ * Copyright (c) 2015  Weida Hi-Tech Co., Ltd.
+ * HN Chen <hn.chen@weidahitech.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ */
+
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/firmware.h>
+#include <linux/input/mt.h>
+#include <linux/acpi.h>
+#include <asm/unaligned.h>
+
+#define WDT87XX_NAME		"wdt87xx_i2c"
+#define WDT87XX_DRV_VER		"0.9.6"
+#define WDT87XX_FW_NAME		"wdt87xx_fw.bin"
+#define WDT87XX_CFG_NAME	"wdt87xx_cfg.bin"
+
+#define MODE_ACTIVE			0x01
+#define MODE_READY			0x02
+#define MODE_IDLE			0x03
+#define MODE_SLEEP			0x04
+#define MODE_STOP			0xFF
+
+#define WDT_MAX_FINGER			10
+#define WDT_RAW_BUF_COUNT		54
+#define WDT_V1_RAW_BUF_COUNT		74
+#define WDT_FIRMWARE_ID			0xa9e368f5
+
+#define PG_SIZE				0x1000
+#define MAX_RETRIES			3
+
+#define MAX_UNIT_AXIS			0x7FFF
+
+#define PKT_READ_SIZE			72
+#define PKT_WRITE_SIZE			80
+
+/* the finger definition of the report event */
+#define FINGER_EV_OFFSET_ID		0
+#define FINGER_EV_OFFSET_X		1
+#define FINGER_EV_OFFSET_Y		3
+#define FINGER_EV_SIZE			5
+
+#define FINGER_EV_V1_OFFSET_ID		0
+#define FINGER_EV_V1_OFFSET_W		1
+#define FINGER_EV_V1_OFFSET_P		2
+#define FINGER_EV_V1_OFFSET_X		3
+#define FINGER_EV_V1_OFFSET_Y		5
+#define FINGER_EV_V1_SIZE		7
+
+/* The definition of a report packet */
+#define TOUCH_PK_OFFSET_REPORT_ID	0
+#define TOUCH_PK_OFFSET_EVENT		1
+#define TOUCH_PK_OFFSET_SCAN_TIME	51
+#define TOUCH_PK_OFFSET_FNGR_NUM	53
+
+#define TOUCH_PK_V1_OFFSET_REPORT_ID	0
+#define TOUCH_PK_V1_OFFSET_EVENT	1
+#define TOUCH_PK_V1_OFFSET_SCAN_TIME	71
+#define TOUCH_PK_V1_OFFSET_FNGR_NUM	73
+
+/* The definition of the controller parameters */
+#define CTL_PARAM_OFFSET_FW_ID		0
+#define CTL_PARAM_OFFSET_PLAT_ID	2
+#define CTL_PARAM_OFFSET_XMLS_ID1	4
+#define CTL_PARAM_OFFSET_XMLS_ID2	6
+#define CTL_PARAM_OFFSET_PHY_CH_X	8
+#define CTL_PARAM_OFFSET_PHY_CH_Y	10
+#define CTL_PARAM_OFFSET_PHY_X0		12
+#define CTL_PARAM_OFFSET_PHY_X1		14
+#define CTL_PARAM_OFFSET_PHY_Y0		16
+#define CTL_PARAM_OFFSET_PHY_Y1		18
+#define CTL_PARAM_OFFSET_PHY_W		22
+#define CTL_PARAM_OFFSET_PHY_H		24
+#define CTL_PARAM_OFFSET_FACTOR		32
+
+/* Communication commands */
+#define PACKET_SIZE			56
+#define VND_REQ_READ			0x06
+#define VND_READ_DATA			0x07
+#define VND_REQ_WRITE			0x08
+
+#define VND_CMD_START			0x00
+#define VND_CMD_STOP			0x01
+#define VND_CMD_RESET			0x09
+
+#define VND_CMD_ERASE			0x1A
+
+#define VND_GET_CHECKSUM		0x66
+
+#define VND_SET_DATA			0x83
+#define VND_SET_COMMAND_DATA		0x84
+#define VND_SET_CHECKSUM_CALC		0x86
+#define VND_SET_CHECKSUM_LENGTH		0x87
+
+#define VND_CMD_SFLCK			0xFC
+#define VND_CMD_SFUNL			0xFD
+
+#define CMD_SFLCK_KEY			0xC39B
+#define CMD_SFUNL_KEY			0x95DA
+
+#define STRIDX_PLATFORM_ID		0x80
+#define STRIDX_PARAMETERS		0x81
+
+#define CMD_BUF_SIZE			8
+#define PKT_BUF_SIZE			64
+
+/* The definition of the command packet */
+#define CMD_REPORT_ID_OFFSET		0x0
+#define CMD_TYPE_OFFSET			0x1
+#define CMD_INDEX_OFFSET		0x2
+#define CMD_KEY_OFFSET			0x3
+#define CMD_LENGTH_OFFSET		0x4
+#define CMD_DATA_OFFSET			0x8
+
+/* The definition of firmware chunk tags */
+#define FOURCC_ID_RIFF			0x46464952
+#define FOURCC_ID_WHIF			0x46494857
+#define FOURCC_ID_FRMT			0x544D5246
+#define FOURCC_ID_FRWR			0x52575246
+#define FOURCC_ID_CNFG			0x47464E43
+
+#define CHUNK_ID_FRMT			FOURCC_ID_FRMT
+#define CHUNK_ID_FRWR			FOURCC_ID_FRWR
+#define CHUNK_ID_CNFG			FOURCC_ID_CNFG
+
+#define FW_FOURCC1_OFFSET		0
+#define FW_SIZE_OFFSET			4
+#define FW_FOURCC2_OFFSET		8
+#define FW_PAYLOAD_OFFSET		40
+
+#define FW_CHUNK_ID_OFFSET		0
+#define FW_CHUNK_SIZE_OFFSET		4
+#define FW_CHUNK_TGT_START_OFFSET	8
+#define FW_CHUNK_PAYLOAD_LEN_OFFSET	12
+#define FW_CHUNK_SRC_START_OFFSET	16
+#define FW_CHUNK_VERSION_OFFSET		20
+#define FW_CHUNK_ATTR_OFFSET		24
+#define FW_CHUNK_PAYLOAD_OFFSET		32
+
+/* Controller requires minimum 300us between commands */
+#define WDT_COMMAND_DELAY_MS		2
+#define WDT_FLASH_WRITE_DELAY_MS	4
+
+struct wdt87xx_sys_param {
+	u16	fw_id;
+	u16	plat_id;
+	u16	xmls_id1;
+	u16	xmls_id2;
+	u16	phy_ch_x;
+	u16	phy_ch_y;
+	u16	phy_w;
+	u16	phy_h;
+	u16	scaling_factor;
+	u32	max_x;
+	u32	max_y;
+};
+
+struct wdt87xx_data {
+	struct i2c_client		*client;
+	struct input_dev		*input;
+	/* Mutex for fw update to prevent concurrent access */
+	struct mutex			fw_mutex;
+	struct wdt87xx_sys_param	param;
+	u8				phys[32];
+};
+
+static int wdt87xx_i2c_xfer(struct i2c_client *client,
+			    void *txdata, size_t txlen,
+			    void *rxdata, size_t rxlen)
+{
+	struct i2c_msg msgs[] = {
+		{
+			.addr	= client->addr,
+			.flags	= 0,
+			.len	= txlen,
+			.buf	= txdata,
+		},
+		{
+			.addr	= client->addr,
+			.flags	= I2C_M_RD,
+			.len	= rxlen,
+			.buf	= rxdata,
+		},
+	};
+	int error;
+	int ret;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret != ARRAY_SIZE(msgs)) {
+		error = ret < 0 ? ret : -EIO;
+		dev_err(&client->dev, "%s: i2c transfer failed: %d\n",
+			__func__, error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int wdt87xx_get_string(struct i2c_client *client, u8 str_idx,
+			      u8 *buf, size_t len)
+{
+	u8 tx_buf[] = { 0x22, 0x00, 0x13, 0x0E, str_idx, 0x23, 0x00 };
+	u8 rx_buf[PKT_WRITE_SIZE];
+	size_t rx_len = len + 2;
+	int error;
+
+	if (rx_len > sizeof(rx_buf))
+		return -EINVAL;
+
+	error = wdt87xx_i2c_xfer(client, tx_buf, sizeof(tx_buf),
+				 rx_buf, rx_len);
+	if (error) {
+		dev_err(&client->dev, "get string failed: %d\n", error);
+		return error;
+	}
+
+	if (rx_buf[1] != 0x03) {
+		dev_err(&client->dev, "unexpected response to get string: %d\n",
+			rx_buf[1]);
+		return -EINVAL;
+	}
+
+	rx_len = min_t(size_t, len, rx_buf[0]);
+	memcpy(buf, &rx_buf[2], rx_len);
+
+	mdelay(WDT_COMMAND_DELAY_MS);
+
+	return 0;
+}
+
+static int wdt87xx_get_feature(struct i2c_client *client,
+			       u8 *buf, size_t buf_size)
+{
+	u8 tx_buf[8];
+	u8 rx_buf[PKT_WRITE_SIZE];
+	size_t tx_len = 0;
+	size_t rx_len = buf_size + 2;
+	int error;
+
+	if (rx_len > sizeof(rx_buf))
+		return -EINVAL;
+
+	/* Get feature command packet */
+	tx_buf[tx_len++] = 0x22;
+	tx_buf[tx_len++] = 0x00;
+	if (buf[CMD_REPORT_ID_OFFSET] > 0xF) {
+		tx_buf[tx_len++] = 0x30;
+		tx_buf[tx_len++] = 0x02;
+		tx_buf[tx_len++] = buf[CMD_REPORT_ID_OFFSET];
+	} else {
+		tx_buf[tx_len++] = 0x30 | buf[CMD_REPORT_ID_OFFSET];
+		tx_buf[tx_len++] = 0x02;
+	}
+	tx_buf[tx_len++] = 0x23;
+	tx_buf[tx_len++] = 0x00;
+
+	error = wdt87xx_i2c_xfer(client, tx_buf, tx_len, rx_buf, rx_len);
+	if (error) {
+		dev_err(&client->dev, "get feature failed: %d\n", error);
+		return error;
+	}
+
+	rx_len = min_t(size_t, buf_size, get_unaligned_le16(rx_buf));
+	memcpy(buf, &rx_buf[2], rx_len);
+
+	mdelay(WDT_COMMAND_DELAY_MS);
+
+	return 0;
+}
+
+static int wdt87xx_set_feature(struct i2c_client *client,
+			       const u8 *buf, size_t buf_size)
+{
+	u8 tx_buf[PKT_WRITE_SIZE];
+	int tx_len = 0;
+	int error;
+
+	/* Set feature command packet */
+	tx_buf[tx_len++] = 0x22;
+	tx_buf[tx_len++] = 0x00;
+	if (buf[CMD_REPORT_ID_OFFSET] > 0xF) {
+		tx_buf[tx_len++] = 0x30;
+		tx_buf[tx_len++] = 0x03;
+		tx_buf[tx_len++] = buf[CMD_REPORT_ID_OFFSET];
+	} else {
+		tx_buf[tx_len++] = 0x30 | buf[CMD_REPORT_ID_OFFSET];
+		tx_buf[tx_len++] = 0x03;
+	}
+	tx_buf[tx_len++] = 0x23;
+	tx_buf[tx_len++] = 0x00;
+	tx_buf[tx_len++] = (buf_size & 0xFF);
+	tx_buf[tx_len++] = ((buf_size & 0xFF00) >> 8);
+
+	if (tx_len + buf_size > sizeof(tx_buf))
+		return -EINVAL;
+
+	memcpy(&tx_buf[tx_len], buf, buf_size);
+	tx_len += buf_size;
+
+	error = i2c_master_send(client, tx_buf, tx_len);
+	if (error < 0) {
+		dev_err(&client->dev, "set feature failed: %d\n", error);
+		return error;
+	}
+
+	mdelay(WDT_COMMAND_DELAY_MS);
+
+	return 0;
+}
+
+static int wdt87xx_send_command(struct i2c_client *client, int cmd, int value)
+{
+	u8 cmd_buf[CMD_BUF_SIZE];
+
+	/* Set the command packet */
+	cmd_buf[CMD_REPORT_ID_OFFSET] = VND_REQ_WRITE;
+	cmd_buf[CMD_TYPE_OFFSET] = VND_SET_COMMAND_DATA;
+	put_unaligned_le16((u16)cmd, &cmd_buf[CMD_INDEX_OFFSET]);
+
+	switch (cmd) {
+	case VND_CMD_START:
+	case VND_CMD_STOP:
+	case VND_CMD_RESET:
+		/* Mode selector */
+		put_unaligned_le32((value & 0xFF), &cmd_buf[CMD_LENGTH_OFFSET]);
+		break;
+
+	case VND_CMD_SFLCK:
+		put_unaligned_le16(CMD_SFLCK_KEY, &cmd_buf[CMD_KEY_OFFSET]);
+		break;
+
+	case VND_CMD_SFUNL:
+		put_unaligned_le16(CMD_SFUNL_KEY, &cmd_buf[CMD_KEY_OFFSET]);
+		break;
+
+	case VND_CMD_ERASE:
+	case VND_SET_CHECKSUM_CALC:
+	case VND_SET_CHECKSUM_LENGTH:
+		put_unaligned_le32(value, &cmd_buf[CMD_KEY_OFFSET]);
+		break;
+
+	default:
+		cmd_buf[CMD_REPORT_ID_OFFSET] = 0;
+		dev_err(&client->dev, "Invalid command: %d\n", cmd);
+		return -EINVAL;
+	}
+
+	return wdt87xx_set_feature(client, cmd_buf, sizeof(cmd_buf));
+}
+
+static int wdt87xx_sw_reset(struct i2c_client *client)
+{
+	int error;
+
+	dev_dbg(&client->dev, "resetting device now\n");
+
+	error = wdt87xx_send_command(client, VND_CMD_RESET, 0);
+	if (error) {
+		dev_err(&client->dev, "reset failed\n");
+		return error;
+	}
+
+	/* Wait the device to be ready */
+	msleep(200);
+
+	return 0;
+}
+
+static const void *wdt87xx_get_fw_chunk(const struct firmware *fw, u32 id)
+{
+	size_t pos = FW_PAYLOAD_OFFSET;
+	u32 chunk_id, chunk_size;
+
+	while (pos < fw->size) {
+		chunk_id = get_unaligned_le32(fw->data +
+					      pos + FW_CHUNK_ID_OFFSET);
+		if (chunk_id == id)
+			return fw->data + pos;
+
+		chunk_size = get_unaligned_le32(fw->data +
+						pos + FW_CHUNK_SIZE_OFFSET);
+		pos += chunk_size + 2 * sizeof(u32); /* chunk ID + size */
+	}
+
+	return NULL;
+}
+
+static int wdt87xx_get_sysparam(struct i2c_client *client,
+				struct wdt87xx_sys_param *param)
+{
+	u8 buf[PKT_READ_SIZE];
+	int error;
+
+	error = wdt87xx_get_string(client, STRIDX_PARAMETERS, buf, 34);
+	if (error) {
+		dev_err(&client->dev, "failed to get parameters\n");
+		return error;
+	}
+
+	param->xmls_id1 = get_unaligned_le16(buf + CTL_PARAM_OFFSET_XMLS_ID1);
+	param->xmls_id2 = get_unaligned_le16(buf + CTL_PARAM_OFFSET_XMLS_ID2);
+	param->phy_ch_x = get_unaligned_le16(buf + CTL_PARAM_OFFSET_PHY_CH_X);
+	param->phy_ch_y = get_unaligned_le16(buf + CTL_PARAM_OFFSET_PHY_CH_Y);
+	param->phy_w = get_unaligned_le16(buf + CTL_PARAM_OFFSET_PHY_W) / 10;
+	param->phy_h = get_unaligned_le16(buf + CTL_PARAM_OFFSET_PHY_H) / 10;
+
+	/* Get the scaling factor of pixel to logical coordinate */
+	param->scaling_factor =
+			get_unaligned_le16(buf + CTL_PARAM_OFFSET_FACTOR);
+
+	param->max_x = MAX_UNIT_AXIS;
+	param->max_y = DIV_ROUND_CLOSEST(MAX_UNIT_AXIS * param->phy_h,
+					 param->phy_w);
+
+	error = wdt87xx_get_string(client, STRIDX_PLATFORM_ID, buf, 8);
+	if (error) {
+		dev_err(&client->dev, "failed to get platform id\n");
+		return error;
+	}
+
+	param->plat_id = buf[1];
+
+	buf[0] = 0xf2;
+	error = wdt87xx_get_feature(client, buf, 16);
+	if (error) {
+		dev_err(&client->dev, "failed to get firmware id\n");
+		return error;
+	}
+
+	if (buf[0] != 0xf2) {
+		dev_err(&client->dev, "wrong id of fw response: 0x%x\n",
+			buf[0]);
+		return -EINVAL;
+	}
+
+	param->fw_id = get_unaligned_le16(&buf[1]);
+
+	dev_info(&client->dev,
+		 "fw_id: 0x%x, plat_id: 0x%x, xml_id1: %04x, xml_id2: %04x\n",
+		 param->fw_id, param->plat_id,
+		 param->xmls_id1, param->xmls_id2);
+
+	return 0;
+}
+
+static int wdt87xx_validate_firmware(struct wdt87xx_data *wdt,
+				     const struct firmware *fw)
+{
+	const void *fw_chunk;
+	u32 data1, data2;
+	u32 size;
+	u8 fw_chip_id;
+	u8 chip_id;
+
+	data1 = get_unaligned_le32(fw->data + FW_FOURCC1_OFFSET);
+	data2 = get_unaligned_le32(fw->data + FW_FOURCC2_OFFSET);
+	if (data1 != FOURCC_ID_RIFF || data2 != FOURCC_ID_WHIF) {
+		dev_err(&wdt->client->dev, "check fw tag failed\n");
+		return -EINVAL;
+	}
+
+	size = get_unaligned_le32(fw->data + FW_SIZE_OFFSET);
+	if (size != fw->size) {
+		dev_err(&wdt->client->dev,
+			"fw size mismatch: expected %d, actual %zu\n",
+			size, fw->size);
+		return -EINVAL;
+	}
+
+	/*
+	 * Get the chip_id from the firmware. Make sure that it is the
+	 * right controller to do the firmware and config update.
+	 */
+	fw_chunk = wdt87xx_get_fw_chunk(fw, CHUNK_ID_FRWR);
+	if (!fw_chunk) {
+		dev_err(&wdt->client->dev,
+			"unable to locate firmware chunk\n");
+		return -EINVAL;
+	}
+
+	fw_chip_id = (get_unaligned_le32(fw_chunk +
+					 FW_CHUNK_VERSION_OFFSET) >> 12) & 0xF;
+	chip_id = (wdt->param.fw_id >> 12) & 0xF;
+
+	if (fw_chip_id != chip_id) {
+		dev_err(&wdt->client->dev,
+			"fw version mismatch: fw %d vs. chip %d\n",
+			fw_chip_id, chip_id);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int wdt87xx_validate_fw_chunk(const void *data, int id)
+{
+	if (id == CHUNK_ID_FRWR) {
+		u32 fw_id;
+
+		fw_id = get_unaligned_le32(data + FW_CHUNK_PAYLOAD_OFFSET);
+		if (fw_id != WDT_FIRMWARE_ID)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int wdt87xx_write_data(struct i2c_client *client, const char *data,
+			      u32 address, int length)
+{
+	u16 packet_size;
+	int count = 0;
+	int error;
+	u8 pkt_buf[PKT_BUF_SIZE];
+
+	/* Address and length should be 4 bytes aligned */
+	if ((address & 0x3) != 0 || (length & 0x3) != 0) {
+		dev_err(&client->dev,
+			"addr & len must be 4 bytes aligned %x, %x\n",
+			address, length);
+		return -EINVAL;
+	}
+
+	while (length) {
+		packet_size = min(length, PACKET_SIZE);
+
+		pkt_buf[CMD_REPORT_ID_OFFSET] = VND_REQ_WRITE;
+		pkt_buf[CMD_TYPE_OFFSET] = VND_SET_DATA;
+		put_unaligned_le16(packet_size, &pkt_buf[CMD_INDEX_OFFSET]);
+		put_unaligned_le32(address, &pkt_buf[CMD_LENGTH_OFFSET]);
+		memcpy(&pkt_buf[CMD_DATA_OFFSET], data, packet_size);
+
+		error = wdt87xx_set_feature(client, pkt_buf, sizeof(pkt_buf));
+		if (error)
+			return error;
+
+		length -= packet_size;
+		data += packet_size;
+		address += packet_size;
+
+		/* Wait for the controller to finish the write */
+		mdelay(WDT_FLASH_WRITE_DELAY_MS);
+
+		if ((++count % 32) == 0) {
+			/* Delay for fw to clear watch dog */
+			msleep(20);
+		}
+	}
+
+	return 0;
+}
+
+static u16 misr(u16 cur_value, u8 new_value)
+{
+	u32 a, b;
+	u32 bit0;
+	u32 y;
+
+	a = cur_value;
+	b = new_value;
+	bit0 = a ^ (b & 1);
+	bit0 ^= a >> 1;
+	bit0 ^= a >> 2;
+	bit0 ^= a >> 4;
+	bit0 ^= a >> 5;
+	bit0 ^= a >> 7;
+	bit0 ^= a >> 11;
+	bit0 ^= a >> 15;
+	y = (a << 1) ^ b;
+	y = (y & ~1) | (bit0 & 1);
+
+	return (u16)y;
+}
+
+static u16 wdt87xx_calculate_checksum(const u8 *data, size_t length)
+{
+	u16 checksum = 0;
+	size_t i;
+
+	for (i = 0; i < length; i++)
+		checksum = misr(checksum, data[i]);
+
+	return checksum;
+}
+
+static int wdt87xx_get_checksum(struct i2c_client *client, u16 *checksum,
+				u32 address, int length)
+{
+	int error;
+	int time_delay;
+	u8 pkt_buf[PKT_BUF_SIZE];
+	u8 cmd_buf[CMD_BUF_SIZE];
+
+	error = wdt87xx_send_command(client, VND_SET_CHECKSUM_LENGTH, length);
+	if (error) {
+		dev_err(&client->dev, "failed to set checksum length\n");
+		return error;
+	}
+
+	error = wdt87xx_send_command(client, VND_SET_CHECKSUM_CALC, address);
+	if (error) {
+		dev_err(&client->dev, "failed to set checksum address\n");
+		return error;
+	}
+
+	/* Wait the operation to complete */
+	time_delay = DIV_ROUND_UP(length, 1024);
+	msleep(time_delay * 30);
+
+	memset(cmd_buf, 0, sizeof(cmd_buf));
+	cmd_buf[CMD_REPORT_ID_OFFSET] = VND_REQ_READ;
+	cmd_buf[CMD_TYPE_OFFSET] = VND_GET_CHECKSUM;
+	error = wdt87xx_set_feature(client, cmd_buf, sizeof(cmd_buf));
+	if (error) {
+		dev_err(&client->dev, "failed to request checksum\n");
+		return error;
+	}
+
+	memset(pkt_buf, 0, sizeof(pkt_buf));
+	pkt_buf[CMD_REPORT_ID_OFFSET] = VND_READ_DATA;
+	error = wdt87xx_get_feature(client, pkt_buf, sizeof(pkt_buf));
+	if (error) {
+		dev_err(&client->dev, "failed to read checksum\n");
+		return error;
+	}
+
+	*checksum = get_unaligned_le16(&pkt_buf[CMD_DATA_OFFSET]);
+	return 0;
+}
+
+static int wdt87xx_write_firmware(struct i2c_client *client, const void *chunk)
+{
+	u32 start_addr = get_unaligned_le32(chunk + FW_CHUNK_TGT_START_OFFSET);
+	u32 size = get_unaligned_le32(chunk + FW_CHUNK_PAYLOAD_LEN_OFFSET);
+	const void *data = chunk + FW_CHUNK_PAYLOAD_OFFSET;
+	int error;
+	int err1;
+	int page_size;
+	int retry = 0;
+	u16 device_checksum, firmware_checksum;
+
+	dev_dbg(&client->dev, "start 4k page program\n");
+
+	error = wdt87xx_send_command(client, VND_CMD_STOP, MODE_STOP);
+	if (error) {
+		dev_err(&client->dev, "stop report mode failed\n");
+		return error;
+	}
+
+	error = wdt87xx_send_command(client, VND_CMD_SFUNL, 0);
+	if (error) {
+		dev_err(&client->dev, "unlock failed\n");
+		goto out_enable_reporting;
+	}
+
+	mdelay(10);
+
+	while (size) {
+		dev_dbg(&client->dev, "%s: %x, %x\n", __func__,
+			start_addr, size);
+
+		page_size = min_t(u32, size, PG_SIZE);
+		size -= page_size;
+
+		for (retry = 0; retry < MAX_RETRIES; retry++) {
+			error = wdt87xx_send_command(client, VND_CMD_ERASE,
+						     start_addr);
+			if (error) {
+				dev_err(&client->dev,
+					"erase failed at %#08x\n", start_addr);
+				break;
+			}
+
+			msleep(50);
+
+			error = wdt87xx_write_data(client, data, start_addr,
+						   page_size);
+			if (error) {
+				dev_err(&client->dev,
+					"write failed at %#08x (%d bytes)\n",
+					start_addr, page_size);
+				break;
+			}
+
+			error = wdt87xx_get_checksum(client, &device_checksum,
+						     start_addr, page_size);
+			if (error) {
+				dev_err(&client->dev,
+					"failed to retrieve checksum for %#08x (len: %d)\n",
+					start_addr, page_size);
+				break;
+			}
+
+			firmware_checksum =
+				wdt87xx_calculate_checksum(data, page_size);
+
+			if (device_checksum == firmware_checksum)
+				break;
+
+			dev_err(&client->dev,
+				"checksum fail: %d vs %d, retry %d\n",
+				device_checksum, firmware_checksum, retry);
+		}
+
+		if (retry == MAX_RETRIES) {
+			dev_err(&client->dev, "page write failed\n");
+			error = -EIO;
+			goto out_lock_device;
+		}
+
+		start_addr = start_addr + page_size;
+		data = data + page_size;
+	}
+
+out_lock_device:
+	err1 = wdt87xx_send_command(client, VND_CMD_SFLCK, 0);
+	if (err1)
+		dev_err(&client->dev, "lock failed\n");
+
+	mdelay(10);
+
+out_enable_reporting:
+	err1 = wdt87xx_send_command(client, VND_CMD_START, 0);
+	if (err1)
+		dev_err(&client->dev, "start to report failed\n");
+
+	return error ? error : err1;
+}
+
+static int wdt87xx_load_chunk(struct i2c_client *client,
+			      const struct firmware *fw, u32 ck_id)
+{
+	const void *chunk;
+	int error;
+
+	chunk = wdt87xx_get_fw_chunk(fw, ck_id);
+	if (!chunk) {
+		dev_err(&client->dev, "unable to locate chunk (type %d)\n",
+			ck_id);
+		return -EINVAL;
+	}
+
+	error = wdt87xx_validate_fw_chunk(chunk, ck_id);
+	if (error) {
+		dev_err(&client->dev, "invalid chunk (type %d): %d\n",
+			ck_id, error);
+		return error;
+	}
+
+	error = wdt87xx_write_firmware(client, chunk);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to write fw chunk (type %d): %d\n",
+			ck_id, error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int wdt87xx_do_update_firmware(struct i2c_client *client,
+				      const struct firmware *fw,
+				      unsigned int chunk_id)
+{
+	struct wdt87xx_data *wdt = i2c_get_clientdata(client);
+	int error;
+
+	error = wdt87xx_validate_firmware(wdt, fw);
+	if (error)
+		return error;
+
+	error = mutex_lock_interruptible(&wdt->fw_mutex);
+	if (error)
+		return error;
+
+	disable_irq(client->irq);
+
+	error = wdt87xx_load_chunk(client, fw, chunk_id);
+	if (error) {
+		dev_err(&client->dev,
+			"firmware load failed (type: %d): %d\n",
+			chunk_id, error);
+		goto out;
+	}
+
+	error = wdt87xx_sw_reset(client);
+	if (error) {
+		dev_err(&client->dev, "soft reset failed: %d\n", error);
+		goto out;
+	}
+
+	/* Refresh the parameters */
+	error = wdt87xx_get_sysparam(client, &wdt->param);
+	if (error)
+		dev_err(&client->dev,
+			"failed to refresh system paramaters: %d\n", error);
+out:
+	enable_irq(client->irq);
+	mutex_unlock(&wdt->fw_mutex);
+
+	return error ? error : 0;
+}
+
+static int wdt87xx_update_firmware(struct device *dev,
+				   const char *fw_name, unsigned int chunk_id)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	const struct firmware *fw;
+	int error;
+
+	error = request_firmware(&fw, fw_name, dev);
+	if (error) {
+		dev_err(&client->dev, "unable to retrieve firmware %s: %d\n",
+			fw_name, error);
+		return error;
+	}
+
+	error = wdt87xx_do_update_firmware(client, fw, chunk_id);
+
+	release_firmware(fw);
+
+	return error ? error : 0;
+}
+
+static ssize_t config_csum_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct wdt87xx_data *wdt = i2c_get_clientdata(client);
+	u32 cfg_csum;
+
+	cfg_csum = wdt->param.xmls_id1;
+	cfg_csum = (cfg_csum << 16) | wdt->param.xmls_id2;
+
+	return scnprintf(buf, PAGE_SIZE, "%x\n", cfg_csum);
+}
+
+static ssize_t fw_version_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct wdt87xx_data *wdt = i2c_get_clientdata(client);
+
+	return scnprintf(buf, PAGE_SIZE, "%x\n", wdt->param.fw_id);
+}
+
+static ssize_t plat_id_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct wdt87xx_data *wdt = i2c_get_clientdata(client);
+
+	return scnprintf(buf, PAGE_SIZE, "%x\n", wdt->param.plat_id);
+}
+
+static ssize_t update_config_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count)
+{
+	int error;
+
+	error = wdt87xx_update_firmware(dev, WDT87XX_CFG_NAME, CHUNK_ID_CNFG);
+
+	return error ? error : count;
+}
+
+static ssize_t update_fw_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	int error;
+
+	error = wdt87xx_update_firmware(dev, WDT87XX_FW_NAME, CHUNK_ID_FRWR);
+
+	return error ? error : count;
+}
+
+static DEVICE_ATTR_RO(config_csum);
+static DEVICE_ATTR_RO(fw_version);
+static DEVICE_ATTR_RO(plat_id);
+static DEVICE_ATTR_WO(update_config);
+static DEVICE_ATTR_WO(update_fw);
+
+static struct attribute *wdt87xx_attrs[] = {
+	&dev_attr_config_csum.attr,
+	&dev_attr_fw_version.attr,
+	&dev_attr_plat_id.attr,
+	&dev_attr_update_config.attr,
+	&dev_attr_update_fw.attr,
+	NULL
+};
+
+static const struct attribute_group wdt87xx_attr_group = {
+	.attrs = wdt87xx_attrs,
+};
+
+static void wdt87xx_report_contact(struct input_dev *input,
+				   struct wdt87xx_sys_param *param,
+				   u8 *buf)
+{
+	int finger_id;
+	u32 x, y, w;
+	u8 p;
+
+	finger_id = (buf[FINGER_EV_V1_OFFSET_ID] >> 3) - 1;
+	if (finger_id < 0)
+		return;
+
+	/* Check if this is an active contact */
+	if (!(buf[FINGER_EV_V1_OFFSET_ID] & 0x1))
+		return;
+
+	w = buf[FINGER_EV_V1_OFFSET_W];
+	w *= param->scaling_factor;
+
+	p = buf[FINGER_EV_V1_OFFSET_P];
+
+	x = get_unaligned_le16(buf + FINGER_EV_V1_OFFSET_X);
+
+	y = get_unaligned_le16(buf + FINGER_EV_V1_OFFSET_Y);
+	y = DIV_ROUND_CLOSEST(y * param->phy_h, param->phy_w);
+
+	/* Refuse incorrect coordinates */
+	if (x > param->max_x || y > param->max_y)
+		return;
+
+	dev_dbg(input->dev.parent, "tip on (%d), x(%d), y(%d)\n",
+		finger_id, x, y);
+
+	input_mt_slot(input, finger_id);
+	input_mt_report_slot_state(input, MT_TOOL_FINGER, 1);
+	input_report_abs(input, ABS_MT_TOUCH_MAJOR, w);
+	input_report_abs(input, ABS_MT_PRESSURE, p);
+	input_report_abs(input, ABS_MT_POSITION_X, x);
+	input_report_abs(input, ABS_MT_POSITION_Y, y);
+}
+
+static irqreturn_t wdt87xx_ts_interrupt(int irq, void *dev_id)
+{
+	struct wdt87xx_data *wdt = dev_id;
+	struct i2c_client *client = wdt->client;
+	int i, fingers;
+	int error;
+	u8 raw_buf[WDT_V1_RAW_BUF_COUNT] = {0};
+
+	error = i2c_master_recv(client, raw_buf, WDT_V1_RAW_BUF_COUNT);
+	if (error < 0) {
+		dev_err(&client->dev, "read v1 raw data failed: %d\n", error);
+		goto irq_exit;
+	}
+
+	fingers = raw_buf[TOUCH_PK_V1_OFFSET_FNGR_NUM];
+	if (!fingers)
+		goto irq_exit;
+
+	for (i = 0; i < WDT_MAX_FINGER; i++)
+		wdt87xx_report_contact(wdt->input,
+				       &wdt->param,
+				       &raw_buf[TOUCH_PK_V1_OFFSET_EVENT +
+						i * FINGER_EV_V1_SIZE]);
+
+	input_mt_sync_frame(wdt->input);
+	input_sync(wdt->input);
+
+irq_exit:
+	return IRQ_HANDLED;
+}
+
+static int wdt87xx_ts_create_input_device(struct wdt87xx_data *wdt)
+{
+	struct device *dev = &wdt->client->dev;
+	struct input_dev *input;
+	unsigned int res = DIV_ROUND_CLOSEST(MAX_UNIT_AXIS, wdt->param.phy_w);
+	int error;
+
+	input = devm_input_allocate_device(dev);
+	if (!input) {
+		dev_err(dev, "failed to allocate input device\n");
+		return -ENOMEM;
+	}
+	wdt->input = input;
+
+	input->name = "WDT87xx Touchscreen";
+	input->id.bustype = BUS_I2C;
+	input->phys = wdt->phys;
+
+	input_set_abs_params(input, ABS_MT_POSITION_X, 0,
+			     wdt->param.max_x, 0, 0);
+	input_set_abs_params(input, ABS_MT_POSITION_Y, 0,
+			     wdt->param.max_y, 0, 0);
+	input_abs_set_res(input, ABS_MT_POSITION_X, res);
+	input_abs_set_res(input, ABS_MT_POSITION_Y, res);
+
+	input_set_abs_params(input, ABS_MT_TOUCH_MAJOR,
+			     0, wdt->param.max_x, 0, 0);
+	input_set_abs_params(input, ABS_MT_PRESSURE, 0, 0xFF, 0, 0);
+
+	input_mt_init_slots(input, WDT_MAX_FINGER,
+			    INPUT_MT_DIRECT | INPUT_MT_DROP_UNUSED);
+
+	error = input_register_device(input);
+	if (error) {
+		dev_err(dev, "failed to register input device: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int wdt87xx_ts_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct wdt87xx_data *wdt;
+	int error;
+
+	dev_dbg(&client->dev, "adapter=%d, client irq: %d\n",
+		client->adapter->nr, client->irq);
+
+	/* Check if the I2C function is ok in this adaptor */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
+		return -ENXIO;
+
+	wdt = devm_kzalloc(&client->dev, sizeof(*wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+
+	wdt->client = client;
+	mutex_init(&wdt->fw_mutex);
+	i2c_set_clientdata(client, wdt);
+
+	snprintf(wdt->phys, sizeof(wdt->phys), "i2c-%u-%04x/input0",
+		 client->adapter->nr, client->addr);
+
+	error = wdt87xx_get_sysparam(client, &wdt->param);
+	if (error)
+		return error;
+
+	error = wdt87xx_ts_create_input_device(wdt);
+	if (error)
+		return error;
+
+	error = devm_request_threaded_irq(&client->dev, client->irq,
+					  NULL, wdt87xx_ts_interrupt,
+					  IRQF_ONESHOT,
+					  client->name, wdt);
+	if (error) {
+		dev_err(&client->dev, "request irq failed: %d\n", error);
+		return error;
+	}
+
+	error = sysfs_create_group(&client->dev.kobj, &wdt87xx_attr_group);
+	if (error) {
+		dev_err(&client->dev, "create sysfs failed: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int wdt87xx_ts_remove(struct i2c_client *client)
+{
+	sysfs_remove_group(&client->dev.kobj, &wdt87xx_attr_group);
+
+	return 0;
+}
+
+static int __maybe_unused wdt87xx_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	int error;
+
+	disable_irq(client->irq);
+
+	error = wdt87xx_send_command(client, VND_CMD_STOP, MODE_IDLE);
+	if (error) {
+		enable_irq(client->irq);
+		dev_err(&client->dev,
+			"failed to stop device when suspending: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int __maybe_unused wdt87xx_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	int error;
+
+	/*
+	 * The chip may have been reset while system is resuming,
+	 * give it some time to settle.
+	 */
+	mdelay(100);
+
+	error = wdt87xx_send_command(client, VND_CMD_START, 0);
+	if (error)
+		dev_err(&client->dev,
+			"failed to start device when resuming: %d\n",
+			error);
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(wdt87xx_pm_ops, wdt87xx_suspend, wdt87xx_resume);
+
+static const struct i2c_device_id wdt87xx_dev_id[] = {
+	{ WDT87XX_NAME, 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, wdt87xx_dev_id);
+
+static const struct acpi_device_id wdt87xx_acpi_id[] = {
+	{ "WDHT0001", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, wdt87xx_acpi_id);
+
+static struct i2c_driver wdt87xx_driver = {
+	.probe		= wdt87xx_ts_probe,
+	.remove		= wdt87xx_ts_remove,
+	.id_table	= wdt87xx_dev_id,
+	.driver	= {
+		.name	= WDT87XX_NAME,
+		.pm     = &wdt87xx_pm_ops,
+		.acpi_match_table = ACPI_PTR(wdt87xx_acpi_id),
+	},
+};
+module_i2c_driver(wdt87xx_driver);
+
+MODULE_AUTHOR("HN Chen <hn.chen@weidahitech.com>");
+MODULE_DESCRIPTION("WeidaHiTech WDT87XX Touchscreen driver");
+MODULE_VERSION(WDT87XX_DRV_VER);
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/zforce_ts.c b/drivers/input/touchscreen/zforce_ts.c
index 19880c7385e3..f58a196521a9 100644
--- a/drivers/input/touchscreen/zforce_ts.c
+++ b/drivers/input/touchscreen/zforce_ts.c
@@ -30,7 +30,6 @@
 #include <linux/input/mt.h>
 #include <linux/platform_data/zforce_ts.h>
 #include <linux/regulator/consumer.h>
-#include <linux/delay.h>
 #include <linux/of.h>
 #include <linux/of_gpio.h>
 
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index a5c8d5e62871..f1fb1d3ccc56 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -222,7 +222,7 @@ config TEGRA_IOMMU_SMMU
 	select IOMMU_API
 	help
 	  This driver supports the IOMMU hardware (SMMU) found on NVIDIA Tegra
-	  SoCs (Tegra30 up to Tegra124).
+	  SoCs (Tegra30 up to Tegra132).
 
 config EXYNOS_IOMMU
 	bool "Exynos IOMMU Support"
@@ -342,6 +342,7 @@ config SPAPR_TCE_IOMMU
 	  Enables bits of IOMMU API required by VFIO. The iommu_ops
 	  is not implemented as it is not necessary for VFIO.
 
+# ARM IOMMU support
 config ARM_SMMU
 	bool "ARM Ltd. System MMU (SMMU) Support"
 	depends on (ARM64 || ARM) && MMU
@@ -355,4 +356,16 @@ config ARM_SMMU
 	  Say Y here if your SoC includes an IOMMU device implementing
 	  the ARM SMMU architecture.
 
+config ARM_SMMU_V3
+	bool "ARM Ltd. System MMU Version 3 (SMMUv3) Support"
+	depends on ARM64 && PCI
+	select IOMMU_API
+	select IOMMU_IO_PGTABLE_LPAE
+	help
+	  Support for implementations of the ARM System MMU architecture
+	  version 3 providing translation support to a PCIe root complex.
+
+	  Say Y here if your system includes an IOMMU device implementing
+	  the ARM SMMUv3 architecture.
+
 endif # IOMMU_SUPPORT
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 080ffab4ed1c..c6dcc513d711 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_MSM_IOMMU) += msm_iommu.o msm_iommu_dev.o
 obj-$(CONFIG_AMD_IOMMU) += amd_iommu.o amd_iommu_init.o
 obj-$(CONFIG_AMD_IOMMU_V2) += amd_iommu_v2.o
 obj-$(CONFIG_ARM_SMMU) += arm-smmu.o
+obj-$(CONFIG_ARM_SMMU_V3) += arm-smmu-v3.o
 obj-$(CONFIG_DMAR_TABLE) += dmar.o
 obj-$(CONFIG_INTEL_IOMMU) += intel-iommu.o
 obj-$(CONFIG_IPMMU_VMSA) += ipmmu-vmsa.o
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index e43d48956dea..a57e9b749895 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -34,6 +34,7 @@
 #include <linux/irq.h>
 #include <linux/msi.h>
 #include <linux/dma-contiguous.h>
+#include <linux/irqdomain.h>
 #include <asm/irq_remapping.h>
 #include <asm/io_apic.h>
 #include <asm/apic.h>
@@ -64,10 +65,6 @@
 
 static DEFINE_RWLOCK(amd_iommu_devtable_lock);
 
-/* A list of preallocated protection domains */
-static LIST_HEAD(iommu_pd_list);
-static DEFINE_SPINLOCK(iommu_pd_list_lock);
-
 /* List of all available dev_data structures */
 static LIST_HEAD(dev_data_list);
 static DEFINE_SPINLOCK(dev_data_list_lock);
@@ -119,7 +116,8 @@ struct iommu_cmd {
 struct kmem_cache *amd_iommu_irq_cache;
 
 static void update_domain(struct protection_domain *domain);
-static int __init alloc_passthrough_domain(void);
+static int alloc_passthrough_domain(void);
+static int protection_domain_init(struct protection_domain *domain);
 
 /****************************************************************************
  *
@@ -234,31 +232,38 @@ static bool pdev_pri_erratum(struct pci_dev *pdev, u32 erratum)
 }
 
 /*
- * In this function the list of preallocated protection domains is traversed to
- * find the domain for a specific device
+ * This function actually applies the mapping to the page table of the
+ * dma_ops domain.
  */
-static struct dma_ops_domain *find_protection_domain(u16 devid)
+static void alloc_unity_mapping(struct dma_ops_domain *dma_dom,
+				struct unity_map_entry *e)
 {
-	struct dma_ops_domain *entry, *ret = NULL;
-	unsigned long flags;
-	u16 alias = amd_iommu_alias_table[devid];
-
-	if (list_empty(&iommu_pd_list))
-		return NULL;
-
-	spin_lock_irqsave(&iommu_pd_list_lock, flags);
+	u64 addr;
 
-	list_for_each_entry(entry, &iommu_pd_list, list) {
-		if (entry->target_dev == devid ||
-		    entry->target_dev == alias) {
-			ret = entry;
-			break;
-		}
+	for (addr = e->address_start; addr < e->address_end;
+	     addr += PAGE_SIZE) {
+		if (addr < dma_dom->aperture_size)
+			__set_bit(addr >> PAGE_SHIFT,
+				  dma_dom->aperture[0]->bitmap);
 	}
+}
 
-	spin_unlock_irqrestore(&iommu_pd_list_lock, flags);
+/*
+ * Inits the unity mappings required for a specific device
+ */
+static void init_unity_mappings_for_device(struct device *dev,
+					   struct dma_ops_domain *dma_dom)
+{
+	struct unity_map_entry *e;
+	u16 devid;
 
-	return ret;
+	devid = get_device_id(dev);
+
+	list_for_each_entry(e, &amd_iommu_unity_map, list) {
+		if (!(devid >= e->devid_start && devid <= e->devid_end))
+			continue;
+		alloc_unity_mapping(dma_dom, e);
+	}
 }
 
 /*
@@ -290,11 +295,23 @@ static bool check_device(struct device *dev)
 
 static void init_iommu_group(struct device *dev)
 {
+	struct dma_ops_domain *dma_domain;
+	struct iommu_domain *domain;
 	struct iommu_group *group;
 
 	group = iommu_group_get_for_dev(dev);
-	if (!IS_ERR(group))
-		iommu_group_put(group);
+	if (IS_ERR(group))
+		return;
+
+	domain = iommu_group_default_domain(group);
+	if (!domain)
+		goto out;
+
+	dma_domain = to_pdomain(domain)->priv;
+
+	init_unity_mappings_for_device(dev, dma_domain);
+out:
+	iommu_group_put(group);
 }
 
 static int __last_alias(struct pci_dev *pdev, u16 alias, void *data)
@@ -434,64 +451,15 @@ static void iommu_uninit_device(struct device *dev)
 	/* Unlink from alias, it may change if another device is re-plugged */
 	dev_data->alias_data = NULL;
 
+	/* Remove dma-ops */
+	dev->archdata.dma_ops = NULL;
+
 	/*
 	 * We keep dev_data around for unplugged devices and reuse it when the
 	 * device is re-plugged - not doing so would introduce a ton of races.
 	 */
 }
 
-void __init amd_iommu_uninit_devices(void)
-{
-	struct iommu_dev_data *dev_data, *n;
-	struct pci_dev *pdev = NULL;
-
-	for_each_pci_dev(pdev) {
-
-		if (!check_device(&pdev->dev))
-			continue;
-
-		iommu_uninit_device(&pdev->dev);
-	}
-
-	/* Free all of our dev_data structures */
-	list_for_each_entry_safe(dev_data, n, &dev_data_list, dev_data_list)
-		free_dev_data(dev_data);
-}
-
-int __init amd_iommu_init_devices(void)
-{
-	struct pci_dev *pdev = NULL;
-	int ret = 0;
-
-	for_each_pci_dev(pdev) {
-
-		if (!check_device(&pdev->dev))
-			continue;
-
-		ret = iommu_init_device(&pdev->dev);
-		if (ret == -ENOTSUPP)
-			iommu_ignore_device(&pdev->dev);
-		else if (ret)
-			goto out_free;
-	}
-
-	/*
-	 * Initialize IOMMU groups only after iommu_init_device() has
-	 * had a chance to populate any IVRS defined aliases.
-	 */
-	for_each_pci_dev(pdev) {
-		if (check_device(&pdev->dev))
-			init_iommu_group(&pdev->dev);
-	}
-
-	return 0;
-
-out_free:
-
-	amd_iommu_uninit_devices();
-
-	return ret;
-}
 #ifdef CONFIG_AMD_IOMMU_STATS
 
 /*
@@ -1463,94 +1431,6 @@ static unsigned long iommu_unmap_page(struct protection_domain *dom,
 	return unmapped;
 }
 
-/*
- * This function checks if a specific unity mapping entry is needed for
- * this specific IOMMU.
- */
-static int iommu_for_unity_map(struct amd_iommu *iommu,
-			       struct unity_map_entry *entry)
-{
-	u16 bdf, i;
-
-	for (i = entry->devid_start; i <= entry->devid_end; ++i) {
-		bdf = amd_iommu_alias_table[i];
-		if (amd_iommu_rlookup_table[bdf] == iommu)
-			return 1;
-	}
-
-	return 0;
-}
-
-/*
- * This function actually applies the mapping to the page table of the
- * dma_ops domain.
- */
-static int dma_ops_unity_map(struct dma_ops_domain *dma_dom,
-			     struct unity_map_entry *e)
-{
-	u64 addr;
-	int ret;
-
-	for (addr = e->address_start; addr < e->address_end;
-	     addr += PAGE_SIZE) {
-		ret = iommu_map_page(&dma_dom->domain, addr, addr, e->prot,
-				     PAGE_SIZE);
-		if (ret)
-			return ret;
-		/*
-		 * if unity mapping is in aperture range mark the page
-		 * as allocated in the aperture
-		 */
-		if (addr < dma_dom->aperture_size)
-			__set_bit(addr >> PAGE_SHIFT,
-				  dma_dom->aperture[0]->bitmap);
-	}
-
-	return 0;
-}
-
-/*
- * Init the unity mappings for a specific IOMMU in the system
- *
- * Basically iterates over all unity mapping entries and applies them to
- * the default domain DMA of that IOMMU if necessary.
- */
-static int iommu_init_unity_mappings(struct amd_iommu *iommu)
-{
-	struct unity_map_entry *entry;
-	int ret;
-
-	list_for_each_entry(entry, &amd_iommu_unity_map, list) {
-		if (!iommu_for_unity_map(iommu, entry))
-			continue;
-		ret = dma_ops_unity_map(iommu->default_dom, entry);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-
-/*
- * Inits the unity mappings required for a specific device
- */
-static int init_unity_mappings_for_device(struct dma_ops_domain *dma_dom,
-					  u16 devid)
-{
-	struct unity_map_entry *e;
-	int ret;
-
-	list_for_each_entry(e, &amd_iommu_unity_map, list) {
-		if (!(devid >= e->devid_start && devid <= e->devid_end))
-			continue;
-		ret = dma_ops_unity_map(dma_dom, e);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-
 /****************************************************************************
  *
  * The next functions belong to the address allocator for the dma_ops
@@ -1704,14 +1584,16 @@ static unsigned long dma_ops_area_alloc(struct device *dev,
 	unsigned long next_bit = dom->next_address % APERTURE_RANGE_SIZE;
 	int max_index = dom->aperture_size >> APERTURE_RANGE_SHIFT;
 	int i = start >> APERTURE_RANGE_SHIFT;
-	unsigned long boundary_size;
+	unsigned long boundary_size, mask;
 	unsigned long address = -1;
 	unsigned long limit;
 
 	next_bit >>= PAGE_SHIFT;
 
-	boundary_size = ALIGN(dma_get_seg_boundary(dev) + 1,
-			PAGE_SIZE) >> PAGE_SHIFT;
+	mask = dma_get_seg_boundary(dev);
+
+	boundary_size = mask + 1 ? ALIGN(mask + 1, PAGE_SIZE) >> PAGE_SHIFT :
+				   1UL << (BITS_PER_LONG - PAGE_SHIFT);
 
 	for (;i < max_index; ++i) {
 		unsigned long offset = dom->aperture[i]->offset >> PAGE_SHIFT;
@@ -1869,9 +1751,15 @@ static void free_pt_##LVL (unsigned long __pt)			\
 	pt = (u64 *)__pt;					\
 								\
 	for (i = 0; i < 512; ++i) {				\
+		/* PTE present? */				\
 		if (!IOMMU_PTE_PRESENT(pt[i]))			\
 			continue;				\
 								\
+		/* Large PTE? */				\
+		if (PM_PTE_LEVEL(pt[i]) == 0 ||			\
+		    PM_PTE_LEVEL(pt[i]) == 7)			\
+			continue;				\
+								\
 		p = (unsigned long)IOMMU_PTE_PAGE(pt[i]);	\
 		FN(p);						\
 	}							\
@@ -1994,12 +1882,9 @@ static struct dma_ops_domain *dma_ops_domain_alloc(void)
 	if (!dma_dom)
 		return NULL;
 
-	spin_lock_init(&dma_dom->domain.lock);
-
-	dma_dom->domain.id = domain_id_alloc();
-	if (dma_dom->domain.id == 0)
+	if (protection_domain_init(&dma_dom->domain))
 		goto free_dma_dom;
-	INIT_LIST_HEAD(&dma_dom->domain.dev_list);
+
 	dma_dom->domain.mode = PAGE_MODE_2_LEVEL;
 	dma_dom->domain.pt_root = (void *)get_zeroed_page(GFP_KERNEL);
 	dma_dom->domain.flags = PD_DMA_OPS_MASK;
@@ -2008,7 +1893,6 @@ static struct dma_ops_domain *dma_ops_domain_alloc(void)
 		goto free_dma_dom;
 
 	dma_dom->need_flush = false;
-	dma_dom->target_dev = 0xffff;
 
 	add_domain_to_list(&dma_dom->domain);
 
@@ -2373,110 +2257,67 @@ static void detach_device(struct device *dev)
 	dev_data->ats.enabled = false;
 }
 
-/*
- * Find out the protection domain structure for a given PCI device. This
- * will give us the pointer to the page table root for example.
- */
-static struct protection_domain *domain_for_device(struct device *dev)
-{
-	struct iommu_dev_data *dev_data;
-	struct protection_domain *dom = NULL;
-	unsigned long flags;
-
-	dev_data   = get_dev_data(dev);
-
-	if (dev_data->domain)
-		return dev_data->domain;
-
-	if (dev_data->alias_data != NULL) {
-		struct iommu_dev_data *alias_data = dev_data->alias_data;
-
-		read_lock_irqsave(&amd_iommu_devtable_lock, flags);
-		if (alias_data->domain != NULL) {
-			__attach_device(dev_data, alias_data->domain);
-			dom = alias_data->domain;
-		}
-		read_unlock_irqrestore(&amd_iommu_devtable_lock, flags);
-	}
-
-	return dom;
-}
-
-static int device_change_notifier(struct notifier_block *nb,
-				  unsigned long action, void *data)
+static int amd_iommu_add_device(struct device *dev)
 {
-	struct dma_ops_domain *dma_domain;
-	struct protection_domain *domain;
 	struct iommu_dev_data *dev_data;
-	struct device *dev = data;
+	struct iommu_domain *domain;
 	struct amd_iommu *iommu;
-	unsigned long flags;
 	u16 devid;
+	int ret;
 
-	if (!check_device(dev))
+	if (!check_device(dev) || get_dev_data(dev))
 		return 0;
 
-	devid    = get_device_id(dev);
-	iommu    = amd_iommu_rlookup_table[devid];
-	dev_data = get_dev_data(dev);
-
-	switch (action) {
-	case BUS_NOTIFY_ADD_DEVICE:
+	devid = get_device_id(dev);
+	iommu = amd_iommu_rlookup_table[devid];
 
-		iommu_init_device(dev);
-		init_iommu_group(dev);
+	ret = iommu_init_device(dev);
+	if (ret) {
+		if (ret != -ENOTSUPP)
+			pr_err("Failed to initialize device %s - trying to proceed anyway\n",
+				dev_name(dev));
 
-		/*
-		 * dev_data is still NULL and
-		 * got initialized in iommu_init_device
-		 */
-		dev_data = get_dev_data(dev);
+		iommu_ignore_device(dev);
+		dev->archdata.dma_ops = &nommu_dma_ops;
+		goto out;
+	}
+	init_iommu_group(dev);
 
-		if (iommu_pass_through || dev_data->iommu_v2) {
-			dev_data->passthrough = true;
-			attach_device(dev, pt_domain);
-			break;
-		}
+	dev_data = get_dev_data(dev);
 
-		domain = domain_for_device(dev);
+	BUG_ON(!dev_data);
 
-		/* allocate a protection domain if a device is added */
-		dma_domain = find_protection_domain(devid);
-		if (!dma_domain) {
-			dma_domain = dma_ops_domain_alloc();
-			if (!dma_domain)
-				goto out;
-			dma_domain->target_dev = devid;
-
-			spin_lock_irqsave(&iommu_pd_list_lock, flags);
-			list_add_tail(&dma_domain->list, &iommu_pd_list);
-			spin_unlock_irqrestore(&iommu_pd_list_lock, flags);
-		}
+	if (dev_data->iommu_v2)
+		iommu_request_dm_for_dev(dev);
 
+	/* Domains are initialized for this device - have a look what we ended up with */
+	domain = iommu_get_domain_for_dev(dev);
+	if (domain->type == IOMMU_DOMAIN_IDENTITY) {
+		dev_data->passthrough = true;
+		dev->archdata.dma_ops = &nommu_dma_ops;
+	} else {
 		dev->archdata.dma_ops = &amd_iommu_dma_ops;
-
-		break;
-	case BUS_NOTIFY_REMOVED_DEVICE:
-
-		iommu_uninit_device(dev);
-
-	default:
-		goto out;
 	}
 
+out:
 	iommu_completion_wait(iommu);
 
-out:
 	return 0;
 }
 
-static struct notifier_block device_nb = {
-	.notifier_call = device_change_notifier,
-};
-
-void amd_iommu_init_notifier(void)
+static void amd_iommu_remove_device(struct device *dev)
 {
-	bus_register_notifier(&pci_bus_type, &device_nb);
+	struct amd_iommu *iommu;
+	u16 devid;
+
+	if (!check_device(dev))
+		return;
+
+	devid = get_device_id(dev);
+	iommu = amd_iommu_rlookup_table[devid];
+
+	iommu_uninit_device(dev);
+	iommu_completion_wait(iommu);
 }
 
 /*****************************************************************************
@@ -2495,28 +2336,20 @@ void amd_iommu_init_notifier(void)
 static struct protection_domain *get_domain(struct device *dev)
 {
 	struct protection_domain *domain;
-	struct dma_ops_domain *dma_dom;
-	u16 devid = get_device_id(dev);
+	struct iommu_domain *io_domain;
 
 	if (!check_device(dev))
 		return ERR_PTR(-EINVAL);
 
-	domain = domain_for_device(dev);
-	if (domain != NULL && !dma_ops_domain(domain))
-		return ERR_PTR(-EBUSY);
-
-	if (domain != NULL)
-		return domain;
+	io_domain = iommu_get_domain_for_dev(dev);
+	if (!io_domain)
+		return NULL;
 
-	/* Device not bound yet - bind it */
-	dma_dom = find_protection_domain(devid);
-	if (!dma_dom)
-		dma_dom = amd_iommu_rlookup_table[devid]->default_dom;
-	attach_device(dev, &dma_dom->domain);
-	DUMP_printk("Using protection domain %d for device %s\n",
-		    dma_dom->domain.id, dev_name(dev));
+	domain = to_pdomain(io_domain);
+	if (!dma_ops_domain(domain))
+		return ERR_PTR(-EBUSY);
 
-	return &dma_dom->domain;
+	return domain;
 }
 
 static void update_device_table(struct protection_domain *domain)
@@ -2930,6 +2763,7 @@ static void *alloc_coherent(struct device *dev, size_t size,
 	size	  = PAGE_ALIGN(size);
 	dma_mask  = dev->coherent_dma_mask;
 	flag     &= ~(__GFP_DMA | __GFP_HIGHMEM | __GFP_DMA32);
+	flag     |= __GFP_ZERO;
 
 	page = alloc_pages(flag | __GFP_NOWARN,  get_order(size));
 	if (!page) {
@@ -3011,54 +2845,6 @@ static int amd_iommu_dma_supported(struct device *dev, u64 mask)
 	return check_device(dev);
 }
 
-/*
- * The function for pre-allocating protection domains.
- *
- * If the driver core informs the DMA layer if a driver grabs a device
- * we don't need to preallocate the protection domains anymore.
- * For now we have to.
- */
-static void __init prealloc_protection_domains(void)
-{
-	struct iommu_dev_data *dev_data;
-	struct dma_ops_domain *dma_dom;
-	struct pci_dev *dev = NULL;
-	u16 devid;
-
-	for_each_pci_dev(dev) {
-
-		/* Do we handle this device? */
-		if (!check_device(&dev->dev))
-			continue;
-
-		dev_data = get_dev_data(&dev->dev);
-		if (!amd_iommu_force_isolation && dev_data->iommu_v2) {
-			/* Make sure passthrough domain is allocated */
-			alloc_passthrough_domain();
-			dev_data->passthrough = true;
-			attach_device(&dev->dev, pt_domain);
-			pr_info("AMD-Vi: Using passthrough domain for device %s\n",
-				dev_name(&dev->dev));
-		}
-
-		/* Is there already any domain for it? */
-		if (domain_for_device(&dev->dev))
-			continue;
-
-		devid = get_device_id(&dev->dev);
-
-		dma_dom = dma_ops_domain_alloc();
-		if (!dma_dom)
-			continue;
-		init_unity_mappings_for_device(dma_dom, devid);
-		dma_dom->target_dev = devid;
-
-		attach_device(&dev->dev, &dma_dom->domain);
-
-		list_add_tail(&dma_dom->list, &iommu_pd_list);
-	}
-}
-
 static struct dma_map_ops amd_iommu_dma_ops = {
 	.alloc = alloc_coherent,
 	.free = free_coherent,
@@ -3069,76 +2855,16 @@ static struct dma_map_ops amd_iommu_dma_ops = {
 	.dma_supported = amd_iommu_dma_supported,
 };
 
-static unsigned device_dma_ops_init(void)
+int __init amd_iommu_init_api(void)
 {
-	struct iommu_dev_data *dev_data;
-	struct pci_dev *pdev = NULL;
-	unsigned unhandled = 0;
-
-	for_each_pci_dev(pdev) {
-		if (!check_device(&pdev->dev)) {
-
-			iommu_ignore_device(&pdev->dev);
-
-			unhandled += 1;
-			continue;
-		}
-
-		dev_data = get_dev_data(&pdev->dev);
-
-		if (!dev_data->passthrough)
-			pdev->dev.archdata.dma_ops = &amd_iommu_dma_ops;
-		else
-			pdev->dev.archdata.dma_ops = &nommu_dma_ops;
-	}
-
-	return unhandled;
-}
-
-/*
- * The function which clues the AMD IOMMU driver into dma_ops.
- */
-
-void __init amd_iommu_init_api(void)
-{
-	bus_set_iommu(&pci_bus_type, &amd_iommu_ops);
+	return bus_set_iommu(&pci_bus_type, &amd_iommu_ops);
 }
 
 int __init amd_iommu_init_dma_ops(void)
 {
-	struct amd_iommu *iommu;
-	int ret, unhandled;
-
-	/*
-	 * first allocate a default protection domain for every IOMMU we
-	 * found in the system. Devices not assigned to any other
-	 * protection domain will be assigned to the default one.
-	 */
-	for_each_iommu(iommu) {
-		iommu->default_dom = dma_ops_domain_alloc();
-		if (iommu->default_dom == NULL)
-			return -ENOMEM;
-		iommu->default_dom->domain.flags |= PD_DEFAULT_MASK;
-		ret = iommu_init_unity_mappings(iommu);
-		if (ret)
-			goto free_domains;
-	}
-
-	/*
-	 * Pre-allocate the protection domains for each device.
-	 */
-	prealloc_protection_domains();
-
 	iommu_detected = 1;
 	swiotlb = 0;
 
-	/* Make the driver finally visible to the drivers */
-	unhandled = device_dma_ops_init();
-	if (unhandled && max_pfn > MAX_DMA32_PFN) {
-		/* There are unhandled devices - initialize swiotlb for them */
-		swiotlb = 1;
-	}
-
 	amd_iommu_stats_init();
 
 	if (amd_iommu_unmap_flush)
@@ -3147,14 +2873,6 @@ int __init amd_iommu_init_dma_ops(void)
 		pr_info("AMD-Vi: Lazy IO/TLB flushing enabled\n");
 
 	return 0;
-
-free_domains:
-
-	for_each_iommu(iommu) {
-		dma_ops_domain_free(iommu->default_dom);
-	}
-
-	return ret;
 }
 
 /*****************************************************************************
@@ -3196,6 +2914,18 @@ static void protection_domain_free(struct protection_domain *domain)
 	kfree(domain);
 }
 
+static int protection_domain_init(struct protection_domain *domain)
+{
+	spin_lock_init(&domain->lock);
+	mutex_init(&domain->api_lock);
+	domain->id = domain_id_alloc();
+	if (!domain->id)
+		return -ENOMEM;
+	INIT_LIST_HEAD(&domain->dev_list);
+
+	return 0;
+}
+
 static struct protection_domain *protection_domain_alloc(void)
 {
 	struct protection_domain *domain;
@@ -3204,12 +2934,8 @@ static struct protection_domain *protection_domain_alloc(void)
 	if (!domain)
 		return NULL;
 
-	spin_lock_init(&domain->lock);
-	mutex_init(&domain->api_lock);
-	domain->id = domain_id_alloc();
-	if (!domain->id)
+	if (protection_domain_init(domain))
 		goto out_err;
-	INIT_LIST_HEAD(&domain->dev_list);
 
 	add_domain_to_list(domain);
 
@@ -3221,7 +2947,7 @@ out_err:
 	return NULL;
 }
 
-static int __init alloc_passthrough_domain(void)
+static int alloc_passthrough_domain(void)
 {
 	if (pt_domain != NULL)
 		return 0;
@@ -3239,30 +2965,46 @@ static int __init alloc_passthrough_domain(void)
 static struct iommu_domain *amd_iommu_domain_alloc(unsigned type)
 {
 	struct protection_domain *pdomain;
+	struct dma_ops_domain *dma_domain;
 
-	/* We only support unmanaged domains for now */
-	if (type != IOMMU_DOMAIN_UNMANAGED)
-		return NULL;
-
-	pdomain = protection_domain_alloc();
-	if (!pdomain)
-		goto out_free;
+	switch (type) {
+	case IOMMU_DOMAIN_UNMANAGED:
+		pdomain = protection_domain_alloc();
+		if (!pdomain)
+			return NULL;
 
-	pdomain->mode    = PAGE_MODE_3_LEVEL;
-	pdomain->pt_root = (void *)get_zeroed_page(GFP_KERNEL);
-	if (!pdomain->pt_root)
-		goto out_free;
+		pdomain->mode    = PAGE_MODE_3_LEVEL;
+		pdomain->pt_root = (void *)get_zeroed_page(GFP_KERNEL);
+		if (!pdomain->pt_root) {
+			protection_domain_free(pdomain);
+			return NULL;
+		}
 
-	pdomain->domain.geometry.aperture_start = 0;
-	pdomain->domain.geometry.aperture_end   = ~0ULL;
-	pdomain->domain.geometry.force_aperture = true;
+		pdomain->domain.geometry.aperture_start = 0;
+		pdomain->domain.geometry.aperture_end   = ~0ULL;
+		pdomain->domain.geometry.force_aperture = true;
 
-	return &pdomain->domain;
+		break;
+	case IOMMU_DOMAIN_DMA:
+		dma_domain = dma_ops_domain_alloc();
+		if (!dma_domain) {
+			pr_err("AMD-Vi: Failed to allocate\n");
+			return NULL;
+		}
+		pdomain = &dma_domain->domain;
+		break;
+	case IOMMU_DOMAIN_IDENTITY:
+		pdomain = protection_domain_alloc();
+		if (!pdomain)
+			return NULL;
 
-out_free:
-	protection_domain_free(pdomain);
+		pdomain->mode = PAGE_MODE_NONE;
+		break;
+	default:
+		return NULL;
+	}
 
-	return NULL;
+	return &pdomain->domain;
 }
 
 static void amd_iommu_domain_free(struct iommu_domain *dom)
@@ -3412,6 +3154,47 @@ static bool amd_iommu_capable(enum iommu_cap cap)
 	return false;
 }
 
+static void amd_iommu_get_dm_regions(struct device *dev,
+				     struct list_head *head)
+{
+	struct unity_map_entry *entry;
+	u16 devid;
+
+	devid = get_device_id(dev);
+
+	list_for_each_entry(entry, &amd_iommu_unity_map, list) {
+		struct iommu_dm_region *region;
+
+		if (devid < entry->devid_start || devid > entry->devid_end)
+			continue;
+
+		region = kzalloc(sizeof(*region), GFP_KERNEL);
+		if (!region) {
+			pr_err("Out of memory allocating dm-regions for %s\n",
+				dev_name(dev));
+			return;
+		}
+
+		region->start = entry->address_start;
+		region->length = entry->address_end - entry->address_start;
+		if (entry->prot & IOMMU_PROT_IR)
+			region->prot |= IOMMU_READ;
+		if (entry->prot & IOMMU_PROT_IW)
+			region->prot |= IOMMU_WRITE;
+
+		list_add_tail(&region->list, head);
+	}
+}
+
+static void amd_iommu_put_dm_regions(struct device *dev,
+				     struct list_head *head)
+{
+	struct iommu_dm_region *entry, *next;
+
+	list_for_each_entry_safe(entry, next, head, list)
+		kfree(entry);
+}
+
 static const struct iommu_ops amd_iommu_ops = {
 	.capable = amd_iommu_capable,
 	.domain_alloc = amd_iommu_domain_alloc,
@@ -3422,6 +3205,10 @@ static const struct iommu_ops amd_iommu_ops = {
 	.unmap = amd_iommu_unmap,
 	.map_sg = default_iommu_map_sg,
 	.iova_to_phys = amd_iommu_iova_to_phys,
+	.add_device = amd_iommu_add_device,
+	.remove_device = amd_iommu_remove_device,
+	.get_dm_regions = amd_iommu_get_dm_regions,
+	.put_dm_regions = amd_iommu_put_dm_regions,
 	.pgsize_bitmap	= AMD_IOMMU_PGSIZES,
 };
 
@@ -3851,6 +3638,21 @@ union irte {
 	} fields;
 };
 
+struct irq_2_irte {
+	u16 devid; /* Device ID for IRTE table */
+	u16 index; /* Index into IRTE table*/
+};
+
+struct amd_ir_data {
+	struct irq_2_irte			irq_2_irte;
+	union irte				irte_entry;
+	union {
+		struct msi_msg			msi_entry;
+	};
+};
+
+static struct irq_chip amd_ir_chip;
+
 #define DTE_IRQ_PHYS_ADDR_MASK	(((1ULL << 45)-1) << 6)
 #define DTE_IRQ_REMAP_INTCTL    (2ULL << 60)
 #define DTE_IRQ_TABLE_LEN       (8ULL << 1)
@@ -3944,7 +3746,7 @@ out_unlock:
 	return table;
 }
 
-static int alloc_irq_index(struct irq_cfg *cfg, u16 devid, int count)
+static int alloc_irq_index(u16 devid, int count)
 {
 	struct irq_remap_table *table;
 	unsigned long flags;
@@ -3966,18 +3768,10 @@ static int alloc_irq_index(struct irq_cfg *cfg, u16 devid, int count)
 			c = 0;
 
 		if (c == count)	{
-			struct irq_2_irte *irte_info;
-
 			for (; c != 0; --c)
 				table->table[index - c + 1] = IRTE_ALLOCATED;
 
 			index -= count - 1;
-
-			cfg->remapped	      = 1;
-			irte_info             = &cfg->irq_2_irte;
-			irte_info->devid      = devid;
-			irte_info->index      = index;
-
 			goto out;
 		}
 	}
@@ -3990,22 +3784,6 @@ out:
 	return index;
 }
 
-static int get_irte(u16 devid, int index, union irte *irte)
-{
-	struct irq_remap_table *table;
-	unsigned long flags;
-
-	table = get_irq_table(devid, false);
-	if (!table)
-		return -ENOMEM;
-
-	spin_lock_irqsave(&table->lock, flags);
-	irte->val = table->table[index];
-	spin_unlock_irqrestore(&table->lock, flags);
-
-	return 0;
-}
-
 static int modify_irte(u16 devid, int index, union irte irte)
 {
 	struct irq_remap_table *table;
@@ -4052,243 +3830,316 @@ static void free_irte(u16 devid, int index)
 	iommu_completion_wait(iommu);
 }
 
-static int setup_ioapic_entry(int irq, struct IO_APIC_route_entry *entry,
-			      unsigned int destination, int vector,
-			      struct io_apic_irq_attr *attr)
+static int get_devid(struct irq_alloc_info *info)
 {
-	struct irq_remap_table *table;
-	struct irq_2_irte *irte_info;
-	struct irq_cfg *cfg;
-	union irte irte;
-	int ioapic_id;
-	int index;
-	int devid;
-	int ret;
-
-	cfg = irq_cfg(irq);
-	if (!cfg)
-		return -EINVAL;
-
-	irte_info = &cfg->irq_2_irte;
-	ioapic_id = mpc_ioapic_id(attr->ioapic);
-	devid     = get_ioapic_devid(ioapic_id);
+	int devid = -1;
 
-	if (devid < 0)
-		return devid;
-
-	table = get_irq_table(devid, true);
-	if (table == NULL)
-		return -ENOMEM;
-
-	index = attr->ioapic_pin;
-
-	/* Setup IRQ remapping info */
-	cfg->remapped	      = 1;
-	irte_info->devid      = devid;
-	irte_info->index      = index;
+	switch (info->type) {
+	case X86_IRQ_ALLOC_TYPE_IOAPIC:
+		devid     = get_ioapic_devid(info->ioapic_id);
+		break;
+	case X86_IRQ_ALLOC_TYPE_HPET:
+		devid     = get_hpet_devid(info->hpet_id);
+		break;
+	case X86_IRQ_ALLOC_TYPE_MSI:
+	case X86_IRQ_ALLOC_TYPE_MSIX:
+		devid = get_device_id(&info->msi_dev->dev);
+		break;
+	default:
+		BUG_ON(1);
+		break;
+	}
 
-	/* Setup IRTE for IOMMU */
-	irte.val		= 0;
-	irte.fields.vector      = vector;
-	irte.fields.int_type    = apic->irq_delivery_mode;
-	irte.fields.destination = destination;
-	irte.fields.dm          = apic->irq_dest_mode;
-	irte.fields.valid       = 1;
-
-	ret = modify_irte(devid, index, irte);
-	if (ret)
-		return ret;
+	return devid;
+}
 
-	/* Setup IOAPIC entry */
-	memset(entry, 0, sizeof(*entry));
+static struct irq_domain *get_ir_irq_domain(struct irq_alloc_info *info)
+{
+	struct amd_iommu *iommu;
+	int devid;
 
-	entry->vector        = index;
-	entry->mask          = 0;
-	entry->trigger       = attr->trigger;
-	entry->polarity      = attr->polarity;
+	if (!info)
+		return NULL;
 
-	/*
-	 * Mask level triggered irqs.
-	 */
-	if (attr->trigger)
-		entry->mask = 1;
+	devid = get_devid(info);
+	if (devid >= 0) {
+		iommu = amd_iommu_rlookup_table[devid];
+		if (iommu)
+			return iommu->ir_domain;
+	}
 
-	return 0;
+	return NULL;
 }
 
-static int set_affinity(struct irq_data *data, const struct cpumask *mask,
-			bool force)
+static struct irq_domain *get_irq_domain(struct irq_alloc_info *info)
 {
-	struct irq_2_irte *irte_info;
-	unsigned int dest, irq;
-	struct irq_cfg *cfg;
-	union irte irte;
-	int err;
-
-	if (!config_enabled(CONFIG_SMP))
-		return -1;
-
-	cfg       = irqd_cfg(data);
-	irq       = data->irq;
-	irte_info = &cfg->irq_2_irte;
+	struct amd_iommu *iommu;
+	int devid;
 
-	if (!cpumask_intersects(mask, cpu_online_mask))
-		return -EINVAL;
+	if (!info)
+		return NULL;
 
-	if (get_irte(irte_info->devid, irte_info->index, &irte))
-		return -EBUSY;
+	switch (info->type) {
+	case X86_IRQ_ALLOC_TYPE_MSI:
+	case X86_IRQ_ALLOC_TYPE_MSIX:
+		devid = get_device_id(&info->msi_dev->dev);
+		if (devid >= 0) {
+			iommu = amd_iommu_rlookup_table[devid];
+			if (iommu)
+				return iommu->msi_domain;
+		}
+		break;
+	default:
+		break;
+	}
 
-	if (assign_irq_vector(irq, cfg, mask))
-		return -EBUSY;
+	return NULL;
+}
 
-	err = apic->cpu_mask_to_apicid_and(cfg->domain, mask, &dest);
-	if (err) {
-		if (assign_irq_vector(irq, cfg, data->affinity))
-			pr_err("AMD-Vi: Failed to recover vector for irq %d\n", irq);
-		return err;
-	}
+struct irq_remap_ops amd_iommu_irq_ops = {
+	.prepare		= amd_iommu_prepare,
+	.enable			= amd_iommu_enable,
+	.disable		= amd_iommu_disable,
+	.reenable		= amd_iommu_reenable,
+	.enable_faulting	= amd_iommu_enable_faulting,
+	.get_ir_irq_domain	= get_ir_irq_domain,
+	.get_irq_domain		= get_irq_domain,
+};
 
-	irte.fields.vector      = cfg->vector;
-	irte.fields.destination = dest;
+static void irq_remapping_prepare_irte(struct amd_ir_data *data,
+				       struct irq_cfg *irq_cfg,
+				       struct irq_alloc_info *info,
+				       int devid, int index, int sub_handle)
+{
+	struct irq_2_irte *irte_info = &data->irq_2_irte;
+	struct msi_msg *msg = &data->msi_entry;
+	union irte *irte = &data->irte_entry;
+	struct IO_APIC_route_entry *entry;
 
-	modify_irte(irte_info->devid, irte_info->index, irte);
+	data->irq_2_irte.devid = devid;
+	data->irq_2_irte.index = index + sub_handle;
 
-	if (cfg->move_in_progress)
-		send_cleanup_vector(cfg);
+	/* Setup IRTE for IOMMU */
+	irte->val = 0;
+	irte->fields.vector      = irq_cfg->vector;
+	irte->fields.int_type    = apic->irq_delivery_mode;
+	irte->fields.destination = irq_cfg->dest_apicid;
+	irte->fields.dm          = apic->irq_dest_mode;
+	irte->fields.valid       = 1;
+
+	switch (info->type) {
+	case X86_IRQ_ALLOC_TYPE_IOAPIC:
+		/* Setup IOAPIC entry */
+		entry = info->ioapic_entry;
+		info->ioapic_entry = NULL;
+		memset(entry, 0, sizeof(*entry));
+		entry->vector        = index;
+		entry->mask          = 0;
+		entry->trigger       = info->ioapic_trigger;
+		entry->polarity      = info->ioapic_polarity;
+		/* Mask level triggered irqs. */
+		if (info->ioapic_trigger)
+			entry->mask = 1;
+		break;
 
-	cpumask_copy(data->affinity, mask);
+	case X86_IRQ_ALLOC_TYPE_HPET:
+	case X86_IRQ_ALLOC_TYPE_MSI:
+	case X86_IRQ_ALLOC_TYPE_MSIX:
+		msg->address_hi = MSI_ADDR_BASE_HI;
+		msg->address_lo = MSI_ADDR_BASE_LO;
+		msg->data = irte_info->index;
+		break;
 
-	return 0;
+	default:
+		BUG_ON(1);
+		break;
+	}
 }
 
-static int free_irq(int irq)
+static int irq_remapping_alloc(struct irq_domain *domain, unsigned int virq,
+			       unsigned int nr_irqs, void *arg)
 {
-	struct irq_2_irte *irte_info;
+	struct irq_alloc_info *info = arg;
+	struct irq_data *irq_data;
+	struct amd_ir_data *data;
 	struct irq_cfg *cfg;
+	int i, ret, devid;
+	int index = -1;
 
-	cfg = irq_cfg(irq);
-	if (!cfg)
+	if (!info)
+		return -EINVAL;
+	if (nr_irqs > 1 && info->type != X86_IRQ_ALLOC_TYPE_MSI &&
+	    info->type != X86_IRQ_ALLOC_TYPE_MSIX)
 		return -EINVAL;
 
-	irte_info = &cfg->irq_2_irte;
-
-	free_irte(irte_info->devid, irte_info->index);
+	/*
+	 * With IRQ remapping enabled, don't need contiguous CPU vectors
+	 * to support multiple MSI interrupts.
+	 */
+	if (info->type == X86_IRQ_ALLOC_TYPE_MSI)
+		info->flags &= ~X86_IRQ_ALLOC_CONTIGUOUS_VECTORS;
 
-	return 0;
-}
+	devid = get_devid(info);
+	if (devid < 0)
+		return -EINVAL;
 
-static void compose_msi_msg(struct pci_dev *pdev,
-			    unsigned int irq, unsigned int dest,
-			    struct msi_msg *msg, u8 hpet_id)
-{
-	struct irq_2_irte *irte_info;
-	struct irq_cfg *cfg;
-	union irte irte;
+	ret = irq_domain_alloc_irqs_parent(domain, virq, nr_irqs, arg);
+	if (ret < 0)
+		return ret;
 
-	cfg = irq_cfg(irq);
-	if (!cfg)
-		return;
+	ret = -ENOMEM;
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		goto out_free_parent;
 
-	irte_info = &cfg->irq_2_irte;
+	if (info->type == X86_IRQ_ALLOC_TYPE_IOAPIC) {
+		if (get_irq_table(devid, true))
+			index = info->ioapic_pin;
+		else
+			ret = -ENOMEM;
+	} else {
+		index = alloc_irq_index(devid, nr_irqs);
+	}
+	if (index < 0) {
+		pr_warn("Failed to allocate IRTE\n");
+		kfree(data);
+		goto out_free_parent;
+	}
 
-	irte.val		= 0;
-	irte.fields.vector	= cfg->vector;
-	irte.fields.int_type    = apic->irq_delivery_mode;
-	irte.fields.destination	= dest;
-	irte.fields.dm		= apic->irq_dest_mode;
-	irte.fields.valid	= 1;
+	for (i = 0; i < nr_irqs; i++) {
+		irq_data = irq_domain_get_irq_data(domain, virq + i);
+		cfg = irqd_cfg(irq_data);
+		if (!irq_data || !cfg) {
+			ret = -EINVAL;
+			goto out_free_data;
+		}
 
-	modify_irte(irte_info->devid, irte_info->index, irte);
+		if (i > 0) {
+			data = kzalloc(sizeof(*data), GFP_KERNEL);
+			if (!data)
+				goto out_free_data;
+		}
+		irq_data->hwirq = (devid << 16) + i;
+		irq_data->chip_data = data;
+		irq_data->chip = &amd_ir_chip;
+		irq_remapping_prepare_irte(data, cfg, info, devid, index, i);
+		irq_set_status_flags(virq + i, IRQ_MOVE_PCNTXT);
+	}
+	return 0;
 
-	msg->address_hi = MSI_ADDR_BASE_HI;
-	msg->address_lo = MSI_ADDR_BASE_LO;
-	msg->data       = irte_info->index;
+out_free_data:
+	for (i--; i >= 0; i--) {
+		irq_data = irq_domain_get_irq_data(domain, virq + i);
+		if (irq_data)
+			kfree(irq_data->chip_data);
+	}
+	for (i = 0; i < nr_irqs; i++)
+		free_irte(devid, index + i);
+out_free_parent:
+	irq_domain_free_irqs_common(domain, virq, nr_irqs);
+	return ret;
 }
 
-static int msi_alloc_irq(struct pci_dev *pdev, int irq, int nvec)
+static void irq_remapping_free(struct irq_domain *domain, unsigned int virq,
+			       unsigned int nr_irqs)
 {
-	struct irq_cfg *cfg;
-	int index;
-	u16 devid;
-
-	if (!pdev)
-		return -EINVAL;
+	struct irq_2_irte *irte_info;
+	struct irq_data *irq_data;
+	struct amd_ir_data *data;
+	int i;
 
-	cfg = irq_cfg(irq);
-	if (!cfg)
-		return -EINVAL;
+	for (i = 0; i < nr_irqs; i++) {
+		irq_data = irq_domain_get_irq_data(domain, virq  + i);
+		if (irq_data && irq_data->chip_data) {
+			data = irq_data->chip_data;
+			irte_info = &data->irq_2_irte;
+			free_irte(irte_info->devid, irte_info->index);
+			kfree(data);
+		}
+	}
+	irq_domain_free_irqs_common(domain, virq, nr_irqs);
+}
 
-	devid = get_device_id(&pdev->dev);
-	index = alloc_irq_index(cfg, devid, nvec);
+static void irq_remapping_activate(struct irq_domain *domain,
+				   struct irq_data *irq_data)
+{
+	struct amd_ir_data *data = irq_data->chip_data;
+	struct irq_2_irte *irte_info = &data->irq_2_irte;
 
-	return index < 0 ? MAX_IRQS_PER_TABLE : index;
+	modify_irte(irte_info->devid, irte_info->index, data->irte_entry);
 }
 
-static int msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
-			 int index, int offset)
+static void irq_remapping_deactivate(struct irq_domain *domain,
+				     struct irq_data *irq_data)
 {
-	struct irq_2_irte *irte_info;
-	struct irq_cfg *cfg;
-	u16 devid;
+	struct amd_ir_data *data = irq_data->chip_data;
+	struct irq_2_irte *irte_info = &data->irq_2_irte;
+	union irte entry;
 
-	if (!pdev)
-		return -EINVAL;
+	entry.val = 0;
+	modify_irte(irte_info->devid, irte_info->index, data->irte_entry);
+}
 
-	cfg = irq_cfg(irq);
-	if (!cfg)
-		return -EINVAL;
+static struct irq_domain_ops amd_ir_domain_ops = {
+	.alloc = irq_remapping_alloc,
+	.free = irq_remapping_free,
+	.activate = irq_remapping_activate,
+	.deactivate = irq_remapping_deactivate,
+};
 
-	if (index >= MAX_IRQS_PER_TABLE)
-		return 0;
+static int amd_ir_set_affinity(struct irq_data *data,
+			       const struct cpumask *mask, bool force)
+{
+	struct amd_ir_data *ir_data = data->chip_data;
+	struct irq_2_irte *irte_info = &ir_data->irq_2_irte;
+	struct irq_cfg *cfg = irqd_cfg(data);
+	struct irq_data *parent = data->parent_data;
+	int ret;
 
-	devid		= get_device_id(&pdev->dev);
-	irte_info	= &cfg->irq_2_irte;
+	ret = parent->chip->irq_set_affinity(parent, mask, force);
+	if (ret < 0 || ret == IRQ_SET_MASK_OK_DONE)
+		return ret;
 
-	cfg->remapped	      = 1;
-	irte_info->devid      = devid;
-	irte_info->index      = index + offset;
+	/*
+	 * Atomically updates the IRTE with the new destination, vector
+	 * and flushes the interrupt entry cache.
+	 */
+	ir_data->irte_entry.fields.vector = cfg->vector;
+	ir_data->irte_entry.fields.destination = cfg->dest_apicid;
+	modify_irte(irte_info->devid, irte_info->index, ir_data->irte_entry);
 
-	return 0;
+	/*
+	 * After this point, all the interrupts will start arriving
+	 * at the new destination. So, time to cleanup the previous
+	 * vector allocation.
+	 */
+	send_cleanup_vector(cfg);
+
+	return IRQ_SET_MASK_OK_DONE;
 }
 
-static int alloc_hpet_msi(unsigned int irq, unsigned int id)
+static void ir_compose_msi_msg(struct irq_data *irq_data, struct msi_msg *msg)
 {
-	struct irq_2_irte *irte_info;
-	struct irq_cfg *cfg;
-	int index, devid;
+	struct amd_ir_data *ir_data = irq_data->chip_data;
 
-	cfg = irq_cfg(irq);
-	if (!cfg)
-		return -EINVAL;
+	*msg = ir_data->msi_entry;
+}
 
-	irte_info = &cfg->irq_2_irte;
-	devid     = get_hpet_devid(id);
-	if (devid < 0)
-		return devid;
+static struct irq_chip amd_ir_chip = {
+	.irq_ack = ir_ack_apic_edge,
+	.irq_set_affinity = amd_ir_set_affinity,
+	.irq_compose_msi_msg = ir_compose_msi_msg,
+};
 
-	index = alloc_irq_index(cfg, devid, 1);
-	if (index < 0)
-		return index;
+int amd_iommu_create_irq_domain(struct amd_iommu *iommu)
+{
+	iommu->ir_domain = irq_domain_add_tree(NULL, &amd_ir_domain_ops, iommu);
+	if (!iommu->ir_domain)
+		return -ENOMEM;
 
-	cfg->remapped	      = 1;
-	irte_info->devid      = devid;
-	irte_info->index      = index;
+	iommu->ir_domain->parent = arch_get_ir_parent_domain();
+	iommu->msi_domain = arch_create_msi_irq_domain(iommu->ir_domain);
 
 	return 0;
 }
-
-struct irq_remap_ops amd_iommu_irq_ops = {
-	.prepare		= amd_iommu_prepare,
-	.enable			= amd_iommu_enable,
-	.disable		= amd_iommu_disable,
-	.reenable		= amd_iommu_reenable,
-	.enable_faulting	= amd_iommu_enable_faulting,
-	.setup_ioapic_entry	= setup_ioapic_entry,
-	.set_affinity		= set_affinity,
-	.free_irq		= free_irq,
-	.compose_msi_msg	= compose_msi_msg,
-	.msi_alloc_irq		= msi_alloc_irq,
-	.msi_setup_irq		= msi_setup_irq,
-	.alloc_hpet_msi		= alloc_hpet_msi,
-};
 #endif
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 450ef5001a65..dbda9ae68c5d 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -226,6 +226,7 @@ static enum iommu_init_state init_state = IOMMU_START_STATE;
 
 static int amd_iommu_enable_interrupts(void);
 static int __init iommu_go_to_state(enum iommu_init_state state);
+static void init_device_table_dma(void);
 
 static inline void update_last_devid(u16 devid)
 {
@@ -1124,6 +1125,10 @@ static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 	if (ret)
 		return ret;
 
+	ret = amd_iommu_create_irq_domain(iommu);
+	if (ret)
+		return ret;
+
 	/*
 	 * Make sure IOMMU is not considered to translate itself. The IVRS
 	 * table tells us so, but this is a lie!
@@ -1385,9 +1390,15 @@ static int __init amd_iommu_init_pci(void)
 			break;
 	}
 
-	ret = amd_iommu_init_devices();
+	init_device_table_dma();
+
+	for_each_iommu(iommu)
+		iommu_flush_all_caches(iommu);
 
-	print_iommu_info();
+	ret = amd_iommu_init_api();
+
+	if (!ret)
+		print_iommu_info();
 
 	return ret;
 }
@@ -1825,8 +1836,6 @@ static bool __init check_ioapic_information(void)
 
 static void __init free_dma_resources(void)
 {
-	amd_iommu_uninit_devices();
-
 	free_pages((unsigned long)amd_iommu_pd_alloc_bitmap,
 		   get_order(MAX_DOMAIN_ID/8));
 
@@ -2019,27 +2028,10 @@ static bool detect_ivrs(void)
 
 static int amd_iommu_init_dma(void)
 {
-	struct amd_iommu *iommu;
-	int ret;
-
 	if (iommu_pass_through)
-		ret = amd_iommu_init_passthrough();
+		return amd_iommu_init_passthrough();
 	else
-		ret = amd_iommu_init_dma_ops();
-
-	if (ret)
-		return ret;
-
-	init_device_table_dma();
-
-	for_each_iommu(iommu)
-		iommu_flush_all_caches(iommu);
-
-	amd_iommu_init_api();
-
-	amd_iommu_init_notifier();
-
-	return 0;
+		return amd_iommu_init_dma_ops();
 }
 
 /****************************************************************************
diff --git a/drivers/iommu/amd_iommu_proto.h b/drivers/iommu/amd_iommu_proto.h
index 72b0fd455e24..0bd9eb374462 100644
--- a/drivers/iommu/amd_iommu_proto.h
+++ b/drivers/iommu/amd_iommu_proto.h
@@ -30,7 +30,7 @@ extern void amd_iommu_reset_cmd_buffer(struct amd_iommu *iommu);
 extern int amd_iommu_init_devices(void);
 extern void amd_iommu_uninit_devices(void);
 extern void amd_iommu_init_notifier(void);
-extern void amd_iommu_init_api(void);
+extern int amd_iommu_init_api(void);
 
 /* Needed for interrupt remapping */
 extern int amd_iommu_prepare(void);
@@ -62,6 +62,15 @@ extern u8 amd_iommu_pc_get_max_counters(u16 devid);
 extern int amd_iommu_pc_get_set_reg_val(u16 devid, u8 bank, u8 cntr, u8 fxn,
 				    u64 *value, bool is_write);
 
+#ifdef CONFIG_IRQ_REMAP
+extern int amd_iommu_create_irq_domain(struct amd_iommu *iommu);
+#else
+static inline int amd_iommu_create_irq_domain(struct amd_iommu *iommu)
+{
+	return 0;
+}
+#endif
+
 #define PPR_SUCCESS			0x0
 #define PPR_INVALID			0x1
 #define PPR_FAILURE			0xf
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index 05030e523771..f65908841be0 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -398,6 +398,7 @@ struct amd_iommu_fault {
 
 
 struct iommu_domain;
+struct irq_domain;
 
 /*
  * This structure contains generic data for  IOMMU protection domains
@@ -446,8 +447,6 @@ struct aperture_range {
  * Data container for a dma_ops specific protection domain
  */
 struct dma_ops_domain {
-	struct list_head list;
-
 	/* generic protection domain information */
 	struct protection_domain domain;
 
@@ -462,12 +461,6 @@ struct dma_ops_domain {
 
 	/* This will be set to true when TLB needs to be flushed */
 	bool need_flush;
-
-	/*
-	 * if this is a preallocated domain, keep the device for which it was
-	 * preallocated in this variable
-	 */
-	u16 target_dev;
 };
 
 /*
@@ -552,9 +545,6 @@ struct amd_iommu {
 	/* if one, we need to send a completion wait command */
 	bool need_sync;
 
-	/* default dma_ops domain for that IOMMU */
-	struct dma_ops_domain *default_dom;
-
 	/* IOMMU sysfs device */
 	struct device *iommu_dev;
 
@@ -579,6 +569,10 @@ struct amd_iommu {
 	/* The maximum PC banks and counters/bank (PCSup=1) */
 	u8 max_banks;
 	u8 max_counters;
+#ifdef CONFIG_IRQ_REMAP
+	struct irq_domain *ir_domain;
+	struct irq_domain *msi_domain;
+#endif
 };
 
 struct devid_map {
diff --git a/drivers/iommu/arm-smmu-v3.c b/drivers/iommu/arm-smmu-v3.c
new file mode 100644
index 000000000000..8e9ec81ce4bb
--- /dev/null
+++ b/drivers/iommu/arm-smmu-v3.c
@@ -0,0 +1,2669 @@
+/*
+ * IOMMU API for ARM architected SMMUv3 implementations.
+ *
+ * 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/>.
+ *
+ * Copyright (C) 2015 ARM Limited
+ *
+ * Author: Will Deacon <will.deacon@arm.com>
+ *
+ * This driver is powered by bad coffee and bombay mix.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/iommu.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+
+#include "io-pgtable.h"
+
+/* MMIO registers */
+#define ARM_SMMU_IDR0			0x0
+#define IDR0_ST_LVL_SHIFT		27
+#define IDR0_ST_LVL_MASK		0x3
+#define IDR0_ST_LVL_2LVL		(1 << IDR0_ST_LVL_SHIFT)
+#define IDR0_STALL_MODEL		(3 << 24)
+#define IDR0_TTENDIAN_SHIFT		21
+#define IDR0_TTENDIAN_MASK		0x3
+#define IDR0_TTENDIAN_LE		(2 << IDR0_TTENDIAN_SHIFT)
+#define IDR0_TTENDIAN_BE		(3 << IDR0_TTENDIAN_SHIFT)
+#define IDR0_TTENDIAN_MIXED		(0 << IDR0_TTENDIAN_SHIFT)
+#define IDR0_CD2L			(1 << 19)
+#define IDR0_VMID16			(1 << 18)
+#define IDR0_PRI			(1 << 16)
+#define IDR0_SEV			(1 << 14)
+#define IDR0_MSI			(1 << 13)
+#define IDR0_ASID16			(1 << 12)
+#define IDR0_ATS			(1 << 10)
+#define IDR0_HYP			(1 << 9)
+#define IDR0_COHACC			(1 << 4)
+#define IDR0_TTF_SHIFT			2
+#define IDR0_TTF_MASK			0x3
+#define IDR0_TTF_AARCH64		(2 << IDR0_TTF_SHIFT)
+#define IDR0_S1P			(1 << 1)
+#define IDR0_S2P			(1 << 0)
+
+#define ARM_SMMU_IDR1			0x4
+#define IDR1_TABLES_PRESET		(1 << 30)
+#define IDR1_QUEUES_PRESET		(1 << 29)
+#define IDR1_REL			(1 << 28)
+#define IDR1_CMDQ_SHIFT			21
+#define IDR1_CMDQ_MASK			0x1f
+#define IDR1_EVTQ_SHIFT			16
+#define IDR1_EVTQ_MASK			0x1f
+#define IDR1_PRIQ_SHIFT			11
+#define IDR1_PRIQ_MASK			0x1f
+#define IDR1_SSID_SHIFT			6
+#define IDR1_SSID_MASK			0x1f
+#define IDR1_SID_SHIFT			0
+#define IDR1_SID_MASK			0x3f
+
+#define ARM_SMMU_IDR5			0x14
+#define IDR5_STALL_MAX_SHIFT		16
+#define IDR5_STALL_MAX_MASK		0xffff
+#define IDR5_GRAN64K			(1 << 6)
+#define IDR5_GRAN16K			(1 << 5)
+#define IDR5_GRAN4K			(1 << 4)
+#define IDR5_OAS_SHIFT			0
+#define IDR5_OAS_MASK			0x7
+#define IDR5_OAS_32_BIT			(0 << IDR5_OAS_SHIFT)
+#define IDR5_OAS_36_BIT			(1 << IDR5_OAS_SHIFT)
+#define IDR5_OAS_40_BIT			(2 << IDR5_OAS_SHIFT)
+#define IDR5_OAS_42_BIT			(3 << IDR5_OAS_SHIFT)
+#define IDR5_OAS_44_BIT			(4 << IDR5_OAS_SHIFT)
+#define IDR5_OAS_48_BIT			(5 << IDR5_OAS_SHIFT)
+
+#define ARM_SMMU_CR0			0x20
+#define CR0_CMDQEN			(1 << 3)
+#define CR0_EVTQEN			(1 << 2)
+#define CR0_PRIQEN			(1 << 1)
+#define CR0_SMMUEN			(1 << 0)
+
+#define ARM_SMMU_CR0ACK			0x24
+
+#define ARM_SMMU_CR1			0x28
+#define CR1_SH_NSH			0
+#define CR1_SH_OSH			2
+#define CR1_SH_ISH			3
+#define CR1_CACHE_NC			0
+#define CR1_CACHE_WB			1
+#define CR1_CACHE_WT			2
+#define CR1_TABLE_SH_SHIFT		10
+#define CR1_TABLE_OC_SHIFT		8
+#define CR1_TABLE_IC_SHIFT		6
+#define CR1_QUEUE_SH_SHIFT		4
+#define CR1_QUEUE_OC_SHIFT		2
+#define CR1_QUEUE_IC_SHIFT		0
+
+#define ARM_SMMU_CR2			0x2c
+#define CR2_PTM				(1 << 2)
+#define CR2_RECINVSID			(1 << 1)
+#define CR2_E2H				(1 << 0)
+
+#define ARM_SMMU_IRQ_CTRL		0x50
+#define IRQ_CTRL_EVTQ_IRQEN		(1 << 2)
+#define IRQ_CTRL_GERROR_IRQEN		(1 << 0)
+
+#define ARM_SMMU_IRQ_CTRLACK		0x54
+
+#define ARM_SMMU_GERROR			0x60
+#define GERROR_SFM_ERR			(1 << 8)
+#define GERROR_MSI_GERROR_ABT_ERR	(1 << 7)
+#define GERROR_MSI_PRIQ_ABT_ERR		(1 << 6)
+#define GERROR_MSI_EVTQ_ABT_ERR		(1 << 5)
+#define GERROR_MSI_CMDQ_ABT_ERR		(1 << 4)
+#define GERROR_PRIQ_ABT_ERR		(1 << 3)
+#define GERROR_EVTQ_ABT_ERR		(1 << 2)
+#define GERROR_CMDQ_ERR			(1 << 0)
+#define GERROR_ERR_MASK			0xfd
+
+#define ARM_SMMU_GERRORN		0x64
+
+#define ARM_SMMU_GERROR_IRQ_CFG0	0x68
+#define ARM_SMMU_GERROR_IRQ_CFG1	0x70
+#define ARM_SMMU_GERROR_IRQ_CFG2	0x74
+
+#define ARM_SMMU_STRTAB_BASE		0x80
+#define STRTAB_BASE_RA			(1UL << 62)
+#define STRTAB_BASE_ADDR_SHIFT		6
+#define STRTAB_BASE_ADDR_MASK		0x3ffffffffffUL
+
+#define ARM_SMMU_STRTAB_BASE_CFG	0x88
+#define STRTAB_BASE_CFG_LOG2SIZE_SHIFT	0
+#define STRTAB_BASE_CFG_LOG2SIZE_MASK	0x3f
+#define STRTAB_BASE_CFG_SPLIT_SHIFT	6
+#define STRTAB_BASE_CFG_SPLIT_MASK	0x1f
+#define STRTAB_BASE_CFG_FMT_SHIFT	16
+#define STRTAB_BASE_CFG_FMT_MASK	0x3
+#define STRTAB_BASE_CFG_FMT_LINEAR	(0 << STRTAB_BASE_CFG_FMT_SHIFT)
+#define STRTAB_BASE_CFG_FMT_2LVL	(1 << STRTAB_BASE_CFG_FMT_SHIFT)
+
+#define ARM_SMMU_CMDQ_BASE		0x90
+#define ARM_SMMU_CMDQ_PROD		0x98
+#define ARM_SMMU_CMDQ_CONS		0x9c
+
+#define ARM_SMMU_EVTQ_BASE		0xa0
+#define ARM_SMMU_EVTQ_PROD		0x100a8
+#define ARM_SMMU_EVTQ_CONS		0x100ac
+#define ARM_SMMU_EVTQ_IRQ_CFG0		0xb0
+#define ARM_SMMU_EVTQ_IRQ_CFG1		0xb8
+#define ARM_SMMU_EVTQ_IRQ_CFG2		0xbc
+
+#define ARM_SMMU_PRIQ_BASE		0xc0
+#define ARM_SMMU_PRIQ_PROD		0x100c8
+#define ARM_SMMU_PRIQ_CONS		0x100cc
+#define ARM_SMMU_PRIQ_IRQ_CFG0		0xd0
+#define ARM_SMMU_PRIQ_IRQ_CFG1		0xd8
+#define ARM_SMMU_PRIQ_IRQ_CFG2		0xdc
+
+/* Common MSI config fields */
+#define MSI_CFG0_SH_SHIFT		60
+#define MSI_CFG0_SH_NSH			(0UL << MSI_CFG0_SH_SHIFT)
+#define MSI_CFG0_SH_OSH			(2UL << MSI_CFG0_SH_SHIFT)
+#define MSI_CFG0_SH_ISH			(3UL << MSI_CFG0_SH_SHIFT)
+#define MSI_CFG0_MEMATTR_SHIFT		56
+#define MSI_CFG0_MEMATTR_DEVICE_nGnRE	(0x1 << MSI_CFG0_MEMATTR_SHIFT)
+#define MSI_CFG0_ADDR_SHIFT		2
+#define MSI_CFG0_ADDR_MASK		0x3fffffffffffUL
+
+#define Q_IDX(q, p)			((p) & ((1 << (q)->max_n_shift) - 1))
+#define Q_WRP(q, p)			((p) & (1 << (q)->max_n_shift))
+#define Q_OVERFLOW_FLAG			(1 << 31)
+#define Q_OVF(q, p)			((p) & Q_OVERFLOW_FLAG)
+#define Q_ENT(q, p)			((q)->base +			\
+					 Q_IDX(q, p) * (q)->ent_dwords)
+
+#define Q_BASE_RWA			(1UL << 62)
+#define Q_BASE_ADDR_SHIFT		5
+#define Q_BASE_ADDR_MASK		0xfffffffffffUL
+#define Q_BASE_LOG2SIZE_SHIFT		0
+#define Q_BASE_LOG2SIZE_MASK		0x1fUL
+
+/*
+ * Stream table.
+ *
+ * Linear: Enough to cover 1 << IDR1.SIDSIZE entries
+ * 2lvl: 8k L1 entries, 256 lazy entries per table (each table covers a PCI bus)
+ */
+#define STRTAB_L1_SZ_SHIFT		16
+#define STRTAB_SPLIT			8
+
+#define STRTAB_L1_DESC_DWORDS		1
+#define STRTAB_L1_DESC_SPAN_SHIFT	0
+#define STRTAB_L1_DESC_SPAN_MASK	0x1fUL
+#define STRTAB_L1_DESC_L2PTR_SHIFT	6
+#define STRTAB_L1_DESC_L2PTR_MASK	0x3ffffffffffUL
+
+#define STRTAB_STE_DWORDS		8
+#define STRTAB_STE_0_V			(1UL << 0)
+#define STRTAB_STE_0_CFG_SHIFT		1
+#define STRTAB_STE_0_CFG_MASK		0x7UL
+#define STRTAB_STE_0_CFG_ABORT		(0UL << STRTAB_STE_0_CFG_SHIFT)
+#define STRTAB_STE_0_CFG_BYPASS		(4UL << STRTAB_STE_0_CFG_SHIFT)
+#define STRTAB_STE_0_CFG_S1_TRANS	(5UL << STRTAB_STE_0_CFG_SHIFT)
+#define STRTAB_STE_0_CFG_S2_TRANS	(6UL << STRTAB_STE_0_CFG_SHIFT)
+
+#define STRTAB_STE_0_S1FMT_SHIFT	4
+#define STRTAB_STE_0_S1FMT_LINEAR	(0UL << STRTAB_STE_0_S1FMT_SHIFT)
+#define STRTAB_STE_0_S1CTXPTR_SHIFT	6
+#define STRTAB_STE_0_S1CTXPTR_MASK	0x3ffffffffffUL
+#define STRTAB_STE_0_S1CDMAX_SHIFT	59
+#define STRTAB_STE_0_S1CDMAX_MASK	0x1fUL
+
+#define STRTAB_STE_1_S1C_CACHE_NC	0UL
+#define STRTAB_STE_1_S1C_CACHE_WBRA	1UL
+#define STRTAB_STE_1_S1C_CACHE_WT	2UL
+#define STRTAB_STE_1_S1C_CACHE_WB	3UL
+#define STRTAB_STE_1_S1C_SH_NSH		0UL
+#define STRTAB_STE_1_S1C_SH_OSH		2UL
+#define STRTAB_STE_1_S1C_SH_ISH		3UL
+#define STRTAB_STE_1_S1CIR_SHIFT	2
+#define STRTAB_STE_1_S1COR_SHIFT	4
+#define STRTAB_STE_1_S1CSH_SHIFT	6
+
+#define STRTAB_STE_1_S1STALLD		(1UL << 27)
+
+#define STRTAB_STE_1_EATS_ABT		0UL
+#define STRTAB_STE_1_EATS_TRANS		1UL
+#define STRTAB_STE_1_EATS_S1CHK		2UL
+#define STRTAB_STE_1_EATS_SHIFT		28
+
+#define STRTAB_STE_1_STRW_NSEL1		0UL
+#define STRTAB_STE_1_STRW_EL2		2UL
+#define STRTAB_STE_1_STRW_SHIFT		30
+
+#define STRTAB_STE_2_S2VMID_SHIFT	0
+#define STRTAB_STE_2_S2VMID_MASK	0xffffUL
+#define STRTAB_STE_2_VTCR_SHIFT		32
+#define STRTAB_STE_2_VTCR_MASK		0x7ffffUL
+#define STRTAB_STE_2_S2AA64		(1UL << 51)
+#define STRTAB_STE_2_S2ENDI		(1UL << 52)
+#define STRTAB_STE_2_S2PTW		(1UL << 54)
+#define STRTAB_STE_2_S2R		(1UL << 58)
+
+#define STRTAB_STE_3_S2TTB_SHIFT	4
+#define STRTAB_STE_3_S2TTB_MASK		0xfffffffffffUL
+
+/* Context descriptor (stage-1 only) */
+#define CTXDESC_CD_DWORDS		8
+#define CTXDESC_CD_0_TCR_T0SZ_SHIFT	0
+#define ARM64_TCR_T0SZ_SHIFT		0
+#define ARM64_TCR_T0SZ_MASK		0x1fUL
+#define CTXDESC_CD_0_TCR_TG0_SHIFT	6
+#define ARM64_TCR_TG0_SHIFT		14
+#define ARM64_TCR_TG0_MASK		0x3UL
+#define CTXDESC_CD_0_TCR_IRGN0_SHIFT	8
+#define ARM64_TCR_IRGN0_SHIFT		24
+#define ARM64_TCR_IRGN0_MASK		0x3UL
+#define CTXDESC_CD_0_TCR_ORGN0_SHIFT	10
+#define ARM64_TCR_ORGN0_SHIFT		26
+#define ARM64_TCR_ORGN0_MASK		0x3UL
+#define CTXDESC_CD_0_TCR_SH0_SHIFT	12
+#define ARM64_TCR_SH0_SHIFT		12
+#define ARM64_TCR_SH0_MASK		0x3UL
+#define CTXDESC_CD_0_TCR_EPD0_SHIFT	14
+#define ARM64_TCR_EPD0_SHIFT		7
+#define ARM64_TCR_EPD0_MASK		0x1UL
+#define CTXDESC_CD_0_TCR_EPD1_SHIFT	30
+#define ARM64_TCR_EPD1_SHIFT		23
+#define ARM64_TCR_EPD1_MASK		0x1UL
+
+#define CTXDESC_CD_0_ENDI		(1UL << 15)
+#define CTXDESC_CD_0_V			(1UL << 31)
+
+#define CTXDESC_CD_0_TCR_IPS_SHIFT	32
+#define ARM64_TCR_IPS_SHIFT		32
+#define ARM64_TCR_IPS_MASK		0x7UL
+#define CTXDESC_CD_0_TCR_TBI0_SHIFT	38
+#define ARM64_TCR_TBI0_SHIFT		37
+#define ARM64_TCR_TBI0_MASK		0x1UL
+
+#define CTXDESC_CD_0_AA64		(1UL << 41)
+#define CTXDESC_CD_0_R			(1UL << 45)
+#define CTXDESC_CD_0_A			(1UL << 46)
+#define CTXDESC_CD_0_ASET_SHIFT		47
+#define CTXDESC_CD_0_ASET_SHARED	(0UL << CTXDESC_CD_0_ASET_SHIFT)
+#define CTXDESC_CD_0_ASET_PRIVATE	(1UL << CTXDESC_CD_0_ASET_SHIFT)
+#define CTXDESC_CD_0_ASID_SHIFT		48
+#define CTXDESC_CD_0_ASID_MASK		0xffffUL
+
+#define CTXDESC_CD_1_TTB0_SHIFT		4
+#define CTXDESC_CD_1_TTB0_MASK		0xfffffffffffUL
+
+#define CTXDESC_CD_3_MAIR_SHIFT		0
+
+/* Convert between AArch64 (CPU) TCR format and SMMU CD format */
+#define ARM_SMMU_TCR2CD(tcr, fld)					\
+	(((tcr) >> ARM64_TCR_##fld##_SHIFT & ARM64_TCR_##fld##_MASK)	\
+	 << CTXDESC_CD_0_TCR_##fld##_SHIFT)
+
+/* Command queue */
+#define CMDQ_ENT_DWORDS			2
+#define CMDQ_MAX_SZ_SHIFT		8
+
+#define CMDQ_ERR_SHIFT			24
+#define CMDQ_ERR_MASK			0x7f
+#define CMDQ_ERR_CERROR_NONE_IDX	0
+#define CMDQ_ERR_CERROR_ILL_IDX		1
+#define CMDQ_ERR_CERROR_ABT_IDX		2
+
+#define CMDQ_0_OP_SHIFT			0
+#define CMDQ_0_OP_MASK			0xffUL
+#define CMDQ_0_SSV			(1UL << 11)
+
+#define CMDQ_PREFETCH_0_SID_SHIFT	32
+#define CMDQ_PREFETCH_1_SIZE_SHIFT	0
+#define CMDQ_PREFETCH_1_ADDR_MASK	~0xfffUL
+
+#define CMDQ_CFGI_0_SID_SHIFT		32
+#define CMDQ_CFGI_0_SID_MASK		0xffffffffUL
+#define CMDQ_CFGI_1_LEAF		(1UL << 0)
+#define CMDQ_CFGI_1_RANGE_SHIFT		0
+#define CMDQ_CFGI_1_RANGE_MASK		0x1fUL
+
+#define CMDQ_TLBI_0_VMID_SHIFT		32
+#define CMDQ_TLBI_0_ASID_SHIFT		48
+#define CMDQ_TLBI_1_LEAF		(1UL << 0)
+#define CMDQ_TLBI_1_ADDR_MASK		~0xfffUL
+
+#define CMDQ_PRI_0_SSID_SHIFT		12
+#define CMDQ_PRI_0_SSID_MASK		0xfffffUL
+#define CMDQ_PRI_0_SID_SHIFT		32
+#define CMDQ_PRI_0_SID_MASK		0xffffffffUL
+#define CMDQ_PRI_1_GRPID_SHIFT		0
+#define CMDQ_PRI_1_GRPID_MASK		0x1ffUL
+#define CMDQ_PRI_1_RESP_SHIFT		12
+#define CMDQ_PRI_1_RESP_DENY		(0UL << CMDQ_PRI_1_RESP_SHIFT)
+#define CMDQ_PRI_1_RESP_FAIL		(1UL << CMDQ_PRI_1_RESP_SHIFT)
+#define CMDQ_PRI_1_RESP_SUCC		(2UL << CMDQ_PRI_1_RESP_SHIFT)
+
+#define CMDQ_SYNC_0_CS_SHIFT		12
+#define CMDQ_SYNC_0_CS_NONE		(0UL << CMDQ_SYNC_0_CS_SHIFT)
+#define CMDQ_SYNC_0_CS_SEV		(2UL << CMDQ_SYNC_0_CS_SHIFT)
+
+/* Event queue */
+#define EVTQ_ENT_DWORDS			4
+#define EVTQ_MAX_SZ_SHIFT		7
+
+#define EVTQ_0_ID_SHIFT			0
+#define EVTQ_0_ID_MASK			0xffUL
+
+/* PRI queue */
+#define PRIQ_ENT_DWORDS			2
+#define PRIQ_MAX_SZ_SHIFT		8
+
+#define PRIQ_0_SID_SHIFT		0
+#define PRIQ_0_SID_MASK			0xffffffffUL
+#define PRIQ_0_SSID_SHIFT		32
+#define PRIQ_0_SSID_MASK		0xfffffUL
+#define PRIQ_0_OF			(1UL << 57)
+#define PRIQ_0_PERM_PRIV		(1UL << 58)
+#define PRIQ_0_PERM_EXEC		(1UL << 59)
+#define PRIQ_0_PERM_READ		(1UL << 60)
+#define PRIQ_0_PERM_WRITE		(1UL << 61)
+#define PRIQ_0_PRG_LAST			(1UL << 62)
+#define PRIQ_0_SSID_V			(1UL << 63)
+
+#define PRIQ_1_PRG_IDX_SHIFT		0
+#define PRIQ_1_PRG_IDX_MASK		0x1ffUL
+#define PRIQ_1_ADDR_SHIFT		12
+#define PRIQ_1_ADDR_MASK		0xfffffffffffffUL
+
+/* High-level queue structures */
+#define ARM_SMMU_POLL_TIMEOUT_US	100
+
+static bool disable_bypass;
+module_param_named(disable_bypass, disable_bypass, bool, S_IRUGO);
+MODULE_PARM_DESC(disable_bypass,
+	"Disable bypass streams such that incoming transactions from devices that are not attached to an iommu domain will report an abort back to the device and will not be allowed to pass through the SMMU.");
+
+enum pri_resp {
+	PRI_RESP_DENY,
+	PRI_RESP_FAIL,
+	PRI_RESP_SUCC,
+};
+
+struct arm_smmu_cmdq_ent {
+	/* Common fields */
+	u8				opcode;
+	bool				substream_valid;
+
+	/* Command-specific fields */
+	union {
+		#define CMDQ_OP_PREFETCH_CFG	0x1
+		struct {
+			u32			sid;
+			u8			size;
+			u64			addr;
+		} prefetch;
+
+		#define CMDQ_OP_CFGI_STE	0x3
+		#define CMDQ_OP_CFGI_ALL	0x4
+		struct {
+			u32			sid;
+			union {
+				bool		leaf;
+				u8		span;
+			};
+		} cfgi;
+
+		#define CMDQ_OP_TLBI_NH_ASID	0x11
+		#define CMDQ_OP_TLBI_NH_VA	0x12
+		#define CMDQ_OP_TLBI_EL2_ALL	0x20
+		#define CMDQ_OP_TLBI_S12_VMALL	0x28
+		#define CMDQ_OP_TLBI_S2_IPA	0x2a
+		#define CMDQ_OP_TLBI_NSNH_ALL	0x30
+		struct {
+			u16			asid;
+			u16			vmid;
+			bool			leaf;
+			u64			addr;
+		} tlbi;
+
+		#define CMDQ_OP_PRI_RESP	0x41
+		struct {
+			u32			sid;
+			u32			ssid;
+			u16			grpid;
+			enum pri_resp		resp;
+		} pri;
+
+		#define CMDQ_OP_CMD_SYNC	0x46
+	};
+};
+
+struct arm_smmu_queue {
+	int				irq; /* Wired interrupt */
+
+	__le64				*base;
+	dma_addr_t			base_dma;
+	u64				q_base;
+
+	size_t				ent_dwords;
+	u32				max_n_shift;
+	u32				prod;
+	u32				cons;
+
+	u32 __iomem			*prod_reg;
+	u32 __iomem			*cons_reg;
+};
+
+struct arm_smmu_cmdq {
+	struct arm_smmu_queue		q;
+	spinlock_t			lock;
+};
+
+struct arm_smmu_evtq {
+	struct arm_smmu_queue		q;
+	u32				max_stalls;
+};
+
+struct arm_smmu_priq {
+	struct arm_smmu_queue		q;
+};
+
+/* High-level stream table and context descriptor structures */
+struct arm_smmu_strtab_l1_desc {
+	u8				span;
+
+	__le64				*l2ptr;
+	dma_addr_t			l2ptr_dma;
+};
+
+struct arm_smmu_s1_cfg {
+	__le64				*cdptr;
+	dma_addr_t			cdptr_dma;
+
+	struct arm_smmu_ctx_desc {
+		u16	asid;
+		u64	ttbr;
+		u64	tcr;
+		u64	mair;
+	}				cd;
+};
+
+struct arm_smmu_s2_cfg {
+	u16				vmid;
+	u64				vttbr;
+	u64				vtcr;
+};
+
+struct arm_smmu_strtab_ent {
+	bool				valid;
+
+	bool				bypass;	/* Overrides s1/s2 config */
+	struct arm_smmu_s1_cfg		*s1_cfg;
+	struct arm_smmu_s2_cfg		*s2_cfg;
+};
+
+struct arm_smmu_strtab_cfg {
+	__le64				*strtab;
+	dma_addr_t			strtab_dma;
+	struct arm_smmu_strtab_l1_desc	*l1_desc;
+	unsigned int			num_l1_ents;
+
+	u64				strtab_base;
+	u32				strtab_base_cfg;
+};
+
+/* An SMMUv3 instance */
+struct arm_smmu_device {
+	struct device			*dev;
+	void __iomem			*base;
+
+#define ARM_SMMU_FEAT_2_LVL_STRTAB	(1 << 0)
+#define ARM_SMMU_FEAT_2_LVL_CDTAB	(1 << 1)
+#define ARM_SMMU_FEAT_TT_LE		(1 << 2)
+#define ARM_SMMU_FEAT_TT_BE		(1 << 3)
+#define ARM_SMMU_FEAT_PRI		(1 << 4)
+#define ARM_SMMU_FEAT_ATS		(1 << 5)
+#define ARM_SMMU_FEAT_SEV		(1 << 6)
+#define ARM_SMMU_FEAT_MSI		(1 << 7)
+#define ARM_SMMU_FEAT_COHERENCY		(1 << 8)
+#define ARM_SMMU_FEAT_TRANS_S1		(1 << 9)
+#define ARM_SMMU_FEAT_TRANS_S2		(1 << 10)
+#define ARM_SMMU_FEAT_STALLS		(1 << 11)
+#define ARM_SMMU_FEAT_HYP		(1 << 12)
+	u32				features;
+
+	struct arm_smmu_cmdq		cmdq;
+	struct arm_smmu_evtq		evtq;
+	struct arm_smmu_priq		priq;
+
+	int				gerr_irq;
+
+	unsigned long			ias; /* IPA */
+	unsigned long			oas; /* PA */
+
+#define ARM_SMMU_MAX_ASIDS		(1 << 16)
+	unsigned int			asid_bits;
+	DECLARE_BITMAP(asid_map, ARM_SMMU_MAX_ASIDS);
+
+#define ARM_SMMU_MAX_VMIDS		(1 << 16)
+	unsigned int			vmid_bits;
+	DECLARE_BITMAP(vmid_map, ARM_SMMU_MAX_VMIDS);
+
+	unsigned int			ssid_bits;
+	unsigned int			sid_bits;
+
+	struct arm_smmu_strtab_cfg	strtab_cfg;
+	struct list_head		list;
+};
+
+/* SMMU private data for an IOMMU group */
+struct arm_smmu_group {
+	struct arm_smmu_device		*smmu;
+	struct arm_smmu_domain		*domain;
+	int				num_sids;
+	u32				*sids;
+	struct arm_smmu_strtab_ent	ste;
+};
+
+/* SMMU private data for an IOMMU domain */
+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 mutex			init_mutex; /* Protects smmu pointer */
+
+	struct io_pgtable_ops		*pgtbl_ops;
+	spinlock_t			pgtbl_lock;
+
+	enum arm_smmu_domain_stage	stage;
+	union {
+		struct arm_smmu_s1_cfg	s1_cfg;
+		struct arm_smmu_s2_cfg	s2_cfg;
+	};
+
+	struct iommu_domain		domain;
+};
+
+/* Our list of SMMU instances */
+static DEFINE_SPINLOCK(arm_smmu_devices_lock);
+static LIST_HEAD(arm_smmu_devices);
+
+static struct arm_smmu_domain *to_smmu_domain(struct iommu_domain *dom)
+{
+	return container_of(dom, struct arm_smmu_domain, domain);
+}
+
+/* Low-level queue manipulation functions */
+static bool queue_full(struct arm_smmu_queue *q)
+{
+	return Q_IDX(q, q->prod) == Q_IDX(q, q->cons) &&
+	       Q_WRP(q, q->prod) != Q_WRP(q, q->cons);
+}
+
+static bool queue_empty(struct arm_smmu_queue *q)
+{
+	return Q_IDX(q, q->prod) == Q_IDX(q, q->cons) &&
+	       Q_WRP(q, q->prod) == Q_WRP(q, q->cons);
+}
+
+static void queue_sync_cons(struct arm_smmu_queue *q)
+{
+	q->cons = readl_relaxed(q->cons_reg);
+}
+
+static void queue_inc_cons(struct arm_smmu_queue *q)
+{
+	u32 cons = (Q_WRP(q, q->cons) | Q_IDX(q, q->cons)) + 1;
+
+	q->cons = Q_OVF(q, q->cons) | Q_WRP(q, cons) | Q_IDX(q, cons);
+	writel(q->cons, q->cons_reg);
+}
+
+static int queue_sync_prod(struct arm_smmu_queue *q)
+{
+	int ret = 0;
+	u32 prod = readl_relaxed(q->prod_reg);
+
+	if (Q_OVF(q, prod) != Q_OVF(q, q->prod))
+		ret = -EOVERFLOW;
+
+	q->prod = prod;
+	return ret;
+}
+
+static void queue_inc_prod(struct arm_smmu_queue *q)
+{
+	u32 prod = (Q_WRP(q, q->prod) | Q_IDX(q, q->prod)) + 1;
+
+	q->prod = Q_OVF(q, q->prod) | Q_WRP(q, prod) | Q_IDX(q, prod);
+	writel(q->prod, q->prod_reg);
+}
+
+static bool __queue_cons_before(struct arm_smmu_queue *q, u32 until)
+{
+	if (Q_WRP(q, q->cons) == Q_WRP(q, until))
+		return Q_IDX(q, q->cons) < Q_IDX(q, until);
+
+	return Q_IDX(q, q->cons) >= Q_IDX(q, until);
+}
+
+static int queue_poll_cons(struct arm_smmu_queue *q, u32 until, bool wfe)
+{
+	ktime_t timeout = ktime_add_us(ktime_get(), ARM_SMMU_POLL_TIMEOUT_US);
+
+	while (queue_sync_cons(q), __queue_cons_before(q, until)) {
+		if (ktime_compare(ktime_get(), timeout) > 0)
+			return -ETIMEDOUT;
+
+		if (wfe) {
+			wfe();
+		} else {
+			cpu_relax();
+			udelay(1);
+		}
+	}
+
+	return 0;
+}
+
+static void queue_write(__le64 *dst, u64 *src, size_t n_dwords)
+{
+	int i;
+
+	for (i = 0; i < n_dwords; ++i)
+		*dst++ = cpu_to_le64(*src++);
+}
+
+static int queue_insert_raw(struct arm_smmu_queue *q, u64 *ent)
+{
+	if (queue_full(q))
+		return -ENOSPC;
+
+	queue_write(Q_ENT(q, q->prod), ent, q->ent_dwords);
+	queue_inc_prod(q);
+	return 0;
+}
+
+static void queue_read(__le64 *dst, u64 *src, size_t n_dwords)
+{
+	int i;
+
+	for (i = 0; i < n_dwords; ++i)
+		*dst++ = le64_to_cpu(*src++);
+}
+
+static int queue_remove_raw(struct arm_smmu_queue *q, u64 *ent)
+{
+	if (queue_empty(q))
+		return -EAGAIN;
+
+	queue_read(ent, Q_ENT(q, q->cons), q->ent_dwords);
+	queue_inc_cons(q);
+	return 0;
+}
+
+/* High-level queue accessors */
+static int arm_smmu_cmdq_build_cmd(u64 *cmd, struct arm_smmu_cmdq_ent *ent)
+{
+	memset(cmd, 0, CMDQ_ENT_DWORDS << 3);
+	cmd[0] |= (ent->opcode & CMDQ_0_OP_MASK) << CMDQ_0_OP_SHIFT;
+
+	switch (ent->opcode) {
+	case CMDQ_OP_TLBI_EL2_ALL:
+	case CMDQ_OP_TLBI_NSNH_ALL:
+		break;
+	case CMDQ_OP_PREFETCH_CFG:
+		cmd[0] |= (u64)ent->prefetch.sid << CMDQ_PREFETCH_0_SID_SHIFT;
+		cmd[1] |= ent->prefetch.size << CMDQ_PREFETCH_1_SIZE_SHIFT;
+		cmd[1] |= ent->prefetch.addr & CMDQ_PREFETCH_1_ADDR_MASK;
+		break;
+	case CMDQ_OP_CFGI_STE:
+		cmd[0] |= (u64)ent->cfgi.sid << CMDQ_CFGI_0_SID_SHIFT;
+		cmd[1] |= ent->cfgi.leaf ? CMDQ_CFGI_1_LEAF : 0;
+		break;
+	case CMDQ_OP_CFGI_ALL:
+		/* Cover the entire SID range */
+		cmd[1] |= CMDQ_CFGI_1_RANGE_MASK << CMDQ_CFGI_1_RANGE_SHIFT;
+		break;
+	case CMDQ_OP_TLBI_NH_VA:
+		cmd[0] |= (u64)ent->tlbi.asid << CMDQ_TLBI_0_ASID_SHIFT;
+		/* Fallthrough */
+	case CMDQ_OP_TLBI_S2_IPA:
+		cmd[0] |= (u64)ent->tlbi.vmid << CMDQ_TLBI_0_VMID_SHIFT;
+		cmd[1] |= ent->tlbi.leaf ? CMDQ_TLBI_1_LEAF : 0;
+		cmd[1] |= ent->tlbi.addr & CMDQ_TLBI_1_ADDR_MASK;
+		break;
+	case CMDQ_OP_TLBI_NH_ASID:
+		cmd[0] |= (u64)ent->tlbi.asid << CMDQ_TLBI_0_ASID_SHIFT;
+		/* Fallthrough */
+	case CMDQ_OP_TLBI_S12_VMALL:
+		cmd[0] |= (u64)ent->tlbi.vmid << CMDQ_TLBI_0_VMID_SHIFT;
+		break;
+	case CMDQ_OP_PRI_RESP:
+		cmd[0] |= ent->substream_valid ? CMDQ_0_SSV : 0;
+		cmd[0] |= ent->pri.ssid << CMDQ_PRI_0_SSID_SHIFT;
+		cmd[0] |= (u64)ent->pri.sid << CMDQ_PRI_0_SID_SHIFT;
+		cmd[1] |= ent->pri.grpid << CMDQ_PRI_1_GRPID_SHIFT;
+		switch (ent->pri.resp) {
+		case PRI_RESP_DENY:
+			cmd[1] |= CMDQ_PRI_1_RESP_DENY;
+			break;
+		case PRI_RESP_FAIL:
+			cmd[1] |= CMDQ_PRI_1_RESP_FAIL;
+			break;
+		case PRI_RESP_SUCC:
+			cmd[1] |= CMDQ_PRI_1_RESP_SUCC;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case CMDQ_OP_CMD_SYNC:
+		cmd[0] |= CMDQ_SYNC_0_CS_SEV;
+		break;
+	default:
+		return -ENOENT;
+	}
+
+	return 0;
+}
+
+static void arm_smmu_cmdq_skip_err(struct arm_smmu_device *smmu)
+{
+	static const char *cerror_str[] = {
+		[CMDQ_ERR_CERROR_NONE_IDX]	= "No error",
+		[CMDQ_ERR_CERROR_ILL_IDX]	= "Illegal command",
+		[CMDQ_ERR_CERROR_ABT_IDX]	= "Abort on command fetch",
+	};
+
+	int i;
+	u64 cmd[CMDQ_ENT_DWORDS];
+	struct arm_smmu_queue *q = &smmu->cmdq.q;
+	u32 cons = readl_relaxed(q->cons_reg);
+	u32 idx = cons >> CMDQ_ERR_SHIFT & CMDQ_ERR_MASK;
+	struct arm_smmu_cmdq_ent cmd_sync = {
+		.opcode = CMDQ_OP_CMD_SYNC,
+	};
+
+	dev_err(smmu->dev, "CMDQ error (cons 0x%08x): %s\n", cons,
+		cerror_str[idx]);
+
+	switch (idx) {
+	case CMDQ_ERR_CERROR_ILL_IDX:
+		break;
+	case CMDQ_ERR_CERROR_ABT_IDX:
+		dev_err(smmu->dev, "retrying command fetch\n");
+	case CMDQ_ERR_CERROR_NONE_IDX:
+		return;
+	}
+
+	/*
+	 * We may have concurrent producers, so we need to be careful
+	 * not to touch any of the shadow cmdq state.
+	 */
+	queue_read(cmd, Q_ENT(q, idx), q->ent_dwords);
+	dev_err(smmu->dev, "skipping command in error state:\n");
+	for (i = 0; i < ARRAY_SIZE(cmd); ++i)
+		dev_err(smmu->dev, "\t0x%016llx\n", (unsigned long long)cmd[i]);
+
+	/* Convert the erroneous command into a CMD_SYNC */
+	if (arm_smmu_cmdq_build_cmd(cmd, &cmd_sync)) {
+		dev_err(smmu->dev, "failed to convert to CMD_SYNC\n");
+		return;
+	}
+
+	queue_write(cmd, Q_ENT(q, idx), q->ent_dwords);
+}
+
+static void arm_smmu_cmdq_issue_cmd(struct arm_smmu_device *smmu,
+				    struct arm_smmu_cmdq_ent *ent)
+{
+	u32 until;
+	u64 cmd[CMDQ_ENT_DWORDS];
+	bool wfe = !!(smmu->features & ARM_SMMU_FEAT_SEV);
+	struct arm_smmu_queue *q = &smmu->cmdq.q;
+
+	if (arm_smmu_cmdq_build_cmd(cmd, ent)) {
+		dev_warn(smmu->dev, "ignoring unknown CMDQ opcode 0x%x\n",
+			 ent->opcode);
+		return;
+	}
+
+	spin_lock(&smmu->cmdq.lock);
+	while (until = q->prod + 1, queue_insert_raw(q, cmd) == -ENOSPC) {
+		/*
+		 * Keep the queue locked, otherwise the producer could wrap
+		 * twice and we could see a future consumer pointer that looks
+		 * like it's behind us.
+		 */
+		if (queue_poll_cons(q, until, wfe))
+			dev_err_ratelimited(smmu->dev, "CMDQ timeout\n");
+	}
+
+	if (ent->opcode == CMDQ_OP_CMD_SYNC && queue_poll_cons(q, until, wfe))
+		dev_err_ratelimited(smmu->dev, "CMD_SYNC timeout\n");
+	spin_unlock(&smmu->cmdq.lock);
+}
+
+/* Context descriptor manipulation functions */
+static u64 arm_smmu_cpu_tcr_to_cd(u64 tcr)
+{
+	u64 val = 0;
+
+	/* Repack the TCR. Just care about TTBR0 for now */
+	val |= ARM_SMMU_TCR2CD(tcr, T0SZ);
+	val |= ARM_SMMU_TCR2CD(tcr, TG0);
+	val |= ARM_SMMU_TCR2CD(tcr, IRGN0);
+	val |= ARM_SMMU_TCR2CD(tcr, ORGN0);
+	val |= ARM_SMMU_TCR2CD(tcr, SH0);
+	val |= ARM_SMMU_TCR2CD(tcr, EPD0);
+	val |= ARM_SMMU_TCR2CD(tcr, EPD1);
+	val |= ARM_SMMU_TCR2CD(tcr, IPS);
+	val |= ARM_SMMU_TCR2CD(tcr, TBI0);
+
+	return val;
+}
+
+static void arm_smmu_write_ctx_desc(struct arm_smmu_device *smmu,
+				    struct arm_smmu_s1_cfg *cfg)
+{
+	u64 val;
+
+	/*
+	 * We don't need to issue any invalidation here, as we'll invalidate
+	 * the STE when installing the new entry anyway.
+	 */
+	val = arm_smmu_cpu_tcr_to_cd(cfg->cd.tcr) |
+#ifdef __BIG_ENDIAN
+	      CTXDESC_CD_0_ENDI |
+#endif
+	      CTXDESC_CD_0_R | CTXDESC_CD_0_A | CTXDESC_CD_0_ASET_PRIVATE |
+	      CTXDESC_CD_0_AA64 | (u64)cfg->cd.asid << CTXDESC_CD_0_ASID_SHIFT |
+	      CTXDESC_CD_0_V;
+	cfg->cdptr[0] = cpu_to_le64(val);
+
+	val = cfg->cd.ttbr & CTXDESC_CD_1_TTB0_MASK << CTXDESC_CD_1_TTB0_SHIFT;
+	cfg->cdptr[1] = cpu_to_le64(val);
+
+	cfg->cdptr[3] = cpu_to_le64(cfg->cd.mair << CTXDESC_CD_3_MAIR_SHIFT);
+}
+
+/* Stream table manipulation functions */
+static void
+arm_smmu_write_strtab_l1_desc(__le64 *dst, struct arm_smmu_strtab_l1_desc *desc)
+{
+	u64 val = 0;
+
+	val |= (desc->span & STRTAB_L1_DESC_SPAN_MASK)
+		<< STRTAB_L1_DESC_SPAN_SHIFT;
+	val |= desc->l2ptr_dma &
+	       STRTAB_L1_DESC_L2PTR_MASK << STRTAB_L1_DESC_L2PTR_SHIFT;
+
+	*dst = cpu_to_le64(val);
+}
+
+static void arm_smmu_sync_ste_for_sid(struct arm_smmu_device *smmu, u32 sid)
+{
+	struct arm_smmu_cmdq_ent cmd = {
+		.opcode	= CMDQ_OP_CFGI_STE,
+		.cfgi	= {
+			.sid	= sid,
+			.leaf	= true,
+		},
+	};
+
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+	cmd.opcode = CMDQ_OP_CMD_SYNC;
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+}
+
+static void arm_smmu_write_strtab_ent(struct arm_smmu_device *smmu, u32 sid,
+				      __le64 *dst, struct arm_smmu_strtab_ent *ste)
+{
+	/*
+	 * This is hideously complicated, but we only really care about
+	 * three cases at the moment:
+	 *
+	 * 1. Invalid (all zero) -> bypass  (init)
+	 * 2. Bypass -> translation (attach)
+	 * 3. Translation -> bypass (detach)
+	 *
+	 * Given that we can't update the STE atomically and the SMMU
+	 * doesn't read the thing in a defined order, that leaves us
+	 * with the following maintenance requirements:
+	 *
+	 * 1. Update Config, return (init time STEs aren't live)
+	 * 2. Write everything apart from dword 0, sync, write dword 0, sync
+	 * 3. Update Config, sync
+	 */
+	u64 val = le64_to_cpu(dst[0]);
+	bool ste_live = false;
+	struct arm_smmu_cmdq_ent prefetch_cmd = {
+		.opcode		= CMDQ_OP_PREFETCH_CFG,
+		.prefetch	= {
+			.sid	= sid,
+		},
+	};
+
+	if (val & STRTAB_STE_0_V) {
+		u64 cfg;
+
+		cfg = val & STRTAB_STE_0_CFG_MASK << STRTAB_STE_0_CFG_SHIFT;
+		switch (cfg) {
+		case STRTAB_STE_0_CFG_BYPASS:
+			break;
+		case STRTAB_STE_0_CFG_S1_TRANS:
+		case STRTAB_STE_0_CFG_S2_TRANS:
+			ste_live = true;
+			break;
+		default:
+			BUG(); /* STE corruption */
+		}
+	}
+
+	/* Nuke the existing Config, as we're going to rewrite it */
+	val &= ~(STRTAB_STE_0_CFG_MASK << STRTAB_STE_0_CFG_SHIFT);
+
+	if (ste->valid)
+		val |= STRTAB_STE_0_V;
+	else
+		val &= ~STRTAB_STE_0_V;
+
+	if (ste->bypass) {
+		val |= disable_bypass ? STRTAB_STE_0_CFG_ABORT
+				      : STRTAB_STE_0_CFG_BYPASS;
+		dst[0] = cpu_to_le64(val);
+		dst[2] = 0; /* Nuke the VMID */
+		if (ste_live)
+			arm_smmu_sync_ste_for_sid(smmu, sid);
+		return;
+	}
+
+	if (ste->s1_cfg) {
+		BUG_ON(ste_live);
+		dst[1] = cpu_to_le64(
+			 STRTAB_STE_1_S1C_CACHE_WBRA
+			 << STRTAB_STE_1_S1CIR_SHIFT |
+			 STRTAB_STE_1_S1C_CACHE_WBRA
+			 << STRTAB_STE_1_S1COR_SHIFT |
+			 STRTAB_STE_1_S1C_SH_ISH << STRTAB_STE_1_S1CSH_SHIFT |
+			 STRTAB_STE_1_S1STALLD |
+#ifdef CONFIG_PCI_ATS
+			 STRTAB_STE_1_EATS_TRANS << STRTAB_STE_1_EATS_SHIFT |
+#endif
+			 STRTAB_STE_1_STRW_NSEL1 << STRTAB_STE_1_STRW_SHIFT);
+
+		val |= (ste->s1_cfg->cdptr_dma & STRTAB_STE_0_S1CTXPTR_MASK
+		        << STRTAB_STE_0_S1CTXPTR_SHIFT) |
+			STRTAB_STE_0_CFG_S1_TRANS;
+
+	}
+
+	if (ste->s2_cfg) {
+		BUG_ON(ste_live);
+		dst[2] = cpu_to_le64(
+			 ste->s2_cfg->vmid << STRTAB_STE_2_S2VMID_SHIFT |
+			 (ste->s2_cfg->vtcr & STRTAB_STE_2_VTCR_MASK)
+			  << STRTAB_STE_2_VTCR_SHIFT |
+#ifdef __BIG_ENDIAN
+			 STRTAB_STE_2_S2ENDI |
+#endif
+			 STRTAB_STE_2_S2PTW | STRTAB_STE_2_S2AA64 |
+			 STRTAB_STE_2_S2R);
+
+		dst[3] = cpu_to_le64(ste->s2_cfg->vttbr &
+			 STRTAB_STE_3_S2TTB_MASK << STRTAB_STE_3_S2TTB_SHIFT);
+
+		val |= STRTAB_STE_0_CFG_S2_TRANS;
+	}
+
+	arm_smmu_sync_ste_for_sid(smmu, sid);
+	dst[0] = cpu_to_le64(val);
+	arm_smmu_sync_ste_for_sid(smmu, sid);
+
+	/* It's likely that we'll want to use the new STE soon */
+	arm_smmu_cmdq_issue_cmd(smmu, &prefetch_cmd);
+}
+
+static void arm_smmu_init_bypass_stes(u64 *strtab, unsigned int nent)
+{
+	unsigned int i;
+	struct arm_smmu_strtab_ent ste = {
+		.valid	= true,
+		.bypass	= true,
+	};
+
+	for (i = 0; i < nent; ++i) {
+		arm_smmu_write_strtab_ent(NULL, -1, strtab, &ste);
+		strtab += STRTAB_STE_DWORDS;
+	}
+}
+
+static int arm_smmu_init_l2_strtab(struct arm_smmu_device *smmu, u32 sid)
+{
+	size_t size;
+	void *strtab;
+	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
+	struct arm_smmu_strtab_l1_desc *desc = &cfg->l1_desc[sid >> STRTAB_SPLIT];
+
+	if (desc->l2ptr)
+		return 0;
+
+	size = 1 << (STRTAB_SPLIT + ilog2(STRTAB_STE_DWORDS) + 3);
+	strtab = &cfg->strtab[sid >> STRTAB_SPLIT << STRTAB_L1_DESC_DWORDS];
+
+	desc->span = STRTAB_SPLIT + 1;
+	desc->l2ptr = dma_zalloc_coherent(smmu->dev, size, &desc->l2ptr_dma,
+					  GFP_KERNEL);
+	if (!desc->l2ptr) {
+		dev_err(smmu->dev,
+			"failed to allocate l2 stream table for SID %u\n",
+			sid);
+		return -ENOMEM;
+	}
+
+	arm_smmu_init_bypass_stes(desc->l2ptr, 1 << STRTAB_SPLIT);
+	arm_smmu_write_strtab_l1_desc(strtab, desc);
+	return 0;
+}
+
+/* IRQ and event handlers */
+static irqreturn_t arm_smmu_evtq_thread(int irq, void *dev)
+{
+	int i;
+	struct arm_smmu_device *smmu = dev;
+	struct arm_smmu_queue *q = &smmu->evtq.q;
+	u64 evt[EVTQ_ENT_DWORDS];
+
+	while (!queue_remove_raw(q, evt)) {
+		u8 id = evt[0] >> EVTQ_0_ID_SHIFT & EVTQ_0_ID_MASK;
+
+		dev_info(smmu->dev, "event 0x%02x received:\n", id);
+		for (i = 0; i < ARRAY_SIZE(evt); ++i)
+			dev_info(smmu->dev, "\t0x%016llx\n",
+				 (unsigned long long)evt[i]);
+	}
+
+	/* Sync our overflow flag, as we believe we're up to speed */
+	q->cons = Q_OVF(q, q->prod) | Q_WRP(q, q->cons) | Q_IDX(q, q->cons);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t arm_smmu_evtq_handler(int irq, void *dev)
+{
+	irqreturn_t ret = IRQ_WAKE_THREAD;
+	struct arm_smmu_device *smmu = dev;
+	struct arm_smmu_queue *q = &smmu->evtq.q;
+
+	/*
+	 * Not much we can do on overflow, so scream and pretend we're
+	 * trying harder.
+	 */
+	if (queue_sync_prod(q) == -EOVERFLOW)
+		dev_err(smmu->dev, "EVTQ overflow detected -- events lost\n");
+	else if (queue_empty(q))
+		ret = IRQ_NONE;
+
+	return ret;
+}
+
+static irqreturn_t arm_smmu_priq_thread(int irq, void *dev)
+{
+	struct arm_smmu_device *smmu = dev;
+	struct arm_smmu_queue *q = &smmu->priq.q;
+	u64 evt[PRIQ_ENT_DWORDS];
+
+	while (!queue_remove_raw(q, evt)) {
+		u32 sid, ssid;
+		u16 grpid;
+		bool ssv, last;
+
+		sid = evt[0] >> PRIQ_0_SID_SHIFT & PRIQ_0_SID_MASK;
+		ssv = evt[0] & PRIQ_0_SSID_V;
+		ssid = ssv ? evt[0] >> PRIQ_0_SSID_SHIFT & PRIQ_0_SSID_MASK : 0;
+		last = evt[0] & PRIQ_0_PRG_LAST;
+		grpid = evt[1] >> PRIQ_1_PRG_IDX_SHIFT & PRIQ_1_PRG_IDX_MASK;
+
+		dev_info(smmu->dev, "unexpected PRI request received:\n");
+		dev_info(smmu->dev,
+			 "\tsid 0x%08x.0x%05x: [%u%s] %sprivileged %s%s%s access at iova 0x%016llx\n",
+			 sid, ssid, grpid, last ? "L" : "",
+			 evt[0] & PRIQ_0_PERM_PRIV ? "" : "un",
+			 evt[0] & PRIQ_0_PERM_READ ? "R" : "",
+			 evt[0] & PRIQ_0_PERM_WRITE ? "W" : "",
+			 evt[0] & PRIQ_0_PERM_EXEC ? "X" : "",
+			 evt[1] & PRIQ_1_ADDR_MASK << PRIQ_1_ADDR_SHIFT);
+
+		if (last) {
+			struct arm_smmu_cmdq_ent cmd = {
+				.opcode			= CMDQ_OP_PRI_RESP,
+				.substream_valid	= ssv,
+				.pri			= {
+					.sid	= sid,
+					.ssid	= ssid,
+					.grpid	= grpid,
+					.resp	= PRI_RESP_DENY,
+				},
+			};
+
+			arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+		}
+	}
+
+	/* Sync our overflow flag, as we believe we're up to speed */
+	q->cons = Q_OVF(q, q->prod) | Q_WRP(q, q->cons) | Q_IDX(q, q->cons);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t arm_smmu_priq_handler(int irq, void *dev)
+{
+	irqreturn_t ret = IRQ_WAKE_THREAD;
+	struct arm_smmu_device *smmu = dev;
+	struct arm_smmu_queue *q = &smmu->priq.q;
+
+	/* PRIQ overflow indicates a programming error */
+	if (queue_sync_prod(q) == -EOVERFLOW)
+		dev_err(smmu->dev, "PRIQ overflow detected -- requests lost\n");
+	else if (queue_empty(q))
+		ret = IRQ_NONE;
+
+	return ret;
+}
+
+static irqreturn_t arm_smmu_cmdq_sync_handler(int irq, void *dev)
+{
+	/* We don't actually use CMD_SYNC interrupts for anything */
+	return IRQ_HANDLED;
+}
+
+static int arm_smmu_device_disable(struct arm_smmu_device *smmu);
+
+static irqreturn_t arm_smmu_gerror_handler(int irq, void *dev)
+{
+	u32 gerror, gerrorn;
+	struct arm_smmu_device *smmu = dev;
+
+	gerror = readl_relaxed(smmu->base + ARM_SMMU_GERROR);
+	gerrorn = readl_relaxed(smmu->base + ARM_SMMU_GERRORN);
+
+	gerror ^= gerrorn;
+	if (!(gerror & GERROR_ERR_MASK))
+		return IRQ_NONE; /* No errors pending */
+
+	dev_warn(smmu->dev,
+		 "unexpected global error reported (0x%08x), this could be serious\n",
+		 gerror);
+
+	if (gerror & GERROR_SFM_ERR) {
+		dev_err(smmu->dev, "device has entered Service Failure Mode!\n");
+		arm_smmu_device_disable(smmu);
+	}
+
+	if (gerror & GERROR_MSI_GERROR_ABT_ERR)
+		dev_warn(smmu->dev, "GERROR MSI write aborted\n");
+
+	if (gerror & GERROR_MSI_PRIQ_ABT_ERR) {
+		dev_warn(smmu->dev, "PRIQ MSI write aborted\n");
+		arm_smmu_priq_handler(irq, smmu->dev);
+	}
+
+	if (gerror & GERROR_MSI_EVTQ_ABT_ERR) {
+		dev_warn(smmu->dev, "EVTQ MSI write aborted\n");
+		arm_smmu_evtq_handler(irq, smmu->dev);
+	}
+
+	if (gerror & GERROR_MSI_CMDQ_ABT_ERR) {
+		dev_warn(smmu->dev, "CMDQ MSI write aborted\n");
+		arm_smmu_cmdq_sync_handler(irq, smmu->dev);
+	}
+
+	if (gerror & GERROR_PRIQ_ABT_ERR)
+		dev_err(smmu->dev, "PRIQ write aborted -- events may have been lost\n");
+
+	if (gerror & GERROR_EVTQ_ABT_ERR)
+		dev_err(smmu->dev, "EVTQ write aborted -- events may have been lost\n");
+
+	if (gerror & GERROR_CMDQ_ERR)
+		arm_smmu_cmdq_skip_err(smmu);
+
+	writel(gerror, smmu->base + ARM_SMMU_GERRORN);
+	return IRQ_HANDLED;
+}
+
+/* IO_PGTABLE API */
+static void __arm_smmu_tlb_sync(struct arm_smmu_device *smmu)
+{
+	struct arm_smmu_cmdq_ent cmd;
+
+	cmd.opcode = CMDQ_OP_CMD_SYNC;
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+}
+
+static void arm_smmu_tlb_sync(void *cookie)
+{
+	struct arm_smmu_domain *smmu_domain = cookie;
+	__arm_smmu_tlb_sync(smmu_domain->smmu);
+}
+
+static void arm_smmu_tlb_inv_context(void *cookie)
+{
+	struct arm_smmu_domain *smmu_domain = cookie;
+	struct arm_smmu_device *smmu = smmu_domain->smmu;
+	struct arm_smmu_cmdq_ent cmd;
+
+	if (smmu_domain->stage == ARM_SMMU_DOMAIN_S1) {
+		cmd.opcode	= CMDQ_OP_TLBI_NH_ASID;
+		cmd.tlbi.asid	= smmu_domain->s1_cfg.cd.asid;
+		cmd.tlbi.vmid	= 0;
+	} else {
+		cmd.opcode	= CMDQ_OP_TLBI_S12_VMALL;
+		cmd.tlbi.vmid	= smmu_domain->s2_cfg.vmid;
+	}
+
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+	__arm_smmu_tlb_sync(smmu);
+}
+
+static void arm_smmu_tlb_inv_range_nosync(unsigned long iova, size_t size,
+					  bool leaf, void *cookie)
+{
+	struct arm_smmu_domain *smmu_domain = cookie;
+	struct arm_smmu_device *smmu = smmu_domain->smmu;
+	struct arm_smmu_cmdq_ent cmd = {
+		.tlbi = {
+			.leaf	= leaf,
+			.addr	= iova,
+		},
+	};
+
+	if (smmu_domain->stage == ARM_SMMU_DOMAIN_S1) {
+		cmd.opcode	= CMDQ_OP_TLBI_NH_VA;
+		cmd.tlbi.asid	= smmu_domain->s1_cfg.cd.asid;
+	} else {
+		cmd.opcode	= CMDQ_OP_TLBI_S2_IPA;
+		cmd.tlbi.vmid	= smmu_domain->s2_cfg.vmid;
+	}
+
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+}
+
+static void arm_smmu_flush_pgtable(void *addr, size_t size, void *cookie)
+{
+	struct arm_smmu_domain *smmu_domain = cookie;
+	struct arm_smmu_device *smmu = smmu_domain->smmu;
+	unsigned long offset = (unsigned long)addr & ~PAGE_MASK;
+
+	if (smmu->features & ARM_SMMU_FEAT_COHERENCY) {
+		dsb(ishst);
+	} else {
+		dma_addr_t dma_addr;
+		struct device *dev = smmu->dev;
+
+		dma_addr = dma_map_page(dev, virt_to_page(addr), offset, size,
+					DMA_TO_DEVICE);
+
+		if (dma_mapping_error(dev, dma_addr))
+			dev_err(dev, "failed to flush pgtable at %p\n", addr);
+		else
+			dma_unmap_page(dev, dma_addr, size, DMA_TO_DEVICE);
+	}
+}
+
+static struct iommu_gather_ops arm_smmu_gather_ops = {
+	.tlb_flush_all	= arm_smmu_tlb_inv_context,
+	.tlb_add_flush	= arm_smmu_tlb_inv_range_nosync,
+	.tlb_sync	= arm_smmu_tlb_sync,
+	.flush_pgtable	= arm_smmu_flush_pgtable,
+};
+
+/* IOMMU API */
+static bool arm_smmu_capable(enum iommu_cap cap)
+{
+	switch (cap) {
+	case IOMMU_CAP_CACHE_COHERENCY:
+		return true;
+	case IOMMU_CAP_INTR_REMAP:
+		return true; /* MSIs are just memory writes */
+	case IOMMU_CAP_NOEXEC:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static struct iommu_domain *arm_smmu_domain_alloc(unsigned type)
+{
+	struct arm_smmu_domain *smmu_domain;
+
+	if (type != IOMMU_DOMAIN_UNMANAGED)
+		return NULL;
+
+	/*
+	 * Allocate the domain and initialise some of its data structures.
+	 * We can't really do anything meaningful until we've added a
+	 * master.
+	 */
+	smmu_domain = kzalloc(sizeof(*smmu_domain), GFP_KERNEL);
+	if (!smmu_domain)
+		return NULL;
+
+	mutex_init(&smmu_domain->init_mutex);
+	spin_lock_init(&smmu_domain->pgtbl_lock);
+	return &smmu_domain->domain;
+}
+
+static int arm_smmu_bitmap_alloc(unsigned long *map, int span)
+{
+	int idx, size = 1 << span;
+
+	do {
+		idx = find_first_zero_bit(map, size);
+		if (idx == size)
+			return -ENOSPC;
+	} while (test_and_set_bit(idx, map));
+
+	return idx;
+}
+
+static void arm_smmu_bitmap_free(unsigned long *map, int idx)
+{
+	clear_bit(idx, map);
+}
+
+static void arm_smmu_domain_free(struct iommu_domain *domain)
+{
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct arm_smmu_device *smmu = smmu_domain->smmu;
+
+	free_io_pgtable_ops(smmu_domain->pgtbl_ops);
+
+	/* Free the CD and ASID, if we allocated them */
+	if (smmu_domain->stage == ARM_SMMU_DOMAIN_S1) {
+		struct arm_smmu_s1_cfg *cfg = &smmu_domain->s1_cfg;
+
+		if (cfg->cdptr) {
+			dma_free_coherent(smmu_domain->smmu->dev,
+					  CTXDESC_CD_DWORDS << 3,
+					  cfg->cdptr,
+					  cfg->cdptr_dma);
+
+			arm_smmu_bitmap_free(smmu->asid_map, cfg->cd.asid);
+		}
+	} else {
+		struct arm_smmu_s2_cfg *cfg = &smmu_domain->s2_cfg;
+		if (cfg->vmid)
+			arm_smmu_bitmap_free(smmu->vmid_map, cfg->vmid);
+	}
+
+	kfree(smmu_domain);
+}
+
+static int arm_smmu_domain_finalise_s1(struct arm_smmu_domain *smmu_domain,
+				       struct io_pgtable_cfg *pgtbl_cfg)
+{
+	int ret;
+	u16 asid;
+	struct arm_smmu_device *smmu = smmu_domain->smmu;
+	struct arm_smmu_s1_cfg *cfg = &smmu_domain->s1_cfg;
+
+	asid = arm_smmu_bitmap_alloc(smmu->asid_map, smmu->asid_bits);
+	if (IS_ERR_VALUE(asid))
+		return asid;
+
+	cfg->cdptr = dma_zalloc_coherent(smmu->dev, CTXDESC_CD_DWORDS << 3,
+					 &cfg->cdptr_dma, GFP_KERNEL);
+	if (!cfg->cdptr) {
+		dev_warn(smmu->dev, "failed to allocate context descriptor\n");
+		goto out_free_asid;
+	}
+
+	cfg->cd.asid	= asid;
+	cfg->cd.ttbr	= pgtbl_cfg->arm_lpae_s1_cfg.ttbr[0];
+	cfg->cd.tcr	= pgtbl_cfg->arm_lpae_s1_cfg.tcr;
+	cfg->cd.mair	= pgtbl_cfg->arm_lpae_s1_cfg.mair[0];
+	return 0;
+
+out_free_asid:
+	arm_smmu_bitmap_free(smmu->asid_map, asid);
+	return ret;
+}
+
+static int arm_smmu_domain_finalise_s2(struct arm_smmu_domain *smmu_domain,
+				       struct io_pgtable_cfg *pgtbl_cfg)
+{
+	u16 vmid;
+	struct arm_smmu_device *smmu = smmu_domain->smmu;
+	struct arm_smmu_s2_cfg *cfg = &smmu_domain->s2_cfg;
+
+	vmid = arm_smmu_bitmap_alloc(smmu->vmid_map, smmu->vmid_bits);
+	if (IS_ERR_VALUE(vmid))
+		return vmid;
+
+	cfg->vmid	= vmid;
+	cfg->vttbr	= pgtbl_cfg->arm_lpae_s2_cfg.vttbr;
+	cfg->vtcr	= pgtbl_cfg->arm_lpae_s2_cfg.vtcr;
+	return 0;
+}
+
+static struct iommu_ops arm_smmu_ops;
+
+static int arm_smmu_domain_finalise(struct iommu_domain *domain)
+{
+	int ret;
+	unsigned long ias, oas;
+	enum io_pgtable_fmt fmt;
+	struct io_pgtable_cfg pgtbl_cfg;
+	struct io_pgtable_ops *pgtbl_ops;
+	int (*finalise_stage_fn)(struct arm_smmu_domain *,
+				 struct io_pgtable_cfg *);
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct arm_smmu_device *smmu = smmu_domain->smmu;
+
+	/* Restrict the stage to what we can actually support */
+	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:
+		ias = VA_BITS;
+		oas = smmu->ias;
+		fmt = ARM_64_LPAE_S1;
+		finalise_stage_fn = arm_smmu_domain_finalise_s1;
+		break;
+	case ARM_SMMU_DOMAIN_NESTED:
+	case ARM_SMMU_DOMAIN_S2:
+		ias = smmu->ias;
+		oas = smmu->oas;
+		fmt = ARM_64_LPAE_S2;
+		finalise_stage_fn = arm_smmu_domain_finalise_s2;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	pgtbl_cfg = (struct io_pgtable_cfg) {
+		.pgsize_bitmap	= arm_smmu_ops.pgsize_bitmap,
+		.ias		= ias,
+		.oas		= oas,
+		.tlb		= &arm_smmu_gather_ops,
+	};
+
+	pgtbl_ops = alloc_io_pgtable_ops(fmt, &pgtbl_cfg, smmu_domain);
+	if (!pgtbl_ops)
+		return -ENOMEM;
+
+	arm_smmu_ops.pgsize_bitmap = pgtbl_cfg.pgsize_bitmap;
+	smmu_domain->pgtbl_ops = pgtbl_ops;
+
+	ret = finalise_stage_fn(smmu_domain, &pgtbl_cfg);
+	if (IS_ERR_VALUE(ret))
+		free_io_pgtable_ops(pgtbl_ops);
+
+	return ret;
+}
+
+static struct arm_smmu_group *arm_smmu_group_get(struct device *dev)
+{
+	struct iommu_group *group;
+	struct arm_smmu_group *smmu_group;
+
+	group = iommu_group_get(dev);
+	if (!group)
+		return NULL;
+
+	smmu_group = iommu_group_get_iommudata(group);
+	iommu_group_put(group);
+	return smmu_group;
+}
+
+static __le64 *arm_smmu_get_step_for_sid(struct arm_smmu_device *smmu, u32 sid)
+{
+	__le64 *step;
+	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
+
+	if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB) {
+		struct arm_smmu_strtab_l1_desc *l1_desc;
+		int idx;
+
+		/* Two-level walk */
+		idx = (sid >> STRTAB_SPLIT) * STRTAB_L1_DESC_DWORDS;
+		l1_desc = &cfg->l1_desc[idx];
+		idx = (sid & ((1 << STRTAB_SPLIT) - 1)) * STRTAB_STE_DWORDS;
+		step = &l1_desc->l2ptr[idx];
+	} else {
+		/* Simple linear lookup */
+		step = &cfg->strtab[sid * STRTAB_STE_DWORDS];
+	}
+
+	return step;
+}
+
+static int arm_smmu_install_ste_for_group(struct arm_smmu_group *smmu_group)
+{
+	int i;
+	struct arm_smmu_domain *smmu_domain = smmu_group->domain;
+	struct arm_smmu_strtab_ent *ste = &smmu_group->ste;
+	struct arm_smmu_device *smmu = smmu_group->smmu;
+
+	if (smmu_domain->stage == ARM_SMMU_DOMAIN_S1) {
+		ste->s1_cfg = &smmu_domain->s1_cfg;
+		ste->s2_cfg = NULL;
+		arm_smmu_write_ctx_desc(smmu, ste->s1_cfg);
+	} else {
+		ste->s1_cfg = NULL;
+		ste->s2_cfg = &smmu_domain->s2_cfg;
+	}
+
+	for (i = 0; i < smmu_group->num_sids; ++i) {
+		u32 sid = smmu_group->sids[i];
+		__le64 *step = arm_smmu_get_step_for_sid(smmu, sid);
+
+		arm_smmu_write_strtab_ent(smmu, sid, step, ste);
+	}
+
+	return 0;
+}
+
+static int arm_smmu_attach_dev(struct iommu_domain *domain, struct device *dev)
+{
+	int ret = 0;
+	struct arm_smmu_device *smmu;
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct arm_smmu_group *smmu_group = arm_smmu_group_get(dev);
+
+	if (!smmu_group)
+		return -ENOENT;
+
+	/* Already attached to a different domain? */
+	if (smmu_group->domain && smmu_group->domain != smmu_domain)
+		return -EEXIST;
+
+	smmu = smmu_group->smmu;
+	mutex_lock(&smmu_domain->init_mutex);
+
+	if (!smmu_domain->smmu) {
+		smmu_domain->smmu = smmu;
+		ret = arm_smmu_domain_finalise(domain);
+		if (ret) {
+			smmu_domain->smmu = NULL;
+			goto out_unlock;
+		}
+	} else if (smmu_domain->smmu != smmu) {
+		dev_err(dev,
+			"cannot attach to SMMU %s (upstream of %s)\n",
+			dev_name(smmu_domain->smmu->dev),
+			dev_name(smmu->dev));
+		ret = -ENXIO;
+		goto out_unlock;
+	}
+
+	/* Group already attached to this domain? */
+	if (smmu_group->domain)
+		goto out_unlock;
+
+	smmu_group->domain	= smmu_domain;
+	smmu_group->ste.bypass	= false;
+
+	ret = arm_smmu_install_ste_for_group(smmu_group);
+	if (IS_ERR_VALUE(ret))
+		smmu_group->domain = NULL;
+
+out_unlock:
+	mutex_unlock(&smmu_domain->init_mutex);
+	return ret;
+}
+
+static void arm_smmu_detach_dev(struct iommu_domain *domain, struct device *dev)
+{
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct arm_smmu_group *smmu_group = arm_smmu_group_get(dev);
+
+	BUG_ON(!smmu_domain);
+	BUG_ON(!smmu_group);
+
+	mutex_lock(&smmu_domain->init_mutex);
+	BUG_ON(smmu_group->domain != smmu_domain);
+
+	smmu_group->ste.bypass = true;
+	if (IS_ERR_VALUE(arm_smmu_install_ste_for_group(smmu_group)))
+		dev_warn(dev, "failed to install bypass STE\n");
+
+	smmu_group->domain = NULL;
+	mutex_unlock(&smmu_domain->init_mutex);
+}
+
+static int arm_smmu_map(struct iommu_domain *domain, unsigned long iova,
+			phys_addr_t paddr, size_t size, int prot)
+{
+	int ret;
+	unsigned long flags;
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct io_pgtable_ops *ops = smmu_domain->pgtbl_ops;
+
+	if (!ops)
+		return -ENODEV;
+
+	spin_lock_irqsave(&smmu_domain->pgtbl_lock, flags);
+	ret = ops->map(ops, iova, paddr, size, prot);
+	spin_unlock_irqrestore(&smmu_domain->pgtbl_lock, flags);
+	return ret;
+}
+
+static size_t
+arm_smmu_unmap(struct iommu_domain *domain, unsigned long iova, size_t size)
+{
+	size_t ret;
+	unsigned long flags;
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct io_pgtable_ops *ops = smmu_domain->pgtbl_ops;
+
+	if (!ops)
+		return 0;
+
+	spin_lock_irqsave(&smmu_domain->pgtbl_lock, flags);
+	ret = ops->unmap(ops, iova, size);
+	spin_unlock_irqrestore(&smmu_domain->pgtbl_lock, flags);
+	return ret;
+}
+
+static phys_addr_t
+arm_smmu_iova_to_phys(struct iommu_domain *domain, dma_addr_t iova)
+{
+	phys_addr_t ret;
+	unsigned long flags;
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+	struct io_pgtable_ops *ops = smmu_domain->pgtbl_ops;
+
+	if (!ops)
+		return 0;
+
+	spin_lock_irqsave(&smmu_domain->pgtbl_lock, flags);
+	ret = ops->iova_to_phys(ops, iova);
+	spin_unlock_irqrestore(&smmu_domain->pgtbl_lock, flags);
+
+	return ret;
+}
+
+static int __arm_smmu_get_pci_sid(struct pci_dev *pdev, u16 alias, void *sidp)
+{
+	*(u32 *)sidp = alias;
+	return 0; /* Continue walking */
+}
+
+static void __arm_smmu_release_pci_iommudata(void *data)
+{
+	kfree(data);
+}
+
+static struct arm_smmu_device *arm_smmu_get_for_pci_dev(struct pci_dev *pdev)
+{
+	struct device_node *of_node;
+	struct arm_smmu_device *curr, *smmu = NULL;
+	struct pci_bus *bus = pdev->bus;
+
+	/* Walk up to the root bus */
+	while (!pci_is_root_bus(bus))
+		bus = bus->parent;
+
+	/* Follow the "iommus" phandle from the host controller */
+	of_node = of_parse_phandle(bus->bridge->parent->of_node, "iommus", 0);
+	if (!of_node)
+		return NULL;
+
+	/* See if we can find an SMMU corresponding to the phandle */
+	spin_lock(&arm_smmu_devices_lock);
+	list_for_each_entry(curr, &arm_smmu_devices, list) {
+		if (curr->dev->of_node == of_node) {
+			smmu = curr;
+			break;
+		}
+	}
+	spin_unlock(&arm_smmu_devices_lock);
+	of_node_put(of_node);
+	return smmu;
+}
+
+static bool arm_smmu_sid_in_range(struct arm_smmu_device *smmu, u32 sid)
+{
+	unsigned long limit = smmu->strtab_cfg.num_l1_ents;
+
+	if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB)
+		limit *= 1UL << STRTAB_SPLIT;
+
+	return sid < limit;
+}
+
+static int arm_smmu_add_device(struct device *dev)
+{
+	int i, ret;
+	u32 sid, *sids;
+	struct pci_dev *pdev;
+	struct iommu_group *group;
+	struct arm_smmu_group *smmu_group;
+	struct arm_smmu_device *smmu;
+
+	/* We only support PCI, for now */
+	if (!dev_is_pci(dev))
+		return -ENODEV;
+
+	pdev = to_pci_dev(dev);
+	group = iommu_group_get_for_dev(dev);
+	if (IS_ERR(group))
+		return PTR_ERR(group);
+
+	smmu_group = iommu_group_get_iommudata(group);
+	if (!smmu_group) {
+		smmu = arm_smmu_get_for_pci_dev(pdev);
+		if (!smmu) {
+			ret = -ENOENT;
+			goto out_put_group;
+		}
+
+		smmu_group = kzalloc(sizeof(*smmu_group), GFP_KERNEL);
+		if (!smmu_group) {
+			ret = -ENOMEM;
+			goto out_put_group;
+		}
+
+		smmu_group->ste.valid	= true;
+		smmu_group->smmu	= smmu;
+		iommu_group_set_iommudata(group, smmu_group,
+					  __arm_smmu_release_pci_iommudata);
+	} else {
+		smmu = smmu_group->smmu;
+	}
+
+	/* Assume SID == RID until firmware tells us otherwise */
+	pci_for_each_dma_alias(pdev, __arm_smmu_get_pci_sid, &sid);
+	for (i = 0; i < smmu_group->num_sids; ++i) {
+		/* If we already know about this SID, then we're done */
+		if (smmu_group->sids[i] == sid)
+			return 0;
+	}
+
+	/* Check the SID is in range of the SMMU and our stream table */
+	if (!arm_smmu_sid_in_range(smmu, sid)) {
+		ret = -ERANGE;
+		goto out_put_group;
+	}
+
+	/* Ensure l2 strtab is initialised */
+	if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB) {
+		ret = arm_smmu_init_l2_strtab(smmu, sid);
+		if (ret)
+			goto out_put_group;
+	}
+
+	/* Resize the SID array for the group */
+	smmu_group->num_sids++;
+	sids = krealloc(smmu_group->sids, smmu_group->num_sids * sizeof(*sids),
+			GFP_KERNEL);
+	if (!sids) {
+		smmu_group->num_sids--;
+		ret = -ENOMEM;
+		goto out_put_group;
+	}
+
+	/* Add the new SID */
+	sids[smmu_group->num_sids - 1] = sid;
+	smmu_group->sids = sids;
+	return 0;
+
+out_put_group:
+	iommu_group_put(group);
+	return ret;
+}
+
+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 = to_smmu_domain(domain);
+
+	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)
+{
+	int ret = 0;
+	struct arm_smmu_domain *smmu_domain = to_smmu_domain(domain);
+
+	mutex_lock(&smmu_domain->init_mutex);
+
+	switch (attr) {
+	case DOMAIN_ATTR_NESTING:
+		if (smmu_domain->smmu) {
+			ret = -EPERM;
+			goto out_unlock;
+		}
+
+		if (*(int *)data)
+			smmu_domain->stage = ARM_SMMU_DOMAIN_NESTED;
+		else
+			smmu_domain->stage = ARM_SMMU_DOMAIN_S1;
+
+		break;
+	default:
+		ret = -ENODEV;
+	}
+
+out_unlock:
+	mutex_unlock(&smmu_domain->init_mutex);
+	return ret;
+}
+
+static struct iommu_ops arm_smmu_ops = {
+	.capable		= arm_smmu_capable,
+	.domain_alloc		= arm_smmu_domain_alloc,
+	.domain_free		= arm_smmu_domain_free,
+	.attach_dev		= arm_smmu_attach_dev,
+	.detach_dev		= arm_smmu_detach_dev,
+	.map			= arm_smmu_map,
+	.unmap			= arm_smmu_unmap,
+	.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		= -1UL, /* Restricted during device attach */
+};
+
+/* Probing and initialisation functions */
+static int arm_smmu_init_one_queue(struct arm_smmu_device *smmu,
+				   struct arm_smmu_queue *q,
+				   unsigned long prod_off,
+				   unsigned long cons_off,
+				   size_t dwords)
+{
+	size_t qsz = ((1 << q->max_n_shift) * dwords) << 3;
+
+	q->base = dma_alloc_coherent(smmu->dev, qsz, &q->base_dma, GFP_KERNEL);
+	if (!q->base) {
+		dev_err(smmu->dev, "failed to allocate queue (0x%zx bytes)\n",
+			qsz);
+		return -ENOMEM;
+	}
+
+	q->prod_reg	= smmu->base + prod_off;
+	q->cons_reg	= smmu->base + cons_off;
+	q->ent_dwords	= dwords;
+
+	q->q_base  = Q_BASE_RWA;
+	q->q_base |= q->base_dma & Q_BASE_ADDR_MASK << Q_BASE_ADDR_SHIFT;
+	q->q_base |= (q->max_n_shift & Q_BASE_LOG2SIZE_MASK)
+		     << Q_BASE_LOG2SIZE_SHIFT;
+
+	q->prod = q->cons = 0;
+	return 0;
+}
+
+static void arm_smmu_free_one_queue(struct arm_smmu_device *smmu,
+				    struct arm_smmu_queue *q)
+{
+	size_t qsz = ((1 << q->max_n_shift) * q->ent_dwords) << 3;
+
+	dma_free_coherent(smmu->dev, qsz, q->base, q->base_dma);
+}
+
+static void arm_smmu_free_queues(struct arm_smmu_device *smmu)
+{
+	arm_smmu_free_one_queue(smmu, &smmu->cmdq.q);
+	arm_smmu_free_one_queue(smmu, &smmu->evtq.q);
+
+	if (smmu->features & ARM_SMMU_FEAT_PRI)
+		arm_smmu_free_one_queue(smmu, &smmu->priq.q);
+}
+
+static int arm_smmu_init_queues(struct arm_smmu_device *smmu)
+{
+	int ret;
+
+	/* cmdq */
+	spin_lock_init(&smmu->cmdq.lock);
+	ret = arm_smmu_init_one_queue(smmu, &smmu->cmdq.q, ARM_SMMU_CMDQ_PROD,
+				      ARM_SMMU_CMDQ_CONS, CMDQ_ENT_DWORDS);
+	if (ret)
+		goto out;
+
+	/* evtq */
+	ret = arm_smmu_init_one_queue(smmu, &smmu->evtq.q, ARM_SMMU_EVTQ_PROD,
+				      ARM_SMMU_EVTQ_CONS, EVTQ_ENT_DWORDS);
+	if (ret)
+		goto out_free_cmdq;
+
+	/* priq */
+	if (!(smmu->features & ARM_SMMU_FEAT_PRI))
+		return 0;
+
+	ret = arm_smmu_init_one_queue(smmu, &smmu->priq.q, ARM_SMMU_PRIQ_PROD,
+				      ARM_SMMU_PRIQ_CONS, PRIQ_ENT_DWORDS);
+	if (ret)
+		goto out_free_evtq;
+
+	return 0;
+
+out_free_evtq:
+	arm_smmu_free_one_queue(smmu, &smmu->evtq.q);
+out_free_cmdq:
+	arm_smmu_free_one_queue(smmu, &smmu->cmdq.q);
+out:
+	return ret;
+}
+
+static void arm_smmu_free_l2_strtab(struct arm_smmu_device *smmu)
+{
+	int i;
+	size_t size;
+	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
+
+	size = 1 << (STRTAB_SPLIT + ilog2(STRTAB_STE_DWORDS) + 3);
+	for (i = 0; i < cfg->num_l1_ents; ++i) {
+		struct arm_smmu_strtab_l1_desc *desc = &cfg->l1_desc[i];
+
+		if (!desc->l2ptr)
+			continue;
+
+		dma_free_coherent(smmu->dev, size, desc->l2ptr,
+				  desc->l2ptr_dma);
+	}
+}
+
+static int arm_smmu_init_l1_strtab(struct arm_smmu_device *smmu)
+{
+	unsigned int i;
+	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
+	size_t size = sizeof(*cfg->l1_desc) * cfg->num_l1_ents;
+	void *strtab = smmu->strtab_cfg.strtab;
+
+	cfg->l1_desc = devm_kzalloc(smmu->dev, size, GFP_KERNEL);
+	if (!cfg->l1_desc) {
+		dev_err(smmu->dev, "failed to allocate l1 stream table desc\n");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < cfg->num_l1_ents; ++i) {
+		arm_smmu_write_strtab_l1_desc(strtab, &cfg->l1_desc[i]);
+		strtab += STRTAB_L1_DESC_DWORDS << 3;
+	}
+
+	return 0;
+}
+
+static int arm_smmu_init_strtab_2lvl(struct arm_smmu_device *smmu)
+{
+	void *strtab;
+	u64 reg;
+	u32 size;
+	int ret;
+	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
+
+	/* Calculate the L1 size, capped to the SIDSIZE */
+	size = STRTAB_L1_SZ_SHIFT - (ilog2(STRTAB_L1_DESC_DWORDS) + 3);
+	size = min(size, smmu->sid_bits - STRTAB_SPLIT);
+	if (size + STRTAB_SPLIT < smmu->sid_bits)
+		dev_warn(smmu->dev,
+			 "2-level strtab only covers %u/%u bits of SID\n",
+			 size + STRTAB_SPLIT, smmu->sid_bits);
+
+	cfg->num_l1_ents = 1 << size;
+	size = cfg->num_l1_ents * (STRTAB_L1_DESC_DWORDS << 3);
+	strtab = dma_zalloc_coherent(smmu->dev, size, &cfg->strtab_dma,
+				     GFP_KERNEL);
+	if (!strtab) {
+		dev_err(smmu->dev,
+			"failed to allocate l1 stream table (%u bytes)\n",
+			size);
+		return -ENOMEM;
+	}
+	cfg->strtab = strtab;
+
+	/* Configure strtab_base_cfg for 2 levels */
+	reg  = STRTAB_BASE_CFG_FMT_2LVL;
+	reg |= (size & STRTAB_BASE_CFG_LOG2SIZE_MASK)
+		<< STRTAB_BASE_CFG_LOG2SIZE_SHIFT;
+	reg |= (STRTAB_SPLIT & STRTAB_BASE_CFG_SPLIT_MASK)
+		<< STRTAB_BASE_CFG_SPLIT_SHIFT;
+	cfg->strtab_base_cfg = reg;
+
+	ret = arm_smmu_init_l1_strtab(smmu);
+	if (ret)
+		dma_free_coherent(smmu->dev,
+				  cfg->num_l1_ents *
+				  (STRTAB_L1_DESC_DWORDS << 3),
+				  strtab,
+				  cfg->strtab_dma);
+	return ret;
+}
+
+static int arm_smmu_init_strtab_linear(struct arm_smmu_device *smmu)
+{
+	void *strtab;
+	u64 reg;
+	u32 size;
+	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
+
+	size = (1 << smmu->sid_bits) * (STRTAB_STE_DWORDS << 3);
+	strtab = dma_zalloc_coherent(smmu->dev, size, &cfg->strtab_dma,
+				     GFP_KERNEL);
+	if (!strtab) {
+		dev_err(smmu->dev,
+			"failed to allocate linear stream table (%u bytes)\n",
+			size);
+		return -ENOMEM;
+	}
+	cfg->strtab = strtab;
+	cfg->num_l1_ents = 1 << smmu->sid_bits;
+
+	/* Configure strtab_base_cfg for a linear table covering all SIDs */
+	reg  = STRTAB_BASE_CFG_FMT_LINEAR;
+	reg |= (smmu->sid_bits & STRTAB_BASE_CFG_LOG2SIZE_MASK)
+		<< STRTAB_BASE_CFG_LOG2SIZE_SHIFT;
+	cfg->strtab_base_cfg = reg;
+
+	arm_smmu_init_bypass_stes(strtab, cfg->num_l1_ents);
+	return 0;
+}
+
+static int arm_smmu_init_strtab(struct arm_smmu_device *smmu)
+{
+	u64 reg;
+	int ret;
+
+	if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB)
+		ret = arm_smmu_init_strtab_2lvl(smmu);
+	else
+		ret = arm_smmu_init_strtab_linear(smmu);
+
+	if (ret)
+		return ret;
+
+	/* Set the strtab base address */
+	reg  = smmu->strtab_cfg.strtab_dma &
+	       STRTAB_BASE_ADDR_MASK << STRTAB_BASE_ADDR_SHIFT;
+	reg |= STRTAB_BASE_RA;
+	smmu->strtab_cfg.strtab_base = reg;
+
+	/* Allocate the first VMID for stage-2 bypass STEs */
+	set_bit(0, smmu->vmid_map);
+	return 0;
+}
+
+static void arm_smmu_free_strtab(struct arm_smmu_device *smmu)
+{
+	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
+	u32 size = cfg->num_l1_ents;
+
+	if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB) {
+		arm_smmu_free_l2_strtab(smmu);
+		size *= STRTAB_L1_DESC_DWORDS << 3;
+	} else {
+		size *= STRTAB_STE_DWORDS * 3;
+	}
+
+	dma_free_coherent(smmu->dev, size, cfg->strtab, cfg->strtab_dma);
+}
+
+static int arm_smmu_init_structures(struct arm_smmu_device *smmu)
+{
+	int ret;
+
+	ret = arm_smmu_init_queues(smmu);
+	if (ret)
+		return ret;
+
+	ret = arm_smmu_init_strtab(smmu);
+	if (ret)
+		goto out_free_queues;
+
+	return 0;
+
+out_free_queues:
+	arm_smmu_free_queues(smmu);
+	return ret;
+}
+
+static void arm_smmu_free_structures(struct arm_smmu_device *smmu)
+{
+	arm_smmu_free_strtab(smmu);
+	arm_smmu_free_queues(smmu);
+}
+
+static int arm_smmu_write_reg_sync(struct arm_smmu_device *smmu, u32 val,
+				   unsigned int reg_off, unsigned int ack_off)
+{
+	u32 reg;
+
+	writel_relaxed(val, smmu->base + reg_off);
+	return readl_relaxed_poll_timeout(smmu->base + ack_off, reg, reg == val,
+					  1, ARM_SMMU_POLL_TIMEOUT_US);
+}
+
+static int arm_smmu_setup_irqs(struct arm_smmu_device *smmu)
+{
+	int ret, irq;
+
+	/* Disable IRQs first */
+	ret = arm_smmu_write_reg_sync(smmu, 0, ARM_SMMU_IRQ_CTRL,
+				      ARM_SMMU_IRQ_CTRLACK);
+	if (ret) {
+		dev_err(smmu->dev, "failed to disable irqs\n");
+		return ret;
+	}
+
+	/* Clear the MSI address regs */
+	writeq_relaxed(0, smmu->base + ARM_SMMU_GERROR_IRQ_CFG0);
+	writeq_relaxed(0, smmu->base + ARM_SMMU_EVTQ_IRQ_CFG0);
+
+	/* Request wired interrupt lines */
+	irq = smmu->evtq.q.irq;
+	if (irq) {
+		ret = devm_request_threaded_irq(smmu->dev, irq,
+						arm_smmu_evtq_handler,
+						arm_smmu_evtq_thread,
+						0, "arm-smmu-v3-evtq", smmu);
+		if (IS_ERR_VALUE(ret))
+			dev_warn(smmu->dev, "failed to enable evtq irq\n");
+	}
+
+	irq = smmu->cmdq.q.irq;
+	if (irq) {
+		ret = devm_request_irq(smmu->dev, irq,
+				       arm_smmu_cmdq_sync_handler, 0,
+				       "arm-smmu-v3-cmdq-sync", smmu);
+		if (IS_ERR_VALUE(ret))
+			dev_warn(smmu->dev, "failed to enable cmdq-sync irq\n");
+	}
+
+	irq = smmu->gerr_irq;
+	if (irq) {
+		ret = devm_request_irq(smmu->dev, irq, arm_smmu_gerror_handler,
+				       0, "arm-smmu-v3-gerror", smmu);
+		if (IS_ERR_VALUE(ret))
+			dev_warn(smmu->dev, "failed to enable gerror irq\n");
+	}
+
+	if (smmu->features & ARM_SMMU_FEAT_PRI) {
+		writeq_relaxed(0, smmu->base + ARM_SMMU_PRIQ_IRQ_CFG0);
+
+		irq = smmu->priq.q.irq;
+		if (irq) {
+			ret = devm_request_threaded_irq(smmu->dev, irq,
+							arm_smmu_priq_handler,
+							arm_smmu_priq_thread,
+							0, "arm-smmu-v3-priq",
+							smmu);
+			if (IS_ERR_VALUE(ret))
+				dev_warn(smmu->dev,
+					 "failed to enable priq irq\n");
+		}
+	}
+
+	/* Enable interrupt generation on the SMMU */
+	ret = arm_smmu_write_reg_sync(smmu,
+				      IRQ_CTRL_EVTQ_IRQEN |
+				      IRQ_CTRL_GERROR_IRQEN,
+				      ARM_SMMU_IRQ_CTRL, ARM_SMMU_IRQ_CTRLACK);
+	if (ret)
+		dev_warn(smmu->dev, "failed to enable irqs\n");
+
+	return 0;
+}
+
+static int arm_smmu_device_disable(struct arm_smmu_device *smmu)
+{
+	int ret;
+
+	ret = arm_smmu_write_reg_sync(smmu, 0, ARM_SMMU_CR0, ARM_SMMU_CR0ACK);
+	if (ret)
+		dev_err(smmu->dev, "failed to clear cr0\n");
+
+	return ret;
+}
+
+static int arm_smmu_device_reset(struct arm_smmu_device *smmu)
+{
+	int ret;
+	u32 reg, enables;
+	struct arm_smmu_cmdq_ent cmd;
+
+	/* Clear CR0 and sync (disables SMMU and queue processing) */
+	reg = readl_relaxed(smmu->base + ARM_SMMU_CR0);
+	if (reg & CR0_SMMUEN)
+		dev_warn(smmu->dev, "SMMU currently enabled! Resetting...\n");
+
+	ret = arm_smmu_device_disable(smmu);
+	if (ret)
+		return ret;
+
+	/* CR1 (table and queue memory attributes) */
+	reg = (CR1_SH_ISH << CR1_TABLE_SH_SHIFT) |
+	      (CR1_CACHE_WB << CR1_TABLE_OC_SHIFT) |
+	      (CR1_CACHE_WB << CR1_TABLE_IC_SHIFT) |
+	      (CR1_SH_ISH << CR1_QUEUE_SH_SHIFT) |
+	      (CR1_CACHE_WB << CR1_QUEUE_OC_SHIFT) |
+	      (CR1_CACHE_WB << CR1_QUEUE_IC_SHIFT);
+	writel_relaxed(reg, smmu->base + ARM_SMMU_CR1);
+
+	/* CR2 (random crap) */
+	reg = CR2_PTM | CR2_RECINVSID | CR2_E2H;
+	writel_relaxed(reg, smmu->base + ARM_SMMU_CR2);
+
+	/* Stream table */
+	writeq_relaxed(smmu->strtab_cfg.strtab_base,
+		       smmu->base + ARM_SMMU_STRTAB_BASE);
+	writel_relaxed(smmu->strtab_cfg.strtab_base_cfg,
+		       smmu->base + ARM_SMMU_STRTAB_BASE_CFG);
+
+	/* Command queue */
+	writeq_relaxed(smmu->cmdq.q.q_base, smmu->base + ARM_SMMU_CMDQ_BASE);
+	writel_relaxed(smmu->cmdq.q.prod, smmu->base + ARM_SMMU_CMDQ_PROD);
+	writel_relaxed(smmu->cmdq.q.cons, smmu->base + ARM_SMMU_CMDQ_CONS);
+
+	enables = CR0_CMDQEN;
+	ret = arm_smmu_write_reg_sync(smmu, enables, ARM_SMMU_CR0,
+				      ARM_SMMU_CR0ACK);
+	if (ret) {
+		dev_err(smmu->dev, "failed to enable command queue\n");
+		return ret;
+	}
+
+	/* Invalidate any cached configuration */
+	cmd.opcode = CMDQ_OP_CFGI_ALL;
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+	cmd.opcode = CMDQ_OP_CMD_SYNC;
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+
+	/* Invalidate any stale TLB entries */
+	if (smmu->features & ARM_SMMU_FEAT_HYP) {
+		cmd.opcode = CMDQ_OP_TLBI_EL2_ALL;
+		arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+	}
+
+	cmd.opcode = CMDQ_OP_TLBI_NSNH_ALL;
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+	cmd.opcode = CMDQ_OP_CMD_SYNC;
+	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+
+	/* Event queue */
+	writeq_relaxed(smmu->evtq.q.q_base, smmu->base + ARM_SMMU_EVTQ_BASE);
+	writel_relaxed(smmu->evtq.q.prod, smmu->base + ARM_SMMU_EVTQ_PROD);
+	writel_relaxed(smmu->evtq.q.cons, smmu->base + ARM_SMMU_EVTQ_CONS);
+
+	enables |= CR0_EVTQEN;
+	ret = arm_smmu_write_reg_sync(smmu, enables, ARM_SMMU_CR0,
+				      ARM_SMMU_CR0ACK);
+	if (ret) {
+		dev_err(smmu->dev, "failed to enable event queue\n");
+		return ret;
+	}
+
+	/* PRI queue */
+	if (smmu->features & ARM_SMMU_FEAT_PRI) {
+		writeq_relaxed(smmu->priq.q.q_base,
+			       smmu->base + ARM_SMMU_PRIQ_BASE);
+		writel_relaxed(smmu->priq.q.prod,
+			       smmu->base + ARM_SMMU_PRIQ_PROD);
+		writel_relaxed(smmu->priq.q.cons,
+			       smmu->base + ARM_SMMU_PRIQ_CONS);
+
+		enables |= CR0_PRIQEN;
+		ret = arm_smmu_write_reg_sync(smmu, enables, ARM_SMMU_CR0,
+					      ARM_SMMU_CR0ACK);
+		if (ret) {
+			dev_err(smmu->dev, "failed to enable PRI queue\n");
+			return ret;
+		}
+	}
+
+	ret = arm_smmu_setup_irqs(smmu);
+	if (ret) {
+		dev_err(smmu->dev, "failed to setup irqs\n");
+		return ret;
+	}
+
+	/* Enable the SMMU interface */
+	enables |= CR0_SMMUEN;
+	ret = arm_smmu_write_reg_sync(smmu, enables, ARM_SMMU_CR0,
+				      ARM_SMMU_CR0ACK);
+	if (ret) {
+		dev_err(smmu->dev, "failed to enable SMMU interface\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int arm_smmu_device_probe(struct arm_smmu_device *smmu)
+{
+	u32 reg;
+	bool coherent;
+	unsigned long pgsize_bitmap = 0;
+
+	/* IDR0 */
+	reg = readl_relaxed(smmu->base + ARM_SMMU_IDR0);
+
+	/* 2-level structures */
+	if ((reg & IDR0_ST_LVL_MASK << IDR0_ST_LVL_SHIFT) == IDR0_ST_LVL_2LVL)
+		smmu->features |= ARM_SMMU_FEAT_2_LVL_STRTAB;
+
+	if (reg & IDR0_CD2L)
+		smmu->features |= ARM_SMMU_FEAT_2_LVL_CDTAB;
+
+	/*
+	 * Translation table endianness.
+	 * We currently require the same endianness as the CPU, but this
+	 * could be changed later by adding a new IO_PGTABLE_QUIRK.
+	 */
+	switch (reg & IDR0_TTENDIAN_MASK << IDR0_TTENDIAN_SHIFT) {
+	case IDR0_TTENDIAN_MIXED:
+		smmu->features |= ARM_SMMU_FEAT_TT_LE | ARM_SMMU_FEAT_TT_BE;
+		break;
+#ifdef __BIG_ENDIAN
+	case IDR0_TTENDIAN_BE:
+		smmu->features |= ARM_SMMU_FEAT_TT_BE;
+		break;
+#else
+	case IDR0_TTENDIAN_LE:
+		smmu->features |= ARM_SMMU_FEAT_TT_LE;
+		break;
+#endif
+	default:
+		dev_err(smmu->dev, "unknown/unsupported TT endianness!\n");
+		return -ENXIO;
+	}
+
+	/* Boolean feature flags */
+	if (IS_ENABLED(CONFIG_PCI_PRI) && reg & IDR0_PRI)
+		smmu->features |= ARM_SMMU_FEAT_PRI;
+
+	if (IS_ENABLED(CONFIG_PCI_ATS) && reg & IDR0_ATS)
+		smmu->features |= ARM_SMMU_FEAT_ATS;
+
+	if (reg & IDR0_SEV)
+		smmu->features |= ARM_SMMU_FEAT_SEV;
+
+	if (reg & IDR0_MSI)
+		smmu->features |= ARM_SMMU_FEAT_MSI;
+
+	if (reg & IDR0_HYP)
+		smmu->features |= ARM_SMMU_FEAT_HYP;
+
+	/*
+	 * The dma-coherent property is used in preference to the ID
+	 * register, but warn on mismatch.
+	 */
+	coherent = of_dma_is_coherent(smmu->dev->of_node);
+	if (coherent)
+		smmu->features |= ARM_SMMU_FEAT_COHERENCY;
+
+	if (!!(reg & IDR0_COHACC) != coherent)
+		dev_warn(smmu->dev, "IDR0.COHACC overridden by dma-coherent property (%s)\n",
+			 coherent ? "true" : "false");
+
+	if (reg & IDR0_STALL_MODEL)
+		smmu->features |= ARM_SMMU_FEAT_STALLS;
+
+	if (reg & IDR0_S1P)
+		smmu->features |= ARM_SMMU_FEAT_TRANS_S1;
+
+	if (reg & IDR0_S2P)
+		smmu->features |= ARM_SMMU_FEAT_TRANS_S2;
+
+	if (!(reg & (IDR0_S1P | IDR0_S2P))) {
+		dev_err(smmu->dev, "no translation support!\n");
+		return -ENXIO;
+	}
+
+	/* We only support the AArch64 table format at present */
+	if ((reg & IDR0_TTF_MASK << IDR0_TTF_SHIFT) < IDR0_TTF_AARCH64) {
+		dev_err(smmu->dev, "AArch64 table format not supported!\n");
+		return -ENXIO;
+	}
+
+	/* ASID/VMID sizes */
+	smmu->asid_bits = reg & IDR0_ASID16 ? 16 : 8;
+	smmu->vmid_bits = reg & IDR0_VMID16 ? 16 : 8;
+
+	/* IDR1 */
+	reg = readl_relaxed(smmu->base + ARM_SMMU_IDR1);
+	if (reg & (IDR1_TABLES_PRESET | IDR1_QUEUES_PRESET | IDR1_REL)) {
+		dev_err(smmu->dev, "embedded implementation not supported\n");
+		return -ENXIO;
+	}
+
+	/* Queue sizes, capped at 4k */
+	smmu->cmdq.q.max_n_shift = min((u32)CMDQ_MAX_SZ_SHIFT,
+				       reg >> IDR1_CMDQ_SHIFT & IDR1_CMDQ_MASK);
+	if (!smmu->cmdq.q.max_n_shift) {
+		/* Odd alignment restrictions on the base, so ignore for now */
+		dev_err(smmu->dev, "unit-length command queue not supported\n");
+		return -ENXIO;
+	}
+
+	smmu->evtq.q.max_n_shift = min((u32)EVTQ_MAX_SZ_SHIFT,
+				       reg >> IDR1_EVTQ_SHIFT & IDR1_EVTQ_MASK);
+	smmu->priq.q.max_n_shift = min((u32)PRIQ_MAX_SZ_SHIFT,
+				       reg >> IDR1_PRIQ_SHIFT & IDR1_PRIQ_MASK);
+
+	/* SID/SSID sizes */
+	smmu->ssid_bits = reg >> IDR1_SSID_SHIFT & IDR1_SSID_MASK;
+	smmu->sid_bits = reg >> IDR1_SID_SHIFT & IDR1_SID_MASK;
+
+	/* IDR5 */
+	reg = readl_relaxed(smmu->base + ARM_SMMU_IDR5);
+
+	/* Maximum number of outstanding stalls */
+	smmu->evtq.max_stalls = reg >> IDR5_STALL_MAX_SHIFT
+				& IDR5_STALL_MAX_MASK;
+
+	/* Page sizes */
+	if (reg & IDR5_GRAN64K)
+		pgsize_bitmap |= SZ_64K | SZ_512M;
+	if (reg & IDR5_GRAN16K)
+		pgsize_bitmap |= SZ_16K | SZ_32M;
+	if (reg & IDR5_GRAN4K)
+		pgsize_bitmap |= SZ_4K | SZ_2M | SZ_1G;
+
+	arm_smmu_ops.pgsize_bitmap &= pgsize_bitmap;
+
+	/* Output address size */
+	switch (reg & IDR5_OAS_MASK << IDR5_OAS_SHIFT) {
+	case IDR5_OAS_32_BIT:
+		smmu->oas = 32;
+		break;
+	case IDR5_OAS_36_BIT:
+		smmu->oas = 36;
+		break;
+	case IDR5_OAS_40_BIT:
+		smmu->oas = 40;
+		break;
+	case IDR5_OAS_42_BIT:
+		smmu->oas = 42;
+		break;
+	case IDR5_OAS_44_BIT:
+		smmu->oas = 44;
+		break;
+	case IDR5_OAS_48_BIT:
+		smmu->oas = 48;
+		break;
+	default:
+		dev_err(smmu->dev, "unknown output address size!\n");
+		return -ENXIO;
+	}
+
+	/* Set the DMA mask for our table walker */
+	if (dma_set_mask_and_coherent(smmu->dev, DMA_BIT_MASK(smmu->oas)))
+		dev_warn(smmu->dev,
+			 "failed to set DMA mask for table walker\n");
+
+	if (!smmu->ias)
+		smmu->ias = smmu->oas;
+
+	dev_info(smmu->dev, "ias %lu-bit, oas %lu-bit (features 0x%08x)\n",
+		 smmu->ias, smmu->oas, smmu->features);
+	return 0;
+}
+
+static int arm_smmu_device_dt_probe(struct platform_device *pdev)
+{
+	int irq, ret;
+	struct resource *res;
+	struct arm_smmu_device *smmu;
+	struct device *dev = &pdev->dev;
+
+	smmu = devm_kzalloc(dev, sizeof(*smmu), GFP_KERNEL);
+	if (!smmu) {
+		dev_err(dev, "failed to allocate arm_smmu_device\n");
+		return -ENOMEM;
+	}
+	smmu->dev = dev;
+
+	/* Base address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (resource_size(res) + 1 < SZ_128K) {
+		dev_err(dev, "MMIO region too small (%pr)\n", res);
+		return -EINVAL;
+	}
+
+	smmu->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(smmu->base))
+		return PTR_ERR(smmu->base);
+
+	/* Interrupt lines */
+	irq = platform_get_irq_byname(pdev, "eventq");
+	if (irq > 0)
+		smmu->evtq.q.irq = irq;
+
+	irq = platform_get_irq_byname(pdev, "priq");
+	if (irq > 0)
+		smmu->priq.q.irq = irq;
+
+	irq = platform_get_irq_byname(pdev, "cmdq-sync");
+	if (irq > 0)
+		smmu->cmdq.q.irq = irq;
+
+	irq = platform_get_irq_byname(pdev, "gerror");
+	if (irq > 0)
+		smmu->gerr_irq = irq;
+
+	/* Probe the h/w */
+	ret = arm_smmu_device_probe(smmu);
+	if (ret)
+		return ret;
+
+	/* Initialise in-memory data structures */
+	ret = arm_smmu_init_structures(smmu);
+	if (ret)
+		return ret;
+
+	/* Reset the device */
+	ret = arm_smmu_device_reset(smmu);
+	if (ret)
+		goto out_free_structures;
+
+	/* Record our private device structure */
+	INIT_LIST_HEAD(&smmu->list);
+	spin_lock(&arm_smmu_devices_lock);
+	list_add(&smmu->list, &arm_smmu_devices);
+	spin_unlock(&arm_smmu_devices_lock);
+	return 0;
+
+out_free_structures:
+	arm_smmu_free_structures(smmu);
+	return ret;
+}
+
+static int arm_smmu_device_remove(struct platform_device *pdev)
+{
+	struct arm_smmu_device *curr, *smmu = NULL;
+	struct device *dev = &pdev->dev;
+
+	spin_lock(&arm_smmu_devices_lock);
+	list_for_each_entry(curr, &arm_smmu_devices, list) {
+		if (curr->dev == dev) {
+			smmu = curr;
+			list_del(&smmu->list);
+			break;
+		}
+	}
+	spin_unlock(&arm_smmu_devices_lock);
+
+	if (!smmu)
+		return -ENODEV;
+
+	arm_smmu_device_disable(smmu);
+	arm_smmu_free_structures(smmu);
+	return 0;
+}
+
+static struct of_device_id arm_smmu_of_match[] = {
+	{ .compatible = "arm,smmu-v3", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, arm_smmu_of_match);
+
+static struct platform_driver arm_smmu_driver = {
+	.driver	= {
+		.name		= "arm-smmu-v3",
+		.of_match_table	= of_match_ptr(arm_smmu_of_match),
+	},
+	.probe	= arm_smmu_device_dt_probe,
+	.remove	= arm_smmu_device_remove,
+};
+
+static int __init arm_smmu_init(void)
+{
+	struct device_node *np;
+	int ret;
+
+	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;
+
+	return bus_set_iommu(&pci_bus_type, &arm_smmu_ops);
+}
+
+static void __exit arm_smmu_exit(void)
+{
+	return platform_driver_unregister(&arm_smmu_driver);
+}
+
+subsys_initcall(arm_smmu_init);
+module_exit(arm_smmu_exit);
+
+MODULE_DESCRIPTION("IOMMU API for ARM architected SMMUv3 implementations");
+MODULE_AUTHOR("Will Deacon <will.deacon@arm.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/arm-smmu.c b/drivers/iommu/arm-smmu.c
index 66a803b9dd3a..4cd0c29cb585 100644
--- a/drivers/iommu/arm-smmu.c
+++ b/drivers/iommu/arm-smmu.c
@@ -202,8 +202,7 @@
 #define ARM_SMMU_CB_S1_TLBIVAL		0x620
 #define ARM_SMMU_CB_S2_TLBIIPAS2	0x630
 #define ARM_SMMU_CB_S2_TLBIIPAS2L	0x638
-#define ARM_SMMU_CB_ATS1PR_LO		0x800
-#define ARM_SMMU_CB_ATS1PR_HI		0x804
+#define ARM_SMMU_CB_ATS1PR		0x800
 #define ARM_SMMU_CB_ATSR		0x8f0
 
 #define SCTLR_S1_ASIDPNE		(1 << 12)
@@ -247,7 +246,7 @@
 #define FSYNR0_WNR			(1 << 4)
 
 static int force_stage;
-module_param_named(force_stage, force_stage, int, S_IRUGO | S_IWUSR);
+module_param_named(force_stage, force_stage, int, S_IRUGO);
 MODULE_PARM_DESC(force_stage,
 	"Force SMMU mappings to be installed at a particular stage of translation. A value of '1' or '2' forces the corresponding stage. All other values are ignored (i.e. no stage is forced). Note that selecting a specific stage will disable support for nested translation.");
 
@@ -1229,18 +1228,18 @@ static phys_addr_t arm_smmu_iova_to_phys_hard(struct iommu_domain *domain,
 	void __iomem *cb_base;
 	u32 tmp;
 	u64 phys;
+	unsigned long va;
 
 	cb_base = ARM_SMMU_CB_BASE(smmu) + ARM_SMMU_CB(smmu, cfg->cbndx);
 
-	if (smmu->version == 1) {
-		u32 reg = iova & ~0xfff;
-		writel_relaxed(reg, cb_base + ARM_SMMU_CB_ATS1PR_LO);
-	} else {
-		u32 reg = iova & ~0xfff;
-		writel_relaxed(reg, cb_base + ARM_SMMU_CB_ATS1PR_LO);
-		reg = ((u64)iova & ~0xfff) >> 32;
-		writel_relaxed(reg, cb_base + ARM_SMMU_CB_ATS1PR_HI);
-	}
+	/* ATS1 registers can only be written atomically */
+	va = iova & ~0xfffUL;
+#ifdef CONFIG_64BIT
+	if (smmu->version == ARM_SMMU_V2)
+		writeq_relaxed(va, cb_base + ARM_SMMU_CB_ATS1PR);
+	else
+#endif
+		writel_relaxed(va, cb_base + ARM_SMMU_CB_ATS1PR);
 
 	if (readl_poll_timeout_atomic(cb_base + ARM_SMMU_CB_ATSR, tmp,
 				      !(tmp & ATSR_ACTIVE), 5, 50)) {
@@ -1567,7 +1566,7 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 		return -ENODEV;
 	}
 
-	if ((id & ID0_S1TS) && ((smmu->version == 1) || (id & ID0_ATOSNS))) {
+	if ((id & ID0_S1TS) && ((smmu->version == 1) || !(id & ID0_ATOSNS))) {
 		smmu->features |= ARM_SMMU_FEAT_TRANS_OPS;
 		dev_notice(smmu->dev, "\taddress translation ops\n");
 	}
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index 9847613085e1..c9db04d4ef39 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -26,7 +26,7 @@
  * These routines are used by both DMA-remapping and Interrupt-remapping
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt /* has to precede printk.h */
+#define pr_fmt(fmt)     "DMAR: " fmt
 
 #include <linux/pci.h>
 #include <linux/dmar.h>
@@ -555,7 +555,7 @@ static int dmar_walk_remapping_entries(struct acpi_dmar_header *start,
 			break;
 		} else if (next > end) {
 			/* Avoid passing table end */
-			pr_warn(FW_BUG "record passes table end\n");
+			pr_warn(FW_BUG "Record passes table end\n");
 			ret = -EINVAL;
 			break;
 		}
@@ -802,7 +802,7 @@ int __init dmar_table_init(void)
 		ret = parse_dmar_table();
 		if (ret < 0) {
 			if (ret != -ENODEV)
-				pr_info("parse DMAR table failure.\n");
+				pr_info("Parse DMAR table failure.\n");
 		} else  if (list_empty(&dmar_drhd_units)) {
 			pr_info("No DMAR devices found\n");
 			ret = -ENODEV;
@@ -847,7 +847,7 @@ dmar_validate_one_drhd(struct acpi_dmar_header *entry, void *arg)
 	else
 		addr = early_ioremap(drhd->address, VTD_PAGE_SIZE);
 	if (!addr) {
-		pr_warn("IOMMU: can't validate: %llx\n", drhd->address);
+		pr_warn("Can't validate DRHD address: %llx\n", drhd->address);
 		return -EINVAL;
 	}
 
@@ -921,14 +921,14 @@ static int map_iommu(struct intel_iommu *iommu, u64 phys_addr)
 	iommu->reg_size = VTD_PAGE_SIZE;
 
 	if (!request_mem_region(iommu->reg_phys, iommu->reg_size, iommu->name)) {
-		pr_err("IOMMU: can't reserve memory\n");
+		pr_err("Can't reserve memory\n");
 		err = -EBUSY;
 		goto out;
 	}
 
 	iommu->reg = ioremap(iommu->reg_phys, iommu->reg_size);
 	if (!iommu->reg) {
-		pr_err("IOMMU: can't map the region\n");
+		pr_err("Can't map the region\n");
 		err = -ENOMEM;
 		goto release;
 	}
@@ -952,13 +952,13 @@ static int map_iommu(struct intel_iommu *iommu, u64 phys_addr)
 		iommu->reg_size = map_size;
 		if (!request_mem_region(iommu->reg_phys, iommu->reg_size,
 					iommu->name)) {
-			pr_err("IOMMU: can't reserve memory\n");
+			pr_err("Can't reserve memory\n");
 			err = -EBUSY;
 			goto out;
 		}
 		iommu->reg = ioremap(iommu->reg_phys, iommu->reg_size);
 		if (!iommu->reg) {
-			pr_err("IOMMU: can't map the region\n");
+			pr_err("Can't map the region\n");
 			err = -ENOMEM;
 			goto release;
 		}
@@ -1014,14 +1014,14 @@ static int alloc_iommu(struct dmar_drhd_unit *drhd)
 		return -ENOMEM;
 
 	if (dmar_alloc_seq_id(iommu) < 0) {
-		pr_err("IOMMU: failed to allocate seq_id\n");
+		pr_err("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);
+		pr_err("Failed to map %s\n", iommu->name);
 		goto error_free_seq_id;
 	}
 
@@ -1045,8 +1045,8 @@ static int alloc_iommu(struct dmar_drhd_unit *drhd)
 	iommu->node = -1;
 
 	ver = readl(iommu->reg + DMAR_VER_REG);
-	pr_info("IOMMU %d: reg_base_addr %llx ver %d:%d cap %llx ecap %llx\n",
-		iommu->seq_id,
+	pr_info("%s: reg_base_addr %llx ver %d:%d cap %llx ecap %llx\n",
+		iommu->name,
 		(unsigned long long)drhd->reg_base_addr,
 		DMAR_VER_MAJOR(ver), DMAR_VER_MINOR(ver),
 		(unsigned long long)iommu->cap,
@@ -1087,8 +1087,8 @@ static void free_iommu(struct intel_iommu *iommu)
 
 	if (iommu->irq) {
 		free_irq(iommu->irq, iommu);
-		irq_set_handler_data(iommu->irq, NULL);
 		dmar_free_hwirq(iommu->irq);
+		iommu->irq = 0;
 	}
 
 	if (iommu->qi) {
@@ -1642,26 +1642,17 @@ int dmar_set_interrupt(struct intel_iommu *iommu)
 	if (iommu->irq)
 		return 0;
 
-	irq = dmar_alloc_hwirq();
-	if (irq <= 0) {
-		pr_err("IOMMU: no free vectors\n");
+	irq = dmar_alloc_hwirq(iommu->seq_id, iommu->node, iommu);
+	if (irq > 0) {
+		iommu->irq = irq;
+	} else {
+		pr_err("No free IRQ vectors\n");
 		return -EINVAL;
 	}
 
-	irq_set_handler_data(irq, iommu);
-	iommu->irq = irq;
-
-	ret = arch_setup_dmar_msi(irq);
-	if (ret) {
-		irq_set_handler_data(irq, NULL);
-		iommu->irq = 0;
-		dmar_free_hwirq(irq);
-		return ret;
-	}
-
 	ret = request_irq(irq, dmar_fault, IRQF_NO_THREAD, iommu->name, iommu);
 	if (ret)
-		pr_err("IOMMU: can't request irq\n");
+		pr_err("Can't request irq\n");
 	return ret;
 }
 
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 3e898504a7c4..97c41b8ab5d9 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -12,21 +12,22 @@
 #define DEBUG
 #endif
 
-#include <linux/io.h>
-#include <linux/interrupt.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include <linux/pm_runtime.h>
 #include <linux/clk.h>
+#include <linux/dma-mapping.h>
 #include <linux/err.h>
-#include <linux/mm.h>
+#include <linux/io.h>
 #include <linux/iommu.h>
-#include <linux/errno.h>
+#include <linux/interrupt.h>
 #include <linux/list.h>
-#include <linux/memblock.h>
-#include <linux/export.h>
+#include <linux/of.h>
+#include <linux/of_iommu.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
 
 #include <asm/cacheflush.h>
+#include <asm/dma-iommu.h>
 #include <asm/pgtable.h>
 
 typedef u32 sysmmu_iova_t;
@@ -184,35 +185,50 @@ static char *sysmmu_fault_name[SYSMMU_FAULTS_NUM] = {
 	"UNKNOWN FAULT"
 };
 
-/* attached to dev.archdata.iommu of the master device */
+/*
+ * This structure is attached to dev.archdata.iommu of the master device
+ * on device add, contains a list of SYSMMU controllers defined by device tree,
+ * which are bound to given master device. It is usually referenced by 'owner'
+ * pointer.
+*/
 struct exynos_iommu_owner {
-	struct list_head client; /* entry of exynos_iommu_domain.clients */
-	struct device *dev;
-	struct device *sysmmu;
-	struct iommu_domain *domain;
-	void *vmm_data;         /* IO virtual memory manager's data */
-	spinlock_t lock;        /* Lock to preserve consistency of System MMU */
+	struct list_head controllers;	/* list of sysmmu_drvdata.owner_node */
 };
 
+/*
+ * This structure exynos specific generalization of struct iommu_domain.
+ * It contains list of SYSMMU controllers from all master devices, which has
+ * been attached to this domain and page tables of IO address space defined by
+ * it. It is usually referenced by 'domain' pointer.
+ */
 struct exynos_iommu_domain {
-	struct list_head clients; /* list of sysmmu_drvdata.node */
-	sysmmu_pte_t *pgtable; /* lv1 page table, 16KB */
-	short *lv2entcnt; /* free lv2 entry counter for each section */
-	spinlock_t lock; /* lock for this structure */
-	spinlock_t pgtablelock; /* lock for modifying page table @ pgtable */
+	struct list_head clients; /* list of sysmmu_drvdata.domain_node */
+	sysmmu_pte_t *pgtable;	/* lv1 page table, 16KB */
+	short *lv2entcnt;	/* free lv2 entry counter for each section */
+	spinlock_t lock;	/* lock for modyfying list of clients */
+	spinlock_t pgtablelock;	/* lock for modifying page table @ pgtable */
 	struct iommu_domain domain; /* generic domain data structure */
 };
 
+/*
+ * This structure hold all data of a single SYSMMU controller, this includes
+ * hw resources like registers and clocks, pointers and list nodes to connect
+ * it to all other structures, internal state and parameters read from device
+ * tree. It is usually referenced by 'data' pointer.
+ */
 struct sysmmu_drvdata {
-	struct device *sysmmu;	/* System MMU's device descriptor */
-	struct device *master;	/* Owner of system MMU */
-	void __iomem *sfrbase;
-	struct clk *clk;
-	struct clk *clk_master;
-	int activations;
-	spinlock_t lock;
-	struct iommu_domain *domain;
-	phys_addr_t pgtable;
+	struct device *sysmmu;		/* SYSMMU controller device */
+	struct device *master;		/* master device (owner) */
+	void __iomem *sfrbase;		/* our registers */
+	struct clk *clk;		/* SYSMMU's clock */
+	struct clk *clk_master;		/* master's device clock */
+	int activations;		/* number of calls to sysmmu_enable */
+	spinlock_t lock;		/* lock for modyfying state */
+	struct exynos_iommu_domain *domain; /* domain we belong to */
+	struct list_head domain_node;	/* node for domain clients list */
+	struct list_head owner_node;	/* node for owner controllers list */
+	phys_addr_t pgtable;		/* assigned page table structure */
+	unsigned int version;		/* our version */
 };
 
 static struct exynos_iommu_domain *to_exynos_domain(struct iommu_domain *dom)
@@ -244,11 +260,6 @@ static void sysmmu_unblock(void __iomem *sfrbase)
 	__raw_writel(CTRL_ENABLE, sfrbase + REG_MMU_CTRL);
 }
 
-static unsigned int __raw_sysmmu_version(struct sysmmu_drvdata *data)
-{
-	return MMU_RAW_VER(__raw_readl(data->sfrbase + REG_MMU_VERSION));
-}
-
 static bool sysmmu_block(void __iomem *sfrbase)
 {
 	int i = 120;
@@ -345,7 +356,7 @@ static irqreturn_t exynos_sysmmu_irq(int irq, void *dev_id)
 		show_fault_information(dev_name(data->sysmmu),
 					itype, base, addr);
 		if (data->domain)
-			ret = report_iommu_fault(data->domain,
+			ret = report_iommu_fault(&data->domain->domain,
 					data->master, addr, itype);
 	}
 
@@ -408,7 +419,7 @@ static void __sysmmu_init_config(struct sysmmu_drvdata *data)
 	unsigned int cfg = CFG_LRU | CFG_QOS(15);
 	unsigned int ver;
 
-	ver = __raw_sysmmu_version(data);
+	ver = MMU_RAW_VER(__raw_readl(data->sfrbase + REG_MMU_VERSION));
 	if (MMU_MAJ_VER(ver) == 3) {
 		if (MMU_MIN_VER(ver) >= 2) {
 			cfg |= CFG_FLPDCACHE;
@@ -422,6 +433,7 @@ static void __sysmmu_init_config(struct sysmmu_drvdata *data)
 	}
 
 	__raw_writel(cfg, data->sfrbase + REG_MMU_CFG);
+	data->version = ver;
 }
 
 static void __sysmmu_enable_nocount(struct sysmmu_drvdata *data)
@@ -442,8 +454,8 @@ static void __sysmmu_enable_nocount(struct sysmmu_drvdata *data)
 		clk_disable(data->clk_master);
 }
 
-static int __sysmmu_enable(struct sysmmu_drvdata *data,
-			phys_addr_t pgtable, struct iommu_domain *domain)
+static int __sysmmu_enable(struct sysmmu_drvdata *data, phys_addr_t pgtable,
+			   struct exynos_iommu_domain *domain)
 {
 	int ret = 0;
 	unsigned long flags;
@@ -470,77 +482,17 @@ static int __sysmmu_enable(struct sysmmu_drvdata *data,
 	return ret;
 }
 
-/* __exynos_sysmmu_enable: Enables System MMU
- *
- * returns -error if an error occurred and System MMU is not enabled,
- * 0 if the System MMU has been just enabled and 1 if System MMU was already
- * enabled before.
- */
-static int __exynos_sysmmu_enable(struct device *dev, phys_addr_t pgtable,
-				  struct iommu_domain *domain)
-{
-	int ret = 0;
-	unsigned long flags;
-	struct exynos_iommu_owner *owner = dev->archdata.iommu;
-	struct sysmmu_drvdata *data;
-
-	BUG_ON(!has_sysmmu(dev));
-
-	spin_lock_irqsave(&owner->lock, flags);
-
-	data = dev_get_drvdata(owner->sysmmu);
-
-	ret = __sysmmu_enable(data, pgtable, domain);
-	if (ret >= 0)
-		data->master = dev;
-
-	spin_unlock_irqrestore(&owner->lock, flags);
-
-	return ret;
-}
-
-int exynos_sysmmu_enable(struct device *dev, phys_addr_t pgtable)
-{
-	BUG_ON(!memblock_is_memory(pgtable));
-
-	return __exynos_sysmmu_enable(dev, pgtable, NULL);
-}
-
-static bool exynos_sysmmu_disable(struct device *dev)
-{
-	unsigned long flags;
-	bool disabled = true;
-	struct exynos_iommu_owner *owner = dev->archdata.iommu;
-	struct sysmmu_drvdata *data;
-
-	BUG_ON(!has_sysmmu(dev));
-
-	spin_lock_irqsave(&owner->lock, flags);
-
-	data = dev_get_drvdata(owner->sysmmu);
-
-	disabled = __sysmmu_disable(data);
-	if (disabled)
-		data->master = NULL;
-
-	spin_unlock_irqrestore(&owner->lock, flags);
-
-	return disabled;
-}
-
 static void __sysmmu_tlb_invalidate_flpdcache(struct sysmmu_drvdata *data,
 					      sysmmu_iova_t iova)
 {
-	if (__raw_sysmmu_version(data) == MAKE_MMU_VER(3, 3))
+	if (data->version == MAKE_MMU_VER(3, 3))
 		__raw_writel(iova | 0x1, data->sfrbase + REG_MMU_FLUSH_ENTRY);
 }
 
-static void sysmmu_tlb_invalidate_flpdcache(struct device *dev,
+static void sysmmu_tlb_invalidate_flpdcache(struct sysmmu_drvdata *data,
 					    sysmmu_iova_t iova)
 {
 	unsigned long flags;
-	struct exynos_iommu_owner *owner = dev->archdata.iommu;
-	struct sysmmu_drvdata *data = dev_get_drvdata(owner->sysmmu);
 
 	if (!IS_ERR(data->clk_master))
 		clk_enable(data->clk_master);
@@ -554,14 +506,10 @@ static void sysmmu_tlb_invalidate_flpdcache(struct device *dev,
 		clk_disable(data->clk_master);
 }
 
-static void sysmmu_tlb_invalidate_entry(struct device *dev, sysmmu_iova_t iova,
-					size_t size)
+static void sysmmu_tlb_invalidate_entry(struct sysmmu_drvdata *data,
+					sysmmu_iova_t iova, size_t size)
 {
-	struct exynos_iommu_owner *owner = dev->archdata.iommu;
 	unsigned long flags;
-	struct sysmmu_drvdata *data;
-
-	data = dev_get_drvdata(owner->sysmmu);
 
 	spin_lock_irqsave(&data->lock, flags);
 	if (is_sysmmu_active(data)) {
@@ -580,7 +528,7 @@ static void sysmmu_tlb_invalidate_entry(struct device *dev, sysmmu_iova_t iova,
 		 * 1MB page can be cached in one of all sets.
 		 * 64KB page can be one of 16 consecutive sets.
 		 */
-		if (MMU_MAJ_VER(__raw_sysmmu_version(data)) == 2)
+		if (MMU_MAJ_VER(data->version) == 2)
 			num_inv = min_t(unsigned int, size / PAGE_SIZE, 64);
 
 		if (sysmmu_block(data->sfrbase)) {
@@ -591,32 +539,8 @@ static void sysmmu_tlb_invalidate_entry(struct device *dev, sysmmu_iova_t iova,
 		if (!IS_ERR(data->clk_master))
 			clk_disable(data->clk_master);
 	} else {
-		dev_dbg(dev, "disabled. Skipping TLB invalidation @ %#x\n",
-			iova);
-	}
-	spin_unlock_irqrestore(&data->lock, flags);
-}
-
-void exynos_sysmmu_tlb_invalidate(struct device *dev)
-{
-	struct exynos_iommu_owner *owner = dev->archdata.iommu;
-	unsigned long flags;
-	struct sysmmu_drvdata *data;
-
-	data = dev_get_drvdata(owner->sysmmu);
-
-	spin_lock_irqsave(&data->lock, flags);
-	if (is_sysmmu_active(data)) {
-		if (!IS_ERR(data->clk_master))
-			clk_enable(data->clk_master);
-		if (sysmmu_block(data->sfrbase)) {
-			__sysmmu_tlb_invalidate(data->sfrbase);
-			sysmmu_unblock(data->sfrbase);
-		}
-		if (!IS_ERR(data->clk_master))
-			clk_disable(data->clk_master);
-	} else {
-		dev_dbg(dev, "disabled. Skipping TLB invalidation\n");
+		dev_dbg(data->master,
+			"disabled. Skipping TLB invalidation @ %#x\n", iova);
 	}
 	spin_unlock_irqrestore(&data->lock, flags);
 }
@@ -682,6 +606,36 @@ static int __init exynos_sysmmu_probe(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int exynos_sysmmu_suspend(struct device *dev)
+{
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "suspend\n");
+	if (is_sysmmu_active(data)) {
+		__sysmmu_disable_nocount(data);
+		pm_runtime_put(dev);
+	}
+	return 0;
+}
+
+static int exynos_sysmmu_resume(struct device *dev)
+{
+	struct sysmmu_drvdata *data = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "resume\n");
+	if (is_sysmmu_active(data)) {
+		pm_runtime_get_sync(dev);
+		__sysmmu_enable_nocount(data);
+	}
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops sysmmu_pm_ops = {
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(exynos_sysmmu_suspend, exynos_sysmmu_resume)
+};
+
 static const struct of_device_id sysmmu_of_match[] __initconst = {
 	{ .compatible	= "samsung,exynos-sysmmu", },
 	{ },
@@ -692,6 +646,7 @@ static struct platform_driver exynos_sysmmu_driver __refdata = {
 	.driver	= {
 		.name		= "exynos-sysmmu",
 		.of_match_table	= sysmmu_of_match,
+		.pm		= &sysmmu_pm_ops,
 	}
 };
 
@@ -704,104 +659,108 @@ static inline void pgtable_flush(void *vastart, void *vaend)
 
 static struct iommu_domain *exynos_iommu_domain_alloc(unsigned type)
 {
-	struct exynos_iommu_domain *exynos_domain;
+	struct exynos_iommu_domain *domain;
 	int i;
 
 	if (type != IOMMU_DOMAIN_UNMANAGED)
 		return NULL;
 
-	exynos_domain = kzalloc(sizeof(*exynos_domain), GFP_KERNEL);
-	if (!exynos_domain)
+	domain = kzalloc(sizeof(*domain), GFP_KERNEL);
+	if (!domain)
 		return NULL;
 
-	exynos_domain->pgtable = (sysmmu_pte_t *)__get_free_pages(GFP_KERNEL, 2);
-	if (!exynos_domain->pgtable)
+	domain->pgtable = (sysmmu_pte_t *)__get_free_pages(GFP_KERNEL, 2);
+	if (!domain->pgtable)
 		goto err_pgtable;
 
-	exynos_domain->lv2entcnt = (short *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, 1);
-	if (!exynos_domain->lv2entcnt)
+	domain->lv2entcnt = (short *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, 1);
+	if (!domain->lv2entcnt)
 		goto err_counter;
 
 	/* Workaround for System MMU v3.3 to prevent caching 1MiB mapping */
 	for (i = 0; i < NUM_LV1ENTRIES; i += 8) {
-		exynos_domain->pgtable[i + 0] = ZERO_LV2LINK;
-		exynos_domain->pgtable[i + 1] = ZERO_LV2LINK;
-		exynos_domain->pgtable[i + 2] = ZERO_LV2LINK;
-		exynos_domain->pgtable[i + 3] = ZERO_LV2LINK;
-		exynos_domain->pgtable[i + 4] = ZERO_LV2LINK;
-		exynos_domain->pgtable[i + 5] = ZERO_LV2LINK;
-		exynos_domain->pgtable[i + 6] = ZERO_LV2LINK;
-		exynos_domain->pgtable[i + 7] = ZERO_LV2LINK;
+		domain->pgtable[i + 0] = ZERO_LV2LINK;
+		domain->pgtable[i + 1] = ZERO_LV2LINK;
+		domain->pgtable[i + 2] = ZERO_LV2LINK;
+		domain->pgtable[i + 3] = ZERO_LV2LINK;
+		domain->pgtable[i + 4] = ZERO_LV2LINK;
+		domain->pgtable[i + 5] = ZERO_LV2LINK;
+		domain->pgtable[i + 6] = ZERO_LV2LINK;
+		domain->pgtable[i + 7] = ZERO_LV2LINK;
 	}
 
-	pgtable_flush(exynos_domain->pgtable, exynos_domain->pgtable + NUM_LV1ENTRIES);
+	pgtable_flush(domain->pgtable, domain->pgtable + NUM_LV1ENTRIES);
 
-	spin_lock_init(&exynos_domain->lock);
-	spin_lock_init(&exynos_domain->pgtablelock);
-	INIT_LIST_HEAD(&exynos_domain->clients);
+	spin_lock_init(&domain->lock);
+	spin_lock_init(&domain->pgtablelock);
+	INIT_LIST_HEAD(&domain->clients);
 
-	exynos_domain->domain.geometry.aperture_start = 0;
-	exynos_domain->domain.geometry.aperture_end   = ~0UL;
-	exynos_domain->domain.geometry.force_aperture = true;
+	domain->domain.geometry.aperture_start = 0;
+	domain->domain.geometry.aperture_end   = ~0UL;
+	domain->domain.geometry.force_aperture = true;
 
-	return &exynos_domain->domain;
+	return &domain->domain;
 
 err_counter:
-	free_pages((unsigned long)exynos_domain->pgtable, 2);
+	free_pages((unsigned long)domain->pgtable, 2);
 err_pgtable:
-	kfree(exynos_domain);
+	kfree(domain);
 	return NULL;
 }
 
-static void exynos_iommu_domain_free(struct iommu_domain *domain)
+static void exynos_iommu_domain_free(struct iommu_domain *iommu_domain)
 {
-	struct exynos_iommu_domain *priv = to_exynos_domain(domain);
-	struct exynos_iommu_owner *owner;
+	struct exynos_iommu_domain *domain = to_exynos_domain(iommu_domain);
+	struct sysmmu_drvdata *data, *next;
 	unsigned long flags;
 	int i;
 
-	WARN_ON(!list_empty(&priv->clients));
+	WARN_ON(!list_empty(&domain->clients));
 
-	spin_lock_irqsave(&priv->lock, flags);
+	spin_lock_irqsave(&domain->lock, flags);
 
-	list_for_each_entry(owner, &priv->clients, client) {
-		while (!exynos_sysmmu_disable(owner->dev))
-			; /* until System MMU is actually disabled */
+	list_for_each_entry_safe(data, next, &domain->clients, domain_node) {
+		if (__sysmmu_disable(data))
+			data->master = NULL;
+		list_del_init(&data->domain_node);
 	}
 
-	while (!list_empty(&priv->clients))
-		list_del_init(priv->clients.next);
-
-	spin_unlock_irqrestore(&priv->lock, flags);
+	spin_unlock_irqrestore(&domain->lock, flags);
 
 	for (i = 0; i < NUM_LV1ENTRIES; i++)
-		if (lv1ent_page(priv->pgtable + i))
+		if (lv1ent_page(domain->pgtable + i))
 			kmem_cache_free(lv2table_kmem_cache,
-				phys_to_virt(lv2table_base(priv->pgtable + i)));
+				phys_to_virt(lv2table_base(domain->pgtable + i)));
 
-	free_pages((unsigned long)priv->pgtable, 2);
-	free_pages((unsigned long)priv->lv2entcnt, 1);
-	kfree(priv);
+	free_pages((unsigned long)domain->pgtable, 2);
+	free_pages((unsigned long)domain->lv2entcnt, 1);
+	kfree(domain);
 }
 
-static int exynos_iommu_attach_device(struct iommu_domain *domain,
+static int exynos_iommu_attach_device(struct iommu_domain *iommu_domain,
 				   struct device *dev)
 {
 	struct exynos_iommu_owner *owner = dev->archdata.iommu;
-	struct exynos_iommu_domain *priv = to_exynos_domain(domain);
-	phys_addr_t pagetable = virt_to_phys(priv->pgtable);
+	struct exynos_iommu_domain *domain = to_exynos_domain(iommu_domain);
+	struct sysmmu_drvdata *data;
+	phys_addr_t pagetable = virt_to_phys(domain->pgtable);
 	unsigned long flags;
-	int ret;
+	int ret = -ENODEV;
 
-	spin_lock_irqsave(&priv->lock, flags);
+	if (!has_sysmmu(dev))
+		return -ENODEV;
 
-	ret = __exynos_sysmmu_enable(dev, pagetable, domain);
-	if (ret == 0) {
-		list_add_tail(&owner->client, &priv->clients);
-		owner->domain = domain;
-	}
+	list_for_each_entry(data, &owner->controllers, owner_node) {
+		pm_runtime_get_sync(data->sysmmu);
+		ret = __sysmmu_enable(data, pagetable, domain);
+		if (ret >= 0) {
+			data->master = dev;
 
-	spin_unlock_irqrestore(&priv->lock, flags);
+			spin_lock_irqsave(&domain->lock, flags);
+			list_add_tail(&data->domain_node, &domain->clients);
+			spin_unlock_irqrestore(&domain->lock, flags);
+		}
+	}
 
 	if (ret < 0) {
 		dev_err(dev, "%s: Failed to attach IOMMU with pgtable %pa\n",
@@ -815,36 +774,39 @@ static int exynos_iommu_attach_device(struct iommu_domain *domain,
 	return ret;
 }
 
-static void exynos_iommu_detach_device(struct iommu_domain *domain,
+static void exynos_iommu_detach_device(struct iommu_domain *iommu_domain,
 				    struct device *dev)
 {
-	struct exynos_iommu_owner *owner;
-	struct exynos_iommu_domain *priv = to_exynos_domain(domain);
-	phys_addr_t pagetable = virt_to_phys(priv->pgtable);
+	struct exynos_iommu_domain *domain = to_exynos_domain(iommu_domain);
+	phys_addr_t pagetable = virt_to_phys(domain->pgtable);
+	struct sysmmu_drvdata *data, *next;
 	unsigned long flags;
+	bool found = false;
 
-	spin_lock_irqsave(&priv->lock, flags);
+	if (!has_sysmmu(dev))
+		return;
 
-	list_for_each_entry(owner, &priv->clients, client) {
-		if (owner == dev->archdata.iommu) {
-			if (exynos_sysmmu_disable(dev)) {
-				list_del_init(&owner->client);
-				owner->domain = NULL;
+	spin_lock_irqsave(&domain->lock, flags);
+	list_for_each_entry_safe(data, next, &domain->clients, domain_node) {
+		if (data->master == dev) {
+			if (__sysmmu_disable(data)) {
+				data->master = NULL;
+				list_del_init(&data->domain_node);
 			}
-			break;
+			pm_runtime_put(data->sysmmu);
+			found = true;
 		}
 	}
+	spin_unlock_irqrestore(&domain->lock, flags);
 
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	if (owner == dev->archdata.iommu)
+	if (found)
 		dev_dbg(dev, "%s: Detached IOMMU with pgtable %pa\n",
 					__func__, &pagetable);
 	else
 		dev_err(dev, "%s: No IOMMU is attached\n", __func__);
 }
 
-static sysmmu_pte_t *alloc_lv2entry(struct exynos_iommu_domain *priv,
+static sysmmu_pte_t *alloc_lv2entry(struct exynos_iommu_domain *domain,
 		sysmmu_pte_t *sent, sysmmu_iova_t iova, short *pgcounter)
 {
 	if (lv1ent_section(sent)) {
@@ -862,6 +824,7 @@ static sysmmu_pte_t *alloc_lv2entry(struct exynos_iommu_domain *priv,
 			return ERR_PTR(-ENOMEM);
 
 		*sent = mk_lv1ent_page(virt_to_phys(pent));
+		kmemleak_ignore(pent);
 		*pgcounter = NUM_LV2ENTRIES;
 		pgtable_flush(pent, pent + NUM_LV2ENTRIES);
 		pgtable_flush(sent, sent + 1);
@@ -884,20 +847,19 @@ static sysmmu_pte_t *alloc_lv2entry(struct exynos_iommu_domain *priv,
 		 * not currently mapped.
 		 */
 		if (need_flush_flpd_cache) {
-			struct exynos_iommu_owner *owner;
+			struct sysmmu_drvdata *data;
 
-			spin_lock(&priv->lock);
-			list_for_each_entry(owner, &priv->clients, client)
-				sysmmu_tlb_invalidate_flpdcache(
-							owner->dev, iova);
-			spin_unlock(&priv->lock);
+			spin_lock(&domain->lock);
+			list_for_each_entry(data, &domain->clients, domain_node)
+				sysmmu_tlb_invalidate_flpdcache(data, iova);
+			spin_unlock(&domain->lock);
 		}
 	}
 
 	return page_entry(sent, iova);
 }
 
-static int lv1set_section(struct exynos_iommu_domain *priv,
+static int lv1set_section(struct exynos_iommu_domain *domain,
 			  sysmmu_pte_t *sent, sysmmu_iova_t iova,
 			  phys_addr_t paddr, short *pgcnt)
 {
@@ -922,17 +884,17 @@ static int lv1set_section(struct exynos_iommu_domain *priv,
 
 	pgtable_flush(sent, sent + 1);
 
-	spin_lock(&priv->lock);
+	spin_lock(&domain->lock);
 	if (lv1ent_page_zero(sent)) {
-		struct exynos_iommu_owner *owner;
+		struct sysmmu_drvdata *data;
 		/*
 		 * Flushing FLPD cache in System MMU v3.3 that may cache a FLPD
 		 * entry by speculative prefetch of SLPD which has no mapping.
 		 */
-		list_for_each_entry(owner, &priv->clients, client)
-			sysmmu_tlb_invalidate_flpdcache(owner->dev, iova);
+		list_for_each_entry(data, &domain->clients, domain_node)
+			sysmmu_tlb_invalidate_flpdcache(data, iova);
 	}
-	spin_unlock(&priv->lock);
+	spin_unlock(&domain->lock);
 
 	return 0;
 }
@@ -992,74 +954,75 @@ static int lv2set_page(sysmmu_pte_t *pent, phys_addr_t paddr, size_t size,
  *   than or equal to 128KiB.
  * - Start address of an I/O virtual region must be aligned by 128KiB.
  */
-static int exynos_iommu_map(struct iommu_domain *domain, unsigned long l_iova,
-			 phys_addr_t paddr, size_t size, int prot)
+static int exynos_iommu_map(struct iommu_domain *iommu_domain,
+			    unsigned long l_iova, phys_addr_t paddr, size_t size,
+			    int prot)
 {
-	struct exynos_iommu_domain *priv = to_exynos_domain(domain);
+	struct exynos_iommu_domain *domain = to_exynos_domain(iommu_domain);
 	sysmmu_pte_t *entry;
 	sysmmu_iova_t iova = (sysmmu_iova_t)l_iova;
 	unsigned long flags;
 	int ret = -ENOMEM;
 
-	BUG_ON(priv->pgtable == NULL);
+	BUG_ON(domain->pgtable == NULL);
 
-	spin_lock_irqsave(&priv->pgtablelock, flags);
+	spin_lock_irqsave(&domain->pgtablelock, flags);
 
-	entry = section_entry(priv->pgtable, iova);
+	entry = section_entry(domain->pgtable, iova);
 
 	if (size == SECT_SIZE) {
-		ret = lv1set_section(priv, entry, iova, paddr,
-					&priv->lv2entcnt[lv1ent_offset(iova)]);
+		ret = lv1set_section(domain, entry, iova, paddr,
+				     &domain->lv2entcnt[lv1ent_offset(iova)]);
 	} else {
 		sysmmu_pte_t *pent;
 
-		pent = alloc_lv2entry(priv, entry, iova,
-					&priv->lv2entcnt[lv1ent_offset(iova)]);
+		pent = alloc_lv2entry(domain, entry, iova,
+				      &domain->lv2entcnt[lv1ent_offset(iova)]);
 
 		if (IS_ERR(pent))
 			ret = PTR_ERR(pent);
 		else
 			ret = lv2set_page(pent, paddr, size,
-					&priv->lv2entcnt[lv1ent_offset(iova)]);
+				       &domain->lv2entcnt[lv1ent_offset(iova)]);
 	}
 
 	if (ret)
 		pr_err("%s: Failed(%d) to map %#zx bytes @ %#x\n",
 			__func__, ret, size, iova);
 
-	spin_unlock_irqrestore(&priv->pgtablelock, flags);
+	spin_unlock_irqrestore(&domain->pgtablelock, flags);
 
 	return ret;
 }
 
-static void exynos_iommu_tlb_invalidate_entry(struct exynos_iommu_domain *priv,
-						sysmmu_iova_t iova, size_t size)
+static void exynos_iommu_tlb_invalidate_entry(struct exynos_iommu_domain *domain,
+					      sysmmu_iova_t iova, size_t size)
 {
-	struct exynos_iommu_owner *owner;
+	struct sysmmu_drvdata *data;
 	unsigned long flags;
 
-	spin_lock_irqsave(&priv->lock, flags);
+	spin_lock_irqsave(&domain->lock, flags);
 
-	list_for_each_entry(owner, &priv->clients, client)
-		sysmmu_tlb_invalidate_entry(owner->dev, iova, size);
+	list_for_each_entry(data, &domain->clients, domain_node)
+		sysmmu_tlb_invalidate_entry(data, iova, size);
 
-	spin_unlock_irqrestore(&priv->lock, flags);
+	spin_unlock_irqrestore(&domain->lock, flags);
 }
 
-static size_t exynos_iommu_unmap(struct iommu_domain *domain,
-					unsigned long l_iova, size_t size)
+static size_t exynos_iommu_unmap(struct iommu_domain *iommu_domain,
+				 unsigned long l_iova, size_t size)
 {
-	struct exynos_iommu_domain *priv = to_exynos_domain(domain);
+	struct exynos_iommu_domain *domain = to_exynos_domain(iommu_domain);
 	sysmmu_iova_t iova = (sysmmu_iova_t)l_iova;
 	sysmmu_pte_t *ent;
 	size_t err_pgsize;
 	unsigned long flags;
 
-	BUG_ON(priv->pgtable == NULL);
+	BUG_ON(domain->pgtable == NULL);
 
-	spin_lock_irqsave(&priv->pgtablelock, flags);
+	spin_lock_irqsave(&domain->pgtablelock, flags);
 
-	ent = section_entry(priv->pgtable, iova);
+	ent = section_entry(domain->pgtable, iova);
 
 	if (lv1ent_section(ent)) {
 		if (WARN_ON(size < SECT_SIZE)) {
@@ -1093,7 +1056,7 @@ static size_t exynos_iommu_unmap(struct iommu_domain *domain,
 		*ent = 0;
 		size = SPAGE_SIZE;
 		pgtable_flush(ent, ent + 1);
-		priv->lv2entcnt[lv1ent_offset(iova)] += 1;
+		domain->lv2entcnt[lv1ent_offset(iova)] += 1;
 		goto done;
 	}
 
@@ -1107,15 +1070,15 @@ static size_t exynos_iommu_unmap(struct iommu_domain *domain,
 	pgtable_flush(ent, ent + SPAGES_PER_LPAGE);
 
 	size = LPAGE_SIZE;
-	priv->lv2entcnt[lv1ent_offset(iova)] += SPAGES_PER_LPAGE;
+	domain->lv2entcnt[lv1ent_offset(iova)] += SPAGES_PER_LPAGE;
 done:
-	spin_unlock_irqrestore(&priv->pgtablelock, flags);
+	spin_unlock_irqrestore(&domain->pgtablelock, flags);
 
-	exynos_iommu_tlb_invalidate_entry(priv, iova, size);
+	exynos_iommu_tlb_invalidate_entry(domain, iova, size);
 
 	return size;
 err:
-	spin_unlock_irqrestore(&priv->pgtablelock, flags);
+	spin_unlock_irqrestore(&domain->pgtablelock, flags);
 
 	pr_err("%s: Failed: size(%#zx) @ %#x is smaller than page size %#zx\n",
 		__func__, size, iova, err_pgsize);
@@ -1123,17 +1086,17 @@ err:
 	return 0;
 }
 
-static phys_addr_t exynos_iommu_iova_to_phys(struct iommu_domain *domain,
+static phys_addr_t exynos_iommu_iova_to_phys(struct iommu_domain *iommu_domain,
 					  dma_addr_t iova)
 {
-	struct exynos_iommu_domain *priv = to_exynos_domain(domain);
+	struct exynos_iommu_domain *domain = to_exynos_domain(iommu_domain);
 	sysmmu_pte_t *entry;
 	unsigned long flags;
 	phys_addr_t phys = 0;
 
-	spin_lock_irqsave(&priv->pgtablelock, flags);
+	spin_lock_irqsave(&domain->pgtablelock, flags);
 
-	entry = section_entry(priv->pgtable, iova);
+	entry = section_entry(domain->pgtable, iova);
 
 	if (lv1ent_section(entry)) {
 		phys = section_phys(entry) + section_offs(iova);
@@ -1146,7 +1109,7 @@ static phys_addr_t exynos_iommu_iova_to_phys(struct iommu_domain *domain,
 			phys = spage_phys(entry) + spage_offs(iova);
 	}
 
-	spin_unlock_irqrestore(&priv->pgtablelock, flags);
+	spin_unlock_irqrestore(&domain->pgtablelock, flags);
 
 	return phys;
 }
@@ -1156,6 +1119,9 @@ static int exynos_iommu_add_device(struct device *dev)
 	struct iommu_group *group;
 	int ret;
 
+	if (!has_sysmmu(dev))
+		return -ENODEV;
+
 	group = iommu_group_get(dev);
 
 	if (!group) {
@@ -1174,10 +1140,40 @@ static int exynos_iommu_add_device(struct device *dev)
 
 static void exynos_iommu_remove_device(struct device *dev)
 {
+	if (!has_sysmmu(dev))
+		return;
+
 	iommu_group_remove_device(dev);
 }
 
-static const struct iommu_ops exynos_iommu_ops = {
+static int exynos_iommu_of_xlate(struct device *dev,
+				 struct of_phandle_args *spec)
+{
+	struct exynos_iommu_owner *owner = dev->archdata.iommu;
+	struct platform_device *sysmmu = of_find_device_by_node(spec->np);
+	struct sysmmu_drvdata *data;
+
+	if (!sysmmu)
+		return -ENODEV;
+
+	data = platform_get_drvdata(sysmmu);
+	if (!data)
+		return -ENODEV;
+
+	if (!owner) {
+		owner = kzalloc(sizeof(*owner), GFP_KERNEL);
+		if (!owner)
+			return -ENOMEM;
+
+		INIT_LIST_HEAD(&owner->controllers);
+		dev->archdata.iommu = owner;
+	}
+
+	list_add_tail(&data->owner_node, &owner->controllers);
+	return 0;
+}
+
+static struct iommu_ops exynos_iommu_ops = {
 	.domain_alloc = exynos_iommu_domain_alloc,
 	.domain_free = exynos_iommu_domain_free,
 	.attach_dev = exynos_iommu_attach_device,
@@ -1189,19 +1185,15 @@ static const struct iommu_ops exynos_iommu_ops = {
 	.add_device = exynos_iommu_add_device,
 	.remove_device = exynos_iommu_remove_device,
 	.pgsize_bitmap = SECT_SIZE | LPAGE_SIZE | SPAGE_SIZE,
+	.of_xlate = exynos_iommu_of_xlate,
 };
 
+static bool init_done;
+
 static int __init exynos_iommu_init(void)
 {
-	struct device_node *np;
 	int ret;
 
-	np = of_find_matching_node(NULL, sysmmu_of_match);
-	if (!np)
-		return 0;
-
-	of_node_put(np);
-
 	lv2table_kmem_cache = kmem_cache_create("exynos-iommu-lv2table",
 				LV2TABLE_SIZE, LV2TABLE_SIZE, 0, NULL);
 	if (!lv2table_kmem_cache) {
@@ -1230,6 +1222,8 @@ static int __init exynos_iommu_init(void)
 		goto err_set_iommu;
 	}
 
+	init_done = true;
+
 	return 0;
 err_set_iommu:
 	kmem_cache_free(lv2table_kmem_cache, zero_lv2_table);
@@ -1239,4 +1233,21 @@ err_reg_driver:
 	kmem_cache_destroy(lv2table_kmem_cache);
 	return ret;
 }
-subsys_initcall(exynos_iommu_init);
+
+static int __init exynos_iommu_of_setup(struct device_node *np)
+{
+	struct platform_device *pdev;
+
+	if (!init_done)
+		exynos_iommu_init();
+
+	pdev = of_platform_device_create(np, NULL, platform_bus_type.dev_root);
+	if (IS_ERR(pdev))
+		return PTR_ERR(pdev);
+
+	of_iommu_set_ops(np, &exynos_iommu_ops);
+	return 0;
+}
+
+IOMMU_OF_DECLARE(exynos_iommu_of, "samsung,exynos-sysmmu",
+		 exynos_iommu_of_setup);
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index 68d43beccb7e..a98a7b27aca1 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -15,8 +15,11 @@
  *          Shaohua Li <shaohua.li@intel.com>,
  *          Anil S Keshavamurthy <anil.s.keshavamurthy@intel.com>,
  *          Fenghua Yu <fenghua.yu@intel.com>
+ *          Joerg Roedel <jroedel@suse.de>
  */
 
+#define pr_fmt(fmt)     "DMAR: " fmt
+
 #include <linux/init.h>
 #include <linux/bitmap.h>
 #include <linux/debugfs.h>
@@ -40,6 +43,7 @@
 #include <linux/pci-ats.h>
 #include <linux/memblock.h>
 #include <linux/dma-contiguous.h>
+#include <linux/crash_dump.h>
 #include <asm/irq_remapping.h>
 #include <asm/cacheflush.h>
 #include <asm/iommu.h>
@@ -190,7 +194,29 @@ struct root_entry {
 };
 #define ROOT_ENTRY_NR (VTD_PAGE_SIZE/sizeof(struct root_entry))
 
+/*
+ * Take a root_entry and return the Lower Context Table Pointer (LCTP)
+ * if marked present.
+ */
+static phys_addr_t root_entry_lctp(struct root_entry *re)
+{
+	if (!(re->lo & 1))
+		return 0;
+
+	return re->lo & VTD_PAGE_MASK;
+}
+
+/*
+ * Take a root_entry and return the Upper Context Table Pointer (UCTP)
+ * if marked present.
+ */
+static phys_addr_t root_entry_uctp(struct root_entry *re)
+{
+	if (!(re->hi & 1))
+		return 0;
 
+	return re->hi & VTD_PAGE_MASK;
+}
 /*
  * low 64 bits:
  * 0: present
@@ -207,10 +233,38 @@ struct context_entry {
 	u64 hi;
 };
 
-static inline bool context_present(struct context_entry *context)
+static inline void context_clear_pasid_enable(struct context_entry *context)
+{
+	context->lo &= ~(1ULL << 11);
+}
+
+static inline bool context_pasid_enabled(struct context_entry *context)
+{
+	return !!(context->lo & (1ULL << 11));
+}
+
+static inline void context_set_copied(struct context_entry *context)
+{
+	context->hi |= (1ull << 3);
+}
+
+static inline bool context_copied(struct context_entry *context)
+{
+	return !!(context->hi & (1ULL << 3));
+}
+
+static inline bool __context_present(struct context_entry *context)
 {
 	return (context->lo & 1);
 }
+
+static inline bool context_present(struct context_entry *context)
+{
+	return context_pasid_enabled(context) ?
+	     __context_present(context) :
+	     __context_present(context) && !context_copied(context);
+}
+
 static inline void context_set_present(struct context_entry *context)
 {
 	context->lo |= 1;
@@ -247,6 +301,11 @@ static inline void context_set_domain_id(struct context_entry *context,
 	context->hi |= (value & ((1 << 16) - 1)) << 8;
 }
 
+static inline int context_domain_id(struct context_entry *c)
+{
+	return((c->hi >> 8) & 0xffff);
+}
+
 static inline void context_clear_entry(struct context_entry *context)
 {
 	context->lo = 0;
@@ -422,6 +481,14 @@ static int dmar_map_gfx = 1;
 static int dmar_forcedac;
 static int intel_iommu_strict;
 static int intel_iommu_superpage = 1;
+static int intel_iommu_ecs = 1;
+
+/* We only actually use ECS when PASID support (on the new bit 40)
+ * is also advertised. Some early implementations — the ones with
+ * PASID support on bit 28 — have issues even when we *only* use
+ * extended root/context tables. */
+#define ecs_enabled(iommu) (intel_iommu_ecs && ecap_ecs(iommu->ecap) && \
+			    ecap_pasid(iommu->ecap))
 
 int intel_iommu_gfx_mapped;
 EXPORT_SYMBOL_GPL(intel_iommu_gfx_mapped);
@@ -432,6 +499,25 @@ static LIST_HEAD(device_domain_list);
 
 static const struct iommu_ops intel_iommu_ops;
 
+static bool translation_pre_enabled(struct intel_iommu *iommu)
+{
+	return (iommu->flags & VTD_FLAG_TRANS_PRE_ENABLED);
+}
+
+static void clear_translation_pre_enabled(struct intel_iommu *iommu)
+{
+	iommu->flags &= ~VTD_FLAG_TRANS_PRE_ENABLED;
+}
+
+static void init_translation_status(struct intel_iommu *iommu)
+{
+	u32 gsts;
+
+	gsts = readl(iommu->reg + DMAR_GSTS_REG);
+	if (gsts & DMA_GSTS_TES)
+		iommu->flags |= VTD_FLAG_TRANS_PRE_ENABLED;
+}
+
 /* Convert generic 'struct iommu_domain to private struct dmar_domain */
 static struct dmar_domain *to_dmar_domain(struct iommu_domain *dom)
 {
@@ -445,26 +531,26 @@ static int __init intel_iommu_setup(char *str)
 	while (*str) {
 		if (!strncmp(str, "on", 2)) {
 			dmar_disabled = 0;
-			printk(KERN_INFO "Intel-IOMMU: enabled\n");
+			pr_info("IOMMU enabled\n");
 		} else if (!strncmp(str, "off", 3)) {
 			dmar_disabled = 1;
-			printk(KERN_INFO "Intel-IOMMU: disabled\n");
+			pr_info("IOMMU disabled\n");
 		} else if (!strncmp(str, "igfx_off", 8)) {
 			dmar_map_gfx = 0;
-			printk(KERN_INFO
-				"Intel-IOMMU: disable GFX device mapping\n");
+			pr_info("Disable GFX device mapping\n");
 		} else if (!strncmp(str, "forcedac", 8)) {
-			printk(KERN_INFO
-				"Intel-IOMMU: Forcing DAC for PCI devices\n");
+			pr_info("Forcing DAC for PCI devices\n");
 			dmar_forcedac = 1;
 		} else if (!strncmp(str, "strict", 6)) {
-			printk(KERN_INFO
-				"Intel-IOMMU: disable batched IOTLB flush\n");
+			pr_info("Disable batched IOTLB flush\n");
 			intel_iommu_strict = 1;
 		} else if (!strncmp(str, "sp_off", 6)) {
-			printk(KERN_INFO
-				"Intel-IOMMU: disable supported super page\n");
+			pr_info("Disable supported super page\n");
 			intel_iommu_superpage = 0;
+		} else if (!strncmp(str, "ecs_off", 7)) {
+			printk(KERN_INFO
+				"Intel-IOMMU: disable extended context table support\n");
+			intel_iommu_ecs = 0;
 		}
 
 		str += strcspn(str, ",");
@@ -669,7 +755,7 @@ static inline struct context_entry *iommu_context_addr(struct intel_iommu *iommu
 	struct context_entry *context;
 	u64 *entry;
 
-	if (ecap_ecs(iommu->ecap)) {
+	if (ecs_enabled(iommu)) {
 		if (devfn >= 0x80) {
 			devfn -= 0x80;
 			entry = &root->hi;
@@ -696,6 +782,11 @@ static inline struct context_entry *iommu_context_addr(struct intel_iommu *iommu
 	return &context[devfn];
 }
 
+static int iommu_dummy(struct device *dev)
+{
+	return dev->archdata.iommu == DUMMY_DEVICE_DOMAIN_INFO;
+}
+
 static struct intel_iommu *device_to_iommu(struct device *dev, u8 *bus, u8 *devfn)
 {
 	struct dmar_drhd_unit *drhd = NULL;
@@ -705,6 +796,9 @@ static struct intel_iommu *device_to_iommu(struct device *dev, u8 *bus, u8 *devf
 	u16 segment = 0;
 	int i;
 
+	if (iommu_dummy(dev))
+		return NULL;
+
 	if (dev_is_pci(dev)) {
 		pdev = to_pci_dev(dev);
 		segment = pci_domain_nr(pdev->bus);
@@ -798,7 +892,7 @@ static void free_context_table(struct intel_iommu *iommu)
 		if (context)
 			free_pgtable_page(context);
 
-		if (!ecap_ecs(iommu->ecap))
+		if (!ecs_enabled(iommu))
 			continue;
 
 		context = iommu_context_addr(iommu, i, 0x80, 0);
@@ -1112,7 +1206,7 @@ static int iommu_alloc_root_entry(struct intel_iommu *iommu)
 
 	root = (struct root_entry *)alloc_pgtable_page(iommu->node);
 	if (!root) {
-		pr_err("IOMMU: allocating root entry for %s failed\n",
+		pr_err("Allocating root entry for %s failed\n",
 			iommu->name);
 		return -ENOMEM;
 	}
@@ -1133,7 +1227,7 @@ static void iommu_set_root_entry(struct intel_iommu *iommu)
 	unsigned long flag;
 
 	addr = virt_to_phys(iommu->root_entry);
-	if (ecap_ecs(iommu->ecap))
+	if (ecs_enabled(iommu))
 		addr |= DMA_RTADDR_RTT;
 
 	raw_spin_lock_irqsave(&iommu->register_lock, flag);
@@ -1250,9 +1344,9 @@ static void __iommu_flush_iotlb(struct intel_iommu *iommu, u16 did,
 
 	/* check IOTLB invalidation granularity */
 	if (DMA_TLB_IAIG(val) == 0)
-		printk(KERN_ERR"IOMMU: flush IOTLB failed\n");
+		pr_err("Flush IOTLB failed\n");
 	if (DMA_TLB_IAIG(val) != DMA_TLB_IIRG(type))
-		pr_debug("IOMMU: tlb flush request %Lx, actual %Lx\n",
+		pr_debug("TLB flush request %Lx, actual %Lx\n",
 			(unsigned long long)DMA_TLB_IIRG(type),
 			(unsigned long long)DMA_TLB_IAIG(val));
 }
@@ -1423,8 +1517,8 @@ static int iommu_init_domains(struct intel_iommu *iommu)
 	unsigned long nlongs;
 
 	ndomains = cap_ndoms(iommu->cap);
-	pr_debug("IOMMU%d: Number of Domains supported <%ld>\n",
-		 iommu->seq_id, ndomains);
+	pr_debug("%s: Number of Domains supported <%ld>\n",
+		 iommu->name, ndomains);
 	nlongs = BITS_TO_LONGS(ndomains);
 
 	spin_lock_init(&iommu->lock);
@@ -1434,15 +1528,15 @@ static int iommu_init_domains(struct intel_iommu *iommu)
 	 */
 	iommu->domain_ids = kcalloc(nlongs, sizeof(unsigned long), GFP_KERNEL);
 	if (!iommu->domain_ids) {
-		pr_err("IOMMU%d: allocating domain id array failed\n",
-		       iommu->seq_id);
+		pr_err("%s: Allocating domain id array failed\n",
+		       iommu->name);
 		return -ENOMEM;
 	}
 	iommu->domains = kcalloc(ndomains, sizeof(struct dmar_domain *),
 			GFP_KERNEL);
 	if (!iommu->domains) {
-		pr_err("IOMMU%d: allocating domain array failed\n",
-		       iommu->seq_id);
+		pr_err("%s: Allocating domain array failed\n",
+		       iommu->name);
 		kfree(iommu->domain_ids);
 		iommu->domain_ids = NULL;
 		return -ENOMEM;
@@ -1547,7 +1641,7 @@ static int iommu_attach_domain(struct dmar_domain *domain,
 	num = __iommu_attach_domain(domain, iommu);
 	spin_unlock_irqrestore(&iommu->lock, flags);
 	if (num < 0)
-		pr_err("IOMMU: no free domain ids\n");
+		pr_err("%s: No free domain ids\n", iommu->name);
 
 	return num;
 }
@@ -1639,7 +1733,7 @@ static int dmar_init_reserved_ranges(void)
 	iova = reserve_iova(&reserved_iova_list, IOVA_PFN(IOAPIC_RANGE_START),
 		IOVA_PFN(IOAPIC_RANGE_END));
 	if (!iova) {
-		printk(KERN_ERR "Reserve IOAPIC range failed\n");
+		pr_err("Reserve IOAPIC range failed\n");
 		return -ENODEV;
 	}
 
@@ -1655,7 +1749,7 @@ static int dmar_init_reserved_ranges(void)
 					    IOVA_PFN(r->start),
 					    IOVA_PFN(r->end));
 			if (!iova) {
-				printk(KERN_ERR "Reserve iova failed\n");
+				pr_err("Reserve iova failed\n");
 				return -ENODEV;
 			}
 		}
@@ -1702,7 +1796,7 @@ static int domain_init(struct dmar_domain *domain, int guest_width)
 	sagaw = cap_sagaw(iommu->cap);
 	if (!test_bit(agaw, &sagaw)) {
 		/* hardware doesn't support it, choose a bigger one */
-		pr_debug("IOMMU: hardware doesn't support agaw %d\n", agaw);
+		pr_debug("Hardware doesn't support agaw %d\n", agaw);
 		agaw = find_next_bit(&sagaw, 5, agaw);
 		if (agaw >= 5)
 			return -ENODEV;
@@ -1795,6 +1889,8 @@ static int domain_context_mapping_one(struct dmar_domain *domain,
 		return 0;
 	}
 
+	context_clear_entry(context);
+
 	id = domain->id;
 	pgd = domain->pgd;
 
@@ -1803,7 +1899,7 @@ static int domain_context_mapping_one(struct dmar_domain *domain,
 			id = iommu_attach_vm_domain(domain, iommu);
 			if (id < 0) {
 				spin_unlock_irqrestore(&iommu->lock, flags);
-				pr_err("IOMMU: no free domain ids\n");
+				pr_err("%s: No free domain ids\n", iommu->name);
 				return -EFAULT;
 			}
 		}
@@ -2030,8 +2126,8 @@ static int __domain_mapping(struct dmar_domain *domain, unsigned long iov_pfn,
 		tmp = cmpxchg64_local(&pte->val, 0ULL, pteval);
 		if (tmp) {
 			static int dumps = 5;
-			printk(KERN_CRIT "ERROR: DMA PTE for vPFN 0x%lx already set (to %llx not %llx)\n",
-			       iov_pfn, tmp, (unsigned long long)pteval);
+			pr_crit("ERROR: DMA PTE for vPFN 0x%lx already set (to %llx not %llx)\n",
+				iov_pfn, tmp, (unsigned long long)pteval);
 			if (dumps) {
 				dumps--;
 				debug_dma_dump_mappings(NULL);
@@ -2303,7 +2399,7 @@ static int iommu_domain_identity_map(struct dmar_domain *domain,
 
 	if (!reserve_iova(&domain->iovad, dma_to_mm_pfn(first_vpfn),
 			  dma_to_mm_pfn(last_vpfn))) {
-		printk(KERN_ERR "IOMMU: reserve iova failed\n");
+		pr_err("Reserving iova failed\n");
 		return -ENOMEM;
 	}
 
@@ -2336,15 +2432,14 @@ static int iommu_prepare_identity_map(struct device *dev,
 	   range which is reserved in E820, so which didn't get set
 	   up to start with in si_domain */
 	if (domain == si_domain && hw_pass_through) {
-		printk("Ignoring identity map for HW passthrough device %s [0x%Lx - 0x%Lx]\n",
-		       dev_name(dev), start, end);
+		pr_warn("Ignoring identity map for HW passthrough device %s [0x%Lx - 0x%Lx]\n",
+			dev_name(dev), start, end);
 		return 0;
 	}
 
-	printk(KERN_INFO
-	       "IOMMU: Setting identity map for device %s [0x%Lx - 0x%Lx]\n",
-	       dev_name(dev), start, end);
-	
+	pr_info("Setting identity map for device %s [0x%Lx - 0x%Lx]\n",
+		dev_name(dev), start, end);
+
 	if (end < start) {
 		WARN(1, "Your BIOS is broken; RMRR ends before it starts!\n"
 			"BIOS vendor: %s; Ver: %s; Product Version: %s\n",
@@ -2401,12 +2496,11 @@ static inline void iommu_prepare_isa(void)
 	if (!pdev)
 		return;
 
-	printk(KERN_INFO "IOMMU: Prepare 0-16MiB unity mapping for LPC\n");
+	pr_info("Prepare 0-16MiB unity mapping for LPC\n");
 	ret = iommu_prepare_identity_map(&pdev->dev, 0, 16*1024*1024 - 1);
 
 	if (ret)
-		printk(KERN_ERR "IOMMU: Failed to create 0-16MiB identity map; "
-		       "floppy might not work\n");
+		pr_err("Failed to create 0-16MiB identity map - floppy might not work\n");
 
 	pci_dev_put(pdev);
 }
@@ -2450,7 +2544,7 @@ static int __init si_domain_init(int hw)
 		return -EFAULT;
 	}
 
-	pr_debug("IOMMU: identity mapping domain is domain %d\n",
+	pr_debug("Identity mapping domain is domain %d\n",
 		 si_domain->id);
 
 	if (hw)
@@ -2650,8 +2744,8 @@ static int __init dev_prepare_static_identity_mapping(struct device *dev, int hw
 				  hw ? CONTEXT_TT_PASS_THROUGH :
 				       CONTEXT_TT_MULTI_LEVEL);
 	if (!ret)
-		pr_info("IOMMU: %s identity mapping for device %s\n",
-			hw ? "hardware" : "software", dev_name(dev));
+		pr_info("%s identity mapping for device %s\n",
+			hw ? "Hardware" : "Software", dev_name(dev));
 	else if (ret == -ENODEV)
 		/* device not associated with an iommu */
 		ret = 0;
@@ -2669,10 +2763,6 @@ static int __init iommu_prepare_static_identity_mapping(int hw)
 	int i;
 	int ret = 0;
 
-	ret = si_domain_init(hw);
-	if (ret)
-		return -EFAULT;
-
 	for_each_pci_dev(pdev) {
 		ret = dev_prepare_static_identity_mapping(&pdev->dev, hw);
 		if (ret)
@@ -2686,7 +2776,7 @@ static int __init iommu_prepare_static_identity_mapping(int hw)
 
 			if (dev->bus != &acpi_bus_type)
 				continue;
-				
+
 			adev= to_acpi_device(dev);
 			mutex_lock(&adev->physical_node_lock);
 			list_for_each_entry(pn, &adev->physical_node_list, node) {
@@ -2728,19 +2818,200 @@ static void intel_iommu_init_qi(struct intel_iommu *iommu)
 		 */
 		iommu->flush.flush_context = __iommu_flush_context;
 		iommu->flush.flush_iotlb = __iommu_flush_iotlb;
-		pr_info("IOMMU: %s using Register based invalidation\n",
+		pr_info("%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);
+		pr_info("%s: Using Queued invalidation\n", iommu->name);
 	}
 }
 
+static int copy_context_table(struct intel_iommu *iommu,
+			      struct root_entry *old_re,
+			      struct context_entry **tbl,
+			      int bus, bool ext)
+{
+	struct context_entry *old_ce = NULL, *new_ce = NULL, ce;
+	int tbl_idx, pos = 0, idx, devfn, ret = 0, did;
+	phys_addr_t old_ce_phys;
+
+	tbl_idx = ext ? bus * 2 : bus;
+
+	for (devfn = 0; devfn < 256; devfn++) {
+		/* First calculate the correct index */
+		idx = (ext ? devfn * 2 : devfn) % 256;
+
+		if (idx == 0) {
+			/* First save what we may have and clean up */
+			if (new_ce) {
+				tbl[tbl_idx] = new_ce;
+				__iommu_flush_cache(iommu, new_ce,
+						    VTD_PAGE_SIZE);
+				pos = 1;
+			}
+
+			if (old_ce)
+				iounmap(old_ce);
+
+			ret = 0;
+			if (devfn < 0x80)
+				old_ce_phys = root_entry_lctp(old_re);
+			else
+				old_ce_phys = root_entry_uctp(old_re);
+
+			if (!old_ce_phys) {
+				if (ext && devfn == 0) {
+					/* No LCTP, try UCTP */
+					devfn = 0x7f;
+					continue;
+				} else {
+					goto out;
+				}
+			}
+
+			ret = -ENOMEM;
+			old_ce = ioremap_cache(old_ce_phys, PAGE_SIZE);
+			if (!old_ce)
+				goto out;
+
+			new_ce = alloc_pgtable_page(iommu->node);
+			if (!new_ce)
+				goto out_unmap;
+
+			ret = 0;
+		}
+
+		/* Now copy the context entry */
+		ce = old_ce[idx];
+
+		if (!__context_present(&ce))
+			continue;
+
+		did = context_domain_id(&ce);
+		if (did >= 0 && did < cap_ndoms(iommu->cap))
+			set_bit(did, iommu->domain_ids);
+
+		/*
+		 * We need a marker for copied context entries. This
+		 * marker needs to work for the old format as well as
+		 * for extended context entries.
+		 *
+		 * Bit 67 of the context entry is used. In the old
+		 * format this bit is available to software, in the
+		 * extended format it is the PGE bit, but PGE is ignored
+		 * by HW if PASIDs are disabled (and thus still
+		 * available).
+		 *
+		 * So disable PASIDs first and then mark the entry
+		 * copied. This means that we don't copy PASID
+		 * translations from the old kernel, but this is fine as
+		 * faults there are not fatal.
+		 */
+		context_clear_pasid_enable(&ce);
+		context_set_copied(&ce);
+
+		new_ce[idx] = ce;
+	}
+
+	tbl[tbl_idx + pos] = new_ce;
+
+	__iommu_flush_cache(iommu, new_ce, VTD_PAGE_SIZE);
+
+out_unmap:
+	iounmap(old_ce);
+
+out:
+	return ret;
+}
+
+static int copy_translation_tables(struct intel_iommu *iommu)
+{
+	struct context_entry **ctxt_tbls;
+	struct root_entry *old_rt;
+	phys_addr_t old_rt_phys;
+	int ctxt_table_entries;
+	unsigned long flags;
+	u64 rtaddr_reg;
+	int bus, ret;
+	bool new_ext, ext;
+
+	rtaddr_reg = dmar_readq(iommu->reg + DMAR_RTADDR_REG);
+	ext        = !!(rtaddr_reg & DMA_RTADDR_RTT);
+	new_ext    = !!ecap_ecs(iommu->ecap);
+
+	/*
+	 * The RTT bit can only be changed when translation is disabled,
+	 * but disabling translation means to open a window for data
+	 * corruption. So bail out and don't copy anything if we would
+	 * have to change the bit.
+	 */
+	if (new_ext != ext)
+		return -EINVAL;
+
+	old_rt_phys = rtaddr_reg & VTD_PAGE_MASK;
+	if (!old_rt_phys)
+		return -EINVAL;
+
+	old_rt = ioremap_cache(old_rt_phys, PAGE_SIZE);
+	if (!old_rt)
+		return -ENOMEM;
+
+	/* This is too big for the stack - allocate it from slab */
+	ctxt_table_entries = ext ? 512 : 256;
+	ret = -ENOMEM;
+	ctxt_tbls = kzalloc(ctxt_table_entries * sizeof(void *), GFP_KERNEL);
+	if (!ctxt_tbls)
+		goto out_unmap;
+
+	for (bus = 0; bus < 256; bus++) {
+		ret = copy_context_table(iommu, &old_rt[bus],
+					 ctxt_tbls, bus, ext);
+		if (ret) {
+			pr_err("%s: Failed to copy context table for bus %d\n",
+				iommu->name, bus);
+			continue;
+		}
+	}
+
+	spin_lock_irqsave(&iommu->lock, flags);
+
+	/* Context tables are copied, now write them to the root_entry table */
+	for (bus = 0; bus < 256; bus++) {
+		int idx = ext ? bus * 2 : bus;
+		u64 val;
+
+		if (ctxt_tbls[idx]) {
+			val = virt_to_phys(ctxt_tbls[idx]) | 1;
+			iommu->root_entry[bus].lo = val;
+		}
+
+		if (!ext || !ctxt_tbls[idx + 1])
+			continue;
+
+		val = virt_to_phys(ctxt_tbls[idx + 1]) | 1;
+		iommu->root_entry[bus].hi = val;
+	}
+
+	spin_unlock_irqrestore(&iommu->lock, flags);
+
+	kfree(ctxt_tbls);
+
+	__iommu_flush_cache(iommu, iommu->root_entry, PAGE_SIZE);
+
+	ret = 0;
+
+out_unmap:
+	iounmap(old_rt);
+
+	return ret;
+}
+
 static int __init init_dmars(void)
 {
 	struct dmar_drhd_unit *drhd;
 	struct dmar_rmrr_unit *rmrr;
+	bool copied_tables = false;
 	struct device *dev;
 	struct intel_iommu *iommu;
 	int i, ret;
@@ -2761,8 +3032,7 @@ static int __init init_dmars(void)
 			g_num_of_iommus++;
 			continue;
 		}
-		printk_once(KERN_ERR "intel-iommu: exceeded %d IOMMUs\n",
-			  DMAR_UNITS_SUPPORTED);
+		pr_err_once("Exceeded %d IOMMUs\n", DMAR_UNITS_SUPPORTED);
 	}
 
 	/* Preallocate enough resources for IOMMU hot-addition */
@@ -2772,7 +3042,7 @@ static int __init init_dmars(void)
 	g_iommus = kcalloc(g_num_of_iommus, sizeof(struct intel_iommu *),
 			GFP_KERNEL);
 	if (!g_iommus) {
-		printk(KERN_ERR "Allocating global iommu array failed\n");
+		pr_err("Allocating global iommu array failed\n");
 		ret = -ENOMEM;
 		goto error;
 	}
@@ -2787,10 +3057,21 @@ static int __init init_dmars(void)
 	for_each_active_iommu(iommu, drhd) {
 		g_iommus[iommu->seq_id] = iommu;
 
+		intel_iommu_init_qi(iommu);
+
 		ret = iommu_init_domains(iommu);
 		if (ret)
 			goto free_iommu;
 
+		init_translation_status(iommu);
+
+		if (translation_pre_enabled(iommu) && !is_kdump_kernel()) {
+			iommu_disable_translation(iommu);
+			clear_translation_pre_enabled(iommu);
+			pr_warn("Translation was enabled for %s but we are not in kdump mode\n",
+				iommu->name);
+		}
+
 		/*
 		 * TBD:
 		 * we could share the same root & context tables
@@ -2799,13 +3080,41 @@ static int __init init_dmars(void)
 		ret = iommu_alloc_root_entry(iommu);
 		if (ret)
 			goto free_iommu;
+
+		if (translation_pre_enabled(iommu)) {
+			pr_info("Translation already enabled - trying to copy translation structures\n");
+
+			ret = copy_translation_tables(iommu);
+			if (ret) {
+				/*
+				 * We found the IOMMU with translation
+				 * enabled - but failed to copy over the
+				 * old root-entry table. Try to proceed
+				 * by disabling translation now and
+				 * allocating a clean root-entry table.
+				 * This might cause DMAR faults, but
+				 * probably the dump will still succeed.
+				 */
+				pr_err("Failed to copy translation tables from previous kernel for %s\n",
+				       iommu->name);
+				iommu_disable_translation(iommu);
+				clear_translation_pre_enabled(iommu);
+			} else {
+				pr_info("Copied translation tables from previous kernel for %s\n",
+					iommu->name);
+				copied_tables = true;
+			}
+		}
+
+		iommu_flush_write_buffer(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);
+
 		if (!ecap_pass_through(iommu->ecap))
 			hw_pass_through = 0;
 	}
 
-	for_each_active_iommu(iommu, drhd)
-		intel_iommu_init_qi(iommu);
-
 	if (iommu_pass_through)
 		iommu_identity_mapping |= IDENTMAP_ALL;
 
@@ -2813,9 +3122,24 @@ static int __init init_dmars(void)
 	iommu_identity_mapping |= IDENTMAP_GFX;
 #endif
 
+	if (iommu_identity_mapping) {
+		ret = si_domain_init(hw_pass_through);
+		if (ret)
+			goto free_iommu;
+	}
+
 	check_tylersburg_isoch();
 
 	/*
+	 * If we copied translations from a previous kernel in the kdump
+	 * case, we can not assign the devices to domains now, as that
+	 * would eliminate the old mappings. So skip this part and defer
+	 * the assignment to device driver initialization time.
+	 */
+	if (copied_tables)
+		goto domains_done;
+
+	/*
 	 * If pass through is not set or not enabled, setup context entries for
 	 * identity mappings for rmrr, gfx, and isa and may fall back to static
 	 * identity mapping if iommu_identity_mapping is set.
@@ -2823,7 +3147,7 @@ static int __init init_dmars(void)
 	if (iommu_identity_mapping) {
 		ret = iommu_prepare_static_identity_mapping(hw_pass_through);
 		if (ret) {
-			printk(KERN_CRIT "Failed to setup IOMMU pass-through\n");
+			pr_crit("Failed to setup IOMMU pass-through\n");
 			goto free_iommu;
 		}
 	}
@@ -2841,20 +3165,21 @@ static int __init init_dmars(void)
 	 *    endfor
 	 * endfor
 	 */
-	printk(KERN_INFO "IOMMU: Setting RMRR:\n");
+	pr_info("Setting RMRR:\n");
 	for_each_rmrr_units(rmrr) {
 		/* some BIOS lists non-exist devices in DMAR table. */
 		for_each_active_dev_scope(rmrr->devices, rmrr->devices_cnt,
 					  i, dev) {
 			ret = iommu_prepare_rmrr_dev(rmrr, dev);
 			if (ret)
-				printk(KERN_ERR
-				       "IOMMU: mapping reserved region failed\n");
+				pr_err("Mapping reserved region failed\n");
 		}
 	}
 
 	iommu_prepare_isa();
 
+domains_done:
+
 	/*
 	 * for each drhd
 	 *   enable fault log
@@ -2879,11 +3204,9 @@ static int __init init_dmars(void)
 		if (ret)
 			goto free_iommu;
 
-		iommu_set_root_entry(iommu);
+		if (!translation_pre_enabled(iommu))
+			iommu_enable_translation(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);
 		iommu_disable_protect_mem_regions(iommu);
 	}
 
@@ -2924,7 +3247,7 @@ static struct iova *intel_alloc_iova(struct device *dev,
 	}
 	iova = alloc_iova(&domain->iovad, nrpages, IOVA_PFN(dma_mask), 1);
 	if (unlikely(!iova)) {
-		printk(KERN_ERR "Allocating %ld-page iova for %s failed",
+		pr_err("Allocating %ld-page iova for %s failed",
 		       nrpages, dev_name(dev));
 		return NULL;
 	}
@@ -2939,7 +3262,7 @@ static struct dmar_domain *__get_valid_domain_for_dev(struct device *dev)
 
 	domain = get_domain_for_dev(dev, DEFAULT_DOMAIN_ADDRESS_WIDTH);
 	if (!domain) {
-		printk(KERN_ERR "Allocating domain for %s failed",
+		pr_err("Allocating domain for %s failed\n",
 		       dev_name(dev));
 		return NULL;
 	}
@@ -2948,7 +3271,7 @@ static struct dmar_domain *__get_valid_domain_for_dev(struct device *dev)
 	if (unlikely(!domain_context_mapped(dev))) {
 		ret = domain_context_mapping(domain, dev, CONTEXT_TT_MULTI_LEVEL);
 		if (ret) {
-			printk(KERN_ERR "Domain context map for %s failed",
+			pr_err("Domain context map for %s failed\n",
 			       dev_name(dev));
 			return NULL;
 		}
@@ -2969,11 +3292,6 @@ static inline struct dmar_domain *get_valid_domain_for_dev(struct device *dev)
 	return __get_valid_domain_for_dev(dev);
 }
 
-static int iommu_dummy(struct device *dev)
-{
-	return dev->archdata.iommu == DUMMY_DEVICE_DOMAIN_INFO;
-}
-
 /* Check if the dev needs to go through non-identity map and unmap process.*/
 static int iommu_no_mapping(struct device *dev)
 {
@@ -2995,8 +3313,8 @@ static int iommu_no_mapping(struct device *dev)
 			 * to non-identity mapping.
 			 */
 			domain_remove_one_dev_info(si_domain, dev);
-			printk(KERN_INFO "32bit %s uses non-identity mapping\n",
-			       dev_name(dev));
+			pr_info("32bit %s uses non-identity mapping\n",
+				dev_name(dev));
 			return 0;
 		}
 	} else {
@@ -3011,8 +3329,8 @@ static int iommu_no_mapping(struct device *dev)
 						  CONTEXT_TT_PASS_THROUGH :
 						  CONTEXT_TT_MULTI_LEVEL);
 			if (!ret) {
-				printk(KERN_INFO "64bit %s uses identity mapping\n",
-				       dev_name(dev));
+				pr_info("64bit %s uses identity mapping\n",
+					dev_name(dev));
 				return 1;
 			}
 		}
@@ -3081,7 +3399,7 @@ static dma_addr_t __intel_map_single(struct device *dev, phys_addr_t paddr,
 error:
 	if (iova)
 		__free_iova(&domain->iovad, iova);
-	printk(KERN_ERR"Device %s request: %zx@%llx dir %d --- failed\n",
+	pr_err("Device %s request: %zx@%llx dir %d --- failed\n",
 		dev_name(dev), size, (unsigned long long)paddr, dir);
 	return 0;
 }
@@ -3396,7 +3714,7 @@ static inline int iommu_domain_cache_init(void)
 
 					 NULL);
 	if (!iommu_domain_cache) {
-		printk(KERN_ERR "Couldn't create iommu_domain cache\n");
+		pr_err("Couldn't create iommu_domain cache\n");
 		ret = -ENOMEM;
 	}
 
@@ -3413,7 +3731,7 @@ static inline int iommu_devinfo_cache_init(void)
 					 SLAB_HWCACHE_ALIGN,
 					 NULL);
 	if (!iommu_devinfo_cache) {
-		printk(KERN_ERR "Couldn't create devinfo cache\n");
+		pr_err("Couldn't create devinfo cache\n");
 		ret = -ENOMEM;
 	}
 
@@ -3790,19 +4108,19 @@ static int intel_iommu_add(struct dmar_drhd_unit *dmaru)
 		return 0;
 
 	if (hw_pass_through && !ecap_pass_through(iommu->ecap)) {
-		pr_warn("IOMMU: %s doesn't support hardware pass through.\n",
+		pr_warn("%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",
+		pr_warn("%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",
+		pr_warn("%s: Doesn't support large page.\n",
 			iommu->name);
 		return -ENXIO;
 	}
@@ -4033,7 +4351,7 @@ static int intel_iommu_memory_notifier(struct notifier_block *nb,
 		start = mhp->start_pfn << PAGE_SHIFT;
 		end = ((mhp->start_pfn + mhp->nr_pages) << PAGE_SHIFT) - 1;
 		if (iommu_domain_identity_map(si_domain, start, end)) {
-			pr_warn("dmar: failed to build identity map for [%llx-%llx]\n",
+			pr_warn("Failed to build identity map for [%llx-%llx]\n",
 				start, end);
 			return NOTIFY_BAD;
 		}
@@ -4051,7 +4369,7 @@ static int intel_iommu_memory_notifier(struct notifier_block *nb,
 
 			iova = find_iova(&si_domain->iovad, start_vpfn);
 			if (iova == NULL) {
-				pr_debug("dmar: failed get IOVA for PFN %lx\n",
+				pr_debug("Failed get IOVA for PFN %lx\n",
 					 start_vpfn);
 				break;
 			}
@@ -4059,7 +4377,7 @@ static int intel_iommu_memory_notifier(struct notifier_block *nb,
 			iova = split_and_remove_iova(&si_domain->iovad, iova,
 						     start_vpfn, last_vpfn);
 			if (iova == NULL) {
-				pr_warn("dmar: failed to split IOVA PFN [%lx-%lx]\n",
+				pr_warn("Failed to split IOVA PFN [%lx-%lx]\n",
 					start_vpfn, last_vpfn);
 				return NOTIFY_BAD;
 			}
@@ -4168,13 +4486,6 @@ int __init intel_iommu_init(void)
 		goto out_free_dmar;
 	}
 
-	/*
-	 * Disable translation if already enabled prior to OS handover.
-	 */
-	for_each_active_iommu(iommu, drhd)
-		if (iommu->gcmd & DMA_GCMD_TE)
-			iommu_disable_translation(iommu);
-
 	if (dmar_dev_scope_init() < 0) {
 		if (force_on)
 			panic("tboot: Failed to initialize DMAR device scope\n");
@@ -4185,10 +4496,10 @@ int __init intel_iommu_init(void)
 		goto out_free_dmar;
 
 	if (list_empty(&dmar_rmrr_units))
-		printk(KERN_INFO "DMAR: No RMRR found\n");
+		pr_info("No RMRR found\n");
 
 	if (list_empty(&dmar_atsr_units))
-		printk(KERN_INFO "DMAR: No ATSR found\n");
+		pr_info("No ATSR found\n");
 
 	if (dmar_init_reserved_ranges()) {
 		if (force_on)
@@ -4202,12 +4513,11 @@ int __init intel_iommu_init(void)
 	if (ret) {
 		if (force_on)
 			panic("tboot: Failed to initialize DMARs\n");
-		printk(KERN_ERR "IOMMU: dmar init failed\n");
+		pr_err("Initialization failed\n");
 		goto out_free_reserved_range;
 	}
 	up_write(&dmar_global_lock);
-	printk(KERN_INFO
-	"PCI-DMA: Intel(R) Virtualization Technology for Directed I/O\n");
+	pr_info("Intel(R) Virtualization Technology for Directed I/O\n");
 
 	init_timer(&unmap_timer);
 #ifdef CONFIG_SWIOTLB
@@ -4349,13 +4659,11 @@ static struct iommu_domain *intel_iommu_domain_alloc(unsigned type)
 
 	dmar_domain = alloc_domain(DOMAIN_FLAG_VIRTUAL_MACHINE);
 	if (!dmar_domain) {
-		printk(KERN_ERR
-			"intel_iommu_domain_init: dmar_domain == NULL\n");
+		pr_err("Can't allocate dmar_domain\n");
 		return NULL;
 	}
 	if (md_domain_init(dmar_domain, DEFAULT_DOMAIN_ADDRESS_WIDTH)) {
-		printk(KERN_ERR
-			"intel_iommu_domain_init() failed\n");
+		pr_err("Domain initialization failed\n");
 		domain_exit(dmar_domain);
 		return NULL;
 	}
@@ -4414,7 +4722,7 @@ static int intel_iommu_attach_device(struct iommu_domain *domain,
 		addr_width = cap_mgaw(iommu->cap);
 
 	if (dmar_domain->max_addr > (1LL << addr_width)) {
-		printk(KERN_ERR "%s: iommu width (%d) is not "
+		pr_err("%s: iommu width (%d) is not "
 		       "sufficient for the mapped address (%llx)\n",
 		       __func__, addr_width, dmar_domain->max_addr);
 		return -EFAULT;
@@ -4468,7 +4776,7 @@ static int intel_iommu_map(struct iommu_domain *domain,
 		/* check if minimum agaw is sufficient for mapped address */
 		end = __DOMAIN_MAX_ADDR(dmar_domain->gaw) + 1;
 		if (end < max_addr) {
-			printk(KERN_ERR "%s: iommu width (%d) is not "
+			pr_err("%s: iommu width (%d) is not "
 			       "sufficient for the mapped address (%llx)\n",
 			       __func__, dmar_domain->gaw, max_addr);
 			return -EFAULT;
@@ -4609,7 +4917,7 @@ static const struct iommu_ops intel_iommu_ops = {
 static void quirk_iommu_g4x_gfx(struct pci_dev *dev)
 {
 	/* G4x/GM45 integrated gfx dmar support is totally busted. */
-	printk(KERN_INFO "DMAR: Disabling IOMMU for graphics on this chipset\n");
+	pr_info("Disabling IOMMU for graphics on this chipset\n");
 	dmar_map_gfx = 0;
 }
 
@@ -4627,7 +4935,7 @@ static void quirk_iommu_rwbf(struct pci_dev *dev)
 	 * Mobile 4 Series Chipset neglects to set RWBF capability,
 	 * but needs it. Same seems to hold for the desktop versions.
 	 */
-	printk(KERN_INFO "DMAR: Forcing write-buffer flush capability\n");
+	pr_info("Forcing write-buffer flush capability\n");
 	rwbf_quirk = 1;
 }
 
@@ -4657,11 +4965,11 @@ static void quirk_calpella_no_shadow_gtt(struct pci_dev *dev)
 		return;
 
 	if (!(ggc & GGC_MEMORY_VT_ENABLED)) {
-		printk(KERN_INFO "DMAR: BIOS has allocated no shadow GTT; disabling IOMMU for graphics\n");
+		pr_info("BIOS has allocated no shadow GTT; disabling IOMMU for graphics\n");
 		dmar_map_gfx = 0;
 	} else if (dmar_map_gfx) {
 		/* we have to ensure the gfx device is idle before we flush */
-		printk(KERN_INFO "DMAR: Disabling batched IOTLB flush on Ironlake\n");
+		pr_info("Disabling batched IOTLB flush on Ironlake\n");
 		intel_iommu_strict = 1;
        }
 }
@@ -4723,7 +5031,7 @@ static void __init check_tylersburg_isoch(void)
 		iommu_identity_mapping |= IDENTMAP_AZALIA;
 		return;
 	}
-	
-	printk(KERN_WARNING "DMAR: Recommended TLB entries for ISOCH unit is 16; your BIOS set %d\n",
+
+	pr_warn("Recommended TLB entries for ISOCH unit is 16; your BIOS set %d\n",
 	       vtisochctrl);
 }
diff --git a/drivers/iommu/intel_irq_remapping.c b/drivers/iommu/intel_irq_remapping.c
index 5709ae9c3e77..f15692a410c7 100644
--- a/drivers/iommu/intel_irq_remapping.c
+++ b/drivers/iommu/intel_irq_remapping.c
@@ -1,3 +1,6 @@
+
+#define pr_fmt(fmt)     "DMAR-IR: " fmt
+
 #include <linux/interrupt.h>
 #include <linux/dmar.h>
 #include <linux/spinlock.h>
@@ -8,6 +11,8 @@
 #include <linux/irq.h>
 #include <linux/intel-iommu.h>
 #include <linux/acpi.h>
+#include <linux/irqdomain.h>
+#include <linux/crash_dump.h>
 #include <asm/io_apic.h>
 #include <asm/smp.h>
 #include <asm/cpu.h>
@@ -17,6 +22,11 @@
 
 #include "irq_remapping.h"
 
+enum irq_mode {
+	IRQ_REMAPPING,
+	IRQ_POSTING,
+};
+
 struct ioapic_scope {
 	struct intel_iommu *iommu;
 	unsigned int id;
@@ -31,6 +41,22 @@ struct hpet_scope {
 	unsigned int devfn;
 };
 
+struct irq_2_iommu {
+	struct intel_iommu *iommu;
+	u16 irte_index;
+	u16 sub_handle;
+	u8  irte_mask;
+	enum irq_mode mode;
+};
+
+struct intel_ir_data {
+	struct irq_2_iommu			irq_2_iommu;
+	struct irte				irte_entry;
+	union {
+		struct msi_msg			msi_entry;
+	};
+};
+
 #define IR_X2APIC_MODE(mode) (mode ? (1 << 11) : 0)
 #define IRTE_DEST(dest) ((eim_mode) ? dest : dest << 8)
 
@@ -50,43 +76,34 @@ static struct hpet_scope ir_hpet[MAX_HPET_TBS];
  * the dmar_global_lock.
  */
 static DEFINE_RAW_SPINLOCK(irq_2_ir_lock);
+static struct irq_domain_ops intel_ir_domain_ops;
 
+static void iommu_disable_irq_remapping(struct intel_iommu *iommu);
 static int __init parse_ioapics_under_ir(void);
 
-static struct irq_2_iommu *irq_2_iommu(unsigned int irq)
+static bool ir_pre_enabled(struct intel_iommu *iommu)
 {
-	struct irq_cfg *cfg = irq_cfg(irq);
-	return cfg ? &cfg->irq_2_iommu : NULL;
+	return (iommu->flags & VTD_FLAG_IRQ_REMAP_PRE_ENABLED);
 }
 
-static int get_irte(int irq, struct irte *entry)
+static void clear_ir_pre_enabled(struct intel_iommu *iommu)
 {
-	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
-	unsigned long flags;
-	int index;
-
-	if (!entry || !irq_iommu)
-		return -1;
-
-	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
-
-	if (unlikely(!irq_iommu->iommu)) {
-		raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
-		return -1;
-	}
+	iommu->flags &= ~VTD_FLAG_IRQ_REMAP_PRE_ENABLED;
+}
 
-	index = irq_iommu->irte_index + irq_iommu->sub_handle;
-	*entry = *(irq_iommu->iommu->ir_table->base + index);
+static void init_ir_status(struct intel_iommu *iommu)
+{
+	u32 gsts;
 
-	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
-	return 0;
+	gsts = readl(iommu->reg + DMAR_GSTS_REG);
+	if (gsts & DMA_GSTS_IRES)
+		iommu->flags |= VTD_FLAG_IRQ_REMAP_PRE_ENABLED;
 }
 
-static int alloc_irte(struct intel_iommu *iommu, int irq, u16 count)
+static int alloc_irte(struct intel_iommu *iommu, int irq,
+		      struct irq_2_iommu *irq_iommu, u16 count)
 {
 	struct ir_table *table = iommu->ir_table;
-	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
-	struct irq_cfg *cfg = irq_cfg(irq);
 	unsigned int mask = 0;
 	unsigned long flags;
 	int index;
@@ -100,8 +117,7 @@ static int alloc_irte(struct intel_iommu *iommu, int irq, u16 count)
 	}
 
 	if (mask > ecap_max_handle_mask(iommu->ecap)) {
-		printk(KERN_ERR
-		       "Requested mask %x exceeds the max invalidation handle"
+		pr_err("Requested mask %x exceeds the max invalidation handle"
 		       " mask value %Lx\n", mask,
 		       ecap_max_handle_mask(iommu->ecap));
 		return -1;
@@ -113,11 +129,11 @@ static int alloc_irte(struct intel_iommu *iommu, int irq, u16 count)
 	if (index < 0) {
 		pr_warn("IR%d: can't allocate an IRTE\n", iommu->seq_id);
 	} else {
-		cfg->remapped = 1;
 		irq_iommu->iommu = iommu;
 		irq_iommu->irte_index =  index;
 		irq_iommu->sub_handle = 0;
 		irq_iommu->irte_mask = mask;
+		irq_iommu->mode = IRQ_REMAPPING;
 	}
 	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 
@@ -135,47 +151,9 @@ static int qi_flush_iec(struct intel_iommu *iommu, int index, int mask)
 	return qi_submit_sync(&desc, iommu);
 }
 
-static int map_irq_to_irte_handle(int irq, u16 *sub_handle)
-{
-	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
-	unsigned long flags;
-	int index;
-
-	if (!irq_iommu)
-		return -1;
-
-	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
-	*sub_handle = irq_iommu->sub_handle;
-	index = irq_iommu->irte_index;
-	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
-	return index;
-}
-
-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_cfg(irq);
-	unsigned long flags;
-
-	if (!irq_iommu)
-		return -1;
-
-	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
-
-	cfg->remapped = 1;
-	irq_iommu->iommu = iommu;
-	irq_iommu->irte_index = index;
-	irq_iommu->sub_handle = subhandle;
-	irq_iommu->irte_mask = 0;
-
-	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
-
-	return 0;
-}
-
-static int modify_irte(int irq, struct irte *irte_modified)
+static int modify_irte(struct irq_2_iommu *irq_iommu,
+		       struct irte *irte_modified)
 {
-	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
 	struct intel_iommu *iommu;
 	unsigned long flags;
 	struct irte *irte;
@@ -196,6 +174,9 @@ static int modify_irte(int irq, struct irte *irte_modified)
 	__iommu_flush_cache(iommu, irte, sizeof(*irte));
 
 	rc = qi_flush_iec(iommu, index, 0);
+
+	/* Update iommu mode according to the IRTE mode */
+	irq_iommu->mode = irte->pst ? IRQ_POSTING : IRQ_REMAPPING;
 	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 
 	return rc;
@@ -242,7 +223,7 @@ static int clear_entries(struct irq_2_iommu *irq_iommu)
 		return 0;
 
 	iommu = irq_iommu->iommu;
-	index = irq_iommu->irte_index + irq_iommu->sub_handle;
+	index = irq_iommu->irte_index;
 
 	start = iommu->ir_table->base + index;
 	end = start + (1 << irq_iommu->irte_mask);
@@ -257,29 +238,6 @@ static int clear_entries(struct irq_2_iommu *irq_iommu)
 	return qi_flush_iec(iommu, index, irq_iommu->irte_mask);
 }
 
-static int free_irte(int irq)
-{
-	struct irq_2_iommu *irq_iommu = irq_2_iommu(irq);
-	unsigned long flags;
-	int rc;
-
-	if (!irq_iommu)
-		return -1;
-
-	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
-
-	rc = clear_entries(irq_iommu);
-
-	irq_iommu->iommu = NULL;
-	irq_iommu->irte_index = 0;
-	irq_iommu->sub_handle = 0;
-	irq_iommu->irte_mask = 0;
-
-	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
-
-	return rc;
-}
-
 /*
  * source validation type
  */
@@ -333,7 +291,7 @@ static int set_ioapic_sid(struct irte *irte, int apic)
 	up_read(&dmar_global_lock);
 
 	if (sid == 0) {
-		pr_warning("Failed to set source-id of IOAPIC (%d)\n", apic);
+		pr_warn("Failed to set source-id of IOAPIC (%d)\n", apic);
 		return -1;
 	}
 
@@ -360,7 +318,7 @@ static int set_hpet_sid(struct irte *irte, u8 id)
 	up_read(&dmar_global_lock);
 
 	if (sid == 0) {
-		pr_warning("Failed to set source-id of HPET block (%d)\n", id);
+		pr_warn("Failed to set source-id of HPET block (%d)\n", id);
 		return -1;
 	}
 
@@ -424,11 +382,59 @@ static int set_msi_sid(struct irte *irte, struct pci_dev *dev)
 	return 0;
 }
 
+static int iommu_load_old_irte(struct intel_iommu *iommu)
+{
+	struct irte *old_ir_table;
+	phys_addr_t irt_phys;
+	unsigned int i;
+	size_t size;
+	u64 irta;
+
+	if (!is_kdump_kernel()) {
+		pr_warn("IRQ remapping was enabled on %s but we are not in kdump mode\n",
+			iommu->name);
+		clear_ir_pre_enabled(iommu);
+		iommu_disable_irq_remapping(iommu);
+		return -EINVAL;
+	}
+
+	/* Check whether the old ir-table has the same size as ours */
+	irta = dmar_readq(iommu->reg + DMAR_IRTA_REG);
+	if ((irta & INTR_REMAP_TABLE_REG_SIZE_MASK)
+	     != INTR_REMAP_TABLE_REG_SIZE)
+		return -EINVAL;
+
+	irt_phys = irta & VTD_PAGE_MASK;
+	size     = INTR_REMAP_TABLE_ENTRIES*sizeof(struct irte);
+
+	/* Map the old IR table */
+	old_ir_table = ioremap_cache(irt_phys, size);
+	if (!old_ir_table)
+		return -ENOMEM;
+
+	/* Copy data over */
+	memcpy(iommu->ir_table->base, old_ir_table, size);
+
+	__iommu_flush_cache(iommu, iommu->ir_table->base, size);
+
+	/*
+	 * Now check the table for used entries and mark those as
+	 * allocated in the bitmap
+	 */
+	for (i = 0; i < INTR_REMAP_TABLE_ENTRIES; i++) {
+		if (iommu->ir_table->base[i].present)
+			bitmap_set(iommu->ir_table->bitmap, i, 1);
+	}
+
+	return 0;
+}
+
+
 static void iommu_set_irq_remapping(struct intel_iommu *iommu, int mode)
 {
+	unsigned long flags;
 	u64 addr;
 	u32 sts;
-	unsigned long flags;
 
 	addr = virt_to_phys((void *)iommu->ir_table->base);
 
@@ -445,10 +451,16 @@ static void iommu_set_irq_remapping(struct intel_iommu *iommu, int mode)
 	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
 
 	/*
-	 * global invalidation of interrupt entry cache before enabling
-	 * interrupt-remapping.
+	 * Global invalidation of interrupt entry cache to make sure the
+	 * hardware uses the new irq remapping table.
 	 */
 	qi_global_iec(iommu);
+}
+
+static void iommu_enable_irq_remapping(struct intel_iommu *iommu)
+{
+	unsigned long flags;
+	u32 sts;
 
 	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 
@@ -488,7 +500,6 @@ static int intel_setup_irq_remapping(struct intel_iommu *iommu)
 
 	pages = alloc_pages_node(iommu->node, GFP_KERNEL | __GFP_ZERO,
 				 INTR_REMAP_PAGE_ORDER);
-
 	if (!pages) {
 		pr_err("IR%d: failed to allocate pages of order %d\n",
 		       iommu->seq_id, INTR_REMAP_PAGE_ORDER);
@@ -502,21 +513,75 @@ static int intel_setup_irq_remapping(struct intel_iommu *iommu)
 		goto out_free_pages;
 	}
 
+	iommu->ir_domain = irq_domain_add_hierarchy(arch_get_ir_parent_domain(),
+						    0, INTR_REMAP_TABLE_ENTRIES,
+						    NULL, &intel_ir_domain_ops,
+						    iommu);
+	if (!iommu->ir_domain) {
+		pr_err("IR%d: failed to allocate irqdomain\n", iommu->seq_id);
+		goto out_free_bitmap;
+	}
+	iommu->ir_msi_domain = arch_create_msi_irq_domain(iommu->ir_domain);
+
 	ir_table->base = page_address(pages);
 	ir_table->bitmap = bitmap;
 	iommu->ir_table = ir_table;
+
+	/*
+	 * If the queued invalidation is already initialized,
+	 * shouldn't disable it.
+	 */
+	if (!iommu->qi) {
+		/*
+		 * Clear previous faults.
+		 */
+		dmar_fault(-1, iommu);
+		dmar_disable_qi(iommu);
+
+		if (dmar_enable_qi(iommu)) {
+			pr_err("Failed to enable queued invalidation\n");
+			goto out_free_bitmap;
+		}
+	}
+
+	init_ir_status(iommu);
+
+	if (ir_pre_enabled(iommu)) {
+		if (iommu_load_old_irte(iommu))
+			pr_err("Failed to copy IR table for %s from previous kernel\n",
+			       iommu->name);
+		else
+			pr_info("Copied IR table for %s from previous kernel\n",
+				iommu->name);
+	}
+
+	iommu_set_irq_remapping(iommu, eim_mode);
+
 	return 0;
 
+out_free_bitmap:
+	kfree(bitmap);
 out_free_pages:
 	__free_pages(pages, INTR_REMAP_PAGE_ORDER);
 out_free_table:
 	kfree(ir_table);
+
+	iommu->ir_table  = NULL;
+
 	return -ENOMEM;
 }
 
 static void intel_teardown_irq_remapping(struct intel_iommu *iommu)
 {
 	if (iommu && iommu->ir_table) {
+		if (iommu->ir_msi_domain) {
+			irq_domain_remove(iommu->ir_msi_domain);
+			iommu->ir_msi_domain = NULL;
+		}
+		if (iommu->ir_domain) {
+			irq_domain_remove(iommu->ir_domain);
+			iommu->ir_domain = NULL;
+		}
 		free_pages((unsigned long)iommu->ir_table->base,
 			   INTR_REMAP_PAGE_ORDER);
 		kfree(iommu->ir_table->bitmap);
@@ -580,17 +645,17 @@ static void __init intel_cleanup_irq_remapping(void)
 	}
 
 	if (x2apic_supported())
-		pr_warn("Failed to enable irq remapping.  You are vulnerable to irq-injection attacks.\n");
+		pr_warn("Failed to enable irq remapping. You are vulnerable to irq-injection attacks.\n");
 }
 
 static int __init intel_prepare_irq_remapping(void)
 {
 	struct dmar_drhd_unit *drhd;
 	struct intel_iommu *iommu;
+	int eim = 0;
 
 	if (irq_remap_broken) {
-		printk(KERN_WARNING
-			"This system BIOS has enabled interrupt remapping\n"
+		pr_warn("This system BIOS has enabled interrupt remapping\n"
 			"on a chipset that contains an erratum making that\n"
 			"feature unstable.  To maintain system stability\n"
 			"interrupt remapping is being disabled.  Please\n"
@@ -606,7 +671,7 @@ static int __init intel_prepare_irq_remapping(void)
 		return -ENODEV;
 
 	if (parse_ioapics_under_ir() != 1) {
-		printk(KERN_INFO "Not enabling interrupt remapping\n");
+		pr_info("Not enabling interrupt remapping\n");
 		goto error;
 	}
 
@@ -615,85 +680,74 @@ static int __init intel_prepare_irq_remapping(void)
 		if (!ecap_ir_support(iommu->ecap))
 			goto error;
 
-	/* Do the allocations early */
-	for_each_iommu(iommu, drhd)
-		if (intel_setup_irq_remapping(iommu))
-			goto error;
-
-	return 0;
-
-error:
-	intel_cleanup_irq_remapping();
-	return -ENODEV;
-}
-
-static int __init intel_enable_irq_remapping(void)
-{
-	struct dmar_drhd_unit *drhd;
-	struct intel_iommu *iommu;
-	bool setup = false;
-	int eim = 0;
-
+	/* Detect remapping mode: lapic or x2apic */
 	if (x2apic_supported()) {
 		eim = !dmar_x2apic_optout();
-		if (!eim)
-			pr_info("x2apic is disabled because BIOS sets x2apic opt out bit. You can use 'intremap=no_x2apic_optout' to override the BIOS setting.\n");
+		if (!eim) {
+			pr_info("x2apic is disabled because BIOS sets x2apic opt out bit.");
+			pr_info("Use 'intremap=no_x2apic_optout' to override the BIOS setting.\n");
+		}
 	}
 
 	for_each_iommu(iommu, drhd) {
-		/*
-		 * If the queued invalidation is already initialized,
-		 * shouldn't disable it.
-		 */
-		if (iommu->qi)
-			continue;
-
-		/*
-		 * Clear previous faults.
-		 */
-		dmar_fault(-1, iommu);
-
-		/*
-		 * Disable intr remapping and queued invalidation, if already
-		 * enabled prior to OS handover.
-		 */
-		iommu_disable_irq_remapping(iommu);
-
-		dmar_disable_qi(iommu);
-	}
-
-	/*
-	 * check for the Interrupt-remapping support
-	 */
-	for_each_iommu(iommu, drhd)
 		if (eim && !ecap_eim_support(iommu->ecap)) {
-			printk(KERN_INFO "DRHD %Lx: EIM not supported by DRHD, "
-			       " ecap %Lx\n", drhd->reg_base_addr, iommu->ecap);
+			pr_info("%s does not support EIM\n", iommu->name);
 			eim = 0;
 		}
+	}
+
 	eim_mode = eim;
 	if (eim)
 		pr_info("Queued invalidation will be enabled to support x2apic and Intr-remapping.\n");
 
-	/*
-	 * Enable queued invalidation for all the DRHD's.
-	 */
+	/* Do the initializations early */
 	for_each_iommu(iommu, drhd) {
-		int ret = dmar_enable_qi(iommu);
-
-		if (ret) {
-			printk(KERN_ERR "DRHD %Lx: failed to enable queued, "
-			       " invalidation, ecap %Lx, ret %d\n",
-			       drhd->reg_base_addr, iommu->ecap, ret);
+		if (intel_setup_irq_remapping(iommu)) {
+			pr_err("Failed to setup irq remapping for %s\n",
+			       iommu->name);
 			goto error;
 		}
 	}
 
+	return 0;
+
+error:
+	intel_cleanup_irq_remapping();
+	return -ENODEV;
+}
+
+/*
+ * Set Posted-Interrupts capability.
+ */
+static inline void set_irq_posting_cap(void)
+{
+	struct dmar_drhd_unit *drhd;
+	struct intel_iommu *iommu;
+
+	if (!disable_irq_post) {
+		intel_irq_remap_ops.capability |= 1 << IRQ_POSTING_CAP;
+
+		for_each_iommu(iommu, drhd)
+			if (!cap_pi_support(iommu->cap)) {
+				intel_irq_remap_ops.capability &=
+						~(1 << IRQ_POSTING_CAP);
+				break;
+			}
+	}
+}
+
+static int __init intel_enable_irq_remapping(void)
+{
+	struct dmar_drhd_unit *drhd;
+	struct intel_iommu *iommu;
+	bool setup = false;
+
 	/*
 	 * Setup Interrupt-remapping for all the DRHD's now.
 	 */
 	for_each_iommu(iommu, drhd) {
-		iommu_set_irq_remapping(iommu, eim);
+		if (!ir_pre_enabled(iommu))
+			iommu_enable_irq_remapping(iommu);
 		setup = true;
 	}
 
@@ -702,16 +756,11 @@ static int __init intel_enable_irq_remapping(void)
 
 	irq_remapping_enabled = 1;
 
-	/*
-	 * VT-d has a different layout for IO-APIC entries when
-	 * interrupt remapping is enabled. So it needs a special routine
-	 * to print IO-APIC entries for debugging purposes too.
-	 */
-	x86_io_apic_ops.print_entries = intel_ir_io_apic_print_entries;
+	set_irq_posting_cap();
 
-	pr_info("Enabled IRQ remapping in %s mode\n", eim ? "x2apic" : "xapic");
+	pr_info("Enabled IRQ remapping in %s mode\n", eim_mode ? "x2apic" : "xapic");
 
-	return eim ? IRQ_REMAP_X2APIC_MODE : IRQ_REMAP_XAPIC_MODE;
+	return eim_mode ? IRQ_REMAP_X2APIC_MODE : IRQ_REMAP_XAPIC_MODE;
 
 error:
 	intel_cleanup_irq_remapping();
@@ -909,6 +958,12 @@ static void disable_irq_remapping(void)
 
 		iommu_disable_irq_remapping(iommu);
 	}
+
+	/*
+	 * Clear Posted-Interrupts capability.
+	 */
+	if (!disable_irq_post)
+		intel_irq_remap_ops.capability &= ~(1 << IRQ_POSTING_CAP);
 }
 
 static int reenable_irq_remapping(int eim)
@@ -930,12 +985,15 @@ static int reenable_irq_remapping(int eim)
 
 		/* Set up interrupt remapping for iommu.*/
 		iommu_set_irq_remapping(iommu, eim);
+		iommu_enable_irq_remapping(iommu);
 		setup = true;
 	}
 
 	if (!setup)
 		goto error;
 
+	set_irq_posting_cap();
+
 	return 0;
 
 error:
@@ -945,8 +1003,7 @@ error:
 	return -1;
 }
 
-static void prepare_irte(struct irte *irte, int vector,
-			 unsigned int dest)
+static void prepare_irte(struct irte *irte, int vector, unsigned int dest)
 {
 	memset(irte, 0, sizeof(*irte));
 
@@ -966,76 +1023,63 @@ static void prepare_irte(struct irte *irte, int vector,
 	irte->redir_hint = 1;
 }
 
-static int intel_setup_ioapic_entry(int irq,
-				    struct IO_APIC_route_entry *route_entry,
-				    unsigned int destination, int vector,
-				    struct io_apic_irq_attr *attr)
+static struct irq_domain *intel_get_ir_irq_domain(struct irq_alloc_info *info)
 {
-	int ioapic_id = mpc_ioapic_id(attr->ioapic);
-	struct intel_iommu *iommu;
-	struct IR_IO_APIC_route_entry *entry;
-	struct irte irte;
-	int index;
-
-	down_read(&dmar_global_lock);
-	iommu = map_ioapic_to_ir(ioapic_id);
-	if (!iommu) {
-		pr_warn("No mapping iommu for ioapic %d\n", ioapic_id);
-		index = -ENODEV;
-	} else {
-		index = alloc_irte(iommu, irq, 1);
-		if (index < 0) {
-			pr_warn("Failed to allocate IRTE for ioapic %d\n",
-				ioapic_id);
-			index = -ENOMEM;
-		}
-	}
-	up_read(&dmar_global_lock);
-	if (index < 0)
-		return index;
-
-	prepare_irte(&irte, vector, destination);
+	struct intel_iommu *iommu = NULL;
 
-	/* Set source-id of interrupt request */
-	set_ioapic_sid(&irte, ioapic_id);
+	if (!info)
+		return NULL;
 
-	modify_irte(irq, &irte);
+	switch (info->type) {
+	case X86_IRQ_ALLOC_TYPE_IOAPIC:
+		iommu = map_ioapic_to_ir(info->ioapic_id);
+		break;
+	case X86_IRQ_ALLOC_TYPE_HPET:
+		iommu = map_hpet_to_ir(info->hpet_id);
+		break;
+	case X86_IRQ_ALLOC_TYPE_MSI:
+	case X86_IRQ_ALLOC_TYPE_MSIX:
+		iommu = map_dev_to_ir(info->msi_dev);
+		break;
+	default:
+		BUG_ON(1);
+		break;
+	}
 
-	apic_printk(APIC_VERBOSE, KERN_DEBUG "IOAPIC[%d]: "
-		"Set IRTE entry (P:%d FPD:%d Dst_Mode:%d "
-		"Redir_hint:%d Trig_Mode:%d Dlvry_Mode:%X "
-		"Avail:%X Vector:%02X Dest:%08X "
-		"SID:%04X SQ:%X SVT:%X)\n",
-		attr->ioapic, irte.present, irte.fpd, irte.dst_mode,
-		irte.redir_hint, irte.trigger_mode, irte.dlvry_mode,
-		irte.avail, irte.vector, irte.dest_id,
-		irte.sid, irte.sq, irte.svt);
+	return iommu ? iommu->ir_domain : NULL;
+}
 
-	entry = (struct IR_IO_APIC_route_entry *)route_entry;
-	memset(entry, 0, sizeof(*entry));
+static struct irq_domain *intel_get_irq_domain(struct irq_alloc_info *info)
+{
+	struct intel_iommu *iommu;
 
-	entry->index2	= (index >> 15) & 0x1;
-	entry->zero	= 0;
-	entry->format	= 1;
-	entry->index	= (index & 0x7fff);
-	/*
-	 * IO-APIC RTE will be configured with virtual vector.
-	 * irq handler will do the explicit EOI to the io-apic.
-	 */
-	entry->vector	= attr->ioapic_pin;
-	entry->mask	= 0;			/* enable IRQ */
-	entry->trigger	= attr->trigger;
-	entry->polarity	= attr->polarity;
+	if (!info)
+		return NULL;
 
-	/* Mask level triggered irqs.
-	 * Use IRQ_DELAYED_DISABLE for edge triggered irqs.
-	 */
-	if (attr->trigger)
-		entry->mask = 1;
+	switch (info->type) {
+	case X86_IRQ_ALLOC_TYPE_MSI:
+	case X86_IRQ_ALLOC_TYPE_MSIX:
+		iommu = map_dev_to_ir(info->msi_dev);
+		if (iommu)
+			return iommu->ir_msi_domain;
+		break;
+	default:
+		break;
+	}
 
-	return 0;
+	return NULL;
 }
 
+struct irq_remap_ops intel_irq_remap_ops = {
+	.prepare		= intel_prepare_irq_remapping,
+	.enable			= intel_enable_irq_remapping,
+	.disable		= disable_irq_remapping,
+	.reenable		= reenable_irq_remapping,
+	.enable_faulting	= enable_drhd_fault_handling,
+	.get_ir_irq_domain	= intel_get_ir_irq_domain,
+	.get_irq_domain		= intel_get_irq_domain,
+};
+
 /*
  * Migrate the IO-APIC irq in the presence of intr-remapping.
  *
@@ -1051,170 +1095,281 @@ static int intel_setup_ioapic_entry(int irq,
  * is used to migrate MSI irq's in the presence of interrupt-remapping.
  */
 static int
-intel_ioapic_set_affinity(struct irq_data *data, const struct cpumask *mask,
-			  bool force)
+intel_ir_set_affinity(struct irq_data *data, const struct cpumask *mask,
+		      bool force)
 {
+	struct intel_ir_data *ir_data = data->chip_data;
+	struct irte *irte = &ir_data->irte_entry;
 	struct irq_cfg *cfg = irqd_cfg(data);
-	unsigned int dest, irq = data->irq;
-	struct irte irte;
-	int err;
-
-	if (!config_enabled(CONFIG_SMP))
-		return -EINVAL;
-
-	if (!cpumask_intersects(mask, cpu_online_mask))
-		return -EINVAL;
-
-	if (get_irte(irq, &irte))
-		return -EBUSY;
-
-	err = assign_irq_vector(irq, cfg, mask);
-	if (err)
-		return err;
-
-	err = apic->cpu_mask_to_apicid_and(cfg->domain, mask, &dest);
-	if (err) {
-		if (assign_irq_vector(irq, cfg, data->affinity))
-			pr_err("Failed to recover vector for irq %d\n", irq);
-		return err;
-	}
+	struct irq_data *parent = data->parent_data;
+	int ret;
 
-	irte.vector = cfg->vector;
-	irte.dest_id = IRTE_DEST(dest);
+	ret = parent->chip->irq_set_affinity(parent, mask, force);
+	if (ret < 0 || ret == IRQ_SET_MASK_OK_DONE)
+		return ret;
 
 	/*
 	 * Atomically updates the IRTE with the new destination, vector
 	 * and flushes the interrupt entry cache.
 	 */
-	modify_irte(irq, &irte);
+	irte->vector = cfg->vector;
+	irte->dest_id = IRTE_DEST(cfg->dest_apicid);
+
+	/* Update the hardware only if the interrupt is in remapped mode. */
+	if (ir_data->irq_2_iommu.mode == IRQ_REMAPPING)
+		modify_irte(&ir_data->irq_2_iommu, irte);
 
 	/*
 	 * After this point, all the interrupts will start arriving
 	 * at the new destination. So, time to cleanup the previous
 	 * vector allocation.
 	 */
-	if (cfg->move_in_progress)
-		send_cleanup_vector(cfg);
+	send_cleanup_vector(cfg);
 
-	cpumask_copy(data->affinity, mask);
-	return 0;
+	return IRQ_SET_MASK_OK_DONE;
 }
 
-static void intel_compose_msi_msg(struct pci_dev *pdev,
-				  unsigned int irq, unsigned int dest,
-				  struct msi_msg *msg, u8 hpet_id)
+static void intel_ir_compose_msi_msg(struct irq_data *irq_data,
+				     struct msi_msg *msg)
 {
-	struct irq_cfg *cfg;
-	struct irte irte;
-	u16 sub_handle = 0;
-	int ir_index;
-
-	cfg = irq_cfg(irq);
+	struct intel_ir_data *ir_data = irq_data->chip_data;
 
-	ir_index = map_irq_to_irte_handle(irq, &sub_handle);
-	BUG_ON(ir_index == -1);
+	*msg = ir_data->msi_entry;
+}
 
-	prepare_irte(&irte, cfg->vector, dest);
+static int intel_ir_set_vcpu_affinity(struct irq_data *data, void *info)
+{
+	struct intel_ir_data *ir_data = data->chip_data;
+	struct vcpu_data *vcpu_pi_info = info;
 
-	/* Set source-id of interrupt request */
-	if (pdev)
-		set_msi_sid(&irte, pdev);
-	else
-		set_hpet_sid(&irte, hpet_id);
+	/* stop posting interrupts, back to remapping mode */
+	if (!vcpu_pi_info) {
+		modify_irte(&ir_data->irq_2_iommu, &ir_data->irte_entry);
+	} else {
+		struct irte irte_pi;
 
-	modify_irte(irq, &irte);
+		/*
+		 * We are not caching the posted interrupt entry. We
+		 * copy the data from the remapped entry and modify
+		 * the fields which are relevant for posted mode. The
+		 * cached remapped entry is used for switching back to
+		 * remapped mode.
+		 */
+		memset(&irte_pi, 0, sizeof(irte_pi));
+		dmar_copy_shared_irte(&irte_pi, &ir_data->irte_entry);
+
+		/* Update the posted mode fields */
+		irte_pi.p_pst = 1;
+		irte_pi.p_urgent = 0;
+		irte_pi.p_vector = vcpu_pi_info->vector;
+		irte_pi.pda_l = (vcpu_pi_info->pi_desc_addr >>
+				(32 - PDA_LOW_BIT)) & ~(-1UL << PDA_LOW_BIT);
+		irte_pi.pda_h = (vcpu_pi_info->pi_desc_addr >> 32) &
+				~(-1UL << PDA_HIGH_BIT);
+
+		modify_irte(&ir_data->irq_2_iommu, &irte_pi);
+	}
 
-	msg->address_hi = MSI_ADDR_BASE_HI;
-	msg->data = sub_handle;
-	msg->address_lo = MSI_ADDR_BASE_LO | MSI_ADDR_IR_EXT_INT |
-			  MSI_ADDR_IR_SHV |
-			  MSI_ADDR_IR_INDEX1(ir_index) |
-			  MSI_ADDR_IR_INDEX2(ir_index);
+	return 0;
 }
 
-/*
- * Map the PCI dev to the corresponding remapping hardware unit
- * and allocate 'nvec' consecutive interrupt-remapping table entries
- * in it.
- */
-static int intel_msi_alloc_irq(struct pci_dev *dev, int irq, int nvec)
+static struct irq_chip intel_ir_chip = {
+	.irq_ack = ir_ack_apic_edge,
+	.irq_set_affinity = intel_ir_set_affinity,
+	.irq_compose_msi_msg = intel_ir_compose_msi_msg,
+	.irq_set_vcpu_affinity = intel_ir_set_vcpu_affinity,
+};
+
+static void intel_irq_remapping_prepare_irte(struct intel_ir_data *data,
+					     struct irq_cfg *irq_cfg,
+					     struct irq_alloc_info *info,
+					     int index, int sub_handle)
 {
-	struct intel_iommu *iommu;
-	int index;
+	struct IR_IO_APIC_route_entry *entry;
+	struct irte *irte = &data->irte_entry;
+	struct msi_msg *msg = &data->msi_entry;
+
+	prepare_irte(irte, irq_cfg->vector, irq_cfg->dest_apicid);
+	switch (info->type) {
+	case X86_IRQ_ALLOC_TYPE_IOAPIC:
+		/* Set source-id of interrupt request */
+		set_ioapic_sid(irte, info->ioapic_id);
+		apic_printk(APIC_VERBOSE, KERN_DEBUG "IOAPIC[%d]: Set IRTE entry (P:%d FPD:%d Dst_Mode:%d Redir_hint:%d Trig_Mode:%d Dlvry_Mode:%X Avail:%X Vector:%02X Dest:%08X SID:%04X SQ:%X SVT:%X)\n",
+			info->ioapic_id, irte->present, irte->fpd,
+			irte->dst_mode, irte->redir_hint,
+			irte->trigger_mode, irte->dlvry_mode,
+			irte->avail, irte->vector, irte->dest_id,
+			irte->sid, irte->sq, irte->svt);
+
+		entry = (struct IR_IO_APIC_route_entry *)info->ioapic_entry;
+		info->ioapic_entry = NULL;
+		memset(entry, 0, sizeof(*entry));
+		entry->index2	= (index >> 15) & 0x1;
+		entry->zero	= 0;
+		entry->format	= 1;
+		entry->index	= (index & 0x7fff);
+		/*
+		 * IO-APIC RTE will be configured with virtual vector.
+		 * irq handler will do the explicit EOI to the io-apic.
+		 */
+		entry->vector	= info->ioapic_pin;
+		entry->mask	= 0;			/* enable IRQ */
+		entry->trigger	= info->ioapic_trigger;
+		entry->polarity	= info->ioapic_polarity;
+		if (info->ioapic_trigger)
+			entry->mask = 1; /* Mask level triggered irqs. */
+		break;
+
+	case X86_IRQ_ALLOC_TYPE_HPET:
+	case X86_IRQ_ALLOC_TYPE_MSI:
+	case X86_IRQ_ALLOC_TYPE_MSIX:
+		if (info->type == X86_IRQ_ALLOC_TYPE_HPET)
+			set_hpet_sid(irte, info->hpet_id);
+		else
+			set_msi_sid(irte, info->msi_dev);
+
+		msg->address_hi = MSI_ADDR_BASE_HI;
+		msg->data = sub_handle;
+		msg->address_lo = MSI_ADDR_BASE_LO | MSI_ADDR_IR_EXT_INT |
+				  MSI_ADDR_IR_SHV |
+				  MSI_ADDR_IR_INDEX1(index) |
+				  MSI_ADDR_IR_INDEX2(index);
+		break;
+
+	default:
+		BUG_ON(1);
+		break;
+	}
+}
 
-	down_read(&dmar_global_lock);
-	iommu = map_dev_to_ir(dev);
-	if (!iommu) {
-		printk(KERN_ERR
-		       "Unable to map PCI %s to iommu\n", pci_name(dev));
-		index = -ENOENT;
-	} else {
-		index = alloc_irte(iommu, irq, nvec);
-		if (index < 0) {
-			printk(KERN_ERR
-			       "Unable to allocate %d IRTE for PCI %s\n",
-			       nvec, pci_name(dev));
-			index = -ENOSPC;
+static void intel_free_irq_resources(struct irq_domain *domain,
+				     unsigned int virq, unsigned int nr_irqs)
+{
+	struct irq_data *irq_data;
+	struct intel_ir_data *data;
+	struct irq_2_iommu *irq_iommu;
+	unsigned long flags;
+	int i;
+	for (i = 0; i < nr_irqs; i++) {
+		irq_data = irq_domain_get_irq_data(domain, virq  + i);
+		if (irq_data && irq_data->chip_data) {
+			data = irq_data->chip_data;
+			irq_iommu = &data->irq_2_iommu;
+			raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
+			clear_entries(irq_iommu);
+			raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
+			irq_domain_reset_irq_data(irq_data);
+			kfree(data);
 		}
 	}
-	up_read(&dmar_global_lock);
-
-	return index;
 }
 
-static int intel_msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
-			       int index, int sub_handle)
+static int intel_irq_remapping_alloc(struct irq_domain *domain,
+				     unsigned int virq, unsigned int nr_irqs,
+				     void *arg)
 {
-	struct intel_iommu *iommu;
-	int ret = -ENOENT;
+	struct intel_iommu *iommu = domain->host_data;
+	struct irq_alloc_info *info = arg;
+	struct intel_ir_data *data, *ird;
+	struct irq_data *irq_data;
+	struct irq_cfg *irq_cfg;
+	int i, ret, index;
+
+	if (!info || !iommu)
+		return -EINVAL;
+	if (nr_irqs > 1 && info->type != X86_IRQ_ALLOC_TYPE_MSI &&
+	    info->type != X86_IRQ_ALLOC_TYPE_MSIX)
+		return -EINVAL;
+
+	/*
+	 * With IRQ remapping enabled, don't need contiguous CPU vectors
+	 * to support multiple MSI interrupts.
+	 */
+	if (info->type == X86_IRQ_ALLOC_TYPE_MSI)
+		info->flags &= ~X86_IRQ_ALLOC_CONTIGUOUS_VECTORS;
+
+	ret = irq_domain_alloc_irqs_parent(domain, virq, nr_irqs, arg);
+	if (ret < 0)
+		return ret;
+
+	ret = -ENOMEM;
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		goto out_free_parent;
 
 	down_read(&dmar_global_lock);
-	iommu = map_dev_to_ir(pdev);
-	if (iommu) {
-		/*
-		 * setup the mapping between the irq and the IRTE
-		 * base index, the sub_handle pointing to the
-		 * appropriate interrupt remap table entry.
-		 */
-		set_irte_irq(irq, iommu, index, sub_handle);
-		ret = 0;
-	}
+	index = alloc_irte(iommu, virq, &data->irq_2_iommu, nr_irqs);
 	up_read(&dmar_global_lock);
+	if (index < 0) {
+		pr_warn("Failed to allocate IRTE\n");
+		kfree(data);
+		goto out_free_parent;
+	}
+
+	for (i = 0; i < nr_irqs; i++) {
+		irq_data = irq_domain_get_irq_data(domain, virq + i);
+		irq_cfg = irqd_cfg(irq_data);
+		if (!irq_data || !irq_cfg) {
+			ret = -EINVAL;
+			goto out_free_data;
+		}
+
+		if (i > 0) {
+			ird = kzalloc(sizeof(*ird), GFP_KERNEL);
+			if (!ird)
+				goto out_free_data;
+			/* Initialize the common data */
+			ird->irq_2_iommu = data->irq_2_iommu;
+			ird->irq_2_iommu.sub_handle = i;
+		} else {
+			ird = data;
+		}
 
+		irq_data->hwirq = (index << 16) + i;
+		irq_data->chip_data = ird;
+		irq_data->chip = &intel_ir_chip;
+		intel_irq_remapping_prepare_irte(ird, irq_cfg, info, index, i);
+		irq_set_status_flags(virq + i, IRQ_MOVE_PCNTXT);
+	}
+	return 0;
+
+out_free_data:
+	intel_free_irq_resources(domain, virq, i);
+out_free_parent:
+	irq_domain_free_irqs_common(domain, virq, nr_irqs);
 	return ret;
 }
 
-static int intel_alloc_hpet_msi(unsigned int irq, unsigned int id)
+static void intel_irq_remapping_free(struct irq_domain *domain,
+				     unsigned int virq, unsigned int nr_irqs)
 {
-	int ret = -1;
-	struct intel_iommu *iommu;
-	int index;
+	intel_free_irq_resources(domain, virq, nr_irqs);
+	irq_domain_free_irqs_common(domain, virq, nr_irqs);
+}
 
-	down_read(&dmar_global_lock);
-	iommu = map_hpet_to_ir(id);
-	if (iommu) {
-		index = alloc_irte(iommu, irq, 1);
-		if (index >= 0)
-			ret = 0;
-	}
-	up_read(&dmar_global_lock);
+static void intel_irq_remapping_activate(struct irq_domain *domain,
+					 struct irq_data *irq_data)
+{
+	struct intel_ir_data *data = irq_data->chip_data;
 
-	return ret;
+	modify_irte(&data->irq_2_iommu, &data->irte_entry);
 }
 
-struct irq_remap_ops intel_irq_remap_ops = {
-	.prepare		= intel_prepare_irq_remapping,
-	.enable			= intel_enable_irq_remapping,
-	.disable		= disable_irq_remapping,
-	.reenable		= reenable_irq_remapping,
-	.enable_faulting	= enable_drhd_fault_handling,
-	.setup_ioapic_entry	= intel_setup_ioapic_entry,
-	.set_affinity		= intel_ioapic_set_affinity,
-	.free_irq		= free_irte,
-	.compose_msi_msg	= intel_compose_msi_msg,
-	.msi_alloc_irq		= intel_msi_alloc_irq,
-	.msi_setup_irq		= intel_msi_setup_irq,
-	.alloc_hpet_msi		= intel_alloc_hpet_msi,
+static void intel_irq_remapping_deactivate(struct irq_domain *domain,
+					   struct irq_data *irq_data)
+{
+	struct intel_ir_data *data = irq_data->chip_data;
+	struct irte entry;
+
+	memset(&entry, 0, sizeof(entry));
+	modify_irte(&data->irq_2_iommu, &entry);
+}
+
+static struct irq_domain_ops intel_ir_domain_ops = {
+	.alloc = intel_irq_remapping_alloc,
+	.free = intel_irq_remapping_free,
+	.activate = intel_irq_remapping_activate,
+	.deactivate = intel_irq_remapping_deactivate,
 };
 
 /*
@@ -1242,28 +1397,12 @@ static int dmar_ir_add(struct dmar_drhd_unit *dmaru, struct intel_iommu *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);
+		pr_err("Failed to setup irq remapping for %s\n",
+		       iommu->name);
 		intel_teardown_irq_remapping(iommu);
 		ir_remove_ioapic_hpet_scope(iommu);
+	} else {
+		iommu_enable_irq_remapping(iommu);
 	}
 
 	return ret;
@@ -1280,6 +1419,9 @@ int dmar_ir_hotplug(struct dmar_drhd_unit *dmaru, bool insert)
 		return -EINVAL;
 	if (!ecap_ir_support(iommu->ecap))
 		return 0;
+	if (irq_remapping_cap(IRQ_POSTING_CAP) &&
+	    !cap_pi_support(iommu->cap))
+		return -EBUSY;
 
 	if (insert) {
 		if (!iommu->ir_table)
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index d4f527e56679..f286090931cc 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -16,7 +16,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  */
 
-#define pr_fmt(fmt)    "%s: " fmt, __func__
+#define pr_fmt(fmt)    "iommu: " fmt
 
 #include <linux/device.h>
 #include <linux/kernel.h>
@@ -51,6 +51,8 @@ struct iommu_group {
 	void (*iommu_data_release)(void *iommu_data);
 	char *name;
 	int id;
+	struct iommu_domain *default_domain;
+	struct iommu_domain *domain;
 };
 
 struct iommu_device {
@@ -75,6 +77,15 @@ struct iommu_group_attribute iommu_group_attr_##_name =		\
 #define to_iommu_group(_kobj)		\
 	container_of(_kobj, struct iommu_group, kobj)
 
+static struct iommu_domain *__iommu_domain_alloc(struct bus_type *bus,
+						 unsigned type);
+static int __iommu_attach_device(struct iommu_domain *domain,
+				 struct device *dev);
+static int __iommu_attach_group(struct iommu_domain *domain,
+				struct iommu_group *group);
+static void __iommu_detach_group(struct iommu_domain *domain,
+				 struct iommu_group *group);
+
 static ssize_t iommu_group_attr_show(struct kobject *kobj,
 				     struct attribute *__attr, char *buf)
 {
@@ -128,6 +139,8 @@ static void iommu_group_release(struct kobject *kobj)
 {
 	struct iommu_group *group = to_iommu_group(kobj);
 
+	pr_debug("Releasing group %d\n", group->id);
+
 	if (group->iommu_data_release)
 		group->iommu_data_release(group->iommu_data);
 
@@ -135,6 +148,9 @@ static void iommu_group_release(struct kobject *kobj)
 	ida_remove(&iommu_group_ida, group->id);
 	mutex_unlock(&iommu_group_mutex);
 
+	if (group->default_domain)
+		iommu_domain_free(group->default_domain);
+
 	kfree(group->name);
 	kfree(group);
 }
@@ -207,6 +223,8 @@ again:
 	 */
 	kobject_put(&group->kobj);
 
+	pr_debug("Allocated group %d\n", group->id);
+
 	return group;
 }
 EXPORT_SYMBOL_GPL(iommu_group_alloc);
@@ -307,6 +325,52 @@ int iommu_group_set_name(struct iommu_group *group, const char *name)
 }
 EXPORT_SYMBOL_GPL(iommu_group_set_name);
 
+static int iommu_group_create_direct_mappings(struct iommu_group *group,
+					      struct device *dev)
+{
+	struct iommu_domain *domain = group->default_domain;
+	struct iommu_dm_region *entry;
+	struct list_head mappings;
+	unsigned long pg_size;
+	int ret = 0;
+
+	if (!domain || domain->type != IOMMU_DOMAIN_DMA)
+		return 0;
+
+	BUG_ON(!domain->ops->pgsize_bitmap);
+
+	pg_size = 1UL << __ffs(domain->ops->pgsize_bitmap);
+	INIT_LIST_HEAD(&mappings);
+
+	iommu_get_dm_regions(dev, &mappings);
+
+	/* We need to consider overlapping regions for different devices */
+	list_for_each_entry(entry, &mappings, list) {
+		dma_addr_t start, end, addr;
+
+		start = ALIGN(entry->start, pg_size);
+		end   = ALIGN(entry->start + entry->length, pg_size);
+
+		for (addr = start; addr < end; addr += pg_size) {
+			phys_addr_t phys_addr;
+
+			phys_addr = iommu_iova_to_phys(domain, addr);
+			if (phys_addr)
+				continue;
+
+			ret = iommu_map(domain, addr, addr, pg_size, entry->prot);
+			if (ret)
+				goto out;
+		}
+
+	}
+
+out:
+	iommu_put_dm_regions(dev, &mappings);
+
+	return ret;
+}
+
 /**
  * iommu_group_add_device - add a device to an iommu group
  * @group: the group into which to add the device (reference should be held)
@@ -363,8 +427,12 @@ rename:
 
 	dev->iommu_group = group;
 
+	iommu_group_create_direct_mappings(group, dev);
+
 	mutex_lock(&group->mutex);
 	list_add_tail(&device->list, &group->devices);
+	if (group->domain)
+		__iommu_attach_device(group->domain, dev);
 	mutex_unlock(&group->mutex);
 
 	/* Notify any listeners about change to group. */
@@ -372,6 +440,9 @@ rename:
 				     IOMMU_GROUP_NOTIFY_ADD_DEVICE, dev);
 
 	trace_add_device_to_group(group->id, dev);
+
+	pr_info("Adding device %s to group %d\n", dev_name(dev), group->id);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(iommu_group_add_device);
@@ -388,6 +459,8 @@ void iommu_group_remove_device(struct device *dev)
 	struct iommu_group *group = dev->iommu_group;
 	struct iommu_device *tmp_device, *device = NULL;
 
+	pr_info("Removing device %s from group %d\n", dev_name(dev), group->id);
+
 	/* Pre-notify listeners that a device is being removed. */
 	blocking_notifier_call_chain(&group->notifier,
 				     IOMMU_GROUP_NOTIFY_DEL_DEVICE, dev);
@@ -417,6 +490,17 @@ void iommu_group_remove_device(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(iommu_group_remove_device);
 
+static int iommu_group_device_count(struct iommu_group *group)
+{
+	struct iommu_device *entry;
+	int ret = 0;
+
+	list_for_each_entry(entry, &group->devices, list)
+		ret++;
+
+	return ret;
+}
+
 /**
  * iommu_group_for_each_dev - iterate over each device in the group
  * @group: the group
@@ -428,19 +512,30 @@ EXPORT_SYMBOL_GPL(iommu_group_remove_device);
  * The group->mutex is held across callbacks, which will block calls to
  * iommu_group_add/remove_device.
  */
-int iommu_group_for_each_dev(struct iommu_group *group, void *data,
-			     int (*fn)(struct device *, void *))
+static int __iommu_group_for_each_dev(struct iommu_group *group, void *data,
+				      int (*fn)(struct device *, void *))
 {
 	struct iommu_device *device;
 	int ret = 0;
 
-	mutex_lock(&group->mutex);
 	list_for_each_entry(device, &group->devices, list) {
 		ret = fn(device->dev, data);
 		if (ret)
 			break;
 	}
+	return ret;
+}
+
+
+int iommu_group_for_each_dev(struct iommu_group *group, void *data,
+			     int (*fn)(struct device *, void *))
+{
+	int ret;
+
+	mutex_lock(&group->mutex);
+	ret = __iommu_group_for_each_dev(group, data, fn);
 	mutex_unlock(&group->mutex);
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(iommu_group_for_each_dev);
@@ -692,7 +787,19 @@ static struct iommu_group *iommu_group_get_for_pci_dev(struct pci_dev *pdev)
 		return group;
 
 	/* No shared group found, allocate new */
-	return iommu_group_alloc();
+	group = iommu_group_alloc();
+	if (IS_ERR(group))
+		return NULL;
+
+	/*
+	 * Try to allocate a default domain - needs support from the
+	 * IOMMU driver.
+	 */
+	group->default_domain = __iommu_domain_alloc(pdev->dev.bus,
+						     IOMMU_DOMAIN_DMA);
+	group->domain = group->default_domain;
+
+	return group;
 }
 
 /**
@@ -731,17 +838,42 @@ struct iommu_group *iommu_group_get_for_dev(struct device *dev)
 	return group;
 }
 
+struct iommu_domain *iommu_group_default_domain(struct iommu_group *group)
+{
+	return group->default_domain;
+}
+
 static int add_iommu_group(struct device *dev, void *data)
 {
 	struct iommu_callback_data *cb = data;
 	const struct iommu_ops *ops = cb->ops;
+	int ret;
 
 	if (!ops->add_device)
 		return 0;
 
 	WARN_ON(dev->iommu_group);
 
-	ops->add_device(dev);
+	ret = ops->add_device(dev);
+
+	/*
+	 * We ignore -ENODEV errors for now, as they just mean that the
+	 * device is not translated by an IOMMU. We still care about
+	 * other errors and fail to initialize when they happen.
+	 */
+	if (ret == -ENODEV)
+		ret = 0;
+
+	return ret;
+}
+
+static int remove_iommu_group(struct device *dev, void *data)
+{
+	struct iommu_callback_data *cb = data;
+	const struct iommu_ops *ops = cb->ops;
+
+	if (ops->remove_device && dev->iommu_group)
+		ops->remove_device(dev);
 
 	return 0;
 }
@@ -761,7 +893,7 @@ static int iommu_bus_notifier(struct notifier_block *nb,
 	if (action == BUS_NOTIFY_ADD_DEVICE) {
 		if (ops->add_device)
 			return ops->add_device(dev);
-	} else if (action == BUS_NOTIFY_DEL_DEVICE) {
+	} else if (action == BUS_NOTIFY_REMOVED_DEVICE) {
 		if (ops->remove_device && dev->iommu_group) {
 			ops->remove_device(dev);
 			return 0;
@@ -814,19 +946,25 @@ static int iommu_bus_init(struct bus_type *bus, const struct iommu_ops *ops)
 	nb->notifier_call = iommu_bus_notifier;
 
 	err = bus_register_notifier(bus, nb);
-	if (err) {
-		kfree(nb);
-		return err;
-	}
+	if (err)
+		goto out_free;
 
 	err = bus_for_each_dev(bus, NULL, &cb, add_iommu_group);
-	if (err) {
-		bus_unregister_notifier(bus, nb);
-		kfree(nb);
-		return err;
-	}
+	if (err)
+		goto out_err;
+
 
 	return 0;
+
+out_err:
+	/* Clean up */
+	bus_for_each_dev(bus, NULL, &cb, remove_iommu_group);
+	bus_unregister_notifier(bus, nb);
+
+out_free:
+	kfree(nb);
+
+	return err;
 }
 
 /**
@@ -898,22 +1036,28 @@ void iommu_set_fault_handler(struct iommu_domain *domain,
 }
 EXPORT_SYMBOL_GPL(iommu_set_fault_handler);
 
-struct iommu_domain *iommu_domain_alloc(struct bus_type *bus)
+static struct iommu_domain *__iommu_domain_alloc(struct bus_type *bus,
+						 unsigned type)
 {
 	struct iommu_domain *domain;
 
 	if (bus == NULL || bus->iommu_ops == NULL)
 		return NULL;
 
-	domain = bus->iommu_ops->domain_alloc(IOMMU_DOMAIN_UNMANAGED);
+	domain = bus->iommu_ops->domain_alloc(type);
 	if (!domain)
 		return NULL;
 
 	domain->ops  = bus->iommu_ops;
-	domain->type = IOMMU_DOMAIN_UNMANAGED;
+	domain->type = type;
 
 	return domain;
 }
+
+struct iommu_domain *iommu_domain_alloc(struct bus_type *bus)
+{
+	return __iommu_domain_alloc(bus, IOMMU_DOMAIN_UNMANAGED);
+}
 EXPORT_SYMBOL_GPL(iommu_domain_alloc);
 
 void iommu_domain_free(struct iommu_domain *domain)
@@ -922,7 +1066,8 @@ void iommu_domain_free(struct iommu_domain *domain)
 }
 EXPORT_SYMBOL_GPL(iommu_domain_free);
 
-int iommu_attach_device(struct iommu_domain *domain, struct device *dev)
+static int __iommu_attach_device(struct iommu_domain *domain,
+				 struct device *dev)
 {
 	int ret;
 	if (unlikely(domain->ops->attach_dev == NULL))
@@ -933,9 +1078,38 @@ int iommu_attach_device(struct iommu_domain *domain, struct device *dev)
 		trace_attach_device_to_domain(dev);
 	return ret;
 }
+
+int iommu_attach_device(struct iommu_domain *domain, struct device *dev)
+{
+	struct iommu_group *group;
+	int ret;
+
+	group = iommu_group_get(dev);
+	/* FIXME: Remove this when groups a mandatory for iommu drivers */
+	if (group == NULL)
+		return __iommu_attach_device(domain, dev);
+
+	/*
+	 * We have a group - lock it to make sure the device-count doesn't
+	 * change while we are attaching
+	 */
+	mutex_lock(&group->mutex);
+	ret = -EINVAL;
+	if (iommu_group_device_count(group) != 1)
+		goto out_unlock;
+
+	ret = __iommu_attach_group(domain, group);
+
+out_unlock:
+	mutex_unlock(&group->mutex);
+	iommu_group_put(group);
+
+	return ret;
+}
 EXPORT_SYMBOL_GPL(iommu_attach_device);
 
-void iommu_detach_device(struct iommu_domain *domain, struct device *dev)
+static void __iommu_detach_device(struct iommu_domain *domain,
+				  struct device *dev)
 {
 	if (unlikely(domain->ops->detach_dev == NULL))
 		return;
@@ -943,8 +1117,48 @@ void iommu_detach_device(struct iommu_domain *domain, struct device *dev)
 	domain->ops->detach_dev(domain, dev);
 	trace_detach_device_from_domain(dev);
 }
+
+void iommu_detach_device(struct iommu_domain *domain, struct device *dev)
+{
+	struct iommu_group *group;
+
+	group = iommu_group_get(dev);
+	/* FIXME: Remove this when groups a mandatory for iommu drivers */
+	if (group == NULL)
+		return __iommu_detach_device(domain, dev);
+
+	mutex_lock(&group->mutex);
+	if (iommu_group_device_count(group) != 1) {
+		WARN_ON(1);
+		goto out_unlock;
+	}
+
+	__iommu_detach_group(domain, group);
+
+out_unlock:
+	mutex_unlock(&group->mutex);
+	iommu_group_put(group);
+}
 EXPORT_SYMBOL_GPL(iommu_detach_device);
 
+struct iommu_domain *iommu_get_domain_for_dev(struct device *dev)
+{
+	struct iommu_domain *domain;
+	struct iommu_group *group;
+
+	group = iommu_group_get(dev);
+	/* FIXME: Remove this when groups a mandatory for iommu drivers */
+	if (group == NULL)
+		return NULL;
+
+	domain = group->domain;
+
+	iommu_group_put(group);
+
+	return domain;
+}
+EXPORT_SYMBOL_GPL(iommu_get_domain_for_dev);
+
 /*
  * IOMMU groups are really the natrual working unit of the IOMMU, but
  * the IOMMU API works on domains and devices.  Bridge that gap by
@@ -959,13 +1173,34 @@ static int iommu_group_do_attach_device(struct device *dev, void *data)
 {
 	struct iommu_domain *domain = data;
 
-	return iommu_attach_device(domain, dev);
+	return __iommu_attach_device(domain, dev);
+}
+
+static int __iommu_attach_group(struct iommu_domain *domain,
+				struct iommu_group *group)
+{
+	int ret;
+
+	if (group->default_domain && group->domain != group->default_domain)
+		return -EBUSY;
+
+	ret = __iommu_group_for_each_dev(group, domain,
+					 iommu_group_do_attach_device);
+	if (ret == 0)
+		group->domain = domain;
+
+	return ret;
 }
 
 int iommu_attach_group(struct iommu_domain *domain, struct iommu_group *group)
 {
-	return iommu_group_for_each_dev(group, domain,
-					iommu_group_do_attach_device);
+	int ret;
+
+	mutex_lock(&group->mutex);
+	ret = __iommu_attach_group(domain, group);
+	mutex_unlock(&group->mutex);
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(iommu_attach_group);
 
@@ -973,14 +1208,40 @@ static int iommu_group_do_detach_device(struct device *dev, void *data)
 {
 	struct iommu_domain *domain = data;
 
-	iommu_detach_device(domain, dev);
+	__iommu_detach_device(domain, dev);
 
 	return 0;
 }
 
+static void __iommu_detach_group(struct iommu_domain *domain,
+				 struct iommu_group *group)
+{
+	int ret;
+
+	if (!group->default_domain) {
+		__iommu_group_for_each_dev(group, domain,
+					   iommu_group_do_detach_device);
+		group->domain = NULL;
+		return;
+	}
+
+	if (group->domain == group->default_domain)
+		return;
+
+	/* Detach by re-attaching to the default domain */
+	ret = __iommu_group_for_each_dev(group, group->default_domain,
+					 iommu_group_do_attach_device);
+	if (ret != 0)
+		WARN_ON(1);
+	else
+		group->domain = group->default_domain;
+}
+
 void iommu_detach_group(struct iommu_domain *domain, struct iommu_group *group)
 {
-	iommu_group_for_each_dev(group, domain, iommu_group_do_detach_device);
+	mutex_lock(&group->mutex);
+	__iommu_detach_group(domain, group);
+	mutex_unlock(&group->mutex);
 }
 EXPORT_SYMBOL_GPL(iommu_detach_group);
 
@@ -1207,7 +1468,7 @@ static int __init iommu_init(void)
 
 	return 0;
 }
-arch_initcall(iommu_init);
+core_initcall(iommu_init);
 
 int iommu_domain_get_attr(struct iommu_domain *domain,
 			  enum iommu_attr attr, void *data)
@@ -1273,3 +1534,72 @@ int iommu_domain_set_attr(struct iommu_domain *domain,
 	return ret;
 }
 EXPORT_SYMBOL_GPL(iommu_domain_set_attr);
+
+void iommu_get_dm_regions(struct device *dev, struct list_head *list)
+{
+	const struct iommu_ops *ops = dev->bus->iommu_ops;
+
+	if (ops && ops->get_dm_regions)
+		ops->get_dm_regions(dev, list);
+}
+
+void iommu_put_dm_regions(struct device *dev, struct list_head *list)
+{
+	const struct iommu_ops *ops = dev->bus->iommu_ops;
+
+	if (ops && ops->put_dm_regions)
+		ops->put_dm_regions(dev, list);
+}
+
+/* Request that a device is direct mapped by the IOMMU */
+int iommu_request_dm_for_dev(struct device *dev)
+{
+	struct iommu_domain *dm_domain;
+	struct iommu_group *group;
+	int ret;
+
+	/* Device must already be in a group before calling this function */
+	group = iommu_group_get_for_dev(dev);
+	if (IS_ERR(group))
+		return PTR_ERR(group);
+
+	mutex_lock(&group->mutex);
+
+	/* Check if the default domain is already direct mapped */
+	ret = 0;
+	if (group->default_domain &&
+	    group->default_domain->type == IOMMU_DOMAIN_IDENTITY)
+		goto out;
+
+	/* Don't change mappings of existing devices */
+	ret = -EBUSY;
+	if (iommu_group_device_count(group) != 1)
+		goto out;
+
+	/* Allocate a direct mapped domain */
+	ret = -ENOMEM;
+	dm_domain = __iommu_domain_alloc(dev->bus, IOMMU_DOMAIN_IDENTITY);
+	if (!dm_domain)
+		goto out;
+
+	/* Attach the device to the domain */
+	ret = __iommu_attach_group(dm_domain, group);
+	if (ret) {
+		iommu_domain_free(dm_domain);
+		goto out;
+	}
+
+	/* Make the direct mapped domain the default for this group */
+	if (group->default_domain)
+		iommu_domain_free(group->default_domain);
+	group->default_domain = dm_domain;
+
+	pr_info("Using direct mapping for device %s\n", dev_name(dev));
+
+	ret = 0;
+out:
+	mutex_unlock(&group->mutex);
+	iommu_group_put(group);
+
+	return ret;
+}
diff --git a/drivers/iommu/iova.c b/drivers/iommu/iova.c
index 9dd8208312c2..b7c3d923f3e1 100644
--- a/drivers/iommu/iova.c
+++ b/drivers/iommu/iova.c
@@ -227,6 +227,7 @@ iova_insert_rbtree(struct rb_root *root, struct iova *iova)
 	/* Figure out where to put new node */
 	while (*new) {
 		struct iova *this = container_of(*new, struct iova, node);
+
 		parent = *new;
 
 		if (iova->pfn_lo < this->pfn_lo)
@@ -350,6 +351,7 @@ void
 free_iova(struct iova_domain *iovad, unsigned long pfn)
 {
 	struct iova *iova = find_iova(iovad, pfn);
+
 	if (iova)
 		__free_iova(iovad, iova);
 
@@ -369,6 +371,7 @@ void put_iova_domain(struct iova_domain *iovad)
 	node = rb_first(&iovad->rbroot);
 	while (node) {
 		struct iova *iova = container_of(node, struct iova, node);
+
 		rb_erase(node, &iovad->rbroot);
 		free_iova_mem(iova);
 		node = rb_first(&iovad->rbroot);
@@ -482,6 +485,7 @@ copy_reserved_iova(struct iova_domain *from, struct iova_domain *to)
 	for (node = rb_first(&from->rbroot); node; node = rb_next(node)) {
 		struct iova *iova = container_of(node, struct iova, node);
 		struct iova *new_iova;
+
 		new_iova = reserve_iova(to, iova->pfn_lo, iova->pfn_hi);
 		if (!new_iova)
 			printk(KERN_ERR "Reserve iova range %lx@%lx failed\n",
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
index 390079ee1350..2d9993062ded 100644
--- a/drivers/iommu/irq_remapping.c
+++ b/drivers/iommu/irq_remapping.c
@@ -6,6 +6,7 @@
 #include <linux/msi.h>
 #include <linux/irq.h>
 #include <linux/pci.h>
+#include <linux/irqdomain.h>
 
 #include <asm/hw_irq.h>
 #include <asm/irq_remapping.h>
@@ -21,21 +22,11 @@ int irq_remap_broken;
 int disable_sourceid_checking;
 int no_x2apic_optout;
 
+int disable_irq_post = 1;
+
 static int disable_irq_remap;
 static struct irq_remap_ops *remap_ops;
 
-static int msi_alloc_remapped_irq(struct pci_dev *pdev, int irq, int nvec);
-static int msi_setup_remapped_irq(struct pci_dev *pdev, unsigned int irq,
-				  int index, int sub_handle);
-static int set_remapped_irq_affinity(struct irq_data *data,
-				     const struct cpumask *mask,
-				     bool force);
-
-static bool irq_remapped(struct irq_cfg *cfg)
-{
-	return (cfg->remapped == 1);
-}
-
 static void irq_remapping_disable_io_apic(void)
 {
 	/*
@@ -49,117 +40,9 @@ static void irq_remapping_disable_io_apic(void)
 		disconnect_bsp_APIC(0);
 }
 
-static int do_setup_msi_irqs(struct pci_dev *dev, int nvec)
-{
-	int ret, sub_handle, nvec_pow2, index = 0;
-	unsigned int irq;
-	struct msi_desc *msidesc;
-
-	msidesc = list_entry(dev->msi_list.next, struct msi_desc, list);
-
-	irq = irq_alloc_hwirqs(nvec, dev_to_node(&dev->dev));
-	if (irq == 0)
-		return -ENOSPC;
-
-	nvec_pow2 = __roundup_pow_of_two(nvec);
-	for (sub_handle = 0; sub_handle < nvec; sub_handle++) {
-		if (!sub_handle) {
-			index = msi_alloc_remapped_irq(dev, irq, nvec_pow2);
-			if (index < 0) {
-				ret = index;
-				goto error;
-			}
-		} else {
-			ret = msi_setup_remapped_irq(dev, irq + sub_handle,
-						     index, sub_handle);
-			if (ret < 0)
-				goto error;
-		}
-		ret = setup_msi_irq(dev, msidesc, irq, sub_handle);
-		if (ret < 0)
-			goto error;
-	}
-	return 0;
-
-error:
-	irq_free_hwirqs(irq, nvec);
-
-	/*
-	 * Restore altered MSI descriptor fields and prevent just destroyed
-	 * IRQs from tearing down again in default_teardown_msi_irqs()
-	 */
-	msidesc->irq = 0;
-
-	return ret;
-}
-
-static int do_setup_msix_irqs(struct pci_dev *dev, int nvec)
-{
-	int node, ret, sub_handle, index = 0;
-	struct msi_desc *msidesc;
-	unsigned int irq;
-
-	node		= dev_to_node(&dev->dev);
-	sub_handle	= 0;
-
-	list_for_each_entry(msidesc, &dev->msi_list, list) {
-
-		irq = irq_alloc_hwirq(node);
-		if (irq == 0)
-			return -1;
-
-		if (sub_handle == 0)
-			ret = index = msi_alloc_remapped_irq(dev, irq, nvec);
-		else
-			ret = msi_setup_remapped_irq(dev, irq, index, sub_handle);
-
-		if (ret < 0)
-			goto error;
-
-		ret = setup_msi_irq(dev, msidesc, irq, 0);
-		if (ret < 0)
-			goto error;
-
-		sub_handle += 1;
-		irq        += 1;
-	}
-
-	return 0;
-
-error:
-	irq_free_hwirq(irq);
-	return ret;
-}
-
-static int irq_remapping_setup_msi_irqs(struct pci_dev *dev,
-					int nvec, int type)
-{
-	if (type == PCI_CAP_ID_MSI)
-		return do_setup_msi_irqs(dev, nvec);
-	else
-		return do_setup_msix_irqs(dev, nvec);
-}
-
-static void eoi_ioapic_pin_remapped(int apic, int pin, int vector)
-{
-	/*
-	 * Intr-remapping uses pin number as the virtual vector
-	 * in the RTE. Actual vector is programmed in
-	 * intr-remapping table entry. Hence for the io-apic
-	 * EOI we use the pin number.
-	 */
-	io_apic_eoi(apic, pin);
-}
-
 static void __init irq_remapping_modify_x86_ops(void)
 {
 	x86_io_apic_ops.disable		= irq_remapping_disable_io_apic;
-	x86_io_apic_ops.set_affinity	= set_remapped_irq_affinity;
-	x86_io_apic_ops.setup_entry	= setup_ioapic_remapped_entry;
-	x86_io_apic_ops.eoi_ioapic_pin	= eoi_ioapic_pin_remapped;
-	x86_msi.setup_msi_irqs		= irq_remapping_setup_msi_irqs;
-	x86_msi.setup_hpet_msi		= setup_hpet_msi_remapped;
-	x86_msi.compose_msi_msg		= compose_remapped_msi_msg;
 }
 
 static __init int setup_nointremap(char *str)
@@ -198,6 +81,15 @@ void set_irq_remapping_broken(void)
 	irq_remap_broken = 1;
 }
 
+bool irq_remapping_cap(enum irq_remap_cap cap)
+{
+	if (!remap_ops || disable_irq_post)
+		return 0;
+
+	return (remap_ops->capability & (1 << cap));
+}
+EXPORT_SYMBOL_GPL(irq_remapping_cap);
+
 int __init irq_remapping_prepare(void)
 {
 	if (disable_irq_remap)
@@ -254,113 +146,48 @@ int __init irq_remap_enable_fault_handling(void)
 	return remap_ops->enable_faulting();
 }
 
-int setup_ioapic_remapped_entry(int irq,
-				struct IO_APIC_route_entry *entry,
-				unsigned int destination, int vector,
-				struct io_apic_irq_attr *attr)
-{
-	if (!remap_ops->setup_ioapic_entry)
-		return -ENODEV;
-
-	return remap_ops->setup_ioapic_entry(irq, entry, destination,
-					     vector, attr);
-}
-
-static int set_remapped_irq_affinity(struct irq_data *data,
-				     const struct cpumask *mask, bool force)
-{
-	if (!config_enabled(CONFIG_SMP) || !remap_ops->set_affinity)
-		return 0;
-
-	return remap_ops->set_affinity(data, mask, force);
-}
-
-void free_remapped_irq(int irq)
-{
-	struct irq_cfg *cfg = irq_cfg(irq);
-
-	if (irq_remapped(cfg) && remap_ops->free_irq)
-		remap_ops->free_irq(irq);
-}
-
-void compose_remapped_msi_msg(struct pci_dev *pdev,
-			      unsigned int irq, unsigned int dest,
-			      struct msi_msg *msg, u8 hpet_id)
-{
-	struct irq_cfg *cfg = irq_cfg(irq);
-
-	if (!irq_remapped(cfg))
-		native_compose_msi_msg(pdev, irq, dest, msg, hpet_id);
-	else if (remap_ops->compose_msi_msg)
-		remap_ops->compose_msi_msg(pdev, irq, dest, msg, hpet_id);
-}
-
-static int msi_alloc_remapped_irq(struct pci_dev *pdev, int irq, int nvec)
-{
-	if (!remap_ops->msi_alloc_irq)
-		return -ENODEV;
-
-	return remap_ops->msi_alloc_irq(pdev, irq, nvec);
-}
-
-static int msi_setup_remapped_irq(struct pci_dev *pdev, unsigned int irq,
-				  int index, int sub_handle)
-{
-	if (!remap_ops->msi_setup_irq)
-		return -ENODEV;
-
-	return remap_ops->msi_setup_irq(pdev, irq, index, sub_handle);
-}
-
-int setup_hpet_msi_remapped(unsigned int irq, unsigned int id)
-{
-	int ret;
-
-	if (!remap_ops->alloc_hpet_msi)
-		return -ENODEV;
-
-	ret = remap_ops->alloc_hpet_msi(irq, id);
-	if (ret)
-		return -EINVAL;
-
-	return default_setup_hpet_msi(irq, id);
-}
-
 void panic_if_irq_remap(const char *msg)
 {
 	if (irq_remapping_enabled)
 		panic(msg);
 }
 
-static void ir_ack_apic_edge(struct irq_data *data)
+void ir_ack_apic_edge(struct irq_data *data)
 {
 	ack_APIC_irq();
 }
 
-static void ir_ack_apic_level(struct irq_data *data)
+/**
+ * irq_remapping_get_ir_irq_domain - Get the irqdomain associated with the IOMMU
+ *				     device serving request @info
+ * @info: interrupt allocation information, used to identify the IOMMU device
+ *
+ * It's used to get parent irqdomain for HPET and IOAPIC irqdomains.
+ * Returns pointer to IRQ domain, or NULL on failure.
+ */
+struct irq_domain *
+irq_remapping_get_ir_irq_domain(struct irq_alloc_info *info)
 {
-	ack_APIC_irq();
-	eoi_ioapic_irq(data->irq, irqd_cfg(data));
-}
+	if (!remap_ops || !remap_ops->get_ir_irq_domain)
+		return NULL;
 
-static void ir_print_prefix(struct irq_data *data, struct seq_file *p)
-{
-	seq_printf(p, " IR-%s", data->chip->name);
+	return remap_ops->get_ir_irq_domain(info);
 }
 
-void irq_remap_modify_chip_defaults(struct irq_chip *chip)
+/**
+ * irq_remapping_get_irq_domain - Get the irqdomain serving the request @info
+ * @info: interrupt allocation information, used to identify the IOMMU device
+ *
+ * There will be one PCI MSI/MSIX irqdomain associated with each interrupt
+ * remapping device, so this interface is used to retrieve the PCI MSI/MSIX
+ * irqdomain serving request @info.
+ * Returns pointer to IRQ domain, or NULL on failure.
+ */
+struct irq_domain *
+irq_remapping_get_irq_domain(struct irq_alloc_info *info)
 {
-	chip->irq_print_chip = ir_print_prefix;
-	chip->irq_ack = ir_ack_apic_edge;
-	chip->irq_eoi = ir_ack_apic_level;
-	chip->irq_set_affinity = x86_io_apic_ops.set_affinity;
-}
+	if (!remap_ops || !remap_ops->get_irq_domain)
+		return NULL;
 
-bool setup_remapped_irq(int irq, struct irq_cfg *cfg, struct irq_chip *chip)
-{
-	if (!irq_remapped(cfg))
-		return false;
-	irq_set_status_flags(irq, IRQ_MOVE_PCNTXT);
-	irq_remap_modify_chip_defaults(chip);
-	return true;
+	return remap_ops->get_irq_domain(info);
 }
diff --git a/drivers/iommu/irq_remapping.h b/drivers/iommu/irq_remapping.h
index 7c70cc29ffe6..039c7af7b190 100644
--- a/drivers/iommu/irq_remapping.h
+++ b/drivers/iommu/irq_remapping.h
@@ -24,19 +24,22 @@
 
 #ifdef CONFIG_IRQ_REMAP
 
-struct IO_APIC_route_entry;
-struct io_apic_irq_attr;
 struct irq_data;
-struct cpumask;
-struct pci_dev;
 struct msi_msg;
+struct irq_domain;
+struct irq_alloc_info;
 
 extern int irq_remap_broken;
 extern int disable_sourceid_checking;
 extern int no_x2apic_optout;
 extern int irq_remapping_enabled;
 
+extern int disable_irq_post;
+
 struct irq_remap_ops {
+	/* The supported capabilities */
+	int capability;
+
 	/* Initializes hardware and makes it ready for remapping interrupts */
 	int  (*prepare)(void);
 
@@ -52,40 +55,23 @@ struct irq_remap_ops {
 	/* Enable fault handling */
 	int  (*enable_faulting)(void);
 
-	/* IO-APIC setup routine */
-	int (*setup_ioapic_entry)(int irq, struct IO_APIC_route_entry *,
-				  unsigned int, int,
-				  struct io_apic_irq_attr *);
-
-	/* Set the CPU affinity of a remapped interrupt */
-	int (*set_affinity)(struct irq_data *data, const struct cpumask *mask,
-			    bool force);
-
-	/* Free an IRQ */
-	int (*free_irq)(int);
+	/* Get the irqdomain associated the IOMMU device */
+	struct irq_domain *(*get_ir_irq_domain)(struct irq_alloc_info *);
 
-	/* Create MSI msg to use for interrupt remapping */
-	void (*compose_msi_msg)(struct pci_dev *,
-				unsigned int, unsigned int,
-				struct msi_msg *, u8);
-
-	/* Allocate remapping resources for MSI */
-	int (*msi_alloc_irq)(struct pci_dev *, int, int);
-
-	/* Setup the remapped MSI irq */
-	int (*msi_setup_irq)(struct pci_dev *, unsigned int, int, int);
-
-	/* Setup interrupt remapping for an HPET MSI */
-	int (*alloc_hpet_msi)(unsigned int, unsigned int);
+	/* Get the MSI irqdomain associated with the IOMMU device */
+	struct irq_domain *(*get_irq_domain)(struct irq_alloc_info *);
 };
 
 extern struct irq_remap_ops intel_irq_remap_ops;
 extern struct irq_remap_ops amd_iommu_irq_ops;
 
+extern void ir_ack_apic_edge(struct irq_data *data);
+
 #else  /* CONFIG_IRQ_REMAP */
 
 #define irq_remapping_enabled 0
 #define irq_remap_broken      0
+#define disable_irq_post      1
 
 #endif /* CONFIG_IRQ_REMAP */
 
diff --git a/drivers/iommu/rockchip-iommu.c b/drivers/iommu/rockchip-iommu.c
index cab214544237..ebf0adb8e7ea 100644
--- a/drivers/iommu/rockchip-iommu.c
+++ b/drivers/iommu/rockchip-iommu.c
@@ -551,6 +551,15 @@ static void rk_iommu_zap_iova(struct rk_iommu_domain *rk_domain,
 	spin_unlock_irqrestore(&rk_domain->iommus_lock, flags);
 }
 
+static void rk_iommu_zap_iova_first_last(struct rk_iommu_domain *rk_domain,
+					 dma_addr_t iova, size_t size)
+{
+	rk_iommu_zap_iova(rk_domain, iova, SPAGE_SIZE);
+	if (size > SPAGE_SIZE)
+		rk_iommu_zap_iova(rk_domain, iova + size - SPAGE_SIZE,
+					SPAGE_SIZE);
+}
+
 static u32 *rk_dte_get_page_table(struct rk_iommu_domain *rk_domain,
 				  dma_addr_t iova)
 {
@@ -575,12 +584,6 @@ static u32 *rk_dte_get_page_table(struct rk_iommu_domain *rk_domain,
 	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);
@@ -630,6 +633,14 @@ static int rk_iommu_map_iova(struct rk_iommu_domain *rk_domain, u32 *pte_addr,
 
 	rk_table_flush(pte_addr, pte_count);
 
+	/*
+	 * Zap the first and last iova to evict from iotlb any previously
+	 * mapped cachelines holding stale values for its dte and pte.
+	 * We only zap the first and last iova, since only they could have
+	 * dte or pte shared with an existing mapping.
+	 */
+	rk_iommu_zap_iova_first_last(rk_domain, iova, size);
+
 	return 0;
 unwind:
 	/* Unmap the range of iovas that we just mapped */
@@ -774,7 +785,7 @@ static int rk_iommu_attach_device(struct iommu_domain *domain,
 	list_add_tail(&iommu->node, &rk_domain->iommus);
 	spin_unlock_irqrestore(&rk_domain->iommus_lock, flags);
 
-	dev_info(dev, "Attached to iommu domain\n");
+	dev_dbg(dev, "Attached to iommu domain\n");
 
 	rk_iommu_disable_stall(iommu);
 
@@ -808,7 +819,7 @@ static void rk_iommu_detach_device(struct iommu_domain *domain,
 
 	iommu->domain = NULL;
 
-	dev_info(dev, "Detached from iommu domain\n");
+	dev_dbg(dev, "Detached from iommu domain\n");
 }
 
 static struct iommu_domain *rk_iommu_domain_alloc(unsigned type)
diff --git a/drivers/iommu/tegra-smmu.c b/drivers/iommu/tegra-smmu.c
index c845d99ecf6b..c1f2e521dc52 100644
--- a/drivers/iommu/tegra-smmu.c
+++ b/drivers/iommu/tegra-smmu.c
@@ -7,6 +7,7 @@
  */
 
 #include <linux/bitops.h>
+#include <linux/debugfs.h>
 #include <linux/err.h>
 #include <linux/iommu.h>
 #include <linux/kernel.h>
@@ -31,6 +32,8 @@ struct tegra_smmu {
 	struct mutex lock;
 
 	struct list_head list;
+
+	struct dentry *debugfs;
 };
 
 struct tegra_smmu_as {
@@ -673,6 +676,103 @@ static void tegra_smmu_ahb_enable(void)
 	}
 }
 
+static int tegra_smmu_swgroups_show(struct seq_file *s, void *data)
+{
+	struct tegra_smmu *smmu = s->private;
+	unsigned int i;
+	u32 value;
+
+	seq_printf(s, "swgroup    enabled  ASID\n");
+	seq_printf(s, "------------------------\n");
+
+	for (i = 0; i < smmu->soc->num_swgroups; i++) {
+		const struct tegra_smmu_swgroup *group = &smmu->soc->swgroups[i];
+		const char *status;
+		unsigned int asid;
+
+		value = smmu_readl(smmu, group->reg);
+
+		if (value & SMMU_ASID_ENABLE)
+			status = "yes";
+		else
+			status = "no";
+
+		asid = value & SMMU_ASID_MASK;
+
+		seq_printf(s, "%-9s  %-7s  %#04x\n", group->name, status,
+			   asid);
+	}
+
+	return 0;
+}
+
+static int tegra_smmu_swgroups_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, tegra_smmu_swgroups_show, inode->i_private);
+}
+
+static const struct file_operations tegra_smmu_swgroups_fops = {
+	.open = tegra_smmu_swgroups_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int tegra_smmu_clients_show(struct seq_file *s, void *data)
+{
+	struct tegra_smmu *smmu = s->private;
+	unsigned int i;
+	u32 value;
+
+	seq_printf(s, "client       enabled\n");
+	seq_printf(s, "--------------------\n");
+
+	for (i = 0; i < smmu->soc->num_clients; i++) {
+		const struct tegra_mc_client *client = &smmu->soc->clients[i];
+		const char *status;
+
+		value = smmu_readl(smmu, client->smmu.reg);
+
+		if (value & BIT(client->smmu.bit))
+			status = "yes";
+		else
+			status = "no";
+
+		seq_printf(s, "%-12s %s\n", client->name, status);
+	}
+
+	return 0;
+}
+
+static int tegra_smmu_clients_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, tegra_smmu_clients_show, inode->i_private);
+}
+
+static const struct file_operations tegra_smmu_clients_fops = {
+	.open = tegra_smmu_clients_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static void tegra_smmu_debugfs_init(struct tegra_smmu *smmu)
+{
+	smmu->debugfs = debugfs_create_dir("smmu", NULL);
+	if (!smmu->debugfs)
+		return;
+
+	debugfs_create_file("swgroups", S_IRUGO, smmu->debugfs, smmu,
+			    &tegra_smmu_swgroups_fops);
+	debugfs_create_file("clients", S_IRUGO, smmu->debugfs, smmu,
+			    &tegra_smmu_clients_fops);
+}
+
+static void tegra_smmu_debugfs_exit(struct tegra_smmu *smmu)
+{
+	debugfs_remove_recursive(smmu->debugfs);
+}
+
 struct tegra_smmu *tegra_smmu_probe(struct device *dev,
 				    const struct tegra_smmu_soc *soc,
 				    struct tegra_mc *mc)
@@ -743,5 +843,14 @@ struct tegra_smmu *tegra_smmu_probe(struct device *dev,
 	if (err < 0)
 		return ERR_PTR(err);
 
+	if (IS_ENABLED(CONFIG_DEBUG_FS))
+		tegra_smmu_debugfs_init(smmu);
+
 	return smmu;
 }
+
+void tegra_smmu_remove(struct tegra_smmu *smmu)
+{
+	if (IS_ENABLED(CONFIG_DEBUG_FS))
+		tegra_smmu_debugfs_exit(smmu);
+}
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index 6de62a96e79c..120d81543e53 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -30,6 +30,7 @@ config ARM_GIC_V3_ITS
 config ARM_NVIC
 	bool
 	select IRQ_DOMAIN
+	select IRQ_DOMAIN_HIERARCHY
 	select GENERIC_IRQ_CHIP
 
 config ARM_VIC
@@ -85,6 +86,11 @@ config IMGPDC_IRQ
 	select GENERIC_IRQ_CHIP
 	select IRQ_DOMAIN
 
+config IRQ_MIPS_CPU
+	bool
+	select GENERIC_IRQ_CHIP
+	select IRQ_DOMAIN
+
 config CLPS711X_IRQCHIP
 	bool
 	depends on ARCH_CLPS711X
@@ -158,3 +164,16 @@ config KEYSTONE_IRQ
 config MIPS_GIC
 	bool
 	select MIPS_CM
+
+config INGENIC_IRQ
+	bool
+	depends on MACH_INGENIC
+	default y
+
+config RENESAS_H8300H_INTC
+        bool
+	select IRQ_DOMAIN
+
+config RENESAS_H8S_INTC
+        bool
+	select IRQ_DOMAIN
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index dda4927e47a6..b8d4e9691890 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -28,6 +28,7 @@ obj-$(CONFIG_ARM_VIC)			+= irq-vic.o
 obj-$(CONFIG_ATMEL_AIC_IRQ)		+= irq-atmel-aic-common.o irq-atmel-aic.o
 obj-$(CONFIG_ATMEL_AIC5_IRQ)	+= irq-atmel-aic-common.o irq-atmel-aic5.o
 obj-$(CONFIG_IMGPDC_IRQ)		+= irq-imgpdc.o
+obj-$(CONFIG_IRQ_MIPS_CPU)		+= irq-mips-cpu.o
 obj-$(CONFIG_SIRF_IRQ)			+= irq-sirfsoc.o
 obj-$(CONFIG_RENESAS_INTC_IRQPIN)	+= irq-renesas-intc-irqpin.o
 obj-$(CONFIG_RENESAS_IRQC)		+= irq-renesas-irqc.o
@@ -47,3 +48,7 @@ obj-$(CONFIG_KEYSTONE_IRQ)		+= irq-keystone.o
 obj-$(CONFIG_MIPS_GIC)			+= irq-mips-gic.o
 obj-$(CONFIG_ARCH_MEDIATEK)		+= irq-mtk-sysirq.o
 obj-$(CONFIG_ARCH_DIGICOLOR)		+= irq-digicolor.o
+obj-$(CONFIG_RENESAS_H8300H_INTC)	+= irq-renesas-h8300h.o
+obj-$(CONFIG_RENESAS_H8S_INTC)		+= irq-renesas-h8s.o
+obj-$(CONFIG_ARCH_SA1100)		+= irq-sa11x0.o
+obj-$(CONFIG_INGENIC_IRQ)		+= irq-ingenic.o
diff --git a/drivers/irqchip/exynos-combiner.c b/drivers/irqchip/exynos-combiner.c
index 5945223b73fa..5c82e3bdafdf 100644
--- a/drivers/irqchip/exynos-combiner.c
+++ b/drivers/irqchip/exynos-combiner.c
@@ -13,6 +13,7 @@
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/slab.h>
+#include <linux/syscore_ops.h>
 #include <linux/irqdomain.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/interrupt.h>
@@ -34,9 +35,14 @@ struct combiner_chip_data {
 	unsigned int irq_mask;
 	void __iomem *base;
 	unsigned int parent_irq;
+#ifdef CONFIG_PM
+	u32 pm_save;
+#endif
 };
 
+static struct combiner_chip_data *combiner_data;
 static struct irq_domain *combiner_irq_domain;
+static unsigned int max_nr = 20;
 
 static inline void __iomem *combiner_base(struct irq_data *data)
 {
@@ -164,18 +170,16 @@ static int combiner_irq_domain_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops combiner_irq_domain_ops = {
+static const struct irq_domain_ops combiner_irq_domain_ops = {
 	.xlate	= combiner_irq_domain_xlate,
 	.map	= combiner_irq_domain_map,
 };
 
 static void __init combiner_init(void __iomem *combiner_base,
-				 struct device_node *np,
-				 unsigned int max_nr)
+				 struct device_node *np)
 {
 	int i, irq;
 	unsigned int nr_irq;
-	struct combiner_chip_data *combiner_data;
 
 	nr_irq = max_nr * IRQ_IN_COMBINER;
 
@@ -201,11 +205,59 @@ static void __init combiner_init(void __iomem *combiner_base,
 	}
 }
 
+#ifdef CONFIG_PM
+
+/**
+ * combiner_suspend - save interrupt combiner state before suspend
+ *
+ * Save the interrupt enable set register for all combiner groups since
+ * the state is lost when the system enters into a sleep state.
+ *
+ */
+static int combiner_suspend(void)
+{
+	int i;
+
+	for (i = 0; i < max_nr; i++)
+		combiner_data[i].pm_save =
+			__raw_readl(combiner_data[i].base + COMBINER_ENABLE_SET);
+
+	return 0;
+}
+
+/**
+ * combiner_resume - restore interrupt combiner state after resume
+ *
+ * Restore the interrupt enable set register for all combiner groups since
+ * the state is lost when the system enters into a sleep state on suspend.
+ *
+ */
+static void combiner_resume(void)
+{
+	int i;
+
+	for (i = 0; i < max_nr; i++) {
+		__raw_writel(combiner_data[i].irq_mask,
+			     combiner_data[i].base + COMBINER_ENABLE_CLEAR);
+		__raw_writel(combiner_data[i].pm_save,
+			     combiner_data[i].base + COMBINER_ENABLE_SET);
+	}
+}
+
+#else
+#define combiner_suspend	NULL
+#define combiner_resume		NULL
+#endif
+
+static struct syscore_ops combiner_syscore_ops = {
+	.suspend	= combiner_suspend,
+	.resume		= combiner_resume,
+};
+
 static int __init combiner_of_init(struct device_node *np,
 				   struct device_node *parent)
 {
 	void __iomem *combiner_base;
-	unsigned int max_nr = 20;
 
 	combiner_base = of_iomap(np, 0);
 	if (!combiner_base) {
@@ -219,7 +271,9 @@ static int __init combiner_of_init(struct device_node *np,
 			__func__, max_nr);
 	}
 
-	combiner_init(combiner_base, np, max_nr);
+	combiner_init(combiner_base, np);
+
+	register_syscore_ops(&combiner_syscore_ops);
 
 	return 0;
 }
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index daccc8bdbb42..0d3b0fe2f175 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -409,7 +409,7 @@ static struct notifier_block mpic_cascaded_cpu_notifier = {
 };
 #endif /* CONFIG_SMP */
 
-static struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
+static const struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
 	.map = armada_370_xp_mpic_irq_map,
 	.xlate = irq_domain_xlate_onecell,
 };
diff --git a/drivers/irqchip/irq-atmel-aic5.c b/drivers/irqchip/irq-atmel-aic5.c
index a2e8c3f876cb..459bf4429d36 100644
--- a/drivers/irqchip/irq-atmel-aic5.c
+++ b/drivers/irqchip/irq-atmel-aic5.c
@@ -339,6 +339,15 @@ static int __init aic5_of_init(struct device_node *node,
 	return 0;
 }
 
+#define NR_SAMA5D2_IRQS		77
+
+static int __init sama5d2_aic5_of_init(struct device_node *node,
+				       struct device_node *parent)
+{
+	return aic5_of_init(node, parent, NR_SAMA5D2_IRQS);
+}
+IRQCHIP_DECLARE(sama5d2_aic5, "atmel,sama5d2-aic", sama5d2_aic5_of_init);
+
 #define NR_SAMA5D3_IRQS		48
 
 static int __init sama5d3_aic5_of_init(struct device_node *node,
diff --git a/drivers/irqchip/irq-bcm2835.c b/drivers/irqchip/irq-bcm2835.c
index 5916d6cdafa1..e68c3b60a681 100644
--- a/drivers/irqchip/irq-bcm2835.c
+++ b/drivers/irqchip/irq-bcm2835.c
@@ -135,7 +135,7 @@ static int armctrl_xlate(struct irq_domain *d, struct device_node *ctrlr,
 	return 0;
 }
 
-static struct irq_domain_ops armctrl_ops = {
+static const struct irq_domain_ops armctrl_ops = {
 	.xlate = armctrl_xlate
 };
 
diff --git a/drivers/irqchip/irq-gic-common.c b/drivers/irqchip/irq-gic-common.c
index ad96ebb0c7ab..9448e391cb71 100644
--- a/drivers/irqchip/irq-gic-common.c
+++ b/drivers/irqchip/irq-gic-common.c
@@ -24,11 +24,8 @@
 int gic_configure_irq(unsigned int irq, unsigned int type,
 		       void __iomem *base, void (*sync_access)(void))
 {
-	u32 enablemask = 1 << (irq % 32);
-	u32 enableoff = (irq / 32) * 4;
 	u32 confmask = 0x2 << ((irq % 16) * 2);
 	u32 confoff = (irq / 16) * 4;
-	bool enabled = false;
 	u32 val, oldval;
 	int ret = 0;
 
@@ -43,17 +40,6 @@ int gic_configure_irq(unsigned int irq, unsigned int type,
 		val |= confmask;
 
 	/*
-	 * As recommended by the spec, disable the interrupt before changing
-	 * the configuration
-	 */
-	if (readl_relaxed(base + GIC_DIST_ENABLE_SET + enableoff) & enablemask) {
-		writel_relaxed(enablemask, base + GIC_DIST_ENABLE_CLEAR + enableoff);
-		if (sync_access)
-			sync_access();
-		enabled = true;
-	}
-
-	/*
 	 * Write back the new configuration, and possibly re-enable
 	 * the interrupt. If we tried to write a new configuration and failed,
 	 * return an error.
@@ -62,9 +48,6 @@ int gic_configure_irq(unsigned int irq, unsigned int type,
 	if (readl_relaxed(base + GIC_DIST_CONFIG + confoff) != val && val != oldval)
 		ret = -EINVAL;
 
-	if (enabled)
-		writel_relaxed(enablemask, base + GIC_DIST_ENABLE_SET + enableoff);
-
 	if (sync_access)
 		sync_access();
 
diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index 49875adb6b44..c52f7ba205b4 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -658,6 +658,7 @@ static struct irq_chip gic_chip = {
 	.irq_set_affinity	= gic_set_affinity,
 	.irq_get_irqchip_state	= gic_irq_get_irqchip_state,
 	.irq_set_irqchip_state	= gic_irq_set_irqchip_state,
+	.flags			= IRQCHIP_SET_TYPE_MASKED,
 };
 
 #define GIC_ID_NR		(1U << gic_data.rdists.id_bits)
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 01999d74bd3a..4dd88264dff5 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -324,6 +324,7 @@ static struct irq_chip gic_chip = {
 #endif
 	.irq_get_irqchip_state	= gic_irq_get_irqchip_state,
 	.irq_set_irqchip_state	= gic_irq_set_irqchip_state,
+	.flags			= IRQCHIP_SET_TYPE_MASKED,
 };
 
 void __init gic_cascade_irq(unsigned int gic_nr, unsigned int irq)
@@ -1054,7 +1055,7 @@ gic_acpi_parse_madt_cpu(struct acpi_subtable_header *header,
 
 	processor = (struct acpi_madt_generic_interrupt *)header;
 
-	if (BAD_MADT_ENTRY(processor, end))
+	if (BAD_MADT_GICC_ENTRY(processor, end))
 		return -EINVAL;
 
 	/*
diff --git a/drivers/irqchip/irq-hip04.c b/drivers/irqchip/irq-hip04.c
index 7d6ffb5de84f..0cae45d10695 100644
--- a/drivers/irqchip/irq-hip04.c
+++ b/drivers/irqchip/irq-hip04.c
@@ -202,6 +202,7 @@ static struct irq_chip hip04_irq_chip = {
 #ifdef CONFIG_SMP
 	.irq_set_affinity	= hip04_irq_set_affinity,
 #endif
+	.flags			= IRQCHIP_SET_TYPE_MASKED,
 };
 
 static u16 hip04_get_cpumask(struct hip04_irq_data *intc)
diff --git a/drivers/irqchip/irq-ingenic.c b/drivers/irqchip/irq-ingenic.c
new file mode 100644
index 000000000000..005de3f932ae
--- /dev/null
+++ b/drivers/irqchip/irq-ingenic.c
@@ -0,0 +1,177 @@
+/*
+ *  Copyright (C) 2009-2010, Lars-Peter Clausen <lars@metafoo.de>
+ *  JZ4740 platform IRQ support
+ *
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under  the terms of the GNU General	 Public License as published by the
+ *  Free Software Foundation;  either version 2 of the License, or (at your
+ *  option) any later version.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *  675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/irqchip/ingenic.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/timex.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+
+#include <asm/io.h>
+#include <asm/mach-jz4740/irq.h>
+
+#include "irqchip.h"
+
+struct ingenic_intc_data {
+	void __iomem *base;
+	unsigned num_chips;
+};
+
+#define JZ_REG_INTC_STATUS	0x00
+#define JZ_REG_INTC_MASK	0x04
+#define JZ_REG_INTC_SET_MASK	0x08
+#define JZ_REG_INTC_CLEAR_MASK	0x0c
+#define JZ_REG_INTC_PENDING	0x10
+#define CHIP_SIZE		0x20
+
+static irqreturn_t intc_cascade(int irq, void *data)
+{
+	struct ingenic_intc_data *intc = irq_get_handler_data(irq);
+	uint32_t irq_reg;
+	unsigned i;
+
+	for (i = 0; i < intc->num_chips; i++) {
+		irq_reg = readl(intc->base + (i * CHIP_SIZE) +
+				JZ_REG_INTC_PENDING);
+		if (!irq_reg)
+			continue;
+
+		generic_handle_irq(__fls(irq_reg) + (i * 32) + JZ4740_IRQ_BASE);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void intc_irq_set_mask(struct irq_chip_generic *gc, uint32_t mask)
+{
+	struct irq_chip_regs *regs = &gc->chip_types->regs;
+
+	writel(mask, gc->reg_base + regs->enable);
+	writel(~mask, gc->reg_base + regs->disable);
+}
+
+void ingenic_intc_irq_suspend(struct irq_data *data)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(data);
+	intc_irq_set_mask(gc, gc->wake_active);
+}
+
+void ingenic_intc_irq_resume(struct irq_data *data)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(data);
+	intc_irq_set_mask(gc, gc->mask_cache);
+}
+
+static struct irqaction intc_cascade_action = {
+	.handler = intc_cascade,
+	.name = "SoC intc cascade interrupt",
+};
+
+static int __init ingenic_intc_of_init(struct device_node *node,
+				       unsigned num_chips)
+{
+	struct ingenic_intc_data *intc;
+	struct irq_chip_generic *gc;
+	struct irq_chip_type *ct;
+	struct irq_domain *domain;
+	int parent_irq, err = 0;
+	unsigned i;
+
+	intc = kzalloc(sizeof(*intc), GFP_KERNEL);
+	if (!intc) {
+		err = -ENOMEM;
+		goto out_err;
+	}
+
+	parent_irq = irq_of_parse_and_map(node, 0);
+	if (!parent_irq) {
+		err = -EINVAL;
+		goto out_free;
+	}
+
+	err = irq_set_handler_data(parent_irq, intc);
+	if (err)
+		goto out_unmap_irq;
+
+	intc->num_chips = num_chips;
+	intc->base = of_iomap(node, 0);
+	if (!intc->base) {
+		err = -ENODEV;
+		goto out_unmap_irq;
+	}
+
+	for (i = 0; i < num_chips; i++) {
+		/* Mask all irqs */
+		writel(0xffffffff, intc->base + (i * CHIP_SIZE) +
+		       JZ_REG_INTC_SET_MASK);
+
+		gc = irq_alloc_generic_chip("INTC", 1,
+					    JZ4740_IRQ_BASE + (i * 32),
+					    intc->base + (i * CHIP_SIZE),
+					    handle_level_irq);
+
+		gc->wake_enabled = IRQ_MSK(32);
+
+		ct = gc->chip_types;
+		ct->regs.enable = JZ_REG_INTC_CLEAR_MASK;
+		ct->regs.disable = JZ_REG_INTC_SET_MASK;
+		ct->chip.irq_unmask = irq_gc_unmask_enable_reg;
+		ct->chip.irq_mask = irq_gc_mask_disable_reg;
+		ct->chip.irq_mask_ack = irq_gc_mask_disable_reg;
+		ct->chip.irq_set_wake = irq_gc_set_wake;
+		ct->chip.irq_suspend = ingenic_intc_irq_suspend;
+		ct->chip.irq_resume = ingenic_intc_irq_resume;
+
+		irq_setup_generic_chip(gc, IRQ_MSK(32), 0, 0,
+				       IRQ_NOPROBE | IRQ_LEVEL);
+	}
+
+	domain = irq_domain_add_legacy(node, num_chips * 32, JZ4740_IRQ_BASE, 0,
+				       &irq_domain_simple_ops, NULL);
+	if (!domain)
+		pr_warn("unable to register IRQ domain\n");
+
+	setup_irq(parent_irq, &intc_cascade_action);
+	return 0;
+
+out_unmap_irq:
+	irq_dispose_mapping(parent_irq);
+out_free:
+	kfree(intc);
+out_err:
+	return err;
+}
+
+static int __init intc_1chip_of_init(struct device_node *node,
+				     struct device_node *parent)
+{
+	return ingenic_intc_of_init(node, 1);
+}
+IRQCHIP_DECLARE(jz4740_intc, "ingenic,jz4740-intc", intc_1chip_of_init);
+
+static int __init intc_2chip_of_init(struct device_node *node,
+	struct device_node *parent)
+{
+	return ingenic_intc_of_init(node, 2);
+}
+IRQCHIP_DECLARE(jz4770_intc, "ingenic,jz4770-intc", intc_2chip_of_init);
+IRQCHIP_DECLARE(jz4775_intc, "ingenic,jz4775-intc", intc_2chip_of_init);
+IRQCHIP_DECLARE(jz4780_intc, "ingenic,jz4780-intc", intc_2chip_of_init);
diff --git a/drivers/irqchip/irq-keystone.c b/drivers/irqchip/irq-keystone.c
index 78e8b3ce5252..81e3cf5b9a1f 100644
--- a/drivers/irqchip/irq-keystone.c
+++ b/drivers/irqchip/irq-keystone.c
@@ -131,7 +131,7 @@ static int keystone_irq_map(struct irq_domain *h, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops keystone_irq_ops = {
+static const struct irq_domain_ops keystone_irq_ops = {
 	.map	= keystone_irq_map,
 	.xlate	= irq_domain_xlate_onecell,
 };
@@ -184,8 +184,7 @@ static int keystone_irq_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, kirq);
 
-	irq_set_chained_handler(kirq->irq, keystone_irq_handler);
-	irq_set_handler_data(kirq->irq, kirq);
+	irq_set_chained_handler_and_data(kirq->irq, keystone_irq_handler, kirq);
 
 	/* clear all source bits */
 	keystone_irq_writel(kirq, ~0x0);
diff --git a/drivers/irqchip/irq-mips-cpu.c b/drivers/irqchip/irq-mips-cpu.c
new file mode 100644
index 000000000000..a43c41988009
--- /dev/null
+++ b/drivers/irqchip/irq-mips-cpu.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright 2001 MontaVista Software Inc.
+ * Author: Jun Sun, jsun@mvista.com or jsun@junsun.net
+ *
+ * Copyright (C) 2001 Ralf Baechle
+ * Copyright (C) 2005  MIPS Technologies, Inc.	All rights reserved.
+ *	Author: Maciej W. Rozycki <macro@mips.com>
+ *
+ * This file define the irq handler for MIPS CPU interrupts.
+ *
+ * This program is free software; you can redistribute	it and/or modify it
+ * under  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.
+ */
+
+/*
+ * Almost all MIPS CPUs define 8 interrupt sources.  They are typically
+ * level triggered (i.e., cannot be cleared from CPU; must be cleared from
+ * device).  The first two are software interrupts which we don't really
+ * use or support.  The last one is usually the CPU timer interrupt if
+ * counter register is present or, for CPUs with an external FPU, by
+ * convention it's the FPU exception interrupt.
+ *
+ * Don't even think about using this on SMP.  You have been warned.
+ *
+ * This file exports one global function:
+ *	void mips_cpu_irq_init(void);
+ */
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+
+#include <asm/irq_cpu.h>
+#include <asm/mipsregs.h>
+#include <asm/mipsmtregs.h>
+#include <asm/setup.h>
+
+#include "irqchip.h"
+
+static inline void unmask_mips_irq(struct irq_data *d)
+{
+	set_c0_status(0x100 << (d->irq - MIPS_CPU_IRQ_BASE));
+	irq_enable_hazard();
+}
+
+static inline void mask_mips_irq(struct irq_data *d)
+{
+	clear_c0_status(0x100 << (d->irq - MIPS_CPU_IRQ_BASE));
+	irq_disable_hazard();
+}
+
+static struct irq_chip mips_cpu_irq_controller = {
+	.name		= "MIPS",
+	.irq_ack	= mask_mips_irq,
+	.irq_mask	= mask_mips_irq,
+	.irq_mask_ack	= mask_mips_irq,
+	.irq_unmask	= unmask_mips_irq,
+	.irq_eoi	= unmask_mips_irq,
+	.irq_disable	= mask_mips_irq,
+	.irq_enable	= unmask_mips_irq,
+};
+
+/*
+ * Basically the same as above but taking care of all the MT stuff
+ */
+
+static unsigned int mips_mt_cpu_irq_startup(struct irq_data *d)
+{
+	unsigned int vpflags = dvpe();
+
+	clear_c0_cause(0x100 << (d->irq - MIPS_CPU_IRQ_BASE));
+	evpe(vpflags);
+	unmask_mips_irq(d);
+	return 0;
+}
+
+/*
+ * While we ack the interrupt interrupts are disabled and thus we don't need
+ * to deal with concurrency issues.  Same for mips_cpu_irq_end.
+ */
+static void mips_mt_cpu_irq_ack(struct irq_data *d)
+{
+	unsigned int vpflags = dvpe();
+	clear_c0_cause(0x100 << (d->irq - MIPS_CPU_IRQ_BASE));
+	evpe(vpflags);
+	mask_mips_irq(d);
+}
+
+static struct irq_chip mips_mt_cpu_irq_controller = {
+	.name		= "MIPS",
+	.irq_startup	= mips_mt_cpu_irq_startup,
+	.irq_ack	= mips_mt_cpu_irq_ack,
+	.irq_mask	= mask_mips_irq,
+	.irq_mask_ack	= mips_mt_cpu_irq_ack,
+	.irq_unmask	= unmask_mips_irq,
+	.irq_eoi	= unmask_mips_irq,
+	.irq_disable	= mask_mips_irq,
+	.irq_enable	= unmask_mips_irq,
+};
+
+asmlinkage void __weak plat_irq_dispatch(void)
+{
+	unsigned long pending = read_c0_cause() & read_c0_status() & ST0_IM;
+	int irq;
+
+	if (!pending) {
+		spurious_interrupt();
+		return;
+	}
+
+	pending >>= CAUSEB_IP;
+	while (pending) {
+		irq = fls(pending) - 1;
+		do_IRQ(MIPS_CPU_IRQ_BASE + irq);
+		pending &= ~BIT(irq);
+	}
+}
+
+static int mips_cpu_intc_map(struct irq_domain *d, unsigned int irq,
+			     irq_hw_number_t hw)
+{
+	static struct irq_chip *chip;
+
+	if (hw < 2 && cpu_has_mipsmt) {
+		/* Software interrupts are used for MT/CMT IPI */
+		chip = &mips_mt_cpu_irq_controller;
+	} else {
+		chip = &mips_cpu_irq_controller;
+	}
+
+	if (cpu_has_vint)
+		set_vi_handler(hw, plat_irq_dispatch);
+
+	irq_set_chip_and_handler(irq, chip, handle_percpu_irq);
+
+	return 0;
+}
+
+static const struct irq_domain_ops mips_cpu_intc_irq_domain_ops = {
+	.map = mips_cpu_intc_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+static void __init __mips_cpu_irq_init(struct device_node *of_node)
+{
+	struct irq_domain *domain;
+
+	/* Mask interrupts. */
+	clear_c0_status(ST0_IM);
+	clear_c0_cause(CAUSEF_IP);
+
+	domain = irq_domain_add_legacy(of_node, 8, MIPS_CPU_IRQ_BASE, 0,
+				       &mips_cpu_intc_irq_domain_ops, NULL);
+	if (!domain)
+		panic("Failed to add irqdomain for MIPS CPU");
+}
+
+void __init mips_cpu_irq_init(void)
+{
+	__mips_cpu_irq_init(NULL);
+}
+
+int __init mips_cpu_irq_of_init(struct device_node *of_node,
+				struct device_node *parent)
+{
+	__mips_cpu_irq_init(of_node);
+	return 0;
+}
+IRQCHIP_DECLARE(cpu_intc, "mti,cpu-interrupt-controller", mips_cpu_irq_of_init);
diff --git a/drivers/irqchip/irq-mips-gic.c b/drivers/irqchip/irq-mips-gic.c
index 57f09cb54464..b7d54d428b5e 100644
--- a/drivers/irqchip/irq-mips-gic.c
+++ b/drivers/irqchip/irq-mips-gic.c
@@ -257,21 +257,11 @@ int gic_get_c0_fdc_int(void)
 		return MIPS_CPU_IRQ_BASE + cp0_fdc_irq;
 	}
 
-	/*
-	 * Some cores claim the FDC is routable but it doesn't actually seem to
-	 * be connected.
-	 */
-	switch (current_cpu_type()) {
-	case CPU_INTERAPTIV:
-	case CPU_PROAPTIV:
-		return -1;
-	}
-
 	return irq_create_mapping(gic_irq_domain,
 				  GIC_LOCAL_TO_HWIRQ(GIC_LOCAL_INT_FDC));
 }
 
-static void gic_handle_shared_int(void)
+static void gic_handle_shared_int(bool chained)
 {
 	unsigned int i, intr, virq;
 	unsigned long *pcpu_mask;
@@ -299,7 +289,10 @@ static void gic_handle_shared_int(void)
 	while (intr != gic_shared_intrs) {
 		virq = irq_linear_revmap(gic_irq_domain,
 					 GIC_SHARED_TO_HWIRQ(intr));
-		do_IRQ(virq);
+		if (chained)
+			generic_handle_irq(virq);
+		else
+			do_IRQ(virq);
 
 		/* go to next pending bit */
 		bitmap_clear(pending, intr, 1);
@@ -431,7 +424,7 @@ static struct irq_chip gic_edge_irq_controller = {
 #endif
 };
 
-static void gic_handle_local_int(void)
+static void gic_handle_local_int(bool chained)
 {
 	unsigned long pending, masked;
 	unsigned int intr, virq;
@@ -445,7 +438,10 @@ static void gic_handle_local_int(void)
 	while (intr != GIC_NUM_LOCAL_INTRS) {
 		virq = irq_linear_revmap(gic_irq_domain,
 					 GIC_LOCAL_TO_HWIRQ(intr));
-		do_IRQ(virq);
+		if (chained)
+			generic_handle_irq(virq);
+		else
+			do_IRQ(virq);
 
 		/* go to next pending bit */
 		bitmap_clear(&pending, intr, 1);
@@ -509,13 +505,14 @@ static struct irq_chip gic_all_vpes_local_irq_controller = {
 
 static void __gic_irq_dispatch(void)
 {
-	gic_handle_local_int();
-	gic_handle_shared_int();
+	gic_handle_local_int(false);
+	gic_handle_shared_int(false);
 }
 
 static void gic_irq_dispatch(unsigned int irq, struct irq_desc *desc)
 {
-	__gic_irq_dispatch();
+	gic_handle_local_int(true);
+	gic_handle_shared_int(true);
 }
 
 #ifdef CONFIG_MIPS_GIC_IPI
@@ -739,7 +736,7 @@ static int gic_irq_domain_xlate(struct irq_domain *d, struct device_node *ctrlr,
 	return 0;
 }
 
-static struct irq_domain_ops gic_irq_domain_ops = {
+static const struct irq_domain_ops gic_irq_domain_ops = {
 	.map = gic_irq_domain_map,
 	.xlate = gic_irq_domain_xlate,
 };
diff --git a/drivers/irqchip/irq-mtk-sysirq.c b/drivers/irqchip/irq-mtk-sysirq.c
index eaf0a710e98a..15c13039bba2 100644
--- a/drivers/irqchip/irq-mtk-sysirq.c
+++ b/drivers/irqchip/irq-mtk-sysirq.c
@@ -111,7 +111,7 @@ static int mtk_sysirq_domain_alloc(struct irq_domain *domain, unsigned int virq,
 	return irq_domain_alloc_irqs_parent(domain, virq, nr_irqs, &gic_data);
 }
 
-static struct irq_domain_ops sysirq_domain_ops = {
+static const struct irq_domain_ops sysirq_domain_ops = {
 	.xlate = mtk_sysirq_domain_xlate,
 	.alloc = mtk_sysirq_domain_alloc,
 	.free = irq_domain_free_irqs_common,
@@ -144,7 +144,7 @@ static int __init mtk_sysirq_of_init(struct device_node *node,
 	chip_data->intpol_base = ioremap(res.start, size);
 	if (!chip_data->intpol_base) {
 		pr_err("mtk_sysirq: unable to map sysirq register\n");
-		ret = PTR_ERR(chip_data->intpol_base);
+		ret = -ENXIO;
 		goto out_free;
 	}
 
diff --git a/drivers/irqchip/irq-mxs.c b/drivers/irqchip/irq-mxs.c
index e4acf1e3f8e3..04bf97b289cf 100644
--- a/drivers/irqchip/irq-mxs.c
+++ b/drivers/irqchip/irq-mxs.c
@@ -90,7 +90,7 @@ static int icoll_irq_domain_map(struct irq_domain *d, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops icoll_irq_domain_ops = {
+static const struct irq_domain_ops icoll_irq_domain_ops = {
 	.map = icoll_irq_domain_map,
 	.xlate = irq_domain_xlate_onecell,
 };
diff --git a/drivers/irqchip/irq-nvic.c b/drivers/irqchip/irq-nvic.c
index 4ff0805fca01..5fac9100f6cb 100644
--- a/drivers/irqchip/irq-nvic.c
+++ b/drivers/irqchip/irq-nvic.c
@@ -49,6 +49,31 @@ nvic_handle_irq(irq_hw_number_t hwirq, struct pt_regs *regs)
 	handle_IRQ(irq, regs);
 }
 
+static int nvic_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 = irq_domain_xlate_onecell(domain, irq_data->np, irq_data->args,
+				   irq_data->args_count, &hwirq, &type);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < nr_irqs; i++)
+		irq_map_generic_chip(domain, virq + i, hwirq + i);
+
+	return 0;
+}
+
+static const struct irq_domain_ops nvic_irq_domain_ops = {
+	.xlate = irq_domain_xlate_onecell,
+	.alloc = nvic_irq_domain_alloc,
+	.free = irq_domain_free_irqs_top,
+};
+
 static int __init nvic_of_init(struct device_node *node,
 			       struct device_node *parent)
 {
@@ -70,7 +95,8 @@ static int __init nvic_of_init(struct device_node *node,
 		irqs = NVIC_MAX_IRQ;
 
 	nvic_irq_domain =
-		irq_domain_add_linear(node, irqs, &irq_generic_chip_ops, NULL);
+		irq_domain_add_linear(node, irqs, &nvic_irq_domain_ops, NULL);
+
 	if (!nvic_irq_domain) {
 		pr_warn("Failed to allocate irq domain\n");
 		return -ENOMEM;
diff --git a/drivers/irqchip/irq-renesas-h8300h.c b/drivers/irqchip/irq-renesas-h8300h.c
new file mode 100644
index 000000000000..1870e6bd3dd9
--- /dev/null
+++ b/drivers/irqchip/irq-renesas-h8300h.c
@@ -0,0 +1,95 @@
+/*
+ * H8/300H interrupt controller driver
+ *
+ * Copyright 2015 Yoshinori Sato <ysato@users.sourceforge.jp>
+ */
+
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/irqchip.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <asm/io.h>
+
+#include "irqchip.h"
+
+static const char ipr_bit[] = {
+	 7,  6,  5,  5,
+	 4,  4,  4,  4,  3,  3,  3,  3,
+	 2,  2,  2,  2,  1,  1,  1,  1,
+	 0,  0,  0,  0, 15, 15, 15, 15,
+	14, 14, 14, 14, 13, 13, 13, 13,
+	-1, -1, -1, -1, 11, 11, 11, 11,
+	10, 10, 10, 10,  9,  9,  9,  9,
+};
+
+static void *intc_baseaddr;
+
+#define IPR ((unsigned long)intc_baseaddr + 6)
+
+static void h8300h_disable_irq(struct irq_data *data)
+{
+	int bit;
+	int irq = data->irq - 12;
+
+	bit = ipr_bit[irq];
+	if (bit >= 0) {
+		if (bit < 8)
+			ctrl_bclr(bit & 7, IPR);
+		else
+			ctrl_bclr(bit & 7, (IPR+1));
+	}
+}
+
+static void h8300h_enable_irq(struct irq_data *data)
+{
+	int bit;
+	int irq = data->irq - 12;
+
+	bit = ipr_bit[irq];
+	if (bit >= 0) {
+		if (bit < 8)
+			ctrl_bset(bit & 7, IPR);
+		else
+			ctrl_bset(bit & 7, (IPR+1));
+	}
+}
+
+struct irq_chip h8300h_irq_chip = {
+	.name		= "H8/300H-INTC",
+	.irq_enable	= h8300h_enable_irq,
+	.irq_disable	= h8300h_disable_irq,
+};
+
+static int irq_map(struct irq_domain *h, unsigned int virq,
+		   irq_hw_number_t hw_irq_num)
+{
+       irq_set_chip_and_handler(virq, &h8300h_irq_chip, handle_simple_irq);
+
+       return 0;
+}
+
+static struct irq_domain_ops irq_ops = {
+       .map    = irq_map,
+       .xlate  = irq_domain_xlate_onecell,
+};
+
+static int __init h8300h_intc_of_init(struct device_node *intc,
+				      struct device_node *parent)
+{
+	struct irq_domain *domain;
+
+	intc_baseaddr = of_iomap(intc, 0);
+	BUG_ON(!intc_baseaddr);
+
+	/* All interrupt priority low */
+	ctrl_outb(0x00, IPR + 0);
+	ctrl_outb(0x00, IPR + 1);
+
+	domain = irq_domain_add_linear(intc, NR_IRQS, &irq_ops, NULL);
+	BUG_ON(!domain);
+	irq_set_default_host(domain);
+	return 0;
+}
+
+IRQCHIP_DECLARE(h8300h_intc, "renesas,h8300h-intc", h8300h_intc_of_init);
diff --git a/drivers/irqchip/irq-renesas-h8s.c b/drivers/irqchip/irq-renesas-h8s.c
new file mode 100644
index 000000000000..64425f4de7d9
--- /dev/null
+++ b/drivers/irqchip/irq-renesas-h8s.c
@@ -0,0 +1,101 @@
+/*
+ * H8S interrupt contoller driver
+ *
+ * Copyright 2015 Yoshinori Sato <ysato@users.sourceforge.jp>
+ */
+
+#include <linux/irq.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <asm/io.h>
+#include "irqchip.h"
+
+static void *intc_baseaddr;
+#define IPRA ((unsigned long)intc_baseaddr)
+
+static const unsigned char ipr_table[] = {
+	0x03, 0x02, 0x01, 0x00, 0x13, 0x12, 0x11, 0x10, /* 16 - 23 */
+	0x23, 0x22, 0x21, 0x20, 0x33, 0x32, 0x31, 0x30, /* 24 - 31 */
+	0x43, 0x42, 0x41, 0x40, 0x53, 0x53, 0x52, 0x52, /* 32 - 39 */
+	0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, /* 40 - 47 */
+	0x50, 0x50, 0x50, 0x50, 0x63, 0x63, 0x63, 0x63, /* 48 - 55 */
+	0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, /* 56 - 63 */
+	0x61, 0x61, 0x61, 0x61, 0x60, 0x60, 0x60, 0x60, /* 64 - 71 */
+	0x73, 0x73, 0x73, 0x73, 0x72, 0x72, 0x72, 0x72, /* 72 - 79 */
+	0x71, 0x71, 0x71, 0x71, 0x70, 0x83, 0x82, 0x81, /* 80 - 87 */
+	0x80, 0x80, 0x80, 0x80, 0x93, 0x93, 0x93, 0x93, /* 88 - 95 */
+	0x92, 0x92, 0x92, 0x92, 0x91, 0x91, 0x91, 0x91, /* 96 - 103 */
+	0x90, 0x90, 0x90, 0x90, 0xa3, 0xa3, 0xa3, 0xa3, /* 104 - 111 */
+	0xa2, 0xa2, 0xa2, 0xa2, 0xa1, 0xa1, 0xa1, 0xa1, /* 112 - 119 */
+	0xa0, 0xa0, 0xa0, 0xa0, 0xa0, 0xa0, 0xa0, 0xa0, /* 120 - 127 */
+};
+
+static void h8s_disable_irq(struct irq_data *data)
+{
+	int pos;
+	unsigned int addr;
+	unsigned short pri;
+	int irq = data->irq;
+
+	addr = IPRA + ((ipr_table[irq - 16] & 0xf0) >> 3);
+	pos = (ipr_table[irq - 16] & 0x0f) * 4;
+	pri = ~(0x000f << pos);
+	pri &= ctrl_inw(addr);
+	ctrl_outw(pri, addr);
+}
+
+static void h8s_enable_irq(struct irq_data *data)
+{
+	int pos;
+	unsigned int addr;
+	unsigned short pri;
+	int irq = data->irq;
+
+	addr = IPRA + ((ipr_table[irq - 16] & 0xf0) >> 3);
+	pos = (ipr_table[irq - 16] & 0x0f) * 4;
+	pri = ~(0x000f << pos);
+	pri &= ctrl_inw(addr);
+	pri |= 1 << pos;
+	ctrl_outw(pri, addr);
+}
+
+struct irq_chip h8s_irq_chip = {
+	.name		= "H8S-INTC",
+	.irq_enable	= h8s_enable_irq,
+	.irq_disable	= h8s_disable_irq,
+};
+
+static __init int irq_map(struct irq_domain *h, unsigned int virq,
+			  irq_hw_number_t hw_irq_num)
+{
+       irq_set_chip_and_handler(virq, &h8s_irq_chip, handle_simple_irq);
+
+       return 0;
+}
+
+static struct irq_domain_ops irq_ops = {
+       .map    = irq_map,
+       .xlate  = irq_domain_xlate_onecell,
+};
+
+static int __init h8s_intc_of_init(struct device_node *intc,
+				   struct device_node *parent)
+{
+	struct irq_domain *domain;
+	int n;
+
+	intc_baseaddr = of_iomap(intc, 0);
+	BUG_ON(!intc_baseaddr);
+
+	/* All interrupt priority is 0 (disable) */
+	/* IPRA to IPRK */
+	for (n = 0; n <= 'k' - 'a'; n++)
+		ctrl_outw(0x0000, IPRA + (n * 2));
+
+	domain = irq_domain_add_linear(intc, NR_IRQS, &irq_ops, NULL);
+	BUG_ON(!domain);
+	irq_set_default_host(domain);
+	return 0;
+}
+
+IRQCHIP_DECLARE(h8s_intc, "renesas,h8s-intc", h8s_intc_of_init);
diff --git a/drivers/irqchip/irq-renesas-intc-irqpin.c b/drivers/irqchip/irq-renesas-intc-irqpin.c
index 9a0767b9c89d..0670ab4e3897 100644
--- a/drivers/irqchip/irq-renesas-intc-irqpin.c
+++ b/drivers/irqchip/irq-renesas-intc-irqpin.c
@@ -347,7 +347,7 @@ static int intc_irqpin_irq_domain_map(struct irq_domain *h, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops intc_irqpin_irq_domain_ops = {
+static const struct irq_domain_ops intc_irqpin_irq_domain_ops = {
 	.map	= intc_irqpin_irq_domain_map,
 	.xlate  = irq_domain_xlate_twocell,
 };
diff --git a/drivers/irqchip/irq-renesas-irqc.c b/drivers/irqchip/irq-renesas-irqc.c
index cdf80b7794cd..778bd076aeea 100644
--- a/drivers/irqchip/irq-renesas-irqc.c
+++ b/drivers/irqchip/irq-renesas-irqc.c
@@ -29,7 +29,6 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/module.h>
-#include <linux/platform_data/irq-renesas-irqc.h>
 #include <linux/pm_runtime.h>
 
 #define IRQC_IRQ_MAX	32	/* maximum 32 interrupts per driver instance */
@@ -62,7 +61,6 @@ struct irqc_priv {
 	void __iomem *iomem;
 	void __iomem *cpu_int_base;
 	struct irqc_irq irq[IRQC_IRQ_MAX];
-	struct renesas_irqc_config config;
 	unsigned int number_of_irqs;
 	struct platform_device *pdev;
 	struct irq_chip irq_chip;
@@ -168,14 +166,13 @@ static int irqc_irq_domain_map(struct irq_domain *h, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops irqc_irq_domain_ops = {
+static const struct irq_domain_ops irqc_irq_domain_ops = {
 	.map	= irqc_irq_domain_map,
 	.xlate  = irq_domain_xlate_twocell,
 };
 
 static int irqc_probe(struct platform_device *pdev)
 {
-	struct renesas_irqc_config *pdata = pdev->dev.platform_data;
 	struct irqc_priv *p;
 	struct resource *io;
 	struct resource *irq;
@@ -191,10 +188,6 @@ static int irqc_probe(struct platform_device *pdev)
 		goto err0;
 	}
 
-	/* deal with driver instance configuration */
-	if (pdata)
-		memcpy(&p->config, pdata, sizeof(*pdata));
-
 	p->pdev = pdev;
 	platform_set_drvdata(pdev, p);
 
@@ -251,8 +244,7 @@ static int irqc_probe(struct platform_device *pdev)
 	irq_chip->flags	= IRQCHIP_MASK_ON_SUSPEND;
 
 	p->irq_domain = irq_domain_add_simple(pdev->dev.of_node,
-					      p->number_of_irqs,
-					      p->config.irq_base,
+					      p->number_of_irqs, 0,
 					      &irqc_irq_domain_ops, p);
 	if (!p->irq_domain) {
 		ret = -ENXIO;
@@ -272,13 +264,6 @@ static int irqc_probe(struct platform_device *pdev)
 
 	dev_info(&pdev->dev, "driving %d irqs\n", p->number_of_irqs);
 
-	/* warn in case of mismatch if irq base is specified */
-	if (p->config.irq_base) {
-		if (p->config.irq_base != p->irq[0].domain_irq)
-			dev_warn(&pdev->dev, "irq base mismatch (%d/%d)\n",
-				 p->config.irq_base, p->irq[0].domain_irq);
-	}
-
 	return 0;
 err3:
 	while (--k >= 0)
diff --git a/drivers/irqchip/irq-s3c24xx.c b/drivers/irqchip/irq-s3c24xx.c
index c8d373fcd823..e96717f45ea1 100644
--- a/drivers/irqchip/irq-s3c24xx.c
+++ b/drivers/irqchip/irq-s3c24xx.c
@@ -502,7 +502,7 @@ err:
 	return -EINVAL;
 }
 
-static struct irq_domain_ops s3c24xx_irq_ops = {
+static const struct irq_domain_ops s3c24xx_irq_ops = {
 	.map = s3c24xx_irq_map,
 	.xlate = irq_domain_xlate_twocell,
 };
@@ -1228,7 +1228,7 @@ static int s3c24xx_irq_xlate_of(struct irq_domain *d, struct device_node *n,
 	return 0;
 }
 
-static struct irq_domain_ops s3c24xx_irq_ops_of = {
+static const struct irq_domain_ops s3c24xx_irq_ops_of = {
 	.map = s3c24xx_irq_map_of,
 	.xlate = s3c24xx_irq_xlate_of,
 };
diff --git a/drivers/irqchip/irq-sa11x0.c b/drivers/irqchip/irq-sa11x0.c
new file mode 100644
index 000000000000..46df2875dc1c
--- /dev/null
+++ b/drivers/irqchip/irq-sa11x0.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright (C) 2015 Dmitry Eremin-Solenikov
+ * Copyright (C) 1999-2001 Nicolas Pitre
+ *
+ * Generic IRQ handling for the SA11x0.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/syscore_ops.h>
+#include <linux/irqchip/irq-sa11x0.h>
+
+#include <soc/sa1100/pwer.h>
+
+#include <asm/exception.h>
+
+#define ICIP	0x00  /* IC IRQ Pending reg. */
+#define ICMR	0x04  /* IC Mask Reg.        */
+#define ICLR	0x08  /* IC Level Reg.       */
+#define ICCR	0x0C  /* IC Control Reg.     */
+#define ICFP	0x10  /* IC FIQ Pending reg. */
+#define ICPR	0x20  /* IC Pending Reg.     */
+
+static void __iomem *iobase;
+
+/*
+ * We don't need to ACK IRQs on the SA1100 unless they're GPIOs
+ * this is for internal IRQs i.e. from IRQ LCD to RTCAlrm.
+ */
+static void sa1100_mask_irq(struct irq_data *d)
+{
+	u32 reg;
+
+	reg = readl_relaxed(iobase + ICMR);
+	reg &= ~BIT(d->hwirq);
+	writel_relaxed(reg, iobase + ICMR);
+}
+
+static void sa1100_unmask_irq(struct irq_data *d)
+{
+	u32 reg;
+
+	reg = readl_relaxed(iobase + ICMR);
+	reg |= BIT(d->hwirq);
+	writel_relaxed(reg, iobase + ICMR);
+}
+
+static int sa1100_set_wake(struct irq_data *d, unsigned int on)
+{
+	return sa11x0_sc_set_wake(d->hwirq, on);
+}
+
+static struct irq_chip sa1100_normal_chip = {
+	.name		= "SC",
+	.irq_ack	= sa1100_mask_irq,
+	.irq_mask	= sa1100_mask_irq,
+	.irq_unmask	= sa1100_unmask_irq,
+	.irq_set_wake	= sa1100_set_wake,
+};
+
+static int sa1100_normal_irqdomain_map(struct irq_domain *d,
+		unsigned int irq, irq_hw_number_t hwirq)
+{
+	irq_set_chip_and_handler(irq, &sa1100_normal_chip,
+				 handle_level_irq);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+static const struct irq_domain_ops sa1100_normal_irqdomain_ops = {
+	.map = sa1100_normal_irqdomain_map,
+	.xlate = irq_domain_xlate_onetwocell,
+};
+
+static struct irq_domain *sa1100_normal_irqdomain;
+
+static struct sa1100irq_state {
+	unsigned int	saved;
+	unsigned int	icmr;
+	unsigned int	iclr;
+	unsigned int	iccr;
+} sa1100irq_state;
+
+static int sa1100irq_suspend(void)
+{
+	struct sa1100irq_state *st = &sa1100irq_state;
+
+	st->saved = 1;
+	st->icmr = readl_relaxed(iobase + ICMR);
+	st->iclr = readl_relaxed(iobase + ICLR);
+	st->iccr = readl_relaxed(iobase + ICCR);
+
+	/*
+	 * Disable all GPIO-based interrupts.
+	 */
+	writel_relaxed(st->icmr & 0xfffff000, iobase + ICMR);
+
+	return 0;
+}
+
+static void sa1100irq_resume(void)
+{
+	struct sa1100irq_state *st = &sa1100irq_state;
+
+	if (st->saved) {
+		writel_relaxed(st->iccr, iobase + ICCR);
+		writel_relaxed(st->iclr, iobase + ICLR);
+
+		writel_relaxed(st->icmr, iobase + ICMR);
+	}
+}
+
+static struct syscore_ops sa1100irq_syscore_ops = {
+	.suspend	= sa1100irq_suspend,
+	.resume		= sa1100irq_resume,
+};
+
+static int __init sa1100irq_init_devicefs(void)
+{
+	register_syscore_ops(&sa1100irq_syscore_ops);
+	return 0;
+}
+
+device_initcall(sa1100irq_init_devicefs);
+
+static asmlinkage void __exception_irq_entry
+sa1100_handle_irq(struct pt_regs *regs)
+{
+	uint32_t icip, icmr, mask;
+
+	do {
+		icip = readl_relaxed(iobase + ICIP);
+		icmr = readl_relaxed(iobase + ICMR);
+		mask = icip & icmr;
+
+		if (mask == 0)
+			break;
+
+		handle_domain_irq(sa1100_normal_irqdomain,
+				ffs(mask) - 1, regs);
+	} while (1);
+}
+
+void __init sa11x0_init_irq_nodt(int irq_start, resource_size_t io_start)
+{
+	iobase = ioremap(io_start, SZ_64K);
+	if (WARN_ON(!iobase))
+		return;
+
+	/* disable all IRQs */
+	writel_relaxed(0, iobase + ICMR);
+
+	/* all IRQs are IRQ, not FIQ */
+	writel_relaxed(0, iobase + ICLR);
+
+	/*
+	 * Whatever the doc says, this has to be set for the wait-on-irq
+	 * instruction to work... on a SA1100 rev 9 at least.
+	 */
+	writel_relaxed(1, iobase + ICCR);
+
+	sa1100_normal_irqdomain = irq_domain_add_simple(NULL,
+			32, irq_start,
+			&sa1100_normal_irqdomain_ops, NULL);
+
+	set_handle_irq(sa1100_handle_irq);
+}
diff --git a/drivers/irqchip/irq-sun4i.c b/drivers/irqchip/irq-sun4i.c
index 64155b686081..83d6aa6464ee 100644
--- a/drivers/irqchip/irq-sun4i.c
+++ b/drivers/irqchip/irq-sun4i.c
@@ -89,7 +89,7 @@ static int sun4i_irq_map(struct irq_domain *d, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops sun4i_irq_ops = {
+static const struct irq_domain_ops sun4i_irq_ops = {
 	.map = sun4i_irq_map,
 	.xlate = irq_domain_xlate_onecell,
 };
diff --git a/drivers/irqchip/irq-sunxi-nmi.c b/drivers/irqchip/irq-sunxi-nmi.c
index 4a9ce5b50c5b..6b2b582433bd 100644
--- a/drivers/irqchip/irq-sunxi-nmi.c
+++ b/drivers/irqchip/irq-sunxi-nmi.c
@@ -104,7 +104,7 @@ static int sunxi_sc_nmi_set_type(struct irq_data *data, unsigned int flow_type)
 	irqd_set_trigger_type(data, flow_type);
 	irq_setup_alt_chip(data, flow_type);
 
-	for (i = 0; i <= gc->num_ct; i++, ct++)
+	for (i = 0; i < gc->num_ct; i++, ct++)
 		if (ct->type & flow_type)
 			ctrl_off = ct->regs.type;
 
diff --git a/drivers/irqchip/irq-versatile-fpga.c b/drivers/irqchip/irq-versatile-fpga.c
index 1ab451729a5c..888111b76ea0 100644
--- a/drivers/irqchip/irq-versatile-fpga.c
+++ b/drivers/irqchip/irq-versatile-fpga.c
@@ -132,7 +132,7 @@ static int fpga_irqdomain_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops fpga_irqdomain_ops = {
+static const struct irq_domain_ops fpga_irqdomain_ops = {
 	.map = fpga_irqdomain_map,
 	.xlate = irq_domain_xlate_onetwocell,
 };
diff --git a/drivers/irqchip/irq-vf610-mscm-ir.c b/drivers/irqchip/irq-vf610-mscm-ir.c
index 9521057d4744..f5c01cbcc73a 100644
--- a/drivers/irqchip/irq-vf610-mscm-ir.c
+++ b/drivers/irqchip/irq-vf610-mscm-ir.c
@@ -47,6 +47,7 @@ struct vf610_mscm_ir_chip_data {
 	void __iomem *mscm_ir_base;
 	u16 cpu_mask;
 	u16 saved_irsprc[MSCM_IRSPRC_NUM];
+	bool is_nvic;
 };
 
 static struct vf610_mscm_ir_chip_data *mscm_ir_data;
@@ -101,7 +102,7 @@ static void vf610_mscm_ir_enable(struct irq_data *data)
 	writew_relaxed(chip_data->cpu_mask,
 		       chip_data->mscm_ir_base + MSCM_IRSPRC(hwirq));
 
-	irq_chip_unmask_parent(data);
+	irq_chip_enable_parent(data);
 }
 
 static void vf610_mscm_ir_disable(struct irq_data *data)
@@ -111,7 +112,7 @@ static void vf610_mscm_ir_disable(struct irq_data *data)
 
 	writew_relaxed(0x0, chip_data->mscm_ir_base + MSCM_IRSPRC(hwirq));
 
-	irq_chip_mask_parent(data);
+	irq_chip_disable_parent(data);
 }
 
 static struct irq_chip vf610_mscm_ir_irq_chip = {
@@ -143,10 +144,17 @@ static int vf610_mscm_ir_domain_alloc(struct irq_domain *domain, unsigned int vi
 					      domain->host_data);
 
 	gic_data.np = domain->parent->of_node;
-	gic_data.args_count = 3;
-	gic_data.args[0] = GIC_SPI;
-	gic_data.args[1] = irq_data->args[0];
-	gic_data.args[2] = irq_data->args[1];
+
+	if (mscm_ir_data->is_nvic) {
+		gic_data.args_count = 1;
+		gic_data.args[0] = irq_data->args[0];
+	} else {
+		gic_data.args_count = 3;
+		gic_data.args[0] = GIC_SPI;
+		gic_data.args[1] = irq_data->args[0];
+		gic_data.args[2] = irq_data->args[1];
+	}
+
 	return irq_domain_alloc_irqs_parent(domain, virq, nr_irqs, &gic_data);
 }
 
@@ -174,10 +182,9 @@ static int __init vf610_mscm_ir_of_init(struct device_node *node,
 		return -ENOMEM;
 
 	mscm_ir_data->mscm_ir_base = of_io_request_and_map(node, 0, "mscm-ir");
-
-	if (!mscm_ir_data->mscm_ir_base) {
+	if (IS_ERR(mscm_ir_data->mscm_ir_base)) {
 		pr_err("vf610_mscm_ir: unable to map mscm register\n");
-		ret = -ENOMEM;
+		ret = PTR_ERR(mscm_ir_data->mscm_ir_base);
 		goto out_free;
 	}
 
@@ -199,6 +206,9 @@ static int __init vf610_mscm_ir_of_init(struct device_node *node,
 		goto out_unmap;
 	}
 
+	if (of_device_is_compatible(domain->parent->of_node, "arm,armv7m-nvic"))
+		mscm_ir_data->is_nvic = true;
+
 	cpu_pm_register_notifier(&mscm_ir_notifier_block);
 
 	return 0;
diff --git a/drivers/irqchip/irq-vic.c b/drivers/irqchip/irq-vic.c
index 54089debf2dc..d4ce331ea4a0 100644
--- a/drivers/irqchip/irq-vic.c
+++ b/drivers/irqchip/irq-vic.c
@@ -256,7 +256,7 @@ static void __exception_irq_entry vic_handle_irq(struct pt_regs *regs)
 	} while (handled);
 }
 
-static struct irq_domain_ops vic_irqdomain_ops = {
+static const struct irq_domain_ops vic_irqdomain_ops = {
 	.map = vic_irqdomain_map,
 	.xlate = irq_domain_xlate_onetwocell,
 };
diff --git a/drivers/irqchip/irq-vt8500.c b/drivers/irqchip/irq-vt8500.c
index b7af816f2769..0b297009b856 100644
--- a/drivers/irqchip/irq-vt8500.c
+++ b/drivers/irqchip/irq-vt8500.c
@@ -173,7 +173,7 @@ static int vt8500_irq_map(struct irq_domain *h, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops vt8500_irq_domain_ops = {
+static const struct irq_domain_ops vt8500_irq_domain_ops = {
 	.map = vt8500_irq_map,
 	.xlate = irq_domain_xlate_onecell,
 };
diff --git a/drivers/irqchip/irqchip.h b/drivers/irqchip/irqchip.h
index 0f6486d4f1b0..0f67ae32464f 100644
--- a/drivers/irqchip/irqchip.h
+++ b/drivers/irqchip/irqchip.h
@@ -8,21 +8,4 @@
  * warranty of any kind, whether express or implied.
  */
 
-#ifndef _IRQCHIP_H
-#define _IRQCHIP_H
-
-#include <linux/of.h>
-
-/*
- * This macro must be used by the different irqchip drivers to declare
- * the association between their DT compatible string and their
- * initialization function.
- *
- * @name: name that must be unique accross all IRQCHIP_DECLARE of the
- * same file.
- * @compstr: compatible string of the irqchip driver
- * @fn: initialization function
- */
-#define IRQCHIP_DECLARE(name, compat, fn) OF_DECLARE_2(irqchip, name, compat, fn)
-
-#endif
+#include <linux/irqchip.h>
diff --git a/drivers/irqchip/spear-shirq.c b/drivers/irqchip/spear-shirq.c
index 9c145a7cb056..a45121546caf 100644
--- a/drivers/irqchip/spear-shirq.c
+++ b/drivers/irqchip/spear-shirq.c
@@ -207,8 +207,7 @@ static void __init spear_shirq_register(struct spear_shirq *shirq,
 	if (!shirq->irq_chip)
 		return;
 
-	irq_set_chained_handler(parent_irq, shirq_handler);
-	irq_set_handler_data(parent_irq, shirq);
+	irq_set_chained_handler_and_data(parent_irq, shirq_handler, shirq);
 
 	for (i = 0; i < shirq->nr_irqs; i++) {
 		irq_set_chip_and_handler(shirq->virq_base + i,
diff --git a/drivers/isdn/capi/capidrv.c b/drivers/isdn/capi/capidrv.c
index 1cc6ca8bfbda..85cfa4f8691f 100644
--- a/drivers/isdn/capi/capidrv.c
+++ b/drivers/isdn/capi/capidrv.c
@@ -2264,7 +2264,7 @@ static int capidrv_addcontr(u16 contr, struct capi_profile *profp)
 		return -1;
 	}
 	card->owner = THIS_MODULE;
-	init_timer(&card->listentimer);
+	setup_timer(&card->listentimer, listentimerfunc, (unsigned long)card);
 	strcpy(card->name, id);
 	card->contrnr = contr;
 	card->nbchan = profp->nbchannel;
@@ -2331,8 +2331,6 @@ static int capidrv_addcontr(u16 contr, struct capi_profile *profp)
 	card->cipmask = 0x1FFF03FF;	/* any */
 	card->cipmask2 = 0;
 
-	card->listentimer.data = (unsigned long)card;
-	card->listentimer.function = listentimerfunc;
 	send_listen(card);
 	mod_timer(&card->listentimer, jiffies + 60 * HZ);
 
diff --git a/drivers/isdn/hisax/Kconfig b/drivers/isdn/hisax/Kconfig
index 97465ac5a2d5..eb83d94ab4fe 100644
--- a/drivers/isdn/hisax/Kconfig
+++ b/drivers/isdn/hisax/Kconfig
@@ -237,7 +237,7 @@ config HISAX_MIC
 
 config HISAX_NETJET
 	bool "NETjet card"
-	depends on PCI && (BROKEN || !(PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN)))
+	depends on PCI && (BROKEN || !(PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN) || MICROBLAZE))
 	depends on VIRT_TO_BUS
 	help
 	  This enables HiSax support for the NetJet from Traverse
@@ -249,7 +249,7 @@ config HISAX_NETJET
 
 config HISAX_NETJET_U
 	bool "NETspider U card"
-	depends on PCI && (BROKEN || !(PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN)))
+	depends on PCI && (BROKEN || !(PPC || PARISC || M68K || (MIPS && !CPU_LITTLE_ENDIAN) || FRV || (XTENSA && !CPU_LITTLE_ENDIAN) || MICROBLAZE))
 	depends on VIRT_TO_BUS
 	help
 	  This enables HiSax support for the Netspider U interface ISDN card
diff --git a/drivers/isdn/hisax/st5481_usb.c b/drivers/isdn/hisax/st5481_usb.c
index ead0a4fb7448..a0fdbc074b98 100644
--- a/drivers/isdn/hisax/st5481_usb.c
+++ b/drivers/isdn/hisax/st5481_usb.c
@@ -267,8 +267,8 @@ int st5481_setup_usb(struct st5481_adapter *adapter)
 	}
 
 	// The descriptor is wrong for some early samples of the ST5481 chip
-	altsetting->endpoint[3].desc.wMaxPacketSize = __constant_cpu_to_le16(32);
-	altsetting->endpoint[4].desc.wMaxPacketSize = __constant_cpu_to_le16(32);
+	altsetting->endpoint[3].desc.wMaxPacketSize = cpu_to_le16(32);
+	altsetting->endpoint[4].desc.wMaxPacketSize = cpu_to_le16(32);
 
 	// Use alternative setting 3 on interface 0 to have 2B+D
 	if ((status = usb_set_interface(dev, 0, 3)) < 0) {
diff --git a/drivers/isdn/i4l/isdn_net.c b/drivers/isdn/i4l/isdn_net.c
index 546b7e81161d..aa5dd5668528 100644
--- a/drivers/isdn/i4l/isdn_net.c
+++ b/drivers/isdn/i4l/isdn_net.c
@@ -58,7 +58,7 @@
  * About SOFTNET:
  * Most of the changes were pretty obvious and basically done by HE already.
  *
- * One problem of the isdn net device code is that is uses struct net_device
+ * One problem of the isdn net device code is that it uses struct net_device
  * for masters and slaves. However, only master interface are registered to
  * the network layer, and therefore, it only makes sense to call netif_*
  * functions on them.
diff --git a/drivers/isdn/mISDN/socket.c b/drivers/isdn/mISDN/socket.c
index 8dc7290089bb..0d29b5a6356d 100644
--- a/drivers/isdn/mISDN/socket.c
+++ b/drivers/isdn/mISDN/socket.c
@@ -601,14 +601,14 @@ static const struct proto_ops data_sock_ops = {
 };
 
 static int
-data_sock_create(struct net *net, struct socket *sock, int protocol)
+data_sock_create(struct net *net, struct socket *sock, int protocol, int kern)
 {
 	struct sock *sk;
 
 	if (sock->type != SOCK_DGRAM)
 		return -ESOCKTNOSUPPORT;
 
-	sk = sk_alloc(net, PF_ISDN, GFP_KERNEL, &mISDN_proto);
+	sk = sk_alloc(net, PF_ISDN, GFP_KERNEL, &mISDN_proto, kern);
 	if (!sk)
 		return -ENOMEM;
 
@@ -756,14 +756,14 @@ static const struct proto_ops base_sock_ops = {
 
 
 static int
-base_sock_create(struct net *net, struct socket *sock, int protocol)
+base_sock_create(struct net *net, struct socket *sock, int protocol, int kern)
 {
 	struct sock *sk;
 
 	if (sock->type != SOCK_RAW)
 		return -ESOCKTNOSUPPORT;
 
-	sk = sk_alloc(net, PF_ISDN, GFP_KERNEL, &mISDN_proto);
+	sk = sk_alloc(net, PF_ISDN, GFP_KERNEL, &mISDN_proto, kern);
 	if (!sk)
 		return -ENOMEM;
 
@@ -785,7 +785,7 @@ mISDN_sock_create(struct net *net, struct socket *sock, int proto, int kern)
 
 	switch (proto) {
 	case ISDN_P_BASE:
-		err = base_sock_create(net, sock, proto);
+		err = base_sock_create(net, sock, proto, kern);
 		break;
 	case ISDN_P_TE_S0:
 	case ISDN_P_NT_S0:
@@ -799,7 +799,7 @@ mISDN_sock_create(struct net *net, struct socket *sock, int proto, int kern)
 	case ISDN_P_B_L2DTMF:
 	case ISDN_P_B_L2DSP:
 	case ISDN_P_B_L2DSPHDLC:
-		err = data_sock_create(net, sock, proto);
+		err = data_sock_create(net, sock, proto, kern);
 		break;
 	default:
 		return err;
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 966b9605f5f0..9ad35f72ab4c 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -11,9 +11,6 @@ menuconfig NEW_LEDS
 	  Say Y to enable Linux LED support.  This allows control of supported
 	  LEDs from both userspace and optionally, by kernel events (triggers).
 
-	  This is not related to standard keyboard LEDs which are controlled
-	  via the input system.
-
 if NEW_LEDS
 
 config LEDS_CLASS
@@ -42,6 +39,32 @@ config LEDS_88PM860X
 	  This option enables support for on-chip LED drivers found on Marvell
 	  Semiconductor 88PM8606 PMIC.
 
+config LEDS_AAT1290
+	tristate "LED support for the AAT1290"
+	depends on LEDS_CLASS_FLASH
+	depends on V4L2_FLASH_LED_CLASS || !V4L2_FLASH_LED_CLASS
+	depends on GPIOLIB
+	depends on OF
+	depends on PINCTRL
+	help
+	 This option enables support for the LEDs on the AAT1290.
+
+config LEDS_BCM6328
+	tristate "LED Support for Broadcom BCM6328"
+	depends on LEDS_CLASS
+	depends on OF
+	help
+	  This option enables support for LEDs connected to the BCM6328
+	  LED HW controller accessed via MMIO registers.
+
+config LEDS_BCM6358
+	tristate "LED Support for Broadcom BCM6358"
+	depends on LEDS_CLASS
+	depends on OF
+	help
+	  This option enables support for LEDs connected to the BCM6358
+	  LED HW controller accessed via MMIO registers.
+
 config LEDS_LM3530
 	tristate "LCD Backlight driver for LM3530"
 	depends on LEDS_CLASS
@@ -182,7 +205,7 @@ config LEDS_PCA9532_GPIO
 config LEDS_GPIO
 	tristate "LED Support for GPIO connected LEDs"
 	depends on LEDS_CLASS
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  This option enables support for the LEDs connected to GPIO
 	  outputs. To be useful the particular board must have LEDs
@@ -206,6 +229,7 @@ config LEDS_LP55XX_COMMON
 	tristate "Common Driver for TI/National LP5521/5523/55231/5562/8501"
 	depends on LEDS_LP5521 || LEDS_LP5523 || LEDS_LP5562 || LEDS_LP8501
 	select FW_LOADER
+	select FW_LOADER_USER_HELPER_FALLBACK
 	help
 	  This option supports common operations for LP5521/5523/55231/5562/8501
 	  devices.
@@ -467,6 +491,25 @@ config LEDS_TCA6507
 	  LED driver chips accessed via the I2C bus.
 	  Driver support brightness control and hardware-assisted blinking.
 
+config LEDS_TLC591XX
+	tristate "LED driver for TLC59108 and TLC59116 controllers"
+	depends on LEDS_CLASS && I2C
+	select REGMAP_I2C
+	help
+	  This option enables support for Texas Instruments TLC59108
+	  and TLC59116 LED controllers.
+
+config LEDS_MAX77693
+	tristate "LED support for MAX77693 Flash"
+	depends on LEDS_CLASS_FLASH
+	depends on V4L2_FLASH_LED_CLASS || !V4L2_FLASH_LED_CLASS
+	depends on MFD_MAX77693
+	depends on OF
+	help
+	  This option enables support for the flash part of the MAX77693
+	  multifunction device. It has build in control for two leds in flash
+	  and torch mode.
+
 config LEDS_MAX8997
 	tristate "LED support for MAX8997 PMIC"
 	depends on LEDS_CLASS && MFD_MAX8997
@@ -498,6 +541,15 @@ config LEDS_MENF21BMC
 	  This driver can also be built as a module. If so the module
 	  will be called leds-menf21bmc.
 
+config LEDS_KTD2692
+	tristate "LED support for KTD2692 flash LED controller"
+	depends on LEDS_CLASS_FLASH && GPIOLIB && OF
+	help
+	  This option enables support for KTD2692 LED flash connected
+	  through ExpressWire interface.
+
+	  Say Y to enable this driver.
+
 comment "LED driver for blink(1) USB RGB LED is under Special HID drivers (HID_THINGM)"
 
 config LEDS_BLINKM
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index bf4609338e10..8d6a24a2f513 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -7,6 +7,9 @@ obj-$(CONFIG_LEDS_TRIGGERS)		+= led-triggers.o
 
 # LED Platform Drivers
 obj-$(CONFIG_LEDS_88PM860X)		+= leds-88pm860x.o
+obj-$(CONFIG_LEDS_AAT1290)		+= leds-aat1290.o
+obj-$(CONFIG_LEDS_BCM6328)		+= leds-bcm6328.o
+obj-$(CONFIG_LEDS_BCM6358)		+= leds-bcm6358.o
 obj-$(CONFIG_LEDS_BD2802)		+= leds-bd2802.o
 obj-$(CONFIG_LEDS_LOCOMO)		+= leds-locomo.o
 obj-$(CONFIG_LEDS_LM3530)		+= leds-lm3530.o
@@ -31,6 +34,7 @@ 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_TLC591XX)		+= leds-tlc591xx.o
 obj-$(CONFIG_LEDS_CLEVO_MAIL)		+= leds-clevo-mail.o
 obj-$(CONFIG_LEDS_IPAQ_MICRO)		+= leds-ipaq-micro.o
 obj-$(CONFIG_LEDS_HP6XX)		+= leds-hp6xx.o
@@ -52,6 +56,7 @@ obj-$(CONFIG_LEDS_MC13783)		+= leds-mc13783.o
 obj-$(CONFIG_LEDS_NS2)			+= leds-ns2.o
 obj-$(CONFIG_LEDS_NETXBIG)		+= leds-netxbig.o
 obj-$(CONFIG_LEDS_ASIC3)		+= leds-asic3.o
+obj-$(CONFIG_LEDS_MAX77693)		+= leds-max77693.o
 obj-$(CONFIG_LEDS_MAX8997)		+= leds-max8997.o
 obj-$(CONFIG_LEDS_LM355x)		+= leds-lm355x.o
 obj-$(CONFIG_LEDS_BLINKM)		+= leds-blinkm.o
@@ -59,6 +64,7 @@ obj-$(CONFIG_LEDS_SYSCON)		+= leds-syscon.o
 obj-$(CONFIG_LEDS_VERSATILE)		+= leds-versatile.o
 obj-$(CONFIG_LEDS_MENF21BMC)		+= leds-menf21bmc.o
 obj-$(CONFIG_LEDS_PM8941_WLED)		+= leds-pm8941-wled.o
+obj-$(CONFIG_LEDS_KTD2692)		+= leds-ktd2692.o
 
 # LED SPI Drivers
 obj-$(CONFIG_LEDS_DAC124S085)		+= leds-dac124s085.o
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index 728681debdbe..beabfbc6f7cd 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -121,6 +121,11 @@ static void led_timer_function(unsigned long data)
 	brightness = led_get_brightness(led_cdev);
 	if (!brightness) {
 		/* Time to switch the LED on. */
+		if (led_cdev->delayed_set_value) {
+			led_cdev->blink_brightness =
+					led_cdev->delayed_set_value;
+			led_cdev->delayed_set_value = 0;
+		}
 		brightness = led_cdev->blink_brightness;
 		delay = led_cdev->blink_delay_on;
 	} else {
@@ -187,6 +192,7 @@ void led_classdev_resume(struct led_classdev *led_cdev)
 }
 EXPORT_SYMBOL_GPL(led_classdev_resume);
 
+#ifdef CONFIG_PM_SLEEP
 static int led_suspend(struct device *dev)
 {
 	struct led_classdev *led_cdev = dev_get_drvdata(dev);
@@ -206,11 +212,9 @@ static int led_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
-static const struct dev_pm_ops leds_class_dev_pm_ops = {
-	.suspend        = led_suspend,
-	.resume         = led_resume,
-};
+static SIMPLE_DEV_PM_OPS(leds_class_dev_pm_ops, led_suspend, led_resume);
 
 static int match_name(struct device *dev, const void *data)
 {
diff --git a/drivers/leds/led-core.c b/drivers/leds/led-core.c
index 9886dace5ad2..549de7e24cfd 100644
--- a/drivers/leds/led-core.c
+++ b/drivers/leds/led-core.c
@@ -119,10 +119,11 @@ void led_set_brightness(struct led_classdev *led_cdev,
 {
 	int ret = 0;
 
-	/* delay brightness setting if need to stop soft-blink timer */
+	/* delay brightness if soft-blink is active */
 	if (led_cdev->blink_delay_on || led_cdev->blink_delay_off) {
 		led_cdev->delayed_set_value = brightness;
-		schedule_work(&led_cdev->set_brightness_work);
+		if (brightness == LED_OFF)
+			schedule_work(&led_cdev->set_brightness_work);
 		return;
 	}
 
diff --git a/drivers/leds/leds-aat1290.c b/drivers/leds/leds-aat1290.c
new file mode 100644
index 000000000000..fd7c25fd29c1
--- /dev/null
+++ b/drivers/leds/leds-aat1290.c
@@ -0,0 +1,576 @@
+/*
+ *	LED Flash class driver for the AAT1290
+ *	1.5A Step-Up Current Regulator for Flash LEDs
+ *
+ *	Copyright (C) 2015, Samsung Electronics Co., Ltd.
+ *	Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/led-class-flash.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-flash-led-class.h>
+
+#define AAT1290_MOVIE_MODE_CURRENT_ADDR	17
+#define AAT1290_MAX_MM_CURR_PERCENT_0	16
+#define AAT1290_MAX_MM_CURR_PERCENT_100	1
+
+#define AAT1290_FLASH_SAFETY_TIMER_ADDR	18
+
+#define AAT1290_MOVIE_MODE_CONFIG_ADDR	19
+#define AAT1290_MOVIE_MODE_OFF		1
+#define AAT1290_MOVIE_MODE_ON		3
+
+#define AAT1290_MM_CURRENT_RATIO_ADDR	20
+#define AAT1290_MM_TO_FL_1_92		1
+
+#define AAT1290_MM_TO_FL_RATIO		1000 / 1920
+#define AAT1290_MAX_MM_CURRENT(fl_max)	(fl_max * AAT1290_MM_TO_FL_RATIO)
+
+#define AAT1290_LATCH_TIME_MIN_US	500
+#define AAT1290_LATCH_TIME_MAX_US	1000
+#define AAT1290_EN_SET_TICK_TIME_US	1
+#define AAT1290_FLEN_OFF_DELAY_TIME_US	10
+#define AAT1290_FLASH_TM_NUM_LEVELS	16
+#define AAT1290_MM_CURRENT_SCALE_SIZE	15
+
+
+struct aat1290_led_config_data {
+	/* maximum LED current in movie mode */
+	u32 max_mm_current;
+	/* maximum LED current in flash mode */
+	u32 max_flash_current;
+	/* maximum flash timeout */
+	u32 max_flash_tm;
+	/* external strobe capability */
+	bool has_external_strobe;
+	/* max LED brightness level */
+	enum led_brightness max_brightness;
+};
+
+struct aat1290_led {
+	/* platform device data */
+	struct platform_device *pdev;
+	/* secures access to the device */
+	struct mutex lock;
+
+	/* corresponding LED Flash class device */
+	struct led_classdev_flash fled_cdev;
+	/* V4L2 Flash device */
+	struct v4l2_flash *v4l2_flash;
+
+	/* FLEN pin */
+	struct gpio_desc *gpio_fl_en;
+	/* EN|SET pin  */
+	struct gpio_desc *gpio_en_set;
+	/* movie mode current scale */
+	int *mm_current_scale;
+	/* device mode */
+	bool movie_mode;
+
+	/* brightness cache */
+	unsigned int torch_brightness;
+	/* assures led-triggers compatibility */
+	struct work_struct work_brightness_set;
+};
+
+static struct aat1290_led *fled_cdev_to_led(
+				struct led_classdev_flash *fled_cdev)
+{
+	return container_of(fled_cdev, struct aat1290_led, fled_cdev);
+}
+
+static void aat1290_as2cwire_write(struct aat1290_led *led, int addr, int value)
+{
+	int i;
+
+	gpiod_direction_output(led->gpio_fl_en, 0);
+	gpiod_direction_output(led->gpio_en_set, 0);
+
+	udelay(AAT1290_FLEN_OFF_DELAY_TIME_US);
+
+	/* write address */
+	for (i = 0; i < addr; ++i) {
+		udelay(AAT1290_EN_SET_TICK_TIME_US);
+		gpiod_direction_output(led->gpio_en_set, 0);
+		udelay(AAT1290_EN_SET_TICK_TIME_US);
+		gpiod_direction_output(led->gpio_en_set, 1);
+	}
+
+	usleep_range(AAT1290_LATCH_TIME_MIN_US, AAT1290_LATCH_TIME_MAX_US);
+
+	/* write data */
+	for (i = 0; i < value; ++i) {
+		udelay(AAT1290_EN_SET_TICK_TIME_US);
+		gpiod_direction_output(led->gpio_en_set, 0);
+		udelay(AAT1290_EN_SET_TICK_TIME_US);
+		gpiod_direction_output(led->gpio_en_set, 1);
+	}
+
+	usleep_range(AAT1290_LATCH_TIME_MIN_US, AAT1290_LATCH_TIME_MAX_US);
+}
+
+static void aat1290_set_flash_safety_timer(struct aat1290_led *led,
+					unsigned int micro_sec)
+{
+	struct led_classdev_flash *fled_cdev = &led->fled_cdev;
+	struct led_flash_setting *flash_tm = &fled_cdev->timeout;
+	int flash_tm_reg = AAT1290_FLASH_TM_NUM_LEVELS -
+				(micro_sec / flash_tm->step) + 1;
+
+	aat1290_as2cwire_write(led, AAT1290_FLASH_SAFETY_TIMER_ADDR,
+							flash_tm_reg);
+}
+
+static void aat1290_brightness_set(struct aat1290_led *led,
+					enum led_brightness brightness)
+{
+	mutex_lock(&led->lock);
+
+	if (brightness == 0) {
+		gpiod_direction_output(led->gpio_fl_en, 0);
+		gpiod_direction_output(led->gpio_en_set, 0);
+		led->movie_mode = false;
+	} else {
+		if (!led->movie_mode) {
+			aat1290_as2cwire_write(led,
+				AAT1290_MM_CURRENT_RATIO_ADDR,
+				AAT1290_MM_TO_FL_1_92);
+			led->movie_mode = true;
+		}
+
+		aat1290_as2cwire_write(led, AAT1290_MOVIE_MODE_CURRENT_ADDR,
+				AAT1290_MAX_MM_CURR_PERCENT_0 - brightness);
+		aat1290_as2cwire_write(led, AAT1290_MOVIE_MODE_CONFIG_ADDR,
+				AAT1290_MOVIE_MODE_ON);
+	}
+
+	mutex_unlock(&led->lock);
+}
+
+/* LED subsystem callbacks */
+
+static void aat1290_brightness_set_work(struct work_struct *work)
+{
+	struct aat1290_led *led =
+		container_of(work, struct aat1290_led, work_brightness_set);
+
+	aat1290_brightness_set(led, led->torch_brightness);
+}
+
+static void aat1290_led_brightness_set(struct led_classdev *led_cdev,
+					enum led_brightness brightness)
+{
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
+	struct aat1290_led *led = fled_cdev_to_led(fled_cdev);
+
+	led->torch_brightness = brightness;
+	schedule_work(&led->work_brightness_set);
+}
+
+static int aat1290_led_brightness_set_sync(struct led_classdev *led_cdev,
+					enum led_brightness brightness)
+{
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
+	struct aat1290_led *led = fled_cdev_to_led(fled_cdev);
+
+	aat1290_brightness_set(led, brightness);
+
+	return 0;
+}
+
+static int aat1290_led_flash_strobe_set(struct led_classdev_flash *fled_cdev,
+					 bool state)
+
+{
+	struct aat1290_led *led = fled_cdev_to_led(fled_cdev);
+	struct led_classdev *led_cdev = &fled_cdev->led_cdev;
+	struct led_flash_setting *timeout = &fled_cdev->timeout;
+
+	mutex_lock(&led->lock);
+
+	if (state) {
+		aat1290_set_flash_safety_timer(led, timeout->val);
+		gpiod_direction_output(led->gpio_fl_en, 1);
+	} else {
+		gpiod_direction_output(led->gpio_fl_en, 0);
+		gpiod_direction_output(led->gpio_en_set, 0);
+	}
+
+	/*
+	 * To reenter movie mode after a flash event the part must be cycled
+	 * off and back on to reset the movie mode and reprogrammed via the
+	 * AS2Cwire. Therefore the brightness and movie_mode properties needs
+	 * to be updated here to reflect the actual state.
+	 */
+	led_cdev->brightness = 0;
+	led->movie_mode = false;
+
+	mutex_unlock(&led->lock);
+
+	return 0;
+}
+
+static int aat1290_led_flash_timeout_set(struct led_classdev_flash *fled_cdev,
+						u32 timeout)
+{
+	/*
+	 * Don't do anything - flash timeout is cached in the led-class-flash
+	 * core and will be applied in the strobe_set op, as writing the
+	 * safety timer register spuriously turns the torch mode on.
+	 */
+
+	return 0;
+}
+
+static int aat1290_led_parse_dt(struct aat1290_led *led,
+			struct aat1290_led_config_data *cfg,
+			struct device_node **sub_node)
+{
+	struct led_classdev *led_cdev = &led->fled_cdev.led_cdev;
+	struct device *dev = &led->pdev->dev;
+	struct device_node *child_node;
+#if IS_ENABLED(CONFIG_V4L2_FLASH_LED_CLASS)
+	struct pinctrl *pinctrl;
+#endif
+	int ret = 0;
+
+	led->gpio_fl_en = devm_gpiod_get(dev, "flen", GPIOD_ASIS);
+	if (IS_ERR(led->gpio_fl_en)) {
+		ret = PTR_ERR(led->gpio_fl_en);
+		dev_err(dev, "Unable to claim gpio \"flen\".\n");
+		return ret;
+	}
+
+	led->gpio_en_set = devm_gpiod_get(dev, "enset", GPIOD_ASIS);
+	if (IS_ERR(led->gpio_en_set)) {
+		ret = PTR_ERR(led->gpio_en_set);
+		dev_err(dev, "Unable to claim gpio \"enset\".\n");
+		return ret;
+	}
+
+#if IS_ENABLED(CONFIG_V4L2_FLASH_LED_CLASS)
+	pinctrl = devm_pinctrl_get_select_default(&led->pdev->dev);
+	if (IS_ERR(pinctrl)) {
+		cfg->has_external_strobe = false;
+		dev_info(dev,
+			 "No support for external strobe detected.\n");
+	} else {
+		cfg->has_external_strobe = true;
+	}
+#endif
+
+	child_node = of_get_next_available_child(dev->of_node, NULL);
+	if (!child_node) {
+		dev_err(dev, "No DT child node found for connected LED.\n");
+		return -EINVAL;
+	}
+
+	led_cdev->name = of_get_property(child_node, "label", NULL) ? :
+						child_node->name;
+
+	ret = of_property_read_u32(child_node, "led-max-microamp",
+				&cfg->max_mm_current);
+	/*
+	 * led-max-microamp will default to 1/20 of flash-max-microamp
+	 * in case it is missing.
+	 */
+	if (ret < 0)
+		dev_warn(dev,
+			"led-max-microamp DT property missing\n");
+
+	ret = of_property_read_u32(child_node, "flash-max-microamp",
+				&cfg->max_flash_current);
+	if (ret < 0) {
+		dev_err(dev,
+			"flash-max-microamp DT property missing\n");
+		return ret;
+	}
+
+	ret = of_property_read_u32(child_node, "flash-max-timeout-us",
+				&cfg->max_flash_tm);
+	if (ret < 0) {
+		dev_err(dev,
+			"flash-max-timeout-us DT property missing\n");
+		return ret;
+	}
+
+	of_node_put(child_node);
+
+	*sub_node = child_node;
+
+	return ret;
+}
+
+static void aat1290_led_validate_mm_current(struct aat1290_led *led,
+					struct aat1290_led_config_data *cfg)
+{
+	int i, b = 0, e = AAT1290_MM_CURRENT_SCALE_SIZE;
+
+	while (e - b > 1) {
+		i = b + (e - b) / 2;
+		if (cfg->max_mm_current < led->mm_current_scale[i])
+			e = i;
+		else
+			b = i;
+	}
+
+	cfg->max_mm_current = led->mm_current_scale[b];
+	cfg->max_brightness = b + 1;
+}
+
+int init_mm_current_scale(struct aat1290_led *led,
+			struct aat1290_led_config_data *cfg)
+{
+	int max_mm_current_percent[] = { 20, 22, 25, 28, 32, 36, 40, 45, 50, 56,
+						63, 71, 79, 89, 100 };
+	int i, max_mm_current =
+			AAT1290_MAX_MM_CURRENT(cfg->max_flash_current);
+
+	led->mm_current_scale = devm_kzalloc(&led->pdev->dev,
+					sizeof(max_mm_current_percent),
+					GFP_KERNEL);
+	if (!led->mm_current_scale)
+		return -ENOMEM;
+
+	for (i = 0; i < AAT1290_MM_CURRENT_SCALE_SIZE; ++i)
+		led->mm_current_scale[i] = max_mm_current *
+					  max_mm_current_percent[i] / 100;
+
+	return 0;
+}
+
+static int aat1290_led_get_configuration(struct aat1290_led *led,
+					struct aat1290_led_config_data *cfg,
+					struct device_node **sub_node)
+{
+	int ret;
+
+	ret = aat1290_led_parse_dt(led, cfg, sub_node);
+	if (ret < 0)
+		return ret;
+	/*
+	 * Init non-linear movie mode current scale basing
+	 * on the max flash current from led configuration.
+	 */
+	ret = init_mm_current_scale(led, cfg);
+	if (ret < 0)
+		return ret;
+
+	aat1290_led_validate_mm_current(led, cfg);
+
+#if IS_ENABLED(CONFIG_V4L2_FLASH_LED_CLASS)
+#else
+	devm_kfree(&led->pdev->dev, led->mm_current_scale);
+#endif
+
+	return 0;
+}
+
+static void aat1290_init_flash_timeout(struct aat1290_led *led,
+				struct aat1290_led_config_data *cfg)
+{
+	struct led_classdev_flash *fled_cdev = &led->fled_cdev;
+	struct led_flash_setting *setting;
+
+	/* Init flash timeout setting */
+	setting = &fled_cdev->timeout;
+	setting->min = cfg->max_flash_tm / AAT1290_FLASH_TM_NUM_LEVELS;
+	setting->max = cfg->max_flash_tm;
+	setting->step = setting->min;
+	setting->val = setting->max;
+}
+
+#if IS_ENABLED(CONFIG_V4L2_FLASH_LED_CLASS)
+static enum led_brightness aat1290_intensity_to_brightness(
+					struct v4l2_flash *v4l2_flash,
+					s32 intensity)
+{
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	struct aat1290_led *led = fled_cdev_to_led(fled_cdev);
+	int i;
+
+	for (i = AAT1290_MM_CURRENT_SCALE_SIZE - 1; i >= 0; --i)
+		if (intensity >= led->mm_current_scale[i])
+			return i + 1;
+
+	return 1;
+}
+
+static s32 aat1290_brightness_to_intensity(struct v4l2_flash *v4l2_flash,
+					enum led_brightness brightness)
+{
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	struct aat1290_led *led = fled_cdev_to_led(fled_cdev);
+
+	return led->mm_current_scale[brightness - 1];
+}
+
+static int aat1290_led_external_strobe_set(struct v4l2_flash *v4l2_flash,
+						bool enable)
+{
+	struct aat1290_led *led = fled_cdev_to_led(v4l2_flash->fled_cdev);
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	struct led_classdev *led_cdev = &fled_cdev->led_cdev;
+	struct pinctrl *pinctrl;
+
+	gpiod_direction_output(led->gpio_fl_en, 0);
+	gpiod_direction_output(led->gpio_en_set, 0);
+
+	led->movie_mode = false;
+	led_cdev->brightness = 0;
+
+	pinctrl = devm_pinctrl_get_select(&led->pdev->dev,
+						enable ? "isp" : "host");
+	if (IS_ERR(pinctrl)) {
+		dev_warn(&led->pdev->dev, "Unable to switch strobe source.\n");
+		return PTR_ERR(pinctrl);
+	}
+
+	return 0;
+}
+
+static void aat1290_init_v4l2_flash_config(struct aat1290_led *led,
+					struct aat1290_led_config_data *led_cfg,
+					struct v4l2_flash_config *v4l2_sd_cfg)
+{
+	struct led_classdev *led_cdev = &led->fled_cdev.led_cdev;
+	struct led_flash_setting *s;
+
+	strlcpy(v4l2_sd_cfg->dev_name, led_cdev->name,
+		sizeof(v4l2_sd_cfg->dev_name));
+
+	s = &v4l2_sd_cfg->torch_intensity;
+	s->min = led->mm_current_scale[0];
+	s->max = led_cfg->max_mm_current;
+	s->step = 1;
+	s->val = s->max;
+
+	v4l2_sd_cfg->has_external_strobe = led_cfg->has_external_strobe;
+}
+
+static const struct v4l2_flash_ops v4l2_flash_ops = {
+	.external_strobe_set = aat1290_led_external_strobe_set,
+	.intensity_to_led_brightness = aat1290_intensity_to_brightness,
+	.led_brightness_to_intensity = aat1290_brightness_to_intensity,
+};
+#else
+static inline void aat1290_init_v4l2_flash_config(struct aat1290_led *led,
+				struct aat1290_led_config_data *led_cfg,
+				struct v4l2_flash_config *v4l2_sd_cfg)
+{
+}
+static const struct v4l2_flash_ops v4l2_flash_ops;
+#endif
+
+static const struct led_flash_ops flash_ops = {
+	.strobe_set = aat1290_led_flash_strobe_set,
+	.timeout_set = aat1290_led_flash_timeout_set,
+};
+
+static int aat1290_led_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *sub_node = NULL;
+	struct aat1290_led *led;
+	struct led_classdev *led_cdev;
+	struct led_classdev_flash *fled_cdev;
+	struct aat1290_led_config_data led_cfg = {};
+	struct v4l2_flash_config v4l2_sd_cfg = {};
+	int ret;
+
+	led = devm_kzalloc(dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->pdev = pdev;
+	platform_set_drvdata(pdev, led);
+
+	fled_cdev = &led->fled_cdev;
+	fled_cdev->ops = &flash_ops;
+	led_cdev = &fled_cdev->led_cdev;
+
+	ret = aat1290_led_get_configuration(led, &led_cfg, &sub_node);
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&led->lock);
+
+	/* Initialize LED Flash class device */
+	led_cdev->brightness_set = aat1290_led_brightness_set;
+	led_cdev->brightness_set_sync = aat1290_led_brightness_set_sync;
+	led_cdev->max_brightness = led_cfg.max_brightness;
+	led_cdev->flags |= LED_DEV_CAP_FLASH;
+	INIT_WORK(&led->work_brightness_set, aat1290_brightness_set_work);
+
+	aat1290_init_flash_timeout(led, &led_cfg);
+
+	/* Register LED Flash class device */
+	ret = led_classdev_flash_register(&pdev->dev, fled_cdev);
+	if (ret < 0)
+		goto err_flash_register;
+
+	aat1290_init_v4l2_flash_config(led, &led_cfg, &v4l2_sd_cfg);
+
+	/* Create V4L2 Flash subdev. */
+	led->v4l2_flash = v4l2_flash_init(dev, sub_node, fled_cdev, NULL,
+					  &v4l2_flash_ops, &v4l2_sd_cfg);
+	if (IS_ERR(led->v4l2_flash)) {
+		ret = PTR_ERR(led->v4l2_flash);
+		goto error_v4l2_flash_init;
+	}
+
+	return 0;
+
+error_v4l2_flash_init:
+	led_classdev_flash_unregister(fled_cdev);
+err_flash_register:
+	mutex_destroy(&led->lock);
+
+	return ret;
+}
+
+static int aat1290_led_remove(struct platform_device *pdev)
+{
+	struct aat1290_led *led = platform_get_drvdata(pdev);
+
+	v4l2_flash_release(led->v4l2_flash);
+	led_classdev_flash_unregister(&led->fled_cdev);
+	cancel_work_sync(&led->work_brightness_set);
+
+	mutex_destroy(&led->lock);
+
+	return 0;
+}
+
+static const struct of_device_id aat1290_led_dt_match[] = {
+	{ .compatible = "skyworks,aat1290" },
+	{},
+};
+
+static struct platform_driver aat1290_led_driver = {
+	.probe		= aat1290_led_probe,
+	.remove		= aat1290_led_remove,
+	.driver		= {
+		.name	= "aat1290",
+		.of_match_table = aat1290_led_dt_match,
+	},
+};
+
+module_platform_driver(aat1290_led_driver);
+
+MODULE_AUTHOR("Jacek Anaszewski <j.anaszewski@samsung.com>");
+MODULE_DESCRIPTION("Skyworks Current Regulator for Flash LEDs");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-bcm6328.c b/drivers/leds/leds-bcm6328.c
new file mode 100644
index 000000000000..986fe1e28f84
--- /dev/null
+++ b/drivers/leds/leds-bcm6328.c
@@ -0,0 +1,413 @@
+/*
+ * Driver for BCM6328 memory-mapped LEDs, based on leds-syscon.c
+ *
+ * Copyright 2015 Álvaro Fernández Rojas <noltari@gmail.com>
+ * Copyright 2015 Jonas Gorski <jogo@openwrt.org>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+#include <linux/io.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+
+#define BCM6328_REG_INIT		0x00
+#define BCM6328_REG_MODE_HI		0x04
+#define BCM6328_REG_MODE_LO		0x08
+#define BCM6328_REG_HWDIS		0x0c
+#define BCM6328_REG_STROBE		0x10
+#define BCM6328_REG_LNKACTSEL_HI	0x14
+#define BCM6328_REG_LNKACTSEL_LO	0x18
+#define BCM6328_REG_RBACK		0x1c
+#define BCM6328_REG_SERMUX		0x20
+
+#define BCM6328_LED_MAX_COUNT		24
+#define BCM6328_LED_DEF_DELAY		500
+#define BCM6328_LED_INTERVAL_MS		20
+
+#define BCM6328_LED_INTV_MASK		0x3f
+#define BCM6328_LED_FAST_INTV_SHIFT	6
+#define BCM6328_LED_FAST_INTV_MASK	(BCM6328_LED_INTV_MASK << \
+					 BCM6328_LED_FAST_INTV_SHIFT)
+#define BCM6328_SERIAL_LED_EN		BIT(12)
+#define BCM6328_SERIAL_LED_MUX		BIT(13)
+#define BCM6328_SERIAL_LED_CLK_NPOL	BIT(14)
+#define BCM6328_SERIAL_LED_DATA_PPOL	BIT(15)
+#define BCM6328_SERIAL_LED_SHIFT_DIR	BIT(16)
+#define BCM6328_LED_SHIFT_TEST		BIT(30)
+#define BCM6328_LED_TEST		BIT(31)
+
+#define BCM6328_LED_MODE_MASK		3
+#define BCM6328_LED_MODE_OFF		0
+#define BCM6328_LED_MODE_FAST		1
+#define BCM6328_LED_MODE_BLINK		2
+#define BCM6328_LED_MODE_ON		3
+#define BCM6328_LED_SHIFT(X)		((X) << 1)
+
+/**
+ * struct bcm6328_led - state container for bcm6328 based LEDs
+ * @cdev: LED class device for this LED
+ * @mem: memory resource
+ * @lock: memory lock
+ * @pin: LED pin number
+ * @blink_leds: blinking LEDs
+ * @blink_delay: blinking delay
+ * @active_low: LED is active low
+ */
+struct bcm6328_led {
+	struct led_classdev cdev;
+	void __iomem *mem;
+	spinlock_t *lock;
+	unsigned long pin;
+	unsigned long *blink_leds;
+	unsigned long *blink_delay;
+	bool active_low;
+};
+
+static void bcm6328_led_write(void __iomem *reg, unsigned long data)
+{
+	iowrite32be(data, reg);
+}
+
+static unsigned long bcm6328_led_read(void __iomem *reg)
+{
+	return ioread32be(reg);
+}
+
+/**
+ * LEDMode 64 bits / 24 LEDs
+ * bits [31:0] -> LEDs 8-23
+ * bits [47:32] -> LEDs 0-7
+ * bits [63:48] -> unused
+ */
+static unsigned long bcm6328_pin2shift(unsigned long pin)
+{
+	if (pin < 8)
+		return pin + 16; /* LEDs 0-7 (bits 47:32) */
+	else
+		return pin - 8; /* LEDs 8-23 (bits 31:0) */
+}
+
+static void bcm6328_led_mode(struct bcm6328_led *led, unsigned long value)
+{
+	void __iomem *mode;
+	unsigned long val, shift;
+
+	shift = bcm6328_pin2shift(led->pin);
+	if (shift / 16)
+		mode = led->mem + BCM6328_REG_MODE_HI;
+	else
+		mode = led->mem + BCM6328_REG_MODE_LO;
+
+	val = bcm6328_led_read(mode);
+	val &= ~(BCM6328_LED_MODE_MASK << BCM6328_LED_SHIFT(shift % 16));
+	val |= (value << BCM6328_LED_SHIFT(shift % 16));
+	bcm6328_led_write(mode, val);
+}
+
+static void bcm6328_led_set(struct led_classdev *led_cdev,
+			    enum led_brightness value)
+{
+	struct bcm6328_led *led =
+		container_of(led_cdev, struct bcm6328_led, cdev);
+	unsigned long flags;
+
+	spin_lock_irqsave(led->lock, flags);
+	*(led->blink_leds) &= ~BIT(led->pin);
+	if ((led->active_low && value == LED_OFF) ||
+	    (!led->active_low && value != LED_OFF))
+		bcm6328_led_mode(led, BCM6328_LED_MODE_OFF);
+	else
+		bcm6328_led_mode(led, BCM6328_LED_MODE_ON);
+	spin_unlock_irqrestore(led->lock, flags);
+}
+
+static int bcm6328_blink_set(struct led_classdev *led_cdev,
+			     unsigned long *delay_on, unsigned long *delay_off)
+{
+	struct bcm6328_led *led =
+		container_of(led_cdev, struct bcm6328_led, cdev);
+	unsigned long delay, flags;
+
+	if (!*delay_on)
+		*delay_on = BCM6328_LED_DEF_DELAY;
+	if (!*delay_off)
+		*delay_off = BCM6328_LED_DEF_DELAY;
+
+	if (*delay_on != *delay_off) {
+		dev_dbg(led_cdev->dev,
+			"fallback to soft blinking (delay_on != delay_off)\n");
+		return -EINVAL;
+	}
+
+	delay = *delay_on / BCM6328_LED_INTERVAL_MS;
+	if (delay == 0)
+		delay = 1;
+	else if (delay > BCM6328_LED_INTV_MASK) {
+		dev_dbg(led_cdev->dev,
+			"fallback to soft blinking (delay > %ums)\n",
+			BCM6328_LED_INTV_MASK * BCM6328_LED_INTERVAL_MS);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(led->lock, flags);
+	if (*(led->blink_leds) == 0 ||
+	    *(led->blink_leds) == BIT(led->pin) ||
+	    *(led->blink_delay) == delay) {
+		unsigned long val;
+
+		*(led->blink_leds) |= BIT(led->pin);
+		*(led->blink_delay) = delay;
+
+		val = bcm6328_led_read(led->mem + BCM6328_REG_INIT);
+		val &= ~BCM6328_LED_FAST_INTV_MASK;
+		val |= (delay << BCM6328_LED_FAST_INTV_SHIFT);
+		bcm6328_led_write(led->mem + BCM6328_REG_INIT, val);
+
+		bcm6328_led_mode(led, BCM6328_LED_MODE_BLINK);
+
+		spin_unlock_irqrestore(led->lock, flags);
+	} else {
+		spin_unlock_irqrestore(led->lock, flags);
+		dev_dbg(led_cdev->dev,
+			"fallback to soft blinking (delay already set)\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int bcm6328_hwled(struct device *dev, struct device_node *nc, u32 reg,
+			 void __iomem *mem, spinlock_t *lock)
+{
+	int i, cnt;
+	unsigned long flags, val;
+
+	spin_lock_irqsave(lock, flags);
+	val = bcm6328_led_read(mem + BCM6328_REG_HWDIS);
+	val &= ~BIT(reg);
+	bcm6328_led_write(mem + BCM6328_REG_HWDIS, val);
+	spin_unlock_irqrestore(lock, flags);
+
+	/* Only LEDs 0-7 can be activity/link controlled */
+	if (reg >= 8)
+		return 0;
+
+	cnt = of_property_count_elems_of_size(nc, "brcm,link-signal-sources",
+					      sizeof(u32));
+	for (i = 0; i < cnt; i++) {
+		u32 sel;
+		void __iomem *addr;
+
+		if (reg < 4)
+			addr = mem + BCM6328_REG_LNKACTSEL_LO;
+		else
+			addr = mem + BCM6328_REG_LNKACTSEL_HI;
+
+		of_property_read_u32_index(nc, "brcm,link-signal-sources", i,
+					   &sel);
+
+		if (reg / 4 != sel / 4) {
+			dev_warn(dev, "invalid link signal source\n");
+			continue;
+		}
+
+		spin_lock_irqsave(lock, flags);
+		val = bcm6328_led_read(addr);
+		val |= (BIT(reg) << (((sel % 4) * 4) + 16));
+		bcm6328_led_write(addr, val);
+		spin_unlock_irqrestore(lock, flags);
+	}
+
+	cnt = of_property_count_elems_of_size(nc,
+					      "brcm,activity-signal-sources",
+					      sizeof(u32));
+	for (i = 0; i < cnt; i++) {
+		u32 sel;
+		void __iomem *addr;
+
+		if (reg < 4)
+			addr = mem + BCM6328_REG_LNKACTSEL_LO;
+		else
+			addr = mem + BCM6328_REG_LNKACTSEL_HI;
+
+		of_property_read_u32_index(nc, "brcm,activity-signal-sources",
+					   i, &sel);
+
+		if (reg / 4 != sel / 4) {
+			dev_warn(dev, "invalid activity signal source\n");
+			continue;
+		}
+
+		spin_lock_irqsave(lock, flags);
+		val = bcm6328_led_read(addr);
+		val |= (BIT(reg) << ((sel % 4) * 4));
+		bcm6328_led_write(addr, val);
+		spin_unlock_irqrestore(lock, flags);
+	}
+
+	return 0;
+}
+
+static int bcm6328_led(struct device *dev, struct device_node *nc, u32 reg,
+		       void __iomem *mem, spinlock_t *lock,
+		       unsigned long *blink_leds, unsigned long *blink_delay)
+{
+	struct bcm6328_led *led;
+	unsigned long flags;
+	const char *state;
+	int rc;
+
+	led = devm_kzalloc(dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->pin = reg;
+	led->mem = mem;
+	led->lock = lock;
+	led->blink_leds = blink_leds;
+	led->blink_delay = blink_delay;
+
+	if (of_property_read_bool(nc, "active-low"))
+		led->active_low = true;
+
+	led->cdev.name = of_get_property(nc, "label", NULL) ? : nc->name;
+	led->cdev.default_trigger = of_get_property(nc,
+						    "linux,default-trigger",
+						    NULL);
+
+	if (!of_property_read_string(nc, "default-state", &state)) {
+		spin_lock_irqsave(lock, flags);
+		if (!strcmp(state, "on")) {
+			led->cdev.brightness = LED_FULL;
+			bcm6328_led_mode(led, BCM6328_LED_MODE_ON);
+		} else if (!strcmp(state, "keep")) {
+			void __iomem *mode;
+			unsigned long val, shift;
+
+			shift = bcm6328_pin2shift(led->pin);
+			if (shift / 16)
+				mode = mem + BCM6328_REG_MODE_HI;
+			else
+				mode = mem + BCM6328_REG_MODE_LO;
+
+			val = bcm6328_led_read(mode) >> (shift % 16);
+			val &= BCM6328_LED_MODE_MASK;
+			if (val == BCM6328_LED_MODE_ON)
+				led->cdev.brightness = LED_FULL;
+			else {
+				led->cdev.brightness = LED_OFF;
+				bcm6328_led_mode(led, BCM6328_LED_MODE_OFF);
+			}
+		} else {
+			led->cdev.brightness = LED_OFF;
+			bcm6328_led_mode(led, BCM6328_LED_MODE_OFF);
+		}
+		spin_unlock_irqrestore(lock, flags);
+	}
+
+	led->cdev.brightness_set = bcm6328_led_set;
+	led->cdev.blink_set = bcm6328_blink_set;
+
+	rc = led_classdev_register(dev, &led->cdev);
+	if (rc < 0)
+		return rc;
+
+	dev_dbg(dev, "registered LED %s\n", led->cdev.name);
+
+	return 0;
+}
+
+static int bcm6328_leds_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *child;
+	struct resource *mem_r;
+	void __iomem *mem;
+	spinlock_t *lock;
+	unsigned long val, *blink_leds, *blink_delay;
+
+	mem_r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem_r)
+		return -EINVAL;
+
+	mem = devm_ioremap_resource(dev, mem_r);
+	if (IS_ERR(mem))
+		return PTR_ERR(mem);
+
+	lock = devm_kzalloc(dev, sizeof(*lock), GFP_KERNEL);
+	if (!lock)
+		return -ENOMEM;
+
+	blink_leds = devm_kzalloc(dev, sizeof(*blink_leds), GFP_KERNEL);
+	if (!blink_leds)
+		return -ENOMEM;
+
+	blink_delay = devm_kzalloc(dev, sizeof(*blink_delay), GFP_KERNEL);
+	if (!blink_delay)
+		return -ENOMEM;
+
+	spin_lock_init(lock);
+
+	bcm6328_led_write(mem + BCM6328_REG_HWDIS, ~0);
+	bcm6328_led_write(mem + BCM6328_REG_LNKACTSEL_HI, 0);
+	bcm6328_led_write(mem + BCM6328_REG_LNKACTSEL_LO, 0);
+
+	val = bcm6328_led_read(mem + BCM6328_REG_INIT);
+	val &= ~BCM6328_SERIAL_LED_EN;
+	if (of_property_read_bool(np, "brcm,serial-leds"))
+		val |= BCM6328_SERIAL_LED_EN;
+	bcm6328_led_write(mem + BCM6328_REG_INIT, val);
+
+	for_each_available_child_of_node(np, child) {
+		int rc;
+		u32 reg;
+
+		if (of_property_read_u32(child, "reg", &reg))
+			continue;
+
+		if (reg >= BCM6328_LED_MAX_COUNT) {
+			dev_err(dev, "invalid LED (>= %d)\n",
+				BCM6328_LED_MAX_COUNT);
+			continue;
+		}
+
+		if (of_property_read_bool(child, "brcm,hardware-controlled"))
+			rc = bcm6328_hwled(dev, child, reg, mem, lock);
+		else
+			rc = bcm6328_led(dev, child, reg, mem, lock,
+					 blink_leds, blink_delay);
+
+		if (rc < 0)
+			return rc;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id bcm6328_leds_of_match[] = {
+	{ .compatible = "brcm,bcm6328-leds", },
+	{ },
+};
+
+static struct platform_driver bcm6328_leds_driver = {
+	.probe = bcm6328_leds_probe,
+	.driver = {
+		.name = "leds-bcm6328",
+		.of_match_table = bcm6328_leds_of_match,
+	},
+};
+
+module_platform_driver(bcm6328_leds_driver);
+
+MODULE_AUTHOR("Álvaro Fernández Rojas <noltari@gmail.com>");
+MODULE_AUTHOR("Jonas Gorski <jogo@openwrt.org>");
+MODULE_DESCRIPTION("LED driver for BCM6328 controllers");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:leds-bcm6328");
diff --git a/drivers/leds/leds-bcm6358.c b/drivers/leds/leds-bcm6358.c
new file mode 100644
index 000000000000..21f96930b3be
--- /dev/null
+++ b/drivers/leds/leds-bcm6358.c
@@ -0,0 +1,243 @@
+/*
+ * Driver for BCM6358 memory-mapped LEDs, based on leds-syscon.c
+ *
+ * Copyright 2015 Álvaro Fernández Rojas <noltari@gmail.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+
+#define BCM6358_REG_MODE		0x0
+#define BCM6358_REG_CTRL		0x4
+
+#define BCM6358_SLED_CLKDIV_MASK	3
+#define BCM6358_SLED_CLKDIV_1		0
+#define BCM6358_SLED_CLKDIV_2		1
+#define BCM6358_SLED_CLKDIV_4		2
+#define BCM6358_SLED_CLKDIV_8		3
+
+#define BCM6358_SLED_POLARITY		BIT(2)
+#define BCM6358_SLED_BUSY		BIT(3)
+
+#define BCM6358_SLED_MAX_COUNT		32
+#define BCM6358_SLED_WAIT		100
+
+/**
+ * struct bcm6358_led - state container for bcm6358 based LEDs
+ * @cdev: LED class device for this LED
+ * @mem: memory resource
+ * @lock: memory lock
+ * @pin: LED pin number
+ * @active_low: LED is active low
+ */
+struct bcm6358_led {
+	struct led_classdev cdev;
+	void __iomem *mem;
+	spinlock_t *lock;
+	unsigned long pin;
+	bool active_low;
+};
+
+static void bcm6358_led_write(void __iomem *reg, unsigned long data)
+{
+	iowrite32be(data, reg);
+}
+
+static unsigned long bcm6358_led_read(void __iomem *reg)
+{
+	return ioread32be(reg);
+}
+
+static unsigned long bcm6358_led_busy(void __iomem *mem)
+{
+	unsigned long val;
+
+	while ((val = bcm6358_led_read(mem + BCM6358_REG_CTRL)) &
+		BCM6358_SLED_BUSY)
+		udelay(BCM6358_SLED_WAIT);
+
+	return val;
+}
+
+static void bcm6358_led_mode(struct bcm6358_led *led, unsigned long value)
+{
+	unsigned long val;
+
+	bcm6358_led_busy(led->mem);
+
+	val = bcm6358_led_read(led->mem + BCM6358_REG_MODE);
+	if ((led->active_low && value == LED_OFF) ||
+	    (!led->active_low && value != LED_OFF))
+		val |= BIT(led->pin);
+	else
+		val &= ~(BIT(led->pin));
+	bcm6358_led_write(led->mem + BCM6358_REG_MODE, val);
+}
+
+static void bcm6358_led_set(struct led_classdev *led_cdev,
+			    enum led_brightness value)
+{
+	struct bcm6358_led *led =
+		container_of(led_cdev, struct bcm6358_led, cdev);
+	unsigned long flags;
+
+	spin_lock_irqsave(led->lock, flags);
+	bcm6358_led_mode(led, value);
+	spin_unlock_irqrestore(led->lock, flags);
+}
+
+static int bcm6358_led(struct device *dev, struct device_node *nc, u32 reg,
+		       void __iomem *mem, spinlock_t *lock)
+{
+	struct bcm6358_led *led;
+	unsigned long flags;
+	const char *state;
+	int rc;
+
+	led = devm_kzalloc(dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->pin = reg;
+	led->mem = mem;
+	led->lock = lock;
+
+	if (of_property_read_bool(nc, "active-low"))
+		led->active_low = true;
+
+	led->cdev.name = of_get_property(nc, "label", NULL) ? : nc->name;
+	led->cdev.default_trigger = of_get_property(nc,
+						    "linux,default-trigger",
+						    NULL);
+
+	spin_lock_irqsave(lock, flags);
+	if (!of_property_read_string(nc, "default-state", &state)) {
+		if (!strcmp(state, "on")) {
+			led->cdev.brightness = LED_FULL;
+		} else if (!strcmp(state, "keep")) {
+			unsigned long val;
+
+			bcm6358_led_busy(led->mem);
+
+			val = bcm6358_led_read(led->mem + BCM6358_REG_MODE);
+			val &= BIT(led->pin);
+			if ((led->active_low && !val) ||
+			    (!led->active_low && val))
+				led->cdev.brightness = LED_FULL;
+			else
+				led->cdev.brightness = LED_OFF;
+		} else {
+			led->cdev.brightness = LED_OFF;
+		}
+	} else {
+		led->cdev.brightness = LED_OFF;
+	}
+	bcm6358_led_mode(led, led->cdev.brightness);
+	spin_unlock_irqrestore(lock, flags);
+
+	led->cdev.brightness_set = bcm6358_led_set;
+
+	rc = led_classdev_register(dev, &led->cdev);
+	if (rc < 0)
+		return rc;
+
+	dev_dbg(dev, "registered LED %s\n", led->cdev.name);
+
+	return 0;
+}
+
+static int bcm6358_leds_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *child;
+	struct resource *mem_r;
+	void __iomem *mem;
+	spinlock_t *lock; /* memory lock */
+	unsigned long val;
+	u32 clk_div;
+
+	mem_r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!mem_r)
+		return -EINVAL;
+
+	mem = devm_ioremap_resource(dev, mem_r);
+	if (IS_ERR(mem))
+		return PTR_ERR(mem);
+
+	lock = devm_kzalloc(dev, sizeof(*lock), GFP_KERNEL);
+	if (!lock)
+		return -ENOMEM;
+
+	spin_lock_init(lock);
+
+	val = bcm6358_led_busy(mem);
+	val &= ~(BCM6358_SLED_POLARITY | BCM6358_SLED_CLKDIV_MASK);
+	if (of_property_read_bool(np, "brcm,clk-dat-low"))
+		val |= BCM6358_SLED_POLARITY;
+	of_property_read_u32(np, "brcm,clk-div", &clk_div);
+	switch (clk_div) {
+	case 8:
+		val |= BCM6358_SLED_CLKDIV_8;
+		break;
+	case 4:
+		val |= BCM6358_SLED_CLKDIV_4;
+		break;
+	case 2:
+		val |= BCM6358_SLED_CLKDIV_2;
+		break;
+	default:
+		val |= BCM6358_SLED_CLKDIV_1;
+		break;
+	}
+	bcm6358_led_write(mem + BCM6358_REG_CTRL, val);
+
+	for_each_available_child_of_node(np, child) {
+		int rc;
+		u32 reg;
+
+		if (of_property_read_u32(child, "reg", &reg))
+			continue;
+
+		if (reg >= BCM6358_SLED_MAX_COUNT) {
+			dev_err(dev, "invalid LED (%u >= %d)\n", reg,
+				BCM6358_SLED_MAX_COUNT);
+			continue;
+		}
+
+		rc = bcm6358_led(dev, child, reg, mem, lock);
+		if (rc < 0)
+			return rc;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id bcm6358_leds_of_match[] = {
+	{ .compatible = "brcm,bcm6358-leds", },
+	{ },
+};
+
+static struct platform_driver bcm6358_leds_driver = {
+	.probe = bcm6358_leds_probe,
+	.driver = {
+		.name = "leds-bcm6358",
+		.of_match_table = bcm6358_leds_of_match,
+	},
+};
+
+module_platform_driver(bcm6358_leds_driver);
+
+MODULE_AUTHOR("Álvaro Fernández Rojas <noltari@gmail.com>");
+MODULE_DESCRIPTION("LED driver for BCM6358 controllers");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:leds-bcm6358");
diff --git a/drivers/leds/leds-cobalt-raq.c b/drivers/leds/leds-cobalt-raq.c
index 06dbe18a2065..b316df4a8c1e 100644
--- a/drivers/leds/leds-cobalt-raq.c
+++ b/drivers/leds/leds-cobalt-raq.c
@@ -108,20 +108,8 @@ err_null:
 	return retval;
 }
 
-static int cobalt_raq_led_remove(struct platform_device *pdev)
-{
-	led_classdev_unregister(&raq_power_off_led);
-	led_classdev_unregister(&raq_web_led);
-
-	if (led_port)
-		led_port = NULL;
-
-	return 0;
-}
-
 static struct platform_driver cobalt_raq_led_driver = {
 	.probe	= cobalt_raq_led_probe,
-	.remove	= cobalt_raq_led_remove,
 	.driver = {
 		.name	= "cobalt-raq-leds",
 	},
@@ -131,5 +119,4 @@ static int __init cobalt_raq_led_init(void)
 {
 	return platform_driver_register(&cobalt_raq_led_driver);
 }
-
-module_init(cobalt_raq_led_init);
+device_initcall(cobalt_raq_led_init);
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index 15eb3f86f670..af1876a3a77c 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -16,6 +16,7 @@
 #include <linux/kernel.h>
 #include <linux/leds.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/property.h>
 #include <linux/slab.h>
@@ -191,15 +192,17 @@ static struct gpio_leds_priv *gpio_leds_create(struct platform_device *pdev)
 			goto err;
 		}
 
-		np = of_node(child);
+		np = to_of_node(child);
 
 		if (fwnode_property_present(child, "label")) {
 			fwnode_property_read_string(child, "label", &led.name);
 		} else {
 			if (IS_ENABLED(CONFIG_OF) && !led.name && np)
 				led.name = np->name;
-			if (!led.name)
-				return ERR_PTR(-EINVAL);
+			if (!led.name) {
+				ret = -EINVAL;
+				goto err;
+			}
 		}
 		fwnode_property_read_string(child, "linux,default-trigger",
 					    &led.default_trigger);
@@ -217,18 +220,19 @@ static struct gpio_leds_priv *gpio_leds_create(struct platform_device *pdev)
 		if (fwnode_property_present(child, "retain-state-suspended"))
 			led.retain_state_suspended = 1;
 
-		ret = create_gpio_led(&led, &priv->leds[priv->num_leds++],
+		ret = create_gpio_led(&led, &priv->leds[priv->num_leds],
 				      dev, NULL);
 		if (ret < 0) {
 			fwnode_handle_put(child);
 			goto err;
 		}
+		priv->num_leds++;
 	}
 
 	return priv;
 
 err:
-	for (count = priv->num_leds - 2; count >= 0; count--)
+	for (count = priv->num_leds - 1; count >= 0; count--)
 		delete_gpio_led(&priv->leds[count]);
 	return ERR_PTR(ret);
 }
diff --git a/drivers/leds/leds-ktd2692.c b/drivers/leds/leds-ktd2692.c
new file mode 100644
index 000000000000..2ae8c4d17ff8
--- /dev/null
+++ b/drivers/leds/leds-ktd2692.c
@@ -0,0 +1,443 @@
+/*
+ * LED driver : leds-ktd2692.c
+ *
+ * Copyright (C) 2015 Samsung Electronics
+ * Ingi Kim <ingi2.kim@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio/consumer.h>
+#include <linux/led-class-flash.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/workqueue.h>
+
+/* Value related the movie mode */
+#define KTD2692_MOVIE_MODE_CURRENT_LEVELS	16
+#define KTD2692_MM_TO_FL_RATIO(x)		((x) / 3)
+#define KTD2962_MM_MIN_CURR_THRESHOLD_SCALE	8
+
+/* Value related the flash mode */
+#define KTD2692_FLASH_MODE_TIMEOUT_LEVELS	8
+#define KTD2692_FLASH_MODE_TIMEOUT_DISABLE	0
+#define KTD2692_FLASH_MODE_CURR_PERCENT(x)	(((x) * 16) / 100)
+
+/* Macro for getting offset of flash timeout */
+#define GET_TIMEOUT_OFFSET(timeout, step)	((timeout) / (step))
+
+/* Base register address */
+#define KTD2692_REG_LVP_BASE			0x00
+#define KTD2692_REG_FLASH_TIMEOUT_BASE		0x20
+#define KTD2692_REG_MM_MIN_CURR_THRESHOLD_BASE	0x40
+#define KTD2692_REG_MOVIE_CURRENT_BASE		0x60
+#define KTD2692_REG_FLASH_CURRENT_BASE		0x80
+#define KTD2692_REG_MODE_BASE			0xA0
+
+/* Set bit coding time for expresswire interface */
+#define KTD2692_TIME_RESET_US			700
+#define KTD2692_TIME_DATA_START_TIME_US		10
+#define KTD2692_TIME_HIGH_END_OF_DATA_US	350
+#define KTD2692_TIME_LOW_END_OF_DATA_US		10
+#define KTD2692_TIME_SHORT_BITSET_US		4
+#define KTD2692_TIME_LONG_BITSET_US		12
+
+/* KTD2692 default length of name */
+#define KTD2692_NAME_LENGTH			20
+
+enum ktd2692_bitset {
+	KTD2692_LOW = 0,
+	KTD2692_HIGH,
+};
+
+/* Movie / Flash Mode Control */
+enum ktd2692_led_mode {
+	KTD2692_MODE_DISABLE = 0,	/* default */
+	KTD2692_MODE_MOVIE,
+	KTD2692_MODE_FLASH,
+};
+
+struct ktd2692_led_config_data {
+	/* maximum LED current in movie mode */
+	u32 movie_max_microamp;
+	/* maximum LED current in flash mode */
+	u32 flash_max_microamp;
+	/* maximum flash timeout */
+	u32 flash_max_timeout;
+	/* max LED brightness level */
+	enum led_brightness max_brightness;
+};
+
+struct ktd2692_context {
+	/* Related LED Flash class device */
+	struct led_classdev_flash fled_cdev;
+
+	/* secures access to the device */
+	struct mutex lock;
+	struct regulator *regulator;
+	struct work_struct work_brightness_set;
+
+	struct gpio_desc *aux_gpio;
+	struct gpio_desc *ctrl_gpio;
+
+	enum ktd2692_led_mode mode;
+	enum led_brightness torch_brightness;
+};
+
+static struct ktd2692_context *fled_cdev_to_led(
+				struct led_classdev_flash *fled_cdev)
+{
+	return container_of(fled_cdev, struct ktd2692_context, fled_cdev);
+}
+
+static void ktd2692_expresswire_start(struct ktd2692_context *led)
+{
+	gpiod_direction_output(led->ctrl_gpio, KTD2692_HIGH);
+	udelay(KTD2692_TIME_DATA_START_TIME_US);
+}
+
+static void ktd2692_expresswire_reset(struct ktd2692_context *led)
+{
+	gpiod_direction_output(led->ctrl_gpio, KTD2692_LOW);
+	udelay(KTD2692_TIME_RESET_US);
+}
+
+static void ktd2692_expresswire_end(struct ktd2692_context *led)
+{
+	gpiod_direction_output(led->ctrl_gpio, KTD2692_LOW);
+	udelay(KTD2692_TIME_LOW_END_OF_DATA_US);
+	gpiod_direction_output(led->ctrl_gpio, KTD2692_HIGH);
+	udelay(KTD2692_TIME_HIGH_END_OF_DATA_US);
+}
+
+static void ktd2692_expresswire_set_bit(struct ktd2692_context *led, bool bit)
+{
+	/*
+	 * The Low Bit(0) and High Bit(1) is based on a time detection
+	 * algorithm between time low and time high
+	 * Time_(L_LB) : Low time of the Low Bit(0)
+	 * Time_(H_LB) : High time of the LOW Bit(0)
+	 * Time_(L_HB) : Low time of the High Bit(1)
+	 * Time_(H_HB) : High time of the High Bit(1)
+	 *
+	 * It can be simplified to:
+	 * Low Bit(0) : 2 * Time_(H_LB) < Time_(L_LB)
+	 * High Bit(1) : 2 * Time_(L_HB) < Time_(H_HB)
+	 * HIGH  ___           ____    _..     _________    ___
+	 *          |_________|    |_..  |____|         |__|
+	 * LOW        <L_LB>  <H_LB>     <L_HB>  <H_HB>
+	 *          [  Low Bit (0) ]     [  High Bit(1) ]
+	 */
+	if (bit) {
+		gpiod_direction_output(led->ctrl_gpio, KTD2692_LOW);
+		udelay(KTD2692_TIME_SHORT_BITSET_US);
+		gpiod_direction_output(led->ctrl_gpio, KTD2692_HIGH);
+		udelay(KTD2692_TIME_LONG_BITSET_US);
+	} else {
+		gpiod_direction_output(led->ctrl_gpio, KTD2692_LOW);
+		udelay(KTD2692_TIME_LONG_BITSET_US);
+		gpiod_direction_output(led->ctrl_gpio, KTD2692_HIGH);
+		udelay(KTD2692_TIME_SHORT_BITSET_US);
+	}
+}
+
+static void ktd2692_expresswire_write(struct ktd2692_context *led, u8 value)
+{
+	int i;
+
+	ktd2692_expresswire_start(led);
+	for (i = 7; i >= 0; i--)
+		ktd2692_expresswire_set_bit(led, value & BIT(i));
+	ktd2692_expresswire_end(led);
+}
+
+static void ktd2692_brightness_set(struct ktd2692_context *led,
+				   enum led_brightness brightness)
+{
+	mutex_lock(&led->lock);
+
+	if (brightness == LED_OFF) {
+		led->mode = KTD2692_MODE_DISABLE;
+		gpiod_direction_output(led->aux_gpio, KTD2692_LOW);
+	} else {
+		ktd2692_expresswire_write(led, brightness |
+					KTD2692_REG_MOVIE_CURRENT_BASE);
+		led->mode = KTD2692_MODE_MOVIE;
+	}
+
+	ktd2692_expresswire_write(led, led->mode | KTD2692_REG_MODE_BASE);
+	mutex_unlock(&led->lock);
+}
+
+static void ktd2692_brightness_set_work(struct work_struct *work)
+{
+	struct ktd2692_context *led =
+		container_of(work, struct ktd2692_context, work_brightness_set);
+
+	ktd2692_brightness_set(led, led->torch_brightness);
+}
+
+static void ktd2692_led_brightness_set(struct led_classdev *led_cdev,
+				       enum led_brightness brightness)
+{
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
+	struct ktd2692_context *led = fled_cdev_to_led(fled_cdev);
+
+	led->torch_brightness = brightness;
+	schedule_work(&led->work_brightness_set);
+}
+
+static int ktd2692_led_brightness_set_sync(struct led_classdev *led_cdev,
+					   enum led_brightness brightness)
+{
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
+	struct ktd2692_context *led = fled_cdev_to_led(fled_cdev);
+
+	ktd2692_brightness_set(led, brightness);
+
+	return 0;
+}
+
+static int ktd2692_led_flash_strobe_set(struct led_classdev_flash *fled_cdev,
+					bool state)
+{
+	struct ktd2692_context *led = fled_cdev_to_led(fled_cdev);
+	struct led_flash_setting *timeout = &fled_cdev->timeout;
+	u32 flash_tm_reg;
+
+	mutex_lock(&led->lock);
+
+	if (state) {
+		flash_tm_reg = GET_TIMEOUT_OFFSET(timeout->val, timeout->step);
+		ktd2692_expresswire_write(led, flash_tm_reg
+				| KTD2692_REG_FLASH_TIMEOUT_BASE);
+
+		led->mode = KTD2692_MODE_FLASH;
+		gpiod_direction_output(led->aux_gpio, KTD2692_HIGH);
+	} else {
+		led->mode = KTD2692_MODE_DISABLE;
+		gpiod_direction_output(led->aux_gpio, KTD2692_LOW);
+	}
+
+	ktd2692_expresswire_write(led, led->mode | KTD2692_REG_MODE_BASE);
+
+	fled_cdev->led_cdev.brightness = LED_OFF;
+	led->mode = KTD2692_MODE_DISABLE;
+
+	mutex_unlock(&led->lock);
+
+	return 0;
+}
+
+static int ktd2692_led_flash_timeout_set(struct led_classdev_flash *fled_cdev,
+					 u32 timeout)
+{
+	return 0;
+}
+
+static void ktd2692_init_movie_current_max(struct ktd2692_led_config_data *cfg)
+{
+	u32 offset, step;
+	u32 movie_current_microamp;
+
+	offset = KTD2692_MOVIE_MODE_CURRENT_LEVELS;
+	step = KTD2692_MM_TO_FL_RATIO(cfg->flash_max_microamp)
+		/ KTD2692_MOVIE_MODE_CURRENT_LEVELS;
+
+	do {
+		movie_current_microamp = step * offset;
+		offset--;
+	} while ((movie_current_microamp > cfg->movie_max_microamp) &&
+		(offset > 0));
+
+	cfg->max_brightness = offset;
+}
+
+static void ktd2692_init_flash_timeout(struct led_classdev_flash *fled_cdev,
+				       struct ktd2692_led_config_data *cfg)
+{
+	struct led_flash_setting *setting;
+
+	setting = &fled_cdev->timeout;
+	setting->min = KTD2692_FLASH_MODE_TIMEOUT_DISABLE;
+	setting->max = cfg->flash_max_timeout;
+	setting->step = cfg->flash_max_timeout
+			/ (KTD2692_FLASH_MODE_TIMEOUT_LEVELS - 1);
+	setting->val = cfg->flash_max_timeout;
+}
+
+static void ktd2692_setup(struct ktd2692_context *led)
+{
+	led->mode = KTD2692_MODE_DISABLE;
+	ktd2692_expresswire_reset(led);
+	gpiod_direction_output(led->aux_gpio, KTD2692_LOW);
+
+	ktd2692_expresswire_write(led, (KTD2962_MM_MIN_CURR_THRESHOLD_SCALE - 1)
+				 | KTD2692_REG_MM_MIN_CURR_THRESHOLD_BASE);
+	ktd2692_expresswire_write(led, KTD2692_FLASH_MODE_CURR_PERCENT(45)
+				 | KTD2692_REG_FLASH_CURRENT_BASE);
+}
+
+static int ktd2692_parse_dt(struct ktd2692_context *led, struct device *dev,
+			    struct ktd2692_led_config_data *cfg)
+{
+	struct device_node *np = dev->of_node;
+	struct device_node *child_node;
+	int ret;
+
+	if (!dev->of_node)
+		return -ENXIO;
+
+	led->ctrl_gpio = devm_gpiod_get(dev, "ctrl", GPIOD_ASIS);
+	if (IS_ERR(led->ctrl_gpio)) {
+		ret = PTR_ERR(led->ctrl_gpio);
+		dev_err(dev, "cannot get ctrl-gpios %d\n", ret);
+		return ret;
+	}
+
+	led->aux_gpio = devm_gpiod_get(dev, "aux", GPIOD_ASIS);
+	if (IS_ERR(led->aux_gpio)) {
+		ret = PTR_ERR(led->aux_gpio);
+		dev_err(dev, "cannot get aux-gpios %d\n", ret);
+		return ret;
+	}
+
+	led->regulator = devm_regulator_get(dev, "vin");
+	if (IS_ERR(led->regulator))
+		led->regulator = NULL;
+
+	if (led->regulator) {
+		ret = regulator_enable(led->regulator);
+		if (ret)
+			dev_err(dev, "Failed to enable supply: %d\n", ret);
+	}
+
+	child_node = of_get_next_available_child(np, NULL);
+	if (!child_node) {
+		dev_err(dev, "No DT child node found for connected LED.\n");
+		return -EINVAL;
+	}
+
+	led->fled_cdev.led_cdev.name =
+		of_get_property(child_node, "label", NULL) ? : child_node->name;
+
+	ret = of_property_read_u32(child_node, "led-max-microamp",
+				   &cfg->movie_max_microamp);
+	if (ret) {
+		dev_err(dev, "failed to parse led-max-microamp\n");
+		return ret;
+	}
+
+	ret = of_property_read_u32(child_node, "flash-max-microamp",
+				   &cfg->flash_max_microamp);
+	if (ret) {
+		dev_err(dev, "failed to parse flash-max-microamp\n");
+		return ret;
+	}
+
+	ret = of_property_read_u32(child_node, "flash-max-timeout-us",
+				   &cfg->flash_max_timeout);
+	if (ret)
+		dev_err(dev, "failed to parse flash-max-timeout-us\n");
+
+	of_node_put(child_node);
+	return ret;
+}
+
+static const struct led_flash_ops flash_ops = {
+	.strobe_set = ktd2692_led_flash_strobe_set,
+	.timeout_set = ktd2692_led_flash_timeout_set,
+};
+
+static int ktd2692_probe(struct platform_device *pdev)
+{
+	struct ktd2692_context *led;
+	struct led_classdev *led_cdev;
+	struct led_classdev_flash *fled_cdev;
+	struct ktd2692_led_config_data led_cfg;
+	int ret;
+
+	led = devm_kzalloc(&pdev->dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	fled_cdev = &led->fled_cdev;
+	led_cdev = &fled_cdev->led_cdev;
+
+	ret = ktd2692_parse_dt(led, &pdev->dev, &led_cfg);
+	if (ret)
+		return ret;
+
+	ktd2692_init_flash_timeout(fled_cdev, &led_cfg);
+	ktd2692_init_movie_current_max(&led_cfg);
+
+	fled_cdev->ops = &flash_ops;
+
+	led_cdev->max_brightness = led_cfg.max_brightness;
+	led_cdev->brightness_set = ktd2692_led_brightness_set;
+	led_cdev->brightness_set_sync = ktd2692_led_brightness_set_sync;
+	led_cdev->flags |= LED_CORE_SUSPENDRESUME | LED_DEV_CAP_FLASH;
+
+	mutex_init(&led->lock);
+	INIT_WORK(&led->work_brightness_set, ktd2692_brightness_set_work);
+
+	platform_set_drvdata(pdev, led);
+
+	ret = led_classdev_flash_register(&pdev->dev, fled_cdev);
+	if (ret) {
+		dev_err(&pdev->dev, "can't register LED %s\n", led_cdev->name);
+		mutex_destroy(&led->lock);
+		return ret;
+	}
+
+	ktd2692_setup(led);
+
+	return 0;
+}
+
+static int ktd2692_remove(struct platform_device *pdev)
+{
+	struct ktd2692_context *led = platform_get_drvdata(pdev);
+	int ret;
+
+	led_classdev_flash_unregister(&led->fled_cdev);
+	cancel_work_sync(&led->work_brightness_set);
+
+	if (led->regulator) {
+		ret = regulator_disable(led->regulator);
+		if (ret)
+			dev_err(&pdev->dev,
+				"Failed to disable supply: %d\n", ret);
+	}
+
+	mutex_destroy(&led->lock);
+
+	return 0;
+}
+
+static const struct of_device_id ktd2692_match[] = {
+	{ .compatible = "kinetic,ktd2692", },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver ktd2692_driver = {
+	.driver = {
+		.name  = "ktd2692",
+		.of_match_table = ktd2692_match,
+	},
+	.probe  = ktd2692_probe,
+	.remove = ktd2692_remove,
+};
+
+module_platform_driver(ktd2692_driver);
+
+MODULE_AUTHOR("Ingi Kim <ingi2.kim@samsung.com>");
+MODULE_DESCRIPTION("Kinetic KTD2692 LED driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-lp5523.c b/drivers/leds/leds-lp5523.c
index 9e1716f8098c..584dbbcec659 100644
--- a/drivers/leds/leds-lp5523.c
+++ b/drivers/leds/leds-lp5523.c
@@ -50,6 +50,7 @@
 #define LP5523_REG_OP_MODE		0x01
 #define LP5523_REG_ENABLE_LEDS_MSB	0x04
 #define LP5523_REG_ENABLE_LEDS_LSB	0x05
+#define LP5523_REG_LED_CTRL_BASE	0x06
 #define LP5523_REG_LED_PWM_BASE		0x16
 #define LP5523_REG_LED_CURRENT_BASE	0x26
 #define LP5523_REG_CONFIG		0x36
@@ -57,6 +58,7 @@
 #define LP5523_REG_RESET		0x3D
 #define LP5523_REG_LED_TEST_CTRL	0x41
 #define LP5523_REG_LED_TEST_ADC		0x42
+#define LP5523_REG_MASTER_FADER_BASE	0x48
 #define LP5523_REG_CH1_PROG_START	0x4C
 #define LP5523_REG_CH2_PROG_START	0x4D
 #define LP5523_REG_CH3_PROG_START	0x4E
@@ -78,6 +80,9 @@
 #define LP5523_EXT_CLK_USED		0x08
 #define LP5523_ENG_STATUS_MASK		0x07
 
+#define LP5523_FADER_MAPPING_MASK	0xC0
+#define LP5523_FADER_MAPPING_SHIFT	6
+
 /* Memory Page Selection */
 #define LP5523_PAGE_ENG1		0
 #define LP5523_PAGE_ENG2		1
@@ -666,6 +671,137 @@ release_lock:
 	return pos;
 }
 
+#define show_fader(nr)						\
+static ssize_t show_master_fader##nr(struct device *dev,	\
+			    struct device_attribute *attr,	\
+			    char *buf)				\
+{								\
+	return show_master_fader(dev, attr, buf, nr);		\
+}
+
+#define store_fader(nr)						\
+static ssize_t store_master_fader##nr(struct device *dev,	\
+			     struct device_attribute *attr,	\
+			     const char *buf, size_t len)	\
+{								\
+	return store_master_fader(dev, attr, buf, len, nr);	\
+}
+
+static ssize_t show_master_fader(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf, int nr)
+{
+	struct lp55xx_led *led = i2c_get_clientdata(to_i2c_client(dev));
+	struct lp55xx_chip *chip = led->chip;
+	int ret;
+	u8 val;
+
+	mutex_lock(&chip->lock);
+	ret = lp55xx_read(chip, LP5523_REG_MASTER_FADER_BASE + nr - 1, &val);
+	mutex_unlock(&chip->lock);
+
+	if (ret == 0)
+		ret = sprintf(buf, "%u\n", val);
+
+	return ret;
+}
+show_fader(1)
+show_fader(2)
+show_fader(3)
+
+static ssize_t store_master_fader(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t len, int nr)
+{
+	struct lp55xx_led *led = i2c_get_clientdata(to_i2c_client(dev));
+	struct lp55xx_chip *chip = led->chip;
+	int ret;
+	unsigned long val;
+
+	if (kstrtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if (val > 0xff)
+		return -EINVAL;
+
+	mutex_lock(&chip->lock);
+	ret = lp55xx_write(chip, LP5523_REG_MASTER_FADER_BASE + nr - 1,
+			   (u8)val);
+	mutex_unlock(&chip->lock);
+
+	if (ret == 0)
+		ret = len;
+
+	return ret;
+}
+store_fader(1)
+store_fader(2)
+store_fader(3)
+
+static ssize_t show_master_fader_leds(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct lp55xx_led *led = i2c_get_clientdata(to_i2c_client(dev));
+	struct lp55xx_chip *chip = led->chip;
+	int i, ret, pos = 0;
+	u8 val;
+
+	mutex_lock(&chip->lock);
+
+	for (i = 0; i < LP5523_MAX_LEDS; i++) {
+		ret = lp55xx_read(chip, LP5523_REG_LED_CTRL_BASE + i, &val);
+		if (ret)
+			goto leave;
+
+		val = (val & LP5523_FADER_MAPPING_MASK)
+			>> LP5523_FADER_MAPPING_SHIFT;
+		if (val > 3) {
+			ret = -EINVAL;
+			goto leave;
+		}
+		buf[pos++] = val + '0';
+	}
+	buf[pos++] = '\n';
+	ret = pos;
+leave:
+	mutex_unlock(&chip->lock);
+	return ret;
+}
+
+static ssize_t store_master_fader_leds(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf, size_t len)
+{
+	struct lp55xx_led *led = i2c_get_clientdata(to_i2c_client(dev));
+	struct lp55xx_chip *chip = led->chip;
+	int i, n, ret;
+	u8 val;
+
+	n = min_t(int, len, LP5523_MAX_LEDS);
+
+	mutex_lock(&chip->lock);
+
+	for (i = 0; i < n; i++) {
+		if (buf[i] >= '0' && buf[i] <= '3') {
+			val = (buf[i] - '0') << LP5523_FADER_MAPPING_SHIFT;
+			ret = lp55xx_update_bits(chip,
+						 LP5523_REG_LED_CTRL_BASE + i,
+						 LP5523_FADER_MAPPING_MASK,
+						 val);
+			if (ret)
+				goto leave;
+		} else {
+			ret = -EINVAL;
+			goto leave;
+		}
+	}
+	ret = len;
+leave:
+	mutex_unlock(&chip->lock);
+	return ret;
+}
+
 static void lp5523_led_brightness_work(struct work_struct *work)
 {
 	struct lp55xx_led *led = container_of(work, struct lp55xx_led,
@@ -688,6 +824,14 @@ static LP55XX_DEV_ATTR_WO(engine1_load, store_engine1_load);
 static LP55XX_DEV_ATTR_WO(engine2_load, store_engine2_load);
 static LP55XX_DEV_ATTR_WO(engine3_load, store_engine3_load);
 static LP55XX_DEV_ATTR_RO(selftest, lp5523_selftest);
+static LP55XX_DEV_ATTR_RW(master_fader1, show_master_fader1,
+			  store_master_fader1);
+static LP55XX_DEV_ATTR_RW(master_fader2, show_master_fader2,
+			  store_master_fader2);
+static LP55XX_DEV_ATTR_RW(master_fader3, show_master_fader3,
+			  store_master_fader3);
+static LP55XX_DEV_ATTR_RW(master_fader_leds, show_master_fader_leds,
+			  store_master_fader_leds);
 
 static struct attribute *lp5523_attributes[] = {
 	&dev_attr_engine1_mode.attr,
@@ -700,6 +844,10 @@ static struct attribute *lp5523_attributes[] = {
 	&dev_attr_engine2_leds.attr,
 	&dev_attr_engine3_leds.attr,
 	&dev_attr_selftest.attr,
+	&dev_attr_master_fader1.attr,
+	&dev_attr_master_fader2.attr,
+	&dev_attr_master_fader3.attr,
+	&dev_attr_master_fader_leds.attr,
 	NULL,
 };
 
diff --git a/drivers/leds/leds-lp55xx-common.c b/drivers/leds/leds-lp55xx-common.c
index 77c26bc32eed..96d51e9879c9 100644
--- a/drivers/leds/leds-lp55xx-common.c
+++ b/drivers/leds/leds-lp55xx-common.c
@@ -223,7 +223,7 @@ static int lp55xx_request_firmware(struct lp55xx_chip *chip)
 	const char *name = chip->cl->name;
 	struct device *dev = &chip->cl->dev;
 
-	return request_firmware_nowait(THIS_MODULE, true, name, dev,
+	return request_firmware_nowait(THIS_MODULE, false, name, dev,
 				GFP_KERNEL, chip, lp55xx_firmware_loaded);
 }
 
diff --git a/drivers/leds/leds-max77693.c b/drivers/leds/leds-max77693.c
new file mode 100644
index 000000000000..b8b0eec7b540
--- /dev/null
+++ b/drivers/leds/leds-max77693.c
@@ -0,0 +1,1097 @@
+/*
+ * LED Flash class driver for the flash cell of max77693 mfd.
+ *
+ *	Copyright (C) 2015, Samsung Electronics Co., Ltd.
+ *
+ *	Authors: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *		 Andrzej Hajda <a.hajda@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ */
+
+#include <linux/led-class-flash.h>
+#include <linux/mfd/max77693.h>
+#include <linux/mfd/max77693-private.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <media/v4l2-flash-led-class.h>
+
+#define MODE_OFF		0
+#define MODE_FLASH(a)		(1 << (a))
+#define MODE_TORCH(a)		(1 << (2 + (a)))
+#define MODE_FLASH_EXTERNAL(a)	(1 << (4 + (a)))
+
+#define MODE_FLASH_MASK		(MODE_FLASH(FLED1) | MODE_FLASH(FLED2) | \
+				 MODE_FLASH_EXTERNAL(FLED1) | \
+				 MODE_FLASH_EXTERNAL(FLED2))
+#define MODE_TORCH_MASK		(MODE_TORCH(FLED1) | MODE_TORCH(FLED2))
+
+#define FLED1_IOUT		(1 << 0)
+#define FLED2_IOUT		(1 << 1)
+
+enum max77693_fled {
+	FLED1,
+	FLED2,
+};
+
+enum max77693_led_mode {
+	FLASH,
+	TORCH,
+};
+
+struct max77693_led_config_data {
+	const char *label[2];
+	u32 iout_torch_max[2];
+	u32 iout_flash_max[2];
+	u32 flash_timeout_max[2];
+	u32 num_leds;
+	u32 boost_mode;
+	u32 boost_vout;
+	u32 low_vsys;
+};
+
+struct max77693_sub_led {
+	/* corresponding FLED output identifier */
+	int fled_id;
+	/* corresponding LED Flash class device */
+	struct led_classdev_flash fled_cdev;
+	/* assures led-triggers compatibility */
+	struct work_struct work_brightness_set;
+	/* V4L2 Flash device */
+	struct v4l2_flash *v4l2_flash;
+
+	/* brightness cache */
+	unsigned int torch_brightness;
+	/* flash timeout cache */
+	unsigned int flash_timeout;
+	/* flash faults that may have occurred */
+	u32 flash_faults;
+};
+
+struct max77693_led_device {
+	/* parent mfd regmap */
+	struct regmap *regmap;
+	/* platform device data */
+	struct platform_device *pdev;
+	/* secures access to the device */
+	struct mutex lock;
+
+	/* sub led data */
+	struct max77693_sub_led sub_leds[2];
+
+	/* maximum torch current values for FLED outputs */
+	u32 iout_torch_max[2];
+	/* maximum flash current values for FLED outputs */
+	u32 iout_flash_max[2];
+
+	/* current flash timeout cache */
+	unsigned int current_flash_timeout;
+	/* ITORCH register cache */
+	u8 torch_iout_reg;
+	/* mode of fled outputs */
+	unsigned int mode_flags;
+	/* recently strobed fled */
+	int strobing_sub_led_id;
+	/* bitmask of FLED outputs use state (bit 0. - FLED1, bit 1. - FLED2) */
+	u8 fled_mask;
+	/* FLED modes that can be set */
+	u8 allowed_modes;
+
+	/* arrangement of current outputs */
+	bool iout_joint;
+};
+
+static u8 max77693_led_iout_to_reg(u32 ua)
+{
+	if (ua < FLASH_IOUT_MIN)
+		ua = FLASH_IOUT_MIN;
+	return (ua - FLASH_IOUT_MIN) / FLASH_IOUT_STEP;
+}
+
+static u8 max77693_flash_timeout_to_reg(u32 us)
+{
+	return (us - FLASH_TIMEOUT_MIN) / FLASH_TIMEOUT_STEP;
+}
+
+static inline struct max77693_sub_led *flcdev_to_sub_led(
+					struct led_classdev_flash *fled_cdev)
+{
+	return container_of(fled_cdev, struct max77693_sub_led, fled_cdev);
+}
+
+static inline struct max77693_led_device *sub_led_to_led(
+					struct max77693_sub_led *sub_led)
+{
+	return container_of(sub_led, struct max77693_led_device,
+				sub_leds[sub_led->fled_id]);
+}
+
+static inline u8 max77693_led_vsys_to_reg(u32 mv)
+{
+	return ((mv - MAX_FLASH1_VSYS_MIN) / MAX_FLASH1_VSYS_STEP) << 2;
+}
+
+static inline u8 max77693_led_vout_to_reg(u32 mv)
+{
+	return (mv - FLASH_VOUT_MIN) / FLASH_VOUT_STEP + FLASH_VOUT_RMIN;
+}
+
+static inline bool max77693_fled_used(struct max77693_led_device *led,
+					 int fled_id)
+{
+	u8 fled_bit = (fled_id == FLED1) ? FLED1_IOUT : FLED2_IOUT;
+
+	return led->fled_mask & fled_bit;
+}
+
+static int max77693_set_mode_reg(struct max77693_led_device *led, u8 mode)
+{
+	struct regmap *rmap = led->regmap;
+	int ret, v = 0, i;
+
+	for (i = FLED1; i <= FLED2; ++i) {
+		if (mode & MODE_TORCH(i))
+			v |= FLASH_EN_ON << TORCH_EN_SHIFT(i);
+
+		if (mode & MODE_FLASH(i)) {
+			v |= FLASH_EN_ON << FLASH_EN_SHIFT(i);
+		} else if (mode & MODE_FLASH_EXTERNAL(i)) {
+			v |= FLASH_EN_FLASH << FLASH_EN_SHIFT(i);
+			/*
+			 * Enable hw triggering also for torch mode, as some
+			 * camera sensors use torch led to fathom ambient light
+			 * conditions before strobing the flash.
+			 */
+			v |= FLASH_EN_TORCH << TORCH_EN_SHIFT(i);
+		}
+	}
+
+	/* Reset the register only prior setting flash modes */
+	if (mode & ~(MODE_TORCH(FLED1) | MODE_TORCH(FLED2))) {
+		ret = regmap_write(rmap, MAX77693_LED_REG_FLASH_EN, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	return regmap_write(rmap, MAX77693_LED_REG_FLASH_EN, v);
+}
+
+static int max77693_add_mode(struct max77693_led_device *led, u8 mode)
+{
+	u8 new_mode_flags;
+	int i, ret;
+
+	if (led->iout_joint)
+		/* Span the mode on FLED2 for joint iouts case */
+		mode |= (mode << 1);
+
+	/*
+	 * FLASH_EXTERNAL mode activates FLASHEN and TORCHEN pins in the device.
+	 * Corresponding register bit fields interfere with SW triggered modes,
+	 * thus clear them to ensure proper device configuration.
+	 */
+	for (i = FLED1; i <= FLED2; ++i)
+		if (mode & MODE_FLASH_EXTERNAL(i))
+			led->mode_flags &= (~MODE_TORCH(i) & ~MODE_FLASH(i));
+
+	new_mode_flags = mode | led->mode_flags;
+	new_mode_flags &= led->allowed_modes;
+
+	if (new_mode_flags ^ led->mode_flags)
+		led->mode_flags = new_mode_flags;
+	else
+		return 0;
+
+	ret = max77693_set_mode_reg(led, led->mode_flags);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Clear flash mode flag after setting the mode to avoid spurious flash
+	 * strobing on each subsequent torch mode setting.
+	 */
+	if (mode & MODE_FLASH_MASK)
+		led->mode_flags &= ~mode;
+
+	return ret;
+}
+
+static int max77693_clear_mode(struct max77693_led_device *led,
+				u8 mode)
+{
+	if (led->iout_joint)
+		/* Clear mode also on FLED2 for joint iouts case */
+		mode |= (mode << 1);
+
+	led->mode_flags &= ~mode;
+
+	return max77693_set_mode_reg(led, led->mode_flags);
+}
+
+static void max77693_add_allowed_modes(struct max77693_led_device *led,
+				int fled_id, enum max77693_led_mode mode)
+{
+	if (mode == FLASH)
+		led->allowed_modes |= (MODE_FLASH(fled_id) |
+				       MODE_FLASH_EXTERNAL(fled_id));
+	else
+		led->allowed_modes |= MODE_TORCH(fled_id);
+}
+
+static void max77693_distribute_currents(struct max77693_led_device *led,
+				int fled_id, enum max77693_led_mode mode,
+				u32 micro_amp, u32 iout_max[2], u32 iout[2])
+{
+	if (!led->iout_joint) {
+		iout[fled_id] = micro_amp;
+		max77693_add_allowed_modes(led, fled_id, mode);
+		return;
+	}
+
+	iout[FLED1] = min(micro_amp, iout_max[FLED1]);
+	iout[FLED2] = micro_amp - iout[FLED1];
+
+	if (mode == FLASH)
+		led->allowed_modes &= ~MODE_FLASH_MASK;
+	else
+		led->allowed_modes &= ~MODE_TORCH_MASK;
+
+	max77693_add_allowed_modes(led, FLED1, mode);
+
+	if (iout[FLED2])
+		max77693_add_allowed_modes(led, FLED2, mode);
+}
+
+static int max77693_set_torch_current(struct max77693_led_device *led,
+				int fled_id, u32 micro_amp)
+{
+	struct regmap *rmap = led->regmap;
+	u8 iout1_reg = 0, iout2_reg = 0;
+	u32 iout[2];
+
+	max77693_distribute_currents(led, fled_id, TORCH, micro_amp,
+					led->iout_torch_max, iout);
+
+	if (fled_id == FLED1 || led->iout_joint) {
+		iout1_reg = max77693_led_iout_to_reg(iout[FLED1]);
+		led->torch_iout_reg &= TORCH_IOUT_MASK(TORCH_IOUT2_SHIFT);
+	}
+	if (fled_id == FLED2 || led->iout_joint) {
+		iout2_reg = max77693_led_iout_to_reg(iout[FLED2]);
+		led->torch_iout_reg &= TORCH_IOUT_MASK(TORCH_IOUT1_SHIFT);
+	}
+
+	led->torch_iout_reg |= ((iout1_reg << TORCH_IOUT1_SHIFT) |
+				(iout2_reg << TORCH_IOUT2_SHIFT));
+
+	return regmap_write(rmap, MAX77693_LED_REG_ITORCH,
+						led->torch_iout_reg);
+}
+
+static int max77693_set_flash_current(struct max77693_led_device *led,
+					int fled_id,
+					u32 micro_amp)
+{
+	struct regmap *rmap = led->regmap;
+	u8 iout1_reg, iout2_reg;
+	u32 iout[2];
+	int ret = -EINVAL;
+
+	max77693_distribute_currents(led, fled_id, FLASH, micro_amp,
+					led->iout_flash_max, iout);
+
+	if (fled_id == FLED1 || led->iout_joint) {
+		iout1_reg = max77693_led_iout_to_reg(iout[FLED1]);
+		ret = regmap_write(rmap, MAX77693_LED_REG_IFLASH1,
+							iout1_reg);
+		if (ret < 0)
+			return ret;
+	}
+	if (fled_id == FLED2 || led->iout_joint) {
+		iout2_reg = max77693_led_iout_to_reg(iout[FLED2]);
+		ret = regmap_write(rmap, MAX77693_LED_REG_IFLASH2,
+							iout2_reg);
+	}
+
+	return ret;
+}
+
+static int max77693_set_timeout(struct max77693_led_device *led, u32 microsec)
+{
+	struct regmap *rmap = led->regmap;
+	u8 v;
+	int ret;
+
+	v = max77693_flash_timeout_to_reg(microsec) | FLASH_TMR_LEVEL;
+
+	ret = regmap_write(rmap, MAX77693_LED_REG_FLASH_TIMER, v);
+	if (ret < 0)
+		return ret;
+
+	led->current_flash_timeout = microsec;
+
+	return 0;
+}
+
+static int max77693_get_strobe_status(struct max77693_led_device *led,
+					bool *state)
+{
+	struct regmap *rmap = led->regmap;
+	unsigned int v;
+	int ret;
+
+	ret = regmap_read(rmap, MAX77693_LED_REG_FLASH_STATUS, &v);
+	if (ret < 0)
+		return ret;
+
+	*state = v & FLASH_STATUS_FLASH_ON;
+
+	return ret;
+}
+
+static int max77693_get_flash_faults(struct max77693_sub_led *sub_led)
+{
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	struct regmap *rmap = led->regmap;
+	unsigned int v;
+	u8 fault_open_mask, fault_short_mask;
+	int ret;
+
+	sub_led->flash_faults = 0;
+
+	if (led->iout_joint) {
+		fault_open_mask = FLASH_INT_FLED1_OPEN | FLASH_INT_FLED2_OPEN;
+		fault_short_mask = FLASH_INT_FLED1_SHORT |
+							FLASH_INT_FLED2_SHORT;
+	} else {
+		fault_open_mask = (sub_led->fled_id == FLED1) ?
+						FLASH_INT_FLED1_OPEN :
+						FLASH_INT_FLED2_OPEN;
+		fault_short_mask = (sub_led->fled_id == FLED1) ?
+						FLASH_INT_FLED1_SHORT :
+						FLASH_INT_FLED2_SHORT;
+	}
+
+	ret = regmap_read(rmap, MAX77693_LED_REG_FLASH_INT, &v);
+	if (ret < 0)
+		return ret;
+
+	if (v & fault_open_mask)
+		sub_led->flash_faults |= LED_FAULT_OVER_VOLTAGE;
+	if (v & fault_short_mask)
+		sub_led->flash_faults |= LED_FAULT_SHORT_CIRCUIT;
+	if (v & FLASH_INT_OVER_CURRENT)
+		sub_led->flash_faults |= LED_FAULT_OVER_CURRENT;
+
+	return 0;
+}
+
+static int max77693_setup(struct max77693_led_device *led,
+			 struct max77693_led_config_data *led_cfg)
+{
+	struct regmap *rmap = led->regmap;
+	int i, first_led, last_led, ret;
+	u32 max_flash_curr[2];
+	u8 v;
+
+	/*
+	 * Initialize only flash current. Torch current doesn't
+	 * require initialization as ITORCH register is written with
+	 * new value each time brightness_set op is called.
+	 */
+	if (led->iout_joint) {
+		first_led = FLED1;
+		last_led = FLED1;
+		max_flash_curr[FLED1] = led_cfg->iout_flash_max[FLED1] +
+					led_cfg->iout_flash_max[FLED2];
+	} else {
+		first_led = max77693_fled_used(led, FLED1) ? FLED1 : FLED2;
+		last_led = max77693_fled_used(led, FLED2) ? FLED2 : FLED1;
+		max_flash_curr[FLED1] = led_cfg->iout_flash_max[FLED1];
+		max_flash_curr[FLED2] = led_cfg->iout_flash_max[FLED2];
+	}
+
+	for (i = first_led; i <= last_led; ++i) {
+		ret = max77693_set_flash_current(led, i,
+					max_flash_curr[i]);
+		if (ret < 0)
+			return ret;
+	}
+
+	v = TORCH_TMR_NO_TIMER | MAX77693_LED_TRIG_TYPE_LEVEL;
+	ret = regmap_write(rmap, MAX77693_LED_REG_ITORCHTIMER, v);
+	if (ret < 0)
+		return ret;
+
+	if (led_cfg->low_vsys > 0)
+		v = max77693_led_vsys_to_reg(led_cfg->low_vsys) |
+						MAX_FLASH1_MAX_FL_EN;
+	else
+		v = 0;
+
+	ret = regmap_write(rmap, MAX77693_LED_REG_MAX_FLASH1, v);
+	if (ret < 0)
+		return ret;
+	ret = regmap_write(rmap, MAX77693_LED_REG_MAX_FLASH2, 0);
+	if (ret < 0)
+		return ret;
+
+	if (led_cfg->boost_mode == MAX77693_LED_BOOST_FIXED)
+		v = FLASH_BOOST_FIXED;
+	else
+		v = led_cfg->boost_mode | led_cfg->boost_mode << 1;
+
+	if (max77693_fled_used(led, FLED1) && max77693_fled_used(led, FLED2))
+		v |= FLASH_BOOST_LEDNUM_2;
+
+	ret = regmap_write(rmap, MAX77693_LED_REG_VOUT_CNTL, v);
+	if (ret < 0)
+		return ret;
+
+	v = max77693_led_vout_to_reg(led_cfg->boost_vout);
+	ret = regmap_write(rmap, MAX77693_LED_REG_VOUT_FLASH1, v);
+	if (ret < 0)
+		return ret;
+
+	return max77693_set_mode_reg(led, MODE_OFF);
+}
+
+static int __max77693_led_brightness_set(struct max77693_led_device *led,
+					int fled_id, enum led_brightness value)
+{
+	int ret;
+
+	mutex_lock(&led->lock);
+
+	if (value == 0) {
+		ret = max77693_clear_mode(led, MODE_TORCH(fled_id));
+		if (ret < 0)
+			dev_dbg(&led->pdev->dev,
+				"Failed to clear torch mode (%d)\n",
+				ret);
+		goto unlock;
+	}
+
+	ret = max77693_set_torch_current(led, fled_id, value * TORCH_IOUT_STEP);
+	if (ret < 0) {
+		dev_dbg(&led->pdev->dev,
+			"Failed to set torch current (%d)\n",
+			ret);
+		goto unlock;
+	}
+
+	ret = max77693_add_mode(led, MODE_TORCH(fled_id));
+	if (ret < 0)
+		dev_dbg(&led->pdev->dev,
+			"Failed to set torch mode (%d)\n",
+			ret);
+unlock:
+	mutex_unlock(&led->lock);
+	return ret;
+}
+
+static void max77693_led_brightness_set_work(
+					struct work_struct *work)
+{
+	struct max77693_sub_led *sub_led =
+			container_of(work, struct max77693_sub_led,
+					work_brightness_set);
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+
+	__max77693_led_brightness_set(led, sub_led->fled_id,
+				sub_led->torch_brightness);
+}
+
+/* LED subsystem callbacks */
+
+static int max77693_led_brightness_set_sync(
+				struct led_classdev *led_cdev,
+				enum led_brightness value)
+{
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
+	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+
+	return __max77693_led_brightness_set(led, sub_led->fled_id, value);
+}
+
+static void max77693_led_brightness_set(
+				struct led_classdev *led_cdev,
+				enum led_brightness value)
+{
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
+	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
+
+	sub_led->torch_brightness = value;
+	schedule_work(&sub_led->work_brightness_set);
+}
+
+static int max77693_led_flash_brightness_set(
+				struct led_classdev_flash *fled_cdev,
+				u32 brightness)
+{
+	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	int ret;
+
+	mutex_lock(&led->lock);
+	ret = max77693_set_flash_current(led, sub_led->fled_id, brightness);
+	mutex_unlock(&led->lock);
+
+	return ret;
+}
+
+static int max77693_led_flash_strobe_set(
+				struct led_classdev_flash *fled_cdev,
+				bool state)
+{
+	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	int fled_id = sub_led->fled_id;
+	int ret;
+
+	mutex_lock(&led->lock);
+
+	if (!state) {
+		ret = max77693_clear_mode(led, MODE_FLASH(fled_id));
+		goto unlock;
+	}
+
+	if (sub_led->flash_timeout != led->current_flash_timeout) {
+		ret = max77693_set_timeout(led, sub_led->flash_timeout);
+		if (ret < 0)
+			goto unlock;
+	}
+
+	led->strobing_sub_led_id = fled_id;
+
+	ret = max77693_add_mode(led, MODE_FLASH(fled_id));
+	if (ret < 0)
+		goto unlock;
+
+	ret = max77693_get_flash_faults(sub_led);
+
+unlock:
+	mutex_unlock(&led->lock);
+	return ret;
+}
+
+static int max77693_led_flash_fault_get(
+				struct led_classdev_flash *fled_cdev,
+				u32 *fault)
+{
+	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
+
+	*fault = sub_led->flash_faults;
+
+	return 0;
+}
+
+static int max77693_led_flash_strobe_get(
+				struct led_classdev_flash *fled_cdev,
+				bool *state)
+{
+	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	int ret;
+
+	if (!state)
+		return -EINVAL;
+
+	mutex_lock(&led->lock);
+
+	ret = max77693_get_strobe_status(led, state);
+
+	*state = !!(*state && (led->strobing_sub_led_id == sub_led->fled_id));
+
+	mutex_unlock(&led->lock);
+
+	return ret;
+}
+
+static int max77693_led_flash_timeout_set(
+				struct led_classdev_flash *fled_cdev,
+				u32 timeout)
+{
+	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+
+	mutex_lock(&led->lock);
+	sub_led->flash_timeout = timeout;
+	mutex_unlock(&led->lock);
+
+	return 0;
+}
+
+static int max77693_led_parse_dt(struct max77693_led_device *led,
+				struct max77693_led_config_data *cfg,
+				struct device_node **sub_nodes)
+{
+	struct device *dev = &led->pdev->dev;
+	struct max77693_sub_led *sub_leds = led->sub_leds;
+	struct device_node *node = dev->of_node, *child_node;
+	struct property *prop;
+	u32 led_sources[2];
+	int i, ret, fled_id;
+
+	of_property_read_u32(node, "maxim,boost-mode", &cfg->boost_mode);
+	of_property_read_u32(node, "maxim,boost-mvout", &cfg->boost_vout);
+	of_property_read_u32(node, "maxim,mvsys-min", &cfg->low_vsys);
+
+	for_each_available_child_of_node(node, child_node) {
+		prop = of_find_property(child_node, "led-sources", NULL);
+		if (prop) {
+			const __be32 *srcs = NULL;
+
+			for (i = 0; i < ARRAY_SIZE(led_sources); ++i) {
+				srcs = of_prop_next_u32(prop, srcs,
+							&led_sources[i]);
+				if (!srcs)
+					break;
+			}
+		} else {
+			dev_err(dev,
+				"led-sources DT property missing\n");
+			of_node_put(child_node);
+			return -EINVAL;
+		}
+
+		if (i == 2) {
+			fled_id = FLED1;
+			led->fled_mask = FLED1_IOUT | FLED2_IOUT;
+		} else if (led_sources[0] == FLED1) {
+			fled_id = FLED1;
+			led->fled_mask |= FLED1_IOUT;
+		} else if (led_sources[0] == FLED2) {
+			fled_id = FLED2;
+			led->fled_mask |= FLED2_IOUT;
+		} else {
+			dev_err(dev,
+				"Wrong led-sources DT property value.\n");
+			of_node_put(child_node);
+			return -EINVAL;
+		}
+
+		if (sub_nodes[fled_id]) {
+			dev_err(dev,
+				"Conflicting \"led-sources\" DT properties\n");
+			return -EINVAL;
+		}
+
+		sub_nodes[fled_id] = child_node;
+		sub_leds[fled_id].fled_id = fled_id;
+
+		cfg->label[fled_id] =
+			of_get_property(child_node, "label", NULL) ? :
+						child_node->name;
+
+		ret = of_property_read_u32(child_node, "led-max-microamp",
+					&cfg->iout_torch_max[fled_id]);
+		if (ret < 0) {
+			cfg->iout_torch_max[fled_id] = TORCH_IOUT_MIN;
+			dev_warn(dev, "led-max-microamp DT property missing\n");
+		}
+
+		ret = of_property_read_u32(child_node, "flash-max-microamp",
+					&cfg->iout_flash_max[fled_id]);
+		if (ret < 0) {
+			cfg->iout_flash_max[fled_id] = FLASH_IOUT_MIN;
+			dev_warn(dev,
+				 "flash-max-microamp DT property missing\n");
+		}
+
+		ret = of_property_read_u32(child_node, "flash-max-timeout-us",
+					&cfg->flash_timeout_max[fled_id]);
+		if (ret < 0) {
+			cfg->flash_timeout_max[fled_id] = FLASH_TIMEOUT_MIN;
+			dev_warn(dev,
+				 "flash-max-timeout-us DT property missing\n");
+		}
+
+		if (++cfg->num_leds == 2 ||
+		    (max77693_fled_used(led, FLED1) &&
+		     max77693_fled_used(led, FLED2))) {
+			of_node_put(child_node);
+			break;
+		}
+	}
+
+	if (cfg->num_leds == 0) {
+		dev_err(dev, "No DT child node found for connected LED(s).\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void clamp_align(u32 *v, u32 min, u32 max, u32 step)
+{
+	*v = clamp_val(*v, min, max);
+	if (step > 1)
+		*v = (*v - min) / step * step + min;
+}
+
+static void max77693_align_iout_current(struct max77693_led_device *led,
+					u32 *iout, u32 min, u32 max, u32 step)
+{
+	int i;
+
+	if (led->iout_joint) {
+		if (iout[FLED1] > min) {
+			iout[FLED1] /= 2;
+			iout[FLED2] = iout[FLED1];
+		} else {
+			iout[FLED1] = min;
+			iout[FLED2] = 0;
+			return;
+		}
+	}
+
+	for (i = FLED1; i <= FLED2; ++i)
+		if (max77693_fled_used(led, i))
+			clamp_align(&iout[i], min, max, step);
+		else
+			iout[i] = 0;
+}
+
+static void max77693_led_validate_configuration(struct max77693_led_device *led,
+					struct max77693_led_config_data *cfg)
+{
+	u32 flash_iout_max = cfg->boost_mode ? FLASH_IOUT_MAX_2LEDS :
+					       FLASH_IOUT_MAX_1LED;
+	int i;
+
+	if (cfg->num_leds == 1 &&
+	    max77693_fled_used(led, FLED1) && max77693_fled_used(led, FLED2))
+		led->iout_joint = true;
+
+	cfg->boost_mode = clamp_val(cfg->boost_mode, MAX77693_LED_BOOST_NONE,
+			    MAX77693_LED_BOOST_FIXED);
+
+	/* Boost must be enabled if both current outputs are used */
+	if ((cfg->boost_mode == MAX77693_LED_BOOST_NONE) && led->iout_joint)
+		cfg->boost_mode = MAX77693_LED_BOOST_FIXED;
+
+	max77693_align_iout_current(led, cfg->iout_torch_max,
+			TORCH_IOUT_MIN, TORCH_IOUT_MAX, TORCH_IOUT_STEP);
+
+	max77693_align_iout_current(led, cfg->iout_flash_max,
+			FLASH_IOUT_MIN, flash_iout_max, FLASH_IOUT_STEP);
+
+	for (i = 0; i < ARRAY_SIZE(cfg->flash_timeout_max); ++i)
+		clamp_align(&cfg->flash_timeout_max[i], FLASH_TIMEOUT_MIN,
+				FLASH_TIMEOUT_MAX, FLASH_TIMEOUT_STEP);
+
+	clamp_align(&cfg->boost_vout, FLASH_VOUT_MIN, FLASH_VOUT_MAX,
+							FLASH_VOUT_STEP);
+
+	if (cfg->low_vsys)
+		clamp_align(&cfg->low_vsys, MAX_FLASH1_VSYS_MIN,
+				MAX_FLASH1_VSYS_MAX, MAX_FLASH1_VSYS_STEP);
+}
+
+static int max77693_led_get_configuration(struct max77693_led_device *led,
+				struct max77693_led_config_data *cfg,
+				struct device_node **sub_nodes)
+{
+	int ret;
+
+	ret = max77693_led_parse_dt(led, cfg, sub_nodes);
+	if (ret < 0)
+		return ret;
+
+	max77693_led_validate_configuration(led, cfg);
+
+	memcpy(led->iout_torch_max, cfg->iout_torch_max,
+				sizeof(led->iout_torch_max));
+	memcpy(led->iout_flash_max, cfg->iout_flash_max,
+				sizeof(led->iout_flash_max));
+
+	return 0;
+}
+
+static const struct led_flash_ops flash_ops = {
+	.flash_brightness_set	= max77693_led_flash_brightness_set,
+	.strobe_set		= max77693_led_flash_strobe_set,
+	.strobe_get		= max77693_led_flash_strobe_get,
+	.timeout_set		= max77693_led_flash_timeout_set,
+	.fault_get		= max77693_led_flash_fault_get,
+};
+
+static void max77693_init_flash_settings(struct max77693_sub_led *sub_led,
+				 struct max77693_led_config_data *led_cfg)
+{
+	struct led_classdev_flash *fled_cdev = &sub_led->fled_cdev;
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	int fled_id = sub_led->fled_id;
+	struct led_flash_setting *setting;
+
+	/* Init flash intensity setting */
+	setting = &fled_cdev->brightness;
+	setting->min = FLASH_IOUT_MIN;
+	setting->max = led->iout_joint ?
+		led_cfg->iout_flash_max[FLED1] +
+		led_cfg->iout_flash_max[FLED2] :
+		led_cfg->iout_flash_max[fled_id];
+	setting->step = FLASH_IOUT_STEP;
+	setting->val = setting->max;
+
+	/* Init flash timeout setting */
+	setting = &fled_cdev->timeout;
+	setting->min = FLASH_TIMEOUT_MIN;
+	setting->max = led_cfg->flash_timeout_max[fled_id];
+	setting->step = FLASH_TIMEOUT_STEP;
+	setting->val = setting->max;
+}
+
+#if IS_ENABLED(CONFIG_V4L2_FLASH_LED_CLASS)
+
+static int max77693_led_external_strobe_set(
+				struct v4l2_flash *v4l2_flash,
+				bool enable)
+{
+	struct max77693_sub_led *sub_led =
+				flcdev_to_sub_led(v4l2_flash->fled_cdev);
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	int fled_id = sub_led->fled_id;
+	int ret;
+
+	mutex_lock(&led->lock);
+
+	if (enable)
+		ret = max77693_add_mode(led, MODE_FLASH_EXTERNAL(fled_id));
+	else
+		ret = max77693_clear_mode(led, MODE_FLASH_EXTERNAL(fled_id));
+
+	mutex_unlock(&led->lock);
+
+	return ret;
+}
+
+static void max77693_init_v4l2_flash_config(struct max77693_sub_led *sub_led,
+				struct max77693_led_config_data *led_cfg,
+				struct v4l2_flash_config *v4l2_sd_cfg)
+{
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	struct device *dev = &led->pdev->dev;
+	struct max77693_dev *iodev = dev_get_drvdata(dev->parent);
+	struct i2c_client *i2c = iodev->i2c;
+	struct led_flash_setting *s;
+
+	snprintf(v4l2_sd_cfg->dev_name, sizeof(v4l2_sd_cfg->dev_name),
+		 "%s %d-%04x", sub_led->fled_cdev.led_cdev.name,
+		 i2c_adapter_id(i2c->adapter), i2c->addr);
+
+	s = &v4l2_sd_cfg->torch_intensity;
+	s->min = TORCH_IOUT_MIN;
+	s->max = sub_led->fled_cdev.led_cdev.max_brightness * TORCH_IOUT_STEP;
+	s->step = TORCH_IOUT_STEP;
+	s->val = s->max;
+
+	/* Init flash faults config */
+	v4l2_sd_cfg->flash_faults = LED_FAULT_OVER_VOLTAGE |
+				LED_FAULT_SHORT_CIRCUIT |
+				LED_FAULT_OVER_CURRENT;
+
+	v4l2_sd_cfg->has_external_strobe = true;
+}
+
+static const struct v4l2_flash_ops v4l2_flash_ops = {
+	.external_strobe_set = max77693_led_external_strobe_set,
+};
+#else
+static inline void max77693_init_v4l2_flash_config(
+				struct max77693_sub_led *sub_led,
+				struct max77693_led_config_data *led_cfg,
+				struct v4l2_flash_config *v4l2_sd_cfg)
+{
+}
+static const struct v4l2_flash_ops v4l2_flash_ops;
+#endif
+
+static void max77693_init_fled_cdev(struct max77693_sub_led *sub_led,
+				struct max77693_led_config_data *led_cfg)
+{
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	int fled_id = sub_led->fled_id;
+	struct led_classdev_flash *fled_cdev;
+	struct led_classdev *led_cdev;
+
+	/* Initialize LED Flash class device */
+	fled_cdev = &sub_led->fled_cdev;
+	fled_cdev->ops = &flash_ops;
+	led_cdev = &fled_cdev->led_cdev;
+
+	led_cdev->name = led_cfg->label[fled_id];
+
+	led_cdev->brightness_set = max77693_led_brightness_set;
+	led_cdev->brightness_set_sync = max77693_led_brightness_set_sync;
+	led_cdev->max_brightness = (led->iout_joint ?
+					led_cfg->iout_torch_max[FLED1] +
+					led_cfg->iout_torch_max[FLED2] :
+					led_cfg->iout_torch_max[fled_id]) /
+				   TORCH_IOUT_STEP;
+	led_cdev->flags |= LED_DEV_CAP_FLASH;
+	INIT_WORK(&sub_led->work_brightness_set,
+			max77693_led_brightness_set_work);
+
+	max77693_init_flash_settings(sub_led, led_cfg);
+
+	/* Init flash timeout cache */
+	sub_led->flash_timeout = fled_cdev->timeout.val;
+}
+
+static int max77693_register_led(struct max77693_sub_led *sub_led,
+				 struct max77693_led_config_data *led_cfg,
+				 struct device_node *sub_node)
+{
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	struct led_classdev_flash *fled_cdev = &sub_led->fled_cdev;
+	struct device *dev = &led->pdev->dev;
+	struct v4l2_flash_config v4l2_sd_cfg = {};
+	int ret;
+
+	/* Register in the LED subsystem */
+	ret = led_classdev_flash_register(dev, fled_cdev);
+	if (ret < 0)
+		return ret;
+
+	max77693_init_v4l2_flash_config(sub_led, led_cfg, &v4l2_sd_cfg);
+
+	/* Register in the V4L2 subsystem. */
+	sub_led->v4l2_flash = v4l2_flash_init(dev, sub_node, fled_cdev, NULL,
+					      &v4l2_flash_ops, &v4l2_sd_cfg);
+	if (IS_ERR(sub_led->v4l2_flash)) {
+		ret = PTR_ERR(sub_led->v4l2_flash);
+		goto err_v4l2_flash_init;
+	}
+
+	return 0;
+
+err_v4l2_flash_init:
+	led_classdev_flash_unregister(fled_cdev);
+	return ret;
+}
+
+static int max77693_led_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct max77693_dev *iodev = dev_get_drvdata(dev->parent);
+	struct max77693_led_device *led;
+	struct max77693_sub_led *sub_leds;
+	struct device_node *sub_nodes[2] = {};
+	struct max77693_led_config_data led_cfg = {};
+	int init_fled_cdev[2], i, ret;
+
+	led = devm_kzalloc(dev, sizeof(*led), GFP_KERNEL);
+	if (!led)
+		return -ENOMEM;
+
+	led->pdev = pdev;
+	led->regmap = iodev->regmap;
+	led->allowed_modes = MODE_FLASH_MASK;
+	sub_leds = led->sub_leds;
+
+	platform_set_drvdata(pdev, led);
+	ret = max77693_led_get_configuration(led, &led_cfg, sub_nodes);
+	if (ret < 0)
+		return ret;
+
+	ret = max77693_setup(led, &led_cfg);
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&led->lock);
+
+	init_fled_cdev[FLED1] =
+			led->iout_joint || max77693_fled_used(led, FLED1);
+	init_fled_cdev[FLED2] =
+			!led->iout_joint && max77693_fled_used(led, FLED2);
+
+	for (i = FLED1; i <= FLED2; ++i) {
+		if (!init_fled_cdev[i])
+			continue;
+
+		/* Initialize LED Flash class device */
+		max77693_init_fled_cdev(&sub_leds[i], &led_cfg);
+
+		/*
+		 * Register LED Flash class device and corresponding
+		 * V4L2 Flash device.
+		 */
+		ret = max77693_register_led(&sub_leds[i], &led_cfg,
+						sub_nodes[i]);
+		if (ret < 0) {
+			/*
+			 * At this moment FLED1 might have been already
+			 * registered and it needs to be released.
+			 */
+			if (i == FLED2)
+				goto err_register_led2;
+			else
+				goto err_register_led1;
+		}
+	}
+
+	return 0;
+
+err_register_led2:
+	/* It is possible than only FLED2 was to be registered */
+	if (!init_fled_cdev[FLED1])
+		goto err_register_led1;
+	v4l2_flash_release(sub_leds[FLED1].v4l2_flash);
+	led_classdev_flash_unregister(&sub_leds[FLED1].fled_cdev);
+err_register_led1:
+	mutex_destroy(&led->lock);
+
+	return ret;
+}
+
+static int max77693_led_remove(struct platform_device *pdev)
+{
+	struct max77693_led_device *led = platform_get_drvdata(pdev);
+	struct max77693_sub_led *sub_leds = led->sub_leds;
+
+	if (led->iout_joint || max77693_fled_used(led, FLED1)) {
+		v4l2_flash_release(sub_leds[FLED1].v4l2_flash);
+		led_classdev_flash_unregister(&sub_leds[FLED1].fled_cdev);
+		cancel_work_sync(&sub_leds[FLED1].work_brightness_set);
+	}
+
+	if (!led->iout_joint && max77693_fled_used(led, FLED2)) {
+		v4l2_flash_release(sub_leds[FLED2].v4l2_flash);
+		led_classdev_flash_unregister(&sub_leds[FLED2].fled_cdev);
+		cancel_work_sync(&sub_leds[FLED2].work_brightness_set);
+	}
+
+	mutex_destroy(&led->lock);
+
+	return 0;
+}
+
+static const struct of_device_id max77693_led_dt_match[] = {
+	{ .compatible = "maxim,max77693-led" },
+	{},
+};
+
+static struct platform_driver max77693_led_driver = {
+	.probe		= max77693_led_probe,
+	.remove		= max77693_led_remove,
+	.driver		= {
+		.name	= "max77693-led",
+		.of_match_table = max77693_led_dt_match,
+	},
+};
+
+module_platform_driver(max77693_led_driver);
+
+MODULE_AUTHOR("Jacek Anaszewski <j.anaszewski@samsung.com>");
+MODULE_AUTHOR("Andrzej Hajda <a.hajda@samsung.com>");
+MODULE_DESCRIPTION("Maxim MAX77693 led flash driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/leds-syscon.c b/drivers/leds/leds-syscon.c
index 6896e2d9ba58..d1660b039812 100644
--- a/drivers/leds/leds-syscon.c
+++ b/drivers/leds/leds-syscon.c
@@ -20,6 +20,7 @@
  * MA 02111-1307 USA
  */
 #include <linux/io.h>
+#include <linux/module.h>
 #include <linux/of_device.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
@@ -66,102 +67,101 @@ static void syscon_led_set(struct led_classdev *led_cdev,
 		dev_err(sled->cdev.dev, "error updating LED status\n");
 }
 
-static int __init syscon_leds_spawn(struct device_node *np,
-				    struct device *dev,
-				    struct regmap *map)
+static int syscon_led_probe(struct platform_device *pdev)
 {
-	struct device_node *child;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct device *parent;
+	struct regmap *map;
+	struct syscon_led *sled;
+	const char *state;
 	int ret;
 
-	for_each_available_child_of_node(np, child) {
-		struct syscon_led *sled;
-		const char *state;
-
-		/* Only check for register-bit-leds */
-		if (of_property_match_string(child, "compatible",
-					     "register-bit-led") < 0)
-			continue;
-
-		sled = devm_kzalloc(dev, sizeof(*sled), GFP_KERNEL);
-		if (!sled)
-			return -ENOMEM;
-
-		sled->map = map;
-
-		if (of_property_read_u32(child, "offset", &sled->offset))
-			return -EINVAL;
-		if (of_property_read_u32(child, "mask", &sled->mask))
-			return -EINVAL;
-		sled->cdev.name =
-			of_get_property(child, "label", NULL) ? : child->name;
-		sled->cdev.default_trigger =
-			of_get_property(child, "linux,default-trigger", NULL);
-
-		state = of_get_property(child, "default-state", NULL);
-		if (state) {
-			if (!strcmp(state, "keep")) {
-				u32 val;
-
-				ret = regmap_read(map, sled->offset, &val);
-				if (ret < 0)
-					return ret;
-				sled->state = !!(val & sled->mask);
-			} else if (!strcmp(state, "on")) {
-				sled->state = true;
-				ret = regmap_update_bits(map, sled->offset,
-							 sled->mask,
-							 sled->mask);
-				if (ret < 0)
-					return ret;
-			} else {
-				sled->state = false;
-				ret = regmap_update_bits(map, sled->offset,
-							 sled->mask, 0);
-				if (ret < 0)
-					return ret;
-			}
+	parent = dev->parent;
+	if (!parent) {
+		dev_err(dev, "no parent for syscon LED\n");
+		return -ENODEV;
+	}
+	map = syscon_node_to_regmap(parent->of_node);
+	if (!map) {
+		dev_err(dev, "no regmap for syscon LED parent\n");
+		return -ENODEV;
+	}
+
+	sled = devm_kzalloc(dev, sizeof(*sled), GFP_KERNEL);
+	if (!sled)
+		return -ENOMEM;
+
+	sled->map = map;
+
+	if (of_property_read_u32(np, "offset", &sled->offset))
+		return -EINVAL;
+	if (of_property_read_u32(np, "mask", &sled->mask))
+		return -EINVAL;
+	sled->cdev.name =
+		of_get_property(np, "label", NULL) ? : np->name;
+	sled->cdev.default_trigger =
+		of_get_property(np, "linux,default-trigger", NULL);
+
+	state = of_get_property(np, "default-state", NULL);
+	if (state) {
+		if (!strcmp(state, "keep")) {
+			u32 val;
+
+			ret = regmap_read(map, sled->offset, &val);
+			if (ret < 0)
+				return ret;
+			sled->state = !!(val & sled->mask);
+		} else if (!strcmp(state, "on")) {
+			sled->state = true;
+			ret = regmap_update_bits(map, sled->offset,
+						 sled->mask,
+						 sled->mask);
+			if (ret < 0)
+				return ret;
+		} else {
+			sled->state = false;
+			ret = regmap_update_bits(map, sled->offset,
+						 sled->mask, 0);
+			if (ret < 0)
+				return ret;
 		}
-		sled->cdev.brightness_set = syscon_led_set;
+	}
+	sled->cdev.brightness_set = syscon_led_set;
 
-		ret = led_classdev_register(dev, &sled->cdev);
-		if (ret < 0)
-			return ret;
+	ret = led_classdev_register(dev, &sled->cdev);
+	if (ret < 0)
+		return ret;
+
+	platform_set_drvdata(pdev, sled);
+	dev_info(dev, "registered LED %s\n", sled->cdev.name);
 
-		dev_info(dev, "registered LED %s\n", sled->cdev.name);
-	}
 	return 0;
 }
 
-static int __init syscon_leds_init(void)
+static int syscon_led_remove(struct platform_device *pdev)
 {
-	struct device_node *np;
-
-	for_each_of_allnodes(np) {
-		struct platform_device *pdev;
-		struct regmap *map;
-		int ret;
+	struct syscon_led *sled = platform_get_drvdata(pdev);
 
-		if (!of_device_is_compatible(np, "syscon"))
-			continue;
+	led_classdev_unregister(&sled->cdev);
+	/* Turn it off */
+	regmap_update_bits(sled->map, sled->offset, sled->mask, 0);
+	return 0;
+}
 
-		map = syscon_node_to_regmap(np);
-		if (IS_ERR(map)) {
-			pr_err("error getting regmap for syscon LEDs\n");
-			continue;
-		}
+static const struct of_device_id of_syscon_leds_match[] = {
+	{ .compatible = "register-bit-led", },
+	{},
+};
 
-		/*
-		 * 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");
-	}
+MODULE_DEVICE_TABLE(of, of_syscon_leds_match);
 
-	return 0;
-}
-device_initcall(syscon_leds_init);
+static struct platform_driver syscon_led_driver = {
+	.probe		= syscon_led_probe,
+	.remove		= syscon_led_remove,
+	.driver		= {
+		.name	= "leds-syscon",
+		.of_match_table = of_syscon_leds_match,
+	},
+};
+module_platform_driver(syscon_led_driver);
diff --git a/drivers/leds/leds-tlc591xx.c b/drivers/leds/leds-tlc591xx.c
new file mode 100644
index 000000000000..de16c29d7895
--- /dev/null
+++ b/drivers/leds/leds-tlc591xx.c
@@ -0,0 +1,300 @@
+/*
+ * Copyright 2014 Belkin Inc.
+ * Copyright 2015 Andrew Lunn <andrew@lunn.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; version 2 of the License.
+ */
+
+#include <linux/i2c.h>
+#include <linux/leds.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#define TLC591XX_MAX_LEDS	16
+
+#define TLC591XX_REG_MODE1	0x00
+#define MODE1_RESPON_ADDR_MASK	0xF0
+#define MODE1_NORMAL_MODE	(0 << 4)
+#define MODE1_SPEED_MODE	(1 << 4)
+
+#define TLC591XX_REG_MODE2	0x01
+#define MODE2_DIM		(0 << 5)
+#define MODE2_BLINK		(1 << 5)
+#define MODE2_OCH_STOP		(0 << 3)
+#define MODE2_OCH_ACK		(1 << 3)
+
+#define TLC591XX_REG_PWM(x)	(0x02 + (x))
+
+#define TLC591XX_REG_GRPPWM	0x12
+#define TLC591XX_REG_GRPFREQ	0x13
+
+/* LED Driver Output State, determine the source that drives LED outputs */
+#define LEDOUT_OFF		0x0	/* Output LOW */
+#define LEDOUT_ON		0x1	/* Output HI-Z */
+#define LEDOUT_DIM		0x2	/* Dimming */
+#define LEDOUT_BLINK		0x3	/* Blinking */
+#define LEDOUT_MASK		0x3
+
+#define ldev_to_led(c)		container_of(c, struct tlc591xx_led, ldev)
+#define work_to_led(work)	container_of(work, struct tlc591xx_led, work)
+
+struct tlc591xx_led {
+	bool active;
+	unsigned int led_no;
+	struct led_classdev ldev;
+	struct work_struct work;
+	struct tlc591xx_priv *priv;
+};
+
+struct tlc591xx_priv {
+	struct tlc591xx_led leds[TLC591XX_MAX_LEDS];
+	struct regmap *regmap;
+	unsigned int reg_ledout_offset;
+};
+
+struct tlc591xx {
+	unsigned int max_leds;
+	unsigned int reg_ledout_offset;
+};
+
+static const struct tlc591xx tlc59116 = {
+	.max_leds = 16,
+	.reg_ledout_offset = 0x14,
+};
+
+static const struct tlc591xx tlc59108 = {
+	.max_leds = 8,
+	.reg_ledout_offset = 0x0c,
+};
+
+static int
+tlc591xx_set_mode(struct regmap *regmap, u8 mode)
+{
+	int err;
+	u8 val;
+
+	err = regmap_write(regmap, TLC591XX_REG_MODE1, MODE1_NORMAL_MODE);
+	if (err)
+		return err;
+
+	val = MODE2_OCH_STOP | mode;
+
+	return regmap_write(regmap, TLC591XX_REG_MODE2, val);
+}
+
+static int
+tlc591xx_set_ledout(struct tlc591xx_priv *priv, struct tlc591xx_led *led,
+		    u8 val)
+{
+	unsigned int i = (led->led_no % 4) * 2;
+	unsigned int mask = LEDOUT_MASK << i;
+	unsigned int addr = priv->reg_ledout_offset + (led->led_no >> 2);
+
+	val = val << i;
+
+	return regmap_update_bits(priv->regmap, addr, mask, val);
+}
+
+static int
+tlc591xx_set_pwm(struct tlc591xx_priv *priv, struct tlc591xx_led *led,
+		 u8 brightness)
+{
+	u8 pwm = TLC591XX_REG_PWM(led->led_no);
+
+	return regmap_write(priv->regmap, pwm, brightness);
+}
+
+static void
+tlc591xx_led_work(struct work_struct *work)
+{
+	struct tlc591xx_led *led = work_to_led(work);
+	struct tlc591xx_priv *priv = led->priv;
+	enum led_brightness brightness = led->ldev.brightness;
+	int err;
+
+	switch (brightness) {
+	case 0:
+		err = tlc591xx_set_ledout(priv, led, LEDOUT_OFF);
+		break;
+	case LED_FULL:
+		err = tlc591xx_set_ledout(priv, led, LEDOUT_ON);
+		break;
+	default:
+		err = tlc591xx_set_ledout(priv, led, LEDOUT_DIM);
+		if (!err)
+			err = tlc591xx_set_pwm(priv, led, brightness);
+	}
+
+	if (err)
+		dev_err(led->ldev.dev, "Failed setting brightness\n");
+}
+
+static void
+tlc591xx_brightness_set(struct led_classdev *led_cdev,
+			enum led_brightness brightness)
+{
+	struct tlc591xx_led *led = ldev_to_led(led_cdev);
+
+	led->ldev.brightness = brightness;
+	schedule_work(&led->work);
+}
+
+static void
+tlc591xx_destroy_devices(struct tlc591xx_priv *priv, unsigned int j)
+{
+	int i = j;
+
+	while (--i >= 0) {
+		if (priv->leds[i].active) {
+			led_classdev_unregister(&priv->leds[i].ldev);
+			cancel_work_sync(&priv->leds[i].work);
+		}
+	}
+}
+
+static int
+tlc591xx_configure(struct device *dev,
+		   struct tlc591xx_priv *priv,
+		   const struct tlc591xx *tlc591xx)
+{
+	unsigned int i;
+	int err = 0;
+
+	tlc591xx_set_mode(priv->regmap, MODE2_DIM);
+	for (i = 0; i < TLC591XX_MAX_LEDS; i++) {
+		struct tlc591xx_led *led = &priv->leds[i];
+
+		if (!led->active)
+			continue;
+
+		led->priv = priv;
+		led->led_no = i;
+		led->ldev.brightness_set = tlc591xx_brightness_set;
+		led->ldev.max_brightness = LED_FULL;
+		INIT_WORK(&led->work, tlc591xx_led_work);
+		err = led_classdev_register(dev, &led->ldev);
+		if (err < 0) {
+			dev_err(dev, "couldn't register LED %s\n",
+				led->ldev.name);
+			goto exit;
+		}
+	}
+
+	return 0;
+
+exit:
+	tlc591xx_destroy_devices(priv, i);
+	return err;
+}
+
+static const struct regmap_config tlc591xx_regmap = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0x1e,
+};
+
+static const struct of_device_id of_tlc591xx_leds_match[] = {
+	{ .compatible = "ti,tlc59116",
+	  .data = &tlc59116 },
+	{ .compatible = "ti,tlc59108",
+	  .data = &tlc59108 },
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_tlc591xx_leds_match);
+
+static int
+tlc591xx_probe(struct i2c_client *client,
+	       const struct i2c_device_id *id)
+{
+	struct device_node *np = client->dev.of_node, *child;
+	struct device *dev = &client->dev;
+	const struct of_device_id *match;
+	const struct tlc591xx *tlc591xx;
+	struct tlc591xx_priv *priv;
+	int err, count, reg;
+
+	match = of_match_device(of_tlc591xx_leds_match, dev);
+	if (!match)
+		return -ENODEV;
+
+	tlc591xx = match->data;
+	if (!np)
+		return -ENODEV;
+
+	count = of_get_child_count(np);
+	if (!count || count > tlc591xx->max_leds)
+		return -EINVAL;
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->regmap = devm_regmap_init_i2c(client, &tlc591xx_regmap);
+	if (IS_ERR(priv->regmap)) {
+		err = PTR_ERR(priv->regmap);
+		dev_err(dev, "Failed to allocate register map: %d\n", err);
+		return err;
+	}
+	priv->reg_ledout_offset = tlc591xx->reg_ledout_offset;
+
+	i2c_set_clientdata(client, priv);
+
+	for_each_child_of_node(np, child) {
+		err = of_property_read_u32(child, "reg", &reg);
+		if (err)
+			return err;
+		if (reg < 0 || reg >= tlc591xx->max_leds)
+			return -EINVAL;
+		if (priv->leds[reg].active)
+			return -EINVAL;
+		priv->leds[reg].active = true;
+		priv->leds[reg].ldev.name =
+			of_get_property(child, "label", NULL) ? : child->name;
+		priv->leds[reg].ldev.default_trigger =
+			of_get_property(child, "linux,default-trigger", NULL);
+	}
+	return tlc591xx_configure(dev, priv, tlc591xx);
+}
+
+static int
+tlc591xx_remove(struct i2c_client *client)
+{
+	struct tlc591xx_priv *priv = i2c_get_clientdata(client);
+
+	tlc591xx_destroy_devices(priv, TLC591XX_MAX_LEDS);
+
+	return 0;
+}
+
+static const struct i2c_device_id tlc591xx_id[] = {
+	{ "tlc59116" },
+	{ "tlc59108" },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, tlc591xx_id);
+
+static struct i2c_driver tlc591xx_driver = {
+	.driver = {
+		.name = "tlc591xx",
+		.of_match_table = of_match_ptr(of_tlc591xx_leds_match),
+	},
+	.probe = tlc591xx_probe,
+	.remove = tlc591xx_remove,
+	.id_table = tlc591xx_id,
+};
+
+module_i2c_driver(tlc591xx_driver);
+
+MODULE_AUTHOR("Andrew Lunn <andrew@lunn.ch>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("TLC591XX LED driver");
diff --git a/drivers/leds/leds.h b/drivers/leds/leds.h
index 79efe57c7405..bc89d7ace2c4 100644
--- a/drivers/leds/leds.h
+++ b/drivers/leds/leds.h
@@ -13,7 +13,6 @@
 #ifndef __LEDS_H_INCLUDED
 #define __LEDS_H_INCLUDED
 
-#include <linux/device.h>
 #include <linux/rwsem.h>
 #include <linux/leds.h>
 
@@ -50,27 +49,4 @@ void led_stop_software_blink(struct led_classdev *led_cdev);
 extern struct rw_semaphore leds_list_lock;
 extern struct list_head leds_list;
 
-#ifdef CONFIG_LEDS_TRIGGERS
-void led_trigger_set_default(struct led_classdev *led_cdev);
-void led_trigger_set(struct led_classdev *led_cdev,
-			struct led_trigger *trigger);
-void led_trigger_remove(struct led_classdev *led_cdev);
-
-static inline void *led_get_trigger_data(struct led_classdev *led_cdev)
-{
-	return led_cdev->trigger_data;
-}
-
-#else
-#define led_trigger_set_default(x) do {} while (0)
-#define led_trigger_set(x, y) do {} while (0)
-#define led_trigger_remove(x) do {} while (0)
-#define led_get_trigger_data(x) (NULL)
-#endif
-
-ssize_t led_trigger_store(struct device *dev, struct device_attribute *attr,
-			const char *buf, size_t count);
-ssize_t led_trigger_show(struct device *dev, struct device_attribute *attr,
-			char *buf);
-
 #endif	/* __LEDS_H_INCLUDED */
diff --git a/drivers/lguest/interrupts_and_traps.c b/drivers/lguest/interrupts_and_traps.c
index 5e7559be222a..eb934b0242e0 100644
--- a/drivers/lguest/interrupts_and_traps.c
+++ b/drivers/lguest/interrupts_and_traps.c
@@ -20,7 +20,7 @@
 #include "lg.h"
 
 /* Allow Guests to use a non-128 (ie. non-Linux) syscall trap. */
-static unsigned int syscall_vector = SYSCALL_VECTOR;
+static unsigned int syscall_vector = IA32_SYSCALL_VECTOR;
 module_param(syscall_vector, uint, 0444);
 
 /* The address of the interrupt handler is split into two bits: */
@@ -333,8 +333,8 @@ void set_interrupt(struct lg_cpu *cpu, unsigned int irq)
  */
 static bool could_be_syscall(unsigned int num)
 {
-	/* Normal Linux SYSCALL_VECTOR or reserved vector? */
-	return num == SYSCALL_VECTOR || num == syscall_vector;
+	/* Normal Linux IA32_SYSCALL_VECTOR or reserved vector? */
+	return num == IA32_SYSCALL_VECTOR || num == syscall_vector;
 }
 
 /* The syscall vector it wants must be unused by Host. */
@@ -351,7 +351,7 @@ bool check_syscall_vector(struct lguest *lg)
 int init_interrupts(void)
 {
 	/* If they want some strange system call vector, reserve it now */
-	if (syscall_vector != SYSCALL_VECTOR) {
+	if (syscall_vector != IA32_SYSCALL_VECTOR) {
 		if (test_bit(syscall_vector, used_vectors) ||
 		    vector_used_by_percpu_irq(syscall_vector)) {
 			printk(KERN_ERR "lg: couldn't reserve syscall %u\n",
@@ -366,7 +366,7 @@ int init_interrupts(void)
 
 void free_interrupts(void)
 {
-	if (syscall_vector != SYSCALL_VECTOR)
+	if (syscall_vector != IA32_SYSCALL_VECTOR)
 		clear_bit(syscall_vector, used_vectors);
 }
 
diff --git a/drivers/lguest/x86/core.c b/drivers/lguest/x86/core.c
index 30f2aef69d78..6a4cd771a2be 100644
--- a/drivers/lguest/x86/core.c
+++ b/drivers/lguest/x86/core.c
@@ -46,7 +46,7 @@
 #include <asm/setup.h>
 #include <asm/lguest.h>
 #include <asm/uaccess.h>
-#include <asm/i387.h>
+#include <asm/fpu/internal.h>
 #include <asm/tlbflush.h>
 #include "../lg.h"
 
@@ -251,7 +251,7 @@ void lguest_arch_run_guest(struct lg_cpu *cpu)
 	 * we set it now, so we can trap and pass that trap to the Guest if it
 	 * uses the FPU.
 	 */
-	if (cpu->ts && user_has_fpu())
+	if (cpu->ts && fpregs_active())
 		stts();
 
 	/*
@@ -283,7 +283,7 @@ void lguest_arch_run_guest(struct lg_cpu *cpu)
 		wrmsr(MSR_IA32_SYSENTER_CS, __KERNEL_CS, 0);
 
 	/* Clear the host TS bit if it was set above. */
-	if (cpu->ts && user_has_fpu())
+	if (cpu->ts && fpregs_active())
 		clts();
 
 	/*
@@ -297,12 +297,12 @@ void lguest_arch_run_guest(struct lg_cpu *cpu)
 	/*
 	 * Similarly, if we took a trap because the Guest used the FPU,
 	 * we have to restore the FPU it expects to see.
-	 * math_state_restore() may sleep and we may even move off to
+	 * fpu__restore() may sleep and we may even move off to
 	 * a different CPU. So all the critical stuff should be done
 	 * before this.
 	 */
-	else if (cpu->regs->trapnum == 7 && !user_has_fpu())
-		math_state_restore();
+	else if (cpu->regs->trapnum == 7 && !fpregs_active())
+		fpu__restore(&current->thread.fpu);
 }
 
 /*H:130
diff --git a/drivers/macintosh/nvram.c b/drivers/macintosh/nvram.c
deleted file mode 100644
index f0e03e7937e3..000000000000
--- a/drivers/macintosh/nvram.c
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * /dev/nvram driver for Power Macintosh.
- */
-
-#define NVRAM_VERSION "1.0"
-
-#include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/fs.h>
-#include <linux/miscdevice.h>
-#include <linux/fcntl.h>
-#include <linux/nvram.h>
-#include <linux/init.h>
-#include <asm/uaccess.h>
-#include <asm/nvram.h>
-
-#define NVRAM_SIZE	8192
-
-static loff_t nvram_llseek(struct file *file, loff_t offset, int origin)
-{
-	switch (origin) {
-	case 0:
-		break;
-	case 1:
-		offset += file->f_pos;
-		break;
-	case 2:
-		offset += NVRAM_SIZE;
-		break;
-	default:
-		offset = -1;
-	}
-	if (offset < 0)
-		return -EINVAL;
-
-	file->f_pos = offset;
-	return file->f_pos;
-}
-
-static ssize_t read_nvram(struct file *file, char __user *buf,
-			  size_t count, loff_t *ppos)
-{
-	unsigned int i;
-	char __user *p = buf;
-
-	if (!access_ok(VERIFY_WRITE, buf, count))
-		return -EFAULT;
-	if (*ppos >= NVRAM_SIZE)
-		return 0;
-	for (i = *ppos; count > 0 && i < NVRAM_SIZE; ++i, ++p, --count)
-		if (__put_user(nvram_read_byte(i), p))
-			return -EFAULT;
-	*ppos = i;
-	return p - buf;
-}
-
-static ssize_t write_nvram(struct file *file, const char __user *buf,
-			   size_t count, loff_t *ppos)
-{
-	unsigned int i;
-	const char __user *p = buf;
-	char c;
-
-	if (!access_ok(VERIFY_READ, buf, count))
-		return -EFAULT;
-	if (*ppos >= NVRAM_SIZE)
-		return 0;
-	for (i = *ppos; count > 0 && i < NVRAM_SIZE; ++i, ++p, --count) {
-		if (__get_user(c, p))
-			return -EFAULT;
-		nvram_write_byte(c, i);
-	}
-	*ppos = i;
-	return p - buf;
-}
-
-static long nvram_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
-{
-	switch(cmd) {
-		case PMAC_NVRAM_GET_OFFSET:
-		{
-			int part, offset;
-			if (copy_from_user(&part, (void __user*)arg, sizeof(part)) != 0)
-				return -EFAULT;
-			if (part < pmac_nvram_OF || part > pmac_nvram_NR)
-				return -EINVAL;
-			offset = pmac_get_partition(part);
-			if (copy_to_user((void __user*)arg, &offset, sizeof(offset)) != 0)
-				return -EFAULT;
-			break;
-		}
-
-		default:
-			return -EINVAL;
-	}
-
-	return 0;
-}
-
-const struct file_operations nvram_fops = {
-	.owner		= THIS_MODULE,
-	.llseek		= nvram_llseek,
-	.read		= read_nvram,
-	.write		= write_nvram,
-	.unlocked_ioctl	= nvram_ioctl,
-};
-
-static struct miscdevice nvram_dev = {
-	NVRAM_MINOR,
-	"nvram",
-	&nvram_fops
-};
-
-int __init nvram_init(void)
-{
-	printk(KERN_INFO "Macintosh non-volatile memory driver v%s\n",
-		NVRAM_VERSION);
-	return misc_register(&nvram_dev);
-}
-
-void __exit nvram_cleanup(void)
-{
-        misc_deregister( &nvram_dev );
-}
-
-module_init(nvram_init);
-module_exit(nvram_cleanup);
-MODULE_LICENSE("GPL");
diff --git a/drivers/mailbox/Kconfig b/drivers/mailbox/Kconfig
index 84b0a2d74d60..e269f084497d 100644
--- a/drivers/mailbox/Kconfig
+++ b/drivers/mailbox/Kconfig
@@ -56,8 +56,18 @@ config PCC
 
 config ALTERA_MBOX
 	tristate "Altera Mailbox"
+	depends on HAS_IOMEM
 	help
 	  An implementation of the Altera Mailbox soft core. It is used
 	  to send message between processors. Say Y here if you want to use the
 	  Altera mailbox support.
+
+config BCM2835_MBOX
+	tristate "BCM2835 Mailbox"
+	depends on ARCH_BCM2835
+	help
+	  An implementation of the BCM2385 Mailbox.  It is used to invoke
+	  the services of the Videocore. Say Y here if you want to use the
+	  BCM2835 Mailbox.
+
 endif
diff --git a/drivers/mailbox/Makefile b/drivers/mailbox/Makefile
index b18201e97e29..8e6d82218a09 100644
--- a/drivers/mailbox/Makefile
+++ b/drivers/mailbox/Makefile
@@ -11,3 +11,5 @@ obj-$(CONFIG_OMAP2PLUS_MBOX)	+= omap-mailbox.o
 obj-$(CONFIG_PCC)		+= pcc.o
 
 obj-$(CONFIG_ALTERA_MBOX)	+= mailbox-altera.o
+
+obj-$(CONFIG_BCM2835_MBOX)	+= bcm2835-mailbox.o
diff --git a/drivers/mailbox/arm_mhu.c b/drivers/mailbox/arm_mhu.c
index ac693c635357..d9e99f981aa9 100644
--- a/drivers/mailbox/arm_mhu.c
+++ b/drivers/mailbox/arm_mhu.c
@@ -110,7 +110,7 @@ static void mhu_shutdown(struct mbox_chan *chan)
 	free_irq(mlink->irq, chan);
 }
 
-static struct mbox_chan_ops mhu_ops = {
+static const struct mbox_chan_ops mhu_ops = {
 	.send_data = mhu_send_data,
 	.startup = mhu_startup,
 	.shutdown = mhu_shutdown,
diff --git a/drivers/mailbox/bcm2835-mailbox.c b/drivers/mailbox/bcm2835-mailbox.c
new file mode 100644
index 000000000000..0b47dd42f3bd
--- /dev/null
+++ b/drivers/mailbox/bcm2835-mailbox.c
@@ -0,0 +1,217 @@
+/*
+ *  Copyright (C) 2010,2015 Broadcom
+ *  Copyright (C) 2013-2014 Lubomir Rintel
+ *  Copyright (C) 2013 Craig McGeachie
+ *
+ * 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 device provides a mechanism for writing to the mailboxes,
+ * that are shared between the ARM and the VideoCore processor
+ *
+ * Parts of the driver are based on:
+ *  - arch/arm/mach-bcm2708/vcio.c file written by Gray Girling that was
+ *    obtained from branch "rpi-3.6.y" of git://github.com/raspberrypi/
+ *    linux.git
+ *  - drivers/mailbox/bcm2835-ipc.c by Lubomir Rintel at
+ *    https://github.com/hackerspace/rpi-linux/blob/lr-raspberry-pi/drivers/
+ *    mailbox/bcm2835-ipc.c
+ *  - documentation available on the following web site:
+ *    https://github.com/raspberrypi/firmware/wiki/Mailbox-property-interface
+ */
+
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/mailbox_controller.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+
+/* Mailboxes */
+#define ARM_0_MAIL0	0x00
+#define ARM_0_MAIL1	0x20
+
+/*
+ * Mailbox registers. We basically only support mailbox 0 & 1. We
+ * deliver to the VC in mailbox 1, it delivers to us in mailbox 0. See
+ * BCM2835-ARM-Peripherals.pdf section 1.3 for an explanation about
+ * the placement of memory barriers.
+ */
+#define MAIL0_RD	(ARM_0_MAIL0 + 0x00)
+#define MAIL0_POL	(ARM_0_MAIL0 + 0x10)
+#define MAIL0_STA	(ARM_0_MAIL0 + 0x18)
+#define MAIL0_CNF	(ARM_0_MAIL0 + 0x1C)
+#define MAIL1_WRT	(ARM_0_MAIL1 + 0x00)
+#define MAIL1_STA	(ARM_0_MAIL1 + 0x18)
+
+/* Status register: FIFO state. */
+#define ARM_MS_FULL		BIT(31)
+#define ARM_MS_EMPTY		BIT(30)
+
+/* Configuration register: Enable interrupts. */
+#define ARM_MC_IHAVEDATAIRQEN	BIT(0)
+
+struct bcm2835_mbox {
+	void __iomem *regs;
+	spinlock_t lock;
+	struct mbox_controller controller;
+};
+
+static struct bcm2835_mbox *bcm2835_link_mbox(struct mbox_chan *link)
+{
+	return container_of(link->mbox, struct bcm2835_mbox, controller);
+}
+
+static irqreturn_t bcm2835_mbox_irq(int irq, void *dev_id)
+{
+	struct bcm2835_mbox *mbox = dev_id;
+	struct device *dev = mbox->controller.dev;
+	struct mbox_chan *link = &mbox->controller.chans[0];
+
+	while (!(readl(mbox->regs + MAIL0_STA) & ARM_MS_EMPTY)) {
+		u32 msg = readl(mbox->regs + MAIL0_RD);
+		dev_dbg(dev, "Reply 0x%08X\n", msg);
+		mbox_chan_received_data(link, &msg);
+	}
+	return IRQ_HANDLED;
+}
+
+static int bcm2835_send_data(struct mbox_chan *link, void *data)
+{
+	struct bcm2835_mbox *mbox = bcm2835_link_mbox(link);
+	u32 msg = *(u32 *)data;
+
+	spin_lock(&mbox->lock);
+	writel(msg, mbox->regs + MAIL1_WRT);
+	dev_dbg(mbox->controller.dev, "Request 0x%08X\n", msg);
+	spin_unlock(&mbox->lock);
+	return 0;
+}
+
+static int bcm2835_startup(struct mbox_chan *link)
+{
+	struct bcm2835_mbox *mbox = bcm2835_link_mbox(link);
+
+	/* Enable the interrupt on data reception */
+	writel(ARM_MC_IHAVEDATAIRQEN, mbox->regs + MAIL0_CNF);
+
+	return 0;
+}
+
+static void bcm2835_shutdown(struct mbox_chan *link)
+{
+	struct bcm2835_mbox *mbox = bcm2835_link_mbox(link);
+
+	writel(0, mbox->regs + MAIL0_CNF);
+}
+
+static bool bcm2835_last_tx_done(struct mbox_chan *link)
+{
+	struct bcm2835_mbox *mbox = bcm2835_link_mbox(link);
+	bool ret;
+
+	spin_lock(&mbox->lock);
+	ret = !(readl(mbox->regs + MAIL1_STA) & ARM_MS_FULL);
+	spin_unlock(&mbox->lock);
+	return ret;
+}
+
+static const struct mbox_chan_ops bcm2835_mbox_chan_ops = {
+	.send_data	= bcm2835_send_data,
+	.startup	= bcm2835_startup,
+	.shutdown	= bcm2835_shutdown,
+	.last_tx_done	= bcm2835_last_tx_done
+};
+
+static struct mbox_chan *bcm2835_mbox_index_xlate(struct mbox_controller *mbox,
+		    const struct of_phandle_args *sp)
+{
+	if (sp->args_count != 0)
+		return NULL;
+
+	return &mbox->chans[0];
+}
+
+static int bcm2835_mbox_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	int ret = 0;
+	struct resource *iomem;
+	struct bcm2835_mbox *mbox;
+
+	mbox = devm_kzalloc(dev, sizeof(*mbox), GFP_KERNEL);
+	if (mbox == NULL)
+		return -ENOMEM;
+	spin_lock_init(&mbox->lock);
+
+	ret = devm_request_irq(dev, irq_of_parse_and_map(dev->of_node, 0),
+			       bcm2835_mbox_irq, 0, dev_name(dev), mbox);
+	if (ret) {
+		dev_err(dev, "Failed to register a mailbox IRQ handler: %d\n",
+			ret);
+		return -ENODEV;
+	}
+
+	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mbox->regs = devm_ioremap_resource(&pdev->dev, iomem);
+	if (IS_ERR(mbox->regs)) {
+		ret = PTR_ERR(mbox->regs);
+		dev_err(&pdev->dev, "Failed to remap mailbox regs: %d\n", ret);
+		return ret;
+	}
+
+	mbox->controller.txdone_poll = true;
+	mbox->controller.txpoll_period = 5;
+	mbox->controller.ops = &bcm2835_mbox_chan_ops;
+	mbox->controller.of_xlate = &bcm2835_mbox_index_xlate;
+	mbox->controller.dev = dev;
+	mbox->controller.num_chans = 1;
+	mbox->controller.chans = devm_kzalloc(dev,
+		sizeof(*mbox->controller.chans), GFP_KERNEL);
+	if (!mbox->controller.chans)
+		return -ENOMEM;
+
+	ret = mbox_controller_register(&mbox->controller);
+	if (ret)
+		return ret;
+
+	platform_set_drvdata(pdev, mbox);
+	dev_info(dev, "mailbox enabled\n");
+
+	return ret;
+}
+
+static int bcm2835_mbox_remove(struct platform_device *pdev)
+{
+	struct bcm2835_mbox *mbox = platform_get_drvdata(pdev);
+	mbox_controller_unregister(&mbox->controller);
+	return 0;
+}
+
+static const struct of_device_id bcm2835_mbox_of_match[] = {
+	{ .compatible = "brcm,bcm2835-mbox", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, bcm2835_mbox_of_match);
+
+static struct platform_driver bcm2835_mbox_driver = {
+	.driver = {
+		.name = "bcm2835-mbox",
+		.owner = THIS_MODULE,
+		.of_match_table = bcm2835_mbox_of_match,
+	},
+	.probe		= bcm2835_mbox_probe,
+	.remove		= bcm2835_mbox_remove,
+};
+module_platform_driver(bcm2835_mbox_driver);
+
+MODULE_AUTHOR("Lubomir Rintel <lkundrak@v3.sk>");
+MODULE_DESCRIPTION("BCM2835 mailbox IPC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mailbox/mailbox-altera.c b/drivers/mailbox/mailbox-altera.c
index a266265677d3..bb682c926b0a 100644
--- a/drivers/mailbox/mailbox-altera.c
+++ b/drivers/mailbox/mailbox-altera.c
@@ -285,7 +285,7 @@ static void altera_mbox_shutdown(struct mbox_chan *chan)
 	}
 }
 
-static struct mbox_chan_ops altera_mbox_ops = {
+static const struct mbox_chan_ops altera_mbox_ops = {
 	.send_data = altera_mbox_send_data,
 	.startup = altera_mbox_startup,
 	.shutdown = altera_mbox_shutdown,
diff --git a/drivers/mailbox/mailbox.c b/drivers/mailbox/mailbox.c
index 19b491d2964f..c7fdb57fd166 100644
--- a/drivers/mailbox/mailbox.c
+++ b/drivers/mailbox/mailbox.c
@@ -318,7 +318,7 @@ struct mbox_chan *mbox_request_channel(struct mbox_client *cl, int index)
 		return ERR_PTR(-ENODEV);
 	}
 
-	chan = NULL;
+	chan = ERR_PTR(-EPROBE_DEFER);
 	list_for_each_entry(mbox, &mbox_cons, node)
 		if (mbox->dev->of_node == spec.np) {
 			chan = mbox->of_xlate(mbox, &spec);
@@ -327,7 +327,12 @@ struct mbox_chan *mbox_request_channel(struct mbox_client *cl, int index)
 
 	of_node_put(spec.np);
 
-	if (!chan || chan->cl || !try_module_get(mbox->dev->driver->owner)) {
+	if (IS_ERR(chan)) {
+		mutex_unlock(&con_mutex);
+		return chan;
+	}
+
+	if (chan->cl || !try_module_get(mbox->dev->driver->owner)) {
 		dev_dbg(dev, "%s: mailbox not free\n", __func__);
 		mutex_unlock(&con_mutex);
 		return ERR_PTR(-EBUSY);
@@ -357,6 +362,35 @@ struct mbox_chan *mbox_request_channel(struct mbox_client *cl, int index)
 }
 EXPORT_SYMBOL_GPL(mbox_request_channel);
 
+struct mbox_chan *mbox_request_channel_byname(struct mbox_client *cl,
+					      const char *name)
+{
+	struct device_node *np = cl->dev->of_node;
+	struct property *prop;
+	const char *mbox_name;
+	int index = 0;
+
+	if (!np) {
+		dev_err(cl->dev, "%s() currently only supports DT\n", __func__);
+		return ERR_PTR(-ENOSYS);
+	}
+
+	if (!of_get_property(np, "mbox-names", NULL)) {
+		dev_err(cl->dev,
+			"%s() requires an \"mbox-names\" property\n", __func__);
+		return ERR_PTR(-ENOSYS);
+	}
+
+	of_property_for_each_string(np, "mbox-names", prop, mbox_name) {
+		if (!strncmp(name, mbox_name, strlen(name)))
+			break;
+		index++;
+	}
+
+	return mbox_request_channel(cl, index);
+}
+EXPORT_SYMBOL_GPL(mbox_request_channel_byname);
+
 /**
  * mbox_free_channel - The client relinquishes control of a mailbox
  *			channel by this call.
@@ -390,7 +424,7 @@ of_mbox_index_xlate(struct mbox_controller *mbox,
 	int ind = sp->args[0];
 
 	if (ind >= mbox->num_chans)
-		return NULL;
+		return ERR_PTR(-EINVAL);
 
 	return &mbox->chans[ind];
 }
diff --git a/drivers/mailbox/omap-mailbox.c b/drivers/mailbox/omap-mailbox.c
index 0f332c178b07..a3dbfd9c6479 100644
--- a/drivers/mailbox/omap-mailbox.c
+++ b/drivers/mailbox/omap-mailbox.c
@@ -604,7 +604,7 @@ static int omap_mbox_chan_send_data(struct mbox_chan *chan, void *data)
 	return ret;
 }
 
-static struct mbox_chan_ops omap_mbox_chan_ops = {
+static const struct mbox_chan_ops omap_mbox_chan_ops = {
 	.startup        = omap_mbox_chan_startup,
 	.send_data      = omap_mbox_chan_send_data,
 	.shutdown       = omap_mbox_chan_shutdown,
@@ -639,18 +639,18 @@ static struct mbox_chan *omap_mbox_of_xlate(struct mbox_controller *controller,
 
 	mdev = container_of(controller, struct omap_mbox_device, controller);
 	if (WARN_ON(!mdev))
-		return NULL;
+		return ERR_PTR(-EINVAL);
 
 	node = of_find_node_by_phandle(phandle);
 	if (!node) {
 		pr_err("%s: could not find node phandle 0x%x\n",
 		       __func__, phandle);
-		return NULL;
+		return ERR_PTR(-ENODEV);
 	}
 
 	mbox = omap_mbox_device_find(mdev, node->name);
 	of_node_put(node);
-	return mbox ? mbox->chan : NULL;
+	return mbox ? mbox->chan : ERR_PTR(-ENOENT);
 }
 
 static int omap_mbox_probe(struct platform_device *pdev)
diff --git a/drivers/mailbox/pcc.c b/drivers/mailbox/pcc.c
index 7e91d68a3ac3..26d121d1d501 100644
--- a/drivers/mailbox/pcc.c
+++ b/drivers/mailbox/pcc.c
@@ -198,7 +198,7 @@ static int pcc_send_data(struct mbox_chan *chan, void *data)
 	return 0;
 }
 
-static struct mbox_chan_ops pcc_chan_ops = {
+static const struct mbox_chan_ops pcc_chan_ops = {
 	.send_data = pcc_send_data,
 };
 
diff --git a/drivers/mailbox/pl320-ipc.c b/drivers/mailbox/pl320-ipc.c
index f3755e0aa935..f80acb36ff07 100644
--- a/drivers/mailbox/pl320-ipc.c
+++ b/drivers/mailbox/pl320-ipc.c
@@ -195,4 +195,4 @@ static int __init ipc_init(void)
 {
 	return amba_driver_register(&pl320_driver);
 }
-module_init(ipc_init);
+subsys_initcall(ipc_init);
diff --git a/drivers/md/Kconfig b/drivers/md/Kconfig
index edcf4ab66e00..b59727309072 100644
--- a/drivers/md/Kconfig
+++ b/drivers/md/Kconfig
@@ -304,6 +304,18 @@ config DM_CACHE_MQ
          This is meant to be a general purpose policy.  It prioritises
          reads over writes.
 
+config DM_CACHE_SMQ
+       tristate "Stochastic MQ Cache Policy (EXPERIMENTAL)"
+       depends on DM_CACHE
+       default y
+       ---help---
+         A cache policy that uses a multiqueue ordered by recent hits
+         to select which blocks should be promoted and demoted.
+         This is meant to be a general purpose policy.  It prioritises
+         reads over writes.  This SMQ policy (vs MQ) offers the promise
+         of less memory utilization, improved performance and increased
+         adaptability in the face of changing workloads.
+
 config DM_CACHE_CLEANER
        tristate "Cleaner Cache Policy (EXPERIMENTAL)"
        depends on DM_CACHE
diff --git a/drivers/md/Makefile b/drivers/md/Makefile
index dba4db5985fb..462f443a4f85 100644
--- a/drivers/md/Makefile
+++ b/drivers/md/Makefile
@@ -13,6 +13,7 @@ dm-log-userspace-y \
 dm-thin-pool-y	+= dm-thin.o dm-thin-metadata.o
 dm-cache-y	+= dm-cache-target.o dm-cache-metadata.o dm-cache-policy.o
 dm-cache-mq-y   += dm-cache-policy-mq.o
+dm-cache-smq-y   += dm-cache-policy-smq.o
 dm-cache-cleaner-y += dm-cache-policy-cleaner.o
 dm-era-y	+= dm-era-target.o
 md-mod-y	+= md.o bitmap.o
@@ -54,6 +55,7 @@ obj-$(CONFIG_DM_THIN_PROVISIONING)	+= dm-thin-pool.o
 obj-$(CONFIG_DM_VERITY)		+= dm-verity.o
 obj-$(CONFIG_DM_CACHE)		+= dm-cache.o
 obj-$(CONFIG_DM_CACHE_MQ)	+= dm-cache-mq.o
+obj-$(CONFIG_DM_CACHE_SMQ)	+= dm-cache-smq.o
 obj-$(CONFIG_DM_CACHE_CLEANER)	+= dm-cache-cleaner.o
 obj-$(CONFIG_DM_ERA)		+= dm-era.o
 obj-$(CONFIG_DM_LOG_WRITES)	+= dm-log-writes.o
diff --git a/drivers/md/bcache/io.c b/drivers/md/bcache/io.c
index fa028fa82df4..cb64e64a4789 100644
--- a/drivers/md/bcache/io.c
+++ b/drivers/md/bcache/io.c
@@ -55,7 +55,7 @@ static void bch_bio_submit_split_done(struct closure *cl)
 
 	s->bio->bi_end_io = s->bi_end_io;
 	s->bio->bi_private = s->bi_private;
-	bio_endio_nodec(s->bio, 0);
+	bio_endio(s->bio, 0);
 
 	closure_debug_destroy(&s->cl);
 	mempool_free(s, s->p->bio_split_hook);
diff --git a/drivers/md/bcache/journal.c b/drivers/md/bcache/journal.c
index fe080ad0e558..ce64fc851251 100644
--- a/drivers/md/bcache/journal.c
+++ b/drivers/md/bcache/journal.c
@@ -157,7 +157,7 @@ int bch_journal_read(struct cache_set *c, struct list_head *list)
 
 	for_each_cache(ca, c, iter) {
 		struct journal_device *ja = &ca->journal;
-		unsigned long bitmap[SB_JOURNAL_BUCKETS / BITS_PER_LONG];
+		DECLARE_BITMAP(bitmap, SB_JOURNAL_BUCKETS);
 		unsigned i, l, r, m;
 		uint64_t seq;
 
diff --git a/drivers/md/bcache/request.c b/drivers/md/bcache/request.c
index ab43faddb447..4afb2d26b148 100644
--- a/drivers/md/bcache/request.c
+++ b/drivers/md/bcache/request.c
@@ -15,6 +15,7 @@
 #include <linux/module.h>
 #include <linux/hash.h>
 #include <linux/random.h>
+#include <linux/backing-dev.h>
 
 #include <trace/events/bcache.h>
 
@@ -619,7 +620,7 @@ static void do_bio_hook(struct search *s, struct bio *orig_bio)
 	bio->bi_end_io		= request_endio;
 	bio->bi_private		= &s->cl;
 
-	atomic_set(&bio->bi_cnt, 3);
+	bio_cnt_set(bio, 3);
 }
 
 static void search_free(struct closure *cl)
diff --git a/drivers/md/bcache/super.c b/drivers/md/bcache/super.c
index 4dd2bb7167f0..94980bfca434 100644
--- a/drivers/md/bcache/super.c
+++ b/drivers/md/bcache/super.c
@@ -760,14 +760,8 @@ static void bcache_device_free(struct bcache_device *d)
 	bio_split_pool_free(&d->bio_split_hook);
 	if (d->bio_split)
 		bioset_free(d->bio_split);
-	if (is_vmalloc_addr(d->full_dirty_stripes))
-		vfree(d->full_dirty_stripes);
-	else
-		kfree(d->full_dirty_stripes);
-	if (is_vmalloc_addr(d->stripe_sectors_dirty))
-		vfree(d->stripe_sectors_dirty);
-	else
-		kfree(d->stripe_sectors_dirty);
+	kvfree(d->full_dirty_stripes);
+	kvfree(d->stripe_sectors_dirty);
 
 	closure_debug_destroy(&d->cl);
 }
diff --git a/drivers/md/bcache/util.h b/drivers/md/bcache/util.h
index 98df7572b5f7..1d04c4859c70 100644
--- a/drivers/md/bcache/util.h
+++ b/drivers/md/bcache/util.h
@@ -52,10 +52,7 @@ struct closure;
 
 #define free_heap(heap)							\
 do {									\
-	if (is_vmalloc_addr((heap)->data))				\
-		vfree((heap)->data);					\
-	else								\
-		kfree((heap)->data);					\
+	kvfree((heap)->data);						\
 	(heap)->data = NULL;						\
 } while (0)
 
@@ -163,10 +160,7 @@ do {									\
 
 #define free_fifo(fifo)							\
 do {									\
-	if (is_vmalloc_addr((fifo)->data))				\
-		vfree((fifo)->data);					\
-	else								\
-		kfree((fifo)->data);					\
+	kvfree((fifo)->data);						\
 	(fifo)->data = NULL;						\
 } while (0)
 
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 135a0907e9de..ed2346ddf4c9 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -839,7 +839,7 @@ static void bitmap_file_kick(struct bitmap *bitmap)
 		if (bitmap->storage.file) {
 			path = kmalloc(PAGE_SIZE, GFP_KERNEL);
 			if (path)
-				ptr = d_path(&bitmap->storage.file->f_path,
+				ptr = file_path(bitmap->storage.file,
 					     path, PAGE_SIZE);
 
 			printk(KERN_ALERT
@@ -1927,7 +1927,7 @@ void bitmap_status(struct seq_file *seq, struct bitmap *bitmap)
 		   chunk_kb ? "KB" : "B");
 	if (bitmap->storage.file) {
 		seq_printf(seq, ", file: ");
-		seq_path(seq, &bitmap->storage.file->f_path, " \t\n");
+		seq_file_path(seq, bitmap->storage.file, " \t\n");
 	}
 
 	seq_printf(seq, "\n");
diff --git a/drivers/md/dm-bio-prison.c b/drivers/md/dm-bio-prison.c
index be065300e93c..cd6d1d21e057 100644
--- a/drivers/md/dm-bio-prison.c
+++ b/drivers/md/dm-bio-prison.c
@@ -255,6 +255,32 @@ void dm_cell_visit_release(struct dm_bio_prison *prison,
 }
 EXPORT_SYMBOL_GPL(dm_cell_visit_release);
 
+static int __promote_or_release(struct dm_bio_prison *prison,
+				struct dm_bio_prison_cell *cell)
+{
+	if (bio_list_empty(&cell->bios)) {
+		rb_erase(&cell->node, &prison->cells);
+		return 1;
+	}
+
+	cell->holder = bio_list_pop(&cell->bios);
+	return 0;
+}
+
+int dm_cell_promote_or_release(struct dm_bio_prison *prison,
+			       struct dm_bio_prison_cell *cell)
+{
+	int r;
+	unsigned long flags;
+
+	spin_lock_irqsave(&prison->lock, flags);
+	r = __promote_or_release(prison, cell);
+	spin_unlock_irqrestore(&prison->lock, flags);
+
+	return r;
+}
+EXPORT_SYMBOL_GPL(dm_cell_promote_or_release);
+
 /*----------------------------------------------------------------*/
 
 #define DEFERRED_SET_SIZE 64
diff --git a/drivers/md/dm-bio-prison.h b/drivers/md/dm-bio-prison.h
index 74cf01144b1f..54352f009bfd 100644
--- a/drivers/md/dm-bio-prison.h
+++ b/drivers/md/dm-bio-prison.h
@@ -101,6 +101,19 @@ void dm_cell_visit_release(struct dm_bio_prison *prison,
 			   void (*visit_fn)(void *, struct dm_bio_prison_cell *),
 			   void *context, struct dm_bio_prison_cell *cell);
 
+/*
+ * Rather than always releasing the prisoners in a cell, the client may
+ * want to promote one of them to be the new holder.  There is a race here
+ * though between releasing an empty cell, and other threads adding new
+ * inmates.  So this function makes the decision with its lock held.
+ *
+ * This function can have two outcomes:
+ * i) An inmate is promoted to be the holder of the cell (return value of 0).
+ * ii) The cell has no inmate for promotion and is released (return value of 1).
+ */
+int dm_cell_promote_or_release(struct dm_bio_prison *prison,
+			       struct dm_bio_prison_cell *cell);
+
 /*----------------------------------------------------------------*/
 
 /*
diff --git a/drivers/md/dm-cache-metadata.c b/drivers/md/dm-cache-metadata.c
index c1c010498a21..20cc36b01b77 100644
--- a/drivers/md/dm-cache-metadata.c
+++ b/drivers/md/dm-cache-metadata.c
@@ -39,6 +39,8 @@
 enum superblock_flag_bits {
 	/* for spotting crashes that would invalidate the dirty bitset */
 	CLEAN_SHUTDOWN,
+	/* metadata must be checked using the tools */
+	NEEDS_CHECK,
 };
 
 /*
@@ -107,6 +109,7 @@ struct dm_cache_metadata {
 	struct dm_disk_bitset discard_info;
 
 	struct rw_semaphore root_lock;
+	unsigned long flags;
 	dm_block_t root;
 	dm_block_t hint_root;
 	dm_block_t discard_root;
@@ -129,6 +132,14 @@ struct dm_cache_metadata {
 	 * buffer before the superblock is locked and updated.
 	 */
 	__u8 metadata_space_map_root[SPACE_MAP_ROOT_SIZE];
+
+	/*
+	 * Set if a transaction has to be aborted but the attempt to roll
+	 * back to the previous (good) transaction failed.  The only
+	 * metadata operation permissible in this state is the closing of
+	 * the device.
+	 */
+	bool fail_io:1;
 };
 
 /*-------------------------------------------------------------------
@@ -527,6 +538,7 @@ static unsigned long clear_clean_shutdown(unsigned long flags)
 static void read_superblock_fields(struct dm_cache_metadata *cmd,
 				   struct cache_disk_superblock *disk_super)
 {
+	cmd->flags = le32_to_cpu(disk_super->flags);
 	cmd->root = le64_to_cpu(disk_super->mapping_root);
 	cmd->hint_root = le64_to_cpu(disk_super->hint_root);
 	cmd->discard_root = le64_to_cpu(disk_super->discard_root);
@@ -625,6 +637,7 @@ static int __commit_transaction(struct dm_cache_metadata *cmd,
 	if (mutator)
 		update_flags(disk_super, mutator);
 
+	disk_super->flags = cpu_to_le32(cmd->flags);
 	disk_super->mapping_root = cpu_to_le64(cmd->root);
 	disk_super->hint_root = cpu_to_le64(cmd->hint_root);
 	disk_super->discard_root = cpu_to_le64(cmd->discard_root);
@@ -693,6 +706,7 @@ static struct dm_cache_metadata *metadata_open(struct block_device *bdev,
 	cmd->cache_blocks = 0;
 	cmd->policy_hint_size = policy_hint_size;
 	cmd->changed = true;
+	cmd->fail_io = false;
 
 	r = __create_persistent_data_objects(cmd, may_format_device);
 	if (r) {
@@ -796,7 +810,8 @@ void dm_cache_metadata_close(struct dm_cache_metadata *cmd)
 		list_del(&cmd->list);
 		mutex_unlock(&table_lock);
 
-		__destroy_persistent_data_objects(cmd);
+		if (!cmd->fail_io)
+			__destroy_persistent_data_objects(cmd);
 		kfree(cmd);
 	}
 }
@@ -848,13 +863,26 @@ static int blocks_are_unmapped_or_clean(struct dm_cache_metadata *cmd,
 	return 0;
 }
 
+#define WRITE_LOCK(cmd) \
+	if (cmd->fail_io || dm_bm_is_read_only(cmd->bm)) \
+		return -EINVAL; \
+	down_write(&cmd->root_lock)
+
+#define WRITE_LOCK_VOID(cmd) \
+	if (cmd->fail_io || dm_bm_is_read_only(cmd->bm)) \
+		return; \
+	down_write(&cmd->root_lock)
+
+#define WRITE_UNLOCK(cmd) \
+	up_write(&cmd->root_lock)
+
 int dm_cache_resize(struct dm_cache_metadata *cmd, dm_cblock_t new_cache_size)
 {
 	int r;
 	bool clean;
 	__le64 null_mapping = pack_value(0, 0);
 
-	down_write(&cmd->root_lock);
+	WRITE_LOCK(cmd);
 	__dm_bless_for_disk(&null_mapping);
 
 	if (from_cblock(new_cache_size) < from_cblock(cmd->cache_blocks)) {
@@ -880,7 +908,7 @@ int dm_cache_resize(struct dm_cache_metadata *cmd, dm_cblock_t new_cache_size)
 	cmd->changed = true;
 
 out:
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 
 	return r;
 }
@@ -891,7 +919,7 @@ int dm_cache_discard_bitset_resize(struct dm_cache_metadata *cmd,
 {
 	int r;
 
-	down_write(&cmd->root_lock);
+	WRITE_LOCK(cmd);
 	r = dm_bitset_resize(&cmd->discard_info,
 			     cmd->discard_root,
 			     from_dblock(cmd->discard_nr_blocks),
@@ -903,7 +931,7 @@ int dm_cache_discard_bitset_resize(struct dm_cache_metadata *cmd,
 	}
 
 	cmd->changed = true;
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 
 	return r;
 }
@@ -946,9 +974,9 @@ int dm_cache_set_discard(struct dm_cache_metadata *cmd,
 {
 	int r;
 
-	down_write(&cmd->root_lock);
+	WRITE_LOCK(cmd);
 	r = __discard(cmd, dblock, discard);
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 
 	return r;
 }
@@ -1020,9 +1048,9 @@ int dm_cache_remove_mapping(struct dm_cache_metadata *cmd, dm_cblock_t cblock)
 {
 	int r;
 
-	down_write(&cmd->root_lock);
+	WRITE_LOCK(cmd);
 	r = __remove(cmd, cblock);
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 
 	return r;
 }
@@ -1048,9 +1076,9 @@ int dm_cache_insert_mapping(struct dm_cache_metadata *cmd,
 {
 	int r;
 
-	down_write(&cmd->root_lock);
+	WRITE_LOCK(cmd);
 	r = __insert(cmd, cblock, oblock);
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 
 	return r;
 }
@@ -1234,9 +1262,9 @@ int dm_cache_set_dirty(struct dm_cache_metadata *cmd,
 {
 	int r;
 
-	down_write(&cmd->root_lock);
+	WRITE_LOCK(cmd);
 	r = __dirty(cmd, cblock, dirty);
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 
 	return r;
 }
@@ -1252,9 +1280,9 @@ void dm_cache_metadata_get_stats(struct dm_cache_metadata *cmd,
 void dm_cache_metadata_set_stats(struct dm_cache_metadata *cmd,
 				 struct dm_cache_statistics *stats)
 {
-	down_write(&cmd->root_lock);
+	WRITE_LOCK_VOID(cmd);
 	cmd->stats = *stats;
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 }
 
 int dm_cache_commit(struct dm_cache_metadata *cmd, bool clean_shutdown)
@@ -1263,7 +1291,7 @@ int dm_cache_commit(struct dm_cache_metadata *cmd, bool clean_shutdown)
 	flags_mutator mutator = (clean_shutdown ? set_clean_shutdown :
 				 clear_clean_shutdown);
 
-	down_write(&cmd->root_lock);
+	WRITE_LOCK(cmd);
 	r = __commit_transaction(cmd, mutator);
 	if (r)
 		goto out;
@@ -1271,7 +1299,7 @@ int dm_cache_commit(struct dm_cache_metadata *cmd, bool clean_shutdown)
 	r = __begin_transaction(cmd);
 
 out:
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 	return r;
 }
 
@@ -1376,9 +1404,9 @@ int dm_cache_write_hints(struct dm_cache_metadata *cmd, struct dm_cache_policy *
 {
 	int r;
 
-	down_write(&cmd->root_lock);
+	WRITE_LOCK(cmd);
 	r = write_hints(cmd, policy);
-	up_write(&cmd->root_lock);
+	WRITE_UNLOCK(cmd);
 
 	return r;
 }
@@ -1387,3 +1415,70 @@ int dm_cache_metadata_all_clean(struct dm_cache_metadata *cmd, bool *result)
 {
 	return blocks_are_unmapped_or_clean(cmd, 0, cmd->cache_blocks, result);
 }
+
+void dm_cache_metadata_set_read_only(struct dm_cache_metadata *cmd)
+{
+	WRITE_LOCK_VOID(cmd);
+	dm_bm_set_read_only(cmd->bm);
+	WRITE_UNLOCK(cmd);
+}
+
+void dm_cache_metadata_set_read_write(struct dm_cache_metadata *cmd)
+{
+	WRITE_LOCK_VOID(cmd);
+	dm_bm_set_read_write(cmd->bm);
+	WRITE_UNLOCK(cmd);
+}
+
+int dm_cache_metadata_set_needs_check(struct dm_cache_metadata *cmd)
+{
+	int r;
+	struct dm_block *sblock;
+	struct cache_disk_superblock *disk_super;
+
+	/*
+	 * We ignore fail_io for this function.
+	 */
+	down_write(&cmd->root_lock);
+	set_bit(NEEDS_CHECK, &cmd->flags);
+
+	r = superblock_lock(cmd, &sblock);
+	if (r) {
+		DMERR("couldn't read superblock");
+		goto out;
+	}
+
+	disk_super = dm_block_data(sblock);
+	disk_super->flags = cpu_to_le32(cmd->flags);
+
+	dm_bm_unlock(sblock);
+
+out:
+	up_write(&cmd->root_lock);
+	return r;
+}
+
+bool dm_cache_metadata_needs_check(struct dm_cache_metadata *cmd)
+{
+	bool needs_check;
+
+	down_read(&cmd->root_lock);
+	needs_check = !!test_bit(NEEDS_CHECK, &cmd->flags);
+	up_read(&cmd->root_lock);
+
+	return needs_check;
+}
+
+int dm_cache_metadata_abort(struct dm_cache_metadata *cmd)
+{
+	int r;
+
+	WRITE_LOCK(cmd);
+	__destroy_persistent_data_objects(cmd);
+	r = __create_persistent_data_objects(cmd, false);
+	if (r)
+		cmd->fail_io = true;
+	WRITE_UNLOCK(cmd);
+
+	return r;
+}
diff --git a/drivers/md/dm-cache-metadata.h b/drivers/md/dm-cache-metadata.h
index 4ecc403be283..2ffee21f318d 100644
--- a/drivers/md/dm-cache-metadata.h
+++ b/drivers/md/dm-cache-metadata.h
@@ -102,6 +102,10 @@ struct dm_cache_statistics {
 
 void dm_cache_metadata_get_stats(struct dm_cache_metadata *cmd,
 				 struct dm_cache_statistics *stats);
+
+/*
+ * 'void' because it's no big deal if it fails.
+ */
 void dm_cache_metadata_set_stats(struct dm_cache_metadata *cmd,
 				 struct dm_cache_statistics *stats);
 
@@ -133,6 +137,12 @@ int dm_cache_write_hints(struct dm_cache_metadata *cmd, struct dm_cache_policy *
  */
 int dm_cache_metadata_all_clean(struct dm_cache_metadata *cmd, bool *result);
 
+bool dm_cache_metadata_needs_check(struct dm_cache_metadata *cmd);
+int dm_cache_metadata_set_needs_check(struct dm_cache_metadata *cmd);
+void dm_cache_metadata_set_read_only(struct dm_cache_metadata *cmd);
+void dm_cache_metadata_set_read_write(struct dm_cache_metadata *cmd);
+int dm_cache_metadata_abort(struct dm_cache_metadata *cmd);
+
 /*----------------------------------------------------------------*/
 
 #endif /* DM_CACHE_METADATA_H */
diff --git a/drivers/md/dm-cache-policy-cleaner.c b/drivers/md/dm-cache-policy-cleaner.c
index b04d1f904d07..240c9f0e85e7 100644
--- a/drivers/md/dm-cache-policy-cleaner.c
+++ b/drivers/md/dm-cache-policy-cleaner.c
@@ -171,7 +171,8 @@ static void remove_cache_hash_entry(struct wb_cache_entry *e)
 /* Public interface (see dm-cache-policy.h */
 static int wb_map(struct dm_cache_policy *pe, dm_oblock_t oblock,
 		  bool can_block, bool can_migrate, bool discarded_oblock,
-		  struct bio *bio, struct policy_result *result)
+		  struct bio *bio, struct policy_locker *locker,
+		  struct policy_result *result)
 {
 	struct policy *p = to_policy(pe);
 	struct wb_cache_entry *e;
@@ -358,7 +359,8 @@ static struct wb_cache_entry *get_next_dirty_entry(struct policy *p)
 
 static int wb_writeback_work(struct dm_cache_policy *pe,
 			     dm_oblock_t *oblock,
-			     dm_cblock_t *cblock)
+			     dm_cblock_t *cblock,
+			     bool critical_only)
 {
 	int r = -ENOENT;
 	struct policy *p = to_policy(pe);
diff --git a/drivers/md/dm-cache-policy-internal.h b/drivers/md/dm-cache-policy-internal.h
index 2256a1f24f73..2816018faa7f 100644
--- a/drivers/md/dm-cache-policy-internal.h
+++ b/drivers/md/dm-cache-policy-internal.h
@@ -7,6 +7,7 @@
 #ifndef DM_CACHE_POLICY_INTERNAL_H
 #define DM_CACHE_POLICY_INTERNAL_H
 
+#include <linux/vmalloc.h>
 #include "dm-cache-policy.h"
 
 /*----------------------------------------------------------------*/
@@ -16,9 +17,10 @@
  */
 static inline int policy_map(struct dm_cache_policy *p, dm_oblock_t oblock,
 			     bool can_block, bool can_migrate, bool discarded_oblock,
-			     struct bio *bio, struct policy_result *result)
+			     struct bio *bio, struct policy_locker *locker,
+			     struct policy_result *result)
 {
-	return p->map(p, oblock, can_block, can_migrate, discarded_oblock, bio, result);
+	return p->map(p, oblock, can_block, can_migrate, discarded_oblock, bio, locker, result);
 }
 
 static inline int policy_lookup(struct dm_cache_policy *p, dm_oblock_t oblock, dm_cblock_t *cblock)
@@ -54,9 +56,10 @@ static inline int policy_walk_mappings(struct dm_cache_policy *p,
 
 static inline int policy_writeback_work(struct dm_cache_policy *p,
 					dm_oblock_t *oblock,
-					dm_cblock_t *cblock)
+					dm_cblock_t *cblock,
+					bool critical_only)
 {
-	return p->writeback_work ? p->writeback_work(p, oblock, cblock) : -ENOENT;
+	return p->writeback_work ? p->writeback_work(p, oblock, cblock, critical_only) : -ENOENT;
 }
 
 static inline void policy_remove_mapping(struct dm_cache_policy *p, dm_oblock_t oblock)
@@ -80,19 +83,21 @@ static inline dm_cblock_t policy_residency(struct dm_cache_policy *p)
 	return p->residency(p);
 }
 
-static inline void policy_tick(struct dm_cache_policy *p)
+static inline void policy_tick(struct dm_cache_policy *p, bool can_block)
 {
 	if (p->tick)
-		return p->tick(p);
+		return p->tick(p, can_block);
 }
 
-static inline int policy_emit_config_values(struct dm_cache_policy *p, char *result, unsigned maxlen)
+static inline int policy_emit_config_values(struct dm_cache_policy *p, char *result,
+					    unsigned maxlen, ssize_t *sz_ptr)
 {
-	ssize_t sz = 0;
+	ssize_t sz = *sz_ptr;
 	if (p->emit_config_values)
-		return p->emit_config_values(p, result, maxlen);
+		return p->emit_config_values(p, result, maxlen, sz_ptr);
 
-	DMEMIT("0");
+	DMEMIT("0 ");
+	*sz_ptr = sz;
 	return 0;
 }
 
@@ -105,6 +110,33 @@ static inline int policy_set_config_value(struct dm_cache_policy *p,
 /*----------------------------------------------------------------*/
 
 /*
+ * Some utility functions commonly used by policies and the core target.
+ */
+static inline size_t bitset_size_in_bytes(unsigned nr_entries)
+{
+	return sizeof(unsigned long) * dm_div_up(nr_entries, BITS_PER_LONG);
+}
+
+static inline unsigned long *alloc_bitset(unsigned nr_entries)
+{
+	size_t s = bitset_size_in_bytes(nr_entries);
+	return vzalloc(s);
+}
+
+static inline void clear_bitset(void *bitset, unsigned nr_entries)
+{
+	size_t s = bitset_size_in_bytes(nr_entries);
+	memset(bitset, 0, s);
+}
+
+static inline void free_bitset(unsigned long *bits)
+{
+	vfree(bits);
+}
+
+/*----------------------------------------------------------------*/
+
+/*
  * Creates a new cache policy given a policy name, a cache size, an origin size and the block size.
  */
 struct dm_cache_policy *dm_cache_policy_create(const char *name, dm_cblock_t cache_size,
diff --git a/drivers/md/dm-cache-policy-mq.c b/drivers/md/dm-cache-policy-mq.c
index 3ddd1162334d..32814371b8d3 100644
--- a/drivers/md/dm-cache-policy-mq.c
+++ b/drivers/md/dm-cache-policy-mq.c
@@ -693,9 +693,10 @@ static void requeue(struct mq_policy *mq, struct entry *e)
  * - set the hit count to a hard coded value other than 1, eg, is it better
  *   if it goes in at level 2?
  */
-static int demote_cblock(struct mq_policy *mq, dm_oblock_t *oblock)
+static int demote_cblock(struct mq_policy *mq,
+			 struct policy_locker *locker, dm_oblock_t *oblock)
 {
-	struct entry *demoted = pop(mq, &mq->cache_clean);
+	struct entry *demoted = peek(&mq->cache_clean);
 
 	if (!demoted)
 		/*
@@ -707,6 +708,13 @@ static int demote_cblock(struct mq_policy *mq, dm_oblock_t *oblock)
 		 */
 		return -ENOSPC;
 
+	if (locker->fn(locker, demoted->oblock))
+		/*
+		 * We couldn't lock the demoted block.
+		 */
+		return -EBUSY;
+
+	del(mq, demoted);
 	*oblock = demoted->oblock;
 	free_entry(&mq->cache_pool, demoted);
 
@@ -795,6 +803,7 @@ static int cache_entry_found(struct mq_policy *mq,
  * finding which cache block to use.
  */
 static int pre_cache_to_cache(struct mq_policy *mq, struct entry *e,
+			      struct policy_locker *locker,
 			      struct policy_result *result)
 {
 	int r;
@@ -803,11 +812,12 @@ static int pre_cache_to_cache(struct mq_policy *mq, struct entry *e,
 	/* Ensure there's a free cblock in the cache */
 	if (epool_empty(&mq->cache_pool)) {
 		result->op = POLICY_REPLACE;
-		r = demote_cblock(mq, &result->old_oblock);
+		r = demote_cblock(mq, locker, &result->old_oblock);
 		if (r) {
 			result->op = POLICY_MISS;
 			return 0;
 		}
+
 	} else
 		result->op = POLICY_NEW;
 
@@ -829,7 +839,8 @@ static int pre_cache_to_cache(struct mq_policy *mq, struct entry *e,
 
 static int pre_cache_entry_found(struct mq_policy *mq, struct entry *e,
 				 bool can_migrate, bool discarded_oblock,
-				 int data_dir, struct policy_result *result)
+				 int data_dir, struct policy_locker *locker,
+				 struct policy_result *result)
 {
 	int r = 0;
 
@@ -842,7 +853,7 @@ static int pre_cache_entry_found(struct mq_policy *mq, struct entry *e,
 
 	else {
 		requeue(mq, e);
-		r = pre_cache_to_cache(mq, e, result);
+		r = pre_cache_to_cache(mq, e, locker, result);
 	}
 
 	return r;
@@ -872,6 +883,7 @@ static void insert_in_pre_cache(struct mq_policy *mq,
 }
 
 static void insert_in_cache(struct mq_policy *mq, dm_oblock_t oblock,
+			    struct policy_locker *locker,
 			    struct policy_result *result)
 {
 	int r;
@@ -879,7 +891,7 @@ static void insert_in_cache(struct mq_policy *mq, dm_oblock_t oblock,
 
 	if (epool_empty(&mq->cache_pool)) {
 		result->op = POLICY_REPLACE;
-		r = demote_cblock(mq, &result->old_oblock);
+		r = demote_cblock(mq, locker, &result->old_oblock);
 		if (unlikely(r)) {
 			result->op = POLICY_MISS;
 			insert_in_pre_cache(mq, oblock);
@@ -907,11 +919,12 @@ static void insert_in_cache(struct mq_policy *mq, dm_oblock_t oblock,
 
 static int no_entry_found(struct mq_policy *mq, dm_oblock_t oblock,
 			  bool can_migrate, bool discarded_oblock,
-			  int data_dir, struct policy_result *result)
+			  int data_dir, struct policy_locker *locker,
+			  struct policy_result *result)
 {
 	if (adjusted_promote_threshold(mq, discarded_oblock, data_dir) <= 1) {
 		if (can_migrate)
-			insert_in_cache(mq, oblock, result);
+			insert_in_cache(mq, oblock, locker, result);
 		else
 			return -EWOULDBLOCK;
 	} else {
@@ -928,7 +941,8 @@ static int no_entry_found(struct mq_policy *mq, dm_oblock_t oblock,
  */
 static int map(struct mq_policy *mq, dm_oblock_t oblock,
 	       bool can_migrate, bool discarded_oblock,
-	       int data_dir, struct policy_result *result)
+	       int data_dir, struct policy_locker *locker,
+	       struct policy_result *result)
 {
 	int r = 0;
 	struct entry *e = hash_lookup(mq, oblock);
@@ -942,11 +956,11 @@ static int map(struct mq_policy *mq, dm_oblock_t oblock,
 
 	else if (e)
 		r = pre_cache_entry_found(mq, e, can_migrate, discarded_oblock,
-					  data_dir, result);
+					  data_dir, locker, result);
 
 	else
 		r = no_entry_found(mq, oblock, can_migrate, discarded_oblock,
-				   data_dir, result);
+				   data_dir, locker, result);
 
 	if (r == -EWOULDBLOCK)
 		result->op = POLICY_MISS;
@@ -1012,7 +1026,8 @@ static void copy_tick(struct mq_policy *mq)
 
 static int mq_map(struct dm_cache_policy *p, dm_oblock_t oblock,
 		  bool can_block, bool can_migrate, bool discarded_oblock,
-		  struct bio *bio, struct policy_result *result)
+		  struct bio *bio, struct policy_locker *locker,
+		  struct policy_result *result)
 {
 	int r;
 	struct mq_policy *mq = to_mq_policy(p);
@@ -1028,7 +1043,7 @@ static int mq_map(struct dm_cache_policy *p, dm_oblock_t oblock,
 
 	iot_examine_bio(&mq->tracker, bio);
 	r = map(mq, oblock, can_migrate, discarded_oblock,
-		bio_data_dir(bio), result);
+		bio_data_dir(bio), locker, result);
 
 	mutex_unlock(&mq->lock);
 
@@ -1221,7 +1236,7 @@ static int __mq_writeback_work(struct mq_policy *mq, dm_oblock_t *oblock,
 }
 
 static int mq_writeback_work(struct dm_cache_policy *p, dm_oblock_t *oblock,
-			     dm_cblock_t *cblock)
+			     dm_cblock_t *cblock, bool critical_only)
 {
 	int r;
 	struct mq_policy *mq = to_mq_policy(p);
@@ -1268,7 +1283,7 @@ static dm_cblock_t mq_residency(struct dm_cache_policy *p)
 	return r;
 }
 
-static void mq_tick(struct dm_cache_policy *p)
+static void mq_tick(struct dm_cache_policy *p, bool can_block)
 {
 	struct mq_policy *mq = to_mq_policy(p);
 	unsigned long flags;
@@ -1276,6 +1291,12 @@ static void mq_tick(struct dm_cache_policy *p)
 	spin_lock_irqsave(&mq->tick_lock, flags);
 	mq->tick_protected++;
 	spin_unlock_irqrestore(&mq->tick_lock, flags);
+
+	if (can_block) {
+		mutex_lock(&mq->lock);
+		copy_tick(mq);
+		mutex_unlock(&mq->lock);
+	}
 }
 
 static int mq_set_config_value(struct dm_cache_policy *p,
@@ -1308,22 +1329,24 @@ static int mq_set_config_value(struct dm_cache_policy *p,
 	return 0;
 }
 
-static int mq_emit_config_values(struct dm_cache_policy *p, char *result, unsigned maxlen)
+static int mq_emit_config_values(struct dm_cache_policy *p, char *result,
+				 unsigned maxlen, ssize_t *sz_ptr)
 {
-	ssize_t sz = 0;
+	ssize_t sz = *sz_ptr;
 	struct mq_policy *mq = to_mq_policy(p);
 
 	DMEMIT("10 random_threshold %u "
 	       "sequential_threshold %u "
 	       "discard_promote_adjustment %u "
 	       "read_promote_adjustment %u "
-	       "write_promote_adjustment %u",
+	       "write_promote_adjustment %u ",
 	       mq->tracker.thresholds[PATTERN_RANDOM],
 	       mq->tracker.thresholds[PATTERN_SEQUENTIAL],
 	       mq->discard_promote_adjustment,
 	       mq->read_promote_adjustment,
 	       mq->write_promote_adjustment);
 
+	*sz_ptr = sz;
 	return 0;
 }
 
@@ -1408,21 +1431,12 @@ bad_pre_cache_init:
 
 static struct dm_cache_policy_type mq_policy_type = {
 	.name = "mq",
-	.version = {1, 3, 0},
+	.version = {1, 4, 0},
 	.hint_size = 4,
 	.owner = THIS_MODULE,
 	.create = mq_create
 };
 
-static struct dm_cache_policy_type default_policy_type = {
-	.name = "default",
-	.version = {1, 3, 0},
-	.hint_size = 4,
-	.owner = THIS_MODULE,
-	.create = mq_create,
-	.real = &mq_policy_type
-};
-
 static int __init mq_init(void)
 {
 	int r;
@@ -1432,36 +1446,21 @@ static int __init mq_init(void)
 					   __alignof__(struct entry),
 					   0, NULL);
 	if (!mq_entry_cache)
-		goto bad;
+		return -ENOMEM;
 
 	r = dm_cache_policy_register(&mq_policy_type);
 	if (r) {
 		DMERR("register failed %d", r);
-		goto bad_register_mq;
-	}
-
-	r = dm_cache_policy_register(&default_policy_type);
-	if (!r) {
-		DMINFO("version %u.%u.%u loaded",
-		       mq_policy_type.version[0],
-		       mq_policy_type.version[1],
-		       mq_policy_type.version[2]);
-		return 0;
+		kmem_cache_destroy(mq_entry_cache);
+		return -ENOMEM;
 	}
 
-	DMERR("register failed (as default) %d", r);
-
-	dm_cache_policy_unregister(&mq_policy_type);
-bad_register_mq:
-	kmem_cache_destroy(mq_entry_cache);
-bad:
-	return -ENOMEM;
+	return 0;
 }
 
 static void __exit mq_exit(void)
 {
 	dm_cache_policy_unregister(&mq_policy_type);
-	dm_cache_policy_unregister(&default_policy_type);
 
 	kmem_cache_destroy(mq_entry_cache);
 }
diff --git a/drivers/md/dm-cache-policy-smq.c b/drivers/md/dm-cache-policy-smq.c
new file mode 100644
index 000000000000..b6f22651dd35
--- /dev/null
+++ b/drivers/md/dm-cache-policy-smq.c
@@ -0,0 +1,1791 @@
+/*
+ * Copyright (C) 2015 Red Hat. All rights reserved.
+ *
+ * This file is released under the GPL.
+ */
+
+#include "dm-cache-policy.h"
+#include "dm-cache-policy-internal.h"
+#include "dm.h"
+
+#include <linux/hash.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/vmalloc.h>
+#include <linux/math64.h>
+
+#define DM_MSG_PREFIX "cache-policy-smq"
+
+/*----------------------------------------------------------------*/
+
+/*
+ * Safe division functions that return zero on divide by zero.
+ */
+static unsigned safe_div(unsigned n, unsigned d)
+{
+	return d ? n / d : 0u;
+}
+
+static unsigned safe_mod(unsigned n, unsigned d)
+{
+	return d ? n % d : 0u;
+}
+
+/*----------------------------------------------------------------*/
+
+struct entry {
+	unsigned hash_next:28;
+	unsigned prev:28;
+	unsigned next:28;
+	unsigned level:7;
+	bool dirty:1;
+	bool allocated:1;
+	bool sentinel:1;
+
+	dm_oblock_t oblock;
+};
+
+/*----------------------------------------------------------------*/
+
+#define INDEXER_NULL ((1u << 28u) - 1u)
+
+/*
+ * An entry_space manages a set of entries that we use for the queues.
+ * The clean and dirty queues share entries, so this object is separate
+ * from the queue itself.
+ */
+struct entry_space {
+	struct entry *begin;
+	struct entry *end;
+};
+
+static int space_init(struct entry_space *es, unsigned nr_entries)
+{
+	if (!nr_entries) {
+		es->begin = es->end = NULL;
+		return 0;
+	}
+
+	es->begin = vzalloc(sizeof(struct entry) * nr_entries);
+	if (!es->begin)
+		return -ENOMEM;
+
+	es->end = es->begin + nr_entries;
+	return 0;
+}
+
+static void space_exit(struct entry_space *es)
+{
+	vfree(es->begin);
+}
+
+static struct entry *__get_entry(struct entry_space *es, unsigned block)
+{
+	struct entry *e;
+
+	e = es->begin + block;
+	BUG_ON(e >= es->end);
+
+	return e;
+}
+
+static unsigned to_index(struct entry_space *es, struct entry *e)
+{
+	BUG_ON(e < es->begin || e >= es->end);
+	return e - es->begin;
+}
+
+static struct entry *to_entry(struct entry_space *es, unsigned block)
+{
+	if (block == INDEXER_NULL)
+		return NULL;
+
+	return __get_entry(es, block);
+}
+
+/*----------------------------------------------------------------*/
+
+struct ilist {
+	unsigned nr_elts;	/* excluding sentinel entries */
+	unsigned head, tail;
+};
+
+static void l_init(struct ilist *l)
+{
+	l->nr_elts = 0;
+	l->head = l->tail = INDEXER_NULL;
+}
+
+static struct entry *l_head(struct entry_space *es, struct ilist *l)
+{
+	return to_entry(es, l->head);
+}
+
+static struct entry *l_tail(struct entry_space *es, struct ilist *l)
+{
+	return to_entry(es, l->tail);
+}
+
+static struct entry *l_next(struct entry_space *es, struct entry *e)
+{
+	return to_entry(es, e->next);
+}
+
+static struct entry *l_prev(struct entry_space *es, struct entry *e)
+{
+	return to_entry(es, e->prev);
+}
+
+static bool l_empty(struct ilist *l)
+{
+	return l->head == INDEXER_NULL;
+}
+
+static void l_add_head(struct entry_space *es, struct ilist *l, struct entry *e)
+{
+	struct entry *head = l_head(es, l);
+
+	e->next = l->head;
+	e->prev = INDEXER_NULL;
+
+	if (head)
+		head->prev = l->head = to_index(es, e);
+	else
+		l->head = l->tail = to_index(es, e);
+
+	if (!e->sentinel)
+		l->nr_elts++;
+}
+
+static void l_add_tail(struct entry_space *es, struct ilist *l, struct entry *e)
+{
+	struct entry *tail = l_tail(es, l);
+
+	e->next = INDEXER_NULL;
+	e->prev = l->tail;
+
+	if (tail)
+		tail->next = l->tail = to_index(es, e);
+	else
+		l->head = l->tail = to_index(es, e);
+
+	if (!e->sentinel)
+		l->nr_elts++;
+}
+
+static void l_add_before(struct entry_space *es, struct ilist *l,
+			 struct entry *old, struct entry *e)
+{
+	struct entry *prev = l_prev(es, old);
+
+	if (!prev)
+		l_add_head(es, l, e);
+
+	else {
+		e->prev = old->prev;
+		e->next = to_index(es, old);
+		prev->next = old->prev = to_index(es, e);
+
+		if (!e->sentinel)
+			l->nr_elts++;
+	}
+}
+
+static void l_del(struct entry_space *es, struct ilist *l, struct entry *e)
+{
+	struct entry *prev = l_prev(es, e);
+	struct entry *next = l_next(es, e);
+
+	if (prev)
+		prev->next = e->next;
+	else
+		l->head = e->next;
+
+	if (next)
+		next->prev = e->prev;
+	else
+		l->tail = e->prev;
+
+	if (!e->sentinel)
+		l->nr_elts--;
+}
+
+static struct entry *l_pop_tail(struct entry_space *es, struct ilist *l)
+{
+	struct entry *e;
+
+	for (e = l_tail(es, l); e; e = l_prev(es, e))
+		if (!e->sentinel) {
+			l_del(es, l, e);
+			return e;
+		}
+
+	return NULL;
+}
+
+/*----------------------------------------------------------------*/
+
+/*
+ * The stochastic-multi-queue is a set of lru lists stacked into levels.
+ * Entries are moved up levels when they are used, which loosely orders the
+ * most accessed entries in the top levels and least in the bottom.  This
+ * structure is *much* better than a single lru list.
+ */
+#define MAX_LEVELS 64u
+
+struct queue {
+	struct entry_space *es;
+
+	unsigned nr_elts;
+	unsigned nr_levels;
+	struct ilist qs[MAX_LEVELS];
+
+	/*
+	 * We maintain a count of the number of entries we would like in each
+	 * level.
+	 */
+	unsigned last_target_nr_elts;
+	unsigned nr_top_levels;
+	unsigned nr_in_top_levels;
+	unsigned target_count[MAX_LEVELS];
+};
+
+static void q_init(struct queue *q, struct entry_space *es, unsigned nr_levels)
+{
+	unsigned i;
+
+	q->es = es;
+	q->nr_elts = 0;
+	q->nr_levels = nr_levels;
+
+	for (i = 0; i < q->nr_levels; i++) {
+		l_init(q->qs + i);
+		q->target_count[i] = 0u;
+	}
+
+	q->last_target_nr_elts = 0u;
+	q->nr_top_levels = 0u;
+	q->nr_in_top_levels = 0u;
+}
+
+static unsigned q_size(struct queue *q)
+{
+	return q->nr_elts;
+}
+
+/*
+ * Insert an entry to the back of the given level.
+ */
+static void q_push(struct queue *q, struct entry *e)
+{
+	if (!e->sentinel)
+		q->nr_elts++;
+
+	l_add_tail(q->es, q->qs + e->level, e);
+}
+
+static void q_push_before(struct queue *q, struct entry *old, struct entry *e)
+{
+	if (!e->sentinel)
+		q->nr_elts++;
+
+	l_add_before(q->es, q->qs + e->level, old, e);
+}
+
+static void q_del(struct queue *q, struct entry *e)
+{
+	l_del(q->es, q->qs + e->level, e);
+	if (!e->sentinel)
+		q->nr_elts--;
+}
+
+/*
+ * Return the oldest entry of the lowest populated level.
+ */
+static struct entry *q_peek(struct queue *q, unsigned max_level, bool can_cross_sentinel)
+{
+	unsigned level;
+	struct entry *e;
+
+	max_level = min(max_level, q->nr_levels);
+
+	for (level = 0; level < max_level; level++)
+		for (e = l_head(q->es, q->qs + level); e; e = l_next(q->es, e)) {
+			if (e->sentinel) {
+				if (can_cross_sentinel)
+					continue;
+				else
+					break;
+			}
+
+			return e;
+		}
+
+	return NULL;
+}
+
+static struct entry *q_pop(struct queue *q)
+{
+	struct entry *e = q_peek(q, q->nr_levels, true);
+
+	if (e)
+		q_del(q, e);
+
+	return e;
+}
+
+/*
+ * Pops an entry from a level that is not past a sentinel.
+ */
+static struct entry *q_pop_old(struct queue *q, unsigned max_level)
+{
+	struct entry *e = q_peek(q, max_level, false);
+
+	if (e)
+		q_del(q, e);
+
+	return e;
+}
+
+/*
+ * This function assumes there is a non-sentinel entry to pop.  It's only
+ * used by redistribute, so we know this is true.  It also doesn't adjust
+ * the q->nr_elts count.
+ */
+static struct entry *__redist_pop_from(struct queue *q, unsigned level)
+{
+	struct entry *e;
+
+	for (; level < q->nr_levels; level++)
+		for (e = l_head(q->es, q->qs + level); e; e = l_next(q->es, e))
+			if (!e->sentinel) {
+				l_del(q->es, q->qs + e->level, e);
+				return e;
+			}
+
+	return NULL;
+}
+
+static void q_set_targets_subrange_(struct queue *q, unsigned nr_elts, unsigned lbegin, unsigned lend)
+{
+	unsigned level, nr_levels, entries_per_level, remainder;
+
+	BUG_ON(lbegin > lend);
+	BUG_ON(lend > q->nr_levels);
+	nr_levels = lend - lbegin;
+	entries_per_level = safe_div(nr_elts, nr_levels);
+	remainder = safe_mod(nr_elts, nr_levels);
+
+	for (level = lbegin; level < lend; level++)
+		q->target_count[level] =
+			(level < (lbegin + remainder)) ? entries_per_level + 1u : entries_per_level;
+}
+
+/*
+ * Typically we have fewer elements in the top few levels which allows us
+ * to adjust the promote threshold nicely.
+ */
+static void q_set_targets(struct queue *q)
+{
+	if (q->last_target_nr_elts == q->nr_elts)
+		return;
+
+	q->last_target_nr_elts = q->nr_elts;
+
+	if (q->nr_top_levels > q->nr_levels)
+		q_set_targets_subrange_(q, q->nr_elts, 0, q->nr_levels);
+
+	else {
+		q_set_targets_subrange_(q, q->nr_in_top_levels,
+					q->nr_levels - q->nr_top_levels, q->nr_levels);
+
+		if (q->nr_in_top_levels < q->nr_elts)
+			q_set_targets_subrange_(q, q->nr_elts - q->nr_in_top_levels,
+						0, q->nr_levels - q->nr_top_levels);
+		else
+			q_set_targets_subrange_(q, 0, 0, q->nr_levels - q->nr_top_levels);
+	}
+}
+
+static void q_redistribute(struct queue *q)
+{
+	unsigned target, level;
+	struct ilist *l, *l_above;
+	struct entry *e;
+
+	q_set_targets(q);
+
+	for (level = 0u; level < q->nr_levels - 1u; level++) {
+		l = q->qs + level;
+		target = q->target_count[level];
+
+		/*
+		 * Pull down some entries from the level above.
+		 */
+		while (l->nr_elts < target) {
+			e = __redist_pop_from(q, level + 1u);
+			if (!e) {
+				/* bug in nr_elts */
+				break;
+			}
+
+			e->level = level;
+			l_add_tail(q->es, l, e);
+		}
+
+		/*
+		 * Push some entries up.
+		 */
+		l_above = q->qs + level + 1u;
+		while (l->nr_elts > target) {
+			e = l_pop_tail(q->es, l);
+
+			if (!e)
+				/* bug in nr_elts */
+				break;
+
+			e->level = level + 1u;
+			l_add_head(q->es, l_above, e);
+		}
+	}
+}
+
+static void q_requeue_before(struct queue *q, struct entry *dest, struct entry *e, unsigned extra_levels)
+{
+	struct entry *de;
+	unsigned new_level;
+
+	q_del(q, e);
+
+	if (extra_levels && (e->level < q->nr_levels - 1u)) {
+		new_level = min(q->nr_levels - 1u, e->level + extra_levels);
+		for (de = l_head(q->es, q->qs + new_level); de; de = l_next(q->es, de)) {
+			if (de->sentinel)
+				continue;
+
+			q_del(q, de);
+			de->level = e->level;
+
+			if (dest)
+				q_push_before(q, dest, de);
+			else
+				q_push(q, de);
+			break;
+		}
+
+		e->level = new_level;
+	}
+
+	q_push(q, e);
+}
+
+static void q_requeue(struct queue *q, struct entry *e, unsigned extra_levels)
+{
+	q_requeue_before(q, NULL, e, extra_levels);
+}
+
+/*----------------------------------------------------------------*/
+
+#define FP_SHIFT 8
+#define SIXTEENTH (1u << (FP_SHIFT - 4u))
+#define EIGHTH (1u << (FP_SHIFT - 3u))
+
+struct stats {
+	unsigned hit_threshold;
+	unsigned hits;
+	unsigned misses;
+};
+
+enum performance {
+	Q_POOR,
+	Q_FAIR,
+	Q_WELL
+};
+
+static void stats_init(struct stats *s, unsigned nr_levels)
+{
+	s->hit_threshold = (nr_levels * 3u) / 4u;
+	s->hits = 0u;
+	s->misses = 0u;
+}
+
+static void stats_reset(struct stats *s)
+{
+	s->hits = s->misses = 0u;
+}
+
+static void stats_level_accessed(struct stats *s, unsigned level)
+{
+	if (level >= s->hit_threshold)
+		s->hits++;
+	else
+		s->misses++;
+}
+
+static void stats_miss(struct stats *s)
+{
+	s->misses++;
+}
+
+/*
+ * There are times when we don't have any confidence in the hotspot queue.
+ * Such as when a fresh cache is created and the blocks have been spread
+ * out across the levels, or if an io load changes.  We detect this by
+ * seeing how often a lookup is in the top levels of the hotspot queue.
+ */
+static enum performance stats_assess(struct stats *s)
+{
+	unsigned confidence = safe_div(s->hits << FP_SHIFT, s->hits + s->misses);
+
+	if (confidence < SIXTEENTH)
+		return Q_POOR;
+
+	else if (confidence < EIGHTH)
+		return Q_FAIR;
+
+	else
+		return Q_WELL;
+}
+
+/*----------------------------------------------------------------*/
+
+struct hash_table {
+	struct entry_space *es;
+	unsigned long long hash_bits;
+	unsigned *buckets;
+};
+
+/*
+ * All cache entries are stored in a chained hash table.  To save space we
+ * use indexing again, and only store indexes to the next entry.
+ */
+static int h_init(struct hash_table *ht, struct entry_space *es, unsigned nr_entries)
+{
+	unsigned i, nr_buckets;
+
+	ht->es = es;
+	nr_buckets = roundup_pow_of_two(max(nr_entries / 4u, 16u));
+	ht->hash_bits = ffs(nr_buckets) - 1;
+
+	ht->buckets = vmalloc(sizeof(*ht->buckets) * nr_buckets);
+	if (!ht->buckets)
+		return -ENOMEM;
+
+	for (i = 0; i < nr_buckets; i++)
+		ht->buckets[i] = INDEXER_NULL;
+
+	return 0;
+}
+
+static void h_exit(struct hash_table *ht)
+{
+	vfree(ht->buckets);
+}
+
+static struct entry *h_head(struct hash_table *ht, unsigned bucket)
+{
+	return to_entry(ht->es, ht->buckets[bucket]);
+}
+
+static struct entry *h_next(struct hash_table *ht, struct entry *e)
+{
+	return to_entry(ht->es, e->hash_next);
+}
+
+static void __h_insert(struct hash_table *ht, unsigned bucket, struct entry *e)
+{
+	e->hash_next = ht->buckets[bucket];
+	ht->buckets[bucket] = to_index(ht->es, e);
+}
+
+static void h_insert(struct hash_table *ht, struct entry *e)
+{
+	unsigned h = hash_64(from_oblock(e->oblock), ht->hash_bits);
+	__h_insert(ht, h, e);
+}
+
+static struct entry *__h_lookup(struct hash_table *ht, unsigned h, dm_oblock_t oblock,
+				struct entry **prev)
+{
+	struct entry *e;
+
+	*prev = NULL;
+	for (e = h_head(ht, h); e; e = h_next(ht, e)) {
+		if (e->oblock == oblock)
+			return e;
+
+		*prev = e;
+	}
+
+	return NULL;
+}
+
+static void __h_unlink(struct hash_table *ht, unsigned h,
+		       struct entry *e, struct entry *prev)
+{
+	if (prev)
+		prev->hash_next = e->hash_next;
+	else
+		ht->buckets[h] = e->hash_next;
+}
+
+/*
+ * Also moves each entry to the front of the bucket.
+ */
+static struct entry *h_lookup(struct hash_table *ht, dm_oblock_t oblock)
+{
+	struct entry *e, *prev;
+	unsigned h = hash_64(from_oblock(oblock), ht->hash_bits);
+
+	e = __h_lookup(ht, h, oblock, &prev);
+	if (e && prev) {
+		/*
+		 * Move to the front because this entry is likely
+		 * to be hit again.
+		 */
+		__h_unlink(ht, h, e, prev);
+		__h_insert(ht, h, e);
+	}
+
+	return e;
+}
+
+static void h_remove(struct hash_table *ht, struct entry *e)
+{
+	unsigned h = hash_64(from_oblock(e->oblock), ht->hash_bits);
+	struct entry *prev;
+
+	/*
+	 * The down side of using a singly linked list is we have to
+	 * iterate the bucket to remove an item.
+	 */
+	e = __h_lookup(ht, h, e->oblock, &prev);
+	if (e)
+		__h_unlink(ht, h, e, prev);
+}
+
+/*----------------------------------------------------------------*/
+
+struct entry_alloc {
+	struct entry_space *es;
+	unsigned begin;
+
+	unsigned nr_allocated;
+	struct ilist free;
+};
+
+static void init_allocator(struct entry_alloc *ea, struct entry_space *es,
+			   unsigned begin, unsigned end)
+{
+	unsigned i;
+
+	ea->es = es;
+	ea->nr_allocated = 0u;
+	ea->begin = begin;
+
+	l_init(&ea->free);
+	for (i = begin; i != end; i++)
+		l_add_tail(ea->es, &ea->free, __get_entry(ea->es, i));
+}
+
+static void init_entry(struct entry *e)
+{
+	/*
+	 * We can't memset because that would clear the hotspot and
+	 * sentinel bits which remain constant.
+	 */
+	e->hash_next = INDEXER_NULL;
+	e->next = INDEXER_NULL;
+	e->prev = INDEXER_NULL;
+	e->level = 0u;
+	e->allocated = true;
+}
+
+static struct entry *alloc_entry(struct entry_alloc *ea)
+{
+	struct entry *e;
+
+	if (l_empty(&ea->free))
+		return NULL;
+
+	e = l_pop_tail(ea->es, &ea->free);
+	init_entry(e);
+	ea->nr_allocated++;
+
+	return e;
+}
+
+/*
+ * This assumes the cblock hasn't already been allocated.
+ */
+static struct entry *alloc_particular_entry(struct entry_alloc *ea, unsigned i)
+{
+	struct entry *e = __get_entry(ea->es, ea->begin + i);
+
+	BUG_ON(e->allocated);
+
+	l_del(ea->es, &ea->free, e);
+	init_entry(e);
+	ea->nr_allocated++;
+
+	return e;
+}
+
+static void free_entry(struct entry_alloc *ea, struct entry *e)
+{
+	BUG_ON(!ea->nr_allocated);
+	BUG_ON(!e->allocated);
+
+	ea->nr_allocated--;
+	e->allocated = false;
+	l_add_tail(ea->es, &ea->free, e);
+}
+
+static bool allocator_empty(struct entry_alloc *ea)
+{
+	return l_empty(&ea->free);
+}
+
+static unsigned get_index(struct entry_alloc *ea, struct entry *e)
+{
+	return to_index(ea->es, e) - ea->begin;
+}
+
+static struct entry *get_entry(struct entry_alloc *ea, unsigned index)
+{
+	return __get_entry(ea->es, ea->begin + index);
+}
+
+/*----------------------------------------------------------------*/
+
+#define NR_HOTSPOT_LEVELS 64u
+#define NR_CACHE_LEVELS 64u
+
+#define WRITEBACK_PERIOD (10 * HZ)
+#define DEMOTE_PERIOD (60 * HZ)
+
+#define HOTSPOT_UPDATE_PERIOD (HZ)
+#define CACHE_UPDATE_PERIOD (10u * HZ)
+
+struct smq_policy {
+	struct dm_cache_policy policy;
+
+	/* protects everything */
+	struct mutex lock;
+	dm_cblock_t cache_size;
+	sector_t cache_block_size;
+
+	sector_t hotspot_block_size;
+	unsigned nr_hotspot_blocks;
+	unsigned cache_blocks_per_hotspot_block;
+	unsigned hotspot_level_jump;
+
+	struct entry_space es;
+	struct entry_alloc writeback_sentinel_alloc;
+	struct entry_alloc demote_sentinel_alloc;
+	struct entry_alloc hotspot_alloc;
+	struct entry_alloc cache_alloc;
+
+	unsigned long *hotspot_hit_bits;
+	unsigned long *cache_hit_bits;
+
+	/*
+	 * We maintain three queues of entries.  The cache proper,
+	 * consisting of a clean and dirty queue, containing the currently
+	 * active mappings.  The hotspot queue uses a larger block size to
+	 * track blocks that are being hit frequently and potential
+	 * candidates for promotion to the cache.
+	 */
+	struct queue hotspot;
+	struct queue clean;
+	struct queue dirty;
+
+	struct stats hotspot_stats;
+	struct stats cache_stats;
+
+	/*
+	 * Keeps track of time, incremented by the core.  We use this to
+	 * avoid attributing multiple hits within the same tick.
+	 *
+	 * Access to tick_protected should be done with the spin lock held.
+	 * It's copied to tick at the start of the map function (within the
+	 * mutex).
+	 */
+	spinlock_t tick_lock;
+	unsigned tick_protected;
+	unsigned tick;
+
+	/*
+	 * The hash tables allows us to quickly find an entry by origin
+	 * block.
+	 */
+	struct hash_table table;
+	struct hash_table hotspot_table;
+
+	bool current_writeback_sentinels;
+	unsigned long next_writeback_period;
+
+	bool current_demote_sentinels;
+	unsigned long next_demote_period;
+
+	unsigned write_promote_level;
+	unsigned read_promote_level;
+
+	unsigned long next_hotspot_period;
+	unsigned long next_cache_period;
+};
+
+/*----------------------------------------------------------------*/
+
+static struct entry *get_sentinel(struct entry_alloc *ea, unsigned level, bool which)
+{
+	return get_entry(ea, which ? level : NR_CACHE_LEVELS + level);
+}
+
+static struct entry *writeback_sentinel(struct smq_policy *mq, unsigned level)
+{
+	return get_sentinel(&mq->writeback_sentinel_alloc, level, mq->current_writeback_sentinels);
+}
+
+static struct entry *demote_sentinel(struct smq_policy *mq, unsigned level)
+{
+	return get_sentinel(&mq->demote_sentinel_alloc, level, mq->current_demote_sentinels);
+}
+
+static void __update_writeback_sentinels(struct smq_policy *mq)
+{
+	unsigned level;
+	struct queue *q = &mq->dirty;
+	struct entry *sentinel;
+
+	for (level = 0; level < q->nr_levels; level++) {
+		sentinel = writeback_sentinel(mq, level);
+		q_del(q, sentinel);
+		q_push(q, sentinel);
+	}
+}
+
+static void __update_demote_sentinels(struct smq_policy *mq)
+{
+	unsigned level;
+	struct queue *q = &mq->clean;
+	struct entry *sentinel;
+
+	for (level = 0; level < q->nr_levels; level++) {
+		sentinel = demote_sentinel(mq, level);
+		q_del(q, sentinel);
+		q_push(q, sentinel);
+	}
+}
+
+static void update_sentinels(struct smq_policy *mq)
+{
+	if (time_after(jiffies, mq->next_writeback_period)) {
+		__update_writeback_sentinels(mq);
+		mq->next_writeback_period = jiffies + WRITEBACK_PERIOD;
+		mq->current_writeback_sentinels = !mq->current_writeback_sentinels;
+	}
+
+	if (time_after(jiffies, mq->next_demote_period)) {
+		__update_demote_sentinels(mq);
+		mq->next_demote_period = jiffies + DEMOTE_PERIOD;
+		mq->current_demote_sentinels = !mq->current_demote_sentinels;
+	}
+}
+
+static void __sentinels_init(struct smq_policy *mq)
+{
+	unsigned level;
+	struct entry *sentinel;
+
+	for (level = 0; level < NR_CACHE_LEVELS; level++) {
+		sentinel = writeback_sentinel(mq, level);
+		sentinel->level = level;
+		q_push(&mq->dirty, sentinel);
+
+		sentinel = demote_sentinel(mq, level);
+		sentinel->level = level;
+		q_push(&mq->clean, sentinel);
+	}
+}
+
+static void sentinels_init(struct smq_policy *mq)
+{
+	mq->next_writeback_period = jiffies + WRITEBACK_PERIOD;
+	mq->next_demote_period = jiffies + DEMOTE_PERIOD;
+
+	mq->current_writeback_sentinels = false;
+	mq->current_demote_sentinels = false;
+	__sentinels_init(mq);
+
+	mq->current_writeback_sentinels = !mq->current_writeback_sentinels;
+	mq->current_demote_sentinels = !mq->current_demote_sentinels;
+	__sentinels_init(mq);
+}
+
+/*----------------------------------------------------------------*/
+
+/*
+ * These methods tie together the dirty queue, clean queue and hash table.
+ */
+static void push_new(struct smq_policy *mq, struct entry *e)
+{
+	struct queue *q = e->dirty ? &mq->dirty : &mq->clean;
+	h_insert(&mq->table, e);
+	q_push(q, e);
+}
+
+static void push(struct smq_policy *mq, struct entry *e)
+{
+	struct entry *sentinel;
+
+	h_insert(&mq->table, e);
+
+	/*
+	 * Punch this into the queue just in front of the sentinel, to
+	 * ensure it's cleaned straight away.
+	 */
+	if (e->dirty) {
+		sentinel = writeback_sentinel(mq, e->level);
+		q_push_before(&mq->dirty, sentinel, e);
+	} else {
+		sentinel = demote_sentinel(mq, e->level);
+		q_push_before(&mq->clean, sentinel, e);
+	}
+}
+
+/*
+ * Removes an entry from cache.  Removes from the hash table.
+ */
+static void __del(struct smq_policy *mq, struct queue *q, struct entry *e)
+{
+	q_del(q, e);
+	h_remove(&mq->table, e);
+}
+
+static void del(struct smq_policy *mq, struct entry *e)
+{
+	__del(mq, e->dirty ? &mq->dirty : &mq->clean, e);
+}
+
+static struct entry *pop_old(struct smq_policy *mq, struct queue *q, unsigned max_level)
+{
+	struct entry *e = q_pop_old(q, max_level);
+	if (e)
+		h_remove(&mq->table, e);
+	return e;
+}
+
+static dm_cblock_t infer_cblock(struct smq_policy *mq, struct entry *e)
+{
+	return to_cblock(get_index(&mq->cache_alloc, e));
+}
+
+static void requeue(struct smq_policy *mq, struct entry *e)
+{
+	struct entry *sentinel;
+
+	if (!test_and_set_bit(from_cblock(infer_cblock(mq, e)), mq->cache_hit_bits)) {
+		if (e->dirty) {
+			sentinel = writeback_sentinel(mq, e->level);
+			q_requeue_before(&mq->dirty, sentinel, e, 1u);
+		} else {
+			sentinel = demote_sentinel(mq, e->level);
+			q_requeue_before(&mq->clean, sentinel, e, 1u);
+		}
+	}
+}
+
+static unsigned default_promote_level(struct smq_policy *mq)
+{
+	/*
+	 * The promote level depends on the current performance of the
+	 * cache.
+	 *
+	 * If the cache is performing badly, then we can't afford
+	 * to promote much without causing performance to drop below that
+	 * of the origin device.
+	 *
+	 * If the cache is performing well, then we don't need to promote
+	 * much.  If it isn't broken, don't fix it.
+	 *
+	 * If the cache is middling then we promote more.
+	 *
+	 * This scheme reminds me of a graph of entropy vs probability of a
+	 * binary variable.
+	 */
+	static unsigned table[] = {1, 1, 1, 2, 4, 6, 7, 8, 7, 6, 4, 4, 3, 3, 2, 2, 1};
+
+	unsigned hits = mq->cache_stats.hits;
+	unsigned misses = mq->cache_stats.misses;
+	unsigned index = safe_div(hits << 4u, hits + misses);
+	return table[index];
+}
+
+static void update_promote_levels(struct smq_policy *mq)
+{
+	/*
+	 * If there are unused cache entries then we want to be really
+	 * eager to promote.
+	 */
+	unsigned threshold_level = allocator_empty(&mq->cache_alloc) ?
+		default_promote_level(mq) : (NR_HOTSPOT_LEVELS / 2u);
+
+	/*
+	 * If the hotspot queue is performing badly then we have little
+	 * confidence that we know which blocks to promote.  So we cut down
+	 * the amount of promotions.
+	 */
+	switch (stats_assess(&mq->hotspot_stats)) {
+	case Q_POOR:
+		threshold_level /= 4u;
+		break;
+
+	case Q_FAIR:
+		threshold_level /= 2u;
+		break;
+
+	case Q_WELL:
+		break;
+	}
+
+	mq->read_promote_level = NR_HOTSPOT_LEVELS - threshold_level;
+	mq->write_promote_level = (NR_HOTSPOT_LEVELS - threshold_level) + 2u;
+}
+
+/*
+ * If the hotspot queue is performing badly, then we try and move entries
+ * around more quickly.
+ */
+static void update_level_jump(struct smq_policy *mq)
+{
+	switch (stats_assess(&mq->hotspot_stats)) {
+	case Q_POOR:
+		mq->hotspot_level_jump = 4u;
+		break;
+
+	case Q_FAIR:
+		mq->hotspot_level_jump = 2u;
+		break;
+
+	case Q_WELL:
+		mq->hotspot_level_jump = 1u;
+		break;
+	}
+}
+
+static void end_hotspot_period(struct smq_policy *mq)
+{
+	clear_bitset(mq->hotspot_hit_bits, mq->nr_hotspot_blocks);
+	update_promote_levels(mq);
+
+	if (time_after(jiffies, mq->next_hotspot_period)) {
+		update_level_jump(mq);
+		q_redistribute(&mq->hotspot);
+		stats_reset(&mq->hotspot_stats);
+		mq->next_hotspot_period = jiffies + HOTSPOT_UPDATE_PERIOD;
+	}
+}
+
+static void end_cache_period(struct smq_policy *mq)
+{
+	if (time_after(jiffies, mq->next_cache_period)) {
+		clear_bitset(mq->cache_hit_bits, from_cblock(mq->cache_size));
+
+		q_redistribute(&mq->dirty);
+		q_redistribute(&mq->clean);
+		stats_reset(&mq->cache_stats);
+
+		mq->next_cache_period = jiffies + CACHE_UPDATE_PERIOD;
+	}
+}
+
+static int demote_cblock(struct smq_policy *mq,
+			 struct policy_locker *locker,
+			 dm_oblock_t *oblock)
+{
+	struct entry *demoted = q_peek(&mq->clean, mq->clean.nr_levels, false);
+	if (!demoted)
+		/*
+		 * We could get a block from mq->dirty, but that
+		 * would add extra latency to the triggering bio as it
+		 * waits for the writeback.  Better to not promote this
+		 * time and hope there's a clean block next time this block
+		 * is hit.
+		 */
+		return -ENOSPC;
+
+	if (locker->fn(locker, demoted->oblock))
+		/*
+		 * We couldn't lock this block.
+		 */
+		return -EBUSY;
+
+	del(mq, demoted);
+	*oblock = demoted->oblock;
+	free_entry(&mq->cache_alloc, demoted);
+
+	return 0;
+}
+
+enum promote_result {
+	PROMOTE_NOT,
+	PROMOTE_TEMPORARY,
+	PROMOTE_PERMANENT
+};
+
+/*
+ * Converts a boolean into a promote result.
+ */
+static enum promote_result maybe_promote(bool promote)
+{
+	return promote ? PROMOTE_PERMANENT : PROMOTE_NOT;
+}
+
+static enum promote_result should_promote(struct smq_policy *mq, struct entry *hs_e, struct bio *bio,
+					  bool fast_promote)
+{
+	if (bio_data_dir(bio) == WRITE) {
+		if (!allocator_empty(&mq->cache_alloc) && fast_promote)
+			return PROMOTE_TEMPORARY;
+
+		else
+			return maybe_promote(hs_e->level >= mq->write_promote_level);
+	} else
+		return maybe_promote(hs_e->level >= mq->read_promote_level);
+}
+
+static void insert_in_cache(struct smq_policy *mq, dm_oblock_t oblock,
+			    struct policy_locker *locker,
+			    struct policy_result *result, enum promote_result pr)
+{
+	int r;
+	struct entry *e;
+
+	if (allocator_empty(&mq->cache_alloc)) {
+		result->op = POLICY_REPLACE;
+		r = demote_cblock(mq, locker, &result->old_oblock);
+		if (r) {
+			result->op = POLICY_MISS;
+			return;
+		}
+
+	} else
+		result->op = POLICY_NEW;
+
+	e = alloc_entry(&mq->cache_alloc);
+	BUG_ON(!e);
+	e->oblock = oblock;
+
+	if (pr == PROMOTE_TEMPORARY)
+		push(mq, e);
+	else
+		push_new(mq, e);
+
+	result->cblock = infer_cblock(mq, e);
+}
+
+static dm_oblock_t to_hblock(struct smq_policy *mq, dm_oblock_t b)
+{
+	sector_t r = from_oblock(b);
+	(void) sector_div(r, mq->cache_blocks_per_hotspot_block);
+	return to_oblock(r);
+}
+
+static struct entry *update_hotspot_queue(struct smq_policy *mq, dm_oblock_t b, struct bio *bio)
+{
+	unsigned hi;
+	dm_oblock_t hb = to_hblock(mq, b);
+	struct entry *e = h_lookup(&mq->hotspot_table, hb);
+
+	if (e) {
+		stats_level_accessed(&mq->hotspot_stats, e->level);
+
+		hi = get_index(&mq->hotspot_alloc, e);
+		q_requeue(&mq->hotspot, e,
+			  test_and_set_bit(hi, mq->hotspot_hit_bits) ?
+			  0u : mq->hotspot_level_jump);
+
+	} else {
+		stats_miss(&mq->hotspot_stats);
+
+		e = alloc_entry(&mq->hotspot_alloc);
+		if (!e) {
+			e = q_pop(&mq->hotspot);
+			if (e) {
+				h_remove(&mq->hotspot_table, e);
+				hi = get_index(&mq->hotspot_alloc, e);
+				clear_bit(hi, mq->hotspot_hit_bits);
+			}
+
+		}
+
+		if (e) {
+			e->oblock = hb;
+			q_push(&mq->hotspot, e);
+			h_insert(&mq->hotspot_table, e);
+		}
+	}
+
+	return e;
+}
+
+/*
+ * Looks the oblock up in the hash table, then decides whether to put in
+ * pre_cache, or cache etc.
+ */
+static int map(struct smq_policy *mq, struct bio *bio, dm_oblock_t oblock,
+	       bool can_migrate, bool fast_promote,
+	       struct policy_locker *locker, struct policy_result *result)
+{
+	struct entry *e, *hs_e;
+	enum promote_result pr;
+
+	hs_e = update_hotspot_queue(mq, oblock, bio);
+
+	e = h_lookup(&mq->table, oblock);
+	if (e) {
+		stats_level_accessed(&mq->cache_stats, e->level);
+
+		requeue(mq, e);
+		result->op = POLICY_HIT;
+		result->cblock = infer_cblock(mq, e);
+
+	} else {
+		stats_miss(&mq->cache_stats);
+
+		pr = should_promote(mq, hs_e, bio, fast_promote);
+		if (pr == PROMOTE_NOT)
+			result->op = POLICY_MISS;
+
+		else {
+			if (!can_migrate) {
+				result->op = POLICY_MISS;
+				return -EWOULDBLOCK;
+			}
+
+			insert_in_cache(mq, oblock, locker, result, pr);
+		}
+	}
+
+	return 0;
+}
+
+/*----------------------------------------------------------------*/
+
+/*
+ * Public interface, via the policy struct.  See dm-cache-policy.h for a
+ * description of these.
+ */
+
+static struct smq_policy *to_smq_policy(struct dm_cache_policy *p)
+{
+	return container_of(p, struct smq_policy, policy);
+}
+
+static void smq_destroy(struct dm_cache_policy *p)
+{
+	struct smq_policy *mq = to_smq_policy(p);
+
+	h_exit(&mq->hotspot_table);
+	h_exit(&mq->table);
+	free_bitset(mq->hotspot_hit_bits);
+	free_bitset(mq->cache_hit_bits);
+	space_exit(&mq->es);
+	kfree(mq);
+}
+
+static void copy_tick(struct smq_policy *mq)
+{
+	unsigned long flags, tick;
+
+	spin_lock_irqsave(&mq->tick_lock, flags);
+	tick = mq->tick_protected;
+	if (tick != mq->tick) {
+		update_sentinels(mq);
+		end_hotspot_period(mq);
+		end_cache_period(mq);
+		mq->tick = tick;
+	}
+	spin_unlock_irqrestore(&mq->tick_lock, flags);
+}
+
+static bool maybe_lock(struct smq_policy *mq, bool can_block)
+{
+	if (can_block) {
+		mutex_lock(&mq->lock);
+		return true;
+	} else
+		return mutex_trylock(&mq->lock);
+}
+
+static int smq_map(struct dm_cache_policy *p, dm_oblock_t oblock,
+		   bool can_block, bool can_migrate, bool fast_promote,
+		   struct bio *bio, struct policy_locker *locker,
+		   struct policy_result *result)
+{
+	int r;
+	struct smq_policy *mq = to_smq_policy(p);
+
+	result->op = POLICY_MISS;
+
+	if (!maybe_lock(mq, can_block))
+		return -EWOULDBLOCK;
+
+	copy_tick(mq);
+	r = map(mq, bio, oblock, can_migrate, fast_promote, locker, result);
+	mutex_unlock(&mq->lock);
+
+	return r;
+}
+
+static int smq_lookup(struct dm_cache_policy *p, dm_oblock_t oblock, dm_cblock_t *cblock)
+{
+	int r;
+	struct smq_policy *mq = to_smq_policy(p);
+	struct entry *e;
+
+	if (!mutex_trylock(&mq->lock))
+		return -EWOULDBLOCK;
+
+	e = h_lookup(&mq->table, oblock);
+	if (e) {
+		*cblock = infer_cblock(mq, e);
+		r = 0;
+	} else
+		r = -ENOENT;
+
+	mutex_unlock(&mq->lock);
+
+	return r;
+}
+
+static void __smq_set_clear_dirty(struct smq_policy *mq, dm_oblock_t oblock, bool set)
+{
+	struct entry *e;
+
+	e = h_lookup(&mq->table, oblock);
+	BUG_ON(!e);
+
+	del(mq, e);
+	e->dirty = set;
+	push(mq, e);
+}
+
+static void smq_set_dirty(struct dm_cache_policy *p, dm_oblock_t oblock)
+{
+	struct smq_policy *mq = to_smq_policy(p);
+
+	mutex_lock(&mq->lock);
+	__smq_set_clear_dirty(mq, oblock, true);
+	mutex_unlock(&mq->lock);
+}
+
+static void smq_clear_dirty(struct dm_cache_policy *p, dm_oblock_t oblock)
+{
+	struct smq_policy *mq = to_smq_policy(p);
+
+	mutex_lock(&mq->lock);
+	__smq_set_clear_dirty(mq, oblock, false);
+	mutex_unlock(&mq->lock);
+}
+
+static int smq_load_mapping(struct dm_cache_policy *p,
+			    dm_oblock_t oblock, dm_cblock_t cblock,
+			    uint32_t hint, bool hint_valid)
+{
+	struct smq_policy *mq = to_smq_policy(p);
+	struct entry *e;
+
+	e = alloc_particular_entry(&mq->cache_alloc, from_cblock(cblock));
+	e->oblock = oblock;
+	e->dirty = false;	/* this gets corrected in a minute */
+	e->level = hint_valid ? min(hint, NR_CACHE_LEVELS - 1) : 1;
+	push(mq, e);
+
+	return 0;
+}
+
+static int smq_save_hints(struct smq_policy *mq, struct queue *q,
+			  policy_walk_fn fn, void *context)
+{
+	int r;
+	unsigned level;
+	struct entry *e;
+
+	for (level = 0; level < q->nr_levels; level++)
+		for (e = l_head(q->es, q->qs + level); e; e = l_next(q->es, e)) {
+			if (!e->sentinel) {
+				r = fn(context, infer_cblock(mq, e),
+				       e->oblock, e->level);
+				if (r)
+					return r;
+			}
+		}
+
+	return 0;
+}
+
+static int smq_walk_mappings(struct dm_cache_policy *p, policy_walk_fn fn,
+			     void *context)
+{
+	struct smq_policy *mq = to_smq_policy(p);
+	int r = 0;
+
+	mutex_lock(&mq->lock);
+
+	r = smq_save_hints(mq, &mq->clean, fn, context);
+	if (!r)
+		r = smq_save_hints(mq, &mq->dirty, fn, context);
+
+	mutex_unlock(&mq->lock);
+
+	return r;
+}
+
+static void __remove_mapping(struct smq_policy *mq, dm_oblock_t oblock)
+{
+	struct entry *e;
+
+	e = h_lookup(&mq->table, oblock);
+	BUG_ON(!e);
+
+	del(mq, e);
+	free_entry(&mq->cache_alloc, e);
+}
+
+static void smq_remove_mapping(struct dm_cache_policy *p, dm_oblock_t oblock)
+{
+	struct smq_policy *mq = to_smq_policy(p);
+
+	mutex_lock(&mq->lock);
+	__remove_mapping(mq, oblock);
+	mutex_unlock(&mq->lock);
+}
+
+static int __remove_cblock(struct smq_policy *mq, dm_cblock_t cblock)
+{
+	struct entry *e = get_entry(&mq->cache_alloc, from_cblock(cblock));
+
+	if (!e || !e->allocated)
+		return -ENODATA;
+
+	del(mq, e);
+	free_entry(&mq->cache_alloc, e);
+
+	return 0;
+}
+
+static int smq_remove_cblock(struct dm_cache_policy *p, dm_cblock_t cblock)
+{
+	int r;
+	struct smq_policy *mq = to_smq_policy(p);
+
+	mutex_lock(&mq->lock);
+	r = __remove_cblock(mq, cblock);
+	mutex_unlock(&mq->lock);
+
+	return r;
+}
+
+
+#define CLEAN_TARGET_CRITICAL 5u /* percent */
+
+static bool clean_target_met(struct smq_policy *mq, bool critical)
+{
+	if (critical) {
+		/*
+		 * Cache entries may not be populated.  So we're cannot rely on the
+		 * size of the clean queue.
+		 */
+		unsigned nr_clean = from_cblock(mq->cache_size) - q_size(&mq->dirty);
+		unsigned target = from_cblock(mq->cache_size) * CLEAN_TARGET_CRITICAL / 100u;
+
+		return nr_clean >= target;
+	} else
+		return !q_size(&mq->dirty);
+}
+
+static int __smq_writeback_work(struct smq_policy *mq, dm_oblock_t *oblock,
+				dm_cblock_t *cblock, bool critical_only)
+{
+	struct entry *e = NULL;
+	bool target_met = clean_target_met(mq, critical_only);
+
+	if (critical_only)
+		/*
+		 * Always try and keep the bottom level clean.
+		 */
+		e = pop_old(mq, &mq->dirty, target_met ? 1u : mq->dirty.nr_levels);
+
+	else
+		e = pop_old(mq, &mq->dirty, mq->dirty.nr_levels);
+
+	if (!e)
+		return -ENODATA;
+
+	*oblock = e->oblock;
+	*cblock = infer_cblock(mq, e);
+	e->dirty = false;
+	push_new(mq, e);
+
+	return 0;
+}
+
+static int smq_writeback_work(struct dm_cache_policy *p, dm_oblock_t *oblock,
+			      dm_cblock_t *cblock, bool critical_only)
+{
+	int r;
+	struct smq_policy *mq = to_smq_policy(p);
+
+	mutex_lock(&mq->lock);
+	r = __smq_writeback_work(mq, oblock, cblock, critical_only);
+	mutex_unlock(&mq->lock);
+
+	return r;
+}
+
+static void __force_mapping(struct smq_policy *mq,
+			    dm_oblock_t current_oblock, dm_oblock_t new_oblock)
+{
+	struct entry *e = h_lookup(&mq->table, current_oblock);
+
+	if (e) {
+		del(mq, e);
+		e->oblock = new_oblock;
+		e->dirty = true;
+		push(mq, e);
+	}
+}
+
+static void smq_force_mapping(struct dm_cache_policy *p,
+			      dm_oblock_t current_oblock, dm_oblock_t new_oblock)
+{
+	struct smq_policy *mq = to_smq_policy(p);
+
+	mutex_lock(&mq->lock);
+	__force_mapping(mq, current_oblock, new_oblock);
+	mutex_unlock(&mq->lock);
+}
+
+static dm_cblock_t smq_residency(struct dm_cache_policy *p)
+{
+	dm_cblock_t r;
+	struct smq_policy *mq = to_smq_policy(p);
+
+	mutex_lock(&mq->lock);
+	r = to_cblock(mq->cache_alloc.nr_allocated);
+	mutex_unlock(&mq->lock);
+
+	return r;
+}
+
+static void smq_tick(struct dm_cache_policy *p, bool can_block)
+{
+	struct smq_policy *mq = to_smq_policy(p);
+	unsigned long flags;
+
+	spin_lock_irqsave(&mq->tick_lock, flags);
+	mq->tick_protected++;
+	spin_unlock_irqrestore(&mq->tick_lock, flags);
+
+	if (can_block) {
+		mutex_lock(&mq->lock);
+		copy_tick(mq);
+		mutex_unlock(&mq->lock);
+	}
+}
+
+/* Init the policy plugin interface function pointers. */
+static void init_policy_functions(struct smq_policy *mq)
+{
+	mq->policy.destroy = smq_destroy;
+	mq->policy.map = smq_map;
+	mq->policy.lookup = smq_lookup;
+	mq->policy.set_dirty = smq_set_dirty;
+	mq->policy.clear_dirty = smq_clear_dirty;
+	mq->policy.load_mapping = smq_load_mapping;
+	mq->policy.walk_mappings = smq_walk_mappings;
+	mq->policy.remove_mapping = smq_remove_mapping;
+	mq->policy.remove_cblock = smq_remove_cblock;
+	mq->policy.writeback_work = smq_writeback_work;
+	mq->policy.force_mapping = smq_force_mapping;
+	mq->policy.residency = smq_residency;
+	mq->policy.tick = smq_tick;
+}
+
+static bool too_many_hotspot_blocks(sector_t origin_size,
+				    sector_t hotspot_block_size,
+				    unsigned nr_hotspot_blocks)
+{
+	return (hotspot_block_size * nr_hotspot_blocks) > origin_size;
+}
+
+static void calc_hotspot_params(sector_t origin_size,
+				sector_t cache_block_size,
+				unsigned nr_cache_blocks,
+				sector_t *hotspot_block_size,
+				unsigned *nr_hotspot_blocks)
+{
+	*hotspot_block_size = cache_block_size * 16u;
+	*nr_hotspot_blocks = max(nr_cache_blocks / 4u, 1024u);
+
+	while ((*hotspot_block_size > cache_block_size) &&
+	       too_many_hotspot_blocks(origin_size, *hotspot_block_size, *nr_hotspot_blocks))
+		*hotspot_block_size /= 2u;
+}
+
+static struct dm_cache_policy *smq_create(dm_cblock_t cache_size,
+					  sector_t origin_size,
+					  sector_t cache_block_size)
+{
+	unsigned i;
+	unsigned nr_sentinels_per_queue = 2u * NR_CACHE_LEVELS;
+	unsigned total_sentinels = 2u * nr_sentinels_per_queue;
+	struct smq_policy *mq = kzalloc(sizeof(*mq), GFP_KERNEL);
+
+	if (!mq)
+		return NULL;
+
+	init_policy_functions(mq);
+	mq->cache_size = cache_size;
+	mq->cache_block_size = cache_block_size;
+
+	calc_hotspot_params(origin_size, cache_block_size, from_cblock(cache_size),
+			    &mq->hotspot_block_size, &mq->nr_hotspot_blocks);
+
+	mq->cache_blocks_per_hotspot_block = div64_u64(mq->hotspot_block_size, mq->cache_block_size);
+	mq->hotspot_level_jump = 1u;
+	if (space_init(&mq->es, total_sentinels + mq->nr_hotspot_blocks + from_cblock(cache_size))) {
+		DMERR("couldn't initialize entry space");
+		goto bad_pool_init;
+	}
+
+	init_allocator(&mq->writeback_sentinel_alloc, &mq->es, 0, nr_sentinels_per_queue);
+        for (i = 0; i < nr_sentinels_per_queue; i++)
+		get_entry(&mq->writeback_sentinel_alloc, i)->sentinel = true;
+
+	init_allocator(&mq->demote_sentinel_alloc, &mq->es, nr_sentinels_per_queue, total_sentinels);
+        for (i = 0; i < nr_sentinels_per_queue; i++)
+		get_entry(&mq->demote_sentinel_alloc, i)->sentinel = true;
+
+	init_allocator(&mq->hotspot_alloc, &mq->es, total_sentinels,
+		       total_sentinels + mq->nr_hotspot_blocks);
+
+	init_allocator(&mq->cache_alloc, &mq->es,
+		       total_sentinels + mq->nr_hotspot_blocks,
+		       total_sentinels + mq->nr_hotspot_blocks + from_cblock(cache_size));
+
+	mq->hotspot_hit_bits = alloc_bitset(mq->nr_hotspot_blocks);
+	if (!mq->hotspot_hit_bits) {
+		DMERR("couldn't allocate hotspot hit bitset");
+		goto bad_hotspot_hit_bits;
+	}
+	clear_bitset(mq->hotspot_hit_bits, mq->nr_hotspot_blocks);
+
+	if (from_cblock(cache_size)) {
+		mq->cache_hit_bits = alloc_bitset(from_cblock(cache_size));
+		if (!mq->cache_hit_bits && mq->cache_hit_bits) {
+			DMERR("couldn't allocate cache hit bitset");
+			goto bad_cache_hit_bits;
+		}
+		clear_bitset(mq->cache_hit_bits, from_cblock(mq->cache_size));
+	} else
+		mq->cache_hit_bits = NULL;
+
+	mq->tick_protected = 0;
+	mq->tick = 0;
+	mutex_init(&mq->lock);
+	spin_lock_init(&mq->tick_lock);
+
+	q_init(&mq->hotspot, &mq->es, NR_HOTSPOT_LEVELS);
+	mq->hotspot.nr_top_levels = 8;
+	mq->hotspot.nr_in_top_levels = min(mq->nr_hotspot_blocks / NR_HOTSPOT_LEVELS,
+					   from_cblock(mq->cache_size) / mq->cache_blocks_per_hotspot_block);
+
+	q_init(&mq->clean, &mq->es, NR_CACHE_LEVELS);
+	q_init(&mq->dirty, &mq->es, NR_CACHE_LEVELS);
+
+	stats_init(&mq->hotspot_stats, NR_HOTSPOT_LEVELS);
+	stats_init(&mq->cache_stats, NR_CACHE_LEVELS);
+
+	if (h_init(&mq->table, &mq->es, from_cblock(cache_size)))
+		goto bad_alloc_table;
+
+	if (h_init(&mq->hotspot_table, &mq->es, mq->nr_hotspot_blocks))
+		goto bad_alloc_hotspot_table;
+
+	sentinels_init(mq);
+	mq->write_promote_level = mq->read_promote_level = NR_HOTSPOT_LEVELS;
+
+	mq->next_hotspot_period = jiffies;
+	mq->next_cache_period = jiffies;
+
+	return &mq->policy;
+
+bad_alloc_hotspot_table:
+	h_exit(&mq->table);
+bad_alloc_table:
+	free_bitset(mq->cache_hit_bits);
+bad_cache_hit_bits:
+	free_bitset(mq->hotspot_hit_bits);
+bad_hotspot_hit_bits:
+	space_exit(&mq->es);
+bad_pool_init:
+	kfree(mq);
+
+	return NULL;
+}
+
+/*----------------------------------------------------------------*/
+
+static struct dm_cache_policy_type smq_policy_type = {
+	.name = "smq",
+	.version = {1, 0, 0},
+	.hint_size = 4,
+	.owner = THIS_MODULE,
+	.create = smq_create
+};
+
+static struct dm_cache_policy_type default_policy_type = {
+	.name = "default",
+	.version = {1, 4, 0},
+	.hint_size = 4,
+	.owner = THIS_MODULE,
+	.create = smq_create,
+	.real = &smq_policy_type
+};
+
+static int __init smq_init(void)
+{
+	int r;
+
+	r = dm_cache_policy_register(&smq_policy_type);
+	if (r) {
+		DMERR("register failed %d", r);
+		return -ENOMEM;
+	}
+
+	r = dm_cache_policy_register(&default_policy_type);
+	if (r) {
+		DMERR("register failed (as default) %d", r);
+		dm_cache_policy_unregister(&smq_policy_type);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void __exit smq_exit(void)
+{
+	dm_cache_policy_unregister(&smq_policy_type);
+	dm_cache_policy_unregister(&default_policy_type);
+}
+
+module_init(smq_init);
+module_exit(smq_exit);
+
+MODULE_AUTHOR("Joe Thornber <dm-devel@redhat.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("smq cache policy");
diff --git a/drivers/md/dm-cache-policy.h b/drivers/md/dm-cache-policy.h
index f50fe360c546..05db56eedb6a 100644
--- a/drivers/md/dm-cache-policy.h
+++ b/drivers/md/dm-cache-policy.h
@@ -70,6 +70,18 @@ enum policy_operation {
 };
 
 /*
+ * When issuing a POLICY_REPLACE the policy needs to make a callback to
+ * lock the block being demoted.  This doesn't need to occur during a
+ * writeback operation since the block remains in the cache.
+ */
+struct policy_locker;
+typedef int (*policy_lock_fn)(struct policy_locker *l, dm_oblock_t oblock);
+
+struct policy_locker {
+	policy_lock_fn fn;
+};
+
+/*
  * This is the instruction passed back to the core target.
  */
 struct policy_result {
@@ -122,7 +134,8 @@ struct dm_cache_policy {
 	 */
 	int (*map)(struct dm_cache_policy *p, dm_oblock_t oblock,
 		   bool can_block, bool can_migrate, bool discarded_oblock,
-		   struct bio *bio, struct policy_result *result);
+		   struct bio *bio, struct policy_locker *locker,
+		   struct policy_result *result);
 
 	/*
 	 * Sometimes we want to see if a block is in the cache, without
@@ -165,7 +178,9 @@ struct dm_cache_policy {
 	int (*remove_cblock)(struct dm_cache_policy *p, dm_cblock_t cblock);
 
 	/*
-	 * Provide a dirty block to be written back by the core target.
+	 * Provide a dirty block to be written back by the core target.  If
+	 * critical_only is set then the policy should only provide work if
+	 * it urgently needs it.
 	 *
 	 * Returns:
 	 *
@@ -173,7 +188,8 @@ struct dm_cache_policy {
 	 *
 	 * -ENODATA: no dirty blocks available
 	 */
-	int (*writeback_work)(struct dm_cache_policy *p, dm_oblock_t *oblock, dm_cblock_t *cblock);
+	int (*writeback_work)(struct dm_cache_policy *p, dm_oblock_t *oblock, dm_cblock_t *cblock,
+			      bool critical_only);
 
 	/*
 	 * How full is the cache?
@@ -184,16 +200,16 @@ struct dm_cache_policy {
 	 * Because of where we sit in the block layer, we can be asked to
 	 * map a lot of little bios that are all in the same block (no
 	 * queue merging has occurred).  To stop the policy being fooled by
-	 * these the core target sends regular tick() calls to the policy.
+	 * these, the core target sends regular tick() calls to the policy.
 	 * The policy should only count an entry as hit once per tick.
 	 */
-	void (*tick)(struct dm_cache_policy *p);
+	void (*tick)(struct dm_cache_policy *p, bool can_block);
 
 	/*
 	 * Configuration.
 	 */
-	int (*emit_config_values)(struct dm_cache_policy *p,
-				  char *result, unsigned maxlen);
+	int (*emit_config_values)(struct dm_cache_policy *p, char *result,
+				  unsigned maxlen, ssize_t *sz_ptr);
 	int (*set_config_value)(struct dm_cache_policy *p,
 				const char *key, const char *value);
 
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index 7755af351867..1b4e1756b169 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -25,44 +25,93 @@ DECLARE_DM_KCOPYD_THROTTLE_WITH_MODULE_PARM(cache_copy_throttle,
 
 /*----------------------------------------------------------------*/
 
-/*
- * Glossary:
- *
- * oblock: index of an origin block
- * cblock: index of a cache block
- * promotion: movement of a block from origin to cache
- * demotion: movement of a block from cache to origin
- * migration: movement of a block between the origin and cache device,
- *	      either direction
- */
+#define IOT_RESOLUTION 4
 
-/*----------------------------------------------------------------*/
+struct io_tracker {
+	spinlock_t lock;
 
-static size_t bitset_size_in_bytes(unsigned nr_entries)
+	/*
+	 * Sectors of in-flight IO.
+	 */
+	sector_t in_flight;
+
+	/*
+	 * The time, in jiffies, when this device became idle (if it is
+	 * indeed idle).
+	 */
+	unsigned long idle_time;
+	unsigned long last_update_time;
+};
+
+static void iot_init(struct io_tracker *iot)
 {
-	return sizeof(unsigned long) * dm_div_up(nr_entries, BITS_PER_LONG);
+	spin_lock_init(&iot->lock);
+	iot->in_flight = 0ul;
+	iot->idle_time = 0ul;
+	iot->last_update_time = jiffies;
 }
 
-static unsigned long *alloc_bitset(unsigned nr_entries)
+static bool __iot_idle_for(struct io_tracker *iot, unsigned long jifs)
 {
-	size_t s = bitset_size_in_bytes(nr_entries);
-	return vzalloc(s);
+	if (iot->in_flight)
+		return false;
+
+	return time_after(jiffies, iot->idle_time + jifs);
 }
 
-static void clear_bitset(void *bitset, unsigned nr_entries)
+static bool iot_idle_for(struct io_tracker *iot, unsigned long jifs)
 {
-	size_t s = bitset_size_in_bytes(nr_entries);
-	memset(bitset, 0, s);
+	bool r;
+	unsigned long flags;
+
+	spin_lock_irqsave(&iot->lock, flags);
+	r = __iot_idle_for(iot, jifs);
+	spin_unlock_irqrestore(&iot->lock, flags);
+
+	return r;
 }
 
-static void free_bitset(unsigned long *bits)
+static void iot_io_begin(struct io_tracker *iot, sector_t len)
 {
-	vfree(bits);
+	unsigned long flags;
+
+	spin_lock_irqsave(&iot->lock, flags);
+	iot->in_flight += len;
+	spin_unlock_irqrestore(&iot->lock, flags);
+}
+
+static void __iot_io_end(struct io_tracker *iot, sector_t len)
+{
+	iot->in_flight -= len;
+	if (!iot->in_flight)
+		iot->idle_time = jiffies;
+}
+
+static void iot_io_end(struct io_tracker *iot, sector_t len)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&iot->lock, flags);
+	__iot_io_end(iot, len);
+	spin_unlock_irqrestore(&iot->lock, flags);
 }
 
 /*----------------------------------------------------------------*/
 
 /*
+ * Glossary:
+ *
+ * oblock: index of an origin block
+ * cblock: index of a cache block
+ * promotion: movement of a block from origin to cache
+ * demotion: movement of a block from cache to origin
+ * migration: movement of a block between the origin and cache device,
+ *	      either direction
+ */
+
+/*----------------------------------------------------------------*/
+
+/*
  * There are a couple of places where we let a bio run, but want to do some
  * work before calling its endio function.  We do this by temporarily
  * changing the endio fn.
@@ -86,12 +135,6 @@ static void dm_unhook_bio(struct dm_hook_info *h, struct bio *bio)
 {
 	bio->bi_end_io = h->bi_end_io;
 	bio->bi_private = h->bi_private;
-
-	/*
-	 * Must bump bi_remaining to allow bio to complete with
-	 * restored bi_end_io.
-	 */
-	atomic_inc(&bio->bi_remaining);
 }
 
 /*----------------------------------------------------------------*/
@@ -107,12 +150,10 @@ static void dm_unhook_bio(struct dm_hook_info *h, struct bio *bio)
 #define DATA_DEV_BLOCK_SIZE_MIN_SECTORS (32 * 1024 >> SECTOR_SHIFT)
 #define DATA_DEV_BLOCK_SIZE_MAX_SECTORS (1024 * 1024 * 1024 >> SECTOR_SHIFT)
 
-/*
- * FIXME: the cache is read/write for the time being.
- */
 enum cache_metadata_mode {
 	CM_WRITE,		/* metadata may be changed */
 	CM_READ_ONLY,		/* metadata may not be changed */
+	CM_FAIL
 };
 
 enum cache_io_mode {
@@ -214,6 +255,7 @@ struct cache {
 	int sectors_per_block_shift;
 
 	spinlock_t lock;
+	struct list_head deferred_cells;
 	struct bio_list deferred_bios;
 	struct bio_list deferred_flush_bios;
 	struct bio_list deferred_writethrough_bios;
@@ -288,6 +330,8 @@ struct cache {
 	 */
 	spinlock_t invalidation_lock;
 	struct list_head invalidation_requests;
+
+	struct io_tracker origin_tracker;
 };
 
 struct per_bio_data {
@@ -295,6 +339,7 @@ struct per_bio_data {
 	unsigned req_nr:2;
 	struct dm_deferred_entry *all_io_entry;
 	struct dm_hook_info hook_info;
+	sector_t len;
 
 	/*
 	 * writethrough fields.  These MUST remain at the end of this
@@ -338,6 +383,8 @@ struct prealloc {
 	struct dm_bio_prison_cell *cell2;
 };
 
+static enum cache_metadata_mode get_cache_mode(struct cache *cache);
+
 static void wake_worker(struct cache *cache)
 {
 	queue_work(cache->wq, &cache->worker);
@@ -371,10 +418,13 @@ static struct dm_cache_migration *alloc_migration(struct cache *cache)
 
 static void free_migration(struct dm_cache_migration *mg)
 {
-	if (atomic_dec_and_test(&mg->cache->nr_allocated_migrations))
-		wake_up(&mg->cache->migration_wait);
+	struct cache *cache = mg->cache;
+
+	if (atomic_dec_and_test(&cache->nr_allocated_migrations))
+		wake_up(&cache->migration_wait);
 
-	mempool_free(mg, mg->cache->migration_pool);
+	mempool_free(mg, cache->migration_pool);
+	wake_worker(cache);
 }
 
 static int prealloc_data_structs(struct cache *cache, struct prealloc *p)
@@ -649,6 +699,9 @@ static void save_stats(struct cache *cache)
 {
 	struct dm_cache_statistics stats;
 
+	if (get_cache_mode(cache) >= CM_READ_ONLY)
+		return;
+
 	stats.read_hits = atomic_read(&cache->stats.read_hit);
 	stats.read_misses = atomic_read(&cache->stats.read_miss);
 	stats.write_hits = atomic_read(&cache->stats.write_hit);
@@ -701,6 +754,7 @@ static struct per_bio_data *init_per_bio_data(struct bio *bio, size_t data_size)
 	pb->tick = false;
 	pb->req_nr = dm_bio_get_target_bio_nr(bio);
 	pb->all_io_entry = NULL;
+	pb->len = 0;
 
 	return pb;
 }
@@ -798,12 +852,43 @@ static void inc_ds(struct cache *cache, struct bio *bio,
 	pb->all_io_entry = dm_deferred_entry_inc(cache->all_io_ds);
 }
 
+static bool accountable_bio(struct cache *cache, struct bio *bio)
+{
+	return ((bio->bi_bdev == cache->origin_dev->bdev) &&
+		!(bio->bi_rw & REQ_DISCARD));
+}
+
+static void accounted_begin(struct cache *cache, struct bio *bio)
+{
+	size_t pb_data_size = get_per_bio_data_size(cache);
+	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
+
+	if (accountable_bio(cache, bio)) {
+		pb->len = bio_sectors(bio);
+		iot_io_begin(&cache->origin_tracker, pb->len);
+	}
+}
+
+static void accounted_complete(struct cache *cache, struct bio *bio)
+{
+	size_t pb_data_size = get_per_bio_data_size(cache);
+	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
+
+	iot_io_end(&cache->origin_tracker, pb->len);
+}
+
+static void accounted_request(struct cache *cache, struct bio *bio)
+{
+	accounted_begin(cache, bio);
+	generic_make_request(bio);
+}
+
 static void issue(struct cache *cache, struct bio *bio)
 {
 	unsigned long flags;
 
 	if (!bio_triggers_commit(cache, bio)) {
-		generic_make_request(bio);
+		accounted_request(cache, bio);
 		return;
 	}
 
@@ -876,6 +961,94 @@ static void remap_to_origin_then_cache(struct cache *cache, struct bio *bio,
 }
 
 /*----------------------------------------------------------------
+ * Failure modes
+ *--------------------------------------------------------------*/
+static enum cache_metadata_mode get_cache_mode(struct cache *cache)
+{
+	return cache->features.mode;
+}
+
+static const char *cache_device_name(struct cache *cache)
+{
+	return dm_device_name(dm_table_get_md(cache->ti->table));
+}
+
+static void notify_mode_switch(struct cache *cache, enum cache_metadata_mode mode)
+{
+	const char *descs[] = {
+		"write",
+		"read-only",
+		"fail"
+	};
+
+	dm_table_event(cache->ti->table);
+	DMINFO("%s: switching cache to %s mode",
+	       cache_device_name(cache), descs[(int)mode]);
+}
+
+static void set_cache_mode(struct cache *cache, enum cache_metadata_mode new_mode)
+{
+	bool needs_check = dm_cache_metadata_needs_check(cache->cmd);
+	enum cache_metadata_mode old_mode = get_cache_mode(cache);
+
+	if (new_mode == CM_WRITE && needs_check) {
+		DMERR("%s: unable to switch cache to write mode until repaired.",
+		      cache_device_name(cache));
+		if (old_mode != new_mode)
+			new_mode = old_mode;
+		else
+			new_mode = CM_READ_ONLY;
+	}
+
+	/* Never move out of fail mode */
+	if (old_mode == CM_FAIL)
+		new_mode = CM_FAIL;
+
+	switch (new_mode) {
+	case CM_FAIL:
+	case CM_READ_ONLY:
+		dm_cache_metadata_set_read_only(cache->cmd);
+		break;
+
+	case CM_WRITE:
+		dm_cache_metadata_set_read_write(cache->cmd);
+		break;
+	}
+
+	cache->features.mode = new_mode;
+
+	if (new_mode != old_mode)
+		notify_mode_switch(cache, new_mode);
+}
+
+static void abort_transaction(struct cache *cache)
+{
+	const char *dev_name = cache_device_name(cache);
+
+	if (get_cache_mode(cache) >= CM_READ_ONLY)
+		return;
+
+	if (dm_cache_metadata_set_needs_check(cache->cmd)) {
+		DMERR("%s: failed to set 'needs_check' flag in metadata", dev_name);
+		set_cache_mode(cache, CM_FAIL);
+	}
+
+	DMERR_LIMIT("%s: aborting current metadata transaction", dev_name);
+	if (dm_cache_metadata_abort(cache->cmd)) {
+		DMERR("%s: failed to abort metadata transaction", dev_name);
+		set_cache_mode(cache, CM_FAIL);
+	}
+}
+
+static void metadata_operation_failed(struct cache *cache, const char *op, int r)
+{
+	DMERR_LIMIT("%s: metadata operation '%s' failed: error = %d",
+		    cache_device_name(cache), op, r);
+	abort_transaction(cache);
+	set_cache_mode(cache, CM_READ_ONLY);
+}
+
+/*----------------------------------------------------------------
  * Migration processing
  *
  * Migration covers moving data from the origin device to the cache, or
@@ -891,26 +1064,63 @@ static void dec_io_migrations(struct cache *cache)
 	atomic_dec(&cache->nr_io_migrations);
 }
 
-static void __cell_defer(struct cache *cache, struct dm_bio_prison_cell *cell,
-			 bool holder)
+static void __cell_release(struct cache *cache, struct dm_bio_prison_cell *cell,
+			   bool holder, struct bio_list *bios)
 {
 	(holder ? dm_cell_release : dm_cell_release_no_holder)
-		(cache->prison, cell, &cache->deferred_bios);
+		(cache->prison, cell, bios);
 	free_prison_cell(cache, cell);
 }
 
-static void cell_defer(struct cache *cache, struct dm_bio_prison_cell *cell,
-		       bool holder)
+static bool discard_or_flush(struct bio *bio)
+{
+	return bio->bi_rw & (REQ_FLUSH | REQ_FUA | REQ_DISCARD);
+}
+
+static void __cell_defer(struct cache *cache, struct dm_bio_prison_cell *cell)
+{
+	if (discard_or_flush(cell->holder))
+		/*
+		 * We have to handle these bios
+		 * individually.
+		 */
+		__cell_release(cache, cell, true, &cache->deferred_bios);
+
+	else
+		list_add_tail(&cell->user_list, &cache->deferred_cells);
+}
+
+static void cell_defer(struct cache *cache, struct dm_bio_prison_cell *cell, bool holder)
 {
 	unsigned long flags;
 
+	if (!holder && dm_cell_promote_or_release(cache->prison, cell)) {
+		/*
+		 * There was no prisoner to promote to holder, the
+		 * cell has been released.
+		 */
+		free_prison_cell(cache, cell);
+		return;
+	}
+
 	spin_lock_irqsave(&cache->lock, flags);
-	__cell_defer(cache, cell, holder);
+	__cell_defer(cache, cell);
 	spin_unlock_irqrestore(&cache->lock, flags);
 
 	wake_worker(cache);
 }
 
+static void cell_error_with_code(struct cache *cache, struct dm_bio_prison_cell *cell, int err)
+{
+	dm_cell_error(cache->prison, cell, err);
+	dm_bio_prison_free_cell(cache->prison, cell);
+}
+
+static void cell_requeue(struct cache *cache, struct dm_bio_prison_cell *cell)
+{
+	cell_error_with_code(cache, cell, DM_ENDIO_REQUEUE);
+}
+
 static void free_io_migration(struct dm_cache_migration *mg)
 {
 	dec_io_migrations(mg->cache);
@@ -920,21 +1130,22 @@ static void free_io_migration(struct dm_cache_migration *mg)
 static void migration_failure(struct dm_cache_migration *mg)
 {
 	struct cache *cache = mg->cache;
+	const char *dev_name = cache_device_name(cache);
 
 	if (mg->writeback) {
-		DMWARN_LIMIT("writeback failed; couldn't copy block");
+		DMERR_LIMIT("%s: writeback failed; couldn't copy block", dev_name);
 		set_dirty(cache, mg->old_oblock, mg->cblock);
 		cell_defer(cache, mg->old_ocell, false);
 
 	} else if (mg->demote) {
-		DMWARN_LIMIT("demotion failed; couldn't copy block");
+		DMERR_LIMIT("%s: demotion failed; couldn't copy block", dev_name);
 		policy_force_mapping(cache->policy, mg->new_oblock, mg->old_oblock);
 
 		cell_defer(cache, mg->old_ocell, mg->promote ? false : true);
 		if (mg->promote)
 			cell_defer(cache, mg->new_ocell, true);
 	} else {
-		DMWARN_LIMIT("promotion failed; couldn't copy block");
+		DMERR_LIMIT("%s: promotion failed; couldn't copy block", dev_name);
 		policy_remove_mapping(cache->policy, mg->new_oblock);
 		cell_defer(cache, mg->new_ocell, true);
 	}
@@ -944,6 +1155,7 @@ static void migration_failure(struct dm_cache_migration *mg)
 
 static void migration_success_pre_commit(struct dm_cache_migration *mg)
 {
+	int r;
 	unsigned long flags;
 	struct cache *cache = mg->cache;
 
@@ -954,8 +1166,11 @@ static void migration_success_pre_commit(struct dm_cache_migration *mg)
 		return;
 
 	} else if (mg->demote) {
-		if (dm_cache_remove_mapping(cache->cmd, mg->cblock)) {
-			DMWARN_LIMIT("demotion failed; couldn't update on disk metadata");
+		r = dm_cache_remove_mapping(cache->cmd, mg->cblock);
+		if (r) {
+			DMERR_LIMIT("%s: demotion failed; couldn't update on disk metadata",
+				    cache_device_name(cache));
+			metadata_operation_failed(cache, "dm_cache_remove_mapping", r);
 			policy_force_mapping(cache->policy, mg->new_oblock,
 					     mg->old_oblock);
 			if (mg->promote)
@@ -964,8 +1179,11 @@ static void migration_success_pre_commit(struct dm_cache_migration *mg)
 			return;
 		}
 	} else {
-		if (dm_cache_insert_mapping(cache->cmd, mg->cblock, mg->new_oblock)) {
-			DMWARN_LIMIT("promotion failed; couldn't update on disk metadata");
+		r = dm_cache_insert_mapping(cache->cmd, mg->cblock, mg->new_oblock);
+		if (r) {
+			DMERR_LIMIT("%s: promotion failed; couldn't update on disk metadata",
+				    cache_device_name(cache));
+			metadata_operation_failed(cache, "dm_cache_insert_mapping", r);
 			policy_remove_mapping(cache->policy, mg->new_oblock);
 			free_io_migration(mg);
 			return;
@@ -984,7 +1202,8 @@ static void migration_success_post_commit(struct dm_cache_migration *mg)
 	struct cache *cache = mg->cache;
 
 	if (mg->writeback) {
-		DMWARN("writeback unexpectedly triggered commit");
+		DMWARN_LIMIT("%s: writeback unexpectedly triggered commit",
+			     cache_device_name(cache));
 		return;
 
 	} else if (mg->demote) {
@@ -1060,7 +1279,7 @@ static void issue_copy(struct dm_cache_migration *mg)
 	}
 
 	if (r < 0) {
-		DMERR_LIMIT("issuing migration failed");
+		DMERR_LIMIT("%s: issuing migration failed", cache_device_name(cache));
 		migration_failure(mg);
 	}
 }
@@ -1099,7 +1318,7 @@ static void issue_overwrite(struct dm_cache_migration *mg, struct bio *bio)
 	 * No need to inc_ds() here, since the cell will be held for the
 	 * duration of the io.
 	 */
-	generic_make_request(bio);
+	accounted_request(mg->cache, bio);
 }
 
 static bool bio_writes_complete_block(struct cache *cache, struct bio *bio)
@@ -1445,32 +1664,154 @@ static void inc_miss_counter(struct cache *cache, struct bio *bio)
 		   &cache->stats.read_miss : &cache->stats.write_miss);
 }
 
-static void process_bio(struct cache *cache, struct prealloc *structs,
-			struct bio *bio)
+/*----------------------------------------------------------------*/
+
+struct inc_detail {
+	struct cache *cache;
+	struct bio_list bios_for_issue;
+	struct bio_list unhandled_bios;
+	bool any_writes;
+};
+
+static void inc_fn(void *context, struct dm_bio_prison_cell *cell)
+{
+	struct bio *bio;
+	struct inc_detail *detail = context;
+	struct cache *cache = detail->cache;
+
+	inc_ds(cache, cell->holder, cell);
+	if (bio_data_dir(cell->holder) == WRITE)
+		detail->any_writes = true;
+
+	while ((bio = bio_list_pop(&cell->bios))) {
+		if (discard_or_flush(bio)) {
+			bio_list_add(&detail->unhandled_bios, bio);
+			continue;
+		}
+
+		if (bio_data_dir(bio) == WRITE)
+			detail->any_writes = true;
+
+		bio_list_add(&detail->bios_for_issue, bio);
+		inc_ds(cache, bio, cell);
+	}
+}
+
+// FIXME: refactor these two
+static void remap_cell_to_origin_clear_discard(struct cache *cache,
+					       struct dm_bio_prison_cell *cell,
+					       dm_oblock_t oblock, bool issue_holder)
+{
+	struct bio *bio;
+	unsigned long flags;
+	struct inc_detail detail;
+
+	detail.cache = cache;
+	bio_list_init(&detail.bios_for_issue);
+	bio_list_init(&detail.unhandled_bios);
+	detail.any_writes = false;
+
+	spin_lock_irqsave(&cache->lock, flags);
+	dm_cell_visit_release(cache->prison, inc_fn, &detail, cell);
+	bio_list_merge(&cache->deferred_bios, &detail.unhandled_bios);
+	spin_unlock_irqrestore(&cache->lock, flags);
+
+	remap_to_origin(cache, cell->holder);
+	if (issue_holder)
+		issue(cache, cell->holder);
+	else
+		accounted_begin(cache, cell->holder);
+
+	if (detail.any_writes)
+		clear_discard(cache, oblock_to_dblock(cache, oblock));
+
+	while ((bio = bio_list_pop(&detail.bios_for_issue))) {
+		remap_to_origin(cache, bio);
+		issue(cache, bio);
+	}
+}
+
+static void remap_cell_to_cache_dirty(struct cache *cache, struct dm_bio_prison_cell *cell,
+				      dm_oblock_t oblock, dm_cblock_t cblock, bool issue_holder)
+{
+	struct bio *bio;
+	unsigned long flags;
+	struct inc_detail detail;
+
+	detail.cache = cache;
+	bio_list_init(&detail.bios_for_issue);
+	bio_list_init(&detail.unhandled_bios);
+	detail.any_writes = false;
+
+	spin_lock_irqsave(&cache->lock, flags);
+	dm_cell_visit_release(cache->prison, inc_fn, &detail, cell);
+	bio_list_merge(&cache->deferred_bios, &detail.unhandled_bios);
+	spin_unlock_irqrestore(&cache->lock, flags);
+
+	remap_to_cache(cache, cell->holder, cblock);
+	if (issue_holder)
+		issue(cache, cell->holder);
+	else
+		accounted_begin(cache, cell->holder);
+
+	if (detail.any_writes) {
+		set_dirty(cache, oblock, cblock);
+		clear_discard(cache, oblock_to_dblock(cache, oblock));
+	}
+
+	while ((bio = bio_list_pop(&detail.bios_for_issue))) {
+		remap_to_cache(cache, bio, cblock);
+		issue(cache, bio);
+	}
+}
+
+/*----------------------------------------------------------------*/
+
+struct old_oblock_lock {
+	struct policy_locker locker;
+	struct cache *cache;
+	struct prealloc *structs;
+	struct dm_bio_prison_cell *cell;
+};
+
+static int null_locker(struct policy_locker *locker, dm_oblock_t b)
+{
+	/* This should never be called */
+	BUG();
+	return 0;
+}
+
+static int cell_locker(struct policy_locker *locker, dm_oblock_t b)
+{
+	struct old_oblock_lock *l = container_of(locker, struct old_oblock_lock, locker);
+	struct dm_bio_prison_cell *cell_prealloc = prealloc_get_cell(l->structs);
+
+	return bio_detain(l->cache, b, NULL, cell_prealloc,
+			  (cell_free_fn) prealloc_put_cell,
+			  l->structs, &l->cell);
+}
+
+static void process_cell(struct cache *cache, struct prealloc *structs,
+			 struct dm_bio_prison_cell *new_ocell)
 {
 	int r;
 	bool release_cell = true;
+	struct bio *bio = new_ocell->holder;
 	dm_oblock_t block = get_bio_block(cache, bio);
-	struct dm_bio_prison_cell *cell_prealloc, *old_ocell, *new_ocell;
 	struct policy_result lookup_result;
 	bool passthrough = passthrough_mode(&cache->features);
-	bool discarded_block, can_migrate;
-
-	/*
-	 * Check to see if that block is currently migrating.
-	 */
-	cell_prealloc = prealloc_get_cell(structs);
-	r = bio_detain(cache, block, bio, cell_prealloc,
-		       (cell_free_fn) prealloc_put_cell,
-		       structs, &new_ocell);
-	if (r > 0)
-		return;
+	bool fast_promotion, can_migrate;
+	struct old_oblock_lock ool;
 
-	discarded_block = is_discarded_oblock(cache, block);
-	can_migrate = !passthrough && (discarded_block || spare_migration_bandwidth(cache));
+	fast_promotion = is_discarded_oblock(cache, block) || bio_writes_complete_block(cache, bio);
+	can_migrate = !passthrough && (fast_promotion || spare_migration_bandwidth(cache));
 
-	r = policy_map(cache->policy, block, true, can_migrate, discarded_block,
-		       bio, &lookup_result);
+	ool.locker.fn = cell_locker;
+	ool.cache = cache;
+	ool.structs = structs;
+	ool.cell = NULL;
+	r = policy_map(cache->policy, block, true, can_migrate, fast_promotion,
+		       bio, &ool.locker, &lookup_result);
 
 	if (r == -EWOULDBLOCK)
 		/* migration has been denied */
@@ -1506,9 +1847,9 @@ static void process_bio(struct cache *cache, struct prealloc *structs,
 				remap_to_origin_then_cache(cache, bio, block, lookup_result.cblock);
 				inc_and_issue(cache, bio, new_ocell);
 
-			} else  {
-				remap_to_cache_dirty(cache, bio, block, lookup_result.cblock);
-				inc_and_issue(cache, bio, new_ocell);
+			} else {
+				remap_cell_to_cache_dirty(cache, new_ocell, block, lookup_result.cblock, true);
+				release_cell = false;
 			}
 		}
 
@@ -1516,8 +1857,8 @@ static void process_bio(struct cache *cache, struct prealloc *structs,
 
 	case POLICY_MISS:
 		inc_miss_counter(cache, bio);
-		remap_to_origin_clear_discard(cache, bio, block);
-		inc_and_issue(cache, bio, new_ocell);
+		remap_cell_to_origin_clear_discard(cache, new_ocell, block, true);
+		release_cell = false;
 		break;
 
 	case POLICY_NEW:
@@ -1527,32 +1868,17 @@ static void process_bio(struct cache *cache, struct prealloc *structs,
 		break;
 
 	case POLICY_REPLACE:
-		cell_prealloc = prealloc_get_cell(structs);
-		r = bio_detain(cache, lookup_result.old_oblock, bio, cell_prealloc,
-			       (cell_free_fn) prealloc_put_cell,
-			       structs, &old_ocell);
-		if (r > 0) {
-			/*
-			 * We have to be careful to avoid lock inversion of
-			 * the cells.  So we back off, and wait for the
-			 * old_ocell to become free.
-			 */
-			policy_force_mapping(cache->policy, block,
-					     lookup_result.old_oblock);
-			atomic_inc(&cache->stats.cache_cell_clash);
-			break;
-		}
 		atomic_inc(&cache->stats.demotion);
 		atomic_inc(&cache->stats.promotion);
-
 		demote_then_promote(cache, structs, lookup_result.old_oblock,
 				    block, lookup_result.cblock,
-				    old_ocell, new_ocell);
+				    ool.cell, new_ocell);
 		release_cell = false;
 		break;
 
 	default:
-		DMERR_LIMIT("%s: erroring bio, unknown policy op: %u", __func__,
+		DMERR_LIMIT("%s: %s: erroring bio, unknown policy op: %u",
+			    cache_device_name(cache), __func__,
 			    (unsigned) lookup_result.op);
 		bio_io_error(bio);
 	}
@@ -1561,10 +1887,48 @@ static void process_bio(struct cache *cache, struct prealloc *structs,
 		cell_defer(cache, new_ocell, false);
 }
 
+static void process_bio(struct cache *cache, struct prealloc *structs,
+			struct bio *bio)
+{
+	int r;
+	dm_oblock_t block = get_bio_block(cache, bio);
+	struct dm_bio_prison_cell *cell_prealloc, *new_ocell;
+
+	/*
+	 * Check to see if that block is currently migrating.
+	 */
+	cell_prealloc = prealloc_get_cell(structs);
+	r = bio_detain(cache, block, bio, cell_prealloc,
+		       (cell_free_fn) prealloc_put_cell,
+		       structs, &new_ocell);
+	if (r > 0)
+		return;
+
+	process_cell(cache, structs, new_ocell);
+}
+
 static int need_commit_due_to_time(struct cache *cache)
 {
-	return !time_in_range(jiffies, cache->last_commit_jiffies,
-			      cache->last_commit_jiffies + COMMIT_PERIOD);
+	return jiffies < cache->last_commit_jiffies ||
+	       jiffies > cache->last_commit_jiffies + COMMIT_PERIOD;
+}
+
+/*
+ * A non-zero return indicates read_only or fail_io mode.
+ */
+static int commit(struct cache *cache, bool clean_shutdown)
+{
+	int r;
+
+	if (get_cache_mode(cache) >= CM_READ_ONLY)
+		return -EINVAL;
+
+	atomic_inc(&cache->stats.commit_count);
+	r = dm_cache_commit(cache->cmd, clean_shutdown);
+	if (r)
+		metadata_operation_failed(cache, "dm_cache_commit", r);
+
+	return r;
 }
 
 static int commit_if_needed(struct cache *cache)
@@ -1573,9 +1937,8 @@ static int commit_if_needed(struct cache *cache)
 
 	if ((cache->commit_requested || need_commit_due_to_time(cache)) &&
 	    dm_cache_changed_this_transaction(cache->cmd)) {
-		atomic_inc(&cache->stats.commit_count);
+		r = commit(cache, false);
 		cache->commit_requested = false;
-		r = dm_cache_commit(cache->cmd, false);
 		cache->last_commit_jiffies = jiffies;
 	}
 
@@ -1623,6 +1986,40 @@ static void process_deferred_bios(struct cache *cache)
 	prealloc_free_structs(cache, &structs);
 }
 
+static void process_deferred_cells(struct cache *cache)
+{
+	unsigned long flags;
+	struct dm_bio_prison_cell *cell, *tmp;
+	struct list_head cells;
+	struct prealloc structs;
+
+	memset(&structs, 0, sizeof(structs));
+
+	INIT_LIST_HEAD(&cells);
+
+	spin_lock_irqsave(&cache->lock, flags);
+	list_splice_init(&cache->deferred_cells, &cells);
+	spin_unlock_irqrestore(&cache->lock, flags);
+
+	list_for_each_entry_safe(cell, tmp, &cells, user_list) {
+		/*
+		 * If we've got no free migration structs, and processing
+		 * this bio might require one, we pause until there are some
+		 * prepared mappings to process.
+		 */
+		if (prealloc_data_structs(cache, &structs)) {
+			spin_lock_irqsave(&cache->lock, flags);
+			list_splice(&cells, &cache->deferred_cells);
+			spin_unlock_irqrestore(&cache->lock, flags);
+			break;
+		}
+
+		process_cell(cache, &structs, cell);
+	}
+
+	prealloc_free_structs(cache, &structs);
+}
+
 static void process_deferred_flush_bios(struct cache *cache, bool submit_bios)
 {
 	unsigned long flags;
@@ -1640,7 +2037,7 @@ static void process_deferred_flush_bios(struct cache *cache, bool submit_bios)
 	 * These bios have already been through inc_ds()
 	 */
 	while ((bio = bio_list_pop(&bios)))
-		submit_bios ? generic_make_request(bio) : bio_io_error(bio);
+		submit_bios ? accounted_request(cache, bio) : bio_io_error(bio);
 }
 
 static void process_deferred_writethrough_bios(struct cache *cache)
@@ -1660,7 +2057,7 @@ static void process_deferred_writethrough_bios(struct cache *cache)
 	 * These bios have already been through inc_ds()
 	 */
 	while ((bio = bio_list_pop(&bios)))
-		generic_make_request(bio);
+		accounted_request(cache, bio);
 }
 
 static void writeback_some_dirty_blocks(struct cache *cache)
@@ -1670,6 +2067,7 @@ static void writeback_some_dirty_blocks(struct cache *cache)
 	dm_cblock_t cblock;
 	struct prealloc structs;
 	struct dm_bio_prison_cell *old_ocell;
+	bool busy = !iot_idle_for(&cache->origin_tracker, HZ);
 
 	memset(&structs, 0, sizeof(structs));
 
@@ -1677,7 +2075,7 @@ static void writeback_some_dirty_blocks(struct cache *cache)
 		if (prealloc_data_structs(cache, &structs))
 			break;
 
-		r = policy_writeback_work(cache->policy, &oblock, &cblock);
+		r = policy_writeback_work(cache->policy, &oblock, &cblock, busy);
 		if (r)
 			break;
 
@@ -1708,15 +2106,17 @@ static void process_invalidation_request(struct cache *cache, struct invalidatio
 		r = policy_remove_cblock(cache->policy, to_cblock(begin));
 		if (!r) {
 			r = dm_cache_remove_mapping(cache->cmd, to_cblock(begin));
-			if (r)
+			if (r) {
+				metadata_operation_failed(cache, "dm_cache_remove_mapping", r);
 				break;
+			}
 
 		} else if (r == -ENODATA) {
 			/* harmless, already unmapped */
 			r = 0;
 
 		} else {
-			DMERR("policy_remove_cblock failed");
+			DMERR("%s: policy_remove_cblock failed", cache_device_name(cache));
 			break;
 		}
 
@@ -1789,7 +2189,22 @@ static void stop_worker(struct cache *cache)
 	flush_workqueue(cache->wq);
 }
 
-static void requeue_deferred_io(struct cache *cache)
+static void requeue_deferred_cells(struct cache *cache)
+{
+	unsigned long flags;
+	struct list_head cells;
+	struct dm_bio_prison_cell *cell, *tmp;
+
+	INIT_LIST_HEAD(&cells);
+	spin_lock_irqsave(&cache->lock, flags);
+	list_splice_init(&cache->deferred_cells, &cells);
+	spin_unlock_irqrestore(&cache->lock, flags);
+
+	list_for_each_entry_safe(cell, tmp, &cells, user_list)
+		cell_requeue(cache, cell);
+}
+
+static void requeue_deferred_bios(struct cache *cache)
 {
 	struct bio *bio;
 	struct bio_list bios;
@@ -1810,6 +2225,7 @@ static int more_work(struct cache *cache)
 			!list_empty(&cache->need_commit_migrations);
 	else
 		return !bio_list_empty(&cache->deferred_bios) ||
+			!list_empty(&cache->deferred_cells) ||
 			!bio_list_empty(&cache->deferred_flush_bios) ||
 			!bio_list_empty(&cache->deferred_writethrough_bios) ||
 			!list_empty(&cache->quiesced_migrations) ||
@@ -1827,6 +2243,7 @@ static void do_worker(struct work_struct *ws)
 			writeback_some_dirty_blocks(cache);
 			process_deferred_writethrough_bios(cache);
 			process_deferred_bios(cache);
+			process_deferred_cells(cache);
 			process_invalidation_requests(cache);
 		}
 
@@ -1836,11 +2253,6 @@ static void do_worker(struct work_struct *ws)
 		if (commit_if_needed(cache)) {
 			process_deferred_flush_bios(cache, false);
 			process_migrations(cache, &cache->need_commit_migrations, migration_failure);
-
-			/*
-			 * FIXME: rollback metadata or just go into a
-			 * failure mode and error everything
-			 */
 		} else {
 			process_deferred_flush_bios(cache, true);
 			process_migrations(cache, &cache->need_commit_migrations,
@@ -1859,7 +2271,7 @@ static void do_worker(struct work_struct *ws)
 static void do_waker(struct work_struct *ws)
 {
 	struct cache *cache = container_of(to_delayed_work(ws), struct cache, waker);
-	policy_tick(cache->policy);
+	policy_tick(cache->policy, true);
 	wake_worker(cache);
 	queue_delayed_work(cache->wq, &cache->waker, COMMIT_PERIOD);
 }
@@ -2413,6 +2825,12 @@ static int cache_create(struct cache_args *ca, struct cache **result)
 		goto bad;
 	}
 	cache->cmd = cmd;
+	set_cache_mode(cache, CM_WRITE);
+	if (get_cache_mode(cache) != CM_WRITE) {
+		*error = "Unable to get write access to metadata, please check/repair metadata.";
+		r = -EINVAL;
+		goto bad;
+	}
 
 	if (passthrough_mode(&cache->features)) {
 		bool all_clean;
@@ -2431,6 +2849,7 @@ static int cache_create(struct cache_args *ca, struct cache **result)
 	}
 
 	spin_lock_init(&cache->lock);
+	INIT_LIST_HEAD(&cache->deferred_cells);
 	bio_list_init(&cache->deferred_bios);
 	bio_list_init(&cache->deferred_flush_bios);
 	bio_list_init(&cache->deferred_writethrough_bios);
@@ -2520,6 +2939,8 @@ static int cache_create(struct cache_args *ca, struct cache **result)
 	spin_lock_init(&cache->invalidation_lock);
 	INIT_LIST_HEAD(&cache->invalidation_requests);
 
+	iot_init(&cache->origin_tracker);
+
 	*result = cache;
 	return 0;
 
@@ -2586,15 +3007,23 @@ out:
 	return r;
 }
 
-static int __cache_map(struct cache *cache, struct bio *bio, struct dm_bio_prison_cell **cell)
+/*----------------------------------------------------------------*/
+
+static int cache_map(struct dm_target *ti, struct bio *bio)
 {
+	struct cache *cache = ti->private;
+
 	int r;
+	struct dm_bio_prison_cell *cell = NULL;
 	dm_oblock_t block = get_bio_block(cache, bio);
 	size_t pb_data_size = get_per_bio_data_size(cache);
 	bool can_migrate = false;
-	bool discarded_block;
+	bool fast_promotion;
 	struct policy_result lookup_result;
 	struct per_bio_data *pb = init_per_bio_data(bio, pb_data_size);
+	struct old_oblock_lock ool;
+
+	ool.locker.fn = null_locker;
 
 	if (unlikely(from_oblock(block) >= from_oblock(cache->origin_blocks))) {
 		/*
@@ -2603,10 +3032,11 @@ static int __cache_map(struct cache *cache, struct bio *bio, struct dm_bio_priso
 		 * Just remap to the origin and carry on.
 		 */
 		remap_to_origin(cache, bio);
+		accounted_begin(cache, bio);
 		return DM_MAPIO_REMAPPED;
 	}
 
-	if (bio->bi_rw & (REQ_FLUSH | REQ_FUA | REQ_DISCARD)) {
+	if (discard_or_flush(bio)) {
 		defer_bio(cache, bio);
 		return DM_MAPIO_SUBMITTED;
 	}
@@ -2614,15 +3044,15 @@ static int __cache_map(struct cache *cache, struct bio *bio, struct dm_bio_priso
 	/*
 	 * Check to see if that block is currently migrating.
 	 */
-	*cell = alloc_prison_cell(cache);
-	if (!*cell) {
+	cell = alloc_prison_cell(cache);
+	if (!cell) {
 		defer_bio(cache, bio);
 		return DM_MAPIO_SUBMITTED;
 	}
 
-	r = bio_detain(cache, block, bio, *cell,
+	r = bio_detain(cache, block, bio, cell,
 		       (cell_free_fn) free_prison_cell,
-		       cache, cell);
+		       cache, &cell);
 	if (r) {
 		if (r < 0)
 			defer_bio(cache, bio);
@@ -2630,17 +3060,18 @@ static int __cache_map(struct cache *cache, struct bio *bio, struct dm_bio_priso
 		return DM_MAPIO_SUBMITTED;
 	}
 
-	discarded_block = is_discarded_oblock(cache, block);
+	fast_promotion = is_discarded_oblock(cache, block) || bio_writes_complete_block(cache, bio);
 
-	r = policy_map(cache->policy, block, false, can_migrate, discarded_block,
-		       bio, &lookup_result);
+	r = policy_map(cache->policy, block, false, can_migrate, fast_promotion,
+		       bio, &ool.locker, &lookup_result);
 	if (r == -EWOULDBLOCK) {
-		cell_defer(cache, *cell, true);
+		cell_defer(cache, cell, true);
 		return DM_MAPIO_SUBMITTED;
 
 	} else if (r) {
-		DMERR_LIMIT("Unexpected return from cache replacement policy: %d", r);
-		cell_defer(cache, *cell, false);
+		DMERR_LIMIT("%s: Unexpected return from cache replacement policy: %d",
+			    cache_device_name(cache), r);
+		cell_defer(cache, cell, false);
 		bio_io_error(bio);
 		return DM_MAPIO_SUBMITTED;
 	}
@@ -2654,21 +3085,30 @@ static int __cache_map(struct cache *cache, struct bio *bio, struct dm_bio_priso
 				 * We need to invalidate this block, so
 				 * defer for the worker thread.
 				 */
-				cell_defer(cache, *cell, true);
+				cell_defer(cache, cell, true);
 				r = DM_MAPIO_SUBMITTED;
 
 			} else {
 				inc_miss_counter(cache, bio);
 				remap_to_origin_clear_discard(cache, bio, block);
+				accounted_begin(cache, bio);
+				inc_ds(cache, bio, cell);
+				// FIXME: we want to remap hits or misses straight
+				// away rather than passing over to the worker.
+				cell_defer(cache, cell, false);
 			}
 
 		} else {
 			inc_hit_counter(cache, bio);
 			if (bio_data_dir(bio) == WRITE && writethrough_mode(&cache->features) &&
-			    !is_dirty(cache, lookup_result.cblock))
+			    !is_dirty(cache, lookup_result.cblock)) {
 				remap_to_origin_then_cache(cache, bio, block, lookup_result.cblock);
-			else
-				remap_to_cache_dirty(cache, bio, block, lookup_result.cblock);
+				accounted_begin(cache, bio);
+				inc_ds(cache, bio, cell);
+				cell_defer(cache, cell, false);
+
+			} else
+				remap_cell_to_cache_dirty(cache, cell, block, lookup_result.cblock, false);
 		}
 		break;
 
@@ -2680,18 +3120,19 @@ static int __cache_map(struct cache *cache, struct bio *bio, struct dm_bio_priso
 			 * longer needed because the block has been demoted.
 			 */
 			bio_endio(bio, 0);
-			cell_defer(cache, *cell, false);
+			// FIXME: remap everything as a miss
+			cell_defer(cache, cell, false);
 			r = DM_MAPIO_SUBMITTED;
 
 		} else
-			remap_to_origin_clear_discard(cache, bio, block);
-
+			remap_cell_to_origin_clear_discard(cache, cell, block, false);
 		break;
 
 	default:
-		DMERR_LIMIT("%s: erroring bio: unknown policy op: %u", __func__,
+		DMERR_LIMIT("%s: %s: erroring bio: unknown policy op: %u",
+			    cache_device_name(cache), __func__,
 			    (unsigned) lookup_result.op);
-		cell_defer(cache, *cell, false);
+		cell_defer(cache, cell, false);
 		bio_io_error(bio);
 		r = DM_MAPIO_SUBMITTED;
 	}
@@ -2699,21 +3140,6 @@ static int __cache_map(struct cache *cache, struct bio *bio, struct dm_bio_priso
 	return r;
 }
 
-static int cache_map(struct dm_target *ti, struct bio *bio)
-{
-	int r;
-	struct dm_bio_prison_cell *cell = NULL;
-	struct cache *cache = ti->private;
-
-	r = __cache_map(cache, bio, &cell);
-	if (r == DM_MAPIO_REMAPPED && cell) {
-		inc_ds(cache, bio, cell);
-		cell_defer(cache, cell, false);
-	}
-
-	return r;
-}
-
 static int cache_end_io(struct dm_target *ti, struct bio *bio, int error)
 {
 	struct cache *cache = ti->private;
@@ -2722,7 +3148,7 @@ static int cache_end_io(struct dm_target *ti, struct bio *bio, int error)
 	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
 
 	if (pb->tick) {
-		policy_tick(cache->policy);
+		policy_tick(cache->policy, false);
 
 		spin_lock_irqsave(&cache->lock, flags);
 		cache->need_tick_bio = true;
@@ -2730,6 +3156,7 @@ static int cache_end_io(struct dm_target *ti, struct bio *bio, int error)
 	}
 
 	check_for_quiesced_migrations(cache, pb);
+	accounted_complete(cache, bio);
 
 	return 0;
 }
@@ -2738,11 +3165,16 @@ static int write_dirty_bitset(struct cache *cache)
 {
 	unsigned i, r;
 
+	if (get_cache_mode(cache) >= CM_READ_ONLY)
+		return -EINVAL;
+
 	for (i = 0; i < from_cblock(cache->cache_size); i++) {
 		r = dm_cache_set_dirty(cache->cmd, to_cblock(i),
 				       is_dirty(cache, to_cblock(i)));
-		if (r)
+		if (r) {
+			metadata_operation_failed(cache, "dm_cache_set_dirty", r);
 			return r;
+		}
 	}
 
 	return 0;
@@ -2752,18 +3184,40 @@ static int write_discard_bitset(struct cache *cache)
 {
 	unsigned i, r;
 
+	if (get_cache_mode(cache) >= CM_READ_ONLY)
+		return -EINVAL;
+
 	r = dm_cache_discard_bitset_resize(cache->cmd, cache->discard_block_size,
 					   cache->discard_nr_blocks);
 	if (r) {
-		DMERR("could not resize on-disk discard bitset");
+		DMERR("%s: could not resize on-disk discard bitset", cache_device_name(cache));
+		metadata_operation_failed(cache, "dm_cache_discard_bitset_resize", r);
 		return r;
 	}
 
 	for (i = 0; i < from_dblock(cache->discard_nr_blocks); i++) {
 		r = dm_cache_set_discard(cache->cmd, to_dblock(i),
 					 is_discarded(cache, to_dblock(i)));
-		if (r)
+		if (r) {
+			metadata_operation_failed(cache, "dm_cache_set_discard", r);
 			return r;
+		}
+	}
+
+	return 0;
+}
+
+static int write_hints(struct cache *cache)
+{
+	int r;
+
+	if (get_cache_mode(cache) >= CM_READ_ONLY)
+		return -EINVAL;
+
+	r = dm_cache_write_hints(cache->cmd, cache->policy);
+	if (r) {
+		metadata_operation_failed(cache, "dm_cache_write_hints", r);
+		return r;
 	}
 
 	return 0;
@@ -2778,26 +3232,26 @@ static bool sync_metadata(struct cache *cache)
 
 	r1 = write_dirty_bitset(cache);
 	if (r1)
-		DMERR("could not write dirty bitset");
+		DMERR("%s: could not write dirty bitset", cache_device_name(cache));
 
 	r2 = write_discard_bitset(cache);
 	if (r2)
-		DMERR("could not write discard bitset");
+		DMERR("%s: could not write discard bitset", cache_device_name(cache));
 
 	save_stats(cache);
 
-	r3 = dm_cache_write_hints(cache->cmd, cache->policy);
+	r3 = write_hints(cache);
 	if (r3)
-		DMERR("could not write hints");
+		DMERR("%s: could not write hints", cache_device_name(cache));
 
 	/*
 	 * If writing the above metadata failed, we still commit, but don't
 	 * set the clean shutdown flag.  This will effectively force every
 	 * dirty bit to be set on reload.
 	 */
-	r4 = dm_cache_commit(cache->cmd, !r1 && !r2 && !r3);
+	r4 = commit(cache, !r1 && !r2 && !r3);
 	if (r4)
-		DMERR("could not write cache metadata.  Data loss may occur.");
+		DMERR("%s: could not write cache metadata", cache_device_name(cache));
 
 	return !r1 && !r2 && !r3 && !r4;
 }
@@ -2809,10 +3263,12 @@ static void cache_postsuspend(struct dm_target *ti)
 	start_quiescing(cache);
 	wait_for_migrations(cache);
 	stop_worker(cache);
-	requeue_deferred_io(cache);
+	requeue_deferred_bios(cache);
+	requeue_deferred_cells(cache);
 	stop_quiescing(cache);
 
-	(void) sync_metadata(cache);
+	if (get_cache_mode(cache) == CM_WRITE)
+		(void) sync_metadata(cache);
 }
 
 static int load_mapping(void *context, dm_oblock_t oblock, dm_cblock_t cblock,
@@ -2935,7 +3391,8 @@ static bool can_resize(struct cache *cache, dm_cblock_t new_size)
 	while (from_cblock(new_size) < from_cblock(cache->cache_size)) {
 		new_size = to_cblock(from_cblock(new_size) + 1);
 		if (is_dirty(cache, new_size)) {
-			DMERR("unable to shrink cache; cache block %llu is dirty",
+			DMERR("%s: unable to shrink cache; cache block %llu is dirty",
+			      cache_device_name(cache),
 			      (unsigned long long) from_cblock(new_size));
 			return false;
 		}
@@ -2950,7 +3407,8 @@ static int resize_cache_dev(struct cache *cache, dm_cblock_t new_size)
 
 	r = dm_cache_resize(cache->cmd, new_size);
 	if (r) {
-		DMERR("could not resize cache metadata");
+		DMERR("%s: could not resize cache metadata", cache_device_name(cache));
+		metadata_operation_failed(cache, "dm_cache_resize", r);
 		return r;
 	}
 
@@ -2988,7 +3446,8 @@ static int cache_preresume(struct dm_target *ti)
 		r = dm_cache_load_mappings(cache->cmd, cache->policy,
 					   load_mapping, cache);
 		if (r) {
-			DMERR("could not load cache mappings");
+			DMERR("%s: could not load cache mappings", cache_device_name(cache));
+			metadata_operation_failed(cache, "dm_cache_load_mappings", r);
 			return r;
 		}
 
@@ -3008,7 +3467,8 @@ static int cache_preresume(struct dm_target *ti)
 		discard_load_info_init(cache, &li);
 		r = dm_cache_load_discards(cache->cmd, load_discard, &li);
 		if (r) {
-			DMERR("could not load origin discards");
+			DMERR("%s: could not load origin discards", cache_device_name(cache));
+			metadata_operation_failed(cache, "dm_cache_load_discards", r);
 			return r;
 		}
 		set_discard_range(&li);
@@ -3036,7 +3496,7 @@ static void cache_resume(struct dm_target *ti)
  * <#demotions> <#promotions> <#dirty>
  * <#features> <features>*
  * <#core args> <core args>
- * <policy name> <#policy args> <policy args>*
+ * <policy name> <#policy args> <policy args>* <cache metadata mode>
  */
 static void cache_status(struct dm_target *ti, status_type_t type,
 			 unsigned status_flags, char *result, unsigned maxlen)
@@ -3052,23 +3512,26 @@ static void cache_status(struct dm_target *ti, status_type_t type,
 
 	switch (type) {
 	case STATUSTYPE_INFO:
-		/* Commit to ensure statistics aren't out-of-date */
-		if (!(status_flags & DM_STATUS_NOFLUSH_FLAG) && !dm_suspended(ti)) {
-			r = dm_cache_commit(cache->cmd, false);
-			if (r)
-				DMERR("could not commit metadata for accurate status");
+		if (get_cache_mode(cache) == CM_FAIL) {
+			DMEMIT("Fail");
+			break;
 		}
 
-		r = dm_cache_get_free_metadata_block_count(cache->cmd,
-							   &nr_free_blocks_metadata);
+		/* Commit to ensure statistics aren't out-of-date */
+		if (!(status_flags & DM_STATUS_NOFLUSH_FLAG) && !dm_suspended(ti))
+			(void) commit(cache, false);
+
+		r = dm_cache_get_free_metadata_block_count(cache->cmd, &nr_free_blocks_metadata);
 		if (r) {
-			DMERR("could not get metadata free block count");
+			DMERR("%s: dm_cache_get_free_metadata_block_count returned %d",
+			      cache_device_name(cache), r);
 			goto err;
 		}
 
 		r = dm_cache_get_metadata_dev_size(cache->cmd, &nr_blocks_metadata);
 		if (r) {
-			DMERR("could not get metadata device size");
+			DMERR("%s: dm_cache_get_metadata_dev_size returned %d",
+			      cache_device_name(cache), r);
 			goto err;
 		}
 
@@ -3099,7 +3562,8 @@ static void cache_status(struct dm_target *ti, status_type_t type,
 			DMEMIT("1 writeback ");
 
 		else {
-			DMERR("internal error: unknown io mode: %d", (int) cache->features.io_mode);
+			DMERR("%s: internal error: unknown io mode: %d",
+			      cache_device_name(cache), (int) cache->features.io_mode);
 			goto err;
 		}
 
@@ -3107,11 +3571,17 @@ static void cache_status(struct dm_target *ti, status_type_t type,
 
 		DMEMIT("%s ", dm_cache_policy_get_name(cache->policy));
 		if (sz < maxlen) {
-			r = policy_emit_config_values(cache->policy, result + sz, maxlen - sz);
+			r = policy_emit_config_values(cache->policy, result, maxlen, &sz);
 			if (r)
-				DMERR("policy_emit_config_values returned %d", r);
+				DMERR("%s: policy_emit_config_values returned %d",
+				      cache_device_name(cache), r);
 		}
 
+		if (get_cache_mode(cache) == CM_READ_ONLY)
+			DMEMIT("ro ");
+		else
+			DMEMIT("rw ");
+
 		break;
 
 	case STATUSTYPE_TABLE:
@@ -3173,7 +3643,7 @@ static int parse_cblock_range(struct cache *cache, const char *str,
 		return 0;
 	}
 
-	DMERR("invalid cblock range '%s'", str);
+	DMERR("%s: invalid cblock range '%s'", cache_device_name(cache), str);
 	return -EINVAL;
 }
 
@@ -3184,17 +3654,20 @@ static int validate_cblock_range(struct cache *cache, struct cblock_range *range
 	uint64_t n = from_cblock(cache->cache_size);
 
 	if (b >= n) {
-		DMERR("begin cblock out of range: %llu >= %llu", b, n);
+		DMERR("%s: begin cblock out of range: %llu >= %llu",
+		      cache_device_name(cache), b, n);
 		return -EINVAL;
 	}
 
 	if (e > n) {
-		DMERR("end cblock out of range: %llu > %llu", e, n);
+		DMERR("%s: end cblock out of range: %llu > %llu",
+		      cache_device_name(cache), e, n);
 		return -EINVAL;
 	}
 
 	if (b >= e) {
-		DMERR("invalid cblock range: %llu >= %llu", b, e);
+		DMERR("%s: invalid cblock range: %llu >= %llu",
+		      cache_device_name(cache), b, e);
 		return -EINVAL;
 	}
 
@@ -3228,7 +3701,8 @@ static int process_invalidate_cblocks_message(struct cache *cache, unsigned coun
 	struct cblock_range range;
 
 	if (!passthrough_mode(&cache->features)) {
-		DMERR("cache has to be in passthrough mode for invalidation");
+		DMERR("%s: cache has to be in passthrough mode for invalidation",
+		      cache_device_name(cache));
 		return -EPERM;
 	}
 
@@ -3267,6 +3741,12 @@ static int cache_message(struct dm_target *ti, unsigned argc, char **argv)
 	if (!argc)
 		return -EINVAL;
 
+	if (get_cache_mode(cache) >= CM_READ_ONLY) {
+		DMERR("%s: unable to service cache target messages in READ_ONLY or FAIL mode",
+		      cache_device_name(cache));
+		return -EOPNOTSUPP;
+	}
+
 	if (!strcasecmp(argv[0], "invalidate_cblocks"))
 		return process_invalidate_cblocks_message(cache, argc - 1, (const char **) argv + 1);
 
@@ -3340,7 +3820,7 @@ static void cache_io_hints(struct dm_target *ti, struct queue_limits *limits)
 
 static struct target_type cache_target = {
 	.name = "cache",
-	.version = {1, 6, 0},
+	.version = {1, 7, 0},
 	.module = THIS_MODULE,
 	.ctr = cache_ctr,
 	.dtr = cache_dtr,
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 5503e43e5f28..0f48fed44a17 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -1,7 +1,7 @@
 /*
  * Copyright (C) 2003 Jana Saout <jana@saout.de>
  * Copyright (C) 2004 Clemens Fruhwirth <clemens@endorphin.org>
- * Copyright (C) 2006-2009 Red Hat, Inc. All rights reserved.
+ * Copyright (C) 2006-2015 Red Hat, Inc. All rights reserved.
  * Copyright (C) 2013 Milan Broz <gmazyland@gmail.com>
  *
  * This file is released under the GPL.
@@ -891,6 +891,11 @@ static void crypt_alloc_req(struct crypt_config *cc,
 		ctx->req = mempool_alloc(cc->req_pool, GFP_NOIO);
 
 	ablkcipher_request_set_tfm(ctx->req, cc->tfms[key_index]);
+
+	/*
+	 * Use REQ_MAY_BACKLOG so a cipher driver internally backlogs
+	 * requests if driver request queue is full.
+	 */
 	ablkcipher_request_set_callback(ctx->req,
 	    CRYPTO_TFM_REQ_MAY_BACKLOG | CRYPTO_TFM_REQ_MAY_SLEEP,
 	    kcryptd_async_done, dmreq_of_req(cc, ctx->req));
@@ -924,24 +929,32 @@ static int crypt_convert(struct crypt_config *cc,
 		r = crypt_convert_block(cc, ctx, ctx->req);
 
 		switch (r) {
-		/* async */
+		/*
+		 * The request was queued by a crypto driver
+		 * but the driver request queue is full, let's wait.
+		 */
 		case -EBUSY:
 			wait_for_completion(&ctx->restart);
 			reinit_completion(&ctx->restart);
-			/* fall through*/
+			/* fall through */
+		/*
+		 * The request is queued and processed asynchronously,
+		 * completion function kcryptd_async_done() will be called.
+		 */
 		case -EINPROGRESS:
 			ctx->req = NULL;
 			ctx->cc_sector++;
 			continue;
-
-		/* sync */
+		/*
+		 * The request was already processed (synchronously).
+		 */
 		case 0:
 			atomic_dec(&ctx->cc_pending);
 			ctx->cc_sector++;
 			cond_resched();
 			continue;
 
-		/* error */
+		/* There was an error while processing the request. */
 		default:
 			atomic_dec(&ctx->cc_pending);
 			return r;
@@ -1346,6 +1359,11 @@ static void kcryptd_async_done(struct crypto_async_request *async_req,
 	struct dm_crypt_io *io = container_of(ctx, struct dm_crypt_io, ctx);
 	struct crypt_config *cc = io->cc;
 
+	/*
+	 * A request from crypto driver backlog is going to be processed now,
+	 * finish the completion and continue in crypt_convert().
+	 * (Callback will be called for the second time for this request.)
+	 */
 	if (error == -EINPROGRESS) {
 		complete(&ctx->restart);
 		return;
diff --git a/drivers/md/dm-log-writes.c b/drivers/md/dm-log-writes.c
index 93e08446a87d..ad1b049ae2ab 100644
--- a/drivers/md/dm-log-writes.c
+++ b/drivers/md/dm-log-writes.c
@@ -55,8 +55,8 @@
 #define LOG_DISCARD_FLAG (1 << 2)
 #define LOG_MARK_FLAG (1 << 3)
 
-#define WRITE_LOG_VERSION 1
-#define WRITE_LOG_MAGIC 0x6a736677736872
+#define WRITE_LOG_VERSION 1ULL
+#define WRITE_LOG_MAGIC 0x6a736677736872ULL
 
 /*
  * The disk format for this is braindead simple.
diff --git a/drivers/md/dm-raid.c b/drivers/md/dm-raid.c
index 88e4c7f24986..2daa67793511 100644
--- a/drivers/md/dm-raid.c
+++ b/drivers/md/dm-raid.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (C) 2010-2011 Neil Brown
- * Copyright (C) 2010-2014 Red Hat, Inc. All rights reserved.
+ * Copyright (C) 2010-2015 Red Hat, Inc. All rights reserved.
  *
  * This file is released under the GPL.
  */
@@ -17,6 +17,7 @@
 #include <linux/device-mapper.h>
 
 #define DM_MSG_PREFIX "raid"
+#define	MAX_RAID_DEVICES	253 /* raid4/5/6 limit */
 
 static bool devices_handle_discard_safely = false;
 
@@ -45,25 +46,25 @@ struct raid_dev {
 };
 
 /*
- * Flags for rs->print_flags field.
+ * Flags for rs->ctr_flags field.
  */
-#define DMPF_SYNC              0x1
-#define DMPF_NOSYNC            0x2
-#define DMPF_REBUILD           0x4
-#define DMPF_DAEMON_SLEEP      0x8
-#define DMPF_MIN_RECOVERY_RATE 0x10
-#define DMPF_MAX_RECOVERY_RATE 0x20
-#define DMPF_MAX_WRITE_BEHIND  0x40
-#define DMPF_STRIPE_CACHE      0x80
-#define DMPF_REGION_SIZE       0x100
-#define DMPF_RAID10_COPIES     0x200
-#define DMPF_RAID10_FORMAT     0x400
+#define CTR_FLAG_SYNC              0x1
+#define CTR_FLAG_NOSYNC            0x2
+#define CTR_FLAG_REBUILD           0x4
+#define CTR_FLAG_DAEMON_SLEEP      0x8
+#define CTR_FLAG_MIN_RECOVERY_RATE 0x10
+#define CTR_FLAG_MAX_RECOVERY_RATE 0x20
+#define CTR_FLAG_MAX_WRITE_BEHIND  0x40
+#define CTR_FLAG_STRIPE_CACHE      0x80
+#define CTR_FLAG_REGION_SIZE       0x100
+#define CTR_FLAG_RAID10_COPIES     0x200
+#define CTR_FLAG_RAID10_FORMAT     0x400
 
 struct raid_set {
 	struct dm_target *ti;
 
 	uint32_t bitmap_loaded;
-	uint32_t print_flags;
+	uint32_t ctr_flags;
 
 	struct mddev md;
 	struct raid_type *raid_type;
@@ -81,6 +82,7 @@ static struct raid_type {
 	const unsigned level;		/* RAID level. */
 	const unsigned algorithm;	/* RAID algorithm. */
 } raid_types[] = {
+	{"raid0",    "RAID0 (striping)",                0, 2, 0, 0 /* NONE */},
 	{"raid1",    "RAID1 (mirroring)",               0, 2, 1, 0 /* NONE */},
 	{"raid10",   "RAID10 (striped mirrors)",        0, 2, 10, UINT_MAX /* Varies */},
 	{"raid4",    "RAID4 (dedicated parity disk)",	1, 2, 5, ALGORITHM_PARITY_0},
@@ -119,15 +121,15 @@ static int raid10_format_to_md_layout(char *format, unsigned copies)
 {
 	unsigned n = 1, f = 1;
 
-	if (!strcmp("near", format))
+	if (!strcasecmp("near", format))
 		n = copies;
 	else
 		f = copies;
 
-	if (!strcmp("offset", format))
+	if (!strcasecmp("offset", format))
 		return 0x30000 | (f << 8) | n;
 
-	if (!strcmp("far", format))
+	if (!strcasecmp("far", format))
 		return 0x20000 | (f << 8) | n;
 
 	return (f << 8) | n;
@@ -477,8 +479,6 @@ too_many:
  *                                      will form the "stripe"
  *    [[no]sync]			Force or prevent recovery of the
  *                                      entire array
- *    [devices_handle_discard_safely]	Allow discards on RAID4/5/6; useful if RAID
- *					member device(s) properly support TRIM/UNMAP
  *    [rebuild <idx>]			Rebuild the drive indicated by the index
  *    [daemon_sleep <ms>]		Time between bitmap daemon work to
  *                                      clear bits
@@ -555,12 +555,12 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 	for (i = 0; i < num_raid_params; i++) {
 		if (!strcasecmp(argv[i], "nosync")) {
 			rs->md.recovery_cp = MaxSector;
-			rs->print_flags |= DMPF_NOSYNC;
+			rs->ctr_flags |= CTR_FLAG_NOSYNC;
 			continue;
 		}
 		if (!strcasecmp(argv[i], "sync")) {
 			rs->md.recovery_cp = 0;
-			rs->print_flags |= DMPF_SYNC;
+			rs->ctr_flags |= CTR_FLAG_SYNC;
 			continue;
 		}
 
@@ -585,7 +585,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 				return -EINVAL;
 			}
 			raid10_format = argv[i];
-			rs->print_flags |= DMPF_RAID10_FORMAT;
+			rs->ctr_flags |= CTR_FLAG_RAID10_FORMAT;
 			continue;
 		}
 
@@ -602,7 +602,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 			}
 			clear_bit(In_sync, &rs->dev[value].rdev.flags);
 			rs->dev[value].rdev.recovery_offset = 0;
-			rs->print_flags |= DMPF_REBUILD;
+			rs->ctr_flags |= CTR_FLAG_REBUILD;
 		} else if (!strcasecmp(key, "write_mostly")) {
 			if (rs->raid_type->level != 1) {
 				rs->ti->error = "write_mostly option is only valid for RAID1";
@@ -618,7 +618,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 				rs->ti->error = "max_write_behind option is only valid for RAID1";
 				return -EINVAL;
 			}
-			rs->print_flags |= DMPF_MAX_WRITE_BEHIND;
+			rs->ctr_flags |= CTR_FLAG_MAX_WRITE_BEHIND;
 
 			/*
 			 * In device-mapper, we specify things in sectors, but
@@ -631,14 +631,14 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 			}
 			rs->md.bitmap_info.max_write_behind = value;
 		} else if (!strcasecmp(key, "daemon_sleep")) {
-			rs->print_flags |= DMPF_DAEMON_SLEEP;
+			rs->ctr_flags |= CTR_FLAG_DAEMON_SLEEP;
 			if (!value || (value > MAX_SCHEDULE_TIMEOUT)) {
 				rs->ti->error = "daemon sleep period out of range";
 				return -EINVAL;
 			}
 			rs->md.bitmap_info.daemon_sleep = value;
 		} else if (!strcasecmp(key, "stripe_cache")) {
-			rs->print_flags |= DMPF_STRIPE_CACHE;
+			rs->ctr_flags |= CTR_FLAG_STRIPE_CACHE;
 
 			/*
 			 * In device-mapper, we specify things in sectors, but
@@ -656,21 +656,21 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 				return -EINVAL;
 			}
 		} else if (!strcasecmp(key, "min_recovery_rate")) {
-			rs->print_flags |= DMPF_MIN_RECOVERY_RATE;
+			rs->ctr_flags |= CTR_FLAG_MIN_RECOVERY_RATE;
 			if (value > INT_MAX) {
 				rs->ti->error = "min_recovery_rate out of range";
 				return -EINVAL;
 			}
 			rs->md.sync_speed_min = (int)value;
 		} else if (!strcasecmp(key, "max_recovery_rate")) {
-			rs->print_flags |= DMPF_MAX_RECOVERY_RATE;
+			rs->ctr_flags |= CTR_FLAG_MAX_RECOVERY_RATE;
 			if (value > INT_MAX) {
 				rs->ti->error = "max_recovery_rate out of range";
 				return -EINVAL;
 			}
 			rs->md.sync_speed_max = (int)value;
 		} else if (!strcasecmp(key, "region_size")) {
-			rs->print_flags |= DMPF_REGION_SIZE;
+			rs->ctr_flags |= CTR_FLAG_REGION_SIZE;
 			region_size = value;
 		} else if (!strcasecmp(key, "raid10_copies") &&
 			   (rs->raid_type->level == 10)) {
@@ -678,7 +678,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 				rs->ti->error = "Bad value for 'raid10_copies'";
 				return -EINVAL;
 			}
-			rs->print_flags |= DMPF_RAID10_COPIES;
+			rs->ctr_flags |= CTR_FLAG_RAID10_COPIES;
 			raid10_copies = value;
 		} else {
 			DMERR("Unable to parse RAID parameter: %s", key);
@@ -720,7 +720,7 @@ static int parse_raid_params(struct raid_set *rs, char **argv,
 		rs->md.layout = raid10_format_to_md_layout(raid10_format,
 							   raid10_copies);
 		rs->md.new_layout = rs->md.layout;
-	} else if ((rs->raid_type->level > 1) &&
+	} else if ((!rs->raid_type->level || rs->raid_type->level > 1) &&
 		   sector_div(sectors_per_dev,
 			      (rs->md.raid_disks - rs->raid_type->parity_devs))) {
 		rs->ti->error = "Target length not divisible by number of data devices";
@@ -947,7 +947,7 @@ static int super_init_validation(struct mddev *mddev, struct md_rdev *rdev)
 		return -EINVAL;
 	}
 
-	if (!(rs->print_flags & (DMPF_SYNC | DMPF_NOSYNC)))
+	if (!(rs->ctr_flags & (CTR_FLAG_SYNC | CTR_FLAG_NOSYNC)))
 		mddev->recovery_cp = le64_to_cpu(sb->array_resync_offset);
 
 	/*
@@ -1026,8 +1026,9 @@ static int super_init_validation(struct mddev *mddev, struct md_rdev *rdev)
 	return 0;
 }
 
-static int super_validate(struct mddev *mddev, struct md_rdev *rdev)
+static int super_validate(struct raid_set *rs, struct md_rdev *rdev)
 {
+	struct mddev *mddev = &rs->md;
 	struct dm_raid_superblock *sb = page_address(rdev->sb_page);
 
 	/*
@@ -1037,8 +1038,10 @@ static int super_validate(struct mddev *mddev, struct md_rdev *rdev)
 	if (!mddev->events && super_init_validation(mddev, rdev))
 		return -EINVAL;
 
-	mddev->bitmap_info.offset = 4096 >> 9; /* Enable bitmap creation */
-	rdev->mddev->bitmap_info.default_offset = 4096 >> 9;
+	/* Enable bitmap creation for RAID levels != 0 */
+	mddev->bitmap_info.offset = (rs->raid_type->level) ? to_sector(4096) : 0;
+	rdev->mddev->bitmap_info.default_offset = mddev->bitmap_info.offset;
+
 	if (!test_bit(FirstUse, &rdev->flags)) {
 		rdev->recovery_offset = le64_to_cpu(sb->disk_recovery_offset);
 		if (rdev->recovery_offset != MaxSector)
@@ -1073,7 +1076,7 @@ static int analyse_superblocks(struct dm_target *ti, struct raid_set *rs)
 	freshest = NULL;
 	rdev_for_each_safe(rdev, tmp, mddev) {
 		/*
-		 * Skipping super_load due to DMPF_SYNC will cause
+		 * Skipping super_load due to CTR_FLAG_SYNC will cause
 		 * the array to undergo initialization again as
 		 * though it were new.  This is the intended effect
 		 * of the "sync" directive.
@@ -1082,7 +1085,9 @@ static int analyse_superblocks(struct dm_target *ti, struct raid_set *rs)
 		 * that the "sync" directive is disallowed during the
 		 * reshape.
 		 */
-		if (rs->print_flags & DMPF_SYNC)
+		rdev->sectors = to_sector(i_size_read(rdev->bdev->bd_inode));
+
+		if (rs->ctr_flags & CTR_FLAG_SYNC)
 			continue;
 
 		if (!rdev->meta_bdev)
@@ -1140,11 +1145,11 @@ static int analyse_superblocks(struct dm_target *ti, struct raid_set *rs)
 	 * validation for the remaining devices.
 	 */
 	ti->error = "Unable to assemble array: Invalid superblocks";
-	if (super_validate(mddev, freshest))
+	if (super_validate(rs, freshest))
 		return -EINVAL;
 
 	rdev_for_each(rdev, mddev)
-		if ((rdev != freshest) && super_validate(mddev, rdev))
+		if ((rdev != freshest) && super_validate(rs, rdev))
 			return -EINVAL;
 
 	return 0;
@@ -1243,7 +1248,7 @@ static int raid_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	}
 
 	if ((kstrtoul(argv[num_raid_params], 10, &num_raid_devs) < 0) ||
-	    (num_raid_devs >= INT_MAX)) {
+	    (num_raid_devs > MAX_RAID_DEVICES)) {
 		ti->error = "Cannot understand number of raid devices";
 		return -EINVAL;
 	}
@@ -1282,10 +1287,11 @@ static int raid_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	 */
 	configure_discard_support(ti, rs);
 
-	mutex_lock(&rs->md.reconfig_mutex);
+	/* Has to be held on running the array */
+	mddev_lock_nointr(&rs->md);
 	ret = md_run(&rs->md);
 	rs->md.in_sync = 0; /* Assume already marked dirty */
-	mutex_unlock(&rs->md.reconfig_mutex);
+	mddev_unlock(&rs->md);
 
 	if (ret) {
 		ti->error = "Fail to run raid array";
@@ -1368,34 +1374,40 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 	case STATUSTYPE_INFO:
 		DMEMIT("%s %d ", rs->raid_type->name, rs->md.raid_disks);
 
-		if (test_bit(MD_RECOVERY_RUNNING, &rs->md.recovery))
-			sync = rs->md.curr_resync_completed;
-		else
-			sync = rs->md.recovery_cp;
-
-		if (sync >= rs->md.resync_max_sectors) {
-			/*
-			 * Sync complete.
-			 */
+		if (rs->raid_type->level) {
+			if (test_bit(MD_RECOVERY_RUNNING, &rs->md.recovery))
+				sync = rs->md.curr_resync_completed;
+			else
+				sync = rs->md.recovery_cp;
+
+			if (sync >= rs->md.resync_max_sectors) {
+				/*
+				 * Sync complete.
+				 */
+				array_in_sync = 1;
+				sync = rs->md.resync_max_sectors;
+			} else if (test_bit(MD_RECOVERY_REQUESTED, &rs->md.recovery)) {
+				/*
+				 * If "check" or "repair" is occurring, the array has
+				 * undergone and initial sync and the health characters
+				 * should not be 'a' anymore.
+				 */
+				array_in_sync = 1;
+			} else {
+				/*
+				 * The array may be doing an initial sync, or it may
+				 * be rebuilding individual components.  If all the
+				 * devices are In_sync, then it is the array that is
+				 * being initialized.
+				 */
+				for (i = 0; i < rs->md.raid_disks; i++)
+					if (!test_bit(In_sync, &rs->dev[i].rdev.flags))
+						array_in_sync = 1;
+			}
+		} else {
+			/* RAID0 */
 			array_in_sync = 1;
 			sync = rs->md.resync_max_sectors;
-		} else if (test_bit(MD_RECOVERY_REQUESTED, &rs->md.recovery)) {
-			/*
-			 * If "check" or "repair" is occurring, the array has
-			 * undergone and initial sync and the health characters
-			 * should not be 'a' anymore.
-			 */
-			array_in_sync = 1;
-		} else {
-			/*
-			 * The array may be doing an initial sync, or it may
-			 * be rebuilding individual components.  If all the
-			 * devices are In_sync, then it is the array that is
-			 * being initialized.
-			 */
-			for (i = 0; i < rs->md.raid_disks; i++)
-				if (!test_bit(In_sync, &rs->dev[i].rdev.flags))
-					array_in_sync = 1;
 		}
 
 		/*
@@ -1446,7 +1458,7 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 	case STATUSTYPE_TABLE:
 		/* The string you would use to construct this array */
 		for (i = 0; i < rs->md.raid_disks; i++) {
-			if ((rs->print_flags & DMPF_REBUILD) &&
+			if ((rs->ctr_flags & CTR_FLAG_REBUILD) &&
 			    rs->dev[i].data_dev &&
 			    !test_bit(In_sync, &rs->dev[i].rdev.flags))
 				raid_param_cnt += 2; /* for rebuilds */
@@ -1455,33 +1467,33 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 				raid_param_cnt += 2;
 		}
 
-		raid_param_cnt += (hweight32(rs->print_flags & ~DMPF_REBUILD) * 2);
-		if (rs->print_flags & (DMPF_SYNC | DMPF_NOSYNC))
+		raid_param_cnt += (hweight32(rs->ctr_flags & ~CTR_FLAG_REBUILD) * 2);
+		if (rs->ctr_flags & (CTR_FLAG_SYNC | CTR_FLAG_NOSYNC))
 			raid_param_cnt--;
 
 		DMEMIT("%s %u %u", rs->raid_type->name,
 		       raid_param_cnt, rs->md.chunk_sectors);
 
-		if ((rs->print_flags & DMPF_SYNC) &&
+		if ((rs->ctr_flags & CTR_FLAG_SYNC) &&
 		    (rs->md.recovery_cp == MaxSector))
 			DMEMIT(" sync");
-		if (rs->print_flags & DMPF_NOSYNC)
+		if (rs->ctr_flags & CTR_FLAG_NOSYNC)
 			DMEMIT(" nosync");
 
 		for (i = 0; i < rs->md.raid_disks; i++)
-			if ((rs->print_flags & DMPF_REBUILD) &&
+			if ((rs->ctr_flags & CTR_FLAG_REBUILD) &&
 			    rs->dev[i].data_dev &&
 			    !test_bit(In_sync, &rs->dev[i].rdev.flags))
 				DMEMIT(" rebuild %u", i);
 
-		if (rs->print_flags & DMPF_DAEMON_SLEEP)
+		if (rs->ctr_flags & CTR_FLAG_DAEMON_SLEEP)
 			DMEMIT(" daemon_sleep %lu",
 			       rs->md.bitmap_info.daemon_sleep);
 
-		if (rs->print_flags & DMPF_MIN_RECOVERY_RATE)
+		if (rs->ctr_flags & CTR_FLAG_MIN_RECOVERY_RATE)
 			DMEMIT(" min_recovery_rate %d", rs->md.sync_speed_min);
 
-		if (rs->print_flags & DMPF_MAX_RECOVERY_RATE)
+		if (rs->ctr_flags & CTR_FLAG_MAX_RECOVERY_RATE)
 			DMEMIT(" max_recovery_rate %d", rs->md.sync_speed_max);
 
 		for (i = 0; i < rs->md.raid_disks; i++)
@@ -1489,11 +1501,11 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 			    test_bit(WriteMostly, &rs->dev[i].rdev.flags))
 				DMEMIT(" write_mostly %u", i);
 
-		if (rs->print_flags & DMPF_MAX_WRITE_BEHIND)
+		if (rs->ctr_flags & CTR_FLAG_MAX_WRITE_BEHIND)
 			DMEMIT(" max_write_behind %lu",
 			       rs->md.bitmap_info.max_write_behind);
 
-		if (rs->print_flags & DMPF_STRIPE_CACHE) {
+		if (rs->ctr_flags & CTR_FLAG_STRIPE_CACHE) {
 			struct r5conf *conf = rs->md.private;
 
 			/* convert from kiB to sectors */
@@ -1501,15 +1513,15 @@ static void raid_status(struct dm_target *ti, status_type_t type,
 			       conf ? conf->max_nr_stripes * 2 : 0);
 		}
 
-		if (rs->print_flags & DMPF_REGION_SIZE)
+		if (rs->ctr_flags & CTR_FLAG_REGION_SIZE)
 			DMEMIT(" region_size %lu",
 			       rs->md.bitmap_info.chunksize >> 9);
 
-		if (rs->print_flags & DMPF_RAID10_COPIES)
+		if (rs->ctr_flags & CTR_FLAG_RAID10_COPIES)
 			DMEMIT(" raid10_copies %u",
 			       raid10_md_layout_to_copies(rs->md.layout));
 
-		if (rs->print_flags & DMPF_RAID10_FORMAT)
+		if (rs->ctr_flags & CTR_FLAG_RAID10_FORMAT)
 			DMEMIT(" raid10_format %s",
 			       raid10_md_layout_to_format(rs->md.layout));
 
@@ -1684,26 +1696,48 @@ static void raid_resume(struct dm_target *ti)
 {
 	struct raid_set *rs = ti->private;
 
-	set_bit(MD_CHANGE_DEVS, &rs->md.flags);
-	if (!rs->bitmap_loaded) {
-		bitmap_load(&rs->md);
-		rs->bitmap_loaded = 1;
-	} else {
-		/*
-		 * A secondary resume while the device is active.
-		 * Take this opportunity to check whether any failed
-		 * devices are reachable again.
-		 */
-		attempt_restore_of_faulty_devices(rs);
+	if (rs->raid_type->level) {
+		set_bit(MD_CHANGE_DEVS, &rs->md.flags);
+
+		if (!rs->bitmap_loaded) {
+			bitmap_load(&rs->md);
+			rs->bitmap_loaded = 1;
+		} else {
+			/*
+			 * A secondary resume while the device is active.
+			 * Take this opportunity to check whether any failed
+			 * devices are reachable again.
+			 */
+			attempt_restore_of_faulty_devices(rs);
+		}
+
+		clear_bit(MD_RECOVERY_FROZEN, &rs->md.recovery);
 	}
 
-	clear_bit(MD_RECOVERY_FROZEN, &rs->md.recovery);
 	mddev_resume(&rs->md);
 }
 
+static int raid_merge(struct dm_target *ti, struct bvec_merge_data *bvm,
+		      struct bio_vec *biovec, int max_size)
+{
+	struct raid_set *rs = ti->private;
+	struct md_personality *pers = rs->md.pers;
+
+	if (pers && pers->mergeable_bvec)
+		return min(max_size, pers->mergeable_bvec(&rs->md, bvm, biovec));
+
+	/*
+	 * In case we can't request the personality because
+	 * the raid set is not running yet
+	 *
+	 * -> return safe minimum
+	 */
+	return rs->md.chunk_sectors;
+}
+
 static struct target_type raid_target = {
 	.name = "raid",
-	.version = {1, 6, 0},
+	.version = {1, 7, 0},
 	.module = THIS_MODULE,
 	.ctr = raid_ctr,
 	.dtr = raid_dtr,
@@ -1715,6 +1749,7 @@ static struct target_type raid_target = {
 	.presuspend = raid_presuspend,
 	.postsuspend = raid_postsuspend,
 	.resume = raid_resume,
+	.merge = raid_merge,
 };
 
 static int __init dm_raid_init(void)
diff --git a/drivers/md/dm-raid1.c b/drivers/md/dm-raid1.c
index 089d62751f7f..d83696bf403b 100644
--- a/drivers/md/dm-raid1.c
+++ b/drivers/md/dm-raid1.c
@@ -23,8 +23,10 @@
 
 #define MAX_RECOVERY 1	/* Maximum number of regions recovered in parallel. */
 
-#define DM_RAID1_HANDLE_ERRORS 0x01
+#define DM_RAID1_HANDLE_ERRORS	0x01
+#define DM_RAID1_KEEP_LOG	0x02
 #define errors_handled(p)	((p)->features & DM_RAID1_HANDLE_ERRORS)
+#define keep_log(p)		((p)->features & DM_RAID1_KEEP_LOG)
 
 static DECLARE_WAIT_QUEUE_HEAD(_kmirrord_recovery_stopped);
 
@@ -229,7 +231,7 @@ static void fail_mirror(struct mirror *m, enum dm_raid1_error error_type)
 	if (m != get_default_mirror(ms))
 		goto out;
 
-	if (!ms->in_sync) {
+	if (!ms->in_sync && !keep_log(ms)) {
 		/*
 		 * Better to issue requests to same failing device
 		 * than to risk returning corrupt data.
@@ -370,6 +372,17 @@ static int recover(struct mirror_set *ms, struct dm_region *reg)
 	return r;
 }
 
+static void reset_ms_flags(struct mirror_set *ms)
+{
+	unsigned int m;
+
+	ms->leg_failure = 0;
+	for (m = 0; m < ms->nr_mirrors; m++) {
+		atomic_set(&(ms->mirror[m].error_count), 0);
+		ms->mirror[m].error_type = 0;
+	}
+}
+
 static void do_recovery(struct mirror_set *ms)
 {
 	struct dm_region *reg;
@@ -398,6 +411,7 @@ static void do_recovery(struct mirror_set *ms)
 		/* the sync is complete */
 		dm_table_event(ms->ti->table);
 		ms->in_sync = 1;
+		reset_ms_flags(ms);
 	}
 }
 
@@ -759,7 +773,7 @@ static void do_writes(struct mirror_set *ms, struct bio_list *writes)
 		dm_rh_delay(ms->rh, bio);
 
 	while ((bio = bio_list_pop(&nosync))) {
-		if (unlikely(ms->leg_failure) && errors_handled(ms)) {
+		if (unlikely(ms->leg_failure) && errors_handled(ms) && !keep_log(ms)) {
 			spin_lock_irq(&ms->lock);
 			bio_list_add(&ms->failures, bio);
 			spin_unlock_irq(&ms->lock);
@@ -803,15 +817,21 @@ static void do_failures(struct mirror_set *ms, struct bio_list *failures)
 
 		/*
 		 * If all the legs are dead, fail the I/O.
-		 * If we have been told to handle errors, hold the bio
-		 * and wait for userspace to deal with the problem.
+		 * If the device has failed and keep_log is enabled,
+		 * fail the I/O.
+		 *
+		 * If we have been told to handle errors, and keep_log
+		 * isn't enabled, hold the bio and wait for userspace to
+		 * deal with the problem.
+		 *
 		 * Otherwise pretend that the I/O succeeded. (This would
 		 * be wrong if the failed leg returned after reboot and
 		 * got replicated back to the good legs.)
 		 */
-		if (!get_valid_mirror(ms))
+
+		if (unlikely(!get_valid_mirror(ms) || (keep_log(ms) && ms->log_failure)))
 			bio_endio(bio, -EIO);
-		else if (errors_handled(ms))
+		else if (errors_handled(ms) && !keep_log(ms))
 			hold_bio(ms, bio);
 		else
 			bio_endio(bio, 0);
@@ -987,6 +1007,7 @@ static int parse_features(struct mirror_set *ms, unsigned argc, char **argv,
 	unsigned num_features;
 	struct dm_target *ti = ms->ti;
 	char dummy;
+	int i;
 
 	*args_used = 0;
 
@@ -1007,15 +1028,25 @@ static int parse_features(struct mirror_set *ms, unsigned argc, char **argv,
 		return -EINVAL;
 	}
 
-	if (!strcmp("handle_errors", argv[0]))
-		ms->features |= DM_RAID1_HANDLE_ERRORS;
-	else {
-		ti->error = "Unrecognised feature requested";
+	for (i = 0; i < num_features; i++) {
+		if (!strcmp("handle_errors", argv[0]))
+			ms->features |= DM_RAID1_HANDLE_ERRORS;
+		else if (!strcmp("keep_log", argv[0]))
+			ms->features |= DM_RAID1_KEEP_LOG;
+		else {
+			ti->error = "Unrecognised feature requested";
+			return -EINVAL;
+		}
+
+		argc--;
+		argv++;
+		(*args_used)++;
+	}
+	if (!errors_handled(ms) && keep_log(ms)) {
+		ti->error = "keep_log feature requires the handle_errors feature";
 		return -EINVAL;
 	}
 
-	(*args_used)++;
-
 	return 0;
 }
 
@@ -1029,7 +1060,7 @@ static int parse_features(struct mirror_set *ms, unsigned argc, char **argv,
  * log_type is "core" or "disk"
  * #log_params is between 1 and 3
  *
- * If present, features must be "handle_errors".
+ * If present, supported features are "handle_errors" and "keep_log".
  */
 static int mirror_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 {
@@ -1254,8 +1285,6 @@ static int mirror_end_io(struct dm_target *ti, struct bio *bio, int error)
 			dm_bio_restore(bd, bio);
 			bio_record->details.bi_bdev = NULL;
 
-			atomic_inc(&bio->bi_remaining);
-
 			queue_bio(ms, bio, rw);
 			return DM_ENDIO_INCOMPLETE;
 		}
@@ -1365,6 +1394,7 @@ static void mirror_status(struct dm_target *ti, status_type_t type,
 			  unsigned status_flags, char *result, unsigned maxlen)
 {
 	unsigned int m, sz = 0;
+	int num_feature_args = 0;
 	struct mirror_set *ms = (struct mirror_set *) ti->private;
 	struct dm_dirty_log *log = dm_rh_dirty_log(ms->rh);
 	char buffer[ms->nr_mirrors + 1];
@@ -1394,8 +1424,17 @@ static void mirror_status(struct dm_target *ti, status_type_t type,
 			DMEMIT(" %s %llu", ms->mirror[m].dev->name,
 			       (unsigned long long)ms->mirror[m].offset);
 
-		if (ms->features & DM_RAID1_HANDLE_ERRORS)
-			DMEMIT(" 1 handle_errors");
+		num_feature_args += !!errors_handled(ms);
+		num_feature_args += !!keep_log(ms);
+		if (num_feature_args) {
+			DMEMIT(" %d", num_feature_args);
+			if (errors_handled(ms))
+				DMEMIT(" handle_errors");
+			if (keep_log(ms))
+				DMEMIT(" keep_log");
+		}
+
+		break;
 	}
 }
 
@@ -1415,7 +1454,7 @@ static int mirror_iterate_devices(struct dm_target *ti,
 
 static struct target_type mirror_target = {
 	.name	 = "mirror",
-	.version = {1, 13, 2},
+	.version = {1, 14, 0},
 	.module	 = THIS_MODULE,
 	.ctr	 = mirror_ctr,
 	.dtr	 = mirror_dtr,
diff --git a/drivers/md/dm-snap.c b/drivers/md/dm-snap.c
index f83a0f3fc365..7c82d3ccce87 100644
--- a/drivers/md/dm-snap.c
+++ b/drivers/md/dm-snap.c
@@ -1478,7 +1478,6 @@ out:
 	if (full_bio) {
 		full_bio->bi_end_io = pe->full_bio_end_io;
 		full_bio->bi_private = pe->full_bio_private;
-		atomic_inc(&full_bio->bi_remaining);
 	}
 	increment_pending_exceptions_done_count();
 
diff --git a/drivers/md/dm-stats.c b/drivers/md/dm-stats.c
index f478a4c96d2f..8a8b48fa901a 100644
--- a/drivers/md/dm-stats.c
+++ b/drivers/md/dm-stats.c
@@ -29,30 +29,37 @@ struct dm_stat_percpu {
 	unsigned long long io_ticks[2];
 	unsigned long long io_ticks_total;
 	unsigned long long time_in_queue;
+	unsigned long long *histogram;
 };
 
 struct dm_stat_shared {
 	atomic_t in_flight[2];
-	unsigned long stamp;
+	unsigned long long stamp;
 	struct dm_stat_percpu tmp;
 };
 
 struct dm_stat {
 	struct list_head list_entry;
 	int id;
+	unsigned stat_flags;
 	size_t n_entries;
 	sector_t start;
 	sector_t end;
 	sector_t step;
+	unsigned n_histogram_entries;
+	unsigned long long *histogram_boundaries;
 	const char *program_id;
 	const char *aux_data;
 	struct rcu_head rcu_head;
 	size_t shared_alloc_size;
 	size_t percpu_alloc_size;
+	size_t histogram_alloc_size;
 	struct dm_stat_percpu *stat_percpu[NR_CPUS];
 	struct dm_stat_shared stat_shared[0];
 };
 
+#define STAT_PRECISE_TIMESTAMPS		1
+
 struct dm_stats_last_position {
 	sector_t last_sector;
 	unsigned last_rw;
@@ -160,10 +167,7 @@ static void dm_kvfree(void *ptr, size_t alloc_size)
 
 	free_shared_memory(alloc_size);
 
-	if (is_vmalloc_addr(ptr))
-		vfree(ptr);
-	else
-		kfree(ptr);
+	kvfree(ptr);
 }
 
 static void dm_stat_free(struct rcu_head *head)
@@ -173,8 +177,11 @@ static void dm_stat_free(struct rcu_head *head)
 
 	kfree(s->program_id);
 	kfree(s->aux_data);
-	for_each_possible_cpu(cpu)
+	for_each_possible_cpu(cpu) {
+		dm_kvfree(s->stat_percpu[cpu][0].histogram, s->histogram_alloc_size);
 		dm_kvfree(s->stat_percpu[cpu], s->percpu_alloc_size);
+	}
+	dm_kvfree(s->stat_shared[0].tmp.histogram, s->histogram_alloc_size);
 	dm_kvfree(s, s->shared_alloc_size);
 }
 
@@ -227,7 +234,10 @@ void dm_stats_cleanup(struct dm_stats *stats)
 }
 
 static int dm_stats_create(struct dm_stats *stats, sector_t start, sector_t end,
-			   sector_t step, const char *program_id, const char *aux_data,
+			   sector_t step, unsigned stat_flags,
+			   unsigned n_histogram_entries,
+			   unsigned long long *histogram_boundaries,
+			   const char *program_id, const char *aux_data,
 			   void (*suspend_callback)(struct mapped_device *),
 			   void (*resume_callback)(struct mapped_device *),
 			   struct mapped_device *md)
@@ -238,6 +248,7 @@ static int dm_stats_create(struct dm_stats *stats, sector_t start, sector_t end,
 	size_t ni;
 	size_t shared_alloc_size;
 	size_t percpu_alloc_size;
+	size_t histogram_alloc_size;
 	struct dm_stat_percpu *p;
 	int cpu;
 	int ret_id;
@@ -261,19 +272,34 @@ static int dm_stats_create(struct dm_stats *stats, sector_t start, sector_t end,
 	if (percpu_alloc_size / sizeof(struct dm_stat_percpu) != n_entries)
 		return -EOVERFLOW;
 
-	if (!check_shared_memory(shared_alloc_size + num_possible_cpus() * percpu_alloc_size))
+	histogram_alloc_size = (n_histogram_entries + 1) * (size_t)n_entries * sizeof(unsigned long long);
+	if (histogram_alloc_size / (n_histogram_entries + 1) != (size_t)n_entries * sizeof(unsigned long long))
+		return -EOVERFLOW;
+
+	if (!check_shared_memory(shared_alloc_size + histogram_alloc_size +
+				 num_possible_cpus() * (percpu_alloc_size + histogram_alloc_size)))
 		return -ENOMEM;
 
 	s = dm_kvzalloc(shared_alloc_size, NUMA_NO_NODE);
 	if (!s)
 		return -ENOMEM;
 
+	s->stat_flags = stat_flags;
 	s->n_entries = n_entries;
 	s->start = start;
 	s->end = end;
 	s->step = step;
 	s->shared_alloc_size = shared_alloc_size;
 	s->percpu_alloc_size = percpu_alloc_size;
+	s->histogram_alloc_size = histogram_alloc_size;
+
+	s->n_histogram_entries = n_histogram_entries;
+	s->histogram_boundaries = kmemdup(histogram_boundaries,
+					  s->n_histogram_entries * sizeof(unsigned long long), GFP_KERNEL);
+	if (!s->histogram_boundaries) {
+		r = -ENOMEM;
+		goto out;
+	}
 
 	s->program_id = kstrdup(program_id, GFP_KERNEL);
 	if (!s->program_id) {
@@ -291,6 +317,19 @@ static int dm_stats_create(struct dm_stats *stats, sector_t start, sector_t end,
 		atomic_set(&s->stat_shared[ni].in_flight[WRITE], 0);
 	}
 
+	if (s->n_histogram_entries) {
+		unsigned long long *hi;
+		hi = dm_kvzalloc(s->histogram_alloc_size, NUMA_NO_NODE);
+		if (!hi) {
+			r = -ENOMEM;
+			goto out;
+		}
+		for (ni = 0; ni < n_entries; ni++) {
+			s->stat_shared[ni].tmp.histogram = hi;
+			hi += s->n_histogram_entries + 1;
+		}
+	}
+
 	for_each_possible_cpu(cpu) {
 		p = dm_kvzalloc(percpu_alloc_size, cpu_to_node(cpu));
 		if (!p) {
@@ -298,6 +337,18 @@ static int dm_stats_create(struct dm_stats *stats, sector_t start, sector_t end,
 			goto out;
 		}
 		s->stat_percpu[cpu] = p;
+		if (s->n_histogram_entries) {
+			unsigned long long *hi;
+			hi = dm_kvzalloc(s->histogram_alloc_size, cpu_to_node(cpu));
+			if (!hi) {
+				r = -ENOMEM;
+				goto out;
+			}
+			for (ni = 0; ni < n_entries; ni++) {
+				p[ni].histogram = hi;
+				hi += s->n_histogram_entries + 1;
+			}
+		}
 	}
 
 	/*
@@ -375,9 +426,11 @@ static int dm_stats_delete(struct dm_stats *stats, int id)
 	 * vfree can't be called from RCU callback
 	 */
 	for_each_possible_cpu(cpu)
-		if (is_vmalloc_addr(s->stat_percpu))
+		if (is_vmalloc_addr(s->stat_percpu) ||
+		    is_vmalloc_addr(s->stat_percpu[cpu][0].histogram))
 			goto do_sync_free;
-	if (is_vmalloc_addr(s)) {
+	if (is_vmalloc_addr(s) ||
+	    is_vmalloc_addr(s->stat_shared[0].tmp.histogram)) {
 do_sync_free:
 		synchronize_rcu_expedited();
 		dm_stat_free(&s->rcu_head);
@@ -417,18 +470,24 @@ static int dm_stats_list(struct dm_stats *stats, const char *program,
 	return 1;
 }
 
-static void dm_stat_round(struct dm_stat_shared *shared, struct dm_stat_percpu *p)
+static void dm_stat_round(struct dm_stat *s, struct dm_stat_shared *shared,
+			  struct dm_stat_percpu *p)
 {
 	/*
 	 * This is racy, but so is part_round_stats_single.
 	 */
-	unsigned long now = jiffies;
-	unsigned in_flight_read;
-	unsigned in_flight_write;
-	unsigned long difference = now - shared->stamp;
+	unsigned long long now, difference;
+	unsigned in_flight_read, in_flight_write;
+
+	if (likely(!(s->stat_flags & STAT_PRECISE_TIMESTAMPS)))
+		now = jiffies;
+	else
+		now = ktime_to_ns(ktime_get());
 
+	difference = now - shared->stamp;
 	if (!difference)
 		return;
+
 	in_flight_read = (unsigned)atomic_read(&shared->in_flight[READ]);
 	in_flight_write = (unsigned)atomic_read(&shared->in_flight[WRITE]);
 	if (in_flight_read)
@@ -443,8 +502,9 @@ static void dm_stat_round(struct dm_stat_shared *shared, struct dm_stat_percpu *
 }
 
 static void dm_stat_for_entry(struct dm_stat *s, size_t entry,
-			      unsigned long bi_rw, sector_t len, bool merged,
-			      bool end, unsigned long duration)
+			      unsigned long bi_rw, sector_t len,
+			      struct dm_stats_aux *stats_aux, bool end,
+			      unsigned long duration_jiffies)
 {
 	unsigned long idx = bi_rw & REQ_WRITE;
 	struct dm_stat_shared *shared = &s->stat_shared[entry];
@@ -474,15 +534,35 @@ static void dm_stat_for_entry(struct dm_stat *s, size_t entry,
 	p = &s->stat_percpu[smp_processor_id()][entry];
 
 	if (!end) {
-		dm_stat_round(shared, p);
+		dm_stat_round(s, shared, p);
 		atomic_inc(&shared->in_flight[idx]);
 	} else {
-		dm_stat_round(shared, p);
+		unsigned long long duration;
+		dm_stat_round(s, shared, p);
 		atomic_dec(&shared->in_flight[idx]);
 		p->sectors[idx] += len;
 		p->ios[idx] += 1;
-		p->merges[idx] += merged;
-		p->ticks[idx] += duration;
+		p->merges[idx] += stats_aux->merged;
+		if (!(s->stat_flags & STAT_PRECISE_TIMESTAMPS)) {
+			p->ticks[idx] += duration_jiffies;
+			duration = jiffies_to_msecs(duration_jiffies);
+		} else {
+			p->ticks[idx] += stats_aux->duration_ns;
+			duration = stats_aux->duration_ns;
+		}
+		if (s->n_histogram_entries) {
+			unsigned lo = 0, hi = s->n_histogram_entries + 1;
+			while (lo + 1 < hi) {
+				unsigned mid = (lo + hi) / 2;
+				if (s->histogram_boundaries[mid - 1] > duration) {
+					hi = mid;
+				} else {
+					lo = mid;
+				}
+
+			}
+			p->histogram[lo]++;
+		}
 	}
 
 #if BITS_PER_LONG == 32
@@ -494,7 +574,7 @@ static void dm_stat_for_entry(struct dm_stat *s, size_t entry,
 
 static void __dm_stat_bio(struct dm_stat *s, unsigned long bi_rw,
 			  sector_t bi_sector, sector_t end_sector,
-			  bool end, unsigned long duration,
+			  bool end, unsigned long duration_jiffies,
 			  struct dm_stats_aux *stats_aux)
 {
 	sector_t rel_sector, offset, todo, fragment_len;
@@ -523,7 +603,7 @@ static void __dm_stat_bio(struct dm_stat *s, unsigned long bi_rw,
 		if (fragment_len > s->step - offset)
 			fragment_len = s->step - offset;
 		dm_stat_for_entry(s, entry, bi_rw, fragment_len,
-				  stats_aux->merged, end, duration);
+				  stats_aux, end, duration_jiffies);
 		todo -= fragment_len;
 		entry++;
 		offset = 0;
@@ -532,11 +612,13 @@ static void __dm_stat_bio(struct dm_stat *s, unsigned long bi_rw,
 
 void dm_stats_account_io(struct dm_stats *stats, unsigned long bi_rw,
 			 sector_t bi_sector, unsigned bi_sectors, bool end,
-			 unsigned long duration, struct dm_stats_aux *stats_aux)
+			 unsigned long duration_jiffies,
+			 struct dm_stats_aux *stats_aux)
 {
 	struct dm_stat *s;
 	sector_t end_sector;
 	struct dm_stats_last_position *last;
+	bool got_precise_time;
 
 	if (unlikely(!bi_sectors))
 		return;
@@ -560,8 +642,17 @@ void dm_stats_account_io(struct dm_stats *stats, unsigned long bi_rw,
 
 	rcu_read_lock();
 
-	list_for_each_entry_rcu(s, &stats->list, list_entry)
-		__dm_stat_bio(s, bi_rw, bi_sector, end_sector, end, duration, stats_aux);
+	got_precise_time = false;
+	list_for_each_entry_rcu(s, &stats->list, list_entry) {
+		if (s->stat_flags & STAT_PRECISE_TIMESTAMPS && !got_precise_time) {
+			if (!end)
+				stats_aux->duration_ns = ktime_to_ns(ktime_get());
+			else
+				stats_aux->duration_ns = ktime_to_ns(ktime_get()) - stats_aux->duration_ns;
+			got_precise_time = true;
+		}
+		__dm_stat_bio(s, bi_rw, bi_sector, end_sector, end, duration_jiffies, stats_aux);
+	}
 
 	rcu_read_unlock();
 }
@@ -574,10 +665,25 @@ static void __dm_stat_init_temporary_percpu_totals(struct dm_stat_shared *shared
 
 	local_irq_disable();
 	p = &s->stat_percpu[smp_processor_id()][x];
-	dm_stat_round(shared, p);
+	dm_stat_round(s, shared, p);
 	local_irq_enable();
 
-	memset(&shared->tmp, 0, sizeof(shared->tmp));
+	shared->tmp.sectors[READ] = 0;
+	shared->tmp.sectors[WRITE] = 0;
+	shared->tmp.ios[READ] = 0;
+	shared->tmp.ios[WRITE] = 0;
+	shared->tmp.merges[READ] = 0;
+	shared->tmp.merges[WRITE] = 0;
+	shared->tmp.ticks[READ] = 0;
+	shared->tmp.ticks[WRITE] = 0;
+	shared->tmp.io_ticks[READ] = 0;
+	shared->tmp.io_ticks[WRITE] = 0;
+	shared->tmp.io_ticks_total = 0;
+	shared->tmp.time_in_queue = 0;
+
+	if (s->n_histogram_entries)
+		memset(shared->tmp.histogram, 0, (s->n_histogram_entries + 1) * sizeof(unsigned long long));
+
 	for_each_possible_cpu(cpu) {
 		p = &s->stat_percpu[cpu][x];
 		shared->tmp.sectors[READ] += ACCESS_ONCE(p->sectors[READ]);
@@ -592,6 +698,11 @@ static void __dm_stat_init_temporary_percpu_totals(struct dm_stat_shared *shared
 		shared->tmp.io_ticks[WRITE] += ACCESS_ONCE(p->io_ticks[WRITE]);
 		shared->tmp.io_ticks_total += ACCESS_ONCE(p->io_ticks_total);
 		shared->tmp.time_in_queue += ACCESS_ONCE(p->time_in_queue);
+		if (s->n_histogram_entries) {
+			unsigned i;
+			for (i = 0; i < s->n_histogram_entries + 1; i++)
+				shared->tmp.histogram[i] += ACCESS_ONCE(p->histogram[i]);
+		}
 	}
 }
 
@@ -621,6 +732,15 @@ static void __dm_stat_clear(struct dm_stat *s, size_t idx_start, size_t idx_end,
 		p->io_ticks_total -= shared->tmp.io_ticks_total;
 		p->time_in_queue -= shared->tmp.time_in_queue;
 		local_irq_enable();
+		if (s->n_histogram_entries) {
+			unsigned i;
+			for (i = 0; i < s->n_histogram_entries + 1; i++) {
+				local_irq_disable();
+				p = &s->stat_percpu[smp_processor_id()][x];
+				p->histogram[i] -= shared->tmp.histogram[i];
+				local_irq_enable();
+			}
+		}
 	}
 }
 
@@ -646,11 +766,15 @@ static int dm_stats_clear(struct dm_stats *stats, int id)
 /*
  * This is like jiffies_to_msec, but works for 64-bit values.
  */
-static unsigned long long dm_jiffies_to_msec64(unsigned long long j)
+static unsigned long long dm_jiffies_to_msec64(struct dm_stat *s, unsigned long long j)
 {
-	unsigned long long result = 0;
+	unsigned long long result;
 	unsigned mult;
 
+	if (s->stat_flags & STAT_PRECISE_TIMESTAMPS)
+		return j;
+
+	result = 0;
 	if (j)
 		result = jiffies_to_msecs(j & 0x3fffff);
 	if (j >= 1 << 22) {
@@ -706,22 +830,29 @@ static int dm_stats_print(struct dm_stats *stats, int id,
 
 		__dm_stat_init_temporary_percpu_totals(shared, s, x);
 
-		DMEMIT("%llu+%llu %llu %llu %llu %llu %llu %llu %llu %llu %d %llu %llu %llu %llu\n",
+		DMEMIT("%llu+%llu %llu %llu %llu %llu %llu %llu %llu %llu %d %llu %llu %llu %llu",
 		       (unsigned long long)start,
 		       (unsigned long long)step,
 		       shared->tmp.ios[READ],
 		       shared->tmp.merges[READ],
 		       shared->tmp.sectors[READ],
-		       dm_jiffies_to_msec64(shared->tmp.ticks[READ]),
+		       dm_jiffies_to_msec64(s, shared->tmp.ticks[READ]),
 		       shared->tmp.ios[WRITE],
 		       shared->tmp.merges[WRITE],
 		       shared->tmp.sectors[WRITE],
-		       dm_jiffies_to_msec64(shared->tmp.ticks[WRITE]),
+		       dm_jiffies_to_msec64(s, shared->tmp.ticks[WRITE]),
 		       dm_stat_in_flight(shared),
-		       dm_jiffies_to_msec64(shared->tmp.io_ticks_total),
-		       dm_jiffies_to_msec64(shared->tmp.time_in_queue),
-		       dm_jiffies_to_msec64(shared->tmp.io_ticks[READ]),
-		       dm_jiffies_to_msec64(shared->tmp.io_ticks[WRITE]));
+		       dm_jiffies_to_msec64(s, shared->tmp.io_ticks_total),
+		       dm_jiffies_to_msec64(s, shared->tmp.time_in_queue),
+		       dm_jiffies_to_msec64(s, shared->tmp.io_ticks[READ]),
+		       dm_jiffies_to_msec64(s, shared->tmp.io_ticks[WRITE]));
+		if (s->n_histogram_entries) {
+			unsigned i;
+			for (i = 0; i < s->n_histogram_entries + 1; i++) {
+				DMEMIT("%s%llu", !i ? " " : ":", shared->tmp.histogram[i]);
+			}
+		}
+		DMEMIT("\n");
 
 		if (unlikely(sz + 1 >= maxlen))
 			goto buffer_overflow;
@@ -763,55 +894,134 @@ static int dm_stats_set_aux(struct dm_stats *stats, int id, const char *aux_data
 	return 0;
 }
 
+static int parse_histogram(const char *h, unsigned *n_histogram_entries,
+			   unsigned long long **histogram_boundaries)
+{
+	const char *q;
+	unsigned n;
+	unsigned long long last;
+
+	*n_histogram_entries = 1;
+	for (q = h; *q; q++)
+		if (*q == ',')
+			(*n_histogram_entries)++;
+
+	*histogram_boundaries = kmalloc(*n_histogram_entries * sizeof(unsigned long long), GFP_KERNEL);
+	if (!*histogram_boundaries)
+		return -ENOMEM;
+
+	n = 0;
+	last = 0;
+	while (1) {
+		unsigned long long hi;
+		int s;
+		char ch;
+		s = sscanf(h, "%llu%c", &hi, &ch);
+		if (!s || (s == 2 && ch != ','))
+			return -EINVAL;
+		if (hi <= last)
+			return -EINVAL;
+		last = hi;
+		(*histogram_boundaries)[n] = hi;
+		if (s == 1)
+			return 0;
+		h = strchr(h, ',') + 1;
+		n++;
+	}
+}
+
 static int message_stats_create(struct mapped_device *md,
 				unsigned argc, char **argv,
 				char *result, unsigned maxlen)
 {
+	int r;
 	int id;
 	char dummy;
 	unsigned long long start, end, len, step;
 	unsigned divisor;
 	const char *program_id, *aux_data;
+	unsigned stat_flags = 0;
+
+	unsigned n_histogram_entries = 0;
+	unsigned long long *histogram_boundaries = NULL;
+
+	struct dm_arg_set as, as_backup;
+	const char *a;
+	unsigned feature_args;
 
 	/*
 	 * Input format:
-	 *   <range> <step> [<program_id> [<aux_data>]]
+	 *   <range> <step> [<extra_parameters> <parameters>] [<program_id> [<aux_data>]]
 	 */
 
-	if (argc < 3 || argc > 5)
-		return -EINVAL;
+	if (argc < 3)
+		goto ret_einval;
 
-	if (!strcmp(argv[1], "-")) {
+	as.argc = argc;
+	as.argv = argv;
+	dm_consume_args(&as, 1);
+
+	a = dm_shift_arg(&as);
+	if (!strcmp(a, "-")) {
 		start = 0;
 		len = dm_get_size(md);
 		if (!len)
 			len = 1;
-	} else if (sscanf(argv[1], "%llu+%llu%c", &start, &len, &dummy) != 2 ||
+	} else if (sscanf(a, "%llu+%llu%c", &start, &len, &dummy) != 2 ||
 		   start != (sector_t)start || len != (sector_t)len)
-		return -EINVAL;
+		goto ret_einval;
 
 	end = start + len;
 	if (start >= end)
-		return -EINVAL;
+		goto ret_einval;
 
-	if (sscanf(argv[2], "/%u%c", &divisor, &dummy) == 1) {
+	a = dm_shift_arg(&as);
+	if (sscanf(a, "/%u%c", &divisor, &dummy) == 1) {
+		if (!divisor)
+			return -EINVAL;
 		step = end - start;
 		if (do_div(step, divisor))
 			step++;
 		if (!step)
 			step = 1;
-	} else if (sscanf(argv[2], "%llu%c", &step, &dummy) != 1 ||
+	} else if (sscanf(a, "%llu%c", &step, &dummy) != 1 ||
 		   step != (sector_t)step || !step)
-		return -EINVAL;
+		goto ret_einval;
+
+	as_backup = as;
+	a = dm_shift_arg(&as);
+	if (a && sscanf(a, "%u%c", &feature_args, &dummy) == 1) {
+		while (feature_args--) {
+			a = dm_shift_arg(&as);
+			if (!a)
+				goto ret_einval;
+			if (!strcasecmp(a, "precise_timestamps"))
+				stat_flags |= STAT_PRECISE_TIMESTAMPS;
+			else if (!strncasecmp(a, "histogram:", 10)) {
+				if (n_histogram_entries)
+					goto ret_einval;
+				if ((r = parse_histogram(a + 10, &n_histogram_entries, &histogram_boundaries)))
+					goto ret;
+			} else
+				goto ret_einval;
+		}
+	} else {
+		as = as_backup;
+	}
 
 	program_id = "-";
 	aux_data = "-";
 
-	if (argc > 3)
-		program_id = argv[3];
+	a = dm_shift_arg(&as);
+	if (a)
+		program_id = a;
 
-	if (argc > 4)
-		aux_data = argv[4];
+	a = dm_shift_arg(&as);
+	if (a)
+		aux_data = a;
+
+	if (as.argc)
+		goto ret_einval;
 
 	/*
 	 * If a buffer overflow happens after we created the region,
@@ -820,17 +1030,29 @@ static int message_stats_create(struct mapped_device *md,
 	 * leaked).  So we must detect buffer overflow in advance.
 	 */
 	snprintf(result, maxlen, "%d", INT_MAX);
-	if (dm_message_test_buffer_overflow(result, maxlen))
-		return 1;
+	if (dm_message_test_buffer_overflow(result, maxlen)) {
+		r = 1;
+		goto ret;
+	}
 
-	id = dm_stats_create(dm_get_stats(md), start, end, step, program_id, aux_data,
+	id = dm_stats_create(dm_get_stats(md), start, end, step, stat_flags,
+			     n_histogram_entries, histogram_boundaries, program_id, aux_data,
 			     dm_internal_suspend_fast, dm_internal_resume_fast, md);
-	if (id < 0)
-		return id;
+	if (id < 0) {
+		r = id;
+		goto ret;
+	}
 
 	snprintf(result, maxlen, "%d", id);
 
-	return 1;
+	r = 1;
+	goto ret;
+
+ret_einval:
+	r = -EINVAL;
+ret:
+	kfree(histogram_boundaries);
+	return r;
 }
 
 static int message_stats_delete(struct mapped_device *md,
@@ -933,11 +1155,6 @@ int dm_stats_message(struct mapped_device *md, unsigned argc, char **argv,
 {
 	int r;
 
-	if (dm_request_based(md)) {
-		DMWARN("Statistics are only supported for bio-based devices");
-		return -EOPNOTSUPP;
-	}
-
 	/* All messages here must start with '@' */
 	if (!strcasecmp(argv[0], "@stats_create"))
 		r = message_stats_create(md, argc, argv, result, maxlen);
diff --git a/drivers/md/dm-stats.h b/drivers/md/dm-stats.h
index e7c4984bf235..f1c0956e3843 100644
--- a/drivers/md/dm-stats.h
+++ b/drivers/md/dm-stats.h
@@ -18,6 +18,7 @@ struct dm_stats {
 
 struct dm_stats_aux {
 	bool merged;
+	unsigned long long duration_ns;
 };
 
 void dm_stats_init(struct dm_stats *st);
@@ -30,7 +31,8 @@ int dm_stats_message(struct mapped_device *md, unsigned argc, char **argv,
 
 void dm_stats_account_io(struct dm_stats *stats, unsigned long bi_rw,
 			 sector_t bi_sector, unsigned bi_sectors, bool end,
-			 unsigned long duration, struct dm_stats_aux *aux);
+			 unsigned long duration_jiffies,
+			 struct dm_stats_aux *aux);
 
 static inline bool dm_stats_used(struct dm_stats *st)
 {
diff --git a/drivers/md/dm-stripe.c b/drivers/md/dm-stripe.c
index f8b37d4c05d8..a672a1502c14 100644
--- a/drivers/md/dm-stripe.c
+++ b/drivers/md/dm-stripe.c
@@ -451,10 +451,8 @@ int __init dm_stripe_init(void)
 	int r;
 
 	r = dm_register_target(&stripe_target);
-	if (r < 0) {
+	if (r < 0)
 		DMWARN("target registration failed");
-		return r;
-	}
 
 	return r;
 }
diff --git a/drivers/md/dm-thin-metadata.c b/drivers/md/dm-thin-metadata.c
index 79f694120ddf..48dfe3c4d6aa 100644
--- a/drivers/md/dm-thin-metadata.c
+++ b/drivers/md/dm-thin-metadata.c
@@ -184,7 +184,6 @@ struct dm_pool_metadata {
 	uint64_t trans_id;
 	unsigned long flags;
 	sector_t data_block_size;
-	bool read_only:1;
 
 	/*
 	 * Set if a transaction has to be aborted but the attempt to roll back
@@ -836,7 +835,6 @@ struct dm_pool_metadata *dm_pool_metadata_open(struct block_device *bdev,
 	init_rwsem(&pmd->root_lock);
 	pmd->time = 0;
 	INIT_LIST_HEAD(&pmd->thin_devices);
-	pmd->read_only = false;
 	pmd->fail_io = false;
 	pmd->bdev = bdev;
 	pmd->data_block_size = data_block_size;
@@ -880,7 +878,7 @@ int dm_pool_metadata_close(struct dm_pool_metadata *pmd)
 		return -EBUSY;
 	}
 
-	if (!pmd->read_only && !pmd->fail_io) {
+	if (!dm_bm_is_read_only(pmd->bm) && !pmd->fail_io) {
 		r = __commit_transaction(pmd);
 		if (r < 0)
 			DMWARN("%s: __commit_transaction() failed, error = %d",
@@ -1392,10 +1390,11 @@ int dm_thin_find_block(struct dm_thin_device *td, dm_block_t block,
 	dm_block_t keys[2] = { td->id, block };
 	struct dm_btree_info *info;
 
-	if (pmd->fail_io)
-		return -EINVAL;
-
 	down_read(&pmd->root_lock);
+	if (pmd->fail_io) {
+		up_read(&pmd->root_lock);
+		return -EINVAL;
+	}
 
 	if (can_issue_io) {
 		info = &pmd->info;
@@ -1419,6 +1418,63 @@ int dm_thin_find_block(struct dm_thin_device *td, dm_block_t block,
 	return r;
 }
 
+/* FIXME: write a more efficient one in btree */
+int dm_thin_find_mapped_range(struct dm_thin_device *td,
+			      dm_block_t begin, dm_block_t end,
+			      dm_block_t *thin_begin, dm_block_t *thin_end,
+			      dm_block_t *pool_begin, bool *maybe_shared)
+{
+	int r;
+	dm_block_t pool_end;
+	struct dm_thin_lookup_result lookup;
+
+	if (end < begin)
+		return -ENODATA;
+
+	/*
+	 * Find first mapped block.
+	 */
+	while (begin < end) {
+		r = dm_thin_find_block(td, begin, true, &lookup);
+		if (r) {
+			if (r != -ENODATA)
+				return r;
+		} else
+			break;
+
+		begin++;
+	}
+
+	if (begin == end)
+		return -ENODATA;
+
+	*thin_begin = begin;
+	*pool_begin = lookup.block;
+	*maybe_shared = lookup.shared;
+
+	begin++;
+	pool_end = *pool_begin + 1;
+	while (begin != end) {
+		r = dm_thin_find_block(td, begin, true, &lookup);
+		if (r) {
+			if (r == -ENODATA)
+				break;
+			else
+				return r;
+		}
+
+		if ((lookup.block != pool_end) ||
+		    (lookup.shared != *maybe_shared))
+			break;
+
+		pool_end++;
+		begin++;
+	}
+
+	*thin_end = begin;
+	return 0;
+}
+
 static int __insert(struct dm_thin_device *td, dm_block_t block,
 		    dm_block_t data_block)
 {
@@ -1471,6 +1527,47 @@ static int __remove(struct dm_thin_device *td, dm_block_t block)
 	return 0;
 }
 
+static int __remove_range(struct dm_thin_device *td, dm_block_t begin, dm_block_t end)
+{
+	int r;
+	unsigned count;
+	struct dm_pool_metadata *pmd = td->pmd;
+	dm_block_t keys[1] = { td->id };
+	__le64 value;
+	dm_block_t mapping_root;
+
+	/*
+	 * Find the mapping tree
+	 */
+	r = dm_btree_lookup(&pmd->tl_info, pmd->root, keys, &value);
+	if (r)
+		return r;
+
+	/*
+	 * Remove from the mapping tree, taking care to inc the
+	 * ref count so it doesn't get deleted.
+	 */
+	mapping_root = le64_to_cpu(value);
+	dm_tm_inc(pmd->tm, mapping_root);
+	r = dm_btree_remove(&pmd->tl_info, pmd->root, keys, &pmd->root);
+	if (r)
+		return r;
+
+	r = dm_btree_remove_leaves(&pmd->bl_info, mapping_root, &begin, end, &mapping_root, &count);
+	if (r)
+		return r;
+
+	td->mapped_blocks -= count;
+	td->changed = 1;
+
+	/*
+	 * Reinsert the mapping tree.
+	 */
+	value = cpu_to_le64(mapping_root);
+	__dm_bless_for_disk(&value);
+	return dm_btree_insert(&pmd->tl_info, pmd->root, keys, &value, &pmd->root);
+}
+
 int dm_thin_remove_block(struct dm_thin_device *td, dm_block_t block)
 {
 	int r = -EINVAL;
@@ -1483,6 +1580,19 @@ int dm_thin_remove_block(struct dm_thin_device *td, dm_block_t block)
 	return r;
 }
 
+int dm_thin_remove_range(struct dm_thin_device *td,
+			 dm_block_t begin, dm_block_t end)
+{
+	int r = -EINVAL;
+
+	down_write(&td->pmd->root_lock);
+	if (!td->pmd->fail_io)
+		r = __remove_range(td, begin, end);
+	up_write(&td->pmd->root_lock);
+
+	return r;
+}
+
 int dm_pool_block_is_used(struct dm_pool_metadata *pmd, dm_block_t b, bool *result)
 {
 	int r;
@@ -1739,7 +1849,6 @@ int dm_pool_resize_metadata_dev(struct dm_pool_metadata *pmd, dm_block_t new_cou
 void dm_pool_metadata_read_only(struct dm_pool_metadata *pmd)
 {
 	down_write(&pmd->root_lock);
-	pmd->read_only = true;
 	dm_bm_set_read_only(pmd->bm);
 	up_write(&pmd->root_lock);
 }
@@ -1747,7 +1856,6 @@ void dm_pool_metadata_read_only(struct dm_pool_metadata *pmd)
 void dm_pool_metadata_read_write(struct dm_pool_metadata *pmd)
 {
 	down_write(&pmd->root_lock);
-	pmd->read_only = false;
 	dm_bm_set_read_write(pmd->bm);
 	up_write(&pmd->root_lock);
 }
diff --git a/drivers/md/dm-thin-metadata.h b/drivers/md/dm-thin-metadata.h
index fac01a96d303..a938babe4258 100644
--- a/drivers/md/dm-thin-metadata.h
+++ b/drivers/md/dm-thin-metadata.h
@@ -147,6 +147,15 @@ int dm_thin_find_block(struct dm_thin_device *td, dm_block_t block,
 		       int can_issue_io, struct dm_thin_lookup_result *result);
 
 /*
+ * Retrieve the next run of contiguously mapped blocks.  Useful for working
+ * out where to break up IO.  Returns 0 on success, < 0 on error.
+ */
+int dm_thin_find_mapped_range(struct dm_thin_device *td,
+			      dm_block_t begin, dm_block_t end,
+			      dm_block_t *thin_begin, dm_block_t *thin_end,
+			      dm_block_t *pool_begin, bool *maybe_shared);
+
+/*
  * Obtain an unused block.
  */
 int dm_pool_alloc_data_block(struct dm_pool_metadata *pmd, dm_block_t *result);
@@ -158,6 +167,8 @@ int dm_thin_insert_block(struct dm_thin_device *td, dm_block_t block,
 			 dm_block_t data_block);
 
 int dm_thin_remove_block(struct dm_thin_device *td, dm_block_t block);
+int dm_thin_remove_range(struct dm_thin_device *td,
+			 dm_block_t begin, dm_block_t end);
 
 /*
  * Queries.
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 921aafd12aee..c33f61a4cc28 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -111,22 +111,30 @@ DECLARE_DM_KCOPYD_THROTTLE_WITH_MODULE_PARM(snapshot_copy_throttle,
 /*
  * Key building.
  */
-static void build_data_key(struct dm_thin_device *td,
-			   dm_block_t b, struct dm_cell_key *key)
+enum lock_space {
+	VIRTUAL,
+	PHYSICAL
+};
+
+static void build_key(struct dm_thin_device *td, enum lock_space ls,
+		      dm_block_t b, dm_block_t e, struct dm_cell_key *key)
 {
-	key->virtual = 0;
+	key->virtual = (ls == VIRTUAL);
 	key->dev = dm_thin_dev_id(td);
 	key->block_begin = b;
-	key->block_end = b + 1ULL;
+	key->block_end = e;
+}
+
+static void build_data_key(struct dm_thin_device *td, dm_block_t b,
+			   struct dm_cell_key *key)
+{
+	build_key(td, PHYSICAL, b, b + 1llu, key);
 }
 
 static void build_virtual_key(struct dm_thin_device *td, dm_block_t b,
 			      struct dm_cell_key *key)
 {
-	key->virtual = 1;
-	key->dev = dm_thin_dev_id(td);
-	key->block_begin = b;
-	key->block_end = b + 1ULL;
+	build_key(td, VIRTUAL, b, b + 1llu, key);
 }
 
 /*----------------------------------------------------------------*/
@@ -312,6 +320,138 @@ struct thin_c {
 
 /*----------------------------------------------------------------*/
 
+/**
+ * __blkdev_issue_discard_async - queue a discard with async completion
+ * @bdev:	blockdev to issue discard for
+ * @sector:	start sector
+ * @nr_sects:	number of sectors to discard
+ * @gfp_mask:	memory allocation flags (for bio_alloc)
+ * @flags:	BLKDEV_IFL_* flags to control behaviour
+ * @parent_bio: parent discard bio that all sub discards get chained to
+ *
+ * Description:
+ *    Asynchronously issue a discard request for the sectors in question.
+ *    NOTE: this variant of blk-core's blkdev_issue_discard() is a stop-gap
+ *    that is being kept local to DM thinp until the block changes to allow
+ *    late bio splitting land upstream.
+ */
+static int __blkdev_issue_discard_async(struct block_device *bdev, sector_t sector,
+					sector_t nr_sects, gfp_t gfp_mask, unsigned long flags,
+					struct bio *parent_bio)
+{
+	struct request_queue *q = bdev_get_queue(bdev);
+	int type = REQ_WRITE | REQ_DISCARD;
+	unsigned int max_discard_sectors, granularity;
+	int alignment;
+	struct bio *bio;
+	int ret = 0;
+	struct blk_plug plug;
+
+	if (!q)
+		return -ENXIO;
+
+	if (!blk_queue_discard(q))
+		return -EOPNOTSUPP;
+
+	/* Zero-sector (unknown) and one-sector granularities are the same.  */
+	granularity = max(q->limits.discard_granularity >> 9, 1U);
+	alignment = (bdev_discard_alignment(bdev) >> 9) % granularity;
+
+	/*
+	 * Ensure that max_discard_sectors is of the proper
+	 * granularity, so that requests stay aligned after a split.
+	 */
+	max_discard_sectors = min(q->limits.max_discard_sectors, UINT_MAX >> 9);
+	max_discard_sectors -= max_discard_sectors % granularity;
+	if (unlikely(!max_discard_sectors)) {
+		/* Avoid infinite loop below. Being cautious never hurts. */
+		return -EOPNOTSUPP;
+	}
+
+	if (flags & BLKDEV_DISCARD_SECURE) {
+		if (!blk_queue_secdiscard(q))
+			return -EOPNOTSUPP;
+		type |= REQ_SECURE;
+	}
+
+	blk_start_plug(&plug);
+	while (nr_sects) {
+		unsigned int req_sects;
+		sector_t end_sect, tmp;
+
+		/*
+		 * Required bio_put occurs in bio_endio thanks to bio_chain below
+		 */
+		bio = bio_alloc(gfp_mask, 1);
+		if (!bio) {
+			ret = -ENOMEM;
+			break;
+		}
+
+		req_sects = min_t(sector_t, nr_sects, max_discard_sectors);
+
+		/*
+		 * If splitting a request, and the next starting sector would be
+		 * misaligned, stop the discard at the previous aligned sector.
+		 */
+		end_sect = sector + req_sects;
+		tmp = end_sect;
+		if (req_sects < nr_sects &&
+		    sector_div(tmp, granularity) != alignment) {
+			end_sect = end_sect - alignment;
+			sector_div(end_sect, granularity);
+			end_sect = end_sect * granularity + alignment;
+			req_sects = end_sect - sector;
+		}
+
+		bio_chain(bio, parent_bio);
+
+		bio->bi_iter.bi_sector = sector;
+		bio->bi_bdev = bdev;
+
+		bio->bi_iter.bi_size = req_sects << 9;
+		nr_sects -= req_sects;
+		sector = end_sect;
+
+		submit_bio(type, bio);
+
+		/*
+		 * We can loop for a long time in here, if someone does
+		 * full device discards (like mkfs). Be nice and allow
+		 * us to schedule out to avoid softlocking if preempt
+		 * is disabled.
+		 */
+		cond_resched();
+	}
+	blk_finish_plug(&plug);
+
+	return ret;
+}
+
+static bool block_size_is_power_of_two(struct pool *pool)
+{
+	return pool->sectors_per_block_shift >= 0;
+}
+
+static sector_t block_to_sectors(struct pool *pool, dm_block_t b)
+{
+	return block_size_is_power_of_two(pool) ?
+		(b << pool->sectors_per_block_shift) :
+		(b * pool->sectors_per_block);
+}
+
+static int issue_discard(struct thin_c *tc, dm_block_t data_b, dm_block_t data_e,
+			 struct bio *parent_bio)
+{
+	sector_t s = block_to_sectors(tc->pool, data_b);
+	sector_t len = block_to_sectors(tc->pool, data_e - data_b);
+
+	return __blkdev_issue_discard_async(tc->pool_dev->bdev, s, len,
+					    GFP_NOWAIT, 0, parent_bio);
+}
+
+/*----------------------------------------------------------------*/
+
 /*
  * wake_worker() is used when new work is queued and when pool_resume is
  * ready to continue deferred IO processing.
@@ -461,6 +601,7 @@ struct dm_thin_endio_hook {
 	struct dm_deferred_entry *all_io_entry;
 	struct dm_thin_new_mapping *overwrite_mapping;
 	struct rb_node rb_node;
+	struct dm_bio_prison_cell *cell;
 };
 
 static void __merge_bio_list(struct bio_list *bios, struct bio_list *master)
@@ -541,11 +682,6 @@ static void error_retry_list(struct pool *pool)
  * target.
  */
 
-static bool block_size_is_power_of_two(struct pool *pool)
-{
-	return pool->sectors_per_block_shift >= 0;
-}
-
 static dm_block_t get_bio_block(struct thin_c *tc, struct bio *bio)
 {
 	struct pool *pool = tc->pool;
@@ -559,6 +695,34 @@ static dm_block_t get_bio_block(struct thin_c *tc, struct bio *bio)
 	return block_nr;
 }
 
+/*
+ * Returns the _complete_ blocks that this bio covers.
+ */
+static void get_bio_block_range(struct thin_c *tc, struct bio *bio,
+				dm_block_t *begin, dm_block_t *end)
+{
+	struct pool *pool = tc->pool;
+	sector_t b = bio->bi_iter.bi_sector;
+	sector_t e = b + (bio->bi_iter.bi_size >> SECTOR_SHIFT);
+
+	b += pool->sectors_per_block - 1ull; /* so we round up */
+
+	if (block_size_is_power_of_two(pool)) {
+		b >>= pool->sectors_per_block_shift;
+		e >>= pool->sectors_per_block_shift;
+	} else {
+		(void) sector_div(b, pool->sectors_per_block);
+		(void) sector_div(e, pool->sectors_per_block);
+	}
+
+	if (e < b)
+		/* Can happen if the bio is within a single block. */
+		e = b;
+
+	*begin = b;
+	*end = e;
+}
+
 static void remap(struct thin_c *tc, struct bio *bio, dm_block_t block)
 {
 	struct pool *pool = tc->pool;
@@ -647,7 +811,7 @@ struct dm_thin_new_mapping {
 	struct list_head list;
 
 	bool pass_discard:1;
-	bool definitely_not_shared:1;
+	bool maybe_shared:1;
 
 	/*
 	 * Track quiescing, copying and zeroing preparation actions.  When this
@@ -658,9 +822,9 @@ struct dm_thin_new_mapping {
 
 	int err;
 	struct thin_c *tc;
-	dm_block_t virt_block;
+	dm_block_t virt_begin, virt_end;
 	dm_block_t data_block;
-	struct dm_bio_prison_cell *cell, *cell2;
+	struct dm_bio_prison_cell *cell;
 
 	/*
 	 * If the bio covers the whole area of a block then we can avoid
@@ -705,6 +869,8 @@ static void overwrite_endio(struct bio *bio, int err)
 	struct dm_thin_endio_hook *h = dm_per_bio_data(bio, sizeof(struct dm_thin_endio_hook));
 	struct dm_thin_new_mapping *m = h->overwrite_mapping;
 
+	bio->bi_end_io = m->saved_bi_end_io;
+
 	m->err = err;
 	complete_mapping_preparation(m);
 }
@@ -793,10 +959,6 @@ static void inc_remap_and_issue_cell(struct thin_c *tc,
 
 static void process_prepared_mapping_fail(struct dm_thin_new_mapping *m)
 {
-	if (m->bio) {
-		m->bio->bi_end_io = m->saved_bi_end_io;
-		atomic_inc(&m->bio->bi_remaining);
-	}
 	cell_error(m->tc->pool, m->cell);
 	list_del(&m->list);
 	mempool_free(m, m->tc->pool->mapping_pool);
@@ -806,15 +968,9 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 {
 	struct thin_c *tc = m->tc;
 	struct pool *pool = tc->pool;
-	struct bio *bio;
+	struct bio *bio = m->bio;
 	int r;
 
-	bio = m->bio;
-	if (bio) {
-		bio->bi_end_io = m->saved_bi_end_io;
-		atomic_inc(&bio->bi_remaining);
-	}
-
 	if (m->err) {
 		cell_error(pool, m->cell);
 		goto out;
@@ -825,7 +981,7 @@ static void process_prepared_mapping(struct dm_thin_new_mapping *m)
 	 * Any I/O for this block arriving after this point will get
 	 * remapped to it directly.
 	 */
-	r = dm_thin_insert_block(tc->td, m->virt_block, m->data_block);
+	r = dm_thin_insert_block(tc->td, m->virt_begin, m->data_block);
 	if (r) {
 		metadata_operation_failed(pool, "dm_thin_insert_block", r);
 		cell_error(pool, m->cell);
@@ -852,50 +1008,112 @@ out:
 	mempool_free(m, pool->mapping_pool);
 }
 
-static void process_prepared_discard_fail(struct dm_thin_new_mapping *m)
+/*----------------------------------------------------------------*/
+
+static void free_discard_mapping(struct dm_thin_new_mapping *m)
 {
 	struct thin_c *tc = m->tc;
+	if (m->cell)
+		cell_defer_no_holder(tc, m->cell);
+	mempool_free(m, tc->pool->mapping_pool);
+}
 
+static void process_prepared_discard_fail(struct dm_thin_new_mapping *m)
+{
 	bio_io_error(m->bio);
+	free_discard_mapping(m);
+}
+
+static void process_prepared_discard_success(struct dm_thin_new_mapping *m)
+{
+	bio_endio(m->bio, 0);
+	free_discard_mapping(m);
+}
+
+static void process_prepared_discard_no_passdown(struct dm_thin_new_mapping *m)
+{
+	int r;
+	struct thin_c *tc = m->tc;
+
+	r = dm_thin_remove_range(tc->td, m->cell->key.block_begin, m->cell->key.block_end);
+	if (r) {
+		metadata_operation_failed(tc->pool, "dm_thin_remove_range", r);
+		bio_io_error(m->bio);
+	} else
+		bio_endio(m->bio, 0);
+
 	cell_defer_no_holder(tc, m->cell);
-	cell_defer_no_holder(tc, m->cell2);
 	mempool_free(m, tc->pool->mapping_pool);
 }
 
-static void process_prepared_discard_passdown(struct dm_thin_new_mapping *m)
+static int passdown_double_checking_shared_status(struct dm_thin_new_mapping *m)
 {
+	/*
+	 * We've already unmapped this range of blocks, but before we
+	 * passdown we have to check that these blocks are now unused.
+	 */
+	int r;
+	bool used = true;
 	struct thin_c *tc = m->tc;
+	struct pool *pool = tc->pool;
+	dm_block_t b = m->data_block, e, end = m->data_block + m->virt_end - m->virt_begin;
 
-	inc_all_io_entry(tc->pool, m->bio);
-	cell_defer_no_holder(tc, m->cell);
-	cell_defer_no_holder(tc, m->cell2);
+	while (b != end) {
+		/* find start of unmapped run */
+		for (; b < end; b++) {
+			r = dm_pool_block_is_used(pool->pmd, b, &used);
+			if (r)
+				return r;
 
-	if (m->pass_discard)
-		if (m->definitely_not_shared)
-			remap_and_issue(tc, m->bio, m->data_block);
-		else {
-			bool used = false;
-			if (dm_pool_block_is_used(tc->pool->pmd, m->data_block, &used) || used)
-				bio_endio(m->bio, 0);
-			else
-				remap_and_issue(tc, m->bio, m->data_block);
+			if (!used)
+				break;
 		}
-	else
-		bio_endio(m->bio, 0);
 
-	mempool_free(m, tc->pool->mapping_pool);
+		if (b == end)
+			break;
+
+		/* find end of run */
+		for (e = b + 1; e != end; e++) {
+			r = dm_pool_block_is_used(pool->pmd, e, &used);
+			if (r)
+				return r;
+
+			if (used)
+				break;
+		}
+
+		r = issue_discard(tc, b, e, m->bio);
+		if (r)
+			return r;
+
+		b = e;
+	}
+
+	return 0;
 }
 
-static void process_prepared_discard(struct dm_thin_new_mapping *m)
+static void process_prepared_discard_passdown(struct dm_thin_new_mapping *m)
 {
 	int r;
 	struct thin_c *tc = m->tc;
+	struct pool *pool = tc->pool;
 
-	r = dm_thin_remove_block(tc->td, m->virt_block);
+	r = dm_thin_remove_range(tc->td, m->virt_begin, m->virt_end);
 	if (r)
-		DMERR_LIMIT("dm_thin_remove_block() failed");
+		metadata_operation_failed(pool, "dm_thin_remove_range", r);
+
+	else if (m->maybe_shared)
+		r = passdown_double_checking_shared_status(m);
+	else
+		r = issue_discard(tc, m->data_block, m->data_block + (m->virt_end - m->virt_begin), m->bio);
 
-	process_prepared_discard_passdown(m);
+	/*
+	 * Even if r is set, there could be sub discards in flight that we
+	 * need to wait for.
+	 */
+	bio_endio(m->bio, r);
+	cell_defer_no_holder(tc, m->cell);
+	mempool_free(m, pool->mapping_pool);
 }
 
 static void process_prepared(struct pool *pool, struct list_head *head,
@@ -979,7 +1197,7 @@ static void ll_zero(struct thin_c *tc, struct dm_thin_new_mapping *m,
 }
 
 static void remap_and_issue_overwrite(struct thin_c *tc, struct bio *bio,
-				      dm_block_t data_block,
+				      dm_block_t data_begin,
 				      struct dm_thin_new_mapping *m)
 {
 	struct pool *pool = tc->pool;
@@ -989,7 +1207,7 @@ static void remap_and_issue_overwrite(struct thin_c *tc, struct bio *bio,
 	m->bio = bio;
 	save_and_set_endio(bio, &m->saved_bi_end_io, overwrite_endio);
 	inc_all_io_entry(pool, bio);
-	remap_and_issue(tc, bio, data_block);
+	remap_and_issue(tc, bio, data_begin);
 }
 
 /*
@@ -1006,7 +1224,8 @@ static void schedule_copy(struct thin_c *tc, dm_block_t virt_block,
 	struct dm_thin_new_mapping *m = get_next_mapping(pool);
 
 	m->tc = tc;
-	m->virt_block = virt_block;
+	m->virt_begin = virt_block;
+	m->virt_end = virt_block + 1u;
 	m->data_block = data_dest;
 	m->cell = cell;
 
@@ -1085,7 +1304,8 @@ static void schedule_zero(struct thin_c *tc, dm_block_t virt_block,
 
 	atomic_set(&m->prepare_actions, 1); /* no need to quiesce */
 	m->tc = tc;
-	m->virt_block = virt_block;
+	m->virt_begin = virt_block;
+	m->virt_end = virt_block + 1u;
 	m->data_block = data_block;
 	m->cell = cell;
 
@@ -1094,16 +1314,14 @@ static void schedule_zero(struct thin_c *tc, dm_block_t virt_block,
 	 * zeroing pre-existing data, we can issue the bio immediately.
 	 * Otherwise we use kcopyd to zero the data first.
 	 */
-	if (!pool->pf.zero_new_blocks)
+	if (pool->pf.zero_new_blocks) {
+		if (io_overwrites_block(pool, bio))
+			remap_and_issue_overwrite(tc, bio, data_block, m);
+		else
+			ll_zero(tc, m, data_block * pool->sectors_per_block,
+				(data_block + 1) * pool->sectors_per_block);
+	} else
 		process_prepared_mapping(m);
-
-	else if (io_overwrites_block(pool, bio))
-		remap_and_issue_overwrite(tc, bio, data_block, m);
-
-	else
-		ll_zero(tc, m,
-			data_block * pool->sectors_per_block,
-			(data_block + 1) * pool->sectors_per_block);
 }
 
 static void schedule_external_copy(struct thin_c *tc, dm_block_t virt_block,
@@ -1294,99 +1512,149 @@ static void retry_bios_on_resume(struct pool *pool, struct dm_bio_prison_cell *c
 		retry_on_resume(bio);
 }
 
-static void process_discard_cell(struct thin_c *tc, struct dm_bio_prison_cell *cell)
+static void process_discard_cell_no_passdown(struct thin_c *tc,
+					     struct dm_bio_prison_cell *virt_cell)
 {
-	int r;
-	struct bio *bio = cell->holder;
 	struct pool *pool = tc->pool;
-	struct dm_bio_prison_cell *cell2;
-	struct dm_cell_key key2;
-	dm_block_t block = get_bio_block(tc, bio);
-	struct dm_thin_lookup_result lookup_result;
-	struct dm_thin_new_mapping *m;
+	struct dm_thin_new_mapping *m = get_next_mapping(pool);
 
-	if (tc->requeue_mode) {
-		cell_requeue(pool, cell);
-		return;
-	}
+	/*
+	 * We don't need to lock the data blocks, since there's no
+	 * passdown.  We only lock data blocks for allocation and breaking sharing.
+	 */
+	m->tc = tc;
+	m->virt_begin = virt_cell->key.block_begin;
+	m->virt_end = virt_cell->key.block_end;
+	m->cell = virt_cell;
+	m->bio = virt_cell->holder;
 
-	r = dm_thin_find_block(tc->td, block, 1, &lookup_result);
-	switch (r) {
-	case 0:
-		/*
-		 * Check nobody is fiddling with this pool block.  This can
-		 * happen if someone's in the process of breaking sharing
-		 * on this block.
-		 */
-		build_data_key(tc->td, lookup_result.block, &key2);
-		if (bio_detain(tc->pool, &key2, bio, &cell2)) {
-			cell_defer_no_holder(tc, cell);
-			break;
-		}
+	if (!dm_deferred_set_add_work(pool->all_io_ds, &m->list))
+		pool->process_prepared_discard(m);
+}
 
-		if (io_overlaps_block(pool, bio)) {
-			/*
-			 * IO may still be going to the destination block.  We must
-			 * quiesce before we can do the removal.
-			 */
-			m = get_next_mapping(pool);
-			m->tc = tc;
-			m->pass_discard = pool->pf.discard_passdown;
-			m->definitely_not_shared = !lookup_result.shared;
-			m->virt_block = block;
-			m->data_block = lookup_result.block;
-			m->cell = cell;
-			m->cell2 = cell2;
-			m->bio = bio;
-
-			if (!dm_deferred_set_add_work(pool->all_io_ds, &m->list))
-				pool->process_prepared_discard(m);
+/*
+ * FIXME: DM local hack to defer parent bios's end_io until we
+ * _know_ all chained sub range discard bios have completed.
+ * Will go away once late bio splitting lands upstream!
+ */
+static inline void __bio_inc_remaining(struct bio *bio)
+{
+	bio->bi_flags |= (1 << BIO_CHAIN);
+	smp_mb__before_atomic();
+	atomic_inc(&bio->__bi_remaining);
+}
 
-		} else {
-			inc_all_io_entry(pool, bio);
-			cell_defer_no_holder(tc, cell);
-			cell_defer_no_holder(tc, cell2);
+static void break_up_discard_bio(struct thin_c *tc, dm_block_t begin, dm_block_t end,
+				 struct bio *bio)
+{
+	struct pool *pool = tc->pool;
+
+	int r;
+	bool maybe_shared;
+	struct dm_cell_key data_key;
+	struct dm_bio_prison_cell *data_cell;
+	struct dm_thin_new_mapping *m;
+	dm_block_t virt_begin, virt_end, data_begin;
+
+	while (begin != end) {
+		r = ensure_next_mapping(pool);
+		if (r)
+			/* we did our best */
+			return;
 
+		r = dm_thin_find_mapped_range(tc->td, begin, end, &virt_begin, &virt_end,
+					      &data_begin, &maybe_shared);
+		if (r)
 			/*
-			 * The DM core makes sure that the discard doesn't span
-			 * a block boundary.  So we submit the discard of a
-			 * partial block appropriately.
+			 * Silently fail, letting any mappings we've
+			 * created complete.
 			 */
-			if ((!lookup_result.shared) && pool->pf.discard_passdown)
-				remap_and_issue(tc, bio, lookup_result.block);
-			else
-				bio_endio(bio, 0);
+			break;
+
+		build_key(tc->td, PHYSICAL, data_begin, data_begin + (virt_end - virt_begin), &data_key);
+		if (bio_detain(tc->pool, &data_key, NULL, &data_cell)) {
+			/* contention, we'll give up with this range */
+			begin = virt_end;
+			continue;
 		}
-		break;
 
-	case -ENODATA:
 		/*
-		 * It isn't provisioned, just forget it.
+		 * IO may still be going to the destination block.  We must
+		 * quiesce before we can do the removal.
 		 */
-		cell_defer_no_holder(tc, cell);
-		bio_endio(bio, 0);
-		break;
+		m = get_next_mapping(pool);
+		m->tc = tc;
+		m->maybe_shared = maybe_shared;
+		m->virt_begin = virt_begin;
+		m->virt_end = virt_end;
+		m->data_block = data_begin;
+		m->cell = data_cell;
+		m->bio = bio;
 
-	default:
-		DMERR_LIMIT("%s: dm_thin_find_block() failed: error = %d",
-			    __func__, r);
-		cell_defer_no_holder(tc, cell);
-		bio_io_error(bio);
-		break;
+		/*
+		 * The parent bio must not complete before sub discard bios are
+		 * chained to it (see __blkdev_issue_discard_async's bio_chain)!
+		 *
+		 * This per-mapping bi_remaining increment is paired with
+		 * the implicit decrement that occurs via bio_endio() in
+		 * process_prepared_discard_{passdown,no_passdown}.
+		 */
+		__bio_inc_remaining(bio);
+		if (!dm_deferred_set_add_work(pool->all_io_ds, &m->list))
+			pool->process_prepared_discard(m);
+
+		begin = virt_end;
 	}
 }
 
+static void process_discard_cell_passdown(struct thin_c *tc, struct dm_bio_prison_cell *virt_cell)
+{
+	struct bio *bio = virt_cell->holder;
+	struct dm_thin_endio_hook *h = dm_per_bio_data(bio, sizeof(struct dm_thin_endio_hook));
+
+	/*
+	 * The virt_cell will only get freed once the origin bio completes.
+	 * This means it will remain locked while all the individual
+	 * passdown bios are in flight.
+	 */
+	h->cell = virt_cell;
+	break_up_discard_bio(tc, virt_cell->key.block_begin, virt_cell->key.block_end, bio);
+
+	/*
+	 * We complete the bio now, knowing that the bi_remaining field
+	 * will prevent completion until the sub range discards have
+	 * completed.
+	 */
+	bio_endio(bio, 0);
+}
+
 static void process_discard_bio(struct thin_c *tc, struct bio *bio)
 {
-	struct dm_bio_prison_cell *cell;
-	struct dm_cell_key key;
-	dm_block_t block = get_bio_block(tc, bio);
+	dm_block_t begin, end;
+	struct dm_cell_key virt_key;
+	struct dm_bio_prison_cell *virt_cell;
 
-	build_virtual_key(tc->td, block, &key);
-	if (bio_detain(tc->pool, &key, bio, &cell))
+	get_bio_block_range(tc, bio, &begin, &end);
+	if (begin == end) {
+		/*
+		 * The discard covers less than a block.
+		 */
+		bio_endio(bio, 0);
+		return;
+	}
+
+	build_key(tc->td, VIRTUAL, begin, end, &virt_key);
+	if (bio_detain(tc->pool, &virt_key, bio, &virt_cell))
+		/*
+		 * Potential starvation issue: We're relying on the
+		 * fs/application being well behaved, and not trying to
+		 * send IO to a region at the same time as discarding it.
+		 * If they do this persistently then it's possible this
+		 * cell will never be granted.
+		 */
 		return;
 
-	process_discard_cell(tc, cell);
+	tc->pool->process_discard_cell(tc, virt_cell);
 }
 
 static void break_sharing(struct thin_c *tc, struct bio *bio, dm_block_t block,
@@ -2102,6 +2370,24 @@ static void notify_of_pool_mode_change(struct pool *pool, const char *new_mode)
 	       dm_device_name(pool->pool_md), new_mode);
 }
 
+static bool passdown_enabled(struct pool_c *pt)
+{
+	return pt->adjusted_pf.discard_passdown;
+}
+
+static void set_discard_callbacks(struct pool *pool)
+{
+	struct pool_c *pt = pool->ti->private;
+
+	if (passdown_enabled(pt)) {
+		pool->process_discard_cell = process_discard_cell_passdown;
+		pool->process_prepared_discard = process_prepared_discard_passdown;
+	} else {
+		pool->process_discard_cell = process_discard_cell_no_passdown;
+		pool->process_prepared_discard = process_prepared_discard_no_passdown;
+	}
+}
+
 static void set_pool_mode(struct pool *pool, enum pool_mode new_mode)
 {
 	struct pool_c *pt = pool->ti->private;
@@ -2153,7 +2439,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_cell_success;
 		pool->process_prepared_mapping = process_prepared_mapping_fail;
-		pool->process_prepared_discard = process_prepared_discard_passdown;
+		pool->process_prepared_discard = process_prepared_discard_success;
 
 		error_retry_list(pool);
 		break;
@@ -2172,9 +2458,8 @@ static void set_pool_mode(struct pool *pool, enum pool_mode new_mode)
 		pool->process_bio = process_bio_read_only;
 		pool->process_discard = process_discard_bio;
 		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;
+		set_discard_callbacks(pool);
 
 		if (!pool->pf.error_if_no_space && no_space_timeout)
 			queue_delayed_work(pool->wq, &pool->no_space_timeout, no_space_timeout);
@@ -2187,9 +2472,8 @@ static void set_pool_mode(struct pool *pool, enum pool_mode new_mode)
 		pool->process_bio = process_bio;
 		pool->process_discard = process_discard_bio;
 		pool->process_cell = process_cell;
-		pool->process_discard_cell = process_discard_cell;
 		pool->process_prepared_mapping = process_prepared_mapping;
-		pool->process_prepared_discard = process_prepared_discard;
+		set_discard_callbacks(pool);
 		break;
 	}
 
@@ -2278,6 +2562,7 @@ static void thin_hook_bio(struct thin_c *tc, struct bio *bio)
 	h->shared_read_entry = NULL;
 	h->all_io_entry = NULL;
 	h->overwrite_mapping = NULL;
+	h->cell = NULL;
 }
 
 /*
@@ -2425,7 +2710,6 @@ static void disable_passdown_if_not_supported(struct pool_c *pt)
 	struct pool *pool = pt->pool;
 	struct block_device *data_bdev = pt->data_dev->bdev;
 	struct queue_limits *data_limits = &bdev_get_queue(data_bdev)->limits;
-	sector_t block_size = pool->sectors_per_block << SECTOR_SHIFT;
 	const char *reason = NULL;
 	char buf[BDEVNAME_SIZE];
 
@@ -2438,12 +2722,6 @@ static void disable_passdown_if_not_supported(struct pool_c *pt)
 	else if (data_limits->max_discard_sectors < pool->sectors_per_block)
 		reason = "max discard sectors smaller than a block";
 
-	else if (data_limits->discard_granularity > block_size)
-		reason = "discard granularity larger than a block";
-
-	else if (!is_factor(block_size, data_limits->discard_granularity))
-		reason = "discard granularity not a factor of block size";
-
 	if (reason) {
 		DMWARN("Data device (%s) %s: Disabling discard passdown.", bdevname(data_bdev, buf), reason);
 		pt->adjusted_pf.discard_passdown = false;
@@ -3378,7 +3656,7 @@ static int pool_message(struct dm_target *ti, unsigned argc, char **argv)
 	if (get_pool_mode(pool) >= PM_READ_ONLY) {
 		DMERR("%s: unable to service pool target messages in READ_ONLY or FAIL mode",
 		      dm_device_name(pool->pool_md));
-		return -EINVAL;
+		return -EOPNOTSUPP;
 	}
 
 	if (!strcasecmp(argv[0], "create_thin"))
@@ -3576,24 +3854,6 @@ static int pool_merge(struct dm_target *ti, struct bvec_merge_data *bvm,
 	return min(max_size, q->merge_bvec_fn(q, bvm, biovec));
 }
 
-static void set_discard_limits(struct pool_c *pt, struct queue_limits *limits)
-{
-	struct pool *pool = pt->pool;
-	struct queue_limits *data_limits;
-
-	limits->max_discard_sectors = pool->sectors_per_block;
-
-	/*
-	 * discard_granularity is just a hint, and not enforced.
-	 */
-	if (pt->adjusted_pf.discard_passdown) {
-		data_limits = &bdev_get_queue(pt->data_dev->bdev)->limits;
-		limits->discard_granularity = max(data_limits->discard_granularity,
-						  pool->sectors_per_block << SECTOR_SHIFT);
-	} else
-		limits->discard_granularity = pool->sectors_per_block << SECTOR_SHIFT;
-}
-
 static void pool_io_hints(struct dm_target *ti, struct queue_limits *limits)
 {
 	struct pool_c *pt = ti->private;
@@ -3648,14 +3908,17 @@ static void pool_io_hints(struct dm_target *ti, struct queue_limits *limits)
 
 	disable_passdown_if_not_supported(pt);
 
-	set_discard_limits(pt, limits);
+	/*
+	 * The pool uses the same discard limits as the underlying data
+	 * device.  DM core has already set this up.
+	 */
 }
 
 static struct target_type pool_target = {
 	.name = "thin-pool",
 	.features = DM_TARGET_SINGLETON | DM_TARGET_ALWAYS_WRITEABLE |
 		    DM_TARGET_IMMUTABLE,
-	.version = {1, 14, 0},
+	.version = {1, 15, 0},
 	.module = THIS_MODULE,
 	.ctr = pool_ctr,
 	.dtr = pool_dtr,
@@ -3814,8 +4077,7 @@ static int thin_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	if (tc->pool->pf.discard_enabled) {
 		ti->discards_supported = true;
 		ti->num_discard_bios = 1;
-		/* Discard bios must be split on a block boundary */
-		ti->split_discard_bios = true;
+		ti->split_discard_bios = false;
 	}
 
 	mutex_unlock(&dm_thin_pool_table.mutex);
@@ -3902,6 +4164,9 @@ static int thin_endio(struct dm_target *ti, struct bio *bio, int err)
 		}
 	}
 
+	if (h->cell)
+		cell_defer_no_holder(h->tc, h->cell);
+
 	return 0;
 }
 
@@ -4029,9 +4294,18 @@ static int thin_iterate_devices(struct dm_target *ti,
 	return 0;
 }
 
+static void thin_io_hints(struct dm_target *ti, struct queue_limits *limits)
+{
+	struct thin_c *tc = ti->private;
+	struct pool *pool = tc->pool;
+
+	limits->discard_granularity = pool->sectors_per_block << SECTOR_SHIFT;
+	limits->max_discard_sectors = 2048 * 1024 * 16; /* 16G */
+}
+
 static struct target_type thin_target = {
 	.name = "thin",
-	.version = {1, 14, 0},
+	.version = {1, 15, 0},
 	.module	= THIS_MODULE,
 	.ctr = thin_ctr,
 	.dtr = thin_dtr,
@@ -4043,6 +4317,7 @@ static struct target_type thin_target = {
 	.status = thin_status,
 	.merge = thin_merge,
 	.iterate_devices = thin_iterate_devices,
+	.io_hints = thin_io_hints,
 };
 
 /*----------------------------------------------------------------*/
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity.c
index 66616db33e6f..bb9c6a00e4b0 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity.c
@@ -459,7 +459,7 @@ static void verity_finish_io(struct dm_verity_io *io, int error)
 	bio->bi_end_io = io->orig_bi_end_io;
 	bio->bi_private = io->orig_bi_private;
 
-	bio_endio_nodec(bio, error);
+	bio_endio(bio, error);
 }
 
 static void verity_work(struct work_struct *w)
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 2caf492890d6..f331d888e7f5 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -86,6 +86,9 @@ struct dm_rq_target_io {
 	struct kthread_work work;
 	int error;
 	union map_info info;
+	struct dm_stats_aux stats_aux;
+	unsigned long duration_jiffies;
+	unsigned n_sectors;
 };
 
 /*
@@ -1046,6 +1049,17 @@ static struct dm_rq_target_io *tio_from_request(struct request *rq)
 	return (rq->q->mq_ops ? blk_mq_rq_to_pdu(rq) : rq->special);
 }
 
+static void rq_end_stats(struct mapped_device *md, struct request *orig)
+{
+	if (unlikely(dm_stats_used(&md->stats))) {
+		struct dm_rq_target_io *tio = tio_from_request(orig);
+		tio->duration_jiffies = jiffies - tio->duration_jiffies;
+		dm_stats_account_io(&md->stats, orig->cmd_flags, blk_rq_pos(orig),
+				    tio->n_sectors, true, tio->duration_jiffies,
+				    &tio->stats_aux);
+	}
+}
+
 /*
  * Don't touch any member of the md after calling this function because
  * the md may be freed in dm_put() at the end of this function.
@@ -1131,6 +1145,7 @@ static void dm_end_request(struct request *clone, int error)
 	}
 
 	free_rq_clone(clone);
+	rq_end_stats(md, rq);
 	if (!rq->q->mq_ops)
 		blk_end_request_all(rq, error);
 	else
@@ -1166,13 +1181,14 @@ static void old_requeue_request(struct request *rq)
 	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 
-static void dm_requeue_unmapped_original_request(struct mapped_device *md,
-						 struct request *rq)
+static void dm_requeue_original_request(struct mapped_device *md,
+					struct request *rq)
 {
 	int rw = rq_data_dir(rq);
 
 	dm_unprep_request(rq);
 
+	rq_end_stats(md, rq);
 	if (!rq->q->mq_ops)
 		old_requeue_request(rq);
 	else {
@@ -1183,13 +1199,6 @@ static void dm_requeue_unmapped_original_request(struct mapped_device *md,
 	rq_completed(md, rw, false);
 }
 
-static void dm_requeue_unmapped_request(struct request *clone)
-{
-	struct dm_rq_target_io *tio = clone->end_io_data;
-
-	dm_requeue_unmapped_original_request(tio->md, tio->orig);
-}
-
 static void old_stop_queue(struct request_queue *q)
 {
 	unsigned long flags;
@@ -1253,7 +1262,7 @@ static void dm_done(struct request *clone, int error, bool mapped)
 		return;
 	else if (r == DM_ENDIO_REQUEUE)
 		/* The target wants to requeue the I/O */
-		dm_requeue_unmapped_request(clone);
+		dm_requeue_original_request(tio->md, tio->orig);
 	else {
 		DMWARN("unimplemented target endio return value: %d", r);
 		BUG();
@@ -1271,6 +1280,7 @@ static void dm_softirq_done(struct request *rq)
 	int rw;
 
 	if (!clone) {
+		rq_end_stats(tio->md, rq);
 		rw = rq_data_dir(rq);
 		if (!rq->q->mq_ops) {
 			blk_end_request_all(rq, tio->error);
@@ -1998,7 +2008,7 @@ static int map_request(struct dm_rq_target_io *tio, struct request *rq,
 		break;
 	case DM_MAPIO_REQUEUE:
 		/* The target wants to requeue the I/O */
-		dm_requeue_unmapped_request(clone);
+		dm_requeue_original_request(md, tio->orig);
 		break;
 	default:
 		if (r > 0) {
@@ -2021,7 +2031,7 @@ static void map_tio_request(struct kthread_work *work)
 	struct mapped_device *md = tio->md;
 
 	if (map_request(tio, rq, md) == DM_MAPIO_REQUEUE)
-		dm_requeue_unmapped_original_request(md, rq);
+		dm_requeue_original_request(md, rq);
 }
 
 static void dm_start_request(struct mapped_device *md, struct request *orig)
@@ -2038,6 +2048,14 @@ static void dm_start_request(struct mapped_device *md, struct request *orig)
 		md->last_rq_start_time = ktime_get();
 	}
 
+	if (unlikely(dm_stats_used(&md->stats))) {
+		struct dm_rq_target_io *tio = tio_from_request(orig);
+		tio->duration_jiffies = jiffies;
+		tio->n_sectors = blk_rq_sectors(orig);
+		dm_stats_account_io(&md->stats, orig->cmd_flags, blk_rq_pos(orig),
+				    tio->n_sectors, false, 0, &tio->stats_aux);
+	}
+
 	/*
 	 * Hold the md reference here for the in-flight I/O.
 	 * We can't rely on the reference count by device opener,
@@ -2168,7 +2186,7 @@ static int dm_any_congested(void *congested_data, int bdi_bits)
 			 * the query about congestion status of request_queue
 			 */
 			if (dm_request_based(md))
-				r = md->queue->backing_dev_info.state &
+				r = md->queue->backing_dev_info.wb.state &
 				    bdi_bits;
 			else
 				r = dm_table_any_congested(map, bdi_bits);
@@ -2261,6 +2279,40 @@ static void dm_init_old_md_queue(struct mapped_device *md)
 	blk_queue_bounce_limit(md->queue, BLK_BOUNCE_ANY);
 }
 
+static void cleanup_mapped_device(struct mapped_device *md)
+{
+	cleanup_srcu_struct(&md->io_barrier);
+
+	if (md->wq)
+		destroy_workqueue(md->wq);
+	if (md->kworker_task)
+		kthread_stop(md->kworker_task);
+	if (md->io_pool)
+		mempool_destroy(md->io_pool);
+	if (md->rq_pool)
+		mempool_destroy(md->rq_pool);
+	if (md->bs)
+		bioset_free(md->bs);
+
+	if (md->disk) {
+		spin_lock(&_minor_lock);
+		md->disk->private_data = NULL;
+		spin_unlock(&_minor_lock);
+		if (blk_get_integrity(md->disk))
+			blk_integrity_unregister(md->disk);
+		del_gendisk(md->disk);
+		put_disk(md->disk);
+	}
+
+	if (md->queue)
+		blk_cleanup_queue(md->queue);
+
+	if (md->bdev) {
+		bdput(md->bdev);
+		md->bdev = NULL;
+	}
+}
+
 /*
  * Allocate and initialise a blank device with a given minor.
  */
@@ -2306,13 +2358,13 @@ static struct mapped_device *alloc_dev(int minor)
 
 	md->queue = blk_alloc_queue(GFP_KERNEL);
 	if (!md->queue)
-		goto bad_queue;
+		goto bad;
 
 	dm_init_md_queue(md);
 
 	md->disk = alloc_disk(1);
 	if (!md->disk)
-		goto bad_disk;
+		goto bad;
 
 	atomic_set(&md->pending[0], 0);
 	atomic_set(&md->pending[1], 0);
@@ -2333,11 +2385,11 @@ static struct mapped_device *alloc_dev(int minor)
 
 	md->wq = alloc_workqueue("kdmflush", WQ_MEM_RECLAIM, 0);
 	if (!md->wq)
-		goto bad_thread;
+		goto bad;
 
 	md->bdev = bdget_disk(md->disk, 0);
 	if (!md->bdev)
-		goto bad_bdev;
+		goto bad;
 
 	bio_init(&md->flush_bio);
 	md->flush_bio.bi_bdev = md->bdev;
@@ -2354,15 +2406,8 @@ static struct mapped_device *alloc_dev(int minor)
 
 	return md;
 
-bad_bdev:
-	destroy_workqueue(md->wq);
-bad_thread:
-	del_gendisk(md->disk);
-	put_disk(md->disk);
-bad_disk:
-	blk_cleanup_queue(md->queue);
-bad_queue:
-	cleanup_srcu_struct(&md->io_barrier);
+bad:
+	cleanup_mapped_device(md);
 bad_io_barrier:
 	free_minor(minor);
 bad_minor:
@@ -2379,32 +2424,13 @@ static void free_dev(struct mapped_device *md)
 	int minor = MINOR(disk_devt(md->disk));
 
 	unlock_fs(md);
-	destroy_workqueue(md->wq);
 
-	if (md->kworker_task)
-		kthread_stop(md->kworker_task);
-	if (md->io_pool)
-		mempool_destroy(md->io_pool);
-	if (md->rq_pool)
-		mempool_destroy(md->rq_pool);
-	if (md->bs)
-		bioset_free(md->bs);
+	cleanup_mapped_device(md);
+	if (md->use_blk_mq)
+		blk_mq_free_tag_set(&md->tag_set);
 
-	cleanup_srcu_struct(&md->io_barrier);
 	free_table_devices(&md->table_devices);
 	dm_stats_cleanup(&md->stats);
-
-	spin_lock(&_minor_lock);
-	md->disk->private_data = NULL;
-	spin_unlock(&_minor_lock);
-	if (blk_get_integrity(md->disk))
-		blk_integrity_unregister(md->disk);
-	del_gendisk(md->disk);
-	put_disk(md->disk);
-	blk_cleanup_queue(md->queue);
-	if (md->use_blk_mq)
-		blk_mq_free_tag_set(&md->tag_set);
-	bdput(md->bdev);
 	free_minor(minor);
 
 	module_put(THIS_MODULE);
@@ -2765,6 +2791,7 @@ static int dm_mq_queue_rq(struct blk_mq_hw_ctx *hctx,
 		/* Direct call is fine since .queue_rq allows allocations */
 		if (map_request(tio, rq, md) == DM_MAPIO_REQUEUE) {
 			/* Undo dm_start_request() before requeuing */
+			rq_end_stats(md, rq);
 			rq_completed(md, rq_data_dir(rq), false);
 			return BLK_MQ_RQ_QUEUE_BUSY;
 		}
diff --git a/drivers/md/dm.h b/drivers/md/dm.h
index 6123c2bf9150..4e984993d40a 100644
--- a/drivers/md/dm.h
+++ b/drivers/md/dm.h
@@ -14,6 +14,7 @@
 #include <linux/device-mapper.h>
 #include <linux/list.h>
 #include <linux/blkdev.h>
+#include <linux/backing-dev.h>
 #include <linux/hdreg.h>
 #include <linux/completion.h>
 #include <linux/kobject.h>
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 27506302eb7a..d429c30cd514 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -2024,7 +2024,6 @@ static int bind_rdev_to_array(struct md_rdev *rdev, struct mddev *mddev)
 {
 	char b[BDEVNAME_SIZE];
 	struct kobject *ko;
-	char *s;
 	int err;
 
 	/* prevent duplicates */
@@ -2070,8 +2069,7 @@ static int bind_rdev_to_array(struct md_rdev *rdev, struct mddev *mddev)
 		return -EBUSY;
 	}
 	bdevname(rdev->bdev,b);
-	while ( (s=strchr(b, '/')) != NULL)
-		*s = '!';
+	strreplace(b, '/', '!');
 
 	rdev->mddev = mddev;
 	printk(KERN_INFO "md: bind<%s>\n", b);
@@ -2630,13 +2628,14 @@ errors_show(struct md_rdev *rdev, char *page)
 static ssize_t
 errors_store(struct md_rdev *rdev, const char *buf, size_t len)
 {
-	char *e;
-	unsigned long n = simple_strtoul(buf, &e, 10);
-	if (*buf && (*e == 0 || *e == '\n')) {
-		atomic_set(&rdev->corrected_errors, n);
-		return len;
-	}
-	return -EINVAL;
+	unsigned int n;
+	int rv;
+
+	rv = kstrtouint(buf, 10, &n);
+	if (rv < 0)
+		return rv;
+	atomic_set(&rdev->corrected_errors, n);
+	return len;
 }
 static struct rdev_sysfs_entry rdev_errors =
 __ATTR(errors, S_IRUGO|S_IWUSR, errors_show, errors_store);
@@ -2653,13 +2652,16 @@ slot_show(struct md_rdev *rdev, char *page)
 static ssize_t
 slot_store(struct md_rdev *rdev, const char *buf, size_t len)
 {
-	char *e;
+	int slot;
 	int err;
-	int slot = simple_strtoul(buf, &e, 10);
+
 	if (strncmp(buf, "none", 4)==0)
 		slot = -1;
-	else if (e==buf || (*e && *e!= '\n'))
-		return -EINVAL;
+	else {
+		err = kstrtouint(buf, 10, (unsigned int *)&slot);
+		if (err < 0)
+			return err;
+	}
 	if (rdev->mddev->pers && slot == -1) {
 		/* Setting 'slot' on an active array requires also
 		 * updating the 'rd%d' link, and communicating
@@ -3544,12 +3546,12 @@ layout_show(struct mddev *mddev, char *page)
 static ssize_t
 layout_store(struct mddev *mddev, const char *buf, size_t len)
 {
-	char *e;
-	unsigned long n = simple_strtoul(buf, &e, 10);
+	unsigned int n;
 	int err;
 
-	if (!*buf || (*e && *e != '\n'))
-		return -EINVAL;
+	err = kstrtouint(buf, 10, &n);
+	if (err < 0)
+		return err;
 	err = mddev_lock(mddev);
 	if (err)
 		return err;
@@ -3593,12 +3595,12 @@ static int update_raid_disks(struct mddev *mddev, int raid_disks);
 static ssize_t
 raid_disks_store(struct mddev *mddev, const char *buf, size_t len)
 {
-	char *e;
+	unsigned int n;
 	int err;
-	unsigned long n = simple_strtoul(buf, &e, 10);
 
-	if (!*buf || (*e && *e != '\n'))
-		return -EINVAL;
+	err = kstrtouint(buf, 10, &n);
+	if (err < 0)
+		return err;
 
 	err = mddev_lock(mddev);
 	if (err)
@@ -3645,12 +3647,12 @@ chunk_size_show(struct mddev *mddev, char *page)
 static ssize_t
 chunk_size_store(struct mddev *mddev, const char *buf, size_t len)
 {
+	unsigned long n;
 	int err;
-	char *e;
-	unsigned long n = simple_strtoul(buf, &e, 10);
 
-	if (!*buf || (*e && *e != '\n'))
-		return -EINVAL;
+	err = kstrtoul(buf, 10, &n);
+	if (err < 0)
+		return err;
 
 	err = mddev_lock(mddev);
 	if (err)
@@ -3688,19 +3690,24 @@ resync_start_show(struct mddev *mddev, char *page)
 static ssize_t
 resync_start_store(struct mddev *mddev, const char *buf, size_t len)
 {
+	unsigned long long n;
 	int err;
-	char *e;
-	unsigned long long n = simple_strtoull(buf, &e, 10);
+
+	if (cmd_match(buf, "none"))
+		n = MaxSector;
+	else {
+		err = kstrtoull(buf, 10, &n);
+		if (err < 0)
+			return err;
+		if (n != (sector_t)n)
+			return -EINVAL;
+	}
 
 	err = mddev_lock(mddev);
 	if (err)
 		return err;
 	if (mddev->pers && !test_bit(MD_RECOVERY_FROZEN, &mddev->recovery))
 		err = -EBUSY;
-	else if (cmd_match(buf, "none"))
-		n = MaxSector;
-	else if (!*buf || (*e && *e != '\n'))
-		err = -EINVAL;
 
 	if (!err) {
 		mddev->recovery_cp = n;
@@ -3834,7 +3841,7 @@ array_state_store(struct mddev *mddev, const char *buf, size_t len)
 				err = -EBUSY;
 		}
 		spin_unlock(&mddev->lock);
-		return err;
+		return err ?: len;
 	}
 	err = mddev_lock(mddev);
 	if (err)
@@ -3936,14 +3943,14 @@ max_corrected_read_errors_show(struct mddev *mddev, char *page) {
 static ssize_t
 max_corrected_read_errors_store(struct mddev *mddev, const char *buf, size_t len)
 {
-	char *e;
-	unsigned long n = simple_strtoul(buf, &e, 10);
+	unsigned int n;
+	int rv;
 
-	if (*buf && (*e == 0 || *e == '\n')) {
-		atomic_set(&mddev->max_corr_read_errors, n);
-		return len;
-	}
-	return -EINVAL;
+	rv = kstrtouint(buf, 10, &n);
+	if (rv < 0)
+		return rv;
+	atomic_set(&mddev->max_corr_read_errors, n);
+	return len;
 }
 
 static struct md_sysfs_entry max_corr_read_errors =
@@ -4005,8 +4012,10 @@ new_dev_store(struct mddev *mddev, const char *buf, size_t len)
 	else
 		rdev = md_import_device(dev, -1, -1);
 
-	if (IS_ERR(rdev))
+	if (IS_ERR(rdev)) {
+		mddev_unlock(mddev);
 		return PTR_ERR(rdev);
+	}
 	err = bind_rdev_to_array(rdev, mddev);
  out:
 	if (err)
@@ -4217,13 +4226,14 @@ action_store(struct mddev *mddev, const char *page, size_t len)
 			set_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
 		else
 			clear_bit(MD_RECOVERY_FROZEN, &mddev->recovery);
-		flush_workqueue(md_misc_wq);
-		if (mddev->sync_thread) {
-			set_bit(MD_RECOVERY_INTR, &mddev->recovery);
-			if (mddev_lock(mddev) == 0) {
+		if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) &&
+		    mddev_lock(mddev) == 0) {
+			flush_workqueue(md_misc_wq);
+			if (mddev->sync_thread) {
+				set_bit(MD_RECOVERY_INTR, &mddev->recovery);
 				md_reap_sync_thread(mddev);
-				mddev_unlock(mddev);
 			}
+			mddev_unlock(mddev);
 		}
 	} else if (test_bit(MD_RECOVERY_RUNNING, &mddev->recovery) ||
 		   test_bit(MD_RECOVERY_NEEDED, &mddev->recovery))
@@ -4299,15 +4309,18 @@ sync_min_show(struct mddev *mddev, char *page)
 static ssize_t
 sync_min_store(struct mddev *mddev, const char *buf, size_t len)
 {
-	int min;
-	char *e;
+	unsigned int min;
+	int rv;
+
 	if (strncmp(buf, "system", 6)==0) {
-		mddev->sync_speed_min = 0;
-		return len;
+		min = 0;
+	} else {
+		rv = kstrtouint(buf, 10, &min);
+		if (rv < 0)
+			return rv;
+		if (min == 0)
+			return -EINVAL;
 	}
-	min = simple_strtoul(buf, &e, 10);
-	if (buf == e || (*e && *e != '\n') || min <= 0)
-		return -EINVAL;
 	mddev->sync_speed_min = min;
 	return len;
 }
@@ -4325,15 +4338,18 @@ sync_max_show(struct mddev *mddev, char *page)
 static ssize_t
 sync_max_store(struct mddev *mddev, const char *buf, size_t len)
 {
-	int max;
-	char *e;
+	unsigned int max;
+	int rv;
+
 	if (strncmp(buf, "system", 6)==0) {
-		mddev->sync_speed_max = 0;
-		return len;
+		max = 0;
+	} else {
+		rv = kstrtouint(buf, 10, &max);
+		if (rv < 0)
+			return rv;
+		if (max == 0)
+			return -EINVAL;
 	}
-	max = simple_strtoul(buf, &e, 10);
-	if (buf == e || (*e && *e != '\n') || max <= 0)
-		return -EINVAL;
 	mddev->sync_speed_max = max;
 	return len;
 }
@@ -4516,12 +4532,13 @@ suspend_lo_show(struct mddev *mddev, char *page)
 static ssize_t
 suspend_lo_store(struct mddev *mddev, const char *buf, size_t len)
 {
-	char *e;
-	unsigned long long new = simple_strtoull(buf, &e, 10);
-	unsigned long long old;
+	unsigned long long old, new;
 	int err;
 
-	if (buf == e || (*e && *e != '\n'))
+	err = kstrtoull(buf, 10, &new);
+	if (err < 0)
+		return err;
+	if (new != (sector_t)new)
 		return -EINVAL;
 
 	err = mddev_lock(mddev);
@@ -4558,12 +4575,13 @@ suspend_hi_show(struct mddev *mddev, char *page)
 static ssize_t
 suspend_hi_store(struct mddev *mddev, const char *buf, size_t len)
 {
-	char *e;
-	unsigned long long new = simple_strtoull(buf, &e, 10);
-	unsigned long long old;
+	unsigned long long old, new;
 	int err;
 
-	if (buf == e || (*e && *e != '\n'))
+	err = kstrtoull(buf, 10, &new);
+	if (err < 0)
+		return err;
+	if (new != (sector_t)new)
 		return -EINVAL;
 
 	err = mddev_lock(mddev);
@@ -4605,11 +4623,13 @@ static ssize_t
 reshape_position_store(struct mddev *mddev, const char *buf, size_t len)
 {
 	struct md_rdev *rdev;
-	char *e;
+	unsigned long long new;
 	int err;
-	unsigned long long new = simple_strtoull(buf, &e, 10);
 
-	if (buf == e || (*e && *e != '\n'))
+	err = kstrtoull(buf, 10, &new);
+	if (err < 0)
+		return err;
+	if (new != (sector_t)new)
 		return -EINVAL;
 	err = mddev_lock(mddev);
 	if (err)
@@ -5158,6 +5178,7 @@ int md_run(struct mddev *mddev)
 		mddev_detach(mddev);
 		if (mddev->private)
 			pers->free(mddev, mddev->private);
+		mddev->private = NULL;
 		module_put(pers->owner);
 		bitmap_destroy(mddev);
 		return err;
@@ -5293,6 +5314,7 @@ static void md_clean(struct mddev *mddev)
 	mddev->changed = 0;
 	mddev->degraded = 0;
 	mddev->safemode = 0;
+	mddev->private = NULL;
 	mddev->merge_check_needed = 0;
 	mddev->bitmap_info.offset = 0;
 	mddev->bitmap_info.default_offset = 0;
@@ -5365,6 +5387,7 @@ static void __md_stop(struct mddev *mddev)
 	mddev->pers = NULL;
 	spin_unlock(&mddev->lock);
 	pers->free(mddev, mddev->private);
+	mddev->private = NULL;
 	if (pers->sync_request && mddev->to_remove == NULL)
 		mddev->to_remove = &md_redundancy_group;
 	module_put(pers->owner);
@@ -5743,7 +5766,7 @@ static int get_bitmap_file(struct mddev *mddev, void __user * arg)
 	/* bitmap disabled, zero the first byte and copy out */
 	if (!mddev->bitmap_info.file)
 		file->pathname[0] = '\0';
-	else if ((ptr = d_path(&mddev->bitmap_info.file->f_path,
+	else if ((ptr = file_path(mddev->bitmap_info.file,
 			       file->pathname, sizeof(file->pathname))),
 		 IS_ERR(ptr))
 		err = PTR_ERR(ptr);
@@ -6374,7 +6397,7 @@ static int update_array_info(struct mddev *mddev, mdu_array_info_t *info)
 	    mddev->ctime         != info->ctime         ||
 	    mddev->level         != info->level         ||
 /*	    mddev->layout        != info->layout        || */
-	    !mddev->persistent	 != info->not_persistent||
+	    mddev->persistent	 != !info->not_persistent ||
 	    mddev->chunk_sectors != info->chunk_size >> 9 ||
 	    /* ignore bottom 8 bits of state, and allow SB_BITMAP_PRESENT to change */
 	    ((state^info->state) & 0xfffffe00)
@@ -8105,6 +8128,15 @@ void md_check_recovery(struct mddev *mddev)
 		int spares = 0;
 
 		if (mddev->ro) {
+			struct md_rdev *rdev;
+			if (!mddev->external && mddev->in_sync)
+				/* 'Blocked' flag not needed as failed devices
+				 * will be recorded if array switched to read/write.
+				 * Leaving it set will prevent the device
+				 * from being removed.
+				 */
+				rdev_for_each(rdev, mddev)
+					clear_bit(Blocked, &rdev->flags);
 			/* On a read-only array we can:
 			 * - remove failed devices
 			 * - add already-in_sync devices if the array itself
@@ -8261,6 +8293,7 @@ void md_reap_sync_thread(struct mddev *mddev)
 	if (mddev_is_clustered(mddev))
 		md_cluster_ops->metadata_update_finish(mddev);
 	clear_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
+	clear_bit(MD_RECOVERY_DONE, &mddev->recovery);
 	clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
 	clear_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
 	clear_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
@@ -9011,13 +9044,7 @@ static int get_ro(char *buffer, struct kernel_param *kp)
 }
 static int set_ro(const char *val, struct kernel_param *kp)
 {
-	char *e;
-	int num = simple_strtoul(val, &e, 10);
-	if (*val && (*e == '\0' || *e == '\n')) {
-		start_readonly = num;
-		return 0;
-	}
-	return -EINVAL;
+	return kstrtouint(val, 10, (unsigned int *)&start_readonly);
 }
 
 module_param_call(start_ro, set_ro, get_ro, NULL, S_IRUSR|S_IWUSR);
diff --git a/drivers/md/md.h b/drivers/md/md.h
index 4046a6c6f223..7da6e9c3cb53 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -16,6 +16,7 @@
 #define _MD_MD_H
 
 #include <linux/blkdev.h>
+#include <linux/backing-dev.h>
 #include <linux/kobject.h>
 #include <linux/list.h>
 #include <linux/mm.h>
diff --git a/drivers/md/persistent-data/dm-block-manager.c b/drivers/md/persistent-data/dm-block-manager.c
index 087411c95ffc..4d6c9b689eaa 100644
--- a/drivers/md/persistent-data/dm-block-manager.c
+++ b/drivers/md/persistent-data/dm-block-manager.c
@@ -609,6 +609,12 @@ void dm_bm_prefetch(struct dm_block_manager *bm, dm_block_t b)
 	dm_bufio_prefetch(bm->bufio, b, 1);
 }
 
+bool dm_bm_is_read_only(struct dm_block_manager *bm)
+{
+	return bm->read_only;
+}
+EXPORT_SYMBOL_GPL(dm_bm_is_read_only);
+
 void dm_bm_set_read_only(struct dm_block_manager *bm)
 {
 	bm->read_only = true;
diff --git a/drivers/md/persistent-data/dm-block-manager.h b/drivers/md/persistent-data/dm-block-manager.h
index 1b95dfc17786..84330f59886d 100644
--- a/drivers/md/persistent-data/dm-block-manager.h
+++ b/drivers/md/persistent-data/dm-block-manager.h
@@ -123,6 +123,7 @@ void dm_bm_prefetch(struct dm_block_manager *bm, dm_block_t b);
  * Additionally you should not use dm_bm_unlock_move, however no error will
  * be returned if you do.
  */
+bool dm_bm_is_read_only(struct dm_block_manager *bm);
 void dm_bm_set_read_only(struct dm_block_manager *bm);
 void dm_bm_set_read_write(struct dm_block_manager *bm);
 
diff --git a/drivers/md/persistent-data/dm-btree-remove.c b/drivers/md/persistent-data/dm-btree-remove.c
index b88757cd0d1d..e04cfd2d60ef 100644
--- a/drivers/md/persistent-data/dm-btree-remove.c
+++ b/drivers/md/persistent-data/dm-btree-remove.c
@@ -590,3 +590,130 @@ int dm_btree_remove(struct dm_btree_info *info, dm_block_t root,
 	return r;
 }
 EXPORT_SYMBOL_GPL(dm_btree_remove);
+
+/*----------------------------------------------------------------*/
+
+static int remove_nearest(struct shadow_spine *s, struct dm_btree_info *info,
+			  struct dm_btree_value_type *vt, dm_block_t root,
+			  uint64_t key, int *index)
+{
+	int i = *index, r;
+	struct btree_node *n;
+
+	for (;;) {
+		r = shadow_step(s, root, vt);
+		if (r < 0)
+			break;
+
+		/*
+		 * We have to patch up the parent node, ugly, but I don't
+		 * see a way to do this automatically as part of the spine
+		 * op.
+		 */
+		if (shadow_has_parent(s)) {
+			__le64 location = cpu_to_le64(dm_block_location(shadow_current(s)));
+			memcpy(value_ptr(dm_block_data(shadow_parent(s)), i),
+			       &location, sizeof(__le64));
+		}
+
+		n = dm_block_data(shadow_current(s));
+
+		if (le32_to_cpu(n->header.flags) & LEAF_NODE) {
+			*index = lower_bound(n, key);
+			return 0;
+		}
+
+		r = rebalance_children(s, info, vt, key);
+		if (r)
+			break;
+
+		n = dm_block_data(shadow_current(s));
+		if (le32_to_cpu(n->header.flags) & LEAF_NODE) {
+			*index = lower_bound(n, key);
+			return 0;
+		}
+
+		i = lower_bound(n, key);
+
+		/*
+		 * We know the key is present, or else
+		 * rebalance_children would have returned
+		 * -ENODATA
+		 */
+		root = value64(n, i);
+	}
+
+	return r;
+}
+
+static int remove_one(struct dm_btree_info *info, dm_block_t root,
+		      uint64_t *keys, uint64_t end_key,
+		      dm_block_t *new_root, unsigned *nr_removed)
+{
+	unsigned level, last_level = info->levels - 1;
+	int index = 0, r = 0;
+	struct shadow_spine spine;
+	struct btree_node *n;
+	uint64_t k;
+
+	init_shadow_spine(&spine, info);
+	for (level = 0; level < last_level; level++) {
+		r = remove_raw(&spine, info, &le64_type,
+			       root, keys[level], (unsigned *) &index);
+		if (r < 0)
+			goto out;
+
+		n = dm_block_data(shadow_current(&spine));
+		root = value64(n, index);
+	}
+
+	r = remove_nearest(&spine, info, &info->value_type,
+			   root, keys[last_level], &index);
+	if (r < 0)
+		goto out;
+
+	n = dm_block_data(shadow_current(&spine));
+
+	if (index < 0)
+		index = 0;
+
+	if (index >= le32_to_cpu(n->header.nr_entries)) {
+		r = -ENODATA;
+		goto out;
+	}
+
+	k = le64_to_cpu(n->keys[index]);
+	if (k >= keys[last_level] && k < end_key) {
+		if (info->value_type.dec)
+			info->value_type.dec(info->value_type.context,
+					     value_ptr(n, index));
+
+		delete_at(n, index);
+
+	} else
+		r = -ENODATA;
+
+out:
+	*new_root = shadow_root(&spine);
+	exit_shadow_spine(&spine);
+
+	return r;
+}
+
+int dm_btree_remove_leaves(struct dm_btree_info *info, dm_block_t root,
+			   uint64_t *first_key, uint64_t end_key,
+			   dm_block_t *new_root, unsigned *nr_removed)
+{
+	int r;
+
+	*nr_removed = 0;
+	do {
+		r = remove_one(info, root, first_key, end_key, &root, nr_removed);
+		if (!r)
+			(*nr_removed)++;
+	} while (!r);
+
+	*new_root = root;
+	return r == -ENODATA ? 0 : r;
+}
+EXPORT_SYMBOL_GPL(dm_btree_remove_leaves);
diff --git a/drivers/md/persistent-data/dm-btree.h b/drivers/md/persistent-data/dm-btree.h
index dacfc34180b4..11d8cf78621d 100644
--- a/drivers/md/persistent-data/dm-btree.h
+++ b/drivers/md/persistent-data/dm-btree.h
@@ -135,6 +135,15 @@ int dm_btree_remove(struct dm_btree_info *info, dm_block_t root,
 		    uint64_t *keys, dm_block_t *new_root);
 
 /*
+ * Removes values between 'keys' and keys2, where keys2 is keys with the
+ * final key replaced with 'end_key'.  'end_key' is the one-past-the-end
+ * value.  'keys' may be altered.
+ */
+int dm_btree_remove_leaves(struct dm_btree_info *info, dm_block_t root,
+			   uint64_t *keys, uint64_t end_key,
+			   dm_block_t *new_root, unsigned *nr_removed);
+
+/*
  * Returns < 0 on failure.  Otherwise the number of key entries that have
  * been filled out.  Remember trees can have zero entries, and as such have
  * no lowest key.
diff --git a/drivers/md/persistent-data/dm-space-map-metadata.c b/drivers/md/persistent-data/dm-space-map-metadata.c
index e8a904298887..53091295fce9 100644
--- a/drivers/md/persistent-data/dm-space-map-metadata.c
+++ b/drivers/md/persistent-data/dm-space-map-metadata.c
@@ -204,6 +204,27 @@ static void in(struct sm_metadata *smm)
 	smm->recursion_count++;
 }
 
+static int apply_bops(struct sm_metadata *smm)
+{
+	int r = 0;
+
+	while (!brb_empty(&smm->uncommitted)) {
+		struct block_op bop;
+
+		r = brb_pop(&smm->uncommitted, &bop);
+		if (r) {
+			DMERR("bug in bop ring buffer");
+			break;
+		}
+
+		r = commit_bop(smm, &bop);
+		if (r)
+			break;
+	}
+
+	return r;
+}
+
 static int out(struct sm_metadata *smm)
 {
 	int r = 0;
@@ -216,21 +237,8 @@ static int out(struct sm_metadata *smm)
 		return -ENOMEM;
 	}
 
-	if (smm->recursion_count == 1) {
-		while (!brb_empty(&smm->uncommitted)) {
-			struct block_op bop;
-
-			r = brb_pop(&smm->uncommitted, &bop);
-			if (r) {
-				DMERR("bug in bop ring buffer");
-				break;
-			}
-
-			r = commit_bop(smm, &bop);
-			if (r)
-				break;
-		}
-	}
+	if (smm->recursion_count == 1)
+		apply_bops(smm);
 
 	smm->recursion_count--;
 
@@ -704,6 +712,12 @@ static int sm_metadata_extend(struct dm_space_map *sm, dm_block_t extra_blocks)
 		}
 		old_len = smm->begin;
 
+		r = apply_bops(smm);
+		if (r) {
+			DMERR("%s: apply_bops failed", __func__);
+			goto out;
+		}
+
 		r = sm_ll_commit(&smm->ll);
 		if (r)
 			goto out;
@@ -773,6 +787,12 @@ int dm_sm_metadata_create(struct dm_space_map *sm,
 	if (r)
 		return r;
 
+	r = apply_bops(smm);
+	if (r) {
+		DMERR("%s: apply_bops failed", __func__);
+		return r;
+	}
+
 	return sm_metadata_commit(sm);
 }
 
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index 9157a29c8dbf..f80f1af61ce7 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -745,7 +745,7 @@ static int raid1_congested(struct mddev *mddev, int bits)
 	struct r1conf *conf = mddev->private;
 	int i, ret = 0;
 
-	if ((bits & (1 << BDI_async_congested)) &&
+	if ((bits & (1 << WB_async_congested)) &&
 	    conf->pending_count >= max_queued_requests)
 		return 1;
 
@@ -760,7 +760,7 @@ static int raid1_congested(struct mddev *mddev, int bits)
 			/* Note the '|| 1' - when read_balance prefers
 			 * non-congested targets, it can be removed
 			 */
-			if ((bits & (1<<BDI_async_congested)) || 1)
+			if ((bits & (1 << WB_async_congested)) || 1)
 				ret |= bdi_congested(&q->backing_dev_info, bits);
 			else
 				ret &= bdi_congested(&q->backing_dev_info, bits);
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index e793ab6b3570..940f2f365461 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -914,7 +914,7 @@ static int raid10_congested(struct mddev *mddev, int bits)
 	struct r10conf *conf = mddev->private;
 	int i, ret = 0;
 
-	if ((bits & (1 << BDI_async_congested)) &&
+	if ((bits & (1 << WB_async_congested)) &&
 	    conf->pending_count >= max_queued_requests)
 		return 1;
 
@@ -2099,17 +2099,10 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 		tbio->bi_rw = WRITE;
 		tbio->bi_private = r10_bio;
 		tbio->bi_iter.bi_sector = r10_bio->devs[i].addr;
-
-		for (j=0; j < vcnt ; j++) {
-			tbio->bi_io_vec[j].bv_offset = 0;
-			tbio->bi_io_vec[j].bv_len = PAGE_SIZE;
-
-			memcpy(page_address(tbio->bi_io_vec[j].bv_page),
-			       page_address(fbio->bi_io_vec[j].bv_page),
-			       PAGE_SIZE);
-		}
 		tbio->bi_end_io = end_sync_write;
 
+		bio_copy_data(tbio, fbio);
+
 		d = r10_bio->devs[i].devnum;
 		atomic_inc(&conf->mirrors[d].rdev->nr_pending);
 		atomic_inc(&r10_bio->remaining);
@@ -2124,17 +2117,14 @@ static void sync_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 	 * that are active
 	 */
 	for (i = 0; i < conf->copies; i++) {
-		int j, d;
+		int d;
 
 		tbio = r10_bio->devs[i].repl_bio;
 		if (!tbio || !tbio->bi_end_io)
 			continue;
 		if (r10_bio->devs[i].bio->bi_end_io != end_sync_write
 		    && r10_bio->devs[i].bio != fbio)
-			for (j = 0; j < vcnt; j++)
-				memcpy(page_address(tbio->bi_io_vec[j].bv_page),
-				       page_address(fbio->bi_io_vec[j].bv_page),
-				       PAGE_SIZE);
+			bio_copy_data(tbio, fbio);
 		d = r10_bio->devs[i].devnum;
 		atomic_inc(&r10_bio->remaining);
 		md_sync_acct(conf->mirrors[d].replacement->bdev,
@@ -4156,6 +4146,7 @@ static int raid10_start_reshape(struct mddev *mddev)
 
 	clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
 	clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
+	clear_bit(MD_RECOVERY_DONE, &mddev->recovery);
 	set_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
 	set_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
 
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 553d54b87052..59e44e99eef3 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -344,7 +344,8 @@ static void release_inactive_stripe_list(struct r5conf *conf,
 					 int hash)
 {
 	int size;
-	bool do_wakeup = false;
+	unsigned long do_wakeup = 0;
+	int i = 0;
 	unsigned long flags;
 
 	if (hash == NR_STRIPE_HASH_LOCKS) {
@@ -365,15 +366,21 @@ static void release_inactive_stripe_list(struct r5conf *conf,
 			    !list_empty(list))
 				atomic_dec(&conf->empty_inactive_list_nr);
 			list_splice_tail_init(list, conf->inactive_list + hash);
-			do_wakeup = true;
+			do_wakeup |= 1 << hash;
 			spin_unlock_irqrestore(conf->hash_locks + hash, flags);
 		}
 		size--;
 		hash--;
 	}
 
+	for (i = 0; i < NR_STRIPE_HASH_LOCKS; i++) {
+		if (do_wakeup & (1 << i))
+			wake_up(&conf->wait_for_stripe[i]);
+	}
+
 	if (do_wakeup) {
-		wake_up(&conf->wait_for_stripe);
+		if (atomic_read(&conf->active_stripes) == 0)
+			wake_up(&conf->wait_for_quiescent);
 		if (conf->retry_read_aligned)
 			md_wakeup_thread(conf->mddev->thread);
 	}
@@ -667,15 +674,15 @@ get_active_stripe(struct r5conf *conf, sector_t sector,
 	spin_lock_irq(conf->hash_locks + hash);
 
 	do {
-		wait_event_lock_irq(conf->wait_for_stripe,
+		wait_event_lock_irq(conf->wait_for_quiescent,
 				    conf->quiesce == 0 || noquiesce,
 				    *(conf->hash_locks + hash));
 		sh = __find_stripe(conf, sector, conf->generation - previous);
 		if (!sh) {
 			if (!test_bit(R5_INACTIVE_BLOCKED, &conf->cache_state)) {
 				sh = get_free_stripe(conf, hash);
-				if (!sh && llist_empty(&conf->released_stripes) &&
-				    !test_bit(R5_DID_ALLOC, &conf->cache_state))
+				if (!sh && !test_bit(R5_DID_ALLOC,
+						     &conf->cache_state))
 					set_bit(R5_ALLOC_MORE,
 						&conf->cache_state);
 			}
@@ -684,14 +691,15 @@ get_active_stripe(struct r5conf *conf, sector_t sector,
 			if (!sh) {
 				set_bit(R5_INACTIVE_BLOCKED,
 					&conf->cache_state);
-				wait_event_lock_irq(
-					conf->wait_for_stripe,
+				wait_event_exclusive_cmd(
+					conf->wait_for_stripe[hash],
 					!list_empty(conf->inactive_list + hash) &&
 					(atomic_read(&conf->active_stripes)
 					 < (conf->max_nr_stripes * 3 / 4)
 					 || !test_bit(R5_INACTIVE_BLOCKED,
 						      &conf->cache_state)),
-					*(conf->hash_locks + hash));
+					spin_unlock_irq(conf->hash_locks + hash),
+					spin_lock_irq(conf->hash_locks + hash));
 				clear_bit(R5_INACTIVE_BLOCKED,
 					  &conf->cache_state);
 			} else {
@@ -716,6 +724,9 @@ get_active_stripe(struct r5conf *conf, sector_t sector,
 		}
 	} while (sh == NULL);
 
+	if (!list_empty(conf->inactive_list + hash))
+		wake_up(&conf->wait_for_stripe[hash]);
+
 	spin_unlock_irq(conf->hash_locks + hash);
 	return sh;
 }
@@ -2177,7 +2188,7 @@ static int resize_stripes(struct r5conf *conf, int newsize)
 	cnt = 0;
 	list_for_each_entry(nsh, &newstripes, lru) {
 		lock_device_hash_lock(conf, hash);
-		wait_event_cmd(conf->wait_for_stripe,
+		wait_event_exclusive_cmd(conf->wait_for_stripe[hash],
 				    !list_empty(conf->inactive_list + hash),
 				    unlock_device_hash_lock(conf, hash),
 				    lock_device_hash_lock(conf, hash));
@@ -4760,7 +4771,7 @@ static void raid5_align_endio(struct bio *bi, int error)
 					 raid_bi, 0);
 		bio_endio(raid_bi, 0);
 		if (atomic_dec_and_test(&conf->active_aligned_reads))
-			wake_up(&conf->wait_for_stripe);
+			wake_up(&conf->wait_for_quiescent);
 		return;
 	}
 
@@ -4855,7 +4866,7 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 		align_bi->bi_iter.bi_sector += rdev->data_offset;
 
 		spin_lock_irq(&conf->device_lock);
-		wait_event_lock_irq(conf->wait_for_stripe,
+		wait_event_lock_irq(conf->wait_for_quiescent,
 				    conf->quiesce == 0,
 				    conf->device_lock);
 		atomic_inc(&conf->active_aligned_reads);
@@ -5699,7 +5710,7 @@ static int  retry_aligned_read(struct r5conf *conf, struct bio *raid_bio)
 		bio_endio(raid_bio, 0);
 	}
 	if (atomic_dec_and_test(&conf->active_aligned_reads))
-		wake_up(&conf->wait_for_stripe);
+		wake_up(&conf->wait_for_quiescent);
 	return handled;
 }
 
@@ -6433,7 +6444,10 @@ static struct r5conf *setup_conf(struct mddev *mddev)
 		goto abort;
 	spin_lock_init(&conf->device_lock);
 	seqcount_init(&conf->gen_lock);
-	init_waitqueue_head(&conf->wait_for_stripe);
+	init_waitqueue_head(&conf->wait_for_quiescent);
+	for (i = 0; i < NR_STRIPE_HASH_LOCKS; i++) {
+		init_waitqueue_head(&conf->wait_for_stripe[i]);
+	}
 	init_waitqueue_head(&conf->wait_for_overlap);
 	INIT_LIST_HEAD(&conf->handle_list);
 	INIT_LIST_HEAD(&conf->hold_list);
@@ -7354,6 +7368,7 @@ static int raid5_start_reshape(struct mddev *mddev)
 
 	clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
 	clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
+	clear_bit(MD_RECOVERY_DONE, &mddev->recovery);
 	set_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
 	set_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
 	mddev->sync_thread = md_register_thread(md_do_sync, mddev,
@@ -7465,7 +7480,7 @@ static void raid5_quiesce(struct mddev *mddev, int state)
 		 * active stripes can drain
 		 */
 		conf->quiesce = 2;
-		wait_event_cmd(conf->wait_for_stripe,
+		wait_event_cmd(conf->wait_for_quiescent,
 				    atomic_read(&conf->active_stripes) == 0 &&
 				    atomic_read(&conf->active_aligned_reads) == 0,
 				    unlock_all_device_hash_locks_irq(conf),
@@ -7479,7 +7494,7 @@ static void raid5_quiesce(struct mddev *mddev, int state)
 	case 0: /* re-enable writes */
 		lock_all_device_hash_locks_irq(conf);
 		conf->quiesce = 0;
-		wake_up(&conf->wait_for_stripe);
+		wake_up(&conf->wait_for_quiescent);
 		wake_up(&conf->wait_for_overlap);
 		unlock_all_device_hash_locks_irq(conf);
 		break;
diff --git a/drivers/md/raid5.h b/drivers/md/raid5.h
index 896d603ad0da..02c3bf8fbfe7 100644
--- a/drivers/md/raid5.h
+++ b/drivers/md/raid5.h
@@ -511,7 +511,8 @@ struct r5conf {
 	struct list_head	inactive_list[NR_STRIPE_HASH_LOCKS];
 	atomic_t		empty_inactive_list_nr;
 	struct llist_head	released_stripes;
-	wait_queue_head_t	wait_for_stripe;
+	wait_queue_head_t	wait_for_quiescent;
+	wait_queue_head_t	wait_for_stripe[NR_STRIPE_HASH_LOCKS];
 	wait_queue_head_t	wait_for_overlap;
 	unsigned long		cache_state;
 #define R5_INACTIVE_BLOCKED	1	/* release of inactive stripes blocked,
diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index 3ef0f90b128f..3ef3d6c6bbf8 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -95,8 +95,9 @@ config MEDIA_CONTROLLER
 	  This API is mostly used by camera interfaces in embedded platforms.
 
 config MEDIA_CONTROLLER_DVB
-	bool "Enable Media controller for DVB"
+	bool "Enable Media controller for DVB (EXPERIMENTAL)"
 	depends on MEDIA_CONTROLLER
+	depends on BROKEN
 	---help---
 	  Enable the media controller API support for DVB.
 
diff --git a/drivers/media/common/b2c2/Kconfig b/drivers/media/common/b2c2/Kconfig
index a8c6cdfaa2f5..e5936380b1e5 100644
--- a/drivers/media/common/b2c2/Kconfig
+++ b/drivers/media/common/b2c2/Kconfig
@@ -14,6 +14,7 @@ config DVB_B2C2_FLEXCOP
 	select DVB_S5H1420 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_TUNER_ITD1000 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_ISL6421 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_CX24120 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_CX24123 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_SIMPLE if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_TUNER_CX24113 if MEDIA_SUBDRV_AUTOSELECT
diff --git a/drivers/media/common/b2c2/flexcop-common.h b/drivers/media/common/b2c2/flexcop-common.h
index 437912e49824..2b2460e9e6b4 100644
--- a/drivers/media/common/b2c2/flexcop-common.h
+++ b/drivers/media/common/b2c2/flexcop-common.h
@@ -91,6 +91,7 @@ struct flexcop_device {
 	int feedcount;
 	int pid_filtering;
 	int fullts_streaming_state;
+	int skip_6_hw_pid_filter;
 
 	/* bus specific callbacks */
 	flexcop_ibi_value(*read_ibi_reg) (struct flexcop_device *,
diff --git a/drivers/media/common/b2c2/flexcop-fe-tuner.c b/drivers/media/common/b2c2/flexcop-fe-tuner.c
index 7e14e90d2922..9c59f4306883 100644
--- a/drivers/media/common/b2c2/flexcop-fe-tuner.c
+++ b/drivers/media/common/b2c2/flexcop-fe-tuner.c
@@ -12,6 +12,7 @@
 #include "cx24113.h"
 #include "cx24123.h"
 #include "isl6421.h"
+#include "cx24120.h"
 #include "mt352.h"
 #include "bcm3510.h"
 #include "nxt200x.h"
@@ -26,9 +27,20 @@
 #define FE_SUPPORTED(fe) (defined(CONFIG_DVB_##fe) || \
 	(defined(CONFIG_DVB_##fe##_MODULE) && defined(MODULE)))
 
+#if FE_SUPPORTED(BCM3510) || (FE_SUPPORTED(CX24120) && FE_SUPPORTED(ISL6421))
+static int flexcop_fe_request_firmware(struct dvb_frontend *fe,
+	const struct firmware **fw, char *name)
+{
+	struct flexcop_device *fc = fe->dvb->priv;
+
+	return request_firmware(fw, name, fc->dev);
+}
+#endif
+
 /* lnb control */
 #if FE_SUPPORTED(MT312) || FE_SUPPORTED(STV0299)
-static int flexcop_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int flexcop_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct flexcop_device *fc = fe->dvb->priv;
 	flexcop_ibi_value v;
@@ -67,7 +79,7 @@ static int flexcop_sleep(struct dvb_frontend* fe)
 
 /* SkyStar2 DVB-S rev 2.3 */
 #if FE_SUPPORTED(MT312) && FE_SUPPORTED(PLL)
-static int flexcop_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int flexcop_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 /* u16 wz_half_period_for_45_mhz[] = { 0x01ff, 0x0154, 0x00ff, 0x00cc }; */
 	struct flexcop_device *fc = fe->dvb->priv;
@@ -146,7 +158,7 @@ static int flexcop_diseqc_send_master_cmd(struct dvb_frontend *fe,
 }
 
 static int flexcop_diseqc_send_burst(struct dvb_frontend *fe,
-	fe_sec_mini_cmd_t minicmd)
+				     enum fe_sec_mini_cmd minicmd)
 {
 	return flexcop_send_diseqc_msg(fe, 0, NULL, minicmd);
 }
@@ -445,13 +457,6 @@ static int airstar_dvbt_attach(struct flexcop_device *fc,
 
 /* AirStar ATSC 1st generation */
 #if FE_SUPPORTED(BCM3510)
-static int flexcop_fe_request_firmware(struct dvb_frontend *fe,
-	const struct firmware **fw, char* name)
-{
-	struct flexcop_device *fc = fe->dvb->priv;
-	return request_firmware(fw, name, fc->dev);
-}
-
 static struct bcm3510_config air2pc_atsc_first_gen_config = {
 	.demod_address    = 0x0f,
 	.request_firmware = flexcop_fe_request_firmware,
@@ -619,6 +624,43 @@ fail:
 #define cablestar2_attach NULL
 #endif
 
+/* SkyStar S2 PCI DVB-S/S2 card based on Conexant cx24120/cx24118 */
+#if FE_SUPPORTED(CX24120) && FE_SUPPORTED(ISL6421)
+static const struct cx24120_config skystar2_rev3_3_cx24120_config = {
+	.i2c_addr = 0x55,
+	.xtal_khz = 10111,
+	.initial_mpeg_config = { 0xa1, 0x76, 0x07 },
+	.request_firmware = flexcop_fe_request_firmware,
+	.i2c_wr_max = 4,
+};
+
+static int skystarS2_rev33_attach(struct flexcop_device *fc,
+	struct i2c_adapter *i2c)
+{
+	fc->fe = dvb_attach(cx24120_attach,
+			    &skystar2_rev3_3_cx24120_config, i2c);
+	if (!fc->fe)
+		return 0;
+
+	fc->dev_type = FC_SKYS2_REV33;
+	fc->fc_i2c_adap[2].no_base_addr = 1;
+	if (!dvb_attach(isl6421_attach, fc->fe, &fc->fc_i2c_adap[2].i2c_adap,
+			0x08, 0, 0, false)) {
+		err("ISL6421 could NOT be attached!");
+		fc->fc_i2c_adap[2].no_base_addr = 0;
+		return 0;
+	}
+	info("ISL6421 successfully attached.");
+
+	if (fc->has_32_hw_pid_filter)
+		fc->skip_6_hw_pid_filter = 1;
+
+	return 1;
+}
+#else
+#define skystarS2_rev33_attach NULL
+#endif
+
 static struct {
 	flexcop_device_type_t type;
 	int (*attach)(struct flexcop_device *, struct i2c_adapter *);
@@ -632,6 +674,7 @@ static struct {
 	{ FC_AIR_ATSC1, airstar_atsc1_attach },
 	{ FC_CABLE, cablestar2_attach },
 	{ FC_SKY_REV23, skystar2_rev23_attach },
+	{ FC_SKYS2_REV33, skystarS2_rev33_attach },
 };
 
 /* try to figure out the frontend */
diff --git a/drivers/media/common/b2c2/flexcop-hw-filter.c b/drivers/media/common/b2c2/flexcop-hw-filter.c
index 77e45475f4c7..8220257903ef 100644
--- a/drivers/media/common/b2c2/flexcop-hw-filter.c
+++ b/drivers/media/common/b2c2/flexcop-hw-filter.c
@@ -117,6 +117,10 @@ static void flexcop_pid_control(struct flexcop_device *fc,
 	deb_ts("setting pid: %5d %04x at index %d '%s'\n",
 			pid, pid, index, onoff ? "on" : "off");
 
+	/* First 6 can be buggy - skip over them if option set */
+	if (fc->skip_6_hw_pid_filter)
+		index += 6;
+
 	/* We could use bit magic here to reduce source code size.
 	 * I decided against it, but to use the real register names */
 	switch (index) {
@@ -170,7 +174,10 @@ static int flexcop_toggle_fullts_streaming(struct flexcop_device *fc, int onoff)
 int flexcop_pid_feed_control(struct flexcop_device *fc,
 		struct dvb_demux_feed *dvbdmxfeed, int onoff)
 {
-	int max_pid_filter = 6 + fc->has_32_hw_pid_filter*32;
+	int max_pid_filter = 6;
+
+	max_pid_filter -= 6 * fc->skip_6_hw_pid_filter;
+	max_pid_filter += 32 * fc->has_32_hw_pid_filter;
 
 	fc->feedcount += onoff ? 1 : -1; /* the number of PIDs/Feed currently requested */
 	if (dvbdmxfeed->index >= max_pid_filter)
@@ -217,7 +224,12 @@ void flexcop_hw_filter_init(struct flexcop_device *fc)
 {
 	int i;
 	flexcop_ibi_value v;
-	for (i = 0; i < 6 + 32*fc->has_32_hw_pid_filter; i++)
+	int max_pid_filter = 6;
+
+	max_pid_filter -= 6 * fc->skip_6_hw_pid_filter;
+	max_pid_filter += 32 * fc->has_32_hw_pid_filter;
+
+	for (i = 0; i < max_pid_filter; i++)
 		flexcop_pid_control(fc, i, 0x1fff, 0);
 
 	flexcop_pid_group_filter(fc, 0, 0x1fe0);
diff --git a/drivers/media/common/b2c2/flexcop-misc.c b/drivers/media/common/b2c2/flexcop-misc.c
index f06f3a9070f5..b8eff235367d 100644
--- a/drivers/media/common/b2c2/flexcop-misc.c
+++ b/drivers/media/common/b2c2/flexcop-misc.c
@@ -56,6 +56,7 @@ static const char *flexcop_device_names[] = {
 	[FC_SKY_REV26]	= "Sky2PC/SkyStar 2 DVB-S rev 2.6",
 	[FC_SKY_REV27]	= "Sky2PC/SkyStar 2 DVB-S rev 2.7a/u",
 	[FC_SKY_REV28]	= "Sky2PC/SkyStar 2 DVB-S rev 2.8",
+	[FC_SKYS2_REV33] = "Sky2PC/SkyStar S2 DVB-S/S2 rev 3.3",
 };
 
 static const char *flexcop_bus_names[] = {
diff --git a/drivers/media/common/b2c2/flexcop-reg.h b/drivers/media/common/b2c2/flexcop-reg.h
index dc4528dcbb98..835c54d60e74 100644
--- a/drivers/media/common/b2c2/flexcop-reg.h
+++ b/drivers/media/common/b2c2/flexcop-reg.h
@@ -24,6 +24,7 @@ typedef enum {
 	FC_SKY_REV26,
 	FC_SKY_REV27,
 	FC_SKY_REV28,
+	FC_SKYS2_REV33,
 } flexcop_device_type_t;
 
 typedef enum {
diff --git a/drivers/media/common/siano/smscoreapi.h b/drivers/media/common/siano/smscoreapi.h
index eb8bd689b936..4cc39e4a8318 100644
--- a/drivers/media/common/siano/smscoreapi.h
+++ b/drivers/media/common/siano/smscoreapi.h
@@ -1010,6 +1010,7 @@ struct sms_rx_stats_ex {
 	s32 mrc_in_band_pwr;	/* In band power in dBM */
 };
 
+#define	SRVM_MAX_PID_FILTERS 8
 
 /* statistics information returned as response for
  * SmsHostApiGetstatisticsEx_Req for DVB applications, SMS1100 and up */
@@ -1021,7 +1022,6 @@ struct sms_stats_dvb {
 	struct sms_tx_stats transmission_data;
 
 	/* Burst parameters, valid only for DVB-H */
-#define	SRVM_MAX_PID_FILTERS 8
 	struct sms_pid_data pid_data[SRVM_MAX_PID_FILTERS];
 };
 
@@ -1035,7 +1035,6 @@ struct sms_stats_dvb_ex {
 	struct sms_tx_stats transmission_data;
 
 	/* Burst parameters, valid only for DVB-H */
-#define	SRVM_MAX_PID_FILTERS 8
 	struct sms_pid_data pid_data[SRVM_MAX_PID_FILTERS];
 };
 
diff --git a/drivers/media/common/siano/smsdvb-main.c b/drivers/media/common/siano/smsdvb-main.c
index 367b8e77feb8..f4305ae800f4 100644
--- a/drivers/media/common/siano/smsdvb-main.c
+++ b/drivers/media/common/siano/smsdvb-main.c
@@ -753,7 +753,7 @@ static inline int led_feedback(struct smsdvb_client_t *client)
 				     SMS_LED_HI : SMS_LED_LO);
 }
 
-static int smsdvb_read_status(struct dvb_frontend *fe, fe_status_t *stat)
+static int smsdvb_read_status(struct dvb_frontend *fe, enum fe_status *stat)
 {
 	int rc;
 	struct smsdvb_client_t *client;
@@ -900,7 +900,7 @@ static int smsdvb_dvbt_set_frontend(struct dvb_frontend *fe)
 	/* Disable LNA, if any. An error is returned if no LNA is present */
 	ret = sms_board_lna_control(client->coredev, 0);
 	if (ret == 0) {
-		fe_status_t status;
+		enum fe_status status;
 
 		/* tune with LNA off at first */
 		ret = smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
@@ -971,7 +971,7 @@ static int smsdvb_isdbt_set_frontend(struct dvb_frontend *fe)
 	/* Disable LNA, if any. An error is returned if no LNA is present */
 	ret = sms_board_lna_control(client->coredev, 0);
 	if (ret == 0) {
-		fe_status_t status;
+		enum fe_status status;
 
 		/* tune with LNA off at first */
 		ret = smsdvb_sendrequest_and_wait(client, &msg, sizeof(msg),
diff --git a/drivers/media/common/siano/smsdvb.h b/drivers/media/common/siano/smsdvb.h
index ae36d0ae0fb1..b15754d95ec0 100644
--- a/drivers/media/common/siano/smsdvb.h
+++ b/drivers/media/common/siano/smsdvb.h
@@ -40,7 +40,7 @@ struct smsdvb_client_t {
 	struct dmxdev           dmxdev;
 	struct dvb_frontend     frontend;
 
-	fe_status_t             fe_status;
+	enum fe_status          fe_status;
 
 	struct completion       tune_done;
 	struct completion       stats_done;
diff --git a/drivers/media/common/siano/smsir.c b/drivers/media/common/siano/smsir.c
index 1d60d200d9ab..41f2a3939979 100644
--- a/drivers/media/common/siano/smsir.c
+++ b/drivers/media/common/siano/smsir.c
@@ -78,7 +78,7 @@ int sms_ir_init(struct smscore_device_t *coredev)
 	dev->dev.parent = coredev->device;
 
 #if 0
-	/* TODO: properly initialize the parameters bellow */
+	/* TODO: properly initialize the parameters below */
 	dev->input_id.bustype = BUS_USB;
 	dev->input_id.version = 1;
 	dev->input_id.vendor = le16_to_cpu(dev->udev->descriptor.idVendor);
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index 882ca417f328..842b9c8f80c6 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -40,6 +40,7 @@
 #include <linux/freezer.h>
 #include <linux/jiffies.h>
 #include <linux/kthread.h>
+#include <linux/ktime.h>
 #include <asm/processor.h>
 
 #include "dvb_frontend.h"
@@ -110,7 +111,7 @@ struct dvb_frontend_private {
 	struct task_struct *thread;
 	unsigned long release_jiffies;
 	unsigned int wakeup;
-	fe_status_t status;
+	enum fe_status status;
 	unsigned long tune_mode_flags;
 	unsigned int delay;
 	unsigned int reinitialise;
@@ -198,7 +199,8 @@ static enum dvbv3_emulation_type dvbv3_type(u32 delivery_system)
 	}
 }
 
-static void dvb_frontend_add_event(struct dvb_frontend *fe, fe_status_t status)
+static void dvb_frontend_add_event(struct dvb_frontend *fe,
+				   enum fe_status status)
 {
 	struct dvb_frontend_private *fepriv = fe->frontend_priv;
 	struct dvb_fe_events *events = &fepriv->events;
@@ -429,7 +431,7 @@ static int dvb_frontend_swzigzag_autotune(struct dvb_frontend *fe, int check_wra
 
 static void dvb_frontend_swzigzag(struct dvb_frontend *fe)
 {
-	fe_status_t s = 0;
+	enum fe_status s = 0;
 	int retval = 0;
 	struct dvb_frontend_private *fepriv = fe->frontend_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache, tmp;
@@ -690,7 +692,7 @@ static int dvb_frontend_thread(void *data)
 {
 	struct dvb_frontend *fe = data;
 	struct dvb_frontend_private *fepriv = fe->frontend_priv;
-	fe_status_t s;
+	enum fe_status s;
 	enum dvbfe_algo algo;
 #ifdef CONFIG_MEDIA_CONTROLLER_DVB
 	int ret;
@@ -889,42 +891,21 @@ static void dvb_frontend_stop(struct dvb_frontend *fe)
 				fepriv->thread);
 }
 
-s32 timeval_usec_diff(struct timeval lasttime, struct timeval curtime)
-{
-	return ((curtime.tv_usec < lasttime.tv_usec) ?
-		1000000 - lasttime.tv_usec + curtime.tv_usec :
-		curtime.tv_usec - lasttime.tv_usec);
-}
-EXPORT_SYMBOL(timeval_usec_diff);
-
-static inline void timeval_usec_add(struct timeval *curtime, u32 add_usec)
-{
-	curtime->tv_usec += add_usec;
-	if (curtime->tv_usec >= 1000000) {
-		curtime->tv_usec -= 1000000;
-		curtime->tv_sec++;
-	}
-}
-
 /*
  * Sleep until gettimeofday() > waketime + add_usec
  * This needs to be as precise as possible, but as the delay is
  * usually between 2ms and 32ms, it is done using a scheduled msleep
  * followed by usleep (normally a busy-wait loop) for the remainder
  */
-void dvb_frontend_sleep_until(struct timeval *waketime, u32 add_usec)
+void dvb_frontend_sleep_until(ktime_t *waketime, u32 add_usec)
 {
-	struct timeval lasttime;
 	s32 delta, newdelta;
 
-	timeval_usec_add(waketime, add_usec);
-
-	do_gettimeofday(&lasttime);
-	delta = timeval_usec_diff(lasttime, *waketime);
+	ktime_add_us(*waketime, add_usec);
+	delta = ktime_us_delta(ktime_get_real(), *waketime);
 	if (delta > 2500) {
 		msleep((delta - 1500) / 1000);
-		do_gettimeofday(&lasttime);
-		newdelta = timeval_usec_diff(lasttime, *waketime);
+		newdelta = ktime_us_delta(ktime_get_real(), *waketime);
 		delta = (newdelta > delta) ? 0 : newdelta;
 	}
 	if (delta > 0)
@@ -2216,7 +2197,7 @@ static int dtv_set_frontend(struct dvb_frontend *fe)
 		break;
 	}
 	if (rolloff)
-		c->bandwidth_hz = (c->symbol_rate * rolloff) / 100;
+		c->bandwidth_hz = mult_frac(c->symbol_rate, rolloff, 100);
 
 	/* force auto frequency inversion if requested */
 	if (dvb_force_auto_inversion)
@@ -2341,7 +2322,7 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 	}
 
 	case FE_READ_STATUS: {
-		fe_status_t* status = parg;
+		enum fe_status *status = parg;
 
 		/* if retune was requested but hasn't occurred yet, prevent
 		 * that user get signal state from previous tuning */
@@ -2403,7 +2384,13 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 
 	case FE_DISEQC_SEND_MASTER_CMD:
 		if (fe->ops.diseqc_send_master_cmd) {
-			err = fe->ops.diseqc_send_master_cmd(fe, (struct dvb_diseqc_master_cmd*) parg);
+			struct dvb_diseqc_master_cmd *cmd = parg;
+
+			if (cmd->msg_len > sizeof(cmd->msg)) {
+				err = -EINVAL;
+				break;
+			}
+			err = fe->ops.diseqc_send_master_cmd(fe, cmd);
 			fepriv->state = FESTATE_DISEQC;
 			fepriv->status = 0;
 		}
@@ -2411,7 +2398,8 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 
 	case FE_DISEQC_SEND_BURST:
 		if (fe->ops.diseqc_send_burst) {
-			err = fe->ops.diseqc_send_burst(fe, (fe_sec_mini_cmd_t) parg);
+			err = fe->ops.diseqc_send_burst(fe,
+						(enum fe_sec_mini_cmd)parg);
 			fepriv->state = FESTATE_DISEQC;
 			fepriv->status = 0;
 		}
@@ -2419,8 +2407,9 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 
 	case FE_SET_TONE:
 		if (fe->ops.set_tone) {
-			err = fe->ops.set_tone(fe, (fe_sec_tone_mode_t) parg);
-			fepriv->tone = (fe_sec_tone_mode_t) parg;
+			err = fe->ops.set_tone(fe,
+					       (enum fe_sec_tone_mode)parg);
+			fepriv->tone = (enum fe_sec_tone_mode)parg;
 			fepriv->state = FESTATE_DISEQC;
 			fepriv->status = 0;
 		}
@@ -2428,8 +2417,9 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 
 	case FE_SET_VOLTAGE:
 		if (fe->ops.set_voltage) {
-			err = fe->ops.set_voltage(fe, (fe_sec_voltage_t) parg);
-			fepriv->voltage = (fe_sec_voltage_t) parg;
+			err = fe->ops.set_voltage(fe,
+						  (enum fe_sec_voltage)parg);
+			fepriv->voltage = (enum fe_sec_voltage)parg;
 			fepriv->state = FESTATE_DISEQC;
 			fepriv->status = 0;
 		}
@@ -2437,7 +2427,8 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 
 	case FE_DISHNETWORK_SEND_LEGACY_CMD:
 		if (fe->ops.dishnetwork_send_legacy_command) {
-			err = fe->ops.dishnetwork_send_legacy_command(fe, (unsigned long) parg);
+			err = fe->ops.dishnetwork_send_legacy_command(fe,
+							 (unsigned long)parg);
 			fepriv->state = FESTATE_DISEQC;
 			fepriv->status = 0;
 		} else if (fe->ops.set_voltage) {
@@ -2458,13 +2449,13 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 			 * include the initialization or start bit
 			 */
 			unsigned long swcmd = ((unsigned long) parg) << 1;
-			struct timeval nexttime;
-			struct timeval tv[10];
+			ktime_t nexttime;
+			ktime_t tv[10];
 			int i;
 			u8 last = 1;
 			if (dvb_frontend_debug)
 				printk("%s switch command: 0x%04lx\n", __func__, swcmd);
-			do_gettimeofday(&nexttime);
+			nexttime = ktime_get_real();
 			if (dvb_frontend_debug)
 				tv[0] = nexttime;
 			/* before sending a command, initialize by sending
@@ -2475,7 +2466,7 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 
 			for (i = 0; i < 9; i++) {
 				if (dvb_frontend_debug)
-					do_gettimeofday(&tv[i + 1]);
+					tv[i+1] = ktime_get_real();
 				if ((swcmd & 0x01) != last) {
 					/* set voltage to (last ? 13V : 18V) */
 					fe->ops.set_voltage(fe, (last) ? SEC_VOLTAGE_13 : SEC_VOLTAGE_18);
@@ -2489,7 +2480,8 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 				printk("%s(%d): switch delay (should be 32k followed by all 8k\n",
 					__func__, fe->dvb->num);
 				for (i = 1; i < 10; i++)
-					printk("%d: %d\n", i, timeval_usec_diff(tv[i-1] , tv[i]));
+					printk("%d: %d\n", i,
+					(int) ktime_us_delta(tv[i], tv[i-1]));
 			}
 			err = 0;
 			fepriv->state = FESTATE_DISEQC;
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index 816269e5f706..4816947294fe 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -279,7 +279,7 @@ struct dvb_frontend_ops {
 		    bool re_tune,
 		    unsigned int mode_flags,
 		    unsigned int *delay,
-		    fe_status_t *status);
+		    enum fe_status *status);
 	/* get frontend tuning algorithm from the module */
 	enum dvbfe_algo (*get_frontend_algo)(struct dvb_frontend *fe);
 
@@ -289,7 +289,7 @@ struct dvb_frontend_ops {
 
 	int (*get_frontend)(struct dvb_frontend *fe);
 
-	int (*read_status)(struct dvb_frontend* fe, fe_status_t* status);
+	int (*read_status)(struct dvb_frontend *fe, enum fe_status *status);
 	int (*read_ber)(struct dvb_frontend* fe, u32* ber);
 	int (*read_signal_strength)(struct dvb_frontend* fe, u16* strength);
 	int (*read_snr)(struct dvb_frontend* fe, u16* snr);
@@ -298,9 +298,11 @@ struct dvb_frontend_ops {
 	int (*diseqc_reset_overload)(struct dvb_frontend* fe);
 	int (*diseqc_send_master_cmd)(struct dvb_frontend* fe, struct dvb_diseqc_master_cmd* cmd);
 	int (*diseqc_recv_slave_reply)(struct dvb_frontend* fe, struct dvb_diseqc_slave_reply* reply);
-	int (*diseqc_send_burst)(struct dvb_frontend* fe, fe_sec_mini_cmd_t minicmd);
-	int (*set_tone)(struct dvb_frontend* fe, fe_sec_tone_mode_t tone);
-	int (*set_voltage)(struct dvb_frontend* fe, fe_sec_voltage_t voltage);
+	int (*diseqc_send_burst)(struct dvb_frontend *fe,
+				 enum fe_sec_mini_cmd minicmd);
+	int (*set_tone)(struct dvb_frontend *fe, enum fe_sec_tone_mode tone);
+	int (*set_voltage)(struct dvb_frontend *fe,
+			   enum fe_sec_voltage voltage);
 	int (*enable_high_lnb_voltage)(struct dvb_frontend* fe, long arg);
 	int (*dishnetwork_send_legacy_command)(struct dvb_frontend* fe, unsigned long cmd);
 	int (*i2c_gate_ctrl)(struct dvb_frontend* fe, int enable);
@@ -338,24 +340,24 @@ struct dtv_frontend_properties {
 	u32			state;
 
 	u32			frequency;
-	fe_modulation_t		modulation;
+	enum fe_modulation		modulation;
 
-	fe_sec_voltage_t	voltage;
-	fe_sec_tone_mode_t	sectone;
-	fe_spectral_inversion_t	inversion;
-	fe_code_rate_t		fec_inner;
-	fe_transmit_mode_t	transmission_mode;
+	enum fe_sec_voltage	voltage;
+	enum fe_sec_tone_mode	sectone;
+	enum fe_spectral_inversion	inversion;
+	enum fe_code_rate		fec_inner;
+	enum fe_transmit_mode	transmission_mode;
 	u32			bandwidth_hz;	/* 0 = AUTO */
-	fe_guard_interval_t	guard_interval;
-	fe_hierarchy_t		hierarchy;
+	enum fe_guard_interval	guard_interval;
+	enum fe_hierarchy		hierarchy;
 	u32			symbol_rate;
-	fe_code_rate_t		code_rate_HP;
-	fe_code_rate_t		code_rate_LP;
+	enum fe_code_rate		code_rate_HP;
+	enum fe_code_rate		code_rate_LP;
 
-	fe_pilot_t		pilot;
-	fe_rolloff_t		rolloff;
+	enum fe_pilot		pilot;
+	enum fe_rolloff		rolloff;
 
-	fe_delivery_system_t	delivery_system;
+	enum fe_delivery_system	delivery_system;
 
 	enum fe_interleaving	interleaving;
 
@@ -368,8 +370,8 @@ struct dtv_frontend_properties {
 	u8			isdbt_layer_enabled;
 	struct {
 	    u8			segment_count;
-	    fe_code_rate_t	fec;
-	    fe_modulation_t	modulation;
+	    enum fe_code_rate	fec;
+	    enum fe_modulation	modulation;
 	    u8			interleaving;
 	} layer[3];
 
@@ -439,7 +441,6 @@ extern void dvb_frontend_reinitialise(struct dvb_frontend *fe);
 extern int dvb_frontend_suspend(struct dvb_frontend *fe);
 extern int dvb_frontend_resume(struct dvb_frontend *fe);
 
-extern void dvb_frontend_sleep_until(struct timeval *waketime, u32 add_usec);
-extern s32 timeval_usec_diff(struct timeval lasttime, struct timeval curtime);
+extern void dvb_frontend_sleep_until(ktime_t *waketime, u32 add_usec);
 
 #endif
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index 97c151d5b2e1..0d35f5850ff1 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -36,8 +36,9 @@ config DVB_STV6110x
 	  A Silicon tuner that supports DVB-S and DVB-S2 modes
 
 config DVB_M88DS3103
-	tristate "Montage M88DS3103"
+	tristate "Montage Technology M88DS3103"
 	depends on DVB_CORE && I2C && I2C_MUX
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Say Y when you want to support this frontend.
@@ -223,6 +224,13 @@ config DVB_CX24117
 	help
 	  A Dual DVB-S/S2 tuner module. Say Y when you want to support this frontend.
 
+config DVB_CX24120
+	tristate "Conexant CX24120 based"
+	depends on DVB_CORE && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  A DVB-S/S2 tuner module. Say Y when you want to support this frontend.
+
 config DVB_SI21XX
 	tristate "Silicon Labs SI21XX based"
 	depends on DVB_CORE && I2C
@@ -232,7 +240,8 @@ config DVB_SI21XX
 
 config DVB_TS2020
 	tristate "Montage Tehnology TS2020 based tuners"
-	depends on DVB_CORE && I2C
+	depends on DVB_CORE
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  A DVB-S/S2 silicon tuner. Say Y when you want to support this tuner.
diff --git a/drivers/media/dvb-frontends/Makefile b/drivers/media/dvb-frontends/Makefile
index 23d399bec804..ebab1b83e1fc 100644
--- a/drivers/media/dvb-frontends/Makefile
+++ b/drivers/media/dvb-frontends/Makefile
@@ -83,6 +83,7 @@ obj-$(CONFIG_DVB_DUMMY_FE) += dvb_dummy_fe.o
 obj-$(CONFIG_DVB_AF9013) += af9013.o
 obj-$(CONFIG_DVB_CX24116) += cx24116.o
 obj-$(CONFIG_DVB_CX24117) += cx24117.o
+obj-$(CONFIG_DVB_CX24120) += cx24120.o
 obj-$(CONFIG_DVB_SI21XX) += si21xx.o
 obj-$(CONFIG_DVB_SI2168) += si2168.o
 obj-$(CONFIG_DVB_STV0288) += stv0288.o
diff --git a/drivers/media/dvb-frontends/a8293.c b/drivers/media/dvb-frontends/a8293.c
index 780da58132f1..97ecbe01034c 100644
--- a/drivers/media/dvb-frontends/a8293.c
+++ b/drivers/media/dvb-frontends/a8293.c
@@ -22,8 +22,9 @@
 #include "a8293.h"
 
 struct a8293_priv {
+	u8 i2c_addr;
 	struct i2c_adapter *i2c;
-	const struct a8293_config *cfg;
+	struct i2c_client *client;
 	u8 reg[2];
 };
 
@@ -32,7 +33,7 @@ static int a8293_i2c(struct a8293_priv *priv, u8 *val, int len, bool rd)
 	int ret;
 	struct i2c_msg msg[1] = {
 		{
-			.addr = priv->cfg->i2c_addr,
+			.addr = priv->i2c_addr,
 			.len = len,
 			.buf = val,
 		}
@@ -66,7 +67,7 @@ static int a8293_rd(struct a8293_priv *priv, u8 *val, int len)
 }
 
 static int a8293_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t fe_sec_voltage)
+	enum fe_sec_voltage fe_sec_voltage)
 {
 	struct a8293_priv *priv = fe->sec_priv;
 	int ret;
@@ -128,7 +129,7 @@ struct dvb_frontend *a8293_attach(struct dvb_frontend *fe,
 
 	/* setup the priv */
 	priv->i2c = i2c;
-	priv->cfg = cfg;
+	priv->i2c_addr = cfg->i2c_addr;
 	fe->sec_priv = priv;
 
 	/* check if the SEC is there */
@@ -164,6 +165,86 @@ err:
 }
 EXPORT_SYMBOL(a8293_attach);
 
+static int a8293_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct a8293_priv *dev;
+	struct a8293_platform_data *pdata = client->dev.platform_data;
+	struct dvb_frontend *fe = pdata->dvb_frontend;
+	int ret;
+	u8 buf[2];
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	dev->client = client;
+	dev->i2c = client->adapter;
+	dev->i2c_addr = client->addr;
+
+	/* check if the SEC is there */
+	ret = a8293_rd(dev, buf, 2);
+	if (ret)
+		goto err_kfree;
+
+	/* ENB=0 */
+	dev->reg[0] = 0x10;
+	ret = a8293_wr(dev, &dev->reg[0], 1);
+	if (ret)
+		goto err_kfree;
+
+	/* TMODE=0, TGATE=1 */
+	dev->reg[1] = 0x82;
+	ret = a8293_wr(dev, &dev->reg[1], 1);
+	if (ret)
+		goto err_kfree;
+
+	/* override frontend ops */
+	fe->ops.set_voltage = a8293_set_voltage;
+
+	fe->sec_priv = dev;
+	i2c_set_clientdata(client, dev);
+
+	dev_info(&client->dev, "Allegro A8293 SEC successfully attached\n");
+	return 0;
+err_kfree:
+	kfree(dev);
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
+}
+
+static int a8293_remove(struct i2c_client *client)
+{
+	struct a8293_dev *dev = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	kfree(dev);
+	return 0;
+}
+
+static const struct i2c_device_id a8293_id_table[] = {
+	{"a8293", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, a8293_id_table);
+
+static struct i2c_driver a8293_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "a8293",
+		.suppress_bind_attrs = true,
+	},
+	.probe		= a8293_probe,
+	.remove		= a8293_remove,
+	.id_table	= a8293_id_table,
+};
+
+module_i2c_driver(a8293_driver);
+
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("Allegro A8293 SEC driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb-frontends/a8293.h b/drivers/media/dvb-frontends/a8293.h
index 5f0411939ffc..aff36538f582 100644
--- a/drivers/media/dvb-frontends/a8293.h
+++ b/drivers/media/dvb-frontends/a8293.h
@@ -21,8 +21,23 @@
 #ifndef A8293_H
 #define A8293_H
 
+#include "dvb_frontend.h"
 #include <linux/kconfig.h>
 
+/*
+ * I2C address
+ * 0x08, 0x09, 0x0a, 0x0b
+ */
+
+/**
+ * struct a8293_platform_data - Platform data for the a8293 driver
+ * @dvb_frontend: DVB frontend.
+ */
+struct a8293_platform_data {
+	struct dvb_frontend *dvb_frontend;
+};
+
+
 struct a8293_config {
 	u8 i2c_addr;
 };
diff --git a/drivers/media/dvb-frontends/af9013.c b/drivers/media/dvb-frontends/af9013.c
index 8001690d7576..e23197da84af 100644
--- a/drivers/media/dvb-frontends/af9013.c
+++ b/drivers/media/dvb-frontends/af9013.c
@@ -39,7 +39,7 @@ struct af9013_state {
 	u32 ucblocks;
 	u16 snr;
 	u32 bandwidth_hz;
-	fe_status_t fe_status;
+	enum fe_status fe_status;
 	unsigned long set_frontend_jiffies;
 	unsigned long read_status_jiffies;
 	bool first_tune;
@@ -605,6 +605,10 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 			}
 		}
 
+		/* Return an error if can't find bandwidth or the right clock */
+		if (i == ARRAY_SIZE(coeff_lut))
+			return -EINVAL;
+
 		ret = af9013_wr_regs(state, 0xae00, coeff_lut[i].val,
 			sizeof(coeff_lut[i].val));
 	}
@@ -979,7 +983,7 @@ err:
 	return ret;
 }
 
-static int af9013_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int af9013_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct af9013_state *state = fe->demodulator_priv;
 	int ret;
diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index 82ce47bdf5dc..59018afaa95f 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -35,7 +35,7 @@ struct af9033_dev {
 	bool ts_mode_parallel;
 	bool ts_mode_serial;
 
-	fe_status_t fe_status;
+	enum fe_status fe_status;
 	u64 post_bit_error_prev; /* for old read_ber we return (curr - prev) */
 	u64 post_bit_error;
 	u64 post_bit_count;
@@ -818,7 +818,7 @@ err:
 	return ret;
 }
 
-static int af9033_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int af9033_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct af9033_dev *dev = fe->demodulator_priv;
 	int ret;
diff --git a/drivers/media/dvb-frontends/as102_fe.c b/drivers/media/dvb-frontends/as102_fe.c
index 493665899565..544c5f65d19a 100644
--- a/drivers/media/dvb-frontends/as102_fe.c
+++ b/drivers/media/dvb-frontends/as102_fe.c
@@ -32,7 +32,7 @@ struct as102_state {
 	uint32_t ber;
 };
 
-static uint8_t as102_fe_get_code_rate(fe_code_rate_t arg)
+static uint8_t as102_fe_get_code_rate(enum fe_code_rate arg)
 {
 	uint8_t c;
 
@@ -306,7 +306,7 @@ static int as102_fe_get_tune_settings(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int as102_fe_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int as102_fe_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	int ret = 0;
 	struct as102_state *state = fe->demodulator_priv;
diff --git a/drivers/media/dvb-frontends/atbm8830.c b/drivers/media/dvb-frontends/atbm8830.c
index 4e11dc4b1335..8fe552e293ed 100644
--- a/drivers/media/dvb-frontends/atbm8830.c
+++ b/drivers/media/dvb-frontends/atbm8830.c
@@ -335,7 +335,8 @@ static int atbm8830_get_tune_settings(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int atbm8830_read_status(struct dvb_frontend *fe, fe_status_t *fe_status)
+static int atbm8830_read_status(struct dvb_frontend *fe,
+				enum fe_status *fe_status)
 {
 	struct atbm_state *priv = fe->demodulator_priv;
 	u8 locked = 0;
diff --git a/drivers/media/dvb-frontends/au8522_dig.c b/drivers/media/dvb-frontends/au8522_dig.c
index 5d06c99b0e97..b744a3f8d467 100644
--- a/drivers/media/dvb-frontends/au8522_dig.c
+++ b/drivers/media/dvb-frontends/au8522_dig.c
@@ -552,7 +552,7 @@ static struct {
 };
 
 static int au8522_enable_modulation(struct dvb_frontend *fe,
-				    fe_modulation_t m)
+				    enum fe_modulation m)
 {
 	struct au8522_state *state = fe->demodulator_priv;
 	int i;
@@ -644,7 +644,7 @@ static int au8522_set_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int au8522_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int au8522_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct au8522_state *state = fe->demodulator_priv;
 	u8 reg;
diff --git a/drivers/media/dvb-frontends/au8522_priv.h b/drivers/media/dvb-frontends/au8522_priv.h
index b8aca1c84786..951b3847e6f6 100644
--- a/drivers/media/dvb-frontends/au8522_priv.h
+++ b/drivers/media/dvb-frontends/au8522_priv.h
@@ -55,7 +55,7 @@ struct au8522_state {
 	struct dvb_frontend frontend;
 
 	u32 current_frequency;
-	fe_modulation_t current_modulation;
+	enum fe_modulation current_modulation;
 
 	u32 fe_status;
 	unsigned int led_state;
diff --git a/drivers/media/dvb-frontends/bcm3510.c b/drivers/media/dvb-frontends/bcm3510.c
index 638c7aa0fb7e..d30275f27644 100644
--- a/drivers/media/dvb-frontends/bcm3510.c
+++ b/drivers/media/dvb-frontends/bcm3510.c
@@ -289,7 +289,7 @@ static int bcm3510_refresh_state(struct bcm3510_state *st)
 	return 0;
 }
 
-static int bcm3510_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int bcm3510_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct bcm3510_state* st = fe->demodulator_priv;
 	bcm3510_refresh_state(st);
@@ -685,7 +685,7 @@ static int bcm3510_reset(struct bcm3510_state *st)
 	if ((ret = bcm3510_writeB(st,0xa0,v)) < 0)
 		return ret;
 
-    t = jiffies + 3*HZ;
+	t = jiffies + 3*HZ;
 	while (time_before(jiffies, t)) {
 		msleep(10);
 		if ((ret = bcm3510_readB(st,0xa2,&v)) < 0)
@@ -708,7 +708,7 @@ static int bcm3510_clear_reset(struct bcm3510_state *st)
 	if ((ret = bcm3510_writeB(st,0xa0,v)) < 0)
 		return ret;
 
-    t = jiffies + 3*HZ;
+	t = jiffies + 3*HZ;
 	while (time_before(jiffies, t)) {
 		msleep(10);
 		if ((ret = bcm3510_readB(st,0xa2,&v)) < 0)
diff --git a/drivers/media/dvb-frontends/cx22700.c b/drivers/media/dvb-frontends/cx22700.c
index 86563260d0f2..fd033cca6e11 100644
--- a/drivers/media/dvb-frontends/cx22700.c
+++ b/drivers/media/dvb-frontends/cx22700.c
@@ -191,9 +191,10 @@ static int cx22700_set_tps(struct cx22700_state *state,
 static int cx22700_get_tps(struct cx22700_state *state,
 			   struct dtv_frontend_properties *p)
 {
-	static const fe_modulation_t qam_tab [3] = { QPSK, QAM_16, QAM_64 };
-	static const fe_code_rate_t fec_tab [5] = { FEC_1_2, FEC_2_3, FEC_3_4,
-						    FEC_5_6, FEC_7_8 };
+	static const enum fe_modulation qam_tab[3] = { QPSK, QAM_16, QAM_64 };
+	static const enum fe_code_rate fec_tab[5] = {
+		FEC_1_2, FEC_2_3, FEC_3_4, FEC_5_6, FEC_7_8
+	};
 	u8 val;
 
 	dprintk ("%s\n", __func__);
@@ -253,7 +254,7 @@ static int cx22700_init (struct dvb_frontend* fe)
 	return 0;
 }
 
-static int cx22700_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int cx22700_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cx22700_state* state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/cx22702.c b/drivers/media/dvb-frontends/cx22702.c
index edc8eafc5c09..d2d06dcd7683 100644
--- a/drivers/media/dvb-frontends/cx22702.c
+++ b/drivers/media/dvb-frontends/cx22702.c
@@ -452,7 +452,7 @@ static int cx22702_init(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int cx22702_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int cx22702_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cx22702_state *state = fe->demodulator_priv;
 	u8 reg0A;
diff --git a/drivers/media/dvb-frontends/cx24110.c b/drivers/media/dvb-frontends/cx24110.c
index 7b510f2ae20f..cb36475e322b 100644
--- a/drivers/media/dvb-frontends/cx24110.c
+++ b/drivers/media/dvb-frontends/cx24110.c
@@ -143,7 +143,8 @@ static int cx24110_readreg (struct cx24110_state* state, u8 reg)
 	return b1[0];
 }
 
-static int cx24110_set_inversion (struct cx24110_state* state, fe_spectral_inversion_t inversion)
+static int cx24110_set_inversion(struct cx24110_state *state,
+				 enum fe_spectral_inversion inversion)
 {
 /* fixme (low): error handling */
 
@@ -177,7 +178,7 @@ static int cx24110_set_inversion (struct cx24110_state* state, fe_spectral_inver
 	return 0;
 }
 
-static int cx24110_set_fec(struct cx24110_state* state, fe_code_rate_t fec)
+static int cx24110_set_fec(struct cx24110_state *state, enum fe_code_rate fec)
 {
 	static const int rate[FEC_AUTO] = {-1,    1,    2,    3,    5,    7, -1};
 	static const int g1[FEC_AUTO]   = {-1, 0x01, 0x02, 0x05, 0x15, 0x45, -1};
@@ -220,7 +221,7 @@ static int cx24110_set_fec(struct cx24110_state* state, fe_code_rate_t fec)
 	return 0;
 }
 
-static fe_code_rate_t cx24110_get_fec (struct cx24110_state* state)
+static enum fe_code_rate cx24110_get_fec(struct cx24110_state *state)
 {
 	int i;
 
@@ -365,7 +366,8 @@ static int cx24110_initfe(struct dvb_frontend* fe)
 	return 0;
 }
 
-static int cx24110_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int cx24110_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct cx24110_state *state = fe->demodulator_priv;
 
@@ -379,7 +381,8 @@ static int cx24110_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltag
 	}
 }
 
-static int cx24110_diseqc_send_burst(struct dvb_frontend* fe, fe_sec_mini_cmd_t burst)
+static int cx24110_diseqc_send_burst(struct dvb_frontend *fe,
+				     enum fe_sec_mini_cmd burst)
 {
 	int rv, bit;
 	struct cx24110_state *state = fe->demodulator_priv;
@@ -434,7 +437,8 @@ static int cx24110_send_diseqc_msg(struct dvb_frontend* fe,
 	return 0;
 }
 
-static int cx24110_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int cx24110_read_status(struct dvb_frontend *fe,
+			       enum fe_status *status)
 {
 	struct cx24110_state *state = fe->demodulator_priv;
 
@@ -574,7 +578,8 @@ static int cx24110_get_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int cx24110_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int cx24110_set_tone(struct dvb_frontend *fe,
+			    enum fe_sec_tone_mode tone)
 {
 	struct cx24110_state *state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/cx24116.c b/drivers/media/dvb-frontends/cx24116.c
index 2916d7c74a1d..8814f36d53fb 100644
--- a/drivers/media/dvb-frontends/cx24116.c
+++ b/drivers/media/dvb-frontends/cx24116.c
@@ -160,13 +160,13 @@ enum cmds {
 struct cx24116_tuning {
 	u32 frequency;
 	u32 symbol_rate;
-	fe_spectral_inversion_t inversion;
-	fe_code_rate_t fec;
+	enum fe_spectral_inversion inversion;
+	enum fe_code_rate fec;
 
-	fe_delivery_system_t delsys;
-	fe_modulation_t modulation;
-	fe_pilot_t pilot;
-	fe_rolloff_t rolloff;
+	enum fe_delivery_system delsys;
+	enum fe_modulation modulation;
+	enum fe_pilot pilot;
+	enum fe_rolloff rolloff;
 
 	/* Demod values */
 	u8 fec_val;
@@ -285,7 +285,7 @@ static int cx24116_readreg(struct cx24116_state *state, u8 reg)
 }
 
 static int cx24116_set_inversion(struct cx24116_state *state,
-	fe_spectral_inversion_t inversion)
+	enum fe_spectral_inversion inversion)
 {
 	dprintk("%s(%d)\n", __func__, inversion);
 
@@ -373,9 +373,9 @@ static int cx24116_set_inversion(struct cx24116_state *state,
  * a scheme are support. Especially, no auto detect when in S2 mode.
  */
 static struct cx24116_modfec {
-	fe_delivery_system_t delivery_system;
-	fe_modulation_t modulation;
-	fe_code_rate_t fec;
+	enum fe_delivery_system delivery_system;
+	enum fe_modulation modulation;
+	enum fe_code_rate fec;
 	u8 mask;	/* In DVBS mode this is used to autodetect */
 	u8 val;		/* Passed to the firmware to indicate mode selection */
 } CX24116_MODFEC_MODES[] = {
@@ -415,7 +415,7 @@ static struct cx24116_modfec {
 };
 
 static int cx24116_lookup_fecmod(struct cx24116_state *state,
-	fe_delivery_system_t d, fe_modulation_t m, fe_code_rate_t f)
+	enum fe_delivery_system d, enum fe_modulation m, enum fe_code_rate f)
 {
 	int i, ret = -EOPNOTSUPP;
 
@@ -434,7 +434,9 @@ static int cx24116_lookup_fecmod(struct cx24116_state *state,
 }
 
 static int cx24116_set_fec(struct cx24116_state *state,
-	fe_delivery_system_t delsys, fe_modulation_t mod, fe_code_rate_t fec)
+			   enum fe_delivery_system delsys,
+			   enum fe_modulation mod,
+			   enum fe_code_rate fec)
 {
 	int ret = 0;
 
@@ -683,7 +685,7 @@ static int cx24116_load_firmware(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int cx24116_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int cx24116_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cx24116_state *state = fe->demodulator_priv;
 
@@ -844,7 +846,7 @@ static int cx24116_wait_for_lnb(struct dvb_frontend *fe)
 }
 
 static int cx24116_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t voltage)
+	enum fe_sec_voltage voltage)
 {
 	struct cx24116_cmd cmd;
 	int ret;
@@ -872,7 +874,7 @@ static int cx24116_set_voltage(struct dvb_frontend *fe,
 }
 
 static int cx24116_set_tone(struct dvb_frontend *fe,
-	fe_sec_tone_mode_t tone)
+	enum fe_sec_tone_mode tone)
 {
 	struct cx24116_cmd cmd;
 	int ret;
@@ -963,6 +965,10 @@ static int cx24116_send_diseqc_msg(struct dvb_frontend *fe,
 	struct cx24116_state *state = fe->demodulator_priv;
 	int i, ret;
 
+	/* Validate length */
+	if (d->msg_len > sizeof(d->msg))
+                return -EINVAL;
+
 	/* Dump DiSEqC message */
 	if (debug) {
 		printk(KERN_INFO "cx24116: %s(", __func__);
@@ -974,10 +980,6 @@ static int cx24116_send_diseqc_msg(struct dvb_frontend *fe,
 		printk(") toneburst=%d\n", toneburst);
 	}
 
-	/* Validate length */
-	if (d->msg_len > (CX24116_ARGLEN - CX24116_DISEQC_MSGOFS))
-		return -EINVAL;
-
 	/* DiSEqC message */
 	for (i = 0; i < d->msg_len; i++)
 		state->dsec_cmd.args[CX24116_DISEQC_MSGOFS + i] = d->msg[i];
@@ -1055,7 +1057,7 @@ static int cx24116_send_diseqc_msg(struct dvb_frontend *fe,
 
 /* Send DiSEqC burst */
 static int cx24116_diseqc_send_burst(struct dvb_frontend *fe,
-	fe_sec_mini_cmd_t burst)
+	enum fe_sec_mini_cmd burst)
 {
 	struct cx24116_state *state = fe->demodulator_priv;
 	int ret;
@@ -1220,7 +1222,7 @@ static int cx24116_set_frontend(struct dvb_frontend *fe)
 	struct cx24116_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct cx24116_cmd cmd;
-	fe_status_t tunerstat;
+	enum fe_status tunerstat;
 	int i, status, ret, retune = 1;
 
 	dprintk("%s()\n", __func__);
@@ -1441,7 +1443,7 @@ tuned:  /* Set/Reset B/W */
 }
 
 static int cx24116_tune(struct dvb_frontend *fe, bool re_tune,
-	unsigned int mode_flags, unsigned int *delay, fe_status_t *status)
+	unsigned int mode_flags, unsigned int *delay, enum fe_status *status)
 {
 	/*
 	 * It is safe to discard "params" here, as the DVB core will sync
diff --git a/drivers/media/dvb-frontends/cx24117.c b/drivers/media/dvb-frontends/cx24117.c
index acb965ce0358..5f77bc80a896 100644
--- a/drivers/media/dvb-frontends/cx24117.c
+++ b/drivers/media/dvb-frontends/cx24117.c
@@ -171,13 +171,13 @@ static DEFINE_MUTEX(cx24117_list_mutex);
 struct cx24117_tuning {
 	u32 frequency;
 	u32 symbol_rate;
-	fe_spectral_inversion_t inversion;
-	fe_code_rate_t fec;
+	enum fe_spectral_inversion inversion;
+	enum fe_code_rate fec;
 
-	fe_delivery_system_t delsys;
-	fe_modulation_t modulation;
-	fe_pilot_t pilot;
-	fe_rolloff_t rolloff;
+	enum fe_delivery_system delsys;
+	enum fe_modulation modulation;
+	enum fe_pilot pilot;
+	enum fe_rolloff rolloff;
 
 	/* Demod values */
 	u8 fec_val;
@@ -220,9 +220,9 @@ struct cx24117_state {
 /* modfec (modulation and FEC) lookup table */
 /* Check cx24116.c for a detailed description of each field */
 static struct cx24117_modfec {
-	fe_delivery_system_t delivery_system;
-	fe_modulation_t modulation;
-	fe_code_rate_t fec;
+	enum fe_delivery_system delivery_system;
+	enum fe_modulation modulation;
+	enum fe_code_rate fec;
 	u8 mask;	/* In DVBS mode this is used to autodetect */
 	u8 val;		/* Passed to the firmware to indicate mode selection */
 } cx24117_modfec_modes[] = {
@@ -362,7 +362,7 @@ static int cx24117_readregN(struct cx24117_state *state,
 }
 
 static int cx24117_set_inversion(struct cx24117_state *state,
-	fe_spectral_inversion_t inversion)
+	enum fe_spectral_inversion inversion)
 {
 	dev_dbg(&state->priv->i2c->dev, "%s(%d) demod%d\n",
 		__func__, inversion, state->demod);
@@ -387,7 +387,7 @@ static int cx24117_set_inversion(struct cx24117_state *state,
 }
 
 static int cx24117_lookup_fecmod(struct cx24117_state *state,
-	fe_delivery_system_t d, fe_modulation_t m, fe_code_rate_t f)
+	enum fe_delivery_system d, enum fe_modulation m, enum fe_code_rate f)
 {
 	int i, ret = -EINVAL;
 
@@ -408,7 +408,9 @@ static int cx24117_lookup_fecmod(struct cx24117_state *state,
 }
 
 static int cx24117_set_fec(struct cx24117_state *state,
-	fe_delivery_system_t delsys, fe_modulation_t mod, fe_code_rate_t fec)
+			   enum fe_delivery_system delsys,
+			   enum fe_modulation mod,
+			   enum fe_code_rate fec)
 {
 	int ret;
 
@@ -737,7 +739,7 @@ error:
 	return ret;
 }
 
-static int cx24117_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int cx24117_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cx24117_state *state = fe->demodulator_priv;
 	int lock;
@@ -843,7 +845,7 @@ static int cx24117_read_snr(struct dvb_frontend *fe, u16 *snr)
 static int cx24117_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 {
 	struct cx24117_state *state = fe->demodulator_priv;
-	fe_delivery_system_t delsys = fe->dtv_property_cache.delivery_system;
+	enum fe_delivery_system delsys = fe->dtv_property_cache.delivery_system;
 	int ret;
 	u8 buf[2];
 	u8 reg = (state->demod == 0) ?
@@ -904,7 +906,7 @@ static int cx24117_wait_for_lnb(struct dvb_frontend *fe)
 }
 
 static int cx24117_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t voltage)
+			       enum fe_sec_voltage voltage)
 {
 	struct cx24117_state *state = fe->demodulator_priv;
 	struct cx24117_cmd cmd;
@@ -956,7 +958,7 @@ static int cx24117_set_voltage(struct dvb_frontend *fe,
 }
 
 static int cx24117_set_tone(struct dvb_frontend *fe,
-	fe_sec_tone_mode_t tone)
+			    enum fe_sec_tone_mode tone)
 {
 	struct cx24117_state *state = fe->demodulator_priv;
 	struct cx24117_cmd cmd;
@@ -1043,7 +1045,7 @@ static int cx24117_send_diseqc_msg(struct dvb_frontend *fe,
 	dev_dbg(&state->priv->i2c->dev, ")\n");
 
 	/* Validate length */
-	if (d->msg_len > 15)
+	if (d->msg_len > sizeof(d->msg))
 		return -EINVAL;
 
 	/* DiSEqC message */
@@ -1112,7 +1114,7 @@ static int cx24117_send_diseqc_msg(struct dvb_frontend *fe,
 
 /* Send DiSEqC burst */
 static int cx24117_diseqc_send_burst(struct dvb_frontend *fe,
-	fe_sec_mini_cmd_t burst)
+	enum fe_sec_mini_cmd burst)
 {
 	struct cx24117_state *state = fe->demodulator_priv;
 
@@ -1306,7 +1308,7 @@ static int cx24117_set_frontend(struct dvb_frontend *fe)
 	struct cx24117_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct cx24117_cmd cmd;
-	fe_status_t tunerstat;
+	enum fe_status tunerstat;
 	int i, status, ret, retune = 1;
 	u8 reg_clkdiv, reg_ratediv;
 
@@ -1537,7 +1539,7 @@ static int cx24117_set_frontend(struct dvb_frontend *fe)
 }
 
 static int cx24117_tune(struct dvb_frontend *fe, bool re_tune,
-	unsigned int mode_flags, unsigned int *delay, fe_status_t *status)
+	unsigned int mode_flags, unsigned int *delay, enum fe_status *status)
 {
 	struct cx24117_state *state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/cx24120.c b/drivers/media/dvb-frontends/cx24120.c
new file mode 100644
index 000000000000..3b0ef52bb834
--- /dev/null
+++ b/drivers/media/dvb-frontends/cx24120.c
@@ -0,0 +1,1595 @@
+/*
+    Conexant cx24120/cx24118 - DVBS/S2 Satellite demod/tuner driver
+
+    Copyright (C) 2008 Patrick Boettcher <pb@linuxtv.org>
+    Copyright (C) 2009 Sergey Tyurin <forum.free-x.de>
+    Updated 2012 by Jannis Achstetter <jannis_achstetter@web.de>
+    Copyright (C) 2015 Jemma Denson <jdenson@gmail.com>
+	April 2015
+	    Refactored & simplified driver
+	    Updated to work with delivery system supplied by DVBv5
+	    Add frequency, fec & pilot to get_frontend
+
+	Cards supported: Technisat Skystar S2
+
+    This program is free software; you can redistribute it and/or modify
+    it under 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/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/firmware.h>
+#include "dvb_frontend.h"
+#include "cx24120.h"
+
+#define CX24120_SEARCH_RANGE_KHZ 5000
+#define CX24120_FIRMWARE "dvb-fe-cx24120-1.20.58.2.fw"
+
+/* cx24120 i2c registers  */
+#define CX24120_REG_CMD_START	0x00		/* write cmd_id */
+#define CX24120_REG_CMD_ARGS	0x01		/* write command arguments */
+#define CX24120_REG_CMD_END	0x1f		/* write 0x01 for end */
+
+#define CX24120_REG_MAILBOX	0x33
+#define CX24120_REG_FREQ3	0x34		/* frequency */
+#define CX24120_REG_FREQ2	0x35
+#define CX24120_REG_FREQ1	0x36
+
+#define CX24120_REG_FECMODE	0x39		/* FEC status */
+#define CX24120_REG_STATUS	0x3a		/* Tuner status */
+#define CX24120_REG_SIGSTR_H	0x3a		/* Signal strength high */
+#define CX24120_REG_SIGSTR_L	0x3b		/* Signal strength low byte */
+#define CX24120_REG_QUALITY_H	0x40		/* SNR high byte */
+#define CX24120_REG_QUALITY_L	0x41		/* SNR low byte */
+
+#define CX24120_REG_BER_HH	0x47		/* BER high byte of high word */
+#define CX24120_REG_BER_HL	0x48		/* BER low byte of high word */
+#define CX24120_REG_BER_LH	0x49		/* BER high byte of low word */
+#define CX24120_REG_BER_LL	0x4a		/* BER low byte of low word */
+
+#define CX24120_REG_UCB_H	0x50		/* UCB high byte */
+#define CX24120_REG_UCB_L	0x51		/* UCB low byte  */
+
+#define CX24120_REG_CLKDIV	0xe6
+#define CX24120_REG_RATEDIV	0xf0
+
+#define CX24120_REG_REVISION	0xff		/* Chip revision (ro) */
+
+/* Command messages */
+enum command_message_id {
+	CMD_VCO_SET		= 0x10,		/* cmd.len = 12; */
+	CMD_TUNEREQUEST		= 0x11,		/* cmd.len = 15; */
+
+	CMD_MPEG_ONOFF		= 0x13,		/* cmd.len = 4; */
+	CMD_MPEG_INIT		= 0x14,		/* cmd.len = 7; */
+	CMD_BANDWIDTH		= 0x15,		/* cmd.len = 12; */
+	CMD_CLOCK_READ		= 0x16,		/* read clock */
+	CMD_CLOCK_SET		= 0x17,		/* cmd.len = 10; */
+
+	CMD_DISEQC_MSG1		= 0x20,		/* cmd.len = 11; */
+	CMD_DISEQC_MSG2		= 0x21,		/* cmd.len = d->msg_len + 6; */
+	CMD_SETVOLTAGE		= 0x22,		/* cmd.len = 2; */
+	CMD_SETTONE		= 0x23,		/* cmd.len = 4; */
+	CMD_DISEQC_BURST	= 0x24,		/* cmd.len not used !!! */
+
+	CMD_READ_SNR		= 0x1a,		/* Read signal strength */
+	CMD_START_TUNER		= 0x1b,		/* ??? */
+
+	CMD_FWVERSION		= 0x35,
+
+	CMD_BER_CTRL		= 0x3c,		/* cmd.len = 0x03; */
+};
+
+#define CX24120_MAX_CMD_LEN	30
+
+/* pilot mask */
+#define CX24120_PILOT_OFF	0x00
+#define CX24120_PILOT_ON	0x40
+#define CX24120_PILOT_AUTO	0x80
+
+/* signal status */
+#define CX24120_HAS_SIGNAL	0x01
+#define CX24120_HAS_CARRIER	0x02
+#define CX24120_HAS_VITERBI	0x04
+#define CX24120_HAS_LOCK	0x08
+#define CX24120_HAS_UNK1	0x10
+#define CX24120_HAS_UNK2	0x20
+#define CX24120_STATUS_MASK	0x0f
+#define CX24120_SIGNAL_MASK	0xc0
+
+/* ber window */
+#define CX24120_BER_WINDOW	16
+#define CX24120_BER_WSIZE	((1 << CX24120_BER_WINDOW) * 208 * 8)
+
+#define info(args...) pr_info("cx24120: " args)
+#define err(args...)  pr_err("cx24120: ### ERROR: " args)
+
+/* The Demod/Tuner can't easily provide these, we cache them */
+struct cx24120_tuning {
+	u32 frequency;
+	u32 symbol_rate;
+	enum fe_spectral_inversion inversion;
+	enum fe_code_rate fec;
+
+	enum fe_delivery_system delsys;
+	enum fe_modulation modulation;
+	enum fe_pilot pilot;
+
+	/* Demod values */
+	u8 fec_val;
+	u8 fec_mask;
+	u8 clkdiv;
+	u8 ratediv;
+	u8 inversion_val;
+	u8 pilot_val;
+};
+
+/* Private state */
+struct cx24120_state {
+	struct i2c_adapter *i2c;
+	const struct cx24120_config *config;
+	struct dvb_frontend frontend;
+
+	u8 cold_init;
+	u8 mpeg_enabled;
+	u8 need_clock_set;
+
+	/* current and next tuning parameters */
+	struct cx24120_tuning dcur;
+	struct cx24120_tuning dnxt;
+
+	enum fe_status fe_status;
+
+	/* dvbv5 stats calculations */
+	u32 bitrate;
+	u32 berw_usecs;
+	u32 ber_prev;
+	u32 ucb_offset;
+	unsigned long ber_jiffies_stats;
+	unsigned long per_jiffies_stats;
+};
+
+/* Command message to firmware */
+struct cx24120_cmd {
+	u8 id;
+	u8 len;
+	u8 arg[CX24120_MAX_CMD_LEN];
+};
+
+/* Read single register */
+static int cx24120_readreg(struct cx24120_state *state, u8 reg)
+{
+	int ret;
+	u8 buf = 0;
+	struct i2c_msg msg[] = {
+		{
+			.addr = state->config->i2c_addr,
+			.flags = 0,
+			.len = 1,
+			.buf = &reg
+		}, {
+			.addr = state->config->i2c_addr,
+			.flags = I2C_M_RD,
+			.len = 1,
+			.buf = &buf
+		}
+	};
+
+	ret = i2c_transfer(state->i2c, msg, 2);
+	if (ret != 2) {
+		err("Read error: reg=0x%02x, ret=%i)\n", reg, ret);
+		return ret;
+	}
+
+	dev_dbg(&state->i2c->dev, "reg=0x%02x; data=0x%02x\n", reg, buf);
+
+	return buf;
+}
+
+/* Write single register */
+static int cx24120_writereg(struct cx24120_state *state, u8 reg, u8 data)
+{
+	u8 buf[] = { reg, data };
+	struct i2c_msg msg = {
+		.addr = state->config->i2c_addr,
+		.flags = 0,
+		.buf = buf,
+		.len = 2
+	};
+	int ret;
+
+	ret = i2c_transfer(state->i2c, &msg, 1);
+	if (ret != 1) {
+		err("Write error: i2c_write error(err == %i, 0x%02x: 0x%02x)\n",
+		    ret, reg, data);
+		return ret;
+	}
+
+	dev_dbg(&state->i2c->dev, "reg=0x%02x; data=0x%02x\n", reg, data);
+
+	return 0;
+}
+
+/* Write multiple registers in chunks of i2c_wr_max-sized buffers */
+static int cx24120_writeregs(struct cx24120_state *state,
+			     u8 reg, const u8 *values, u16 len, u8 incr)
+{
+	int ret;
+	u16 max = state->config->i2c_wr_max > 0 ?
+				state->config->i2c_wr_max :
+				len;
+
+	struct i2c_msg msg = {
+		.addr = state->config->i2c_addr,
+		.flags = 0,
+	};
+
+	msg.buf = kmalloc(max + 1, GFP_KERNEL);
+	if (!msg.buf)
+		return -ENOMEM;
+
+	while (len) {
+		msg.buf[0] = reg;
+		msg.len = len > max ? max : len;
+		memcpy(&msg.buf[1], values, msg.len);
+
+		len    -= msg.len;      /* data length revers counter */
+		values += msg.len;      /* incr data pointer */
+
+		if (incr)
+			reg += msg.len;
+		msg.len++;              /* don't forget the addr byte */
+
+		ret = i2c_transfer(state->i2c, &msg, 1);
+		if (ret != 1) {
+			err("i2c_write error(err == %i, 0x%02x)\n", ret, reg);
+			goto out;
+		}
+
+		dev_dbg(&state->i2c->dev, "reg=0x%02x; data=%*ph\n",
+			reg, msg.len - 1, msg.buf + 1);
+	}
+
+	ret = 0;
+
+out:
+	kfree(msg.buf);
+	return ret;
+}
+
+static struct dvb_frontend_ops cx24120_ops;
+
+struct dvb_frontend *cx24120_attach(const struct cx24120_config *config,
+				    struct i2c_adapter *i2c)
+{
+	struct cx24120_state *state;
+	int demod_rev;
+
+	info("Conexant cx24120/cx24118 - DVBS/S2 Satellite demod/tuner\n");
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state) {
+		err("Unable to allocate memory for cx24120_state\n");
+		goto error;
+	}
+
+	/* setup the state */
+	state->config = config;
+	state->i2c = i2c;
+
+	/* check if the demod is present and has proper type */
+	demod_rev = cx24120_readreg(state, CX24120_REG_REVISION);
+	switch (demod_rev) {
+	case 0x07:
+		info("Demod cx24120 rev. 0x07 detected.\n");
+		break;
+	case 0x05:
+		info("Demod cx24120 rev. 0x05 detected.\n");
+		break;
+	default:
+		err("Unsupported demod revision: 0x%x detected.\n", demod_rev);
+		goto error;
+	}
+
+	/* create dvb_frontend */
+	state->cold_init = 0;
+	memcpy(&state->frontend.ops, &cx24120_ops,
+	       sizeof(struct dvb_frontend_ops));
+	state->frontend.demodulator_priv = state;
+
+	info("Conexant cx24120/cx24118 attached.\n");
+	return &state->frontend;
+
+error:
+	kfree(state);
+	return NULL;
+}
+EXPORT_SYMBOL(cx24120_attach);
+
+static int cx24120_test_rom(struct cx24120_state *state)
+{
+	int err, ret;
+
+	err = cx24120_readreg(state, 0xfd);
+	if (err & 4) {
+		ret = cx24120_readreg(state, 0xdf) & 0xfe;
+		err = cx24120_writereg(state, 0xdf, ret);
+	}
+	return err;
+}
+
+static int cx24120_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	if (c->cnr.stat[0].scale != FE_SCALE_DECIBEL)
+		*snr = 0;
+	else
+		*snr = div_s64(c->cnr.stat[0].svalue, 100);
+
+	return 0;
+}
+
+static int cx24120_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	if (c->post_bit_error.stat[0].scale != FE_SCALE_COUNTER) {
+		*ber = 0;
+		return 0;
+	}
+
+	*ber = c->post_bit_error.stat[0].uvalue - state->ber_prev;
+	state->ber_prev = c->post_bit_error.stat[0].uvalue;
+
+	return 0;
+}
+
+static int cx24120_msg_mpeg_output_global_config(struct cx24120_state *state,
+						 u8 flag);
+
+/* Check if we're running a command that needs to disable mpeg out */
+static void cx24120_check_cmd(struct cx24120_state *state, u8 id)
+{
+	switch (id) {
+	case CMD_TUNEREQUEST:
+	case CMD_CLOCK_READ:
+	case CMD_DISEQC_MSG1:
+	case CMD_DISEQC_MSG2:
+	case CMD_SETVOLTAGE:
+	case CMD_SETTONE:
+	case CMD_DISEQC_BURST:
+		cx24120_msg_mpeg_output_global_config(state, 0);
+		/* Old driver would do a msleep(100) here */
+	default:
+		return;
+	}
+}
+
+/* Send a message to the firmware */
+static int cx24120_message_send(struct cx24120_state *state,
+				struct cx24120_cmd *cmd)
+{
+	int ficus;
+
+	if (state->mpeg_enabled) {
+		/* Disable mpeg out on certain commands */
+		cx24120_check_cmd(state, cmd->id);
+	}
+
+	cx24120_writereg(state, CX24120_REG_CMD_START, cmd->id);
+	cx24120_writeregs(state, CX24120_REG_CMD_ARGS, &cmd->arg[0],
+			  cmd->len, 1);
+	cx24120_writereg(state, CX24120_REG_CMD_END, 0x01);
+
+	ficus = 1000;
+	while (cx24120_readreg(state, CX24120_REG_CMD_END)) {
+		msleep(20);
+		ficus -= 20;
+		if (ficus == 0) {
+			err("Error sending message to firmware\n");
+			return -EREMOTEIO;
+		}
+	}
+	dev_dbg(&state->i2c->dev, "sent message 0x%02x\n", cmd->id);
+
+	return 0;
+}
+
+/* Send a message and fill arg[] with the results */
+static int cx24120_message_sendrcv(struct cx24120_state *state,
+				   struct cx24120_cmd *cmd, u8 numreg)
+{
+	int ret, i;
+
+	if (numreg > CX24120_MAX_CMD_LEN) {
+		err("Too many registers to read. cmd->reg = %d", numreg);
+		return -EREMOTEIO;
+	}
+
+	ret = cx24120_message_send(state, cmd);
+	if (ret != 0)
+		return ret;
+
+	if (!numreg)
+		return 0;
+
+	/* Read numreg registers starting from register cmd->len */
+	for (i = 0; i < numreg; i++)
+		cmd->arg[i] = cx24120_readreg(state, (cmd->len + i + 1));
+
+	return 0;
+}
+
+static int cx24120_read_signal_strength(struct dvb_frontend *fe,
+					u16 *signal_strength)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	if (c->strength.stat[0].scale != FE_SCALE_RELATIVE)
+		*signal_strength = 0;
+	else
+		*signal_strength = c->strength.stat[0].uvalue;
+
+	return 0;
+}
+
+static int cx24120_msg_mpeg_output_global_config(struct cx24120_state *state,
+						 u8 enable)
+{
+	struct cx24120_cmd cmd;
+	int ret;
+
+	cmd.id = CMD_MPEG_ONOFF;
+	cmd.len = 4;
+	cmd.arg[0] = 0x01;
+	cmd.arg[1] = 0x00;
+	cmd.arg[2] = enable ? 0 : (u8)(-1);
+	cmd.arg[3] = 0x01;
+
+	ret = cx24120_message_send(state, &cmd);
+	if (ret != 0) {
+		dev_dbg(&state->i2c->dev, "failed to %s MPEG output\n",
+			enable ? "enable" : "disable");
+		return ret;
+	}
+
+	state->mpeg_enabled = enable;
+	dev_dbg(&state->i2c->dev, "MPEG output %s\n",
+		enable ? "enabled" : "disabled");
+
+	return 0;
+}
+
+static int cx24120_msg_mpeg_output_config(struct cx24120_state *state, u8 seq)
+{
+	struct cx24120_cmd cmd;
+	struct cx24120_initial_mpeg_config i =
+			state->config->initial_mpeg_config;
+
+	cmd.id = CMD_MPEG_INIT;
+	cmd.len = 7;
+	cmd.arg[0] = seq; /* sequental number - can be 0,1,2 */
+	cmd.arg[1] = ((i.x1 & 0x01) << 1) | ((i.x1 >> 1) & 0x01);
+	cmd.arg[2] = 0x05;
+	cmd.arg[3] = 0x02;
+	cmd.arg[4] = ((i.x2 >> 1) & 0x01);
+	cmd.arg[5] = (i.x2 & 0xf0) | (i.x3 & 0x0f);
+	cmd.arg[6] = 0x10;
+
+	return cx24120_message_send(state, &cmd);
+}
+
+static int cx24120_diseqc_send_burst(struct dvb_frontend *fe,
+				     enum fe_sec_mini_cmd burst)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct cx24120_cmd cmd;
+
+	dev_dbg(&state->i2c->dev, "\n");
+
+	/*
+	 * Yes, cmd.len is set to zero. The old driver
+	 * didn't specify any len, but also had a
+	 * memset 0 before every use of the cmd struct
+	 * which would have set it to zero.
+	 * This quite probably needs looking into.
+	 */
+	cmd.id = CMD_DISEQC_BURST;
+	cmd.len = 0;
+	cmd.arg[0] = 0x00;
+	cmd.arg[1] = (burst == SEC_MINI_B) ? 0x01 : 0x00;
+
+	return cx24120_message_send(state, &cmd);
+}
+
+static int cx24120_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct cx24120_cmd cmd;
+
+	dev_dbg(&state->i2c->dev, "(%d)\n", tone);
+
+	if ((tone != SEC_TONE_ON) && (tone != SEC_TONE_OFF)) {
+		err("Invalid tone=%d\n", tone);
+		return -EINVAL;
+	}
+
+	cmd.id = CMD_SETTONE;
+	cmd.len = 4;
+	cmd.arg[0] = 0x00;
+	cmd.arg[1] = 0x00;
+	cmd.arg[2] = 0x00;
+	cmd.arg[3] = (tone == SEC_TONE_ON) ? 0x01 : 0x00;
+
+	return cx24120_message_send(state, &cmd);
+}
+
+static int cx24120_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct cx24120_cmd cmd;
+
+	dev_dbg(&state->i2c->dev, "(%d)\n", voltage);
+
+	cmd.id = CMD_SETVOLTAGE;
+	cmd.len = 2;
+	cmd.arg[0] = 0x00;
+	cmd.arg[1] = (voltage == SEC_VOLTAGE_18) ? 0x01 : 0x00;
+
+	return cx24120_message_send(state, &cmd);
+}
+
+static int cx24120_send_diseqc_msg(struct dvb_frontend *fe,
+				   struct dvb_diseqc_master_cmd *d)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct cx24120_cmd cmd;
+	int back_count;
+
+	dev_dbg(&state->i2c->dev, "\n");
+
+	cmd.id = CMD_DISEQC_MSG1;
+	cmd.len = 11;
+	cmd.arg[0] = 0x00;
+	cmd.arg[1] = 0x00;
+	cmd.arg[2] = 0x03;
+	cmd.arg[3] = 0x16;
+	cmd.arg[4] = 0x28;
+	cmd.arg[5] = 0x01;
+	cmd.arg[6] = 0x01;
+	cmd.arg[7] = 0x14;
+	cmd.arg[8] = 0x19;
+	cmd.arg[9] = 0x14;
+	cmd.arg[10] = 0x1e;
+
+	if (cx24120_message_send(state, &cmd)) {
+		err("send 1st message(0x%x) failed\n", cmd.id);
+		return -EREMOTEIO;
+	}
+
+	cmd.id = CMD_DISEQC_MSG2;
+	cmd.len = d->msg_len + 6;
+	cmd.arg[0] = 0x00;
+	cmd.arg[1] = 0x01;
+	cmd.arg[2] = 0x02;
+	cmd.arg[3] = 0x00;
+	cmd.arg[4] = 0x00;
+	cmd.arg[5] = d->msg_len;
+
+	memcpy(&cmd.arg[6], &d->msg, d->msg_len);
+
+	if (cx24120_message_send(state, &cmd)) {
+		err("send 2nd message(0x%x) failed\n", cmd.id);
+		return -EREMOTEIO;
+	}
+
+	back_count = 500;
+	do {
+		if (!(cx24120_readreg(state, 0x93) & 0x01)) {
+			dev_dbg(&state->i2c->dev, "diseqc sequence sent\n");
+			return 0;
+		}
+		msleep(20);
+		back_count -= 20;
+	} while (back_count);
+
+	err("Too long waiting for diseqc.\n");
+	return -ETIMEDOUT;
+}
+
+static void cx24120_get_stats(struct cx24120_state *state)
+{
+	struct dvb_frontend *fe = &state->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct cx24120_cmd cmd;
+	int ret, cnr, msecs;
+	u16 sig, ucb;
+	u32 ber;
+
+	dev_dbg(&state->i2c->dev, "\n");
+
+	/* signal strength */
+	if (state->fe_status & FE_HAS_SIGNAL) {
+		cmd.id = CMD_READ_SNR;
+		cmd.len = 1;
+		cmd.arg[0] = 0x00;
+
+		ret = cx24120_message_send(state, &cmd);
+		if (ret != 0) {
+			err("error reading signal strength\n");
+			return;
+		}
+
+		/* raw */
+		sig = cx24120_readreg(state, CX24120_REG_SIGSTR_H) >> 6;
+		sig = sig << 8;
+		sig |= cx24120_readreg(state, CX24120_REG_SIGSTR_L);
+		dev_dbg(&state->i2c->dev,
+			"signal strength from firmware = 0x%x\n", sig);
+
+		/* cooked */
+		sig = -100 * sig + 94324;
+
+		c->strength.stat[0].scale = FE_SCALE_RELATIVE;
+		c->strength.stat[0].uvalue = sig;
+	} else {
+		c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
+
+	/* CNR */
+	if (state->fe_status & FE_HAS_VITERBI) {
+		cnr = cx24120_readreg(state, CX24120_REG_QUALITY_H) << 8;
+		cnr |= cx24120_readreg(state, CX24120_REG_QUALITY_L);
+		dev_dbg(&state->i2c->dev, "read SNR index = %d\n", cnr);
+
+		/* guessed - seems about right */
+		cnr = cnr * 100;
+
+		c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+		c->cnr.stat[0].svalue = cnr;
+	} else {
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
+
+	/* BER & UCB require lock */
+	if (!(state->fe_status & FE_HAS_LOCK)) {
+		c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		return;
+	}
+
+	/* BER */
+	if (time_after(jiffies, state->ber_jiffies_stats)) {
+		msecs = (state->berw_usecs + 500) / 1000;
+		state->ber_jiffies_stats = jiffies + msecs_to_jiffies(msecs);
+
+		ber = cx24120_readreg(state, CX24120_REG_BER_HH) << 24;
+		ber |= cx24120_readreg(state, CX24120_REG_BER_HL) << 16;
+		ber |= cx24120_readreg(state, CX24120_REG_BER_LH) << 8;
+		ber |= cx24120_readreg(state, CX24120_REG_BER_LL);
+		dev_dbg(&state->i2c->dev, "read BER index = %d\n", ber);
+
+		c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[0].uvalue += ber;
+
+		c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_count.stat[0].uvalue += CX24120_BER_WSIZE;
+	}
+
+	/* UCB */
+	if (time_after(jiffies, state->per_jiffies_stats)) {
+		state->per_jiffies_stats = jiffies + msecs_to_jiffies(1000);
+
+		ucb = cx24120_readreg(state, CX24120_REG_UCB_H) << 8;
+		ucb |= cx24120_readreg(state, CX24120_REG_UCB_L);
+		dev_dbg(&state->i2c->dev, "ucblocks = %d\n", ucb);
+
+		/* handle reset */
+		if (ucb < state->ucb_offset)
+			state->ucb_offset = c->block_error.stat[0].uvalue;
+
+		c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->block_error.stat[0].uvalue = ucb + state->ucb_offset;
+
+		c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+		c->block_count.stat[0].uvalue += state->bitrate / 8 / 208;
+	}
+}
+
+static void cx24120_set_clock_ratios(struct dvb_frontend *fe);
+
+/* Read current tuning status */
+static int cx24120_read_status(struct dvb_frontend *fe, enum fe_status *status)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+	int lock;
+
+	lock = cx24120_readreg(state, CX24120_REG_STATUS);
+
+	dev_dbg(&state->i2c->dev, "status = 0x%02x\n", lock);
+
+	*status = 0;
+
+	if (lock & CX24120_HAS_SIGNAL)
+		*status = FE_HAS_SIGNAL;
+	if (lock & CX24120_HAS_CARRIER)
+		*status |= FE_HAS_CARRIER;
+	if (lock & CX24120_HAS_VITERBI)
+		*status |= FE_HAS_VITERBI | FE_HAS_SYNC;
+	if (lock & CX24120_HAS_LOCK)
+		*status |= FE_HAS_LOCK;
+
+	/*
+	 * TODO: is FE_HAS_SYNC in the right place?
+	 * Other cx241xx drivers have this slightly
+	 * different
+	 */
+
+	state->fe_status = *status;
+	cx24120_get_stats(state);
+
+	/* Set the clock once tuned in */
+	if (state->need_clock_set && *status & FE_HAS_LOCK) {
+		/* Set clock ratios */
+		cx24120_set_clock_ratios(fe);
+
+		/* Old driver would do a msleep(200) here */
+
+		/* Renable mpeg output */
+		if (!state->mpeg_enabled)
+			cx24120_msg_mpeg_output_global_config(state, 1);
+
+		state->need_clock_set = 0;
+	}
+
+	return 0;
+}
+
+/*
+ * FEC & modulation lookup table
+ * Used for decoding the REG_FECMODE register
+ * once tuned in.
+ */
+struct cx24120_modfec {
+	enum fe_delivery_system delsys;
+	enum fe_modulation mod;
+	enum fe_code_rate fec;
+	u8 val;
+};
+
+static const struct cx24120_modfec modfec_lookup_table[] = {
+	/*delsys     mod    fec       val */
+	{ SYS_DVBS,  QPSK,  FEC_1_2,  0x01 },
+	{ SYS_DVBS,  QPSK,  FEC_2_3,  0x02 },
+	{ SYS_DVBS,  QPSK,  FEC_3_4,  0x03 },
+	{ SYS_DVBS,  QPSK,  FEC_4_5,  0x04 },
+	{ SYS_DVBS,  QPSK,  FEC_5_6,  0x05 },
+	{ SYS_DVBS,  QPSK,  FEC_6_7,  0x06 },
+	{ SYS_DVBS,  QPSK,  FEC_7_8,  0x07 },
+
+	{ SYS_DVBS2, QPSK,  FEC_1_2,  0x04 },
+	{ SYS_DVBS2, QPSK,  FEC_3_5,  0x05 },
+	{ SYS_DVBS2, QPSK,  FEC_2_3,  0x06 },
+	{ SYS_DVBS2, QPSK,  FEC_3_4,  0x07 },
+	{ SYS_DVBS2, QPSK,  FEC_4_5,  0x08 },
+	{ SYS_DVBS2, QPSK,  FEC_5_6,  0x09 },
+	{ SYS_DVBS2, QPSK,  FEC_8_9,  0x0a },
+	{ SYS_DVBS2, QPSK,  FEC_9_10, 0x0b },
+
+	{ SYS_DVBS2, PSK_8, FEC_3_5,  0x0c },
+	{ SYS_DVBS2, PSK_8, FEC_2_3,  0x0d },
+	{ SYS_DVBS2, PSK_8, FEC_3_4,  0x0e },
+	{ SYS_DVBS2, PSK_8, FEC_5_6,  0x0f },
+	{ SYS_DVBS2, PSK_8, FEC_8_9,  0x10 },
+	{ SYS_DVBS2, PSK_8, FEC_9_10, 0x11 },
+};
+
+/* Retrieve current fec, modulation & pilot values */
+static int cx24120_get_fec(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct cx24120_state *state = fe->demodulator_priv;
+	int idx;
+	int ret;
+	int fec;
+
+	ret = cx24120_readreg(state, CX24120_REG_FECMODE);
+	fec = ret & 0x3f; /* Lower 6 bits */
+
+	dev_dbg(&state->i2c->dev, "raw fec = %d\n", fec);
+
+	for (idx = 0; idx < ARRAY_SIZE(modfec_lookup_table); idx++) {
+		if (modfec_lookup_table[idx].delsys != state->dcur.delsys)
+			continue;
+		if (modfec_lookup_table[idx].val != fec)
+			continue;
+
+		break; /* found */
+	}
+
+	if (idx >= ARRAY_SIZE(modfec_lookup_table)) {
+		dev_dbg(&state->i2c->dev, "couldn't find fec!\n");
+		return -EINVAL;
+	}
+
+	/* save values back to cache */
+	c->modulation = modfec_lookup_table[idx].mod;
+	c->fec_inner = modfec_lookup_table[idx].fec;
+	c->pilot = (ret & 0x80) ? PILOT_ON : PILOT_OFF;
+
+	dev_dbg(&state->i2c->dev, "mod(%d), fec(%d), pilot(%d)\n",
+		c->modulation, c->fec_inner, c->pilot);
+
+	return 0;
+}
+
+/* Calculate ber window time */
+static void cx24120_calculate_ber_window(struct cx24120_state *state, u32 rate)
+{
+	struct dvb_frontend *fe = &state->frontend;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	u64 tmp;
+
+	/*
+	 * Calculate bitrate from rate in the clock ratios table.
+	 * This isn't *exactly* right but close enough.
+	 */
+	tmp = (u64)c->symbol_rate * rate;
+	do_div(tmp, 256);
+	state->bitrate = tmp;
+
+	/* usecs per ber window */
+	tmp = 1000000ULL * CX24120_BER_WSIZE;
+	do_div(tmp, state->bitrate);
+	state->berw_usecs = tmp;
+
+	dev_dbg(&state->i2c->dev, "bitrate: %u, berw_usecs: %u\n",
+		state->bitrate, state->berw_usecs);
+}
+
+/*
+ * Clock ratios lookup table
+ *
+ * Values obtained from much larger table in old driver
+ * which had numerous entries which would never match.
+ *
+ * There's probably some way of calculating these but I
+ * can't determine the pattern
+ */
+struct cx24120_clock_ratios_table {
+	enum fe_delivery_system delsys;
+	enum fe_pilot pilot;
+	enum fe_modulation mod;
+	enum fe_code_rate fec;
+	u32 m_rat;
+	u32 n_rat;
+	u32 rate;
+};
+
+static const struct cx24120_clock_ratios_table clock_ratios_table[] = {
+	/*delsys     pilot      mod    fec       m_rat    n_rat   rate */
+	{ SYS_DVBS2, PILOT_OFF, QPSK,  FEC_1_2,  273088,  254505, 274 },
+	{ SYS_DVBS2, PILOT_OFF, QPSK,  FEC_3_5,  17272,   13395,  330 },
+	{ SYS_DVBS2, PILOT_OFF, QPSK,  FEC_2_3,  24344,   16967,  367 },
+	{ SYS_DVBS2, PILOT_OFF, QPSK,  FEC_3_4,  410788,  254505, 413 },
+	{ SYS_DVBS2, PILOT_OFF, QPSK,  FEC_4_5,  438328,  254505, 440 },
+	{ SYS_DVBS2, PILOT_OFF, QPSK,  FEC_5_6,  30464,   16967,  459 },
+	{ SYS_DVBS2, PILOT_OFF, QPSK,  FEC_8_9,  487832,  254505, 490 },
+	{ SYS_DVBS2, PILOT_OFF, QPSK,  FEC_9_10, 493952,  254505, 496 },
+	{ SYS_DVBS2, PILOT_OFF, PSK_8, FEC_3_5,  328168,  169905, 494 },
+	{ SYS_DVBS2, PILOT_OFF, PSK_8, FEC_2_3,  24344,   11327,  550 },
+	{ SYS_DVBS2, PILOT_OFF, PSK_8, FEC_3_4,  410788,  169905, 618 },
+	{ SYS_DVBS2, PILOT_OFF, PSK_8, FEC_5_6,  30464,   11327,  688 },
+	{ SYS_DVBS2, PILOT_OFF, PSK_8, FEC_8_9,  487832,  169905, 735 },
+	{ SYS_DVBS2, PILOT_OFF, PSK_8, FEC_9_10, 493952,  169905, 744 },
+	{ SYS_DVBS2, PILOT_ON,  QPSK,  FEC_1_2,  273088,  260709, 268 },
+	{ SYS_DVBS2, PILOT_ON,  QPSK,  FEC_3_5,  328168,  260709, 322 },
+	{ SYS_DVBS2, PILOT_ON,  QPSK,  FEC_2_3,  121720,  86903,  358 },
+	{ SYS_DVBS2, PILOT_ON,  QPSK,  FEC_3_4,  410788,  260709, 403 },
+	{ SYS_DVBS2, PILOT_ON,  QPSK,  FEC_4_5,  438328,  260709, 430 },
+	{ SYS_DVBS2, PILOT_ON,  QPSK,  FEC_5_6,  152320,  86903,  448 },
+	{ SYS_DVBS2, PILOT_ON,  QPSK,  FEC_8_9,  487832,  260709, 479 },
+	{ SYS_DVBS2, PILOT_ON,  QPSK,  FEC_9_10, 493952,  260709, 485 },
+	{ SYS_DVBS2, PILOT_ON,  PSK_8, FEC_3_5,  328168,  173853, 483 },
+	{ SYS_DVBS2, PILOT_ON,  PSK_8, FEC_2_3,  121720,  57951,  537 },
+	{ SYS_DVBS2, PILOT_ON,  PSK_8, FEC_3_4,  410788,  173853, 604 },
+	{ SYS_DVBS2, PILOT_ON,  PSK_8, FEC_5_6,  152320,  57951,  672 },
+	{ SYS_DVBS2, PILOT_ON,  PSK_8, FEC_8_9,  487832,  173853, 718 },
+	{ SYS_DVBS2, PILOT_ON,  PSK_8, FEC_9_10, 493952,  173853, 727 },
+	{ SYS_DVBS,  PILOT_OFF, QPSK,  FEC_1_2,  152592,  152592, 256 },
+	{ SYS_DVBS,  PILOT_OFF, QPSK,  FEC_2_3,  305184,  228888, 341 },
+	{ SYS_DVBS,  PILOT_OFF, QPSK,  FEC_3_4,  457776,  305184, 384 },
+	{ SYS_DVBS,  PILOT_OFF, QPSK,  FEC_5_6,  762960,  457776, 427 },
+	{ SYS_DVBS,  PILOT_OFF, QPSK,  FEC_7_8,  1068144, 610368, 448 },
+};
+
+/* Set clock ratio from lookup table */
+static void cx24120_set_clock_ratios(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct cx24120_cmd cmd;
+	int ret, idx;
+
+	/* Find fec, modulation, pilot */
+	ret = cx24120_get_fec(fe);
+	if (ret != 0)
+		return;
+
+	/* Find the clock ratios in the lookup table */
+	for (idx = 0; idx < ARRAY_SIZE(clock_ratios_table); idx++) {
+		if (clock_ratios_table[idx].delsys != state->dcur.delsys)
+			continue;
+		if (clock_ratios_table[idx].mod != c->modulation)
+			continue;
+		if (clock_ratios_table[idx].fec != c->fec_inner)
+			continue;
+		if (clock_ratios_table[idx].pilot != c->pilot)
+			continue;
+
+		break;		/* found */
+	}
+
+	if (idx >= ARRAY_SIZE(clock_ratios_table)) {
+		info("Clock ratio not found - data reception in danger\n");
+		return;
+	}
+
+	/* Read current values? */
+	cmd.id = CMD_CLOCK_READ;
+	cmd.len = 1;
+	cmd.arg[0] = 0x00;
+	ret = cx24120_message_sendrcv(state, &cmd, 6);
+	if (ret != 0)
+		return;
+	/* in cmd[0]-[5] - result */
+
+	dev_dbg(&state->i2c->dev, "m=%d, n=%d; idx: %d m=%d, n=%d, rate=%d\n",
+		cmd.arg[2] | (cmd.arg[1] << 8) | (cmd.arg[0] << 16),
+		cmd.arg[5] | (cmd.arg[4] << 8) | (cmd.arg[3] << 16),
+		idx,
+		clock_ratios_table[idx].m_rat,
+		clock_ratios_table[idx].n_rat,
+		clock_ratios_table[idx].rate);
+
+	/* Set the clock */
+	cmd.id = CMD_CLOCK_SET;
+	cmd.len = 10;
+	cmd.arg[0] = 0;
+	cmd.arg[1] = 0x10;
+	cmd.arg[2] = (clock_ratios_table[idx].m_rat >> 16) & 0xff;
+	cmd.arg[3] = (clock_ratios_table[idx].m_rat >>  8) & 0xff;
+	cmd.arg[4] = (clock_ratios_table[idx].m_rat >>  0) & 0xff;
+	cmd.arg[5] = (clock_ratios_table[idx].n_rat >> 16) & 0xff;
+	cmd.arg[6] = (clock_ratios_table[idx].n_rat >>  8) & 0xff;
+	cmd.arg[7] = (clock_ratios_table[idx].n_rat >>  0) & 0xff;
+	cmd.arg[8] = (clock_ratios_table[idx].rate >> 8) & 0xff;
+	cmd.arg[9] = (clock_ratios_table[idx].rate >> 0) & 0xff;
+
+	cx24120_message_send(state, &cmd);
+
+	/* Calculate ber window rates for stat work */
+	cx24120_calculate_ber_window(state, clock_ratios_table[idx].rate);
+}
+
+/* Set inversion value */
+static int cx24120_set_inversion(struct cx24120_state *state,
+				 enum fe_spectral_inversion inversion)
+{
+	dev_dbg(&state->i2c->dev, "(%d)\n", inversion);
+
+	switch (inversion) {
+	case INVERSION_OFF:
+		state->dnxt.inversion_val = 0x00;
+		break;
+	case INVERSION_ON:
+		state->dnxt.inversion_val = 0x04;
+		break;
+	case INVERSION_AUTO:
+		state->dnxt.inversion_val = 0x0c;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	state->dnxt.inversion = inversion;
+
+	return 0;
+}
+
+/* FEC lookup table for tuning */
+struct cx24120_modfec_table {
+	enum fe_delivery_system delsys;
+	enum fe_modulation mod;
+	enum fe_code_rate fec;
+	u8 val;
+};
+
+static const struct cx24120_modfec_table modfec_table[] = {
+	/*delsys     mod    fec       val */
+	{ SYS_DVBS,  QPSK,  FEC_1_2,  0x2e },
+	{ SYS_DVBS,  QPSK,  FEC_2_3,  0x2f },
+	{ SYS_DVBS,  QPSK,  FEC_3_4,  0x30 },
+	{ SYS_DVBS,  QPSK,  FEC_5_6,  0x31 },
+	{ SYS_DVBS,  QPSK,  FEC_6_7,  0x32 },
+	{ SYS_DVBS,  QPSK,  FEC_7_8,  0x33 },
+
+	{ SYS_DVBS2, QPSK,  FEC_1_2,  0x04 },
+	{ SYS_DVBS2, QPSK,  FEC_3_5,  0x05 },
+	{ SYS_DVBS2, QPSK,  FEC_2_3,  0x06 },
+	{ SYS_DVBS2, QPSK,  FEC_3_4,  0x07 },
+	{ SYS_DVBS2, QPSK,  FEC_4_5,  0x08 },
+	{ SYS_DVBS2, QPSK,  FEC_5_6,  0x09 },
+	{ SYS_DVBS2, QPSK,  FEC_8_9,  0x0a },
+	{ SYS_DVBS2, QPSK,  FEC_9_10, 0x0b },
+
+	{ SYS_DVBS2, PSK_8, FEC_3_5,  0x0c },
+	{ SYS_DVBS2, PSK_8, FEC_2_3,  0x0d },
+	{ SYS_DVBS2, PSK_8, FEC_3_4,  0x0e },
+	{ SYS_DVBS2, PSK_8, FEC_5_6,  0x0f },
+	{ SYS_DVBS2, PSK_8, FEC_8_9,  0x10 },
+	{ SYS_DVBS2, PSK_8, FEC_9_10, 0x11 },
+};
+
+/* Set fec_val & fec_mask values from delsys, modulation & fec */
+static int cx24120_set_fec(struct cx24120_state *state, enum fe_modulation mod,
+			   enum fe_code_rate fec)
+{
+	int idx;
+
+	dev_dbg(&state->i2c->dev, "(0x%02x,0x%02x)\n", mod, fec);
+
+	state->dnxt.fec = fec;
+
+	/* Lookup fec_val from modfec table */
+	for (idx = 0; idx < ARRAY_SIZE(modfec_table); idx++) {
+		if (modfec_table[idx].delsys != state->dnxt.delsys)
+			continue;
+		if (modfec_table[idx].mod != mod)
+			continue;
+		if (modfec_table[idx].fec != fec)
+			continue;
+
+		/* found */
+		state->dnxt.fec_mask = 0x00;
+		state->dnxt.fec_val = modfec_table[idx].val;
+		return 0;
+	}
+
+	if (state->dnxt.delsys == SYS_DVBS2) {
+		/* DVBS2 auto is 0x00/0x00 */
+		state->dnxt.fec_mask = 0x00;
+		state->dnxt.fec_val  = 0x00;
+	} else {
+		/* Set DVB-S to auto */
+		state->dnxt.fec_val  = 0x2e;
+		state->dnxt.fec_mask = 0xac;
+	}
+
+	return 0;
+}
+
+/* Set pilot */
+static int cx24120_set_pilot(struct cx24120_state *state, enum fe_pilot pilot)
+{
+	dev_dbg(&state->i2c->dev, "(%d)\n", pilot);
+
+	/* Pilot only valid in DVBS2 */
+	if (state->dnxt.delsys != SYS_DVBS2) {
+		state->dnxt.pilot_val = CX24120_PILOT_OFF;
+		return 0;
+	}
+
+	switch (pilot) {
+	case PILOT_OFF:
+		state->dnxt.pilot_val = CX24120_PILOT_OFF;
+		break;
+	case PILOT_ON:
+		state->dnxt.pilot_val = CX24120_PILOT_ON;
+		break;
+	case PILOT_AUTO:
+	default:
+		state->dnxt.pilot_val = CX24120_PILOT_AUTO;
+	}
+
+	return 0;
+}
+
+/* Set symbol rate */
+static int cx24120_set_symbolrate(struct cx24120_state *state, u32 rate)
+{
+	dev_dbg(&state->i2c->dev, "(%d)\n", rate);
+
+	state->dnxt.symbol_rate = rate;
+
+	/* Check symbol rate */
+	if (rate  > 31000000) {
+		state->dnxt.clkdiv  = (-(rate < 31000001) & 3) + 2;
+		state->dnxt.ratediv = (-(rate < 31000001) & 6) + 4;
+	} else {
+		state->dnxt.clkdiv  = 3;
+		state->dnxt.ratediv = 6;
+	}
+
+	return 0;
+}
+
+/* Overwrite the current tuning params, we are about to tune */
+static void cx24120_clone_params(struct dvb_frontend *fe)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+
+	state->dcur = state->dnxt;
+}
+
+static int cx24120_set_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct cx24120_cmd cmd;
+	int ret;
+
+	switch (c->delivery_system) {
+	case SYS_DVBS2:
+		dev_dbg(&state->i2c->dev, "DVB-S2\n");
+		break;
+	case SYS_DVBS:
+		dev_dbg(&state->i2c->dev, "DVB-S\n");
+		break;
+	default:
+		dev_dbg(&state->i2c->dev,
+			"delivery system(%d) not supported\n",
+			c->delivery_system);
+		ret = -EINVAL;
+		break;
+	}
+
+	state->dnxt.delsys = c->delivery_system;
+	state->dnxt.modulation = c->modulation;
+	state->dnxt.frequency = c->frequency;
+	state->dnxt.pilot = c->pilot;
+
+	ret = cx24120_set_inversion(state, c->inversion);
+	if (ret !=  0)
+		return ret;
+
+	ret = cx24120_set_fec(state, c->modulation, c->fec_inner);
+	if (ret !=  0)
+		return ret;
+
+	ret = cx24120_set_pilot(state, c->pilot);
+	if (ret != 0)
+		return ret;
+
+	ret = cx24120_set_symbolrate(state, c->symbol_rate);
+	if (ret !=  0)
+		return ret;
+
+	/* discard the 'current' tuning parameters and prepare to tune */
+	cx24120_clone_params(fe);
+
+	dev_dbg(&state->i2c->dev,
+		"delsys      = %d\n", state->dcur.delsys);
+	dev_dbg(&state->i2c->dev,
+		"modulation  = %d\n", state->dcur.modulation);
+	dev_dbg(&state->i2c->dev,
+		"frequency   = %d\n", state->dcur.frequency);
+	dev_dbg(&state->i2c->dev,
+		"pilot       = %d (val = 0x%02x)\n",
+		state->dcur.pilot, state->dcur.pilot_val);
+	dev_dbg(&state->i2c->dev,
+		"symbol_rate = %d (clkdiv/ratediv = 0x%02x/0x%02x)\n",
+		 state->dcur.symbol_rate,
+		 state->dcur.clkdiv, state->dcur.ratediv);
+	dev_dbg(&state->i2c->dev,
+		"FEC         = %d (mask/val = 0x%02x/0x%02x)\n",
+		state->dcur.fec, state->dcur.fec_mask, state->dcur.fec_val);
+	dev_dbg(&state->i2c->dev,
+		"Inversion   = %d (val = 0x%02x)\n",
+		state->dcur.inversion, state->dcur.inversion_val);
+
+	/* Flag that clock needs to be set after tune */
+	state->need_clock_set = 1;
+
+	/* Tune in */
+	cmd.id = CMD_TUNEREQUEST;
+	cmd.len = 15;
+	cmd.arg[0] = 0;
+	cmd.arg[1]  = (state->dcur.frequency & 0xff0000) >> 16;
+	cmd.arg[2]  = (state->dcur.frequency & 0x00ff00) >> 8;
+	cmd.arg[3]  = (state->dcur.frequency & 0x0000ff);
+	cmd.arg[4]  = ((state->dcur.symbol_rate / 1000) & 0xff00) >> 8;
+	cmd.arg[5]  = ((state->dcur.symbol_rate / 1000) & 0x00ff);
+	cmd.arg[6]  = state->dcur.inversion;
+	cmd.arg[7]  = state->dcur.fec_val | state->dcur.pilot_val;
+	cmd.arg[8]  = CX24120_SEARCH_RANGE_KHZ >> 8;
+	cmd.arg[9]  = CX24120_SEARCH_RANGE_KHZ & 0xff;
+	cmd.arg[10] = 0;		/* maybe rolloff? */
+	cmd.arg[11] = state->dcur.fec_mask;
+	cmd.arg[12] = state->dcur.ratediv;
+	cmd.arg[13] = state->dcur.clkdiv;
+	cmd.arg[14] = 0;
+
+	/* Send tune command */
+	ret = cx24120_message_send(state, &cmd);
+	if (ret != 0)
+		return ret;
+
+	/* Write symbol rate values */
+	ret = cx24120_writereg(state, CX24120_REG_CLKDIV, state->dcur.clkdiv);
+	ret = cx24120_readreg(state, CX24120_REG_RATEDIV);
+	ret &= 0xfffffff0;
+	ret |= state->dcur.ratediv;
+	ret = cx24120_writereg(state, CX24120_REG_RATEDIV, ret);
+
+	return 0;
+}
+
+/* Set vco from config */
+static int cx24120_set_vco(struct cx24120_state *state)
+{
+	struct cx24120_cmd cmd;
+	u32 nxtal_khz, vco;
+	u64 inv_vco;
+	u32 xtal_khz = state->config->xtal_khz;
+
+	nxtal_khz = xtal_khz * 4;
+	vco = nxtal_khz * 10;
+	inv_vco = DIV_ROUND_CLOSEST_ULL(0x400000000ULL, vco);
+
+	dev_dbg(&state->i2c->dev, "xtal=%d, vco=%d, inv_vco=%lld\n",
+		xtal_khz, vco, inv_vco);
+
+	cmd.id = CMD_VCO_SET;
+	cmd.len = 12;
+	cmd.arg[0] = (vco >> 16) & 0xff;
+	cmd.arg[1] = (vco >> 8) & 0xff;
+	cmd.arg[2] = vco & 0xff;
+	cmd.arg[3] = (inv_vco >> 8) & 0xff;
+	cmd.arg[4] = (inv_vco) & 0xff;
+	cmd.arg[5] = 0x03;
+	cmd.arg[6] = (nxtal_khz >> 8) & 0xff;
+	cmd.arg[7] = nxtal_khz & 0xff;
+	cmd.arg[8] = 0x06;
+	cmd.arg[9] = 0x03;
+	cmd.arg[10] = (xtal_khz >> 16) & 0xff;
+	cmd.arg[11] = xtal_khz & 0xff;
+
+	return cx24120_message_send(state, &cmd);
+}
+
+static int cx24120_init(struct dvb_frontend *fe)
+{
+	const struct firmware *fw;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct cx24120_cmd cmd;
+	u8 reg;
+	int ret, i;
+	unsigned char vers[4];
+
+	if (state->cold_init)
+		return 0;
+
+	/* ???? */
+	cx24120_writereg(state, 0xea, 0x00);
+	cx24120_test_rom(state);
+	reg = cx24120_readreg(state, 0xfb) & 0xfe;
+	cx24120_writereg(state, 0xfb, reg);
+	reg = cx24120_readreg(state, 0xfc) & 0xfe;
+	cx24120_writereg(state, 0xfc, reg);
+	cx24120_writereg(state, 0xc3, 0x04);
+	cx24120_writereg(state, 0xc4, 0x04);
+	cx24120_writereg(state, 0xce, 0x00);
+	cx24120_writereg(state, 0xcf, 0x00);
+	reg = cx24120_readreg(state, 0xea) & 0xfe;
+	cx24120_writereg(state, 0xea, reg);
+	cx24120_writereg(state, 0xeb, 0x0c);
+	cx24120_writereg(state, 0xec, 0x06);
+	cx24120_writereg(state, 0xed, 0x05);
+	cx24120_writereg(state, 0xee, 0x03);
+	cx24120_writereg(state, 0xef, 0x05);
+	cx24120_writereg(state, 0xf3, 0x03);
+	cx24120_writereg(state, 0xf4, 0x44);
+
+	for (i = 0; i < 3; i++) {
+		cx24120_writereg(state, 0xf0 + i, 0x04);
+		cx24120_writereg(state, 0xe6 + i, 0x02);
+	}
+
+	cx24120_writereg(state, 0xea, (reg | 0x01));
+	for (i = 0; i < 6; i += 2) {
+		cx24120_writereg(state, 0xc5 + i, 0x00);
+		cx24120_writereg(state, 0xc6 + i, 0x00);
+	}
+
+	cx24120_writereg(state, 0xe4, 0x03);
+	cx24120_writereg(state, 0xeb, 0x0a);
+
+	dev_dbg(&state->i2c->dev, "requesting firmware (%s) to download...\n",
+		CX24120_FIRMWARE);
+
+	ret = state->config->request_firmware(fe, &fw, CX24120_FIRMWARE);
+	if (ret) {
+		err("Could not load firmware (%s): %d\n", CX24120_FIRMWARE,
+		    ret);
+		return ret;
+	}
+
+	dev_dbg(&state->i2c->dev,
+		"Firmware found, size %d bytes (%02x %02x .. %02x %02x)\n",
+		(int)fw->size,			/* firmware_size in bytes */
+		fw->data[0],			/* fw 1st byte */
+		fw->data[1],			/* fw 2d byte */
+		fw->data[fw->size - 2],		/* fw before last byte */
+		fw->data[fw->size - 1]);	/* fw last byte */
+
+	cx24120_test_rom(state);
+	reg = cx24120_readreg(state, 0xfb) & 0xfe;
+	cx24120_writereg(state, 0xfb, reg);
+	cx24120_writereg(state, 0xe0, 0x76);
+	cx24120_writereg(state, 0xf7, 0x81);
+	cx24120_writereg(state, 0xf8, 0x00);
+	cx24120_writereg(state, 0xf9, 0x00);
+	cx24120_writeregs(state, 0xfa, fw->data, (fw->size - 1), 0x00);
+	cx24120_writereg(state, 0xf7, 0xc0);
+	cx24120_writereg(state, 0xe0, 0x00);
+	reg = (fw->size - 2) & 0x00ff;
+	cx24120_writereg(state, 0xf8, reg);
+	reg = ((fw->size - 2) >> 8) & 0x00ff;
+	cx24120_writereg(state, 0xf9, reg);
+	cx24120_writereg(state, 0xf7, 0x00);
+	cx24120_writereg(state, 0xdc, 0x00);
+	cx24120_writereg(state, 0xdc, 0x07);
+	msleep(500);
+
+	/* Check final byte matches final byte of firmware */
+	reg = cx24120_readreg(state, 0xe1);
+	if (reg == fw->data[fw->size - 1]) {
+		dev_dbg(&state->i2c->dev, "Firmware uploaded successfully\n");
+		ret = 0;
+	} else {
+		err("Firmware upload failed. Last byte returned=0x%x\n", ret);
+		ret = -EREMOTEIO;
+	}
+	cx24120_writereg(state, 0xdc, 0x00);
+	release_firmware(fw);
+	if (ret != 0)
+		return ret;
+
+	/* Start tuner */
+	cmd.id = CMD_START_TUNER;
+	cmd.len = 3;
+	cmd.arg[0] = 0x00;
+	cmd.arg[1] = 0x00;
+	cmd.arg[2] = 0x00;
+
+	if (cx24120_message_send(state, &cmd) != 0) {
+		err("Error tuner start! :(\n");
+		return -EREMOTEIO;
+	}
+
+	/* Set VCO */
+	ret = cx24120_set_vco(state);
+	if (ret != 0) {
+		err("Error set VCO! :(\n");
+		return ret;
+	}
+
+	/* set bandwidth */
+	cmd.id = CMD_BANDWIDTH;
+	cmd.len = 12;
+	cmd.arg[0] = 0x00;
+	cmd.arg[1] = 0x00;
+	cmd.arg[2] = 0x00;
+	cmd.arg[3] = 0x00;
+	cmd.arg[4] = 0x05;
+	cmd.arg[5] = 0x02;
+	cmd.arg[6] = 0x02;
+	cmd.arg[7] = 0x00;
+	cmd.arg[8] = 0x05;
+	cmd.arg[9] = 0x02;
+	cmd.arg[10] = 0x02;
+	cmd.arg[11] = 0x00;
+
+	if (cx24120_message_send(state, &cmd)) {
+		err("Error set bandwidth!\n");
+		return -EREMOTEIO;
+	}
+
+	reg = cx24120_readreg(state, 0xba);
+	if (reg > 3) {
+		dev_dbg(&state->i2c->dev, "Reset-readreg 0xba: %x\n", ret);
+		err("Error initialising tuner!\n");
+		return -EREMOTEIO;
+	}
+
+	dev_dbg(&state->i2c->dev, "Tuner initialised correctly.\n");
+
+	/* Initialise mpeg outputs */
+	cx24120_writereg(state, 0xeb, 0x0a);
+	if (cx24120_msg_mpeg_output_global_config(state, 0) ||
+	    cx24120_msg_mpeg_output_config(state, 0) ||
+	    cx24120_msg_mpeg_output_config(state, 1) ||
+	    cx24120_msg_mpeg_output_config(state, 2)) {
+		err("Error initialising mpeg output. :(\n");
+		return -EREMOTEIO;
+	}
+
+	/* Set size of BER window */
+	cmd.id = CMD_BER_CTRL;
+	cmd.len = 3;
+	cmd.arg[0] = 0x00;
+	cmd.arg[1] = CX24120_BER_WINDOW;
+	cmd.arg[2] = CX24120_BER_WINDOW;
+	if (cx24120_message_send(state, &cmd)) {
+		err("Error setting ber window\n");
+		return -EREMOTEIO;
+	}
+
+	/* Firmware CMD 35: Get firmware version */
+	cmd.id = CMD_FWVERSION;
+	cmd.len = 1;
+	for (i = 0; i < 4; i++) {
+		cmd.arg[0] = i;
+		ret = cx24120_message_send(state, &cmd);
+		if (ret != 0)
+			return ret;
+		vers[i] = cx24120_readreg(state, CX24120_REG_MAILBOX);
+	}
+	info("FW version %i.%i.%i.%i\n", vers[0], vers[1], vers[2], vers[3]);
+
+	/* init stats here in order signal app which stats are supported */
+	c->strength.len = 1;
+	c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->cnr.len = 1;
+	c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_error.len = 1;
+	c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_count.len = 1;
+	c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->block_error.len = 1;
+	c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->block_count.len = 1;
+	c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+
+	state->cold_init = 1;
+
+	return 0;
+}
+
+static int cx24120_tune(struct dvb_frontend *fe, bool re_tune,
+			unsigned int mode_flags, unsigned int *delay,
+			enum fe_status *status)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+	int ret;
+
+	dev_dbg(&state->i2c->dev, "(%d)\n", re_tune);
+
+	/* TODO: Do we need to set delay? */
+
+	if (re_tune) {
+		ret = cx24120_set_frontend(fe);
+		if (ret)
+			return ret;
+	}
+
+	return cx24120_read_status(fe, status);
+}
+
+static int cx24120_get_algo(struct dvb_frontend *fe)
+{
+	return DVBFE_ALGO_HW;
+}
+
+static int cx24120_sleep(struct dvb_frontend *fe)
+{
+	return 0;
+}
+
+static int cx24120_get_frontend(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct cx24120_state *state = fe->demodulator_priv;
+	u8 freq1, freq2, freq3;
+
+	dev_dbg(&state->i2c->dev, "\n");
+
+	/* don't return empty data if we're not tuned in */
+	if ((state->fe_status & FE_HAS_LOCK) == 0)
+		return 0;
+
+	/* Get frequency */
+	freq1 = cx24120_readreg(state, CX24120_REG_FREQ1);
+	freq2 = cx24120_readreg(state, CX24120_REG_FREQ2);
+	freq3 = cx24120_readreg(state, CX24120_REG_FREQ3);
+	c->frequency = (freq3 << 16) | (freq2 << 8) | freq1;
+	dev_dbg(&state->i2c->dev, "frequency = %d\n", c->frequency);
+
+	/* Get modulation, fec, pilot */
+	cx24120_get_fec(fe);
+
+	return 0;
+}
+
+static void cx24120_release(struct dvb_frontend *fe)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+
+	dev_dbg(&state->i2c->dev, "Clear state structure\n");
+	kfree(state);
+}
+
+static int cx24120_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+	struct cx24120_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	if (c->block_error.stat[0].scale != FE_SCALE_COUNTER) {
+		*ucblocks = 0;
+		return 0;
+	}
+
+	*ucblocks = c->block_error.stat[0].uvalue - state->ucb_offset;
+
+	return 0;
+}
+
+static struct dvb_frontend_ops cx24120_ops = {
+	.delsys = { SYS_DVBS, SYS_DVBS2 },
+	.info = {
+		.name = "Conexant CX24120/CX24118",
+		.frequency_min = 950000,
+		.frequency_max = 2150000,
+		.frequency_stepsize = 1011, /* kHz for QPSK frontends */
+		.frequency_tolerance = 5000,
+		.symbol_rate_min = 1000000,
+		.symbol_rate_max = 45000000,
+		.caps =	FE_CAN_INVERSION_AUTO |
+			FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
+			FE_CAN_FEC_4_5 | FE_CAN_FEC_5_6 | FE_CAN_FEC_6_7 |
+			FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
+			FE_CAN_2G_MODULATION |
+			FE_CAN_QPSK | FE_CAN_RECOVER
+	},
+	.release =			cx24120_release,
+
+	.init =				cx24120_init,
+	.sleep =			cx24120_sleep,
+
+	.tune =				cx24120_tune,
+	.get_frontend_algo =		cx24120_get_algo,
+	.set_frontend =			cx24120_set_frontend,
+
+	.get_frontend =			cx24120_get_frontend,
+	.read_status =			cx24120_read_status,
+	.read_ber =			cx24120_read_ber,
+	.read_signal_strength =		cx24120_read_signal_strength,
+	.read_snr =			cx24120_read_snr,
+	.read_ucblocks =		cx24120_read_ucblocks,
+
+	.diseqc_send_master_cmd =	cx24120_send_diseqc_msg,
+
+	.diseqc_send_burst =		cx24120_diseqc_send_burst,
+	.set_tone =			cx24120_set_tone,
+	.set_voltage =			cx24120_set_voltage,
+};
+
+MODULE_DESCRIPTION("DVB Frontend module for Conexant CX24120/CX24118 hardware");
+MODULE_AUTHOR("Jemma Denson");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb-frontends/cx24120.h b/drivers/media/dvb-frontends/cx24120.h
new file mode 100644
index 000000000000..f0970423e16f
--- /dev/null
+++ b/drivers/media/dvb-frontends/cx24120.h
@@ -0,0 +1,58 @@
+/*
+ * Conexant CX24120/CX24118 - DVB-S/S2 demod/tuner driver
+ *
+ * Copyright (C) 2008 Patrick Boettcher <pb@linuxtv.org>
+ * Copyright (C) 2009 Sergey Tyurin <forum.free-x.de>
+ * Updated 2012 by Jannis Achstetter <jannis_achstetter@web.de>
+ * Copyright (C) 2015 Jemma Denson <jdenson@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef CX24120_H
+#define CX24120_H
+
+#include <linux/kconfig.h>
+#include <linux/dvb/frontend.h>
+#include <linux/firmware.h>
+
+struct cx24120_initial_mpeg_config {
+	u8 x1;
+	u8 x2;
+	u8 x3;
+};
+
+struct cx24120_config {
+	u8 i2c_addr;
+	u32 xtal_khz;
+	struct cx24120_initial_mpeg_config initial_mpeg_config;
+
+	int (*request_firmware)(struct dvb_frontend *fe,
+				const struct firmware **fw, char *name);
+
+	/* max bytes I2C provider can write at once */
+	u16 i2c_wr_max;
+};
+
+#if IS_REACHABLE(CONFIG_DVB_CX24120)
+struct dvb_frontend *cx24120_attach(const struct cx24120_config *config,
+				    struct i2c_adapter *i2c);
+#else
+static inline
+struct dvb_frontend *cx24120_attach(const struct cx24120_config *config,
+				    struct i2c_adapter *i2c)
+{
+	pr_warn("%s: driver disabled by Kconfig\n", __func__);
+	return NULL;
+}
+#endif
+
+#endif /* CX24120_H */
diff --git a/drivers/media/dvb-frontends/cx24123.c b/drivers/media/dvb-frontends/cx24123.c
index 7975c6608e20..e18cf9e1185e 100644
--- a/drivers/media/dvb-frontends/cx24123.c
+++ b/drivers/media/dvb-frontends/cx24123.c
@@ -290,7 +290,7 @@ static int cx24123_i2c_readreg(struct cx24123_state *state, u8 i2c_addr, u8 reg)
 	cx24123_i2c_writereg(state, state->config->demod_address, reg, val)
 
 static int cx24123_set_inversion(struct cx24123_state *state,
-	fe_spectral_inversion_t inversion)
+				 enum fe_spectral_inversion inversion)
 {
 	u8 nom_reg = cx24123_readreg(state, 0x0e);
 	u8 auto_reg = cx24123_readreg(state, 0x10);
@@ -318,7 +318,7 @@ static int cx24123_set_inversion(struct cx24123_state *state,
 }
 
 static int cx24123_get_inversion(struct cx24123_state *state,
-	fe_spectral_inversion_t *inversion)
+				 enum fe_spectral_inversion *inversion)
 {
 	u8 val;
 
@@ -335,7 +335,7 @@ static int cx24123_get_inversion(struct cx24123_state *state,
 	return 0;
 }
 
-static int cx24123_set_fec(struct cx24123_state *state, fe_code_rate_t fec)
+static int cx24123_set_fec(struct cx24123_state *state, enum fe_code_rate fec)
 {
 	u8 nom_reg = cx24123_readreg(state, 0x0e) & ~0x07;
 
@@ -397,7 +397,7 @@ static int cx24123_set_fec(struct cx24123_state *state, fe_code_rate_t fec)
 	return 0;
 }
 
-static int cx24123_get_fec(struct cx24123_state *state, fe_code_rate_t *fec)
+static int cx24123_get_fec(struct cx24123_state *state, enum fe_code_rate *fec)
 {
 	int ret;
 
@@ -720,7 +720,7 @@ static int cx24123_initfe(struct dvb_frontend *fe)
 }
 
 static int cx24123_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t voltage)
+			       enum fe_sec_voltage voltage)
 {
 	struct cx24123_state *state = fe->demodulator_priv;
 	u8 val;
@@ -795,7 +795,7 @@ static int cx24123_send_diseqc_msg(struct dvb_frontend *fe,
 }
 
 static int cx24123_diseqc_send_burst(struct dvb_frontend *fe,
-	fe_sec_mini_cmd_t burst)
+				     enum fe_sec_mini_cmd burst)
 {
 	struct cx24123_state *state = fe->demodulator_priv;
 	int val, tone;
@@ -831,7 +831,7 @@ static int cx24123_diseqc_send_burst(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int cx24123_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int cx24123_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cx24123_state *state = fe->demodulator_priv;
 	int sync = cx24123_readreg(state, 0x14);
@@ -966,7 +966,7 @@ static int cx24123_get_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int cx24123_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int cx24123_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct cx24123_state *state = fe->demodulator_priv;
 	u8 val;
@@ -995,7 +995,7 @@ static int cx24123_tune(struct dvb_frontend *fe,
 			bool re_tune,
 			unsigned int mode_flags,
 			unsigned int *delay,
-			fe_status_t *status)
+			enum fe_status *status)
 {
 	int retval = 0;
 
diff --git a/drivers/media/dvb-frontends/cx24123.h b/drivers/media/dvb-frontends/cx24123.h
index 758aee5a072f..975f3c926fe8 100644
--- a/drivers/media/dvb-frontends/cx24123.h
+++ b/drivers/media/dvb-frontends/cx24123.h
@@ -50,7 +50,7 @@ static inline struct dvb_frontend *cx24123_attach(
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
-static struct i2c_adapter *
+static inline struct i2c_adapter *
 	cx24123_get_tuner_i2c_adapter(struct dvb_frontend *fe)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
diff --git a/drivers/media/dvb-frontends/cxd2820r_c.c b/drivers/media/dvb-frontends/cxd2820r_c.c
index 72b0e2db3aab..42fad6aa3958 100644
--- a/drivers/media/dvb-frontends/cxd2820r_c.c
+++ b/drivers/media/dvb-frontends/cxd2820r_c.c
@@ -259,7 +259,7 @@ int cxd2820r_read_ucblocks_c(struct dvb_frontend *fe, u32 *ucblocks)
 	return 0;
 }
 
-int cxd2820r_read_status_c(struct dvb_frontend *fe, fe_status_t *status)
+int cxd2820r_read_status_c(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
 	int ret;
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 490e090048ef..def6d21d1445 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -287,7 +287,8 @@ static int cxd2820r_set_frontend(struct dvb_frontend *fe)
 err:
 	return ret;
 }
-static int cxd2820r_read_status(struct dvb_frontend *fe, fe_status_t *status)
+
+static int cxd2820r_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
 	int ret;
@@ -501,7 +502,7 @@ static enum dvbfe_search cxd2820r_search(struct dvb_frontend *fe)
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i;
-	fe_status_t status = 0;
+	enum fe_status status = 0;
 
 	dev_dbg(&priv->i2c->dev, "%s: delsys=%d\n", __func__,
 			fe->dtv_property_cache.delivery_system);
diff --git a/drivers/media/dvb-frontends/cxd2820r_priv.h b/drivers/media/dvb-frontends/cxd2820r_priv.h
index 4b428959b16e..a0d53f01a8bf 100644
--- a/drivers/media/dvb-frontends/cxd2820r_priv.h
+++ b/drivers/media/dvb-frontends/cxd2820r_priv.h
@@ -48,7 +48,7 @@ struct cxd2820r_priv {
 	struct gpio_chip gpio_chip;
 #endif
 
-	fe_delivery_system_t delivery_system;
+	enum fe_delivery_system delivery_system;
 	bool last_tune_failed; /* for switch between T and T2 tune */
 };
 
@@ -80,7 +80,7 @@ int cxd2820r_get_frontend_c(struct dvb_frontend *fe);
 
 int cxd2820r_set_frontend_c(struct dvb_frontend *fe);
 
-int cxd2820r_read_status_c(struct dvb_frontend *fe, fe_status_t *status);
+int cxd2820r_read_status_c(struct dvb_frontend *fe, enum fe_status *status);
 
 int cxd2820r_read_ber_c(struct dvb_frontend *fe, u32 *ber);
 
@@ -103,7 +103,7 @@ int cxd2820r_get_frontend_t(struct dvb_frontend *fe);
 
 int cxd2820r_set_frontend_t(struct dvb_frontend *fe);
 
-int cxd2820r_read_status_t(struct dvb_frontend *fe, fe_status_t *status);
+int cxd2820r_read_status_t(struct dvb_frontend *fe, enum fe_status *status);
 
 int cxd2820r_read_ber_t(struct dvb_frontend *fe, u32 *ber);
 
@@ -126,7 +126,7 @@ int cxd2820r_get_frontend_t2(struct dvb_frontend *fe);
 
 int cxd2820r_set_frontend_t2(struct dvb_frontend *fe);
 
-int cxd2820r_read_status_t2(struct dvb_frontend *fe, fe_status_t *status);
+int cxd2820r_read_status_t2(struct dvb_frontend *fe, enum fe_status *status);
 
 int cxd2820r_read_ber_t2(struct dvb_frontend *fe, u32 *ber);
 
diff --git a/drivers/media/dvb-frontends/cxd2820r_t.c b/drivers/media/dvb-frontends/cxd2820r_t.c
index 008cb2ac8480..21abf1b4ed4d 100644
--- a/drivers/media/dvb-frontends/cxd2820r_t.c
+++ b/drivers/media/dvb-frontends/cxd2820r_t.c
@@ -349,7 +349,7 @@ int cxd2820r_read_ucblocks_t(struct dvb_frontend *fe, u32 *ucblocks)
 	return 0;
 }
 
-int cxd2820r_read_status_t(struct dvb_frontend *fe, fe_status_t *status)
+int cxd2820r_read_status_t(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
 	int ret;
diff --git a/drivers/media/dvb-frontends/cxd2820r_t2.c b/drivers/media/dvb-frontends/cxd2820r_t2.c
index 35fe364c7182..4e028b41c0d5 100644
--- a/drivers/media/dvb-frontends/cxd2820r_t2.c
+++ b/drivers/media/dvb-frontends/cxd2820r_t2.c
@@ -284,7 +284,7 @@ error:
 	return ret;
 }
 
-int cxd2820r_read_status_t2(struct dvb_frontend *fe, fe_status_t *status)
+int cxd2820r_read_status_t2(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
 	int ret;
diff --git a/drivers/media/dvb-frontends/dib0070.c b/drivers/media/dvb-frontends/dib0070.c
index 3b024bfe980a..0b8fb5dd1889 100644
--- a/drivers/media/dvb-frontends/dib0070.c
+++ b/drivers/media/dvb-frontends/dib0070.c
@@ -58,10 +58,10 @@ struct dib0070_state {
 	u16 wbd_ff_offset;
 	u8 revision;
 
-    enum frontend_tune_state tune_state;
-    u32 current_rf;
+	enum frontend_tune_state tune_state;
+	u32 current_rf;
 
-    /* for the captrim binary search */
+	/* for the captrim binary search */
 	s8 step;
 	u16 adc_diff;
 
@@ -72,7 +72,7 @@ struct dib0070_state {
 	const struct dib0070_tuning *current_tune_table_index;
 	const struct dib0070_lna_match *lna_match;
 
-    u8  wbd_gain_current;
+	u8  wbd_gain_current;
 	u16 wbd_offset_3_3[2];
 
 	/* for the I2C transfer */
@@ -151,31 +151,31 @@ static int dib0070_write_reg(struct dib0070_state *state, u8 reg, u16 val)
 } while (0)
 
 static int dib0070_set_bandwidth(struct dvb_frontend *fe)
-{
-    struct dib0070_state *state = fe->tuner_priv;
-    u16 tmp = dib0070_read_reg(state, 0x02) & 0x3fff;
-
-    if (state->fe->dtv_property_cache.bandwidth_hz/1000 > 7000)
-	tmp |= (0 << 14);
-    else if (state->fe->dtv_property_cache.bandwidth_hz/1000 > 6000)
-	tmp |= (1 << 14);
-    else if (state->fe->dtv_property_cache.bandwidth_hz/1000 > 5000)
-	tmp |= (2 << 14);
-    else
-	tmp |= (3 << 14);
-
-    dib0070_write_reg(state, 0x02, tmp);
-
-    /* sharpen the BB filter in ISDB-T to have higher immunity to adjacent channels */
-    if (state->fe->dtv_property_cache.delivery_system == SYS_ISDBT) {
-	u16 value = dib0070_read_reg(state, 0x17);
-
-	dib0070_write_reg(state, 0x17, value & 0xfffc);
-	tmp = dib0070_read_reg(state, 0x01) & 0x01ff;
-	dib0070_write_reg(state, 0x01, tmp | (60 << 9));
-
-	dib0070_write_reg(state, 0x17, value);
-    }
+	{
+	struct dib0070_state *state = fe->tuner_priv;
+	u16 tmp = dib0070_read_reg(state, 0x02) & 0x3fff;
+
+	if (state->fe->dtv_property_cache.bandwidth_hz/1000 > 7000)
+		tmp |= (0 << 14);
+	else if (state->fe->dtv_property_cache.bandwidth_hz/1000 > 6000)
+		tmp |= (1 << 14);
+	else if (state->fe->dtv_property_cache.bandwidth_hz/1000 > 5000)
+		tmp |= (2 << 14);
+	else
+		tmp |= (3 << 14);
+
+	dib0070_write_reg(state, 0x02, tmp);
+
+	/* sharpen the BB filter in ISDB-T to have higher immunity to adjacent channels */
+	if (state->fe->dtv_property_cache.delivery_system == SYS_ISDBT) {
+		u16 value = dib0070_read_reg(state, 0x17);
+
+		dib0070_write_reg(state, 0x17, value & 0xfffc);
+		tmp = dib0070_read_reg(state, 0x01) & 0x01ff;
+		dib0070_write_reg(state, 0x01, tmp | (60 << 9));
+
+		dib0070_write_reg(state, 0x17, value);
+	}
 	return 0;
 }
 
@@ -186,7 +186,6 @@ static int dib0070_captrim(struct dib0070_state *state, enum frontend_tune_state
 	int ret = 0;
 
 	if (*tune_state == CT_TUNER_STEP_0) {
-
 		dib0070_write_reg(state, 0x0f, 0xed10);
 		dib0070_write_reg(state, 0x17,    0x0034);
 
@@ -195,7 +194,7 @@ static int dib0070_captrim(struct dib0070_state *state, enum frontend_tune_state
 		state->adc_diff = 3000;
 		ret = 20;
 
-	*tune_state = CT_TUNER_STEP_1;
+		*tune_state = CT_TUNER_STEP_1;
 	} else if (*tune_state == CT_TUNER_STEP_1) {
 		state->step /= 2;
 		dib0070_write_reg(state, 0x14, state->lo4 | state->captrim);
@@ -220,9 +219,6 @@ static int dib0070_captrim(struct dib0070_state *state, enum frontend_tune_state
 			dprintk("CAPTRIM=%hd is closer to target (%hd/%hd)", state->captrim, adc, state->adc_diff);
 			state->adc_diff = adc;
 			state->fcaptrim = state->captrim;
-
-
-
 		}
 		state->captrim += (step_sign * state->step);
 
@@ -243,7 +239,8 @@ static int dib0070_captrim(struct dib0070_state *state, enum frontend_tune_state
 static int dib0070_set_ctrl_lo5(struct dvb_frontend *fe, u8 vco_bias_trim, u8 hf_div_trim, u8 cp_current, u8 third_order_filt)
 {
 	struct dib0070_state *state = fe->tuner_priv;
-    u16 lo5 = (third_order_filt << 14) | (0 << 13) | (1 << 12) | (3 << 9) | (cp_current << 6) | (hf_div_trim << 3) | (vco_bias_trim << 0);
+	u16 lo5 = (third_order_filt << 14) | (0 << 13) | (1 << 12) | (3 << 9) | (cp_current << 6) | (hf_div_trim << 3) | (vco_bias_trim << 0);
+
 	dprintk("CTRL_LO5: 0x%x", lo5);
 	return dib0070_write_reg(state, 0x15, lo5);
 }
@@ -257,281 +254,282 @@ void dib0070_ctrl_agc_filter(struct dvb_frontend *fe, u8 open)
 		dib0070_write_reg(state, 0x1a, 0x0000);
 	} else {
 		dib0070_write_reg(state, 0x1b, 0x4112);
-	if (state->cfg->vga_filter != 0) {
-		dib0070_write_reg(state, 0x1a, state->cfg->vga_filter);
-		dprintk("vga filter register is set to %x", state->cfg->vga_filter);
-	} else
-		dib0070_write_reg(state, 0x1a, 0x0009);
+		if (state->cfg->vga_filter != 0) {
+			dib0070_write_reg(state, 0x1a, state->cfg->vga_filter);
+			dprintk("vga filter register is set to %x", state->cfg->vga_filter);
+		} else
+			dib0070_write_reg(state, 0x1a, 0x0009);
 	}
 }
 
 EXPORT_SYMBOL(dib0070_ctrl_agc_filter);
 struct dib0070_tuning {
-    u32 max_freq; /* for every frequency less than or equal to that field: this information is correct */
-    u8 switch_trim;
-    u8 vco_band;
-    u8 hfdiv;
-    u8 vco_multi;
-    u8 presc;
-    u8 wbdmux;
-    u16 tuner_enable;
+	u32 max_freq; /* for every frequency less than or equal to that field: this information is correct */
+	u8 switch_trim;
+	u8 vco_band;
+	u8 hfdiv;
+	u8 vco_multi;
+	u8 presc;
+	u8 wbdmux;
+	u16 tuner_enable;
 };
 
 struct dib0070_lna_match {
-    u32 max_freq; /* for every frequency less than or equal to that field: this information is correct */
-    u8 lna_band;
+	u32 max_freq; /* for every frequency less than or equal to that field: this information is correct */
+	u8 lna_band;
 };
 
 static const struct dib0070_tuning dib0070s_tuning_table[] = {
-    {     570000, 2, 1, 3, 6, 6, 2, 0x4000 | 0x0800 }, /* UHF */
-    {     700000, 2, 0, 2, 4, 2, 2, 0x4000 | 0x0800 },
-    {     863999, 2, 1, 2, 4, 2, 2, 0x4000 | 0x0800 },
-    {    1500000, 0, 1, 1, 2, 2, 4, 0x2000 | 0x0400 }, /* LBAND */
-    {    1600000, 0, 1, 1, 2, 2, 4, 0x2000 | 0x0400 },
-    {    2000000, 0, 1, 1, 2, 2, 4, 0x2000 | 0x0400 },
-    { 0xffffffff, 0, 0, 8, 1, 2, 1, 0x8000 | 0x1000 }, /* SBAND */
+	{     570000, 2, 1, 3, 6, 6, 2, 0x4000 | 0x0800 }, /* UHF */
+	{     700000, 2, 0, 2, 4, 2, 2, 0x4000 | 0x0800 },
+	{     863999, 2, 1, 2, 4, 2, 2, 0x4000 | 0x0800 },
+	{    1500000, 0, 1, 1, 2, 2, 4, 0x2000 | 0x0400 }, /* LBAND */
+	{    1600000, 0, 1, 1, 2, 2, 4, 0x2000 | 0x0400 },
+	{    2000000, 0, 1, 1, 2, 2, 4, 0x2000 | 0x0400 },
+	{ 0xffffffff, 0, 0, 8, 1, 2, 1, 0x8000 | 0x1000 }, /* SBAND */
 };
 
 static const struct dib0070_tuning dib0070_tuning_table[] = {
-    {     115000, 1, 0, 7, 24, 2, 1, 0x8000 | 0x1000 }, /* FM below 92MHz cannot be tuned */
-    {     179500, 1, 0, 3, 16, 2, 1, 0x8000 | 0x1000 }, /* VHF */
-    {     189999, 1, 1, 3, 16, 2, 1, 0x8000 | 0x1000 },
-    {     250000, 1, 0, 6, 12, 2, 1, 0x8000 | 0x1000 },
-    {     569999, 2, 1, 5,  6, 2, 2, 0x4000 | 0x0800 }, /* UHF */
-    {     699999, 2, 0, 1,  4, 2, 2, 0x4000 | 0x0800 },
-    {     863999, 2, 1, 1,  4, 2, 2, 0x4000 | 0x0800 },
-    { 0xffffffff, 0, 1, 0,  2, 2, 4, 0x2000 | 0x0400 }, /* LBAND or everything higher than UHF */
+	{     115000, 1, 0, 7, 24, 2, 1, 0x8000 | 0x1000 }, /* FM below 92MHz cannot be tuned */
+	{     179500, 1, 0, 3, 16, 2, 1, 0x8000 | 0x1000 }, /* VHF */
+	{     189999, 1, 1, 3, 16, 2, 1, 0x8000 | 0x1000 },
+	{     250000, 1, 0, 6, 12, 2, 1, 0x8000 | 0x1000 },
+	{     569999, 2, 1, 5,  6, 2, 2, 0x4000 | 0x0800 }, /* UHF */
+	{     699999, 2, 0, 1,  4, 2, 2, 0x4000 | 0x0800 },
+	{     863999, 2, 1, 1,  4, 2, 2, 0x4000 | 0x0800 },
+	{ 0xffffffff, 0, 1, 0,  2, 2, 4, 0x2000 | 0x0400 }, /* LBAND or everything higher than UHF */
 };
 
 static const struct dib0070_lna_match dib0070_lna_flip_chip[] = {
-    {     180000, 0 }, /* VHF */
-    {     188000, 1 },
-    {     196400, 2 },
-    {     250000, 3 },
-    {     550000, 0 }, /* UHF */
-    {     590000, 1 },
-    {     666000, 3 },
-    {     864000, 5 },
-    {    1500000, 0 }, /* LBAND or everything higher than UHF */
-    {    1600000, 1 },
-    {    2000000, 3 },
-    { 0xffffffff, 7 },
+	{     180000, 0 }, /* VHF */
+	{     188000, 1 },
+	{     196400, 2 },
+	{     250000, 3 },
+	{     550000, 0 }, /* UHF */
+	{     590000, 1 },
+	{     666000, 3 },
+	{     864000, 5 },
+	{    1500000, 0 }, /* LBAND or everything higher than UHF */
+	{    1600000, 1 },
+	{    2000000, 3 },
+	{ 0xffffffff, 7 },
 };
 
 static const struct dib0070_lna_match dib0070_lna[] = {
-    {     180000, 0 }, /* VHF */
-    {     188000, 1 },
-    {     196400, 2 },
-    {     250000, 3 },
-    {     550000, 2 }, /* UHF */
-    {     650000, 3 },
-    {     750000, 5 },
-    {     850000, 6 },
-    {     864000, 7 },
-    {    1500000, 0 }, /* LBAND or everything higher than UHF */
-    {    1600000, 1 },
-    {    2000000, 3 },
-    { 0xffffffff, 7 },
+	{     180000, 0 }, /* VHF */
+	{     188000, 1 },
+	{     196400, 2 },
+	{     250000, 3 },
+	{     550000, 2 }, /* UHF */
+	{     650000, 3 },
+	{     750000, 5 },
+	{     850000, 6 },
+	{     864000, 7 },
+	{    1500000, 0 }, /* LBAND or everything higher than UHF */
+	{    1600000, 1 },
+	{    2000000, 3 },
+	{ 0xffffffff, 7 },
 };
 
 #define LPF	100
 static int dib0070_tune_digital(struct dvb_frontend *fe)
 {
-    struct dib0070_state *state = fe->tuner_priv;
+	struct dib0070_state *state = fe->tuner_priv;
 
-    const struct dib0070_tuning *tune;
-    const struct dib0070_lna_match *lna_match;
+	const struct dib0070_tuning *tune;
+	const struct dib0070_lna_match *lna_match;
 
-    enum frontend_tune_state *tune_state = &state->tune_state;
-    int ret = 10; /* 1ms is the default delay most of the time */
+	enum frontend_tune_state *tune_state = &state->tune_state;
+	int ret = 10; /* 1ms is the default delay most of the time */
 
-    u8  band = (u8)BAND_OF_FREQUENCY(fe->dtv_property_cache.frequency/1000);
-    u32 freq = fe->dtv_property_cache.frequency/1000 + (band == BAND_VHF ? state->cfg->freq_offset_khz_vhf : state->cfg->freq_offset_khz_uhf);
+	u8  band = (u8)BAND_OF_FREQUENCY(fe->dtv_property_cache.frequency/1000);
+	u32 freq = fe->dtv_property_cache.frequency/1000 + (band == BAND_VHF ? state->cfg->freq_offset_khz_vhf : state->cfg->freq_offset_khz_uhf);
 
 #ifdef CONFIG_SYS_ISDBT
-    if (state->fe->dtv_property_cache.delivery_system == SYS_ISDBT && state->fe->dtv_property_cache.isdbt_sb_mode == 1)
-		if (((state->fe->dtv_property_cache.isdbt_sb_segment_count % 2)
-		     && (state->fe->dtv_property_cache.isdbt_sb_segment_idx == ((state->fe->dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
-		    || (((state->fe->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-			&& (state->fe->dtv_property_cache.isdbt_sb_segment_idx == (state->fe->dtv_property_cache.isdbt_sb_segment_count / 2)))
-		    || (((state->fe->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
-			&& (state->fe->dtv_property_cache.isdbt_sb_segment_idx == ((state->fe->dtv_property_cache.isdbt_sb_segment_count / 2) + 1))))
-			freq += 850;
+	if (state->fe->dtv_property_cache.delivery_system == SYS_ISDBT && state->fe->dtv_property_cache.isdbt_sb_mode == 1)
+			if (((state->fe->dtv_property_cache.isdbt_sb_segment_count % 2)
+			&& (state->fe->dtv_property_cache.isdbt_sb_segment_idx == ((state->fe->dtv_property_cache.isdbt_sb_segment_count / 2) + 1)))
+			|| (((state->fe->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
+				&& (state->fe->dtv_property_cache.isdbt_sb_segment_idx == (state->fe->dtv_property_cache.isdbt_sb_segment_count / 2)))
+			|| (((state->fe->dtv_property_cache.isdbt_sb_segment_count % 2) == 0)
+				&& (state->fe->dtv_property_cache.isdbt_sb_segment_idx == ((state->fe->dtv_property_cache.isdbt_sb_segment_count / 2) + 1))))
+				freq += 850;
 #endif
-    if (state->current_rf != freq) {
-
-	switch (state->revision) {
-	case DIB0070S_P1A:
-	    tune = dib0070s_tuning_table;
-	    lna_match = dib0070_lna;
-	    break;
-	default:
-	    tune = dib0070_tuning_table;
-	    if (state->cfg->flip_chip)
-		lna_match = dib0070_lna_flip_chip;
-	    else
-		lna_match = dib0070_lna;
-	    break;
-	}
-	while (freq > tune->max_freq) /* find the right one */
-	    tune++;
-	while (freq > lna_match->max_freq) /* find the right one */
-	    lna_match++;
-
-	state->current_tune_table_index = tune;
-	state->lna_match = lna_match;
-    }
-
-    if (*tune_state == CT_TUNER_START) {
-	dprintk("Tuning for Band: %hd (%d kHz)", band, freq);
 	if (state->current_rf != freq) {
-		u8 REFDIV;
-		u32 FBDiv, Rest, FREF, VCOF_kHz;
-		u8 Den;
-
-		state->current_rf = freq;
-		state->lo4 = (state->current_tune_table_index->vco_band << 11) | (state->current_tune_table_index->hfdiv << 7);
-
-
-		dib0070_write_reg(state, 0x17, 0x30);
-
-
-		VCOF_kHz = state->current_tune_table_index->vco_multi * freq * 2;
-
-		switch (band) {
-		case BAND_VHF:
-			REFDIV = (u8) ((state->cfg->clock_khz + 9999) / 10000);
-			break;
-		case BAND_FM:
-			REFDIV = (u8) ((state->cfg->clock_khz) / 1000);
-			break;
-		default:
-			REFDIV = (u8) (state->cfg->clock_khz  / 10000);
-			break;
-		}
-		FREF = state->cfg->clock_khz / REFDIV;
-
-
 
 		switch (state->revision) {
 		case DIB0070S_P1A:
-			FBDiv = (VCOF_kHz / state->current_tune_table_index->presc / FREF);
-			Rest  = (VCOF_kHz / state->current_tune_table_index->presc) - FBDiv * FREF;
-			break;
-
-		case DIB0070_P1G:
-		case DIB0070_P1F:
+		tune = dib0070s_tuning_table;
+		lna_match = dib0070_lna;
+		break;
 		default:
-			FBDiv = (freq / (FREF / 2));
-			Rest  = 2 * freq - FBDiv * FREF;
-			break;
-		}
-
-		if (Rest < LPF)
-			Rest = 0;
-		else if (Rest < 2 * LPF)
-			Rest = 2 * LPF;
-		else if (Rest > (FREF - LPF)) {
-			Rest = 0;
-			FBDiv += 1;
-		} else if (Rest > (FREF - 2 * LPF))
-			Rest = FREF - 2 * LPF;
-		Rest = (Rest * 6528) / (FREF / 10);
-
-		Den = 1;
-		if (Rest > 0) {
-			state->lo4 |= (1 << 14) | (1 << 12);
-			Den = 255;
+		tune = dib0070_tuning_table;
+		if (state->cfg->flip_chip)
+			lna_match = dib0070_lna_flip_chip;
+		else
+			lna_match = dib0070_lna;
+		break;
 		}
+		while (freq > tune->max_freq) /* find the right one */
+			tune++;
+		while (freq > lna_match->max_freq) /* find the right one */
+			lna_match++;
 
+		state->current_tune_table_index = tune;
+		state->lna_match = lna_match;
+	}
 
-		dib0070_write_reg(state, 0x11, (u16)FBDiv);
-		dib0070_write_reg(state, 0x12, (Den << 8) | REFDIV);
-		dib0070_write_reg(state, 0x13, (u16) Rest);
-
-		if (state->revision == DIB0070S_P1A) {
-
-			if (band == BAND_SBAND) {
-				dib0070_set_ctrl_lo5(fe, 2, 4, 3, 0);
-				dib0070_write_reg(state, 0x1d, 0xFFFF);
-			} else
-				dib0070_set_ctrl_lo5(fe, 5, 4, 3, 1);
+	if (*tune_state == CT_TUNER_START) {
+		dprintk("Tuning for Band: %hd (%d kHz)", band, freq);
+		if (state->current_rf != freq) {
+			u8 REFDIV;
+			u32 FBDiv, Rest, FREF, VCOF_kHz;
+			u8 Den;
+
+			state->current_rf = freq;
+			state->lo4 = (state->current_tune_table_index->vco_band << 11) | (state->current_tune_table_index->hfdiv << 7);
+
+
+			dib0070_write_reg(state, 0x17, 0x30);
+
+
+			VCOF_kHz = state->current_tune_table_index->vco_multi * freq * 2;
+
+			switch (band) {
+			case BAND_VHF:
+				REFDIV = (u8) ((state->cfg->clock_khz + 9999) / 10000);
+				break;
+			case BAND_FM:
+				REFDIV = (u8) ((state->cfg->clock_khz) / 1000);
+				break;
+			default:
+				REFDIV = (u8) (state->cfg->clock_khz  / 10000);
+				break;
+			}
+			FREF = state->cfg->clock_khz / REFDIV;
+
+
+
+			switch (state->revision) {
+			case DIB0070S_P1A:
+				FBDiv = (VCOF_kHz / state->current_tune_table_index->presc / FREF);
+				Rest  = (VCOF_kHz / state->current_tune_table_index->presc) - FBDiv * FREF;
+				break;
+
+			case DIB0070_P1G:
+			case DIB0070_P1F:
+			default:
+				FBDiv = (freq / (FREF / 2));
+				Rest  = 2 * freq - FBDiv * FREF;
+				break;
+			}
+
+			if (Rest < LPF)
+				Rest = 0;
+			else if (Rest < 2 * LPF)
+				Rest = 2 * LPF;
+			else if (Rest > (FREF - LPF)) {
+				Rest = 0;
+				FBDiv += 1;
+			} else if (Rest > (FREF - 2 * LPF))
+				Rest = FREF - 2 * LPF;
+			Rest = (Rest * 6528) / (FREF / 10);
+
+			Den = 1;
+			if (Rest > 0) {
+				state->lo4 |= (1 << 14) | (1 << 12);
+				Den = 255;
+			}
+
+
+			dib0070_write_reg(state, 0x11, (u16)FBDiv);
+			dib0070_write_reg(state, 0x12, (Den << 8) | REFDIV);
+			dib0070_write_reg(state, 0x13, (u16) Rest);
+
+			if (state->revision == DIB0070S_P1A) {
+
+				if (band == BAND_SBAND) {
+					dib0070_set_ctrl_lo5(fe, 2, 4, 3, 0);
+					dib0070_write_reg(state, 0x1d, 0xFFFF);
+				} else
+					dib0070_set_ctrl_lo5(fe, 5, 4, 3, 1);
+			}
+
+			dib0070_write_reg(state, 0x20,
+				0x0040 | 0x0020 | 0x0010 | 0x0008 | 0x0002 | 0x0001 | state->current_tune_table_index->tuner_enable);
+
+			dprintk("REFDIV: %hd, FREF: %d", REFDIV, FREF);
+			dprintk("FBDIV: %d, Rest: %d", FBDiv, Rest);
+			dprintk("Num: %hd, Den: %hd, SD: %hd", (u16) Rest, Den, (state->lo4 >> 12) & 0x1);
+			dprintk("HFDIV code: %hd", state->current_tune_table_index->hfdiv);
+			dprintk("VCO = %hd", state->current_tune_table_index->vco_band);
+			dprintk("VCOF: ((%hd*%d) << 1))", state->current_tune_table_index->vco_multi, freq);
+
+			*tune_state = CT_TUNER_STEP_0;
+		} else { /* we are already tuned to this frequency - the configuration is correct  */
+			ret = 50; /* wakeup time */
+			*tune_state = CT_TUNER_STEP_5;
 		}
+	} else if ((*tune_state > CT_TUNER_START) && (*tune_state < CT_TUNER_STEP_4)) {
 
-		dib0070_write_reg(state, 0x20,
-			0x0040 | 0x0020 | 0x0010 | 0x0008 | 0x0002 | 0x0001 | state->current_tune_table_index->tuner_enable);
-
-		dprintk("REFDIV: %hd, FREF: %d", REFDIV, FREF);
-		dprintk("FBDIV: %d, Rest: %d", FBDiv, Rest);
-		dprintk("Num: %hd, Den: %hd, SD: %hd", (u16) Rest, Den, (state->lo4 >> 12) & 0x1);
-		dprintk("HFDIV code: %hd", state->current_tune_table_index->hfdiv);
-		dprintk("VCO = %hd", state->current_tune_table_index->vco_band);
-		dprintk("VCOF: ((%hd*%d) << 1))", state->current_tune_table_index->vco_multi, freq);
-
-		*tune_state = CT_TUNER_STEP_0;
-	} else { /* we are already tuned to this frequency - the configuration is correct  */
-		ret = 50; /* wakeup time */
-		*tune_state = CT_TUNER_STEP_5;
-	}
-    } else if ((*tune_state > CT_TUNER_START) && (*tune_state < CT_TUNER_STEP_4)) {
-
-	ret = dib0070_captrim(state, tune_state);
+		ret = dib0070_captrim(state, tune_state);
 
-    } else if (*tune_state == CT_TUNER_STEP_4) {
-	const struct dib0070_wbd_gain_cfg *tmp = state->cfg->wbd_gain;
-	if (tmp != NULL) {
-		while (freq/1000 > tmp->freq) /* find the right one */
-			tmp++;
-		dib0070_write_reg(state, 0x0f,
-			(0 << 15) | (1 << 14) | (3 << 12)
-			| (tmp->wbd_gain_val << 9) | (0 << 8) | (1 << 7)
-			| (state->current_tune_table_index->wbdmux << 0));
-		state->wbd_gain_current = tmp->wbd_gain_val;
-	} else {
+	} else if (*tune_state == CT_TUNER_STEP_4) {
+		const struct dib0070_wbd_gain_cfg *tmp = state->cfg->wbd_gain;
+		if (tmp != NULL) {
+			while (freq/1000 > tmp->freq) /* find the right one */
+				tmp++;
 			dib0070_write_reg(state, 0x0f,
-					  (0 << 15) | (1 << 14) | (3 << 12) | (6 << 9) | (0 << 8) | (1 << 7) | (state->current_tune_table_index->
-														wbdmux << 0));
-	    state->wbd_gain_current = 6;
-	}
+				(0 << 15) | (1 << 14) | (3 << 12)
+				| (tmp->wbd_gain_val << 9) | (0 << 8) | (1 << 7)
+				| (state->current_tune_table_index->wbdmux << 0));
+			state->wbd_gain_current = tmp->wbd_gain_val;
+		} else {
+			dib0070_write_reg(state, 0x0f,
+					  (0 << 15) | (1 << 14) | (3 << 12)
+					  | (6 << 9) | (0 << 8) | (1 << 7)
+					  | (state->current_tune_table_index->wbdmux << 0));
+			state->wbd_gain_current = 6;
+		}
 
-	dib0070_write_reg(state, 0x06, 0x3fff);
+		dib0070_write_reg(state, 0x06, 0x3fff);
 		dib0070_write_reg(state, 0x07,
 				  (state->current_tune_table_index->switch_trim << 11) | (7 << 8) | (state->lna_match->lna_band << 3) | (3 << 0));
-	dib0070_write_reg(state, 0x08, (state->lna_match->lna_band << 10) | (3 << 7) | (127));
-	dib0070_write_reg(state, 0x0d, 0x0d80);
+		dib0070_write_reg(state, 0x08, (state->lna_match->lna_band << 10) | (3 << 7) | (127));
+		dib0070_write_reg(state, 0x0d, 0x0d80);
 
 
-	dib0070_write_reg(state, 0x18,   0x07ff);
-	dib0070_write_reg(state, 0x17, 0x0033);
+		dib0070_write_reg(state, 0x18,   0x07ff);
+		dib0070_write_reg(state, 0x17, 0x0033);
 
 
-	*tune_state = CT_TUNER_STEP_5;
-    } else if (*tune_state == CT_TUNER_STEP_5) {
-	dib0070_set_bandwidth(fe);
-	*tune_state = CT_TUNER_STOP;
-    } else {
-	ret = FE_CALLBACK_TIME_NEVER; /* tuner finished, time to call again infinite */
-    }
-    return ret;
+		*tune_state = CT_TUNER_STEP_5;
+	} else if (*tune_state == CT_TUNER_STEP_5) {
+		dib0070_set_bandwidth(fe);
+		*tune_state = CT_TUNER_STOP;
+	} else {
+		ret = FE_CALLBACK_TIME_NEVER; /* tuner finished, time to call again infinite */
+	}
+	return ret;
 }
 
 
 static int dib0070_tune(struct dvb_frontend *fe)
 {
-    struct dib0070_state *state = fe->tuner_priv;
-    uint32_t ret;
+	struct dib0070_state *state = fe->tuner_priv;
+	uint32_t ret;
 
-    state->tune_state = CT_TUNER_START;
+	state->tune_state = CT_TUNER_START;
 
-    do {
-	ret = dib0070_tune_digital(fe);
-	if (ret != FE_CALLBACK_TIME_NEVER)
-		msleep(ret/10);
-	else
-	    break;
-    } while (state->tune_state != CT_TUNER_STOP);
+	do {
+		ret = dib0070_tune_digital(fe);
+		if (ret != FE_CALLBACK_TIME_NEVER)
+			msleep(ret/10);
+		else
+		break;
+	} while (state->tune_state != CT_TUNER_STOP);
 
-    return 0;
+	return 0;
 }
 
 static int dib0070_wakeup(struct dvb_frontend *fe)
@@ -610,48 +608,48 @@ static const u16 dib0070_p1f_defaults[] =
 
 static u16 dib0070_read_wbd_offset(struct dib0070_state *state, u8 gain)
 {
-    u16 tuner_en = dib0070_read_reg(state, 0x20);
-    u16 offset;
-
-    dib0070_write_reg(state, 0x18, 0x07ff);
-    dib0070_write_reg(state, 0x20, 0x0800 | 0x4000 | 0x0040 | 0x0020 | 0x0010 | 0x0008 | 0x0002 | 0x0001);
-    dib0070_write_reg(state, 0x0f, (1 << 14) | (2 << 12) | (gain << 9) | (1 << 8) | (1 << 7) | (0 << 0));
-    msleep(9);
-    offset = dib0070_read_reg(state, 0x19);
-    dib0070_write_reg(state, 0x20, tuner_en);
-    return offset;
+	u16 tuner_en = dib0070_read_reg(state, 0x20);
+	u16 offset;
+
+	dib0070_write_reg(state, 0x18, 0x07ff);
+	dib0070_write_reg(state, 0x20, 0x0800 | 0x4000 | 0x0040 | 0x0020 | 0x0010 | 0x0008 | 0x0002 | 0x0001);
+	dib0070_write_reg(state, 0x0f, (1 << 14) | (2 << 12) | (gain << 9) | (1 << 8) | (1 << 7) | (0 << 0));
+	msleep(9);
+	offset = dib0070_read_reg(state, 0x19);
+	dib0070_write_reg(state, 0x20, tuner_en);
+	return offset;
 }
 
 static void dib0070_wbd_offset_calibration(struct dib0070_state *state)
 {
-    u8 gain;
-    for (gain = 6; gain < 8; gain++) {
-	state->wbd_offset_3_3[gain - 6] = ((dib0070_read_wbd_offset(state, gain) * 8 * 18 / 33 + 1) / 2);
-	dprintk("Gain: %d, WBDOffset (3.3V) = %hd", gain, state->wbd_offset_3_3[gain-6]);
-    }
+	u8 gain;
+	for (gain = 6; gain < 8; gain++) {
+		state->wbd_offset_3_3[gain - 6] = ((dib0070_read_wbd_offset(state, gain) * 8 * 18 / 33 + 1) / 2);
+		dprintk("Gain: %d, WBDOffset (3.3V) = %hd", gain, state->wbd_offset_3_3[gain-6]);
+	}
 }
 
 u16 dib0070_wbd_offset(struct dvb_frontend *fe)
 {
-    struct dib0070_state *state = fe->tuner_priv;
-    const struct dib0070_wbd_gain_cfg *tmp = state->cfg->wbd_gain;
-    u32 freq = fe->dtv_property_cache.frequency/1000;
-
-    if (tmp != NULL) {
-	while (freq/1000 > tmp->freq) /* find the right one */
-	    tmp++;
-	state->wbd_gain_current = tmp->wbd_gain_val;
+	struct dib0070_state *state = fe->tuner_priv;
+	const struct dib0070_wbd_gain_cfg *tmp = state->cfg->wbd_gain;
+	u32 freq = fe->dtv_property_cache.frequency/1000;
+
+	if (tmp != NULL) {
+		while (freq/1000 > tmp->freq) /* find the right one */
+			tmp++;
+		state->wbd_gain_current = tmp->wbd_gain_val;
 	} else
-	state->wbd_gain_current = 6;
+		state->wbd_gain_current = 6;
 
-    return state->wbd_offset_3_3[state->wbd_gain_current - 6];
+	return state->wbd_offset_3_3[state->wbd_gain_current - 6];
 }
 EXPORT_SYMBOL(dib0070_wbd_offset);
 
 #define pgm_read_word(w) (*w)
 static int dib0070_reset(struct dvb_frontend *fe)
 {
-    struct dib0070_state *state = fe->tuner_priv;
+	struct dib0070_state *state = fe->tuner_priv;
 	u16 l, r, *n;
 
 	HARD_RESET(state);
@@ -664,7 +662,7 @@ static int dib0070_reset(struct dvb_frontend *fe)
 #else
 #warning forcing SBAND
 #endif
-		state->revision = DIB0070S_P1A;
+	state->revision = DIB0070S_P1A;
 
 	/* P1F or not */
 	dprintk("Revision: %x", state->revision);
@@ -703,24 +701,25 @@ static int dib0070_reset(struct dvb_frontend *fe)
 		dib0070_write_reg(state, 0x02, r | (1 << 5));
 	}
 
-    if (state->revision == DIB0070S_P1A)
-	dib0070_set_ctrl_lo5(fe, 2, 4, 3, 0);
-    else
-		dib0070_set_ctrl_lo5(fe, 5, 4, state->cfg->charge_pump, state->cfg->enable_third_order_filter);
+	if (state->revision == DIB0070S_P1A)
+		dib0070_set_ctrl_lo5(fe, 2, 4, 3, 0);
+	else
+		dib0070_set_ctrl_lo5(fe, 5, 4, state->cfg->charge_pump,
+				     state->cfg->enable_third_order_filter);
 
 	dib0070_write_reg(state, 0x01, (54 << 9) | 0xc8);
 
-    dib0070_wbd_offset_calibration(state);
+	dib0070_wbd_offset_calibration(state);
 
-    return 0;
+	return 0;
 }
 
 static int dib0070_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
-    struct dib0070_state *state = fe->tuner_priv;
+	struct dib0070_state *state = fe->tuner_priv;
 
-    *frequency = 1000 * state->current_rf;
-    return 0;
+	*frequency = 1000 * state->current_rf;
+	return 0;
 }
 
 static int dib0070_release(struct dvb_frontend *fe)
diff --git a/drivers/media/dvb-frontends/dib0090.c b/drivers/media/dvb-frontends/dib0090.c
index 68e2af2650d3..47cb72243b9d 100644
--- a/drivers/media/dvb-frontends/dib0090.c
+++ b/drivers/media/dvb-frontends/dib0090.c
@@ -1696,12 +1696,10 @@ static int dib0090_dc_offset_calibration(struct dib0090_state *state, enum front
 
 		if (state->identity.p1g)
 			state->dc = dc_p1g_table;
-		*tune_state = CT_TUNER_STEP_0;
 
 		/* fall through */
-
 	case CT_TUNER_STEP_0:
-		dprintk("Sart/continue DC calibration for %s path", (state->dc->i == 1) ? "I" : "Q");
+		dprintk("Start/continue DC calibration for %s path", (state->dc->i == 1) ? "I" : "Q");
 		dib0090_write_reg(state, 0x01, state->dc->bb1);
 		dib0090_write_reg(state, 0x07, state->bb7 | (state->dc->i << 7));
 
diff --git a/drivers/media/dvb-frontends/dib3000mb.c b/drivers/media/dvb-frontends/dib3000mb.c
index af91e0c92339..7a61172d0d45 100644
--- a/drivers/media/dvb-frontends/dib3000mb.c
+++ b/drivers/media/dvb-frontends/dib3000mb.c
@@ -118,7 +118,7 @@ static int dib3000mb_set_frontend(struct dvb_frontend *fe, int tuner)
 {
 	struct dib3000_state* state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	fe_code_rate_t fe_cr = FEC_NONE;
+	enum fe_code_rate fe_cr = FEC_NONE;
 	int search_state, seq;
 
 	if (tuner && fe->ops.tuner_ops.set_params) {
@@ -454,7 +454,7 @@ static int dib3000mb_get_frontend(struct dvb_frontend* fe)
 {
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct dib3000_state* state = fe->demodulator_priv;
-	fe_code_rate_t *cr;
+	enum fe_code_rate *cr;
 	u16 tps_val;
 	int inv_test1,inv_test2;
 	u32 dds_val, threshold = 0x800000;
@@ -611,7 +611,8 @@ static int dib3000mb_get_frontend(struct dvb_frontend* fe)
 	return 0;
 }
 
-static int dib3000mb_read_status(struct dvb_frontend* fe, fe_status_t *stat)
+static int dib3000mb_read_status(struct dvb_frontend *fe,
+				 enum fe_status *stat)
 {
 	struct dib3000_state* state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/dib3000mc.c b/drivers/media/dvb-frontends/dib3000mc.c
index ffad181a9692..583d6b7fabed 100644
--- a/drivers/media/dvb-frontends/dib3000mc.c
+++ b/drivers/media/dvb-frontends/dib3000mc.c
@@ -131,7 +131,7 @@ static int dib3000mc_set_timing(struct dib3000mc_state *state, s16 nfft, u32 bw,
 static int dib3000mc_setup_pwm_state(struct dib3000mc_state *state)
 {
 	u16 reg_51, reg_52 = state->cfg->agc->setup & 0xfefb;
-    if (state->cfg->pwm3_inversion) {
+	if (state->cfg->pwm3_inversion) {
 		reg_51 =  (2 << 14) | (0 << 10) | (7 << 6) | (2 << 2) | (2 << 0);
 		reg_52 |= (1 << 2);
 	} else {
@@ -141,12 +141,12 @@ static int dib3000mc_setup_pwm_state(struct dib3000mc_state *state)
 	dib3000mc_write_word(state, 51, reg_51);
 	dib3000mc_write_word(state, 52, reg_52);
 
-    if (state->cfg->use_pwm3)
+	if (state->cfg->use_pwm3)
 		dib3000mc_write_word(state, 245, (1 << 3) | (1 << 0));
 	else
 		dib3000mc_write_word(state, 245, 0);
 
-    dib3000mc_write_word(state, 1040, 0x3);
+	dib3000mc_write_word(state, 1040, 0x3);
 	return 0;
 }
 
@@ -417,7 +417,7 @@ static int dib3000mc_sleep(struct dvb_frontend *demod)
 	dib3000mc_write_word(state, 1032, 0xFFFF);
 	dib3000mc_write_word(state, 1033, 0xFFF0);
 
-    return 0;
+	return 0;
 }
 
 static void dib3000mc_set_adp_cfg(struct dib3000mc_state *state, s16 qam)
@@ -447,10 +447,14 @@ static void dib3000mc_set_channel_cfg(struct dib3000mc_state *state,
 	dib3000mc_set_bandwidth(state, bw);
 	dib3000mc_set_timing(state, ch->transmission_mode, bw, 0);
 
-//	if (boost)
-//		dib3000mc_write_word(state, 100, (11 << 6) + 6);
-//	else
+#if 1
+	dib3000mc_write_word(state, 100, (16 << 6) + 9);
+#else
+	if (boost)
+		dib3000mc_write_word(state, 100, (11 << 6) + 6);
+	else
 		dib3000mc_write_word(state, 100, (16 << 6) + 9);
+#endif
 
 	dib3000mc_write_word(state, 1027, 0x0800);
 	dib3000mc_write_word(state, 1027, 0x0000);
@@ -732,7 +736,7 @@ static int dib3000mc_set_frontend(struct dvb_frontend *fe)
 	return ret;
 }
 
-static int dib3000mc_read_status(struct dvb_frontend *fe, fe_status_t *stat)
+static int dib3000mc_read_status(struct dvb_frontend *fe, enum fe_status *stat)
 {
 	struct dib3000mc_state *state = fe->demodulator_priv;
 	u16 lock = dib3000mc_read_word(state, 509);
diff --git a/drivers/media/dvb-frontends/dib7000m.c b/drivers/media/dvb-frontends/dib7000m.c
index dcb9a15ef0c2..35eb71fe3c2b 100644
--- a/drivers/media/dvb-frontends/dib7000m.c
+++ b/drivers/media/dvb-frontends/dib7000m.c
@@ -1256,7 +1256,7 @@ static int dib7000m_set_frontend(struct dvb_frontend *fe)
 	return ret;
 }
 
-static int dib7000m_read_status(struct dvb_frontend *fe, fe_status_t *stat)
+static int dib7000m_read_status(struct dvb_frontend *fe, enum fe_status *stat)
 {
 	struct dib7000m_state *state = fe->demodulator_priv;
 	u16 lock = dib7000m_read_word(state, 535);
diff --git a/drivers/media/dvb-frontends/dib7000p.c b/drivers/media/dvb-frontends/dib7000p.c
index c505d696f92d..33be5d6b9e10 100644
--- a/drivers/media/dvb-frontends/dib7000p.c
+++ b/drivers/media/dvb-frontends/dib7000p.c
@@ -1558,9 +1558,9 @@ static int dib7000p_set_frontend(struct dvb_frontend *fe)
 	return ret;
 }
 
-static int dib7000p_get_stats(struct dvb_frontend *fe, fe_status_t stat);
+static int dib7000p_get_stats(struct dvb_frontend *fe, enum fe_status stat);
 
-static int dib7000p_read_status(struct dvb_frontend *fe, fe_status_t * stat)
+static int dib7000p_read_status(struct dvb_frontend *fe, enum fe_status *stat)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
 	u16 lock = dib7000p_read_word(state, 509);
@@ -1877,7 +1877,7 @@ static u32 dib7000p_get_time_us(struct dvb_frontend *demod)
 	return time_us;
 }
 
-static int dib7000p_get_stats(struct dvb_frontend *demod, fe_status_t stat)
+static int dib7000p_get_stats(struct dvb_frontend *demod, enum fe_status stat)
 {
 	struct dib7000p_state *state = demod->demodulator_priv;
 	struct dtv_frontend_properties *c = &demod->dtv_property_cache;
diff --git a/drivers/media/dvb-frontends/dib8000.c b/drivers/media/dvb-frontends/dib8000.c
index 8c6663b6399d..94c26270fff0 100644
--- a/drivers/media/dvb-frontends/dib8000.c
+++ b/drivers/media/dvb-frontends/dib8000.c
@@ -3380,13 +3380,13 @@ static int dib8000_sleep(struct dvb_frontend *fe)
 	return dib8000_set_adc_state(state, DIBX000_SLOW_ADC_OFF) | dib8000_set_adc_state(state, DIBX000_ADC_OFF);
 }
 
-static int dib8000_read_status(struct dvb_frontend *fe, fe_status_t * stat);
+static int dib8000_read_status(struct dvb_frontend *fe, enum fe_status *stat);
 
 static int dib8000_get_frontend(struct dvb_frontend *fe)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 i, val = 0;
-	fe_status_t stat = 0;
+	enum fe_status stat = 0;
 	u8 index_frontend, sub_index_frontend;
 
 	fe->dtv_property_cache.bandwidth_hz = 6000000;
@@ -3733,9 +3733,9 @@ static int dib8000_set_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int dib8000_get_stats(struct dvb_frontend *fe, fe_status_t stat);
+static int dib8000_get_stats(struct dvb_frontend *fe, enum fe_status stat);
 
-static int dib8000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
+static int dib8000_read_status(struct dvb_frontend *fe, enum fe_status *stat)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	u16 lock_slave = 0, lock;
@@ -4089,7 +4089,7 @@ static u32 dib8000_get_time_us(struct dvb_frontend *fe, int layer)
 	return time_us;
 }
 
-static int dib8000_get_stats(struct dvb_frontend *fe, fe_status_t stat)
+static int dib8000_get_stats(struct dvb_frontend *fe, enum fe_status stat)
 {
 	struct dib8000_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &state->fe[0]->dtv_property_cache;
diff --git a/drivers/media/dvb-frontends/dib8000.h b/drivers/media/dvb-frontends/dib8000.h
index 780c37bdcb72..2b8b4b1656a2 100644
--- a/drivers/media/dvb-frontends/dib8000.h
+++ b/drivers/media/dvb-frontends/dib8000.h
@@ -66,7 +66,7 @@ struct dib8000_ops {
 #if IS_REACHABLE(CONFIG_DVB_DIB8000)
 void *dib8000_attach(struct dib8000_ops *ops);
 #else
-static inline int dib8000_attach(struct dib8000_ops *ops)
+static inline void *dib8000_attach(struct dib8000_ops *ops)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
diff --git a/drivers/media/dvb-frontends/dib9000.c b/drivers/media/dvb-frontends/dib9000.c
index f75dec443783..8f92aca0b073 100644
--- a/drivers/media/dvb-frontends/dib9000.c
+++ b/drivers/media/dvb-frontends/dib9000.c
@@ -1893,7 +1893,7 @@ static int dib9000_get_frontend(struct dvb_frontend *fe)
 {
 	struct dib9000_state *state = fe->demodulator_priv;
 	u8 index_frontend, sub_index_frontend;
-	fe_status_t stat;
+	enum fe_status stat;
 	int ret = 0;
 
 	if (state->get_frontend_internal == 0) {
@@ -2161,7 +2161,7 @@ static u16 dib9000_read_lock(struct dvb_frontend *fe)
 	return dib9000_read_word(state, 535);
 }
 
-static int dib9000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
+static int dib9000_read_status(struct dvb_frontend *fe, enum fe_status *stat)
 {
 	struct dib9000_state *state = fe->demodulator_priv;
 	u8 index_frontend;
diff --git a/drivers/media/dvb-frontends/drx39xyj/drxj.c b/drivers/media/dvb-frontends/drx39xyj/drxj.c
index 2bfa7a435974..b28b5787b39a 100644
--- a/drivers/media/dvb-frontends/drx39xyj/drxj.c
+++ b/drivers/media/dvb-frontends/drx39xyj/drxj.c
@@ -210,7 +210,7 @@ DEFINES
 
 /**
 * \def DRXJ_DEF_I2C_ADDR
-* \brief Default I2C addres of a demodulator instance.
+* \brief Default I2C address of a demodulator instance.
 */
 #define DRXJ_DEF_I2C_ADDR (0x52)
 
@@ -336,7 +336,7 @@ DEFINES
  * MICROCODE RELATED DEFINES
  */
 
-/* Magic word for checking correct Endianess of microcode data */
+/* Magic word for checking correct Endianness of microcode data */
 #define DRX_UCODE_MAGIC_WORD         ((((u16)'H')<<8)+((u16)'L'))
 
 /* CRC flag in ucode header, flags field. */
@@ -847,9 +847,9 @@ static struct drx_common_attr drxj_default_comm_attr_g = {
 				   static clockrate is selected */
 	 DRX_MPEG_STR_WIDTH_1	/* MPEG Start width in clock cycles */
 	 },
-	/* Initilisations below can be ommited, they require no user input and
+	/* Initilisations below can be omitted, they require no user input and
 	   are initialy 0, NULL or false. The compiler will initialize them to these
-	   values when ommited.  */
+	   values when omitted.  */
 	false,			/* is_opened */
 
 	/* SCAN */
@@ -1175,7 +1175,7 @@ static u32 log1_times100(u32 x)
 	   Now x has binary point between bit[scale] and bit[scale-1]
 	   and 1.0 <= x < 2.0 */
 
-	/* correction for divison: log(x) = log(x/y)+log(y) */
+	/* correction for division: log(x) = log(x/y)+log(y) */
 	y = k * ((((u32) 1) << scale) * 200);
 
 	/* remove integer part */
@@ -1653,7 +1653,7 @@ static int drxdap_fasi_write_block(struct i2c_device_addr *dev_addr,
 		   sequense will be visible: (1) write address {i2c addr,
 		   4 bytes chip address} (2) write data {i2c addr, 4 bytes data }
 		   (3) write address (4) write data etc...
-		   Addres must be rewriten because HI is reset after data transport and
+		   Address must be rewriten because HI is reset after data transport and
 		   expects an address.
 		 */
 		todo = (block_size < datasize ? block_size : datasize);
@@ -2971,7 +2971,7 @@ ctrl_set_cfg_mpeg_output(struct drx_demod_instance *demod, struct drx_cfg_mpeg_o
 			}	/* ext_attr->standard */
 		}
 
-		if (cfg_data->enable_parallel == true) {	/* MPEG data output is paralel -> clear ipr_mode[0] */
+		if (cfg_data->enable_parallel == true) {	/* MPEG data output is parallel -> clear ipr_mode[0] */
 			fec_oc_reg_ipr_mode &= (~(FEC_OC_IPR_MODE_SERIAL__M));
 		} else {	/* MPEG data output is serial -> set ipr_mode[0] */
 			fec_oc_reg_ipr_mode |= FEC_OC_IPR_MODE_SERIAL__M;
@@ -3157,7 +3157,7 @@ ctrl_set_cfg_mpeg_output(struct drx_demod_instance *demod, struct drx_cfg_mpeg_o
 			pr_err("error %d\n", rc);
 			goto rw_error;
 		}
-		if (cfg_data->enable_parallel == true) {	/* MPEG data output is paralel -> set MD1 to MD7 to output mode */
+		if (cfg_data->enable_parallel == true) {	/* MPEG data output is parallel -> set MD1 to MD7 to output mode */
 			sio_pdr_md_cfg =
 			    MPEG_PARALLEL_OUTPUT_PIN_DRIVE_STRENGTH <<
 			    SIO_PDR_MD0_CFG_DRIVE__B | 0x03 <<
@@ -4320,7 +4320,7 @@ static int adc_synchronization(struct drx_demod_instance *demod)
 	}
 
 	if (count == 1) {
-		/* Try sampling on a diffrent edge */
+		/* Try sampling on a different edge */
 		u16 clk_neg = 0;
 
 		rc = drxj_dap_read_reg16(dev_addr, IQM_AF_CLKNEG__A, &clk_neg, 0);
@@ -6461,7 +6461,7 @@ set_qam_measurement(struct drx_demod_instance *demod,
 		    enum drx_modulation constellation, u32 symbol_rate)
 {
 	struct i2c_device_addr *dev_addr = NULL;	/* device address for I2C writes */
-	struct drxj_data *ext_attr = NULL;	/* Global data container for DRXJ specif data */
+	struct drxj_data *ext_attr = NULL;	/* Global data container for DRXJ specific data */
 	int rc;
 	u32 fec_bits_desired = 0;	/* BER accounting period */
 	u16 fec_rs_plen = 0;	/* defines RS BER measurement period */
@@ -8864,7 +8864,7 @@ qam64auto(struct drx_demod_instance *demod,
 	u32 timeout_ofs = 0;
 	u16 data = 0;
 
-	/* external attributes for storing aquired channel constellation */
+	/* external attributes for storing acquired channel constellation */
 	*lock_status = DRX_NOT_LOCKED;
 	start_time = jiffies_to_msecs(jiffies);
 	lck_state = NO_LOCK;
@@ -9011,7 +9011,7 @@ qam256auto(struct drx_demod_instance *demod,
 	u32 d_locked_time = 0;
 	u32 timeout_ofs = DRXJ_QAM_DEMOD_LOCK_EXT_WAITTIME;
 
-	/* external attributes for storing aquired channel constellation */
+	/* external attributes for storing acquired channel constellation */
 	*lock_status = DRX_NOT_LOCKED;
 	start_time = jiffies_to_msecs(jiffies);
 	lck_state = NO_LOCK;
@@ -9087,7 +9087,7 @@ set_qam_channel(struct drx_demod_instance *demod,
 	enum drx_lock_status lock_status = DRX_NOT_LOCKED;
 	bool auto_flag = false;
 
-	/* external attributes for storing aquired channel constellation */
+	/* external attributes for storing acquired channel constellation */
 	ext_attr = (struct drxj_data *) demod->my_ext_attr;
 
 	/* set QAM channel constellation */
@@ -9431,7 +9431,7 @@ rw_error:
 
 /**
 * \fn int ctrl_get_qam_sig_quality()
-* \brief Retreive QAM signal quality from device.
+* \brief Retrieve QAM signal quality from device.
 * \param devmod Pointer to demodulator instance.
 * \param sig_quality Pointer to signal quality data.
 * \return int.
@@ -9541,7 +9541,7 @@ ctrl_get_qam_sig_quality(struct drx_demod_instance *demod)
 	/* ----------------------------------------- */
 	/* Pre Viterbi Symbol Error Rate Calculation */
 	/* ----------------------------------------- */
-	/* pre viterbi SER is good if it is bellow 0.025 */
+	/* pre viterbi SER is good if it is below 0.025 */
 
 	/* get the register value */
 	/*   no of quadrature symbol errors */
@@ -10647,7 +10647,7 @@ rw_error:
 
 /**
 * \fn int ctrl_sig_quality()
-* \brief Retreive signal quality form device.
+* \brief Retrieve signal quality form device.
 * \param devmod Pointer to demodulator instance.
 * \param sig_quality Pointer to signal quality data.
 * \return int.
@@ -10763,7 +10763,7 @@ rw_error:
 
 /**
 * \fn int ctrl_lock_status()
-* \brief Retreive lock status .
+* \brief Retrieve lock status .
 * \param dev_addr Pointer to demodulator device address.
 * \param lock_stat Pointer to lock status structure.
 * \return int.
@@ -10815,7 +10815,7 @@ ctrl_lock_status(struct drx_demod_instance *demod, enum drx_lock_status *lock_st
 		return -EIO;
 	}
 
-	/* define the SCU command paramters and execute the command */
+	/* define the SCU command parameters and execute the command */
 	cmd_scu.parameter_len = 0;
 	cmd_scu.result_len = 2;
 	cmd_scu.parameter = NULL;
@@ -11489,7 +11489,7 @@ static int drxj_open(struct drx_demod_instance *demod)
 	}
 
 	/* Stamp driver version number in SCU data RAM in BCD code
-	   Done to enable field application engineers to retreive drxdriver version
+	   Done to enable field application engineers to retrieve drxdriver version
 	   via I2C from SCU RAM
 	 */
 	driver_version = (VERSION_MAJOR / 100) % 10;
@@ -11892,7 +11892,7 @@ release:
 	return rc;
 }
 
-/* caller is expeced to check if lna is supported before enabling */
+/* caller is expected to check if lna is supported before enabling */
 static int drxj_set_lna_state(struct drx_demod_instance *demod, bool state)
 {
 	struct drxuio_cfg uio_cfg;
@@ -11946,7 +11946,7 @@ static int drx39xxj_set_powerstate(struct dvb_frontend *fe, int enable)
 	return 0;
 }
 
-static int drx39xxj_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int drx39xxj_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct drx39xxj_state *state = fe->demodulator_priv;
 	struct drx_demod_instance *demod = state->demod;
diff --git a/drivers/media/dvb-frontends/drxd_hard.c b/drivers/media/dvb-frontends/drxd_hard.c
index 687e893d29fe..34b9441840da 100644
--- a/drivers/media/dvb-frontends/drxd_hard.c
+++ b/drivers/media/dvb-frontends/drxd_hard.c
@@ -2805,7 +2805,7 @@ static int drxd_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 	return 0;
 }
 
-static int drxd_read_status(struct dvb_frontend *fe, fe_status_t * status)
+static int drxd_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct drxd_state *state = fe->demodulator_priv;
 	u32 lock;
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index d46cf5f7cd2e..b975da099929 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -544,7 +544,7 @@ error:
 static int init_state(struct drxk_state *state)
 {
 	/*
-	 * FIXME: most (all?) of the values bellow should be moved into
+	 * FIXME: most (all?) of the values below should be moved into
 	 * struct drxk_config, as they are probably board-specific
 	 */
 	u32 ul_vsb_if_agc_mode = DRXK_AGC_CTRL_AUTO;
@@ -3262,6 +3262,7 @@ static int dvbt_sc_command(struct drxk_state *state,
 	}
 
 	/* Write needed parameters and the command */
+	status = 0;
 	switch (cmd) {
 		/* All commands using 5 parameters */
 		/* All commands using 4 parameters */
@@ -3270,16 +3271,16 @@ static int dvbt_sc_command(struct drxk_state *state,
 	case OFDM_SC_RA_RAM_CMD_PROC_START:
 	case OFDM_SC_RA_RAM_CMD_SET_PREF_PARAM:
 	case OFDM_SC_RA_RAM_CMD_PROGRAM_PARAM:
-		status = write16(state, OFDM_SC_RA_RAM_PARAM1__A, param1);
+		status |= write16(state, OFDM_SC_RA_RAM_PARAM1__A, param1);
 		/* All commands using 1 parameters */
 	case OFDM_SC_RA_RAM_CMD_SET_ECHO_TIMING:
 	case OFDM_SC_RA_RAM_CMD_USER_IO:
-		status = write16(state, OFDM_SC_RA_RAM_PARAM0__A, param0);
+		status |= write16(state, OFDM_SC_RA_RAM_PARAM0__A, param0);
 		/* All commands using 0 parameters */
 	case OFDM_SC_RA_RAM_CMD_GET_OP_PARAM:
 	case OFDM_SC_RA_RAM_CMD_NULL:
 		/* Write command */
-		status = write16(state, OFDM_SC_RA_RAM_CMD__A, cmd);
+		status |= write16(state, OFDM_SC_RA_RAM_CMD__A, cmd);
 		break;
 	default:
 		/* Unknown command */
@@ -6639,7 +6640,7 @@ error:
 }
 
 
-static int drxk_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int drxk_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct drxk_state *state = fe->demodulator_priv;
 	int rc;
diff --git a/drivers/media/dvb-frontends/drxk_hard.h b/drivers/media/dvb-frontends/drxk_hard.h
index bae9c71dc3e9..9ed88e014942 100644
--- a/drivers/media/dvb-frontends/drxk_hard.h
+++ b/drivers/media/dvb-frontends/drxk_hard.h
@@ -350,7 +350,7 @@ struct drxk_state {
 	bool	antenna_dvbt;
 	u16	antenna_gpio;
 
-	fe_status_t fe_status;
+	enum fe_status fe_status;
 
 	/* Firmware */
 	const char *microcode_name;
diff --git a/drivers/media/dvb-frontends/ds3000.c b/drivers/media/dvb-frontends/ds3000.c
index 9d0d0347758f..e8fc0329ea64 100644
--- a/drivers/media/dvb-frontends/ds3000.c
+++ b/drivers/media/dvb-frontends/ds3000.c
@@ -404,7 +404,8 @@ static int ds3000_load_firmware(struct dvb_frontend *fe,
 	return ret;
 }
 
-static int ds3000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int ds3000_set_voltage(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage)
 {
 	struct ds3000_state *state = fe->demodulator_priv;
 	u8 data;
@@ -431,7 +432,7 @@ static int ds3000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 	return 0;
 }
 
-static int ds3000_read_status(struct dvb_frontend *fe, fe_status_t* status)
+static int ds3000_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct ds3000_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
@@ -666,7 +667,7 @@ static int ds3000_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 	return 0;
 }
 
-static int ds3000_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int ds3000_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct ds3000_state *state = fe->demodulator_priv;
 	u8 data;
@@ -766,7 +767,7 @@ static int ds3000_send_diseqc_msg(struct dvb_frontend *fe,
 
 /* Send DiSEqC burst */
 static int ds3000_diseqc_send_burst(struct dvb_frontend *fe,
-					fe_sec_mini_cmd_t burst)
+				    enum fe_sec_mini_cmd burst)
 {
 	struct ds3000_state *state = fe->demodulator_priv;
 	int i;
@@ -905,7 +906,7 @@ static int ds3000_set_frontend(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
 	int i;
-	fe_status_t status;
+	enum fe_status status;
 	s32 offset_khz;
 	u32 frequency;
 	u16 value;
@@ -1045,7 +1046,7 @@ static int ds3000_tune(struct dvb_frontend *fe,
 			bool re_tune,
 			unsigned int mode_flags,
 			unsigned int *delay,
-			fe_status_t *status)
+			enum fe_status *status)
 {
 	if (re_tune) {
 		int ret = ds3000_set_frontend(fe);
diff --git a/drivers/media/dvb-frontends/dvb_dummy_fe.c b/drivers/media/dvb-frontends/dvb_dummy_fe.c
index d5acc304786b..14e996d45fac 100644
--- a/drivers/media/dvb-frontends/dvb_dummy_fe.c
+++ b/drivers/media/dvb-frontends/dvb_dummy_fe.c
@@ -33,7 +33,8 @@ struct dvb_dummy_fe_state {
 };
 
 
-static int dvb_dummy_fe_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int dvb_dummy_fe_read_status(struct dvb_frontend *fe,
+				    enum fe_status *status)
 {
 	*status = FE_HAS_SIGNAL
 		| FE_HAS_CARRIER
@@ -97,12 +98,14 @@ static int dvb_dummy_fe_init(struct dvb_frontend* fe)
 	return 0;
 }
 
-static int dvb_dummy_fe_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int dvb_dummy_fe_set_tone(struct dvb_frontend *fe,
+				 enum fe_sec_tone_mode tone)
 {
 	return 0;
 }
 
-static int dvb_dummy_fe_set_voltage(struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int dvb_dummy_fe_set_voltage(struct dvb_frontend *fe,
+				    enum fe_sec_voltage voltage)
 {
 	return 0;
 }
diff --git a/drivers/media/dvb-frontends/ec100.c b/drivers/media/dvb-frontends/ec100.c
index 9d424809d06b..c9012e677cd1 100644
--- a/drivers/media/dvb-frontends/ec100.c
+++ b/drivers/media/dvb-frontends/ec100.c
@@ -174,7 +174,7 @@ static int ec100_get_tune_settings(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int ec100_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int ec100_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct ec100_state *state = fe->demodulator_priv;
 	int ret;
diff --git a/drivers/media/dvb-frontends/hd29l2.c b/drivers/media/dvb-frontends/hd29l2.c
index 67c8e6df42e8..40e359f2d17d 100644
--- a/drivers/media/dvb-frontends/hd29l2.c
+++ b/drivers/media/dvb-frontends/hd29l2.c
@@ -211,7 +211,7 @@ err:
 	return ret;
 }
 
-static int hd29l2_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int hd29l2_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	int ret;
 	struct hd29l2_priv *priv = fe->demodulator_priv;
diff --git a/drivers/media/dvb-frontends/hd29l2_priv.h b/drivers/media/dvb-frontends/hd29l2_priv.h
index 4d571a2282d4..6dc225c4bc91 100644
--- a/drivers/media/dvb-frontends/hd29l2_priv.h
+++ b/drivers/media/dvb-frontends/hd29l2_priv.h
@@ -67,7 +67,7 @@ struct hd29l2_priv {
 	struct hd29l2_config cfg;
 	u8 tuner_i2c_addr_programmed:1;
 
-	fe_status_t fe_status;
+	enum fe_status fe_status;
 };
 
 static const struct reg_mod_vals reg_mod_vals_tab[] = {
diff --git a/drivers/media/dvb-frontends/isl6405.c b/drivers/media/dvb-frontends/isl6405.c
index 0c642a5bf823..b46450a10b80 100644
--- a/drivers/media/dvb-frontends/isl6405.c
+++ b/drivers/media/dvb-frontends/isl6405.c
@@ -43,7 +43,8 @@ struct isl6405 {
 	u8			i2c_addr;
 };
 
-static int isl6405_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int isl6405_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct isl6405 *isl6405 = (struct isl6405 *) fe->sec_priv;
 	struct i2c_msg msg = {	.addr = isl6405->i2c_addr, .flags = 0,
diff --git a/drivers/media/dvb-frontends/isl6421.c b/drivers/media/dvb-frontends/isl6421.c
index c77002fcc8e2..3a4d4606a426 100644
--- a/drivers/media/dvb-frontends/isl6421.c
+++ b/drivers/media/dvb-frontends/isl6421.c
@@ -43,7 +43,8 @@ struct isl6421 {
 	u8			i2c_addr;
 };
 
-static int isl6421_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int isl6421_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct isl6421 *isl6421 = (struct isl6421 *) fe->sec_priv;
 	struct i2c_msg msg = {	.addr = isl6421->i2c_addr, .flags = 0,
@@ -89,7 +90,8 @@ static int isl6421_enable_high_lnb_voltage(struct dvb_frontend *fe, long arg)
 	return (i2c_transfer(isl6421->i2c, &msg, 1) == 1) ? 0 : -EIO;
 }
 
-static int isl6421_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int isl6421_set_tone(struct dvb_frontend *fe,
+			    enum fe_sec_tone_mode tone)
 {
 	struct isl6421 *isl6421 = (struct isl6421 *) fe->sec_priv;
 	struct i2c_msg msg = { .addr = isl6421->i2c_addr, .flags = 0,
diff --git a/drivers/media/dvb-frontends/l64781.c b/drivers/media/dvb-frontends/l64781.c
index ddf866c46f8b..0977871232a2 100644
--- a/drivers/media/dvb-frontends/l64781.c
+++ b/drivers/media/dvb-frontends/l64781.c
@@ -359,7 +359,7 @@ static int get_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int l64781_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int l64781_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct l64781_state* state = fe->demodulator_priv;
 	int sync = l64781_readreg (state, 0x32);
diff --git a/drivers/media/dvb-frontends/lg2160.c b/drivers/media/dvb-frontends/lg2160.c
index 99efeba3c31a..7880f71ccd8a 100644
--- a/drivers/media/dvb-frontends/lg2160.c
+++ b/drivers/media/dvb-frontends/lg2160.c
@@ -1203,7 +1203,7 @@ static int lg216x_read_lock_status(struct lg216x_state *state,
 #endif
 }
 
-static int lg216x_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int lg216x_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct lg216x_state *state = fe->demodulator_priv;
 	int ret, acq_lock, sync_lock;
diff --git a/drivers/media/dvb-frontends/lgdt3305.c b/drivers/media/dvb-frontends/lgdt3305.c
index d08570af1c10..47121866163d 100644
--- a/drivers/media/dvb-frontends/lgdt3305.c
+++ b/drivers/media/dvb-frontends/lgdt3305.c
@@ -60,7 +60,7 @@ struct lgdt3305_state {
 
 	struct dvb_frontend frontend;
 
-	fe_modulation_t current_modulation;
+	enum fe_modulation current_modulation;
 	u32 current_frequency;
 	u32 snr;
 };
@@ -912,7 +912,7 @@ fail:
 	return ret;
 }
 
-static int lgdt3305_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int lgdt3305_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct lgdt3305_state *state = fe->demodulator_priv;
 	u8 val;
diff --git a/drivers/media/dvb-frontends/lgdt3306a.c b/drivers/media/dvb-frontends/lgdt3306a.c
index d9a2b0e768e0..721fbc07e9ee 100644
--- a/drivers/media/dvb-frontends/lgdt3306a.c
+++ b/drivers/media/dvb-frontends/lgdt3306a.c
@@ -62,7 +62,7 @@ struct lgdt3306a_state {
 
 	struct dvb_frontend frontend;
 
-	fe_modulation_t current_modulation;
+	enum fe_modulation current_modulation;
 	u32 current_frequency;
 	u32 snr;
 };
@@ -1558,7 +1558,8 @@ lgdt3306a_qam_lock_poll(struct lgdt3306a_state *state)
 	return LG3306_UNLOCK;
 }
 
-static int lgdt3306a_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int lgdt3306a_read_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
 {
 	struct lgdt3306a_state *state = fe->demodulator_priv;
 	u16 strength = 0;
@@ -1705,7 +1706,7 @@ static int lgdt3306a_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 
 static int lgdt3306a_tune(struct dvb_frontend *fe, bool re_tune,
 			  unsigned int mode_flags, unsigned int *delay,
-			  fe_status_t *status)
+			  enum fe_status *status)
 {
 	int ret = 0;
 	struct lgdt3306a_state *state = fe->demodulator_priv;
@@ -1735,7 +1736,7 @@ static int lgdt3306a_get_tune_settings(struct dvb_frontend *fe,
 
 static int lgdt3306a_search(struct dvb_frontend *fe)
 {
-	fe_status_t status = 0;
+	enum fe_status status = 0;
 	int i, ret;
 
 	/* set frontend */
@@ -2101,7 +2102,7 @@ static void lgdt3306a_DumpRegs(struct lgdt3306a_state *state)
 		lgdt3306a_read_reg(state, regtab[i], &regval1[i]);
 		if (regval1[i] != regval2[i]) {
 			lg_debug(" %04X = %02X\n", regtab[i], regval1[i]);
-				 regval2[i] = regval1[i];
+			regval2[i] = regval1[i];
 		}
 	}
 	debug = sav_debug;
diff --git a/drivers/media/dvb-frontends/lgdt330x.c b/drivers/media/dvb-frontends/lgdt330x.c
index 2e1a61893fc1..cf3cc20510da 100644
--- a/drivers/media/dvb-frontends/lgdt330x.c
+++ b/drivers/media/dvb-frontends/lgdt330x.c
@@ -67,7 +67,7 @@ struct lgdt330x_state
 	struct dvb_frontend frontend;
 
 	/* Demodulator private data */
-	fe_modulation_t current_modulation;
+	enum fe_modulation current_modulation;
 	u32 snr; /* Result of last SNR calculation */
 
 	/* Tuner private data */
@@ -447,7 +447,8 @@ static int lgdt330x_get_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int lgdt3302_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int lgdt3302_read_status(struct dvb_frontend *fe,
+				enum fe_status *status)
 {
 	struct lgdt330x_state* state = fe->demodulator_priv;
 	u8 buf[3];
@@ -505,7 +506,8 @@ static int lgdt3302_read_status(struct dvb_frontend* fe, fe_status_t* status)
 	return 0;
 }
 
-static int lgdt3303_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int lgdt3303_read_status(struct dvb_frontend *fe,
+				enum fe_status *status)
 {
 	struct lgdt330x_state* state = fe->demodulator_priv;
 	int err;
diff --git a/drivers/media/dvb-frontends/lgs8gl5.c b/drivers/media/dvb-frontends/lgs8gl5.c
index 416cce3fefc7..7bbb2c18c2dd 100644
--- a/drivers/media/dvb-frontends/lgs8gl5.c
+++ b/drivers/media/dvb-frontends/lgs8gl5.c
@@ -249,7 +249,7 @@ lgs8gl5_init(struct dvb_frontend *fe)
 
 
 static int
-lgs8gl5_read_status(struct dvb_frontend *fe, fe_status_t *status)
+lgs8gl5_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct lgs8gl5_state *state = fe->demodulator_priv;
 	u8 level = lgs8gl5_read_reg(state, REG_STRENGTH);
diff --git a/drivers/media/dvb-frontends/lgs8gxx.c b/drivers/media/dvb-frontends/lgs8gxx.c
index 3c92f36ea5c7..e2c191c8b196 100644
--- a/drivers/media/dvb-frontends/lgs8gxx.c
+++ b/drivers/media/dvb-frontends/lgs8gxx.c
@@ -732,7 +732,8 @@ int lgs8gxx_get_tune_settings(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int lgs8gxx_read_status(struct dvb_frontend *fe, fe_status_t *fe_status)
+static int lgs8gxx_read_status(struct dvb_frontend *fe,
+			       enum fe_status *fe_status)
 {
 	struct lgs8gxx_state *priv = fe->demodulator_priv;
 	s8 ret;
diff --git a/drivers/media/dvb-frontends/lnbp21.c b/drivers/media/dvb-frontends/lnbp21.c
index f3ba7b5faa2e..4aca0fb9a8a7 100644
--- a/drivers/media/dvb-frontends/lnbp21.c
+++ b/drivers/media/dvb-frontends/lnbp21.c
@@ -45,7 +45,7 @@ struct lnbp21 {
 };
 
 static int lnbp21_set_voltage(struct dvb_frontend *fe,
-					fe_sec_voltage_t voltage)
+			      enum fe_sec_voltage voltage)
 {
 	struct lnbp21 *lnbp21 = (struct lnbp21 *) fe->sec_priv;
 	struct i2c_msg msg = {	.addr = lnbp21->i2c_addr, .flags = 0,
@@ -92,7 +92,7 @@ static int lnbp21_enable_high_lnb_voltage(struct dvb_frontend *fe, long arg)
 }
 
 static int lnbp21_set_tone(struct dvb_frontend *fe,
-				fe_sec_tone_mode_t tone)
+			   enum fe_sec_tone_mode tone)
 {
 	struct lnbp21 *lnbp21 = (struct lnbp21 *) fe->sec_priv;
 	struct i2c_msg msg = {	.addr = lnbp21->i2c_addr, .flags = 0,
diff --git a/drivers/media/dvb-frontends/lnbp22.c b/drivers/media/dvb-frontends/lnbp22.c
index c463da7f6dcc..d7ca0fdd0084 100644
--- a/drivers/media/dvb-frontends/lnbp22.c
+++ b/drivers/media/dvb-frontends/lnbp22.c
@@ -48,7 +48,8 @@ struct lnbp22 {
 	struct i2c_adapter *i2c;
 };
 
-static int lnbp22_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int lnbp22_set_voltage(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage)
 {
 	struct lnbp22 *lnbp22 = (struct lnbp22 *)fe->sec_priv;
 	struct i2c_msg msg = {
diff --git a/drivers/media/dvb-frontends/m88ds3103.c b/drivers/media/dvb-frontends/m88ds3103.c
index d3d928e1c0ce..e9b2d2b69b1d 100644
--- a/drivers/media/dvb-frontends/m88ds3103.c
+++ b/drivers/media/dvb-frontends/m88ds3103.c
@@ -1,5 +1,5 @@
 /*
- * Montage M88DS3103/M88RS6000 demodulator driver
+ * Montage Technology M88DS3103/M88RS6000 demodulator driver
  *
  * Copyright (C) 2013 Antti Palosaari <crope@iki.fi>
  *
@@ -18,149 +18,15 @@
 
 static struct dvb_frontend_ops m88ds3103_ops;
 
-/* write multiple registers */
-static int m88ds3103_wr_regs(struct m88ds3103_priv *priv,
-		u8 reg, const u8 *val, int len)
-{
-#define MAX_WR_LEN 32
-#define MAX_WR_XFER_LEN (MAX_WR_LEN + 1)
-	int ret;
-	u8 buf[MAX_WR_XFER_LEN];
-	struct i2c_msg msg[1] = {
-		{
-			.addr = priv->cfg->i2c_addr,
-			.flags = 0,
-			.len = 1 + len,
-			.buf = buf,
-		}
-	};
-
-	if (WARN_ON(len > MAX_WR_LEN))
-		return -EINVAL;
-
-	buf[0] = reg;
-	memcpy(&buf[1], val, len);
-
-	mutex_lock(&priv->i2c_mutex);
-	ret = i2c_transfer(priv->i2c, msg, 1);
-	mutex_unlock(&priv->i2c_mutex);
-	if (ret == 1) {
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev,
-				"%s: i2c wr failed=%d reg=%02x len=%d\n",
-				KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-
-	return ret;
-}
-
-/* read multiple registers */
-static int m88ds3103_rd_regs(struct m88ds3103_priv *priv,
-		u8 reg, u8 *val, int len)
-{
-#define MAX_RD_LEN 3
-#define MAX_RD_XFER_LEN (MAX_RD_LEN)
-	int ret;
-	u8 buf[MAX_RD_XFER_LEN];
-	struct i2c_msg msg[2] = {
-		{
-			.addr = priv->cfg->i2c_addr,
-			.flags = 0,
-			.len = 1,
-			.buf = &reg,
-		}, {
-			.addr = priv->cfg->i2c_addr,
-			.flags = I2C_M_RD,
-			.len = len,
-			.buf = buf,
-		}
-	};
-
-	if (WARN_ON(len > MAX_RD_LEN))
-		return -EINVAL;
-
-	mutex_lock(&priv->i2c_mutex);
-	ret = i2c_transfer(priv->i2c, msg, 2);
-	mutex_unlock(&priv->i2c_mutex);
-	if (ret == 2) {
-		memcpy(val, buf, len);
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev,
-				"%s: i2c rd failed=%d reg=%02x len=%d\n",
-				KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-
-	return ret;
-}
-
-/* write single register */
-static int m88ds3103_wr_reg(struct m88ds3103_priv *priv, u8 reg, u8 val)
-{
-	return m88ds3103_wr_regs(priv, reg, &val, 1);
-}
-
-/* read single register */
-static int m88ds3103_rd_reg(struct m88ds3103_priv *priv, u8 reg, u8 *val)
-{
-	return m88ds3103_rd_regs(priv, reg, val, 1);
-}
-
-/* write single register with mask */
-static int m88ds3103_wr_reg_mask(struct m88ds3103_priv *priv,
-		u8 reg, u8 val, u8 mask)
-{
-	int ret;
-	u8 u8tmp;
-
-	/* no need for read if whole reg is written */
-	if (mask != 0xff) {
-		ret = m88ds3103_rd_regs(priv, reg, &u8tmp, 1);
-		if (ret)
-			return ret;
-
-		val &= mask;
-		u8tmp &= ~mask;
-		val |= u8tmp;
-	}
-
-	return m88ds3103_wr_regs(priv, reg, &val, 1);
-}
-
-/* read single register with mask */
-static int m88ds3103_rd_reg_mask(struct m88ds3103_priv *priv,
-		u8 reg, u8 *val, u8 mask)
-{
-	int ret, i;
-	u8 u8tmp;
-
-	ret = m88ds3103_rd_regs(priv, reg, &u8tmp, 1);
-	if (ret)
-		return ret;
-
-	u8tmp &= mask;
-
-	/* find position of the first bit */
-	for (i = 0; i < 8; i++) {
-		if ((mask >> i) & 0x01)
-			break;
-	}
-	*val = u8tmp >> i;
-
-	return 0;
-}
-
 /* write reg val table using reg addr auto increment */
-static int m88ds3103_wr_reg_val_tab(struct m88ds3103_priv *priv,
+static int m88ds3103_wr_reg_val_tab(struct m88ds3103_dev *dev,
 		const struct m88ds3103_reg_val *tab, int tab_len)
 {
+	struct i2c_client *client = dev->client;
 	int ret, i, j;
 	u8 buf[83];
 
-	dev_dbg(&priv->i2c->dev, "%s: tab_len=%d\n", __func__, tab_len);
+	dev_dbg(&client->dev, "tab_len=%d\n", tab_len);
 
 	if (tab_len > 86) {
 		ret = -EINVAL;
@@ -171,8 +37,8 @@ static int m88ds3103_wr_reg_val_tab(struct m88ds3103_priv *priv,
 		buf[j] = tab[i].val;
 
 		if (i == tab_len - 1 || tab[i].reg != tab[i + 1].reg - 1 ||
-				!((j + 1) % (priv->cfg->i2c_wr_max - 1))) {
-			ret = m88ds3103_wr_regs(priv, tab[i].reg - j, buf, j + 1);
+				!((j + 1) % (dev->cfg->i2c_wr_max - 1))) {
+			ret = regmap_bulk_write(dev->regmap, tab[i].reg - j, buf, j + 1);
 			if (ret)
 				goto err;
 
@@ -182,66 +48,238 @@ static int m88ds3103_wr_reg_val_tab(struct m88ds3103_priv *priv,
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
+}
+
+/*
+ * Get the demodulator AGC PWM voltage setting supplied to the tuner.
+ */
+int m88ds3103_get_agc_pwm(struct dvb_frontend *fe, u8 *_agc_pwm)
+{
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	unsigned tmp;
+	int ret;
+
+	ret = regmap_read(dev->regmap, 0x3f, &tmp);
+	if (ret == 0)
+		*_agc_pwm = tmp;
 	return ret;
 }
+EXPORT_SYMBOL(m88ds3103_get_agc_pwm);
 
-static int m88ds3103_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int m88ds3103_read_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret;
-	u8 u8tmp;
+	int ret, i, itmp;
+	unsigned int utmp;
+	u8 buf[3];
 
 	*status = 0;
 
-	if (!priv->warm) {
+	if (!dev->warm) {
 		ret = -EAGAIN;
 		goto err;
 	}
 
 	switch (c->delivery_system) {
 	case SYS_DVBS:
-		ret = m88ds3103_rd_reg_mask(priv, 0xd1, &u8tmp, 0x07);
+		ret = regmap_read(dev->regmap, 0xd1, &utmp);
 		if (ret)
 			goto err;
 
-		if (u8tmp == 0x07)
+		if ((utmp & 0x07) == 0x07)
 			*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
 					FE_HAS_VITERBI | FE_HAS_SYNC |
 					FE_HAS_LOCK;
 		break;
 	case SYS_DVBS2:
-		ret = m88ds3103_rd_reg_mask(priv, 0x0d, &u8tmp, 0x8f);
+		ret = regmap_read(dev->regmap, 0x0d, &utmp);
 		if (ret)
 			goto err;
 
-		if (u8tmp == 0x8f)
+		if ((utmp & 0x8f) == 0x8f)
 			*status = FE_HAS_SIGNAL | FE_HAS_CARRIER |
 					FE_HAS_VITERBI | FE_HAS_SYNC |
 					FE_HAS_LOCK;
 		break;
 	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid delivery_system\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
-	priv->fe_status = *status;
+	dev->fe_status = *status;
+	dev_dbg(&client->dev, "lock=%02x status=%02x\n", utmp, *status);
+
+	/* CNR */
+	if (dev->fe_status & FE_HAS_VITERBI) {
+		unsigned int cnr, noise, signal, noise_tot, signal_tot;
+
+		cnr = 0;
+		/* more iterations for more accurate estimation */
+		#define M88DS3103_SNR_ITERATIONS 3
+
+		switch (c->delivery_system) {
+		case SYS_DVBS:
+			itmp = 0;
+
+			for (i = 0; i < M88DS3103_SNR_ITERATIONS; i++) {
+				ret = regmap_read(dev->regmap, 0xff, &utmp);
+				if (ret)
+					goto err;
+
+				itmp += utmp;
+			}
+
+			/* use of single register limits max value to 15 dB */
+			/* SNR(X) dB = 10 * ln(X) / ln(10) dB */
+			itmp = DIV_ROUND_CLOSEST(itmp, 8 * M88DS3103_SNR_ITERATIONS);
+			if (itmp)
+				cnr = div_u64((u64) 10000 * intlog2(itmp), intlog2(10));
+			break;
+		case SYS_DVBS2:
+			noise_tot = 0;
+			signal_tot = 0;
+
+			for (i = 0; i < M88DS3103_SNR_ITERATIONS; i++) {
+				ret = regmap_bulk_read(dev->regmap, 0x8c, buf, 3);
+				if (ret)
+					goto err;
+
+				noise = buf[1] << 6;    /* [13:6] */
+				noise |= buf[0] & 0x3f; /*  [5:0] */
+				noise >>= 2;
+				signal = buf[2] * buf[2];
+				signal >>= 1;
+
+				noise_tot += noise;
+				signal_tot += signal;
+			}
+
+			noise = noise_tot / M88DS3103_SNR_ITERATIONS;
+			signal = signal_tot / M88DS3103_SNR_ITERATIONS;
+
+			/* SNR(X) dB = 10 * log10(X) dB */
+			if (signal > noise) {
+				itmp = signal / noise;
+				cnr = div_u64((u64) 10000 * intlog10(itmp), (1 << 24));
+			}
+			break;
+		default:
+			dev_dbg(&client->dev, "invalid delivery_system\n");
+			ret = -EINVAL;
+			goto err;
+		}
+
+		if (cnr) {
+			c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+			c->cnr.stat[0].svalue = cnr;
+		} else {
+			c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		}
+	} else {
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
+
+	/* BER */
+	if (dev->fe_status & FE_HAS_LOCK) {
+		unsigned int utmp, post_bit_error, post_bit_count;
 
-	dev_dbg(&priv->i2c->dev, "%s: lock=%02x status=%02x\n",
-			__func__, u8tmp, *status);
+		switch (c->delivery_system) {
+		case SYS_DVBS:
+			ret = regmap_write(dev->regmap, 0xf9, 0x04);
+			if (ret)
+				goto err;
+
+			ret = regmap_read(dev->regmap, 0xf8, &utmp);
+			if (ret)
+				goto err;
+
+			/* measurement ready? */
+			if (!(utmp & 0x10)) {
+				ret = regmap_bulk_read(dev->regmap, 0xf6, buf, 2);
+				if (ret)
+					goto err;
+
+				post_bit_error = buf[1] << 8 | buf[0] << 0;
+				post_bit_count = 0x800000;
+				dev->post_bit_error += post_bit_error;
+				dev->post_bit_count += post_bit_count;
+				dev->dvbv3_ber = post_bit_error;
+
+				/* restart measurement */
+				utmp |= 0x10;
+				ret = regmap_write(dev->regmap, 0xf8, utmp);
+				if (ret)
+					goto err;
+			}
+			break;
+		case SYS_DVBS2:
+			ret = regmap_bulk_read(dev->regmap, 0xd5, buf, 3);
+			if (ret)
+				goto err;
+
+			utmp = buf[2] << 16 | buf[1] << 8 | buf[0] << 0;
+
+			/* enough data? */
+			if (utmp > 4000) {
+				ret = regmap_bulk_read(dev->regmap, 0xf7, buf, 2);
+				if (ret)
+					goto err;
+
+				post_bit_error = buf[1] << 8 | buf[0] << 0;
+				post_bit_count = 32 * utmp; /* TODO: FEC */
+				dev->post_bit_error += post_bit_error;
+				dev->post_bit_count += post_bit_count;
+				dev->dvbv3_ber = post_bit_error;
+
+				/* restart measurement */
+				ret = regmap_write(dev->regmap, 0xd1, 0x01);
+				if (ret)
+					goto err;
+
+				ret = regmap_write(dev->regmap, 0xf9, 0x01);
+				if (ret)
+					goto err;
+
+				ret = regmap_write(dev->regmap, 0xf9, 0x00);
+				if (ret)
+					goto err;
+
+				ret = regmap_write(dev->regmap, 0xd1, 0x00);
+				if (ret)
+					goto err;
+			}
+			break;
+		default:
+			dev_dbg(&client->dev, "invalid delivery_system\n");
+			ret = -EINVAL;
+			goto err;
+		}
+
+		c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[0].uvalue = dev->post_bit_error;
+		c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_count.stat[0].uvalue = dev->post_bit_count;
+	} else {
+		c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, len;
 	const struct m88ds3103_reg_val *init;
@@ -251,29 +289,28 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 	u32 tuner_frequency, target_mclk;
 	s32 s32tmp;
 
-	dev_dbg(&priv->i2c->dev,
-			"%s: delivery_system=%d modulation=%d frequency=%d symbol_rate=%d inversion=%d pilot=%d rolloff=%d\n",
-			__func__, c->delivery_system,
-			c->modulation, c->frequency, c->symbol_rate,
-			c->inversion, c->pilot, c->rolloff);
+	dev_dbg(&client->dev,
+		"delivery_system=%d modulation=%d frequency=%u symbol_rate=%d inversion=%d pilot=%d rolloff=%d\n",
+		c->delivery_system, c->modulation, c->frequency, c->symbol_rate,
+		c->inversion, c->pilot, c->rolloff);
 
-	if (!priv->warm) {
+	if (!dev->warm) {
 		ret = -EAGAIN;
 		goto err;
 	}
 
 	/* reset */
-	ret = m88ds3103_wr_reg(priv, 0x07, 0x80);
+	ret = regmap_write(dev->regmap, 0x07, 0x80);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0x07, 0x00);
+	ret = regmap_write(dev->regmap, 0x07, 0x00);
 	if (ret)
 		goto err;
 
 	/* Disable demod clock path */
-	if (priv->chip_id == M88RS6000_CHIP_ID) {
-		ret = m88ds3103_wr_reg(priv, 0x06, 0xe0);
+	if (dev->chip_id == M88RS6000_CHIP_ID) {
+		ret = regmap_write(dev->regmap, 0x06, 0xe0);
 		if (ret)
 			goto err;
 	}
@@ -299,11 +336,11 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 	}
 
 	/* select M88RS6000 demod main mclk and ts mclk from tuner die. */
-	if (priv->chip_id == M88RS6000_CHIP_ID) {
+	if (dev->chip_id == M88RS6000_CHIP_ID) {
 		if (c->symbol_rate > 45010000)
-			priv->mclk_khz = 110250;
+			dev->mclk_khz = 110250;
 		else
-			priv->mclk_khz = 96000;
+			dev->mclk_khz = 96000;
 
 		if (c->delivery_system == SYS_DVBS)
 			target_mclk = 96000;
@@ -311,18 +348,18 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 			target_mclk = 144000;
 
 		/* Enable demod clock path */
-		ret = m88ds3103_wr_reg(priv, 0x06, 0x00);
+		ret = regmap_write(dev->regmap, 0x06, 0x00);
 		if (ret)
 			goto err;
 		usleep_range(10000, 20000);
 	} else {
 	/* set M88DS3103 mclk and ts mclk. */
-		priv->mclk_khz = 96000;
+		dev->mclk_khz = 96000;
 
-		switch (priv->cfg->ts_mode) {
+		switch (dev->cfg->ts_mode) {
 		case M88DS3103_TS_SERIAL:
 		case M88DS3103_TS_SERIAL_D7:
-			target_mclk = priv->cfg->ts_clk;
+			target_mclk = dev->cfg->ts_clk;
 			break;
 		case M88DS3103_TS_PARALLEL:
 		case M88DS3103_TS_CI:
@@ -338,8 +375,7 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 			}
 			break;
 		default:
-			dev_dbg(&priv->i2c->dev, "%s: invalid ts_mode\n",
-					__func__);
+			dev_dbg(&client->dev, "invalid ts_mode\n");
 			ret = -EINVAL;
 			goto err;
 		}
@@ -358,25 +394,25 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 			u8tmp2 = 0x00; /* 0b00 */
 			break;
 		}
-		ret = m88ds3103_wr_reg_mask(priv, 0x22, u8tmp1 << 6, 0xc0);
+		ret = regmap_update_bits(dev->regmap, 0x22, 0xc0, u8tmp1 << 6);
 		if (ret)
 			goto err;
-		ret = m88ds3103_wr_reg_mask(priv, 0x24, u8tmp2 << 6, 0xc0);
+		ret = regmap_update_bits(dev->regmap, 0x24, 0xc0, u8tmp2 << 6);
 		if (ret)
 			goto err;
 	}
 
-	ret = m88ds3103_wr_reg(priv, 0xb2, 0x01);
+	ret = regmap_write(dev->regmap, 0xb2, 0x01);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0x00, 0x01);
+	ret = regmap_write(dev->regmap, 0x00, 0x01);
 	if (ret)
 		goto err;
 
 	switch (c->delivery_system) {
 	case SYS_DVBS:
-		if (priv->chip_id == M88RS6000_CHIP_ID) {
+		if (dev->chip_id == M88RS6000_CHIP_ID) {
 			len = ARRAY_SIZE(m88rs6000_dvbs_init_reg_vals);
 			init = m88rs6000_dvbs_init_reg_vals;
 		} else {
@@ -385,7 +421,7 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 		}
 		break;
 	case SYS_DVBS2:
-		if (priv->chip_id == M88RS6000_CHIP_ID) {
+		if (dev->chip_id == M88RS6000_CHIP_ID) {
 			len = ARRAY_SIZE(m88rs6000_dvbs2_init_reg_vals);
 			init = m88rs6000_dvbs2_init_reg_vals;
 		} else {
@@ -394,44 +430,43 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 		}
 		break;
 	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid delivery_system\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
 	/* program init table */
-	if (c->delivery_system != priv->delivery_system) {
-		ret = m88ds3103_wr_reg_val_tab(priv, init, len);
+	if (c->delivery_system != dev->delivery_system) {
+		ret = m88ds3103_wr_reg_val_tab(dev, init, len);
 		if (ret)
 			goto err;
 	}
 
-	if (priv->chip_id == M88RS6000_CHIP_ID) {
+	if (dev->chip_id == M88RS6000_CHIP_ID) {
 		if ((c->delivery_system == SYS_DVBS2)
 			&& ((c->symbol_rate / 1000) <= 5000)) {
-			ret = m88ds3103_wr_reg(priv, 0xc0, 0x04);
+			ret = regmap_write(dev->regmap, 0xc0, 0x04);
 			if (ret)
 				goto err;
 			buf[0] = 0x09;
 			buf[1] = 0x22;
 			buf[2] = 0x88;
-			ret = m88ds3103_wr_regs(priv, 0x8a, buf, 3);
+			ret = regmap_bulk_write(dev->regmap, 0x8a, buf, 3);
 			if (ret)
 				goto err;
 		}
-		ret = m88ds3103_wr_reg_mask(priv, 0x9d, 0x08, 0x08);
+		ret = regmap_update_bits(dev->regmap, 0x9d, 0x08, 0x08);
 		if (ret)
 			goto err;
-		ret = m88ds3103_wr_reg(priv, 0xf1, 0x01);
+		ret = regmap_write(dev->regmap, 0xf1, 0x01);
 		if (ret)
 			goto err;
-		ret = m88ds3103_wr_reg_mask(priv, 0x30, 0x80, 0x80);
+		ret = regmap_update_bits(dev->regmap, 0x30, 0x80, 0x80);
 		if (ret)
 			goto err;
 	}
 
-	switch (priv->cfg->ts_mode) {
+	switch (dev->cfg->ts_mode) {
 	case M88DS3103_TS_SERIAL:
 		u8tmp1 = 0x00;
 		u8tmp = 0x06;
@@ -447,39 +482,39 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 		u8tmp = 0x03;
 		break;
 	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid ts_mode\n", __func__);
+		dev_dbg(&client->dev, "invalid ts_mode\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
-	if (priv->cfg->ts_clk_pol)
+	if (dev->cfg->ts_clk_pol)
 		u8tmp |= 0x40;
 
 	/* TS mode */
-	ret = m88ds3103_wr_reg(priv, 0xfd, u8tmp);
+	ret = regmap_write(dev->regmap, 0xfd, u8tmp);
 	if (ret)
 		goto err;
 
-	switch (priv->cfg->ts_mode) {
+	switch (dev->cfg->ts_mode) {
 	case M88DS3103_TS_SERIAL:
 	case M88DS3103_TS_SERIAL_D7:
-		ret = m88ds3103_wr_reg_mask(priv, 0x29, u8tmp1, 0x20);
+		ret = regmap_update_bits(dev->regmap, 0x29, 0x20, u8tmp1);
 		if (ret)
 			goto err;
 		u8tmp1 = 0;
 		u8tmp2 = 0;
 		break;
 	default:
-		if (priv->cfg->ts_clk) {
-			divide_ratio = DIV_ROUND_UP(target_mclk, priv->cfg->ts_clk);
+		if (dev->cfg->ts_clk) {
+			divide_ratio = DIV_ROUND_UP(target_mclk, dev->cfg->ts_clk);
 			u8tmp1 = divide_ratio / 2;
 			u8tmp2 = DIV_ROUND_UP(divide_ratio, 2);
 		}
 	}
 
-	dev_dbg(&priv->i2c->dev,
-			"%s: target_mclk=%d ts_clk=%d divide_ratio=%d\n",
-			__func__, target_mclk, priv->cfg->ts_clk, divide_ratio);
+	dev_dbg(&client->dev,
+		"target_mclk=%d ts_clk=%d divide_ratio=%d\n",
+		target_mclk, dev->cfg->ts_clk, divide_ratio);
 
 	u8tmp1--;
 	u8tmp2--;
@@ -488,17 +523,17 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 	/* u8tmp2[5:0] => ea[5:0] */
 	u8tmp2 &= 0x3f;
 
-	ret = m88ds3103_rd_reg(priv, 0xfe, &u8tmp);
+	ret = regmap_bulk_read(dev->regmap, 0xfe, &u8tmp, 1);
 	if (ret)
 		goto err;
 
 	u8tmp = ((u8tmp  & 0xf0) << 0) | u8tmp1 >> 2;
-	ret = m88ds3103_wr_reg(priv, 0xfe, u8tmp);
+	ret = regmap_write(dev->regmap, 0xfe, u8tmp);
 	if (ret)
 		goto err;
 
 	u8tmp = ((u8tmp1 & 0x03) << 6) | u8tmp2 >> 0;
-	ret = m88ds3103_wr_reg(priv, 0xea, u8tmp);
+	ret = regmap_write(dev->regmap, 0xea, u8tmp);
 	if (ret)
 		goto err;
 
@@ -509,250 +544,254 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 	else
 		u8tmp = 0x06;
 
-	ret = m88ds3103_wr_reg(priv, 0xc3, 0x08);
+	ret = regmap_write(dev->regmap, 0xc3, 0x08);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0xc8, u8tmp);
+	ret = regmap_write(dev->regmap, 0xc8, u8tmp);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0xc4, 0x08);
+	ret = regmap_write(dev->regmap, 0xc4, 0x08);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0xc7, 0x00);
+	ret = regmap_write(dev->regmap, 0xc7, 0x00);
 	if (ret)
 		goto err;
 
-	u16tmp = DIV_ROUND_CLOSEST((c->symbol_rate / 1000) << 15, priv->mclk_khz / 2);
+	u16tmp = DIV_ROUND_CLOSEST((c->symbol_rate / 1000) << 15, dev->mclk_khz / 2);
 	buf[0] = (u16tmp >> 0) & 0xff;
 	buf[1] = (u16tmp >> 8) & 0xff;
-	ret = m88ds3103_wr_regs(priv, 0x61, buf, 2);
+	ret = regmap_bulk_write(dev->regmap, 0x61, buf, 2);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg_mask(priv, 0x4d, priv->cfg->spec_inv << 1, 0x02);
+	ret = regmap_update_bits(dev->regmap, 0x4d, 0x02, dev->cfg->spec_inv << 1);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg_mask(priv, 0x30, priv->cfg->agc_inv << 4, 0x10);
+	ret = regmap_update_bits(dev->regmap, 0x30, 0x10, dev->cfg->agc_inv << 4);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0x33, priv->cfg->agc);
+	ret = regmap_write(dev->regmap, 0x33, dev->cfg->agc);
 	if (ret)
 		goto err;
 
-	dev_dbg(&priv->i2c->dev, "%s: carrier offset=%d\n", __func__,
-			(tuner_frequency - c->frequency));
+	dev_dbg(&client->dev, "carrier offset=%d\n",
+		(tuner_frequency - c->frequency));
 
 	s32tmp = 0x10000 * (tuner_frequency - c->frequency);
-	s32tmp = DIV_ROUND_CLOSEST(s32tmp, priv->mclk_khz);
+	s32tmp = DIV_ROUND_CLOSEST(s32tmp, dev->mclk_khz);
 	if (s32tmp < 0)
 		s32tmp += 0x10000;
 
 	buf[0] = (s32tmp >> 0) & 0xff;
 	buf[1] = (s32tmp >> 8) & 0xff;
-	ret = m88ds3103_wr_regs(priv, 0x5e, buf, 2);
+	ret = regmap_bulk_write(dev->regmap, 0x5e, buf, 2);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0x00, 0x00);
+	ret = regmap_write(dev->regmap, 0x00, 0x00);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0xb2, 0x00);
+	ret = regmap_write(dev->regmap, 0xb2, 0x00);
 	if (ret)
 		goto err;
 
-	priv->delivery_system = c->delivery_system;
+	dev->delivery_system = c->delivery_system;
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ds3103_init(struct dvb_frontend *fe)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, len, remaining;
+	unsigned int utmp;
 	const struct firmware *fw = NULL;
 	u8 *fw_file;
-	u8 u8tmp;
 
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	/* set cold state by default */
-	priv->warm = false;
+	dev->warm = false;
 
 	/* wake up device from sleep */
-	ret = m88ds3103_wr_reg_mask(priv, 0x08, 0x01, 0x01);
+	ret = regmap_update_bits(dev->regmap, 0x08, 0x01, 0x01);
 	if (ret)
 		goto err;
-
-	ret = m88ds3103_wr_reg_mask(priv, 0x04, 0x00, 0x01);
+	ret = regmap_update_bits(dev->regmap, 0x04, 0x01, 0x00);
 	if (ret)
 		goto err;
-
-	ret = m88ds3103_wr_reg_mask(priv, 0x23, 0x00, 0x10);
+	ret = regmap_update_bits(dev->regmap, 0x23, 0x10, 0x00);
 	if (ret)
 		goto err;
 
 	/* firmware status */
-	ret = m88ds3103_rd_reg(priv, 0xb9, &u8tmp);
+	ret = regmap_read(dev->regmap, 0xb9, &utmp);
 	if (ret)
 		goto err;
 
-	dev_dbg(&priv->i2c->dev, "%s: firmware=%02x\n", __func__, u8tmp);
+	dev_dbg(&client->dev, "firmware=%02x\n", utmp);
 
-	if (u8tmp)
+	if (utmp)
 		goto skip_fw_download;
 
 	/* global reset, global diseqc reset, golbal fec reset */
-	ret = m88ds3103_wr_reg(priv, 0x07, 0xe0);
+	ret = regmap_write(dev->regmap, 0x07, 0xe0);
 	if (ret)
 		goto err;
-
-	ret = m88ds3103_wr_reg(priv, 0x07, 0x00);
+	ret = regmap_write(dev->regmap, 0x07, 0x00);
 	if (ret)
 		goto err;
 
 	/* cold state - try to download firmware */
-	dev_info(&priv->i2c->dev, "%s: found a '%s' in cold state\n",
-			KBUILD_MODNAME, m88ds3103_ops.info.name);
+	dev_info(&client->dev, "found a '%s' in cold state\n",
+		 m88ds3103_ops.info.name);
 
-	if (priv->chip_id == M88RS6000_CHIP_ID)
+	if (dev->chip_id == M88RS6000_CHIP_ID)
 		fw_file = M88RS6000_FIRMWARE;
 	else
 		fw_file = M88DS3103_FIRMWARE;
 	/* request the firmware, this will block and timeout */
-	ret = request_firmware(&fw, fw_file, priv->i2c->dev.parent);
+	ret = request_firmware(&fw, fw_file, &client->dev);
 	if (ret) {
-		dev_err(&priv->i2c->dev, "%s: firmware file '%s' not found\n",
-				KBUILD_MODNAME, fw_file);
+		dev_err(&client->dev, "firmare file '%s' not found\n", fw_file);
 		goto err;
 	}
 
-	dev_info(&priv->i2c->dev, "%s: downloading firmware from file '%s'\n",
-			KBUILD_MODNAME, fw_file);
+	dev_info(&client->dev, "downloading firmware from file '%s'\n",
+		 fw_file);
 
-	ret = m88ds3103_wr_reg(priv, 0xb2, 0x01);
+	ret = regmap_write(dev->regmap, 0xb2, 0x01);
 	if (ret)
 		goto error_fw_release;
 
 	for (remaining = fw->size; remaining > 0;
-			remaining -= (priv->cfg->i2c_wr_max - 1)) {
+			remaining -= (dev->cfg->i2c_wr_max - 1)) {
 		len = remaining;
-		if (len > (priv->cfg->i2c_wr_max - 1))
-			len = (priv->cfg->i2c_wr_max - 1);
+		if (len > (dev->cfg->i2c_wr_max - 1))
+			len = (dev->cfg->i2c_wr_max - 1);
 
-		ret = m88ds3103_wr_regs(priv, 0xb0,
+		ret = regmap_bulk_write(dev->regmap, 0xb0,
 				&fw->data[fw->size - remaining], len);
 		if (ret) {
-			dev_err(&priv->i2c->dev,
-					"%s: firmware download failed=%d\n",
-					KBUILD_MODNAME, ret);
+			dev_err(&client->dev, "firmware download failed=%d\n",
+				ret);
 			goto error_fw_release;
 		}
 	}
 
-	ret = m88ds3103_wr_reg(priv, 0xb2, 0x00);
+	ret = regmap_write(dev->regmap, 0xb2, 0x00);
 	if (ret)
 		goto error_fw_release;
 
 	release_firmware(fw);
 	fw = NULL;
 
-	ret = m88ds3103_rd_reg(priv, 0xb9, &u8tmp);
+	ret = regmap_read(dev->regmap, 0xb9, &utmp);
 	if (ret)
 		goto err;
 
-	if (!u8tmp) {
-		dev_info(&priv->i2c->dev, "%s: firmware did not run\n",
-				KBUILD_MODNAME);
+	if (!utmp) {
+		dev_info(&client->dev, "firmware did not run\n");
 		ret = -EFAULT;
 		goto err;
 	}
 
-	dev_info(&priv->i2c->dev, "%s: found a '%s' in warm state\n",
-			KBUILD_MODNAME, m88ds3103_ops.info.name);
-	dev_info(&priv->i2c->dev, "%s: firmware version %X.%X\n",
-			KBUILD_MODNAME, (u8tmp >> 4) & 0xf, (u8tmp >> 0 & 0xf));
+	dev_info(&client->dev, "found a '%s' in warm state\n",
+		 m88ds3103_ops.info.name);
+	dev_info(&client->dev, "firmware version: %X.%X\n",
+		 (utmp >> 4) & 0xf, (utmp >> 0 & 0xf));
 
 skip_fw_download:
 	/* warm state */
-	priv->warm = true;
+	dev->warm = true;
 
-	return 0;
+	/* init stats here in order signal app which stats are supported */
+	c->cnr.len = 1;
+	c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_error.len = 1;
+	c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_count.len = 1;
+	c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
 
+	return 0;
 error_fw_release:
 	release_firmware(fw);
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ds3103_sleep(struct dvb_frontend *fe)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
-	u8 u8tmp;
+	unsigned int utmp;
 
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
-	priv->delivery_system = SYS_UNDEFINED;
+	dev->fe_status = 0;
+	dev->delivery_system = SYS_UNDEFINED;
 
 	/* TS Hi-Z */
-	if (priv->chip_id == M88RS6000_CHIP_ID)
-		u8tmp = 0x29;
+	if (dev->chip_id == M88RS6000_CHIP_ID)
+		utmp = 0x29;
 	else
-		u8tmp = 0x27;
-	ret = m88ds3103_wr_reg_mask(priv, u8tmp, 0x00, 0x01);
+		utmp = 0x27;
+	ret = regmap_update_bits(dev->regmap, utmp, 0x01, 0x00);
 	if (ret)
 		goto err;
 
 	/* sleep */
-	ret = m88ds3103_wr_reg_mask(priv, 0x08, 0x00, 0x01);
+	ret = regmap_update_bits(dev->regmap, 0x08, 0x01, 0x00);
 	if (ret)
 		goto err;
-
-	ret = m88ds3103_wr_reg_mask(priv, 0x04, 0x01, 0x01);
+	ret = regmap_update_bits(dev->regmap, 0x04, 0x01, 0x01);
 	if (ret)
 		goto err;
-
-	ret = m88ds3103_wr_reg_mask(priv, 0x23, 0x10, 0x10);
+	ret = regmap_update_bits(dev->regmap, 0x23, 0x10, 0x10);
 	if (ret)
 		goto err;
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ds3103_get_frontend(struct dvb_frontend *fe)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 	u8 buf[3];
 
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
-	if (!priv->warm || !(priv->fe_status & FE_HAS_LOCK)) {
-		ret = -EAGAIN;
+	if (!dev->warm || !(dev->fe_status & FE_HAS_LOCK)) {
+		ret = 0;
 		goto err;
 	}
 
 	switch (c->delivery_system) {
 	case SYS_DVBS:
-		ret = m88ds3103_rd_reg(priv, 0xe0, &buf[0]);
+		ret = regmap_bulk_read(dev->regmap, 0xe0, &buf[0], 1);
 		if (ret)
 			goto err;
 
-		ret = m88ds3103_rd_reg(priv, 0xe6, &buf[1]);
+		ret = regmap_bulk_read(dev->regmap, 0xe6, &buf[1], 1);
 		if (ret)
 			goto err;
 
@@ -782,23 +821,22 @@ static int m88ds3103_get_frontend(struct dvb_frontend *fe)
 			c->fec_inner = FEC_1_2;
 			break;
 		default:
-			dev_dbg(&priv->i2c->dev, "%s: invalid fec_inner\n",
-					__func__);
+			dev_dbg(&client->dev, "invalid fec_inner\n");
 		}
 
 		c->modulation = QPSK;
 
 		break;
 	case SYS_DVBS2:
-		ret = m88ds3103_rd_reg(priv, 0x7e, &buf[0]);
+		ret = regmap_bulk_read(dev->regmap, 0x7e, &buf[0], 1);
 		if (ret)
 			goto err;
 
-		ret = m88ds3103_rd_reg(priv, 0x89, &buf[1]);
+		ret = regmap_bulk_read(dev->regmap, 0x89, &buf[1], 1);
 		if (ret)
 			goto err;
 
-		ret = m88ds3103_rd_reg(priv, 0xf2, &buf[2]);
+		ret = regmap_bulk_read(dev->regmap, 0xf2, &buf[2], 1);
 		if (ret)
 			goto err;
 
@@ -831,8 +869,7 @@ static int m88ds3103_get_frontend(struct dvb_frontend *fe)
 			c->fec_inner = FEC_9_10;
 			break;
 		default:
-			dev_dbg(&priv->i2c->dev, "%s: invalid fec_inner\n",
-					__func__);
+			dev_dbg(&client->dev, "invalid fec_inner\n");
 		}
 
 		switch ((buf[0] >> 5) & 0x01) {
@@ -858,8 +895,7 @@ static int m88ds3103_get_frontend(struct dvb_frontend *fe)
 			c->modulation = APSK_32;
 			break;
 		default:
-			dev_dbg(&priv->i2c->dev, "%s: invalid modulation\n",
-					__func__);
+			dev_dbg(&client->dev, "invalid modulation\n");
 		}
 
 		switch ((buf[1] >> 7) & 0x01) {
@@ -882,201 +918,60 @@ static int m88ds3103_get_frontend(struct dvb_frontend *fe)
 			c->rolloff = ROLLOFF_20;
 			break;
 		default:
-			dev_dbg(&priv->i2c->dev, "%s: invalid rolloff\n",
-					__func__);
+			dev_dbg(&client->dev, "invalid rolloff\n");
 		}
 		break;
 	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid delivery_system\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
-	ret = m88ds3103_rd_regs(priv, 0x6d, buf, 2);
+	ret = regmap_bulk_read(dev->regmap, 0x6d, buf, 2);
 	if (ret)
 		goto err;
 
 	c->symbol_rate = 1ull * ((buf[1] << 8) | (buf[0] << 0)) *
-			priv->mclk_khz * 1000 / 0x10000;
+			dev->mclk_khz * 1000 / 0x10000;
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ds3103_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret, i, tmp;
-	u8 buf[3];
-	u16 noise, signal;
-	u32 noise_tot, signal_tot;
-
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
-	/* reports SNR in resolution of 0.1 dB */
-
-	/* more iterations for more accurate estimation */
-	#define M88DS3103_SNR_ITERATIONS 3
-
-	switch (c->delivery_system) {
-	case SYS_DVBS:
-		tmp = 0;
 
-		for (i = 0; i < M88DS3103_SNR_ITERATIONS; i++) {
-			ret = m88ds3103_rd_reg(priv, 0xff, &buf[0]);
-			if (ret)
-				goto err;
-
-			tmp += buf[0];
-		}
-
-		/* use of one register limits max value to 15 dB */
-		/* SNR(X) dB = 10 * ln(X) / ln(10) dB */
-		tmp = DIV_ROUND_CLOSEST(tmp, 8 * M88DS3103_SNR_ITERATIONS);
-		if (tmp)
-			*snr = div_u64((u64) 100 * intlog2(tmp), intlog2(10));
-		else
-			*snr = 0;
-		break;
-	case SYS_DVBS2:
-		noise_tot = 0;
-		signal_tot = 0;
-
-		for (i = 0; i < M88DS3103_SNR_ITERATIONS; i++) {
-			ret = m88ds3103_rd_regs(priv, 0x8c, buf, 3);
-			if (ret)
-				goto err;
-
-			noise = buf[1] << 6;    /* [13:6] */
-			noise |= buf[0] & 0x3f; /*  [5:0] */
-			noise >>= 2;
-			signal = buf[2] * buf[2];
-			signal >>= 1;
-
-			noise_tot += noise;
-			signal_tot += signal;
-		}
-
-		noise = noise_tot / M88DS3103_SNR_ITERATIONS;
-		signal = signal_tot / M88DS3103_SNR_ITERATIONS;
-
-		/* SNR(X) dB = 10 * log10(X) dB */
-		if (signal > noise) {
-			tmp = signal / noise;
-			*snr = div_u64((u64) 100 * intlog10(tmp), (1 << 24));
-		} else {
-			*snr = 0;
-		}
-		break;
-	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
-				__func__);
-		ret = -EINVAL;
-		goto err;
-	}
+	if (c->cnr.stat[0].scale == FE_SCALE_DECIBEL)
+		*snr = div_s64(c->cnr.stat[0].svalue, 100);
+	else
+		*snr = 0;
 
 	return 0;
-err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
 }
 
 static int m88ds3103_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret;
-	unsigned int utmp;
-	u8 buf[3], u8tmp;
-
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
-
-	switch (c->delivery_system) {
-	case SYS_DVBS:
-		ret = m88ds3103_wr_reg(priv, 0xf9, 0x04);
-		if (ret)
-			goto err;
-
-		ret = m88ds3103_rd_reg(priv, 0xf8, &u8tmp);
-		if (ret)
-			goto err;
-
-		if (!(u8tmp & 0x10)) {
-			u8tmp |= 0x10;
-
-			ret = m88ds3103_rd_regs(priv, 0xf6, buf, 2);
-			if (ret)
-				goto err;
-
-			priv->ber = (buf[1] << 8) | (buf[0] << 0);
-
-			/* restart counters */
-			ret = m88ds3103_wr_reg(priv, 0xf8, u8tmp);
-			if (ret)
-				goto err;
-		}
-		break;
-	case SYS_DVBS2:
-		ret = m88ds3103_rd_regs(priv, 0xd5, buf, 3);
-		if (ret)
-			goto err;
-
-		utmp = (buf[2] << 16) | (buf[1] << 8) | (buf[0] << 0);
-
-		if (utmp > 3000) {
-			ret = m88ds3103_rd_regs(priv, 0xf7, buf, 2);
-			if (ret)
-				goto err;
-
-			priv->ber = (buf[1] << 8) | (buf[0] << 0);
-
-			/* restart counters */
-			ret = m88ds3103_wr_reg(priv, 0xd1, 0x01);
-			if (ret)
-				goto err;
-
-			ret = m88ds3103_wr_reg(priv, 0xf9, 0x01);
-			if (ret)
-				goto err;
-
-			ret = m88ds3103_wr_reg(priv, 0xf9, 0x00);
-			if (ret)
-				goto err;
-
-			ret = m88ds3103_wr_reg(priv, 0xd1, 0x00);
-			if (ret)
-				goto err;
-		}
-		break;
-	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid delivery_system\n",
-				__func__);
-		ret = -EINVAL;
-		goto err;
-	}
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
 
-	*ber = priv->ber;
+	*ber = dev->dvbv3_ber;
 
 	return 0;
-err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-	return ret;
 }
 
 static int m88ds3103_set_tone(struct dvb_frontend *fe,
-	fe_sec_tone_mode_t fe_sec_tone_mode)
+	enum fe_sec_tone_mode fe_sec_tone_mode)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
-	u8 u8tmp, tone, reg_a1_mask;
+	unsigned int utmp, tone, reg_a1_mask;
 
-	dev_dbg(&priv->i2c->dev, "%s: fe_sec_tone_mode=%d\n", __func__,
-			fe_sec_tone_mode);
+	dev_dbg(&client->dev, "fe_sec_tone_mode=%d\n", fe_sec_tone_mode);
 
-	if (!priv->warm) {
+	if (!dev->warm) {
 		ret = -EAGAIN;
 		goto err;
 	}
@@ -1091,40 +986,39 @@ static int m88ds3103_set_tone(struct dvb_frontend *fe,
 		reg_a1_mask = 0x00;
 		break;
 	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_tone_mode\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid fe_sec_tone_mode\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
-	u8tmp = tone << 7 | priv->cfg->envelope_mode << 5;
-	ret = m88ds3103_wr_reg_mask(priv, 0xa2, u8tmp, 0xe0);
+	utmp = tone << 7 | dev->cfg->envelope_mode << 5;
+	ret = regmap_update_bits(dev->regmap, 0xa2, 0xe0, utmp);
 	if (ret)
 		goto err;
 
-	u8tmp = 1 << 2;
-	ret = m88ds3103_wr_reg_mask(priv, 0xa1, u8tmp, reg_a1_mask);
+	utmp = 1 << 2;
+	ret = regmap_update_bits(dev->regmap, 0xa1, reg_a1_mask, utmp);
 	if (ret)
 		goto err;
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ds3103_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t fe_sec_voltage)
+	enum fe_sec_voltage fe_sec_voltage)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
-	u8 u8tmp;
+	unsigned int utmp;
 	bool voltage_sel, voltage_dis;
 
-	dev_dbg(&priv->i2c->dev, "%s: fe_sec_voltage=%d\n", __func__,
-			fe_sec_voltage);
+	dev_dbg(&client->dev, "fe_sec_voltage=%d\n", fe_sec_voltage);
 
-	if (!priv->warm) {
+	if (!dev->warm) {
 		ret = -EAGAIN;
 		goto err;
 	}
@@ -1143,38 +1037,39 @@ static int m88ds3103_set_voltage(struct dvb_frontend *fe,
 		voltage_dis = true;
 		break;
 	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_voltage\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid fe_sec_voltage\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
 	/* output pin polarity */
-	voltage_sel ^= priv->cfg->lnb_hv_pol;
-	voltage_dis ^= priv->cfg->lnb_en_pol;
+	voltage_sel ^= dev->cfg->lnb_hv_pol;
+	voltage_dis ^= dev->cfg->lnb_en_pol;
 
-	u8tmp = voltage_dis << 1 | voltage_sel << 0;
-	ret = m88ds3103_wr_reg_mask(priv, 0xa2, u8tmp, 0x03);
+	utmp = voltage_dis << 1 | voltage_sel << 0;
+	ret = regmap_update_bits(dev->regmap, 0xa2, 0x03, utmp);
 	if (ret)
 		goto err;
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ds3103_diseqc_send_master_cmd(struct dvb_frontend *fe,
 		struct dvb_diseqc_master_cmd *diseqc_cmd)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
-	int ret, i;
-	u8 u8tmp;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
+	int ret;
+	unsigned int utmp;
+	unsigned long timeout;
 
-	dev_dbg(&priv->i2c->dev, "%s: msg=%*ph\n", __func__,
-			diseqc_cmd->msg_len, diseqc_cmd->msg);
+	dev_dbg(&client->dev, "msg=%*ph\n",
+		diseqc_cmd->msg_len, diseqc_cmd->msg);
 
-	if (!priv->warm) {
+	if (!dev->warm) {
 		ret = -EAGAIN;
 		goto err;
 	}
@@ -1184,75 +1079,80 @@ static int m88ds3103_diseqc_send_master_cmd(struct dvb_frontend *fe,
 		goto err;
 	}
 
-	u8tmp = priv->cfg->envelope_mode << 5;
-	ret = m88ds3103_wr_reg_mask(priv, 0xa2, u8tmp, 0xe0);
+	utmp = dev->cfg->envelope_mode << 5;
+	ret = regmap_update_bits(dev->regmap, 0xa2, 0xe0, utmp);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_regs(priv, 0xa3, diseqc_cmd->msg,
+	ret = regmap_bulk_write(dev->regmap, 0xa3, diseqc_cmd->msg,
 			diseqc_cmd->msg_len);
 	if (ret)
 		goto err;
 
-	ret = m88ds3103_wr_reg(priv, 0xa1,
+	ret = regmap_write(dev->regmap, 0xa1,
 			(diseqc_cmd->msg_len - 1) << 3 | 0x07);
 	if (ret)
 		goto err;
 
-	/* DiSEqC message typical period is 54 ms */
-	usleep_range(40000, 60000);
-
 	/* wait DiSEqC TX ready */
-	for (i = 20, u8tmp = 1; i && u8tmp; i--) {
-		usleep_range(5000, 10000);
+	#define SEND_MASTER_CMD_TIMEOUT 120
+	timeout = jiffies + msecs_to_jiffies(SEND_MASTER_CMD_TIMEOUT);
+
+	/* DiSEqC message typical period is 54 ms */
+	usleep_range(50000, 54000);
 
-		ret = m88ds3103_rd_reg_mask(priv, 0xa1, &u8tmp, 0x40);
+	for (utmp = 1; !time_after(jiffies, timeout) && utmp;) {
+		ret = regmap_read(dev->regmap, 0xa1, &utmp);
 		if (ret)
 			goto err;
+		utmp = (utmp >> 6) & 0x1;
 	}
 
-	dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
-
-	if (i == 0) {
-		dev_dbg(&priv->i2c->dev, "%s: diseqc tx timeout\n", __func__);
+	if (utmp == 0) {
+		dev_dbg(&client->dev, "diseqc tx took %u ms\n",
+			jiffies_to_msecs(jiffies) -
+			(jiffies_to_msecs(timeout) - SEND_MASTER_CMD_TIMEOUT));
+	} else {
+		dev_dbg(&client->dev, "diseqc tx timeout\n");
 
-		ret = m88ds3103_wr_reg_mask(priv, 0xa1, 0x40, 0xc0);
+		ret = regmap_update_bits(dev->regmap, 0xa1, 0xc0, 0x40);
 		if (ret)
 			goto err;
 	}
 
-	ret = m88ds3103_wr_reg_mask(priv, 0xa2, 0x80, 0xc0);
+	ret = regmap_update_bits(dev->regmap, 0xa2, 0xc0, 0x80);
 	if (ret)
 		goto err;
 
-	if (i == 0) {
+	if (utmp == 1) {
 		ret = -ETIMEDOUT;
 		goto err;
 	}
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ds3103_diseqc_send_burst(struct dvb_frontend *fe,
-	fe_sec_mini_cmd_t fe_sec_mini_cmd)
+	enum fe_sec_mini_cmd fe_sec_mini_cmd)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
-	int ret, i;
-	u8 u8tmp, burst;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
+	int ret;
+	unsigned int utmp, burst;
+	unsigned long timeout;
 
-	dev_dbg(&priv->i2c->dev, "%s: fe_sec_mini_cmd=%d\n", __func__,
-			fe_sec_mini_cmd);
+	dev_dbg(&client->dev, "fe_sec_mini_cmd=%d\n", fe_sec_mini_cmd);
 
-	if (!priv->warm) {
+	if (!dev->warm) {
 		ret = -EAGAIN;
 		goto err;
 	}
 
-	u8tmp = priv->cfg->envelope_mode << 5;
-	ret = m88ds3103_wr_reg_mask(priv, 0xa2, u8tmp, 0xe0);
+	utmp = dev->cfg->envelope_mode << 5;
+	ret = regmap_update_bits(dev->regmap, 0xa2, 0xe0, utmp);
 	if (ret)
 		goto err;
 
@@ -1264,43 +1164,53 @@ static int m88ds3103_diseqc_send_burst(struct dvb_frontend *fe,
 		burst = 0x01;
 		break;
 	default:
-		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_mini_cmd\n",
-				__func__);
+		dev_dbg(&client->dev, "invalid fe_sec_mini_cmd\n");
 		ret = -EINVAL;
 		goto err;
 	}
 
-	ret = m88ds3103_wr_reg(priv, 0xa1, burst);
+	ret = regmap_write(dev->regmap, 0xa1, burst);
 	if (ret)
 		goto err;
 
-	/* DiSEqC ToneBurst period is 12.5 ms */
-	usleep_range(11000, 20000);
-
 	/* wait DiSEqC TX ready */
-	for (i = 5, u8tmp = 1; i && u8tmp; i--) {
-		usleep_range(800, 2000);
+	#define SEND_BURST_TIMEOUT 40
+	timeout = jiffies + msecs_to_jiffies(SEND_BURST_TIMEOUT);
 
-		ret = m88ds3103_rd_reg_mask(priv, 0xa1, &u8tmp, 0x40);
+	/* DiSEqC ToneBurst period is 12.5 ms */
+	usleep_range(8500, 12500);
+
+	for (utmp = 1; !time_after(jiffies, timeout) && utmp;) {
+		ret = regmap_read(dev->regmap, 0xa1, &utmp);
 		if (ret)
 			goto err;
+		utmp = (utmp >> 6) & 0x1;
 	}
 
-	dev_dbg(&priv->i2c->dev, "%s: loop=%d\n", __func__, i);
+	if (utmp == 0) {
+		dev_dbg(&client->dev, "diseqc tx took %u ms\n",
+			jiffies_to_msecs(jiffies) -
+			(jiffies_to_msecs(timeout) - SEND_BURST_TIMEOUT));
+	} else {
+		dev_dbg(&client->dev, "diseqc tx timeout\n");
 
-	ret = m88ds3103_wr_reg_mask(priv, 0xa2, 0x80, 0xc0);
+		ret = regmap_update_bits(dev->regmap, 0xa1, 0xc0, 0x40);
+		if (ret)
+			goto err;
+	}
+
+	ret = regmap_update_bits(dev->regmap, 0xa2, 0xc0, 0x80);
 	if (ret)
 		goto err;
 
-	if (i == 0) {
-		dev_dbg(&priv->i2c->dev, "%s: diseqc tx timeout\n", __func__);
+	if (utmp == 1) {
 		ret = -ETIMEDOUT;
 		goto err;
 	}
 
 	return 0;
 err:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -1314,193 +1224,290 @@ static int m88ds3103_get_tune_settings(struct dvb_frontend *fe,
 
 static void m88ds3103_release(struct dvb_frontend *fe)
 {
-	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	struct m88ds3103_dev *dev = fe->demodulator_priv;
+	struct i2c_client *client = dev->client;
 
-	i2c_del_mux_adapter(priv->i2c_adapter);
-	kfree(priv);
+	i2c_unregister_device(client);
 }
 
 static int m88ds3103_select(struct i2c_adapter *adap, void *mux_priv, u32 chan)
 {
-	struct m88ds3103_priv *priv = mux_priv;
+	struct m88ds3103_dev *dev = mux_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
-	struct i2c_msg gate_open_msg[1] = {
-		{
-			.addr = priv->cfg->i2c_addr,
-			.flags = 0,
-			.len = 2,
-			.buf = "\x03\x11",
-		}
+	struct i2c_msg msg = {
+		.addr = client->addr,
+		.flags = 0,
+		.len = 2,
+		.buf = "\x03\x11",
 	};
 
-	mutex_lock(&priv->i2c_mutex);
-
-	/* open tuner I2C repeater for 1 xfer, closes automatically */
-	ret = __i2c_transfer(priv->i2c, gate_open_msg, 1);
+	/* Open tuner I2C repeater for 1 xfer, closes automatically */
+	ret = __i2c_transfer(client->adapter, &msg, 1);
 	if (ret != 1) {
-		dev_warn(&priv->i2c->dev, "%s: i2c wr failed=%d\n",
-				KBUILD_MODNAME, ret);
+		dev_warn(&client->dev, "i2c wr failed=%d\n", ret);
 		if (ret >= 0)
 			ret = -EREMOTEIO;
-
 		return ret;
 	}
 
 	return 0;
 }
 
-static int m88ds3103_deselect(struct i2c_adapter *adap, void *mux_priv,
-		u32 chan)
+/*
+ * XXX: That is wrapper to m88ds3103_probe() via driver core in order to provide
+ * proper I2C client for legacy media attach binding.
+ * New users must use I2C client binding directly!
+ */
+struct dvb_frontend *m88ds3103_attach(const struct m88ds3103_config *cfg,
+		struct i2c_adapter *i2c, struct i2c_adapter **tuner_i2c_adapter)
 {
-	struct m88ds3103_priv *priv = mux_priv;
+	struct i2c_client *client;
+	struct i2c_board_info board_info;
+	struct m88ds3103_platform_data pdata;
+
+	pdata.clk = cfg->clock;
+	pdata.i2c_wr_max = cfg->i2c_wr_max;
+	pdata.ts_mode = cfg->ts_mode;
+	pdata.ts_clk = cfg->ts_clk;
+	pdata.ts_clk_pol = cfg->ts_clk_pol;
+	pdata.spec_inv = cfg->spec_inv;
+	pdata.agc = cfg->agc;
+	pdata.agc_inv = cfg->agc_inv;
+	pdata.clk_out = cfg->clock_out;
+	pdata.envelope_mode = cfg->envelope_mode;
+	pdata.lnb_hv_pol = cfg->lnb_hv_pol;
+	pdata.lnb_en_pol = cfg->lnb_en_pol;
+	pdata.attach_in_use = true;
+
+	memset(&board_info, 0, sizeof(board_info));
+	strlcpy(board_info.type, "m88ds3103", I2C_NAME_SIZE);
+	board_info.addr = cfg->i2c_addr;
+	board_info.platform_data = &pdata;
+	client = i2c_new_device(i2c, &board_info);
+	if (!client || !client->dev.driver)
+		return NULL;
+
+	*tuner_i2c_adapter = pdata.get_i2c_adapter(client);
+	return pdata.get_dvb_frontend(client);
+}
+EXPORT_SYMBOL(m88ds3103_attach);
+
+static struct dvb_frontend_ops m88ds3103_ops = {
+	.delsys = {SYS_DVBS, SYS_DVBS2},
+	.info = {
+		.name = "Montage Technology M88DS3103",
+		.frequency_min =  950000,
+		.frequency_max = 2150000,
+		.frequency_tolerance = 5000,
+		.symbol_rate_min =  1000000,
+		.symbol_rate_max = 45000000,
+		.caps = FE_CAN_INVERSION_AUTO |
+			FE_CAN_FEC_1_2 |
+			FE_CAN_FEC_2_3 |
+			FE_CAN_FEC_3_4 |
+			FE_CAN_FEC_4_5 |
+			FE_CAN_FEC_5_6 |
+			FE_CAN_FEC_6_7 |
+			FE_CAN_FEC_7_8 |
+			FE_CAN_FEC_8_9 |
+			FE_CAN_FEC_AUTO |
+			FE_CAN_QPSK |
+			FE_CAN_RECOVER |
+			FE_CAN_2G_MODULATION
+	},
 
-	mutex_unlock(&priv->i2c_mutex);
+	.release = m88ds3103_release,
 
-	return 0;
+	.get_tune_settings = m88ds3103_get_tune_settings,
+
+	.init = m88ds3103_init,
+	.sleep = m88ds3103_sleep,
+
+	.set_frontend = m88ds3103_set_frontend,
+	.get_frontend = m88ds3103_get_frontend,
+
+	.read_status = m88ds3103_read_status,
+	.read_snr = m88ds3103_read_snr,
+	.read_ber = m88ds3103_read_ber,
+
+	.diseqc_send_master_cmd = m88ds3103_diseqc_send_master_cmd,
+	.diseqc_send_burst = m88ds3103_diseqc_send_burst,
+
+	.set_tone = m88ds3103_set_tone,
+	.set_voltage = m88ds3103_set_voltage,
+};
+
+static struct dvb_frontend *m88ds3103_get_dvb_frontend(struct i2c_client *client)
+{
+	struct m88ds3103_dev *dev = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	return &dev->fe;
 }
 
-struct dvb_frontend *m88ds3103_attach(const struct m88ds3103_config *cfg,
-		struct i2c_adapter *i2c, struct i2c_adapter **tuner_i2c_adapter)
+static struct i2c_adapter *m88ds3103_get_i2c_adapter(struct i2c_client *client)
+{
+	struct m88ds3103_dev *dev = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	return dev->i2c_adapter;
+}
+
+static int m88ds3103_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
 {
+	struct m88ds3103_dev *dev;
+	struct m88ds3103_platform_data *pdata = client->dev.platform_data;
 	int ret;
-	struct m88ds3103_priv *priv;
-	u8 chip_id, u8tmp;
+	unsigned int utmp;
 
-	/* allocate memory for the internal priv */
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
 		ret = -ENOMEM;
-		dev_err(&i2c->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
 		goto err;
 	}
 
-	priv->cfg = cfg;
-	priv->i2c = i2c;
-	mutex_init(&priv->i2c_mutex);
+	dev->client = client;
+	dev->config.clock = pdata->clk;
+	dev->config.i2c_wr_max = pdata->i2c_wr_max;
+	dev->config.ts_mode = pdata->ts_mode;
+	dev->config.ts_clk = pdata->ts_clk;
+	dev->config.ts_clk_pol = pdata->ts_clk_pol;
+	dev->config.spec_inv = pdata->spec_inv;
+	dev->config.agc_inv = pdata->agc_inv;
+	dev->config.clock_out = pdata->clk_out;
+	dev->config.envelope_mode = pdata->envelope_mode;
+	dev->config.agc = pdata->agc;
+	dev->config.lnb_hv_pol = pdata->lnb_hv_pol;
+	dev->config.lnb_en_pol = pdata->lnb_en_pol;
+	dev->cfg = &dev->config;
+	/* create regmap */
+	dev->regmap_config.reg_bits = 8,
+	dev->regmap_config.val_bits = 8,
+	dev->regmap_config.lock_arg = dev,
+	dev->regmap = devm_regmap_init_i2c(client, &dev->regmap_config);
+	if (IS_ERR(dev->regmap)) {
+		ret = PTR_ERR(dev->regmap);
+		goto err_kfree;
+	}
 
 	/* 0x00: chip id[6:0], 0x01: chip ver[7:0], 0x02: chip ver[15:8] */
-	ret = m88ds3103_rd_reg(priv, 0x00, &chip_id);
+	ret = regmap_read(dev->regmap, 0x00, &utmp);
 	if (ret)
-		goto err;
+		goto err_kfree;
 
-	chip_id >>= 1;
-	dev_info(&priv->i2c->dev, "%s: chip_id=%02x\n", __func__, chip_id);
+	dev->chip_id = utmp >> 1;
+	dev_dbg(&client->dev, "chip_id=%02x\n", dev->chip_id);
 
-	switch (chip_id) {
+	switch (dev->chip_id) {
 	case M88RS6000_CHIP_ID:
 	case M88DS3103_CHIP_ID:
 		break;
 	default:
-		goto err;
+		goto err_kfree;
 	}
-	priv->chip_id = chip_id;
 
-	switch (priv->cfg->clock_out) {
+	switch (dev->cfg->clock_out) {
 	case M88DS3103_CLOCK_OUT_DISABLED:
-		u8tmp = 0x80;
+		utmp = 0x80;
 		break;
 	case M88DS3103_CLOCK_OUT_ENABLED:
-		u8tmp = 0x00;
+		utmp = 0x00;
 		break;
 	case M88DS3103_CLOCK_OUT_ENABLED_DIV2:
-		u8tmp = 0x10;
+		utmp = 0x10;
 		break;
 	default:
-		goto err;
+		ret = -EINVAL;
+		goto err_kfree;
 	}
 
 	/* 0x29 register is defined differently for m88rs6000. */
 	/* set internal tuner address to 0x21 */
-	if (chip_id == M88RS6000_CHIP_ID)
-		u8tmp = 0x00;
+	if (dev->chip_id == M88RS6000_CHIP_ID)
+		utmp = 0x00;
 
-	ret = m88ds3103_wr_reg(priv, 0x29, u8tmp);
+	ret = regmap_write(dev->regmap, 0x29, utmp);
 	if (ret)
-		goto err;
+		goto err_kfree;
 
 	/* sleep */
-	ret = m88ds3103_wr_reg_mask(priv, 0x08, 0x00, 0x01);
+	ret = regmap_update_bits(dev->regmap, 0x08, 0x01, 0x00);
 	if (ret)
-		goto err;
-
-	ret = m88ds3103_wr_reg_mask(priv, 0x04, 0x01, 0x01);
+		goto err_kfree;
+	ret = regmap_update_bits(dev->regmap, 0x04, 0x01, 0x01);
 	if (ret)
-		goto err;
-
-	ret = m88ds3103_wr_reg_mask(priv, 0x23, 0x10, 0x10);
+		goto err_kfree;
+	ret = regmap_update_bits(dev->regmap, 0x23, 0x10, 0x10);
 	if (ret)
-		goto err;
+		goto err_kfree;
 
 	/* create mux i2c adapter for tuner */
-	priv->i2c_adapter = i2c_add_mux_adapter(i2c, &i2c->dev, priv, 0, 0, 0,
-			m88ds3103_select, m88ds3103_deselect);
-	if (priv->i2c_adapter == NULL)
-		goto err;
-
-	*tuner_i2c_adapter = priv->i2c_adapter;
+	dev->i2c_adapter = i2c_add_mux_adapter(client->adapter, &client->dev,
+					       dev, 0, 0, 0, m88ds3103_select,
+					       NULL);
+	if (dev->i2c_adapter == NULL) {
+		ret = -ENOMEM;
+		goto err_kfree;
+	}
 
 	/* create dvb_frontend */
-	memcpy(&priv->fe.ops, &m88ds3103_ops, sizeof(struct dvb_frontend_ops));
-	if (priv->chip_id == M88RS6000_CHIP_ID)
-		strncpy(priv->fe.ops.info.name,
-			"Montage M88RS6000", sizeof(priv->fe.ops.info.name));
-	priv->fe.demodulator_priv = priv;
-
-	return &priv->fe;
+	memcpy(&dev->fe.ops, &m88ds3103_ops, sizeof(struct dvb_frontend_ops));
+	if (dev->chip_id == M88RS6000_CHIP_ID)
+		strncpy(dev->fe.ops.info.name, "Montage Technology M88RS6000",
+			sizeof(dev->fe.ops.info.name));
+	if (!pdata->attach_in_use)
+		dev->fe.ops.release = NULL;
+	dev->fe.demodulator_priv = dev;
+	i2c_set_clientdata(client, dev);
+
+	/* setup callbacks */
+	pdata->get_dvb_frontend = m88ds3103_get_dvb_frontend;
+	pdata->get_i2c_adapter = m88ds3103_get_i2c_adapter;
+	return 0;
+err_kfree:
+	kfree(dev);
 err:
-	dev_dbg(&i2c->dev, "%s: failed=%d\n", __func__, ret);
-	kfree(priv);
-	return NULL;
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
 }
-EXPORT_SYMBOL(m88ds3103_attach);
-
-static struct dvb_frontend_ops m88ds3103_ops = {
-	.delsys = { SYS_DVBS, SYS_DVBS2 },
-	.info = {
-		.name = "Montage M88DS3103",
-		.frequency_min =  950000,
-		.frequency_max = 2150000,
-		.frequency_tolerance = 5000,
-		.symbol_rate_min =  1000000,
-		.symbol_rate_max = 45000000,
-		.caps = FE_CAN_INVERSION_AUTO |
-			FE_CAN_FEC_1_2 |
-			FE_CAN_FEC_2_3 |
-			FE_CAN_FEC_3_4 |
-			FE_CAN_FEC_4_5 |
-			FE_CAN_FEC_5_6 |
-			FE_CAN_FEC_6_7 |
-			FE_CAN_FEC_7_8 |
-			FE_CAN_FEC_8_9 |
-			FE_CAN_FEC_AUTO |
-			FE_CAN_QPSK |
-			FE_CAN_RECOVER |
-			FE_CAN_2G_MODULATION
-	},
 
-	.release = m88ds3103_release,
+static int m88ds3103_remove(struct i2c_client *client)
+{
+	struct m88ds3103_dev *dev = i2c_get_clientdata(client);
 
-	.get_tune_settings = m88ds3103_get_tune_settings,
+	dev_dbg(&client->dev, "\n");
 
-	.init = m88ds3103_init,
-	.sleep = m88ds3103_sleep,
+	i2c_del_mux_adapter(dev->i2c_adapter);
 
-	.set_frontend = m88ds3103_set_frontend,
-	.get_frontend = m88ds3103_get_frontend,
-
-	.read_status = m88ds3103_read_status,
-	.read_snr = m88ds3103_read_snr,
-	.read_ber = m88ds3103_read_ber,
+	kfree(dev);
+	return 0;
+}
 
-	.diseqc_send_master_cmd = m88ds3103_diseqc_send_master_cmd,
-	.diseqc_send_burst = m88ds3103_diseqc_send_burst,
+static const struct i2c_device_id m88ds3103_id_table[] = {
+	{"m88ds3103", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, m88ds3103_id_table);
 
-	.set_tone = m88ds3103_set_tone,
-	.set_voltage = m88ds3103_set_voltage,
+static struct i2c_driver m88ds3103_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "m88ds3103",
+		.suppress_bind_attrs = true,
+	},
+	.probe		= m88ds3103_probe,
+	.remove		= m88ds3103_remove,
+	.id_table	= m88ds3103_id_table,
 };
 
+module_i2c_driver(m88ds3103_driver);
+
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
-MODULE_DESCRIPTION("Montage M88DS3103 DVB-S/S2 demodulator driver");
+MODULE_DESCRIPTION("Montage Technology M88DS3103 DVB-S/S2 demodulator driver");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(M88DS3103_FIRMWARE);
 MODULE_FIRMWARE(M88RS6000_FIRMWARE);
diff --git a/drivers/media/dvb-frontends/m88ds3103.h b/drivers/media/dvb-frontends/m88ds3103.h
index 9b3b4962da7c..04b355a005fb 100644
--- a/drivers/media/dvb-frontends/m88ds3103.h
+++ b/drivers/media/dvb-frontends/m88ds3103.h
@@ -1,5 +1,5 @@
 /*
- * Montage M88DS3103 demodulator driver
+ * Montage Technology M88DS3103/M88RS6000 demodulator driver
  *
  * Copyright (C) 2013 Antti Palosaari <crope@iki.fi>
  *
@@ -19,6 +19,63 @@
 
 #include <linux/dvb/frontend.h>
 
+/*
+ * I2C address
+ * 0x68,
+ */
+
+/**
+ * struct m88ds3103_platform_data - Platform data for the m88ds3103 driver
+ * @clk: Clock frequency.
+ * @i2c_wr_max: Max bytes I2C adapter can write at once.
+ * @ts_mode: TS mode.
+ * @ts_clk: TS clock (KHz).
+ * @ts_clk_pol: TS clk polarity. 1-active at falling edge; 0-active at rising
+ *  edge.
+ * @spec_inv: Input spectrum inversion.
+ * @agc: AGC configuration.
+ * @agc_inv: AGC polarity.
+ * @clk_out: Clock output.
+ * @envelope_mode: DiSEqC envelope mode.
+ * @lnb_hv_pol: LNB H/V pin polarity. 0: pin high set to VOLTAGE_18, pin low to
+ *  set VOLTAGE_13. 1: pin high set to VOLTAGE_13, pin low to set VOLTAGE_18.
+ * @lnb_en_pol: LNB enable pin polarity. 0: pin high to disable, pin low to
+ *  enable. 1: pin high to enable, pin low to disable.
+ * @get_dvb_frontend: Get DVB frontend.
+ * @get_i2c_adapter: Get I2C adapter.
+ */
+
+struct m88ds3103_platform_data {
+	u32 clk;
+	u16 i2c_wr_max;
+#define M88DS3103_TS_SERIAL             0 /* TS output pin D0, normal */
+#define M88DS3103_TS_SERIAL_D7          1 /* TS output pin D7 */
+#define M88DS3103_TS_PARALLEL           2 /* TS Parallel mode */
+#define M88DS3103_TS_CI                 3 /* TS CI Mode */
+	u8 ts_mode:2;
+	u32 ts_clk;
+	u8 ts_clk_pol:1;
+	u8 spec_inv:1;
+	u8 agc;
+	u8 agc_inv:1;
+#define M88DS3103_CLOCK_OUT_DISABLED        0
+#define M88DS3103_CLOCK_OUT_ENABLED         1
+#define M88DS3103_CLOCK_OUT_ENABLED_DIV2    2
+	u8 clk_out:2;
+	u8 envelope_mode:1;
+	u8 lnb_hv_pol:1;
+	u8 lnb_en_pol:1;
+
+	struct dvb_frontend* (*get_dvb_frontend)(struct i2c_client *);
+	struct i2c_adapter* (*get_i2c_adapter)(struct i2c_client *);
+
+/* private: For legacy media attach wrapper. Do not set value. */
+	u8 attach_in_use:1;
+};
+
+/*
+ * Do not add new m88ds3103_attach() users! Use I2C bindings instead.
+ */
 struct m88ds3103_config {
 	/*
 	 * I2C address
@@ -113,18 +170,13 @@ struct m88ds3103_config {
 	u8 lnb_en_pol:1;
 };
 
-/*
- * Driver implements own I2C-adapter for tuner I2C access. That's since chip
- * has I2C-gate control which closes gate automatically after I2C transfer.
- * Using own I2C adapter we can workaround that.
- */
-
 #if defined(CONFIG_DVB_M88DS3103) || \
 		(defined(CONFIG_DVB_M88DS3103_MODULE) && defined(MODULE))
 extern struct dvb_frontend *m88ds3103_attach(
 		const struct m88ds3103_config *config,
 		struct i2c_adapter *i2c,
 		struct i2c_adapter **tuner_i2c);
+extern int m88ds3103_get_agc_pwm(struct dvb_frontend *fe, u8 *_agc_pwm);
 #else
 static inline struct dvb_frontend *m88ds3103_attach(
 		const struct m88ds3103_config *config,
@@ -134,6 +186,7 @@ static inline struct dvb_frontend *m88ds3103_attach(
 	pr_warn("%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
+#define m88ds3103_get_agc_pwm NULL
 #endif
 
 #endif
diff --git a/drivers/media/dvb-frontends/m88ds3103_priv.h b/drivers/media/dvb-frontends/m88ds3103_priv.h
index a2c0958111f8..eee8c22c51ec 100644
--- a/drivers/media/dvb-frontends/m88ds3103_priv.h
+++ b/drivers/media/dvb-frontends/m88ds3103_priv.h
@@ -1,5 +1,5 @@
 /*
- * Montage M88DS3103 demodulator driver
+ * Montage Technology M88DS3103/M88RS6000 demodulator driver
  *
  * Copyright (C) 2013 Antti Palosaari <crope@iki.fi>
  *
@@ -22,6 +22,7 @@
 #include "dvb_math.h"
 #include <linux/firmware.h>
 #include <linux/i2c-mux.h>
+#include <linux/regmap.h>
 #include <linux/math64.h>
 
 #define M88DS3103_FIRMWARE "dvb-demod-m88ds3103.fw"
@@ -30,21 +31,24 @@
 #define M88RS6000_CHIP_ID 0x74
 #define M88DS3103_CHIP_ID 0x70
 
-struct m88ds3103_priv {
-	struct i2c_adapter *i2c;
-	/* mutex needed due to own tuner I2C adapter */
-	struct mutex i2c_mutex;
+struct m88ds3103_dev {
+	struct i2c_client *client;
+	struct regmap_config regmap_config;
+	struct regmap *regmap;
+	struct m88ds3103_config config;
 	const struct m88ds3103_config *cfg;
 	struct dvb_frontend fe;
-	fe_delivery_system_t delivery_system;
-	fe_status_t fe_status;
-	u32 ber;
+	enum fe_delivery_system delivery_system;
+	enum fe_status fe_status;
+	u32 dvbv3_ber; /* for old DVBv3 API read_ber */
 	bool warm; /* FW running */
 	struct i2c_adapter *i2c_adapter;
 	/* auto detect chip id to do different config */
 	u8 chip_id;
 	/* main mclk is calculated for M88RS6000 dynamically */
 	u32 mclk_khz;
+	u64 post_bit_error;
+	u64 post_bit_count;
 };
 
 struct m88ds3103_reg_val {
diff --git a/drivers/media/dvb-frontends/m88rs2000.c b/drivers/media/dvb-frontends/m88rs2000.c
index d63bc9c13dce..9b6f464c48bd 100644
--- a/drivers/media/dvb-frontends/m88rs2000.c
+++ b/drivers/media/dvb-frontends/m88rs2000.c
@@ -41,7 +41,7 @@ struct m88rs2000_state {
 	u8 no_lock_count;
 	u32 tuner_frequency;
 	u32 symbol_rate;
-	fe_code_rate_t fec_inner;
+	enum fe_code_rate fec_inner;
 	u8 tuner_level;
 	int errmode;
 };
@@ -247,7 +247,7 @@ static int m88rs2000_send_diseqc_msg(struct dvb_frontend *fe,
 }
 
 static int m88rs2000_send_diseqc_burst(struct dvb_frontend *fe,
-						fe_sec_mini_cmd_t burst)
+				       enum fe_sec_mini_cmd burst)
 {
 	struct m88rs2000_state *state = fe->demodulator_priv;
 	u8 reg0, reg1;
@@ -264,7 +264,8 @@ static int m88rs2000_send_diseqc_burst(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int m88rs2000_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int m88rs2000_set_tone(struct dvb_frontend *fe,
+			      enum fe_sec_tone_mode tone)
 {
 	struct m88rs2000_state *state = fe->demodulator_priv;
 	u8 reg0, reg1;
@@ -412,7 +413,8 @@ static int m88rs2000_tab_set(struct m88rs2000_state *state,
 	return 0;
 }
 
-static int m88rs2000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
+static int m88rs2000_set_voltage(struct dvb_frontend *fe,
+				 enum fe_sec_voltage volt)
 {
 	struct m88rs2000_state *state = fe->demodulator_priv;
 	u8 data;
@@ -462,7 +464,8 @@ static int m88rs2000_sleep(struct dvb_frontend *fe)
 	return ret;
 }
 
-static int m88rs2000_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int m88rs2000_read_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
 {
 	struct m88rs2000_state *state = fe->demodulator_priv;
 	u8 reg = m88rs2000_readreg(state, 0x8c);
@@ -539,7 +542,7 @@ static int m88rs2000_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 }
 
 static int m88rs2000_set_fec(struct m88rs2000_state *state,
-		fe_code_rate_t fec)
+			     enum fe_code_rate fec)
 {
 	u8 fec_set, reg;
 	int ret;
@@ -574,7 +577,7 @@ static int m88rs2000_set_fec(struct m88rs2000_state *state,
 	return ret;
 }
 
-static fe_code_rate_t m88rs2000_get_fec(struct m88rs2000_state *state)
+static enum fe_code_rate m88rs2000_get_fec(struct m88rs2000_state *state)
 {
 	u8 reg;
 	m88rs2000_writereg(state, 0x9a, 0x30);
@@ -606,7 +609,7 @@ static int m88rs2000_set_frontend(struct dvb_frontend *fe)
 {
 	struct m88rs2000_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	fe_status_t status;
+	enum fe_status status;
 	int i, ret = 0;
 	u32 tuner_freq;
 	s16 offset = 0;
diff --git a/drivers/media/dvb-frontends/mb86a16.c b/drivers/media/dvb-frontends/mb86a16.c
index 3ddea4471d2b..79bc671e8769 100644
--- a/drivers/media/dvb-frontends/mb86a16.c
+++ b/drivers/media/dvb-frontends/mb86a16.c
@@ -593,7 +593,7 @@ err:
 	return -EREMOTEIO;
 }
 
-static int mb86a16_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int mb86a16_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	u8 stat, stat2;
 	struct mb86a16_state *state = fe->demodulator_priv;
@@ -1562,7 +1562,8 @@ err:
 	return -EREMOTEIO;
 }
 
-static int mb86a16_send_diseqc_burst(struct dvb_frontend *fe, fe_sec_mini_cmd_t burst)
+static int mb86a16_send_diseqc_burst(struct dvb_frontend *fe,
+				     enum fe_sec_mini_cmd burst)
 {
 	struct mb86a16_state *state = fe->demodulator_priv;
 
@@ -1590,7 +1591,7 @@ err:
 	return -EREMOTEIO;
 }
 
-static int mb86a16_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int mb86a16_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct mb86a16_state *state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/mb86a16.h b/drivers/media/dvb-frontends/mb86a16.h
index e486dc0d8e60..dbd5f43fa128 100644
--- a/drivers/media/dvb-frontends/mb86a16.h
+++ b/drivers/media/dvb-frontends/mb86a16.h
@@ -28,7 +28,8 @@
 struct mb86a16_config {
 	u8 demod_address;
 
-	int (*set_voltage)(struct dvb_frontend *fe, fe_sec_voltage_t voltage);
+	int (*set_voltage)(struct dvb_frontend *fe,
+			   enum fe_sec_voltage voltage);
 };
 
 
diff --git a/drivers/media/dvb-frontends/mb86a20s.c b/drivers/media/dvb-frontends/mb86a20s.c
index 8f54c39ca63f..cfc005ee11d8 100644
--- a/drivers/media/dvb-frontends/mb86a20s.c
+++ b/drivers/media/dvb-frontends/mb86a20s.c
@@ -294,7 +294,7 @@ static int mb86a20s_i2c_readreg(struct mb86a20s_state *state,
  * The functions below assume that gateway lock has already obtained
  */
 
-static int mb86a20s_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int mb86a20s_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	int val;
@@ -1951,7 +1951,7 @@ static int mb86a20s_set_frontend(struct dvb_frontend *fe)
 }
 
 static int mb86a20s_read_status_and_stats(struct dvb_frontend *fe,
-					  fe_status_t *status)
+					  enum fe_status *status)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	int rc, status_nr;
@@ -2042,7 +2042,7 @@ static int mb86a20s_tune(struct dvb_frontend *fe,
 			bool re_tune,
 			unsigned int mode_flags,
 			unsigned int *delay,
-			fe_status_t *status)
+			enum fe_status *status)
 {
 	struct mb86a20s_state *state = fe->demodulator_priv;
 	int rc = 0;
diff --git a/drivers/media/dvb-frontends/mb86a20s.h b/drivers/media/dvb-frontends/mb86a20s.h
index f749c8ac5f39..a113282d6956 100644
--- a/drivers/media/dvb-frontends/mb86a20s.h
+++ b/drivers/media/dvb-frontends/mb86a20s.h
@@ -45,7 +45,7 @@ static inline struct dvb_frontend *mb86a20s_attach(
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
-static struct i2c_adapter *
+static inline struct i2c_adapter *
 	mb86a20s_get_tuner_i2c_adapter(struct dvb_frontend *fe)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
diff --git a/drivers/media/dvb-frontends/mt312.c b/drivers/media/dvb-frontends/mt312.c
index 2163490c1e6b..c36e6764eead 100644
--- a/drivers/media/dvb-frontends/mt312.c
+++ b/drivers/media/dvb-frontends/mt312.c
@@ -156,7 +156,7 @@ static int mt312_reset(struct mt312_state *state, const u8 full)
 }
 
 static int mt312_get_inversion(struct mt312_state *state,
-			       fe_spectral_inversion_t *i)
+			       enum fe_spectral_inversion *i)
 {
 	int ret;
 	u8 vit_mode;
@@ -225,9 +225,9 @@ static int mt312_get_symbol_rate(struct mt312_state *state, u32 *sr)
 	return 0;
 }
 
-static int mt312_get_code_rate(struct mt312_state *state, fe_code_rate_t *cr)
+static int mt312_get_code_rate(struct mt312_state *state, enum fe_code_rate *cr)
 {
-	const fe_code_rate_t fec_tab[8] =
+	const enum fe_code_rate fec_tab[8] =
 	    { FEC_1_2, FEC_2_3, FEC_3_4, FEC_5_6, FEC_6_7, FEC_7_8,
 		FEC_AUTO, FEC_AUTO };
 
@@ -380,7 +380,8 @@ static int mt312_send_master_cmd(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int mt312_send_burst(struct dvb_frontend *fe, const fe_sec_mini_cmd_t c)
+static int mt312_send_burst(struct dvb_frontend *fe,
+			    const enum fe_sec_mini_cmd c)
 {
 	struct mt312_state *state = fe->demodulator_priv;
 	const u8 mini_tab[2] = { 0x02, 0x03 };
@@ -403,7 +404,8 @@ static int mt312_send_burst(struct dvb_frontend *fe, const fe_sec_mini_cmd_t c)
 	return 0;
 }
 
-static int mt312_set_tone(struct dvb_frontend *fe, const fe_sec_tone_mode_t t)
+static int mt312_set_tone(struct dvb_frontend *fe,
+			  const enum fe_sec_tone_mode t)
 {
 	struct mt312_state *state = fe->demodulator_priv;
 	const u8 tone_tab[2] = { 0x01, 0x00 };
@@ -426,7 +428,8 @@ static int mt312_set_tone(struct dvb_frontend *fe, const fe_sec_tone_mode_t t)
 	return 0;
 }
 
-static int mt312_set_voltage(struct dvb_frontend *fe, const fe_sec_voltage_t v)
+static int mt312_set_voltage(struct dvb_frontend *fe,
+			     const enum fe_sec_voltage v)
 {
 	struct mt312_state *state = fe->demodulator_priv;
 	const u8 volt_tab[3] = { 0x00, 0x40, 0x00 };
@@ -442,7 +445,7 @@ static int mt312_set_voltage(struct dvb_frontend *fe, const fe_sec_voltage_t v)
 	return mt312_writereg(state, DISEQC_MODE, val);
 }
 
-static int mt312_read_status(struct dvb_frontend *fe, fe_status_t *s)
+static int mt312_read_status(struct dvb_frontend *fe, enum fe_status *s)
 {
 	struct mt312_state *state = fe->demodulator_priv;
 	int ret;
diff --git a/drivers/media/dvb-frontends/mt352.c b/drivers/media/dvb-frontends/mt352.c
index 2c3b50e828d7..123bb2f8e4b6 100644
--- a/drivers/media/dvb-frontends/mt352.c
+++ b/drivers/media/dvb-frontends/mt352.c
@@ -417,7 +417,7 @@ static int mt352_get_parameters(struct dvb_frontend* fe)
 	return 0;
 }
 
-static int mt352_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int mt352_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct mt352_state* state = fe->demodulator_priv;
 	int s0, s1, s3;
diff --git a/drivers/media/dvb-frontends/nxt200x.c b/drivers/media/dvb-frontends/nxt200x.c
index 8a8e1ecb762d..79c3040912ab 100644
--- a/drivers/media/dvb-frontends/nxt200x.c
+++ b/drivers/media/dvb-frontends/nxt200x.c
@@ -781,7 +781,7 @@ static int nxt200x_setup_frontend_parameters(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int nxt200x_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int nxt200x_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct nxt200x_state* state = fe->demodulator_priv;
 	u8 lock;
diff --git a/drivers/media/dvb-frontends/nxt6000.c b/drivers/media/dvb-frontends/nxt6000.c
index 90ae6c72c0e3..73f9505367ac 100644
--- a/drivers/media/dvb-frontends/nxt6000.c
+++ b/drivers/media/dvb-frontends/nxt6000.c
@@ -109,7 +109,8 @@ static int nxt6000_set_bandwidth(struct nxt6000_state *state, u32 bandwidth)
 	return nxt6000_writereg(state, OFDM_TRL_NOMINALRATE_2, (nominal_rate >> 8) & 0xFF);
 }
 
-static int nxt6000_set_guard_interval(struct nxt6000_state* state, fe_guard_interval_t guard_interval)
+static int nxt6000_set_guard_interval(struct nxt6000_state *state,
+				      enum fe_guard_interval guard_interval)
 {
 	switch (guard_interval) {
 
@@ -131,7 +132,8 @@ static int nxt6000_set_guard_interval(struct nxt6000_state* state, fe_guard_inte
 	}
 }
 
-static int nxt6000_set_inversion(struct nxt6000_state* state, fe_spectral_inversion_t inversion)
+static int nxt6000_set_inversion(struct nxt6000_state *state,
+				 enum fe_spectral_inversion inversion)
 {
 	switch (inversion) {
 
@@ -147,7 +149,9 @@ static int nxt6000_set_inversion(struct nxt6000_state* state, fe_spectral_invers
 	}
 }
 
-static int nxt6000_set_transmission_mode(struct nxt6000_state* state, fe_transmit_mode_t transmission_mode)
+static int
+nxt6000_set_transmission_mode(struct nxt6000_state *state,
+			      enum fe_transmit_mode transmission_mode)
 {
 	int result;
 
@@ -416,7 +420,7 @@ static void nxt6000_dump_status(struct nxt6000_state *state)
 	printk("\n");
 }
 
-static int nxt6000_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int nxt6000_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	u8 core_status;
 	struct nxt6000_state* state = fe->demodulator_priv;
diff --git a/drivers/media/dvb-frontends/or51132.c b/drivers/media/dvb-frontends/or51132.c
index cbbd259eacfe..35b1053b3640 100644
--- a/drivers/media/dvb-frontends/or51132.c
+++ b/drivers/media/dvb-frontends/or51132.c
@@ -63,7 +63,7 @@ struct or51132_state
 	struct dvb_frontend frontend;
 
 	/* Demodulator private data */
-	fe_modulation_t current_modulation;
+	enum fe_modulation current_modulation;
 	u32 snr; /* Result of last SNR calculation */
 
 	/* Tuner private data */
@@ -292,7 +292,7 @@ static int or51132_setmode(struct dvb_frontend* fe)
 #define MOD_FWCLASS_UNKNOWN	0
 #define MOD_FWCLASS_VSB		1
 #define MOD_FWCLASS_QAM		2
-static int modulation_fw_class(fe_modulation_t modulation)
+static int modulation_fw_class(enum fe_modulation modulation)
 {
 	switch(modulation) {
 	case VSB_8:
@@ -415,7 +415,7 @@ start:
 	return 0;
 }
 
-static int or51132_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int or51132_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct or51132_state* state = fe->demodulator_priv;
 	int reg;
diff --git a/drivers/media/dvb-frontends/or51211.c b/drivers/media/dvb-frontends/or51211.c
index 873ea1da844b..e82413b975e6 100644
--- a/drivers/media/dvb-frontends/or51211.c
+++ b/drivers/media/dvb-frontends/or51211.c
@@ -237,7 +237,7 @@ static int or51211_set_parameters(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int or51211_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int or51211_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct or51211_state* state = fe->demodulator_priv;
 	unsigned char rec_buf[2];
diff --git a/drivers/media/dvb-frontends/rtl2830.c b/drivers/media/dvb-frontends/rtl2830.c
index e1b8df62bd59..3d01f4f22aca 100644
--- a/drivers/media/dvb-frontends/rtl2830.c
+++ b/drivers/media/dvb-frontends/rtl2830.c
@@ -392,7 +392,7 @@ err:
 	return ret;
 }
 
-static int rtl2830_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int rtl2830_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct i2c_client *client = fe->demodulator_priv;
 	struct rtl2830_dev *dev = i2c_get_clientdata(client);
diff --git a/drivers/media/dvb-frontends/rtl2830_priv.h b/drivers/media/dvb-frontends/rtl2830_priv.h
index d50d5376c9c5..cf793f39a09b 100644
--- a/drivers/media/dvb-frontends/rtl2830_priv.h
+++ b/drivers/media/dvb-frontends/rtl2830_priv.h
@@ -34,7 +34,7 @@ struct rtl2830_dev {
 	bool sleeping;
 	unsigned long filters;
 	struct delayed_work stat_work;
-	fe_status_t fe_status;
+	enum fe_status fe_status;
 	u64 post_bit_error_prev; /* for old DVBv3 read_ber() calculation */
 	u64 post_bit_error;
 	u64 post_bit_count;
diff --git a/drivers/media/dvb-frontends/rtl2832.c b/drivers/media/dvb-frontends/rtl2832.c
index b400f7b3c2e7..822ea4b7a7ff 100644
--- a/drivers/media/dvb-frontends/rtl2832.c
+++ b/drivers/media/dvb-frontends/rtl2832.c
@@ -358,6 +358,10 @@ static int rtl2832_init(struct dvb_frontend *fe)
 	dev_dbg(&client->dev, "load settings for tuner=%02x\n",
 		dev->pdata->tuner);
 	switch (dev->pdata->tuner) {
+	case RTL2832_TUNER_FC2580:
+		len = ARRAY_SIZE(rtl2832_tuner_init_fc2580);
+		init = rtl2832_tuner_init_fc2580;
+		break;
 	case RTL2832_TUNER_FC0012:
 	case RTL2832_TUNER_FC0013:
 		len = ARRAY_SIZE(rtl2832_tuner_init_fc0012);
@@ -376,6 +380,10 @@ static int rtl2832_init(struct dvb_frontend *fe)
 		len = ARRAY_SIZE(rtl2832_tuner_init_r820t);
 		init = rtl2832_tuner_init_r820t;
 		break;
+	case RTL2832_TUNER_SI2157:
+		len = ARRAY_SIZE(rtl2832_tuner_init_si2157);
+		init = rtl2832_tuner_init_si2157;
+		break;
 	default:
 		ret = -EINVAL;
 		goto err;
@@ -680,7 +688,7 @@ err:
 	return ret;
 }
 
-static int rtl2832_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int rtl2832_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct rtl2832_dev *dev = fe->demodulator_priv;
 	struct i2c_client *client = dev->client;
diff --git a/drivers/media/dvb-frontends/rtl2832.h b/drivers/media/dvb-frontends/rtl2832.h
index a8e912e679a5..c29a4c2bf71a 100644
--- a/drivers/media/dvb-frontends/rtl2832.h
+++ b/drivers/media/dvb-frontends/rtl2832.h
@@ -41,12 +41,14 @@ struct rtl2832_platform_data {
 	/*
 	 * XXX: This list must be kept sync with dvb_usb_rtl28xxu USB IF driver.
 	 */
+#define RTL2832_TUNER_FC2580    0x21
 #define RTL2832_TUNER_TUA9001   0x24
 #define RTL2832_TUNER_FC0012    0x26
 #define RTL2832_TUNER_E4000     0x27
 #define RTL2832_TUNER_FC0013    0x29
 #define RTL2832_TUNER_R820T     0x2a
 #define RTL2832_TUNER_R828D     0x2b
+#define RTL2832_TUNER_SI2157    0x2c
 	u8 tuner;
 
 	struct dvb_frontend* (*get_dvb_frontend)(struct i2c_client *);
diff --git a/drivers/media/dvb-frontends/rtl2832_priv.h b/drivers/media/dvb-frontends/rtl2832_priv.h
index c3a922c37903..5dcd3a41d23f 100644
--- a/drivers/media/dvb-frontends/rtl2832_priv.h
+++ b/drivers/media/dvb-frontends/rtl2832_priv.h
@@ -39,7 +39,7 @@ struct rtl2832_dev {
 	struct i2c_adapter *i2c_adapter_tuner;
 	struct dvb_frontend fe;
 	struct delayed_work stat_work;
-	fe_status_t fe_status;
+	enum fe_status fe_status;
 	u64 post_bit_error_prev; /* for old DVBv3 read_ber() calculation */
 	u64 post_bit_error;
 	u64 post_bit_count;
@@ -252,6 +252,30 @@ enum DVBT_REG_BIT_NAME {
 	DVBT_REG_BIT_NAME_ITEM_TERMINATOR,
 };
 
+static const struct rtl2832_reg_value rtl2832_tuner_init_fc2580[] = {
+	{DVBT_DAGC_TRG_VAL,             0x39},
+	{DVBT_AGC_TARG_VAL_0,            0x0},
+	{DVBT_AGC_TARG_VAL_8_1,         0x5a},
+	{DVBT_AAGC_LOOP_GAIN,           0x16},
+	{DVBT_LOOP_GAIN2_3_0,            0x6},
+	{DVBT_LOOP_GAIN2_4,              0x1},
+	{DVBT_LOOP_GAIN3,               0x16},
+	{DVBT_VTOP1,                    0x35},
+	{DVBT_VTOP2,                    0x21},
+	{DVBT_VTOP3,                    0x21},
+	{DVBT_KRF1,                      0x0},
+	{DVBT_KRF2,                     0x40},
+	{DVBT_KRF3,                     0x10},
+	{DVBT_KRF4,                     0x10},
+	{DVBT_IF_AGC_MIN,               0x80},
+	{DVBT_IF_AGC_MAX,               0x7f},
+	{DVBT_RF_AGC_MIN,               0x9c},
+	{DVBT_RF_AGC_MAX,               0x7f},
+	{DVBT_POLAR_RF_AGC,              0x0},
+	{DVBT_POLAR_IF_AGC,              0x0},
+	{DVBT_AD7_SETTING,            0xe9f4},
+};
+
 static const struct rtl2832_reg_value rtl2832_tuner_init_tua9001[] = {
 	{DVBT_DAGC_TRG_VAL,             0x39},
 	{DVBT_AGC_TARG_VAL_0,            0x0},
@@ -377,4 +401,29 @@ static const struct rtl2832_reg_value rtl2832_tuner_init_r820t[] = {
 	{DVBT_SPEC_INV,                  0x1},
 };
 
+static const struct rtl2832_reg_value rtl2832_tuner_init_si2157[] = {
+	{DVBT_DAGC_TRG_VAL,             0x39},
+	{DVBT_AGC_TARG_VAL_0,            0x0},
+	{DVBT_AGC_TARG_VAL_8_1,         0x40},
+	{DVBT_AAGC_LOOP_GAIN,           0x16},
+	{DVBT_LOOP_GAIN2_3_0,            0x8},
+	{DVBT_LOOP_GAIN2_4,              0x1},
+	{DVBT_LOOP_GAIN3,               0x18},
+	{DVBT_VTOP1,                    0x35},
+	{DVBT_VTOP2,                    0x21},
+	{DVBT_VTOP3,                    0x21},
+	{DVBT_KRF1,                      0x0},
+	{DVBT_KRF2,                     0x40},
+	{DVBT_KRF3,                     0x10},
+	{DVBT_KRF4,                     0x10},
+	{DVBT_IF_AGC_MIN,               0x80},
+	{DVBT_IF_AGC_MAX,               0x7f},
+	{DVBT_RF_AGC_MIN,               0x80},
+	{DVBT_RF_AGC_MAX,               0x7f},
+	{DVBT_POLAR_RF_AGC,              0x0},
+	{DVBT_POLAR_IF_AGC,              0x0},
+	{DVBT_AD7_SETTING,            0xe9f4},
+	{DVBT_SPEC_INV,                  0x0},
+};
+
 #endif /* RTL2832_PRIV_H */
diff --git a/drivers/media/dvb-frontends/rtl2832_sdr.c b/drivers/media/dvb-frontends/rtl2832_sdr.c
index 3ff8806ca584..7edb885ae9c8 100644
--- a/drivers/media/dvb-frontends/rtl2832_sdr.c
+++ b/drivers/media/dvb-frontends/rtl2832_sdr.c
@@ -39,6 +39,10 @@ static bool rtl2832_sdr_emulated_fmt;
 module_param_named(emulated_formats, rtl2832_sdr_emulated_fmt, bool, 0644);
 MODULE_PARM_DESC(emulated_formats, "enable emulated formats (disappears in future)");
 
+/* Original macro does not contain enough null pointer checks for our need */
+#define V4L2_SUBDEV_HAS_OP(sd, o, f) \
+	((sd) && (sd)->ops && (sd)->ops->o && (sd)->ops->o->f)
+
 #define MAX_BULK_BUFS            (10)
 #define BULK_BUFFER_SIZE         (128 * 512)
 
@@ -108,14 +112,15 @@ struct rtl2832_sdr_frame_buf {
 };
 
 struct rtl2832_sdr_dev {
-#define POWER_ON           (1 << 1)
-#define URB_BUF            (1 << 2)
+#define POWER_ON           0  /* BIT(0) */
+#define URB_BUF            1  /* BIT(1) */
 	unsigned long flags;
 
 	struct platform_device *pdev;
 
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
+	struct v4l2_subdev *v4l2_subdev;
 
 	/* videobuf2 queue and queued buffers list */
 	struct vb2_queue vb_queue;
@@ -351,7 +356,7 @@ static int rtl2832_sdr_free_stream_bufs(struct rtl2832_sdr_dev *dev)
 {
 	struct platform_device *pdev = dev->pdev;
 
-	if (dev->flags & USB_STATE_URB_BUF) {
+	if (test_bit(URB_BUF, &dev->flags)) {
 		while (dev->buf_num) {
 			dev->buf_num--;
 			dev_dbg(&pdev->dev, "free buf=%d\n", dev->buf_num);
@@ -360,7 +365,7 @@ static int rtl2832_sdr_free_stream_bufs(struct rtl2832_sdr_dev *dev)
 					  dev->dma_addr[dev->buf_num]);
 		}
 	}
-	dev->flags &= ~USB_STATE_URB_BUF;
+	clear_bit(URB_BUF, &dev->flags);
 
 	return 0;
 }
@@ -389,7 +394,7 @@ static int rtl2832_sdr_alloc_stream_bufs(struct rtl2832_sdr_dev *dev)
 		dev_dbg(&pdev->dev, "alloc buf=%d %p (dma %llu)\n",
 			dev->buf_num, dev->buf_list[dev->buf_num],
 			(long long)dev->dma_addr[dev->buf_num]);
-		dev->flags |= USB_STATE_URB_BUF;
+		set_bit(URB_BUF, &dev->flags);
 	}
 
 	return 0;
@@ -742,6 +747,29 @@ static int rtl2832_sdr_set_adc(struct rtl2832_sdr_dev *dev)
 		ret = rtl2832_sdr_wr_regs(dev, 0x00e, "\xfc", 1);
 		ret = rtl2832_sdr_wr_regs(dev, 0x011, "\xf4", 1);
 		break;
+	case RTL2832_SDR_TUNER_FC2580:
+		ret = rtl2832_sdr_wr_regs(dev, 0x112, "\x39", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x102, "\x40", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x103, "\x5a", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x1c7, "\x2c", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x104, "\xcc", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x105, "\xbe", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x1c8, "\x16", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x106, "\x35", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x1c9, "\x21", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x1ca, "\x21", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x1cb, "\x00", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x107, "\x40", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x1cd, "\x10", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x1ce, "\x10", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x108, "\x80", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x109, "\x7f", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x10a, "\x9c", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x10b, "\x7f", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x00e, "\xfc", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x00e, "\xfc", 1);
+		ret = rtl2832_sdr_wr_regs(dev, 0x011, "\xe9\xf4", 2);
+		break;
 	default:
 		dev_notice(&pdev->dev, "Unsupported tuner\n");
 	}
@@ -832,8 +860,10 @@ static int rtl2832_sdr_set_tuner_freq(struct rtl2832_sdr_dev *dev)
 	if (!test_bit(POWER_ON, &dev->flags))
 		return 0;
 
-	if (fe->ops.tuner_ops.set_params)
-		fe->ops.tuner_ops.set_params(fe);
+	if (!V4L2_SUBDEV_HAS_OP(dev->v4l2_subdev, tuner, s_frequency)) {
+		if (fe->ops.tuner_ops.set_params)
+			fe->ops.tuner_ops.set_params(fe);
+	}
 
 	return 0;
 };
@@ -891,7 +921,11 @@ static int rtl2832_sdr_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	set_bit(POWER_ON, &dev->flags);
 
-	ret = rtl2832_sdr_set_tuner(dev);
+	/* wake-up tuner */
+	if (V4L2_SUBDEV_HAS_OP(dev->v4l2_subdev, core, s_power))
+		ret = v4l2_subdev_call(dev->v4l2_subdev, core, s_power, 1);
+	else
+		ret = rtl2832_sdr_set_tuner(dev);
 	if (ret)
 		goto err;
 
@@ -939,7 +973,12 @@ static void rtl2832_sdr_stop_streaming(struct vb2_queue *vq)
 	rtl2832_sdr_free_stream_bufs(dev);
 	rtl2832_sdr_cleanup_queued_bufs(dev);
 	rtl2832_sdr_unset_adc(dev);
-	rtl2832_sdr_unset_tuner(dev);
+
+	/* sleep tuner */
+	if (V4L2_SUBDEV_HAS_OP(dev->v4l2_subdev, core, s_power))
+		v4l2_subdev_call(dev->v4l2_subdev, core, s_power, 0);
+	else
+		rtl2832_sdr_unset_tuner(dev);
 
 	clear_bit(POWER_ON, &dev->flags);
 
@@ -968,6 +1007,7 @@ static int rtl2832_sdr_g_tuner(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_dev *dev = video_drvdata(file);
 	struct platform_device *pdev = dev->pdev;
+	int ret;
 
 	dev_dbg(&pdev->dev, "index=%d type=%d\n", v->index, v->type);
 
@@ -977,17 +1017,21 @@ static int rtl2832_sdr_g_tuner(struct file *file, void *priv,
 		v->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
 		v->rangelow =   300000;
 		v->rangehigh = 3200000;
+		ret = 0;
+	} else if (v->index == 1 &&
+		   V4L2_SUBDEV_HAS_OP(dev->v4l2_subdev, tuner, g_tuner)) {
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, g_tuner, v);
 	} else if (v->index == 1) {
 		strlcpy(v->name, "RF: <unknown>", sizeof(v->name));
 		v->type = V4L2_TUNER_RF;
 		v->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
 		v->rangelow =    50000000;
 		v->rangehigh = 2000000000;
+		ret = 0;
 	} else {
-		return -EINVAL;
+		ret = -EINVAL;
 	}
-
-	return 0;
+	return ret;
 }
 
 static int rtl2832_sdr_s_tuner(struct file *file, void *priv,
@@ -995,12 +1039,21 @@ static int rtl2832_sdr_s_tuner(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_dev *dev = video_drvdata(file);
 	struct platform_device *pdev = dev->pdev;
+	int ret;
 
 	dev_dbg(&pdev->dev, "\n");
 
-	if (v->index > 1)
-		return -EINVAL;
-	return 0;
+	if (v->index == 0) {
+		ret = 0;
+	} else if (v->index == 1 &&
+		   V4L2_SUBDEV_HAS_OP(dev->v4l2_subdev, tuner, s_tuner)) {
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, s_tuner, v);
+	} else if (v->index == 1) {
+		ret = 0;
+	} else {
+		ret = -EINVAL;
+	}
+	return ret;
 }
 
 static int rtl2832_sdr_enum_freq_bands(struct file *file, void *priv,
@@ -1008,6 +1061,7 @@ static int rtl2832_sdr_enum_freq_bands(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_dev *dev = video_drvdata(file);
 	struct platform_device *pdev = dev->pdev;
+	int ret;
 
 	dev_dbg(&pdev->dev, "tuner=%d type=%d index=%d\n",
 		band->tuner, band->type, band->index);
@@ -1017,16 +1071,20 @@ static int rtl2832_sdr_enum_freq_bands(struct file *file, void *priv,
 			return -EINVAL;
 
 		*band = bands_adc[band->index];
+		ret = 0;
+	} else if (band->tuner == 1 &&
+		   V4L2_SUBDEV_HAS_OP(dev->v4l2_subdev, tuner, enum_freq_bands)) {
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, enum_freq_bands, band);
 	} else if (band->tuner == 1) {
 		if (band->index >= ARRAY_SIZE(bands_fm))
 			return -EINVAL;
 
 		*band = bands_fm[band->index];
+		ret = 0;
 	} else {
-		return -EINVAL;
+		ret = -EINVAL;
 	}
-
-	return 0;
+	return ret;
 }
 
 static int rtl2832_sdr_g_frequency(struct file *file, void *priv,
@@ -1034,20 +1092,25 @@ static int rtl2832_sdr_g_frequency(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_dev *dev = video_drvdata(file);
 	struct platform_device *pdev = dev->pdev;
-	int ret  = 0;
+	int ret;
 
 	dev_dbg(&pdev->dev, "tuner=%d type=%d\n", f->tuner, f->type);
 
 	if (f->tuner == 0) {
 		f->frequency = dev->f_adc;
 		f->type = V4L2_TUNER_ADC;
+		ret = 0;
+	} else if (f->tuner == 1 &&
+		   V4L2_SUBDEV_HAS_OP(dev->v4l2_subdev, tuner, g_frequency)) {
+		f->type = V4L2_TUNER_RF;
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, g_frequency, f);
 	} else if (f->tuner == 1) {
 		f->frequency = dev->f_tuner;
 		f->type = V4L2_TUNER_RF;
+		ret = 0;
 	} else {
-		return -EINVAL;
+		ret = -EINVAL;
 	}
-
 	return ret;
 }
 
@@ -1074,11 +1137,14 @@ static int rtl2832_sdr_s_frequency(struct file *file, void *priv,
 			band = 2;
 
 		dev->f_adc = clamp_t(unsigned int, f->frequency,
-				bands_adc[band].rangelow,
-				bands_adc[band].rangehigh);
+				     bands_adc[band].rangelow,
+				     bands_adc[band].rangehigh);
 
 		dev_dbg(&pdev->dev, "ADC frequency=%u Hz\n", dev->f_adc);
 		ret = rtl2832_sdr_set_adc(dev);
+	} else if (f->tuner == 1 &&
+		   V4L2_SUBDEV_HAS_OP(dev->v4l2_subdev, tuner, s_frequency)) {
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, s_frequency, f);
 	} else if (f->tuner == 1) {
 		dev->f_tuner = clamp_t(unsigned int, f->frequency,
 				bands_fm[0].rangelow,
@@ -1089,7 +1155,6 @@ static int rtl2832_sdr_s_frequency(struct file *file, void *priv,
 	} else {
 		ret = -EINVAL;
 	}
-
 	return ret;
 }
 
@@ -1329,6 +1394,7 @@ static int rtl2832_sdr_probe(struct platform_device *pdev)
 
 	/* setup the state */
 	subdev = pdata->v4l2_subdev;
+	dev->v4l2_subdev = pdata->v4l2_subdev;
 	dev->pdev = pdev;
 	dev->udev = pdata->dvb_usb_device->udev;
 	dev->f_adc = bands_adc[0].rangelow;
@@ -1388,6 +1454,12 @@ static int rtl2832_sdr_probe(struct platform_device *pdev)
 						   6000000);
 		v4l2_ctrl_auto_cluster(2, &dev->bandwidth_auto, 0, false);
 		break;
+	case RTL2832_SDR_TUNER_FC2580:
+		v4l2_ctrl_handler_init(&dev->hdl, 2);
+		if (subdev)
+			v4l2_ctrl_add_handler(&dev->hdl, subdev->ctrl_handler,
+					      NULL);
+		break;
 	default:
 		v4l2_ctrl_handler_init(&dev->hdl, 0);
 		dev_err(&pdev->dev, "Unsupported tuner\n");
diff --git a/drivers/media/dvb-frontends/rtl2832_sdr.h b/drivers/media/dvb-frontends/rtl2832_sdr.h
index d2594768bff2..342ea84860df 100644
--- a/drivers/media/dvb-frontends/rtl2832_sdr.h
+++ b/drivers/media/dvb-frontends/rtl2832_sdr.h
@@ -47,6 +47,7 @@ struct rtl2832_sdr_platform_data {
 	/*
 	 * XXX: This list must be kept sync with dvb_usb_rtl28xxu USB IF driver.
 	 */
+#define RTL2832_SDR_TUNER_FC2580    0x21
 #define RTL2832_SDR_TUNER_TUA9001   0x24
 #define RTL2832_SDR_TUNER_FC0012    0x26
 #define RTL2832_SDR_TUNER_E4000     0x27
diff --git a/drivers/media/dvb-frontends/s5h1409.c b/drivers/media/dvb-frontends/s5h1409.c
index 5ff474a7ff29..10964848a2f1 100644
--- a/drivers/media/dvb-frontends/s5h1409.c
+++ b/drivers/media/dvb-frontends/s5h1409.c
@@ -38,7 +38,7 @@ struct s5h1409_state {
 	struct dvb_frontend frontend;
 
 	/* previous uncorrected block counter */
-	fe_modulation_t current_modulation;
+	enum fe_modulation current_modulation;
 
 	u32 current_frequency;
 	int if_freq;
@@ -400,7 +400,7 @@ static int s5h1409_set_spectralinversion(struct dvb_frontend *fe, int inverted)
 }
 
 static int s5h1409_enable_modulation(struct dvb_frontend *fe,
-				     fe_modulation_t m)
+				     enum fe_modulation m)
 {
 	struct s5h1409_state *state = fe->demodulator_priv;
 
@@ -755,7 +755,7 @@ static int s5h1409_init(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int s5h1409_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int s5h1409_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct s5h1409_state *state = fe->demodulator_priv;
 	u16 reg;
diff --git a/drivers/media/dvb-frontends/s5h1411.c b/drivers/media/dvb-frontends/s5h1411.c
index 64f35fed7ae1..9afc3f42290e 100644
--- a/drivers/media/dvb-frontends/s5h1411.c
+++ b/drivers/media/dvb-frontends/s5h1411.c
@@ -37,7 +37,7 @@ struct s5h1411_state {
 
 	struct dvb_frontend frontend;
 
-	fe_modulation_t current_modulation;
+	enum fe_modulation current_modulation;
 	unsigned int first_tune:1;
 
 	u32 current_frequency;
@@ -484,7 +484,7 @@ static int s5h1411_set_serialmode(struct dvb_frontend *fe, int serial)
 }
 
 static int s5h1411_enable_modulation(struct dvb_frontend *fe,
-				     fe_modulation_t m)
+				     enum fe_modulation m)
 {
 	struct s5h1411_state *state = fe->demodulator_priv;
 
@@ -659,7 +659,7 @@ static int s5h1411_init(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int s5h1411_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int s5h1411_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct s5h1411_state *state = fe->demodulator_priv;
 	u16 reg;
diff --git a/drivers/media/dvb-frontends/s5h1420.c b/drivers/media/dvb-frontends/s5h1420.c
index 93eeaf7118fd..9c22a4c70d87 100644
--- a/drivers/media/dvb-frontends/s5h1420.c
+++ b/drivers/media/dvb-frontends/s5h1420.c
@@ -52,7 +52,7 @@ struct s5h1420_state {
 	u8 postlocked:1;
 	u32 fclk;
 	u32 tunedfreq;
-	fe_code_rate_t fec_inner;
+	enum fe_code_rate fec_inner;
 	u32 symbol_rate;
 
 	/* FIXME: ugly workaround for flexcop's incapable i2c-controller
@@ -124,7 +124,8 @@ static int s5h1420_writereg (struct s5h1420_state* state, u8 reg, u8 data)
 	return 0;
 }
 
-static int s5h1420_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int s5h1420_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct s5h1420_state* state = fe->demodulator_priv;
 
@@ -149,7 +150,8 @@ static int s5h1420_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltag
 	return 0;
 }
 
-static int s5h1420_set_tone (struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int s5h1420_set_tone(struct dvb_frontend *fe,
+			    enum fe_sec_tone_mode tone)
 {
 	struct s5h1420_state* state = fe->demodulator_priv;
 
@@ -180,7 +182,7 @@ static int s5h1420_send_master_cmd (struct dvb_frontend* fe,
 	int result = 0;
 
 	dprintk("enter %s\n", __func__);
-	if (cmd->msg_len > 8)
+	if (cmd->msg_len > sizeof(cmd->msg))
 		return -EINVAL;
 
 	/* setup for DISEQC */
@@ -270,7 +272,8 @@ exit:
 	return result;
 }
 
-static int s5h1420_send_burst (struct dvb_frontend* fe, fe_sec_mini_cmd_t minicmd)
+static int s5h1420_send_burst(struct dvb_frontend *fe,
+			      enum fe_sec_mini_cmd minicmd)
 {
 	struct s5h1420_state* state = fe->demodulator_priv;
 	u8 val;
@@ -307,10 +310,10 @@ static int s5h1420_send_burst (struct dvb_frontend* fe, fe_sec_mini_cmd_t minicm
 	return result;
 }
 
-static fe_status_t s5h1420_get_status_bits(struct s5h1420_state* state)
+static enum fe_status s5h1420_get_status_bits(struct s5h1420_state *state)
 {
 	u8 val;
-	fe_status_t status = 0;
+	enum fe_status status = 0;
 
 	val = s5h1420_readreg(state, 0x14);
 	if (val & 0x02)
@@ -328,7 +331,8 @@ static fe_status_t s5h1420_get_status_bits(struct s5h1420_state* state)
 	return status;
 }
 
-static int s5h1420_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int s5h1420_read_status(struct dvb_frontend *fe,
+			       enum fe_status *status)
 {
 	struct s5h1420_state* state = fe->demodulator_priv;
 	u8 val;
@@ -561,27 +565,33 @@ static void s5h1420_setfec_inversion(struct s5h1420_state* state,
 	} else {
 		switch (p->fec_inner) {
 		case FEC_1_2:
-			vit08 = 0x01; vit09 = 0x10;
+			vit08 = 0x01;
+			vit09 = 0x10;
 			break;
 
 		case FEC_2_3:
-			vit08 = 0x02; vit09 = 0x11;
+			vit08 = 0x02;
+			vit09 = 0x11;
 			break;
 
 		case FEC_3_4:
-			vit08 = 0x04; vit09 = 0x12;
+			vit08 = 0x04;
+			vit09 = 0x12;
 			break;
 
 		case FEC_5_6:
-			vit08 = 0x08; vit09 = 0x13;
+			vit08 = 0x08;
+			vit09 = 0x13;
 			break;
 
 		case FEC_6_7:
-			vit08 = 0x10; vit09 = 0x14;
+			vit08 = 0x10;
+			vit09 = 0x14;
 			break;
 
 		case FEC_7_8:
-			vit08 = 0x20; vit09 = 0x15;
+			vit08 = 0x20;
+			vit09 = 0x15;
 			break;
 
 		default:
@@ -595,7 +605,7 @@ static void s5h1420_setfec_inversion(struct s5h1420_state* state,
 	dprintk("leave %s\n", __func__);
 }
 
-static fe_code_rate_t s5h1420_getfec(struct s5h1420_state* state)
+static enum fe_code_rate s5h1420_getfec(struct s5h1420_state *state)
 {
 	switch(s5h1420_readreg(state, 0x32) & 0x07) {
 	case 0:
@@ -620,7 +630,8 @@ static fe_code_rate_t s5h1420_getfec(struct s5h1420_state* state)
 	return FEC_NONE;
 }
 
-static fe_spectral_inversion_t s5h1420_getinversion(struct s5h1420_state* state)
+static enum fe_spectral_inversion
+s5h1420_getinversion(struct s5h1420_state *state)
 {
 	if (s5h1420_readreg(state, 0x32) & 0x08)
 		return INVERSION_ON;
diff --git a/drivers/media/dvb-frontends/s5h1432.c b/drivers/media/dvb-frontends/s5h1432.c
index 6ec16a243741..4215652f8eb7 100644
--- a/drivers/media/dvb-frontends/s5h1432.c
+++ b/drivers/media/dvb-frontends/s5h1432.c
@@ -36,7 +36,7 @@ struct s5h1432_state {
 
 	struct dvb_frontend frontend;
 
-	fe_modulation_t current_modulation;
+	enum fe_modulation current_modulation;
 	unsigned int first_tune:1;
 
 	u32 current_frequency;
@@ -302,7 +302,7 @@ static int s5h1432_init(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int s5h1432_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int s5h1432_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	return 0;
 }
diff --git a/drivers/media/dvb-frontends/s921.c b/drivers/media/dvb-frontends/s921.c
index 69862e1fd9e9..b2d9fe13e1a0 100644
--- a/drivers/media/dvb-frontends/s921.c
+++ b/drivers/media/dvb-frontends/s921.c
@@ -348,7 +348,7 @@ static int s921_initfe(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int s921_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int s921_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct s921_state *state = fe->demodulator_priv;
 	int regstatus, rc;
@@ -389,7 +389,7 @@ static int s921_read_status(struct dvb_frontend *fe, fe_status_t *status)
 
 static int s921_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 {
-	fe_status_t	status;
+	enum fe_status	status;
 	struct s921_state *state = fe->demodulator_priv;
 	int rc;
 
@@ -449,7 +449,7 @@ static int s921_tune(struct dvb_frontend *fe,
 			bool re_tune,
 			unsigned int mode_flags,
 			unsigned int *delay,
-			fe_status_t *status)
+			enum fe_status *status)
 {
 	int rc = 0;
 
diff --git a/drivers/media/dvb-frontends/s921.h b/drivers/media/dvb-frontends/s921.h
index 7d3999a4e974..f5b722d8081b 100644
--- a/drivers/media/dvb-frontends/s921.h
+++ b/drivers/media/dvb-frontends/s921.h
@@ -36,7 +36,7 @@ static inline struct dvb_frontend *s921_attach(
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
 }
-static struct i2c_adapter *
+static inline struct i2c_adapter *
 	s921_get_tuner_i2c_adapter(struct dvb_frontend *fe)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
diff --git a/drivers/media/dvb-frontends/si2165.c b/drivers/media/dvb-frontends/si2165.c
index 4cc5d10ed0d4..7c2eeee69757 100644
--- a/drivers/media/dvb-frontends/si2165.c
+++ b/drivers/media/dvb-frontends/si2165.c
@@ -698,7 +698,7 @@ static int si2165_sleep(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int si2165_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int si2165_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	int ret;
 	u8 fec_lock = 0;
diff --git a/drivers/media/dvb-frontends/si2168.c b/drivers/media/dvb-frontends/si2168.c
index 5db588ebfc24..25e238c370e5 100644
--- a/drivers/media/dvb-frontends/si2168.c
+++ b/drivers/media/dvb-frontends/si2168.c
@@ -18,23 +18,53 @@
 
 static const struct dvb_frontend_ops si2168_ops;
 
+/* Own I2C adapter locking is needed because of I2C gate logic. */
+static int si2168_i2c_master_send_unlocked(const struct i2c_client *client,
+					   const char *buf, int count)
+{
+	int ret;
+	struct i2c_msg msg = {
+		.addr = client->addr,
+		.flags = 0,
+		.len = count,
+		.buf = (char *)buf,
+	};
+
+	ret = __i2c_transfer(client->adapter, &msg, 1);
+	return (ret == 1) ? count : ret;
+}
+
+static int si2168_i2c_master_recv_unlocked(const struct i2c_client *client,
+					   char *buf, int count)
+{
+	int ret;
+	struct i2c_msg msg = {
+		.addr = client->addr,
+		.flags = I2C_M_RD,
+		.len = count,
+		.buf = buf,
+	};
+
+	ret = __i2c_transfer(client->adapter, &msg, 1);
+	return (ret == 1) ? count : ret;
+}
+
 /* execute firmware command */
-static int si2168_cmd_execute(struct i2c_client *client, struct si2168_cmd *cmd)
+static int si2168_cmd_execute_unlocked(struct i2c_client *client,
+				       struct si2168_cmd *cmd)
 {
-	struct si2168_dev *dev = i2c_get_clientdata(client);
 	int ret;
 	unsigned long timeout;
 
-	mutex_lock(&dev->i2c_mutex);
-
 	if (cmd->wlen) {
 		/* write cmd and args for firmware */
-		ret = i2c_master_send(client, cmd->args, cmd->wlen);
+		ret = si2168_i2c_master_send_unlocked(client, cmd->args,
+						      cmd->wlen);
 		if (ret < 0) {
-			goto err_mutex_unlock;
+			goto err;
 		} else if (ret != cmd->wlen) {
 			ret = -EREMOTEIO;
-			goto err_mutex_unlock;
+			goto err;
 		}
 	}
 
@@ -43,12 +73,13 @@ static int si2168_cmd_execute(struct i2c_client *client, struct si2168_cmd *cmd)
 		#define TIMEOUT 70
 		timeout = jiffies + msecs_to_jiffies(TIMEOUT);
 		while (!time_after(jiffies, timeout)) {
-			ret = i2c_master_recv(client, cmd->args, cmd->rlen);
+			ret = si2168_i2c_master_recv_unlocked(client, cmd->args,
+							      cmd->rlen);
 			if (ret < 0) {
-				goto err_mutex_unlock;
+				goto err;
 			} else if (ret != cmd->rlen) {
 				ret = -EREMOTEIO;
-				goto err_mutex_unlock;
+				goto err;
 			}
 
 			/* firmware ready? */
@@ -60,22 +91,36 @@ static int si2168_cmd_execute(struct i2c_client *client, struct si2168_cmd *cmd)
 				jiffies_to_msecs(jiffies) -
 				(jiffies_to_msecs(timeout) - TIMEOUT));
 
+		/* error bit set? */
+		if ((cmd->args[0] >> 6) & 0x01) {
+			ret = -EREMOTEIO;
+			goto err;
+		}
+
 		if (!((cmd->args[0] >> 7) & 0x01)) {
 			ret = -ETIMEDOUT;
-			goto err_mutex_unlock;
+			goto err;
 		}
 	}
 
-	mutex_unlock(&dev->i2c_mutex);
 	return 0;
-
-err_mutex_unlock:
-	mutex_unlock(&dev->i2c_mutex);
+err:
 	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-static int si2168_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int si2168_cmd_execute(struct i2c_client *client, struct si2168_cmd *cmd)
+{
+	int ret;
+
+	i2c_lock_adapter(client->adapter);
+	ret = si2168_cmd_execute_unlocked(client, cmd);
+	i2c_unlock_adapter(client->adapter);
+
+	return ret;
+}
+
+static int si2168_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct i2c_client *client = fe->demodulator_priv;
 	struct si2168_dev *dev = i2c_get_clientdata(client);
@@ -508,6 +553,8 @@ static int si2168_init(struct dvb_frontend *fe)
 	/* set ts mode */
 	memcpy(cmd.args, "\x14\x00\x01\x10\x10\x00", 6);
 	cmd.args[4] |= dev->ts_mode;
+	if (dev->ts_clock_gapped)
+		cmd.args[4] |= 0x40;
 	cmd.wlen = 6;
 	cmd.rlen = 4;
 	ret = si2168_cmd_execute(client, &cmd);
@@ -561,60 +608,46 @@ static int si2168_get_tune_settings(struct dvb_frontend *fe,
 
 /*
  * I2C gate logic
- * We must use unlocked i2c_transfer() here because I2C lock is already taken
- * by tuner driver.
+ * We must use unlocked I2C I/O because I2C adapter lock is already taken
+ * by the caller (usually tuner driver).
  */
 static int si2168_select(struct i2c_adapter *adap, void *mux_priv, u32 chan)
 {
 	struct i2c_client *client = mux_priv;
-	struct si2168_dev *dev = i2c_get_clientdata(client);
 	int ret;
-	struct i2c_msg gate_open_msg = {
-		.addr = client->addr,
-		.flags = 0,
-		.len = 3,
-		.buf = "\xc0\x0d\x01",
-	};
-
-	mutex_lock(&dev->i2c_mutex);
+	struct si2168_cmd cmd;
 
-	/* open tuner I2C gate */
-	ret = __i2c_transfer(client->adapter, &gate_open_msg, 1);
-	if (ret != 1) {
-		dev_warn(&client->dev, "i2c write failed=%d\n", ret);
-		if (ret >= 0)
-			ret = -EREMOTEIO;
-	} else {
-		ret = 0;
-	}
+	/* open I2C gate */
+	memcpy(cmd.args, "\xc0\x0d\x01", 3);
+	cmd.wlen = 3;
+	cmd.rlen = 0;
+	ret = si2168_cmd_execute_unlocked(client, &cmd);
+	if (ret)
+		goto err;
 
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int si2168_deselect(struct i2c_adapter *adap, void *mux_priv, u32 chan)
 {
 	struct i2c_client *client = mux_priv;
-	struct si2168_dev *dev = i2c_get_clientdata(client);
 	int ret;
-	struct i2c_msg gate_close_msg = {
-		.addr = client->addr,
-		.flags = 0,
-		.len = 3,
-		.buf = "\xc0\x0d\x00",
-	};
-
-	/* close tuner I2C gate */
-	ret = __i2c_transfer(client->adapter, &gate_close_msg, 1);
-	if (ret != 1) {
-		dev_warn(&client->dev, "i2c write failed=%d\n", ret);
-		if (ret >= 0)
-			ret = -EREMOTEIO;
-	} else {
-		ret = 0;
-	}
+	struct si2168_cmd cmd;
 
-	mutex_unlock(&dev->i2c_mutex);
+	/* close I2C gate */
+	memcpy(cmd.args, "\xc0\x0d\x00", 3);
+	cmd.wlen = 3;
+	cmd.rlen = 0;
+	ret = si2168_cmd_execute_unlocked(client, &cmd);
+	if (ret)
+		goto err;
 
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -671,8 +704,6 @@ static int si2168_probe(struct i2c_client *client,
 		goto err;
 	}
 
-	mutex_init(&dev->i2c_mutex);
-
 	/* create mux i2c adapter for tuner */
 	dev->adapter = i2c_add_mux_adapter(client->adapter, &client->dev,
 			client, 0, 0, 0, si2168_select, si2168_deselect);
@@ -688,6 +719,7 @@ static int si2168_probe(struct i2c_client *client,
 	*config->fe = &dev->fe;
 	dev->ts_mode = config->ts_mode;
 	dev->ts_clock_inv = config->ts_clock_inv;
+	dev->ts_clock_gapped = config->ts_clock_gapped;
 	dev->fw_loaded = false;
 
 	i2c_set_clientdata(client, dev);
diff --git a/drivers/media/dvb-frontends/si2168.h b/drivers/media/dvb-frontends/si2168.h
index 70d702ae6f49..3225d0cc93c7 100644
--- a/drivers/media/dvb-frontends/si2168.h
+++ b/drivers/media/dvb-frontends/si2168.h
@@ -42,6 +42,9 @@ struct si2168_config {
 
 	/* TS clock inverted */
 	bool ts_clock_inv;
+
+	/* TS clock gapped */
+	bool ts_clock_gapped;
 };
 
 #endif
diff --git a/drivers/media/dvb-frontends/si2168_priv.h b/drivers/media/dvb-frontends/si2168_priv.h
index d7efce8043ed..c07e6fe2cb10 100644
--- a/drivers/media/dvb-frontends/si2168_priv.h
+++ b/drivers/media/dvb-frontends/si2168_priv.h
@@ -30,14 +30,14 @@
 /* state struct */
 struct si2168_dev {
 	struct i2c_adapter *adapter;
-	struct mutex i2c_mutex;
 	struct dvb_frontend fe;
-	fe_delivery_system_t delivery_system;
-	fe_status_t fe_status;
+	enum fe_delivery_system delivery_system;
+	enum fe_status fe_status;
 	bool active;
 	bool fw_loaded;
 	u8 ts_mode;
 	bool ts_clock_inv;
+	bool ts_clock_gapped;
 };
 
 /* firmware command struct */
diff --git a/drivers/media/dvb-frontends/si21xx.c b/drivers/media/dvb-frontends/si21xx.c
index 16850e2bf02f..62ad7a7be9f8 100644
--- a/drivers/media/dvb-frontends/si21xx.c
+++ b/drivers/media/dvb-frontends/si21xx.c
@@ -410,7 +410,7 @@ static int si21xx_send_diseqc_msg(struct dvb_frontend *fe,
 }
 
 static int si21xx_send_diseqc_burst(struct dvb_frontend *fe,
-						fe_sec_mini_cmd_t burst)
+				    enum fe_sec_mini_cmd burst)
 {
 	struct si21xx_state *state = fe->demodulator_priv;
 	u8 val;
@@ -434,7 +434,7 @@ static int si21xx_send_diseqc_burst(struct dvb_frontend *fe,
 	return 0;
 }
 /*	30.06.2008 */
-static int si21xx_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int si21xx_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct si21xx_state *state = fe->demodulator_priv;
 	u8 val;
@@ -454,7 +454,7 @@ static int si21xx_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
 	}
 }
 
-static int si21xx_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
+static int si21xx_set_voltage(struct dvb_frontend *fe, enum fe_sec_voltage volt)
 {
 	struct si21xx_state *state = fe->demodulator_priv;
 
@@ -536,7 +536,7 @@ static int si21xx_init(struct dvb_frontend *fe)
 
 }
 
-static int si21_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int si21_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct si21xx_state *state = fe->demodulator_priv;
 	u8 regs_read[2];
@@ -641,7 +641,7 @@ static int si21_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 /*	initiates a channel acquisition sequence
 	using the specified symbol rate and code rate */
 static int si21xx_setacquire(struct dvb_frontend *fe, int symbrate,
-						fe_code_rate_t crate)
+			     enum fe_code_rate crate)
 {
 
 	struct si21xx_state *state = fe->demodulator_priv;
diff --git a/drivers/media/dvb-frontends/sp8870.c b/drivers/media/dvb-frontends/sp8870.c
index 57dc2abaa87b..e87ac30d7fb8 100644
--- a/drivers/media/dvb-frontends/sp8870.c
+++ b/drivers/media/dvb-frontends/sp8870.c
@@ -350,7 +350,8 @@ static int sp8870_init (struct dvb_frontend* fe)
 	return 0;
 }
 
-static int sp8870_read_status (struct dvb_frontend* fe, fe_status_t * fe_status)
+static int sp8870_read_status(struct dvb_frontend *fe,
+			      enum fe_status *fe_status)
 {
 	struct sp8870_state* state = fe->demodulator_priv;
 	int status;
diff --git a/drivers/media/dvb-frontends/sp887x.c b/drivers/media/dvb-frontends/sp887x.c
index 1bb81b5ae6e0..4378fe1b978e 100644
--- a/drivers/media/dvb-frontends/sp887x.c
+++ b/drivers/media/dvb-frontends/sp887x.c
@@ -416,7 +416,7 @@ static int sp887x_setup_frontend_parameters(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int sp887x_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int sp887x_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct sp887x_state* state = fe->demodulator_priv;
 	u16 snr12 = sp887x_readreg(state, 0xf16);
diff --git a/drivers/media/dvb-frontends/stb0899_drv.c b/drivers/media/dvb-frontends/stb0899_drv.c
index c73899d3a53d..756650f154ab 100644
--- a/drivers/media/dvb-frontends/stb0899_drv.c
+++ b/drivers/media/dvb-frontends/stb0899_drv.c
@@ -792,7 +792,8 @@ static int stb0899_wait_diseqc_txidle(struct stb0899_state *state, int timeout)
 	return 0;
 }
 
-static int stb0899_send_diseqc_burst(struct dvb_frontend *fe, fe_sec_mini_cmd_t burst)
+static int stb0899_send_diseqc_burst(struct dvb_frontend *fe,
+				     enum fe_sec_mini_cmd burst)
 {
 	struct stb0899_state *state = fe->demodulator_priv;
 	u8 reg, old_state;
@@ -1178,7 +1179,8 @@ static int stb0899_read_ber(struct dvb_frontend *fe, u32 *ber)
 	return 0;
 }
 
-static int stb0899_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int stb0899_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct stb0899_state *state = fe->demodulator_priv;
 
@@ -1205,7 +1207,7 @@ static int stb0899_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage
 	return 0;
 }
 
-static int stb0899_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int stb0899_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct stb0899_state *state = fe->demodulator_priv;
 	struct stb0899_internal *internal = &state->internal;
diff --git a/drivers/media/dvb-frontends/stv0288.c b/drivers/media/dvb-frontends/stv0288.c
index 632b25156e4c..c93d9a45f7f7 100644
--- a/drivers/media/dvb-frontends/stv0288.c
+++ b/drivers/media/dvb-frontends/stv0288.c
@@ -44,7 +44,7 @@ struct stv0288_state {
 	u8 initialised:1;
 	u32 tuner_frequency;
 	u32 symbol_rate;
-	fe_code_rate_t fec_inner;
+	enum fe_code_rate fec_inner;
 	int errmode;
 };
 
@@ -134,20 +134,20 @@ static int stv0288_set_symbolrate(struct dvb_frontend *fe, u32 srate)
 
 	temp = (unsigned int)srate / 1000;
 
-		temp = temp * 32768;
-		temp = temp / 25;
-		temp = temp / 125;
-		b[0] = (unsigned char)((temp >> 12) & 0xff);
-		b[1] = (unsigned char)((temp >> 4) & 0xff);
-		b[2] = (unsigned char)((temp << 4) & 0xf0);
-		stv0288_writeregI(state, 0x28, 0x80); /* SFRH */
-		stv0288_writeregI(state, 0x29, 0); /* SFRM */
-		stv0288_writeregI(state, 0x2a, 0); /* SFRL */
-
-		stv0288_writeregI(state, 0x28, b[0]);
-		stv0288_writeregI(state, 0x29, b[1]);
-		stv0288_writeregI(state, 0x2a, b[2]);
-		dprintk("stv0288: stv0288_set_symbolrate\n");
+	temp = temp * 32768;
+	temp = temp / 25;
+	temp = temp / 125;
+	b[0] = (unsigned char)((temp >> 12) & 0xff);
+	b[1] = (unsigned char)((temp >> 4) & 0xff);
+	b[2] = (unsigned char)((temp << 4) & 0xf0);
+	stv0288_writeregI(state, 0x28, 0x80); /* SFRH */
+	stv0288_writeregI(state, 0x29, 0); /* SFRM */
+	stv0288_writeregI(state, 0x2a, 0); /* SFRL */
+
+	stv0288_writeregI(state, 0x28, b[0]);
+	stv0288_writeregI(state, 0x29, b[1]);
+	stv0288_writeregI(state, 0x2a, b[2]);
+	dprintk("stv0288: stv0288_set_symbolrate\n");
 
 	return 0;
 }
@@ -174,7 +174,7 @@ static int stv0288_send_diseqc_msg(struct dvb_frontend *fe,
 }
 
 static int stv0288_send_diseqc_burst(struct dvb_frontend *fe,
-						fe_sec_mini_cmd_t burst)
+				     enum fe_sec_mini_cmd burst)
 {
 	struct stv0288_state *state = fe->demodulator_priv;
 
@@ -193,7 +193,7 @@ static int stv0288_send_diseqc_burst(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int stv0288_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int stv0288_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct stv0288_state *state = fe->demodulator_priv;
 
@@ -323,7 +323,8 @@ static u8 stv0288_inittab[] = {
 	0xff, 0xff,
 };
 
-static int stv0288_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
+static int stv0288_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage volt)
 {
 	dprintk("%s: %s\n", __func__,
 		volt == SEC_VOLTAGE_13 ? "SEC_VOLTAGE_13" :
@@ -361,7 +362,7 @@ static int stv0288_init(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int stv0288_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int stv0288_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct stv0288_state *state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/stv0297.c b/drivers/media/dvb-frontends/stv0297.c
index d40f226160ef..75b4d8b25657 100644
--- a/drivers/media/dvb-frontends/stv0297.c
+++ b/drivers/media/dvb-frontends/stv0297.c
@@ -136,10 +136,10 @@ static u32 stv0297_get_symbolrate(struct stv0297_state *state)
 {
 	u64 tmp;
 
-	tmp = stv0297_readreg(state, 0x55);
-	tmp |= stv0297_readreg(state, 0x56) << 8;
-	tmp |= stv0297_readreg(state, 0x57) << 16;
-	tmp |= stv0297_readreg(state, 0x58) << 24;
+	tmp = (u64)(stv0297_readreg(state, 0x55)
+		    | (stv0297_readreg(state, 0x56) << 8)
+		    | (stv0297_readreg(state, 0x57) << 16)
+		    | (stv0297_readreg(state, 0x58) << 24));
 
 	tmp *= STV0297_CLOCK_KHZ;
 	tmp >>= 32;
@@ -233,7 +233,8 @@ static void stv0297_set_initialdemodfreq(struct stv0297_state *state, long freq)
 	stv0297_writereg(state, 0x20, tmp);
 }
 
-static int stv0297_set_qam(struct stv0297_state *state, fe_modulation_t modulation)
+static int stv0297_set_qam(struct stv0297_state *state,
+			   enum fe_modulation modulation)
 {
 	int val = 0;
 
@@ -267,7 +268,8 @@ static int stv0297_set_qam(struct stv0297_state *state, fe_modulation_t modulati
 	return 0;
 }
 
-static int stv0297_set_inversion(struct stv0297_state *state, fe_spectral_inversion_t inversion)
+static int stv0297_set_inversion(struct stv0297_state *state,
+				 enum fe_spectral_inversion inversion)
 {
 	int val = 0;
 
@@ -325,7 +327,8 @@ static int stv0297_sleep(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int stv0297_read_status(struct dvb_frontend *fe, fe_status_t * status)
+static int stv0297_read_status(struct dvb_frontend *fe,
+			       enum fe_status *status)
 {
 	struct stv0297_state *state = fe->demodulator_priv;
 
@@ -415,7 +418,7 @@ static int stv0297_set_frontend(struct dvb_frontend *fe)
 	int sweeprate;
 	int carrieroffset;
 	unsigned long timeout;
-	fe_spectral_inversion_t inversion;
+	enum fe_spectral_inversion inversion;
 
 	switch (p->modulation) {
 	case QAM_16:
diff --git a/drivers/media/dvb-frontends/stv0299.c b/drivers/media/dvb-frontends/stv0299.c
index b57ecf42e75a..a8177807fb65 100644
--- a/drivers/media/dvb-frontends/stv0299.c
+++ b/drivers/media/dvb-frontends/stv0299.c
@@ -44,6 +44,7 @@
 
 #include <linux/init.h>
 #include <linux/kernel.h>
+#include <linux/ktime.h>
 #include <linux/module.h>
 #include <linux/string.h>
 #include <linux/slab.h>
@@ -61,7 +62,7 @@ struct stv0299_state {
 	u8 initialised:1;
 	u32 tuner_frequency;
 	u32 symbol_rate;
-	fe_code_rate_t fec_inner;
+	enum fe_code_rate fec_inner;
 	int errmode;
 	u32 ucblocks;
 	u8 mcr_reg;
@@ -134,7 +135,7 @@ static int stv0299_readregs (struct stv0299_state* state, u8 reg1, u8 *b, u8 len
 	return ret == 2 ? 0 : ret;
 }
 
-static int stv0299_set_FEC (struct stv0299_state* state, fe_code_rate_t fec)
+static int stv0299_set_FEC(struct stv0299_state *state, enum fe_code_rate fec)
 {
 	dprintk ("%s\n", __func__);
 
@@ -170,10 +171,10 @@ static int stv0299_set_FEC (struct stv0299_state* state, fe_code_rate_t fec)
     }
 }
 
-static fe_code_rate_t stv0299_get_fec (struct stv0299_state* state)
+static enum fe_code_rate stv0299_get_fec(struct stv0299_state *state)
 {
-	static fe_code_rate_t fec_tab [] = { FEC_2_3, FEC_3_4, FEC_5_6,
-					     FEC_7_8, FEC_1_2 };
+	static enum fe_code_rate fec_tab[] = { FEC_2_3, FEC_3_4, FEC_5_6,
+					       FEC_7_8, FEC_1_2 };
 	u8 index;
 
 	dprintk ("%s\n", __func__);
@@ -302,7 +303,8 @@ static int stv0299_send_diseqc_msg (struct dvb_frontend* fe,
 	return 0;
 }
 
-static int stv0299_send_diseqc_burst (struct dvb_frontend* fe, fe_sec_mini_cmd_t burst)
+static int stv0299_send_diseqc_burst(struct dvb_frontend *fe,
+				     enum fe_sec_mini_cmd burst)
 {
 	struct stv0299_state* state = fe->demodulator_priv;
 	u8 val;
@@ -329,7 +331,8 @@ static int stv0299_send_diseqc_burst (struct dvb_frontend* fe, fe_sec_mini_cmd_t
 	return 0;
 }
 
-static int stv0299_set_tone (struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int stv0299_set_tone(struct dvb_frontend *fe,
+			    enum fe_sec_tone_mode tone)
 {
 	struct stv0299_state* state = fe->demodulator_priv;
 	u8 val;
@@ -351,7 +354,8 @@ static int stv0299_set_tone (struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
 	}
 }
 
-static int stv0299_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int stv0299_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct stv0299_state* state = fe->demodulator_priv;
 	u8 reg0x08;
@@ -404,8 +408,8 @@ static int stv0299_send_legacy_dish_cmd (struct dvb_frontend* fe, unsigned long
 	u8 lv_mask = 0x40;
 	u8 last = 1;
 	int i;
-	struct timeval nexttime;
-	struct timeval tv[10];
+	ktime_t nexttime;
+	ktime_t tv[10];
 
 	reg0x08 = stv0299_readreg (state, 0x08);
 	reg0x0c = stv0299_readreg (state, 0x0c);
@@ -418,7 +422,7 @@ static int stv0299_send_legacy_dish_cmd (struct dvb_frontend* fe, unsigned long
 	if (debug_legacy_dish_switch)
 		printk ("%s switch command: 0x%04lx\n",__func__, cmd);
 
-	do_gettimeofday (&nexttime);
+	nexttime = ktime_get_real();
 	if (debug_legacy_dish_switch)
 		tv[0] = nexttime;
 	stv0299_writeregI (state, 0x0c, reg0x0c | 0x50); /* set LNB to 18V */
@@ -427,7 +431,7 @@ static int stv0299_send_legacy_dish_cmd (struct dvb_frontend* fe, unsigned long
 
 	for (i=0; i<9; i++) {
 		if (debug_legacy_dish_switch)
-			do_gettimeofday (&tv[i+1]);
+			tv[i+1] = ktime_get_real();
 		if((cmd & 0x01) != last) {
 			/* set voltage to (last ? 13V : 18V) */
 			stv0299_writeregI (state, 0x0c, reg0x0c | (last ? lv_mask : 0x50));
@@ -443,7 +447,8 @@ static int stv0299_send_legacy_dish_cmd (struct dvb_frontend* fe, unsigned long
 		printk ("%s(%d): switch delay (should be 32k followed by all 8k\n",
 			__func__, fe->dvb->num);
 		for (i = 1; i < 10; i++)
-			printk ("%d: %d\n", i, timeval_usec_diff(tv[i-1] , tv[i]));
+			printk("%d: %d\n", i,
+			       (int) ktime_us_delta(tv[i], tv[i-1]));
 	}
 
 	return 0;
@@ -476,7 +481,8 @@ static int stv0299_init (struct dvb_frontend* fe)
 	return 0;
 }
 
-static int stv0299_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int stv0299_read_status(struct dvb_frontend *fe,
+			       enum fe_status *status)
 {
 	struct stv0299_state* state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/stv0367.c b/drivers/media/dvb-frontends/stv0367.c
index b31ff265ff24..ec3e18e5ff50 100644
--- a/drivers/media/dvb-frontends/stv0367.c
+++ b/drivers/media/dvb-frontends/stv0367.c
@@ -59,7 +59,7 @@ struct stv0367cab_state {
 	int locked;			/* channel found		*/
 	u32 freq_khz;			/* found frequency (in kHz)	*/
 	u32 symbol_rate;		/* found symbol rate (in Bds)	*/
-	fe_spectral_inversion_t	spect_inv; /* Spectrum Inversion	*/
+	enum fe_spectral_inversion spect_inv; /* Spectrum Inversion	*/
 };
 
 struct stv0367ter_state {
@@ -67,10 +67,10 @@ struct stv0367ter_state {
 	enum stv0367_ter_signal_type state;
 	enum stv0367_ter_if_iq_mode if_iq_mode;
 	enum stv0367_ter_mode mode;/* mode 2K or 8K */
-	fe_guard_interval_t guard;
+	enum fe_guard_interval guard;
 	enum stv0367_ter_hierarchy hierarchy;
 	u32 frequency;
-	fe_spectral_inversion_t  sense; /*  current search spectrum */
+	enum fe_spectral_inversion sense; /*  current search spectrum */
 	u8  force; /* force mode/guard */
 	u8  bw; /* channel width 6, 7 or 8 in MHz */
 	u8  pBW; /* channel width used during previous lock */
@@ -2074,7 +2074,8 @@ static int stv0367ter_status(struct dvb_frontend *fe)
 	return locked;
 }
 #endif
-static int stv0367ter_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int stv0367ter_read_status(struct dvb_frontend *fe,
+				  enum fe_status *status)
 {
 	struct stv0367_state *state = fe->demodulator_priv;
 
@@ -2716,7 +2717,8 @@ static u32 stv0367cab_GetSymbolRate(struct stv0367_state *state, u32 mclk_hz)
 	return regsym;
 }
 
-static int stv0367cab_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int stv0367cab_read_status(struct dvb_frontend *fe,
+				  enum fe_status *status)
 {
 	struct stv0367_state *state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/stv0367_priv.h b/drivers/media/dvb-frontends/stv0367_priv.h
index 995db0689ddd..89bf6f64b078 100644
--- a/drivers/media/dvb-frontends/stv0367_priv.h
+++ b/drivers/media/dvb-frontends/stv0367_priv.h
@@ -188,7 +188,7 @@ struct stv0367_cab_signal_info {
 	u32 frequency; /* kHz */
 	u32 symbol_rate; /* Mbds */
 	enum stv0367cab_mod modulation;
-	fe_spectral_inversion_t spect_inv;
+	enum fe_spectral_inversion spect_inv;
 	s32 Power_dBmx10;	/* Power of the RF signal (dBm x 10) */
 	u32	CN_dBx10;	/* Carrier to noise ratio (dB x 10) */
 	u32	BER;		/* Bit error rate (x 10000000)	*/
diff --git a/drivers/media/dvb-frontends/stv0900_core.c b/drivers/media/dvb-frontends/stv0900_core.c
index 2c88abfab531..fe31dd541955 100644
--- a/drivers/media/dvb-frontends/stv0900_core.c
+++ b/drivers/media/dvb-frontends/stv0900_core.c
@@ -1744,7 +1744,8 @@ static int stv0900_send_master_cmd(struct dvb_frontend *fe,
 				state->demod);
 }
 
-static int stv0900_send_burst(struct dvb_frontend *fe, fe_sec_mini_cmd_t burst)
+static int stv0900_send_burst(struct dvb_frontend *fe,
+			      enum fe_sec_mini_cmd burst)
 {
 	struct stv0900_state *state = fe->demodulator_priv;
 	struct stv0900_internal *intp = state->internal;
@@ -1793,7 +1794,8 @@ static int stv0900_recv_slave_reply(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int stv0900_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t toneoff)
+static int stv0900_set_tone(struct dvb_frontend *fe,
+			    enum fe_sec_tone_mode toneoff)
 {
 	struct stv0900_state *state = fe->demodulator_priv;
 	struct stv0900_internal *intp = state->internal;
diff --git a/drivers/media/dvb-frontends/stv0900_sw.c b/drivers/media/dvb-frontends/stv0900_sw.c
index a0a7b1664c53..fa63a9e929ce 100644
--- a/drivers/media/dvb-frontends/stv0900_sw.c
+++ b/drivers/media/dvb-frontends/stv0900_sw.c
@@ -1556,8 +1556,8 @@ static u32 stv0900_search_srate_fine(struct dvb_frontend *fe)
 	}
 
 	symbcomp = 13 * (coarse_srate / 10);
-		coarse_freq = (stv0900_read_reg(intp, CFR2) << 8)
-					| stv0900_read_reg(intp, CFR1);
+	coarse_freq = (stv0900_read_reg(intp, CFR2) << 8)
+		      | stv0900_read_reg(intp, CFR1);
 
 	if (symbcomp < intp->symbol_rate[demod])
 		coarse_srate = 0;
@@ -2009,7 +2009,7 @@ enum fe_stv0900_signal_type stv0900_algo(struct dvb_frontend *fe)
 			signal_type = STV0900_NODATA;
 			no_signal = stv0900_check_signal_presence(intp, demod);
 
-				intp->result[demod].locked = FALSE;
+			intp->result[demod].locked = FALSE;
 		}
 	}
 
diff --git a/drivers/media/dvb-frontends/stv090x.c b/drivers/media/dvb-frontends/stv090x.c
index 0b2a934f53e5..25bdf6e0f963 100644
--- a/drivers/media/dvb-frontends/stv090x.c
+++ b/drivers/media/dvb-frontends/stv090x.c
@@ -3732,7 +3732,7 @@ static int stv090x_read_cnr(struct dvb_frontend *fe, u16 *cnr)
 	return 0;
 }
 
-static int stv090x_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int stv090x_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct stv090x_state *state = fe->demodulator_priv;
 	u32 reg;
@@ -3822,7 +3822,8 @@ err:
 	return -1;
 }
 
-static int stv090x_send_diseqc_burst(struct dvb_frontend *fe, fe_sec_mini_cmd_t burst)
+static int stv090x_send_diseqc_burst(struct dvb_frontend *fe,
+				     enum fe_sec_mini_cmd burst)
 {
 	struct stv090x_state *state = fe->demodulator_priv;
 	u32 reg, idle = 0, fifo_full = 1;
diff --git a/drivers/media/dvb-frontends/stv6110.c b/drivers/media/dvb-frontends/stv6110.c
index b1425830a24e..91c6dcf65d2a 100644
--- a/drivers/media/dvb-frontends/stv6110.c
+++ b/drivers/media/dvb-frontends/stv6110.c
@@ -158,7 +158,7 @@ static int stv6110_sleep(struct dvb_frontend *fe)
 	return 0;
 }
 
-static u32 carrier_width(u32 symbol_rate, fe_rolloff_t rolloff)
+static u32 carrier_width(u32 symbol_rate, enum fe_rolloff rolloff)
 {
 	u32 rlf;
 
diff --git a/drivers/media/dvb-frontends/tc90522.c b/drivers/media/dvb-frontends/tc90522.c
index dce22ce35d20..456cdc7fb1e7 100644
--- a/drivers/media/dvb-frontends/tc90522.c
+++ b/drivers/media/dvb-frontends/tc90522.c
@@ -130,7 +130,7 @@ static int tc90522t_set_layers(struct dvb_frontend *fe)
 
 /* frontend ops */
 
-static int tc90522s_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int tc90522s_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct tc90522_state *state;
 	int ret;
@@ -158,7 +158,7 @@ static int tc90522s_read_status(struct dvb_frontend *fe, fe_status_t *status)
 	return 0;
 }
 
-static int tc90522t_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int tc90522t_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct tc90522_state *state;
 	int ret;
@@ -194,7 +194,7 @@ static int tc90522t_read_status(struct dvb_frontend *fe, fe_status_t *status)
 	return 0;
 }
 
-static const fe_code_rate_t fec_conv_sat[] = {
+static const enum fe_code_rate fec_conv_sat[] = {
 	FEC_NONE, /* unused */
 	FEC_1_2, /* for BPSK */
 	FEC_1_2, FEC_2_3, FEC_3_4, FEC_5_6, FEC_7_8, /* for QPSK */
@@ -238,7 +238,10 @@ static int tc90522s_get_frontend(struct dvb_frontend *fe)
 			c->layer[1].segment_count = 0;
 		else
 			c->layer[1].segment_count = val[4] & 0x3f; /* slots */
-		/* actually, BPSK if v==1, but not defined in fe_modulation_t */
+		/*
+		 * actually, BPSK if v==1, but not defined in
+		 * enum fe_modulation
+		 */
 		c->layer[1].modulation = QPSK;
 		layers = (v > 0) ? 2 : 1;
 	}
@@ -319,18 +322,18 @@ static int tc90522s_get_frontend(struct dvb_frontend *fe)
 }
 
 
-static const fe_transmit_mode_t tm_conv[] = {
+static const enum fe_transmit_mode tm_conv[] = {
 	TRANSMISSION_MODE_2K,
 	TRANSMISSION_MODE_4K,
 	TRANSMISSION_MODE_8K,
 	0
 };
 
-static const fe_code_rate_t fec_conv_ter[] = {
+static const enum fe_code_rate fec_conv_ter[] = {
 	FEC_1_2, FEC_2_3, FEC_3_4, FEC_5_6, FEC_7_8, 0, 0, 0
 };
 
-static const fe_modulation_t mod_conv[] = {
+static const enum fe_modulation mod_conv[] = {
 	DQPSK, QPSK, QAM_16, QAM_64, 0, 0, 0, 0
 };
 
diff --git a/drivers/media/dvb-frontends/tda10021.c b/drivers/media/dvb-frontends/tda10021.c
index 1bff7f457e19..a684424e665a 100644
--- a/drivers/media/dvb-frontends/tda10021.c
+++ b/drivers/media/dvb-frontends/tda10021.c
@@ -129,8 +129,8 @@ static int unlock_tuner(struct tda10021_state* state)
 	return 0;
 }
 
-static int tda10021_setup_reg0 (struct tda10021_state* state, u8 reg0,
-				fe_spectral_inversion_t inversion)
+static int tda10021_setup_reg0(struct tda10021_state *state, u8 reg0,
+			       enum fe_spectral_inversion inversion)
 {
 	reg0 |= state->reg0 & 0x63;
 
@@ -258,7 +258,7 @@ static int tda10021_set_parameters(struct dvb_frontend *fe)
 	}
 
 	/*
-	 * gcc optimizes the code bellow the same way as it would code:
+	 * gcc optimizes the code below the same way as it would code:
 	 *           "if (qam > 5) return -EINVAL;"
 	 * Yet, the code is clearer, as it shows what QAM standards are
 	 * supported by the driver, and avoids the usage of magic numbers on
@@ -308,7 +308,8 @@ static int tda10021_set_parameters(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int tda10021_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int tda10021_read_status(struct dvb_frontend *fe,
+				enum fe_status *status)
 {
 	struct tda10021_state* state = fe->demodulator_priv;
 	int sync;
diff --git a/drivers/media/dvb-frontends/tda10023.c b/drivers/media/dvb-frontends/tda10023.c
index ca1e0d54b69a..44a55656093f 100644
--- a/drivers/media/dvb-frontends/tda10023.c
+++ b/drivers/media/dvb-frontends/tda10023.c
@@ -331,7 +331,7 @@ static int tda10023_set_parameters(struct dvb_frontend *fe)
 	}
 
 	/*
-	 * gcc optimizes the code bellow the same way as it would code:
+	 * gcc optimizes the code below the same way as it would code:
 	 *		 "if (qam > 5) return -EINVAL;"
 	 * Yet, the code is clearer, as it shows what QAM standards are
 	 * supported by the driver, and avoids the usage of magic numbers on
@@ -376,7 +376,8 @@ static int tda10023_set_parameters(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int tda10023_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int tda10023_read_status(struct dvb_frontend *fe,
+				enum fe_status *status)
 {
 	struct tda10023_state* state = fe->demodulator_priv;
 	int sync;
diff --git a/drivers/media/dvb-frontends/tda10048.c b/drivers/media/dvb-frontends/tda10048.c
index 71fb63299de7..8451086c563f 100644
--- a/drivers/media/dvb-frontends/tda10048.c
+++ b/drivers/media/dvb-frontends/tda10048.c
@@ -792,7 +792,7 @@ static int tda10048_init(struct dvb_frontend *fe)
 	return ret;
 }
 
-static int tda10048_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int tda10048_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct tda10048_state *state = fe->demodulator_priv;
 	u8 reg;
diff --git a/drivers/media/dvb-frontends/tda1004x.c b/drivers/media/dvb-frontends/tda1004x.c
index a2631be7ffac..0e209b56c76c 100644
--- a/drivers/media/dvb-frontends/tda1004x.c
+++ b/drivers/media/dvb-frontends/tda1004x.c
@@ -650,7 +650,7 @@ static int tda10046_init(struct dvb_frontend* fe)
 
 	if (tda10046_fwupload(fe)) {
 		printk("tda1004x: firmware upload failed\n");
-			return -EIO;
+		return -EIO;
 	}
 
 	// tda setup
@@ -1005,7 +1005,8 @@ static int tda1004x_get_fe(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int tda1004x_read_status(struct dvb_frontend* fe, fe_status_t * fe_status)
+static int tda1004x_read_status(struct dvb_frontend *fe,
+				enum fe_status *fe_status)
 {
 	struct tda1004x_state* state = fe->demodulator_priv;
 	int status;
diff --git a/drivers/media/dvb-frontends/tda10071.c b/drivers/media/dvb-frontends/tda10071.c
index 4a19b85995f1..f6dc6307d35a 100644
--- a/drivers/media/dvb-frontends/tda10071.c
+++ b/drivers/media/dvb-frontends/tda10071.c
@@ -203,7 +203,7 @@ error:
 }
 
 static int tda10071_set_tone(struct dvb_frontend *fe,
-	fe_sec_tone_mode_t fe_sec_tone_mode)
+	enum fe_sec_tone_mode fe_sec_tone_mode)
 {
 	struct tda10071_priv *priv = fe->demodulator_priv;
 	struct tda10071_cmd cmd;
@@ -249,7 +249,7 @@ error:
 }
 
 static int tda10071_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t fe_sec_voltage)
+	enum fe_sec_voltage fe_sec_voltage)
 {
 	struct tda10071_priv *priv = fe->demodulator_priv;
 	struct tda10071_cmd cmd;
@@ -413,7 +413,7 @@ error:
 }
 
 static int tda10071_diseqc_send_burst(struct dvb_frontend *fe,
-	fe_sec_mini_cmd_t fe_sec_mini_cmd)
+	enum fe_sec_mini_cmd fe_sec_mini_cmd)
 {
 	struct tda10071_priv *priv = fe->demodulator_priv;
 	struct tda10071_cmd cmd;
@@ -476,7 +476,7 @@ error:
 	return ret;
 }
 
-static int tda10071_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int tda10071_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct tda10071_priv *priv = fe->demodulator_priv;
 	int ret;
@@ -668,7 +668,7 @@ static int tda10071_set_frontend(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i;
 	u8 mode, rolloff, pilot, inversion, div;
-	fe_modulation_t modulation;
+	enum fe_modulation modulation;
 
 	dev_dbg(&priv->i2c->dev,
 			"%s: delivery_system=%d modulation=%d frequency=%d symbol_rate=%d inversion=%d pilot=%d rolloff=%d\n",
@@ -1313,6 +1313,113 @@ static struct dvb_frontend_ops tda10071_ops = {
 	.set_voltage = tda10071_set_voltage,
 };
 
+static struct dvb_frontend *tda10071_get_dvb_frontend(struct i2c_client *client)
+{
+	struct tda10071_priv *dev = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	return &dev->fe;
+}
+
+static int tda10071_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct tda10071_priv *dev;
+	struct tda10071_platform_data *pdata = client->dev.platform_data;
+	int ret;
+	u8 u8tmp;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	dev->client = client;
+	dev->i2c = client->adapter;
+	dev->cfg.demod_i2c_addr = client->addr;
+	dev->cfg.i2c_wr_max = pdata->i2c_wr_max;
+	dev->cfg.ts_mode = pdata->ts_mode;
+	dev->cfg.spec_inv = pdata->spec_inv;
+	dev->cfg.xtal = pdata->clk;
+	dev->cfg.pll_multiplier = pdata->pll_multiplier;
+	dev->cfg.tuner_i2c_addr = pdata->tuner_i2c_addr;
+
+	/* chip ID */
+	ret = tda10071_rd_reg(dev, 0xff, &u8tmp);
+	if (ret)
+		goto err_kfree;
+	if (u8tmp != 0x0f) {
+		ret = -ENODEV;
+		goto err_kfree;
+	}
+
+	/* chip type */
+	ret = tda10071_rd_reg(dev, 0xdd, &u8tmp);
+	if (ret)
+		goto err_kfree;
+	if (u8tmp != 0x00) {
+		ret = -ENODEV;
+		goto err_kfree;
+	}
+
+	/* chip version */
+	ret = tda10071_rd_reg(dev, 0xfe, &u8tmp);
+	if (ret)
+		goto err_kfree;
+	if (u8tmp != 0x01) {
+		ret = -ENODEV;
+		goto err_kfree;
+	}
+
+	/* create dvb_frontend */
+	memcpy(&dev->fe.ops, &tda10071_ops, sizeof(struct dvb_frontend_ops));
+	dev->fe.ops.release = NULL;
+	dev->fe.demodulator_priv = dev;
+	i2c_set_clientdata(client, dev);
+
+	/* setup callbacks */
+	pdata->get_dvb_frontend = tda10071_get_dvb_frontend;
+
+	dev_info(&client->dev, "NXP TDA10071 successfully identified\n");
+	return 0;
+err_kfree:
+	kfree(dev);
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
+}
+
+static int tda10071_remove(struct i2c_client *client)
+{
+	struct tda10071_dev *dev = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	kfree(dev);
+	return 0;
+}
+
+static const struct i2c_device_id tda10071_id_table[] = {
+	{"tda10071_cx24118", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, tda10071_id_table);
+
+static struct i2c_driver tda10071_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tda10071",
+		.suppress_bind_attrs = true,
+	},
+	.probe		= tda10071_probe,
+	.remove		= tda10071_remove,
+	.id_table	= tda10071_id_table,
+};
+
+module_i2c_driver(tda10071_driver);
+
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("NXP TDA10071 DVB-S/S2 demodulator driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb-frontends/tda10071.h b/drivers/media/dvb-frontends/tda10071.h
index da89f4249846..0ffbfa5b2dfb 100644
--- a/drivers/media/dvb-frontends/tda10071.h
+++ b/drivers/media/dvb-frontends/tda10071.h
@@ -24,6 +24,35 @@
 #include <linux/kconfig.h>
 #include <linux/dvb/frontend.h>
 
+/*
+ * I2C address
+ * 0x55,
+ */
+
+/**
+ * struct tda10071_platform_data - Platform data for the tda10071 driver
+ * @clk: Clock frequency.
+ * @i2c_wr_max: Max bytes I2C adapter can write at once.
+ * @ts_mode: TS mode.
+ * @spec_inv: Input spectrum inversion.
+ * @pll_multiplier: PLL multiplier.
+ * @tuner_i2c_addr: CX24118A tuner I2C address (0x14, 0x54, ...).
+ * @get_dvb_frontend: Get DVB frontend.
+ */
+
+struct tda10071_platform_data {
+	u32 clk;
+	u16 i2c_wr_max;
+#define TDA10071_TS_SERIAL        0
+#define TDA10071_TS_PARALLEL      1
+	u8 ts_mode;
+	bool spec_inv;
+	u8 pll_multiplier;
+	u8 tuner_i2c_addr;
+
+	struct dvb_frontend* (*get_dvb_frontend)(struct i2c_client *);
+};
+
 struct tda10071_config {
 	/* Demodulator I2C address.
 	 * Default: none, must set
diff --git a/drivers/media/dvb-frontends/tda10071_priv.h b/drivers/media/dvb-frontends/tda10071_priv.h
index 03f839c431e9..54d7c713eec8 100644
--- a/drivers/media/dvb-frontends/tda10071_priv.h
+++ b/drivers/media/dvb-frontends/tda10071_priv.h
@@ -28,20 +28,21 @@
 struct tda10071_priv {
 	struct i2c_adapter *i2c;
 	struct dvb_frontend fe;
+	struct i2c_client *client;
 	struct tda10071_config cfg;
 
 	u8 meas_count[2];
 	u32 ber;
 	u32 ucb;
-	fe_status_t fe_status;
-	fe_delivery_system_t delivery_system;
+	enum fe_status fe_status;
+	enum fe_delivery_system delivery_system;
 	bool warm; /* FW running */
 };
 
 static struct tda10071_modcod {
-	fe_delivery_system_t delivery_system;
-	fe_modulation_t modulation;
-	fe_code_rate_t fec;
+	enum fe_delivery_system delivery_system;
+	enum fe_modulation modulation;
+	enum fe_code_rate fec;
 	u8 val;
 } TDA10071_MODCOD[] = {
 	/* NBC-QPSK */
diff --git a/drivers/media/dvb-frontends/tda10086.c b/drivers/media/dvb-frontends/tda10086.c
index fcfe2e080cb0..95a33e187f8e 100644
--- a/drivers/media/dvb-frontends/tda10086.c
+++ b/drivers/media/dvb-frontends/tda10086.c
@@ -185,7 +185,8 @@ static void tda10086_diseqc_wait(struct tda10086_state *state)
 	}
 }
 
-static int tda10086_set_tone (struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int tda10086_set_tone(struct dvb_frontend *fe,
+			     enum fe_sec_tone_mode tone)
 {
 	struct tda10086_state* state = fe->demodulator_priv;
 	u8 t22k_off = 0x80;
@@ -238,7 +239,8 @@ static int tda10086_send_master_cmd (struct dvb_frontend* fe,
 	return 0;
 }
 
-static int tda10086_send_burst (struct dvb_frontend* fe, fe_sec_mini_cmd_t minicmd)
+static int tda10086_send_burst(struct dvb_frontend *fe,
+			       enum fe_sec_mini_cmd minicmd)
 {
 	struct tda10086_state* state = fe->demodulator_priv;
 	u8 oldval = tda10086_read_byte(state, 0x36);
@@ -472,8 +474,8 @@ static int tda10086_get_frontend(struct dvb_frontend *fe)
 		return -EINVAL;
 
 	/* calculate the updated frequency (note: we convert from Hz->kHz) */
-	tmp64 = tda10086_read_byte(state, 0x52);
-	tmp64 |= (tda10086_read_byte(state, 0x51) << 8);
+	tmp64 = ((u64)tda10086_read_byte(state, 0x52)
+		| (tda10086_read_byte(state, 0x51) << 8));
 	if (tmp64 & 0x8000)
 		tmp64 |= 0xffffffffffff0000ULL;
 	tmp64 = (tmp64 * (SACLK/1000ULL));
@@ -551,7 +553,8 @@ static int tda10086_get_frontend(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int tda10086_read_status(struct dvb_frontend* fe, fe_status_t *fe_status)
+static int tda10086_read_status(struct dvb_frontend *fe,
+				enum fe_status *fe_status)
 {
 	struct tda10086_state* state = fe->demodulator_priv;
 	u8 val;
diff --git a/drivers/media/dvb-frontends/tda8083.c b/drivers/media/dvb-frontends/tda8083.c
index 69e62f42e2e1..796543fa2c8d 100644
--- a/drivers/media/dvb-frontends/tda8083.c
+++ b/drivers/media/dvb-frontends/tda8083.c
@@ -97,7 +97,8 @@ static inline u8 tda8083_readreg (struct tda8083_state* state, u8 reg)
 	return val;
 }
 
-static int tda8083_set_inversion (struct tda8083_state* state, fe_spectral_inversion_t inversion)
+static int tda8083_set_inversion(struct tda8083_state *state,
+				 enum fe_spectral_inversion inversion)
 {
 	/*  XXX FIXME: implement other modes than FEC_AUTO */
 	if (inversion == INVERSION_AUTO)
@@ -106,7 +107,7 @@ static int tda8083_set_inversion (struct tda8083_state* state, fe_spectral_inver
 	return -EINVAL;
 }
 
-static int tda8083_set_fec (struct tda8083_state* state, fe_code_rate_t fec)
+static int tda8083_set_fec(struct tda8083_state *state, enum fe_code_rate fec)
 {
 	if (fec == FEC_AUTO)
 		return tda8083_writereg (state, 0x07, 0xff);
@@ -117,11 +118,13 @@ static int tda8083_set_fec (struct tda8083_state* state, fe_code_rate_t fec)
 	return -EINVAL;
 }
 
-static fe_code_rate_t tda8083_get_fec (struct tda8083_state* state)
+static enum fe_code_rate tda8083_get_fec(struct tda8083_state *state)
 {
 	u8 index;
-	static fe_code_rate_t fec_tab [] = { FEC_8_9, FEC_1_2, FEC_2_3, FEC_3_4,
-				       FEC_4_5, FEC_5_6, FEC_6_7, FEC_7_8 };
+	static enum fe_code_rate fec_tab[] = {
+		FEC_8_9, FEC_1_2, FEC_2_3, FEC_3_4,
+		FEC_4_5, FEC_5_6, FEC_6_7, FEC_7_8
+	};
 
 	index = tda8083_readreg(state, 0x0e) & 0x07;
 
@@ -178,7 +181,8 @@ static void tda8083_wait_diseqc_fifo (struct tda8083_state* state, int timeout)
 	}
 }
 
-static int tda8083_set_tone (struct tda8083_state* state, fe_sec_tone_mode_t tone)
+static int tda8083_set_tone(struct tda8083_state *state,
+			    enum fe_sec_tone_mode tone)
 {
 	tda8083_writereg (state, 0x26, 0xf1);
 
@@ -192,7 +196,8 @@ static int tda8083_set_tone (struct tda8083_state* state, fe_sec_tone_mode_t ton
 	}
 }
 
-static int tda8083_set_voltage (struct tda8083_state* state, fe_sec_voltage_t voltage)
+static int tda8083_set_voltage(struct tda8083_state *state,
+			       enum fe_sec_voltage voltage)
 {
 	switch (voltage) {
 	case SEC_VOLTAGE_13:
@@ -204,7 +209,8 @@ static int tda8083_set_voltage (struct tda8083_state* state, fe_sec_voltage_t vo
 	}
 }
 
-static int tda8083_send_diseqc_burst (struct tda8083_state* state, fe_sec_mini_cmd_t burst)
+static int tda8083_send_diseqc_burst(struct tda8083_state *state,
+				     enum fe_sec_mini_cmd burst)
 {
 	switch (burst) {
 	case SEC_MINI_A:
@@ -222,8 +228,8 @@ static int tda8083_send_diseqc_burst (struct tda8083_state* state, fe_sec_mini_c
 	return 0;
 }
 
-static int tda8083_send_diseqc_msg (struct dvb_frontend* fe,
-				    struct dvb_diseqc_master_cmd *m)
+static int tda8083_send_diseqc_msg(struct dvb_frontend *fe,
+				   struct dvb_diseqc_master_cmd *m)
 {
 	struct tda8083_state* state = fe->demodulator_priv;
 	int i;
@@ -240,7 +246,8 @@ static int tda8083_send_diseqc_msg (struct dvb_frontend* fe,
 	return 0;
 }
 
-static int tda8083_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int tda8083_read_status(struct dvb_frontend *fe,
+			       enum fe_status *status)
 {
 	struct tda8083_state* state = fe->demodulator_priv;
 
@@ -372,7 +379,8 @@ static int tda8083_init(struct dvb_frontend* fe)
 	return 0;
 }
 
-static int tda8083_diseqc_send_burst(struct dvb_frontend* fe, fe_sec_mini_cmd_t burst)
+static int tda8083_diseqc_send_burst(struct dvb_frontend *fe,
+				     enum fe_sec_mini_cmd burst)
 {
 	struct tda8083_state* state = fe->demodulator_priv;
 
@@ -383,7 +391,8 @@ static int tda8083_diseqc_send_burst(struct dvb_frontend* fe, fe_sec_mini_cmd_t
 	return 0;
 }
 
-static int tda8083_diseqc_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int tda8083_diseqc_set_tone(struct dvb_frontend *fe,
+				   enum fe_sec_tone_mode tone)
 {
 	struct tda8083_state* state = fe->demodulator_priv;
 
@@ -394,7 +403,8 @@ static int tda8083_diseqc_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t t
 	return 0;
 }
 
-static int tda8083_diseqc_set_voltage(struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int tda8083_diseqc_set_voltage(struct dvb_frontend *fe,
+				      enum fe_sec_voltage voltage)
 {
 	struct tda8083_state* state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/ts2020.c b/drivers/media/dvb-frontends/ts2020.c
index 90164a38cd36..f61b143a0052 100644
--- a/drivers/media/dvb-frontends/ts2020.c
+++ b/drivers/media/dvb-frontends/ts2020.c
@@ -21,23 +21,32 @@
 
 #include "dvb_frontend.h"
 #include "ts2020.h"
+#include <linux/regmap.h>
+#include <linux/math64.h>
 
 #define TS2020_XTAL_FREQ   27000 /* in kHz */
 #define FREQ_OFFSET_LOW_SYM_RATE 3000
 
 struct ts2020_priv {
+	struct i2c_client *client;
+	struct mutex regmap_mutex;
+	struct regmap_config regmap_config;
+	struct regmap *regmap;
 	struct dvb_frontend *fe;
+	struct delayed_work stat_work;
+	int (*get_agc_pwm)(struct dvb_frontend *fe, u8 *_agc_pwm);
 	/* i2c details */
-	int i2c_address;
 	struct i2c_adapter *i2c;
+	int i2c_address;
+	bool loop_through:1;
 	u8 clk_out:2;
 	u8 clk_out_div:5;
-	u32 frequency;
-	u32 frequency_div;
+	bool dont_poll:1;
+	u32 frequency_div; /* LO output divider switch frequency */
+	u32 frequency_khz; /* actual used LO frequency */
 #define TS2020_M88TS2020 0
 #define TS2020_M88TS2022 1
 	u8 tuner;
-	u8 loop_through:1;
 };
 
 struct ts2020_reg_val {
@@ -45,84 +54,23 @@ struct ts2020_reg_val {
 	u8 val;
 };
 
-static int ts2020_release(struct dvb_frontend *fe)
-{
-	kfree(fe->tuner_priv);
-	fe->tuner_priv = NULL;
-	return 0;
-}
+static void ts2020_stat_work(struct work_struct *work);
 
-static int ts2020_writereg(struct dvb_frontend *fe, int reg, int data)
+static int ts2020_release(struct dvb_frontend *fe)
 {
 	struct ts2020_priv *priv = fe->tuner_priv;
-	u8 buf[] = { reg, data };
-	struct i2c_msg msg[] = {
-		{
-			.addr = priv->i2c_address,
-			.flags = 0,
-			.buf = buf,
-			.len = 2
-		}
-	};
-	int err;
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1);
-
-	err = i2c_transfer(priv->i2c, msg, 1);
-	if (err != 1) {
-		printk(KERN_ERR
-		       "%s: writereg error(err == %i, reg == 0x%02x, value == 0x%02x)\n",
-		       __func__, err, reg, data);
-		return -EREMOTEIO;
-	}
+	struct i2c_client *client = priv->client;
 
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
+	dev_dbg(&client->dev, "\n");
 
+	i2c_unregister_device(client);
 	return 0;
 }
 
-static int ts2020_readreg(struct dvb_frontend *fe, u8 reg)
-{
-	struct ts2020_priv *priv = fe->tuner_priv;
-	int ret;
-	u8 b0[] = { reg };
-	u8 b1[] = { 0 };
-	struct i2c_msg msg[] = {
-		{
-			.addr = priv->i2c_address,
-			.flags = 0,
-			.buf = b0,
-			.len = 1
-		}, {
-			.addr = priv->i2c_address,
-			.flags = I2C_M_RD,
-			.buf = b1,
-			.len = 1
-		}
-	};
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1);
-
-	ret = i2c_transfer(priv->i2c, msg, 2);
-
-	if (ret != 2) {
-		printk(KERN_ERR "%s: reg=0x%x(error=%d)\n",
-		       __func__, reg, ret);
-		return ret;
-	}
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
-
-	return b1[0];
-}
-
 static int ts2020_sleep(struct dvb_frontend *fe)
 {
 	struct ts2020_priv *priv = fe->tuner_priv;
+	int ret;
 	u8 u8tmp;
 
 	if (priv->tuner == TS2020_M88TS2020)
@@ -130,24 +78,32 @@ static int ts2020_sleep(struct dvb_frontend *fe)
 	else
 		u8tmp = 0x00;
 
-	return ts2020_writereg(fe, u8tmp, 0x00);
+	ret = regmap_write(priv->regmap, u8tmp, 0x00);
+	if (ret < 0)
+		return ret;
+
+	/* stop statistics polling */
+	if (!priv->dont_poll)
+		cancel_delayed_work_sync(&priv->stat_work);
+	return 0;
 }
 
 static int ts2020_init(struct dvb_frontend *fe)
 {
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct ts2020_priv *priv = fe->tuner_priv;
 	int i;
 	u8 u8tmp;
 
 	if (priv->tuner == TS2020_M88TS2020) {
-		ts2020_writereg(fe, 0x42, 0x73);
-		ts2020_writereg(fe, 0x05, priv->clk_out_div);
-		ts2020_writereg(fe, 0x20, 0x27);
-		ts2020_writereg(fe, 0x07, 0x02);
-		ts2020_writereg(fe, 0x11, 0xff);
-		ts2020_writereg(fe, 0x60, 0xf9);
-		ts2020_writereg(fe, 0x08, 0x01);
-		ts2020_writereg(fe, 0x00, 0x41);
+		regmap_write(priv->regmap, 0x42, 0x73);
+		regmap_write(priv->regmap, 0x05, priv->clk_out_div);
+		regmap_write(priv->regmap, 0x20, 0x27);
+		regmap_write(priv->regmap, 0x07, 0x02);
+		regmap_write(priv->regmap, 0x11, 0xff);
+		regmap_write(priv->regmap, 0x60, 0xf9);
+		regmap_write(priv->regmap, 0x08, 0x01);
+		regmap_write(priv->regmap, 0x00, 0x41);
 	} else {
 		static const struct ts2020_reg_val reg_vals[] = {
 			{0x7d, 0x9d},
@@ -163,8 +119,8 @@ static int ts2020_init(struct dvb_frontend *fe)
 			{0x12, 0xa0},
 		};
 
-		ts2020_writereg(fe, 0x00, 0x01);
-		ts2020_writereg(fe, 0x00, 0x03);
+		regmap_write(priv->regmap, 0x00, 0x01);
+		regmap_write(priv->regmap, 0x00, 0x03);
 
 		switch (priv->clk_out) {
 		case TS2020_CLK_OUT_DISABLED:
@@ -172,7 +128,7 @@ static int ts2020_init(struct dvb_frontend *fe)
 			break;
 		case TS2020_CLK_OUT_ENABLED:
 			u8tmp = 0x70;
-			ts2020_writereg(fe, 0x05, priv->clk_out_div);
+			regmap_write(priv->regmap, 0x05, priv->clk_out_div);
 			break;
 		case TS2020_CLK_OUT_ENABLED_XTALOUT:
 			u8tmp = 0x6c;
@@ -182,50 +138,61 @@ static int ts2020_init(struct dvb_frontend *fe)
 			break;
 		}
 
-		ts2020_writereg(fe, 0x42, u8tmp);
+		regmap_write(priv->regmap, 0x42, u8tmp);
 
 		if (priv->loop_through)
 			u8tmp = 0xec;
 		else
 			u8tmp = 0x6c;
 
-		ts2020_writereg(fe, 0x62, u8tmp);
+		regmap_write(priv->regmap, 0x62, u8tmp);
 
 		for (i = 0; i < ARRAY_SIZE(reg_vals); i++)
-			ts2020_writereg(fe, reg_vals[i].reg, reg_vals[i].val);
+			regmap_write(priv->regmap, reg_vals[i].reg,
+				     reg_vals[i].val);
 	}
 
+	/* Initialise v5 stats here */
+	c->strength.len = 1;
+	c->strength.stat[0].scale = FE_SCALE_DECIBEL;
+	c->strength.stat[0].uvalue = 0;
+
+	/* Start statistics polling by invoking the work function */
+	ts2020_stat_work(&priv->stat_work.work);
 	return 0;
 }
 
 static int ts2020_tuner_gate_ctrl(struct dvb_frontend *fe, u8 offset)
 {
+	struct ts2020_priv *priv = fe->tuner_priv;
 	int ret;
-	ret = ts2020_writereg(fe, 0x51, 0x1f - offset);
-	ret |= ts2020_writereg(fe, 0x51, 0x1f);
-	ret |= ts2020_writereg(fe, 0x50, offset);
-	ret |= ts2020_writereg(fe, 0x50, 0x00);
+	ret = regmap_write(priv->regmap, 0x51, 0x1f - offset);
+	ret |= regmap_write(priv->regmap, 0x51, 0x1f);
+	ret |= regmap_write(priv->regmap, 0x50, offset);
+	ret |= regmap_write(priv->regmap, 0x50, 0x00);
 	msleep(20);
 	return ret;
 }
 
 static int ts2020_set_tuner_rf(struct dvb_frontend *fe)
 {
-	int reg;
-
-	reg = ts2020_readreg(fe, 0x3d);
-	reg &= 0x7f;
-	if (reg < 0x16)
-		reg = 0xa1;
-	else if (reg == 0x16)
-		reg = 0x99;
+	struct ts2020_priv *dev = fe->tuner_priv;
+	int ret;
+	unsigned int utmp;
+
+	ret = regmap_read(dev->regmap, 0x3d, &utmp);
+	utmp &= 0x7f;
+	if (utmp < 0x16)
+		utmp = 0xa1;
+	else if (utmp == 0x16)
+		utmp = 0x99;
 	else
-		reg = 0xf9;
+		utmp = 0xf9;
 
-	ts2020_writereg(fe, 0x60, reg);
-	reg = ts2020_tuner_gate_ctrl(fe, 0x08);
+	regmap_write(dev->regmap, 0x60, utmp);
+	ret = ts2020_tuner_gate_ctrl(fe, 0x08);
 
-	return reg;
+	return ret;
 }
 
 static int ts2020_set_params(struct dvb_frontend *fe)
@@ -233,45 +200,62 @@ static int ts2020_set_params(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct ts2020_priv *priv = fe->tuner_priv;
 	int ret;
-	u32 frequency = c->frequency;
-	s32 offset_khz;
-	u32 symbol_rate = (c->symbol_rate / 1000);
+	unsigned int utmp;
 	u32 f3db, gdiv28;
-	u16 value, ndiv, lpf_coeff;
-	u8 lpf_mxdiv, mlpf_max, mlpf_min, nlpf;
-	u8 lo = 0x01, div4 = 0x0;
-
-	/* Calculate frequency divider */
-	if (frequency < priv->frequency_div) {
-		lo |= 0x10;
-		div4 = 0x1;
-		ndiv = (frequency * 14 * 4) / TS2020_XTAL_FREQ;
-	} else
-		ndiv = (frequency * 14 * 2) / TS2020_XTAL_FREQ;
-	ndiv = ndiv + ndiv % 2;
-	ndiv = ndiv - 1024;
+	u16 u16tmp, value, lpf_coeff;
+	u8 buf[3], reg10, lpf_mxdiv, mlpf_max, mlpf_min, nlpf;
+	unsigned int f_ref_khz, f_vco_khz, div_ref, div_out, pll_n;
+	unsigned int frequency_khz = c->frequency;
+
+	/*
+	 * Integer-N PLL synthesizer
+	 * kHz is used for all calculations to keep calculations within 32-bit
+	 */
+	f_ref_khz = TS2020_XTAL_FREQ;
+	div_ref = DIV_ROUND_CLOSEST(f_ref_khz, 2000);
+
+	/* select LO output divider */
+	if (frequency_khz < priv->frequency_div) {
+		div_out = 4;
+		reg10 = 0x10;
+	} else {
+		div_out = 2;
+		reg10 = 0x00;
+	}
+
+	f_vco_khz = frequency_khz * div_out;
+	pll_n = f_vco_khz * div_ref / f_ref_khz;
+	pll_n += pll_n % 2;
+	priv->frequency_khz = pll_n * f_ref_khz / div_ref / div_out;
+
+	pr_debug("frequency=%u offset=%d f_vco_khz=%u pll_n=%u div_ref=%u div_out=%u\n",
+		 priv->frequency_khz, priv->frequency_khz - c->frequency,
+		 f_vco_khz, pll_n, div_ref, div_out);
 
 	if (priv->tuner == TS2020_M88TS2020) {
 		lpf_coeff = 2766;
-		ret = ts2020_writereg(fe, 0x10, 0x80 | lo);
+		reg10 |= 0x01;
+		ret = regmap_write(priv->regmap, 0x10, reg10);
 	} else {
 		lpf_coeff = 3200;
-		ret = ts2020_writereg(fe, 0x10, 0x0b);
-		ret |= ts2020_writereg(fe, 0x11, 0x40);
+		reg10 |= 0x0b;
+		ret = regmap_write(priv->regmap, 0x10, reg10);
+		ret |= regmap_write(priv->regmap, 0x11, 0x40);
 	}
 
-	/* Set frequency divider */
-	ret |= ts2020_writereg(fe, 0x01, (ndiv >> 8) & 0xf);
-	ret |= ts2020_writereg(fe, 0x02, ndiv & 0xff);
+	u16tmp = pll_n - 1024;
+	buf[0] = (u16tmp >> 8) & 0xff;
+	buf[1] = (u16tmp >> 0) & 0xff;
+	buf[2] = div_ref - 8;
+
+	ret |= regmap_write(priv->regmap, 0x01, buf[0]);
+	ret |= regmap_write(priv->regmap, 0x02, buf[1]);
+	ret |= regmap_write(priv->regmap, 0x03, buf[2]);
 
-	ret |= ts2020_writereg(fe, 0x03, 0x06);
 	ret |= ts2020_tuner_gate_ctrl(fe, 0x10);
 	if (ret < 0)
 		return -ENODEV;
 
-	/* Tuner Frequency Range */
-	ret = ts2020_writereg(fe, 0x10, lo);
-
 	ret |= ts2020_tuner_gate_ctrl(fe, 0x08);
 
 	/* Tuner RF */
@@ -279,28 +263,26 @@ static int ts2020_set_params(struct dvb_frontend *fe)
 		ret |= ts2020_set_tuner_rf(fe);
 
 	gdiv28 = (TS2020_XTAL_FREQ / 1000 * 1694 + 500) / 1000;
-	ret |= ts2020_writereg(fe, 0x04, gdiv28 & 0xff);
+	ret |= regmap_write(priv->regmap, 0x04, gdiv28 & 0xff);
 	ret |= ts2020_tuner_gate_ctrl(fe, 0x04);
 	if (ret < 0)
 		return -ENODEV;
 
 	if (priv->tuner == TS2020_M88TS2022) {
-		ret = ts2020_writereg(fe, 0x25, 0x00);
-		ret |= ts2020_writereg(fe, 0x27, 0x70);
-		ret |= ts2020_writereg(fe, 0x41, 0x09);
-		ret |= ts2020_writereg(fe, 0x08, 0x0b);
+		ret = regmap_write(priv->regmap, 0x25, 0x00);
+		ret |= regmap_write(priv->regmap, 0x27, 0x70);
+		ret |= regmap_write(priv->regmap, 0x41, 0x09);
+		ret |= regmap_write(priv->regmap, 0x08, 0x0b);
 		if (ret < 0)
 			return -ENODEV;
 	}
 
-	value = ts2020_readreg(fe, 0x26);
+	regmap_read(priv->regmap, 0x26, &utmp);
+	value = utmp;
 
-	f3db = (symbol_rate * 135) / 200 + 2000;
-	f3db += FREQ_OFFSET_LOW_SYM_RATE;
-	if (f3db < 7000)
-		f3db = 7000;
-	if (f3db > 40000)
-		f3db = 40000;
+	f3db = (c->bandwidth_hz / 1000 / 2) + 2000;
+	f3db += FREQ_OFFSET_LOW_SYM_RATE; /* FIXME: ~always too wide filter */
+	f3db = clamp(f3db, 7000U, 40000U);
 
 	gdiv28 = gdiv28 * 207 / (value * 2 + 151);
 	mlpf_max = gdiv28 * 135 / 100;
@@ -327,19 +309,14 @@ static int ts2020_set_params(struct dvb_frontend *fe)
 	if (lpf_mxdiv > mlpf_max)
 		lpf_mxdiv = mlpf_max;
 
-	ret = ts2020_writereg(fe, 0x04, lpf_mxdiv);
-	ret |= ts2020_writereg(fe, 0x06, nlpf);
+	ret = regmap_write(priv->regmap, 0x04, lpf_mxdiv);
+	ret |= regmap_write(priv->regmap, 0x06, nlpf);
 
 	ret |= ts2020_tuner_gate_ctrl(fe, 0x04);
 
 	ret |= ts2020_tuner_gate_ctrl(fe, 0x01);
 
 	msleep(80);
-	/* calculate offset assuming 96000kHz*/
-	offset_khz = (ndiv - ndiv % 2 + 1024) * TS2020_XTAL_FREQ
-		/ (6 + 8) / (div4 + 1) / 2;
-
-	priv->frequency = offset_khz;
 
 	return (ret < 0) ? -EINVAL : 0;
 }
@@ -347,8 +324,8 @@ static int ts2020_set_params(struct dvb_frontend *fe)
 static int ts2020_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
 	struct ts2020_priv *priv = fe->tuner_priv;
-	*frequency = priv->frequency;
 
+	*frequency = priv->frequency_khz;
 	return 0;
 }
 
@@ -358,28 +335,164 @@ static int ts2020_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
 	return 0;
 }
 
-/* read TS2020 signal strength */
-static int ts2020_read_signal_strength(struct dvb_frontend *fe,
-						u16 *signal_strength)
+/*
+ * Get the tuner gain.
+ * @fe: The front end for which we're determining the gain
+ * @v_agc: The voltage of the AGC from the demodulator (0-2600mV)
+ * @_gain: Where to store the gain (in 0.001dB units)
+ *
+ * Returns 0 or a negative error code.
+ */
+static int ts2020_read_tuner_gain(struct dvb_frontend *fe, unsigned v_agc,
+				  __s64 *_gain)
+{
+	struct ts2020_priv *priv = fe->tuner_priv;
+	unsigned long gain1, gain2, gain3;
+	unsigned utmp;
+	int ret;
+
+	/* Read the RF gain */
+	ret = regmap_read(priv->regmap, 0x3d, &utmp);
+	if (ret < 0)
+		return ret;
+	gain1 = utmp & 0x1f;
+
+	/* Read the baseband gain */
+	ret = regmap_read(priv->regmap, 0x21, &utmp);
+	if (ret < 0)
+		return ret;
+	gain2 = utmp & 0x1f;
+
+	switch (priv->tuner) {
+	case TS2020_M88TS2020:
+		gain1 = clamp_t(long, gain1, 0, 15);
+		gain2 = clamp_t(long, gain2, 0, 13);
+		v_agc = clamp_t(long, v_agc, 400, 1100);
+
+		*_gain = -(gain1 * 2330 +
+			   gain2 * 3500 +
+			   v_agc * 24 / 10 * 10 +
+			   10000);
+		/* gain in range -19600 to -116850 in units of 0.001dB */
+		break;
+
+	case TS2020_M88TS2022:
+		ret = regmap_read(priv->regmap, 0x66, &utmp);
+		if (ret < 0)
+			return ret;
+		gain3 = (utmp >> 3) & 0x07;
+
+		gain1 = clamp_t(long, gain1, 0, 15);
+		gain2 = clamp_t(long, gain2, 2, 16);
+		gain3 = clamp_t(long, gain3, 0, 6);
+		v_agc = clamp_t(long, v_agc, 600, 1600);
+
+		*_gain = -(gain1 * 2650 +
+			   gain2 * 3380 +
+			   gain3 * 2850 +
+			   v_agc * 176 / 100 * 10 -
+			   30000);
+		/* gain in range -47320 to -158950 in units of 0.001dB */
+		break;
+	}
+
+	return 0;
+}
+
+/*
+ * Get the AGC information from the demodulator and use that to calculate the
+ * tuner gain.
+ */
+static int ts2020_get_tuner_gain(struct dvb_frontend *fe, __s64 *_gain)
 {
-	u16 sig_reading, sig_strength;
-	u8 rfgain, bbgain;
+	struct ts2020_priv *priv = fe->tuner_priv;
+	int v_agc = 0, ret;
+	u8 agc_pwm;
 
-	rfgain = ts2020_readreg(fe, 0x3d) & 0x1f;
-	bbgain = ts2020_readreg(fe, 0x21) & 0x1f;
+	/* Read the AGC PWM rate from the demodulator */
+	if (priv->get_agc_pwm) {
+		ret = priv->get_agc_pwm(fe, &agc_pwm);
+		if (ret < 0)
+			return ret;
 
-	if (rfgain > 15)
-		rfgain = 15;
-	if (bbgain > 13)
-		bbgain = 13;
+		switch (priv->tuner) {
+		case TS2020_M88TS2020:
+			v_agc = (int)agc_pwm * 20 - 1166;
+			break;
+		case TS2020_M88TS2022:
+			v_agc = (int)agc_pwm * 16 - 670;
+			break;
+		}
 
-	sig_reading = rfgain * 2 + bbgain * 3;
+		if (v_agc < 0)
+			v_agc = 0;
+	}
 
-	sig_strength = 40 + (64 - sig_reading) * 50 / 64 ;
+	return ts2020_read_tuner_gain(fe, v_agc, _gain);
+}
 
-	/* cook the value to be suitable for szap-s2 human readable output */
-	*signal_strength = sig_strength * 1000;
+/*
+ * Gather statistics on a regular basis
+ */
+static void ts2020_stat_work(struct work_struct *work)
+{
+	struct ts2020_priv *priv = container_of(work, struct ts2020_priv,
+					       stat_work.work);
+	struct i2c_client *client = priv->client;
+	struct dtv_frontend_properties *c = &priv->fe->dtv_property_cache;
+	int ret;
+
+	dev_dbg(&client->dev, "\n");
+
+	ret = ts2020_get_tuner_gain(priv->fe, &c->strength.stat[0].svalue);
+	if (ret < 0)
+		goto err;
 
+	c->strength.stat[0].scale = FE_SCALE_DECIBEL;
+
+	if (!priv->dont_poll)
+		schedule_delayed_work(&priv->stat_work, msecs_to_jiffies(2000));
+	return;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+}
+
+/*
+ * Read TS2020 signal strength in v3 format.
+ */
+static int ts2020_read_signal_strength(struct dvb_frontend *fe,
+				       u16 *_signal_strength)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct ts2020_priv *priv = fe->tuner_priv;
+	unsigned strength;
+	__s64 gain;
+
+	if (priv->dont_poll)
+		ts2020_stat_work(&priv->stat_work.work);
+
+	if (c->strength.stat[0].scale == FE_SCALE_NOT_AVAILABLE) {
+		*_signal_strength = 0;
+		return 0;
+	}
+
+	gain = c->strength.stat[0].svalue;
+
+	/* Calculate the signal strength based on the total gain of the tuner */
+	if (gain < -85000)
+		/* 0%: no signal or weak signal */
+		strength = 0;
+	else if (gain < -65000)
+		/* 0% - 60%: weak signal */
+		strength = 0 + div64_s64((85000 + gain) * 3, 1000);
+	else if (gain < -45000)
+		/* 60% - 90%: normal signal */
+		strength = 60 + div64_s64((65000 + gain) * 3, 2000);
+	else
+		/* 90% - 99%: strong signal */
+		strength = 90 + div64_s64((45000 + gain), 5000);
+
+	*_signal_strength = strength * 65535 / 100;
 	return 0;
 }
 
@@ -402,53 +515,50 @@ struct dvb_frontend *ts2020_attach(struct dvb_frontend *fe,
 					const struct ts2020_config *config,
 					struct i2c_adapter *i2c)
 {
-	struct ts2020_priv *priv = NULL;
-	u8 buf;
-
-	priv = kzalloc(sizeof(struct ts2020_priv), GFP_KERNEL);
-	if (priv == NULL)
+	struct i2c_client *client;
+	struct i2c_board_info board_info;
+
+	/* This is only used by ts2020_probe() so can be on the stack */
+	struct ts2020_config pdata;
+
+	memcpy(&pdata, config, sizeof(pdata));
+	pdata.fe = fe;
+	pdata.attach_in_use = true;
+
+	memset(&board_info, 0, sizeof(board_info));
+	strlcpy(board_info.type, "ts2020", I2C_NAME_SIZE);
+	board_info.addr = config->tuner_address;
+	board_info.platform_data = &pdata;
+	client = i2c_new_device(i2c, &board_info);
+	if (!client || !client->dev.driver)
 		return NULL;
 
-	priv->i2c_address = config->tuner_address;
-	priv->i2c = i2c;
-	priv->clk_out = config->clk_out;
-	priv->clk_out_div = config->clk_out_div;
-	priv->frequency_div = config->frequency_div;
-	priv->fe = fe;
-	fe->tuner_priv = priv;
-
-	if (!priv->frequency_div)
-		priv->frequency_div = 1060000;
-
-	/* Wake Up the tuner */
-	if ((0x03 & ts2020_readreg(fe, 0x00)) == 0x00) {
-		ts2020_writereg(fe, 0x00, 0x01);
-		msleep(2);
-	}
+	return fe;
+}
+EXPORT_SYMBOL(ts2020_attach);
 
-	ts2020_writereg(fe, 0x00, 0x03);
-	msleep(2);
-
-	/* Check the tuner version */
-	buf = ts2020_readreg(fe, 0x00);
-	if ((buf == 0x01) || (buf == 0x41) || (buf == 0x81)) {
-		printk(KERN_INFO "%s: Find tuner TS2020!\n", __func__);
-		priv->tuner = TS2020_M88TS2020;
-	} else if ((buf == 0x83) || (buf == 0xc3)) {
-		printk(KERN_INFO "%s: Find tuner TS2022!\n", __func__);
-		priv->tuner = TS2020_M88TS2022;
-	} else {
-		printk(KERN_ERR "%s: Read tuner reg[0] = %d\n", __func__, buf);
-		kfree(priv);
-		return NULL;
-	}
+/*
+ * We implement own regmap locking due to legacy DVB attach which uses frontend
+ * gate control callback to control I2C bus access. We can open / close gate and
+ * serialize whole open / I2C-operation / close sequence at the same.
+ */
+static void ts2020_regmap_lock(void *__dev)
+{
+	struct ts2020_priv *dev = __dev;
 
-	memcpy(&fe->ops.tuner_ops, &ts2020_tuner_ops,
-				sizeof(struct dvb_tuner_ops));
+	mutex_lock(&dev->regmap_mutex);
+	if (dev->fe->ops.i2c_gate_ctrl)
+		dev->fe->ops.i2c_gate_ctrl(dev->fe, 1);
+}
 
-	return fe;
+static void ts2020_regmap_unlock(void *__dev)
+{
+	struct ts2020_priv *dev = __dev;
+
+	if (dev->fe->ops.i2c_gate_ctrl)
+		dev->fe->ops.i2c_gate_ctrl(dev->fe, 0);
+	mutex_unlock(&dev->regmap_mutex);
 }
-EXPORT_SYMBOL(ts2020_attach);
 
 static int ts2020_probe(struct i2c_client *client,
 		const struct i2c_device_id *id)
@@ -467,38 +577,54 @@ static int ts2020_probe(struct i2c_client *client,
 		goto err;
 	}
 
+	/* create regmap */
+	mutex_init(&dev->regmap_mutex);
+	dev->regmap_config.reg_bits = 8,
+	dev->regmap_config.val_bits = 8,
+	dev->regmap_config.lock = ts2020_regmap_lock,
+	dev->regmap_config.unlock = ts2020_regmap_unlock,
+	dev->regmap_config.lock_arg = dev,
+	dev->regmap = regmap_init_i2c(client, &dev->regmap_config);
+	if (IS_ERR(dev->regmap)) {
+		ret = PTR_ERR(dev->regmap);
+		goto err_kfree;
+	}
+
 	dev->i2c = client->adapter;
 	dev->i2c_address = client->addr;
+	dev->loop_through = pdata->loop_through;
 	dev->clk_out = pdata->clk_out;
 	dev->clk_out_div = pdata->clk_out_div;
+	dev->dont_poll = pdata->dont_poll;
 	dev->frequency_div = pdata->frequency_div;
 	dev->fe = fe;
+	dev->get_agc_pwm = pdata->get_agc_pwm;
 	fe->tuner_priv = dev;
+	dev->client = client;
+	INIT_DELAYED_WORK(&dev->stat_work, ts2020_stat_work);
 
 	/* check if the tuner is there */
-	ret = ts2020_readreg(fe, 0x00);
-	if (ret < 0)
-		goto err;
-	utmp = ret;
+	ret = regmap_read(dev->regmap, 0x00, &utmp);
+	if (ret)
+		goto err_regmap_exit;
 
 	if ((utmp & 0x03) == 0x00) {
-		ret = ts2020_writereg(fe, 0x00, 0x01);
+		ret = regmap_write(dev->regmap, 0x00, 0x01);
 		if (ret)
-			goto err;
+			goto err_regmap_exit;
 
 		usleep_range(2000, 50000);
 	}
 
-	ret = ts2020_writereg(fe, 0x00, 0x03);
+	ret = regmap_write(dev->regmap, 0x00, 0x03);
 	if (ret)
-		goto err;
+		goto err_regmap_exit;
 
 	usleep_range(2000, 50000);
 
-	ret = ts2020_readreg(fe, 0x00);
-	if (ret < 0)
-		goto err;
-	utmp = ret;
+	ret = regmap_read(dev->regmap, 0x00, &utmp);
+	if (ret)
+		goto err_regmap_exit;
 
 	dev_dbg(&client->dev, "chip_id=%02x\n", utmp);
 
@@ -520,7 +646,7 @@ static int ts2020_probe(struct i2c_client *client,
 		break;
 	default:
 		ret = -ENODEV;
-		goto err;
+		goto err_regmap_exit;
 	}
 
 	if (dev->tuner == TS2020_M88TS2022) {
@@ -530,63 +656,64 @@ static int ts2020_probe(struct i2c_client *client,
 			break;
 		case TS2020_CLK_OUT_ENABLED:
 			u8tmp = 0x70;
-			ret = ts2020_writereg(fe, 0x05, dev->clk_out_div);
+			ret = regmap_write(dev->regmap, 0x05, dev->clk_out_div);
 			if (ret)
-				goto err;
+				goto err_regmap_exit;
 			break;
 		case TS2020_CLK_OUT_ENABLED_XTALOUT:
 			u8tmp = 0x6c;
 			break;
 		default:
 			ret = -EINVAL;
-			goto err;
+			goto err_regmap_exit;
 		}
 
-		ret = ts2020_writereg(fe, 0x42, u8tmp);
+		ret = regmap_write(dev->regmap, 0x42, u8tmp);
 		if (ret)
-			goto err;
+			goto err_regmap_exit;
 
 		if (dev->loop_through)
 			u8tmp = 0xec;
 		else
 			u8tmp = 0x6c;
 
-		ret = ts2020_writereg(fe, 0x62, u8tmp);
+		ret = regmap_write(dev->regmap, 0x62, u8tmp);
 		if (ret)
-			goto err;
+			goto err_regmap_exit;
 	}
 
 	/* sleep */
-	ret = ts2020_writereg(fe, 0x00, 0x00);
+	ret = regmap_write(dev->regmap, 0x00, 0x00);
 	if (ret)
-		goto err;
+		goto err_regmap_exit;
 
 	dev_info(&client->dev,
 		 "Montage Technology %s successfully identified\n", chip_str);
 
 	memcpy(&fe->ops.tuner_ops, &ts2020_tuner_ops,
 			sizeof(struct dvb_tuner_ops));
-	fe->ops.tuner_ops.release = NULL;
+	if (!pdata->attach_in_use)
+		fe->ops.tuner_ops.release = NULL;
 
 	i2c_set_clientdata(client, dev);
 	return 0;
+err_regmap_exit:
+	regmap_exit(dev->regmap);
+err_kfree:
+	kfree(dev);
 err:
 	dev_dbg(&client->dev, "failed=%d\n", ret);
-	kfree(dev);
 	return ret;
 }
 
 static int ts2020_remove(struct i2c_client *client)
 {
 	struct ts2020_priv *dev = i2c_get_clientdata(client);
-	struct dvb_frontend *fe = dev->fe;
 
 	dev_dbg(&client->dev, "\n");
 
-	memset(&fe->ops.tuner_ops, 0, sizeof(struct dvb_tuner_ops));
-	fe->tuner_priv = NULL;
+	regmap_exit(dev->regmap);
 	kfree(dev);
-
 	return 0;
 }
 
diff --git a/drivers/media/dvb-frontends/ts2020.h b/drivers/media/dvb-frontends/ts2020.h
index 1714af94eca2..9220e5cf0d21 100644
--- a/drivers/media/dvb-frontends/ts2020.h
+++ b/drivers/media/dvb-frontends/ts2020.h
@@ -32,7 +32,7 @@ struct ts2020_config {
 	/*
 	 * RF loop-through
 	 */
-	u8 loop_through:1;
+	bool loop_through:1;
 
 	/*
 	 * clock output
@@ -48,14 +48,27 @@ struct ts2020_config {
 	 */
 	u8 clk_out_div:5;
 
+	/* Set to true to suppress stat polling */
+	bool dont_poll:1;
+
 	/*
 	 * pointer to DVB frontend
 	 */
 	struct dvb_frontend *fe;
+
+	/*
+	 * driver private, do not set value
+	 */
+	u8 attach_in_use:1;
+
+	/* Operation to be called by the ts2020 driver to get the value of the
+	 * AGC PWM tuner input as theoretically output by the demodulator.
+	 */
+	int (*get_agc_pwm)(struct dvb_frontend *fe, u8 *_agc_pwm);
 };
 
+/* Do not add new ts2020_attach() users! Use I2C bindings instead. */
 #if IS_REACHABLE(CONFIG_DVB_TS2020)
-
 extern struct dvb_frontend *ts2020_attach(
 	struct dvb_frontend *fe,
 	const struct ts2020_config *config,
diff --git a/drivers/media/dvb-frontends/ves1820.c b/drivers/media/dvb-frontends/ves1820.c
index bb42b563c42d..aacfdda3e005 100644
--- a/drivers/media/dvb-frontends/ves1820.c
+++ b/drivers/media/dvb-frontends/ves1820.c
@@ -90,7 +90,8 @@ static u8 ves1820_readreg(struct ves1820_state *state, u8 reg)
 	return b1[0];
 }
 
-static int ves1820_setup_reg0(struct ves1820_state *state, u8 reg0, fe_spectral_inversion_t inversion)
+static int ves1820_setup_reg0(struct ves1820_state *state,
+			      u8 reg0, enum fe_spectral_inversion inversion)
 {
 	reg0 |= state->reg0 & 0x62;
 
@@ -237,7 +238,8 @@ static int ves1820_set_parameters(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int ves1820_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int ves1820_read_status(struct dvb_frontend *fe,
+			       enum fe_status *status)
 {
 	struct ves1820_state* state = fe->demodulator_priv;
 	int sync;
diff --git a/drivers/media/dvb-frontends/ves1x93.c b/drivers/media/dvb-frontends/ves1x93.c
index 9c17eacaec24..526952396422 100644
--- a/drivers/media/dvb-frontends/ves1x93.c
+++ b/drivers/media/dvb-frontends/ves1x93.c
@@ -41,7 +41,7 @@ struct ves1x93_state {
 	struct dvb_frontend frontend;
 
 	/* previous uncorrected block counter */
-	fe_spectral_inversion_t inversion;
+	enum fe_spectral_inversion inversion;
 	u8 *init_1x93_tab;
 	u8 *init_1x93_wtab;
 	u8 tab_size;
@@ -130,7 +130,8 @@ static int ves1x93_clr_bit (struct ves1x93_state* state)
 	return 0;
 }
 
-static int ves1x93_set_inversion (struct ves1x93_state* state, fe_spectral_inversion_t inversion)
+static int ves1x93_set_inversion(struct ves1x93_state *state,
+				 enum fe_spectral_inversion inversion)
 {
 	u8 val;
 
@@ -156,7 +157,7 @@ static int ves1x93_set_inversion (struct ves1x93_state* state, fe_spectral_inver
 	return ves1x93_writereg (state, 0x0c, (state->init_1x93_tab[0x0c] & 0x3f) | val);
 }
 
-static int ves1x93_set_fec (struct ves1x93_state* state, fe_code_rate_t fec)
+static int ves1x93_set_fec(struct ves1x93_state *state, enum fe_code_rate fec)
 {
 	if (fec == FEC_AUTO)
 		return ves1x93_writereg (state, 0x0d, 0x08);
@@ -166,7 +167,7 @@ static int ves1x93_set_fec (struct ves1x93_state* state, fe_code_rate_t fec)
 		return ves1x93_writereg (state, 0x0d, fec - FEC_1_2);
 }
 
-static fe_code_rate_t ves1x93_get_fec (struct ves1x93_state* state)
+static enum fe_code_rate ves1x93_get_fec(struct ves1x93_state *state)
 {
 	return FEC_1_2 + ((ves1x93_readreg (state, 0x0d) >> 4) & 0x7);
 }
@@ -281,7 +282,8 @@ static int ves1x93_init (struct dvb_frontend* fe)
 	return 0;
 }
 
-static int ves1x93_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int ves1x93_set_voltage(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct ves1x93_state* state = fe->demodulator_priv;
 
@@ -297,7 +299,8 @@ static int ves1x93_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltag
 	}
 }
 
-static int ves1x93_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int ves1x93_read_status(struct dvb_frontend *fe,
+			       enum fe_status *status)
 {
 	struct ves1x93_state* state = fe->demodulator_priv;
 
diff --git a/drivers/media/dvb-frontends/zl10353.c b/drivers/media/dvb-frontends/zl10353.c
index 82946cd517f5..ef9764a02d4c 100644
--- a/drivers/media/dvb-frontends/zl10353.c
+++ b/drivers/media/dvb-frontends/zl10353.c
@@ -462,7 +462,7 @@ static int zl10353_get_parameters(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int zl10353_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int zl10353_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct zl10353_state *state = fe->demodulator_priv;
 	int s6, s7, s8;
@@ -533,13 +533,13 @@ static int zl10353_read_snr(struct dvb_frontend *fe, u16 *snr)
 static int zl10353_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 {
 	struct zl10353_state *state = fe->demodulator_priv;
-       u32 ubl = 0;
+	u32 ubl = 0;
 
-       ubl = zl10353_read_register(state, RS_UBC_1) << 8 |
-	     zl10353_read_register(state, RS_UBC_0);
+	ubl = zl10353_read_register(state, RS_UBC_1) << 8 |
+	      zl10353_read_register(state, RS_UBC_0);
 
-       state->ucblocks += ubl;
-       *ucblocks = state->ucblocks;
+	state->ucblocks += ubl;
+	*ucblocks = state->ucblocks;
 
 	return 0;
 }
diff --git a/drivers/media/firewire/firedtv-fe.c b/drivers/media/firewire/firedtv-fe.c
index 6fe9793b98b3..17acda6bcb6e 100644
--- a/drivers/media/firewire/firedtv-fe.c
+++ b/drivers/media/firewire/firedtv-fe.c
@@ -61,12 +61,12 @@ static int fdtv_diseqc_send_master_cmd(struct dvb_frontend *fe,
 }
 
 static int fdtv_diseqc_send_burst(struct dvb_frontend *fe,
-				  fe_sec_mini_cmd_t minicmd)
+				  enum fe_sec_mini_cmd minicmd)
 {
 	return 0;
 }
 
-static int fdtv_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int fdtv_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct firedtv *fdtv = fe->sec_priv;
 
@@ -75,7 +75,7 @@ static int fdtv_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
 }
 
 static int fdtv_set_voltage(struct dvb_frontend *fe,
-			    fe_sec_voltage_t voltage)
+			    enum fe_sec_voltage voltage)
 {
 	struct firedtv *fdtv = fe->sec_priv;
 
@@ -83,7 +83,7 @@ static int fdtv_set_voltage(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int fdtv_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int fdtv_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct firedtv *fdtv = fe->sec_priv;
 	struct firedtv_tuner_status stat;
diff --git a/drivers/media/firewire/firedtv.h b/drivers/media/firewire/firedtv.h
index 346a85be6de2..345d1eda8c05 100644
--- a/drivers/media/firewire/firedtv.h
+++ b/drivers/media/firewire/firedtv.h
@@ -99,8 +99,8 @@ struct firedtv {
 	s8			isochannel;
 	struct fdtv_ir_context	*ir_context;
 
-	fe_sec_voltage_t	voltage;
-	fe_sec_tone_mode_t	tone;
+	enum fe_sec_voltage	voltage;
+	enum fe_sec_tone_mode	tone;
 
 	struct mutex		demux_mutex;
 	unsigned long		channel_active;
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index 6f30ea76151a..71ee8f586430 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -196,7 +196,8 @@ config VIDEO_ADV7183
 
 config VIDEO_ADV7604
 	tristate "Analog Devices ADV7604 decoder"
-	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
+	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && GPIOLIB
+	select HDMI
 	---help---
 	  Support for the Analog Devices ADV7604 video decoder.
 
@@ -424,6 +425,7 @@ config VIDEO_ADV7393
 config VIDEO_ADV7511
 	tristate "Analog Devices ADV7511 encoder"
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
+	select HDMI
 	---help---
 	  Support for the Analog Devices ADV7511 video encoder.
 
diff --git a/drivers/media/i2c/adp1653.c b/drivers/media/i2c/adp1653.c
index 873fe1949e98..c70ababce954 100644
--- a/drivers/media/i2c/adp1653.c
+++ b/drivers/media/i2c/adp1653.c
@@ -8,6 +8,7 @@
  * Contributors:
  *	Sakari Ailus <sakari.ailus@iki.fi>
  *	Tuukka Toivonen <tuukkat76@gmail.com>
+ *	Pavel Machek <pavel@ucw.cz>
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -34,6 +35,8 @@
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/gpio/consumer.h>
 #include <media/adp1653.h>
 #include <media/v4l2-device.h>
 
@@ -308,16 +311,28 @@ __adp1653_set_power(struct adp1653_flash *flash, int on)
 {
 	int ret;
 
-	ret = flash->platform_data->power(&flash->subdev, on);
-	if (ret < 0)
-		return ret;
+	if (flash->platform_data->power) {
+		ret = flash->platform_data->power(&flash->subdev, on);
+		if (ret < 0)
+			return ret;
+	} else {
+		gpiod_set_value(flash->platform_data->enable_gpio, on);
+		if (on)
+			/* Some delay is apparently required. */
+			udelay(20);
+	}
 
 	if (!on)
 		return 0;
 
 	ret = adp1653_init_device(flash);
-	if (ret < 0)
+	if (ret >= 0)
+		return ret;
+
+	if (flash->platform_data->power)
 		flash->platform_data->power(&flash->subdev, 0);
+	else
+		gpiod_set_value(flash->platform_data->enable_gpio, 0);
 
 	return ret;
 }
@@ -407,21 +422,85 @@ static int adp1653_resume(struct device *dev)
 
 #endif /* CONFIG_PM */
 
+static int adp1653_of_init(struct i2c_client *client,
+			   struct adp1653_flash *flash,
+			   struct device_node *node)
+{
+	struct adp1653_platform_data *pd;
+	struct device_node *child;
+
+	pd = devm_kzalloc(&client->dev, sizeof(*pd), GFP_KERNEL);
+	if (!pd)
+		return -ENOMEM;
+	flash->platform_data = pd;
+
+	child = of_get_child_by_name(node, "flash");
+	if (!child)
+		return -EINVAL;
+
+	if (of_property_read_u32(child, "flash-timeout-us",
+				 &pd->max_flash_timeout))
+		goto err;
+
+	if (of_property_read_u32(child, "flash-max-microamp",
+				 &pd->max_flash_intensity))
+		goto err;
+
+	pd->max_flash_intensity /= 1000;
+
+	if (of_property_read_u32(child, "led-max-microamp",
+				 &pd->max_torch_intensity))
+		goto err;
+
+	pd->max_torch_intensity /= 1000;
+	of_node_put(child);
+
+	child = of_get_child_by_name(node, "indicator");
+	if (!child)
+		return -EINVAL;
+
+	if (of_property_read_u32(child, "led-max-microamp",
+				 &pd->max_indicator_intensity))
+		goto err;
+
+	of_node_put(child);
+
+	pd->enable_gpio = devm_gpiod_get(&client->dev, "enable");
+	if (!pd->enable_gpio) {
+		dev_err(&client->dev, "Error getting GPIO\n");
+		return -EINVAL;
+	}
+
+	return 0;
+err:
+	dev_err(&client->dev, "Required property not found\n");
+	of_node_put(child);
+	return -EINVAL;
+}
+
+
 static int adp1653_probe(struct i2c_client *client,
 			 const struct i2c_device_id *devid)
 {
 	struct adp1653_flash *flash;
 	int ret;
 
-	/* we couldn't work without platform data */
-	if (client->dev.platform_data == NULL)
-		return -ENODEV;
-
 	flash = devm_kzalloc(&client->dev, sizeof(*flash), GFP_KERNEL);
 	if (flash == NULL)
 		return -ENOMEM;
 
-	flash->platform_data = client->dev.platform_data;
+	if (client->dev.of_node) {
+		ret = adp1653_of_init(client, flash, client->dev.of_node);
+		if (ret)
+			return ret;
+	} else {
+		if (!client->dev.platform_data) {
+			dev_err(&client->dev,
+				"Neither DT not platform data provided\n");
+			return EINVAL;
+		}
+		flash->platform_data = client->dev.platform_data;
+	}
 
 	mutex_init(&flash->power_lock);
 
@@ -442,6 +521,7 @@ static int adp1653_probe(struct i2c_client *client,
 	return 0;
 
 free_and_quit:
+	dev_err(&client->dev, "adp1653: failed to register device\n");
 	v4l2_ctrl_handler_free(&flash->ctrls);
 	return ret;
 }
@@ -464,7 +544,7 @@ static const struct i2c_device_id adp1653_id_table[] = {
 };
 MODULE_DEVICE_TABLE(i2c, adp1653_id_table);
 
-static struct dev_pm_ops adp1653_pm_ops = {
+static const struct dev_pm_ops adp1653_pm_ops = {
 	.suspend	= adp1653_suspend,
 	.resume		= adp1653_resume,
 };
diff --git a/drivers/media/i2c/adv7170.c b/drivers/media/i2c/adv7170.c
index 40a1a95c7ce9..f0d3f5a2da46 100644
--- a/drivers/media/i2c/adv7170.c
+++ b/drivers/media/i2c/adv7170.c
@@ -262,21 +262,27 @@ static int adv7170_s_routing(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv7170_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-				u32 *code)
+static int adv7170_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(adv7170_codes))
+	if (code->pad || code->index >= ARRAY_SIZE(adv7170_codes))
 		return -EINVAL;
 
-	*code = adv7170_codes[index];
+	code->code = adv7170_codes[code->index];
 	return 0;
 }
 
-static int adv7170_g_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *mf)
+static int adv7170_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	u8 val = adv7170_read(sd, 0x7);
 
+	if (format->pad)
+		return -EINVAL;
+
 	if ((val & 0x40) == (1 << 6))
 		mf->code = MEDIA_BUS_FMT_UYVY8_1X16;
 	else
@@ -290,11 +296,16 @@ static int adv7170_g_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv7170_s_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *mf)
+static int adv7170_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	u8 val = adv7170_read(sd, 0x7);
-	int ret;
+	int ret = 0;
+
+	if (format->pad)
+		return -EINVAL;
 
 	switch (mf->code) {
 	case MEDIA_BUS_FMT_UYVY8_2X8:
@@ -311,7 +322,8 @@ static int adv7170_s_fmt(struct v4l2_subdev *sd,
 		return -EINVAL;
 	}
 
-	ret = adv7170_write(sd, 0x7, val);
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		ret = adv7170_write(sd, 0x7, val);
 
 	return ret;
 }
@@ -321,13 +333,17 @@ static int adv7170_s_fmt(struct v4l2_subdev *sd,
 static const struct v4l2_subdev_video_ops adv7170_video_ops = {
 	.s_std_output = adv7170_s_std_output,
 	.s_routing = adv7170_s_routing,
-	.s_mbus_fmt = adv7170_s_fmt,
-	.g_mbus_fmt = adv7170_g_fmt,
-	.enum_mbus_fmt  = adv7170_enum_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops adv7170_pad_ops = {
+	.enum_mbus_code = adv7170_enum_mbus_code,
+	.get_fmt = adv7170_get_fmt,
+	.set_fmt = adv7170_set_fmt,
 };
 
 static const struct v4l2_subdev_ops adv7170_ops = {
 	.video = &adv7170_video_ops,
+	.pad = &adv7170_pad_ops,
 };
 
 /* ----------------------------------------------------------------------- */
diff --git a/drivers/media/i2c/adv7175.c b/drivers/media/i2c/adv7175.c
index d220af579a64..321834ba8f57 100644
--- a/drivers/media/i2c/adv7175.c
+++ b/drivers/media/i2c/adv7175.c
@@ -300,21 +300,27 @@ static int adv7175_s_routing(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv7175_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-				u32 *code)
+static int adv7175_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(adv7175_codes))
+	if (code->pad || code->index >= ARRAY_SIZE(adv7175_codes))
 		return -EINVAL;
 
-	*code = adv7175_codes[index];
+	code->code = adv7175_codes[code->index];
 	return 0;
 }
 
-static int adv7175_g_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *mf)
+static int adv7175_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	u8 val = adv7175_read(sd, 0x7);
 
+	if (format->pad)
+		return -EINVAL;
+
 	if ((val & 0x40) == (1 << 6))
 		mf->code = MEDIA_BUS_FMT_UYVY8_1X16;
 	else
@@ -328,11 +334,16 @@ static int adv7175_g_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv7175_s_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *mf)
+static int adv7175_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	u8 val = adv7175_read(sd, 0x7);
-	int ret;
+	int ret = 0;
+
+	if (format->pad)
+		return -EINVAL;
 
 	switch (mf->code) {
 	case MEDIA_BUS_FMT_UYVY8_2X8:
@@ -349,7 +360,8 @@ static int adv7175_s_fmt(struct v4l2_subdev *sd,
 		return -EINVAL;
 	}
 
-	ret = adv7175_write(sd, 0x7, val);
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		ret = adv7175_write(sd, 0x7, val);
 
 	return ret;
 }
@@ -374,14 +386,18 @@ static const struct v4l2_subdev_core_ops adv7175_core_ops = {
 static const struct v4l2_subdev_video_ops adv7175_video_ops = {
 	.s_std_output = adv7175_s_std_output,
 	.s_routing = adv7175_s_routing,
-	.s_mbus_fmt = adv7175_s_fmt,
-	.g_mbus_fmt = adv7175_g_fmt,
-	.enum_mbus_fmt  = adv7175_enum_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops adv7175_pad_ops = {
+	.enum_mbus_code = adv7175_enum_mbus_code,
+	.get_fmt = adv7175_get_fmt,
+	.set_fmt = adv7175_set_fmt,
 };
 
 static const struct v4l2_subdev_ops adv7175_ops = {
 	.core = &adv7175_core_ops,
 	.video = &adv7175_video_ops,
+	.pad = &adv7175_pad_ops,
 };
 
 /* ----------------------------------------------------------------------- */
diff --git a/drivers/media/i2c/adv7183.c b/drivers/media/i2c/adv7183.c
index 28940cc3a766..e2dd1617662f 100644
--- a/drivers/media/i2c/adv7183.c
+++ b/drivers/media/i2c/adv7183.c
@@ -420,20 +420,26 @@ static int adv7183_g_input_status(struct v4l2_subdev *sd, u32 *status)
 	return 0;
 }
 
-static int adv7183_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
-				u32 *code)
+static int adv7183_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index > 0)
+	if (code->pad || code->index > 0)
 		return -EINVAL;
 
-	*code = MEDIA_BUS_FMT_UYVY8_2X8;
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
 	return 0;
 }
 
-static int adv7183_try_mbus_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *fmt)
+static int adv7183_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
 	struct adv7183 *decoder = to_adv7183(sd);
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
 
 	fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
@@ -446,25 +452,23 @@ static int adv7183_try_mbus_fmt(struct v4l2_subdev *sd,
 		fmt->width = 720;
 		fmt->height = 576;
 	}
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		decoder->fmt = *fmt;
+	else
+		cfg->try_fmt = *fmt;
 	return 0;
 }
 
-static int adv7183_s_mbus_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *fmt)
+static int adv7183_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
 	struct adv7183 *decoder = to_adv7183(sd);
 
-	adv7183_try_mbus_fmt(sd, fmt);
-	decoder->fmt = *fmt;
-	return 0;
-}
-
-static int adv7183_g_mbus_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *fmt)
-{
-	struct adv7183 *decoder = to_adv7183(sd);
+	if (format->pad)
+		return -EINVAL;
 
-	*fmt = decoder->fmt;
+	format->format = decoder->fmt;
 	return 0;
 }
 
@@ -514,16 +518,19 @@ static const struct v4l2_subdev_video_ops adv7183_video_ops = {
 	.s_routing = adv7183_s_routing,
 	.querystd = adv7183_querystd,
 	.g_input_status = adv7183_g_input_status,
-	.enum_mbus_fmt = adv7183_enum_mbus_fmt,
-	.try_mbus_fmt = adv7183_try_mbus_fmt,
-	.s_mbus_fmt = adv7183_s_mbus_fmt,
-	.g_mbus_fmt = adv7183_g_mbus_fmt,
 	.s_stream = adv7183_s_stream,
 };
 
+static const struct v4l2_subdev_pad_ops adv7183_pad_ops = {
+	.enum_mbus_code = adv7183_enum_mbus_code,
+	.get_fmt = adv7183_get_fmt,
+	.set_fmt = adv7183_set_fmt,
+};
+
 static const struct v4l2_subdev_ops adv7183_ops = {
 	.core = &adv7183_core_ops,
 	.video = &adv7183_video_ops,
+	.pad = &adv7183_pad_ops,
 };
 
 static int adv7183_probe(struct i2c_client *client,
@@ -533,7 +540,9 @@ static int adv7183_probe(struct i2c_client *client,
 	struct v4l2_subdev *sd;
 	struct v4l2_ctrl_handler *hdl;
 	int ret;
-	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	const unsigned *pin_array;
 
 	/* Check if the adapter supports the needed features */
@@ -603,9 +612,9 @@ static int adv7183_probe(struct i2c_client *client,
 
 	adv7183_writeregs(sd, adv7183_init_regs, ARRAY_SIZE(adv7183_init_regs));
 	adv7183_s_std(sd, decoder->std);
-	fmt.width = 720;
-	fmt.height = 576;
-	adv7183_s_mbus_fmt(sd, &fmt);
+	fmt.format.width = 720;
+	fmt.format.height = 576;
+	adv7183_set_fmt(sd, NULL, &fmt);
 
 	/* initialize the hardware to the default control values */
 	ret = v4l2_ctrl_handler_setup(hdl);
diff --git a/drivers/media/i2c/adv7511.c b/drivers/media/i2c/adv7511.c
index 12d93203d405..95bcd4026451 100644
--- a/drivers/media/i2c/adv7511.c
+++ b/drivers/media/i2c/adv7511.c
@@ -77,7 +77,7 @@ struct adv7511_state_edid {
 	u32 blocks;
 	/* Number of segments read */
 	u32 segments;
-	uint8_t data[EDID_MAX_SEGM * 256];
+	u8 data[EDID_MAX_SEGM * 256];
 	/* Number of EDID read retries left */
 	unsigned read_retries;
 	bool complete;
@@ -89,8 +89,9 @@ struct adv7511_state {
 	struct media_pad pad;
 	struct v4l2_ctrl_handler hdl;
 	int chip_revision;
-	uint8_t i2c_edid_addr;
-	uint8_t i2c_cec_addr;
+	u8 i2c_edid_addr;
+	u8 i2c_cec_addr;
+	u8 i2c_pktmem_addr;
 	/* Is the adv7511 powered on? */
 	bool power_on;
 	/* Did we receive hotplug and rx-sense signals? */
@@ -101,6 +102,7 @@ struct adv7511_state {
 	u32 colorspace;
 	u32 ycbcr_enc;
 	u32 quantization;
+	u32 xfer_func;
 	/* controls */
 	struct v4l2_ctrl *hdmi_mode_ctrl;
 	struct v4l2_ctrl *hotplug_ctrl;
@@ -108,6 +110,7 @@ struct adv7511_state {
 	struct v4l2_ctrl *have_edid0_ctrl;
 	struct v4l2_ctrl *rgb_quantization_range_ctrl;
 	struct i2c_client *i2c_edid;
+	struct i2c_client *i2c_pktmem;
 	struct adv7511_state_edid edid;
 	/* Running counter of the number of detected EDIDs (for debugging) */
 	unsigned edid_detect_counter;
@@ -200,7 +203,7 @@ static int adv7511_wr(struct v4l2_subdev *sd, u8 reg, u8 val)
 
 /* To set specific bits in the register, a clear-mask is given (to be AND-ed),
    and then the value-mask (to be OR-ed). */
-static inline void adv7511_wr_and_or(struct v4l2_subdev *sd, u8 reg, uint8_t clr_mask, uint8_t val_mask)
+static inline void adv7511_wr_and_or(struct v4l2_subdev *sd, u8 reg, u8 clr_mask, u8 val_mask)
 {
 	adv7511_wr(sd, reg, (adv7511_rd(sd, reg) & clr_mask) | val_mask);
 }
@@ -222,7 +225,7 @@ static int adv_smbus_read_i2c_block_data(struct i2c_client *client,
 	return ret;
 }
 
-static inline void adv7511_edid_rd(struct v4l2_subdev *sd, uint16_t len, uint8_t *buf)
+static inline void adv7511_edid_rd(struct v4l2_subdev *sd, u16 len, u8 *buf)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
 	int i;
@@ -237,6 +240,35 @@ static inline void adv7511_edid_rd(struct v4l2_subdev *sd, uint16_t len, uint8_t
 		v4l2_err(sd, "%s: i2c read error\n", __func__);
 }
 
+static int adv7511_pktmem_rd(struct v4l2_subdev *sd, u8 reg)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+
+	return adv_smbus_read_byte_data(state->i2c_pktmem, reg);
+}
+
+static int adv7511_pktmem_wr(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	struct adv7511_state *state = get_adv7511_state(sd);
+	int ret;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = i2c_smbus_write_byte_data(state->i2c_pktmem, reg, val);
+		if (ret == 0)
+			return 0;
+	}
+	v4l2_err(sd, "%s: i2c write error\n", __func__);
+	return ret;
+}
+
+/* To set specific bits in the register, a clear-mask is given (to be AND-ed),
+   and then the value-mask (to be OR-ed). */
+static inline void adv7511_pktmem_wr_and_or(struct v4l2_subdev *sd, u8 reg, u8 clr_mask, u8 val_mask)
+{
+	adv7511_pktmem_wr(sd, reg, (adv7511_pktmem_rd(sd, reg) & clr_mask) | val_mask);
+}
+
 static inline bool adv7511_have_hotplug(struct v4l2_subdev *sd)
 {
 	return adv7511_rd(sd, 0x42) & MASK_ADV7511_HPD_DETECT;
@@ -247,7 +279,7 @@ static inline bool adv7511_have_rx_sense(struct v4l2_subdev *sd)
 	return adv7511_rd(sd, 0x42) & MASK_ADV7511_MSEN_DETECT;
 }
 
-static void adv7511_csc_conversion_mode(struct v4l2_subdev *sd, uint8_t mode)
+static void adv7511_csc_conversion_mode(struct v4l2_subdev *sd, u8 mode)
 {
 	adv7511_wr_and_or(sd, 0x18, 0x9f, (mode & 0x3)<<5);
 }
@@ -291,7 +323,7 @@ static void adv7511_csc_coeff(struct v4l2_subdev *sd,
 static void adv7511_csc_rgb_full2limit(struct v4l2_subdev *sd, bool enable)
 {
 	if (enable) {
-		uint8_t csc_mode = 0;
+		u8 csc_mode = 0;
 		adv7511_csc_conversion_mode(sd, csc_mode);
 		adv7511_csc_coeff(sd,
 				  4096-564, 0, 0, 256,
@@ -414,6 +446,80 @@ static int adv7511_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_regi
 }
 #endif
 
+struct adv7511_cfg_read_infoframe {
+	const char *desc;
+	u8 present_reg;
+	u8 present_mask;
+	u8 header[3];
+	u16 payload_addr;
+};
+
+static u8 hdmi_infoframe_checksum(u8 *ptr, size_t size)
+{
+	u8 csum = 0;
+	size_t i;
+
+	/* compute checksum */
+	for (i = 0; i < size; i++)
+		csum += ptr[i];
+
+	return 256 - csum;
+}
+
+static void log_infoframe(struct v4l2_subdev *sd, const struct adv7511_cfg_read_infoframe *cri)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	union hdmi_infoframe frame;
+	u8 buffer[32];
+	u8 len;
+	int i;
+
+	if (!(adv7511_rd(sd, cri->present_reg) & cri->present_mask)) {
+		v4l2_info(sd, "%s infoframe not transmitted\n", cri->desc);
+		return;
+	}
+
+	memcpy(buffer, cri->header, sizeof(cri->header));
+
+	len = buffer[2];
+
+	if (len + 4 > sizeof(buffer)) {
+		v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__, cri->desc, len);
+		return;
+	}
+
+	if (cri->payload_addr >= 0x100) {
+		for (i = 0; i < len; i++)
+			buffer[i + 4] = adv7511_pktmem_rd(sd, cri->payload_addr + i - 0x100);
+	} else {
+		for (i = 0; i < len; i++)
+			buffer[i + 4] = adv7511_rd(sd, cri->payload_addr + i);
+	}
+	buffer[3] = 0;
+	buffer[3] = hdmi_infoframe_checksum(buffer, len + 4);
+
+	if (hdmi_infoframe_unpack(&frame, buffer) < 0) {
+		v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__, cri->desc);
+		return;
+	}
+
+	hdmi_infoframe_log(KERN_INFO, dev, &frame);
+}
+
+static void adv7511_log_infoframes(struct v4l2_subdev *sd)
+{
+	static const struct adv7511_cfg_read_infoframe cri[] = {
+		{ "AVI", 0x44, 0x10, { 0x82, 2, 13 }, 0x55 },
+		{ "Audio", 0x44, 0x08, { 0x84, 1, 10 }, 0x73 },
+		{ "SDP", 0x40, 0x40, { 0x83, 1, 25 }, 0x103 },
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(cri); i++)
+		log_infoframe(sd, &cri[i]);
+}
+
 static int adv7511_log_status(struct v4l2_subdev *sd)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
@@ -479,6 +585,7 @@ static int adv7511_log_status(struct v4l2_subdev *sd)
 			  manual_cts ? "manual" : "automatic", N, CTS);
 		v4l2_info(sd, "VIC: detected %d, sent %d\n",
 			  vic_detect, vic_sent);
+		adv7511_log_infoframes(sd);
 	}
 	if (state->dv_timings.type == V4L2_DV_BT_656_1120)
 		v4l2_print_dv_timings(sd->name, "timings: ",
@@ -487,6 +594,7 @@ static int adv7511_log_status(struct v4l2_subdev *sd)
 		v4l2_info(sd, "no timings set\n");
 	v4l2_info(sd, "i2c edid addr: 0x%x\n", state->i2c_edid_addr);
 	v4l2_info(sd, "i2c cec addr: 0x%x\n", state->i2c_cec_addr);
+	v4l2_info(sd, "i2c pktmem addr: 0x%x\n", state->i2c_pktmem_addr);
 	return 0;
 }
 
@@ -536,6 +644,7 @@ static int adv7511_s_power(struct v4l2_subdev *sd, int on)
 	adv7511_wr(sd, 0xf9, 0x00);
 
 	adv7511_wr(sd, 0x43, state->i2c_edid_addr);
+	adv7511_wr(sd, 0x45, state->i2c_pktmem_addr);
 
 	/* Set number of attempts to read the EDID */
 	adv7511_wr(sd, 0xc9, 0xf);
@@ -545,8 +654,8 @@ static int adv7511_s_power(struct v4l2_subdev *sd, int on)
 /* Enable interrupts */
 static void adv7511_set_isr(struct v4l2_subdev *sd, bool enable)
 {
-	uint8_t irqs = MASK_ADV7511_HPD_INT | MASK_ADV7511_MSEN_INT;
-	uint8_t irqs_rd;
+	u8 irqs = MASK_ADV7511_HPD_INT | MASK_ADV7511_MSEN_INT;
+	u8 irqs_rd;
 	int retries = 100;
 
 	v4l2_dbg(2, debug, sd, "%s: %s\n", __func__, enable ? "enable" : "disable");
@@ -579,7 +688,7 @@ static void adv7511_set_isr(struct v4l2_subdev *sd, bool enable)
 /* Interrupt handler */
 static int adv7511_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 {
-	uint8_t irq_status;
+	u8 irq_status;
 
 	/* disable interrupts to prevent a race condition */
 	adv7511_set_isr(sd, false);
@@ -861,11 +970,13 @@ static int adv7511_get_fmt(struct v4l2_subdev *sd,
 		format->format.colorspace = fmt->colorspace;
 		format->format.ycbcr_enc = fmt->ycbcr_enc;
 		format->format.quantization = fmt->quantization;
+		format->format.xfer_func = fmt->xfer_func;
 	} else {
 		format->format.code = state->fmt_code;
 		format->format.colorspace = state->colorspace;
 		format->format.ycbcr_enc = state->ycbcr_enc;
 		format->format.quantization = state->quantization;
+		format->format.xfer_func = state->xfer_func;
 	}
 
 	return 0;
@@ -912,6 +1023,7 @@ static int adv7511_set_fmt(struct v4l2_subdev *sd,
 		fmt->colorspace = format->format.colorspace;
 		fmt->ycbcr_enc = format->format.ycbcr_enc;
 		fmt->quantization = format->format.quantization;
+		fmt->xfer_func = format->format.xfer_func;
 		return 0;
 	}
 
@@ -936,6 +1048,7 @@ static int adv7511_set_fmt(struct v4l2_subdev *sd,
 	state->colorspace = format->format.colorspace;
 	state->ycbcr_enc = format->format.ycbcr_enc;
 	state->quantization = format->format.quantization;
+	state->xfer_func = format->format.xfer_func;
 
 	switch (format->format.colorspace) {
 	case V4L2_COLORSPACE_ADOBERGB:
@@ -1028,7 +1141,7 @@ static const struct v4l2_subdev_ops adv7511_ops = {
 };
 
 /* ----------------------------------------------------------------------- */
-static void adv7511_dbg_dump_edid(int lvl, int debug, struct v4l2_subdev *sd, int segment, uint8_t *buf)
+static void adv7511_dbg_dump_edid(int lvl, int debug, struct v4l2_subdev *sd, int segment, u8 *buf)
 {
 	if (debug >= lvl) {
 		int i, j;
@@ -1140,7 +1253,7 @@ static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
 	/* read hotplug and rx-sense state */
-	uint8_t status = adv7511_rd(sd, 0x42);
+	u8 status = adv7511_rd(sd, 0x42);
 
 	v4l2_dbg(1, debug, sd, "%s: status: 0x%x%s%s\n",
 			 __func__,
@@ -1184,9 +1297,9 @@ static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd)
 	}
 }
 
-static bool edid_block_verify_crc(uint8_t *edid_block)
+static bool edid_block_verify_crc(u8 *edid_block)
 {
-	uint8_t sum = 0;
+	u8 sum = 0;
 	int i;
 
 	for (i = 0; i < 128; i++)
@@ -1198,7 +1311,7 @@ static bool edid_verify_crc(struct v4l2_subdev *sd, u32 segment)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
 	u32 blocks = state->edid.blocks;
-	uint8_t *data = state->edid.data;
+	u8 *data = state->edid.data;
 
 	if (!edid_block_verify_crc(&data[segment * 256]))
 		return false;
@@ -1223,7 +1336,7 @@ static bool edid_verify_header(struct v4l2_subdev *sd, u32 segment)
 static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
-	uint8_t edidRdy = adv7511_rd(sd, 0xc5);
+	u8 edidRdy = adv7511_rd(sd, 0xc5);
 
 	v4l2_dbg(1, debug, sd, "%s: edid ready (retries: %d)\n",
 			 __func__, EDID_MAX_RETRIES - state->edid.read_retries);
@@ -1376,6 +1489,7 @@ static int adv7511_probe(struct i2c_client *client, const struct i2c_device_id *
 	/* EDID and CEC i2c addr */
 	state->i2c_edid_addr = state->pdata.i2c_edid << 1;
 	state->i2c_cec_addr = state->pdata.i2c_cec << 1;
+	state->i2c_pktmem_addr = state->pdata.i2c_pktmem << 1;
 
 	state->chip_revision = adv7511_rd(sd, 0x0);
 	chip_id[0] = adv7511_rd(sd, 0xf5);
@@ -1393,12 +1507,19 @@ static int adv7511_probe(struct i2c_client *client, const struct i2c_device_id *
 		goto err_entity;
 	}
 
+	state->i2c_pktmem = i2c_new_dummy(client->adapter, state->i2c_pktmem_addr >> 1);
+	if (state->i2c_pktmem == NULL) {
+		v4l2_err(sd, "failed to register pktmem i2c client\n");
+		err = -ENOMEM;
+		goto err_unreg_edid;
+	}
+
 	adv7511_wr(sd, 0xe2, 0x01); /* power down cec section */
 	state->work_queue = create_singlethread_workqueue(sd->name);
 	if (state->work_queue == NULL) {
 		v4l2_err(sd, "could not create workqueue\n");
 		err = -ENOMEM;
-		goto err_unreg_cec;
+		goto err_unreg_pktmem;
 	}
 
 	INIT_DELAYED_WORK(&state->edid_handler, adv7511_edid_handler);
@@ -1411,7 +1532,9 @@ static int adv7511_probe(struct i2c_client *client, const struct i2c_device_id *
 			  client->addr << 1, client->adapter->name);
 	return 0;
 
-err_unreg_cec:
+err_unreg_pktmem:
+	i2c_unregister_device(state->i2c_pktmem);
+err_unreg_edid:
 	i2c_unregister_device(state->i2c_edid);
 err_entity:
 	media_entity_cleanup(&sd->entity);
@@ -1435,6 +1558,7 @@ static int adv7511_remove(struct i2c_client *client)
 	adv7511_init_setup(sd);
 	cancel_delayed_work(&state->edid_handler);
 	i2c_unregister_device(state->i2c_edid);
+	i2c_unregister_device(state->i2c_pktmem);
 	destroy_workqueue(state->work_queue);
 	v4l2_device_unregister_subdev(sd);
 	media_entity_cleanup(&sd->entity);
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index 60ffcf098bef..808360fd6539 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -29,6 +29,7 @@
 
 #include <linux/delay.h>
 #include <linux/gpio/consumer.h>
+#include <linux/hdmi.h>
 #include <linux/i2c.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -95,6 +96,13 @@ struct adv76xx_format_info {
 	u8 op_format_sel;
 };
 
+struct adv76xx_cfg_read_infoframe {
+	const char *desc;
+	u8 present_mask;
+	u8 head_addr;
+	u8 payload_addr;
+};
+
 struct adv76xx_chip_info {
 	enum adv76xx_type type;
 
@@ -124,6 +132,20 @@ struct adv76xx_chip_info {
 	unsigned int num_recommended_settings[2];
 
 	unsigned long page_mask;
+
+	/* Masks for timings */
+	unsigned int linewidth_mask;
+	unsigned int field0_height_mask;
+	unsigned int field1_height_mask;
+	unsigned int hfrontporch_mask;
+	unsigned int hsync_mask;
+	unsigned int hbackporch_mask;
+	unsigned int field0_vfrontporch_mask;
+	unsigned int field1_vfrontporch_mask;
+	unsigned int field0_vsync_mask;
+	unsigned int field1_vsync_mask;
+	unsigned int field0_vbackporch_mask;
+	unsigned int field1_vbackporch_mask;
 };
 
 /*
@@ -327,6 +349,11 @@ static const struct adv76xx_video_standards adv76xx_prim_mode_hdmi_gr[] = {
 	{ },
 };
 
+static const struct v4l2_event adv76xx_ev_fmt = {
+	.type = V4L2_EVENT_SOURCE_CHANGE,
+	.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+};
+
 /* ----------------------------------------------------------------------- */
 
 static inline struct adv76xx_state *to_state(struct v4l2_subdev *sd)
@@ -1304,12 +1331,12 @@ static int stdi2dv_timings(struct v4l2_subdev *sd,
 	if (v4l2_detect_cvt(stdi->lcf + 1, hfreq, stdi->lcvs,
 			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
 			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
-			timings))
+			false, timings))
 		return 0;
 	if (v4l2_detect_gtf(stdi->lcf + 1, hfreq, stdi->lcvs,
 			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
 			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
-			state->aspect_ratio, timings))
+			false, state->aspect_ratio, timings))
 		return 0;
 
 	v4l2_dbg(2, debug, sd,
@@ -1504,23 +1531,28 @@ static int adv76xx_query_dv_timings(struct v4l2_subdev *sd,
 	if (is_digital_input(sd)) {
 		timings->type = V4L2_DV_BT_656_1120;
 
-		/* FIXME: All masks are incorrect for ADV7611 */
-		bt->width = hdmi_read16(sd, 0x07, 0xfff);
-		bt->height = hdmi_read16(sd, 0x09, 0xfff);
+		bt->width = hdmi_read16(sd, 0x07, info->linewidth_mask);
+		bt->height = hdmi_read16(sd, 0x09, info->field0_height_mask);
 		bt->pixelclock = info->read_hdmi_pixelclock(sd);
-		bt->hfrontporch = hdmi_read16(sd, 0x20, 0x3ff);
-		bt->hsync = hdmi_read16(sd, 0x22, 0x3ff);
-		bt->hbackporch = hdmi_read16(sd, 0x24, 0x3ff);
-		bt->vfrontporch = hdmi_read16(sd, 0x2a, 0x1fff) / 2;
-		bt->vsync = hdmi_read16(sd, 0x2e, 0x1fff) / 2;
-		bt->vbackporch = hdmi_read16(sd, 0x32, 0x1fff) / 2;
+		bt->hfrontporch = hdmi_read16(sd, 0x20, info->hfrontporch_mask);
+		bt->hsync = hdmi_read16(sd, 0x22, info->hsync_mask);
+		bt->hbackporch = hdmi_read16(sd, 0x24, info->hbackporch_mask);
+		bt->vfrontporch = hdmi_read16(sd, 0x2a,
+			info->field0_vfrontporch_mask) / 2;
+		bt->vsync = hdmi_read16(sd, 0x2e, info->field0_vsync_mask) / 2;
+		bt->vbackporch = hdmi_read16(sd, 0x32,
+			info->field0_vbackporch_mask) / 2;
 		bt->polarities = ((hdmi_read(sd, 0x05) & 0x10) ? V4L2_DV_VSYNC_POS_POL : 0) |
 			((hdmi_read(sd, 0x05) & 0x20) ? V4L2_DV_HSYNC_POS_POL : 0);
 		if (bt->interlaced == V4L2_DV_INTERLACED) {
-			bt->height += hdmi_read16(sd, 0x0b, 0xfff);
-			bt->il_vfrontporch = hdmi_read16(sd, 0x2c, 0x1fff) / 2;
-			bt->il_vsync = hdmi_read16(sd, 0x30, 0x1fff) / 2;
-			bt->il_vbackporch = hdmi_read16(sd, 0x34, 0x1fff) / 2;
+			bt->height += hdmi_read16(sd, 0x0b,
+				info->field1_height_mask);
+			bt->il_vfrontporch = hdmi_read16(sd, 0x2c,
+				info->field1_vfrontporch_mask) / 2;
+			bt->il_vsync = hdmi_read16(sd, 0x30,
+				info->field1_vsync_mask) / 2;
+			bt->il_vbackporch = hdmi_read16(sd, 0x34,
+				info->field1_vbackporch_mask) / 2;
 		}
 		adv76xx_fill_optional_dv_timings_fields(sd, timings);
 	} else {
@@ -1725,11 +1757,11 @@ static int adv76xx_s_routing(struct v4l2_subdev *sd,
 	state->selected_input = input;
 
 	disable_input(sd);
-
 	select_input(sd);
-
 	enable_input(sd);
 
+	v4l2_subdev_notify(sd, V4L2_DEVICE_NOTIFY_EVENT,
+			   (void *)&adv76xx_ev_fmt);
 	return 0;
 }
 
@@ -1896,7 +1928,8 @@ static int adv76xx_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 			"%s: fmt_change = 0x%x, fmt_change_digital = 0x%x\n",
 			__func__, fmt_change, fmt_change_digital);
 
-		v4l2_subdev_notify(sd, ADV76XX_FMT_CHANGE, NULL);
+		v4l2_subdev_notify(sd, V4L2_DEVICE_NOTIFY_EVENT,
+				   (void *)&adv76xx_ev_fmt);
 
 		if (handled)
 			*handled = true;
@@ -2102,46 +2135,67 @@ static int adv76xx_set_edid(struct v4l2_subdev *sd, struct v4l2_edid *edid)
 
 /*********** avi info frame CEA-861-E **************/
 
-static void print_avi_infoframe(struct v4l2_subdev *sd)
+static const struct adv76xx_cfg_read_infoframe adv76xx_cri[] = {
+	{ "AVI", 0x01, 0xe0, 0x00 },
+	{ "Audio", 0x02, 0xe3, 0x1c },
+	{ "SDP", 0x04, 0xe6, 0x2a },
+	{ "Vendor", 0x10, 0xec, 0x54 }
+};
+
+static int adv76xx_read_infoframe(struct v4l2_subdev *sd, int index,
+				  union hdmi_infoframe *frame)
 {
+	uint8_t buffer[32];
+	u8 len;
 	int i;
-	u8 buf[14];
-	u8 avi_len;
-	u8 avi_ver;
 
-	if (!is_hdmi(sd)) {
-		v4l2_info(sd, "receive DVI-D signal (AVI infoframe not supported)\n");
-		return;
+	if (!(io_read(sd, 0x60) & adv76xx_cri[index].present_mask)) {
+		v4l2_info(sd, "%s infoframe not received\n",
+			  adv76xx_cri[index].desc);
+		return -ENOENT;
 	}
-	if (!(io_read(sd, 0x60) & 0x01)) {
-		v4l2_info(sd, "AVI infoframe not received\n");
-		return;
+
+	for (i = 0; i < 3; i++)
+		buffer[i] = infoframe_read(sd,
+					   adv76xx_cri[index].head_addr + i);
+
+	len = buffer[2] + 1;
+
+	if (len + 3 > sizeof(buffer)) {
+		v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__,
+			 adv76xx_cri[index].desc, len);
+		return -ENOENT;
 	}
 
-	if (io_read(sd, 0x83) & 0x01) {
-		v4l2_info(sd, "AVI infoframe checksum error has occurred earlier\n");
-		io_write(sd, 0x85, 0x01); /* clear AVI_INF_CKS_ERR_RAW */
-		if (io_read(sd, 0x83) & 0x01) {
-			v4l2_info(sd, "AVI infoframe checksum error still present\n");
-			io_write(sd, 0x85, 0x01); /* clear AVI_INF_CKS_ERR_RAW */
-		}
+	for (i = 0; i < len; i++)
+		buffer[i + 3] = infoframe_read(sd,
+				       adv76xx_cri[index].payload_addr + i);
+
+	if (hdmi_infoframe_unpack(frame, buffer) < 0) {
+		v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__,
+			 adv76xx_cri[index].desc);
+		return -ENOENT;
 	}
+	return 0;
+}
 
-	avi_len = infoframe_read(sd, 0xe2);
-	avi_ver = infoframe_read(sd, 0xe1);
-	v4l2_info(sd, "AVI infoframe version %d (%d byte)\n",
-			avi_ver, avi_len);
+static void adv76xx_log_infoframes(struct v4l2_subdev *sd)
+{
+	int i;
 
-	if (avi_ver != 0x02)
+	if (!is_hdmi(sd)) {
+		v4l2_info(sd, "receive DVI-D signal, no infoframes\n");
 		return;
+	}
 
-	for (i = 0; i < 14; i++)
-		buf[i] = infoframe_read(sd, i);
+	for (i = 0; i < ARRAY_SIZE(adv76xx_cri); i++) {
+		union hdmi_infoframe frame;
+		struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-	v4l2_info(sd,
-		"\t%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
-		buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7],
-		buf[8], buf[9], buf[10], buf[11], buf[12], buf[13]);
+		if (adv76xx_read_infoframe(sd, i, &frame))
+			return;
+		hdmi_infoframe_log(KERN_INFO, &client->dev, &frame);
+	}
 }
 
 static int adv76xx_log_status(struct v4l2_subdev *sd)
@@ -2168,6 +2222,14 @@ static int adv76xx_log_status(struct v4l2_subdev *sd)
 		"invalid", "invalid", "invalid", "invalid", "invalid",
 		"invalid", "invalid", "automatic"
 	};
+	static const char * const hdmi_color_space_txt[16] = {
+		"RGB limited range (16-235)", "RGB full range (0-255)",
+		"YCbCr Bt.601 (16-235)", "YCbCr Bt.709 (16-235)",
+		"xvYCC Bt.601", "xvYCC Bt.709",
+		"YCbCr Bt.601 (0-255)", "YCbCr Bt.709 (0-255)",
+		"sYCC", "Adobe YCC 601", "AdobeRGB", "invalid", "invalid",
+		"invalid", "invalid", "invalid"
+	};
 	static const char * const rgb_quantization_range_txt[] = {
 		"Automatic",
 		"RGB limited range (16-235)",
@@ -2235,11 +2297,12 @@ static int adv76xx_log_status(struct v4l2_subdev *sd)
 			rgb_quantization_range_txt[state->rgb_quantization_range]);
 	v4l2_info(sd, "Input color space: %s\n",
 			input_color_space_txt[reg_io_0x02 >> 4]);
-	v4l2_info(sd, "Output color space: %s %s, saturator %s\n",
+	v4l2_info(sd, "Output color space: %s %s, saturator %s, alt-gamma %s\n",
 			(reg_io_0x02 & 0x02) ? "RGB" : "YCbCr",
 			(reg_io_0x02 & 0x04) ? "(16-235)" : "(0-255)",
-			((reg_io_0x02 & 0x04) ^ (reg_io_0x02 & 0x01)) ?
-				"enabled" : "disabled");
+			(((reg_io_0x02 >> 2) & 0x01) ^ (reg_io_0x02 & 0x01)) ?
+				"enabled" : "disabled",
+			(reg_io_0x02 & 0x08) ? "enabled" : "disabled");
 	v4l2_info(sd, "Color space conversion: %s\n",
 			csc_coeff_sel_rb[cp_read(sd, info->cp_csc) >> 4]);
 
@@ -2276,8 +2339,9 @@ static int adv76xx_log_status(struct v4l2_subdev *sd)
 		v4l2_info(sd, "AV Mute: %s\n", (hdmi_read(sd, 0x04) & 0x40) ? "on" : "off");
 
 		v4l2_info(sd, "Deep color mode: %s\n", deep_color_mode_txt[(hdmi_read(sd, 0x0b) & 0x60) >> 5]);
+		v4l2_info(sd, "HDMI colorspace: %s\n", hdmi_color_space_txt[hdmi_read(sd, 0x53) & 0xf]);
 
-		print_avi_infoframe(sd);
+		adv76xx_log_infoframes(sd);
 	}
 
 	return 0;
@@ -2567,6 +2631,18 @@ static const struct adv76xx_chip_info adv76xx_chip_info[] = {
 			BIT(ADV76XX_PAGE_EDID) | BIT(ADV76XX_PAGE_HDMI) |
 			BIT(ADV76XX_PAGE_TEST) | BIT(ADV76XX_PAGE_CP) |
 			BIT(ADV7604_PAGE_VDP),
+		.linewidth_mask = 0xfff,
+		.field0_height_mask = 0xfff,
+		.field1_height_mask = 0xfff,
+		.hfrontporch_mask = 0x3ff,
+		.hsync_mask = 0x3ff,
+		.hbackporch_mask = 0x3ff,
+		.field0_vfrontporch_mask = 0x1fff,
+		.field0_vsync_mask = 0x1fff,
+		.field0_vbackporch_mask = 0x1fff,
+		.field1_vfrontporch_mask = 0x1fff,
+		.field1_vsync_mask = 0x1fff,
+		.field1_vbackporch_mask = 0x1fff,
 	},
 	[ADV7611] = {
 		.type = ADV7611,
@@ -2596,17 +2672,29 @@ static const struct adv76xx_chip_info adv76xx_chip_info[] = {
 			BIT(ADV76XX_PAGE_INFOFRAME) | BIT(ADV76XX_PAGE_AFE) |
 			BIT(ADV76XX_PAGE_REP) |  BIT(ADV76XX_PAGE_EDID) |
 			BIT(ADV76XX_PAGE_HDMI) | BIT(ADV76XX_PAGE_CP),
+		.linewidth_mask = 0x1fff,
+		.field0_height_mask = 0x1fff,
+		.field1_height_mask = 0x1fff,
+		.hfrontporch_mask = 0x1fff,
+		.hsync_mask = 0x1fff,
+		.hbackporch_mask = 0x1fff,
+		.field0_vfrontporch_mask = 0x3fff,
+		.field0_vsync_mask = 0x3fff,
+		.field0_vbackporch_mask = 0x3fff,
+		.field1_vfrontporch_mask = 0x3fff,
+		.field1_vsync_mask = 0x3fff,
+		.field1_vbackporch_mask = 0x3fff,
 	},
 };
 
-static struct i2c_device_id adv76xx_i2c_id[] = {
+static const struct i2c_device_id adv76xx_i2c_id[] = {
 	{ "adv7604", (kernel_ulong_t)&adv76xx_chip_info[ADV7604] },
 	{ "adv7611", (kernel_ulong_t)&adv76xx_chip_info[ADV7611] },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, adv76xx_i2c_id);
 
-static struct of_device_id adv76xx_of_id[] __maybe_unused = {
+static const struct of_device_id adv76xx_of_id[] __maybe_unused = {
 	{ .compatible = "adi,adv7611", .data = &adv76xx_chip_info[ADV7611] },
 	{ }
 };
diff --git a/drivers/media/i2c/adv7842.c b/drivers/media/i2c/adv7842.c
index b5a37fe10a6a..4cf79b2422d4 100644
--- a/drivers/media/i2c/adv7842.c
+++ b/drivers/media/i2c/adv7842.c
@@ -56,6 +56,28 @@ MODULE_LICENSE("GPL");
 /* ADV7842 system clock frequency */
 #define ADV7842_fsc (28636360)
 
+#define ADV7842_RGB_OUT					(1 << 1)
+
+#define ADV7842_OP_FORMAT_SEL_8BIT			(0 << 0)
+#define ADV7842_OP_FORMAT_SEL_10BIT			(1 << 0)
+#define ADV7842_OP_FORMAT_SEL_12BIT			(2 << 0)
+
+#define ADV7842_OP_MODE_SEL_SDR_422			(0 << 5)
+#define ADV7842_OP_MODE_SEL_DDR_422			(1 << 5)
+#define ADV7842_OP_MODE_SEL_SDR_444			(2 << 5)
+#define ADV7842_OP_MODE_SEL_DDR_444			(3 << 5)
+#define ADV7842_OP_MODE_SEL_SDR_422_2X			(4 << 5)
+#define ADV7842_OP_MODE_SEL_ADI_CM			(5 << 5)
+
+#define ADV7842_OP_CH_SEL_GBR				(0 << 5)
+#define ADV7842_OP_CH_SEL_GRB				(1 << 5)
+#define ADV7842_OP_CH_SEL_BGR				(2 << 5)
+#define ADV7842_OP_CH_SEL_RGB				(3 << 5)
+#define ADV7842_OP_CH_SEL_BRG				(4 << 5)
+#define ADV7842_OP_CH_SEL_RBG				(5 << 5)
+
+#define ADV7842_OP_SWAP_CB_CR				(1 << 0)
+
 /*
 **********************************************************************
 *
@@ -64,6 +86,14 @@ MODULE_LICENSE("GPL");
 **********************************************************************
 */
 
+struct adv7842_format_info {
+	u32 code;
+	u8 op_ch_sel;
+	bool rgb_out;
+	bool swap_cb_cr;
+	u8 op_format_sel;
+};
+
 struct adv7842_state {
 	struct adv7842_platform_data pdata;
 	struct v4l2_subdev sd;
@@ -72,6 +102,9 @@ struct adv7842_state {
 	enum adv7842_mode mode;
 	struct v4l2_dv_timings timings;
 	enum adv7842_vid_std_select vid_std_select;
+
+	const struct adv7842_format_info *format;
+
 	v4l2_std_id norm;
 	struct {
 		u8 edid[256];
@@ -209,6 +242,11 @@ static const struct adv7842_video_standards adv7842_prim_mode_hdmi_gr[] = {
 	{ },
 };
 
+static const struct v4l2_event adv7842_ev_fmt = {
+	.type = V4L2_EVENT_SOURCE_CHANGE,
+	.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+};
+
 /* ----------------------------------------------------------------------- */
 
 static inline struct adv7842_state *to_state(struct v4l2_subdev *sd)
@@ -221,11 +259,21 @@ static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
 	return &container_of(ctrl->handler, struct adv7842_state, hdl)->sd;
 }
 
+static inline unsigned hblanking(const struct v4l2_bt_timings *t)
+{
+	return V4L2_DV_BT_BLANKING_WIDTH(t);
+}
+
 static inline unsigned htotal(const struct v4l2_bt_timings *t)
 {
 	return V4L2_DV_BT_FRAME_WIDTH(t);
 }
 
+static inline unsigned vblanking(const struct v4l2_bt_timings *t)
+{
+	return V4L2_DV_BT_BLANKING_HEIGHT(t);
+}
+
 static inline unsigned vtotal(const struct v4l2_bt_timings *t)
 {
 	return V4L2_DV_BT_FRAME_HEIGHT(t);
@@ -335,6 +383,12 @@ static inline int io_write_and_or(struct v4l2_subdev *sd, u8 reg, u8 mask, u8 va
 	return io_write(sd, reg, (io_read(sd, reg) & mask) | val);
 }
 
+static inline int io_write_clr_set(struct v4l2_subdev *sd,
+				   u8 reg, u8 mask, u8 val)
+{
+	return io_write(sd, reg, (io_read(sd, reg) & ~mask) | val);
+}
+
 static inline int avlink_read(struct v4l2_subdev *sd, u8 reg)
 {
 	struct adv7842_state *state = to_state(sd);
@@ -535,6 +589,64 @@ static void main_reset(struct v4l2_subdev *sd)
 	mdelay(5);
 }
 
+/* -----------------------------------------------------------------------------
+ * Format helpers
+ */
+
+static const struct adv7842_format_info adv7842_formats[] = {
+	{ MEDIA_BUS_FMT_RGB888_1X24, ADV7842_OP_CH_SEL_RGB, true, false,
+	  ADV7842_OP_MODE_SEL_SDR_444 | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_2X8, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_2X8, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV10_2X10, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YVYU10_2X10, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YUYV12_2X12, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YVYU12_2X12, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422 | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, ADV7842_OP_CH_SEL_RBG, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, ADV7842_OP_CH_SEL_RBG, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_8BIT },
+	{ MEDIA_BUS_FMT_UYVY10_1X20, ADV7842_OP_CH_SEL_RBG, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_VYUY10_1X20, ADV7842_OP_CH_SEL_RBG, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YUYV10_1X20, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_YVYU10_1X20, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_10BIT },
+	{ MEDIA_BUS_FMT_UYVY12_1X24, ADV7842_OP_CH_SEL_RBG, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_VYUY12_1X24, ADV7842_OP_CH_SEL_RBG, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YUYV12_1X24, ADV7842_OP_CH_SEL_RGB, false, false,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_12BIT },
+	{ MEDIA_BUS_FMT_YVYU12_1X24, ADV7842_OP_CH_SEL_RGB, false, true,
+	  ADV7842_OP_MODE_SEL_SDR_422_2X | ADV7842_OP_FORMAT_SEL_12BIT },
+};
+
+static const struct adv7842_format_info *
+adv7842_format_info(struct adv7842_state *state, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(adv7842_formats); ++i) {
+		if (adv7842_formats[i].code == code)
+			return &adv7842_formats[i];
+	}
+
+	return NULL;
+}
+
 /* ----------------------------------------------------------------------- */
 
 static inline bool is_analog_input(struct v4l2_subdev *sd)
@@ -1333,12 +1445,12 @@ static int stdi2dv_timings(struct v4l2_subdev *sd,
 	if (v4l2_detect_cvt(stdi->lcf + 1, hfreq, stdi->lcvs,
 			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
 			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
-			    timings))
+			false, timings))
 		return 0;
 	if (v4l2_detect_gtf(stdi->lcf + 1, hfreq, stdi->lcvs,
 			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
 			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
-			    state->aspect_ratio, timings))
+			false, state->aspect_ratio, timings))
 		return 0;
 
 	v4l2_dbg(2, debug, sd,
@@ -1440,9 +1552,11 @@ static int adv7842_query_dv_timings(struct v4l2_subdev *sd,
 	}
 	bt->interlaced = stdi.interlaced ?
 		V4L2_DV_INTERLACED : V4L2_DV_PROGRESSIVE;
+	bt->standards = V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_GTF | V4L2_DV_BT_STD_CVT;
 
 	if (is_digital_input(sd)) {
-		uint32_t freq;
+		u32 freq;
 
 		timings->type = V4L2_DV_BT_656_1120;
 
@@ -1478,6 +1592,10 @@ static int adv7842_query_dv_timings(struct v4l2_subdev *sd,
 					hdmi_read(sd, 0x31)) / 2;
 			bt->il_vbackporch = ((hdmi_read(sd, 0x34) & 0x1f) * 256 +
 					hdmi_read(sd, 0x35)) / 2;
+		} else {
+			bt->il_vfrontporch = 0;
+			bt->il_vsync = 0;
+			bt->il_vbackporch = 0;
 		}
 		adv7842_fill_optional_dv_timings_fields(sd, timings);
 	} else {
@@ -1862,50 +1980,155 @@ static int adv7842_s_routing(struct v4l2_subdev *sd,
 	select_input(sd, state->vid_std_select);
 	enable_input(sd);
 
-	v4l2_subdev_notify(sd, ADV7842_FMT_CHANGE, NULL);
+	v4l2_subdev_notify(sd, V4L2_DEVICE_NOTIFY_EVENT,
+			   (void *)&adv7842_ev_fmt);
 
 	return 0;
 }
 
-static int adv7842_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
-				 u32 *code)
+static int adv7842_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index)
+	if (code->index >= ARRAY_SIZE(adv7842_formats))
 		return -EINVAL;
-	/* Good enough for now */
-	*code = MEDIA_BUS_FMT_FIXED;
+	code->code = adv7842_formats[code->index].code;
 	return 0;
 }
 
-static int adv7842_g_mbus_fmt(struct v4l2_subdev *sd,
-			      struct v4l2_mbus_framefmt *fmt)
+static void adv7842_fill_format(struct adv7842_state *state,
+				struct v4l2_mbus_framefmt *format)
+{
+	memset(format, 0, sizeof(*format));
+
+	format->width = state->timings.bt.width;
+	format->height = state->timings.bt.height;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	if (state->timings.bt.flags & V4L2_DV_FL_IS_CE_VIDEO)
+		format->colorspace = (state->timings.bt.height <= 576) ?
+			V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+}
+
+/*
+ * Compute the op_ch_sel value required to obtain on the bus the component order
+ * corresponding to the selected format taking into account bus reordering
+ * applied by the board at the output of the device.
+ *
+ * The following table gives the op_ch_value from the format component order
+ * (expressed as op_ch_sel value in column) and the bus reordering (expressed as
+ * adv7842_bus_order value in row).
+ *
+ *           |	GBR(0)	GRB(1)	BGR(2)	RGB(3)	BRG(4)	RBG(5)
+ * ----------+-------------------------------------------------
+ * RGB (NOP) |	GBR	GRB	BGR	RGB	BRG	RBG
+ * GRB (1-2) |	BGR	RGB	GBR	GRB	RBG	BRG
+ * RBG (2-3) |	GRB	GBR	BRG	RBG	BGR	RGB
+ * BGR (1-3) |	RBG	BRG	RGB	BGR	GRB	GBR
+ * BRG (ROR) |	BRG	RBG	GRB	GBR	RGB	BGR
+ * GBR (ROL) |	RGB	BGR	RBG	BRG	GBR	GRB
+ */
+static unsigned int adv7842_op_ch_sel(struct adv7842_state *state)
+{
+#define _SEL(a, b, c, d, e, f)	{ \
+	ADV7842_OP_CH_SEL_##a, ADV7842_OP_CH_SEL_##b, ADV7842_OP_CH_SEL_##c, \
+	ADV7842_OP_CH_SEL_##d, ADV7842_OP_CH_SEL_##e, ADV7842_OP_CH_SEL_##f }
+#define _BUS(x)			[ADV7842_BUS_ORDER_##x]
+
+	static const unsigned int op_ch_sel[6][6] = {
+		_BUS(RGB) /* NOP */ = _SEL(GBR, GRB, BGR, RGB, BRG, RBG),
+		_BUS(GRB) /* 1-2 */ = _SEL(BGR, RGB, GBR, GRB, RBG, BRG),
+		_BUS(RBG) /* 2-3 */ = _SEL(GRB, GBR, BRG, RBG, BGR, RGB),
+		_BUS(BGR) /* 1-3 */ = _SEL(RBG, BRG, RGB, BGR, GRB, GBR),
+		_BUS(BRG) /* ROR */ = _SEL(BRG, RBG, GRB, GBR, RGB, BGR),
+		_BUS(GBR) /* ROL */ = _SEL(RGB, BGR, RBG, BRG, GBR, GRB),
+	};
+
+	return op_ch_sel[state->pdata.bus_order][state->format->op_ch_sel >> 5];
+}
+
+static void adv7842_setup_format(struct adv7842_state *state)
+{
+	struct v4l2_subdev *sd = &state->sd;
+
+	io_write_clr_set(sd, 0x02, 0x02,
+			state->format->rgb_out ? ADV7842_RGB_OUT : 0);
+	io_write(sd, 0x03, state->format->op_format_sel |
+		 state->pdata.op_format_mode_sel);
+	io_write_clr_set(sd, 0x04, 0xe0, adv7842_op_ch_sel(state));
+	io_write_clr_set(sd, 0x05, 0x01,
+			state->format->swap_cb_cr ? ADV7842_OP_SWAP_CB_CR : 0);
+}
+
+static int adv7842_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *format)
 {
 	struct adv7842_state *state = to_state(sd);
 
-	fmt->width = state->timings.bt.width;
-	fmt->height = state->timings.bt.height;
-	fmt->code = MEDIA_BUS_FMT_FIXED;
-	fmt->field = V4L2_FIELD_NONE;
+	if (format->pad != ADV7842_PAD_SOURCE)
+		return -EINVAL;
 
 	if (state->mode == ADV7842_MODE_SDP) {
 		/* SPD block */
-		if (!(sdp_read(sd, 0x5A) & 0x01))
+		if (!(sdp_read(sd, 0x5a) & 0x01))
 			return -EINVAL;
-		fmt->width = 720;
+		format->format.code = MEDIA_BUS_FMT_YUYV8_2X8;
+		format->format.width = 720;
 		/* valid signal */
 		if (state->norm & V4L2_STD_525_60)
-			fmt->height = 480;
+			format->format.height = 480;
 		else
-			fmt->height = 576;
-		fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+			format->format.height = 576;
+		format->format.colorspace = V4L2_COLORSPACE_SMPTE170M;
 		return 0;
 	}
 
-	fmt->colorspace = V4L2_COLORSPACE_SRGB;
-	if (state->timings.bt.flags & V4L2_DV_FL_IS_CE_VIDEO) {
-		fmt->colorspace = (state->timings.bt.height <= 576) ?
-			V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_REC709;
+	adv7842_fill_format(state, &format->format);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, cfg, format->pad);
+		format->format.code = fmt->code;
+	} else {
+		format->format.code = state->format->code;
+	}
+
+	return 0;
+}
+
+static int adv7842_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *format)
+{
+	struct adv7842_state *state = to_state(sd);
+	const struct adv7842_format_info *info;
+
+	if (format->pad != ADV7842_PAD_SOURCE)
+		return -EINVAL;
+
+	if (state->mode == ADV7842_MODE_SDP)
+		return adv7842_get_format(sd, cfg, format);
+
+	info = adv7842_format_info(state, format->format.code);
+	if (info == NULL)
+		info = adv7842_format_info(state, MEDIA_BUS_FMT_YUYV8_2X8);
+
+	adv7842_fill_format(state, &format->format);
+	format->format.code = info->code;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_get_try_format(sd, cfg, format->pad);
+		fmt->code = format->format.code;
+	} else {
+		state->format = info;
+		adv7842_setup_format(state);
 	}
+
 	return 0;
 }
 
@@ -1991,7 +2214,8 @@ static int adv7842_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 			 "%s: fmt_change_cp = 0x%x, fmt_change_digital = 0x%x, fmt_change_sdp = 0x%x\n",
 			 __func__, fmt_change_cp, fmt_change_digital,
 			 fmt_change_sdp);
-		v4l2_subdev_notify(sd, ADV7842_FMT_CHANGE, NULL);
+		v4l2_subdev_notify(sd, V4L2_DEVICE_NOTIFY_EVENT,
+				   (void *)&adv7842_ev_fmt);
 		if (handled)
 			*handled = true;
 	}
@@ -2110,7 +2334,7 @@ struct adv7842_cfg_read_infoframe {
 static void log_infoframe(struct v4l2_subdev *sd, struct adv7842_cfg_read_infoframe *cri)
 {
 	int i;
-	uint8_t buffer[32];
+	u8 buffer[32];
 	union hdmi_infoframe frame;
 	u8 len;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -2183,7 +2407,7 @@ static const char * const prim_mode_txt[] = {
 static int adv7842_sdp_log_status(struct v4l2_subdev *sd)
 {
 	/* SDP (Standard definition processor) block */
-	uint8_t sdp_signal_detected = sdp_read(sd, 0x5A) & 0x01;
+	u8 sdp_signal_detected = sdp_read(sd, 0x5A) & 0x01;
 
 	v4l2_info(sd, "Chip powered %s\n", no_power(sd) ? "off" : "on");
 	v4l2_info(sd, "Prim-mode = 0x%x, video std = 0x%x\n",
@@ -2227,10 +2451,10 @@ static int adv7842_cp_log_status(struct v4l2_subdev *sd)
 	/* CP block */
 	struct adv7842_state *state = to_state(sd);
 	struct v4l2_dv_timings timings;
-	uint8_t reg_io_0x02 = io_read(sd, 0x02);
-	uint8_t reg_io_0x21 = io_read(sd, 0x21);
-	uint8_t reg_rep_0x77 = rep_read(sd, 0x77);
-	uint8_t reg_rep_0x7d = rep_read(sd, 0x7d);
+	u8 reg_io_0x02 = io_read(sd, 0x02);
+	u8 reg_io_0x21 = io_read(sd, 0x21);
+	u8 reg_rep_0x77 = rep_read(sd, 0x77);
+	u8 reg_rep_0x7d = rep_read(sd, 0x7d);
 	bool audio_pll_locked = hdmi_read(sd, 0x04) & 0x01;
 	bool audio_sample_packet_detect = hdmi_read(sd, 0x18) & 0x01;
 	bool audio_mute = io_read(sd, 0x65) & 0x40;
@@ -2302,10 +2526,10 @@ static int adv7842_cp_log_status(struct v4l2_subdev *sd)
 	if (no_cp_signal(sd)) {
 		v4l2_info(sd, "STDI: not locked\n");
 	} else {
-		uint32_t bl = ((cp_read(sd, 0xb1) & 0x3f) << 8) | cp_read(sd, 0xb2);
-		uint32_t lcf = ((cp_read(sd, 0xb3) & 0x7) << 8) | cp_read(sd, 0xb4);
-		uint32_t lcvs = cp_read(sd, 0xb3) >> 3;
-		uint32_t fcl = ((cp_read(sd, 0xb8) & 0x1f) << 8) | cp_read(sd, 0xb9);
+		u32 bl = ((cp_read(sd, 0xb1) & 0x3f) << 8) | cp_read(sd, 0xb2);
+		u32 lcf = ((cp_read(sd, 0xb3) & 0x7) << 8) | cp_read(sd, 0xb4);
+		u32 lcvs = cp_read(sd, 0xb3) >> 3;
+		u32 fcl = ((cp_read(sd, 0xb8) & 0x1f) << 8) | cp_read(sd, 0xb9);
 		char hs_pol = ((cp_read(sd, 0xb5) & 0x10) ?
 				((cp_read(sd, 0xb5) & 0x08) ? '+' : '-') : 'x');
 		char vs_pol = ((cp_read(sd, 0xb5) & 0x40) ?
@@ -2545,14 +2769,11 @@ static int adv7842_core_init(struct v4l2_subdev *sd)
 		 0xf0 |
 		 pdata->alt_gamma << 3 |
 		 pdata->op_656_range << 2 |
-		 pdata->rgb_out << 1 |
 		 pdata->alt_data_sat << 0);
-	io_write(sd, 0x03, pdata->op_format_sel);
-	io_write_and_or(sd, 0x04, 0x1f, pdata->op_ch_sel << 5);
 	io_write_and_or(sd, 0x05, 0xf0, pdata->blank_data << 3 |
 			pdata->insert_av_codes << 2 |
-			pdata->replicate_av_codes << 1 |
-			pdata->invert_cbcr << 0);
+			pdata->replicate_av_codes << 1);
+	adv7842_setup_format(state);
 
 	/* HDMI audio */
 	hdmi_write_and_or(sd, 0x1a, 0xf1, 0x08); /* Wait 1 s before unmute */
@@ -2809,13 +3030,12 @@ static const struct v4l2_subdev_video_ops adv7842_video_ops = {
 	.s_dv_timings = adv7842_s_dv_timings,
 	.g_dv_timings = adv7842_g_dv_timings,
 	.query_dv_timings = adv7842_query_dv_timings,
-	.enum_mbus_fmt = adv7842_enum_mbus_fmt,
-	.g_mbus_fmt = adv7842_g_mbus_fmt,
-	.try_mbus_fmt = adv7842_g_mbus_fmt,
-	.s_mbus_fmt = adv7842_g_mbus_fmt,
 };
 
 static const struct v4l2_subdev_pad_ops adv7842_pad_ops = {
+	.enum_mbus_code = adv7842_enum_mbus_code,
+	.get_fmt = adv7842_get_format,
+	.set_fmt = adv7842_set_format,
 	.get_edid = adv7842_get_edid,
 	.set_edid = adv7842_set_edid,
 	.enum_dv_timings = adv7842_enum_dv_timings,
@@ -2986,6 +3206,7 @@ static int adv7842_probe(struct i2c_client *client,
 	/* platform data */
 	state->pdata = *pdata;
 	state->timings = cea640x480;
+	state->format = adv7842_format_info(state, MEDIA_BUS_FMT_YUYV8_2X8);
 
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &adv7842_ops);
diff --git a/drivers/media/i2c/ak881x.c b/drivers/media/i2c/ak881x.c
index 69aeaf397624..29846245aa3b 100644
--- a/drivers/media/i2c/ak881x.c
+++ b/drivers/media/i2c/ak881x.c
@@ -93,12 +93,17 @@ static int ak881x_s_register(struct v4l2_subdev *sd,
 }
 #endif
 
-static int ak881x_try_g_mbus_fmt(struct v4l2_subdev *sd,
-				 struct v4l2_mbus_framefmt *mf)
+static int ak881x_fill_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ak881x *ak881x = to_ak881x(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	v4l_bound_align_image(&mf->width, 0, 720, 2,
 			      &mf->height, 0, ak881x->lines, 1, 0);
 	mf->field	= V4L2_FIELD_INTERLACED;
@@ -108,23 +113,14 @@ static int ak881x_try_g_mbus_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ak881x_s_mbus_fmt(struct v4l2_subdev *sd,
-			     struct v4l2_mbus_framefmt *mf)
+static int ak881x_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (mf->field != V4L2_FIELD_INTERLACED ||
-	    mf->code != MEDIA_BUS_FMT_YUYV8_2X8)
+	if (code->pad || code->index)
 		return -EINVAL;
 
-	return ak881x_try_g_mbus_fmt(sd, mf);
-}
-
-static int ak881x_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
-				u32 *code)
-{
-	if (index)
-		return -EINVAL;
-
-	*code = MEDIA_BUS_FMT_YUYV8_2X8;
+	code->code = MEDIA_BUS_FMT_YUYV8_2X8;
 	return 0;
 }
 
@@ -211,18 +207,21 @@ static struct v4l2_subdev_core_ops ak881x_subdev_core_ops = {
 };
 
 static struct v4l2_subdev_video_ops ak881x_subdev_video_ops = {
-	.s_mbus_fmt	= ak881x_s_mbus_fmt,
-	.g_mbus_fmt	= ak881x_try_g_mbus_fmt,
-	.try_mbus_fmt	= ak881x_try_g_mbus_fmt,
 	.cropcap	= ak881x_cropcap,
-	.enum_mbus_fmt	= ak881x_enum_mbus_fmt,
 	.s_std_output	= ak881x_s_std_output,
 	.s_stream	= ak881x_s_stream,
 };
 
+static const struct v4l2_subdev_pad_ops ak881x_subdev_pad_ops = {
+	.enum_mbus_code = ak881x_enum_mbus_code,
+	.set_fmt	= ak881x_fill_fmt,
+	.get_fmt	= ak881x_fill_fmt,
+};
+
 static struct v4l2_subdev_ops ak881x_subdev_ops = {
 	.core	= &ak881x_subdev_core_ops,
 	.video	= &ak881x_subdev_video_ops,
+	.pad	= &ak881x_subdev_pad_ops,
 };
 
 static int ak881x_probe(struct i2c_client *client,
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index bd496447749a..e15a789ad596 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -971,7 +971,7 @@ static void input_change(struct i2c_client *client)
 		   not used by any public broadcast network, force
 		   6.5 MHz carrier to be interpreted as System DK,
 		   this avoids DK audio detection instability */
-	       cx25840_write(client, 0x80b, 0x00);
+		cx25840_write(client, 0x80b, 0x00);
 	} else if (std & V4L2_STD_SECAM) {
 		/* Autodetect audio standard and audio system */
 		cx25840_write(client, 0x808, 0xff);
@@ -1366,14 +1366,17 @@ static int cx25840_s_ctrl(struct v4l2_ctrl *ctrl)
 
 /* ----------------------------------------------------------------------- */
 
-static int cx25840_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+static int cx25840_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *fmt = &format->format;
 	struct cx25840_state *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	int HSC, VSC, Vsrc, Hsrc, filter, Vlines;
 	int is_50Hz = !(state->std & V4L2_STD_525_60);
 
-	if (fmt->code != MEDIA_BUS_FMT_FIXED)
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_FIXED)
 		return -EINVAL;
 
 	fmt->field = V4L2_FIELD_INTERLACED;
@@ -1403,6 +1406,8 @@ static int cx25840_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt
 				fmt->width, fmt->height);
 		return -ERANGE;
 	}
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
 
 	HSC = (Hsrc * (1 << 20)) / fmt->width - (1 << 20);
 	VSC = (1 << 16) - (Vsrc * (1 << 9) / Vlines - (1 << 9));
@@ -5068,7 +5073,6 @@ static const struct v4l2_subdev_video_ops cx25840_video_ops = {
 	.s_std = cx25840_s_std,
 	.g_std = cx25840_g_std,
 	.s_routing = cx25840_s_video_routing,
-	.s_mbus_fmt = cx25840_s_mbus_fmt,
 	.s_stream = cx25840_s_stream,
 	.g_input_status = cx25840_g_input_status,
 };
@@ -5080,12 +5084,17 @@ static const struct v4l2_subdev_vbi_ops cx25840_vbi_ops = {
 	.g_sliced_fmt = cx25840_g_sliced_fmt,
 };
 
+static const struct v4l2_subdev_pad_ops cx25840_pad_ops = {
+	.set_fmt = cx25840_set_fmt,
+};
+
 static const struct v4l2_subdev_ops cx25840_ops = {
 	.core = &cx25840_core_ops,
 	.tuner = &cx25840_tuner_ops,
 	.audio = &cx25840_audio_ops,
 	.video = &cx25840_video_ops,
 	.vbi = &cx25840_vbi_ops,
+	.pad = &cx25840_pad_ops,
 	.ir = &cx25840_ir_ops,
 };
 
diff --git a/drivers/media/i2c/ml86v7667.c b/drivers/media/i2c/ml86v7667.c
index d7307862c2c5..af5eaf2db2a0 100644
--- a/drivers/media/i2c/ml86v7667.c
+++ b/drivers/media/i2c/ml86v7667.c
@@ -191,21 +191,27 @@ static int ml86v7667_g_input_status(struct v4l2_subdev *sd, u32 *status)
 	return 0;
 }
 
-static int ml86v7667_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
-				   u32 *code)
+static int ml86v7667_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index > 0)
+	if (code->pad || code->index > 0)
 		return -EINVAL;
 
-	*code = MEDIA_BUS_FMT_YUYV8_2X8;
+	code->code = MEDIA_BUS_FMT_YUYV8_2X8;
 
 	return 0;
 }
 
-static int ml86v7667_mbus_fmt(struct v4l2_subdev *sd,
-			      struct v4l2_mbus_framefmt *fmt)
+static int ml86v7667_fill_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
 	struct ml86v7667_priv *priv = to_ml86v7667(sd);
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
 
 	fmt->code = MEDIA_BUS_FMT_YUYV8_2X8;
 	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
@@ -279,13 +285,15 @@ static struct v4l2_subdev_video_ops ml86v7667_subdev_video_ops = {
 	.s_std = ml86v7667_s_std,
 	.querystd = ml86v7667_querystd,
 	.g_input_status = ml86v7667_g_input_status,
-	.enum_mbus_fmt = ml86v7667_enum_mbus_fmt,
-	.try_mbus_fmt = ml86v7667_mbus_fmt,
-	.g_mbus_fmt = ml86v7667_mbus_fmt,
-	.s_mbus_fmt = ml86v7667_mbus_fmt,
 	.g_mbus_config = ml86v7667_g_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops ml86v7667_subdev_pad_ops = {
+	.enum_mbus_code = ml86v7667_enum_mbus_code,
+	.get_fmt = ml86v7667_fill_fmt,
+	.set_fmt = ml86v7667_fill_fmt,
+};
+
 static struct v4l2_subdev_core_ops ml86v7667_subdev_core_ops = {
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register = ml86v7667_g_register,
@@ -296,6 +304,7 @@ static struct v4l2_subdev_core_ops ml86v7667_subdev_core_ops = {
 static struct v4l2_subdev_ops ml86v7667_subdev_ops = {
 	.core = &ml86v7667_subdev_core_ops,
 	.video = &ml86v7667_subdev_video_ops,
+	.pad = &ml86v7667_subdev_pad_ops,
 };
 
 static int ml86v7667_init(struct ml86v7667_priv *priv)
diff --git a/drivers/media/i2c/mt9v011.c b/drivers/media/i2c/mt9v011.c
index a10f7f8f0558..57132cdba5ea 100644
--- a/drivers/media/i2c/mt9v011.c
+++ b/drivers/media/i2c/mt9v011.c
@@ -324,19 +324,25 @@ static int mt9v011_reset(struct v4l2_subdev *sd, u32 val)
 	return 0;
 }
 
-static int mt9v011_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
-					u32 *code)
+static int mt9v011_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index > 0)
+	if (code->pad || code->index > 0)
 		return -EINVAL;
 
-	*code = MEDIA_BUS_FMT_SGRBG8_1X8;
+	code->code = MEDIA_BUS_FMT_SGRBG8_1X8;
 	return 0;
 }
 
-static int mt9v011_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+static int mt9v011_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
-	if (fmt->code != MEDIA_BUS_FMT_SGRBG8_1X8)
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct mt9v011 *core = to_mt9v011(sd);
+
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_SGRBG8_1X8)
 		return -EINVAL;
 
 	v4l_bound_align_image(&fmt->width, 48, 639, 1,
@@ -344,6 +350,15 @@ static int mt9v011_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefm
 	fmt->field = V4L2_FIELD_NONE;
 	fmt->colorspace = V4L2_COLORSPACE_SRGB;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		core->width = fmt->width;
+		core->height = fmt->height;
+
+		set_res(sd);
+	} else {
+		cfg->try_fmt = *fmt;
+	}
+
 	return 0;
 }
 
@@ -385,23 +400,6 @@ static int mt9v011_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
 	return 0;
 }
 
-static int mt9v011_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
-{
-	struct mt9v011 *core = to_mt9v011(sd);
-	int rc;
-
-	rc = mt9v011_try_mbus_fmt(sd, fmt);
-	if (rc < 0)
-		return -EINVAL;
-
-	core->width = fmt->width;
-	core->height = fmt->height;
-
-	set_res(sd);
-
-	return 0;
-}
-
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int mt9v011_g_register(struct v4l2_subdev *sd,
 			      struct v4l2_dbg_register *reg)
@@ -469,16 +467,19 @@ static const struct v4l2_subdev_core_ops mt9v011_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops mt9v011_video_ops = {
-	.enum_mbus_fmt = mt9v011_enum_mbus_fmt,
-	.try_mbus_fmt = mt9v011_try_mbus_fmt,
-	.s_mbus_fmt = mt9v011_s_mbus_fmt,
 	.g_parm = mt9v011_g_parm,
 	.s_parm = mt9v011_s_parm,
 };
 
+static const struct v4l2_subdev_pad_ops mt9v011_pad_ops = {
+	.enum_mbus_code = mt9v011_enum_mbus_code,
+	.set_fmt = mt9v011_set_fmt,
+};
+
 static const struct v4l2_subdev_ops mt9v011_ops = {
 	.core  = &mt9v011_core_ops,
 	.video = &mt9v011_video_ops,
+	.pad   = &mt9v011_pad_ops,
 };
 
 
diff --git a/drivers/media/i2c/ov2659.c b/drivers/media/i2c/ov2659.c
index edebd114279d..6edffc7b74e3 100644
--- a/drivers/media/i2c/ov2659.c
+++ b/drivers/media/i2c/ov2659.c
@@ -1046,16 +1046,21 @@ static int ov2659_get_fmt(struct v4l2_subdev *sd,
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov2659 *ov2659 = to_ov2659(sd);
-	struct v4l2_mbus_framefmt *mf;
 
 	dev_dbg(&client->dev, "ov2659_get_fmt\n");
 
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
+		struct v4l2_mbus_framefmt *mf;
+
 		mf = v4l2_subdev_get_try_format(sd, cfg, 0);
 		mutex_lock(&ov2659->lock);
 		fmt->format = *mf;
 		mutex_unlock(&ov2659->lock);
 		return 0;
+#else
+	return -ENOTTY;
+#endif
 	}
 
 	mutex_lock(&ov2659->lock);
@@ -1102,7 +1107,7 @@ static int ov2659_set_fmt(struct v4l2_subdev *sd,
 			  struct v4l2_subdev_format *fmt)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	unsigned int index = ARRAY_SIZE(ov2659_formats);
+	int index = ARRAY_SIZE(ov2659_formats);
 	struct v4l2_mbus_framefmt *mf = &fmt->format;
 	const struct ov2659_framesize *size = NULL;
 	struct ov2659 *ov2659 = to_ov2659(sd);
@@ -1126,8 +1131,12 @@ static int ov2659_set_fmt(struct v4l2_subdev *sd,
 	mutex_lock(&ov2659->lock);
 
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
 		mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
 		*mf = fmt->format;
+#else
+		return -ENOTTY;
+#endif
 	} else {
 		s64 val;
 
@@ -1257,6 +1266,7 @@ static const char * const ov2659_test_pattern_menu[] = {
  * V4L2 subdev internal operations
  */
 
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
 static int ov2659_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
@@ -1269,6 +1279,7 @@ static int ov2659_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 
 	return 0;
 }
+#endif
 
 static const struct v4l2_subdev_core_ops ov2659_subdev_core_ops = {
 	.log_status = v4l2_ctrl_subdev_log_status,
@@ -1287,6 +1298,7 @@ static const struct v4l2_subdev_pad_ops ov2659_subdev_pad_ops = {
 	.set_fmt = ov2659_set_fmt,
 };
 
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
 static const struct v4l2_subdev_ops ov2659_subdev_ops = {
 	.core  = &ov2659_subdev_core_ops,
 	.video = &ov2659_subdev_video_ops,
@@ -1296,6 +1308,7 @@ static const struct v4l2_subdev_ops ov2659_subdev_ops = {
 static const struct v4l2_subdev_internal_ops ov2659_subdev_internal_ops = {
 	.open = ov2659_open,
 };
+#endif
 
 static int ov2659_detect(struct v4l2_subdev *sd)
 {
@@ -1340,8 +1353,8 @@ static struct ov2659_platform_data *
 ov2659_get_pdata(struct i2c_client *client)
 {
 	struct ov2659_platform_data *pdata;
+	struct v4l2_of_endpoint *bus_cfg;
 	struct device_node *endpoint;
-	int ret;
 
 	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
 		return client->dev.platform_data;
@@ -1350,18 +1363,27 @@ ov2659_get_pdata(struct i2c_client *client)
 	if (!endpoint)
 		return NULL;
 
+	bus_cfg = v4l2_of_alloc_parse_endpoint(endpoint);
+	if (IS_ERR(bus_cfg)) {
+		pdata = NULL;
+		goto done;
+	}
+
 	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		goto done;
 
-	ret = of_property_read_u64(endpoint, "link-frequencies",
-				   &pdata->link_frequency);
-	if (ret) {
-		dev_err(&client->dev, "link-frequencies property not found\n");
+	if (!bus_cfg->nr_of_link_frequencies) {
+		dev_err(&client->dev,
+			"link-frequencies property not found or too many\n");
 		pdata = NULL;
+		goto done;
 	}
 
+	pdata->link_frequency = bus_cfg->link_frequencies[0];
+
 done:
+	v4l2_of_free_endpoint(bus_cfg);
 	of_node_put(endpoint);
 	return pdata;
 }
@@ -1417,11 +1439,13 @@ static int ov2659_probe(struct i2c_client *client,
 
 	sd = &ov2659->sd;
 	client->flags |= I2C_CLIENT_SCCB;
+#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
 	v4l2_i2c_subdev_init(sd, client, &ov2659_subdev_ops);
 
 	sd->internal_ops = &ov2659_subdev_internal_ops;
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
 		     V4L2_SUBDEV_FL_HAS_EVENTS;
+#endif
 
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	ov2659->pad.flags = MEDIA_PAD_FL_SOURCE;
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index b9847527eb5a..2d1e25f10973 100644
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -639,7 +639,7 @@ static struct ov7670_format_struct {
 } ov7670_formats[] = {
 	{
 		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
-		.colorspace	= V4L2_COLORSPACE_JPEG,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
 		.regs 		= ov7670_fmt_yuv422,
 		.cmatrix	= { 128, -128, 0, -34, -94, 128 },
 	},
@@ -899,13 +899,14 @@ static int ov7670_set_hw(struct v4l2_subdev *sd, int hstart, int hstop,
 }
 
 
-static int ov7670_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
-					u32 *code)
+static int ov7670_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= N_OV7670_FMTS)
+	if (code->pad || code->index >= N_OV7670_FMTS)
 		return -EINVAL;
 
-	*code = ov7670_formats[index].mbus_code;
+	code->code = ov7670_formats[code->index].mbus_code;
 	return 0;
 }
 
@@ -970,17 +971,12 @@ static int ov7670_try_fmt_internal(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov7670_try_mbus_fmt(struct v4l2_subdev *sd,
-			    struct v4l2_mbus_framefmt *fmt)
-{
-	return ov7670_try_fmt_internal(sd, fmt, NULL, NULL);
-}
-
 /*
  * Set a format.
  */
-static int ov7670_s_mbus_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *fmt)
+static int ov7670_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
 	struct ov7670_format_struct *ovfmt;
 	struct ov7670_win_size *wsize;
@@ -988,7 +984,18 @@ static int ov7670_s_mbus_fmt(struct v4l2_subdev *sd,
 	unsigned char com7;
 	int ret;
 
-	ret = ov7670_try_fmt_internal(sd, fmt, &ovfmt, &wsize);
+	if (format->pad)
+		return -EINVAL;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		ret = ov7670_try_fmt_internal(sd, &format->format, NULL, NULL);
+		if (ret)
+			return ret;
+		cfg->try_fmt = format->format;
+		return 0;
+	}
+
+	ret = ov7670_try_fmt_internal(sd, &format->format, &ovfmt, &wsize);
 
 	if (ret)
 		return ret;
@@ -1073,10 +1080,33 @@ static int ov7670_enum_frame_interval(struct v4l2_subdev *sd,
 				      struct v4l2_subdev_pad_config *cfg,
 				      struct v4l2_subdev_frame_interval_enum *fie)
 {
+	struct ov7670_info *info = to_state(sd);
+	unsigned int n_win_sizes = info->devtype->n_win_sizes;
+	int i;
+
 	if (fie->pad)
 		return -EINVAL;
 	if (fie->index >= ARRAY_SIZE(ov7670_frame_rates))
 		return -EINVAL;
+
+	/*
+	 * Check if the width/height is valid.
+	 *
+	 * If a minimum width/height was requested, filter out the capture
+	 * windows that fall outside that.
+	 */
+	for (i = 0; i < n_win_sizes; i++) {
+		struct ov7670_win_size *win = &info->devtype->win_sizes[i];
+
+		if (info->min_width && win->width < info->min_width)
+			continue;
+		if (info->min_height && win->height < info->min_height)
+			continue;
+		if (fie->width == win->width && fie->height == win->height)
+			break;
+	}
+	if (i == n_win_sizes)
+		return -EINVAL;
 	fie->interval.numerator = 1;
 	fie->interval.denominator = ov7670_frame_rates[fie->index];
 	return 0;
@@ -1362,7 +1392,7 @@ static int ov7670_s_exp(struct v4l2_subdev *sd, int value)
 	unsigned char com1, com8, aech, aechh;
 
 	ret = ov7670_read(sd, REG_COM1, &com1) +
-		ov7670_read(sd, REG_COM8, &com8);
+		ov7670_read(sd, REG_COM8, &com8) +
 		ov7670_read(sd, REG_AECHH, &aechh);
 	if (ret)
 		return ret;
@@ -1485,9 +1515,6 @@ static const struct v4l2_subdev_core_ops ov7670_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops ov7670_video_ops = {
-	.enum_mbus_fmt = ov7670_enum_mbus_fmt,
-	.try_mbus_fmt = ov7670_try_mbus_fmt,
-	.s_mbus_fmt = ov7670_s_mbus_fmt,
 	.s_parm = ov7670_s_parm,
 	.g_parm = ov7670_g_parm,
 };
@@ -1495,6 +1522,8 @@ static const struct v4l2_subdev_video_ops ov7670_video_ops = {
 static const struct v4l2_subdev_pad_ops ov7670_pad_ops = {
 	.enum_frame_interval = ov7670_enum_frame_interval,
 	.enum_frame_size = ov7670_enum_frame_size,
+	.enum_mbus_code = ov7670_enum_mbus_code,
+	.set_fmt = ov7670_set_fmt,
 };
 
 static const struct v4l2_subdev_ops ov7670_ops = {
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index 08b234bd2962..53c5ea89f0b9 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -1453,7 +1453,7 @@ static int s5c73m3_oif_set_power(struct v4l2_subdev *sd, int on)
 			state->apply_fiv = 1;
 			state->apply_fmt = 1;
 		}
-	} else if (!on == state->power) {
+	} else if (state->power == !on) {
 		ret = s5c73m3_set_af_softlanding(state);
 		if (!ret)
 			ret = __s5c73m3_power_off(state);
diff --git a/drivers/media/i2c/s5k5baf.c b/drivers/media/i2c/s5k5baf.c
index 297ef04e146a..774e0d0c94cb 100644
--- a/drivers/media/i2c/s5k5baf.c
+++ b/drivers/media/i2c/s5k5baf.c
@@ -491,7 +491,7 @@ static void s5k5baf_write_arr_seq(struct s5k5baf *state, u16 addr,
 	v4l2_dbg(3, debug, c, "i2c_write_seq(count=%d): %*ph\n", count,
 		 min(2 * count, 64), seq);
 
-	buf[0] = __constant_cpu_to_be16(REG_CMD_BUF);
+	buf[0] = cpu_to_be16(REG_CMD_BUF);
 
 	while (count > 0) {
 		int n = min_t(int, count, ARRAY_SIZE(buf) - 1);
@@ -1054,7 +1054,7 @@ static int s5k5baf_set_power(struct v4l2_subdev *sd, int on)
 
 	mutex_lock(&state->lock);
 
-	if (!on != state->power)
+	if (state->power != !on)
 		goto out;
 
 	if (on) {
diff --git a/drivers/media/i2c/s5k6aa.c b/drivers/media/i2c/s5k6aa.c
index de803a11efb4..d0ad6a25bdab 100644
--- a/drivers/media/i2c/s5k6aa.c
+++ b/drivers/media/i2c/s5k6aa.c
@@ -875,7 +875,7 @@ static int s5k6aa_set_power(struct v4l2_subdev *sd, int on)
 
 	mutex_lock(&s5k6aa->lock);
 
-	if (!on == s5k6aa->power) {
+	if (s5k6aa->power == !on) {
 		if (on) {
 			ret = __s5k6aa_power_on(s5k6aa);
 			if (!ret)
diff --git a/drivers/media/i2c/saa6752hs.c b/drivers/media/i2c/saa6752hs.c
index f14c0e6435a3..ba3c4156644d 100644
--- a/drivers/media/i2c/saa6752hs.c
+++ b/drivers/media/i2c/saa6752hs.c
@@ -554,10 +554,16 @@ static int saa6752hs_init(struct v4l2_subdev *sd, u32 leading_null_bytes)
 	return 0;
 }
 
-static int saa6752hs_g_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
+static int saa6752hs_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *f = &format->format;
 	struct saa6752hs_state *h = to_state(sd);
 
+	if (format->pad)
+		return -EINVAL;
+
 	if (h->video_format == SAA6752HS_VF_UNKNOWN)
 		h->video_format = SAA6752HS_VF_D1;
 	f->width = v4l2_format_table[h->video_format].fmt.pix.width;
@@ -568,10 +574,17 @@ static int saa6752hs_g_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefm
 	return 0;
 }
 
-static int saa6752hs_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
+static int saa6752hs_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *f = &format->format;
+	struct saa6752hs_state *h = to_state(sd);
 	int dist_352, dist_480, dist_720;
 
+	if (format->pad)
+		return -EINVAL;
+
 	f->code = MEDIA_BUS_FMT_FIXED;
 
 	dist_352 = abs(f->width - 352);
@@ -592,15 +605,11 @@ static int saa6752hs_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_frame
 	}
 	f->field = V4L2_FIELD_INTERLACED;
 	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
-	return 0;
-}
-
-static int saa6752hs_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
-{
-	struct saa6752hs_state *h = to_state(sd);
 
-	if (f->code != MEDIA_BUS_FMT_FIXED)
-		return -EINVAL;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *f;
+		return 0;
+	}
 
 	/*
 	  FIXME: translate and round width/height into EMPRESS
@@ -614,7 +623,9 @@ static int saa6752hs_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefm
 	  D1     | 720x576 | 720x480
 	*/
 
-	saa6752hs_try_mbus_fmt(sd, f);
+	if (f->code != MEDIA_BUS_FMT_FIXED)
+		return -EINVAL;
+
 	if (f->width == 720)
 		h->video_format = SAA6752HS_VF_D1;
 	else if (f->width == 480)
@@ -647,14 +658,17 @@ static const struct v4l2_subdev_core_ops saa6752hs_core_ops = {
 
 static const struct v4l2_subdev_video_ops saa6752hs_video_ops = {
 	.s_std = saa6752hs_s_std,
-	.s_mbus_fmt = saa6752hs_s_mbus_fmt,
-	.try_mbus_fmt = saa6752hs_try_mbus_fmt,
-	.g_mbus_fmt = saa6752hs_g_mbus_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops saa6752hs_pad_ops = {
+	.get_fmt = saa6752hs_get_fmt,
+	.set_fmt = saa6752hs_set_fmt,
 };
 
 static const struct v4l2_subdev_ops saa6752hs_ops = {
 	.core = &saa6752hs_core_ops,
 	.video = &saa6752hs_video_ops,
+	.pad = &saa6752hs_pad_ops,
 };
 
 static int saa6752hs_probe(struct i2c_client *client,
diff --git a/drivers/media/i2c/saa7115.c b/drivers/media/i2c/saa7115.c
index 7147c8b68fac..0eae5f4471e2 100644
--- a/drivers/media/i2c/saa7115.c
+++ b/drivers/media/i2c/saa7115.c
@@ -1170,12 +1170,18 @@ static int saa711x_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_f
 	return 0;
 }
 
-static int saa711x_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+static int saa711x_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
-	if (fmt->code != MEDIA_BUS_FMT_FIXED)
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_FIXED)
 		return -EINVAL;
 	fmt->field = V4L2_FIELD_INTERLACED;
 	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
 	return saa711x_set_size(sd, fmt->width, fmt->height);
 }
 
@@ -1603,7 +1609,6 @@ static const struct v4l2_subdev_video_ops saa711x_video_ops = {
 	.s_std = saa711x_s_std,
 	.s_routing = saa711x_s_routing,
 	.s_crystal_freq = saa711x_s_crystal_freq,
-	.s_mbus_fmt = saa711x_s_mbus_fmt,
 	.s_stream = saa711x_s_stream,
 	.querystd = saa711x_querystd,
 	.g_input_status = saa711x_g_input_status,
@@ -1617,12 +1622,17 @@ static const struct v4l2_subdev_vbi_ops saa711x_vbi_ops = {
 	.s_raw_fmt = saa711x_s_raw_fmt,
 };
 
+static const struct v4l2_subdev_pad_ops saa711x_pad_ops = {
+	.set_fmt = saa711x_set_fmt,
+};
+
 static const struct v4l2_subdev_ops saa711x_ops = {
 	.core = &saa711x_core_ops,
 	.tuner = &saa711x_tuner_ops,
 	.audio = &saa711x_audio_ops,
 	.video = &saa711x_video_ops,
 	.vbi = &saa711x_vbi_ops,
+	.pad = &saa711x_pad_ops,
 };
 
 #define CHIP_VER_SIZE	16
diff --git a/drivers/media/i2c/saa717x.c b/drivers/media/i2c/saa717x.c
index 0d0f9a917cd3..7d517361e419 100644
--- a/drivers/media/i2c/saa717x.c
+++ b/drivers/media/i2c/saa717x.c
@@ -152,9 +152,9 @@ static u32 saa717x_read(struct v4l2_subdev *sd, u32 reg)
 	i2c_transfer(adap, msgs, 2);
 
 	if (fw_addr)
-		value = (mm2[2] & 0xff)  | ((mm2[1] & 0xff) >> 8) | ((mm2[0] & 0xff) >> 16);
+		value = (mm2[2] << 16)  | (mm2[1] << 8) | mm2[0];
 	else
-		value = mm2[0] & 0xff;
+		value = mm2[0];
 
 	v4l2_dbg(2, debug, sd, "read:  reg 0x%03x=0x%08x\n", reg, value);
 	return value;
@@ -992,13 +992,16 @@ static int saa717x_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_regi
 }
 #endif
 
-static int saa717x_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+static int saa717x_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *fmt = &format->format;
 	int prescale, h_scale, v_scale;
 
 	v4l2_dbg(1, debug, sd, "decoder set size\n");
 
-	if (fmt->code != MEDIA_BUS_FMT_FIXED)
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_FIXED)
 		return -EINVAL;
 
 	/* FIXME need better bounds checking here */
@@ -1010,6 +1013,9 @@ static int saa717x_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt
 	fmt->field = V4L2_FIELD_INTERLACED;
 	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
 	/* scaling setting */
 	/* NTSC and interlace only */
 	prescale = SAA717X_NTSC_WIDTH / fmt->width;
@@ -1217,7 +1223,6 @@ static const struct v4l2_subdev_tuner_ops saa717x_tuner_ops = {
 static const struct v4l2_subdev_video_ops saa717x_video_ops = {
 	.s_std = saa717x_s_std,
 	.s_routing = saa717x_s_video_routing,
-	.s_mbus_fmt = saa717x_s_mbus_fmt,
 	.s_stream = saa717x_s_stream,
 };
 
@@ -1225,11 +1230,16 @@ static const struct v4l2_subdev_audio_ops saa717x_audio_ops = {
 	.s_routing = saa717x_s_audio_routing,
 };
 
+static const struct v4l2_subdev_pad_ops saa717x_pad_ops = {
+	.set_fmt = saa717x_set_fmt,
+};
+
 static const struct v4l2_subdev_ops saa717x_ops = {
 	.core = &saa717x_core_ops,
 	.tuner = &saa717x_tuner_ops,
 	.audio = &saa717x_audio_ops,
 	.video = &saa717x_video_ops,
+	.pad = &saa717x_pad_ops,
 };
 
 /* ----------------------------------------------------------------------- */
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index 557f25def3a0..636ebd6fe5dc 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -2975,9 +2975,9 @@ static int smiapp_resume(struct device *dev)
 static struct smiapp_platform_data *smiapp_get_pdata(struct device *dev)
 {
 	struct smiapp_platform_data *pdata;
-	struct v4l2_of_endpoint bus_cfg;
+	struct v4l2_of_endpoint *bus_cfg;
 	struct device_node *ep;
-	uint32_t asize;
+	int i;
 	int rval;
 
 	if (!dev->of_node)
@@ -2987,13 +2987,15 @@ static struct smiapp_platform_data *smiapp_get_pdata(struct device *dev)
 	if (!ep)
 		return NULL;
 
+	bus_cfg = v4l2_of_alloc_parse_endpoint(ep);
+	if (IS_ERR(bus_cfg))
+		goto out_err;
+
 	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		goto out_err;
 
-	v4l2_of_parse_endpoint(ep, &bus_cfg);
-
-	switch (bus_cfg.bus_type) {
+	switch (bus_cfg->bus_type) {
 	case V4L2_MBUS_CSI2:
 		pdata->csi_signalling_mode = SMIAPP_CSI_SIGNALLING_MODE_CSI2;
 		break;
@@ -3002,7 +3004,7 @@ static struct smiapp_platform_data *smiapp_get_pdata(struct device *dev)
 		goto out_err;
 	}
 
-	pdata->lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+	pdata->lanes = bus_cfg->bus.mipi_csi2.num_data_lanes;
 	dev_dbg(dev, "lanes %u\n", pdata->lanes);
 
 	/* xshutdown GPIO is optional */
@@ -3022,34 +3024,30 @@ static struct smiapp_platform_data *smiapp_get_pdata(struct device *dev)
 	dev_dbg(dev, "reset %d, nvm %d, clk %d, csi %d\n", pdata->xshutdown,
 		pdata->nvm_size, pdata->ext_clk, pdata->csi_signalling_mode);
 
-	rval = of_get_property(ep, "link-frequencies", &asize) ? 0 : -ENOENT;
-	if (rval) {
-		dev_warn(dev, "can't get link-frequencies array size\n");
+	if (!bus_cfg->nr_of_link_frequencies) {
+		dev_warn(dev, "no link frequencies defined\n");
 		goto out_err;
 	}
 
-	pdata->op_sys_clock = devm_kzalloc(dev, asize, GFP_KERNEL);
+	pdata->op_sys_clock = devm_kcalloc(
+		dev, bus_cfg->nr_of_link_frequencies + 1 /* guardian */,
+		sizeof(*pdata->op_sys_clock), GFP_KERNEL);
 	if (!pdata->op_sys_clock) {
 		rval = -ENOMEM;
 		goto out_err;
 	}
 
-	asize /= sizeof(*pdata->op_sys_clock);
-	rval = of_property_read_u64_array(
-		ep, "link-frequencies", pdata->op_sys_clock, asize);
-	if (rval) {
-		dev_warn(dev, "can't get link-frequencies\n");
-		goto out_err;
+	for (i = 0; i < bus_cfg->nr_of_link_frequencies; i++) {
+		pdata->op_sys_clock[i] = bus_cfg->link_frequencies[i];
+		dev_dbg(dev, "freq %d: %lld\n", i, pdata->op_sys_clock[i]);
 	}
 
-	for (; asize > 0; asize--)
-		dev_dbg(dev, "freq %d: %lld\n", asize - 1,
-			pdata->op_sys_clock[asize - 1]);
-
+	v4l2_of_free_endpoint(bus_cfg);
 	of_node_put(ep);
 	return pdata;
 
 out_err:
+	v4l2_of_free_endpoint(bus_cfg);
 	of_node_put(ep);
 	return NULL;
 }
diff --git a/drivers/media/i2c/soc_camera/imx074.c b/drivers/media/i2c/soc_camera/imx074.c
index ec89cfa927a2..f68c2352c63c 100644
--- a/drivers/media/i2c/soc_camera/imx074.c
+++ b/drivers/media/i2c/soc_camera/imx074.c
@@ -153,14 +153,24 @@ static int reg_read(struct i2c_client *client, const u16 addr)
 	return buf[0] & 0xff; /* no sign-extension */
 }
 
-static int imx074_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
+static int imx074_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	const struct imx074_datafmt *fmt = imx074_find_datafmt(mf->code);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx074 *priv = to_imx074(client);
+
+	if (format->pad)
+		return -EINVAL;
 
 	dev_dbg(sd->v4l2_dev->dev, "%s(%u)\n", __func__, mf->code);
 
 	if (!fmt) {
+		/* MIPI CSI could have changed the format, double-check */
+		if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			return -EINVAL;
 		mf->code	= imx074_colour_fmts[0].code;
 		mf->colorspace	= imx074_colour_fmts[0].colorspace;
 	}
@@ -169,36 +179,27 @@ static int imx074_try_fmt(struct v4l2_subdev *sd,
 	mf->height	= IMX074_HEIGHT;
 	mf->field	= V4L2_FIELD_NONE;
 
-	return 0;
-}
-
-static int imx074_s_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct imx074 *priv = to_imx074(client);
-
-	dev_dbg(sd->v4l2_dev->dev, "%s(%u)\n", __func__, mf->code);
-
-	/* MIPI CSI could have changed the format, double-check */
-	if (!imx074_find_datafmt(mf->code))
-		return -EINVAL;
-
-	imx074_try_fmt(sd, mf);
-
-	priv->fmt = imx074_find_datafmt(mf->code);
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		priv->fmt = imx074_find_datafmt(mf->code);
+	else
+		cfg->try_fmt = *mf;
 
 	return 0;
 }
 
-static int imx074_g_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
+static int imx074_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct imx074 *priv = to_imx074(client);
 
 	const struct imx074_datafmt *fmt = priv->fmt;
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->code	= fmt->code;
 	mf->colorspace	= fmt->colorspace;
 	mf->width	= IMX074_WIDTH;
@@ -235,13 +236,15 @@ static int imx074_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int imx074_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int imx074_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if ((unsigned int)index >= ARRAY_SIZE(imx074_colour_fmts))
+	if (code->pad ||
+	    (unsigned int)code->index >= ARRAY_SIZE(imx074_colour_fmts))
 		return -EINVAL;
 
-	*code = imx074_colour_fmts[index].code;
+	code->code = imx074_colour_fmts[code->index].code;
 	return 0;
 }
 
@@ -275,10 +278,6 @@ static int imx074_g_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops imx074_subdev_video_ops = {
 	.s_stream	= imx074_s_stream,
-	.s_mbus_fmt	= imx074_s_fmt,
-	.g_mbus_fmt	= imx074_g_fmt,
-	.try_mbus_fmt	= imx074_try_fmt,
-	.enum_mbus_fmt	= imx074_enum_fmt,
 	.g_crop		= imx074_g_crop,
 	.cropcap	= imx074_cropcap,
 	.g_mbus_config	= imx074_g_mbus_config,
@@ -288,9 +287,16 @@ static struct v4l2_subdev_core_ops imx074_subdev_core_ops = {
 	.s_power	= imx074_s_power,
 };
 
+static const struct v4l2_subdev_pad_ops imx074_subdev_pad_ops = {
+	.enum_mbus_code = imx074_enum_mbus_code,
+	.get_fmt	= imx074_get_fmt,
+	.set_fmt	= imx074_set_fmt,
+};
+
 static struct v4l2_subdev_ops imx074_subdev_ops = {
 	.core	= &imx074_subdev_core_ops,
 	.video	= &imx074_subdev_video_ops,
+	.pad	= &imx074_subdev_pad_ops,
 };
 
 static int imx074_video_probe(struct i2c_client *client)
diff --git a/drivers/media/i2c/soc_camera/mt9m001.c b/drivers/media/i2c/soc_camera/mt9m001.c
index 2e9a53502551..4fbdd1e9f7ee 100644
--- a/drivers/media/i2c/soc_camera/mt9m001.c
+++ b/drivers/media/i2c/soc_camera/mt9m001.c
@@ -205,7 +205,7 @@ static int mt9m001_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 
 	/*
 	 * The caller provides a supported format, as verified per
-	 * call to .try_mbus_fmt()
+	 * call to .set_fmt(FORMAT_TRY).
 	 */
 	if (!ret)
 		ret = reg_write(client, MT9M001_COLUMN_START, rect.left);
@@ -250,11 +250,16 @@ static int mt9m001_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int mt9m001_g_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
+static int mt9m001_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9m001 *mt9m001 = to_mt9m001(client);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
 
 	mf->width	= mt9m001->rect.width;
 	mf->height	= mt9m001->rect.height;
@@ -293,13 +298,18 @@ static int mt9m001_s_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int mt9m001_try_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_mbus_framefmt *mf)
+static int mt9m001_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9m001 *mt9m001 = to_mt9m001(client);
 	const struct mt9m001_datafmt *fmt;
 
+	if (format->pad)
+		return -EINVAL;
+
 	v4l_bound_align_image(&mf->width, MT9M001_MIN_WIDTH,
 		MT9M001_MAX_WIDTH, 1,
 		&mf->height, MT9M001_MIN_HEIGHT + mt9m001->y_skip_top,
@@ -317,6 +327,9 @@ static int mt9m001_try_fmt(struct v4l2_subdev *sd,
 
 	mf->colorspace	= fmt->colorspace;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return mt9m001_s_fmt(sd, mf);
+	cfg->try_fmt = *mf;
 	return 0;
 }
 
@@ -562,16 +575,17 @@ static struct v4l2_subdev_core_ops mt9m001_subdev_core_ops = {
 	.s_power	= mt9m001_s_power,
 };
 
-static int mt9m001_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			    u32 *code)
+static int mt9m001_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9m001 *mt9m001 = to_mt9m001(client);
 
-	if (index >= mt9m001->num_fmts)
+	if (code->pad || code->index >= mt9m001->num_fmts)
 		return -EINVAL;
 
-	*code = mt9m001->fmts[index].code;
+	code->code = mt9m001->fmts[code->index].code;
 	return 0;
 }
 
@@ -611,13 +625,9 @@ static int mt9m001_s_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops mt9m001_subdev_video_ops = {
 	.s_stream	= mt9m001_s_stream,
-	.s_mbus_fmt	= mt9m001_s_fmt,
-	.g_mbus_fmt	= mt9m001_g_fmt,
-	.try_mbus_fmt	= mt9m001_try_fmt,
 	.s_crop		= mt9m001_s_crop,
 	.g_crop		= mt9m001_g_crop,
 	.cropcap	= mt9m001_cropcap,
-	.enum_mbus_fmt	= mt9m001_enum_fmt,
 	.g_mbus_config	= mt9m001_g_mbus_config,
 	.s_mbus_config	= mt9m001_s_mbus_config,
 };
@@ -626,10 +636,17 @@ static struct v4l2_subdev_sensor_ops mt9m001_subdev_sensor_ops = {
 	.g_skip_top_lines	= mt9m001_g_skip_top_lines,
 };
 
+static const struct v4l2_subdev_pad_ops mt9m001_subdev_pad_ops = {
+	.enum_mbus_code = mt9m001_enum_mbus_code,
+	.get_fmt	= mt9m001_get_fmt,
+	.set_fmt	= mt9m001_set_fmt,
+};
+
 static struct v4l2_subdev_ops mt9m001_subdev_ops = {
 	.core	= &mt9m001_subdev_core_ops,
 	.video	= &mt9m001_subdev_video_ops,
 	.sensor	= &mt9m001_subdev_sensor_ops,
+	.pad	= &mt9m001_subdev_pad_ops,
 };
 
 static int mt9m001_probe(struct i2c_client *client,
diff --git a/drivers/media/i2c/soc_camera/mt9m111.c b/drivers/media/i2c/soc_camera/mt9m111.c
index 441e0fda24fe..6dfaead6aaa8 100644
--- a/drivers/media/i2c/soc_camera/mt9m111.c
+++ b/drivers/media/i2c/soc_camera/mt9m111.c
@@ -447,11 +447,16 @@ static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int mt9m111_g_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
+static int mt9m111_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->width	= mt9m111->width;
 	mf->height	= mt9m111->height;
 	mf->code	= mt9m111->fmt->code;
@@ -531,14 +536,20 @@ static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
 	return ret;
 }
 
-static int mt9m111_try_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_mbus_framefmt *mf)
+static int mt9m111_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
 	const struct mt9m111_datafmt *fmt;
 	struct v4l2_rect *rect = &mt9m111->rect;
 	bool bayer;
+	int ret;
+
+	if (format->pad)
+		return -EINVAL;
 
 	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
 
@@ -572,20 +583,10 @@ static int mt9m111_try_fmt(struct v4l2_subdev *sd,
 	mf->code = fmt->code;
 	mf->colorspace = fmt->colorspace;
 
-	return 0;
-}
-
-static int mt9m111_s_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
-{
-	const struct mt9m111_datafmt *fmt;
-	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
-	struct v4l2_rect *rect = &mt9m111->rect;
-	int ret;
-
-	mt9m111_try_fmt(sd, mf);
-	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
-	/* try_fmt() guarantees fmt != NULL && fmt->code == mf->code */
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *mf;
+		return 0;
+	}
 
 	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
 	if (!ret)
@@ -839,13 +840,14 @@ static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
 #endif
 };
 
-static int mt9m111_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			    u32 *code)
+static int mt9m111_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(mt9m111_colour_fmts))
+	if (code->pad || code->index >= ARRAY_SIZE(mt9m111_colour_fmts))
 		return -EINVAL;
 
-	*code = mt9m111_colour_fmts[index].code;
+	code->code = mt9m111_colour_fmts[code->index].code;
 	return 0;
 }
 
@@ -865,19 +867,22 @@ static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
 }
 
 static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
-	.s_mbus_fmt	= mt9m111_s_fmt,
-	.g_mbus_fmt	= mt9m111_g_fmt,
-	.try_mbus_fmt	= mt9m111_try_fmt,
 	.s_crop		= mt9m111_s_crop,
 	.g_crop		= mt9m111_g_crop,
 	.cropcap	= mt9m111_cropcap,
-	.enum_mbus_fmt	= mt9m111_enum_fmt,
 	.g_mbus_config	= mt9m111_g_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops mt9m111_subdev_pad_ops = {
+	.enum_mbus_code = mt9m111_enum_mbus_code,
+	.get_fmt	= mt9m111_get_fmt,
+	.set_fmt	= mt9m111_set_fmt,
+};
+
 static struct v4l2_subdev_ops mt9m111_subdev_ops = {
 	.core	= &mt9m111_subdev_core_ops,
 	.video	= &mt9m111_subdev_video_ops,
+	.pad	= &mt9m111_subdev_pad_ops,
 };
 
 /*
diff --git a/drivers/media/i2c/soc_camera/mt9t031.c b/drivers/media/i2c/soc_camera/mt9t031.c
index 35d9c8d25589..3b6eeed2e2b9 100644
--- a/drivers/media/i2c/soc_camera/mt9t031.c
+++ b/drivers/media/i2c/soc_camera/mt9t031.c
@@ -264,7 +264,7 @@ static int mt9t031_set_params(struct i2c_client *client,
 
 	/*
 	 * The caller provides a supported format, as guaranteed by
-	 * .try_mbus_fmt(), soc_camera_s_crop() and soc_camera_cropcap()
+	 * .set_fmt(FORMAT_TRY), soc_camera_s_crop() and soc_camera_cropcap()
 	 */
 	if (ret >= 0)
 		ret = reg_write(client, MT9T031_COLUMN_START, rect->left);
@@ -337,12 +337,17 @@ static int mt9t031_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int mt9t031_g_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
+static int mt9t031_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t031 *mt9t031 = to_mt9t031(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->width	= mt9t031->rect.width / mt9t031->xskip;
 	mf->height	= mt9t031->rect.height / mt9t031->yskip;
 	mf->code	= MEDIA_BUS_FMT_SBGGR10_1X10;
@@ -352,16 +357,36 @@ static int mt9t031_g_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int mt9t031_s_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
+/*
+ * If a user window larger than sensor window is requested, we'll increase the
+ * sensor window.
+ */
+static int mt9t031_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t031 *mt9t031 = to_mt9t031(client);
 	u16 xskip, yskip;
 	struct v4l2_rect rect = mt9t031->rect;
 
+	if (format->pad)
+		return -EINVAL;
+
+	mf->code	= MEDIA_BUS_FMT_SBGGR10_1X10;
+	mf->colorspace	= V4L2_COLORSPACE_SRGB;
+	v4l_bound_align_image(
+			&mf->width, MT9T031_MIN_WIDTH, MT9T031_MAX_WIDTH, 1,
+			&mf->height, MT9T031_MIN_HEIGHT, MT9T031_MAX_HEIGHT, 1, 0);
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *mf;
+		return 0;
+	}
+
 	/*
-	 * try_fmt has put width and height within limits.
+	 * Width and height are within limits.
 	 * S_FMT: use binning and skipping for scaling
 	 */
 	xskip = mt9t031_skip(&rect.width, mf->width, MT9T031_MAX_WIDTH);
@@ -374,23 +399,6 @@ static int mt9t031_s_fmt(struct v4l2_subdev *sd,
 	return mt9t031_set_params(client, &rect, xskip, yskip);
 }
 
-/*
- * If a user window larger than sensor window is requested, we'll increase the
- * sensor window.
- */
-static int mt9t031_try_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_mbus_framefmt *mf)
-{
-	v4l_bound_align_image(
-		&mf->width, MT9T031_MIN_WIDTH, MT9T031_MAX_WIDTH, 1,
-		&mf->height, MT9T031_MIN_HEIGHT, MT9T031_MAX_HEIGHT, 1, 0);
-
-	mf->code	= MEDIA_BUS_FMT_SBGGR10_1X10;
-	mf->colorspace	= V4L2_COLORSPACE_SRGB;
-
-	return 0;
-}
-
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int mt9t031_g_register(struct v4l2_subdev *sd,
 			      struct v4l2_dbg_register *reg)
@@ -672,13 +680,14 @@ static struct v4l2_subdev_core_ops mt9t031_subdev_core_ops = {
 #endif
 };
 
-static int mt9t031_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			    u32 *code)
+static int mt9t031_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index)
+	if (code->pad || code->index)
 		return -EINVAL;
 
-	*code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 	return 0;
 }
 
@@ -712,13 +721,9 @@ static int mt9t031_s_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops mt9t031_subdev_video_ops = {
 	.s_stream	= mt9t031_s_stream,
-	.s_mbus_fmt	= mt9t031_s_fmt,
-	.g_mbus_fmt	= mt9t031_g_fmt,
-	.try_mbus_fmt	= mt9t031_try_fmt,
 	.s_crop		= mt9t031_s_crop,
 	.g_crop		= mt9t031_g_crop,
 	.cropcap	= mt9t031_cropcap,
-	.enum_mbus_fmt	= mt9t031_enum_fmt,
 	.g_mbus_config	= mt9t031_g_mbus_config,
 	.s_mbus_config	= mt9t031_s_mbus_config,
 };
@@ -727,10 +732,17 @@ static struct v4l2_subdev_sensor_ops mt9t031_subdev_sensor_ops = {
 	.g_skip_top_lines	= mt9t031_g_skip_top_lines,
 };
 
+static const struct v4l2_subdev_pad_ops mt9t031_subdev_pad_ops = {
+	.enum_mbus_code = mt9t031_enum_mbus_code,
+	.get_fmt	= mt9t031_get_fmt,
+	.set_fmt	= mt9t031_set_fmt,
+};
+
 static struct v4l2_subdev_ops mt9t031_subdev_ops = {
 	.core	= &mt9t031_subdev_core_ops,
 	.video	= &mt9t031_subdev_video_ops,
 	.sensor	= &mt9t031_subdev_sensor_ops,
+	.pad	= &mt9t031_subdev_pad_ops,
 };
 
 static int mt9t031_probe(struct i2c_client *client,
diff --git a/drivers/media/i2c/soc_camera/mt9t112.c b/drivers/media/i2c/soc_camera/mt9t112.c
index 64f08365e6b2..de10a76ba6df 100644
--- a/drivers/media/i2c/soc_camera/mt9t112.c
+++ b/drivers/media/i2c/soc_camera/mt9t112.c
@@ -904,12 +904,17 @@ static int mt9t112_s_crop(struct v4l2_subdev *sd, const struct v4l2_crop *a)
 	return mt9t112_set_params(priv, rect, priv->format->code);
 }
 
-static int mt9t112_g_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
+static int mt9t112_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t112_priv *priv = to_mt9t112(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->width	= priv->frame.width;
 	mf->height	= priv->frame.height;
 	mf->colorspace	= priv->format->colorspace;
@@ -940,14 +945,19 @@ static int mt9t112_s_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int mt9t112_try_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_mbus_framefmt *mf)
+static int mt9t112_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t112_priv *priv = to_mt9t112(client);
 	unsigned int top, left;
 	int i;
 
+	if (format->pad)
+		return -EINVAL;
+
 	for (i = 0; i < priv->num_formats; i++)
 		if (mt9t112_cfmts[i].code == mf->code)
 			break;
@@ -963,19 +973,23 @@ static int mt9t112_try_fmt(struct v4l2_subdev *sd,
 
 	mf->field = V4L2_FIELD_NONE;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return mt9t112_s_fmt(sd, mf);
+	cfg->try_fmt = *mf;
 	return 0;
 }
 
-static int mt9t112_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int mt9t112_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9t112_priv *priv = to_mt9t112(client);
 
-	if (index >= priv->num_formats)
+	if (code->pad || code->index >= priv->num_formats)
 		return -EINVAL;
 
-	*code = mt9t112_cfmts[index].code;
+	code->code = mt9t112_cfmts[code->index].code;
 
 	return 0;
 }
@@ -1010,23 +1024,26 @@ static int mt9t112_s_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops mt9t112_subdev_video_ops = {
 	.s_stream	= mt9t112_s_stream,
-	.g_mbus_fmt	= mt9t112_g_fmt,
-	.s_mbus_fmt	= mt9t112_s_fmt,
-	.try_mbus_fmt	= mt9t112_try_fmt,
 	.cropcap	= mt9t112_cropcap,
 	.g_crop		= mt9t112_g_crop,
 	.s_crop		= mt9t112_s_crop,
-	.enum_mbus_fmt	= mt9t112_enum_fmt,
 	.g_mbus_config	= mt9t112_g_mbus_config,
 	.s_mbus_config	= mt9t112_s_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops mt9t112_subdev_pad_ops = {
+	.enum_mbus_code = mt9t112_enum_mbus_code,
+	.get_fmt	= mt9t112_get_fmt,
+	.set_fmt	= mt9t112_set_fmt,
+};
+
 /************************************************************************
 			i2c driver
 ************************************************************************/
 static struct v4l2_subdev_ops mt9t112_subdev_ops = {
 	.core	= &mt9t112_subdev_core_ops,
 	.video	= &mt9t112_subdev_video_ops,
+	.pad	= &mt9t112_subdev_pad_ops,
 };
 
 static int mt9t112_camera_probe(struct i2c_client *client)
diff --git a/drivers/media/i2c/soc_camera/mt9v022.c b/drivers/media/i2c/soc_camera/mt9v022.c
index a246d4d64b8b..f31377408550 100644
--- a/drivers/media/i2c/soc_camera/mt9v022.c
+++ b/drivers/media/i2c/soc_camera/mt9v022.c
@@ -375,12 +375,17 @@ static int mt9v022_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int mt9v022_g_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
+static int mt9v022_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->width	= mt9v022->rect.width;
 	mf->height	= mt9v022->rect.height;
 	mf->code	= mt9v022->fmt->code;
@@ -407,7 +412,7 @@ static int mt9v022_s_fmt(struct v4l2_subdev *sd,
 
 	/*
 	 * The caller provides a supported format, as verified per call to
-	 * .try_mbus_fmt(), datawidth is from our supported format list
+	 * .set_fmt(FORMAT_TRY), datawidth is from our supported format list
 	 */
 	switch (mf->code) {
 	case MEDIA_BUS_FMT_Y8_1X8:
@@ -437,15 +442,20 @@ static int mt9v022_s_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int mt9v022_try_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_mbus_framefmt *mf)
+static int mt9v022_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
 	const struct mt9v022_datafmt *fmt;
 	int align = mf->code == MEDIA_BUS_FMT_SBGGR8_1X8 ||
 		mf->code == MEDIA_BUS_FMT_SBGGR10_1X10;
 
+	if (format->pad)
+		return -EINVAL;
+
 	v4l_bound_align_image(&mf->width, MT9V022_MIN_WIDTH,
 		MT9V022_MAX_WIDTH, align,
 		&mf->height, MT9V022_MIN_HEIGHT + mt9v022->y_skip_top,
@@ -460,6 +470,9 @@ static int mt9v022_try_fmt(struct v4l2_subdev *sd,
 
 	mf->colorspace	= fmt->colorspace;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return mt9v022_s_fmt(sd, mf);
+	cfg->try_fmt = *mf;
 	return 0;
 }
 
@@ -758,16 +771,17 @@ static struct v4l2_subdev_core_ops mt9v022_subdev_core_ops = {
 	.s_power	= mt9v022_s_power,
 };
 
-static int mt9v022_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			    u32 *code)
+static int mt9v022_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
 
-	if (index >= mt9v022->num_fmts)
+	if (code->pad || code->index >= mt9v022->num_fmts)
 		return -EINVAL;
 
-	*code = mt9v022->fmts[index].code;
+	code->code = mt9v022->fmts[code->index].code;
 	return 0;
 }
 
@@ -839,13 +853,9 @@ static int mt9v022_s_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops mt9v022_subdev_video_ops = {
 	.s_stream	= mt9v022_s_stream,
-	.s_mbus_fmt	= mt9v022_s_fmt,
-	.g_mbus_fmt	= mt9v022_g_fmt,
-	.try_mbus_fmt	= mt9v022_try_fmt,
 	.s_crop		= mt9v022_s_crop,
 	.g_crop		= mt9v022_g_crop,
 	.cropcap	= mt9v022_cropcap,
-	.enum_mbus_fmt	= mt9v022_enum_fmt,
 	.g_mbus_config	= mt9v022_g_mbus_config,
 	.s_mbus_config	= mt9v022_s_mbus_config,
 };
@@ -854,10 +864,17 @@ static struct v4l2_subdev_sensor_ops mt9v022_subdev_sensor_ops = {
 	.g_skip_top_lines	= mt9v022_g_skip_top_lines,
 };
 
+static const struct v4l2_subdev_pad_ops mt9v022_subdev_pad_ops = {
+	.enum_mbus_code = mt9v022_enum_mbus_code,
+	.get_fmt	= mt9v022_get_fmt,
+	.set_fmt	= mt9v022_set_fmt,
+};
+
 static struct v4l2_subdev_ops mt9v022_subdev_ops = {
 	.core	= &mt9v022_subdev_core_ops,
 	.video	= &mt9v022_subdev_video_ops,
 	.sensor	= &mt9v022_subdev_sensor_ops,
+	.pad	= &mt9v022_subdev_pad_ops,
 };
 
 static int mt9v022_probe(struct i2c_client *client,
diff --git a/drivers/media/i2c/soc_camera/ov2640.c b/drivers/media/i2c/soc_camera/ov2640.c
index e3c907a97765..9b4f5deec748 100644
--- a/drivers/media/i2c/soc_camera/ov2640.c
+++ b/drivers/media/i2c/soc_camera/ov2640.c
@@ -845,12 +845,17 @@ err:
 	return ret;
 }
 
-static int ov2640_g_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
+static int ov2640_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client  *client = v4l2_get_subdevdata(sd);
 	struct ov2640_priv *priv = to_ov2640(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	if (!priv->win) {
 		u32 width = SVGA_WIDTH, height = SVGA_HEIGHT;
 		priv->win = ov2640_select_win(&width, &height);
@@ -876,33 +881,16 @@ static int ov2640_g_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov2640_s_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
+static int ov2640_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	int ret;
 
+	if (format->pad)
+		return -EINVAL;
 
-	switch (mf->code) {
-	case MEDIA_BUS_FMT_RGB565_2X8_BE:
-	case MEDIA_BUS_FMT_RGB565_2X8_LE:
-		mf->colorspace = V4L2_COLORSPACE_SRGB;
-		break;
-	default:
-		mf->code = MEDIA_BUS_FMT_UYVY8_2X8;
-	case MEDIA_BUS_FMT_YUYV8_2X8:
-	case MEDIA_BUS_FMT_UYVY8_2X8:
-		mf->colorspace = V4L2_COLORSPACE_JPEG;
-	}
-
-	ret = ov2640_set_params(client, &mf->width, &mf->height, mf->code);
-
-	return ret;
-}
-
-static int ov2640_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
-{
 	/*
 	 * select suitable win, but don't store it
 	 */
@@ -922,16 +910,21 @@ static int ov2640_try_fmt(struct v4l2_subdev *sd,
 		mf->colorspace = V4L2_COLORSPACE_JPEG;
 	}
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return ov2640_set_params(client, &mf->width,
+					 &mf->height, mf->code);
+	cfg->try_fmt = *mf;
 	return 0;
 }
 
-static int ov2640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int ov2640_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(ov2640_codes))
+	if (code->pad || code->index >= ARRAY_SIZE(ov2640_codes))
 		return -EINVAL;
 
-	*code = ov2640_codes[index];
+	code->code = ov2640_codes[code->index];
 	return 0;
 }
 
@@ -1031,18 +1024,21 @@ static int ov2640_g_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops ov2640_subdev_video_ops = {
 	.s_stream	= ov2640_s_stream,
-	.g_mbus_fmt	= ov2640_g_fmt,
-	.s_mbus_fmt	= ov2640_s_fmt,
-	.try_mbus_fmt	= ov2640_try_fmt,
 	.cropcap	= ov2640_cropcap,
 	.g_crop		= ov2640_g_crop,
-	.enum_mbus_fmt	= ov2640_enum_fmt,
 	.g_mbus_config	= ov2640_g_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops ov2640_subdev_pad_ops = {
+	.enum_mbus_code = ov2640_enum_mbus_code,
+	.get_fmt	= ov2640_get_fmt,
+	.set_fmt	= ov2640_set_fmt,
+};
+
 static struct v4l2_subdev_ops ov2640_subdev_ops = {
 	.core	= &ov2640_subdev_core_ops,
 	.video	= &ov2640_subdev_video_ops,
+	.pad	= &ov2640_subdev_pad_ops,
 };
 
 /* OF probe functions */
diff --git a/drivers/media/i2c/soc_camera/ov5642.c b/drivers/media/i2c/soc_camera/ov5642.c
index 93ae031bdafb..bab9ac0c1764 100644
--- a/drivers/media/i2c/soc_camera/ov5642.c
+++ b/drivers/media/i2c/soc_camera/ov5642.c
@@ -786,50 +786,50 @@ static int ov5642_set_resolution(struct v4l2_subdev *sd)
 	return ret;
 }
 
-static int ov5642_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
+static int ov5642_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov5642 *priv = to_ov5642(client);
 	const struct ov5642_datafmt *fmt = ov5642_find_datafmt(mf->code);
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->width = priv->crop_rect.width;
 	mf->height = priv->crop_rect.height;
 
 	if (!fmt) {
+		if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+			return -EINVAL;
 		mf->code	= ov5642_colour_fmts[0].code;
 		mf->colorspace	= ov5642_colour_fmts[0].colorspace;
 	}
 
 	mf->field	= V4L2_FIELD_NONE;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		priv->fmt = ov5642_find_datafmt(mf->code);
+	else
+		cfg->try_fmt = *mf;
 	return 0;
 }
 
-static int ov5642_s_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct ov5642 *priv = to_ov5642(client);
-
-	/* MIPI CSI could have changed the format, double-check */
-	if (!ov5642_find_datafmt(mf->code))
-		return -EINVAL;
-
-	ov5642_try_fmt(sd, mf);
-	priv->fmt = ov5642_find_datafmt(mf->code);
-
-	return 0;
-}
-
-static int ov5642_g_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
+static int ov5642_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov5642 *priv = to_ov5642(client);
 
 	const struct ov5642_datafmt *fmt = priv->fmt;
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->code	= fmt->code;
 	mf->colorspace	= fmt->colorspace;
 	mf->width	= priv->crop_rect.width;
@@ -839,13 +839,14 @@ static int ov5642_g_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov5642_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int ov5642_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(ov5642_colour_fmts))
+	if (code->pad || code->index >= ARRAY_SIZE(ov5642_colour_fmts))
 		return -EINVAL;
 
-	*code = ov5642_colour_fmts[index].code;
+	code->code = ov5642_colour_fmts[code->index].code;
 	return 0;
 }
 
@@ -939,16 +940,18 @@ static int ov5642_s_power(struct v4l2_subdev *sd, int on)
 }
 
 static struct v4l2_subdev_video_ops ov5642_subdev_video_ops = {
-	.s_mbus_fmt	= ov5642_s_fmt,
-	.g_mbus_fmt	= ov5642_g_fmt,
-	.try_mbus_fmt	= ov5642_try_fmt,
-	.enum_mbus_fmt	= ov5642_enum_fmt,
 	.s_crop		= ov5642_s_crop,
 	.g_crop		= ov5642_g_crop,
 	.cropcap	= ov5642_cropcap,
 	.g_mbus_config	= ov5642_g_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops ov5642_subdev_pad_ops = {
+	.enum_mbus_code = ov5642_enum_mbus_code,
+	.get_fmt	= ov5642_get_fmt,
+	.set_fmt	= ov5642_set_fmt,
+};
+
 static struct v4l2_subdev_core_ops ov5642_subdev_core_ops = {
 	.s_power	= ov5642_s_power,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -960,6 +963,7 @@ static struct v4l2_subdev_core_ops ov5642_subdev_core_ops = {
 static struct v4l2_subdev_ops ov5642_subdev_ops = {
 	.core	= &ov5642_subdev_core_ops,
 	.video	= &ov5642_subdev_video_ops,
+	.pad	= &ov5642_subdev_pad_ops,
 };
 
 static int ov5642_video_probe(struct i2c_client *client)
diff --git a/drivers/media/i2c/soc_camera/ov6650.c b/drivers/media/i2c/soc_camera/ov6650.c
index f4eef2fa6f6f..1f8af1ee8352 100644
--- a/drivers/media/i2c/soc_camera/ov6650.c
+++ b/drivers/media/i2c/soc_camera/ov6650.c
@@ -499,12 +499,17 @@ static int ov6650_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int ov6650_g_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
+static int ov6650_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov6650 *priv = to_ov6650(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->width	= priv->rect.width >> priv->half_scale;
 	mf->height	= priv->rect.height >> priv->half_scale;
 	mf->code	= priv->code;
@@ -680,16 +685,20 @@ static int ov6650_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 		mf->width = priv->rect.width >> half_scale;
 		mf->height = priv->rect.height >> half_scale;
 	}
-
 	return ret;
 }
 
-static int ov6650_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
+static int ov6650_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov6650 *priv = to_ov6650(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	if (is_unscaled_ok(mf->width, mf->height, &priv->rect))
 		v4l_bound_align_image(&mf->width, 2, W_CIF, 1,
 				&mf->height, 2, H_CIF, 1, 0);
@@ -713,16 +722,21 @@ static int ov6650_try_fmt(struct v4l2_subdev *sd,
 		break;
 	}
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return ov6650_s_fmt(sd, mf);
+	cfg->try_fmt = *mf;
+
 	return 0;
 }
 
-static int ov6650_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int ov6650_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(ov6650_codes))
+	if (code->pad || code->index >= ARRAY_SIZE(ov6650_codes))
 		return -EINVAL;
 
-	*code = ov6650_codes[index];
+	code->code = ov6650_codes[code->index];
 	return 0;
 }
 
@@ -929,10 +943,6 @@ static int ov6650_s_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops ov6650_video_ops = {
 	.s_stream	= ov6650_s_stream,
-	.g_mbus_fmt	= ov6650_g_fmt,
-	.s_mbus_fmt	= ov6650_s_fmt,
-	.try_mbus_fmt	= ov6650_try_fmt,
-	.enum_mbus_fmt	= ov6650_enum_fmt,
 	.cropcap	= ov6650_cropcap,
 	.g_crop		= ov6650_g_crop,
 	.s_crop		= ov6650_s_crop,
@@ -942,9 +952,16 @@ static struct v4l2_subdev_video_ops ov6650_video_ops = {
 	.s_mbus_config	= ov6650_s_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops ov6650_pad_ops = {
+	.enum_mbus_code = ov6650_enum_mbus_code,
+	.get_fmt	= ov6650_get_fmt,
+	.set_fmt	= ov6650_set_fmt,
+};
+
 static struct v4l2_subdev_ops ov6650_subdev_ops = {
 	.core	= &ov6650_core_ops,
 	.video	= &ov6650_video_ops,
+	.pad	= &ov6650_pad_ops,
 };
 
 /*
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
index 8daac88b33fe..f150a8bd94dc 100644
--- a/drivers/media/i2c/soc_camera/ov772x.c
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -876,11 +876,16 @@ static int ov772x_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int ov772x_g_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
+static int ov772x_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct ov772x_priv *priv = to_ov772x(sd);
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->width	= priv->win->rect.width;
 	mf->height	= priv->win->rect.height;
 	mf->code	= priv->cfmt->code;
@@ -915,12 +920,17 @@ static int ov772x_s_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
 	return 0;
 }
 
-static int ov772x_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
+static int ov772x_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	const struct ov772x_color_format *cfmt;
 	const struct ov772x_win_size *win;
 
+	if (format->pad)
+		return -EINVAL;
+
 	ov772x_select_params(mf, &cfmt, &win);
 
 	mf->code = cfmt->code;
@@ -929,6 +939,9 @@ static int ov772x_try_fmt(struct v4l2_subdev *sd,
 	mf->field = V4L2_FIELD_NONE;
 	mf->colorspace = cfmt->colorspace;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return ov772x_s_fmt(sd, mf);
+	cfg->try_fmt = *mf;
 	return 0;
 }
 
@@ -989,13 +1002,14 @@ static struct v4l2_subdev_core_ops ov772x_subdev_core_ops = {
 	.s_power	= ov772x_s_power,
 };
 
-static int ov772x_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int ov772x_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(ov772x_cfmts))
+	if (code->pad || code->index >= ARRAY_SIZE(ov772x_cfmts))
 		return -EINVAL;
 
-	*code = ov772x_cfmts[index].code;
+	code->code = ov772x_cfmts[code->index].code;
 	return 0;
 }
 
@@ -1016,18 +1030,21 @@ static int ov772x_g_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops ov772x_subdev_video_ops = {
 	.s_stream	= ov772x_s_stream,
-	.g_mbus_fmt	= ov772x_g_fmt,
-	.s_mbus_fmt	= ov772x_s_fmt,
-	.try_mbus_fmt	= ov772x_try_fmt,
 	.cropcap	= ov772x_cropcap,
 	.g_crop		= ov772x_g_crop,
-	.enum_mbus_fmt	= ov772x_enum_fmt,
 	.g_mbus_config	= ov772x_g_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops ov772x_subdev_pad_ops = {
+	.enum_mbus_code = ov772x_enum_mbus_code,
+	.get_fmt	= ov772x_get_fmt,
+	.set_fmt	= ov772x_set_fmt,
+};
+
 static struct v4l2_subdev_ops ov772x_subdev_ops = {
 	.core	= &ov772x_subdev_core_ops,
 	.video	= &ov772x_subdev_video_ops,
+	.pad	= &ov772x_subdev_pad_ops,
 };
 
 /*
diff --git a/drivers/media/i2c/soc_camera/ov9640.c b/drivers/media/i2c/soc_camera/ov9640.c
index aa93d2e88572..8caae1c07541 100644
--- a/drivers/media/i2c/soc_camera/ov9640.c
+++ b/drivers/media/i2c/soc_camera/ov9640.c
@@ -519,9 +519,15 @@ static int ov9640_s_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int ov9640_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
+static int ov9640_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+
 	ov9640_res_roundup(&mf->width, &mf->height);
 
 	mf->field = V4L2_FIELD_NONE;
@@ -537,16 +543,21 @@ static int ov9640_try_fmt(struct v4l2_subdev *sd,
 		mf->colorspace = V4L2_COLORSPACE_JPEG;
 	}
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return ov9640_s_fmt(sd, mf);
+
+	cfg->try_fmt = *mf;
 	return 0;
 }
 
-static int ov9640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int ov9640_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(ov9640_codes))
+	if (code->pad || code->index >= ARRAY_SIZE(ov9640_codes))
 		return -EINVAL;
 
-	*code = ov9640_codes[index];
+	code->code = ov9640_codes[code->index];
 	return 0;
 }
 
@@ -656,17 +667,20 @@ static int ov9640_g_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops ov9640_video_ops = {
 	.s_stream	= ov9640_s_stream,
-	.s_mbus_fmt	= ov9640_s_fmt,
-	.try_mbus_fmt	= ov9640_try_fmt,
-	.enum_mbus_fmt	= ov9640_enum_fmt,
 	.cropcap	= ov9640_cropcap,
 	.g_crop		= ov9640_g_crop,
 	.g_mbus_config	= ov9640_g_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops ov9640_pad_ops = {
+	.enum_mbus_code = ov9640_enum_mbus_code,
+	.set_fmt	= ov9640_set_fmt,
+};
+
 static struct v4l2_subdev_ops ov9640_subdev_ops = {
 	.core	= &ov9640_core_ops,
 	.video	= &ov9640_video_ops,
+	.pad	= &ov9640_pad_ops,
 };
 
 /*
diff --git a/drivers/media/i2c/soc_camera/ov9740.c b/drivers/media/i2c/soc_camera/ov9740.c
index 841dc55457cf..03a7fc7316ae 100644
--- a/drivers/media/i2c/soc_camera/ov9740.c
+++ b/drivers/media/i2c/soc_camera/ov9740.c
@@ -704,25 +704,35 @@ static int ov9740_s_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int ov9740_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
+static int ov9740_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
+
+	if (format->pad)
+		return -EINVAL;
+
 	ov9740_res_roundup(&mf->width, &mf->height);
 
 	mf->field = V4L2_FIELD_NONE;
 	mf->code = MEDIA_BUS_FMT_YUYV8_2X8;
 	mf->colorspace = V4L2_COLORSPACE_SRGB;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return ov9740_s_fmt(sd, mf);
+	cfg->try_fmt = *mf;
 	return 0;
 }
 
-static int ov9740_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int ov9740_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(ov9740_codes))
+	if (code->pad || code->index >= ARRAY_SIZE(ov9740_codes))
 		return -EINVAL;
 
-	*code = ov9740_codes[index];
+	code->code = ov9740_codes[code->index];
 
 	return 0;
 }
@@ -904,9 +914,6 @@ static int ov9740_g_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops ov9740_video_ops = {
 	.s_stream	= ov9740_s_stream,
-	.s_mbus_fmt	= ov9740_s_fmt,
-	.try_mbus_fmt	= ov9740_try_fmt,
-	.enum_mbus_fmt	= ov9740_enum_fmt,
 	.cropcap	= ov9740_cropcap,
 	.g_crop		= ov9740_g_crop,
 	.g_mbus_config	= ov9740_g_mbus_config,
@@ -920,9 +927,15 @@ static struct v4l2_subdev_core_ops ov9740_core_ops = {
 #endif
 };
 
+static const struct v4l2_subdev_pad_ops ov9740_pad_ops = {
+	.enum_mbus_code = ov9740_enum_mbus_code,
+	.set_fmt	= ov9740_set_fmt,
+};
+
 static struct v4l2_subdev_ops ov9740_subdev_ops = {
-	.core			= &ov9740_core_ops,
-	.video			= &ov9740_video_ops,
+	.core	= &ov9740_core_ops,
+	.video	= &ov9740_video_ops,
+	.pad	= &ov9740_pad_ops,
 };
 
 static const struct v4l2_ctrl_ops ov9740_ctrl_ops = {
diff --git a/drivers/media/i2c/soc_camera/rj54n1cb0c.c b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
index 1752428c43c5..c769cf663f84 100644
--- a/drivers/media/i2c/soc_camera/rj54n1cb0c.c
+++ b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
@@ -485,13 +485,14 @@ static int reg_write_multiple(struct i2c_client *client,
 	return 0;
 }
 
-static int rj54n1_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int rj54n1_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(rj54n1_colour_fmts))
+	if (code->pad || code->index >= ARRAY_SIZE(rj54n1_colour_fmts))
 		return -EINVAL;
 
-	*code = rj54n1_colour_fmts[index].code;
+	code->code = rj54n1_colour_fmts[code->index].code;
 	return 0;
 }
 
@@ -597,12 +598,17 @@ static int rj54n1_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int rj54n1_g_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
+static int rj54n1_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct rj54n1 *rj54n1 = to_rj54n1(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	mf->code	= rj54n1->fmt->code;
 	mf->colorspace	= rj54n1->fmt->colorspace;
 	mf->field	= V4L2_FIELD_NONE;
@@ -959,17 +965,25 @@ static int rj54n1_reg_init(struct i2c_client *client)
 	return ret;
 }
 
-static int rj54n1_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
+static int rj54n1_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct rj54n1 *rj54n1 = to_rj54n1(client);
 	const struct rj54n1_datafmt *fmt;
+	int output_w, output_h, max_w, max_h,
+		input_w = rj54n1->rect.width, input_h = rj54n1->rect.height;
 	int align = mf->code == MEDIA_BUS_FMT_SBGGR10_1X10 ||
 		mf->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_BE ||
 		mf->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_BE ||
 		mf->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE ||
 		mf->code == MEDIA_BUS_FMT_SBGGR10_2X8_PADLO_LE;
+	int ret;
+
+	if (format->pad)
+		return -EINVAL;
 
 	dev_dbg(&client->dev, "%s: code = %d, width = %u, height = %u\n",
 		__func__, mf->code, mf->width, mf->height);
@@ -987,24 +1001,10 @@ static int rj54n1_try_fmt(struct v4l2_subdev *sd,
 	v4l_bound_align_image(&mf->width, 112, RJ54N1_MAX_WIDTH, align,
 			      &mf->height, 84, RJ54N1_MAX_HEIGHT, align, 0);
 
-	return 0;
-}
-
-static int rj54n1_s_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct rj54n1 *rj54n1 = to_rj54n1(client);
-	const struct rj54n1_datafmt *fmt;
-	int output_w, output_h, max_w, max_h,
-		input_w = rj54n1->rect.width, input_h = rj54n1->rect.height;
-	int ret;
-
-	/*
-	 * The host driver can call us without .try_fmt(), so, we have to take
-	 * care ourseleves
-	 */
-	rj54n1_try_fmt(sd, mf);
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *mf;
+		return 0;
+	}
 
 	/*
 	 * Verify if the sensor has just been powered on. TODO: replace this
@@ -1020,9 +1020,6 @@ static int rj54n1_s_fmt(struct v4l2_subdev *sd,
 			return ret;
 	}
 
-	dev_dbg(&client->dev, "%s: code = %d, width = %u, height = %u\n",
-		__func__, mf->code, mf->width, mf->height);
-
 	/* RA_SEL_UL is only relevant for raw modes, ignored otherwise. */
 	switch (mf->code) {
 	case MEDIA_BUS_FMT_YUYV8_2X8:
@@ -1249,10 +1246,6 @@ static int rj54n1_s_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops rj54n1_subdev_video_ops = {
 	.s_stream	= rj54n1_s_stream,
-	.s_mbus_fmt	= rj54n1_s_fmt,
-	.g_mbus_fmt	= rj54n1_g_fmt,
-	.try_mbus_fmt	= rj54n1_try_fmt,
-	.enum_mbus_fmt	= rj54n1_enum_fmt,
 	.g_crop		= rj54n1_g_crop,
 	.s_crop		= rj54n1_s_crop,
 	.cropcap	= rj54n1_cropcap,
@@ -1260,9 +1253,16 @@ static struct v4l2_subdev_video_ops rj54n1_subdev_video_ops = {
 	.s_mbus_config	= rj54n1_s_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops rj54n1_subdev_pad_ops = {
+	.enum_mbus_code = rj54n1_enum_mbus_code,
+	.get_fmt	= rj54n1_get_fmt,
+	.set_fmt	= rj54n1_set_fmt,
+};
+
 static struct v4l2_subdev_ops rj54n1_subdev_ops = {
 	.core	= &rj54n1_subdev_core_ops,
 	.video	= &rj54n1_subdev_video_ops,
+	.pad	= &rj54n1_subdev_pad_ops,
 };
 
 /*
diff --git a/drivers/media/i2c/soc_camera/tw9910.c b/drivers/media/i2c/soc_camera/tw9910.c
index 9b853215d146..42bec9bf1892 100644
--- a/drivers/media/i2c/soc_camera/tw9910.c
+++ b/drivers/media/i2c/soc_camera/tw9910.c
@@ -691,12 +691,17 @@ static int tw9910_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
 	return 0;
 }
 
-static int tw9910_g_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *mf)
+static int tw9910_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct tw9910_priv *priv = to_tw9910(client);
 
+	if (format->pad)
+		return -EINVAL;
+
 	if (!priv->scale) {
 		priv->scale = tw9910_select_norm(priv->norm, 640, 480);
 		if (!priv->scale)
@@ -737,13 +742,18 @@ static int tw9910_s_fmt(struct v4l2_subdev *sd,
 	return ret;
 }
 
-static int tw9910_try_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_mbus_framefmt *mf)
+static int tw9910_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct tw9910_priv *priv = to_tw9910(client);
 	const struct tw9910_scale_ctrl *scale;
 
+	if (format->pad)
+		return -EINVAL;
+
 	if (V4L2_FIELD_ANY == mf->field) {
 		mf->field = V4L2_FIELD_INTERLACED_BT;
 	} else if (V4L2_FIELD_INTERLACED_BT != mf->field) {
@@ -764,6 +774,9 @@ static int tw9910_try_fmt(struct v4l2_subdev *sd,
 	mf->width	= scale->width;
 	mf->height	= scale->height;
 
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return tw9910_s_fmt(sd, mf);
+	cfg->try_fmt = *mf;
 	return 0;
 }
 
@@ -821,13 +834,14 @@ static struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
 	.s_power	= tw9910_s_power,
 };
 
-static int tw9910_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			   u32 *code)
+static int tw9910_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index)
+	if (code->pad || code->index)
 		return -EINVAL;
 
-	*code = MEDIA_BUS_FMT_UYVY8_2X8;
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
 	return 0;
 }
 
@@ -880,20 +894,23 @@ static struct v4l2_subdev_video_ops tw9910_subdev_video_ops = {
 	.s_std		= tw9910_s_std,
 	.g_std		= tw9910_g_std,
 	.s_stream	= tw9910_s_stream,
-	.g_mbus_fmt	= tw9910_g_fmt,
-	.s_mbus_fmt	= tw9910_s_fmt,
-	.try_mbus_fmt	= tw9910_try_fmt,
 	.cropcap	= tw9910_cropcap,
 	.g_crop		= tw9910_g_crop,
-	.enum_mbus_fmt	= tw9910_enum_fmt,
 	.g_mbus_config	= tw9910_g_mbus_config,
 	.s_mbus_config	= tw9910_s_mbus_config,
 	.g_tvnorms	= tw9910_g_tvnorms,
 };
 
+static const struct v4l2_subdev_pad_ops tw9910_subdev_pad_ops = {
+	.enum_mbus_code = tw9910_enum_mbus_code,
+	.get_fmt	= tw9910_get_fmt,
+	.set_fmt	= tw9910_set_fmt,
+};
+
 static struct v4l2_subdev_ops tw9910_subdev_ops = {
 	.core	= &tw9910_subdev_core_ops,
 	.video	= &tw9910_subdev_video_ops,
+	.pad	= &tw9910_subdev_pad_ops,
 };
 
 /*
diff --git a/drivers/media/i2c/sr030pc30.c b/drivers/media/i2c/sr030pc30.c
index 10c735c3a082..b62b6ddc4356 100644
--- a/drivers/media/i2c/sr030pc30.c
+++ b/drivers/media/i2c/sr030pc30.c
@@ -471,25 +471,31 @@ static int sr030pc30_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static int sr030pc30_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-			      u32 *code)
+static int sr030pc30_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (!code || index >= ARRAY_SIZE(sr030pc30_formats))
+	if (!code || code->pad ||
+	    code->index >= ARRAY_SIZE(sr030pc30_formats))
 		return -EINVAL;
 
-	*code = sr030pc30_formats[index].code;
+	code->code = sr030pc30_formats[code->index].code;
 	return 0;
 }
 
-static int sr030pc30_g_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_mbus_framefmt *mf)
+static int sr030pc30_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *mf;
 	struct sr030pc30_info *info = to_sr030pc30(sd);
 	int ret;
 
-	if (!mf)
+	if (!format || format->pad)
 		return -EINVAL;
 
+	mf = &format->format;
+
 	if (!info->curr_win || !info->curr_fmt) {
 		ret = sr030pc30_set_params(sd);
 		if (ret)
@@ -523,25 +529,28 @@ static const struct sr030pc30_format *try_fmt(struct v4l2_subdev *sd,
 }
 
 /* Return nearest media bus frame format. */
-static int sr030pc30_try_fmt(struct v4l2_subdev *sd,
-			     struct v4l2_mbus_framefmt *mf)
+static int sr030pc30_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
-	if (!sd || !mf)
-		return -EINVAL;
-
-	try_fmt(sd, mf);
-	return 0;
-}
+	struct sr030pc30_info *info = sd ? to_sr030pc30(sd) : NULL;
+	const struct sr030pc30_format *fmt;
+	struct v4l2_mbus_framefmt *mf;
 
-static int sr030pc30_s_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_mbus_framefmt *mf)
-{
-	struct sr030pc30_info *info = to_sr030pc30(sd);
+	if (!sd || !format)
+		return -EINVAL;
 
-	if (!sd || !mf)
+	mf = &format->format;
+	if (format->pad)
 		return -EINVAL;
 
-	info->curr_fmt = try_fmt(sd, mf);
+	fmt = try_fmt(sd, mf);
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *mf;
+		return 0;
+	}
+
+	info->curr_fmt = fmt;
 
 	return sr030pc30_set_params(sd);
 }
@@ -636,16 +645,15 @@ static const struct v4l2_subdev_core_ops sr030pc30_core_ops = {
 	.querymenu = v4l2_subdev_querymenu,
 };
 
-static const struct v4l2_subdev_video_ops sr030pc30_video_ops = {
-	.g_mbus_fmt	= sr030pc30_g_fmt,
-	.s_mbus_fmt	= sr030pc30_s_fmt,
-	.try_mbus_fmt	= sr030pc30_try_fmt,
-	.enum_mbus_fmt	= sr030pc30_enum_fmt,
+static const struct v4l2_subdev_pad_ops sr030pc30_pad_ops = {
+	.enum_mbus_code = sr030pc30_enum_mbus_code,
+	.get_fmt	= sr030pc30_get_fmt,
+	.set_fmt	= sr030pc30_set_fmt,
 };
 
 static const struct v4l2_subdev_ops sr030pc30_ops = {
 	.core	= &sr030pc30_core_ops,
-	.video	= &sr030pc30_video_ops,
+	.pad	= &sr030pc30_pad_ops,
 };
 
 /*
diff --git a/drivers/media/i2c/tvaudio.c b/drivers/media/i2c/tvaudio.c
index 070c152da95a..0c50e5285cf6 100644
--- a/drivers/media/i2c/tvaudio.c
+++ b/drivers/media/i2c/tvaudio.c
@@ -272,7 +272,7 @@ static int chip_cmd(struct CHIPSTATE *chip, char *name, audiocmd *cmd)
 		return -EINVAL;
 	}
 
-	/* FIXME: it seems that the shadow bytes are wrong bellow !*/
+	/* FIXME: it seems that the shadow bytes are wrong below !*/
 
 	/* update our shadow register set; print bytes if (debug > 0) */
 	v4l2_dbg(1, debug, sd, "chip_cmd(%s): reg=%d, data:",
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index 1c6bc306ecdc..24e47279e30c 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -747,54 +747,6 @@ static int tvp514x_s_ctrl(struct v4l2_ctrl *ctrl)
 }
 
 /**
- * tvp514x_enum_mbus_fmt() - V4L2 decoder interface handler for enum_mbus_fmt
- * @sd: pointer to standard V4L2 sub-device structure
- * @index: index of pixelcode to retrieve
- * @code: receives the pixelcode
- *
- * Enumerates supported mediabus formats
- */
-static int
-tvp514x_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
-					u32 *code)
-{
-	if (index)
-		return -EINVAL;
-
-	*code = MEDIA_BUS_FMT_YUYV10_2X10;
-	return 0;
-}
-
-/**
- * tvp514x_mbus_fmt() - V4L2 decoder interface handler for try/s/g_mbus_fmt
- * @sd: pointer to standard V4L2 sub-device structure
- * @f: pointer to the mediabus format structure
- *
- * Negotiates the image capture size and mediabus format.
- */
-static int
-tvp514x_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
-{
-	struct tvp514x_decoder *decoder = to_decoder(sd);
-	enum tvp514x_std current_std;
-
-	if (f == NULL)
-		return -EINVAL;
-
-	/* Calculate height and width based on current standard */
-	current_std = decoder->current_std;
-
-	f->code = MEDIA_BUS_FMT_YUYV8_2X8;
-	f->width = decoder->std_list[current_std].width;
-	f->height = decoder->std_list[current_std].height;
-	f->field = V4L2_FIELD_INTERLACED;
-	f->colorspace = V4L2_COLORSPACE_SMPTE170M;
-	v4l2_dbg(1, debug, sd, "MBUS_FMT: Width - %d, Height - %d\n",
-			f->width, f->height);
-	return 0;
-}
-
-/**
  * tvp514x_g_parm() - V4L2 decoder interface handler for g_parm
  * @sd: pointer to standard V4L2 sub-device structure
  * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
@@ -962,6 +914,9 @@ static int tvp514x_get_pad_format(struct v4l2_subdev *sd,
 	struct tvp514x_decoder *decoder = to_decoder(sd);
 	__u32 which = format->which;
 
+	if (format->pad)
+		return -EINVAL;
+
 	if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 		format->format = decoder->format;
 		return 0;
@@ -1016,10 +971,6 @@ static const struct v4l2_subdev_video_ops tvp514x_video_ops = {
 	.s_std = tvp514x_s_std,
 	.s_routing = tvp514x_s_routing,
 	.querystd = tvp514x_querystd,
-	.enum_mbus_fmt = tvp514x_enum_mbus_fmt,
-	.g_mbus_fmt = tvp514x_mbus_fmt,
-	.try_mbus_fmt = tvp514x_mbus_fmt,
-	.s_mbus_fmt = tvp514x_mbus_fmt,
 	.g_parm = tvp514x_g_parm,
 	.s_parm = tvp514x_s_parm,
 	.s_stream = tvp514x_s_stream,
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 68cdab9c0903..e4fa0746f75e 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -817,24 +817,29 @@ static v4l2_std_id tvp5150_read_std(struct v4l2_subdev *sd)
 	}
 }
 
-static int tvp5150_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
-						u32 *code)
+static int tvp5150_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index)
+	if (code->pad || code->index)
 		return -EINVAL;
 
-	*code = MEDIA_BUS_FMT_UYVY8_2X8;
+	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
 	return 0;
 }
 
-static int tvp5150_mbus_fmt(struct v4l2_subdev *sd,
-			    struct v4l2_mbus_framefmt *f)
+static int tvp5150_fill_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *f;
 	struct tvp5150 *decoder = to_tvp5150(sd);
 
-	if (f == NULL)
+	if (!format || format->pad)
 		return -EINVAL;
 
+	f = &format->format;
+
 	tvp5150_reset(sd, 0);
 
 	f->width = decoder->rect.width;
@@ -1068,10 +1073,6 @@ static const struct v4l2_subdev_tuner_ops tvp5150_tuner_ops = {
 static const struct v4l2_subdev_video_ops tvp5150_video_ops = {
 	.s_std = tvp5150_s_std,
 	.s_routing = tvp5150_s_routing,
-	.enum_mbus_fmt = tvp5150_enum_mbus_fmt,
-	.s_mbus_fmt = tvp5150_mbus_fmt,
-	.try_mbus_fmt = tvp5150_mbus_fmt,
-	.g_mbus_fmt = tvp5150_mbus_fmt,
 	.s_crop = tvp5150_s_crop,
 	.g_crop = tvp5150_g_crop,
 	.cropcap = tvp5150_cropcap,
@@ -1084,11 +1085,18 @@ static const struct v4l2_subdev_vbi_ops tvp5150_vbi_ops = {
 	.s_raw_fmt = tvp5150_s_raw_fmt,
 };
 
+static const struct v4l2_subdev_pad_ops tvp5150_pad_ops = {
+	.enum_mbus_code = tvp5150_enum_mbus_code,
+	.set_fmt = tvp5150_fill_fmt,
+	.get_fmt = tvp5150_fill_fmt,
+};
+
 static const struct v4l2_subdev_ops tvp5150_ops = {
 	.core = &tvp5150_core_ops,
 	.tuner = &tvp5150_tuner_ops,
 	.video = &tvp5150_video_ops,
 	.vbi = &tvp5150_vbi_ops,
+	.pad = &tvp5150_pad_ops,
 };
 
 
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index 787cdfb08749..05077cffd235 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -611,31 +611,6 @@ static int tvp7002_s_ctrl(struct v4l2_ctrl *ctrl)
 }
 
 /*
- * tvp7002_mbus_fmt() - V4L2 decoder interface handler for try/s/g_mbus_fmt
- * @sd: pointer to standard V4L2 sub-device structure
- * @f: pointer to mediabus format structure
- *
- * Negotiate the image capture size and mediabus format.
- * There is only one possible format, so this single function works for
- * get, set and try.
- */
-static int tvp7002_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
-{
-	struct tvp7002 *device = to_tvp7002(sd);
-	const struct v4l2_bt_timings *bt = &device->current_timings->timings.bt;
-
-	f->width = bt->width;
-	f->height = bt->height;
-	f->code = MEDIA_BUS_FMT_YUYV10_1X20;
-	f->field = device->current_timings->scanmode;
-	f->colorspace = device->current_timings->color_space;
-
-	v4l2_dbg(1, debug, sd, "MBUS_FMT: Width - %d, Height - %d",
-			f->width, f->height);
-	return 0;
-}
-
-/*
  * tvp7002_query_dv() - query DV timings
  * @sd: pointer to standard V4L2 sub-device structure
  * @index: index into the tvp7002_timings array
@@ -747,25 +722,6 @@ static int tvp7002_s_register(struct v4l2_subdev *sd,
 #endif
 
 /*
- * tvp7002_enum_mbus_fmt() - Enum supported mediabus formats
- * @sd: pointer to standard V4L2 sub-device structure
- * @index: format index
- * @code: pointer to mediabus format
- *
- * Enumerate supported mediabus formats.
- */
-
-static int tvp7002_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
-					u32 *code)
-{
-	/* Check requested format index is within range */
-	if (index)
-		return -EINVAL;
-	*code = MEDIA_BUS_FMT_YUYV10_1X20;
-	return 0;
-}
-
-/*
  * tvp7002_s_stream() - V4L2 decoder i/f handler for s_stream
  * @sd: pointer to standard V4L2 sub-device structure
  * @enable: streaming enable or disable
@@ -924,10 +880,6 @@ static const struct v4l2_subdev_video_ops tvp7002_video_ops = {
 	.s_dv_timings = tvp7002_s_dv_timings,
 	.query_dv_timings = tvp7002_query_dv_timings,
 	.s_stream = tvp7002_s_stream,
-	.g_mbus_fmt = tvp7002_mbus_fmt,
-	.try_mbus_fmt = tvp7002_mbus_fmt,
-	.s_mbus_fmt = tvp7002_mbus_fmt,
-	.enum_mbus_fmt = tvp7002_enum_mbus_fmt,
 };
 
 /* media pad related operation handlers */
diff --git a/drivers/media/i2c/vs6624.c b/drivers/media/i2c/vs6624.c
index 00e7f043977e..4c72a18c0b8c 100644
--- a/drivers/media/i2c/vs6624.c
+++ b/drivers/media/i2c/vs6624.c
@@ -557,21 +557,28 @@ static int vs6624_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static int vs6624_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
-				u32 *code)
+static int vs6624_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (index >= ARRAY_SIZE(vs6624_formats))
+	if (code->pad || code->index >= ARRAY_SIZE(vs6624_formats))
 		return -EINVAL;
 
-	*code = vs6624_formats[index].mbus_code;
+	code->code = vs6624_formats[code->index].mbus_code;
 	return 0;
 }
 
-static int vs6624_try_mbus_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *fmt)
+static int vs6624_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct vs6624 *sensor = to_vs6624(sd);
 	int index;
 
+	if (format->pad)
+		return -EINVAL;
+
 	for (index = 0; index < ARRAY_SIZE(vs6624_formats); index++)
 		if (vs6624_formats[index].mbus_code == fmt->code)
 			break;
@@ -590,18 +597,11 @@ static int vs6624_try_mbus_fmt(struct v4l2_subdev *sd,
 	fmt->height = fmt->height & (~3);
 	fmt->field = V4L2_FIELD_NONE;
 	fmt->colorspace = vs6624_formats[index].colorspace;
-	return 0;
-}
-
-static int vs6624_s_mbus_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *fmt)
-{
-	struct vs6624 *sensor = to_vs6624(sd);
-	int ret;
 
-	ret = vs6624_try_mbus_fmt(sd, fmt);
-	if (ret)
-		return ret;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *fmt;
+		return 0;
+	}
 
 	/* set image format */
 	switch (fmt->code) {
@@ -648,12 +648,16 @@ static int vs6624_s_mbus_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int vs6624_g_mbus_fmt(struct v4l2_subdev *sd,
-				struct v4l2_mbus_framefmt *fmt)
+static int vs6624_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
 	struct vs6624 *sensor = to_vs6624(sd);
 
-	*fmt = sensor->fmt;
+	if (format->pad)
+		return -EINVAL;
+
+	format->format = sensor->fmt;
 	return 0;
 }
 
@@ -738,18 +742,21 @@ static const struct v4l2_subdev_core_ops vs6624_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops vs6624_video_ops = {
-	.enum_mbus_fmt = vs6624_enum_mbus_fmt,
-	.try_mbus_fmt = vs6624_try_mbus_fmt,
-	.s_mbus_fmt = vs6624_s_mbus_fmt,
-	.g_mbus_fmt = vs6624_g_mbus_fmt,
 	.s_parm = vs6624_s_parm,
 	.g_parm = vs6624_g_parm,
 	.s_stream = vs6624_s_stream,
 };
 
+static const struct v4l2_subdev_pad_ops vs6624_pad_ops = {
+	.enum_mbus_code = vs6624_enum_mbus_code,
+	.get_fmt = vs6624_get_fmt,
+	.set_fmt = vs6624_set_fmt,
+};
+
 static const struct v4l2_subdev_ops vs6624_ops = {
 	.core = &vs6624_core_ops,
 	.video = &vs6624_video_ops,
+	.pad = &vs6624_pad_ops,
 };
 
 static int vs6624_probe(struct i2c_client *client,
diff --git a/drivers/media/pci/Kconfig b/drivers/media/pci/Kconfig
index 218144a99016..f318ae9bb57a 100644
--- a/drivers/media/pci/Kconfig
+++ b/drivers/media/pci/Kconfig
@@ -21,6 +21,7 @@ source "drivers/media/pci/zoran/Kconfig"
 source "drivers/media/pci/saa7146/Kconfig"
 source "drivers/media/pci/solo6x10/Kconfig"
 source "drivers/media/pci/tw68/Kconfig"
+source "drivers/media/pci/dt3155/Kconfig"
 endif
 
 if MEDIA_ANALOG_TV_SUPPORT || MEDIA_DIGITAL_TV_SUPPORT
@@ -32,6 +33,7 @@ source "drivers/media/pci/cx88/Kconfig"
 source "drivers/media/pci/bt8xx/Kconfig"
 source "drivers/media/pci/saa7134/Kconfig"
 source "drivers/media/pci/saa7164/Kconfig"
+source "drivers/media/pci/cobalt/Kconfig"
 
 endif
 
diff --git a/drivers/media/pci/Makefile b/drivers/media/pci/Makefile
index 0baf0d2967ee..23ce53bd47c3 100644
--- a/drivers/media/pci/Makefile
+++ b/drivers/media/pci/Makefile
@@ -24,6 +24,8 @@ obj-$(CONFIG_VIDEO_BT848) += bt8xx/
 obj-$(CONFIG_VIDEO_SAA7134) += saa7134/
 obj-$(CONFIG_VIDEO_SAA7164) += saa7164/
 obj-$(CONFIG_VIDEO_TW68) += tw68/
+obj-$(CONFIG_VIDEO_DT3155) += dt3155/
 obj-$(CONFIG_VIDEO_MEYE) += meye/
 obj-$(CONFIG_STA2X11_VIP) += sta2x11/
 obj-$(CONFIG_VIDEO_SOLO6X10) += solo6x10/
+obj-$(CONFIG_VIDEO_COBALT) += cobalt/
diff --git a/drivers/media/pci/bt8xx/bttv-audio-hook.c b/drivers/media/pci/bt8xx/bttv-audio-hook.c
index 2364d16586b3..9f1f9169fb5b 100644
--- a/drivers/media/pci/bt8xx/bttv-audio-hook.c
+++ b/drivers/media/pci/bt8xx/bttv-audio-hook.c
@@ -54,23 +54,33 @@ void winview_volume(struct bttv *btv, __u16 volume)
 
 void gvbctv3pci_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 {
-	unsigned int con = 0;
+	unsigned int con;
 
-	if (set) {
-		gpio_inout(0x300, 0x300);
-		if (t->audmode & V4L2_TUNER_MODE_LANG1)
-			con = 0x000;
-		if (t->audmode & V4L2_TUNER_MODE_LANG2)
-			con = 0x300;
-		if (t->audmode & V4L2_TUNER_MODE_STEREO)
-			con = 0x200;
-/*		if (t->audmode & V4L2_TUNER_MODE_MONO)
- *			con = 0x100; */
-		gpio_bits(0x300, con);
-	} else {
-		t->audmode = V4L2_TUNER_MODE_STEREO |
-			  V4L2_TUNER_MODE_LANG1  | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
+		return;
+	}
+
+	gpio_inout(0x300, 0x300);
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_LANG1:
+	default:
+		con = 0x000;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		con = 0x300;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		con = 0x200;
+		break;
 	}
+	gpio_bits(0x300, con);
 }
 
 void gvbctv5pci_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
@@ -82,47 +92,51 @@ void gvbctv5pci_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 
 	val = gpio_read();
 	if (set) {
-		con = 0x000;
-		if (t->audmode & V4L2_TUNER_MODE_LANG2) {
-			if (t->audmode & V4L2_TUNER_MODE_LANG1) {
-				/* LANG1 + LANG2 */
-				con = 0x100;
-			}
-			else {
-				/* LANG2 */
-				con = 0x300;
-			}
+		switch (t->audmode) {
+		case V4L2_TUNER_MODE_LANG2:
+			con = 0x300;
+			break;
+		case V4L2_TUNER_MODE_LANG1_LANG2:
+			con = 0x100;
+			break;
+		default:
+			con = 0x000;
+			break;
 		}
 		if (con != (val & 0x300)) {
 			gpio_bits(0x300, con);
 			if (bttv_gpio)
-				bttv_gpio_tracking(btv,"gvbctv5pci");
+				bttv_gpio_tracking(btv, "gvbctv5pci");
 		}
 	} else {
 		switch (val & 0x70) {
 		  case 0x10:
 			t->rxsubchans = V4L2_TUNER_SUB_LANG1 |  V4L2_TUNER_SUB_LANG2;
+			t->audmode = V4L2_TUNER_MODE_LANG1_LANG2;
 			break;
 		  case 0x30:
 			t->rxsubchans = V4L2_TUNER_SUB_LANG2;
+			t->audmode = V4L2_TUNER_MODE_LANG1_LANG2;
 			break;
 		  case 0x50:
 			t->rxsubchans = V4L2_TUNER_SUB_LANG1;
+			t->audmode = V4L2_TUNER_MODE_LANG1_LANG2;
 			break;
 		  case 0x60:
 			t->rxsubchans = V4L2_TUNER_SUB_STEREO;
+			t->audmode = V4L2_TUNER_MODE_STEREO;
 			break;
 		  case 0x70:
 			t->rxsubchans = V4L2_TUNER_SUB_MONO;
+			t->audmode = V4L2_TUNER_MODE_MONO;
 			break;
 		  default:
 			t->rxsubchans = V4L2_TUNER_SUB_MONO |
 					 V4L2_TUNER_SUB_STEREO |
 					 V4L2_TUNER_SUB_LANG1 |
 					 V4L2_TUNER_SUB_LANG2;
+			t->audmode = V4L2_TUNER_MODE_LANG1;
 		}
-		t->audmode = V4L2_TUNER_MODE_STEREO |
-			  V4L2_TUNER_MODE_LANG1  | V4L2_TUNER_MODE_LANG2;
 	}
 }
 
@@ -142,23 +156,32 @@ void gvbctv5pci_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 
 void avermedia_tvphone_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 {
-	int val = 0;
+	int val;
 
-	if (set) {
-		if (t->audmode & V4L2_TUNER_MODE_LANG2)   /* SAP */
-			val = 0x02;
-		if (t->audmode & V4L2_TUNER_MODE_STEREO)
-			val = 0x01;
-		if (val) {
-			gpio_bits(0x03,val);
-			if (bttv_gpio)
-				bttv_gpio_tracking(btv,"avermedia");
-		}
-	} else {
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			V4L2_TUNER_MODE_LANG1;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
+		return;
+	}
+
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_LANG2:   /* SAP */
+		val = 0x02;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		val = 0x01;
+		break;
+	default:
 		return;
 	}
+	gpio_bits(0x03, val);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "avermedia");
 }
 
 
@@ -166,19 +189,31 @@ void avermedia_tv_stereo_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 {
 	int val = 0;
 
-	if (set) {
-		if (t->audmode & V4L2_TUNER_MODE_LANG2)   /* SAP */
-			val = 0x01;
-		if (t->audmode & V4L2_TUNER_MODE_STEREO)  /* STEREO */
-			val = 0x02;
-		btaor(val, ~0x03, BT848_GPIO_DATA);
-		if (bttv_gpio)
-			bttv_gpio_tracking(btv,"avermedia");
-	} else {
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
 		return;
 	}
+
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_LANG2:   /* SAP */
+		val = 0x01;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		val = 0x02;
+		break;
+	default:
+		val = 0;
+		break;
+	}
+	btaor(val, ~0x03, BT848_GPIO_DATA);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "avermedia");
 }
 
 /* Lifetec 9415 handling */
@@ -192,23 +227,32 @@ void lt9415_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 		return;
 	}
 
-	if (set) {
-		if (t->audmode & V4L2_TUNER_MODE_LANG2)  /* A2 SAP */
-			val = 0x0080;
-		if (t->audmode & V4L2_TUNER_MODE_STEREO) /* A2 stereo */
-			val = 0x0880;
-		if ((t->audmode & V4L2_TUNER_MODE_LANG1) ||
-		    (t->audmode & V4L2_TUNER_MODE_MONO))
-			val = 0;
-		gpio_bits(0x0880, val);
-		if (bttv_gpio)
-			bttv_gpio_tracking(btv,"lt9415");
-	} else {
-		/* autodetect doesn't work with this card :-( */
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
 		return;
 	}
+
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_LANG2:	/* A2 SAP */
+		val = 0x0080;
+		break;
+	case V4L2_TUNER_MODE_STEREO:	/* A2 stereo */
+		val = 0x0880;
+		break;
+	default:
+		val = 0;
+		break;
+	}
+
+	gpio_bits(0x0880, val);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "lt9415");
 }
 
 /* TDA9821 on TerraTV+ Bt848, Bt878 */
@@ -216,45 +260,69 @@ void terratv_audio(struct bttv *btv,  struct v4l2_tuner *t, int set)
 {
 	unsigned int con = 0;
 
-	if (set) {
-		gpio_inout(0x180000,0x180000);
-		if (t->audmode & V4L2_TUNER_MODE_LANG2)
-			con = 0x080000;
-		if (t->audmode & V4L2_TUNER_MODE_STEREO)
-			con = 0x180000;
-		gpio_bits(0x180000, con);
-		if (bttv_gpio)
-			bttv_gpio_tracking(btv,"terratv");
-	} else {
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
+		return;
+	}
+
+	gpio_inout(0x180000, 0x180000);
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_LANG2:
+		con = 0x080000;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		con = 0x180000;
+		break;
+	default:
+		con = 0;
+		break;
 	}
+	gpio_bits(0x180000, con);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "terratv");
 }
 
 
 void winfast2000_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 {
-	unsigned long val = 0;
+	unsigned long val;
 
-	if (set) {
-		/*btor (0xc32000, BT848_GPIO_OUT_EN);*/
-		if (t->audmode & V4L2_TUNER_MODE_MONO)		/* Mono */
-			val = 0x420000;
-		if (t->audmode & V4L2_TUNER_MODE_LANG1)	/* Mono */
-			val = 0x420000;
-		if (t->audmode & V4L2_TUNER_MODE_LANG2)	/* SAP */
-			val = 0x410000;
-		if (t->audmode & V4L2_TUNER_MODE_STEREO)	/* Stereo */
-			val = 0x020000;
-		if (val) {
-			gpio_bits(0x430000, val);
-			if (bttv_gpio)
-				bttv_gpio_tracking(btv,"winfast2000");
-		}
-	} else {
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			  V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
+		return;
 	}
+
+	/*btor (0xc32000, BT848_GPIO_OUT_EN);*/
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_MONO:
+	case V4L2_TUNER_MODE_LANG1:
+		val = 0x420000;
+		break;
+	case V4L2_TUNER_MODE_LANG2: /* SAP */
+		val = 0x410000;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		val = 0x020000;
+		break;
+	default:
+		return;
+	}
+
+	gpio_bits(0x430000, val);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "winfast2000");
 }
 
 /*
@@ -272,23 +340,33 @@ void pvbt878p9b_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 	if (btv->radio_user)
 		return;
 
-	if (set) {
-		if (t->audmode & V4L2_TUNER_MODE_MONO)	{
-			val = 0x01;
-		}
-		if ((t->audmode & (V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2))
-		    || (t->audmode & V4L2_TUNER_MODE_STEREO)) {
-			val = 0x02;
-		}
-		if (val) {
-			gpio_bits(0x03,val);
-			if (bttv_gpio)
-				bttv_gpio_tracking(btv,"pvbt878p9b");
-		}
-	} else {
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
+		return;
 	}
+
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_MONO:
+		val = 0x01;
+		break;
+	case V4L2_TUNER_MODE_LANG1:
+	case V4L2_TUNER_MODE_LANG2:
+	case V4L2_TUNER_MODE_STEREO:
+		val = 0x02;
+		break;
+	default:
+		return;
+	}
+
+	gpio_bits(0x03, val);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "pvbt878p9b");
 }
 
 /*
@@ -298,28 +376,37 @@ void pvbt878p9b_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
  */
 void fv2000s_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 {
-	unsigned int val = 0xffff;
+	unsigned int val;
 
 	if (btv->radio_user)
 		return;
 
-	if (set) {
-		if (t->audmode & V4L2_TUNER_MODE_MONO)	{
-			val = 0x0000;
-		}
-		if ((t->audmode & (V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2))
-		    || (t->audmode & V4L2_TUNER_MODE_STEREO)) {
-			val = 0x1080; /*-dk-???: 0x0880, 0x0080, 0x1800 ... */
-		}
-		if (val != 0xffff) {
-			gpio_bits(0x1800, val);
-			if (bttv_gpio)
-				bttv_gpio_tracking(btv,"fv2000s");
-		}
-	} else {
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
+		return;
 	}
+
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_MONO:
+		val = 0x0000;
+		break;
+	case V4L2_TUNER_MODE_LANG1:
+	case V4L2_TUNER_MODE_LANG2:
+	case V4L2_TUNER_MODE_STEREO:
+		val = 0x1080; /*-dk-???: 0x0880, 0x0080, 0x1800 ... */
+		break;
+	default:
+		return;
+	}
+	gpio_bits(0x1800, val);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "fv2000s");
 }
 
 /*
@@ -328,26 +415,33 @@ void fv2000s_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
  */
 void windvr_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 {
-	unsigned long val = 0;
+	unsigned long val;
 
-	if (set) {
-		if (t->audmode & V4L2_TUNER_MODE_MONO)
-			val = 0x040000;
-		if (t->audmode & V4L2_TUNER_MODE_LANG1)
-			val = 0;
-		if (t->audmode & V4L2_TUNER_MODE_LANG2)
-			val = 0x100000;
-		if (t->audmode & V4L2_TUNER_MODE_STEREO)
-			val = 0;
-		if (val) {
-			gpio_bits(0x140000, val);
-			if (bttv_gpio)
-				bttv_gpio_tracking(btv,"windvr");
-		}
-	} else {
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			  V4L2_TUNER_MODE_LANG1 | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
+		return;
+	}
+
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_MONO:
+		val = 0x040000;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		val = 0x100000;
+		break;
+	default:
+		return;
 	}
+
+	gpio_bits(0x140000, val);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "windvr");
 }
 
 /*
@@ -360,23 +454,36 @@ void adtvk503_audio(struct bttv *btv, struct v4l2_tuner *t, int set)
 
 	/* btaor(0x1e0000, ~0x1e0000, BT848_GPIO_OUT_EN); */
 
-	if (set) {
-		/* btor(***, BT848_GPIO_OUT_EN); */
-		if (t->audmode & V4L2_TUNER_MODE_LANG1)
-			con = 0x00000000;
-		if (t->audmode & V4L2_TUNER_MODE_LANG2)
-			con = 0x00180000;
-		if (t->audmode & V4L2_TUNER_MODE_STEREO)
-			con = 0x00000000;
-		if (t->audmode & V4L2_TUNER_MODE_MONO)
-			con = 0x00060000;
-		if (con != 0xffffff) {
-			gpio_bits(0x1e0000,con);
-			if (bttv_gpio)
-				bttv_gpio_tracking(btv, "adtvk503");
-		}
-	} else {
-		t->audmode = V4L2_TUNER_MODE_MONO | V4L2_TUNER_MODE_STEREO |
-			  V4L2_TUNER_MODE_LANG1  | V4L2_TUNER_MODE_LANG2;
+	if (!set) {
+		/* Not much to do here */
+		t->audmode = V4L2_TUNER_MODE_LANG1;
+		t->rxsubchans = V4L2_TUNER_SUB_MONO |
+				V4L2_TUNER_SUB_STEREO |
+				V4L2_TUNER_SUB_LANG1 |
+				V4L2_TUNER_SUB_LANG2;
+
+		return;
 	}
+
+	/* btor(***, BT848_GPIO_OUT_EN); */
+	switch (t->audmode) {
+	case V4L2_TUNER_MODE_LANG1:
+		con = 0x00000000;
+		break;
+	case V4L2_TUNER_MODE_LANG2:
+		con = 0x00180000;
+		break;
+	case V4L2_TUNER_MODE_STEREO:
+		con = 0x00000000;
+		break;
+	case V4L2_TUNER_MODE_MONO:
+		con = 0x00060000;
+		break;
+	default:
+		return;
+	}
+
+	gpio_bits(0x1e0000, con);
+	if (bttv_gpio)
+		bttv_gpio_tracking(btv, "adtvk503");
 }
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index bc12060e0882..3632958f2158 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -2676,7 +2676,8 @@ static int bttv_s_fbuf(struct file *file, void *f,
 		fh->ov.w.height = fb->fmt.height;
 		btv->init.ov.w.width  = fb->fmt.width;
 		btv->init.ov.w.height = fb->fmt.height;
-			kfree(fh->ov.clips);
+
+		kfree(fh->ov.clips);
 		fh->ov.clips = NULL;
 		fh->ov.nclips = 0;
 
@@ -4238,6 +4239,7 @@ fail0:
 		iounmap(btv->bt848_mmio);
 	release_mem_region(pci_resource_start(btv->c.pci,0),
 			   pci_resource_len(btv->c.pci,0));
+	pci_disable_device(btv->c.pci);
 	return result;
 }
 
@@ -4281,6 +4283,7 @@ static void bttv_remove(struct pci_dev *pci_dev)
 	iounmap(btv->bt848_mmio);
 	release_mem_region(pci_resource_start(btv->c.pci,0),
 			   pci_resource_len(btv->c.pci,0));
+	pci_disable_device(btv->c.pci);
 
 	v4l2_device_unregister(&btv->c.v4l2_dev);
 	bttvs[btv->c.nr] = NULL;
diff --git a/drivers/media/pci/bt8xx/dst.c b/drivers/media/pci/bt8xx/dst.c
index f2261dfe5d1a..4a90eee5e3bb 100644
--- a/drivers/media/pci/bt8xx/dst.c
+++ b/drivers/media/pci/bt8xx/dst.c
@@ -425,7 +425,8 @@ static int dst_set_bandwidth(struct dst_state *state, u32 bandwidth)
 	return 0;
 }
 
-static int dst_set_inversion(struct dst_state *state, fe_spectral_inversion_t inversion)
+static int dst_set_inversion(struct dst_state *state,
+			     enum fe_spectral_inversion inversion)
 {
 	state->inversion = inversion;
 	switch (inversion) {
@@ -442,13 +443,13 @@ static int dst_set_inversion(struct dst_state *state, fe_spectral_inversion_t in
 	return 0;
 }
 
-static int dst_set_fec(struct dst_state *state, fe_code_rate_t fec)
+static int dst_set_fec(struct dst_state *state, enum fe_code_rate fec)
 {
 	state->fec = fec;
 	return 0;
 }
 
-static fe_code_rate_t dst_get_fec(struct dst_state *state)
+static enum fe_code_rate dst_get_fec(struct dst_state *state)
 {
 	return state->fec;
 }
@@ -499,7 +500,8 @@ static int dst_set_symbolrate(struct dst_state *state, u32 srate)
 	return 0;
 }
 
-static int dst_set_modulation(struct dst_state *state, fe_modulation_t modulation)
+static int dst_set_modulation(struct dst_state *state,
+			      enum fe_modulation modulation)
 {
 	if (state->dst_type != DST_TYPE_IS_CABLE)
 		return -EOPNOTSUPP;
@@ -536,7 +538,7 @@ static int dst_set_modulation(struct dst_state *state, fe_modulation_t modulatio
 	return 0;
 }
 
-static fe_modulation_t dst_get_modulation(struct dst_state *state)
+static enum fe_modulation dst_get_modulation(struct dst_state *state)
 {
 	return state->modulation;
 }
@@ -1376,7 +1378,8 @@ static int dst_get_tuna(struct dst_state *state)
 	return 1;
 }
 
-static int dst_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage);
+static int dst_set_voltage(struct dvb_frontend *fe,
+			   enum fe_sec_voltage voltage);
 
 static int dst_write_tuna(struct dvb_frontend *fe)
 {
@@ -1466,7 +1469,7 @@ static int dst_set_diseqc(struct dvb_frontend *fe, struct dvb_diseqc_master_cmd
 	return dst_command(state, paket, 8);
 }
 
-static int dst_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int dst_set_voltage(struct dvb_frontend *fe, enum fe_sec_voltage voltage)
 {
 	int need_cmd, retval = 0;
 	struct dst_state *state = fe->demodulator_priv;
@@ -1500,7 +1503,7 @@ static int dst_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 	return retval;
 }
 
-static int dst_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
+static int dst_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct dst_state *state = fe->demodulator_priv;
 
@@ -1525,7 +1528,7 @@ static int dst_set_tone(struct dvb_frontend *fe, fe_sec_tone_mode_t tone)
 	return dst_tone_power_cmd(state);
 }
 
-static int dst_send_burst(struct dvb_frontend *fe, fe_sec_mini_cmd_t minicmd)
+static int dst_send_burst(struct dvb_frontend *fe, enum fe_sec_mini_cmd minicmd)
 {
 	struct dst_state *state = fe->demodulator_priv;
 
@@ -1575,7 +1578,7 @@ static int bt8xx_dst_init(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int dst_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int dst_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct dst_state *state = fe->demodulator_priv;
 
@@ -1646,7 +1649,7 @@ static int dst_tune_frontend(struct dvb_frontend* fe,
 			    bool re_tune,
 			    unsigned int mode_flags,
 			    unsigned int *delay,
-			    fe_status_t *status)
+			    enum fe_status *status)
 {
 	struct dst_state *state = fe->demodulator_priv;
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
diff --git a/drivers/media/pci/bt8xx/dst_ca.c b/drivers/media/pci/bt8xx/dst_ca.c
index c22c4ae06844..c5cc14ef8347 100644
--- a/drivers/media/pci/bt8xx/dst_ca.c
+++ b/drivers/media/pci/bt8xx/dst_ca.c
@@ -320,29 +320,27 @@ static int ca_get_message(struct dst_state *state, struct ca_msg *p_ca_message,
 	if (copy_from_user(p_ca_message, arg, sizeof (struct ca_msg)))
 		return -EFAULT;
 
-	if (p_ca_message->msg) {
-		dprintk(verbose, DST_CA_NOTICE, 1, " Message = [%*ph]",
-			3, p_ca_message->msg);
-
-		for (i = 0; i < 3; i++) {
-			command = command | p_ca_message->msg[i];
-			if (i < 2)
-				command = command << 8;
-		}
-		dprintk(verbose, DST_CA_NOTICE, 1, " Command=[0x%x]", command);
+	dprintk(verbose, DST_CA_NOTICE, 1, " Message = [%*ph]",
+		3, p_ca_message->msg);
 
-		switch (command) {
-		case CA_APP_INFO:
-			memcpy(p_ca_message->msg, state->messages, 128);
-			if (copy_to_user(arg, p_ca_message, sizeof (struct ca_msg)) )
-				return -EFAULT;
-			break;
-		case CA_INFO:
-			memcpy(p_ca_message->msg, state->messages, 128);
-			if (copy_to_user(arg, p_ca_message, sizeof (struct ca_msg)) )
-				return -EFAULT;
-			break;
-		}
+	for (i = 0; i < 3; i++) {
+		command = command | p_ca_message->msg[i];
+		if (i < 2)
+			command = command << 8;
+	}
+	dprintk(verbose, DST_CA_NOTICE, 1, " Command=[0x%x]", command);
+
+	switch (command) {
+	case CA_APP_INFO:
+		memcpy(p_ca_message->msg, state->messages, 128);
+		if (copy_to_user(arg, p_ca_message, sizeof (struct ca_msg)) )
+			return -EFAULT;
+		break;
+	case CA_INFO:
+		memcpy(p_ca_message->msg, state->messages, 128);
+		if (copy_to_user(arg, p_ca_message, sizeof (struct ca_msg)) )
+			return -EFAULT;
+		break;
 	}
 
 	return 0;
@@ -494,60 +492,58 @@ static int ca_send_message(struct dst_state *state, struct ca_msg *p_ca_message,
 		goto free_mem_and_exit;
 	}
 
+	/*	EN50221 tag	*/
+	command = 0;
 
-	if (p_ca_message->msg) {
-		/*	EN50221 tag	*/
-		command = 0;
+	for (i = 0; i < 3; i++) {
+		command = command | p_ca_message->msg[i];
+		if (i < 2)
+			command = command << 8;
+	}
+	dprintk(verbose, DST_CA_DEBUG, 1, " Command=[0x%x]\n", command);
 
-		for (i = 0; i < 3; i++) {
-			command = command | p_ca_message->msg[i];
-			if (i < 2)
-				command = command << 8;
+	switch (command) {
+	case CA_PMT:
+		dprintk(verbose, DST_CA_DEBUG, 1, "Command = SEND_CA_PMT");
+		if ((ca_set_pmt(state, p_ca_message, hw_buffer, 0, 0)) < 0) {	// code simplification started
+			dprintk(verbose, DST_CA_ERROR, 1, " -->CA_PMT Failed !");
+			result = -1;
+			goto free_mem_and_exit;
 		}
-		dprintk(verbose, DST_CA_DEBUG, 1, " Command=[0x%x]\n", command);
-
-		switch (command) {
-		case CA_PMT:
-			dprintk(verbose, DST_CA_DEBUG, 1, "Command = SEND_CA_PMT");
-			if ((ca_set_pmt(state, p_ca_message, hw_buffer, 0, 0)) < 0) {	// code simplification started
-				dprintk(verbose, DST_CA_ERROR, 1, " -->CA_PMT Failed !");
-				result = -1;
-				goto free_mem_and_exit;
-			}
-			dprintk(verbose, DST_CA_INFO, 1, " -->CA_PMT Success !");
-			break;
-		case CA_PMT_REPLY:
-			dprintk(verbose, DST_CA_INFO, 1, "Command = CA_PMT_REPLY");
-			/*      Have to handle the 2 basic types of cards here  */
-			if ((dst_check_ca_pmt(state, p_ca_message, hw_buffer)) < 0) {
-				dprintk(verbose, DST_CA_ERROR, 1, " -->CA_PMT_REPLY Failed !");
-				result = -1;
-				goto free_mem_and_exit;
-			}
-			dprintk(verbose, DST_CA_INFO, 1, " -->CA_PMT_REPLY Success !");
-			break;
-		case CA_APP_INFO_ENQUIRY:		// only for debugging
-			dprintk(verbose, DST_CA_INFO, 1, " Getting Cam Application information");
-
-			if ((ca_get_app_info(state)) < 0) {
-				dprintk(verbose, DST_CA_ERROR, 1, " -->CA_APP_INFO_ENQUIRY Failed !");
-				result = -1;
-				goto free_mem_and_exit;
-			}
-			dprintk(verbose, DST_CA_INFO, 1, " -->CA_APP_INFO_ENQUIRY Success !");
-			break;
-		case CA_INFO_ENQUIRY:
-			dprintk(verbose, DST_CA_INFO, 1, " Getting CA Information");
-
-			if ((ca_get_ca_info(state)) < 0) {
-				dprintk(verbose, DST_CA_ERROR, 1, " -->CA_INFO_ENQUIRY Failed !");
-				result = -1;
-				goto free_mem_and_exit;
-			}
-			dprintk(verbose, DST_CA_INFO, 1, " -->CA_INFO_ENQUIRY Success !");
-			break;
+		dprintk(verbose, DST_CA_INFO, 1, " -->CA_PMT Success !");
+		break;
+	case CA_PMT_REPLY:
+		dprintk(verbose, DST_CA_INFO, 1, "Command = CA_PMT_REPLY");
+		/*      Have to handle the 2 basic types of cards here  */
+		if ((dst_check_ca_pmt(state, p_ca_message, hw_buffer)) < 0) {
+			dprintk(verbose, DST_CA_ERROR, 1, " -->CA_PMT_REPLY Failed !");
+			result = -1;
+			goto free_mem_and_exit;
+		}
+		dprintk(verbose, DST_CA_INFO, 1, " -->CA_PMT_REPLY Success !");
+		break;
+	case CA_APP_INFO_ENQUIRY:		// only for debugging
+		dprintk(verbose, DST_CA_INFO, 1, " Getting Cam Application information");
+
+		if ((ca_get_app_info(state)) < 0) {
+			dprintk(verbose, DST_CA_ERROR, 1, " -->CA_APP_INFO_ENQUIRY Failed !");
+			result = -1;
+			goto free_mem_and_exit;
 		}
+		dprintk(verbose, DST_CA_INFO, 1, " -->CA_APP_INFO_ENQUIRY Success !");
+		break;
+	case CA_INFO_ENQUIRY:
+		dprintk(verbose, DST_CA_INFO, 1, " Getting CA Information");
+
+		if ((ca_get_ca_info(state)) < 0) {
+			dprintk(verbose, DST_CA_ERROR, 1, " -->CA_INFO_ENQUIRY Failed !");
+			result = -1;
+			goto free_mem_and_exit;
+		}
+		dprintk(verbose, DST_CA_INFO, 1, " -->CA_INFO_ENQUIRY Success !");
+		break;
 	}
+
 free_mem_and_exit:
 	kfree (hw_buffer);
 
diff --git a/drivers/media/pci/bt8xx/dst_common.h b/drivers/media/pci/bt8xx/dst_common.h
index d70d98f1a571..6a2cfdd44e3e 100644
--- a/drivers/media/pci/bt8xx/dst_common.h
+++ b/drivers/media/pci/bt8xx/dst_common.h
@@ -113,11 +113,11 @@ struct dst_state {
 	u8 dst_type;
 	u32 type_flags;
 	u32 frequency;		/* intermediate frequency in kHz for QPSK */
-	fe_spectral_inversion_t inversion;
+	enum fe_spectral_inversion inversion;
 	u32 symbol_rate;	/* symbol rate in Symbols per second */
-	fe_code_rate_t fec;
-	fe_sec_voltage_t voltage;
-	fe_sec_tone_mode_t tone;
+	enum fe_code_rate fec;
+	enum fe_sec_voltage voltage;
+	enum fe_sec_tone_mode tone;
 	u32 decode_freq;
 	u8 decode_lock;
 	u16 decode_strength;
@@ -127,8 +127,8 @@ struct dst_state {
 	u32 bandwidth;
 	u32 dst_hw_cap;
 	u8 dst_fw_version;
-	fe_sec_mini_cmd_t minicmd;
-	fe_modulation_t modulation;
+	enum fe_sec_mini_cmd minicmd;
+	enum fe_modulation modulation;
 	u8 messages[256];
 	u8 mac_address[8];
 	u8 fw_version[8];
diff --git a/drivers/media/pci/cobalt/Kconfig b/drivers/media/pci/cobalt/Kconfig
new file mode 100644
index 000000000000..3be1b2c3c386
--- /dev/null
+++ b/drivers/media/pci/cobalt/Kconfig
@@ -0,0 +1,18 @@
+config VIDEO_COBALT
+	tristate "Cisco Cobalt support"
+	depends on VIDEO_V4L2 && I2C && MEDIA_CONTROLLER
+	depends on PCI_MSI && MTD_COMPLEX_MAPPINGS && GPIOLIB
+	select I2C_ALGOBIT
+	select VIDEO_ADV7604
+	select VIDEO_ADV7511
+	select VIDEO_ADV7842
+	select VIDEOBUF2_DMA_SG
+	---help---
+	  This is a video4linux driver for the Cisco PCIe Cobalt card.
+
+	  This board is sadly not available outside of Cisco, but it is
+	  very useful as an example of a real driver that uses all the
+	  latest frameworks and APIs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cobalt.
diff --git a/drivers/media/pci/cobalt/Makefile b/drivers/media/pci/cobalt/Makefile
new file mode 100644
index 000000000000..b328955abbd2
--- /dev/null
+++ b/drivers/media/pci/cobalt/Makefile
@@ -0,0 +1,5 @@
+cobalt-objs    := cobalt-driver.o cobalt-irq.o cobalt-v4l2.o \
+		  cobalt-i2c.o cobalt-omnitek.o cobalt-flash.o cobalt-cpld.o \
+		  cobalt-alsa-main.o cobalt-alsa-pcm.o
+
+obj-$(CONFIG_VIDEO_COBALT) += cobalt.o
diff --git a/drivers/media/pci/cobalt/cobalt-alsa-main.c b/drivers/media/pci/cobalt/cobalt-alsa-main.c
new file mode 100644
index 000000000000..720e3ad93a9e
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-alsa-main.c
@@ -0,0 +1,162 @@
+/*
+ *  ALSA interface to cobalt PCM capture streams
+ *
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/spinlock.h>
+
+#include <media/v4l2-device.h>
+
+#include <sound/core.h>
+#include <sound/initval.h>
+
+#include "cobalt-driver.h"
+#include "cobalt-alsa.h"
+#include "cobalt-alsa-pcm.h"
+
+static void snd_cobalt_card_free(struct snd_cobalt_card *cobsc)
+{
+	if (cobsc == NULL)
+		return;
+
+	cobsc->s->alsa = NULL;
+
+	kfree(cobsc);
+}
+
+static void snd_cobalt_card_private_free(struct snd_card *sc)
+{
+	if (sc == NULL)
+		return;
+	snd_cobalt_card_free(sc->private_data);
+	sc->private_data = NULL;
+	sc->private_free = NULL;
+}
+
+static int snd_cobalt_card_create(struct cobalt_stream *s,
+				       struct snd_card *sc,
+				       struct snd_cobalt_card **cobsc)
+{
+	*cobsc = kzalloc(sizeof(struct snd_cobalt_card), GFP_KERNEL);
+	if (*cobsc == NULL)
+		return -ENOMEM;
+
+	(*cobsc)->s = s;
+	(*cobsc)->sc = sc;
+
+	sc->private_data = *cobsc;
+	sc->private_free = snd_cobalt_card_private_free;
+
+	return 0;
+}
+
+static int snd_cobalt_card_set_names(struct snd_cobalt_card *cobsc)
+{
+	struct cobalt_stream *s = cobsc->s;
+	struct cobalt *cobalt = s->cobalt;
+	struct snd_card *sc = cobsc->sc;
+
+	/* sc->driver is used by alsa-lib's configurator: simple, unique */
+	strlcpy(sc->driver, "cobalt", sizeof(sc->driver));
+
+	/* sc->shortname is a symlink in /proc/asound: COBALT-M -> cardN */
+	snprintf(sc->shortname,  sizeof(sc->shortname), "cobalt-%d-%d",
+		 cobalt->instance, s->video_channel);
+
+	/* sc->longname is read from /proc/asound/cards */
+	snprintf(sc->longname, sizeof(sc->longname),
+		 "Cobalt %d HDMI %d",
+		 cobalt->instance, s->video_channel);
+
+	return 0;
+}
+
+int cobalt_alsa_init(struct cobalt_stream *s)
+{
+	struct cobalt *cobalt = s->cobalt;
+	struct snd_card *sc = NULL;
+	struct snd_cobalt_card *cobsc;
+	int ret;
+
+	/* Numbrs steps from "Writing an ALSA Driver" by Takashi Iwai */
+
+	/* (1) Check and increment the device index */
+	/* This is a no-op for us.  We'll use the cobalt->instance */
+
+	/* (2) Create a card instance */
+	ret = snd_card_new(&cobalt->pci_dev->dev, SNDRV_DEFAULT_IDX1,
+			   SNDRV_DEFAULT_STR1, THIS_MODULE, 0, &sc);
+	if (ret) {
+		cobalt_err("snd_card_new() failed with err %d\n", ret);
+		goto err_exit;
+	}
+
+	/* (3) Create a main component */
+	ret = snd_cobalt_card_create(s, sc, &cobsc);
+	if (ret) {
+		cobalt_err("snd_cobalt_card_create() failed with err %d\n",
+			   ret);
+		goto err_exit_free;
+	}
+
+	/* (4) Set the driver ID and name strings */
+	snd_cobalt_card_set_names(cobsc);
+
+	ret = snd_cobalt_pcm_create(cobsc);
+	if (ret) {
+		cobalt_err("snd_cobalt_pcm_create() failed with err %d\n",
+			   ret);
+		goto err_exit_free;
+	}
+	/* FIXME - proc files */
+
+	/* (7) Set the driver data and return 0 */
+	/* We do this out of normal order for PCI drivers to avoid races */
+	s->alsa = cobsc;
+
+	/* (6) Register the card instance */
+	ret = snd_card_register(sc);
+	if (ret) {
+		s->alsa = NULL;
+		cobalt_err("snd_card_register() failed with err %d\n", ret);
+		goto err_exit_free;
+	}
+
+	return 0;
+
+err_exit_free:
+	if (sc != NULL)
+		snd_card_free(sc);
+	kfree(cobsc);
+err_exit:
+	return ret;
+}
+
+void cobalt_alsa_exit(struct cobalt_stream *s)
+{
+	struct snd_cobalt_card *cobsc = s->alsa;
+
+	if (cobsc)
+		snd_card_free(cobsc->sc);
+	s->alsa = NULL;
+}
diff --git a/drivers/media/pci/cobalt/cobalt-alsa-pcm.c b/drivers/media/pci/cobalt/cobalt-alsa-pcm.c
new file mode 100644
index 000000000000..f0bdf10cfd57
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-alsa-pcm.c
@@ -0,0 +1,603 @@
+/*
+ *  ALSA PCM device for the
+ *  ALSA interface to cobalt PCM capture streams
+ *
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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/init.h>
+#include <linux/kernel.h>
+#include <linux/vmalloc.h>
+#include <linux/delay.h>
+
+#include <media/v4l2-device.h>
+
+#include <sound/core.h>
+#include <sound/pcm.h>
+
+#include "cobalt-driver.h"
+#include "cobalt-alsa.h"
+#include "cobalt-alsa-pcm.h"
+
+static unsigned int pcm_debug;
+module_param(pcm_debug, int, 0644);
+MODULE_PARM_DESC(pcm_debug, "enable debug messages for pcm");
+
+#define dprintk(fmt, arg...) \
+	do { \
+		if (pcm_debug) \
+			pr_info("cobalt-alsa-pcm %s: " fmt, __func__, ##arg); \
+	} while (0)
+
+static struct snd_pcm_hardware snd_cobalt_hdmi_capture = {
+	.info = SNDRV_PCM_INFO_BLOCK_TRANSFER |
+		SNDRV_PCM_INFO_MMAP           |
+		SNDRV_PCM_INFO_INTERLEAVED    |
+		SNDRV_PCM_INFO_MMAP_VALID,
+
+	.formats = SNDRV_PCM_FMTBIT_S16_LE | SNDRV_PCM_FMTBIT_S32_LE,
+
+	.rates = SNDRV_PCM_RATE_48000,
+
+	.rate_min = 48000,
+	.rate_max = 48000,
+	.channels_min = 1,
+	.channels_max = 8,
+	.buffer_bytes_max = 4 * 240 * 8 * 4,	/* 5 ms of data */
+	.period_bytes_min = 1920,		/* 1 sample = 8 * 4 bytes */
+	.period_bytes_max = 240 * 8 * 4,	/* 5 ms of 8 channel data */
+	.periods_min = 1,
+	.periods_max = 4,
+};
+
+static struct snd_pcm_hardware snd_cobalt_playback = {
+	.info = SNDRV_PCM_INFO_BLOCK_TRANSFER |
+		SNDRV_PCM_INFO_MMAP           |
+		SNDRV_PCM_INFO_INTERLEAVED    |
+		SNDRV_PCM_INFO_MMAP_VALID,
+
+	.formats = SNDRV_PCM_FMTBIT_S16_LE | SNDRV_PCM_FMTBIT_S32_LE,
+
+	.rates = SNDRV_PCM_RATE_48000,
+
+	.rate_min = 48000,
+	.rate_max = 48000,
+	.channels_min = 1,
+	.channels_max = 8,
+	.buffer_bytes_max = 4 * 240 * 8 * 4,	/* 5 ms of data */
+	.period_bytes_min = 1920,		/* 1 sample = 8 * 4 bytes */
+	.period_bytes_max = 240 * 8 * 4,	/* 5 ms of 8 channel data */
+	.periods_min = 1,
+	.periods_max = 4,
+};
+
+static void sample_cpy(u8 *dst, const u8 *src, u32 len, bool is_s32)
+{
+	static const unsigned map[8] = { 0, 1, 5, 4, 2, 3, 6, 7 };
+	unsigned idx = 0;
+
+	while (len >= (is_s32 ? 4 : 2)) {
+		unsigned offset = map[idx] * 4;
+		u32 val = src[offset + 1] + (src[offset + 2] << 8) +
+			 (src[offset + 3] << 16);
+
+		if (is_s32) {
+			*dst++ = 0;
+			*dst++ = val & 0xff;
+		}
+		*dst++ = (val >> 8) & 0xff;
+		*dst++ = (val >> 16) & 0xff;
+		len -= is_s32 ? 4 : 2;
+		idx++;
+	}
+}
+
+static void cobalt_alsa_announce_pcm_data(struct snd_cobalt_card *cobsc,
+					u8 *pcm_data,
+					size_t skip,
+					size_t samples)
+{
+	struct snd_pcm_substream *substream;
+	struct snd_pcm_runtime *runtime;
+	unsigned long flags;
+	unsigned int oldptr;
+	unsigned int stride;
+	int length = samples;
+	int period_elapsed = 0;
+	bool is_s32;
+
+	dprintk("cobalt alsa announce ptr=%p data=%p num_bytes=%zd\n", cobsc,
+		pcm_data, samples);
+
+	substream = cobsc->capture_pcm_substream;
+	if (substream == NULL) {
+		dprintk("substream was NULL\n");
+		return;
+	}
+
+	runtime = substream->runtime;
+	if (runtime == NULL) {
+		dprintk("runtime was NULL\n");
+		return;
+	}
+	is_s32 = runtime->format == SNDRV_PCM_FORMAT_S32_LE;
+
+	stride = runtime->frame_bits >> 3;
+	if (stride == 0) {
+		dprintk("stride is zero\n");
+		return;
+	}
+
+	if (length == 0) {
+		dprintk("%s: length was zero\n", __func__);
+		return;
+	}
+
+	if (runtime->dma_area == NULL) {
+		dprintk("dma area was NULL - ignoring\n");
+		return;
+	}
+
+	oldptr = cobsc->hwptr_done_capture;
+	if (oldptr + length >= runtime->buffer_size) {
+		unsigned int cnt = runtime->buffer_size - oldptr;
+		unsigned i;
+
+		for (i = 0; i < cnt; i++)
+			sample_cpy(runtime->dma_area + (oldptr + i) * stride,
+					pcm_data + i * skip,
+					stride, is_s32);
+		for (i = cnt; i < length; i++)
+			sample_cpy(runtime->dma_area + (i - cnt) * stride,
+					pcm_data + i * skip, stride, is_s32);
+	} else {
+		unsigned i;
+
+		for (i = 0; i < length; i++)
+			sample_cpy(runtime->dma_area + (oldptr + i) * stride,
+					pcm_data + i * skip,
+					stride, is_s32);
+	}
+	snd_pcm_stream_lock_irqsave(substream, flags);
+
+	cobsc->hwptr_done_capture += length;
+	if (cobsc->hwptr_done_capture >=
+	    runtime->buffer_size)
+		cobsc->hwptr_done_capture -=
+			runtime->buffer_size;
+
+	cobsc->capture_transfer_done += length;
+	if (cobsc->capture_transfer_done >=
+	    runtime->period_size) {
+		cobsc->capture_transfer_done -=
+			runtime->period_size;
+		period_elapsed = 1;
+	}
+
+	snd_pcm_stream_unlock_irqrestore(substream, flags);
+
+	if (period_elapsed)
+		snd_pcm_period_elapsed(substream);
+}
+
+static int alsa_fnc(struct vb2_buffer *vb, void *priv)
+{
+	struct cobalt_stream *s = priv;
+	unsigned char *p = vb2_plane_vaddr(vb, 0);
+	int i;
+
+	if (pcm_debug) {
+		pr_info("alsa: ");
+		for (i = 0; i < 8 * 4; i++) {
+			if (!(i & 3))
+				pr_cont(" ");
+			pr_cont("%02x", p[i]);
+		}
+		pr_cont("\n");
+	}
+	cobalt_alsa_announce_pcm_data(s->alsa,
+			vb2_plane_vaddr(vb, 0),
+			8 * 4,
+			vb2_get_plane_payload(vb, 0) / (8 * 4));
+	return 0;
+}
+
+static int snd_cobalt_pcm_capture_open(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+	struct cobalt_stream *s = cobsc->s;
+
+	runtime->hw = snd_cobalt_hdmi_capture;
+	snd_pcm_hw_constraint_integer(runtime, SNDRV_PCM_HW_PARAM_PERIODS);
+	cobsc->capture_pcm_substream = substream;
+	runtime->private_data = s;
+	cobsc->alsa_record_cnt++;
+	if (cobsc->alsa_record_cnt == 1) {
+		int rc;
+
+		rc = vb2_thread_start(&s->q, alsa_fnc, s, s->vdev.name);
+		if (rc) {
+			cobsc->alsa_record_cnt--;
+			return rc;
+		}
+	}
+	return 0;
+}
+
+static int snd_cobalt_pcm_capture_close(struct snd_pcm_substream *substream)
+{
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+	struct cobalt_stream *s = cobsc->s;
+
+	cobsc->alsa_record_cnt--;
+	if (cobsc->alsa_record_cnt == 0)
+		vb2_thread_stop(&s->q);
+	return 0;
+}
+
+static int snd_cobalt_pcm_ioctl(struct snd_pcm_substream *substream,
+		     unsigned int cmd, void *arg)
+{
+	return snd_pcm_lib_ioctl(substream, cmd, arg);
+}
+
+
+static int snd_pcm_alloc_vmalloc_buffer(struct snd_pcm_substream *subs,
+					size_t size)
+{
+	struct snd_pcm_runtime *runtime = subs->runtime;
+
+	dprintk("Allocating vbuffer\n");
+	if (runtime->dma_area) {
+		if (runtime->dma_bytes > size)
+			return 0;
+
+		vfree(runtime->dma_area);
+	}
+	runtime->dma_area = vmalloc(size);
+	if (!runtime->dma_area)
+		return -ENOMEM;
+
+	runtime->dma_bytes = size;
+
+	return 0;
+}
+
+static int snd_cobalt_pcm_hw_params(struct snd_pcm_substream *substream,
+			 struct snd_pcm_hw_params *params)
+{
+	dprintk("%s called\n", __func__);
+
+	return snd_pcm_alloc_vmalloc_buffer(substream,
+					   params_buffer_bytes(params));
+}
+
+static int snd_cobalt_pcm_hw_free(struct snd_pcm_substream *substream)
+{
+	if (substream->runtime->dma_area) {
+		dprintk("freeing pcm capture region\n");
+		vfree(substream->runtime->dma_area);
+		substream->runtime->dma_area = NULL;
+	}
+
+	return 0;
+}
+
+static int snd_cobalt_pcm_prepare(struct snd_pcm_substream *substream)
+{
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+
+	cobsc->hwptr_done_capture = 0;
+	cobsc->capture_transfer_done = 0;
+
+	return 0;
+}
+
+static int snd_cobalt_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
+{
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+	case SNDRV_PCM_TRIGGER_STOP:
+		return 0;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static
+snd_pcm_uframes_t snd_cobalt_pcm_pointer(struct snd_pcm_substream *substream)
+{
+	snd_pcm_uframes_t hwptr_done;
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+
+	hwptr_done = cobsc->hwptr_done_capture;
+
+	return hwptr_done;
+}
+
+static void pb_sample_cpy(u8 *dst, const u8 *src, u32 len, bool is_s32)
+{
+	static const unsigned map[8] = { 0, 1, 5, 4, 2, 3, 6, 7 };
+	unsigned idx = 0;
+
+	while (len >= (is_s32 ? 4 : 2)) {
+		unsigned offset = map[idx] * 4;
+		u8 *out = dst + offset;
+
+		*out++ = 0;
+		if (is_s32) {
+			src++;
+			*out++ = *src++;
+		} else {
+			*out++ = 0;
+		}
+		*out++ = *src++;
+		*out = *src++;
+		len -= is_s32 ? 4 : 2;
+		idx++;
+	}
+}
+
+static void cobalt_alsa_pb_pcm_data(struct snd_cobalt_card *cobsc,
+					u8 *pcm_data,
+					size_t skip,
+					size_t samples)
+{
+	struct snd_pcm_substream *substream;
+	struct snd_pcm_runtime *runtime;
+	unsigned long flags;
+	unsigned int pos;
+	unsigned int stride;
+	bool is_s32;
+	unsigned i;
+
+	dprintk("cobalt alsa pb ptr=%p data=%p samples=%zd\n", cobsc,
+		pcm_data, samples);
+
+	substream = cobsc->playback_pcm_substream;
+	if (substream == NULL) {
+		dprintk("substream was NULL\n");
+		return;
+	}
+
+	runtime = substream->runtime;
+	if (runtime == NULL) {
+		dprintk("runtime was NULL\n");
+		return;
+	}
+
+	is_s32 = runtime->format == SNDRV_PCM_FORMAT_S32_LE;
+	stride = runtime->frame_bits >> 3;
+	if (stride == 0) {
+		dprintk("stride is zero\n");
+		return;
+	}
+
+	if (samples == 0) {
+		dprintk("%s: samples was zero\n", __func__);
+		return;
+	}
+
+	if (runtime->dma_area == NULL) {
+		dprintk("dma area was NULL - ignoring\n");
+		return;
+	}
+
+	pos = cobsc->pb_pos % cobsc->pb_size;
+	for (i = 0; i < cobsc->pb_count / (8 * 4); i++)
+		pb_sample_cpy(pcm_data + i * skip,
+				runtime->dma_area + pos + i * stride,
+				stride, is_s32);
+	snd_pcm_stream_lock_irqsave(substream, flags);
+
+	cobsc->pb_pos += i * stride;
+
+	snd_pcm_stream_unlock_irqrestore(substream, flags);
+	if (cobsc->pb_pos % cobsc->pb_count == 0)
+		snd_pcm_period_elapsed(substream);
+}
+
+static int alsa_pb_fnc(struct vb2_buffer *vb, void *priv)
+{
+	struct cobalt_stream *s = priv;
+
+	if (s->alsa->alsa_pb_channel)
+		cobalt_alsa_pb_pcm_data(s->alsa,
+				vb2_plane_vaddr(vb, 0),
+				8 * 4,
+				vb2_get_plane_payload(vb, 0) / (8 * 4));
+	return 0;
+}
+
+static int snd_cobalt_pcm_playback_open(struct snd_pcm_substream *substream)
+{
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct cobalt_stream *s = cobsc->s;
+
+	runtime->hw = snd_cobalt_playback;
+	snd_pcm_hw_constraint_integer(runtime, SNDRV_PCM_HW_PARAM_PERIODS);
+	cobsc->playback_pcm_substream = substream;
+	runtime->private_data = s;
+	cobsc->alsa_playback_cnt++;
+	if (cobsc->alsa_playback_cnt == 1) {
+		int rc;
+
+		rc = vb2_thread_start(&s->q, alsa_pb_fnc, s, s->vdev.name);
+		if (rc) {
+			cobsc->alsa_playback_cnt--;
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+static int snd_cobalt_pcm_playback_close(struct snd_pcm_substream *substream)
+{
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+	struct cobalt_stream *s = cobsc->s;
+
+	cobsc->alsa_playback_cnt--;
+	if (cobsc->alsa_playback_cnt == 0)
+		vb2_thread_stop(&s->q);
+	return 0;
+}
+
+static int snd_cobalt_pcm_pb_prepare(struct snd_pcm_substream *substream)
+{
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+
+	cobsc->pb_size = snd_pcm_lib_buffer_bytes(substream);
+	cobsc->pb_count = snd_pcm_lib_period_bytes(substream);
+	cobsc->pb_pos = 0;
+
+	return 0;
+}
+
+static int snd_cobalt_pcm_pb_trigger(struct snd_pcm_substream *substream,
+				     int cmd)
+{
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+		if (cobsc->alsa_pb_channel)
+			return -EBUSY;
+		cobsc->alsa_pb_channel = true;
+		return 0;
+	case SNDRV_PCM_TRIGGER_STOP:
+		cobsc->alsa_pb_channel = false;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static
+snd_pcm_uframes_t snd_cobalt_pcm_pb_pointer(struct snd_pcm_substream *substream)
+{
+	struct snd_cobalt_card *cobsc = snd_pcm_substream_chip(substream);
+	size_t ptr;
+
+	ptr = cobsc->pb_pos;
+
+	return bytes_to_frames(substream->runtime, ptr) %
+	       substream->runtime->buffer_size;
+}
+
+static struct page *snd_pcm_get_vmalloc_page(struct snd_pcm_substream *subs,
+					     unsigned long offset)
+{
+	void *pageptr = subs->runtime->dma_area + offset;
+
+	return vmalloc_to_page(pageptr);
+}
+
+static struct snd_pcm_ops snd_cobalt_pcm_capture_ops = {
+	.open		= snd_cobalt_pcm_capture_open,
+	.close		= snd_cobalt_pcm_capture_close,
+	.ioctl		= snd_cobalt_pcm_ioctl,
+	.hw_params	= snd_cobalt_pcm_hw_params,
+	.hw_free	= snd_cobalt_pcm_hw_free,
+	.prepare	= snd_cobalt_pcm_prepare,
+	.trigger	= snd_cobalt_pcm_trigger,
+	.pointer	= snd_cobalt_pcm_pointer,
+	.page		= snd_pcm_get_vmalloc_page,
+};
+
+static struct snd_pcm_ops snd_cobalt_pcm_playback_ops = {
+	.open		= snd_cobalt_pcm_playback_open,
+	.close		= snd_cobalt_pcm_playback_close,
+	.ioctl		= snd_cobalt_pcm_ioctl,
+	.hw_params	= snd_cobalt_pcm_hw_params,
+	.hw_free	= snd_cobalt_pcm_hw_free,
+	.prepare	= snd_cobalt_pcm_pb_prepare,
+	.trigger	= snd_cobalt_pcm_pb_trigger,
+	.pointer	= snd_cobalt_pcm_pb_pointer,
+	.page		= snd_pcm_get_vmalloc_page,
+};
+
+int snd_cobalt_pcm_create(struct snd_cobalt_card *cobsc)
+{
+	struct snd_pcm *sp;
+	struct snd_card *sc = cobsc->sc;
+	struct cobalt_stream *s = cobsc->s;
+	struct cobalt *cobalt = s->cobalt;
+	int ret;
+
+	s->q.gfp_flags |= __GFP_ZERO;
+
+	if (!s->is_output) {
+		cobalt_s_bit_sysctrl(cobalt,
+			COBALT_SYS_CTRL_AUDIO_IPP_RESETN_BIT(s->video_channel),
+			0);
+		mdelay(2);
+		cobalt_s_bit_sysctrl(cobalt,
+			COBALT_SYS_CTRL_AUDIO_IPP_RESETN_BIT(s->video_channel),
+			1);
+		mdelay(1);
+
+		ret = snd_pcm_new(sc, "Cobalt PCM-In HDMI",
+			0, /* PCM device 0, the only one for this card */
+			0, /* 0 playback substreams */
+			1, /* 1 capture substream */
+			&sp);
+		if (ret) {
+			cobalt_err("snd_cobalt_pcm_create() failed for input with err %d\n",
+				   ret);
+			goto err_exit;
+		}
+
+		snd_pcm_set_ops(sp, SNDRV_PCM_STREAM_CAPTURE,
+				&snd_cobalt_pcm_capture_ops);
+		sp->info_flags = 0;
+		sp->private_data = cobsc;
+		strlcpy(sp->name, "cobalt", sizeof(sp->name));
+	} else {
+		cobalt_s_bit_sysctrl(cobalt,
+			COBALT_SYS_CTRL_AUDIO_OPP_RESETN_BIT, 0);
+		mdelay(2);
+		cobalt_s_bit_sysctrl(cobalt,
+			COBALT_SYS_CTRL_AUDIO_OPP_RESETN_BIT, 1);
+		mdelay(1);
+
+		ret = snd_pcm_new(sc, "Cobalt PCM-Out HDMI",
+			0, /* PCM device 0, the only one for this card */
+			1, /* 0 playback substreams */
+			0, /* 1 capture substream */
+			&sp);
+		if (ret) {
+			cobalt_err("snd_cobalt_pcm_create() failed for output with err %d\n",
+				   ret);
+			goto err_exit;
+		}
+
+		snd_pcm_set_ops(sp, SNDRV_PCM_STREAM_PLAYBACK,
+				&snd_cobalt_pcm_playback_ops);
+		sp->info_flags = 0;
+		sp->private_data = cobsc;
+		strlcpy(sp->name, "cobalt", sizeof(sp->name));
+	}
+
+	return 0;
+
+err_exit:
+	return ret;
+}
diff --git a/drivers/media/pci/cobalt/cobalt-alsa-pcm.h b/drivers/media/pci/cobalt/cobalt-alsa-pcm.h
new file mode 100644
index 000000000000..513fb1f71794
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-alsa-pcm.h
@@ -0,0 +1,22 @@
+/*
+ *  ALSA PCM device for the
+ *  ALSA interface to cobalt PCM capture streams
+ *
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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.
+ */
+
+int snd_cobalt_pcm_create(struct snd_cobalt_card *cobsc);
diff --git a/drivers/media/pci/cobalt/cobalt-alsa.h b/drivers/media/pci/cobalt/cobalt-alsa.h
new file mode 100644
index 000000000000..08db699ced37
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-alsa.h
@@ -0,0 +1,41 @@
+/*
+ *  ALSA interface to cobalt PCM capture streams
+ *
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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.
+ */
+
+struct snd_card;
+
+struct snd_cobalt_card {
+	struct cobalt_stream *s;
+	struct snd_card *sc;
+	unsigned int capture_transfer_done;
+	unsigned int hwptr_done_capture;
+	unsigned alsa_record_cnt;
+	struct snd_pcm_substream *capture_pcm_substream;
+
+	unsigned int pb_size;
+	unsigned int pb_count;
+	unsigned int pb_pos;
+	unsigned pb_filled;
+	bool alsa_pb_channel;
+	unsigned alsa_playback_cnt;
+	struct snd_pcm_substream *playback_pcm_substream;
+};
+
+int cobalt_alsa_init(struct cobalt_stream *s);
+void cobalt_alsa_exit(struct cobalt_stream *s);
diff --git a/drivers/media/pci/cobalt/cobalt-cpld.c b/drivers/media/pci/cobalt/cobalt-cpld.c
new file mode 100644
index 000000000000..e83f5c9f7e7d
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-cpld.c
@@ -0,0 +1,341 @@
+/*
+ *  Cobalt CPLD functions
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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/delay.h>
+
+#include "cobalt-cpld.h"
+
+#define ADRS(offset) (COBALT_BUS_CPLD_BASE + offset)
+
+static u16 cpld_read(struct cobalt *cobalt, u32 offset)
+{
+	return cobalt_bus_read32(cobalt->bar1, ADRS(offset));
+}
+
+static void cpld_write(struct cobalt *cobalt, u32 offset, u16 val)
+{
+	return cobalt_bus_write32(cobalt->bar1, ADRS(offset), val);
+}
+
+static void cpld_info_ver3(struct cobalt *cobalt)
+{
+	u32 rd;
+	u32 tmp;
+
+	cobalt_info("CPLD System control register (read/write)\n");
+	cobalt_info("\t\tSystem control:  0x%04x (0x0f00)\n",
+		    cpld_read(cobalt, 0));
+	cobalt_info("CPLD Clock control register (read/write)\n");
+	cobalt_info("\t\tClock control:   0x%04x (0x0000)\n",
+		    cpld_read(cobalt, 0x04));
+	cobalt_info("CPLD HSMA Clk Osc register (read/write) - Must set wr trigger to load default values\n");
+	cobalt_info("\t\tRegister #7:\t0x%04x (0x0022)\n",
+		    cpld_read(cobalt, 0x08));
+	cobalt_info("\t\tRegister #8:\t0x%04x (0x0047)\n",
+		    cpld_read(cobalt, 0x0c));
+	cobalt_info("\t\tRegister #9:\t0x%04x (0x00fa)\n",
+		    cpld_read(cobalt, 0x10));
+	cobalt_info("\t\tRegister #10:\t0x%04x (0x0061)\n",
+		    cpld_read(cobalt, 0x14));
+	cobalt_info("\t\tRegister #11:\t0x%04x (0x001e)\n",
+		    cpld_read(cobalt, 0x18));
+	cobalt_info("\t\tRegister #12:\t0x%04x (0x0045)\n",
+		    cpld_read(cobalt, 0x1c));
+	cobalt_info("\t\tRegister #135:\t0x%04x\n",
+		    cpld_read(cobalt, 0x20));
+	cobalt_info("\t\tRegister #137:\t0x%04x\n",
+		    cpld_read(cobalt, 0x24));
+	cobalt_info("CPLD System status register (read only)\n");
+	cobalt_info("\t\tSystem status:  0x%04x\n",
+		    cpld_read(cobalt, 0x28));
+	cobalt_info("CPLD MAXII info register (read only)\n");
+	cobalt_info("\t\tBoard serial number:     0x%04x\n",
+		    cpld_read(cobalt, 0x2c));
+	cobalt_info("\t\tMAXII program revision:  0x%04x\n",
+		    cpld_read(cobalt, 0x30));
+	cobalt_info("CPLD temp and voltage ADT7411 registers (read only)\n");
+	cobalt_info("\t\tBoard temperature:  %u Celcius\n",
+		    cpld_read(cobalt, 0x34) / 4);
+	cobalt_info("\t\tFPGA temperature:   %u Celcius\n",
+		    cpld_read(cobalt, 0x38) / 4);
+	rd = cpld_read(cobalt, 0x3c);
+	tmp = (rd * 33 * 1000) / (483 * 10);
+	cobalt_info("\t\tVDD 3V3:      %u,%03uV\n", tmp / 1000, tmp % 1000);
+	rd = cpld_read(cobalt, 0x40);
+	tmp = (rd * 74 * 2197) / (27 * 1000);
+	cobalt_info("\t\tADC ch3 5V:   %u,%03uV\n", tmp / 1000, tmp % 1000);
+	rd = cpld_read(cobalt, 0x44);
+	tmp = (rd * 74 * 2197) / (47 * 1000);
+	cobalt_info("\t\tADC ch4 3V:   %u,%03uV\n", tmp / 1000, tmp % 1000);
+	rd = cpld_read(cobalt, 0x48);
+	tmp = (rd * 57 * 2197) / (47 * 1000);
+	cobalt_info("\t\tADC ch5 2V5:  %u,%03uV\n", tmp / 1000, tmp % 1000);
+	rd = cpld_read(cobalt, 0x4c);
+	tmp = (rd * 2197) / 1000;
+	cobalt_info("\t\tADC ch6 1V8:  %u,%03uV\n", tmp / 1000, tmp % 1000);
+	rd = cpld_read(cobalt, 0x50);
+	tmp = (rd * 2197) / 1000;
+	cobalt_info("\t\tADC ch7 1V5:  %u,%03uV\n", tmp / 1000, tmp % 1000);
+	rd = cpld_read(cobalt, 0x54);
+	tmp = (rd * 2197) / 1000;
+	cobalt_info("\t\tADC ch8 0V9:  %u,%03uV\n", tmp / 1000, tmp % 1000);
+}
+
+void cobalt_cpld_status(struct cobalt *cobalt)
+{
+	u32 rev = cpld_read(cobalt, 0x30);
+
+	switch (rev) {
+	case 3:
+	case 4:
+	case 5:
+		cpld_info_ver3(cobalt);
+		break;
+	default:
+		cobalt_info("CPLD revision %u is not supported!\n", rev);
+		break;
+	}
+}
+
+#define DCO_MIN 4850000000ULL
+#define DCO_MAX 5670000000ULL
+
+#define SI570_CLOCK_CTRL   0x04
+#define S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_WR_TRIGGER 0x200
+#define S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_RST_TRIGGER 0x100
+#define S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_FPGA_CTRL 0x80
+#define S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_EN 0x40
+
+#define SI570_REG7   0x08
+#define SI570_REG8   0x0c
+#define SI570_REG9   0x10
+#define SI570_REG10  0x14
+#define SI570_REG11  0x18
+#define SI570_REG12  0x1c
+#define SI570_REG135 0x20
+#define SI570_REG137 0x24
+
+struct multiplier {
+	unsigned mult, hsdiv, n1;
+};
+
+/* List all possible multipliers (= hsdiv * n1). There are lots of duplicates,
+   which are all removed in this list to keep the list as short as possible.
+   The values for hsdiv and n1 are the actual values, not the register values.
+ */
+static const struct multiplier multipliers[] = {
+	{    4,  4,   1 }, {    5,  5,   1 }, {    6,  6,   1 },
+	{    7,  7,   1 }, {    8,  4,   2 }, {    9,  9,   1 },
+	{   10,  5,   2 }, {   11, 11,   1 }, {   12,  6,   2 },
+	{   14,  7,   2 }, {   16,  4,   4 }, {   18,  9,   2 },
+	{   20,  5,   4 }, {   22, 11,   2 }, {   24,  4,   6 },
+	{   28,  7,   4 }, {   30,  5,   6 }, {   32,  4,   8 },
+	{   36,  6,   6 }, {   40,  4,  10 }, {   42,  7,   6 },
+	{   44, 11,   4 }, {   48,  4,  12 }, {   50,  5,  10 },
+	{   54,  9,   6 }, {   56,  4,  14 }, {   60,  5,  12 },
+	{   64,  4,  16 }, {   66, 11,   6 }, {   70,  5,  14 },
+	{   72,  4,  18 }, {   80,  4,  20 }, {   84,  6,  14 },
+	{   88, 11,   8 }, {   90,  5,  18 }, {   96,  4,  24 },
+	{   98,  7,  14 }, {  100,  5,  20 }, {  104,  4,  26 },
+	{  108,  6,  18 }, {  110, 11,  10 }, {  112,  4,  28 },
+	{  120,  4,  30 }, {  126,  7,  18 }, {  128,  4,  32 },
+	{  130,  5,  26 }, {  132, 11,  12 }, {  136,  4,  34 },
+	{  140,  5,  28 }, {  144,  4,  36 }, {  150,  5,  30 },
+	{  152,  4,  38 }, {  154, 11,  14 }, {  156,  6,  26 },
+	{  160,  4,  40 }, {  162,  9,  18 }, {  168,  4,  42 },
+	{  170,  5,  34 }, {  176, 11,  16 }, {  180,  5,  36 },
+	{  182,  7,  26 }, {  184,  4,  46 }, {  190,  5,  38 },
+	{  192,  4,  48 }, {  196,  7,  28 }, {  198, 11,  18 },
+	{  198,  9,  22 }, {  200,  4,  50 }, {  204,  6,  34 },
+	{  208,  4,  52 }, {  210,  5,  42 }, {  216,  4,  54 },
+	{  220, 11,  20 }, {  224,  4,  56 }, {  228,  6,  38 },
+	{  230,  5,  46 }, {  232,  4,  58 }, {  234,  9,  26 },
+	{  238,  7,  34 }, {  240,  4,  60 }, {  242, 11,  22 },
+	{  248,  4,  62 }, {  250,  5,  50 }, {  252,  6,  42 },
+	{  256,  4,  64 }, {  260,  5,  52 }, {  264, 11,  24 },
+	{  266,  7,  38 }, {  270,  5,  54 }, {  272,  4,  68 },
+	{  276,  6,  46 }, {  280,  4,  70 }, {  286, 11,  26 },
+	{  288,  4,  72 }, {  290,  5,  58 }, {  294,  7,  42 },
+	{  296,  4,  74 }, {  300,  5,  60 }, {  304,  4,  76 },
+	{  306,  9,  34 }, {  308, 11,  28 }, {  310,  5,  62 },
+	{  312,  4,  78 }, {  320,  4,  80 }, {  322,  7,  46 },
+	{  324,  6,  54 }, {  328,  4,  82 }, {  330, 11,  30 },
+	{  336,  4,  84 }, {  340,  5,  68 }, {  342,  9,  38 },
+	{  344,  4,  86 }, {  348,  6,  58 }, {  350,  5,  70 },
+	{  352, 11,  32 }, {  360,  4,  90 }, {  364,  7,  52 },
+	{  368,  4,  92 }, {  370,  5,  74 }, {  372,  6,  62 },
+	{  374, 11,  34 }, {  376,  4,  94 }, {  378,  7,  54 },
+	{  380,  5,  76 }, {  384,  4,  96 }, {  390,  5,  78 },
+	{  392,  4,  98 }, {  396, 11,  36 }, {  400,  4, 100 },
+	{  406,  7,  58 }, {  408,  4, 102 }, {  410,  5,  82 },
+	{  414,  9,  46 }, {  416,  4, 104 }, {  418, 11,  38 },
+	{  420,  5,  84 }, {  424,  4, 106 }, {  430,  5,  86 },
+	{  432,  4, 108 }, {  434,  7,  62 }, {  440, 11,  40 },
+	{  444,  6,  74 }, {  448,  4, 112 }, {  450,  5,  90 },
+	{  456,  4, 114 }, {  460,  5,  92 }, {  462, 11,  42 },
+	{  464,  4, 116 }, {  468,  6,  78 }, {  470,  5,  94 },
+	{  472,  4, 118 }, {  476,  7,  68 }, {  480,  4, 120 },
+	{  484, 11,  44 }, {  486,  9,  54 }, {  488,  4, 122 },
+	{  490,  5,  98 }, {  492,  6,  82 }, {  496,  4, 124 },
+	{  500,  5, 100 }, {  504,  4, 126 }, {  506, 11,  46 },
+	{  510,  5, 102 }, {  512,  4, 128 }, {  516,  6,  86 },
+	{  518,  7,  74 }, {  520,  5, 104 }, {  522,  9,  58 },
+	{  528, 11,  48 }, {  530,  5, 106 }, {  532,  7,  76 },
+	{  540,  5, 108 }, {  546,  7,  78 }, {  550, 11,  50 },
+	{  552,  6,  92 }, {  558,  9,  62 }, {  560,  5, 112 },
+	{  564,  6,  94 }, {  570,  5, 114 }, {  572, 11,  52 },
+	{  574,  7,  82 }, {  576,  6,  96 }, {  580,  5, 116 },
+	{  588,  6,  98 }, {  590,  5, 118 }, {  594, 11,  54 },
+	{  600,  5, 120 }, {  602,  7,  86 }, {  610,  5, 122 },
+	{  612,  6, 102 }, {  616, 11,  56 }, {  620,  5, 124 },
+	{  624,  6, 104 }, {  630,  5, 126 }, {  636,  6, 106 },
+	{  638, 11,  58 }, {  640,  5, 128 }, {  644,  7,  92 },
+	{  648,  6, 108 }, {  658,  7,  94 }, {  660, 11,  60 },
+	{  666,  9,  74 }, {  672,  6, 112 }, {  682, 11,  62 },
+	{  684,  6, 114 }, {  686,  7,  98 }, {  696,  6, 116 },
+	{  700,  7, 100 }, {  702,  9,  78 }, {  704, 11,  64 },
+	{  708,  6, 118 }, {  714,  7, 102 }, {  720,  6, 120 },
+	{  726, 11,  66 }, {  728,  7, 104 }, {  732,  6, 122 },
+	{  738,  9,  82 }, {  742,  7, 106 }, {  744,  6, 124 },
+	{  748, 11,  68 }, {  756,  6, 126 }, {  768,  6, 128 },
+	{  770, 11,  70 }, {  774,  9,  86 }, {  784,  7, 112 },
+	{  792, 11,  72 }, {  798,  7, 114 }, {  810,  9,  90 },
+	{  812,  7, 116 }, {  814, 11,  74 }, {  826,  7, 118 },
+	{  828,  9,  92 }, {  836, 11,  76 }, {  840,  7, 120 },
+	{  846,  9,  94 }, {  854,  7, 122 }, {  858, 11,  78 },
+	{  864,  9,  96 }, {  868,  7, 124 }, {  880, 11,  80 },
+	{  882,  7, 126 }, {  896,  7, 128 }, {  900,  9, 100 },
+	{  902, 11,  82 }, {  918,  9, 102 }, {  924, 11,  84 },
+	{  936,  9, 104 }, {  946, 11,  86 }, {  954,  9, 106 },
+	{  968, 11,  88 }, {  972,  9, 108 }, {  990, 11,  90 },
+	{ 1008,  9, 112 }, { 1012, 11,  92 }, { 1026,  9, 114 },
+	{ 1034, 11,  94 }, { 1044,  9, 116 }, { 1056, 11,  96 },
+	{ 1062,  9, 118 }, { 1078, 11,  98 }, { 1080,  9, 120 },
+	{ 1098,  9, 122 }, { 1100, 11, 100 }, { 1116,  9, 124 },
+	{ 1122, 11, 102 }, { 1134,  9, 126 }, { 1144, 11, 104 },
+	{ 1152,  9, 128 }, { 1166, 11, 106 }, { 1188, 11, 108 },
+	{ 1210, 11, 110 }, { 1232, 11, 112 }, { 1254, 11, 114 },
+	{ 1276, 11, 116 }, { 1298, 11, 118 }, { 1320, 11, 120 },
+	{ 1342, 11, 122 }, { 1364, 11, 124 }, { 1386, 11, 126 },
+	{ 1408, 11, 128 },
+};
+
+bool cobalt_cpld_set_freq(struct cobalt *cobalt, unsigned f_out)
+{
+	const unsigned f_xtal = 39170000;	/* xtal for si598 */
+	u64 dco;
+	u64 rfreq;
+	unsigned delta = 0xffffffff;
+	unsigned i_best = 0;
+	unsigned i;
+	u8 n1, hsdiv;
+	u8 regs[6];
+	int found = 0;
+	u16 clock_ctrl;
+	int retries = 3;
+
+	for (i = 0; i < ARRAY_SIZE(multipliers); i++) {
+		unsigned mult = multipliers[i].mult;
+		u32 d;
+
+		dco = (u64)f_out * mult;
+		if (dco < DCO_MIN || dco > DCO_MAX)
+			continue;
+		div_u64_rem((dco << 28) + f_xtal / 2, f_xtal, &d);
+		if (d < delta) {
+			found = 1;
+			i_best = i;
+			delta = d;
+		}
+	}
+	if (!found)
+		return false;
+	dco = (u64)f_out * multipliers[i_best].mult;
+	n1 = multipliers[i_best].n1 - 1;
+	hsdiv = multipliers[i_best].hsdiv - 4;
+	rfreq = div_u64(dco << 28, f_xtal);
+
+	clock_ctrl = cpld_read(cobalt, SI570_CLOCK_CTRL);
+	clock_ctrl |= S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_FPGA_CTRL;
+	clock_ctrl |= S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_EN;
+
+	regs[0] = (hsdiv << 5) | (n1 >> 2);
+	regs[1] = ((n1 & 0x3) << 6) | (rfreq >> 32);
+	regs[2] = (rfreq >> 24) & 0xff;
+	regs[3] = (rfreq >> 16) & 0xff;
+	regs[4] = (rfreq >> 8) & 0xff;
+	regs[5] = rfreq & 0xff;
+
+	/* The sequence of clock_ctrl flags to set is very weird. It looks
+	   like I have to reset it, then set the new frequency and reset it
+	   again. It shouldn't be necessary to do a reset, but if I don't,
+	   then a strange frequency is set (156.412034 MHz, or register values
+	   0x01, 0xc7, 0xfc, 0x7f, 0x53, 0x62).
+	 */
+
+	cobalt_dbg(1, "%u: %02x %02x %02x %02x %02x %02x\n", f_out,
+			regs[0], regs[1], regs[2], regs[3], regs[4], regs[5]);
+	while (retries--) {
+		u8 read_regs[6];
+
+		cpld_write(cobalt, SI570_CLOCK_CTRL,
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_EN |
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_FPGA_CTRL);
+		usleep_range(10000, 15000);
+		cpld_write(cobalt, SI570_REG7, regs[0]);
+		cpld_write(cobalt, SI570_REG8, regs[1]);
+		cpld_write(cobalt, SI570_REG9, regs[2]);
+		cpld_write(cobalt, SI570_REG10, regs[3]);
+		cpld_write(cobalt, SI570_REG11, regs[4]);
+		cpld_write(cobalt, SI570_REG12, regs[5]);
+		cpld_write(cobalt, SI570_CLOCK_CTRL,
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_EN |
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_WR_TRIGGER);
+		usleep_range(10000, 15000);
+		cpld_write(cobalt, SI570_CLOCK_CTRL,
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_EN |
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_FPGA_CTRL);
+		usleep_range(10000, 15000);
+		read_regs[0] = cpld_read(cobalt, SI570_REG7);
+		read_regs[1] = cpld_read(cobalt, SI570_REG8);
+		read_regs[2] = cpld_read(cobalt, SI570_REG9);
+		read_regs[3] = cpld_read(cobalt, SI570_REG10);
+		read_regs[4] = cpld_read(cobalt, SI570_REG11);
+		read_regs[5] = cpld_read(cobalt, SI570_REG12);
+		cpld_write(cobalt, SI570_CLOCK_CTRL,
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_EN |
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_FPGA_CTRL |
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_RST_TRIGGER);
+		usleep_range(10000, 15000);
+		cpld_write(cobalt, SI570_CLOCK_CTRL,
+			S01755_REG_CLOCK_CTRL_BITMAP_CLKHSMA_EN);
+		usleep_range(10000, 15000);
+
+		if (!memcmp(read_regs, regs, sizeof(read_regs)))
+			break;
+		cobalt_dbg(1, "retry: %02x %02x %02x %02x %02x %02x\n",
+			read_regs[0], read_regs[1], read_regs[2],
+			read_regs[3], read_regs[4], read_regs[5]);
+	}
+	if (2 - retries)
+		cobalt_info("Needed %d retries\n", 2 - retries);
+
+	return true;
+}
diff --git a/drivers/media/pci/cobalt/cobalt-cpld.h b/drivers/media/pci/cobalt/cobalt-cpld.h
new file mode 100644
index 000000000000..0fc88fd5fa7b
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-cpld.h
@@ -0,0 +1,29 @@
+/*
+ *  Cobalt CPLD functions
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef COBALT_CPLD_H
+#define COBALT_CPLD_H
+
+#include "cobalt-driver.h"
+
+void cobalt_cpld_status(struct cobalt *cobalt);
+bool cobalt_cpld_set_freq(struct cobalt *cobalt, unsigned freq);
+
+#endif
diff --git a/drivers/media/pci/cobalt/cobalt-driver.c b/drivers/media/pci/cobalt/cobalt-driver.c
new file mode 100644
index 000000000000..b994b8efdc99
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-driver.c
@@ -0,0 +1,832 @@
+/*
+ *  cobalt driver initialization and card probing
+ *
+ *  Derived from cx18-driver.c
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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/delay.h>
+#include <media/adv7604.h>
+#include <media/adv7842.h>
+#include <media/adv7511.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ctrls.h>
+
+#include "cobalt-driver.h"
+#include "cobalt-irq.h"
+#include "cobalt-i2c.h"
+#include "cobalt-v4l2.h"
+#include "cobalt-flash.h"
+#include "cobalt-alsa.h"
+#include "cobalt-omnitek.h"
+
+/* add your revision and whatnot here */
+static struct pci_device_id cobalt_pci_tbl[] = {
+	{PCI_VENDOR_ID_CISCO, PCI_DEVICE_ID_COBALT,
+	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, cobalt_pci_tbl);
+
+static atomic_t cobalt_instance = ATOMIC_INIT(0);
+
+int cobalt_debug;
+module_param_named(debug, cobalt_debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug level. Default: 0\n");
+
+int cobalt_ignore_err;
+module_param_named(ignore_err, cobalt_ignore_err, int, 0644);
+MODULE_PARM_DESC(ignore_err,
+	"If set then ignore missing i2c adapters/receivers. Default: 0\n");
+
+MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com> & Morten Hestnes");
+MODULE_DESCRIPTION("cobalt driver");
+MODULE_LICENSE("GPL");
+
+static u8 edid[256] = {
+	0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00,
+	0x50, 0x21, 0x9C, 0x27, 0x00, 0x00, 0x00, 0x00,
+	0x19, 0x12, 0x01, 0x03, 0x80, 0x00, 0x00, 0x78,
+	0x0E, 0x00, 0xB2, 0xA0, 0x57, 0x49, 0x9B, 0x26,
+	0x10, 0x48, 0x4F, 0x2F, 0xCF, 0x00, 0x31, 0x59,
+	0x45, 0x59, 0x61, 0x59, 0x81, 0x99, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x3A,
+	0x80, 0x18, 0x71, 0x38, 0x2D, 0x40, 0x58, 0x2C,
+	0x46, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1E,
+	0x00, 0x00, 0x00, 0xFD, 0x00, 0x31, 0x55, 0x18,
+	0x5E, 0x11, 0x00, 0x0A, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x43,
+	0x20, 0x39, 0x30, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A,
+	0x0A, 0x0A, 0x0A, 0x0A, 0x00, 0x00, 0x00, 0x10,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x68,
+	0x02, 0x03, 0x1a, 0xc0, 0x48, 0xa2, 0x10, 0x04,
+	0x02, 0x01, 0x21, 0x14, 0x13, 0x23, 0x09, 0x07,
+	0x07, 0x65, 0x03, 0x0c, 0x00, 0x10, 0x00, 0xe2,
+	0x00, 0x2a, 0x01, 0x1d, 0x00, 0x80, 0x51, 0xd0,
+	0x1c, 0x20, 0x40, 0x80, 0x35, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x1e, 0x8c, 0x0a, 0xd0, 0x8a,
+	0x20, 0xe0, 0x2d, 0x10, 0x10, 0x3e, 0x96, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd7
+};
+
+static void cobalt_set_interrupt(struct cobalt *cobalt, bool enable)
+{
+	if (enable) {
+		unsigned irqs = COBALT_SYSSTAT_VI0_INT1_MSK |
+				COBALT_SYSSTAT_VI1_INT1_MSK |
+				COBALT_SYSSTAT_VI2_INT1_MSK |
+				COBALT_SYSSTAT_VI3_INT1_MSK |
+				COBALT_SYSSTAT_VI0_INT2_MSK |
+				COBALT_SYSSTAT_VI1_INT2_MSK |
+				COBALT_SYSSTAT_VI2_INT2_MSK |
+				COBALT_SYSSTAT_VI3_INT2_MSK |
+				COBALT_SYSSTAT_VI0_LOST_DATA_MSK |
+				COBALT_SYSSTAT_VI1_LOST_DATA_MSK |
+				COBALT_SYSSTAT_VI2_LOST_DATA_MSK |
+				COBALT_SYSSTAT_VI3_LOST_DATA_MSK |
+				COBALT_SYSSTAT_AUD_IN_LOST_DATA_MSK;
+
+		if (cobalt->have_hsma_rx)
+			irqs |= COBALT_SYSSTAT_VIHSMA_INT1_MSK |
+				COBALT_SYSSTAT_VIHSMA_INT2_MSK |
+				COBALT_SYSSTAT_VIHSMA_LOST_DATA_MSK;
+
+		if (cobalt->have_hsma_tx)
+			irqs |= COBALT_SYSSTAT_VOHSMA_INT1_MSK |
+				COBALT_SYSSTAT_VOHSMA_LOST_DATA_MSK |
+				COBALT_SYSSTAT_AUD_OUT_LOST_DATA_MSK;
+		/* Clear any existing interrupts */
+		cobalt_write_bar1(cobalt, COBALT_SYS_STAT_EDGE, 0xffffffff);
+		/* PIO Core interrupt mask register.
+		   Enable ADV7604 INT1 interrupts */
+		cobalt_write_bar1(cobalt, COBALT_SYS_STAT_MASK, irqs);
+	} else {
+		/* Disable all ADV7604 interrupts */
+		cobalt_write_bar1(cobalt, COBALT_SYS_STAT_MASK, 0);
+	}
+}
+
+static unsigned cobalt_get_sd_nr(struct v4l2_subdev *sd)
+{
+	struct cobalt *cobalt = to_cobalt(sd->v4l2_dev);
+	unsigned i;
+
+	for (i = 0; i < COBALT_NUM_NODES; i++)
+		if (sd == cobalt->streams[i].sd)
+			return i;
+	cobalt_err("Invalid adv7604 subdev pointer!\n");
+	return 0;
+}
+
+static void cobalt_notify(struct v4l2_subdev *sd,
+			  unsigned int notification, void *arg)
+{
+	struct cobalt *cobalt = to_cobalt(sd->v4l2_dev);
+	unsigned sd_nr = cobalt_get_sd_nr(sd);
+	struct cobalt_stream *s = &cobalt->streams[sd_nr];
+	bool hotplug = arg ? *((int *)arg) : false;
+
+	if (s->is_output)
+		return;
+
+	switch (notification) {
+	case ADV76XX_HOTPLUG:
+		cobalt_s_bit_sysctrl(cobalt,
+			COBALT_SYS_CTRL_HPD_TO_CONNECTOR_BIT(sd_nr), hotplug);
+		cobalt_dbg(1, "Set hotplug for adv %d to %d\n", sd_nr, hotplug);
+		break;
+	case V4L2_DEVICE_NOTIFY_EVENT:
+		cobalt_dbg(1, "Format changed for adv %d\n", sd_nr);
+		v4l2_event_queue(&s->vdev, arg);
+		break;
+	default:
+		break;
+	}
+}
+
+static int get_payload_size(u16 code)
+{
+	switch (code) {
+	case 0: return 128;
+	case 1: return 256;
+	case 2: return 512;
+	case 3: return 1024;
+	case 4: return 2048;
+	case 5: return 4096;
+	default: return 0;
+	}
+	return 0;
+}
+
+static const char *get_link_speed(u16 stat)
+{
+	switch (stat & PCI_EXP_LNKSTA_CLS) {
+	case 1:	return "2.5 Gbit/s";
+	case 2:	return "5 Gbit/s";
+	case 3:	return "10 Gbit/s";
+	}
+	return "Unknown speed";
+}
+
+void cobalt_pcie_status_show(struct cobalt *cobalt)
+{
+	struct pci_dev *pci_dev = cobalt->pci_dev;
+	struct pci_dev *pci_bus_dev = cobalt->pci_dev->bus->self;
+	int offset;
+	int bus_offset;
+	u32 capa;
+	u16 stat, ctrl;
+
+	offset = pci_find_capability(pci_dev, PCI_CAP_ID_EXP);
+	bus_offset = pci_find_capability(pci_bus_dev, PCI_CAP_ID_EXP);
+
+	/* Device */
+	pci_read_config_dword(pci_dev, offset + PCI_EXP_DEVCAP, &capa);
+	pci_read_config_word(pci_dev, offset + PCI_EXP_DEVCTL, &ctrl);
+	pci_read_config_word(pci_dev, offset + PCI_EXP_DEVSTA, &stat);
+	cobalt_info("PCIe device capability 0x%08x: Max payload %d\n",
+		    capa, get_payload_size(capa & PCI_EXP_DEVCAP_PAYLOAD));
+	cobalt_info("PCIe device control 0x%04x: Max payload %d. Max read request %d\n",
+		    ctrl,
+		    get_payload_size((ctrl & PCI_EXP_DEVCTL_PAYLOAD) >> 5),
+		    get_payload_size((ctrl & PCI_EXP_DEVCTL_READRQ) >> 12));
+	cobalt_info("PCIe device status 0x%04x\n", stat);
+
+	/* Link */
+	pci_read_config_dword(pci_dev, offset + PCI_EXP_LNKCAP, &capa);
+	pci_read_config_word(pci_dev, offset + PCI_EXP_LNKCTL, &ctrl);
+	pci_read_config_word(pci_dev, offset + PCI_EXP_LNKSTA, &stat);
+	cobalt_info("PCIe link capability 0x%08x: %s per lane and %u lanes\n",
+			capa, get_link_speed(capa),
+			(capa & PCI_EXP_LNKCAP_MLW) >> 4);
+	cobalt_info("PCIe link control 0x%04x\n", ctrl);
+	cobalt_info("PCIe link status 0x%04x: %s per lane and %u lanes\n",
+		    stat, get_link_speed(stat),
+		    (stat & PCI_EXP_LNKSTA_NLW) >> 4);
+
+	/* Bus */
+	pci_read_config_dword(pci_bus_dev, bus_offset + PCI_EXP_LNKCAP, &capa);
+	cobalt_info("PCIe bus link capability 0x%08x: %s per lane and %u lanes\n",
+			capa, get_link_speed(capa),
+			(capa & PCI_EXP_LNKCAP_MLW) >> 4);
+
+	/* Slot */
+	pci_read_config_dword(pci_dev, offset + PCI_EXP_SLTCAP, &capa);
+	pci_read_config_word(pci_dev, offset + PCI_EXP_SLTCTL, &ctrl);
+	pci_read_config_word(pci_dev, offset + PCI_EXP_SLTSTA, &stat);
+	cobalt_info("PCIe slot capability 0x%08x\n", capa);
+	cobalt_info("PCIe slot control 0x%04x\n", ctrl);
+	cobalt_info("PCIe slot status 0x%04x\n", stat);
+}
+
+static unsigned pcie_link_get_lanes(struct cobalt *cobalt)
+{
+	struct pci_dev *pci_dev = cobalt->pci_dev;
+	unsigned offset;
+	u16 link;
+
+	offset = pci_find_capability(pci_dev, PCI_CAP_ID_EXP);
+	if (!offset)
+		return 0;
+	pci_read_config_word(pci_dev, offset + PCI_EXP_LNKSTA, &link);
+	return (link & PCI_EXP_LNKSTA_NLW) >> 4;
+}
+
+static unsigned pcie_bus_link_get_lanes(struct cobalt *cobalt)
+{
+	struct pci_dev *pci_dev = cobalt->pci_dev->bus->self;
+	unsigned offset;
+	u32 link;
+
+	offset = pci_find_capability(pci_dev, PCI_CAP_ID_EXP);
+	if (!offset)
+		return 0;
+	pci_read_config_dword(pci_dev, offset + PCI_EXP_LNKCAP, &link);
+	return (link & PCI_EXP_LNKCAP_MLW) >> 4;
+}
+
+static void msi_config_show(struct cobalt *cobalt, struct pci_dev *pci_dev)
+{
+	u16 ctrl, data;
+	u32 adrs_l, adrs_h;
+
+	pci_read_config_word(pci_dev, 0x52, &ctrl);
+	cobalt_info("MSI %s\n", ctrl & 1 ? "enable" : "disable");
+	cobalt_info("MSI multiple message: Capable %u. Enable %u\n",
+		    (1 << ((ctrl >> 1) & 7)), (1 << ((ctrl >> 4) & 7)));
+	if (ctrl & 0x80)
+		cobalt_info("MSI: 64-bit address capable\n");
+	pci_read_config_dword(pci_dev, 0x54, &adrs_l);
+	pci_read_config_dword(pci_dev, 0x58, &adrs_h);
+	pci_read_config_word(pci_dev, 0x5c, &data);
+	if (ctrl & 0x80)
+		cobalt_info("MSI: Address 0x%08x%08x. Data 0x%04x\n",
+				adrs_h, adrs_l, data);
+	else
+		cobalt_info("MSI: Address 0x%08x. Data 0x%04x\n",
+				adrs_l, data);
+}
+
+static void cobalt_pci_iounmap(struct cobalt *cobalt, struct pci_dev *pci_dev)
+{
+	if (cobalt->bar0) {
+		pci_iounmap(pci_dev, cobalt->bar0);
+		cobalt->bar0 = NULL;
+	}
+	if (cobalt->bar1) {
+		pci_iounmap(pci_dev, cobalt->bar1);
+		cobalt->bar1 = NULL;
+	}
+}
+
+static void cobalt_free_msi(struct cobalt *cobalt, struct pci_dev *pci_dev)
+{
+	free_irq(pci_dev->irq, (void *)cobalt);
+
+	if (cobalt->msi_enabled)
+		pci_disable_msi(pci_dev);
+}
+
+static int cobalt_setup_pci(struct cobalt *cobalt, struct pci_dev *pci_dev,
+			    const struct pci_device_id *pci_id)
+{
+	u32 ctrl;
+	int ret;
+
+	cobalt_dbg(1, "enabling pci device\n");
+
+	ret = pci_enable_device(pci_dev);
+	if (ret) {
+		cobalt_err("can't enable device\n");
+		return ret;
+	}
+	pci_set_master(pci_dev);
+	pci_read_config_byte(pci_dev, PCI_CLASS_REVISION, &cobalt->card_rev);
+	pci_read_config_word(pci_dev, PCI_DEVICE_ID, &cobalt->device_id);
+
+	switch (cobalt->device_id) {
+	case PCI_DEVICE_ID_COBALT:
+		cobalt_info("PCI Express interface from Omnitek\n");
+		break;
+	default:
+		cobalt_info("PCI Express interface provider is unknown!\n");
+		break;
+	}
+
+	if (pcie_link_get_lanes(cobalt) != 8) {
+		cobalt_err("PCI Express link width is not 8 lanes (%d)\n",
+				pcie_link_get_lanes(cobalt));
+		if (pcie_bus_link_get_lanes(cobalt) < 8)
+			cobalt_err("The current slot only supports %d lanes, at least 8 are needed\n",
+					pcie_bus_link_get_lanes(cobalt));
+		else
+			cobalt_err("The card is most likely not seated correctly in the PCIe slot\n");
+		ret = -EIO;
+		goto err_disable;
+	}
+
+	if (pci_set_dma_mask(pci_dev, DMA_BIT_MASK(64))) {
+		ret = pci_set_dma_mask(pci_dev, DMA_BIT_MASK(32));
+		if (ret) {
+			cobalt_err("no suitable DMA available\n");
+			goto err_disable;
+		}
+	}
+
+	ret = pci_request_regions(pci_dev, "cobalt");
+	if (ret) {
+		cobalt_err("error requesting regions\n");
+		goto err_disable;
+	}
+
+	cobalt_pcie_status_show(cobalt);
+
+	cobalt->bar0 = pci_iomap(pci_dev, 0, 0);
+	cobalt->bar1 = pci_iomap(pci_dev, 1, 0);
+	if (cobalt->bar1 == NULL) {
+		cobalt->bar1 = pci_iomap(pci_dev, 2, 0);
+		cobalt_info("64-bit BAR\n");
+	}
+	if (!cobalt->bar0 || !cobalt->bar1) {
+		ret = -EIO;
+		goto err_release;
+	}
+
+	/* Reset the video inputs before enabling any interrupts */
+	ctrl = cobalt_read_bar1(cobalt, COBALT_SYS_CTRL_BASE);
+	cobalt_write_bar1(cobalt, COBALT_SYS_CTRL_BASE, ctrl & ~0xf00);
+
+	/* Disable interrupts to prevent any spurious interrupts
+	   from being generated. */
+	cobalt_set_interrupt(cobalt, false);
+
+	if (pci_enable_msi_range(pci_dev, 1, 1) < 1) {
+		cobalt_err("Could not enable MSI\n");
+		cobalt->msi_enabled = false;
+		ret = -EIO;
+		goto err_release;
+	}
+	msi_config_show(cobalt, pci_dev);
+	cobalt->msi_enabled = true;
+
+	/* Register IRQ */
+	if (request_irq(pci_dev->irq, cobalt_irq_handler, IRQF_SHARED,
+			cobalt->v4l2_dev.name, (void *)cobalt)) {
+		cobalt_err("Failed to register irq %d\n", pci_dev->irq);
+		ret = -EIO;
+		goto err_msi;
+	}
+
+	omni_sg_dma_init(cobalt);
+	return 0;
+
+err_msi:
+	pci_disable_msi(pci_dev);
+
+err_release:
+	cobalt_pci_iounmap(cobalt, pci_dev);
+	pci_release_regions(pci_dev);
+
+err_disable:
+	pci_disable_device(cobalt->pci_dev);
+	return ret;
+}
+
+static int cobalt_hdl_info_get(struct cobalt *cobalt)
+{
+	int i;
+
+	for (i = 0; i < COBALT_HDL_INFO_SIZE; i++)
+		cobalt->hdl_info[i] =
+			ioread8(cobalt->bar1 + COBALT_HDL_INFO_BASE + i);
+	cobalt->hdl_info[COBALT_HDL_INFO_SIZE - 1] = '\0';
+	if (strstr(cobalt->hdl_info, COBALT_HDL_SEARCH_STR))
+		return 0;
+
+	return 1;
+}
+
+static void cobalt_stream_struct_init(struct cobalt *cobalt)
+{
+	int i;
+
+	for (i = 0; i < COBALT_NUM_STREAMS; i++) {
+		struct cobalt_stream *s = &cobalt->streams[i];
+
+		s->cobalt = cobalt;
+		s->flags = 0;
+		s->is_audio = false;
+		s->is_output = false;
+		s->is_dummy = true;
+
+		/* The Memory DMA channels will always get a lower channel
+		 * number than the FIFO DMA. Video input should map to the
+		 * stream 0-3. The other can use stream struct from 4 and
+		 * higher */
+		if (i <= COBALT_HSMA_IN_NODE) {
+			s->dma_channel = i + cobalt->first_fifo_channel;
+			s->video_channel = i;
+			s->dma_fifo_mask =
+				COBALT_SYSSTAT_VI0_LOST_DATA_MSK << (4 * i);
+			s->adv_irq_mask =
+				COBALT_SYSSTAT_VI0_INT1_MSK << (4 * i);
+		} else if (i >= COBALT_AUDIO_IN_STREAM &&
+			   i <= COBALT_AUDIO_IN_STREAM + 4) {
+			unsigned idx = i - COBALT_AUDIO_IN_STREAM;
+
+			s->dma_channel = 6 + idx;
+			s->is_audio = true;
+			s->video_channel = idx;
+			s->dma_fifo_mask = COBALT_SYSSTAT_AUD_IN_LOST_DATA_MSK;
+		} else if (i == COBALT_HSMA_OUT_NODE) {
+			s->dma_channel = 11;
+			s->is_output = true;
+			s->video_channel = 5;
+			s->dma_fifo_mask = COBALT_SYSSTAT_VOHSMA_LOST_DATA_MSK;
+			s->adv_irq_mask = COBALT_SYSSTAT_VOHSMA_INT1_MSK;
+		} else if (i == COBALT_AUDIO_OUT_STREAM) {
+			s->dma_channel = 12;
+			s->is_audio = true;
+			s->is_output = true;
+			s->video_channel = 5;
+			s->dma_fifo_mask = COBALT_SYSSTAT_AUD_OUT_LOST_DATA_MSK;
+		} else {
+			/* FIXME: Memory DMA for debug purpose */
+			s->dma_channel = i - COBALT_NUM_NODES;
+		}
+		cobalt_info("stream #%d -> dma channel #%d <- video channel %d\n",
+			    i, s->dma_channel, s->video_channel);
+	}
+}
+
+static int cobalt_subdevs_init(struct cobalt *cobalt)
+{
+	static struct adv76xx_platform_data adv7604_pdata = {
+		.disable_pwrdnb = 1,
+		.ain_sel = ADV7604_AIN7_8_9_NC_SYNC_3_1,
+		.bus_order = ADV7604_BUS_ORDER_BRG,
+		.blank_data = 1,
+		.op_656_range = 1,
+		.op_format_mode_sel = ADV7604_OP_FORMAT_MODE0,
+		.int1_config = ADV76XX_INT1_CONFIG_ACTIVE_HIGH,
+		.dr_str_data = ADV76XX_DR_STR_HIGH,
+		.dr_str_clk = ADV76XX_DR_STR_HIGH,
+		.dr_str_sync = ADV76XX_DR_STR_HIGH,
+		.hdmi_free_run_mode = 1,
+		.inv_vs_pol = 1,
+		.inv_hs_pol = 1,
+	};
+	static struct i2c_board_info adv7604_info = {
+		.type = "adv7604",
+		.addr = 0x20,
+		.platform_data = &adv7604_pdata,
+	};
+
+	struct cobalt_stream *s = cobalt->streams;
+	int i;
+
+	for (i = 0; i < COBALT_NUM_INPUTS; i++) {
+		struct v4l2_subdev_format sd_fmt = {
+			.pad = ADV7604_PAD_SOURCE,
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+			.format.code = MEDIA_BUS_FMT_YUYV8_1X16,
+		};
+		struct v4l2_subdev_edid cobalt_edid = {
+			.pad = ADV76XX_PAD_HDMI_PORT_A,
+			.start_block = 0,
+			.blocks = 2,
+			.edid = edid,
+		};
+		int err;
+
+		s[i].pad_source = ADV7604_PAD_SOURCE;
+		s[i].i2c_adap = &cobalt->i2c_adap[i];
+		if (s[i].i2c_adap->dev.parent == NULL)
+			continue;
+		cobalt_s_bit_sysctrl(cobalt,
+				COBALT_SYS_CTRL_NRESET_TO_HDMI_BIT(i), 1);
+		s[i].sd = v4l2_i2c_new_subdev_board(&cobalt->v4l2_dev,
+			s[i].i2c_adap, &adv7604_info, NULL);
+		if (!s[i].sd) {
+			if (cobalt_ignore_err)
+				continue;
+			return -ENODEV;
+		}
+		err = v4l2_subdev_call(s[i].sd, video, s_routing,
+				ADV76XX_PAD_HDMI_PORT_A, 0, 0);
+		if (err)
+			return err;
+		err = v4l2_subdev_call(s[i].sd, pad, set_edid,
+				&cobalt_edid);
+		if (err)
+			return err;
+		err = v4l2_subdev_call(s[i].sd, pad, set_fmt, NULL,
+				&sd_fmt);
+		if (err)
+			return err;
+		/* Reset channel video module */
+		cobalt_s_bit_sysctrl(cobalt,
+				COBALT_SYS_CTRL_VIDEO_RX_RESETN_BIT(i), 0);
+		mdelay(2);
+		cobalt_s_bit_sysctrl(cobalt,
+				COBALT_SYS_CTRL_VIDEO_RX_RESETN_BIT(i), 1);
+		mdelay(1);
+		s[i].is_dummy = false;
+		cobalt->streams[i + COBALT_AUDIO_IN_STREAM].is_dummy = false;
+	}
+	return 0;
+}
+
+static int cobalt_subdevs_hsma_init(struct cobalt *cobalt)
+{
+	static struct adv7842_platform_data adv7842_pdata = {
+		.disable_pwrdnb = 1,
+		.ain_sel = ADV7842_AIN1_2_3_NC_SYNC_1_2,
+		.bus_order = ADV7842_BUS_ORDER_RBG,
+		.op_format_mode_sel = ADV7842_OP_FORMAT_MODE0,
+		.blank_data = 1,
+		.op_656_range = 1,
+		.dr_str_data = 3,
+		.dr_str_clk = 3,
+		.dr_str_sync = 3,
+		.mode = ADV7842_MODE_HDMI,
+		.hdmi_free_run_enable = 1,
+		.vid_std_select = ADV7842_HDMI_COMP_VID_STD_HD_1250P,
+		.i2c_sdp_io = 0x4a,
+		.i2c_sdp = 0x48,
+		.i2c_cp = 0x22,
+		.i2c_vdp = 0x24,
+		.i2c_afe = 0x26,
+		.i2c_hdmi = 0x34,
+		.i2c_repeater = 0x32,
+		.i2c_edid = 0x36,
+		.i2c_infoframe = 0x3e,
+		.i2c_cec = 0x40,
+		.i2c_avlink = 0x42,
+	};
+	static struct i2c_board_info adv7842_info = {
+		.type = "adv7842",
+		.addr = 0x20,
+		.platform_data = &adv7842_pdata,
+	};
+	static struct v4l2_subdev_format sd_fmt = {
+		.pad = ADV7842_PAD_SOURCE,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format.code = MEDIA_BUS_FMT_YUYV8_1X16,
+	};
+	static struct adv7511_platform_data adv7511_pdata = {
+		.i2c_edid = 0x7e >> 1,
+		.i2c_cec = 0x7c >> 1,
+		.i2c_pktmem = 0x70 >> 1,
+		.cec_clk = 12000000,
+	};
+	static struct i2c_board_info adv7511_info = {
+		.type = "adv7511",
+		.addr = 0x39, /* 0x39 or 0x3d */
+		.platform_data = &adv7511_pdata,
+	};
+	struct v4l2_subdev_edid cobalt_edid = {
+		.pad = ADV7842_EDID_PORT_A,
+		.start_block = 0,
+		.blocks = 2,
+		.edid = edid,
+	};
+	struct cobalt_stream *s = &cobalt->streams[COBALT_HSMA_IN_NODE];
+
+	s->i2c_adap = &cobalt->i2c_adap[COBALT_NUM_ADAPTERS - 1];
+	if (s->i2c_adap->dev.parent == NULL)
+		return 0;
+	cobalt_s_bit_sysctrl(cobalt, COBALT_SYS_CTRL_NRESET_TO_HDMI_BIT(4), 1);
+
+	s->sd = v4l2_i2c_new_subdev_board(&cobalt->v4l2_dev,
+			s->i2c_adap, &adv7842_info, NULL);
+	if (s->sd) {
+		int err = v4l2_subdev_call(s->sd, pad, set_edid, &cobalt_edid);
+
+		if (err)
+			return err;
+		err = v4l2_subdev_call(s->sd, pad, set_fmt, NULL,
+				&sd_fmt);
+		if (err)
+			return err;
+		cobalt->have_hsma_rx = true;
+		s->pad_source = ADV7842_PAD_SOURCE;
+		s->is_dummy = false;
+		cobalt->streams[4 + COBALT_AUDIO_IN_STREAM].is_dummy = false;
+		/* Reset channel video module */
+		cobalt_s_bit_sysctrl(cobalt,
+				COBALT_SYS_CTRL_VIDEO_RX_RESETN_BIT(4), 0);
+		mdelay(2);
+		cobalt_s_bit_sysctrl(cobalt,
+				COBALT_SYS_CTRL_VIDEO_RX_RESETN_BIT(4), 1);
+		mdelay(1);
+		return err;
+	}
+	cobalt_s_bit_sysctrl(cobalt, COBALT_SYS_CTRL_NRESET_TO_HDMI_BIT(4), 0);
+	cobalt_s_bit_sysctrl(cobalt, COBALT_SYS_CTRL_PWRDN0_TO_HSMA_TX_BIT, 0);
+	s++;
+	s->i2c_adap = &cobalt->i2c_adap[COBALT_NUM_ADAPTERS - 1];
+	s->sd = v4l2_i2c_new_subdev_board(&cobalt->v4l2_dev,
+			s->i2c_adap, &adv7511_info, NULL);
+	if (s->sd) {
+		/* A transmitter is hooked up, so we can set this bit */
+		cobalt_s_bit_sysctrl(cobalt,
+				COBALT_SYS_CTRL_HSMA_TX_ENABLE_BIT, 1);
+		cobalt_s_bit_sysctrl(cobalt,
+				COBALT_SYS_CTRL_VIDEO_RX_RESETN_BIT(4), 0);
+		cobalt_s_bit_sysctrl(cobalt,
+				COBALT_SYS_CTRL_VIDEO_TX_RESETN_BIT, 1);
+		cobalt->have_hsma_tx = true;
+		v4l2_subdev_call(s->sd, core, s_power, 1);
+		v4l2_subdev_call(s->sd, video, s_stream, 1);
+		v4l2_subdev_call(s->sd, audio, s_stream, 1);
+		v4l2_ctrl_s_ctrl(v4l2_ctrl_find(s->sd->ctrl_handler,
+				 V4L2_CID_DV_TX_MODE), V4L2_DV_TX_MODE_HDMI);
+		s->is_dummy = false;
+		cobalt->streams[COBALT_AUDIO_OUT_STREAM].is_dummy = false;
+		return 0;
+	}
+	return -ENODEV;
+}
+
+static int cobalt_probe(struct pci_dev *pci_dev,
+				  const struct pci_device_id *pci_id)
+{
+	struct cobalt *cobalt;
+	int retval = 0;
+	int i;
+
+	/* FIXME - module parameter arrays constrain max instances */
+	i = atomic_inc_return(&cobalt_instance) - 1;
+
+	cobalt = kzalloc(sizeof(struct cobalt), GFP_ATOMIC);
+	if (cobalt == NULL)
+		return -ENOMEM;
+	cobalt->pci_dev = pci_dev;
+	cobalt->instance = i;
+
+	cobalt->alloc_ctx = vb2_dma_sg_init_ctx(&pci_dev->dev);
+	if (IS_ERR(cobalt->alloc_ctx)) {
+		kfree(cobalt);
+		return -ENOMEM;
+	}
+
+	retval = v4l2_device_register(&pci_dev->dev, &cobalt->v4l2_dev);
+	if (retval) {
+		pr_err("cobalt: v4l2_device_register of card %d failed\n",
+				cobalt->instance);
+		vb2_dma_sg_cleanup_ctx(cobalt->alloc_ctx);
+		kfree(cobalt);
+		return retval;
+	}
+	snprintf(cobalt->v4l2_dev.name, sizeof(cobalt->v4l2_dev.name),
+		 "cobalt-%d", cobalt->instance);
+	cobalt->v4l2_dev.notify = cobalt_notify;
+	cobalt_info("Initializing card %d\n", cobalt->instance);
+
+	cobalt->irq_work_queues =
+		create_singlethread_workqueue(cobalt->v4l2_dev.name);
+	if (cobalt->irq_work_queues == NULL) {
+		cobalt_err("Could not create workqueue\n");
+		retval = -ENOMEM;
+		goto err;
+	}
+
+	INIT_WORK(&cobalt->irq_work_queue, cobalt_irq_work_handler);
+
+	/* PCI Device Setup */
+	retval = cobalt_setup_pci(cobalt, pci_dev, pci_id);
+	if (retval != 0)
+		goto err_wq;
+
+	/* Show HDL version info */
+	if (cobalt_hdl_info_get(cobalt))
+		cobalt_info("Not able to read the HDL info\n");
+	else
+		cobalt_info("%s", cobalt->hdl_info);
+
+	retval = cobalt_i2c_init(cobalt);
+	if (retval)
+		goto err_pci;
+
+	cobalt_stream_struct_init(cobalt);
+
+	retval = cobalt_subdevs_init(cobalt);
+	if (retval)
+		goto err_i2c;
+
+	if (!(cobalt_read_bar1(cobalt, COBALT_SYS_STAT_BASE) &
+			COBALT_SYSSTAT_HSMA_PRSNTN_MSK)) {
+		retval = cobalt_subdevs_hsma_init(cobalt);
+		if (retval)
+			goto err_i2c;
+	}
+
+	retval = v4l2_device_register_subdev_nodes(&cobalt->v4l2_dev);
+	if (retval)
+		goto err_i2c;
+	retval = cobalt_nodes_register(cobalt);
+	if (retval) {
+		cobalt_err("Error %d registering device nodes\n", retval);
+		goto err_i2c;
+	}
+	cobalt_set_interrupt(cobalt, true);
+	v4l2_device_call_all(&cobalt->v4l2_dev, 0, core,
+					interrupt_service_routine, 0, NULL);
+
+	cobalt_info("Initialized cobalt card\n");
+
+	cobalt_flash_probe(cobalt);
+
+	return 0;
+
+err_i2c:
+	cobalt_i2c_exit(cobalt);
+	cobalt_s_bit_sysctrl(cobalt, COBALT_SYS_CTRL_HSMA_TX_ENABLE_BIT, 0);
+err_pci:
+	cobalt_free_msi(cobalt, pci_dev);
+	cobalt_pci_iounmap(cobalt, pci_dev);
+	pci_release_regions(cobalt->pci_dev);
+	pci_disable_device(cobalt->pci_dev);
+err_wq:
+	destroy_workqueue(cobalt->irq_work_queues);
+err:
+	if (retval == 0)
+		retval = -ENODEV;
+	cobalt_err("error %d on initialization\n", retval);
+
+	v4l2_device_unregister(&cobalt->v4l2_dev);
+	vb2_dma_sg_cleanup_ctx(cobalt->alloc_ctx);
+	kfree(cobalt);
+	return retval;
+}
+
+static void cobalt_remove(struct pci_dev *pci_dev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+	struct cobalt *cobalt = to_cobalt(v4l2_dev);
+	int i;
+
+	cobalt_flash_remove(cobalt);
+	cobalt_set_interrupt(cobalt, false);
+	flush_workqueue(cobalt->irq_work_queues);
+	cobalt_nodes_unregister(cobalt);
+	for (i = 0; i < COBALT_NUM_ADAPTERS; i++) {
+		struct v4l2_subdev *sd = cobalt->streams[i].sd;
+		struct i2c_client *client;
+
+		if (sd == NULL)
+			continue;
+		client = v4l2_get_subdevdata(sd);
+		v4l2_device_unregister_subdev(sd);
+		i2c_unregister_device(client);
+	}
+	cobalt_i2c_exit(cobalt);
+	cobalt_free_msi(cobalt, pci_dev);
+	cobalt_s_bit_sysctrl(cobalt, COBALT_SYS_CTRL_HSMA_TX_ENABLE_BIT, 0);
+	cobalt_pci_iounmap(cobalt, pci_dev);
+	pci_release_regions(cobalt->pci_dev);
+	pci_disable_device(cobalt->pci_dev);
+	destroy_workqueue(cobalt->irq_work_queues);
+
+	cobalt_info("removed cobalt card\n");
+
+	v4l2_device_unregister(v4l2_dev);
+	vb2_dma_sg_cleanup_ctx(cobalt->alloc_ctx);
+	kfree(cobalt);
+}
+
+/* define a pci_driver for card detection */
+static struct pci_driver cobalt_pci_driver = {
+	.name =     "cobalt",
+	.id_table = cobalt_pci_tbl,
+	.probe =    cobalt_probe,
+	.remove =   cobalt_remove,
+};
+
+module_pci_driver(cobalt_pci_driver);
diff --git a/drivers/media/pci/cobalt/cobalt-driver.h b/drivers/media/pci/cobalt/cobalt-driver.h
new file mode 100644
index 000000000000..c206df930669
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-driver.h
@@ -0,0 +1,380 @@
+/*
+ *  cobalt driver internal defines and structures
+ *
+ *  Derived from cx18-driver.h
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef COBALT_DRIVER_H
+#define COBALT_DRIVER_H
+
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+#include <linux/i2c.h>
+#include <linux/list.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "m00233_video_measure_memmap_package.h"
+#include "m00235_fdma_packer_memmap_package.h"
+#include "m00389_cvi_memmap_package.h"
+#include "m00460_evcnt_memmap_package.h"
+#include "m00473_freewheel_memmap_package.h"
+#include "m00479_clk_loss_detector_memmap_package.h"
+#include "m00514_syncgen_flow_evcnt_memmap_package.h"
+
+/* System device ID */
+#define PCI_DEVICE_ID_COBALT	0x2732
+
+/* Number of cobalt device nodes. */
+#define COBALT_NUM_INPUTS	4
+#define COBALT_NUM_NODES	6
+
+/* Number of cobalt device streams. */
+#define COBALT_NUM_STREAMS	12
+
+#define COBALT_HSMA_IN_NODE	4
+#define COBALT_HSMA_OUT_NODE	5
+
+/* Cobalt audio streams */
+#define COBALT_AUDIO_IN_STREAM	6
+#define COBALT_AUDIO_OUT_STREAM 11
+
+/* DMA stuff */
+#define DMA_CHANNELS_MAX	16
+
+/* i2c stuff */
+#define I2C_CLIENTS_MAX		16
+#define COBALT_NUM_ADAPTERS	5
+
+#define COBALT_CLK		50000000
+
+/* System status register */
+#define COBALT_SYSSTAT_DIP0_MSK			(1 << 0)
+#define COBALT_SYSSTAT_DIP1_MSK			(1 << 1)
+#define COBALT_SYSSTAT_HSMA_PRSNTN_MSK		(1 << 2)
+#define COBALT_SYSSTAT_FLASH_RDYBSYN_MSK	(1 << 3)
+#define COBALT_SYSSTAT_VI0_5V_MSK		(1 << 4)
+#define COBALT_SYSSTAT_VI0_INT1_MSK		(1 << 5)
+#define COBALT_SYSSTAT_VI0_INT2_MSK		(1 << 6)
+#define COBALT_SYSSTAT_VI0_LOST_DATA_MSK	(1 << 7)
+#define COBALT_SYSSTAT_VI1_5V_MSK		(1 << 8)
+#define COBALT_SYSSTAT_VI1_INT1_MSK		(1 << 9)
+#define COBALT_SYSSTAT_VI1_INT2_MSK		(1 << 10)
+#define COBALT_SYSSTAT_VI1_LOST_DATA_MSK	(1 << 11)
+#define COBALT_SYSSTAT_VI2_5V_MSK		(1 << 12)
+#define COBALT_SYSSTAT_VI2_INT1_MSK		(1 << 13)
+#define COBALT_SYSSTAT_VI2_INT2_MSK		(1 << 14)
+#define COBALT_SYSSTAT_VI2_LOST_DATA_MSK	(1 << 15)
+#define COBALT_SYSSTAT_VI3_5V_MSK		(1 << 16)
+#define COBALT_SYSSTAT_VI3_INT1_MSK		(1 << 17)
+#define COBALT_SYSSTAT_VI3_INT2_MSK		(1 << 18)
+#define COBALT_SYSSTAT_VI3_LOST_DATA_MSK	(1 << 19)
+#define COBALT_SYSSTAT_VIHSMA_5V_MSK		(1 << 20)
+#define COBALT_SYSSTAT_VIHSMA_INT1_MSK		(1 << 21)
+#define COBALT_SYSSTAT_VIHSMA_INT2_MSK		(1 << 22)
+#define COBALT_SYSSTAT_VIHSMA_LOST_DATA_MSK	(1 << 23)
+#define COBALT_SYSSTAT_VOHSMA_INT1_MSK		(1 << 24)
+#define COBALT_SYSSTAT_VOHSMA_PLL_LOCKED_MSK	(1 << 25)
+#define COBALT_SYSSTAT_VOHSMA_LOST_DATA_MSK	(1 << 26)
+#define COBALT_SYSSTAT_AUD_PLL_LOCKED_MSK	(1 << 28)
+#define COBALT_SYSSTAT_AUD_IN_LOST_DATA_MSK	(1 << 29)
+#define COBALT_SYSSTAT_AUD_OUT_LOST_DATA_MSK	(1 << 30)
+#define COBALT_SYSSTAT_PCIE_SMBCLK_MSK		(1 << 31)
+
+/* Cobalt memory map */
+#define COBALT_I2C_0_BASE			0x0
+#define COBALT_I2C_1_BASE			0x080
+#define COBALT_I2C_2_BASE			0x100
+#define COBALT_I2C_3_BASE			0x180
+#define COBALT_I2C_HSMA_BASE			0x200
+
+#define COBALT_SYS_CTRL_BASE			0x400
+#define COBALT_SYS_CTRL_HSMA_TX_ENABLE_BIT	1
+#define COBALT_SYS_CTRL_VIDEO_RX_RESETN_BIT(n)	(4 + 4 * (n))
+#define COBALT_SYS_CTRL_NRESET_TO_HDMI_BIT(n)	(5 + 4 * (n))
+#define COBALT_SYS_CTRL_HPD_TO_CONNECTOR_BIT(n)	(6 + 4 * (n))
+#define COBALT_SYS_CTRL_AUDIO_IPP_RESETN_BIT(n)	(7 + 4 * (n))
+#define COBALT_SYS_CTRL_PWRDN0_TO_HSMA_TX_BIT	24
+#define COBALT_SYS_CTRL_VIDEO_TX_RESETN_BIT	25
+#define COBALT_SYS_CTRL_AUDIO_OPP_RESETN_BIT	27
+
+#define COBALT_SYS_STAT_BASE			0x500
+#define COBALT_SYS_STAT_MASK			(COBALT_SYS_STAT_BASE + 0x08)
+#define COBALT_SYS_STAT_EDGE			(COBALT_SYS_STAT_BASE + 0x0c)
+
+#define COBALT_HDL_INFO_BASE			0x4800
+#define COBALT_HDL_INFO_SIZE			0x200
+
+#define COBALT_VID_BASE				0x10000
+#define COBALT_VID_SIZE				0x1000
+
+#define COBALT_CVI(cobalt, c) \
+	(cobalt->bar1 + COBALT_VID_BASE + (c) * COBALT_VID_SIZE)
+#define COBALT_CVI_VMR(cobalt, c) \
+	(cobalt->bar1 + COBALT_VID_BASE + (c) * COBALT_VID_SIZE + 0x100)
+#define COBALT_CVI_EVCNT(cobalt, c) \
+	(cobalt->bar1 + COBALT_VID_BASE + (c) * COBALT_VID_SIZE + 0x200)
+#define COBALT_CVI_FREEWHEEL(cobalt, c) \
+	(cobalt->bar1 + COBALT_VID_BASE + (c) * COBALT_VID_SIZE + 0x300)
+#define COBALT_CVI_CLK_LOSS(cobalt, c) \
+	(cobalt->bar1 + COBALT_VID_BASE + (c) * COBALT_VID_SIZE + 0x400)
+#define COBALT_CVI_PACKER(cobalt, c) \
+	(cobalt->bar1 + COBALT_VID_BASE + (c) * COBALT_VID_SIZE + 0x500)
+
+#define COBALT_TX_BASE(cobalt) (cobalt->bar1 + COBALT_VID_BASE + 0x5000)
+
+#define DMA_INTERRUPT_STATUS_REG		0x08
+
+#define COBALT_HDL_SEARCH_STR			"** HDL version info **"
+
+/* Cobalt CPU bus interface */
+#define COBALT_BUS_BAR1_BASE			0x600
+#define COBALT_BUS_SRAM_BASE			0x0
+#define COBALT_BUS_CPLD_BASE			0x00600000
+#define COBALT_BUS_FLASH_BASE			0x08000000
+
+/* FDMA to PCIe packing */
+#define COBALT_BYTES_PER_PIXEL_YUYV		2
+#define COBALT_BYTES_PER_PIXEL_RGB24		3
+#define COBALT_BYTES_PER_PIXEL_RGB32		4
+
+/* debugging */
+extern int cobalt_debug;
+extern int cobalt_ignore_err;
+
+#define cobalt_err(fmt, arg...)  v4l2_err(&cobalt->v4l2_dev, fmt, ## arg)
+#define cobalt_warn(fmt, arg...) v4l2_warn(&cobalt->v4l2_dev, fmt, ## arg)
+#define cobalt_info(fmt, arg...) v4l2_info(&cobalt->v4l2_dev, fmt, ## arg)
+#define cobalt_dbg(level, fmt, arg...) \
+	v4l2_dbg(level, cobalt_debug, &cobalt->v4l2_dev, fmt, ## arg)
+
+struct cobalt;
+struct cobalt_i2c_regs;
+
+/* Per I2C bus private algo callback data */
+struct cobalt_i2c_data {
+	struct cobalt *cobalt;
+	struct cobalt_i2c_regs __iomem *regs;
+};
+
+struct pci_consistent_buffer {
+	void *virt;
+	dma_addr_t bus;
+	size_t bytes;
+};
+
+struct sg_dma_desc_info {
+	void *virt;
+	dma_addr_t bus;
+	unsigned size;
+	void *last_desc_virt;
+	struct device *dev;
+};
+
+#define COBALT_MAX_WIDTH			1920
+#define COBALT_MAX_HEIGHT			1200
+#define COBALT_MAX_BPP				3
+#define COBALT_MAX_FRAMESZ \
+	(COBALT_MAX_WIDTH * COBALT_MAX_HEIGHT * COBALT_MAX_BPP)
+
+#define NR_BUFS					VIDEO_MAX_FRAME
+
+#define COBALT_STREAM_FL_DMA_IRQ		0
+#define COBALT_STREAM_FL_ADV_IRQ		1
+
+struct cobalt_buffer {
+	struct vb2_buffer vb;
+	struct list_head list;
+};
+
+static inline struct cobalt_buffer *to_cobalt_buffer(struct vb2_buffer *vb2)
+{
+	return container_of(vb2, struct cobalt_buffer, vb);
+}
+
+struct cobalt_stream {
+	struct video_device vdev;
+	struct vb2_queue q;
+	struct list_head bufs;
+	struct i2c_adapter *i2c_adap;
+	struct v4l2_subdev *sd;
+	struct mutex lock;
+	spinlock_t irqlock;
+	struct v4l2_dv_timings timings;
+	u32 input;
+	u32 pad_source;
+	u32 width, height, bpp;
+	u32 stride;
+	u32 pixfmt;
+	u32 sequence;
+	u32 colorspace;
+	u32 xfer_func;
+	u32 ycbcr_enc;
+	u32 quantization;
+
+	u8 dma_channel;
+	int video_channel;
+	unsigned dma_fifo_mask;
+	unsigned adv_irq_mask;
+	struct sg_dma_desc_info dma_desc_info[NR_BUFS];
+	unsigned long flags;
+	bool unstable_frame;
+	bool enable_cvi;
+	bool enable_freewheel;
+	unsigned skip_first_frames;
+	bool is_output;
+	bool is_audio;
+	bool is_dummy;
+
+	struct cobalt *cobalt;
+	struct snd_cobalt_card *alsa;
+};
+
+struct snd_cobalt_card;
+
+/* Struct to hold info about cobalt cards */
+struct cobalt {
+	int instance;
+	struct pci_dev *pci_dev;
+	struct v4l2_device v4l2_dev;
+	void *alloc_ctx;
+
+	void __iomem *bar0, *bar1;
+
+	u8 card_rev;
+	u16 device_id;
+
+	/* device nodes */
+	struct cobalt_stream streams[DMA_CHANNELS_MAX];
+	struct i2c_adapter i2c_adap[COBALT_NUM_ADAPTERS];
+	struct cobalt_i2c_data i2c_data[COBALT_NUM_ADAPTERS];
+	bool have_hsma_rx;
+	bool have_hsma_tx;
+
+	/* irq */
+	struct workqueue_struct *irq_work_queues;
+	struct work_struct irq_work_queue;              /* work entry */
+	/* irq counters */
+	u32 irq_adv1;
+	u32 irq_adv2;
+	u32 irq_advout;
+	u32 irq_dma_tot;
+	u32 irq_dma[COBALT_NUM_STREAMS];
+	u32 irq_none;
+	u32 irq_full_fifo;
+
+	bool msi_enabled;
+
+	/* omnitek dma */
+	int dma_channels;
+	int first_fifo_channel;
+	bool pci_32_bit;
+
+	char hdl_info[COBALT_HDL_INFO_SIZE];
+
+	/* NOR flash */
+	struct mtd_info *mtd;
+};
+
+static inline struct cobalt *to_cobalt(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct cobalt, v4l2_dev);
+}
+
+static inline void cobalt_write_bar0(struct cobalt *cobalt, u32 reg, u32 val)
+{
+	iowrite32(val, cobalt->bar0 + reg);
+}
+
+static inline u32 cobalt_read_bar0(struct cobalt *cobalt, u32 reg)
+{
+	return ioread32(cobalt->bar0 + reg);
+}
+
+static inline void cobalt_write_bar1(struct cobalt *cobalt, u32 reg, u32 val)
+{
+	iowrite32(val, cobalt->bar1 + reg);
+}
+
+static inline u32 cobalt_read_bar1(struct cobalt *cobalt, u32 reg)
+{
+	return ioread32(cobalt->bar1 + reg);
+}
+
+static inline u32 cobalt_g_sysctrl(struct cobalt *cobalt)
+{
+	return cobalt_read_bar1(cobalt, COBALT_SYS_CTRL_BASE);
+}
+
+static inline void cobalt_s_bit_sysctrl(struct cobalt *cobalt,
+					int bit, int val)
+{
+	u32 ctrl = cobalt_read_bar1(cobalt, COBALT_SYS_CTRL_BASE);
+
+	cobalt_write_bar1(cobalt, COBALT_SYS_CTRL_BASE,
+			(ctrl & ~(1UL << bit)) | (val << bit));
+}
+
+static inline u32 cobalt_g_sysstat(struct cobalt *cobalt)
+{
+	return cobalt_read_bar1(cobalt, COBALT_SYS_STAT_BASE);
+}
+
+#define ADRS_REG (bar1 + COBALT_BUS_BAR1_BASE + 0)
+#define LOWER_DATA (bar1 + COBALT_BUS_BAR1_BASE + 4)
+#define UPPER_DATA (bar1 + COBALT_BUS_BAR1_BASE + 6)
+
+static inline u32 cobalt_bus_read32(void __iomem *bar1, u32 bus_adrs)
+{
+	iowrite32(bus_adrs, ADRS_REG);
+	return ioread32(LOWER_DATA);
+}
+
+static inline void cobalt_bus_write16(void __iomem *bar1,
+				      u32 bus_adrs, u16 data)
+{
+	iowrite32(bus_adrs, ADRS_REG);
+	if (bus_adrs & 2)
+		iowrite16(data, UPPER_DATA);
+	else
+		iowrite16(data, LOWER_DATA);
+}
+
+static inline void cobalt_bus_write32(void __iomem *bar1,
+				      u32 bus_adrs, u16 data)
+{
+	iowrite32(bus_adrs, ADRS_REG);
+	if (bus_adrs & 2)
+		iowrite32(data, UPPER_DATA);
+	else
+		iowrite32(data, LOWER_DATA);
+}
+
+/*==============Prototypes==================*/
+
+void cobalt_pcie_status_show(struct cobalt *cobalt);
+
+#endif
diff --git a/drivers/media/pci/cobalt/cobalt-flash.c b/drivers/media/pci/cobalt/cobalt-flash.c
new file mode 100644
index 000000000000..04dcaf9198d2
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-flash.c
@@ -0,0 +1,128 @@
+/*
+ *  Cobalt NOR flash functions
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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/mtd/mtd.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/cfi.h>
+#include <linux/time.h>
+
+#include "cobalt-flash.h"
+
+#define ADRS(offset) (COBALT_BUS_FLASH_BASE + offset)
+
+static struct map_info cobalt_flash_map = {
+	.name =		"cobalt-flash",
+	.bankwidth =	2,         /* 16 bits */
+	.size =		0x4000000, /* 64MB */
+	.phys =		0,         /* offset  */
+};
+
+static map_word flash_read16(struct map_info *map, unsigned long offset)
+{
+	map_word r;
+
+	r.x[0] = cobalt_bus_read32(map->virt, ADRS(offset));
+	if (offset & 0x2)
+		r.x[0] >>= 16;
+	else
+		r.x[0] &= 0x0000ffff;
+
+	return r;
+}
+
+static void flash_write16(struct map_info *map, const map_word datum,
+			  unsigned long offset)
+{
+	u16 data = (u16)datum.x[0];
+
+	cobalt_bus_write16(map->virt, ADRS(offset), data);
+}
+
+static void flash_copy_from(struct map_info *map, void *to,
+			    unsigned long from, ssize_t len)
+{
+	u32 src = from;
+	u8 *dest = to;
+	u32 data;
+
+	while (len) {
+		data = cobalt_bus_read32(map->virt, ADRS(src));
+		do {
+			*dest = data >> (8 * (src & 3));
+			src++;
+			dest++;
+			len--;
+		} while (len && (src % 4));
+	}
+}
+
+static void flash_copy_to(struct map_info *map, unsigned long to,
+			  const void *from, ssize_t len)
+{
+	const u8 *src = from;
+	u32 dest = to;
+
+	pr_info("%s: offset 0x%x: length %zu\n", __func__, dest, len);
+	while (len) {
+		u16 data = 0xffff;
+
+		do {
+			data = *src << (8 * (dest & 1));
+			src++;
+			dest++;
+			len--;
+		} while (len && (dest % 2));
+
+		cobalt_bus_write16(map->virt, ADRS(dest - 2), data);
+	}
+}
+
+int cobalt_flash_probe(struct cobalt *cobalt)
+{
+	struct map_info *map = &cobalt_flash_map;
+	struct mtd_info *mtd;
+
+	BUG_ON(!map_bankwidth_supported(map->bankwidth));
+	map->virt = cobalt->bar1;
+	map->read = flash_read16;
+	map->write = flash_write16;
+	map->copy_from = flash_copy_from;
+	map->copy_to = flash_copy_to;
+
+	mtd = do_map_probe("cfi_probe", map);
+	cobalt->mtd = mtd;
+	if (!mtd) {
+		cobalt_err("Probe CFI flash failed!\n");
+		return -1;
+	}
+
+	mtd->owner = THIS_MODULE;
+	mtd->dev.parent = &cobalt->pci_dev->dev;
+	mtd_device_register(mtd, NULL, 0);
+	return 0;
+}
+
+void cobalt_flash_remove(struct cobalt *cobalt)
+{
+	if (cobalt->mtd) {
+		mtd_device_unregister(cobalt->mtd);
+		map_destroy(cobalt->mtd);
+	}
+}
diff --git a/drivers/media/pci/cobalt/cobalt-flash.h b/drivers/media/pci/cobalt/cobalt-flash.h
new file mode 100644
index 000000000000..8077daea51cd
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-flash.h
@@ -0,0 +1,29 @@
+/*
+ *  Cobalt NOR flash functions
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef COBALT_FLASH_H
+#define COBALT_FLASH_H
+
+#include "cobalt-driver.h"
+
+int cobalt_flash_probe(struct cobalt *cobalt);
+void cobalt_flash_remove(struct cobalt *cobalt);
+
+#endif
diff --git a/drivers/media/pci/cobalt/cobalt-i2c.c b/drivers/media/pci/cobalt/cobalt-i2c.c
new file mode 100644
index 000000000000..ad16b89b8d0c
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-i2c.c
@@ -0,0 +1,396 @@
+/*
+ *  cobalt I2C functions
+ *
+ *  Derived from cx18-i2c.c
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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 "cobalt-driver.h"
+#include "cobalt-i2c.h"
+
+struct cobalt_i2c_regs {
+	/* Clock prescaler register lo-byte */
+	u8 prerlo;
+	u8 dummy0[3];
+	/* Clock prescaler register high-byte */
+	u8 prerhi;
+	u8 dummy1[3];
+	/* Control register */
+	u8 ctr;
+	u8 dummy2[3];
+	/* Transmit/Receive register */
+	u8 txr_rxr;
+	u8 dummy3[3];
+	/* Command and Status register */
+	u8 cr_sr;
+	u8 dummy4[3];
+};
+
+/* CTR[7:0] - Control register */
+
+/* I2C Core enable bit */
+#define M00018_CTR_BITMAP_EN_MSK	(1 << 7)
+
+/* I2C Core interrupt enable bit */
+#define M00018_CTR_BITMAP_IEN_MSK	(1 << 6)
+
+/* CR[7:0] - Command register */
+
+/* I2C start condition */
+#define M00018_CR_BITMAP_STA_MSK	(1 << 7)
+
+/* I2C stop condition */
+#define M00018_CR_BITMAP_STO_MSK	(1 << 6)
+
+/* I2C read from slave */
+#define M00018_CR_BITMAP_RD_MSK		(1 << 5)
+
+/* I2C write to slave */
+#define M00018_CR_BITMAP_WR_MSK		(1 << 4)
+
+/* I2C ack */
+#define M00018_CR_BITMAP_ACK_MSK	(1 << 3)
+
+/* I2C Interrupt ack */
+#define M00018_CR_BITMAP_IACK_MSK	(1 << 0)
+
+/* SR[7:0] - Status register */
+
+/* Receive acknowledge from slave */
+#define M00018_SR_BITMAP_RXACK_MSK	(1 << 7)
+
+/* Busy, I2C bus busy (as defined by start / stop bits) */
+#define M00018_SR_BITMAP_BUSY_MSK	(1 << 6)
+
+/* Arbitration lost - core lost arbitration */
+#define M00018_SR_BITMAP_AL_MSK		(1 << 5)
+
+/* Transfer in progress */
+#define M00018_SR_BITMAP_TIP_MSK	(1 << 1)
+
+/* Interrupt flag */
+#define M00018_SR_BITMAP_IF_MSK		(1 << 0)
+
+/* Frequency, in Hz */
+#define I2C_FREQUENCY			400000
+#define ALT_CPU_FREQ			83333333
+
+static struct cobalt_i2c_regs __iomem *
+cobalt_i2c_regs(struct cobalt *cobalt, unsigned idx)
+{
+	switch (idx) {
+	case 0:
+	default:
+		return (struct cobalt_i2c_regs __iomem *)
+			(cobalt->bar1 + COBALT_I2C_0_BASE);
+	case 1:
+		return (struct cobalt_i2c_regs __iomem *)
+			(cobalt->bar1 + COBALT_I2C_1_BASE);
+	case 2:
+		return (struct cobalt_i2c_regs __iomem *)
+			(cobalt->bar1 + COBALT_I2C_2_BASE);
+	case 3:
+		return (struct cobalt_i2c_regs __iomem *)
+			(cobalt->bar1 + COBALT_I2C_3_BASE);
+	case 4:
+		return (struct cobalt_i2c_regs __iomem *)
+			(cobalt->bar1 + COBALT_I2C_HSMA_BASE);
+	}
+}
+
+/* Do low-level i2c byte transfer.
+ * Returns -1 in case of an error or 0 otherwise.
+ */
+static int cobalt_tx_bytes(struct cobalt_i2c_regs __iomem *regs,
+		struct i2c_adapter *adap, bool start, bool stop,
+		u8 *data, u16 len)
+{
+	unsigned long start_time;
+	int status;
+	int cmd;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		/* Setup data */
+		iowrite8(data[i], &regs->txr_rxr);
+
+		/* Setup command */
+		if (i == 0 && start != 0) {
+			/* Write + Start */
+			cmd = M00018_CR_BITMAP_WR_MSK |
+			      M00018_CR_BITMAP_STA_MSK;
+		} else if (i == len - 1 && stop != 0) {
+			/* Write + Stop */
+			cmd = M00018_CR_BITMAP_WR_MSK |
+			      M00018_CR_BITMAP_STO_MSK;
+		} else {
+			/* Write only */
+			cmd = M00018_CR_BITMAP_WR_MSK;
+		}
+
+		/* Execute command */
+		iowrite8(cmd, &regs->cr_sr);
+
+		/* Wait for transfer to complete (TIP = 0) */
+		start_time = jiffies;
+		status = ioread8(&regs->cr_sr);
+		while (status & M00018_SR_BITMAP_TIP_MSK) {
+			if (time_after(jiffies, start_time + adap->timeout))
+				return -ETIMEDOUT;
+			cond_resched();
+			status = ioread8(&regs->cr_sr);
+		}
+
+		/* Verify ACK */
+		if (status & M00018_SR_BITMAP_RXACK_MSK) {
+			/* NO ACK! */
+			return -EIO;
+		}
+
+		/* Verify arbitration */
+		if (status & M00018_SR_BITMAP_AL_MSK) {
+			/* Arbitration lost! */
+			return -EIO;
+		}
+	}
+	return 0;
+}
+
+/* Do low-level i2c byte read.
+ * Returns -1 in case of an error or 0 otherwise.
+ */
+static int cobalt_rx_bytes(struct cobalt_i2c_regs __iomem *regs,
+		struct i2c_adapter *adap, bool start, bool stop,
+		u8 *data, u16 len)
+{
+	unsigned long start_time;
+	int status;
+	int cmd;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		/* Setup command */
+		if (i == 0 && start != 0) {
+			/* Read + Start */
+			cmd = M00018_CR_BITMAP_RD_MSK |
+			      M00018_CR_BITMAP_STA_MSK;
+		} else if (i == len - 1 && stop != 0) {
+			/* Read + Stop */
+			cmd = M00018_CR_BITMAP_RD_MSK |
+			      M00018_CR_BITMAP_STO_MSK;
+		} else {
+			/* Read only */
+			cmd = M00018_CR_BITMAP_RD_MSK;
+		}
+
+		/* Last byte to read, no ACK */
+		if (i == len - 1)
+			cmd |= M00018_CR_BITMAP_ACK_MSK;
+
+		/* Execute command */
+		iowrite8(cmd, &regs->cr_sr);
+
+		/* Wait for transfer to complete (TIP = 0) */
+		start_time = jiffies;
+		status = ioread8(&regs->cr_sr);
+		while (status & M00018_SR_BITMAP_TIP_MSK) {
+			if (time_after(jiffies, start_time + adap->timeout))
+				return -ETIMEDOUT;
+			cond_resched();
+			status = ioread8(&regs->cr_sr);
+		}
+
+		/* Verify arbitration */
+		if (status & M00018_SR_BITMAP_AL_MSK) {
+			/* Arbitration lost! */
+			return -EIO;
+		}
+
+		/* Store data */
+		data[i] = ioread8(&regs->txr_rxr);
+	}
+	return 0;
+}
+
+/* Generate stop condition on i2c bus.
+ * The m00018 stop isn't doing the right thing (wrong timing).
+ * So instead send a start condition, 8 zeroes and a stop condition.
+ */
+static int cobalt_stop(struct cobalt_i2c_regs __iomem *regs,
+		struct i2c_adapter *adap)
+{
+	u8 data = 0;
+
+	return cobalt_tx_bytes(regs, adap, true, true, &data, 1);
+}
+
+static int cobalt_xfer(struct i2c_adapter *adap,
+			struct i2c_msg msgs[], int num)
+{
+	struct cobalt_i2c_data *data = adap->algo_data;
+	struct cobalt_i2c_regs __iomem *regs = data->regs;
+	struct i2c_msg *pmsg;
+	unsigned short flags;
+	int ret = 0;
+	int i, j;
+
+	for (i = 0; i < num; i++) {
+		int stop = (i == num - 1);
+
+		pmsg = &msgs[i];
+		flags = pmsg->flags;
+
+		if (!(pmsg->flags & I2C_M_NOSTART)) {
+			u8 addr = pmsg->addr << 1;
+
+			if (flags & I2C_M_RD)
+				addr |= 1;
+			if (flags & I2C_M_REV_DIR_ADDR)
+				addr ^= 1;
+			for (j = 0; j < adap->retries; j++) {
+				ret = cobalt_tx_bytes(regs, adap, true, false,
+						      &addr, 1);
+				if (!ret)
+					break;
+				cobalt_stop(regs, adap);
+			}
+			if (ret < 0)
+				return ret;
+			ret = 0;
+		}
+		if (pmsg->flags & I2C_M_RD) {
+			/* read bytes into buffer */
+			ret = cobalt_rx_bytes(regs, adap, false, stop,
+					pmsg->buf, pmsg->len);
+			if (ret < 0)
+				goto bailout;
+		} else {
+			/* write bytes from buffer */
+			ret = cobalt_tx_bytes(regs, adap, false, stop,
+					pmsg->buf, pmsg->len);
+			if (ret < 0)
+				goto bailout;
+		}
+	}
+	ret = i;
+
+bailout:
+	if (ret < 0)
+		cobalt_stop(regs, adap);
+	return ret;
+}
+
+static u32 cobalt_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+}
+
+/* template for i2c-bit-algo */
+static struct i2c_adapter cobalt_i2c_adap_template = {
+	.name = "cobalt i2c driver",
+	.algo = NULL,                   /* set by i2c-algo-bit */
+	.algo_data = NULL,              /* filled from template */
+	.owner = THIS_MODULE,
+};
+
+static const struct i2c_algorithm cobalt_algo = {
+	.master_xfer	= cobalt_xfer,
+	.functionality	= cobalt_func,
+};
+
+/* init + register i2c algo-bit adapter */
+int cobalt_i2c_init(struct cobalt *cobalt)
+{
+	int i, err;
+	int status;
+	int prescale;
+	unsigned long start_time;
+
+	cobalt_dbg(1, "i2c init\n");
+
+	/* Define I2C clock prescaler */
+	prescale = ((ALT_CPU_FREQ) / (5 * I2C_FREQUENCY)) - 1;
+
+	for (i = 0; i < COBALT_NUM_ADAPTERS; i++) {
+		struct cobalt_i2c_regs __iomem *regs =
+			cobalt_i2c_regs(cobalt, i);
+		struct i2c_adapter *adap = &cobalt->i2c_adap[i];
+
+		/* Disable I2C */
+		iowrite8(M00018_CTR_BITMAP_EN_MSK, &regs->cr_sr);
+		iowrite8(0, &regs->ctr);
+		iowrite8(0, &regs->cr_sr);
+
+		start_time = jiffies;
+		do {
+			if (time_after(jiffies, start_time + HZ)) {
+				if (cobalt_ignore_err) {
+					adap->dev.parent = NULL;
+					return 0;
+				}
+				return -ETIMEDOUT;
+			}
+			status = ioread8(&regs->cr_sr);
+		} while (status & M00018_SR_BITMAP_TIP_MSK);
+
+		/* Disable I2C */
+		iowrite8(0, &regs->ctr);
+		iowrite8(0, &regs->cr_sr);
+
+		/* Calculate i2c prescaler */
+		iowrite8(prescale & 0xff, &regs->prerlo);
+		iowrite8((prescale >> 8) & 0xff, &regs->prerhi);
+		/* Enable I2C, interrupts disabled */
+		iowrite8(M00018_CTR_BITMAP_EN_MSK, &regs->ctr);
+		/* Setup algorithm for adapter */
+		cobalt->i2c_data[i].cobalt = cobalt;
+		cobalt->i2c_data[i].regs = regs;
+		*adap = cobalt_i2c_adap_template;
+		adap->algo = &cobalt_algo;
+		adap->algo_data = &cobalt->i2c_data[i];
+		adap->retries = 3;
+		sprintf(adap->name + strlen(adap->name),
+				" #%d-%d", cobalt->instance, i);
+		i2c_set_adapdata(adap, &cobalt->v4l2_dev);
+		adap->dev.parent = &cobalt->pci_dev->dev;
+		err = i2c_add_adapter(adap);
+		if (err) {
+			if (cobalt_ignore_err) {
+				adap->dev.parent = NULL;
+				return 0;
+			}
+			while (i--)
+				i2c_del_adapter(&cobalt->i2c_adap[i]);
+			return err;
+		}
+		cobalt_info("registered bus %s\n", adap->name);
+	}
+	return 0;
+}
+
+void cobalt_i2c_exit(struct cobalt *cobalt)
+{
+	int i;
+
+	cobalt_dbg(1, "i2c exit\n");
+
+	for (i = 0; i < COBALT_NUM_ADAPTERS; i++) {
+		cobalt_err("unregistered bus %s\n", cobalt->i2c_adap[i].name);
+		i2c_del_adapter(&cobalt->i2c_adap[i]);
+	}
+}
diff --git a/drivers/media/pci/cobalt/cobalt-i2c.h b/drivers/media/pci/cobalt/cobalt-i2c.h
new file mode 100644
index 000000000000..a4c1cfaacf95
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-i2c.h
@@ -0,0 +1,25 @@
+/*
+ *  cobalt I2C functions
+ *
+ *  Derived from cx18-i2c.h
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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.
+ */
+
+/* init + register i2c algo-bit adapter */
+int cobalt_i2c_init(struct cobalt *cobalt);
+void cobalt_i2c_exit(struct cobalt *cobalt);
diff --git a/drivers/media/pci/cobalt/cobalt-irq.c b/drivers/media/pci/cobalt/cobalt-irq.c
new file mode 100644
index 000000000000..dd4bff9cf339
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-irq.c
@@ -0,0 +1,258 @@
+/*
+ *  cobalt interrupt handling
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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 <media/adv7604.h>
+
+#include "cobalt-driver.h"
+#include "cobalt-irq.h"
+#include "cobalt-omnitek.h"
+
+static void cobalt_dma_stream_queue_handler(struct cobalt_stream *s)
+{
+	struct cobalt *cobalt = s->cobalt;
+	int rx = s->video_channel;
+	struct m00473_freewheel_regmap __iomem *fw =
+		COBALT_CVI_FREEWHEEL(s->cobalt, rx);
+	struct m00233_video_measure_regmap __iomem *vmr =
+		COBALT_CVI_VMR(s->cobalt, rx);
+	struct m00389_cvi_regmap __iomem *cvi =
+		COBALT_CVI(s->cobalt, rx);
+	struct m00479_clk_loss_detector_regmap __iomem *clkloss =
+		COBALT_CVI_CLK_LOSS(s->cobalt, rx);
+	struct cobalt_buffer *cb;
+	bool skip = false;
+
+	spin_lock(&s->irqlock);
+
+	if (list_empty(&s->bufs)) {
+		pr_err("no buffers!\n");
+		spin_unlock(&s->irqlock);
+		return;
+	}
+
+	/* Give the fresh filled up buffer to the user.
+	 * Note that the interrupt is only sent if the DMA can continue
+	 * with a new buffer, so it is always safe to return this buffer
+	 * to userspace. */
+	cb = list_first_entry(&s->bufs, struct cobalt_buffer, list);
+	list_del(&cb->list);
+	spin_unlock(&s->irqlock);
+
+	if (s->is_audio || s->is_output)
+		goto done;
+
+	if (s->unstable_frame) {
+		uint32_t stat = ioread32(&vmr->irq_status);
+
+		iowrite32(stat, &vmr->irq_status);
+		if (!(ioread32(&vmr->status) &
+		      M00233_STATUS_BITMAP_INIT_DONE_MSK)) {
+			cobalt_dbg(1, "!init_done\n");
+			if (s->enable_freewheel)
+				goto restart_fw;
+			goto done;
+		}
+
+		if (ioread32(&clkloss->status) &
+		    M00479_STATUS_BITMAP_CLOCK_MISSING_MSK) {
+			iowrite32(0, &clkloss->ctrl);
+			iowrite32(M00479_CTRL_BITMAP_ENABLE_MSK, &clkloss->ctrl);
+			cobalt_dbg(1, "no clock\n");
+			if (s->enable_freewheel)
+				goto restart_fw;
+			goto done;
+		}
+		if ((stat & (M00233_IRQ_STATUS_BITMAP_VACTIVE_AREA_MSK |
+			     M00233_IRQ_STATUS_BITMAP_HACTIVE_AREA_MSK)) ||
+				ioread32(&vmr->vactive_area) != s->timings.bt.height ||
+				ioread32(&vmr->hactive_area) != s->timings.bt.width) {
+			cobalt_dbg(1, "unstable\n");
+			if (s->enable_freewheel)
+				goto restart_fw;
+			goto done;
+		}
+		if (!s->enable_cvi) {
+			s->enable_cvi = true;
+			iowrite32(M00389_CONTROL_BITMAP_ENABLE_MSK, &cvi->control);
+			goto done;
+		}
+		if (!(ioread32(&cvi->status) & M00389_STATUS_BITMAP_LOCK_MSK)) {
+			cobalt_dbg(1, "cvi no lock\n");
+			if (s->enable_freewheel)
+				goto restart_fw;
+			goto done;
+		}
+		if (!s->enable_freewheel) {
+			cobalt_dbg(1, "stable\n");
+			s->enable_freewheel = true;
+			iowrite32(0, &fw->ctrl);
+			goto done;
+		}
+		cobalt_dbg(1, "enabled fw\n");
+		iowrite32(M00233_CONTROL_BITMAP_ENABLE_MEASURE_MSK |
+			  M00233_CONTROL_BITMAP_ENABLE_INTERRUPT_MSK,
+			  &vmr->control);
+		iowrite32(M00473_CTRL_BITMAP_ENABLE_MSK, &fw->ctrl);
+		s->enable_freewheel = false;
+		s->unstable_frame = false;
+		s->skip_first_frames = 2;
+		skip = true;
+		goto done;
+	}
+	if (ioread32(&fw->status) & M00473_STATUS_BITMAP_FREEWHEEL_MODE_MSK) {
+restart_fw:
+		cobalt_dbg(1, "lost lock\n");
+		iowrite32(M00233_CONTROL_BITMAP_ENABLE_MEASURE_MSK,
+			  &vmr->control);
+		iowrite32(M00473_CTRL_BITMAP_ENABLE_MSK |
+			  M00473_CTRL_BITMAP_FORCE_FREEWHEEL_MODE_MSK,
+			  &fw->ctrl);
+		iowrite32(0, &cvi->control);
+		s->unstable_frame = true;
+		s->enable_freewheel = false;
+		s->enable_cvi = false;
+	}
+done:
+	if (s->skip_first_frames) {
+		skip = true;
+		s->skip_first_frames--;
+	}
+	v4l2_get_timestamp(&cb->vb.v4l2_buf.timestamp);
+	/* TODO: the sequence number should be read from the FPGA so we
+	   also know about dropped frames. */
+	cb->vb.v4l2_buf.sequence = s->sequence++;
+	vb2_buffer_done(&cb->vb, (skip || s->unstable_frame) ?
+			VB2_BUF_STATE_QUEUED : VB2_BUF_STATE_DONE);
+}
+
+irqreturn_t cobalt_irq_handler(int irq, void *dev_id)
+{
+	struct cobalt *cobalt = (struct cobalt *)dev_id;
+	u32 dma_interrupt =
+		cobalt_read_bar0(cobalt, DMA_INTERRUPT_STATUS_REG) & 0xffff;
+	u32 mask = cobalt_read_bar1(cobalt, COBALT_SYS_STAT_MASK);
+	u32 edge = cobalt_read_bar1(cobalt, COBALT_SYS_STAT_EDGE);
+	int i;
+
+	/* Clear DMA interrupt */
+	cobalt_write_bar0(cobalt, DMA_INTERRUPT_STATUS_REG, dma_interrupt);
+	cobalt_write_bar1(cobalt, COBALT_SYS_STAT_MASK, mask & ~edge);
+	cobalt_write_bar1(cobalt, COBALT_SYS_STAT_EDGE, edge);
+
+	for (i = 0; i < COBALT_NUM_STREAMS; i++) {
+		struct cobalt_stream *s = &cobalt->streams[i];
+		unsigned dma_fifo_mask = s->dma_fifo_mask;
+
+		if (dma_interrupt & (1 << s->dma_channel)) {
+			cobalt->irq_dma[i]++;
+			/* Give fresh buffer to user and chain newly
+			 * queued buffers */
+			cobalt_dma_stream_queue_handler(s);
+			if (!s->is_audio) {
+				edge &= ~dma_fifo_mask;
+				cobalt_write_bar1(cobalt, COBALT_SYS_STAT_MASK,
+						  mask & ~edge);
+			}
+		}
+		if (s->is_audio)
+			continue;
+		if (edge & s->adv_irq_mask)
+			set_bit(COBALT_STREAM_FL_ADV_IRQ, &s->flags);
+		if ((edge & mask & dma_fifo_mask) && vb2_is_streaming(&s->q)) {
+			cobalt_info("full rx FIFO %d\n", i);
+			cobalt->irq_full_fifo++;
+		}
+	}
+
+	queue_work(cobalt->irq_work_queues, &cobalt->irq_work_queue);
+
+	if (edge & mask & (COBALT_SYSSTAT_VI0_INT1_MSK |
+			   COBALT_SYSSTAT_VI1_INT1_MSK |
+			   COBALT_SYSSTAT_VI2_INT1_MSK |
+			   COBALT_SYSSTAT_VI3_INT1_MSK |
+			   COBALT_SYSSTAT_VIHSMA_INT1_MSK |
+			   COBALT_SYSSTAT_VOHSMA_INT1_MSK))
+		cobalt->irq_adv1++;
+	if (edge & mask & (COBALT_SYSSTAT_VI0_INT2_MSK |
+			   COBALT_SYSSTAT_VI1_INT2_MSK |
+			   COBALT_SYSSTAT_VI2_INT2_MSK |
+			   COBALT_SYSSTAT_VI3_INT2_MSK |
+			   COBALT_SYSSTAT_VIHSMA_INT2_MSK))
+		cobalt->irq_adv2++;
+	if (edge & mask & COBALT_SYSSTAT_VOHSMA_INT1_MSK)
+		cobalt->irq_advout++;
+	if (dma_interrupt)
+		cobalt->irq_dma_tot++;
+	if (!(edge & mask) && !dma_interrupt)
+		cobalt->irq_none++;
+	dma_interrupt = cobalt_read_bar0(cobalt, DMA_INTERRUPT_STATUS_REG);
+
+	return IRQ_HANDLED;
+}
+
+void cobalt_irq_work_handler(struct work_struct *work)
+{
+	struct cobalt *cobalt =
+		container_of(work, struct cobalt, irq_work_queue);
+	int i;
+
+	for (i = 0; i < COBALT_NUM_NODES; i++) {
+		struct cobalt_stream *s = &cobalt->streams[i];
+
+		if (test_and_clear_bit(COBALT_STREAM_FL_ADV_IRQ, &s->flags)) {
+			u32 mask;
+
+			v4l2_subdev_call(cobalt->streams[i].sd, core,
+					interrupt_service_routine, 0, NULL);
+			mask = cobalt_read_bar1(cobalt, COBALT_SYS_STAT_MASK);
+			cobalt_write_bar1(cobalt, COBALT_SYS_STAT_MASK,
+				mask | s->adv_irq_mask);
+		}
+	}
+}
+
+void cobalt_irq_log_status(struct cobalt *cobalt)
+{
+	u32 mask;
+	int i;
+
+	cobalt_info("irq: adv1=%u adv2=%u advout=%u none=%u full=%u\n",
+		    cobalt->irq_adv1, cobalt->irq_adv2, cobalt->irq_advout,
+		    cobalt->irq_none, cobalt->irq_full_fifo);
+	cobalt_info("irq: dma_tot=%u (", cobalt->irq_dma_tot);
+	for (i = 0; i < COBALT_NUM_STREAMS; i++)
+		pr_cont("%s%u", i ? "/" : "", cobalt->irq_dma[i]);
+	pr_cont(")\n");
+	cobalt->irq_dma_tot = cobalt->irq_adv1 = cobalt->irq_adv2 = 0;
+	cobalt->irq_advout = cobalt->irq_none = cobalt->irq_full_fifo = 0;
+	memset(cobalt->irq_dma, 0, sizeof(cobalt->irq_dma));
+
+	mask = cobalt_read_bar1(cobalt, COBALT_SYS_STAT_MASK);
+	cobalt_write_bar1(cobalt, COBALT_SYS_STAT_MASK,
+			mask |
+			COBALT_SYSSTAT_VI0_LOST_DATA_MSK |
+			COBALT_SYSSTAT_VI1_LOST_DATA_MSK |
+			COBALT_SYSSTAT_VI2_LOST_DATA_MSK |
+			COBALT_SYSSTAT_VI3_LOST_DATA_MSK |
+			COBALT_SYSSTAT_VIHSMA_LOST_DATA_MSK |
+			COBALT_SYSSTAT_VOHSMA_LOST_DATA_MSK |
+			COBALT_SYSSTAT_AUD_IN_LOST_DATA_MSK |
+			COBALT_SYSSTAT_AUD_OUT_LOST_DATA_MSK);
+}
diff --git a/drivers/media/pci/cobalt/cobalt-irq.h b/drivers/media/pci/cobalt/cobalt-irq.h
new file mode 100644
index 000000000000..5119484a24d9
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-irq.h
@@ -0,0 +1,25 @@
+/*
+ *  cobalt interrupt handling
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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/interrupt.h>
+
+irqreturn_t cobalt_irq_handler(int irq, void *dev_id);
+void cobalt_irq_work_handler(struct work_struct *work);
+void cobalt_irq_log_status(struct cobalt *cobalt);
diff --git a/drivers/media/pci/cobalt/cobalt-omnitek.c b/drivers/media/pci/cobalt/cobalt-omnitek.c
new file mode 100644
index 000000000000..a28a8482c1d4
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-omnitek.c
@@ -0,0 +1,341 @@
+/*
+ *  Omnitek Scatter-Gather DMA Controller
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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/string.h>
+#include <linux/io.h>
+#include <linux/pci_regs.h>
+#include <linux/spinlock.h>
+
+#include "cobalt-driver.h"
+#include "cobalt-omnitek.h"
+
+/* descriptor */
+#define END_OF_CHAIN		(1 << 1)
+#define INTERRUPT_ENABLE	(1 << 2)
+#define WRITE_TO_PCI		(1 << 3)
+#define READ_FROM_PCI		(0 << 3)
+#define DESCRIPTOR_FLAG_MSK	(END_OF_CHAIN | INTERRUPT_ENABLE | WRITE_TO_PCI)
+#define NEXT_ADRS_MSK		0xffffffe0
+
+/* control/status register */
+#define ENABLE                  (1 << 0)
+#define START                   (1 << 1)
+#define ABORT                   (1 << 2)
+#define DONE                    (1 << 4)
+#define SG_INTERRUPT            (1 << 5)
+#define EVENT_INTERRUPT         (1 << 6)
+#define SCATTER_GATHER_MODE     (1 << 8)
+#define DISABLE_VIDEO_RESYNC    (1 << 9)
+#define EVENT_INTERRUPT_ENABLE  (1 << 10)
+#define DIRECTIONAL_MSK         (3 << 16)
+#define INPUT_ONLY              (0 << 16)
+#define OUTPUT_ONLY             (1 << 16)
+#define BIDIRECTIONAL           (2 << 16)
+#define DMA_TYPE_MEMORY         (0 << 18)
+#define DMA_TYPE_FIFO		(1 << 18)
+
+#define BASE			(cobalt->bar0)
+#define CAPABILITY_HEADER	(BASE)
+#define CAPABILITY_REGISTER	(BASE + 0x04)
+#define PCI_64BIT		(1 << 8)
+#define LOCAL_64BIT		(1 << 9)
+#define INTERRUPT_STATUS	(BASE + 0x08)
+#define PCI(c)			(BASE + 0x40 + ((c) * 0x40))
+#define SIZE(c)			(BASE + 0x58 + ((c) * 0x40))
+#define DESCRIPTOR(c)		(BASE + 0x50 + ((c) * 0x40))
+#define CS_REG(c)		(BASE + 0x60 + ((c) * 0x40))
+#define BYTES_TRANSFERRED(c)	(BASE + 0x64 + ((c) * 0x40))
+
+
+static char *get_dma_direction(u32 status)
+{
+	switch (status & DIRECTIONAL_MSK) {
+	case INPUT_ONLY: return "Input";
+	case OUTPUT_ONLY: return "Output";
+	case BIDIRECTIONAL: return "Bidirectional";
+	}
+	return "";
+}
+
+static void show_dma_capability(struct cobalt *cobalt)
+{
+	u32 header = ioread32(CAPABILITY_HEADER);
+	u32 capa = ioread32(CAPABILITY_REGISTER);
+	u32 i;
+
+	cobalt_info("Omnitek DMA capability: ID 0x%02x Version 0x%02x Next 0x%x Size 0x%x\n",
+		    header & 0xff, (header >> 8) & 0xff,
+		    (header >> 16) & 0xffff, (capa >> 24) & 0xff);
+
+	switch ((capa >> 8) & 0x3) {
+	case 0:
+		cobalt_info("Omnitek DMA: 32 bits PCIe and Local\n");
+		break;
+	case 1:
+		cobalt_info("Omnitek DMA: 64 bits PCIe, 32 bits Local\n");
+		break;
+	case 3:
+		cobalt_info("Omnitek DMA: 64 bits PCIe and Local\n");
+		break;
+	}
+
+	for (i = 0;  i < (capa & 0xf);  i++) {
+		u32 status = ioread32(CS_REG(i));
+
+		cobalt_info("Omnitek DMA channel #%d: %s %s\n", i,
+			    status & DMA_TYPE_FIFO ? "FIFO" : "MEMORY",
+			    get_dma_direction(status));
+	}
+}
+
+void omni_sg_dma_start(struct cobalt_stream *s, struct sg_dma_desc_info *desc)
+{
+	struct cobalt *cobalt = s->cobalt;
+
+	iowrite32((u32)((u64)desc->bus >> 32), DESCRIPTOR(s->dma_channel) + 4);
+	iowrite32((u32)desc->bus & NEXT_ADRS_MSK, DESCRIPTOR(s->dma_channel));
+	iowrite32(ENABLE | SCATTER_GATHER_MODE | START, CS_REG(s->dma_channel));
+}
+
+bool is_dma_done(struct cobalt_stream *s)
+{
+	struct cobalt *cobalt = s->cobalt;
+
+	if (ioread32(CS_REG(s->dma_channel)) & DONE)
+		return true;
+
+	return false;
+}
+
+void omni_sg_dma_abort_channel(struct cobalt_stream *s)
+{
+	struct cobalt *cobalt = s->cobalt;
+
+	if (is_dma_done(s) == false)
+		iowrite32(ABORT, CS_REG(s->dma_channel));
+}
+
+int omni_sg_dma_init(struct cobalt *cobalt)
+{
+	u32 capa = ioread32(CAPABILITY_REGISTER);
+	int i;
+
+	cobalt->first_fifo_channel = 0;
+	cobalt->dma_channels = capa & 0xf;
+	if (capa & PCI_64BIT)
+		cobalt->pci_32_bit = false;
+	else
+		cobalt->pci_32_bit = true;
+
+	for (i = 0; i < cobalt->dma_channels; i++) {
+		u32 status = ioread32(CS_REG(i));
+		u32 ctrl = ioread32(CS_REG(i));
+
+		if (!(ctrl & DONE))
+			iowrite32(ABORT, CS_REG(i));
+
+		if (!(status & DMA_TYPE_FIFO))
+			cobalt->first_fifo_channel++;
+	}
+	show_dma_capability(cobalt);
+	return 0;
+}
+
+int descriptor_list_create(struct cobalt *cobalt,
+		struct scatterlist *scatter_list, bool to_pci, unsigned sglen,
+		unsigned size, unsigned width, unsigned stride,
+		struct sg_dma_desc_info *desc)
+{
+	struct sg_dma_descriptor *d = (struct sg_dma_descriptor *)desc->virt;
+	dma_addr_t next = desc->bus;
+	unsigned offset = 0;
+	unsigned copy_bytes = width;
+	unsigned copied = 0;
+	bool first = true;
+
+	/* Must be 4-byte aligned */
+	WARN_ON(sg_dma_address(scatter_list) & 3);
+	WARN_ON(size & 3);
+	WARN_ON(next & 3);
+	WARN_ON(stride & 3);
+	WARN_ON(stride < width);
+	if (width >= stride)
+		copy_bytes = stride = size;
+
+	while (size) {
+		dma_addr_t addr = sg_dma_address(scatter_list) + offset;
+		unsigned bytes;
+
+		if (addr == 0)
+			return -EFAULT;
+		if (cobalt->pci_32_bit) {
+			WARN_ON((u64)addr >> 32);
+			if ((u64)addr >> 32)
+				return -EFAULT;
+		}
+
+		/* PCIe address */
+		d->pci_l = addr & 0xffffffff;
+		/* If dma_addr_t is 32 bits, then addr >> 32 is actually the
+		   equivalent of addr >> 0 in gcc. So must cast to u64. */
+		d->pci_h = (u64)addr >> 32;
+
+		/* Sync to start of streaming frame */
+		d->local = 0;
+		d->reserved0 = 0;
+
+		/* Transfer bytes */
+		bytes = min(sg_dma_len(scatter_list) - offset,
+				copy_bytes - copied);
+
+		if (first) {
+			if (to_pci)
+				d->local = 0x11111111;
+			first = false;
+			if (sglen == 1) {
+				/* Make sure there are always at least two
+				 * descriptors */
+				d->bytes = (bytes / 2) & ~3;
+				d->reserved1 = 0;
+				size -= d->bytes;
+				copied += d->bytes;
+				offset += d->bytes;
+				addr += d->bytes;
+				next += sizeof(struct sg_dma_descriptor);
+				d->next_h = (u32)((u64)next >> 32);
+				d->next_l = (u32)next |
+					(to_pci ? WRITE_TO_PCI : 0);
+				bytes -= d->bytes;
+				d++;
+				/* PCIe address */
+				d->pci_l = addr & 0xffffffff;
+				/* If dma_addr_t is 32 bits, then addr >> 32
+				 * is actually the equivalent of addr >> 0 in
+				 * gcc. So must cast to u64. */
+				d->pci_h = (u64)addr >> 32;
+
+				/* Sync to start of streaming frame */
+				d->local = 0;
+				d->reserved0 = 0;
+			}
+		}
+
+		d->bytes = bytes;
+		d->reserved1 = 0;
+		size -= bytes;
+		copied += bytes;
+		offset += bytes;
+
+		if (copied == copy_bytes) {
+			while (copied < stride) {
+				bytes = min(sg_dma_len(scatter_list) - offset,
+						stride - copied);
+				copied += bytes;
+				offset += bytes;
+				size -= bytes;
+				if (sg_dma_len(scatter_list) == offset) {
+					offset = 0;
+					scatter_list = sg_next(scatter_list);
+				}
+			}
+			copied = 0;
+		} else {
+			offset = 0;
+			scatter_list = sg_next(scatter_list);
+		}
+
+		/* Next descriptor + control bits */
+		next += sizeof(struct sg_dma_descriptor);
+		if (size == 0) {
+			/* Loopback to the first descriptor */
+			d->next_h = (u32)((u64)desc->bus >> 32);
+			d->next_l = (u32)desc->bus |
+				(to_pci ? WRITE_TO_PCI : 0) | INTERRUPT_ENABLE;
+			if (!to_pci)
+				d->local = 0x22222222;
+			desc->last_desc_virt = d;
+		} else {
+			d->next_h = (u32)((u64)next >> 32);
+			d->next_l = (u32)next | (to_pci ? WRITE_TO_PCI : 0);
+		}
+		d++;
+	}
+	return 0;
+}
+
+void descriptor_list_chain(struct sg_dma_desc_info *this,
+			   struct sg_dma_desc_info *next)
+{
+	struct sg_dma_descriptor *d = this->last_desc_virt;
+	u32 direction = d->next_l & WRITE_TO_PCI;
+
+	if (next == NULL) {
+		d->next_h = 0;
+		d->next_l = direction | INTERRUPT_ENABLE | END_OF_CHAIN;
+	} else {
+		d->next_h = (u32)((u64)next->bus >> 32);
+		d->next_l = (u32)next->bus | direction | INTERRUPT_ENABLE;
+	}
+}
+
+void *descriptor_list_allocate(struct sg_dma_desc_info *desc, size_t bytes)
+{
+	desc->size = bytes;
+	desc->virt = dma_alloc_coherent(desc->dev, bytes,
+					&desc->bus, GFP_KERNEL);
+	return desc->virt;
+}
+
+void descriptor_list_free(struct sg_dma_desc_info *desc)
+{
+	if (desc->virt)
+		dma_free_coherent(desc->dev, desc->size,
+				  desc->virt, desc->bus);
+	desc->virt = NULL;
+}
+
+void descriptor_list_interrupt_enable(struct sg_dma_desc_info *desc)
+{
+	struct sg_dma_descriptor *d = desc->last_desc_virt;
+
+	d->next_l |= INTERRUPT_ENABLE;
+}
+
+void descriptor_list_interrupt_disable(struct sg_dma_desc_info *desc)
+{
+	struct sg_dma_descriptor *d = desc->last_desc_virt;
+
+	d->next_l &= ~INTERRUPT_ENABLE;
+}
+
+void descriptor_list_loopback(struct sg_dma_desc_info *desc)
+{
+	struct sg_dma_descriptor *d = desc->last_desc_virt;
+
+	d->next_h = (u32)((u64)desc->bus >> 32);
+	d->next_l = (u32)desc->bus | (d->next_l & DESCRIPTOR_FLAG_MSK);
+}
+
+void descriptor_list_end_of_chain(struct sg_dma_desc_info *desc)
+{
+	struct sg_dma_descriptor *d = desc->last_desc_virt;
+
+	d->next_l |= END_OF_CHAIN;
+}
diff --git a/drivers/media/pci/cobalt/cobalt-omnitek.h b/drivers/media/pci/cobalt/cobalt-omnitek.h
new file mode 100644
index 000000000000..e5c6d032c6f2
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-omnitek.h
@@ -0,0 +1,62 @@
+/*
+ *  Omnitek Scatter-Gather DMA Controller
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef COBALT_OMNITEK_H
+#define COBALT_OMNITEK_H
+
+#include <linux/scatterlist.h>
+#include "cobalt-driver.h"
+
+struct sg_dma_descriptor {
+	u32 pci_l;
+	u32 pci_h;
+
+	u32 local;
+	u32 reserved0;
+
+	u32 next_l;
+	u32 next_h;
+
+	u32 bytes;
+	u32 reserved1;
+};
+
+int omni_sg_dma_init(struct cobalt *cobalt);
+void omni_sg_dma_abort_channel(struct cobalt_stream *s);
+void omni_sg_dma_start(struct cobalt_stream *s, struct sg_dma_desc_info *desc);
+bool is_dma_done(struct cobalt_stream *s);
+
+int descriptor_list_create(struct cobalt *cobalt,
+	struct scatterlist *scatter_list, bool to_pci, unsigned sglen,
+	unsigned size, unsigned width, unsigned stride,
+	struct sg_dma_desc_info *desc);
+
+void descriptor_list_chain(struct sg_dma_desc_info *this,
+			   struct sg_dma_desc_info *next);
+void descriptor_list_loopback(struct sg_dma_desc_info *desc);
+void descriptor_list_end_of_chain(struct sg_dma_desc_info *desc);
+
+void *descriptor_list_allocate(struct sg_dma_desc_info *desc, size_t bytes);
+void descriptor_list_free(struct sg_dma_desc_info *desc);
+
+void descriptor_list_interrupt_enable(struct sg_dma_desc_info *desc);
+void descriptor_list_interrupt_disable(struct sg_dma_desc_info *desc);
+
+#endif
diff --git a/drivers/media/pci/cobalt/cobalt-v4l2.c b/drivers/media/pci/cobalt/cobalt-v4l2.c
new file mode 100644
index 000000000000..b40c2d141b58
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-v4l2.c
@@ -0,0 +1,1272 @@
+/*
+ *  cobalt V4L2 API
+ *
+ *  Derived from ivtv-ioctl.c and cx18-fileops.c
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <linux/pci.h>
+#include <linux/v4l2-dv-timings.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/adv7604.h>
+#include <media/adv7842.h>
+
+#include "cobalt-alsa.h"
+#include "cobalt-cpld.h"
+#include "cobalt-driver.h"
+#include "cobalt-v4l2.h"
+#include "cobalt-irq.h"
+#include "cobalt-omnitek.h"
+
+static const struct v4l2_dv_timings cea1080p60 = V4L2_DV_BT_CEA_1920X1080P60;
+
+/* vb2 DMA streaming ops */
+
+static int cobalt_queue_setup(struct vb2_queue *q,
+			const struct v4l2_format *fmt,
+			unsigned int *num_buffers, unsigned int *num_planes,
+			unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct cobalt_stream *s = q->drv_priv;
+	unsigned size = s->stride * s->height;
+
+	if (*num_buffers < 3)
+		*num_buffers = 3;
+	if (*num_buffers > NR_BUFS)
+		*num_buffers = NR_BUFS;
+	*num_planes = 1;
+	if (fmt) {
+		if (fmt->fmt.pix.sizeimage < size)
+			return -EINVAL;
+		size = fmt->fmt.pix.sizeimage;
+	}
+	sizes[0] = size;
+	alloc_ctxs[0] = s->cobalt->alloc_ctx;
+	return 0;
+}
+
+static int cobalt_buf_init(struct vb2_buffer *vb)
+{
+	struct cobalt_stream *s = vb->vb2_queue->drv_priv;
+	struct cobalt *cobalt = s->cobalt;
+	const size_t max_pages_per_line =
+		(COBALT_MAX_WIDTH * COBALT_MAX_BPP) / PAGE_SIZE + 2;
+	const size_t bytes =
+		COBALT_MAX_HEIGHT * max_pages_per_line * 0x20;
+	const size_t audio_bytes = ((1920 * 4) / PAGE_SIZE + 1) * 0x20;
+	struct sg_dma_desc_info *desc = &s->dma_desc_info[vb->v4l2_buf.index];
+	struct sg_table *sg_desc = vb2_dma_sg_plane_desc(vb, 0);
+	unsigned size;
+	int ret;
+
+	size = s->stride * s->height;
+	if (vb2_plane_size(vb, 0) < size) {
+		cobalt_info("data will not fit into plane (%lu < %u)\n",
+					vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	if (desc->virt == NULL) {
+		desc->dev = &cobalt->pci_dev->dev;
+		descriptor_list_allocate(desc,
+			s->is_audio ? audio_bytes : bytes);
+		if (desc->virt == NULL)
+			return -ENOMEM;
+	}
+	ret = descriptor_list_create(cobalt, sg_desc->sgl,
+			!s->is_output, sg_desc->nents, size,
+			s->width * s->bpp, s->stride, desc);
+	if (ret)
+		descriptor_list_free(desc);
+	return ret;
+}
+
+static void cobalt_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct cobalt_stream *s = vb->vb2_queue->drv_priv;
+	struct sg_dma_desc_info *desc = &s->dma_desc_info[vb->v4l2_buf.index];
+
+	descriptor_list_free(desc);
+}
+
+static int cobalt_buf_prepare(struct vb2_buffer *vb)
+{
+	struct cobalt_stream *s = vb->vb2_queue->drv_priv;
+
+	vb2_set_plane_payload(vb, 0, s->stride * s->height);
+	vb->v4l2_buf.field = V4L2_FIELD_NONE;
+	return 0;
+}
+
+static void chain_all_buffers(struct cobalt_stream *s)
+{
+	struct sg_dma_desc_info *desc[NR_BUFS];
+	struct cobalt_buffer *cb;
+	struct list_head *p;
+	int i = 0;
+
+	list_for_each(p, &s->bufs) {
+		cb = list_entry(p, struct cobalt_buffer, list);
+		desc[i] = &s->dma_desc_info[cb->vb.v4l2_buf.index];
+		if (i > 0)
+			descriptor_list_chain(desc[i-1], desc[i]);
+		i++;
+	}
+}
+
+static void cobalt_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_queue *q = vb->vb2_queue;
+	struct cobalt_stream *s = q->drv_priv;
+	struct cobalt_buffer *cb = to_cobalt_buffer(vb);
+	struct sg_dma_desc_info *desc = &s->dma_desc_info[vb->v4l2_buf.index];
+	unsigned long flags;
+
+	/* Prepare new buffer */
+	descriptor_list_loopback(desc);
+	descriptor_list_interrupt_disable(desc);
+
+	spin_lock_irqsave(&s->irqlock, flags);
+	list_add_tail(&cb->list, &s->bufs);
+	chain_all_buffers(s);
+	spin_unlock_irqrestore(&s->irqlock, flags);
+}
+
+static void cobalt_enable_output(struct cobalt_stream *s)
+{
+	struct cobalt *cobalt = s->cobalt;
+	struct v4l2_bt_timings *bt = &s->timings.bt;
+	struct m00514_syncgen_flow_evcnt_regmap __iomem *vo =
+		COBALT_TX_BASE(cobalt);
+	unsigned fmt = s->pixfmt != V4L2_PIX_FMT_BGR32 ?
+			M00514_CONTROL_BITMAP_FORMAT_16_BPP_MSK : 0;
+	struct v4l2_subdev_format sd_fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	if (!cobalt_cpld_set_freq(cobalt, bt->pixelclock)) {
+		cobalt_err("pixelclock out of range\n");
+		return;
+	}
+
+	sd_fmt.format.colorspace = s->colorspace;
+	sd_fmt.format.xfer_func = s->xfer_func;
+	sd_fmt.format.ycbcr_enc = s->ycbcr_enc;
+	sd_fmt.format.quantization = s->quantization;
+	sd_fmt.format.width = bt->width;
+	sd_fmt.format.height = bt->height;
+
+	/* Set up FDMA packer */
+	switch (s->pixfmt) {
+	case V4L2_PIX_FMT_YUYV:
+		sd_fmt.format.code = MEDIA_BUS_FMT_UYVY8_1X16;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		sd_fmt.format.code = MEDIA_BUS_FMT_RGB888_1X24;
+		break;
+	}
+	v4l2_subdev_call(s->sd, pad, set_fmt, NULL, &sd_fmt);
+
+	iowrite32(0, &vo->control);
+	/* 1080p60 */
+	iowrite32(bt->hsync, &vo->sync_generator_h_sync_length);
+	iowrite32(bt->hbackporch, &vo->sync_generator_h_backporch_length);
+	iowrite32(bt->width, &vo->sync_generator_h_active_length);
+	iowrite32(bt->hfrontporch, &vo->sync_generator_h_frontporch_length);
+	iowrite32(bt->vsync, &vo->sync_generator_v_sync_length);
+	iowrite32(bt->vbackporch, &vo->sync_generator_v_backporch_length);
+	iowrite32(bt->height, &vo->sync_generator_v_active_length);
+	iowrite32(bt->vfrontporch, &vo->sync_generator_v_frontporch_length);
+	iowrite32(0x9900c1, &vo->error_color);
+
+	iowrite32(M00514_CONTROL_BITMAP_SYNC_GENERATOR_LOAD_PARAM_MSK | fmt,
+		  &vo->control);
+	iowrite32(M00514_CONTROL_BITMAP_EVCNT_CLEAR_MSK | fmt, &vo->control);
+	iowrite32(M00514_CONTROL_BITMAP_SYNC_GENERATOR_ENABLE_MSK |
+		  M00514_CONTROL_BITMAP_FLOW_CTRL_OUTPUT_ENABLE_MSK |
+		  fmt, &vo->control);
+}
+
+static void cobalt_enable_input(struct cobalt_stream *s)
+{
+	struct cobalt *cobalt = s->cobalt;
+	int ch = (int)s->video_channel;
+	struct m00235_fdma_packer_regmap __iomem *packer;
+	struct v4l2_subdev_format sd_fmt_yuyv = {
+		.pad = s->pad_source,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format.code = MEDIA_BUS_FMT_YUYV8_1X16,
+	};
+	struct v4l2_subdev_format sd_fmt_rgb = {
+		.pad = s->pad_source,
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format.code = MEDIA_BUS_FMT_RGB888_1X24,
+	};
+
+	cobalt_dbg(1, "video_channel %d (%s, %s)\n",
+		   s->video_channel,
+		   s->input == 0 ? "hdmi" : "generator",
+		   "YUYV");
+
+	packer = COBALT_CVI_PACKER(cobalt, ch);
+
+	/* Set up FDMA packer */
+	switch (s->pixfmt) {
+	case V4L2_PIX_FMT_YUYV:
+		iowrite32(M00235_CONTROL_BITMAP_ENABLE_MSK |
+			  (1 << M00235_CONTROL_BITMAP_PACK_FORMAT_OFST),
+			  &packer->control);
+		v4l2_subdev_call(s->sd, pad, set_fmt, NULL,
+				 &sd_fmt_yuyv);
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		iowrite32(M00235_CONTROL_BITMAP_ENABLE_MSK |
+			  (2 << M00235_CONTROL_BITMAP_PACK_FORMAT_OFST),
+			  &packer->control);
+		v4l2_subdev_call(s->sd, pad, set_fmt, NULL,
+				 &sd_fmt_rgb);
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		iowrite32(M00235_CONTROL_BITMAP_ENABLE_MSK |
+			  M00235_CONTROL_BITMAP_ENDIAN_FORMAT_MSK |
+			  (3 << M00235_CONTROL_BITMAP_PACK_FORMAT_OFST),
+			  &packer->control);
+		v4l2_subdev_call(s->sd, pad, set_fmt, NULL,
+				 &sd_fmt_rgb);
+		break;
+	}
+}
+
+static void cobalt_dma_start_streaming(struct cobalt_stream *s)
+{
+	struct cobalt *cobalt = s->cobalt;
+	int rx = s->video_channel;
+	struct m00460_evcnt_regmap __iomem *evcnt =
+		COBALT_CVI_EVCNT(cobalt, rx);
+	struct cobalt_buffer *cb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&s->irqlock, flags);
+	if (!s->is_output) {
+		iowrite32(M00460_CONTROL_BITMAP_CLEAR_MSK, &evcnt->control);
+		iowrite32(M00460_CONTROL_BITMAP_ENABLE_MSK, &evcnt->control);
+	} else {
+		struct m00514_syncgen_flow_evcnt_regmap __iomem *vo =
+			COBALT_TX_BASE(cobalt);
+		u32 ctrl = ioread32(&vo->control);
+
+		ctrl &= ~(M00514_CONTROL_BITMAP_EVCNT_ENABLE_MSK |
+			  M00514_CONTROL_BITMAP_EVCNT_CLEAR_MSK);
+		iowrite32(ctrl | M00514_CONTROL_BITMAP_EVCNT_CLEAR_MSK,
+			  &vo->control);
+		iowrite32(ctrl | M00514_CONTROL_BITMAP_EVCNT_ENABLE_MSK,
+			  &vo->control);
+	}
+	cb = list_first_entry(&s->bufs, struct cobalt_buffer, list);
+	omni_sg_dma_start(s, &s->dma_desc_info[cb->vb.v4l2_buf.index]);
+	spin_unlock_irqrestore(&s->irqlock, flags);
+}
+
+static int cobalt_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct cobalt_stream *s = q->drv_priv;
+	struct cobalt *cobalt = s->cobalt;
+	struct m00233_video_measure_regmap __iomem *vmr;
+	struct m00473_freewheel_regmap __iomem *fw;
+	struct m00479_clk_loss_detector_regmap __iomem *clkloss;
+	int rx = s->video_channel;
+	struct m00389_cvi_regmap __iomem *cvi = COBALT_CVI(cobalt, rx);
+	struct m00460_evcnt_regmap __iomem *evcnt = COBALT_CVI_EVCNT(cobalt, rx);
+	struct v4l2_bt_timings *bt = &s->timings.bt;
+	u64 tot_size;
+	u32 clk_freq;
+
+	if (s->is_audio)
+		goto done;
+	if (s->is_output) {
+		s->unstable_frame = false;
+		cobalt_enable_output(s);
+		goto done;
+	}
+
+	cobalt_enable_input(s);
+
+	fw = COBALT_CVI_FREEWHEEL(cobalt, rx);
+	vmr = COBALT_CVI_VMR(cobalt, rx);
+	clkloss = COBALT_CVI_CLK_LOSS(cobalt, rx);
+
+	iowrite32(M00460_CONTROL_BITMAP_CLEAR_MSK, &evcnt->control);
+	iowrite32(M00460_CONTROL_BITMAP_ENABLE_MSK, &evcnt->control);
+	iowrite32(bt->width, &cvi->frame_width);
+	iowrite32(bt->height, &cvi->frame_height);
+	tot_size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+	iowrite32(div_u64((u64)V4L2_DV_BT_FRAME_WIDTH(bt) * COBALT_CLK * 4,
+			  bt->pixelclock), &vmr->hsync_timeout_val);
+	iowrite32(M00233_CONTROL_BITMAP_ENABLE_MEASURE_MSK, &vmr->control);
+	clk_freq = ioread32(&fw->clk_freq);
+	iowrite32(clk_freq / 1000000, &clkloss->ref_clk_cnt_val);
+	/* The lower bound for the clock frequency is 0.5% lower as is
+	 * allowed by the spec */
+	iowrite32(div_u64(bt->pixelclock * 995, 1000000000),
+		  &clkloss->test_clk_cnt_val);
+	/* will be enabled after the first frame has been received */
+	iowrite32(bt->width * bt->height, &fw->active_length);
+	iowrite32(div_u64((u64)clk_freq * tot_size, bt->pixelclock),
+		  &fw->total_length);
+	iowrite32(M00233_IRQ_TRIGGERS_BITMAP_VACTIVE_AREA_MSK |
+		  M00233_IRQ_TRIGGERS_BITMAP_HACTIVE_AREA_MSK,
+		  &vmr->irq_triggers);
+	iowrite32(0, &cvi->control);
+	iowrite32(M00233_CONTROL_BITMAP_ENABLE_MEASURE_MSK, &vmr->control);
+
+	iowrite32(0xff, &fw->output_color);
+	iowrite32(M00479_CTRL_BITMAP_ENABLE_MSK, &clkloss->ctrl);
+	iowrite32(M00473_CTRL_BITMAP_ENABLE_MSK |
+		  M00473_CTRL_BITMAP_FORCE_FREEWHEEL_MODE_MSK, &fw->ctrl);
+	s->unstable_frame = true;
+	s->enable_freewheel = false;
+	s->enable_cvi = false;
+	s->skip_first_frames = 0;
+
+done:
+	s->sequence = 0;
+	cobalt_dma_start_streaming(s);
+	return 0;
+}
+
+static void cobalt_dma_stop_streaming(struct cobalt_stream *s)
+{
+	struct cobalt *cobalt = s->cobalt;
+	struct sg_dma_desc_info *desc;
+	struct cobalt_buffer *cb;
+	struct list_head *p;
+	unsigned long flags;
+	int timeout_msec = 100;
+	int rx = s->video_channel;
+	struct m00460_evcnt_regmap __iomem *evcnt =
+		COBALT_CVI_EVCNT(cobalt, rx);
+
+	if (!s->is_output) {
+		iowrite32(0, &evcnt->control);
+	} else if (!s->is_audio) {
+		struct m00514_syncgen_flow_evcnt_regmap __iomem *vo =
+			COBALT_TX_BASE(cobalt);
+
+		iowrite32(M00514_CONTROL_BITMAP_EVCNT_CLEAR_MSK, &vo->control);
+		iowrite32(0, &vo->control);
+	}
+
+	/* Try to stop the DMA engine gracefully */
+	spin_lock_irqsave(&s->irqlock, flags);
+	list_for_each(p, &s->bufs) {
+		cb = list_entry(p, struct cobalt_buffer, list);
+		desc = &s->dma_desc_info[cb->vb.v4l2_buf.index];
+		/* Stop DMA after this descriptor chain */
+		descriptor_list_end_of_chain(desc);
+	}
+	spin_unlock_irqrestore(&s->irqlock, flags);
+
+	/* Wait 100 milisecond for DMA to finish, abort on timeout. */
+	if (!wait_event_timeout(s->q.done_wq, is_dma_done(s),
+				msecs_to_jiffies(timeout_msec))) {
+		omni_sg_dma_abort_channel(s);
+		pr_warn("aborted\n");
+	}
+	cobalt_write_bar0(cobalt, DMA_INTERRUPT_STATUS_REG,
+			1 << s->dma_channel);
+}
+
+static void cobalt_stop_streaming(struct vb2_queue *q)
+{
+	struct cobalt_stream *s = q->drv_priv;
+	struct cobalt *cobalt = s->cobalt;
+	int rx = s->video_channel;
+	struct m00233_video_measure_regmap __iomem *vmr;
+	struct m00473_freewheel_regmap __iomem *fw;
+	struct m00479_clk_loss_detector_regmap __iomem *clkloss;
+	struct cobalt_buffer *cb;
+	struct list_head *p, *safe;
+	unsigned long flags;
+
+	cobalt_dma_stop_streaming(s);
+
+	/* Return all buffers to user space */
+	spin_lock_irqsave(&s->irqlock, flags);
+	list_for_each_safe(p, safe, &s->bufs) {
+		cb = list_entry(p, struct cobalt_buffer, list);
+		list_del(&cb->list);
+		vb2_buffer_done(&cb->vb, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&s->irqlock, flags);
+
+	if (s->is_audio || s->is_output)
+		return;
+
+	fw = COBALT_CVI_FREEWHEEL(cobalt, rx);
+	vmr = COBALT_CVI_VMR(cobalt, rx);
+	clkloss = COBALT_CVI_CLK_LOSS(cobalt, rx);
+	iowrite32(0, &vmr->control);
+	iowrite32(M00233_CONTROL_BITMAP_ENABLE_MEASURE_MSK, &vmr->control);
+	iowrite32(0, &fw->ctrl);
+	iowrite32(0, &clkloss->ctrl);
+}
+
+static const struct vb2_ops cobalt_qops = {
+	.queue_setup = cobalt_queue_setup,
+	.buf_init = cobalt_buf_init,
+	.buf_cleanup = cobalt_buf_cleanup,
+	.buf_prepare = cobalt_buf_prepare,
+	.buf_queue = cobalt_buf_queue,
+	.start_streaming = cobalt_start_streaming,
+	.stop_streaming = cobalt_stop_streaming,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+};
+
+/* V4L2 ioctls */
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int cobalt_cobaltc(struct cobalt *cobalt, unsigned int cmd, void *arg)
+{
+	struct v4l2_dbg_register *regs = arg;
+	void __iomem *adrs = cobalt->bar1 + regs->reg;
+
+	cobalt_info("cobalt_cobaltc: adrs = %p\n", adrs);
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	regs->size = 4;
+	if (cmd == VIDIOC_DBG_S_REGISTER)
+		iowrite32(regs->val, adrs);
+	else
+		regs->val = ioread32(adrs);
+	return 0;
+}
+
+static int cobalt_g_register(struct file *file, void *priv_fh,
+		struct v4l2_dbg_register *reg)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct cobalt *cobalt = s->cobalt;
+
+	return cobalt_cobaltc(cobalt, VIDIOC_DBG_G_REGISTER, reg);
+}
+
+static int cobalt_s_register(struct file *file, void *priv_fh,
+		const struct v4l2_dbg_register *reg)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct cobalt *cobalt = s->cobalt;
+
+	return cobalt_cobaltc(cobalt, VIDIOC_DBG_S_REGISTER,
+			(struct v4l2_dbg_register *)reg);
+}
+#endif
+
+static int cobalt_querycap(struct file *file, void *priv_fh,
+				struct v4l2_capability *vcap)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct cobalt *cobalt = s->cobalt;
+
+	strlcpy(vcap->driver, "cobalt", sizeof(vcap->driver));
+	strlcpy(vcap->card, "cobalt", sizeof(vcap->card));
+	snprintf(vcap->bus_info, sizeof(vcap->bus_info),
+		 "PCIe:%s", pci_name(cobalt->pci_dev));
+	vcap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	if (s->is_output)
+		vcap->device_caps |= V4L2_CAP_VIDEO_OUTPUT;
+	else
+		vcap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	vcap->capabilities = vcap->device_caps | V4L2_CAP_DEVICE_CAPS |
+		V4L2_CAP_VIDEO_CAPTURE;
+	if (cobalt->have_hsma_tx)
+		vcap->capabilities |= V4L2_CAP_VIDEO_OUTPUT;
+	return 0;
+}
+
+static void cobalt_video_input_status_show(struct cobalt_stream *s)
+{
+	struct m00389_cvi_regmap __iomem *cvi;
+	struct m00233_video_measure_regmap __iomem *vmr;
+	struct m00473_freewheel_regmap __iomem *fw;
+	struct m00479_clk_loss_detector_regmap __iomem *clkloss;
+	struct m00235_fdma_packer_regmap __iomem *packer;
+	int rx = s->video_channel;
+	struct cobalt *cobalt = s->cobalt;
+	u32 cvi_ctrl, cvi_stat;
+	u32 vmr_ctrl, vmr_stat;
+
+	cvi = COBALT_CVI(cobalt, rx);
+	vmr = COBALT_CVI_VMR(cobalt, rx);
+	fw = COBALT_CVI_FREEWHEEL(cobalt, rx);
+	clkloss = COBALT_CVI_CLK_LOSS(cobalt, rx);
+	packer = COBALT_CVI_PACKER(cobalt, rx);
+	cvi_ctrl = ioread32(&cvi->control);
+	cvi_stat = ioread32(&cvi->status);
+	vmr_ctrl = ioread32(&vmr->control);
+	vmr_stat = ioread32(&vmr->control);
+	cobalt_info("rx%d: cvi resolution: %dx%d\n", rx,
+		    ioread32(&cvi->frame_width), ioread32(&cvi->frame_height));
+	cobalt_info("rx%d: cvi control: %s%s%s\n", rx,
+		(cvi_ctrl & M00389_CONTROL_BITMAP_ENABLE_MSK) ?
+			"enable " : "disable ",
+		(cvi_ctrl & M00389_CONTROL_BITMAP_HSYNC_POLARITY_LOW_MSK) ?
+			"HSync- " : "HSync+ ",
+		(cvi_ctrl & M00389_CONTROL_BITMAP_VSYNC_POLARITY_LOW_MSK) ?
+			"VSync- " : "VSync+ ");
+	cobalt_info("rx%d: cvi status: %s%s\n", rx,
+		(cvi_stat & M00389_STATUS_BITMAP_LOCK_MSK) ?
+			"lock " : "no-lock ",
+		(cvi_stat & M00389_STATUS_BITMAP_ERROR_MSK) ?
+			"error " : "no-error ");
+
+	cobalt_info("rx%d: Measurements: %s%s%s%s%s%s%s\n", rx,
+		(vmr_ctrl & M00233_CONTROL_BITMAP_HSYNC_POLARITY_LOW_MSK) ?
+			"HSync- " : "HSync+ ",
+		(vmr_ctrl & M00233_CONTROL_BITMAP_VSYNC_POLARITY_LOW_MSK) ?
+			"VSync- " : "VSync+ ",
+		(vmr_ctrl & M00233_CONTROL_BITMAP_ENABLE_MEASURE_MSK) ?
+			"enabled " : "disabled ",
+		(vmr_ctrl & M00233_CONTROL_BITMAP_ENABLE_INTERRUPT_MSK) ?
+			"irq-enabled " : "irq-disabled ",
+		(vmr_ctrl & M00233_CONTROL_BITMAP_UPDATE_ON_HSYNC_MSK) ?
+			"update-on-hsync " : "",
+		(vmr_stat & M00233_STATUS_BITMAP_HSYNC_TIMEOUT_MSK) ?
+			"hsync-timeout " : "",
+		(vmr_stat & M00233_STATUS_BITMAP_INIT_DONE_MSK) ?
+			"init-done" : "");
+	cobalt_info("rx%d: irq_status: 0x%02x irq_triggers: 0x%02x\n", rx,
+			ioread32(&vmr->irq_status) & 0xff,
+			ioread32(&vmr->irq_triggers) & 0xff);
+	cobalt_info("rx%d: vsync: %d\n", rx, ioread32(&vmr->vsync_time));
+	cobalt_info("rx%d: vbp: %d\n", rx, ioread32(&vmr->vback_porch));
+	cobalt_info("rx%d: vact: %d\n", rx, ioread32(&vmr->vactive_area));
+	cobalt_info("rx%d: vfb: %d\n", rx, ioread32(&vmr->vfront_porch));
+	cobalt_info("rx%d: hsync: %d\n", rx, ioread32(&vmr->hsync_time));
+	cobalt_info("rx%d: hbp: %d\n", rx, ioread32(&vmr->hback_porch));
+	cobalt_info("rx%d: hact: %d\n", rx, ioread32(&vmr->hactive_area));
+	cobalt_info("rx%d: hfb: %d\n", rx, ioread32(&vmr->hfront_porch));
+	cobalt_info("rx%d: Freewheeling: %s%s%s\n", rx,
+		(ioread32(&fw->ctrl) & M00473_CTRL_BITMAP_ENABLE_MSK) ?
+			"enabled " : "disabled ",
+		(ioread32(&fw->ctrl) & M00473_CTRL_BITMAP_FORCE_FREEWHEEL_MODE_MSK) ?
+			"forced " : "",
+		(ioread32(&fw->status) & M00473_STATUS_BITMAP_FREEWHEEL_MODE_MSK) ?
+			"freewheeling " : "video-passthrough ");
+	iowrite32(0xff, &vmr->irq_status);
+	cobalt_info("rx%d: Clock Loss Detection: %s%s\n", rx,
+		(ioread32(&clkloss->ctrl) & M00479_CTRL_BITMAP_ENABLE_MSK) ?
+			"enabled " : "disabled ",
+		(ioread32(&clkloss->status) & M00479_STATUS_BITMAP_CLOCK_MISSING_MSK) ?
+			"clock-missing " : "found-clock ");
+	cobalt_info("rx%d: Packer: %x\n", rx, ioread32(&packer->control));
+}
+
+static int cobalt_log_status(struct file *file, void *priv_fh)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct cobalt *cobalt = s->cobalt;
+	struct m00514_syncgen_flow_evcnt_regmap __iomem *vo =
+		COBALT_TX_BASE(cobalt);
+	u8 stat;
+
+	cobalt_info("%s", cobalt->hdl_info);
+	cobalt_info("sysctrl: %08x, sysstat: %08x\n",
+			cobalt_g_sysctrl(cobalt),
+			cobalt_g_sysstat(cobalt));
+	cobalt_info("dma channel: %d, video channel: %d\n",
+			s->dma_channel, s->video_channel);
+	cobalt_pcie_status_show(cobalt);
+	cobalt_cpld_status(cobalt);
+	cobalt_irq_log_status(cobalt);
+	v4l2_subdev_call(s->sd, core, log_status);
+	if (!s->is_output) {
+		cobalt_video_input_status_show(s);
+		return 0;
+	}
+
+	stat = ioread32(&vo->rd_status);
+
+	cobalt_info("tx: status: %s%s\n",
+		(stat & M00514_RD_STATUS_BITMAP_FLOW_CTRL_NO_DATA_ERROR_MSK) ?
+			"no_data " : "",
+		(stat & M00514_RD_STATUS_BITMAP_READY_BUFFER_FULL_MSK) ?
+			"ready_buffer_full " : "");
+	cobalt_info("tx: evcnt: %d\n", ioread32(&vo->rd_evcnt_count));
+	return 0;
+}
+
+static int cobalt_enum_dv_timings(struct file *file, void *priv_fh,
+				    struct v4l2_enum_dv_timings *timings)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+
+	if (s->input == 1) {
+		if (timings->index)
+			return -EINVAL;
+		memset(timings->reserved, 0, sizeof(timings->reserved));
+		timings->timings = cea1080p60;
+		return 0;
+	}
+	timings->pad = 0;
+	return v4l2_subdev_call(s->sd,
+			pad, enum_dv_timings, timings);
+}
+
+static int cobalt_s_dv_timings(struct file *file, void *priv_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	int err;
+
+	if (vb2_is_busy(&s->q))
+		return -EBUSY;
+
+	if (s->input == 1) {
+		*timings = cea1080p60;
+		return 0;
+	}
+	err = v4l2_subdev_call(s->sd,
+			video, s_dv_timings, timings);
+	if (!err) {
+		s->timings = *timings;
+		s->width = timings->bt.width;
+		s->height = timings->bt.height;
+		s->stride = timings->bt.width * s->bpp;
+	}
+	return err;
+}
+
+static int cobalt_g_dv_timings(struct file *file, void *priv_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+
+	if (s->input == 1) {
+		*timings = cea1080p60;
+		return 0;
+	}
+	return v4l2_subdev_call(s->sd,
+			video, g_dv_timings, timings);
+}
+
+static int cobalt_query_dv_timings(struct file *file, void *priv_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+
+	if (s->input == 1) {
+		*timings = cea1080p60;
+		return 0;
+	}
+	return v4l2_subdev_call(s->sd,
+			video, query_dv_timings, timings);
+}
+
+static int cobalt_dv_timings_cap(struct file *file, void *priv_fh,
+				    struct v4l2_dv_timings_cap *cap)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+
+	cap->pad = 0;
+	return v4l2_subdev_call(s->sd,
+			pad, dv_timings_cap, cap);
+}
+
+static int cobalt_enum_fmt_vid_cap(struct file *file, void *priv_fh,
+		struct v4l2_fmtdesc *f)
+{
+	switch (f->index) {
+	case 0:
+		strlcpy(f->description, "YUV 4:2:2", sizeof(f->description));
+		f->pixelformat = V4L2_PIX_FMT_YUYV;
+		break;
+	case 1:
+		strlcpy(f->description, "RGB24", sizeof(f->description));
+		f->pixelformat = V4L2_PIX_FMT_RGB24;
+		break;
+	case 2:
+		strlcpy(f->description, "RGB32", sizeof(f->description));
+		f->pixelformat = V4L2_PIX_FMT_BGR32;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cobalt_g_fmt_vid_cap(struct file *file, void *priv_fh,
+		struct v4l2_format *f)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format sd_fmt;
+
+	pix->width = s->width;
+	pix->height = s->height;
+	pix->bytesperline = s->stride;
+	pix->field = V4L2_FIELD_NONE;
+
+	if (s->input == 1) {
+		pix->colorspace = V4L2_COLORSPACE_SRGB;
+	} else {
+		sd_fmt.pad = s->pad_source;
+		sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		v4l2_subdev_call(s->sd, pad, get_fmt, NULL, &sd_fmt);
+		v4l2_fill_pix_format(pix, &sd_fmt.format);
+	}
+
+	pix->pixelformat = s->pixfmt;
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	return 0;
+}
+
+static int cobalt_try_fmt_vid_cap(struct file *file, void *priv_fh,
+		struct v4l2_format *f)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format sd_fmt;
+
+	/* Check for min (QCIF) and max (Full HD) size */
+	if ((pix->width < 176) || (pix->height < 144)) {
+		pix->width = 176;
+		pix->height = 144;
+	}
+
+	if ((pix->width > 1920) || (pix->height > 1080)) {
+		pix->width = 1920;
+		pix->height = 1080;
+	}
+
+	/* Make width multiple of 4 */
+	pix->width &= ~0x3;
+
+	/* Make height multiple of 2 */
+	pix->height &= ~0x1;
+
+	if (s->input == 1) {
+		/* Generator => fixed format only */
+		pix->width = 1920;
+		pix->height = 1080;
+		pix->colorspace = V4L2_COLORSPACE_SRGB;
+	} else {
+		sd_fmt.pad = s->pad_source;
+		sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+		v4l2_subdev_call(s->sd, pad, get_fmt, NULL, &sd_fmt);
+		v4l2_fill_pix_format(pix, &sd_fmt.format);
+	}
+
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+	default:
+		pix->bytesperline = max(pix->bytesperline & ~0x3,
+				pix->width * COBALT_BYTES_PER_PIXEL_YUYV);
+		pix->pixelformat = V4L2_PIX_FMT_YUYV;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		pix->bytesperline = max(pix->bytesperline & ~0x3,
+				pix->width * COBALT_BYTES_PER_PIXEL_RGB24);
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		pix->bytesperline = max(pix->bytesperline & ~0x3,
+				pix->width * COBALT_BYTES_PER_PIXEL_RGB32);
+		break;
+	}
+
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->field = V4L2_FIELD_NONE;
+	pix->priv = 0;
+
+	return 0;
+}
+
+static int cobalt_s_fmt_vid_cap(struct file *file, void *priv_fh,
+		struct v4l2_format *f)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	if (vb2_is_busy(&s->q))
+		return -EBUSY;
+
+	if (cobalt_try_fmt_vid_cap(file, priv_fh, f))
+		return -EINVAL;
+
+	s->width = pix->width;
+	s->height = pix->height;
+	s->stride = pix->bytesperline;
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+		s->bpp = COBALT_BYTES_PER_PIXEL_YUYV;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		s->bpp = COBALT_BYTES_PER_PIXEL_RGB24;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		s->bpp = COBALT_BYTES_PER_PIXEL_RGB32;
+		break;
+	default:
+		return -EINVAL;
+	}
+	s->pixfmt = pix->pixelformat;
+	cobalt_enable_input(s);
+
+	return 0;
+}
+
+static int cobalt_try_fmt_vid_out(struct file *file, void *priv_fh,
+		struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	/* Check for min (QCIF) and max (Full HD) size */
+	if ((pix->width < 176) || (pix->height < 144)) {
+		pix->width = 176;
+		pix->height = 144;
+	}
+
+	if ((pix->width > 1920) || (pix->height > 1080)) {
+		pix->width = 1920;
+		pix->height = 1080;
+	}
+
+	/* Make width multiple of 4 */
+	pix->width &= ~0x3;
+
+	/* Make height multiple of 2 */
+	pix->height &= ~0x1;
+
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+	default:
+		pix->bytesperline = max(pix->bytesperline & ~0x3,
+				pix->width * COBALT_BYTES_PER_PIXEL_YUYV);
+		pix->pixelformat = V4L2_PIX_FMT_YUYV;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		pix->bytesperline = max(pix->bytesperline & ~0x3,
+				pix->width * COBALT_BYTES_PER_PIXEL_RGB32);
+		break;
+	}
+
+	pix->sizeimage = pix->bytesperline * pix->height;
+	pix->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int cobalt_g_fmt_vid_out(struct file *file, void *priv_fh,
+		struct v4l2_format *f)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+
+	pix->width = s->width;
+	pix->height = s->height;
+	pix->bytesperline = s->stride;
+	pix->field = V4L2_FIELD_NONE;
+	pix->pixelformat = s->pixfmt;
+	pix->colorspace = s->colorspace;
+	pix->xfer_func = s->xfer_func;
+	pix->ycbcr_enc = s->ycbcr_enc;
+	pix->quantization = s->quantization;
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	return 0;
+}
+
+static int cobalt_enum_fmt_vid_out(struct file *file, void *priv_fh,
+		struct v4l2_fmtdesc *f)
+{
+	switch (f->index) {
+	case 0:
+		strlcpy(f->description, "YUV 4:2:2", sizeof(f->description));
+		f->pixelformat = V4L2_PIX_FMT_YUYV;
+		break;
+	case 1:
+		strlcpy(f->description, "RGB32", sizeof(f->description));
+		f->pixelformat = V4L2_PIX_FMT_BGR32;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cobalt_s_fmt_vid_out(struct file *file, void *priv_fh,
+		struct v4l2_format *f)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_subdev_format sd_fmt = { 0 };
+	u32 code;
+
+	if (cobalt_try_fmt_vid_out(file, priv_fh, f))
+		return -EINVAL;
+
+	if (vb2_is_busy(&s->q) && (pix->pixelformat != s->pixfmt ||
+	    pix->width != s->width || pix->height != s->height ||
+	    pix->bytesperline != s->stride))
+		return -EBUSY;
+
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_YUYV:
+		s->bpp = COBALT_BYTES_PER_PIXEL_YUYV;
+		code = MEDIA_BUS_FMT_UYVY8_1X16;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+		s->bpp = COBALT_BYTES_PER_PIXEL_RGB32;
+		code = MEDIA_BUS_FMT_RGB888_1X24;
+		break;
+	default:
+		return -EINVAL;
+	}
+	s->width = pix->width;
+	s->height = pix->height;
+	s->stride = pix->bytesperline;
+	s->pixfmt = pix->pixelformat;
+	s->colorspace = pix->colorspace;
+	s->xfer_func = pix->xfer_func;
+	s->ycbcr_enc = pix->ycbcr_enc;
+	s->quantization = pix->quantization;
+	sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	v4l2_fill_mbus_format(&sd_fmt.format, pix, code);
+	v4l2_subdev_call(s->sd, pad, set_fmt, NULL, &sd_fmt);
+	return 0;
+}
+
+static int cobalt_enum_input(struct file *file, void *priv_fh,
+				 struct v4l2_input *inp)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+
+	if (inp->index > 1)
+		return -EINVAL;
+	if (inp->index == 0)
+		snprintf(inp->name, sizeof(inp->name),
+				"HDMI-%d", s->video_channel);
+	else
+		snprintf(inp->name, sizeof(inp->name),
+				"Generator-%d", s->video_channel);
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
+	if (inp->index == 1)
+		return 0;
+	return v4l2_subdev_call(s->sd,
+			video, g_input_status, &inp->status);
+}
+
+static int cobalt_g_input(struct file *file, void *priv_fh, unsigned int *i)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+
+	*i = s->input;
+	return 0;
+}
+
+static int cobalt_s_input(struct file *file, void *priv_fh, unsigned int i)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+
+	if (i >= 2)
+		return -EINVAL;
+	if (vb2_is_busy(&s->q))
+		return -EBUSY;
+	s->input = i;
+
+	cobalt_enable_input(s);
+
+	if (s->input == 1) /* Test Pattern Generator */
+		return 0;
+
+	return v4l2_subdev_call(s->sd, video, s_routing,
+			ADV76XX_PAD_HDMI_PORT_A, 0, 0);
+}
+
+static int cobalt_enum_output(struct file *file, void *priv_fh,
+				 struct v4l2_output *out)
+{
+	if (out->index)
+		return -EINVAL;
+	snprintf(out->name, sizeof(out->name), "HDMI-%d", out->index);
+	out->type = V4L2_OUTPUT_TYPE_ANALOG;
+	out->capabilities = V4L2_OUT_CAP_DV_TIMINGS;
+	return 0;
+}
+
+static int cobalt_g_output(struct file *file, void *priv_fh, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int cobalt_s_output(struct file *file, void *priv_fh, unsigned int i)
+{
+	return i ? -EINVAL : 0;
+}
+
+static int cobalt_g_edid(struct file *file, void *fh, struct v4l2_edid *edid)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	u32 pad = edid->pad;
+	int ret;
+
+	if (edid->pad >= (s->is_output ? 1 : 2))
+		return -EINVAL;
+	edid->pad = 0;
+	ret = v4l2_subdev_call(s->sd, pad, get_edid, edid);
+	edid->pad = pad;
+	return ret;
+}
+
+static int cobalt_s_edid(struct file *file, void *fh, struct v4l2_edid *edid)
+{
+	struct cobalt_stream *s = video_drvdata(file);
+	u32 pad = edid->pad;
+	int ret;
+
+	if (edid->pad >= 2)
+		return -EINVAL;
+	edid->pad = 0;
+	ret = v4l2_subdev_call(s->sd, pad, set_edid, edid);
+	edid->pad = pad;
+	return ret;
+}
+
+static int cobalt_subscribe_event(struct v4l2_fh *fh,
+				  const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_event_subscribe(fh, sub, 4, NULL);
+	}
+	return v4l2_ctrl_subscribe_event(fh, sub);
+}
+
+static int cobalt_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
+{
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	a->parm.capture.timeperframe.numerator = 1;
+	a->parm.capture.timeperframe.denominator = 60;
+	a->parm.capture.readbuffers = 3;
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops cobalt_ioctl_ops = {
+	.vidioc_querycap		= cobalt_querycap,
+	.vidioc_g_parm			= cobalt_g_parm,
+	.vidioc_log_status		= cobalt_log_status,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	.vidioc_enum_input		= cobalt_enum_input,
+	.vidioc_g_input			= cobalt_g_input,
+	.vidioc_s_input			= cobalt_s_input,
+	.vidioc_enum_fmt_vid_cap	= cobalt_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap		= cobalt_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= cobalt_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= cobalt_try_fmt_vid_cap,
+	.vidioc_enum_output		= cobalt_enum_output,
+	.vidioc_g_output		= cobalt_g_output,
+	.vidioc_s_output		= cobalt_s_output,
+	.vidioc_enum_fmt_vid_out	= cobalt_enum_fmt_vid_out,
+	.vidioc_g_fmt_vid_out		= cobalt_g_fmt_vid_out,
+	.vidioc_s_fmt_vid_out		= cobalt_s_fmt_vid_out,
+	.vidioc_try_fmt_vid_out		= cobalt_try_fmt_vid_out,
+	.vidioc_s_dv_timings		= cobalt_s_dv_timings,
+	.vidioc_g_dv_timings		= cobalt_g_dv_timings,
+	.vidioc_query_dv_timings	= cobalt_query_dv_timings,
+	.vidioc_enum_dv_timings		= cobalt_enum_dv_timings,
+	.vidioc_dv_timings_cap		= cobalt_dv_timings_cap,
+	.vidioc_g_edid			= cobalt_g_edid,
+	.vidioc_s_edid			= cobalt_s_edid,
+	.vidioc_subscribe_event		= cobalt_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register              = cobalt_g_register,
+	.vidioc_s_register              = cobalt_s_register,
+#endif
+};
+
+static const struct v4l2_ioctl_ops cobalt_ioctl_empty_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register              = cobalt_g_register,
+	.vidioc_s_register              = cobalt_s_register,
+#endif
+};
+
+/* Register device nodes */
+
+static const struct v4l2_file_operations cobalt_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.unlocked_ioctl = video_ioctl2,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+	.read = vb2_fop_read,
+};
+
+static const struct v4l2_file_operations cobalt_out_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.unlocked_ioctl = video_ioctl2,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+	.write = vb2_fop_write,
+};
+
+static const struct v4l2_file_operations cobalt_empty_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.unlocked_ioctl = video_ioctl2,
+	.release = v4l2_fh_release,
+};
+
+static int cobalt_node_register(struct cobalt *cobalt, int node)
+{
+	static const struct v4l2_dv_timings dv1080p60 =
+		V4L2_DV_BT_CEA_1920X1080P60;
+	struct cobalt_stream *s = cobalt->streams + node;
+	struct video_device *vdev = &s->vdev;
+	struct vb2_queue *q = &s->q;
+	int ret;
+
+	mutex_init(&s->lock);
+	spin_lock_init(&s->irqlock);
+
+	snprintf(vdev->name, sizeof(vdev->name),
+			"%s-%d", cobalt->v4l2_dev.name, node);
+	s->width = 1920;
+	/* Audio frames are just 4 lines of 1920 bytes */
+	s->height = s->is_audio ? 4 : 1080;
+
+	if (s->is_audio) {
+		s->bpp = 1;
+		s->pixfmt = V4L2_PIX_FMT_GREY;
+	} else if (s->is_output) {
+		s->bpp = COBALT_BYTES_PER_PIXEL_RGB32;
+		s->pixfmt = V4L2_PIX_FMT_BGR32;
+	} else {
+		s->bpp = COBALT_BYTES_PER_PIXEL_YUYV;
+		s->pixfmt = V4L2_PIX_FMT_YUYV;
+	}
+	s->colorspace = V4L2_COLORSPACE_SRGB;
+	s->stride = s->width * s->bpp;
+
+	if (!s->is_audio) {
+		if (s->is_dummy)
+			cobalt_warn("Setting up dummy video node %d\n", node);
+		vdev->v4l2_dev = &cobalt->v4l2_dev;
+		if (s->is_dummy)
+			vdev->fops = &cobalt_empty_fops;
+		else
+			vdev->fops = s->is_output ? &cobalt_out_fops :
+						    &cobalt_fops;
+		vdev->release = video_device_release_empty;
+		vdev->vfl_dir = s->is_output ? VFL_DIR_TX : VFL_DIR_RX;
+		vdev->lock = &s->lock;
+		if (s->sd)
+			vdev->ctrl_handler = s->sd->ctrl_handler;
+		s->timings = dv1080p60;
+		v4l2_subdev_call(s->sd, video, s_dv_timings, &s->timings);
+		if (!s->is_output && s->sd)
+			cobalt_enable_input(s);
+		vdev->ioctl_ops = s->is_dummy ? &cobalt_ioctl_empty_ops :
+				  &cobalt_ioctl_ops;
+	}
+
+	INIT_LIST_HEAD(&s->bufs);
+	q->type = s->is_output ? V4L2_BUF_TYPE_VIDEO_OUTPUT :
+				 V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+	q->io_modes |= s->is_output ? VB2_WRITE : VB2_READ;
+	q->drv_priv = s;
+	q->buf_struct_size = sizeof(struct cobalt_buffer);
+	q->ops = &cobalt_qops;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->min_buffers_needed = 2;
+	q->lock = &s->lock;
+	vdev->queue = q;
+
+	video_set_drvdata(vdev, s);
+	ret = vb2_queue_init(q);
+	if (!s->is_audio && ret == 0)
+		ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
+	else if (!s->is_dummy)
+		ret = cobalt_alsa_init(s);
+
+	if (ret < 0) {
+		if (!s->is_audio)
+			cobalt_err("couldn't register v4l2 device node %d\n",
+					node);
+		return ret;
+	}
+	cobalt_info("registered node %d\n", node);
+	return 0;
+}
+
+/* Initialize v4l2 variables and register v4l2 devices */
+int cobalt_nodes_register(struct cobalt *cobalt)
+{
+	int node, ret;
+
+	/* Setup V4L2 Devices */
+	for (node = 0; node < COBALT_NUM_STREAMS; node++) {
+		ret = cobalt_node_register(cobalt, node);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+/* Unregister v4l2 devices */
+void cobalt_nodes_unregister(struct cobalt *cobalt)
+{
+	int node;
+
+	/* Teardown all streams */
+	for (node = 0; node < COBALT_NUM_STREAMS; node++) {
+		struct cobalt_stream *s = cobalt->streams + node;
+		struct video_device *vdev = &s->vdev;
+
+		if (!s->is_audio)
+			video_unregister_device(vdev);
+		else if (!s->is_dummy)
+			cobalt_alsa_exit(s);
+	}
+}
diff --git a/drivers/media/pci/cobalt/cobalt-v4l2.h b/drivers/media/pci/cobalt/cobalt-v4l2.h
new file mode 100644
index 000000000000..62be553cd8e2
--- /dev/null
+++ b/drivers/media/pci/cobalt/cobalt-v4l2.h
@@ -0,0 +1,22 @@
+/*
+ *  cobalt V4L2 API
+ *
+ *  Copyright 2012-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  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.
+ */
+
+int cobalt_nodes_register(struct cobalt *cobalt);
+void cobalt_nodes_unregister(struct cobalt *cobalt);
diff --git a/drivers/media/pci/cobalt/m00233_video_measure_memmap_package.h b/drivers/media/pci/cobalt/m00233_video_measure_memmap_package.h
new file mode 100644
index 000000000000..9bc9ef1fd3a8
--- /dev/null
+++ b/drivers/media/pci/cobalt/m00233_video_measure_memmap_package.h
@@ -0,0 +1,115 @@
+/*
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef M00233_VIDEO_MEASURE_MEMMAP_PACKAGE_H
+#define M00233_VIDEO_MEASURE_MEMMAP_PACKAGE_H
+
+/*******************************************************************
+ * Register Block
+ * M00233_VIDEO_MEASURE_MEMMAP_PACKAGE_VHD_REGMAP
+ *******************************************************************/
+struct m00233_video_measure_regmap {
+	uint32_t irq_status;        /* Reg 0x0000 */
+	/* The vertical counter starts on rising edge of vsync */
+	uint32_t vsync_time;        /* Reg 0x0004 */
+	uint32_t vback_porch;       /* Reg 0x0008 */
+	uint32_t vactive_area;      /* Reg 0x000c */
+	uint32_t vfront_porch;      /* Reg 0x0010 */
+	/* The horizontal counter starts on rising edge of hsync. */
+	uint32_t hsync_time;        /* Reg 0x0014 */
+	uint32_t hback_porch;       /* Reg 0x0018 */
+	uint32_t hactive_area;      /* Reg 0x001c */
+	uint32_t hfront_porch;      /* Reg 0x0020 */
+	uint32_t control;           /* Reg 0x0024, Default=0x0 */
+	uint32_t irq_triggers;      /* Reg 0x0028, Default=0xff */
+	/* Value is given in number of register bus clock periods between */
+	/* falling and rising edge of hsync. Must be non-zero. */
+	uint32_t hsync_timeout_val; /* Reg 0x002c, Default=0x1fff */
+	uint32_t status;            /* Reg 0x0030 */
+};
+
+#define M00233_VIDEO_MEASURE_REG_IRQ_STATUS_OFST 0
+#define M00233_VIDEO_MEASURE_REG_VSYNC_TIME_OFST 4
+#define M00233_VIDEO_MEASURE_REG_VBACK_PORCH_OFST 8
+#define M00233_VIDEO_MEASURE_REG_VACTIVE_AREA_OFST 12
+#define M00233_VIDEO_MEASURE_REG_VFRONT_PORCH_OFST 16
+#define M00233_VIDEO_MEASURE_REG_HSYNC_TIME_OFST 20
+#define M00233_VIDEO_MEASURE_REG_HBACK_PORCH_OFST 24
+#define M00233_VIDEO_MEASURE_REG_HACTIVE_AREA_OFST 28
+#define M00233_VIDEO_MEASURE_REG_HFRONT_PORCH_OFST 32
+#define M00233_VIDEO_MEASURE_REG_CONTROL_OFST 36
+#define M00233_VIDEO_MEASURE_REG_IRQ_TRIGGERS_OFST 40
+#define M00233_VIDEO_MEASURE_REG_HSYNC_TIMEOUT_VAL_OFST 44
+#define M00233_VIDEO_MEASURE_REG_STATUS_OFST 48
+
+/*******************************************************************
+ * Bit Mask for register
+ * M00233_VIDEO_MEASURE_MEMMAP_PACKAGE_VHD_BITMAP
+ *******************************************************************/
+/* irq_status [7:0] */
+#define M00233_IRQ_STATUS_BITMAP_VSYNC_TIME_OFST      (0)
+#define M00233_IRQ_STATUS_BITMAP_VSYNC_TIME_MSK       (0x1 << M00233_IRQ_STATUS_BITMAP_VSYNC_TIME_OFST)
+#define M00233_IRQ_STATUS_BITMAP_VBACK_PORCH_OFST     (1)
+#define M00233_IRQ_STATUS_BITMAP_VBACK_PORCH_MSK      (0x1 << M00233_IRQ_STATUS_BITMAP_VBACK_PORCH_OFST)
+#define M00233_IRQ_STATUS_BITMAP_VACTIVE_AREA_OFST    (2)
+#define M00233_IRQ_STATUS_BITMAP_VACTIVE_AREA_MSK     (0x1 << M00233_IRQ_STATUS_BITMAP_VACTIVE_AREA_OFST)
+#define M00233_IRQ_STATUS_BITMAP_VFRONT_PORCH_OFST    (3)
+#define M00233_IRQ_STATUS_BITMAP_VFRONT_PORCH_MSK     (0x1 << M00233_IRQ_STATUS_BITMAP_VFRONT_PORCH_OFST)
+#define M00233_IRQ_STATUS_BITMAP_HSYNC_TIME_OFST      (4)
+#define M00233_IRQ_STATUS_BITMAP_HSYNC_TIME_MSK       (0x1 << M00233_IRQ_STATUS_BITMAP_HSYNC_TIME_OFST)
+#define M00233_IRQ_STATUS_BITMAP_HBACK_PORCH_OFST     (5)
+#define M00233_IRQ_STATUS_BITMAP_HBACK_PORCH_MSK      (0x1 << M00233_IRQ_STATUS_BITMAP_HBACK_PORCH_OFST)
+#define M00233_IRQ_STATUS_BITMAP_HACTIVE_AREA_OFST    (6)
+#define M00233_IRQ_STATUS_BITMAP_HACTIVE_AREA_MSK     (0x1 << M00233_IRQ_STATUS_BITMAP_HACTIVE_AREA_OFST)
+#define M00233_IRQ_STATUS_BITMAP_HFRONT_PORCH_OFST    (7)
+#define M00233_IRQ_STATUS_BITMAP_HFRONT_PORCH_MSK     (0x1 << M00233_IRQ_STATUS_BITMAP_HFRONT_PORCH_OFST)
+/* control [4:0] */
+#define M00233_CONTROL_BITMAP_HSYNC_POLARITY_LOW_OFST (0)
+#define M00233_CONTROL_BITMAP_HSYNC_POLARITY_LOW_MSK  (0x1 << M00233_CONTROL_BITMAP_HSYNC_POLARITY_LOW_OFST)
+#define M00233_CONTROL_BITMAP_VSYNC_POLARITY_LOW_OFST (1)
+#define M00233_CONTROL_BITMAP_VSYNC_POLARITY_LOW_MSK  (0x1 << M00233_CONTROL_BITMAP_VSYNC_POLARITY_LOW_OFST)
+#define M00233_CONTROL_BITMAP_ENABLE_MEASURE_OFST     (2)
+#define M00233_CONTROL_BITMAP_ENABLE_MEASURE_MSK      (0x1 << M00233_CONTROL_BITMAP_ENABLE_MEASURE_OFST)
+#define M00233_CONTROL_BITMAP_ENABLE_INTERRUPT_OFST   (3)
+#define M00233_CONTROL_BITMAP_ENABLE_INTERRUPT_MSK    (0x1 << M00233_CONTROL_BITMAP_ENABLE_INTERRUPT_OFST)
+#define M00233_CONTROL_BITMAP_UPDATE_ON_HSYNC_OFST    (4)
+#define M00233_CONTROL_BITMAP_UPDATE_ON_HSYNC_MSK     (0x1 << M00233_CONTROL_BITMAP_UPDATE_ON_HSYNC_OFST)
+/* irq_triggers [7:0] */
+#define M00233_IRQ_TRIGGERS_BITMAP_VSYNC_TIME_OFST    (0)
+#define M00233_IRQ_TRIGGERS_BITMAP_VSYNC_TIME_MSK     (0x1 << M00233_IRQ_TRIGGERS_BITMAP_VSYNC_TIME_OFST)
+#define M00233_IRQ_TRIGGERS_BITMAP_VBACK_PORCH_OFST   (1)
+#define M00233_IRQ_TRIGGERS_BITMAP_VBACK_PORCH_MSK    (0x1 << M00233_IRQ_TRIGGERS_BITMAP_VBACK_PORCH_OFST)
+#define M00233_IRQ_TRIGGERS_BITMAP_VACTIVE_AREA_OFST  (2)
+#define M00233_IRQ_TRIGGERS_BITMAP_VACTIVE_AREA_MSK   (0x1 << M00233_IRQ_TRIGGERS_BITMAP_VACTIVE_AREA_OFST)
+#define M00233_IRQ_TRIGGERS_BITMAP_VFRONT_PORCH_OFST  (3)
+#define M00233_IRQ_TRIGGERS_BITMAP_VFRONT_PORCH_MSK   (0x1 << M00233_IRQ_TRIGGERS_BITMAP_VFRONT_PORCH_OFST)
+#define M00233_IRQ_TRIGGERS_BITMAP_HSYNC_TIME_OFST    (4)
+#define M00233_IRQ_TRIGGERS_BITMAP_HSYNC_TIME_MSK     (0x1 << M00233_IRQ_TRIGGERS_BITMAP_HSYNC_TIME_OFST)
+#define M00233_IRQ_TRIGGERS_BITMAP_HBACK_PORCH_OFST   (5)
+#define M00233_IRQ_TRIGGERS_BITMAP_HBACK_PORCH_MSK    (0x1 << M00233_IRQ_TRIGGERS_BITMAP_HBACK_PORCH_OFST)
+#define M00233_IRQ_TRIGGERS_BITMAP_HACTIVE_AREA_OFST  (6)
+#define M00233_IRQ_TRIGGERS_BITMAP_HACTIVE_AREA_MSK   (0x1 << M00233_IRQ_TRIGGERS_BITMAP_HACTIVE_AREA_OFST)
+#define M00233_IRQ_TRIGGERS_BITMAP_HFRONT_PORCH_OFST  (7)
+#define M00233_IRQ_TRIGGERS_BITMAP_HFRONT_PORCH_MSK   (0x1 << M00233_IRQ_TRIGGERS_BITMAP_HFRONT_PORCH_OFST)
+/* status [1:0] */
+#define M00233_STATUS_BITMAP_HSYNC_TIMEOUT_OFST       (0)
+#define M00233_STATUS_BITMAP_HSYNC_TIMEOUT_MSK        (0x1 << M00233_STATUS_BITMAP_HSYNC_TIMEOUT_OFST)
+#define M00233_STATUS_BITMAP_INIT_DONE_OFST           (1)
+#define M00233_STATUS_BITMAP_INIT_DONE_MSK            (0x1 << M00233_STATUS_BITMAP_INIT_DONE_OFST)
+
+#endif /*M00233_VIDEO_MEASURE_MEMMAP_PACKAGE_H*/
diff --git a/drivers/media/pci/cobalt/m00235_fdma_packer_memmap_package.h b/drivers/media/pci/cobalt/m00235_fdma_packer_memmap_package.h
new file mode 100644
index 000000000000..a480529f561e
--- /dev/null
+++ b/drivers/media/pci/cobalt/m00235_fdma_packer_memmap_package.h
@@ -0,0 +1,44 @@
+/*
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef M00235_FDMA_PACKER_MEMMAP_PACKAGE_H
+#define M00235_FDMA_PACKER_MEMMAP_PACKAGE_H
+
+/*******************************************************************
+ * Register Block
+ * M00235_FDMA_PACKER_MEMMAP_PACKAGE_VHD_REGMAP
+ *******************************************************************/
+struct m00235_fdma_packer_regmap {
+	uint32_t control; /* Reg 0x0000, Default=0x0 */
+};
+
+#define M00235_FDMA_PACKER_REG_CONTROL_OFST 0
+
+/*******************************************************************
+ * Bit Mask for register
+ * M00235_FDMA_PACKER_MEMMAP_PACKAGE_VHD_BITMAP
+ *******************************************************************/
+/* control [3:0] */
+#define M00235_CONTROL_BITMAP_ENABLE_OFST        (0)
+#define M00235_CONTROL_BITMAP_ENABLE_MSK         (0x1 << M00235_CONTROL_BITMAP_ENABLE_OFST)
+#define M00235_CONTROL_BITMAP_PACK_FORMAT_OFST   (1)
+#define M00235_CONTROL_BITMAP_PACK_FORMAT_MSK    (0x3 << M00235_CONTROL_BITMAP_PACK_FORMAT_OFST)
+#define M00235_CONTROL_BITMAP_ENDIAN_FORMAT_OFST (3)
+#define M00235_CONTROL_BITMAP_ENDIAN_FORMAT_MSK  (0x1 << M00235_CONTROL_BITMAP_ENDIAN_FORMAT_OFST)
+
+#endif /*M00235_FDMA_PACKER_MEMMAP_PACKAGE_H*/
diff --git a/drivers/media/pci/cobalt/m00389_cvi_memmap_package.h b/drivers/media/pci/cobalt/m00389_cvi_memmap_package.h
new file mode 100644
index 000000000000..602419e589d3
--- /dev/null
+++ b/drivers/media/pci/cobalt/m00389_cvi_memmap_package.h
@@ -0,0 +1,59 @@
+/*
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef M00389_CVI_MEMMAP_PACKAGE_H
+#define M00389_CVI_MEMMAP_PACKAGE_H
+
+/*******************************************************************
+ * Register Block
+ * M00389_CVI_MEMMAP_PACKAGE_VHD_REGMAP
+ *******************************************************************/
+struct m00389_cvi_regmap {
+	uint32_t control;          /* Reg 0x0000, Default=0x0 */
+	uint32_t frame_width;      /* Reg 0x0004, Default=0x10 */
+	uint32_t frame_height;     /* Reg 0x0008, Default=0xc */
+	uint32_t freewheel_period; /* Reg 0x000c, Default=0x0 */
+	uint32_t error_color;      /* Reg 0x0010, Default=0x0 */
+	uint32_t status;           /* Reg 0x0014 */
+};
+
+#define M00389_CVI_REG_CONTROL_OFST 0
+#define M00389_CVI_REG_FRAME_WIDTH_OFST 4
+#define M00389_CVI_REG_FRAME_HEIGHT_OFST 8
+#define M00389_CVI_REG_FREEWHEEL_PERIOD_OFST 12
+#define M00389_CVI_REG_ERROR_COLOR_OFST 16
+#define M00389_CVI_REG_STATUS_OFST 20
+
+/*******************************************************************
+ * Bit Mask for register
+ * M00389_CVI_MEMMAP_PACKAGE_VHD_BITMAP
+ *******************************************************************/
+/* control [2:0] */
+#define M00389_CONTROL_BITMAP_ENABLE_OFST             (0)
+#define M00389_CONTROL_BITMAP_ENABLE_MSK              (0x1 << M00389_CONTROL_BITMAP_ENABLE_OFST)
+#define M00389_CONTROL_BITMAP_HSYNC_POLARITY_LOW_OFST (1)
+#define M00389_CONTROL_BITMAP_HSYNC_POLARITY_LOW_MSK  (0x1 << M00389_CONTROL_BITMAP_HSYNC_POLARITY_LOW_OFST)
+#define M00389_CONTROL_BITMAP_VSYNC_POLARITY_LOW_OFST (2)
+#define M00389_CONTROL_BITMAP_VSYNC_POLARITY_LOW_MSK  (0x1 << M00389_CONTROL_BITMAP_VSYNC_POLARITY_LOW_OFST)
+/* status [1:0] */
+#define M00389_STATUS_BITMAP_LOCK_OFST                (0)
+#define M00389_STATUS_BITMAP_LOCK_MSK                 (0x1 << M00389_STATUS_BITMAP_LOCK_OFST)
+#define M00389_STATUS_BITMAP_ERROR_OFST               (1)
+#define M00389_STATUS_BITMAP_ERROR_MSK                (0x1 << M00389_STATUS_BITMAP_ERROR_OFST)
+
+#endif /*M00389_CVI_MEMMAP_PACKAGE_H*/
diff --git a/drivers/media/pci/cobalt/m00460_evcnt_memmap_package.h b/drivers/media/pci/cobalt/m00460_evcnt_memmap_package.h
new file mode 100644
index 000000000000..95471c995067
--- /dev/null
+++ b/drivers/media/pci/cobalt/m00460_evcnt_memmap_package.h
@@ -0,0 +1,44 @@
+/*
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef M00460_EVCNT_MEMMAP_PACKAGE_H
+#define M00460_EVCNT_MEMMAP_PACKAGE_H
+
+/*******************************************************************
+ * Register Block
+ * M00460_EVCNT_MEMMAP_PACKAGE_VHD_REGMAP
+ *******************************************************************/
+struct m00460_evcnt_regmap {
+	uint32_t control; /* Reg 0x0000, Default=0x0 */
+	uint32_t count;   /* Reg 0x0004 */
+};
+
+#define M00460_EVCNT_REG_CONTROL_OFST 0
+#define M00460_EVCNT_REG_COUNT_OFST 4
+
+/*******************************************************************
+ * Bit Mask for register
+ * M00460_EVCNT_MEMMAP_PACKAGE_VHD_BITMAP
+ *******************************************************************/
+/* control [1:0] */
+#define M00460_CONTROL_BITMAP_ENABLE_OFST (0)
+#define M00460_CONTROL_BITMAP_ENABLE_MSK  (0x1 << M00460_CONTROL_BITMAP_ENABLE_OFST)
+#define M00460_CONTROL_BITMAP_CLEAR_OFST  (1)
+#define M00460_CONTROL_BITMAP_CLEAR_MSK   (0x1 << M00460_CONTROL_BITMAP_CLEAR_OFST)
+
+#endif /*M00460_EVCNT_MEMMAP_PACKAGE_H*/
diff --git a/drivers/media/pci/cobalt/m00473_freewheel_memmap_package.h b/drivers/media/pci/cobalt/m00473_freewheel_memmap_package.h
new file mode 100644
index 000000000000..384a3e156301
--- /dev/null
+++ b/drivers/media/pci/cobalt/m00473_freewheel_memmap_package.h
@@ -0,0 +1,57 @@
+/*
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef M00473_FREEWHEEL_MEMMAP_PACKAGE_H
+#define M00473_FREEWHEEL_MEMMAP_PACKAGE_H
+
+/*******************************************************************
+ * Register Block
+ * M00473_FREEWHEEL_MEMMAP_PACKAGE_VHD_REGMAP
+ *******************************************************************/
+struct m00473_freewheel_regmap {
+	uint32_t ctrl;          /* Reg 0x0000, Default=0x0 */
+	uint32_t status;        /* Reg 0x0004 */
+	uint32_t active_length; /* Reg 0x0008, Default=0x1fa400 */
+	uint32_t total_length;  /* Reg 0x000c, Default=0x31151b */
+	uint32_t data_width;    /* Reg 0x0010 */
+	uint32_t output_color;  /* Reg 0x0014, Default=0xffff */
+	uint32_t clk_freq;      /* Reg 0x0018 */
+};
+
+#define M00473_FREEWHEEL_REG_CTRL_OFST 0
+#define M00473_FREEWHEEL_REG_STATUS_OFST 4
+#define M00473_FREEWHEEL_REG_ACTIVE_LENGTH_OFST 8
+#define M00473_FREEWHEEL_REG_TOTAL_LENGTH_OFST 12
+#define M00473_FREEWHEEL_REG_DATA_WIDTH_OFST 16
+#define M00473_FREEWHEEL_REG_OUTPUT_COLOR_OFST 20
+#define M00473_FREEWHEEL_REG_CLK_FREQ_OFST 24
+
+/*******************************************************************
+ * Bit Mask for register
+ * M00473_FREEWHEEL_MEMMAP_PACKAGE_VHD_BITMAP
+ *******************************************************************/
+/* ctrl [1:0] */
+#define M00473_CTRL_BITMAP_ENABLE_OFST               (0)
+#define M00473_CTRL_BITMAP_ENABLE_MSK                (0x1 << M00473_CTRL_BITMAP_ENABLE_OFST)
+#define M00473_CTRL_BITMAP_FORCE_FREEWHEEL_MODE_OFST (1)
+#define M00473_CTRL_BITMAP_FORCE_FREEWHEEL_MODE_MSK  (0x1 << M00473_CTRL_BITMAP_FORCE_FREEWHEEL_MODE_OFST)
+/* status [0:0] */
+#define M00473_STATUS_BITMAP_FREEWHEEL_MODE_OFST     (0)
+#define M00473_STATUS_BITMAP_FREEWHEEL_MODE_MSK      (0x1 << M00473_STATUS_BITMAP_FREEWHEEL_MODE_OFST)
+
+#endif /*M00473_FREEWHEEL_MEMMAP_PACKAGE_H*/
diff --git a/drivers/media/pci/cobalt/m00479_clk_loss_detector_memmap_package.h b/drivers/media/pci/cobalt/m00479_clk_loss_detector_memmap_package.h
new file mode 100644
index 000000000000..2a029026bf82
--- /dev/null
+++ b/drivers/media/pci/cobalt/m00479_clk_loss_detector_memmap_package.h
@@ -0,0 +1,53 @@
+/*
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef M00479_CLK_LOSS_DETECTOR_MEMMAP_PACKAGE_H
+#define M00479_CLK_LOSS_DETECTOR_MEMMAP_PACKAGE_H
+
+/*******************************************************************
+ * Register Block
+ * M00479_CLK_LOSS_DETECTOR_MEMMAP_PACKAGE_VHD_REGMAP
+ *******************************************************************/
+struct m00479_clk_loss_detector_regmap {
+	/* Control module */
+	uint32_t ctrl;             /* Reg 0x0000, Default=0x0 */
+	uint32_t status;           /* Reg 0x0004 */
+	/* Number of ref clk cycles before checking the clock under test */
+	uint32_t ref_clk_cnt_val;  /* Reg 0x0008, Default=0xc4 */
+	/* Number of test clk cycles required in the ref_clk_cnt_val period
+	 * to ensure that the test clock is performing as expected */
+	uint32_t test_clk_cnt_val; /* Reg 0x000c, Default=0xa */
+};
+
+#define M00479_CLK_LOSS_DETECTOR_REG_CTRL_OFST 0
+#define M00479_CLK_LOSS_DETECTOR_REG_STATUS_OFST 4
+#define M00479_CLK_LOSS_DETECTOR_REG_REF_CLK_CNT_VAL_OFST 8
+#define M00479_CLK_LOSS_DETECTOR_REG_TEST_CLK_CNT_VAL_OFST 12
+
+/*******************************************************************
+ * Bit Mask for register
+ * M00479_CLK_LOSS_DETECTOR_MEMMAP_PACKAGE_VHD_BITMAP
+ *******************************************************************/
+/* ctrl [0:0] */
+#define M00479_CTRL_BITMAP_ENABLE_OFST          (0)
+#define M00479_CTRL_BITMAP_ENABLE_MSK           (0x1 << M00479_CTRL_BITMAP_ENABLE_OFST)
+/* status [0:0] */
+#define M00479_STATUS_BITMAP_CLOCK_MISSING_OFST (0)
+#define M00479_STATUS_BITMAP_CLOCK_MISSING_MSK  (0x1 << M00479_STATUS_BITMAP_CLOCK_MISSING_OFST)
+
+#endif /*M00479_CLK_LOSS_DETECTOR_MEMMAP_PACKAGE_H*/
diff --git a/drivers/media/pci/cobalt/m00514_syncgen_flow_evcnt_memmap_package.h b/drivers/media/pci/cobalt/m00514_syncgen_flow_evcnt_memmap_package.h
new file mode 100644
index 000000000000..bdef2df5d689
--- /dev/null
+++ b/drivers/media/pci/cobalt/m00514_syncgen_flow_evcnt_memmap_package.h
@@ -0,0 +1,88 @@
+/*
+ *  Copyright 2014-2015 Cisco Systems, Inc. and/or its affiliates.
+ *  All rights reserved.
+ *
+ *  This program is free software; you may 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.
+ *
+ *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ *  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ *  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ *  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ *  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ *  ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ *  CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ *  SOFTWARE.
+ */
+
+#ifndef M00514_SYNCGEN_FLOW_EVCNT_MEMMAP_PACKAGE_H
+#define M00514_SYNCGEN_FLOW_EVCNT_MEMMAP_PACKAGE_H
+
+/*******************************************************************
+ * Register Block
+ * M00514_SYNCGEN_FLOW_EVCNT_MEMMAP_PACKAGE_VHD_REGMAP
+ *******************************************************************/
+struct m00514_syncgen_flow_evcnt_regmap {
+	uint32_t control;                            /* Reg 0x0000, Default=0x0 */
+	uint32_t sync_generator_h_sync_length;       /* Reg 0x0004, Default=0x0 */
+	uint32_t sync_generator_h_backporch_length;  /* Reg 0x0008, Default=0x0 */
+	uint32_t sync_generator_h_active_length;     /* Reg 0x000c, Default=0x0 */
+	uint32_t sync_generator_h_frontporch_length; /* Reg 0x0010, Default=0x0 */
+	uint32_t sync_generator_v_sync_length;       /* Reg 0x0014, Default=0x0 */
+	uint32_t sync_generator_v_backporch_length;  /* Reg 0x0018, Default=0x0 */
+	uint32_t sync_generator_v_active_length;     /* Reg 0x001c, Default=0x0 */
+	uint32_t sync_generator_v_frontporch_length; /* Reg 0x0020, Default=0x0 */
+	uint32_t error_color;                        /* Reg 0x0024, Default=0x0 */
+	uint32_t rd_status;                          /* Reg 0x0028 */
+	uint32_t rd_evcnt_count;                     /* Reg 0x002c */
+};
+
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_CONTROL_OFST 0
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_SYNC_GENERATOR_H_SYNC_LENGTH_OFST 4
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_SYNC_GENERATOR_H_BACKPORCH_LENGTH_OFST 8
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_SYNC_GENERATOR_H_ACTIVE_LENGTH_OFST 12
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_SYNC_GENERATOR_H_FRONTPORCH_LENGTH_OFST 16
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_SYNC_GENERATOR_V_SYNC_LENGTH_OFST 20
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_SYNC_GENERATOR_V_BACKPORCH_LENGTH_OFST 24
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_SYNC_GENERATOR_V_ACTIVE_LENGTH_OFST 28
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_SYNC_GENERATOR_V_FRONTPORCH_LENGTH_OFST 32
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_ERROR_COLOR_OFST 36
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_RD_STATUS_OFST 40
+#define M00514_SYNCGEN_FLOW_EVCNT_REG_RD_EVCNT_COUNT_OFST 44
+
+/*******************************************************************
+ * Bit Mask for register
+ * M00514_SYNCGEN_FLOW_EVCNT_MEMMAP_PACKAGE_VHD_BITMAP
+ *******************************************************************/
+/* control [7:0] */
+#define M00514_CONTROL_BITMAP_SYNC_GENERATOR_LOAD_PARAM_OFST (0)
+#define M00514_CONTROL_BITMAP_SYNC_GENERATOR_LOAD_PARAM_MSK  (0x1 << M00514_CONTROL_BITMAP_SYNC_GENERATOR_LOAD_PARAM_OFST)
+#define M00514_CONTROL_BITMAP_SYNC_GENERATOR_ENABLE_OFST     (1)
+#define M00514_CONTROL_BITMAP_SYNC_GENERATOR_ENABLE_MSK      (0x1 << M00514_CONTROL_BITMAP_SYNC_GENERATOR_ENABLE_OFST)
+#define M00514_CONTROL_BITMAP_FLOW_CTRL_OUTPUT_ENABLE_OFST   (2)
+#define M00514_CONTROL_BITMAP_FLOW_CTRL_OUTPUT_ENABLE_MSK    (0x1 << M00514_CONTROL_BITMAP_FLOW_CTRL_OUTPUT_ENABLE_OFST)
+#define M00514_CONTROL_BITMAP_HSYNC_POLARITY_LOW_OFST        (3)
+#define M00514_CONTROL_BITMAP_HSYNC_POLARITY_LOW_MSK         (0x1 << M00514_CONTROL_BITMAP_HSYNC_POLARITY_LOW_OFST)
+#define M00514_CONTROL_BITMAP_VSYNC_POLARITY_LOW_OFST        (4)
+#define M00514_CONTROL_BITMAP_VSYNC_POLARITY_LOW_MSK         (0x1 << M00514_CONTROL_BITMAP_VSYNC_POLARITY_LOW_OFST)
+#define M00514_CONTROL_BITMAP_EVCNT_ENABLE_OFST              (5)
+#define M00514_CONTROL_BITMAP_EVCNT_ENABLE_MSK               (0x1 << M00514_CONTROL_BITMAP_EVCNT_ENABLE_OFST)
+#define M00514_CONTROL_BITMAP_EVCNT_CLEAR_OFST               (6)
+#define M00514_CONTROL_BITMAP_EVCNT_CLEAR_MSK                (0x1 << M00514_CONTROL_BITMAP_EVCNT_CLEAR_OFST)
+#define M00514_CONTROL_BITMAP_FORMAT_16_BPP_OFST             (7)
+#define M00514_CONTROL_BITMAP_FORMAT_16_BPP_MSK              (0x1 << M00514_CONTROL_BITMAP_FORMAT_16_BPP_OFST)
+/* error_color [23:0] */
+#define M00514_ERROR_COLOR_BITMAP_BLUE_OFST                  (0)
+#define M00514_ERROR_COLOR_BITMAP_BLUE_MSK                   (0xff << M00514_ERROR_COLOR_BITMAP_BLUE_OFST)
+#define M00514_ERROR_COLOR_BITMAP_GREEN_OFST                 (8)
+#define M00514_ERROR_COLOR_BITMAP_GREEN_MSK                  (0xff << M00514_ERROR_COLOR_BITMAP_GREEN_OFST)
+#define M00514_ERROR_COLOR_BITMAP_RED_OFST                   (16)
+#define M00514_ERROR_COLOR_BITMAP_RED_MSK                    (0xff << M00514_ERROR_COLOR_BITMAP_RED_OFST)
+/* rd_status [1:0] */
+#define M00514_RD_STATUS_BITMAP_FLOW_CTRL_NO_DATA_ERROR_OFST (0)
+#define M00514_RD_STATUS_BITMAP_FLOW_CTRL_NO_DATA_ERROR_MSK  (0x1 << M00514_RD_STATUS_BITMAP_FLOW_CTRL_NO_DATA_ERROR_OFST)
+#define M00514_RD_STATUS_BITMAP_READY_BUFFER_FULL_OFST       (1)
+#define M00514_RD_STATUS_BITMAP_READY_BUFFER_FULL_MSK        (0x1 << M00514_RD_STATUS_BITMAP_READY_BUFFER_FULL_OFST)
+
+#endif /*M00514_SYNCGEN_FLOW_EVCNT_MEMMAP_PACKAGE_H*/
diff --git a/drivers/media/pci/cx18/cx18-av-core.c b/drivers/media/pci/cx18/cx18-av-core.c
index 5a55630d09db..30bbe8d1ea55 100644
--- a/drivers/media/pci/cx18/cx18-av-core.c
+++ b/drivers/media/pci/cx18/cx18-av-core.c
@@ -945,14 +945,17 @@ static int cx18_av_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static int cx18_av_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+static int cx18_av_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *fmt = &format->format;
 	struct cx18_av_state *state = to_cx18_av_state(sd);
 	struct cx18 *cx = v4l2_get_subdevdata(sd);
 	int HSC, VSC, Vsrc, Hsrc, filter, Vlines;
 	int is_50Hz = !(state->std & V4L2_STD_525_60);
 
-	if (fmt->code != MEDIA_BUS_FMT_FIXED)
+	if (format->pad || fmt->code != MEDIA_BUS_FMT_FIXED)
 		return -EINVAL;
 
 	fmt->field = V4L2_FIELD_INTERLACED;
@@ -987,6 +990,9 @@ static int cx18_av_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt
 		return -ERANGE;
 	}
 
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
 	HSC = (Hsrc * (1 << 20)) / fmt->width - (1 << 20);
 	VSC = (1 << 16) - (Vsrc * (1 << 9) / Vlines - (1 << 9));
 	VSC &= 0x1fff;
@@ -1285,7 +1291,6 @@ static const struct v4l2_subdev_video_ops cx18_av_video_ops = {
 	.s_std = cx18_av_s_std,
 	.s_routing = cx18_av_s_video_routing,
 	.s_stream = cx18_av_s_stream,
-	.s_mbus_fmt = cx18_av_s_mbus_fmt,
 };
 
 static const struct v4l2_subdev_vbi_ops cx18_av_vbi_ops = {
@@ -1295,12 +1300,17 @@ static const struct v4l2_subdev_vbi_ops cx18_av_vbi_ops = {
 	.s_raw_fmt = cx18_av_s_raw_fmt,
 };
 
+static const struct v4l2_subdev_pad_ops cx18_av_pad_ops = {
+	.set_fmt = cx18_av_set_fmt,
+};
+
 static const struct v4l2_subdev_ops cx18_av_ops = {
 	.core = &cx18_av_general_ops,
 	.tuner = &cx18_av_tuner_ops,
 	.audio = &cx18_av_audio_ops,
 	.video = &cx18_av_video_ops,
 	.vbi = &cx18_av_vbi_ops,
+	.pad = &cx18_av_pad_ops,
 };
 
 int cx18_av_probe(struct cx18 *cx)
diff --git a/drivers/media/pci/cx18/cx18-controls.c b/drivers/media/pci/cx18/cx18-controls.c
index 4aeb7c6b8ce1..71227a155cba 100644
--- a/drivers/media/pci/cx18/cx18-controls.c
+++ b/drivers/media/pci/cx18/cx18-controls.c
@@ -93,13 +93,16 @@ static int cx18_s_video_encoding(struct cx2341x_handler *cxhdl, u32 val)
 {
 	struct cx18 *cx = container_of(cxhdl, struct cx18, cxhdl);
 	int is_mpeg1 = val == V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *fmt = &format.format;
 
 	/* fix videodecoder resolution */
-	fmt.width = cxhdl->width / (is_mpeg1 ? 2 : 1);
-	fmt.height = cxhdl->height;
-	fmt.code = MEDIA_BUS_FMT_FIXED;
-	v4l2_subdev_call(cx->sd_av, video, s_mbus_fmt, &fmt);
+	fmt->width = cxhdl->width / (is_mpeg1 ? 2 : 1);
+	fmt->height = cxhdl->height;
+	fmt->code = MEDIA_BUS_FMT_FIXED;
+	v4l2_subdev_call(cx->sd_av, pad, set_fmt, NULL, &format);
 	return 0;
 }
 
diff --git a/drivers/media/pci/cx18/cx18-driver.c b/drivers/media/pci/cx18/cx18-driver.c
index 83f5074706f9..260e462d91b4 100644
--- a/drivers/media/pci/cx18/cx18-driver.c
+++ b/drivers/media/pci/cx18/cx18-driver.c
@@ -786,11 +786,11 @@ static void cx18_init_struct2(struct cx18 *cx)
 {
 	int i;
 
-	for (i = 0; i < CX18_CARD_MAX_VIDEO_INPUTS; i++)
+	for (i = 0; i < CX18_CARD_MAX_VIDEO_INPUTS - 1; i++)
 		if (cx->card->video_inputs[i].video_type == 0)
 			break;
 	cx->nof_inputs = i;
-	for (i = 0; i < CX18_CARD_MAX_AUDIO_INPUTS; i++)
+	for (i = 0; i < CX18_CARD_MAX_AUDIO_INPUTS - 1; i++)
 		if (cx->card->audio_inputs[i].audio_type == 0)
 			break;
 	cx->nof_audio_inputs = i;
diff --git a/drivers/media/pci/cx18/cx18-ioctl.c b/drivers/media/pci/cx18/cx18-ioctl.c
index 79aee30d5fd8..55525af1f482 100644
--- a/drivers/media/pci/cx18/cx18-ioctl.c
+++ b/drivers/media/pci/cx18/cx18-ioctl.c
@@ -267,7 +267,9 @@ static int cx18_s_fmt_vid_cap(struct file *file, void *fh,
 {
 	struct cx18_open_id *id = fh2id(fh);
 	struct cx18 *cx = id->cx;
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	struct cx18_stream *s = &cx->streams[id->type];
 	int ret;
 	int w, h;
@@ -296,10 +298,10 @@ static int cx18_s_fmt_vid_cap(struct file *file, void *fh,
 		s->vb_bytes_per_line = 1440; /* Packed */
 	}
 
-	mbus_fmt.width = cx->cxhdl.width = w;
-	mbus_fmt.height = cx->cxhdl.height = h;
-	mbus_fmt.code = MEDIA_BUS_FMT_FIXED;
-	v4l2_subdev_call(cx->sd_av, video, s_mbus_fmt, &mbus_fmt);
+	format.format.width = cx->cxhdl.width = w;
+	format.format.height = cx->cxhdl.height = h;
+	format.format.code = MEDIA_BUS_FMT_FIXED;
+	v4l2_subdev_call(cx->sd_av, pad, set_fmt, NULL, &format);
 	return cx18_g_fmt_vid_cap(file, fh, fmt);
 }
 
diff --git a/drivers/media/pci/cx18/cx18-streams.c b/drivers/media/pci/cx18/cx18-streams.c
index c82d25d53341..c9860845264f 100644
--- a/drivers/media/pci/cx18/cx18-streams.c
+++ b/drivers/media/pci/cx18/cx18-streams.c
@@ -90,6 +90,7 @@ static struct {
 		"encoder PCM audio",
 		VFL_TYPE_GRABBER, CX18_V4L2_ENC_PCM_OFFSET,
 		PCI_DMA_FROMDEVICE,
+		V4L2_CAP_TUNER | V4L2_CAP_AUDIO | V4L2_CAP_READWRITE,
 	},
 	{	/* CX18_ENC_STREAM_TYPE_IDX */
 		"encoder IDX",
diff --git a/drivers/media/pci/cx23885/altera-ci.c b/drivers/media/pci/cx23885/altera-ci.c
index 0a91df2c9f08..aaf4e46ff3e9 100644
--- a/drivers/media/pci/cx23885/altera-ci.c
+++ b/drivers/media/pci/cx23885/altera-ci.c
@@ -759,7 +759,7 @@ int altera_ci_init(struct altera_ci_config *config, int ci_nr)
 	if (0 != ret)
 		goto err;
 
-       inter->state[ci_nr - 1] = state;
+	inter->state[ci_nr - 1] = state;
 
 	altera_hw_filt_init(config, ci_nr);
 
diff --git a/drivers/media/pci/cx23885/cx23885-dvb.c b/drivers/media/pci/cx23885/cx23885-dvb.c
index 745caabe3397..6e8c24cdb2cd 100644
--- a/drivers/media/pci/cx23885/cx23885-dvb.c
+++ b/drivers/media/pci/cx23885/cx23885-dvb.c
@@ -572,7 +572,8 @@ static struct stb6100_config prof_8000_stb6100_config = {
 	.refclock = 27000000,
 };
 
-static int p8000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int p8000_set_voltage(struct dvb_frontend *fe,
+			     enum fe_sec_voltage voltage)
 {
 	struct cx23885_tsport *port = fe->dvb->priv;
 	struct cx23885_dev *dev = port->dev;
@@ -587,7 +588,7 @@ static int p8000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 }
 
 static int dvbsky_t9580_set_voltage(struct dvb_frontend *fe,
-					fe_sec_voltage_t voltage)
+					enum fe_sec_voltage voltage)
 {
 	struct cx23885_tsport *port = fe->dvb->priv;
 	struct cx23885_dev *dev = port->dev;
@@ -616,7 +617,7 @@ static int dvbsky_t9580_set_voltage(struct dvb_frontend *fe,
 }
 
 static int dvbsky_s952_portc_set_voltage(struct dvb_frontend *fe,
-					fe_sec_voltage_t voltage)
+					enum fe_sec_voltage voltage)
 {
 	struct cx23885_tsport *port = fe->dvb->priv;
 	struct cx23885_dev *dev = port->dev;
@@ -856,18 +857,12 @@ static struct mt2063_config terratec_mt2063_config[] = {
 	},
 };
 
-static const struct tda10071_config hauppauge_tda10071_config = {
-	.demod_i2c_addr = 0x05,
-	.tuner_i2c_addr = 0x54,
+static const struct tda10071_platform_data hauppauge_tda10071_pdata = {
+	.clk = 40444000, /* 40.444 MHz */
 	.i2c_wr_max = 64,
 	.ts_mode = TDA10071_TS_SERIAL,
-	.spec_inv = 0,
-	.xtal = 40444000, /* 40.444 MHz */
 	.pll_multiplier = 20,
-};
-
-static const struct a8293_config hauppauge_a8293_config = {
-	.i2c_addr = 0x0b,
+	.tuner_i2c_addr = 0x54,
 };
 
 static const struct si2165_config hauppauge_hvr4400_si2165_config = {
@@ -1190,8 +1185,10 @@ static int dvb_register(struct cx23885_tsport *port)
 	struct i2c_board_info info;
 	struct i2c_adapter *adapter;
 	struct i2c_client *client_demod = NULL, *client_tuner = NULL;
+	struct i2c_client *client_sec = NULL;
 	const struct m88ds3103_config *p_m88ds3103_config = NULL;
-	int (*p_set_voltage)(struct dvb_frontend *fe, fe_sec_voltage_t voltage) = NULL;
+	int (*p_set_voltage)(struct dvb_frontend *fe,
+			     enum fe_sec_voltage voltage) = NULL;
 	int mfe_shared = 0; /* bus not shared by default */
 	int ret;
 
@@ -1797,21 +1794,46 @@ static int dvb_register(struct cx23885_tsport *port)
 
 		fe0->dvb.frontend->ops.set_voltage = p8000_set_voltage;
 		break;
-	case CX23885_BOARD_HAUPPAUGE_HVR4400:
+	case CX23885_BOARD_HAUPPAUGE_HVR4400: {
+		struct tda10071_platform_data tda10071_pdata = hauppauge_tda10071_pdata;
+		struct a8293_platform_data a8293_pdata = {};
+
 		i2c_bus = &dev->i2c_bus[0];
 		i2c_bus2 = &dev->i2c_bus[1];
 		switch (port->nr) {
 		/* port b */
 		case 1:
-			fe0->dvb.frontend = dvb_attach(tda10071_attach,
-						&hauppauge_tda10071_config,
-						&i2c_bus->i2c_adap);
-			if (fe0->dvb.frontend == NULL)
-				break;
-			if (!dvb_attach(a8293_attach, fe0->dvb.frontend,
-					&i2c_bus->i2c_adap,
-					&hauppauge_a8293_config))
+			/* attach demod + tuner combo */
+			memset(&info, 0, sizeof(info));
+			strlcpy(info.type, "tda10071_cx24118", I2C_NAME_SIZE);
+			info.addr = 0x05;
+			info.platform_data = &tda10071_pdata;
+			request_module("tda10071");
+			client_demod = i2c_new_device(&i2c_bus->i2c_adap, &info);
+			if (!client_demod || !client_demod->dev.driver)
+				goto frontend_detach;
+			if (!try_module_get(client_demod->dev.driver->owner)) {
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			fe0->dvb.frontend = tda10071_pdata.get_dvb_frontend(client_demod);
+			port->i2c_client_demod = client_demod;
+
+			/* attach SEC */
+			a8293_pdata.dvb_frontend = fe0->dvb.frontend;
+			memset(&info, 0, sizeof(info));
+			strlcpy(info.type, "a8293", I2C_NAME_SIZE);
+			info.addr = 0x0b;
+			info.platform_data = &a8293_pdata;
+			request_module("a8293");
+			client_sec = i2c_new_device(&i2c_bus->i2c_adap, &info);
+			if (!client_sec || !client_sec->dev.driver)
+				goto frontend_detach;
+			if (!try_module_get(client_sec->dev.driver->owner)) {
+				i2c_unregister_device(client_sec);
 				goto frontend_detach;
+			}
+			port->i2c_client_sec = client_sec;
 			break;
 		/* port c */
 		case 2:
@@ -1829,17 +1851,46 @@ static int dvb_register(struct cx23885_tsport *port)
 			break;
 		}
 		break;
-	case CX23885_BOARD_HAUPPAUGE_STARBURST:
+	}
+	case CX23885_BOARD_HAUPPAUGE_STARBURST: {
+		struct tda10071_platform_data tda10071_pdata = hauppauge_tda10071_pdata;
+		struct a8293_platform_data a8293_pdata = {};
+
 		i2c_bus = &dev->i2c_bus[0];
-		fe0->dvb.frontend = dvb_attach(tda10071_attach,
-						&hauppauge_tda10071_config,
-						&i2c_bus->i2c_adap);
-		if (fe0->dvb.frontend != NULL) {
-			dvb_attach(a8293_attach, fe0->dvb.frontend,
-				   &i2c_bus->i2c_adap,
-				   &hauppauge_a8293_config);
+
+		/* attach demod + tuner combo */
+		memset(&info, 0, sizeof(info));
+		strlcpy(info.type, "tda10071_cx24118", I2C_NAME_SIZE);
+		info.addr = 0x05;
+		info.platform_data = &tda10071_pdata;
+		request_module("tda10071");
+		client_demod = i2c_new_device(&i2c_bus->i2c_adap, &info);
+		if (!client_demod || !client_demod->dev.driver)
+			goto frontend_detach;
+		if (!try_module_get(client_demod->dev.driver->owner)) {
+			i2c_unregister_device(client_demod);
+			goto frontend_detach;
 		}
+		fe0->dvb.frontend = tda10071_pdata.get_dvb_frontend(client_demod);
+		port->i2c_client_demod = client_demod;
+
+		/* attach SEC */
+		a8293_pdata.dvb_frontend = fe0->dvb.frontend;
+		memset(&info, 0, sizeof(info));
+		strlcpy(info.type, "a8293", I2C_NAME_SIZE);
+		info.addr = 0x0b;
+		info.platform_data = &a8293_pdata;
+		request_module("a8293");
+		client_sec = i2c_new_device(&i2c_bus->i2c_adap, &info);
+		if (!client_sec || !client_sec->dev.driver)
+			goto frontend_detach;
+		if (!try_module_get(client_sec->dev.driver->owner)) {
+			i2c_unregister_device(client_sec);
+			goto frontend_detach;
+		}
+		port->i2c_client_sec = client_sec;
 		break;
+	}
 	case CX23885_BOARD_DVBSKY_T9580:
 	case CX23885_BOARD_DVBSKY_S950:
 		i2c_bus = &dev->i2c_bus[0];
@@ -1857,6 +1908,7 @@ static int dvb_register(struct cx23885_tsport *port)
 			/* attach tuner */
 			memset(&ts2020_config, 0, sizeof(ts2020_config));
 			ts2020_config.fe = fe0->dvb.frontend;
+			ts2020_config.get_agc_pwm = m88ds3103_get_agc_pwm;
 			memset(&info, 0, sizeof(struct i2c_board_info));
 			strlcpy(info.type, "ts2020", I2C_NAME_SIZE);
 			info.addr = 0x60;
@@ -1912,6 +1964,7 @@ static int dvb_register(struct cx23885_tsport *port)
 			/* attach tuner */
 			memset(&si2157_config, 0, sizeof(si2157_config));
 			si2157_config.fe = fe0->dvb.frontend;
+			si2157_config.if_port = 1;
 			memset(&info, 0, sizeof(struct i2c_board_info));
 			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 			info.addr = 0x60;
@@ -1957,6 +2010,7 @@ static int dvb_register(struct cx23885_tsport *port)
 		/* attach tuner */
 		memset(&si2157_config, 0, sizeof(si2157_config));
 		si2157_config.fe = fe0->dvb.frontend;
+		si2157_config.if_port = 1;
 		memset(&info, 0, sizeof(struct i2c_board_info));
 		strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 		info.addr = 0x60;
@@ -1986,6 +2040,7 @@ static int dvb_register(struct cx23885_tsport *port)
 		/* attach tuner */
 		memset(&ts2020_config, 0, sizeof(ts2020_config));
 		ts2020_config.fe = fe0->dvb.frontend;
+		ts2020_config.get_agc_pwm = m88ds3103_get_agc_pwm;
 		memset(&info, 0, sizeof(struct i2c_board_info));
 		strlcpy(info.type, "ts2020", I2C_NAME_SIZE);
 		info.addr = 0x60;
@@ -2031,6 +2086,7 @@ static int dvb_register(struct cx23885_tsport *port)
 		/* attach tuner */
 		memset(&ts2020_config, 0, sizeof(ts2020_config));
 		ts2020_config.fe = fe0->dvb.frontend;
+		ts2020_config.get_agc_pwm = m88ds3103_get_agc_pwm;
 		memset(&info, 0, sizeof(struct i2c_board_info));
 		strlcpy(info.type, "ts2020", I2C_NAME_SIZE);
 		info.addr = 0x60;
@@ -2093,6 +2149,7 @@ static int dvb_register(struct cx23885_tsport *port)
 		/* attach tuner */
 		memset(&si2157_config, 0, sizeof(si2157_config));
 		si2157_config.fe = fe0->dvb.frontend;
+		si2157_config.if_port = 1;
 		memset(&info, 0, sizeof(struct i2c_board_info));
 		strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 		info.addr = 0x60;
@@ -2111,6 +2168,7 @@ static int dvb_register(struct cx23885_tsport *port)
 	case CX23885_BOARD_HAUPPAUGE_HVR5525:
 		switch (port->nr) {
 		struct m88rs6000t_config m88rs6000t_config;
+		struct a8293_platform_data a8293_pdata = {};
 
 		/* port b - satellite */
 		case 1:
@@ -2122,10 +2180,20 @@ static int dvb_register(struct cx23885_tsport *port)
 				break;
 
 			/* attach SEC */
-			if (!dvb_attach(a8293_attach, fe0->dvb.frontend,
-					&dev->i2c_bus[0].i2c_adap,
-					&hauppauge_a8293_config))
+			a8293_pdata.dvb_frontend = fe0->dvb.frontend;
+			memset(&info, 0, sizeof(info));
+			strlcpy(info.type, "a8293", I2C_NAME_SIZE);
+			info.addr = 0x0b;
+			info.platform_data = &a8293_pdata;
+			request_module("a8293");
+			client_sec = i2c_new_device(&dev->i2c_bus[0].i2c_adap, &info);
+			if (!client_sec || !client_sec->dev.driver)
+				goto frontend_detach;
+			if (!try_module_get(client_sec->dev.driver->owner)) {
+				i2c_unregister_device(client_sec);
 				goto frontend_detach;
+			}
+			port->i2c_client_sec = client_sec;
 
 			/* attach tuner */
 			memset(&m88rs6000t_config, 0, sizeof(m88rs6000t_config));
@@ -2172,6 +2240,7 @@ static int dvb_register(struct cx23885_tsport *port)
 			/* attach tuner */
 			memset(&si2157_config, 0, sizeof(si2157_config));
 			si2157_config.fe = fe0->dvb.frontend;
+			si2157_config.if_port = 1;
 			memset(&info, 0, sizeof(struct i2c_board_info));
 			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 			info.addr = 0x60;
@@ -2238,6 +2307,14 @@ static int dvb_register(struct cx23885_tsport *port)
 	return 0;
 
 frontend_detach:
+	/* remove I2C client for SEC */
+	client_sec = port->i2c_client_sec;
+	if (client_sec) {
+		module_put(client_sec->dev.driver->owner);
+		i2c_unregister_device(client_sec);
+		port->i2c_client_sec = NULL;
+	}
+
 	/* remove I2C client for tuner */
 	client_tuner = port->i2c_client_tuner;
 	if (client_tuner) {
@@ -2339,6 +2416,13 @@ int cx23885_dvb_unregister(struct cx23885_tsport *port)
 		i2c_unregister_device(client);
 	}
 
+	/* remove I2C client for SEC */
+	client = port->i2c_client_sec;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+
 	/* remove I2C client for tuner */
 	client = port->i2c_client_tuner;
 	if (client) {
diff --git a/drivers/media/pci/cx23885/cx23885-f300.c b/drivers/media/pci/cx23885/cx23885-f300.c
index 6f817d8732da..a6c45eb0a105 100644
--- a/drivers/media/pci/cx23885/cx23885-f300.c
+++ b/drivers/media/pci/cx23885/cx23885-f300.c
@@ -144,7 +144,7 @@ static u8 f300_xfer(struct dvb_frontend *fe, u8 *buf)
 	return ret;
 }
 
-int f300_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+int f300_set_voltage(struct dvb_frontend *fe, enum fe_sec_voltage voltage)
 {
 	u8 buf[16];
 
diff --git a/drivers/media/pci/cx23885/cx23885-f300.h b/drivers/media/pci/cx23885/cx23885-f300.h
index e73344c94963..be14d7de7cd8 100644
--- a/drivers/media/pci/cx23885/cx23885-f300.h
+++ b/drivers/media/pci/cx23885/cx23885-f300.h
@@ -1,2 +1,2 @@
 extern int f300_set_voltage(struct dvb_frontend *fe,
-				fe_sec_voltage_t voltage);
+			    enum fe_sec_voltage voltage);
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 2232b389c441..ec76470d12a4 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -581,7 +581,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	struct v4l2_format *f)
 {
 	struct cx23885_dev *dev = video_drvdata(file);
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	int err;
 
 	dprintk(2, "%s()\n", __func__);
@@ -600,10 +602,10 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	dev->field	= f->fmt.pix.field;
 	dprintk(2, "%s() width=%d height=%d field=%d\n", __func__,
 		dev->width, dev->height, dev->field);
-	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, MEDIA_BUS_FMT_FIXED);
-	call_all(dev, video, s_mbus_fmt, &mbus_fmt);
-	v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
-	/* s_mbus_fmt overwrites f->fmt.pix.field, restore it */
+	v4l2_fill_mbus_format(&format.format, &f->fmt.pix, MEDIA_BUS_FMT_FIXED);
+	call_all(dev, pad, set_fmt, NULL, &format);
+	v4l2_fill_pix_format(&f->fmt.pix, &format.format);
+	/* set_fmt overwrites f->fmt.pix.field, restore it */
 	f->fmt.pix.field = dev->field;
 	return 0;
 }
diff --git a/drivers/media/pci/cx23885/cx23885.h b/drivers/media/pci/cx23885/cx23885.h
index aeda8d3990ae..027ead438194 100644
--- a/drivers/media/pci/cx23885/cx23885.h
+++ b/drivers/media/pci/cx23885/cx23885.h
@@ -304,11 +304,12 @@ struct cx23885_tsport {
 
 	struct i2c_client *i2c_client_demod;
 	struct i2c_client *i2c_client_tuner;
+	struct i2c_client *i2c_client_sec;
 	struct i2c_client *i2c_client_ci;
 
 	int (*set_frontend)(struct dvb_frontend *fe);
 	int (*fe_set_voltage)(struct dvb_frontend *fe,
-				fe_sec_voltage_t voltage);
+			      enum fe_sec_voltage voltage);
 };
 
 struct cx23885_kernel_ir {
diff --git a/drivers/media/pci/cx25821/cx25821-medusa-reg.h b/drivers/media/pci/cx25821/cx25821-medusa-reg.h
index c98ac946b277..2e10643a86b7 100644
--- a/drivers/media/pci/cx25821/cx25821-medusa-reg.h
+++ b/drivers/media/pci/cx25821/cx25821-medusa-reg.h
@@ -84,9 +84,9 @@
 #define	ABIST_BIN4_VGA3				0x01D4
 #define	ABIST_BIN5_VGA4				0x01D8
 #define	ABIST_BIN6_VGA5				0x01DC
-#define	ABIST_BIN7_VGA6				0x0x1E0
-#define	ABIST_CLAMP_A				0x0x1E4
-#define	ABIST_CLAMP_B				0x0x1E8
+#define	ABIST_BIN7_VGA6				0x01E0
+#define	ABIST_CLAMP_A				0x01E4
+#define	ABIST_CLAMP_B				0x01E8
 #define	ABIST_CLAMP_C				0x01EC
 #define	ABIST_CLAMP_D				0x01F0
 #define	ABIST_CLAMP_E				0x01F4
diff --git a/drivers/media/pci/cx88/cx88-core.c b/drivers/media/pci/cx88/cx88-core.c
index 3501be9f19d8..aab7cf4c9825 100644
--- a/drivers/media/pci/cx88/cx88-core.c
+++ b/drivers/media/pci/cx88/cx88-core.c
@@ -519,6 +519,8 @@ void cx88_wakeup(struct cx88_core *core,
 	buf = list_entry(q->active.next,
 			 struct cx88_buffer, list);
 	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
+	buf->vb.v4l2_buf.field = core->field;
+	buf->vb.v4l2_buf.sequence = q->count++;
 	list_del(&buf->list);
 	vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
 }
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index 1b2ed238cdb6..9dfa5ee32a8f 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -449,7 +449,7 @@ static int cx24123_set_ts_param(struct dvb_frontend* fe,
 }
 
 static int kworld_dvbs_100_set_voltage(struct dvb_frontend* fe,
-				       fe_sec_voltage_t voltage)
+				       enum fe_sec_voltage voltage)
 {
 	struct cx8802_dev *dev= fe->dvb->priv;
 	struct cx88_core *core = dev->core;
@@ -465,7 +465,7 @@ static int kworld_dvbs_100_set_voltage(struct dvb_frontend* fe,
 }
 
 static int geniatech_dvbs_set_voltage(struct dvb_frontend *fe,
-				      fe_sec_voltage_t voltage)
+				      enum fe_sec_voltage voltage)
 {
 	struct cx8802_dev *dev= fe->dvb->priv;
 	struct cx88_core *core = dev->core;
@@ -481,7 +481,7 @@ static int geniatech_dvbs_set_voltage(struct dvb_frontend *fe,
 }
 
 static int tevii_dvbs_set_voltage(struct dvb_frontend *fe,
-				      fe_sec_voltage_t voltage)
+				  enum fe_sec_voltage voltage)
 {
 	struct cx8802_dev *dev= fe->dvb->priv;
 	struct cx88_core *core = dev->core;
@@ -505,7 +505,7 @@ static int tevii_dvbs_set_voltage(struct dvb_frontend *fe,
 }
 
 static int vp1027_set_voltage(struct dvb_frontend *fe,
-				    fe_sec_voltage_t voltage)
+			      enum fe_sec_voltage voltage)
 {
 	struct cx8802_dev *dev = fe->dvb->priv;
 	struct cx88_core *core = dev->core;
@@ -897,7 +897,7 @@ static int samsung_smt_7020_tuner_set_params(struct dvb_frontend *fe)
 }
 
 static int samsung_smt_7020_set_tone(struct dvb_frontend *fe,
-	fe_sec_tone_mode_t tone)
+	enum fe_sec_tone_mode tone)
 {
 	struct cx8802_dev *dev = fe->dvb->priv;
 	struct cx88_core *core = dev->core;
@@ -919,7 +919,7 @@ static int samsung_smt_7020_set_tone(struct dvb_frontend *fe,
 }
 
 static int samsung_smt_7020_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t voltage)
+					enum fe_sec_voltage voltage)
 {
 	struct cx8802_dev *dev = fe->dvb->priv;
 	struct cx88_core *core = dev->core;
diff --git a/drivers/media/pci/cx88/cx88-mpeg.c b/drivers/media/pci/cx88/cx88-mpeg.c
index 98344540c51f..34f505744477 100644
--- a/drivers/media/pci/cx88/cx88-mpeg.c
+++ b/drivers/media/pci/cx88/cx88-mpeg.c
@@ -173,7 +173,7 @@ int cx8802_start_dma(struct cx8802_dev    *dev,
 
 	/* reset counter */
 	cx_write(MO_TS_GPCNTRL, GP_COUNT_CONTROL_RESET);
-	q->count = 1;
+	q->count = 0;
 
 	/* enable irqs */
 	dprintk( 1, "setting the interrupt mask\n" );
@@ -216,8 +216,6 @@ static int cx8802_restart_queue(struct cx8802_dev    *dev,
 	dprintk(2,"restart_queue [%p/%d]: restart dma\n",
 		buf, buf->vb.v4l2_buf.index);
 	cx8802_start_dma(dev, q, buf);
-	list_for_each_entry(buf, &q->active, list)
-		buf->count = q->count++;
 	return 0;
 }
 
@@ -260,7 +258,6 @@ void cx8802_buf_queue(struct cx8802_dev *dev, struct cx88_buffer *buf)
 	if (list_empty(&cx88q->active)) {
 		dprintk( 1, "queue is empty - first active\n" );
 		list_add_tail(&buf->list, &cx88q->active);
-		buf->count    = cx88q->count++;
 		dprintk(1,"[%p/%d] %s - first active\n",
 			buf, buf->vb.v4l2_buf.index, __func__);
 
@@ -269,7 +266,6 @@ void cx8802_buf_queue(struct cx8802_dev *dev, struct cx88_buffer *buf)
 		dprintk( 1, "queue is not empty - append to active\n" );
 		prev = list_entry(cx88q->active.prev, struct cx88_buffer, list);
 		list_add_tail(&buf->list, &cx88q->active);
-		buf->count    = cx88q->count++;
 		prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
 		dprintk( 1, "[%p/%d] %s - append to active\n",
 			buf, buf->vb.v4l2_buf.index, __func__);
diff --git a/drivers/media/pci/cx88/cx88-vbi.c b/drivers/media/pci/cx88/cx88-vbi.c
index 32eb7fdb875e..7510e80eb2ff 100644
--- a/drivers/media/pci/cx88/cx88-vbi.c
+++ b/drivers/media/pci/cx88/cx88-vbi.c
@@ -59,7 +59,7 @@ static int cx8800_start_vbi_dma(struct cx8800_dev    *dev,
 
 	/* reset counter */
 	cx_write(MO_VBI_GPCNTRL, GP_COUNT_CONTROL_RESET);
-	q->count = 1;
+	q->count = 0;
 
 	/* enable irqs */
 	cx_set(MO_PCI_INTMSK, core->pci_irqmask | PCI_INT_VIDINT);
@@ -102,8 +102,6 @@ int cx8800_restart_vbi_queue(struct cx8800_dev    *dev,
 	dprintk(2,"restart_queue [%p/%d]: restart dma\n",
 		buf, buf->vb.v4l2_buf.index);
 	cx8800_start_vbi_dma(dev, q, buf);
-	list_for_each_entry(buf, &q->active, list)
-		buf->count = q->count++;
 	return 0;
 }
 
@@ -175,7 +173,6 @@ static void buffer_queue(struct vb2_buffer *vb)
 	if (list_empty(&q->active)) {
 		list_add_tail(&buf->list, &q->active);
 		cx8800_start_vbi_dma(dev, q, buf);
-		buf->count    = q->count++;
 		dprintk(2,"[%p/%d] vbi_queue - first active\n",
 			buf, buf->vb.v4l2_buf.index);
 
@@ -183,7 +180,6 @@ static void buffer_queue(struct vb2_buffer *vb)
 		buf->risc.cpu[0] |= cpu_to_le32(RISC_IRQ1);
 		prev = list_entry(q->active.prev, struct cx88_buffer, list);
 		list_add_tail(&buf->list, &q->active);
-		buf->count    = q->count++;
 		prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
 		dprintk(2,"[%p/%d] buffer_queue - append to active\n",
 			buf, buf->vb.v4l2_buf.index);
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index c9decd80bf61..400e5caefd58 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -370,7 +370,7 @@ static int start_video_dma(struct cx8800_dev    *dev,
 
 	/* reset counter */
 	cx_write(MO_VIDY_GPCNTRL,GP_COUNT_CONTROL_RESET);
-	q->count = 1;
+	q->count = 0;
 
 	/* enable irqs */
 	cx_set(MO_PCI_INTMSK, core->pci_irqmask | PCI_INT_VIDINT);
@@ -410,7 +410,6 @@ static int stop_video_dma(struct cx8800_dev    *dev)
 	cx_clear(MO_VID_INTMSK, 0x0f0011);
 	return 0;
 }
-#endif
 
 static int restart_video_queue(struct cx8800_dev    *dev,
 			       struct cx88_dmaqueue *q)
@@ -423,11 +422,10 @@ static int restart_video_queue(struct cx8800_dev    *dev,
 		dprintk(2,"restart_queue [%p/%d]: restart dma\n",
 			buf, buf->vb.v4l2_buf.index);
 		start_video_dma(dev, q, buf);
-		list_for_each_entry(buf, &q->active, list)
-			buf->count = q->count++;
 	}
 	return 0;
 }
+#endif
 
 /* ------------------------------------------------------------------ */
 
@@ -523,7 +521,6 @@ static void buffer_queue(struct vb2_buffer *vb)
 
 	if (list_empty(&q->active)) {
 		list_add_tail(&buf->list, &q->active);
-		buf->count    = q->count++;
 		dprintk(2,"[%p/%d] buffer_queue - first active\n",
 			buf, buf->vb.v4l2_buf.index);
 
@@ -531,7 +528,6 @@ static void buffer_queue(struct vb2_buffer *vb)
 		buf->risc.cpu[0] |= cpu_to_le32(RISC_IRQ1);
 		prev = list_entry(q->active.prev, struct cx88_buffer, list);
 		list_add_tail(&buf->list, &q->active);
-		buf->count    = q->count++;
 		prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
 		dprintk(2, "[%p/%d] buffer_queue - append to active\n",
 			buf, buf->vb.v4l2_buf.index);
@@ -771,6 +767,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		(f->fmt.pix.width * fmt->depth) >> 3;
 	f->fmt.pix.sizeimage =
 		f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }
diff --git a/drivers/media/pci/cx88/cx88.h b/drivers/media/pci/cx88/cx88.h
index b9fe1ac24803..785fe2e0d702 100644
--- a/drivers/media/pci/cx88/cx88.h
+++ b/drivers/media/pci/cx88/cx88.h
@@ -327,7 +327,6 @@ struct cx88_buffer {
 	/* cx88 specific */
 	unsigned int           bpl;
 	struct cx88_riscmem    risc;
-	u32                    count;
 };
 
 struct cx88_dmaqueue {
@@ -376,9 +375,10 @@ struct cx88_core {
 
 	/* config info -- dvb */
 #if IS_ENABLED(CONFIG_VIDEO_CX88_DVB)
-	int 			   (*prev_set_voltage)(struct dvb_frontend *fe, fe_sec_voltage_t voltage);
+	int	(*prev_set_voltage)(struct dvb_frontend *fe,
+				    enum fe_sec_voltage voltage);
 #endif
-	void			   (*gate_ctrl)(struct cx88_core  *core, int open);
+	void	(*gate_ctrl)(struct cx88_core *core, int open);
 
 	/* state info */
 	struct task_struct         *kthread;
diff --git a/drivers/media/pci/ddbridge/ddbridge-core.c b/drivers/media/pci/ddbridge/ddbridge-core.c
index 9e3492e20766..0ac2dd35fe50 100644
--- a/drivers/media/pci/ddbridge/ddbridge-core.c
+++ b/drivers/media/pci/ddbridge/ddbridge-core.c
@@ -1630,7 +1630,8 @@ fail1:
 	printk(KERN_ERR "fail1\n");
 	if (dev->msi)
 		pci_disable_msi(dev->pdev);
-	free_irq(dev->pdev->irq, dev);
+	if (stat == 0)
+		free_irq(dev->pdev->irq, dev);
 fail:
 	printk(KERN_ERR "fail\n");
 	ddb_unmap(dev);
diff --git a/drivers/media/pci/dm1105/dm1105.c b/drivers/media/pci/dm1105/dm1105.c
index ed11716731e9..88915fb87e80 100644
--- a/drivers/media/pci/dm1105/dm1105.c
+++ b/drivers/media/pci/dm1105/dm1105.c
@@ -591,7 +591,8 @@ static inline struct dm1105_dev *frontend_to_dm1105_dev(struct dvb_frontend *fe)
 	return container_of(fe->dvb, struct dm1105_dev, dvb_adapter);
 }
 
-static int dm1105_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int dm1105_set_voltage(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage)
 {
 	struct dm1105_dev *dev = frontend_to_dm1105_dev(fe);
 
diff --git a/drivers/media/pci/dt3155/Kconfig b/drivers/media/pci/dt3155/Kconfig
new file mode 100644
index 000000000000..5145e0dfa2aa
--- /dev/null
+++ b/drivers/media/pci/dt3155/Kconfig
@@ -0,0 +1,13 @@
+config VIDEO_DT3155
+	tristate "DT3155 frame grabber"
+	depends on PCI && VIDEO_DEV && VIDEO_V4L2
+	depends on HAS_DMA
+	select VIDEOBUF2_DMA_CONTIG
+	default n
+	---help---
+	  Enables dt3155 device driver for the DataTranslation DT3155 frame grabber.
+	  Say Y here if you have this hardware.
+	  In doubt, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called dt3155.
diff --git a/drivers/media/pci/dt3155/Makefile b/drivers/media/pci/dt3155/Makefile
new file mode 100644
index 000000000000..89fa637ec54c
--- /dev/null
+++ b/drivers/media/pci/dt3155/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_VIDEO_DT3155)	+= dt3155.o
diff --git a/drivers/media/pci/dt3155/dt3155.c b/drivers/media/pci/dt3155/dt3155.c
new file mode 100644
index 000000000000..8df634518927
--- /dev/null
+++ b/drivers/media/pci/dt3155/dt3155.c
@@ -0,0 +1,631 @@
+/***************************************************************************
+ *   Copyright (C) 2006-2010 by Marin Mitov                                *
+ *   mitov@issp.bas.bg                                                     *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under 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/stringify.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-common.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "dt3155.h"
+
+#define DT3155_DEVICE_ID 0x1223
+
+/**
+ * read_i2c_reg - reads an internal i2c register
+ *
+ * @addr:	dt3155 mmio base address
+ * @index:	index (internal address) of register to read
+ * @data:	pointer to byte the read data will be placed in
+ *
+ * returns:	zero on success or error code
+ *
+ * This function starts reading the specified (by index) register
+ * and busy waits for the process to finish. The result is placed
+ * in a byte pointed by data.
+ */
+static int read_i2c_reg(void __iomem *addr, u8 index, u8 *data)
+{
+	u32 tmp = index;
+
+	iowrite32((tmp << 17) | IIC_READ, addr + IIC_CSR2);
+	mmiowb();
+	udelay(45); /* wait at least 43 usec for NEW_CYCLE to clear */
+	if (ioread32(addr + IIC_CSR2) & NEW_CYCLE)
+		return -EIO; /* error: NEW_CYCLE not cleared */
+	tmp = ioread32(addr + IIC_CSR1);
+	if (tmp & DIRECT_ABORT) {
+		/* reset DIRECT_ABORT bit */
+		iowrite32(DIRECT_ABORT, addr + IIC_CSR1);
+		return -EIO; /* error: DIRECT_ABORT set */
+	}
+	*data = tmp >> 24;
+	return 0;
+}
+
+/**
+ * write_i2c_reg - writes to an internal i2c register
+ *
+ * @addr:	dt3155 mmio base address
+ * @index:	index (internal address) of register to read
+ * @data:	data to be written
+ *
+ * returns:	zero on success or error code
+ *
+ * This function starts writing the specified (by index) register
+ * and busy waits for the process to finish.
+ */
+static int write_i2c_reg(void __iomem *addr, u8 index, u8 data)
+{
+	u32 tmp = index;
+
+	iowrite32((tmp << 17) | IIC_WRITE | data, addr + IIC_CSR2);
+	mmiowb();
+	udelay(65); /* wait at least 63 usec for NEW_CYCLE to clear */
+	if (ioread32(addr + IIC_CSR2) & NEW_CYCLE)
+		return -EIO; /* error: NEW_CYCLE not cleared */
+	if (ioread32(addr + IIC_CSR1) & DIRECT_ABORT) {
+		/* reset DIRECT_ABORT bit */
+		iowrite32(DIRECT_ABORT, addr + IIC_CSR1);
+		return -EIO; /* error: DIRECT_ABORT set */
+	}
+	return 0;
+}
+
+/**
+ * write_i2c_reg_nowait - writes to an internal i2c register
+ *
+ * @addr:	dt3155 mmio base address
+ * @index:	index (internal address) of register to read
+ * @data:	data to be written
+ *
+ * This function starts writing the specified (by index) register
+ * and then returns.
+ */
+static void write_i2c_reg_nowait(void __iomem *addr, u8 index, u8 data)
+{
+	u32 tmp = index;
+
+	iowrite32((tmp << 17) | IIC_WRITE | data, addr + IIC_CSR2);
+	mmiowb();
+}
+
+/**
+ * wait_i2c_reg - waits the read/write to finish
+ *
+ * @addr:	dt3155 mmio base address
+ *
+ * returns:	zero on success or error code
+ *
+ * This function waits reading/writing to finish.
+ */
+static int wait_i2c_reg(void __iomem *addr)
+{
+	if (ioread32(addr + IIC_CSR2) & NEW_CYCLE)
+		udelay(65); /* wait at least 63 usec for NEW_CYCLE to clear */
+	if (ioread32(addr + IIC_CSR2) & NEW_CYCLE)
+		return -EIO; /* error: NEW_CYCLE not cleared */
+	if (ioread32(addr + IIC_CSR1) & DIRECT_ABORT) {
+		/* reset DIRECT_ABORT bit */
+		iowrite32(DIRECT_ABORT, addr + IIC_CSR1);
+		return -EIO; /* error: DIRECT_ABORT set */
+	}
+	return 0;
+}
+
+static int
+dt3155_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+		unsigned int *nbuffers, unsigned int *num_planes,
+		unsigned int sizes[], void *alloc_ctxs[])
+
+{
+	struct dt3155_priv *pd = vb2_get_drv_priv(vq);
+	unsigned size = pd->width * pd->height;
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+	if (fmt && fmt->fmt.pix.sizeimage < size)
+		return -EINVAL;
+	*num_planes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : size;
+	alloc_ctxs[0] = pd->alloc_ctx;
+	return 0;
+}
+
+static int dt3155_buf_prepare(struct vb2_buffer *vb)
+{
+	struct dt3155_priv *pd = vb2_get_drv_priv(vb->vb2_queue);
+
+	vb2_set_plane_payload(vb, 0, pd->width * pd->height);
+	return 0;
+}
+
+static int dt3155_start_streaming(struct vb2_queue *q, unsigned count)
+{
+	struct dt3155_priv *pd = vb2_get_drv_priv(q);
+	struct vb2_buffer *vb = pd->curr_buf;
+	dma_addr_t dma_addr;
+
+	pd->sequence = 0;
+	dma_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+	iowrite32(dma_addr, pd->regs + EVEN_DMA_START);
+	iowrite32(dma_addr + pd->width, pd->regs + ODD_DMA_START);
+	iowrite32(pd->width, pd->regs + EVEN_DMA_STRIDE);
+	iowrite32(pd->width, pd->regs + ODD_DMA_STRIDE);
+	/* enable interrupts, clear all irq flags */
+	iowrite32(FLD_START_EN | FLD_END_ODD_EN | FLD_START |
+			FLD_END_EVEN | FLD_END_ODD, pd->regs + INT_CSR);
+	iowrite32(FIFO_EN | SRST | FLD_CRPT_ODD | FLD_CRPT_EVEN |
+		  FLD_DN_ODD | FLD_DN_EVEN | CAP_CONT_EVEN | CAP_CONT_ODD,
+							pd->regs + CSR1);
+	wait_i2c_reg(pd->regs);
+	write_i2c_reg(pd->regs, CONFIG, pd->config);
+	write_i2c_reg(pd->regs, EVEN_CSR, CSR_ERROR | CSR_DONE);
+	write_i2c_reg(pd->regs, ODD_CSR, CSR_ERROR | CSR_DONE);
+
+	/*  start the board  */
+	write_i2c_reg(pd->regs, CSR2, pd->csr2 | BUSY_EVEN | BUSY_ODD);
+	return 0;
+}
+
+static void dt3155_stop_streaming(struct vb2_queue *q)
+{
+	struct dt3155_priv *pd = vb2_get_drv_priv(q);
+	struct vb2_buffer *vb;
+
+	spin_lock_irq(&pd->lock);
+	/* stop the board */
+	write_i2c_reg_nowait(pd->regs, CSR2, pd->csr2);
+	iowrite32(FIFO_EN | SRST | FLD_CRPT_ODD | FLD_CRPT_EVEN |
+		  FLD_DN_ODD | FLD_DN_EVEN, pd->regs + CSR1);
+	/* disable interrupts, clear all irq flags */
+	iowrite32(FLD_START | FLD_END_EVEN | FLD_END_ODD, pd->regs + INT_CSR);
+	spin_unlock_irq(&pd->lock);
+
+	/*
+	 * It is not clear whether the DMA stops at once or whether it
+	 * will finish the current frame or field first. To be on the
+	 * safe side we wait a bit.
+	 */
+	msleep(45);
+
+	spin_lock_irq(&pd->lock);
+	if (pd->curr_buf) {
+		vb2_buffer_done(pd->curr_buf, VB2_BUF_STATE_ERROR);
+		pd->curr_buf = NULL;
+	}
+
+	while (!list_empty(&pd->dmaq)) {
+		vb = list_first_entry(&pd->dmaq, typeof(*vb), done_entry);
+		list_del(&vb->done_entry);
+		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irq(&pd->lock);
+}
+
+static void dt3155_buf_queue(struct vb2_buffer *vb)
+{
+	struct dt3155_priv *pd = vb2_get_drv_priv(vb->vb2_queue);
+
+	/*  pd->vidq.streaming = 1 when dt3155_buf_queue() is invoked  */
+	spin_lock_irq(&pd->lock);
+	if (pd->curr_buf)
+		list_add_tail(&vb->done_entry, &pd->dmaq);
+	else
+		pd->curr_buf = vb;
+	spin_unlock_irq(&pd->lock);
+}
+
+static const struct vb2_ops q_ops = {
+	.queue_setup = dt3155_queue_setup,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.buf_prepare = dt3155_buf_prepare,
+	.start_streaming = dt3155_start_streaming,
+	.stop_streaming = dt3155_stop_streaming,
+	.buf_queue = dt3155_buf_queue,
+};
+
+static irqreturn_t dt3155_irq_handler_even(int irq, void *dev_id)
+{
+	struct dt3155_priv *ipd = dev_id;
+	struct vb2_buffer *ivb;
+	dma_addr_t dma_addr;
+	u32 tmp;
+
+	tmp = ioread32(ipd->regs + INT_CSR) & (FLD_START | FLD_END_ODD);
+	if (!tmp)
+		return IRQ_NONE;  /* not our irq */
+	if ((tmp & FLD_START) && !(tmp & FLD_END_ODD)) {
+		iowrite32(FLD_START_EN | FLD_END_ODD_EN | FLD_START,
+							ipd->regs + INT_CSR);
+		return IRQ_HANDLED; /* start of field irq */
+	}
+	tmp = ioread32(ipd->regs + CSR1) & (FLD_CRPT_EVEN | FLD_CRPT_ODD);
+	if (tmp) {
+		iowrite32(FIFO_EN | SRST | FLD_CRPT_ODD | FLD_CRPT_EVEN |
+						FLD_DN_ODD | FLD_DN_EVEN |
+						CAP_CONT_EVEN | CAP_CONT_ODD,
+							ipd->regs + CSR1);
+		mmiowb();
+	}
+
+	spin_lock(&ipd->lock);
+	if (ipd->curr_buf && !list_empty(&ipd->dmaq)) {
+		v4l2_get_timestamp(&ipd->curr_buf->v4l2_buf.timestamp);
+		ipd->curr_buf->v4l2_buf.sequence = ipd->sequence++;
+		ipd->curr_buf->v4l2_buf.field = V4L2_FIELD_NONE;
+		vb2_buffer_done(ipd->curr_buf, VB2_BUF_STATE_DONE);
+
+		ivb = list_first_entry(&ipd->dmaq, typeof(*ivb), done_entry);
+		list_del(&ivb->done_entry);
+		ipd->curr_buf = ivb;
+		dma_addr = vb2_dma_contig_plane_dma_addr(ivb, 0);
+		iowrite32(dma_addr, ipd->regs + EVEN_DMA_START);
+		iowrite32(dma_addr + ipd->width, ipd->regs + ODD_DMA_START);
+		iowrite32(ipd->width, ipd->regs + EVEN_DMA_STRIDE);
+		iowrite32(ipd->width, ipd->regs + ODD_DMA_STRIDE);
+		mmiowb();
+	}
+
+	/* enable interrupts, clear all irq flags */
+	iowrite32(FLD_START_EN | FLD_END_ODD_EN | FLD_START |
+			FLD_END_EVEN | FLD_END_ODD, ipd->regs + INT_CSR);
+	spin_unlock(&ipd->lock);
+	return IRQ_HANDLED;
+}
+
+static const struct v4l2_file_operations dt3155_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.unlocked_ioctl = video_ioctl2,
+	.read = vb2_fop_read,
+	.mmap = vb2_fop_mmap,
+	.poll = vb2_fop_poll
+};
+
+static int dt3155_querycap(struct file *filp, void *p,
+			   struct v4l2_capability *cap)
+{
+	struct dt3155_priv *pd = video_drvdata(filp);
+
+	strcpy(cap->driver, DT3155_NAME);
+	strcpy(cap->card, DT3155_NAME " frame grabber");
+	sprintf(cap->bus_info, "PCI:%s", pci_name(pd->pdev));
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int dt3155_enum_fmt_vid_cap(struct file *filp,
+				   void *p, struct v4l2_fmtdesc *f)
+{
+	if (f->index)
+		return -EINVAL;
+	f->pixelformat = V4L2_PIX_FMT_GREY;
+	strcpy(f->description, "8-bit Greyscale");
+	return 0;
+}
+
+static int dt3155_fmt_vid_cap(struct file *filp, void *p, struct v4l2_format *f)
+{
+	struct dt3155_priv *pd = video_drvdata(filp);
+
+	f->fmt.pix.width = pd->width;
+	f->fmt.pix.height = pd->height;
+	f->fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.bytesperline = f->fmt.pix.width;
+	f->fmt.pix.sizeimage = f->fmt.pix.width * f->fmt.pix.height;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	return 0;
+}
+
+static int dt3155_g_std(struct file *filp, void *p, v4l2_std_id *norm)
+{
+	struct dt3155_priv *pd = video_drvdata(filp);
+
+	*norm = pd->std;
+	return 0;
+}
+
+static int dt3155_s_std(struct file *filp, void *p, v4l2_std_id norm)
+{
+	struct dt3155_priv *pd = video_drvdata(filp);
+
+	if (pd->std == norm)
+		return 0;
+	if (vb2_is_busy(&pd->vidq))
+		return -EBUSY;
+	pd->std = norm;
+	if (pd->std & V4L2_STD_525_60) {
+		pd->csr2 = VT_60HZ;
+		pd->width = 640;
+		pd->height = 480;
+	} else {
+		pd->csr2 = VT_50HZ;
+		pd->width = 768;
+		pd->height = 576;
+	}
+	return 0;
+}
+
+static int dt3155_enum_input(struct file *filp, void *p,
+			     struct v4l2_input *input)
+{
+	if (input->index > 3)
+		return -EINVAL;
+	if (input->index)
+		snprintf(input->name, sizeof(input->name), "VID%d",
+			 input->index);
+	else
+		strlcpy(input->name, "J2/VID0", sizeof(input->name));
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	input->std = V4L2_STD_ALL;
+	input->status = 0;
+	return 0;
+}
+
+static int dt3155_g_input(struct file *filp, void *p, unsigned int *i)
+{
+	struct dt3155_priv *pd = video_drvdata(filp);
+
+	*i = pd->input;
+	return 0;
+}
+
+static int dt3155_s_input(struct file *filp, void *p, unsigned int i)
+{
+	struct dt3155_priv *pd = video_drvdata(filp);
+
+	if (i > 3)
+		return -EINVAL;
+	pd->input = i;
+	write_i2c_reg(pd->regs, AD_ADDR, AD_CMD_REG);
+	write_i2c_reg(pd->regs, AD_CMD, (i << 6) | (i << 4) | SYNC_LVL_3);
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops dt3155_ioctl_ops = {
+	.vidioc_querycap = dt3155_querycap,
+	.vidioc_enum_fmt_vid_cap = dt3155_enum_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap = dt3155_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap = dt3155_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap = dt3155_fmt_vid_cap,
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_g_std = dt3155_g_std,
+	.vidioc_s_std = dt3155_s_std,
+	.vidioc_enum_input = dt3155_enum_input,
+	.vidioc_g_input = dt3155_g_input,
+	.vidioc_s_input = dt3155_s_input,
+};
+
+static int dt3155_init_board(struct dt3155_priv *pd)
+{
+	struct pci_dev *pdev = pd->pdev;
+	int i;
+	u8 tmp = 0;
+
+	pci_set_master(pdev); /* dt3155 needs it */
+
+	/*  resetting the adapter  */
+	iowrite32(ADDR_ERR_ODD | ADDR_ERR_EVEN | FLD_CRPT_ODD | FLD_CRPT_EVEN |
+			FLD_DN_ODD | FLD_DN_EVEN, pd->regs + CSR1);
+	mmiowb();
+	msleep(20);
+
+	/*  initializing adapter registers  */
+	iowrite32(FIFO_EN | SRST, pd->regs + CSR1);
+	mmiowb();
+	iowrite32(0xEEEEEE01, pd->regs + EVEN_PIXEL_FMT);
+	iowrite32(0xEEEEEE01, pd->regs + ODD_PIXEL_FMT);
+	iowrite32(0x00000020, pd->regs + FIFO_TRIGER);
+	iowrite32(0x00000103, pd->regs + XFER_MODE);
+	iowrite32(0, pd->regs + RETRY_WAIT_CNT);
+	iowrite32(0, pd->regs + INT_CSR);
+	iowrite32(1, pd->regs + EVEN_FLD_MASK);
+	iowrite32(1, pd->regs + ODD_FLD_MASK);
+	iowrite32(0, pd->regs + MASK_LENGTH);
+	iowrite32(0x0005007C, pd->regs + FIFO_FLAG_CNT);
+	iowrite32(0x01010101, pd->regs + IIC_CLK_DUR);
+	mmiowb();
+
+	/* verifying that we have a DT3155 board (not just a SAA7116 chip) */
+	read_i2c_reg(pd->regs, DT_ID, &tmp);
+	if (tmp != DT3155_ID)
+		return -ENODEV;
+
+	/* initialize AD LUT */
+	write_i2c_reg(pd->regs, AD_ADDR, 0);
+	for (i = 0; i < 256; i++)
+		write_i2c_reg(pd->regs, AD_LUT, i);
+
+	/* initialize ADC references */
+	/* FIXME: pos_ref & neg_ref depend on VT_50HZ */
+	write_i2c_reg(pd->regs, AD_ADDR, AD_CMD_REG);
+	write_i2c_reg(pd->regs, AD_CMD, VIDEO_CNL_1 | SYNC_CNL_1 | SYNC_LVL_3);
+	write_i2c_reg(pd->regs, AD_ADDR, AD_POS_REF);
+	write_i2c_reg(pd->regs, AD_CMD, 34);
+	write_i2c_reg(pd->regs, AD_ADDR, AD_NEG_REF);
+	write_i2c_reg(pd->regs, AD_CMD, 0);
+
+	/* initialize PM LUT */
+	write_i2c_reg(pd->regs, CONFIG, pd->config | PM_LUT_PGM);
+	for (i = 0; i < 256; i++) {
+		write_i2c_reg(pd->regs, PM_LUT_ADDR, i);
+		write_i2c_reg(pd->regs, PM_LUT_DATA, i);
+	}
+	write_i2c_reg(pd->regs, CONFIG, pd->config | PM_LUT_PGM | PM_LUT_SEL);
+	for (i = 0; i < 256; i++) {
+		write_i2c_reg(pd->regs, PM_LUT_ADDR, i);
+		write_i2c_reg(pd->regs, PM_LUT_DATA, i);
+	}
+	write_i2c_reg(pd->regs, CONFIG, pd->config); /*  ACQ_MODE_EVEN  */
+
+	/* select channel 1 for input and set sync level */
+	write_i2c_reg(pd->regs, AD_ADDR, AD_CMD_REG);
+	write_i2c_reg(pd->regs, AD_CMD, VIDEO_CNL_1 | SYNC_CNL_1 | SYNC_LVL_3);
+
+	/* disable all irqs, clear all irq flags */
+	iowrite32(FLD_START | FLD_END_EVEN | FLD_END_ODD,
+			pd->regs + INT_CSR);
+
+	return 0;
+}
+
+static struct video_device dt3155_vdev = {
+	.name = DT3155_NAME,
+	.fops = &dt3155_fops,
+	.ioctl_ops = &dt3155_ioctl_ops,
+	.minor = -1,
+	.release = video_device_release_empty,
+	.tvnorms = V4L2_STD_ALL,
+};
+
+static int dt3155_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	int err;
+	struct dt3155_priv *pd;
+
+	err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+	if (err)
+		return -ENODEV;
+	pd = devm_kzalloc(&pdev->dev, sizeof(*pd), GFP_KERNEL);
+	if (!pd)
+		return -ENOMEM;
+
+	err = v4l2_device_register(&pdev->dev, &pd->v4l2_dev);
+	if (err)
+		return err;
+	pd->vdev = dt3155_vdev;
+	pd->vdev.v4l2_dev = &pd->v4l2_dev;
+	video_set_drvdata(&pd->vdev, pd);  /* for use in video_fops */
+	pd->pdev = pdev;
+	pd->std = V4L2_STD_625_50;
+	pd->csr2 = VT_50HZ;
+	pd->width = 768;
+	pd->height = 576;
+	INIT_LIST_HEAD(&pd->dmaq);
+	mutex_init(&pd->mux);
+	pd->vdev.lock = &pd->mux; /* for locking v4l2_file_operations */
+	pd->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	pd->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	pd->vidq.io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ;
+	pd->vidq.ops = &q_ops;
+	pd->vidq.mem_ops = &vb2_dma_contig_memops;
+	pd->vidq.drv_priv = pd;
+	pd->vidq.min_buffers_needed = 2;
+	pd->vidq.gfp_flags = GFP_DMA32;
+	pd->vidq.lock = &pd->mux; /* for locking v4l2_file_operations */
+	pd->vdev.queue = &pd->vidq;
+	err = vb2_queue_init(&pd->vidq);
+	if (err < 0)
+		goto err_v4l2_dev_unreg;
+	pd->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(pd->alloc_ctx)) {
+		dev_err(&pdev->dev, "Can't allocate buffer context");
+		err = PTR_ERR(pd->alloc_ctx);
+		goto err_v4l2_dev_unreg;
+	}
+	spin_lock_init(&pd->lock);
+	pd->config = ACQ_MODE_EVEN;
+	err = pci_enable_device(pdev);
+	if (err)
+		goto err_free_ctx;
+	err = pci_request_region(pdev, 0, pci_name(pdev));
+	if (err)
+		goto err_pci_disable;
+	pd->regs = pci_iomap(pdev, 0, pci_resource_len(pd->pdev, 0));
+	if (!pd->regs) {
+		err = -ENOMEM;
+		goto err_free_reg;
+	}
+	err = dt3155_init_board(pd);
+	if (err)
+		goto err_iounmap;
+	err = request_irq(pd->pdev->irq, dt3155_irq_handler_even,
+					IRQF_SHARED, DT3155_NAME, pd);
+	if (err)
+		goto err_iounmap;
+	err = video_register_device(&pd->vdev, VFL_TYPE_GRABBER, -1);
+	if (err)
+		goto err_free_irq;
+	dev_info(&pdev->dev, "/dev/video%i is ready\n", pd->vdev.minor);
+	return 0;  /*   success   */
+
+err_free_irq:
+	free_irq(pd->pdev->irq, pd);
+err_iounmap:
+	pci_iounmap(pdev, pd->regs);
+err_free_reg:
+	pci_release_region(pdev, 0);
+err_pci_disable:
+	pci_disable_device(pdev);
+err_free_ctx:
+	vb2_dma_contig_cleanup_ctx(pd->alloc_ctx);
+err_v4l2_dev_unreg:
+	v4l2_device_unregister(&pd->v4l2_dev);
+	return err;
+}
+
+static void dt3155_remove(struct pci_dev *pdev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
+	struct dt3155_priv *pd = container_of(v4l2_dev, struct dt3155_priv,
+					      v4l2_dev);
+
+	video_unregister_device(&pd->vdev);
+	free_irq(pd->pdev->irq, pd);
+	vb2_queue_release(&pd->vidq);
+	v4l2_device_unregister(&pd->v4l2_dev);
+	pci_iounmap(pdev, pd->regs);
+	pci_release_region(pdev, 0);
+	pci_disable_device(pdev);
+	vb2_dma_contig_cleanup_ctx(pd->alloc_ctx);
+}
+
+static const struct pci_device_id pci_ids[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, DT3155_DEVICE_ID) },
+	{ 0, /* zero marks the end */ },
+};
+MODULE_DEVICE_TABLE(pci, pci_ids);
+
+static struct pci_driver pci_driver = {
+	.name = DT3155_NAME,
+	.id_table = pci_ids,
+	.probe = dt3155_probe,
+	.remove = dt3155_remove,
+};
+
+module_pci_driver(pci_driver);
+
+MODULE_DESCRIPTION("video4linux pci-driver for dt3155 frame grabber");
+MODULE_AUTHOR("Marin Mitov <mitov@issp.bas.bg>");
+MODULE_VERSION(DT3155_VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/dt3155v4l/dt3155v4l.h b/drivers/media/pci/dt3155/dt3155.h
index 96f01a0c7581..4e1f4d598d57 100644
--- a/drivers/staging/media/dt3155v4l/dt3155v4l.h
+++ b/drivers/media/pci/dt3155/dt3155.h
@@ -12,24 +12,20 @@
  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  *   GNU General Public License for more details.                          *
  *                                                                         *
- *   You 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.             *
  ***************************************************************************/
 
 /*    DT3155 header file    */
 #ifndef _DT3155_H_
 #define _DT3155_H_
 
-#ifdef __KERNEL__
-
 #include <linux/pci.h>
 #include <linux/interrupt.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dev.h>
 
 #define DT3155_NAME "dt3155"
-#define DT3155_VER_MAJ 1
-#define DT3155_VER_MIN 1
+#define DT3155_VER_MAJ 2
+#define DT3155_VER_MIN 0
 #define DT3155_VER_EXT 0
 #define DT3155_VERSION  __stringify(DT3155_VER_MAJ)	"."		\
 			__stringify(DT3155_VER_MIN)	"."		\
@@ -78,7 +74,10 @@
 #define AD_NEG_REF   0x02
 
 /* CSR1 bit masks */
+#define RANGE_EN       0x00008000
 #define CRPT_DIS       0x00004000
+#define ADDR_ERR_ODD   0x00000800
+#define ADDR_ERR_EVEN  0x00000400
 #define FLD_CRPT_ODD   0x00000200
 #define FLD_CRPT_EVEN  0x00000100
 #define FIFO_EN        0x00000080
@@ -153,60 +152,45 @@
 /* DT3155 identificator */
 #define DT3155_ID   0x20
 
-#ifdef CONFIG_DT3155_CCIR
-#define DMA_STRIDE 768
-#else
-#define DMA_STRIDE 640
-#endif
-
-/**
- * struct dt3155_stats - statistics structure
- *
- * @free_bufs_empty:	no free image buffers
- * @corrupted_fields:	corrupted fields
- * @dma_map_failed:	dma mapping failed
- * @start_before_end:	new started before old ended
- */
-struct dt3155_stats {
-	int free_bufs_empty;
-	int corrupted_fields;
-	int dma_map_failed;
-	int start_before_end;
-};
-
 /*    per board private data structure   */
 /**
  * struct dt3155_priv - private data structure
  *
+ * @v4l2_dev:		v4l2_device structure
  * @vdev:		video_device structure
  * @pdev:		pointer to pci_dev structure
- * @q			pointer to vb2_queue structure
+ * @vidq:		vb2_queue structure
+ * @alloc_ctx:		dma_contig allocation context
  * @curr_buf:		pointer to curren buffer
  * @mux:		mutex to protect the instance
- * @dmaq		queue for dma buffers
- * @lock		spinlock for dma queue
- * @field_count		fields counter
+ * @dmaq:		queue for dma buffers
+ * @lock:		spinlock for dma queue
+ * @std:		input standard
+ * @width:		frame width
+ * @height:		frame height
+ * @input:		current input
+ * @sequence:		frame counter
  * @stats:		statistics structure
- * @users		open count
  * @regs:		local copy of mmio base register
  * @csr2:		local copy of csr2 register
  * @config:		local copy of config register
  */
 struct dt3155_priv {
+	struct v4l2_device v4l2_dev;
 	struct video_device vdev;
 	struct pci_dev *pdev;
-	struct vb2_queue *q;
+	struct vb2_queue vidq;
+	struct vb2_alloc_ctx *alloc_ctx;
 	struct vb2_buffer *curr_buf;
 	struct mutex mux;
 	struct list_head dmaq;
 	spinlock_t lock;
-	unsigned int field_count;
-	struct dt3155_stats stats;
+	v4l2_std_id std;
+	unsigned width, height;
+	unsigned input;
+	unsigned int sequence;
 	void __iomem *regs;
-	int users;
 	u8 csr2, config;
 };
 
-#endif /*  __KERNEL__  */
-
 #endif /*  _DT3155_H_  */
diff --git a/drivers/media/pci/ivtv/Kconfig b/drivers/media/pci/ivtv/Kconfig
index dd6ee57e3a4c..6e5867c57305 100644
--- a/drivers/media/pci/ivtv/Kconfig
+++ b/drivers/media/pci/ivtv/Kconfig
@@ -57,5 +57,8 @@ config VIDEO_FB_IVTV
 	  This is used in the Hauppauge PVR-350 card. There is a driver
 	  homepage at <http://www.ivtvdriver.org>.
 
+	  In order to use this module, you will need to boot with PAT disabled
+	  on x86 systems, using the nopat kernel parameter.
+
 	  To compile this driver as a module, choose M here: the
 	  module will be called ivtvfb.
diff --git a/drivers/media/pci/ivtv/ivtv-controls.c b/drivers/media/pci/ivtv/ivtv-controls.c
index ccf548c255f1..8a55ccb8f0c9 100644
--- a/drivers/media/pci/ivtv/ivtv-controls.c
+++ b/drivers/media/pci/ivtv/ivtv-controls.c
@@ -64,13 +64,15 @@ static int ivtv_s_video_encoding(struct cx2341x_handler *cxhdl, u32 val)
 {
 	struct ivtv *itv = container_of(cxhdl, struct ivtv, cxhdl);
 	int is_mpeg1 = val == V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 
 	/* fix videodecoder resolution */
-	fmt.width = cxhdl->width / (is_mpeg1 ? 2 : 1);
-	fmt.height = cxhdl->height;
-	fmt.code = MEDIA_BUS_FMT_FIXED;
-	v4l2_subdev_call(itv->sd_video, video, s_mbus_fmt, &fmt);
+	format.format.width = cxhdl->width / (is_mpeg1 ? 2 : 1);
+	format.format.height = cxhdl->height;
+	format.format.code = MEDIA_BUS_FMT_FIXED;
+	v4l2_subdev_call(itv->sd_video, pad, set_fmt, NULL, &format);
 	return 0;
 }
 
diff --git a/drivers/media/pci/ivtv/ivtv-driver.c b/drivers/media/pci/ivtv/ivtv-driver.c
index c2e60b4f292d..8616fa8193bc 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.c
+++ b/drivers/media/pci/ivtv/ivtv-driver.c
@@ -805,11 +805,11 @@ static void ivtv_init_struct2(struct ivtv *itv)
 {
 	int i;
 
-	for (i = 0; i < IVTV_CARD_MAX_VIDEO_INPUTS; i++)
+	for (i = 0; i < IVTV_CARD_MAX_VIDEO_INPUTS - 1; i++)
 		if (itv->card->video_inputs[i].video_type == 0)
 			break;
 	itv->nof_inputs = i;
-	for (i = 0; i < IVTV_CARD_MAX_AUDIO_INPUTS; i++)
+	for (i = 0; i < IVTV_CARD_MAX_AUDIO_INPUTS - 1; i++)
 		if (itv->card->audio_inputs[i].audio_type == 0)
 			break;
 	itv->nof_audio_inputs = i;
diff --git a/drivers/media/pci/ivtv/ivtv-driver.h b/drivers/media/pci/ivtv/ivtv-driver.h
index e8b6c7ad2ba9..ee0ef6e48c7d 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.h
+++ b/drivers/media/pci/ivtv/ivtv-driver.h
@@ -830,7 +830,8 @@ static inline int ivtv_raw_vbi(const struct ivtv *itv)
 	do {								\
 		struct v4l2_subdev *__sd;				\
 		__v4l2_device_call_subdevs_p(&(itv)->v4l2_dev, __sd,	\
-			!(hw) || (__sd->grp_id & (hw)), o, f , ##args);	\
+			 !(hw) ? true : (__sd->grp_id & (hw)),		\
+			 o, f, ##args);					\
 	} while (0)
 
 #define ivtv_call_all(itv, o, f, args...) ivtv_call_hw(itv, 0, o, f , ##args)
diff --git a/drivers/media/pci/ivtv/ivtv-ioctl.c b/drivers/media/pci/ivtv/ivtv-ioctl.c
index 6fe6c4a0e858..9a21c17fc376 100644
--- a/drivers/media/pci/ivtv/ivtv-ioctl.c
+++ b/drivers/media/pci/ivtv/ivtv-ioctl.c
@@ -581,7 +581,9 @@ static int ivtv_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f
 {
 	struct ivtv_open_id *id = fh2id(fh);
 	struct ivtv *itv = id->itv;
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	int ret = ivtv_try_fmt_vid_cap(file, fh, fmt);
 	int w = fmt->fmt.pix.width;
 	int h = fmt->fmt.pix.height;
@@ -599,10 +601,10 @@ static int ivtv_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f
 	itv->cxhdl.height = h;
 	if (v4l2_ctrl_g_ctrl(itv->cxhdl.video_encoding) == V4L2_MPEG_VIDEO_ENCODING_MPEG_1)
 		fmt->fmt.pix.width /= 2;
-	mbus_fmt.width = fmt->fmt.pix.width;
-	mbus_fmt.height = h;
-	mbus_fmt.code = MEDIA_BUS_FMT_FIXED;
-	v4l2_subdev_call(itv->sd_video, video, s_mbus_fmt, &mbus_fmt);
+	format.format.width = fmt->fmt.pix.width;
+	format.format.height = h;
+	format.format.code = MEDIA_BUS_FMT_FIXED;
+	v4l2_subdev_call(itv->sd_video, pad, set_fmt, NULL, &format);
 	return ivtv_g_fmt_vid_cap(file, fh, fmt);
 }
 
@@ -1529,7 +1531,8 @@ static int ivtv_log_status(struct file *file, void *fh)
 	ivtv_get_audio_input(itv, itv->audio_input, &audin);
 	IVTV_INFO("Video Input:  %s\n", vidin.name);
 	IVTV_INFO("Audio Input:  %s%s\n", audin.name,
-		(itv->dualwatch_stereo_mode & ~0x300) == 0x200 ? " (Bilingual)" : "");
+		itv->dualwatch_stereo_mode == V4L2_MPEG_AUDIO_MODE_DUAL ?
+			" (Bilingual)" : "");
 	if (has_output) {
 		struct v4l2_output vidout;
 		struct v4l2_audioout audout;
diff --git a/drivers/media/pci/ivtv/ivtvfb.c b/drivers/media/pci/ivtv/ivtvfb.c
index 9ff1230192e8..4cb365d4ffdc 100644
--- a/drivers/media/pci/ivtv/ivtvfb.c
+++ b/drivers/media/pci/ivtv/ivtvfb.c
@@ -44,8 +44,8 @@
 #include <linux/ivtvfb.h>
 #include <linux/slab.h>
 
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
+#ifdef CONFIG_X86_64
+#include <asm/pat.h>
 #endif
 
 #include "ivtv-driver.h"
@@ -155,12 +155,11 @@ struct osd_info {
 	/* Buffer size */
 	u32 video_buffer_size;
 
-#ifdef CONFIG_MTRR
 	/* video_base rounded down as required by hardware MTRRs */
 	unsigned long fb_start_aligned_physaddr;
 	/* video_base rounded up as required by hardware MTRRs */
 	unsigned long fb_end_aligned_physaddr;
-#endif
+	int wc_cookie;
 
 	/* Store the buffer offset */
 	int set_osd_coords_x;
@@ -1099,6 +1098,8 @@ static int ivtvfb_init_vidmode(struct ivtv *itv)
 static int ivtvfb_init_io(struct ivtv *itv)
 {
 	struct osd_info *oi = itv->osd_info;
+	/* Find the largest power of two that maps the whole buffer */
+	int size_shift = 31;
 
 	mutex_lock(&itv->serialize_lock);
 	if (ivtv_init_on_first_open(itv)) {
@@ -1132,29 +1133,16 @@ static int ivtvfb_init_io(struct ivtv *itv)
 			oi->video_pbase, oi->video_vbase,
 			oi->video_buffer_size / 1024);
 
-#ifdef CONFIG_MTRR
-	{
-		/* Find the largest power of two that maps the whole buffer */
-		int size_shift = 31;
-
-		while (!(oi->video_buffer_size & (1 << size_shift))) {
-			size_shift--;
-		}
-		size_shift++;
-		oi->fb_start_aligned_physaddr = oi->video_pbase & ~((1 << size_shift) - 1);
-		oi->fb_end_aligned_physaddr = oi->video_pbase + oi->video_buffer_size;
-		oi->fb_end_aligned_physaddr += (1 << size_shift) - 1;
-		oi->fb_end_aligned_physaddr &= ~((1 << size_shift) - 1);
-		if (mtrr_add(oi->fb_start_aligned_physaddr,
-			oi->fb_end_aligned_physaddr - oi->fb_start_aligned_physaddr,
-			     MTRR_TYPE_WRCOMB, 1) < 0) {
-			IVTVFB_INFO("disabled mttr\n");
-			oi->fb_start_aligned_physaddr = 0;
-			oi->fb_end_aligned_physaddr = 0;
-		}
-	}
-#endif
-
+	while (!(oi->video_buffer_size & (1 << size_shift)))
+		size_shift--;
+	size_shift++;
+	oi->fb_start_aligned_physaddr = oi->video_pbase & ~((1 << size_shift) - 1);
+	oi->fb_end_aligned_physaddr = oi->video_pbase + oi->video_buffer_size;
+	oi->fb_end_aligned_physaddr += (1 << size_shift) - 1;
+	oi->fb_end_aligned_physaddr &= ~((1 << size_shift) - 1);
+	oi->wc_cookie = arch_phys_wc_add(oi->fb_start_aligned_physaddr,
+					 oi->fb_end_aligned_physaddr -
+					 oi->fb_start_aligned_physaddr);
 	/* Blank the entire osd. */
 	memset_io(oi->video_vbase, 0, oi->video_buffer_size);
 
@@ -1172,14 +1160,7 @@ static void ivtvfb_release_buffers (struct ivtv *itv)
 
 	/* Release pseudo palette */
 	kfree(oi->ivtvfb_info.pseudo_palette);
-
-#ifdef CONFIG_MTRR
-	if (oi->fb_end_aligned_physaddr) {
-		mtrr_del(-1, oi->fb_start_aligned_physaddr,
-			oi->fb_end_aligned_physaddr - oi->fb_start_aligned_physaddr);
-	}
-#endif
-
+	arch_phys_wc_del(oi->wc_cookie);
 	kfree(oi);
 	itv->osd_info = NULL;
 }
@@ -1284,6 +1265,13 @@ static int __init ivtvfb_init(void)
 	int registered = 0;
 	int err;
 
+#ifdef CONFIG_X86_64
+	if (WARN(pat_enabled(),
+		 "ivtvfb needs PAT disabled, boot with nopat kernel parameter\n")) {
+		return -ENODEV;
+	}
+#endif
+
 	if (ivtvfb_card_id < -1 || ivtvfb_card_id >= IVTV_MAX_CARDS) {
 		printk(KERN_ERR "ivtvfb:  ivtvfb_card_id parameter is out of range (valid range: -1 - %d)\n",
 		     IVTV_MAX_CARDS - 1);
diff --git a/drivers/media/pci/mantis/hopper_cards.c b/drivers/media/pci/mantis/hopper_cards.c
index 104914a5bf06..68b5800030b7 100644
--- a/drivers/media/pci/mantis/hopper_cards.c
+++ b/drivers/media/pci/mantis/hopper_cards.c
@@ -106,6 +106,10 @@ static irqreturn_t hopper_irq_handler(int irq, void *dev_id)
 	}
 	if (stat & MANTIS_INT_IRQ1) {
 		dprintk(MANTIS_DEBUG, 0, "<%s>", label[2]);
+		spin_lock(&mantis->intmask_lock);
+		mmwrite(mmread(MANTIS_INT_MASK) & ~MANTIS_INT_IRQ1,
+			MANTIS_INT_MASK);
+		spin_unlock(&mantis->intmask_lock);
 		schedule_work(&mantis->uart_work);
 	}
 	if (stat & MANTIS_INT_OCERR) {
@@ -154,6 +158,7 @@ static irqreturn_t hopper_irq_handler(int irq, void *dev_id)
 static int hopper_pci_probe(struct pci_dev *pdev,
 			    const struct pci_device_id *pci_id)
 {
+	struct mantis_pci_drvdata *drvdata;
 	struct mantis_pci *mantis;
 	struct mantis_hwconfig *config;
 	int err = 0;
@@ -165,12 +170,16 @@ static int hopper_pci_probe(struct pci_dev *pdev,
 		goto fail0;
 	}
 
+	drvdata			= (void *)pci_id->driver_data;
 	mantis->num		= devs;
 	mantis->verbose		= verbose;
 	mantis->pdev		= pdev;
-	config			= (struct mantis_hwconfig *) pci_id->driver_data;
+	config			= drvdata->hwconfig;
 	config->irq_handler	= &hopper_irq_handler;
 	mantis->hwconfig	= config;
+	mantis->rc_map_name	= drvdata->rc_map_name;
+
+	spin_lock_init(&mantis->intmask_lock);
 
 	err = mantis_pci_init(mantis);
 	if (err) {
@@ -247,7 +256,8 @@ static void hopper_pci_remove(struct pci_dev *pdev)
 }
 
 static struct pci_device_id hopper_pci_table[] = {
-	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_3028_DVB_T, &vp3028_config),
+	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_3028_DVB_T, &vp3028_config,
+		   NULL),
 	{ }
 };
 
diff --git a/drivers/media/pci/mantis/mantis_cards.c b/drivers/media/pci/mantis/mantis_cards.c
index 801fc55b6167..cdefffc16d9e 100644
--- a/drivers/media/pci/mantis/mantis_cards.c
+++ b/drivers/media/pci/mantis/mantis_cards.c
@@ -25,6 +25,7 @@
 #include <linux/slab.h>
 #include <asm/irq.h>
 #include <linux/interrupt.h>
+#include <media/rc-map.h>
 
 #include "dmxdev.h"
 #include "dvbdev.h"
@@ -49,6 +50,7 @@
 #include "mantis_pci.h"
 #include "mantis_i2c.h"
 #include "mantis_reg.h"
+#include "mantis_input.h"
 
 static unsigned int verbose;
 module_param(verbose, int, 0644);
@@ -114,6 +116,10 @@ static irqreturn_t mantis_irq_handler(int irq, void *dev_id)
 	}
 	if (stat & MANTIS_INT_IRQ1) {
 		dprintk(MANTIS_DEBUG, 0, "<%s>", label[2]);
+		spin_lock(&mantis->intmask_lock);
+		mmwrite(mmread(MANTIS_INT_MASK) & ~MANTIS_INT_IRQ1,
+			MANTIS_INT_MASK);
+		spin_unlock(&mantis->intmask_lock);
 		schedule_work(&mantis->uart_work);
 	}
 	if (stat & MANTIS_INT_OCERR) {
@@ -162,6 +168,7 @@ static irqreturn_t mantis_irq_handler(int irq, void *dev_id)
 static int mantis_pci_probe(struct pci_dev *pdev,
 			    const struct pci_device_id *pci_id)
 {
+	struct mantis_pci_drvdata *drvdata;
 	struct mantis_pci *mantis;
 	struct mantis_hwconfig *config;
 	int err = 0;
@@ -169,84 +176,91 @@ static int mantis_pci_probe(struct pci_dev *pdev,
 	mantis = kzalloc(sizeof(struct mantis_pci), GFP_KERNEL);
 	if (mantis == NULL) {
 		printk(KERN_ERR "%s ERROR: Out of memory\n", __func__);
-		err = -ENOMEM;
-		goto fail0;
+		return -ENOMEM;
 	}
 
+	drvdata			= (void *)pci_id->driver_data;
 	mantis->num		= devs;
 	mantis->verbose		= verbose;
 	mantis->pdev		= pdev;
-	config			= (struct mantis_hwconfig *) pci_id->driver_data;
+	config			= drvdata->hwconfig;
 	config->irq_handler	= &mantis_irq_handler;
 	mantis->hwconfig	= config;
+	mantis->rc_map_name	= drvdata->rc_map_name;
+
+	spin_lock_init(&mantis->intmask_lock);
 
 	err = mantis_pci_init(mantis);
 	if (err) {
 		dprintk(MANTIS_ERROR, 1, "ERROR: Mantis PCI initialization failed <%d>", err);
-		goto fail1;
+		goto err_free_mantis;
 	}
 
 	err = mantis_stream_control(mantis, STREAM_TO_HIF);
 	if (err < 0) {
 		dprintk(MANTIS_ERROR, 1, "ERROR: Mantis stream control failed <%d>", err);
-		goto fail1;
+		goto err_pci_exit;
 	}
 
 	err = mantis_i2c_init(mantis);
 	if (err < 0) {
 		dprintk(MANTIS_ERROR, 1, "ERROR: Mantis I2C initialization failed <%d>", err);
-		goto fail2;
+		goto err_pci_exit;
 	}
 
 	err = mantis_get_mac(mantis);
 	if (err < 0) {
 		dprintk(MANTIS_ERROR, 1, "ERROR: Mantis MAC address read failed <%d>", err);
-		goto fail2;
+		goto err_i2c_exit;
 	}
 
 	err = mantis_dma_init(mantis);
 	if (err < 0) {
 		dprintk(MANTIS_ERROR, 1, "ERROR: Mantis DMA initialization failed <%d>", err);
-		goto fail3;
+		goto err_i2c_exit;
 	}
 
 	err = mantis_dvb_init(mantis);
 	if (err < 0) {
 		dprintk(MANTIS_ERROR, 1, "ERROR: Mantis DVB initialization failed <%d>", err);
-		goto fail4;
+		goto err_dma_exit;
+	}
+
+	err = mantis_input_init(mantis);
+	if (err < 0) {
+		dprintk(MANTIS_ERROR, 1,
+			"ERROR: Mantis DVB initialization failed <%d>", err);
+		goto err_dvb_exit;
 	}
+
 	err = mantis_uart_init(mantis);
 	if (err < 0) {
 		dprintk(MANTIS_ERROR, 1, "ERROR: Mantis UART initialization failed <%d>", err);
-		goto fail6;
+		goto err_input_exit;
 	}
 
 	devs++;
 
-	return err;
+	return 0;
 
+err_input_exit:
+	mantis_input_exit(mantis);
 
-	dprintk(MANTIS_ERROR, 1, "ERROR: Mantis UART exit! <%d>", err);
-	mantis_uart_exit(mantis);
+err_dvb_exit:
+	mantis_dvb_exit(mantis);
 
-fail6:
-fail4:
-	dprintk(MANTIS_ERROR, 1, "ERROR: Mantis DMA exit! <%d>", err);
+err_dma_exit:
 	mantis_dma_exit(mantis);
 
-fail3:
-	dprintk(MANTIS_ERROR, 1, "ERROR: Mantis I2C exit! <%d>", err);
+err_i2c_exit:
 	mantis_i2c_exit(mantis);
 
-fail2:
-	dprintk(MANTIS_ERROR, 1, "ERROR: Mantis PCI exit! <%d>", err);
+err_pci_exit:
 	mantis_pci_exit(mantis);
 
-fail1:
-	dprintk(MANTIS_ERROR, 1, "ERROR: Mantis free! <%d>", err);
+err_free_mantis:
 	kfree(mantis);
 
-fail0:
 	return err;
 }
 
@@ -257,6 +271,7 @@ static void mantis_pci_remove(struct pci_dev *pdev)
 	if (mantis) {
 
 		mantis_uart_exit(mantis);
+		mantis_input_exit(mantis);
 		mantis_dvb_exit(mantis);
 		mantis_dma_exit(mantis);
 		mantis_i2c_exit(mantis);
@@ -267,17 +282,28 @@ static void mantis_pci_remove(struct pci_dev *pdev)
 }
 
 static struct pci_device_id mantis_pci_table[] = {
-	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_1033_DVB_S, &vp1033_config),
-	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_1034_DVB_S, &vp1034_config),
-	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_1041_DVB_S2, &vp1041_config),
-	MAKE_ENTRY(TECHNISAT, SKYSTAR_HD2_10, &vp1041_config),
-	MAKE_ENTRY(TECHNISAT, SKYSTAR_HD2_20, &vp1041_config),
-	MAKE_ENTRY(TERRATEC, CINERGY_S2_PCI_HD, &vp1041_config),
-	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_2033_DVB_C, &vp2033_config),
-	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_2040_DVB_C, &vp2040_config),
-	MAKE_ENTRY(TECHNISAT, CABLESTAR_HD2, &vp2040_config),
-	MAKE_ENTRY(TERRATEC, CINERGY_C, &vp2040_config),
-	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_3030_DVB_T, &vp3030_config),
+	MAKE_ENTRY(TECHNISAT, CABLESTAR_HD2, &vp2040_config,
+		   RC_MAP_TECHNISAT_TS35),
+	MAKE_ENTRY(TECHNISAT, SKYSTAR_HD2_10, &vp1041_config,
+		   NULL),
+	MAKE_ENTRY(TECHNISAT, SKYSTAR_HD2_20, &vp1041_config,
+		   NULL),
+	MAKE_ENTRY(TERRATEC, CINERGY_C, &vp2040_config,
+		   RC_MAP_TERRATEC_CINERGY_C_PCI),
+	MAKE_ENTRY(TERRATEC, CINERGY_S2_PCI_HD, &vp1041_config,
+		   RC_MAP_TERRATEC_CINERGY_S2_HD),
+	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_1033_DVB_S, &vp1033_config,
+		   NULL),
+	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_1034_DVB_S, &vp1034_config,
+		   NULL),
+	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_1041_DVB_S2, &vp1041_config,
+		   RC_MAP_TWINHAN_DTV_CAB_CI),
+	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_2033_DVB_C, &vp2033_config,
+		   RC_MAP_TWINHAN_DTV_CAB_CI),
+	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_2040_DVB_C, &vp2040_config,
+		   NULL),
+	MAKE_ENTRY(TWINHAN_TECHNOLOGIES, MANTIS_VP_3030_DVB_T, &vp3030_config,
+		   NULL),
 	{ }
 };
 
diff --git a/drivers/media/pci/mantis/mantis_common.h b/drivers/media/pci/mantis/mantis_common.h
index 8ff448bb792d..d48778a366a9 100644
--- a/drivers/media/pci/mantis/mantis_common.h
+++ b/drivers/media/pci/mantis/mantis_common.h
@@ -25,6 +25,7 @@
 #include <linux/mutex.h>
 #include <linux/workqueue.h>
 
+#include "mantis_reg.h"
 #include "mantis_uart.h"
 
 #include "mantis_link.h"
@@ -68,12 +69,13 @@
 #define TECHNISAT		0x1ae4
 #define TERRATEC		0x153b
 
-#define MAKE_ENTRY(__subven, __subdev, __configptr) {			\
+#define MAKE_ENTRY(__subven, __subdev, __configptr, __rc) {		\
 		.vendor		= TWINHAN_TECHNOLOGIES,			\
 		.device		= MANTIS,				\
 		.subvendor	= (__subven),				\
 		.subdevice	= (__subdev),				\
-		.driver_data	= (unsigned long) (__configptr)		\
+		.driver_data	= (unsigned long)			\
+			&(struct mantis_pci_drvdata){__configptr, __rc}	\
 }
 
 enum mantis_i2c_mode {
@@ -101,6 +103,11 @@ struct mantis_hwconfig {
 	enum mantis_i2c_mode	i2c_mode;
 };
 
+struct mantis_pci_drvdata {
+	struct mantis_hwconfig *hwconfig;
+	char *rc_map_name;
+};
+
 struct mantis_pci {
 	unsigned int		verbose;
 
@@ -131,6 +138,7 @@ struct mantis_pci {
 	dma_addr_t		risc_dma;
 
 	struct tasklet_struct	tasklet;
+	spinlock_t		intmask_lock;
 
 	struct i2c_adapter	adapter;
 	int			i2c_rc;
@@ -165,15 +173,32 @@ struct mantis_pci {
 
 	struct mantis_ca	*mantis_ca;
 
-	wait_queue_head_t	uart_wq;
 	struct work_struct	uart_work;
-	spinlock_t		uart_lock;
 
 	struct rc_dev		*rc;
 	char			input_name[80];
 	char			input_phys[80];
+	char			*rc_map_name;
 };
 
 #define MANTIS_HIF_STATUS	(mantis->gpio_status)
 
+static inline void mantis_mask_ints(struct mantis_pci *mantis, u32 mask)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mantis->intmask_lock, flags);
+	mmwrite(mmread(MANTIS_INT_MASK) & ~mask, MANTIS_INT_MASK);
+	spin_unlock_irqrestore(&mantis->intmask_lock, flags);
+}
+
+static inline void mantis_unmask_ints(struct mantis_pci *mantis, u32 mask)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mantis->intmask_lock, flags);
+	mmwrite(mmread(MANTIS_INT_MASK) | mask, MANTIS_INT_MASK);
+	spin_unlock_irqrestore(&mantis->intmask_lock, flags);
+}
+
 #endif /* __MANTIS_COMMON_H */
diff --git a/drivers/media/pci/mantis/mantis_dma.c b/drivers/media/pci/mantis/mantis_dma.c
index 566c407175a4..1d59c7e039f7 100644
--- a/drivers/media/pci/mantis/mantis_dma.c
+++ b/drivers/media/pci/mantis/mantis_dma.c
@@ -190,7 +190,7 @@ void mantis_dma_start(struct mantis_pci *mantis)
 	mmwrite(0, MANTIS_DMA_CTL);
 	mantis->last_block = mantis->busy_block = 0;
 
-	mmwrite(mmread(MANTIS_INT_MASK) | MANTIS_INT_RISCI, MANTIS_INT_MASK);
+	mantis_unmask_ints(mantis, MANTIS_INT_RISCI);
 
 	mmwrite(MANTIS_FIFO_EN | MANTIS_DCAP_EN
 			       | MANTIS_RISC_EN, MANTIS_DMA_CTL);
@@ -209,8 +209,7 @@ void mantis_dma_stop(struct mantis_pci *mantis)
 
 	mmwrite(mmread(MANTIS_INT_STAT), MANTIS_INT_STAT);
 
-	mmwrite(mmread(MANTIS_INT_MASK) & ~(MANTIS_INT_RISCI |
-					    MANTIS_INT_RISCEN), MANTIS_INT_MASK);
+	mantis_mask_ints(mantis, MANTIS_INT_RISCI | MANTIS_INT_RISCEN);
 }
 
 
diff --git a/drivers/media/pci/mantis/mantis_i2c.c b/drivers/media/pci/mantis/mantis_i2c.c
index 895ddba3c0fb..d72ee47dc6e4 100644
--- a/drivers/media/pci/mantis/mantis_i2c.c
+++ b/drivers/media/pci/mantis/mantis_i2c.c
@@ -219,7 +219,7 @@ static struct i2c_algorithm mantis_algo = {
 
 int mantis_i2c_init(struct mantis_pci *mantis)
 {
-	u32 intstat, intmask;
+	u32 intstat;
 	struct i2c_adapter *i2c_adapter = &mantis->adapter;
 	struct pci_dev *pdev		= mantis->pdev;
 
@@ -242,11 +242,10 @@ int mantis_i2c_init(struct mantis_pci *mantis)
 	dprintk(MANTIS_DEBUG, 1, "Initializing I2C ..");
 
 	intstat = mmread(MANTIS_INT_STAT);
-	intmask = mmread(MANTIS_INT_MASK);
+	mmread(MANTIS_INT_MASK);
 	mmwrite(intstat, MANTIS_INT_STAT);
 	dprintk(MANTIS_DEBUG, 1, "Disabling I2C interrupt");
-	intmask = mmread(MANTIS_INT_MASK);
-	mmwrite((intmask & ~MANTIS_INT_I2CDONE), MANTIS_INT_MASK);
+	mantis_mask_ints(mantis, MANTIS_INT_I2CDONE);
 
 	return 0;
 }
@@ -254,11 +253,8 @@ EXPORT_SYMBOL_GPL(mantis_i2c_init);
 
 int mantis_i2c_exit(struct mantis_pci *mantis)
 {
-	u32 intmask;
-
 	dprintk(MANTIS_DEBUG, 1, "Disabling I2C interrupt");
-	intmask = mmread(MANTIS_INT_MASK);
-	mmwrite((intmask & ~MANTIS_INT_I2CDONE), MANTIS_INT_MASK);
+	mantis_mask_ints(mantis, MANTIS_INT_I2CDONE);
 
 	dprintk(MANTIS_DEBUG, 1, "Removing I2C adapter");
 	i2c_del_adapter(&mantis->adapter);
diff --git a/drivers/media/pci/mantis/mantis_input.c b/drivers/media/pci/mantis/mantis_input.c
index 0e5252e5c0ef..7f7f1d4d7bb1 100644
--- a/drivers/media/pci/mantis/mantis_input.c
+++ b/drivers/media/pci/mantis/mantis_input.c
@@ -12,14 +12,8 @@
 	but WITHOUT ANY WARRANTY; without even the implied warranty of
 	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 	GNU General Public License for more details.
-
-	You should have received a copy of the GNU General Public License
-	along with this program; if not, write to the Free Software
-	Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
-#if 0 /* Currently unused */
-
 #include <media/rc-core.h>
 #include <linux/pci.h>
 
@@ -30,100 +24,32 @@
 #include "dvb_net.h"
 
 #include "mantis_common.h"
-#include "mantis_reg.h"
-#include "mantis_uart.h"
+#include "mantis_input.h"
 
 #define MODULE_NAME "mantis_core"
-#define RC_MAP_MANTIS "rc-mantis"
-
-static struct rc_map_table mantis_ir_table[] = {
-	{ 0x29, KEY_POWER	},
-	{ 0x28, KEY_FAVORITES	},
-	{ 0x30, KEY_TEXT	},
-	{ 0x17, KEY_INFO	}, /* Preview */
-	{ 0x23, KEY_EPG		},
-	{ 0x3b, KEY_F22		}, /* Record List */
-	{ 0x3c, KEY_1		},
-	{ 0x3e, KEY_2		},
-	{ 0x39, KEY_3		},
-	{ 0x36, KEY_4		},
-	{ 0x22, KEY_5		},
-	{ 0x20, KEY_6		},
-	{ 0x32, KEY_7		},
-	{ 0x26, KEY_8		},
-	{ 0x24, KEY_9		},
-	{ 0x2a, KEY_0		},
-
-	{ 0x33, KEY_CANCEL	},
-	{ 0x2c, KEY_BACK	},
-	{ 0x15, KEY_CLEAR	},
-	{ 0x3f, KEY_TAB		},
-	{ 0x10, KEY_ENTER	},
-	{ 0x14, KEY_UP		},
-	{ 0x0d, KEY_RIGHT	},
-	{ 0x0e, KEY_DOWN	},
-	{ 0x11, KEY_LEFT	},
-
-	{ 0x21, KEY_VOLUMEUP	},
-	{ 0x35, KEY_VOLUMEDOWN	},
-	{ 0x3d, KEY_CHANNELDOWN	},
-	{ 0x3a, KEY_CHANNELUP	},
-	{ 0x2e, KEY_RECORD	},
-	{ 0x2b, KEY_PLAY	},
-	{ 0x13, KEY_PAUSE	},
-	{ 0x25, KEY_STOP	},
-
-	{ 0x1f, KEY_REWIND	},
-	{ 0x2d, KEY_FASTFORWARD	},
-	{ 0x1e, KEY_PREVIOUS	}, /* Replay |< */
-	{ 0x1d, KEY_NEXT	}, /* Skip   >| */
-
-	{ 0x0b, KEY_CAMERA	}, /* Capture */
-	{ 0x0f, KEY_LANGUAGE	}, /* SAP */
-	{ 0x18, KEY_MODE	}, /* PIP */
-	{ 0x12, KEY_ZOOM	}, /* Full screen */
-	{ 0x1c, KEY_SUBTITLE	},
-	{ 0x2f, KEY_MUTE	},
-	{ 0x16, KEY_F20		}, /* L/R */
-	{ 0x38, KEY_F21		}, /* Hibernate */
-
-	{ 0x37, KEY_SWITCHVIDEOMODE }, /* A/V */
-	{ 0x31, KEY_AGAIN	}, /* Recall */
-	{ 0x1a, KEY_KPPLUS	}, /* Zoom+ */
-	{ 0x19, KEY_KPMINUS	}, /* Zoom- */
-	{ 0x27, KEY_RED		},
-	{ 0x0C, KEY_GREEN	},
-	{ 0x01, KEY_YELLOW	},
-	{ 0x00, KEY_BLUE	},
-};
-
-static struct rc_map_list ir_mantis_map = {
-	.map = {
-		.scan = mantis_ir_table,
-		.size = ARRAY_SIZE(mantis_ir_table),
-		.rc_type = RC_TYPE_UNKNOWN,
-		.name = RC_MAP_MANTIS,
-	}
-};
+
+void mantis_input_process(struct mantis_pci *mantis, int scancode)
+{
+	if (mantis->rc)
+		rc_keydown(mantis->rc, RC_TYPE_UNKNOWN, scancode, 0);
+}
 
 int mantis_input_init(struct mantis_pci *mantis)
 {
 	struct rc_dev *dev;
 	int err;
 
-	err = rc_map_register(&ir_mantis_map);
-	if (err)
-		goto out;
-
 	dev = rc_allocate_device();
 	if (!dev) {
 		dprintk(MANTIS_ERROR, 1, "Remote device allocation failed");
 		err = -ENOMEM;
-		goto out_map;
+		goto out;
 	}
 
-	sprintf(mantis->input_name, "Mantis %s IR receiver", mantis->hwconfig->model_name);
-	sprintf(mantis->input_phys, "pci-%s/ir0", pci_name(mantis->pdev));
+	snprintf(mantis->input_name, sizeof(mantis->input_name),
+		 "Mantis %s IR receiver", mantis->hwconfig->model_name);
+	snprintf(mantis->input_phys, sizeof(mantis->input_phys),
+		 "pci-%s/ir0", pci_name(mantis->pdev));
 
 	dev->input_name         = mantis->input_name;
 	dev->input_phys         = mantis->input_phys;
@@ -132,7 +58,7 @@ int mantis_input_init(struct mantis_pci *mantis)
 	dev->input_id.product   = mantis->device_id;
 	dev->input_id.version   = 1;
 	dev->driver_name        = MODULE_NAME;
-	dev->map_name           = RC_MAP_MANTIS;
+	dev->map_name           = mantis->rc_map_name ? : RC_MAP_EMPTY;
 	dev->dev.parent         = &mantis->pdev->dev;
 
 	err = rc_register_device(dev);
@@ -146,17 +72,13 @@ int mantis_input_init(struct mantis_pci *mantis)
 
 out_dev:
 	rc_free_device(dev);
-out_map:
-	rc_map_unregister(&ir_mantis_map);
 out:
 	return err;
 }
+EXPORT_SYMBOL_GPL(mantis_input_init);
 
-int mantis_init_exit(struct mantis_pci *mantis)
+void mantis_input_exit(struct mantis_pci *mantis)
 {
 	rc_unregister_device(mantis->rc);
-	rc_map_unregister(&ir_mantis_map);
-	return 0;
 }
-
-#endif
+EXPORT_SYMBOL_GPL(mantis_input_exit);
diff --git a/drivers/media/pci/mantis/mantis_input.h b/drivers/media/pci/mantis/mantis_input.h
new file mode 100644
index 000000000000..0fbd92987c02
--- /dev/null
+++ b/drivers/media/pci/mantis/mantis_input.h
@@ -0,0 +1,24 @@
+/*
+	Mantis PCI bridge driver
+
+	Copyright (C) Manu Abraham (abraham.manu@gmail.com)
+
+	This program is free software; you can redistribute it and/or modify
+	it under the terms of the GNU General Public License as published by
+	the Free Software Foundation; either version 2 of the License, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+	GNU General Public License for more details.
+*/
+
+#ifndef __MANTIS_INPUT_H
+#define __MANTIS_INPUT_H
+
+int mantis_input_init(struct mantis_pci *mantis);
+void mantis_input_exit(struct mantis_pci *mantis);
+void mantis_input_process(struct mantis_pci *mantis, int scancode);
+
+#endif /* __MANTIS_UART_H */
diff --git a/drivers/media/pci/mantis/mantis_pcmcia.c b/drivers/media/pci/mantis/mantis_pcmcia.c
index 2f188c089666..b2dbc7b2e0f6 100644
--- a/drivers/media/pci/mantis/mantis_pcmcia.c
+++ b/drivers/media/pci/mantis/mantis_pcmcia.c
@@ -89,7 +89,7 @@ int mantis_pcmcia_init(struct mantis_ca *ca)
 
 	u32 gpif_stat, card_stat;
 
-	mmwrite(mmread(MANTIS_INT_MASK) | MANTIS_INT_IRQ0, MANTIS_INT_MASK);
+	mantis_unmask_ints(mantis, MANTIS_INT_IRQ0);
 	gpif_stat = mmread(MANTIS_GPIF_STATUS);
 	card_stat = mmread(MANTIS_GPIF_IRQCFG);
 
@@ -117,5 +117,5 @@ void mantis_pcmcia_exit(struct mantis_ca *ca)
 	struct mantis_pci *mantis = ca->ca_priv;
 
 	mmwrite(mmread(MANTIS_GPIF_STATUS) & (~MANTIS_CARD_PLUGOUT | ~MANTIS_CARD_PLUGIN), MANTIS_GPIF_STATUS);
-	mmwrite(mmread(MANTIS_INT_MASK) & ~MANTIS_INT_IRQ0, MANTIS_INT_MASK);
+	mantis_mask_ints(mantis, MANTIS_INT_IRQ0);
 }
diff --git a/drivers/media/pci/mantis/mantis_uart.c b/drivers/media/pci/mantis/mantis_uart.c
index a70719218631..f1c96aec8c7b 100644
--- a/drivers/media/pci/mantis/mantis_uart.c
+++ b/drivers/media/pci/mantis/mantis_uart.c
@@ -25,6 +25,7 @@
 #include <linux/signal.h>
 #include <linux/sched.h>
 #include <linux/interrupt.h>
+#include <linux/pci.h>
 
 #include "dmxdev.h"
 #include "dvbdev.h"
@@ -35,6 +36,7 @@
 #include "mantis_common.h"
 #include "mantis_reg.h"
 #include "mantis_uart.h"
+#include "mantis_input.h"
 
 struct mantis_uart_params {
 	enum mantis_baud	baud_rate;
@@ -59,51 +61,54 @@ static struct {
 	{ "EVEN" }
 };
 
-#define UART_MAX_BUF			16
-
-static int mantis_uart_read(struct mantis_pci *mantis, u8 *data)
+static void mantis_uart_read(struct mantis_pci *mantis)
 {
 	struct mantis_hwconfig *config = mantis->hwconfig;
-	u32 stat = 0, i;
+	int i, scancode = 0, err = 0;
 
 	/* get data */
+	dprintk(MANTIS_DEBUG, 1, "UART Reading ...");
 	for (i = 0; i < (config->bytes + 1); i++) {
+		int data = mmread(MANTIS_UART_RXD);
 
-		stat = mmread(MANTIS_UART_STAT);
-
-		if (stat & MANTIS_UART_RXFIFO_FULL) {
-			dprintk(MANTIS_ERROR, 1, "RX Fifo FULL");
-		}
-		data[i] = mmread(MANTIS_UART_RXD) & 0x3f;
+		dprintk(MANTIS_DEBUG, 0, " <%02x>", data);
 
-		dprintk(MANTIS_DEBUG, 1, "Reading ... <%02x>", data[i] & 0x3f);
+		scancode = (scancode << 8) | (data & 0x3f);
+		err |= data;
 
-		if (data[i] & (1 << 7)) {
+		if (data & (1 << 7))
 			dprintk(MANTIS_ERROR, 1, "UART framing error");
-			return -EINVAL;
-		}
-		if (data[i] & (1 << 6)) {
+
+		if (data & (1 << 6))
 			dprintk(MANTIS_ERROR, 1, "UART parity error");
-			return -EINVAL;
-		}
 	}
+	dprintk(MANTIS_DEBUG, 0, "\n");
 
-	return 0;
+	if ((err & 0xC0) == 0)
+		mantis_input_process(mantis, scancode);
 }
 
 static void mantis_uart_work(struct work_struct *work)
 {
 	struct mantis_pci *mantis = container_of(work, struct mantis_pci, uart_work);
-	struct mantis_hwconfig *config = mantis->hwconfig;
-	u8 buf[16];
-	int i;
+	u32 stat;
 
-	mantis_uart_read(mantis, buf);
+	stat = mmread(MANTIS_UART_STAT);
 
-	for (i = 0; i < (config->bytes + 1); i++)
-		dprintk(MANTIS_INFO, 1, "UART BUF:%d <%02x> ", i, buf[i]);
+	if (stat & MANTIS_UART_RXFIFO_FULL)
+		dprintk(MANTIS_ERROR, 1, "RX Fifo FULL");
 
-	dprintk(MANTIS_DEBUG, 0, "\n");
+	/*
+	 * MANTIS_UART_RXFIFO_DATA is only set if at least
+	 * config->bytes + 1 bytes are in the FIFO.
+	 */
+	while (stat & MANTIS_UART_RXFIFO_DATA) {
+		mantis_uart_read(mantis);
+		stat = mmread(MANTIS_UART_STAT);
+	}
+
+	/* re-enable UART (RX) interrupt */
+	mantis_unmask_ints(mantis, MANTIS_INT_IRQ1);
 }
 
 static int mantis_uart_setup(struct mantis_pci *mantis,
@@ -152,9 +157,6 @@ int mantis_uart_init(struct mantis_pci *mantis)
 		rates[params.baud_rate].string,
 		parity[params.parity].string);
 
-	init_waitqueue_head(&mantis->uart_wq);
-	spin_lock_init(&mantis->uart_lock);
-
 	INIT_WORK(&mantis->uart_work, mantis_uart_work);
 
 	/* disable interrupt */
@@ -169,8 +171,8 @@ int mantis_uart_init(struct mantis_pci *mantis)
 	mmwrite((mmread(MANTIS_UART_CTL) | MANTIS_UART_RXFLUSH), MANTIS_UART_CTL);
 
 	/* enable interrupt */
-	mmwrite(mmread(MANTIS_INT_MASK) | 0x800, MANTIS_INT_MASK);
 	mmwrite(mmread(MANTIS_UART_CTL) | MANTIS_UART_RXINT, MANTIS_UART_CTL);
+	mantis_unmask_ints(mantis, MANTIS_INT_IRQ1);
 
 	schedule_work(&mantis->uart_work);
 	dprintk(MANTIS_DEBUG, 1, "UART successfully initialized");
@@ -182,6 +184,7 @@ EXPORT_SYMBOL_GPL(mantis_uart_init);
 void mantis_uart_exit(struct mantis_pci *mantis)
 {
 	/* disable interrupt */
+	mantis_mask_ints(mantis, MANTIS_INT_IRQ1);
 	mmwrite(mmread(MANTIS_UART_CTL) & 0xffef, MANTIS_UART_CTL);
 	flush_work(&mantis->uart_work);
 }
diff --git a/drivers/media/pci/mantis/mantis_vp1034.c b/drivers/media/pci/mantis/mantis_vp1034.c
index 7c1bd167225c..3b1928594b12 100644
--- a/drivers/media/pci/mantis/mantis_vp1034.c
+++ b/drivers/media/pci/mantis/mantis_vp1034.c
@@ -44,7 +44,7 @@ static struct mb86a16_config vp1034_mb86a16_config = {
 #define MANTIS_MODEL_NAME	"VP-1034"
 #define MANTIS_DEV_TYPE		"DVB-S/DSS"
 
-int vp1034_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+int vp1034_set_voltage(struct dvb_frontend *fe, enum fe_sec_voltage voltage)
 {
 	struct mantis_pci *mantis = fe->dvb->priv;
 
diff --git a/drivers/media/pci/mantis/mantis_vp1034.h b/drivers/media/pci/mantis/mantis_vp1034.h
index 323f38ef8e3d..764b1c66ea1b 100644
--- a/drivers/media/pci/mantis/mantis_vp1034.h
+++ b/drivers/media/pci/mantis/mantis_vp1034.h
@@ -28,6 +28,7 @@
 #define MANTIS_VP_1034_DVB_S	0x0014
 
 extern struct mantis_hwconfig vp1034_config;
-extern int vp1034_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage);
+extern int vp1034_set_voltage(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage);
 
 #endif /* __MANTIS_VP1034_H */
diff --git a/drivers/media/pci/ngene/ngene-core.c b/drivers/media/pci/ngene/ngene-core.c
index e29bc3af4baf..1b92d836a564 100644
--- a/drivers/media/pci/ngene/ngene-core.c
+++ b/drivers/media/pci/ngene/ngene-core.c
@@ -1526,10 +1526,12 @@ static int init_channel(struct ngene_channel *chan)
 	if (chan->fe2) {
 		if (dvb_register_frontend(adapter, chan->fe2) < 0)
 			goto err;
-		chan->fe2->tuner_priv = chan->fe->tuner_priv;
-		memcpy(&chan->fe2->ops.tuner_ops,
-		       &chan->fe->ops.tuner_ops,
-		       sizeof(struct dvb_tuner_ops));
+		if (chan->fe) {
+			chan->fe2->tuner_priv = chan->fe->tuner_priv;
+			memcpy(&chan->fe2->ops.tuner_ops,
+			       &chan->fe->ops.tuner_ops,
+			       sizeof(struct dvb_tuner_ops));
+		}
 	}
 
 	if (chan->has_demux) {
diff --git a/drivers/media/pci/ngene/ngene.h b/drivers/media/pci/ngene/ngene.h
index 51e2fbd18b1b..fa30930d7047 100644
--- a/drivers/media/pci/ngene/ngene.h
+++ b/drivers/media/pci/ngene/ngene.h
@@ -682,7 +682,7 @@ struct ngene_channel {
 	int                   AudioDTOUpdated;
 	u32                   AudioDTOValue;
 
-	int (*set_tone)(struct dvb_frontend *, fe_sec_tone_mode_t);
+	int (*set_tone)(struct dvb_frontend *, enum fe_sec_tone_mode);
 	u8 lnbh;
 
 	/* stuff from analog driver */
diff --git a/drivers/media/pci/pt1/pt1.c b/drivers/media/pci/pt1/pt1.c
index acc35b42e53c..e7e4428109c3 100644
--- a/drivers/media/pci/pt1/pt1.c
+++ b/drivers/media/pci/pt1/pt1.c
@@ -101,11 +101,11 @@ struct pt1_adapter {
 	struct dmxdev dmxdev;
 	struct dvb_frontend *fe;
 	int (*orig_set_voltage)(struct dvb_frontend *fe,
-				fe_sec_voltage_t voltage);
+				enum fe_sec_voltage voltage);
 	int (*orig_sleep)(struct dvb_frontend *fe);
 	int (*orig_init)(struct dvb_frontend *fe);
 
-	fe_sec_voltage_t voltage;
+	enum fe_sec_voltage voltage;
 	int sleep;
 };
 
@@ -575,7 +575,7 @@ pt1_update_power(struct pt1 *pt1)
 	mutex_unlock(&pt1->lock);
 }
 
-static int pt1_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int pt1_set_voltage(struct dvb_frontend *fe, enum fe_sec_voltage voltage)
 {
 	struct pt1_adapter *adap;
 
diff --git a/drivers/media/pci/pt1/va1j5jf8007s.c b/drivers/media/pci/pt1/va1j5jf8007s.c
index 1b637b74ef58..d0e70dc0e16f 100644
--- a/drivers/media/pci/pt1/va1j5jf8007s.c
+++ b/drivers/media/pci/pt1/va1j5jf8007s.c
@@ -108,7 +108,7 @@ static int va1j5jf8007s_get_frontend_algo(struct dvb_frontend *fe)
 }
 
 static int
-va1j5jf8007s_read_status(struct dvb_frontend *fe, fe_status_t *status)
+va1j5jf8007s_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct va1j5jf8007s_state *state;
 
@@ -387,7 +387,7 @@ static int
 va1j5jf8007s_tune(struct dvb_frontend *fe,
 		  bool re_tune,
 		  unsigned int mode_flags,  unsigned int *delay,
-		  fe_status_t *status)
+		  enum fe_status *status)
 {
 	struct va1j5jf8007s_state *state;
 	int ret;
diff --git a/drivers/media/pci/pt1/va1j5jf8007t.c b/drivers/media/pci/pt1/va1j5jf8007t.c
index 2db15159d514..0268f20b8097 100644
--- a/drivers/media/pci/pt1/va1j5jf8007t.c
+++ b/drivers/media/pci/pt1/va1j5jf8007t.c
@@ -98,7 +98,7 @@ static int va1j5jf8007t_get_frontend_algo(struct dvb_frontend *fe)
 }
 
 static int
-va1j5jf8007t_read_status(struct dvb_frontend *fe, fe_status_t *status)
+va1j5jf8007t_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct va1j5jf8007t_state *state;
 
@@ -266,7 +266,7 @@ static int
 va1j5jf8007t_tune(struct dvb_frontend *fe,
 		  bool re_tune,
 		  unsigned int mode_flags,  unsigned int *delay,
-		  fe_status_t *status)
+		  enum fe_status *status)
 {
 	struct va1j5jf8007t_state *state;
 	int ret;
diff --git a/drivers/media/pci/pt3/pt3.c b/drivers/media/pci/pt3/pt3.c
index 7a37e8fe2ee2..0d2e2b217121 100644
--- a/drivers/media/pci/pt3/pt3.c
+++ b/drivers/media/pci/pt3/pt3.c
@@ -188,7 +188,7 @@ static int pt3_set_lna(struct dvb_frontend *fe)
 	return ret;
 }
 
-static int pt3_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
+static int pt3_set_voltage(struct dvb_frontend *fe, enum fe_sec_voltage volt)
 {
 	struct pt3_adapter *adap;
 	struct pt3_board *pt3;
diff --git a/drivers/media/pci/saa7134/saa7134-alsa.c b/drivers/media/pci/saa7134/saa7134-alsa.c
index ac3cd74e824e..1d2c310ce838 100644
--- a/drivers/media/pci/saa7134/saa7134-alsa.c
+++ b/drivers/media/pci/saa7134/saa7134-alsa.c
@@ -16,6 +16,9 @@
  *
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/time.h>
@@ -29,13 +32,6 @@
 #include <linux/interrupt.h>
 #include <linux/vmalloc.h>
 
-#include "saa7134.h"
-#include "saa7134-reg.h"
-
-static unsigned int debug;
-module_param(debug, int, 0644);
-MODULE_PARM_DESC(debug,"enable debug messages [alsa]");
-
 /*
  * Configuration macros
  */
@@ -57,11 +53,6 @@ module_param_array(enable, int, NULL, 0444);
 MODULE_PARM_DESC(index, "Index value for SAA7134 capture interface(s).");
 MODULE_PARM_DESC(enable, "Enable (or not) the SAA7134 capture interface(s).");
 
-#define dprintk(fmt, arg...)    if (debug) \
-	printk(KERN_DEBUG "%s/alsa: " fmt, dev->name , ##arg)
-
-
-
 /*
  * Main chip structure
  */
@@ -149,11 +140,11 @@ static void saa7134_irq_alsa_done(struct saa7134_dev *dev,
 
 	spin_lock(&dev->slock);
 	if (UNSET == dev->dmasound.dma_blk) {
-		dprintk("irq: recording stopped\n");
+		pr_debug("irq: recording stopped\n");
 		goto done;
 	}
 	if (0 != (status & 0x0f000000))
-		dprintk("irq: lost %ld\n", (status >> 24) & 0x0f);
+		pr_debug("irq: lost %ld\n", (status >> 24) & 0x0f);
 	if (0 == (status & 0x10000000)) {
 		/* odd */
 		if (0 == (dev->dmasound.dma_blk & 0x01))
@@ -164,13 +155,14 @@ static void saa7134_irq_alsa_done(struct saa7134_dev *dev,
 			reg = SAA7134_RS_BA2(6);
 	}
 	if (0 == reg) {
-		dprintk("irq: field oops [%s]\n",
+		pr_debug("irq: field oops [%s]\n",
 			(status & 0x10000000) ? "even" : "odd");
 		goto done;
 	}
 
 	if (dev->dmasound.read_count >= dev->dmasound.blksize * (dev->dmasound.blocks-2)) {
-		dprintk("irq: overrun [full=%d/%d] - Blocks in %d\n",dev->dmasound.read_count,
+		pr_debug("irq: overrun [full=%d/%d] - Blocks in %d\n",
+			dev->dmasound.read_count,
 			dev->dmasound.bufsize, dev->dmasound.blocks);
 		spin_unlock(&dev->slock);
 		snd_pcm_stop_xrun(dev->dmasound.substream);
@@ -180,10 +172,10 @@ static void saa7134_irq_alsa_done(struct saa7134_dev *dev,
 	/* next block addr */
 	next_blk = (dev->dmasound.dma_blk + 2) % dev->dmasound.blocks;
 	saa_writel(reg,next_blk * dev->dmasound.blksize);
-	if (debug > 2)
-		dprintk("irq: ok, %s, next_blk=%d, addr=%x, blocks=%u, size=%u, read=%u\n",
-			(status & 0x10000000) ? "even" : "odd ", next_blk,
-			next_blk * dev->dmasound.blksize, dev->dmasound.blocks, dev->dmasound.blksize, dev->dmasound.read_count);
+	pr_debug("irq: ok, %s, next_blk=%d, addr=%x, blocks=%u, size=%u, read=%u\n",
+		(status & 0x10000000) ? "even" : "odd ", next_blk,
+		 next_blk * dev->dmasound.blksize, dev->dmasound.blocks,
+		 dev->dmasound.blksize, dev->dmasound.read_count);
 
 	/* update status & wake waiting readers */
 	dev->dmasound.dma_blk = (dev->dmasound.dma_blk + 1) % dev->dmasound.blocks;
@@ -233,7 +225,7 @@ static irqreturn_t saa7134_alsa_irq(int irq, void *dev_id)
 	}
 
 	if (loop == 10) {
-		dprintk("error! looping IRQ!");
+		pr_debug("error! looping IRQ!");
 	}
 
 out:
@@ -281,11 +273,11 @@ static int saa7134_alsa_dma_init(struct saa7134_dev *dev, int nr_pages)
 
 	dma->vaddr = vmalloc_32(nr_pages << PAGE_SHIFT);
 	if (NULL == dma->vaddr) {
-		dprintk("vmalloc_32(%d pages) failed\n", nr_pages);
+		pr_debug("vmalloc_32(%d pages) failed\n", nr_pages);
 		return -ENOMEM;
 	}
 
-	dprintk("vmalloc is at addr 0x%08lx, size=%d\n",
+	pr_debug("vmalloc is at addr 0x%08lx, size=%d\n",
 				(unsigned long)dma->vaddr,
 				nr_pages << PAGE_SHIFT);
 
@@ -572,7 +564,7 @@ static int snd_card_saa7134_capture_prepare(struct snd_pcm_substream * substream
 		break;
 	}
 
-	dprintk("rec_start: afmt=%d ch=%d  =>  fmt=0x%x swap=%c\n",
+	pr_debug("rec_start: afmt=%d ch=%d  =>  fmt=0x%x swap=%c\n",
 		runtime->format, runtime->channels, fmt,
 		bswap ? 'b' : '-');
 	/* dma: setup channel 6 (= AUDIO) */
@@ -821,7 +813,7 @@ static int snd_card_saa7134_capture_open(struct snd_pcm_substream * substream)
 	int amux, err;
 
 	if (!saa7134) {
-		printk(KERN_ERR "BUG: saa7134 can't find device struct."
+		pr_err("BUG: saa7134 can't find device struct."
 				" Can't proceed with open\n");
 		return -ENODEV;
 	}
@@ -1175,7 +1167,7 @@ static int alsa_card_saa7134_create(struct saa7134_dev *dev, int devnum)
 				(void*) &dev->dmasound);
 
 	if (err < 0) {
-		printk(KERN_ERR "%s: can't get IRQ %d for ALSA\n",
+		pr_err("%s: can't get IRQ %d for ALSA\n",
 			dev->name, dev->pci->irq);
 		goto __nodev;
 	}
@@ -1196,7 +1188,8 @@ static int alsa_card_saa7134_create(struct saa7134_dev *dev, int devnum)
 	sprintf(card->longname, "%s at 0x%lx irq %d",
 		chip->dev->name, chip->iobase, chip->irq);
 
-	printk(KERN_INFO "%s/alsa: %s registered as card %d\n",dev->name,card->longname,index[devnum]);
+	pr_info("%s/alsa: %s registered as card %d\n",
+		dev->name, card->longname, index[devnum]);
 
 	if ((err = snd_card_register(card)) == 0) {
 		snd_saa7134_cards[devnum] = card;
@@ -1240,19 +1233,19 @@ static int saa7134_alsa_init(void)
 	saa7134_dmasound_init = alsa_device_init;
 	saa7134_dmasound_exit = alsa_device_exit;
 
-	printk(KERN_INFO "saa7134 ALSA driver for DMA sound loaded\n");
+	pr_info("saa7134 ALSA driver for DMA sound loaded\n");
 
 	list_for_each(list,&saa7134_devlist) {
 		dev = list_entry(list, struct saa7134_dev, devlist);
 		if (dev->pci->device == PCI_DEVICE_ID_PHILIPS_SAA7130)
-			printk(KERN_INFO "%s/alsa: %s doesn't support digital audio\n",
+			pr_info("%s/alsa: %s doesn't support digital audio\n",
 				dev->name, saa7134_boards[dev->board].name);
 		else
 			alsa_device_init(dev);
 	}
 
 	if (dev == NULL)
-		printk(KERN_INFO "saa7134 ALSA: no saa7134 cards found\n");
+		pr_info("saa7134 ALSA: no saa7134 cards found\n");
 
 	return 0;
 
@@ -1272,7 +1265,7 @@ static void saa7134_alsa_exit(void)
 
 	saa7134_dmasound_init = NULL;
 	saa7134_dmasound_exit = NULL;
-	printk(KERN_INFO "saa7134 ALSA driver for DMA sound unloaded\n");
+	pr_info("saa7134 ALSA driver for DMA sound unloaded\n");
 
 	return;
 }
diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
index 3ca078057755..c7405766609c 100644
--- a/drivers/media/pci/saa7134/saa7134-cards.c
+++ b/drivers/media/pci/saa7134/saa7134-cards.c
@@ -20,13 +20,14 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
 #include "tuner-xc2028.h"
 #include <media/v4l2-common.h>
 #include <media/tveeprom.h>
@@ -5850,6 +5851,39 @@ struct saa7134_board saa7134_boards[] = {
 		.amux = LINE1,
 		} },
 	},
+	[SAA7134_BOARD_AVERMEDIA_505] = {
+		/* much like the "studio" version but without radio
+		* and another tuner (dbaryshkov@gmail.com) */
+		.name           = "AverMedia AverTV/505",
+		.audio_clock    = 0x00187de7,
+		.tuner_type     = TUNER_PHILIPS_FQ1216ME,
+		.radio_type     = UNSET,
+		.tuner_addr	= ADDR_UNSET,
+		.radio_addr	= ADDR_UNSET,
+		.tda9887_conf   = TDA9887_PRESENT,
+		.inputs         = {{
+			.name = name_tv,
+			.vmux = 1,
+			.amux = LINE2,
+			.tv   = 1,
+		}, {
+			.name = name_comp1,
+			.vmux = 0,
+			.amux = LINE2,
+		}, {
+			.name = name_comp2,
+			.vmux = 3,
+			.amux = LINE2,
+		}, {
+			.name = name_svideo,
+			.vmux = 8,
+			.amux = LINE2,
+		} },
+		.mute = {
+			.name = name_mute,
+			.amux = LINE1,
+		},
+	},
 
 };
 
@@ -7109,6 +7143,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
 		.subdevice    = 0x7007,
 		.driver_data  = SAA7134_BOARD_WIS_VOYAGER,
 	}, {
+		.vendor       = PCI_VENDOR_ID_PHILIPS,
+		.device       = PCI_DEVICE_ID_PHILIPS_SAA7130,
+		.subvendor    = 0x1461, /* Avermedia Technologies Inc */
+		.subdevice    = 0xa10a,
+		.driver_data  = SAA7134_BOARD_AVERMEDIA_505,
+	}, {
 		/* --- boards without eeprom + subsystem ID --- */
 		.vendor       = PCI_VENDOR_ID_PHILIPS,
 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
@@ -7158,10 +7198,10 @@ MODULE_DEVICE_TABLE(pci, saa7134_pci_tbl);
 
 static void board_flyvideo(struct saa7134_dev *dev)
 {
-	printk("%s: there are different flyvideo cards with different tuners\n"
-	       "%s: out there, you might have to use the tuner=<nr> insmod\n"
-	       "%s: option to override the default value.\n",
-	       dev->name, dev->name, dev->name);
+	pr_warn("%s: there are different flyvideo cards with different tuners\n"
+		"%s: out there, you might have to use the tuner=<nr> insmod\n"
+		"%s: option to override the default value.\n",
+		dev->name, dev->name, dev->name);
 }
 
 static int saa7134_xc2028_callback(struct saa7134_dev *dev,
@@ -7194,7 +7234,7 @@ static int saa7134_xc2028_callback(struct saa7134_dev *dev,
 			saa7134_set_gpio(dev, 20, 1);
 		break;
 		}
-	return 0;
+		return 0;
 	}
 	return -EINVAL;
 }
@@ -7380,7 +7420,7 @@ int saa7134_tuner_callback(void *priv, int component, int command, int arg)
 			return saa7134_xc5000_callback(dev, command, arg);
 		}
 	} else {
-		printk(KERN_ERR "saa7134: Error - device struct undefined.\n");
+		pr_err("saa7134: Error - device struct undefined.\n");
 		return -EINVAL;
 	}
 	return -EINVAL;
@@ -7411,12 +7451,12 @@ static void hauppauge_eeprom(struct saa7134_dev *dev, u8 *eeprom_data)
 	case 67659: /* WinTV-HVR1110 (OEM, no IR, hybrid, FM, SVid/Comp, RCA aud) */
 		break;
 	default:
-		printk(KERN_WARNING "%s: warning: "
+		pr_warn("%s: warning: "
 		       "unknown hauppauge model #%d\n", dev->name, tv.model);
 		break;
 	}
 
-	printk(KERN_INFO "%s: hauppauge eeprom: model=%d\n",
+	pr_info("%s: hauppauge eeprom: model=%d\n",
 	       dev->name, tv.model);
 }
 
@@ -7427,7 +7467,7 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 	/* Always print gpio, often manufacturers encode tuner type and other info. */
 	saa_writel(SAA7134_GPIO_GPMODE0 >> 2, 0);
 	dev->gpio_value = saa_readl(SAA7134_GPIO_GPSTATUS0 >> 2);
-	printk(KERN_INFO "%s: board init: gpio is %x\n", dev->name, dev->gpio_value);
+	pr_info("%s: board init: gpio is %x\n", dev->name, dev->gpio_value);
 
 	switch (dev->board) {
 	case SAA7134_BOARD_FLYVIDEO2000:
@@ -7448,8 +7488,9 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 	case SAA7134_BOARD_KWORLD_VSTREAM_XPERT:
 	case SAA7134_BOARD_KWORLD_XPERT:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_305:
-	case SAA7134_BOARD_AVERMEDIA_STUDIO_505:
 	case SAA7134_BOARD_AVERMEDIA_305:
+	case SAA7134_BOARD_AVERMEDIA_STUDIO_505:
+	case SAA7134_BOARD_AVERMEDIA_505:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_307:
 	case SAA7134_BOARD_AVERMEDIA_307:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_507:
@@ -7512,10 +7553,10 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 		dev->has_remote = SAA7134_REMOTE_GPIO;
 		break;
 	case SAA7134_BOARD_MD5044:
-		printk("%s: seems there are two different versions of the MD5044\n"
-		       "%s: (with the same ID) out there.  If sound doesn't work for\n"
-		       "%s: you try the audio_clock_override=0x200000 insmod option.\n",
-		       dev->name,dev->name,dev->name);
+		pr_warn("%s: seems there are two different versions of the MD5044\n"
+			"%s: (with the same ID) out there.  If sound doesn't work for\n"
+			"%s: you try the audio_clock_override=0x200000 insmod option.\n",
+			dev->name, dev->name, dev->name);
 		break;
 	case SAA7134_BOARD_CINERGY400_CARDBUS:
 		/* power-up tuner chip */
@@ -7640,10 +7681,10 @@ int saa7134_board_init1(struct saa7134_dev *dev)
 		dev->has_remote = SAA7134_REMOTE_I2C;
 		break;
 	case SAA7134_BOARD_AVERMEDIA_A169_B:
-		printk("%s: %s: dual saa713x broadcast decoders\n"
-		       "%s: Sorry, none of the inputs to this chip are supported yet.\n"
-		       "%s: Dual decoder functionality is disabled for now, use the other chip.\n",
-		       dev->name,card(dev).name,dev->name,dev->name);
+		pr_warn("%s: %s: dual saa713x broadcast decoders\n"
+			"%s: Sorry, none of the inputs to this chip are supported yet.\n"
+			"%s: Dual decoder functionality is disabled for now, use the other chip.\n",
+			dev->name, card(dev).name, dev->name, dev->name);
 		break;
 	case SAA7134_BOARD_AVERMEDIA_M102:
 		/* enable tuner */
@@ -7789,7 +7830,7 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 		if (board == dev->board)
 			break;
 		dev->board = board;
-		printk("%s: board type fixup: %s\n", dev->name,
+		pr_warn("%s: board type fixup: %s\n", dev->name,
 		saa7134_boards[dev->board].name);
 		dev->tuner_type = saa7134_boards[dev->board].tuner_type;
 
@@ -7797,10 +7838,11 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 	case SAA7134_BOARD_MD7134:
 	{
 		u8 subaddr;
-		u8 data[3];
+		u8 data[3], data1[] = { 0x09, 0x9f, 0x86, 0x11};
 		int ret, tuner_t;
-		struct i2c_msg msg[] = {{.addr=0x50, .flags=0, .buf=&subaddr, .len = 1},
-					{.addr=0x50, .flags=I2C_M_RD, .buf=data, .len = 3}};
+		struct i2c_msg msg[] = {{.addr = 0x50, .flags = 0, .buf = &subaddr, .len = 1},
+					{.addr = 0x50, .flags = I2C_M_RD, .buf = data, .len = 3}},
+				msg1 = {.addr = 0x61, .flags = 0, .buf = data1, .len = sizeof(data1)};
 
 		subaddr= 0x14;
 		tuner_t = 0;
@@ -7810,7 +7852,7 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 		 */
 		ret = i2c_transfer(&dev->i2c_adap, msg, 2);
 		if (ret != 2) {
-			printk(KERN_ERR "EEPROM read failure\n");
+			pr_err("EEPROM read failure\n");
 		} else if ((data[0] != 0) && (data[0] != 0xff)) {
 			/* old config structure */
 			subaddr = data[0] + 2;
@@ -7825,7 +7867,8 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 				dev->tuner_type = TUNER_PHILIPS_FM1216ME_MK3;
 				break;
 			default:
-				printk(KERN_ERR "%s Can't determine tuner type %x from EEPROM\n", dev->name, tuner_t);
+				pr_err("%s Can't determine tuner type %x from EEPROM\n",
+				       dev->name, tuner_t);
 			}
 		} else if ((data[1] != 0) && (data[1] != 0xff)) {
 			/* new config structure */
@@ -7842,16 +7885,28 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 				break;
 			case 0x001d:
 				dev->tuner_type = TUNER_PHILIPS_FMD1216ME_MK3;
-					printk(KERN_INFO "%s Board has DVB-T\n", dev->name);
+				pr_info("%s Board has DVB-T\n",
+				       dev->name);
 				break;
 			default:
-				printk(KERN_ERR "%s Can't determine tuner type %x from EEPROM\n", dev->name, tuner_t);
+				pr_err("%s Can't determine tuner type %x from EEPROM\n",
+				       dev->name, tuner_t);
 			}
 		} else {
-			printk(KERN_ERR "%s unexpected config structure\n", dev->name);
+			pr_err("%s unexpected config structure\n", dev->name);
 		}
 
-		printk(KERN_INFO "%s Tuner type is %d\n", dev->name, dev->tuner_type);
+		pr_info("%s Tuner type is %d\n", dev->name, dev->tuner_type);
+
+		/* The tuner TUNER_PHILIPS_FMD1216ME_MK3 after hardware    */
+		/* start has disabled IF and enabled DVB-T. When saa7134   */
+		/* scan I2C devices it will not detect IF tda9887 and can`t*/
+		/* watch TV without software reboot. To solve this problem */
+		/* switch the tuner to analog TV mode manually.            */
+		if (dev->tuner_type == TUNER_PHILIPS_FMD1216ME_MK3) {
+			if (i2c_transfer(&dev->i2c_adap, &msg1, 1) != 1)
+				printk(KERN_WARNING "%s: Unable to enable IF of the tuner.\n", dev->name);
+		}
 		break;
 	}
 	case SAA7134_BOARD_PHILIPS_EUROPA:
@@ -7859,7 +7914,7 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 			/* Reconfigure board as Snake reference design */
 			dev->board = SAA7134_BOARD_PHILIPS_SNAKE;
 			dev->tuner_type = saa7134_boards[dev->board].tuner_type;
-			printk(KERN_INFO "%s: Reconfigured board as %s\n",
+			pr_info("%s: Reconfigured board as %s\n",
 				dev->name, saa7134_boards[dev->board].name);
 			break;
 		}
@@ -7887,7 +7942,7 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 		struct i2c_msg msg = {.addr=0x08, .flags=0, .buf=data, .len = sizeof(data)};
 		if (dev->autodetected && (dev->eedata[0x49] == 0x50)) {
 			dev->board = SAA7134_BOARD_PHILIPS_TIGER_S;
-			printk(KERN_INFO "%s: Reconfigured board as %s\n",
+			pr_info("%s: Reconfigured board as %s\n",
 				dev->name, saa7134_boards[dev->board].name);
 		}
 		if (dev->board == SAA7134_BOARD_PHILIPS_TIGER_S) {
@@ -7903,13 +7958,14 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 	case SAA7134_BOARD_ASUSTeK_TVFM7135:
 	/* The card below is detected as card=53, but is different */
 	       if (dev->autodetected && (dev->eedata[0x27] == 0x03)) {
-		       dev->board = SAA7134_BOARD_ASUSTeK_P7131_ANALOG;
-		       printk(KERN_INFO "%s: P7131 analog only, using "
-						       "entry of %s\n",
-		       dev->name, saa7134_boards[dev->board].name);
+			dev->board = SAA7134_BOARD_ASUSTeK_P7131_ANALOG;
+			pr_info("%s: P7131 analog only, using entry of %s\n",
+				dev->name, saa7134_boards[dev->board].name);
 
-			/* IR init has already happened for other cards, so
-			 * we have to catch up. */
+			/*
+			 * IR init has already happened for other cards, so
+			 * we have to catch up.
+			 */
 			dev->has_remote = SAA7134_REMOTE_GPIO;
 			saa7134_input_init1(dev);
 	       }
@@ -7972,12 +8028,12 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 		msg.addr = 0x0b;
 		msg.len = 1;
 		if (1 != i2c_transfer(&dev->i2c_adap, &msg, 1)) {
-			printk(KERN_WARNING "%s: send wake up byte to pic16C505"
+			pr_warn("%s: send wake up byte to pic16C505"
 					"(IR chip) failed\n", dev->name);
 		} else {
 			msg.flags = I2C_M_RD;
 			rc = i2c_transfer(&dev->i2c_adap, &msg, 1);
-			printk(KERN_INFO "%s: probe IR chip @ i2c 0x%02x: %s\n",
+			pr_info("%s: probe IR chip @ i2c 0x%02x: %s\n",
 				   dev->name, msg.addr,
 				   (1 == rc) ? "yes" : "no");
 			if (rc == 1)
@@ -8018,10 +8074,10 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 			dev->board = SAA7134_BOARD_VIDEOMATE_DVBT_200A;
 			dev->tuner_type   = saa7134_boards[dev->board].tuner_type;
 			dev->tda9887_conf = saa7134_boards[dev->board].tda9887_conf;
-			printk(KERN_INFO "%s: Reconfigured board as %s\n",
+			pr_info("%s: Reconfigured board as %s\n",
 				dev->name, saa7134_boards[dev->board].name);
 		} else {
-			printk(KERN_WARNING "%s: Unexpected tuner type info: %x in eeprom\n",
+			pr_warn("%s: Unexpected tuner type info: %x in eeprom\n",
 				dev->name, dev->eedata[0x41]);
 			break;
 		}
@@ -8043,9 +8099,8 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 			msg.buf = &buffer[i][0];
 			msg.len = ARRAY_SIZE(buffer[0]);
 			if (i2c_transfer(&dev->i2c_adap, &msg, 1) != 1)
-				printk(KERN_WARNING
-				       "%s: Unable to enable tuner(%i).\n",
-				       dev->name, i);
+				pr_warn("%s: Unable to enable tuner(%i).\n",
+					dev->name, i);
 		}
 		break;
 	}
@@ -8061,9 +8116,8 @@ int saa7134_board_init2(struct saa7134_dev *dev)
 		/* watch TV without software reboot. For solve this problem */
 		/* switch the tuner to analog TV mode manually.             */
 		if (i2c_transfer(&dev->i2c_adap, &msg, 1) != 1)
-				printk(KERN_WARNING
-				      "%s: Unable to enable IF of the tuner.\n",
-				       dev->name);
+			pr_warn("%s: Unable to enable IF of the tuner.\n",
+				dev->name);
 		break;
 	}
 	case SAA7134_BOARD_KWORLD_PCI_SBTVD_FULLSEG:
diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
index a349e964e0bc..72d7f992375e 100644
--- a/drivers/media/pci/saa7134/saa7134-core.c
+++ b/drivers/media/pci/saa7134/saa7134-core.c
@@ -20,6 +20,9 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
@@ -33,9 +36,6 @@
 #include <linux/dma-mapping.h>
 #include <linux/pm.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
-
 MODULE_DESCRIPTION("v4l2 driver module for saa7130/34 based TV cards");
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
 MODULE_LICENSE("GPL");
@@ -102,8 +102,15 @@ static unsigned int saa7134_devcount;
 int (*saa7134_dmasound_init)(struct saa7134_dev *dev);
 int (*saa7134_dmasound_exit)(struct saa7134_dev *dev);
 
-#define dprintk(fmt, arg...)	if (core_debug) \
-	printk(KERN_DEBUG "%s/core: " fmt, dev->name , ## arg)
+#define core_dbg(fmt, arg...) do { \
+	if (core_debug) \
+		printk(KERN_DEBUG pr_fmt("core: " fmt), ## arg); \
+	} while (0)
+
+#define irq_dbg(level, fmt, arg...)  do {\
+	if (irq_debug > level) \
+		printk(KERN_DEBUG pr_fmt("irq: " fmt), ## arg); \
+	} while (0)
 
 void saa7134_track_gpio(struct saa7134_dev *dev, char *msg)
 {
@@ -116,8 +123,7 @@ void saa7134_track_gpio(struct saa7134_dev *dev, char *msg)
 	saa_andorb(SAA7134_GPIO_GPMODE3,SAA7134_GPIO_GPRESCAN,SAA7134_GPIO_GPRESCAN);
 	mode   = saa_readl(SAA7134_GPIO_GPMODE0   >> 2) & 0xfffffff;
 	status = saa_readl(SAA7134_GPIO_GPSTATUS0 >> 2) & 0xfffffff;
-	printk(KERN_DEBUG
-	       "%s: gpio: mode=0x%07lx in=0x%07lx out=0x%07lx [%s]\n",
+	core_dbg("%s: gpio: mode=0x%07lx in=0x%07lx out=0x%07lx [%s]\n",
 	       dev->name, mode, (~mode) & status, mode & status, msg);
 }
 
@@ -128,7 +134,8 @@ void saa7134_set_gpio(struct saa7134_dev *dev, int bit_no, int value)
 	index = 1 << bit_no;
 	switch (value) {
 	case 0: /* static value */
-	case 1:	dprintk("setting GPIO%d to static %d\n", bit_no, value);
+	case 1:
+		core_dbg("setting GPIO%d to static %d\n", bit_no, value);
 		/* turn sync mode off if necessary */
 		if (index & 0x00c00000)
 			saa_andorb(SAA7134_VIDEO_PORT_CTRL6, 0x0f, 0x00);
@@ -140,7 +147,7 @@ void saa7134_set_gpio(struct saa7134_dev *dev, int bit_no, int value)
 		saa_andorl(SAA7134_GPIO_GPSTATUS0 >> 2, index, bitval);
 		break;
 	case 3:	/* tristate */
-		dprintk("setting GPIO%d to tristate\n", bit_no);
+		core_dbg("setting GPIO%d to tristate\n", bit_no);
 		saa_andorl(SAA7134_GPIO_GPMODE0 >> 2, index, 0);
 		break;
 	}
@@ -274,7 +281,7 @@ int saa7134_buffer_queue(struct saa7134_dev *dev,
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->slock, flags);
-	dprintk("buffer_queue %p\n", buf);
+	core_dbg("buffer_queue %p\n", buf);
 	if (NULL == q->curr) {
 		if (!q->need_two) {
 			q->curr = buf;
@@ -298,7 +305,7 @@ void saa7134_buffer_finish(struct saa7134_dev *dev,
 			   struct saa7134_dmaqueue *q,
 			   unsigned int state)
 {
-	dprintk("buffer_finish %p\n", q->curr);
+	core_dbg("buffer_finish %p\n", q->curr);
 
 	/* finish current buffer */
 	v4l2_get_timestamp(&q->curr->vb2.v4l2_buf.timestamp);
@@ -318,18 +325,18 @@ void saa7134_buffer_next(struct saa7134_dev *dev,
 	if (!list_empty(&q->queue)) {
 		/* activate next one from queue */
 		buf = list_entry(q->queue.next, struct saa7134_buf, entry);
-		dprintk("buffer_next %p [prev=%p/next=%p]\n",
+		core_dbg("buffer_next %p [prev=%p/next=%p]\n",
 			buf, q->queue.prev, q->queue.next);
 		list_del(&buf->entry);
 		if (!list_empty(&q->queue))
 			next = list_entry(q->queue.next, struct saa7134_buf, entry);
 		q->curr = buf;
 		buf->activate(dev, buf, next);
-		dprintk("buffer_next #2 prev=%p/next=%p\n",
+		core_dbg("buffer_next #2 prev=%p/next=%p\n",
 			q->queue.prev, q->queue.next);
 	} else {
 		/* nothing to do -- just stop DMA */
-		dprintk("buffer_next %p\n", NULL);
+		core_dbg("buffer_next %p\n", NULL);
 		saa7134_set_dmabits(dev);
 		del_timer(&q->timeout);
 	}
@@ -351,7 +358,7 @@ void saa7134_buffer_timeout(unsigned long data)
 	/* flag current buffer as failed,
 	   try to start over with the next one. */
 	if (q->curr) {
-		dprintk("timeout on %p\n", q->curr);
+		core_dbg("timeout on %p\n", q->curr);
 		saa7134_buffer_finish(dev, q, VB2_BUF_STATE_ERROR);
 	}
 	saa7134_buffer_next(dev, q);
@@ -474,7 +481,7 @@ int saa7134_set_dmabits(struct saa7134_dev *dev)
 		   SAA7134_MAIN_CTRL_TE5 |
 		   SAA7134_MAIN_CTRL_TE6,
 		   ctrl);
-	dprintk("dmabits: task=0x%02x ctrl=0x%02x irq=0x%x split=%s\n",
+	core_dbg("dmabits: task=0x%02x ctrl=0x%02x irq=0x%x split=%s\n",
 		task, ctrl, irq, split ? "no" : "yes");
 
 	return 0;
@@ -496,21 +503,21 @@ static void print_irqstatus(struct saa7134_dev *dev, int loop,
 {
 	unsigned int i;
 
-	printk(KERN_DEBUG "%s/irq[%d,%ld]: r=0x%lx s=0x%02lx",
-	       dev->name,loop,jiffies,report,status);
+	irq_dbg(1, "[%d,%ld]: r=0x%lx s=0x%02lx",
+		loop, jiffies, report, status);
 	for (i = 0; i < IRQBITS; i++) {
 		if (!(report & (1 << i)))
 			continue;
-		printk(" %s",irqbits[i]);
+		pr_cont(" %s", irqbits[i]);
 	}
 	if (report & SAA7134_IRQ_REPORT_DONE_RA0) {
-		printk(" | RA0=%s,%s,%s,%ld",
-		       (status & 0x40) ? "vbi"  : "video",
-		       (status & 0x20) ? "b"    : "a",
-		       (status & 0x10) ? "odd"  : "even",
-		       (status & 0x0f));
+		pr_cont(" | RA0=%s,%s,%s,%ld",
+			(status & 0x40) ? "vbi"  : "video",
+			(status & 0x20) ? "b"    : "a",
+			(status & 0x10) ? "odd"  : "even",
+			(status & 0x0f));
 	}
-	printk("\n");
+	pr_cont("\n");
 }
 
 static irqreturn_t saa7134_irq(int irq, void *dev_id)
@@ -532,16 +539,12 @@ static irqreturn_t saa7134_irq(int irq, void *dev_id)
 		if ((report & SAA7134_IRQ_REPORT_DONE_RA3) &&
 			(dev->dmasound.priv_data != NULL) )
 		{
-			if (irq_debug > 1)
-				printk(KERN_DEBUG "%s/irq: preserving DMA sound interrupt\n",
-				       dev->name);
+			irq_dbg(2, "preserving DMA sound interrupt\n");
 			report &= ~SAA7134_IRQ_REPORT_DONE_RA3;
 		}
 
 		if (0 == report) {
-			if (irq_debug > 1)
-				printk(KERN_DEBUG "%s/irq: no (more) work\n",
-				       dev->name);
+			irq_dbg(2, "no (more) work\n");
 			goto out;
 		}
 
@@ -614,24 +617,24 @@ static irqreturn_t saa7134_irq(int irq, void *dev_id)
 		print_irqstatus(dev,loop,report,status);
 		if (report & SAA7134_IRQ_REPORT_PE) {
 			/* disable all parity error */
-			printk(KERN_WARNING "%s/irq: looping -- "
+			pr_warn("%s/irq: looping -- "
 			       "clearing PE (parity error!) enable bit\n",dev->name);
 			saa_clearl(SAA7134_IRQ2,SAA7134_IRQ2_INTE_PE);
 		} else if (report & SAA7134_IRQ_REPORT_GPIO16) {
 			/* disable gpio16 IRQ */
-			printk(KERN_WARNING "%s/irq: looping -- "
+			pr_warn("%s/irq: looping -- "
 			       "clearing GPIO16 enable bit\n",dev->name);
 			saa_clearl(SAA7134_IRQ2, SAA7134_IRQ2_INTE_GPIO16_P);
 			saa_clearl(SAA7134_IRQ2, SAA7134_IRQ2_INTE_GPIO16_N);
 		} else if (report & SAA7134_IRQ_REPORT_GPIO18) {
 			/* disable gpio18 IRQs */
-			printk(KERN_WARNING "%s/irq: looping -- "
+			pr_warn("%s/irq: looping -- "
 			       "clearing GPIO18 enable bit\n",dev->name);
 			saa_clearl(SAA7134_IRQ2, SAA7134_IRQ2_INTE_GPIO18_P);
 			saa_clearl(SAA7134_IRQ2, SAA7134_IRQ2_INTE_GPIO18_N);
 		} else {
 			/* disable all irqs */
-			printk(KERN_WARNING "%s/irq: looping -- "
+			pr_warn("%s/irq: looping -- "
 			       "clearing all enable bits\n",dev->name);
 			saa_writel(SAA7134_IRQ1,0);
 			saa_writel(SAA7134_IRQ2,0);
@@ -680,7 +683,7 @@ static int saa7134_hw_enable1(struct saa7134_dev *dev)
 
 static int saa7134_hwinit1(struct saa7134_dev *dev)
 {
-	dprintk("hwinit1\n");
+	core_dbg("hwinit1\n");
 
 	saa_writel(SAA7134_IRQ1, 0);
 	saa_writel(SAA7134_IRQ2, 0);
@@ -742,7 +745,7 @@ static int saa7134_hw_enable2(struct saa7134_dev *dev)
 static int saa7134_hwinit2(struct saa7134_dev *dev)
 {
 
-	dprintk("hwinit2\n");
+	core_dbg("hwinit2\n");
 
 	saa7134_video_init2(dev);
 	saa7134_tvaudio_init2(dev);
@@ -756,7 +759,7 @@ static int saa7134_hwinit2(struct saa7134_dev *dev)
 /* shutdown */
 static int saa7134_hwfini(struct saa7134_dev *dev)
 {
-	dprintk("hwfini\n");
+	core_dbg("hwfini\n");
 
 	if (card_has_mpeg(dev))
 		saa7134_ts_fini(dev);
@@ -772,34 +775,32 @@ static void must_configure_manually(int has_eeprom)
 	unsigned int i,p;
 
 	if (!has_eeprom)
-		printk(KERN_WARNING
-		       "saa7134: <rant>\n"
-		       "saa7134:  Congratulations!  Your TV card vendor saved a few\n"
-		       "saa7134:  cents for a eeprom, thus your pci board has no\n"
-		       "saa7134:  subsystem ID and I can't identify it automatically\n"
-		       "saa7134: </rant>\n"
-		       "saa7134: I feel better now.  Ok, here are the good news:\n"
-		       "saa7134: You can use the card=<nr> insmod option to specify\n"
-		       "saa7134: which board do you have.  The list:\n");
+		pr_warn("saa7134: <rant>\n"
+			"saa7134:  Congratulations!  Your TV card vendor saved a few\n"
+			"saa7134:  cents for a eeprom, thus your pci board has no\n"
+			"saa7134:  subsystem ID and I can't identify it automatically\n"
+			"saa7134: </rant>\n"
+			"saa7134: I feel better now.  Ok, here are the good news:\n"
+			"saa7134: You can use the card=<nr> insmod option to specify\n"
+			"saa7134: which board do you have.  The list:\n");
 	else
-		printk(KERN_WARNING
-		       "saa7134: Board is currently unknown. You might try to use the card=<nr>\n"
-		       "saa7134: insmod option to specify which board do you have, but this is\n"
-		       "saa7134: somewhat risky, as might damage your card. It is better to ask\n"
-		       "saa7134: for support at linux-media@vger.kernel.org.\n"
-		       "saa7134: The supported cards are:\n");
+		pr_warn("saa7134: Board is currently unknown. You might try to use the card=<nr>\n"
+			"saa7134: insmod option to specify which board do you have, but this is\n"
+			"saa7134: somewhat risky, as might damage your card. It is better to ask\n"
+			"saa7134: for support at linux-media@vger.kernel.org.\n"
+			"saa7134: The supported cards are:\n");
 
 	for (i = 0; i < saa7134_bcount; i++) {
-		printk(KERN_WARNING "saa7134:   card=%d -> %-40.40s",
+		pr_warn("saa7134:   card=%d -> %-40.40s",
 		       i,saa7134_boards[i].name);
 		for (p = 0; saa7134_pci_tbl[p].driver_data; p++) {
 			if (saa7134_pci_tbl[p].driver_data != i)
 				continue;
-			printk(" %04x:%04x",
+			pr_cont(" %04x:%04x",
 			       saa7134_pci_tbl[p].subvendor,
 			       saa7134_pci_tbl[p].subdevice);
 		}
-		printk("\n");
+		pr_cont("\n");
 	}
 }
 
@@ -903,31 +904,31 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 	/* pci quirks */
 	if (pci_pci_problems) {
 		if (pci_pci_problems & PCIPCI_TRITON)
-			printk(KERN_INFO "%s: quirk: PCIPCI_TRITON\n", dev->name);
+			pr_info("%s: quirk: PCIPCI_TRITON\n", dev->name);
 		if (pci_pci_problems & PCIPCI_NATOMA)
-			printk(KERN_INFO "%s: quirk: PCIPCI_NATOMA\n", dev->name);
+			pr_info("%s: quirk: PCIPCI_NATOMA\n", dev->name);
 		if (pci_pci_problems & PCIPCI_VIAETBF)
-			printk(KERN_INFO "%s: quirk: PCIPCI_VIAETBF\n", dev->name);
+			pr_info("%s: quirk: PCIPCI_VIAETBF\n", dev->name);
 		if (pci_pci_problems & PCIPCI_VSFX)
-			printk(KERN_INFO "%s: quirk: PCIPCI_VSFX\n",dev->name);
+			pr_info("%s: quirk: PCIPCI_VSFX\n", dev->name);
 #ifdef PCIPCI_ALIMAGIK
 		if (pci_pci_problems & PCIPCI_ALIMAGIK) {
-			printk(KERN_INFO "%s: quirk: PCIPCI_ALIMAGIK -- latency fixup\n",
+			pr_info("%s: quirk: PCIPCI_ALIMAGIK -- latency fixup\n",
 			       dev->name);
 			latency = 0x0A;
 		}
 #endif
 		if (pci_pci_problems & (PCIPCI_FAIL|PCIAGP_FAIL)) {
-			printk(KERN_INFO "%s: quirk: this driver and your "
+			pr_info("%s: quirk: this driver and your "
 					"chipset may not work together"
 					" in overlay mode.\n",dev->name);
 			if (!saa7134_no_overlay) {
-				printk(KERN_INFO "%s: quirk: overlay "
+				pr_info("%s: quirk: overlay "
 						"mode will be disabled.\n",
 						dev->name);
 				saa7134_no_overlay = 1;
 			} else {
-				printk(KERN_INFO "%s: quirk: overlay "
+				pr_info("%s: quirk: overlay "
 						"mode will be forced. Use this"
 						" option at your own risk.\n",
 						dev->name);
@@ -935,7 +936,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 		}
 	}
 	if (UNSET != latency) {
-		printk(KERN_INFO "%s: setting pci latency timer to %d\n",
+		pr_info("%s: setting pci latency timer to %d\n",
 		       dev->name,latency);
 		pci_write_config_byte(pci_dev, PCI_LATENCY_TIMER, latency);
 	}
@@ -943,13 +944,13 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 	/* print pci info */
 	dev->pci_rev = pci_dev->revision;
 	pci_read_config_byte(pci_dev, PCI_LATENCY_TIMER,  &dev->pci_lat);
-	printk(KERN_INFO "%s: found at %s, rev: %d, irq: %d, "
+	pr_info("%s: found at %s, rev: %d, irq: %d, "
 	       "latency: %d, mmio: 0x%llx\n", dev->name,
 	       pci_name(pci_dev), dev->pci_rev, pci_dev->irq,
 	       dev->pci_lat,(unsigned long long)pci_resource_start(pci_dev,0));
 	pci_set_master(pci_dev);
 	if (!pci_dma_supported(pci_dev, DMA_BIT_MASK(32))) {
-		printk("%s: Oops: no 32bit PCI DMA ???\n",dev->name);
+		pr_warn("%s: Oops: no 32bit PCI DMA ???\n", dev->name);
 		err = -EIO;
 		goto fail1;
 	}
@@ -972,7 +973,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 	dev->tda9887_conf = saa7134_boards[dev->board].tda9887_conf;
 	if (UNSET != tuner[dev->nr])
 		dev->tuner_type = tuner[dev->nr];
-	printk(KERN_INFO "%s: subsystem: %04x:%04x, board: %s [card=%d,%s]\n",
+	pr_info("%s: subsystem: %04x:%04x, board: %s [card=%d,%s]\n",
 		dev->name,pci_dev->subsystem_vendor,
 		pci_dev->subsystem_device,saa7134_boards[dev->board].name,
 		dev->board, dev->autodetected ?
@@ -983,7 +984,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 				pci_resource_len(pci_dev,0),
 				dev->name)) {
 		err = -EBUSY;
-		printk(KERN_ERR "%s: can't get MMIO memory @ 0x%llx\n",
+		pr_err("%s: can't get MMIO memory @ 0x%llx\n",
 		       dev->name,(unsigned long long)pci_resource_start(pci_dev,0));
 		goto fail1;
 	}
@@ -992,7 +993,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 	dev->bmmio = (__u8 __iomem *)dev->lmmio;
 	if (NULL == dev->lmmio) {
 		err = -EIO;
-		printk(KERN_ERR "%s: can't ioremap() MMIO memory\n",
+		pr_err("%s: can't ioremap() MMIO memory\n",
 		       dev->name);
 		goto fail2;
 	}
@@ -1010,7 +1011,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 	err = request_irq(pci_dev->irq, saa7134_irq,
 			  IRQF_SHARED, dev->name, dev);
 	if (err < 0) {
-		printk(KERN_ERR "%s: can't get IRQ %d\n",
+		pr_err("%s: can't get IRQ %d\n",
 		       dev->name,pci_dev->irq);
 		goto fail4;
 	}
@@ -1040,7 +1041,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 				&dev->i2c_adap, "saa6588",
 				0, I2C_ADDRS(saa7134_boards[dev->board].rds_addr));
 		if (sd) {
-			printk(KERN_INFO "%s: found RDS decoder\n", dev->name);
+			pr_info("%s: found RDS decoder\n", dev->name);
 			dev->has_rds = 1;
 		}
 	}
@@ -1059,7 +1060,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 
 	/* register v4l devices */
 	if (saa7134_no_overlay > 0)
-		printk(KERN_INFO "%s: Overlay support disabled.\n", dev->name);
+		pr_info("%s: Overlay support disabled.\n", dev->name);
 
 	dev->video_dev = vdev_init(dev,&saa7134_video_template,"video");
 	dev->video_dev->ctrl_handler = &dev->ctrl_handler;
@@ -1068,11 +1069,11 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 	err = video_register_device(dev->video_dev,VFL_TYPE_GRABBER,
 				    video_nr[dev->nr]);
 	if (err < 0) {
-		printk(KERN_INFO "%s: can't register video device\n",
+		pr_info("%s: can't register video device\n",
 		       dev->name);
 		goto fail5;
 	}
-	printk(KERN_INFO "%s: registered device %s [v4l2]\n",
+	pr_info("%s: registered device %s [v4l2]\n",
 	       dev->name, video_device_node_name(dev->video_dev));
 
 	dev->vbi_dev = vdev_init(dev, &saa7134_video_template, "vbi");
@@ -1084,7 +1085,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 				    vbi_nr[dev->nr]);
 	if (err < 0)
 		goto fail5;
-	printk(KERN_INFO "%s: registered device %s\n",
+	pr_info("%s: registered device %s\n",
 	       dev->name, video_device_node_name(dev->vbi_dev));
 
 	if (card_has_radio(dev)) {
@@ -1095,7 +1096,7 @@ static int saa7134_initdev(struct pci_dev *pci_dev,
 					    radio_nr[dev->nr]);
 		if (err < 0)
 			goto fail5;
-		printk(KERN_INFO "%s: registered device %s\n",
+		pr_info("%s: registered device %s\n",
 		       dev->name, video_device_node_name(dev->radio_dev));
 	}
 
@@ -1204,12 +1205,12 @@ static int saa7134_buffer_requeue(struct saa7134_dev *dev,
 
 	buf  = q->curr;
 	next = buf;
-	dprintk("buffer_requeue\n");
+	core_dbg("buffer_requeue\n");
 
 	if (!buf)
 		return 0;
 
-	dprintk("buffer_requeue : resending active buffers \n");
+	core_dbg("buffer_requeue : resending active buffer\n");
 
 	if (!list_empty(&q->queue))
 		next = list_entry(q->queue.next, struct saa7134_buf,
@@ -1358,7 +1359,7 @@ static struct pci_driver saa7134_pci_driver = {
 static int __init saa7134_init(void)
 {
 	INIT_LIST_HEAD(&saa7134_devlist);
-	printk(KERN_INFO "saa7130/34: v4l2 driver version %s loaded\n",
+	pr_info("saa7130/34: v4l2 driver version %s loaded\n",
 	       SAA7134_VERSION);
 	return pci_register_driver(&saa7134_pci_driver);
 }
diff --git a/drivers/media/pci/saa7134/saa7134-dvb.c b/drivers/media/pci/saa7134/saa7134-dvb.c
index 73ffbabf831c..101ba8729416 100644
--- a/drivers/media/pci/saa7134/saa7134-dvb.c
+++ b/drivers/media/pci/saa7134/saa7134-dvb.c
@@ -20,6 +20,9 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
@@ -28,8 +31,6 @@
 #include <linux/kthread.h>
 #include <linux/suspend.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
 #include <media/v4l2-common.h>
 #include "dvb-pll.h"
 #include <dvb_frontend.h>
@@ -75,19 +76,8 @@ static int use_frontend;
 module_param(use_frontend, int, 0644);
 MODULE_PARM_DESC(use_frontend,"for cards with multiple frontends (0: terrestrial, 1: satellite)");
 
-static int debug;
-module_param(debug, int, 0644);
-MODULE_PARM_DESC(debug, "Turn on/off module debugging (default:off).");
-
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
-#define dprintk(fmt, arg...)	do { if (debug) \
-	printk(KERN_DEBUG "%s/dvb: " fmt, dev->name , ## arg); } while(0)
-
-/* Print a warning */
-#define wprintk(fmt, arg...) \
-	printk(KERN_WARNING "%s/dvb: " fmt, dev->name, ## arg)
-
 /* ------------------------------------------------------------------
  * mt352 based DVB-T cards
  */
@@ -112,7 +102,7 @@ static int pinnacle_antenna_pwr(struct saa7134_dev *dev, int on)
 	saa_setl(SAA7134_GPIO_GPSTATUS0 >> 2,   (1 << 28));
 	udelay(10);
 	ok = saa_readl(SAA7134_GPIO_GPSTATUS0) & (1 << 27);
-	dprintk("%s %s\n", __func__, ok ? "on" : "off");
+	pr_debug("%s %s\n", __func__, ok ? "on" : "off");
 
 	if (!ok)
 		saa_clearl(SAA7134_GPIO_GPSTATUS0 >> 2,   (1 << 26));
@@ -130,9 +120,8 @@ static int mt352_pinnacle_init(struct dvb_frontend* fe)
 	static u8 gpp_ctl_cfg []   = { GPP_CTL,    0x0f };
 	static u8 scan_ctl_cfg []  = { SCAN_CTL,   0x0d };
 	static u8 irq_cfg []       = { INTERRUPT_EN_0, 0x00, 0x00, 0x00, 0x00 };
-	struct saa7134_dev *dev= fe->dvb->priv;
 
-	dprintk("%s called\n", __func__);
+	pr_debug("%s called\n", __func__);
 
 	mt352_write(fe, clock_config,   sizeof(clock_config));
 	udelay(200);
@@ -258,7 +247,7 @@ static int kworld_sbtvd_gate_ctrl(struct dvb_frontend* fe, int enable)
 	struct i2c_msg msg = {.addr = 0x4b, .flags = 0, .buf = initmsg, .len = 2};
 
 	if (i2c_transfer(&dev->i2c_adap, &msg, 1) != 1) {
-		wprintk("could not access the I2C gate\n");
+		pr_warn("could not access the I2C gate\n");
 		return -EIO;
 	}
 	if (enable)
@@ -266,7 +255,7 @@ static int kworld_sbtvd_gate_ctrl(struct dvb_frontend* fe, int enable)
 	else
 		msg.buf = msg_disable;
 	if (i2c_transfer(&dev->i2c_adap, &msg, 1) != 1) {
-		wprintk("could not access the I2C gate\n");
+		pr_warn("could not access the I2C gate\n");
 		return -EIO;
 	}
 	msleep(20);
@@ -369,7 +358,7 @@ static int philips_tda6651_pll_set(struct dvb_frontend *fe)
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
 	if (i2c_transfer(&dev->i2c_adap, &tuner_msg, 1) != 1) {
-		wprintk("could not write to tuner at addr: 0x%02x\n",
+		pr_warn("could not write to tuner at addr: 0x%02x\n",
 			addr << 1);
 		return -EIO;
 	}
@@ -556,8 +545,7 @@ static int tda8290_i2c_gate_ctrl( struct dvb_frontend* fe, int enable)
 		tda8290_msg.buf = tda8290_open;
 	}
 	if (i2c_transfer(state->i2c, &tda8290_msg, 1) != 1) {
-		struct saa7134_dev *dev = fe->dvb->priv;
-		wprintk("could not access tda8290 I2C gate\n");
+		pr_warn("could not access tda8290 I2C gate\n");
 		return -EIO;
 	}
 	msleep(20);
@@ -570,11 +558,14 @@ static int philips_tda827x_tuner_init(struct dvb_frontend *fe)
 	struct tda1004x_state *state = fe->demodulator_priv;
 
 	switch (state->config->antenna_switch) {
-	case 0: break;
-	case 1:	dprintk("setting GPIO21 to 0 (TV antenna?)\n");
+	case 0:
+		break;
+	case 1:
+		pr_debug("setting GPIO21 to 0 (TV antenna?)\n");
 		saa7134_set_gpio(dev, 21, 0);
 		break;
-	case 2: dprintk("setting GPIO21 to 1 (Radio antenna?)\n");
+	case 2:
+		pr_debug("setting GPIO21 to 1 (Radio antenna?)\n");
 		saa7134_set_gpio(dev, 21, 1);
 		break;
 	}
@@ -587,11 +578,14 @@ static int philips_tda827x_tuner_sleep(struct dvb_frontend *fe)
 	struct tda1004x_state *state = fe->demodulator_priv;
 
 	switch (state->config->antenna_switch) {
-	case 0: break;
-	case 1: dprintk("setting GPIO21 to 1 (Radio antenna?)\n");
+	case 0:
+		break;
+	case 1:
+		pr_debug("setting GPIO21 to 1 (Radio antenna?)\n");
 		saa7134_set_gpio(dev, 21, 1);
 		break;
-	case 2:	dprintk("setting GPIO21 to 0 (TV antenna?)\n");
+	case 2:
+		pr_debug("setting GPIO21 to 0 (TV antenna?)\n");
 		saa7134_set_gpio(dev, 21, 0);
 		break;
 	}
@@ -619,7 +613,7 @@ static int configure_tda827x_fe(struct saa7134_dev *dev,
 			       &dev->i2c_adap, tuner_conf))
 			return 0;
 
-		wprintk("no tda827x tuner found at addr: %02x\n",
+		pr_warn("no tda827x tuner found at addr: %02x\n",
 				cdec_conf->tuner_address);
 	}
 	return -EINVAL;
@@ -993,7 +987,8 @@ static struct tda10086_config sd1878_4m = {
  * special case: lnb supply is connected to the gated i2c
  */
 
-static int md8800_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int md8800_set_voltage(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage)
 {
 	int res = -EIO;
 	struct saa7134_dev *dev = fe->dvb->priv;
@@ -1019,7 +1014,8 @@ static int md8800_set_high_voltage(struct dvb_frontend *fe, long arg)
 	return res;
 };
 
-static int md8800_set_voltage2(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int md8800_set_voltage2(struct dvb_frontend *fe,
+			       enum fe_sec_voltage voltage)
 {
 	struct saa7134_dev *dev = fe->dvb->priv;
 	u8 wbuf[2] = { 0x1f, 00 };
@@ -1041,8 +1037,8 @@ static int md8800_set_voltage2(struct dvb_frontend *fe, fe_sec_voltage_t voltage
 
 static int md8800_set_high_voltage2(struct dvb_frontend *fe, long arg)
 {
-	struct saa7134_dev *dev = fe->dvb->priv;
-	wprintk("%s: sorry can't set high LNB supply voltage from here\n", __func__);
+	pr_warn("%s: sorry can't set high LNB supply voltage from here\n",
+		__func__);
 	return -EIO;
 }
 
@@ -1222,10 +1218,10 @@ static int dvb_init(struct saa7134_dev *dev)
 	mutex_init(&dev->frontends.lock);
 	INIT_LIST_HEAD(&dev->frontends.felist);
 
-	printk(KERN_INFO "%s() allocating 1 frontend\n", __func__);
+	pr_info("%s() allocating 1 frontend\n", __func__);
 	fe0 = vb2_dvb_alloc_frontend(&dev->frontends, 1);
 	if (!fe0) {
-		printk(KERN_ERR "%s() failed to alloc\n", __func__);
+		pr_err("%s() failed to alloc\n", __func__);
 		return -ENOMEM;
 	}
 
@@ -1250,7 +1246,7 @@ static int dvb_init(struct saa7134_dev *dev)
 
 	switch (dev->board) {
 	case SAA7134_BOARD_PINNACLE_300I_DVBT_PAL:
-		dprintk("pinnacle 300i dvb setup\n");
+		pr_debug("pinnacle 300i dvb setup\n");
 		fe0->dvb.frontend = dvb_attach(mt352_attach, &pinnacle_300i,
 					       &dev->i2c_adap);
 		if (fe0->dvb.frontend) {
@@ -1259,7 +1255,7 @@ static int dvb_init(struct saa7134_dev *dev)
 		break;
 	case SAA7134_BOARD_AVERMEDIA_777:
 	case SAA7134_BOARD_AVERMEDIA_A16AR:
-		dprintk("avertv 777 dvb setup\n");
+		pr_debug("avertv 777 dvb setup\n");
 		fe0->dvb.frontend = dvb_attach(mt352_attach, &avermedia_777,
 					       &dev->i2c_adap);
 		if (fe0->dvb.frontend) {
@@ -1269,7 +1265,7 @@ static int dvb_init(struct saa7134_dev *dev)
 		}
 		break;
 	case SAA7134_BOARD_AVERMEDIA_A16D:
-		dprintk("AverMedia A16D dvb setup\n");
+		pr_debug("AverMedia A16D dvb setup\n");
 		fe0->dvb.frontend = dvb_attach(mt352_attach,
 						&avermedia_xc3028_mt352_dev,
 						&dev->i2c_adap);
@@ -1401,13 +1397,15 @@ static int dvb_init(struct saa7134_dev *dev)
 			if (fe0->dvb.frontend) {
 				if (dvb_attach(tda826x_attach, fe0->dvb.frontend, 0x63,
 									&dev->i2c_adap, 0) == NULL) {
-					wprintk("%s: Lifeview Trio, No tda826x found!\n", __func__);
+					pr_warn("%s: Lifeview Trio, No tda826x found!\n",
+						__func__);
 					goto detach_frontend;
 				}
 				if (dvb_attach(isl6421_attach, fe0->dvb.frontend,
 					       &dev->i2c_adap,
 					       0x08, 0, 0, false) == NULL) {
-					wprintk("%s: Lifeview Trio, No ISL6421 found!\n", __func__);
+					pr_warn("%s: Lifeview Trio, No ISL6421 found!\n",
+						__func__);
 					goto detach_frontend;
 				}
 			}
@@ -1422,12 +1420,12 @@ static int dvb_init(struct saa7134_dev *dev)
 			if (dvb_attach(tda827x_attach,fe0->dvb.frontend,
 				   ads_tech_duo_config.tuner_address, &dev->i2c_adap,
 								&ads_duo_cfg) == NULL) {
-				wprintk("no tda827x tuner found at addr: %02x\n",
+				pr_warn("no tda827x tuner found at addr: %02x\n",
 					ads_tech_duo_config.tuner_address);
 				goto detach_frontend;
 			}
 		} else
-			wprintk("failed to attach tda10046\n");
+			pr_warn("failed to attach tda10046\n");
 		break;
 	case SAA7134_BOARD_TEVION_DVBT_220RF:
 		if (configure_tda827x_fe(dev, &tevion_dvbt220rf_config,
@@ -1450,7 +1448,7 @@ static int dvb_init(struct saa7134_dev *dev)
 
 				if (dvb_attach(tda826x_attach, fe0->dvb.frontend,
 						0x60, &dev->i2c_adap, 0) == NULL) {
-					wprintk("%s: Medion Quadro, no tda826x "
+					pr_warn("%s: Medion Quadro, no tda826x "
 						"found !\n", __func__);
 					goto detach_frontend;
 				}
@@ -1459,7 +1457,7 @@ static int dvb_init(struct saa7134_dev *dev)
 					fe->ops.i2c_gate_ctrl(fe, 1);
 					if (dvb_attach(isl6405_attach, fe,
 							&dev->i2c_adap, 0x08, 0, 0) == NULL) {
-						wprintk("%s: Medion Quadro, no ISL6405 "
+						pr_warn("%s: Medion Quadro, no ISL6405 "
 							"found !\n", __func__);
 						goto detach_frontend;
 					}
@@ -1519,13 +1517,13 @@ static int dvb_init(struct saa7134_dev *dev)
 		if (fe0->dvb.frontend) {
 			if (dvb_attach(tda826x_attach, fe0->dvb.frontend, 0x60,
 				       &dev->i2c_adap, 0) == NULL) {
-				wprintk("%s: No tda826x found!\n", __func__);
+				pr_warn("%s: No tda826x found!\n", __func__);
 				goto detach_frontend;
 			}
 			if (dvb_attach(isl6421_attach, fe0->dvb.frontend,
 				       &dev->i2c_adap,
 				       0x08, 0, 0, false) == NULL) {
-				wprintk("%s: No ISL6421 found!\n", __func__);
+				pr_warn("%s: No ISL6421 found!\n", __func__);
 				goto detach_frontend;
 			}
 		}
@@ -1593,12 +1591,12 @@ static int dvb_init(struct saa7134_dev *dev)
 		if (fe0->dvb.frontend) {
 			if (dvb_attach(tda826x_attach, fe0->dvb.frontend, 0x60,
 					&dev->i2c_adap, 0) == NULL) {
-				wprintk("%s: No tda826x found!\n", __func__);
+				pr_warn("%s: No tda826x found!\n", __func__);
 				goto detach_frontend;
 			}
 			if (dvb_attach(lnbp21_attach, fe0->dvb.frontend,
 					&dev->i2c_adap, 0, 0) == NULL) {
-				wprintk("%s: No lnbp21 found!\n", __func__);
+				pr_warn("%s: No lnbp21 found!\n", __func__);
 				goto detach_frontend;
 			}
 		}
@@ -1614,7 +1612,7 @@ static int dvb_init(struct saa7134_dev *dev)
 			goto detach_frontend;
 		break;
 	case SAA7134_BOARD_AVERMEDIA_CARDBUS_506:
-		dprintk("AverMedia E506R dvb setup\n");
+		pr_debug("AverMedia E506R dvb setup\n");
 		saa7134_set_gpio(dev, 25, 0);
 		msleep(10);
 		saa7134_set_gpio(dev, 25, 1);
@@ -1630,7 +1628,7 @@ static int dvb_init(struct saa7134_dev *dev)
 			struct dvb_frontend *fe;
 			if (dvb_attach(dvb_pll_attach, fe0->dvb.frontend, 0x60,
 				  &dev->i2c_adap, DVB_PLL_PHILIPS_SD1878_TDA8261) == NULL) {
-				wprintk("%s: MD7134 DVB-S, no SD1878 "
+				pr_warn("%s: MD7134 DVB-S, no SD1878 "
 					"found !\n", __func__);
 				goto detach_frontend;
 			}
@@ -1639,7 +1637,7 @@ static int dvb_init(struct saa7134_dev *dev)
 			fe->ops.i2c_gate_ctrl(fe, 1);
 			if (dvb_attach(isl6405_attach, fe,
 					&dev->i2c_adap, 0x08, 0, 0) == NULL) {
-				wprintk("%s: MD7134 DVB-S, no ISL6405 "
+				pr_warn("%s: MD7134 DVB-S, no ISL6405 "
 					"found !\n", __func__);
 				goto detach_frontend;
 			}
@@ -1671,15 +1669,15 @@ static int dvb_init(struct saa7134_dev *dev)
 				if (dvb_attach(tda826x_attach,
 						fe0->dvb.frontend, 0x60,
 						&dev->i2c_adap, 0) == NULL) {
-					wprintk("%s: Asus Tiger 3in1, no "
+					pr_warn("%s: Asus Tiger 3in1, no "
 						"tda826x found!\n", __func__);
 					goto detach_frontend;
 				}
 				if (dvb_attach(lnbp21_attach, fe0->dvb.frontend,
 						&dev->i2c_adap, 0, 0) == NULL) {
-					wprintk("%s: Asus Tiger 3in1, no lnbp21"
+					pr_warn("%s: Asus Tiger 3in1, no lnbp21"
 						" found!\n", __func__);
-				       goto detach_frontend;
+					goto detach_frontend;
 			       }
 		       }
 	       }
@@ -1696,13 +1694,13 @@ static int dvb_init(struct saa7134_dev *dev)
 				if (dvb_attach(tda826x_attach,
 					       fe0->dvb.frontend, 0x60,
 					       &dev->i2c_adap, 0) == NULL) {
-					wprintk("%s: Asus My Cinema PS3-100, no "
+					pr_warn("%s: Asus My Cinema PS3-100, no "
 						"tda826x found!\n", __func__);
 					goto detach_frontend;
 				}
 				if (dvb_attach(lnbp21_attach, fe0->dvb.frontend,
 					       &dev->i2c_adap, 0, 0) == NULL) {
-					wprintk("%s: Asus My Cinema PS3-100, no lnbp21"
+					pr_warn("%s: Asus My Cinema PS3-100, no lnbp21"
 						" found!\n", __func__);
 					goto detach_frontend;
 				}
@@ -1750,7 +1748,7 @@ static int dvb_init(struct saa7134_dev *dev)
 		if (fe0->dvb.frontend) {
 			if (dvb_attach(zl10036_attach, fe0->dvb.frontend,
 					&avertv_a700_tuner, &dev->i2c_adap) == NULL) {
-				wprintk("%s: No zl10036 found!\n",
+				pr_warn("%s: No zl10036 found!\n",
 					__func__);
 			}
 		}
@@ -1761,7 +1759,7 @@ static int dvb_init(struct saa7134_dev *dev)
 		if (fe0->dvb.frontend)
 			if (dvb_attach(zl10039_attach, fe0->dvb.frontend,
 					0x60, &dev->i2c_adap) == NULL)
-				wprintk("%s: No zl10039 found!\n",
+				pr_warn("%s: No zl10039 found!\n",
 					__func__);
 
 		break;
@@ -1774,7 +1772,7 @@ static int dvb_init(struct saa7134_dev *dev)
 					fe0->dvb.frontend,
 					&dev->i2c_adap,
 					&videomate_t750_qt1010_config) == NULL)
-				wprintk("error attaching QT1010\n");
+				pr_warn("error attaching QT1010\n");
 		}
 		break;
 	case SAA7134_BOARD_ZOLID_HYBRID_PCI:
@@ -1850,12 +1848,12 @@ static int dvb_init(struct saa7134_dev *dev)
 			fe0->dvb.frontend->ops.i2c_gate_ctrl = NULL;
 			if (dvb_attach(zl10039_attach, fe0->dvb.frontend,
 					0x60, &dev->i2c_adap) == NULL)
-				wprintk("%s: No zl10039 found!\n",
+				pr_warn("%s: No zl10039 found!\n",
 					__func__);
 		}
 		break;
 	default:
-		wprintk("Huh? unknown DVB card?\n");
+		pr_warn("Huh? unknown DVB card?\n");
 		break;
 	}
 
@@ -1871,14 +1869,14 @@ static int dvb_init(struct saa7134_dev *dev)
 
 		fe = dvb_attach(xc2028_attach, fe0->dvb.frontend, &cfg);
 		if (!fe) {
-			printk(KERN_ERR "%s/2: xc3028 attach failed\n",
+			pr_err("%s/2: xc3028 attach failed\n",
 			       dev->name);
 			goto detach_frontend;
 		}
 	}
 
 	if (NULL == fe0->dvb.frontend) {
-		printk(KERN_ERR "%s/dvb: frontend initialization failed\n", dev->name);
+		pr_err("%s/dvb: frontend initialization failed\n", dev->name);
 		goto detach_frontend;
 	}
 	/* define general-purpose callback pointer */
diff --git a/drivers/media/pci/saa7134/saa7134-empress.c b/drivers/media/pci/saa7134/saa7134-empress.c
index 594dc3ad4750..56b932c97196 100644
--- a/drivers/media/pci/saa7134/saa7134-empress.c
+++ b/drivers/media/pci/saa7134/saa7134-empress.c
@@ -17,6 +17,9 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
@@ -26,9 +29,6 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-event.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
-
 /* ------------------------------------------------------------------ */
 
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
@@ -39,13 +39,6 @@ static unsigned int empress_nr[] = {[0 ... (SAA7134_MAXBOARDS - 1)] = UNSET };
 module_param_array(empress_nr, int, NULL, 0444);
 MODULE_PARM_DESC(empress_nr,"ts device number");
 
-static unsigned int debug;
-module_param(debug, int, 0644);
-MODULE_PARM_DESC(debug,"enable debug messages");
-
-#define dprintk(fmt, arg...)	if (debug)			\
-	printk(KERN_DEBUG "%s/empress: " fmt, dev->name , ## arg)
-
 /* ------------------------------------------------------------------ */
 
 static int start_streaming(struct vb2_queue *vq, unsigned int count)
@@ -121,11 +114,14 @@ static int empress_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
 	struct saa7134_dev *dev = video_drvdata(file);
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt.format;
 
-	saa_call_all(dev, video, g_mbus_fmt, &mbus_fmt);
+	saa_call_all(dev, pad, get_fmt, NULL, &fmt);
 
-	v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
+	v4l2_fill_pix_format(&f->fmt.pix, mbus_fmt);
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.sizeimage    = TS_PACKET_SIZE * dev->ts.nr_packets;
 	f->fmt.pix.bytesperline = 0;
@@ -137,11 +133,13 @@ static int empress_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
 	struct saa7134_dev *dev = video_drvdata(file);
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 
-	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, MEDIA_BUS_FMT_FIXED);
-	saa_call_all(dev, video, s_mbus_fmt, &mbus_fmt);
-	v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
+	v4l2_fill_mbus_format(&format.format, &f->fmt.pix, MEDIA_BUS_FMT_FIXED);
+	saa_call_all(dev, pad, set_fmt, NULL, &format);
+	v4l2_fill_pix_format(&f->fmt.pix, &format.format);
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.sizeimage    = TS_PACKET_SIZE * dev->ts.nr_packets;
@@ -154,11 +152,14 @@ static int empress_try_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
 	struct saa7134_dev *dev = video_drvdata(file);
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
 
-	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, MEDIA_BUS_FMT_FIXED);
-	saa_call_all(dev, video, try_mbus_fmt, &mbus_fmt);
-	v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
+	v4l2_fill_mbus_format(&format.format, &f->fmt.pix, MEDIA_BUS_FMT_FIXED);
+	saa_call_all(dev, pad, set_fmt, &pad_cfg, &format);
+	v4l2_fill_pix_format(&f->fmt.pix, &format.format);
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.sizeimage    = TS_PACKET_SIZE * dev->ts.nr_packets;
@@ -221,9 +222,9 @@ static void empress_signal_update(struct work_struct *work)
 		container_of(work, struct saa7134_dev, empress_workqueue);
 
 	if (dev->nosignal) {
-		dprintk("no video signal\n");
+		pr_debug("no video signal\n");
 	} else {
-		dprintk("video signal acquired\n");
+		pr_debug("video signal acquired\n");
 	}
 }
 
@@ -255,7 +256,7 @@ static int empress_init(struct saa7134_dev *dev)
 	struct vb2_queue *q;
 	int err;
 
-	dprintk("%s: %s\n",dev->name,__func__);
+	pr_debug("%s: %s\n", dev->name, __func__);
 	dev->empress_dev = video_device_alloc();
 	if (NULL == dev->empress_dev)
 		return -ENOMEM;
@@ -302,13 +303,13 @@ static int empress_init(struct saa7134_dev *dev)
 	err = video_register_device(dev->empress_dev,VFL_TYPE_GRABBER,
 				    empress_nr[dev->nr]);
 	if (err < 0) {
-		printk(KERN_INFO "%s: can't register video device\n",
+		pr_info("%s: can't register video device\n",
 		       dev->name);
 		video_device_release(dev->empress_dev);
 		dev->empress_dev = NULL;
 		return err;
 	}
-	printk(KERN_INFO "%s: registered device %s [mpeg]\n",
+	pr_info("%s: registered device %s [mpeg]\n",
 	       dev->name, video_device_node_name(dev->empress_dev));
 
 	empress_signal_update(&dev->empress_workqueue);
@@ -317,7 +318,7 @@ static int empress_init(struct saa7134_dev *dev)
 
 static int empress_fini(struct saa7134_dev *dev)
 {
-	dprintk("%s: %s\n",dev->name,__func__);
+	pr_debug("%s: %s\n", dev->name, __func__);
 
 	if (NULL == dev->empress_dev)
 		return 0;
diff --git a/drivers/media/pci/saa7134/saa7134-go7007.c b/drivers/media/pci/saa7134/saa7134-go7007.c
index 54e650b4dff1..8a2abb34186b 100644
--- a/drivers/media/pci/saa7134/saa7134-go7007.c
+++ b/drivers/media/pci/saa7134/saa7134-go7007.c
@@ -11,6 +11,9 @@
  * GNU General Public License for more details.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
@@ -27,8 +30,6 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 
-#include "saa7134.h"
-#include "saa7134-reg.h"
 #include "go7007-priv.h"
 
 /*#define GO7007_HPI_DEBUG*/
@@ -288,9 +289,9 @@ static int saa7134_go7007_stream_start(struct go7007 *go)
 
 	/* Set up transfer block size */
 	saa_writeb(SAA7134_TS_PARALLEL_SERIAL, 128 - 1);
-	saa_writeb(SAA7134_TS_DMA0, (PAGE_SIZE >> 7) - 1);
-	saa_writeb(SAA7134_TS_DMA1, 0);
-	saa_writeb(SAA7134_TS_DMA2, 0);
+	saa_writeb(SAA7134_TS_DMA0, ((PAGE_SIZE >> 7) - 1) & 0xff);
+	saa_writeb(SAA7134_TS_DMA1, (PAGE_SIZE >> 15) & 0xff);
+	saa_writeb(SAA7134_TS_DMA2, (PAGE_SIZE >> 31) & 0x3f);
 
 	/* Enable video streaming mode */
 	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_VIDEO);
diff --git a/drivers/media/pci/saa7134/saa7134-i2c.c b/drivers/media/pci/saa7134/saa7134-i2c.c
index f4da674e7f26..8ef6399d794f 100644
--- a/drivers/media/pci/saa7134/saa7134-i2c.c
+++ b/drivers/media/pci/saa7134/saa7134-i2c.c
@@ -20,14 +20,15 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/delay.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
 #include <media/v4l2-common.h>
 
 /* ----------------------------------------------------------- */
@@ -40,8 +41,15 @@ static unsigned int i2c_scan;
 module_param(i2c_scan, int, 0444);
 MODULE_PARM_DESC(i2c_scan,"scan i2c bus at insmod time");
 
-#define d1printk if (1 == i2c_debug) printk
-#define d2printk if (2 == i2c_debug) printk
+#define i2c_dbg(level, fmt, arg...) do { \
+	if (i2c_debug == level) \
+		printk(KERN_DEBUG pr_fmt("i2c: " fmt), ## arg); \
+	} while (0)
+
+#define i2c_cont(level, fmt, arg...) do { \
+	if (i2c_debug == level) \
+		pr_cont(fmt, ## arg); \
+	} while (0)
 
 #define I2C_WAIT_DELAY  32
 #define I2C_WAIT_RETRY  16
@@ -89,23 +97,20 @@ static inline enum i2c_status i2c_get_status(struct saa7134_dev *dev)
 	enum i2c_status status;
 
 	status = saa_readb(SAA7134_I2C_ATTR_STATUS) & 0x0f;
-	d2printk(KERN_DEBUG "%s: i2c stat <= %s\n",dev->name,
-		 str_i2c_status[status]);
+	i2c_dbg(2, "i2c stat <= %s\n", str_i2c_status[status]);
 	return status;
 }
 
 static inline void i2c_set_status(struct saa7134_dev *dev,
 				  enum i2c_status status)
 {
-	d2printk(KERN_DEBUG "%s: i2c stat => %s\n",dev->name,
-		 str_i2c_status[status]);
+	i2c_dbg(2, "i2c stat => %s\n", str_i2c_status[status]);
 	saa_andorb(SAA7134_I2C_ATTR_STATUS,0x0f,status);
 }
 
 static inline void i2c_set_attr(struct saa7134_dev *dev, enum i2c_attr attr)
 {
-	d2printk(KERN_DEBUG "%s: i2c attr => %s\n",dev->name,
-		 str_i2c_attr[attr]);
+	i2c_dbg(2, "i2c attr => %s\n", str_i2c_attr[attr]);
 	saa_andorb(SAA7134_I2C_ATTR_STATUS,0xc0,attr << 6);
 }
 
@@ -168,7 +173,7 @@ static int i2c_reset(struct saa7134_dev *dev)
 	enum i2c_status status;
 	int count;
 
-	d2printk(KERN_DEBUG "%s: i2c reset\n",dev->name);
+	i2c_dbg(2, "i2c reset\n");
 	status = i2c_get_status(dev);
 	if (!i2c_is_error(status))
 		return true;
@@ -206,7 +211,7 @@ static inline int i2c_send_byte(struct saa7134_dev *dev,
 //	dword |= 0x40 << 16;  /* 400 kHz */
 	dword |= 0xf0 << 24;
 	saa_writel(SAA7134_I2C_ATTR_STATUS >> 2, dword);
-	d2printk(KERN_DEBUG "%s: i2c data => 0x%x\n",dev->name,data);
+	i2c_dbg(2, "i2c data => 0x%x\n", data);
 
 	if (!i2c_is_busy_wait(dev))
 		return -EIO;
@@ -228,7 +233,7 @@ static inline int i2c_recv_byte(struct saa7134_dev *dev)
 	if (i2c_is_error(status))
 		return -EIO;
 	data = saa_readb(SAA7134_I2C_DATA);
-	d2printk(KERN_DEBUG "%s: i2c data <= 0x%x\n",dev->name,data);
+	i2c_dbg(2, "i2c data <= 0x%x\n", data);
 	return data;
 }
 
@@ -245,12 +250,12 @@ static int saa7134_i2c_xfer(struct i2c_adapter *i2c_adap,
 		if (!i2c_reset(dev))
 			return -EIO;
 
-	d2printk("start xfer\n");
-	d1printk(KERN_DEBUG "%s: i2c xfer:",dev->name);
+	i2c_dbg(2, "start xfer\n");
+	i2c_dbg(1, "i2c xfer:");
 	for (i = 0; i < num; i++) {
 		if (!(msgs[i].flags & I2C_M_NOSTART) || 0 == i) {
 			/* send address */
-			d2printk("send address\n");
+			i2c_dbg(2, "send address\n");
 			addr  = msgs[i].addr << 1;
 			if (msgs[i].flags & I2C_M_RD)
 				addr |= 1;
@@ -262,50 +267,50 @@ static int saa7134_i2c_xfer(struct i2c_adapter *i2c_adap,
 				 * needed to talk to the mt352 demux
 				 * thanks to pinnacle for the hint */
 				int quirk = 0xfe;
-				d1printk(" [%02x quirk]",quirk);
+				i2c_cont(1, " [%02x quirk]", quirk);
 				i2c_send_byte(dev,START,quirk);
 				i2c_recv_byte(dev);
 			}
-			d1printk(" < %02x", addr);
+			i2c_cont(1, " < %02x", addr);
 			rc = i2c_send_byte(dev,START,addr);
 			if (rc < 0)
 				 goto err;
 		}
 		if (msgs[i].flags & I2C_M_RD) {
 			/* read bytes */
-			d2printk("read bytes\n");
+			i2c_dbg(2, "read bytes\n");
 			for (byte = 0; byte < msgs[i].len; byte++) {
-				d1printk(" =");
+				i2c_cont(1, " =");
 				rc = i2c_recv_byte(dev);
 				if (rc < 0)
 					goto err;
-				d1printk("%02x", rc);
+				i2c_cont(1, "%02x", rc);
 				msgs[i].buf[byte] = rc;
 			}
 			/* discard mysterious extra byte when reading
 			   from Samsung S5H1411.  i2c bus gets error
 			   if we do not. */
 			if (0x19 == msgs[i].addr) {
-				d1printk(" ?");
+				i2c_cont(1, " ?");
 				rc = i2c_recv_byte(dev);
 				if (rc < 0)
 					goto err;
-				d1printk("%02x", rc);
+				i2c_cont(1, "%02x", rc);
 			}
 		} else {
 			/* write bytes */
-			d2printk("write bytes\n");
+			i2c_dbg(2, "write bytes\n");
 			for (byte = 0; byte < msgs[i].len; byte++) {
 				data = msgs[i].buf[byte];
-				d1printk(" %02x", data);
+				i2c_cont(1, " %02x", data);
 				rc = i2c_send_byte(dev,CONTINUE,data);
 				if (rc < 0)
 					goto err;
 			}
 		}
 	}
-	d2printk("xfer done\n");
-	d1printk(" >");
+	i2c_dbg(2, "xfer done\n");
+	i2c_cont(1, " >");
 	i2c_set_attr(dev,STOP);
 	rc = -EIO;
 	if (!i2c_is_busy_wait(dev))
@@ -316,12 +321,12 @@ static int saa7134_i2c_xfer(struct i2c_adapter *i2c_adap,
 	/* ensure that the bus is idle for at least one bit slot */
 	msleep(1);
 
-	d1printk("\n");
+	i2c_cont(1, "\n");
 	return num;
  err:
 	if (1 == i2c_debug) {
 		status = i2c_get_status(dev);
-		printk(" ERROR: %s\n",str_i2c_status[status]);
+		i2c_cont(1, " ERROR: %s\n", str_i2c_status[status]);
 	}
 	return rc;
 }
@@ -359,22 +364,22 @@ saa7134_i2c_eeprom(struct saa7134_dev *dev, unsigned char *eedata, int len)
 	dev->i2c_client.addr = 0xa0 >> 1;
 	buf = 0;
 	if (1 != (err = i2c_master_send(&dev->i2c_client,&buf,1))) {
-		printk(KERN_INFO "%s: Huh, no eeprom present (err=%d)?\n",
+		pr_info("%s: Huh, no eeprom present (err=%d)?\n",
 		       dev->name,err);
 		return -1;
 	}
 	if (len != (err = i2c_master_recv(&dev->i2c_client,eedata,len))) {
-		printk(KERN_WARNING "%s: i2c eeprom read error (err=%d)\n",
+		pr_warn("%s: i2c eeprom read error (err=%d)\n",
 		       dev->name,err);
 		return -1;
 	}
-	for (i = 0; i < len; i++) {
-		if (0 == (i % 16))
-			printk(KERN_INFO "%s: i2c eeprom %02x:",dev->name,i);
-		printk(" %02x",eedata[i]);
-		if (15 == (i % 16))
-			printk("\n");
+
+	for (i = 0; i < len; i += 16) {
+		int size = (len - i) > 16 ? 16 : len - i;
+
+		pr_info("i2c eeprom %02x: %*ph\n", i, size, &eedata[i]);
 	}
+
 	return 0;
 }
 
@@ -386,7 +391,7 @@ static char *i2c_devs[128] = {
 	[ 0x5a >> 1 ] = "remote control",
 };
 
-static void do_i2c_scan(char *name, struct i2c_client *c)
+static void do_i2c_scan(struct i2c_client *c)
 {
 	unsigned char buf;
 	int i,rc;
@@ -396,8 +401,8 @@ static void do_i2c_scan(char *name, struct i2c_client *c)
 		rc = i2c_master_recv(c,&buf,0);
 		if (rc < 0)
 			continue;
-		printk("%s: i2c scan: found device @ 0x%x  [%s]\n",
-		       name, i << 1, i2c_devs[i] ? i2c_devs[i] : "???");
+		pr_info("i2c scan: found device @ 0x%x  [%s]\n",
+			 i << 1, i2c_devs[i] ? i2c_devs[i] : "???");
 	}
 }
 
@@ -415,7 +420,7 @@ int saa7134_i2c_register(struct saa7134_dev *dev)
 
 	saa7134_i2c_eeprom(dev,dev->eedata,sizeof(dev->eedata));
 	if (i2c_scan)
-		do_i2c_scan(dev->name,&dev->i2c_client);
+		do_i2c_scan(&dev->i2c_client);
 
 	/* Instantiate the IR receiver device, if present */
 	saa7134_probe_i2c_ir(dev);
diff --git a/drivers/media/pci/saa7134/saa7134-input.c b/drivers/media/pci/saa7134/saa7134-input.c
index dc3d6516edf7..11a172000291 100644
--- a/drivers/media/pci/saa7134/saa7134-input.c
+++ b/drivers/media/pci/saa7134/saa7134-input.c
@@ -18,15 +18,15 @@
  *
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
-
 #define MODULE_NAME "saa7134"
 
 static unsigned int disable_ir;
@@ -41,10 +41,14 @@ static int pinnacle_remote;
 module_param(pinnacle_remote, int, 0644);    /* Choose Pinnacle PCTV remote */
 MODULE_PARM_DESC(pinnacle_remote, "Specify Pinnacle PCTV remote: 0=coloured, 1=grey (defaults to 0)");
 
-#define dprintk(fmt, arg...)	if (ir_debug) \
-	printk(KERN_DEBUG "%s/ir: " fmt, dev->name , ## arg)
-#define i2cdprintk(fmt, arg...)    if (ir_debug) \
-	printk(KERN_DEBUG "%s/ir: " fmt, ir->name , ## arg)
+#define input_dbg(fmt, arg...) do { \
+	if (ir_debug) \
+		printk(KERN_DEBUG pr_fmt("input: " fmt), ## arg); \
+	} while (0)
+#define ir_dbg(ir, fmt, arg...) do { \
+	if (ir_debug) \
+		printk(KERN_DEBUG pr_fmt("ir %s: " fmt), ir->name, ## arg); \
+	} while (0)
 
 /* Helper function for raw decoding at GPIO16 or GPIO18 */
 static int saa7134_raw_decode_irq(struct saa7134_dev *dev);
@@ -75,7 +79,7 @@ static int build_key(struct saa7134_dev *dev)
 	}
 
 	data = ir_extract_bits(gpio, ir->mask_keycode);
-	dprintk("build_key gpio=0x%x mask=0x%x data=%d\n",
+	input_dbg("build_key gpio=0x%x mask=0x%x data=%d\n",
 		gpio, ir->mask_keycode, data);
 
 	switch (dev->board) {
@@ -119,7 +123,7 @@ static int get_key_flydvb_trio(struct IR_i2c *ir, enum rc_type *protocol,
 	struct saa7134_dev *dev = ir->c->adapter->algo_data;
 
 	if (dev == NULL) {
-		i2cdprintk("get_key_flydvb_trio: "
+		ir_dbg(ir, "get_key_flydvb_trio: "
 			   "ir->c->adapter->algo_data is NULL!\n");
 		return -EIO;
 	}
@@ -146,12 +150,12 @@ static int get_key_flydvb_trio(struct IR_i2c *ir, enum rc_type *protocol,
 			msleep(10);
 			continue;
 		}
-		i2cdprintk("send wake up byte to pic16C505 (IR chip)"
+		ir_dbg(ir, "send wake up byte to pic16C505 (IR chip)"
 			   "failed %dx\n", attempt);
 		return -EIO;
 	}
 	if (1 != i2c_master_recv(ir->c, &b, 1)) {
-		i2cdprintk("read error\n");
+		ir_dbg(ir, "read error\n");
 		return -EIO;
 	}
 
@@ -170,7 +174,7 @@ static int get_key_msi_tvanywhere_plus(struct IR_i2c *ir, enum rc_type *protocol
 	/* <dev> is needed to access GPIO. Used by the saa_readl macro. */
 	struct saa7134_dev *dev = ir->c->adapter->algo_data;
 	if (dev == NULL) {
-		i2cdprintk("get_key_msi_tvanywhere_plus: "
+		ir_dbg(ir, "get_key_msi_tvanywhere_plus: "
 			   "ir->c->adapter->algo_data is NULL!\n");
 		return -EIO;
 	}
@@ -191,7 +195,7 @@ static int get_key_msi_tvanywhere_plus(struct IR_i2c *ir, enum rc_type *protocol
 	/* GPIO says there is a button press. Get it. */
 
 	if (1 != i2c_master_recv(ir->c, &b, 1)) {
-		i2cdprintk("read error\n");
+		ir_dbg(ir, "read error\n");
 		return -EIO;
 	}
 
@@ -202,7 +206,7 @@ static int get_key_msi_tvanywhere_plus(struct IR_i2c *ir, enum rc_type *protocol
 
 	/* Button pressed */
 
-	dprintk("get_key_msi_tvanywhere_plus: Key = 0x%02X\n", b);
+	input_dbg("get_key_msi_tvanywhere_plus: Key = 0x%02X\n", b);
 	*protocol = RC_TYPE_UNKNOWN;
 	*scancode = b;
 	*toggle = 0;
@@ -219,7 +223,7 @@ static int get_key_kworld_pc150u(struct IR_i2c *ir, enum rc_type *protocol,
 	/* <dev> is needed to access GPIO. Used by the saa_readl macro. */
 	struct saa7134_dev *dev = ir->c->adapter->algo_data;
 	if (dev == NULL) {
-		i2cdprintk("get_key_kworld_pc150u: "
+		ir_dbg(ir, "get_key_kworld_pc150u: "
 			   "ir->c->adapter->algo_data is NULL!\n");
 		return -EIO;
 	}
@@ -240,7 +244,7 @@ static int get_key_kworld_pc150u(struct IR_i2c *ir, enum rc_type *protocol,
 	/* GPIO says there is a button press. Get it. */
 
 	if (1 != i2c_master_recv(ir->c, &b, 1)) {
-		i2cdprintk("read error\n");
+		ir_dbg(ir, "read error\n");
 		return -EIO;
 	}
 
@@ -251,7 +255,7 @@ static int get_key_kworld_pc150u(struct IR_i2c *ir, enum rc_type *protocol,
 
 	/* Button pressed */
 
-	dprintk("get_key_kworld_pc150u: Key = 0x%02X\n", b);
+	input_dbg("get_key_kworld_pc150u: Key = 0x%02X\n", b);
 	*protocol = RC_TYPE_UNKNOWN;
 	*scancode = b;
 	*toggle = 0;
@@ -265,7 +269,7 @@ static int get_key_purpletv(struct IR_i2c *ir, enum rc_type *protocol,
 
 	/* poll IR chip */
 	if (1 != i2c_master_recv(ir->c, &b, 1)) {
-		i2cdprintk("read error\n");
+		ir_dbg(ir, "read error\n");
 		return -EIO;
 	}
 
@@ -334,7 +338,7 @@ static int get_key_beholdm6xx(struct IR_i2c *ir, enum rc_type *protocol,
 	ir->c->addr = 0x5a >> 1;
 
 	if (12 != i2c_master_recv(ir->c, data, 12)) {
-		i2cdprintk("read error\n");
+		ir_dbg(ir, "read error\n");
 		return -EIO;
 	}
 
@@ -359,7 +363,7 @@ static int get_key_pinnacle(struct IR_i2c *ir, enum rc_type *protocol,
 
 	/* poll IR chip */
 	if (4 != i2c_master_recv(ir->c, b, 4)) {
-		i2cdprintk("read error\n");
+		ir_dbg(ir, "read error\n");
 		return -EIO;
 	}
 
@@ -391,7 +395,7 @@ static int get_key_pinnacle(struct IR_i2c *ir, enum rc_type *protocol,
 	*scancode = code;
 	*toggle = 0;
 
-	i2cdprintk("Pinnacle PCTV key %02x\n", code);
+	ir_dbg(ir, "Pinnacle PCTV key %02x\n", code);
 	return 1;
 }
 
@@ -481,6 +485,7 @@ static int __saa7134_ir_start(void *priv)
 	case SAA7134_BOARD_KWORLD_VSTREAM_XPERT:
 	case SAA7134_BOARD_AVERMEDIA_305:
 	case SAA7134_BOARD_AVERMEDIA_307:
+	case SAA7134_BOARD_AVERMEDIA_505:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_305:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_505:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_307:
@@ -629,6 +634,7 @@ int saa7134_input_init1(struct saa7134_dev *dev)
 	case SAA7134_BOARD_KWORLD_VSTREAM_XPERT:
 	case SAA7134_BOARD_AVERMEDIA_305:
 	case SAA7134_BOARD_AVERMEDIA_307:
+	case SAA7134_BOARD_AVERMEDIA_505:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_305:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_505:
 	case SAA7134_BOARD_AVERMEDIA_STUDIO_307:
@@ -831,8 +837,7 @@ int saa7134_input_init1(struct saa7134_dev *dev)
 		break;
 	}
 	if (NULL == ir_codes) {
-		printk("%s: Oops: IR config error [card=%d]\n",
-		       dev->name, dev->board);
+		pr_err("Oops: IR config error [card=%d]\n", dev->board);
 		return -ENODEV;
 	}
 
@@ -916,7 +921,7 @@ void saa7134_probe_i2c_ir(struct saa7134_dev *dev)
 	int rc;
 
 	if (disable_ir) {
-		dprintk("IR has been disabled, not probing for i2c remote\n");
+		input_dbg("IR has been disabled, not probing for i2c remote\n");
 		return;
 	}
 
@@ -959,7 +964,7 @@ void saa7134_probe_i2c_ir(struct saa7134_dev *dev)
 		   an existing device. Weird...
 		   REVISIT: might no longer be needed */
 		rc = i2c_transfer(&dev->i2c_adap, &msg_msi, 1);
-		dprintk("probe 0x%02x @ %s: %s\n",
+		input_dbg("probe 0x%02x @ %s: %s\n",
 			msg_msi.addr, dev->i2c_adap.name,
 			(1 == rc) ? "yes" : "no");
 		break;
@@ -974,7 +979,7 @@ void saa7134_probe_i2c_ir(struct saa7134_dev *dev)
 		   an existing device. Weird...
 		   REVISIT: might no longer be needed */
 		rc = i2c_transfer(&dev->i2c_adap, &msg_msi, 1);
-		dprintk("probe 0x%02x @ %s: %s\n",
+		input_dbg("probe 0x%02x @ %s: %s\n",
 			msg_msi.addr, dev->i2c_adap.name,
 			(1 == rc) ? "yes" : "no");
 		break;
@@ -1019,7 +1024,7 @@ void saa7134_probe_i2c_ir(struct saa7134_dev *dev)
 		info.addr = 0x0b;
 		break;
 	default:
-		dprintk("No I2C IR support for board %x\n", dev->board);
+		input_dbg("No I2C IR support for board %x\n", dev->board);
 		return;
 	}
 
diff --git a/drivers/media/pci/saa7134/saa7134-ts.c b/drivers/media/pci/saa7134/saa7134-ts.c
index 2709b83d57b1..4b202fa5fbc4 100644
--- a/drivers/media/pci/saa7134/saa7134-ts.c
+++ b/drivers/media/pci/saa7134/saa7134-ts.c
@@ -20,23 +20,25 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/delay.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
-
 /* ------------------------------------------------------------------ */
 
 static unsigned int ts_debug;
 module_param(ts_debug, int, 0644);
 MODULE_PARM_DESC(ts_debug,"enable debug messages [ts]");
 
-#define dprintk(fmt, arg...)	if (ts_debug) \
-	printk(KERN_DEBUG "%s/ts: " fmt, dev->name , ## arg)
+#define ts_dbg(fmt, arg...) do { \
+	if (ts_debug) \
+		printk(KERN_DEBUG pr_fmt("ts: " fmt), ## arg); \
+	} while (0)
 
 /* ------------------------------------------------------------------ */
 static int buffer_activate(struct saa7134_dev *dev,
@@ -44,7 +46,7 @@ static int buffer_activate(struct saa7134_dev *dev,
 			   struct saa7134_buf *next)
 {
 
-	dprintk("buffer_activate [%p]",buf);
+	ts_dbg("buffer_activate [%p]", buf);
 	buf->top_seen = 0;
 
 	if (!dev->ts_started)
@@ -53,12 +55,12 @@ static int buffer_activate(struct saa7134_dev *dev,
 	if (NULL == next)
 		next = buf;
 	if (V4L2_FIELD_TOP == dev->ts_field) {
-		dprintk("- [top]     buf=%p next=%p\n",buf,next);
+		ts_dbg("- [top]     buf=%p next=%p\n", buf, next);
 		saa_writel(SAA7134_RS_BA1(5),saa7134_buffer_base(buf));
 		saa_writel(SAA7134_RS_BA2(5),saa7134_buffer_base(next));
 		dev->ts_field = V4L2_FIELD_BOTTOM;
 	} else {
-		dprintk("- [bottom]  buf=%p next=%p\n",buf,next);
+		ts_dbg("- [bottom]  buf=%p next=%p\n", buf, next);
 		saa_writel(SAA7134_RS_BA1(5),saa7134_buffer_base(next));
 		saa_writel(SAA7134_RS_BA2(5),saa7134_buffer_base(buf));
 		dev->ts_field = V4L2_FIELD_TOP;
@@ -95,7 +97,7 @@ int saa7134_ts_buffer_prepare(struct vb2_buffer *vb2)
 	struct sg_table *dma = vb2_dma_sg_plane_desc(vb2, 0);
 	unsigned int lines, llength, size;
 
-	dprintk("buffer_prepare [%p]\n", buf);
+	ts_dbg("buffer_prepare [%p]\n", buf);
 
 	llength = TS_PACKET_SIZE;
 	lines = dev->ts.nr_packets;
@@ -239,7 +241,7 @@ int saa7134_ts_init1(struct saa7134_dev *dev)
 /* Function for stop TS */
 int saa7134_ts_stop(struct saa7134_dev *dev)
 {
-	dprintk("TS stop\n");
+	ts_dbg("TS stop\n");
 
 	if (!dev->ts_started)
 		return 0;
@@ -261,7 +263,7 @@ int saa7134_ts_stop(struct saa7134_dev *dev)
 /* Function for start TS */
 int saa7134_ts_start(struct saa7134_dev *dev)
 {
-	dprintk("TS start\n");
+	ts_dbg("TS start\n");
 
 	if (WARN_ON(dev->ts_started))
 		return 0;
diff --git a/drivers/media/pci/saa7134/saa7134-tvaudio.c b/drivers/media/pci/saa7134/saa7134-tvaudio.c
index 3afbcb70b518..21a579309575 100644
--- a/drivers/media/pci/saa7134/saa7134-tvaudio.c
+++ b/drivers/media/pci/saa7134/saa7134-tvaudio.c
@@ -20,6 +20,9 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
@@ -29,9 +32,6 @@
 #include <linux/freezer.h>
 #include <asm/div64.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
-
 /* ------------------------------------------------------------------ */
 
 static unsigned int audio_debug;
@@ -49,13 +49,10 @@ static int audio_clock_tweak;
 module_param(audio_clock_tweak, int, 0644);
 MODULE_PARM_DESC(audio_clock_tweak, "Audio clock tick fine tuning for cards with audio crystal that's slightly off (range [-1024 .. 1024])");
 
-#define dprintk(fmt, arg...)	if (audio_debug) \
-	printk(KERN_DEBUG "%s/audio: " fmt, dev->name , ## arg)
-#define d2printk(fmt, arg...)	if (audio_debug > 1) \
-	printk(KERN_DEBUG "%s/audio: " fmt, dev->name, ## arg)
-
-#define print_regb(reg) printk("%s:   reg 0x%03x [%-16s]: 0x%02x\n", \
-		dev->name,(SAA7134_##reg),(#reg),saa_readb((SAA7134_##reg)))
+#define audio_dbg(level, fmt, arg...) do { \
+	if (audio_debug >= level) \
+		printk(KERN_DEBUG pr_fmt("audio: " fmt), ## arg); \
+	} while (0)
 
 /* msecs */
 #define SCAN_INITIAL_DELAY     1000
@@ -206,13 +203,14 @@ static void mute_input_7134(struct saa7134_dev *dev)
 
 	if (dev->hw_mute  == mute &&
 		dev->hw_input == in && !dev->insuspend) {
-		dprintk("mute/input: nothing to do [mute=%d,input=%s]\n",
-			mute,in->name);
+		audio_dbg(1, "mute/input: nothing to do [mute=%d,input=%s]\n",
+			  mute, in->name);
 		return;
 	}
 
-	dprintk("ctl_mute=%d automute=%d input=%s  =>  mute=%d input=%s\n",
-		dev->ctl_mute,dev->automute,dev->input->name,mute,in->name);
+	audio_dbg(1, "ctl_mute=%d automute=%d input=%s  =>  mute=%d input=%s\n",
+		  dev->ctl_mute, dev->automute,
+		  dev->input->name, mute, in->name);
 	dev->hw_mute  = mute;
 	dev->hw_input = in;
 
@@ -265,8 +263,8 @@ static void tvaudio_setmode(struct saa7134_dev *dev,
 		tweak = audio_clock_tweak;
 
 	if (note)
-		dprintk("tvaudio_setmode: %s %s [%d.%03d/%d.%03d MHz] acpf=%d%+d\n",
-			note,audio->name,
+		audio_dbg(1, "tvaudio_setmode: %s %s [%d.%03d/%d.%03d MHz] acpf=%d%+d\n",
+			note, audio->name,
 			audio->carr1 / 1000, audio->carr1 % 1000,
 			audio->carr2 / 1000, audio->carr2 % 1000,
 			acpf, tweak);
@@ -334,14 +332,14 @@ static int tvaudio_checkcarrier(struct saa7134_dev *dev, struct mainscan *scan)
 
 	if (!(dev->tvnorm->id & scan->std)) {
 		value = 0;
-		dprintk("skipping %d.%03d MHz [%4s]\n",
-			scan->carr / 1000, scan->carr % 1000, scan->name);
+		audio_dbg(1, "skipping %d.%03d MHz [%4s]\n",
+			  scan->carr / 1000, scan->carr % 1000, scan->name);
 		return 0;
 	}
 
 	if (audio_debug > 1) {
 		int i;
-		dprintk("debug %d:",scan->carr);
+		audio_dbg(1, "debug %d:", scan->carr);
 		for (i = -150; i <= 150; i += 30) {
 			tvaudio_setcarrier(dev,scan->carr+i,scan->carr+i);
 			saa_readl(SAA7134_LEVEL_READOUT1 >> 2);
@@ -349,11 +347,11 @@ static int tvaudio_checkcarrier(struct saa7134_dev *dev, struct mainscan *scan)
 				return -1;
 			value = saa_readl(SAA7134_LEVEL_READOUT1 >> 2);
 			if (0 == i)
-				printk("  #  %6d  # ",value >> 16);
+				pr_cont("  #  %6d  # ", value >> 16);
 			else
-				printk(" %6d",value >> 16);
+				pr_cont(" %6d", value >> 16);
 		}
-		printk("\n");
+		pr_cont("\n");
 	}
 
 	tvaudio_setcarrier(dev,scan->carr-90,scan->carr-90);
@@ -371,9 +369,9 @@ static int tvaudio_checkcarrier(struct saa7134_dev *dev, struct mainscan *scan)
 	left >>= 16;
 	right >>= 16;
 	value = left > right ? left - right : right - left;
-	dprintk("scanning %d.%03d MHz [%4s] =>  dc is %5d [%d/%d]\n",
-		scan->carr / 1000, scan->carr % 1000,
-		scan->name, value, left, right);
+	audio_dbg(1, "scanning %d.%03d MHz [%4s] =>  dc is %5d [%d/%d]\n",
+		  scan->carr / 1000, scan->carr % 1000,
+		  scan->name, value, left, right);
 	return value;
 }
 
@@ -389,7 +387,7 @@ static int tvaudio_getstereo(struct saa7134_dev *dev, struct saa7134_tvaudio *au
 	case TVAUDIO_FM_K_STEREO:
 	case TVAUDIO_FM_BG_STEREO:
 		idp = (saa_readb(SAA7134_IDENT_SIF) & 0xe0) >> 5;
-		dprintk("getstereo: fm/stereo: idp=0x%x\n",idp);
+		audio_dbg(1, "getstereo: fm/stereo: idp=0x%x\n", idp);
 		if (0x03 == (idp & 0x03))
 			retval = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
 		else if (0x05 == (idp & 0x05))
@@ -403,10 +401,11 @@ static int tvaudio_getstereo(struct saa7134_dev *dev, struct saa7134_tvaudio *au
 	case TVAUDIO_NICAM_FM:
 	case TVAUDIO_NICAM_AM:
 		nicam = saa_readb(SAA7134_AUDIO_STATUS);
-		dprintk("getstereo: nicam=0x%x\n",nicam);
+		audio_dbg(1, "getstereo: nicam=0x%x\n", nicam);
 		if (nicam & 0x1) {
 			nicam_status = saa_readb(SAA7134_NICAM_STATUS);
-			dprintk("getstereo: nicam_status=0x%x\n", nicam_status);
+			audio_dbg(1, "getstereo: nicam_status=0x%x\n",
+				  nicam_status);
 
 			switch (nicam_status & 0x03) {
 			    case 0x01:
@@ -424,7 +423,7 @@ static int tvaudio_getstereo(struct saa7134_dev *dev, struct saa7134_tvaudio *au
 		break;
 	}
 	if (retval != -1)
-		dprintk("found audio subchannels:%s%s%s%s\n",
+		audio_dbg(1, "found audio subchannels:%s%s%s%s\n",
 			(retval & V4L2_TUNER_SUB_MONO)   ? " mono"   : "",
 			(retval & V4L2_TUNER_SUB_STEREO) ? " stereo" : "",
 			(retval & V4L2_TUNER_SUB_LANG1)  ? " lang1"  : "",
@@ -459,8 +458,8 @@ static int tvaudio_setstereo(struct saa7134_dev *dev, struct saa7134_tvaudio *au
 	case TVAUDIO_FM_BG_STEREO:
 	case TVAUDIO_NICAM_AM:
 	case TVAUDIO_NICAM_FM:
-		dprintk("setstereo [fm] => %s\n",
-			name[ mode % ARRAY_SIZE(name) ]);
+		audio_dbg(1, "setstereo [fm] => %s\n",
+			  name[mode % ARRAY_SIZE(name)]);
 		reg = fm[ mode % ARRAY_SIZE(fm) ];
 		saa_writeb(SAA7134_FM_DEMATRIX, reg);
 		break;
@@ -489,7 +488,8 @@ static int tvaudio_thread(void *data)
 		try_to_freeze();
 
 		dev->thread.scan1 = dev->thread.scan2;
-		dprintk("tvaudio thread scan start [%d]\n",dev->thread.scan1);
+		audio_dbg(1, "tvaudio thread scan start [%d]\n",
+			  dev->thread.scan1);
 		dev->tvaudio  = NULL;
 
 		saa_writeb(SAA7134_MONITOR_SELECT,   0xa0);
@@ -519,7 +519,7 @@ static int tvaudio_thread(void *data)
 
 		if (1 == nscan) {
 			/* only one candidate -- skip scan ;) */
-			dprintk("only one main carrier candidate - skipping scan\n");
+			audio_dbg(1, "only one main carrier candidate - skipping scan\n");
 			max1 = 12345;
 			carrier = default_carrier;
 		} else {
@@ -544,26 +544,24 @@ static int tvaudio_thread(void *data)
 
 		if (0 != carrier && max1 > 2000 && max1 > max2*3) {
 			/* found good carrier */
-			dprintk("found %s main sound carrier @ %d.%03d MHz [%d/%d]\n",
-				dev->tvnorm->name, carrier/1000, carrier%1000,
-				max1, max2);
+			audio_dbg(1, "found %s main sound carrier @ %d.%03d MHz [%d/%d]\n",
+				  dev->tvnorm->name, carrier/1000, carrier%1000,
+				  max1, max2);
 			dev->last_carrier = carrier;
 			dev->automute = 0;
 
 		} else if (0 != dev->last_carrier) {
 			/* no carrier -- try last detected one as fallback */
 			carrier = dev->last_carrier;
-			dprintk("audio carrier scan failed, "
-				"using %d.%03d MHz [last detected]\n",
-				carrier/1000, carrier%1000);
+			audio_dbg(1, "audio carrier scan failed, using %d.%03d MHz [last detected]\n",
+				  carrier/1000, carrier%1000);
 			dev->automute = 1;
 
 		} else {
 			/* no carrier + no fallback -- use default */
 			carrier = default_carrier;
-			dprintk("audio carrier scan failed, "
-				"using %d.%03d MHz [default]\n",
-				carrier/1000, carrier%1000);
+			audio_dbg(1, "audio carrier scan failed, using %d.%03d MHz [default]\n",
+				  carrier/1000, carrier%1000);
 			dev->automute = 1;
 		}
 		tvaudio_setcarrier(dev,carrier,carrier);
@@ -661,7 +659,7 @@ static inline int saa_dsp_reset_error_bit(struct saa7134_dev *dev)
 {
 	int state = saa_readb(SAA7135_DSP_RWSTATE);
 	if (unlikely(state & SAA7135_DSP_RWSTATE_ERR)) {
-		d2printk("%s: resetting error bit\n", dev->name);
+		audio_dbg(2, "%s: resetting error bit\n", dev->name);
 		saa_writeb(SAA7135_DSP_RWCLEAR, SAA7135_DSP_RWCLEAR_RERR);
 	}
 	return 0;
@@ -673,18 +671,17 @@ static inline int saa_dsp_wait_bit(struct saa7134_dev *dev, int bit)
 
 	state = saa_readb(SAA7135_DSP_RWSTATE);
 	if (unlikely(state & SAA7135_DSP_RWSTATE_ERR)) {
-		printk(KERN_WARNING "%s: dsp access error\n", dev->name);
+		pr_warn("%s: dsp access error\n", dev->name);
 		saa_dsp_reset_error_bit(dev);
 		return -EIO;
 	}
 	while (0 == (state & bit)) {
 		if (unlikely(0 == count)) {
-			printk("%s: dsp access wait timeout [bit=%s]\n",
-			       dev->name,
-			       (bit & SAA7135_DSP_RWSTATE_WRR) ? "WRR" :
-			       (bit & SAA7135_DSP_RWSTATE_RDB) ? "RDB" :
-			       (bit & SAA7135_DSP_RWSTATE_IDA) ? "IDA" :
-			       "???");
+			pr_err("dsp access wait timeout [bit=%s]\n",
+				 (bit & SAA7135_DSP_RWSTATE_WRR) ? "WRR" :
+				 (bit & SAA7135_DSP_RWSTATE_RDB) ? "RDB" :
+				 (bit & SAA7135_DSP_RWSTATE_IDA) ? "IDA" :
+				 "???");
 			return -EIO;
 		}
 		saa_wait(DSP_DELAY);
@@ -699,7 +696,7 @@ int saa_dsp_writel(struct saa7134_dev *dev, int reg, u32 value)
 {
 	int err;
 
-	d2printk("dsp write reg 0x%x = 0x%06x\n",reg<<2,value);
+	audio_dbg(2, "dsp write reg 0x%x = 0x%06x\n", reg << 2, value);
 	err = saa_dsp_wait_bit(dev,SAA7135_DSP_RWSTATE_WRR);
 	if (err < 0)
 		return err;
@@ -786,14 +783,16 @@ static int tvaudio_thread_ddep(void *data)
 		try_to_freeze();
 
 		dev->thread.scan1 = dev->thread.scan2;
-		dprintk("tvaudio thread scan start [%d]\n",dev->thread.scan1);
+		audio_dbg(1, "tvaudio thread scan start [%d]\n",
+			  dev->thread.scan1);
 
 		if (audio_ddep >= 0x04 && audio_ddep <= 0x0e) {
 			/* insmod option override */
 			norms = (audio_ddep << 2) | 0x01;
-			dprintk("ddep override: %s\n",stdres[audio_ddep]);
+			audio_dbg(1, "ddep override: %s\n",
+				  stdres[audio_ddep]);
 		} else if (&card(dev).radio == dev->input) {
-			dprintk("FM Radio\n");
+			audio_dbg(1, "FM Radio\n");
 			if (dev->tuner_type == TUNER_PHILIPS_TDA8290) {
 				norms = (0x11 << 2) | 0x01;
 				/* set IF frequency to 5.5 MHz */
@@ -816,12 +815,12 @@ static int tvaudio_thread_ddep(void *data)
 				norms |= 0x10;
 			if (0 == norms)
 				norms = 0x7c; /* all */
-			dprintk("scanning:%s%s%s%s%s\n",
-				(norms & 0x04) ? " B/G"  : "",
-				(norms & 0x08) ? " D/K"  : "",
-				(norms & 0x10) ? " L/L'" : "",
-				(norms & 0x20) ? " I"    : "",
-				(norms & 0x40) ? " M"    : "");
+			audio_dbg(1, "scanning:%s%s%s%s%s\n",
+				  (norms & 0x04) ? " B/G"  : "",
+				  (norms & 0x08) ? " D/K"  : "",
+				  (norms & 0x10) ? " L/L'" : "",
+				  (norms & 0x20) ? " I"    : "",
+				  (norms & 0x40) ? " M"    : "");
 		}
 
 		/* kick automatic standard detection */
@@ -836,29 +835,28 @@ static int tvaudio_thread_ddep(void *data)
 			goto restart;
 		value = saa_readl(0x528 >> 2) & 0xffffff;
 
-		dprintk("tvaudio thread status: 0x%x [%s%s%s]\n",
-			value, stdres[value & 0x1f],
-			(value & 0x000020) ? ",stereo" : "",
-			(value & 0x000040) ? ",dual"   : "");
-		dprintk("detailed status: "
-			"%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s\n",
-			(value & 0x000080) ? " A2/EIAJ pilot tone "     : "",
-			(value & 0x000100) ? " A2/EIAJ dual "           : "",
-			(value & 0x000200) ? " A2/EIAJ stereo "         : "",
-			(value & 0x000400) ? " A2/EIAJ noise mute "     : "",
-
-			(value & 0x000800) ? " BTSC/FM radio pilot "    : "",
-			(value & 0x001000) ? " SAP carrier "            : "",
-			(value & 0x002000) ? " BTSC stereo noise mute " : "",
-			(value & 0x004000) ? " SAP noise mute "         : "",
-			(value & 0x008000) ? " VDSP "                   : "",
-
-			(value & 0x010000) ? " NICST "                  : "",
-			(value & 0x020000) ? " NICDU "                  : "",
-			(value & 0x040000) ? " NICAM muted "            : "",
-			(value & 0x080000) ? " NICAM reserve sound "    : "",
-
-			(value & 0x100000) ? " init done "              : "");
+		audio_dbg(1, "tvaudio thread status: 0x%x [%s%s%s]\n",
+			  value, stdres[value & 0x1f],
+			  (value & 0x000020) ? ",stereo" : "",
+			  (value & 0x000040) ? ",dual"   : "");
+		audio_dbg(1, "detailed status: %s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s#%s\n",
+			  (value & 0x000080) ? " A2/EIAJ pilot tone "     : "",
+			  (value & 0x000100) ? " A2/EIAJ dual "           : "",
+			  (value & 0x000200) ? " A2/EIAJ stereo "         : "",
+			  (value & 0x000400) ? " A2/EIAJ noise mute "     : "",
+
+			  (value & 0x000800) ? " BTSC/FM radio pilot "    : "",
+			  (value & 0x001000) ? " SAP carrier "            : "",
+			  (value & 0x002000) ? " BTSC stereo noise mute " : "",
+			  (value & 0x004000) ? " SAP noise mute "         : "",
+			  (value & 0x008000) ? " VDSP "                   : "",
+
+			  (value & 0x010000) ? " NICST "                  : "",
+			  (value & 0x020000) ? " NICDU "                  : "",
+			  (value & 0x040000) ? " NICAM muted "            : "",
+			  (value & 0x080000) ? " NICAM reserve sound "    : "",
+
+			  (value & 0x100000) ? " init done "              : "");
 	}
 
  done:
@@ -1031,7 +1029,7 @@ int saa7134_tvaudio_init2(struct saa7134_dev *dev)
 		/* start tvaudio thread */
 		dev->thread.thread = kthread_run(my_thread, dev, "%s", dev->name);
 		if (IS_ERR(dev->thread.thread)) {
-			printk(KERN_WARNING "%s: kernel_thread() failed\n",
+			pr_warn("%s: kernel_thread() failed\n",
 			       dev->name);
 			/* XXX: missing error handling here */
 		}
@@ -1061,7 +1059,7 @@ int saa7134_tvaudio_fini(struct saa7134_dev *dev)
 int saa7134_tvaudio_do_scan(struct saa7134_dev *dev)
 {
 	if (dev->input->amux != TV) {
-		dprintk("sound IF not in use, skipping scan\n");
+		audio_dbg(1, "sound IF not in use, skipping scan\n");
 		dev->automute = 0;
 		saa7134_tvaudio_setmute(dev);
 	} else if (dev->thread.thread) {
diff --git a/drivers/media/pci/saa7134/saa7134-vbi.c b/drivers/media/pci/saa7134/saa7134-vbi.c
index 5306e549e526..4d36586ad752 100644
--- a/drivers/media/pci/saa7134/saa7134-vbi.c
+++ b/drivers/media/pci/saa7134/saa7134-vbi.c
@@ -20,14 +20,14 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
-
 /* ------------------------------------------------------------------ */
 
 static unsigned int vbi_debug;
@@ -38,8 +38,10 @@ static unsigned int vbibufs = 4;
 module_param(vbibufs, int, 0444);
 MODULE_PARM_DESC(vbibufs,"number of vbi buffers, range 2-32");
 
-#define dprintk(fmt, arg...)	if (vbi_debug) \
-	printk(KERN_DEBUG "%s/vbi: " fmt, dev->name , ## arg)
+#define vbi_dbg(fmt, arg...) do { \
+	if (vbi_debug) \
+		printk(KERN_DEBUG pr_fmt("vbi: " fmt), ## arg); \
+	} while (0)
 
 /* ------------------------------------------------------------------ */
 
@@ -84,7 +86,7 @@ static int buffer_activate(struct saa7134_dev *dev,
 	struct saa7134_dmaqueue *dmaq = buf->vb2.vb2_queue->drv_priv;
 	unsigned long control, base;
 
-	dprintk("buffer_activate [%p]\n", buf);
+	vbi_dbg("buffer_activate [%p]\n", buf);
 	buf->top_seen = 0;
 
 	task_init(dev, buf, TASK_A);
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 99d09a7566d3..035039cfae6d 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -20,6 +20,9 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "saa7134.h"
+#include "saa7134-reg.h"
+
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/module.h>
@@ -31,9 +34,6 @@
 #include <media/v4l2-event.h>
 #include <media/saa6588.h>
 
-#include "saa7134-reg.h"
-#include "saa7134.h"
-
 /* ------------------------------------------------------------------ */
 
 unsigned int video_debug;
@@ -52,8 +52,10 @@ module_param_string(secam, secam, sizeof(secam), 0644);
 MODULE_PARM_DESC(secam, "force SECAM variant, either DK,L or Lc");
 
 
-#define dprintk(fmt, arg...)	if (video_debug&0x04) \
-	printk(KERN_DEBUG "%s/video: " fmt, dev->name , ## arg)
+#define video_dbg(fmt, arg...) do { \
+	if (video_debug & 0x04) \
+		printk(KERN_DEBUG pr_fmt("video: " fmt), ## arg); \
+	} while (0)
 
 /* ------------------------------------------------------------------ */
 /* Defines for Video Output Port Register at address 0x191            */
@@ -385,7 +387,7 @@ static struct saa7134_format* format_by_fourcc(unsigned int fourcc)
 
 static void set_tvnorm(struct saa7134_dev *dev, struct saa7134_tvnorm *norm)
 {
-	dprintk("set tv norm = %s\n",norm->name);
+	video_dbg("set tv norm = %s\n", norm->name);
 	dev->tvnorm = norm;
 
 	/* setup cropping */
@@ -407,7 +409,7 @@ static void set_tvnorm(struct saa7134_dev *dev, struct saa7134_tvnorm *norm)
 
 static void video_mux(struct saa7134_dev *dev, int input)
 {
-	dprintk("video input = %d [%s]\n", input, card_in(dev, input).name);
+	video_dbg("video input = %d [%s]\n", input, card_in(dev, input).name);
 	dev->ctl_input = input;
 	set_tvnorm(dev, dev->tvnorm);
 	saa7134_tvaudio_setinput(dev, &card_in(dev, input));
@@ -531,14 +533,14 @@ static void set_v_scale(struct saa7134_dev *dev, int task, int yscale)
 	mirror = (dev->ctl_mirror) ? 0x02 : 0x00;
 	if (yscale < 2048) {
 		/* LPI */
-		dprintk("yscale LPI yscale=%d\n",yscale);
+		video_dbg("yscale LPI yscale=%d\n", yscale);
 		saa_writeb(SAA7134_V_FILTER(task), 0x00 | mirror);
 		saa_writeb(SAA7134_LUMA_CONTRAST(task), 0x40);
 		saa_writeb(SAA7134_CHROMA_SATURATION(task), 0x40);
 	} else {
 		/* ACM */
 		val = 0x40 * 1024 / yscale;
-		dprintk("yscale ACM yscale=%d val=0x%x\n",yscale,val);
+		video_dbg("yscale ACM yscale=%d val=0x%x\n", yscale, val);
 		saa_writeb(SAA7134_V_FILTER(task), 0x01 | mirror);
 		saa_writeb(SAA7134_LUMA_CONTRAST(task), val);
 		saa_writeb(SAA7134_CHROMA_SATURATION(task), val);
@@ -573,7 +575,8 @@ static void set_size(struct saa7134_dev *dev, int task,
 		prescale = 1;
 	xscale = 1024 * dev->crop_current.width / prescale / width;
 	yscale = 512 * div * dev->crop_current.height / height;
-	dprintk("prescale=%d xscale=%d yscale=%d\n",prescale,xscale,yscale);
+	video_dbg("prescale=%d xscale=%d yscale=%d\n",
+		  prescale, xscale, yscale);
 	set_h_prescale(dev,task,prescale);
 	saa_writeb(SAA7134_H_SCALE_INC1(task),      xscale &  0xff);
 	saa_writeb(SAA7134_H_SCALE_INC2(task),      xscale >> 8);
@@ -615,7 +618,7 @@ static void set_cliplist(struct saa7134_dev *dev, int reg,
 		saa_writeb(reg + 0, winbits);
 		saa_writeb(reg + 2, cl[i].position & 0xff);
 		saa_writeb(reg + 3, cl[i].position >> 8);
-		dprintk("clip: %s winbits=%02x pos=%d\n",
+		video_dbg("clip: %s winbits=%02x pos=%d\n",
 			name,winbits,cl[i].position);
 		reg += 8;
 	}
@@ -730,7 +733,7 @@ static int start_preview(struct saa7134_dev *dev)
 		return err;
 
 	dev->ovfield = dev->win.field;
-	dprintk("start_preview %dx%d+%d+%d %s field=%s\n",
+	video_dbg("start_preview %dx%d+%d+%d %s field=%s\n",
 		dev->win.w.width, dev->win.w.height,
 		dev->win.w.left, dev->win.w.top,
 		dev->ovfmt->name, v4l2_field_names[dev->ovfield]);
@@ -792,7 +795,7 @@ static int buffer_activate(struct saa7134_dev *dev,
 	unsigned long base,control,bpl;
 	unsigned long bpl_uv,lines_uv,base2,base3,tmp; /* planar */
 
-	dprintk("buffer_activate buf=%p\n",buf);
+	video_dbg("buffer_activate buf=%p\n", buf);
 	buf->top_seen = 0;
 
 	set_size(dev, TASK_A, dev->width, dev->height,
@@ -837,7 +840,7 @@ static int buffer_activate(struct saa7134_dev *dev,
 		base3    = base2 + bpl_uv * lines_uv;
 		if (dev->fmt->uvswap)
 			tmp = base2, base2 = base3, base3 = tmp;
-		dprintk("uv: bpl=%ld lines=%ld base2/3=%ld/%ld\n",
+		video_dbg("uv: bpl=%ld lines=%ld base2/3=%ld/%ld\n",
 			bpl_uv,lines_uv,base2,base3);
 		if (V4L2_FIELD_HAS_BOTH(dev->field)) {
 			/* interlaced */
@@ -1229,7 +1232,7 @@ static int saa7134_g_fmt_vid_overlay(struct file *file, void *priv,
 	int i;
 
 	if (saa7134_no_overlay > 0) {
-		printk(KERN_ERR "V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
+		pr_err("V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
 		return -EINVAL;
 	}
 	f->fmt.win = dev->win;
@@ -1305,7 +1308,7 @@ static int saa7134_try_fmt_vid_overlay(struct file *file, void *priv,
 	struct saa7134_dev *dev = video_drvdata(file);
 
 	if (saa7134_no_overlay > 0) {
-		printk(KERN_ERR "V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
+		pr_err("V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
 		return -EINVAL;
 	}
 
@@ -1339,7 +1342,7 @@ static int saa7134_s_fmt_vid_overlay(struct file *file, void *priv,
 	unsigned long flags;
 
 	if (saa7134_no_overlay > 0) {
-		printk(KERN_ERR "V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
+		pr_err("V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
 		return -EINVAL;
 	}
 	if (f->fmt.win.clips == NULL)
@@ -1738,7 +1741,7 @@ static int saa7134_enum_fmt_vid_overlay(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
 {
 	if (saa7134_no_overlay > 0) {
-		printk(KERN_ERR "V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
+		pr_err("V4L2_BUF_TYPE_VIDEO_OVERLAY: no_overlay\n");
 		return -EINVAL;
 	}
 
@@ -1795,7 +1798,7 @@ static int saa7134_overlay(struct file *file, void *priv, unsigned int on)
 
 	if (on) {
 		if (saa7134_no_overlay > 0) {
-			dprintk("no_overlay\n");
+			video_dbg("no_overlay\n");
 			return -EINVAL;
 		}
 
@@ -2184,7 +2187,7 @@ void saa7134_irq_video_signalchange(struct saa7134_dev *dev)
 
 	st1 = saa_readb(SAA7134_STATUS_VIDEO1);
 	st2 = saa_readb(SAA7134_STATUS_VIDEO2);
-	dprintk("DCSDT: pll: %s, sync: %s, norm: %s\n",
+	video_dbg("DCSDT: pll: %s, sync: %s, norm: %s\n",
 		(st1 & 0x40) ? "not locked" : "locked",
 		(st2 & 0x40) ? "no"         : "yes",
 		st[st1 & 0x03]);
diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
index 8bf0553b8d2f..6b5f6f45d285 100644
--- a/drivers/media/pci/saa7134/saa7134.h
+++ b/drivers/media/pci/saa7134/saa7134.h
@@ -21,6 +21,8 @@
 
 #define SAA7134_VERSION "0, 2, 17"
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/pci.h>
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
@@ -339,6 +341,7 @@ struct saa7134_card_ir {
 #define SAA7134_BOARD_HAWELL_HW_9004V1      191
 #define SAA7134_BOARD_AVERMEDIA_A706		192
 #define SAA7134_BOARD_WIS_VOYAGER           193
+#define SAA7134_BOARD_AVERMEDIA_505         194
 
 #define SAA7134_MAXBOARDS 32
 #define SAA7134_INPUT_MAX 8
@@ -654,7 +657,8 @@ struct saa7134_dev {
 	/* SAA7134_MPEG_DVB only */
 	struct vb2_dvb_frontends frontends;
 	int (*original_demod_sleep)(struct dvb_frontend *fe);
-	int (*original_set_voltage)(struct dvb_frontend *fe, fe_sec_voltage_t voltage);
+	int (*original_set_voltage)(struct dvb_frontend *fe,
+				    enum fe_sec_voltage voltage);
 	int (*original_set_high_voltage)(struct dvb_frontend *fe, long arg);
 #endif
 	void (*gate_ctrl)(struct saa7134_dev *dev, int open);
diff --git a/drivers/media/pci/saa7164/saa7164-api.c b/drivers/media/pci/saa7164/saa7164-api.c
index 4f3b1dd18ba4..e7e586c1ba53 100644
--- a/drivers/media/pci/saa7164/saa7164-api.c
+++ b/drivers/media/pci/saa7164/saa7164-api.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
@@ -1373,7 +1373,8 @@ int saa7164_api_i2c_read(struct saa7164_i2c *bus, u8 addr, u32 reglen, u8 *reg,
 	u8 buf[256];
 	int ret;
 
-	dprintk(DBGLVL_API, "%s()\n", __func__);
+	dprintk(DBGLVL_API, "%s() addr=%x reglen=%d datalen=%d\n",
+		__func__, addr, reglen, datalen);
 
 	if (reglen > 4)
 		return -EIO;
@@ -1434,7 +1435,8 @@ int saa7164_api_i2c_write(struct saa7164_i2c *bus, u8 addr, u32 datalen,
 	u8 buf[256];
 	int ret;
 
-	dprintk(DBGLVL_API, "%s()\n", __func__);
+	dprintk(DBGLVL_API, "%s() addr=0x%2x len=0x%x\n",
+		__func__, addr, datalen);
 
 	if ((datalen == 0) || (datalen > 232))
 		return -EIO;
@@ -1464,7 +1466,8 @@ int saa7164_api_i2c_write(struct saa7164_i2c *bus, u8 addr, u32 datalen,
 		return -EIO;
 	}
 
-	dprintk(DBGLVL_API, "%s() len = %d bytes\n", __func__, len);
+	dprintk(DBGLVL_API, "%s() len = %d bytes unitid=0x%x\n", __func__,
+		len, unitid);
 
 	/* Prepare the send buffer */
 	/* Bytes 00-03 dest register length
diff --git a/drivers/media/pci/saa7164/saa7164-buffer.c b/drivers/media/pci/saa7164/saa7164-buffer.c
index 9bd1f73f82da..f30758e24f5d 100644
--- a/drivers/media/pci/saa7164/saa7164-buffer.c
+++ b/drivers/media/pci/saa7164/saa7164-buffer.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
diff --git a/drivers/media/pci/saa7164/saa7164-bus.c b/drivers/media/pci/saa7164/saa7164-bus.c
index 6c73f5b155f6..a18fe5d47238 100644
--- a/drivers/media/pci/saa7164/saa7164-bus.c
+++ b/drivers/media/pci/saa7164/saa7164-bus.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
diff --git a/drivers/media/pci/saa7164/saa7164-cards.c b/drivers/media/pci/saa7164/saa7164-cards.c
index 5b72da5ce418..c2b738227f58 100644
--- a/drivers/media/pci/saa7164/saa7164-cards.c
+++ b/drivers/media/pci/saa7164/saa7164-cards.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
@@ -30,6 +30,7 @@
  * attached I2C devices, so we can simplify the virtual i2c mechansms
  * and keep the -i2c.c implementation clean.
  */
+#define REGLEN_0bit	0
 #define REGLEN_8bit	1
 #define REGLEN_16bit	2
 
@@ -499,6 +500,144 @@ struct saa7164_board saa7164_boards[] = {
 			.i2c_reg_len	= REGLEN_8bit,
 		} },
 	},
+	[SAA7164_BOARD_HAUPPAUGE_HVR2255proto] = {
+		.name		= "Hauppauge WinTV-HVR2255(proto)",
+		.porta		= SAA7164_MPEG_DVB,
+		.portb		= SAA7164_MPEG_DVB,
+		.portc		= SAA7164_MPEG_ENCODER,
+		.portd		= SAA7164_MPEG_ENCODER,
+		.porte		= SAA7164_MPEG_VBI,
+		.portf		= SAA7164_MPEG_VBI,
+		.chiprev	= SAA7164_CHIP_REV3,
+		.unit		= {{
+			.id		= 0x27,
+			.type		= SAA7164_UNIT_EEPROM,
+			.name		= "4K EEPROM",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_0,
+			.i2c_bus_addr	= 0xa0 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x04,
+			.type		= SAA7164_UNIT_TUNER,
+			.name		= "SI2157-1",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_0,
+			.i2c_bus_addr	= 0xc0 >> 1,
+			.i2c_reg_len	= REGLEN_0bit,
+		}, {
+			.id		= 0x06,
+			.type		= SAA7164_UNIT_DIGITAL_DEMODULATOR,
+			.name		= "LGDT3306",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0xb2 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x24,
+			.type		= SAA7164_UNIT_TUNER,
+			.name		= "SI2157-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_1,
+			.i2c_bus_addr	= 0xc0 >> 1,
+			.i2c_reg_len	= REGLEN_0bit,
+		}, {
+			.id		= 0x26,
+			.type		= SAA7164_UNIT_DIGITAL_DEMODULATOR,
+			.name		= "LGDT3306-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0x1c >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		} },
+	},
+	[SAA7164_BOARD_HAUPPAUGE_HVR2255] = {
+		.name		= "Hauppauge WinTV-HVR2255",
+		.porta		= SAA7164_MPEG_DVB,
+		.portb		= SAA7164_MPEG_DVB,
+		.portc		= SAA7164_MPEG_ENCODER,
+		.portd		= SAA7164_MPEG_ENCODER,
+		.porte		= SAA7164_MPEG_VBI,
+		.portf		= SAA7164_MPEG_VBI,
+		.chiprev	= SAA7164_CHIP_REV3,
+		.unit		= {{
+			.id		= 0x28,
+			.type		= SAA7164_UNIT_EEPROM,
+			.name		= "4K EEPROM",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_0,
+			.i2c_bus_addr	= 0xa0 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x04,
+			.type		= SAA7164_UNIT_TUNER,
+			.name		= "SI2157-1",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_0,
+			.i2c_bus_addr	= 0xc0 >> 1,
+			.i2c_reg_len	= REGLEN_0bit,
+		}, {
+			.id		= 0x06,
+			.type		= SAA7164_UNIT_DIGITAL_DEMODULATOR,
+			.name		= "LGDT3306-1",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0xb2 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x25,
+			.type		= SAA7164_UNIT_TUNER,
+			.name		= "SI2157-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_1,
+			.i2c_bus_addr	= 0xc0 >> 1,
+			.i2c_reg_len	= REGLEN_0bit,
+		}, {
+			.id		= 0x27,
+			.type		= SAA7164_UNIT_DIGITAL_DEMODULATOR,
+			.name		= "LGDT3306-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0x1c >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		} },
+	},
+	[SAA7164_BOARD_HAUPPAUGE_HVR2205] = {
+		.name		= "Hauppauge WinTV-HVR2205",
+		.porta		= SAA7164_MPEG_DVB,
+		.portb		= SAA7164_MPEG_DVB,
+		.portc		= SAA7164_MPEG_ENCODER,
+		.portd		= SAA7164_MPEG_ENCODER,
+		.porte		= SAA7164_MPEG_VBI,
+		.portf		= SAA7164_MPEG_VBI,
+		.chiprev	= SAA7164_CHIP_REV3,
+		.unit		= {{
+			.id		= 0x28,
+			.type		= SAA7164_UNIT_EEPROM,
+			.name		= "4K EEPROM",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_0,
+			.i2c_bus_addr	= 0xa0 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x04,
+			.type		= SAA7164_UNIT_TUNER,
+			.name		= "SI2157-1",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_0,
+			.i2c_bus_addr	= 0xc0 >> 1,
+			.i2c_reg_len	= REGLEN_0bit,
+		}, {
+			.id		= 0x06,
+			.type		= SAA7164_UNIT_DIGITAL_DEMODULATOR,
+			.name		= "SI2168-1",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0xc8 >> 1,
+			.i2c_reg_len	= REGLEN_0bit,
+		}, {
+			.id		= 0x25,
+			.type		= SAA7164_UNIT_TUNER,
+			.name		= "SI2157-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_1,
+			.i2c_bus_addr	= 0xc0 >> 1,
+			.i2c_reg_len	= REGLEN_0bit,
+		}, {
+			.id		= 0x27,
+			.type		= SAA7164_UNIT_DIGITAL_DEMODULATOR,
+			.name		= "SI2168-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0xcc >> 1,
+			.i2c_reg_len	= REGLEN_0bit,
+		} },
+	},
 };
 const unsigned int saa7164_bcount = ARRAY_SIZE(saa7164_boards);
 
@@ -546,6 +685,21 @@ struct saa7164_subid saa7164_subids[] = {
 		.subvendor = 0x0070,
 		.subdevice = 0x8953,
 		.card      = SAA7164_BOARD_HAUPPAUGE_HVR2200_5,
+	}, {
+		.subvendor = 0x0070,
+		.subdevice = 0xf111,
+		.card      = SAA7164_BOARD_HAUPPAUGE_HVR2255,
+		/* Prototype card left here for documenation purposes.
+		.card      = SAA7164_BOARD_HAUPPAUGE_HVR2255proto,
+		*/
+	}, {
+		.subvendor = 0x0070,
+		.subdevice = 0xf123,
+		.card      = SAA7164_BOARD_HAUPPAUGE_HVR2205,
+	}, {
+		.subvendor = 0x0070,
+		.subdevice = 0xf120,
+		.card      = SAA7164_BOARD_HAUPPAUGE_HVR2205,
 	},
 };
 const unsigned int saa7164_idcount = ARRAY_SIZE(saa7164_subids);
@@ -594,12 +748,26 @@ void saa7164_gpio_setup(struct saa7164_dev *dev)
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250_2:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250_3:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2255proto:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2255:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2205:
 		/*
+		HVR2200 / HVR2250
 		GPIO 2: s5h1411 / tda10048-1 demod reset
 		GPIO 3: s5h1411 / tda10048-2 demod reset
 		GPIO 7: IRBlaster Zilog reset
 		 */
 
+		/* HVR2255
+		 * GPIO 2: lgdg3306-1 demod reset
+		 * GPIO 3: lgdt3306-2 demod reset
+		 */
+
+		/* HVR2205
+		 * GPIO 2: si2168-1 demod reset
+		 * GPIO 3: si2168-2 demod reset
+		 */
+
 		/* Reset parts by going in and out of reset */
 		saa7164_api_clear_gpiobit(dev, PCIEBRIDGE_UNITID, 2);
 		saa7164_api_clear_gpiobit(dev, PCIEBRIDGE_UNITID, 3);
@@ -647,6 +815,21 @@ static void hauppauge_eeprom(struct saa7164_dev *dev, u8 *eeprom_data)
 		/* WinTV-HVR2200 (PCIe, Retail, half-height)
 		 * DVB-T (TDA18271/TDA10048) and basic analog, no IR */
 		break;
+	case 151009:
+		/* First production board rev B2I6 */
+		/* WinTV-HVR2205 (PCIe, Retail, full-height bracket)
+		 * DVB-T/T2/C (SI2157/SI2168) and basic analog, FM */
+		break;
+	case 151609:
+		/* First production board rev B2I6 */
+		/* WinTV-HVR2205 (PCIe, Retail, half-height bracket)
+		 * DVB-T/T2/C (SI2157/SI2168) and basic analog, FM */
+		break;
+	case 151061:
+		/* First production board rev B1I6 */
+		/* WinTV-HVR2255 (PCIe, Retail, full-height bracket)
+		 * ATSC/QAM (SI2157/LGDT3306) and basic analog, FM */
+		break;
 	default:
 		printk(KERN_ERR "%s: Warning: Unknown Hauppauge model #%d\n",
 			dev->name, tv.model);
@@ -676,6 +859,9 @@ void saa7164_card_setup(struct saa7164_dev *dev)
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250_2:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250_3:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2255proto:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2255:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2205:
 		hauppauge_eeprom(dev, &eeprom[0]);
 		break;
 	}
diff --git a/drivers/media/pci/saa7164/saa7164-cmd.c b/drivers/media/pci/saa7164/saa7164-cmd.c
index cfabcbacc33d..3285c37b4583 100644
--- a/drivers/media/pci/saa7164/saa7164-cmd.c
+++ b/drivers/media/pci/saa7164/saa7164-cmd.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
diff --git a/drivers/media/pci/saa7164/saa7164-core.c b/drivers/media/pci/saa7164/saa7164-core.c
index 9cf3c6cba498..3206a826b80d 100644
--- a/drivers/media/pci/saa7164/saa7164-core.c
+++ b/drivers/media/pci/saa7164/saa7164-core.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
@@ -85,6 +85,11 @@ module_param(guard_checking, int, 0644);
 MODULE_PARM_DESC(guard_checking,
 	"enable dma sanity checking for buffer overruns");
 
+static bool enable_msi = true;
+module_param(enable_msi, bool, 0444);
+MODULE_PARM_DESC(enable_msi,
+		"enable the use of an msi interrupt if available");
+
 static unsigned int saa7164_devcount;
 
 static DEFINE_MUTEX(devlist);
@@ -618,12 +623,7 @@ static irqreturn_t saa7164_irq_ts(struct saa7164_port *port)
 static irqreturn_t saa7164_irq(int irq, void *dev_id)
 {
 	struct saa7164_dev *dev = dev_id;
-	struct saa7164_port *porta = &dev->ports[SAA7164_PORT_TS1];
-	struct saa7164_port *portb = &dev->ports[SAA7164_PORT_TS2];
-	struct saa7164_port *portc = &dev->ports[SAA7164_PORT_ENC1];
-	struct saa7164_port *portd = &dev->ports[SAA7164_PORT_ENC2];
-	struct saa7164_port *porte = &dev->ports[SAA7164_PORT_VBI1];
-	struct saa7164_port *portf = &dev->ports[SAA7164_PORT_VBI2];
+	struct saa7164_port *porta, *portb, *portc, *portd, *porte, *portf;
 
 	u32 intid, intstat[INT_SIZE/4];
 	int i, handled = 0, bit;
@@ -634,6 +634,13 @@ static irqreturn_t saa7164_irq(int irq, void *dev_id)
 		goto out;
 	}
 
+	porta = &dev->ports[SAA7164_PORT_TS1];
+	portb = &dev->ports[SAA7164_PORT_TS2];
+	portc = &dev->ports[SAA7164_PORT_ENC1];
+	portd = &dev->ports[SAA7164_PORT_ENC2];
+	porte = &dev->ports[SAA7164_PORT_VBI1];
+	portf = &dev->ports[SAA7164_PORT_VBI2];
+
 	/* Check that the hardware is accessible. If the status bytes are
 	 * 0xFF then the device is not accessible, the the IRQ belongs
 	 * to another driver.
@@ -1184,6 +1191,39 @@ static int saa7164_thread_function(void *data)
 	return 0;
 }
 
+static bool saa7164_enable_msi(struct pci_dev *pci_dev, struct saa7164_dev *dev)
+{
+	int err;
+
+	if (!enable_msi) {
+		printk(KERN_WARNING "%s() MSI disabled by module parameter 'enable_msi'"
+		       , __func__);
+		return false;
+	}
+
+	err = pci_enable_msi(pci_dev);
+
+	if (err) {
+		printk(KERN_ERR "%s() Failed to enable MSI interrupt."
+			" Falling back to a shared IRQ\n", __func__);
+		return false;
+	}
+
+	/* no error - so request an msi interrupt */
+	err = request_irq(pci_dev->irq, saa7164_irq, 0,
+						dev->name, dev);
+
+	if (err) {
+		/* fall back to legacy interrupt */
+		printk(KERN_ERR "%s() Failed to get an MSI interrupt."
+		       " Falling back to a shared IRQ\n", __func__);
+		pci_disable_msi(pci_dev);
+		return false;
+	}
+
+	return true;
+}
+
 static int saa7164_initdev(struct pci_dev *pci_dev,
 			   const struct pci_device_id *pci_id)
 {
@@ -1230,13 +1270,22 @@ static int saa7164_initdev(struct pci_dev *pci_dev,
 		goto fail_irq;
 	}
 
-	err = request_irq(pci_dev->irq, saa7164_irq,
-		IRQF_SHARED, dev->name, dev);
-	if (err < 0) {
-		printk(KERN_ERR "%s: can't get IRQ %d\n", dev->name,
-			pci_dev->irq);
-		err = -EIO;
-		goto fail_irq;
+	/* irq bit */
+	if (saa7164_enable_msi(pci_dev, dev)) {
+		dev->msi = true;
+	} else {
+		/* if we have an error (i.e. we don't have an interrupt)
+			 or msi is not enabled - fallback to shared interrupt */
+
+		err = request_irq(pci_dev->irq, saa7164_irq,
+				IRQF_SHARED, dev->name, dev);
+
+		if (err < 0) {
+			printk(KERN_ERR "%s: can't get IRQ %d\n", dev->name,
+			       pci_dev->irq);
+			err = -EIO;
+			goto fail_irq;
+		}
 	}
 
 	pci_set_drvdata(pci_dev, dev);
@@ -1439,6 +1488,11 @@ static void saa7164_finidev(struct pci_dev *pci_dev)
 	/* unregister stuff */
 	free_irq(pci_dev->irq, dev);
 
+	if (dev->msi) {
+		pci_disable_msi(pci_dev);
+		dev->msi = false;
+	}
+
 	pci_disable_device(pci_dev);
 
 	mutex_lock(&devlist);
diff --git a/drivers/media/pci/saa7164/saa7164-dvb.c b/drivers/media/pci/saa7164/saa7164-dvb.c
index 16ae71592e8c..e9a783b71b45 100644
--- a/drivers/media/pci/saa7164/saa7164-dvb.c
+++ b/drivers/media/pci/saa7164/saa7164-dvb.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
@@ -24,6 +24,9 @@
 #include "tda10048.h"
 #include "tda18271.h"
 #include "s5h1411.h"
+#include "si2157.h"
+#include "si2168.h"
+#include "lgdt3306a.h"
 
 #define DRIVER_NAME "saa7164"
 
@@ -82,6 +85,65 @@ static struct s5h1411_config hauppauge_s5h1411_config = {
 	.mpeg_timing   = S5H1411_MPEGTIMING_CONTINOUS_NONINVERTING_CLOCK,
 };
 
+static struct lgdt3306a_config hauppauge_hvr2255a_config = {
+	.i2c_addr               = 0xb2 >> 1,
+	.qam_if_khz             = 4000,
+	.vsb_if_khz             = 3250,
+	.deny_i2c_rptr          = 1, /* Disabled */
+	.spectral_inversion     = 0, /* Disabled */
+	.mpeg_mode              = LGDT3306A_MPEG_SERIAL,
+	.tpclk_edge             = LGDT3306A_TPCLK_RISING_EDGE,
+	.tpvalid_polarity       = LGDT3306A_TP_VALID_HIGH,
+	.xtalMHz                = 25, /* 24 or 25 */
+};
+
+static struct lgdt3306a_config hauppauge_hvr2255b_config = {
+	.i2c_addr               = 0x1c >> 1,
+	.qam_if_khz             = 4000,
+	.vsb_if_khz             = 3250,
+	.deny_i2c_rptr          = 1, /* Disabled */
+	.spectral_inversion     = 0, /* Disabled */
+	.mpeg_mode              = LGDT3306A_MPEG_SERIAL,
+	.tpclk_edge             = LGDT3306A_TPCLK_RISING_EDGE,
+	.tpvalid_polarity       = LGDT3306A_TP_VALID_HIGH,
+	.xtalMHz                = 25, /* 24 or 25 */
+};
+
+static struct si2157_config hauppauge_hvr2255_tuner_config = {
+	.inversion = 1,
+	.if_port = 1,
+};
+
+static int si2157_attach(struct saa7164_port *port, struct i2c_adapter *adapter,
+	struct dvb_frontend *fe, u8 addr8bit, struct si2157_config *cfg)
+{
+	struct i2c_board_info bi;
+	struct i2c_client *tuner;
+
+	cfg->fe = fe;
+
+	memset(&bi, 0, sizeof(bi));
+
+	strlcpy(bi.type, "si2157", I2C_NAME_SIZE);
+	bi.platform_data = cfg;
+	bi.addr = addr8bit >> 1;
+
+	request_module(bi.type);
+
+	tuner = i2c_new_device(adapter, &bi);
+	if (tuner == NULL || tuner->dev.driver == NULL)
+		return -ENODEV;
+
+	if (!try_module_get(tuner->dev.driver->owner)) {
+		i2c_unregister_device(tuner);
+		return -ENODEV;
+	}
+
+	port->i2c_client_tuner = tuner;
+
+	return 0;
+}
+
 static int saa7164_dvb_stop_port(struct saa7164_port *port)
 {
 	struct saa7164_dev *dev = port->dev;
@@ -242,14 +304,16 @@ static int saa7164_dvb_start_feed(struct dvb_demux_feed *feed)
 	if (!demux->dmx.frontend)
 		return -EINVAL;
 
-	mutex_lock(&dvb->lock);
-	if (dvb->feeding++ == 0) {
-		/* Start transport */
-		ret = saa7164_dvb_start_port(port);
+	if (dvb) {
+		mutex_lock(&dvb->lock);
+		if (dvb->feeding++ == 0) {
+			/* Start transport */
+			ret = saa7164_dvb_start_port(port);
+		}
+		mutex_unlock(&dvb->lock);
+		dprintk(DBGLVL_DVB, "%s(port=%d) now feeding = %d\n",
+			__func__, port->nr, dvb->feeding);
 	}
-	mutex_unlock(&dvb->lock);
-	dprintk(DBGLVL_DVB, "%s(port=%d) now feeding = %d\n",
-		__func__, port->nr, dvb->feeding);
 
 	return ret;
 }
@@ -264,14 +328,16 @@ static int saa7164_dvb_stop_feed(struct dvb_demux_feed *feed)
 
 	dprintk(DBGLVL_DVB, "%s(port=%d)\n", __func__, port->nr);
 
-	mutex_lock(&dvb->lock);
-	if (--dvb->feeding == 0) {
-		/* Stop transport */
-		ret = saa7164_dvb_stop_streaming(port);
+	if (dvb) {
+		mutex_lock(&dvb->lock);
+		if (--dvb->feeding == 0) {
+			/* Stop transport */
+			ret = saa7164_dvb_stop_streaming(port);
+		}
+		mutex_unlock(&dvb->lock);
+		dprintk(DBGLVL_DVB, "%s(port=%d) now feeding = %d\n",
+			__func__, port->nr, dvb->feeding);
 	}
-	mutex_unlock(&dvb->lock);
-	dprintk(DBGLVL_DVB, "%s(port=%d) now feeding = %d\n",
-		__func__, port->nr, dvb->feeding);
 
 	return ret;
 }
@@ -425,6 +491,7 @@ int saa7164_dvb_unregister(struct saa7164_port *port)
 	struct saa7164_dev *dev = port->dev;
 	struct saa7164_buffer *b;
 	struct list_head *c, *n;
+	struct i2c_client *client;
 
 	dprintk(DBGLVL_DVB, "%s()\n", __func__);
 
@@ -443,6 +510,20 @@ int saa7164_dvb_unregister(struct saa7164_port *port)
 	if (dvb->frontend == NULL)
 		return 0;
 
+	/* remove I2C client for tuner */
+	client = port->i2c_client_tuner;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+
+	/* remove I2C client for demodulator */
+	client = port->i2c_client_demod;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+
 	dvb_net_release(&dvb->net);
 	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_mem);
 	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_hw);
@@ -462,6 +543,12 @@ int saa7164_dvb_register(struct saa7164_port *port)
 	struct saa7164_dev *dev = port->dev;
 	struct saa7164_dvb *dvb = &port->dvb;
 	struct saa7164_i2c *i2c_bus = NULL;
+	struct si2168_config si2168_config;
+	struct si2157_config si2157_config;
+	struct i2c_adapter *adapter;
+	struct i2c_board_info info;
+	struct i2c_client *client_demod;
+	struct i2c_client *client_tuner;
 	int ret;
 
 	dprintk(DBGLVL_DVB, "%s()\n", __func__);
@@ -528,6 +615,126 @@ int saa7164_dvb_register(struct saa7164_port *port)
 		}
 
 		break;
+	case SAA7164_BOARD_HAUPPAUGE_HVR2255proto:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2255:
+		i2c_bus = &dev->i2c_bus[2];
+
+		if (port->nr == 0) {
+			port->dvb.frontend = dvb_attach(lgdt3306a_attach,
+				&hauppauge_hvr2255a_config, &i2c_bus->i2c_adap);
+		} else {
+			port->dvb.frontend = dvb_attach(lgdt3306a_attach,
+				&hauppauge_hvr2255b_config, &i2c_bus->i2c_adap);
+		}
+
+		if (port->dvb.frontend != NULL) {
+
+			if (port->nr == 0) {
+				si2157_attach(port, &dev->i2c_bus[0].i2c_adap,
+					      port->dvb.frontend, 0xc0,
+					      &hauppauge_hvr2255_tuner_config);
+			} else {
+				si2157_attach(port, &dev->i2c_bus[1].i2c_adap,
+					      port->dvb.frontend, 0xc0,
+					      &hauppauge_hvr2255_tuner_config);
+			}
+		}
+		break;
+	case SAA7164_BOARD_HAUPPAUGE_HVR2205:
+
+		if (port->nr == 0) {
+			/* attach frontend */
+			memset(&si2168_config, 0, sizeof(si2168_config));
+			si2168_config.i2c_adapter = &adapter;
+			si2168_config.fe = &port->dvb.frontend;
+			si2168_config.ts_mode = SI2168_TS_SERIAL;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2168", I2C_NAME_SIZE);
+			info.addr = 0xc8 >> 1;
+			info.platform_data = &si2168_config;
+			request_module(info.type);
+			client_demod = i2c_new_device(&dev->i2c_bus[2].i2c_adap,
+						      &info);
+			if (!client_demod || !client_demod->dev.driver)
+				goto frontend_detach;
+
+			if (!try_module_get(client_demod->dev.driver->owner)) {
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			port->i2c_client_demod = client_demod;
+
+			/* attach tuner */
+			memset(&si2157_config, 0, sizeof(si2157_config));
+			si2157_config.if_port = 1;
+			si2157_config.fe = port->dvb.frontend;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
+			info.addr = 0xc0 >> 1;
+			info.platform_data = &si2157_config;
+			request_module(info.type);
+			client_tuner = i2c_new_device(&dev->i2c_bus[0].i2c_adap,
+						      &info);
+			if (!client_tuner || !client_tuner->dev.driver) {
+				module_put(client_demod->dev.driver->owner);
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			if (!try_module_get(client_tuner->dev.driver->owner)) {
+				i2c_unregister_device(client_tuner);
+				module_put(client_demod->dev.driver->owner);
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			port->i2c_client_tuner = client_tuner;
+		} else {
+			/* attach frontend */
+			memset(&si2168_config, 0, sizeof(si2168_config));
+			si2168_config.i2c_adapter = &adapter;
+			si2168_config.fe = &port->dvb.frontend;
+			si2168_config.ts_mode = SI2168_TS_SERIAL;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2168", I2C_NAME_SIZE);
+			info.addr = 0xcc >> 1;
+			info.platform_data = &si2168_config;
+			request_module(info.type);
+			client_demod = i2c_new_device(&dev->i2c_bus[2].i2c_adap,
+						      &info);
+			if (!client_demod || !client_demod->dev.driver)
+				goto frontend_detach;
+
+			if (!try_module_get(client_demod->dev.driver->owner)) {
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			port->i2c_client_demod = client_demod;
+
+			/* attach tuner */
+			memset(&si2157_config, 0, sizeof(si2157_config));
+			si2157_config.fe = port->dvb.frontend;
+			si2157_config.if_port = 1;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
+			info.addr = 0xc0 >> 1;
+			info.platform_data = &si2157_config;
+			request_module(info.type);
+			client_tuner = i2c_new_device(&dev->i2c_bus[1].i2c_adap,
+						      &info);
+			if (!client_tuner || !client_tuner->dev.driver) {
+				module_put(client_demod->dev.driver->owner);
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			if (!try_module_get(client_tuner->dev.driver->owner)) {
+				i2c_unregister_device(client_tuner);
+				module_put(client_demod->dev.driver->owner);
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			port->i2c_client_tuner = client_tuner;
+		}
+
+		break;
 	default:
 		printk(KERN_ERR "%s: The frontend isn't supported\n",
 		       dev->name);
@@ -548,5 +755,9 @@ int saa7164_dvb_register(struct saa7164_port *port)
 	}
 
 	return 0;
+
+frontend_detach:
+	printk(KERN_ERR "%s() Frontend/I2C initialization failed\n", __func__);
+	return -1;
 }
 
diff --git a/drivers/media/pci/saa7164/saa7164-encoder.c b/drivers/media/pci/saa7164/saa7164-encoder.c
index 9266965412c3..4434e0f28c26 100644
--- a/drivers/media/pci/saa7164/saa7164-encoder.c
+++ b/drivers/media/pci/saa7164/saa7164-encoder.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
@@ -721,13 +721,14 @@ static int vidioc_querycap(struct file *file, void  *priv,
 		sizeof(cap->card));
 	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
 
-	cap->capabilities =
+	cap->device_caps =
 		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_READWRITE     |
-		0;
+		V4L2_CAP_READWRITE |
+		V4L2_CAP_TUNER;
 
-	cap->capabilities |= V4L2_CAP_TUNER;
-	cap->version = 0;
+	cap->capabilities = cap->device_caps |
+		V4L2_CAP_VBI_CAPTURE |
+		V4L2_CAP_DEVICE_CAPS;
 
 	return 0;
 }
diff --git a/drivers/media/pci/saa7164/saa7164-fw.c b/drivers/media/pci/saa7164/saa7164-fw.c
index add06ab5124d..269e0782c7b6 100644
--- a/drivers/media/pci/saa7164/saa7164-fw.c
+++ b/drivers/media/pci/saa7164/saa7164-fw.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
diff --git a/drivers/media/pci/saa7164/saa7164-i2c.c b/drivers/media/pci/saa7164/saa7164-i2c.c
index 4f7e3b42263f..0342d84913b8 100644
--- a/drivers/media/pci/saa7164/saa7164-i2c.c
+++ b/drivers/media/pci/saa7164/saa7164-i2c.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
@@ -39,9 +39,10 @@ static int i2c_xfer(struct i2c_adapter *i2c_adap, struct i2c_msg *msgs, int num)
 		dprintk(DBGLVL_I2C, "%s(num = %d) addr = 0x%02x  len = 0x%x\n",
 			__func__, num, msgs[i].addr, msgs[i].len);
 		if (msgs[i].flags & I2C_M_RD) {
-			/* Unsupported - Yet*/
-			printk(KERN_ERR "%s() Unsupported - Yet\n", __func__);
-			continue;
+			retval = saa7164_api_i2c_read(bus,
+				msgs[i].addr,
+				0 /* reglen */,
+				NULL /* reg */, msgs[i].len, msgs[i].buf);
 		} else if (i + 1 < num && (msgs[i + 1].flags & I2C_M_RD) &&
 			   msgs[i].addr == msgs[i + 1].addr) {
 			/* write then read from same address */
diff --git a/drivers/media/pci/saa7164/saa7164-reg.h b/drivers/media/pci/saa7164/saa7164-reg.h
index 2bbf81583d33..37521a2ee504 100644
--- a/drivers/media/pci/saa7164/saa7164-reg.h
+++ b/drivers/media/pci/saa7164/saa7164-reg.h
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
diff --git a/drivers/media/pci/saa7164/saa7164-types.h b/drivers/media/pci/saa7164/saa7164-types.h
index f48ba978f835..1efba6c64ebf 100644
--- a/drivers/media/pci/saa7164/saa7164-types.h
+++ b/drivers/media/pci/saa7164/saa7164-types.h
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
diff --git a/drivers/media/pci/saa7164/saa7164-vbi.c b/drivers/media/pci/saa7164/saa7164-vbi.c
index 6e025fea2542..859fd03d82f9 100644
--- a/drivers/media/pci/saa7164/saa7164-vbi.c
+++ b/drivers/media/pci/saa7164/saa7164-vbi.c
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
@@ -660,13 +660,14 @@ static int vidioc_querycap(struct file *file, void  *priv,
 		sizeof(cap->card));
 	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
 
-	cap->capabilities =
+	cap->device_caps =
 		V4L2_CAP_VBI_CAPTURE |
-		V4L2_CAP_READWRITE     |
-		0;
+		V4L2_CAP_READWRITE |
+		V4L2_CAP_TUNER;
 
-	cap->capabilities |= V4L2_CAP_TUNER;
-	cap->version = 0;
+	cap->capabilities = cap->device_caps |
+		V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_DEVICE_CAPS;
 
 	return 0;
 }
diff --git a/drivers/media/pci/saa7164/saa7164.h b/drivers/media/pci/saa7164/saa7164.h
index cd1a07ce27cb..18906e0c80e1 100644
--- a/drivers/media/pci/saa7164/saa7164.h
+++ b/drivers/media/pci/saa7164/saa7164.h
@@ -1,7 +1,7 @@
 /*
  *  Driver for the NXP SAA7164 PCIe bridge
  *
- *  Copyright (c) 2010 Steven Toth <stoth@kernellabs.com>
+ *  Copyright (c) 2010-2015 Steven Toth <stoth@kernellabs.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
@@ -83,6 +83,9 @@
 #define SAA7164_BOARD_HAUPPAUGE_HVR2250_3	8
 #define SAA7164_BOARD_HAUPPAUGE_HVR2200_4	9
 #define SAA7164_BOARD_HAUPPAUGE_HVR2200_5	10
+#define SAA7164_BOARD_HAUPPAUGE_HVR2255proto	11
+#define SAA7164_BOARD_HAUPPAUGE_HVR2255		12
+#define SAA7164_BOARD_HAUPPAUGE_HVR2205		13
 
 #define SAA7164_MAX_UNITS		8
 #define SAA7164_TS_NUMBER_OF_LINES	312
@@ -371,6 +374,8 @@ struct saa7164_port {
 
 	/* --- DVB Transport Specific --- */
 	struct saa7164_dvb dvb;
+	struct i2c_client *i2c_client_demod;
+	struct i2c_client *i2c_client_tuner;
 
 	/* --- Encoder/V4L related attributes --- */
 	/* Encoder */
@@ -459,6 +464,7 @@ struct saa7164_dev {
 	/* Interrupt status and ack registers */
 	u32 int_status;
 	u32 int_ack;
+	bool msi;
 
 	struct cmd			cmds[SAA_CMD_MAX_MSG_UNITS];
 	struct mutex			lock;
diff --git a/drivers/media/pci/smipcie/smipcie.c b/drivers/media/pci/smipcie/smipcie.c
index 411592524c63..143fd7899ecd 100644
--- a/drivers/media/pci/smipcie/smipcie.c
+++ b/drivers/media/pci/smipcie/smipcie.c
@@ -657,6 +657,7 @@ static int smi_dvbsky_sit2_fe_attach(struct smi_port *port)
 	/* attach tuner */
 	memset(&si2157_config, 0, sizeof(si2157_config));
 	si2157_config.fe = port->fe;
+	si2157_config.if_port = 1;
 
 	memset(&client_info, 0, sizeof(struct i2c_board_info));
 	strlcpy(client_info.type, "si2157", I2C_NAME_SIZE);
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.c b/drivers/media/pci/sta2x11/sta2x11_vip.c
index d384a6b0b09f..59b3a36a3639 100644
--- a/drivers/media/pci/sta2x11/sta2x11_vip.c
+++ b/drivers/media/pci/sta2x11/sta2x11_vip.c
@@ -813,7 +813,7 @@ static irqreturn_t vip_irq(int irq, struct sta2x11_vip *vip)
 		/* Disable acquisition */
 		reg_write(vip, DVP_CTL, reg_read(vip, DVP_CTL) & ~DVP_CTL_ENA);
 		/* Remove the active buffer from the list */
-		do_gettimeofday(&vip->active->vb.v4l2_buf.timestamp);
+		v4l2_get_timestamp(&vip->active->vb.v4l2_buf.timestamp);
 		vip->active->vb.v4l2_buf.sequence = vip->sequence++;
 		vb2_buffer_done(&vip->active->vb, VB2_BUF_STATE_DONE);
 	}
@@ -864,6 +864,7 @@ static int sta2x11_vip_init_buffer(struct sta2x11_vip *vip)
 	vip->vb_vidq.buf_struct_size = sizeof(struct vip_buffer);
 	vip->vb_vidq.ops = &vip_video_qops;
 	vip->vb_vidq.mem_ops = &vb2_dma_contig_memops;
+	vip->vb_vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	err = vb2_queue_init(&vip->vb_vidq);
 	if (err)
 		return err;
diff --git a/drivers/media/pci/ttpci/av7110.c b/drivers/media/pci/ttpci/av7110.c
index 45199a12b9d9..3f24fce74fc1 100644
--- a/drivers/media/pci/ttpci/av7110.c
+++ b/drivers/media/pci/ttpci/av7110.c
@@ -1172,7 +1172,7 @@ static int dvb_get_stc(struct dmx_demux *demux, unsigned int num,
  ******************************************************************************/
 
 
-static int av7110_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int av7110_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct av7110* av7110 = fe->dvb->priv;
 
@@ -1197,7 +1197,7 @@ static int av7110_diseqc_send_master_cmd(struct dvb_frontend* fe,
 }
 
 static int av7110_diseqc_send_burst(struct dvb_frontend* fe,
-				    fe_sec_mini_cmd_t minicmd)
+				    enum fe_sec_mini_cmd minicmd)
 {
 	struct av7110* av7110 = fe->dvb->priv;
 
@@ -1946,7 +1946,7 @@ static struct l64781_config grundig_29504_401_config = {
 
 
 
-static int av7110_fe_lock_fix(struct av7110* av7110, fe_status_t status)
+static int av7110_fe_lock_fix(struct av7110 *av7110, enum fe_status status)
 {
 	int ret = 0;
 	int synced = (status & FE_HAS_LOCK) ? 1 : 0;
@@ -2008,7 +2008,8 @@ static int av7110_fe_init(struct dvb_frontend* fe)
 	return ret;
 }
 
-static int av7110_fe_read_status(struct dvb_frontend* fe, fe_status_t* status)
+static int av7110_fe_read_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
 {
 	struct av7110* av7110 = fe->dvb->priv;
 
@@ -2043,7 +2044,8 @@ static int av7110_fe_diseqc_send_master_cmd(struct dvb_frontend* fe,
 	return ret;
 }
 
-static int av7110_fe_diseqc_send_burst(struct dvb_frontend* fe, fe_sec_mini_cmd_t minicmd)
+static int av7110_fe_diseqc_send_burst(struct dvb_frontend *fe,
+				       enum fe_sec_mini_cmd minicmd)
 {
 	struct av7110* av7110 = fe->dvb->priv;
 
@@ -2055,7 +2057,8 @@ static int av7110_fe_diseqc_send_burst(struct dvb_frontend* fe, fe_sec_mini_cmd_
 	return ret;
 }
 
-static int av7110_fe_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int av7110_fe_set_tone(struct dvb_frontend *fe,
+			      enum fe_sec_tone_mode tone)
 {
 	struct av7110* av7110 = fe->dvb->priv;
 
@@ -2067,7 +2070,8 @@ static int av7110_fe_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
 	return ret;
 }
 
-static int av7110_fe_set_voltage(struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int av7110_fe_set_voltage(struct dvb_frontend *fe,
+				 enum fe_sec_voltage voltage)
 {
 	struct av7110* av7110 = fe->dvb->priv;
 
diff --git a/drivers/media/pci/ttpci/av7110.h b/drivers/media/pci/ttpci/av7110.h
index 835635b0c712..3a55927edb95 100644
--- a/drivers/media/pci/ttpci/av7110.h
+++ b/drivers/media/pci/ttpci/av7110.h
@@ -269,25 +269,30 @@ struct av7110 {
 	unsigned long size_root;
 
 	struct dvb_frontend* fe;
-	fe_status_t fe_status;
+	enum fe_status fe_status;
 
 	struct mutex ioctl_mutex;
 
 	/* crash recovery */
 	void				(*recover)(struct av7110* av7110);
-	fe_sec_voltage_t		saved_voltage;
-	fe_sec_tone_mode_t		saved_tone;
+	enum fe_sec_voltage		saved_voltage;
+	enum fe_sec_tone_mode		saved_tone;
 	struct dvb_diseqc_master_cmd	saved_master_cmd;
-	fe_sec_mini_cmd_t		saved_minicmd;
+	enum fe_sec_mini_cmd		saved_minicmd;
 
 	int (*fe_init)(struct dvb_frontend* fe);
-	int (*fe_read_status)(struct dvb_frontend* fe, fe_status_t* status);
-	int (*fe_diseqc_reset_overload)(struct dvb_frontend* fe);
-	int (*fe_diseqc_send_master_cmd)(struct dvb_frontend* fe, struct dvb_diseqc_master_cmd* cmd);
-	int (*fe_diseqc_send_burst)(struct dvb_frontend* fe, fe_sec_mini_cmd_t minicmd);
-	int (*fe_set_tone)(struct dvb_frontend* fe, fe_sec_tone_mode_t tone);
-	int (*fe_set_voltage)(struct dvb_frontend* fe, fe_sec_voltage_t voltage);
-	int (*fe_dishnetwork_send_legacy_command)(struct dvb_frontend* fe, unsigned long cmd);
+	int (*fe_read_status)(struct dvb_frontend *fe, enum fe_status *status);
+	int (*fe_diseqc_reset_overload)(struct dvb_frontend *fe);
+	int (*fe_diseqc_send_master_cmd)(struct dvb_frontend *fe,
+					 struct dvb_diseqc_master_cmd *cmd);
+	int (*fe_diseqc_send_burst)(struct dvb_frontend *fe,
+				    enum fe_sec_mini_cmd minicmd);
+	int (*fe_set_tone)(struct dvb_frontend *fe,
+			   enum fe_sec_tone_mode tone);
+	int (*fe_set_voltage)(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage);
+	int (*fe_dishnetwork_send_legacy_command)(struct dvb_frontend *fe,
+						  unsigned long cmd);
 	int (*fe_set_frontend)(struct dvb_frontend *fe);
 };
 
diff --git a/drivers/media/pci/ttpci/budget-core.c b/drivers/media/pci/ttpci/budget-core.c
index 23e05499b509..e9674b40007c 100644
--- a/drivers/media/pci/ttpci/budget-core.c
+++ b/drivers/media/pci/ttpci/budget-core.c
@@ -161,7 +161,8 @@ static int start_ts_capture(struct budget *budget)
 	return 0;
 }
 
-static int budget_read_fe_status(struct dvb_frontend *fe, fe_status_t *status)
+static int budget_read_fe_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
 {
 	struct budget *budget = (struct budget *) fe->dvb->priv;
 	int synced;
diff --git a/drivers/media/pci/ttpci/budget-patch.c b/drivers/media/pci/ttpci/budget-patch.c
index a4d8867e1d7b..b5b65962ce8f 100644
--- a/drivers/media/pci/ttpci/budget-patch.c
+++ b/drivers/media/pci/ttpci/budget-patch.c
@@ -128,9 +128,9 @@ static int SendDiSEqCMsg (struct budget *budget, int len, u8 *msg, unsigned long
 	return 0;
 }
 
-/* shamelessly copy/pasted from budget.c
-*/
-static int budget_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+/* shamelessly copy/pasted from budget.c */
+static int budget_set_tone(struct dvb_frontend *fe,
+			   enum fe_sec_tone_mode tone)
 {
 	struct budget* budget = (struct budget*) fe->dvb->priv;
 
@@ -159,7 +159,8 @@ static int budget_diseqc_send_master_cmd(struct dvb_frontend* fe, struct dvb_dis
 	return 0;
 }
 
-static int budget_diseqc_send_burst(struct dvb_frontend* fe, fe_sec_mini_cmd_t minicmd)
+static int budget_diseqc_send_burst(struct dvb_frontend *fe,
+				    enum fe_sec_mini_cmd minicmd)
 {
 	struct budget* budget = (struct budget*) fe->dvb->priv;
 
@@ -223,7 +224,8 @@ static int av7110_send_diseqc_msg(struct budget_patch *budget, int len, u8 *msg,
 	return 0;
 }
 
-static int budget_patch_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int budget_patch_set_tone(struct dvb_frontend *fe,
+				 enum fe_sec_tone_mode tone)
 {
 	struct budget_patch* budget = (struct budget_patch*) fe->dvb->priv;
 
@@ -252,7 +254,8 @@ static int budget_patch_diseqc_send_master_cmd(struct dvb_frontend* fe, struct d
 	return 0;
 }
 
-static int budget_patch_diseqc_send_burst(struct dvb_frontend* fe, fe_sec_mini_cmd_t minicmd)
+static int budget_patch_diseqc_send_burst(struct dvb_frontend *fe,
+					  enum fe_sec_mini_cmd minicmd)
 {
 	struct budget_patch* budget = (struct budget_patch*) fe->dvb->priv;
 
diff --git a/drivers/media/pci/ttpci/budget.c b/drivers/media/pci/ttpci/budget.c
index 6ccc48833fd8..99972beca262 100644
--- a/drivers/media/pci/ttpci/budget.c
+++ b/drivers/media/pci/ttpci/budget.c
@@ -132,7 +132,8 @@ static int SendDiSEqCMsg (struct budget *budget, int len, u8 *msg, unsigned long
  *   Voltage must be set here.
  *   GPIO 1: LNBP EN, GPIO 2: LNBP VSEL
  */
-static int SetVoltage_Activy (struct budget *budget, fe_sec_voltage_t voltage)
+static int SetVoltage_Activy(struct budget *budget,
+			     enum fe_sec_voltage voltage)
 {
 	struct saa7146_dev *dev=budget->dev;
 
@@ -157,14 +158,16 @@ static int SetVoltage_Activy (struct budget *budget, fe_sec_voltage_t voltage)
 	return 0;
 }
 
-static int siemens_budget_set_voltage(struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int siemens_budget_set_voltage(struct dvb_frontend *fe,
+				      enum fe_sec_voltage voltage)
 {
 	struct budget* budget = (struct budget*) fe->dvb->priv;
 
 	return SetVoltage_Activy (budget, voltage);
 }
 
-static int budget_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int budget_set_tone(struct dvb_frontend *fe,
+			   enum fe_sec_tone_mode tone)
 {
 	struct budget* budget = (struct budget*) fe->dvb->priv;
 
@@ -193,7 +196,8 @@ static int budget_diseqc_send_master_cmd(struct dvb_frontend* fe, struct dvb_dis
 	return 0;
 }
 
-static int budget_diseqc_send_burst(struct dvb_frontend* fe, fe_sec_mini_cmd_t minicmd)
+static int budget_diseqc_send_burst(struct dvb_frontend *fe,
+				    enum fe_sec_mini_cmd minicmd)
 {
 	struct budget* budget = (struct budget*) fe->dvb->priv;
 
diff --git a/drivers/media/pci/ttpci/budget.h b/drivers/media/pci/ttpci/budget.h
index 3d8a806c20bb..1ccbe1a49a4b 100644
--- a/drivers/media/pci/ttpci/budget.h
+++ b/drivers/media/pci/ttpci/budget.h
@@ -72,7 +72,7 @@ struct budget {
 
 	struct dvb_adapter dvb_adapter;
 	struct dvb_frontend *dvb_frontend;
-	int (*read_fe_status)(struct dvb_frontend *fe, fe_status_t *status);
+	int (*read_fe_status)(struct dvb_frontend *fe, enum fe_status *status);
 	int fe_synced;
 
 	void *priv;
diff --git a/drivers/media/pci/zoran/zoran_device.c b/drivers/media/pci/zoran/zoran_device.c
index b6801e035ea4..40119b3c52c1 100644
--- a/drivers/media/pci/zoran/zoran_device.c
+++ b/drivers/media/pci/zoran/zoran_device.c
@@ -1584,14 +1584,11 @@ zoran_init_hardware (struct zoran *zr)
 	jpeg_codec_sleep(zr, 1);
 	jpeg_codec_sleep(zr, 0);
 
-	/* set individual interrupt enables (without GIRQ1)
-	 * but don't global enable until zoran_open() */
-
-	//btwrite(IRQ_MASK & ~ZR36057_ISR_GIRQ1, ZR36057_ICR);  // SW
-	// It looks like using only JPEGRepIRQEn is not always reliable,
-	// may be when JPEG codec crashes it won't generate IRQ? So,
-	 /*CP*/			//        btwrite(IRQ_MASK, ZR36057_ICR); // Enable Vsync interrupts too. SM    WHY ? LP
-	    zr36057_init_vfe(zr);
+	/*
+	 * set individual interrupt enables (without GIRQ1)
+	 * but don't global enable until zoran_open()
+	 */
+	zr36057_init_vfe(zr);
 
 	zr36057_enable_jpg(zr, BUZ_MODE_IDLE);
 
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 19306f7229c0..f6bed197130c 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -212,6 +212,16 @@ config VIDEO_SAMSUNG_EXYNOS_GSC
 	help
 	  This is a v4l2 driver for Samsung EXYNOS5 SoC G-Scaler.
 
+config VIDEO_STI_BDISP
+	tristate "STMicroelectronics BDISP 2D blitter driver"
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on ARCH_STI || COMPILE_TEST
+	depends on HAVE_DMA_ATTRS
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_MEM2MEM_DEV
+	help
+	  This v4l2 mem2mem driver is a 2D blitter for STMicroelectronics SoC.
+
 config VIDEO_SH_VEU
 	tristate "SuperH VEU mem2mem video processing driver"
 	depends on VIDEO_DEV && VIDEO_V4L2 && HAS_DMA
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 8f855616c237..114f9aba1c00 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -34,6 +34,8 @@ obj-$(CONFIG_VIDEO_SAMSUNG_S5P_TV)	+= s5p-tv/
 obj-$(CONFIG_VIDEO_SAMSUNG_S5P_G2D)	+= s5p-g2d/
 obj-$(CONFIG_VIDEO_SAMSUNG_EXYNOS_GSC)	+= exynos-gsc/
 
+obj-$(CONFIG_VIDEO_STI_BDISP)		+= sti/bdisp/
+
 obj-$(CONFIG_BLACKFIN)                  += blackfin/
 
 obj-$(CONFIG_ARCH_DAVINCI)		+= davinci/
diff --git a/drivers/media/platform/am437x/am437x-vpfe.c b/drivers/media/platform/am437x/am437x-vpfe.c
index a30cc2f7e4f1..1fba339cddc1 100644
--- a/drivers/media/platform/am437x/am437x-vpfe.c
+++ b/drivers/media/platform/am437x/am437x-vpfe.c
@@ -288,7 +288,8 @@ cmp_v4l2_format(const struct v4l2_format *lhs, const struct v4l2_format *rhs)
 		lhs->fmt.pix.field == rhs->fmt.pix.field &&
 		lhs->fmt.pix.colorspace == rhs->fmt.pix.colorspace &&
 		lhs->fmt.pix.ycbcr_enc == rhs->fmt.pix.ycbcr_enc &&
-		lhs->fmt.pix.quantization == rhs->fmt.pix.quantization;
+		lhs->fmt.pix.quantization == rhs->fmt.pix.quantization &&
+		lhs->fmt.pix.xfer_func == rhs->fmt.pix.xfer_func;
 }
 
 static inline u32 vpfe_reg_read(struct vpfe_ccdc *ccdc, u32 offset)
@@ -430,7 +431,7 @@ vpfe_ccdc_update_raw_params(struct vpfe_ccdc *ccdc,
 	struct vpfe_ccdc_config_params_raw *config_params =
 				&ccdc->ccdc_cfg.bayer.config_params;
 
-	config_params = raw_params;
+	*config_params = *raw_params;
 }
 
 /*
@@ -510,7 +511,7 @@ static int vpfe_ccdc_set_params(struct vpfe_ccdc *ccdc, void __user *params)
 
 	if (!vpfe_ccdc_validate_param(ccdc, &raw_params)) {
 		vpfe_ccdc_update_raw_params(ccdc, &raw_params);
-			return 0;
+		return 0;
 	}
 
 	return -EINVAL;
@@ -1095,7 +1096,7 @@ static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe)
  * For a given standard, this functions sets up the default
  * pix format & crop values in the vpfe device and ccdc.  It first
  * starts with defaults based values from the standard table.
- * It then checks if sub device support g_mbus_fmt and then override the
+ * It then checks if sub device supports get_fmt and then override the
  * values based on that.Sets crop values to match with scan resolution
  * starting at 0,0. It calls vpfe_config_ccdc_image_format() set the
  * values in ccdc
@@ -1432,8 +1433,8 @@ static int __vpfe_get_format(struct vpfe_device *vpfe,
 	} else {
 		ret = v4l2_device_call_until_err(&vpfe->v4l2_dev,
 						 sdinfo->grp_id,
-						 video, g_mbus_fmt,
-						 &mbus_fmt);
+						 pad, get_fmt,
+						 NULL, &fmt);
 		if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
 			return ret;
 		v4l2_fill_pix_format(&format->fmt.pix, &mbus_fmt);
@@ -1455,7 +1456,6 @@ static int __vpfe_get_format(struct vpfe_device *vpfe,
 static int __vpfe_set_format(struct vpfe_device *vpfe,
 			     struct v4l2_format *format, unsigned int *bpp)
 {
-	struct v4l2_mbus_framefmt mbus_fmt;
 	struct vpfe_subdev_info *sdinfo;
 	struct v4l2_subdev_format fmt;
 	int ret;
@@ -1472,23 +1472,11 @@ static int __vpfe_set_format(struct vpfe_device *vpfe,
 	pix_to_mbus(vpfe, &format->fmt.pix, &fmt.format);
 
 	ret = v4l2_subdev_call(sdinfo->sd, pad, set_fmt, NULL, &fmt);
-	if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
+	if (ret)
 		return ret;
 
-	if (!ret) {
-		v4l2_fill_pix_format(&format->fmt.pix, &fmt.format);
-		mbus_to_pix(vpfe, &fmt.format, &format->fmt.pix, bpp);
-	} else {
-		ret = v4l2_device_call_until_err(&vpfe->v4l2_dev,
-						 sdinfo->grp_id,
-						 video, s_mbus_fmt,
-						 &mbus_fmt);
-		if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
-			return ret;
-
-		v4l2_fill_pix_format(&format->fmt.pix, &mbus_fmt);
-		mbus_to_pix(vpfe, &mbus_fmt, &format->fmt.pix, bpp);
-	}
+	v4l2_fill_pix_format(&format->fmt.pix, &fmt.format);
+	mbus_to_pix(vpfe, &fmt.format, &format->fmt.pix, bpp);
 
 	format->type = vpfe->fmt.type;
 
@@ -1675,12 +1663,9 @@ vpfe_get_subdev_input_index(struct vpfe_device *vpfe,
 			    int *subdev_input_index,
 			    int app_input_index)
 {
-	struct vpfe_config *cfg = vpfe->cfg;
-	struct vpfe_subdev_info *sdinfo;
 	int i, j = 0;
 
 	for (i = 0; i < ARRAY_SIZE(vpfe->cfg->asd); i++) {
-		sdinfo = &cfg->sub_devs[i];
 		if (app_input_index < (j + 1)) {
 			*subdev_index = i;
 			*subdev_input_index = app_input_index - j;
diff --git a/drivers/media/platform/blackfin/bfin_capture.c b/drivers/media/platform/blackfin/bfin_capture.c
index 6a437f86dcdc..b7e70fb05eb8 100644
--- a/drivers/media/platform/blackfin/bfin_capture.c
+++ b/drivers/media/platform/blackfin/bfin_capture.c
@@ -156,14 +156,18 @@ static struct bcap_buffer *to_bcap_vb(struct vb2_buffer *vb)
 
 static int bcap_init_sensor_formats(struct bcap_device *bcap_dev)
 {
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	struct bcap_format *sf;
 	unsigned int num_formats = 0;
 	int i, j;
 
-	while (!v4l2_subdev_call(bcap_dev->sd, video,
-				enum_mbus_fmt, num_formats, &code))
+	while (!v4l2_subdev_call(bcap_dev->sd, pad,
+				enum_mbus_code, NULL, &code)) {
 		num_formats++;
+		code.index++;
+	}
 	if (!num_formats)
 		return -ENXIO;
 
@@ -172,10 +176,11 @@ static int bcap_init_sensor_formats(struct bcap_device *bcap_dev)
 		return -ENOMEM;
 
 	for (i = 0; i < num_formats; i++) {
-		v4l2_subdev_call(bcap_dev->sd, video,
-				enum_mbus_fmt, i, &code);
+		code.index = i;
+		v4l2_subdev_call(bcap_dev->sd, pad,
+				enum_mbus_code, NULL, &code);
 		for (j = 0; j < BCAP_MAX_FMTS; j++)
-			if (code == bcap_formats[j].mbus_code)
+			if (code.code == bcap_formats[j].mbus_code)
 				break;
 		if (j == BCAP_MAX_FMTS) {
 			/* we don't allow this sensor working with our bridge */
@@ -597,7 +602,10 @@ static int bcap_try_format(struct bcap_device *bcap,
 {
 	struct bcap_format *sf = bcap->sensor_formats;
 	struct bcap_format *fmt = NULL;
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
 	int ret, i;
 
 	for (i = 0; i < bcap->num_sensor_formats; i++) {
@@ -608,16 +616,16 @@ static int bcap_try_format(struct bcap_device *bcap,
 	if (i == bcap->num_sensor_formats)
 		fmt = &sf[0];
 
-	v4l2_fill_mbus_format(&mbus_fmt, pixfmt, fmt->mbus_code);
-	ret = v4l2_subdev_call(bcap->sd, video,
-				try_mbus_fmt, &mbus_fmt);
+	v4l2_fill_mbus_format(&format.format, pixfmt, fmt->mbus_code);
+	ret = v4l2_subdev_call(bcap->sd, pad, set_fmt, &pad_cfg,
+				&format);
 	if (ret < 0)
 		return ret;
-	v4l2_fill_pix_format(pixfmt, &mbus_fmt);
+	v4l2_fill_pix_format(pixfmt, &format.format);
 	if (bcap_fmt) {
 		for (i = 0; i < bcap->num_sensor_formats; i++) {
 			fmt = &sf[i];
-			if (mbus_fmt.code == fmt->mbus_code)
+			if (format.format.code == fmt->mbus_code)
 				break;
 		}
 		*bcap_fmt = *fmt;
@@ -666,7 +674,9 @@ static int bcap_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *fmt)
 {
 	struct bcap_device *bcap_dev = video_drvdata(file);
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	struct bcap_format bcap_fmt;
 	struct v4l2_pix_format *pixfmt = &fmt->fmt.pix;
 	int ret;
@@ -679,8 +689,8 @@ static int bcap_s_fmt_vid_cap(struct file *file, void *priv,
 	if (ret < 0)
 		return ret;
 
-	v4l2_fill_mbus_format(&mbus_fmt, pixfmt, bcap_fmt.mbus_code);
-	ret = v4l2_subdev_call(bcap_dev->sd, video, s_mbus_fmt, &mbus_fmt);
+	v4l2_fill_mbus_format(&format.format, pixfmt, bcap_fmt.mbus_code);
+	ret = v4l2_subdev_call(bcap_dev->sd, pad, set_fmt, NULL, &format);
 	if (ret < 0)
 		return ret;
 	bcap_dev->fmt = *pixfmt;
diff --git a/drivers/media/platform/coda/coda-bit.c b/drivers/media/platform/coda/coda-bit.c
index d0430071d2ee..109797bb8fbb 100644
--- a/drivers/media/platform/coda/coda-bit.c
+++ b/drivers/media/platform/coda/coda-bit.c
@@ -1305,7 +1305,7 @@ static void coda_finish_encode(struct coda_ctx *ctx)
 	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
 
 	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
-	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+	coda_m2m_buf_done(ctx, dst_buf, VB2_BUF_STATE_DONE);
 
 	ctx->gopcounter--;
 	if (ctx->gopcounter < 0)
@@ -1975,7 +1975,7 @@ static void coda_finish_decode(struct coda_ctx *ctx)
 		}
 		vb2_set_plane_payload(dst_buf, 0, payload);
 
-		v4l2_m2m_buf_done(dst_buf, ctx->frame_errors[display_idx] ?
+		coda_m2m_buf_done(ctx, dst_buf, ctx->frame_errors[display_idx] ?
 				  VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 
 		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
diff --git a/drivers/media/platform/coda/coda-common.c b/drivers/media/platform/coda/coda-common.c
index 8e6fe0200117..58f65486de33 100644
--- a/drivers/media/platform/coda/coda-common.c
+++ b/drivers/media/platform/coda/coda-common.c
@@ -724,35 +724,30 @@ static int coda_qbuf(struct file *file, void *priv,
 }
 
 static bool coda_buf_is_end_of_stream(struct coda_ctx *ctx,
-				      struct v4l2_buffer *buf)
+				      struct vb2_buffer *buf)
 {
 	struct vb2_queue *src_vq;
 
 	src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 
 	return ((ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG) &&
-		(buf->sequence == (ctx->qsequence - 1)));
+		(buf->v4l2_buf.sequence == (ctx->qsequence - 1)));
 }
 
-static int coda_dqbuf(struct file *file, void *priv,
-		      struct v4l2_buffer *buf)
+void coda_m2m_buf_done(struct coda_ctx *ctx, struct vb2_buffer *buf,
+		       enum vb2_buffer_state state)
 {
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-	int ret;
+	const struct v4l2_event eos_event = {
+		.type = V4L2_EVENT_EOS
+	};
 
-	ret = v4l2_m2m_dqbuf(file, ctx->fh.m2m_ctx, buf);
-
-	/* If this is the last capture buffer, emit an end-of-stream event */
-	if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-	    coda_buf_is_end_of_stream(ctx, buf)) {
-		const struct v4l2_event eos_event = {
-			.type = V4L2_EVENT_EOS
-		};
+	if (coda_buf_is_end_of_stream(ctx, buf)) {
+		buf->v4l2_buf.flags |= V4L2_BUF_FLAG_LAST;
 
 		v4l2_event_queue_fh(&ctx->fh, &eos_event);
 	}
 
-	return ret;
+	v4l2_m2m_buf_done(buf, state);
 }
 
 static int coda_g_selection(struct file *file, void *fh,
@@ -865,7 +860,7 @@ static const struct v4l2_ioctl_ops coda_ioctl_ops = {
 
 	.vidioc_qbuf		= coda_qbuf,
 	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
-	.vidioc_dqbuf		= coda_dqbuf,
+	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
 	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
 
 	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
@@ -2160,9 +2155,9 @@ static int coda_probe(struct platform_device *pdev)
 	}
 
 	/* Get IRAM pool from device tree or platform data */
-	pool = of_get_named_gen_pool(np, "iram", 0);
+	pool = of_gen_pool_get(np, "iram", 0);
 	if (!pool && pdata)
-		pool = dev_get_gen_pool(pdata->iram_dev);
+		pool = gen_pool_get(pdata->iram_dev);
 	if (!pool) {
 		dev_err(&pdev->dev, "iram pool not available\n");
 		return -ENOMEM;
diff --git a/drivers/media/platform/coda/coda.h b/drivers/media/platform/coda/coda.h
index 6a5c8f6c688e..8e0af221b2e9 100644
--- a/drivers/media/platform/coda/coda.h
+++ b/drivers/media/platform/coda/coda.h
@@ -287,6 +287,9 @@ static inline unsigned int coda_get_bitstream_payload(struct coda_ctx *ctx)
 
 void coda_bit_stream_end_flag(struct coda_ctx *ctx);
 
+void coda_m2m_buf_done(struct coda_ctx *ctx, struct vb2_buffer *buf,
+		       enum vb2_buffer_state state);
+
 int coda_h264_padding(int size, char *p);
 
 bool coda_jpeg_check_buffer(struct coda_ctx *ctx, struct vb2_buffer *vb);
diff --git a/drivers/media/platform/coda/trace.h b/drivers/media/platform/coda/trace.h
index d1d06cbd1f6a..781bf7286d53 100644
--- a/drivers/media/platform/coda/trace.h
+++ b/drivers/media/platform/coda/trace.h
@@ -9,8 +9,6 @@
 
 #include "coda.h"
 
-#define TRACE_SYSTEM_STRING __stringify(TRACE_SYSTEM)
-
 TRACE_EVENT(coda_bit_run,
 	TP_PROTO(struct coda_ctx *ctx, int cmd),
 
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index c4ab46f5bd92..f69cdd7da10c 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -71,15 +71,10 @@ static int venc_is_second_field(struct vpbe_display *disp_dev)
 static void vpbe_isr_even_field(struct vpbe_display *disp_obj,
 				struct vpbe_layer *layer)
 {
-	struct timespec timevalue;
-
 	if (layer->cur_frm == layer->next_frm)
 		return;
-	ktime_get_ts(&timevalue);
-	layer->cur_frm->vb.v4l2_buf.timestamp.tv_sec =
-		timevalue.tv_sec;
-	layer->cur_frm->vb.v4l2_buf.timestamp.tv_usec =
-		timevalue.tv_nsec / NSEC_PER_USEC;
+
+	v4l2_get_timestamp(&layer->cur_frm->vb.v4l2_buf.timestamp);
 	vb2_buffer_done(&layer->cur_frm->vb, VB2_BUF_STATE_DONE);
 	/* Make cur_frm pointing to next_frm */
 	layer->cur_frm = layer->next_frm;
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index ccfcf3f528d3..7767e072d623 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -370,7 +370,7 @@ static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe_dev)
  * For a given standard, this functions sets up the default
  * pix format & crop values in the vpfe device and ccdc.  It first
  * starts with defaults based values from the standard table.
- * It then checks if sub device support g_mbus_fmt and then override the
+ * It then checks if sub device supports get_fmt and then override the
  * values based on that.Sets crop values to match with scan resolution
  * starting at 0,0. It calls vpfe_config_ccdc_image_format() set the
  * values in ccdc
@@ -379,7 +379,10 @@ static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
 				    v4l2_std_id std_id)
 {
 	struct vpfe_subdev_info *sdinfo = vpfe_dev->current_subdev;
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mbus_fmt = &fmt.format;
 	struct v4l2_pix_format *pix = &vpfe_dev->fmt.fmt.pix;
 	int i, ret = 0;
 
@@ -413,26 +416,26 @@ static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
 		pix->field = V4L2_FIELD_INTERLACED;
 		/* assume V4L2_PIX_FMT_UYVY as default */
 		pix->pixelformat = V4L2_PIX_FMT_UYVY;
-		v4l2_fill_mbus_format(&mbus_fmt, pix,
+		v4l2_fill_mbus_format(mbus_fmt, pix,
 				MEDIA_BUS_FMT_YUYV10_2X10);
 	} else {
 		pix->field = V4L2_FIELD_NONE;
 		/* assume V4L2_PIX_FMT_SBGGR8 */
 		pix->pixelformat = V4L2_PIX_FMT_SBGGR8;
-		v4l2_fill_mbus_format(&mbus_fmt, pix,
+		v4l2_fill_mbus_format(mbus_fmt, pix,
 				MEDIA_BUS_FMT_SBGGR8_1X8);
 	}
 
-	/* if sub device supports g_mbus_fmt, override the defaults */
+	/* if sub device supports get_fmt, override the defaults */
 	ret = v4l2_device_call_until_err(&vpfe_dev->v4l2_dev,
-			sdinfo->grp_id, video, g_mbus_fmt, &mbus_fmt);
+			sdinfo->grp_id, pad, get_fmt, NULL, &fmt);
 
 	if (ret && ret != -ENOIOCTLCMD) {
 		v4l2_err(&vpfe_dev->v4l2_dev,
-			"error in getting g_mbus_fmt from sub device\n");
+			"error in getting get_fmt from sub device\n");
 		return ret;
 	}
-	v4l2_fill_pix_format(pix, &mbus_fmt);
+	v4l2_fill_pix_format(pix, mbus_fmt);
 	pix->bytesperline = pix->width * 2;
 	pix->sizeimage = pix->bytesperline * pix->height;
 
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.c b/drivers/media/platform/exynos-gsc/gsc-core.c
index fd2891c886a3..9b9e423e4fc4 100644
--- a/drivers/media/platform/exynos-gsc/gsc-core.c
+++ b/drivers/media/platform/exynos-gsc/gsc-core.c
@@ -967,7 +967,7 @@ static struct gsc_driverdata gsc_v_100_drvdata = {
 	.lclk_frequency = 266000000UL,
 };
 
-static struct platform_device_id gsc_driver_ids[] = {
+static const struct platform_device_id gsc_driver_ids[] = {
 	{
 		.name		= "exynos-gsc",
 		.driver_data	= (unsigned long)&gsc_v_100_drvdata,
diff --git a/drivers/media/platform/exynos4-is/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
index b7b2e472240a..40423c6c5324 100644
--- a/drivers/media/platform/exynos4-is/Kconfig
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -57,6 +57,7 @@ endif
 
 config VIDEO_EXYNOS4_FIMC_IS
 	tristate "EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver"
+	depends on I2C
 	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	depends on OF
diff --git a/drivers/media/platform/exynos4-is/media-dev.c b/drivers/media/platform/exynos4-is/media-dev.c
index f315ef946cd4..4f5586a4cbff 100644
--- a/drivers/media/platform/exynos4-is/media-dev.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -1451,7 +1451,7 @@ static int fimc_md_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id fimc_driver_ids[] __always_unused = {
+static const struct platform_device_id fimc_driver_ids[] __always_unused = {
 	{ .name = "s5p-fimc-md" },
 	{ },
 };
diff --git a/drivers/media/platform/fsl-viu.c b/drivers/media/platform/fsl-viu.c
index bbf428104871..5b76e3db6a92 100644
--- a/drivers/media/platform/fsl-viu.c
+++ b/drivers/media/platform/fsl-viu.c
@@ -1664,7 +1664,7 @@ static int viu_resume(struct platform_device *op)
 /*
  * Initialization and module stuff
  */
-static struct of_device_id mpc512x_viu_of_match[] = {
+static const struct of_device_id mpc512x_viu_of_match[] = {
 	{
 		.compatible = "fsl,mpc5121-viu",
 	},
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index 92d954973ccf..c07f367aa436 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -1060,7 +1060,6 @@ static int deinterlace_probe(struct platform_device *pdev)
 
 	return 0;
 
-	v4l2_m2m_release(pcdev->m2m_dev);
 err_m2m:
 	video_unregister_device(&pcdev->vfd);
 err_ctx:
diff --git a/drivers/media/platform/marvell-ccic/cafe-driver.c b/drivers/media/platform/marvell-ccic/cafe-driver.c
index 562845361246..77890bd0deab 100644
--- a/drivers/media/platform/marvell-ccic/cafe-driver.c
+++ b/drivers/media/platform/marvell-ccic/cafe-driver.c
@@ -339,17 +339,21 @@ static int cafe_smbus_setup(struct cafe_camera *cam)
 	adap = kzalloc(sizeof(*adap), GFP_KERNEL);
 	if (adap == NULL)
 		return -ENOMEM;
-	cam->mcam.i2c_adapter = adap;
-	cafe_smbus_enable_irq(cam);
 	adap->owner = THIS_MODULE;
 	adap->algo = &cafe_smbus_algo;
 	strcpy(adap->name, "cafe_ccic");
 	adap->dev.parent = &cam->pdev->dev;
 	i2c_set_adapdata(adap, cam);
 	ret = i2c_add_adapter(adap);
-	if (ret)
+	if (ret) {
 		printk(KERN_ERR "Unable to register cafe i2c adapter\n");
-	return ret;
+		kfree(adap);
+		return ret;
+	}
+
+	cam->mcam.i2c_adapter = adap;
+	cafe_smbus_enable_irq(cam);
+	return 0;
 }
 
 static void cafe_smbus_shutdown(struct cafe_camera *cam)
@@ -476,6 +480,7 @@ static int cafe_pci_probe(struct pci_dev *pdev,
 	mcam->plat_power_up = cafe_ctlr_power_up;
 	mcam->plat_power_down = cafe_ctlr_power_down;
 	mcam->dev = &pdev->dev;
+	snprintf(mcam->bus_info, sizeof(mcam->bus_info), "PCI:%s", pci_name(pdev));
 	/*
 	 * Set the clock speed for the XO 1; I don't believe this
 	 * driver has ever run anywhere else.
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.c b/drivers/media/platform/marvell-ccic/mcam-core.c
index 110fd70c7326..5e2b4df48b3c 100644
--- a/drivers/media/platform/marvell-ccic/mcam-core.c
+++ b/drivers/media/platform/marvell-ccic/mcam-core.c
@@ -24,6 +24,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
 #include <media/ov7670.h>
 #include <media/videobuf2-vmalloc.h>
 #include <media/videobuf2-dma-contig.h>
@@ -123,29 +124,22 @@ static struct mcam_format_struct {
 		.planar		= false,
 	},
 	{
-		.desc		= "YUV 4:2:2 PLANAR",
-		.pixelformat	= V4L2_PIX_FMT_YUV422P,
-		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
-		.bpp		= 2,
-		.planar		= true,
-	},
-	{
 		.desc		= "YUV 4:2:0 PLANAR",
 		.pixelformat	= V4L2_PIX_FMT_YUV420,
 		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
-		.bpp		= 2,
+		.bpp		= 1,
 		.planar		= true,
 	},
 	{
 		.desc		= "YVU 4:2:0 PLANAR",
 		.pixelformat	= V4L2_PIX_FMT_YVU420,
 		.mbus_code	= MEDIA_BUS_FMT_YUYV8_2X8,
-		.bpp		= 2,
+		.bpp		= 1,
 		.planar		= true,
 	},
 	{
-		.desc		= "RGB 444",
-		.pixelformat	= V4L2_PIX_FMT_RGB444,
+		.desc		= "XRGB 444",
+		.pixelformat	= V4L2_PIX_FMT_XRGB444,
 		.mbus_code	= MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE,
 		.bpp		= 2,
 		.planar		= false,
@@ -188,6 +182,7 @@ static const struct v4l2_pix_format mcam_def_pix_format = {
 	.field		= V4L2_FIELD_NONE,
 	.bytesperline	= VGA_WIDTH*2,
 	.sizeimage	= VGA_WIDTH*VGA_HEIGHT*2,
+	.colorspace	= V4L2_COLORSPACE_SRGB,
 };
 
 static const u32 mcam_def_mbus_code = MEDIA_BUS_FMT_YUYV8_2X8;
@@ -204,12 +199,6 @@ struct mcam_dma_desc {
 	u32 segment_len;
 };
 
-struct yuv_pointer_t {
-	dma_addr_t y;
-	dma_addr_t u;
-	dma_addr_t v;
-};
-
 /*
  * Our buffer type for working with videobuf2.  Note that the vb2
  * developers have decreed that struct vb2_buffer must be at the
@@ -221,7 +210,6 @@ struct mcam_vb_buffer {
 	struct mcam_dma_desc *dma_desc;	/* Descriptor virtual address */
 	dma_addr_t dma_desc_pa;		/* Descriptor physical address */
 	int dma_desc_nent;		/* Number of mapped descriptors */
-	struct yuv_pointer_t yuv_p;
 };
 
 static inline struct mcam_vb_buffer *vb_to_mvb(struct vb2_buffer *vb)
@@ -237,6 +225,8 @@ static void mcam_buffer_done(struct mcam_camera *cam, int frame,
 {
 	vbuf->v4l2_buf.bytesused = cam->pix_format.sizeimage;
 	vbuf->v4l2_buf.sequence = cam->buf_seq[frame];
+	vbuf->v4l2_buf.field = V4L2_FIELD_NONE;
+	v4l2_get_timestamp(&vbuf->v4l2_buf.timestamp);
 	vb2_set_plane_payload(vbuf, 0, cam->pix_format.sizeimage);
 	vb2_buffer_done(vbuf, VB2_BUF_STATE_DONE);
 }
@@ -337,6 +327,43 @@ static void mcam_disable_mipi(struct mcam_camera *mcam)
 	mcam->mipi_enabled = false;
 }
 
+static bool mcam_fmt_is_planar(__u32 pfmt)
+{
+	struct mcam_format_struct *f;
+
+	f = mcam_find_format(pfmt);
+	return f->planar;
+}
+
+static void mcam_write_yuv_bases(struct mcam_camera *cam,
+				 unsigned frame, dma_addr_t base)
+{
+	struct v4l2_pix_format *fmt = &cam->pix_format;
+	u32 pixel_count = fmt->width * fmt->height;
+	dma_addr_t y, u = 0, v = 0;
+
+	y = base;
+
+	switch (fmt->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		u = y + pixel_count;
+		v = u + pixel_count / 4;
+		break;
+	case V4L2_PIX_FMT_YVU420:
+		v = y + pixel_count;
+		u = v + pixel_count / 4;
+		break;
+	default:
+		break;
+	}
+
+	mcam_reg_write(cam, REG_Y0BAR + frame * 4, y);
+	if (mcam_fmt_is_planar(fmt->pixelformat)) {
+		mcam_reg_write(cam, REG_U0BAR + frame * 4, u);
+		mcam_reg_write(cam, REG_V0BAR + frame * 4, v);
+	}
+}
+
 /* ------------------------------------------------------------------- */
 
 #ifdef MCAM_MODE_VMALLOC
@@ -407,15 +434,14 @@ static void mcam_free_dma_bufs(struct mcam_camera *cam)
 static void mcam_ctlr_dma_vmalloc(struct mcam_camera *cam)
 {
 	/*
-	 * Store the first two Y buffers (we aren't supporting
-	 * planar formats for now, so no UV bufs).  Then either
+	 * Store the first two YUV buffers. Then either
 	 * set the third if it exists, or tell the controller
 	 * to just use two.
 	 */
-	mcam_reg_write(cam, REG_Y0BAR, cam->dma_handles[0]);
-	mcam_reg_write(cam, REG_Y1BAR, cam->dma_handles[1]);
+	mcam_write_yuv_bases(cam, 0, cam->dma_handles[0]);
+	mcam_write_yuv_bases(cam, 1, cam->dma_handles[1]);
 	if (cam->nbufs > 2) {
-		mcam_reg_write(cam, REG_Y2BAR, cam->dma_handles[2]);
+		mcam_write_yuv_bases(cam, 2, cam->dma_handles[2]);
 		mcam_reg_clear_bit(cam, REG_CTRL1, C1_TWOBUFS);
 	} else
 		mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
@@ -510,14 +536,6 @@ static inline int mcam_check_dma_buffers(struct mcam_camera *cam)
  * DMA-contiguous code.
  */
 
-static bool mcam_fmt_is_planar(__u32 pfmt)
-{
-	struct mcam_format_struct *f;
-
-	f = mcam_find_format(pfmt);
-	return f->planar;
-}
-
 /*
  * Set up a contiguous buffer for the given frame.  Here also is where
  * the underrun strategy is set: if there is no buffer available, reuse
@@ -529,9 +547,7 @@ static bool mcam_fmt_is_planar(__u32 pfmt)
 static void mcam_set_contig_buffer(struct mcam_camera *cam, int frame)
 {
 	struct mcam_vb_buffer *buf;
-	struct v4l2_pix_format *fmt = &cam->pix_format;
 	dma_addr_t dma_handle;
-	u32 pixel_count = fmt->width * fmt->height;
 	struct vb2_buffer *vb;
 
 	/*
@@ -555,32 +571,7 @@ static void mcam_set_contig_buffer(struct mcam_camera *cam, int frame)
 	vb = &buf->vb_buf;
 
 	dma_handle = vb2_dma_contig_plane_dma_addr(vb, 0);
-	buf->yuv_p.y = dma_handle;
-
-	switch (cam->pix_format.pixelformat) {
-	case V4L2_PIX_FMT_YUV422P:
-		buf->yuv_p.u = buf->yuv_p.y + pixel_count;
-		buf->yuv_p.v = buf->yuv_p.u + pixel_count / 2;
-		break;
-	case V4L2_PIX_FMT_YUV420:
-		buf->yuv_p.u = buf->yuv_p.y + pixel_count;
-		buf->yuv_p.v = buf->yuv_p.u + pixel_count / 4;
-		break;
-	case V4L2_PIX_FMT_YVU420:
-		buf->yuv_p.v = buf->yuv_p.y + pixel_count;
-		buf->yuv_p.u = buf->yuv_p.v + pixel_count / 4;
-		break;
-	default:
-		break;
-	}
-
-	mcam_reg_write(cam, frame == 0 ? REG_Y0BAR : REG_Y1BAR, buf->yuv_p.y);
-	if (mcam_fmt_is_planar(fmt->pixelformat)) {
-		mcam_reg_write(cam, frame == 0 ?
-					REG_U0BAR : REG_U1BAR, buf->yuv_p.u);
-		mcam_reg_write(cam, frame == 0 ?
-					REG_V0BAR : REG_V1BAR, buf->yuv_p.v);
-	}
+	mcam_write_yuv_bases(cam, frame, dma_handle);
 }
 
 /*
@@ -603,6 +594,7 @@ static void mcam_dma_contig_done(struct mcam_camera *cam, int frame)
 
 	if (!test_bit(CF_SINGLE_BUFFER, &cam->flags)) {
 		cam->frame_state.delivered++;
+		cam->vb_bufs[frame] = NULL;
 		mcam_buffer_done(cam, frame, &buf->vb_buf);
 	}
 	mcam_set_contig_buffer(cam, frame);
@@ -752,12 +744,6 @@ static void mcam_ctlr_image(struct mcam_camera *cam)
 		widthy = fmt->width * 2;
 		widthuv = 0;
 		break;
-	case V4L2_PIX_FMT_JPEG:
-		imgsz_h = (fmt->sizeimage / fmt->bytesperline) << IMGSZ_V_SHIFT;
-		widthy = fmt->bytesperline;
-		widthuv = 0;
-		break;
-	case V4L2_PIX_FMT_YUV422P:
 	case V4L2_PIX_FMT_YUV420:
 	case V4L2_PIX_FMT_YVU420:
 		widthy = fmt->width;
@@ -766,6 +752,7 @@ static void mcam_ctlr_image(struct mcam_camera *cam)
 	default:
 		widthy = fmt->bytesperline;
 		widthuv = 0;
+		break;
 	}
 
 	mcam_reg_write_mask(cam, REG_IMGPITCH, widthuv << 16 | widthy,
@@ -777,10 +764,6 @@ static void mcam_ctlr_image(struct mcam_camera *cam)
 	 * Tell the controller about the image format we are using.
 	 */
 	switch (fmt->pixelformat) {
-	case V4L2_PIX_FMT_YUV422P:
-		mcam_reg_write_mask(cam, REG_CTRL0,
-			C0_DF_YUV | C0_YUV_PLANAR | C0_YUVE_YVYU, C0_DF_MASK);
-		break;
 	case V4L2_PIX_FMT_YUV420:
 	case V4L2_PIX_FMT_YVU420:
 		mcam_reg_write_mask(cam, REG_CTRL0,
@@ -794,19 +777,18 @@ static void mcam_ctlr_image(struct mcam_camera *cam)
 		mcam_reg_write_mask(cam, REG_CTRL0,
 			C0_DF_YUV | C0_YUV_PACKED | C0_YUVE_SWAP24, C0_DF_MASK);
 		break;
-	case V4L2_PIX_FMT_JPEG:
-		mcam_reg_write_mask(cam, REG_CTRL0,
-			C0_DF_YUV | C0_YUV_PACKED | C0_YUVE_YUYV, C0_DF_MASK);
-		break;
-	case V4L2_PIX_FMT_RGB444:
+	case V4L2_PIX_FMT_XRGB444:
 		mcam_reg_write_mask(cam, REG_CTRL0,
-			C0_DF_RGB | C0_RGBF_444 | C0_RGB4_XRGB, C0_DF_MASK);
-		/* Alpha value? */
+			C0_DF_RGB | C0_RGBF_444 | C0_RGB4_XBGR, C0_DF_MASK);
 		break;
 	case V4L2_PIX_FMT_RGB565:
 		mcam_reg_write_mask(cam, REG_CTRL0,
 			C0_DF_RGB | C0_RGBF_565 | C0_RGB5_BGGR, C0_DF_MASK);
 		break;
+	case V4L2_PIX_FMT_SBGGR8:
+		mcam_reg_write_mask(cam, REG_CTRL0,
+			C0_DF_RGB | C0_RGB5_GRBG, C0_DF_MASK);
+		break;
 	default:
 		cam_err(cam, "camera: unknown format: %#x\n", fmt->pixelformat);
 		break;
@@ -969,7 +951,6 @@ static int mcam_cam_init(struct mcam_camera *cam)
 {
 	int ret;
 
-	mutex_lock(&cam->s_mutex);
 	if (cam->state != S_NOTREADY)
 		cam_warn(cam, "Cam init with device in funky state %d",
 				cam->state);
@@ -977,7 +958,6 @@ static int mcam_cam_init(struct mcam_camera *cam)
 	/* Get/set parameters? */
 	cam->state = S_IDLE;
 	mcam_ctlr_power_down(cam);
-	mutex_unlock(&cam->s_mutex);
 	return ret;
 }
 
@@ -998,13 +978,15 @@ static int mcam_cam_set_flip(struct mcam_camera *cam)
 
 static int mcam_cam_configure(struct mcam_camera *cam)
 {
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	int ret;
 
-	v4l2_fill_mbus_format(&mbus_fmt, &cam->pix_format, cam->mbus_code);
+	v4l2_fill_mbus_format(&format.format, &cam->pix_format, cam->mbus_code);
 	ret = sensor_call(cam, core, init, 0);
 	if (ret == 0)
-		ret = sensor_call(cam, video, s_mbus_fmt, &mbus_fmt);
+		ret = sensor_call(cam, pad, set_fmt, NULL, &format);
 	/*
 	 * OV7670 does weird things if flip is set *before* format...
 	 */
@@ -1073,7 +1055,9 @@ static int mcam_vb_queue_setup(struct vb2_queue *vq,
 	struct mcam_camera *cam = vb2_get_drv_priv(vq);
 	int minbufs = (cam->buffer_mode == B_DMA_contig) ? 3 : 2;
 
-	sizes[0] = cam->pix_format.sizeimage;
+	if (fmt && fmt->fmt.pix.sizeimage < cam->pix_format.sizeimage)
+		return -EINVAL;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : cam->pix_format.sizeimage;
 	*num_planes = 1; /* Someday we have to support planar formats... */
 	if (*nbufs < minbufs)
 		*nbufs = minbufs;
@@ -1102,6 +1086,30 @@ static void mcam_vb_buf_queue(struct vb2_buffer *vb)
 		mcam_read_setup(cam);
 }
 
+static void mcam_vb_requeue_bufs(struct vb2_queue *vq,
+				 enum vb2_buffer_state state)
+{
+	struct mcam_camera *cam = vb2_get_drv_priv(vq);
+	struct mcam_vb_buffer *buf, *node;
+	unsigned long flags;
+	unsigned i;
+
+	spin_lock_irqsave(&cam->dev_lock, flags);
+	list_for_each_entry_safe(buf, node, &cam->buffers, queue) {
+		vb2_buffer_done(&buf->vb_buf, state);
+		list_del(&buf->queue);
+	}
+	for (i = 0; i < MAX_DMA_BUFS; i++) {
+		buf = cam->vb_bufs[i];
+
+		if (buf) {
+			vb2_buffer_done(&buf->vb_buf, state);
+			cam->vb_bufs[i] = NULL;
+		}
+	}
+	spin_unlock_irqrestore(&cam->dev_lock, flags);
+}
+
 /*
  * These need to be called with the mutex held from vb2
  */
@@ -1109,11 +1117,15 @@ static int mcam_vb_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct mcam_camera *cam = vb2_get_drv_priv(vq);
 	unsigned int frame;
+	int ret;
 
 	if (cam->state != S_IDLE) {
-		INIT_LIST_HEAD(&cam->buffers);
+		mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_QUEUED);
 		return -EINVAL;
 	}
+	cam->frame_state.frames = 0;
+	cam->frame_state.singles = 0;
+	cam->frame_state.delivered = 0;
 	cam->sequence = 0;
 	/*
 	 * Videobuf2 sneakily hoards all the buffers and won't
@@ -1134,14 +1146,19 @@ static int mcam_vb_start_streaming(struct vb2_queue *vq, unsigned int count)
 	for (frame = 0; frame < cam->nbufs; frame++)
 		clear_bit(CF_FRAME_SOF0 + frame, &cam->flags);
 
-	return mcam_read_setup(cam);
+	ret = mcam_read_setup(cam);
+	if (ret)
+		mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_QUEUED);
+	return ret;
 }
 
 static void mcam_vb_stop_streaming(struct vb2_queue *vq)
 {
 	struct mcam_camera *cam = vb2_get_drv_priv(vq);
-	unsigned long flags;
 
+	cam_dbg(cam, "stop_streaming: %d frames, %d singles, %d delivered\n",
+			cam->frame_state.frames, cam->frame_state.singles,
+			cam->frame_state.delivered);
 	if (cam->state == S_BUFWAIT) {
 		/* They never gave us buffers */
 		cam->state = S_IDLE;
@@ -1160,9 +1177,7 @@ static void mcam_vb_stop_streaming(struct vb2_queue *vq)
 	 * VB2 reclaims the buffers, so we need to forget
 	 * about them.
 	 */
-	spin_lock_irqsave(&cam->dev_lock, flags);
-	INIT_LIST_HEAD(&cam->buffers);
-	spin_unlock_irqrestore(&cam->dev_lock, flags);
+	mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_ERROR);
 }
 
 
@@ -1246,14 +1261,15 @@ static int mcam_setup_vb2(struct mcam_camera *cam)
 	vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	vq->drv_priv = cam;
 	vq->lock = &cam->s_mutex;
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+	vq->buf_struct_size = sizeof(struct mcam_vb_buffer);
 	INIT_LIST_HEAD(&cam->buffers);
 	switch (cam->buffer_mode) {
 	case B_DMA_contig:
 #ifdef MCAM_MODE_DMA_CONTIG
 		vq->ops = &mcam_vb2_ops;
 		vq->mem_ops = &vb2_dma_contig_memops;
-		vq->buf_struct_size = sizeof(struct mcam_vb_buffer);
-		vq->io_modes = VB2_MMAP | VB2_USERPTR;
 		cam->dma_setup = mcam_ctlr_dma_contig;
 		cam->frame_complete = mcam_dma_contig_done;
 		cam->vb_alloc_ctx = vb2_dma_contig_init_ctx(cam->dev);
@@ -1265,8 +1281,6 @@ static int mcam_setup_vb2(struct mcam_camera *cam)
 #ifdef MCAM_MODE_DMA_SG
 		vq->ops = &mcam_vb2_sg_ops;
 		vq->mem_ops = &vb2_dma_sg_memops;
-		vq->buf_struct_size = sizeof(struct mcam_vb_buffer);
-		vq->io_modes = VB2_MMAP | VB2_USERPTR;
 		cam->dma_setup = mcam_ctlr_dma_sg;
 		cam->frame_complete = mcam_dma_sg_done;
 		cam->vb_alloc_ctx_sg = vb2_dma_sg_init_ctx(cam->dev);
@@ -1280,8 +1294,6 @@ static int mcam_setup_vb2(struct mcam_camera *cam)
 				(unsigned long) cam);
 		vq->ops = &mcam_vb2_ops;
 		vq->mem_ops = &vb2_vmalloc_memops;
-		vq->buf_struct_size = sizeof(struct mcam_vb_buffer);
-		vq->io_modes = VB2_MMAP;
 		cam->dma_setup = mcam_ctlr_dma_vmalloc;
 		cam->frame_complete = mcam_vmalloc_done;
 #endif
@@ -1292,7 +1304,6 @@ static int mcam_setup_vb2(struct mcam_camera *cam)
 
 static void mcam_cleanup_vb2(struct mcam_camera *cam)
 {
-	vb2_queue_release(&cam->vb_queue);
 #ifdef MCAM_MODE_DMA_CONTIG
 	if (cam->buffer_mode == B_DMA_contig)
 		vb2_dma_contig_cleanup_ctx(cam->vb_alloc_ctx);
@@ -1309,86 +1320,14 @@ static void mcam_cleanup_vb2(struct mcam_camera *cam)
  * The long list of V4L2 ioctl() operations.
  */
 
-static int mcam_vidioc_streamon(struct file *filp, void *priv,
-		enum v4l2_buf_type type)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_streamon(&cam->vb_queue, type);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
-
-static int mcam_vidioc_streamoff(struct file *filp, void *priv,
-		enum v4l2_buf_type type)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_streamoff(&cam->vb_queue, type);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
-
-static int mcam_vidioc_reqbufs(struct file *filp, void *priv,
-		struct v4l2_requestbuffers *req)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_reqbufs(&cam->vb_queue, req);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
-
-static int mcam_vidioc_querybuf(struct file *filp, void *priv,
-		struct v4l2_buffer *buf)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_querybuf(&cam->vb_queue, buf);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
-static int mcam_vidioc_qbuf(struct file *filp, void *priv,
-		struct v4l2_buffer *buf)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_qbuf(&cam->vb_queue, buf);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
-static int mcam_vidioc_dqbuf(struct file *filp, void *priv,
-		struct v4l2_buffer *buf)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_dqbuf(&cam->vb_queue, buf, filp->f_flags & O_NONBLOCK);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
 static int mcam_vidioc_querycap(struct file *file, void *priv,
 		struct v4l2_capability *cap)
 {
+	struct mcam_camera *cam = video_drvdata(file);
+
 	strcpy(cap->driver, "marvell_ccic");
 	strcpy(cap->card, "marvell_ccic");
+	strlcpy(cap->bus_info, cam->bus_info, sizeof(cap->bus_info));
 	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
 		V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
@@ -1410,36 +1349,38 @@ static int mcam_vidioc_enum_fmt_vid_cap(struct file *filp,
 static int mcam_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
 		struct v4l2_format *fmt)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(filp);
 	struct mcam_format_struct *f;
 	struct v4l2_pix_format *pix = &fmt->fmt.pix;
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
 	int ret;
 
 	f = mcam_find_format(pix->pixelformat);
 	pix->pixelformat = f->pixelformat;
-	v4l2_fill_mbus_format(&mbus_fmt, pix, f->mbus_code);
-	mutex_lock(&cam->s_mutex);
-	ret = sensor_call(cam, video, try_mbus_fmt, &mbus_fmt);
-	mutex_unlock(&cam->s_mutex);
-	v4l2_fill_pix_format(pix, &mbus_fmt);
+	v4l2_fill_mbus_format(&format.format, pix, f->mbus_code);
+	ret = sensor_call(cam, pad, set_fmt, &pad_cfg, &format);
+	v4l2_fill_pix_format(pix, &format.format);
+	pix->bytesperline = pix->width * f->bpp;
 	switch (f->pixelformat) {
 	case V4L2_PIX_FMT_YUV420:
 	case V4L2_PIX_FMT_YVU420:
-		pix->bytesperline = pix->width * 3 / 2;
+		pix->sizeimage = pix->height * pix->bytesperline * 3 / 2;
 		break;
 	default:
-		pix->bytesperline = pix->width * f->bpp;
+		pix->sizeimage = pix->height * pix->bytesperline;
 		break;
 	}
-	pix->sizeimage = pix->height * pix->bytesperline;
+	pix->colorspace = V4L2_COLORSPACE_SRGB;
 	return ret;
 }
 
 static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
 		struct v4l2_format *fmt)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(filp);
 	struct mcam_format_struct *f;
 	int ret;
 
@@ -1447,7 +1388,7 @@ static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
 	 * Can't do anything if the device is not idle
 	 * Also can't if there are streaming buffers in place.
 	 */
-	if (cam->state != S_IDLE || cam->vb_queue.num_buffers > 0)
+	if (cam->state != S_IDLE || vb2_is_busy(&cam->vb_queue))
 		return -EBUSY;
 
 	f = mcam_find_format(fmt->fmt.pix.pixelformat);
@@ -1462,7 +1403,6 @@ static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
 	 * Now we start to change things for real, so let's do it
 	 * under lock.
 	 */
-	mutex_lock(&cam->s_mutex);
 	cam->pix_format = fmt->fmt.pix;
 	cam->mbus_code = f->mbus_code;
 
@@ -1476,7 +1416,6 @@ static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
 	}
 	mcam_set_config_needed(cam, 1);
 out:
-	mutex_unlock(&cam->s_mutex);
 	return ret;
 }
 
@@ -1488,7 +1427,7 @@ out:
 static int mcam_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
 		struct v4l2_format *f)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(filp);
 
 	f->fmt.pix = cam->pix_format;
 	return 0;
@@ -1504,7 +1443,6 @@ static int mcam_vidioc_enum_input(struct file *filp, void *priv,
 		return -EINVAL;
 
 	input->type = V4L2_INPUT_TYPE_CAMERA;
-	input->std = V4L2_STD_ALL; /* Not sure what should go here */
 	strcpy(input->name, "Camera");
 	return 0;
 }
@@ -1522,18 +1460,6 @@ static int mcam_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
 	return 0;
 }
 
-/* from vivi.c */
-static int mcam_vidioc_s_std(struct file *filp, void *priv, v4l2_std_id a)
-{
-	return 0;
-}
-
-static int mcam_vidioc_g_std(struct file *filp, void *priv, v4l2_std_id *a)
-{
-	*a = V4L2_STD_NTSC_M;
-	return 0;
-}
-
 /*
  * G/S_PARM.  Most of this is done by the sensor, but we are
  * the level which controls the number of read buffers.
@@ -1541,12 +1467,10 @@ static int mcam_vidioc_g_std(struct file *filp, void *priv, v4l2_std_id *a)
 static int mcam_vidioc_g_parm(struct file *filp, void *priv,
 		struct v4l2_streamparm *parms)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(filp);
 	int ret;
 
-	mutex_lock(&cam->s_mutex);
 	ret = sensor_call(cam, video, g_parm, parms);
-	mutex_unlock(&cam->s_mutex);
 	parms->parm.capture.readbuffers = n_dma_bufs;
 	return ret;
 }
@@ -1554,12 +1478,10 @@ static int mcam_vidioc_g_parm(struct file *filp, void *priv,
 static int mcam_vidioc_s_parm(struct file *filp, void *priv,
 		struct v4l2_streamparm *parms)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(filp);
 	int ret;
 
-	mutex_lock(&cam->s_mutex);
 	ret = sensor_call(cam, video, s_parm, parms);
-	mutex_unlock(&cam->s_mutex);
 	parms->parm.capture.readbuffers = n_dma_bufs;
 	return ret;
 }
@@ -1567,7 +1489,7 @@ static int mcam_vidioc_s_parm(struct file *filp, void *priv,
 static int mcam_vidioc_enum_framesizes(struct file *filp, void *priv,
 		struct v4l2_frmsizeenum *sizes)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(filp);
 	struct mcam_format_struct *f;
 	struct v4l2_subdev_frame_size_enum fse = {
 		.index = sizes->index,
@@ -1579,9 +1501,7 @@ static int mcam_vidioc_enum_framesizes(struct file *filp, void *priv,
 	if (f->pixelformat != sizes->pixel_format)
 		return -EINVAL;
 	fse.code = f->mbus_code;
-	mutex_lock(&cam->s_mutex);
 	ret = sensor_call(cam, pad, enum_frame_size, NULL, &fse);
-	mutex_unlock(&cam->s_mutex);
 	if (ret)
 		return ret;
 	if (fse.min_width == fse.max_width &&
@@ -1604,7 +1524,7 @@ static int mcam_vidioc_enum_framesizes(struct file *filp, void *priv,
 static int mcam_vidioc_enum_frameintervals(struct file *filp, void *priv,
 		struct v4l2_frmivalenum *interval)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(filp);
 	struct mcam_format_struct *f;
 	struct v4l2_subdev_frame_interval_enum fie = {
 		.index = interval->index,
@@ -1618,9 +1538,7 @@ static int mcam_vidioc_enum_frameintervals(struct file *filp, void *priv,
 	if (f->pixelformat != interval->pixel_format)
 		return -EINVAL;
 	fie.code = f->mbus_code;
-	mutex_lock(&cam->s_mutex);
 	ret = sensor_call(cam, pad, enum_frame_interval, NULL, &fie);
-	mutex_unlock(&cam->s_mutex);
 	if (ret)
 		return ret;
 	interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
@@ -1632,7 +1550,7 @@ static int mcam_vidioc_enum_frameintervals(struct file *filp, void *priv,
 static int mcam_vidioc_g_register(struct file *file, void *priv,
 		struct v4l2_dbg_register *reg)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(file);
 
 	if (reg->reg > cam->regs_size - 4)
 		return -EINVAL;
@@ -1644,7 +1562,7 @@ static int mcam_vidioc_g_register(struct file *file, void *priv,
 static int mcam_vidioc_s_register(struct file *file, void *priv,
 		const struct v4l2_dbg_register *reg)
 {
-	struct mcam_camera *cam = priv;
+	struct mcam_camera *cam = video_drvdata(file);
 
 	if (reg->reg > cam->regs_size - 4)
 		return -EINVAL;
@@ -1662,18 +1580,20 @@ static const struct v4l2_ioctl_ops mcam_v4l_ioctl_ops = {
 	.vidioc_enum_input	= mcam_vidioc_enum_input,
 	.vidioc_g_input		= mcam_vidioc_g_input,
 	.vidioc_s_input		= mcam_vidioc_s_input,
-	.vidioc_s_std		= mcam_vidioc_s_std,
-	.vidioc_g_std		= mcam_vidioc_g_std,
-	.vidioc_reqbufs		= mcam_vidioc_reqbufs,
-	.vidioc_querybuf	= mcam_vidioc_querybuf,
-	.vidioc_qbuf		= mcam_vidioc_qbuf,
-	.vidioc_dqbuf		= mcam_vidioc_dqbuf,
-	.vidioc_streamon	= mcam_vidioc_streamon,
-	.vidioc_streamoff	= mcam_vidioc_streamoff,
+	.vidioc_reqbufs		= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs	= vb2_ioctl_create_bufs,
+	.vidioc_querybuf	= vb2_ioctl_querybuf,
+	.vidioc_qbuf		= vb2_ioctl_qbuf,
+	.vidioc_dqbuf		= vb2_ioctl_dqbuf,
+	.vidioc_expbuf		= vb2_ioctl_expbuf,
+	.vidioc_streamon	= vb2_ioctl_streamon,
+	.vidioc_streamoff	= vb2_ioctl_streamoff,
 	.vidioc_g_parm		= mcam_vidioc_g_parm,
 	.vidioc_s_parm		= mcam_vidioc_s_parm,
 	.vidioc_enum_framesizes = mcam_vidioc_enum_framesizes,
 	.vidioc_enum_frameintervals = mcam_vidioc_enum_frameintervals,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.vidioc_g_register	= mcam_vidioc_g_register,
 	.vidioc_s_register	= mcam_vidioc_s_register,
@@ -1687,43 +1607,36 @@ static const struct v4l2_ioctl_ops mcam_v4l_ioctl_ops = {
 static int mcam_v4l_open(struct file *filp)
 {
 	struct mcam_camera *cam = video_drvdata(filp);
-	int ret = 0;
-
-	filp->private_data = cam;
+	int ret;
 
-	cam->frame_state.frames = 0;
-	cam->frame_state.singles = 0;
-	cam->frame_state.delivered = 0;
 	mutex_lock(&cam->s_mutex);
-	if (cam->users == 0) {
-		ret = mcam_setup_vb2(cam);
-		if (ret)
-			goto out;
+	ret = v4l2_fh_open(filp);
+	if (ret)
+		goto out;
+	if (v4l2_fh_is_singular_file(filp)) {
 		ret = mcam_ctlr_power_up(cam);
 		if (ret)
 			goto out;
 		__mcam_cam_reset(cam);
 		mcam_set_config_needed(cam, 1);
 	}
-	(cam->users)++;
 out:
 	mutex_unlock(&cam->s_mutex);
+	if (ret)
+		v4l2_fh_release(filp);
 	return ret;
 }
 
 
 static int mcam_v4l_release(struct file *filp)
 {
-	struct mcam_camera *cam = filp->private_data;
+	struct mcam_camera *cam = video_drvdata(filp);
+	bool last_open;
 
-	cam_dbg(cam, "Release, %d frames, %d singles, %d delivered\n",
-			cam->frame_state.frames, cam->frame_state.singles,
-			cam->frame_state.delivered);
 	mutex_lock(&cam->s_mutex);
-	(cam->users)--;
-	if (cam->users == 0) {
-		mcam_ctlr_stop_dma(cam);
-		mcam_cleanup_vb2(cam);
+	last_open = v4l2_fh_is_singular_file(filp);
+	_vb2_fop_release(filp, NULL);
+	if (last_open) {
 		mcam_disable_mipi(cam);
 		mcam_ctlr_power_down(cam);
 		if (cam->buffer_mode == B_vmalloc && alloc_bufs_at_read)
@@ -1734,54 +1647,13 @@ static int mcam_v4l_release(struct file *filp)
 	return 0;
 }
 
-static ssize_t mcam_v4l_read(struct file *filp,
-		char __user *buffer, size_t len, loff_t *pos)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_read(&cam->vb_queue, buffer, len, pos,
-			filp->f_flags & O_NONBLOCK);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
-
-
-static unsigned int mcam_v4l_poll(struct file *filp,
-		struct poll_table_struct *pt)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_poll(&cam->vb_queue, filp, pt);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
-
-static int mcam_v4l_mmap(struct file *filp, struct vm_area_struct *vma)
-{
-	struct mcam_camera *cam = filp->private_data;
-	int ret;
-
-	mutex_lock(&cam->s_mutex);
-	ret = vb2_mmap(&cam->vb_queue, vma);
-	mutex_unlock(&cam->s_mutex);
-	return ret;
-}
-
-
-
 static const struct v4l2_file_operations mcam_v4l_fops = {
 	.owner = THIS_MODULE,
 	.open = mcam_v4l_open,
 	.release = mcam_v4l_release,
-	.read = mcam_v4l_read,
-	.poll = mcam_v4l_poll,
-	.mmap = mcam_v4l_mmap,
+	.read = vb2_fop_read,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
 	.unlocked_ioctl = video_ioctl2,
 };
 
@@ -1792,8 +1664,6 @@ static const struct v4l2_file_operations mcam_v4l_fops = {
  */
 static struct video_device mcam_v4l_template = {
 	.name = "mcam",
-	.tvnorms = V4L2_STD_NTSC_M,
-
 	.fops = &mcam_v4l_fops,
 	.ioctl_ops = &mcam_v4l_ioctl_ops,
 	.release = video_device_release_empty,
@@ -1811,7 +1681,7 @@ static void mcam_frame_complete(struct mcam_camera *cam, int frame)
 	set_bit(frame, &cam->flags);
 	clear_bit(CF_DMA_ACTIVE, &cam->flags);
 	cam->next_buf = frame;
-	cam->buf_seq[frame] = ++(cam->sequence);
+	cam->buf_seq[frame] = cam->sequence++;
 	cam->frame_state.frames++;
 	/*
 	 * "This should never happen"
@@ -1924,10 +1794,17 @@ int mccic_register(struct mcam_camera *cam)
 	mcam_set_config_needed(cam, 1);
 	cam->pix_format = mcam_def_pix_format;
 	cam->mbus_code = mcam_def_mbus_code;
-	INIT_LIST_HEAD(&cam->buffers);
 	mcam_ctlr_init(cam);
 
 	/*
+	 * Get the v4l2 setup done.
+	 */
+	ret = v4l2_ctrl_handler_init(&cam->ctrl_handler, 10);
+	if (ret)
+		goto out_unregister;
+	cam->v4l2_dev.ctrl_handler = &cam->ctrl_handler;
+
+	/*
 	 * Try to find the sensor.
 	 */
 	sensor_cfg.clock_speed = cam->clock_speed;
@@ -1943,21 +1820,22 @@ int mccic_register(struct mcam_camera *cam)
 	ret = mcam_cam_init(cam);
 	if (ret)
 		goto out_unregister;
-	/*
-	 * Get the v4l2 setup done.
-	 */
-	ret = v4l2_ctrl_handler_init(&cam->ctrl_handler, 10);
+
+	ret = mcam_setup_vb2(cam);
 	if (ret)
 		goto out_unregister;
-	cam->v4l2_dev.ctrl_handler = &cam->ctrl_handler;
 
 	mutex_lock(&cam->s_mutex);
 	cam->vdev = mcam_v4l_template;
 	cam->vdev.v4l2_dev = &cam->v4l2_dev;
+	cam->vdev.lock = &cam->s_mutex;
+	cam->vdev.queue = &cam->vb_queue;
 	video_set_drvdata(&cam->vdev, cam);
 	ret = video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1);
-	if (ret)
-		goto out;
+	if (ret) {
+		mutex_unlock(&cam->s_mutex);
+		goto out_unregister;
+	}
 
 	/*
 	 * If so requested, try to get our DMA buffers now.
@@ -1968,11 +1846,11 @@ int mccic_register(struct mcam_camera *cam)
 					" will try again later.");
 	}
 
-out:
-	v4l2_ctrl_handler_free(&cam->ctrl_handler);
 	mutex_unlock(&cam->s_mutex);
-	return ret;
+	return 0;
+
 out_unregister:
+	v4l2_ctrl_handler_free(&cam->ctrl_handler);
 	v4l2_device_unregister(&cam->v4l2_dev);
 	return ret;
 }
@@ -1986,11 +1864,11 @@ void mccic_shutdown(struct mcam_camera *cam)
 	 * take it down again will wedge the machine, which is frowned
 	 * upon.
 	 */
-	if (cam->users > 0) {
+	if (!list_empty(&cam->vdev.fh_list)) {
 		cam_warn(cam, "Removing a device with users!\n");
 		mcam_ctlr_power_down(cam);
 	}
-	vb2_queue_release(&cam->vb_queue);
+	mcam_cleanup_vb2(cam);
 	if (cam->buffer_mode == B_vmalloc)
 		mcam_free_dma_bufs(cam);
 	video_unregister_device(&cam->vdev);
@@ -2006,7 +1884,7 @@ void mccic_shutdown(struct mcam_camera *cam)
 void mccic_suspend(struct mcam_camera *cam)
 {
 	mutex_lock(&cam->s_mutex);
-	if (cam->users > 0) {
+	if (!list_empty(&cam->vdev.fh_list)) {
 		enum mcam_state cstate = cam->state;
 
 		mcam_ctlr_stop_dma(cam);
@@ -2021,7 +1899,7 @@ int mccic_resume(struct mcam_camera *cam)
 	int ret = 0;
 
 	mutex_lock(&cam->s_mutex);
-	if (cam->users > 0) {
+	if (!list_empty(&cam->vdev.fh_list)) {
 		ret = mcam_ctlr_power_up(cam);
 		if (ret) {
 			mutex_unlock(&cam->s_mutex);
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.h b/drivers/media/platform/marvell-ccic/mcam-core.h
index 7ffdf4dbaf8c..97167f6ffd1e 100644
--- a/drivers/media/platform/marvell-ccic/mcam-core.h
+++ b/drivers/media/platform/marvell-ccic/mcam-core.h
@@ -146,7 +146,6 @@ struct mcam_camera {
 	struct v4l2_ctrl_handler ctrl_handler;
 	enum mcam_state state;
 	unsigned long flags;		/* Buffer status, mainly (dev_lock) */
-	int users;			/* How many open FDs */
 
 	struct mcam_frame_state frame_state;	/* Frame state counter */
 	/*
@@ -163,6 +162,8 @@ struct mcam_camera {
 	unsigned int nbufs;		/* How many are alloc'd */
 	int next_buf;			/* Next to consume (dev_lock) */
 
+	char bus_info[32];		/* querycap bus_info */
+
 	/* DMA buffers - vmalloc mode */
 #ifdef MCAM_MODE_VMALLOC
 	unsigned int dma_buf_size;	/* allocated size */
diff --git a/drivers/media/platform/marvell-ccic/mmp-driver.c b/drivers/media/platform/marvell-ccic/mmp-driver.c
index 0ed9b3adfcdf..b5f165a68566 100644
--- a/drivers/media/platform/marvell-ccic/mmp-driver.c
+++ b/drivers/media/platform/marvell-ccic/mmp-driver.c
@@ -371,6 +371,7 @@ static int mmpcam_probe(struct platform_device *pdev)
 	mcam->lane = pdata->lane;
 	mcam->chip_id = MCAM_ARMADA610;
 	mcam->buffer_mode = B_DMA_sg;
+	strlcpy(mcam->bus_info, "platform:mmp-camera", sizeof(mcam->bus_info));
 	spin_lock_init(&mcam->dev_lock);
 	/*
 	 * Get our I/O memory.
diff --git a/drivers/media/platform/omap/omap_vout.c b/drivers/media/platform/omap/omap_vout.c
index 17b189a81ec5..f09c5f17a42f 100644
--- a/drivers/media/platform/omap/omap_vout.c
+++ b/drivers/media/platform/omap/omap_vout.c
@@ -445,7 +445,7 @@ static int omapvid_init(struct omap_vout_device *vout, u32 addr)
 	int ret = 0, i;
 	struct v4l2_window *win;
 	struct omap_overlay *ovl;
-	int posx, posy, outw, outh, temp;
+	int posx, posy, outw, outh;
 	struct omap_video_timings *timing;
 	struct omapvideo_info *ovid = &vout->vid_info;
 
@@ -468,9 +468,7 @@ static int omapvid_init(struct omap_vout_device *vout, u32 addr)
 			/* Invert the height and width for 90
 			 * and 270 degree rotation
 			 */
-			temp = outw;
-			outw = outh;
-			outh = temp;
+			swap(outw, outh);
 			posy = (timing->y_res - win->w.width) - win->w.left;
 			posx = win->w.top;
 			break;
@@ -481,9 +479,7 @@ static int omapvid_init(struct omap_vout_device *vout, u32 addr)
 			break;
 
 		case dss_rotation_270_degree:
-			temp = outw;
-			outw = outh;
-			outh = temp;
+			swap(outw, outh);
 			posy = win->w.left;
 			posx = (timing->x_res - win->w.height) - win->w.top;
 			break;
diff --git a/drivers/media/platform/omap3isp/isppreview.c b/drivers/media/platform/omap3isp/isppreview.c
index 15cb254ccc39..13803270d104 100644
--- a/drivers/media/platform/omap3isp/isppreview.c
+++ b/drivers/media/platform/omap3isp/isppreview.c
@@ -929,14 +929,10 @@ static void preview_setup_hw(struct isp_prev_device *prev, u32 update,
 			     u32 active)
 {
 	unsigned int i;
-	u32 features;
 
 	if (update == 0)
 		return;
 
-	features = (prev->params.params[0].features & active)
-		 | (prev->params.params[1].features & ~active);
-
 	for (i = 0; i < ARRAY_SIZE(update_attrs); i++) {
 		const struct preview_update *attr = &update_attrs[i];
 		struct prev_params *params;
diff --git a/drivers/media/platform/s3c-camif/camif-capture.c b/drivers/media/platform/s3c-camif/camif-capture.c
index f6a61b9ceff4..76e6289a5612 100644
--- a/drivers/media/platform/s3c-camif/camif-capture.c
+++ b/drivers/media/platform/s3c-camif/camif-capture.c
@@ -115,7 +115,7 @@ static int sensor_set_power(struct camif_dev *camif, int on)
 	struct cam_sensor *sensor = &camif->sensor;
 	int err = 0;
 
-	if (!on == camif->sensor.power_count)
+	if (camif->sensor.power_count == !on)
 		err = v4l2_subdev_call(sensor->sd, core, s_power, on);
 	if (!err)
 		sensor->power_count += on ? 1 : -1;
@@ -131,7 +131,7 @@ static int sensor_set_streaming(struct camif_dev *camif, int on)
 	struct cam_sensor *sensor = &camif->sensor;
 	int err = 0;
 
-	if (!on == camif->sensor.stream_count)
+	if (camif->sensor.stream_count == !on)
 		err = v4l2_subdev_call(sensor->sd, video, s_stream, on);
 	if (!err)
 		sensor->stream_count += on ? 1 : -1;
@@ -449,19 +449,22 @@ static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *pfmt,
 	struct camif_vp *vp = vb2_get_drv_priv(vq);
 	struct camif_dev *camif = vp->camif;
 	struct camif_frame *frame = &vp->out_frame;
-	const struct camif_fmt *fmt = vp->out_fmt;
+	const struct camif_fmt *fmt;
 	unsigned int size;
 
 	if (pfmt) {
 		pix = &pfmt->fmt.pix;
 		fmt = s3c_camif_find_format(vp, &pix->pixelformat, -1);
+		if (fmt == NULL)
+			return -EINVAL;
 		size = (pix->width * pix->height * fmt->depth) / 8;
 	} else {
+		fmt = vp->out_fmt;
+		if (fmt == NULL)
+			return -EINVAL;
 		size = (frame->f_width * frame->f_height * fmt->depth) / 8;
 	}
 
-	if (fmt == NULL)
-		return -EINVAL;
 	*num_planes = 1;
 
 	if (pix)
diff --git a/drivers/media/platform/s3c-camif/camif-core.c b/drivers/media/platform/s3c-camif/camif-core.c
index 2d5bd3ac7f81..f47b332f0418 100644
--- a/drivers/media/platform/s3c-camif/camif-core.c
+++ b/drivers/media/platform/s3c-camif/camif-core.c
@@ -628,7 +628,7 @@ static struct s3c_camif_drvdata s3c6410_camif_drvdata = {
 	.bus_clk_freq	= 133000000UL,
 };
 
-static struct platform_device_id s3c_camif_driver_ids[] = {
+static const struct platform_device_id s3c_camif_driver_ids[] = {
 	{
 		.name		= "s3c2440-camif",
 		.driver_data	= (unsigned long)&s3c244x_camif_drvdata,
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index ec3e1248923d..421a7c3b595b 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -787,7 +787,7 @@ static const struct of_device_id exynos_g2d_match[] = {
 };
 MODULE_DEVICE_TABLE(of, exynos_g2d_match);
 
-static struct platform_device_id g2d_driver_ids[] = {
+static const struct platform_device_id g2d_driver_ids[] = {
 	{
 		.name = "s5p-g2d",
 		.driver_data = (unsigned long)&g2d_drvdata_v3x,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index 8333fbc2fe96..8de61dc1e142 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -211,6 +211,7 @@ static void s5p_mfc_handle_frame_all_extracted(struct s5p_mfc_ctx *ctx)
 			dst_buf->b->v4l2_buf.field = V4L2_FIELD_NONE;
 		else
 			dst_buf->b->v4l2_buf.field = V4L2_FIELD_INTERLACED;
+		dst_buf->b->v4l2_buf.flags |= V4L2_BUF_FLAG_LAST;
 
 		ctx->dec_dst_flag &= ~(1 << dst_buf->b->v4l2_buf.index);
 		vb2_buffer_done(dst_buf->b, VB2_BUF_STATE_DONE);
@@ -1337,8 +1338,6 @@ static int s5p_mfc_runtime_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
 
-	if (!m_dev->alloc_ctx)
-		return 0;
 	atomic_set(&m_dev->pm.power, 1);
 	return 0;
 }
@@ -1463,7 +1462,7 @@ static struct s5p_mfc_variant mfc_drvdata_v8 = {
 	.fw_name[0]     = "s5p-mfc-v8.fw",
 };
 
-static struct platform_device_id mfc_driver_ids[] = {
+static const struct platform_device_id mfc_driver_ids[] = {
 	{
 		.name = "s5p-mfc",
 		.driver_data = (unsigned long)&mfc_drvdata_v5,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
index b09bcd140491..9a923b1a9bac 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -184,7 +184,7 @@ static int s5p_mfc_alloc_codec_buffers_v5(struct s5p_mfc_ctx *ctx)
 		ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_r, &ctx->bank2);
 		if (ret) {
 			mfc_err("Failed to allocate Bank2 temporary buffer\n");
-		s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
+			s5p_mfc_release_priv_buf(ctx->dev->mem_dev_l, &ctx->bank1);
 			return ret;
 		}
 		BUG_ON(ctx->bank2.dma & ((1 << MFC_BANK2_ALIGN_ORDER) - 1));
@@ -263,7 +263,7 @@ static void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
 static void s5p_mfc_write_info_v5(struct s5p_mfc_ctx *ctx, unsigned int data,
 			unsigned int ofs)
 {
-	writel(data, (void *)(ctx->shm.virt + ofs));
+	*(u32 *)(ctx->shm.virt + ofs) = data;
 	wmb();
 }
 
@@ -271,7 +271,7 @@ static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
 				unsigned long ofs)
 {
 	rmb();
-	return readl((void *)(ctx->shm.virt + ofs));
+	return *(u32 *)(ctx->shm.virt + ofs);
 }
 
 static void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
index cefad184fe96..12497f5ed8e9 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -1852,7 +1852,7 @@ static void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
 		unsigned int ofs)
 {
 	s5p_mfc_clock_on();
-	writel(data, (void *)((unsigned long)ofs));
+	writel(data, (void __iomem *)((unsigned long)ofs));
 	s5p_mfc_clock_off();
 }
 
@@ -1862,7 +1862,7 @@ s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned long ofs)
 	int ret;
 
 	s5p_mfc_clock_on();
-	ret = readl((void *)ofs);
+	ret = readl((void __iomem *)ofs);
 	s5p_mfc_clock_off();
 
 	return ret;
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
index 0e74aabf5f9a..79940757b34f 100644
--- a/drivers/media/platform/s5p-tv/hdmi_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmi_drv.c
@@ -96,7 +96,7 @@ struct hdmi_device {
 	struct hdmi_resources res;
 };
 
-static struct platform_device_id hdmi_driver_types[] = {
+static const struct platform_device_id hdmi_driver_types[] = {
 	{
 		.name		= "s5pv210-hdmi",
 	}, {
@@ -648,15 +648,20 @@ static int hdmi_g_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int hdmi_g_mbus_fmt(struct v4l2_subdev *sd,
-	  struct v4l2_mbus_framefmt *fmt)
+static int hdmi_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *fmt = &format->format;
 	struct hdmi_device *hdev = sd_to_hdmi_dev(sd);
 	const struct hdmi_timings *t = hdev->cur_conf;
 
 	dev_dbg(hdev->dev, "%s\n", __func__);
 	if (!hdev->cur_conf)
 		return -EINVAL;
+	if (format->pad)
+		return -EINVAL;
+
 	memset(fmt, 0, sizeof(*fmt));
 	fmt->width = t->hact.end - t->hact.beg;
 	fmt->height = t->vact[0].end - t->vact[0].beg;
@@ -712,18 +717,19 @@ static const struct v4l2_subdev_core_ops hdmi_sd_core_ops = {
 static const struct v4l2_subdev_video_ops hdmi_sd_video_ops = {
 	.s_dv_timings = hdmi_s_dv_timings,
 	.g_dv_timings = hdmi_g_dv_timings,
-	.g_mbus_fmt = hdmi_g_mbus_fmt,
 	.s_stream = hdmi_s_stream,
 };
 
 static const struct v4l2_subdev_pad_ops hdmi_sd_pad_ops = {
 	.enum_dv_timings = hdmi_enum_dv_timings,
 	.dv_timings_cap = hdmi_dv_timings_cap,
+	.get_fmt = hdmi_get_fmt,
 };
 
 static const struct v4l2_subdev_ops hdmi_sd_ops = {
 	.core = &hdmi_sd_core_ops,
 	.video = &hdmi_sd_video_ops,
+	.pad = &hdmi_sd_pad_ops,
 };
 
 static int hdmi_runtime_suspend(struct device *dev)
diff --git a/drivers/media/platform/s5p-tv/mixer_drv.c b/drivers/media/platform/s5p-tv/mixer_drv.c
index 2a9501d7e7c8..5ef67774971d 100644
--- a/drivers/media/platform/s5p-tv/mixer_drv.c
+++ b/drivers/media/platform/s5p-tv/mixer_drv.c
@@ -46,11 +46,15 @@ void mxr_get_mbus_fmt(struct mxr_device *mdev,
 	struct v4l2_mbus_framefmt *mbus_fmt)
 {
 	struct v4l2_subdev *sd;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	int ret;
 
 	mutex_lock(&mdev->mutex);
 	sd = to_outsd(mdev);
-	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, mbus_fmt);
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
+	*mbus_fmt = fmt.format;
 	WARN(ret, "failed to get mbus_fmt for output %s\n", sd->name);
 	mutex_unlock(&mdev->mutex);
 }
@@ -62,7 +66,10 @@ void mxr_streamer_get(struct mxr_device *mdev)
 	mxr_dbg(mdev, "%s(%d)\n", __func__, mdev->n_streamer);
 	if (mdev->n_streamer == 1) {
 		struct v4l2_subdev *sd = to_outsd(mdev);
-		struct v4l2_mbus_framefmt mbus_fmt;
+		struct v4l2_subdev_format fmt = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
+		struct v4l2_mbus_framefmt *mbus_fmt = &fmt.format;
 		struct mxr_resources *res = &mdev->res;
 		int ret;
 
@@ -72,12 +79,12 @@ void mxr_streamer_get(struct mxr_device *mdev)
 			clk_set_parent(res->sclk_mixer, res->sclk_hdmi);
 		mxr_reg_s_output(mdev, to_output(mdev)->cookie);
 
-		ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mbus_fmt);
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 		WARN(ret, "failed to get mbus_fmt for output %s\n", sd->name);
 		ret = v4l2_subdev_call(sd, video, s_stream, 1);
 		WARN(ret, "starting stream failed for output %s\n", sd->name);
 
-		mxr_reg_set_mbus_fmt(mdev, &mbus_fmt);
+		mxr_reg_set_mbus_fmt(mdev, mbus_fmt);
 		mxr_reg_streamon(mdev);
 		ret = mxr_reg_wait4vsync(mdev);
 		WARN(ret, "failed to get vsync (%d) from output\n", ret);
diff --git a/drivers/media/platform/s5p-tv/sdo_drv.c b/drivers/media/platform/s5p-tv/sdo_drv.c
index 3621af91d460..c75d4354d182 100644
--- a/drivers/media/platform/s5p-tv/sdo_drv.c
+++ b/drivers/media/platform/s5p-tv/sdo_drv.c
@@ -160,13 +160,17 @@ static int sdo_g_std_output(struct v4l2_subdev *sd, v4l2_std_id *std)
 	return 0;
 }
 
-static int sdo_g_mbus_fmt(struct v4l2_subdev *sd,
-	struct v4l2_mbus_framefmt *fmt)
+static int sdo_get_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *fmt = &format->format;
 	struct sdo_device *sdev = sd_to_sdev(sd);
 
 	if (!sdev->fmt)
 		return -ENXIO;
+	if (format->pad)
+		return -EINVAL;
 	/* all modes are 720 pixels wide */
 	fmt->width = 720;
 	fmt->height = sdev->fmt->height;
@@ -256,13 +260,17 @@ static const struct v4l2_subdev_video_ops sdo_sd_video_ops = {
 	.s_std_output = sdo_s_std_output,
 	.g_std_output = sdo_g_std_output,
 	.g_tvnorms_output = sdo_g_tvnorms_output,
-	.g_mbus_fmt = sdo_g_mbus_fmt,
 	.s_stream = sdo_s_stream,
 };
 
+static const struct v4l2_subdev_pad_ops sdo_sd_pad_ops = {
+	.get_fmt = sdo_get_fmt,
+};
+
 static const struct v4l2_subdev_ops sdo_sd_ops = {
 	.core = &sdo_sd_core_ops,
 	.video = &sdo_sd_video_ops,
+	.pad = &sdo_sd_pad_ops,
 };
 
 static int sdo_runtime_suspend(struct device *dev)
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index dde1ccc730be..8b799bae01b8 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -600,7 +600,7 @@ static void vou_adjust_output(struct sh_vou_geometry *geo, v4l2_std_id std)
 {
 	unsigned int best_err = UINT_MAX, best = geo->in_width,
 		width_max, height_max, img_height_max;
-	int i, idx = 0;
+	int i, idx_h = 0, idx_v = 0;
 
 	if (std & V4L2_STD_525_60) {
 		width_max = 858;
@@ -625,7 +625,7 @@ static void vou_adjust_output(struct sh_vou_geometry *geo, v4l2_std_id std)
 		err = abs(found - geo->output.width);
 		if (err < best_err) {
 			best_err = err;
-			idx = i;
+			idx_h = i;
 			best = found;
 		}
 		if (!err)
@@ -633,12 +633,12 @@ static void vou_adjust_output(struct sh_vou_geometry *geo, v4l2_std_id std)
 	}
 
 	geo->output.width = best;
-	geo->scale_idx_h = idx;
+	geo->scale_idx_h = idx_h;
 	if (geo->output.left + best > width_max)
 		geo->output.left = width_max - best;
 
 	pr_debug("%s(): W %u * %u/%u = %u\n", __func__, geo->in_width,
-		 vou_scale_h_num[idx], vou_scale_h_den[idx], best);
+		 vou_scale_h_num[idx_h], vou_scale_h_den[idx_h], best);
 
 	best_err = UINT_MAX;
 
@@ -655,7 +655,7 @@ static void vou_adjust_output(struct sh_vou_geometry *geo, v4l2_std_id std)
 		err = abs(found - geo->output.height);
 		if (err < best_err) {
 			best_err = err;
-			idx = i;
+			idx_v = i;
 			best = found;
 		}
 		if (!err)
@@ -663,12 +663,12 @@ static void vou_adjust_output(struct sh_vou_geometry *geo, v4l2_std_id std)
 	}
 
 	geo->output.height = best;
-	geo->scale_idx_v = idx;
+	geo->scale_idx_v = idx_v;
 	if (geo->output.top + best > height_max)
 		geo->output.top = height_max - best;
 
 	pr_debug("%s(): H %u * %u/%u = %u\n", __func__, geo->in_height,
-		 vou_scale_v_num[idx], vou_scale_v_den[idx], best);
+		 vou_scale_v_num[idx_v], vou_scale_v_den[idx_v], best);
 }
 
 static int sh_vou_s_fmt_vid_out(struct file *file, void *priv,
@@ -679,12 +679,14 @@ static int sh_vou_s_fmt_vid_out(struct file *file, void *priv,
 	unsigned int img_height_max;
 	int pix_idx;
 	struct sh_vou_geometry geo;
-	struct v4l2_mbus_framefmt mbfmt = {
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 		/* Revisit: is this the correct code? */
-		.code = MEDIA_BUS_FMT_YUYV8_2X8,
-		.field = V4L2_FIELD_INTERLACED,
-		.colorspace = V4L2_COLORSPACE_SMPTE170M,
+		.format.code = MEDIA_BUS_FMT_YUYV8_2X8,
+		.format.field = V4L2_FIELD_INTERLACED,
+		.format.colorspace = V4L2_COLORSPACE_SMPTE170M,
 	};
+	struct v4l2_mbus_framefmt *mbfmt = &format.format;
 	int ret;
 
 	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): %ux%u -> %ux%u\n", __func__,
@@ -720,27 +722,27 @@ static int sh_vou_s_fmt_vid_out(struct file *file, void *priv,
 
 	vou_adjust_output(&geo, vou_dev->std);
 
-	mbfmt.width = geo.output.width;
-	mbfmt.height = geo.output.height;
-	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, video,
-					 s_mbus_fmt, &mbfmt);
+	mbfmt->width = geo.output.width;
+	mbfmt->height = geo.output.height;
+	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, pad,
+					 set_fmt, NULL, &format);
 	/* Must be implemented, so, don't check for -ENOIOCTLCMD */
 	if (ret < 0)
 		return ret;
 
 	dev_dbg(vou_dev->v4l2_dev.dev, "%s(): %ux%u -> %ux%u\n", __func__,
-		geo.output.width, geo.output.height, mbfmt.width, mbfmt.height);
+		geo.output.width, geo.output.height, mbfmt->width, mbfmt->height);
 
 	/* Sanity checks */
-	if ((unsigned)mbfmt.width > VOU_MAX_IMAGE_WIDTH ||
-	    (unsigned)mbfmt.height > img_height_max ||
-	    mbfmt.code != MEDIA_BUS_FMT_YUYV8_2X8)
+	if ((unsigned)mbfmt->width > VOU_MAX_IMAGE_WIDTH ||
+	    (unsigned)mbfmt->height > img_height_max ||
+	    mbfmt->code != MEDIA_BUS_FMT_YUYV8_2X8)
 		return -EIO;
 
-	if (mbfmt.width != geo.output.width ||
-	    mbfmt.height != geo.output.height) {
-		geo.output.width = mbfmt.width;
-		geo.output.height = mbfmt.height;
+	if (mbfmt->width != geo.output.width ||
+	    mbfmt->height != geo.output.height) {
+		geo.output.width = mbfmt->width;
+		geo.output.height = mbfmt->height;
 
 		vou_adjust_input(&geo, vou_dev->std);
 	}
@@ -942,11 +944,12 @@ static int sh_vou_s_crop(struct file *file, void *fh, const struct v4l2_crop *a)
 	struct v4l2_crop sd_crop = {.type = V4L2_BUF_TYPE_VIDEO_OUTPUT};
 	struct v4l2_pix_format *pix = &vou_dev->pix;
 	struct sh_vou_geometry geo;
-	struct v4l2_mbus_framefmt mbfmt = {
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 		/* Revisit: is this the correct code? */
-		.code = MEDIA_BUS_FMT_YUYV8_2X8,
-		.field = V4L2_FIELD_INTERLACED,
-		.colorspace = V4L2_COLORSPACE_SMPTE170M,
+		.format.code = MEDIA_BUS_FMT_YUYV8_2X8,
+		.format.field = V4L2_FIELD_INTERLACED,
+		.format.colorspace = V4L2_COLORSPACE_SMPTE170M,
 	};
 	unsigned int img_height_max;
 	int ret;
@@ -984,22 +987,22 @@ static int sh_vou_s_crop(struct file *file, void *fh, const struct v4l2_crop *a)
 	 */
 	v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, video,
 				   s_crop, &sd_crop);
-	mbfmt.width = geo.output.width;
-	mbfmt.height = geo.output.height;
-	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, video,
-					 s_mbus_fmt, &mbfmt);
+	format.format.width = geo.output.width;
+	format.format.height = geo.output.height;
+	ret = v4l2_device_call_until_err(&vou_dev->v4l2_dev, 0, pad,
+					 set_fmt, NULL, &format);
 	/* Must be implemented, so, don't check for -ENOIOCTLCMD */
 	if (ret < 0)
 		return ret;
 
 	/* Sanity checks */
-	if ((unsigned)mbfmt.width > VOU_MAX_IMAGE_WIDTH ||
-	    (unsigned)mbfmt.height > img_height_max ||
-	    mbfmt.code != MEDIA_BUS_FMT_YUYV8_2X8)
+	if ((unsigned)format.format.width > VOU_MAX_IMAGE_WIDTH ||
+	    (unsigned)format.format.height > img_height_max ||
+	    format.format.code != MEDIA_BUS_FMT_YUYV8_2X8)
 		return -EIO;
 
-	geo.output.width = mbfmt.width;
-	geo.output.height = mbfmt.height;
+	geo.output.width = format.format.width;
+	geo.output.height = format.format.height;
 
 	/*
 	 * No down-scaling. According to the API, current call has precedence:
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index c835beb2a1a8..287902681164 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -487,7 +487,10 @@ static int isi_camera_set_fmt(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	int ret;
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
@@ -500,27 +503,27 @@ static int isi_camera_set_fmt(struct soc_camera_device *icd,
 	dev_dbg(icd->parent, "Plan to set format %dx%d\n",
 			pix->width, pix->height);
 
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
 	if (ret < 0)
 		return ret;
 
-	if (mf.code != xlate->code)
+	if (mf->code != xlate->code)
 		return -EINVAL;
 
 	ret = configure_geometry(isi, pix->width, pix->height, xlate->code);
 	if (ret < 0)
 		return ret;
 
-	pix->width		= mf.width;
-	pix->height		= mf.height;
-	pix->field		= mf.field;
-	pix->colorspace		= mf.colorspace;
+	pix->width		= mf->width;
+	pix->height		= mf->height;
+	pix->field		= mf->field;
+	pix->colorspace		= mf->colorspace;
 	icd->current_fmt	= xlate;
 
 	dev_dbg(icd->parent, "Finally set format %dx%d\n",
@@ -535,7 +538,11 @@ static int isi_camera_try_fmt(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	u32 pixfmt = pix->pixelformat;
 	int ret;
 
@@ -552,21 +559,21 @@ static int isi_camera_try_fmt(struct soc_camera_device *icd,
 		pix->width = MAX_SUPPORT_WIDTH;
 
 	/* limit to sensor capabilities */
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
-	pix->width	= mf.width;
-	pix->height	= mf.height;
-	pix->colorspace	= mf.colorspace;
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->colorspace	= mf->colorspace;
 
-	switch (mf.field) {
+	switch (mf->field) {
 	case V4L2_FIELD_ANY:
 		pix->field = V4L2_FIELD_NONE;
 		break;
@@ -574,7 +581,7 @@ static int isi_camera_try_fmt(struct soc_camera_device *icd,
 		break;
 	default:
 		dev_err(icd->parent, "Field type %d unsupported.\n",
-			mf.field);
+			mf->field);
 		ret = -EINVAL;
 	}
 
@@ -648,19 +655,22 @@ static int isi_camera_get_formats(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	int formats = 0, ret;
 	/* sensor format */
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
 	/* soc camera host format */
 	const struct soc_mbus_pixelfmt *fmt;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		/* No more formats */
 		return 0;
 
-	fmt = soc_mbus_get_fmtdesc(code);
+	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
 		dev_err(icd->parent,
-			"Invalid format code #%u: %d\n", idx, code);
+			"Invalid format code #%u: %d\n", idx, code.code);
 		return 0;
 	}
 
@@ -672,7 +682,7 @@ static int isi_camera_get_formats(struct soc_camera_device *icd,
 		return 0;
 	}
 
-	switch (code) {
+	switch (code.code) {
 	case MEDIA_BUS_FMT_UYVY8_2X8:
 	case MEDIA_BUS_FMT_VYUY8_2X8:
 	case MEDIA_BUS_FMT_YUYV8_2X8:
@@ -680,10 +690,10 @@ static int isi_camera_get_formats(struct soc_camera_device *icd,
 		formats++;
 		if (xlate) {
 			xlate->host_fmt	= &isi_camera_formats[0];
-			xlate->code	= code;
+			xlate->code	= code.code;
 			xlate++;
 			dev_dbg(icd->parent, "Providing format %s using code %d\n",
-				isi_camera_formats[0].name, code);
+				isi_camera_formats[0].name, code.code);
 		}
 		break;
 	default:
@@ -699,7 +709,7 @@ static int isi_camera_get_formats(struct soc_camera_device *icd,
 	formats++;
 	if (xlate) {
 		xlate->host_fmt	= fmt;
-		xlate->code	= code;
+		xlate->code	= code.code;
 		xlate++;
 	}
 
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index 192377f55840..ea4c423f0cf8 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -912,7 +912,10 @@ static int mx2_camera_set_crop(struct soc_camera_device *icd,
 	struct v4l2_crop a_writable = *a;
 	struct v4l2_rect *rect = &a_writable.c;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	int ret;
 
 	soc_camera_limit_side(&rect->left, &rect->width, 0, 2, 4096);
@@ -923,15 +926,15 @@ static int mx2_camera_set_crop(struct soc_camera_device *icd,
 		return ret;
 
 	/* The capture device might have changed its output  */
-	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 	if (ret < 0)
 		return ret;
 
 	dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
-		mf.width, mf.height);
+		mf->width, mf->height);
 
-	icd->user_width		= mf.width;
-	icd->user_height	= mf.height;
+	icd->user_width		= mf->width;
+	icd->user_height	= mf->height;
 
 	return ret;
 }
@@ -943,22 +946,25 @@ static int mx2_camera_get_formats(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_mbus_pixelfmt *fmt;
 	struct device *dev = icd->parent;
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
 	int ret, formats = 0;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		/* no more formats */
 		return 0;
 
-	fmt = soc_mbus_get_fmtdesc(code);
+	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
-		dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
+		dev_err(dev, "Invalid format code #%u: %d\n", idx, code.code);
 		return 0;
 	}
 
-	if (code == MEDIA_BUS_FMT_YUYV8_2X8 ||
-	    code == MEDIA_BUS_FMT_UYVY8_2X8) {
+	if (code.code == MEDIA_BUS_FMT_YUYV8_2X8 ||
+	    code.code == MEDIA_BUS_FMT_UYVY8_2X8) {
 		formats++;
 		if (xlate) {
 			/*
@@ -967,21 +973,21 @@ static int mx2_camera_get_formats(struct soc_camera_device *icd,
 			 */
 			xlate->host_fmt =
 				soc_mbus_get_fmtdesc(MEDIA_BUS_FMT_YUYV8_1_5X8);
-			xlate->code	= code;
+			xlate->code	= code.code;
 			dev_dbg(dev, "Providing host format %s for sensor code %d\n",
-			       xlate->host_fmt->name, code);
+			       xlate->host_fmt->name, code.code);
 			xlate++;
 		}
 	}
 
-	if (code == MEDIA_BUS_FMT_UYVY8_2X8) {
+	if (code.code == MEDIA_BUS_FMT_UYVY8_2X8) {
 		formats++;
 		if (xlate) {
 			xlate->host_fmt =
 				soc_mbus_get_fmtdesc(MEDIA_BUS_FMT_YUYV8_2X8);
-			xlate->code	= code;
+			xlate->code	= code.code;
 			dev_dbg(dev, "Providing host format %s for sensor code %d\n",
-				xlate->host_fmt->name, code);
+				xlate->host_fmt->name, code.code);
 			xlate++;
 		}
 	}
@@ -990,7 +996,7 @@ static int mx2_camera_get_formats(struct soc_camera_device *icd,
 	formats++;
 	if (xlate) {
 		xlate->host_fmt = fmt;
-		xlate->code	= code;
+		xlate->code	= code.code;
 		xlate++;
 	}
 	return formats;
@@ -1121,7 +1127,10 @@ static int mx2_camera_set_fmt(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	int ret;
 
 	dev_dbg(icd->parent, "%s: requested params: width = %d, height = %d\n",
@@ -1134,19 +1143,19 @@ static int mx2_camera_set_fmt(struct soc_camera_device *icd,
 		return -EINVAL;
 	}
 
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
 	if (ret < 0 && ret != -ENOIOCTLCMD)
 		return ret;
 
 	/* Store width and height returned by the sensor for resizing */
-	pcdev->s_width = mf.width;
-	pcdev->s_height = mf.height;
+	pcdev->s_width = mf->width;
+	pcdev->s_height = mf->height;
 	dev_dbg(icd->parent, "%s: sensor params: width = %d, height = %d\n",
 		__func__, pcdev->s_width, pcdev->s_height);
 
@@ -1154,19 +1163,19 @@ static int mx2_camera_set_fmt(struct soc_camera_device *icd,
 						   xlate->host_fmt->fourcc);
 
 	memset(pcdev->resizing, 0, sizeof(pcdev->resizing));
-	if ((mf.width != pix->width || mf.height != pix->height) &&
+	if ((mf->width != pix->width || mf->height != pix->height) &&
 		pcdev->emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
-		if (mx2_emmaprp_resize(pcdev, &mf, pix, true) < 0)
+		if (mx2_emmaprp_resize(pcdev, mf, pix, true) < 0)
 			dev_dbg(icd->parent, "%s: can't resize\n", __func__);
 	}
 
-	if (mf.code != xlate->code)
+	if (mf->code != xlate->code)
 		return -EINVAL;
 
-	pix->width		= mf.width;
-	pix->height		= mf.height;
-	pix->field		= mf.field;
-	pix->colorspace		= mf.colorspace;
+	pix->width		= mf->width;
+	pix->height		= mf->height;
+	pix->field		= mf->field;
+	pix->colorspace		= mf->colorspace;
 	icd->current_fmt	= xlate;
 
 	dev_dbg(icd->parent, "%s: returned params: width = %d, height = %d\n",
@@ -1181,7 +1190,11 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	__u32 pixfmt = pix->pixelformat;
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx2_camera_dev *pcdev = ici->priv;
@@ -1204,13 +1217,13 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 	pix->width &= ~0x7;
 
 	/* limit to sensor capabilities */
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
@@ -1221,29 +1234,29 @@ static int mx2_camera_try_fmt(struct soc_camera_device *icd,
 	emma_prp = mx27_emma_prp_get_format(xlate->code,
 					    xlate->host_fmt->fourcc);
 
-	if ((mf.width != pix->width || mf.height != pix->height) &&
+	if ((mf->width != pix->width || mf->height != pix->height) &&
 		emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) {
-		if (mx2_emmaprp_resize(pcdev, &mf, pix, false) < 0)
+		if (mx2_emmaprp_resize(pcdev, mf, pix, false) < 0)
 			dev_dbg(icd->parent, "%s: can't resize\n", __func__);
 	}
 
-	if (mf.field == V4L2_FIELD_ANY)
-		mf.field = V4L2_FIELD_NONE;
+	if (mf->field == V4L2_FIELD_ANY)
+		mf->field = V4L2_FIELD_NONE;
 	/*
 	 * Driver supports interlaced images provided they have
 	 * both fields so that they can be processed as if they
 	 * were progressive.
 	 */
-	if (mf.field != V4L2_FIELD_NONE && !V4L2_FIELD_HAS_BOTH(mf.field)) {
+	if (mf->field != V4L2_FIELD_NONE && !V4L2_FIELD_HAS_BOTH(mf->field)) {
 		dev_err(icd->parent, "Field type %d unsupported.\n",
-				mf.field);
+				mf->field);
 		return -EINVAL;
 	}
 
-	pix->width	= mf.width;
-	pix->height	= mf.height;
-	pix->field	= mf.field;
-	pix->colorspace	= mf.colorspace;
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->field	= mf->field;
+	pix->colorspace	= mf->colorspace;
 
 	dev_dbg(icd->parent, "%s: returned params: width = %d, height = %d\n",
 		__func__, pix->width, pix->height);
diff --git a/drivers/media/platform/soc_camera/mx3_camera.c b/drivers/media/platform/soc_camera/mx3_camera.c
index 3435fd2ca8ec..ace41f53caca 100644
--- a/drivers/media/platform/soc_camera/mx3_camera.c
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@ -659,18 +659,21 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	struct device *dev = icd->parent;
 	int formats = 0, ret;
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
 	const struct soc_mbus_pixelfmt *fmt;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		/* No more formats */
 		return 0;
 
-	fmt = soc_mbus_get_fmtdesc(code);
+	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
 		dev_warn(icd->parent,
-			 "Unsupported format code #%u: 0x%x\n", idx, code);
+			 "Unsupported format code #%u: 0x%x\n", idx, code.code);
 		return 0;
 	}
 
@@ -679,25 +682,25 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	if (ret < 0)
 		return 0;
 
-	switch (code) {
+	switch (code.code) {
 	case MEDIA_BUS_FMT_SBGGR10_1X10:
 		formats++;
 		if (xlate) {
 			xlate->host_fmt	= &mx3_camera_formats[0];
-			xlate->code	= code;
+			xlate->code	= code.code;
 			xlate++;
 			dev_dbg(dev, "Providing format %s using code 0x%x\n",
-				mx3_camera_formats[0].name, code);
+				mx3_camera_formats[0].name, code.code);
 		}
 		break;
 	case MEDIA_BUS_FMT_Y10_1X10:
 		formats++;
 		if (xlate) {
 			xlate->host_fmt	= &mx3_camera_formats[1];
-			xlate->code	= code;
+			xlate->code	= code.code;
 			xlate++;
 			dev_dbg(dev, "Providing format %s using code 0x%x\n",
-				mx3_camera_formats[1].name, code);
+				mx3_camera_formats[1].name, code.code);
 		}
 		break;
 	default:
@@ -709,7 +712,7 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	formats++;
 	if (xlate) {
 		xlate->host_fmt	= fmt;
-		xlate->code	= code;
+		xlate->code	= code.code;
 		dev_dbg(dev, "Providing format %c%c%c%c in pass-through mode\n",
 			(fmt->fourcc >> (0*8)) & 0xFF,
 			(fmt->fourcc >> (1*8)) & 0xFF,
@@ -801,7 +804,10 @@ static int mx3_camera_set_crop(struct soc_camera_device *icd,
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx3_camera_dev *mx3_cam = ici->priv;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	int ret;
 
 	soc_camera_limit_side(&rect->left, &rect->width, 0, 2, 4096);
@@ -812,30 +818,30 @@ static int mx3_camera_set_crop(struct soc_camera_device *icd,
 		return ret;
 
 	/* The capture device might have changed its output sizes */
-	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 	if (ret < 0)
 		return ret;
 
-	if (mf.code != icd->current_fmt->code)
+	if (mf->code != icd->current_fmt->code)
 		return -EINVAL;
 
-	if (mf.width & 7) {
+	if (mf->width & 7) {
 		/* Ouch! We can only handle 8-byte aligned width... */
-		stride_align(&mf.width);
-		ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+		stride_align(&mf->width);
+		ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
 		if (ret < 0)
 			return ret;
 	}
 
-	if (mf.width != icd->user_width || mf.height != icd->user_height)
-		configure_geometry(mx3_cam, mf.width, mf.height,
+	if (mf->width != icd->user_width || mf->height != icd->user_height)
+		configure_geometry(mx3_cam, mf->width, mf->height,
 				   icd->current_fmt->host_fmt);
 
 	dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
-		mf.width, mf.height);
+		mf->width, mf->height);
 
-	icd->user_width		= mf.width;
-	icd->user_height	= mf.height;
+	icd->user_width		= mf->width;
+	icd->user_height	= mf->height;
 
 	return ret;
 }
@@ -848,7 +854,10 @@ static int mx3_camera_set_fmt(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	int ret;
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
@@ -869,17 +878,17 @@ static int mx3_camera_set_fmt(struct soc_camera_device *icd,
 
 	configure_geometry(mx3_cam, pix->width, pix->height, xlate->host_fmt);
 
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
 	if (ret < 0)
 		return ret;
 
-	if (mf.code != xlate->code)
+	if (mf->code != xlate->code)
 		return -EINVAL;
 
 	if (!mx3_cam->idmac_channel[0]) {
@@ -888,11 +897,11 @@ static int mx3_camera_set_fmt(struct soc_camera_device *icd,
 			return ret;
 	}
 
-	pix->width		= mf.width;
-	pix->height		= mf.height;
-	pix->field		= mf.field;
-	mx3_cam->field		= mf.field;
-	pix->colorspace		= mf.colorspace;
+	pix->width		= mf->width;
+	pix->height		= mf->height;
+	pix->field		= mf->field;
+	mx3_cam->field		= mf->field;
+	pix->colorspace		= mf->colorspace;
 	icd->current_fmt	= xlate;
 
 	dev_dbg(icd->parent, "Sensor set %dx%d\n", pix->width, pix->height);
@@ -906,7 +915,11 @@ static int mx3_camera_try_fmt(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	__u32 pixfmt = pix->pixelformat;
 	int ret;
 
@@ -923,21 +936,21 @@ static int mx3_camera_try_fmt(struct soc_camera_device *icd,
 		pix->width = 4096;
 
 	/* limit to sensor capabilities */
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
-	pix->width	= mf.width;
-	pix->height	= mf.height;
-	pix->colorspace	= mf.colorspace;
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->colorspace	= mf->colorspace;
 
-	switch (mf.field) {
+	switch (mf->field) {
 	case V4L2_FIELD_ANY:
 		pix->field = V4L2_FIELD_NONE;
 		break;
@@ -945,7 +958,7 @@ static int mx3_camera_try_fmt(struct soc_camera_device *icd,
 		break;
 	default:
 		dev_err(icd->parent, "Field type %d unsupported.\n",
-			mf.field);
+			mf->field);
 		ret = -EINVAL;
 	}
 
diff --git a/drivers/media/platform/soc_camera/omap1_camera.c b/drivers/media/platform/soc_camera/omap1_camera.c
index 16f65ecb70a3..ba8dcd11ae0e 100644
--- a/drivers/media/platform/soc_camera/omap1_camera.c
+++ b/drivers/media/platform/soc_camera/omap1_camera.c
@@ -1068,18 +1068,21 @@ static int omap1_cam_get_formats(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	struct device *dev = icd->parent;
 	int formats = 0, ret;
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
 	const struct soc_mbus_pixelfmt *fmt;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		/* No more formats */
 		return 0;
 
-	fmt = soc_mbus_get_fmtdesc(code);
+	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
 		dev_warn(dev, "%s: unsupported format code #%d: %d\n", __func__,
-				idx, code);
+				idx, code.code);
 		return 0;
 	}
 
@@ -1087,7 +1090,7 @@ static int omap1_cam_get_formats(struct soc_camera_device *icd,
 	if (fmt->bits_per_sample != 8)
 		return 0;
 
-	switch (code) {
+	switch (code.code) {
 	case MEDIA_BUS_FMT_YUYV8_2X8:
 	case MEDIA_BUS_FMT_YVYU8_2X8:
 	case MEDIA_BUS_FMT_UYVY8_2X8:
@@ -1098,14 +1101,14 @@ static int omap1_cam_get_formats(struct soc_camera_device *icd,
 	case MEDIA_BUS_FMT_RGB565_2X8_LE:
 		formats++;
 		if (xlate) {
-			xlate->host_fmt	= soc_mbus_find_fmtdesc(code,
+			xlate->host_fmt	= soc_mbus_find_fmtdesc(code.code,
 						omap1_cam_formats,
 						ARRAY_SIZE(omap1_cam_formats));
-			xlate->code	= code;
+			xlate->code	= code.code;
 			xlate++;
 			dev_dbg(dev,
 				"%s: providing format %s as byte swapped code #%d\n",
-				__func__, xlate->host_fmt->name, code);
+				__func__, xlate->host_fmt->name, code.code);
 		}
 	default:
 		if (xlate)
@@ -1116,7 +1119,7 @@ static int omap1_cam_get_formats(struct soc_camera_device *icd,
 	formats++;
 	if (xlate) {
 		xlate->host_fmt	= fmt;
-		xlate->code	= code;
+		xlate->code	= code.code;
 		xlate++;
 	}
 
@@ -1154,7 +1157,7 @@ static int dma_align(int *width, int *height,
 	return 1;
 }
 
-#define subdev_call_with_sense(pcdev, dev, icd, sd, function, args...)		     \
+#define subdev_call_with_sense(pcdev, dev, icd, sd, op, function, args...)		     \
 ({										     \
 	struct soc_camera_sense sense = {					     \
 		.master_clock		= pcdev->camexclk,			     \
@@ -1165,7 +1168,7 @@ static int dma_align(int *width, int *height,
 	if (pcdev->pdata)							     \
 		sense.pixel_clock_max = pcdev->pdata->lclk_khz_max * 1000;	     \
 	icd->sense = &sense;							     \
-	__ret = v4l2_subdev_call(sd, video, function, ##args);			     \
+	__ret = v4l2_subdev_call(sd, op, function, ##args);			     \
 	icd->sense = NULL;							     \
 										     \
 	if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {				     \
@@ -1179,16 +1182,17 @@ static int dma_align(int *width, int *height,
 	__ret;									     \
 })
 
-static int set_mbus_format(struct omap1_cam_dev *pcdev, struct device *dev,
+static int set_format(struct omap1_cam_dev *pcdev, struct device *dev,
 		struct soc_camera_device *icd, struct v4l2_subdev *sd,
-		struct v4l2_mbus_framefmt *mf,
+		struct v4l2_subdev_format *format,
 		const struct soc_camera_format_xlate *xlate)
 {
 	s32 bytes_per_line;
-	int ret = subdev_call_with_sense(pcdev, dev, icd, sd, s_mbus_fmt, mf);
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	int ret = subdev_call_with_sense(pcdev, dev, icd, sd, pad, set_fmt, NULL, format);
 
 	if (ret < 0) {
-		dev_err(dev, "%s: s_mbus_fmt failed\n", __func__);
+		dev_err(dev, "%s: set_fmt failed\n", __func__);
 		return ret;
 	}
 
@@ -1221,42 +1225,45 @@ static int omap1_cam_set_crop(struct soc_camera_device *icd,
 	struct device *dev = icd->parent;
 	struct soc_camera_host *ici = to_soc_camera_host(dev);
 	struct omap1_cam_dev *pcdev = ici->priv;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	int ret;
 
-	ret = subdev_call_with_sense(pcdev, dev, icd, sd, s_crop, crop);
+	ret = subdev_call_with_sense(pcdev, dev, icd, sd, video, s_crop, crop);
 	if (ret < 0) {
 		dev_warn(dev, "%s: failed to crop to %ux%u@%u:%u\n", __func__,
 			 rect->width, rect->height, rect->left, rect->top);
 		return ret;
 	}
 
-	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 	if (ret < 0) {
 		dev_warn(dev, "%s: failed to fetch current format\n", __func__);
 		return ret;
 	}
 
-	ret = dma_align(&mf.width, &mf.height, xlate->host_fmt, pcdev->vb_mode,
+	ret = dma_align(&mf->width, &mf->height, xlate->host_fmt, pcdev->vb_mode,
 			false);
 	if (ret < 0) {
 		dev_err(dev, "%s: failed to align %ux%u %s with DMA\n",
-				__func__, mf.width, mf.height,
+				__func__, mf->width, mf->height,
 				xlate->host_fmt->name);
 		return ret;
 	}
 
 	if (!ret) {
 		/* sensor returned geometry not DMA aligned, trying to fix */
-		ret = set_mbus_format(pcdev, dev, icd, sd, &mf, xlate);
+		ret = set_format(pcdev, dev, icd, sd, &fmt, xlate);
 		if (ret < 0) {
 			dev_err(dev, "%s: failed to set format\n", __func__);
 			return ret;
 		}
 	}
 
-	icd->user_width	 = mf.width;
-	icd->user_height = mf.height;
+	icd->user_width	 = mf->width;
+	icd->user_height = mf->height;
 
 	return 0;
 }
@@ -1270,7 +1277,10 @@ static int omap1_cam_set_fmt(struct soc_camera_device *icd,
 	struct soc_camera_host *ici = to_soc_camera_host(dev);
 	struct omap1_cam_dev *pcdev = ici->priv;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	int ret;
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
@@ -1280,13 +1290,13 @@ static int omap1_cam_set_fmt(struct soc_camera_device *icd,
 		return -EINVAL;
 	}
 
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = dma_align(&mf.width, &mf.height, xlate->host_fmt, pcdev->vb_mode,
+	ret = dma_align(&mf->width, &mf->height, xlate->host_fmt, pcdev->vb_mode,
 			true);
 	if (ret < 0) {
 		dev_err(dev, "%s: failed to align %ux%u %s with DMA\n",
@@ -1295,16 +1305,16 @@ static int omap1_cam_set_fmt(struct soc_camera_device *icd,
 		return ret;
 	}
 
-	ret = set_mbus_format(pcdev, dev, icd, sd, &mf, xlate);
+	ret = set_format(pcdev, dev, icd, sd, &format, xlate);
 	if (ret < 0) {
 		dev_err(dev, "%s: failed to set format\n", __func__);
 		return ret;
 	}
 
-	pix->width	 = mf.width;
-	pix->height	 = mf.height;
-	pix->field	 = mf.field;
-	pix->colorspace  = mf.colorspace;
+	pix->width	 = mf->width;
+	pix->height	 = mf->height;
+	pix->field	 = mf->field;
+	pix->colorspace  = mf->colorspace;
 	icd->current_fmt = xlate;
 
 	return 0;
@@ -1316,7 +1326,11 @@ static int omap1_cam_try_fmt(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	int ret;
 	/* TODO: limit to mx1 hardware capabilities */
 
@@ -1327,21 +1341,21 @@ static int omap1_cam_try_fmt(struct soc_camera_device *icd,
 		return -EINVAL;
 	}
 
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
 	/* limit to sensor capabilities */
-	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
-	pix->width	= mf.width;
-	pix->height	= mf.height;
-	pix->field	= mf.field;
-	pix->colorspace	= mf.colorspace;
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->field	= mf->field;
+	pix->colorspace	= mf->colorspace;
 
 	return 0;
 }
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 8d6e343fec0f..fcb942de0c7f 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -1253,17 +1253,20 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	struct device *dev = icd->parent;
 	int formats = 0, ret;
 	struct pxa_cam *cam;
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
 	const struct soc_mbus_pixelfmt *fmt;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		/* No more formats */
 		return 0;
 
-	fmt = soc_mbus_get_fmtdesc(code);
+	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
-		dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
+		dev_err(dev, "Invalid format code #%u: %d\n", idx, code.code);
 		return 0;
 	}
 
@@ -1282,15 +1285,15 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 		cam = icd->host_priv;
 	}
 
-	switch (code) {
+	switch (code.code) {
 	case MEDIA_BUS_FMT_UYVY8_2X8:
 		formats++;
 		if (xlate) {
 			xlate->host_fmt	= &pxa_camera_formats[0];
-			xlate->code	= code;
+			xlate->code	= code.code;
 			xlate++;
 			dev_dbg(dev, "Providing format %s using code %d\n",
-				pxa_camera_formats[0].name, code);
+				pxa_camera_formats[0].name, code.code);
 		}
 	case MEDIA_BUS_FMT_VYUY8_2X8:
 	case MEDIA_BUS_FMT_YUYV8_2X8:
@@ -1314,7 +1317,7 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int id
 	formats++;
 	if (xlate) {
 		xlate->host_fmt	= fmt;
-		xlate->code	= code;
+		xlate->code	= code.code;
 		xlate++;
 	}
 
@@ -1346,7 +1349,10 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 		.master_clock = pcdev->mclk,
 		.pixel_clock_max = pcdev->ciclk / 4,
 	};
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	struct pxa_cam *cam = icd->host_priv;
 	u32 fourcc = icd->current_fmt->host_fmt->fourcc;
 	int ret;
@@ -1365,23 +1371,23 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 		return ret;
 	}
 
-	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 	if (ret < 0)
 		return ret;
 
-	if (pxa_camera_check_frame(mf.width, mf.height)) {
+	if (pxa_camera_check_frame(mf->width, mf->height)) {
 		/*
 		 * Camera cropping produced a frame beyond our capabilities.
 		 * FIXME: just extract a subframe, that we can process.
 		 */
-		v4l_bound_align_image(&mf.width, 48, 2048, 1,
-			&mf.height, 32, 2048, 0,
+		v4l_bound_align_image(&mf->width, 48, 2048, 1,
+			&mf->height, 32, 2048, 0,
 			fourcc == V4L2_PIX_FMT_YUV422P ? 4 : 0);
-		ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+		ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &fmt);
 		if (ret < 0)
 			return ret;
 
-		if (pxa_camera_check_frame(mf.width, mf.height)) {
+		if (pxa_camera_check_frame(mf->width, mf->height)) {
 			dev_warn(icd->parent,
 				 "Inconsistent state. Use S_FMT to repair\n");
 			return -EINVAL;
@@ -1398,8 +1404,8 @@ static int pxa_camera_set_crop(struct soc_camera_device *icd,
 		recalculate_fifo_timeout(pcdev, sense.pixel_clock);
 	}
 
-	icd->user_width		= mf.width;
-	icd->user_height	= mf.height;
+	icd->user_width		= mf->width;
+	icd->user_height	= mf->height;
 
 	pxa_camera_setup_cicr(icd, cam->flags, fourcc);
 
@@ -1419,7 +1425,10 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 		.pixel_clock_max = pcdev->ciclk / 4,
 	};
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	int ret;
 
 	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
@@ -1433,15 +1442,15 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 		/* The caller holds a mutex. */
 		icd->sense = &sense;
 
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format);
 
-	if (mf.code != xlate->code)
+	if (mf->code != xlate->code)
 		return -EINVAL;
 
 	icd->sense = NULL;
@@ -1449,10 +1458,10 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 	if (ret < 0) {
 		dev_warn(dev, "Failed to configure for format %x\n",
 			 pix->pixelformat);
-	} else if (pxa_camera_check_frame(mf.width, mf.height)) {
+	} else if (pxa_camera_check_frame(mf->width, mf->height)) {
 		dev_warn(dev,
 			 "Camera driver produced an unsupported frame %dx%d\n",
-			 mf.width, mf.height);
+			 mf->width, mf->height);
 		ret = -EINVAL;
 	} else if (sense.flags & SOCAM_SENSE_PCLK_CHANGED) {
 		if (sense.pixel_clock > sense.pixel_clock_max) {
@@ -1467,10 +1476,10 @@ static int pxa_camera_set_fmt(struct soc_camera_device *icd,
 	if (ret < 0)
 		return ret;
 
-	pix->width		= mf.width;
-	pix->height		= mf.height;
-	pix->field		= mf.field;
-	pix->colorspace		= mf.colorspace;
+	pix->width		= mf->width;
+	pix->height		= mf->height;
+	pix->field		= mf->field;
+	pix->colorspace		= mf->colorspace;
 	icd->current_fmt	= xlate;
 
 	return ret;
@@ -1482,7 +1491,11 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	__u32 pixfmt = pix->pixelformat;
 	int ret;
 
@@ -1503,22 +1516,22 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 			      pixfmt == V4L2_PIX_FMT_YUV422P ? 4 : 0);
 
 	/* limit to sensor capabilities */
-	mf.width	= pix->width;
-	mf.height	= pix->height;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
 	/* Only progressive video supported so far */
-	mf.field	= V4L2_FIELD_NONE;
-	mf.colorspace	= pix->colorspace;
-	mf.code		= xlate->code;
+	mf->field	= V4L2_FIELD_NONE;
+	mf->colorspace	= pix->colorspace;
+	mf->code	= xlate->code;
 
-	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
-	pix->width	= mf.width;
-	pix->height	= mf.height;
-	pix->colorspace	= mf.colorspace;
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->colorspace	= mf->colorspace;
 
-	switch (mf.field) {
+	switch (mf->field) {
 	case V4L2_FIELD_ANY:
 	case V4L2_FIELD_NONE:
 		pix->field	= V4L2_FIELD_NONE;
@@ -1526,7 +1539,7 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
 	default:
 		/* TODO: support interlaced at least in pass-through mode */
 		dev_err(icd->parent, "Field type %d unsupported.\n",
-			mf.field);
+			mf->field);
 		return -EINVAL;
 	}
 
diff --git a/drivers/media/platform/soc_camera/rcar_vin.c b/drivers/media/platform/soc_camera/rcar_vin.c
index 6460f8e1b07f..db7700b0af7c 100644
--- a/drivers/media/platform/soc_camera/rcar_vin.c
+++ b/drivers/media/platform/soc_camera/rcar_vin.c
@@ -899,7 +899,7 @@ static irqreturn_t rcar_vin_irq(int irq, void *data)
 
 		priv->queue_buf[slot]->v4l2_buf.field = priv->field;
 		priv->queue_buf[slot]->v4l2_buf.sequence = priv->sequence++;
-		do_gettimeofday(&priv->queue_buf[slot]->v4l2_buf.timestamp);
+		v4l2_get_timestamp(&priv->queue_buf[slot]->v4l2_buf.timestamp);
 		vb2_buffer_done(priv->queue_buf[slot], VB2_BUF_STATE_DONE);
 		priv->queue_buf[slot] = NULL;
 
@@ -1323,16 +1323,19 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 	int ret, k, n;
 	int formats = 0;
 	struct rcar_vin_cam *cam;
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
 	const struct soc_mbus_pixelfmt *fmt;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		return 0;
 
-	fmt = soc_mbus_get_fmtdesc(code);
+	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
-		dev_warn(dev, "unsupported format code #%u: %d\n", idx, code);
+		dev_warn(dev, "unsupported format code #%u: %d\n", idx, code.code);
 		return 0;
 	}
 
@@ -1341,12 +1344,15 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 		return 0;
 
 	if (!icd->host_priv) {
-		struct v4l2_mbus_framefmt mf;
+		struct v4l2_subdev_format fmt = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
+		struct v4l2_mbus_framefmt *mf = &fmt.format;
 		struct v4l2_rect rect;
 		struct device *dev = icd->parent;
 		int shift;
 
-		ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 		if (ret < 0)
 			return ret;
 
@@ -1356,8 +1362,8 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 			/* Sensor driver doesn't support cropping */
 			rect.left = 0;
 			rect.top = 0;
-			rect.width = mf.width;
-			rect.height = mf.height;
+			rect.width = mf->width;
+			rect.height = mf->height;
 		} else if (ret < 0) {
 			return ret;
 		}
@@ -1367,16 +1373,16 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 		 * 1280x960, 640x480, 320x240
 		 */
 		for (shift = 0; shift < 3; shift++) {
-			if (mf.width <= VIN_MAX_WIDTH &&
-			    mf.height <= VIN_MAX_HEIGHT)
+			if (mf->width <= VIN_MAX_WIDTH &&
+			    mf->height <= VIN_MAX_HEIGHT)
 				break;
 
-			mf.width = 1280 >> shift;
-			mf.height = 960 >> shift;
+			mf->width = 1280 >> shift;
+			mf->height = 960 >> shift;
 			ret = v4l2_device_call_until_err(sd->v4l2_dev,
 							 soc_camera_grp_id(icd),
-							 video, s_mbus_fmt,
-							 &mf);
+							 pad, set_fmt, NULL,
+							 &fmt);
 			if (ret < 0)
 				return ret;
 		}
@@ -1384,11 +1390,11 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 		if (shift == 3) {
 			dev_err(dev,
 				"Failed to configure the client below %ux%u\n",
-				mf.width, mf.height);
+				mf->width, mf->height);
 			return -EIO;
 		}
 
-		dev_dbg(dev, "camera fmt %ux%u\n", mf.width, mf.height);
+		dev_dbg(dev, "camera fmt %ux%u\n", mf->width, mf->height);
 
 		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
 		if (!cam)
@@ -1399,10 +1405,10 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 		 */
 		cam->rect = rect;
 		cam->subrect = rect;
-		cam->width = mf.width;
-		cam->height = mf.height;
-		cam->out_width	= mf.width;
-		cam->out_height	= mf.height;
+		cam->width = mf->width;
+		cam->height = mf->height;
+		cam->out_width	= mf->width;
+		cam->out_height	= mf->height;
 
 		icd->host_priv = cam;
 	} else {
@@ -1413,7 +1419,7 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 	if (!idx)
 		cam->extra_fmt = NULL;
 
-	switch (code) {
+	switch (code.code) {
 	case MEDIA_BUS_FMT_YUYV8_1X16:
 	case MEDIA_BUS_FMT_YUYV8_2X8:
 	case MEDIA_BUS_FMT_YUYV10_2X10:
@@ -1427,9 +1433,9 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 		formats += n;
 		for (k = 0; xlate && k < n; k++, xlate++) {
 			xlate->host_fmt = &rcar_vin_formats[k];
-			xlate->code = code;
+			xlate->code = code.code;
 			dev_dbg(dev, "Providing format %s using code %d\n",
-				rcar_vin_formats[k].name, code);
+				rcar_vin_formats[k].name, code.code);
 		}
 		break;
 	default:
@@ -1445,7 +1451,7 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 	formats++;
 	if (xlate) {
 		xlate->host_fmt = fmt;
-		xlate->code = code;
+		xlate->code = code.code;
 		xlate++;
 	}
 
@@ -1470,7 +1476,10 @@ static int rcar_vin_set_crop(struct soc_camera_device *icd,
 	struct v4l2_rect *cam_rect = &cam_crop.c;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	struct device *dev = icd->parent;
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	u32 vnmc;
 	int ret, i;
 
@@ -1494,16 +1503,16 @@ static int rcar_vin_set_crop(struct soc_camera_device *icd,
 	/* On success cam_crop contains current camera crop */
 
 	/* Retrieve camera output window */
-	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 	if (ret < 0)
 		return ret;
 
-	if (mf.width > VIN_MAX_WIDTH || mf.height > VIN_MAX_HEIGHT)
+	if (mf->width > VIN_MAX_WIDTH || mf->height > VIN_MAX_HEIGHT)
 		return -EINVAL;
 
 	/* Cache camera output window */
-	cam->width = mf.width;
-	cam->height = mf.height;
+	cam->width = mf->width;
+	cam->height = mf->height;
 
 	icd->user_width  = cam->width;
 	icd->user_height = cam->height;
@@ -1679,7 +1688,11 @@ static int rcar_vin_try_fmt(struct soc_camera_device *icd,
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	__u32 pixfmt = pix->pixelformat;
 	int width, height;
 	int ret;
@@ -1706,25 +1719,25 @@ static int rcar_vin_try_fmt(struct soc_camera_device *icd,
 	pix->sizeimage = 0;
 
 	/* limit to sensor capabilities */
-	mf.width = pix->width;
-	mf.height = pix->height;
-	mf.field = pix->field;
-	mf.code = xlate->code;
-	mf.colorspace = pix->colorspace;
+	mf->width = pix->width;
+	mf->height = pix->height;
+	mf->field = pix->field;
+	mf->code = xlate->code;
+	mf->colorspace = pix->colorspace;
 
 	ret = v4l2_device_call_until_err(sd->v4l2_dev, soc_camera_grp_id(icd),
-					 video, try_mbus_fmt, &mf);
+					 pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
 	/* 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;
+	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;
+	pix->field = mf->field;
+	pix->colorspace = mf->colorspace;
 
 	if (pixfmt == V4L2_PIX_FMT_NV16) {
 		/* FIXME: check against rect_max after converting soc-camera */
@@ -1735,12 +1748,12 @@ static int rcar_vin_try_fmt(struct soc_camera_device *icd,
 			 * requested a bigger rectangle, it will not return a
 			 * smaller one.
 			 */
-			mf.width = VIN_MAX_WIDTH;
-			mf.height = VIN_MAX_HEIGHT;
+			mf->width = VIN_MAX_WIDTH;
+			mf->height = VIN_MAX_HEIGHT;
 			ret = v4l2_device_call_until_err(sd->v4l2_dev,
 							 soc_camera_grp_id(icd),
-							 video, try_mbus_fmt,
-							 &mf);
+							 pad, set_fmt, &pad_cfg,
+							 &format);
 			if (ret < 0) {
 				dev_err(icd->parent,
 					"client try_fmt() = %d\n", ret);
@@ -1748,9 +1761,9 @@ static int rcar_vin_try_fmt(struct soc_camera_device *icd,
 			}
 		}
 		/* We will scale exactly */
-		if (mf.width > width)
+		if (mf->width > width)
 			pix->width = width;
-		if (mf.height > height)
+		if (mf->height > height)
 			pix->height = height;
 	}
 
@@ -1808,7 +1821,7 @@ static struct soc_camera_host_ops rcar_vin_host_ops = {
 };
 
 #ifdef CONFIG_OF
-static struct of_device_id rcar_vin_of_table[] = {
+static const 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 },
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 9ce202f53934..c5c6c4e91f7b 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -1048,17 +1048,20 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 	int ret, k, n;
 	int formats = 0;
 	struct sh_mobile_ceu_cam *cam;
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.index = idx,
+	};
 	const struct soc_mbus_pixelfmt *fmt;
 
-	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
 	if (ret < 0)
 		/* No more formats */
 		return 0;
 
-	fmt = soc_mbus_get_fmtdesc(code);
+	fmt = soc_mbus_get_fmtdesc(code.code);
 	if (!fmt) {
-		dev_warn(dev, "unsupported format code #%u: %d\n", idx, code);
+		dev_warn(dev, "unsupported format code #%u: %d\n", idx, code.code);
 		return 0;
 	}
 
@@ -1070,7 +1073,10 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 	}
 
 	if (!icd->host_priv) {
-		struct v4l2_mbus_framefmt mf;
+		struct v4l2_subdev_format fmt = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
+		struct v4l2_mbus_framefmt *mf = &fmt.format;
 		struct v4l2_rect rect;
 		int shift = 0;
 
@@ -1088,7 +1094,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 			return ret;
 
 		/* First time */
-		ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
+		ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 		if (ret < 0)
 			return ret;
 
@@ -1099,14 +1105,14 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 		 * sizes, just try VGA multiples. If needed, this can be
 		 * adjusted in the future.
 		 */
-		while ((mf.width > pcdev->max_width ||
-			mf.height > pcdev->max_height) && shift < 4) {
+		while ((mf->width > pcdev->max_width ||
+			mf->height > pcdev->max_height) && shift < 4) {
 			/* Try 2560x1920, 1280x960, 640x480, 320x240 */
-			mf.width	= 2560 >> shift;
-			mf.height	= 1920 >> shift;
+			mf->width	= 2560 >> shift;
+			mf->height	= 1920 >> shift;
 			ret = v4l2_device_call_until_err(sd->v4l2_dev,
-					soc_camera_grp_id(icd), video,
-					s_mbus_fmt, &mf);
+					soc_camera_grp_id(icd), pad,
+					set_fmt, NULL, &fmt);
 			if (ret < 0)
 				return ret;
 			shift++;
@@ -1114,11 +1120,11 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 
 		if (shift == 4) {
 			dev_err(dev, "Failed to configure the client below %ux%x\n",
-				mf.width, mf.height);
+				mf->width, mf->height);
 			return -EIO;
 		}
 
-		dev_geo(dev, "camera fmt %ux%u\n", mf.width, mf.height);
+		dev_geo(dev, "camera fmt %ux%u\n", mf->width, mf->height);
 
 		cam = kzalloc(sizeof(*cam), GFP_KERNEL);
 		if (!cam)
@@ -1128,8 +1134,8 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 		cam->rect	= rect;
 		cam->subrect	= rect;
 
-		cam->width	= mf.width;
-		cam->height	= mf.height;
+		cam->width	= mf->width;
+		cam->height	= mf->height;
 
 		icd->host_priv = cam;
 	} else {
@@ -1140,7 +1146,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 	if (!idx)
 		cam->extra_fmt = NULL;
 
-	switch (code) {
+	switch (code.code) {
 	case MEDIA_BUS_FMT_UYVY8_2X8:
 	case MEDIA_BUS_FMT_VYUY8_2X8:
 	case MEDIA_BUS_FMT_YUYV8_2X8:
@@ -1163,10 +1169,10 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 		formats += n;
 		for (k = 0; xlate && k < n; k++) {
 			xlate->host_fmt	= &sh_mobile_ceu_formats[k];
-			xlate->code	= code;
+			xlate->code	= code.code;
 			xlate++;
 			dev_dbg(dev, "Providing format %s using code %d\n",
-				sh_mobile_ceu_formats[k].name, code);
+				sh_mobile_ceu_formats[k].name, code.code);
 		}
 		break;
 	default:
@@ -1178,7 +1184,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int
 	formats++;
 	if (xlate) {
 		xlate->host_fmt	= fmt;
-		xlate->code	= code;
+		xlate->code	= code.code;
 		xlate++;
 		dev_dbg(dev, "Providing format %s in pass-through mode\n",
 			fmt->name);
@@ -1214,7 +1220,10 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 	struct sh_mobile_ceu_cam *cam = icd->host_priv;
 	struct v4l2_rect *cam_rect = &cam_crop.c;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	unsigned int scale_cam_h, scale_cam_v, scale_ceu_h, scale_ceu_v,
 		out_width, out_height;
 	int interm_width, interm_height;
@@ -1244,16 +1253,16 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 	/* On success cam_crop contains current camera crop */
 
 	/* 3. Retrieve camera output window */
-	ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
+	ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt);
 	if (ret < 0)
 		return ret;
 
-	if (mf.width > pcdev->max_width || mf.height > pcdev->max_height)
+	if (mf->width > pcdev->max_width || mf->height > pcdev->max_height)
 		return -EINVAL;
 
 	/* 4. Calculate camera scales */
-	scale_cam_h	= calc_generic_scale(cam_rect->width, mf.width);
-	scale_cam_v	= calc_generic_scale(cam_rect->height, mf.height);
+	scale_cam_h	= calc_generic_scale(cam_rect->width, mf->width);
+	scale_cam_v	= calc_generic_scale(cam_rect->height, mf->height);
 
 	/* Calculate intermediate window */
 	interm_width	= scale_down(rect->width, scale_cam_h);
@@ -1264,7 +1273,7 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 
 		new_scale_h = calc_generic_scale(rect->width, icd->user_width);
 
-		mf.width = scale_down(cam_rect->width, new_scale_h);
+		mf->width = scale_down(cam_rect->width, new_scale_h);
 	}
 
 	if (interm_height < icd->user_height) {
@@ -1272,26 +1281,26 @@ static int sh_mobile_ceu_set_crop(struct soc_camera_device *icd,
 
 		new_scale_v = calc_generic_scale(rect->height, icd->user_height);
 
-		mf.height = scale_down(cam_rect->height, new_scale_v);
+		mf->height = scale_down(cam_rect->height, new_scale_v);
 	}
 
 	if (interm_width < icd->user_width || interm_height < icd->user_height) {
 		ret = v4l2_device_call_until_err(sd->v4l2_dev,
-					soc_camera_grp_id(icd), video,
-					s_mbus_fmt, &mf);
+					soc_camera_grp_id(icd), pad,
+					set_fmt, NULL, &fmt);
 		if (ret < 0)
 			return ret;
 
-		dev_geo(dev, "New camera output %ux%u\n", mf.width, mf.height);
-		scale_cam_h	= calc_generic_scale(cam_rect->width, mf.width);
-		scale_cam_v	= calc_generic_scale(cam_rect->height, mf.height);
+		dev_geo(dev, "New camera output %ux%u\n", mf->width, mf->height);
+		scale_cam_h	= calc_generic_scale(cam_rect->width, mf->width);
+		scale_cam_v	= calc_generic_scale(cam_rect->height, mf->height);
 		interm_width	= scale_down(rect->width, scale_cam_h);
 		interm_height	= scale_down(rect->height, scale_cam_v);
 	}
 
 	/* Cache camera output window */
-	cam->width	= mf.width;
-	cam->height	= mf.height;
+	cam->width	= mf->width;
+	cam->height	= mf->height;
 
 	if (pcdev->image_mode) {
 		out_width	= min(interm_width, icd->user_width);
@@ -1490,7 +1499,11 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 	const struct soc_camera_format_xlate *xlate;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
+	struct v4l2_mbus_framefmt *mf = &format.format;
 	__u32 pixfmt = pix->pixelformat;
 	int width, height;
 	int ret;
@@ -1518,21 +1531,21 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 	height = pix->height;
 
 	/* limit to sensor capabilities */
-	mf.width	= pix->width;
-	mf.height	= pix->height;
-	mf.field	= pix->field;
-	mf.code		= xlate->code;
-	mf.colorspace	= pix->colorspace;
+	mf->width	= pix->width;
+	mf->height	= pix->height;
+	mf->field	= pix->field;
+	mf->code	= xlate->code;
+	mf->colorspace	= pix->colorspace;
 
 	ret = v4l2_device_call_until_err(sd->v4l2_dev, soc_camera_grp_id(icd),
-					 video, try_mbus_fmt, &mf);
+					 pad, set_fmt, &pad_cfg, &format);
 	if (ret < 0)
 		return ret;
 
-	pix->width	= mf.width;
-	pix->height	= mf.height;
-	pix->field	= mf.field;
-	pix->colorspace	= mf.colorspace;
+	pix->width	= mf->width;
+	pix->height	= mf->height;
+	pix->field	= mf->field;
+	pix->colorspace	= mf->colorspace;
 
 	switch (pixfmt) {
 	case V4L2_PIX_FMT_NV12:
@@ -1547,11 +1560,11 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 			 * requested a bigger rectangle, it will not return a
 			 * smaller one.
 			 */
-			mf.width = pcdev->max_width;
-			mf.height = pcdev->max_height;
+			mf->width = pcdev->max_width;
+			mf->height = pcdev->max_height;
 			ret = v4l2_device_call_until_err(sd->v4l2_dev,
-					soc_camera_grp_id(icd), video,
-					try_mbus_fmt, &mf);
+					soc_camera_grp_id(icd), pad,
+					set_fmt, &pad_cfg, &format);
 			if (ret < 0) {
 				/* Shouldn't actually happen... */
 				dev_err(icd->parent,
@@ -1560,9 +1573,9 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 			}
 		}
 		/* We will scale exactly */
-		if (mf.width > width)
+		if (mf->width > width)
 			pix->width = width;
-		if (mf.height > height)
+		if (mf->height > height)
 			pix->height = height;
 
 		pix->bytesperline = max(pix->bytesperline, pix->width);
diff --git a/drivers/media/platform/soc_camera/sh_mobile_csi2.c b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
index cd93241eb497..12d3626ecf22 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_csi2.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
@@ -45,11 +45,17 @@ struct sh_csi2 {
 
 static void sh_csi2_hwinit(struct sh_csi2 *priv);
 
-static int sh_csi2_try_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_mbus_framefmt *mf)
+static int sh_csi2_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
 	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
 	struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
+	struct v4l2_mbus_framefmt *mf = &format->format;
+	u32 tmp = (priv->client->channel & 3) << 8;
+
+	if (format->pad)
+		return -EINVAL;
 
 	if (mf->width > 8188)
 		mf->width = 8188;
@@ -85,21 +91,11 @@ static int sh_csi2_try_fmt(struct v4l2_subdev *sd,
 		break;
 	}
 
-	return 0;
-}
-
-/*
- * We have done our best in try_fmt to try and tell the sensor, which formats
- * we support. If now the configuration is unsuitable for us we can only
- * error out.
- */
-static int sh_csi2_s_fmt(struct v4l2_subdev *sd,
-			 struct v4l2_mbus_framefmt *mf)
-{
-	struct sh_csi2 *priv = container_of(sd, struct sh_csi2, subdev);
-	u32 tmp = (priv->client->channel & 3) << 8;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *mf;
+		return 0;
+	}
 
-	dev_dbg(sd->v4l2_dev->dev, "%s(%u)\n", __func__, mf->code);
 	if (mf->width > 8188 || mf->width & 1)
 		return -EINVAL;
 
@@ -211,12 +207,14 @@ static int sh_csi2_s_mbus_config(struct v4l2_subdev *sd,
 }
 
 static struct v4l2_subdev_video_ops sh_csi2_subdev_video_ops = {
-	.s_mbus_fmt	= sh_csi2_s_fmt,
-	.try_mbus_fmt	= sh_csi2_try_fmt,
 	.g_mbus_config	= sh_csi2_g_mbus_config,
 	.s_mbus_config	= sh_csi2_s_mbus_config,
 };
 
+static struct v4l2_subdev_pad_ops sh_csi2_subdev_pad_ops = {
+	.set_fmt	= sh_csi2_set_fmt,
+};
+
 static void sh_csi2_hwinit(struct sh_csi2 *priv)
 {
 	struct sh_csi2_pdata *pdata = priv->pdev->dev.platform_data;
@@ -313,6 +311,7 @@ static struct v4l2_subdev_core_ops sh_csi2_subdev_core_ops = {
 static struct v4l2_subdev_ops sh_csi2_subdev_ops = {
 	.core	= &sh_csi2_subdev_core_ops,
 	.video	= &sh_csi2_subdev_video_ops,
+	.pad	= &sh_csi2_subdev_pad_ops,
 };
 
 static int sh_csi2_probe(struct platform_device *pdev)
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index 7bfe7665687f..d708df410f74 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -484,10 +484,14 @@ static int soc_camera_init_user_formats(struct soc_camera_device *icd)
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	unsigned int i, fmts = 0, raw_fmts = 0;
 	int ret;
-	u32 code;
+	struct v4l2_subdev_mbus_code_enum code = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 
-	while (!v4l2_subdev_call(sd, video, enum_mbus_fmt, raw_fmts, &code))
+	while (!v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code)) {
 		raw_fmts++;
+		code.index++;
+	}
 
 	if (!ici->ops->get_formats)
 		/*
@@ -521,11 +525,12 @@ static int soc_camera_init_user_formats(struct soc_camera_device *icd)
 	fmts = 0;
 	for (i = 0; i < raw_fmts; i++)
 		if (!ici->ops->get_formats) {
-			v4l2_subdev_call(sd, video, enum_mbus_fmt, i, &code);
+			code.index = i;
+			v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code);
 			icd->user_formats[fmts].host_fmt =
-				soc_mbus_get_fmtdesc(code);
+				soc_mbus_get_fmtdesc(code.code);
 			if (icd->user_formats[fmts].host_fmt)
-				icd->user_formats[fmts++].code = code;
+				icd->user_formats[fmts++].code = code.code;
 		} else {
 			ret = ici->ops->get_formats(icd, i,
 						    &icd->user_formats[fmts]);
@@ -1284,7 +1289,10 @@ static struct soc_camera_device *soc_camera_add_pdev(struct soc_camera_async_cli
 static int soc_camera_probe_finish(struct soc_camera_device *icd)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	struct v4l2_mbus_framefmt mf;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	struct v4l2_mbus_framefmt *mf = &fmt.format;
 	int ret;
 
 	sd->grp_id = soc_camera_grp_id(icd);
@@ -1314,11 +1322,11 @@ static int soc_camera_probe_finish(struct soc_camera_device *icd)
 		goto evidstart;
 
 	/* Try to improve our guess of a reasonable window format */
-	if (!v4l2_subdev_call(sd, video, g_mbus_fmt, &mf)) {
-		icd->user_width		= mf.width;
-		icd->user_height	= mf.height;
-		icd->colorspace		= mf.colorspace;
-		icd->field		= mf.field;
+	if (!v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt)) {
+		icd->user_width		= mf->width;
+		icd->user_height	= mf->height;
+		icd->colorspace		= mf->colorspace;
+		icd->field		= mf->field;
 	}
 	soc_camera_remove_device(icd);
 
diff --git a/drivers/media/platform/soc_camera/soc_camera_platform.c b/drivers/media/platform/soc_camera/soc_camera_platform.c
index f535910b4187..cc8eb0758219 100644
--- a/drivers/media/platform/soc_camera/soc_camera_platform.c
+++ b/drivers/media/platform/soc_camera/soc_camera_platform.c
@@ -37,9 +37,11 @@ static int soc_camera_platform_s_stream(struct v4l2_subdev *sd, int enable)
 }
 
 static int soc_camera_platform_fill_fmt(struct v4l2_subdev *sd,
-					struct v4l2_mbus_framefmt *mf)
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
 	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf = &format->format;
 
 	mf->width	= p->format.width;
 	mf->height	= p->format.height;
@@ -61,15 +63,16 @@ static struct v4l2_subdev_core_ops platform_subdev_core_ops = {
 	.s_power = soc_camera_platform_s_power,
 };
 
-static int soc_camera_platform_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
-					u32 *code)
+static int soc_camera_platform_enum_mbus_code(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_mbus_code_enum *code)
 {
 	struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
 
-	if (index)
+	if (code->pad || code->index)
 		return -EINVAL;
 
-	*code = p->format.code;
+	code->code = p->format.code;
 	return 0;
 }
 
@@ -117,18 +120,21 @@ static int soc_camera_platform_g_mbus_config(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_video_ops platform_subdev_video_ops = {
 	.s_stream	= soc_camera_platform_s_stream,
-	.enum_mbus_fmt	= soc_camera_platform_enum_fmt,
 	.cropcap	= soc_camera_platform_cropcap,
 	.g_crop		= soc_camera_platform_g_crop,
-	.try_mbus_fmt	= soc_camera_platform_fill_fmt,
-	.g_mbus_fmt	= soc_camera_platform_fill_fmt,
-	.s_mbus_fmt	= soc_camera_platform_fill_fmt,
 	.g_mbus_config	= soc_camera_platform_g_mbus_config,
 };
 
+static const struct v4l2_subdev_pad_ops platform_subdev_pad_ops = {
+	.enum_mbus_code = soc_camera_platform_enum_mbus_code,
+	.get_fmt	= soc_camera_platform_fill_fmt,
+	.set_fmt	= soc_camera_platform_fill_fmt,
+};
+
 static struct v4l2_subdev_ops platform_subdev_ops = {
 	.core	= &platform_subdev_core_ops,
 	.video	= &platform_subdev_video_ops,
+	.pad	= &platform_subdev_pad_ops,
 };
 
 static int soc_camera_platform_probe(struct platform_device *pdev)
diff --git a/drivers/media/platform/soc_camera/soc_scale_crop.c b/drivers/media/platform/soc_camera/soc_scale_crop.c
index 8e74fb7f2a07..bda29bc1b933 100644
--- a/drivers/media/platform/soc_camera/soc_scale_crop.c
+++ b/drivers/media/platform/soc_camera/soc_scale_crop.c
@@ -211,22 +211,23 @@ int soc_camera_client_s_crop(struct v4l2_subdev *sd,
 }
 EXPORT_SYMBOL(soc_camera_client_s_crop);
 
-/* Iterative s_mbus_fmt, also updates cached client crop on success */
-static int client_s_fmt(struct soc_camera_device *icd,
+/* Iterative set_fmt, also updates cached client crop on success */
+static int client_set_fmt(struct soc_camera_device *icd,
 			struct v4l2_rect *rect, struct v4l2_rect *subrect,
 			unsigned int max_width, unsigned int max_height,
-			struct v4l2_mbus_framefmt *mf, bool host_can_scale)
+			struct v4l2_subdev_format *format, bool host_can_scale)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 	struct device *dev = icd->parent;
+	struct v4l2_mbus_framefmt *mf = &format->format;
 	unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
 	struct v4l2_cropcap cap;
 	bool host_1to1;
 	int ret;
 
 	ret = v4l2_device_call_until_err(sd->v4l2_dev,
-					 soc_camera_grp_id(icd), video,
-					 s_mbus_fmt, mf);
+					 soc_camera_grp_id(icd), pad,
+					 set_fmt, NULL, format);
 	if (ret < 0)
 		return ret;
 
@@ -265,8 +266,8 @@ static int client_s_fmt(struct soc_camera_device *icd,
 		mf->width = tmp_w;
 		mf->height = tmp_h;
 		ret = v4l2_device_call_until_err(sd->v4l2_dev,
-					soc_camera_grp_id(icd), video,
-					s_mbus_fmt, mf);
+					soc_camera_grp_id(icd), pad,
+					set_fmt, NULL, format);
 		dev_geo(dev, "Camera scaled to %ux%u\n",
 			mf->width, mf->height);
 		if (ret < 0) {
@@ -309,7 +310,11 @@ int soc_camera_client_scale(struct soc_camera_device *icd,
 			bool host_can_scale, unsigned int shift)
 {
 	struct device *dev = icd->parent;
-	struct v4l2_mbus_framefmt mf_tmp = *mf;
+	struct v4l2_subdev_format fmt_tmp = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format = *mf,
+	};
+	struct v4l2_mbus_framefmt *mf_tmp = &fmt_tmp.format;
 	unsigned int scale_h, scale_v;
 	int ret;
 
@@ -317,25 +322,25 @@ int soc_camera_client_scale(struct soc_camera_device *icd,
 	 * 5. Apply iterative camera S_FMT for camera user window (also updates
 	 *    client crop cache and the imaginary sub-rectangle).
 	 */
-	ret = client_s_fmt(icd, rect, subrect, *width, *height,
-			   &mf_tmp, host_can_scale);
+	ret = client_set_fmt(icd, rect, subrect, *width, *height,
+			   &fmt_tmp, host_can_scale);
 	if (ret < 0)
 		return ret;
 
 	dev_geo(dev, "5: camera scaled to %ux%u\n",
-		mf_tmp.width, mf_tmp.height);
+		mf_tmp->width, mf_tmp->height);
 
 	/* 6. Retrieve camera output window (g_fmt) */
 
 	/* unneeded - it is already in "mf_tmp" */
 
 	/* 7. Calculate new client scales. */
-	scale_h = soc_camera_calc_scale(rect->width, shift, mf_tmp.width);
-	scale_v = soc_camera_calc_scale(rect->height, shift, mf_tmp.height);
+	scale_h = soc_camera_calc_scale(rect->width, shift, mf_tmp->width);
+	scale_v = soc_camera_calc_scale(rect->height, shift, mf_tmp->height);
 
-	mf->width	= mf_tmp.width;
-	mf->height	= mf_tmp.height;
-	mf->colorspace	= mf_tmp.colorspace;
+	mf->width	= mf_tmp->width;
+	mf->height	= mf_tmp->height;
+	mf->colorspace	= mf_tmp->colorspace;
 
 	/*
 	 * 8. Calculate new host crop - apply camera scales to previously
diff --git a/drivers/media/platform/sti/bdisp/Makefile b/drivers/media/platform/sti/bdisp/Makefile
new file mode 100644
index 000000000000..bc53496fa74c
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_STI_BDISP) := bdisp.o
+
+bdisp-objs := bdisp-v4l2.o bdisp-hw.o bdisp-debug.o
diff --git a/drivers/media/platform/sti/bdisp/bdisp-debug.c b/drivers/media/platform/sti/bdisp/bdisp-debug.c
new file mode 100644
index 000000000000..18282a0f80c9
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-debug.c
@@ -0,0 +1,679 @@
+/*
+ * 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/debugfs.h>
+#include <linux/pm_runtime.h>
+
+#include "bdisp.h"
+#include "bdisp-filter.h"
+#include "bdisp-reg.h"
+
+void bdisp_dbg_perf_begin(struct bdisp_dev *bdisp)
+{
+	bdisp->dbg.hw_start = ktime_get();
+}
+
+void bdisp_dbg_perf_end(struct bdisp_dev *bdisp)
+{
+	s64 time_us;
+
+	time_us = ktime_us_delta(ktime_get(), bdisp->dbg.hw_start);
+
+	if (!bdisp->dbg.min_duration)
+		bdisp->dbg.min_duration = time_us;
+	else
+		bdisp->dbg.min_duration = min(time_us, bdisp->dbg.min_duration);
+
+	bdisp->dbg.last_duration = time_us;
+	bdisp->dbg.max_duration = max(time_us, bdisp->dbg.max_duration);
+	bdisp->dbg.tot_duration += time_us;
+}
+
+static void bdisp_dbg_dump_ins(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "INS\t0x%08X\t", val);
+
+	switch (val & BLT_INS_S1_MASK) {
+	case BLT_INS_S1_OFF:
+		break;
+	case BLT_INS_S1_MEM:
+		seq_puts(s, "SRC1=mem - ");
+		break;
+	case BLT_INS_S1_CF:
+		seq_puts(s, "SRC1=ColorFill - ");
+		break;
+	case BLT_INS_S1_COPY:
+		seq_puts(s, "SRC1=copy - ");
+		break;
+	case BLT_INS_S1_FILL:
+		seq_puts(s, "SRC1=fil - ");
+		break;
+	default:
+		seq_puts(s, "SRC1=??? - ");
+		break;
+	}
+
+	switch (val & BLT_INS_S2_MASK) {
+	case BLT_INS_S2_OFF:
+		break;
+	case BLT_INS_S2_MEM:
+		seq_puts(s, "SRC2=mem - ");
+		break;
+	case BLT_INS_S2_CF:
+		seq_puts(s, "SRC2=ColorFill - ");
+		break;
+	default:
+		seq_puts(s, "SRC2=??? - ");
+		break;
+	}
+
+	if ((val & BLT_INS_S3_MASK) == BLT_INS_S3_MEM)
+		seq_puts(s, "SRC3=mem - ");
+
+	if (val & BLT_INS_IVMX)
+		seq_puts(s, "IVMX - ");
+	if (val & BLT_INS_CLUT)
+		seq_puts(s, "CLUT - ");
+	if (val & BLT_INS_SCALE)
+		seq_puts(s, "Scale - ");
+	if (val & BLT_INS_FLICK)
+		seq_puts(s, "Flicker - ");
+	if (val & BLT_INS_CLIP)
+		seq_puts(s, "Clip - ");
+	if (val & BLT_INS_CKEY)
+		seq_puts(s, "ColorKey - ");
+	if (val & BLT_INS_OVMX)
+		seq_puts(s, "OVMX - ");
+	if (val & BLT_INS_DEI)
+		seq_puts(s, "Deint - ");
+	if (val & BLT_INS_PMASK)
+		seq_puts(s, "PlaneMask - ");
+	if (val & BLT_INS_VC1R)
+		seq_puts(s, "VC1R - ");
+	if (val & BLT_INS_ROTATE)
+		seq_puts(s, "Rotate - ");
+	if (val & BLT_INS_GRAD)
+		seq_puts(s, "GradFill - ");
+	if (val & BLT_INS_AQLOCK)
+		seq_puts(s, "AQLock - ");
+	if (val & BLT_INS_PACE)
+		seq_puts(s, "Pace - ");
+	if (val & BLT_INS_IRQ)
+		seq_puts(s, "IRQ - ");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_tty(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "TTY\t0x%08X\t", val);
+	seq_printf(s, "Pitch=%d - ", val & 0xFFFF);
+
+	switch ((val & BLT_TTY_COL_MASK) >> BLT_TTY_COL_SHIFT) {
+	case BDISP_RGB565:
+		seq_puts(s, "RGB565 - ");
+		break;
+	case BDISP_XRGB8888:
+		seq_puts(s, "xRGB888 - ");
+		break;
+	case BDISP_ARGB8888:
+		seq_puts(s, "ARGB8888 - ");
+		break;
+	case BDISP_NV12:
+		seq_puts(s, "NV12 - ");
+		break;
+	case BDISP_YUV_3B:
+		seq_puts(s, "YUV420P - ");
+		break;
+	default:
+		seq_puts(s, "ColorFormat ??? - ");
+		break;
+	}
+
+	if (val & BLT_TTY_ALPHA_R)
+		seq_puts(s, "AlphaRange - ");
+	if (val & BLT_TTY_CR_NOT_CB)
+		seq_puts(s, "CrNotCb - ");
+	if (val & BLT_TTY_MB)
+		seq_puts(s, "MB - ");
+	if (val & BLT_TTY_HSO)
+		seq_puts(s, "HSO inverse - ");
+	if (val & BLT_TTY_VSO)
+		seq_puts(s, "VSO inverse - ");
+	if (val & BLT_TTY_DITHER)
+		seq_puts(s, "Dither - ");
+	if (val & BLT_TTY_CHROMA)
+		seq_puts(s, "Write CHROMA - ");
+	if (val & BLT_TTY_BIG_END)
+		seq_puts(s, "BigEndian - ");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_xy(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+	seq_printf(s, "(%d,%d)\n", val & 0xFFFF, (val >> 16));
+}
+
+static void bdisp_dbg_dump_sz(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+	seq_printf(s, "%dx%d\n", val & 0x1FFF, (val >> 16) & 0x1FFF);
+}
+
+static void bdisp_dbg_dump_sty(struct seq_file *s,
+			       u32 val, u32 addr, char *name)
+{
+	bool s1, s2, s3;
+
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!addr || !name || (strlen(name) < 2))
+		goto done;
+
+	s1 = name[strlen(name) - 1] == '1';
+	s2 = name[strlen(name) - 1] == '2';
+	s3 = name[strlen(name) - 1] == '3';
+
+	seq_printf(s, "Pitch=%d - ", val & 0xFFFF);
+
+	switch ((val & BLT_TTY_COL_MASK) >> BLT_TTY_COL_SHIFT) {
+	case BDISP_RGB565:
+		seq_puts(s, "RGB565 - ");
+		break;
+	case BDISP_XRGB8888:
+		seq_puts(s, "xRGB888 - ");
+		break;
+	case BDISP_ARGB8888:
+		seq_puts(s, "ARGB888 - ");
+		break;
+	case BDISP_NV12:
+		seq_puts(s, "NV12 - ");
+		break;
+	case BDISP_YUV_3B:
+		seq_puts(s, "YUV420P - ");
+		break;
+	default:
+		seq_puts(s, "ColorFormat ??? - ");
+		break;
+	}
+
+	if ((val & BLT_TTY_ALPHA_R) && !s3)
+		seq_puts(s, "AlphaRange - ");
+	if ((val & BLT_S1TY_A1_SUBSET) && !s3)
+		seq_puts(s, "A1SubSet - ");
+	if ((val & BLT_TTY_MB) && !s1)
+		seq_puts(s, "MB - ");
+	if (val & BLT_TTY_HSO)
+		seq_puts(s, "HSO inverse - ");
+	if (val & BLT_TTY_VSO)
+		seq_puts(s, "VSO inverse - ");
+	if ((val & BLT_S1TY_CHROMA_EXT) && (s1 || s2))
+		seq_puts(s, "ChromaExt - ");
+	if ((val & BLT_S3TY_BLANK_ACC) && s3)
+		seq_puts(s, "Blank Acc - ");
+	if ((val & BTL_S1TY_SUBBYTE) && !s3)
+		seq_puts(s, "SubByte - ");
+	if ((val & BLT_S1TY_RGB_EXP) && !s3)
+		seq_puts(s, "RGBExpand - ");
+	if ((val & BLT_TTY_BIG_END) && !s3)
+		seq_puts(s, "BigEndian - ");
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_fctl(struct seq_file *s, u32 val)
+{
+	seq_printf(s, "FCTL\t0x%08X\t", val);
+
+	if ((val & BLT_FCTL_Y_HV_SCALE) == BLT_FCTL_Y_HV_SCALE)
+		seq_puts(s, "Resize Luma - ");
+	else if ((val & BLT_FCTL_Y_HV_SCALE) == BLT_FCTL_Y_HV_SAMPLE)
+		seq_puts(s, "Sample Luma - ");
+
+	if ((val & BLT_FCTL_HV_SCALE) == BLT_FCTL_HV_SCALE)
+		seq_puts(s, "Resize Chroma");
+	else if ((val & BLT_FCTL_HV_SCALE) == BLT_FCTL_HV_SAMPLE)
+		seq_puts(s, "Sample Chroma");
+
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_rsf(struct seq_file *s, u32 val, char *name)
+{
+	u32 inc;
+
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!val)
+		goto done;
+
+	inc = val & 0xFFFF;
+	seq_printf(s, "H: %d(6.10) / scale~%dx0.1 - ", inc, 1024 * 10 / inc);
+
+	inc = val >> 16;
+	seq_printf(s, "V: %d(6.10) / scale~%dx0.1", inc, 1024 * 10 / inc);
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_rzi(struct seq_file *s, u32 val, char *name)
+{
+	seq_printf(s, "%s\t0x%08X\t", name, val);
+
+	if (!val)
+		goto done;
+
+	seq_printf(s, "H: init=%d repeat=%d - ", val & 0x3FF, (val >> 12) & 7);
+	val >>= 16;
+	seq_printf(s, "V: init=%d repeat=%d", val & 0x3FF, (val >> 12) & 7);
+
+done:
+	seq_puts(s, "\n");
+}
+
+static void bdisp_dbg_dump_ivmx(struct seq_file *s,
+				u32 c0, u32 c1, u32 c2, u32 c3)
+{
+	seq_printf(s, "IVMX0\t0x%08X\n", c0);
+	seq_printf(s, "IVMX1\t0x%08X\n", c1);
+	seq_printf(s, "IVMX2\t0x%08X\n", c2);
+	seq_printf(s, "IVMX3\t0x%08X\t", c3);
+
+	if (!c0 && !c1 && !c2 && !c3) {
+		seq_puts(s, "\n");
+		return;
+	}
+
+	if ((c0 == bdisp_rgb_to_yuv[0]) &&
+	    (c1 == bdisp_rgb_to_yuv[1]) &&
+	    (c2 == bdisp_rgb_to_yuv[2]) &&
+	    (c3 == bdisp_rgb_to_yuv[3])) {
+		seq_puts(s, "RGB to YUV\n");
+		return;
+	}
+
+	if ((c0 == bdisp_yuv_to_rgb[0]) &&
+	    (c1 == bdisp_yuv_to_rgb[1]) &&
+	    (c2 == bdisp_yuv_to_rgb[2]) &&
+	    (c3 == bdisp_yuv_to_rgb[3])) {
+		seq_puts(s, "YUV to RGB\n");
+		return;
+	}
+	seq_puts(s, "Unknown conversion\n");
+}
+
+static int bdisp_dbg_last_nodes(struct seq_file *s, void *data)
+{
+	/* Not dumping all fields, focusing on significant ones */
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_node *node;
+	int i = 0;
+
+	if (!bdisp->dbg.copy_node[0]) {
+		seq_puts(s, "No node built yet\n");
+		return 0;
+	}
+
+	do {
+		node = bdisp->dbg.copy_node[i];
+		if (!node)
+			break;
+		seq_printf(s, "--------\nNode %d:\n", i);
+		seq_puts(s, "-- General --\n");
+		seq_printf(s, "NIP\t0x%08X\n", node->nip);
+		seq_printf(s, "CIC\t0x%08X\n", node->cic);
+		bdisp_dbg_dump_ins(s, node->ins);
+		seq_printf(s, "ACK\t0x%08X\n", node->ack);
+		seq_puts(s, "-- Target --\n");
+		seq_printf(s, "TBA\t0x%08X\n", node->tba);
+		bdisp_dbg_dump_tty(s, node->tty);
+		bdisp_dbg_dump_xy(s, node->txy, "TXY");
+		bdisp_dbg_dump_sz(s, node->tsz, "TSZ");
+		/* Color Fill not dumped */
+		seq_puts(s, "-- Source 1 --\n");
+		seq_printf(s, "S1BA\t0x%08X\n", node->s1ba);
+		bdisp_dbg_dump_sty(s, node->s1ty, node->s1ba, "S1TY");
+		bdisp_dbg_dump_xy(s, node->s1xy, "S1XY");
+		seq_puts(s, "-- Source 2 --\n");
+		seq_printf(s, "S2BA\t0x%08X\n", node->s2ba);
+		bdisp_dbg_dump_sty(s, node->s2ty, node->s2ba, "S2TY");
+		bdisp_dbg_dump_xy(s, node->s2xy, "S2XY");
+		bdisp_dbg_dump_sz(s, node->s2sz, "S2SZ");
+		seq_puts(s, "-- Source 3 --\n");
+		seq_printf(s, "S3BA\t0x%08X\n", node->s3ba);
+		bdisp_dbg_dump_sty(s, node->s3ty, node->s3ba, "S3TY");
+		bdisp_dbg_dump_xy(s, node->s3xy, "S3XY");
+		bdisp_dbg_dump_sz(s, node->s3sz, "S3SZ");
+		/* Clipping not dumped */
+		/* CLUT not dumped */
+		seq_puts(s, "-- Filter & Mask --\n");
+		bdisp_dbg_dump_fctl(s, node->fctl);
+		/* PMK not dumped */
+		seq_puts(s, "-- Chroma Filter --\n");
+		bdisp_dbg_dump_rsf(s, node->rsf, "RSF");
+		bdisp_dbg_dump_rzi(s, node->rzi, "RZI");
+		seq_printf(s, "HFP\t0x%08X\n", node->hfp);
+		seq_printf(s, "VFP\t0x%08X\n", node->vfp);
+		seq_puts(s, "-- Luma Filter --\n");
+		bdisp_dbg_dump_rsf(s, node->y_rsf, "Y_RSF");
+		bdisp_dbg_dump_rzi(s, node->y_rzi, "Y_RZI");
+		seq_printf(s, "Y_HFP\t0x%08X\n", node->y_hfp);
+		seq_printf(s, "Y_VFP\t0x%08X\n", node->y_vfp);
+		/* Flicker not dumped */
+		/* Color key not dumped */
+		/* Reserved not dumped */
+		/* Static Address & User not dumped */
+		seq_puts(s, "-- Input Versatile Matrix --\n");
+		bdisp_dbg_dump_ivmx(s, node->ivmx0, node->ivmx1,
+				    node->ivmx2, node->ivmx3);
+		/* Output Versatile Matrix not dumped */
+		/* Pace not dumped */
+		/* VC1R & DEI not dumped */
+		/* Gradient Fill not dumped */
+	} while ((++i < MAX_NB_NODE) && node->nip);
+
+	return 0;
+}
+
+static int bdisp_dbg_last_nodes_raw(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_node *node;
+	u32 *val;
+	int j, i = 0;
+
+	if (!bdisp->dbg.copy_node[0]) {
+		seq_puts(s, "No node built yet\n");
+		return 0;
+	}
+
+	do {
+		node = bdisp->dbg.copy_node[i];
+		if (!node)
+			break;
+
+		seq_printf(s, "--------\nNode %d:\n", i);
+		val = (u32 *)node;
+		for (j = 0; j < sizeof(struct bdisp_node) / sizeof(u32); j++)
+			seq_printf(s, "0x%08X\n", *val++);
+	} while ((++i < MAX_NB_NODE) && node->nip);
+
+	return 0;
+}
+
+static const char *bdisp_fmt_to_str(struct bdisp_frame frame)
+{
+	switch (frame.fmt->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		return "YUV420P";
+	case V4L2_PIX_FMT_NV12:
+		if (frame.field == V4L2_FIELD_INTERLACED)
+			return "NV12 interlaced";
+		else
+			return "NV12";
+	case V4L2_PIX_FMT_RGB565:
+		return "RGB16";
+	case V4L2_PIX_FMT_XBGR32:
+		return "XRGB";
+	case V4L2_PIX_FMT_ABGR32:
+		return "ARGB";
+	default:
+		return "????";
+	}
+}
+
+static int bdisp_dbg_last_request(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_request *request = &bdisp->dbg.copy_request;
+	struct bdisp_frame src, dst;
+
+	if (!request->nb_req) {
+		seq_puts(s, "No request\n");
+		return 0;
+	}
+
+	src = request->src;
+	dst = request->dst;
+
+	seq_printf(s, "\nRequest #%d\n", request->nb_req);
+
+	seq_printf(s, "Format:    %s\t\t\t%s\n",
+		   bdisp_fmt_to_str(src), bdisp_fmt_to_str(dst));
+	seq_printf(s, "Crop area: %dx%d @ %d,%d  ==>\t%dx%d @ %d,%d\n",
+		   src.crop.width, src.crop.height,
+		   src.crop.left, src.crop.top,
+		   dst.crop.width, dst.crop.height,
+		   dst.crop.left, dst.crop.top);
+	seq_printf(s, "Buff size: %dx%d\t\t%dx%d\n\n",
+		   src.width, src.height, dst.width, dst.height);
+
+	if (request->hflip)
+		seq_puts(s, "Horizontal flip\n\n");
+
+	if (request->vflip)
+		seq_puts(s, "Vertical flip\n\n");
+
+	return 0;
+}
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08X\n", readl(bdisp->regs + reg))
+
+static int bdisp_dbg_regs(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	int ret;
+	unsigned int i;
+
+	ret = pm_runtime_get_sync(bdisp->dev);
+	if (ret < 0) {
+		seq_puts(s, "Cannot wake up IP\n");
+		return 0;
+	}
+
+	seq_printf(s, "Reg @ = 0x%p\n", bdisp->regs);
+
+	seq_puts(s, "\nStatic:\n");
+	DUMP(BLT_CTL);
+	DUMP(BLT_ITS);
+	DUMP(BLT_STA1);
+	DUMP(BLT_AQ1_CTL);
+	DUMP(BLT_AQ1_IP);
+	DUMP(BLT_AQ1_LNA);
+	DUMP(BLT_AQ1_STA);
+	DUMP(BLT_ITM0);
+
+	seq_puts(s, "\nPlugs:\n");
+	DUMP(BLT_PLUGS1_OP2);
+	DUMP(BLT_PLUGS1_CHZ);
+	DUMP(BLT_PLUGS1_MSZ);
+	DUMP(BLT_PLUGS1_PGZ);
+	DUMP(BLT_PLUGS2_OP2);
+	DUMP(BLT_PLUGS2_CHZ);
+	DUMP(BLT_PLUGS2_MSZ);
+	DUMP(BLT_PLUGS2_PGZ);
+	DUMP(BLT_PLUGS3_OP2);
+	DUMP(BLT_PLUGS3_CHZ);
+	DUMP(BLT_PLUGS3_MSZ);
+	DUMP(BLT_PLUGS3_PGZ);
+	DUMP(BLT_PLUGT_OP2);
+	DUMP(BLT_PLUGT_CHZ);
+	DUMP(BLT_PLUGT_MSZ);
+	DUMP(BLT_PLUGT_PGZ);
+
+	seq_puts(s, "\nNode:\n");
+	DUMP(BLT_NIP);
+	DUMP(BLT_CIC);
+	DUMP(BLT_INS);
+	DUMP(BLT_ACK);
+	DUMP(BLT_TBA);
+	DUMP(BLT_TTY);
+	DUMP(BLT_TXY);
+	DUMP(BLT_TSZ);
+	DUMP(BLT_S1BA);
+	DUMP(BLT_S1TY);
+	DUMP(BLT_S1XY);
+	DUMP(BLT_S2BA);
+	DUMP(BLT_S2TY);
+	DUMP(BLT_S2XY);
+	DUMP(BLT_S2SZ);
+	DUMP(BLT_S3BA);
+	DUMP(BLT_S3TY);
+	DUMP(BLT_S3XY);
+	DUMP(BLT_S3SZ);
+	DUMP(BLT_FCTL);
+	DUMP(BLT_RSF);
+	DUMP(BLT_RZI);
+	DUMP(BLT_HFP);
+	DUMP(BLT_VFP);
+	DUMP(BLT_Y_RSF);
+	DUMP(BLT_Y_RZI);
+	DUMP(BLT_Y_HFP);
+	DUMP(BLT_Y_VFP);
+	DUMP(BLT_IVMX0);
+	DUMP(BLT_IVMX1);
+	DUMP(BLT_IVMX2);
+	DUMP(BLT_IVMX3);
+	DUMP(BLT_OVMX0);
+	DUMP(BLT_OVMX1);
+	DUMP(BLT_OVMX2);
+	DUMP(BLT_OVMX3);
+	DUMP(BLT_DEI);
+
+	seq_puts(s, "\nFilter:\n");
+	for (i = 0; i < BLT_NB_H_COEF; i++) {
+		seq_printf(s, "BLT_HFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_HFC_N + i * 4));
+	}
+	for (i = 0; i < BLT_NB_V_COEF; i++) {
+		seq_printf(s, "BLT_VFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_VFC_N + i * 4));
+	}
+
+	seq_puts(s, "\nLuma filter:\n");
+	for (i = 0; i < BLT_NB_H_COEF; i++) {
+		seq_printf(s, "BLT_Y_HFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_Y_HFC_N + i * 4));
+	}
+	for (i = 0; i < BLT_NB_V_COEF; i++) {
+		seq_printf(s, "BLT_Y_VFC%d \t0x%08X\n", i,
+			   readl(bdisp->regs + BLT_Y_VFC_N + i * 4));
+	}
+
+	pm_runtime_put(bdisp->dev);
+
+	return 0;
+}
+
+#define SECOND 1000000
+
+static int bdisp_dbg_perf(struct seq_file *s, void *data)
+{
+	struct bdisp_dev *bdisp = s->private;
+	struct bdisp_request *request = &bdisp->dbg.copy_request;
+	s64 avg_time_us;
+	int avg_fps, min_fps, max_fps, last_fps;
+
+	if (!request->nb_req) {
+		seq_puts(s, "No request\n");
+		return 0;
+	}
+
+	avg_time_us = div64_s64(bdisp->dbg.tot_duration, request->nb_req);
+	if (avg_time_us > SECOND)
+		avg_fps = 0;
+	else
+		avg_fps = SECOND / (s32)avg_time_us;
+
+	if (bdisp->dbg.min_duration > SECOND)
+		min_fps = 0;
+	else
+		min_fps = SECOND / (s32)bdisp->dbg.min_duration;
+
+	if (bdisp->dbg.max_duration > SECOND)
+		max_fps = 0;
+	else
+		max_fps = SECOND / (s32)bdisp->dbg.max_duration;
+
+	if (bdisp->dbg.last_duration > SECOND)
+		last_fps = 0;
+	else
+		last_fps = SECOND / (s32)bdisp->dbg.last_duration;
+
+	seq_printf(s, "HW processing (%d requests):\n", request->nb_req);
+	seq_printf(s, " Average: %5lld us  (%3d fps)\n",
+		   avg_time_us, avg_fps);
+	seq_printf(s, " Min-Max: %5lld us  (%3d fps) - %5lld us  (%3d fps)\n",
+		   bdisp->dbg.min_duration, min_fps,
+		   bdisp->dbg.max_duration, max_fps);
+	seq_printf(s, " Last:    %5lld us  (%3d fps)\n",
+		   bdisp->dbg.last_duration, last_fps);
+
+	return 0;
+}
+
+#define bdisp_dbg_declare(name) \
+	static int bdisp_dbg_##name##_open(struct inode *i, struct file *f) \
+	{ \
+		return single_open(f, bdisp_dbg_##name, i->i_private); \
+	} \
+	static const struct file_operations bdisp_dbg_##name##_fops = { \
+		.open           = bdisp_dbg_##name##_open, \
+		.read           = seq_read, \
+		.llseek         = seq_lseek, \
+		.release        = single_release, \
+	}
+
+#define bdisp_dbg_create_entry(name) \
+	debugfs_create_file(#name, S_IRUGO, bdisp->dbg.debugfs_entry, bdisp, \
+			    &bdisp_dbg_##name##_fops)
+
+bdisp_dbg_declare(regs);
+bdisp_dbg_declare(last_nodes);
+bdisp_dbg_declare(last_nodes_raw);
+bdisp_dbg_declare(last_request);
+bdisp_dbg_declare(perf);
+
+int bdisp_debugfs_create(struct bdisp_dev *bdisp)
+{
+	char dirname[16];
+
+	snprintf(dirname, sizeof(dirname), "%s%d", BDISP_NAME, bdisp->id);
+	bdisp->dbg.debugfs_entry = debugfs_create_dir(dirname, NULL);
+	if (!bdisp->dbg.debugfs_entry)
+		goto err;
+
+	if (!bdisp_dbg_create_entry(regs))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_nodes))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_nodes_raw))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(last_request))
+		goto err;
+
+	if (!bdisp_dbg_create_entry(perf))
+		goto err;
+
+	return 0;
+
+err:
+	bdisp_debugfs_remove(bdisp);
+	return 0;
+}
+
+void bdisp_debugfs_remove(struct bdisp_dev *bdisp)
+{
+	debugfs_remove_recursive(bdisp->dbg.debugfs_entry);
+	bdisp->dbg.debugfs_entry = NULL;
+}
diff --git a/drivers/media/platform/sti/bdisp/bdisp-filter.h b/drivers/media/platform/sti/bdisp/bdisp-filter.h
new file mode 100644
index 000000000000..fc8c54f725ad
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-filter.h
@@ -0,0 +1,346 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#define BDISP_HF_NB             64
+#define BDISP_VF_NB             40
+
+/**
+ * struct bdisp_filter_h_spec - Horizontal filter specification
+ *
+ * @min:        min scale factor for this filter (6.10 fixed point)
+ * @max:        max scale factor for this filter (6.10 fixed point)
+ * coef:        filter coefficients
+ */
+struct bdisp_filter_h_spec {
+	const u16 min;
+	const u16 max;
+	const u8 coef[BDISP_HF_NB];
+};
+
+static const struct bdisp_filter_h_spec bdisp_h_spec[] = {
+	{
+		.min = 0,
+		.max = 921,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0x00, 0x00, 0xff, 0x07, 0x3d, 0xfc, 0x01, 0x00,
+			0x00, 0x01, 0xfd, 0x11, 0x36, 0xf9, 0x02, 0x00,
+			0x00, 0x01, 0xfb, 0x1b, 0x2e, 0xf9, 0x02, 0x00,
+			0x00, 0x01, 0xf9, 0x26, 0x26, 0xf9, 0x01, 0x00,
+			0x00, 0x02, 0xf9, 0x30, 0x19, 0xfb, 0x01, 0x00,
+			0x00, 0x02, 0xf9, 0x39, 0x0e, 0xfd, 0x01, 0x00,
+			0x00, 0x01, 0xfc, 0x3e, 0x06, 0xff, 0x00, 0x00
+		}
+	},
+	{
+		.min = 921,
+		.max = 1024,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1024,
+		.max = 1126,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1126,
+		.max = 1228,
+		.coef = {
+			0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+			0xff, 0x03, 0xfd, 0x08, 0x3e, 0xf9, 0x04, 0xfe,
+			0xfd, 0x06, 0xf8, 0x13, 0x3b, 0xf4, 0x07, 0xfc,
+			0xfb, 0x08, 0xf5, 0x1f, 0x34, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x2b, 0x2a, 0xf1, 0x09, 0xfb,
+			0xfb, 0x09, 0xf2, 0x35, 0x1e, 0xf4, 0x08, 0xfb,
+			0xfc, 0x07, 0xf5, 0x3c, 0x12, 0xf7, 0x06, 0xfd,
+			0xfe, 0x04, 0xfa, 0x3f, 0x07, 0xfc, 0x03, 0xff
+		}
+	},
+	{
+		.min = 1228,
+		.max = 1331,
+		.coef = {
+			0xfd, 0x04, 0xfc, 0x05, 0x39, 0x05, 0xfc, 0x04,
+			0xfc, 0x06, 0xf9, 0x0c, 0x39, 0xfe, 0x00, 0x02,
+			0xfb, 0x08, 0xf6, 0x17, 0x35, 0xf9, 0x02, 0x00,
+			0xfc, 0x08, 0xf4, 0x20, 0x30, 0xf4, 0x05, 0xff,
+			0xfd, 0x07, 0xf4, 0x29, 0x28, 0xf3, 0x07, 0xfd,
+			0xff, 0x05, 0xf5, 0x31, 0x1f, 0xf3, 0x08, 0xfc,
+			0x00, 0x02, 0xf9, 0x38, 0x14, 0xf6, 0x08, 0xfb,
+			0x02, 0x00, 0xff, 0x3a, 0x0b, 0xf8, 0x06, 0xfc
+		}
+	},
+	{
+		.min = 1331,
+		.max = 1433,
+		.coef = {
+			0xfc, 0x06, 0xf9, 0x09, 0x34, 0x09, 0xf9, 0x06,
+			0xfd, 0x07, 0xf7, 0x10, 0x32, 0x02, 0xfc, 0x05,
+			0xfe, 0x07, 0xf6, 0x17, 0x2f, 0xfc, 0xff, 0x04,
+			0xff, 0x06, 0xf5, 0x20, 0x2a, 0xf9, 0x01, 0x02,
+			0x00, 0x04, 0xf6, 0x27, 0x25, 0xf6, 0x04, 0x00,
+			0x02, 0x01, 0xf9, 0x2d, 0x1d, 0xf5, 0x06, 0xff,
+			0x04, 0xff, 0xfd, 0x31, 0x15, 0xf5, 0x07, 0xfe,
+			0x05, 0xfc, 0x02, 0x35, 0x0d, 0xf7, 0x07, 0xfd
+		}
+	},
+	{
+		.min = 1433,
+		.max = 1536,
+		.coef = {
+			0xfe, 0x06, 0xf8, 0x0b, 0x30, 0x0b, 0xf8, 0x06,
+			0xff, 0x06, 0xf7, 0x12, 0x2d, 0x05, 0xfa, 0x06,
+			0x00, 0x04, 0xf6, 0x18, 0x2c, 0x00, 0xfc, 0x06,
+			0x01, 0x02, 0xf7, 0x1f, 0x27, 0xfd, 0xff, 0x04,
+			0x03, 0x00, 0xf9, 0x24, 0x24, 0xf9, 0x00, 0x03,
+			0x04, 0xff, 0xfd, 0x29, 0x1d, 0xf7, 0x02, 0x01,
+			0x06, 0xfc, 0x00, 0x2d, 0x17, 0xf6, 0x04, 0x00,
+			0x06, 0xfa, 0x05, 0x30, 0x0f, 0xf7, 0x06, 0xff
+		}
+	},
+	{
+		.min = 1536,
+		.max = 2048,
+		.coef = {
+			0x05, 0xfd, 0xfb, 0x13, 0x25, 0x13, 0xfb, 0xfd,
+			0x05, 0xfc, 0xfd, 0x17, 0x24, 0x0f, 0xf9, 0xff,
+			0x04, 0xfa, 0xff, 0x1b, 0x24, 0x0b, 0xf9, 0x00,
+			0x03, 0xf9, 0x01, 0x1f, 0x23, 0x08, 0xf8, 0x01,
+			0x02, 0xf9, 0x04, 0x22, 0x20, 0x04, 0xf9, 0x02,
+			0x01, 0xf8, 0x08, 0x25, 0x1d, 0x01, 0xf9, 0x03,
+			0x00, 0xf9, 0x0c, 0x25, 0x1a, 0xfe, 0xfa, 0x04,
+			0xff, 0xf9, 0x10, 0x26, 0x15, 0xfc, 0xfc, 0x05
+		}
+	},
+	{
+		.min = 2048,
+		.max = 3072,
+		.coef = {
+			0xfc, 0xfd, 0x06, 0x13, 0x18, 0x13, 0x06, 0xfd,
+			0xfc, 0xfe, 0x08, 0x15, 0x17, 0x12, 0x04, 0xfc,
+			0xfb, 0xfe, 0x0a, 0x16, 0x18, 0x10, 0x03, 0xfc,
+			0xfb, 0x00, 0x0b, 0x18, 0x17, 0x0f, 0x01, 0xfb,
+			0xfb, 0x00, 0x0d, 0x19, 0x17, 0x0d, 0x00, 0xfb,
+			0xfb, 0x01, 0x0f, 0x19, 0x16, 0x0b, 0x00, 0xfb,
+			0xfc, 0x03, 0x11, 0x19, 0x15, 0x09, 0xfe, 0xfb,
+			0xfc, 0x04, 0x12, 0x1a, 0x12, 0x08, 0xfe, 0xfc
+		}
+	},
+	{
+		.min = 3072,
+		.max = 4096,
+		.coef = {
+			0xfe, 0x02, 0x09, 0x0f, 0x0e, 0x0f, 0x09, 0x02,
+			0xff, 0x02, 0x09, 0x0f, 0x10, 0x0e, 0x08, 0x01,
+			0xff, 0x03, 0x0a, 0x10, 0x10, 0x0d, 0x07, 0x00,
+			0x00, 0x04, 0x0b, 0x10, 0x0f, 0x0c, 0x06, 0x00,
+			0x00, 0x05, 0x0c, 0x10, 0x0e, 0x0c, 0x05, 0x00,
+			0x00, 0x06, 0x0c, 0x11, 0x0e, 0x0b, 0x04, 0x00,
+			0x00, 0x07, 0x0d, 0x11, 0x0f, 0x0a, 0x03, 0xff,
+			0x01, 0x08, 0x0e, 0x11, 0x0e, 0x09, 0x02, 0xff
+		}
+	},
+	{
+		.min = 4096,
+		.max = 5120,
+		.coef = {
+			0x00, 0x04, 0x09, 0x0c, 0x0e, 0x0c, 0x09, 0x04,
+			0x01, 0x05, 0x09, 0x0c, 0x0d, 0x0c, 0x08, 0x04,
+			0x01, 0x05, 0x0a, 0x0c, 0x0e, 0x0b, 0x08, 0x03,
+			0x02, 0x06, 0x0a, 0x0d, 0x0c, 0x0b, 0x07, 0x03,
+			0x02, 0x07, 0x0a, 0x0d, 0x0d, 0x0a, 0x07, 0x02,
+			0x03, 0x07, 0x0b, 0x0d, 0x0c, 0x0a, 0x06, 0x02,
+			0x03, 0x08, 0x0b, 0x0d, 0x0d, 0x0a, 0x05, 0x01,
+			0x04, 0x08, 0x0c, 0x0d, 0x0c, 0x09, 0x05, 0x01
+		}
+	},
+	{
+		.min = 5120,
+		.max = 65535,
+		.coef = {
+			0x03, 0x06, 0x09, 0x0b, 0x09, 0x0b, 0x09, 0x06,
+			0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+			0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+			0x04, 0x07, 0x09, 0x0b, 0x0b, 0x0a, 0x08, 0x04,
+			0x04, 0x07, 0x0a, 0x0b, 0x0b, 0x0a, 0x07, 0x04,
+			0x04, 0x08, 0x0a, 0x0b, 0x0b, 0x09, 0x07, 0x04,
+			0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03,
+			0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03
+		}
+	}
+};
+
+/**
+ * struct bdisp_filter_v_spec - Vertical filter specification
+ *
+ * @min:	min scale factor for this filter (6.10 fixed point)
+ * @max:	max scale factor for this filter (6.10 fixed point)
+ * coef:	filter coefficients
+ */
+struct bdisp_filter_v_spec {
+	const u16 min;
+	const u16 max;
+	const u8 coef[BDISP_VF_NB];
+};
+
+static const struct bdisp_filter_v_spec bdisp_v_spec[] = {
+	{
+		.min = 0,
+		.max = 1024,
+		.coef = {
+			0x00, 0x00, 0x40, 0x00, 0x00,
+			0x00, 0x06, 0x3d, 0xfd, 0x00,
+			0xfe, 0x0f, 0x38, 0xfb, 0x00,
+			0xfd, 0x19, 0x2f, 0xfb, 0x00,
+			0xfc, 0x24, 0x24, 0xfc, 0x00,
+			0xfb, 0x2f, 0x19, 0xfd, 0x00,
+			0xfb, 0x38, 0x0f, 0xfe, 0x00,
+			0xfd, 0x3d, 0x06, 0x00, 0x00
+		}
+	},
+	{
+		.min = 1024,
+		.max = 1331,
+		.coef = {
+			0xfc, 0x05, 0x3e, 0x05, 0xfc,
+			0xf8, 0x0e, 0x3b, 0xff, 0x00,
+			0xf5, 0x18, 0x38, 0xf9, 0x02,
+			0xf4, 0x21, 0x31, 0xf5, 0x05,
+			0xf4, 0x2a, 0x27, 0xf4, 0x07,
+			0xf6, 0x30, 0x1e, 0xf4, 0x08,
+			0xf9, 0x35, 0x15, 0xf6, 0x07,
+			0xff, 0x37, 0x0b, 0xf9, 0x06
+		}
+	},
+	{
+		.min = 1331,
+		.max = 1433,
+		.coef = {
+			0xf8, 0x0a, 0x3c, 0x0a, 0xf8,
+			0xf6, 0x12, 0x3b, 0x02, 0xfb,
+			0xf4, 0x1b, 0x35, 0xfd, 0xff,
+			0xf4, 0x23, 0x30, 0xf8, 0x01,
+			0xf6, 0x29, 0x27, 0xf6, 0x04,
+			0xf9, 0x2e, 0x1e, 0xf5, 0x06,
+			0xfd, 0x31, 0x16, 0xf6, 0x06,
+			0x02, 0x32, 0x0d, 0xf8, 0x07
+		}
+	},
+	{
+		.min = 1433,
+		.max = 1536,
+		.coef = {
+			0xf6, 0x0e, 0x38, 0x0e, 0xf6,
+			0xf5, 0x15, 0x38, 0x06, 0xf8,
+			0xf5, 0x1d, 0x33, 0x00, 0xfb,
+			0xf6, 0x23, 0x2d, 0xfc, 0xfe,
+			0xf9, 0x28, 0x26, 0xf9, 0x00,
+			0xfc, 0x2c, 0x1e, 0xf7, 0x03,
+			0x00, 0x2e, 0x18, 0xf6, 0x04,
+			0x05, 0x2e, 0x11, 0xf7, 0x05
+		}
+	},
+	{
+		.min = 1536,
+		.max = 2048,
+		.coef = {
+			0xfb, 0x13, 0x24, 0x13, 0xfb,
+			0xfd, 0x17, 0x23, 0x0f, 0xfa,
+			0xff, 0x1a, 0x23, 0x0b, 0xf9,
+			0x01, 0x1d, 0x22, 0x07, 0xf9,
+			0x04, 0x20, 0x1f, 0x04, 0xf9,
+			0x07, 0x22, 0x1c, 0x01, 0xfa,
+			0x0b, 0x24, 0x17, 0xff, 0xfb,
+			0x0f, 0x24, 0x14, 0xfd, 0xfc
+		}
+	},
+	{
+		.min = 2048,
+		.max = 3072,
+		.coef = {
+			0x05, 0x10, 0x16, 0x10, 0x05,
+			0x06, 0x11, 0x16, 0x0f, 0x04,
+			0x08, 0x13, 0x15, 0x0e, 0x02,
+			0x09, 0x14, 0x16, 0x0c, 0x01,
+			0x0b, 0x15, 0x15, 0x0b, 0x00,
+			0x0d, 0x16, 0x13, 0x0a, 0x00,
+			0x0f, 0x17, 0x13, 0x08, 0xff,
+			0x11, 0x18, 0x12, 0x07, 0xfe
+		}
+	},
+	{
+		.min = 3072,
+		.max = 4096,
+		.coef = {
+			0x09, 0x0f, 0x10, 0x0f, 0x09,
+			0x09, 0x0f, 0x12, 0x0e, 0x08,
+			0x0a, 0x10, 0x11, 0x0e, 0x07,
+			0x0b, 0x11, 0x11, 0x0d, 0x06,
+			0x0c, 0x11, 0x12, 0x0c, 0x05,
+			0x0d, 0x12, 0x11, 0x0c, 0x04,
+			0x0e, 0x12, 0x11, 0x0b, 0x04,
+			0x0f, 0x13, 0x11, 0x0a, 0x03
+		}
+	},
+	{
+		.min = 4096,
+		.max = 5120,
+		.coef = {
+			0x0a, 0x0e, 0x10, 0x0e, 0x0a,
+			0x0b, 0x0e, 0x0f, 0x0e, 0x0a,
+			0x0b, 0x0f, 0x10, 0x0d, 0x09,
+			0x0c, 0x0f, 0x10, 0x0d, 0x08,
+			0x0d, 0x0f, 0x0f, 0x0d, 0x08,
+			0x0d, 0x10, 0x10, 0x0c, 0x07,
+			0x0e, 0x10, 0x0f, 0x0c, 0x07,
+			0x0f, 0x10, 0x10, 0x0b, 0x06
+		}
+	},
+	{
+		.min = 5120,
+		.max = 65535,
+		.coef = {
+			0x0b, 0x0e, 0x0e, 0x0e, 0x0b,
+			0x0b, 0x0e, 0x0f, 0x0d, 0x0b,
+			0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+			0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+			0x0d, 0x0f, 0x0e, 0x0d, 0x09,
+			0x0d, 0x0f, 0x0f, 0x0c, 0x09,
+			0x0e, 0x0f, 0x0e, 0x0c, 0x09,
+			0x0e, 0x0f, 0x0f, 0x0c, 0x08
+		}
+	}
+};
+
+#define NB_H_FILTER ARRAY_SIZE(bdisp_h_spec)
+#define NB_V_FILTER ARRAY_SIZE(bdisp_v_spec)
+
+/* RGB YUV 601 standard conversion */
+static const u32 bdisp_rgb_to_yuv[] = {
+		0x0e1e8bee, 0x08420419, 0xfb5ed471, 0x08004080,
+};
+
+static const u32 bdisp_yuv_to_rgb[] = {
+		0x3324a800, 0xe604ab9c, 0x0004a957, 0x32121eeb,
+};
diff --git a/drivers/media/platform/sti/bdisp/bdisp-hw.c b/drivers/media/platform/sti/bdisp/bdisp-hw.c
new file mode 100644
index 000000000000..465828e859e2
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-hw.c
@@ -0,0 +1,823 @@
+/*
+ * 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/delay.h>
+
+#include "bdisp.h"
+#include "bdisp-filter.h"
+#include "bdisp-reg.h"
+
+/* Max width of the source frame in a single node */
+#define MAX_SRC_WIDTH           2048
+
+/* Reset & boot poll config */
+#define POLL_RST_MAX            50
+#define POLL_RST_DELAY_MS       20
+
+enum bdisp_target_plan {
+	BDISP_RGB,
+	BDISP_Y,
+	BDISP_CBCR
+};
+
+struct bdisp_op_cfg {
+	bool cconv;          /* RGB - YUV conversion */
+	bool hflip;          /* Horizontal flip */
+	bool vflip;          /* Vertical flip */
+	bool wide;           /* Wide (>MAX_SRC_WIDTH) */
+	bool scale;          /* Scale */
+	u16  h_inc;          /* Horizontal increment in 6.10 format */
+	u16  v_inc;          /* Vertical increment in 6.10 format */
+	bool src_interlaced; /* is the src an interlaced buffer */
+	u8   src_nbp;        /* nb of planes of the src */
+	bool src_yuv;        /* is the src a YUV color format */
+	bool src_420;        /* is the src 4:2:0 chroma subsampled */
+	u8   dst_nbp;        /* nb of planes of the dst */
+	bool dst_yuv;        /* is the dst a YUV color format */
+	bool dst_420;        /* is the dst 4:2:0 chroma subsampled */
+};
+
+struct bdisp_filter_addr {
+	u16 min;             /* Filter min scale factor (6.10 fixed point) */
+	u16 max;             /* Filter max scale factor (6.10 fixed point) */
+	void *virt;          /* Virtual address for filter table */
+	dma_addr_t paddr;    /* Physical address for filter table */
+};
+
+static struct bdisp_filter_addr bdisp_h_filter[NB_H_FILTER];
+static struct bdisp_filter_addr bdisp_v_filter[NB_V_FILTER];
+
+/**
+ * bdisp_hw_reset
+ * @bdisp:      bdisp entity
+ *
+ * Resets HW
+ *
+ * RETURNS:
+ * 0 on success.
+ */
+int bdisp_hw_reset(struct bdisp_dev *bdisp)
+{
+	unsigned int i;
+
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+
+	/* Mask Interrupt */
+	writel(0, bdisp->regs + BLT_ITM0);
+
+	/* Reset */
+	writel(readl(bdisp->regs + BLT_CTL) | BLT_CTL_RESET,
+	       bdisp->regs + BLT_CTL);
+	writel(0, bdisp->regs + BLT_CTL);
+
+	/* Wait for reset done */
+	for (i = 0; i < POLL_RST_MAX; i++) {
+		if (readl(bdisp->regs + BLT_STA1) & BLT_STA1_IDLE)
+			break;
+		msleep(POLL_RST_DELAY_MS);
+	}
+	if (i == POLL_RST_MAX)
+		dev_err(bdisp->dev, "Reset timeout\n");
+
+	return (i == POLL_RST_MAX) ? -EAGAIN : 0;
+}
+
+/**
+ * bdisp_hw_get_and_clear_irq
+ * @bdisp:      bdisp entity
+ *
+ * Read then reset interrupt status
+ *
+ * RETURNS:
+ * 0 if expected interrupt was raised.
+ */
+int bdisp_hw_get_and_clear_irq(struct bdisp_dev *bdisp)
+{
+	u32 its;
+
+	its = readl(bdisp->regs + BLT_ITS);
+
+	/* Check for the only expected IT: LastNode of AQ1 */
+	if (!(its & BLT_ITS_AQ1_LNA)) {
+		dev_dbg(bdisp->dev, "Unexpected IT status: 0x%08X\n", its);
+		writel(its, bdisp->regs + BLT_ITS);
+		return -1;
+	}
+
+	/* Clear and mask */
+	writel(its, bdisp->regs + BLT_ITS);
+	writel(0, bdisp->regs + BLT_ITM0);
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_free_nodes
+ * @ctx:        bdisp context
+ *
+ * Free node memory
+ *
+ * RETURNS:
+ * None
+ */
+void bdisp_hw_free_nodes(struct bdisp_ctx *ctx)
+{
+	if (ctx && ctx->node[0]) {
+		DEFINE_DMA_ATTRS(attrs);
+
+		dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+		dma_free_attrs(ctx->bdisp_dev->dev,
+			       sizeof(struct bdisp_node) * MAX_NB_NODE,
+			       ctx->node[0], ctx->node_paddr[0], &attrs);
+	}
+}
+
+/**
+ * bdisp_hw_alloc_nodes
+ * @ctx:        bdisp context
+ *
+ * Allocate dma memory for nodes
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_alloc_nodes(struct bdisp_ctx *ctx)
+{
+	struct device *dev = ctx->bdisp_dev->dev;
+	unsigned int i, node_size = sizeof(struct bdisp_node);
+	void *base;
+	dma_addr_t paddr;
+	DEFINE_DMA_ATTRS(attrs);
+
+	/* Allocate all the nodes within a single memory page */
+	dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+	base = dma_alloc_attrs(dev, node_size * MAX_NB_NODE, &paddr,
+			       GFP_KERNEL | GFP_DMA, &attrs);
+	if (!base) {
+		dev_err(dev, "%s no mem\n", __func__);
+		return -ENOMEM;
+	}
+
+	memset(base, 0, node_size * MAX_NB_NODE);
+
+	for (i = 0; i < MAX_NB_NODE; i++) {
+		ctx->node[i] = base;
+		ctx->node_paddr[i] = paddr;
+		dev_dbg(dev, "node[%d]=0x%p (paddr=%pad)\n", i, ctx->node[i],
+			&paddr);
+		base += node_size;
+		paddr += node_size;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_free_filters
+ * @dev:        device
+ *
+ * Free filters memory
+ *
+ * RETURNS:
+ * None
+ */
+void bdisp_hw_free_filters(struct device *dev)
+{
+	int size = (BDISP_HF_NB * NB_H_FILTER) + (BDISP_VF_NB * NB_V_FILTER);
+
+	if (bdisp_h_filter[0].virt) {
+		DEFINE_DMA_ATTRS(attrs);
+
+		dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+		dma_free_attrs(dev, size, bdisp_h_filter[0].virt,
+			       bdisp_h_filter[0].paddr, &attrs);
+	}
+}
+
+/**
+ * bdisp_hw_alloc_filters
+ * @dev:        device
+ *
+ * Allocate dma memory for filters
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_alloc_filters(struct device *dev)
+{
+	unsigned int i, size;
+	void *base;
+	dma_addr_t paddr;
+	DEFINE_DMA_ATTRS(attrs);
+
+	/* Allocate all the filters within a single memory page */
+	size = (BDISP_HF_NB * NB_H_FILTER) + (BDISP_VF_NB * NB_V_FILTER);
+	dma_set_attr(DMA_ATTR_WRITE_COMBINE, &attrs);
+	base = dma_alloc_attrs(dev, size, &paddr, GFP_KERNEL | GFP_DMA, &attrs);
+	if (!base)
+		return -ENOMEM;
+
+	/* Setup filter addresses */
+	for (i = 0; i < NB_H_FILTER; i++) {
+		bdisp_h_filter[i].min = bdisp_h_spec[i].min;
+		bdisp_h_filter[i].max = bdisp_h_spec[i].max;
+		memcpy(base, bdisp_h_spec[i].coef, BDISP_HF_NB);
+		bdisp_h_filter[i].virt = base;
+		bdisp_h_filter[i].paddr = paddr;
+		base += BDISP_HF_NB;
+		paddr += BDISP_HF_NB;
+	}
+
+	for (i = 0; i < NB_V_FILTER; i++) {
+		bdisp_v_filter[i].min = bdisp_v_spec[i].min;
+		bdisp_v_filter[i].max = bdisp_v_spec[i].max;
+		memcpy(base, bdisp_v_spec[i].coef, BDISP_VF_NB);
+		bdisp_v_filter[i].virt = base;
+		bdisp_v_filter[i].paddr = paddr;
+		base += BDISP_VF_NB;
+		paddr += BDISP_VF_NB;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_hf_addr
+ * @inc:        resize increment
+ *
+ * Find the horizontal filter table that fits the resize increment
+ *
+ * RETURNS:
+ * table physical address
+ */
+static dma_addr_t bdisp_hw_get_hf_addr(u16 inc)
+{
+	unsigned int i;
+
+	for (i = NB_H_FILTER - 1; i > 0; i--)
+		if ((bdisp_h_filter[i].min < inc) &&
+		    (inc <= bdisp_h_filter[i].max))
+			break;
+
+	return bdisp_h_filter[i].paddr;
+}
+
+/**
+ * bdisp_hw_get_vf_addr
+ * @inc:        resize increment
+ *
+ * Find the vertical filter table that fits the resize increment
+ *
+ * RETURNS:
+ * table physical address
+ */
+static dma_addr_t bdisp_hw_get_vf_addr(u16 inc)
+{
+	unsigned int i;
+
+	for (i = NB_V_FILTER - 1; i > 0; i--)
+		if ((bdisp_v_filter[i].min < inc) &&
+		    (inc <= bdisp_v_filter[i].max))
+			break;
+
+	return bdisp_v_filter[i].paddr;
+}
+
+/**
+ * bdisp_hw_get_inc
+ * @from:       input size
+ * @to:         output size
+ * @inc:        resize increment in 6.10 format
+ *
+ * Computes the increment (inverse of scale) in 6.10 format
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_inc(u32 from, u32 to, u16 *inc)
+{
+	u32 tmp;
+
+	if (!to)
+		return -EINVAL;
+
+	if (to == from) {
+		*inc = 1 << 10;
+		return 0;
+	}
+
+	tmp = (from << 10) / to;
+	if ((tmp > 0xFFFF) || (!tmp))
+		/* overflow (downscale x 63) or too small (upscale x 1024) */
+		return -EINVAL;
+
+	*inc = (u16)tmp;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_hv_inc
+ * @ctx:        device context
+ * @h_inc:      horizontal increment
+ * @v_inc:      vertical increment
+ *
+ * Computes the horizontal & vertical increments (inverse of scale)
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_hv_inc(struct bdisp_ctx *ctx, u16 *h_inc, u16 *v_inc)
+{
+	u32 src_w, src_h, dst_w, dst_h;
+
+	src_w = ctx->src.crop.width;
+	src_h = ctx->src.crop.height;
+	dst_w = ctx->dst.width;
+	dst_h = ctx->dst.height;
+
+	if (bdisp_hw_get_inc(src_w, dst_w, h_inc) ||
+	    bdisp_hw_get_inc(src_h, dst_h, v_inc)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"scale factors failed (%dx%d)->(%dx%d)\n",
+			src_w, src_h, dst_w, dst_h);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_get_op_cfg
+ * @ctx:        device context
+ * @c:          operation configuration
+ *
+ * Check which blitter operations are expected and sets the scaling increments
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_get_op_cfg(struct bdisp_ctx *ctx, struct bdisp_op_cfg *c)
+{
+	struct device *dev = ctx->bdisp_dev->dev;
+	struct bdisp_frame *src = &ctx->src;
+	struct bdisp_frame *dst = &ctx->dst;
+
+	if (src->width > MAX_SRC_WIDTH * MAX_VERTICAL_STRIDES) {
+		dev_err(dev, "Image width out of HW caps\n");
+		return -EINVAL;
+	}
+
+	c->wide = src->width > MAX_SRC_WIDTH;
+
+	c->hflip = ctx->hflip;
+	c->vflip = ctx->vflip;
+
+	c->src_interlaced = (src->field == V4L2_FIELD_INTERLACED);
+
+	c->src_nbp = src->fmt->nb_planes;
+	c->src_yuv = (src->fmt->pixelformat == V4L2_PIX_FMT_NV12) ||
+			(src->fmt->pixelformat == V4L2_PIX_FMT_YUV420);
+	c->src_420 = c->src_yuv;
+
+	c->dst_nbp = dst->fmt->nb_planes;
+	c->dst_yuv = (dst->fmt->pixelformat == V4L2_PIX_FMT_NV12) ||
+			(dst->fmt->pixelformat == V4L2_PIX_FMT_YUV420);
+	c->dst_420 = c->dst_yuv;
+
+	c->cconv = (c->src_yuv != c->dst_yuv);
+
+	if (bdisp_hw_get_hv_inc(ctx, &c->h_inc, &c->v_inc)) {
+		dev_err(dev, "Scale factor out of HW caps\n");
+		return -EINVAL;
+	}
+
+	/* Deinterlacing adjustment : stretch a field to a frame */
+	if (c->src_interlaced)
+		c->v_inc /= 2;
+
+	if ((c->h_inc != (1 << 10)) || (c->v_inc != (1 << 10)))
+		c->scale = true;
+	else
+		c->scale = false;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_color_format
+ * @pixelformat: v4l2 pixel format
+ *
+ * v4l2 to bdisp pixel format convert
+ *
+ * RETURNS:
+ * bdisp pixel format
+ */
+static u32 bdisp_hw_color_format(u32 pixelformat)
+{
+	u32 ret;
+
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+		ret = (BDISP_YUV_3B << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_NV12:
+		ret = (BDISP_NV12 << BLT_TTY_COL_SHIFT) | BLT_TTY_BIG_END;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		ret = (BDISP_RGB565 << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_XBGR32: /* This V4L format actually refers to xRGB */
+		ret = (BDISP_XRGB8888 << BLT_TTY_COL_SHIFT);
+		break;
+	case V4L2_PIX_FMT_RGB24:  /* RGB888 format */
+		ret = (BDISP_RGB888 << BLT_TTY_COL_SHIFT) | BLT_TTY_BIG_END;
+		break;
+	case V4L2_PIX_FMT_ABGR32: /* This V4L format actually refers to ARGB */
+
+	default:
+		ret = (BDISP_ARGB8888 << BLT_TTY_COL_SHIFT) | BLT_TTY_ALPHA_R;
+		break;
+	}
+
+	return ret;
+}
+
+/**
+ * bdisp_hw_build_node
+ * @ctx:        device context
+ * @cfg:        operation configuration
+ * @node:       node to be set
+ * @t_plan:     whether the node refers to a RGB/Y or a CbCr plane
+ * @src_x_offset: x offset in the source image
+ *
+ * Build a node
+ *
+ * RETURNS:
+ * None
+ */
+static void bdisp_hw_build_node(struct bdisp_ctx *ctx,
+				struct bdisp_op_cfg *cfg,
+				struct bdisp_node *node,
+				enum bdisp_target_plan t_plan, int src_x_offset)
+{
+	struct bdisp_frame *src = &ctx->src;
+	struct bdisp_frame *dst = &ctx->dst;
+	u16 h_inc, v_inc, yh_inc, yv_inc;
+	struct v4l2_rect src_rect = src->crop;
+	struct v4l2_rect dst_rect = dst->crop;
+	int dst_x_offset;
+	s32 dst_width = dst->crop.width;
+	u32 src_fmt, dst_fmt;
+	const u32 *ivmx;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	memset(node, 0, sizeof(*node));
+
+	/* Adjust src and dst areas wrt src_x_offset */
+	src_rect.left += src_x_offset;
+	src_rect.width -= src_x_offset;
+	src_rect.width = min_t(__s32, MAX_SRC_WIDTH, src_rect.width);
+
+	dst_x_offset = (src_x_offset * dst->width) / ctx->src.crop.width;
+	dst_rect.left += dst_x_offset;
+	dst_rect.width = (src_rect.width * dst->width) / ctx->src.crop.width;
+
+	/* General */
+	src_fmt = src->fmt->pixelformat;
+	dst_fmt = dst->fmt->pixelformat;
+
+	node->nip = 0;
+	node->cic = BLT_CIC_ALL_GRP;
+	node->ack = BLT_ACK_BYPASS_S2S3;
+
+	switch (cfg->src_nbp) {
+	case 1:
+		/* Src2 = RGB / Src1 = Src3 = off */
+		node->ins = BLT_INS_S1_OFF | BLT_INS_S2_MEM | BLT_INS_S3_OFF;
+		break;
+	case 2:
+		/* Src3 = Y
+		 * Src2 = CbCr or ColorFill if writing the Y plane
+		 * Src1 = off */
+		node->ins = BLT_INS_S1_OFF | BLT_INS_S3_MEM;
+		if (t_plan == BDISP_Y)
+			node->ins |= BLT_INS_S2_CF;
+		else
+			node->ins |= BLT_INS_S2_MEM;
+		break;
+	case 3:
+	default:
+		/* Src3 = Y
+		 * Src2 = Cb or ColorFill if writing the Y plane
+		 * Src1 = Cr or ColorFill if writing the Y plane */
+		node->ins = BLT_INS_S3_MEM;
+		if (t_plan == BDISP_Y)
+			node->ins |= BLT_INS_S2_CF | BLT_INS_S1_CF;
+		else
+			node->ins |= BLT_INS_S2_MEM | BLT_INS_S1_MEM;
+		break;
+	}
+
+	/* Color convert */
+	node->ins |= cfg->cconv ? BLT_INS_IVMX : 0;
+	/* Scale needed if scaling OR 4:2:0 up/downsampling */
+	node->ins |= (cfg->scale || cfg->src_420 || cfg->dst_420) ?
+			BLT_INS_SCALE : 0;
+
+	/* Target */
+	node->tba = (t_plan == BDISP_CBCR) ? dst->paddr[1] : dst->paddr[0];
+
+	node->tty = dst->bytesperline;
+	node->tty |= bdisp_hw_color_format(dst_fmt);
+	node->tty |= BLT_TTY_DITHER;
+	node->tty |= (t_plan == BDISP_CBCR) ? BLT_TTY_CHROMA : 0;
+	node->tty |= cfg->hflip ? BLT_TTY_HSO : 0;
+	node->tty |= cfg->vflip ? BLT_TTY_VSO : 0;
+
+	if (cfg->dst_420 && (t_plan == BDISP_CBCR)) {
+		/* 420 chroma downsampling */
+		dst_rect.height /= 2;
+		dst_rect.width /= 2;
+		dst_rect.left /= 2;
+		dst_rect.top /= 2;
+		dst_x_offset /= 2;
+		dst_width /= 2;
+	}
+
+	node->txy = cfg->vflip ? (dst_rect.height - 1) : dst_rect.top;
+	node->txy <<= 16;
+	node->txy |= cfg->hflip ? (dst_width - dst_x_offset - 1) :
+			dst_rect.left;
+
+	node->tsz = dst_rect.height << 16 | dst_rect.width;
+
+	if (cfg->src_interlaced) {
+		/* handle only the top field which is half height of a frame */
+		src_rect.top /= 2;
+		src_rect.height /= 2;
+	}
+
+	if (cfg->src_nbp == 1) {
+		/* Src 2 : RGB */
+		node->s2ba = src->paddr[0];
+
+		node->s2ty = src->bytesperline;
+		if (cfg->src_interlaced)
+			node->s2ty *= 2;
+
+		node->s2ty |= bdisp_hw_color_format(src_fmt);
+
+		node->s2xy = src_rect.top << 16 | src_rect.left;
+		node->s2sz = src_rect.height << 16 | src_rect.width;
+	} else {
+		/* Src 2 : Cb or CbCr */
+		if (cfg->src_420) {
+			/* 420 chroma upsampling */
+			src_rect.top /= 2;
+			src_rect.left /= 2;
+			src_rect.width /= 2;
+			src_rect.height /= 2;
+		}
+
+		node->s2ba = src->paddr[1];
+
+		node->s2ty = src->bytesperline;
+		if (cfg->src_nbp == 3)
+			node->s2ty /= 2;
+		if (cfg->src_interlaced)
+			node->s2ty *= 2;
+
+		node->s2ty |= bdisp_hw_color_format(src_fmt);
+
+		node->s2xy = src_rect.top << 16 | src_rect.left;
+		node->s2sz = src_rect.height << 16 | src_rect.width;
+
+		if (cfg->src_nbp == 3) {
+			/* Src 1 : Cr */
+			node->s1ba = src->paddr[2];
+
+			node->s1ty = node->s2ty;
+			node->s1xy = node->s2xy;
+		}
+
+		/* Src 3 : Y */
+		node->s3ba = src->paddr[0];
+
+		node->s3ty = src->bytesperline;
+		if (cfg->src_interlaced)
+			node->s3ty *= 2;
+		node->s3ty |= bdisp_hw_color_format(src_fmt);
+
+		if ((t_plan != BDISP_CBCR) && cfg->src_420) {
+			/* No chroma upsampling for output RGB / Y plane */
+			node->s3xy = node->s2xy * 2;
+			node->s3sz = node->s2sz * 2;
+		} else {
+			/* No need to read Y (Src3) when writing Chroma */
+			node->s3ty |= BLT_S3TY_BLANK_ACC;
+			node->s3xy = node->s2xy;
+			node->s3sz = node->s2sz;
+		}
+	}
+
+	/* Resize (scale OR 4:2:0: chroma up/downsampling) */
+	if (node->ins & BLT_INS_SCALE) {
+		/* no need to compute Y when writing CbCr from RGB input */
+		bool skip_y = (t_plan == BDISP_CBCR) && !cfg->src_yuv;
+
+		/* FCTL */
+		if (cfg->scale) {
+			node->fctl = BLT_FCTL_HV_SCALE;
+			if (!skip_y)
+				node->fctl |= BLT_FCTL_Y_HV_SCALE;
+		} else {
+			node->fctl = BLT_FCTL_HV_SAMPLE;
+			if (!skip_y)
+				node->fctl |= BLT_FCTL_Y_HV_SAMPLE;
+		}
+
+		/* RSF - Chroma may need to be up/downsampled */
+		h_inc = cfg->h_inc;
+		v_inc = cfg->v_inc;
+		if (!cfg->src_420 && cfg->dst_420 && (t_plan == BDISP_CBCR)) {
+			/* RGB to 4:2:0 for Chroma: downsample */
+			h_inc *= 2;
+			v_inc *= 2;
+		} else if (cfg->src_420 && !cfg->dst_420) {
+			/* 4:2:0: to RGB: upsample*/
+			h_inc /= 2;
+			v_inc /= 2;
+		}
+		node->rsf = v_inc << 16 | h_inc;
+
+		/* RZI */
+		node->rzi = BLT_RZI_DEFAULT;
+
+		/* Filter table physical addr */
+		node->hfp = bdisp_hw_get_hf_addr(h_inc);
+		node->vfp = bdisp_hw_get_vf_addr(v_inc);
+
+		/* Y version */
+		if (!skip_y) {
+			yh_inc = cfg->h_inc;
+			yv_inc = cfg->v_inc;
+
+			node->y_rsf = yv_inc << 16 | yh_inc;
+			node->y_rzi = BLT_RZI_DEFAULT;
+			node->y_hfp = bdisp_hw_get_hf_addr(yh_inc);
+			node->y_vfp = bdisp_hw_get_vf_addr(yv_inc);
+		}
+	}
+
+	/* Versatile matrix for RGB / YUV conversion */
+	if (cfg->cconv) {
+		ivmx = cfg->src_yuv ? bdisp_yuv_to_rgb : bdisp_rgb_to_yuv;
+
+		node->ivmx0 = ivmx[0];
+		node->ivmx1 = ivmx[1];
+		node->ivmx2 = ivmx[2];
+		node->ivmx3 = ivmx[3];
+	}
+}
+
+/**
+ * bdisp_hw_build_all_nodes
+ * @ctx:        device context
+ *
+ * Build all the nodes for the blitter operation
+ *
+ * RETURNS:
+ * 0 on success
+ */
+static int bdisp_hw_build_all_nodes(struct bdisp_ctx *ctx)
+{
+	struct bdisp_op_cfg cfg;
+	unsigned int i, nid = 0;
+	int src_x_offset = 0;
+
+	for (i = 0; i < MAX_NB_NODE; i++)
+		if (!ctx->node[i]) {
+			dev_err(ctx->bdisp_dev->dev, "node %d is null\n", i);
+			return -EINVAL;
+		}
+
+	/* Get configuration (scale, flip, ...) */
+	if (bdisp_hw_get_op_cfg(ctx, &cfg))
+		return -EINVAL;
+
+	/* Split source in vertical strides (HW constraint) */
+	for (i = 0; i < MAX_VERTICAL_STRIDES; i++) {
+		/* Build RGB/Y node and link it to the previous node */
+		bdisp_hw_build_node(ctx, &cfg, ctx->node[nid],
+				    cfg.dst_nbp == 1 ? BDISP_RGB : BDISP_Y,
+				    src_x_offset);
+		if (nid)
+			ctx->node[nid - 1]->nip = ctx->node_paddr[nid];
+		nid++;
+
+		/* Build additional Cb(Cr) node, link it to the previous one */
+		if (cfg.dst_nbp > 1) {
+			bdisp_hw_build_node(ctx, &cfg, ctx->node[nid],
+					    BDISP_CBCR, src_x_offset);
+			ctx->node[nid - 1]->nip = ctx->node_paddr[nid];
+			nid++;
+		}
+
+		/* Next stride until full width covered */
+		src_x_offset += MAX_SRC_WIDTH;
+		if (src_x_offset >= ctx->src.crop.width)
+			break;
+	}
+
+	/* Mark last node as the last */
+	ctx->node[nid - 1]->nip = 0;
+
+	return 0;
+}
+
+/**
+ * bdisp_hw_save_request
+ * @ctx:        device context
+ *
+ * Save a copy of the request and of the built nodes
+ *
+ * RETURNS:
+ * None
+ */
+static void bdisp_hw_save_request(struct bdisp_ctx *ctx)
+{
+	struct bdisp_node **copy_node = ctx->bdisp_dev->dbg.copy_node;
+	struct bdisp_request *request = &ctx->bdisp_dev->dbg.copy_request;
+	struct bdisp_node **node = ctx->node;
+	int i;
+
+	/* Request copy */
+	request->src = ctx->src;
+	request->dst = ctx->dst;
+	request->hflip = ctx->hflip;
+	request->vflip = ctx->vflip;
+	request->nb_req++;
+
+	/* Nodes copy */
+	for (i = 0; i < MAX_NB_NODE; i++) {
+		/* Allocate memory if not done yet */
+		if (!copy_node[i]) {
+			copy_node[i] = devm_kzalloc(ctx->bdisp_dev->dev,
+						    sizeof(*copy_node),
+						    GFP_KERNEL);
+			if (!copy_node[i])
+				return;
+		}
+		copy_node[i] = node[i];
+	}
+}
+
+/**
+ * bdisp_hw_update
+ * @ctx:        device context
+ *
+ * Send the request to the HW
+ *
+ * RETURNS:
+ * 0 on success
+ */
+int bdisp_hw_update(struct bdisp_ctx *ctx)
+{
+	int ret;
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+	struct device *dev = bdisp->dev;
+	unsigned int node_id;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	/* build nodes */
+	ret = bdisp_hw_build_all_nodes(ctx);
+	if (ret) {
+		dev_err(dev, "cannot build nodes (%d)\n", ret);
+		return ret;
+	}
+
+	/* Save a copy of the request */
+	bdisp_hw_save_request(ctx);
+
+	/* Configure interrupt to 'Last Node Reached for AQ1' */
+	writel(BLT_AQ1_CTL_CFG, bdisp->regs + BLT_AQ1_CTL);
+	writel(BLT_ITS_AQ1_LNA, bdisp->regs + BLT_ITM0);
+
+	/* Write first node addr */
+	writel(ctx->node_paddr[0], bdisp->regs + BLT_AQ1_IP);
+
+	/* Find and write last node addr : this starts the HW processing */
+	for (node_id = 0; node_id < MAX_NB_NODE - 1; node_id++) {
+		if (!ctx->node[node_id]->nip)
+			break;
+	}
+	writel(ctx->node_paddr[node_id], bdisp->regs + BLT_AQ1_LNA);
+
+	return 0;
+}
diff --git a/drivers/media/platform/sti/bdisp/bdisp-reg.h b/drivers/media/platform/sti/bdisp/bdisp-reg.h
new file mode 100644
index 000000000000..e7e1a425f65a
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-reg.h
@@ -0,0 +1,235 @@
+/*
+ * Copyright (C) STMicroelectronics SA 2014
+ * Authors: Fabien Dessenne <fabien.dessenne@st.com> for STMicroelectronics.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+struct bdisp_node {
+	/* 0 - General */
+	u32 nip;
+	u32 cic;
+	u32 ins;
+	u32 ack;
+	/* 1 - Target */
+	u32 tba;
+	u32 tty;
+	u32 txy;
+	u32 tsz;
+	/* 2 - Color Fill */
+	u32 s1cf;
+	u32 s2cf;
+	/* 3 - Source 1 */
+	u32 s1ba;
+	u32 s1ty;
+	u32 s1xy;
+	u32 s1sz_tsz;
+	/* 4 - Source 2 */
+	u32 s2ba;
+	u32 s2ty;
+	u32 s2xy;
+	u32 s2sz;
+	/* 5 - Source 3 */
+	u32 s3ba;
+	u32 s3ty;
+	u32 s3xy;
+	u32 s3sz;
+	/* 6 - Clipping */
+	u32 cwo;
+	u32 cws;
+	/* 7 - CLUT */
+	u32 cco;
+	u32 cml;
+	/* 8 - Filter & Mask */
+	u32 fctl;
+	u32 pmk;
+	/* 9 - Chroma Filter */
+	u32 rsf;
+	u32 rzi;
+	u32 hfp;
+	u32 vfp;
+	/* 10 - Luma Filter */
+	u32 y_rsf;
+	u32 y_rzi;
+	u32 y_hfp;
+	u32 y_vfp;
+	/* 11 - Flicker */
+	u32 ff0;
+	u32 ff1;
+	u32 ff2;
+	u32 ff3;
+	/* 12 - Color Key */
+	u32 key1;
+	u32 key2;
+	/* 14 - Static Address & User */
+	u32 sar;
+	u32 usr;
+	/* 15 - Input Versatile Matrix */
+	u32 ivmx0;
+	u32 ivmx1;
+	u32 ivmx2;
+	u32 ivmx3;
+	/* 16 - Output Versatile Matrix */
+	u32 ovmx0;
+	u32 ovmx1;
+	u32 ovmx2;
+	u32 ovmx3;
+	/* 17 - Pace */
+	u32 pace;
+	/* 18 - VC1R & DEI */
+	u32 vc1r;
+	u32 dei;
+	/* 19 - Gradient Fill */
+	u32 hgf;
+	u32 vgf;
+};
+
+/* HW registers : static */
+#define BLT_CTL                 0x0A00
+#define BLT_ITS                 0x0A04
+#define BLT_STA1                0x0A08
+#define BLT_AQ1_CTL             0x0A60
+#define BLT_AQ1_IP              0x0A64
+#define BLT_AQ1_LNA             0x0A68
+#define BLT_AQ1_STA             0x0A6C
+#define BLT_ITM0                0x0AD0
+/* HW registers : plugs */
+#define BLT_PLUGS1_OP2          0x0B04
+#define BLT_PLUGS1_CHZ          0x0B08
+#define BLT_PLUGS1_MSZ          0x0B0C
+#define BLT_PLUGS1_PGZ          0x0B10
+#define BLT_PLUGS2_OP2          0x0B24
+#define BLT_PLUGS2_CHZ          0x0B28
+#define BLT_PLUGS2_MSZ          0x0B2C
+#define BLT_PLUGS2_PGZ          0x0B30
+#define BLT_PLUGS3_OP2          0x0B44
+#define BLT_PLUGS3_CHZ          0x0B48
+#define BLT_PLUGS3_MSZ          0x0B4C
+#define BLT_PLUGS3_PGZ          0x0B50
+#define BLT_PLUGT_OP2           0x0B84
+#define BLT_PLUGT_CHZ           0x0B88
+#define BLT_PLUGT_MSZ           0x0B8C
+#define BLT_PLUGT_PGZ           0x0B90
+/* HW registers : node */
+#define BLT_NIP                 0x0C00
+#define BLT_CIC                 0x0C04
+#define BLT_INS                 0x0C08
+#define BLT_ACK                 0x0C0C
+#define BLT_TBA                 0x0C10
+#define BLT_TTY                 0x0C14
+#define BLT_TXY                 0x0C18
+#define BLT_TSZ                 0x0C1C
+#define BLT_S1BA                0x0C28
+#define BLT_S1TY                0x0C2C
+#define BLT_S1XY                0x0C30
+#define BLT_S2BA                0x0C38
+#define BLT_S2TY                0x0C3C
+#define BLT_S2XY                0x0C40
+#define BLT_S2SZ                0x0C44
+#define BLT_S3BA                0x0C48
+#define BLT_S3TY                0x0C4C
+#define BLT_S3XY                0x0C50
+#define BLT_S3SZ                0x0C54
+#define BLT_FCTL                0x0C68
+#define BLT_RSF                 0x0C70
+#define BLT_RZI                 0x0C74
+#define BLT_HFP                 0x0C78
+#define BLT_VFP                 0x0C7C
+#define BLT_Y_RSF               0x0C80
+#define BLT_Y_RZI               0x0C84
+#define BLT_Y_HFP               0x0C88
+#define BLT_Y_VFP               0x0C8C
+#define BLT_IVMX0               0x0CC0
+#define BLT_IVMX1               0x0CC4
+#define BLT_IVMX2               0x0CC8
+#define BLT_IVMX3               0x0CCC
+#define BLT_OVMX0               0x0CD0
+#define BLT_OVMX1               0x0CD4
+#define BLT_OVMX2               0x0CD8
+#define BLT_OVMX3               0x0CDC
+#define BLT_DEI                 0x0CEC
+/* HW registers : filters */
+#define BLT_HFC_N               0x0D00
+#define BLT_VFC_N               0x0D90
+#define BLT_Y_HFC_N             0x0E00
+#define BLT_Y_VFC_N             0x0E90
+#define BLT_NB_H_COEF           16
+#define BLT_NB_V_COEF           10
+
+/* Registers values */
+#define BLT_CTL_RESET           BIT(31)         /* Global soft reset */
+
+#define BLT_ITS_AQ1_LNA         BIT(12)         /* AQ1 LNA reached */
+
+#define BLT_STA1_IDLE           BIT(0)          /* BDISP idle */
+
+#define BLT_AQ1_CTL_CFG         0x80400003      /* Enable, P3, LNA reached */
+
+#define BLT_INS_S1_MASK         (BIT(0) | BIT(1) | BIT(2))
+#define BLT_INS_S1_OFF          0x00000000      /* src1 disabled */
+#define BLT_INS_S1_MEM          0x00000001      /* src1 fetched from memory */
+#define BLT_INS_S1_CF           0x00000003      /* src1 color fill */
+#define BLT_INS_S1_COPY         0x00000004      /* src1 direct copy */
+#define BLT_INS_S1_FILL         0x00000007      /* src1 firect fill */
+#define BLT_INS_S2_MASK         (BIT(3) | BIT(4))
+#define BLT_INS_S2_OFF          0x00000000      /* src2 disabled */
+#define BLT_INS_S2_MEM          0x00000008      /* src2 fetched from memory */
+#define BLT_INS_S2_CF           0x00000018      /* src2 color fill */
+#define BLT_INS_S3_MASK         BIT(5)
+#define BLT_INS_S3_OFF          0x00000000      /* src3 disabled */
+#define BLT_INS_S3_MEM          0x00000020      /* src3 fetched from memory */
+#define BLT_INS_IVMX            BIT(6)          /* Input versatile matrix */
+#define BLT_INS_CLUT            BIT(7)          /* Color Look Up Table */
+#define BLT_INS_SCALE           BIT(8)          /* Scaling */
+#define BLT_INS_FLICK           BIT(9)          /* Flicker filter */
+#define BLT_INS_CLIP            BIT(10)         /* Clipping */
+#define BLT_INS_CKEY            BIT(11)         /* Color key */
+#define BLT_INS_OVMX            BIT(12)         /* Output versatile matrix */
+#define BLT_INS_DEI             BIT(13)         /* Deinterlace */
+#define BLT_INS_PMASK           BIT(14)         /* Plane mask */
+#define BLT_INS_VC1R            BIT(17)         /* VC1 Range mapping */
+#define BLT_INS_ROTATE          BIT(18)         /* Rotation */
+#define BLT_INS_GRAD            BIT(19)         /* Gradient fill */
+#define BLT_INS_AQLOCK          BIT(29)         /* AQ lock */
+#define BLT_INS_PACE            BIT(30)         /* Pace down */
+#define BLT_INS_IRQ             BIT(31)         /* Raise IRQ when node done */
+#define BLT_CIC_ALL_GRP         0x000FDFFC      /* all valid groups present */
+#define BLT_ACK_BYPASS_S2S3     0x00000007      /* Bypass src2 and src3 */
+
+#define BLT_TTY_COL_SHIFT       16              /* Color format */
+#define BLT_TTY_COL_MASK        0x001F0000      /* Color format mask */
+#define BLT_TTY_ALPHA_R         BIT(21)         /* Alpha range */
+#define BLT_TTY_CR_NOT_CB       BIT(22)         /* CR not Cb */
+#define BLT_TTY_MB              BIT(23)         /* MB frame / field*/
+#define BLT_TTY_HSO             BIT(24)         /* H scan order */
+#define BLT_TTY_VSO             BIT(25)         /* V scan order */
+#define BLT_TTY_DITHER          BIT(26)         /* Dithering */
+#define BLT_TTY_CHROMA          BIT(27)         /* Write chroma / luma */
+#define BLT_TTY_BIG_END         BIT(30)         /* Big endianness */
+
+#define BLT_S1TY_A1_SUBSET      BIT(22)         /* A1 subset */
+#define BLT_S1TY_CHROMA_EXT     BIT(26)         /* Chroma Extended */
+#define BTL_S1TY_SUBBYTE        BIT(28)         /* Sub-byte fmt, pixel order */
+#define BLT_S1TY_RGB_EXP        BIT(29)         /* RGB expansion mode */
+
+#define BLT_S2TY_A1_SUBSET      BIT(22)         /* A1 subset */
+#define BLT_S2TY_CHROMA_EXT     BIT(26)         /* Chroma Extended */
+#define BTL_S2TY_SUBBYTE        BIT(28)         /* Sub-byte fmt, pixel order */
+#define BLT_S2TY_RGB_EXP        BIT(29)         /* RGB expansion mode */
+
+#define BLT_S3TY_BLANK_ACC      BIT(26)         /* Blank access */
+
+#define BLT_FCTL_HV_SCALE       0x00000055      /* H/V resize + color filter */
+#define BLT_FCTL_Y_HV_SCALE     0x33000000      /* Luma version */
+
+#define BLT_FCTL_HV_SAMPLE      0x00000044      /* H/V resize */
+#define BLT_FCTL_Y_HV_SAMPLE    0x22000000      /* Luma version */
+
+#define BLT_RZI_DEFAULT         0x20003000      /* H/VNB_repeat = 3/2 */
+
+/* Color format */
+#define BDISP_RGB565            0x00            /* RGB565 */
+#define BDISP_RGB888            0x01            /* RGB888 */
+#define BDISP_XRGB8888          0x02            /* RGB888_32 */
+#define BDISP_ARGB8888          0x05            /* ARGB888 */
+#define BDISP_NV12              0x16            /* YCbCr42x R2B */
+#define BDISP_YUV_3B            0x1E            /* YUV (3 buffer) */
diff --git a/drivers/media/platform/sti/bdisp/bdisp-v4l2.c b/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
new file mode 100644
index 000000000000..9e782ebe18da
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
@@ -0,0 +1,1416 @@
+/*
+ * 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/errno.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+
+#include "bdisp.h"
+
+#define BDISP_MAX_CTRL_NUM      10
+
+#define BDISP_WORK_TIMEOUT      ((100 * HZ) / 1000)
+
+/* User configuration change */
+#define BDISP_PARAMS            BIT(0) /* Config updated */
+#define BDISP_SRC_FMT           BIT(1) /* Source set */
+#define BDISP_DST_FMT           BIT(2) /* Destination set */
+#define BDISP_CTX_STOP_REQ      BIT(3) /* Stop request */
+#define BDISP_CTX_ABORT         BIT(4) /* Abort while device run */
+
+#define BDISP_MIN_W             1
+#define BDISP_MAX_W             8191
+#define BDISP_MIN_H             1
+#define BDISP_MAX_H             8191
+
+#define fh_to_ctx(__fh) container_of(__fh, struct bdisp_ctx, fh)
+
+enum bdisp_dev_flags {
+	ST_M2M_OPEN,            /* Driver opened */
+	ST_M2M_RUNNING,         /* HW device running */
+	ST_M2M_SUSPENDED,       /* Driver suspended */
+	ST_M2M_SUSPENDING,      /* Driver being suspended */
+};
+
+static const struct bdisp_fmt bdisp_formats[] = {
+	/* ARGB888. [31:0] A:R:G:B 8:8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_ABGR32, /* is actually ARGB */
+		.nb_planes      = 1,
+		.bpp            = 32,
+		.bpp_plane0     = 32,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* XRGB888. [31:0] x:R:G:B 8:8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_XBGR32, /* is actually xRGB */
+		.nb_planes      = 1,
+		.bpp            = 32,
+		.bpp_plane0     = 32,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* RGB565. [15:0] R:G:B 5:6:5 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_RGB565,
+		.nb_planes      = 1,
+		.bpp            = 16,
+		.bpp_plane0     = 16,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* NV12. YUV420SP - 1 plane for Y + 1 plane for (CbCr) */
+	{
+		.pixelformat    = V4L2_PIX_FMT_NV12,
+		.nb_planes      = 2,
+		.bpp            = 12,
+		.bpp_plane0     = 8,
+		.w_align        = 2,
+		.h_align        = 2
+	},
+	/* RGB888. [23:0] B:G:R 8:8:8 little endian */
+	{
+		.pixelformat    = V4L2_PIX_FMT_RGB24,
+		.nb_planes      = 1,
+		.bpp            = 24,
+		.bpp_plane0     = 24,
+		.w_align        = 1,
+		.h_align        = 1
+	},
+	/* YU12. YUV420P - 1 plane for Y + 1 plane for Cb + 1 plane for Cr
+	 * To keep as the LAST element of this table (no support on capture)
+	 */
+	{
+		.pixelformat    = V4L2_PIX_FMT_YUV420,
+		.nb_planes      = 3,
+		.bpp            = 12,
+		.bpp_plane0     = 8,
+		.w_align        = 2,
+		.h_align        = 2
+	}
+};
+
+/* Default format : HD ARGB32*/
+#define BDISP_DEF_WIDTH         1920
+#define BDISP_DEF_HEIGHT        1080
+
+static const struct bdisp_frame bdisp_dflt_fmt = {
+		.width          = BDISP_DEF_WIDTH,
+		.height         = BDISP_DEF_HEIGHT,
+		.fmt            = &bdisp_formats[0],
+		.field          = V4L2_FIELD_NONE,
+		.bytesperline   = BDISP_DEF_WIDTH * 4,
+		.sizeimage      = BDISP_DEF_WIDTH * BDISP_DEF_HEIGHT * 4,
+		.colorspace     = V4L2_COLORSPACE_REC709,
+		.crop           = {0, 0, BDISP_DEF_WIDTH, BDISP_DEF_HEIGHT},
+		.paddr          = {0, 0, 0, 0}
+};
+
+static inline void bdisp_ctx_state_lock_set(u32 state, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ctx->state |= state;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+}
+
+static inline void bdisp_ctx_state_lock_clear(u32 state, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ctx->state &= ~state;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+}
+
+static inline bool bdisp_ctx_state_is_set(u32 mask, struct bdisp_ctx *ctx)
+{
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ret = (ctx->state & mask) == mask;
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+
+	return ret;
+}
+
+static const struct bdisp_fmt *bdisp_find_fmt(u32 pixelformat)
+{
+	const struct bdisp_fmt *fmt;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(bdisp_formats); i++) {
+		fmt = &bdisp_formats[i];
+		if (fmt->pixelformat == pixelformat)
+			return fmt;
+	}
+
+	return NULL;
+}
+
+static struct bdisp_frame *ctx_get_frame(struct bdisp_ctx *ctx,
+					 enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &ctx->src;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &ctx->dst;
+	default:
+		dev_err(ctx->bdisp_dev->dev,
+			"Wrong buffer/video queue type (%d)\n", type);
+		break;
+	}
+
+	return ERR_PTR(-EINVAL);
+}
+
+static void bdisp_job_finish(struct bdisp_ctx *ctx, int vb_state)
+{
+	struct vb2_buffer *src_vb, *dst_vb;
+
+	if (WARN(!ctx || !ctx->fh.m2m_ctx, "Null hardware context\n"))
+		return;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+	if (src_vb && dst_vb) {
+		dst_vb->v4l2_buf.timestamp = src_vb->v4l2_buf.timestamp;
+		dst_vb->v4l2_buf.timecode = src_vb->v4l2_buf.timecode;
+		dst_vb->v4l2_buf.flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+		dst_vb->v4l2_buf.flags |= src_vb->v4l2_buf.flags &
+					  V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+
+		v4l2_m2m_buf_done(src_vb, vb_state);
+		v4l2_m2m_buf_done(dst_vb, vb_state);
+
+		v4l2_m2m_job_finish(ctx->bdisp_dev->m2m.m2m_dev,
+				    ctx->fh.m2m_ctx);
+	}
+}
+
+static int bdisp_ctx_stop_req(struct bdisp_ctx *ctx)
+{
+	struct bdisp_ctx *curr_ctx;
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+	int ret;
+
+	dev_dbg(ctx->bdisp_dev->dev, "%s\n", __func__);
+
+	cancel_delayed_work(&bdisp->timeout_work);
+
+	curr_ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+	if (!test_bit(ST_M2M_RUNNING, &bdisp->state) || (curr_ctx != ctx))
+		return 0;
+
+	bdisp_ctx_state_lock_set(BDISP_CTX_STOP_REQ, ctx);
+
+	ret = wait_event_timeout(bdisp->irq_queue,
+			!bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx),
+			BDISP_WORK_TIMEOUT);
+
+	if (!ret) {
+		dev_err(ctx->bdisp_dev->dev, "%s IRQ timeout\n", __func__);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void __bdisp_job_abort(struct bdisp_ctx *ctx)
+{
+	int ret;
+
+	ret = bdisp_ctx_stop_req(ctx);
+	if ((ret == -ETIMEDOUT) || (ctx->state & BDISP_CTX_ABORT)) {
+		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ | BDISP_CTX_ABORT,
+					   ctx);
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static void bdisp_job_abort(void *priv)
+{
+	__bdisp_job_abort((struct bdisp_ctx *)priv);
+}
+
+static int bdisp_get_addr(struct bdisp_ctx *ctx, struct vb2_buffer *vb,
+			  struct bdisp_frame *frame, dma_addr_t *paddr)
+{
+	if (!vb || !frame)
+		return -EINVAL;
+
+	paddr[0] = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	if (frame->fmt->nb_planes > 1)
+		/* UV (NV12) or U (420P) */
+		paddr[1] = (dma_addr_t)(paddr[0] +
+				frame->bytesperline * frame->height);
+
+	if (frame->fmt->nb_planes > 2)
+		/* V (420P) */
+		paddr[2] = (dma_addr_t)(paddr[1] +
+				(frame->bytesperline * frame->height) / 4);
+
+	if (frame->fmt->nb_planes > 3)
+		dev_dbg(ctx->bdisp_dev->dev, "ignoring some planes\n");
+
+	dev_dbg(ctx->bdisp_dev->dev,
+		"%s plane[0]=%pad plane[1]=%pad plane[2]=%pad\n",
+		__func__, &paddr[0], &paddr[1], &paddr[2]);
+
+	return 0;
+}
+
+static int bdisp_get_bufs(struct bdisp_ctx *ctx)
+{
+	struct bdisp_frame *src, *dst;
+	struct vb2_buffer *src_vb, *dst_vb;
+	int ret;
+
+	src = &ctx->src;
+	dst = &ctx->dst;
+
+	src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	ret = bdisp_get_addr(ctx, src_vb, src, src->paddr);
+	if (ret)
+		return ret;
+
+	dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	ret = bdisp_get_addr(ctx, dst_vb, dst, dst->paddr);
+	if (ret)
+		return ret;
+
+	dst_vb->v4l2_buf.timestamp = src_vb->v4l2_buf.timestamp;
+
+	return 0;
+}
+
+static void bdisp_device_run(void *priv)
+{
+	struct bdisp_ctx *ctx = priv;
+	struct bdisp_dev *bdisp;
+	unsigned long flags;
+	int err = 0;
+
+	if (WARN(!ctx, "Null hardware context\n"))
+		return;
+
+	bdisp = ctx->bdisp_dev;
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+	spin_lock_irqsave(&bdisp->slock, flags);
+
+	if (bdisp->m2m.ctx != ctx) {
+		dev_dbg(bdisp->dev, "ctx updated: %p -> %p\n",
+			bdisp->m2m.ctx, ctx);
+		ctx->state |= BDISP_PARAMS;
+		bdisp->m2m.ctx = ctx;
+	}
+
+	if (ctx->state & BDISP_CTX_STOP_REQ) {
+		ctx->state &= ~BDISP_CTX_STOP_REQ;
+		ctx->state |= BDISP_CTX_ABORT;
+		wake_up(&bdisp->irq_queue);
+		goto out;
+	}
+
+	err = bdisp_get_bufs(ctx);
+	if (err) {
+		dev_err(bdisp->dev, "cannot get address\n");
+		goto out;
+	}
+
+	bdisp_dbg_perf_begin(bdisp);
+
+	err = bdisp_hw_reset(bdisp);
+	if (err) {
+		dev_err(bdisp->dev, "could not get HW ready\n");
+		goto out;
+	}
+
+	err = bdisp_hw_update(ctx);
+	if (err) {
+		dev_err(bdisp->dev, "could not send HW request\n");
+		goto out;
+	}
+
+	queue_delayed_work(bdisp->work_queue, &bdisp->timeout_work,
+			   BDISP_WORK_TIMEOUT);
+	set_bit(ST_M2M_RUNNING, &bdisp->state);
+out:
+	ctx->state &= ~BDISP_PARAMS;
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+	if (err)
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+}
+
+static struct v4l2_m2m_ops bdisp_m2m_ops = {
+	.device_run     = bdisp_device_run,
+	.job_abort      = bdisp_job_abort,
+};
+
+static int __bdisp_s_ctrl(struct bdisp_ctx *ctx, struct v4l2_ctrl *ctrl)
+{
+	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ctx->hflip = ctrl->val;
+		break;
+	case V4L2_CID_VFLIP:
+		ctx->vflip = ctrl->val;
+		break;
+	default:
+		dev_err(ctx->bdisp_dev->dev, "unknown control %d\n", ctrl->id);
+		return -EINVAL;
+	}
+
+	ctx->state |= BDISP_PARAMS;
+
+	return 0;
+}
+
+static int bdisp_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct bdisp_ctx *ctx = container_of(ctrl->handler, struct bdisp_ctx,
+						ctrl_handler);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&ctx->bdisp_dev->slock, flags);
+	ret = __bdisp_s_ctrl(ctx, ctrl);
+	spin_unlock_irqrestore(&ctx->bdisp_dev->slock, flags);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops bdisp_c_ops = {
+	.s_ctrl = bdisp_s_ctrl,
+};
+
+static int bdisp_ctrls_create(struct bdisp_ctx *ctx)
+{
+	if (ctx->ctrls_rdy)
+		return 0;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, BDISP_MAX_CTRL_NUM);
+
+	ctx->bdisp_ctrls.hflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&bdisp_c_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	ctx->bdisp_ctrls.vflip = v4l2_ctrl_new_std(&ctx->ctrl_handler,
+				&bdisp_c_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+	if (ctx->ctrl_handler.error) {
+		int err = ctx->ctrl_handler.error;
+
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		return err;
+	}
+
+	ctx->ctrls_rdy = true;
+
+	return 0;
+}
+
+static void bdisp_ctrls_delete(struct bdisp_ctx *ctx)
+{
+	if (ctx->ctrls_rdy) {
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+		ctx->ctrls_rdy = false;
+	}
+}
+
+static int bdisp_queue_setup(struct vb2_queue *vq,
+			     const struct v4l2_format *fmt,
+			     unsigned int *nb_buf, unsigned int *nb_planes,
+			     unsigned int sizes[], void *allocators[])
+{
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vq);
+	struct bdisp_frame *frame = ctx_get_frame(ctx, vq->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	if (!frame->fmt) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid format\n");
+		return -EINVAL;
+	}
+
+	if (fmt && fmt->fmt.pix.sizeimage < frame->sizeimage)
+		return -EINVAL;
+
+	*nb_planes = 1;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : frame->sizeimage;
+	allocators[0] = ctx->bdisp_dev->alloc_ctx;
+
+	return 0;
+}
+
+static int bdisp_buf_prepare(struct vb2_buffer *vb)
+{
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct bdisp_frame *frame = ctx_get_frame(ctx, vb->vb2_queue->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		vb2_set_plane_payload(vb, 0, frame->sizeimage);
+
+	return 0;
+}
+
+static void bdisp_buf_queue(struct vb2_buffer *vb)
+{
+	struct bdisp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	/* return to V4L2 any 0-size buffer so it can be dequeued by user */
+	if (!vb2_get_plane_payload(vb, 0)) {
+		dev_dbg(ctx->bdisp_dev->dev, "0 data buffer, skip it\n");
+		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+		return;
+	}
+
+	if (ctx->fh.m2m_ctx)
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
+}
+
+static int bdisp_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct bdisp_ctx *ctx = q->drv_priv;
+	struct vb2_buffer *buf;
+	int ret = pm_runtime_get_sync(ctx->bdisp_dev->dev);
+
+	if (ret < 0) {
+		dev_err(ctx->bdisp_dev->dev, "failed to set runtime PM\n");
+
+		if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+			while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+		} else {
+			while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+				v4l2_m2m_buf_done(buf, VB2_BUF_STATE_QUEUED);
+		}
+
+		return ret;
+	}
+
+	return 0;
+}
+
+static void bdisp_stop_streaming(struct vb2_queue *q)
+{
+	struct bdisp_ctx *ctx = q->drv_priv;
+
+	__bdisp_job_abort(ctx);
+
+	pm_runtime_put(ctx->bdisp_dev->dev);
+}
+
+static struct vb2_ops bdisp_qops = {
+	.queue_setup     = bdisp_queue_setup,
+	.buf_prepare     = bdisp_buf_prepare,
+	.buf_queue       = bdisp_buf_queue,
+	.wait_prepare    = vb2_ops_wait_prepare,
+	.wait_finish     = vb2_ops_wait_finish,
+	.stop_streaming  = bdisp_stop_streaming,
+	.start_streaming = bdisp_start_streaming,
+};
+
+static int queue_init(void *priv,
+		      struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
+{
+	struct bdisp_ctx *ctx = priv;
+	int ret;
+
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->ops = &bdisp_qops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &ctx->bdisp_dev->lock;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->ops = &bdisp_qops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &ctx->bdisp_dev->lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+static int bdisp_open(struct file *file)
+{
+	struct bdisp_dev *bdisp = video_drvdata(file);
+	struct bdisp_ctx *ctx = NULL;
+	int ret;
+
+	if (mutex_lock_interruptible(&bdisp->lock))
+		return -ERESTARTSYS;
+
+	/* Allocate memory for both context and node */
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+	ctx->bdisp_dev = bdisp;
+
+	if (bdisp_hw_alloc_nodes(ctx)) {
+		dev_err(bdisp->dev, "no memory for nodes\n");
+		ret = -ENOMEM;
+		goto mem_ctx;
+	}
+
+	v4l2_fh_init(&ctx->fh, bdisp->m2m.vdev);
+
+	ret = bdisp_ctrls_create(ctx);
+	if (ret) {
+		dev_err(bdisp->dev, "Failed to create control\n");
+		goto error_fh;
+	}
+
+	/* Use separate control handler per file handle */
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+
+	/* Default format */
+	ctx->src = bdisp_dflt_fmt;
+	ctx->dst = bdisp_dflt_fmt;
+
+	/* Setup the device context for mem2mem mode. */
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(bdisp->m2m.m2m_dev, ctx,
+					    queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		dev_err(bdisp->dev, "Failed to initialize m2m context\n");
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto error_ctrls;
+	}
+
+	bdisp->m2m.refcnt++;
+	set_bit(ST_M2M_OPEN, &bdisp->state);
+
+	dev_dbg(bdisp->dev, "driver opened, ctx = 0x%p\n", ctx);
+
+	mutex_unlock(&bdisp->lock);
+
+	return 0;
+
+error_ctrls:
+	bdisp_ctrls_delete(ctx);
+error_fh:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	bdisp_hw_free_nodes(ctx);
+mem_ctx:
+	kfree(ctx);
+unlock:
+	mutex_unlock(&bdisp->lock);
+
+	return ret;
+}
+
+static int bdisp_release(struct file *file)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(file->private_data);
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+
+	dev_dbg(bdisp->dev, "%s\n", __func__);
+
+	if (mutex_lock_interruptible(&bdisp->lock))
+		return -ERESTARTSYS;
+
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+
+	bdisp_ctrls_delete(ctx);
+
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+
+	if (--bdisp->m2m.refcnt <= 0)
+		clear_bit(ST_M2M_OPEN, &bdisp->state);
+
+	bdisp_hw_free_nodes(ctx);
+
+	kfree(ctx);
+
+	mutex_unlock(&bdisp->lock);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations bdisp_fops = {
+	.owner          = THIS_MODULE,
+	.open           = bdisp_open,
+	.release        = bdisp_release,
+	.poll           = v4l2_m2m_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = v4l2_m2m_fop_mmap,
+};
+
+static int bdisp_querycap(struct file *file, void *fh,
+			  struct v4l2_capability *cap)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct bdisp_dev *bdisp = ctx->bdisp_dev;
+
+	strlcpy(cap->driver, bdisp->pdev->name, sizeof(cap->driver));
+	strlcpy(cap->card, bdisp->pdev->name, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s%d",
+		 BDISP_NAME, bdisp->id);
+
+	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M;
+
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int bdisp_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	const struct bdisp_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(bdisp_formats))
+		return -EINVAL;
+
+	fmt = &bdisp_formats[f->index];
+
+	if ((fmt->pixelformat == V4L2_PIX_FMT_YUV420) &&
+	    (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
+		dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
+		return -EINVAL;
+	}
+	f->pixelformat = fmt->pixelformat;
+
+	return 0;
+}
+
+static int bdisp_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct bdisp_frame *frame  = ctx_get_frame(ctx, f->type);
+
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	pix = &f->fmt.pix;
+	pix->width = frame->width;
+	pix->height = frame->height;
+	pix->pixelformat = frame->fmt->pixelformat;
+	pix->field = frame->field;
+	pix->bytesperline = frame->bytesperline;
+	pix->sizeimage = frame->sizeimage;
+	pix->colorspace = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
+				frame->colorspace : bdisp_dflt_fmt.colorspace;
+
+	return 0;
+}
+
+static int bdisp_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	const struct bdisp_fmt *format;
+	u32 in_w, in_h;
+
+	format = bdisp_find_fmt(pix->pixelformat);
+	if (!format) {
+		dev_dbg(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
+			pix->pixelformat);
+		return -EINVAL;
+	}
+
+	/* YUV420P only supported for VIDEO_OUTPUT */
+	if ((format->pixelformat == V4L2_PIX_FMT_YUV420) &&
+	    (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
+		dev_dbg(ctx->bdisp_dev->dev, "No YU12 on capture\n");
+		return -EINVAL;
+	}
+
+	/* Field (interlaced only supported on OUTPUT) */
+	if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+	    (pix->field != V4L2_FIELD_INTERLACED))
+		pix->field = V4L2_FIELD_NONE;
+
+	/* Adjust width & height */
+	in_w = pix->width;
+	in_h = pix->height;
+	v4l_bound_align_image(&pix->width,
+			      BDISP_MIN_W, BDISP_MAX_W,
+			      ffs(format->w_align) - 1,
+			      &pix->height,
+			      BDISP_MIN_H, BDISP_MAX_H,
+			      ffs(format->h_align) - 1,
+			      0);
+	if ((pix->width != in_w) || (pix->height != in_h))
+		dev_dbg(ctx->bdisp_dev->dev,
+			"%s size updated: %dx%d -> %dx%d\n", __func__,
+			in_w, in_h, pix->width, pix->height);
+
+	pix->bytesperline = (pix->width * format->bpp_plane0) / 8;
+	pix->sizeimage = (pix->width * pix->height * format->bpp) / 8;
+
+	if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		pix->colorspace = bdisp_dflt_fmt.colorspace;
+
+	return 0;
+}
+
+static int bdisp_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct vb2_queue *vq;
+	struct bdisp_frame *frame;
+	struct v4l2_pix_format *pix;
+	int ret;
+	u32 state;
+
+	ret = bdisp_try_fmt(file, fh, f);
+	if (ret) {
+		dev_err(ctx->bdisp_dev->dev, "Cannot set format\n");
+		return ret;
+	}
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (vb2_is_streaming(vq)) {
+		dev_err(ctx->bdisp_dev->dev, "queue (%d) busy\n", f->type);
+		return -EBUSY;
+	}
+
+	frame = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
+			&ctx->src : &ctx->dst;
+	pix = &f->fmt.pix;
+	frame->fmt = bdisp_find_fmt(pix->pixelformat);
+	if (!frame->fmt) {
+		dev_err(ctx->bdisp_dev->dev, "Unknown format 0x%x\n",
+			pix->pixelformat);
+		return -EINVAL;
+	}
+
+	frame->width = pix->width;
+	frame->height = pix->height;
+	frame->bytesperline = pix->bytesperline;
+	frame->sizeimage = pix->sizeimage;
+	frame->field = pix->field;
+	if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		frame->colorspace = pix->colorspace;
+
+	frame->crop.width = frame->width;
+	frame->crop.height = frame->height;
+	frame->crop.left = 0;
+	frame->crop.top = 0;
+
+	state = BDISP_PARAMS;
+	state |= (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ?
+			BDISP_DST_FMT : BDISP_SRC_FMT;
+	bdisp_ctx_state_lock_set(state, ctx);
+
+	return 0;
+}
+
+static int bdisp_g_selection(struct file *file, void *fh,
+			     struct v4l2_selection *s)
+{
+	struct bdisp_frame *frame;
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		/* Composing  / capture is not supported */
+		dev_dbg(ctx->bdisp_dev->dev, "Not supported for capture\n");
+		return -EINVAL;
+	}
+
+	frame = ctx_get_frame(ctx, s->type);
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		/* cropped frame */
+		s->r = frame->crop;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		/* complete frame */
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = frame->width;
+		s->r.height = frame->height;
+		break;
+	default:
+		dev_dbg(ctx->bdisp_dev->dev, "Invalid target\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int is_rect_enclosed(struct v4l2_rect *a, struct v4l2_rect *b)
+{
+	/* Return 1 if a is enclosed in b, or 0 otherwise. */
+
+	if (a->left < b->left || a->top < b->top)
+		return 0;
+
+	if (a->left + a->width > b->left + b->width)
+		return 0;
+
+	if (a->top + a->height > b->top + b->height)
+		return 0;
+
+	return 1;
+}
+
+static int bdisp_s_selection(struct file *file, void *fh,
+			     struct v4l2_selection *s)
+{
+	struct bdisp_frame *frame;
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+	struct v4l2_rect *in, out;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		/* Composing  / capture is not supported */
+		dev_dbg(ctx->bdisp_dev->dev, "Not supported for capture\n");
+		return -EINVAL;
+	}
+
+	if (s->target != V4L2_SEL_TGT_CROP) {
+		dev_dbg(ctx->bdisp_dev->dev, "Invalid target\n");
+		return -EINVAL;
+	}
+
+	frame = ctx_get_frame(ctx, s->type);
+	if (IS_ERR(frame)) {
+		dev_err(ctx->bdisp_dev->dev, "Invalid frame (%p)\n", frame);
+		return PTR_ERR(frame);
+	}
+
+	in = &s->r;
+	out = *in;
+
+	/* Align and check origin */
+	out.left = ALIGN(in->left, frame->fmt->w_align);
+	out.top = ALIGN(in->top, frame->fmt->h_align);
+
+	if ((out.left < 0) || (out.left >= frame->width) ||
+	    (out.top < 0) || (out.top >= frame->height)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
+			out.width, out.height, out.left, out.top,
+			frame->width, frame->height);
+		return -EINVAL;
+	}
+
+	/* Align and check size */
+	out.width = ALIGN(in->width, frame->fmt->w_align);
+	out.height = ALIGN(in->height, frame->fmt->w_align);
+
+	if (((out.left + out.width) > frame->width) ||
+	    ((out.top + out.height) > frame->height)) {
+		dev_err(ctx->bdisp_dev->dev,
+			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
+			out.width, out.height, out.left, out.top,
+			frame->width, frame->height);
+		return -EINVAL;
+	}
+
+	/* Checks adjust constraints flags */
+	if (s->flags & V4L2_SEL_FLAG_LE && !is_rect_enclosed(&out, in))
+		return -ERANGE;
+
+	if (s->flags & V4L2_SEL_FLAG_GE && !is_rect_enclosed(in, &out))
+		return -ERANGE;
+
+	if ((out.left != in->left) || (out.top != in->top) ||
+	    (out.width != in->width) || (out.height != in->height)) {
+		dev_dbg(ctx->bdisp_dev->dev,
+			"%s crop updated: %dx%d@(%d,%d) -> %dx%d@(%d,%d)\n",
+			__func__, in->width, in->height, in->left, in->top,
+			out.width, out.height, out.left, out.top);
+		*in = out;
+	}
+
+	frame->crop = out;
+
+	bdisp_ctx_state_lock_set(BDISP_PARAMS, ctx);
+
+	return 0;
+}
+
+static int bdisp_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct bdisp_ctx *ctx = fh_to_ctx(fh);
+
+	if ((type == V4L2_BUF_TYPE_VIDEO_OUTPUT) &&
+	    !bdisp_ctx_state_is_set(BDISP_SRC_FMT, ctx)) {
+		dev_err(ctx->bdisp_dev->dev, "src not defined\n");
+		return -EINVAL;
+	}
+
+	if ((type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+	    !bdisp_ctx_state_is_set(BDISP_DST_FMT, ctx)) {
+		dev_err(ctx->bdisp_dev->dev, "dst not defined\n");
+		return -EINVAL;
+	}
+
+	return v4l2_m2m_streamon(file, ctx->fh.m2m_ctx, type);
+}
+
+static const struct v4l2_ioctl_ops bdisp_ioctl_ops = {
+	.vidioc_querycap                = bdisp_querycap,
+	.vidioc_enum_fmt_vid_cap        = bdisp_enum_fmt,
+	.vidioc_enum_fmt_vid_out        = bdisp_enum_fmt,
+	.vidioc_g_fmt_vid_cap           = bdisp_g_fmt,
+	.vidioc_g_fmt_vid_out           = bdisp_g_fmt,
+	.vidioc_try_fmt_vid_cap         = bdisp_try_fmt,
+	.vidioc_try_fmt_vid_out         = bdisp_try_fmt,
+	.vidioc_s_fmt_vid_cap           = bdisp_s_fmt,
+	.vidioc_s_fmt_vid_out           = bdisp_s_fmt,
+	.vidioc_g_selection		= bdisp_g_selection,
+	.vidioc_s_selection		= bdisp_s_selection,
+	.vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
+	.vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_streamon                = bdisp_streamon,
+	.vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
+	.vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
+};
+
+static int bdisp_register_device(struct bdisp_dev *bdisp)
+{
+	int ret;
+
+	if (!bdisp)
+		return -ENODEV;
+
+	bdisp->vdev.fops        = &bdisp_fops;
+	bdisp->vdev.ioctl_ops   = &bdisp_ioctl_ops;
+	bdisp->vdev.release     = video_device_release_empty;
+	bdisp->vdev.lock        = &bdisp->lock;
+	bdisp->vdev.vfl_dir     = VFL_DIR_M2M;
+	bdisp->vdev.v4l2_dev    = &bdisp->v4l2_dev;
+	snprintf(bdisp->vdev.name, sizeof(bdisp->vdev.name), "%s.%d",
+		 BDISP_NAME, bdisp->id);
+
+	video_set_drvdata(&bdisp->vdev, bdisp);
+
+	bdisp->m2m.vdev = &bdisp->vdev;
+	bdisp->m2m.m2m_dev = v4l2_m2m_init(&bdisp_m2m_ops);
+	if (IS_ERR(bdisp->m2m.m2m_dev)) {
+		dev_err(bdisp->dev, "failed to initialize v4l2-m2m device\n");
+		return PTR_ERR(bdisp->m2m.m2m_dev);
+	}
+
+	ret = video_register_device(&bdisp->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret) {
+		dev_err(bdisp->dev,
+			"%s(): failed to register video device\n", __func__);
+		v4l2_m2m_release(bdisp->m2m.m2m_dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void bdisp_unregister_device(struct bdisp_dev *bdisp)
+{
+	if (!bdisp)
+		return;
+
+	if (bdisp->m2m.m2m_dev)
+		v4l2_m2m_release(bdisp->m2m.m2m_dev);
+
+	video_unregister_device(bdisp->m2m.vdev);
+}
+
+static irqreturn_t bdisp_irq_thread(int irq, void *priv)
+{
+	struct bdisp_dev *bdisp = priv;
+	struct bdisp_ctx *ctx;
+
+	spin_lock(&bdisp->slock);
+
+	bdisp_dbg_perf_end(bdisp);
+
+	cancel_delayed_work(&bdisp->timeout_work);
+
+	if (!test_and_clear_bit(ST_M2M_RUNNING, &bdisp->state))
+		goto isr_unlock;
+
+	if (test_and_clear_bit(ST_M2M_SUSPENDING, &bdisp->state)) {
+		set_bit(ST_M2M_SUSPENDED, &bdisp->state);
+		wake_up(&bdisp->irq_queue);
+		goto isr_unlock;
+	}
+
+	ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+	if (!ctx || !ctx->fh.m2m_ctx)
+		goto isr_unlock;
+
+	spin_unlock(&bdisp->slock);
+
+	bdisp_job_finish(ctx, VB2_BUF_STATE_DONE);
+
+	if (bdisp_ctx_state_is_set(BDISP_CTX_STOP_REQ, ctx)) {
+		bdisp_ctx_state_lock_clear(BDISP_CTX_STOP_REQ, ctx);
+		wake_up(&bdisp->irq_queue);
+	}
+
+	return IRQ_HANDLED;
+
+isr_unlock:
+	spin_unlock(&bdisp->slock);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t bdisp_irq_handler(int irq, void *priv)
+{
+	if (bdisp_hw_get_and_clear_irq((struct bdisp_dev *)priv))
+		return IRQ_NONE;
+	else
+		return IRQ_WAKE_THREAD;
+}
+
+static void bdisp_irq_timeout(struct work_struct *ptr)
+{
+	struct delayed_work *twork = to_delayed_work(ptr);
+	struct bdisp_dev *bdisp = container_of(twork, struct bdisp_dev,
+			timeout_work);
+	struct bdisp_ctx *ctx;
+
+	ctx = v4l2_m2m_get_curr_priv(bdisp->m2m.m2m_dev);
+
+	dev_err(ctx->bdisp_dev->dev, "Device work timeout\n");
+
+	spin_lock(&bdisp->slock);
+	clear_bit(ST_M2M_RUNNING, &bdisp->state);
+	spin_unlock(&bdisp->slock);
+
+	bdisp_hw_reset(bdisp);
+
+	bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+}
+
+static int bdisp_m2m_suspend(struct bdisp_dev *bdisp)
+{
+	unsigned long flags;
+	int timeout;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	if (!test_bit(ST_M2M_RUNNING, &bdisp->state)) {
+		spin_unlock_irqrestore(&bdisp->slock, flags);
+		return 0;
+	}
+	clear_bit(ST_M2M_SUSPENDED, &bdisp->state);
+	set_bit(ST_M2M_SUSPENDING, &bdisp->state);
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	timeout = wait_event_timeout(bdisp->irq_queue,
+				     test_bit(ST_M2M_SUSPENDED, &bdisp->state),
+				     BDISP_WORK_TIMEOUT);
+
+	clear_bit(ST_M2M_SUSPENDING, &bdisp->state);
+
+	if (!timeout) {
+		dev_err(bdisp->dev, "%s IRQ timeout\n", __func__);
+		return -EAGAIN;
+	}
+
+	return 0;
+}
+
+static int bdisp_m2m_resume(struct bdisp_dev *bdisp)
+{
+	struct bdisp_ctx *ctx;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	ctx = bdisp->m2m.ctx;
+	bdisp->m2m.ctx = NULL;
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	if (test_and_clear_bit(ST_M2M_SUSPENDED, &bdisp->state))
+		bdisp_job_finish(ctx, VB2_BUF_STATE_ERROR);
+
+	return 0;
+}
+
+static int bdisp_runtime_resume(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	int ret = clk_enable(bdisp->clock);
+
+	if (ret)
+		return ret;
+
+	return bdisp_m2m_resume(bdisp);
+}
+
+static int bdisp_runtime_suspend(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	int ret = bdisp_m2m_suspend(bdisp);
+
+	if (!ret)
+		clk_disable(bdisp->clock);
+
+	return ret;
+}
+
+static int bdisp_resume(struct device *dev)
+{
+	struct bdisp_dev *bdisp = dev_get_drvdata(dev);
+	unsigned long flags;
+	int opened;
+
+	spin_lock_irqsave(&bdisp->slock, flags);
+	opened = test_bit(ST_M2M_OPEN, &bdisp->state);
+	spin_unlock_irqrestore(&bdisp->slock, flags);
+
+	if (!opened)
+		return 0;
+
+	if (!pm_runtime_suspended(dev))
+		return bdisp_runtime_resume(dev);
+
+	return 0;
+}
+
+static int bdisp_suspend(struct device *dev)
+{
+	if (!pm_runtime_suspended(dev))
+		return bdisp_runtime_suspend(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops bdisp_pm_ops = {
+	.suspend                = bdisp_suspend,
+	.resume                 = bdisp_resume,
+	.runtime_suspend        = bdisp_runtime_suspend,
+	.runtime_resume         = bdisp_runtime_resume,
+};
+
+static int bdisp_remove(struct platform_device *pdev)
+{
+	struct bdisp_dev *bdisp = platform_get_drvdata(pdev);
+
+	bdisp_unregister_device(bdisp);
+
+	bdisp_hw_free_filters(bdisp->dev);
+
+	vb2_dma_contig_cleanup_ctx(bdisp->alloc_ctx);
+
+	pm_runtime_disable(&pdev->dev);
+
+	bdisp_debugfs_remove(bdisp);
+
+	v4l2_device_unregister(&bdisp->v4l2_dev);
+
+	if (!IS_ERR(bdisp->clock))
+		clk_unprepare(bdisp->clock);
+
+	dev_dbg(&pdev->dev, "%s driver unloaded\n", pdev->name);
+
+	return 0;
+}
+
+static int bdisp_probe(struct platform_device *pdev)
+{
+	struct bdisp_dev *bdisp;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	bdisp = devm_kzalloc(dev, sizeof(struct bdisp_dev), GFP_KERNEL);
+	if (!bdisp)
+		return -ENOMEM;
+
+	bdisp->pdev = pdev;
+	bdisp->dev = dev;
+	platform_set_drvdata(pdev, bdisp);
+
+	if (dev->of_node)
+		bdisp->id = of_alias_get_id(pdev->dev.of_node, BDISP_NAME);
+	else
+		bdisp->id = pdev->id;
+
+	init_waitqueue_head(&bdisp->irq_queue);
+	INIT_DELAYED_WORK(&bdisp->timeout_work, bdisp_irq_timeout);
+	bdisp->work_queue = create_workqueue(BDISP_NAME);
+
+	spin_lock_init(&bdisp->slock);
+	mutex_init(&bdisp->lock);
+
+	/* get resources */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	bdisp->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(bdisp->regs)) {
+		dev_err(dev, "failed to get regs\n");
+		return PTR_ERR(bdisp->regs);
+	}
+
+	bdisp->clock = devm_clk_get(dev, BDISP_NAME);
+	if (IS_ERR(bdisp->clock)) {
+		dev_err(dev, "failed to get clock\n");
+		return PTR_ERR(bdisp->clock);
+	}
+
+	ret = clk_prepare(bdisp->clock);
+	if (ret < 0) {
+		dev_err(dev, "clock prepare failed\n");
+		bdisp->clock = ERR_PTR(-EINVAL);
+		return ret;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(dev, "failed to get IRQ resource\n");
+		goto err_clk;
+	}
+
+	ret = devm_request_threaded_irq(dev, res->start, bdisp_irq_handler,
+					bdisp_irq_thread, IRQF_ONESHOT,
+					pdev->name, bdisp);
+	if (ret) {
+		dev_err(dev, "failed to install irq\n");
+		goto err_clk;
+	}
+
+	/* v4l2 register */
+	ret = v4l2_device_register(dev, &bdisp->v4l2_dev);
+	if (ret) {
+		dev_err(dev, "failed to register\n");
+		goto err_clk;
+	}
+
+	/* Debug */
+	ret = bdisp_debugfs_create(bdisp);
+	if (ret) {
+		dev_err(dev, "failed to create debugfs\n");
+		goto err_v4l2;
+	}
+
+	/* Power management */
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(dev);
+	if (ret < 0) {
+		dev_err(dev, "failed to set PM\n");
+		goto err_dbg;
+	}
+
+	/* Continuous memory allocator */
+	bdisp->alloc_ctx = vb2_dma_contig_init_ctx(dev);
+	if (IS_ERR(bdisp->alloc_ctx)) {
+		ret = PTR_ERR(bdisp->alloc_ctx);
+		goto err_pm;
+	}
+
+	/* Filters */
+	if (bdisp_hw_alloc_filters(bdisp->dev)) {
+		dev_err(bdisp->dev, "no memory for filters\n");
+		ret = -ENOMEM;
+		goto err_vb2_dma;
+	}
+
+	/* Register */
+	ret = bdisp_register_device(bdisp);
+	if (ret) {
+		dev_err(dev, "failed to register\n");
+		goto err_filter;
+	}
+
+	dev_info(dev, "%s%d registered as /dev/video%d\n", BDISP_NAME,
+		 bdisp->id, bdisp->vdev.num);
+
+	pm_runtime_put(dev);
+
+	return 0;
+
+err_filter:
+	bdisp_hw_free_filters(bdisp->dev);
+err_vb2_dma:
+	vb2_dma_contig_cleanup_ctx(bdisp->alloc_ctx);
+err_pm:
+	pm_runtime_put(dev);
+err_dbg:
+	bdisp_debugfs_remove(bdisp);
+err_v4l2:
+	v4l2_device_unregister(&bdisp->v4l2_dev);
+err_clk:
+	if (!IS_ERR(bdisp->clock))
+		clk_unprepare(bdisp->clock);
+
+	return ret;
+}
+
+static const struct of_device_id bdisp_match_types[] = {
+	{
+		.compatible = "st,stih407-bdisp",
+	},
+	{ /* end node */ }
+};
+
+MODULE_DEVICE_TABLE(of, bdisp_match_types);
+
+static struct platform_driver bdisp_driver = {
+	.probe          = bdisp_probe,
+	.remove         = bdisp_remove,
+	.driver         = {
+		.name           = BDISP_NAME,
+		.of_match_table = bdisp_match_types,
+		.pm             = &bdisp_pm_ops,
+	},
+};
+
+module_platform_driver(bdisp_driver);
+
+MODULE_DESCRIPTION("2D blitter for STMicroelectronics SoC");
+MODULE_AUTHOR("Fabien Dessenne <fabien.dessenne@st.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/sti/bdisp/bdisp.h b/drivers/media/platform/sti/bdisp/bdisp.h
new file mode 100644
index 000000000000..0cf98577222c
--- /dev/null
+++ b/drivers/media/platform/sti/bdisp/bdisp.h
@@ -0,0 +1,216 @@
+/*
+ * 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/ktime.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-mem2mem.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#define BDISP_NAME              "bdisp"
+
+/*
+ *  Max nb of nodes in node-list:
+ *   - 2 nodes to handle wide 4K pictures
+ *   - 2 nodes to handle two planes (Y & CbCr) */
+#define MAX_OUTPUT_PLANES       2
+#define MAX_VERTICAL_STRIDES    2
+#define MAX_NB_NODE             (MAX_OUTPUT_PLANES * MAX_VERTICAL_STRIDES)
+
+/* struct bdisp_ctrls - bdisp control set
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ */
+struct bdisp_ctrls {
+	struct v4l2_ctrl        *hflip;
+	struct v4l2_ctrl        *vflip;
+};
+
+/**
+ * struct bdisp_fmt - driver's internal color format data
+ * @pixelformat:fourcc code for this format
+ * @nb_planes:  number of planes  (ex: [0]=RGB/Y - [1]=Cb/Cr, ...)
+ * @bpp:        bits per pixel (general)
+ * @bpp_plane0: byte per pixel for the 1st plane
+ * @w_align:    width alignment in pixel (multiple of)
+ * @h_align:    height alignment in pixel (multiple of)
+ */
+struct bdisp_fmt {
+	u32                     pixelformat;
+	u8                      nb_planes;
+	u8                      bpp;
+	u8                      bpp_plane0;
+	u8                      w_align;
+	u8                      h_align;
+};
+
+/**
+ * struct bdisp_frame - frame properties
+ *
+ * @width:      frame width (including padding)
+ * @height:     frame height (including padding)
+ * @fmt:        pointer to frame format descriptor
+ * @field:      frame / field type
+ * @bytesperline: stride of the 1st plane
+ * @sizeimage:  image size in bytes
+ * @colorspace: colorspace
+ * @crop:       crop area
+ * @paddr:      image physical addresses per plane ([0]=RGB/Y - [1]=Cb/Cr, ...)
+ */
+struct bdisp_frame {
+	u32                     width;
+	u32                     height;
+	const struct bdisp_fmt  *fmt;
+	enum v4l2_field         field;
+	u32                     bytesperline;
+	u32                     sizeimage;
+	enum v4l2_colorspace    colorspace;
+	struct v4l2_rect        crop;
+	dma_addr_t              paddr[4];
+};
+
+/**
+ * struct bdisp_request - bdisp request
+ *
+ * @src:        source frame properties
+ * @dst:        destination frame properties
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ * @nb_req:     number of run request
+ */
+struct bdisp_request {
+	struct bdisp_frame      src;
+	struct bdisp_frame      dst;
+	unsigned int            hflip:1;
+	unsigned int            vflip:1;
+	int                     nb_req;
+};
+
+/**
+ * struct bdisp_ctx - device context data
+ *
+ * @src:        source frame properties
+ * @dst:        destination frame properties
+ * @state:      flags to keep track of user configuration
+ * @hflip:      horizontal flip
+ * @vflip:      vertical flip
+ * @bdisp_dev:  the device this context applies to
+ * @node:       node array
+ * @node_paddr: node physical address array
+ * @fh:         v4l2 file handle
+ * @ctrl_handler: v4l2 controls handler
+ * @bdisp_ctrls: bdisp control set
+ * @ctrls_rdy:  true if the control handler is initialized
+ */
+struct bdisp_ctx {
+	struct bdisp_frame      src;
+	struct bdisp_frame      dst;
+	u32                     state;
+	unsigned int            hflip:1;
+	unsigned int            vflip:1;
+	struct bdisp_dev        *bdisp_dev;
+	struct bdisp_node       *node[MAX_NB_NODE];
+	dma_addr_t              node_paddr[MAX_NB_NODE];
+	struct v4l2_fh          fh;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct bdisp_ctrls      bdisp_ctrls;
+	bool                    ctrls_rdy;
+};
+
+/**
+ * struct bdisp_m2m_device - v4l2 memory-to-memory device data
+ *
+ * @vdev:       video device node for v4l2 m2m mode
+ * @m2m_dev:    v4l2 m2m device data
+ * @ctx:        hardware context data
+ * @refcnt:     reference counter
+ */
+struct bdisp_m2m_device {
+	struct video_device     *vdev;
+	struct v4l2_m2m_dev     *m2m_dev;
+	struct bdisp_ctx        *ctx;
+	int                     refcnt;
+};
+
+/**
+ * struct bdisp_dbg - debug info
+ *
+ * @debugfs_entry: debugfs
+ * @copy_node:     array of last used nodes
+ * @copy_request:  last bdisp request
+ * @hw_start:      start time of last HW request
+ * @last_duration: last HW processing duration in microsecs
+ * @min_duration:  min HW processing duration in microsecs
+ * @max_duration:  max HW processing duration in microsecs
+ * @tot_duration:  total HW processing duration in microsecs
+ */
+struct bdisp_dbg {
+	struct dentry           *debugfs_entry;
+	struct bdisp_node       *copy_node[MAX_NB_NODE];
+	struct bdisp_request    copy_request;
+	ktime_t                 hw_start;
+	s64                     last_duration;
+	s64                     min_duration;
+	s64                     max_duration;
+	s64                     tot_duration;
+};
+
+/**
+ * struct bdisp_dev - abstraction for bdisp entity
+ *
+ * @v4l2_dev:   v4l2 device
+ * @vdev:       video device
+ * @pdev:       platform device
+ * @dev:        device
+ * @lock:       mutex protecting this data structure
+ * @slock:      spinlock protecting this data structure
+ * @id:         device index
+ * @m2m:        memory-to-memory V4L2 device information
+ * @state:      flags used to synchronize m2m and capture mode operation
+ * @alloc_ctx:  videobuf2 memory allocator context
+ * @clock:      IP clock
+ * @regs:       registers
+ * @irq_queue:  interrupt handler waitqueue
+ * @work_queue: workqueue to handle timeouts
+ * @timeout_work: IRQ timeout structure
+ * @dbg:        debug info
+ */
+struct bdisp_dev {
+	struct v4l2_device      v4l2_dev;
+	struct video_device     vdev;
+	struct platform_device  *pdev;
+	struct device           *dev;
+	spinlock_t              slock;
+	struct mutex            lock;
+	u16                     id;
+	struct bdisp_m2m_device m2m;
+	unsigned long           state;
+	struct vb2_alloc_ctx    *alloc_ctx;
+	struct clk              *clock;
+	void __iomem            *regs;
+	wait_queue_head_t       irq_queue;
+	struct workqueue_struct *work_queue;
+	struct delayed_work     timeout_work;
+	struct bdisp_dbg        dbg;
+};
+
+void bdisp_hw_free_nodes(struct bdisp_ctx *ctx);
+int bdisp_hw_alloc_nodes(struct bdisp_ctx *ctx);
+void bdisp_hw_free_filters(struct device *dev);
+int bdisp_hw_alloc_filters(struct device *dev);
+int bdisp_hw_reset(struct bdisp_dev *bdisp);
+int bdisp_hw_get_and_clear_irq(struct bdisp_dev *bdisp);
+int bdisp_hw_update(struct bdisp_ctx *ctx);
+
+void bdisp_debugfs_remove(struct bdisp_dev *bdisp);
+int bdisp_debugfs_create(struct bdisp_dev *bdisp);
+void bdisp_dbg_perf_begin(struct bdisp_dev *bdisp);
+void bdisp_dbg_perf_end(struct bdisp_dev *bdisp);
diff --git a/drivers/media/platform/via-camera.c b/drivers/media/platform/via-camera.c
index 678ed9f353cb..32e4ff46daf3 100644
--- a/drivers/media/platform/via-camera.c
+++ b/drivers/media/platform/via-camera.c
@@ -249,13 +249,15 @@ static int viacam_set_flip(struct via_camera *cam)
  */
 static int viacam_configure_sensor(struct via_camera *cam)
 {
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	int ret;
 
-	v4l2_fill_mbus_format(&mbus_fmt, &cam->sensor_format, cam->mbus_code);
+	v4l2_fill_mbus_format(&format.format, &cam->sensor_format, cam->mbus_code);
 	ret = sensor_call(cam, core, init, 0);
 	if (ret == 0)
-		ret = sensor_call(cam, video, s_mbus_fmt, &mbus_fmt);
+		ret = sensor_call(cam, pad, set_fmt, NULL, &format);
 	/*
 	 * OV7670 does weird things if flip is set *before* format...
 	 */
@@ -903,14 +905,17 @@ static int viacam_do_try_fmt(struct via_camera *cam,
 		struct v4l2_pix_format *upix, struct v4l2_pix_format *spix)
 {
 	int ret;
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_pad_config pad_cfg;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+	};
 	struct via_format *f = via_find_format(upix->pixelformat);
 
 	upix->pixelformat = f->pixelformat;
 	viacam_fmt_pre(upix, spix);
-	v4l2_fill_mbus_format(&mbus_fmt, spix, f->mbus_code);
-	ret = sensor_call(cam, video, try_mbus_fmt, &mbus_fmt);
-	v4l2_fill_pix_format(spix, &mbus_fmt);
+	v4l2_fill_mbus_format(&format.format, spix, f->mbus_code);
+	ret = sensor_call(cam, pad, set_fmt, &pad_cfg, &format);
+	v4l2_fill_pix_format(spix, &format.format);
 	viacam_fmt_post(upix, spix);
 	return ret;
 }
diff --git a/drivers/media/platform/vim2m.c b/drivers/media/platform/vim2m.c
index 4d6b4cc57c57..295fde5fdb75 100644
--- a/drivers/media/platform/vim2m.c
+++ b/drivers/media/platform/vim2m.c
@@ -80,7 +80,6 @@ static struct platform_device vim2m_pdev = {
 };
 
 struct vim2m_fmt {
-	char	*name;
 	u32	fourcc;
 	int	depth;
 	/* Types the format can be used for */
@@ -89,14 +88,12 @@ struct vim2m_fmt {
 
 static struct vim2m_fmt formats[] = {
 	{
-		.name	= "RGB565 (BE)",
 		.fourcc	= V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
 		.depth	= 16,
 		/* Both capture and output format */
 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
 	},
 	{
-		.name	= "4:2:2, packed, YUYV",
 		.fourcc	= V4L2_PIX_FMT_YUYV,
 		.depth	= 16,
 		/* Output-only format */
@@ -458,7 +455,6 @@ static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
 	if (i < NUM_FORMATS) {
 		/* Format found */
 		fmt = &formats[i];
-		strncpy(f->description, fmt->name, sizeof(f->description) - 1);
 		f->pixelformat = fmt->fourcc;
 		return 0;
 	}
@@ -697,6 +693,8 @@ static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
 	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
 	.vidioc_qbuf		= v4l2_m2m_ioctl_qbuf,
 	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
+	.vidioc_prepare_buf	= v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
 	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
 
 	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
@@ -724,6 +722,12 @@ static int vim2m_queue_setup(struct vb2_queue *vq,
 
 	size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
 
+	if (fmt) {
+		if (fmt->fmt.pix.sizeimage < size)
+			return -EINVAL;
+		size = fmt->fmt.pix.sizeimage;
+	}
+
 	while (size * count > MEM2MEM_VID_MEM_LIMIT)
 		(count)--;
 
diff --git a/drivers/media/platform/vivid/vivid-core.c b/drivers/media/platform/vivid/vivid-core.c
index d33f16495dbc..a047b4716741 100644
--- a/drivers/media/platform/vivid/vivid-core.c
+++ b/drivers/media/platform/vivid/vivid-core.c
@@ -392,6 +392,17 @@ static int vidioc_s_parm(struct file *file, void *fh,
 	return vivid_vid_out_g_parm(file, fh, parm);
 }
 
+static int vidioc_log_status(struct file *file, void *fh)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	v4l2_ctrl_log_status(file, fh);
+	if (vdev->vfl_dir == VFL_DIR_RX && vdev->vfl_type == VFL_TYPE_GRABBER)
+		tpg_log_status(&dev->tpg);
+	return 0;
+}
+
 static ssize_t vivid_radio_read(struct file *file, char __user *buf,
 			 size_t size, loff_t *offset)
 {
@@ -548,8 +559,8 @@ static const struct v4l2_ioctl_ops vivid_ioctl_ops = {
 
 	.vidioc_enum_fmt_sdr_cap	= vidioc_enum_fmt_sdr_cap,
 	.vidioc_g_fmt_sdr_cap		= vidioc_g_fmt_sdr_cap,
-	.vidioc_try_fmt_sdr_cap		= vidioc_g_fmt_sdr_cap,
-	.vidioc_s_fmt_sdr_cap		= vidioc_g_fmt_sdr_cap,
+	.vidioc_try_fmt_sdr_cap		= vidioc_try_fmt_sdr_cap,
+	.vidioc_s_fmt_sdr_cap		= vidioc_s_fmt_sdr_cap,
 
 	.vidioc_overlay			= vidioc_overlay,
 	.vidioc_enum_framesizes		= vidioc_enum_framesizes,
@@ -610,7 +621,7 @@ static const struct v4l2_ioctl_ops vivid_ioctl_ops = {
 	.vidioc_g_edid			= vidioc_g_edid,
 	.vidioc_s_edid			= vidioc_s_edid,
 
-	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_log_status		= vidioc_log_status,
 	.vidioc_subscribe_event		= vidioc_subscribe_event,
 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
 };
@@ -966,6 +977,9 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 	dev->radio_tx_subchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_RDS;
 	dev->sdr_adc_freq = 300000;
 	dev->sdr_fm_freq = 50000000;
+	dev->sdr_pixelformat = V4L2_SDR_FMT_CU8;
+	dev->sdr_buffersize = SDR_CAP_SAMPLES_PER_BUF * 2;
+
 	dev->edid_max_blocks = dev->edid_blocks = 2;
 	memcpy(dev->edid, vivid_hdmi_edid, sizeof(vivid_hdmi_edid));
 	ktime_get_ts(&dev->radio_rds_init_ts);
diff --git a/drivers/media/platform/vivid/vivid-core.h b/drivers/media/platform/vivid/vivid-core.h
index 9e15aee9a52e..c72349c83fab 100644
--- a/drivers/media/platform/vivid/vivid-core.h
+++ b/drivers/media/platform/vivid/vivid-core.h
@@ -77,7 +77,6 @@ extern const struct v4l2_rect vivid_max_rect;
 extern unsigned vivid_debug;
 
 struct vivid_fmt {
-	const char *name;
 	u32	fourcc;          /* v4l2 format id */
 	bool	is_yuv;
 	bool	can_do_overlay;
@@ -140,7 +139,7 @@ struct vivid_dev {
 	struct v4l2_ctrl_handler	ctrl_hdl_user_aud;
 	struct v4l2_ctrl_handler	ctrl_hdl_streaming;
 	struct v4l2_ctrl_handler	ctrl_hdl_sdtv_cap;
-	struct v4l2_ctrl_handler	ctrl_hdl_loop_out;
+	struct v4l2_ctrl_handler	ctrl_hdl_loop_cap;
 	struct video_device		vid_cap_dev;
 	struct v4l2_ctrl_handler	ctrl_hdl_vid_cap;
 	struct video_device		vid_out_dev;
@@ -333,6 +332,7 @@ struct vivid_dev {
 	u32				colorspace_out;
 	u32				ycbcr_enc_out;
 	u32				quantization_out;
+	u32				xfer_func_out;
 	u32				service_set_out;
 	unsigned			bytesperline_out[TPG_MAX_PLANES];
 	unsigned			tv_field_out;
@@ -447,6 +447,8 @@ struct vivid_dev {
 	/* SDR capture */
 	struct vb2_queue		vb_sdr_cap_q;
 	struct list_head		sdr_cap_active;
+	u32				sdr_pixelformat; /* v4l2 format id */
+	unsigned			sdr_buffersize;
 	unsigned			sdr_adc_freq;
 	unsigned			sdr_fm_freq;
 	int				sdr_fixp_src_phase;
diff --git a/drivers/media/platform/vivid/vivid-ctrls.c b/drivers/media/platform/vivid/vivid-ctrls.c
index 2b9070098b08..339c8b7e53c8 100644
--- a/drivers/media/platform/vivid/vivid-ctrls.c
+++ b/drivers/media/platform/vivid/vivid-ctrls.c
@@ -62,21 +62,22 @@
 #define VIVID_CID_DV_TIMINGS_ASPECT_RATIO	(VIVID_CID_VIVID_BASE + 23)
 #define VIVID_CID_TSTAMP_SRC		(VIVID_CID_VIVID_BASE + 24)
 #define VIVID_CID_COLORSPACE		(VIVID_CID_VIVID_BASE + 25)
-#define VIVID_CID_YCBCR_ENC		(VIVID_CID_VIVID_BASE + 26)
-#define VIVID_CID_QUANTIZATION		(VIVID_CID_VIVID_BASE + 27)
-#define VIVID_CID_LIMITED_RGB_RANGE	(VIVID_CID_VIVID_BASE + 28)
-#define VIVID_CID_ALPHA_MODE		(VIVID_CID_VIVID_BASE + 29)
-#define VIVID_CID_HAS_CROP_CAP		(VIVID_CID_VIVID_BASE + 30)
-#define VIVID_CID_HAS_COMPOSE_CAP	(VIVID_CID_VIVID_BASE + 31)
-#define VIVID_CID_HAS_SCALER_CAP	(VIVID_CID_VIVID_BASE + 32)
-#define VIVID_CID_HAS_CROP_OUT		(VIVID_CID_VIVID_BASE + 33)
-#define VIVID_CID_HAS_COMPOSE_OUT	(VIVID_CID_VIVID_BASE + 34)
-#define VIVID_CID_HAS_SCALER_OUT	(VIVID_CID_VIVID_BASE + 35)
-#define VIVID_CID_LOOP_VIDEO		(VIVID_CID_VIVID_BASE + 36)
-#define VIVID_CID_SEQ_WRAP		(VIVID_CID_VIVID_BASE + 37)
-#define VIVID_CID_TIME_WRAP		(VIVID_CID_VIVID_BASE + 38)
-#define VIVID_CID_MAX_EDID_BLOCKS	(VIVID_CID_VIVID_BASE + 39)
-#define VIVID_CID_PERCENTAGE_FILL	(VIVID_CID_VIVID_BASE + 40)
+#define VIVID_CID_XFER_FUNC		(VIVID_CID_VIVID_BASE + 26)
+#define VIVID_CID_YCBCR_ENC		(VIVID_CID_VIVID_BASE + 27)
+#define VIVID_CID_QUANTIZATION		(VIVID_CID_VIVID_BASE + 28)
+#define VIVID_CID_LIMITED_RGB_RANGE	(VIVID_CID_VIVID_BASE + 29)
+#define VIVID_CID_ALPHA_MODE		(VIVID_CID_VIVID_BASE + 30)
+#define VIVID_CID_HAS_CROP_CAP		(VIVID_CID_VIVID_BASE + 31)
+#define VIVID_CID_HAS_COMPOSE_CAP	(VIVID_CID_VIVID_BASE + 32)
+#define VIVID_CID_HAS_SCALER_CAP	(VIVID_CID_VIVID_BASE + 33)
+#define VIVID_CID_HAS_CROP_OUT		(VIVID_CID_VIVID_BASE + 34)
+#define VIVID_CID_HAS_COMPOSE_OUT	(VIVID_CID_VIVID_BASE + 35)
+#define VIVID_CID_HAS_SCALER_OUT	(VIVID_CID_VIVID_BASE + 36)
+#define VIVID_CID_LOOP_VIDEO		(VIVID_CID_VIVID_BASE + 37)
+#define VIVID_CID_SEQ_WRAP		(VIVID_CID_VIVID_BASE + 38)
+#define VIVID_CID_TIME_WRAP		(VIVID_CID_VIVID_BASE + 39)
+#define VIVID_CID_MAX_EDID_BLOCKS	(VIVID_CID_VIVID_BASE + 40)
+#define VIVID_CID_PERCENTAGE_FILL	(VIVID_CID_VIVID_BASE + 41)
 
 #define VIVID_CID_STD_SIGNAL_MODE	(VIVID_CID_VIVID_BASE + 60)
 #define VIVID_CID_STANDARD		(VIVID_CID_VIVID_BASE + 61)
@@ -360,6 +361,13 @@ static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
 		vivid_send_source_change(dev, HDMI);
 		vivid_send_source_change(dev, WEBCAM);
 		break;
+	case VIVID_CID_XFER_FUNC:
+		tpg_s_xfer_func(&dev->tpg, ctrl->val);
+		vivid_send_source_change(dev, TV);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		vivid_send_source_change(dev, WEBCAM);
+		break;
 	case VIVID_CID_YCBCR_ENC:
 		tpg_s_ycbcr_enc(&dev->tpg, ctrl->val);
 		vivid_send_source_change(dev, TV);
@@ -709,6 +717,25 @@ static const struct v4l2_ctrl_config vivid_ctrl_colorspace = {
 	.qmenu = vivid_ctrl_colorspace_strings,
 };
 
+static const char * const vivid_ctrl_xfer_func_strings[] = {
+	"Default",
+	"Rec. 709",
+	"sRGB",
+	"AdobeRGB",
+	"SMPTE 240M",
+	"None",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_xfer_func = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_XFER_FUNC,
+	.name = "Transfer Function",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 5,
+	.qmenu = vivid_ctrl_xfer_func_strings,
+};
+
 static const char * const vivid_ctrl_ycbcr_enc_strings[] = {
 	"Default",
 	"ITU-R 601",
@@ -766,6 +793,37 @@ static const struct v4l2_ctrl_config vivid_ctrl_limited_rgb_range = {
 };
 
 
+/* Video Loop Control */
+
+static int vivid_loop_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_loop_cap);
+
+	switch (ctrl->id) {
+	case VIVID_CID_LOOP_VIDEO:
+		dev->loop_video = ctrl->val;
+		vivid_update_quality(dev);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_loop_cap_ctrl_ops = {
+	.s_ctrl = vivid_loop_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_loop_video = {
+	.ops = &vivid_loop_cap_ctrl_ops,
+	.id = VIVID_CID_LOOP_VIDEO,
+	.name = "Loop Video",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
 /* VBI Capture Control */
 
 static int vivid_vbi_cap_s_ctrl(struct v4l2_ctrl *ctrl)
@@ -1199,38 +1257,6 @@ static const struct v4l2_ctrl_config vivid_ctrl_radio_tx_rds_blockio = {
 };
 
 
-
-/* Video Loop Control */
-
-static int vivid_loop_out_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_loop_out);
-
-	switch (ctrl->id) {
-	case VIVID_CID_LOOP_VIDEO:
-		dev->loop_video = ctrl->val;
-		vivid_update_quality(dev);
-		vivid_send_source_change(dev, SVID);
-		vivid_send_source_change(dev, HDMI);
-		break;
-	}
-	return 0;
-}
-
-static const struct v4l2_ctrl_ops vivid_loop_out_ctrl_ops = {
-	.s_ctrl = vivid_loop_out_s_ctrl,
-};
-
-static const struct v4l2_ctrl_config vivid_ctrl_loop_video = {
-	.ops = &vivid_loop_out_ctrl_ops,
-	.id = VIVID_CID_LOOP_VIDEO,
-	.name = "Loop Video",
-	.type = V4L2_CTRL_TYPE_BOOLEAN,
-	.max = 1,
-	.step = 1,
-};
-
-
 static const struct v4l2_ctrl_config vivid_ctrl_class = {
 	.ops = &vivid_user_gen_ctrl_ops,
 	.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
@@ -1248,7 +1274,7 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 	struct v4l2_ctrl_handler *hdl_user_aud = &dev->ctrl_hdl_user_aud;
 	struct v4l2_ctrl_handler *hdl_streaming = &dev->ctrl_hdl_streaming;
 	struct v4l2_ctrl_handler *hdl_sdtv_cap = &dev->ctrl_hdl_sdtv_cap;
-	struct v4l2_ctrl_handler *hdl_loop_out = &dev->ctrl_hdl_loop_out;
+	struct v4l2_ctrl_handler *hdl_loop_cap = &dev->ctrl_hdl_loop_cap;
 	struct v4l2_ctrl_handler *hdl_vid_cap = &dev->ctrl_hdl_vid_cap;
 	struct v4l2_ctrl_handler *hdl_vid_out = &dev->ctrl_hdl_vid_out;
 	struct v4l2_ctrl_handler *hdl_vbi_cap = &dev->ctrl_hdl_vbi_cap;
@@ -1274,8 +1300,8 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 	v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_class, NULL);
 	v4l2_ctrl_handler_init(hdl_sdtv_cap, 2);
 	v4l2_ctrl_new_custom(hdl_sdtv_cap, &vivid_ctrl_class, NULL);
-	v4l2_ctrl_handler_init(hdl_loop_out, 1);
-	v4l2_ctrl_new_custom(hdl_loop_out, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_loop_cap, 1);
+	v4l2_ctrl_new_custom(hdl_loop_cap, &vivid_ctrl_class, NULL);
 	v4l2_ctrl_handler_init(hdl_vid_cap, 55);
 	v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_class, NULL);
 	v4l2_ctrl_handler_init(hdl_vid_out, 26);
@@ -1365,6 +1391,7 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_tstamp_src, NULL);
 		dev->colorspace = v4l2_ctrl_new_custom(hdl_vid_cap,
 			&vivid_ctrl_colorspace, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_xfer_func, NULL);
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_ycbcr_enc, NULL);
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_quantization, NULL);
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_alpha_mode, NULL);
@@ -1445,7 +1472,7 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 	}
 	if ((dev->has_vid_cap && dev->has_vid_out) ||
 	    (dev->has_vbi_cap && dev->has_vbi_out))
-		v4l2_ctrl_new_custom(hdl_loop_out, &vivid_ctrl_loop_video, NULL);
+		v4l2_ctrl_new_custom(hdl_loop_cap, &vivid_ctrl_loop_video, NULL);
 
 	if (dev->has_fb)
 		v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_clear_fb, NULL);
@@ -1528,8 +1555,8 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 		return hdl_streaming->error;
 	if (hdl_sdr_cap->error)
 		return hdl_sdr_cap->error;
-	if (hdl_loop_out->error)
-		return hdl_loop_out->error;
+	if (hdl_loop_cap->error)
+		return hdl_loop_cap->error;
 
 	if (dev->autogain)
 		v4l2_ctrl_auto_cluster(2, &dev->autogain, 0, true);
@@ -1540,6 +1567,7 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_aud, NULL);
 		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_streaming, NULL);
 		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_sdtv_cap, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_loop_cap, NULL);
 		if (hdl_vid_cap->error)
 			return hdl_vid_cap->error;
 		dev->vid_cap_dev.ctrl_handler = hdl_vid_cap;
@@ -1548,7 +1576,6 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 		v4l2_ctrl_add_handler(hdl_vid_out, hdl_user_gen, NULL);
 		v4l2_ctrl_add_handler(hdl_vid_out, hdl_user_aud, NULL);
 		v4l2_ctrl_add_handler(hdl_vid_out, hdl_streaming, NULL);
-		v4l2_ctrl_add_handler(hdl_vid_out, hdl_loop_out, NULL);
 		if (hdl_vid_out->error)
 			return hdl_vid_out->error;
 		dev->vid_out_dev.ctrl_handler = hdl_vid_out;
@@ -1557,6 +1584,7 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_user_gen, NULL);
 		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_streaming, NULL);
 		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_sdtv_cap, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_loop_cap, NULL);
 		if (hdl_vbi_cap->error)
 			return hdl_vbi_cap->error;
 		dev->vbi_cap_dev.ctrl_handler = hdl_vbi_cap;
@@ -1564,7 +1592,6 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 	if (dev->has_vbi_out) {
 		v4l2_ctrl_add_handler(hdl_vbi_out, hdl_user_gen, NULL);
 		v4l2_ctrl_add_handler(hdl_vbi_out, hdl_streaming, NULL);
-		v4l2_ctrl_add_handler(hdl_vbi_out, hdl_loop_out, NULL);
 		if (hdl_vbi_out->error)
 			return hdl_vbi_out->error;
 		dev->vbi_out_dev.ctrl_handler = hdl_vbi_out;
@@ -1607,5 +1634,5 @@ void vivid_free_controls(struct vivid_dev *dev)
 	v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_aud);
 	v4l2_ctrl_handler_free(&dev->ctrl_hdl_streaming);
 	v4l2_ctrl_handler_free(&dev->ctrl_hdl_sdtv_cap);
-	v4l2_ctrl_handler_free(&dev->ctrl_hdl_loop_out);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_loop_cap);
 }
diff --git a/drivers/media/platform/vivid/vivid-radio-rx.c b/drivers/media/platform/vivid/vivid-radio-rx.c
index c7651a506668..f99092ca8f5c 100644
--- a/drivers/media/platform/vivid/vivid-radio-rx.c
+++ b/drivers/media/platform/vivid/vivid-radio-rx.c
@@ -195,6 +195,8 @@ int vivid_radio_rx_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2
 			if (dev->radio_rx_freq >= vivid_radio_bands[band].rangelow &&
 			    dev->radio_rx_freq <= vivid_radio_bands[band].rangehigh)
 				break;
+		if (band == TOT_BANDS)
+			return -EINVAL;
 		low = vivid_radio_bands[band].rangelow;
 		high = vivid_radio_bands[band].rangehigh;
 	}
diff --git a/drivers/media/platform/vivid/vivid-sdr-cap.c b/drivers/media/platform/vivid/vivid-sdr-cap.c
index caf131666e37..d2f2188a0efe 100644
--- a/drivers/media/platform/vivid/vivid-sdr-cap.c
+++ b/drivers/media/platform/vivid/vivid-sdr-cap.c
@@ -33,6 +33,25 @@
 #include "vivid-ctrls.h"
 #include "vivid-sdr-cap.h"
 
+/* stream formats */
+struct vivid_format {
+	u32	pixelformat;
+	u32	buffersize;
+};
+
+/* format descriptions for capture and preview */
+static struct vivid_format formats[] = {
+	{
+		.pixelformat	= V4L2_SDR_FMT_CU8,
+		.buffersize	= SDR_CAP_SAMPLES_PER_BUF * 2,
+	}, {
+		.pixelformat	= V4L2_SDR_FMT_CS8,
+		.buffersize	= SDR_CAP_SAMPLES_PER_BUF * 2,
+	},
+};
+
+static const unsigned int NUM_FORMATS = ARRAY_SIZE(formats);
+
 static const struct v4l2_frequency_band bands_adc[] = {
 	{
 		.tuner = 0,
@@ -409,21 +428,63 @@ int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
 
 int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 {
-	if (f->index)
+	if (f->index >= ARRAY_SIZE(formats))
 		return -EINVAL;
-	f->pixelformat = V4L2_SDR_FMT_CU8;
-	strlcpy(f->description, "IQ U8", sizeof(f->description));
+	f->pixelformat = formats[f->index].pixelformat;
 	return 0;
 }
 
 int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
 {
-	f->fmt.sdr.pixelformat = V4L2_SDR_FMT_CU8;
-	f->fmt.sdr.buffersize = SDR_CAP_SAMPLES_PER_BUF * 2;
+	struct vivid_dev *dev = video_drvdata(file);
+
+	f->fmt.sdr.pixelformat = dev->sdr_pixelformat;
+	f->fmt.sdr.buffersize = dev->sdr_buffersize;
 	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
 	return 0;
 }
 
+int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct vb2_queue *q = &dev->vb_sdr_cap_q;
+	int i;
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
+			dev->sdr_pixelformat = formats[i].pixelformat;
+			dev->sdr_buffersize = formats[i].buffersize;
+			f->fmt.sdr.buffersize = formats[i].buffersize;
+			return 0;
+		}
+	}
+	dev->sdr_pixelformat = formats[0].pixelformat;
+	dev->sdr_buffersize = formats[0].buffersize;
+	f->fmt.sdr.pixelformat = formats[0].pixelformat;
+	f->fmt.sdr.buffersize = formats[0].buffersize;
+	return 0;
+}
+
+int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	int i;
+
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	for (i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
+			f->fmt.sdr.buffersize = formats[i].buffersize;
+			return 0;
+		}
+	}
+	f->fmt.sdr.pixelformat = formats[0].pixelformat;
+	f->fmt.sdr.buffersize = formats[0].buffersize;
+	return 0;
+}
+
 #define FIXP_N    (15)
 #define FIXP_FRAC (1 << FIXP_N)
 #define FIXP_2PI  ((int)(2 * 3.141592653589 * FIXP_FRAC))
@@ -477,11 +538,24 @@ void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
 		fixp_i >>= (31 - FIXP_N);
 		fixp_q >>= (31 - FIXP_N);
 
-		/* convert 'fixp float' to u8 */
-		/* u8 = X * 127.5f + 127.5f; where X is float [-1.0 / +1.0] */
-		fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275;
-		fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275;
-		*vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
-		*vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
+		switch (dev->sdr_pixelformat) {
+		case V4L2_SDR_FMT_CU8:
+			/* convert 'fixp float' to u8 */
+			/* u8 = X * 127.5 + 127.5; X is float [-1.0, +1.0] */
+			fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275;
+			fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275;
+			*vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
+			*vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
+			break;
+		case V4L2_SDR_FMT_CS8:
+			/* convert 'fixp float' to s8 */
+			fixp_i = fixp_i * 1275;
+			fixp_q = fixp_q * 1275;
+			*vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
+			*vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
+			break;
+		default:
+			break;
+		}
 	}
 }
diff --git a/drivers/media/platform/vivid/vivid-sdr-cap.h b/drivers/media/platform/vivid/vivid-sdr-cap.h
index 79c1890de972..43014b2733db 100644
--- a/drivers/media/platform/vivid/vivid-sdr-cap.h
+++ b/drivers/media/platform/vivid/vivid-sdr-cap.h
@@ -27,6 +27,8 @@ int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
 int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
 int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f);
 int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
 void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
 
 extern const struct vb2_ops vivid_sdr_cap_qops;
diff --git a/drivers/media/platform/vivid/vivid-tpg-colors.c b/drivers/media/platform/vivid/vivid-tpg-colors.c
index 424aa7abc723..8f231a6e68c9 100644
--- a/drivers/media/platform/vivid/vivid-tpg-colors.c
+++ b/drivers/media/platform/vivid/vivid-tpg-colors.c
@@ -598,71 +598,327 @@ const unsigned short tpg_linear_to_rec709[255 * 16 + 1] = {
 };
 
 /* Generated table */
-const struct color16 tpg_csc_colors[V4L2_COLORSPACE_BT2020 + 1][TPG_COLOR_CSC_BLACK + 1] = {
-	[V4L2_COLORSPACE_SMPTE170M][0] = { 2939, 2939, 2939 },
-	[V4L2_COLORSPACE_SMPTE170M][1] = { 2953, 2963, 586 },
-	[V4L2_COLORSPACE_SMPTE170M][2] = { 0, 2967, 2937 },
-	[V4L2_COLORSPACE_SMPTE170M][3] = { 88, 2990, 575 },
-	[V4L2_COLORSPACE_SMPTE170M][4] = { 3016, 259, 2933 },
-	[V4L2_COLORSPACE_SMPTE170M][5] = { 3030, 405, 558 },
-	[V4L2_COLORSPACE_SMPTE170M][6] = { 478, 428, 2931 },
-	[V4L2_COLORSPACE_SMPTE170M][7] = { 547, 547, 547 },
-	[V4L2_COLORSPACE_SMPTE240M][0] = { 2926, 2926, 2926 },
-	[V4L2_COLORSPACE_SMPTE240M][1] = { 2941, 2950, 546 },
-	[V4L2_COLORSPACE_SMPTE240M][2] = { 0, 2954, 2924 },
-	[V4L2_COLORSPACE_SMPTE240M][3] = { 78, 2978, 536 },
-	[V4L2_COLORSPACE_SMPTE240M][4] = { 3004, 230, 2920 },
-	[V4L2_COLORSPACE_SMPTE240M][5] = { 3018, 363, 518 },
-	[V4L2_COLORSPACE_SMPTE240M][6] = { 437, 387, 2918 },
-	[V4L2_COLORSPACE_SMPTE240M][7] = { 507, 507, 507 },
-	[V4L2_COLORSPACE_REC709][0] = { 2939, 2939, 2939 },
-	[V4L2_COLORSPACE_REC709][1] = { 2939, 2939, 547 },
-	[V4L2_COLORSPACE_REC709][2] = { 547, 2939, 2939 },
-	[V4L2_COLORSPACE_REC709][3] = { 547, 2939, 547 },
-	[V4L2_COLORSPACE_REC709][4] = { 2939, 547, 2939 },
-	[V4L2_COLORSPACE_REC709][5] = { 2939, 547, 547 },
-	[V4L2_COLORSPACE_REC709][6] = { 547, 547, 2939 },
-	[V4L2_COLORSPACE_REC709][7] = { 547, 547, 547 },
-	[V4L2_COLORSPACE_470_SYSTEM_M][0] = { 2892, 2988, 2807 },
-	[V4L2_COLORSPACE_470_SYSTEM_M][1] = { 2846, 3070, 843 },
-	[V4L2_COLORSPACE_470_SYSTEM_M][2] = { 1656, 2962, 2783 },
-	[V4L2_COLORSPACE_470_SYSTEM_M][3] = { 1572, 3045, 763 },
-	[V4L2_COLORSPACE_470_SYSTEM_M][4] = { 2476, 229, 2742 },
-	[V4L2_COLORSPACE_470_SYSTEM_M][5] = { 2420, 672, 614 },
-	[V4L2_COLORSPACE_470_SYSTEM_M][6] = { 725, 63, 2718 },
-	[V4L2_COLORSPACE_470_SYSTEM_M][7] = { 534, 561, 509 },
-	[V4L2_COLORSPACE_470_SYSTEM_BG][0] = { 2939, 2939, 2939 },
-	[V4L2_COLORSPACE_470_SYSTEM_BG][1] = { 2939, 2939, 464 },
-	[V4L2_COLORSPACE_470_SYSTEM_BG][2] = { 786, 2939, 2939 },
-	[V4L2_COLORSPACE_470_SYSTEM_BG][3] = { 786, 2939, 464 },
-	[V4L2_COLORSPACE_470_SYSTEM_BG][4] = { 2879, 547, 2956 },
-	[V4L2_COLORSPACE_470_SYSTEM_BG][5] = { 2879, 547, 547 },
-	[V4L2_COLORSPACE_470_SYSTEM_BG][6] = { 547, 547, 2956 },
-	[V4L2_COLORSPACE_470_SYSTEM_BG][7] = { 547, 547, 547 },
-	[V4L2_COLORSPACE_SRGB][0] = { 3056, 3056, 3056 },
-	[V4L2_COLORSPACE_SRGB][1] = { 3056, 3056, 800 },
-	[V4L2_COLORSPACE_SRGB][2] = { 800, 3056, 3056 },
-	[V4L2_COLORSPACE_SRGB][3] = { 800, 3056, 800 },
-	[V4L2_COLORSPACE_SRGB][4] = { 3056, 800, 3056 },
-	[V4L2_COLORSPACE_SRGB][5] = { 3056, 800, 800 },
-	[V4L2_COLORSPACE_SRGB][6] = { 800, 800, 3056 },
-	[V4L2_COLORSPACE_SRGB][7] = { 800, 800, 800 },
-	[V4L2_COLORSPACE_ADOBERGB][0] = { 3033, 3033, 3033 },
-	[V4L2_COLORSPACE_ADOBERGB][1] = { 3033, 3033, 1063 },
-	[V4L2_COLORSPACE_ADOBERGB][2] = { 1828, 3033, 3033 },
-	[V4L2_COLORSPACE_ADOBERGB][3] = { 1828, 3033, 1063 },
-	[V4L2_COLORSPACE_ADOBERGB][4] = { 2633, 851, 2979 },
-	[V4L2_COLORSPACE_ADOBERGB][5] = { 2633, 851, 851 },
-	[V4L2_COLORSPACE_ADOBERGB][6] = { 851, 851, 2979 },
-	[V4L2_COLORSPACE_ADOBERGB][7] = { 851, 851, 851 },
-	[V4L2_COLORSPACE_BT2020][0] = { 2939, 2939, 2939 },
-	[V4L2_COLORSPACE_BT2020][1] = { 2877, 2923, 1058 },
-	[V4L2_COLORSPACE_BT2020][2] = { 1837, 2840, 2916 },
-	[V4L2_COLORSPACE_BT2020][3] = { 1734, 2823, 993 },
-	[V4L2_COLORSPACE_BT2020][4] = { 2427, 961, 2812 },
-	[V4L2_COLORSPACE_BT2020][5] = { 2351, 912, 648 },
-	[V4L2_COLORSPACE_BT2020][6] = { 792, 618, 2788 },
-	[V4L2_COLORSPACE_BT2020][7] = { 547, 547, 547 },
+const struct color16 tpg_csc_colors[V4L2_COLORSPACE_BT2020 + 1][V4L2_XFER_FUNC_NONE + 1][TPG_COLOR_CSC_BLACK + 1] = {
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][1] = { 2953, 2963, 586 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][2] = { 0, 2967, 2937 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][3] = { 88, 2990, 575 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][4] = { 3016, 259, 2933 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][5] = { 3030, 405, 558 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][6] = { 478, 428, 2931 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][1] = { 3068, 3077, 838 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][2] = { 0, 3081, 3053 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][3] = { 241, 3102, 828 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][4] = { 3126, 504, 3050 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][5] = { 3138, 657, 810 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][6] = { 731, 680, 3048 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SRGB][7] = { 800, 799, 800 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][1] = { 3046, 3054, 886 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][2] = { 0, 3058, 3031 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][3] = { 360, 3079, 877 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][4] = { 3103, 587, 3027 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][5] = { 3116, 723, 861 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][6] = { 789, 744, 3025 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][1] = { 2941, 2950, 546 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][2] = { 0, 2954, 2924 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][3] = { 78, 2978, 536 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][4] = { 3004, 230, 2920 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][5] = { 3018, 363, 518 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][6] = { 437, 387, 2918 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][1] = { 2145, 2159, 142 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][2] = { 0, 2164, 2122 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][3] = { 19, 2198, 138 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][4] = { 2236, 57, 2116 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][5] = { 2256, 90, 133 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][6] = { 110, 96, 2113 },
+	[V4L2_COLORSPACE_SMPTE170M][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][1] = { 2953, 2963, 586 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][2] = { 0, 2967, 2937 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][3] = { 88, 2990, 575 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][4] = { 3016, 259, 2933 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][5] = { 3030, 405, 558 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][6] = { 478, 428, 2931 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][1] = { 3068, 3077, 838 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][2] = { 0, 3081, 3053 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][3] = { 241, 3102, 828 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][4] = { 3126, 504, 3050 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][5] = { 3138, 657, 810 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][6] = { 731, 680, 3048 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SRGB][7] = { 800, 799, 800 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][1] = { 3046, 3054, 886 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][2] = { 0, 3058, 3031 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][3] = { 360, 3079, 877 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][4] = { 3103, 587, 3027 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][5] = { 3116, 723, 861 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][6] = { 789, 744, 3025 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][1] = { 2941, 2950, 546 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][2] = { 0, 2954, 2924 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][3] = { 78, 2978, 536 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][4] = { 3004, 230, 2920 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][5] = { 3018, 363, 518 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][6] = { 437, 387, 2918 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][1] = { 2145, 2159, 142 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][2] = { 0, 2164, 2122 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][3] = { 19, 2198, 138 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][4] = { 2236, 57, 2116 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][5] = { 2256, 90, 133 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][6] = { 110, 96, 2113 },
+	[V4L2_COLORSPACE_SMPTE240M][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][1] = { 2939, 2939, 547 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][2] = { 547, 2939, 2939 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][3] = { 547, 2939, 547 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][4] = { 2939, 547, 2939 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][5] = { 2939, 547, 547 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][6] = { 547, 547, 2939 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][1] = { 3056, 3056, 800 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][2] = { 800, 3056, 3056 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][3] = { 800, 3056, 800 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][4] = { 3056, 800, 3056 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][5] = { 3056, 800, 800 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][6] = { 800, 800, 3056 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 800 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][1] = { 3033, 3033, 851 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][2] = { 851, 3033, 3033 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][3] = { 851, 3033, 851 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][4] = { 3033, 851, 3033 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][5] = { 3033, 851, 851 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][6] = { 851, 851, 3033 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][1] = { 2926, 2926, 507 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][2] = { 507, 2926, 2926 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][3] = { 507, 2926, 507 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][4] = { 2926, 507, 2926 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][5] = { 2926, 507, 507 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][6] = { 507, 507, 2926 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][1] = { 2125, 2125, 130 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][2] = { 130, 2125, 2125 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][3] = { 130, 2125, 130 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][4] = { 2125, 130, 2125 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][5] = { 2125, 130, 130 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][6] = { 130, 130, 2125 },
+	[V4L2_COLORSPACE_REC709][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][0] = { 2892, 2988, 2807 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][1] = { 2846, 3070, 843 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][2] = { 1656, 2962, 2783 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][3] = { 1572, 3045, 763 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][4] = { 2476, 229, 2742 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][5] = { 2420, 672, 614 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][6] = { 725, 63, 2718 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_709][7] = { 534, 561, 509 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][0] = { 3013, 3099, 2935 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][1] = { 2970, 3174, 1091 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][2] = { 1871, 3076, 2913 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][3] = { 1791, 3152, 1013 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][4] = { 2632, 468, 2876 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][5] = { 2581, 924, 866 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][6] = { 976, 180, 2854 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SRGB][7] = { 786, 813, 762 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][0] = { 2990, 3077, 2912 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][1] = { 2947, 3153, 1119 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][2] = { 1859, 3053, 2889 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][3] = { 1782, 3130, 1047 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][4] = { 2608, 556, 2852 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][5] = { 2557, 964, 912 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][6] = { 1013, 309, 2830 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_ADOBERGB][7] = { 839, 864, 817 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][0] = { 2879, 2975, 2793 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][1] = { 2832, 3059, 806 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][2] = { 1629, 2949, 2768 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][3] = { 1543, 3033, 725 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][4] = { 2457, 203, 2727 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][5] = { 2401, 633, 574 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][6] = { 687, 56, 2702 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_SMPTE240M][7] = { 493, 521, 469 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][0] = { 2060, 2194, 1943 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][1] = { 1995, 2314, 237 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][2] = { 725, 2157, 1911 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][3] = { 660, 2278, 205 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][4] = { 1525, 50, 1857 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][5] = { 1461, 171, 151 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][6] = { 190, 14, 1825 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][V4L2_XFER_FUNC_NONE][7] = { 126, 134, 118 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][1] = { 2939, 2939, 464 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][2] = { 786, 2939, 2939 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][3] = { 786, 2939, 464 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][4] = { 2879, 547, 2956 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][5] = { 2879, 547, 547 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][6] = { 547, 547, 2956 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][1] = { 3056, 3056, 717 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][2] = { 1036, 3056, 3056 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][3] = { 1036, 3056, 717 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][4] = { 3001, 800, 3071 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][5] = { 3001, 800, 799 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][6] = { 800, 800, 3071 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 799 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][1] = { 3033, 3033, 776 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][2] = { 1068, 3033, 3033 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][3] = { 1068, 3033, 776 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][4] = { 2977, 851, 3048 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][5] = { 2977, 851, 851 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][6] = { 851, 851, 3048 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][1] = { 2926, 2926, 423 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][2] = { 749, 2926, 2926 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][3] = { 749, 2926, 423 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][4] = { 2865, 507, 2943 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][5] = { 2865, 507, 507 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][6] = { 507, 507, 2943 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][1] = { 2125, 2125, 106 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][2] = { 214, 2125, 2125 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][3] = { 214, 2125, 106 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][4] = { 2041, 130, 2149 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][5] = { 2041, 130, 130 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][6] = { 130, 130, 2149 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][1] = { 2939, 2939, 547 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][2] = { 547, 2939, 2939 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][3] = { 547, 2939, 547 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][4] = { 2939, 547, 2939 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][5] = { 2939, 547, 547 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][6] = { 547, 547, 2939 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][1] = { 3056, 3056, 800 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][2] = { 800, 3056, 3056 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][3] = { 800, 3056, 800 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][4] = { 3056, 800, 3056 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][5] = { 3056, 800, 800 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][6] = { 800, 800, 3056 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 800 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][1] = { 3033, 3033, 851 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][2] = { 851, 3033, 3033 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][3] = { 851, 3033, 851 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][4] = { 3033, 851, 3033 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][5] = { 3033, 851, 851 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][6] = { 851, 851, 3033 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][1] = { 2926, 2926, 507 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][2] = { 507, 2926, 2926 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][3] = { 507, 2926, 507 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][4] = { 2926, 507, 2926 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][5] = { 2926, 507, 507 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][6] = { 507, 507, 2926 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][1] = { 2125, 2125, 130 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][2] = { 130, 2125, 2125 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][3] = { 130, 2125, 130 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][4] = { 2125, 130, 2125 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][5] = { 2125, 130, 130 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][6] = { 130, 130, 2125 },
+	[V4L2_COLORSPACE_SRGB][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][1] = { 2939, 2939, 781 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][2] = { 1622, 2939, 2939 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][3] = { 1622, 2939, 781 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][4] = { 2502, 547, 2881 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][5] = { 2502, 547, 547 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][6] = { 547, 547, 2881 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][1] = { 3056, 3056, 1031 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][2] = { 1838, 3056, 3056 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][3] = { 1838, 3056, 1031 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][4] = { 2657, 800, 3002 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][5] = { 2657, 800, 800 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][6] = { 800, 800, 3002 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 800 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][1] = { 3033, 3033, 1063 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][2] = { 1828, 3033, 3033 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][3] = { 1828, 3033, 1063 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][4] = { 2633, 851, 2979 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][5] = { 2633, 851, 851 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][6] = { 851, 851, 2979 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][1] = { 2926, 2926, 744 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][2] = { 1594, 2926, 2926 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][3] = { 1594, 2926, 744 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][4] = { 2484, 507, 2867 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][5] = { 2484, 507, 507 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][6] = { 507, 507, 2867 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][1] = { 2125, 2125, 212 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][2] = { 698, 2125, 2125 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][3] = { 698, 2125, 212 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][4] = { 1557, 130, 2043 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][5] = { 1557, 130, 130 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][6] = { 130, 130, 2043 },
+	[V4L2_COLORSPACE_ADOBERGB][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][1] = { 2877, 2923, 1058 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][2] = { 1837, 2840, 2916 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][3] = { 1734, 2823, 993 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][4] = { 2427, 961, 2812 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][5] = { 2351, 912, 648 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][6] = { 792, 618, 2788 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][1] = { 2999, 3041, 1301 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][2] = { 2040, 2965, 3034 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][3] = { 1944, 2950, 1238 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][4] = { 2587, 1207, 2940 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][5] = { 2517, 1159, 900 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][6] = { 1042, 870, 2917 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SRGB][7] = { 800, 800, 800 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][0] = { 3033, 3033, 3033 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][1] = { 2976, 3018, 1315 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][2] = { 2024, 2942, 3011 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][3] = { 1930, 2926, 1256 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][4] = { 2563, 1227, 2916 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][5] = { 2494, 1183, 943 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][6] = { 1073, 916, 2894 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_ADOBERGB][7] = { 851, 851, 851 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][1] = { 2864, 2910, 1024 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][2] = { 1811, 2826, 2903 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][3] = { 1707, 2809, 958 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][4] = { 2408, 926, 2798 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][5] = { 2331, 876, 609 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][6] = { 755, 579, 2773 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][0] = { 2125, 2125, 2125 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][1] = { 2039, 2102, 338 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][2] = { 873, 1987, 2092 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][3] = { 787, 1965, 305 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][4] = { 1468, 290, 1949 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][5] = { 1382, 268, 162 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][6] = { 216, 152, 1917 },
+	[V4L2_COLORSPACE_BT2020][V4L2_XFER_FUNC_NONE][7] = { 130, 130, 130 },
 };
 
 #else
@@ -764,50 +1020,38 @@ static double transfer_srgb_to_rec709(double v)
 	return transfer_rgb_to_rec709(transfer_srgb_to_rgb(v));
 }
 
-static void csc(enum v4l2_colorspace colorspace, double *r, double *g, double *b)
+static void csc(enum v4l2_colorspace colorspace, enum v4l2_xfer_func xfer_func,
+		double *r, double *g, double *b)
 {
 	int clamp = 1;
 
+	*r = transfer_srgb_to_rgb(*r);
+	*g = transfer_srgb_to_rgb(*g);
+	*b = transfer_srgb_to_rgb(*b);
+
 	/* Convert the primaries of Rec. 709 Linear RGB */
 	switch (colorspace) {
 	case V4L2_COLORSPACE_SMPTE240M:
-		*r = transfer_srgb_to_rgb(*r);
-		*g = transfer_srgb_to_rgb(*g);
-		*b = transfer_srgb_to_rgb(*b);
 		mult_matrix(r, g, b, rec709_to_240m);
 		break;
 	case V4L2_COLORSPACE_SMPTE170M:
-		*r = transfer_srgb_to_rgb(*r);
-		*g = transfer_srgb_to_rgb(*g);
-		*b = transfer_srgb_to_rgb(*b);
 		mult_matrix(r, g, b, rec709_to_170m);
 		break;
 	case V4L2_COLORSPACE_470_SYSTEM_BG:
-		*r = transfer_srgb_to_rgb(*r);
-		*g = transfer_srgb_to_rgb(*g);
-		*b = transfer_srgb_to_rgb(*b);
 		mult_matrix(r, g, b, rec709_to_ebu);
 		break;
 	case V4L2_COLORSPACE_470_SYSTEM_M:
-		*r = transfer_srgb_to_rgb(*r);
-		*g = transfer_srgb_to_rgb(*g);
-		*b = transfer_srgb_to_rgb(*b);
 		mult_matrix(r, g, b, rec709_to_ntsc1953);
 		break;
 	case V4L2_COLORSPACE_ADOBERGB:
-		*r = transfer_srgb_to_rgb(*r);
-		*g = transfer_srgb_to_rgb(*g);
-		*b = transfer_srgb_to_rgb(*b);
 		mult_matrix(r, g, b, rec709_to_adobergb);
 		break;
 	case V4L2_COLORSPACE_BT2020:
-		*r = transfer_srgb_to_rgb(*r);
-		*g = transfer_srgb_to_rgb(*g);
-		*b = transfer_srgb_to_rgb(*b);
 		mult_matrix(r, g, b, rec709_to_bt2020);
 		break;
 	case V4L2_COLORSPACE_SRGB:
 	case V4L2_COLORSPACE_REC709:
+		break;
 	default:
 		break;
 	}
@@ -818,33 +1062,28 @@ static void csc(enum v4l2_colorspace colorspace, double *r, double *g, double *b
 		*b = ((*b) < 0) ? 0 : (((*b) > 1) ? 1 : (*b));
 	}
 
-	/* Encode to gamma corrected colorspace */
-	switch (colorspace) {
-	case V4L2_COLORSPACE_SMPTE240M:
-		*r = transfer_rgb_to_smpte240m(*r);
-		*g = transfer_rgb_to_smpte240m(*g);
-		*b = transfer_rgb_to_smpte240m(*b);
-		break;
-	case V4L2_COLORSPACE_SMPTE170M:
-	case V4L2_COLORSPACE_470_SYSTEM_M:
-	case V4L2_COLORSPACE_470_SYSTEM_BG:
-	case V4L2_COLORSPACE_BT2020:
+	switch (xfer_func) {
+	case V4L2_XFER_FUNC_709:
 		*r = transfer_rgb_to_rec709(*r);
 		*g = transfer_rgb_to_rec709(*g);
 		*b = transfer_rgb_to_rec709(*b);
 		break;
-	case V4L2_COLORSPACE_SRGB:
+	case V4L2_XFER_FUNC_SRGB:
+		*r = transfer_rgb_to_srgb(*r);
+		*g = transfer_rgb_to_srgb(*g);
+		*b = transfer_rgb_to_srgb(*b);
 		break;
-	case V4L2_COLORSPACE_ADOBERGB:
+	case V4L2_XFER_FUNC_ADOBERGB:
 		*r = transfer_rgb_to_adobergb(*r);
 		*g = transfer_rgb_to_adobergb(*g);
 		*b = transfer_rgb_to_adobergb(*b);
 		break;
-	case V4L2_COLORSPACE_REC709:
-	default:
-		*r = transfer_srgb_to_rec709(*r);
-		*g = transfer_srgb_to_rec709(*g);
-		*b = transfer_srgb_to_rec709(*b);
+	case V4L2_XFER_FUNC_SMPTE240M:
+		*r = transfer_rgb_to_smpte240m(*r);
+		*g = transfer_rgb_to_smpte240m(*g);
+		*b = transfer_rgb_to_smpte240m(*b);
+		break;
+	case V4L2_XFER_FUNC_NONE:
 		break;
 	}
 }
@@ -877,7 +1116,16 @@ int main(int argc, char **argv)
 		"V4L2_COLORSPACE_ADOBERGB",
 		"V4L2_COLORSPACE_BT2020",
 	};
+	static const char * const xfer_func_names[] = {
+		"",
+		"V4L2_XFER_FUNC_709",
+		"V4L2_XFER_FUNC_SRGB",
+		"V4L2_XFER_FUNC_ADOBERGB",
+		"V4L2_XFER_FUNC_SMPTE240M",
+		"V4L2_XFER_FUNC_NONE",
+	};
 	int i;
+	int x;
 	int c;
 
 	printf("/* Generated table */\n");
@@ -905,22 +1153,26 @@ int main(int argc, char **argv)
 	printf("\n};\n\n");
 
 	printf("/* Generated table */\n");
-	printf("const struct color16 tpg_csc_colors[V4L2_COLORSPACE_BT2020 + 1][TPG_COLOR_CSC_BLACK + 1] = {\n");
+	printf("const struct color16 tpg_csc_colors[V4L2_COLORSPACE_BT2020 + 1][V4L2_XFER_FUNC_NONE + 1][TPG_COLOR_CSC_BLACK + 1] = {\n");
 	for (c = 0; c <= V4L2_COLORSPACE_BT2020; c++) {
-		for (i = 0; i <= TPG_COLOR_CSC_BLACK; i++) {
-			double r, g, b;
+		for (x = 1; x <= V4L2_XFER_FUNC_NONE; x++) {
+			for (i = 0; i <= TPG_COLOR_CSC_BLACK; i++) {
+				double r, g, b;
 
-			if (colorspaces[c] == 0)
-				continue;
+				if (colorspaces[c] == 0)
+					continue;
 
-			r = tpg_colors[i].r / 255.0;
-			g = tpg_colors[i].g / 255.0;
-			b = tpg_colors[i].b / 255.0;
+				r = tpg_colors[i].r / 255.0;
+				g = tpg_colors[i].g / 255.0;
+				b = tpg_colors[i].b / 255.0;
 
-			csc(c, &r, &g, &b);
+				csc(c, x, &r, &g, &b);
 
-			printf("\t[%s][%d] = { %d, %d, %d },\n", colorspace_names[c], i,
-				(int)(r * 4080), (int)(g * 4080), (int)(b * 4080));
+				printf("\t[%s][%s][%d] = { %d, %d, %d },\n",
+					colorspace_names[c],
+					xfer_func_names[x], i,
+					(int)(r * 4080), (int)(g * 4080), (int)(b * 4080));
+			}
 		}
 	}
 	printf("};\n\n");
diff --git a/drivers/media/platform/vivid/vivid-tpg-colors.h b/drivers/media/platform/vivid/vivid-tpg-colors.h
index 2c333356451c..86b8bf3fe745 100644
--- a/drivers/media/platform/vivid/vivid-tpg-colors.h
+++ b/drivers/media/platform/vivid/vivid-tpg-colors.h
@@ -61,6 +61,8 @@ enum tpg_color {
 extern const struct color tpg_colors[TPG_COLOR_MAX];
 extern const unsigned short tpg_rec709_to_linear[255 * 16 + 1];
 extern const unsigned short tpg_linear_to_rec709[255 * 16 + 1];
-extern const struct color16 tpg_csc_colors[V4L2_COLORSPACE_BT2020 + 1][TPG_COLOR_CSC_BLACK + 1];
+extern const struct color16 tpg_csc_colors[V4L2_COLORSPACE_BT2020 + 1]
+					  [V4L2_XFER_FUNC_NONE + 1]
+					  [TPG_COLOR_CSC_BLACK + 1];
 
 #endif
diff --git a/drivers/media/platform/vivid/vivid-tpg.c b/drivers/media/platform/vivid/vivid-tpg.c
index cb766eb154e7..1458c7955547 100644
--- a/drivers/media/platform/vivid/vivid-tpg.c
+++ b/drivers/media/platform/vivid/vivid-tpg.c
@@ -220,6 +220,8 @@ bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc)
 	case V4L2_PIX_FMT_ARGB32:
 	case V4L2_PIX_FMT_ABGR32:
 	case V4L2_PIX_FMT_GREY:
+	case V4L2_PIX_FMT_Y16:
+	case V4L2_PIX_FMT_Y16_BE:
 		tpg->is_yuv = false;
 		break;
 	case V4L2_PIX_FMT_YUV444:
@@ -292,6 +294,7 @@ bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc)
 	}
 
 	switch (fourcc) {
+	case V4L2_PIX_FMT_GREY:
 	case V4L2_PIX_FMT_RGB332:
 		tpg->twopixelsize[0] = 2;
 		break;
@@ -313,6 +316,8 @@ bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc)
 	case V4L2_PIX_FMT_YUV444:
 	case V4L2_PIX_FMT_YUV555:
 	case V4L2_PIX_FMT_YUV565:
+	case V4L2_PIX_FMT_Y16:
+	case V4L2_PIX_FMT_Y16_BE:
 		tpg->twopixelsize[0] = 2 * 2;
 		break;
 	case V4L2_PIX_FMT_RGB24:
@@ -329,9 +334,6 @@ bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc)
 	case V4L2_PIX_FMT_YUV32:
 		tpg->twopixelsize[0] = 2 * 4;
 		break;
-	case V4L2_PIX_FMT_GREY:
-		tpg->twopixelsize[0] = 2;
-		break;
 	case V4L2_PIX_FMT_NV12:
 	case V4L2_PIX_FMT_NV21:
 	case V4L2_PIX_FMT_NV12M:
@@ -479,44 +481,71 @@ static void color_to_ycbcr(struct tpg_data *tpg, int r, int g, int b,
 		{ COEFF(-0.116, 224), COEFF(-0.384, 224), COEFF(0.5, 224)    },
 		{ COEFF(0.5, 224),    COEFF(-0.445, 224), COEFF(-0.055, 224) },
 	};
+	static const int smpte240m_full[3][3] = {
+		{ COEFF(0.212, 255),  COEFF(0.701, 255),  COEFF(0.087, 255)  },
+		{ COEFF(-0.116, 255), COEFF(-0.384, 255), COEFF(0.5, 255)    },
+		{ COEFF(0.5, 255),    COEFF(-0.445, 255), COEFF(-0.055, 255) },
+	};
 	static const int bt2020[3][3] = {
 		{ COEFF(0.2627, 219),  COEFF(0.6780, 219),  COEFF(0.0593, 219)  },
 		{ COEFF(-0.1396, 224), COEFF(-0.3604, 224), COEFF(0.5, 224)     },
 		{ COEFF(0.5, 224),     COEFF(-0.4598, 224), COEFF(-0.0402, 224) },
 	};
+	static const int bt2020_full[3][3] = {
+		{ COEFF(0.2627, 255),  COEFF(0.6780, 255),  COEFF(0.0593, 255)  },
+		{ COEFF(-0.1396, 255), COEFF(-0.3604, 255), COEFF(0.5, 255)     },
+		{ COEFF(0.5, 255),     COEFF(-0.4698, 255), COEFF(-0.0402, 255) },
+	};
+	static const int bt2020c[4] = {
+		COEFF(1.0 / 1.9404, 224), COEFF(1.0 / 1.5816, 224),
+		COEFF(1.0 / 1.7184, 224), COEFF(1.0 / 0.9936, 224),
+	};
+	static const int bt2020c_full[4] = {
+		COEFF(1.0 / 1.9404, 255), COEFF(1.0 / 1.5816, 255),
+		COEFF(1.0 / 1.7184, 255), COEFF(1.0 / 0.9936, 255),
+	};
+
 	bool full = tpg->real_quantization == V4L2_QUANTIZATION_FULL_RANGE;
 	unsigned y_offset = full ? 0 : 16;
 	int lin_y, yc;
 
 	switch (tpg->real_ycbcr_enc) {
 	case V4L2_YCBCR_ENC_601:
-	case V4L2_YCBCR_ENC_XV601:
 	case V4L2_YCBCR_ENC_SYCC:
 		rgb2ycbcr(full ? bt601_full : bt601, r, g, b, y_offset, y, cb, cr);
 		break;
+	case V4L2_YCBCR_ENC_XV601:
+		/* Ignore quantization range, there is only one possible
+		 * Y'CbCr encoding. */
+		rgb2ycbcr(bt601, r, g, b, 16, y, cb, cr);
+		break;
+	case V4L2_YCBCR_ENC_XV709:
+		/* Ignore quantization range, there is only one possible
+		 * Y'CbCr encoding. */
+		rgb2ycbcr(rec709, r, g, b, 16, y, cb, cr);
+		break;
 	case V4L2_YCBCR_ENC_BT2020:
-		rgb2ycbcr(bt2020, r, g, b, 16, y, cb, cr);
+		rgb2ycbcr(full ? bt2020_full : bt2020, r, g, b, y_offset, y, cb, cr);
 		break;
 	case V4L2_YCBCR_ENC_BT2020_CONST_LUM:
 		lin_y = (COEFF(0.2627, 255) * rec709_to_linear(r) +
 			 COEFF(0.6780, 255) * rec709_to_linear(g) +
 			 COEFF(0.0593, 255) * rec709_to_linear(b)) >> 16;
 		yc = linear_to_rec709(lin_y);
-		*y = (yc * 219) / 255 + (16 << 4);
+		*y = full ? yc : (yc * 219) / 255 + (16 << 4);
 		if (b <= yc)
-			*cb = (((b - yc) * COEFF(1.0 / 1.9404, 224)) >> 16) + (128 << 4);
+			*cb = (((b - yc) * (full ? bt2020c_full[0] : bt2020c[0])) >> 16) + (128 << 4);
 		else
-			*cb = (((b - yc) * COEFF(1.0 / 1.5816, 224)) >> 16) + (128 << 4);
+			*cb = (((b - yc) * (full ? bt2020c_full[1] : bt2020c[1])) >> 16) + (128 << 4);
 		if (r <= yc)
-			*cr = (((r - yc) * COEFF(1.0 / 1.7184, 224)) >> 16) + (128 << 4);
+			*cr = (((r - yc) * (full ? bt2020c_full[2] : bt2020c[2])) >> 16) + (128 << 4);
 		else
-			*cr = (((r - yc) * COEFF(1.0 / 0.9936, 224)) >> 16) + (128 << 4);
+			*cr = (((r - yc) * (full ? bt2020c_full[3] : bt2020c[3])) >> 16) + (128 << 4);
 		break;
 	case V4L2_YCBCR_ENC_SMPTE240M:
-		rgb2ycbcr(smpte240m, r, g, b, 16, y, cb, cr);
+		rgb2ycbcr(full ? smpte240m_full : smpte240m, r, g, b, y_offset, y, cb, cr);
 		break;
 	case V4L2_YCBCR_ENC_709:
-	case V4L2_YCBCR_ENC_XV709:
 	default:
 		rgb2ycbcr(full ? rec709_full : rec709, r, g, b, y_offset, y, cb, cr);
 		break;
@@ -567,42 +596,71 @@ static void ycbcr_to_color(struct tpg_data *tpg, int y, int cb, int cr,
 		{ COEFF(1, 219), COEFF(-0.2253, 224), COEFF(-0.4767, 224) },
 		{ COEFF(1, 219), COEFF(1.8270, 224),  COEFF(0, 224)       },
 	};
+	static const int smpte240m_full[3][3] = {
+		{ COEFF(1, 255), COEFF(0, 255),       COEFF(1.5756, 255)  },
+		{ COEFF(1, 255), COEFF(-0.2253, 255), COEFF(-0.4767, 255) },
+		{ COEFF(1, 255), COEFF(1.8270, 255),  COEFF(0, 255)       },
+	};
 	static const int bt2020[3][3] = {
 		{ COEFF(1, 219), COEFF(0, 224),       COEFF(1.4746, 224)  },
 		{ COEFF(1, 219), COEFF(-0.1646, 224), COEFF(-0.5714, 224) },
 		{ COEFF(1, 219), COEFF(1.8814, 224),  COEFF(0, 224)       },
 	};
+	static const int bt2020_full[3][3] = {
+		{ COEFF(1, 255), COEFF(0, 255),       COEFF(1.4746, 255)  },
+		{ COEFF(1, 255), COEFF(-0.1646, 255), COEFF(-0.5714, 255) },
+		{ COEFF(1, 255), COEFF(1.8814, 255),  COEFF(0, 255)       },
+	};
+	static const int bt2020c[4] = {
+		COEFF(1.9404, 224), COEFF(1.5816, 224),
+		COEFF(1.7184, 224), COEFF(0.9936, 224),
+	};
+	static const int bt2020c_full[4] = {
+		COEFF(1.9404, 255), COEFF(1.5816, 255),
+		COEFF(1.7184, 255), COEFF(0.9936, 255),
+	};
+
 	bool full = tpg->real_quantization == V4L2_QUANTIZATION_FULL_RANGE;
 	unsigned y_offset = full ? 0 : 16;
+	int y_fac = full ? COEFF(1.0, 255) : COEFF(1.0, 219);
 	int lin_r, lin_g, lin_b, lin_y;
 
 	switch (tpg->real_ycbcr_enc) {
 	case V4L2_YCBCR_ENC_601:
-	case V4L2_YCBCR_ENC_XV601:
 	case V4L2_YCBCR_ENC_SYCC:
 		ycbcr2rgb(full ? bt601_full : bt601, y, cb, cr, y_offset, r, g, b);
 		break;
+	case V4L2_YCBCR_ENC_XV601:
+		/* Ignore quantization range, there is only one possible
+		 * Y'CbCr encoding. */
+		ycbcr2rgb(bt601, y, cb, cr, 16, r, g, b);
+		break;
+	case V4L2_YCBCR_ENC_XV709:
+		/* Ignore quantization range, there is only one possible
+		 * Y'CbCr encoding. */
+		ycbcr2rgb(rec709, y, cb, cr, 16, r, g, b);
+		break;
 	case V4L2_YCBCR_ENC_BT2020:
-		ycbcr2rgb(bt2020, y, cb, cr, 16, r, g, b);
+		ycbcr2rgb(full ? bt2020_full : bt2020, y, cb, cr, y_offset, r, g, b);
 		break;
 	case V4L2_YCBCR_ENC_BT2020_CONST_LUM:
-		y -= 16 << 4;
+		y -= full ? 0 : 16 << 4;
 		cb -= 128 << 4;
 		cr -= 128 << 4;
 
 		if (cb <= 0)
-			*b = COEFF(1.0, 219) * y + COEFF(1.9404, 224) * cb;
+			*b = y_fac * y + (full ? bt2020c_full[0] : bt2020c[0]) * cb;
 		else
-			*b = COEFF(1.0, 219) * y + COEFF(1.5816, 224) * cb;
+			*b = y_fac * y + (full ? bt2020c_full[1] : bt2020c[1]) * cb;
 		*b = *b >> 12;
 		if (cr <= 0)
-			*r = COEFF(1.0, 219) * y + COEFF(1.7184, 224) * cr;
+			*r = y_fac * y + (full ? bt2020c_full[2] : bt2020c[2]) * cr;
 		else
-			*r = COEFF(1.0, 219) * y + COEFF(0.9936, 224) * cr;
+			*r = y_fac * y + (full ? bt2020c_full[3] : bt2020c[3]) * cr;
 		*r = *r >> 12;
 		lin_r = rec709_to_linear(*r);
 		lin_b = rec709_to_linear(*b);
-		lin_y = rec709_to_linear((y * 255) / 219);
+		lin_y = rec709_to_linear((y * 255) / (full ? 255 : 219));
 
 		lin_g = COEFF(1.0 / 0.6780, 255) * lin_y -
 			COEFF(0.2627 / 0.6780, 255) * lin_r -
@@ -610,10 +668,9 @@ static void ycbcr_to_color(struct tpg_data *tpg, int y, int cb, int cr,
 		*g = linear_to_rec709(lin_g >> 12);
 		break;
 	case V4L2_YCBCR_ENC_SMPTE240M:
-		ycbcr2rgb(smpte240m, y, cb, cr, 16, r, g, b);
+		ycbcr2rgb(full ? smpte240m_full : smpte240m, y, cb, cr, y_offset, r, g, b);
 		break;
 	case V4L2_YCBCR_ENC_709:
-	case V4L2_YCBCR_ENC_XV709:
 	default:
 		ycbcr2rgb(full ? rec709_full : rec709, y, cb, cr, y_offset, r, g, b);
 		break;
@@ -649,15 +706,17 @@ static void precalculate_color(struct tpg_data *tpg, int k)
 	}
 
 	if (tpg->pattern == TPG_PAT_CSC_COLORBAR && col <= TPG_COLOR_CSC_BLACK) {
-		r = tpg_csc_colors[tpg->colorspace][col].r;
-		g = tpg_csc_colors[tpg->colorspace][col].g;
-		b = tpg_csc_colors[tpg->colorspace][col].b;
+		r = tpg_csc_colors[tpg->colorspace][tpg->real_xfer_func][col].r;
+		g = tpg_csc_colors[tpg->colorspace][tpg->real_xfer_func][col].g;
+		b = tpg_csc_colors[tpg->colorspace][tpg->real_xfer_func][col].b;
 	} else {
 		r <<= 4;
 		g <<= 4;
 		b <<= 4;
 	}
-	if (tpg->qual == TPG_QUAL_GRAY || tpg->fourcc == V4L2_PIX_FMT_GREY) {
+	if (tpg->qual == TPG_QUAL_GRAY || tpg->fourcc == V4L2_PIX_FMT_GREY ||
+	    tpg->fourcc == V4L2_PIX_FMT_Y16 ||
+	    tpg->fourcc == V4L2_PIX_FMT_Y16_BE) {
 		/* Rec. 709 Luma function */
 		/* (0.2126, 0.7152, 0.0722) * (255 * 256) */
 		r = g = b = (13879 * r + 46688 * g + 4713 * b) >> 16;
@@ -840,6 +899,21 @@ static void gen_twopix(struct tpg_data *tpg,
 	case V4L2_PIX_FMT_GREY:
 		buf[0][offset] = r_y;
 		break;
+	case V4L2_PIX_FMT_Y16:
+		/*
+		 * Ideally both bytes should be set to r_y, but then you won't
+		 * be able to detect endian problems. So keep it 0 except for
+		 * the corner case where r_y is 0xff so white really will be
+		 * white (0xffff).
+		 */
+		buf[0][offset] = r_y == 0xff ? r_y : 0;
+		buf[0][offset+1] = r_y;
+		break;
+	case V4L2_PIX_FMT_Y16_BE:
+		/* See comment for V4L2_PIX_FMT_Y16 above */
+		buf[0][offset] = r_y;
+		buf[0][offset+1] = r_y == 0xff ? r_y : 0;
+		break;
 	case V4L2_PIX_FMT_YUV422P:
 	case V4L2_PIX_FMT_YUV420:
 	case V4L2_PIX_FMT_YUV420M:
@@ -1395,42 +1469,10 @@ static void tpg_precalculate_line(struct tpg_data *tpg)
 /* need this to do rgb24 rendering */
 typedef struct { u16 __; u8 _; } __packed x24;
 
-void tpg_gen_text(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
-		  int y, int x, char *text)
-{
-	int line;
-	unsigned step = V4L2_FIELD_HAS_T_OR_B(tpg->field) ? 2 : 1;
-	unsigned div = step;
-	unsigned first = 0;
-	unsigned len = strlen(text);
-	unsigned p;
-
-	if (font8x16 == NULL || basep == NULL)
-		return;
-
-	/* Checks if it is possible to show string */
-	if (y + 16 >= tpg->compose.height || x + 8 >= tpg->compose.width)
-		return;
-
-	if (len > (tpg->compose.width - x) / 8)
-		len = (tpg->compose.width - x) / 8;
-	if (tpg->vflip)
-		y = tpg->compose.height - y - 16;
-	if (tpg->hflip)
-		x = tpg->compose.width - x - 8;
-	y += tpg->compose.top;
-	x += tpg->compose.left;
-	if (tpg->field == V4L2_FIELD_BOTTOM)
-		first = 1;
-	else if (tpg->field == V4L2_FIELD_SEQ_TB || tpg->field == V4L2_FIELD_SEQ_BT)
-		div = 2;
-
-	for (p = 0; p < tpg->planes; p++) {
-		unsigned vdiv = tpg->vdownsampling[p];
-		unsigned hdiv = tpg->hdownsampling[p];
-
-		/* Print text */
 #define PRINTSTR(PIXTYPE) do {	\
+	unsigned vdiv = tpg->vdownsampling[p]; \
+	unsigned hdiv = tpg->hdownsampling[p]; \
+	int line;	\
 	PIXTYPE fg;	\
 	PIXTYPE bg;	\
 	memcpy(&fg, tpg->textfg[p], sizeof(PIXTYPE));	\
@@ -1481,15 +1523,82 @@ void tpg_gen_text(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
 	}	\
 } while (0)
 
+static noinline void tpg_print_str_2(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+			unsigned p, unsigned first, unsigned div, unsigned step,
+			int y, int x, char *text, unsigned len)
+{
+	PRINTSTR(u8);
+}
+
+static noinline void tpg_print_str_4(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+			unsigned p, unsigned first, unsigned div, unsigned step,
+			int y, int x, char *text, unsigned len)
+{
+	PRINTSTR(u16);
+}
+
+static noinline void tpg_print_str_6(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+			unsigned p, unsigned first, unsigned div, unsigned step,
+			int y, int x, char *text, unsigned len)
+{
+	PRINTSTR(x24);
+}
+
+static noinline void tpg_print_str_8(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+			unsigned p, unsigned first, unsigned div, unsigned step,
+			int y, int x, char *text, unsigned len)
+{
+	PRINTSTR(u32);
+}
+
+void tpg_gen_text(const struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+		  int y, int x, char *text)
+{
+	unsigned step = V4L2_FIELD_HAS_T_OR_B(tpg->field) ? 2 : 1;
+	unsigned div = step;
+	unsigned first = 0;
+	unsigned len = strlen(text);
+	unsigned p;
+
+	if (font8x16 == NULL || basep == NULL)
+		return;
+
+	/* Checks if it is possible to show string */
+	if (y + 16 >= tpg->compose.height || x + 8 >= tpg->compose.width)
+		return;
+
+	if (len > (tpg->compose.width - x) / 8)
+		len = (tpg->compose.width - x) / 8;
+	if (tpg->vflip)
+		y = tpg->compose.height - y - 16;
+	if (tpg->hflip)
+		x = tpg->compose.width - x - 8;
+	y += tpg->compose.top;
+	x += tpg->compose.left;
+	if (tpg->field == V4L2_FIELD_BOTTOM)
+		first = 1;
+	else if (tpg->field == V4L2_FIELD_SEQ_TB || tpg->field == V4L2_FIELD_SEQ_BT)
+		div = 2;
+
+	for (p = 0; p < tpg->planes; p++) {
+		/* Print text */
 		switch (tpg->twopixelsize[p]) {
 		case 2:
-			PRINTSTR(u8); break;
+			tpg_print_str_2(tpg, basep, p, first, div, step, y, x,
+					text, len);
+			break;
 		case 4:
-			PRINTSTR(u16); break;
+			tpg_print_str_4(tpg, basep, p, first, div, step, y, x,
+					text, len);
+			break;
 		case 6:
-			PRINTSTR(x24); break;
+			tpg_print_str_6(tpg, basep, p, first, div, step, y, x,
+					text, len);
+			break;
 		case 8:
-			PRINTSTR(u32); break;
+			tpg_print_str_8(tpg, basep, p, first, div, step, y, x,
+					text, len);
+			break;
 		}
 	}
 }
@@ -1583,50 +1692,23 @@ static void tpg_recalc(struct tpg_data *tpg)
 	if (tpg->recalc_colors) {
 		tpg->recalc_colors = false;
 		tpg->recalc_lines = true;
+		tpg->real_xfer_func = tpg->xfer_func;
 		tpg->real_ycbcr_enc = tpg->ycbcr_enc;
 		tpg->real_quantization = tpg->quantization;
-		if (tpg->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT) {
-			switch (tpg->colorspace) {
-			case V4L2_COLORSPACE_REC709:
-				tpg->real_ycbcr_enc = V4L2_YCBCR_ENC_709;
-				break;
-			case V4L2_COLORSPACE_SRGB:
-				tpg->real_ycbcr_enc = V4L2_YCBCR_ENC_SYCC;
-				break;
-			case V4L2_COLORSPACE_BT2020:
-				tpg->real_ycbcr_enc = V4L2_YCBCR_ENC_BT2020;
-				break;
-			case V4L2_COLORSPACE_SMPTE240M:
-				tpg->real_ycbcr_enc = V4L2_YCBCR_ENC_SMPTE240M;
-				break;
-			case V4L2_COLORSPACE_SMPTE170M:
-			case V4L2_COLORSPACE_470_SYSTEM_M:
-			case V4L2_COLORSPACE_470_SYSTEM_BG:
-			case V4L2_COLORSPACE_ADOBERGB:
-			default:
-				tpg->real_ycbcr_enc = V4L2_YCBCR_ENC_601;
-				break;
-			}
-		}
-		if (tpg->quantization == V4L2_QUANTIZATION_DEFAULT) {
-			tpg->real_quantization = V4L2_QUANTIZATION_FULL_RANGE;
-			if (tpg->is_yuv) {
-				switch (tpg->real_ycbcr_enc) {
-				case V4L2_YCBCR_ENC_SYCC:
-				case V4L2_YCBCR_ENC_XV601:
-				case V4L2_YCBCR_ENC_XV709:
-					break;
-				default:
-					tpg->real_quantization =
-						V4L2_QUANTIZATION_LIM_RANGE;
-					break;
-				}
-			} else if (tpg->colorspace == V4L2_COLORSPACE_BT2020) {
-				/* R'G'B' BT.2020 is limited range */
-				tpg->real_quantization =
-					V4L2_QUANTIZATION_LIM_RANGE;
-			}
-		}
+
+		if (tpg->xfer_func == V4L2_XFER_FUNC_DEFAULT)
+			tpg->real_xfer_func =
+				V4L2_MAP_XFER_FUNC_DEFAULT(tpg->colorspace);
+
+		if (tpg->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT)
+			tpg->real_ycbcr_enc =
+				V4L2_MAP_YCBCR_ENC_DEFAULT(tpg->colorspace);
+
+		if (tpg->quantization == V4L2_QUANTIZATION_DEFAULT)
+			tpg->real_quantization =
+				V4L2_MAP_QUANTIZATION_DEFAULT(!tpg->is_yuv,
+					tpg->colorspace, tpg->real_ycbcr_enc);
+
 		tpg_precalculate_colors(tpg);
 	}
 	if (tpg->recalc_square_border) {
@@ -1670,6 +1752,23 @@ static int tpg_pattern_avg(const struct tpg_data *tpg,
 	return -1;
 }
 
+void tpg_log_status(struct tpg_data *tpg)
+{
+	pr_info("tpg source WxH: %ux%u (%s)\n",
+			tpg->src_width, tpg->src_height,
+			tpg->is_yuv ? "YCbCr" : "RGB");
+	pr_info("tpg field: %u\n", tpg->field);
+	pr_info("tpg crop: %ux%u@%dx%d\n", tpg->crop.width, tpg->crop.height,
+			tpg->crop.left, tpg->crop.top);
+	pr_info("tpg compose: %ux%u@%dx%d\n", tpg->compose.width, tpg->compose.height,
+			tpg->compose.left, tpg->compose.top);
+	pr_info("tpg colorspace: %d\n", tpg->colorspace);
+	pr_info("tpg transfer function: %d/%d\n", tpg->xfer_func, tpg->real_xfer_func);
+	pr_info("tpg Y'CbCr encoding: %d/%d\n", tpg->ycbcr_enc, tpg->real_ycbcr_enc);
+	pr_info("tpg quantization: %d/%d\n", tpg->quantization, tpg->real_quantization);
+	pr_info("tpg RGB range: %d/%d\n", tpg->rgb_range, tpg->real_rgb_range);
+}
+
 /*
  * This struct contains common parameters used by both the drawing of the
  * test pattern and the drawing of the extras (borders, square, etc.)
diff --git a/drivers/media/platform/vivid/vivid-tpg.h b/drivers/media/platform/vivid/vivid-tpg.h
index a50cd2e2535b..9baed6a10334 100644
--- a/drivers/media/platform/vivid/vivid-tpg.h
+++ b/drivers/media/platform/vivid/vivid-tpg.h
@@ -122,8 +122,14 @@ struct tpg_data {
 	u32				fourcc;
 	bool				is_yuv;
 	u32				colorspace;
+	u32				xfer_func;
 	u32				ycbcr_enc;
 	/*
+	 * Stores the actual transfer function, i.e. will never be
+	 * V4L2_XFER_FUNC_DEFAULT.
+	 */
+	u32				real_xfer_func;
+	/*
 	 * Stores the actual Y'CbCr encoding, i.e. will never be
 	 * V4L2_YCBCR_ENC_DEFAULT.
 	 */
@@ -192,6 +198,7 @@ int tpg_alloc(struct tpg_data *tpg, unsigned max_w);
 void tpg_free(struct tpg_data *tpg);
 void tpg_reset_source(struct tpg_data *tpg, unsigned width, unsigned height,
 		       u32 field);
+void tpg_log_status(struct tpg_data *tpg);
 
 void tpg_set_font(const u8 *f);
 void tpg_gen_text(const struct tpg_data *tpg,
@@ -328,6 +335,19 @@ static inline u32 tpg_g_ycbcr_enc(const struct tpg_data *tpg)
 	return tpg->ycbcr_enc;
 }
 
+static inline void tpg_s_xfer_func(struct tpg_data *tpg, u32 xfer_func)
+{
+	if (tpg->xfer_func == xfer_func)
+		return;
+	tpg->xfer_func = xfer_func;
+	tpg->recalc_colors = true;
+}
+
+static inline u32 tpg_g_xfer_func(const struct tpg_data *tpg)
+{
+	return tpg->xfer_func;
+}
+
 static inline void tpg_s_quantization(struct tpg_data *tpg, u32 quantization)
 {
 	if (tpg->quantization == quantization)
diff --git a/drivers/media/platform/vivid/vivid-vid-cap.c b/drivers/media/platform/vivid/vivid-vid-cap.c
index dab5990f45a0..c4268d1b2f82 100644
--- a/drivers/media/platform/vivid/vivid-vid-cap.c
+++ b/drivers/media/platform/vivid/vivid-vid-cap.c
@@ -40,7 +40,6 @@ static const struct v4l2_fract
 
 static const struct vivid_fmt formats_ovl[] = {
 	{
-		.name     = "RGB565 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -48,7 +47,6 @@ static const struct vivid_fmt formats_ovl[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "XRGB555 (LE)",
 		.fourcc   = V4L2_PIX_FMT_XRGB555, /* gggbbbbb arrrrrgg */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -56,7 +54,6 @@ static const struct vivid_fmt formats_ovl[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "ARGB555 (LE)",
 		.fourcc   = V4L2_PIX_FMT_ARGB555, /* gggbbbbb arrrrrgg */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -66,7 +63,7 @@ static const struct vivid_fmt formats_ovl[] = {
 };
 
 /* The number of discrete webcam framesizes */
-#define VIVID_WEBCAM_SIZES 3
+#define VIVID_WEBCAM_SIZES 4
 /* The number of discrete webcam frameintervals */
 #define VIVID_WEBCAM_IVALS (VIVID_WEBCAM_SIZES * 2)
 
@@ -75,6 +72,7 @@ static const struct v4l2_frmsize_discrete webcam_sizes[VIVID_WEBCAM_SIZES] = {
 	{  320, 180 },
 	{  640, 360 },
 	{ 1280, 720 },
+	{ 1920, 1080 },
 };
 
 /*
@@ -82,6 +80,8 @@ static const struct v4l2_frmsize_discrete webcam_sizes[VIVID_WEBCAM_SIZES] = {
  * elements in this array as there are in webcam_sizes.
  */
 static const struct v4l2_fract webcam_intervals[VIVID_WEBCAM_IVALS] = {
+	{  1, 2 },
+	{  1, 5 },
 	{  1, 10 },
 	{  1, 15 },
 	{  1, 25 },
@@ -501,6 +501,13 @@ static unsigned vivid_colorspace_cap(struct vivid_dev *dev)
 	return dev->colorspace_out;
 }
 
+static unsigned vivid_xfer_func_cap(struct vivid_dev *dev)
+{
+	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+		return tpg_g_xfer_func(&dev->tpg);
+	return dev->xfer_func_out;
+}
+
 static unsigned vivid_ycbcr_enc_cap(struct vivid_dev *dev)
 {
 	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
@@ -527,6 +534,7 @@ int vivid_g_fmt_vid_cap(struct file *file, void *priv,
 	mp->field        = dev->field_cap;
 	mp->pixelformat  = dev->fmt_cap->fourcc;
 	mp->colorspace   = vivid_colorspace_cap(dev);
+	mp->xfer_func    = vivid_xfer_func_cap(dev);
 	mp->ycbcr_enc    = vivid_ycbcr_enc_cap(dev);
 	mp->quantization = vivid_quantization_cap(dev);
 	mp->num_planes = dev->fmt_cap->buffers;
@@ -616,6 +624,7 @@ int vivid_try_fmt_vid_cap(struct file *file, void *priv,
 	}
 	mp->colorspace = vivid_colorspace_cap(dev);
 	mp->ycbcr_enc = vivid_ycbcr_enc_cap(dev);
+	mp->xfer_func = vivid_xfer_func_cap(dev);
 	mp->quantization = vivid_quantization_cap(dev);
 	memset(mp->reserved, 0, sizeof(mp->reserved));
 	return 0;
@@ -720,8 +729,8 @@ int vivid_s_fmt_vid_cap(struct file *file, void *priv,
 					webcam_sizes[i].height == mp->height)
 				break;
 		dev->webcam_size_idx = i;
-		if (dev->webcam_ival_idx >= 2 * (3 - i))
-			dev->webcam_ival_idx = 2 * (3 - i) - 1;
+		if (dev->webcam_ival_idx >= 2 * (VIVID_WEBCAM_SIZES - i))
+			dev->webcam_ival_idx = 2 * (VIVID_WEBCAM_SIZES - i) - 1;
 		vivid_update_format_cap(dev, false);
 	} else {
 		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
@@ -1030,7 +1039,6 @@ int vidioc_enum_fmt_vid_overlay(struct file *file, void  *priv,
 
 	fmt = &formats_ovl[f->index];
 
-	strlcpy(f->description, fmt->name, sizeof(f->description));
 	f->pixelformat = fmt->fourcc;
 	return 0;
 }
@@ -1620,7 +1628,7 @@ static bool valid_cvt_gtf_timings(struct v4l2_dv_timings *timings)
 
 	if (bt->standards == 0 || (bt->standards & V4L2_DV_BT_STD_CVT)) {
 		if (v4l2_detect_cvt(total_v_lines, h_freq, bt->vsync,
-				    bt->polarities, timings))
+				    bt->polarities, bt->interlaced, timings))
 			return true;
 	}
 
@@ -1631,7 +1639,8 @@ static bool valid_cvt_gtf_timings(struct v4l2_dv_timings *timings)
 				  &aspect_ratio.numerator,
 				  &aspect_ratio.denominator);
 		if (v4l2_detect_gtf(total_v_lines, h_freq, bt->vsync,
-				    bt->polarities, aspect_ratio, timings))
+				    bt->polarities, bt->interlaced,
+				    aspect_ratio, timings))
 			return true;
 	}
 	return false;
@@ -1768,7 +1777,7 @@ int vidioc_enum_frameintervals(struct file *file, void *priv,
 			break;
 	if (i == ARRAY_SIZE(webcam_sizes))
 		return -EINVAL;
-	if (fival->index >= 2 * (3 - i))
+	if (fival->index >= 2 * (VIVID_WEBCAM_SIZES - i))
 		return -EINVAL;
 	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 	fival->discrete = webcam_intervals[fival->index];
@@ -1798,7 +1807,7 @@ int vivid_vid_cap_s_parm(struct file *file, void *priv,
 			  struct v4l2_streamparm *parm)
 {
 	struct vivid_dev *dev = video_drvdata(file);
-	unsigned ival_sz = 2 * (3 - dev->webcam_size_idx);
+	unsigned ival_sz = 2 * (VIVID_WEBCAM_SIZES - dev->webcam_size_idx);
 	struct v4l2_fract tpf;
 	unsigned i;
 
diff --git a/drivers/media/platform/vivid/vivid-vid-common.c b/drivers/media/platform/vivid/vivid-vid-common.c
index aa446271ad34..fc73927a4abc 100644
--- a/drivers/media/platform/vivid/vivid-vid-common.c
+++ b/drivers/media/platform/vivid/vivid-vid-common.c
@@ -45,7 +45,6 @@ const struct v4l2_dv_timings_cap vivid_dv_timings_cap = {
 
 struct vivid_fmt vivid_formats[] = {
 	{
-		.name     = "4:2:2, packed, YUYV",
 		.fourcc   = V4L2_PIX_FMT_YUYV,
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -55,7 +54,6 @@ struct vivid_fmt vivid_formats[] = {
 		.data_offset = { PLANE0_DATA_OFFSET },
 	},
 	{
-		.name     = "4:2:2, packed, UYVY",
 		.fourcc   = V4L2_PIX_FMT_UYVY,
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -64,7 +62,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "4:2:2, packed, YVYU",
 		.fourcc   = V4L2_PIX_FMT_YVYU,
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -73,7 +70,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "4:2:2, packed, VYUY",
 		.fourcc   = V4L2_PIX_FMT_VYUY,
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -82,7 +78,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YUV 4:2:2 triplanar",
 		.fourcc   = V4L2_PIX_FMT_YUV422P,
 		.vdownsampling = { 1, 1, 1 },
 		.bit_depth = { 8, 4, 4 },
@@ -91,7 +86,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YUV 4:2:0 triplanar",
 		.fourcc   = V4L2_PIX_FMT_YUV420,
 		.vdownsampling = { 1, 2, 2 },
 		.bit_depth = { 8, 4, 4 },
@@ -100,7 +94,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YVU 4:2:0 triplanar",
 		.fourcc   = V4L2_PIX_FMT_YVU420,
 		.vdownsampling = { 1, 2, 2 },
 		.bit_depth = { 8, 4, 4 },
@@ -109,7 +102,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YUV 4:2:0 biplanar",
 		.fourcc   = V4L2_PIX_FMT_NV12,
 		.vdownsampling = { 1, 2 },
 		.bit_depth = { 8, 8 },
@@ -118,7 +110,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YVU 4:2:0 biplanar",
 		.fourcc   = V4L2_PIX_FMT_NV21,
 		.vdownsampling = { 1, 2 },
 		.bit_depth = { 8, 8 },
@@ -127,7 +118,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YUV 4:2:2 biplanar",
 		.fourcc   = V4L2_PIX_FMT_NV16,
 		.vdownsampling = { 1, 1 },
 		.bit_depth = { 8, 8 },
@@ -136,7 +126,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YVU 4:2:2 biplanar",
 		.fourcc   = V4L2_PIX_FMT_NV61,
 		.vdownsampling = { 1, 1 },
 		.bit_depth = { 8, 8 },
@@ -145,7 +134,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YUV 4:4:4 biplanar",
 		.fourcc   = V4L2_PIX_FMT_NV24,
 		.vdownsampling = { 1, 1 },
 		.bit_depth = { 8, 16 },
@@ -154,7 +142,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YVU 4:4:4 biplanar",
 		.fourcc   = V4L2_PIX_FMT_NV42,
 		.vdownsampling = { 1, 1 },
 		.bit_depth = { 8, 16 },
@@ -163,7 +150,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YUV555 (LE)",
 		.fourcc   = V4L2_PIX_FMT_YUV555, /* uuuvvvvv ayyyyyuu */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -172,7 +158,6 @@ struct vivid_fmt vivid_formats[] = {
 		.alpha_mask = 0x8000,
 	},
 	{
-		.name     = "YUV565 (LE)",
 		.fourcc   = V4L2_PIX_FMT_YUV565, /* uuuvvvvv yyyyyuuu */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -180,7 +165,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "YUV444",
 		.fourcc   = V4L2_PIX_FMT_YUV444, /* uuuuvvvv aaaayyyy */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -189,7 +173,6 @@ struct vivid_fmt vivid_formats[] = {
 		.alpha_mask = 0xf000,
 	},
 	{
-		.name     = "YUV32 (LE)",
 		.fourcc   = V4L2_PIX_FMT_YUV32, /* ayuv */
 		.vdownsampling = { 1 },
 		.bit_depth = { 32 },
@@ -198,7 +181,6 @@ struct vivid_fmt vivid_formats[] = {
 		.alpha_mask = 0x000000ff,
 	},
 	{
-		.name     = "Monochrome",
 		.fourcc   = V4L2_PIX_FMT_GREY,
 		.vdownsampling = { 1 },
 		.bit_depth = { 8 },
@@ -207,7 +189,22 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "RGB332",
+		.fourcc   = V4L2_PIX_FMT_Y16,
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
+		.fourcc   = V4L2_PIX_FMT_Y16_BE,
+		.vdownsampling = { 1 },
+		.bit_depth = { 16 },
+		.is_yuv   = true,
+		.planes   = 1,
+		.buffers = 1,
+	},
+	{
 		.fourcc   = V4L2_PIX_FMT_RGB332, /* rrrgggbb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 8 },
@@ -215,7 +212,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "RGB565 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -224,7 +220,6 @@ struct vivid_fmt vivid_formats[] = {
 		.can_do_overlay = true,
 	},
 	{
-		.name     = "RGB565 (BE)",
 		.fourcc   = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -233,7 +228,6 @@ struct vivid_fmt vivid_formats[] = {
 		.can_do_overlay = true,
 	},
 	{
-		.name     = "RGB444",
 		.fourcc   = V4L2_PIX_FMT_RGB444, /* xxxxrrrr ggggbbbb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -241,7 +235,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "XRGB444",
 		.fourcc   = V4L2_PIX_FMT_XRGB444, /* xxxxrrrr ggggbbbb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -249,7 +242,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "ARGB444",
 		.fourcc   = V4L2_PIX_FMT_ARGB444, /* aaaarrrr ggggbbbb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -258,7 +250,6 @@ struct vivid_fmt vivid_formats[] = {
 		.alpha_mask = 0x00f0,
 	},
 	{
-		.name     = "RGB555 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB555, /* gggbbbbb xrrrrrgg */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -267,7 +258,6 @@ struct vivid_fmt vivid_formats[] = {
 		.can_do_overlay = true,
 	},
 	{
-		.name     = "XRGB555 (LE)",
 		.fourcc   = V4L2_PIX_FMT_XRGB555, /* gggbbbbb xrrrrrgg */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -276,7 +266,6 @@ struct vivid_fmt vivid_formats[] = {
 		.can_do_overlay = true,
 	},
 	{
-		.name     = "ARGB555 (LE)",
 		.fourcc   = V4L2_PIX_FMT_ARGB555, /* gggbbbbb arrrrrgg */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -286,7 +275,6 @@ struct vivid_fmt vivid_formats[] = {
 		.alpha_mask = 0x8000,
 	},
 	{
-		.name     = "RGB555 (BE)",
 		.fourcc   = V4L2_PIX_FMT_RGB555X, /* xrrrrrgg gggbbbbb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -294,7 +282,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "XRGB555 (BE)",
 		.fourcc   = V4L2_PIX_FMT_XRGB555X, /* xrrrrrgg gggbbbbb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -302,7 +289,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "ARGB555 (BE)",
 		.fourcc   = V4L2_PIX_FMT_ARGB555X, /* arrrrrgg gggbbbbb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 16 },
@@ -311,7 +297,6 @@ struct vivid_fmt vivid_formats[] = {
 		.alpha_mask = 0x0080,
 	},
 	{
-		.name     = "RGB24 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB24, /* rgb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 24 },
@@ -319,7 +304,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "RGB24 (BE)",
 		.fourcc   = V4L2_PIX_FMT_BGR24, /* bgr */
 		.vdownsampling = { 1 },
 		.bit_depth = { 24 },
@@ -327,7 +311,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "BGR666",
 		.fourcc   = V4L2_PIX_FMT_BGR666, /* bbbbbbgg ggggrrrr rrxxxxxx */
 		.vdownsampling = { 1 },
 		.bit_depth = { 32 },
@@ -335,7 +318,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "RGB32 (LE)",
 		.fourcc   = V4L2_PIX_FMT_RGB32, /* xrgb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 32 },
@@ -343,7 +325,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "RGB32 (BE)",
 		.fourcc   = V4L2_PIX_FMT_BGR32, /* bgrx */
 		.vdownsampling = { 1 },
 		.bit_depth = { 32 },
@@ -351,7 +332,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "XRGB32 (LE)",
 		.fourcc   = V4L2_PIX_FMT_XRGB32, /* xrgb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 32 },
@@ -359,7 +339,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "XRGB32 (BE)",
 		.fourcc   = V4L2_PIX_FMT_XBGR32, /* bgrx */
 		.vdownsampling = { 1 },
 		.bit_depth = { 32 },
@@ -367,7 +346,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "ARGB32 (LE)",
 		.fourcc   = V4L2_PIX_FMT_ARGB32, /* argb */
 		.vdownsampling = { 1 },
 		.bit_depth = { 32 },
@@ -376,7 +354,6 @@ struct vivid_fmt vivid_formats[] = {
 		.alpha_mask = 0x000000ff,
 	},
 	{
-		.name     = "ARGB32 (BE)",
 		.fourcc   = V4L2_PIX_FMT_ABGR32, /* bgra */
 		.vdownsampling = { 1 },
 		.bit_depth = { 32 },
@@ -385,7 +362,6 @@ struct vivid_fmt vivid_formats[] = {
 		.alpha_mask = 0xff000000,
 	},
 	{
-		.name     = "Bayer BG/GR",
 		.fourcc   = V4L2_PIX_FMT_SBGGR8, /* Bayer BG/GR */
 		.vdownsampling = { 1 },
 		.bit_depth = { 8 },
@@ -393,7 +369,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "Bayer GB/RG",
 		.fourcc   = V4L2_PIX_FMT_SGBRG8, /* Bayer GB/RG */
 		.vdownsampling = { 1 },
 		.bit_depth = { 8 },
@@ -401,7 +376,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "Bayer GR/BG",
 		.fourcc   = V4L2_PIX_FMT_SGRBG8, /* Bayer GR/BG */
 		.vdownsampling = { 1 },
 		.bit_depth = { 8 },
@@ -409,7 +383,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "Bayer RG/GB",
 		.fourcc   = V4L2_PIX_FMT_SRGGB8, /* Bayer RG/GB */
 		.vdownsampling = { 1 },
 		.bit_depth = { 8 },
@@ -417,7 +390,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 1,
 	},
 	{
-		.name     = "4:2:2, biplanar, YUV",
 		.fourcc   = V4L2_PIX_FMT_NV16M,
 		.vdownsampling = { 1, 1 },
 		.bit_depth = { 8, 8 },
@@ -427,7 +399,6 @@ struct vivid_fmt vivid_formats[] = {
 		.data_offset = { PLANE0_DATA_OFFSET, 0 },
 	},
 	{
-		.name     = "4:2:2, biplanar, YVU",
 		.fourcc   = V4L2_PIX_FMT_NV61M,
 		.vdownsampling = { 1, 1 },
 		.bit_depth = { 8, 8 },
@@ -437,7 +408,6 @@ struct vivid_fmt vivid_formats[] = {
 		.data_offset = { 0, PLANE0_DATA_OFFSET },
 	},
 	{
-		.name     = "4:2:0, triplanar, YUV",
 		.fourcc   = V4L2_PIX_FMT_YUV420M,
 		.vdownsampling = { 1, 2, 2 },
 		.bit_depth = { 8, 4, 4 },
@@ -446,7 +416,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 3,
 	},
 	{
-		.name     = "4:2:0, triplanar, YVU",
 		.fourcc   = V4L2_PIX_FMT_YVU420M,
 		.vdownsampling = { 1, 2, 2 },
 		.bit_depth = { 8, 4, 4 },
@@ -455,7 +424,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 3,
 	},
 	{
-		.name     = "4:2:0, biplanar, YUV",
 		.fourcc   = V4L2_PIX_FMT_NV12M,
 		.vdownsampling = { 1, 2 },
 		.bit_depth = { 8, 8 },
@@ -464,7 +432,6 @@ struct vivid_fmt vivid_formats[] = {
 		.buffers = 2,
 	},
 	{
-		.name     = "4:2:0, biplanar, YVU",
 		.fourcc   = V4L2_PIX_FMT_NV21M,
 		.vdownsampling = { 1, 2 },
 		.bit_depth = { 8, 8 },
@@ -557,6 +524,7 @@ void fmt_sp2mp(const struct v4l2_format *sp_fmt, struct v4l2_format *mp_fmt)
 	mp->pixelformat = pix->pixelformat;
 	mp->field = pix->field;
 	mp->colorspace = pix->colorspace;
+	mp->xfer_func = pix->xfer_func;
 	mp->ycbcr_enc = pix->ycbcr_enc;
 	mp->quantization = pix->quantization;
 	mp->num_planes = 1;
@@ -585,6 +553,7 @@ int fmt_sp2mp_func(struct file *file, void *priv,
 	pix->pixelformat = mp->pixelformat;
 	pix->field = mp->field;
 	pix->colorspace = mp->colorspace;
+	pix->xfer_func = mp->xfer_func;
 	pix->ycbcr_enc = mp->ycbcr_enc;
 	pix->quantization = mp->quantization;
 	pix->sizeimage = ppix->sizeimage;
@@ -750,7 +719,6 @@ int vivid_enum_fmt_vid(struct file *file, void  *priv,
 
 	fmt = &vivid_formats[f->index];
 
-	strlcpy(f->description, fmt->name, sizeof(f->description));
 	f->pixelformat = fmt->fourcc;
 	return 0;
 }
diff --git a/drivers/media/platform/vivid/vivid-vid-out.c b/drivers/media/platform/vivid/vivid-vid-out.c
index 0af43dc7715c..0862c1f24f57 100644
--- a/drivers/media/platform/vivid/vivid-vid-out.c
+++ b/drivers/media/platform/vivid/vivid-vid-out.c
@@ -258,6 +258,7 @@ void vivid_update_format_out(struct vivid_dev *dev)
 		}
 		break;
 	}
+	dev->xfer_func_out = V4L2_XFER_FUNC_DEFAULT;
 	dev->ycbcr_enc_out = V4L2_YCBCR_ENC_DEFAULT;
 	dev->quantization_out = V4L2_QUANTIZATION_DEFAULT;
 	dev->compose_out = dev->sink_rect;
@@ -320,6 +321,7 @@ int vivid_g_fmt_vid_out(struct file *file, void *priv,
 	mp->field        = dev->field_out;
 	mp->pixelformat  = fmt->fourcc;
 	mp->colorspace   = dev->colorspace_out;
+	mp->xfer_func    = dev->xfer_func_out;
 	mp->ycbcr_enc    = dev->ycbcr_enc_out;
 	mp->quantization = dev->quantization_out;
 	mp->num_planes = fmt->buffers;
@@ -407,6 +409,7 @@ int vivid_try_fmt_vid_out(struct file *file, void *priv,
 	for (p = fmt->buffers; p < fmt->planes; p++)
 		pfmt[0].sizeimage += (pfmt[0].bytesperline * fmt->bit_depth[p]) /
 				     (fmt->bit_depth[0] * fmt->vdownsampling[p]);
+	mp->xfer_func = V4L2_XFER_FUNC_DEFAULT;
 	mp->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
 	mp->quantization = V4L2_QUANTIZATION_DEFAULT;
 	if (vivid_is_svid_out(dev)) {
@@ -546,6 +549,7 @@ int vivid_s_fmt_vid_out(struct file *file, void *priv,
 
 set_colorspace:
 	dev->colorspace_out = mp->colorspace;
+	dev->xfer_func_out = mp->xfer_func;
 	dev->ycbcr_enc_out = mp->ycbcr_enc;
 	dev->quantization_out = mp->quantization;
 	if (dev->loop_video) {
@@ -1152,7 +1156,8 @@ int vivid_vid_out_g_parm(struct file *file, void *priv,
 	parm->parm.output.capability   = V4L2_CAP_TIMEPERFRAME;
 	parm->parm.output.timeperframe = dev->timeperframe_vid_out;
 	parm->parm.output.writebuffers  = 1;
-return 0;
+
+	return 0;
 }
 
 int vidioc_subscribe_event(struct v4l2_fh *fh,
diff --git a/drivers/media/platform/xilinx/Kconfig b/drivers/media/platform/xilinx/Kconfig
index d7324c726fc2..84bae795b70d 100644
--- a/drivers/media/platform/xilinx/Kconfig
+++ b/drivers/media/platform/xilinx/Kconfig
@@ -1,6 +1,6 @@
 config VIDEO_XILINX
 	tristate "Xilinx Video IP (EXPERIMENTAL)"
-	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && OF
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && OF && HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  Driver for Xilinx Video IP Pipelines
diff --git a/drivers/media/platform/xilinx/xilinx-dma.c b/drivers/media/platform/xilinx/xilinx-dma.c
index efde88adf624..98e50e446d57 100644
--- a/drivers/media/platform/xilinx/xilinx-dma.c
+++ b/drivers/media/platform/xilinx/xilinx-dma.c
@@ -653,7 +653,7 @@ static const struct v4l2_file_operations xvip_dma_fops = {
 int xvip_dma_init(struct xvip_composite_device *xdev, struct xvip_dma *dma,
 		  enum v4l2_buf_type type, unsigned int port)
 {
-	char name[14];
+	char name[16];
 	int ret;
 
 	dma->xdev = xdev;
@@ -725,7 +725,7 @@ int xvip_dma_init(struct xvip_composite_device *xdev, struct xvip_dma *dma,
 	}
 
 	/* ... and the DMA channel. */
-	sprintf(name, "port%u", port);
+	snprintf(name, sizeof(name), "port%u", port);
 	dma->dma = dma_request_slave_channel(dma->xdev->dev, name);
 	if (dma->dma == NULL) {
 		dev_err(dma->xdev->dev, "no VDMA channel found\n");
diff --git a/drivers/media/radio/radio-si476x.c b/drivers/media/radio/radio-si476x.c
index dccf58691650..9cbb8cdf0ac0 100644
--- a/drivers/media/radio/radio-si476x.c
+++ b/drivers/media/radio/radio-si476x.c
@@ -568,8 +568,8 @@ static int si476x_radio_do_post_powerup_init(struct si476x_radio *radio,
 	err = regcache_sync_region(radio->core->regmap,
 				   SI476X_PROP_DIGITAL_IO_INPUT_SAMPLE_RATE,
 				   SI476X_PROP_DIGITAL_IO_OUTPUT_FORMAT);
-		if (err < 0)
-			return err;
+	if (err < 0)
+		return err;
 
 	err = regcache_sync_region(radio->core->regmap,
 				   SI476X_PROP_AUDIO_DEEMPHASIS,
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index e6b55edc8f8d..04baafe5e901 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -138,8 +138,10 @@ static int timbradio_probe(struct platform_device *pdev)
 		i2c_get_adapter(pdata->i2c_adapter), pdata->tuner, NULL);
 	tr->sd_dsp = v4l2_i2c_new_subdev_board(&tr->v4l2_dev,
 		i2c_get_adapter(pdata->i2c_adapter), pdata->dsp, NULL);
-	if (tr->sd_tuner == NULL || tr->sd_dsp == NULL)
+	if (tr->sd_tuner == NULL || tr->sd_dsp == NULL) {
+		err = -ENODEV;
 		goto err_video_req;
+	}
 
 	tr->v4l2_dev.ctrl_handler = tr->sd_dsp->ctrl_handler;
 
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index 2a497c80c77f..471d6a8ae8a4 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -384,14 +384,14 @@ static int si470x_i2c_probe(struct i2c_client *client,
 		goto err_radio;
 	}
 	dev_info(&client->dev, "DeviceID=0x%4.4hx ChipID=0x%4.4hx\n",
-			radio->registers[DEVICEID], radio->registers[CHIPID]);
-	if ((radio->registers[CHIPID] & CHIPID_FIRMWARE) < RADIO_FW_VERSION) {
+			radio->registers[DEVICEID], radio->registers[SI_CHIPID]);
+	if ((radio->registers[SI_CHIPID] & SI_CHIPID_FIRMWARE) < RADIO_FW_VERSION) {
 		dev_warn(&client->dev,
 			"This driver is known to work with "
 			"firmware version %hu,\n", RADIO_FW_VERSION);
 		dev_warn(&client->dev,
 			"but the device has firmware version %hu.\n",
-			radio->registers[CHIPID] & CHIPID_FIRMWARE);
+			radio->registers[SI_CHIPID] & SI_CHIPID_FIRMWARE);
 		version_warning = 1;
 	}
 
@@ -421,7 +421,8 @@ static int si470x_i2c_probe(struct i2c_client *client,
 	init_waitqueue_head(&radio->read_queue);
 
 	retval = request_threaded_irq(client->irq, NULL, si470x_i2c_interrupt,
-			IRQF_TRIGGER_FALLING, DRIVER_NAME, radio);
+			IRQF_TRIGGER_FALLING | IRQF_ONESHOT, DRIVER_NAME,
+			radio);
 	if (retval) {
 		dev_err(&client->dev, "Failed to register interrupt\n");
 		goto err_rds;
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index 57f0bc3b60e7..091d793f6583 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -686,14 +686,14 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 		goto err_ctrl;
 	}
 	dev_info(&intf->dev, "DeviceID=0x%4.4hx ChipID=0x%4.4hx\n",
-			radio->registers[DEVICEID], radio->registers[CHIPID]);
-	if ((radio->registers[CHIPID] & CHIPID_FIRMWARE) < RADIO_FW_VERSION) {
+			radio->registers[DEVICEID], radio->registers[SI_CHIPID]);
+	if ((radio->registers[SI_CHIPID] & SI_CHIPID_FIRMWARE) < RADIO_FW_VERSION) {
 		dev_warn(&intf->dev,
 			"This driver is known to work with "
 			"firmware version %hu,\n", RADIO_FW_VERSION);
 		dev_warn(&intf->dev,
 			"but the device has firmware version %hu.\n",
-			radio->registers[CHIPID] & CHIPID_FIRMWARE);
+			radio->registers[SI_CHIPID] & SI_CHIPID_FIRMWARE);
 		version_warning = 1;
 	}
 
diff --git a/drivers/media/radio/si470x/radio-si470x.h b/drivers/media/radio/si470x/radio-si470x.h
index 4b7660470e2f..6c0ca900702e 100644
--- a/drivers/media/radio/si470x/radio-si470x.h
+++ b/drivers/media/radio/si470x/radio-si470x.h
@@ -54,10 +54,10 @@
 #define DEVICEID_PN		0xf000	/* bits 15..12: Part Number */
 #define DEVICEID_MFGID		0x0fff	/* bits 11..00: Manufacturer ID */
 
-#define CHIPID			1	/* Chip ID */
-#define CHIPID_REV		0xfc00	/* bits 15..10: Chip Version */
-#define CHIPID_DEV		0x0200	/* bits 09..09: Device */
-#define CHIPID_FIRMWARE		0x01ff	/* bits 08..00: Firmware Version */
+#define SI_CHIPID		1	/* Chip ID */
+#define SI_CHIPID_REV		0xfc00	/* bits 15..10: Chip Version */
+#define SI_CHIPID_DEV		0x0200	/* bits 09..09: Device */
+#define SI_CHIPID_FIRMWARE	0x01ff	/* bits 08..00: Firmware Version */
 
 #define POWERCFG		2	/* Power Configuration */
 #define POWERCFG_DSMUTE		0x8000	/* bits 15..15: Softmute Disable */
diff --git a/drivers/media/radio/si4713/si4713.c b/drivers/media/radio/si4713/si4713.c
index e9d03ac69a27..0b04b56571da 100644
--- a/drivers/media/radio/si4713/si4713.c
+++ b/drivers/media/radio/si4713/si4713.c
@@ -1609,8 +1609,10 @@ static int si4713_probe(struct i2c_client *client,
 		return 0;
 
 	si4713_pdev = platform_device_alloc("radio-si4713", -1);
-	if (!si4713_pdev)
+	if (!si4713_pdev) {
+		rval = -ENOMEM;
 		goto put_main_pdev;
+	}
 
 	si4713_pdev_pdata.subdev = client;
 	rval = platform_device_add_data(si4713_pdev, &si4713_pdev_pdata,
diff --git a/drivers/media/radio/wl128x/Kconfig b/drivers/media/radio/wl128x/Kconfig
index 9d6574bebf78..c9e349b169c4 100644
--- a/drivers/media/radio/wl128x/Kconfig
+++ b/drivers/media/radio/wl128x/Kconfig
@@ -4,8 +4,8 @@
 menu "Texas Instruments WL128x FM driver (ST based)"
 config RADIO_WL128X
 	tristate "Texas Instruments WL128x FM Radio"
-	depends on VIDEO_V4L2 && RFKILL && GPIOLIB && TTY
-	depends on TI_ST
+	depends on VIDEO_V4L2 && RFKILL && TTY && TI_ST
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	Choose Y here if you have this FM radio chip.
 
diff --git a/drivers/media/radio/wl128x/fmdrv.h b/drivers/media/radio/wl128x/fmdrv.h
index a587c9bac930..dd203de5de95 100644
--- a/drivers/media/radio/wl128x/fmdrv.h
+++ b/drivers/media/radio/wl128x/fmdrv.h
@@ -210,7 +210,7 @@ struct fmdev {
 	spinlock_t resp_skb_lock; /* To protect access to received SKB */
 
 	long flag;		/*  FM driver state machine info */
-	u8 streg_cbdata; /* status of ST registration */
+	int streg_cbdata; /* status of ST registration */
 
 	struct sk_buff_head rx_q;	/* RX queue */
 	struct tasklet_struct rx_task;	/* RX Tasklet */
diff --git a/drivers/media/rc/fintek-cir.c b/drivers/media/rc/fintek-cir.c
index 5c63c2ec6183..bd7b3bdb1a88 100644
--- a/drivers/media/rc/fintek-cir.c
+++ b/drivers/media/rc/fintek-cir.c
@@ -33,7 +33,6 @@
 #include <linux/sched.h>
 #include <linux/slab.h>
 #include <media/rc-core.h>
-#include <linux/pci_ids.h>
 
 #include "fintek-cir.h"
 
diff --git a/drivers/media/rc/gpio-ir-recv.c b/drivers/media/rc/gpio-ir-recv.c
index 229853d68451..7dbc9ca6d885 100644
--- a/drivers/media/rc/gpio-ir-recv.c
+++ b/drivers/media/rc/gpio-ir-recv.c
@@ -59,7 +59,7 @@ static int gpio_ir_recv_get_devtree_pdata(struct device *dev,
 	return 0;
 }
 
-static struct of_device_id gpio_ir_recv_of_match[] = {
+static const struct of_device_id gpio_ir_recv_of_match[] = {
 	{ .compatible = "gpio-ir-receiver", },
 	{ },
 };
@@ -78,7 +78,7 @@ static irqreturn_t gpio_ir_recv_irq(int irq, void *dev_id)
 	int rc = 0;
 	enum raw_event_type type = IR_SPACE;
 
-	gval = gpio_get_value_cansleep(gpio_dev->gpio_nr);
+	gval = gpio_get_value(gpio_dev->gpio_nr);
 
 	if (gval < 0)
 		goto err_get_value;
diff --git a/drivers/media/rc/ir-hix5hd2.c b/drivers/media/rc/ir-hix5hd2.c
index 58ec5986274e..1c087cb76815 100644
--- a/drivers/media/rc/ir-hix5hd2.c
+++ b/drivers/media/rc/ir-hix5hd2.c
@@ -63,7 +63,7 @@
 
 struct hix5hd2_ir_priv {
 	int			irq;
-	void volatile __iomem	*base;
+	void __iomem		*base;
 	struct device		*dev;
 	struct rc_dev		*rdev;
 	struct regmap		*regmap;
@@ -213,8 +213,8 @@ static int hix5hd2_ir_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	priv->base = devm_ioremap_resource(dev, res);
-	if (IS_ERR((__force void *)priv->base))
-		return PTR_ERR((__force void *)priv->base);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
 
 	priv->irq = platform_get_irq(pdev, 0);
 	if (priv->irq < 0) {
@@ -319,7 +319,7 @@ static int hix5hd2_ir_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(hix5hd2_ir_pm_ops, hix5hd2_ir_suspend,
 			 hix5hd2_ir_resume);
 
-static struct of_device_id hix5hd2_ir_table[] = {
+static const struct of_device_id hix5hd2_ir_table[] = {
 	{ .compatible = "hisilicon,hix5hd2-ir", },
 	{},
 };
diff --git a/drivers/media/rc/ir-rc5-decoder.c b/drivers/media/rc/ir-rc5-decoder.c
index 84fa6e9b59a1..8939ebd74391 100644
--- a/drivers/media/rc/ir-rc5-decoder.c
+++ b/drivers/media/rc/ir-rc5-decoder.c
@@ -184,9 +184,125 @@ out:
 	return -EINVAL;
 }
 
+static struct ir_raw_timings_manchester ir_rc5_timings = {
+	.leader			= RC5_UNIT,
+	.pulse_space_start	= 0,
+	.clock			= RC5_UNIT,
+	.trailer_space		= RC5_UNIT * 10,
+};
+
+static struct ir_raw_timings_manchester ir_rc5x_timings[2] = {
+	{
+		.leader			= RC5_UNIT,
+		.pulse_space_start	= 0,
+		.clock			= RC5_UNIT,
+		.trailer_space		= RC5X_SPACE,
+	},
+	{
+		.clock			= RC5_UNIT,
+		.trailer_space		= RC5_UNIT * 10,
+	},
+};
+
+static struct ir_raw_timings_manchester ir_rc5_sz_timings = {
+	.leader				= RC5_UNIT,
+	.pulse_space_start		= 0,
+	.clock				= RC5_UNIT,
+	.trailer_space			= RC5_UNIT * 10,
+};
+
+static int ir_rc5_validate_filter(const struct rc_scancode_filter *scancode,
+				  unsigned int important_bits)
+{
+	/* all important bits of scancode should be set in mask */
+	if (~scancode->mask & important_bits)
+		return -EINVAL;
+	/* extra bits in mask should be zero in data */
+	if (scancode->mask & scancode->data & ~important_bits)
+		return -EINVAL;
+	return 0;
+}
+
+/**
+ * ir_rc5_encode() - Encode a scancode as a stream of raw events
+ *
+ * @protocols:	allowed protocols
+ * @scancode:	scancode filter describing scancode (helps distinguish between
+ *		protocol subtypes when scancode is ambiguous)
+ * @events:	array of raw ir events to write into
+ * @max:	maximum size of @events
+ *
+ * Returns:	The number of events written.
+ *		-ENOBUFS if there isn't enough space in the array to fit the
+ *		encoding. In this case all @max events will have been written.
+ *		-EINVAL if the scancode is ambiguous or invalid.
+ */
+static int ir_rc5_encode(u64 protocols,
+			 const struct rc_scancode_filter *scancode,
+			 struct ir_raw_event *events, unsigned int max)
+{
+	int ret;
+	struct ir_raw_event *e = events;
+	unsigned int data, xdata, command, commandx, system;
+
+	/* Detect protocol and convert scancode to raw data */
+	if (protocols & RC_BIT_RC5 &&
+	    !ir_rc5_validate_filter(scancode, 0x1f7f)) {
+		/* decode scancode */
+		command  = (scancode->data & 0x003f) >> 0;
+		commandx = (scancode->data & 0x0040) >> 6;
+		system   = (scancode->data & 0x1f00) >> 8;
+		/* encode data */
+		data = !commandx << 12 | system << 6 | command;
+
+		/* Modulate the data */
+		ret = ir_raw_gen_manchester(&e, max, &ir_rc5_timings, RC5_NBITS,
+					    data);
+		if (ret < 0)
+			return ret;
+	} else if (protocols & RC_BIT_RC5X &&
+		   !ir_rc5_validate_filter(scancode, 0x1f7f3f)) {
+		/* decode scancode */
+		xdata    = (scancode->data & 0x00003f) >> 0;
+		command  = (scancode->data & 0x003f00) >> 8;
+		commandx = (scancode->data & 0x004000) >> 14;
+		system   = (scancode->data & 0x1f0000) >> 16;
+		/* commandx and system overlap, bits must match when encoded */
+		if (commandx == (system & 0x1))
+			return -EINVAL;
+		/* encode data */
+		data = 1 << 18 | system << 12 | command << 6 | xdata;
+
+		/* Modulate the data */
+		ret = ir_raw_gen_manchester(&e, max, &ir_rc5x_timings[0],
+					CHECK_RC5X_NBITS,
+					data >> (RC5X_NBITS-CHECK_RC5X_NBITS));
+		if (ret < 0)
+			return ret;
+		ret = ir_raw_gen_manchester(&e, max - (e - events),
+					&ir_rc5x_timings[1],
+					RC5X_NBITS - CHECK_RC5X_NBITS,
+					data);
+		if (ret < 0)
+			return ret;
+	} else if (protocols & RC_BIT_RC5_SZ &&
+		   !ir_rc5_validate_filter(scancode, 0x2fff)) {
+		/* RC5-SZ scancode is raw enough for Manchester as it is */
+		ret = ir_raw_gen_manchester(&e, max, &ir_rc5_sz_timings,
+					RC5_SZ_NBITS, scancode->data & 0x2fff);
+		if (ret < 0)
+			return ret;
+	} else {
+		return -EINVAL;
+	}
+
+	return e - events;
+}
+
 static struct ir_raw_handler rc5_handler = {
 	.protocols	= RC_BIT_RC5 | RC_BIT_RC5X | RC_BIT_RC5_SZ,
 	.decode		= ir_rc5_decode,
+	.encode		= ir_rc5_encode,
 };
 
 static int __init ir_rc5_decode_init(void)
diff --git a/drivers/media/rc/ir-rc6-decoder.c b/drivers/media/rc/ir-rc6-decoder.c
index d16bc67af732..f9c70baf6e0c 100644
--- a/drivers/media/rc/ir-rc6-decoder.c
+++ b/drivers/media/rc/ir-rc6-decoder.c
@@ -291,11 +291,133 @@ out:
 	return -EINVAL;
 }
 
+static struct ir_raw_timings_manchester ir_rc6_timings[4] = {
+	{
+		.leader			= RC6_PREFIX_PULSE,
+		.pulse_space_start	= 0,
+		.clock			= RC6_UNIT,
+		.invert			= 1,
+		.trailer_space		= RC6_PREFIX_SPACE,
+	},
+	{
+		.clock			= RC6_UNIT,
+		.invert			= 1,
+	},
+	{
+		.clock			= RC6_UNIT * 2,
+		.invert			= 1,
+	},
+	{
+		.clock			= RC6_UNIT,
+		.invert			= 1,
+		.trailer_space		= RC6_SUFFIX_SPACE,
+	},
+};
+
+static int ir_rc6_validate_filter(const struct rc_scancode_filter *scancode,
+				  unsigned int important_bits)
+{
+	/* all important bits of scancode should be set in mask */
+	if (~scancode->mask & important_bits)
+		return -EINVAL;
+	/* extra bits in mask should be zero in data */
+	if (scancode->mask & scancode->data & ~important_bits)
+		return -EINVAL;
+	return 0;
+}
+
+/**
+ * ir_rc6_encode() - Encode a scancode as a stream of raw events
+ *
+ * @protocols:	allowed protocols
+ * @scancode:	scancode filter describing scancode (helps distinguish between
+ *		protocol subtypes when scancode is ambiguous)
+ * @events:	array of raw ir events to write into
+ * @max:	maximum size of @events
+ *
+ * Returns:	The number of events written.
+ *		-ENOBUFS if there isn't enough space in the array to fit the
+ *		encoding. In this case all @max events will have been written.
+ *		-EINVAL if the scancode is ambiguous or invalid.
+ */
+static int ir_rc6_encode(u64 protocols,
+			 const struct rc_scancode_filter *scancode,
+			 struct ir_raw_event *events, unsigned int max)
+{
+	int ret;
+	struct ir_raw_event *e = events;
+
+	if (protocols & RC_BIT_RC6_0 &&
+	    !ir_rc6_validate_filter(scancode, 0xffff)) {
+
+		/* Modulate the preamble */
+		ret = ir_raw_gen_manchester(&e, max, &ir_rc6_timings[0], 0, 0);
+		if (ret < 0)
+			return ret;
+
+		/* Modulate the header (Start Bit & Mode-0) */
+		ret = ir_raw_gen_manchester(&e, max - (e - events),
+					    &ir_rc6_timings[1],
+					    RC6_HEADER_NBITS, (1 << 3));
+		if (ret < 0)
+			return ret;
+
+		/* Modulate Trailer Bit */
+		ret = ir_raw_gen_manchester(&e, max - (e - events),
+					    &ir_rc6_timings[2], 1, 0);
+		if (ret < 0)
+			return ret;
+
+		/* Modulate rest of the data */
+		ret = ir_raw_gen_manchester(&e, max - (e - events),
+					    &ir_rc6_timings[3], RC6_0_NBITS,
+					    scancode->data);
+		if (ret < 0)
+			return ret;
+
+	} else if (protocols & (RC_BIT_RC6_6A_20 | RC_BIT_RC6_6A_24 |
+				RC_BIT_RC6_6A_32 | RC_BIT_RC6_MCE) &&
+		   !ir_rc6_validate_filter(scancode, 0x8fffffff)) {
+
+		/* Modulate the preamble */
+		ret = ir_raw_gen_manchester(&e, max, &ir_rc6_timings[0], 0, 0);
+		if (ret < 0)
+			return ret;
+
+		/* Modulate the header (Start Bit & Header-version 6 */
+		ret = ir_raw_gen_manchester(&e, max - (e - events),
+					    &ir_rc6_timings[1],
+					    RC6_HEADER_NBITS, (1 << 3 | 6));
+		if (ret < 0)
+			return ret;
+
+		/* Modulate Trailer Bit */
+		ret = ir_raw_gen_manchester(&e, max - (e - events),
+					    &ir_rc6_timings[2], 1, 0);
+		if (ret < 0)
+			return ret;
+
+		/* Modulate rest of the data */
+		ret = ir_raw_gen_manchester(&e, max - (e - events),
+					    &ir_rc6_timings[3],
+					    fls(scancode->mask),
+					    scancode->data);
+		if (ret < 0)
+			return ret;
+
+	} else {
+		return -EINVAL;
+	}
+
+	return e - events;
+}
+
 static struct ir_raw_handler rc6_handler = {
 	.protocols	= RC_BIT_RC6_0 | RC_BIT_RC6_6A_20 |
 			  RC_BIT_RC6_6A_24 | RC_BIT_RC6_6A_32 |
 			  RC_BIT_RC6_MCE,
 	.decode		= ir_rc6_decode,
+	.encode		= ir_rc6_encode,
 };
 
 static int __init ir_rc6_decode_init(void)
diff --git a/drivers/media/rc/ir-sony-decoder.c b/drivers/media/rc/ir-sony-decoder.c
index d12dc3da5931..58ef06f35175 100644
--- a/drivers/media/rc/ir-sony-decoder.c
+++ b/drivers/media/rc/ir-sony-decoder.c
@@ -125,30 +125,27 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 
 		switch (data->count) {
 		case 12:
-			if (!(dev->enabled_protocols & RC_BIT_SONY12)) {
-				data->state = STATE_INACTIVE;
-				return 0;
-			}
+			if (!(dev->enabled_protocols & RC_BIT_SONY12))
+				goto finish_state_machine;
+
 			device    = bitrev8((data->bits <<  3) & 0xF8);
 			subdevice = 0;
 			function  = bitrev8((data->bits >>  4) & 0xFE);
 			protocol = RC_TYPE_SONY12;
 			break;
 		case 15:
-			if (!(dev->enabled_protocols & RC_BIT_SONY15)) {
-				data->state = STATE_INACTIVE;
-				return 0;
-			}
+			if (!(dev->enabled_protocols & RC_BIT_SONY15))
+				goto finish_state_machine;
+
 			device    = bitrev8((data->bits >>  0) & 0xFF);
 			subdevice = 0;
 			function  = bitrev8((data->bits >>  7) & 0xFE);
 			protocol = RC_TYPE_SONY15;
 			break;
 		case 20:
-			if (!(dev->enabled_protocols & RC_BIT_SONY20)) {
-				data->state = STATE_INACTIVE;
-				return 0;
-			}
+			if (!(dev->enabled_protocols & RC_BIT_SONY20))
+				goto finish_state_machine;
+
 			device    = bitrev8((data->bits >>  5) & 0xF8);
 			subdevice = bitrev8((data->bits >>  0) & 0xFF);
 			function  = bitrev8((data->bits >> 12) & 0xFE);
@@ -162,8 +159,7 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 		scancode = device << 16 | subdevice << 8 | function;
 		IR_dprintk(1, "Sony(%u) scancode 0x%05x\n", data->count, scancode);
 		rc_keydown(dev, protocol, scancode, 0);
-		data->state = STATE_INACTIVE;
-		return 0;
+		goto finish_state_machine;
 	}
 
 out:
@@ -171,6 +167,10 @@ out:
 		   data->state, TO_US(ev.duration), TO_STR(ev.pulse));
 	data->state = STATE_INACTIVE;
 	return -EINVAL;
+
+finish_state_machine:
+	data->state = STATE_INACTIVE;
+	return 0;
 }
 
 static struct ir_raw_handler sony_handler = {
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index abf60794223d..fbbd3bbcd252 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -84,7 +84,10 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-snapstream-firefly.o \
 			rc-streamzap.o \
 			rc-tbs-nec.o \
+			rc-technisat-ts35.o \
 			rc-technisat-usb2.o \
+			rc-terratec-cinergy-c-pci.o \
+			rc-terratec-cinergy-s2-hd.o \
 			rc-terratec-cinergy-xs.o \
 			rc-terratec-slim.o \
 			rc-terratec-slim-2.o \
@@ -94,6 +97,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-total-media-in-hand-02.o \
 			rc-trekstor.o \
 			rc-tt-1500.o \
+			rc-twinhan-dtv-cab-ci.o \
 			rc-twinhan1027.o \
 			rc-videomate-m1f.o \
 			rc-videomate-s350.o \
diff --git a/drivers/media/rc/keymaps/rc-technisat-ts35.c b/drivers/media/rc/keymaps/rc-technisat-ts35.c
new file mode 100644
index 000000000000..3328cbefabad
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-technisat-ts35.c
@@ -0,0 +1,76 @@
+/* rc-technisat-ts35.c - Keytable for TechniSat TS35 remote
+ *
+ * Copyright (c) 2013 by Jan Klötzke <jan@kloetzke.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 <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table technisat_ts35[] = {
+	{0x32, KEY_MUTE},
+	{0x07, KEY_MEDIA},
+	{0x1c, KEY_AB},
+	{0x33, KEY_POWER},
+
+	{0x3e, KEY_1},
+	{0x3d, KEY_2},
+	{0x3c, KEY_3},
+	{0x3b, KEY_4},
+	{0x3a, KEY_5},
+	{0x39, KEY_6},
+	{0x38, KEY_7},
+	{0x37, KEY_8},
+	{0x36, KEY_9},
+	{0x3f, KEY_0},
+	{0x35, KEY_DIGITS},
+	{0x2c, KEY_TV},
+
+	{0x20, KEY_INFO},
+	{0x2d, KEY_MENU},
+	{0x1f, KEY_UP},
+	{0x1e, KEY_DOWN},
+	{0x2e, KEY_LEFT},
+	{0x2f, KEY_RIGHT},
+	{0x28, KEY_OK},
+	{0x10, KEY_EPG},
+	{0x1d, KEY_BACK},
+
+	{0x14, KEY_RED},
+	{0x13, KEY_GREEN},
+	{0x12, KEY_YELLOW},
+	{0x11, KEY_BLUE},
+
+	{0x09, KEY_SELECT},
+	{0x03, KEY_TEXT},
+	{0x16, KEY_STOP},
+	{0x30, KEY_HELP},
+};
+
+static struct rc_map_list technisat_ts35_map = {
+	.map = {
+		.scan    = technisat_ts35,
+		.size    = ARRAY_SIZE(technisat_ts35),
+		.rc_type = RC_TYPE_UNKNOWN,
+		.name    = RC_MAP_TECHNISAT_TS35,
+	}
+};
+
+static int __init init_rc_map(void)
+{
+	return rc_map_register(&technisat_ts35_map);
+}
+
+static void __exit exit_rc_map(void)
+{
+	rc_map_unregister(&technisat_ts35_map);
+}
+
+module_init(init_rc_map)
+module_exit(exit_rc_map)
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/keymaps/rc-terratec-cinergy-c-pci.c b/drivers/media/rc/keymaps/rc-terratec-cinergy-c-pci.c
new file mode 100644
index 000000000000..7958f458527a
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-terratec-cinergy-c-pci.c
@@ -0,0 +1,88 @@
+/* keytable for Terratec Cinergy C PCI Remote Controller
+ *
+ * Copyright (c) 2010 by Igor M. Liplianin <liplianin@me.by>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table terratec_cinergy_c_pci[] = {
+	{ 0x3e, KEY_POWER},
+	{ 0x3d, KEY_1},
+	{ 0x3c, KEY_2},
+	{ 0x3b, KEY_3},
+	{ 0x3a, KEY_4},
+	{ 0x39, KEY_5},
+	{ 0x38, KEY_6},
+	{ 0x37, KEY_7},
+	{ 0x36, KEY_8},
+	{ 0x35, KEY_9},
+	{ 0x34, KEY_VIDEO_NEXT}, /* AV */
+	{ 0x33, KEY_0},
+	{ 0x32, KEY_REFRESH},
+	{ 0x30, KEY_EPG},
+	{ 0x2f, KEY_UP},
+	{ 0x2e, KEY_LEFT},
+	{ 0x2d, KEY_OK},
+	{ 0x2c, KEY_RIGHT},
+	{ 0x2b, KEY_DOWN},
+	{ 0x29, KEY_INFO},
+	{ 0x28, KEY_RED},
+	{ 0x27, KEY_GREEN},
+	{ 0x26, KEY_YELLOW},
+	{ 0x25, KEY_BLUE},
+	{ 0x24, KEY_CHANNELUP},
+	{ 0x23, KEY_VOLUMEUP},
+	{ 0x22, KEY_MUTE},
+	{ 0x21, KEY_VOLUMEDOWN},
+	{ 0x20, KEY_CHANNELDOWN},
+	{ 0x1f, KEY_PAUSE},
+	{ 0x1e, KEY_HOME},
+	{ 0x1d, KEY_MENU}, /* DVD Menu */
+	{ 0x1c, KEY_SUBTITLE},
+	{ 0x1b, KEY_TEXT}, /* Teletext */
+	{ 0x1a, KEY_DELETE},
+	{ 0x19, KEY_TV},
+	{ 0x18, KEY_DVD},
+	{ 0x17, KEY_STOP},
+	{ 0x16, KEY_VIDEO},
+	{ 0x15, KEY_AUDIO}, /* Music */
+	{ 0x14, KEY_SCREEN}, /* Pic */
+	{ 0x13, KEY_PLAY},
+	{ 0x12, KEY_BACK},
+	{ 0x11, KEY_REWIND},
+	{ 0x10, KEY_FASTFORWARD},
+	{ 0x0b, KEY_PREVIOUS},
+	{ 0x07, KEY_RECORD},
+	{ 0x03, KEY_NEXT},
+
+};
+
+static struct rc_map_list terratec_cinergy_c_pci_map = {
+	.map = {
+		.scan    = terratec_cinergy_c_pci,
+		.size    = ARRAY_SIZE(terratec_cinergy_c_pci),
+		.rc_type = RC_TYPE_UNKNOWN,	/* Legacy IR type */
+		.name    = RC_MAP_TERRATEC_CINERGY_C_PCI,
+	}
+};
+
+static int __init init_rc_map_terratec_cinergy_c_pci(void)
+{
+	return rc_map_register(&terratec_cinergy_c_pci_map);
+}
+
+static void __exit exit_rc_map_terratec_cinergy_c_pci(void)
+{
+	rc_map_unregister(&terratec_cinergy_c_pci_map);
+}
+
+module_init(init_rc_map_terratec_cinergy_c_pci);
+module_exit(exit_rc_map_terratec_cinergy_c_pci);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/keymaps/rc-terratec-cinergy-s2-hd.c b/drivers/media/rc/keymaps/rc-terratec-cinergy-s2-hd.c
new file mode 100644
index 000000000000..1e096bbda4a0
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-terratec-cinergy-s2-hd.c
@@ -0,0 +1,86 @@
+/* keytable for Terratec Cinergy S2 HD Remote Controller
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table terratec_cinergy_s2_hd[] = {
+	{ 0x03, KEY_NEXT},               /* >| */
+	{ 0x07, KEY_RECORD},
+	{ 0x0b, KEY_PREVIOUS},           /* |< */
+	{ 0x10, KEY_FASTFORWARD},        /* >> */
+	{ 0x11, KEY_REWIND},             /* << */
+	{ 0x12, KEY_ESC},                /* Back */
+	{ 0x13, KEY_PLAY},
+	{ 0x14, KEY_IMAGES},
+	{ 0x15, KEY_AUDIO},
+	{ 0x16, KEY_MEDIA},              /* Video-Menu */
+	{ 0x17, KEY_STOP},
+	{ 0x18, KEY_DVD},
+	{ 0x19, KEY_TV},
+	{ 0x1a, KEY_DELETE},
+	{ 0x1b, KEY_TEXT},
+	{ 0x1c, KEY_SUBTITLE},
+	{ 0x1d, KEY_MENU},               /* DVD-Menu */
+	{ 0x1e, KEY_HOME},
+	{ 0x1f, KEY_PAUSE},
+	{ 0x20, KEY_CHANNELDOWN},
+	{ 0x21, KEY_VOLUMEDOWN},
+	{ 0x22, KEY_MUTE},
+	{ 0x23, KEY_VOLUMEUP},
+	{ 0x24, KEY_CHANNELUP},
+	{ 0x25, KEY_BLUE},
+	{ 0x26, KEY_YELLOW},
+	{ 0x27, KEY_GREEN},
+	{ 0x28, KEY_RED},
+	{ 0x29, KEY_INFO},
+	{ 0x2b, KEY_DOWN},
+	{ 0x2c, KEY_RIGHT},
+	{ 0x2d, KEY_OK},
+	{ 0x2e, KEY_LEFT},
+	{ 0x2f, KEY_UP},
+	{ 0x30, KEY_EPG},
+	{ 0x32, KEY_VIDEO},              /* A<=>B */
+	{ 0x33, KEY_0},
+	{ 0x34, KEY_VCR},                /* AV */
+	{ 0x35, KEY_9},
+	{ 0x36, KEY_8},
+	{ 0x37, KEY_7},
+	{ 0x38, KEY_6},
+	{ 0x39, KEY_5},
+	{ 0x3a, KEY_4},
+	{ 0x3b, KEY_3},
+	{ 0x3c, KEY_2},
+	{ 0x3d, KEY_1},
+	{ 0x3e, KEY_POWER},
+
+};
+
+static struct rc_map_list terratec_cinergy_s2_hd_map = {
+	.map = {
+		.scan    = terratec_cinergy_s2_hd,
+		.size    = ARRAY_SIZE(terratec_cinergy_s2_hd),
+		.rc_type = RC_TYPE_UNKNOWN,	/* Legacy IR type */
+		.name    = RC_MAP_TERRATEC_CINERGY_S2_HD,
+	}
+};
+
+static int __init init_rc_map_terratec_cinergy_s2_hd(void)
+{
+	return rc_map_register(&terratec_cinergy_s2_hd_map);
+}
+
+static void __exit exit_rc_map_terratec_cinergy_s2_hd(void)
+{
+	rc_map_unregister(&terratec_cinergy_s2_hd_map);
+}
+
+module_init(init_rc_map_terratec_cinergy_s2_hd);
+module_exit(exit_rc_map_terratec_cinergy_s2_hd);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/keymaps/rc-twinhan-dtv-cab-ci.c b/drivers/media/rc/keymaps/rc-twinhan-dtv-cab-ci.c
new file mode 100644
index 000000000000..202500cb3061
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-twinhan-dtv-cab-ci.c
@@ -0,0 +1,98 @@
+/* keytable for Twinhan DTV CAB CI Remote Controller
+ *
+ * Copyright (c) 2010 by Igor M. Liplianin <liplianin@me.by>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table twinhan_dtv_cab_ci[] = {
+	{ 0x29, KEY_POWER},
+	{ 0x28, KEY_FAVORITES},
+	{ 0x30, KEY_TEXT},
+	{ 0x17, KEY_INFO},              /* Preview */
+	{ 0x23, KEY_EPG},
+	{ 0x3b, KEY_F22},               /* Record List */
+
+	{ 0x3c, KEY_1},
+	{ 0x3e, KEY_2},
+	{ 0x39, KEY_3},
+	{ 0x36, KEY_4},
+	{ 0x22, KEY_5},
+	{ 0x20, KEY_6},
+	{ 0x32, KEY_7},
+	{ 0x26, KEY_8},
+	{ 0x24, KEY_9},
+	{ 0x2a, KEY_0},
+
+	{ 0x33, KEY_CANCEL},
+	{ 0x2c, KEY_BACK},
+	{ 0x15, KEY_CLEAR},
+	{ 0x3f, KEY_TAB},
+	{ 0x10, KEY_ENTER},
+	{ 0x14, KEY_UP},
+	{ 0x0d, KEY_RIGHT},
+	{ 0x0e, KEY_DOWN},
+	{ 0x11, KEY_LEFT},
+
+	{ 0x21, KEY_VOLUMEUP},
+	{ 0x35, KEY_VOLUMEDOWN},
+	{ 0x3d, KEY_CHANNELDOWN},
+	{ 0x3a, KEY_CHANNELUP},
+	{ 0x2e, KEY_RECORD},
+	{ 0x2b, KEY_PLAY},
+	{ 0x13, KEY_PAUSE},
+	{ 0x25, KEY_STOP},
+
+	{ 0x1f, KEY_REWIND},
+	{ 0x2d, KEY_FASTFORWARD},
+	{ 0x1e, KEY_PREVIOUS},          /* Replay |< */
+	{ 0x1d, KEY_NEXT},              /* Skip   >| */
+
+	{ 0x0b, KEY_CAMERA},            /* Capture */
+	{ 0x0f, KEY_LANGUAGE},          /* SAP */
+	{ 0x18, KEY_MODE},              /* PIP */
+	{ 0x12, KEY_ZOOM},              /* Full screen */
+	{ 0x1c, KEY_SUBTITLE},
+	{ 0x2f, KEY_MUTE},
+	{ 0x16, KEY_F20},               /* L/R */
+	{ 0x38, KEY_F21},               /* Hibernate */
+
+	{ 0x37, KEY_SWITCHVIDEOMODE},   /* A/V */
+	{ 0x31, KEY_AGAIN},             /* Recall */
+	{ 0x1a, KEY_KPPLUS},            /* Zoom+ */
+	{ 0x19, KEY_KPMINUS},           /* Zoom- */
+	{ 0x27, KEY_RED},
+	{ 0x0C, KEY_GREEN},
+	{ 0x01, KEY_YELLOW},
+	{ 0x00, KEY_BLUE},
+};
+
+static struct rc_map_list twinhan_dtv_cab_ci_map = {
+	.map = {
+		.scan    = twinhan_dtv_cab_ci,
+		.size    = ARRAY_SIZE(twinhan_dtv_cab_ci),
+		.rc_type = RC_TYPE_UNKNOWN,	/* Legacy IR type */
+		.name    = RC_MAP_TWINHAN_DTV_CAB_CI,
+	}
+};
+
+static int __init init_rc_map_twinhan_dtv_cab_ci(void)
+{
+	return rc_map_register(&twinhan_dtv_cab_ci_map);
+}
+
+static void __exit exit_rc_map_twinhan_dtv_cab_ci(void)
+{
+	rc_map_unregister(&twinhan_dtv_cab_ci_map);
+}
+
+module_init(init_rc_map_twinhan_dtv_cab_ci);
+module_exit(exit_rc_map_twinhan_dtv_cab_ci);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 85af7a869167..baeb5971fd52 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -526,6 +526,130 @@ static int nvt_set_tx_carrier(struct rc_dev *dev, u32 carrier)
 	return 0;
 }
 
+static int nvt_write_wakeup_codes(struct rc_dev *dev,
+				  const u8 *wakeup_sample_buf, int count)
+{
+	int i = 0;
+	u8 reg, reg_learn_mode;
+	unsigned long flags;
+	struct nvt_dev *nvt = dev->priv;
+
+	nvt_dbg_wake("writing wakeup samples");
+
+	reg = nvt_cir_wake_reg_read(nvt, CIR_WAKE_IRCON);
+	reg_learn_mode = reg & ~CIR_WAKE_IRCON_MODE0;
+	reg_learn_mode |= CIR_WAKE_IRCON_MODE1;
+
+	/* Lock the learn area to prevent racing with wake-isr */
+	spin_lock_irqsave(&nvt->nvt_lock, flags);
+
+	/* Enable fifo writes */
+	nvt_cir_wake_reg_write(nvt, reg_learn_mode, CIR_WAKE_IRCON);
+
+	/* Clear cir wake rx fifo */
+	nvt_clear_cir_wake_fifo(nvt);
+
+	if (count > WAKE_FIFO_LEN) {
+		nvt_dbg_wake("HW FIFO too small for all wake samples");
+		count = WAKE_FIFO_LEN;
+	}
+
+	if (count)
+		pr_info("Wake samples (%d) =", count);
+	else
+		pr_info("Wake sample fifo cleared");
+
+	/* Write wake samples to fifo */
+	for (i = 0; i < count; i++) {
+		pr_cont(" %02x", wakeup_sample_buf[i]);
+		nvt_cir_wake_reg_write(nvt, wakeup_sample_buf[i],
+				       CIR_WAKE_WR_FIFO_DATA);
+	}
+	pr_cont("\n");
+
+	/* Switch cir to wakeup mode and disable fifo writing */
+	nvt_cir_wake_reg_write(nvt, reg, CIR_WAKE_IRCON);
+
+	/* Set number of bytes needed for wake */
+	nvt_cir_wake_reg_write(nvt, count ? count :
+			       CIR_WAKE_FIFO_CMP_BYTES,
+			       CIR_WAKE_FIFO_CMP_DEEP);
+
+	spin_unlock_irqrestore(&nvt->nvt_lock, flags);
+
+	return 0;
+}
+
+static int nvt_ir_raw_set_wakeup_filter(struct rc_dev *dev,
+					struct rc_scancode_filter *sc_filter)
+{
+	u8 *reg_buf;
+	u8 buf_val;
+	int i, ret, count;
+	unsigned int val;
+	struct ir_raw_event *raw;
+	bool complete;
+
+	/* Require both mask and data to be set before actually committing */
+	if (!sc_filter->mask || !sc_filter->data)
+		return 0;
+
+	raw = kmalloc_array(WAKE_FIFO_LEN, sizeof(*raw), GFP_KERNEL);
+	if (!raw)
+		return -ENOMEM;
+
+	ret = ir_raw_encode_scancode(dev->enabled_wakeup_protocols, sc_filter,
+				     raw, WAKE_FIFO_LEN);
+	complete = (ret != -ENOBUFS);
+	if (!complete)
+		ret = WAKE_FIFO_LEN;
+	else if (ret < 0)
+		goto out_raw;
+
+	reg_buf = kmalloc_array(WAKE_FIFO_LEN, sizeof(*reg_buf), GFP_KERNEL);
+	if (!reg_buf) {
+		ret = -ENOMEM;
+		goto out_raw;
+	}
+
+	/* Inspect the ir samples */
+	for (i = 0, count = 0; i < ret && count < WAKE_FIFO_LEN; ++i) {
+		val = NS_TO_US((raw[i]).duration) / SAMPLE_PERIOD;
+
+		/* Split too large values into several smaller ones */
+		while (val > 0 && count < WAKE_FIFO_LEN) {
+
+			/* Skip last value for better comparison tolerance */
+			if (complete && i == ret - 1 && val < BUF_LEN_MASK)
+				break;
+
+			/* Clamp values to BUF_LEN_MASK at most */
+			buf_val = (val > BUF_LEN_MASK) ? BUF_LEN_MASK : val;
+
+			reg_buf[count] = buf_val;
+			val -= buf_val;
+			if ((raw[i]).pulse)
+				reg_buf[count] |= BUF_PULSE_BIT;
+			count++;
+		}
+	}
+
+	ret = nvt_write_wakeup_codes(dev, reg_buf, count);
+
+	kfree(reg_buf);
+out_raw:
+	kfree(raw);
+
+	return ret;
+}
+
+/* Dummy implementation. nuvoton is agnostic to the protocol used */
+static int nvt_ir_raw_change_wakeup_protocol(struct rc_dev *dev,
+					     u64 *rc_type)
+{
+	return 0;
+}
+
 /*
  * nvt_tx_ir
  *
@@ -1043,11 +1167,14 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 	/* Set up the rc device */
 	rdev->priv = nvt;
 	rdev->driver_type = RC_DRIVER_IR_RAW;
+	rdev->encode_wakeup = true;
 	rdev->allowed_protocols = RC_BIT_ALL;
 	rdev->open = nvt_open;
 	rdev->close = nvt_close;
 	rdev->tx_ir = nvt_tx_ir;
 	rdev->s_tx_carrier = nvt_set_tx_carrier;
+	rdev->s_wakeup_filter = nvt_ir_raw_set_wakeup_filter;
+	rdev->change_wakeup_protocol = nvt_ir_raw_change_wakeup_protocol;
 	rdev->input_name = "Nuvoton w836x7hg Infrared Remote Transceiver";
 	rdev->input_phys = "nuvoton/cir0";
 	rdev->input_id.bustype = BUS_HOST;
diff --git a/drivers/media/rc/nuvoton-cir.h b/drivers/media/rc/nuvoton-cir.h
index e1cf23c3875b..9d0e161c2a88 100644
--- a/drivers/media/rc/nuvoton-cir.h
+++ b/drivers/media/rc/nuvoton-cir.h
@@ -63,6 +63,7 @@ static int debug;
  */
 #define TX_BUF_LEN 256
 #define RX_BUF_LEN 32
+#define WAKE_FIFO_LEN 67
 
 struct nvt_dev {
 	struct pnp_dev *pdev;
diff --git a/drivers/media/rc/rc-core-priv.h b/drivers/media/rc/rc-core-priv.h
index b68d4f762734..4b994aa2f2a7 100644
--- a/drivers/media/rc/rc-core-priv.h
+++ b/drivers/media/rc/rc-core-priv.h
@@ -25,6 +25,8 @@ struct ir_raw_handler {
 
 	u64 protocols; /* which are handled by this handler */
 	int (*decode)(struct rc_dev *dev, struct ir_raw_event event);
+	int (*encode)(u64 protocols, const struct rc_scancode_filter *scancode,
+		      struct ir_raw_event *events, unsigned int max);
 
 	/* These two should only be used by the lirc decoder */
 	int (*raw_register)(struct rc_dev *dev);
@@ -150,10 +152,44 @@ static inline bool is_timing_event(struct ir_raw_event ev)
 #define TO_US(duration)			DIV_ROUND_CLOSEST((duration), 1000)
 #define TO_STR(is_pulse)		((is_pulse) ? "pulse" : "space")
 
+/* functions for IR encoders */
+
+static inline void init_ir_raw_event_duration(struct ir_raw_event *ev,
+					      unsigned int pulse,
+					      u32 duration)
+{
+	init_ir_raw_event(ev);
+	ev->duration = duration;
+	ev->pulse = pulse;
+}
+
+/**
+ * struct ir_raw_timings_manchester - Manchester coding timings
+ * @leader:		duration of leader pulse (if any) 0 if continuing
+ *			existing signal (see @pulse_space_start)
+ * @pulse_space_start:	1 for starting with pulse (0 for starting with space)
+ * @clock:		duration of each pulse/space in ns
+ * @invert:		if set clock logic is inverted
+ *			(0 = space + pulse, 1 = pulse + space)
+ * @trailer_space:	duration of trailer space in ns
+ */
+struct ir_raw_timings_manchester {
+	unsigned int leader;
+	unsigned int pulse_space_start:1;
+	unsigned int clock;
+	unsigned int invert:1;
+	unsigned int trailer_space;
+};
+
+int ir_raw_gen_manchester(struct ir_raw_event **ev, unsigned int max,
+			  const struct ir_raw_timings_manchester *timings,
+			  unsigned int n, unsigned int data);
+
 /*
  * Routines from rc-raw.c to be used internally and by decoders
  */
 u64 ir_raw_get_allowed_protocols(void);
+u64 ir_raw_get_encode_protocols(void);
 int ir_raw_event_register(struct rc_dev *dev);
 void ir_raw_event_unregister(struct rc_dev *dev);
 int ir_raw_handler_register(struct ir_raw_handler *ir_raw_handler);
diff --git a/drivers/media/rc/rc-ir-raw.c b/drivers/media/rc/rc-ir-raw.c
index b732ac6a26d8..b9e4645c731c 100644
--- a/drivers/media/rc/rc-ir-raw.c
+++ b/drivers/media/rc/rc-ir-raw.c
@@ -30,6 +30,7 @@ static LIST_HEAD(ir_raw_client_list);
 static DEFINE_MUTEX(ir_raw_handler_lock);
 static LIST_HEAD(ir_raw_handler_list);
 static u64 available_protocols;
+static u64 encode_protocols;
 
 static int ir_raw_event_thread(void *data)
 {
@@ -240,12 +241,146 @@ ir_raw_get_allowed_protocols(void)
 	return protocols;
 }
 
+/* used internally by the sysfs interface */
+u64
+ir_raw_get_encode_protocols(void)
+{
+	u64 protocols;
+
+	mutex_lock(&ir_raw_handler_lock);
+	protocols = encode_protocols;
+	mutex_unlock(&ir_raw_handler_lock);
+	return protocols;
+}
+
 static int change_protocol(struct rc_dev *dev, u64 *rc_type)
 {
 	/* the caller will update dev->enabled_protocols */
 	return 0;
 }
 
+/**
+ * ir_raw_gen_manchester() - Encode data with Manchester (bi-phase) modulation.
+ * @ev:		Pointer to pointer to next free event. *@ev is incremented for
+ *		each raw event filled.
+ * @max:	Maximum number of raw events to fill.
+ * @timings:	Manchester modulation timings.
+ * @n:		Number of bits of data.
+ * @data:	Data bits to encode.
+ *
+ * Encodes the @n least significant bits of @data using Manchester (bi-phase)
+ * modulation with the timing characteristics described by @timings, writing up
+ * to @max raw IR events using the *@ev pointer.
+ *
+ * Returns:	0 on success.
+ *		-ENOBUFS if there isn't enough space in the array to fit the
+ *		full encoded data. In this case all @max events will have been
+ *		written.
+ */
+int ir_raw_gen_manchester(struct ir_raw_event **ev, unsigned int max,
+			  const struct ir_raw_timings_manchester *timings,
+			  unsigned int n, unsigned int data)
+{
+	bool need_pulse;
+	unsigned int i;
+	int ret = -ENOBUFS;
+
+	i = 1 << (n - 1);
+
+	if (timings->leader) {
+		if (!max--)
+			return ret;
+		if (timings->pulse_space_start) {
+			init_ir_raw_event_duration((*ev)++, 1, timings->leader);
+
+			if (!max--)
+				return ret;
+			init_ir_raw_event_duration((*ev), 0, timings->leader);
+		} else {
+			init_ir_raw_event_duration((*ev), 1, timings->leader);
+		}
+		i >>= 1;
+	} else {
+		/* continue existing signal */
+		--(*ev);
+	}
+	/* from here on *ev will point to the last event rather than the next */
+
+	while (n && i > 0) {
+		need_pulse = !(data & i);
+		if (timings->invert)
+			need_pulse = !need_pulse;
+		if (need_pulse == !!(*ev)->pulse) {
+			(*ev)->duration += timings->clock;
+		} else {
+			if (!max--)
+				goto nobufs;
+			init_ir_raw_event_duration(++(*ev), need_pulse,
+						   timings->clock);
+		}
+
+		if (!max--)
+			goto nobufs;
+		init_ir_raw_event_duration(++(*ev), !need_pulse,
+					   timings->clock);
+		i >>= 1;
+	}
+
+	if (timings->trailer_space) {
+		if (!(*ev)->pulse)
+			(*ev)->duration += timings->trailer_space;
+		else if (!max--)
+			goto nobufs;
+		else
+			init_ir_raw_event_duration(++(*ev), 0,
+						   timings->trailer_space);
+	}
+
+	ret = 0;
+nobufs:
+	/* point to the next event rather than last event before returning */
+	++(*ev);
+	return ret;
+}
+EXPORT_SYMBOL(ir_raw_gen_manchester);
+
+/**
+ * ir_raw_encode_scancode() - Encode a scancode as raw events
+ *
+ * @protocols:		permitted protocols
+ * @scancode:		scancode filter describing a single scancode
+ * @events:		array of raw events to write into
+ * @max:		max number of raw events
+ *
+ * Attempts to encode the scancode as raw events.
+ *
+ * Returns:	The number of events written.
+ *		-ENOBUFS if there isn't enough space in the array to fit the
+ *		encoding. In this case all @max events will have been written.
+ *		-EINVAL if the scancode is ambiguous or invalid, or if no
+ *		compatible encoder was found.
+ */
+int ir_raw_encode_scancode(u64 protocols,
+			   const struct rc_scancode_filter *scancode,
+			   struct ir_raw_event *events, unsigned int max)
+{
+	struct ir_raw_handler *handler;
+	int ret = -EINVAL;
+
+	mutex_lock(&ir_raw_handler_lock);
+	list_for_each_entry(handler, &ir_raw_handler_list, list) {
+		if (handler->protocols & protocols && handler->encode) {
+			ret = handler->encode(protocols, scancode, events, max);
+			if (ret >= 0 || ret == -ENOBUFS)
+				break;
+		}
+	}
+	mutex_unlock(&ir_raw_handler_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(ir_raw_encode_scancode);
+
 /*
  * Used to (un)register raw event clients
  */
@@ -328,6 +463,8 @@ int ir_raw_handler_register(struct ir_raw_handler *ir_raw_handler)
 		list_for_each_entry(raw, &ir_raw_client_list, list)
 			ir_raw_handler->raw_register(raw->dev);
 	available_protocols |= ir_raw_handler->protocols;
+	if (ir_raw_handler->encode)
+		encode_protocols |= ir_raw_handler->protocols;
 	mutex_unlock(&ir_raw_handler_lock);
 
 	return 0;
@@ -344,6 +481,8 @@ void ir_raw_handler_unregister(struct ir_raw_handler *ir_raw_handler)
 		list_for_each_entry(raw, &ir_raw_client_list, list)
 			ir_raw_handler->raw_unregister(raw->dev);
 	available_protocols &= ~ir_raw_handler->protocols;
+	if (ir_raw_handler->encode)
+		encode_protocols &= ~ir_raw_handler->protocols;
 	mutex_unlock(&ir_raw_handler_lock);
 }
 EXPORT_SYMBOL(ir_raw_handler_unregister);
diff --git a/drivers/media/rc/rc-loopback.c b/drivers/media/rc/rc-loopback.c
index 63dace8198b0..d8bdf63ce985 100644
--- a/drivers/media/rc/rc-loopback.c
+++ b/drivers/media/rc/rc-loopback.c
@@ -26,6 +26,7 @@
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/sched.h>
+#include <linux/slab.h>
 #include <media/rc-core.h>
 
 #define DRIVER_NAME	"rc-loopback"
@@ -176,6 +177,39 @@ static int loop_set_carrier_report(struct rc_dev *dev, int enable)
 	return 0;
 }
 
+static int loop_set_wakeup_filter(struct rc_dev *dev,
+				  struct rc_scancode_filter *sc_filter)
+{
+	static const unsigned int max = 512;
+	struct ir_raw_event *raw;
+	int ret;
+	int i;
+
+	/* fine to disable filter */
+	if (!sc_filter->mask)
+		return 0;
+
+	/* encode the specified filter and loop it back */
+	raw = kmalloc_array(max, sizeof(*raw), GFP_KERNEL);
+	ret = ir_raw_encode_scancode(dev->enabled_wakeup_protocols, sc_filter,
+				     raw, max);
+	/* still loop back the partial raw IR even if it's incomplete */
+	if (ret == -ENOBUFS)
+		ret = max;
+	if (ret >= 0) {
+		/* do the loopback */
+		for (i = 0; i < ret; ++i)
+			ir_raw_event_store(dev, &raw[i]);
+		ir_raw_event_handle(dev);
+
+		ret = 0;
+	}
+
+	kfree(raw);
+
+	return ret;
+}
+
 static int __init loop_init(void)
 {
 	struct rc_dev *rc;
@@ -195,6 +229,7 @@ static int __init loop_init(void)
 	rc->map_name		= RC_MAP_EMPTY;
 	rc->priv		= &loopdev;
 	rc->driver_type		= RC_DRIVER_IR_RAW;
+	rc->encode_wakeup	= true;
 	rc->allowed_protocols	= RC_BIT_ALL;
 	rc->timeout		= 100 * 1000 * 1000; /* 100 ms */
 	rc->min_timeout		= 1;
@@ -209,6 +244,7 @@ static int __init loop_init(void)
 	rc->s_idle		= loop_set_idle;
 	rc->s_learning_mode	= loop_set_learning_mode;
 	rc->s_carrier_report	= loop_set_carrier_report;
+	rc->s_wakeup_filter	= loop_set_wakeup_filter;
 
 	loopdev.txmask		= RXMASK_REGULAR;
 	loopdev.txcarrier	= 36000;
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index f8c5e47a30aa..9d015db65280 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -746,7 +746,7 @@ void rc_close(struct rc_dev *rdev)
 	if (rdev) {
 		mutex_lock(&rdev->lock);
 
-		 if (!--rdev->users && rdev->close != NULL)
+		if (!--rdev->users && rdev->close != NULL)
 			rdev->close(rdev);
 
 		mutex_unlock(&rdev->lock);
@@ -865,6 +865,8 @@ static ssize_t show_protocols(struct device *device,
 	} else {
 		enabled = dev->enabled_wakeup_protocols;
 		allowed = dev->allowed_wakeup_protocols;
+		if (dev->encode_wakeup && !allowed)
+			allowed = ir_raw_get_encode_protocols();
 	}
 
 	mutex_unlock(&dev->lock);
@@ -1406,13 +1408,16 @@ int rc_register_device(struct rc_dev *dev)
 		path ? path : "N/A");
 	kfree(path);
 
-	if (dev->driver_type == RC_DRIVER_IR_RAW) {
+	if (dev->driver_type == RC_DRIVER_IR_RAW || dev->encode_wakeup) {
 		/* Load raw decoders, if they aren't already */
 		if (!raw_init) {
 			IR_dprintk(1, "Loading raw decoders\n");
 			ir_raw_init();
 			raw_init = true;
 		}
+	}
+
+	if (dev->driver_type == RC_DRIVER_IR_RAW) {
 		/* calls ir_register_device so unlock mutex here*/
 		mutex_unlock(&dev->lock);
 		rc = ir_raw_event_register(dev);
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index c4def66f9aa2..ec74244a3853 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -322,7 +322,7 @@ static u32 redrat3_us_to_len(u32 microsec)
 	u32 result;
 	u32 divisor;
 
-	microsec &= IR_MAX_DURATION;
+	microsec = (microsec > IR_MAX_DURATION) ? IR_MAX_DURATION : microsec;
 	divisor = (RR3_CLK_CONV_FACTOR / 1000);
 	result = (u32)(microsec * divisor) / 1000;
 
@@ -380,7 +380,8 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 		if (i == 0)
 			trailer = rawir.duration;
 		/* cap the value to IR_MAX_DURATION */
-		rawir.duration &= IR_MAX_DURATION;
+		rawir.duration = (rawir.duration > IR_MAX_DURATION) ?
+				 IR_MAX_DURATION : rawir.duration;
 
 		dev_dbg(dev, "storing %s with duration %d (i: %d)\n",
 			rawir.pulse ? "pulse" : "space", rawir.duration, i);
@@ -405,7 +406,7 @@ static void redrat3_process_ir_data(struct redrat3_dev *rr3)
 }
 
 /* Util fn to send rr3 cmds */
-static u8 redrat3_send_cmd(int cmd, struct redrat3_dev *rr3)
+static int redrat3_send_cmd(int cmd, struct redrat3_dev *rr3)
 {
 	struct usb_device *udev;
 	u8 *data;
diff --git a/drivers/media/rc/st_rc.c b/drivers/media/rc/st_rc.c
index 0e758ae2e529..37d040158dff 100644
--- a/drivers/media/rc/st_rc.c
+++ b/drivers/media/rc/st_rc.c
@@ -22,8 +22,8 @@ struct st_rc_device {
 	int				irq;
 	int				irq_wake;
 	struct clk			*sys_clock;
-	volatile void __iomem		*base;	/* Register base address */
-	volatile void __iomem		*rx_base;/* RX Register base address */
+	void __iomem			*base;	/* Register base address */
+	void __iomem			*rx_base;/* RX Register base address */
 	struct rc_dev			*rdev;
 	bool				overclocking;
 	int				sample_mult;
@@ -267,8 +267,8 @@ static int st_rc_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
 	rc_dev->base = devm_ioremap_resource(dev, res);
-	if (IS_ERR((__force void *)rc_dev->base)) {
-		ret = PTR_ERR((__force void *)rc_dev->base);
+	if (IS_ERR(rc_dev->base)) {
+		ret = PTR_ERR(rc_dev->base);
 		goto err;
 	}
 
@@ -334,7 +334,7 @@ err:
 	return ret;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int st_rc_suspend(struct device *dev)
 {
 	struct st_rc_device *rc_dev = dev_get_drvdata(dev);
@@ -381,7 +381,7 @@ static int st_rc_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(st_rc_pm_ops, st_rc_suspend, st_rc_resume);
 
 #ifdef CONFIG_OF
-static struct of_device_id st_rc_match[] = {
+static const struct of_device_id st_rc_match[] = {
 	{ .compatible = "st,comms-irb", },
 	{},
 };
diff --git a/drivers/media/rc/streamzap.c b/drivers/media/rc/streamzap.c
index bf4a44272f0e..5a17cb88ff27 100644
--- a/drivers/media/rc/streamzap.c
+++ b/drivers/media/rc/streamzap.c
@@ -152,7 +152,8 @@ static void sz_push_full_pulse(struct streamzap_ir *sz,
 				sz->signal_last.tv_usec);
 			rawir.duration -= sz->sum;
 			rawir.duration = US_TO_NS(rawir.duration);
-			rawir.duration &= IR_MAX_DURATION;
+			rawir.duration = (rawir.duration > IR_MAX_DURATION) ?
+					 IR_MAX_DURATION : rawir.duration;
 		}
 		sz_push(sz, rawir);
 
@@ -165,7 +166,8 @@ static void sz_push_full_pulse(struct streamzap_ir *sz,
 	rawir.duration += SZ_RESOLUTION / 2;
 	sz->sum += rawir.duration;
 	rawir.duration = US_TO_NS(rawir.duration);
-	rawir.duration &= IR_MAX_DURATION;
+	rawir.duration = (rawir.duration > IR_MAX_DURATION) ?
+			 IR_MAX_DURATION : rawir.duration;
 	sz_push(sz, rawir);
 }
 
diff --git a/drivers/media/tuners/Kconfig b/drivers/media/tuners/Kconfig
index 983510d282f6..8294af909174 100644
--- a/drivers/media/tuners/Kconfig
+++ b/drivers/media/tuners/Kconfig
@@ -220,6 +220,7 @@ config MEDIA_TUNER_E4000
 config MEDIA_TUNER_FC2580
 	tristate "FCI FC2580 silicon tuner"
 	depends on MEDIA_SUPPORT && I2C
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  FCI FC2580 silicon tuner driver.
@@ -233,8 +234,9 @@ config MEDIA_TUNER_M88RS6000T
 	  Montage M88RS6000 internal tuner.
 
 config MEDIA_TUNER_TUA9001
-	tristate "Infineon TUA 9001 silicon tuner"
+	tristate "Infineon TUA9001 silicon tuner"
 	depends on MEDIA_SUPPORT && I2C
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Infineon TUA 9001 silicon tuner driver.
@@ -258,6 +260,7 @@ config MEDIA_TUNER_R820T
 	tristate "Rafael Micro R820T silicon tuner"
 	depends on MEDIA_SUPPORT && I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
+	select BITREVERSE
 	help
 	  Rafael Micro R820T silicon tuner driver.
 
diff --git a/drivers/media/tuners/e4000.c b/drivers/media/tuners/e4000.c
index 510239f80c0d..03538f88f488 100644
--- a/drivers/media/tuners/e4000.c
+++ b/drivers/media/tuners/e4000.c
@@ -19,164 +19,176 @@
  */
 
 #include "e4000_priv.h"
-#include <linux/math64.h>
 
-static int e4000_init(struct dvb_frontend *fe)
+static int e4000_init(struct e4000_dev *dev)
 {
-	struct e4000 *s = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
 
-	dev_dbg(&s->client->dev, "\n");
-
-	/* dummy I2C to ensure I2C wakes up */
-	ret = regmap_write(s->regmap, 0x02, 0x40);
+	dev_dbg(&client->dev, "\n");
 
 	/* reset */
-	ret = regmap_write(s->regmap, 0x00, 0x01);
+	ret = regmap_write(dev->regmap, 0x00, 0x01);
 	if (ret)
 		goto err;
 
 	/* disable output clock */
-	ret = regmap_write(s->regmap, 0x06, 0x00);
+	ret = regmap_write(dev->regmap, 0x06, 0x00);
 	if (ret)
 		goto err;
 
-	ret = regmap_write(s->regmap, 0x7a, 0x96);
+	ret = regmap_write(dev->regmap, 0x7a, 0x96);
 	if (ret)
 		goto err;
 
 	/* configure gains */
-	ret = regmap_bulk_write(s->regmap, 0x7e, "\x01\xfe", 2);
+	ret = regmap_bulk_write(dev->regmap, 0x7e, "\x01\xfe", 2);
 	if (ret)
 		goto err;
 
-	ret = regmap_write(s->regmap, 0x82, 0x00);
+	ret = regmap_write(dev->regmap, 0x82, 0x00);
 	if (ret)
 		goto err;
 
-	ret = regmap_write(s->regmap, 0x24, 0x05);
+	ret = regmap_write(dev->regmap, 0x24, 0x05);
 	if (ret)
 		goto err;
 
-	ret = regmap_bulk_write(s->regmap, 0x87, "\x20\x01", 2);
+	ret = regmap_bulk_write(dev->regmap, 0x87, "\x20\x01", 2);
 	if (ret)
 		goto err;
 
-	ret = regmap_bulk_write(s->regmap, 0x9f, "\x7f\x07", 2);
+	ret = regmap_bulk_write(dev->regmap, 0x9f, "\x7f\x07", 2);
 	if (ret)
 		goto err;
 
 	/* DC offset control */
-	ret = regmap_write(s->regmap, 0x2d, 0x1f);
+	ret = regmap_write(dev->regmap, 0x2d, 0x1f);
 	if (ret)
 		goto err;
 
-	ret = regmap_bulk_write(s->regmap, 0x70, "\x01\x01", 2);
+	ret = regmap_bulk_write(dev->regmap, 0x70, "\x01\x01", 2);
 	if (ret)
 		goto err;
 
 	/* gain control */
-	ret = regmap_write(s->regmap, 0x1a, 0x17);
+	ret = regmap_write(dev->regmap, 0x1a, 0x17);
 	if (ret)
 		goto err;
 
-	ret = regmap_write(s->regmap, 0x1f, 0x1a);
+	ret = regmap_write(dev->regmap, 0x1f, 0x1a);
 	if (ret)
 		goto err;
 
-	s->active = true;
-err:
-	if (ret)
-		dev_dbg(&s->client->dev, "failed=%d\n", ret);
+	dev->active = true;
 
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-static int e4000_sleep(struct dvb_frontend *fe)
+static int e4000_sleep(struct e4000_dev *dev)
 {
-	struct e4000 *s = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
 
-	dev_dbg(&s->client->dev, "\n");
+	dev_dbg(&client->dev, "\n");
 
-	s->active = false;
+	dev->active = false;
 
-	ret = regmap_write(s->regmap, 0x00, 0x00);
+	ret = regmap_write(dev->regmap, 0x00, 0x00);
 	if (ret)
 		goto err;
-err:
-	if (ret)
-		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-static int e4000_set_params(struct dvb_frontend *fe)
+static int e4000_set_params(struct e4000_dev *dev)
 {
-	struct e4000 *s = fe->tuner_priv;
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret, i, sigma_delta;
-	unsigned int pll_n, pll_f;
+	struct i2c_client *client = dev->client;
+	int ret, i;
+	unsigned int div_n, k, k_cw, div_out;
 	u64 f_vco;
 	u8 buf[5], i_data[4], q_data[4];
 
-	dev_dbg(&s->client->dev,
-			"delivery_system=%d frequency=%u bandwidth_hz=%u\n",
-			c->delivery_system, c->frequency, c->bandwidth_hz);
+	if (!dev->active) {
+		dev_dbg(&client->dev, "tuner is sleeping\n");
+		return 0;
+	}
 
 	/* gain control manual */
-	ret = regmap_write(s->regmap, 0x1a, 0x00);
-	if (ret)
-		goto err;
-
-	/* PLL */
+	ret = regmap_write(dev->regmap, 0x1a, 0x00);
+	if (ret)
+		goto err;
+
+	/*
+	 * Fractional-N synthesizer
+	 *
+	 *           +----------------------------+
+	 *           v                            |
+	 *  Fref   +----+     +-------+         +------+     +---+
+	 * ------> | PD | --> |  VCO  | ------> | /N.F | <-- | K |
+	 *         +----+     +-------+         +------+     +---+
+	 *                      |
+	 *                      |
+	 *                      v
+	 *                    +-------+  Fout
+	 *                    | /Rout | ------>
+	 *                    +-------+
+	 */
 	for (i = 0; i < ARRAY_SIZE(e4000_pll_lut); i++) {
-		if (c->frequency <= e4000_pll_lut[i].freq)
+		if (dev->f_frequency <= e4000_pll_lut[i].freq)
 			break;
 	}
-
 	if (i == ARRAY_SIZE(e4000_pll_lut)) {
 		ret = -EINVAL;
 		goto err;
 	}
 
-	f_vco = 1ull * c->frequency * e4000_pll_lut[i].mul;
-	pll_n = div_u64_rem(f_vco, s->clock, &pll_f);
-	sigma_delta = div_u64(0x10000ULL * pll_f, s->clock);
-	buf[0] = pll_n;
-	buf[1] = (sigma_delta >> 0) & 0xff;
-	buf[2] = (sigma_delta >> 8) & 0xff;
+	#define F_REF dev->clk
+	div_out = e4000_pll_lut[i].div_out;
+	f_vco = (u64) dev->f_frequency * div_out;
+	/* calculate PLL integer and fractional control word */
+	div_n = div_u64_rem(f_vco, F_REF, &k);
+	k_cw = div_u64((u64) k * 0x10000, F_REF);
+
+	dev_dbg(&client->dev,
+		"frequency=%u bandwidth=%u f_vco=%llu F_REF=%u div_n=%u k=%u k_cw=%04x div_out=%u\n",
+		dev->f_frequency, dev->f_bandwidth, f_vco, F_REF, div_n, k,
+		k_cw, div_out);
+
+	buf[0] = div_n;
+	buf[1] = (k_cw >> 0) & 0xff;
+	buf[2] = (k_cw >> 8) & 0xff;
 	buf[3] = 0x00;
-	buf[4] = e4000_pll_lut[i].div;
-
-	dev_dbg(&s->client->dev, "f_vco=%llu pll div=%d sigma_delta=%04x\n",
-			f_vco, buf[0], sigma_delta);
-
-	ret = regmap_bulk_write(s->regmap, 0x09, buf, 5);
+	buf[4] = e4000_pll_lut[i].div_out_reg;
+	ret = regmap_bulk_write(dev->regmap, 0x09, buf, 5);
 	if (ret)
 		goto err;
 
 	/* LNA filter (RF filter) */
 	for (i = 0; i < ARRAY_SIZE(e400_lna_filter_lut); i++) {
-		if (c->frequency <= e400_lna_filter_lut[i].freq)
+		if (dev->f_frequency <= e400_lna_filter_lut[i].freq)
 			break;
 	}
-
 	if (i == ARRAY_SIZE(e400_lna_filter_lut)) {
 		ret = -EINVAL;
 		goto err;
 	}
 
-	ret = regmap_write(s->regmap, 0x10, e400_lna_filter_lut[i].val);
+	ret = regmap_write(dev->regmap, 0x10, e400_lna_filter_lut[i].val);
 	if (ret)
 		goto err;
 
 	/* IF filters */
 	for (i = 0; i < ARRAY_SIZE(e4000_if_filter_lut); i++) {
-		if (c->bandwidth_hz <= e4000_if_filter_lut[i].freq)
+		if (dev->f_bandwidth <= e4000_if_filter_lut[i].freq)
 			break;
 	}
-
 	if (i == ARRAY_SIZE(e4000_if_filter_lut)) {
 		ret = -EINVAL;
 		goto err;
@@ -185,48 +197,47 @@ static int e4000_set_params(struct dvb_frontend *fe)
 	buf[0] = e4000_if_filter_lut[i].reg11_val;
 	buf[1] = e4000_if_filter_lut[i].reg12_val;
 
-	ret = regmap_bulk_write(s->regmap, 0x11, buf, 2);
+	ret = regmap_bulk_write(dev->regmap, 0x11, buf, 2);
 	if (ret)
 		goto err;
 
 	/* frequency band */
 	for (i = 0; i < ARRAY_SIZE(e4000_band_lut); i++) {
-		if (c->frequency <= e4000_band_lut[i].freq)
+		if (dev->f_frequency <= e4000_band_lut[i].freq)
 			break;
 	}
-
 	if (i == ARRAY_SIZE(e4000_band_lut)) {
 		ret = -EINVAL;
 		goto err;
 	}
 
-	ret = regmap_write(s->regmap, 0x07, e4000_band_lut[i].reg07_val);
+	ret = regmap_write(dev->regmap, 0x07, e4000_band_lut[i].reg07_val);
 	if (ret)
 		goto err;
 
-	ret = regmap_write(s->regmap, 0x78, e4000_band_lut[i].reg78_val);
+	ret = regmap_write(dev->regmap, 0x78, e4000_band_lut[i].reg78_val);
 	if (ret)
 		goto err;
 
 	/* DC offset */
 	for (i = 0; i < 4; i++) {
 		if (i == 0)
-			ret = regmap_bulk_write(s->regmap, 0x15, "\x00\x7e\x24", 3);
+			ret = regmap_bulk_write(dev->regmap, 0x15, "\x00\x7e\x24", 3);
 		else if (i == 1)
-			ret = regmap_bulk_write(s->regmap, 0x15, "\x00\x7f", 2);
+			ret = regmap_bulk_write(dev->regmap, 0x15, "\x00\x7f", 2);
 		else if (i == 2)
-			ret = regmap_bulk_write(s->regmap, 0x15, "\x01", 1);
+			ret = regmap_bulk_write(dev->regmap, 0x15, "\x01", 1);
 		else
-			ret = regmap_bulk_write(s->regmap, 0x16, "\x7e", 1);
+			ret = regmap_bulk_write(dev->regmap, 0x16, "\x7e", 1);
 
 		if (ret)
 			goto err;
 
-		ret = regmap_write(s->regmap, 0x29, 0x01);
+		ret = regmap_write(dev->regmap, 0x29, 0x01);
 		if (ret)
 			goto err;
 
-		ret = regmap_bulk_read(s->regmap, 0x2a, buf, 3);
+		ret = regmap_bulk_read(dev->regmap, 0x2a, buf, 3);
 		if (ret)
 			goto err;
 
@@ -237,174 +248,294 @@ static int e4000_set_params(struct dvb_frontend *fe)
 	swap(q_data[2], q_data[3]);
 	swap(i_data[2], i_data[3]);
 
-	ret = regmap_bulk_write(s->regmap, 0x50, q_data, 4);
+	ret = regmap_bulk_write(dev->regmap, 0x50, q_data, 4);
 	if (ret)
 		goto err;
 
-	ret = regmap_bulk_write(s->regmap, 0x60, i_data, 4);
+	ret = regmap_bulk_write(dev->regmap, 0x60, i_data, 4);
 	if (ret)
 		goto err;
 
 	/* gain control auto */
-	ret = regmap_write(s->regmap, 0x1a, 0x17);
+	ret = regmap_write(dev->regmap, 0x1a, 0x17);
 	if (ret)
 		goto err;
+
+	return 0;
 err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
+}
+
+/*
+ * V4L2 API
+ */
+#if IS_ENABLED(CONFIG_VIDEO_V4L2)
+static const struct v4l2_frequency_band bands[] = {
+	{
+		.type = V4L2_TUNER_RF,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =    59000000,
+		.rangehigh  =  1105000000,
+	},
+	{
+		.type = V4L2_TUNER_RF,
+		.index = 1,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =  1249000000,
+		.rangehigh  =  2208000000UL,
+	},
+};
+
+static inline struct e4000_dev *e4000_subdev_to_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct e4000_dev, sd);
+}
+
+static int e4000_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct e4000_dev *dev = e4000_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+	int ret;
+
+	dev_dbg(&client->dev, "on=%d\n", on);
+
+	if (on)
+		ret = e4000_init(dev);
+	else
+		ret = e4000_sleep(dev);
 	if (ret)
-		dev_dbg(&s->client->dev, "failed=%d\n", ret);
+		return ret;
 
-	return ret;
+	return e4000_set_params(dev);
+}
+
+static const struct v4l2_subdev_core_ops e4000_subdev_core_ops = {
+	.s_power                  = e4000_s_power,
+};
+
+static int e4000_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
+{
+	struct e4000_dev *dev = e4000_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+
+	dev_dbg(&client->dev, "index=%d\n", v->index);
+
+	strlcpy(v->name, "Elonics E4000", sizeof(v->name));
+	v->type = V4L2_TUNER_RF;
+	v->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+	v->rangelow  = bands[0].rangelow;
+	v->rangehigh = bands[1].rangehigh;
+	return 0;
 }
 
-static int e4000_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+static int e4000_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *v)
 {
-	struct e4000 *s = fe->tuner_priv;
+	struct e4000_dev *dev = e4000_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
 
-	dev_dbg(&s->client->dev, "\n");
+	dev_dbg(&client->dev, "index=%d\n", v->index);
+	return 0;
+}
 
-	*frequency = 0; /* Zero-IF */
+static int e4000_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
+{
+	struct e4000_dev *dev = e4000_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
 
+	dev_dbg(&client->dev, "tuner=%d\n", f->tuner);
+	f->frequency = dev->f_frequency;
 	return 0;
 }
 
-#if IS_ENABLED(CONFIG_VIDEO_V4L2)
+static int e4000_s_frequency(struct v4l2_subdev *sd,
+			      const struct v4l2_frequency *f)
+{
+	struct e4000_dev *dev = e4000_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+
+	dev_dbg(&client->dev, "tuner=%d type=%d frequency=%u\n",
+		f->tuner, f->type, f->frequency);
+
+	dev->f_frequency = clamp_t(unsigned int, f->frequency,
+				   bands[0].rangelow, bands[1].rangehigh);
+	return e4000_set_params(dev);
+}
+
+static int e4000_enum_freq_bands(struct v4l2_subdev *sd,
+				  struct v4l2_frequency_band *band)
+{
+	struct e4000_dev *dev = e4000_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+
+	dev_dbg(&client->dev, "tuner=%d type=%d index=%d\n",
+		band->tuner, band->type, band->index);
+
+	if (band->index >= ARRAY_SIZE(bands))
+		return -EINVAL;
+
+	band->capability = bands[band->index].capability;
+	band->rangelow = bands[band->index].rangelow;
+	band->rangehigh = bands[band->index].rangehigh;
+	return 0;
+}
+
+static const struct v4l2_subdev_tuner_ops e4000_subdev_tuner_ops = {
+	.g_tuner                  = e4000_g_tuner,
+	.s_tuner                  = e4000_s_tuner,
+	.g_frequency              = e4000_g_frequency,
+	.s_frequency              = e4000_s_frequency,
+	.enum_freq_bands          = e4000_enum_freq_bands,
+};
+
+static const struct v4l2_subdev_ops e4000_subdev_ops = {
+	.core                     = &e4000_subdev_core_ops,
+	.tuner                    = &e4000_subdev_tuner_ops,
+};
+
 static int e4000_set_lna_gain(struct dvb_frontend *fe)
 {
-	struct e4000 *s = fe->tuner_priv;
+	struct e4000_dev *dev = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
 	u8 u8tmp;
 
-	dev_dbg(&s->client->dev, "lna auto=%d->%d val=%d->%d\n",
-			s->lna_gain_auto->cur.val, s->lna_gain_auto->val,
-			s->lna_gain->cur.val, s->lna_gain->val);
+	dev_dbg(&client->dev, "lna auto=%d->%d val=%d->%d\n",
+		dev->lna_gain_auto->cur.val, dev->lna_gain_auto->val,
+		dev->lna_gain->cur.val, dev->lna_gain->val);
 
-	if (s->lna_gain_auto->val && s->if_gain_auto->cur.val)
+	if (dev->lna_gain_auto->val && dev->if_gain_auto->cur.val)
 		u8tmp = 0x17;
-	else if (s->lna_gain_auto->val)
+	else if (dev->lna_gain_auto->val)
 		u8tmp = 0x19;
-	else if (s->if_gain_auto->cur.val)
+	else if (dev->if_gain_auto->cur.val)
 		u8tmp = 0x16;
 	else
 		u8tmp = 0x10;
 
-	ret = regmap_write(s->regmap, 0x1a, u8tmp);
+	ret = regmap_write(dev->regmap, 0x1a, u8tmp);
 	if (ret)
 		goto err;
 
-	if (s->lna_gain_auto->val == false) {
-		ret = regmap_write(s->regmap, 0x14, s->lna_gain->val);
+	if (dev->lna_gain_auto->val == false) {
+		ret = regmap_write(dev->regmap, 0x14, dev->lna_gain->val);
 		if (ret)
 			goto err;
 	}
-err:
-	if (ret)
-		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int e4000_set_mixer_gain(struct dvb_frontend *fe)
 {
-	struct e4000 *s = fe->tuner_priv;
+	struct e4000_dev *dev = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
 	u8 u8tmp;
 
-	dev_dbg(&s->client->dev, "mixer auto=%d->%d val=%d->%d\n",
-			s->mixer_gain_auto->cur.val, s->mixer_gain_auto->val,
-			s->mixer_gain->cur.val, s->mixer_gain->val);
+	dev_dbg(&client->dev, "mixer auto=%d->%d val=%d->%d\n",
+		dev->mixer_gain_auto->cur.val, dev->mixer_gain_auto->val,
+		dev->mixer_gain->cur.val, dev->mixer_gain->val);
 
-	if (s->mixer_gain_auto->val)
+	if (dev->mixer_gain_auto->val)
 		u8tmp = 0x15;
 	else
 		u8tmp = 0x14;
 
-	ret = regmap_write(s->regmap, 0x20, u8tmp);
+	ret = regmap_write(dev->regmap, 0x20, u8tmp);
 	if (ret)
 		goto err;
 
-	if (s->mixer_gain_auto->val == false) {
-		ret = regmap_write(s->regmap, 0x15, s->mixer_gain->val);
+	if (dev->mixer_gain_auto->val == false) {
+		ret = regmap_write(dev->regmap, 0x15, dev->mixer_gain->val);
 		if (ret)
 			goto err;
 	}
-err:
-	if (ret)
-		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int e4000_set_if_gain(struct dvb_frontend *fe)
 {
-	struct e4000 *s = fe->tuner_priv;
+	struct e4000_dev *dev = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
 	u8 buf[2];
 	u8 u8tmp;
 
-	dev_dbg(&s->client->dev, "if auto=%d->%d val=%d->%d\n",
-			s->if_gain_auto->cur.val, s->if_gain_auto->val,
-			s->if_gain->cur.val, s->if_gain->val);
+	dev_dbg(&client->dev, "if auto=%d->%d val=%d->%d\n",
+		dev->if_gain_auto->cur.val, dev->if_gain_auto->val,
+		dev->if_gain->cur.val, dev->if_gain->val);
 
-	if (s->if_gain_auto->val && s->lna_gain_auto->cur.val)
+	if (dev->if_gain_auto->val && dev->lna_gain_auto->cur.val)
 		u8tmp = 0x17;
-	else if (s->lna_gain_auto->cur.val)
+	else if (dev->lna_gain_auto->cur.val)
 		u8tmp = 0x19;
-	else if (s->if_gain_auto->val)
+	else if (dev->if_gain_auto->val)
 		u8tmp = 0x16;
 	else
 		u8tmp = 0x10;
 
-	ret = regmap_write(s->regmap, 0x1a, u8tmp);
+	ret = regmap_write(dev->regmap, 0x1a, u8tmp);
 	if (ret)
 		goto err;
 
-	if (s->if_gain_auto->val == false) {
-		buf[0] = e4000_if_gain_lut[s->if_gain->val].reg16_val;
-		buf[1] = e4000_if_gain_lut[s->if_gain->val].reg17_val;
-		ret = regmap_bulk_write(s->regmap, 0x16, buf, 2);
+	if (dev->if_gain_auto->val == false) {
+		buf[0] = e4000_if_gain_lut[dev->if_gain->val].reg16_val;
+		buf[1] = e4000_if_gain_lut[dev->if_gain->val].reg17_val;
+		ret = regmap_bulk_write(dev->regmap, 0x16, buf, 2);
 		if (ret)
 			goto err;
 	}
-err:
-	if (ret)
-		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int e4000_pll_lock(struct dvb_frontend *fe)
 {
-	struct e4000 *s = fe->tuner_priv;
+	struct e4000_dev *dev = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
-	unsigned int utmp;
+	unsigned int uitmp;
 
-	ret = regmap_read(s->regmap, 0x07, &utmp);
+	ret = regmap_read(dev->regmap, 0x07, &uitmp);
 	if (ret)
 		goto err;
 
-	s->pll_lock->val = (utmp & 0x01);
-err:
-	if (ret)
-		dev_dbg(&s->client->dev, "failed=%d\n", ret);
+	dev->pll_lock->val = (uitmp & 0x01);
 
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int e4000_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct e4000 *s = container_of(ctrl->handler, struct e4000, hdl);
+	struct e4000_dev *dev = container_of(ctrl->handler, struct e4000_dev, hdl);
+	struct i2c_client *client = dev->client;
 	int ret;
 
-	if (!s->active)
+	if (!dev->active)
 		return 0;
 
 	switch (ctrl->id) {
 	case  V4L2_CID_RF_TUNER_PLL_LOCK:
-		ret = e4000_pll_lock(s->fe);
+		ret = e4000_pll_lock(dev->fe);
 		break;
 	default:
-		dev_dbg(&s->client->dev, "unknown ctrl: id=%d name=%s\n",
-				ctrl->id, ctrl->name);
+		dev_dbg(&client->dev, "unknown ctrl: id=%d name=%s\n",
+			ctrl->id, ctrl->name);
 		ret = -EINVAL;
 	}
 
@@ -413,35 +544,39 @@ static int e4000_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 
 static int e4000_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct e4000 *s = container_of(ctrl->handler, struct e4000, hdl);
-	struct dvb_frontend *fe = s->fe;
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct e4000_dev *dev = container_of(ctrl->handler, struct e4000_dev, hdl);
+	struct i2c_client *client = dev->client;
 	int ret;
 
-	if (!s->active)
+	if (!dev->active)
 		return 0;
 
 	switch (ctrl->id) {
 	case V4L2_CID_RF_TUNER_BANDWIDTH_AUTO:
 	case V4L2_CID_RF_TUNER_BANDWIDTH:
-		c->bandwidth_hz = s->bandwidth->val;
-		ret = e4000_set_params(s->fe);
+		/*
+		 * TODO: Auto logic does not work 100% correctly as tuner driver
+		 * do not have information to calculate maximum suitable
+		 * bandwidth. Calculating it is responsible of master driver.
+		 */
+		dev->f_bandwidth = dev->bandwidth->val;
+		ret = e4000_set_params(dev);
 		break;
 	case  V4L2_CID_RF_TUNER_LNA_GAIN_AUTO:
 	case  V4L2_CID_RF_TUNER_LNA_GAIN:
-		ret = e4000_set_lna_gain(s->fe);
+		ret = e4000_set_lna_gain(dev->fe);
 		break;
 	case  V4L2_CID_RF_TUNER_MIXER_GAIN_AUTO:
 	case  V4L2_CID_RF_TUNER_MIXER_GAIN:
-		ret = e4000_set_mixer_gain(s->fe);
+		ret = e4000_set_mixer_gain(dev->fe);
 		break;
 	case  V4L2_CID_RF_TUNER_IF_GAIN_AUTO:
 	case  V4L2_CID_RF_TUNER_IF_GAIN:
-		ret = e4000_set_if_gain(s->fe);
+		ret = e4000_set_if_gain(dev->fe);
 		break;
 	default:
-		dev_dbg(&s->client->dev, "unknown ctrl: id=%d name=%s\n",
-				ctrl->id, ctrl->name);
+		dev_dbg(&client->dev, "unknown ctrl: id=%d name=%s\n",
+			ctrl->id, ctrl->name);
 		ret = -EINVAL;
 	}
 
@@ -454,157 +589,176 @@ static const struct v4l2_ctrl_ops e4000_ctrl_ops = {
 };
 #endif
 
-static const struct dvb_tuner_ops e4000_tuner_ops = {
+/*
+ * DVB API
+ */
+static int e4000_dvb_set_params(struct dvb_frontend *fe)
+{
+	struct e4000_dev *dev = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	dev->f_frequency = c->frequency;
+	dev->f_bandwidth = c->bandwidth_hz;
+	return e4000_set_params(dev);
+}
+
+static int e4000_dvb_init(struct dvb_frontend *fe)
+{
+	return e4000_init(fe->tuner_priv);
+}
+
+static int e4000_dvb_sleep(struct dvb_frontend *fe)
+{
+	return e4000_sleep(fe->tuner_priv);
+}
+
+static int e4000_dvb_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	*frequency = 0; /* Zero-IF */
+	return 0;
+}
+
+static const struct dvb_tuner_ops e4000_dvb_tuner_ops = {
 	.info = {
 		.name           = "Elonics E4000",
 		.frequency_min  = 174000000,
 		.frequency_max  = 862000000,
 	},
 
-	.init = e4000_init,
-	.sleep = e4000_sleep,
-	.set_params = e4000_set_params,
+	.init = e4000_dvb_init,
+	.sleep = e4000_dvb_sleep,
+	.set_params = e4000_dvb_set_params,
 
-	.get_if_frequency = e4000_get_if_frequency,
+	.get_if_frequency = e4000_dvb_get_if_frequency,
 };
 
-/*
- * Use V4L2 subdev to carry V4L2 control handler, even we don't implement
- * subdev itself, just to avoid reinventing the wheel.
- */
 static int e4000_probe(struct i2c_client *client,
-		const struct i2c_device_id *id)
+		       const struct i2c_device_id *id)
 {
+	struct e4000_dev *dev;
 	struct e4000_config *cfg = client->dev.platform_data;
 	struct dvb_frontend *fe = cfg->fe;
-	struct e4000 *s;
 	int ret;
-	unsigned int utmp;
+	unsigned int uitmp;
 	static const struct regmap_config regmap_config = {
 		.reg_bits = 8,
 		.val_bits = 8,
-		.max_register = 0xff,
 	};
 
-	s = kzalloc(sizeof(struct e4000), GFP_KERNEL);
-	if (!s) {
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
 		ret = -ENOMEM;
-		dev_err(&client->dev, "kzalloc() failed\n");
 		goto err;
 	}
 
-	s->clock = cfg->clock;
-	s->client = client;
-	s->fe = cfg->fe;
-	s->regmap = devm_regmap_init_i2c(client, &regmap_config);
-	if (IS_ERR(s->regmap)) {
-		ret = PTR_ERR(s->regmap);
-		goto err;
+	dev->clk = cfg->clock;
+	dev->client = client;
+	dev->fe = cfg->fe;
+	dev->regmap = devm_regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(dev->regmap)) {
+		ret = PTR_ERR(dev->regmap);
+		goto err_kfree;
 	}
 
 	/* check if the tuner is there */
-	ret = regmap_read(s->regmap, 0x02, &utmp);
+	ret = regmap_read(dev->regmap, 0x02, &uitmp);
 	if (ret)
-		goto err;
+		goto err_kfree;
 
-	dev_dbg(&s->client->dev, "chip id=%02x\n", utmp);
+	dev_dbg(&client->dev, "chip id=%02x\n", uitmp);
 
-	if (utmp != 0x40) {
+	if (uitmp != 0x40) {
 		ret = -ENODEV;
-		goto err;
+		goto err_kfree;
 	}
 
 	/* put sleep as chip seems to be in normal mode by default */
-	ret = regmap_write(s->regmap, 0x00, 0x00);
+	ret = regmap_write(dev->regmap, 0x00, 0x00);
 	if (ret)
-		goto err;
+		goto err_kfree;
 
 #if IS_ENABLED(CONFIG_VIDEO_V4L2)
 	/* Register controls */
-	v4l2_ctrl_handler_init(&s->hdl, 9);
-	s->bandwidth_auto = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	v4l2_ctrl_handler_init(&dev->hdl, 9);
+	dev->bandwidth_auto = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_BANDWIDTH_AUTO, 0, 1, 1, 1);
-	s->bandwidth = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	dev->bandwidth = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_BANDWIDTH, 4300000, 11000000, 100000, 4300000);
-	v4l2_ctrl_auto_cluster(2, &s->bandwidth_auto, 0, false);
-	s->lna_gain_auto = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	v4l2_ctrl_auto_cluster(2, &dev->bandwidth_auto, 0, false);
+	dev->lna_gain_auto = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_LNA_GAIN_AUTO, 0, 1, 1, 1);
-	s->lna_gain = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	dev->lna_gain = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_LNA_GAIN, 0, 15, 1, 10);
-	v4l2_ctrl_auto_cluster(2, &s->lna_gain_auto, 0, false);
-	s->mixer_gain_auto = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	v4l2_ctrl_auto_cluster(2, &dev->lna_gain_auto, 0, false);
+	dev->mixer_gain_auto = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_MIXER_GAIN_AUTO, 0, 1, 1, 1);
-	s->mixer_gain = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	dev->mixer_gain = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_MIXER_GAIN, 0, 1, 1, 1);
-	v4l2_ctrl_auto_cluster(2, &s->mixer_gain_auto, 0, false);
-	s->if_gain_auto = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	v4l2_ctrl_auto_cluster(2, &dev->mixer_gain_auto, 0, false);
+	dev->if_gain_auto = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_IF_GAIN_AUTO, 0, 1, 1, 1);
-	s->if_gain = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	dev->if_gain = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_IF_GAIN, 0, 54, 1, 0);
-	v4l2_ctrl_auto_cluster(2, &s->if_gain_auto, 0, false);
-	s->pll_lock = v4l2_ctrl_new_std(&s->hdl, &e4000_ctrl_ops,
+	v4l2_ctrl_auto_cluster(2, &dev->if_gain_auto, 0, false);
+	dev->pll_lock = v4l2_ctrl_new_std(&dev->hdl, &e4000_ctrl_ops,
 			V4L2_CID_RF_TUNER_PLL_LOCK,  0, 1, 1, 0);
-	if (s->hdl.error) {
-		ret = s->hdl.error;
-		dev_err(&s->client->dev, "Could not initialize controls\n");
-		v4l2_ctrl_handler_free(&s->hdl);
-		goto err;
+	if (dev->hdl.error) {
+		ret = dev->hdl.error;
+		dev_err(&client->dev, "Could not initialize controls\n");
+		v4l2_ctrl_handler_free(&dev->hdl);
+		goto err_kfree;
 	}
 
-	s->sd.ctrl_handler = &s->hdl;
+	dev->sd.ctrl_handler = &dev->hdl;
+	dev->f_frequency = bands[0].rangelow;
+	dev->f_bandwidth = dev->bandwidth->val;
+	v4l2_i2c_subdev_init(&dev->sd, client, &e4000_subdev_ops);
 #endif
+	fe->tuner_priv = dev;
+	memcpy(&fe->ops.tuner_ops, &e4000_dvb_tuner_ops,
+	       sizeof(fe->ops.tuner_ops));
+	v4l2_set_subdevdata(&dev->sd, client);
+	i2c_set_clientdata(client, &dev->sd);
 
-	dev_info(&s->client->dev, "Elonics E4000 successfully identified\n");
-
-	fe->tuner_priv = s;
-	memcpy(&fe->ops.tuner_ops, &e4000_tuner_ops,
-			sizeof(struct dvb_tuner_ops));
-
-	v4l2_set_subdevdata(&s->sd, client);
-	i2c_set_clientdata(client, &s->sd);
-
+	dev_info(&client->dev, "Elonics E4000 successfully identified\n");
 	return 0;
+err_kfree:
+	kfree(dev);
 err:
-	if (ret) {
-		dev_dbg(&client->dev, "failed=%d\n", ret);
-		kfree(s);
-	}
-
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int e4000_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
-	struct e4000 *s = container_of(sd, struct e4000, sd);
-	struct dvb_frontend *fe = s->fe;
+	struct e4000_dev *dev = container_of(sd, struct e4000_dev, sd);
 
 	dev_dbg(&client->dev, "\n");
 
 #if IS_ENABLED(CONFIG_VIDEO_V4L2)
-	v4l2_ctrl_handler_free(&s->hdl);
+	v4l2_ctrl_handler_free(&dev->hdl);
 #endif
-	memset(&fe->ops.tuner_ops, 0, sizeof(struct dvb_tuner_ops));
-	fe->tuner_priv = NULL;
-	kfree(s);
+	kfree(dev);
 
 	return 0;
 }
 
-static const struct i2c_device_id e4000_id[] = {
+static const struct i2c_device_id e4000_id_table[] = {
 	{"e4000", 0},
 	{}
 };
-MODULE_DEVICE_TABLE(i2c, e4000_id);
+MODULE_DEVICE_TABLE(i2c, e4000_id_table);
 
 static struct i2c_driver e4000_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
 		.name	= "e4000",
+		.suppress_bind_attrs = true,
 	},
 	.probe		= e4000_probe,
 	.remove		= e4000_remove,
-	.id_table	= e4000_id,
+	.id_table	= e4000_id_table,
 };
 
 module_i2c_driver(e4000_driver);
diff --git a/drivers/media/tuners/e4000.h b/drivers/media/tuners/e4000.h
index e74b8b2f2fc3..aa9340c05b43 100644
--- a/drivers/media/tuners/e4000.h
+++ b/drivers/media/tuners/e4000.h
@@ -21,7 +21,6 @@
 #ifndef E4000_H
 #define E4000_H
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
 /*
diff --git a/drivers/media/tuners/e4000_priv.h b/drivers/media/tuners/e4000_priv.h
index cb0070483e65..d6d5d11bbfe0 100644
--- a/drivers/media/tuners/e4000_priv.h
+++ b/drivers/media/tuners/e4000_priv.h
@@ -22,17 +22,20 @@
 #define E4000_PRIV_H
 
 #include "e4000.h"
+#include <linux/math64.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-subdev.h>
 #include <linux/regmap.h>
 
-struct e4000 {
+struct e4000_dev {
 	struct i2c_client *client;
 	struct regmap *regmap;
-	u32 clock;
+	u32 clk;
 	struct dvb_frontend *fe;
 	struct v4l2_subdev sd;
 	bool active;
+	unsigned int f_frequency;
+	unsigned int f_bandwidth;
 
 	/* Controls */
 	struct v4l2_ctrl_handler hdl;
@@ -49,8 +52,8 @@ struct e4000 {
 
 struct e4000_pll {
 	u32 freq;
-	u8 div;
-	u8 mul;
+	u8 div_out_reg;
+	u8 div_out;
 };
 
 static const struct e4000_pll e4000_pll_lut[] = {
diff --git a/drivers/media/tuners/fc0013.c b/drivers/media/tuners/fc0013.c
index b4162315773d..522690d97b42 100644
--- a/drivers/media/tuners/fc0013.c
+++ b/drivers/media/tuners/fc0013.c
@@ -217,8 +217,6 @@ static int fc0013_set_vhf_track(struct fc0013_priv *priv, u32 freq)
 	} else {			/* UHF and GPS */
 		ret = fc0013_writereg(priv, 0x1d, tmp | 0x1c);
 	}
-	if (ret)
-		goto error_out;
 error_out:
 	return ret;
 }
diff --git a/drivers/media/tuners/fc2580.c b/drivers/media/tuners/fc2580.c
index f0c9c42867de..12f916e53150 100644
--- a/drivers/media/tuners/fc2580.c
+++ b/drivers/media/tuners/fc2580.c
@@ -20,535 +20,628 @@
 
 #include "fc2580_priv.h"
 
-/* Max transfer size done by I2C transfer functions */
-#define MAX_XFER_SIZE  64
-
 /*
  * TODO:
  * I2C write and read works only for one single register. Multiple registers
  * could not be accessed using normal register address auto-increment.
  * There could be (very likely) register to change that behavior....
- *
- * Due to that limitation functions:
- *   fc2580_wr_regs()
- *   fc2580_rd_regs()
- * could not be used for accessing more than one register at once.
- *
- * TODO:
- * Currently it blind writes bunch of static registers from the
- * fc2580_freq_regs_lut[] when fc2580_set_params() is called. Add some
- * logic to reduce unneeded register writes.
  */
 
-/* write multiple registers */
-static int fc2580_wr_regs(struct fc2580_priv *priv, u8 reg, u8 *val, int len)
-{
-	int ret;
-	u8 buf[MAX_XFER_SIZE];
-	struct i2c_msg msg[1] = {
-		{
-			.addr = priv->cfg->i2c_addr,
-			.flags = 0,
-			.len = 1 + len,
-			.buf = buf,
-		}
-	};
-
-	if (1 + len > sizeof(buf)) {
-		dev_warn(&priv->i2c->dev,
-			 "%s: i2c wr reg=%04x: len=%d is too big!\n",
-			 KBUILD_MODNAME, reg, len);
-		return -EINVAL;
-	}
-
-	buf[0] = reg;
-	memcpy(&buf[1], val, len);
-
-	ret = i2c_transfer(priv->i2c, msg, 1);
-	if (ret == 1) {
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev, "%s: i2c wr failed=%d reg=%02x " \
-				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-	return ret;
-}
-
-/* read multiple registers */
-static int fc2580_rd_regs(struct fc2580_priv *priv, u8 reg, u8 *val, int len)
-{
-	int ret;
-	u8 buf[MAX_XFER_SIZE];
-	struct i2c_msg msg[2] = {
-		{
-			.addr = priv->cfg->i2c_addr,
-			.flags = 0,
-			.len = 1,
-			.buf = &reg,
-		}, {
-			.addr = priv->cfg->i2c_addr,
-			.flags = I2C_M_RD,
-			.len = len,
-			.buf = buf,
-		}
-	};
-
-	if (len > sizeof(buf)) {
-		dev_warn(&priv->i2c->dev,
-			 "%s: i2c rd reg=%04x: len=%d is too big!\n",
-			 KBUILD_MODNAME, reg, len);
-		return -EINVAL;
-	}
-
-	ret = i2c_transfer(priv->i2c, msg, 2);
-	if (ret == 2) {
-		memcpy(val, buf, len);
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev, "%s: i2c rd failed=%d reg=%02x " \
-				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-
-	return ret;
-}
-
-/* write single register */
-static int fc2580_wr_reg(struct fc2580_priv *priv, u8 reg, u8 val)
-{
-	return fc2580_wr_regs(priv, reg, &val, 1);
-}
-
-/* read single register */
-static int fc2580_rd_reg(struct fc2580_priv *priv, u8 reg, u8 *val)
-{
-	return fc2580_rd_regs(priv, reg, val, 1);
-}
-
 /* write single register conditionally only when value differs from 0xff
  * XXX: This is special routine meant only for writing fc2580_freq_regs_lut[]
  * values. Do not use for the other purposes. */
-static int fc2580_wr_reg_ff(struct fc2580_priv *priv, u8 reg, u8 val)
+static int fc2580_wr_reg_ff(struct fc2580_dev *dev, u8 reg, u8 val)
 {
 	if (val == 0xff)
 		return 0;
 	else
-		return fc2580_wr_regs(priv, reg, &val, 1);
+		return regmap_write(dev->regmap, reg, val);
 }
 
-static int fc2580_set_params(struct dvb_frontend *fe)
+static int fc2580_set_params(struct fc2580_dev *dev)
 {
-	struct fc2580_priv *priv = fe->tuner_priv;
-	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret = 0, i;
-	unsigned int r_val, n_val, k_val, k_val_reg, f_ref;
-	u8 tmp_val, r18_val;
+	struct i2c_client *client = dev->client;
+	int ret, i;
+	unsigned int uitmp, div_ref, div_ref_val, div_n, k, k_cw, div_out;
 	u64 f_vco;
+	u8 synth_config;
+	unsigned long timeout;
+
+	if (!dev->active) {
+		dev_dbg(&client->dev, "tuner is sleeping\n");
+		return 0;
+	}
 
 	/*
-	 * Fractional-N synthesizer/PLL.
-	 * Most likely all those PLL calculations are not correct. I am not
-	 * sure, but it looks like it is divider based Fractional-N synthesizer.
-	 * There is divider for reference clock too?
-	 * Anyhow, synthesizer calculation results seems to be quite correct.
+	 * Fractional-N synthesizer
+	 *
+	 *                      +---------------------------------------+
+	 *                      v                                       |
+	 *  Fref   +----+     +----+     +-------+         +----+     +------+     +---+
+	 * ------> | /R | --> | PD | --> |  VCO  | ------> | /2 | --> | /N.F | <-- | K |
+	 *         +----+     +----+     +-------+         +----+     +------+     +---+
+	 *                                 |
+	 *                                 |
+	 *                                 v
+	 *                               +-------+  Fout
+	 *                               | /Rout | ------>
+	 *                               +-------+
 	 */
-
-	dev_dbg(&priv->i2c->dev, "%s: delivery_system=%d frequency=%d " \
-			"bandwidth_hz=%d\n", __func__,
-			c->delivery_system, c->frequency, c->bandwidth_hz);
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1);
-
-	/* PLL */
 	for (i = 0; i < ARRAY_SIZE(fc2580_pll_lut); i++) {
-		if (c->frequency <= fc2580_pll_lut[i].freq)
+		if (dev->f_frequency <= fc2580_pll_lut[i].freq)
 			break;
 	}
-
-	if (i == ARRAY_SIZE(fc2580_pll_lut))
+	if (i == ARRAY_SIZE(fc2580_pll_lut)) {
+		ret = -EINVAL;
 		goto err;
+	}
 
-	f_vco = c->frequency;
-	f_vco *= fc2580_pll_lut[i].div;
-
-	if (f_vco >= 2600000000UL)
-		tmp_val = 0x0e | fc2580_pll_lut[i].band;
+	#define DIV_PRE_N 2
+	#define F_REF dev->clk
+	div_out = fc2580_pll_lut[i].div_out;
+	f_vco = (u64) dev->f_frequency * div_out;
+	synth_config = fc2580_pll_lut[i].band;
+	if (f_vco < 2600000000ULL)
+		synth_config |= 0x06;
 	else
-		tmp_val = 0x06 | fc2580_pll_lut[i].band;
-
-	ret = fc2580_wr_reg(priv, 0x02, tmp_val);
-	if (ret < 0)
-		goto err;
-
-	if (f_vco >= 2UL * 76 * priv->cfg->clock) {
-		r_val = 1;
-		r18_val = 0x00;
-	} else if (f_vco >= 1UL * 76 * priv->cfg->clock) {
-		r_val = 2;
-		r18_val = 0x10;
+		synth_config |= 0x0e;
+
+	/* select reference divider R (keep PLL div N in valid range) */
+	#define DIV_N_MIN 76
+	if (f_vco >= div_u64((u64) DIV_PRE_N * DIV_N_MIN * F_REF, 1)) {
+		div_ref = 1;
+		div_ref_val = 0x00;
+	} else if (f_vco >= div_u64((u64) DIV_PRE_N * DIV_N_MIN * F_REF, 2)) {
+		div_ref = 2;
+		div_ref_val = 0x10;
 	} else {
-		r_val = 4;
-		r18_val = 0x20;
+		div_ref = 4;
+		div_ref_val = 0x20;
 	}
 
-	f_ref = 2UL * priv->cfg->clock / r_val;
-	n_val = div_u64_rem(f_vco, f_ref, &k_val);
-	k_val_reg = div_u64(1ULL * k_val * (1 << 20), f_ref);
+	/* calculate PLL integer and fractional control word */
+	uitmp = DIV_PRE_N * F_REF / div_ref;
+	div_n = div_u64_rem(f_vco, uitmp, &k);
+	k_cw = div_u64((u64) k * 0x100000, uitmp);
 
-	ret = fc2580_wr_reg(priv, 0x18, r18_val | ((k_val_reg >> 16) & 0xff));
-	if (ret < 0)
-		goto err;
+	dev_dbg(&client->dev,
+		"frequency=%u bandwidth=%u f_vco=%llu F_REF=%u div_ref=%u div_n=%u k=%u div_out=%u k_cw=%0x\n",
+		dev->f_frequency, dev->f_bandwidth, f_vco, F_REF, div_ref,
+		div_n, k, div_out, k_cw);
 
-	ret = fc2580_wr_reg(priv, 0x1a, (k_val_reg >> 8) & 0xff);
-	if (ret < 0)
+	ret = regmap_write(dev->regmap, 0x02, synth_config);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg(priv, 0x1b, (k_val_reg >> 0) & 0xff);
-	if (ret < 0)
+	ret = regmap_write(dev->regmap, 0x18, div_ref_val << 0 | k_cw >> 16);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg(priv, 0x1c, n_val);
-	if (ret < 0)
+	ret = regmap_write(dev->regmap, 0x1a, (k_cw >> 8) & 0xff);
+	if (ret)
 		goto err;
 
-	if (priv->cfg->clock >= 28000000) {
-		ret = fc2580_wr_reg(priv, 0x4b, 0x22);
-		if (ret < 0)
-			goto err;
-	}
-
-	if (fc2580_pll_lut[i].band == 0x00) {
-		if (c->frequency <= 794000000)
-			tmp_val = 0x9f;
-		else
-			tmp_val = 0x8f;
+	ret = regmap_write(dev->regmap, 0x1b, (k_cw >> 0) & 0xff);
+	if (ret)
+		goto err;
 
-		ret = fc2580_wr_reg(priv, 0x2d, tmp_val);
-		if (ret < 0)
-			goto err;
-	}
+	ret = regmap_write(dev->regmap, 0x1c, div_n);
+	if (ret)
+		goto err;
 
 	/* registers */
 	for (i = 0; i < ARRAY_SIZE(fc2580_freq_regs_lut); i++) {
-		if (c->frequency <= fc2580_freq_regs_lut[i].freq)
+		if (dev->f_frequency <= fc2580_freq_regs_lut[i].freq)
 			break;
 	}
-
-	if (i == ARRAY_SIZE(fc2580_freq_regs_lut))
+	if (i == ARRAY_SIZE(fc2580_freq_regs_lut)) {
+		ret = -EINVAL;
 		goto err;
+	}
 
-	ret = fc2580_wr_reg_ff(priv, 0x25, fc2580_freq_regs_lut[i].r25_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x25, fc2580_freq_regs_lut[i].r25_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x27, fc2580_freq_regs_lut[i].r27_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x27, fc2580_freq_regs_lut[i].r27_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x28, fc2580_freq_regs_lut[i].r28_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x28, fc2580_freq_regs_lut[i].r28_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x29, fc2580_freq_regs_lut[i].r29_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x29, fc2580_freq_regs_lut[i].r29_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x2b, fc2580_freq_regs_lut[i].r2b_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x2b, fc2580_freq_regs_lut[i].r2b_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x2c, fc2580_freq_regs_lut[i].r2c_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x2c, fc2580_freq_regs_lut[i].r2c_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x2d, fc2580_freq_regs_lut[i].r2d_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x2d, fc2580_freq_regs_lut[i].r2d_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x30, fc2580_freq_regs_lut[i].r30_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x30, fc2580_freq_regs_lut[i].r30_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x44, fc2580_freq_regs_lut[i].r44_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x44, fc2580_freq_regs_lut[i].r44_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x50, fc2580_freq_regs_lut[i].r50_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x50, fc2580_freq_regs_lut[i].r50_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x53, fc2580_freq_regs_lut[i].r53_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x53, fc2580_freq_regs_lut[i].r53_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x5f, fc2580_freq_regs_lut[i].r5f_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x5f, fc2580_freq_regs_lut[i].r5f_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x61, fc2580_freq_regs_lut[i].r61_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x61, fc2580_freq_regs_lut[i].r61_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x62, fc2580_freq_regs_lut[i].r62_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x62, fc2580_freq_regs_lut[i].r62_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x63, fc2580_freq_regs_lut[i].r63_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x63, fc2580_freq_regs_lut[i].r63_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x67, fc2580_freq_regs_lut[i].r67_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x67, fc2580_freq_regs_lut[i].r67_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x68, fc2580_freq_regs_lut[i].r68_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x68, fc2580_freq_regs_lut[i].r68_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x69, fc2580_freq_regs_lut[i].r69_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x69, fc2580_freq_regs_lut[i].r69_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x6a, fc2580_freq_regs_lut[i].r6a_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x6a, fc2580_freq_regs_lut[i].r6a_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x6b, fc2580_freq_regs_lut[i].r6b_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x6b, fc2580_freq_regs_lut[i].r6b_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x6c, fc2580_freq_regs_lut[i].r6c_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x6c, fc2580_freq_regs_lut[i].r6c_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x6d, fc2580_freq_regs_lut[i].r6d_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x6d, fc2580_freq_regs_lut[i].r6d_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x6e, fc2580_freq_regs_lut[i].r6e_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x6e, fc2580_freq_regs_lut[i].r6e_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg_ff(priv, 0x6f, fc2580_freq_regs_lut[i].r6f_val);
-	if (ret < 0)
+	ret = fc2580_wr_reg_ff(dev, 0x6f, fc2580_freq_regs_lut[i].r6f_val);
+	if (ret)
 		goto err;
 
 	/* IF filters */
 	for (i = 0; i < ARRAY_SIZE(fc2580_if_filter_lut); i++) {
-		if (c->bandwidth_hz <= fc2580_if_filter_lut[i].freq)
+		if (dev->f_bandwidth <= fc2580_if_filter_lut[i].freq)
 			break;
 	}
-
-	if (i == ARRAY_SIZE(fc2580_if_filter_lut))
-		goto err;
-
-	ret = fc2580_wr_reg(priv, 0x36, fc2580_if_filter_lut[i].r36_val);
-	if (ret < 0)
+	if (i == ARRAY_SIZE(fc2580_if_filter_lut)) {
+		ret = -EINVAL;
 		goto err;
+	}
 
-	ret = fc2580_wr_reg(priv, 0x37, div_u64(1ULL * priv->cfg->clock *
-			fc2580_if_filter_lut[i].mul, 1000000000));
-	if (ret < 0)
+	ret = regmap_write(dev->regmap, 0x36, fc2580_if_filter_lut[i].r36_val);
+	if (ret)
 		goto err;
 
-	ret = fc2580_wr_reg(priv, 0x39, fc2580_if_filter_lut[i].r39_val);
-	if (ret < 0)
+	uitmp = (unsigned int) 8058000 - (dev->f_bandwidth * 122 / 100 / 2);
+	uitmp = div64_u64((u64) dev->clk * uitmp, 1000000000000ULL);
+	ret = regmap_write(dev->regmap, 0x37, uitmp);
+	if (ret)
 		goto err;
 
-	/* calibration? */
-	ret = fc2580_wr_reg(priv, 0x2e, 0x09);
-	if (ret < 0)
+	ret = regmap_write(dev->regmap, 0x39, fc2580_if_filter_lut[i].r39_val);
+	if (ret)
 		goto err;
 
-	for (i = 0; i < 5; i++) {
-		ret = fc2580_rd_reg(priv, 0x2f, &tmp_val);
-		if (ret < 0)
+	timeout = jiffies + msecs_to_jiffies(30);
+	for (uitmp = ~0xc0; !time_after(jiffies, timeout) && uitmp != 0xc0;) {
+		/* trigger filter */
+		ret = regmap_write(dev->regmap, 0x2e, 0x09);
+		if (ret)
 			goto err;
 
-		/* done when [7:6] are set */
-		if ((tmp_val & 0xc0) == 0xc0)
-			break;
-
-		ret = fc2580_wr_reg(priv, 0x2e, 0x01);
-		if (ret < 0)
+		/* locked when [7:6] are set (val: d7 6MHz, d5 7MHz, cd 8MHz) */
+		ret = regmap_read(dev->regmap, 0x2f, &uitmp);
+		if (ret)
 			goto err;
+		uitmp &= 0xc0;
 
-		ret = fc2580_wr_reg(priv, 0x2e, 0x09);
-		if (ret < 0)
+		ret = regmap_write(dev->regmap, 0x2e, 0x01);
+		if (ret)
 			goto err;
-
-		usleep_range(5000, 25000);
 	}
-
-	dev_dbg(&priv->i2c->dev, "%s: loop=%i\n", __func__, i);
-
-	ret = fc2580_wr_reg(priv, 0x2e, 0x01);
-	if (ret < 0)
-		goto err;
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
+	if (uitmp != 0xc0)
+		dev_dbg(&client->dev, "filter did not lock %02x\n", uitmp);
 
 	return 0;
 err:
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
-
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-static int fc2580_init(struct dvb_frontend *fe)
+static int fc2580_init(struct fc2580_dev *dev)
 {
-	struct fc2580_priv *priv = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	int ret, i;
 
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1);
+	dev_dbg(&client->dev, "\n");
 
 	for (i = 0; i < ARRAY_SIZE(fc2580_init_reg_vals); i++) {
-		ret = fc2580_wr_reg(priv, fc2580_init_reg_vals[i].reg,
+		ret = regmap_write(dev->regmap, fc2580_init_reg_vals[i].reg,
 				fc2580_init_reg_vals[i].val);
-		if (ret < 0)
+		if (ret)
 			goto err;
 	}
 
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
-
+	dev->active = true;
 	return 0;
 err:
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
-
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-static int fc2580_sleep(struct dvb_frontend *fe)
+static int fc2580_sleep(struct fc2580_dev *dev)
 {
-	struct fc2580_priv *priv = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	int ret;
 
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1);
+	dev->active = false;
 
-	ret = fc2580_wr_reg(priv, 0x02, 0x0a);
-	if (ret < 0)
+	ret = regmap_write(dev->regmap, 0x02, 0x0a);
+	if (ret)
 		goto err;
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
-
 	return 0;
 err:
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
-
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
-static int fc2580_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+/*
+ * DVB API
+ */
+static int fc2580_dvb_set_params(struct dvb_frontend *fe)
 {
-	struct fc2580_priv *priv = fe->tuner_priv;
-
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
-
-	*frequency = 0; /* Zero-IF */
+	struct fc2580_dev *dev = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
-	return 0;
+	dev->f_frequency = c->frequency;
+	dev->f_bandwidth = c->bandwidth_hz;
+	return fc2580_set_params(dev);
 }
 
-static int fc2580_release(struct dvb_frontend *fe)
+static int fc2580_dvb_init(struct dvb_frontend *fe)
 {
-	struct fc2580_priv *priv = fe->tuner_priv;
-
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+	return fc2580_init(fe->tuner_priv);
+}
 
-	kfree(fe->tuner_priv);
+static int fc2580_dvb_sleep(struct dvb_frontend *fe)
+{
+	return fc2580_sleep(fe->tuner_priv);
+}
 
+static int fc2580_dvb_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
+{
+	*frequency = 0; /* Zero-IF */
 	return 0;
 }
 
-static const struct dvb_tuner_ops fc2580_tuner_ops = {
+static const struct dvb_tuner_ops fc2580_dvb_tuner_ops = {
 	.info = {
 		.name           = "FCI FC2580",
 		.frequency_min  = 174000000,
 		.frequency_max  = 862000000,
 	},
 
-	.release = fc2580_release,
+	.init = fc2580_dvb_init,
+	.sleep = fc2580_dvb_sleep,
+	.set_params = fc2580_dvb_set_params,
+
+	.get_if_frequency = fc2580_dvb_get_if_frequency,
+};
+
+/*
+ * V4L2 API
+ */
+#if IS_ENABLED(CONFIG_VIDEO_V4L2)
+static const struct v4l2_frequency_band bands[] = {
+	{
+		.type = V4L2_TUNER_RF,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =   130000000,
+		.rangehigh  =  2000000000,
+	},
+};
+
+static inline struct fc2580_dev *fc2580_subdev_to_dev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct fc2580_dev, subdev);
+}
+
+static int fc2580_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct fc2580_dev *dev = fc2580_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+	int ret;
+
+	dev_dbg(&client->dev, "on=%d\n", on);
+
+	if (on)
+		ret = fc2580_init(dev);
+	else
+		ret = fc2580_sleep(dev);
+	if (ret)
+		return ret;
+
+	return fc2580_set_params(dev);
+}
+
+static const struct v4l2_subdev_core_ops fc2580_subdev_core_ops = {
+	.s_power                  = fc2580_s_power,
+};
+
+static int fc2580_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
+{
+	struct fc2580_dev *dev = fc2580_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+
+	dev_dbg(&client->dev, "index=%d\n", v->index);
+
+	strlcpy(v->name, "FCI FC2580", sizeof(v->name));
+	v->type = V4L2_TUNER_RF;
+	v->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+	v->rangelow  = bands[0].rangelow;
+	v->rangehigh = bands[0].rangehigh;
+	return 0;
+}
+
+static int fc2580_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *v)
+{
+	struct fc2580_dev *dev = fc2580_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+
+	dev_dbg(&client->dev, "index=%d\n", v->index);
+	return 0;
+}
+
+static int fc2580_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
+{
+	struct fc2580_dev *dev = fc2580_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+
+	dev_dbg(&client->dev, "tuner=%d\n", f->tuner);
+	f->frequency = dev->f_frequency;
+	return 0;
+}
+
+static int fc2580_s_frequency(struct v4l2_subdev *sd,
+			      const struct v4l2_frequency *f)
+{
+	struct fc2580_dev *dev = fc2580_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+
+	dev_dbg(&client->dev, "tuner=%d type=%d frequency=%u\n",
+		f->tuner, f->type, f->frequency);
+
+	dev->f_frequency = clamp_t(unsigned int, f->frequency,
+				   bands[0].rangelow, bands[0].rangehigh);
+	return fc2580_set_params(dev);
+}
+
+static int fc2580_enum_freq_bands(struct v4l2_subdev *sd,
+				  struct v4l2_frequency_band *band)
+{
+	struct fc2580_dev *dev = fc2580_subdev_to_dev(sd);
+	struct i2c_client *client = dev->client;
+
+	dev_dbg(&client->dev, "tuner=%d type=%d index=%d\n",
+		band->tuner, band->type, band->index);
 
-	.init = fc2580_init,
-	.sleep = fc2580_sleep,
-	.set_params = fc2580_set_params,
+	if (band->index >= ARRAY_SIZE(bands))
+		return -EINVAL;
 
-	.get_if_frequency = fc2580_get_if_frequency,
+	band->capability = bands[band->index].capability;
+	band->rangelow = bands[band->index].rangelow;
+	band->rangehigh = bands[band->index].rangehigh;
+	return 0;
+}
+
+static const struct v4l2_subdev_tuner_ops fc2580_subdev_tuner_ops = {
+	.g_tuner                  = fc2580_g_tuner,
+	.s_tuner                  = fc2580_s_tuner,
+	.g_frequency              = fc2580_g_frequency,
+	.s_frequency              = fc2580_s_frequency,
+	.enum_freq_bands          = fc2580_enum_freq_bands,
+};
+
+static const struct v4l2_subdev_ops fc2580_subdev_ops = {
+	.core                     = &fc2580_subdev_core_ops,
+	.tuner                    = &fc2580_subdev_tuner_ops,
 };
 
-struct dvb_frontend *fc2580_attach(struct dvb_frontend *fe,
-		struct i2c_adapter *i2c, const struct fc2580_config *cfg)
+static int fc2580_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct fc2580_priv *priv;
+	struct fc2580_dev *dev = container_of(ctrl->handler, struct fc2580_dev, hdl);
+	struct i2c_client *client = dev->client;
 	int ret;
-	u8 chip_id;
 
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1);
+	dev_dbg(&client->dev, "ctrl: id=%d name=%s cur.val=%d val=%d\n",
+		ctrl->id, ctrl->name, ctrl->cur.val, ctrl->val);
+
+	switch (ctrl->id) {
+	case V4L2_CID_RF_TUNER_BANDWIDTH_AUTO:
+	case V4L2_CID_RF_TUNER_BANDWIDTH:
+		/*
+		 * TODO: Auto logic does not work 100% correctly as tuner driver
+		 * do not have information to calculate maximum suitable
+		 * bandwidth. Calculating it is responsible of master driver.
+		 */
+		dev->f_bandwidth = dev->bandwidth->val;
+		ret = fc2580_set_params(dev);
+		break;
+	default:
+		dev_dbg(&client->dev, "unknown ctrl");
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops fc2580_ctrl_ops = {
+	.s_ctrl = fc2580_s_ctrl,
+};
+#endif
+
+static struct v4l2_subdev *fc2580_get_v4l2_subdev(struct i2c_client *client)
+{
+	struct fc2580_dev *dev = i2c_get_clientdata(client);
+
+	if (dev->subdev.ops)
+		return &dev->subdev;
+	else
+		return NULL;
+}
+
+static int fc2580_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct fc2580_dev *dev;
+	struct fc2580_platform_data *pdata = client->dev.platform_data;
+	struct dvb_frontend *fe = pdata->dvb_frontend;
+	int ret;
+	unsigned int uitmp;
+	static const struct regmap_config regmap_config = {
+		.reg_bits = 8,
+		.val_bits = 8,
+	};
 
-	priv = kzalloc(sizeof(struct fc2580_priv), GFP_KERNEL);
-	if (!priv) {
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
 		ret = -ENOMEM;
-		dev_err(&i2c->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
 		goto err;
 	}
 
-	priv->cfg = cfg;
-	priv->i2c = i2c;
+	if (pdata->clk)
+		dev->clk = pdata->clk;
+	else
+		dev->clk = 16384000; /* internal clock */
+	dev->client = client;
+	dev->regmap = devm_regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(dev->regmap)) {
+		ret = PTR_ERR(dev->regmap);
+		goto err_kfree;
+	}
 
 	/* check if the tuner is there */
-	ret = fc2580_rd_reg(priv, 0x01, &chip_id);
-	if (ret < 0)
-		goto err;
+	ret = regmap_read(dev->regmap, 0x01, &uitmp);
+	if (ret)
+		goto err_kfree;
 
-	dev_dbg(&priv->i2c->dev, "%s: chip_id=%02x\n", __func__, chip_id);
+	dev_dbg(&client->dev, "chip_id=%02x\n", uitmp);
 
-	switch (chip_id) {
+	switch (uitmp) {
 	case 0x56:
 	case 0x5a:
 		break;
 	default:
-		goto err;
+		ret = -ENODEV;
+		goto err_kfree;
 	}
 
-	dev_info(&priv->i2c->dev,
-			"%s: FCI FC2580 successfully identified\n",
-			KBUILD_MODNAME);
-
-	fe->tuner_priv = priv;
-	memcpy(&fe->ops.tuner_ops, &fc2580_tuner_ops,
-			sizeof(struct dvb_tuner_ops));
+#if IS_ENABLED(CONFIG_VIDEO_V4L2)
+	/* Register controls */
+	v4l2_ctrl_handler_init(&dev->hdl, 2);
+	dev->bandwidth_auto = v4l2_ctrl_new_std(&dev->hdl, &fc2580_ctrl_ops,
+						V4L2_CID_RF_TUNER_BANDWIDTH_AUTO,
+						0, 1, 1, 1);
+	dev->bandwidth = v4l2_ctrl_new_std(&dev->hdl, &fc2580_ctrl_ops,
+					   V4L2_CID_RF_TUNER_BANDWIDTH,
+					   3000, 10000000, 1, 3000);
+	v4l2_ctrl_auto_cluster(2, &dev->bandwidth_auto, 0, false);
+	if (dev->hdl.error) {
+		ret = dev->hdl.error;
+		dev_err(&client->dev, "Could not initialize controls\n");
+		v4l2_ctrl_handler_free(&dev->hdl);
+		goto err_kfree;
+	}
+	dev->subdev.ctrl_handler = &dev->hdl;
+	dev->f_frequency = bands[0].rangelow;
+	dev->f_bandwidth = dev->bandwidth->val;
+	v4l2_i2c_subdev_init(&dev->subdev, client, &fc2580_subdev_ops);
+#endif
+	fe->tuner_priv = dev;
+	memcpy(&fe->ops.tuner_ops, &fc2580_dvb_tuner_ops,
+	       sizeof(fe->ops.tuner_ops));
+	pdata->get_v4l2_subdev = fc2580_get_v4l2_subdev;
+	i2c_set_clientdata(client, dev);
+
+	dev_info(&client->dev, "FCI FC2580 successfully identified\n");
+	return 0;
+err_kfree:
+	kfree(dev);
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
+}
 
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
+static int fc2580_remove(struct i2c_client *client)
+{
+	struct fc2580_dev *dev = i2c_get_clientdata(client);
 
-	return fe;
-err:
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0);
+	dev_dbg(&client->dev, "\n");
 
-	dev_dbg(&i2c->dev, "%s: failed=%d\n", __func__, ret);
-	kfree(priv);
-	return NULL;
+#if IS_ENABLED(CONFIG_VIDEO_V4L2)
+	v4l2_ctrl_handler_free(&dev->hdl);
+#endif
+	kfree(dev);
+	return 0;
 }
-EXPORT_SYMBOL(fc2580_attach);
+
+static const struct i2c_device_id fc2580_id_table[] = {
+	{"fc2580", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, fc2580_id_table);
+
+static struct i2c_driver fc2580_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "fc2580",
+		.suppress_bind_attrs = true,
+	},
+	.probe		= fc2580_probe,
+	.remove		= fc2580_remove,
+	.id_table	= fc2580_id_table,
+};
+
+module_i2c_driver(fc2580_driver);
 
 MODULE_DESCRIPTION("FCI FC2580 silicon tuner driver");
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
diff --git a/drivers/media/tuners/fc2580.h b/drivers/media/tuners/fc2580.h
index b1ce6770f88e..862ea46995d7 100644
--- a/drivers/media/tuners/fc2580.h
+++ b/drivers/media/tuners/fc2580.h
@@ -21,32 +21,26 @@
 #ifndef FC2580_H
 #define FC2580_H
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
+#include <media/v4l2-subdev.h>
+#include <linux/i2c.h>
 
-struct fc2580_config {
-	/*
-	 * I2C address
-	 * 0x56, ...
-	 */
-	u8 i2c_addr;
+/*
+ * I2C address
+ * 0x56, ...
+ */
 
-	/*
-	 * clock
-	 */
-	u32 clock;
-};
+/**
+ * struct fc2580_platform_data - Platform data for the fc2580 driver
+ * @clk: Clock frequency (0 = internal clock).
+ * @dvb_frontend: DVB frontend.
+ * @get_v4l2_subdev: Get V4L2 subdev.
+ */
+struct fc2580_platform_data {
+	u32 clk;
+	struct dvb_frontend *dvb_frontend;
 
-#if IS_REACHABLE(CONFIG_MEDIA_TUNER_FC2580)
-extern struct dvb_frontend *fc2580_attach(struct dvb_frontend *fe,
-	struct i2c_adapter *i2c, const struct fc2580_config *cfg);
-#else
-static inline struct dvb_frontend *fc2580_attach(struct dvb_frontend *fe,
-	struct i2c_adapter *i2c, const struct fc2580_config *cfg)
-{
-	pr_warn("%s: driver disabled by Kconfig\n", __func__);
-	return NULL;
-}
-#endif
+	struct v4l2_subdev* (*get_v4l2_subdev)(struct i2c_client *);
+};
 
 #endif
diff --git a/drivers/media/tuners/fc2580_priv.h b/drivers/media/tuners/fc2580_priv.h
index 646c99452136..031a43d7e7af 100644
--- a/drivers/media/tuners/fc2580_priv.h
+++ b/drivers/media/tuners/fc2580_priv.h
@@ -22,6 +22,9 @@
 #define FC2580_PRIV_H
 
 #include "fc2580.h"
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+#include <linux/regmap.h>
 #include <linux/math64.h>
 
 struct fc2580_reg_val {
@@ -50,7 +53,7 @@ static const struct fc2580_reg_val fc2580_init_reg_vals[] = {
 
 struct fc2580_pll {
 	u32 freq;
-	u8 div;
+	u8 div_out;
 	u8 band;
 };
 
@@ -63,16 +66,15 @@ static const struct fc2580_pll fc2580_pll_lut[] = {
 
 struct fc2580_if_filter {
 	u32 freq;
-	u16 mul;
 	u8 r36_val;
 	u8 r39_val;
 };
 
 static const struct fc2580_if_filter fc2580_if_filter_lut[] = {
-	{   6000000, 4400, 0x18, 0x00},
-	{   7000000, 3910, 0x18, 0x80},
-	{   8000000, 3300, 0x18, 0x80},
-	{0xffffffff, 3300, 0x18, 0x80},
+	{   6000000, 0x18, 0x00},
+	{   7000000, 0x18, 0x80},
+	{   8000000, 0x18, 0x80},
+	{0xffffffff, 0x18, 0x80},
 };
 
 struct fc2580_freq_regs {
@@ -110,15 +112,15 @@ static const struct fc2580_freq_regs fc2580_freq_regs_lut[] = {
 		0x50, 0x0f, 0x07, 0x00, 0x15, 0x03, 0x05, 0x10, 0x12, 0x08,
 		0x0a, 0x78, 0x32, 0x54},
 	{ 538000000,
-		0xf0, 0x77, 0x53, 0x60, 0xff, 0xff, 0xff, 0x09, 0xff, 0x8c,
+		0xf0, 0x77, 0x53, 0x60, 0xff, 0xff, 0x9f, 0x09, 0xff, 0x8c,
 		0x50, 0x13, 0x07, 0x06, 0x15, 0x06, 0x08, 0x10, 0x12, 0x0b,
 		0x0c, 0x78, 0x32, 0x14},
 	{ 794000000,
-		0xf0, 0x77, 0x53, 0x60, 0xff, 0xff, 0xff, 0x09, 0xff, 0x8c,
+		0xf0, 0x77, 0x53, 0x60, 0xff, 0xff, 0x9f, 0x09, 0xff, 0x8c,
 		0x50, 0x15, 0x03, 0x03, 0x15, 0x03, 0x05, 0x0c, 0x0e, 0x0b,
 		0x0c, 0x78, 0x32, 0x14},
 	{1000000000,
-		0xf0, 0x77, 0x53, 0x60, 0xff, 0xff, 0xff, 0x09, 0xff, 0x8c,
+		0xf0, 0x77, 0x53, 0x60, 0xff, 0xff, 0x8f, 0x09, 0xff, 0x8c,
 		0x50, 0x15, 0x07, 0x06, 0x15, 0x07, 0x09, 0x10, 0x12, 0x0b,
 		0x0c, 0x78, 0x32, 0x14},
 	{0xffffffff,
@@ -127,9 +129,19 @@ static const struct fc2580_freq_regs fc2580_freq_regs_lut[] = {
 		0x0a, 0xa0, 0x50, 0x14},
 };
 
-struct fc2580_priv {
-	const struct fc2580_config *cfg;
-	struct i2c_adapter *i2c;
+struct fc2580_dev {
+	u32 clk;
+	struct i2c_client *client;
+	struct regmap *regmap;
+	struct v4l2_subdev subdev;
+	bool active;
+	unsigned int f_frequency;
+	unsigned int f_bandwidth;
+
+	/* Controls */
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *bandwidth_auto;
+	struct v4l2_ctrl *bandwidth;
 };
 
 #endif
diff --git a/drivers/media/tuners/msi001.c b/drivers/media/tuners/msi001.c
index 74cfc3c98edb..b533240f8ec0 100644
--- a/drivers/media/tuners/msi001.c
+++ b/drivers/media/tuners/msi001.c
@@ -36,7 +36,7 @@ static const struct v4l2_frequency_band bands[] = {
 	},
 };
 
-struct msi001 {
+struct msi001_dev {
 	struct spi_device *spi;
 	struct v4l2_subdev sd;
 
@@ -51,25 +51,26 @@ struct msi001 {
 	unsigned int f_tuner;
 };
 
-static inline struct msi001 *sd_to_msi001(struct v4l2_subdev *sd)
+static inline struct msi001_dev *sd_to_msi001_dev(struct v4l2_subdev *sd)
 {
-	return container_of(sd, struct msi001, sd);
+	return container_of(sd, struct msi001_dev, sd);
 }
 
-static int msi001_wreg(struct msi001 *s, u32 data)
+static int msi001_wreg(struct msi001_dev *dev, u32 data)
 {
 	/* Register format: 4 bits addr + 20 bits value */
-	return spi_write(s->spi, &data, 3);
+	return spi_write(dev->spi, &data, 3);
 };
 
-static int msi001_set_gain(struct msi001 *s, int lna_gain, int mixer_gain,
-		int if_gain)
+static int msi001_set_gain(struct msi001_dev *dev, int lna_gain, int mixer_gain,
+			   int if_gain)
 {
+	struct spi_device *spi = dev->spi;
 	int ret;
 	u32 reg;
 
-	dev_dbg(&s->spi->dev, "lna=%d mixer=%d if=%d\n",
-			lna_gain, mixer_gain, if_gain);
+	dev_dbg(&spi->dev, "lna=%d mixer=%d if=%d\n",
+		lna_gain, mixer_gain, if_gain);
 
 	reg = 1 << 0;
 	reg |= (59 - if_gain) << 4;
@@ -78,28 +79,29 @@ static int msi001_set_gain(struct msi001 *s, int lna_gain, int mixer_gain,
 	reg |= (1 - lna_gain) << 13;
 	reg |= 4 << 14;
 	reg |= 0 << 17;
-	ret = msi001_wreg(s, reg);
+	ret = msi001_wreg(dev, reg);
 	if (ret)
 		goto err;
 
 	return 0;
 err:
-	dev_dbg(&s->spi->dev, "failed %d\n", ret);
+	dev_dbg(&spi->dev, "failed %d\n", ret);
 	return ret;
 };
 
-static int msi001_set_tuner(struct msi001 *s)
+static int msi001_set_tuner(struct msi001_dev *dev)
 {
+	struct spi_device *spi = dev->spi;
 	int ret, i;
-	unsigned int n, m, thresh, frac, vco_step, tmp, f_if1;
+	unsigned int uitmp, div_n, k, k_thresh, k_frac, div_lo, f_if1;
 	u32 reg;
-	u64 f_vco, tmp64;
-	u8 mode, filter_mode, lo_div;
+	u64 f_vco;
+	u8 mode, filter_mode;
 
 	static const struct {
 		u32 rf;
 		u8 mode;
-		u8 lo_div;
+		u8 div_lo;
 	} band_lut[] = {
 		{ 50000000, 0xe1, 16}, /* AM_MODE2, antenna 2 */
 		{108000000, 0x42, 32}, /* VHF_MODE */
@@ -130,7 +132,7 @@ static int msi001_set_tuner(struct msi001 *s)
 		{8000000, 0x07}, /* 8 MHz */
 	};
 
-	unsigned int f_rf = s->f_tuner;
+	unsigned int f_rf = dev->f_tuner;
 
 	/*
 	 * bandwidth (Hz)
@@ -144,19 +146,18 @@ static int msi001_set_tuner(struct msi001 *s)
 	 */
 	unsigned int f_if = 0;
 	#define F_REF 24000000
-	#define R_REF 4
-	#define F_OUT_STEP 1
+	#define DIV_PRE_N 4
+	#define	F_VCO_STEP div_lo
 
-	dev_dbg(&s->spi->dev, "f_rf=%d f_if=%d\n", f_rf, f_if);
+	dev_dbg(&spi->dev, "f_rf=%d f_if=%d\n", f_rf, f_if);
 
 	for (i = 0; i < ARRAY_SIZE(band_lut); i++) {
 		if (f_rf <= band_lut[i].rf) {
 			mode = band_lut[i].mode;
-			lo_div = band_lut[i].lo_div;
+			div_lo = band_lut[i].div_lo;
 			break;
 		}
 	}
-
 	if (i == ARRAY_SIZE(band_lut)) {
 		ret = -EINVAL;
 		goto err;
@@ -174,14 +175,13 @@ static int msi001_set_tuner(struct msi001 *s)
 			break;
 		}
 	}
-
 	if (i == ARRAY_SIZE(if_freq_lut)) {
 		ret = -EINVAL;
 		goto err;
 	}
 
 	/* filters */
-	bandwidth = s->bandwidth->val;
+	bandwidth = dev->bandwidth->val;
 	bandwidth = clamp(bandwidth, 200000U, 8000000U);
 
 	for (i = 0; i < ARRAY_SIZE(bandwidth_lut); i++) {
@@ -190,48 +190,61 @@ static int msi001_set_tuner(struct msi001 *s)
 			break;
 		}
 	}
-
 	if (i == ARRAY_SIZE(bandwidth_lut)) {
 		ret = -EINVAL;
 		goto err;
 	}
 
-	s->bandwidth->val = bandwidth_lut[i].freq;
+	dev->bandwidth->val = bandwidth_lut[i].freq;
 
-	dev_dbg(&s->spi->dev, "bandwidth selected=%d\n", bandwidth_lut[i].freq);
+	dev_dbg(&spi->dev, "bandwidth selected=%d\n", bandwidth_lut[i].freq);
 
-	f_vco = (u64) (f_rf + f_if + f_if1) * lo_div;
-	tmp64 = f_vco;
-	m = do_div(tmp64, F_REF * R_REF);
-	n = (unsigned int) tmp64;
+	/*
+	 * Fractional-N synthesizer
+	 *
+	 *           +---------------------------------------+
+	 *           v                                       |
+	 *  Fref   +----+     +-------+         +----+     +------+     +---+
+	 * ------> | PD | --> |  VCO  | ------> | /4 | --> | /N.F | <-- | K |
+	 *         +----+     +-------+         +----+     +------+     +---+
+	 *                      |
+	 *                      |
+	 *                      v
+	 *                    +-------+  Fout
+	 *                    | /Rout | ------>
+	 *                    +-------+
+	 */
 
-	vco_step = F_OUT_STEP * lo_div;
-	thresh = (F_REF * R_REF) / vco_step;
-	frac = 1ul * thresh * m / (F_REF * R_REF);
+	/* Calculate PLL integer and fractional control word. */
+	f_vco = (u64) (f_rf + f_if + f_if1) * div_lo;
+	div_n = div_u64_rem(f_vco, DIV_PRE_N * F_REF, &k);
+	k_thresh = (DIV_PRE_N * F_REF) / F_VCO_STEP;
+	k_frac = div_u64((u64) k * k_thresh, (DIV_PRE_N * F_REF));
 
 	/* Find out greatest common divisor and divide to smaller. */
-	tmp = gcd(thresh, frac);
-	thresh /= tmp;
-	frac /= tmp;
+	uitmp = gcd(k_thresh, k_frac);
+	k_thresh /= uitmp;
+	k_frac /= uitmp;
 
 	/* Force divide to reg max. Resolution will be reduced. */
-	tmp = DIV_ROUND_UP(thresh, 4095);
-	thresh = DIV_ROUND_CLOSEST(thresh, tmp);
-	frac = DIV_ROUND_CLOSEST(frac, tmp);
+	uitmp = DIV_ROUND_UP(k_thresh, 4095);
+	k_thresh = DIV_ROUND_CLOSEST(k_thresh, uitmp);
+	k_frac = DIV_ROUND_CLOSEST(k_frac, uitmp);
 
-	/* calc real RF set */
-	tmp = 1ul * F_REF * R_REF * n;
-	tmp += 1ul * F_REF * R_REF * frac / thresh;
-	tmp /= lo_div;
+	/* Calculate real RF set. */
+	uitmp = (unsigned int) F_REF * DIV_PRE_N * div_n;
+	uitmp += (unsigned int) F_REF * DIV_PRE_N * k_frac / k_thresh;
+	uitmp /= div_lo;
 
-	dev_dbg(&s->spi->dev, "rf=%u:%u n=%d thresh=%d frac=%d\n",
-				f_rf, tmp, n, thresh, frac);
+	dev_dbg(&spi->dev,
+		"f_rf=%u:%u f_vco=%llu div_n=%u k_thresh=%u k_frac=%u div_lo=%u\n",
+		f_rf, uitmp, f_vco, div_n, k_thresh, k_frac, div_lo);
 
-	ret = msi001_wreg(s, 0x00000e);
+	ret = msi001_wreg(dev, 0x00000e);
 	if (ret)
 		goto err;
 
-	ret = msi001_wreg(s, 0x000003);
+	ret = msi001_wreg(dev, 0x000003);
 	if (ret)
 		goto err;
 
@@ -241,54 +254,55 @@ static int msi001_set_tuner(struct msi001 *s)
 	reg |= bandwidth << 14;
 	reg |= 0x02 << 17;
 	reg |= 0x00 << 20;
-	ret = msi001_wreg(s, reg);
+	ret = msi001_wreg(dev, reg);
 	if (ret)
 		goto err;
 
 	reg = 5 << 0;
-	reg |= thresh << 4;
+	reg |= k_thresh << 4;
 	reg |= 1 << 19;
 	reg |= 1 << 21;
-	ret = msi001_wreg(s, reg);
+	ret = msi001_wreg(dev, reg);
 	if (ret)
 		goto err;
 
 	reg = 2 << 0;
-	reg |= frac << 4;
-	reg |= n << 16;
-	ret = msi001_wreg(s, reg);
+	reg |= k_frac << 4;
+	reg |= div_n << 16;
+	ret = msi001_wreg(dev, reg);
 	if (ret)
 		goto err;
 
-	ret = msi001_set_gain(s, s->lna_gain->cur.val, s->mixer_gain->cur.val,
-			s->if_gain->cur.val);
+	ret = msi001_set_gain(dev, dev->lna_gain->cur.val,
+			      dev->mixer_gain->cur.val, dev->if_gain->cur.val);
 	if (ret)
 		goto err;
 
 	reg = 6 << 0;
 	reg |= 63 << 4;
 	reg |= 4095 << 10;
-	ret = msi001_wreg(s, reg);
+	ret = msi001_wreg(dev, reg);
 	if (ret)
 		goto err;
 
 	return 0;
 err:
-	dev_dbg(&s->spi->dev, "failed %d\n", ret);
+	dev_dbg(&spi->dev, "failed %d\n", ret);
 	return ret;
-};
+}
 
 static int msi001_s_power(struct v4l2_subdev *sd, int on)
 {
-	struct msi001 *s = sd_to_msi001(sd);
+	struct msi001_dev *dev = sd_to_msi001_dev(sd);
+	struct spi_device *spi = dev->spi;
 	int ret;
 
-	dev_dbg(&s->spi->dev, "on=%d\n", on);
+	dev_dbg(&spi->dev, "on=%d\n", on);
 
 	if (on)
 		ret = 0;
 	else
-		ret = msi001_wreg(s, 0x000000);
+		ret = msi001_wreg(dev, 0x000000);
 
 	return ret;
 }
@@ -299,9 +313,10 @@ static const struct v4l2_subdev_core_ops msi001_core_ops = {
 
 static int msi001_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
 {
-	struct msi001 *s = sd_to_msi001(sd);
+	struct msi001_dev *dev = sd_to_msi001_dev(sd);
+	struct spi_device *spi = dev->spi;
 
-	dev_dbg(&s->spi->dev, "index=%d\n", v->index);
+	dev_dbg(&spi->dev, "index=%d\n", v->index);
 
 	strlcpy(v->name, "Mirics MSi001", sizeof(v->name));
 	v->type = V4L2_TUNER_RF;
@@ -314,47 +329,51 @@ static int msi001_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
 
 static int msi001_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *v)
 {
-	struct msi001 *s = sd_to_msi001(sd);
+	struct msi001_dev *dev = sd_to_msi001_dev(sd);
+	struct spi_device *spi = dev->spi;
 
-	dev_dbg(&s->spi->dev, "index=%d\n", v->index);
+	dev_dbg(&spi->dev, "index=%d\n", v->index);
 	return 0;
 }
 
 static int msi001_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
 {
-	struct msi001 *s = sd_to_msi001(sd);
+	struct msi001_dev *dev = sd_to_msi001_dev(sd);
+	struct spi_device *spi = dev->spi;
 
-	dev_dbg(&s->spi->dev, "tuner=%d\n", f->tuner);
-	f->frequency = s->f_tuner;
+	dev_dbg(&spi->dev, "tuner=%d\n", f->tuner);
+	f->frequency = dev->f_tuner;
 	return 0;
 }
 
 static int msi001_s_frequency(struct v4l2_subdev *sd,
-		const struct v4l2_frequency *f)
+			      const struct v4l2_frequency *f)
 {
-	struct msi001 *s = sd_to_msi001(sd);
+	struct msi001_dev *dev = sd_to_msi001_dev(sd);
+	struct spi_device *spi = dev->spi;
 	unsigned int band;
 
-	dev_dbg(&s->spi->dev, "tuner=%d type=%d frequency=%u\n",
-			f->tuner, f->type, f->frequency);
+	dev_dbg(&spi->dev, "tuner=%d type=%d frequency=%u\n",
+		f->tuner, f->type, f->frequency);
 
 	if (f->frequency < ((bands[0].rangehigh + bands[1].rangelow) / 2))
 		band = 0;
 	else
 		band = 1;
-	s->f_tuner = clamp_t(unsigned int, f->frequency,
-			bands[band].rangelow, bands[band].rangehigh);
+	dev->f_tuner = clamp_t(unsigned int, f->frequency,
+			       bands[band].rangelow, bands[band].rangehigh);
 
-	return msi001_set_tuner(s);
+	return msi001_set_tuner(dev);
 }
 
 static int msi001_enum_freq_bands(struct v4l2_subdev *sd,
-		struct v4l2_frequency_band *band)
+				  struct v4l2_frequency_band *band)
 {
-	struct msi001 *s = sd_to_msi001(sd);
+	struct msi001_dev *dev = sd_to_msi001_dev(sd);
+	struct spi_device *spi = dev->spi;
 
-	dev_dbg(&s->spi->dev, "tuner=%d type=%d index=%d\n",
-			band->tuner, band->type, band->index);
+	dev_dbg(&spi->dev, "tuner=%d type=%d index=%d\n",
+		band->tuner, band->type, band->index);
 
 	if (band->index >= ARRAY_SIZE(bands))
 		return -EINVAL;
@@ -381,34 +400,37 @@ static const struct v4l2_subdev_ops msi001_ops = {
 
 static int msi001_s_ctrl(struct v4l2_ctrl *ctrl)
 {
-	struct msi001 *s = container_of(ctrl->handler, struct msi001, hdl);
+	struct msi001_dev *dev = container_of(ctrl->handler, struct msi001_dev, hdl);
+	struct spi_device *spi = dev->spi;
 
 	int ret;
 
-	dev_dbg(&s->spi->dev,
-			"id=%d name=%s val=%d min=%lld max=%lld step=%lld\n",
-			ctrl->id, ctrl->name, ctrl->val,
-			ctrl->minimum, ctrl->maximum, ctrl->step);
+	dev_dbg(&spi->dev, "id=%d name=%s val=%d min=%lld max=%lld step=%lld\n",
+		ctrl->id, ctrl->name, ctrl->val, ctrl->minimum, ctrl->maximum,
+		ctrl->step);
 
 	switch (ctrl->id) {
 	case V4L2_CID_RF_TUNER_BANDWIDTH_AUTO:
 	case V4L2_CID_RF_TUNER_BANDWIDTH:
-		ret = msi001_set_tuner(s);
+		ret = msi001_set_tuner(dev);
 		break;
 	case  V4L2_CID_RF_TUNER_LNA_GAIN:
-		ret = msi001_set_gain(s, s->lna_gain->val,
-				s->mixer_gain->cur.val, s->if_gain->cur.val);
+		ret = msi001_set_gain(dev, dev->lna_gain->val,
+				      dev->mixer_gain->cur.val,
+				      dev->if_gain->cur.val);
 		break;
 	case  V4L2_CID_RF_TUNER_MIXER_GAIN:
-		ret = msi001_set_gain(s, s->lna_gain->cur.val,
-				s->mixer_gain->val, s->if_gain->cur.val);
+		ret = msi001_set_gain(dev, dev->lna_gain->cur.val,
+				      dev->mixer_gain->val,
+				      dev->if_gain->cur.val);
 		break;
 	case  V4L2_CID_RF_TUNER_IF_GAIN:
-		ret = msi001_set_gain(s, s->lna_gain->cur.val,
-				s->mixer_gain->cur.val, s->if_gain->val);
+		ret = msi001_set_gain(dev, dev->lna_gain->cur.val,
+				      dev->mixer_gain->cur.val,
+				      dev->if_gain->val);
 		break;
 	default:
-		dev_dbg(&s->spi->dev, "unknown control %d\n", ctrl->id);
+		dev_dbg(&spi->dev, "unknown control %d\n", ctrl->id);
 		ret = -EINVAL;
 	}
 
@@ -421,56 +443,54 @@ static const struct v4l2_ctrl_ops msi001_ctrl_ops = {
 
 static int msi001_probe(struct spi_device *spi)
 {
-	struct msi001 *s;
+	struct msi001_dev *dev;
 	int ret;
 
 	dev_dbg(&spi->dev, "\n");
 
-	s = kzalloc(sizeof(struct msi001), GFP_KERNEL);
-	if (s == NULL) {
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
 		ret = -ENOMEM;
-		dev_dbg(&spi->dev, "Could not allocate memory for msi001\n");
-		goto err_kfree;
+		goto err;
 	}
 
-	s->spi = spi;
-	s->f_tuner = bands[0].rangelow;
-	v4l2_spi_subdev_init(&s->sd, spi, &msi001_ops);
+	dev->spi = spi;
+	dev->f_tuner = bands[0].rangelow;
+	v4l2_spi_subdev_init(&dev->sd, spi, &msi001_ops);
 
 	/* Register controls */
-	v4l2_ctrl_handler_init(&s->hdl, 5);
-	s->bandwidth_auto = v4l2_ctrl_new_std(&s->hdl, &msi001_ctrl_ops,
+	v4l2_ctrl_handler_init(&dev->hdl, 5);
+	dev->bandwidth_auto = v4l2_ctrl_new_std(&dev->hdl, &msi001_ctrl_ops,
 			V4L2_CID_RF_TUNER_BANDWIDTH_AUTO, 0, 1, 1, 1);
-	s->bandwidth = v4l2_ctrl_new_std(&s->hdl, &msi001_ctrl_ops,
+	dev->bandwidth = v4l2_ctrl_new_std(&dev->hdl, &msi001_ctrl_ops,
 			V4L2_CID_RF_TUNER_BANDWIDTH, 200000, 8000000, 1, 200000);
-	v4l2_ctrl_auto_cluster(2, &s->bandwidth_auto, 0, false);
-	s->lna_gain = v4l2_ctrl_new_std(&s->hdl, &msi001_ctrl_ops,
+	v4l2_ctrl_auto_cluster(2, &dev->bandwidth_auto, 0, false);
+	dev->lna_gain = v4l2_ctrl_new_std(&dev->hdl, &msi001_ctrl_ops,
 			V4L2_CID_RF_TUNER_LNA_GAIN, 0, 1, 1, 1);
-	s->mixer_gain = v4l2_ctrl_new_std(&s->hdl, &msi001_ctrl_ops,
+	dev->mixer_gain = v4l2_ctrl_new_std(&dev->hdl, &msi001_ctrl_ops,
 			V4L2_CID_RF_TUNER_MIXER_GAIN, 0, 1, 1, 1);
-	s->if_gain = v4l2_ctrl_new_std(&s->hdl, &msi001_ctrl_ops,
+	dev->if_gain = v4l2_ctrl_new_std(&dev->hdl, &msi001_ctrl_ops,
 			V4L2_CID_RF_TUNER_IF_GAIN, 0, 59, 1, 0);
-	if (s->hdl.error) {
-		ret = s->hdl.error;
-		dev_err(&s->spi->dev, "Could not initialize controls\n");
+	if (dev->hdl.error) {
+		ret = dev->hdl.error;
+		dev_err(&spi->dev, "Could not initialize controls\n");
 		/* control init failed, free handler */
 		goto err_ctrl_handler_free;
 	}
 
-	s->sd.ctrl_handler = &s->hdl;
+	dev->sd.ctrl_handler = &dev->hdl;
 	return 0;
-
 err_ctrl_handler_free:
-	v4l2_ctrl_handler_free(&s->hdl);
-err_kfree:
-	kfree(s);
+	v4l2_ctrl_handler_free(&dev->hdl);
+	kfree(dev);
+err:
 	return ret;
 }
 
 static int msi001_remove(struct spi_device *spi)
 {
 	struct v4l2_subdev *sd = spi_get_drvdata(spi);
-	struct msi001 *s = sd_to_msi001(sd);
+	struct msi001_dev *dev = sd_to_msi001_dev(sd);
 
 	dev_dbg(&spi->dev, "\n");
 
@@ -478,26 +498,27 @@ static int msi001_remove(struct spi_device *spi)
 	 * Registered by v4l2_spi_new_subdev() from master driver, but we must
 	 * unregister it from here. Weird.
 	 */
-	v4l2_device_unregister_subdev(&s->sd);
-	v4l2_ctrl_handler_free(&s->hdl);
-	kfree(s);
+	v4l2_device_unregister_subdev(&dev->sd);
+	v4l2_ctrl_handler_free(&dev->hdl);
+	kfree(dev);
 	return 0;
 }
 
-static const struct spi_device_id msi001_id[] = {
+static const struct spi_device_id msi001_id_table[] = {
 	{"msi001", 0},
 	{}
 };
-MODULE_DEVICE_TABLE(spi, msi001_id);
+MODULE_DEVICE_TABLE(spi, msi001_id_table);
 
 static struct spi_driver msi001_driver = {
 	.driver = {
 		.name	= "msi001",
 		.owner	= THIS_MODULE,
+		.suppress_bind_attrs = true,
 	},
 	.probe		= msi001_probe,
 	.remove		= msi001_remove,
-	.id_table	= msi001_id,
+	.id_table	= msi001_id_table,
 };
 module_spi_driver(msi001_driver);
 
diff --git a/drivers/media/tuners/qt1010.c b/drivers/media/tuners/qt1010.c
index bc419f8a9671..ae8cbece6d2b 100644
--- a/drivers/media/tuners/qt1010.c
+++ b/drivers/media/tuners/qt1010.c
@@ -294,7 +294,7 @@ static int qt1010_init(struct dvb_frontend *fe)
 	int err = 0;
 	u8 i, tmpval, *valptr = NULL;
 
-	qt1010_i2c_oper_t i2c_data[] = {
+	static const qt1010_i2c_oper_t i2c_data[] = {
 		{ QT1010_WR, 0x01, 0x80 },
 		{ QT1010_WR, 0x0d, 0x84 },
 		{ QT1010_WR, 0x0e, 0xb7 },
@@ -354,13 +354,17 @@ static int qt1010_init(struct dvb_frontend *fe)
 				valptr = &priv->reg1f_init_val;
 			else
 				valptr = &tmpval;
+
+			BUG_ON(i >= ARRAY_SIZE(i2c_data) - 1);
+
 			err = qt1010_init_meas1(priv, i2c_data[i+1].reg,
 						i2c_data[i].reg,
 						i2c_data[i].val, valptr);
 			i++;
 			break;
 		}
-		if (err) return err;
+		if (err)
+			return err;
 	}
 
 	for (i = 0x31; i < 0x3a; i++) /* 0x31 - 0x39 */
diff --git a/drivers/media/tuners/r820t.c b/drivers/media/tuners/r820t.c
index 71159a58860f..a7a8452e99d2 100644
--- a/drivers/media/tuners/r820t.c
+++ b/drivers/media/tuners/r820t.c
@@ -941,8 +941,8 @@ static int r820t_sysfreq_sel(struct r820t_priv *priv, u32 freq,
 		rc = r820t_write_reg_mask(priv, 0x10, 0x00, 0x04);
 		if (rc < 0)
 			return rc;
-	 }
-	 return 0;
+	}
+	return 0;
 }
 
 static int r820t_set_tv_standard(struct r820t_priv *priv,
diff --git a/drivers/media/tuners/si2157.c b/drivers/media/tuners/si2157.c
index d74ae26621ca..a6245ef379c4 100644
--- a/drivers/media/tuners/si2157.c
+++ b/drivers/media/tuners/si2157.c
@@ -79,6 +79,7 @@ static int si2157_init(struct dvb_frontend *fe)
 {
 	struct i2c_client *client = fe->tuner_priv;
 	struct si2157_dev *dev = i2c_get_clientdata(client);
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, len, remaining;
 	struct si2157_cmd cmd;
 	const struct firmware *fw;
@@ -201,9 +202,14 @@ skip_fw_download:
 	dev->fw_loaded = true;
 
 warm:
+	/* init statistics in order signal app which are supported */
+	c->strength.len = 1;
+	c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	/* start statistics polling */
+	schedule_delayed_work(&dev->stat_work, msecs_to_jiffies(1000));
+
 	dev->active = true;
 	return 0;
-
 err_release_firmware:
 	release_firmware(fw);
 err:
@@ -222,6 +228,9 @@ static int si2157_sleep(struct dvb_frontend *fe)
 
 	dev->active = false;
 
+	/* stop statistics polling */
+	cancel_delayed_work_sync(&dev->stat_work);
+
 	/* standby */
 	memcpy(cmd.args, "\x16\x00", 2);
 	cmd.wlen = 2;
@@ -298,7 +307,8 @@ static int si2157_set_params(struct dvb_frontend *fe)
 	if (dev->chiptype == SI2157_CHIPTYPE_SI2146)
 		memcpy(cmd.args, "\x14\x00\x02\x07\x00\x01", 6);
 	else
-		memcpy(cmd.args, "\x14\x00\x02\x07\x01\x00", 6);
+		memcpy(cmd.args, "\x14\x00\x02\x07\x00\x00", 6);
+	cmd.args[4] = dev->if_port;
 	cmd.wlen = 6;
 	cmd.rlen = 4;
 	ret = si2157_cmd_execute(client, &cmd);
@@ -359,6 +369,34 @@ static const struct dvb_tuner_ops si2157_ops = {
 	.get_if_frequency = si2157_get_if_frequency,
 };
 
+static void si2157_stat_work(struct work_struct *work)
+{
+	struct si2157_dev *dev = container_of(work, struct si2157_dev, stat_work.work);
+	struct dvb_frontend *fe = dev->fe;
+	struct i2c_client *client = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct si2157_cmd cmd;
+	int ret;
+
+	dev_dbg(&client->dev, "\n");
+
+	memcpy(cmd.args, "\x42\x00", 2);
+	cmd.wlen = 2;
+	cmd.rlen = 12;
+	ret = si2157_cmd_execute(client, &cmd);
+	if (ret)
+		goto err;
+
+	c->strength.stat[0].scale = FE_SCALE_DECIBEL;
+	c->strength.stat[0].svalue = (s8) cmd.args[3] * 1000;
+
+	schedule_delayed_work(&dev->stat_work, msecs_to_jiffies(2000));
+	return;
+err:
+	c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+}
+
 static int si2157_probe(struct i2c_client *client,
 		const struct i2c_device_id *id)
 {
@@ -378,10 +416,12 @@ static int si2157_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, dev);
 	dev->fe = cfg->fe;
 	dev->inversion = cfg->inversion;
+	dev->if_port = cfg->if_port;
 	dev->fw_loaded = false;
 	dev->chiptype = (u8)id->driver_data;
 	dev->if_frequency = 5000000; /* default value of property 0x0706 */
 	mutex_init(&dev->i2c_mutex);
+	INIT_DELAYED_WORK(&dev->stat_work, si2157_stat_work);
 
 	/* check if the tuner is there */
 	cmd.wlen = 0;
diff --git a/drivers/media/tuners/si2157.h b/drivers/media/tuners/si2157.h
index a564c4a9fba7..4db97ab744d6 100644
--- a/drivers/media/tuners/si2157.h
+++ b/drivers/media/tuners/si2157.h
@@ -34,6 +34,12 @@ struct si2157_config {
 	 * Spectral Inversion
 	 */
 	bool inversion;
+
+	/*
+	 * Port selection
+	 * Select the RF interface to use (pins 9+11 or 12+13)
+	 */
+	u8 if_port;
 };
 
 #endif
diff --git a/drivers/media/tuners/si2157_priv.h b/drivers/media/tuners/si2157_priv.h
index cd8fa5b25304..ecc463db8f69 100644
--- a/drivers/media/tuners/si2157_priv.h
+++ b/drivers/media/tuners/si2157_priv.h
@@ -28,7 +28,9 @@ struct si2157_dev {
 	bool fw_loaded;
 	bool inversion;
 	u8 chiptype;
+	u8 if_port;
 	u32 if_frequency;
+	struct delayed_work stat_work;
 };
 
 #define SI2157_CHIPTYPE_SI2157 0
diff --git a/drivers/media/tuners/tua9001.c b/drivers/media/tuners/tua9001.c
index 83a6240f64d3..d4f6ca0c4d92 100644
--- a/drivers/media/tuners/tua9001.c
+++ b/drivers/media/tuners/tua9001.c
@@ -1,5 +1,5 @@
 /*
- * Infineon TUA 9001 silicon tuner driver
+ * Infineon TUA9001 silicon tuner driver
  *
  * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
  *
@@ -12,138 +12,87 @@
  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *    GNU General Public License for more details.
- *
- *    You should have received a copy of the GNU General Public License along
- *    with this program; if not, write to the Free Software Foundation, Inc.,
- *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
 
-#include "tua9001.h"
 #include "tua9001_priv.h"
 
-/* write register */
-static int tua9001_wr_reg(struct tua9001_priv *priv, u8 reg, u16 val)
-{
-	int ret;
-	u8 buf[3] = { reg, (val >> 8) & 0xff, (val >> 0) & 0xff };
-	struct i2c_msg msg[1] = {
-		{
-			.addr = priv->cfg->i2c_addr,
-			.flags = 0,
-			.len = sizeof(buf),
-			.buf = buf,
-		}
-	};
-
-	ret = i2c_transfer(priv->i2c, msg, 1);
-	if (ret == 1) {
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev, "%s: i2c wr failed=%d reg=%02x\n",
-				KBUILD_MODNAME, ret, reg);
-		ret = -EREMOTEIO;
-	}
-
-	return ret;
-}
-
-static int tua9001_release(struct dvb_frontend *fe)
-{
-	struct tua9001_priv *priv = fe->tuner_priv;
-	int ret = 0;
-
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
-
-	if (fe->callback)
-		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
-				TUA9001_CMD_CEN, 0);
-
-	kfree(fe->tuner_priv);
-	fe->tuner_priv = NULL;
-
-	return ret;
-}
-
 static int tua9001_init(struct dvb_frontend *fe)
 {
-	struct tua9001_priv *priv = fe->tuner_priv;
-	int ret = 0;
-	u8 i;
-	struct reg_val data[] = {
-		{ 0x1e, 0x6512 },
-		{ 0x25, 0xb888 },
-		{ 0x39, 0x5460 },
-		{ 0x3b, 0x00c0 },
-		{ 0x3a, 0xf000 },
-		{ 0x08, 0x0000 },
-		{ 0x32, 0x0030 },
-		{ 0x41, 0x703a },
-		{ 0x40, 0x1c78 },
-		{ 0x2c, 0x1c00 },
-		{ 0x36, 0xc013 },
-		{ 0x37, 0x6f18 },
-		{ 0x27, 0x0008 },
-		{ 0x2a, 0x0001 },
-		{ 0x34, 0x0a40 },
+	struct tua9001_dev *dev = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
+	int ret, i;
+	static const struct tua9001_reg_val data[] = {
+		{0x1e, 0x6512},
+		{0x25, 0xb888},
+		{0x39, 0x5460},
+		{0x3b, 0x00c0},
+		{0x3a, 0xf000},
+		{0x08, 0x0000},
+		{0x32, 0x0030},
+		{0x41, 0x703a},
+		{0x40, 0x1c78},
+		{0x2c, 0x1c00},
+		{0x36, 0xc013},
+		{0x37, 0x6f18},
+		{0x27, 0x0008},
+		{0x2a, 0x0001},
+		{0x34, 0x0a40},
 	};
 
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	if (fe->callback) {
-		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
-				TUA9001_CMD_RESETN, 0);
-		if (ret < 0)
+		ret = fe->callback(client->adapter,
+				   DVB_FRONTEND_COMPONENT_TUNER,
+				   TUA9001_CMD_RESETN, 0);
+		if (ret)
 			goto err;
 	}
 
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1); /* open i2c-gate */
-
 	for (i = 0; i < ARRAY_SIZE(data); i++) {
-		ret = tua9001_wr_reg(priv, data[i].reg, data[i].val);
-		if (ret < 0)
-			goto err_i2c_gate_ctrl;
+		ret = regmap_write(dev->regmap, data[i].reg, data[i].val);
+		if (ret)
+			goto err;
 	}
-
-err_i2c_gate_ctrl:
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0); /* close i2c-gate */
+	return 0;
 err:
-	if (ret < 0)
-		dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int tua9001_sleep(struct dvb_frontend *fe)
 {
-	struct tua9001_priv *priv = fe->tuner_priv;
-	int ret = 0;
-
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
-
-	if (fe->callback)
-		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
-				TUA9001_CMD_RESETN, 1);
+	struct tua9001_dev *dev = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
+	int ret;
 
-	if (ret < 0)
-		dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "\n");
 
+	if (fe->callback) {
+		ret = fe->callback(client->adapter,
+				   DVB_FRONTEND_COMPONENT_TUNER,
+				   TUA9001_CMD_RESETN, 1);
+		if (ret)
+			goto err;
+	}
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int tua9001_set_params(struct dvb_frontend *fe)
 {
-	struct tua9001_priv *priv = fe->tuner_priv;
+	struct tua9001_dev *dev = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
-	int ret = 0, i;
+	int ret, i;
 	u16 val;
-	u32 frequency;
-	struct reg_val data[2];
+	struct tua9001_reg_val data[2];
 
-	dev_dbg(&priv->i2c->dev, "%s: delivery_system=%d frequency=%d " \
-			"bandwidth_hz=%d\n", __func__,
-			c->delivery_system, c->frequency, c->bandwidth_hz);
+	dev_dbg(&client->dev,
+		"delivery_system=%u frequency=%u bandwidth_hz=%u\n",
+		c->delivery_system, c->frequency, c->bandwidth_hz);
 
 	switch (c->delivery_system) {
 	case SYS_DVBT:
@@ -172,70 +121,54 @@ static int tua9001_set_params(struct dvb_frontend *fe)
 
 	data[0].reg = 0x04;
 	data[0].val = val;
-
-	frequency = (c->frequency - 150000000);
-	frequency /= 100;
-	frequency *= 48;
-	frequency /= 10000;
-
 	data[1].reg = 0x1f;
-	data[1].val = frequency;
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1); /* open i2c-gate */
+	data[1].val = div_u64((u64) (c->frequency - 150000000) * 48, 1000000);
 
 	if (fe->callback) {
-		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
-				TUA9001_CMD_RXEN, 0);
-		if (ret < 0)
-			goto err_i2c_gate_ctrl;
+		ret = fe->callback(client->adapter,
+				   DVB_FRONTEND_COMPONENT_TUNER,
+				   TUA9001_CMD_RXEN, 0);
+		if (ret)
+			goto err;
 	}
 
 	for (i = 0; i < ARRAY_SIZE(data); i++) {
-		ret = tua9001_wr_reg(priv, data[i].reg, data[i].val);
-		if (ret < 0)
-			goto err_i2c_gate_ctrl;
+		ret = regmap_write(dev->regmap, data[i].reg, data[i].val);
+		if (ret)
+			goto err;
 	}
 
 	if (fe->callback) {
-		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
-				TUA9001_CMD_RXEN, 1);
-		if (ret < 0)
-			goto err_i2c_gate_ctrl;
+		ret = fe->callback(client->adapter,
+				   DVB_FRONTEND_COMPONENT_TUNER,
+				   TUA9001_CMD_RXEN, 1);
+		if (ret)
+			goto err;
 	}
-
-err_i2c_gate_ctrl:
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0); /* close i2c-gate */
+	return 0;
 err:
-	if (ret < 0)
-		dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
-
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int tua9001_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
-	struct tua9001_priv *priv = fe->tuner_priv;
+	struct tua9001_dev *dev = fe->tuner_priv;
+	struct i2c_client *client = dev->client;
 
-	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	*frequency = 0; /* Zero-IF */
-
 	return 0;
 }
 
 static const struct dvb_tuner_ops tua9001_tuner_ops = {
 	.info = {
-		.name           = "Infineon TUA 9001",
-
+		.name           = "Infineon TUA9001",
 		.frequency_min  = 170000000,
 		.frequency_max  = 862000000,
-		.frequency_step = 0,
 	},
 
-	.release = tua9001_release,
-
 	.init = tua9001_init,
 	.sleep = tua9001_sleep,
 	.set_params = tua9001_set_params,
@@ -243,52 +176,108 @@ static const struct dvb_tuner_ops tua9001_tuner_ops = {
 	.get_if_frequency = tua9001_get_if_frequency,
 };
 
-struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
-		struct i2c_adapter *i2c, struct tua9001_config *cfg)
+static int tua9001_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
 {
-	struct tua9001_priv *priv = NULL;
+	struct tua9001_dev *dev;
+	struct tua9001_platform_data *pdata = client->dev.platform_data;
+	struct dvb_frontend *fe = pdata->dvb_frontend;
 	int ret;
+	static const struct regmap_config regmap_config = {
+		.reg_bits =  8,
+		.val_bits = 16,
+	};
 
-	priv = kzalloc(sizeof(struct tua9001_priv), GFP_KERNEL);
-	if (priv == NULL)
-		return NULL;
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		ret = -ENOMEM;
+		goto err;
+	}
 
-	priv->cfg = cfg;
-	priv->i2c = i2c;
+	dev->fe = pdata->dvb_frontend;
+	dev->client = client;
+	dev->regmap = devm_regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(dev->regmap)) {
+		ret = PTR_ERR(dev->regmap);
+		goto err_kfree;
+	}
 
 	if (fe->callback) {
-		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
-				TUA9001_CMD_CEN, 1);
-		if (ret < 0)
-			goto err;
-
-		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
-				TUA9001_CMD_RXEN, 0);
-		if (ret < 0)
-			goto err;
-
-		ret = fe->callback(priv->i2c, DVB_FRONTEND_COMPONENT_TUNER,
-				TUA9001_CMD_RESETN, 1);
-		if (ret < 0)
-			goto err;
+		ret = fe->callback(client->adapter,
+				   DVB_FRONTEND_COMPONENT_TUNER,
+				   TUA9001_CMD_CEN, 1);
+		if (ret)
+			goto err_kfree;
+
+		ret = fe->callback(client->adapter,
+				   DVB_FRONTEND_COMPONENT_TUNER,
+				   TUA9001_CMD_RXEN, 0);
+		if (ret)
+			goto err_kfree;
+
+		ret = fe->callback(client->adapter,
+				   DVB_FRONTEND_COMPONENT_TUNER,
+				   TUA9001_CMD_RESETN, 1);
+		if (ret)
+			goto err_kfree;
 	}
 
-	dev_info(&priv->i2c->dev,
-			"%s: Infineon TUA 9001 successfully attached\n",
-			KBUILD_MODNAME);
-
+	fe->tuner_priv = dev;
 	memcpy(&fe->ops.tuner_ops, &tua9001_tuner_ops,
 			sizeof(struct dvb_tuner_ops));
+	i2c_set_clientdata(client, dev);
 
-	fe->tuner_priv = priv;
-	return fe;
+	dev_info(&client->dev, "Infineon TUA9001 successfully attached\n");
+	return 0;
+err_kfree:
+	kfree(dev);
 err:
-	dev_dbg(&i2c->dev, "%s: failed=%d\n", __func__, ret);
-	kfree(priv);
-	return NULL;
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
+}
+
+static int tua9001_remove(struct i2c_client *client)
+{
+	struct tua9001_dev *dev = i2c_get_clientdata(client);
+	struct dvb_frontend *fe = dev->fe;
+	int ret;
+
+	dev_dbg(&client->dev, "\n");
+
+	if (fe->callback) {
+		ret = fe->callback(client->adapter,
+				   DVB_FRONTEND_COMPONENT_TUNER,
+				   TUA9001_CMD_CEN, 0);
+		if (ret)
+			goto err_kfree;
+	}
+	kfree(dev);
+	return 0;
+err_kfree:
+	kfree(dev);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
 }
-EXPORT_SYMBOL(tua9001_attach);
 
-MODULE_DESCRIPTION("Infineon TUA 9001 silicon tuner driver");
+static const struct i2c_device_id tua9001_id_table[] = {
+	{"tua9001", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, tua9001_id_table);
+
+static struct i2c_driver tua9001_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tua9001",
+		.suppress_bind_attrs = true,
+	},
+	.probe		= tua9001_probe,
+	.remove		= tua9001_remove,
+	.id_table	= tua9001_id_table,
+};
+
+module_i2c_driver(tua9001_driver);
+
+MODULE_DESCRIPTION("Infineon TUA9001 silicon tuner driver");
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/tua9001.h b/drivers/media/tuners/tua9001.h
index 2c3375c7aeb9..7b0548181cdc 100644
--- a/drivers/media/tuners/tua9001.h
+++ b/drivers/media/tuners/tua9001.h
@@ -1,5 +1,5 @@
 /*
- * Infineon TUA 9001 silicon tuner driver
+ * Infineon TUA9001 silicon tuner driver
  *
  * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
  *
@@ -12,23 +12,24 @@
  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *    GNU General Public License for more details.
- *
- *    You should have received a copy of the GNU General Public License along
- *    with this program; if not, write to the Free Software Foundation, Inc.,
- *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
 
 #ifndef TUA9001_H
 #define TUA9001_H
 
-#include <linux/kconfig.h>
 #include "dvb_frontend.h"
 
-struct tua9001_config {
-	/*
-	 * I2C address
-	 */
-	u8 i2c_addr;
+/*
+ * I2C address
+ * 0x60,
+ */
+
+/**
+ * struct tua9001_platform_data - Platform data for the tua9001 driver
+ * @dvb_frontend: DVB frontend.
+ */
+struct tua9001_platform_data {
+	struct dvb_frontend *dvb_frontend;
 };
 
 /*
@@ -51,16 +52,4 @@ struct tua9001_config {
 #define TUA9001_CMD_RESETN  1
 #define TUA9001_CMD_RXEN    2
 
-#if IS_REACHABLE(CONFIG_MEDIA_TUNER_TUA9001)
-extern struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
-		struct i2c_adapter *i2c, struct tua9001_config *cfg);
-#else
-static inline struct dvb_frontend *tua9001_attach(struct dvb_frontend *fe,
-		struct i2c_adapter *i2c, struct tua9001_config *cfg)
-{
-	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
-	return NULL;
-}
-#endif
-
 #endif
diff --git a/drivers/media/tuners/tua9001_priv.h b/drivers/media/tuners/tua9001_priv.h
index 73cc1ce0575c..bc406c5ec69d 100644
--- a/drivers/media/tuners/tua9001_priv.h
+++ b/drivers/media/tuners/tua9001_priv.h
@@ -1,5 +1,5 @@
 /*
- * Infineon TUA 9001 silicon tuner driver
+ * Infineon TUA9001 silicon tuner driver
  *
  * Copyright (C) 2009 Antti Palosaari <crope@iki.fi>
  *
@@ -12,23 +12,24 @@
  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *    GNU General Public License for more details.
- *
- *    You should have received a copy of the GNU General Public License along
- *    with this program; if not, write to the Free Software Foundation, Inc.,
- *    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
 
 #ifndef TUA9001_PRIV_H
 #define TUA9001_PRIV_H
 
-struct reg_val {
+#include "tua9001.h"
+#include <linux/math64.h>
+#include <linux/regmap.h>
+
+struct tua9001_reg_val {
 	u8 reg;
 	u16 val;
 };
 
-struct tua9001_priv {
-	struct tua9001_config *cfg;
-	struct i2c_adapter *i2c;
+struct tua9001_dev {
+	struct dvb_frontend *fe;
+	struct i2c_client *client;
+	struct regmap *regmap;
 };
 
 #endif
diff --git a/drivers/media/tuners/tuner-i2c.h b/drivers/media/tuners/tuner-i2c.h
index 18f005634c67..bda67a5a76f2 100644
--- a/drivers/media/tuners/tuner-i2c.h
+++ b/drivers/media/tuners/tuner-i2c.h
@@ -33,7 +33,8 @@ struct tuner_i2c_props {
 	char *name;
 };
 
-static inline int tuner_i2c_xfer_send(struct tuner_i2c_props *props, char *buf, int len)
+static inline int tuner_i2c_xfer_send(struct tuner_i2c_props *props,
+				      unsigned char *buf, int len)
 {
 	struct i2c_msg msg = { .addr = props->addr, .flags = 0,
 			       .buf = buf, .len = len };
@@ -42,7 +43,8 @@ static inline int tuner_i2c_xfer_send(struct tuner_i2c_props *props, char *buf,
 	return (ret == 1) ? len : ret;
 }
 
-static inline int tuner_i2c_xfer_recv(struct tuner_i2c_props *props, char *buf, int len)
+static inline int tuner_i2c_xfer_recv(struct tuner_i2c_props *props,
+				      unsigned char *buf, int len)
 {
 	struct i2c_msg msg = { .addr = props->addr, .flags = I2C_M_RD,
 			       .buf = buf, .len = len };
@@ -52,8 +54,8 @@ static inline int tuner_i2c_xfer_recv(struct tuner_i2c_props *props, char *buf,
 }
 
 static inline int tuner_i2c_xfer_send_recv(struct tuner_i2c_props *props,
-					   char *obuf, int olen,
-					   char *ibuf, int ilen)
+					   unsigned char *obuf, int olen,
+					   unsigned char *ibuf, int ilen)
 {
 	struct i2c_msg msg[2] = { { .addr = props->addr, .flags = 0,
 				    .buf = obuf, .len = olen },
diff --git a/drivers/media/tuners/tuner-xc2028.c b/drivers/media/tuners/tuner-xc2028.c
index d12f5e4ad8bf..4e941f00b600 100644
--- a/drivers/media/tuners/tuner-xc2028.c
+++ b/drivers/media/tuners/tuner-xc2028.c
@@ -1094,7 +1094,7 @@ static int generic_set_freq(struct dvb_frontend *fe, u32 freq /* in HZ */,
 		 * Still need tests for XC3028L (firmware 3.2 or upper)
 		 * So, for now, let's just comment the per-firmware
 		 * version of this change. Reports with xc3028l working
-		 * with and without the lines bellow are welcome
+		 * with and without the lines below are welcome
 		 */
 
 		if (priv->firm_version < 0x0302) {
diff --git a/drivers/media/usb/as102/as102_drv.c b/drivers/media/usb/as102/as102_drv.c
index 8be1474b2c36..9dd7c7cb06b1 100644
--- a/drivers/media/usb/as102/as102_drv.c
+++ b/drivers/media/usb/as102/as102_drv.c
@@ -337,6 +337,7 @@ int as102_dvb_register(struct as102_dev_t *as102_dev)
 				       &as102_dev->bus_adap,
 				       as102_dev->elna_cfg);
 	if (!as102_dev->dvb_fe) {
+		ret = -ENODEV;
 		dev_err(dev, "%s: as102_attach() failed: %d",
 		    __func__, ret);
 		goto efereg;
diff --git a/drivers/media/usb/au0828/au0828-cards.c b/drivers/media/usb/au0828/au0828-cards.c
index edc27355f271..6b469e8c4c6e 100644
--- a/drivers/media/usb/au0828/au0828-cards.c
+++ b/drivers/media/usb/au0828/au0828-cards.c
@@ -195,8 +195,6 @@ void au0828_card_setup(struct au0828_dev *dev)
 
 	dprintk(1, "%s()\n", __func__);
 
-	dev->board = au0828_boards[dev->boardnr];
-
 	if (dev->i2c_rc == 0) {
 		dev->i2c_client.addr = 0xa0 >> 1;
 		tveeprom_read(&dev->i2c_client, eeprom, sizeof(eeprom));
diff --git a/drivers/media/usb/au0828/au0828-core.c b/drivers/media/usb/au0828/au0828-core.c
index 082ae6ba492f..0934024fb89d 100644
--- a/drivers/media/usb/au0828/au0828-core.c
+++ b/drivers/media/usb/au0828/au0828-core.c
@@ -222,6 +222,8 @@ static int au0828_usb_probe(struct usb_interface *interface,
 	mutex_init(&dev->dvb.lock);
 	dev->usbdev = usbdev;
 	dev->boardnr = id->driver_info;
+	dev->board = au0828_boards[dev->boardnr];
+
 
 #ifdef CONFIG_VIDEO_AU0828_V4L2
 	dev->v4l2_dev.release = au0828_usb_v4l2_release;
diff --git a/drivers/media/usb/cx231xx/cx231xx-417.c b/drivers/media/usb/cx231xx/cx231xx-417.c
index 983ea8339154..47a98a2014a5 100644
--- a/drivers/media/usb/cx231xx/cx231xx-417.c
+++ b/drivers/media/usb/cx231xx/cx231xx-417.c
@@ -1160,9 +1160,9 @@ static int cx231xx_initialize_codec(struct cx231xx *dev)
 	}
 
 	cx231xx_enable656(dev);
-			/* stop mpeg capture */
-			cx231xx_api_cmd(dev, CX2341X_ENC_STOP_CAPTURE,
-				 3, 0, 1, 3, 4);
+
+	/* stop mpeg capture */
+	cx231xx_api_cmd(dev, CX2341X_ENC_STOP_CAPTURE, 3, 0, 1, 3, 4);
 
 	cx231xx_codec_settings(dev);
 	msleep(60);
@@ -1249,8 +1249,7 @@ static void free_buffer(struct videobuf_queue *vq, struct cx231xx_buffer *buf)
 	struct cx231xx *dev = fh->dev;
 	unsigned long flags = 0;
 
-	if (in_interrupt())
-		BUG();
+	BUG_ON(in_interrupt());
 
 	spin_lock_irqsave(&dev->video_mode.slock, flags);
 	if (dev->USE_ISO) {
@@ -1878,13 +1877,15 @@ static int cx231xx_s_video_encoding(struct cx2341x_handler *cxhdl, u32 val)
 {
 	struct cx231xx *dev = container_of(cxhdl, struct cx231xx, mpeg_ctrl_handler);
 	int is_mpeg1 = val == V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-	struct v4l2_mbus_framefmt fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 
 	/* fix videodecoder resolution */
-	fmt.width = cxhdl->width / (is_mpeg1 ? 2 : 1);
-	fmt.height = cxhdl->height;
-	fmt.code = MEDIA_BUS_FMT_FIXED;
-	v4l2_subdev_call(dev->sd_cx25840, video, s_mbus_fmt, &fmt);
+	format.format.width = cxhdl->width / (is_mpeg1 ? 2 : 1);
+	format.format.height = cxhdl->height;
+	format.format.code = MEDIA_BUS_FMT_FIXED;
+	v4l2_subdev_call(dev->sd_cx25840, pad, set_fmt, NULL, &format);
 	return 0;
 }
 
diff --git a/drivers/media/usb/cx231xx/cx231xx-avcore.c b/drivers/media/usb/cx231xx/cx231xx-avcore.c
index 39e887925e3d..491913778bcc 100644
--- a/drivers/media/usb/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/usb/cx231xx/cx231xx-avcore.c
@@ -1595,31 +1595,31 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 		/*pll_freq_word = 0x3463497;*/
 		vid_blk_write_word(dev, DIF_PLL_FREQ_WORD,  pll_freq_word);
 
-	if (spectral_invert) {
-		if_freq -= 400000;
-		/* Enable Spectral Invert*/
-		vid_blk_read_word(dev, DIF_MISC_CTRL,
-					&dif_misc_ctrl_value);
-		dif_misc_ctrl_value = dif_misc_ctrl_value | 0x00200000;
-		vid_blk_write_word(dev, DIF_MISC_CTRL,
-					dif_misc_ctrl_value);
-	} else {
-		if_freq += 400000;
-		/* Disable Spectral Invert*/
-		vid_blk_read_word(dev, DIF_MISC_CTRL,
-					&dif_misc_ctrl_value);
-		dif_misc_ctrl_value = dif_misc_ctrl_value & 0xFFDFFFFF;
-		vid_blk_write_word(dev, DIF_MISC_CTRL,
-					dif_misc_ctrl_value);
-	}
+		if (spectral_invert) {
+			if_freq -= 400000;
+			/* Enable Spectral Invert*/
+			vid_blk_read_word(dev, DIF_MISC_CTRL,
+					  &dif_misc_ctrl_value);
+			dif_misc_ctrl_value = dif_misc_ctrl_value | 0x00200000;
+			vid_blk_write_word(dev, DIF_MISC_CTRL,
+					  dif_misc_ctrl_value);
+		} else {
+			if_freq += 400000;
+			/* Disable Spectral Invert*/
+			vid_blk_read_word(dev, DIF_MISC_CTRL,
+					  &dif_misc_ctrl_value);
+			dif_misc_ctrl_value = dif_misc_ctrl_value & 0xFFDFFFFF;
+			vid_blk_write_word(dev, DIF_MISC_CTRL,
+					  dif_misc_ctrl_value);
+		}
 
-	if_freq = (if_freq/100000)*100000;
+		if_freq = (if_freq / 100000) * 100000;
 
-	if (if_freq < 3000000)
-		if_freq = 3000000;
+		if (if_freq < 3000000)
+			if_freq = 3000000;
 
-	if (if_freq > 16000000)
-		if_freq = 16000000;
+		if (if_freq > 16000000)
+			if_freq = 16000000;
 	}
 
 	dev_dbg(dev->dev, "Enter IF=%zu\n", ARRAY_SIZE(Dif_set_array));
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index fe00da105e77..4a117a58c39a 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -815,6 +815,32 @@ struct cx231xx_board cx231xx_boards[] = {
 			.gpio = NULL,
 		} },
 	},
+	[CX231XX_BOARD_TERRATEC_GRABBY] = {
+		.name = "Terratec Grabby",
+		.tuner_type = TUNER_ABSENT,
+		.decoder = CX231XX_AVDECODER,
+		.output_mode = OUT_MODE_VIP11,
+		.demod_xfer_mode = 0,
+		.ctl_pin_status_mask = 0xFFFFFFC4,
+		.agc_analog_digital_select_gpio = 0x0c,
+		.gpio_pin_status_mask = 0x4001000,
+		.norm = V4L2_STD_PAL,
+		.no_alt_vanc = 1,
+		.external_av = 1,
+		.input = {{
+			.type = CX231XX_VMUX_COMPOSITE1,
+			.vmux = CX231XX_VIN_2_1,
+			.amux = CX231XX_AMUX_LINE_IN,
+			.gpio = NULL,
+		}, {
+			.type = CX231XX_VMUX_SVIDEO,
+			.vmux = CX231XX_VIN_1_1 |
+				(CX231XX_VIN_1_2 << 8) |
+				CX25840_SVIDEO_ON,
+			.amux = CX231XX_AMUX_LINE_IN,
+			.gpio = NULL,
+		} },
+	},
 };
 const unsigned int cx231xx_bcount = ARRAY_SIZE(cx231xx_boards);
 
@@ -880,6 +906,8 @@ struct usb_device_id cx231xx_id_table[] = {
 	 .driver_info = CX231XX_BOARD_ELGATO_VIDEO_CAPTURE_V2},
 	{USB_DEVICE(0x1f4d, 0x0102),
 	 .driver_info = CX231XX_BOARD_OTG102},
+	{USB_DEVICE(USB_VID_TERRATEC, 0x00a6),
+	 .driver_info = CX231XX_BOARD_TERRATEC_GRABBY},
 	{},
 };
 
@@ -1092,17 +1120,25 @@ void cx231xx_card_setup(struct cx231xx *dev)
 	case CX231XX_BOARD_HAUPPAUGE_930C_HD_1114xx:
 	case CX231XX_BOARD_HAUPPAUGE_955Q:
 		{
-			struct tveeprom tvee;
-			static u8 eeprom[256];
-			struct i2c_client client;
-
-			memset(&client, 0, sizeof(client));
-			client.adapter = cx231xx_get_i2c_adap(dev, I2C_1_MUX_1);
-			client.addr = 0xa0 >> 1;
+			struct eeprom {
+				struct tveeprom tvee;
+				u8 eeprom[256];
+				struct i2c_client client;
+			};
+			struct eeprom *e = kzalloc(sizeof(*e), GFP_KERNEL);
+
+			if (e == NULL) {
+				dev_err(dev->dev,
+					"failed to allocate memory to read eeprom\n");
+				break;
+			}
+			e->client.adapter = cx231xx_get_i2c_adap(dev, I2C_1_MUX_1);
+			e->client.addr = 0xa0 >> 1;
 
-			read_eeprom(dev, &client, eeprom, sizeof(eeprom));
-			tveeprom_hauppauge_analog(&client,
-						&tvee, eeprom + 0xc0);
+			read_eeprom(dev, &e->client, e->eeprom, sizeof(e->eeprom));
+			tveeprom_hauppauge_analog(&e->client,
+						&e->tvee, e->eeprom + 0xc0);
+			kfree(e);
 			break;
 		}
 	}
diff --git a/drivers/media/usb/cx231xx/cx231xx-core.c b/drivers/media/usb/cx231xx/cx231xx-core.c
index e42bde081cd7..a2fd49b6be83 100644
--- a/drivers/media/usb/cx231xx/cx231xx-core.c
+++ b/drivers/media/usb/cx231xx/cx231xx-core.c
@@ -653,22 +653,20 @@ int cx231xx_demod_reset(struct cx231xx *dev)
 
 	cx231xx_coredbg("Enter cx231xx_demod_reset()\n");
 
-		value[1] = (u8) 0x3;
-		status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-						PWR_CTL_EN, value, 4);
-			msleep(10);
-
-		value[1] = (u8) 0x0;
-		status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-						PWR_CTL_EN, value, 4);
-			msleep(10);
-
-		value[1] = (u8) 0x3;
-		status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
-						PWR_CTL_EN, value, 4);
-			msleep(10);
-
-
+	value[1] = (u8) 0x3;
+	status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+					PWR_CTL_EN, value, 4);
+	msleep(10);
+
+	value[1] = (u8) 0x0;
+	status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+					PWR_CTL_EN, value, 4);
+	msleep(10);
+
+	value[1] = (u8) 0x3;
+	status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER,
+					PWR_CTL_EN, value, 4);
+	msleep(10);
 
 	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, PWR_CTL_EN,
 				 value, 4);
diff --git a/drivers/media/usb/cx231xx/cx231xx-dvb.c b/drivers/media/usb/cx231xx/cx231xx-dvb.c
index 610d5675bde6..66ee161fc7ba 100644
--- a/drivers/media/usb/cx231xx/cx231xx-dvb.c
+++ b/drivers/media/usb/cx231xx/cx231xx-dvb.c
@@ -797,6 +797,7 @@ static int dvb_init(struct cx231xx *dev)
 		/* attach tuner */
 		memset(&si2157_config, 0, sizeof(si2157_config));
 		si2157_config.fe = dev->dvb->frontend;
+		si2157_config.if_port = 1;
 		si2157_config.inversion = true;
 		strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 		info.addr = 0x60;
@@ -852,6 +853,7 @@ static int dvb_init(struct cx231xx *dev)
 		/* attach tuner */
 		memset(&si2157_config, 0, sizeof(si2157_config));
 		si2157_config.fe = dev->dvb->frontend;
+		si2157_config.if_port = 1;
 		si2157_config.inversion = true;
 		strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 		info.addr = 0x60;
diff --git a/drivers/media/usb/cx231xx/cx231xx-vbi.c b/drivers/media/usb/cx231xx/cx231xx-vbi.c
index 80261ac40208..a08014d20a5c 100644
--- a/drivers/media/usb/cx231xx/cx231xx-vbi.c
+++ b/drivers/media/usb/cx231xx/cx231xx-vbi.c
@@ -192,8 +192,7 @@ static void free_buffer(struct videobuf_queue *vq, struct cx231xx_buffer *buf)
 	struct cx231xx_fh *fh = vq->priv_data;
 	struct cx231xx *dev = fh->dev;
 	unsigned long flags = 0;
-	if (in_interrupt())
-		BUG();
+	BUG_ON(in_interrupt());
 
 	/* We used to wait for the buffer to finish here, but this didn't work
 	   because, as we were keeping the state as VIDEOBUF_QUEUED,
diff --git a/drivers/media/usb/cx231xx/cx231xx-video.c b/drivers/media/usb/cx231xx/cx231xx-video.c
index c261e160c158..c6ff8968286a 100644
--- a/drivers/media/usb/cx231xx/cx231xx-video.c
+++ b/drivers/media/usb/cx231xx/cx231xx-video.c
@@ -749,8 +749,7 @@ static void free_buffer(struct videobuf_queue *vq, struct cx231xx_buffer *buf)
 	struct cx231xx *dev = fh->dev;
 	unsigned long flags = 0;
 
-	if (in_interrupt())
-		BUG();
+	BUG_ON(in_interrupt());
 
 	/* We used to wait for the buffer to finish here, but this didn't work
 	   because, as we were keeping the state as VIDEOBUF_QUEUED,
@@ -1013,7 +1012,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	struct cx231xx *dev = fh->dev;
 	int rc;
 	struct cx231xx_fmt *fmt;
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 
 	rc = check_dev(dev);
 	if (rc < 0)
@@ -1041,9 +1042,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	dev->height = f->fmt.pix.height;
 	dev->format = fmt;
 
-	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, MEDIA_BUS_FMT_FIXED);
-	call_all(dev, video, s_mbus_fmt, &mbus_fmt);
-	v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
+	v4l2_fill_mbus_format(&format.format, &f->fmt.pix, MEDIA_BUS_FMT_FIXED);
+	call_all(dev, pad, set_fmt, NULL, &format);
+	v4l2_fill_pix_format(&f->fmt.pix, &format.format);
 
 	return rc;
 }
@@ -1061,7 +1062,9 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
-	struct v4l2_mbus_framefmt mbus_fmt;
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
 	int rc;
 
 	rc = check_dev(dev);
@@ -1085,11 +1088,10 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id norm)
 	/* We need to reset basic properties in the decoder related to
 	   resolution (since a standard change effects things like the number
 	   of lines in VACT, etc) */
-	memset(&mbus_fmt, 0, sizeof(mbus_fmt));
-	mbus_fmt.code = MEDIA_BUS_FMT_FIXED;
-	mbus_fmt.width = dev->width;
-	mbus_fmt.height = dev->height;
-	call_all(dev, video, s_mbus_fmt, &mbus_fmt);
+	format.format.code = MEDIA_BUS_FMT_FIXED;
+	format.format.width = dev->width;
+	format.format.height = dev->height;
+	call_all(dev, pad, set_fmt, NULL, &format);
 
 	/* do mode control overrides */
 	cx231xx_do_mode_ctrl_overrides(dev);
diff --git a/drivers/media/usb/cx231xx/cx231xx.h b/drivers/media/usb/cx231xx/cx231xx.h
index 00d3bce9a690..54790fbe8fdc 100644
--- a/drivers/media/usb/cx231xx/cx231xx.h
+++ b/drivers/media/usb/cx231xx/cx231xx.h
@@ -77,6 +77,7 @@
 #define CX231XX_BOARD_HAUPPAUGE_930C_HD_1113xx 19
 #define CX231XX_BOARD_HAUPPAUGE_930C_HD_1114xx 20
 #define CX231XX_BOARD_HAUPPAUGE_955Q 21
+#define CX231XX_BOARD_TERRATEC_GRABBY 22
 
 /* Limits minimum and default number of buffers */
 #define CX231XX_MIN_BUF                 4
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index 16c0b7d4f8e7..95a7388e89d4 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -641,7 +641,7 @@ static int af9015_af9013_set_frontend(struct dvb_frontend *fe)
 
 /* override demod callbacks for resource locking */
 static int af9015_af9013_read_status(struct dvb_frontend *fe,
-	fe_status_t *status)
+	enum fe_status *status)
 {
 	int ret;
 	struct af9015_state *state = fe_to_priv(fe);
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.h b/drivers/media/usb/dvb-usb-v2/af9015.h
index 3a6f3ad1eadb..1db1bb0d57bc 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.h
+++ b/drivers/media/usb/dvb-usb-v2/af9015.h
@@ -133,7 +133,7 @@ struct af9015_state {
 
 	/* for demod callback override */
 	int (*set_frontend[2]) (struct dvb_frontend *fe);
-	int (*read_status[2]) (struct dvb_frontend *fe, fe_status_t *status);
+	int (*read_status[2]) (struct dvb_frontend *fe, enum fe_status *status);
 	int (*init[2]) (struct dvb_frontend *fe);
 	int (*sleep[2]) (struct dvb_frontend *fe);
 	int (*tuner_init[2]) (struct dvb_frontend *fe);
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index 80a29f5377ea..6e02a15d39ce 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -1234,10 +1234,6 @@ static int af9035_frontend_detach(struct dvb_usb_adapter *adap)
 	return 0;
 }
 
-static struct tua9001_config af9035_tua9001_config = {
-	.i2c_addr = 0x60,
-};
-
 static const struct fc0011_config af9035_fc0011_config = {
 	.i2c_address = 0x60,
 };
@@ -1265,11 +1261,6 @@ static struct tda18218_config af9035_tda18218_config = {
 	.i2c_wr_max = 21,
 };
 
-static const struct fc2580_config af9035_fc2580_config = {
-	.i2c_addr = 0x56,
-	.clock = 16384000,
-};
-
 static const struct fc0012_config af9035_fc0012_config[] = {
 	{
 		.i2c_address = 0x63,
@@ -1301,9 +1292,15 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 	 */
 
 	switch (state->af9033_config[adap->id].tuner) {
-	case AF9033_TUNER_TUA9001:
-		/* AF9035 gpiot3 = TUA9001 RESETN
-		   AF9035 gpiot2 = TUA9001 RXEN */
+	case AF9033_TUNER_TUA9001: {
+		struct tua9001_platform_data tua9001_pdata = {
+			.dvb_frontend = adap->fe[0],
+		};
+
+		/*
+		 * AF9035 gpiot3 = TUA9001 RESETN
+		 * AF9035 gpiot2 = TUA9001 RXEN
+		 */
 
 		/* configure gpiot2 and gpiot2 as output */
 		ret = af9035_wr_reg_mask(d, 0x00d8ec, 0x01, 0x01);
@@ -1323,9 +1320,14 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 			goto err;
 
 		/* attach tuner */
-		fe = dvb_attach(tua9001_attach, adap->fe[0],
-				&d->i2c_adap, &af9035_tua9001_config);
+		ret = af9035_add_i2c_dev(d, "tua9001", 0x60, &tua9001_pdata,
+					 &d->i2c_adap);
+		if (ret)
+			goto err;
+
+		fe = adap->fe[0];
 		break;
+	}
 	case AF9033_TUNER_FC0011:
 		fe = dvb_attach(fc0011_attach, adap->fe[0],
 				&d->i2c_adap, &af9035_fc0011_config);
@@ -1390,7 +1392,11 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 		fe = dvb_attach(tda18218_attach, adap->fe[0],
 				&d->i2c_adap, &af9035_tda18218_config);
 		break;
-	case AF9033_TUNER_FC2580:
+	case AF9033_TUNER_FC2580: {
+		struct fc2580_platform_data fc2580_pdata = {
+			.dvb_frontend = adap->fe[0],
+		};
+
 		/* Tuner enable using gpiot2_o, gpiot2_en and gpiot2_on  */
 		ret = af9035_wr_reg_mask(d, 0xd8eb, 0x01, 0x01);
 		if (ret < 0)
@@ -1406,9 +1412,14 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 
 		usleep_range(10000, 50000);
 		/* attach tuner */
-		fe = dvb_attach(fc2580_attach, adap->fe[0],
-				&d->i2c_adap, &af9035_fc2580_config);
+		ret = af9035_add_i2c_dev(d, "fc2580", 0x56, &fc2580_pdata,
+					 &d->i2c_adap);
+		if (ret)
+			goto err;
+
+		fe = adap->fe[0];
 		break;
+	}
 	case AF9033_TUNER_FC0012:
 		/*
 		 * AF9035 gpiot2 = FC0012 enable
@@ -1569,6 +1580,7 @@ static int it930x_tuner_attach(struct dvb_usb_adapter *adap)
 
 	memset(&si2157_config, 0, sizeof(si2157_config));
 	si2157_config.fe = adap->fe[0];
+	si2157_config.if_port = 1;
 	ret = af9035_add_i2c_dev(d, "si2157", 0x63,
 			&si2157_config, state->i2c_adapter_demod);
 
@@ -1611,6 +1623,8 @@ static int af9035_tuner_detach(struct dvb_usb_adapter *adap)
 	dev_dbg(&d->udev->dev, "%s: adap->id=%d\n", __func__, adap->id);
 
 	switch (state->af9033_config[adap->id].tuner) {
+	case AF9033_TUNER_TUA9001:
+	case AF9033_TUNER_FC2580:
 	case AF9033_TUNER_IT9135_38:
 	case AF9033_TUNER_IT9135_51:
 	case AF9033_TUNER_IT9135_52:
@@ -2021,6 +2035,9 @@ static const struct usb_device_id af9035_id_table[] = {
 		&af9035_props, "Asus U3100Mini Plus", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, 0x00aa,
 		&af9035_props, "TerraTec Cinergy T Stick (rev. 2)", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_AVERMEDIA, 0x0337,
+		&af9035_props, "AVerMedia HD Volar (A867)", NULL) },
+
 	/* IT9135 devices */
 	{ DVB_USB_DEVICE(USB_VID_ITETECH, USB_PID_ITETECH_IT9135,
 		&af9035_props, "ITE 9135 Generic", RC_MAP_IT913X_V1) },
@@ -2046,9 +2063,6 @@ static const struct usb_device_id af9035_id_table[] = {
 	{ DVB_USB_DEVICE(USB_VID_KWORLD_2, USB_PID_CTVDIGDUAL_V2,
 		&af9035_props, "Digital Dual TV Receiver CTVDIGDUAL_V2",
 							RC_MAP_IT913X_V1) },
-	/* IT930x devices */
-	{ DVB_USB_DEVICE(USB_VID_ITETECH, USB_PID_ITETECH_IT9303,
-		&it930x_props, "ITE 9303 Generic", NULL) },
 	/* XXX: that same ID [0ccd:0099] is used by af9015 driver too */
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, 0x0099,
 		&af9035_props, "TerraTec Cinergy T Stick Dual RC (rev. 2)",
@@ -2061,6 +2075,10 @@ static const struct usb_device_id af9035_id_table[] = {
 		&af9035_props, "PCTV AndroiDTV (78e)", RC_MAP_IT913X_V1) },
 	{ DVB_USB_DEVICE(USB_VID_PCTV, USB_PID_PCTV_79E,
 		&af9035_props, "PCTV microStick (79e)", RC_MAP_IT913X_V2) },
+
+	/* IT930x devices */
+	{ DVB_USB_DEVICE(USB_VID_ITETECH, USB_PID_ITETECH_IT9303,
+		&it930x_props, "ITE 9303 Generic", NULL) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, af9035_id_table);
diff --git a/drivers/media/usb/dvb-usb-v2/dvbsky.c b/drivers/media/usb/dvb-usb-v2/dvbsky.c
index cdf59bcd760c..0376c092bab8 100644
--- a/drivers/media/usb/dvb-usb-v2/dvbsky.c
+++ b/drivers/media/usb/dvb-usb-v2/dvbsky.c
@@ -45,9 +45,9 @@ struct dvbsky_state {
 
 	/* fe hook functions*/
 	int (*fe_set_voltage)(struct dvb_frontend *fe,
-		fe_sec_voltage_t voltage);
+		enum fe_sec_voltage voltage);
 	int (*fe_read_status)(struct dvb_frontend *fe,
-		fe_status_t *status);
+		enum fe_status *status);
 };
 
 static int dvbsky_usb_generic_rw(struct dvb_usb_device *d,
@@ -237,7 +237,7 @@ static int dvbsky_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
 #endif
 
 static int dvbsky_usb_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t voltage)
+	enum fe_sec_voltage voltage)
 {
 	struct dvb_usb_device *d = fe_to_d(fe);
 	struct dvbsky_state *state = d_to_priv(d);
@@ -277,7 +277,8 @@ static int dvbsky_read_mac_addr(struct dvb_usb_adapter *adap, u8 mac[6])
 	return 0;
 }
 
-static int dvbsky_usb_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int dvbsky_usb_read_status(struct dvb_frontend *fe,
+				  enum fe_status *status)
 {
 	struct dvb_usb_device *d = fe_to_d(fe);
 	struct dvbsky_state *state = d_to_priv(d);
@@ -331,6 +332,7 @@ static int dvbsky_s960_attach(struct dvb_usb_adapter *adap)
 
 	/* attach tuner */
 	ts2020_config.fe = adap->fe[0];
+	ts2020_config.get_agc_pwm = m88ds3103_get_agc_pwm;
 	strlcpy(info.type, "ts2020", I2C_NAME_SIZE);
 	info.addr = 0x60;
 	info.platform_data = &ts2020_config;
@@ -368,7 +370,7 @@ fail_attach:
 }
 
 static int dvbsky_usb_ci_set_voltage(struct dvb_frontend *fe,
-	fe_sec_voltage_t voltage)
+	enum fe_sec_voltage voltage)
 {
 	struct dvb_usb_device *d = fe_to_d(fe);
 	struct dvbsky_state *state = d_to_priv(d);
@@ -453,6 +455,7 @@ static int dvbsky_s960c_attach(struct dvb_usb_adapter *adap)
 
 	/* attach tuner */
 	ts2020_config.fe = adap->fe[0];
+	ts2020_config.get_agc_pwm = m88ds3103_get_agc_pwm;
 	strlcpy(info.type, "ts2020", I2C_NAME_SIZE);
 	info.addr = 0x60;
 	info.platform_data = &ts2020_config;
@@ -549,6 +552,7 @@ static int dvbsky_t680c_attach(struct dvb_usb_adapter *adap)
 	/* attach tuner */
 	memset(&si2157_config, 0, sizeof(si2157_config));
 	si2157_config.fe = adap->fe[0];
+	si2157_config.if_port = 1;
 	memset(&info, 0, sizeof(struct i2c_board_info));
 	strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 	info.addr = 0x60;
@@ -615,7 +619,8 @@ static int dvbsky_t330_attach(struct dvb_usb_adapter *adap)
 	memset(&si2168_config, 0, sizeof(si2168_config));
 	si2168_config.i2c_adapter = &i2c_adapter;
 	si2168_config.fe = &adap->fe[0];
-	si2168_config.ts_mode = SI2168_TS_PARALLEL | 0x40;
+	si2168_config.ts_mode = SI2168_TS_PARALLEL;
+	si2168_config.ts_clock_gapped = true;
 	memset(&info, 0, sizeof(struct i2c_board_info));
 	strlcpy(info.type, "si2168", I2C_NAME_SIZE);
 	info.addr = 0x64;
@@ -632,6 +637,7 @@ static int dvbsky_t330_attach(struct dvb_usb_adapter *adap)
 	/* attach tuner */
 	memset(&si2157_config, 0, sizeof(si2157_config));
 	si2157_config.fe = adap->fe[0];
+	si2157_config.if_port = 1;
 	memset(&info, 0, sizeof(struct i2c_board_info));
 	strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 	info.addr = 0x60;
diff --git a/drivers/media/usb/dvb-usb-v2/lmedm04.c b/drivers/media/usb/dvb-usb-v2/lmedm04.c
index 5de6f7c04d09..4cc55b3a0558 100644
--- a/drivers/media/usb/dvb-usb-v2/lmedm04.c
+++ b/drivers/media/usb/dvb-usb-v2/lmedm04.c
@@ -126,7 +126,7 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 struct lme2510_state {
 	unsigned long int_urb_due;
-	fe_status_t lock_status;
+	enum fe_status lock_status;
 	u8 id;
 	u8 tuner_config;
 	u8 signal_level;
@@ -144,12 +144,12 @@ struct lme2510_state {
 	struct urb *lme_urb;
 	void *usb_buffer;
 	/* Frontend original calls */
-	int (*fe_read_status)(struct dvb_frontend *, fe_status_t *);
+	int (*fe_read_status)(struct dvb_frontend *, enum fe_status *);
 	int (*fe_read_signal_strength)(struct dvb_frontend *, u16 *);
 	int (*fe_read_snr)(struct dvb_frontend *, u16 *);
 	int (*fe_read_ber)(struct dvb_frontend *, u32 *);
 	int (*fe_read_ucblocks)(struct dvb_frontend *, u32 *);
-	int (*fe_set_voltage)(struct dvb_frontend *, fe_sec_voltage_t);
+	int (*fe_set_voltage)(struct dvb_frontend *, enum fe_sec_voltage);
 	u8 dvb_usb_lme2510_firmware;
 };
 
@@ -257,6 +257,62 @@ static int lme2510_enable_pid(struct dvb_usb_device *d, u8 index, u16 pid_out)
 	return ret;
 }
 
+/* Convert range from 0x00-0xff to 0x0000-0xffff */
+#define reg_to_16bits(x)	((x) | ((x) << 8))
+
+static void lme2510_update_stats(struct dvb_usb_adapter *adap)
+{
+	struct lme2510_state *st = adap_to_priv(adap);
+	struct dvb_frontend *fe = adap->fe[0];
+	struct dtv_frontend_properties *c;
+	u32 s_tmp = 0, c_tmp = 0;
+
+	if (!fe)
+		return;
+
+	c = &fe->dtv_property_cache;
+
+	c->block_count.len = 1;
+	c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->block_error.len = 1;
+	c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_count.len = 1;
+	c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_error.len = 1;
+	c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+
+	if (st->i2c_talk_onoff) {
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+		return;
+	}
+
+	switch (st->tuner_config) {
+	case TUNER_LG:
+		s_tmp = reg_to_16bits(0xff - st->signal_level);
+		c_tmp = reg_to_16bits(0xff - st->signal_sn);
+		break;
+	case TUNER_S7395:
+	case TUNER_S0194:
+		s_tmp = 0xffff - (((st->signal_level * 2) << 8) * 5 / 4);
+		c_tmp = reg_to_16bits((0xff - st->signal_sn - 0xa1) * 3);
+		break;
+	case TUNER_RS2000:
+		s_tmp = reg_to_16bits(st->signal_level);
+		c_tmp = reg_to_16bits(st->signal_sn);
+	}
+
+	c->strength.len = 1;
+	c->strength.stat[0].scale = FE_SCALE_RELATIVE;
+	c->strength.stat[0].uvalue = (u64)s_tmp;
+
+	c->cnr.len = 1;
+	c->cnr.stat[0].scale = FE_SCALE_RELATIVE;
+	c->cnr.stat[0].uvalue = (u64)c_tmp;
+}
+
 static void lme2510_int_response(struct urb *lme_urb)
 {
 	struct dvb_usb_adapter *adap = lme_urb->context;
@@ -337,6 +393,8 @@ static void lme2510_int_response(struct urb *lme_urb)
 			if (!signal_lock)
 				st->lock_status &= ~FE_HAS_LOCK;
 
+			lme2510_update_stats(adap);
+
 			debug_data_snipet(5, "INT Remote data snipet in", ibuf);
 		break;
 		case 0xcc:
@@ -799,10 +857,11 @@ static struct m88rs2000_config m88rs2000_config = {
 static struct ts2020_config ts2020_config = {
 	.tuner_address = 0x60,
 	.clk_out_div = 7,
+	.dont_poll = true
 };
 
 static int dm04_lme2510_set_voltage(struct dvb_frontend *fe,
-					fe_sec_voltage_t voltage)
+				    enum fe_sec_voltage voltage)
 {
 	struct dvb_usb_device *d = fe_to_d(fe);
 	struct lme2510_state *st = fe_to_priv(fe);
@@ -837,7 +896,7 @@ static int dm04_lme2510_set_voltage(struct dvb_frontend *fe,
 	return (ret < 0) ? -ENODEV : 0;
 }
 
-static int dm04_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int dm04_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct dvb_usb_device *d = fe_to_d(fe);
 	struct lme2510_state *st = d->priv;
@@ -871,56 +930,45 @@ static int dm04_read_status(struct dvb_frontend *fe, fe_status_t *status)
 
 	*status = st->lock_status;
 
-	if (!(*status & FE_HAS_LOCK))
+	if (!(*status & FE_HAS_LOCK)) {
+		struct dvb_usb_adapter *adap = fe_to_adap(fe);
+
 		st->i2c_talk_onoff = 1;
 
+		lme2510_update_stats(adap);
+	}
+
 	return ret;
 }
 
 static int dm04_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 {
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct lme2510_state *st = fe_to_priv(fe);
 
 	if (st->fe_read_signal_strength && !st->stream_on)
 		return st->fe_read_signal_strength(fe, strength);
 
-	switch (st->tuner_config) {
-	case TUNER_LG:
-		*strength = 0xff - st->signal_level;
-		*strength |= *strength << 8;
-		break;
-	/* fall through */
-	case TUNER_S7395:
-	case TUNER_S0194:
-		*strength = 0xffff - (((st->signal_level * 2) << 8) * 5 / 4);
-		break;
-	case TUNER_RS2000:
-		*strength = (u16)((u32)st->signal_level * 0xffff / 0xff);
-	}
+	if (c->strength.stat[0].scale == FE_SCALE_RELATIVE)
+		*strength = (u16)c->strength.stat[0].uvalue;
+	else
+		*strength = 0;
 
 	return 0;
 }
 
 static int dm04_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct lme2510_state *st = fe_to_priv(fe);
 
 	if (st->fe_read_snr && !st->stream_on)
 		return st->fe_read_snr(fe, snr);
 
-	switch (st->tuner_config) {
-	case TUNER_LG:
-		*snr = 0xff - st->signal_sn;
-		*snr |= *snr << 8;
-		break;
-	/* fall through */
-	case TUNER_S7395:
-	case TUNER_S0194:
-		*snr = (u16)((0xff - st->signal_sn - 0xa1) * 3) << 8;
-		break;
-	case TUNER_RS2000:
-		*snr = (u16)((u32)st->signal_sn * 0xffff / 0x7f);
-	}
+	if (c->cnr.stat[0].scale == FE_SCALE_RELATIVE)
+		*snr = (u16)c->cnr.stat[0].uvalue;
+	else
+		*snr = 0;
 
 	return 0;
 }
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
index ecefa5c477fa..ea3753653368 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
@@ -72,7 +72,7 @@ int mxl111sf_demod_program_regs(struct mxl111sf_demod_state *state,
 
 static
 int mxl1x1sf_demod_get_tps_code_rate(struct mxl111sf_demod_state *state,
-				     fe_code_rate_t *code_rate)
+				     enum fe_code_rate *code_rate)
 {
 	u8 val;
 	int ret = mxl111sf_demod_read_reg(state, V6_CODE_RATE_TPS_REG, &val);
@@ -103,7 +103,7 @@ fail:
 
 static
 int mxl1x1sf_demod_get_tps_modulation(struct mxl111sf_demod_state *state,
-					 fe_modulation_t *modulation)
+				      enum fe_modulation *modulation)
 {
 	u8 val;
 	int ret = mxl111sf_demod_read_reg(state, V6_MODORDER_TPS_REG, &val);
@@ -128,7 +128,7 @@ fail:
 
 static
 int mxl1x1sf_demod_get_tps_guard_fft_mode(struct mxl111sf_demod_state *state,
-					  fe_transmit_mode_t *fft_mode)
+					  enum fe_transmit_mode *fft_mode)
 {
 	u8 val;
 	int ret = mxl111sf_demod_read_reg(state, V6_MODE_TPS_REG, &val);
@@ -153,7 +153,7 @@ fail:
 
 static
 int mxl1x1sf_demod_get_tps_guard_interval(struct mxl111sf_demod_state *state,
-					  fe_guard_interval_t *guard)
+					  enum fe_guard_interval *guard)
 {
 	u8 val;
 	int ret = mxl111sf_demod_read_reg(state, V6_CP_TPS_REG, &val);
@@ -181,7 +181,7 @@ fail:
 
 static
 int mxl1x1sf_demod_get_tps_hierarchy(struct mxl111sf_demod_state *state,
-				     fe_hierarchy_t *hierarchy)
+				     enum fe_hierarchy *hierarchy)
 {
 	u8 val;
 	int ret = mxl111sf_demod_read_reg(state, V6_TPS_HIERACHY_REG, &val);
@@ -441,7 +441,7 @@ fail:
 }
 
 static int mxl111sf_demod_read_status(struct dvb_frontend *fe,
-				      fe_status_t *status)
+				      enum fe_status *status)
 {
 	struct mxl111sf_demod_state *state = fe->demodulator_priv;
 	int ret, locked, cr_lock, sync_lock, fec_lock;
@@ -480,7 +480,7 @@ static int mxl111sf_demod_read_signal_strength(struct dvb_frontend *fe,
 					       u16 *signal_strength)
 {
 	struct mxl111sf_demod_state *state = fe->demodulator_priv;
-	fe_modulation_t modulation;
+	enum fe_modulation modulation;
 	u16 snr;
 
 	mxl111sf_demod_calc_snr(state, &snr);
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index 895441fe90f7..c3cac4c12fb3 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -217,7 +217,7 @@ static int rtl28xxu_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				req.data = &msg[0].buf[1];
 				ret = rtl28xxu_ctrl_msg(d, &req);
 			}
-		} else if (msg[0].len < 23) {
+		} else if ((msg[0].len < 23) && (!dev->new_i2c_write)) {
 			/* method 2 - old I2C */
 			req.value = (msg[0].buf[0] << 8) | (msg[0].addr << 1);
 			req.index = CMD_I2C_WR;
@@ -232,8 +232,14 @@ static int rtl28xxu_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			req.data = msg[0].buf;
 			ret = rtl28xxu_ctrl_msg(d, &req);
 		}
+	} else if (num == 1 && (msg[0].flags & I2C_M_RD)) {
+		req.value = (msg[0].addr << 1);
+		req.index = CMD_I2C_DA_RD;
+		req.size = msg[0].len;
+		req.data = msg[0].buf;
+		ret = rtl28xxu_ctrl_msg(d, &req);
 	} else {
-		ret = -EINVAL;
+		ret = -EOPNOTSUPP;
 	}
 
 err_mutex_unlock:
@@ -357,6 +363,8 @@ static int rtl2832u_read_config(struct dvb_usb_device *d)
 	struct rtl28xxu_req req_r828d = {0x0074, CMD_I2C_RD, 1, buf};
 	struct rtl28xxu_req req_mn88472 = {0xff38, CMD_I2C_RD, 1, buf};
 	struct rtl28xxu_req req_mn88473 = {0xff38, CMD_I2C_RD, 1, buf};
+	struct rtl28xxu_req req_si2157 = {0x00c0, CMD_I2C_RD, 1, buf};
+	struct rtl28xxu_req req_si2168 = {0x00c8, CMD_I2C_RD, 1, buf};
 
 	dev_dbg(&d->intf->dev, "\n");
 
@@ -477,6 +485,35 @@ static int rtl2832u_read_config(struct dvb_usb_device *d)
 		goto tuner_found;
 	}
 
+	/* GPIO0 and GPIO5 to reset Si2157/Si2168 tuner and demod */
+	ret = rtl28xxu_wr_reg_mask(d, SYS_GPIO_OUT_VAL, 0x00, 0x21);
+	if (ret)
+		goto err;
+
+	ret = rtl28xxu_wr_reg_mask(d, SYS_GPIO_OUT_EN, 0x00, 0x21);
+	if (ret)
+		goto err;
+
+	msleep(50);
+
+	ret = rtl28xxu_wr_reg_mask(d, SYS_GPIO_OUT_VAL, 0x21, 0x21);
+	if (ret)
+		goto err;
+
+	ret = rtl28xxu_wr_reg_mask(d, SYS_GPIO_OUT_EN, 0x21, 0x21);
+	if (ret)
+		goto err;
+
+	msleep(50);
+
+	/* check Si2157 ID register; reg=c0 val=80 */
+	ret = rtl28xxu_ctrl_msg(d, &req_si2157);
+	if (ret == 0 && ((buf[0] & 0x80) == 0x80)) {
+		dev->tuner = TUNER_RTL2832_SI2157;
+		dev->tuner_name = "SI2157";
+		goto tuner_found;
+	}
+
 tuner_found:
 	dev_dbg(&d->intf->dev, "tuner=%s\n", dev->tuner_name);
 
@@ -510,6 +547,15 @@ tuner_found:
 			goto demod_found;
 		}
 	}
+	if (dev->tuner == TUNER_RTL2832_SI2157) {
+		/* check Si2168 ID register; reg=c8 val=80 */
+		ret = rtl28xxu_ctrl_msg(d, &req_si2168);
+		if (ret == 0 && ((buf[0] & 0x80) == 0x80)) {
+			dev_dbg(&d->intf->dev, "Si2168 found\n");
+			dev->slave_demod = SLAVE_DEMOD_SI2168;
+			goto demod_found;
+		}
+	}
 
 demod_found:
 	/* close demod I2C gate */
@@ -643,6 +689,11 @@ err:
 	return ret;
 }
 
+static const struct rtl2832_platform_data rtl2832_fc2580_platform_data = {
+	.clk = 28800000,
+	.tuner = TUNER_RTL2832_FC2580,
+};
+
 static const struct rtl2832_platform_data rtl2832_fc0012_platform_data = {
 	.clk = 28800000,
 	.tuner = TUNER_RTL2832_FC0012
@@ -668,6 +719,11 @@ static const struct rtl2832_platform_data rtl2832_r820t_platform_data = {
 	.tuner = TUNER_RTL2832_R820T,
 };
 
+static const struct rtl2832_platform_data rtl2832_si2157_platform_data = {
+	.clk = 28800000,
+	.tuner = TUNER_RTL2832_SI2157,
+};
+
 static int rtl2832u_fc0012_tuner_callback(struct dvb_usb_device *d,
 		int cmd, int arg)
 {
@@ -804,8 +860,7 @@ static int rtl2832u_frontend_attach(struct dvb_usb_adapter *adap)
 		*pdata = rtl2832_fc0013_platform_data;
 		break;
 	case TUNER_RTL2832_FC2580:
-		/* FIXME: do not abuse fc0012 settings */
-		*pdata = rtl2832_fc0012_platform_data;
+		*pdata = rtl2832_fc2580_platform_data;
 		break;
 	case TUNER_RTL2832_TUA9001:
 		*pdata = rtl2832_tua9001_platform_data;
@@ -817,6 +872,9 @@ static int rtl2832u_frontend_attach(struct dvb_usb_adapter *adap)
 	case TUNER_RTL2832_R828D:
 		*pdata = rtl2832_r820t_platform_data;
 		break;
+	case TUNER_RTL2832_SI2157:
+		*pdata = rtl2832_si2157_platform_data;
+		break;
 	default:
 		dev_err(&d->intf->dev, "unknown tuner %s\n", dev->tuner_name);
 		ret = -ENODEV;
@@ -884,7 +942,7 @@ static int rtl2832u_frontend_attach(struct dvb_usb_adapter *adap)
 			}
 
 			dev->i2c_client_slave_demod = client;
-		} else {
+		} else if (dev->slave_demod == SLAVE_DEMOD_MN88473) {
 			struct mn88473_config mn88473_config = {};
 
 			mn88473_config.fe = &adap->fe[1];
@@ -906,9 +964,37 @@ static int rtl2832u_frontend_attach(struct dvb_usb_adapter *adap)
 			}
 
 			dev->i2c_client_slave_demod = client;
+		} else {
+			struct si2168_config si2168_config = {};
+			struct i2c_adapter *adapter;
+
+			si2168_config.i2c_adapter = &adapter;
+			si2168_config.fe = &adap->fe[1];
+			si2168_config.ts_mode = SI2168_TS_SERIAL;
+			si2168_config.ts_clock_inv = false;
+			si2168_config.ts_clock_gapped = true;
+			strlcpy(info.type, "si2168", I2C_NAME_SIZE);
+			info.addr = 0x64;
+			info.platform_data = &si2168_config;
+			request_module(info.type);
+			client = i2c_new_device(&d->i2c_adap, &info);
+			if (client == NULL || client->dev.driver == NULL) {
+				dev->slave_demod = SLAVE_DEMOD_NONE;
+				goto err_slave_demod_failed;
+			}
+
+			if (!try_module_get(client->dev.driver->owner)) {
+				i2c_unregister_device(client);
+				dev->slave_demod = SLAVE_DEMOD_NONE;
+				goto err_slave_demod_failed;
+			}
+
+			dev->i2c_client_slave_demod = client;
+
+			/* for Si2168 devices use only new I2C write method */
+			dev->new_i2c_write = true;
 		}
 	}
-
 	return 0;
 err_slave_demod_failed:
 err:
@@ -1018,15 +1104,6 @@ err:
 	return ret;
 }
 
-static const struct fc2580_config rtl2832u_fc2580_config = {
-	.i2c_addr = 0x56,
-	.clock = 16384000,
-};
-
-static struct tua9001_config rtl2832u_tua9001_config = {
-	.i2c_addr = 0x60,
-};
-
 static const struct fc0012_config rtl2832u_fc0012_config = {
 	.i2c_address = 0x63, /* 0xc6 >> 1 */
 	.xtal_freq = FC_XTAL_28_8_MHZ,
@@ -1105,12 +1182,34 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 			subdev = i2c_get_clientdata(client);
 		}
 		break;
-	case TUNER_RTL2832_FC2580:
-		fe = dvb_attach(fc2580_attach, adap->fe[0],
-				dev->demod_i2c_adapter,
-				&rtl2832u_fc2580_config);
+	case TUNER_RTL2832_FC2580: {
+			struct fc2580_platform_data fc2580_pdata = {
+				.dvb_frontend = adap->fe[0],
+			};
+			struct i2c_board_info board_info = {};
+
+			strlcpy(board_info.type, "fc2580", I2C_NAME_SIZE);
+			board_info.addr = 0x56;
+			board_info.platform_data = &fc2580_pdata;
+			request_module("fc2580");
+			client = i2c_new_device(dev->demod_i2c_adapter,
+						&board_info);
+			if (client == NULL || client->dev.driver == NULL)
+				break;
+			if (!try_module_get(client->dev.driver->owner)) {
+				i2c_unregister_device(client);
+				break;
+			}
+			dev->i2c_client_tuner = client;
+			subdev = fc2580_pdata.get_v4l2_subdev(client);
+		}
 		break;
-	case TUNER_RTL2832_TUA9001:
+	case TUNER_RTL2832_TUA9001: {
+		struct tua9001_platform_data tua9001_pdata = {
+			.dvb_frontend = adap->fe[0],
+		};
+		struct i2c_board_info board_info = {};
+
 		/* enable GPIO1 and GPIO4 as output */
 		ret = rtl28xxu_wr_reg_mask(d, SYS_GPIO_DIR, 0x00, 0x12);
 		if (ret)
@@ -1120,10 +1219,20 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 		if (ret)
 			goto err;
 
-		fe = dvb_attach(tua9001_attach, adap->fe[0],
-				dev->demod_i2c_adapter,
-				&rtl2832u_tua9001_config);
+		strlcpy(board_info.type, "tua9001", I2C_NAME_SIZE);
+		board_info.addr = 0x60;
+		board_info.platform_data = &tua9001_pdata;
+		request_module("tua9001");
+		client = i2c_new_device(dev->demod_i2c_adapter, &board_info);
+		if (client == NULL || client->dev.driver == NULL)
+			break;
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			break;
+		}
+		dev->i2c_client_tuner = client;
 		break;
+	}
 	case TUNER_RTL2832_R820T:
 		fe = dvb_attach(r820t_attach, adap->fe[0],
 				dev->demod_i2c_adapter,
@@ -1148,6 +1257,39 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 					adap->fe[1]->ops.tuner_ops.get_rf_strength;
 		}
 		break;
+	case TUNER_RTL2832_SI2157: {
+			struct si2157_config si2157_config = {
+				.fe = adap->fe[0],
+				.if_port = 0,
+				.inversion = false,
+			};
+
+			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
+			info.addr = 0x60;
+			info.platform_data = &si2157_config;
+			request_module(info.type);
+			client = i2c_new_device(&d->i2c_adap, &info);
+			if (client == NULL || client->dev.driver == NULL)
+				break;
+
+			if (!try_module_get(client->dev.driver->owner)) {
+				i2c_unregister_device(client);
+				break;
+			}
+
+			dev->i2c_client_tuner = client;
+			subdev = i2c_get_clientdata(client);
+
+			/* copy tuner ops for 2nd FE as tuner is shared */
+			if (adap->fe[1]) {
+				adap->fe[1]->tuner_priv =
+						adap->fe[0]->tuner_priv;
+				memcpy(&adap->fe[1]->ops.tuner_ops,
+						&adap->fe[0]->ops.tuner_ops,
+						sizeof(struct dvb_tuner_ops));
+			}
+		}
+		break;
 	default:
 		dev_err(&d->intf->dev, "unknown tuner %d\n", dev->tuner);
 	}
@@ -1158,6 +1300,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 
 	/* register SDR */
 	switch (dev->tuner) {
+	case TUNER_RTL2832_FC2580:
 	case TUNER_RTL2832_FC0012:
 	case TUNER_RTL2832_FC0013:
 	case TUNER_RTL2832_E4000:
@@ -1178,7 +1321,7 @@ static int rtl2832u_tuner_attach(struct dvb_usb_adapter *adap)
 						     "rtl2832_sdr",
 						     PLATFORM_DEVID_AUTO,
 						     &pdata, sizeof(pdata));
-		if (pdev == NULL || pdev->dev.driver == NULL)
+		if (IS_ERR(pdev) || pdev->dev.driver == NULL)
 			break;
 		dev->platform_device_sdr = pdev;
 		break;
@@ -1764,6 +1907,8 @@ static const struct usb_device_id rtl28xxu_id_table[] = {
 	/* RTL2832P devices: */
 	{ DVB_USB_DEVICE(USB_VID_HANFTEK, 0x0131,
 		&rtl28xxu_props, "Astrometa DVB-T2", NULL) },
+	{ DVB_USB_DEVICE(0x5654, 0xca42,
+		&rtl28xxu_props, "GoTView MasterHD 3", NULL) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, rtl28xxu_id_table);
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.h b/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
index 1b5d7ffb685e..9f6115a2ee01 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.h
@@ -41,6 +41,8 @@
 #include "fc2580.h"
 #include "tua9001.h"
 #include "r820t.h"
+#include "si2168.h"
+#include "si2157.h"
 
 /*
  * USB commands
@@ -76,6 +78,7 @@ struct rtl28xxu_dev {
 	u8 page; /* integrated demod active register page */
 	struct i2c_adapter *demod_i2c_adapter;
 	bool rc_active;
+	bool new_i2c_write;
 	struct i2c_client *i2c_client_demod;
 	struct i2c_client *i2c_client_tuner;
 	struct i2c_client *i2c_client_slave_demod;
@@ -83,6 +86,7 @@ struct rtl28xxu_dev {
 	#define SLAVE_DEMOD_NONE           0
 	#define SLAVE_DEMOD_MN88472        1
 	#define SLAVE_DEMOD_MN88473        2
+	#define SLAVE_DEMOD_SI2168         3
 	unsigned int slave_demod:2;
 	union {
 		struct rtl2830_platform_data rtl2830_platform_data;
@@ -116,6 +120,7 @@ enum rtl28xxu_tuner {
 	TUNER_RTL2832_FC0013,
 	TUNER_RTL2832_R820T,
 	TUNER_RTL2832_R828D,
+	TUNER_RTL2832_SI2157,
 };
 
 struct rtl28xxu_req {
diff --git a/drivers/media/usb/dvb-usb/af9005-fe.c b/drivers/media/usb/dvb-usb/af9005-fe.c
index 740f3f496f12..ac97075d75f7 100644
--- a/drivers/media/usb/dvb-usb/af9005-fe.c
+++ b/drivers/media/usb/dvb-usb/af9005-fe.c
@@ -29,7 +29,7 @@
 
 struct af9005_fe_state {
 	struct dvb_usb_device *d;
-	fe_status_t stat;
+	enum fe_status stat;
 
 	/* retraining parameters */
 	u32 original_fcw;
@@ -437,7 +437,8 @@ static int af9005_fe_refresh_state(struct dvb_frontend *fe)
 	return 0;
 }
 
-static int af9005_fe_read_status(struct dvb_frontend *fe, fe_status_t * stat)
+static int af9005_fe_read_status(struct dvb_frontend *fe,
+				 enum fe_status *stat)
 {
 	struct af9005_fe_state *state = fe->demodulator_priv;
 	u8 temp;
@@ -481,7 +482,7 @@ static int af9005_fe_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 		return ret;
 	if (temp != state->strong) {
 		deb_info("adjust for strong signal %d\n", temp);
-			state->strong = temp;
+		state->strong = temp;
 	}
 	return 0;
 }
diff --git a/drivers/media/usb/dvb-usb/az6027.c b/drivers/media/usb/dvb-usb/az6027.c
index 0df52ab32a7b..92e47d6c3ee3 100644
--- a/drivers/media/usb/dvb-usb/az6027.c
+++ b/drivers/media/usb/dvb-usb/az6027.c
@@ -778,7 +778,8 @@ static int az6027_read_mac_addr(struct dvb_usb_device *d, u8 mac[6])
 }
 */
 
-static int az6027_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int az6027_set_voltage(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage)
 {
 
 	u8 buf;
diff --git a/drivers/media/usb/dvb-usb/cinergyT2-fe.c b/drivers/media/usb/dvb-usb/cinergyT2-fe.c
index c890fe46acd3..b3ec743a7a2e 100644
--- a/drivers/media/usb/dvb-usb/cinergyT2-fe.c
+++ b/drivers/media/usb/dvb-usb/cinergyT2-fe.c
@@ -142,7 +142,7 @@ struct cinergyt2_fe_state {
 };
 
 static int cinergyt2_fe_read_status(struct dvb_frontend *fe,
-					fe_status_t *status)
+				    enum fe_status *status)
 {
 	struct cinergyt2_fe_state *state = fe->demodulator_priv;
 	struct dvbt_get_status_msg result;
diff --git a/drivers/media/usb/dvb-usb/cxusb.c b/drivers/media/usb/dvb-usb/cxusb.c
index ffc3704abded..ab7151181728 100644
--- a/drivers/media/usb/dvb-usb/cxusb.c
+++ b/drivers/media/usb/dvb-usb/cxusb.c
@@ -1350,6 +1350,7 @@ static int cxusb_mygica_t230_frontend_attach(struct dvb_usb_adapter *adap)
 	/* attach tuner */
 	memset(&si2157_config, 0, sizeof(si2157_config));
 	si2157_config.fe = adap->fe_adap[0].fe;
+	si2157_config.if_port = 1;
 	memset(&info, 0, sizeof(struct i2c_board_info));
 	strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 	info.addr = 0x60;
diff --git a/drivers/media/usb/dvb-usb/dib0700.h b/drivers/media/usb/dvb-usb/dib0700.h
index 927617d95616..8fd8f5b489d2 100644
--- a/drivers/media/usb/dvb-usb/dib0700.h
+++ b/drivers/media/usb/dvb-usb/dib0700.h
@@ -48,7 +48,7 @@ struct dib0700_state {
 	u8 disable_streaming_master_mode;
 	u32 fw_version;
 	u32 nb_packet_buffer_size;
-	int (*read_status)(struct dvb_frontend *, fe_status_t *);
+	int (*read_status)(struct dvb_frontend *, enum fe_status *);
 	int (*sleep)(struct dvb_frontend* fe);
 	u8 buf[255];
 };
diff --git a/drivers/media/usb/dvb-usb/dib0700_core.c b/drivers/media/usb/dvb-usb/dib0700_core.c
index 2b40393836ff..0d248ce02a9b 100644
--- a/drivers/media/usb/dvb-usb/dib0700_core.c
+++ b/drivers/media/usb/dvb-usb/dib0700_core.c
@@ -655,10 +655,20 @@ out:
 struct dib0700_rc_response {
 	u8 report_id;
 	u8 data_state;
-	u8 system;
-	u8 not_system;
-	u8 data;
-	u8 not_data;
+	union {
+		struct {
+			u8 system;
+			u8 not_system;
+			u8 data;
+			u8 not_data;
+		} nec;
+		struct {
+			u8 not_used;
+			u8 system;
+			u8 data;
+			u8 not_data;
+		} rc5;
+	};
 };
 #define RC_MSG_SIZE_V1_20 6
 
@@ -694,8 +704,8 @@ static void dib0700_rc_urb_completion(struct urb *purb)
 
 	deb_data("IR ID = %02X state = %02X System = %02X %02X Cmd = %02X %02X (len %d)\n",
 		 poll_reply->report_id, poll_reply->data_state,
-		 poll_reply->system, poll_reply->not_system,
-		 poll_reply->data, poll_reply->not_data,
+		 poll_reply->nec.system, poll_reply->nec.not_system,
+		 poll_reply->nec.data, poll_reply->nec.not_data,
 		 purb->actual_length);
 
 	switch (d->props.rc.core.protocol) {
@@ -704,30 +714,30 @@ static void dib0700_rc_urb_completion(struct urb *purb)
 		toggle = 0;
 
 		/* NEC protocol sends repeat code as 0 0 0 FF */
-		if (poll_reply->system     == 0x00 &&
-		    poll_reply->not_system == 0x00 &&
-		    poll_reply->data       == 0x00 &&
-		    poll_reply->not_data   == 0xff) {
+		if (poll_reply->nec.system     == 0x00 &&
+		    poll_reply->nec.not_system == 0x00 &&
+		    poll_reply->nec.data       == 0x00 &&
+		    poll_reply->nec.not_data   == 0xff) {
 			poll_reply->data_state = 2;
 			break;
 		}
 
-		if ((poll_reply->data ^ poll_reply->not_data) != 0xff) {
+		if ((poll_reply->nec.data ^ poll_reply->nec.not_data) != 0xff) {
 			deb_data("NEC32 protocol\n");
-			keycode = RC_SCANCODE_NEC32(poll_reply->system     << 24 |
-						     poll_reply->not_system << 16 |
-						     poll_reply->data       << 8  |
-						     poll_reply->not_data);
-		} else if ((poll_reply->system ^ poll_reply->not_system) != 0xff) {
+			keycode = RC_SCANCODE_NEC32(poll_reply->nec.system     << 24 |
+						     poll_reply->nec.not_system << 16 |
+						     poll_reply->nec.data       << 8  |
+						     poll_reply->nec.not_data);
+		} else if ((poll_reply->nec.system ^ poll_reply->nec.not_system) != 0xff) {
 			deb_data("NEC extended protocol\n");
-			keycode = RC_SCANCODE_NECX(poll_reply->system << 8 |
-						    poll_reply->not_system,
-						    poll_reply->data);
+			keycode = RC_SCANCODE_NECX(poll_reply->nec.system << 8 |
+						    poll_reply->nec.not_system,
+						    poll_reply->nec.data);
 
 		} else {
 			deb_data("NEC normal protocol\n");
-			keycode = RC_SCANCODE_NEC(poll_reply->system,
-						   poll_reply->data);
+			keycode = RC_SCANCODE_NEC(poll_reply->nec.system,
+						   poll_reply->nec.data);
 		}
 
 		break;
@@ -735,19 +745,19 @@ static void dib0700_rc_urb_completion(struct urb *purb)
 		deb_data("RC5 protocol\n");
 		protocol = RC_TYPE_RC5;
 		toggle = poll_reply->report_id;
-		keycode = RC_SCANCODE_RC5(poll_reply->system, poll_reply->data);
+		keycode = RC_SCANCODE_RC5(poll_reply->rc5.system, poll_reply->rc5.data);
+
+		if ((poll_reply->rc5.data ^ poll_reply->rc5.not_data) != 0xff) {
+			/* Key failed integrity check */
+			err("key failed integrity check: %02x %02x %02x %02x",
+			    poll_reply->rc5.not_used, poll_reply->rc5.system,
+			    poll_reply->rc5.data, poll_reply->rc5.not_data);
+			goto resubmit;
+		}
 
 		break;
 	}
 
-	if ((poll_reply->data + poll_reply->not_data) != 0xff) {
-		/* Key failed integrity check */
-		err("key failed integrity check: %02x %02x %02x %02x",
-		    poll_reply->system,  poll_reply->not_system,
-		    poll_reply->data, poll_reply->not_data);
-		goto resubmit;
-	}
-
 	rc_keydown(d->rc_dev, protocol, keycode, toggle);
 
 resubmit:
diff --git a/drivers/media/usb/dvb-usb/dib0700_devices.c b/drivers/media/usb/dvb-usb/dib0700_devices.c
index d7d55a20e959..7ed49646a699 100644
--- a/drivers/media/usb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/usb/dvb-usb/dib0700_devices.c
@@ -861,22 +861,22 @@ static int dib7770_set_param_override(struct dvb_frontend *fe)
 	struct dvb_usb_adapter *adap = fe->dvb->priv;
 	struct dib0700_adapter_state *state = adap->priv;
 
-	 u16 offset;
-	 u8 band = BAND_OF_FREQUENCY(p->frequency/1000);
-	 switch (band) {
-	 case BAND_VHF:
-		  state->dib7000p_ops.set_gpio(fe, 0, 0, 1);
-		  offset = 850;
-		  break;
-	 case BAND_UHF:
-	 default:
-		  state->dib7000p_ops.set_gpio(fe, 0, 0, 0);
-		  offset = 250;
-		  break;
-	 }
-	 deb_info("WBD for DiB7000P: %d\n", offset + dib0070_wbd_offset(fe));
-	 state->dib7000p_ops.set_wbd_ref(fe, offset + dib0070_wbd_offset(fe));
-	 return state->set_param_save(fe);
+	u16 offset;
+	u8 band = BAND_OF_FREQUENCY(p->frequency/1000);
+	switch (band) {
+	case BAND_VHF:
+		state->dib7000p_ops.set_gpio(fe, 0, 0, 1);
+		offset = 850;
+		break;
+	case BAND_UHF:
+	default:
+		state->dib7000p_ops.set_gpio(fe, 0, 0, 0);
+		offset = 250;
+		break;
+	}
+	deb_info("WBD for DiB7000P: %d\n", offset + dib0070_wbd_offset(fe));
+	state->dib7000p_ops.set_wbd_ref(fe, offset + dib0070_wbd_offset(fe));
+	return state->set_param_save(fe);
 }
 
 static int dib7770p_tuner_attach(struct dvb_usb_adapter *adap)
@@ -3309,7 +3309,7 @@ static int stk7070pd_frontend_attach1(struct dvb_usb_adapter *adap)
 }
 
 static int novatd_read_status_override(struct dvb_frontend *fe,
-		fe_status_t *stat)
+				       enum fe_status *stat)
 {
 	struct dvb_usb_adapter *adap = fe->dvb->priv;
 	struct dvb_usb_device *dev = adap->dev;
@@ -3821,6 +3821,10 @@ MODULE_DEVICE_TABLE(usb, dib0700_usb_id_table);
 		} \
 	}
 
+#define DIB0700_NUM_FRONTENDS(n) \
+	.num_frontends = n, \
+	.size_of_priv     = sizeof(struct dib0700_adapter_state)
+
 struct dvb_usb_device_properties dib0700_devices[] = {
 	{
 		DIB0700_DEFAULT_DEVICE_PROPERTIES,
@@ -3828,7 +3832,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -3839,7 +3843,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -3893,7 +3896,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.frontend_attach  = bristol_frontend_attach,
 				.tuner_attach     = bristol_tuner_attach,
@@ -3901,7 +3904,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
 			}, {
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.frontend_attach  = bristol_frontend_attach,
 				.tuner_attach     = bristol_tuner_attach,
@@ -3933,7 +3936,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -3945,7 +3948,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
 			}, {
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -3998,7 +4001,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4043,7 +4046,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4054,7 +4057,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4125,7 +4127,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4136,7 +4138,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4171,7 +4172,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4182,9 +4183,8 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			}, {
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4195,7 +4195,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			}
 		},
 
@@ -4230,7 +4229,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4241,9 +4240,8 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			}, {
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4254,7 +4252,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			}
 		},
 
@@ -4298,7 +4295,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4309,9 +4306,8 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			}, {
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4322,7 +4318,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
 			}},
-				.size_of_priv     = sizeof(struct dib0700_adapter_state),
 			}
 		},
 
@@ -4349,7 +4344,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4360,8 +4355,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv = sizeof(struct
-						dib0700_adapter_state),
 			},
 		},
 
@@ -4419,15 +4412,13 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.frontend_attach  = s5h1411_frontend_attach,
 				.tuner_attach     = xc5000_tuner_attach,
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv = sizeof(struct
-						dib0700_adapter_state),
 			},
 		},
 
@@ -4457,15 +4448,13 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.frontend_attach  = lgdt3305_frontend_attach,
 				.tuner_attach     = mxl5007t_tuner_attach,
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv = sizeof(struct
-						dib0700_adapter_state),
 			},
 		},
 
@@ -4485,7 +4474,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4496,8 +4485,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4537,7 +4524,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4548,8 +4535,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4583,7 +4568,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4594,11 +4579,9 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER | DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
 				.pid_filter_count = 32,
@@ -4609,8 +4592,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4636,7 +4617,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4648,8 +4629,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4675,7 +4654,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4687,8 +4666,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4714,7 +4691,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4726,8 +4703,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4753,7 +4728,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4765,8 +4740,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4792,7 +4765,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4804,8 +4777,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4831,7 +4802,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 2,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4843,11 +4814,9 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 					DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4859,8 +4828,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4886,15 +4853,13 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-			.num_frontends = 1,
+			DIB0700_NUM_FRONTENDS(1),
 			.fe = {{
 				.frontend_attach  = pctv340e_frontend_attach,
 				.tuner_attach     = xc4000_tuner_attach,
 
 				DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 			}},
-				.size_of_priv = sizeof(struct
-						dib0700_adapter_state),
 			},
 		},
 
@@ -4923,7 +4888,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-				.num_frontends = 1,
+				DIB0700_NUM_FRONTENDS(1),
 				.fe = {{
 					.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 						DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4935,9 +4900,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 
 					DIB0700_DEFAULT_STREAMING_CONFIG(0x03),
 				} },
-
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
@@ -4963,7 +4925,7 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 		.num_adapters = 1,
 		.adapter = {
 			{
-				.num_frontends = 1,
+				DIB0700_NUM_FRONTENDS(1),
 				.fe = {{
 					.caps  = DVB_USB_ADAP_HAS_PID_FILTER |
 						DVB_USB_ADAP_PID_FILTER_CAN_BE_TURNED_OFF,
@@ -4976,9 +4938,6 @@ struct dvb_usb_device_properties dib0700_devices[] = {
 					DIB0700_DEFAULT_STREAMING_CONFIG(0x02),
 
 				} },
-
-				.size_of_priv =
-					sizeof(struct dib0700_adapter_state),
 			},
 		},
 
diff --git a/drivers/media/usb/dvb-usb/dtt200u-fe.c b/drivers/media/usb/dvb-usb/dtt200u-fe.c
index 3d81daa49172..8637ad1be6be 100644
--- a/drivers/media/usb/dvb-usb/dtt200u-fe.c
+++ b/drivers/media/usb/dvb-usb/dtt200u-fe.c
@@ -14,13 +14,14 @@
 struct dtt200u_fe_state {
 	struct dvb_usb_device *d;
 
-	fe_status_t stat;
+	enum fe_status stat;
 
 	struct dtv_frontend_properties fep;
 	struct dvb_frontend frontend;
 };
 
-static int dtt200u_fe_read_status(struct dvb_frontend* fe, fe_status_t *stat)
+static int dtt200u_fe_read_status(struct dvb_frontend *fe,
+				  enum fe_status *stat)
 {
 	struct dtt200u_fe_state *state = fe->demodulator_priv;
 	u8 st = GET_TUNE_STATUS, b[3];
@@ -105,7 +106,7 @@ static int dtt200u_fe_set_frontend(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *fep = &fe->dtv_property_cache;
 	struct dtt200u_fe_state *state = fe->demodulator_priv;
 	int i;
-	fe_status_t st;
+	enum fe_status st;
 	u16 freq = fep->frequency / 250000;
 	u8 bwbuf[2] = { SET_BANDWIDTH, 0 },freqbuf[3] = { SET_RF_FREQ, 0, 0 };
 
diff --git a/drivers/media/usb/dvb-usb/dw2102.c b/drivers/media/usb/dvb-usb/dw2102.c
index f1f357f43ff0..14ef25dc6cd3 100644
--- a/drivers/media/usb/dvb-usb/dw2102.c
+++ b/drivers/media/usb/dvb-usb/dw2102.c
@@ -117,8 +117,13 @@
 
 struct dw2102_state {
 	u8 initialized;
+	u8 last_lock;
 	struct i2c_client *i2c_client_tuner;
-	int (*old_set_voltage)(struct dvb_frontend *f, fe_sec_voltage_t v);
+
+	/* fe hook functions*/
+	int (*old_set_voltage)(struct dvb_frontend *f, enum fe_sec_voltage v);
+	int (*fe_read_status)(struct dvb_frontend *fe,
+			      enum fe_status *status);
 };
 
 /* debug */
@@ -437,7 +442,7 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 						ibuf, msg[j].len + 2,
 						DW210X_READ_MSG);
 				memcpy(msg[j].buf, ibuf + 2, msg[j].len);
-			mdelay(10);
+				mdelay(10);
 			} else if (((msg[j].buf[0] == 0xb0) &&
 						(msg[j].addr == 0x68)) ||
 						((msg[j].buf[0] == 0xf7) &&
@@ -928,8 +933,6 @@ static int su3000_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
 			break;
 		else
 			mac[i] = ibuf[0];
-
-		debug_dump(mac, 6, printk);
 	}
 
 	return 0;
@@ -946,7 +949,8 @@ static int su3000_identify_state(struct usb_device *udev,
 	return 0;
 }
 
-static int dw210x_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int dw210x_set_voltage(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage)
 {
 	static u8 command_13v[] = {0x00, 0x01};
 	static u8 command_18v[] = {0x01, 0x01};
@@ -970,7 +974,8 @@ static int dw210x_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 	return 0;
 }
 
-static int s660_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int s660_set_voltage(struct dvb_frontend *fe,
+			    enum fe_sec_voltage voltage)
 {
 	struct dvb_usb_adapter *d =
 		(struct dvb_usb_adapter *)(fe->dvb->priv);
@@ -1001,6 +1006,24 @@ static void dw210x_led_ctrl(struct dvb_frontend *fe, int offon)
 	i2c_transfer(&udev_adap->dev->i2c_adap, &msg, 1);
 }
 
+static int tt_s2_4600_read_status(struct dvb_frontend *fe,
+				  enum fe_status *status)
+{
+	struct dvb_usb_adapter *d =
+		(struct dvb_usb_adapter *)(fe->dvb->priv);
+	struct dw2102_state *st = (struct dw2102_state *)d->dev->priv;
+	int ret;
+
+	ret = st->fe_read_status(fe, status);
+
+	/* resync slave fifo when signal change from unlock to lock */
+	if ((*status & FE_HAS_LOCK) && (!st->last_lock))
+		su3000_streaming_ctrl(d, 1);
+
+	st->last_lock = (*status & FE_HAS_LOCK) ? 1 : 0;
+	return ret;
+}
+
 static struct stv0299_config sharp_z0194a_config = {
 	.demod_address = 0x68,
 	.inittab = sharp_z0194a_inittab,
@@ -1553,6 +1576,12 @@ static int tt_s2_4600_frontend_attach(struct dvb_usb_adapter *adap)
 
 	state->i2c_client_tuner = client;
 
+	/* hook fe: need to resync the slave fifo when signal locks */
+	state->fe_read_status = adap->fe_adap[0].fe->ops.read_status;
+	adap->fe_adap[0].fe->ops.read_status = tt_s2_4600_read_status;
+
+	state->last_lock = 0;
+
 	return 0;
 }
 
@@ -1657,6 +1686,8 @@ enum dw2102_table_entry {
 	GOTVIEW_SAT_HD,
 	GENIATECH_T220,
 	TECHNOTREND_S2_4600,
+	TEVII_S482_1,
+	TEVII_S482_2,
 };
 
 static struct usb_device_id dw2102_table[] = {
@@ -1682,6 +1713,8 @@ static struct usb_device_id dw2102_table[] = {
 	[GENIATECH_T220] = {USB_DEVICE(0x1f4d, 0xD220)},
 	[TECHNOTREND_S2_4600] = {USB_DEVICE(USB_VID_TECHNOTREND,
 		USB_PID_TECHNOTREND_CONNECT_S2_4600)},
+	[TEVII_S482_1] = {USB_DEVICE(0x9022, 0xd483)},
+	[TEVII_S482_2] = {USB_DEVICE(0x9022, 0xd484)},
 	{ }
 };
 
@@ -2199,12 +2232,20 @@ static struct dvb_usb_device_properties tt_s2_4600_properties = {
 		} },
 		}
 	},
-	.num_device_descs = 1,
+	.num_device_descs = 3,
 	.devices = {
 		{ "TechnoTrend TT-connect S2-4600",
 			{ &dw2102_table[TECHNOTREND_S2_4600], NULL },
 			{ NULL },
 		},
+		{ "TeVii S482 (tuner 1)",
+			{ &dw2102_table[TEVII_S482_1], NULL },
+			{ NULL },
+		},
+		{ "TeVii S482 (tuner 2)",
+			{ &dw2102_table[TEVII_S482_2], NULL },
+			{ NULL },
+		},
 	}
 };
 
diff --git a/drivers/media/usb/dvb-usb/friio-fe.c b/drivers/media/usb/dvb-usb/friio-fe.c
index d56f927fc31a..8ec92fbeabad 100644
--- a/drivers/media/usb/dvb-usb/friio-fe.c
+++ b/drivers/media/usb/dvb-usb/friio-fe.c
@@ -210,7 +210,8 @@ error:
 	return -EREMOTEIO;
 }
 
-static int jdvbt90502_read_status(struct dvb_frontend *fe, fe_status_t *state)
+static int jdvbt90502_read_status(struct dvb_frontend *fe,
+				  enum fe_status *state)
 {
 	u8 result;
 	int ret;
diff --git a/drivers/media/usb/dvb-usb/gp8psk-fe.c b/drivers/media/usb/dvb-usb/gp8psk-fe.c
index 67957dd99ede..db6eb79cde07 100644
--- a/drivers/media/usb/dvb-usb/gp8psk-fe.c
+++ b/drivers/media/usb/dvb-usb/gp8psk-fe.c
@@ -51,7 +51,8 @@ static int gp8psk_fe_update_status(struct gp8psk_fe_state *st)
 	return 0;
 }
 
-static int gp8psk_fe_read_status(struct dvb_frontend* fe, fe_status_t *status)
+static int gp8psk_fe_read_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
 {
 	struct gp8psk_fe_state *st = fe->demodulator_priv;
 	gp8psk_fe_update_status(st);
@@ -236,8 +237,8 @@ static int gp8psk_fe_send_diseqc_msg (struct dvb_frontend* fe,
 	return 0;
 }
 
-static int gp8psk_fe_send_diseqc_burst (struct dvb_frontend* fe,
-				    fe_sec_mini_cmd_t burst)
+static int gp8psk_fe_send_diseqc_burst(struct dvb_frontend *fe,
+				       enum fe_sec_mini_cmd burst)
 {
 	struct gp8psk_fe_state *st = fe->demodulator_priv;
 	u8 cmd;
@@ -254,7 +255,8 @@ static int gp8psk_fe_send_diseqc_burst (struct dvb_frontend* fe,
 	return 0;
 }
 
-static int gp8psk_fe_set_tone (struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int gp8psk_fe_set_tone(struct dvb_frontend *fe,
+			      enum fe_sec_tone_mode tone)
 {
 	struct gp8psk_fe_state* state = fe->demodulator_priv;
 
@@ -265,7 +267,8 @@ static int gp8psk_fe_set_tone (struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
 	return 0;
 }
 
-static int gp8psk_fe_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int gp8psk_fe_set_voltage(struct dvb_frontend *fe,
+				 enum fe_sec_voltage voltage)
 {
 	struct gp8psk_fe_state* state = fe->demodulator_priv;
 
diff --git a/drivers/media/usb/dvb-usb/opera1.c b/drivers/media/usb/dvb-usb/opera1.c
index 14a2119912ba..2566d2f1c2ad 100644
--- a/drivers/media/usb/dvb-usb/opera1.c
+++ b/drivers/media/usb/dvb-usb/opera1.c
@@ -167,7 +167,8 @@ static struct i2c_algorithm opera1_i2c_algo = {
 	.functionality = opera1_i2c_func,
 };
 
-static int opera1_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
+static int opera1_set_voltage(struct dvb_frontend *fe,
+			      enum fe_sec_voltage voltage)
 {
 	static u8 command_13v[1]={0x00};
 	static u8 command_18v[1]={0x01};
diff --git a/drivers/media/usb/dvb-usb/technisat-usb2.c b/drivers/media/usb/dvb-usb/technisat-usb2.c
index 5801ae7f672a..03f334d3a8f4 100644
--- a/drivers/media/usb/dvb-usb/technisat-usb2.c
+++ b/drivers/media/usb/dvb-usb/technisat-usb2.c
@@ -453,7 +453,7 @@ static struct stv090x_config technisat_usb2_stv090x_config;
 
 /* frontend attach */
 static int technisat_usb2_set_voltage(struct dvb_frontend *fe,
-		fe_sec_voltage_t voltage)
+				      enum fe_sec_voltage voltage)
 {
 	int i;
 	u8 gpio[3] = { 0 }; /* 0 = 2, 1 = 3, 2 = 4 */
diff --git a/drivers/media/usb/dvb-usb/vp702x-fe.c b/drivers/media/usb/dvb-usb/vp702x-fe.c
index 5eab468dd904..d361a72ca0fa 100644
--- a/drivers/media/usb/dvb-usb/vp702x-fe.c
+++ b/drivers/media/usb/dvb-usb/vp702x-fe.c
@@ -26,8 +26,8 @@ struct vp702x_fe_state {
 
 	struct dvb_frontend_ops ops;
 
-	fe_sec_voltage_t voltage;
-	fe_sec_tone_mode_t tone_mode;
+	enum fe_sec_voltage voltage;
+	enum fe_sec_tone_mode tone_mode;
 
 	u8 lnb_buf[8];
 
@@ -72,7 +72,8 @@ static u8 vp702x_chksum(u8 *buf,int f, int count)
 	return ~s+1;
 }
 
-static int vp702x_fe_read_status(struct dvb_frontend* fe, fe_status_t *status)
+static int vp702x_fe_read_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
 {
 	struct vp702x_fe_state *st = fe->demodulator_priv;
 	vp702x_fe_refresh_state(st);
@@ -243,13 +244,15 @@ static int vp702x_fe_send_diseqc_msg (struct dvb_frontend* fe,
 	return 0;
 }
 
-static int vp702x_fe_send_diseqc_burst (struct dvb_frontend* fe, fe_sec_mini_cmd_t burst)
+static int vp702x_fe_send_diseqc_burst(struct dvb_frontend *fe,
+				       enum fe_sec_mini_cmd burst)
 {
 	deb_fe("%s\n",__func__);
 	return 0;
 }
 
-static int vp702x_fe_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int vp702x_fe_set_tone(struct dvb_frontend *fe,
+			      enum fe_sec_tone_mode tone)
 {
 	struct vp702x_fe_state *st = fe->demodulator_priv;
 	struct vp702x_device_state *dst = st->d->priv;
@@ -282,8 +285,8 @@ static int vp702x_fe_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
 	return 0;
 }
 
-static int vp702x_fe_set_voltage (struct dvb_frontend* fe, fe_sec_voltage_t
-		voltage)
+static int vp702x_fe_set_voltage(struct dvb_frontend *fe,
+				 enum fe_sec_voltage voltage)
 {
 	struct vp702x_fe_state *st = fe->demodulator_priv;
 	struct vp702x_device_state *dst = st->d->priv;
diff --git a/drivers/media/usb/dvb-usb/vp702x.c b/drivers/media/usb/dvb-usb/vp702x.c
index 22cf9f96cb9e..ee1e19e36445 100644
--- a/drivers/media/usb/dvb-usb/vp702x.c
+++ b/drivers/media/usb/dvb-usb/vp702x.c
@@ -259,12 +259,11 @@ static struct rc_map_table rc_map_vp702x_table[] = {
 /* remote control stuff (does not work with my box) */
 static int vp702x_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 {
+/* remove the following return to enabled remote querying */
+#if 0
 	u8 *key;
 	int i;
 
-/* remove the following return to enabled remote querying */
-	return 0;
-
 	key = kmalloc(10, GFP_KERNEL);
 	if (!key)
 		return -ENOMEM;
@@ -286,6 +285,8 @@ static int vp702x_rc_query(struct dvb_usb_device *d, u32 *event, int *state)
 			break;
 		}
 	kfree(key);
+#endif
+
 	return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb/vp7045-fe.c b/drivers/media/usb/dvb-usb/vp7045-fe.c
index b8825b18c003..e708afc6a57f 100644
--- a/drivers/media/usb/dvb-usb/vp7045-fe.c
+++ b/drivers/media/usb/dvb-usb/vp7045-fe.c
@@ -26,7 +26,8 @@ struct vp7045_fe_state {
 	struct dvb_usb_device *d;
 };
 
-static int vp7045_fe_read_status(struct dvb_frontend* fe, fe_status_t *status)
+static int vp7045_fe_read_status(struct dvb_frontend *fe,
+				 enum fe_status *status)
 {
 	struct vp7045_fe_state *state = fe->demodulator_priv;
 	u8 s0 = vp7045_read_reg(state->d,0x00),
diff --git a/drivers/media/usb/em28xx/em28xx-camera.c b/drivers/media/usb/em28xx/em28xx-camera.c
index a4b22c2c3ba7..ed0b3a87983e 100644
--- a/drivers/media/usb/em28xx/em28xx-camera.c
+++ b/drivers/media/usb/em28xx/em28xx-camera.c
@@ -404,7 +404,9 @@ int em28xx_init_camera(struct em28xx *dev)
 			.addr = client->addr,
 			.platform_data = &camlink,
 		};
-		struct v4l2_mbus_framefmt fmt;
+		struct v4l2_subdev_format format = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
 
 		/*
 		 * FIXME: sensor supports resolutions up to 1600x1200, but
@@ -425,10 +427,10 @@ int em28xx_init_camera(struct em28xx *dev)
 			break;
 		}
 
-		fmt.code = MEDIA_BUS_FMT_YUYV8_2X8;
-		fmt.width = 640;
-		fmt.height = 480;
-		v4l2_subdev_call(subdev, video, s_mbus_fmt, &fmt);
+		format.format.code = MEDIA_BUS_FMT_YUYV8_2X8;
+		format.format.width = 640;
+		format.format.height = 480;
+		v4l2_subdev_call(subdev, pad, set_fmt, NULL, &format);
 
 		/* NOTE: for UXGA=1600x1200 switch to 12MHz */
 		dev->board.xclk = EM28XX_XCLK_FREQUENCY_24MHZ;
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index a5b22c5a240c..a38248360833 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -96,6 +96,7 @@ struct em28xx_dvb {
 	int			lna_gpio;
 	struct i2c_client	*i2c_client_demod;
 	struct i2c_client	*i2c_client_tuner;
+	struct i2c_client	*i2c_client_sec;
 };
 
 static inline void print_err_status(struct em28xx *dev,
@@ -807,16 +808,6 @@ static struct tda18271_config em28xx_cxd2820r_tda18271_config = {
 	.gate = TDA18271_GATE_DIGITAL,
 };
 
-static const struct tda10071_config em28xx_tda10071_config = {
-	.demod_i2c_addr = 0x55, /* (0xaa >> 1) */
-	.tuner_i2c_addr = 0x14,
-	.i2c_wr_max = 64,
-	.ts_mode = TDA10071_TS_SERIAL,
-	.spec_inv = 0,
-	.xtal = 40444000, /* 40.444 MHz */
-	.pll_multiplier = 20,
-};
-
 static const struct a8293_config em28xx_a8293_config = {
 	.i2c_addr = 0x08, /* (0x10 >> 1) */
 };
@@ -1331,16 +1322,60 @@ static int em28xx_dvb_init(struct em28xx *dev)
 				   &dev->i2c_adap[dev->def_i2c_bus],
 				   &c3tech_duo_tda18271_config);
 		break;
-	case EM28174_BOARD_PCTV_460E:
-		/* attach demod */
-		dvb->fe[0] = dvb_attach(tda10071_attach,
-			&em28xx_tda10071_config, &dev->i2c_adap[dev->def_i2c_bus]);
+	case EM28174_BOARD_PCTV_460E: {
+		struct i2c_client *client;
+		struct i2c_board_info board_info;
+		struct tda10071_platform_data tda10071_pdata = {};
+		struct a8293_platform_data a8293_pdata = {};
+
+		/* attach demod + tuner combo */
+		tda10071_pdata.clk = 40444000, /* 40.444 MHz */
+		tda10071_pdata.i2c_wr_max = 64,
+		tda10071_pdata.ts_mode = TDA10071_TS_SERIAL,
+		tda10071_pdata.pll_multiplier = 20,
+		tda10071_pdata.tuner_i2c_addr = 0x14,
+		memset(&board_info, 0, sizeof(board_info));
+		strlcpy(board_info.type, "tda10071_cx24118", I2C_NAME_SIZE);
+		board_info.addr = 0x55;
+		board_info.platform_data = &tda10071_pdata;
+		request_module("tda10071");
+		client = i2c_new_device(&dev->i2c_adap[dev->def_i2c_bus], &board_info);
+		if (client == NULL || client->dev.driver == NULL) {
+			result = -ENODEV;
+			goto out_free;
+		}
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			result = -ENODEV;
+			goto out_free;
+		}
+		dvb->fe[0] = tda10071_pdata.get_dvb_frontend(client);
+		dvb->i2c_client_demod = client;
 
 		/* attach SEC */
-		if (dvb->fe[0])
-			dvb_attach(a8293_attach, dvb->fe[0], &dev->i2c_adap[dev->def_i2c_bus],
-				   &em28xx_a8293_config);
+		a8293_pdata.dvb_frontend = dvb->fe[0];
+		memset(&board_info, 0, sizeof(board_info));
+		strlcpy(board_info.type, "a8293", I2C_NAME_SIZE);
+		board_info.addr = 0x08;
+		board_info.platform_data = &a8293_pdata;
+		request_module("a8293");
+		client = i2c_new_device(&dev->i2c_adap[dev->def_i2c_bus], &board_info);
+		if (client == NULL || client->dev.driver == NULL) {
+			module_put(dvb->i2c_client_demod->dev.driver->owner);
+			i2c_unregister_device(dvb->i2c_client_demod);
+			result = -ENODEV;
+			goto out_free;
+		}
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			module_put(dvb->i2c_client_demod->dev.driver->owner);
+			i2c_unregister_device(dvb->i2c_client_demod);
+			result = -ENODEV;
+			goto out_free;
+		}
+		dvb->i2c_client_sec = client;
 		break;
+	}
 	case EM2874_BOARD_DELOCK_61959:
 	case EM2874_BOARD_MAXMEDIA_UB425_TC:
 		/* attach demodulator */
@@ -1486,64 +1521,94 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			}
 		}
 		break;
-	case EM28178_BOARD_PCTV_461E:
-		{
-			/* demod I2C adapter */
-			struct i2c_adapter *i2c_adapter;
-			struct i2c_client *client;
-			struct i2c_board_info info;
-			struct ts2020_config ts2020_config = {
-			};
-			memset(&info, 0, sizeof(struct i2c_board_info));
-
-			/* attach demod */
-			dvb->fe[0] = dvb_attach(m88ds3103_attach,
-					&pctv_461e_m88ds3103_config,
-					&dev->i2c_adap[dev->def_i2c_bus],
-					&i2c_adapter);
-			if (dvb->fe[0] == NULL) {
-				result = -ENODEV;
-				goto out_free;
-			}
-
-			/* attach tuner */
-			ts2020_config.fe = dvb->fe[0];
-			strlcpy(info.type, "ts2022", I2C_NAME_SIZE);
-			info.addr = 0x60;
-			info.platform_data = &ts2020_config;
-			request_module("ts2020");
-			client = i2c_new_device(i2c_adapter, &info);
-			if (client == NULL || client->dev.driver == NULL) {
-				dvb_frontend_detach(dvb->fe[0]);
-				result = -ENODEV;
-				goto out_free;
-			}
-
-			if (!try_module_get(client->dev.driver->owner)) {
-				i2c_unregister_device(client);
-				dvb_frontend_detach(dvb->fe[0]);
-				result = -ENODEV;
-				goto out_free;
-			}
+	case EM28178_BOARD_PCTV_461E: {
+		struct i2c_client *client;
+		struct i2c_adapter *i2c_adapter;
+		struct i2c_board_info board_info;
+		struct m88ds3103_platform_data m88ds3103_pdata = {};
+		struct ts2020_config ts2020_config = {};
+		struct a8293_platform_data a8293_pdata = {};
 
-			/* delegate signal strength measurement to tuner */
-			dvb->fe[0]->ops.read_signal_strength =
-					dvb->fe[0]->ops.tuner_ops.get_rf_strength;
+		/* attach demod */
+		m88ds3103_pdata.clk = 27000000;
+		m88ds3103_pdata.i2c_wr_max = 33;
+		m88ds3103_pdata.ts_mode = M88DS3103_TS_PARALLEL;
+		m88ds3103_pdata.ts_clk = 16000;
+		m88ds3103_pdata.ts_clk_pol = 1;
+		m88ds3103_pdata.agc = 0x99;
+		memset(&board_info, 0, sizeof(board_info));
+		strlcpy(board_info.type, "m88ds3103", I2C_NAME_SIZE);
+		board_info.addr = 0x68;
+		board_info.platform_data = &m88ds3103_pdata;
+		request_module("m88ds3103");
+		client = i2c_new_device(&dev->i2c_adap[dev->def_i2c_bus], &board_info);
+		if (client == NULL || client->dev.driver == NULL) {
+			result = -ENODEV;
+			goto out_free;
+		}
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			result = -ENODEV;
+			goto out_free;
+		}
+		dvb->fe[0] = m88ds3103_pdata.get_dvb_frontend(client);
+		i2c_adapter = m88ds3103_pdata.get_i2c_adapter(client);
+		dvb->i2c_client_demod = client;
 
-			/* attach SEC */
-			if (!dvb_attach(a8293_attach, dvb->fe[0],
-					&dev->i2c_adap[dev->def_i2c_bus],
-					&em28xx_a8293_config)) {
-				module_put(client->dev.driver->owner);
-				i2c_unregister_device(client);
-				dvb_frontend_detach(dvb->fe[0]);
-				result = -ENODEV;
-				goto out_free;
-			}
+		/* attach tuner */
+		ts2020_config.fe = dvb->fe[0];
+		memset(&board_info, 0, sizeof(board_info));
+		strlcpy(board_info.type, "ts2022", I2C_NAME_SIZE);
+		board_info.addr = 0x60;
+		board_info.platform_data = &ts2020_config;
+		request_module("ts2020");
+		client = i2c_new_device(i2c_adapter, &board_info);
+		if (client == NULL || client->dev.driver == NULL) {
+			module_put(dvb->i2c_client_demod->dev.driver->owner);
+			i2c_unregister_device(dvb->i2c_client_demod);
+			result = -ENODEV;
+			goto out_free;
+		}
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			module_put(dvb->i2c_client_demod->dev.driver->owner);
+			i2c_unregister_device(dvb->i2c_client_demod);
+			result = -ENODEV;
+			goto out_free;
+		}
+		dvb->i2c_client_tuner = client;
+		/* delegate signal strength measurement to tuner */
+		dvb->fe[0]->ops.read_signal_strength =
+				dvb->fe[0]->ops.tuner_ops.get_rf_strength;
 
-			dvb->i2c_client_tuner = client;
+		/* attach SEC */
+		a8293_pdata.dvb_frontend = dvb->fe[0];
+		memset(&board_info, 0, sizeof(board_info));
+		strlcpy(board_info.type, "a8293", I2C_NAME_SIZE);
+		board_info.addr = 0x08;
+		board_info.platform_data = &a8293_pdata;
+		request_module("a8293");
+		client = i2c_new_device(&dev->i2c_adap[dev->def_i2c_bus], &board_info);
+		if (client == NULL || client->dev.driver == NULL) {
+			module_put(dvb->i2c_client_tuner->dev.driver->owner);
+			i2c_unregister_device(dvb->i2c_client_tuner);
+			module_put(dvb->i2c_client_demod->dev.driver->owner);
+			i2c_unregister_device(dvb->i2c_client_demod);
+			result = -ENODEV;
+			goto out_free;
 		}
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			module_put(dvb->i2c_client_tuner->dev.driver->owner);
+			i2c_unregister_device(dvb->i2c_client_tuner);
+			module_put(dvb->i2c_client_demod->dev.driver->owner);
+			i2c_unregister_device(dvb->i2c_client_demod);
+			result = -ENODEV;
+			goto out_free;
+		}
+		dvb->i2c_client_sec = client;
 		break;
+	}
 	case EM28178_BOARD_PCTV_292E:
 		{
 			struct i2c_adapter *adapter;
@@ -1579,6 +1644,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			/* attach tuner */
 			memset(&si2157_config, 0, sizeof(si2157_config));
 			si2157_config.fe = dvb->fe[0];
+			si2157_config.if_port = 1;
 			memset(&info, 0, sizeof(struct i2c_board_info));
 			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
 			info.addr = 0x60;
@@ -1639,6 +1705,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			/* attach tuner */
 			memset(&si2157_config, 0, sizeof(si2157_config));
 			si2157_config.fe = dvb->fe[0];
+			si2157_config.if_port = 0;
 			memset(&info, 0, sizeof(struct i2c_board_info));
 			strlcpy(info.type, "si2146", I2C_NAME_SIZE);
 			info.addr = 0x60;
@@ -1727,7 +1794,6 @@ static int em28xx_dvb_fini(struct em28xx *dev)
 	em28xx_info("Closing DVB extension\n");
 
 	dvb = dev->dvb;
-	client = dvb->i2c_client_tuner;
 
 	em28xx_uninit_usb_xfer(dev, EM28XX_DIGITAL_MODE);
 
@@ -1744,7 +1810,15 @@ static int em28xx_dvb_fini(struct em28xx *dev)
 		}
 	}
 
+	/* remove I2C SEC */
+	client = dvb->i2c_client_sec;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+
 	/* remove I2C tuner */
+	client = dvb->i2c_client_tuner;
 	if (client) {
 		module_put(client->dev.driver->owner);
 		i2c_unregister_device(client);
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 14eba9c65de3..4397ce5e78df 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -839,7 +839,6 @@ static int get_ressource(enum v4l2_buf_type f_type)
 		return EM28XX_RESOURCE_VBI;
 	default:
 		BUG();
-		return 0;
 	}
 }
 
diff --git a/drivers/media/usb/go7007/go7007-driver.c b/drivers/media/usb/go7007/go7007-driver.c
index 95cffb771a62..0ab81ec8897a 100644
--- a/drivers/media/usb/go7007/go7007-driver.c
+++ b/drivers/media/usb/go7007/go7007-driver.c
@@ -446,7 +446,7 @@ static void go7007_motion_regions(struct go7007 *go, struct go7007_buffer *vb)
  */
 static struct go7007_buffer *frame_boundary(struct go7007 *go, struct go7007_buffer *vb)
 {
-	u32 *bytesused = &vb->vb.v4l2_planes[0].bytesused;
+	u32 *bytesused;
 	struct go7007_buffer *vb_tmp = NULL;
 
 	if (vb == NULL) {
@@ -458,6 +458,7 @@ static struct go7007_buffer *frame_boundary(struct go7007 *go, struct go7007_buf
 		go->next_seq++;
 		return vb;
 	}
+	bytesused = &vb->vb.v4l2_planes[0].bytesused;
 
 	vb->vb.v4l2_buf.sequence = go->next_seq++;
 	if (vb->modet_active && *bytesused + 216 < GO7007_BUF_SIZE)
diff --git a/drivers/media/usb/go7007/go7007-usb.c b/drivers/media/usb/go7007/go7007-usb.c
index 3f986e1178ce..4857c467e76c 100644
--- a/drivers/media/usb/go7007/go7007-usb.c
+++ b/drivers/media/usb/go7007/go7007-usb.c
@@ -338,6 +338,7 @@ static const struct go7007_usb_board board_matrix_revolution = {
 	},
 };
 
+#if 0
 static const struct go7007_usb_board board_lifeview_lr192 = {
 	.flags		= GO7007_USB_EZUSB,
 	.main_info	= {
@@ -364,6 +365,7 @@ static const struct go7007_usb_board board_lifeview_lr192 = {
 		},
 	},
 };
+#endif
 
 static const struct go7007_usb_board board_endura = {
 	.flags		= 0,
@@ -1096,8 +1098,10 @@ static int go7007_usb_probe(struct usb_interface *intf,
 	case GO7007_BOARDID_LIFEVIEW_LR192:
 		dev_err(&intf->dev, "The Lifeview TV Walker Ultra is not supported. Sorry!\n");
 		return -ENODEV;
+#if 0
 		name = "Lifeview TV Walker Ultra";
 		board = &board_lifeview_lr192;
+#endif
 		break;
 	case GO7007_BOARDID_SENSORAY_2250:
 		dev_info(&intf->dev, "Sensoray 2250 found\n");
diff --git a/drivers/media/usb/go7007/go7007-v4l2.c b/drivers/media/usb/go7007/go7007-v4l2.c
index d6bf982efa42..c57207e268c3 100644
--- a/drivers/media/usb/go7007/go7007-v4l2.c
+++ b/drivers/media/usb/go7007/go7007-v4l2.c
@@ -250,15 +250,17 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 	go->encoder_v_offset = go->board_info->sensor_v_offset;
 
 	if (go->board_info->sensor_flags & GO7007_SENSOR_SCALING) {
-		struct v4l2_mbus_framefmt mbus_fmt;
+		struct v4l2_subdev_format format = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
 
-		mbus_fmt.code = MEDIA_BUS_FMT_FIXED;
-		mbus_fmt.width = fmt ? fmt->fmt.pix.width : width;
-		mbus_fmt.height = height;
+		format.format.code = MEDIA_BUS_FMT_FIXED;
+		format.format.width = fmt ? fmt->fmt.pix.width : width;
+		format.format.height = height;
 		go->encoder_h_halve = 0;
 		go->encoder_v_halve = 0;
 		go->encoder_subsample = 0;
-		call_all(&go->v4l2_dev, video, s_mbus_fmt, &mbus_fmt);
+		call_all(&go->v4l2_dev, pad, set_fmt, NULL, &format);
 	} else {
 		if (width <= sensor_width / 4) {
 			go->encoder_h_halve = 1;
diff --git a/drivers/media/usb/go7007/s2250-board.c b/drivers/media/usb/go7007/s2250-board.c
index bb846680bcd4..5c2a49534d2b 100644
--- a/drivers/media/usb/go7007/s2250-board.c
+++ b/drivers/media/usb/go7007/s2250-board.c
@@ -405,12 +405,20 @@ static int s2250_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static int s2250_s_mbus_fmt(struct v4l2_subdev *sd,
-			struct v4l2_mbus_framefmt *fmt)
+static int s2250_set_fmt(struct v4l2_subdev *sd,
+		struct v4l2_subdev_pad_config *cfg,
+		struct v4l2_subdev_format *format)
 {
+	struct v4l2_mbus_framefmt *fmt = &format->format;
 	struct s2250 *state = to_state(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 
+	if (format->pad)
+		return -EINVAL;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
 	if (fmt->height < 640) {
 		write_reg_fp(client, 0x12b, state->reg12b_val | 0x400);
 		write_reg_fp(client, 0x140, 0x060);
@@ -479,13 +487,17 @@ static const struct v4l2_subdev_audio_ops s2250_audio_ops = {
 static const struct v4l2_subdev_video_ops s2250_video_ops = {
 	.s_std = s2250_s_std,
 	.s_routing = s2250_s_video_routing,
-	.s_mbus_fmt = s2250_s_mbus_fmt,
+};
+
+static const struct v4l2_subdev_pad_ops s2250_pad_ops = {
+	.set_fmt = s2250_set_fmt,
 };
 
 static const struct v4l2_subdev_ops s2250_ops = {
 	.core = &s2250_core_ops,
 	.audio = &s2250_audio_ops,
 	.video = &s2250_video_ops,
+	.pad = &s2250_pad_ops,
 };
 
 /* --------------------------------------------------------------------------*/
diff --git a/drivers/media/usb/gspca/benq.c b/drivers/media/usb/gspca/benq.c
index 05f406deae13..790baed33963 100644
--- a/drivers/media/usb/gspca/benq.c
+++ b/drivers/media/usb/gspca/benq.c
@@ -236,8 +236,8 @@ static void sd_isoc_irq(struct urb *urb)
 		}
 		data = (u8 *) urb->transfer_buffer
 					+ urb->iso_frame_desc[i].offset;
-			gspca_frame_add(gspca_dev, INTER_PACKET,
-					data, SD_PKT_SZ);
+		gspca_frame_add(gspca_dev, INTER_PACKET,
+				data, SD_PKT_SZ);
 	}
 
 	/* resubmit the URBs */
diff --git a/drivers/media/usb/gspca/sn9c2028.c b/drivers/media/usb/gspca/sn9c2028.c
index 39b6b2e02963..c75b7388a85c 100644
--- a/drivers/media/usb/gspca/sn9c2028.c
+++ b/drivers/media/usb/gspca/sn9c2028.c
@@ -33,6 +33,16 @@ struct sd {
 	struct gspca_dev gspca_dev;  /* !! must be the first item */
 	u8 sof_read;
 	u16 model;
+
+#define MIN_AVG_LUM 8500
+#define MAX_AVG_LUM 10000
+	int avg_lum;
+	u8 avg_lum_l;
+
+	struct { /* autogain and gain control cluster */
+		struct v4l2_ctrl *autogain;
+		struct v4l2_ctrl *gain;
+	};
 };
 
 struct init_command {
@@ -128,7 +138,7 @@ static int sn9c2028_long_command(struct gspca_dev *gspca_dev, u8 *command)
 	status = -1;
 	for (i = 0; i < 256 && status < 2; i++)
 		status = sn9c2028_read1(gspca_dev);
-	if (status != 2) {
+	if (status < 0) {
 		pr_err("long command status read error %d\n", status);
 		return (status < 0) ? status : -EIO;
 	}
@@ -178,6 +188,9 @@ static int sd_config(struct gspca_dev *gspca_dev,
 	case 0x7005:
 		PDEBUG(D_PROBE, "Genius Smart 300 camera");
 		break;
+	case 0x7003:
+		PDEBUG(D_PROBE, "Genius Videocam Live v2");
+		break;
 	case 0x8000:
 		PDEBUG(D_PROBE, "DC31VC");
 		break;
@@ -248,6 +261,78 @@ static int run_start_commands(struct gspca_dev *gspca_dev,
 	return 0;
 }
 
+static void set_gain(struct gspca_dev *gspca_dev, s32 g)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	struct init_command genius_vcam_live_gain_cmds[] = {
+		{{0x1d, 0x25, 0x10 /* This byte is gain */,
+		  0x20, 0xab, 0x00}, 0},
+	};
+	if (!gspca_dev->streaming)
+		return;
+
+	switch (sd->model) {
+	case 0x7003:
+		genius_vcam_live_gain_cmds[0].instruction[2] = g;
+		run_start_commands(gspca_dev, genius_vcam_live_gain_cmds,
+				   ARRAY_SIZE(genius_vcam_live_gain_cmds));
+		break;
+	default:
+		break;
+	}
+}
+
+static int sd_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gspca_dev *gspca_dev =
+		container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->usb_err = 0;
+
+	if (!gspca_dev->streaming)
+		return 0;
+
+	switch (ctrl->id) {
+	/* standalone gain control */
+	case V4L2_CID_GAIN:
+		set_gain(gspca_dev, ctrl->val);
+		break;
+	/* autogain */
+	case V4L2_CID_AUTOGAIN:
+		set_gain(gspca_dev, sd->gain->val);
+		break;
+	}
+	return gspca_dev->usb_err;
+}
+
+static const struct v4l2_ctrl_ops sd_ctrl_ops = {
+	.s_ctrl = sd_s_ctrl,
+};
+
+
+static int sd_init_controls(struct gspca_dev *gspca_dev)
+{
+	struct v4l2_ctrl_handler *hdl = &gspca_dev->ctrl_handler;
+	struct sd *sd = (struct sd *)gspca_dev;
+
+	gspca_dev->vdev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_init(hdl, 2);
+
+	switch (sd->model) {
+	case 0x7003:
+		sd->gain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_GAIN, 0, 20, 1, 0);
+		sd->autogain = v4l2_ctrl_new_std(hdl, &sd_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
 static int start_spy_cam(struct gspca_dev *gspca_dev)
 {
 	struct init_command spy_start_commands[] = {
@@ -530,6 +615,119 @@ static int start_genius_cam(struct gspca_dev *gspca_dev)
 				  ARRAY_SIZE(genius_start_commands));
 }
 
+static int start_genius_videocam_live(struct gspca_dev *gspca_dev)
+{
+	int r;
+	struct sd *sd = (struct sd *) gspca_dev;
+	struct init_command genius_vcam_live_start_commands[] = {
+		{{0x0c, 0x01, 0x00, 0x00, 0x00, 0x00}, 0},
+		{{0x16, 0x01, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x10, 0x00, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x25, 0x01, 0x16, 0x00, 0x00}, 4},
+		{{0x13, 0x26, 0x01, 0x12, 0x00, 0x00}, 4},
+
+		{{0x13, 0x28, 0x01, 0x0e, 0x00, 0x00}, 4},
+		{{0x13, 0x27, 0x01, 0x20, 0x00, 0x00}, 4},
+		{{0x13, 0x29, 0x01, 0x22, 0x00, 0x00}, 4},
+		{{0x13, 0x2c, 0x01, 0x02, 0x00, 0x00}, 4},
+		{{0x13, 0x2d, 0x01, 0x02, 0x00, 0x00}, 4},
+		{{0x13, 0x2e, 0x01, 0x09, 0x00, 0x00}, 4},
+		{{0x13, 0x2f, 0x01, 0x07, 0x00, 0x00}, 4},
+		{{0x11, 0x20, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x21, 0x2d, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x22, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x23, 0x03, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x10, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x11, 0x64, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x12, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x13, 0x91, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x14, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x15, 0x20, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x16, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x17, 0x60, 0x00, 0x00, 0x00}, 4},
+		{{0x1c, 0x20, 0x00, 0x2d, 0x00, 0x00}, 4},
+		{{0x13, 0x20, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x21, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x22, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x23, 0x01, 0x01, 0x00, 0x00}, 4},
+		{{0x13, 0x24, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x25, 0x01, 0x16, 0x00, 0x00}, 4},
+		{{0x13, 0x26, 0x01, 0x12, 0x00, 0x00}, 4},
+		{{0x13, 0x27, 0x01, 0x20, 0x00, 0x00}, 4},
+		{{0x13, 0x28, 0x01, 0x0e, 0x00, 0x00}, 4},
+		{{0x13, 0x29, 0x01, 0x22, 0x00, 0x00}, 4},
+		{{0x13, 0x2a, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x2b, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x2c, 0x01, 0x02, 0x00, 0x00}, 4},
+		{{0x13, 0x2d, 0x01, 0x02, 0x00, 0x00}, 4},
+		{{0x13, 0x2e, 0x01, 0x09, 0x00, 0x00}, 4},
+		{{0x13, 0x2f, 0x01, 0x07, 0x00, 0x00}, 4},
+		{{0x12, 0x34, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x34, 0x01, 0xa1, 0x00, 0x00}, 4},
+		{{0x13, 0x35, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x01, 0x04, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x02, 0x92, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x10, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x11, 0x64, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x12, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x13, 0x91, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x14, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x15, 0x20, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x16, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x17, 0x60, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x20, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x21, 0x2d, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x22, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x23, 0x03, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x25, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x26, 0x02, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x27, 0x88, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x30, 0x38, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x31, 0x2a, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x32, 0x2a, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x33, 0x2a, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x34, 0x02, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x5b, 0x0a, 0x00, 0x00, 0x00}, 4},
+		{{0x13, 0x25, 0x01, 0x28, 0x00, 0x00}, 4},
+		{{0x13, 0x26, 0x01, 0x1e, 0x00, 0x00}, 4},
+		{{0x13, 0x28, 0x01, 0x0e, 0x00, 0x00}, 4},
+		{{0x13, 0x27, 0x01, 0x20, 0x00, 0x00}, 4},
+		{{0x13, 0x29, 0x01, 0x62, 0x00, 0x00}, 4},
+		{{0x13, 0x2c, 0x01, 0x02, 0x00, 0x00}, 4},
+		{{0x13, 0x2d, 0x01, 0x03, 0x00, 0x00}, 4},
+		{{0x13, 0x2e, 0x01, 0x0f, 0x00, 0x00}, 4},
+		{{0x13, 0x2f, 0x01, 0x0c, 0x00, 0x00}, 4},
+		{{0x11, 0x20, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x21, 0x2a, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x22, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x23, 0x28, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x10, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x11, 0x04, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x12, 0x00, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x13, 0x03, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x14, 0x01, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x15, 0xe0, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x16, 0x02, 0x00, 0x00, 0x00}, 4},
+		{{0x11, 0x17, 0x80, 0x00, 0x00, 0x00}, 4},
+		{{0x1c, 0x20, 0x00, 0x2a, 0x00, 0x00}, 1},
+		{{0x20, 0x34, 0xa1, 0x00, 0x00, 0x00}, 0},
+		/* Camera should start to capture now. */
+		{{0x12, 0x27, 0x01, 0x00, 0x00, 0x00}, 0},
+		{{0x1b, 0x32, 0x26, 0x00, 0x00, 0x00}, 0},
+		{{0x1d, 0x25, 0x10, 0x20, 0xab, 0x00}, 0},
+	};
+
+	r = run_start_commands(gspca_dev, genius_vcam_live_start_commands,
+				  ARRAY_SIZE(genius_vcam_live_start_commands));
+	if (r < 0)
+		return r;
+
+	if (sd->gain)
+		set_gain(gspca_dev, v4l2_ctrl_g_ctrl(sd->gain));
+
+	return r;
+}
+
 static int start_vivitar_cam(struct gspca_dev *gspca_dev)
 {
 	struct init_command vivitar_start_commands[] = {
@@ -623,6 +821,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
 	case 0x7005:
 		err_code = start_genius_cam(gspca_dev);
 		break;
+	case 0x7003:
+		err_code = start_genius_videocam_live(gspca_dev);
+		break;
 	case 0x8001:
 		err_code = start_spy_cam(gspca_dev);
 		break;
@@ -640,6 +841,8 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		return -ENXIO;
 	}
 
+	sd->avg_lum = -1;
+
 	return err_code;
 }
 
@@ -659,6 +862,39 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
 		PERR("Camera Stop command failed");
 }
 
+static void do_autogain(struct gspca_dev *gspca_dev, int avg_lum)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+	s32 cur_gain = v4l2_ctrl_g_ctrl(sd->gain);
+
+	if (avg_lum == -1)
+		return;
+
+	if (avg_lum < MIN_AVG_LUM) {
+		if (cur_gain == sd->gain->maximum)
+			return;
+		cur_gain++;
+		v4l2_ctrl_s_ctrl(sd->gain, cur_gain);
+	}
+	if (avg_lum > MAX_AVG_LUM) {
+		if (cur_gain == sd->gain->minimum)
+			return;
+		cur_gain--;
+		v4l2_ctrl_s_ctrl(sd->gain, cur_gain);
+	}
+
+}
+
+static void sd_dqcallback(struct gspca_dev *gspca_dev)
+{
+	struct sd *sd = (struct sd *) gspca_dev;
+
+	if (sd->autogain == NULL || !v4l2_ctrl_g_ctrl(sd->autogain))
+		return;
+
+	do_autogain(gspca_dev, sd->avg_lum);
+}
+
 /* Include sn9c2028 sof detection functions */
 #include "sn9c2028.h"
 
@@ -693,14 +929,17 @@ static const struct sd_desc sd_desc = {
 	.name = MODULE_NAME,
 	.config = sd_config,
 	.init = sd_init,
+	.init_controls = sd_init_controls,
 	.start = sd_start,
 	.stopN = sd_stopN,
+	.dq_callback = sd_dqcallback,
 	.pkt_scan = sd_pkt_scan,
 };
 
 /* -- module initialisation -- */
 static const struct usb_device_id device_table[] = {
 	{USB_DEVICE(0x0458, 0x7005)}, /* Genius Smart 300, version 2 */
+	{USB_DEVICE(0x0458, 0x7003)}, /* Genius Videocam Live v2  */
 	/* The Genius Smart is untested. I can't find an owner ! */
 	/* {USB_DEVICE(0x0c45, 0x8000)}, DC31VC, Don't know this camera */
 	{USB_DEVICE(0x0c45, 0x8001)}, /* Wild Planet digital spy cam */
diff --git a/drivers/media/usb/gspca/sn9c2028.h b/drivers/media/usb/gspca/sn9c2028.h
index 8fd1d3e05665..f85bc106bc52 100644
--- a/drivers/media/usb/gspca/sn9c2028.h
+++ b/drivers/media/usb/gspca/sn9c2028.h
@@ -21,8 +21,15 @@
  *
  */
 
-static const unsigned char sn9c2028_sof_marker[5] =
-	{ 0xff, 0xff, 0x00, 0xc4, 0xc4 };
+static const unsigned char sn9c2028_sof_marker[] = {
+	0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96,
+	0x00,
+	0x00, /* seq */
+	0x00,
+	0x00,
+	0x00, /* avg luminance lower 8 bit */
+	0x00, /* avg luminance higher 8 bit */
+};
 
 static unsigned char *sn9c2028_find_sof(struct gspca_dev *gspca_dev,
 					unsigned char *m, int len)
@@ -32,8 +39,13 @@ static unsigned char *sn9c2028_find_sof(struct gspca_dev *gspca_dev,
 
 	/* Search for the SOF marker (fixed part) in the header */
 	for (i = 0; i < len; i++) {
-		if (m[i] == sn9c2028_sof_marker[sd->sof_read]) {
+		if ((m[i] == sn9c2028_sof_marker[sd->sof_read]) ||
+		    (sd->sof_read > 5)) {
 			sd->sof_read++;
+			if (sd->sof_read == 11)
+				sd->avg_lum_l = m[i];
+			if (sd->sof_read == 12)
+				sd->avg_lum = (m[i] << 8) + sd->avg_lum_l;
 			if (sd->sof_read == sizeof(sn9c2028_sof_marker)) {
 				PDEBUG(D_FRAM,
 					"SOF found, bytes to analyze: %u."
diff --git a/drivers/media/usb/gspca/sonixj.c b/drivers/media/usb/gspca/sonixj.c
index c69b45d7cfbf..fd1c8706d86a 100644
--- a/drivers/media/usb/gspca/sonixj.c
+++ b/drivers/media/usb/gspca/sonixj.c
@@ -1789,7 +1789,7 @@ static u32 expo_adjust(struct gspca_dev *gspca_dev,
 
 		if (expo > 0x03ff)
 			expo = 0x03ff;
-		 if (expo < 0x0001)
+		if (expo < 0x0001)
 			expo = 0x0001;
 		gainOm[3] = expo >> 2;
 		i2c_w8(gspca_dev, gainOm);
diff --git a/drivers/media/usb/gspca/stk014.c b/drivers/media/usb/gspca/stk014.c
index b0c70fea760b..d324d001e114 100644
--- a/drivers/media/usb/gspca/stk014.c
+++ b/drivers/media/usb/gspca/stk014.c
@@ -276,7 +276,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
 		gspca_dev->usb_err = ret;
 		goto out;
 	}
-	 reg_r(gspca_dev, 0x0630);
+	reg_r(gspca_dev, 0x0630);
 	rcv_val(gspca_dev, 0x000020);	/* << (value ff ff ff ff) */
 	reg_r(gspca_dev, 0x0650);
 	snd_val(gspca_dev, 0x000020, 0xffffffff);
diff --git a/drivers/media/usb/gspca/xirlink_cit.c b/drivers/media/usb/gspca/xirlink_cit.c
index a41aa7817c54..d5ed9d36ce25 100644
--- a/drivers/media/usb/gspca/xirlink_cit.c
+++ b/drivers/media/usb/gspca/xirlink_cit.c
@@ -1772,7 +1772,8 @@ static int cit_start_model2(struct gspca_dev *gspca_dev)
 		cit_write_reg(gspca_dev, 0x0070, 0x0119);	/* All except 176x144 */
 		sd->sof_len = 2;
 		break;
-	/* case VIDEOSIZE_352x240: */
+#if 0
+	case VIDEOSIZE_352x240:
 		cit_write_reg(gspca_dev, 0x002c, 0x0103);	/* All except 320x240 */
 		cit_write_reg(gspca_dev, 0x0000, 0x0104);	/* Same */
 		cit_write_reg(gspca_dev, 0x001e, 0x0105);	/* 320x240, 352x240 */
@@ -1780,6 +1781,7 @@ static int cit_start_model2(struct gspca_dev *gspca_dev)
 		cit_write_reg(gspca_dev, 0x0070, 0x0119);	/* All except 176x144 */
 		sd->sof_len = 2;
 		break;
+#endif
 	case 352: /* 352x288 */
 		cit_write_reg(gspca_dev, 0x002c, 0x0103);	/* All except 320x240 */
 		cit_write_reg(gspca_dev, 0x0000, 0x0104);	/* Same */
@@ -1853,13 +1855,15 @@ static int cit_start_model2(struct gspca_dev *gspca_dev)
 		cit_model2_Packet1(gspca_dev, 0x0018, 0x0044); /* Another hardware setting */
 		clock_div = 8;
 		break;
-	/* case VIDEOSIZE_352x240: */
+#if 0
+	case VIDEOSIZE_352x240:
 		/* This mode doesn't work as Windows programs it; changed to work */
 		cit_model2_Packet1(gspca_dev, 0x0014, 0x0009); /* Windows sets this to 8 */
 		cit_model2_Packet1(gspca_dev, 0x0016, 0x0003); /* Horizontal shift */
 		cit_model2_Packet1(gspca_dev, 0x0018, 0x0044); /* Windows sets this to 0x0045 */
 		clock_div = 10;
 		break;
+#endif
 	case 352: /* 352x288 */
 		cit_model2_Packet1(gspca_dev, 0x0014, 0x0003);
 		cit_model2_Packet1(gspca_dev, 0x0016, 0x0002); /* Horizontal shift */
@@ -1906,9 +1910,11 @@ static int cit_start_model2(struct gspca_dev *gspca_dev)
 	case 320: /* 320x240 */
 		cit_model2_Packet1(gspca_dev, 0x0026, 0x0044);
 		break;
-	/* case VIDEOSIZE_352x240: */
+#if 0
+	case VIDEOSIZE_352x240:
 		cit_model2_Packet1(gspca_dev, 0x0026, 0x0046);
 		break;
+#endif
 	case 352: /* 352x288 */
 		cit_model2_Packet1(gspca_dev, 0x0026, 0x0048);
 		break;
diff --git a/drivers/media/usb/gspca/zc3xx.c b/drivers/media/usb/gspca/zc3xx.c
index d3e1b6d8bf49..c5d8ee6fa3c7 100644
--- a/drivers/media/usb/gspca/zc3xx.c
+++ b/drivers/media/usb/gspca/zc3xx.c
@@ -5942,23 +5942,23 @@ static void transfer_update(struct work_struct *work)
 	reg07 = 0;
 
 	good = 0;
-	for (;;) {
+	while (1) {
 		msleep(100);
 
 		/* To protect gspca_dev->usb_buf and gspca_dev->usb_err */
 		mutex_lock(&gspca_dev->usb_lock);
 #ifdef CONFIG_PM
 		if (gspca_dev->frozen)
-			goto err;
+			break;
 #endif
 		if (!gspca_dev->present || !gspca_dev->streaming)
-			goto err;
+			break;
 
 		/* Bit 0 of register 11 indicates FIFO overflow */
 		gspca_dev->usb_err = 0;
 		reg11 = reg_r(gspca_dev, 0x0011);
 		if (gspca_dev->usb_err)
-			goto err;
+			break;
 
 		change = reg11 & 0x01;
 		if (change) {				/* overflow */
@@ -5987,12 +5987,12 @@ static void transfer_update(struct work_struct *work)
 			gspca_dev->usb_err = 0;
 			reg_w(gspca_dev, reg07, 0x0007);
 			if (gspca_dev->usb_err)
-				goto err;
+				break;
 		}
 		mutex_unlock(&gspca_dev->usb_lock);
 	}
-	return;
-err:
+
+	/* Something went wrong. Unlock and return */
 	mutex_unlock(&gspca_dev->usb_lock);
 }
 
@@ -6360,7 +6360,7 @@ static int zcxx_s_ctrl(struct v4l2_ctrl *ctrl)
 			if (ctrl->val <= jpeg_qual[i])
 				break;
 		}
-		if (i > 0 && i == qual && ctrl->val < jpeg_qual[i])
+		if (i == ARRAY_SIZE(jpeg_qual) || (i > 0 && i == qual && ctrl->val < jpeg_qual[i]))
 			i--;
 
 		/* With high quality settings we need max bandwidth */
diff --git a/drivers/media/usb/msi2500/msi2500.c b/drivers/media/usb/msi2500/msi2500.c
index efc761c78f72..3f276d921cca 100644
--- a/drivers/media/usb/msi2500/msi2500.c
+++ b/drivers/media/usb/msi2500/msi2500.c
@@ -1,4 +1,5 @@
 /*
+ * Mirics MSi2500 driver
  * Mirics MSi3101 SDR Dongle driver
  *
  * Copyright (C) 2013 Antti Palosaari <crope@iki.fi>
@@ -13,10 +14,6 @@
  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *    GNU General Public License for more details.
  *
- *    You 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.
- *
  * That driver is somehow based of pwc driver:
  *  (C) 1999-2004 Nemosoft Unv.
  *  (C) 2004-2006 Luc Saillard (luc@saillard.org)
@@ -119,7 +116,7 @@ struct msi2500_frame_buf {
 	struct list_head list;
 };
 
-struct msi2500_state {
+struct msi2500_dev {
 	struct device *dev;
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
@@ -158,19 +155,19 @@ struct msi2500_state {
 
 /* Private functions */
 static struct msi2500_frame_buf *msi2500_get_next_fill_buf(
-		struct msi2500_state *s)
+							struct msi2500_dev *dev)
 {
 	unsigned long flags;
 	struct msi2500_frame_buf *buf = NULL;
 
-	spin_lock_irqsave(&s->queued_bufs_lock, flags);
-	if (list_empty(&s->queued_bufs))
+	spin_lock_irqsave(&dev->queued_bufs_lock, flags);
+	if (list_empty(&dev->queued_bufs))
 		goto leave;
 
-	buf = list_entry(s->queued_bufs.next, struct msi2500_frame_buf, list);
+	buf = list_entry(dev->queued_bufs.next, struct msi2500_frame_buf, list);
 	list_del(&buf->list);
 leave:
-	spin_unlock_irqrestore(&s->queued_bufs_lock, flags);
+	spin_unlock_irqrestore(&dev->queued_bufs_lock, flags);
 	return buf;
 }
 
@@ -256,8 +253,8 @@ leave:
  * signed 14-bit sample
  */
 
-static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
-		unsigned int src_len)
+static int msi2500_convert_stream(struct msi2500_dev *dev, u8 *dst, u8 *src,
+				  unsigned int src_len)
 {
 	unsigned int i, j, transactions, dst_len = 0;
 	u32 sample[3];
@@ -268,26 +265,27 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
 	for (i = 0; i < transactions; i++) {
 		sample[i] = src[3] << 24 | src[2] << 16 | src[1] << 8 |
 				src[0] << 0;
-		if (i == 0 && s->next_sample != sample[0]) {
-			dev_dbg_ratelimited(s->dev,
-					"%d samples lost, %d %08x:%08x\n",
-					sample[0] - s->next_sample,
-					src_len, s->next_sample, sample[0]);
+		if (i == 0 && dev->next_sample != sample[0]) {
+			dev_dbg_ratelimited(dev->dev,
+					    "%d samples lost, %d %08x:%08x\n",
+					    sample[0] - dev->next_sample,
+					    src_len, dev->next_sample,
+					    sample[0]);
 		}
 
 		/*
 		 * Dump all unknown 'garbage' data - maybe we will discover
 		 * someday if there is something rational...
 		 */
-		dev_dbg_ratelimited(s->dev, "%*ph\n", 12, &src[4]);
+		dev_dbg_ratelimited(dev->dev, "%*ph\n", 12, &src[4]);
 
 		src += 16; /* skip header */
 
-		switch (s->pixelformat) {
+		switch (dev->pixelformat) {
 		case V4L2_SDR_FMT_CU8: /* 504 x IQ samples */
 		{
-			s8 *s8src = (s8 *) src;
-			u8 *u8dst = (u8 *) dst;
+			s8 *s8src = (s8 *)src;
+			u8 *u8dst = (u8 *)dst;
 
 			for (j = 0; j < 1008; j++)
 				*u8dst++ = *s8src++ + 128;
@@ -295,13 +293,13 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
 			src += 1008;
 			dst += 1008;
 			dst_len += 1008;
-			s->next_sample = sample[i] + 504;
+			dev->next_sample = sample[i] + 504;
 			break;
 		}
 		case  V4L2_SDR_FMT_CU16LE: /* 252 x IQ samples */
 		{
-			s16 *s16src = (s16 *) src;
-			u16 *u16dst = (u16 *) dst;
+			s16 *s16src = (s16 *)src;
+			u16 *u16dst = (u16 *)dst;
 			struct {signed int x:14; } se; /* sign extension */
 			unsigned int utmp;
 
@@ -317,38 +315,38 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
 			src += 1008;
 			dst += 1008;
 			dst_len += 1008;
-			s->next_sample = sample[i] + 252;
+			dev->next_sample = sample[i] + 252;
 			break;
 		}
 		case MSI2500_PIX_FMT_SDR_MSI2500_384: /* 384 x IQ samples */
 			/* Dump unknown 'garbage' data */
-			dev_dbg_ratelimited(s->dev, "%*ph\n", 24, &src[1000]);
+			dev_dbg_ratelimited(dev->dev, "%*ph\n", 24, &src[1000]);
 			memcpy(dst, src, 984);
 			src += 984 + 24;
 			dst += 984;
 			dst_len += 984;
-			s->next_sample = sample[i] + 384;
+			dev->next_sample = sample[i] + 384;
 			break;
 		case V4L2_SDR_FMT_CS8:         /* 504 x IQ samples */
 			memcpy(dst, src, 1008);
 			src += 1008;
 			dst += 1008;
 			dst_len += 1008;
-			s->next_sample = sample[i] + 504;
+			dev->next_sample = sample[i] + 504;
 			break;
 		case MSI2500_PIX_FMT_SDR_S12:  /* 336 x IQ samples */
 			memcpy(dst, src, 1008);
 			src += 1008;
 			dst += 1008;
 			dst_len += 1008;
-			s->next_sample = sample[i] + 336;
+			dev->next_sample = sample[i] + 336;
 			break;
 		case V4L2_SDR_FMT_CS14LE:      /* 252 x IQ samples */
 			memcpy(dst, src, 1008);
 			src += 1008;
 			dst += 1008;
 			dst_len += 1008;
-			s->next_sample = sample[i] + 252;
+			dev->next_sample = sample[i] + 252;
 			break;
 		default:
 			break;
@@ -356,17 +354,17 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
 	}
 
 	/* calculate sample rate and output it in 10 seconds intervals */
-	if (unlikely(time_is_before_jiffies(s->jiffies_next))) {
+	if (unlikely(time_is_before_jiffies(dev->jiffies_next))) {
 		#define MSECS 10000UL
 		unsigned int msecs = jiffies_to_msecs(jiffies -
-				s->jiffies_next + msecs_to_jiffies(MSECS));
-		unsigned int samples = s->next_sample - s->sample;
-
-		s->jiffies_next = jiffies + msecs_to_jiffies(MSECS);
-		s->sample = s->next_sample;
-		dev_dbg(s->dev, "size=%u samples=%u msecs=%u sample rate=%lu\n",
-				src_len, samples, msecs,
-				samples * 1000UL / msecs);
+				dev->jiffies_next + msecs_to_jiffies(MSECS));
+		unsigned int samples = dev->next_sample - dev->sample;
+
+		dev->jiffies_next = jiffies + msecs_to_jiffies(MSECS);
+		dev->sample = dev->next_sample;
+		dev_dbg(dev->dev, "size=%u samples=%u msecs=%u sample rate=%lu\n",
+			src_len, samples, msecs,
+			samples * 1000UL / msecs);
 	}
 
 	return dst_len;
@@ -378,27 +376,28 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
  */
 static void msi2500_isoc_handler(struct urb *urb)
 {
-	struct msi2500_state *s = (struct msi2500_state *)urb->context;
+	struct msi2500_dev *dev = (struct msi2500_dev *)urb->context;
 	int i, flen, fstatus;
 	unsigned char *iso_buf = NULL;
 	struct msi2500_frame_buf *fbuf;
 
-	if (unlikely(urb->status == -ENOENT || urb->status == -ECONNRESET ||
-			urb->status == -ESHUTDOWN)) {
-		dev_dbg(s->dev, "URB (%p) unlinked %ssynchronuously\n",
-				urb, urb->status == -ENOENT ? "" : "a");
+	if (unlikely(urb->status == -ENOENT ||
+		     urb->status == -ECONNRESET ||
+		     urb->status == -ESHUTDOWN)) {
+		dev_dbg(dev->dev, "URB (%p) unlinked %ssynchronuously\n",
+			urb, urb->status == -ENOENT ? "" : "a");
 		return;
 	}
 
 	if (unlikely(urb->status != 0)) {
-		dev_dbg(s->dev, "called with status %d\n", urb->status);
+		dev_dbg(dev->dev, "called with status %d\n", urb->status);
 		/* Give up after a number of contiguous errors */
-		if (++s->isoc_errors > MAX_ISOC_ERRORS)
-			dev_dbg(s->dev, "Too many ISOC errors, bailing out\n");
+		if (++dev->isoc_errors > MAX_ISOC_ERRORS)
+			dev_dbg(dev->dev, "Too many ISOC errors, bailing out\n");
 		goto handler_end;
 	} else {
 		/* Reset ISOC error counter. We did get here, after all. */
-		s->isoc_errors = 0;
+		dev->isoc_errors = 0;
 	}
 
 	/* Compact data */
@@ -408,9 +407,9 @@ static void msi2500_isoc_handler(struct urb *urb)
 		/* Check frame error */
 		fstatus = urb->iso_frame_desc[i].status;
 		if (unlikely(fstatus)) {
-			dev_dbg_ratelimited(s->dev,
-					"frame=%d/%d has error %d skipping\n",
-					i, urb->number_of_packets, fstatus);
+			dev_dbg_ratelimited(dev->dev,
+					    "frame=%d/%d has error %d skipping\n",
+					    i, urb->number_of_packets, fstatus);
 			continue;
 		}
 
@@ -422,18 +421,18 @@ static void msi2500_isoc_handler(struct urb *urb)
 		iso_buf = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
 
 		/* Get free framebuffer */
-		fbuf = msi2500_get_next_fill_buf(s);
+		fbuf = msi2500_get_next_fill_buf(dev);
 		if (unlikely(fbuf == NULL)) {
-			s->vb_full++;
-			dev_dbg_ratelimited(s->dev,
-					"videobuf is full, %d packets dropped\n",
-					s->vb_full);
+			dev->vb_full++;
+			dev_dbg_ratelimited(dev->dev,
+					    "videobuf is full, %d packets dropped\n",
+					    dev->vb_full);
 			continue;
 		}
 
 		/* fill framebuffer */
 		ptr = vb2_plane_vaddr(&fbuf->vb, 0);
-		flen = msi2500_convert_stream(s, ptr, iso_buf, flen);
+		flen = msi2500_convert_stream(dev, ptr, iso_buf, flen);
 		vb2_set_plane_payload(&fbuf->vb, 0, flen);
 		vb2_buffer_done(&fbuf->vb, VB2_BUF_STATE_DONE);
 	}
@@ -441,66 +440,66 @@ static void msi2500_isoc_handler(struct urb *urb)
 handler_end:
 	i = usb_submit_urb(urb, GFP_ATOMIC);
 	if (unlikely(i != 0))
-		dev_dbg(s->dev, "Error (%d) re-submitting urb\n", i);
+		dev_dbg(dev->dev, "Error (%d) re-submitting urb\n", i);
 }
 
-static void msi2500_iso_stop(struct msi2500_state *s)
+static void msi2500_iso_stop(struct msi2500_dev *dev)
 {
 	int i;
 
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
 	/* Unlinking ISOC buffers one by one */
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
-		if (s->urbs[i]) {
-			dev_dbg(s->dev, "Unlinking URB %p\n", s->urbs[i]);
-			usb_kill_urb(s->urbs[i]);
+		if (dev->urbs[i]) {
+			dev_dbg(dev->dev, "Unlinking URB %p\n", dev->urbs[i]);
+			usb_kill_urb(dev->urbs[i]);
 		}
 	}
 }
 
-static void msi2500_iso_free(struct msi2500_state *s)
+static void msi2500_iso_free(struct msi2500_dev *dev)
 {
 	int i;
 
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
 	/* Freeing ISOC buffers one by one */
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
-		if (s->urbs[i]) {
-			dev_dbg(s->dev, "Freeing URB\n");
-			if (s->urbs[i]->transfer_buffer) {
-				usb_free_coherent(s->udev,
-					s->urbs[i]->transfer_buffer_length,
-					s->urbs[i]->transfer_buffer,
-					s->urbs[i]->transfer_dma);
+		if (dev->urbs[i]) {
+			dev_dbg(dev->dev, "Freeing URB\n");
+			if (dev->urbs[i]->transfer_buffer) {
+				usb_free_coherent(dev->udev,
+					dev->urbs[i]->transfer_buffer_length,
+					dev->urbs[i]->transfer_buffer,
+					dev->urbs[i]->transfer_dma);
 			}
-			usb_free_urb(s->urbs[i]);
-			s->urbs[i] = NULL;
+			usb_free_urb(dev->urbs[i]);
+			dev->urbs[i] = NULL;
 		}
 	}
 }
 
 /* Both v4l2_lock and vb_queue_lock should be locked when calling this */
-static void msi2500_isoc_cleanup(struct msi2500_state *s)
+static void msi2500_isoc_cleanup(struct msi2500_dev *dev)
 {
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
-	msi2500_iso_stop(s);
-	msi2500_iso_free(s);
+	msi2500_iso_stop(dev);
+	msi2500_iso_free(dev);
 }
 
 /* Both v4l2_lock and vb_queue_lock should be locked when calling this */
-static int msi2500_isoc_init(struct msi2500_state *s)
+static int msi2500_isoc_init(struct msi2500_dev *dev)
 {
 	struct urb *urb;
 	int i, j, ret;
 
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
-	s->isoc_errors = 0;
+	dev->isoc_errors = 0;
 
-	ret = usb_set_interface(s->udev, 0, 1);
+	ret = usb_set_interface(dev->udev, 0, 1);
 	if (ret)
 		return ret;
 
@@ -508,29 +507,29 @@ static int msi2500_isoc_init(struct msi2500_state *s)
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
 		urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
 		if (urb == NULL) {
-			dev_err(s->dev, "Failed to allocate urb %d\n", i);
-			msi2500_isoc_cleanup(s);
+			dev_err(dev->dev, "Failed to allocate urb %d\n", i);
+			msi2500_isoc_cleanup(dev);
 			return -ENOMEM;
 		}
-		s->urbs[i] = urb;
-		dev_dbg(s->dev, "Allocated URB at 0x%p\n", urb);
+		dev->urbs[i] = urb;
+		dev_dbg(dev->dev, "Allocated URB at 0x%p\n", urb);
 
 		urb->interval = 1;
-		urb->dev = s->udev;
-		urb->pipe = usb_rcvisocpipe(s->udev, 0x81);
+		urb->dev = dev->udev;
+		urb->pipe = usb_rcvisocpipe(dev->udev, 0x81);
 		urb->transfer_flags = URB_ISO_ASAP | URB_NO_TRANSFER_DMA_MAP;
-		urb->transfer_buffer = usb_alloc_coherent(s->udev,
+		urb->transfer_buffer = usb_alloc_coherent(dev->udev,
 				ISO_BUFFER_SIZE,
 				GFP_KERNEL, &urb->transfer_dma);
 		if (urb->transfer_buffer == NULL) {
-			dev_err(s->dev, "Failed to allocate urb buffer %d\n",
-					i);
-			msi2500_isoc_cleanup(s);
+			dev_err(dev->dev,
+				"Failed to allocate urb buffer %d\n", i);
+			msi2500_isoc_cleanup(dev);
 			return -ENOMEM;
 		}
 		urb->transfer_buffer_length = ISO_BUFFER_SIZE;
 		urb->complete = msi2500_isoc_handler;
-		urb->context = s;
+		urb->context = dev;
 		urb->start_frame = 0;
 		urb->number_of_packets = ISO_FRAMES_PER_DESC;
 		for (j = 0; j < ISO_FRAMES_PER_DESC; j++) {
@@ -541,14 +540,15 @@ static int msi2500_isoc_init(struct msi2500_state *s)
 
 	/* link */
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
-		ret = usb_submit_urb(s->urbs[i], GFP_KERNEL);
+		ret = usb_submit_urb(dev->urbs[i], GFP_KERNEL);
 		if (ret) {
-			dev_err(s->dev, "usb_submit_urb %d failed with error %d\n",
-					i, ret);
-			msi2500_isoc_cleanup(s);
+			dev_err(dev->dev,
+				"usb_submit_urb %d failed with error %d\n",
+				i, ret);
+			msi2500_isoc_cleanup(dev);
 			return ret;
 		}
-		dev_dbg(s->dev, "URB 0x%p submitted.\n", s->urbs[i]);
+		dev_dbg(dev->dev, "URB 0x%p submitted.\n", dev->urbs[i]);
 	}
 
 	/* All is done... */
@@ -556,56 +556,56 @@ static int msi2500_isoc_init(struct msi2500_state *s)
 }
 
 /* Must be called with vb_queue_lock hold */
-static void msi2500_cleanup_queued_bufs(struct msi2500_state *s)
+static void msi2500_cleanup_queued_bufs(struct msi2500_dev *dev)
 {
 	unsigned long flags;
 
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
-	spin_lock_irqsave(&s->queued_bufs_lock, flags);
-	while (!list_empty(&s->queued_bufs)) {
+	spin_lock_irqsave(&dev->queued_bufs_lock, flags);
+	while (!list_empty(&dev->queued_bufs)) {
 		struct msi2500_frame_buf *buf;
 
-		buf = list_entry(s->queued_bufs.next, struct msi2500_frame_buf,
-				 list);
+		buf = list_entry(dev->queued_bufs.next,
+				 struct msi2500_frame_buf, list);
 		list_del(&buf->list);
 		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 	}
-	spin_unlock_irqrestore(&s->queued_bufs_lock, flags);
+	spin_unlock_irqrestore(&dev->queued_bufs_lock, flags);
 }
 
 /* The user yanked out the cable... */
 static void msi2500_disconnect(struct usb_interface *intf)
 {
 	struct v4l2_device *v = usb_get_intfdata(intf);
-	struct msi2500_state *s =
-			container_of(v, struct msi2500_state, v4l2_dev);
+	struct msi2500_dev *dev =
+			container_of(v, struct msi2500_dev, v4l2_dev);
 
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
-	mutex_lock(&s->vb_queue_lock);
-	mutex_lock(&s->v4l2_lock);
+	mutex_lock(&dev->vb_queue_lock);
+	mutex_lock(&dev->v4l2_lock);
 	/* No need to keep the urbs around after disconnection */
-	s->udev = NULL;
-	v4l2_device_disconnect(&s->v4l2_dev);
-	video_unregister_device(&s->vdev);
-	spi_unregister_master(s->master);
-	mutex_unlock(&s->v4l2_lock);
-	mutex_unlock(&s->vb_queue_lock);
-
-	v4l2_device_put(&s->v4l2_dev);
+	dev->udev = NULL;
+	v4l2_device_disconnect(&dev->v4l2_dev);
+	video_unregister_device(&dev->vdev);
+	spi_unregister_master(dev->master);
+	mutex_unlock(&dev->v4l2_lock);
+	mutex_unlock(&dev->vb_queue_lock);
+
+	v4l2_device_put(&dev->v4l2_dev);
 }
 
 static int msi2500_querycap(struct file *file, void *fh,
-		struct v4l2_capability *cap)
+			    struct v4l2_capability *cap)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
 	strlcpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
-	strlcpy(cap->card, s->vdev.name, sizeof(cap->card));
-	usb_make_path(s->udev, cap->bus_info, sizeof(cap->bus_info));
+	strlcpy(cap->card, dev->vdev.name, sizeof(cap->card));
+	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
 	cap->device_caps = V4L2_CAP_SDR_CAPTURE | V4L2_CAP_STREAMING |
 			V4L2_CAP_READWRITE | V4L2_CAP_TUNER;
 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
@@ -614,43 +614,46 @@ static int msi2500_querycap(struct file *file, void *fh,
 
 /* Videobuf2 operations */
 static int msi2500_queue_setup(struct vb2_queue *vq,
-		const struct v4l2_format *fmt, unsigned int *nbuffers,
-		unsigned int *nplanes, unsigned int sizes[], void *alloc_ctxs[])
+			       const struct v4l2_format *fmt,
+			       unsigned int *nbuffers,
+			       unsigned int *nplanes, unsigned int sizes[],
+			       void *alloc_ctxs[])
 {
-	struct msi2500_state *s = vb2_get_drv_priv(vq);
+	struct msi2500_dev *dev = vb2_get_drv_priv(vq);
 
-	dev_dbg(s->dev, "nbuffers=%d\n", *nbuffers);
+	dev_dbg(dev->dev, "nbuffers=%d\n", *nbuffers);
 
 	/* Absolute min and max number of buffers available for mmap() */
 	*nbuffers = clamp_t(unsigned int, *nbuffers, 8, 32);
 	*nplanes = 1;
-	sizes[0] = PAGE_ALIGN(s->buffersize);
-	dev_dbg(s->dev, "nbuffers=%d sizes[0]=%d\n", *nbuffers, sizes[0]);
+	sizes[0] = PAGE_ALIGN(dev->buffersize);
+	dev_dbg(dev->dev, "nbuffers=%d sizes[0]=%d\n", *nbuffers, sizes[0]);
 	return 0;
 }
 
 static void msi2500_buf_queue(struct vb2_buffer *vb)
 {
-	struct msi2500_state *s = vb2_get_drv_priv(vb->vb2_queue);
-	struct msi2500_frame_buf *buf =
-			container_of(vb, struct msi2500_frame_buf, vb);
+	struct msi2500_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct msi2500_frame_buf *buf = container_of(vb,
+						     struct msi2500_frame_buf,
+						     vb);
 	unsigned long flags;
 
 	/* Check the device has not disconnected between prep and queuing */
-	if (unlikely(!s->udev)) {
+	if (unlikely(!dev->udev)) {
 		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 		return;
 	}
 
-	spin_lock_irqsave(&s->queued_bufs_lock, flags);
-	list_add_tail(&buf->list, &s->queued_bufs);
-	spin_unlock_irqrestore(&s->queued_bufs_lock, flags);
+	spin_lock_irqsave(&dev->queued_bufs_lock, flags);
+	list_add_tail(&buf->list, &dev->queued_bufs);
+	spin_unlock_irqrestore(&dev->queued_bufs_lock, flags);
 }
 
 #define CMD_WREG               0x41
 #define CMD_START_STREAMING    0x43
 #define CMD_STOP_STREAMING     0x45
-#define CMD_READ_UNKNOW        0x48
+#define CMD_READ_UNKNOWN       0x48
 
 #define msi2500_dbg_usb_control_msg(_dev, _r, _t, _v, _i, _b, _l) { \
 	char *_direction; \
@@ -663,7 +666,7 @@ static void msi2500_buf_queue(struct vb2_buffer *vb)
 			_l & 0xff, _l >> 8, _direction, _l, _b); \
 }
 
-static int msi2500_ctrl_msg(struct msi2500_state *s, u8 cmd, u32 data)
+static int msi2500_ctrl_msg(struct msi2500_dev *dev, u8 cmd, u32 data)
 {
 	int ret;
 	u8 request = cmd;
@@ -671,39 +674,38 @@ static int msi2500_ctrl_msg(struct msi2500_state *s, u8 cmd, u32 data)
 	u16 value = (data >> 0) & 0xffff;
 	u16 index = (data >> 16) & 0xffff;
 
-	msi2500_dbg_usb_control_msg(s->dev,
-			request, requesttype, value, index, NULL, 0);
-	ret = usb_control_msg(s->udev, usb_sndctrlpipe(s->udev, 0),
-			request, requesttype, value, index, NULL, 0, 2000);
+	msi2500_dbg_usb_control_msg(dev->dev, request, requesttype,
+				    value, index, NULL, 0);
+	ret = usb_control_msg(dev->udev, usb_sndctrlpipe(dev->udev, 0), request,
+			      requesttype, value, index, NULL, 0, 2000);
 	if (ret)
-		dev_err(s->dev, "failed %d, cmd %02x, data %04x\n",
-				ret, cmd, data);
+		dev_err(dev->dev, "failed %d, cmd %02x, data %04x\n",
+			ret, cmd, data);
 
 	return ret;
 }
 
-#define F_REF 24000000
-#define DIV_R_IN 2
-static int msi2500_set_usb_adc(struct msi2500_state *s)
+static int msi2500_set_usb_adc(struct msi2500_dev *dev)
 {
-	int ret, div_n, div_m, div_r_out, f_sr, f_vco, fract;
+	int ret;
+	unsigned int f_vco, f_sr, div_n, k, k_cw, div_out;
 	u32 reg3, reg4, reg7;
 	struct v4l2_ctrl *bandwidth_auto;
 	struct v4l2_ctrl *bandwidth;
 
-	f_sr = s->f_adc;
+	f_sr = dev->f_adc;
 
 	/* set tuner, subdev, filters according to sampling rate */
-	bandwidth_auto = v4l2_ctrl_find(&s->hdl,
+	bandwidth_auto = v4l2_ctrl_find(&dev->hdl,
 			V4L2_CID_RF_TUNER_BANDWIDTH_AUTO);
 	if (v4l2_ctrl_g_ctrl(bandwidth_auto)) {
-		bandwidth = v4l2_ctrl_find(&s->hdl,
+		bandwidth = v4l2_ctrl_find(&dev->hdl,
 				V4L2_CID_RF_TUNER_BANDWIDTH);
-		v4l2_ctrl_s_ctrl(bandwidth, s->f_adc);
+		v4l2_ctrl_s_ctrl(bandwidth, dev->f_adc);
 	}
 
 	/* select stream format */
-	switch (s->pixelformat) {
+	switch (dev->pixelformat) {
 	case V4L2_SDR_FMT_CU8:
 		reg7 = 0x000c9407; /* 504 */
 		break;
@@ -728,6 +730,21 @@ static int msi2500_set_usb_adc(struct msi2500_state *s)
 	}
 
 	/*
+	 * Fractional-N synthesizer
+	 *
+	 *           +----------------------------------------+
+	 *           v                                        |
+	 *  Fref   +----+     +-------+     +-----+         +------+     +---+
+	 * ------> | PD | --> |  VCO  | --> | /2  | ------> | /N.F | <-- | K |
+	 *         +----+     +-------+     +-----+         +------+     +---+
+	 *                      |
+	 *                      |
+	 *                      v
+	 *                    +-------+     +-----+  Fout
+	 *                    | /Rout | --> | /12 | ------>
+	 *                    +-------+     +-----+
+	 */
+	/*
 	 * Synthesizer config is just a educated guess...
 	 *
 	 * [7:0]   0x03, register address
@@ -754,10 +771,14 @@ static int msi2500_set_usb_adc(struct msi2500_state *s)
 	 *
 	 * VCO 202000000 - 720000000++
 	 */
+
+	#define F_REF 24000000
+	#define DIV_PRE_N 2
+	#define DIV_LO_OUT 12
 	reg3 = 0x01000303;
 	reg4 = 0x00000004;
 
-	/* XXX: Filters? AGC? */
+	/* XXX: Filters? AGC? VCO band? */
 	if (f_sr < 6000000)
 		reg3 |= 0x1 << 20;
 	else if (f_sr < 7000000)
@@ -767,54 +788,55 @@ static int msi2500_set_usb_adc(struct msi2500_state *s)
 	else
 		reg3 |= 0xd << 20;
 
-	for (div_r_out = 4; div_r_out < 16; div_r_out += 2) {
-		f_vco = f_sr * div_r_out * 12;
-		dev_dbg(s->dev, "div_r_out=%d f_vco=%d\n", div_r_out, f_vco);
+	for (div_out = 4; div_out < 16; div_out += 2) {
+		f_vco = f_sr * div_out * DIV_LO_OUT;
+		dev_dbg(dev->dev, "div_out=%u f_vco=%u\n", div_out, f_vco);
 		if (f_vco >= 202000000)
 			break;
 	}
 
-	div_n = f_vco / (F_REF * DIV_R_IN);
-	div_m = f_vco % (F_REF * DIV_R_IN);
-	fract = 0x200000ul * div_m / (F_REF * DIV_R_IN);
+	/* Calculate PLL integer and fractional control word. */
+	div_n = div_u64_rem(f_vco, DIV_PRE_N * F_REF, &k);
+	k_cw = div_u64((u64) k * 0x200000, DIV_PRE_N * F_REF);
 
 	reg3 |= div_n << 16;
-	reg3 |= (div_r_out / 2 - 1) << 10;
-	reg3 |= ((fract >> 20) & 0x000001) << 15; /* [20] */
-	reg4 |= ((fract >>  0) & 0x0fffff) <<  8; /* [19:0] */
+	reg3 |= (div_out / 2 - 1) << 10;
+	reg3 |= ((k_cw >> 20) & 0x000001) << 15; /* [20] */
+	reg4 |= ((k_cw >>  0) & 0x0fffff) <<  8; /* [19:0] */
 
-	dev_dbg(s->dev, "f_sr=%d f_vco=%d div_n=%d div_m=%d div_r_out=%d reg3=%08x reg4=%08x\n",
-			f_sr, f_vco, div_n, div_m, div_r_out, reg3, reg4);
+	dev_dbg(dev->dev,
+		"f_sr=%u f_vco=%u div_n=%u k=%u div_out=%u reg3=%08x reg4=%08x\n",
+		f_sr, f_vco, div_n, k, div_out, reg3, reg4);
 
-	ret = msi2500_ctrl_msg(s, CMD_WREG, 0x00608008);
+	ret = msi2500_ctrl_msg(dev, CMD_WREG, 0x00608008);
 	if (ret)
 		goto err;
 
-	ret = msi2500_ctrl_msg(s, CMD_WREG, 0x00000c05);
+	ret = msi2500_ctrl_msg(dev, CMD_WREG, 0x00000c05);
 	if (ret)
 		goto err;
 
-	ret = msi2500_ctrl_msg(s, CMD_WREG, 0x00020000);
+	ret = msi2500_ctrl_msg(dev, CMD_WREG, 0x00020000);
 	if (ret)
 		goto err;
 
-	ret = msi2500_ctrl_msg(s, CMD_WREG, 0x00480102);
+	ret = msi2500_ctrl_msg(dev, CMD_WREG, 0x00480102);
 	if (ret)
 		goto err;
 
-	ret = msi2500_ctrl_msg(s, CMD_WREG, 0x00f38008);
+	ret = msi2500_ctrl_msg(dev, CMD_WREG, 0x00f38008);
 	if (ret)
 		goto err;
 
-	ret = msi2500_ctrl_msg(s, CMD_WREG, reg7);
+	ret = msi2500_ctrl_msg(dev, CMD_WREG, reg7);
 	if (ret)
 		goto err;
 
-	ret = msi2500_ctrl_msg(s, CMD_WREG, reg4);
+	ret = msi2500_ctrl_msg(dev, CMD_WREG, reg4);
 	if (ret)
 		goto err;
 
-	ret = msi2500_ctrl_msg(s, CMD_WREG, reg3);
+	ret = msi2500_ctrl_msg(dev, CMD_WREG, reg3);
 	if (ret)
 		goto err;
 err:
@@ -823,57 +845,57 @@ err:
 
 static int msi2500_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
-	struct msi2500_state *s = vb2_get_drv_priv(vq);
+	struct msi2500_dev *dev = vb2_get_drv_priv(vq);
 	int ret;
 
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
-	if (!s->udev)
+	if (!dev->udev)
 		return -ENODEV;
 
-	if (mutex_lock_interruptible(&s->v4l2_lock))
+	if (mutex_lock_interruptible(&dev->v4l2_lock))
 		return -ERESTARTSYS;
 
 	/* wake-up tuner */
-	v4l2_subdev_call(s->v4l2_subdev, core, s_power, 1);
+	v4l2_subdev_call(dev->v4l2_subdev, core, s_power, 1);
 
-	ret = msi2500_set_usb_adc(s);
+	ret = msi2500_set_usb_adc(dev);
 
-	ret = msi2500_isoc_init(s);
+	ret = msi2500_isoc_init(dev);
 	if (ret)
-		msi2500_cleanup_queued_bufs(s);
+		msi2500_cleanup_queued_bufs(dev);
 
-	ret = msi2500_ctrl_msg(s, CMD_START_STREAMING, 0);
+	ret = msi2500_ctrl_msg(dev, CMD_START_STREAMING, 0);
 
-	mutex_unlock(&s->v4l2_lock);
+	mutex_unlock(&dev->v4l2_lock);
 
 	return ret;
 }
 
 static void msi2500_stop_streaming(struct vb2_queue *vq)
 {
-	struct msi2500_state *s = vb2_get_drv_priv(vq);
+	struct msi2500_dev *dev = vb2_get_drv_priv(vq);
 
-	dev_dbg(s->dev, "\n");
+	dev_dbg(dev->dev, "\n");
 
-	mutex_lock(&s->v4l2_lock);
+	mutex_lock(&dev->v4l2_lock);
 
-	if (s->udev)
-		msi2500_isoc_cleanup(s);
+	if (dev->udev)
+		msi2500_isoc_cleanup(dev);
 
-	msi2500_cleanup_queued_bufs(s);
+	msi2500_cleanup_queued_bufs(dev);
 
 	/* according to tests, at least 700us delay is required  */
 	msleep(20);
-	if (!msi2500_ctrl_msg(s, CMD_STOP_STREAMING, 0)) {
+	if (!msi2500_ctrl_msg(dev, CMD_STOP_STREAMING, 0)) {
 		/* sleep USB IF / ADC */
-		msi2500_ctrl_msg(s, CMD_WREG, 0x01000003);
+		msi2500_ctrl_msg(dev, CMD_WREG, 0x01000003);
 	}
 
 	/* sleep tuner */
-	v4l2_subdev_call(s->v4l2_subdev, core, s_power, 0);
+	v4l2_subdev_call(dev->v4l2_subdev, core, s_power, 0);
 
-	mutex_unlock(&s->v4l2_lock);
+	mutex_unlock(&dev->v4l2_lock);
 }
 
 static struct vb2_ops msi2500_vb2_ops = {
@@ -886,13 +908,13 @@ static struct vb2_ops msi2500_vb2_ops = {
 };
 
 static int msi2500_enum_fmt_sdr_cap(struct file *file, void *priv,
-		struct v4l2_fmtdesc *f)
+				    struct v4l2_fmtdesc *f)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 
-	dev_dbg(s->dev, "index=%d\n", f->index);
+	dev_dbg(dev->dev, "index=%d\n", f->index);
 
-	if (f->index >= s->num_formats)
+	if (f->index >= dev->num_formats)
 		return -EINVAL;
 
 	strlcpy(f->description, formats[f->index].name, sizeof(f->description));
@@ -902,45 +924,45 @@ static int msi2500_enum_fmt_sdr_cap(struct file *file, void *priv,
 }
 
 static int msi2500_g_fmt_sdr_cap(struct file *file, void *priv,
-		struct v4l2_format *f)
+				 struct v4l2_format *f)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 
-	dev_dbg(s->dev, "pixelformat fourcc %4.4s\n",
-			(char *)&s->pixelformat);
+	dev_dbg(dev->dev, "pixelformat fourcc %4.4s\n",
+		(char *)&dev->pixelformat);
 
-	f->fmt.sdr.pixelformat = s->pixelformat;
-	f->fmt.sdr.buffersize = s->buffersize;
+	f->fmt.sdr.pixelformat = dev->pixelformat;
+	f->fmt.sdr.buffersize = dev->buffersize;
 	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
 
 	return 0;
 }
 
 static int msi2500_s_fmt_sdr_cap(struct file *file, void *priv,
-		struct v4l2_format *f)
+				 struct v4l2_format *f)
 {
-	struct msi2500_state *s = video_drvdata(file);
-	struct vb2_queue *q = &s->vb_queue;
+	struct msi2500_dev *dev = video_drvdata(file);
+	struct vb2_queue *q = &dev->vb_queue;
 	int i;
 
-	dev_dbg(s->dev, "pixelformat fourcc %4.4s\n",
-			(char *)&f->fmt.sdr.pixelformat);
+	dev_dbg(dev->dev, "pixelformat fourcc %4.4s\n",
+		(char *)&f->fmt.sdr.pixelformat);
 
 	if (vb2_is_busy(q))
 		return -EBUSY;
 
 	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
-	for (i = 0; i < s->num_formats; i++) {
+	for (i = 0; i < dev->num_formats; i++) {
 		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
-			s->pixelformat = formats[i].pixelformat;
-			s->buffersize = formats[i].buffersize;
+			dev->pixelformat = formats[i].pixelformat;
+			dev->buffersize = formats[i].buffersize;
 			f->fmt.sdr.buffersize = formats[i].buffersize;
 			return 0;
 		}
 	}
 
-	s->pixelformat = formats[0].pixelformat;
-	s->buffersize = formats[0].buffersize;
+	dev->pixelformat = formats[0].pixelformat;
+	dev->buffersize = formats[0].buffersize;
 	f->fmt.sdr.pixelformat = formats[0].pixelformat;
 	f->fmt.sdr.buffersize = formats[0].buffersize;
 
@@ -948,16 +970,16 @@ static int msi2500_s_fmt_sdr_cap(struct file *file, void *priv,
 }
 
 static int msi2500_try_fmt_sdr_cap(struct file *file, void *priv,
-		struct v4l2_format *f)
+				   struct v4l2_format *f)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 	int i;
 
-	dev_dbg(s->dev, "pixelformat fourcc %4.4s\n",
-			(char *)&f->fmt.sdr.pixelformat);
+	dev_dbg(dev->dev, "pixelformat fourcc %4.4s\n",
+		(char *)&f->fmt.sdr.pixelformat);
 
 	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
-	for (i = 0; i < s->num_formats; i++) {
+	for (i = 0; i < dev->num_formats; i++) {
 		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
 			f->fmt.sdr.buffersize = formats[i].buffersize;
 			return 0;
@@ -971,17 +993,17 @@ static int msi2500_try_fmt_sdr_cap(struct file *file, void *priv,
 }
 
 static int msi2500_s_tuner(struct file *file, void *priv,
-		const struct v4l2_tuner *v)
+			   const struct v4l2_tuner *v)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 	int ret;
 
-	dev_dbg(s->dev, "index=%d\n", v->index);
+	dev_dbg(dev->dev, "index=%d\n", v->index);
 
 	if (v->index == 0)
 		ret = 0;
 	else if (v->index == 1)
-		ret = v4l2_subdev_call(s->v4l2_subdev, tuner, s_tuner, v);
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, s_tuner, v);
 	else
 		ret = -EINVAL;
 
@@ -990,10 +1012,10 @@ static int msi2500_s_tuner(struct file *file, void *priv,
 
 static int msi2500_g_tuner(struct file *file, void *priv, struct v4l2_tuner *v)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 	int ret;
 
-	dev_dbg(s->dev, "index=%d\n", v->index);
+	dev_dbg(dev->dev, "index=%d\n", v->index);
 
 	if (v->index == 0) {
 		strlcpy(v->name, "Mirics MSi2500", sizeof(v->name));
@@ -1003,7 +1025,7 @@ static int msi2500_g_tuner(struct file *file, void *priv, struct v4l2_tuner *v)
 		v->rangehigh = 15000000;
 		ret = 0;
 	} else if (v->index == 1) {
-		ret = v4l2_subdev_call(s->v4l2_subdev, tuner, g_tuner, v);
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, g_tuner, v);
 	} else {
 		ret = -EINVAL;
 	}
@@ -1012,19 +1034,19 @@ static int msi2500_g_tuner(struct file *file, void *priv, struct v4l2_tuner *v)
 }
 
 static int msi2500_g_frequency(struct file *file, void *priv,
-		struct v4l2_frequency *f)
+			       struct v4l2_frequency *f)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 	int ret  = 0;
 
-	dev_dbg(s->dev, "tuner=%d type=%d\n", f->tuner, f->type);
+	dev_dbg(dev->dev, "tuner=%d type=%d\n", f->tuner, f->type);
 
 	if (f->tuner == 0) {
-		f->frequency = s->f_adc;
+		f->frequency = dev->f_adc;
 		ret = 0;
 	} else if (f->tuner == 1) {
 		f->type = V4L2_TUNER_RF;
-		ret = v4l2_subdev_call(s->v4l2_subdev, tuner, g_frequency, f);
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, g_frequency, f);
 	} else {
 		ret = -EINVAL;
 	}
@@ -1033,22 +1055,22 @@ static int msi2500_g_frequency(struct file *file, void *priv,
 }
 
 static int msi2500_s_frequency(struct file *file, void *priv,
-		const struct v4l2_frequency *f)
+			       const struct v4l2_frequency *f)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 	int ret;
 
-	dev_dbg(s->dev, "tuner=%d type=%d frequency=%u\n",
-			f->tuner, f->type, f->frequency);
+	dev_dbg(dev->dev, "tuner=%d type=%d frequency=%u\n",
+		f->tuner, f->type, f->frequency);
 
 	if (f->tuner == 0) {
-		s->f_adc = clamp_t(unsigned int, f->frequency,
-				bands[0].rangelow,
-				bands[0].rangehigh);
-		dev_dbg(s->dev, "ADC frequency=%u Hz\n", s->f_adc);
-		ret = msi2500_set_usb_adc(s);
+		dev->f_adc = clamp_t(unsigned int, f->frequency,
+				     bands[0].rangelow,
+				     bands[0].rangehigh);
+		dev_dbg(dev->dev, "ADC frequency=%u Hz\n", dev->f_adc);
+		ret = msi2500_set_usb_adc(dev);
 	} else if (f->tuner == 1) {
-		ret = v4l2_subdev_call(s->v4l2_subdev, tuner, s_frequency, f);
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner, s_frequency, f);
 	} else {
 		ret = -EINVAL;
 	}
@@ -1057,13 +1079,13 @@ static int msi2500_s_frequency(struct file *file, void *priv,
 }
 
 static int msi2500_enum_freq_bands(struct file *file, void *priv,
-		struct v4l2_frequency_band *band)
+				   struct v4l2_frequency_band *band)
 {
-	struct msi2500_state *s = video_drvdata(file);
+	struct msi2500_dev *dev = video_drvdata(file);
 	int ret;
 
-	dev_dbg(s->dev, "tuner=%d type=%d index=%d\n",
-			band->tuner, band->type, band->index);
+	dev_dbg(dev->dev, "tuner=%d type=%d index=%d\n",
+		band->tuner, band->type, band->index);
 
 	if (band->tuner == 0) {
 		if (band->index >= ARRAY_SIZE(bands)) {
@@ -1073,8 +1095,8 @@ static int msi2500_enum_freq_bands(struct file *file, void *priv,
 			ret = 0;
 		}
 	} else if (band->tuner == 1) {
-		ret = v4l2_subdev_call(s->v4l2_subdev, tuner,
-				enum_freq_bands, band);
+		ret = v4l2_subdev_call(dev->v4l2_subdev, tuner,
+				       enum_freq_bands, band);
 	} else {
 		ret = -EINVAL;
 	}
@@ -1131,29 +1153,28 @@ static struct video_device msi2500_template = {
 
 static void msi2500_video_release(struct v4l2_device *v)
 {
-	struct msi2500_state *s =
-			container_of(v, struct msi2500_state, v4l2_dev);
+	struct msi2500_dev *dev = container_of(v, struct msi2500_dev, v4l2_dev);
 
-	v4l2_ctrl_handler_free(&s->hdl);
-	v4l2_device_unregister(&s->v4l2_dev);
-	kfree(s);
+	v4l2_ctrl_handler_free(&dev->hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	kfree(dev);
 }
 
 static int msi2500_transfer_one_message(struct spi_master *master,
-		struct spi_message *m)
+					struct spi_message *m)
 {
-	struct msi2500_state *s = spi_master_get_devdata(master);
+	struct msi2500_dev *dev = spi_master_get_devdata(master);
 	struct spi_transfer *t;
 	int ret = 0;
 	u32 data;
 
 	list_for_each_entry(t, &m->transfers, transfer_list) {
-		dev_dbg(s->dev, "msg=%*ph\n", t->len, t->tx_buf);
+		dev_dbg(dev->dev, "msg=%*ph\n", t->len, t->tx_buf);
 		data = 0x09; /* reg 9 is SPI adapter */
 		data |= ((u8 *)t->tx_buf)[0] << 8;
 		data |= ((u8 *)t->tx_buf)[1] << 16;
 		data |= ((u8 *)t->tx_buf)[2] << 24;
-		ret = msi2500_ctrl_msg(s, CMD_WREG, data);
+		ret = msi2500_ctrl_msg(dev, CMD_WREG, data);
 	}
 
 	m->status = ret;
@@ -1162,9 +1183,9 @@ static int msi2500_transfer_one_message(struct spi_master *master,
 }
 
 static int msi2500_probe(struct usb_interface *intf,
-		const struct usb_device_id *id)
+			 const struct usb_device_id *id)
 {
-	struct msi2500_state *s;
+	struct msi2500_dev *dev;
 	struct v4l2_subdev *sd;
 	struct spi_master *master;
 	int ret;
@@ -1175,65 +1196,65 @@ static int msi2500_probe(struct usb_interface *intf,
 		.max_speed_hz		= 12000000,
 	};
 
-	s = kzalloc(sizeof(struct msi2500_state), GFP_KERNEL);
-	if (s == NULL) {
-		dev_err(&intf->dev, "Could not allocate memory for state\n");
-		return -ENOMEM;
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		ret = -ENOMEM;
+		goto err;
 	}
 
-	mutex_init(&s->v4l2_lock);
-	mutex_init(&s->vb_queue_lock);
-	spin_lock_init(&s->queued_bufs_lock);
-	INIT_LIST_HEAD(&s->queued_bufs);
-	s->dev = &intf->dev;
-	s->udev = interface_to_usbdev(intf);
-	s->f_adc = bands[0].rangelow;
-	s->pixelformat = formats[0].pixelformat;
-	s->buffersize = formats[0].buffersize;
-	s->num_formats = NUM_FORMATS;
+	mutex_init(&dev->v4l2_lock);
+	mutex_init(&dev->vb_queue_lock);
+	spin_lock_init(&dev->queued_bufs_lock);
+	INIT_LIST_HEAD(&dev->queued_bufs);
+	dev->dev = &intf->dev;
+	dev->udev = interface_to_usbdev(intf);
+	dev->f_adc = bands[0].rangelow;
+	dev->pixelformat = formats[0].pixelformat;
+	dev->buffersize = formats[0].buffersize;
+	dev->num_formats = NUM_FORMATS;
 	if (!msi2500_emulated_fmt)
-		s->num_formats -= 2;
+		dev->num_formats -= 2;
 
 	/* Init videobuf2 queue structure */
-	s->vb_queue.type = V4L2_BUF_TYPE_SDR_CAPTURE;
-	s->vb_queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
-	s->vb_queue.drv_priv = s;
-	s->vb_queue.buf_struct_size = sizeof(struct msi2500_frame_buf);
-	s->vb_queue.ops = &msi2500_vb2_ops;
-	s->vb_queue.mem_ops = &vb2_vmalloc_memops;
-	s->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	ret = vb2_queue_init(&s->vb_queue);
+	dev->vb_queue.type = V4L2_BUF_TYPE_SDR_CAPTURE;
+	dev->vb_queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	dev->vb_queue.drv_priv = dev;
+	dev->vb_queue.buf_struct_size = sizeof(struct msi2500_frame_buf);
+	dev->vb_queue.ops = &msi2500_vb2_ops;
+	dev->vb_queue.mem_ops = &vb2_vmalloc_memops;
+	dev->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	ret = vb2_queue_init(&dev->vb_queue);
 	if (ret) {
-		dev_err(s->dev, "Could not initialize vb2 queue\n");
+		dev_err(dev->dev, "Could not initialize vb2 queue\n");
 		goto err_free_mem;
 	}
 
 	/* Init video_device structure */
-	s->vdev = msi2500_template;
-	s->vdev.queue = &s->vb_queue;
-	s->vdev.queue->lock = &s->vb_queue_lock;
-	video_set_drvdata(&s->vdev, s);
+	dev->vdev = msi2500_template;
+	dev->vdev.queue = &dev->vb_queue;
+	dev->vdev.queue->lock = &dev->vb_queue_lock;
+	video_set_drvdata(&dev->vdev, dev);
 
 	/* Register the v4l2_device structure */
-	s->v4l2_dev.release = msi2500_video_release;
-	ret = v4l2_device_register(&intf->dev, &s->v4l2_dev);
+	dev->v4l2_dev.release = msi2500_video_release;
+	ret = v4l2_device_register(&intf->dev, &dev->v4l2_dev);
 	if (ret) {
-		dev_err(s->dev, "Failed to register v4l2-device (%d)\n", ret);
+		dev_err(dev->dev, "Failed to register v4l2-device (%d)\n", ret);
 		goto err_free_mem;
 	}
 
 	/* SPI master adapter */
-	master = spi_alloc_master(s->dev, 0);
+	master = spi_alloc_master(dev->dev, 0);
 	if (master == NULL) {
 		ret = -ENOMEM;
 		goto err_unregister_v4l2_dev;
 	}
 
-	s->master = master;
+	dev->master = master;
 	master->bus_num = 0;
 	master->num_chipselect = 1;
 	master->transfer_one_message = msi2500_transfer_one_message;
-	spi_master_set_devdata(master, s);
+	spi_master_set_devdata(master, dev);
 	ret = spi_register_master(master);
 	if (ret) {
 		spi_master_put(master);
@@ -1241,57 +1262,57 @@ static int msi2500_probe(struct usb_interface *intf,
 	}
 
 	/* load v4l2 subdevice */
-	sd = v4l2_spi_new_subdev(&s->v4l2_dev, master, &board_info);
-	s->v4l2_subdev = sd;
+	sd = v4l2_spi_new_subdev(&dev->v4l2_dev, master, &board_info);
+	dev->v4l2_subdev = sd;
 	if (sd == NULL) {
-		dev_err(s->dev, "cannot get v4l2 subdevice\n");
+		dev_err(dev->dev, "cannot get v4l2 subdevice\n");
 		ret = -ENODEV;
 		goto err_unregister_master;
 	}
 
 	/* Register controls */
-	v4l2_ctrl_handler_init(&s->hdl, 0);
-	if (s->hdl.error) {
-		ret = s->hdl.error;
-		dev_err(s->dev, "Could not initialize controls\n");
+	v4l2_ctrl_handler_init(&dev->hdl, 0);
+	if (dev->hdl.error) {
+		ret = dev->hdl.error;
+		dev_err(dev->dev, "Could not initialize controls\n");
 		goto err_free_controls;
 	}
 
 	/* currently all controls are from subdev */
-	v4l2_ctrl_add_handler(&s->hdl, sd->ctrl_handler, NULL);
+	v4l2_ctrl_add_handler(&dev->hdl, sd->ctrl_handler, NULL);
 
-	s->v4l2_dev.ctrl_handler = &s->hdl;
-	s->vdev.v4l2_dev = &s->v4l2_dev;
-	s->vdev.lock = &s->v4l2_lock;
+	dev->v4l2_dev.ctrl_handler = &dev->hdl;
+	dev->vdev.v4l2_dev = &dev->v4l2_dev;
+	dev->vdev.lock = &dev->v4l2_lock;
 
-	ret = video_register_device(&s->vdev, VFL_TYPE_SDR, -1);
+	ret = video_register_device(&dev->vdev, VFL_TYPE_SDR, -1);
 	if (ret) {
-		dev_err(s->dev, "Failed to register as video device (%d)\n",
-				ret);
+		dev_err(dev->dev,
+			"Failed to register as video device (%d)\n", ret);
 		goto err_unregister_v4l2_dev;
 	}
-	dev_info(s->dev, "Registered as %s\n",
-			video_device_node_name(&s->vdev));
-	dev_notice(s->dev, "SDR API is still slightly experimental and functionality changes may follow\n");
-
+	dev_info(dev->dev, "Registered as %s\n",
+		 video_device_node_name(&dev->vdev));
+	dev_notice(dev->dev,
+		   "SDR API is still slightly experimental and functionality changes may follow\n");
 	return 0;
-
 err_free_controls:
-	v4l2_ctrl_handler_free(&s->hdl);
+	v4l2_ctrl_handler_free(&dev->hdl);
 err_unregister_master:
-	spi_unregister_master(s->master);
+	spi_unregister_master(dev->master);
 err_unregister_v4l2_dev:
-	v4l2_device_unregister(&s->v4l2_dev);
+	v4l2_device_unregister(&dev->v4l2_dev);
 err_free_mem:
-	kfree(s);
+	kfree(dev);
+err:
 	return ret;
 }
 
 /* USB device ID list */
 static struct usb_device_id msi2500_id_table[] = {
-	{ USB_DEVICE(0x1df7, 0x2500) }, /* Mirics MSi3101 SDR Dongle */
-	{ USB_DEVICE(0x2040, 0xd300) }, /* Hauppauge WinTV 133559 LF */
-	{ }
+	{USB_DEVICE(0x1df7, 0x2500)}, /* Mirics MSi3101 SDR Dongle */
+	{USB_DEVICE(0x2040, 0xd300)}, /* Hauppauge WinTV 133559 LF */
+	{}
 };
 MODULE_DEVICE_TABLE(usb, msi2500_id_table);
 
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-context.c b/drivers/media/usb/pvrusb2/pvrusb2-context.c
index 924fc4c6019a..fd888a604462 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-context.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-context.c
@@ -398,7 +398,8 @@ int pvr2_channel_claim_stream(struct pvr2_channel *cp,
 		if (!sp) break;
 		sp->user = cp;
 		cp->stream = sp;
-	} while (0); pvr2_context_exit(cp->mc_head);
+	} while (0);
+	pvr2_context_exit(cp->mc_head);
 	return code;
 }
 
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
index 930593d7028d..0533ef20decf 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
@@ -2602,14 +2602,16 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
 			   "Error registering with v4l core, giving up");
 		goto fail;
 	}
-	mutex_lock(&pvr2_unit_mtx); do {
+	mutex_lock(&pvr2_unit_mtx);
+	do {
 		for (idx = 0; idx < PVR_NUM; idx++) {
 			if (unit_pointers[idx]) continue;
 			hdw->unit_number = idx;
 			unit_pointers[idx] = hdw;
 			break;
 		}
-	} while (0); mutex_unlock(&pvr2_unit_mtx);
+	} while (0);
+	mutex_unlock(&pvr2_unit_mtx);
 
 	cnt1 = 0;
 	cnt2 = scnprintf(hdw->name+cnt1,sizeof(hdw->name)-cnt1,"pvrusb2");
@@ -2730,13 +2732,15 @@ void pvr2_hdw_destroy(struct pvr2_hdw *hdw)
 	pvr2_i2c_core_done(hdw);
 	v4l2_device_unregister(&hdw->v4l2_dev);
 	pvr2_hdw_remove_usb_stuff(hdw);
-	mutex_lock(&pvr2_unit_mtx); do {
+	mutex_lock(&pvr2_unit_mtx);
+	do {
 		if ((hdw->unit_number >= 0) &&
 		    (hdw->unit_number < PVR_NUM) &&
 		    (unit_pointers[hdw->unit_number] == hdw)) {
 			unit_pointers[hdw->unit_number] = NULL;
 		}
-	} while (0); mutex_unlock(&pvr2_unit_mtx);
+	} while (0);
+	mutex_unlock(&pvr2_unit_mtx);
 	kfree(hdw->controls);
 	kfree(hdw->mpeg_ctrl_info);
 	kfree(hdw);
@@ -2958,14 +2962,17 @@ static void pvr2_subdev_update(struct pvr2_hdw *hdw)
 	}
 
 	if (hdw->res_hor_dirty || hdw->res_ver_dirty || hdw->force_dirty) {
-		struct v4l2_mbus_framefmt fmt;
-		memset(&fmt, 0, sizeof(fmt));
-		fmt.width = hdw->res_hor_val;
-		fmt.height = hdw->res_ver_val;
-		fmt.code = MEDIA_BUS_FMT_FIXED;
+		struct v4l2_subdev_format format = {
+			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		};
+
+		format.format.width = hdw->res_hor_val;
+		format.format.height = hdw->res_ver_val;
+		format.format.code = MEDIA_BUS_FMT_FIXED;
 		pvr2_trace(PVR2_TRACE_CHIPS, "subdev v4l2 set_size(%dx%d)",
-			   fmt.width, fmt.height);
-		v4l2_device_call_all(&hdw->v4l2_dev, 0, video, s_mbus_fmt, &fmt);
+			   format.format.width, format.format.height);
+		v4l2_device_call_all(&hdw->v4l2_dev, 0, pad, set_fmt,
+				     NULL, &format);
 	}
 
 	if (hdw->srate_dirty || hdw->force_dirty) {
@@ -3343,14 +3350,16 @@ struct pvr2_stream *pvr2_hdw_get_video_stream(struct pvr2_hdw *hp)
 void pvr2_hdw_trigger_module_log(struct pvr2_hdw *hdw)
 {
 	int nr = pvr2_hdw_get_unit_number(hdw);
-	LOCK_TAKE(hdw->big_lock); do {
+	LOCK_TAKE(hdw->big_lock);
+	do {
 		printk(KERN_INFO "pvrusb2: =================  START STATUS CARD #%d  =================\n", nr);
 		v4l2_device_call_all(&hdw->v4l2_dev, 0, core, log_status);
 		pvr2_trace(PVR2_TRACE_INFO,"cx2341x config:");
 		cx2341x_log_status(&hdw->enc_ctl_state, "pvrusb2");
 		pvr2_hdw_state_log_state(hdw);
 		printk(KERN_INFO "pvrusb2: ==================  END STATUS CARD #%d  ==================\n", nr);
-	} while (0); LOCK_GIVE(hdw->big_lock);
+	} while (0);
+	LOCK_GIVE(hdw->big_lock);
 }
 
 
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-io.c b/drivers/media/usb/pvrusb2/pvrusb2-io.c
index 0c08f22bdfce..d860344de84e 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-io.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-io.c
@@ -514,12 +514,14 @@ void pvr2_stream_set_callback(struct pvr2_stream *sp,
 			      void *data)
 {
 	unsigned long irq_flags;
-	mutex_lock(&sp->mutex); do {
+	mutex_lock(&sp->mutex);
+	do {
 		spin_lock_irqsave(&sp->list_lock,irq_flags);
 		sp->callback_data = data;
 		sp->callback_func = func;
 		spin_unlock_irqrestore(&sp->list_lock,irq_flags);
-	} while(0); mutex_unlock(&sp->mutex);
+	} while(0);
+	mutex_unlock(&sp->mutex);
 }
 
 void pvr2_stream_get_stats(struct pvr2_stream *sp,
@@ -554,10 +556,12 @@ int pvr2_stream_set_buffer_count(struct pvr2_stream *sp,unsigned int cnt)
 {
 	int ret;
 	if (sp->buffer_target_count == cnt) return 0;
-	mutex_lock(&sp->mutex); do {
+	mutex_lock(&sp->mutex);
+	do {
 		sp->buffer_target_count = cnt;
 		ret = pvr2_stream_achieve_buffer_count(sp);
-	} while(0); mutex_unlock(&sp->mutex);
+	} while(0);
+	mutex_unlock(&sp->mutex);
 	return ret;
 }
 
@@ -590,7 +594,8 @@ int pvr2_stream_get_ready_count(struct pvr2_stream *sp)
 void pvr2_stream_kill(struct pvr2_stream *sp)
 {
 	struct pvr2_buffer *bp;
-	mutex_lock(&sp->mutex); do {
+	mutex_lock(&sp->mutex);
+	do {
 		pvr2_stream_internal_flush(sp);
 		while ((bp = pvr2_stream_get_ready_buffer(sp)) != NULL) {
 			pvr2_buffer_set_idle(bp);
@@ -598,7 +603,8 @@ void pvr2_stream_kill(struct pvr2_stream *sp)
 		if (sp->buffer_total_count != sp->buffer_target_count) {
 			pvr2_stream_achieve_buffer_count(sp);
 		}
-	} while(0); mutex_unlock(&sp->mutex);
+	} while(0);
+	mutex_unlock(&sp->mutex);
 }
 
 int pvr2_buffer_queue(struct pvr2_buffer *bp)
@@ -612,7 +618,8 @@ int pvr2_buffer_queue(struct pvr2_buffer *bp)
 	struct pvr2_stream *sp;
 	if (!bp) return -EINVAL;
 	sp = bp->stream;
-	mutex_lock(&sp->mutex); do {
+	mutex_lock(&sp->mutex);
+	do {
 		pvr2_buffer_wipe(bp);
 		if (!sp->dev) {
 			ret = -EIO;
@@ -636,7 +643,8 @@ int pvr2_buffer_queue(struct pvr2_buffer *bp)
 				  buffer_complete,
 				  bp);
 		usb_submit_urb(bp->purb,GFP_KERNEL);
-	} while(0); mutex_unlock(&sp->mutex);
+	} while(0);
+	mutex_unlock(&sp->mutex);
 	return ret;
 }
 
@@ -647,7 +655,8 @@ int pvr2_buffer_set_buffer(struct pvr2_buffer *bp,void *ptr,unsigned int cnt)
 	struct pvr2_stream *sp;
 	if (!bp) return -EINVAL;
 	sp = bp->stream;
-	mutex_lock(&sp->mutex); do {
+	mutex_lock(&sp->mutex);
+	do {
 		spin_lock_irqsave(&sp->list_lock,irq_flags);
 		if (bp->state != pvr2_buffer_state_idle) {
 			ret = -EPERM;
@@ -664,7 +673,8 @@ int pvr2_buffer_set_buffer(struct pvr2_buffer *bp,void *ptr,unsigned int cnt)
 				   bp->stream->i_bcount,bp->stream->i_count);
 		}
 		spin_unlock_irqrestore(&sp->list_lock,irq_flags);
-	} while(0); mutex_unlock(&sp->mutex);
+	} while(0);
+	mutex_unlock(&sp->mutex);
 	return ret;
 }
 
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-ioread.c b/drivers/media/usb/pvrusb2/pvrusb2-ioread.c
index cd995b54732e..614d55767a4e 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-ioread.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-ioread.c
@@ -205,7 +205,8 @@ int pvr2_ioread_setup(struct pvr2_ioread *cp,struct pvr2_stream *sp)
 	unsigned int idx;
 	struct pvr2_buffer *bp;
 
-	mutex_lock(&cp->mutex); do {
+	mutex_lock(&cp->mutex);
+	do {
 		if (cp->stream) {
 			pvr2_trace(PVR2_TRACE_START_STOP,
 				   "/*---TRACE_READ---*/"
@@ -235,7 +236,8 @@ int pvr2_ioread_setup(struct pvr2_ioread *cp,struct pvr2_stream *sp)
 			}
 			cp->stream = sp;
 		}
-	} while (0); mutex_unlock(&cp->mutex);
+	} while (0);
+	mutex_unlock(&cp->mutex);
 
 	return 0;
 }
@@ -245,13 +247,15 @@ int pvr2_ioread_set_enabled(struct pvr2_ioread *cp,int fl)
 	int ret = 0;
 	if ((!fl) == (!(cp->enabled))) return ret;
 
-	mutex_lock(&cp->mutex); do {
+	mutex_lock(&cp->mutex);
+	do {
 		if (fl) {
 			ret = pvr2_ioread_start(cp);
 		} else {
 			pvr2_ioread_stop(cp);
 		}
-	} while (0); mutex_unlock(&cp->mutex);
+	} while (0);
+	mutex_unlock(&cp->mutex);
 	return ret;
 }
 
@@ -315,7 +319,8 @@ static void pvr2_ioread_filter(struct pvr2_ioread *cp)
 	// Search the stream for our synchronization key.  This is made
 	// complicated by the fact that in order to be honest with
 	// ourselves here we must search across buffer boundaries...
-	mutex_lock(&cp->mutex); while (1) {
+	mutex_lock(&cp->mutex);
+	while (1) {
 		// Ensure we have a buffer
 		if (!pvr2_ioread_get_buffer(cp)) break;
 		if (!cp->c_data_len) break;
@@ -362,7 +367,8 @@ static void pvr2_ioread_filter(struct pvr2_ioread *cp)
 		}
 
 		continue; // (for clarity)
-	} mutex_unlock(&cp->mutex);
+	}
+	mutex_unlock(&cp->mutex);
 }
 
 int pvr2_ioread_avail(struct pvr2_ioread *cp)
@@ -422,7 +428,8 @@ int pvr2_ioread_read(struct pvr2_ioread *cp,void __user *buf,unsigned int cnt)
 
 	cp->stream_running = !0;
 
-	mutex_lock(&cp->mutex); do {
+	mutex_lock(&cp->mutex);
+	do {
 
 		// Suck data out of the buffers and copy to the user
 		copied_cnt = 0;
@@ -480,7 +487,8 @@ int pvr2_ioread_read(struct pvr2_ioread *cp,void __user *buf,unsigned int cnt)
 			}
 		}
 
-	} while (0); mutex_unlock(&cp->mutex);
+	} while (0);
+	mutex_unlock(&cp->mutex);
 
 	if (!ret) {
 		if (copied_cnt) {
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index 749ad5603c9e..4d313ed4c32e 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -500,6 +500,7 @@ static const struct v4l2_ioctl_ops stk1160_ioctl_ops = {
 	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
 	.vidioc_streamon      = vb2_ioctl_streamon,
 	.vidioc_streamoff     = vb2_ioctl_streamoff,
+	.vidioc_expbuf        = vb2_ioctl_expbuf,
 
 	.vidioc_log_status  = v4l2_ctrl_log_status,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
@@ -645,7 +646,7 @@ int stk1160_vb2_setup(struct stk1160 *dev)
 
 	q = &dev->vb_vidq;
 	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR;
+	q->io_modes = VB2_READ | VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 	q->drv_priv = dev;
 	q->buf_struct_size = sizeof(struct stk1160_buffer);
 	q->ops = &stk1160_video_qops;
diff --git a/drivers/media/usb/tm6000/tm6000-video.c b/drivers/media/usb/tm6000/tm6000-video.c
index 77ce9efe1f24..fa5e8bda2ae4 100644
--- a/drivers/media/usb/tm6000/tm6000-video.c
+++ b/drivers/media/usb/tm6000/tm6000-video.c
@@ -621,7 +621,7 @@ static int tm6000_prepare_isoc(struct tm6000_core *dev)
 		    dev->isoc_in.maxsize, size);
 
 
-	if (!dev->urb_buffer && tm6000_alloc_urb_buffers(dev) < 0) {
+	if (tm6000_alloc_urb_buffers(dev) < 0) {
 		tm6000_err("cannot allocate memory for urb buffers\n");
 
 		/* call free, as some buffers might have been allocated */
@@ -714,8 +714,7 @@ static void free_buffer(struct videobuf_queue *vq, struct tm6000_buffer *buf)
 	struct tm6000_core   *dev = fh->dev;
 	unsigned long flags;
 
-	if (in_interrupt())
-		BUG();
+	BUG_ON(in_interrupt());
 
 	/* We used to wait for the buffer to finish here, but this didn't work
 	   because, as we were keeping the state as VIDEOBUF_QUEUED,
diff --git a/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c b/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
index cef7a00099ea..d52d4a8d39ad 100644
--- a/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
+++ b/drivers/media/usb/ttusb-budget/dvb-ttusb-budget.c
@@ -111,8 +111,8 @@ struct ttusb {
 	int last_filter;
 
 	u8 c;			/* transaction counter, wraps around...  */
-	fe_sec_tone_mode_t tone;
-	fe_sec_voltage_t voltage;
+	enum fe_sec_tone_mode tone;
+	enum fe_sec_voltage voltage;
 
 	int mux_state;		// 0..2 - MuxSyncWord, 3 - nMuxPacks,    4 - muxpack
 	u8 mux_npacks;
@@ -511,7 +511,8 @@ static int ttusb_update_lnb(struct ttusb *ttusb)
 	return err;
 }
 
-static int ttusb_set_voltage(struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int ttusb_set_voltage(struct dvb_frontend *fe,
+			     enum fe_sec_voltage voltage)
 {
 	struct ttusb* ttusb = (struct ttusb*) fe->dvb->priv;
 
@@ -520,7 +521,7 @@ static int ttusb_set_voltage(struct dvb_frontend* fe, fe_sec_voltage_t voltage)
 }
 
 #ifdef TTUSB_TONE
-static int ttusb_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int ttusb_set_tone(struct dvb_frontend *fe, enum fe_sec_tone_mode tone)
 {
 	struct ttusb* ttusb = (struct ttusb*) fe->dvb->priv;
 
diff --git a/drivers/media/usb/ttusb-dec/ttusb_dec.c b/drivers/media/usb/ttusb-dec/ttusb_dec.c
index 15ab584cf265..322b53a4f1dd 100644
--- a/drivers/media/usb/ttusb-dec/ttusb_dec.c
+++ b/drivers/media/usb/ttusb-dec/ttusb_dec.c
@@ -1431,8 +1431,8 @@ static int ttusb_dec_init_stb(struct ttusb_dec *dec)
 			       __func__, model);
 			return -ENOENT;
 		}
-			if (version >= 0x01770000)
-				dec->can_playback = 1;
+		if (version >= 0x01770000)
+			dec->can_playback = 1;
 	}
 	return 0;
 }
diff --git a/drivers/media/usb/ttusb-dec/ttusbdecfe.c b/drivers/media/usb/ttusb-dec/ttusbdecfe.c
index 9c29552aedec..8781335ab92f 100644
--- a/drivers/media/usb/ttusb-dec/ttusbdecfe.c
+++ b/drivers/media/usb/ttusb-dec/ttusbdecfe.c
@@ -39,7 +39,7 @@ struct ttusbdecfe_state {
 
 
 static int ttusbdecfe_dvbs_read_status(struct dvb_frontend *fe,
-	fe_status_t *status)
+				       enum fe_status *status)
 {
 	*status = FE_HAS_SIGNAL | FE_HAS_VITERBI |
 		FE_HAS_SYNC | FE_HAS_CARRIER | FE_HAS_LOCK;
@@ -48,7 +48,7 @@ static int ttusbdecfe_dvbs_read_status(struct dvb_frontend *fe,
 
 
 static int ttusbdecfe_dvbt_read_status(struct dvb_frontend *fe,
-	fe_status_t *status)
+				       enum fe_status *status)
 {
 	struct ttusbdecfe_state* state = fe->demodulator_priv;
 	u8 b[] = { 0x00, 0x00, 0x00, 0x00,
@@ -169,7 +169,8 @@ static int ttusbdecfe_dvbs_diseqc_send_master_cmd(struct dvb_frontend* fe, struc
 }
 
 
-static int ttusbdecfe_dvbs_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t tone)
+static int ttusbdecfe_dvbs_set_tone(struct dvb_frontend *fe,
+				    enum fe_sec_tone_mode tone)
 {
 	struct ttusbdecfe_state* state = (struct ttusbdecfe_state*) fe->demodulator_priv;
 
@@ -179,7 +180,8 @@ static int ttusbdecfe_dvbs_set_tone(struct dvb_frontend* fe, fe_sec_tone_mode_t
 }
 
 
-static int ttusbdecfe_dvbs_set_voltage(struct dvb_frontend* fe, fe_sec_voltage_t voltage)
+static int ttusbdecfe_dvbs_set_voltage(struct dvb_frontend *fe,
+				       enum fe_sec_voltage voltage)
 {
 	struct ttusbdecfe_state* state = (struct ttusbdecfe_state*) fe->demodulator_priv;
 
diff --git a/drivers/media/usb/usbtv/usbtv-video.c b/drivers/media/usb/usbtv/usbtv-video.c
index 9d3525f659f0..08fb0f2da64d 100644
--- a/drivers/media/usb/usbtv/usbtv-video.c
+++ b/drivers/media/usb/usbtv/usbtv-video.c
@@ -599,15 +599,18 @@ static struct v4l2_file_operations usbtv_fops = {
 };
 
 static int usbtv_queue_setup(struct vb2_queue *vq,
-	const struct v4l2_format *v4l_fmt, unsigned int *nbuffers,
+	const struct v4l2_format *fmt, unsigned int *nbuffers,
 	unsigned int *nplanes, unsigned int sizes[], void *alloc_ctxs[])
 {
 	struct usbtv *usbtv = vb2_get_drv_priv(vq);
+	unsigned size = USBTV_CHUNK * usbtv->n_chunks * 2 * sizeof(u32);
 
-	if (*nbuffers < 2)
-		*nbuffers = 2;
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
 	*nplanes = 1;
-	sizes[0] = USBTV_CHUNK * usbtv->n_chunks * 2 * sizeof(u32);
+	if (fmt && fmt->fmt.pix.sizeimage < size)
+		return -EINVAL;
+	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : size;
 
 	return 0;
 }
@@ -635,6 +638,7 @@ static int usbtv_start_streaming(struct vb2_queue *vq, unsigned int count)
 	if (usbtv->udev == NULL)
 		return -ENODEV;
 
+	usbtv->sequence = 0;
 	return usbtv_start(usbtv);
 }
 
diff --git a/drivers/media/usb/usbvision/usbvision-core.c b/drivers/media/usb/usbvision/usbvision-core.c
index 44b0c28d69b6..7c04ef697fb6 100644
--- a/drivers/media/usb/usbvision/usbvision-core.c
+++ b/drivers/media/usb/usbvision/usbvision-core.c
@@ -2390,8 +2390,8 @@ int usbvision_init_isoc(struct usb_usbvision *usbvision)
 
 	/* Submit all URBs */
 	for (buf_idx = 0; buf_idx < USBVISION_NUMSBUF; buf_idx++) {
-			err_code = usb_submit_urb(usbvision->sbuf[buf_idx].urb,
-						 GFP_KERNEL);
+		err_code = usb_submit_urb(usbvision->sbuf[buf_idx].urb,
+					 GFP_KERNEL);
 		if (err_code) {
 			dev_err(&usbvision->dev->dev,
 				"%s: usb_submit_urb(%d) failed: error %d\n",
diff --git a/drivers/media/usb/usbvision/usbvision-video.c b/drivers/media/usb/usbvision/usbvision-video.c
index 12b403e78d52..1c6d31f7c1b9 100644
--- a/drivers/media/usb/usbvision/usbvision-video.c
+++ b/drivers/media/usb/usbvision/usbvision-video.c
@@ -1061,13 +1061,24 @@ static ssize_t usbvision_read(struct file *file, char __user *buf,
 	       __func__,
 	       (unsigned long)count, frame->bytes_read);
 
-	/* For now, forget the frame if it has not been read in one shot. */
-/*	if (frame->bytes_read >= frame->scanlength) {*/ /* All data has been read */
+#if 1
+	/*
+	 * FIXME:
+	 * For now, forget the frame if it has not been read in one shot.
+	 */
+	frame->bytes_read = 0;
+
+	/* Mark it as available to be used again. */
+	frame->grabstate = frame_state_unused;
+#else
+	if (frame->bytes_read >= frame->scanlength) {
+		/* All data has been read */
 		frame->bytes_read = 0;
 
 		/* Mark it as available to be used again. */
 		frame->grabstate = frame_state_unused;
-/*	} */
+	}
+#endif
 
 	return count;
 }
diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driver.c
index 5970dd6a1c1c..4b5b3e8fb7d3 100644
--- a/drivers/media/usb/uvc/uvc_driver.c
+++ b/drivers/media/usb/uvc/uvc_driver.c
@@ -1967,8 +1967,6 @@ static void uvc_disconnect(struct usb_interface *intf)
 	    UVC_SC_VIDEOSTREAMING)
 		return;
 
-	dev->state |= UVC_DEV_DISCONNECTED;
-
 	uvc_unregister_video(dev);
 }
 
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index 87a19f33e460..f16b9b42689d 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -270,6 +270,18 @@ int uvc_queue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf)
 	return ret;
 }
 
+int uvc_export_buffer(struct uvc_video_queue *queue,
+		      struct v4l2_exportbuffer *exp)
+{
+	int ret;
+
+	mutex_lock(&queue->mutex);
+	ret = vb2_expbuf(&queue->queue, exp);
+	mutex_unlock(&queue->mutex);
+
+	return ret;
+}
+
 int uvc_dequeue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf,
 		       int nonblocking)
 {
diff --git a/drivers/media/usb/uvc/uvc_v4l2.c b/drivers/media/usb/uvc/uvc_v4l2.c
index c4b1ac6750d8..2764f43607c1 100644
--- a/drivers/media/usb/uvc/uvc_v4l2.c
+++ b/drivers/media/usb/uvc/uvc_v4l2.c
@@ -483,9 +483,6 @@ static int uvc_v4l2_open(struct file *file)
 	uvc_trace(UVC_TRACE_CALLS, "uvc_v4l2_open\n");
 	stream = video_drvdata(file);
 
-	if (stream->dev->state & UVC_DEV_DISCONNECTED)
-		return -ENODEV;
-
 	ret = usb_autopm_get_interface(stream->dev->intf);
 	if (ret < 0)
 		return ret;
@@ -723,6 +720,18 @@ static int uvc_ioctl_qbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 	return uvc_queue_buffer(&stream->queue, buf);
 }
 
+static int uvc_ioctl_expbuf(struct file *file, void *fh,
+			    struct v4l2_exportbuffer *exp)
+{
+	struct uvc_fh *handle = fh;
+	struct uvc_streaming *stream = handle->stream;
+
+	if (!uvc_has_privileges(handle))
+		return -EBUSY;
+
+	return uvc_export_buffer(&stream->queue, exp);
+}
+
 static int uvc_ioctl_dqbuf(struct file *file, void *fh, struct v4l2_buffer *buf)
 {
 	struct uvc_fh *handle = fh;
@@ -1478,6 +1487,7 @@ const struct v4l2_ioctl_ops uvc_ioctl_ops = {
 	.vidioc_reqbufs = uvc_ioctl_reqbufs,
 	.vidioc_querybuf = uvc_ioctl_querybuf,
 	.vidioc_qbuf = uvc_ioctl_qbuf,
+	.vidioc_expbuf = uvc_ioctl_expbuf,
 	.vidioc_dqbuf = uvc_ioctl_dqbuf,
 	.vidioc_create_bufs = uvc_ioctl_create_bufs,
 	.vidioc_streamon = uvc_ioctl_streamon,
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 20ccc9d315dc..f839654ea436 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -119,6 +119,14 @@ static void uvc_fixup_video_ctrl(struct uvc_streaming *stream,
 		ctrl->dwMaxVideoFrameSize =
 			frame->dwMaxVideoFrameBufferSize;
 
+	/* The "TOSHIBA Web Camera - 5M" Chicony device (04f2:b50b) seems to
+	 * compute the bandwidth on 16 bits and erroneously sign-extend it to
+	 * 32 bits, resulting in a huge bandwidth value. Detect and fix that
+	 * condition by setting the 16 MSBs to 0 when they're all equal to 1.
+	 */
+	if ((ctrl->dwMaxPayloadTransferSize & 0xffff0000) == 0xffff0000)
+		ctrl->dwMaxPayloadTransferSize &= ~0xffff0000;
+
 	if (!(format->flags & UVC_FMT_FLAG_COMPRESSED) &&
 	    stream->dev->quirks & UVC_QUIRK_FIX_BANDWIDTH &&
 	    stream->intf->num_altsetting > 1) {
diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h
index 1b594c203992..816dd1a0fd81 100644
--- a/drivers/media/usb/uvc/uvcvideo.h
+++ b/drivers/media/usb/uvc/uvcvideo.h
@@ -517,10 +517,6 @@ struct uvc_streaming {
 	} clock;
 };
 
-enum uvc_device_state {
-	UVC_DEV_DISCONNECTED = 1,
-};
-
 struct uvc_device {
 	struct usb_device *udev;
 	struct usb_interface *intf;
@@ -529,7 +525,6 @@ struct uvc_device {
 	int intfnum;
 	char name[32];
 
-	enum uvc_device_state state;
 	struct mutex lock;		/* Protects users */
 	unsigned int users;
 	atomic_t nmappings;
@@ -635,6 +630,8 @@ extern int uvc_create_buffers(struct uvc_video_queue *queue,
 		struct v4l2_create_buffers *v4l2_cb);
 extern int uvc_queue_buffer(struct uvc_video_queue *queue,
 		struct v4l2_buffer *v4l2_buf);
+extern int uvc_export_buffer(struct uvc_video_queue *queue,
+		struct v4l2_exportbuffer *exp);
 extern int uvc_dequeue_buffer(struct uvc_video_queue *queue,
 		struct v4l2_buffer *v4l2_buf, int nonblocking);
 extern int uvc_queue_streamon(struct uvc_video_queue *queue,
diff --git a/drivers/media/usb/zr364xx/zr364xx.c b/drivers/media/usb/zr364xx/zr364xx.c
index ca850316d379..7433ba5c4bad 100644
--- a/drivers/media/usb/zr364xx/zr364xx.c
+++ b/drivers/media/usb/zr364xx/zr364xx.c
@@ -377,8 +377,7 @@ static void free_buffer(struct videobuf_queue *vq, struct zr364xx_buffer *buf)
 {
 	_DBG("%s\n", __func__);
 
-	if (in_interrupt())
-		BUG();
+	BUG_ON(in_interrupt());
 
 	videobuf_vmalloc_free(&buf->vb);
 	buf->vb.state = VIDEOBUF_NEEDS_INIT;
diff --git a/drivers/media/v4l2-core/Kconfig b/drivers/media/v4l2-core/Kconfig
index ba7e21a73023..b4b022933e29 100644
--- a/drivers/media/v4l2-core/Kconfig
+++ b/drivers/media/v4l2-core/Kconfig
@@ -44,6 +44,17 @@ config V4L2_MEM2MEM_DEV
         tristate
         depends on VIDEOBUF2_CORE
 
+# Used by LED subsystem flash drivers
+config V4L2_FLASH_LED_CLASS
+	tristate "V4L2 flash API for LED flash class devices"
+	depends on VIDEO_V4L2_SUBDEV_API
+	depends on LEDS_CLASS_FLASH
+	---help---
+	  Say Y here to enable V4L2 flash API support for LED flash
+	  class drivers.
+
+	  When in doubt, say N.
+
 # Used by drivers that need Videobuf modules
 config VIDEOBUF_GEN
 	tristate
@@ -89,7 +100,7 @@ config VIDEOBUF2_VMALLOC
 
 config VIDEOBUF2_DMA_SG
 	tristate
-	#depends on HAS_DMA
+	depends on HAS_DMA
 	select VIDEOBUF2_CORE
 	select VIDEOBUF2_MEMOPS
 
diff --git a/drivers/media/v4l2-core/Makefile b/drivers/media/v4l2-core/Makefile
index 63d29f27538c..dc3de00d68b5 100644
--- a/drivers/media/v4l2-core/Makefile
+++ b/drivers/media/v4l2-core/Makefile
@@ -22,6 +22,8 @@ obj-$(CONFIG_VIDEO_TUNER) += tuner.o
 
 obj-$(CONFIG_V4L2_MEM2MEM_DEV) += v4l2-mem2mem.o
 
+obj-$(CONFIG_V4L2_FLASH_LED_CLASS) += v4l2-flash-led-class.o
+
 obj-$(CONFIG_VIDEOBUF_GEN) += videobuf-core.o
 obj-$(CONFIG_VIDEOBUF_DMA_SG) += videobuf-dma-sg.o
 obj-$(CONFIG_VIDEOBUF_DMA_CONTIG) += videobuf-dma-contig.o
diff --git a/drivers/media/v4l2-core/v4l2-async.c b/drivers/media/v4l2-core/v4l2-async.c
index 85a6a34128a8..5bada202b2d3 100644
--- a/drivers/media/v4l2-core/v4l2-async.c
+++ b/drivers/media/v4l2-core/v4l2-async.c
@@ -22,10 +22,10 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 
-static bool match_i2c(struct device *dev, struct v4l2_async_subdev *asd)
+static bool match_i2c(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
 {
 #if IS_ENABLED(CONFIG_I2C)
-	struct i2c_client *client = i2c_verify_client(dev);
+	struct i2c_client *client = i2c_verify_client(sd->dev);
 	return client &&
 		asd->match.i2c.adapter_id == client->adapter->nr &&
 		asd->match.i2c.address == client->addr;
@@ -34,14 +34,24 @@ static bool match_i2c(struct device *dev, struct v4l2_async_subdev *asd)
 #endif
 }
 
-static bool match_devname(struct device *dev, struct v4l2_async_subdev *asd)
+static bool match_devname(struct v4l2_subdev *sd,
+			  struct v4l2_async_subdev *asd)
 {
-	return !strcmp(asd->match.device_name.name, dev_name(dev));
+	return !strcmp(asd->match.device_name.name, dev_name(sd->dev));
 }
 
-static bool match_of(struct device *dev, struct v4l2_async_subdev *asd)
+static bool match_of(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
 {
-	return dev->of_node == asd->match.of.node;
+	return sd->of_node == asd->match.of.node;
+}
+
+static bool match_custom(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
+{
+	if (!asd->match.custom.match)
+		/* Match always */
+		return true;
+
+	return asd->match.custom.match(sd->dev, asd);
 }
 
 static LIST_HEAD(subdev_list);
@@ -51,17 +61,14 @@ static DEFINE_MUTEX(list_lock);
 static struct v4l2_async_subdev *v4l2_async_belongs(struct v4l2_async_notifier *notifier,
 						    struct v4l2_subdev *sd)
 {
+	bool (*match)(struct v4l2_subdev *, struct v4l2_async_subdev *);
 	struct v4l2_async_subdev *asd;
-	bool (*match)(struct device *, struct v4l2_async_subdev *);
 
 	list_for_each_entry(asd, &notifier->waiting, list) {
 		/* bus_type has been verified valid before */
 		switch (asd->match_type) {
 		case V4L2_ASYNC_MATCH_CUSTOM:
-			match = asd->match.custom.match;
-			if (!match)
-				/* Match always */
-				return asd;
+			match = match_custom;
 			break;
 		case V4L2_ASYNC_MATCH_DEVNAME:
 			match = match_devname;
@@ -79,7 +86,7 @@ static struct v4l2_async_subdev *v4l2_async_belongs(struct v4l2_async_notifier *
 		}
 
 		/* match cannot be NULL here */
-		if (match(sd->dev, asd))
+		if (match(sd, asd))
 			return asd;
 	}
 
@@ -266,6 +273,14 @@ int v4l2_async_register_subdev(struct v4l2_subdev *sd)
 {
 	struct v4l2_async_notifier *notifier;
 
+	/*
+	 * No reference taken. The reference is held by the device
+	 * (struct v4l2_subdev.dev), and async sub-device does not
+	 * exist independently of the device at any point of time.
+	 */
+	if (!sd->of_node && sd->dev)
+		sd->of_node = sd->dev->of_node;
+
 	mutex_lock(&list_lock);
 
 	INIT_LIST_HEAD(&sd->async_list);
diff --git a/drivers/media/v4l2-core/v4l2-dv-timings.c b/drivers/media/v4l2-core/v4l2-dv-timings.c
index c0e96382feba..04dc71e3ebf0 100644
--- a/drivers/media/v4l2-core/v4l2-dv-timings.c
+++ b/drivers/media/v4l2-core/v4l2-dv-timings.c
@@ -25,6 +25,7 @@
 #include <linux/videodev2.h>
 #include <linux/v4l2-dv-timings.h>
 #include <media/v4l2-dv-timings.h>
+#include <linux/math64.h>
 
 MODULE_AUTHOR("Hans Verkuil");
 MODULE_DESCRIPTION("V4L2 DV Timings Helper Functions");
@@ -261,6 +262,8 @@ void v4l2_print_dv_timings(const char *dev_prefix, const char *prefix,
 
 	htot = V4L2_DV_BT_FRAME_WIDTH(bt);
 	vtot = V4L2_DV_BT_FRAME_HEIGHT(bt);
+	if (bt->interlaced)
+		vtot /= 2;
 
 	if (prefix == NULL)
 		prefix = "";
@@ -281,6 +284,11 @@ void v4l2_print_dv_timings(const char *dev_prefix, const char *prefix,
 			dev_prefix, bt->vfrontporch,
 			(bt->polarities & V4L2_DV_VSYNC_POS_POL) ? "+" : "-",
 			bt->vsync, bt->vbackporch);
+	if (bt->interlaced)
+		pr_info("%s: vertical bottom field: fp = %u, %ssync = %u, bp = %u\n",
+			dev_prefix, bt->il_vfrontporch,
+			(bt->polarities & V4L2_DV_VSYNC_POS_POL) ? "+" : "-",
+			bt->il_vsync, bt->il_vbackporch);
 	pr_info("%s: pixelclock: %llu\n", dev_prefix, bt->pixelclock);
 	pr_info("%s: flags (0x%x):%s%s%s%s%s\n", dev_prefix, bt->flags,
 			(bt->flags & V4L2_DV_FL_REDUCED_BLANKING) ?
@@ -313,6 +321,7 @@ EXPORT_SYMBOL_GPL(v4l2_print_dv_timings);
 #define CVT_MIN_V_BPORCH	7	/* lines */
 #define CVT_MIN_V_PORCH_RND	3	/* lines */
 #define CVT_MIN_VSYNC_BP	550	/* min time of vsync + back porch (us) */
+#define CVT_HSYNC_PERCENT       8       /* nominal hsync as percentage of line */
 
 /* Normal blanking for CVT uses GTF to calculate horizontal blanking */
 #define CVT_CELL_GRAN		8	/* character cell granularity */
@@ -337,6 +346,7 @@ EXPORT_SYMBOL_GPL(v4l2_print_dv_timings);
  * @vsync - the height of the vertical sync in lines.
  * @polarities - the horizontal and vertical polarities (same as struct
  *		v4l2_bt_timings polarities).
+ * @interlaced - if this flag is true, it indicates interlaced format
  * @fmt - the resulting timings.
  *
  * This function will attempt to detect if the given values correspond to a
@@ -348,7 +358,7 @@ EXPORT_SYMBOL_GPL(v4l2_print_dv_timings);
  * detection function.
  */
 bool v4l2_detect_cvt(unsigned frame_height, unsigned hfreq, unsigned vsync,
-		u32 polarities, struct v4l2_dv_timings *fmt)
+		u32 polarities, bool interlaced, struct v4l2_dv_timings *fmt)
 {
 	int  v_fp, v_bp, h_fp, h_bp, hsync;
 	int  frame_width, image_height, image_width;
@@ -365,22 +375,32 @@ bool v4l2_detect_cvt(unsigned frame_height, unsigned hfreq, unsigned vsync,
 	else
 		return false;
 
+	if (hfreq == 0)
+		return false;
+
 	/* Vertical */
 	if (reduced_blanking) {
 		v_fp = CVT_RB_V_FPORCH;
-		v_bp = (CVT_RB_MIN_V_BLANK * hfreq + 1999999) / 1000000;
+		v_bp = (CVT_RB_MIN_V_BLANK * hfreq) / 1000000 + 1;
 		v_bp -= vsync + v_fp;
 
 		if (v_bp < CVT_RB_MIN_V_BPORCH)
 			v_bp = CVT_RB_MIN_V_BPORCH;
 	} else {
 		v_fp = CVT_MIN_V_PORCH_RND;
-		v_bp = (CVT_MIN_VSYNC_BP * hfreq + 1999999) / 1000000 - vsync;
+		v_bp = (CVT_MIN_VSYNC_BP * hfreq) / 1000000 + 1 - vsync;
 
 		if (v_bp < CVT_MIN_V_BPORCH)
 			v_bp = CVT_MIN_V_BPORCH;
 	}
-	image_height = (frame_height - v_fp - vsync - v_bp + 1) & ~0x1;
+
+	if (interlaced)
+		image_height = (frame_height - 2 * v_fp - 2 * vsync - 2 * v_bp) & ~0x1;
+	else
+		image_height = (frame_height - v_fp - vsync - v_bp + 1) & ~0x1;
+
+	if (image_height < 0)
+		return false;
 
 	/* Aspect ratio based on vsync */
 	switch (vsync) {
@@ -436,8 +456,8 @@ bool v4l2_detect_cvt(unsigned frame_height, unsigned hfreq, unsigned vsync,
 		h_bp = h_blank / 2;
 		frame_width = image_width + h_blank;
 
-		hsync = (frame_width * 8 + 50) / 100;
-		hsync = hsync - hsync % CVT_CELL_GRAN;
+		hsync = frame_width * CVT_HSYNC_PERCENT / 100;
+		hsync = (hsync / CVT_CELL_GRAN) * CVT_CELL_GRAN;
 		h_fp = h_blank - hsync - h_bp;
 	}
 
@@ -450,11 +470,27 @@ bool v4l2_detect_cvt(unsigned frame_height, unsigned hfreq, unsigned vsync,
 	fmt->bt.hsync = hsync;
 	fmt->bt.vsync = vsync;
 	fmt->bt.hbackporch = frame_width - image_width - h_fp - hsync;
-	fmt->bt.vbackporch = frame_height - image_height - v_fp - vsync;
+
+	if (!interlaced) {
+		fmt->bt.vbackporch = frame_height - image_height - v_fp - vsync;
+		fmt->bt.interlaced = V4L2_DV_PROGRESSIVE;
+	} else {
+		fmt->bt.vbackporch = (frame_height - image_height - 2 * v_fp -
+				      2 * vsync) / 2;
+		fmt->bt.il_vbackporch = frame_height - image_height - 2 * v_fp -
+					2 * vsync - fmt->bt.vbackporch;
+		fmt->bt.il_vfrontporch = v_fp;
+		fmt->bt.il_vsync = vsync;
+		fmt->bt.flags |= V4L2_DV_FL_HALF_LINE;
+		fmt->bt.interlaced = V4L2_DV_INTERLACED;
+	}
+
 	fmt->bt.pixelclock = pix_clk;
 	fmt->bt.standards = V4L2_DV_BT_STD_CVT;
+
 	if (reduced_blanking)
 		fmt->bt.flags |= V4L2_DV_FL_REDUCED_BLANKING;
+
 	return true;
 }
 EXPORT_SYMBOL_GPL(v4l2_detect_cvt);
@@ -493,6 +529,7 @@ EXPORT_SYMBOL_GPL(v4l2_detect_cvt);
  * @vsync - the height of the vertical sync in lines.
  * @polarities - the horizontal and vertical polarities (same as struct
  *		v4l2_bt_timings polarities).
+ * @interlaced - if this flag is true, it indicates interlaced format
  * @aspect - preferred aspect ratio. GTF has no method of determining the
  *		aspect ratio in order to derive the image width from the
  *		image height, so it has to be passed explicitly. Usually
@@ -508,6 +545,7 @@ bool v4l2_detect_gtf(unsigned frame_height,
 		unsigned hfreq,
 		unsigned vsync,
 		u32 polarities,
+		bool interlaced,
 		struct v4l2_fract aspect,
 		struct v4l2_dv_timings *fmt)
 {
@@ -527,10 +565,19 @@ bool v4l2_detect_gtf(unsigned frame_height,
 	else
 		return false;
 
+	if (hfreq == 0)
+		return false;
+
 	/* Vertical */
 	v_fp = GTF_V_FP;
-	v_bp = (GTF_MIN_VSYNC_BP * hfreq + 999999) / 1000000 - vsync;
-	image_height = (frame_height - v_fp - vsync - v_bp + 1) & ~0x1;
+	v_bp = (GTF_MIN_VSYNC_BP * hfreq + 500000) / 1000000 - vsync;
+	if (interlaced)
+		image_height = (frame_height - 2 * v_fp - 2 * vsync - 2 * v_bp) & ~0x1;
+	else
+		image_height = (frame_height - v_fp - vsync - v_bp + 1) & ~0x1;
+
+	if (image_height < 0)
+		return false;
 
 	if (aspect.numerator == 0 || aspect.denominator == 0) {
 		aspect.numerator = 16;
@@ -540,25 +587,35 @@ bool v4l2_detect_gtf(unsigned frame_height,
 	image_width = (image_width + GTF_CELL_GRAN/2) & ~(GTF_CELL_GRAN - 1);
 
 	/* Horizontal */
-	if (default_gtf)
-		h_blank = ((image_width * GTF_D_C_PRIME * hfreq) -
-					(image_width * GTF_D_M_PRIME * 1000) +
-			(hfreq * (100 - GTF_D_C_PRIME) + GTF_D_M_PRIME * 1000) / 2) /
-			(hfreq * (100 - GTF_D_C_PRIME) + GTF_D_M_PRIME * 1000);
-	else
-		h_blank = ((image_width * GTF_S_C_PRIME * hfreq) -
-					(image_width * GTF_S_M_PRIME * 1000) +
-			(hfreq * (100 - GTF_S_C_PRIME) + GTF_S_M_PRIME * 1000) / 2) /
-			(hfreq * (100 - GTF_S_C_PRIME) + GTF_S_M_PRIME * 1000);
+	if (default_gtf) {
+		u64 num;
+		u32 den;
+
+		num = ((image_width * GTF_D_C_PRIME * (u64)hfreq) -
+		      ((u64)image_width * GTF_D_M_PRIME * 1000));
+		den = (hfreq * (100 - GTF_D_C_PRIME) + GTF_D_M_PRIME * 1000) *
+		      (2 * GTF_CELL_GRAN);
+		h_blank = div_u64((num + (den >> 1)), den);
+		h_blank *= (2 * GTF_CELL_GRAN);
+	} else {
+		u64 num;
+		u32 den;
+
+		num = ((image_width * GTF_S_C_PRIME * (u64)hfreq) -
+		      ((u64)image_width * GTF_S_M_PRIME * 1000));
+		den = (hfreq * (100 - GTF_S_C_PRIME) + GTF_S_M_PRIME * 1000) *
+		      (2 * GTF_CELL_GRAN);
+		h_blank = div_u64((num + (den >> 1)), den);
+		h_blank *= (2 * GTF_CELL_GRAN);
+	}
 
-	h_blank = h_blank - h_blank % (2 * GTF_CELL_GRAN);
 	frame_width = image_width + h_blank;
 
 	pix_clk = (image_width + h_blank) * hfreq;
 	pix_clk = pix_clk / GTF_PXL_CLK_GRAN * GTF_PXL_CLK_GRAN;
 
 	hsync = (frame_width * 8 + 50) / 100;
-	hsync = hsync - hsync % GTF_CELL_GRAN;
+	hsync = ((hsync + GTF_CELL_GRAN / 2) / GTF_CELL_GRAN) * GTF_CELL_GRAN;
 
 	h_fp = h_blank / 2 - hsync;
 
@@ -571,11 +628,27 @@ bool v4l2_detect_gtf(unsigned frame_height,
 	fmt->bt.hsync = hsync;
 	fmt->bt.vsync = vsync;
 	fmt->bt.hbackporch = frame_width - image_width - h_fp - hsync;
-	fmt->bt.vbackporch = frame_height - image_height - v_fp - vsync;
+
+	if (!interlaced) {
+		fmt->bt.vbackporch = frame_height - image_height - v_fp - vsync;
+		fmt->bt.interlaced = V4L2_DV_PROGRESSIVE;
+	} else {
+		fmt->bt.vbackporch = (frame_height - image_height - 2 * v_fp -
+				      2 * vsync) / 2;
+		fmt->bt.il_vbackporch = frame_height - image_height - 2 * v_fp -
+					2 * vsync - fmt->bt.vbackporch;
+		fmt->bt.il_vfrontporch = v_fp;
+		fmt->bt.il_vsync = vsync;
+		fmt->bt.flags |= V4L2_DV_FL_HALF_LINE;
+		fmt->bt.interlaced = V4L2_DV_INTERLACED;
+	}
+
 	fmt->bt.pixelclock = pix_clk;
 	fmt->bt.standards = V4L2_DV_BT_STD_GTF;
+
 	if (!default_gtf)
 		fmt->bt.flags |= V4L2_DV_FL_REDUCED_BLANKING;
+
 	return true;
 }
 EXPORT_SYMBOL_GPL(v4l2_detect_gtf);
diff --git a/drivers/media/v4l2-core/v4l2-flash-led-class.c b/drivers/media/v4l2-core/v4l2-flash-led-class.c
new file mode 100644
index 000000000000..5bdfb8d5263a
--- /dev/null
+++ b/drivers/media/v4l2-core/v4l2-flash-led-class.c
@@ -0,0 +1,710 @@
+/*
+ * V4L2 flash LED sub-device registration helpers.
+ *
+ *	Copyright (C) 2015 Samsung Electronics Co., Ltd
+ *	Author: Jacek Anaszewski <j.anaszewski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/led-class-flash.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-flash-led-class.h>
+
+#define has_flash_op(v4l2_flash, op)				\
+	(v4l2_flash && v4l2_flash->ops->op)
+
+#define call_flash_op(v4l2_flash, op, arg)			\
+		(has_flash_op(v4l2_flash, op) ?			\
+			v4l2_flash->ops->op(v4l2_flash, arg) :	\
+			-EINVAL)
+
+enum ctrl_init_data_id {
+	LED_MODE,
+	TORCH_INTENSITY,
+	FLASH_INTENSITY,
+	INDICATOR_INTENSITY,
+	FLASH_TIMEOUT,
+	STROBE_SOURCE,
+	/*
+	 * Only above values are applicable to
+	 * the 'ctrls' array in the struct v4l2_flash.
+	 */
+	FLASH_STROBE,
+	STROBE_STOP,
+	STROBE_STATUS,
+	FLASH_FAULT,
+	NUM_FLASH_CTRLS,
+};
+
+static enum led_brightness __intensity_to_led_brightness(
+					struct v4l2_ctrl *ctrl, s32 intensity)
+{
+	intensity -= ctrl->minimum;
+	intensity /= (u32) ctrl->step;
+
+	/*
+	 * Indicator LEDs, unlike torch LEDs, are turned on/off basing on
+	 * the state of V4L2_CID_FLASH_INDICATOR_INTENSITY control only.
+	 * Therefore it must be possible to set it to 0 level which in
+	 * the LED subsystem reflects LED_OFF state.
+	 */
+	if (ctrl->minimum)
+		++intensity;
+
+	return intensity;
+}
+
+static s32 __led_brightness_to_intensity(struct v4l2_ctrl *ctrl,
+					 enum led_brightness brightness)
+{
+	/*
+	 * Indicator LEDs, unlike torch LEDs, are turned on/off basing on
+	 * the state of V4L2_CID_FLASH_INDICATOR_INTENSITY control only.
+	 * Do not decrement brightness read from the LED subsystem for
+	 * indicator LED as it may equal 0. For torch LEDs this function
+	 * is called only when V4L2_FLASH_LED_MODE_TORCH is set and the
+	 * brightness read is guaranteed to be greater than 0. In the mode
+	 * V4L2_FLASH_LED_MODE_NONE the cached torch intensity value is used.
+	 */
+	if (ctrl->id != V4L2_CID_FLASH_INDICATOR_INTENSITY)
+		--brightness;
+
+	return (brightness * ctrl->step) + ctrl->minimum;
+}
+
+static void v4l2_flash_set_led_brightness(struct v4l2_flash *v4l2_flash,
+					struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_ctrl **ctrls = v4l2_flash->ctrls;
+	enum led_brightness brightness;
+
+	if (has_flash_op(v4l2_flash, intensity_to_led_brightness))
+		brightness = call_flash_op(v4l2_flash,
+					intensity_to_led_brightness,
+					ctrl->val);
+	else
+		brightness = __intensity_to_led_brightness(ctrl, ctrl->val);
+	/*
+	 * In case a LED Flash class driver provides ops for custom
+	 * brightness <-> intensity conversion, it also must have defined
+	 * related v4l2 control step == 1. In such a case a backward conversion
+	 * from led brightness to v4l2 intensity is required to find out the
+	 * the aligned intensity value.
+	 */
+	if (has_flash_op(v4l2_flash, led_brightness_to_intensity))
+		ctrl->val = call_flash_op(v4l2_flash,
+					led_brightness_to_intensity,
+					brightness);
+
+	if (ctrl == ctrls[TORCH_INTENSITY]) {
+		if (ctrls[LED_MODE]->val != V4L2_FLASH_LED_MODE_TORCH)
+			return;
+
+		led_set_brightness(&v4l2_flash->fled_cdev->led_cdev,
+					brightness);
+	} else {
+		led_set_brightness(&v4l2_flash->iled_cdev->led_cdev,
+					brightness);
+	}
+}
+
+static int v4l2_flash_update_led_brightness(struct v4l2_flash *v4l2_flash,
+					struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_ctrl **ctrls = v4l2_flash->ctrls;
+	struct led_classdev *led_cdev;
+	int ret;
+
+	if (ctrl == ctrls[TORCH_INTENSITY]) {
+		/*
+		 * Update torch brightness only if in TORCH_MODE. In other modes
+		 * torch led is turned off, which would spuriously inform the
+		 * user space that V4L2_CID_FLASH_TORCH_INTENSITY control value
+		 * has changed to 0.
+		 */
+		if (ctrls[LED_MODE]->val != V4L2_FLASH_LED_MODE_TORCH)
+			return 0;
+		led_cdev = &v4l2_flash->fled_cdev->led_cdev;
+	} else {
+		led_cdev = &v4l2_flash->iled_cdev->led_cdev;
+	}
+
+	ret = led_update_brightness(led_cdev);
+	if (ret < 0)
+		return ret;
+
+	if (has_flash_op(v4l2_flash, led_brightness_to_intensity))
+		ctrl->val = call_flash_op(v4l2_flash,
+						led_brightness_to_intensity,
+						led_cdev->brightness);
+	else
+		ctrl->val = __led_brightness_to_intensity(ctrl,
+						led_cdev->brightness);
+
+	return 0;
+}
+
+static int v4l2_flash_g_volatile_ctrl(struct v4l2_ctrl *c)
+{
+	struct v4l2_flash *v4l2_flash = v4l2_ctrl_to_v4l2_flash(c);
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	bool is_strobing;
+	int ret;
+
+	switch (c->id) {
+	case V4L2_CID_FLASH_TORCH_INTENSITY:
+	case V4L2_CID_FLASH_INDICATOR_INTENSITY:
+		return v4l2_flash_update_led_brightness(v4l2_flash, c);
+	case V4L2_CID_FLASH_INTENSITY:
+		ret = led_update_flash_brightness(fled_cdev);
+		if (ret < 0)
+			return ret;
+		/*
+		 * No conversion is needed as LED Flash class also uses
+		 * microamperes for flash intensity units.
+		 */
+		c->val = fled_cdev->brightness.val;
+		return 0;
+	case V4L2_CID_FLASH_STROBE_STATUS:
+		ret = led_get_flash_strobe(fled_cdev, &is_strobing);
+		if (ret < 0)
+			return ret;
+		c->val = is_strobing;
+		return 0;
+	case V4L2_CID_FLASH_FAULT:
+		/* LED faults map directly to V4L2 flash faults */
+		return led_get_flash_fault(fled_cdev, &c->val);
+	default:
+		return -EINVAL;
+	}
+}
+
+static bool __software_strobe_mode_inactive(struct v4l2_ctrl **ctrls)
+{
+	return ((ctrls[LED_MODE]->val != V4L2_FLASH_LED_MODE_FLASH) ||
+		(ctrls[STROBE_SOURCE] && (ctrls[STROBE_SOURCE]->val !=
+				V4L2_FLASH_STROBE_SOURCE_SOFTWARE)));
+}
+
+static int v4l2_flash_s_ctrl(struct v4l2_ctrl *c)
+{
+	struct v4l2_flash *v4l2_flash = v4l2_ctrl_to_v4l2_flash(c);
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	struct led_classdev *led_cdev = &fled_cdev->led_cdev;
+	struct v4l2_ctrl **ctrls = v4l2_flash->ctrls;
+	bool external_strobe;
+	int ret = 0;
+
+	switch (c->id) {
+	case V4L2_CID_FLASH_LED_MODE:
+		switch (c->val) {
+		case V4L2_FLASH_LED_MODE_NONE:
+			led_set_brightness(led_cdev, LED_OFF);
+			return led_set_flash_strobe(fled_cdev, false);
+		case V4L2_FLASH_LED_MODE_FLASH:
+			/* Turn the torch LED off */
+			led_set_brightness(led_cdev, LED_OFF);
+			if (ctrls[STROBE_SOURCE]) {
+				external_strobe = (ctrls[STROBE_SOURCE]->val ==
+					V4L2_FLASH_STROBE_SOURCE_EXTERNAL);
+
+				ret = call_flash_op(v4l2_flash,
+						external_strobe_set,
+						external_strobe);
+			}
+			return ret;
+		case V4L2_FLASH_LED_MODE_TORCH:
+			if (ctrls[STROBE_SOURCE]) {
+				ret = call_flash_op(v4l2_flash,
+						external_strobe_set,
+						false);
+				if (ret < 0)
+					return ret;
+			}
+			/* Stop flash strobing */
+			ret = led_set_flash_strobe(fled_cdev, false);
+			if (ret < 0)
+				return ret;
+
+			v4l2_flash_set_led_brightness(v4l2_flash,
+							ctrls[TORCH_INTENSITY]);
+			return 0;
+		}
+		break;
+	case V4L2_CID_FLASH_STROBE_SOURCE:
+		external_strobe = (c->val == V4L2_FLASH_STROBE_SOURCE_EXTERNAL);
+		/*
+		 * For some hardware arrangements setting strobe source may
+		 * affect torch mode. Therefore, if not in the flash mode,
+		 * cache only this setting. It will be applied upon switching
+		 * to flash mode.
+		 */
+		if (ctrls[LED_MODE]->val != V4L2_FLASH_LED_MODE_FLASH)
+			return 0;
+
+		return call_flash_op(v4l2_flash, external_strobe_set,
+					external_strobe);
+	case V4L2_CID_FLASH_STROBE:
+		if (__software_strobe_mode_inactive(ctrls))
+			return -EBUSY;
+		return led_set_flash_strobe(fled_cdev, true);
+	case V4L2_CID_FLASH_STROBE_STOP:
+		if (__software_strobe_mode_inactive(ctrls))
+			return -EBUSY;
+		return led_set_flash_strobe(fled_cdev, false);
+	case V4L2_CID_FLASH_TIMEOUT:
+		/*
+		 * No conversion is needed as LED Flash class also uses
+		 * microseconds for flash timeout units.
+		 */
+		return led_set_flash_timeout(fled_cdev, c->val);
+	case V4L2_CID_FLASH_INTENSITY:
+		/*
+		 * No conversion is needed as LED Flash class also uses
+		 * microamperes for flash intensity units.
+		 */
+		return led_set_flash_brightness(fled_cdev, c->val);
+	case V4L2_CID_FLASH_TORCH_INTENSITY:
+	case V4L2_CID_FLASH_INDICATOR_INTENSITY:
+		v4l2_flash_set_led_brightness(v4l2_flash, c);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops v4l2_flash_ctrl_ops = {
+	.g_volatile_ctrl = v4l2_flash_g_volatile_ctrl,
+	.s_ctrl = v4l2_flash_s_ctrl,
+};
+
+static void __lfs_to_v4l2_ctrl_config(struct led_flash_setting *s,
+				struct v4l2_ctrl_config *c)
+{
+	c->min = s->min;
+	c->max = s->max;
+	c->step = s->step;
+	c->def = s->val;
+}
+
+static void __fill_ctrl_init_data(struct v4l2_flash *v4l2_flash,
+			  struct v4l2_flash_config *flash_cfg,
+			  struct v4l2_flash_ctrl_data *ctrl_init_data)
+{
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	const struct led_flash_ops *fled_cdev_ops = fled_cdev->ops;
+	struct led_classdev *led_cdev = &fled_cdev->led_cdev;
+	struct v4l2_ctrl_config *ctrl_cfg;
+	u32 mask;
+
+	/* Init FLASH_FAULT ctrl data */
+	if (flash_cfg->flash_faults) {
+		ctrl_init_data[FLASH_FAULT].cid = V4L2_CID_FLASH_FAULT;
+		ctrl_cfg = &ctrl_init_data[FLASH_FAULT].config;
+		ctrl_cfg->id = V4L2_CID_FLASH_FAULT;
+		ctrl_cfg->max = flash_cfg->flash_faults;
+		ctrl_cfg->flags = V4L2_CTRL_FLAG_VOLATILE |
+				  V4L2_CTRL_FLAG_READ_ONLY;
+	}
+
+	/* Init FLASH_LED_MODE ctrl data */
+	mask = 1 << V4L2_FLASH_LED_MODE_NONE |
+	       1 << V4L2_FLASH_LED_MODE_TORCH;
+	if (led_cdev->flags & LED_DEV_CAP_FLASH)
+		mask |= 1 << V4L2_FLASH_LED_MODE_FLASH;
+
+	ctrl_init_data[LED_MODE].cid = V4L2_CID_FLASH_LED_MODE;
+	ctrl_cfg = &ctrl_init_data[LED_MODE].config;
+	ctrl_cfg->id = V4L2_CID_FLASH_LED_MODE;
+	ctrl_cfg->max = V4L2_FLASH_LED_MODE_TORCH;
+	ctrl_cfg->menu_skip_mask = ~mask;
+	ctrl_cfg->def = V4L2_FLASH_LED_MODE_NONE;
+	ctrl_cfg->flags = 0;
+
+	/* Init TORCH_INTENSITY ctrl data */
+	ctrl_init_data[TORCH_INTENSITY].cid = V4L2_CID_FLASH_TORCH_INTENSITY;
+	ctrl_cfg = &ctrl_init_data[TORCH_INTENSITY].config;
+	__lfs_to_v4l2_ctrl_config(&flash_cfg->torch_intensity, ctrl_cfg);
+	ctrl_cfg->id = V4L2_CID_FLASH_TORCH_INTENSITY;
+	ctrl_cfg->flags = V4L2_CTRL_FLAG_VOLATILE |
+			  V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
+
+	/* Init INDICATOR_INTENSITY ctrl data */
+	if (v4l2_flash->iled_cdev) {
+		ctrl_init_data[INDICATOR_INTENSITY].cid =
+					V4L2_CID_FLASH_INDICATOR_INTENSITY;
+		ctrl_cfg = &ctrl_init_data[INDICATOR_INTENSITY].config;
+		__lfs_to_v4l2_ctrl_config(&flash_cfg->indicator_intensity,
+					  ctrl_cfg);
+		ctrl_cfg->id = V4L2_CID_FLASH_INDICATOR_INTENSITY;
+		ctrl_cfg->min = 0;
+		ctrl_cfg->flags = V4L2_CTRL_FLAG_VOLATILE |
+				  V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
+	}
+
+	if (!(led_cdev->flags & LED_DEV_CAP_FLASH))
+		return;
+
+	/* Init FLASH_STROBE ctrl data */
+	ctrl_init_data[FLASH_STROBE].cid = V4L2_CID_FLASH_STROBE;
+	ctrl_cfg = &ctrl_init_data[FLASH_STROBE].config;
+	ctrl_cfg->id = V4L2_CID_FLASH_STROBE;
+
+	/* Init STROBE_STOP ctrl data */
+	ctrl_init_data[STROBE_STOP].cid = V4L2_CID_FLASH_STROBE_STOP;
+	ctrl_cfg = &ctrl_init_data[STROBE_STOP].config;
+	ctrl_cfg->id = V4L2_CID_FLASH_STROBE_STOP;
+
+	/* Init FLASH_STROBE_SOURCE ctrl data */
+	if (flash_cfg->has_external_strobe) {
+		mask = (1 << V4L2_FLASH_STROBE_SOURCE_SOFTWARE) |
+		       (1 << V4L2_FLASH_STROBE_SOURCE_EXTERNAL);
+		ctrl_init_data[STROBE_SOURCE].cid =
+					V4L2_CID_FLASH_STROBE_SOURCE;
+		ctrl_cfg = &ctrl_init_data[STROBE_SOURCE].config;
+		ctrl_cfg->id = V4L2_CID_FLASH_STROBE_SOURCE;
+		ctrl_cfg->max = V4L2_FLASH_STROBE_SOURCE_EXTERNAL;
+		ctrl_cfg->menu_skip_mask = ~mask;
+		ctrl_cfg->def = V4L2_FLASH_STROBE_SOURCE_SOFTWARE;
+	}
+
+	/* Init STROBE_STATUS ctrl data */
+	if (fled_cdev_ops->strobe_get) {
+		ctrl_init_data[STROBE_STATUS].cid =
+					V4L2_CID_FLASH_STROBE_STATUS;
+		ctrl_cfg = &ctrl_init_data[STROBE_STATUS].config;
+		ctrl_cfg->id = V4L2_CID_FLASH_STROBE_STATUS;
+		ctrl_cfg->flags = V4L2_CTRL_FLAG_VOLATILE |
+				  V4L2_CTRL_FLAG_READ_ONLY;
+	}
+
+	/* Init FLASH_TIMEOUT ctrl data */
+	if (fled_cdev_ops->timeout_set) {
+		ctrl_init_data[FLASH_TIMEOUT].cid = V4L2_CID_FLASH_TIMEOUT;
+		ctrl_cfg = &ctrl_init_data[FLASH_TIMEOUT].config;
+		__lfs_to_v4l2_ctrl_config(&fled_cdev->timeout, ctrl_cfg);
+		ctrl_cfg->id = V4L2_CID_FLASH_TIMEOUT;
+	}
+
+	/* Init FLASH_INTENSITY ctrl data */
+	if (fled_cdev_ops->flash_brightness_set) {
+		ctrl_init_data[FLASH_INTENSITY].cid = V4L2_CID_FLASH_INTENSITY;
+		ctrl_cfg = &ctrl_init_data[FLASH_INTENSITY].config;
+		__lfs_to_v4l2_ctrl_config(&fled_cdev->brightness, ctrl_cfg);
+		ctrl_cfg->id = V4L2_CID_FLASH_INTENSITY;
+		ctrl_cfg->flags = V4L2_CTRL_FLAG_VOLATILE |
+				  V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
+	}
+}
+
+static int v4l2_flash_init_controls(struct v4l2_flash *v4l2_flash,
+				struct v4l2_flash_config *flash_cfg)
+
+{
+	struct v4l2_flash_ctrl_data *ctrl_init_data;
+	struct v4l2_ctrl *ctrl;
+	struct v4l2_ctrl_config *ctrl_cfg;
+	int i, ret, num_ctrls = 0;
+
+	v4l2_flash->ctrls = devm_kzalloc(v4l2_flash->sd.dev,
+					sizeof(*v4l2_flash->ctrls) *
+					(STROBE_SOURCE + 1), GFP_KERNEL);
+	if (!v4l2_flash->ctrls)
+		return -ENOMEM;
+
+	/* allocate memory dynamically so as not to exceed stack frame size */
+	ctrl_init_data = kcalloc(NUM_FLASH_CTRLS, sizeof(*ctrl_init_data),
+					GFP_KERNEL);
+	if (!ctrl_init_data)
+		return -ENOMEM;
+
+	__fill_ctrl_init_data(v4l2_flash, flash_cfg, ctrl_init_data);
+
+	for (i = 0; i < NUM_FLASH_CTRLS; ++i)
+		if (ctrl_init_data[i].cid)
+			++num_ctrls;
+
+	v4l2_ctrl_handler_init(&v4l2_flash->hdl, num_ctrls);
+
+	for (i = 0; i < NUM_FLASH_CTRLS; ++i) {
+		ctrl_cfg = &ctrl_init_data[i].config;
+		if (!ctrl_init_data[i].cid)
+			continue;
+
+		if (ctrl_cfg->id == V4L2_CID_FLASH_LED_MODE ||
+		    ctrl_cfg->id == V4L2_CID_FLASH_STROBE_SOURCE)
+			ctrl = v4l2_ctrl_new_std_menu(&v4l2_flash->hdl,
+						&v4l2_flash_ctrl_ops,
+						ctrl_cfg->id,
+						ctrl_cfg->max,
+						ctrl_cfg->menu_skip_mask,
+						ctrl_cfg->def);
+		else
+			ctrl = v4l2_ctrl_new_std(&v4l2_flash->hdl,
+						&v4l2_flash_ctrl_ops,
+						ctrl_cfg->id,
+						ctrl_cfg->min,
+						ctrl_cfg->max,
+						ctrl_cfg->step,
+						ctrl_cfg->def);
+
+		if (ctrl)
+			ctrl->flags |= ctrl_cfg->flags;
+
+		if (i <= STROBE_SOURCE)
+			v4l2_flash->ctrls[i] = ctrl;
+	}
+
+	kfree(ctrl_init_data);
+
+	if (v4l2_flash->hdl.error) {
+		ret = v4l2_flash->hdl.error;
+		goto error_free_handler;
+	}
+
+	v4l2_ctrl_handler_setup(&v4l2_flash->hdl);
+
+	v4l2_flash->sd.ctrl_handler = &v4l2_flash->hdl;
+
+	return 0;
+
+error_free_handler:
+	v4l2_ctrl_handler_free(&v4l2_flash->hdl);
+	return ret;
+}
+
+static int __sync_device_with_v4l2_controls(struct v4l2_flash *v4l2_flash)
+{
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	struct v4l2_ctrl **ctrls = v4l2_flash->ctrls;
+	int ret = 0;
+
+	v4l2_flash_set_led_brightness(v4l2_flash, ctrls[TORCH_INTENSITY]);
+
+	if (ctrls[INDICATOR_INTENSITY])
+		v4l2_flash_set_led_brightness(v4l2_flash,
+						ctrls[INDICATOR_INTENSITY]);
+
+	if (ctrls[FLASH_TIMEOUT]) {
+		ret = led_set_flash_timeout(fled_cdev,
+					ctrls[FLASH_TIMEOUT]->val);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (ctrls[FLASH_INTENSITY]) {
+		ret = led_set_flash_brightness(fled_cdev,
+					ctrls[FLASH_INTENSITY]->val);
+		if (ret < 0)
+			return ret;
+	}
+
+	/*
+	 * For some hardware arrangements setting strobe source may affect
+	 * torch mode. Synchronize strobe source setting only if not in torch
+	 * mode. For torch mode case it will get synchronized upon switching
+	 * to flash mode.
+	 */
+	if (ctrls[STROBE_SOURCE] &&
+	    ctrls[LED_MODE]->val != V4L2_FLASH_LED_MODE_TORCH)
+		ret = call_flash_op(v4l2_flash, external_strobe_set,
+					ctrls[STROBE_SOURCE]->val);
+
+	return ret;
+}
+
+/*
+ * V4L2 subdev internal operations
+ */
+
+static int v4l2_flash_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_flash *v4l2_flash = v4l2_subdev_to_v4l2_flash(sd);
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	struct led_classdev *led_cdev = &fled_cdev->led_cdev;
+	struct led_classdev_flash *iled_cdev = v4l2_flash->iled_cdev;
+	struct led_classdev *led_cdev_ind = NULL;
+	int ret = 0;
+
+	if (!v4l2_fh_is_singular(&fh->vfh))
+		return 0;
+
+	mutex_lock(&led_cdev->led_access);
+
+	led_sysfs_disable(led_cdev);
+	led_trigger_remove(led_cdev);
+
+	mutex_unlock(&led_cdev->led_access);
+
+	if (iled_cdev) {
+		led_cdev_ind = &iled_cdev->led_cdev;
+
+		mutex_lock(&led_cdev_ind->led_access);
+
+		led_sysfs_disable(led_cdev_ind);
+		led_trigger_remove(led_cdev_ind);
+
+		mutex_unlock(&led_cdev_ind->led_access);
+	}
+
+	ret = __sync_device_with_v4l2_controls(v4l2_flash);
+	if (ret < 0)
+		goto out_sync_device;
+
+	return 0;
+out_sync_device:
+	mutex_lock(&led_cdev->led_access);
+	led_sysfs_enable(led_cdev);
+	mutex_unlock(&led_cdev->led_access);
+
+	if (led_cdev_ind) {
+		mutex_lock(&led_cdev_ind->led_access);
+		led_sysfs_enable(led_cdev_ind);
+		mutex_unlock(&led_cdev_ind->led_access);
+	}
+
+	return ret;
+}
+
+static int v4l2_flash_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+	struct v4l2_flash *v4l2_flash = v4l2_subdev_to_v4l2_flash(sd);
+	struct led_classdev_flash *fled_cdev = v4l2_flash->fled_cdev;
+	struct led_classdev *led_cdev = &fled_cdev->led_cdev;
+	struct led_classdev_flash *iled_cdev = v4l2_flash->iled_cdev;
+	int ret = 0;
+
+	if (!v4l2_fh_is_singular(&fh->vfh))
+		return 0;
+
+	mutex_lock(&led_cdev->led_access);
+
+	if (v4l2_flash->ctrls[STROBE_SOURCE])
+		ret = v4l2_ctrl_s_ctrl(v4l2_flash->ctrls[STROBE_SOURCE],
+				V4L2_FLASH_STROBE_SOURCE_SOFTWARE);
+	led_sysfs_enable(led_cdev);
+
+	mutex_unlock(&led_cdev->led_access);
+
+	if (iled_cdev) {
+		struct led_classdev *led_cdev_ind = &iled_cdev->led_cdev;
+
+		mutex_lock(&led_cdev_ind->led_access);
+		led_sysfs_enable(led_cdev_ind);
+		mutex_unlock(&led_cdev_ind->led_access);
+	}
+
+	return ret;
+}
+
+static const struct v4l2_subdev_internal_ops v4l2_flash_subdev_internal_ops = {
+	.open = v4l2_flash_open,
+	.close = v4l2_flash_close,
+};
+
+static const struct v4l2_subdev_core_ops v4l2_flash_core_ops = {
+	.queryctrl = v4l2_subdev_queryctrl,
+	.querymenu = v4l2_subdev_querymenu,
+};
+
+static const struct v4l2_subdev_ops v4l2_flash_subdev_ops = {
+	.core = &v4l2_flash_core_ops,
+};
+
+struct v4l2_flash *v4l2_flash_init(
+	struct device *dev, struct device_node *of_node,
+	struct led_classdev_flash *fled_cdev,
+	struct led_classdev_flash *iled_cdev,
+	const struct v4l2_flash_ops *ops,
+	struct v4l2_flash_config *config)
+{
+	struct v4l2_flash *v4l2_flash;
+	struct led_classdev *led_cdev;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	if (!fled_cdev || !ops || !config)
+		return ERR_PTR(-EINVAL);
+
+	led_cdev = &fled_cdev->led_cdev;
+
+	v4l2_flash = devm_kzalloc(led_cdev->dev, sizeof(*v4l2_flash),
+					GFP_KERNEL);
+	if (!v4l2_flash)
+		return ERR_PTR(-ENOMEM);
+
+	sd = &v4l2_flash->sd;
+	v4l2_flash->fled_cdev = fled_cdev;
+	v4l2_flash->iled_cdev = iled_cdev;
+	v4l2_flash->ops = ops;
+	sd->dev = dev;
+	sd->of_node = of_node;
+	v4l2_subdev_init(sd, &v4l2_flash_subdev_ops);
+	sd->internal_ops = &v4l2_flash_subdev_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	strlcpy(sd->name, config->dev_name, sizeof(sd->name));
+
+	ret = media_entity_init(&sd->entity, 0, NULL, 0);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_FLASH;
+
+	ret = v4l2_flash_init_controls(v4l2_flash, config);
+	if (ret < 0)
+		goto err_init_controls;
+
+	if (sd->of_node)
+		of_node_get(sd->of_node);
+	else
+		of_node_get(led_cdev->dev->of_node);
+
+	ret = v4l2_async_register_subdev(sd);
+	if (ret < 0)
+		goto err_async_register_sd;
+
+	return v4l2_flash;
+
+err_async_register_sd:
+	of_node_put(led_cdev->dev->of_node);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+err_init_controls:
+	media_entity_cleanup(&sd->entity);
+
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(v4l2_flash_init);
+
+void v4l2_flash_release(struct v4l2_flash *v4l2_flash)
+{
+	struct v4l2_subdev *sd;
+	struct led_classdev *led_cdev;
+
+	if (IS_ERR_OR_NULL(v4l2_flash))
+		return;
+
+	sd = &v4l2_flash->sd;
+	led_cdev = &v4l2_flash->fled_cdev->led_cdev;
+
+	v4l2_async_unregister_subdev(sd);
+
+	if (sd->of_node)
+		of_node_put(sd->of_node);
+	else
+		of_node_put(led_cdev->dev->of_node);
+
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	media_entity_cleanup(&sd->entity);
+}
+EXPORT_SYMBOL_GPL(v4l2_flash_release);
+
+MODULE_AUTHOR("Jacek Anaszewski <j.anaszewski@samsung.com>");
+MODULE_DESCRIPTION("V4L2 Flash sub-device helpers");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index aa407cb5f830..85de4557f696 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -142,6 +142,7 @@ const char *v4l2_field_names[] = {
 EXPORT_SYMBOL(v4l2_field_names);
 
 const char *v4l2_type_names[] = {
+	[0]				   = "0",
 	[V4L2_BUF_TYPE_VIDEO_CAPTURE]      = "vid-cap",
 	[V4L2_BUF_TYPE_VIDEO_OVERLAY]      = "vid-overlay",
 	[V4L2_BUF_TYPE_VIDEO_OUTPUT]       = "vid-out",
@@ -257,7 +258,8 @@ static void v4l_print_format(const void *arg, bool write_only)
 		pr_cont(", width=%u, height=%u, "
 			"pixelformat=%c%c%c%c, field=%s, "
 			"bytesperline=%u, sizeimage=%u, colorspace=%d, "
-			"flags=0x%x, ycbcr_enc=%u, quantization=%u\n",
+			"flags=0x%x, ycbcr_enc=%u, quantization=%u, "
+			"xfer_func=%u\n",
 			pix->width, pix->height,
 			(pix->pixelformat & 0xff),
 			(pix->pixelformat >>  8) & 0xff,
@@ -266,7 +268,7 @@ static void v4l_print_format(const void *arg, bool write_only)
 			prt_names(pix->field, v4l2_field_names),
 			pix->bytesperline, pix->sizeimage,
 			pix->colorspace, pix->flags, pix->ycbcr_enc,
-			pix->quantization);
+			pix->quantization, pix->xfer_func);
 		break;
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
@@ -274,7 +276,7 @@ static void v4l_print_format(const void *arg, bool write_only)
 		pr_cont(", width=%u, height=%u, "
 			"format=%c%c%c%c, field=%s, "
 			"colorspace=%d, num_planes=%u, flags=0x%x, "
-			"ycbcr_enc=%u, quantization=%u\n",
+			"ycbcr_enc=%u, quantization=%u, xfer_func=%u\n",
 			mp->width, mp->height,
 			(mp->pixelformat & 0xff),
 			(mp->pixelformat >>  8) & 0xff,
@@ -282,7 +284,7 @@ static void v4l_print_format(const void *arg, bool write_only)
 			(mp->pixelformat >> 24) & 0xff,
 			prt_names(mp->field, v4l2_field_names),
 			mp->colorspace, mp->num_planes, mp->flags,
-			mp->ycbcr_enc, mp->quantization);
+			mp->ycbcr_enc, mp->quantization, mp->xfer_func);
 		for (i = 0; i < mp->num_planes; i++)
 			printk(KERN_DEBUG "plane %u: bytesperline=%u sizeimage=%u\n", i,
 					mp->plane_fmt[i].bytesperline,
@@ -1103,6 +1105,183 @@ static int v4l_enumoutput(const struct v4l2_ioctl_ops *ops,
 	return ops->vidioc_enum_output(file, fh, p);
 }
 
+static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
+{
+	const unsigned sz = sizeof(fmt->description);
+	const char *descr = NULL;
+	u32 flags = 0;
+
+	/*
+	 * We depart from the normal coding style here since the descriptions
+	 * should be aligned so it is easy to see which descriptions will be
+	 * longer than 31 characters (the max length for a description).
+	 * And frankly, this is easier to read anyway.
+	 *
+	 * Note that gcc will use O(log N) comparisons to find the right case.
+	 */
+	switch (fmt->pixelformat) {
+	/* Max description length mask:	descr = "0123456789012345678901234567890" */
+	case V4L2_PIX_FMT_RGB332:	descr = "8-bit RGB 3-3-2"; break;
+	case V4L2_PIX_FMT_RGB444:	descr = "16-bit A/XRGB 4-4-4-4"; break;
+	case V4L2_PIX_FMT_ARGB444:	descr = "16-bit ARGB 4-4-4-4"; break;
+	case V4L2_PIX_FMT_XRGB444:	descr = "16-bit XRGB 4-4-4-4"; break;
+	case V4L2_PIX_FMT_RGB555:	descr = "16-bit A/XRGB 1-5-5-5"; break;
+	case V4L2_PIX_FMT_ARGB555:	descr = "16-bit ARGB 1-5-5-5"; break;
+	case V4L2_PIX_FMT_XRGB555:	descr = "16-bit XRGB 1-5-5-5"; break;
+	case V4L2_PIX_FMT_RGB565:	descr = "16-bit RGB 5-6-5"; break;
+	case V4L2_PIX_FMT_RGB555X:	descr = "16-bit A/XRGB 1-5-5-5 BE"; break;
+	case V4L2_PIX_FMT_ARGB555X:	descr = "16-bit ARGB 1-5-5-5 BE"; break;
+	case V4L2_PIX_FMT_XRGB555X:	descr = "16-bit XRGB 1-5-5-5 BE"; break;
+	case V4L2_PIX_FMT_RGB565X:	descr = "16-bit RGB 5-6-5 BE"; break;
+	case V4L2_PIX_FMT_BGR666:	descr = "18-bit BGRX 6-6-6-14"; break;
+	case V4L2_PIX_FMT_BGR24:	descr = "24-bit BGR 8-8-8"; break;
+	case V4L2_PIX_FMT_RGB24:	descr = "24-bit RGB 8-8-8"; break;
+	case V4L2_PIX_FMT_BGR32:	descr = "32-bit BGRA/X 8-8-8-8"; break;
+	case V4L2_PIX_FMT_ABGR32:	descr = "32-bit BGRA 8-8-8-8"; break;
+	case V4L2_PIX_FMT_XBGR32:	descr = "32-bit BGRX 8-8-8-8"; break;
+	case V4L2_PIX_FMT_RGB32:	descr = "32-bit A/XRGB 8-8-8-8"; break;
+	case V4L2_PIX_FMT_ARGB32:	descr = "32-bit ARGB 8-8-8-8"; break;
+	case V4L2_PIX_FMT_XRGB32:	descr = "32-bit XRGB 8-8-8-8"; break;
+	case V4L2_PIX_FMT_GREY:		descr = "8-bit Greyscale"; break;
+	case V4L2_PIX_FMT_Y4:		descr = "4-bit Greyscale"; break;
+	case V4L2_PIX_FMT_Y6:		descr = "6-bit Greyscale"; break;
+	case V4L2_PIX_FMT_Y10:		descr = "10-bit Greyscale"; break;
+	case V4L2_PIX_FMT_Y12:		descr = "12-bit Greyscale"; break;
+	case V4L2_PIX_FMT_Y16:		descr = "16-bit Greyscale"; break;
+	case V4L2_PIX_FMT_Y16_BE:	descr = "16-bit Greyscale BE"; break;
+	case V4L2_PIX_FMT_Y10BPACK:	descr = "10-bit Greyscale (Packed)"; break;
+	case V4L2_PIX_FMT_PAL8:		descr = "8-bit Palette"; break;
+	case V4L2_PIX_FMT_UV8:		descr = "8-bit Chrominance UV 4-4"; break;
+	case V4L2_PIX_FMT_YVU410:	descr = "Planar YVU 4:1:0"; break;
+	case V4L2_PIX_FMT_YVU420:	descr = "Planar YVU 4:2:0"; break;
+	case V4L2_PIX_FMT_YUYV:		descr = "YUYV 4:2:2"; break;
+	case V4L2_PIX_FMT_YYUV:		descr = "YYUV 4:2:2"; break;
+	case V4L2_PIX_FMT_YVYU:		descr = "YVYU 4:2:2"; break;
+	case V4L2_PIX_FMT_UYVY:		descr = "UYVY 4:2:2"; break;
+	case V4L2_PIX_FMT_VYUY:		descr = "VYUY 4:2:2"; break;
+	case V4L2_PIX_FMT_YUV422P:	descr = "Planar YVU 4:2:2"; break;
+	case V4L2_PIX_FMT_YUV411P:	descr = "Planar YUV 4:1:1"; break;
+	case V4L2_PIX_FMT_Y41P:		descr = "YUV 4:1:1 (Packed)"; break;
+	case V4L2_PIX_FMT_YUV444:	descr = "16-bit A/XYUV 4-4-4-4"; break;
+	case V4L2_PIX_FMT_YUV555:	descr = "16-bit A/XYUV 1-5-5-5"; break;
+	case V4L2_PIX_FMT_YUV565:	descr = "16-bit YUV 5-6-5"; break;
+	case V4L2_PIX_FMT_YUV32:	descr = "32-bit A/XYUV 8-8-8-8"; break;
+	case V4L2_PIX_FMT_YUV410:	descr = "Planar YUV 4:1:0"; break;
+	case V4L2_PIX_FMT_YUV420:	descr = "Planar YUV 4:2:0"; break;
+	case V4L2_PIX_FMT_HI240:	descr = "8-bit Dithered RGB (BTTV)"; break;
+	case V4L2_PIX_FMT_HM12:		descr = "YUV 4:2:0 (16x16 Macroblocks)"; break;
+	case V4L2_PIX_FMT_M420:		descr = "YUV 4:2:0 (M420)"; break;
+	case V4L2_PIX_FMT_NV12:		descr = "Y/CbCr 4:2:0"; break;
+	case V4L2_PIX_FMT_NV21:		descr = "Y/CrCb 4:2:0"; break;
+	case V4L2_PIX_FMT_NV16:		descr = "Y/CbCr 4:2:2"; break;
+	case V4L2_PIX_FMT_NV61:		descr = "Y/CrCb 4:2:2"; break;
+	case V4L2_PIX_FMT_NV24:		descr = "Y/CbCr 4:4:4"; break;
+	case V4L2_PIX_FMT_NV42:		descr = "Y/CrCb 4:4:4"; break;
+	case V4L2_PIX_FMT_NV12M:	descr = "Y/CbCr 4:2:0 (N-C)"; break;
+	case V4L2_PIX_FMT_NV21M:	descr = "Y/CrCb 4:2:0 (N-C)"; break;
+	case V4L2_PIX_FMT_NV16M:	descr = "Y/CbCr 4:2:2 (N-C)"; break;
+	case V4L2_PIX_FMT_NV61M:	descr = "Y/CrCb 4:2:2 (N-C)"; break;
+	case V4L2_PIX_FMT_NV12MT:	descr = "Y/CbCr 4:2:0 (64x32 MB, N-C)"; break;
+	case V4L2_PIX_FMT_NV12MT_16X16:	descr = "Y/CbCr 4:2:0 (16x16 MB, N-C)"; break;
+	case V4L2_PIX_FMT_YUV420M:	descr = "Planar YUV 4:2:0 (N-C)"; break;
+	case V4L2_PIX_FMT_YVU420M:	descr = "Planar YVU 4:2:0 (N-C)"; break;
+	case V4L2_PIX_FMT_SBGGR8:	descr = "8-bit Bayer BGBG/GRGR"; break;
+	case V4L2_PIX_FMT_SGBRG8:	descr = "8-bit Bayer GBGB/RGRG"; break;
+	case V4L2_PIX_FMT_SGRBG8:	descr = "8-bit Bayer GRGR/BGBG"; break;
+	case V4L2_PIX_FMT_SRGGB8:	descr = "8-bit Bayer RGRG/GBGB"; break;
+	case V4L2_PIX_FMT_SBGGR10:	descr = "10-bit Bayer BGBG/GRGR"; break;
+	case V4L2_PIX_FMT_SGBRG10:	descr = "10-bit Bayer GBGB/RGRG"; break;
+	case V4L2_PIX_FMT_SGRBG10:	descr = "10-bit Bayer GRGR/BGBG"; break;
+	case V4L2_PIX_FMT_SRGGB10:	descr = "10-bit Bayer RGRG/GBGB"; break;
+	case V4L2_PIX_FMT_SBGGR12:	descr = "12-bit Bayer BGBG/GRGR"; break;
+	case V4L2_PIX_FMT_SGBRG12:	descr = "12-bit Bayer GBGB/RGRG"; break;
+	case V4L2_PIX_FMT_SGRBG12:	descr = "12-bit Bayer GRGR/BGBG"; break;
+	case V4L2_PIX_FMT_SRGGB12:	descr = "12-bit Bayer RGRG/GBGB"; break;
+	case V4L2_PIX_FMT_SBGGR10P:	descr = "10-bit Bayer BGBG/GRGR Packed"; break;
+	case V4L2_PIX_FMT_SGBRG10P:	descr = "10-bit Bayer GBGB/RGRG Packed"; break;
+	case V4L2_PIX_FMT_SGRBG10P:	descr = "10-bit Bayer GRGR/BGBG Packed"; break;
+	case V4L2_PIX_FMT_SRGGB10P:	descr = "10-bit Bayer RGRG/GBGB Packed"; break;
+	case V4L2_PIX_FMT_SBGGR10ALAW8:	descr = "8-bit Bayer BGBG/GRGR (A-law)"; break;
+	case V4L2_PIX_FMT_SGBRG10ALAW8:	descr = "8-bit Bayer GBGB/RGRG (A-law)"; break;
+	case V4L2_PIX_FMT_SGRBG10ALAW8:	descr = "8-bit Bayer GRGR/BGBG (A-law)"; break;
+	case V4L2_PIX_FMT_SRGGB10ALAW8:	descr = "8-bit Bayer RGRG/GBGB (A-law)"; break;
+	case V4L2_PIX_FMT_SBGGR10DPCM8:	descr = "8-bit Bayer BGBG/GRGR (DPCM)"; break;
+	case V4L2_PIX_FMT_SGBRG10DPCM8:	descr = "8-bit Bayer GBGB/RGRG (DPCM)"; break;
+	case V4L2_PIX_FMT_SGRBG10DPCM8:	descr = "8-bit Bayer GRGR/BGBG (DPCM)"; break;
+	case V4L2_PIX_FMT_SRGGB10DPCM8:	descr = "8-bit Bayer RGRG/GBGB (DPCM)"; break;
+	case V4L2_PIX_FMT_SBGGR16:	descr = "16-bit Bayer BGBG/GRGR (Exp.)"; break;
+	case V4L2_PIX_FMT_SN9C20X_I420:	descr = "GSPCA SN9C20X I420"; break;
+	case V4L2_PIX_FMT_SPCA501:	descr = "GSPCA SPCA501"; break;
+	case V4L2_PIX_FMT_SPCA505:	descr = "GSPCA SPCA505"; break;
+	case V4L2_PIX_FMT_SPCA508:	descr = "GSPCA SPCA508"; break;
+	case V4L2_PIX_FMT_STV0680:	descr = "GSPCA STV0680"; break;
+	case V4L2_PIX_FMT_TM6000:	descr = "A/V + VBI Mux Packet"; break;
+	case V4L2_PIX_FMT_CIT_YYVYUY:	descr = "GSPCA CIT YYVYUY"; break;
+	case V4L2_PIX_FMT_KONICA420:	descr = "GSPCA KONICA420"; break;
+	case V4L2_SDR_FMT_CU8:		descr = "Complex U8"; break;
+	case V4L2_SDR_FMT_CU16LE:	descr = "Complex U16LE"; break;
+	case V4L2_SDR_FMT_CS8:		descr = "Complex S8"; break;
+	case V4L2_SDR_FMT_CS14LE:	descr = "Complex S14LE"; break;
+	case V4L2_SDR_FMT_RU12LE:	descr = "Real U12LE"; break;
+
+	default:
+		/* Compressed formats */
+		flags = V4L2_FMT_FLAG_COMPRESSED;
+		switch (fmt->pixelformat) {
+		/* Max description length mask:	descr = "0123456789012345678901234567890" */
+		case V4L2_PIX_FMT_MJPEG:	descr = "Motion-JPEG"; break;
+		case V4L2_PIX_FMT_JPEG:		descr = "JFIF JPEG"; break;
+		case V4L2_PIX_FMT_DV:		descr = "1394"; break;
+		case V4L2_PIX_FMT_MPEG:		descr = "MPEG-1/2/4"; break;
+		case V4L2_PIX_FMT_H264:		descr = "H.264"; break;
+		case V4L2_PIX_FMT_H264_NO_SC:	descr = "H.264 (No Start Codes)"; break;
+		case V4L2_PIX_FMT_H264_MVC:	descr = "H.264 MVC"; break;
+		case V4L2_PIX_FMT_H263:		descr = "H.263"; break;
+		case V4L2_PIX_FMT_MPEG1:	descr = "MPEG-1 ES"; break;
+		case V4L2_PIX_FMT_MPEG2:	descr = "MPEG-2 ES"; break;
+		case V4L2_PIX_FMT_MPEG4:	descr = "MPEG-4 part 2 ES"; break;
+		case V4L2_PIX_FMT_XVID:		descr = "Xvid"; break;
+		case V4L2_PIX_FMT_VC1_ANNEX_G:	descr = "VC-1 (SMPTE 412M Annex G)"; break;
+		case V4L2_PIX_FMT_VC1_ANNEX_L:	descr = "VC-1 (SMPTE 412M Annex L)"; break;
+		case V4L2_PIX_FMT_VP8:		descr = "VP8"; break;
+		case V4L2_PIX_FMT_CPIA1:	descr = "GSPCA CPiA YUV"; break;
+		case V4L2_PIX_FMT_WNVA:		descr = "WNVA"; break;
+		case V4L2_PIX_FMT_SN9C10X:	descr = "GSPCA SN9C10X"; break;
+		case V4L2_PIX_FMT_PWC1:		descr = "Raw Philips Webcam Type (Old)"; break;
+		case V4L2_PIX_FMT_PWC2:		descr = "Raw Philips Webcam Type (New)"; break;
+		case V4L2_PIX_FMT_ET61X251:	descr = "GSPCA ET61X251"; break;
+		case V4L2_PIX_FMT_SPCA561:	descr = "GSPCA SPCA561"; break;
+		case V4L2_PIX_FMT_PAC207:	descr = "GSPCA PAC207"; break;
+		case V4L2_PIX_FMT_MR97310A:	descr = "GSPCA MR97310A"; break;
+		case V4L2_PIX_FMT_JL2005BCD:	descr = "GSPCA JL2005BCD"; break;
+		case V4L2_PIX_FMT_SN9C2028:	descr = "GSPCA SN9C2028"; break;
+		case V4L2_PIX_FMT_SQ905C:	descr = "GSPCA SQ905C"; break;
+		case V4L2_PIX_FMT_PJPG:		descr = "GSPCA PJPG"; break;
+		case V4L2_PIX_FMT_OV511:	descr = "GSPCA OV511"; break;
+		case V4L2_PIX_FMT_OV518:	descr = "GSPCA OV518"; break;
+		case V4L2_PIX_FMT_JPGL:		descr = "JPEG Lite"; break;
+		case V4L2_PIX_FMT_SE401:	descr = "GSPCA SE401"; break;
+		case V4L2_PIX_FMT_S5C_UYVY_JPG:	descr = "S5C73MX interleaved UYVY/JPEG"; break;
+		default:
+			WARN(1, "Unknown pixelformat 0x%08x\n", fmt->pixelformat);
+			if (fmt->description[0])
+				return;
+			flags = 0;
+			snprintf(fmt->description, sz, "%c%c%c%c%s",
+					(char)(fmt->pixelformat & 0x7f),
+					(char)((fmt->pixelformat >> 8) & 0x7f),
+					(char)((fmt->pixelformat >> 16) & 0x7f),
+					(char)((fmt->pixelformat >> 24) & 0x7f),
+					(fmt->pixelformat & (1 << 31)) ? "-BE" : "");
+			break;
+		}
+	}
+
+	if (descr)
+		WARN_ON(strlcpy(fmt->description, descr, sz) >= sz);
+	fmt->flags = flags;
+}
+
 static int v4l_enum_fmt(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
@@ -1112,34 +1291,43 @@ static int v4l_enum_fmt(const struct v4l2_ioctl_ops *ops,
 	bool is_sdr = vfd->vfl_type == VFL_TYPE_SDR;
 	bool is_rx = vfd->vfl_dir != VFL_DIR_TX;
 	bool is_tx = vfd->vfl_dir != VFL_DIR_RX;
+	int ret = -EINVAL;
 
 	switch (p->type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 		if (unlikely(!is_rx || !is_vid || !ops->vidioc_enum_fmt_vid_cap))
 			break;
-		return ops->vidioc_enum_fmt_vid_cap(file, fh, arg);
+		ret = ops->vidioc_enum_fmt_vid_cap(file, fh, arg);
+		break;
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 		if (unlikely(!is_rx || !is_vid || !ops->vidioc_enum_fmt_vid_cap_mplane))
 			break;
-		return ops->vidioc_enum_fmt_vid_cap_mplane(file, fh, arg);
+		ret = ops->vidioc_enum_fmt_vid_cap_mplane(file, fh, arg);
+		break;
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 		if (unlikely(!is_rx || !is_vid || !ops->vidioc_enum_fmt_vid_overlay))
 			break;
-		return ops->vidioc_enum_fmt_vid_overlay(file, fh, arg);
+		ret = ops->vidioc_enum_fmt_vid_overlay(file, fh, arg);
+		break;
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 		if (unlikely(!is_tx || !is_vid || !ops->vidioc_enum_fmt_vid_out))
 			break;
-		return ops->vidioc_enum_fmt_vid_out(file, fh, arg);
+		ret = ops->vidioc_enum_fmt_vid_out(file, fh, arg);
+		break;
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 		if (unlikely(!is_tx || !is_vid || !ops->vidioc_enum_fmt_vid_out_mplane))
 			break;
-		return ops->vidioc_enum_fmt_vid_out_mplane(file, fh, arg);
+		ret = ops->vidioc_enum_fmt_vid_out_mplane(file, fh, arg);
+		break;
 	case V4L2_BUF_TYPE_SDR_CAPTURE:
 		if (unlikely(!is_rx || !is_sdr || !ops->vidioc_enum_fmt_sdr_cap))
 			break;
-		return ops->vidioc_enum_fmt_sdr_cap(file, fh, arg);
+		ret = ops->vidioc_enum_fmt_sdr_cap(file, fh, arg);
+		break;
 	}
-	return -EINVAL;
+	if (ret == 0)
+		v4l_fill_fmtdesc(p);
+	return ret;
 }
 
 static int v4l_g_fmt(const struct v4l2_ioctl_ops *ops,
@@ -1618,6 +1806,8 @@ static int v4l_create_bufs(const struct v4l2_ioctl_ops *ops,
 	if (ret)
 		return ret;
 
+	CLEAR_AFTER_FIELD(create, format);
+
 	v4l_sanitize_format(&create->format);
 
 	ret = ops->vidioc_create_bufs(file, fh, create);
@@ -2354,7 +2544,7 @@ static long __video_do_ioctl(struct file *file,
 	if (v4l2_is_known_ioctl(cmd)) {
 		info = &v4l2_ioctls[_IOC_NR(cmd)];
 
-	        if (!test_bit(_IOC_NR(cmd), vfd->valid_ioctls) &&
+		if (!test_bit(_IOC_NR(cmd), vfd->valid_ioctls) &&
 		    !((info->flags & INFO_FL_CTRL) && vfh && vfh->ctrl_handler))
 			goto done;
 
diff --git a/drivers/media/v4l2-core/v4l2-mem2mem.c b/drivers/media/v4l2-core/v4l2-mem2mem.c
index 73824a5ada83..dc853e57f91f 100644
--- a/drivers/media/v4l2-core/v4l2-mem2mem.c
+++ b/drivers/media/v4l2-core/v4l2-mem2mem.c
@@ -427,6 +427,25 @@ int v4l2_m2m_dqbuf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 EXPORT_SYMBOL_GPL(v4l2_m2m_dqbuf);
 
 /**
+ * v4l2_m2m_prepare_buf() - prepare a source or destination buffer, depending on
+ * the type
+ */
+int v4l2_m2m_prepare_buf(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
+			 struct v4l2_buffer *buf)
+{
+	struct vb2_queue *vq;
+	int ret;
+
+	vq = v4l2_m2m_get_vq(m2m_ctx, buf->type);
+	ret = vb2_prepare_buf(vq, buf);
+	if (!ret)
+		v4l2_m2m_try_schedule(m2m_ctx);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_prepare_buf);
+
+/**
  * v4l2_m2m_create_bufs() - create a source or destination buffer, depending
  * on the type
  */
@@ -564,8 +583,16 @@ unsigned int v4l2_m2m_poll(struct file *file, struct v4l2_m2m_ctx *m2m_ctx,
 
 	if (list_empty(&src_q->done_list))
 		poll_wait(file, &src_q->done_wq, wait);
-	if (list_empty(&dst_q->done_list))
+	if (list_empty(&dst_q->done_list)) {
+		/*
+		 * If the last buffer was dequeued from the capture queue,
+		 * return immediately. DQBUF will return -EPIPE.
+		 */
+		if (dst_q->last_buffer_dequeued)
+			return rc | POLLIN | POLLRDNORM;
+
 		poll_wait(file, &dst_q->done_wq, wait);
+	}
 
 	if (m2m_ctx->m2m_dev->m2m_ops->lock)
 		m2m_ctx->m2m_dev->m2m_ops->lock(m2m_ctx->priv);
@@ -803,6 +830,15 @@ int v4l2_m2m_ioctl_dqbuf(struct file *file, void *priv,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_dqbuf);
 
+int v4l2_m2m_ioctl_prepare_buf(struct file *file, void *priv,
+			       struct v4l2_buffer *buf)
+{
+	struct v4l2_fh *fh = file->private_data;
+
+	return v4l2_m2m_prepare_buf(file, fh->m2m_ctx, buf);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_prepare_buf);
+
 int v4l2_m2m_ioctl_expbuf(struct file *file, void *priv,
 				struct v4l2_exportbuffer *eb)
 {
diff --git a/drivers/media/v4l2-core/v4l2-of.c b/drivers/media/v4l2-core/v4l2-of.c
index 83143d39dea7..b27cbb1f5afe 100644
--- a/drivers/media/v4l2-core/v4l2-of.c
+++ b/drivers/media/v4l2-core/v4l2-of.c
@@ -14,6 +14,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/types.h>
 
@@ -92,10 +93,6 @@ static void v4l2_of_parse_parallel_bus(const struct device_node *node,
 		flags |= v ? V4L2_MBUS_VSYNC_ACTIVE_HIGH :
 			V4L2_MBUS_VSYNC_ACTIVE_LOW;
 
-	if (!of_property_read_u32(node, "pclk-sample", &v))
-		flags |= v ? V4L2_MBUS_PCLK_SAMPLE_RISING :
-			V4L2_MBUS_PCLK_SAMPLE_FALLING;
-
 	if (!of_property_read_u32(node, "field-even-active", &v))
 		flags |= v ? V4L2_MBUS_FIELD_EVEN_HIGH :
 			V4L2_MBUS_FIELD_EVEN_LOW;
@@ -104,6 +101,10 @@ static void v4l2_of_parse_parallel_bus(const struct device_node *node,
 	else
 		endpoint->bus_type = V4L2_MBUS_BT656;
 
+	if (!of_property_read_u32(node, "pclk-sample", &v))
+		flags |= v ? V4L2_MBUS_PCLK_SAMPLE_RISING :
+			V4L2_MBUS_PCLK_SAMPLE_FALLING;
+
 	if (!of_property_read_u32(node, "data-active", &v))
 		flags |= v ? V4L2_MBUS_DATA_ACTIVE_HIGH :
 			V4L2_MBUS_DATA_ACTIVE_LOW;
@@ -141,6 +142,10 @@ static void v4l2_of_parse_parallel_bus(const struct device_node *node,
  * V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag.
  * The caller should hold a reference to @node.
  *
+ * NOTE: This function does not parse properties the size of which is
+ * variable without a low fixed limit. Please use
+ * v4l2_of_alloc_parse_endpoint() in new drivers instead.
+ *
  * Return: 0.
  */
 int v4l2_of_parse_endpoint(const struct device_node *node,
@@ -149,8 +154,9 @@ int v4l2_of_parse_endpoint(const struct device_node *node,
 	int rval;
 
 	of_graph_parse_endpoint(node, &endpoint->base);
-	endpoint->bus_type = 0;
-	memset(&endpoint->bus, 0, sizeof(endpoint->bus));
+	/* Zero fields from bus_type to until the end */
+	memset(&endpoint->bus_type, 0, sizeof(*endpoint) -
+	       offsetof(typeof(*endpoint), bus_type));
 
 	rval = v4l2_of_parse_csi_bus(node, endpoint);
 	if (rval)
@@ -166,6 +172,88 @@ int v4l2_of_parse_endpoint(const struct device_node *node,
 }
 EXPORT_SYMBOL(v4l2_of_parse_endpoint);
 
+/*
+ * v4l2_of_free_endpoint() - free the endpoint acquired by
+ * v4l2_of_alloc_parse_endpoint()
+ * @endpoint - the endpoint the resources of which are to be released
+ *
+ * It is safe to call this function with NULL argument or on an
+ * endpoint the parsing of which failed.
+ */
+void v4l2_of_free_endpoint(struct v4l2_of_endpoint *endpoint)
+{
+	if (IS_ERR_OR_NULL(endpoint))
+		return;
+
+	kfree(endpoint->link_frequencies);
+	kfree(endpoint);
+}
+EXPORT_SYMBOL(v4l2_of_free_endpoint);
+
+/**
+ * v4l2_of_alloc_parse_endpoint() - parse all endpoint node properties
+ * @node: pointer to endpoint device_node
+ *
+ * All properties are optional. If none are found, we don't set any flags.
+ * This means the port has a static configuration and no properties have
+ * to be specified explicitly.
+ * If any properties that identify the bus as parallel are found and
+ * slave-mode isn't set, we set V4L2_MBUS_MASTER. Similarly, if we recognise
+ * the bus as serial CSI-2 and clock-noncontinuous isn't set, we set the
+ * V4L2_MBUS_CSI2_CONTINUOUS_CLOCK flag.
+ * The caller should hold a reference to @node.
+ *
+ * v4l2_of_alloc_parse_endpoint() has two important differences to
+ * v4l2_of_parse_endpoint():
+ *
+ * 1. It also parses variable size data and
+ *
+ * 2. The memory it has allocated to store the variable size data must
+ *    be freed using v4l2_of_free_endpoint() when no longer needed.
+ *
+ * Return: Pointer to v4l2_of_endpoint if successful, on error a
+ * negative error code.
+ */
+struct v4l2_of_endpoint *v4l2_of_alloc_parse_endpoint(
+	const struct device_node *node)
+{
+	struct v4l2_of_endpoint *endpoint;
+	int len;
+	int rval;
+
+	endpoint = kzalloc(sizeof(*endpoint), GFP_KERNEL);
+	if (!endpoint)
+		return ERR_PTR(-ENOMEM);
+
+	rval = v4l2_of_parse_endpoint(node, endpoint);
+	if (rval < 0)
+		goto out_err;
+
+	if (of_get_property(node, "link-frequencies", &len)) {
+		endpoint->link_frequencies = kmalloc(len, GFP_KERNEL);
+		if (!endpoint->link_frequencies) {
+			rval = -ENOMEM;
+			goto out_err;
+		}
+
+		endpoint->nr_of_link_frequencies =
+			len / sizeof(*endpoint->link_frequencies);
+
+		rval = of_property_read_u64_array(
+			node, "link-frequencies", endpoint->link_frequencies,
+			endpoint->nr_of_link_frequencies);
+		if (rval < 0)
+			goto out_err;
+	}
+
+	return endpoint;
+
+out_err:
+	v4l2_of_free_endpoint(endpoint);
+	return ERR_PTR(rval);
+}
+EXPORT_SYMBOL(v4l2_of_alloc_parse_endpoint);
+
 /**
  * v4l2_of_parse_link() - parse a link between two endpoints
  * @node: pointer to the endpoint at the local end of the link
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index 66ada01c796c..93b315459098 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -182,6 +182,7 @@ module_param(debug, int, 0644);
 				 V4L2_BUF_FLAG_KEYFRAME | V4L2_BUF_FLAG_TIMECODE)
 
 static void __vb2_queue_cancel(struct vb2_queue *q);
+static void __enqueue_in_driver(struct vb2_buffer *vb);
 
 /**
  * __vb2_buf_mem_alloc() - allocate video memory for the given buffer
@@ -1153,8 +1154,9 @@ EXPORT_SYMBOL_GPL(vb2_plane_cookie);
 /**
  * vb2_buffer_done() - inform videobuf that an operation on a buffer is finished
  * @vb:		vb2_buffer returned from the driver
- * @state:	either VB2_BUF_STATE_DONE if the operation finished successfully
- *		or VB2_BUF_STATE_ERROR if the operation finished with an error.
+ * @state:	either VB2_BUF_STATE_DONE if the operation finished successfully,
+ *		VB2_BUF_STATE_ERROR if the operation finished with an error or
+ *		VB2_BUF_STATE_QUEUED if the driver wants to requeue buffers.
  *		If start_streaming fails then it should return buffers with state
  *		VB2_BUF_STATE_QUEUED to put them back into the queue.
  *
@@ -1205,8 +1207,11 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 	atomic_dec(&q->owned_by_drv_count);
 	spin_unlock_irqrestore(&q->done_lock, flags);
 
-	if (state == VB2_BUF_STATE_QUEUED)
+	if (state == VB2_BUF_STATE_QUEUED) {
+		if (q->start_streaming_called)
+			__enqueue_in_driver(vb);
 		return;
+	}
 
 	/* Inform any processes that may be waiting for buffers */
 	wake_up(&q->done_wq);
@@ -1237,6 +1242,23 @@ void vb2_discard_done(struct vb2_queue *q)
 }
 EXPORT_SYMBOL_GPL(vb2_discard_done);
 
+static void vb2_warn_zero_bytesused(struct vb2_buffer *vb)
+{
+	static bool __check_once __read_mostly;
+
+	if (__check_once)
+		return;
+
+	__check_once = true;
+	__WARN();
+
+	pr_warn_once("use of bytesused == 0 is deprecated and will be removed in the future,\n");
+	if (vb->vb2_queue->allow_zero_bytesused)
+		pr_warn_once("use VIDIOC_DECODER_CMD(V4L2_DEC_CMD_STOP) instead.\n");
+	else
+		pr_warn_once("use the actual size instead.\n");
+}
+
 /**
  * __fill_vb2_buffer() - fill a vb2_buffer with information provided in a
  * v4l2_buffer by the userspace. The caller has already verified that struct
@@ -1247,16 +1269,6 @@ static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b
 {
 	unsigned int plane;
 
-	if (V4L2_TYPE_IS_OUTPUT(b->type)) {
-		if (WARN_ON_ONCE(b->bytesused == 0)) {
-			pr_warn_once("use of bytesused == 0 is deprecated and will be removed in the future,\n");
-			if (vb->vb2_queue->allow_zero_bytesused)
-				pr_warn_once("use VIDIOC_DECODER_CMD(V4L2_DEC_CMD_STOP) instead.\n");
-			else
-				pr_warn_once("use the actual size instead.\n");
-		}
-	}
-
 	if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
 		if (b->memory == V4L2_MEMORY_USERPTR) {
 			for (plane = 0; plane < vb->num_planes; ++plane) {
@@ -1297,6 +1309,9 @@ static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b
 				struct v4l2_plane *pdst = &v4l2_planes[plane];
 				struct v4l2_plane *psrc = &b->m.planes[plane];
 
+				if (psrc->bytesused == 0)
+					vb2_warn_zero_bytesused(vb);
+
 				if (vb->vb2_queue->allow_zero_bytesused)
 					pdst->bytesused = psrc->bytesused;
 				else
@@ -1331,6 +1346,9 @@ static void __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b
 		}
 
 		if (V4L2_TYPE_IS_OUTPUT(b->type)) {
+			if (b->bytesused == 0)
+				vb2_warn_zero_bytesused(vb);
+
 			if (vb->vb2_queue->allow_zero_bytesused)
 				v4l2_planes[0].bytesused = b->bytesused;
 			else
@@ -1945,6 +1963,11 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
 			return -EIO;
 		}
 
+		if (q->last_buffer_dequeued) {
+			dprintk(3, "last buffer dequeued already, will not wait for buffers\n");
+			return -EPIPE;
+		}
+
 		if (!list_empty(&q->done_list)) {
 			/*
 			 * Found a buffer that we were waiting for.
@@ -2100,6 +2123,9 @@ static int vb2_internal_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool n
 	/* Remove from videobuf queue */
 	list_del(&vb->queued_entry);
 	q->queued_count--;
+	if (!V4L2_TYPE_IS_OUTPUT(q->type) &&
+	    vb->v4l2_buf.flags & V4L2_BUF_FLAG_LAST)
+		q->last_buffer_dequeued = true;
 	/* go back to dequeued state */
 	__vb2_dqbuf(vb);
 
@@ -2313,6 +2339,7 @@ static int vb2_internal_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
 	 */
 	__vb2_queue_cancel(q);
 	q->waiting_for_buffers = !V4L2_TYPE_IS_OUTPUT(q->type);
+	q->last_buffer_dequeued = false;
 
 	dprintk(3, "successful\n");
 	return 0;
@@ -2655,8 +2682,16 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 	if (V4L2_TYPE_IS_OUTPUT(q->type) && q->queued_count < q->num_buffers)
 		return res | POLLOUT | POLLWRNORM;
 
-	if (list_empty(&q->done_list))
+	if (list_empty(&q->done_list)) {
+		/*
+		 * If the last buffer was dequeued from a capture queue,
+		 * return immediately. DQBUF will return -EPIPE.
+		 */
+		if (q->last_buffer_dequeued)
+			return res | POLLIN | POLLRDNORM;
+
 		poll_wait(file, &q->done_wq, wait);
+	}
 
 	/*
 	 * Take first buffer available for dequeuing.
diff --git a/drivers/media/v4l2-core/videobuf2-dma-contig.c b/drivers/media/v4l2-core/videobuf2-dma-contig.c
index 644dec73d220..94c1e6455d36 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-contig.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-contig.c
@@ -299,7 +299,6 @@ static struct sg_table *vb2_dc_dmabuf_ops_map(
 	/* stealing dmabuf mutex to serialize map/unmap operations */
 	struct mutex *lock = &db_attach->dmabuf->lock;
 	struct sg_table *sgt;
-	int ret;
 
 	mutex_lock(lock);
 
@@ -318,8 +317,9 @@ static struct sg_table *vb2_dc_dmabuf_ops_map(
 	}
 
 	/* mapping to the client with new direction */
-	ret = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents, dma_dir);
-	if (ret <= 0) {
+	sgt->nents = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
+				dma_dir);
+	if (!sgt->nents) {
 		pr_err("failed to map scatterlist\n");
 		mutex_unlock(lock);
 		return ERR_PTR(-EIO);
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index 45c708e463b9..7289b81bd7b7 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -147,8 +147,9 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size,
 	 * No need to sync to the device, this will happen later when the
 	 * prepare() memop is called.
 	 */
-	if (dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->nents,
-			     buf->dma_dir, &attrs) == 0)
+	sgt->nents = dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
+				      buf->dma_dir, &attrs);
+	if (!sgt->nents)
 		goto fail_map;
 
 	buf->handler.refcount = &buf->refcount;
@@ -187,7 +188,7 @@ static void vb2_dma_sg_put(void *buf_priv)
 		dma_set_attr(DMA_ATTR_SKIP_CPU_SYNC, &attrs);
 		dprintk(1, "%s: Freeing buffer of %d pages\n", __func__,
 			buf->num_pages);
-		dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->nents,
+		dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
 				   buf->dma_dir, &attrs);
 		if (buf->vaddr)
 			vm_unmap_ram(buf->vaddr, buf->num_pages);
@@ -314,9 +315,11 @@ static void *vb2_dma_sg_get_userptr(void *alloc_ctx, unsigned long vaddr,
 	 * No need to sync to the device, this will happen later when the
 	 * prepare() memop is called.
 	 */
-	if (dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->nents,
-			     buf->dma_dir, &attrs) == 0)
+	sgt->nents = dma_map_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents,
+				      buf->dma_dir, &attrs);
+	if (!sgt->nents)
 		goto userptr_fail_map;
+
 	return buf;
 
 userptr_fail_map:
@@ -351,7 +354,8 @@ static void vb2_dma_sg_put_userptr(void *buf_priv)
 
 	dprintk(1, "%s: Releasing userspace buffer of %d pages\n",
 	       __func__, buf->num_pages);
-	dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->nents, buf->dma_dir, &attrs);
+	dma_unmap_sg_attrs(buf->dev, sgt->sgl, sgt->orig_nents, buf->dma_dir,
+			   &attrs);
 	if (buf->vaddr)
 		vm_unmap_ram(buf->vaddr, buf->num_pages);
 	sg_free_table(buf->dma_sgt);
@@ -502,7 +506,6 @@ static struct sg_table *vb2_dma_sg_dmabuf_ops_map(
 	/* stealing dmabuf mutex to serialize map/unmap operations */
 	struct mutex *lock = &db_attach->dmabuf->lock;
 	struct sg_table *sgt;
-	int ret;
 
 	mutex_lock(lock);
 
@@ -521,8 +524,9 @@ static struct sg_table *vb2_dma_sg_dmabuf_ops_map(
 	}
 
 	/* mapping to the client with new direction */
-	ret = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents, dma_dir);
-	if (ret <= 0) {
+	sgt->nents = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
+				dma_dir);
+	if (!sgt->nents) {
 		pr_err("failed to map scatterlist\n");
 		mutex_unlock(lock);
 		return ERR_PTR(-EIO);
diff --git a/drivers/media/v4l2-core/videobuf2-vmalloc.c b/drivers/media/v4l2-core/videobuf2-vmalloc.c
index 657ab302a5cf..2fe4c27f524a 100644
--- a/drivers/media/v4l2-core/videobuf2-vmalloc.c
+++ b/drivers/media/v4l2-core/videobuf2-vmalloc.c
@@ -287,7 +287,6 @@ static struct sg_table *vb2_vmalloc_dmabuf_ops_map(
 	/* stealing dmabuf mutex to serialize map/unmap operations */
 	struct mutex *lock = &db_attach->dmabuf->lock;
 	struct sg_table *sgt;
-	int ret;
 
 	mutex_lock(lock);
 
@@ -306,8 +305,9 @@ static struct sg_table *vb2_vmalloc_dmabuf_ops_map(
 	}
 
 	/* mapping to the client with new direction */
-	ret = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents, dma_dir);
-	if (ret <= 0) {
+	sgt->nents = dma_map_sg(db_attach->dev, sgt->sgl, sgt->orig_nents,
+				dma_dir);
+	if (!sgt->nents) {
 		pr_err("failed to map scatterlist\n");
 		mutex_unlock(lock);
 		return ERR_PTR(-EIO);
diff --git a/drivers/memory/Kconfig b/drivers/memory/Kconfig
index 868036f70f8f..8406c668ecdc 100644
--- a/drivers/memory/Kconfig
+++ b/drivers/memory/Kconfig
@@ -49,6 +49,14 @@ config OMAP_GPMC
 	  interfacing to a variety of asynchronous as well as synchronous
 	  memory drives like NOR, NAND, OneNAND, SRAM.
 
+config OMAP_GPMC_DEBUG
+	bool
+	depends on OMAP_GPMC
+	help
+	  Enables verbose debugging mostly to decode the bootloader provided
+	  timings. Enable this during development to configure devices
+	  connected to the GPMC bus.
+
 config MVEBU_DEVBUS
 	bool "Marvell EBU Device Bus Controller"
 	default y
diff --git a/drivers/memory/omap-gpmc.c b/drivers/memory/omap-gpmc.c
index c94ea0d68746..3a27a84ad3ec 100644
--- a/drivers/memory/omap-gpmc.c
+++ b/drivers/memory/omap-gpmc.c
@@ -403,7 +403,7 @@ static void gpmc_cs_bool_timings(int cs, const struct gpmc_bool_timings *p)
 			   p->cycle2cyclediffcsen);
 }
 
-#ifdef DEBUG
+#ifdef CONFIG_OMAP_GPMC_DEBUG
 /**
  * get_gpmc_timing_reg - read a timing parameter and print DTS settings for it.
  * @cs:      Chip Select Region
@@ -612,7 +612,7 @@ static int set_gpmc_timing_reg(int cs, int reg, int st_bit, int end_bit, int max
 	}
 
 	l = gpmc_cs_read_reg(cs, reg);
-#ifdef DEBUG
+#ifdef CONFIG_OMAP_GPMC_DEBUG
 	pr_info(
 		"GPMC CS%d: %-17s: %3d ticks, %3lu ns (was %3i ticks) %3d ns\n",
 	       cs, name, ticks, gpmc_get_clk_period(cs, cd) * ticks / 1000,
@@ -767,7 +767,7 @@ int gpmc_cs_set_timings(int cs, const struct gpmc_timings *t,
 			    GPMC_CONFIG1_CLKACTIVATIONTIME_MAX,
 			    clk_activation, GPMC_CD_FCLK);
 
-#ifdef DEBUG
+#ifdef CONFIG_OMAP_GPMC_DEBUG
 	pr_info("GPMC CS%d CLK period is %lu ns (div %d)\n",
 			cs, (div * gpmc_get_fclk_period()) / 1000, div);
 #endif
@@ -2074,14 +2074,8 @@ static int gpmc_probe_dt(struct platform_device *pdev)
 			ret = gpmc_probe_nand_child(pdev, child);
 		else if (of_node_cmp(child->name, "onenand") == 0)
 			ret = gpmc_probe_onenand_child(pdev, child);
-		else if (of_node_cmp(child->name, "ethernet") == 0 ||
-			 of_node_cmp(child->name, "nor") == 0 ||
-			 of_node_cmp(child->name, "uart") == 0)
+		else
 			ret = gpmc_probe_generic_child(pdev, child);
-
-		if (WARN(ret < 0, "%s: probing gpmc child %s failed\n",
-			 __func__, child->full_name))
-			of_node_put(child);
 	}
 
 	return 0;
diff --git a/drivers/memory/tegra/Kconfig b/drivers/memory/tegra/Kconfig
index 571087621827..6d74e499e18d 100644
--- a/drivers/memory/tegra/Kconfig
+++ b/drivers/memory/tegra/Kconfig
@@ -5,3 +5,13 @@ config TEGRA_MC
 	help
 	  This driver supports the Memory Controller (MC) hardware found on
 	  NVIDIA Tegra SoCs.
+
+config TEGRA124_EMC
+	bool "NVIDIA Tegra124 External Memory Controller driver"
+	default y
+	depends on TEGRA_MC && ARCH_TEGRA_124_SOC
+	help
+	  This driver is for the External Memory Controller (EMC) found on
+	  Tegra124 chips. The EMC controls the external DRAM on the board.
+	  This driver is required to change memory timings / clock rate for
+	  external memory.
diff --git a/drivers/memory/tegra/Makefile b/drivers/memory/tegra/Makefile
index 0d9f497b786c..6a0b9ac54f05 100644
--- a/drivers/memory/tegra/Makefile
+++ b/drivers/memory/tegra/Makefile
@@ -3,5 +3,8 @@ tegra-mc-y := mc.o
 tegra-mc-$(CONFIG_ARCH_TEGRA_3x_SOC)  += tegra30.o
 tegra-mc-$(CONFIG_ARCH_TEGRA_114_SOC) += tegra114.o
 tegra-mc-$(CONFIG_ARCH_TEGRA_124_SOC) += tegra124.o
+tegra-mc-$(CONFIG_ARCH_TEGRA_132_SOC) += tegra124.o
 
 obj-$(CONFIG_TEGRA_MC) += tegra-mc.o
+
+obj-$(CONFIG_TEGRA124_EMC) += tegra124-emc.o
diff --git a/drivers/memory/tegra/mc.c b/drivers/memory/tegra/mc.c
index fe3c44e7e1d1..c71ede67e6c8 100644
--- a/drivers/memory/tegra/mc.c
+++ b/drivers/memory/tegra/mc.c
@@ -13,6 +13,9 @@
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
+#include <linux/sort.h>
+
+#include <soc/tegra/fuse.h>
 
 #include "mc.h"
 
@@ -48,6 +51,9 @@
 #define  MC_EMEM_ARB_CFG_CYCLES_PER_UPDATE_MASK	0x1ff
 #define MC_EMEM_ARB_MISC0 0xd8
 
+#define MC_EMEM_ADR_CFG 0x54
+#define MC_EMEM_ADR_CFG_EMEM_NUMDEV BIT(0)
+
 static const struct of_device_id tegra_mc_of_match[] = {
 #ifdef CONFIG_ARCH_TEGRA_3x_SOC
 	{ .compatible = "nvidia,tegra30-mc", .data = &tegra30_mc_soc },
@@ -58,6 +64,9 @@ static const struct of_device_id tegra_mc_of_match[] = {
 #ifdef CONFIG_ARCH_TEGRA_124_SOC
 	{ .compatible = "nvidia,tegra124-mc", .data = &tegra124_mc_soc },
 #endif
+#ifdef CONFIG_ARCH_TEGRA_132_SOC
+	{ .compatible = "nvidia,tegra132-mc", .data = &tegra132_mc_soc },
+#endif
 	{ }
 };
 MODULE_DEVICE_TABLE(of, tegra_mc_of_match);
@@ -91,6 +100,130 @@ static int tegra_mc_setup_latency_allowance(struct tegra_mc *mc)
 	return 0;
 }
 
+void tegra_mc_write_emem_configuration(struct tegra_mc *mc, unsigned long rate)
+{
+	unsigned int i;
+	struct tegra_mc_timing *timing = NULL;
+
+	for (i = 0; i < mc->num_timings; i++) {
+		if (mc->timings[i].rate == rate) {
+			timing = &mc->timings[i];
+			break;
+		}
+	}
+
+	if (!timing) {
+		dev_err(mc->dev, "no memory timing registered for rate %lu\n",
+			rate);
+		return;
+	}
+
+	for (i = 0; i < mc->soc->num_emem_regs; ++i)
+		mc_writel(mc, timing->emem_data[i], mc->soc->emem_regs[i]);
+}
+
+unsigned int tegra_mc_get_emem_device_count(struct tegra_mc *mc)
+{
+	u8 dram_count;
+
+	dram_count = mc_readl(mc, MC_EMEM_ADR_CFG);
+	dram_count &= MC_EMEM_ADR_CFG_EMEM_NUMDEV;
+	dram_count++;
+
+	return dram_count;
+}
+
+static int load_one_timing(struct tegra_mc *mc,
+			   struct tegra_mc_timing *timing,
+			   struct device_node *node)
+{
+	int err;
+	u32 tmp;
+
+	err = of_property_read_u32(node, "clock-frequency", &tmp);
+	if (err) {
+		dev_err(mc->dev,
+			"timing %s: failed to read rate\n", node->name);
+		return err;
+	}
+
+	timing->rate = tmp;
+	timing->emem_data = devm_kcalloc(mc->dev, mc->soc->num_emem_regs,
+					 sizeof(u32), GFP_KERNEL);
+	if (!timing->emem_data)
+		return -ENOMEM;
+
+	err = of_property_read_u32_array(node, "nvidia,emem-configuration",
+					 timing->emem_data,
+					 mc->soc->num_emem_regs);
+	if (err) {
+		dev_err(mc->dev,
+			"timing %s: failed to read EMEM configuration\n",
+			node->name);
+		return err;
+	}
+
+	return 0;
+}
+
+static int load_timings(struct tegra_mc *mc, struct device_node *node)
+{
+	struct device_node *child;
+	struct tegra_mc_timing *timing;
+	int child_count = of_get_child_count(node);
+	int i = 0, err;
+
+	mc->timings = devm_kcalloc(mc->dev, child_count, sizeof(*timing),
+				   GFP_KERNEL);
+	if (!mc->timings)
+		return -ENOMEM;
+
+	mc->num_timings = child_count;
+
+	for_each_child_of_node(node, child) {
+		timing = &mc->timings[i++];
+
+		err = load_one_timing(mc, timing, child);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int tegra_mc_setup_timings(struct tegra_mc *mc)
+{
+	struct device_node *node;
+	u32 ram_code, node_ram_code;
+	int err;
+
+	ram_code = tegra_read_ram_code();
+
+	mc->num_timings = 0;
+
+	for_each_child_of_node(mc->dev->of_node, node) {
+		err = of_property_read_u32(node, "nvidia,ram-code",
+					   &node_ram_code);
+		if (err || (node_ram_code != ram_code)) {
+			of_node_put(node);
+			continue;
+		}
+
+		err = load_timings(mc, node);
+		if (err)
+			return err;
+		of_node_put(node);
+		break;
+	}
+
+	if (mc->num_timings == 0)
+		dev_warn(mc->dev,
+			 "no memory timings for RAM code %u registered\n",
+			 ram_code);
+
+	return 0;
+}
+
 static const char *const status_names[32] = {
 	[ 1] = "External interrupt",
 	[ 6] = "EMEM address decode error",
@@ -248,6 +381,12 @@ static int tegra_mc_probe(struct platform_device *pdev)
 		return err;
 	}
 
+	err = tegra_mc_setup_timings(mc);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to setup timings: %d\n", err);
+		return err;
+	}
+
 	if (IS_ENABLED(CONFIG_TEGRA_IOMMU_SMMU)) {
 		mc->smmu = tegra_smmu_probe(&pdev->dev, mc->soc->smmu, mc);
 		if (IS_ERR(mc->smmu)) {
@@ -273,8 +412,8 @@ static int tegra_mc_probe(struct platform_device *pdev)
 
 	value = MC_INT_DECERR_MTS | MC_INT_SECERR_SEC | MC_INT_DECERR_VPR |
 		MC_INT_INVALID_APB_ASID_UPDATE | MC_INT_INVALID_SMMU_PAGE |
-		MC_INT_ARBITRATION_EMEM | MC_INT_SECURITY_VIOLATION |
-		MC_INT_DECERR_EMEM;
+		MC_INT_SECURITY_VIOLATION | MC_INT_DECERR_EMEM;
+
 	mc_writel(mc, value, MC_INTMASK);
 
 	return 0;
diff --git a/drivers/memory/tegra/mc.h b/drivers/memory/tegra/mc.h
index d5d21147fc77..b7361b0a6696 100644
--- a/drivers/memory/tegra/mc.h
+++ b/drivers/memory/tegra/mc.h
@@ -37,4 +37,8 @@ extern const struct tegra_mc_soc tegra114_mc_soc;
 extern const struct tegra_mc_soc tegra124_mc_soc;
 #endif
 
+#ifdef CONFIG_ARCH_TEGRA_132_SOC
+extern const struct tegra_mc_soc tegra132_mc_soc;
+#endif
+
 #endif /* MEMORY_TEGRA_MC_H */
diff --git a/drivers/memory/tegra/tegra114.c b/drivers/memory/tegra/tegra114.c
index 511e9a25c151..9f579589e800 100644
--- a/drivers/memory/tegra/tegra114.c
+++ b/drivers/memory/tegra/tegra114.c
@@ -896,22 +896,22 @@ static const struct tegra_mc_client tegra114_mc_clients[] = {
 };
 
 static const struct tegra_smmu_swgroup tegra114_swgroups[] = {
-	{ .swgroup = TEGRA_SWGROUP_DC,        .reg = 0x240 },
-	{ .swgroup = TEGRA_SWGROUP_DCB,       .reg = 0x244 },
-	{ .swgroup = TEGRA_SWGROUP_EPP,       .reg = 0x248 },
-	{ .swgroup = TEGRA_SWGROUP_G2,        .reg = 0x24c },
-	{ .swgroup = TEGRA_SWGROUP_AVPC,      .reg = 0x23c },
-	{ .swgroup = TEGRA_SWGROUP_NV,        .reg = 0x268 },
-	{ .swgroup = TEGRA_SWGROUP_HDA,       .reg = 0x254 },
-	{ .swgroup = TEGRA_SWGROUP_HC,        .reg = 0x250 },
-	{ .swgroup = TEGRA_SWGROUP_MSENC,     .reg = 0x264 },
-	{ .swgroup = TEGRA_SWGROUP_PPCS,      .reg = 0x270 },
-	{ .swgroup = TEGRA_SWGROUP_VDE,       .reg = 0x27c },
-	{ .swgroup = TEGRA_SWGROUP_VI,        .reg = 0x280 },
-	{ .swgroup = TEGRA_SWGROUP_ISP,       .reg = 0x258 },
-	{ .swgroup = TEGRA_SWGROUP_XUSB_HOST, .reg = 0x288 },
-	{ .swgroup = TEGRA_SWGROUP_XUSB_DEV,  .reg = 0x28c },
-	{ .swgroup = TEGRA_SWGROUP_TSEC,      .reg = 0x294 },
+	{ .name = "dc",        .swgroup = TEGRA_SWGROUP_DC,        .reg = 0x240 },
+	{ .name = "dcb",       .swgroup = TEGRA_SWGROUP_DCB,       .reg = 0x244 },
+	{ .name = "epp",       .swgroup = TEGRA_SWGROUP_EPP,       .reg = 0x248 },
+	{ .name = "g2",        .swgroup = TEGRA_SWGROUP_G2,        .reg = 0x24c },
+	{ .name = "avpc",      .swgroup = TEGRA_SWGROUP_AVPC,      .reg = 0x23c },
+	{ .name = "nv",        .swgroup = TEGRA_SWGROUP_NV,        .reg = 0x268 },
+	{ .name = "hda",       .swgroup = TEGRA_SWGROUP_HDA,       .reg = 0x254 },
+	{ .name = "hc",        .swgroup = TEGRA_SWGROUP_HC,        .reg = 0x250 },
+	{ .name = "msenc",     .swgroup = TEGRA_SWGROUP_MSENC,     .reg = 0x264 },
+	{ .name = "ppcs",      .swgroup = TEGRA_SWGROUP_PPCS,      .reg = 0x270 },
+	{ .name = "vde",       .swgroup = TEGRA_SWGROUP_VDE,       .reg = 0x27c },
+	{ .name = "vi",        .swgroup = TEGRA_SWGROUP_VI,        .reg = 0x280 },
+	{ .name = "isp",       .swgroup = TEGRA_SWGROUP_ISP,       .reg = 0x258 },
+	{ .name = "xusb_host", .swgroup = TEGRA_SWGROUP_XUSB_HOST, .reg = 0x288 },
+	{ .name = "xusb_dev",  .swgroup = TEGRA_SWGROUP_XUSB_DEV,  .reg = 0x28c },
+	{ .name = "tsec",      .swgroup = TEGRA_SWGROUP_TSEC,      .reg = 0x294 },
 };
 
 static void tegra114_flush_dcache(struct page *page, unsigned long offset,
diff --git a/drivers/memory/tegra/tegra124-emc.c b/drivers/memory/tegra/tegra124-emc.c
new file mode 100644
index 000000000000..8620355776fe
--- /dev/null
+++ b/drivers/memory/tegra/tegra124-emc.c
@@ -0,0 +1,1140 @@
+/*
+ * 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/clk-provider.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/sort.h>
+#include <linux/string.h>
+
+#include <soc/tegra/emc.h>
+#include <soc/tegra/fuse.h>
+#include <soc/tegra/mc.h>
+
+#define EMC_FBIO_CFG5				0x104
+#define	EMC_FBIO_CFG5_DRAM_TYPE_MASK		0x3
+#define	EMC_FBIO_CFG5_DRAM_TYPE_SHIFT		0
+
+#define EMC_INTSTATUS				0x0
+#define EMC_INTSTATUS_CLKCHANGE_COMPLETE	BIT(4)
+
+#define EMC_CFG					0xc
+#define EMC_CFG_DRAM_CLKSTOP_PD			BIT(31)
+#define EMC_CFG_DRAM_CLKSTOP_SR			BIT(30)
+#define EMC_CFG_DRAM_ACPD			BIT(29)
+#define EMC_CFG_DYN_SREF			BIT(28)
+#define EMC_CFG_PWR_MASK			((0xF << 28) | BIT(18))
+#define EMC_CFG_DSR_VTTGEN_DRV_EN		BIT(18)
+
+#define EMC_REFCTRL				0x20
+#define EMC_REFCTRL_DEV_SEL_SHIFT		0
+#define EMC_REFCTRL_ENABLE			BIT(31)
+
+#define EMC_TIMING_CONTROL			0x28
+#define EMC_RC					0x2c
+#define EMC_RFC					0x30
+#define EMC_RAS					0x34
+#define EMC_RP					0x38
+#define EMC_R2W					0x3c
+#define EMC_W2R					0x40
+#define EMC_R2P					0x44
+#define EMC_W2P					0x48
+#define EMC_RD_RCD				0x4c
+#define EMC_WR_RCD				0x50
+#define EMC_RRD					0x54
+#define EMC_REXT				0x58
+#define EMC_WDV					0x5c
+#define EMC_QUSE				0x60
+#define EMC_QRST				0x64
+#define EMC_QSAFE				0x68
+#define EMC_RDV					0x6c
+#define EMC_REFRESH				0x70
+#define EMC_BURST_REFRESH_NUM			0x74
+#define EMC_PDEX2WR				0x78
+#define EMC_PDEX2RD				0x7c
+#define EMC_PCHG2PDEN				0x80
+#define EMC_ACT2PDEN				0x84
+#define EMC_AR2PDEN				0x88
+#define EMC_RW2PDEN				0x8c
+#define EMC_TXSR				0x90
+#define EMC_TCKE				0x94
+#define EMC_TFAW				0x98
+#define EMC_TRPAB				0x9c
+#define EMC_TCLKSTABLE				0xa0
+#define EMC_TCLKSTOP				0xa4
+#define EMC_TREFBW				0xa8
+#define EMC_ODT_WRITE				0xb0
+#define EMC_ODT_READ				0xb4
+#define EMC_WEXT				0xb8
+#define EMC_CTT					0xbc
+#define EMC_RFC_SLR				0xc0
+#define EMC_MRS_WAIT_CNT2			0xc4
+
+#define EMC_MRS_WAIT_CNT			0xc8
+#define EMC_MRS_WAIT_CNT_SHORT_WAIT_SHIFT	0
+#define EMC_MRS_WAIT_CNT_SHORT_WAIT_MASK	\
+	(0x3FF << EMC_MRS_WAIT_CNT_SHORT_WAIT_SHIFT)
+#define EMC_MRS_WAIT_CNT_LONG_WAIT_SHIFT	16
+#define EMC_MRS_WAIT_CNT_LONG_WAIT_MASK		\
+	(0x3FF << EMC_MRS_WAIT_CNT_LONG_WAIT_SHIFT)
+
+#define EMC_MRS					0xcc
+#define EMC_MODE_SET_DLL_RESET			BIT(8)
+#define EMC_MODE_SET_LONG_CNT			BIT(26)
+#define EMC_EMRS				0xd0
+#define EMC_REF					0xd4
+#define EMC_PRE					0xd8
+
+#define EMC_SELF_REF				0xe0
+#define EMC_SELF_REF_CMD_ENABLED		BIT(0)
+#define EMC_SELF_REF_DEV_SEL_SHIFT		30
+
+#define EMC_MRW					0xe8
+
+#define EMC_MRR					0xec
+#define EMC_MRR_MA_SHIFT			16
+#define LPDDR2_MR4_TEMP_SHIFT			0
+
+#define EMC_XM2DQSPADCTRL3			0xf8
+#define EMC_FBIO_SPARE				0x100
+
+#define EMC_FBIO_CFG6				0x114
+#define EMC_EMRS2				0x12c
+#define EMC_MRW2				0x134
+#define EMC_MRW4				0x13c
+#define EMC_EINPUT				0x14c
+#define EMC_EINPUT_DURATION			0x150
+#define EMC_PUTERM_EXTRA			0x154
+#define EMC_TCKESR				0x158
+#define EMC_TPD					0x15c
+
+#define EMC_AUTO_CAL_CONFIG			0x2a4
+#define EMC_AUTO_CAL_CONFIG_AUTO_CAL_START	BIT(31)
+#define EMC_AUTO_CAL_INTERVAL			0x2a8
+#define EMC_AUTO_CAL_STATUS			0x2ac
+#define EMC_AUTO_CAL_STATUS_ACTIVE		BIT(31)
+#define EMC_STATUS				0x2b4
+#define EMC_STATUS_TIMING_UPDATE_STALLED	BIT(23)
+
+#define EMC_CFG_2				0x2b8
+#define EMC_CFG_2_MODE_SHIFT			0
+#define EMC_CFG_2_DIS_STP_OB_CLK_DURING_NON_WR	BIT(6)
+
+#define EMC_CFG_DIG_DLL				0x2bc
+#define EMC_CFG_DIG_DLL_PERIOD			0x2c0
+#define EMC_RDV_MASK				0x2cc
+#define EMC_WDV_MASK				0x2d0
+#define EMC_CTT_DURATION			0x2d8
+#define EMC_CTT_TERM_CTRL			0x2dc
+#define EMC_ZCAL_INTERVAL			0x2e0
+#define EMC_ZCAL_WAIT_CNT			0x2e4
+
+#define EMC_ZQ_CAL				0x2ec
+#define EMC_ZQ_CAL_CMD				BIT(0)
+#define EMC_ZQ_CAL_LONG				BIT(4)
+#define EMC_ZQ_CAL_LONG_CMD_DEV0		\
+	(DRAM_DEV_SEL_0 | EMC_ZQ_CAL_LONG | EMC_ZQ_CAL_CMD)
+#define EMC_ZQ_CAL_LONG_CMD_DEV1		\
+	(DRAM_DEV_SEL_1 | EMC_ZQ_CAL_LONG | EMC_ZQ_CAL_CMD)
+
+#define EMC_XM2CMDPADCTRL			0x2f0
+#define EMC_XM2DQSPADCTRL			0x2f8
+#define EMC_XM2DQSPADCTRL2			0x2fc
+#define EMC_XM2DQSPADCTRL2_RX_FT_REC_ENABLE	BIT(0)
+#define EMC_XM2DQSPADCTRL2_VREF_ENABLE		BIT(5)
+#define EMC_XM2DQPADCTRL			0x300
+#define EMC_XM2DQPADCTRL2			0x304
+#define EMC_XM2CLKPADCTRL			0x308
+#define EMC_XM2COMPPADCTRL			0x30c
+#define EMC_XM2VTTGENPADCTRL			0x310
+#define EMC_XM2VTTGENPADCTRL2			0x314
+#define EMC_XM2VTTGENPADCTRL3			0x318
+#define EMC_XM2DQSPADCTRL4			0x320
+#define EMC_DLL_XFORM_DQS0			0x328
+#define EMC_DLL_XFORM_DQS1			0x32c
+#define EMC_DLL_XFORM_DQS2			0x330
+#define EMC_DLL_XFORM_DQS3			0x334
+#define EMC_DLL_XFORM_DQS4			0x338
+#define EMC_DLL_XFORM_DQS5			0x33c
+#define EMC_DLL_XFORM_DQS6			0x340
+#define EMC_DLL_XFORM_DQS7			0x344
+#define EMC_DLL_XFORM_QUSE0			0x348
+#define EMC_DLL_XFORM_QUSE1			0x34c
+#define EMC_DLL_XFORM_QUSE2			0x350
+#define EMC_DLL_XFORM_QUSE3			0x354
+#define EMC_DLL_XFORM_QUSE4			0x358
+#define EMC_DLL_XFORM_QUSE5			0x35c
+#define EMC_DLL_XFORM_QUSE6			0x360
+#define EMC_DLL_XFORM_QUSE7			0x364
+#define EMC_DLL_XFORM_DQ0			0x368
+#define EMC_DLL_XFORM_DQ1			0x36c
+#define EMC_DLL_XFORM_DQ2			0x370
+#define EMC_DLL_XFORM_DQ3			0x374
+#define EMC_DLI_TRIM_TXDQS0			0x3a8
+#define EMC_DLI_TRIM_TXDQS1			0x3ac
+#define EMC_DLI_TRIM_TXDQS2			0x3b0
+#define EMC_DLI_TRIM_TXDQS3			0x3b4
+#define EMC_DLI_TRIM_TXDQS4			0x3b8
+#define EMC_DLI_TRIM_TXDQS5			0x3bc
+#define EMC_DLI_TRIM_TXDQS6			0x3c0
+#define EMC_DLI_TRIM_TXDQS7			0x3c4
+#define EMC_STALL_THEN_EXE_AFTER_CLKCHANGE	0x3cc
+#define EMC_SEL_DPD_CTRL			0x3d8
+#define EMC_SEL_DPD_CTRL_DATA_SEL_DPD		BIT(8)
+#define EMC_SEL_DPD_CTRL_ODT_SEL_DPD		BIT(5)
+#define EMC_SEL_DPD_CTRL_RESET_SEL_DPD		BIT(4)
+#define EMC_SEL_DPD_CTRL_CA_SEL_DPD		BIT(3)
+#define EMC_SEL_DPD_CTRL_CLK_SEL_DPD		BIT(2)
+#define EMC_SEL_DPD_CTRL_DDR3_MASK	\
+	((0xf << 2) | BIT(8))
+#define EMC_SEL_DPD_CTRL_MASK \
+	((0x3 << 2) | BIT(5) | BIT(8))
+#define EMC_PRE_REFRESH_REQ_CNT			0x3dc
+#define EMC_DYN_SELF_REF_CONTROL		0x3e0
+#define EMC_TXSRDLL				0x3e4
+#define EMC_CCFIFO_ADDR				0x3e8
+#define EMC_CCFIFO_DATA				0x3ec
+#define EMC_CCFIFO_STATUS			0x3f0
+#define EMC_CDB_CNTL_1				0x3f4
+#define EMC_CDB_CNTL_2				0x3f8
+#define EMC_XM2CLKPADCTRL2			0x3fc
+#define EMC_AUTO_CAL_CONFIG2			0x458
+#define EMC_AUTO_CAL_CONFIG3			0x45c
+#define EMC_IBDLY				0x468
+#define EMC_DLL_XFORM_ADDR0			0x46c
+#define EMC_DLL_XFORM_ADDR1			0x470
+#define EMC_DLL_XFORM_ADDR2			0x474
+#define EMC_DSR_VTTGEN_DRV			0x47c
+#define EMC_TXDSRVTTGEN				0x480
+#define EMC_XM2CMDPADCTRL4			0x484
+#define EMC_XM2CMDPADCTRL5			0x488
+#define EMC_DLL_XFORM_DQS8			0x4a0
+#define EMC_DLL_XFORM_DQS9			0x4a4
+#define EMC_DLL_XFORM_DQS10			0x4a8
+#define EMC_DLL_XFORM_DQS11			0x4ac
+#define EMC_DLL_XFORM_DQS12			0x4b0
+#define EMC_DLL_XFORM_DQS13			0x4b4
+#define EMC_DLL_XFORM_DQS14			0x4b8
+#define EMC_DLL_XFORM_DQS15			0x4bc
+#define EMC_DLL_XFORM_QUSE8			0x4c0
+#define EMC_DLL_XFORM_QUSE9			0x4c4
+#define EMC_DLL_XFORM_QUSE10			0x4c8
+#define EMC_DLL_XFORM_QUSE11			0x4cc
+#define EMC_DLL_XFORM_QUSE12			0x4d0
+#define EMC_DLL_XFORM_QUSE13			0x4d4
+#define EMC_DLL_XFORM_QUSE14			0x4d8
+#define EMC_DLL_XFORM_QUSE15			0x4dc
+#define EMC_DLL_XFORM_DQ4			0x4e0
+#define EMC_DLL_XFORM_DQ5			0x4e4
+#define EMC_DLL_XFORM_DQ6			0x4e8
+#define EMC_DLL_XFORM_DQ7			0x4ec
+#define EMC_DLI_TRIM_TXDQS8			0x520
+#define EMC_DLI_TRIM_TXDQS9			0x524
+#define EMC_DLI_TRIM_TXDQS10			0x528
+#define EMC_DLI_TRIM_TXDQS11			0x52c
+#define EMC_DLI_TRIM_TXDQS12			0x530
+#define EMC_DLI_TRIM_TXDQS13			0x534
+#define EMC_DLI_TRIM_TXDQS14			0x538
+#define EMC_DLI_TRIM_TXDQS15			0x53c
+#define EMC_CDB_CNTL_3				0x540
+#define EMC_XM2DQSPADCTRL5			0x544
+#define EMC_XM2DQSPADCTRL6			0x548
+#define EMC_XM2DQPADCTRL3			0x54c
+#define EMC_DLL_XFORM_ADDR3			0x550
+#define EMC_DLL_XFORM_ADDR4			0x554
+#define EMC_DLL_XFORM_ADDR5			0x558
+#define EMC_CFG_PIPE				0x560
+#define EMC_QPOP				0x564
+#define EMC_QUSE_WIDTH				0x568
+#define EMC_PUTERM_WIDTH			0x56c
+#define EMC_BGBIAS_CTL0				0x570
+#define EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD_IBIAS_RX BIT(3)
+#define EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD_IBIAS_VTTGEN BIT(2)
+#define EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD	BIT(1)
+#define EMC_PUTERM_ADJ				0x574
+
+#define DRAM_DEV_SEL_ALL			0
+#define DRAM_DEV_SEL_0				(2 << 30)
+#define DRAM_DEV_SEL_1				(1 << 30)
+
+#define EMC_CFG_POWER_FEATURES_MASK		\
+	(EMC_CFG_DYN_SREF | EMC_CFG_DRAM_ACPD | EMC_CFG_DRAM_CLKSTOP_SR | \
+	EMC_CFG_DRAM_CLKSTOP_PD | EMC_CFG_DSR_VTTGEN_DRV_EN)
+#define EMC_REFCTRL_DEV_SEL(n) (((n > 1) ? 0 : 2) << EMC_REFCTRL_DEV_SEL_SHIFT)
+#define EMC_DRAM_DEV_SEL(n) ((n > 1) ? DRAM_DEV_SEL_ALL : DRAM_DEV_SEL_0)
+
+/* Maximum amount of time in us. to wait for changes to become effective */
+#define EMC_STATUS_UPDATE_TIMEOUT		1000
+
+enum emc_dram_type {
+	DRAM_TYPE_DDR3 = 0,
+	DRAM_TYPE_DDR1 = 1,
+	DRAM_TYPE_LPDDR3 = 2,
+	DRAM_TYPE_DDR2 = 3
+};
+
+enum emc_dll_change {
+	DLL_CHANGE_NONE,
+	DLL_CHANGE_ON,
+	DLL_CHANGE_OFF
+};
+
+static const unsigned long emc_burst_regs[] = {
+	EMC_RC,
+	EMC_RFC,
+	EMC_RFC_SLR,
+	EMC_RAS,
+	EMC_RP,
+	EMC_R2W,
+	EMC_W2R,
+	EMC_R2P,
+	EMC_W2P,
+	EMC_RD_RCD,
+	EMC_WR_RCD,
+	EMC_RRD,
+	EMC_REXT,
+	EMC_WEXT,
+	EMC_WDV,
+	EMC_WDV_MASK,
+	EMC_QUSE,
+	EMC_QUSE_WIDTH,
+	EMC_IBDLY,
+	EMC_EINPUT,
+	EMC_EINPUT_DURATION,
+	EMC_PUTERM_EXTRA,
+	EMC_PUTERM_WIDTH,
+	EMC_PUTERM_ADJ,
+	EMC_CDB_CNTL_1,
+	EMC_CDB_CNTL_2,
+	EMC_CDB_CNTL_3,
+	EMC_QRST,
+	EMC_QSAFE,
+	EMC_RDV,
+	EMC_RDV_MASK,
+	EMC_REFRESH,
+	EMC_BURST_REFRESH_NUM,
+	EMC_PRE_REFRESH_REQ_CNT,
+	EMC_PDEX2WR,
+	EMC_PDEX2RD,
+	EMC_PCHG2PDEN,
+	EMC_ACT2PDEN,
+	EMC_AR2PDEN,
+	EMC_RW2PDEN,
+	EMC_TXSR,
+	EMC_TXSRDLL,
+	EMC_TCKE,
+	EMC_TCKESR,
+	EMC_TPD,
+	EMC_TFAW,
+	EMC_TRPAB,
+	EMC_TCLKSTABLE,
+	EMC_TCLKSTOP,
+	EMC_TREFBW,
+	EMC_FBIO_CFG6,
+	EMC_ODT_WRITE,
+	EMC_ODT_READ,
+	EMC_FBIO_CFG5,
+	EMC_CFG_DIG_DLL,
+	EMC_CFG_DIG_DLL_PERIOD,
+	EMC_DLL_XFORM_DQS0,
+	EMC_DLL_XFORM_DQS1,
+	EMC_DLL_XFORM_DQS2,
+	EMC_DLL_XFORM_DQS3,
+	EMC_DLL_XFORM_DQS4,
+	EMC_DLL_XFORM_DQS5,
+	EMC_DLL_XFORM_DQS6,
+	EMC_DLL_XFORM_DQS7,
+	EMC_DLL_XFORM_DQS8,
+	EMC_DLL_XFORM_DQS9,
+	EMC_DLL_XFORM_DQS10,
+	EMC_DLL_XFORM_DQS11,
+	EMC_DLL_XFORM_DQS12,
+	EMC_DLL_XFORM_DQS13,
+	EMC_DLL_XFORM_DQS14,
+	EMC_DLL_XFORM_DQS15,
+	EMC_DLL_XFORM_QUSE0,
+	EMC_DLL_XFORM_QUSE1,
+	EMC_DLL_XFORM_QUSE2,
+	EMC_DLL_XFORM_QUSE3,
+	EMC_DLL_XFORM_QUSE4,
+	EMC_DLL_XFORM_QUSE5,
+	EMC_DLL_XFORM_QUSE6,
+	EMC_DLL_XFORM_QUSE7,
+	EMC_DLL_XFORM_ADDR0,
+	EMC_DLL_XFORM_ADDR1,
+	EMC_DLL_XFORM_ADDR2,
+	EMC_DLL_XFORM_ADDR3,
+	EMC_DLL_XFORM_ADDR4,
+	EMC_DLL_XFORM_ADDR5,
+	EMC_DLL_XFORM_QUSE8,
+	EMC_DLL_XFORM_QUSE9,
+	EMC_DLL_XFORM_QUSE10,
+	EMC_DLL_XFORM_QUSE11,
+	EMC_DLL_XFORM_QUSE12,
+	EMC_DLL_XFORM_QUSE13,
+	EMC_DLL_XFORM_QUSE14,
+	EMC_DLL_XFORM_QUSE15,
+	EMC_DLI_TRIM_TXDQS0,
+	EMC_DLI_TRIM_TXDQS1,
+	EMC_DLI_TRIM_TXDQS2,
+	EMC_DLI_TRIM_TXDQS3,
+	EMC_DLI_TRIM_TXDQS4,
+	EMC_DLI_TRIM_TXDQS5,
+	EMC_DLI_TRIM_TXDQS6,
+	EMC_DLI_TRIM_TXDQS7,
+	EMC_DLI_TRIM_TXDQS8,
+	EMC_DLI_TRIM_TXDQS9,
+	EMC_DLI_TRIM_TXDQS10,
+	EMC_DLI_TRIM_TXDQS11,
+	EMC_DLI_TRIM_TXDQS12,
+	EMC_DLI_TRIM_TXDQS13,
+	EMC_DLI_TRIM_TXDQS14,
+	EMC_DLI_TRIM_TXDQS15,
+	EMC_DLL_XFORM_DQ0,
+	EMC_DLL_XFORM_DQ1,
+	EMC_DLL_XFORM_DQ2,
+	EMC_DLL_XFORM_DQ3,
+	EMC_DLL_XFORM_DQ4,
+	EMC_DLL_XFORM_DQ5,
+	EMC_DLL_XFORM_DQ6,
+	EMC_DLL_XFORM_DQ7,
+	EMC_XM2CMDPADCTRL,
+	EMC_XM2CMDPADCTRL4,
+	EMC_XM2CMDPADCTRL5,
+	EMC_XM2DQPADCTRL2,
+	EMC_XM2DQPADCTRL3,
+	EMC_XM2CLKPADCTRL,
+	EMC_XM2CLKPADCTRL2,
+	EMC_XM2COMPPADCTRL,
+	EMC_XM2VTTGENPADCTRL,
+	EMC_XM2VTTGENPADCTRL2,
+	EMC_XM2VTTGENPADCTRL3,
+	EMC_XM2DQSPADCTRL3,
+	EMC_XM2DQSPADCTRL4,
+	EMC_XM2DQSPADCTRL5,
+	EMC_XM2DQSPADCTRL6,
+	EMC_DSR_VTTGEN_DRV,
+	EMC_TXDSRVTTGEN,
+	EMC_FBIO_SPARE,
+	EMC_ZCAL_WAIT_CNT,
+	EMC_MRS_WAIT_CNT2,
+	EMC_CTT,
+	EMC_CTT_DURATION,
+	EMC_CFG_PIPE,
+	EMC_DYN_SELF_REF_CONTROL,
+	EMC_QPOP
+};
+
+struct emc_timing {
+	unsigned long rate;
+
+	u32 emc_burst_data[ARRAY_SIZE(emc_burst_regs)];
+
+	u32 emc_auto_cal_config;
+	u32 emc_auto_cal_config2;
+	u32 emc_auto_cal_config3;
+	u32 emc_auto_cal_interval;
+	u32 emc_bgbias_ctl0;
+	u32 emc_cfg;
+	u32 emc_cfg_2;
+	u32 emc_ctt_term_ctrl;
+	u32 emc_mode_1;
+	u32 emc_mode_2;
+	u32 emc_mode_4;
+	u32 emc_mode_reset;
+	u32 emc_mrs_wait_cnt;
+	u32 emc_sel_dpd_ctrl;
+	u32 emc_xm2dqspadctrl2;
+	u32 emc_zcal_cnt_long;
+	u32 emc_zcal_interval;
+};
+
+struct tegra_emc {
+	struct device *dev;
+
+	struct tegra_mc *mc;
+
+	void __iomem *regs;
+
+	enum emc_dram_type dram_type;
+	unsigned int dram_num;
+
+	struct emc_timing last_timing;
+	struct emc_timing *timings;
+	unsigned int num_timings;
+};
+
+/* Timing change sequence functions */
+
+static void emc_ccfifo_writel(struct tegra_emc *emc, u32 value,
+			      unsigned long offset)
+{
+	writel(value, emc->regs + EMC_CCFIFO_DATA);
+	writel(offset, emc->regs + EMC_CCFIFO_ADDR);
+}
+
+static void emc_seq_update_timing(struct tegra_emc *emc)
+{
+	unsigned int i;
+	u32 value;
+
+	writel(1, emc->regs + EMC_TIMING_CONTROL);
+
+	for (i = 0; i < EMC_STATUS_UPDATE_TIMEOUT; ++i) {
+		value = readl(emc->regs + EMC_STATUS);
+		if ((value & EMC_STATUS_TIMING_UPDATE_STALLED) == 0)
+			return;
+		udelay(1);
+	}
+
+	dev_err(emc->dev, "timing update timed out\n");
+}
+
+static void emc_seq_disable_auto_cal(struct tegra_emc *emc)
+{
+	unsigned int i;
+	u32 value;
+
+	writel(0, emc->regs + EMC_AUTO_CAL_INTERVAL);
+
+	for (i = 0; i < EMC_STATUS_UPDATE_TIMEOUT; ++i) {
+		value = readl(emc->regs + EMC_AUTO_CAL_STATUS);
+		if ((value & EMC_AUTO_CAL_STATUS_ACTIVE) == 0)
+			return;
+		udelay(1);
+	}
+
+	dev_err(emc->dev, "auto cal disable timed out\n");
+}
+
+static void emc_seq_wait_clkchange(struct tegra_emc *emc)
+{
+	unsigned int i;
+	u32 value;
+
+	for (i = 0; i < EMC_STATUS_UPDATE_TIMEOUT; ++i) {
+		value = readl(emc->regs + EMC_INTSTATUS);
+		if (value & EMC_INTSTATUS_CLKCHANGE_COMPLETE)
+			return;
+		udelay(1);
+	}
+
+	dev_err(emc->dev, "clock change timed out\n");
+}
+
+static struct emc_timing *tegra_emc_find_timing(struct tegra_emc *emc,
+						unsigned long rate)
+{
+	struct emc_timing *timing = NULL;
+	unsigned int i;
+
+	for (i = 0; i < emc->num_timings; i++) {
+		if (emc->timings[i].rate == rate) {
+			timing = &emc->timings[i];
+			break;
+		}
+	}
+
+	if (!timing) {
+		dev_err(emc->dev, "no timing for rate %lu\n", rate);
+		return NULL;
+	}
+
+	return timing;
+}
+
+int tegra_emc_prepare_timing_change(struct tegra_emc *emc,
+				    unsigned long rate)
+{
+	struct emc_timing *timing = tegra_emc_find_timing(emc, rate);
+	struct emc_timing *last = &emc->last_timing;
+	enum emc_dll_change dll_change;
+	unsigned int pre_wait = 0;
+	u32 val, val2, mask;
+	bool update = false;
+	unsigned int i;
+
+	if (!timing)
+		return -ENOENT;
+
+	if ((last->emc_mode_1 & 0x1) == (timing->emc_mode_1 & 0x1))
+		dll_change = DLL_CHANGE_NONE;
+	else if (timing->emc_mode_1 & 0x1)
+		dll_change = DLL_CHANGE_ON;
+	else
+		dll_change = DLL_CHANGE_OFF;
+
+	/* Clear CLKCHANGE_COMPLETE interrupts */
+	writel(EMC_INTSTATUS_CLKCHANGE_COMPLETE, emc->regs + EMC_INTSTATUS);
+
+	/* Disable dynamic self-refresh */
+	val = readl(emc->regs + EMC_CFG);
+	if (val & EMC_CFG_PWR_MASK) {
+		val &= ~EMC_CFG_POWER_FEATURES_MASK;
+		writel(val, emc->regs + EMC_CFG);
+
+		pre_wait = 5;
+	}
+
+	/* Disable SEL_DPD_CTRL for clock change */
+	if (emc->dram_type == DRAM_TYPE_DDR3)
+		mask = EMC_SEL_DPD_CTRL_DDR3_MASK;
+	else
+		mask = EMC_SEL_DPD_CTRL_MASK;
+
+	val = readl(emc->regs + EMC_SEL_DPD_CTRL);
+	if (val & mask) {
+		val &= ~mask;
+		writel(val, emc->regs + EMC_SEL_DPD_CTRL);
+	}
+
+	/* Prepare DQ/DQS for clock change */
+	val = readl(emc->regs + EMC_BGBIAS_CTL0);
+	val2 = last->emc_bgbias_ctl0;
+	if (!(timing->emc_bgbias_ctl0 &
+	      EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD_IBIAS_RX) &&
+	    (val & EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD_IBIAS_RX)) {
+		val2 &= ~EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD_IBIAS_RX;
+		update = true;
+	}
+
+	if ((val & EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD) ||
+	    (val & EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD_IBIAS_VTTGEN)) {
+		update = true;
+	}
+
+	if (update) {
+		writel(val2, emc->regs + EMC_BGBIAS_CTL0);
+		if (pre_wait < 5)
+			pre_wait = 5;
+	}
+
+	update = false;
+	val = readl(emc->regs + EMC_XM2DQSPADCTRL2);
+	if (timing->emc_xm2dqspadctrl2 & EMC_XM2DQSPADCTRL2_VREF_ENABLE &&
+	    !(val & EMC_XM2DQSPADCTRL2_VREF_ENABLE)) {
+		val |= EMC_XM2DQSPADCTRL2_VREF_ENABLE;
+		update = true;
+	}
+
+	if (timing->emc_xm2dqspadctrl2 & EMC_XM2DQSPADCTRL2_RX_FT_REC_ENABLE &&
+	    !(val & EMC_XM2DQSPADCTRL2_RX_FT_REC_ENABLE)) {
+		val |= EMC_XM2DQSPADCTRL2_RX_FT_REC_ENABLE;
+		update = true;
+	}
+
+	if (update) {
+		writel(val, emc->regs + EMC_XM2DQSPADCTRL2);
+		if (pre_wait < 30)
+			pre_wait = 30;
+	}
+
+	/* Wait to settle */
+	if (pre_wait) {
+		emc_seq_update_timing(emc);
+		udelay(pre_wait);
+	}
+
+	/* Program CTT_TERM control */
+	if (last->emc_ctt_term_ctrl != timing->emc_ctt_term_ctrl) {
+		emc_seq_disable_auto_cal(emc);
+		writel(timing->emc_ctt_term_ctrl,
+		       emc->regs + EMC_CTT_TERM_CTRL);
+		emc_seq_update_timing(emc);
+	}
+
+	/* Program burst shadow registers */
+	for (i = 0; i < ARRAY_SIZE(timing->emc_burst_data); ++i)
+		writel(timing->emc_burst_data[i],
+		       emc->regs + emc_burst_regs[i]);
+
+	writel(timing->emc_xm2dqspadctrl2, emc->regs + EMC_XM2DQSPADCTRL2);
+	writel(timing->emc_zcal_interval, emc->regs + EMC_ZCAL_INTERVAL);
+
+	tegra_mc_write_emem_configuration(emc->mc, timing->rate);
+
+	val = timing->emc_cfg & ~EMC_CFG_POWER_FEATURES_MASK;
+	emc_ccfifo_writel(emc, val, EMC_CFG);
+
+	/* Program AUTO_CAL_CONFIG */
+	if (timing->emc_auto_cal_config2 != last->emc_auto_cal_config2)
+		emc_ccfifo_writel(emc, timing->emc_auto_cal_config2,
+				  EMC_AUTO_CAL_CONFIG2);
+
+	if (timing->emc_auto_cal_config3 != last->emc_auto_cal_config3)
+		emc_ccfifo_writel(emc, timing->emc_auto_cal_config3,
+				  EMC_AUTO_CAL_CONFIG3);
+
+	if (timing->emc_auto_cal_config != last->emc_auto_cal_config) {
+		val = timing->emc_auto_cal_config;
+		val &= EMC_AUTO_CAL_CONFIG_AUTO_CAL_START;
+		emc_ccfifo_writel(emc, val, EMC_AUTO_CAL_CONFIG);
+	}
+
+	/* DDR3: predict MRS long wait count */
+	if (emc->dram_type == DRAM_TYPE_DDR3 &&
+	    dll_change == DLL_CHANGE_ON) {
+		u32 cnt = 512;
+
+		if (timing->emc_zcal_interval != 0 &&
+		    last->emc_zcal_interval == 0)
+			cnt -= emc->dram_num * 256;
+
+		val = (timing->emc_mrs_wait_cnt
+			& EMC_MRS_WAIT_CNT_SHORT_WAIT_MASK)
+			>> EMC_MRS_WAIT_CNT_SHORT_WAIT_SHIFT;
+		if (cnt < val)
+			cnt = val;
+
+		val = timing->emc_mrs_wait_cnt
+			& ~EMC_MRS_WAIT_CNT_LONG_WAIT_MASK;
+		val |= (cnt << EMC_MRS_WAIT_CNT_LONG_WAIT_SHIFT)
+			& EMC_MRS_WAIT_CNT_LONG_WAIT_MASK;
+
+		writel(val, emc->regs + EMC_MRS_WAIT_CNT);
+	}
+
+	val = timing->emc_cfg_2;
+	val &= ~EMC_CFG_2_DIS_STP_OB_CLK_DURING_NON_WR;
+	emc_ccfifo_writel(emc, val, EMC_CFG_2);
+
+	/* DDR3: Turn off DLL and enter self-refresh */
+	if (emc->dram_type == DRAM_TYPE_DDR3 && dll_change == DLL_CHANGE_OFF)
+		emc_ccfifo_writel(emc, timing->emc_mode_1, EMC_EMRS);
+
+	/* Disable refresh controller */
+	emc_ccfifo_writel(emc, EMC_REFCTRL_DEV_SEL(emc->dram_num),
+			  EMC_REFCTRL);
+	if (emc->dram_type == DRAM_TYPE_DDR3)
+		emc_ccfifo_writel(emc, EMC_DRAM_DEV_SEL(emc->dram_num) |
+				       EMC_SELF_REF_CMD_ENABLED,
+				  EMC_SELF_REF);
+
+	/* Flow control marker */
+	emc_ccfifo_writel(emc, 1, EMC_STALL_THEN_EXE_AFTER_CLKCHANGE);
+
+	/* DDR3: Exit self-refresh */
+	if (emc->dram_type == DRAM_TYPE_DDR3)
+		emc_ccfifo_writel(emc, EMC_DRAM_DEV_SEL(emc->dram_num),
+				  EMC_SELF_REF);
+	emc_ccfifo_writel(emc, EMC_REFCTRL_DEV_SEL(emc->dram_num) |
+			       EMC_REFCTRL_ENABLE,
+			  EMC_REFCTRL);
+
+	/* Set DRAM mode registers */
+	if (emc->dram_type == DRAM_TYPE_DDR3) {
+		if (timing->emc_mode_1 != last->emc_mode_1)
+			emc_ccfifo_writel(emc, timing->emc_mode_1, EMC_EMRS);
+		if (timing->emc_mode_2 != last->emc_mode_2)
+			emc_ccfifo_writel(emc, timing->emc_mode_2, EMC_EMRS2);
+
+		if ((timing->emc_mode_reset != last->emc_mode_reset) ||
+		    dll_change == DLL_CHANGE_ON) {
+			val = timing->emc_mode_reset;
+			if (dll_change == DLL_CHANGE_ON) {
+				val |= EMC_MODE_SET_DLL_RESET;
+				val |= EMC_MODE_SET_LONG_CNT;
+			} else {
+				val &= ~EMC_MODE_SET_DLL_RESET;
+			}
+			emc_ccfifo_writel(emc, val, EMC_MRS);
+		}
+	} else {
+		if (timing->emc_mode_2 != last->emc_mode_2)
+			emc_ccfifo_writel(emc, timing->emc_mode_2, EMC_MRW2);
+		if (timing->emc_mode_1 != last->emc_mode_1)
+			emc_ccfifo_writel(emc, timing->emc_mode_1, EMC_MRW);
+		if (timing->emc_mode_4 != last->emc_mode_4)
+			emc_ccfifo_writel(emc, timing->emc_mode_4, EMC_MRW4);
+	}
+
+	/*  Issue ZCAL command if turning ZCAL on */
+	if (timing->emc_zcal_interval != 0 && last->emc_zcal_interval == 0) {
+		emc_ccfifo_writel(emc, EMC_ZQ_CAL_LONG_CMD_DEV0, EMC_ZQ_CAL);
+		if (emc->dram_num > 1)
+			emc_ccfifo_writel(emc, EMC_ZQ_CAL_LONG_CMD_DEV1,
+					  EMC_ZQ_CAL);
+	}
+
+	/*  Write to RO register to remove stall after change */
+	emc_ccfifo_writel(emc, 0, EMC_CCFIFO_STATUS);
+
+	if (timing->emc_cfg_2 & EMC_CFG_2_DIS_STP_OB_CLK_DURING_NON_WR)
+		emc_ccfifo_writel(emc, timing->emc_cfg_2, EMC_CFG_2);
+
+	/* Disable AUTO_CAL for clock change */
+	emc_seq_disable_auto_cal(emc);
+
+	/* Read register to wait until programming has settled */
+	readl(emc->regs + EMC_INTSTATUS);
+
+	return 0;
+}
+
+void tegra_emc_complete_timing_change(struct tegra_emc *emc,
+				      unsigned long rate)
+{
+	struct emc_timing *timing = tegra_emc_find_timing(emc, rate);
+	struct emc_timing *last = &emc->last_timing;
+	u32 val;
+
+	if (!timing)
+		return;
+
+	/* Wait until the state machine has settled */
+	emc_seq_wait_clkchange(emc);
+
+	/* Restore AUTO_CAL */
+	if (timing->emc_ctt_term_ctrl != last->emc_ctt_term_ctrl)
+		writel(timing->emc_auto_cal_interval,
+		       emc->regs + EMC_AUTO_CAL_INTERVAL);
+
+	/* Restore dynamic self-refresh */
+	if (timing->emc_cfg & EMC_CFG_PWR_MASK)
+		writel(timing->emc_cfg, emc->regs + EMC_CFG);
+
+	/* Set ZCAL wait count */
+	writel(timing->emc_zcal_cnt_long, emc->regs + EMC_ZCAL_WAIT_CNT);
+
+	/* LPDDR3: Turn off BGBIAS if low frequency */
+	if (emc->dram_type == DRAM_TYPE_LPDDR3 &&
+	    timing->emc_bgbias_ctl0 &
+	      EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD_IBIAS_RX) {
+		val = timing->emc_bgbias_ctl0;
+		val |= EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD_IBIAS_VTTGEN;
+		val |= EMC_BGBIAS_CTL0_BIAS0_DSC_E_PWRD;
+		writel(val, emc->regs + EMC_BGBIAS_CTL0);
+	} else {
+		if (emc->dram_type == DRAM_TYPE_DDR3 &&
+		    readl(emc->regs + EMC_BGBIAS_CTL0) !=
+		      timing->emc_bgbias_ctl0) {
+			writel(timing->emc_bgbias_ctl0,
+			       emc->regs + EMC_BGBIAS_CTL0);
+		}
+
+		writel(timing->emc_auto_cal_interval,
+		       emc->regs + EMC_AUTO_CAL_INTERVAL);
+	}
+
+	/* Wait for timing to settle */
+	udelay(2);
+
+	/* Reprogram SEL_DPD_CTRL */
+	writel(timing->emc_sel_dpd_ctrl, emc->regs + EMC_SEL_DPD_CTRL);
+	emc_seq_update_timing(emc);
+
+	emc->last_timing = *timing;
+}
+
+/* Initialization and deinitialization */
+
+static void emc_read_current_timing(struct tegra_emc *emc,
+				    struct emc_timing *timing)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(emc_burst_regs); ++i)
+		timing->emc_burst_data[i] =
+			readl(emc->regs + emc_burst_regs[i]);
+
+	timing->emc_cfg = readl(emc->regs + EMC_CFG);
+
+	timing->emc_auto_cal_interval = 0;
+	timing->emc_zcal_cnt_long = 0;
+	timing->emc_mode_1 = 0;
+	timing->emc_mode_2 = 0;
+	timing->emc_mode_4 = 0;
+	timing->emc_mode_reset = 0;
+}
+
+static int emc_init(struct tegra_emc *emc)
+{
+	emc->dram_type = readl(emc->regs + EMC_FBIO_CFG5);
+	emc->dram_type &= EMC_FBIO_CFG5_DRAM_TYPE_MASK;
+	emc->dram_type >>= EMC_FBIO_CFG5_DRAM_TYPE_SHIFT;
+
+	emc->dram_num = tegra_mc_get_emem_device_count(emc->mc);
+
+	emc_read_current_timing(emc, &emc->last_timing);
+
+	return 0;
+}
+
+static int load_one_timing_from_dt(struct tegra_emc *emc,
+				   struct emc_timing *timing,
+				   struct device_node *node)
+{
+	u32 value;
+	int err;
+
+	err = of_property_read_u32(node, "clock-frequency", &value);
+	if (err) {
+		dev_err(emc->dev, "timing %s: failed to read rate: %d\n",
+			node->name, err);
+		return err;
+	}
+
+	timing->rate = value;
+
+	err = of_property_read_u32_array(node, "nvidia,emc-configuration",
+					 timing->emc_burst_data,
+					 ARRAY_SIZE(timing->emc_burst_data));
+	if (err) {
+		dev_err(emc->dev,
+			"timing %s: failed to read emc burst data: %d\n",
+			node->name, err);
+		return err;
+	}
+
+#define EMC_READ_PROP(prop, dtprop) { \
+	err = of_property_read_u32(node, dtprop, &timing->prop); \
+	if (err) { \
+		dev_err(emc->dev, "timing %s: failed to read " #prop ": %d\n", \
+			node->name, err); \
+		return err; \
+	} \
+}
+
+	EMC_READ_PROP(emc_auto_cal_config, "nvidia,emc-auto-cal-config")
+	EMC_READ_PROP(emc_auto_cal_config2, "nvidia,emc-auto-cal-config2")
+	EMC_READ_PROP(emc_auto_cal_config3, "nvidia,emc-auto-cal-config3")
+	EMC_READ_PROP(emc_auto_cal_interval, "nvidia,emc-auto-cal-interval")
+	EMC_READ_PROP(emc_bgbias_ctl0, "nvidia,emc-bgbias-ctl0")
+	EMC_READ_PROP(emc_cfg, "nvidia,emc-cfg")
+	EMC_READ_PROP(emc_cfg_2, "nvidia,emc-cfg-2")
+	EMC_READ_PROP(emc_ctt_term_ctrl, "nvidia,emc-ctt-term-ctrl")
+	EMC_READ_PROP(emc_mode_1, "nvidia,emc-mode-1")
+	EMC_READ_PROP(emc_mode_2, "nvidia,emc-mode-2")
+	EMC_READ_PROP(emc_mode_4, "nvidia,emc-mode-4")
+	EMC_READ_PROP(emc_mode_reset, "nvidia,emc-mode-reset")
+	EMC_READ_PROP(emc_mrs_wait_cnt, "nvidia,emc-mrs-wait-cnt")
+	EMC_READ_PROP(emc_sel_dpd_ctrl, "nvidia,emc-sel-dpd-ctrl")
+	EMC_READ_PROP(emc_xm2dqspadctrl2, "nvidia,emc-xm2dqspadctrl2")
+	EMC_READ_PROP(emc_zcal_cnt_long, "nvidia,emc-zcal-cnt-long")
+	EMC_READ_PROP(emc_zcal_interval, "nvidia,emc-zcal-interval")
+
+#undef EMC_READ_PROP
+
+	return 0;
+}
+
+static int cmp_timings(const void *_a, const void *_b)
+{
+	const struct emc_timing *a = _a;
+	const struct emc_timing *b = _b;
+
+	if (a->rate < b->rate)
+		return -1;
+	else if (a->rate == b->rate)
+		return 0;
+	else
+		return 1;
+}
+
+static int tegra_emc_load_timings_from_dt(struct tegra_emc *emc,
+					  struct device_node *node)
+{
+	int child_count = of_get_child_count(node);
+	struct device_node *child;
+	struct emc_timing *timing;
+	unsigned int i = 0;
+	int err;
+
+	emc->timings = devm_kcalloc(emc->dev, child_count, sizeof(*timing),
+				    GFP_KERNEL);
+	if (!emc->timings)
+		return -ENOMEM;
+
+	emc->num_timings = child_count;
+
+	for_each_child_of_node(node, child) {
+		timing = &emc->timings[i++];
+
+		err = load_one_timing_from_dt(emc, timing, child);
+		if (err)
+			return err;
+	}
+
+	sort(emc->timings, emc->num_timings, sizeof(*timing), cmp_timings,
+	     NULL);
+
+	return 0;
+}
+
+static const struct of_device_id tegra_emc_of_match[] = {
+	{ .compatible = "nvidia,tegra124-emc" },
+	{}
+};
+
+static struct device_node *
+tegra_emc_find_node_by_ram_code(struct device_node *node, u32 ram_code)
+{
+	struct device_node *np;
+	int err;
+
+	for_each_child_of_node(node, np) {
+		u32 value;
+
+		err = of_property_read_u32(np, "nvidia,ram-code", &value);
+		if (err || (value != ram_code)) {
+			of_node_put(np);
+			continue;
+		}
+
+		return np;
+	}
+
+	return NULL;
+}
+
+/* Debugfs entry */
+
+static int emc_debug_rate_get(void *data, u64 *rate)
+{
+	struct clk *c = data;
+
+	*rate = clk_get_rate(c);
+
+	return 0;
+}
+
+static int emc_debug_rate_set(void *data, u64 rate)
+{
+	struct clk *c = data;
+
+	return clk_set_rate(c, rate);
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(emc_debug_rate_fops, emc_debug_rate_get,
+			emc_debug_rate_set, "%lld\n");
+
+static void emc_debugfs_init(struct device *dev)
+{
+	struct dentry *root, *file;
+	struct clk *clk;
+
+	root = debugfs_create_dir("emc", NULL);
+	if (!root) {
+		dev_err(dev, "failed to create debugfs directory\n");
+		return;
+	}
+
+	clk = clk_get_sys("tegra-clk-debug", "emc");
+	if (IS_ERR(clk)) {
+		dev_err(dev, "failed to get debug clock: %ld\n", PTR_ERR(clk));
+		return;
+	}
+
+	file = debugfs_create_file("rate", S_IRUGO | S_IWUSR, root, clk,
+				   &emc_debug_rate_fops);
+	if (!file)
+		dev_err(dev, "failed to create debugfs entry\n");
+}
+
+static int tegra_emc_probe(struct platform_device *pdev)
+{
+	struct platform_device *mc;
+	struct device_node *np;
+	struct tegra_emc *emc;
+	struct resource *res;
+	u32 ram_code;
+	int err;
+
+	emc = devm_kzalloc(&pdev->dev, sizeof(*emc), GFP_KERNEL);
+	if (!emc)
+		return -ENOMEM;
+
+	emc->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	emc->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(emc->regs))
+		return PTR_ERR(emc->regs);
+
+	np = of_parse_phandle(pdev->dev.of_node, "nvidia,memory-controller", 0);
+	if (!np) {
+		dev_err(&pdev->dev, "could not get memory controller\n");
+		return -ENOENT;
+	}
+
+	mc = of_find_device_by_node(np);
+	if (!mc)
+		return -ENOENT;
+
+	of_node_put(np);
+
+	emc->mc = platform_get_drvdata(mc);
+	if (!emc->mc)
+		return -EPROBE_DEFER;
+
+	ram_code = tegra_read_ram_code();
+
+	np = tegra_emc_find_node_by_ram_code(pdev->dev.of_node, ram_code);
+	if (!np) {
+		dev_err(&pdev->dev,
+			"no memory timings for RAM code %u found in DT\n",
+			ram_code);
+		return -ENOENT;
+	}
+
+	err = tegra_emc_load_timings_from_dt(emc, np);
+
+	of_node_put(np);
+
+	if (err)
+		return err;
+
+	if (emc->num_timings == 0) {
+		dev_err(&pdev->dev,
+			"no memory timings for RAM code %u registered\n",
+			ram_code);
+		return -ENOENT;
+	}
+
+	err = emc_init(emc);
+	if (err) {
+		dev_err(&pdev->dev, "EMC initialization failed: %d\n", err);
+		return err;
+	}
+
+	platform_set_drvdata(pdev, emc);
+
+	if (IS_ENABLED(CONFIG_DEBUG_FS))
+		emc_debugfs_init(&pdev->dev);
+
+	return 0;
+};
+
+static struct platform_driver tegra_emc_driver = {
+	.probe = tegra_emc_probe,
+	.driver = {
+		.name = "tegra-emc",
+		.of_match_table = tegra_emc_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+static int tegra_emc_init(void)
+{
+	return platform_driver_register(&tegra_emc_driver);
+}
+subsys_initcall(tegra_emc_init);
diff --git a/drivers/memory/tegra/tegra124.c b/drivers/memory/tegra/tegra124.c
index 278d40b854c1..966e1557e6f4 100644
--- a/drivers/memory/tegra/tegra124.c
+++ b/drivers/memory/tegra/tegra124.c
@@ -15,6 +15,48 @@
 
 #include "mc.h"
 
+#define MC_EMEM_ARB_CFG				0x90
+#define MC_EMEM_ARB_OUTSTANDING_REQ		0x94
+#define MC_EMEM_ARB_TIMING_RCD			0x98
+#define MC_EMEM_ARB_TIMING_RP			0x9c
+#define MC_EMEM_ARB_TIMING_RC			0xa0
+#define MC_EMEM_ARB_TIMING_RAS			0xa4
+#define MC_EMEM_ARB_TIMING_FAW			0xa8
+#define MC_EMEM_ARB_TIMING_RRD			0xac
+#define MC_EMEM_ARB_TIMING_RAP2PRE		0xb0
+#define MC_EMEM_ARB_TIMING_WAP2PRE		0xb4
+#define MC_EMEM_ARB_TIMING_R2R			0xb8
+#define MC_EMEM_ARB_TIMING_W2W			0xbc
+#define MC_EMEM_ARB_TIMING_R2W			0xc0
+#define MC_EMEM_ARB_TIMING_W2R			0xc4
+#define MC_EMEM_ARB_DA_TURNS			0xd0
+#define MC_EMEM_ARB_DA_COVERS			0xd4
+#define MC_EMEM_ARB_MISC0			0xd8
+#define MC_EMEM_ARB_MISC1			0xdc
+#define MC_EMEM_ARB_RING1_THROTTLE		0xe0
+
+static const unsigned long tegra124_mc_emem_regs[] = {
+	MC_EMEM_ARB_CFG,
+	MC_EMEM_ARB_OUTSTANDING_REQ,
+	MC_EMEM_ARB_TIMING_RCD,
+	MC_EMEM_ARB_TIMING_RP,
+	MC_EMEM_ARB_TIMING_RC,
+	MC_EMEM_ARB_TIMING_RAS,
+	MC_EMEM_ARB_TIMING_FAW,
+	MC_EMEM_ARB_TIMING_RRD,
+	MC_EMEM_ARB_TIMING_RAP2PRE,
+	MC_EMEM_ARB_TIMING_WAP2PRE,
+	MC_EMEM_ARB_TIMING_R2R,
+	MC_EMEM_ARB_TIMING_W2W,
+	MC_EMEM_ARB_TIMING_R2W,
+	MC_EMEM_ARB_TIMING_W2R,
+	MC_EMEM_ARB_DA_TURNS,
+	MC_EMEM_ARB_DA_COVERS,
+	MC_EMEM_ARB_MISC0,
+	MC_EMEM_ARB_MISC1,
+	MC_EMEM_ARB_RING1_THROTTLE
+};
+
 static const struct tegra_mc_client tegra124_mc_clients[] = {
 	{
 		.id = 0x00,
@@ -934,29 +976,29 @@ static const struct tegra_mc_client tegra124_mc_clients[] = {
 };
 
 static const struct tegra_smmu_swgroup tegra124_swgroups[] = {
-	{ .swgroup = TEGRA_SWGROUP_DC,        .reg = 0x240 },
-	{ .swgroup = TEGRA_SWGROUP_DCB,       .reg = 0x244 },
-	{ .swgroup = TEGRA_SWGROUP_AFI,       .reg = 0x238 },
-	{ .swgroup = TEGRA_SWGROUP_AVPC,      .reg = 0x23c },
-	{ .swgroup = TEGRA_SWGROUP_HDA,       .reg = 0x254 },
-	{ .swgroup = TEGRA_SWGROUP_HC,        .reg = 0x250 },
-	{ .swgroup = TEGRA_SWGROUP_MSENC,     .reg = 0x264 },
-	{ .swgroup = TEGRA_SWGROUP_PPCS,      .reg = 0x270 },
-	{ .swgroup = TEGRA_SWGROUP_SATA,      .reg = 0x274 },
-	{ .swgroup = TEGRA_SWGROUP_VDE,       .reg = 0x27c },
-	{ .swgroup = TEGRA_SWGROUP_ISP2,      .reg = 0x258 },
-	{ .swgroup = TEGRA_SWGROUP_XUSB_HOST, .reg = 0x288 },
-	{ .swgroup = TEGRA_SWGROUP_XUSB_DEV,  .reg = 0x28c },
-	{ .swgroup = TEGRA_SWGROUP_ISP2B,     .reg = 0xaa4 },
-	{ .swgroup = TEGRA_SWGROUP_TSEC,      .reg = 0x294 },
-	{ .swgroup = TEGRA_SWGROUP_A9AVP,     .reg = 0x290 },
-	{ .swgroup = TEGRA_SWGROUP_GPU,       .reg = 0xaac },
-	{ .swgroup = TEGRA_SWGROUP_SDMMC1A,   .reg = 0xa94 },
-	{ .swgroup = TEGRA_SWGROUP_SDMMC2A,   .reg = 0xa98 },
-	{ .swgroup = TEGRA_SWGROUP_SDMMC3A,   .reg = 0xa9c },
-	{ .swgroup = TEGRA_SWGROUP_SDMMC4A,   .reg = 0xaa0 },
-	{ .swgroup = TEGRA_SWGROUP_VIC,       .reg = 0x284 },
-	{ .swgroup = TEGRA_SWGROUP_VI,        .reg = 0x280 },
+	{ .name = "dc",        .swgroup = TEGRA_SWGROUP_DC,        .reg = 0x240 },
+	{ .name = "dcb",       .swgroup = TEGRA_SWGROUP_DCB,       .reg = 0x244 },
+	{ .name = "afi",       .swgroup = TEGRA_SWGROUP_AFI,       .reg = 0x238 },
+	{ .name = "avpc",      .swgroup = TEGRA_SWGROUP_AVPC,      .reg = 0x23c },
+	{ .name = "hda",       .swgroup = TEGRA_SWGROUP_HDA,       .reg = 0x254 },
+	{ .name = "hc",        .swgroup = TEGRA_SWGROUP_HC,        .reg = 0x250 },
+	{ .name = "msenc",     .swgroup = TEGRA_SWGROUP_MSENC,     .reg = 0x264 },
+	{ .name = "ppcs",      .swgroup = TEGRA_SWGROUP_PPCS,      .reg = 0x270 },
+	{ .name = "sata",      .swgroup = TEGRA_SWGROUP_SATA,      .reg = 0x274 },
+	{ .name = "vde",       .swgroup = TEGRA_SWGROUP_VDE,       .reg = 0x27c },
+	{ .name = "isp2",      .swgroup = TEGRA_SWGROUP_ISP2,      .reg = 0x258 },
+	{ .name = "xusb_host", .swgroup = TEGRA_SWGROUP_XUSB_HOST, .reg = 0x288 },
+	{ .name = "xusb_dev",  .swgroup = TEGRA_SWGROUP_XUSB_DEV,  .reg = 0x28c },
+	{ .name = "isp2b",     .swgroup = TEGRA_SWGROUP_ISP2B,     .reg = 0xaa4 },
+	{ .name = "tsec",      .swgroup = TEGRA_SWGROUP_TSEC,      .reg = 0x294 },
+	{ .name = "a9avp",     .swgroup = TEGRA_SWGROUP_A9AVP,     .reg = 0x290 },
+	{ .name = "gpu",       .swgroup = TEGRA_SWGROUP_GPU,       .reg = 0xaac },
+	{ .name = "sdmmc1a",   .swgroup = TEGRA_SWGROUP_SDMMC1A,   .reg = 0xa94 },
+	{ .name = "sdmmc2a",   .swgroup = TEGRA_SWGROUP_SDMMC2A,   .reg = 0xa98 },
+	{ .name = "sdmmc3a",   .swgroup = TEGRA_SWGROUP_SDMMC3A,   .reg = 0xa9c },
+	{ .name = "sdmmc4a",   .swgroup = TEGRA_SWGROUP_SDMMC4A,   .reg = 0xaa0 },
+	{ .name = "vic",       .swgroup = TEGRA_SWGROUP_VIC,       .reg = 0x284 },
+	{ .name = "vi",        .swgroup = TEGRA_SWGROUP_VI,        .reg = 0x280 },
 };
 
 #ifdef CONFIG_ARCH_TEGRA_124_SOC
@@ -991,5 +1033,40 @@ const struct tegra_mc_soc tegra124_mc_soc = {
 	.num_address_bits = 34,
 	.atom_size = 32,
 	.smmu = &tegra124_smmu_soc,
+	.emem_regs = tegra124_mc_emem_regs,
+	.num_emem_regs = ARRAY_SIZE(tegra124_mc_emem_regs),
 };
 #endif /* CONFIG_ARCH_TEGRA_124_SOC */
+
+#ifdef CONFIG_ARCH_TEGRA_132_SOC
+static void tegra132_flush_dcache(struct page *page, unsigned long offset,
+				  size_t size)
+{
+	void *virt = page_address(page) + offset;
+
+	__flush_dcache_area(virt, size);
+}
+
+static const struct tegra_smmu_ops tegra132_smmu_ops = {
+	.flush_dcache = tegra132_flush_dcache,
+};
+
+static const struct tegra_smmu_soc tegra132_smmu_soc = {
+	.clients = tegra124_mc_clients,
+	.num_clients = ARRAY_SIZE(tegra124_mc_clients),
+	.swgroups = tegra124_swgroups,
+	.num_swgroups = ARRAY_SIZE(tegra124_swgroups),
+	.supports_round_robin_arbitration = true,
+	.supports_request_limit = true,
+	.num_asids = 128,
+	.ops = &tegra132_smmu_ops,
+};
+
+const struct tegra_mc_soc tegra132_mc_soc = {
+	.clients = tegra124_mc_clients,
+	.num_clients = ARRAY_SIZE(tegra124_mc_clients),
+	.num_address_bits = 34,
+	.atom_size = 32,
+	.smmu = &tegra132_smmu_soc,
+};
+#endif /* CONFIG_ARCH_TEGRA_132_SOC */
diff --git a/drivers/memory/tegra/tegra30.c b/drivers/memory/tegra/tegra30.c
index 71fe9376fe53..1abcd8f6f3ba 100644
--- a/drivers/memory/tegra/tegra30.c
+++ b/drivers/memory/tegra/tegra30.c
@@ -918,22 +918,22 @@ static const struct tegra_mc_client tegra30_mc_clients[] = {
 };
 
 static const struct tegra_smmu_swgroup tegra30_swgroups[] = {
-	{ .swgroup = TEGRA_SWGROUP_DC,   .reg = 0x240 },
-	{ .swgroup = TEGRA_SWGROUP_DCB,  .reg = 0x244 },
-	{ .swgroup = TEGRA_SWGROUP_EPP,  .reg = 0x248 },
-	{ .swgroup = TEGRA_SWGROUP_G2,   .reg = 0x24c },
-	{ .swgroup = TEGRA_SWGROUP_MPE,  .reg = 0x264 },
-	{ .swgroup = TEGRA_SWGROUP_VI,   .reg = 0x280 },
-	{ .swgroup = TEGRA_SWGROUP_AFI,  .reg = 0x238 },
-	{ .swgroup = TEGRA_SWGROUP_AVPC, .reg = 0x23c },
-	{ .swgroup = TEGRA_SWGROUP_NV,   .reg = 0x268 },
-	{ .swgroup = TEGRA_SWGROUP_NV2,  .reg = 0x26c },
-	{ .swgroup = TEGRA_SWGROUP_HDA,  .reg = 0x254 },
-	{ .swgroup = TEGRA_SWGROUP_HC,   .reg = 0x250 },
-	{ .swgroup = TEGRA_SWGROUP_PPCS, .reg = 0x270 },
-	{ .swgroup = TEGRA_SWGROUP_SATA, .reg = 0x278 },
-	{ .swgroup = TEGRA_SWGROUP_VDE,  .reg = 0x27c },
-	{ .swgroup = TEGRA_SWGROUP_ISP,  .reg = 0x258 },
+	{ .name = "dc",   .swgroup = TEGRA_SWGROUP_DC,   .reg = 0x240 },
+	{ .name = "dcb",  .swgroup = TEGRA_SWGROUP_DCB,  .reg = 0x244 },
+	{ .name = "epp",  .swgroup = TEGRA_SWGROUP_EPP,  .reg = 0x248 },
+	{ .name = "g2",   .swgroup = TEGRA_SWGROUP_G2,   .reg = 0x24c },
+	{ .name = "mpe",  .swgroup = TEGRA_SWGROUP_MPE,  .reg = 0x264 },
+	{ .name = "vi",   .swgroup = TEGRA_SWGROUP_VI,   .reg = 0x280 },
+	{ .name = "afi",  .swgroup = TEGRA_SWGROUP_AFI,  .reg = 0x238 },
+	{ .name = "avpc", .swgroup = TEGRA_SWGROUP_AVPC, .reg = 0x23c },
+	{ .name = "nv",   .swgroup = TEGRA_SWGROUP_NV,   .reg = 0x268 },
+	{ .name = "nv2",  .swgroup = TEGRA_SWGROUP_NV2,  .reg = 0x26c },
+	{ .name = "hda",  .swgroup = TEGRA_SWGROUP_HDA,  .reg = 0x254 },
+	{ .name = "hc",   .swgroup = TEGRA_SWGROUP_HC,   .reg = 0x250 },
+	{ .name = "ppcs", .swgroup = TEGRA_SWGROUP_PPCS, .reg = 0x270 },
+	{ .name = "sata", .swgroup = TEGRA_SWGROUP_SATA, .reg = 0x278 },
+	{ .name = "vde",  .swgroup = TEGRA_SWGROUP_VDE,  .reg = 0x27c },
+	{ .name = "isp",  .swgroup = TEGRA_SWGROUP_ISP,  .reg = 0x258 },
 };
 
 static void tegra30_flush_dcache(struct page *page, unsigned long offset,
diff --git a/drivers/memstick/host/jmb38x_ms.c b/drivers/memstick/host/jmb38x_ms.c
index aeabaa5aedf7..48db922075e2 100644
--- a/drivers/memstick/host/jmb38x_ms.c
+++ b/drivers/memstick/host/jmb38x_ms.c
@@ -419,10 +419,10 @@ static int jmb38x_ms_issue_cmd(struct memstick_host *msh)
 	}
 
 	if (host->cmd_flags & DMA_DATA) {
-		if (1 != pci_map_sg(host->chip->pdev, &host->req->sg, 1,
+		if (1 != dma_map_sg(&host->chip->pdev->dev, &host->req->sg, 1,
 				    host->req->data_dir == READ
-				    ? PCI_DMA_FROMDEVICE
-				    : PCI_DMA_TODEVICE)) {
+				    ? DMA_FROM_DEVICE
+				    : DMA_TO_DEVICE)) {
 			host->req->error = -ENOMEM;
 			return host->req->error;
 		}
@@ -487,9 +487,9 @@ static void jmb38x_ms_complete_cmd(struct memstick_host *msh, int last)
 	writel(0, host->addr + DMA_CONTROL);
 
 	if (host->cmd_flags & DMA_DATA) {
-		pci_unmap_sg(host->chip->pdev, &host->req->sg, 1,
+		dma_unmap_sg(&host->chip->pdev->dev, &host->req->sg, 1,
 			     host->req->data_dir == READ
-			     ? PCI_DMA_FROMDEVICE : PCI_DMA_TODEVICE);
+			     ? DMA_FROM_DEVICE : DMA_TO_DEVICE);
 	} else {
 		t_val = readl(host->addr + INT_STATUS_ENABLE);
 		if (host->req->data_dir == READ)
@@ -925,7 +925,7 @@ static int jmb38x_ms_probe(struct pci_dev *pdev,
 	int pci_dev_busy = 0;
 	int rc, cnt;
 
-	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+	rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (rc)
 		return rc;
 
diff --git a/drivers/memstick/host/r592.c b/drivers/memstick/host/r592.c
index e2a4f5f415b2..ef09ba0289d7 100644
--- a/drivers/memstick/host/r592.c
+++ b/drivers/memstick/host/r592.c
@@ -754,7 +754,7 @@ static int r592_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto error2;
 
 	pci_set_master(pdev);
-	error = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+	error = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 	if (error)
 		goto error3;
 
@@ -787,8 +787,8 @@ static int r592_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	}
 
 	/* This is just a precation, so don't fail */
-	dev->dummy_dma_page = pci_alloc_consistent(pdev, PAGE_SIZE,
-		&dev->dummy_dma_page_physical_address);
+	dev->dummy_dma_page = dma_alloc_coherent(&pdev->dev, PAGE_SIZE,
+		&dev->dummy_dma_page_physical_address, GFP_KERNEL);
 	r592_stop_dma(dev , 0);
 
 	if (request_irq(dev->irq, &r592_irq, IRQF_SHARED,
@@ -805,7 +805,7 @@ error7:
 	free_irq(dev->irq, dev);
 error6:
 	if (dev->dummy_dma_page)
-		pci_free_consistent(pdev, PAGE_SIZE, dev->dummy_dma_page,
+		dma_free_coherent(&pdev->dev, PAGE_SIZE, dev->dummy_dma_page,
 			dev->dummy_dma_page_physical_address);
 
 	kthread_stop(dev->io_thread);
@@ -845,7 +845,7 @@ static void r592_remove(struct pci_dev *pdev)
 	memstick_free_host(dev->host);
 
 	if (dev->dummy_dma_page)
-		pci_free_consistent(pdev, PAGE_SIZE, dev->dummy_dma_page,
+		dma_free_coherent(&pdev->dev, PAGE_SIZE, dev->dummy_dma_page,
 			dev->dummy_dma_page_physical_address);
 }
 
diff --git a/drivers/message/fusion/mptbase.c b/drivers/message/fusion/mptbase.c
index 187f83629f7e..5dcc0313c38a 100644
--- a/drivers/message/fusion/mptbase.c
+++ b/drivers/message/fusion/mptbase.c
@@ -59,10 +59,6 @@
 #include <linux/delay.h>
 #include <linux/interrupt.h>		/* needed for in_interrupt() proto */
 #include <linux/dma-mapping.h>
-#include <asm/io.h>
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
 #include <linux/kthread.h>
 #include <scsi/scsi_host.h>
 
@@ -2820,13 +2816,6 @@ mpt_adapter_dispose(MPT_ADAPTER *ioc)
 	pci_disable_device(ioc->pcidev);
 	pci_release_selected_regions(ioc->pcidev, ioc->bars);
 
-#if defined(CONFIG_MTRR) && 0
-	if (ioc->mtrr_reg > 0) {
-		mtrr_del(ioc->mtrr_reg, 0, 0);
-		dprintk(ioc, printk(MYIOC_s_INFO_FMT "MTRR region de-registered\n", ioc->name));
-	}
-#endif
-
 	/*  Zap the adapter lookup ptr!  */
 	list_del(&ioc->list);
 
@@ -4512,19 +4501,6 @@ PrimeIocFifos(MPT_ADAPTER *ioc)
 
 		ioc->req_frames_low_dma = (u32) (alloc_dma & 0xFFFFFFFF);
 
-#if defined(CONFIG_MTRR) && 0
-		/*
-		 *  Enable Write Combining MTRR for IOC's memory region.
-		 *  (at least as much as we can; "size and base must be
-		 *  multiples of 4 kiB"
-		 */
-		ioc->mtrr_reg = mtrr_add(ioc->req_frames_dma,
-					 sz,
-					 MTRR_TYPE_WRCOMB, 1);
-		dprintk(ioc, printk(MYIOC_s_DEBUG_FMT "MTRR region registered (base:size=%08x:%x)\n",
-				ioc->name, ioc->req_frames_dma, sz));
-#endif
-
 		for (i = 0; i < ioc->req_depth; i++) {
 			alloc_dma += ioc->req_sz;
 			mem += ioc->req_sz;
diff --git a/drivers/message/fusion/mptbase.h b/drivers/message/fusion/mptbase.h
index 8f14090b8b71..813d46311f6a 100644
--- a/drivers/message/fusion/mptbase.h
+++ b/drivers/message/fusion/mptbase.h
@@ -671,7 +671,6 @@ typedef struct _MPT_ADAPTER
 	u8			*HostPageBuffer; /* SAS - host page buffer support */
 	u32			HostPageBuffer_sz;
 	dma_addr_t		HostPageBuffer_dma;
-	int			 mtrr_reg;
 	struct pci_dev		*pcidev;	/* struct pci_dev pointer */
 	int			bars;		/* bitmask of BAR's that must be configured */
 	int			msi_enable;
diff --git a/drivers/message/fusion/mptsas.c b/drivers/message/fusion/mptsas.c
index 5bdaae15a742..005a88b9f440 100644
--- a/drivers/message/fusion/mptsas.c
+++ b/drivers/message/fusion/mptsas.c
@@ -4090,7 +4090,7 @@ mptsas_handle_queue_full_event(struct fw_event_work *fw_event)
 					continue;
 				}
 				depth = scsi_track_queue_full(sdev,
-				    current_depth - 1);
+					sdev->queue_depth - 1);
 				if (depth > 0)
 					sdev_printk(KERN_INFO, sdev,
 					"Queue depth reduced to (%d)\n",
@@ -4100,7 +4100,7 @@ mptsas_handle_queue_full_event(struct fw_event_work *fw_event)
 					"Tagged Command Queueing is being "
 					"disabled\n");
 				else if (depth == 0)
-					sdev_printk(KERN_INFO, sdev,
+					sdev_printk(KERN_DEBUG, sdev,
 					"Queue depth not changed yet\n");
 			}
 		}
diff --git a/drivers/mfd/88pm860x-core.c b/drivers/mfd/88pm860x-core.c
index d2a85cde68da..e03b7f45b8f7 100644
--- a/drivers/mfd/88pm860x-core.c
+++ b/drivers/mfd/88pm860x-core.c
@@ -566,7 +566,7 @@ static int pm860x_irq_domain_map(struct irq_domain *d, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops pm860x_irq_domain_ops = {
+static const struct irq_domain_ops pm860x_irq_domain_ops = {
 	.map	= pm860x_irq_domain_map,
 	.xlate	= irq_domain_xlate_onetwocell,
 };
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index d5ad04dad081..653815950aa2 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -52,7 +52,8 @@ config PMIC_ADP5520
 config MFD_AAT2870_CORE
 	bool "AnalogicTech AAT2870"
 	select MFD_CORE
-	depends on I2C=y && GPIOLIB
+	depends on I2C=y
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  If you say yes here you get support for the AAT2870.
 	  This driver provides common support for accessing the device,
@@ -94,6 +95,8 @@ config MFD_AXP20X
 config MFD_CROS_EC
 	tristate "ChromeOS Embedded Controller"
 	select MFD_CORE
+	select CHROME_PLATFORMS
+	select CROS_EC_PROTO
 	help
 	  If you say Y here you get support for the ChromeOS Embedded
 	  Controller (EC) providing keyboard, battery and power services.
@@ -102,7 +105,7 @@ config MFD_CROS_EC
 
 config MFD_CROS_EC_I2C
 	tristate "ChromeOS Embedded Controller (I2C)"
-	depends on MFD_CROS_EC && I2C
+	depends on MFD_CROS_EC && CROS_EC_PROTO && I2C
 
 	help
 	  If you say Y here, you get support for talking to the ChromeOS
@@ -112,7 +115,7 @@ config MFD_CROS_EC_I2C
 
 config MFD_CROS_EC_SPI
 	tristate "ChromeOS Embedded Controller (SPI)"
-	depends on MFD_CROS_EC && SPI && OF
+	depends on MFD_CROS_EC && CROS_EC_PROTO && SPI && OF
 
 	---help---
 	  If you say Y here, you get support for talking to the ChromeOS EC
@@ -1115,7 +1118,8 @@ config MFD_TPS6586X
 
 config MFD_TPS65910
 	bool "TI TPS65910 Power Management chip"
-	depends on I2C=y && GPIOLIB
+	depends on I2C=y
+	depends on GPIOLIB || COMPILE_TEST
 	select MFD_CORE
 	select REGMAP_I2C
 	select REGMAP_IRQ
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 0e5cfeba107c..ea40e076cb61 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -39,13 +39,13 @@ obj-$(CONFIG_MFD_ARIZONA)	+= arizona-core.o
 obj-$(CONFIG_MFD_ARIZONA)	+= arizona-irq.o
 obj-$(CONFIG_MFD_ARIZONA_I2C)	+= arizona-i2c.o
 obj-$(CONFIG_MFD_ARIZONA_SPI)	+= arizona-spi.o
-ifneq ($(CONFIG_MFD_WM5102),n)
+ifeq ($(CONFIG_MFD_WM5102),y)
 obj-$(CONFIG_MFD_ARIZONA)	+= wm5102-tables.o
 endif
-ifneq ($(CONFIG_MFD_WM5110),n)
+ifeq ($(CONFIG_MFD_WM5110),y)
 obj-$(CONFIG_MFD_ARIZONA)	+= wm5110-tables.o
 endif
-ifneq ($(CONFIG_MFD_WM8997),n)
+ifeq ($(CONFIG_MFD_WM8997),y)
 obj-$(CONFIG_MFD_ARIZONA)	+= wm8997-tables.o
 endif
 obj-$(CONFIG_MFD_WM8400)	+= wm8400-core.o
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index c80a2925f8e5..000da72a0ae9 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -574,7 +574,7 @@ static int ab8500_irq_map(struct irq_domain *d, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops ab8500_irq_ops = {
+static const struct irq_domain_ops ab8500_irq_ops = {
 	.map    = ab8500_irq_map,
 	.xlate  = irq_domain_xlate_twocell,
 };
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index cdd6f3d63314..0236cd7cdce4 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -2885,7 +2885,7 @@ static ssize_t ab8500_subscribe_write(struct file *file,
 	}
 
 	err = request_threaded_irq(user_val, NULL, ab8500_debug_handler,
-				   IRQF_SHARED | IRQF_NO_SUSPEND,
+				   IRQF_SHARED | IRQF_NO_SUSPEND | IRQF_ONESHOT,
 				   "ab8500-debug", &dev->kobj);
 	if (err < 0) {
 		pr_info("request_threaded_irq failed %d, %lu\n",
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index dabbc93abdd7..c51c1b188d64 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -948,7 +948,8 @@ static int ab8500_gpadc_probe(struct platform_device *pdev)
 	if (gpadc->irq_sw >= 0) {
 		ret = request_threaded_irq(gpadc->irq_sw, NULL,
 			ab8500_bm_gpadcconvend_handler,
-			IRQF_NO_SUSPEND | IRQF_SHARED, "ab8500-gpadc-sw",
+			IRQF_NO_SUSPEND | IRQF_SHARED | IRQF_ONESHOT,
+			"ab8500-gpadc-sw",
 			gpadc);
 		if (ret < 0) {
 			dev_err(gpadc->dev,
@@ -961,7 +962,8 @@ static int ab8500_gpadc_probe(struct platform_device *pdev)
 	if (gpadc->irq_hw >= 0) {
 		ret = request_threaded_irq(gpadc->irq_hw, NULL,
 			ab8500_bm_gpadcconvend_handler,
-			IRQF_NO_SUSPEND | IRQF_SHARED, "ab8500-gpadc-hw",
+			IRQF_NO_SUSPEND | IRQF_SHARED | IRQF_ONESHOT,
+			"ab8500-gpadc-hw",
 			gpadc);
 		if (ret < 0) {
 			dev_err(gpadc->dev,
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
index 6ca6dfab50eb..bebf58a06a6b 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -250,20 +250,50 @@ static int arizona_wait_for_boot(struct arizona *arizona)
 	return ret;
 }
 
-static int arizona_apply_hardware_patch(struct arizona* arizona)
+static inline void arizona_enable_reset(struct arizona *arizona)
+{
+	if (arizona->pdata.reset)
+		gpio_set_value_cansleep(arizona->pdata.reset, 0);
+}
+
+static void arizona_disable_reset(struct arizona *arizona)
+{
+	if (arizona->pdata.reset) {
+		switch (arizona->type) {
+		case WM5110:
+		case WM8280:
+			/* Meet requirements for minimum reset duration */
+			msleep(5);
+			break;
+		default:
+			break;
+		}
+
+		gpio_set_value_cansleep(arizona->pdata.reset, 1);
+		msleep(1);
+	}
+}
+
+struct arizona_sysclk_state {
+	unsigned int fll;
+	unsigned int sysclk;
+};
+
+static int arizona_enable_freerun_sysclk(struct arizona *arizona,
+					 struct arizona_sysclk_state *state)
 {
-	unsigned int fll, sysclk;
 	int ret, err;
 
 	/* Cache existing FLL and SYSCLK settings */
-	ret = regmap_read(arizona->regmap, ARIZONA_FLL1_CONTROL_1, &fll);
-	if (ret != 0) {
+	ret = regmap_read(arizona->regmap, ARIZONA_FLL1_CONTROL_1, &state->fll);
+	if (ret) {
 		dev_err(arizona->dev, "Failed to cache FLL settings: %d\n",
 			ret);
 		return ret;
 	}
-	ret = regmap_read(arizona->regmap, ARIZONA_SYSTEM_CLOCK_1, &sysclk);
-	if (ret != 0) {
+	ret = regmap_read(arizona->regmap, ARIZONA_SYSTEM_CLOCK_1,
+			  &state->sysclk);
+	if (ret) {
 		dev_err(arizona->dev, "Failed to cache SYSCLK settings: %d\n",
 			ret);
 		return ret;
@@ -272,7 +302,7 @@ static int arizona_apply_hardware_patch(struct arizona* arizona)
 	/* Start up SYSCLK using the FLL in free running mode */
 	ret = regmap_write(arizona->regmap, ARIZONA_FLL1_CONTROL_1,
 			ARIZONA_FLL1_ENA | ARIZONA_FLL1_FREERUN);
-	if (ret != 0) {
+	if (ret) {
 		dev_err(arizona->dev,
 			"Failed to start FLL in freerunning mode: %d\n",
 			ret);
@@ -281,53 +311,137 @@ static int arizona_apply_hardware_patch(struct arizona* arizona)
 	ret = arizona_poll_reg(arizona, 25, ARIZONA_INTERRUPT_RAW_STATUS_5,
 			       ARIZONA_FLL1_CLOCK_OK_STS,
 			       ARIZONA_FLL1_CLOCK_OK_STS);
-	if (ret != 0) {
+	if (ret) {
 		ret = -ETIMEDOUT;
 		goto err_fll;
 	}
 
 	ret = regmap_write(arizona->regmap, ARIZONA_SYSTEM_CLOCK_1, 0x0144);
-	if (ret != 0) {
+	if (ret) {
 		dev_err(arizona->dev, "Failed to start SYSCLK: %d\n", ret);
 		goto err_fll;
 	}
 
+	return 0;
+
+err_fll:
+	err = regmap_write(arizona->regmap, ARIZONA_FLL1_CONTROL_1, state->fll);
+	if (err)
+		dev_err(arizona->dev,
+			"Failed to re-apply old FLL settings: %d\n", err);
+
+	return ret;
+}
+
+static int arizona_disable_freerun_sysclk(struct arizona *arizona,
+					  struct arizona_sysclk_state *state)
+{
+	int ret;
+
+	ret = regmap_write(arizona->regmap, ARIZONA_SYSTEM_CLOCK_1,
+			   state->sysclk);
+	if (ret) {
+		dev_err(arizona->dev,
+			"Failed to re-apply old SYSCLK settings: %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_write(arizona->regmap, ARIZONA_FLL1_CONTROL_1, state->fll);
+	if (ret) {
+		dev_err(arizona->dev,
+			"Failed to re-apply old FLL settings: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int wm5102_apply_hardware_patch(struct arizona *arizona)
+{
+	struct arizona_sysclk_state state;
+	int err, ret;
+
+	ret = arizona_enable_freerun_sysclk(arizona, &state);
+	if (ret)
+		return ret;
+
 	/* Start the write sequencer and wait for it to finish */
 	ret = regmap_write(arizona->regmap, ARIZONA_WRITE_SEQUENCER_CTRL_0,
-			ARIZONA_WSEQ_ENA | ARIZONA_WSEQ_START | 160);
-	if (ret != 0) {
+			   ARIZONA_WSEQ_ENA | ARIZONA_WSEQ_START | 160);
+	if (ret) {
 		dev_err(arizona->dev, "Failed to start write sequencer: %d\n",
 			ret);
-		goto err_sysclk;
+		goto err;
 	}
+
 	ret = arizona_poll_reg(arizona, 5, ARIZONA_WRITE_SEQUENCER_CTRL_1,
 			       ARIZONA_WSEQ_BUSY, 0);
-	if (ret != 0) {
+	if (ret) {
 		regmap_write(arizona->regmap, ARIZONA_WRITE_SEQUENCER_CTRL_0,
-				ARIZONA_WSEQ_ABORT);
+			     ARIZONA_WSEQ_ABORT);
 		ret = -ETIMEDOUT;
 	}
 
-err_sysclk:
-	err = regmap_write(arizona->regmap, ARIZONA_SYSTEM_CLOCK_1, sysclk);
-	if (err != 0) {
-		dev_err(arizona->dev,
-			"Failed to re-apply old SYSCLK settings: %d\n",
-			err);
-	}
+err:
+	err = arizona_disable_freerun_sysclk(arizona, &state);
 
-err_fll:
-	err = regmap_write(arizona->regmap, ARIZONA_FLL1_CONTROL_1, fll);
-	if (err != 0) {
+	return ret ?: err;
+}
+
+/*
+ * Register patch to some of the CODECs internal write sequences
+ * to ensure a clean exit from the low power sleep state.
+ */
+static const struct reg_default wm5110_sleep_patch[] = {
+	{ 0x337A, 0xC100 },
+	{ 0x337B, 0x0041 },
+	{ 0x3300, 0xA210 },
+	{ 0x3301, 0x050C },
+};
+
+static int wm5110_apply_sleep_patch(struct arizona *arizona)
+{
+	struct arizona_sysclk_state state;
+	int err, ret;
+
+	ret = arizona_enable_freerun_sysclk(arizona, &state);
+	if (ret)
+		return ret;
+
+	ret = regmap_multi_reg_write_bypassed(arizona->regmap,
+					      wm5110_sleep_patch,
+					      ARRAY_SIZE(wm5110_sleep_patch));
+
+	err = arizona_disable_freerun_sysclk(arizona, &state);
+
+	return ret ?: err;
+}
+
+static int wm5102_clear_write_sequencer(struct arizona *arizona)
+{
+	int ret;
+
+	ret = regmap_write(arizona->regmap, ARIZONA_WRITE_SEQUENCER_CTRL_3,
+			   0x0);
+	if (ret) {
 		dev_err(arizona->dev,
-			"Failed to re-apply old FLL settings: %d\n",
-			err);
+			"Failed to clear write sequencer state: %d\n", ret);
+		return ret;
 	}
 
-	if (ret != 0)
+	arizona_enable_reset(arizona);
+	regulator_disable(arizona->dcvdd);
+
+	msleep(20);
+
+	ret = regulator_enable(arizona->dcvdd);
+	if (ret) {
+		dev_err(arizona->dev, "Failed to re-enable DCVDD: %d\n", ret);
 		return ret;
-	else
-		return err;
+	}
+	arizona_disable_reset(arizona);
+
+	return 0;
 }
 
 #ifdef CONFIG_PM
@@ -338,12 +452,33 @@ static int arizona_runtime_resume(struct device *dev)
 
 	dev_dbg(arizona->dev, "Leaving AoD mode\n");
 
+	if (arizona->has_fully_powered_off) {
+		dev_dbg(arizona->dev, "Re-enabling core supplies\n");
+
+		ret = regulator_bulk_enable(arizona->num_core_supplies,
+					    arizona->core_supplies);
+		if (ret) {
+			dev_err(dev, "Failed to enable core supplies: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
 	ret = regulator_enable(arizona->dcvdd);
 	if (ret != 0) {
 		dev_err(arizona->dev, "Failed to enable DCVDD: %d\n", ret);
+		if (arizona->has_fully_powered_off)
+			regulator_bulk_disable(arizona->num_core_supplies,
+					       arizona->core_supplies);
 		return ret;
 	}
 
+	if (arizona->has_fully_powered_off) {
+		arizona_disable_reset(arizona);
+		enable_irq(arizona->irq);
+		arizona->has_fully_powered_off = false;
+	}
+
 	regcache_cache_only(arizona->regmap, false);
 
 	switch (arizona->type) {
@@ -366,14 +501,53 @@ static int arizona_runtime_resume(struct device *dev)
 			goto err;
 		}
 
-		ret = arizona_apply_hardware_patch(arizona);
-		if (ret != 0) {
+		ret = wm5102_apply_hardware_patch(arizona);
+		if (ret) {
 			dev_err(arizona->dev,
 				"Failed to apply hardware patch: %d\n",
 				ret);
 			goto err;
 		}
 		break;
+	case WM5110:
+	case WM8280:
+		ret = arizona_wait_for_boot(arizona);
+		if (ret)
+			goto err;
+
+		if (arizona->external_dcvdd) {
+			ret = regmap_update_bits(arizona->regmap,
+						 ARIZONA_ISOLATION_CONTROL,
+						 ARIZONA_ISOLATE_DCVDD1, 0);
+			if (ret) {
+				dev_err(arizona->dev,
+					"Failed to connect DCVDD: %d\n", ret);
+				goto err;
+			}
+		} else {
+			/*
+			 * As this is only called for the internal regulator
+			 * (where we know voltage ranges available) it is ok
+			 * to request an exact range.
+			 */
+			ret = regulator_set_voltage(arizona->dcvdd,
+						    1200000, 1200000);
+			if (ret < 0) {
+				dev_err(arizona->dev,
+					"Failed to set resume voltage: %d\n",
+					ret);
+				goto err;
+			}
+		}
+
+		ret = wm5110_apply_sleep_patch(arizona);
+		if (ret) {
+			dev_err(arizona->dev,
+				"Failed to re-apply sleep patch: %d\n",
+				ret);
+			goto err;
+		}
+		break;
 	default:
 		ret = arizona_wait_for_boot(arizona);
 		if (ret != 0) {
@@ -410,10 +584,17 @@ err:
 static int arizona_runtime_suspend(struct device *dev)
 {
 	struct arizona *arizona = dev_get_drvdata(dev);
+	unsigned int val;
 	int ret;
 
 	dev_dbg(arizona->dev, "Entering AoD mode\n");
 
+	ret = regmap_read(arizona->regmap, ARIZONA_JACK_DETECT_ANALOGUE, &val);
+	if (ret) {
+		dev_err(dev, "Failed to check jack det status: %d\n", ret);
+		return ret;
+	}
+
 	if (arizona->external_dcvdd) {
 		ret = regmap_update_bits(arizona->regmap,
 					 ARIZONA_ISOLATION_CONTROL,
@@ -426,10 +607,56 @@ static int arizona_runtime_suspend(struct device *dev)
 		}
 	}
 
+	switch (arizona->type) {
+	case WM5110:
+	case WM8280:
+		if (arizona->external_dcvdd)
+			break;
+
+		/*
+		 * As this is only called for the internal regulator
+		 * (where we know voltage ranges available) it is ok
+		 * to request an exact range.
+		 */
+		ret = regulator_set_voltage(arizona->dcvdd, 1175000, 1175000);
+		if (ret < 0) {
+			dev_err(arizona->dev,
+				"Failed to set suspend voltage: %d\n", ret);
+			return ret;
+		}
+		break;
+	case WM5102:
+		if (!(val & ARIZONA_JD1_ENA)) {
+			ret = regmap_write(arizona->regmap,
+					   ARIZONA_WRITE_SEQUENCER_CTRL_3, 0x0);
+			if (ret) {
+				dev_err(arizona->dev,
+					"Failed to clear write sequencer: %d\n",
+					ret);
+				return ret;
+			}
+		}
+		break;
+	default:
+		break;
+	}
+
 	regcache_cache_only(arizona->regmap, true);
 	regcache_mark_dirty(arizona->regmap);
 	regulator_disable(arizona->dcvdd);
 
+	/* Allow us to completely power down if no jack detection */
+	if (!(val & ARIZONA_JD1_ENA)) {
+		dev_dbg(arizona->dev, "Fully powering off\n");
+
+		arizona->has_fully_powered_off = true;
+
+		disable_irq(arizona->irq);
+		arizona_enable_reset(arizona);
+		regulator_bulk_disable(arizona->num_core_supplies,
+				       arizona->core_supplies);
+	}
+
 	return 0;
 }
 #endif
@@ -728,9 +955,9 @@ int arizona_dev_init(struct arizona *arizona)
 
 	if (arizona->pdata.reset) {
 		/* Start out with /RESET low to put the chip into reset */
-		ret = gpio_request_one(arizona->pdata.reset,
-				       GPIOF_DIR_OUT | GPIOF_INIT_LOW,
-				       "arizona /RESET");
+		ret = devm_gpio_request_one(arizona->dev, arizona->pdata.reset,
+					    GPIOF_DIR_OUT | GPIOF_INIT_LOW,
+					    "arizona /RESET");
 		if (ret != 0) {
 			dev_err(dev, "Failed to request /RESET: %d\n", ret);
 			goto err_dcvdd;
@@ -751,10 +978,7 @@ int arizona_dev_init(struct arizona *arizona)
 		goto err_enable;
 	}
 
-	if (arizona->pdata.reset) {
-		gpio_set_value_cansleep(arizona->pdata.reset, 1);
-		msleep(1);
-	}
+	arizona_disable_reset(arizona);
 
 	regcache_cache_only(arizona->regmap, false);
 
@@ -777,8 +1001,6 @@ int arizona_dev_init(struct arizona *arizona)
 
 	/* If we have a /RESET GPIO we'll already be reset */
 	if (!arizona->pdata.reset) {
-		regcache_mark_dirty(arizona->regmap);
-
 		ret = regmap_write(arizona->regmap, ARIZONA_SOFTWARE_RESET, 0);
 		if (ret != 0) {
 			dev_err(dev, "Failed to reset device: %d\n", ret);
@@ -786,12 +1008,6 @@ int arizona_dev_init(struct arizona *arizona)
 		}
 
 		msleep(1);
-
-		ret = regcache_sync(arizona->regmap);
-		if (ret != 0) {
-			dev_err(dev, "Failed to sync device: %d\n", ret);
-			goto err_reset;
-		}
 	}
 
 	/* Ensure device startup is complete */
@@ -799,21 +1015,24 @@ int arizona_dev_init(struct arizona *arizona)
 	case WM5102:
 		ret = regmap_read(arizona->regmap,
 				  ARIZONA_WRITE_SEQUENCER_CTRL_3, &val);
-		if (ret != 0)
+		if (ret) {
 			dev_err(dev,
 				"Failed to check write sequencer state: %d\n",
 				ret);
-		else if (val & 0x01)
-			break;
-		/* Fall through */
-	default:
-		ret = arizona_wait_for_boot(arizona);
-		if (ret != 0) {
-			dev_err(arizona->dev,
-				"Device failed initial boot: %d\n", ret);
-			goto err_reset;
+		} else if (val & 0x01) {
+			ret = wm5102_clear_write_sequencer(arizona);
+			if (ret)
+				return ret;
 		}
 		break;
+	default:
+		break;
+	}
+
+	ret = arizona_wait_for_boot(arizona);
+	if (ret) {
+		dev_err(arizona->dev, "Device failed initial boot: %d\n", ret);
+		goto err_reset;
 	}
 
 	/* Read the device ID information & do device specific stuff */
@@ -891,14 +1110,24 @@ int arizona_dev_init(struct arizona *arizona)
 
 		switch (arizona->type) {
 		case WM5102:
-			ret = arizona_apply_hardware_patch(arizona);
-			if (ret != 0) {
+			ret = wm5102_apply_hardware_patch(arizona);
+			if (ret) {
 				dev_err(arizona->dev,
 					"Failed to apply hardware patch: %d\n",
 					ret);
 				goto err_reset;
 			}
 			break;
+		case WM5110:
+		case WM8280:
+			ret = wm5110_apply_sleep_patch(arizona);
+			if (ret) {
+				dev_err(arizona->dev,
+					"Failed to apply sleep patch: %d\n",
+					ret);
+				goto err_reset;
+			}
+			break;
 		default:
 			break;
 		}
@@ -977,12 +1206,16 @@ int arizona_dev_init(struct arizona *arizona)
 		/* Default for both is 0 so noop with defaults */
 		val = arizona->pdata.dmic_ref[i]
 			<< ARIZONA_IN1_DMIC_SUP_SHIFT;
-		val |= arizona->pdata.inmode[i] << ARIZONA_IN1_MODE_SHIFT;
+		if (arizona->pdata.inmode[i] & ARIZONA_INMODE_DMIC)
+			val |= 1 << ARIZONA_IN1_MODE_SHIFT;
+		if (arizona->pdata.inmode[i] & ARIZONA_INMODE_SE)
+			val |= 1 << ARIZONA_IN1_SINGLE_ENDED_SHIFT;
 
 		regmap_update_bits(arizona->regmap,
 				   ARIZONA_IN1L_CONTROL + (i * 8),
 				   ARIZONA_IN1_DMIC_SUP_MASK |
-				   ARIZONA_IN1_MODE_MASK, val);
+				   ARIZONA_IN1_MODE_MASK |
+				   ARIZONA_IN1_SINGLE_ENDED_MASK, val);
 	}
 
 	for (i = 0; i < ARIZONA_MAX_OUTPUT; i++) {
@@ -1054,10 +1287,7 @@ int arizona_dev_init(struct arizona *arizona)
 err_irq:
 	arizona_irq_exit(arizona);
 err_reset:
-	if (arizona->pdata.reset) {
-		gpio_set_value_cansleep(arizona->pdata.reset, 0);
-		gpio_free(arizona->pdata.reset);
-	}
+	arizona_enable_reset(arizona);
 	regulator_disable(arizona->dcvdd);
 err_enable:
 	regulator_bulk_disable(arizona->num_core_supplies,
@@ -1082,8 +1312,7 @@ int arizona_dev_exit(struct arizona *arizona)
 	arizona_free_irq(arizona, ARIZONA_IRQ_OVERCLOCKED, arizona);
 	arizona_free_irq(arizona, ARIZONA_IRQ_CLKGEN_ERR, arizona);
 	arizona_irq_exit(arizona);
-	if (arizona->pdata.reset)
-		gpio_set_value_cansleep(arizona->pdata.reset, 0);
+	arizona_enable_reset(arizona);
 
 	regulator_bulk_disable(arizona->num_core_supplies,
 			       arizona->core_supplies);
diff --git a/drivers/mfd/arizona-irq.c b/drivers/mfd/arizona-irq.c
index d063b94b94b5..2b9965d53e4e 100644
--- a/drivers/mfd/arizona-irq.c
+++ b/drivers/mfd/arizona-irq.c
@@ -186,7 +186,7 @@ static int arizona_irq_map(struct irq_domain *h, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops arizona_domain_ops = {
+static const struct irq_domain_ops arizona_domain_ops = {
 	.map	= arizona_irq_map,
 	.xlate	= irq_domain_xlate_twocell,
 };
diff --git a/drivers/mfd/asic3.c b/drivers/mfd/asic3.c
index 977bd3a3eed0..120df5c08741 100644
--- a/drivers/mfd/asic3.c
+++ b/drivers/mfd/asic3.c
@@ -417,9 +417,8 @@ static int __init asic3_irq_probe(struct platform_device *pdev)
 	asic3_write_register(asic, ASIC3_OFFSET(INTR, INT_MASK),
 			     ASIC3_INTMASK_GINTMASK);
 
-	irq_set_chained_handler(asic->irq_nr, asic3_irq_demux);
+	irq_set_chained_handler_and_data(asic->irq_nr, asic3_irq_demux, asic);
 	irq_set_irq_type(asic->irq_nr, IRQ_TYPE_EDGE_RISING);
-	irq_set_handler_data(asic->irq_nr, asic);
 
 	return 0;
 }
diff --git a/drivers/mfd/axp20x.c b/drivers/mfd/axp20x.c
index d18029be6a78..6df91556faf3 100644
--- a/drivers/mfd/axp20x.c
+++ b/drivers/mfd/axp20x.c
@@ -32,6 +32,7 @@
 static const char * const axp20x_model_names[] = {
 	"AXP202",
 	"AXP209",
+	"AXP221",
 	"AXP288",
 };
 
@@ -54,6 +55,25 @@ static const struct regmap_access_table axp20x_volatile_table = {
 	.n_yes_ranges	= ARRAY_SIZE(axp20x_volatile_ranges),
 };
 
+static const struct regmap_range axp22x_writeable_ranges[] = {
+	regmap_reg_range(AXP20X_DATACACHE(0), AXP20X_IRQ5_STATE),
+	regmap_reg_range(AXP20X_DCDC_MODE, AXP22X_BATLOW_THRES1),
+};
+
+static const struct regmap_range axp22x_volatile_ranges[] = {
+	regmap_reg_range(AXP20X_IRQ1_EN, AXP20X_IRQ5_STATE),
+};
+
+static const struct regmap_access_table axp22x_writeable_table = {
+	.yes_ranges	= axp22x_writeable_ranges,
+	.n_yes_ranges	= ARRAY_SIZE(axp22x_writeable_ranges),
+};
+
+static const struct regmap_access_table axp22x_volatile_table = {
+	.yes_ranges	= axp22x_volatile_ranges,
+	.n_yes_ranges	= ARRAY_SIZE(axp22x_volatile_ranges),
+};
+
 static const struct regmap_range axp288_writeable_ranges[] = {
 	regmap_reg_range(AXP20X_DATACACHE(0), AXP20X_IRQ6_STATE),
 	regmap_reg_range(AXP20X_DCDC_MODE, AXP288_FG_TUNE5),
@@ -87,6 +107,20 @@ static struct resource axp20x_pek_resources[] = {
 	},
 };
 
+static struct resource axp22x_pek_resources[] = {
+	{
+		.name   = "PEK_DBR",
+		.start  = AXP22X_IRQ_PEK_RIS_EDGE,
+		.end    = AXP22X_IRQ_PEK_RIS_EDGE,
+		.flags  = IORESOURCE_IRQ,
+	}, {
+		.name   = "PEK_DBF",
+		.start  = AXP22X_IRQ_PEK_FAL_EDGE,
+		.end    = AXP22X_IRQ_PEK_FAL_EDGE,
+		.flags  = IORESOURCE_IRQ,
+	},
+};
+
 static struct resource axp288_fuel_gauge_resources[] = {
 	{
 		.start = AXP288_IRQ_QWBTU,
@@ -129,6 +163,15 @@ static const struct regmap_config axp20x_regmap_config = {
 	.cache_type	= REGCACHE_RBTREE,
 };
 
+static const struct regmap_config axp22x_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.wr_table	= &axp22x_writeable_table,
+	.volatile_table	= &axp22x_volatile_table,
+	.max_register	= AXP22X_BATLOW_THRES1,
+	.cache_type	= REGCACHE_RBTREE,
+};
+
 static const struct regmap_config axp288_regmap_config = {
 	.reg_bits	= 8,
 	.val_bits	= 8,
@@ -181,6 +224,34 @@ static const struct regmap_irq axp20x_regmap_irqs[] = {
 	INIT_REGMAP_IRQ(AXP20X, GPIO0_INPUT,		4, 0),
 };
 
+static const struct regmap_irq axp22x_regmap_irqs[] = {
+	INIT_REGMAP_IRQ(AXP22X, ACIN_OVER_V,		0, 7),
+	INIT_REGMAP_IRQ(AXP22X, ACIN_PLUGIN,		0, 6),
+	INIT_REGMAP_IRQ(AXP22X, ACIN_REMOVAL,	        0, 5),
+	INIT_REGMAP_IRQ(AXP22X, VBUS_OVER_V,		0, 4),
+	INIT_REGMAP_IRQ(AXP22X, VBUS_PLUGIN,		0, 3),
+	INIT_REGMAP_IRQ(AXP22X, VBUS_REMOVAL,	        0, 2),
+	INIT_REGMAP_IRQ(AXP22X, VBUS_V_LOW,		0, 1),
+	INIT_REGMAP_IRQ(AXP22X, BATT_PLUGIN,		1, 7),
+	INIT_REGMAP_IRQ(AXP22X, BATT_REMOVAL,	        1, 6),
+	INIT_REGMAP_IRQ(AXP22X, BATT_ENT_ACT_MODE,	1, 5),
+	INIT_REGMAP_IRQ(AXP22X, BATT_EXIT_ACT_MODE,	1, 4),
+	INIT_REGMAP_IRQ(AXP22X, CHARG,		        1, 3),
+	INIT_REGMAP_IRQ(AXP22X, CHARG_DONE,		1, 2),
+	INIT_REGMAP_IRQ(AXP22X, BATT_TEMP_HIGH,	        1, 1),
+	INIT_REGMAP_IRQ(AXP22X, BATT_TEMP_LOW,	        1, 0),
+	INIT_REGMAP_IRQ(AXP22X, DIE_TEMP_HIGH,	        2, 7),
+	INIT_REGMAP_IRQ(AXP22X, PEK_SHORT,		2, 1),
+	INIT_REGMAP_IRQ(AXP22X, PEK_LONG,		2, 0),
+	INIT_REGMAP_IRQ(AXP22X, LOW_PWR_LVL1,	        3, 1),
+	INIT_REGMAP_IRQ(AXP22X, LOW_PWR_LVL2,	        3, 0),
+	INIT_REGMAP_IRQ(AXP22X, TIMER,		        4, 7),
+	INIT_REGMAP_IRQ(AXP22X, PEK_RIS_EDGE,	        4, 6),
+	INIT_REGMAP_IRQ(AXP22X, PEK_FAL_EDGE,	        4, 5),
+	INIT_REGMAP_IRQ(AXP22X, GPIO1_INPUT,		4, 1),
+	INIT_REGMAP_IRQ(AXP22X, GPIO0_INPUT,		4, 0),
+};
+
 /* some IRQs are compatible with axp20x models */
 static const struct regmap_irq axp288_regmap_irqs[] = {
 	INIT_REGMAP_IRQ(AXP288, VBUS_FALL,              0, 2),
@@ -224,6 +295,7 @@ static const struct regmap_irq axp288_regmap_irqs[] = {
 static const struct of_device_id axp20x_of_match[] = {
 	{ .compatible = "x-powers,axp202", .data = (void *) AXP202_ID },
 	{ .compatible = "x-powers,axp209", .data = (void *) AXP209_ID },
+	{ .compatible = "x-powers,axp221", .data = (void *) AXP221_ID },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, axp20x_of_match);
@@ -258,6 +330,18 @@ static const struct regmap_irq_chip axp20x_regmap_irq_chip = {
 
 };
 
+static const struct regmap_irq_chip axp22x_regmap_irq_chip = {
+	.name			= "axp22x_irq_chip",
+	.status_base		= AXP20X_IRQ1_STATE,
+	.ack_base		= AXP20X_IRQ1_STATE,
+	.mask_base		= AXP20X_IRQ1_EN,
+	.mask_invert		= true,
+	.init_ack_masked	= true,
+	.irqs			= axp22x_regmap_irqs,
+	.num_irqs		= ARRAY_SIZE(axp22x_regmap_irqs),
+	.num_regs		= 5,
+};
+
 static const struct regmap_irq_chip axp288_regmap_irq_chip = {
 	.name			= "axp288_irq_chip",
 	.status_base		= AXP20X_IRQ1_STATE,
@@ -281,6 +365,16 @@ static struct mfd_cell axp20x_cells[] = {
 	},
 };
 
+static struct mfd_cell axp22x_cells[] = {
+	{
+		.name			= "axp20x-pek",
+		.num_resources		= ARRAY_SIZE(axp22x_pek_resources),
+		.resources		= axp22x_pek_resources,
+	}, {
+		.name			= "axp20x-regulator",
+	},
+};
+
 static struct resource axp288_adc_resources[] = {
 	{
 		.name  = "GPADC",
@@ -426,6 +520,12 @@ static int axp20x_match_device(struct axp20x_dev *axp20x, struct device *dev)
 		axp20x->regmap_cfg = &axp20x_regmap_config;
 		axp20x->regmap_irq_chip = &axp20x_regmap_irq_chip;
 		break;
+	case AXP221_ID:
+		axp20x->nr_cells = ARRAY_SIZE(axp22x_cells);
+		axp20x->cells = axp22x_cells;
+		axp20x->regmap_cfg = &axp22x_regmap_config;
+		axp20x->regmap_irq_chip = &axp22x_regmap_irq_chip;
+		break;
 	case AXP288_ID:
 		axp20x->cells = axp288_cells;
 		axp20x->nr_cells = ARRAY_SIZE(axp288_cells);
diff --git a/drivers/mfd/cros_ec.c b/drivers/mfd/cros_ec.c
index c4aecc6f8373..0eee63542038 100644
--- a/drivers/mfd/cros_ec.c
+++ b/drivers/mfd/cros_ec.c
@@ -17,111 +17,36 @@
  * battery charging and regulator control, firmware update.
  */
 
+#include <linux/of_platform.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/cros_ec.h>
-#include <linux/mfd/cros_ec_commands.h>
-#include <linux/delay.h>
 
-#define EC_COMMAND_RETRIES	50
+#define CROS_EC_DEV_EC_INDEX 0
+#define CROS_EC_DEV_PD_INDEX 1
 
-int cros_ec_prepare_tx(struct cros_ec_device *ec_dev,
-		       struct cros_ec_command *msg)
-{
-	uint8_t *out;
-	int csum, i;
-
-	BUG_ON(msg->outsize > EC_PROTO2_MAX_PARAM_SIZE);
-	out = ec_dev->dout;
-	out[0] = EC_CMD_VERSION0 + msg->version;
-	out[1] = msg->command;
-	out[2] = msg->outsize;
-	csum = out[0] + out[1] + out[2];
-	for (i = 0; i < msg->outsize; i++)
-		csum += out[EC_MSG_TX_HEADER_BYTES + i] = msg->outdata[i];
-	out[EC_MSG_TX_HEADER_BYTES + msg->outsize] = (uint8_t)(csum & 0xff);
-
-	return EC_MSG_TX_PROTO_BYTES + msg->outsize;
-}
-EXPORT_SYMBOL(cros_ec_prepare_tx);
-
-int cros_ec_check_result(struct cros_ec_device *ec_dev,
-			 struct cros_ec_command *msg)
-{
-	switch (msg->result) {
-	case EC_RES_SUCCESS:
-		return 0;
-	case EC_RES_IN_PROGRESS:
-		dev_dbg(ec_dev->dev, "command 0x%02x in progress\n",
-			msg->command);
-		return -EAGAIN;
-	default:
-		dev_dbg(ec_dev->dev, "command 0x%02x returned %d\n",
-			msg->command, msg->result);
-		return 0;
-	}
-}
-EXPORT_SYMBOL(cros_ec_check_result);
-
-int cros_ec_cmd_xfer(struct cros_ec_device *ec_dev,
-		     struct cros_ec_command *msg)
-{
-	int ret;
-
-	mutex_lock(&ec_dev->lock);
-	ret = ec_dev->cmd_xfer(ec_dev, msg);
-	if (msg->result == EC_RES_IN_PROGRESS) {
-		int i;
-		struct cros_ec_command status_msg = { };
-		struct ec_response_get_comms_status *status;
-
-		status_msg.command = EC_CMD_GET_COMMS_STATUS;
-		status_msg.insize = sizeof(*status);
-
-		/*
-		 * Query the EC's status until it's no longer busy or
-		 * we encounter an error.
-		 */
-		for (i = 0; i < EC_COMMAND_RETRIES; i++) {
-			usleep_range(10000, 11000);
-
-			ret = ec_dev->cmd_xfer(ec_dev, &status_msg);
-			if (ret < 0)
-				break;
+static struct cros_ec_platform ec_p = {
+	.ec_name = CROS_EC_DEV_NAME,
+	.cmd_offset = EC_CMD_PASSTHRU_OFFSET(CROS_EC_DEV_EC_INDEX),
+};
 
-			msg->result = status_msg.result;
-			if (status_msg.result != EC_RES_SUCCESS)
-				break;
+static struct cros_ec_platform pd_p = {
+	.ec_name = CROS_EC_DEV_PD_NAME,
+	.cmd_offset = EC_CMD_PASSTHRU_OFFSET(CROS_EC_DEV_PD_INDEX),
+};
 
-			status = (struct ec_response_get_comms_status *)
-				 status_msg.indata;
-			if (!(status->flags & EC_COMMS_STATUS_PROCESSING))
-				break;
-		}
-	}
-	mutex_unlock(&ec_dev->lock);
+static const struct mfd_cell ec_cell = {
+	.name = "cros-ec-ctl",
+	.platform_data = &ec_p,
+	.pdata_size = sizeof(ec_p),
+};
 
-	return ret;
-}
-EXPORT_SYMBOL(cros_ec_cmd_xfer);
-
-static const struct mfd_cell cros_devs[] = {
-	{
-		.name = "cros-ec-keyb",
-		.id = 1,
-		.of_compatible = "google,cros-ec-keyb",
-	},
-	{
-		.name = "cros-ec-i2c-tunnel",
-		.id = 2,
-		.of_compatible = "google,cros-ec-i2c-tunnel",
-	},
-	{
-		.name = "cros-ec-ctl",
-		.id = 3,
-	},
+static const struct mfd_cell ec_pd_cell = {
+	.name = "cros-ec-ctl",
+	.platform_data = &pd_p,
+	.pdata_size = sizeof(pd_p),
 };
 
 int cros_ec_register(struct cros_ec_device *ec_dev)
@@ -129,27 +54,59 @@ int cros_ec_register(struct cros_ec_device *ec_dev)
 	struct device *dev = ec_dev->dev;
 	int err = 0;
 
-	if (ec_dev->din_size) {
-		ec_dev->din = devm_kzalloc(dev, ec_dev->din_size, GFP_KERNEL);
-		if (!ec_dev->din)
-			return -ENOMEM;
-	}
-	if (ec_dev->dout_size) {
-		ec_dev->dout = devm_kzalloc(dev, ec_dev->dout_size, GFP_KERNEL);
-		if (!ec_dev->dout)
-			return -ENOMEM;
-	}
+	ec_dev->max_request = sizeof(struct ec_params_hello);
+	ec_dev->max_response = sizeof(struct ec_response_get_protocol_info);
+	ec_dev->max_passthru = 0;
+
+	ec_dev->din = devm_kzalloc(dev, ec_dev->din_size, GFP_KERNEL);
+	if (!ec_dev->din)
+		return -ENOMEM;
+
+	ec_dev->dout = devm_kzalloc(dev, ec_dev->dout_size, GFP_KERNEL);
+	if (!ec_dev->dout)
+		return -ENOMEM;
 
 	mutex_init(&ec_dev->lock);
 
-	err = mfd_add_devices(dev, 0, cros_devs,
-			      ARRAY_SIZE(cros_devs),
+	cros_ec_query_all(ec_dev);
+
+	err = mfd_add_devices(ec_dev->dev, PLATFORM_DEVID_AUTO, &ec_cell, 1,
 			      NULL, ec_dev->irq, NULL);
 	if (err) {
-		dev_err(dev, "failed to add mfd devices\n");
+		dev_err(dev,
+			"Failed to register Embedded Controller subdevice %d\n",
+			err);
 		return err;
 	}
 
+	if (ec_dev->max_passthru) {
+		/*
+		 * Register a PD device as well on top of this device.
+		 * We make the following assumptions:
+		 * - behind an EC, we have a pd
+		 * - only one device added.
+		 * - the EC is responsive at init time (it is not true for a
+		 *   sensor hub.
+		 */
+		err = mfd_add_devices(ec_dev->dev, PLATFORM_DEVID_AUTO,
+				      &ec_pd_cell, 1, NULL, ec_dev->irq, NULL);
+		if (err) {
+			dev_err(dev,
+				"Failed to register Power Delivery subdevice %d\n",
+				err);
+			return err;
+		}
+	}
+
+	if (IS_ENABLED(CONFIG_OF) && dev->of_node) {
+		err = of_platform_populate(dev->of_node, NULL, NULL, dev);
+		if (err) {
+			mfd_remove_devices(dev);
+			dev_err(dev, "Failed to register sub-devices\n");
+			return err;
+		}
+	}
+
 	dev_info(dev, "Chrome EC device registered\n");
 
 	return 0;
diff --git a/drivers/mfd/cros_ec_i2c.c b/drivers/mfd/cros_ec_i2c.c
index c0c30f4f946f..b9a0963ca5c3 100644
--- a/drivers/mfd/cros_ec_i2c.c
+++ b/drivers/mfd/cros_ec_i2c.c
@@ -13,6 +13,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/delay.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/i2c.h>
@@ -22,6 +23,32 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 
+/**
+ * Request format for protocol v3
+ * byte 0	0xda (EC_COMMAND_PROTOCOL_3)
+ * byte 1-8	struct ec_host_request
+ * byte 10-	response data
+ */
+struct ec_host_request_i2c {
+	/* Always 0xda to backward compatible with v2 struct */
+	uint8_t  command_protocol;
+	struct ec_host_request ec_request;
+} __packed;
+
+
+/*
+ * Response format for protocol v3
+ * byte 0	result code
+ * byte 1	packet_length
+ * byte 2-9	struct ec_host_response
+ * byte 10-	response data
+ */
+struct ec_host_response_i2c {
+	uint8_t result;
+	uint8_t packet_length;
+	struct ec_host_response ec_response;
+} __packed;
+
 static inline struct cros_ec_device *to_ec_dev(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
@@ -29,6 +56,134 @@ static inline struct cros_ec_device *to_ec_dev(struct device *dev)
 	return i2c_get_clientdata(client);
 }
 
+static int cros_ec_pkt_xfer_i2c(struct cros_ec_device *ec_dev,
+				struct cros_ec_command *msg)
+{
+	struct i2c_client *client = ec_dev->priv;
+	int ret = -ENOMEM;
+	int i;
+	int packet_len;
+	u8 *out_buf = NULL;
+	u8 *in_buf = NULL;
+	u8 sum;
+	struct i2c_msg i2c_msg[2];
+	struct ec_host_response *ec_response;
+	struct ec_host_request_i2c *ec_request_i2c;
+	struct ec_host_response_i2c *ec_response_i2c;
+	int request_header_size = sizeof(struct ec_host_request_i2c);
+	int response_header_size = sizeof(struct ec_host_response_i2c);
+
+	i2c_msg[0].addr = client->addr;
+	i2c_msg[0].flags = 0;
+	i2c_msg[1].addr = client->addr;
+	i2c_msg[1].flags = I2C_M_RD;
+
+	packet_len = msg->insize + response_header_size;
+	BUG_ON(packet_len > ec_dev->din_size);
+	in_buf = ec_dev->din;
+	i2c_msg[1].len = packet_len;
+	i2c_msg[1].buf = (char *) in_buf;
+
+	packet_len = msg->outsize + request_header_size;
+	BUG_ON(packet_len > ec_dev->dout_size);
+	out_buf = ec_dev->dout;
+	i2c_msg[0].len = packet_len;
+	i2c_msg[0].buf = (char *) out_buf;
+
+	/* create request data */
+	ec_request_i2c = (struct ec_host_request_i2c *) out_buf;
+	ec_request_i2c->command_protocol = EC_COMMAND_PROTOCOL_3;
+
+	ec_dev->dout++;
+	ret = cros_ec_prepare_tx(ec_dev, msg);
+	ec_dev->dout--;
+
+	/* send command to EC and read answer */
+	ret = i2c_transfer(client->adapter, i2c_msg, 2);
+	if (ret < 0) {
+		dev_dbg(ec_dev->dev, "i2c transfer failed: %d\n", ret);
+		goto done;
+	} else if (ret != 2) {
+		dev_err(ec_dev->dev, "failed to get response: %d\n", ret);
+		ret = -EIO;
+		goto done;
+	}
+
+	ec_response_i2c = (struct ec_host_response_i2c *) in_buf;
+	msg->result = ec_response_i2c->result;
+	ec_response = &ec_response_i2c->ec_response;
+
+	switch (msg->result) {
+	case EC_RES_SUCCESS:
+		break;
+	case EC_RES_IN_PROGRESS:
+		ret = -EAGAIN;
+		dev_dbg(ec_dev->dev, "command 0x%02x in progress\n",
+			msg->command);
+		goto done;
+
+	default:
+		dev_dbg(ec_dev->dev, "command 0x%02x returned %d\n",
+			msg->command, msg->result);
+		/*
+		 * When we send v3 request to v2 ec, ec won't recognize the
+		 * 0xda (EC_COMMAND_PROTOCOL_3) and will return with status
+		 * EC_RES_INVALID_COMMAND with zero data length.
+		 *
+		 * In case of invalid command for v3 protocol the data length
+		 * will be at least sizeof(struct ec_host_response)
+		 */
+		if (ec_response_i2c->result == EC_RES_INVALID_COMMAND &&
+		    ec_response_i2c->packet_length == 0) {
+			ret = -EPROTONOSUPPORT;
+			goto done;
+		}
+	}
+
+	if (ec_response_i2c->packet_length < sizeof(struct ec_host_response)) {
+		dev_err(ec_dev->dev,
+			"response of %u bytes too short; not a full header\n",
+			ec_response_i2c->packet_length);
+		ret = -EBADMSG;
+		goto done;
+	}
+
+	if (msg->insize < ec_response->data_len) {
+		dev_err(ec_dev->dev,
+			"response data size is too large: expected %u, got %u\n",
+			msg->insize,
+			ec_response->data_len);
+		ret = -EMSGSIZE;
+		goto done;
+	}
+
+	/* copy response packet payload and compute checksum */
+	sum = 0;
+	for (i = 0; i < sizeof(struct ec_host_response); i++)
+		sum += ((u8 *)ec_response)[i];
+
+	memcpy(msg->data,
+	       in_buf + response_header_size,
+	       ec_response->data_len);
+	for (i = 0; i < ec_response->data_len; i++)
+		sum += msg->data[i];
+
+	/* All bytes should sum to zero */
+	if (sum) {
+		dev_err(ec_dev->dev, "bad packet checksum\n");
+		ret = -EBADMSG;
+		goto done;
+	}
+
+	ret = ec_response->data_len;
+
+done:
+	if (msg->command == EC_CMD_REBOOT_EC)
+		msleep(EC_REBOOT_DELAY_MS);
+
+	return ret;
+}
+
 static int cros_ec_cmd_xfer_i2c(struct cros_ec_device *ec_dev,
 				struct cros_ec_command *msg)
 {
@@ -76,7 +231,7 @@ static int cros_ec_cmd_xfer_i2c(struct cros_ec_device *ec_dev,
 	/* copy message payload and compute checksum */
 	sum = out_buf[0] + out_buf[1] + out_buf[2];
 	for (i = 0; i < msg->outsize; i++) {
-		out_buf[3 + i] = msg->outdata[i];
+		out_buf[3 + i] = msg->data[i];
 		sum += out_buf[3 + i];
 	}
 	out_buf[3 + msg->outsize] = sum;
@@ -109,7 +264,7 @@ static int cros_ec_cmd_xfer_i2c(struct cros_ec_device *ec_dev,
 	/* copy response packet payload and compute checksum */
 	sum = in_buf[0] + in_buf[1];
 	for (i = 0; i < len; i++) {
-		msg->indata[i] = in_buf[2 + i];
+		msg->data[i] = in_buf[2 + i];
 		sum += in_buf[2 + i];
 	}
 	dev_dbg(ec_dev->dev, "packet: %*ph, sum = %02x\n",
@@ -121,9 +276,12 @@ static int cros_ec_cmd_xfer_i2c(struct cros_ec_device *ec_dev,
 	}
 
 	ret = len;
- done:
+done:
 	kfree(in_buf);
 	kfree(out_buf);
+	if (msg->command == EC_CMD_REBOOT_EC)
+		msleep(EC_REBOOT_DELAY_MS);
+
 	return ret;
 }
 
@@ -143,9 +301,11 @@ static int cros_ec_i2c_probe(struct i2c_client *client,
 	ec_dev->priv = client;
 	ec_dev->irq = client->irq;
 	ec_dev->cmd_xfer = cros_ec_cmd_xfer_i2c;
-	ec_dev->ec_name = client->name;
+	ec_dev->pkt_xfer = cros_ec_pkt_xfer_i2c;
 	ec_dev->phys_name = client->adapter->name;
-	ec_dev->parent = &client->dev;
+	ec_dev->din_size = sizeof(struct ec_host_response_i2c) +
+			   sizeof(struct ec_response_get_protocol_info);
+	ec_dev->dout_size = sizeof(struct ec_host_request_i2c);
 
 	err = cros_ec_register(ec_dev);
 	if (err) {
diff --git a/drivers/mfd/cros_ec_spi.c b/drivers/mfd/cros_ec_spi.c
index bf6e08e8013e..16f228dc243f 100644
--- a/drivers/mfd/cros_ec_spi.c
+++ b/drivers/mfd/cros_ec_spi.c
@@ -65,29 +65,26 @@
   */
 #define EC_SPI_RECOVERY_TIME_NS	(200 * 1000)
 
-/*
- * The EC is unresponsive for a time after a reboot command.  Add a
- * simple delay to make sure that the bus stays locked.
- */
-#define EC_REBOOT_DELAY_MS	50
-
 /**
  * struct cros_ec_spi - information about a SPI-connected EC
  *
  * @spi: SPI device we are connected to
  * @last_transfer_ns: time that we last finished a transfer, or 0 if there
  *	if no record
+ * @start_of_msg_delay: used to set the delay_usecs on the spi_transfer that
+ *      is sent when we want to turn on CS at the start of a transaction.
  * @end_of_msg_delay: used to set the delay_usecs on the spi_transfer that
  *      is sent when we want to turn off CS at the end of a transaction.
  */
 struct cros_ec_spi {
 	struct spi_device *spi;
 	s64 last_transfer_ns;
+	unsigned int start_of_msg_delay;
 	unsigned int end_of_msg_delay;
 };
 
 static void debug_packet(struct device *dev, const char *name, u8 *ptr,
-			  int len)
+			 int len)
 {
 #ifdef DEBUG
 	int i;
@@ -100,6 +97,172 @@ static void debug_packet(struct device *dev, const char *name, u8 *ptr,
 #endif
 }
 
+static int terminate_request(struct cros_ec_device *ec_dev)
+{
+	struct cros_ec_spi *ec_spi = ec_dev->priv;
+	struct spi_message msg;
+	struct spi_transfer trans;
+	int ret;
+
+	/*
+	 * Turn off CS, possibly adding a delay to ensure the rising edge
+	 * doesn't come too soon after the end of the data.
+	 */
+	spi_message_init(&msg);
+	memset(&trans, 0, sizeof(trans));
+	trans.delay_usecs = ec_spi->end_of_msg_delay;
+	spi_message_add_tail(&trans, &msg);
+
+	ret = spi_sync(ec_spi->spi, &msg);
+
+	/* Reset end-of-response timer */
+	ec_spi->last_transfer_ns = ktime_get_ns();
+	if (ret < 0) {
+		dev_err(ec_dev->dev,
+			"cs-deassert spi transfer failed: %d\n",
+			ret);
+	}
+
+	return ret;
+}
+
+/**
+ * receive_n_bytes - receive n bytes from the EC.
+ *
+ * Assumes buf is a pointer into the ec_dev->din buffer
+ */
+static int receive_n_bytes(struct cros_ec_device *ec_dev, u8 *buf, int n)
+{
+	struct cros_ec_spi *ec_spi = ec_dev->priv;
+	struct spi_transfer trans;
+	struct spi_message msg;
+	int ret;
+
+	BUG_ON(buf - ec_dev->din + n > ec_dev->din_size);
+
+	memset(&trans, 0, sizeof(trans));
+	trans.cs_change = 1;
+	trans.rx_buf = buf;
+	trans.len = n;
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&trans, &msg);
+	ret = spi_sync(ec_spi->spi, &msg);
+	if (ret < 0)
+		dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+
+	return ret;
+}
+
+/**
+ * cros_ec_spi_receive_packet - Receive a packet from the EC.
+ *
+ * This function has two phases: reading the preamble bytes (since if we read
+ * data from the EC before it is ready to send, we just get preamble) and
+ * reading the actual message.
+ *
+ * The received data is placed into ec_dev->din.
+ *
+ * @ec_dev: ChromeOS EC device
+ * @need_len: Number of message bytes we need to read
+ */
+static int cros_ec_spi_receive_packet(struct cros_ec_device *ec_dev,
+				      int need_len)
+{
+	struct ec_host_response *response;
+	u8 *ptr, *end;
+	int ret;
+	unsigned long deadline;
+	int todo;
+
+	BUG_ON(EC_MSG_PREAMBLE_COUNT > ec_dev->din_size);
+
+	/* Receive data until we see the header byte */
+	deadline = jiffies + msecs_to_jiffies(EC_MSG_DEADLINE_MS);
+	while (true) {
+		unsigned long start_jiffies = jiffies;
+
+		ret = receive_n_bytes(ec_dev,
+				      ec_dev->din,
+				      EC_MSG_PREAMBLE_COUNT);
+		if (ret < 0)
+			return ret;
+
+		ptr = ec_dev->din;
+		for (end = ptr + EC_MSG_PREAMBLE_COUNT; ptr != end; ptr++) {
+			if (*ptr == EC_SPI_FRAME_START) {
+				dev_dbg(ec_dev->dev, "msg found at %zd\n",
+					ptr - ec_dev->din);
+				break;
+			}
+		}
+		if (ptr != end)
+			break;
+
+		/*
+		 * Use the time at the start of the loop as a timeout.  This
+		 * gives us one last shot at getting the transfer and is useful
+		 * in case we got context switched out for a while.
+		 */
+		if (time_after(start_jiffies, deadline)) {
+			dev_warn(ec_dev->dev, "EC failed to respond in time\n");
+			return -ETIMEDOUT;
+		}
+	}
+
+	/*
+	 * ptr now points to the header byte. Copy any valid data to the
+	 * start of our buffer
+	 */
+	todo = end - ++ptr;
+	BUG_ON(todo < 0 || todo > ec_dev->din_size);
+	todo = min(todo, need_len);
+	memmove(ec_dev->din, ptr, todo);
+	ptr = ec_dev->din + todo;
+	dev_dbg(ec_dev->dev, "need %d, got %d bytes from preamble\n",
+		need_len, todo);
+	need_len -= todo;
+
+	/* If the entire response struct wasn't read, get the rest of it. */
+	if (todo < sizeof(*response)) {
+		ret = receive_n_bytes(ec_dev, ptr, sizeof(*response) - todo);
+		if (ret < 0)
+			return -EBADMSG;
+		ptr += (sizeof(*response) - todo);
+		todo = sizeof(*response);
+	}
+
+	response = (struct ec_host_response *)ec_dev->din;
+
+	/* Abort if data_len is too large. */
+	if (response->data_len > ec_dev->din_size)
+		return -EMSGSIZE;
+
+	/* Receive data until we have it all */
+	while (need_len > 0) {
+		/*
+		 * We can't support transfers larger than the SPI FIFO size
+		 * unless we have DMA. We don't have DMA on the ISP SPI ports
+		 * for Exynos. We need a way of asking SPI driver for
+		 * maximum-supported transfer size.
+		 */
+		todo = min(need_len, 256);
+		dev_dbg(ec_dev->dev, "loop, todo=%d, need_len=%d, ptr=%zd\n",
+			todo, need_len, ptr - ec_dev->din);
+
+		ret = receive_n_bytes(ec_dev, ptr, todo);
+		if (ret < 0)
+			return ret;
+
+		ptr += todo;
+		need_len -= todo;
+	}
+
+	dev_dbg(ec_dev->dev, "loop done, ptr=%zd\n", ptr - ec_dev->din);
+
+	return 0;
+}
+
 /**
  * cros_ec_spi_receive_response - Receive a response from the EC.
  *
@@ -115,34 +278,27 @@ static void debug_packet(struct device *dev, const char *name, u8 *ptr,
 static int cros_ec_spi_receive_response(struct cros_ec_device *ec_dev,
 					int need_len)
 {
-	struct cros_ec_spi *ec_spi = ec_dev->priv;
-	struct spi_transfer trans;
-	struct spi_message msg;
 	u8 *ptr, *end;
 	int ret;
 	unsigned long deadline;
 	int todo;
 
+	BUG_ON(EC_MSG_PREAMBLE_COUNT > ec_dev->din_size);
+
 	/* Receive data until we see the header byte */
 	deadline = jiffies + msecs_to_jiffies(EC_MSG_DEADLINE_MS);
 	while (true) {
 		unsigned long start_jiffies = jiffies;
 
-		memset(&trans, 0, sizeof(trans));
-		trans.cs_change = 1;
-		trans.rx_buf = ptr = ec_dev->din;
-		trans.len = EC_MSG_PREAMBLE_COUNT;
-
-		spi_message_init(&msg);
-		spi_message_add_tail(&trans, &msg);
-		ret = spi_sync(ec_spi->spi, &msg);
-		if (ret < 0) {
-			dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+		ret = receive_n_bytes(ec_dev,
+				      ec_dev->din,
+				      EC_MSG_PREAMBLE_COUNT);
+		if (ret < 0)
 			return ret;
-		}
 
+		ptr = ec_dev->din;
 		for (end = ptr + EC_MSG_PREAMBLE_COUNT; ptr != end; ptr++) {
-			if (*ptr == EC_MSG_HEADER) {
+			if (*ptr == EC_SPI_FRAME_START) {
 				dev_dbg(ec_dev->dev, "msg found at %zd\n",
 					ptr - ec_dev->din);
 				break;
@@ -187,21 +343,9 @@ static int cros_ec_spi_receive_response(struct cros_ec_device *ec_dev,
 		dev_dbg(ec_dev->dev, "loop, todo=%d, need_len=%d, ptr=%zd\n",
 			todo, need_len, ptr - ec_dev->din);
 
-		memset(&trans, 0, sizeof(trans));
-		trans.cs_change = 1;
-		trans.rx_buf = ptr;
-		trans.len = todo;
-		spi_message_init(&msg);
-		spi_message_add_tail(&trans, &msg);
-
-		/* send command to EC and read answer */
-		BUG_ON((u8 *)trans.rx_buf - ec_dev->din + todo >
-				ec_dev->din_size);
-		ret = spi_sync(ec_spi->spi, &msg);
-		if (ret < 0) {
-			dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+		ret = receive_n_bytes(ec_dev, ptr, todo);
+		if (ret < 0)
 			return ret;
-		}
 
 		debug_packet(ec_dev->dev, "interim", ptr, todo);
 		ptr += todo;
@@ -214,6 +358,138 @@ static int cros_ec_spi_receive_response(struct cros_ec_device *ec_dev,
 }
 
 /**
+ * cros_ec_pkt_xfer_spi - Transfer a packet over SPI and receive the reply
+ *
+ * @ec_dev: ChromeOS EC device
+ * @ec_msg: Message to transfer
+ */
+static int cros_ec_pkt_xfer_spi(struct cros_ec_device *ec_dev,
+				struct cros_ec_command *ec_msg)
+{
+	struct ec_host_request *request;
+	struct ec_host_response *response;
+	struct cros_ec_spi *ec_spi = ec_dev->priv;
+	struct spi_transfer trans, trans_delay;
+	struct spi_message msg;
+	int i, len;
+	u8 *ptr;
+	u8 *rx_buf;
+	u8 sum;
+	int ret = 0, final_ret;
+
+	len = cros_ec_prepare_tx(ec_dev, ec_msg);
+	request = (struct ec_host_request *)ec_dev->dout;
+	dev_dbg(ec_dev->dev, "prepared, len=%d\n", len);
+
+	/* If it's too soon to do another transaction, wait */
+	if (ec_spi->last_transfer_ns) {
+		unsigned long delay;	/* The delay completed so far */
+
+		delay = ktime_get_ns() - ec_spi->last_transfer_ns;
+		if (delay < EC_SPI_RECOVERY_TIME_NS)
+			ndelay(EC_SPI_RECOVERY_TIME_NS - delay);
+	}
+
+	rx_buf = kzalloc(len, GFP_KERNEL);
+	if (!rx_buf) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	/*
+	 * Leave a gap between CS assertion and clocking of data to allow the
+	 * EC time to wakeup.
+	 */
+	spi_message_init(&msg);
+	if (ec_spi->start_of_msg_delay) {
+		memset(&trans_delay, 0, sizeof(trans_delay));
+		trans_delay.delay_usecs = ec_spi->start_of_msg_delay;
+		spi_message_add_tail(&trans_delay, &msg);
+	}
+
+	/* Transmit phase - send our message */
+	memset(&trans, 0, sizeof(trans));
+	trans.tx_buf = ec_dev->dout;
+	trans.rx_buf = rx_buf;
+	trans.len = len;
+	trans.cs_change = 1;
+	spi_message_add_tail(&trans, &msg);
+	ret = spi_sync(ec_spi->spi, &msg);
+
+	/* Get the response */
+	if (!ret) {
+		/* Verify that EC can process command */
+		for (i = 0; i < len; i++) {
+			switch (rx_buf[i]) {
+			case EC_SPI_PAST_END:
+			case EC_SPI_RX_BAD_DATA:
+			case EC_SPI_NOT_READY:
+				ret = -EAGAIN;
+				ec_msg->result = EC_RES_IN_PROGRESS;
+			default:
+				break;
+			}
+			if (ret)
+				break;
+		}
+		if (!ret)
+			ret = cros_ec_spi_receive_packet(ec_dev,
+					ec_msg->insize + sizeof(*response));
+	} else {
+		dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+	}
+
+	final_ret = terminate_request(ec_dev);
+	if (!ret)
+		ret = final_ret;
+	if (ret < 0)
+		goto exit;
+
+	ptr = ec_dev->din;
+
+	/* check response error code */
+	response = (struct ec_host_response *)ptr;
+	ec_msg->result = response->result;
+
+	ret = cros_ec_check_result(ec_dev, ec_msg);
+	if (ret)
+		goto exit;
+
+	len = response->data_len;
+	sum = 0;
+	if (len > ec_msg->insize) {
+		dev_err(ec_dev->dev, "packet too long (%d bytes, expected %d)",
+			len, ec_msg->insize);
+		ret = -EMSGSIZE;
+		goto exit;
+	}
+
+	for (i = 0; i < sizeof(*response); i++)
+		sum += ptr[i];
+
+	/* copy response packet payload and compute checksum */
+	memcpy(ec_msg->data, ptr + sizeof(*response), len);
+	for (i = 0; i < len; i++)
+		sum += ec_msg->data[i];
+
+	if (sum) {
+		dev_err(ec_dev->dev,
+			"bad packet checksum, calculated %x\n",
+			sum);
+		ret = -EBADMSG;
+		goto exit;
+	}
+
+	ret = len;
+exit:
+	kfree(rx_buf);
+	if (ec_msg->command == EC_CMD_REBOOT_EC)
+		msleep(EC_REBOOT_DELAY_MS);
+
+	return ret;
+}
+
+/**
  * cros_ec_cmd_xfer_spi - Transfer a message over SPI and receive the reply
  *
  * @ec_dev: ChromeOS EC device
@@ -227,6 +503,7 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 	struct spi_message msg;
 	int i, len;
 	u8 *ptr;
+	u8 *rx_buf;
 	int sum;
 	int ret = 0, final_ret;
 
@@ -242,10 +519,17 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 			ndelay(EC_SPI_RECOVERY_TIME_NS - delay);
 	}
 
+	rx_buf = kzalloc(len, GFP_KERNEL);
+	if (!rx_buf) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
 	/* Transmit phase - send our message */
 	debug_packet(ec_dev->dev, "out", ec_dev->dout, len);
 	memset(&trans, 0, sizeof(trans));
 	trans.tx_buf = ec_dev->dout;
+	trans.rx_buf = rx_buf;
 	trans.len = len;
 	trans.cs_change = 1;
 	spi_message_init(&msg);
@@ -254,29 +538,32 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 
 	/* Get the response */
 	if (!ret) {
-		ret = cros_ec_spi_receive_response(ec_dev,
-				ec_msg->insize + EC_MSG_TX_PROTO_BYTES);
+		/* Verify that EC can process command */
+		for (i = 0; i < len; i++) {
+			switch (rx_buf[i]) {
+			case EC_SPI_PAST_END:
+			case EC_SPI_RX_BAD_DATA:
+			case EC_SPI_NOT_READY:
+				ret = -EAGAIN;
+				ec_msg->result = EC_RES_IN_PROGRESS;
+			default:
+				break;
+			}
+			if (ret)
+				break;
+		}
+		if (!ret)
+			ret = cros_ec_spi_receive_response(ec_dev,
+					ec_msg->insize + EC_MSG_TX_PROTO_BYTES);
 	} else {
 		dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
 	}
 
-	/*
-	 * Turn off CS, possibly adding a delay to ensure the rising edge
-	 * doesn't come too soon after the end of the data.
-	 */
-	spi_message_init(&msg);
-	memset(&trans, 0, sizeof(trans));
-	trans.delay_usecs = ec_spi->end_of_msg_delay;
-	spi_message_add_tail(&trans, &msg);
-
-	final_ret = spi_sync(ec_spi->spi, &msg);
-	ec_spi->last_transfer_ns = ktime_get_ns();
+	final_ret = terminate_request(ec_dev);
 	if (!ret)
 		ret = final_ret;
-	if (ret < 0) {
-		dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
+	if (ret < 0)
 		goto exit;
-	}
 
 	ptr = ec_dev->din;
 
@@ -299,7 +586,7 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 	for (i = 0; i < len; i++) {
 		sum += ptr[i + 2];
 		if (ec_msg->insize)
-			ec_msg->indata[i] = ptr[i + 2];
+			ec_msg->data[i] = ptr[i + 2];
 	}
 	sum &= 0xff;
 
@@ -315,6 +602,7 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 
 	ret = len;
 exit:
+	kfree(rx_buf);
 	if (ec_msg->command == EC_CMD_REBOOT_EC)
 		msleep(EC_REBOOT_DELAY_MS);
 
@@ -327,6 +615,10 @@ static void cros_ec_spi_dt_probe(struct cros_ec_spi *ec_spi, struct device *dev)
 	u32 val;
 	int ret;
 
+	ret = of_property_read_u32(np, "google,cros-ec-spi-pre-delay", &val);
+	if (!ret)
+		ec_spi->start_of_msg_delay = val;
+
 	ret = of_property_read_u32(np, "google,cros-ec-spi-msg-delay", &val);
 	if (!ret)
 		ec_spi->end_of_msg_delay = val;
@@ -361,11 +653,13 @@ static int cros_ec_spi_probe(struct spi_device *spi)
 	ec_dev->priv = ec_spi;
 	ec_dev->irq = spi->irq;
 	ec_dev->cmd_xfer = cros_ec_cmd_xfer_spi;
-	ec_dev->ec_name = ec_spi->spi->modalias;
+	ec_dev->pkt_xfer = cros_ec_pkt_xfer_spi;
 	ec_dev->phys_name = dev_name(&ec_spi->spi->dev);
-	ec_dev->parent = &ec_spi->spi->dev;
-	ec_dev->din_size = EC_MSG_BYTES + EC_MSG_PREAMBLE_COUNT;
-	ec_dev->dout_size = EC_MSG_BYTES;
+	ec_dev->din_size = EC_MSG_PREAMBLE_COUNT +
+			   sizeof(struct ec_host_response) +
+			   sizeof(struct ec_response_get_protocol_info);
+	ec_dev->dout_size = sizeof(struct ec_host_request);
+
 
 	err = cros_ec_register(ec_dev);
 	if (err) {
diff --git a/drivers/mfd/da9052-irq.c b/drivers/mfd/da9052-irq.c
index e65ca194fa98..f4cb4613140b 100644
--- a/drivers/mfd/da9052-irq.c
+++ b/drivers/mfd/da9052-irq.c
@@ -35,7 +35,7 @@
 #define DA9052_IRQ_MASK_POS_7		0x40
 #define DA9052_IRQ_MASK_POS_8		0x80
 
-static struct regmap_irq da9052_irqs[] = {
+static const struct regmap_irq da9052_irqs[] = {
 	[DA9052_IRQ_DCIN] = {
 		.reg_offset = 0,
 		.mask = DA9052_IRQ_MASK_POS_1,
@@ -166,7 +166,7 @@ static struct regmap_irq da9052_irqs[] = {
 	},
 };
 
-static struct regmap_irq_chip da9052_regmap_irq_chip = {
+static const struct regmap_irq_chip da9052_regmap_irq_chip = {
 	.name = "da9052_irq",
 	.status_base = DA9052_EVENT_A_REG,
 	.mask_base = DA9052_IRQ_MASK_A_REG,
diff --git a/drivers/mfd/da9055-core.c b/drivers/mfd/da9055-core.c
index b4d920c1ead1..177e65a12c12 100644
--- a/drivers/mfd/da9055-core.c
+++ b/drivers/mfd/da9055-core.c
@@ -222,7 +222,7 @@ static bool da9055_register_volatile(struct device *dev, unsigned int reg)
 	}
 }
 
-static struct regmap_irq da9055_irqs[] = {
+static const struct regmap_irq da9055_irqs[] = {
 	[DA9055_IRQ_NONKEY] = {
 		.reg_offset = 0,
 		.mask = DA9055_IRQ_NONKEY_MASK,
@@ -245,7 +245,7 @@ static struct regmap_irq da9055_irqs[] = {
 	},
 };
 
-struct regmap_config da9055_regmap_config = {
+const struct regmap_config da9055_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 
@@ -367,7 +367,7 @@ static const struct mfd_cell da9055_devs[] = {
 	},
 };
 
-static struct regmap_irq_chip da9055_regmap_irq_chip = {
+static const struct regmap_irq_chip da9055_regmap_irq_chip = {
 	.name = "da9055_irq",
 	.status_base = DA9055_REG_EVENT_A,
 	.mask_base = DA9055_REG_IRQ_MASK_A,
diff --git a/drivers/mfd/da9063-core.c b/drivers/mfd/da9063-core.c
index facd3610ac77..af841c165787 100644
--- a/drivers/mfd/da9063-core.c
+++ b/drivers/mfd/da9063-core.c
@@ -60,6 +60,7 @@ static struct resource da9063_rtc_resources[] = {
 
 static struct resource da9063_onkey_resources[] = {
 	{
+		.name	= "ONKEY",
 		.start	= DA9063_IRQ_ONKEY,
 		.end	= DA9063_IRQ_ONKEY,
 		.flags	= IORESOURCE_IRQ,
@@ -97,6 +98,7 @@ static const struct mfd_cell da9063_devs[] = {
 		.name		= DA9063_DRVNAME_ONKEY,
 		.num_resources	= ARRAY_SIZE(da9063_onkey_resources),
 		.resources	= da9063_onkey_resources,
+		.of_compatible = "dlg,da9063-onkey",
 	},
 	{
 		.name		= DA9063_DRVNAME_RTC,
@@ -109,12 +111,64 @@ static const struct mfd_cell da9063_devs[] = {
 	},
 };
 
+static int da9063_clear_fault_log(struct da9063 *da9063)
+{
+	int ret = 0;
+	int fault_log = 0;
+
+	ret = regmap_read(da9063->regmap, DA9063_REG_FAULT_LOG, &fault_log);
+	if (ret < 0) {
+		dev_err(da9063->dev, "Cannot read FAULT_LOG.\n");
+		return -EIO;
+	}
+
+	if (fault_log) {
+		if (fault_log & DA9063_TWD_ERROR)
+			dev_dbg(da9063->dev,
+				"Fault log entry detected: DA9063_TWD_ERROR\n");
+		if (fault_log & DA9063_POR)
+			dev_dbg(da9063->dev,
+				"Fault log entry detected: DA9063_POR\n");
+		if (fault_log & DA9063_VDD_FAULT)
+			dev_dbg(da9063->dev,
+				"Fault log entry detected: DA9063_VDD_FAULT\n");
+		if (fault_log & DA9063_VDD_START)
+			dev_dbg(da9063->dev,
+				"Fault log entry detected: DA9063_VDD_START\n");
+		if (fault_log & DA9063_TEMP_CRIT)
+			dev_dbg(da9063->dev,
+				"Fault log entry detected: DA9063_TEMP_CRIT\n");
+		if (fault_log & DA9063_KEY_RESET)
+			dev_dbg(da9063->dev,
+				"Fault log entry detected: DA9063_KEY_RESET\n");
+		if (fault_log & DA9063_NSHUTDOWN)
+			dev_dbg(da9063->dev,
+				"Fault log entry detected: DA9063_NSHUTDOWN\n");
+		if (fault_log & DA9063_WAIT_SHUT)
+			dev_dbg(da9063->dev,
+				"Fault log entry detected: DA9063_WAIT_SHUT\n");
+	}
+
+	ret = regmap_write(da9063->regmap,
+			   DA9063_REG_FAULT_LOG,
+			   fault_log);
+	if (ret < 0)
+		dev_err(da9063->dev,
+			"Cannot reset FAULT_LOG values %d\n", ret);
+
+	return ret;
+}
+
 int da9063_device_init(struct da9063 *da9063, unsigned int irq)
 {
 	struct da9063_pdata *pdata = da9063->dev->platform_data;
 	int model, variant_id, variant_code;
 	int ret;
 
+	ret = da9063_clear_fault_log(da9063);
+	if (ret < 0)
+		dev_err(da9063->dev, "Cannot clear fault log\n");
+
 	if (pdata) {
 		da9063->flags = pdata->flags;
 		da9063->irq_base = pdata->irq_base;
diff --git a/drivers/mfd/da9063-irq.c b/drivers/mfd/da9063-irq.c
index 822922602ce9..eaf1ec9208b2 100644
--- a/drivers/mfd/da9063-irq.c
+++ b/drivers/mfd/da9063-irq.c
@@ -34,7 +34,7 @@ struct da9063_irq_data {
 	u8 mask;
 };
 
-static struct regmap_irq da9063_irqs[] = {
+static const struct regmap_irq da9063_irqs[] = {
 	/* DA9063 event A register */
 	[DA9063_IRQ_ONKEY] = {
 		.reg_offset = DA9063_REG_EVENT_A_OFFSET,
@@ -153,7 +153,7 @@ static struct regmap_irq da9063_irqs[] = {
 	},
 };
 
-static struct regmap_irq_chip da9063_irq_chip = {
+static const struct regmap_irq_chip da9063_irq_chip = {
 	.name = "da9063-irq",
 	.irqs = da9063_irqs,
 	.num_irqs = DA9063_NUM_IRQ,
diff --git a/drivers/mfd/da9150-core.c b/drivers/mfd/da9150-core.c
index 5549817df32e..94b9bbd1a69b 100644
--- a/drivers/mfd/da9150-core.c
+++ b/drivers/mfd/da9150-core.c
@@ -164,7 +164,7 @@ void da9150_bulk_write(struct da9150 *da9150, u16 reg, int count, const u8 *buf)
 }
 EXPORT_SYMBOL_GPL(da9150_bulk_write);
 
-static struct regmap_irq da9150_irqs[] = {
+static const struct regmap_irq da9150_irqs[] = {
 	[DA9150_IRQ_VBUS] = {
 		.reg_offset = 0,
 		.mask = DA9150_E_VBUS_MASK,
@@ -251,7 +251,7 @@ static struct regmap_irq da9150_irqs[] = {
 	},
 };
 
-static struct regmap_irq_chip da9150_regmap_irq_chip = {
+static const struct regmap_irq_chip da9150_regmap_irq_chip = {
 	.name = "da9150_irq",
 	.status_base = DA9150_EVENT_E,
 	.mask_base = DA9150_IRQ_MASK_E,
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index cc1a404328c2..8b14740f9fca 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -2659,7 +2659,7 @@ static int db8500_irq_map(struct irq_domain *d, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops db8500_irq_ops = {
+static const struct irq_domain_ops db8500_irq_ops = {
 	.map    = db8500_irq_map,
 	.xlate  = irq_domain_xlate_twocell,
 };
diff --git a/drivers/mfd/htc-i2cpld.c b/drivers/mfd/htc-i2cpld.c
index ebb9cf19e347..b54baad30164 100644
--- a/drivers/mfd/htc-i2cpld.c
+++ b/drivers/mfd/htc-i2cpld.c
@@ -564,7 +564,8 @@ static int htcpld_core_probe(struct platform_device *pdev)
 		htcpld->chained_irq = res->start;
 
 		/* Setup the chained interrupt handler */
-		flags = IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING;
+		flags = IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING |
+			IRQF_ONESHOT;
 		ret = request_threaded_irq(htcpld->chained_irq,
 					   NULL, htcpld_handler,
 					   flags, pdev->name, htcpld);
diff --git a/drivers/mfd/intel_soc_pmic_core.h b/drivers/mfd/intel_soc_pmic_core.h
index 9498d6719847..ff2464bc172f 100644
--- a/drivers/mfd/intel_soc_pmic_core.h
+++ b/drivers/mfd/intel_soc_pmic_core.h
@@ -24,7 +24,7 @@ struct intel_soc_pmic_config {
 	struct mfd_cell *cell_dev;
 	int n_cell_devs;
 	const struct regmap_config *regmap_config;
-	struct regmap_irq_chip *irq_chip;
+	const struct regmap_irq_chip *irq_chip;
 };
 
 extern struct intel_soc_pmic_config intel_soc_pmic_config_crc;
diff --git a/drivers/mfd/intel_soc_pmic_crc.c b/drivers/mfd/intel_soc_pmic_crc.c
index 4cc1b324e971..7436075e8983 100644
--- a/drivers/mfd/intel_soc_pmic_crc.c
+++ b/drivers/mfd/intel_soc_pmic_crc.c
@@ -143,7 +143,7 @@ static const struct regmap_irq crystal_cove_irqs[] = {
 	},
 };
 
-static struct regmap_irq_chip crystal_cove_irq_chip = {
+static const struct regmap_irq_chip crystal_cove_irq_chip = {
 	.name = "Crystal Cove",
 	.irqs = crystal_cove_irqs,
 	.num_irqs = ARRAY_SIZE(crystal_cove_irqs),
diff --git a/drivers/mfd/janz-cmodio.c b/drivers/mfd/janz-cmodio.c
index 433f823037dd..ec1f46a6be3a 100644
--- a/drivers/mfd/janz-cmodio.c
+++ b/drivers/mfd/janz-cmodio.c
@@ -267,6 +267,10 @@ static void cmodio_pci_remove(struct pci_dev *dev)
 static const struct pci_device_id cmodio_pci_ids[] = {
 	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9030, PCI_VENDOR_ID_JANZ, 0x0101 },
 	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050, PCI_VENDOR_ID_JANZ, 0x0100 },
+	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9030, PCI_VENDOR_ID_JANZ, 0x0201 },
+	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9030, PCI_VENDOR_ID_JANZ, 0x0202 },
+	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050, PCI_VENDOR_ID_JANZ, 0x0201 },
+	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050, PCI_VENDOR_ID_JANZ, 0x0202 },
 	{ 0, }
 };
 MODULE_DEVICE_TABLE(pci, cmodio_pci_ids);
diff --git a/drivers/mfd/lp8788-irq.c b/drivers/mfd/lp8788-irq.c
index 23982dbf014d..a87f2b548f71 100644
--- a/drivers/mfd/lp8788-irq.c
+++ b/drivers/mfd/lp8788-irq.c
@@ -151,7 +151,7 @@ static int lp8788_irq_map(struct irq_domain *d, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops lp8788_domain_ops = {
+static const struct irq_domain_ops lp8788_domain_ops = {
 	.map = lp8788_irq_map,
 };
 
diff --git a/drivers/mfd/lpc_ich.c b/drivers/mfd/lpc_ich.c
index 12d960a60ec4..8de34398abc0 100644
--- a/drivers/mfd/lpc_ich.c
+++ b/drivers/mfd/lpc_ich.c
@@ -934,8 +934,8 @@ gpe0_done:
 	lpc_ich_enable_gpio_space(dev);
 
 	lpc_ich_finalize_cell(dev, &lpc_ich_cells[LPC_GPIO]);
-	ret = mfd_add_devices(&dev->dev, -1, &lpc_ich_cells[LPC_GPIO],
-			      1, NULL, 0, NULL);
+	ret = mfd_add_devices(&dev->dev, PLATFORM_DEVID_AUTO,
+			      &lpc_ich_cells[LPC_GPIO], 1, NULL, 0, NULL);
 
 gpio_done:
 	if (acpi_conflict)
@@ -1008,8 +1008,8 @@ static int lpc_ich_init_wdt(struct pci_dev *dev)
 	}
 
 	lpc_ich_finalize_cell(dev, &lpc_ich_cells[LPC_WDT]);
-	ret = mfd_add_devices(&dev->dev, -1, &lpc_ich_cells[LPC_WDT],
-			      1, NULL, 0, NULL);
+	ret = mfd_add_devices(&dev->dev, PLATFORM_DEVID_AUTO,
+			      &lpc_ich_cells[LPC_WDT], 1, NULL, 0, NULL);
 
 wdt_done:
 	return ret;
diff --git a/drivers/mfd/max8925-core.c b/drivers/mfd/max8925-core.c
index 97a787ab3d51..8520bd68c1ff 100644
--- a/drivers/mfd/max8925-core.c
+++ b/drivers/mfd/max8925-core.c
@@ -658,7 +658,7 @@ static int max8925_irq_domain_map(struct irq_domain *d, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops max8925_irq_domain_ops = {
+static const struct irq_domain_ops max8925_irq_domain_ops = {
 	.map	= max8925_irq_domain_map,
 	.xlate	= irq_domain_xlate_onetwocell,
 };
diff --git a/drivers/mfd/max8997-irq.c b/drivers/mfd/max8997-irq.c
index 43fa61413e93..d3025be57f39 100644
--- a/drivers/mfd/max8997-irq.c
+++ b/drivers/mfd/max8997-irq.c
@@ -303,7 +303,7 @@ static int max8997_irq_domain_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops max8997_irq_domain_ops = {
+static const struct irq_domain_ops max8997_irq_domain_ops = {
 	.map = max8997_irq_domain_map,
 };
 
diff --git a/drivers/mfd/max8998-irq.c b/drivers/mfd/max8998-irq.c
index c469477eb778..3702056628a8 100644
--- a/drivers/mfd/max8998-irq.c
+++ b/drivers/mfd/max8998-irq.c
@@ -214,7 +214,7 @@ static int max8998_irq_domain_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops max8998_irq_domain_ops = {
+static const struct irq_domain_ops max8998_irq_domain_ops = {
 	.map = max8998_irq_domain_map,
 };
 
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index 25fd7116493a..3f9f4c874d2a 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -163,7 +163,7 @@ int mc13xxx_irq_request(struct mc13xxx *mc13xxx, int irq,
 	int virq = regmap_irq_get_virq(mc13xxx->irq_data, irq);
 
 	return devm_request_threaded_irq(mc13xxx->dev, virq, NULL, handler,
-					 0, name, dev);
+					 IRQF_ONESHOT, name, dev);
 }
 EXPORT_SYMBOL(mc13xxx_irq_request);
 
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index 1aed3b7b8d9b..14fd5cbcf0f2 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -207,9 +207,11 @@ static int mfd_add_device(struct device *parent, int id,
 		}
 
 		if (!cell->ignore_resource_conflicts) {
-			ret = acpi_check_resource_conflict(&res[r]);
-			if (ret)
-				goto fail_alias;
+			if (has_acpi_companion(&pdev->dev)) {
+				ret = acpi_check_resource_conflict(&res[r]);
+				if (ret)
+					goto fail_alias;
+			}
 		}
 	}
 
diff --git a/drivers/mfd/mt6397-core.c b/drivers/mfd/mt6397-core.c
index 09bc7804952a..03929a6c6fc4 100644
--- a/drivers/mfd/mt6397-core.c
+++ b/drivers/mfd/mt6397-core.c
@@ -21,9 +21,27 @@
 #include <linux/mfd/mt6397/core.h>
 #include <linux/mfd/mt6397/registers.h>
 
+#define MT6397_RTC_BASE		0xe000
+#define MT6397_RTC_SIZE		0x3e
+
+static const struct resource mt6397_rtc_resources[] = {
+	{
+		.start = MT6397_RTC_BASE,
+		.end   = MT6397_RTC_BASE + MT6397_RTC_SIZE,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = MT6397_IRQ_RTC,
+		.end   = MT6397_IRQ_RTC,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
 static const struct mfd_cell mt6397_devs[] = {
 	{
 		.name = "mt6397-rtc",
+		.num_resources = ARRAY_SIZE(mt6397_rtc_resources),
+		.resources = mt6397_rtc_resources,
 		.of_compatible = "mediatek,mt6397-rtc",
 	}, {
 		.name = "mt6397-regulator",
@@ -34,6 +52,9 @@ static const struct mfd_cell mt6397_devs[] = {
 	}, {
 		.name = "mt6397-clk",
 		.of_compatible = "mediatek,mt6397-clk",
+	}, {
+		.name = "mt6397-pinctrl",
+		.of_compatible = "mediatek,mt6397-pinctrl",
 	},
 };
 
@@ -130,7 +151,7 @@ static int mt6397_irq_domain_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops mt6397_irq_domain_ops = {
+static const struct irq_domain_ops mt6397_irq_domain_ops = {
 	.map = mt6397_irq_domain_map,
 };
 
diff --git a/drivers/mfd/si476x-i2c.c b/drivers/mfd/si476x-i2c.c
index 7f87c62d91b3..e3deb466628b 100644
--- a/drivers/mfd/si476x-i2c.c
+++ b/drivers/mfd/si476x-i2c.c
@@ -777,7 +777,8 @@ static int si476x_core_probe(struct i2c_client *client,
 		rval = devm_request_threaded_irq(&client->dev,
 						 client->irq, NULL,
 						 si476x_core_interrupt,
-						 IRQF_TRIGGER_FALLING,
+						 IRQF_TRIGGER_FALLING |
+						 IRQF_ONESHOT,
 						 client->name, core);
 		if (rval < 0) {
 			dev_err(&client->dev, "Could not request IRQ %d\n",
diff --git a/drivers/mfd/stmpe.c b/drivers/mfd/stmpe.c
index 2d7fae94c861..18c4d72d1d2a 100644
--- a/drivers/mfd/stmpe.c
+++ b/drivers/mfd/stmpe.c
@@ -989,7 +989,7 @@ static void stmpe_irq_unmap(struct irq_domain *d, unsigned int virq)
 		irq_set_chip_data(virq, NULL);
 }
 
-static struct irq_domain_ops stmpe_irq_ops = {
+static const struct irq_domain_ops stmpe_irq_ops = {
         .map    = stmpe_irq_map,
         .unmap  = stmpe_irq_unmap,
         .xlate  = irq_domain_xlate_twocell,
diff --git a/drivers/mfd/tc3589x.c b/drivers/mfd/tc3589x.c
index cf356395c9e9..96d420dfc15d 100644
--- a/drivers/mfd/tc3589x.c
+++ b/drivers/mfd/tc3589x.c
@@ -233,7 +233,7 @@ static void tc3589x_irq_unmap(struct irq_domain *d, unsigned int virq)
 	irq_set_chip_data(virq, NULL);
 }
 
-static struct irq_domain_ops tc3589x_irq_ops = {
+static const struct irq_domain_ops tc3589x_irq_ops = {
 	.map    = tc3589x_irq_map,
 	.unmap  = tc3589x_irq_unmap,
 	.xlate  = irq_domain_xlate_onecell,
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index 8e1dbc469580..e0a2583916ce 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -311,7 +311,7 @@ static int tps6586x_irq_map(struct irq_domain *h, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops tps6586x_domain_ops = {
+static const struct irq_domain_ops tps6586x_domain_ops = {
 	.map    = tps6586x_irq_map,
 	.xlate  = irq_domain_xlate_twocell,
 };
diff --git a/drivers/mfd/twl4030-irq.c b/drivers/mfd/twl4030-irq.c
index 1b772ef761cb..a3fa7f4f1fb4 100644
--- a/drivers/mfd/twl4030-irq.c
+++ b/drivers/mfd/twl4030-irq.c
@@ -674,7 +674,7 @@ int twl4030_sih_setup(struct device *dev, int module, int irq_base)
 	irq_set_handler_data(irq, agent);
 	agent->irq_name = kasprintf(GFP_KERNEL, "twl4030_%s", sih->name);
 	status = request_threaded_irq(irq, NULL, handle_twl4030_sih,
-				      IRQF_EARLY_RESUME,
+				      IRQF_EARLY_RESUME | IRQF_ONESHOT,
 				      agent->irq_name ?: sih->name, NULL);
 
 	dev_info(dev, "%s (irq %d) chaining IRQs %d..%d\n", sih->name,
diff --git a/drivers/mfd/twl4030-power.c b/drivers/mfd/twl4030-power.c
index f440aed61305..04b539850e72 100644
--- a/drivers/mfd/twl4030-power.c
+++ b/drivers/mfd/twl4030-power.c
@@ -264,7 +264,9 @@ out:
 	return err;
 }
 
-static int twl4030_config_wakeup12_sequence(u8 address)
+static int
+twl4030_config_wakeup12_sequence(const struct twl4030_power_data *pdata,
+				 u8 address)
 {
 	int err = 0;
 	u8 data;
@@ -293,13 +295,14 @@ static int twl4030_config_wakeup12_sequence(u8 address)
 	if (err)
 		goto out;
 
-	if (machine_is_omap_3430sdp() || machine_is_omap_ldp()) {
+	if (pdata->ac_charger_quirk || machine_is_omap_3430sdp() ||
+	    machine_is_omap_ldp()) {
 		/* Disabling AC charger effect on sleep-active transitions */
 		err = twl_i2c_read_u8(TWL_MODULE_PM_MASTER, &data,
 				      R_CFG_P1_TRANSITION);
 		if (err)
 			goto out;
-		data &= ~(1<<1);
+		data &= ~STARTON_CHG;
 		err = twl_i2c_write_u8(TWL_MODULE_PM_MASTER, data,
 				       R_CFG_P1_TRANSITION);
 		if (err)
@@ -459,8 +462,9 @@ static int twl4030_configure_resource(struct twl4030_resconfig *rconfig)
 	return 0;
 }
 
-static int load_twl4030_script(struct twl4030_script *tscript,
-	       u8 address)
+static int load_twl4030_script(const struct twl4030_power_data *pdata,
+			       struct twl4030_script *tscript,
+			       u8 address)
 {
 	int err;
 	static int order;
@@ -487,7 +491,7 @@ static int load_twl4030_script(struct twl4030_script *tscript,
 		if (err)
 			goto out;
 
-		err = twl4030_config_wakeup12_sequence(address);
+		err = twl4030_config_wakeup12_sequence(pdata, address);
 		if (err)
 			goto out;
 		order = 1;
@@ -567,7 +571,7 @@ twl4030_power_configure_scripts(const struct twl4030_power_data *pdata)
 	u8 address = twl4030_start_script_address;
 
 	for (i = 0; i < pdata->num; i++) {
-		err = load_twl4030_script(pdata->scripts[i], address);
+		err = load_twl4030_script(pdata, pdata->scripts[i], address);
 		if (err)
 			return err;
 		address += pdata->scripts[i]->size;
@@ -829,6 +833,21 @@ static struct twl4030_power_data osc_off_idle = {
 	.board_config		= osc_off_rconfig,
 };
 
+static struct twl4030_power_data omap3_idle_ac_quirk = {
+	.scripts		= omap3_idle_scripts,
+	.num			= ARRAY_SIZE(omap3_idle_scripts),
+	.resource_config	= omap3_idle_rconfig,
+	.ac_charger_quirk	= true,
+};
+
+static struct twl4030_power_data omap3_idle_ac_quirk_osc_off = {
+	.scripts		= omap3_idle_scripts,
+	.num			= ARRAY_SIZE(omap3_idle_scripts),
+	.resource_config	= omap3_idle_rconfig,
+	.board_config		= osc_off_rconfig,
+	.ac_charger_quirk	= true,
+};
+
 static const struct of_device_id twl4030_power_of_match[] = {
 	{
 		.compatible = "ti,twl4030-power",
@@ -845,6 +864,18 @@ static const struct of_device_id twl4030_power_of_match[] = {
 		.compatible = "ti,twl4030-power-idle-osc-off",
 		.data = &osc_off_idle,
 	},
+	{
+		.compatible = "ti,twl4030-power-omap3-sdp",
+		.data = &omap3_idle_ac_quirk,
+	},
+	{
+		.compatible = "ti,twl4030-power-omap3-ldp",
+		.data = &omap3_idle_ac_quirk_osc_off,
+	},
+	{
+		.compatible = "ti,twl4030-power-omap3-evm",
+		.data = &omap3_idle_ac_quirk,
+	},
 	{ },
 };
 MODULE_DEVICE_TABLE(of, twl4030_power_of_match);
diff --git a/drivers/mfd/twl6030-irq.c b/drivers/mfd/twl6030-irq.c
index 2807e1a95663..20fb58179ada 100644
--- a/drivers/mfd/twl6030-irq.c
+++ b/drivers/mfd/twl6030-irq.c
@@ -376,7 +376,7 @@ static void twl6030_irq_unmap(struct irq_domain *d, unsigned int virq)
 	irq_set_chip_data(virq, NULL);
 }
 
-static struct irq_domain_ops twl6030_irq_domain_ops = {
+static const struct irq_domain_ops twl6030_irq_domain_ops = {
 	.map	= twl6030_irq_map,
 	.unmap	= twl6030_irq_unmap,
 	.xlate	= irq_domain_xlate_onetwocell,
diff --git a/drivers/mfd/ucb1x00-core.c b/drivers/mfd/ucb1x00-core.c
index 58ea9fdd3a15..3591550598ad 100644
--- a/drivers/mfd/ucb1x00-core.c
+++ b/drivers/mfd/ucb1x00-core.c
@@ -566,8 +566,7 @@ static int ucb1x00_probe(struct mcp *mcp)
 	}
 
 	irq_set_irq_type(ucb->irq, IRQ_TYPE_EDGE_RISING);
-	irq_set_handler_data(ucb->irq, ucb);
-	irq_set_chained_handler(ucb->irq, ucb1x00_irq);
+	irq_set_chained_handler_and_data(ucb->irq, ucb1x00_irq, ucb);
 
 	if (pdata && pdata->gpio_base) {
 		ucb->gpio.label = dev_name(&ucb->dev);
diff --git a/drivers/mfd/wm831x-auxadc.c b/drivers/mfd/wm831x-auxadc.c
index 6ee3018d8653..fd789d2eb0f5 100644
--- a/drivers/mfd/wm831x-auxadc.c
+++ b/drivers/mfd/wm831x-auxadc.c
@@ -285,7 +285,8 @@ void wm831x_auxadc_init(struct wm831x *wm831x)
 
 		ret = request_threaded_irq(wm831x_irq(wm831x,
 						      WM831X_IRQ_AUXADC_DATA),
-					   NULL, wm831x_auxadc_irq, 0,
+					   NULL, wm831x_auxadc_irq,
+					   IRQF_ONESHOT,
 					   "auxadc", wm831x);
 		if (ret < 0) {
 			dev_err(wm831x->dev, "AUXADC IRQ request failed: %d\n",
diff --git a/drivers/mfd/wm831x-irq.c b/drivers/mfd/wm831x-irq.c
index 64e512eadf17..3da81263c764 100644
--- a/drivers/mfd/wm831x-irq.c
+++ b/drivers/mfd/wm831x-irq.c
@@ -564,7 +564,7 @@ static int wm831x_irq_map(struct irq_domain *h, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops wm831x_irq_domain_ops = {
+static const struct irq_domain_ops wm831x_irq_domain_ops = {
 	.map	= wm831x_irq_map,
 	.xlate	= irq_domain_xlate_twocell,
 };
diff --git a/drivers/mfd/wm8350-core.c b/drivers/mfd/wm8350-core.c
index f5124a8acad8..8a07c5634aee 100644
--- a/drivers/mfd/wm8350-core.c
+++ b/drivers/mfd/wm8350-core.c
@@ -404,7 +404,8 @@ int wm8350_device_init(struct wm8350 *wm8350, int irq,
 	if (wm8350->irq_base) {
 		ret = request_threaded_irq(wm8350->irq_base +
 					   WM8350_IRQ_AUXADC_DATARDY,
-					   NULL, wm8350_auxadc_irq, 0,
+					   NULL, wm8350_auxadc_irq,
+					   IRQF_ONESHOT,
 					   "auxadc", wm8350);
 		if (ret < 0)
 			dev_warn(wm8350->dev,
diff --git a/drivers/mfd/wm8994-irq.c b/drivers/mfd/wm8994-irq.c
index a14407edbd89..55c380a67686 100644
--- a/drivers/mfd/wm8994-irq.c
+++ b/drivers/mfd/wm8994-irq.c
@@ -28,7 +28,7 @@
 
 #include <linux/delay.h>
 
-static struct regmap_irq wm8994_irqs[] = {
+static const struct regmap_irq wm8994_irqs[] = {
 	[WM8994_IRQ_TEMP_SHUT] = {
 		.reg_offset = 1,
 		.mask = WM8994_TEMP_SHUT_EINT,
@@ -128,7 +128,7 @@ static struct regmap_irq wm8994_irqs[] = {
 	},
 };
 
-static struct regmap_irq_chip wm8994_irq_chip = {
+static const struct regmap_irq_chip wm8994_irq_chip = {
 	.name = "wm8994",
 	.irqs = wm8994_irqs,
 	.num_irqs = ARRAY_SIZE(wm8994_irqs),
@@ -184,7 +184,7 @@ static int wm8994_edge_irq_map(struct irq_domain *h, unsigned int virq,
 	return 0;
 }
 
-static struct irq_domain_ops wm8994_edge_irq_ops = {
+static const struct irq_domain_ops wm8994_edge_irq_ops = {
 	.map	= wm8994_edge_irq_map,
 	.xlate	= irq_domain_xlate_twocell,
 };
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 006242c8bca0..42c38525904b 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -520,7 +520,6 @@ source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
 source "drivers/misc/ti-st/Kconfig"
 source "drivers/misc/lis3lv02d/Kconfig"
-source "drivers/misc/carma/Kconfig"
 source "drivers/misc/altera-stapl/Kconfig"
 source "drivers/misc/mei/Kconfig"
 source "drivers/misc/vmw_vmci/Kconfig"
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 7d5c4cd118c4..d056fb7186fe 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -44,7 +44,6 @@ obj-$(CONFIG_ARM_CHARLCD)	+= arm-charlcd.o
 obj-$(CONFIG_PCH_PHUB)		+= pch_phub.o
 obj-y				+= ti-st/
 obj-y				+= lis3lv02d/
-obj-y				+= carma/
 obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o
 obj-$(CONFIG_ALTERA_STAPL)	+=altera-stapl/
 obj-$(CONFIG_INTEL_MEI)		+= mei/
diff --git a/drivers/misc/altera-stapl/altera.c b/drivers/misc/altera-stapl/altera.c
index bca2630d006f..f53e217e963f 100644
--- a/drivers/misc/altera-stapl/altera.c
+++ b/drivers/misc/altera-stapl/altera.c
@@ -2451,7 +2451,7 @@ int altera_init(struct altera_config *config, const struct firmware *fw)
 
 	astate->config = config;
 	if (!astate->config->jtag_io) {
-		dprintk(KERN_INFO "%s: using byteblaster!\n", __func__);
+		dprintk("%s: using byteblaster!\n", __func__);
 		astate->config->jtag_io = netup_jtag_io_lpt;
 	}
 
diff --git a/drivers/misc/carma/Kconfig b/drivers/misc/carma/Kconfig
deleted file mode 100644
index 295882bfb14e..000000000000
--- a/drivers/misc/carma/Kconfig
+++ /dev/null
@@ -1,15 +0,0 @@
-config CARMA_FPGA
-	tristate "CARMA DATA-FPGA Access Driver"
-	depends on FSL_SOC && PPC_83xx && HAS_DMA && FSL_DMA
-	default n
-	help
-	  Say Y here to include support for communicating with the data
-	  processing FPGAs on the OVRO CARMA board.
-
-config CARMA_FPGA_PROGRAM
-	tristate "CARMA DATA-FPGA Programmer"
-	depends on FSL_SOC && PPC_83xx && HAS_DMA && FSL_DMA
-	default n
-	help
-	  Say Y here to include support for programming the data processing
-	  FPGAs on the OVRO CARMA board.
diff --git a/drivers/misc/carma/Makefile b/drivers/misc/carma/Makefile
deleted file mode 100644
index ff36ac2ce534..000000000000
--- a/drivers/misc/carma/Makefile
+++ /dev/null
@@ -1,2 +0,0 @@
-obj-$(CONFIG_CARMA_FPGA)		+= carma-fpga.o
-obj-$(CONFIG_CARMA_FPGA_PROGRAM)	+= carma-fpga-program.o
diff --git a/drivers/misc/carma/carma-fpga-program.c b/drivers/misc/carma/carma-fpga-program.c
deleted file mode 100644
index 0b1bd85e4ae6..000000000000
--- a/drivers/misc/carma/carma-fpga-program.c
+++ /dev/null
@@ -1,1182 +0,0 @@
-/*
- * CARMA Board DATA-FPGA Programmer
- *
- * Copyright (c) 2009-2011 Ira W. Snyder <iws@ovro.caltech.edu>
- *
- * This program is free software; you can redistribute it and/or modify it
- * under 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/dma-mapping.h>
-#include <linux/of_address.h>
-#include <linux/of_irq.h>
-#include <linux/of_platform.h>
-#include <linux/completion.h>
-#include <linux/miscdevice.h>
-#include <linux/dmaengine.h>
-#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>
-#include <linux/delay.h>
-#include <linux/init.h>
-#include <linux/leds.h>
-#include <linux/slab.h>
-#include <linux/kref.h>
-#include <linux/fs.h>
-#include <linux/io.h>
-
-/* MPC8349EMDS specific get_immrbase() */
-#include <sysdev/fsl_soc.h>
-
-static const char drv_name[] = "carma-fpga-program";
-
-/*
- * Firmware images are always this exact size
- *
- * 12849552 bytes for a CARMA Digitizer Board (EP2S90 FPGAs)
- * 18662880 bytes for a CARMA Correlator Board (EP2S130 FPGAs)
- */
-#define FW_SIZE_EP2S90		12849552
-#define FW_SIZE_EP2S130		18662880
-
-struct fpga_dev {
-	struct miscdevice miscdev;
-
-	/* Reference count */
-	struct kref ref;
-
-	/* Device Registers */
-	struct device *dev;
-	void __iomem *regs;
-	void __iomem *immr;
-
-	/* Freescale DMA Device */
-	struct dma_chan *chan;
-
-	/* Interrupts */
-	int irq, status;
-	struct completion completion;
-
-	/* FPGA Bitfile */
-	struct mutex lock;
-
-	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
- */
-
-/**
- * fpga_drop_firmware_data() - drop the bitfile image from memory
- * @priv: the driver's private data structure
- *
- * LOCKING: must hold priv->lock
- */
-static void fpga_drop_firmware_data(struct fpga_dev *priv)
-{
-	vfree(priv->sglist);
-	vfree(priv->vaddr);
-	priv->buf_allocated = false;
-	priv->bytes = 0;
-}
-
-/*
- * Private Data Reference Count
- */
-
-static void fpga_dev_remove(struct kref *ref)
-{
-	struct fpga_dev *priv = container_of(ref, struct fpga_dev, ref);
-
-	/* free any firmware image that was not programmed */
-	fpga_drop_firmware_data(priv);
-
-	mutex_destroy(&priv->lock);
-	kfree(priv);
-}
-
-/*
- * LED Trigger (could be a seperate module)
- */
-
-/*
- * NOTE: this whole thing does have the problem that whenever the led's are
- * NOTE: first set to use the fpga trigger, they could be in the wrong state
- */
-
-DEFINE_LED_TRIGGER(ledtrig_fpga);
-
-static void ledtrig_fpga_programmed(bool enabled)
-{
-	if (enabled)
-		led_trigger_event(ledtrig_fpga, LED_FULL);
-	else
-		led_trigger_event(ledtrig_fpga, LED_OFF);
-}
-
-/*
- * FPGA Register Helpers
- */
-
-/* Register Definitions */
-#define FPGA_CONFIG_CONTROL		0x40
-#define FPGA_CONFIG_STATUS		0x44
-#define FPGA_CONFIG_FIFO_SIZE		0x48
-#define FPGA_CONFIG_FIFO_USED		0x4C
-#define FPGA_CONFIG_TOTAL_BYTE_COUNT	0x50
-#define FPGA_CONFIG_CUR_BYTE_COUNT	0x54
-
-#define FPGA_FIFO_ADDRESS		0x3000
-
-static int fpga_fifo_size(void __iomem *regs)
-{
-	return ioread32be(regs + FPGA_CONFIG_FIFO_SIZE);
-}
-
-#define CFG_STATUS_ERR_MASK	0xfffe
-
-static int fpga_config_error(void __iomem *regs)
-{
-	return ioread32be(regs + FPGA_CONFIG_STATUS) & CFG_STATUS_ERR_MASK;
-}
-
-static int fpga_fifo_empty(void __iomem *regs)
-{
-	return ioread32be(regs + FPGA_CONFIG_FIFO_USED) == 0;
-}
-
-static void fpga_fifo_write(void __iomem *regs, u32 val)
-{
-	iowrite32be(val, regs + FPGA_FIFO_ADDRESS);
-}
-
-static void fpga_set_byte_count(void __iomem *regs, u32 count)
-{
-	iowrite32be(count, regs + FPGA_CONFIG_TOTAL_BYTE_COUNT);
-}
-
-#define CFG_CTL_ENABLE	(1 << 0)
-#define CFG_CTL_RESET	(1 << 1)
-#define CFG_CTL_DMA	(1 << 2)
-
-static void fpga_programmer_enable(struct fpga_dev *priv, bool dma)
-{
-	u32 val;
-
-	val = (dma) ? (CFG_CTL_ENABLE | CFG_CTL_DMA) : CFG_CTL_ENABLE;
-	iowrite32be(val, priv->regs + FPGA_CONFIG_CONTROL);
-}
-
-static void fpga_programmer_disable(struct fpga_dev *priv)
-{
-	iowrite32be(0x0, priv->regs + FPGA_CONFIG_CONTROL);
-}
-
-static void fpga_dump_registers(struct fpga_dev *priv)
-{
-	u32 control, status, size, used, total, curr;
-
-	/* good status: do nothing */
-	if (priv->status == 0)
-		return;
-
-	/* Dump all status registers */
-	control = ioread32be(priv->regs + FPGA_CONFIG_CONTROL);
-	status = ioread32be(priv->regs + FPGA_CONFIG_STATUS);
-	size = ioread32be(priv->regs + FPGA_CONFIG_FIFO_SIZE);
-	used = ioread32be(priv->regs + FPGA_CONFIG_FIFO_USED);
-	total = ioread32be(priv->regs + FPGA_CONFIG_TOTAL_BYTE_COUNT);
-	curr = ioread32be(priv->regs + FPGA_CONFIG_CUR_BYTE_COUNT);
-
-	dev_err(priv->dev, "Configuration failed, dumping status registers\n");
-	dev_err(priv->dev, "Control:    0x%.8x\n", control);
-	dev_err(priv->dev, "Status:     0x%.8x\n", status);
-	dev_err(priv->dev, "FIFO Size:  0x%.8x\n", size);
-	dev_err(priv->dev, "FIFO Used:  0x%.8x\n", used);
-	dev_err(priv->dev, "FIFO Total: 0x%.8x\n", total);
-	dev_err(priv->dev, "FIFO Curr:  0x%.8x\n", curr);
-}
-
-/*
- * FPGA Power Supply Code
- */
-
-#define CTL_PWR_CONTROL		0x2006
-#define CTL_PWR_STATUS		0x200A
-#define CTL_PWR_FAIL		0x200B
-
-#define PWR_CONTROL_ENABLE	0x01
-
-#define PWR_STATUS_ERROR_MASK	0x10
-#define PWR_STATUS_GOOD		0x0f
-
-/*
- * Determine if the FPGA power is good for all supplies
- */
-static bool fpga_power_good(struct fpga_dev *priv)
-{
-	u8 val;
-
-	val = ioread8(priv->regs + CTL_PWR_STATUS);
-	if (val & PWR_STATUS_ERROR_MASK)
-		return false;
-
-	return val == PWR_STATUS_GOOD;
-}
-
-/*
- * Disable the FPGA power supplies
- */
-static void fpga_disable_power_supplies(struct fpga_dev *priv)
-{
-	unsigned long start;
-	u8 val;
-
-	iowrite8(0x0, priv->regs + CTL_PWR_CONTROL);
-
-	/*
-	 * Wait 500ms for the power rails to discharge
-	 *
-	 * Without this delay, the CTL-CPLD state machine can get into a
-	 * state where it is waiting for the power-goods to assert, but they
-	 * never do. This only happens when enabling and disabling the
-	 * power sequencer very rapidly.
-	 *
-	 * The loop below will also wait for the power goods to de-assert,
-	 * but testing has shown that they are always disabled by the time
-	 * the sleep completes. However, omitting the sleep and only waiting
-	 * for the power-goods to de-assert was not sufficient to ensure
-	 * that the power sequencer would not wedge itself.
-	 */
-	msleep(500);
-
-	start = jiffies;
-	while (time_before(jiffies, start + HZ)) {
-		val = ioread8(priv->regs + CTL_PWR_STATUS);
-		if (!(val & PWR_STATUS_GOOD))
-			break;
-
-		usleep_range(5000, 10000);
-	}
-
-	val = ioread8(priv->regs + CTL_PWR_STATUS);
-	if (val & PWR_STATUS_GOOD) {
-		dev_err(priv->dev, "power disable failed: "
-				   "power goods: status 0x%.2x\n", val);
-	}
-
-	if (val & PWR_STATUS_ERROR_MASK) {
-		dev_err(priv->dev, "power disable failed: "
-				   "alarm bit set: status 0x%.2x\n", val);
-	}
-}
-
-/**
- * fpga_enable_power_supplies() - enable the DATA-FPGA power supplies
- * @priv: the driver's private data structure
- *
- * Enable the DATA-FPGA power supplies, waiting up to 1 second for
- * them to enable successfully.
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static int fpga_enable_power_supplies(struct fpga_dev *priv)
-{
-	unsigned long start = jiffies;
-
-	if (fpga_power_good(priv)) {
-		dev_dbg(priv->dev, "power was already good\n");
-		return 0;
-	}
-
-	iowrite8(PWR_CONTROL_ENABLE, priv->regs + CTL_PWR_CONTROL);
-	while (time_before(jiffies, start + HZ)) {
-		if (fpga_power_good(priv))
-			return 0;
-
-		usleep_range(5000, 10000);
-	}
-
-	return fpga_power_good(priv) ? 0 : -ETIMEDOUT;
-}
-
-/*
- * Determine if the FPGA power supplies are all enabled
- */
-static bool fpga_power_enabled(struct fpga_dev *priv)
-{
-	u8 val;
-
-	val = ioread8(priv->regs + CTL_PWR_CONTROL);
-	if (val & PWR_CONTROL_ENABLE)
-		return true;
-
-	return false;
-}
-
-/*
- * Determine if the FPGA's are programmed and running correctly
- */
-static bool fpga_running(struct fpga_dev *priv)
-{
-	if (!fpga_power_good(priv))
-		return false;
-
-	/* Check the config done bit */
-	return ioread32be(priv->regs + FPGA_CONFIG_STATUS) & (1 << 18);
-}
-
-/*
- * FPGA Programming Code
- */
-
-/**
- * fpga_program_block() - put a block of data into the programmer's FIFO
- * @priv: the driver's private data structure
- * @buf: the data to program
- * @count: the length of data to program (must be a multiple of 4 bytes)
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static int fpga_program_block(struct fpga_dev *priv, void *buf, size_t count)
-{
-	u32 *data = buf;
-	int size = fpga_fifo_size(priv->regs);
-	int i, len;
-	unsigned long timeout;
-
-	/* enforce correct data length for the FIFO */
-	BUG_ON(count % 4 != 0);
-
-	while (count > 0) {
-
-		/* Get the size of the block to write (maximum is FIFO_SIZE) */
-		len = min_t(size_t, count, size);
-		timeout = jiffies + HZ / 4;
-
-		/* Write the block */
-		for (i = 0; i < len / 4; i++)
-			fpga_fifo_write(priv->regs, data[i]);
-
-		/* Update the amounts left */
-		count -= len;
-		data += len / 4;
-
-		/* Wait for the fifo to empty */
-		while (true) {
-
-			if (fpga_fifo_empty(priv->regs)) {
-				break;
-			} else {
-				dev_dbg(priv->dev, "Fifo not empty\n");
-				cpu_relax();
-			}
-
-			if (fpga_config_error(priv->regs)) {
-				dev_err(priv->dev, "Error detected\n");
-				return -EIO;
-			}
-
-			if (time_after(jiffies, timeout)) {
-				dev_err(priv->dev, "Fifo drain timeout\n");
-				return -ETIMEDOUT;
-			}
-
-			usleep_range(5000, 10000);
-		}
-	}
-
-	return 0;
-}
-
-/**
- * fpga_program_cpu() - program the DATA-FPGA's using the CPU
- * @priv: the driver's private data structure
- *
- * This is useful when the DMA programming method fails. It is possible to
- * wedge the Freescale DMA controller such that the DMA programming method
- * always fails. This method has always succeeded.
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static noinline int fpga_program_cpu(struct fpga_dev *priv)
-{
-	int ret;
-	unsigned long timeout;
-
-	/* Disable the programmer */
-	fpga_programmer_disable(priv);
-
-	/* Set the total byte count */
-	fpga_set_byte_count(priv->regs, priv->bytes);
-	dev_dbg(priv->dev, "total byte count %u bytes\n", priv->bytes);
-
-	/* Enable the controller for programming */
-	fpga_programmer_enable(priv, false);
-	dev_dbg(priv->dev, "enabled the controller\n");
-
-	/* Write each chunk of the FPGA bitfile to FPGA programmer */
-	ret = fpga_program_block(priv, priv->vaddr, priv->bytes);
-	if (ret)
-		goto out_disable_controller;
-
-	/* Wait for the interrupt handler to signal that programming finished */
-	timeout = wait_for_completion_timeout(&priv->completion, 2 * HZ);
-	if (!timeout) {
-		dev_err(priv->dev, "Timed out waiting for completion\n");
-		ret = -ETIMEDOUT;
-		goto out_disable_controller;
-	}
-
-	/* Retrieve the status from the interrupt handler */
-	ret = priv->status;
-
-out_disable_controller:
-	fpga_programmer_disable(priv);
-	return ret;
-}
-
-#define FIFO_DMA_ADDRESS	0xf0003000
-#define FIFO_MAX_LEN		4096
-
-/**
- * fpga_program_dma() - program the DATA-FPGA's using the DMA engine
- * @priv: the driver's private data structure
- *
- * Program the DATA-FPGA's using the Freescale DMA engine. This requires that
- * the engine is programmed such that the hardware DMA request lines can
- * control the entire DMA transaction. The system controller FPGA then
- * completely offloads the programming from the CPU.
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static noinline int fpga_program_dma(struct fpga_dev *priv)
-{
-	struct dma_chan *chan = priv->chan;
-	struct dma_async_tx_descriptor *tx;
-	size_t num_pages, len, avail = 0;
-	struct dma_slave_config config;
-	struct scatterlist *sg;
-	struct sg_table table;
-	dma_cookie_t cookie;
-	int ret, i;
-	unsigned long timeout;
-
-	/* Disable the programmer */
-	fpga_programmer_disable(priv);
-
-	/* Allocate a scatterlist for the DMA destination */
-	num_pages = DIV_ROUND_UP(priv->bytes, FIFO_MAX_LEN);
-	ret = sg_alloc_table(&table, num_pages, GFP_KERNEL);
-	if (ret) {
-		dev_err(priv->dev, "Unable to allocate dst scatterlist\n");
-		ret = -ENOMEM;
-		goto out_return;
-	}
-
-	/*
-	 * This is an ugly hack
-	 *
-	 * We fill in a scatterlist as if it were mapped for DMA. This is
-	 * necessary because there exists no better structure for this
-	 * inside the kernel code.
-	 *
-	 * As an added bonus, we can use the DMAEngine API for all of this,
-	 * rather than inventing another extremely similar API.
-	 */
-	avail = priv->bytes;
-	for_each_sg(table.sgl, sg, num_pages, i) {
-		len = min_t(size_t, avail, FIFO_MAX_LEN);
-		sg_dma_address(sg) = FIFO_DMA_ADDRESS;
-		sg_dma_len(sg) = len;
-
-		avail -= len;
-	}
-
-	/* Map the buffer for DMA */
-	ret = fpga_dma_map(priv);
-	if (ret) {
-		dev_err(priv->dev, "Unable to map buffer for DMA\n");
-		goto out_free_table;
-	}
-
-	/*
-	 * Configure the DMA channel to transfer FIFO_SIZE / 2 bytes per
-	 * transaction, and then put it under external control
-	 */
-	memset(&config, 0, sizeof(config));
-	config.direction = DMA_MEM_TO_DEV;
-	config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
-	config.dst_maxburst = fpga_fifo_size(priv->regs) / 2 / 4;
-	ret = dmaengine_slave_config(chan, &config);
-	if (ret) {
-		dev_err(priv->dev, "DMA slave configuration failed\n");
-		goto out_dma_unmap;
-	}
-
-	ret = fsl_dma_external_start(chan, 1);
-	if (ret) {
-		dev_err(priv->dev, "DMA external control setup failed\n");
-		goto out_dma_unmap;
-	}
-
-	/* setup and submit the DMA transaction */
-
-	tx = dmaengine_prep_dma_sg(chan, table.sgl, num_pages,
-			priv->sglist, priv->sglen, 0);
-	if (!tx) {
-		dev_err(priv->dev, "Unable to prep DMA transaction\n");
-		ret = -ENOMEM;
-		goto out_dma_unmap;
-	}
-
-	cookie = tx->tx_submit(tx);
-	if (dma_submit_error(cookie)) {
-		dev_err(priv->dev, "Unable to submit DMA transaction\n");
-		ret = -ENOMEM;
-		goto out_dma_unmap;
-	}
-
-	dma_async_issue_pending(chan);
-
-	/* Set the total byte count */
-	fpga_set_byte_count(priv->regs, priv->bytes);
-	dev_dbg(priv->dev, "total byte count %u bytes\n", priv->bytes);
-
-	/* Enable the controller for DMA programming */
-	fpga_programmer_enable(priv, true);
-	dev_dbg(priv->dev, "enabled the controller\n");
-
-	/* Wait for the interrupt handler to signal that programming finished */
-	timeout = wait_for_completion_timeout(&priv->completion, 2 * HZ);
-	if (!timeout) {
-		dev_err(priv->dev, "Timed out waiting for completion\n");
-		ret = -ETIMEDOUT;
-		goto out_disable_controller;
-	}
-
-	/* Retrieve the status from the interrupt handler */
-	ret = priv->status;
-
-out_disable_controller:
-	fpga_programmer_disable(priv);
-out_dma_unmap:
-	fpga_dma_unmap(priv);
-out_free_table:
-	sg_free_table(&table);
-out_return:
-	return ret;
-}
-
-/*
- * Interrupt Handling
- */
-
-static irqreturn_t fpga_irq(int irq, void *dev_id)
-{
-	struct fpga_dev *priv = dev_id;
-
-	/* Save the status */
-	priv->status = fpga_config_error(priv->regs) ? -EIO : 0;
-	dev_dbg(priv->dev, "INTERRUPT status %d\n", priv->status);
-	fpga_dump_registers(priv);
-
-	/* Disabling the programmer clears the interrupt */
-	fpga_programmer_disable(priv);
-
-	/* Notify any waiters */
-	complete(&priv->completion);
-
-	return IRQ_HANDLED;
-}
-
-/*
- * SYSFS Helpers
- */
-
-/**
- * fpga_do_stop() - deconfigure (reset) the DATA-FPGA's
- * @priv: the driver's private data structure
- *
- * LOCKING: must hold priv->lock
- */
-static int fpga_do_stop(struct fpga_dev *priv)
-{
-	u32 val;
-
-	/* Set the led to unprogrammed */
-	ledtrig_fpga_programmed(false);
-
-	/* Pulse the config line to reset the FPGA's */
-	val = CFG_CTL_ENABLE | CFG_CTL_RESET;
-	iowrite32be(val, priv->regs + FPGA_CONFIG_CONTROL);
-	iowrite32be(0x0, priv->regs + FPGA_CONFIG_CONTROL);
-
-	return 0;
-}
-
-static noinline int fpga_do_program(struct fpga_dev *priv)
-{
-	int ret;
-
-	if (priv->bytes != priv->fw_size) {
-		dev_err(priv->dev, "Incorrect bitfile size: got %zu bytes, "
-				   "should be %zu bytes\n",
-				   priv->bytes, priv->fw_size);
-		return -EINVAL;
-	}
-
-	if (!fpga_power_enabled(priv)) {
-		dev_err(priv->dev, "Power not enabled\n");
-		return -EINVAL;
-	}
-
-	if (!fpga_power_good(priv)) {
-		dev_err(priv->dev, "Power not good\n");
-		return -EINVAL;
-	}
-
-	/* Set the LED to unprogrammed */
-	ledtrig_fpga_programmed(false);
-
-	/* Try to program the FPGA's using DMA */
-	ret = fpga_program_dma(priv);
-
-	/* If DMA failed or doesn't exist, try with CPU */
-	if (ret) {
-		dev_warn(priv->dev, "Falling back to CPU programming\n");
-		ret = fpga_program_cpu(priv);
-	}
-
-	if (ret) {
-		dev_err(priv->dev, "Unable to program FPGA's\n");
-		return ret;
-	}
-
-	/* Drop the firmware bitfile from memory */
-	fpga_drop_firmware_data(priv);
-
-	dev_dbg(priv->dev, "FPGA programming successful\n");
-	ledtrig_fpga_programmed(true);
-
-	return 0;
-}
-
-/*
- * File Operations
- */
-
-static int fpga_open(struct inode *inode, struct file *filp)
-{
-	/*
-	 * The miscdevice layer puts our struct miscdevice into the
-	 * filp->private_data field. We use this to find our private
-	 * data and then overwrite it with our own private structure.
-	 */
-	struct fpga_dev *priv = container_of(filp->private_data,
-					     struct fpga_dev, miscdev);
-	unsigned int nr_pages;
-	int ret;
-
-	/* We only allow one process at a time */
-	ret = mutex_lock_interruptible(&priv->lock);
-	if (ret)
-		return ret;
-
-	filp->private_data = priv;
-	kref_get(&priv->ref);
-
-	/* Truncation: drop any existing data */
-	if (filp->f_flags & O_TRUNC)
-		priv->bytes = 0;
-
-	/* Check if we have already allocated a buffer */
-	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 = fpga_dma_init(priv, nr_pages);
-	if (ret) {
-		dev_err(priv->dev, "unable to allocate data buffer\n");
-		mutex_unlock(&priv->lock);
-		kref_put(&priv->ref, fpga_dev_remove);
-		return ret;
-	}
-
-	priv->buf_allocated = true;
-	return 0;
-}
-
-static int fpga_release(struct inode *inode, struct file *filp)
-{
-	struct fpga_dev *priv = filp->private_data;
-
-	mutex_unlock(&priv->lock);
-	kref_put(&priv->ref, fpga_dev_remove);
-	return 0;
-}
-
-static ssize_t fpga_write(struct file *filp, const char __user *buf,
-			  size_t count, loff_t *f_pos)
-{
-	struct fpga_dev *priv = filp->private_data;
-
-	/* FPGA bitfiles have an exact size: disallow anything else */
-	if (priv->bytes >= priv->fw_size)
-		return -ENOSPC;
-
-	count = min_t(size_t, priv->fw_size - priv->bytes, count);
-	if (copy_from_user(priv->vaddr + priv->bytes, buf, count))
-		return -EFAULT;
-
-	priv->bytes += count;
-	return count;
-}
-
-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, 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;
-
-	/* only read-only opens are allowed to seek */
-	if ((filp->f_flags & O_ACCMODE) != O_RDONLY)
-		return -EINVAL;
-
-	return fixed_size_llseek(filp, offset, origin, priv->fw_size);
-}
-
-static const struct file_operations fpga_fops = {
-	.open		= fpga_open,
-	.release	= fpga_release,
-	.write		= fpga_write,
-	.read		= fpga_read,
-	.llseek		= fpga_llseek,
-};
-
-/*
- * Device Attributes
- */
-
-static ssize_t pfail_show(struct device *dev, struct device_attribute *attr,
-			  char *buf)
-{
-	struct fpga_dev *priv = dev_get_drvdata(dev);
-	u8 val;
-
-	val = ioread8(priv->regs + CTL_PWR_FAIL);
-	return snprintf(buf, PAGE_SIZE, "0x%.2x\n", val);
-}
-
-static ssize_t pgood_show(struct device *dev, struct device_attribute *attr,
-			  char *buf)
-{
-	struct fpga_dev *priv = dev_get_drvdata(dev);
-	return snprintf(buf, PAGE_SIZE, "%d\n", fpga_power_good(priv));
-}
-
-static ssize_t penable_show(struct device *dev, struct device_attribute *attr,
-			    char *buf)
-{
-	struct fpga_dev *priv = dev_get_drvdata(dev);
-	return snprintf(buf, PAGE_SIZE, "%d\n", fpga_power_enabled(priv));
-}
-
-static ssize_t penable_store(struct device *dev, struct device_attribute *attr,
-			     const char *buf, size_t count)
-{
-	struct fpga_dev *priv = dev_get_drvdata(dev);
-	unsigned long val;
-	int ret;
-
-	ret = kstrtoul(buf, 0, &val);
-	if (ret)
-		return ret;
-
-	if (val) {
-		ret = fpga_enable_power_supplies(priv);
-		if (ret)
-			return ret;
-	} else {
-		fpga_do_stop(priv);
-		fpga_disable_power_supplies(priv);
-	}
-
-	return count;
-}
-
-static ssize_t program_show(struct device *dev, struct device_attribute *attr,
-			    char *buf)
-{
-	struct fpga_dev *priv = dev_get_drvdata(dev);
-	return snprintf(buf, PAGE_SIZE, "%d\n", fpga_running(priv));
-}
-
-static ssize_t program_store(struct device *dev, struct device_attribute *attr,
-			     const char *buf, size_t count)
-{
-	struct fpga_dev *priv = dev_get_drvdata(dev);
-	unsigned long val;
-	int ret;
-
-	ret = kstrtoul(buf, 0, &val);
-	if (ret)
-		return ret;
-
-	/* We can't have an image writer and be programming simultaneously */
-	if (mutex_lock_interruptible(&priv->lock))
-		return -ERESTARTSYS;
-
-	/* Program or Reset the FPGA's */
-	ret = val ? fpga_do_program(priv) : fpga_do_stop(priv);
-	if (ret)
-		goto out_unlock;
-
-	/* Success */
-	ret = count;
-
-out_unlock:
-	mutex_unlock(&priv->lock);
-	return ret;
-}
-
-static DEVICE_ATTR(power_fail, S_IRUGO, pfail_show, NULL);
-static DEVICE_ATTR(power_good, S_IRUGO, pgood_show, NULL);
-static DEVICE_ATTR(power_enable, S_IRUGO | S_IWUSR,
-		   penable_show, penable_store);
-
-static DEVICE_ATTR(program, S_IRUGO | S_IWUSR,
-		   program_show, program_store);
-
-static struct attribute *fpga_attributes[] = {
-	&dev_attr_power_fail.attr,
-	&dev_attr_power_good.attr,
-	&dev_attr_power_enable.attr,
-	&dev_attr_program.attr,
-	NULL,
-};
-
-static const struct attribute_group fpga_attr_group = {
-	.attrs = fpga_attributes,
-};
-
-/*
- * OpenFirmware Device Subsystem
- */
-
-#define SYS_REG_VERSION		0x00
-#define SYS_REG_GEOGRAPHIC	0x10
-
-static bool dma_filter(struct dma_chan *chan, void *data)
-{
-	/*
-	 * DMA Channel #0 is the only acceptable device
-	 *
-	 * This probably won't survive an unload/load cycle of the Freescale
-	 * DMAEngine driver, but that won't be a problem
-	 */
-	return chan->chan_id == 0 && chan->device->dev_id == 0;
-}
-
-static int fpga_of_remove(struct platform_device *op)
-{
-	struct fpga_dev *priv = platform_get_drvdata(op);
-	struct device *this_device = priv->miscdev.this_device;
-
-	sysfs_remove_group(&this_device->kobj, &fpga_attr_group);
-	misc_deregister(&priv->miscdev);
-
-	free_irq(priv->irq, priv);
-	irq_dispose_mapping(priv->irq);
-
-	/* make sure the power supplies are off */
-	fpga_disable_power_supplies(priv);
-
-	/* unmap registers */
-	iounmap(priv->immr);
-	iounmap(priv->regs);
-
-	dma_release_channel(priv->chan);
-
-	/* drop our reference to the private data structure */
-	kref_put(&priv->ref, fpga_dev_remove);
-	return 0;
-}
-
-/* CTL-CPLD Version Register */
-#define CTL_CPLD_VERSION	0x2000
-
-static int fpga_of_probe(struct platform_device *op)
-{
-	struct device_node *of_node = op->dev.of_node;
-	struct device *this_device;
-	struct fpga_dev *priv;
-	dma_cap_mask_t mask;
-	u32 ver;
-	int ret;
-
-	/* Allocate private data */
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&op->dev, "Unable to allocate private data\n");
-		ret = -ENOMEM;
-		goto out_return;
-	}
-
-	/* Setup the miscdevice */
-	priv->miscdev.minor = MISC_DYNAMIC_MINOR;
-	priv->miscdev.name = drv_name;
-	priv->miscdev.fops = &fpga_fops;
-
-	kref_init(&priv->ref);
-
-	platform_set_drvdata(op, priv);
-	priv->dev = &op->dev;
-	mutex_init(&priv->lock);
-	init_completion(&priv->completion);
-
-	dev_set_drvdata(priv->dev, priv);
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_MEMCPY, mask);
-	dma_cap_set(DMA_SLAVE, mask);
-	dma_cap_set(DMA_SG, mask);
-
-	/* Get control of DMA channel #0 */
-	priv->chan = dma_request_channel(mask, dma_filter, NULL);
-	if (!priv->chan) {
-		dev_err(&op->dev, "Unable to acquire DMA channel #0\n");
-		ret = -ENODEV;
-		goto out_free_priv;
-	}
-
-	/* Remap the registers for use */
-	priv->regs = of_iomap(of_node, 0);
-	if (!priv->regs) {
-		dev_err(&op->dev, "Unable to ioremap registers\n");
-		ret = -ENOMEM;
-		goto out_dma_release_channel;
-	}
-
-	/* Remap the IMMR for use */
-	priv->immr = ioremap(get_immrbase(), 0x100000);
-	if (!priv->immr) {
-		dev_err(&op->dev, "Unable to ioremap IMMR\n");
-		ret = -ENOMEM;
-		goto out_unmap_regs;
-	}
-
-	/*
-	 * Check that external DMA is configured
-	 *
-	 * U-Boot does this for us, but we should check it and bail out if
-	 * there is a problem. Failing to have this register setup correctly
-	 * will cause the DMA controller to transfer a single cacheline
-	 * worth of data, then wedge itself.
-	 */
-	if ((ioread32be(priv->immr + 0x114) & 0xE00) != 0xE00) {
-		dev_err(&op->dev, "External DMA control not configured\n");
-		ret = -ENODEV;
-		goto out_unmap_immr;
-	}
-
-	/*
-	 * Check the CTL-CPLD version
-	 *
-	 * This driver uses the CTL-CPLD DATA-FPGA power sequencer, and we
-	 * don't want to run on any version of the CTL-CPLD that does not use
-	 * a compatible register layout.
-	 *
-	 * v2: changed register layout, added power sequencer
-	 * v3: added glitch filter on the i2c overcurrent/overtemp outputs
-	 */
-	ver = ioread8(priv->regs + CTL_CPLD_VERSION);
-	if (ver != 0x02 && ver != 0x03) {
-		dev_err(&op->dev, "CTL-CPLD is not version 0x02 or 0x03!\n");
-		ret = -ENODEV;
-		goto out_unmap_immr;
-	}
-
-	/* Set the exact size that the firmware image should be */
-	ver = ioread32be(priv->regs + SYS_REG_VERSION);
-	priv->fw_size = (ver & (1 << 18)) ? FW_SIZE_EP2S130 : FW_SIZE_EP2S90;
-
-	/* Find the correct IRQ number */
-	priv->irq = irq_of_parse_and_map(of_node, 0);
-	if (priv->irq == NO_IRQ) {
-		dev_err(&op->dev, "Unable to find IRQ line\n");
-		ret = -ENODEV;
-		goto out_unmap_immr;
-	}
-
-	/* Request the IRQ */
-	ret = request_irq(priv->irq, fpga_irq, IRQF_SHARED, drv_name, priv);
-	if (ret) {
-		dev_err(&op->dev, "Unable to request IRQ %d\n", priv->irq);
-		ret = -ENODEV;
-		goto out_irq_dispose_mapping;
-	}
-
-	/* Reset and stop the FPGA's, just in case */
-	fpga_do_stop(priv);
-
-	/* Register the miscdevice */
-	ret = misc_register(&priv->miscdev);
-	if (ret) {
-		dev_err(&op->dev, "Unable to register miscdevice\n");
-		goto out_free_irq;
-	}
-
-	/* Create the sysfs files */
-	this_device = priv->miscdev.this_device;
-	dev_set_drvdata(this_device, priv);
-	ret = sysfs_create_group(&this_device->kobj, &fpga_attr_group);
-	if (ret) {
-		dev_err(&op->dev, "Unable to create sysfs files\n");
-		goto out_misc_deregister;
-	}
-
-	dev_info(priv->dev, "CARMA FPGA Programmer: %s rev%s with %s FPGAs\n",
-			(ver & (1 << 17)) ? "Correlator" : "Digitizer",
-			(ver & (1 << 16)) ? "B" : "A",
-			(ver & (1 << 18)) ? "EP2S130" : "EP2S90");
-
-	return 0;
-
-out_misc_deregister:
-	misc_deregister(&priv->miscdev);
-out_free_irq:
-	free_irq(priv->irq, priv);
-out_irq_dispose_mapping:
-	irq_dispose_mapping(priv->irq);
-out_unmap_immr:
-	iounmap(priv->immr);
-out_unmap_regs:
-	iounmap(priv->regs);
-out_dma_release_channel:
-	dma_release_channel(priv->chan);
-out_free_priv:
-	kref_put(&priv->ref, fpga_dev_remove);
-out_return:
-	return ret;
-}
-
-static const struct of_device_id fpga_of_match[] = {
-	{ .compatible = "carma,fpga-programmer", },
-	{},
-};
-
-static struct platform_driver fpga_of_driver = {
-	.probe		= fpga_of_probe,
-	.remove		= fpga_of_remove,
-	.driver		= {
-		.name		= drv_name,
-		.of_match_table	= fpga_of_match,
-	},
-};
-
-/*
- * Module Init / Exit
- */
-
-static int __init fpga_init(void)
-{
-	led_trigger_register_simple("fpga", &ledtrig_fpga);
-	return platform_driver_register(&fpga_of_driver);
-}
-
-static void __exit fpga_exit(void)
-{
-	platform_driver_unregister(&fpga_of_driver);
-	led_trigger_unregister_simple(ledtrig_fpga);
-}
-
-MODULE_AUTHOR("Ira W. Snyder <iws@ovro.caltech.edu>");
-MODULE_DESCRIPTION("CARMA Board DATA-FPGA Programmer");
-MODULE_LICENSE("GPL");
-
-module_init(fpga_init);
-module_exit(fpga_exit);
diff --git a/drivers/misc/carma/carma-fpga.c b/drivers/misc/carma/carma-fpga.c
deleted file mode 100644
index 5aba3fd789de..000000000000
--- a/drivers/misc/carma/carma-fpga.c
+++ /dev/null
@@ -1,1507 +0,0 @@
-/*
- * CARMA DATA-FPGA Access Driver
- *
- * Copyright (c) 2009-2011 Ira W. Snyder <iws@ovro.caltech.edu>
- *
- * This program is free software; you can redistribute it and/or modify it
- * under 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.
- */
-
-/*
- * FPGA Memory Dump Format
- *
- * FPGA #0 control registers (32 x 32-bit words)
- * FPGA #1 control registers (32 x 32-bit words)
- * FPGA #2 control registers (32 x 32-bit words)
- * FPGA #3 control registers (32 x 32-bit words)
- * SYSFPGA control registers (32 x 32-bit words)
- * FPGA #0 correlation array (NUM_CORL0 correlation blocks)
- * FPGA #1 correlation array (NUM_CORL1 correlation blocks)
- * FPGA #2 correlation array (NUM_CORL2 correlation blocks)
- * FPGA #3 correlation array (NUM_CORL3 correlation blocks)
- *
- * Each correlation array consists of:
- *
- * Correlation Data      (2 x NUM_LAGSn x 32-bit words)
- * Pipeline Metadata     (2 x NUM_METAn x 32-bit words)
- * Quantization Counters (2 x NUM_QCNTn x 32-bit words)
- *
- * The NUM_CORLn, NUM_LAGSn, NUM_METAn, and NUM_QCNTn values come from
- * the FPGA configuration registers. They do not change once the FPGA's
- * have been programmed, they only change on re-programming.
- */
-
-/*
- * Basic Description:
- *
- * This driver is used to capture correlation spectra off of the four data
- * processing FPGAs. The FPGAs are often reprogrammed at runtime, therefore
- * this driver supports dynamic enable/disable of capture while the device
- * remains open.
- *
- * The nominal capture rate is 64Hz (every 15.625ms). To facilitate this fast
- * capture rate, all buffers are pre-allocated to avoid any potentially long
- * running memory allocations while capturing.
- *
- * There are two lists and one pointer which are used to keep track of the
- * different states of data buffers.
- *
- * 1) free list
- * This list holds all empty data buffers which are ready to receive data.
- *
- * 2) inflight pointer
- * This pointer holds the currently inflight data buffer. This buffer is having
- * data copied into it by the DMA engine.
- *
- * 3) used list
- * This list holds data buffers which have been filled, and are waiting to be
- * read by userspace.
- *
- * All buffers start life on the free list, then move successively to the
- * inflight pointer, and then to the used list. After they have been read by
- * userspace, they are moved back to the free list. The cycle repeats as long
- * as necessary.
- *
- * It should be noted that all buffers are mapped and ready for DMA when they
- * are on any of the three lists. They are only unmapped when they are in the
- * process of being read by userspace.
- */
-
-/*
- * Notes on the IRQ masking scheme:
- *
- * The IRQ masking scheme here is different than most other hardware. The only
- * way for the DATA-FPGAs to detect if the kernel has taken too long to copy
- * the data is if the status registers are not cleared before the next
- * correlation data dump is ready.
- *
- * The interrupt line is connected to the status registers, such that when they
- * are cleared, the interrupt is de-asserted. Therein lies our problem. We need
- * to schedule a long-running DMA operation and return from the interrupt
- * handler quickly, but we cannot clear the status registers.
- *
- * To handle this, the system controller FPGA has the capability to connect the
- * interrupt line to a user-controlled GPIO pin. This pin is driven high
- * (unasserted) and left that way. To mask the interrupt, we change the
- * interrupt source to the GPIO pin. Tada, we hid the interrupt. :)
- */
-
-#include <linux/of_address.h>
-#include <linux/of_irq.h>
-#include <linux/of_platform.h>
-#include <linux/dma-mapping.h>
-#include <linux/miscdevice.h>
-#include <linux/interrupt.h>
-#include <linux/dmaengine.h>
-#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>
-#include <linux/slab.h>
-#include <linux/kref.h>
-#include <linux/io.h>
-
-/* system controller registers */
-#define SYS_IRQ_SOURCE_CTL	0x24
-#define SYS_IRQ_OUTPUT_EN	0x28
-#define SYS_IRQ_OUTPUT_DATA	0x2C
-#define SYS_IRQ_INPUT_DATA	0x30
-#define SYS_FPGA_CONFIG_STATUS	0x44
-
-/* GPIO IRQ line assignment */
-#define IRQ_CORL_DONE		0x10
-
-/* FPGA registers */
-#define MMAP_REG_VERSION	0x00
-#define MMAP_REG_CORL_CONF1	0x08
-#define MMAP_REG_CORL_CONF2	0x0C
-#define MMAP_REG_STATUS		0x48
-
-#define SYS_FPGA_BLOCK		0xF0000000
-
-#define DATA_FPGA_START		0x400000
-#define DATA_FPGA_SIZE		0x80000
-
-static const char drv_name[] = "carma-fpga";
-
-#define NUM_FPGA	4
-
-#define MIN_DATA_BUFS	8
-#define MAX_DATA_BUFS	64
-
-struct fpga_info {
-	unsigned int num_lag_ram;
-	unsigned int blk_size;
-};
-
-struct data_buf {
-	struct list_head entry;
-	void *vaddr;
-	struct scatterlist *sglist;
-	int sglen;
-	int nr_pages;
-	size_t size;
-};
-
-struct fpga_device {
-	/* character device */
-	struct miscdevice miscdev;
-	struct device *dev;
-	struct mutex mutex;
-
-	/* reference count */
-	struct kref ref;
-
-	/* FPGA registers and information */
-	struct fpga_info info[NUM_FPGA];
-	void __iomem *regs;
-	int irq;
-
-	/* FPGA Physical Address/Size Information */
-	resource_size_t phys_addr;
-	size_t phys_size;
-
-	/* DMA structures */
-	struct sg_table corl_table;
-	unsigned int corl_nents;
-	struct dma_chan *chan;
-
-	/* Protection for all members below */
-	spinlock_t lock;
-
-	/* Device enable/disable flag */
-	bool enabled;
-
-	/* Correlation data buffers */
-	wait_queue_head_t wait;
-	struct list_head free;
-	struct list_head used;
-	struct data_buf *inflight;
-
-	/* Information about data buffers */
-	unsigned int num_dropped;
-	unsigned int num_buffers;
-	size_t bufsize;
-	struct dentry *dbg_entry;
-};
-
-struct fpga_reader {
-	struct fpga_device *priv;
-	struct data_buf *buf;
-	off_t buf_start;
-};
-
-static void fpga_device_release(struct kref *ref)
-{
-	struct fpga_device *priv = container_of(ref, struct fpga_device, ref);
-
-	/* the last reader has exited, cleanup the last bits */
-	mutex_destroy(&priv->mutex);
-	kfree(priv);
-}
-
-/*
- * 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
- *
- * This will free all of the pages allocated to the given data buffer, and
- * then free the structure itself
- */
-static void data_free_buffer(struct data_buf *buf)
-{
-	/* It is ok to free a NULL buffer */
-	if (!buf)
-		return;
-
-	/* free all memory */
-	vfree(buf->sglist);
-	vfree(buf->vaddr);
-	kfree(buf);
-}
-
-/**
- * data_alloc_buffer() - allocate and fill a data buffer with pages
- * @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 carma_dma_map().
- *
- * Returns NULL on failure
- */
-static struct data_buf *data_alloc_buffer(const size_t bytes)
-{
-	unsigned int nr_pages;
-	struct data_buf *buf;
-	int ret;
-
-	/* calculate the number of pages necessary */
-	nr_pages = DIV_ROUND_UP(bytes, PAGE_SIZE);
-
-	/* allocate the buffer structure */
-	buf = kzalloc(sizeof(*buf), GFP_KERNEL);
-	if (!buf)
-		goto out_return;
-
-	/* initialize internal fields */
-	INIT_LIST_HEAD(&buf->entry);
-	buf->size = bytes;
-
-	/* allocate the buffer */
-	ret = carma_dma_init(buf, nr_pages);
-	if (ret)
-		goto out_free_buf;
-
-	return buf;
-
-out_free_buf:
-	kfree(buf);
-out_return:
-	return NULL;
-}
-
-/**
- * data_free_buffers() - free all allocated buffers
- * @priv: the driver's private data structure
- *
- * Free all buffers allocated by the driver (except those currently in the
- * process of being read by userspace).
- *
- * LOCKING: must hold dev->mutex
- * CONTEXT: user
- */
-static void data_free_buffers(struct fpga_device *priv)
-{
-	struct data_buf *buf, *tmp;
-
-	/* the device should be stopped, no DMA in progress */
-	BUG_ON(priv->inflight != NULL);
-
-	list_for_each_entry_safe(buf, tmp, &priv->free, entry) {
-		list_del_init(&buf->entry);
-		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);
-		carma_dma_unmap(priv->dev, buf);
-		data_free_buffer(buf);
-	}
-
-	priv->num_buffers = 0;
-	priv->bufsize = 0;
-}
-
-/**
- * data_alloc_buffers() - allocate 1 seconds worth of data buffers
- * @priv: the driver's private data structure
- *
- * Allocate enough buffers for a whole second worth of data
- *
- * This routine will attempt to degrade nicely by succeeding even if a full
- * second worth of data buffers could not be allocated, as long as a minimum
- * number were allocated. In this case, it will print a message to the kernel
- * log.
- *
- * The device must not be modifying any lists when this is called.
- *
- * CONTEXT: user
- * LOCKING: must hold dev->mutex
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static int data_alloc_buffers(struct fpga_device *priv)
-{
-	struct data_buf *buf;
-	int i, ret;
-
-	for (i = 0; i < MAX_DATA_BUFS; i++) {
-
-		/* allocate a buffer */
-		buf = data_alloc_buffer(priv->bufsize);
-		if (!buf)
-			break;
-
-		/* map it for DMA */
-		ret = carma_dma_map(priv->dev, buf);
-		if (ret) {
-			data_free_buffer(buf);
-			break;
-		}
-
-		/* add it to the list of free buffers */
-		list_add_tail(&buf->entry, &priv->free);
-		priv->num_buffers++;
-	}
-
-	/* Make sure we allocated the minimum required number of buffers */
-	if (priv->num_buffers < MIN_DATA_BUFS) {
-		dev_err(priv->dev, "Unable to allocate enough data buffers\n");
-		data_free_buffers(priv);
-		return -ENOMEM;
-	}
-
-	/* Warn if we are running in a degraded state, but do not fail */
-	if (priv->num_buffers < MAX_DATA_BUFS) {
-		dev_warn(priv->dev,
-			 "Unable to allocate %d buffers, using %d buffers instead\n",
-			 MAX_DATA_BUFS, i);
-	}
-
-	return 0;
-}
-
-/*
- * DMA Operations Helpers
- */
-
-/**
- * fpga_start_addr() - get the physical address a DATA-FPGA
- * @priv: the driver's private data structure
- * @fpga: the DATA-FPGA number (zero based)
- */
-static dma_addr_t fpga_start_addr(struct fpga_device *priv, unsigned int fpga)
-{
-	return priv->phys_addr + 0x400000 + (0x80000 * fpga);
-}
-
-/**
- * fpga_block_addr() - get the physical address of a correlation data block
- * @priv: the driver's private data structure
- * @fpga: the DATA-FPGA number (zero based)
- * @blknum: the correlation block number (zero based)
- */
-static dma_addr_t fpga_block_addr(struct fpga_device *priv, unsigned int fpga,
-				  unsigned int blknum)
-{
-	return fpga_start_addr(priv, fpga) + (0x10000 * (1 + blknum));
-}
-
-#define REG_BLOCK_SIZE	(32 * 4)
-
-/**
- * data_setup_corl_table() - create the scatterlist for correlation dumps
- * @priv: the driver's private data structure
- *
- * Create the scatterlist for transferring a correlation dump from the
- * DATA FPGAs. This structure will be reused for each buffer than needs
- * to be filled with correlation data.
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static int data_setup_corl_table(struct fpga_device *priv)
-{
-	struct sg_table *table = &priv->corl_table;
-	struct scatterlist *sg;
-	struct fpga_info *info;
-	int i, j, ret;
-
-	/* Calculate the number of entries needed */
-	priv->corl_nents = (1 + NUM_FPGA) * REG_BLOCK_SIZE;
-	for (i = 0; i < NUM_FPGA; i++)
-		priv->corl_nents += priv->info[i].num_lag_ram;
-
-	/* Allocate the scatterlist table */
-	ret = sg_alloc_table(table, priv->corl_nents, GFP_KERNEL);
-	if (ret) {
-		dev_err(priv->dev, "unable to allocate DMA table\n");
-		return ret;
-	}
-
-	/* Add the DATA FPGA registers to the scatterlist */
-	sg = table->sgl;
-	for (i = 0; i < NUM_FPGA; i++) {
-		sg_dma_address(sg) = fpga_start_addr(priv, i);
-		sg_dma_len(sg) = REG_BLOCK_SIZE;
-		sg = sg_next(sg);
-	}
-
-	/* Add the SYS-FPGA registers to the scatterlist */
-	sg_dma_address(sg) = SYS_FPGA_BLOCK;
-	sg_dma_len(sg) = REG_BLOCK_SIZE;
-	sg = sg_next(sg);
-
-	/* Add the FPGA correlation data blocks to the scatterlist */
-	for (i = 0; i < NUM_FPGA; i++) {
-		info = &priv->info[i];
-		for (j = 0; j < info->num_lag_ram; j++) {
-			sg_dma_address(sg) = fpga_block_addr(priv, i, j);
-			sg_dma_len(sg) = info->blk_size;
-			sg = sg_next(sg);
-		}
-	}
-
-	/*
-	 * All physical addresses and lengths are present in the structure
-	 * now. It can be reused for every FPGA DATA interrupt
-	 */
-	return 0;
-}
-
-/*
- * FPGA Register Access Helpers
- */
-
-static void fpga_write_reg(struct fpga_device *priv, unsigned int fpga,
-			   unsigned int reg, u32 val)
-{
-	const int fpga_start = DATA_FPGA_START + (fpga * DATA_FPGA_SIZE);
-	iowrite32be(val, priv->regs + fpga_start + reg);
-}
-
-static u32 fpga_read_reg(struct fpga_device *priv, unsigned int fpga,
-			 unsigned int reg)
-{
-	const int fpga_start = DATA_FPGA_START + (fpga * DATA_FPGA_SIZE);
-	return ioread32be(priv->regs + fpga_start + reg);
-}
-
-/**
- * data_calculate_bufsize() - calculate the data buffer size required
- * @priv: the driver's private data structure
- *
- * Calculate the total buffer size needed to hold a single block
- * of correlation data
- *
- * CONTEXT: user
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static int data_calculate_bufsize(struct fpga_device *priv)
-{
-	u32 num_corl, num_lags, num_meta, num_qcnt, num_pack;
-	u32 conf1, conf2, version;
-	u32 num_lag_ram, blk_size;
-	int i;
-
-	/* Each buffer starts with the 5 FPGA register areas */
-	priv->bufsize = (1 + NUM_FPGA) * REG_BLOCK_SIZE;
-
-	/* Read and store the configuration data for each FPGA */
-	for (i = 0; i < NUM_FPGA; i++) {
-		version = fpga_read_reg(priv, i, MMAP_REG_VERSION);
-		conf1 = fpga_read_reg(priv, i, MMAP_REG_CORL_CONF1);
-		conf2 = fpga_read_reg(priv, i, MMAP_REG_CORL_CONF2);
-
-		/* minor version 2 and later */
-		if ((version & 0x000000FF) >= 2) {
-			num_corl = (conf1 & 0x000000F0) >> 4;
-			num_pack = (conf1 & 0x00000F00) >> 8;
-			num_lags = (conf1 & 0x00FFF000) >> 12;
-			num_meta = (conf1 & 0x7F000000) >> 24;
-			num_qcnt = (conf2 & 0x00000FFF) >> 0;
-		} else {
-			num_corl = (conf1 & 0x000000F0) >> 4;
-			num_pack = 1; /* implied */
-			num_lags = (conf1 & 0x000FFF00) >> 8;
-			num_meta = (conf1 & 0x7FF00000) >> 20;
-			num_qcnt = (conf2 & 0x00000FFF) >> 0;
-		}
-
-		num_lag_ram = (num_corl + num_pack - 1) / num_pack;
-		blk_size = ((num_pack * num_lags) + num_meta + num_qcnt) * 8;
-
-		priv->info[i].num_lag_ram = num_lag_ram;
-		priv->info[i].blk_size = blk_size;
-		priv->bufsize += num_lag_ram * blk_size;
-
-		dev_dbg(priv->dev, "FPGA %d NUM_CORL: %d\n", i, num_corl);
-		dev_dbg(priv->dev, "FPGA %d NUM_PACK: %d\n", i, num_pack);
-		dev_dbg(priv->dev, "FPGA %d NUM_LAGS: %d\n", i, num_lags);
-		dev_dbg(priv->dev, "FPGA %d NUM_META: %d\n", i, num_meta);
-		dev_dbg(priv->dev, "FPGA %d NUM_QCNT: %d\n", i, num_qcnt);
-		dev_dbg(priv->dev, "FPGA %d BLK_SIZE: %d\n", i, blk_size);
-	}
-
-	dev_dbg(priv->dev, "TOTAL BUFFER SIZE: %zu bytes\n", priv->bufsize);
-	return 0;
-}
-
-/*
- * Interrupt Handling
- */
-
-/**
- * data_disable_interrupts() - stop the device from generating interrupts
- * @priv: the driver's private data structure
- *
- * Hide interrupts by switching to GPIO interrupt source
- *
- * LOCKING: must hold dev->lock
- */
-static void data_disable_interrupts(struct fpga_device *priv)
-{
-	/* hide the interrupt by switching the IRQ driver to GPIO */
-	iowrite32be(0x2F, priv->regs + SYS_IRQ_SOURCE_CTL);
-}
-
-/**
- * data_enable_interrupts() - allow the device to generate interrupts
- * @priv: the driver's private data structure
- *
- * Unhide interrupts by switching to the FPGA interrupt source. At the
- * same time, clear the DATA-FPGA status registers.
- *
- * LOCKING: must hold dev->lock
- */
-static void data_enable_interrupts(struct fpga_device *priv)
-{
-	/* clear the actual FPGA corl_done interrupt */
-	fpga_write_reg(priv, 0, MMAP_REG_STATUS, 0x0);
-	fpga_write_reg(priv, 1, MMAP_REG_STATUS, 0x0);
-	fpga_write_reg(priv, 2, MMAP_REG_STATUS, 0x0);
-	fpga_write_reg(priv, 3, MMAP_REG_STATUS, 0x0);
-
-	/* flush the writes */
-	fpga_read_reg(priv, 0, MMAP_REG_STATUS);
-	fpga_read_reg(priv, 1, MMAP_REG_STATUS);
-	fpga_read_reg(priv, 2, MMAP_REG_STATUS);
-	fpga_read_reg(priv, 3, MMAP_REG_STATUS);
-
-	/* switch back to the external interrupt source */
-	iowrite32be(0x3F, priv->regs + SYS_IRQ_SOURCE_CTL);
-}
-
-/**
- * data_dma_cb() - DMAEngine callback for DMA completion
- * @data: the driver's private data structure
- *
- * Complete a DMA transfer from the DATA-FPGA's
- *
- * This is called via the DMA callback mechanism, and will handle moving the
- * completed DMA transaction to the used list, and then wake any processes
- * waiting for new data
- *
- * CONTEXT: any, softirq expected
- */
-static void data_dma_cb(void *data)
-{
-	struct fpga_device *priv = data;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	/* If there is no inflight buffer, we've got a bug */
-	BUG_ON(priv->inflight == NULL);
-
-	/* Move the inflight buffer onto the used list */
-	list_move_tail(&priv->inflight->entry, &priv->used);
-	priv->inflight = NULL;
-
-	/*
-	 * If data dumping is still enabled, then clear the FPGA
-	 * status registers and re-enable FPGA interrupts
-	 */
-	if (priv->enabled)
-		data_enable_interrupts(priv);
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	/*
-	 * We've changed both the inflight and used lists, so we need
-	 * to wake up any processes that are blocking for those events
-	 */
-	wake_up(&priv->wait);
-}
-
-/**
- * data_submit_dma() - prepare and submit the required DMA to fill a buffer
- * @priv: the driver's private data structure
- * @buf: the data buffer
- *
- * Prepare and submit the necessary DMA transactions to fill a correlation
- * data buffer.
- *
- * LOCKING: must hold dev->lock
- * CONTEXT: hardirq only
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static int data_submit_dma(struct fpga_device *priv, struct data_buf *buf)
-{
-	struct scatterlist *dst_sg, *src_sg;
-	unsigned int dst_nents, src_nents;
-	struct dma_chan *chan = priv->chan;
-	struct dma_async_tx_descriptor *tx;
-	dma_cookie_t cookie;
-	dma_addr_t dst, src;
-	unsigned long dma_flags = 0;
-
-	dst_sg = buf->sglist;
-	dst_nents = buf->sglen;
-
-	src_sg = priv->corl_table.sgl;
-	src_nents = priv->corl_nents;
-
-	/*
-	 * All buffers passed to this function should be ready and mapped
-	 * for DMA already. Therefore, we don't need to do anything except
-	 * submit it to the Freescale DMA Engine for processing
-	 */
-
-	/* setup the scatterlist to scatterlist transfer */
-	tx = chan->device->device_prep_dma_sg(chan,
-					      dst_sg, dst_nents,
-					      src_sg, src_nents,
-					      0);
-	if (!tx) {
-		dev_err(priv->dev, "unable to prep scatterlist DMA\n");
-		return -ENOMEM;
-	}
-
-	/* submit the transaction to the DMA controller */
-	cookie = tx->tx_submit(tx);
-	if (dma_submit_error(cookie)) {
-		dev_err(priv->dev, "unable to submit scatterlist DMA\n");
-		return -ENOMEM;
-	}
-
-	/* Prepare the re-read of the SYS-FPGA block */
-	dst = sg_dma_address(dst_sg) + (NUM_FPGA * REG_BLOCK_SIZE);
-	src = SYS_FPGA_BLOCK;
-	tx = chan->device->device_prep_dma_memcpy(chan, dst, src,
-						  REG_BLOCK_SIZE,
-						  dma_flags);
-	if (!tx) {
-		dev_err(priv->dev, "unable to prep SYS-FPGA DMA\n");
-		return -ENOMEM;
-	}
-
-	/* Setup the callback */
-	tx->callback = data_dma_cb;
-	tx->callback_param = priv;
-
-	/* submit the transaction to the DMA controller */
-	cookie = tx->tx_submit(tx);
-	if (dma_submit_error(cookie)) {
-		dev_err(priv->dev, "unable to submit SYS-FPGA DMA\n");
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-
-#define CORL_DONE	0x1
-#define CORL_ERR	0x2
-
-static irqreturn_t data_irq(int irq, void *dev_id)
-{
-	struct fpga_device *priv = dev_id;
-	bool submitted = false;
-	struct data_buf *buf;
-	u32 status;
-	int i;
-
-	/* detect spurious interrupts via FPGA status */
-	for (i = 0; i < 4; i++) {
-		status = fpga_read_reg(priv, i, MMAP_REG_STATUS);
-		if (!(status & (CORL_DONE | CORL_ERR))) {
-			dev_err(priv->dev, "spurious irq detected (FPGA)\n");
-			return IRQ_NONE;
-		}
-	}
-
-	/* detect spurious interrupts via raw IRQ pin readback */
-	status = ioread32be(priv->regs + SYS_IRQ_INPUT_DATA);
-	if (status & IRQ_CORL_DONE) {
-		dev_err(priv->dev, "spurious irq detected (IRQ)\n");
-		return IRQ_NONE;
-	}
-
-	spin_lock(&priv->lock);
-
-	/*
-	 * This is an error case that should never happen.
-	 *
-	 * If this driver has a bug and manages to re-enable interrupts while
-	 * a DMA is in progress, then we will hit this statement and should
-	 * start paying attention immediately.
-	 */
-	BUG_ON(priv->inflight != NULL);
-
-	/* hide the interrupt by switching the IRQ driver to GPIO */
-	data_disable_interrupts(priv);
-
-	/* If there are no free buffers, drop this data */
-	if (list_empty(&priv->free)) {
-		priv->num_dropped++;
-		goto out;
-	}
-
-	buf = list_first_entry(&priv->free, struct data_buf, entry);
-	list_del_init(&buf->entry);
-	BUG_ON(buf->size != priv->bufsize);
-
-	/* Submit a DMA transfer to get the correlation data */
-	if (data_submit_dma(priv, buf)) {
-		dev_err(priv->dev, "Unable to setup DMA transfer\n");
-		list_move_tail(&buf->entry, &priv->free);
-		goto out;
-	}
-
-	/* Save the buffer for the DMA callback */
-	priv->inflight = buf;
-	submitted = true;
-
-	/* Start the DMA Engine */
-	dma_async_issue_pending(priv->chan);
-
-out:
-	/* If no DMA was submitted, re-enable interrupts */
-	if (!submitted)
-		data_enable_interrupts(priv);
-
-	spin_unlock(&priv->lock);
-	return IRQ_HANDLED;
-}
-
-/*
- * Realtime Device Enable Helpers
- */
-
-/**
- * data_device_enable() - enable the device for buffered dumping
- * @priv: the driver's private data structure
- *
- * Enable the device for buffered dumping. Allocates buffers and hooks up
- * the interrupt handler. When this finishes, data will come pouring in.
- *
- * LOCKING: must hold dev->mutex
- * CONTEXT: user context only
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static int data_device_enable(struct fpga_device *priv)
-{
-	bool enabled;
-	u32 val;
-	int ret;
-
-	/* multiple enables are safe: they do nothing */
-	spin_lock_irq(&priv->lock);
-	enabled = priv->enabled;
-	spin_unlock_irq(&priv->lock);
-	if (enabled)
-		return 0;
-
-	/* check that the FPGAs are programmed */
-	val = ioread32be(priv->regs + SYS_FPGA_CONFIG_STATUS);
-	if (!(val & (1 << 18))) {
-		dev_err(priv->dev, "DATA-FPGAs are not enabled\n");
-		return -ENODATA;
-	}
-
-	/* read the FPGAs to calculate the buffer size */
-	ret = data_calculate_bufsize(priv);
-	if (ret) {
-		dev_err(priv->dev, "unable to calculate buffer size\n");
-		goto out_error;
-	}
-
-	/* allocate the correlation data buffers */
-	ret = data_alloc_buffers(priv);
-	if (ret) {
-		dev_err(priv->dev, "unable to allocate buffers\n");
-		goto out_error;
-	}
-
-	/* setup the source scatterlist for dumping correlation data */
-	ret = data_setup_corl_table(priv);
-	if (ret) {
-		dev_err(priv->dev, "unable to setup correlation DMA table\n");
-		goto out_error;
-	}
-
-	/* prevent the FPGAs from generating interrupts */
-	data_disable_interrupts(priv);
-
-	/* hookup the irq handler */
-	ret = request_irq(priv->irq, data_irq, IRQF_SHARED, drv_name, priv);
-	if (ret) {
-		dev_err(priv->dev, "unable to request IRQ handler\n");
-		goto out_error;
-	}
-
-	/* allow the DMA callback to re-enable FPGA interrupts */
-	spin_lock_irq(&priv->lock);
-	priv->enabled = true;
-	spin_unlock_irq(&priv->lock);
-
-	/* allow the FPGAs to generate interrupts */
-	data_enable_interrupts(priv);
-	return 0;
-
-out_error:
-	sg_free_table(&priv->corl_table);
-	priv->corl_nents = 0;
-
-	data_free_buffers(priv);
-	return ret;
-}
-
-/**
- * data_device_disable() - disable the device for buffered dumping
- * @priv: the driver's private data structure
- *
- * Disable the device for buffered dumping. Stops new DMA transactions from
- * being generated, waits for all outstanding DMA to complete, and then frees
- * all buffers.
- *
- * LOCKING: must hold dev->mutex
- * CONTEXT: user only
- *
- * Returns 0 on success, -ERRNO otherwise
- */
-static int data_device_disable(struct fpga_device *priv)
-{
-	spin_lock_irq(&priv->lock);
-
-	/* allow multiple disable */
-	if (!priv->enabled) {
-		spin_unlock_irq(&priv->lock);
-		return 0;
-	}
-
-	/*
-	 * Mark the device disabled
-	 *
-	 * This stops DMA callbacks from re-enabling interrupts
-	 */
-	priv->enabled = false;
-
-	/* prevent the FPGAs from generating interrupts */
-	data_disable_interrupts(priv);
-
-	/* wait until all ongoing DMA has finished */
-	while (priv->inflight != NULL) {
-		spin_unlock_irq(&priv->lock);
-		wait_event(priv->wait, priv->inflight == NULL);
-		spin_lock_irq(&priv->lock);
-	}
-
-	spin_unlock_irq(&priv->lock);
-
-	/* unhook the irq handler */
-	free_irq(priv->irq, priv);
-
-	/* free the correlation table */
-	sg_free_table(&priv->corl_table);
-	priv->corl_nents = 0;
-
-	/* free all buffers: the free and used lists are not being changed */
-	data_free_buffers(priv);
-	return 0;
-}
-
-/*
- * DEBUGFS Interface
- */
-#ifdef CONFIG_DEBUG_FS
-
-/*
- * Count the number of entries in the given list
- */
-static unsigned int list_num_entries(struct list_head *list)
-{
-	struct list_head *entry;
-	unsigned int ret = 0;
-
-	list_for_each(entry, list)
-		ret++;
-
-	return ret;
-}
-
-static int data_debug_show(struct seq_file *f, void *offset)
-{
-	struct fpga_device *priv = f->private;
-
-	spin_lock_irq(&priv->lock);
-
-	seq_printf(f, "enabled: %d\n", priv->enabled);
-	seq_printf(f, "bufsize: %d\n", priv->bufsize);
-	seq_printf(f, "num_buffers: %d\n", priv->num_buffers);
-	seq_printf(f, "num_free: %d\n", list_num_entries(&priv->free));
-	seq_printf(f, "inflight: %d\n", priv->inflight != NULL);
-	seq_printf(f, "num_used: %d\n", list_num_entries(&priv->used));
-	seq_printf(f, "num_dropped: %d\n", priv->num_dropped);
-
-	spin_unlock_irq(&priv->lock);
-	return 0;
-}
-
-static int data_debug_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, data_debug_show, inode->i_private);
-}
-
-static const struct file_operations data_debug_fops = {
-	.owner		= THIS_MODULE,
-	.open		= data_debug_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-static int data_debugfs_init(struct fpga_device *priv)
-{
-	priv->dbg_entry = debugfs_create_file(drv_name, S_IRUGO, NULL, priv,
-					      &data_debug_fops);
-	return PTR_ERR_OR_ZERO(priv->dbg_entry);
-}
-
-static void data_debugfs_exit(struct fpga_device *priv)
-{
-	debugfs_remove(priv->dbg_entry);
-}
-
-#else
-
-static inline int data_debugfs_init(struct fpga_device *priv)
-{
-	return 0;
-}
-
-static inline void data_debugfs_exit(struct fpga_device *priv)
-{
-}
-
-#endif	/* CONFIG_DEBUG_FS */
-
-/*
- * SYSFS Attributes
- */
-
-static ssize_t data_en_show(struct device *dev, struct device_attribute *attr,
-			    char *buf)
-{
-	struct fpga_device *priv = dev_get_drvdata(dev);
-	int ret;
-
-	spin_lock_irq(&priv->lock);
-	ret = snprintf(buf, PAGE_SIZE, "%u\n", priv->enabled);
-	spin_unlock_irq(&priv->lock);
-
-	return ret;
-}
-
-static ssize_t data_en_set(struct device *dev, struct device_attribute *attr,
-			   const char *buf, size_t count)
-{
-	struct fpga_device *priv = dev_get_drvdata(dev);
-	unsigned long enable;
-	int ret;
-
-	ret = kstrtoul(buf, 0, &enable);
-	if (ret) {
-		dev_err(priv->dev, "unable to parse enable input\n");
-		return ret;
-	}
-
-	/* protect against concurrent enable/disable */
-	ret = mutex_lock_interruptible(&priv->mutex);
-	if (ret)
-		return ret;
-
-	if (enable)
-		ret = data_device_enable(priv);
-	else
-		ret = data_device_disable(priv);
-
-	if (ret) {
-		dev_err(priv->dev, "device %s failed\n",
-			enable ? "enable" : "disable");
-		count = ret;
-		goto out_unlock;
-	}
-
-out_unlock:
-	mutex_unlock(&priv->mutex);
-	return count;
-}
-
-static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO, data_en_show, data_en_set);
-
-static struct attribute *data_sysfs_attrs[] = {
-	&dev_attr_enable.attr,
-	NULL,
-};
-
-static const struct attribute_group rt_sysfs_attr_group = {
-	.attrs = data_sysfs_attrs,
-};
-
-/*
- * FPGA Realtime Data Character Device
- */
-
-static int data_open(struct inode *inode, struct file *filp)
-{
-	/*
-	 * The miscdevice layer puts our struct miscdevice into the
-	 * filp->private_data field. We use this to find our private
-	 * data and then overwrite it with our own private structure.
-	 */
-	struct fpga_device *priv = container_of(filp->private_data,
-						struct fpga_device, miscdev);
-	struct fpga_reader *reader;
-	int ret;
-
-	/* allocate private data */
-	reader = kzalloc(sizeof(*reader), GFP_KERNEL);
-	if (!reader)
-		return -ENOMEM;
-
-	reader->priv = priv;
-	reader->buf = NULL;
-
-	filp->private_data = reader;
-	ret = nonseekable_open(inode, filp);
-	if (ret) {
-		dev_err(priv->dev, "nonseekable-open failed\n");
-		kfree(reader);
-		return ret;
-	}
-
-	/*
-	 * success, increase the reference count of the private data structure
-	 * so that it doesn't disappear if the device is unbound
-	 */
-	kref_get(&priv->ref);
-	return 0;
-}
-
-static int data_release(struct inode *inode, struct file *filp)
-{
-	struct fpga_reader *reader = filp->private_data;
-	struct fpga_device *priv = reader->priv;
-
-	/* free the per-reader structure */
-	data_free_buffer(reader->buf);
-	kfree(reader);
-	filp->private_data = NULL;
-
-	/* decrement our reference count to the private data */
-	kref_put(&priv->ref, fpga_device_release);
-	return 0;
-}
-
-static ssize_t data_read(struct file *filp, char __user *ubuf, size_t count,
-			 loff_t *f_pos)
-{
-	struct fpga_reader *reader = filp->private_data;
-	struct fpga_device *priv = reader->priv;
-	struct list_head *used = &priv->used;
-	bool drop_buffer = false;
-	struct data_buf *dbuf;
-	size_t avail;
-	void *data;
-	int ret;
-
-	/* check if we already have a partial buffer */
-	if (reader->buf) {
-		dbuf = reader->buf;
-		goto have_buffer;
-	}
-
-	spin_lock_irq(&priv->lock);
-
-	/* Block until there is at least one buffer on the used list */
-	while (list_empty(used)) {
-		spin_unlock_irq(&priv->lock);
-
-		if (filp->f_flags & O_NONBLOCK)
-			return -EAGAIN;
-
-		ret = wait_event_interruptible(priv->wait, !list_empty(used));
-		if (ret)
-			return ret;
-
-		spin_lock_irq(&priv->lock);
-	}
-
-	/* Grab the first buffer off of the used list */
-	dbuf = list_first_entry(used, struct data_buf, entry);
-	list_del_init(&dbuf->entry);
-
-	spin_unlock_irq(&priv->lock);
-
-	/* Buffers are always mapped: unmap it */
-	carma_dma_unmap(priv->dev, dbuf);
-
-	/* save the buffer for later */
-	reader->buf = dbuf;
-	reader->buf_start = 0;
-
-have_buffer:
-	/* Get the number of bytes available */
-	avail = dbuf->size - reader->buf_start;
-	data = dbuf->vaddr + reader->buf_start;
-
-	/* Get the number of bytes we can transfer */
-	count = min(count, avail);
-
-	/* Copy the data to the userspace buffer */
-	if (copy_to_user(ubuf, data, count))
-		return -EFAULT;
-
-	/* Update the amount of available space */
-	avail -= count;
-
-	/*
-	 * If there is still some data available, save the buffer for the
-	 * next userspace call to read() and return
-	 */
-	if (avail > 0) {
-		reader->buf_start += count;
-		reader->buf = dbuf;
-		return count;
-	}
-
-	/*
-	 * Get the buffer ready to be reused for DMA
-	 *
-	 * If it fails, we pretend that the read never happed and return
-	 * -EFAULT to userspace. The read will be retried.
-	 */
-	ret = carma_dma_map(priv->dev, dbuf);
-	if (ret) {
-		dev_err(priv->dev, "unable to remap buffer for DMA\n");
-		return -EFAULT;
-	}
-
-	/* Lock against concurrent enable/disable */
-	spin_lock_irq(&priv->lock);
-
-	/* the reader is finished with this buffer */
-	reader->buf = NULL;
-
-	/*
-	 * One of two things has happened, the device is disabled, or the
-	 * device has been reconfigured underneath us. In either case, we
-	 * should just throw away the buffer.
-	 *
-	 * Lockdep complains if this is done under the spinlock, so we
-	 * handle it during the unlock path.
-	 */
-	if (!priv->enabled || dbuf->size != priv->bufsize) {
-		drop_buffer = true;
-		goto out_unlock;
-	}
-
-	/* The buffer is safe to reuse, so add it back to the free list */
-	list_add_tail(&dbuf->entry, &priv->free);
-
-out_unlock:
-	spin_unlock_irq(&priv->lock);
-
-	if (drop_buffer) {
-		carma_dma_unmap(priv->dev, dbuf);
-		data_free_buffer(dbuf);
-	}
-
-	return count;
-}
-
-static unsigned int data_poll(struct file *filp, struct poll_table_struct *tbl)
-{
-	struct fpga_reader *reader = filp->private_data;
-	struct fpga_device *priv = reader->priv;
-	unsigned int mask = 0;
-
-	poll_wait(filp, &priv->wait, tbl);
-
-	if (!list_empty(&priv->used))
-		mask |= POLLIN | POLLRDNORM;
-
-	return mask;
-}
-
-static int data_mmap(struct file *filp, struct vm_area_struct *vma)
-{
-	struct fpga_reader *reader = filp->private_data;
-	struct fpga_device *priv = reader->priv;
-	unsigned long offset, vsize, psize, addr;
-
-	/* VMA properties */
-	offset = vma->vm_pgoff << PAGE_SHIFT;
-	vsize = vma->vm_end - vma->vm_start;
-	psize = priv->phys_size - offset;
-	addr = (priv->phys_addr + offset) >> PAGE_SHIFT;
-
-	/* Check against the FPGA region's physical memory size */
-	if (vsize > psize) {
-		dev_err(priv->dev, "requested mmap mapping too large\n");
-		return -EINVAL;
-	}
-
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-
-	return io_remap_pfn_range(vma, vma->vm_start, addr, vsize,
-				  vma->vm_page_prot);
-}
-
-static const struct file_operations data_fops = {
-	.owner		= THIS_MODULE,
-	.open		= data_open,
-	.release	= data_release,
-	.read		= data_read,
-	.poll		= data_poll,
-	.mmap		= data_mmap,
-	.llseek		= no_llseek,
-};
-
-/*
- * OpenFirmware Device Subsystem
- */
-
-static bool dma_filter(struct dma_chan *chan, void *data)
-{
-	/*
-	 * DMA Channel #0 is used for the FPGA Programmer, so ignore it
-	 *
-	 * This probably won't survive an unload/load cycle of the Freescale
-	 * DMAEngine driver, but that won't be a problem
-	 */
-	if (chan->chan_id == 0 && chan->device->dev_id == 0)
-		return false;
-
-	return true;
-}
-
-static int data_of_probe(struct platform_device *op)
-{
-	struct device_node *of_node = op->dev.of_node;
-	struct device *this_device;
-	struct fpga_device *priv;
-	struct resource res;
-	dma_cap_mask_t mask;
-	int ret;
-
-	/* Allocate private data */
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(&op->dev, "Unable to allocate device private data\n");
-		ret = -ENOMEM;
-		goto out_return;
-	}
-
-	platform_set_drvdata(op, priv);
-	priv->dev = &op->dev;
-	kref_init(&priv->ref);
-	mutex_init(&priv->mutex);
-
-	dev_set_drvdata(priv->dev, priv);
-	spin_lock_init(&priv->lock);
-	INIT_LIST_HEAD(&priv->free);
-	INIT_LIST_HEAD(&priv->used);
-	init_waitqueue_head(&priv->wait);
-
-	/* Setup the misc device */
-	priv->miscdev.minor = MISC_DYNAMIC_MINOR;
-	priv->miscdev.name = drv_name;
-	priv->miscdev.fops = &data_fops;
-
-	/* Get the physical address of the FPGA registers */
-	ret = of_address_to_resource(of_node, 0, &res);
-	if (ret) {
-		dev_err(&op->dev, "Unable to find FPGA physical address\n");
-		ret = -ENODEV;
-		goto out_free_priv;
-	}
-
-	priv->phys_addr = res.start;
-	priv->phys_size = resource_size(&res);
-
-	/* ioremap the registers for use */
-	priv->regs = of_iomap(of_node, 0);
-	if (!priv->regs) {
-		dev_err(&op->dev, "Unable to ioremap registers\n");
-		ret = -ENOMEM;
-		goto out_free_priv;
-	}
-
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_MEMCPY, mask);
-	dma_cap_set(DMA_INTERRUPT, mask);
-	dma_cap_set(DMA_SLAVE, mask);
-	dma_cap_set(DMA_SG, mask);
-
-	/* Request a DMA channel */
-	priv->chan = dma_request_channel(mask, dma_filter, NULL);
-	if (!priv->chan) {
-		dev_err(&op->dev, "Unable to request DMA channel\n");
-		ret = -ENODEV;
-		goto out_unmap_regs;
-	}
-
-	/* Find the correct IRQ number */
-	priv->irq = irq_of_parse_and_map(of_node, 0);
-	if (priv->irq == NO_IRQ) {
-		dev_err(&op->dev, "Unable to find IRQ line\n");
-		ret = -ENODEV;
-		goto out_release_dma;
-	}
-
-	/* Drive the GPIO for FPGA IRQ high (no interrupt) */
-	iowrite32be(IRQ_CORL_DONE, priv->regs + SYS_IRQ_OUTPUT_DATA);
-
-	/* Register the miscdevice */
-	ret = misc_register(&priv->miscdev);
-	if (ret) {
-		dev_err(&op->dev, "Unable to register miscdevice\n");
-		goto out_irq_dispose_mapping;
-	}
-
-	/* Create the debugfs files */
-	ret = data_debugfs_init(priv);
-	if (ret) {
-		dev_err(&op->dev, "Unable to create debugfs files\n");
-		goto out_misc_deregister;
-	}
-
-	/* Create the sysfs files */
-	this_device = priv->miscdev.this_device;
-	dev_set_drvdata(this_device, priv);
-	ret = sysfs_create_group(&this_device->kobj, &rt_sysfs_attr_group);
-	if (ret) {
-		dev_err(&op->dev, "Unable to create sysfs files\n");
-		goto out_data_debugfs_exit;
-	}
-
-	dev_info(&op->dev, "CARMA FPGA Realtime Data Driver Loaded\n");
-	return 0;
-
-out_data_debugfs_exit:
-	data_debugfs_exit(priv);
-out_misc_deregister:
-	misc_deregister(&priv->miscdev);
-out_irq_dispose_mapping:
-	irq_dispose_mapping(priv->irq);
-out_release_dma:
-	dma_release_channel(priv->chan);
-out_unmap_regs:
-	iounmap(priv->regs);
-out_free_priv:
-	kref_put(&priv->ref, fpga_device_release);
-out_return:
-	return ret;
-}
-
-static int data_of_remove(struct platform_device *op)
-{
-	struct fpga_device *priv = platform_get_drvdata(op);
-	struct device *this_device = priv->miscdev.this_device;
-
-	/* remove all sysfs files, now the device cannot be re-enabled */
-	sysfs_remove_group(&this_device->kobj, &rt_sysfs_attr_group);
-
-	/* remove all debugfs files */
-	data_debugfs_exit(priv);
-
-	/* disable the device from generating data */
-	data_device_disable(priv);
-
-	/* remove the character device to stop new readers from appearing */
-	misc_deregister(&priv->miscdev);
-
-	/* cleanup everything not needed by readers */
-	irq_dispose_mapping(priv->irq);
-	dma_release_channel(priv->chan);
-	iounmap(priv->regs);
-
-	/* release our reference */
-	kref_put(&priv->ref, fpga_device_release);
-	return 0;
-}
-
-static const struct of_device_id data_of_match[] = {
-	{ .compatible = "carma,carma-fpga", },
-	{},
-};
-
-static struct platform_driver data_of_driver = {
-	.probe		= data_of_probe,
-	.remove		= data_of_remove,
-	.driver		= {
-		.name		= drv_name,
-		.of_match_table	= data_of_match,
-	},
-};
-
-module_platform_driver(data_of_driver);
-
-MODULE_AUTHOR("Ira W. Snyder <iws@ovro.caltech.edu>");
-MODULE_DESCRIPTION("CARMA DATA-FPGA Access Driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/misc/cxl/Kconfig b/drivers/misc/cxl/Kconfig
index a990b39b4dfb..b6db9ebd52c2 100644
--- a/drivers/misc/cxl/Kconfig
+++ b/drivers/misc/cxl/Kconfig
@@ -7,10 +7,15 @@ config CXL_BASE
 	default n
 	select PPC_COPRO_BASE
 
+config CXL_KERNEL_API
+	bool
+	default n
+
 config CXL
 	tristate "Support for IBM Coherent Accelerators (CXL)"
 	depends on PPC_POWERNV && PCI_MSI
 	select CXL_BASE
+	select CXL_KERNEL_API
 	default m
 	help
 	  Select this option to enable driver support for IBM Coherent
diff --git a/drivers/misc/cxl/Makefile b/drivers/misc/cxl/Makefile
index edb494d3ff27..14e3f8219a11 100644
--- a/drivers/misc/cxl/Makefile
+++ b/drivers/misc/cxl/Makefile
@@ -1,4 +1,6 @@
-cxl-y				+= main.o file.o irq.o fault.o native.o context.o sysfs.o debugfs.o pci.o trace.o
+cxl-y				+= main.o file.o irq.o fault.o native.o
+cxl-y				+= context.o sysfs.o debugfs.o pci.o trace.o
+cxl-y				+= vphb.o api.o
 obj-$(CONFIG_CXL)		+= cxl.o
 obj-$(CONFIG_CXL_BASE)		+= base.o
 
diff --git a/drivers/misc/cxl/api.c b/drivers/misc/cxl/api.c
new file mode 100644
index 000000000000..729e0851167d
--- /dev/null
+++ b/drivers/misc/cxl/api.c
@@ -0,0 +1,329 @@
+/*
+ * 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.
+ */
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/anon_inodes.h>
+#include <linux/file.h>
+#include <misc/cxl.h>
+
+#include "cxl.h"
+
+struct cxl_context *cxl_dev_context_init(struct pci_dev *dev)
+{
+	struct cxl_afu *afu;
+	struct cxl_context  *ctx;
+	int rc;
+
+	afu = cxl_pci_to_afu(dev);
+
+	get_device(&afu->dev);
+	ctx = cxl_context_alloc();
+	if (IS_ERR(ctx))
+		return ctx;
+
+	/* Make it a slave context.  We can promote it later? */
+	rc = cxl_context_init(ctx, afu, false, NULL);
+	if (rc) {
+		kfree(ctx);
+		put_device(&afu->dev);
+		return ERR_PTR(-ENOMEM);
+	}
+	cxl_assign_psn_space(ctx);
+
+	return ctx;
+}
+EXPORT_SYMBOL_GPL(cxl_dev_context_init);
+
+struct cxl_context *cxl_get_context(struct pci_dev *dev)
+{
+	return dev->dev.archdata.cxl_ctx;
+}
+EXPORT_SYMBOL_GPL(cxl_get_context);
+
+struct device *cxl_get_phys_dev(struct pci_dev *dev)
+{
+	struct cxl_afu *afu;
+
+	afu = cxl_pci_to_afu(dev);
+
+	return afu->adapter->dev.parent;
+}
+EXPORT_SYMBOL_GPL(cxl_get_phys_dev);
+
+int cxl_release_context(struct cxl_context *ctx)
+{
+	if (ctx->status != CLOSED)
+		return -EBUSY;
+
+	put_device(&ctx->afu->dev);
+
+	cxl_context_free(ctx);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cxl_release_context);
+
+int cxl_allocate_afu_irqs(struct cxl_context *ctx, int num)
+{
+	if (num == 0)
+		num = ctx->afu->pp_irqs;
+	return afu_allocate_irqs(ctx, num);
+}
+EXPORT_SYMBOL_GPL(cxl_allocate_afu_irqs);
+
+void cxl_free_afu_irqs(struct cxl_context *ctx)
+{
+	cxl_release_irq_ranges(&ctx->irqs, ctx->afu->adapter);
+}
+EXPORT_SYMBOL_GPL(cxl_free_afu_irqs);
+
+static irq_hw_number_t cxl_find_afu_irq(struct cxl_context *ctx, int num)
+{
+	__u16 range;
+	int r;
+
+	WARN_ON(num == 0);
+
+	for (r = 0; r < CXL_IRQ_RANGES; r++) {
+		range = ctx->irqs.range[r];
+		if (num < range) {
+			return ctx->irqs.offset[r] + num;
+		}
+		num -= range;
+	}
+	return 0;
+}
+
+int cxl_map_afu_irq(struct cxl_context *ctx, int num,
+		    irq_handler_t handler, void *cookie, char *name)
+{
+	irq_hw_number_t hwirq;
+
+	/*
+	 * Find interrupt we are to register.
+	 */
+	hwirq = cxl_find_afu_irq(ctx, num);
+	if (!hwirq)
+		return -ENOENT;
+
+	return cxl_map_irq(ctx->afu->adapter, hwirq, handler, cookie, name);
+}
+EXPORT_SYMBOL_GPL(cxl_map_afu_irq);
+
+void cxl_unmap_afu_irq(struct cxl_context *ctx, int num, void *cookie)
+{
+	irq_hw_number_t hwirq;
+	unsigned int virq;
+
+	hwirq = cxl_find_afu_irq(ctx, num);
+	if (!hwirq)
+		return;
+
+	virq = irq_find_mapping(NULL, hwirq);
+	if (virq)
+		cxl_unmap_irq(virq, cookie);
+}
+EXPORT_SYMBOL_GPL(cxl_unmap_afu_irq);
+
+/*
+ * Start a context
+ * Code here similar to afu_ioctl_start_work().
+ */
+int cxl_start_context(struct cxl_context *ctx, u64 wed,
+		      struct task_struct *task)
+{
+	int rc = 0;
+	bool kernel = true;
+
+	pr_devel("%s: pe: %i\n", __func__, ctx->pe);
+
+	mutex_lock(&ctx->status_mutex);
+	if (ctx->status == STARTED)
+		goto out; /* already started */
+
+	if (task) {
+		ctx->pid = get_task_pid(task, PIDTYPE_PID);
+		get_pid(ctx->pid);
+		kernel = false;
+	}
+
+	cxl_ctx_get();
+
+	if ((rc = cxl_attach_process(ctx, kernel, wed , 0))) {
+		put_pid(ctx->pid);
+		cxl_ctx_put();
+		goto out;
+	}
+
+	ctx->status = STARTED;
+out:
+	mutex_unlock(&ctx->status_mutex);
+	return rc;
+}
+EXPORT_SYMBOL_GPL(cxl_start_context);
+
+int cxl_process_element(struct cxl_context *ctx)
+{
+	return ctx->pe;
+}
+EXPORT_SYMBOL_GPL(cxl_process_element);
+
+/* Stop a context.  Returns 0 on success, otherwise -Errno */
+int cxl_stop_context(struct cxl_context *ctx)
+{
+	return __detach_context(ctx);
+}
+EXPORT_SYMBOL_GPL(cxl_stop_context);
+
+void cxl_set_master(struct cxl_context *ctx)
+{
+	ctx->master = true;
+	cxl_assign_psn_space(ctx);
+}
+EXPORT_SYMBOL_GPL(cxl_set_master);
+
+/* wrappers around afu_* file ops which are EXPORTED */
+int cxl_fd_open(struct inode *inode, struct file *file)
+{
+	return afu_open(inode, file);
+}
+EXPORT_SYMBOL_GPL(cxl_fd_open);
+int cxl_fd_release(struct inode *inode, struct file *file)
+{
+	return afu_release(inode, file);
+}
+EXPORT_SYMBOL_GPL(cxl_fd_release);
+long cxl_fd_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	return afu_ioctl(file, cmd, arg);
+}
+EXPORT_SYMBOL_GPL(cxl_fd_ioctl);
+int cxl_fd_mmap(struct file *file, struct vm_area_struct *vm)
+{
+	return afu_mmap(file, vm);
+}
+EXPORT_SYMBOL_GPL(cxl_fd_mmap);
+unsigned int cxl_fd_poll(struct file *file, struct poll_table_struct *poll)
+{
+	return afu_poll(file, poll);
+}
+EXPORT_SYMBOL_GPL(cxl_fd_poll);
+ssize_t cxl_fd_read(struct file *file, char __user *buf, size_t count,
+			loff_t *off)
+{
+	return afu_read(file, buf, count, off);
+}
+EXPORT_SYMBOL_GPL(cxl_fd_read);
+
+#define PATCH_FOPS(NAME) if (!fops->NAME) fops->NAME = afu_fops.NAME
+
+/* Get a struct file and fd for a context and attach the ops */
+struct file *cxl_get_fd(struct cxl_context *ctx, struct file_operations *fops,
+			int *fd)
+{
+	struct file *file;
+	int rc, flags, fdtmp;
+
+	flags = O_RDWR | O_CLOEXEC;
+
+	/* This code is similar to anon_inode_getfd() */
+	rc = get_unused_fd_flags(flags);
+	if (rc < 0)
+		return ERR_PTR(rc);
+	fdtmp = rc;
+
+	/*
+	 * Patch the file ops.  Needs to be careful that this is rentrant safe.
+	 */
+	if (fops) {
+		PATCH_FOPS(open);
+		PATCH_FOPS(poll);
+		PATCH_FOPS(read);
+		PATCH_FOPS(release);
+		PATCH_FOPS(unlocked_ioctl);
+		PATCH_FOPS(compat_ioctl);
+		PATCH_FOPS(mmap);
+	} else /* use default ops */
+		fops = (struct file_operations *)&afu_fops;
+
+	file = anon_inode_getfile("cxl", fops, ctx, flags);
+	if (IS_ERR(file))
+		put_unused_fd(fdtmp);
+	*fd = fdtmp;
+	return file;
+}
+EXPORT_SYMBOL_GPL(cxl_get_fd);
+
+struct cxl_context *cxl_fops_get_context(struct file *file)
+{
+	return file->private_data;
+}
+EXPORT_SYMBOL_GPL(cxl_fops_get_context);
+
+int cxl_start_work(struct cxl_context *ctx,
+		   struct cxl_ioctl_start_work *work)
+{
+	int rc;
+
+	/* code taken from afu_ioctl_start_work */
+	if (!(work->flags & CXL_START_WORK_NUM_IRQS))
+		work->num_interrupts = ctx->afu->pp_irqs;
+	else if ((work->num_interrupts < ctx->afu->pp_irqs) ||
+		 (work->num_interrupts > ctx->afu->irqs_max)) {
+		return -EINVAL;
+	}
+
+	rc = afu_register_irqs(ctx, work->num_interrupts);
+	if (rc)
+		return rc;
+
+	rc = cxl_start_context(ctx, work->work_element_descriptor, current);
+	if (rc < 0) {
+		afu_release_irqs(ctx, ctx);
+		return rc;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cxl_start_work);
+
+void __iomem *cxl_psa_map(struct cxl_context *ctx)
+{
+	struct cxl_afu *afu = ctx->afu;
+	int rc;
+
+	rc = cxl_afu_check_and_enable(afu);
+	if (rc)
+		return NULL;
+
+	pr_devel("%s: psn_phys%llx size:%llx\n",
+		 __func__, afu->psn_phys, afu->adapter->ps_size);
+	return ioremap(ctx->psn_phys, ctx->psn_size);
+}
+EXPORT_SYMBOL_GPL(cxl_psa_map);
+
+void cxl_psa_unmap(void __iomem *addr)
+{
+	iounmap(addr);
+}
+EXPORT_SYMBOL_GPL(cxl_psa_unmap);
+
+int cxl_afu_reset(struct cxl_context *ctx)
+{
+	struct cxl_afu *afu = ctx->afu;
+	int rc;
+
+	rc = __cxl_afu_reset(afu);
+	if (rc)
+		return rc;
+
+	return cxl_afu_check_and_enable(afu);
+}
+EXPORT_SYMBOL_GPL(cxl_afu_reset);
diff --git a/drivers/misc/cxl/base.c b/drivers/misc/cxl/base.c
index 0654ad83675e..a9f0dd3255a2 100644
--- a/drivers/misc/cxl/base.c
+++ b/drivers/misc/cxl/base.c
@@ -10,7 +10,7 @@
 #include <linux/module.h>
 #include <linux/rcupdate.h>
 #include <asm/errno.h>
-#include <misc/cxl.h>
+#include <misc/cxl-base.h>
 #include "cxl.h"
 
 /* protected by rcu */
diff --git a/drivers/misc/cxl/context.c b/drivers/misc/cxl/context.c
index d1b55fe62817..1287148629c0 100644
--- a/drivers/misc/cxl/context.c
+++ b/drivers/misc/cxl/context.c
@@ -113,11 +113,11 @@ static int cxl_mmap_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 
 	if (ctx->afu->current_mode == CXL_MODE_DEDICATED) {
 		area = ctx->afu->psn_phys;
-		if (offset > ctx->afu->adapter->ps_size)
+		if (offset >= ctx->afu->adapter->ps_size)
 			return VM_FAULT_SIGBUS;
 	} else {
 		area = ctx->psn_phys;
-		if (offset > ctx->psn_size)
+		if (offset >= ctx->psn_size)
 			return VM_FAULT_SIGBUS;
 	}
 
@@ -145,8 +145,16 @@ static const struct vm_operations_struct cxl_mmap_vmops = {
  */
 int cxl_context_iomap(struct cxl_context *ctx, struct vm_area_struct *vma)
 {
+	u64 start = vma->vm_pgoff << PAGE_SHIFT;
 	u64 len = vma->vm_end - vma->vm_start;
-	len = min(len, ctx->psn_size);
+
+	if (ctx->afu->current_mode == CXL_MODE_DEDICATED) {
+		if (start + len > ctx->afu->adapter->ps_size)
+			return -EINVAL;
+	} else {
+		if (start + len > ctx->psn_size)
+			return -EINVAL;
+	}
 
 	if (ctx->afu->current_mode != CXL_MODE_DEDICATED) {
 		/* make sure there is a valid per process space for this AFU */
@@ -174,7 +182,7 @@ int cxl_context_iomap(struct cxl_context *ctx, struct vm_area_struct *vma)
  * return until all outstanding interrupts for this context have completed. The
  * hardware should no longer access *ctx after this has returned.
  */
-static void __detach_context(struct cxl_context *ctx)
+int __detach_context(struct cxl_context *ctx)
 {
 	enum cxl_context_status status;
 
@@ -183,12 +191,13 @@ static void __detach_context(struct cxl_context *ctx)
 	ctx->status = CLOSED;
 	mutex_unlock(&ctx->status_mutex);
 	if (status != STARTED)
-		return;
+		return -EBUSY;
 
 	WARN_ON(cxl_detach_process(ctx));
-	afu_release_irqs(ctx);
 	flush_work(&ctx->fault_work); /* Only needed for dedicated process */
-	wake_up_all(&ctx->wq);
+	put_pid(ctx->pid);
+	cxl_ctx_put();
+	return 0;
 }
 
 /*
@@ -199,7 +208,14 @@ static void __detach_context(struct cxl_context *ctx)
  */
 void cxl_context_detach(struct cxl_context *ctx)
 {
-	__detach_context(ctx);
+	int rc;
+
+	rc = __detach_context(ctx);
+	if (rc)
+		return;
+
+	afu_release_irqs(ctx, ctx);
+	wake_up_all(&ctx->wq);
 }
 
 /*
@@ -216,7 +232,7 @@ void cxl_context_detach_all(struct cxl_afu *afu)
 		 * Anything done in here needs to be setup before the IDR is
 		 * created and torn down after the IDR removed
 		 */
-		__detach_context(ctx);
+		cxl_context_detach(ctx);
 
 		/*
 		 * We are force detaching - remove any active PSA mappings so
@@ -232,16 +248,20 @@ void cxl_context_detach_all(struct cxl_afu *afu)
 	mutex_unlock(&afu->contexts_lock);
 }
 
-void cxl_context_free(struct cxl_context *ctx)
+static void reclaim_ctx(struct rcu_head *rcu)
 {
-	mutex_lock(&ctx->afu->contexts_lock);
-	idr_remove(&ctx->afu->contexts_idr, ctx->pe);
-	mutex_unlock(&ctx->afu->contexts_lock);
-	synchronize_rcu();
+	struct cxl_context *ctx = container_of(rcu, struct cxl_context, rcu);
 
 	free_page((u64)ctx->sstp);
 	ctx->sstp = NULL;
 
-	put_pid(ctx->pid);
 	kfree(ctx);
 }
+
+void cxl_context_free(struct cxl_context *ctx)
+{
+	mutex_lock(&ctx->afu->contexts_lock);
+	idr_remove(&ctx->afu->contexts_idr, ctx->pe);
+	mutex_unlock(&ctx->afu->contexts_lock);
+	call_rcu(&ctx->rcu, reclaim_ctx);
+}
diff --git a/drivers/misc/cxl/cxl.h b/drivers/misc/cxl/cxl.h
index a1cee4767ec6..4fd66cabde1e 100644
--- a/drivers/misc/cxl/cxl.h
+++ b/drivers/misc/cxl/cxl.h
@@ -18,10 +18,11 @@
 #include <linux/pid.h>
 #include <linux/io.h>
 #include <linux/pci.h>
+#include <linux/fs.h>
 #include <asm/cputable.h>
 #include <asm/mmu.h>
 #include <asm/reg.h>
-#include <misc/cxl.h>
+#include <misc/cxl-base.h>
 
 #include <uapi/misc/cxl.h>
 
@@ -315,8 +316,6 @@ static const cxl_p2n_reg_t CXL_PSL_WED_An     = {0x0A0};
 #define CXL_MAX_SLICES 4
 #define MAX_AFU_MMIO_REGS 3
 
-#define CXL_MODE_DEDICATED   0x1
-#define CXL_MODE_DIRECTED    0x2
 #define CXL_MODE_TIME_SLICED 0x4
 #define CXL_SUPPORTED_MODES (CXL_MODE_DEDICATED | CXL_MODE_DIRECTED)
 
@@ -362,6 +361,10 @@ struct cxl_afu {
 	struct mutex spa_mutex;
 	spinlock_t afu_cntl_lock;
 
+	/* AFU error buffer fields and bin attribute for sysfs */
+	u64 eb_len, eb_offset;
+	struct bin_attribute attr_eb;
+
 	/*
 	 * Only the first part of the SPA is used for the process element
 	 * linked list. The only other part that software needs to worry about
@@ -375,6 +378,9 @@ struct cxl_afu {
 	int spa_max_procs;
 	unsigned int psl_virq;
 
+	/* pointer to the vphb */
+	struct pci_controller *phb;
+
 	int pp_irqs;
 	int irqs_max;
 	int num_procs;
@@ -455,6 +461,8 @@ struct cxl_context {
 	bool pending_irq;
 	bool pending_fault;
 	bool pending_afu_err;
+
+	struct rcu_head rcu;
 };
 
 struct cxl {
@@ -563,6 +571,9 @@ static inline void __iomem *_cxl_p2n_addr(struct cxl_afu *afu, cxl_p2n_reg_t reg
 u16 cxl_afu_cr_read16(struct cxl_afu *afu, int cr, u64 off);
 u8 cxl_afu_cr_read8(struct cxl_afu *afu, int cr, u64 off);
 
+ssize_t cxl_afu_read_err_buffer(struct cxl_afu *afu, char *buf,
+				loff_t off, size_t count);
+
 
 struct cxl_calls {
 	void (*cxl_slbia)(struct mm_struct *mm);
@@ -606,7 +617,7 @@ void cxl_release_psl_err_irq(struct cxl *adapter);
 int cxl_register_serr_irq(struct cxl_afu *afu);
 void cxl_release_serr_irq(struct cxl_afu *afu);
 int afu_register_irqs(struct cxl_context *ctx, u32 count);
-void afu_release_irqs(struct cxl_context *ctx);
+void afu_release_irqs(struct cxl_context *ctx, void *cookie);
 irqreturn_t cxl_slice_irq_err(int irq, void *data);
 
 int cxl_debugfs_init(void);
@@ -629,6 +640,10 @@ 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);
+unsigned int cxl_map_irq(struct cxl *adapter, irq_hw_number_t hwirq,
+			 irq_handler_t handler, void *cookie, const char *name);
+void cxl_unmap_irq(unsigned int virq, void *cookie);
+int __detach_context(struct cxl_context *ctx);
 
 /* This matches the layout of the H_COLLECT_CA_INT_INFO retbuf */
 struct cxl_irq_info {
@@ -642,6 +657,7 @@ struct cxl_irq_info {
 	u64 padding[3]; /* to match the expected retbuf size for plpar_hcall9 */
 };
 
+void cxl_assign_psn_space(struct cxl_context *ctx);
 int cxl_attach_process(struct cxl_context *ctx, bool kernel, u64 wed,
 			    u64 amr);
 int cxl_detach_process(struct cxl_context *ctx);
@@ -653,11 +669,23 @@ int cxl_check_error(struct cxl_afu *afu);
 int cxl_afu_slbia(struct cxl_afu *afu);
 int cxl_tlb_slb_invalidate(struct cxl *adapter);
 int cxl_afu_disable(struct cxl_afu *afu);
-int cxl_afu_reset(struct cxl_afu *afu);
+int __cxl_afu_reset(struct cxl_afu *afu);
+int cxl_afu_check_and_enable(struct cxl_afu *afu);
 int cxl_psl_purge(struct cxl_afu *afu);
 
 void cxl_stop_trace(struct cxl *cxl);
+int cxl_pci_vphb_add(struct cxl_afu *afu);
+void cxl_pci_vphb_remove(struct cxl_afu *afu);
 
 extern struct pci_driver cxl_pci_driver;
+int afu_allocate_irqs(struct cxl_context *ctx, u32 count);
+
+int afu_open(struct inode *inode, struct file *file);
+int afu_release(struct inode *inode, struct file *file);
+long afu_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
+int afu_mmap(struct file *file, struct vm_area_struct *vm);
+unsigned int afu_poll(struct file *file, struct poll_table_struct *poll);
+ssize_t afu_read(struct file *file, char __user *buf, size_t count, loff_t *off);
+extern const struct file_operations afu_fops;
 
 #endif
diff --git a/drivers/misc/cxl/fault.c b/drivers/misc/cxl/fault.c
index 5286b8b704f5..25a5418c55cb 100644
--- a/drivers/misc/cxl/fault.c
+++ b/drivers/misc/cxl/fault.c
@@ -172,8 +172,8 @@ void cxl_handle_fault(struct work_struct *fault_work)
 		container_of(fault_work, struct cxl_context, fault_work);
 	u64 dsisr = ctx->dsisr;
 	u64 dar = ctx->dar;
-	struct task_struct *task;
-	struct mm_struct *mm;
+	struct task_struct *task = NULL;
+	struct mm_struct *mm = NULL;
 
 	if (cxl_p2n_read(ctx->afu, CXL_PSL_DSISR_An) != dsisr ||
 	    cxl_p2n_read(ctx->afu, CXL_PSL_DAR_An) != dar ||
@@ -194,17 +194,19 @@ void cxl_handle_fault(struct work_struct *fault_work)
 	pr_devel("CXL BOTTOM HALF handling fault for afu pe: %i. "
 		"DSISR: %#llx DAR: %#llx\n", ctx->pe, dsisr, dar);
 
-	if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
-		pr_devel("cxl_handle_fault unable to get task %i\n",
-			 pid_nr(ctx->pid));
-		cxl_ack_ae(ctx);
-		return;
-	}
-	if (!(mm = get_task_mm(task))) {
-		pr_devel("cxl_handle_fault unable to get mm %i\n",
-			 pid_nr(ctx->pid));
-		cxl_ack_ae(ctx);
-		goto out;
+	if (!ctx->kernel) {
+		if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
+			pr_devel("cxl_handle_fault unable to get task %i\n",
+				 pid_nr(ctx->pid));
+			cxl_ack_ae(ctx);
+			return;
+		}
+		if (!(mm = get_task_mm(task))) {
+			pr_devel("cxl_handle_fault unable to get mm %i\n",
+				 pid_nr(ctx->pid));
+			cxl_ack_ae(ctx);
+			goto out;
+		}
 	}
 
 	if (dsisr & CXL_PSL_DSISR_An_DS)
@@ -214,9 +216,11 @@ void cxl_handle_fault(struct work_struct *fault_work)
 	else
 		WARN(1, "cxl_handle_fault has nothing to handle\n");
 
-	mmput(mm);
+	if (mm)
+		mmput(mm);
 out:
-	put_task_struct(task);
+	if (task)
+		put_task_struct(task);
 }
 
 static void cxl_prefault_one(struct cxl_context *ctx, u64 ea)
diff --git a/drivers/misc/cxl/file.c b/drivers/misc/cxl/file.c
index 2364bcadb9a9..e3f4b69527a9 100644
--- a/drivers/misc/cxl/file.c
+++ b/drivers/misc/cxl/file.c
@@ -96,7 +96,8 @@ err_put_adapter:
 	put_device(&adapter->dev);
 	return rc;
 }
-static int afu_open(struct inode *inode, struct file *file)
+
+int afu_open(struct inode *inode, struct file *file)
 {
 	return __afu_open(inode, file, false);
 }
@@ -106,7 +107,7 @@ static int afu_master_open(struct inode *inode, struct file *file)
 	return __afu_open(inode, file, true);
 }
 
-static int afu_release(struct inode *inode, struct file *file)
+int afu_release(struct inode *inode, struct file *file)
 {
 	struct cxl_context *ctx = file->private_data;
 
@@ -128,7 +129,6 @@ static int afu_release(struct inode *inode, struct file *file)
 	 */
 	cxl_context_free(ctx);
 
-	cxl_ctx_put();
 	return 0;
 }
 
@@ -191,7 +191,7 @@ static long afu_ioctl_start_work(struct cxl_context *ctx,
 
 	if ((rc = cxl_attach_process(ctx, false, work.work_element_descriptor,
 				     amr))) {
-		afu_release_irqs(ctx);
+		afu_release_irqs(ctx, ctx);
 		goto out;
 	}
 
@@ -212,7 +212,26 @@ static long afu_ioctl_process_element(struct cxl_context *ctx,
 	return 0;
 }
 
-static long afu_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+static long afu_ioctl_get_afu_id(struct cxl_context *ctx,
+				 struct cxl_afu_id __user *upafuid)
+{
+	struct cxl_afu_id afuid = { 0 };
+
+	afuid.card_id = ctx->afu->adapter->adapter_num;
+	afuid.afu_offset = ctx->afu->slice;
+	afuid.afu_mode = ctx->afu->current_mode;
+
+	/* set the flag bit in case the afu is a slave */
+	if (ctx->afu->current_mode == CXL_MODE_DIRECTED && !ctx->master)
+		afuid.flags |= CXL_AFUID_FLAG_SLAVE;
+
+	if (copy_to_user(upafuid, &afuid, sizeof(afuid)))
+		return -EFAULT;
+
+	return 0;
+}
+
+long afu_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	struct cxl_context *ctx = file->private_data;
 
@@ -225,17 +244,20 @@ static long afu_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		return afu_ioctl_start_work(ctx, (struct cxl_ioctl_start_work __user *)arg);
 	case CXL_IOCTL_GET_PROCESS_ELEMENT:
 		return afu_ioctl_process_element(ctx, (__u32 __user *)arg);
+	case CXL_IOCTL_GET_AFU_ID:
+		return afu_ioctl_get_afu_id(ctx, (struct cxl_afu_id __user *)
+					    arg);
 	}
 	return -EINVAL;
 }
 
-static long afu_compat_ioctl(struct file *file, unsigned int cmd,
+long afu_compat_ioctl(struct file *file, unsigned int cmd,
 			     unsigned long arg)
 {
 	return afu_ioctl(file, cmd, arg);
 }
 
-static int afu_mmap(struct file *file, struct vm_area_struct *vm)
+int afu_mmap(struct file *file, struct vm_area_struct *vm)
 {
 	struct cxl_context *ctx = file->private_data;
 
@@ -246,7 +268,7 @@ static int afu_mmap(struct file *file, struct vm_area_struct *vm)
 	return cxl_context_iomap(ctx, vm);
 }
 
-static unsigned int afu_poll(struct file *file, struct poll_table_struct *poll)
+unsigned int afu_poll(struct file *file, struct poll_table_struct *poll)
 {
 	struct cxl_context *ctx = file->private_data;
 	int mask = 0;
@@ -278,7 +300,7 @@ static inline int ctx_event_pending(struct cxl_context *ctx)
 	    ctx->pending_afu_err || (ctx->status == CLOSED));
 }
 
-static ssize_t afu_read(struct file *file, char __user *buf, size_t count,
+ssize_t afu_read(struct file *file, char __user *buf, size_t count,
 			loff_t *off)
 {
 	struct cxl_context *ctx = file->private_data;
@@ -359,7 +381,11 @@ out:
 	return rc;
 }
 
-static const struct file_operations afu_fops = {
+/* 
+ * Note: if this is updated, we need to update api.c to patch the new ones in
+ * too
+ */
+const struct file_operations afu_fops = {
 	.owner		= THIS_MODULE,
 	.open           = afu_open,
 	.poll		= afu_poll,
@@ -370,7 +396,7 @@ static const struct file_operations afu_fops = {
 	.mmap           = afu_mmap,
 };
 
-static const struct file_operations afu_master_fops = {
+const struct file_operations afu_master_fops = {
 	.owner		= THIS_MODULE,
 	.open           = afu_master_open,
 	.poll		= afu_poll,
diff --git a/drivers/misc/cxl/irq.c b/drivers/misc/cxl/irq.c
index c8929c526691..680cd263436d 100644
--- a/drivers/misc/cxl/irq.c
+++ b/drivers/misc/cxl/irq.c
@@ -14,7 +14,7 @@
 #include <linux/slab.h>
 #include <linux/pid.h>
 #include <asm/cputable.h>
-#include <misc/cxl.h>
+#include <misc/cxl-base.h>
 
 #include "cxl.h"
 #include "trace.h"
@@ -416,9 +416,8 @@ void afu_irq_name_free(struct cxl_context *ctx)
 	}
 }
 
-int afu_register_irqs(struct cxl_context *ctx, u32 count)
+int afu_allocate_irqs(struct cxl_context *ctx, u32 count)
 {
-	irq_hw_number_t hwirq;
 	int rc, r, i, j = 1;
 	struct cxl_irq_name *irq_name;
 
@@ -458,6 +457,18 @@ int afu_register_irqs(struct cxl_context *ctx, u32 count)
 			j++;
 		}
 	}
+	return 0;
+
+out:
+	afu_irq_name_free(ctx);
+	return -ENOMEM;
+}
+
+void afu_register_hwirqs(struct cxl_context *ctx)
+{
+	irq_hw_number_t hwirq;
+	struct cxl_irq_name *irq_name;
+	int r,i;
 
 	/* 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);
@@ -469,15 +480,21 @@ int afu_register_irqs(struct cxl_context *ctx, u32 count)
 			irq_name = list_next_entry(irq_name, list);
 		}
 	}
+}
 
-	return 0;
+int afu_register_irqs(struct cxl_context *ctx, u32 count)
+{
+	int rc;
 
-out:
-	afu_irq_name_free(ctx);
-	return -ENOMEM;
-}
+	rc = afu_allocate_irqs(ctx, count);
+	if (rc)
+		return rc;
+
+	afu_register_hwirqs(ctx);
+	return 0;
+ }
 
-void afu_release_irqs(struct cxl_context *ctx)
+void afu_release_irqs(struct cxl_context *ctx, void *cookie)
 {
 	irq_hw_number_t hwirq;
 	unsigned int virq;
@@ -488,7 +505,7 @@ void afu_release_irqs(struct cxl_context *ctx)
 		for (i = 0; i < ctx->irqs.range[r]; hwirq++, i++) {
 			virq = irq_find_mapping(NULL, hwirq);
 			if (virq)
-				cxl_unmap_irq(virq, ctx);
+				cxl_unmap_irq(virq, cookie);
 		}
 	}
 
diff --git a/drivers/misc/cxl/main.c b/drivers/misc/cxl/main.c
index 8ccddceead66..4a164ab8b35a 100644
--- a/drivers/misc/cxl/main.c
+++ b/drivers/misc/cxl/main.c
@@ -20,7 +20,7 @@
 #include <linux/idr.h>
 #include <linux/pci.h>
 #include <asm/cputable.h>
-#include <misc/cxl.h>
+#include <misc/cxl-base.h>
 
 #include "cxl.h"
 #include "trace.h"
@@ -73,7 +73,7 @@ static inline void cxl_slbia_core(struct mm_struct *mm)
 		spin_lock(&adapter->afu_list_lock);
 		for (slice = 0; slice < adapter->slices; slice++) {
 			afu = adapter->afu[slice];
-			if (!afu->enabled)
+			if (!afu || !afu->enabled)
 				continue;
 			rcu_read_lock();
 			idr_for_each_entry(&afu->contexts_idr, ctx, id)
diff --git a/drivers/misc/cxl/native.c b/drivers/misc/cxl/native.c
index 29185fc61276..10567f245818 100644
--- a/drivers/misc/cxl/native.c
+++ b/drivers/misc/cxl/native.c
@@ -15,7 +15,7 @@
 #include <linux/mm.h>
 #include <linux/uaccess.h>
 #include <asm/synch.h>
-#include <misc/cxl.h>
+#include <misc/cxl-base.h>
 
 #include "cxl.h"
 #include "trace.h"
@@ -73,7 +73,7 @@ int cxl_afu_disable(struct cxl_afu *afu)
 }
 
 /* This will disable as well as reset */
-int cxl_afu_reset(struct cxl_afu *afu)
+int __cxl_afu_reset(struct cxl_afu *afu)
 {
 	pr_devel("AFU reset request\n");
 
@@ -83,7 +83,7 @@ int cxl_afu_reset(struct cxl_afu *afu)
 			   false);
 }
 
-static int afu_check_and_enable(struct cxl_afu *afu)
+int cxl_afu_check_and_enable(struct cxl_afu *afu)
 {
 	if (afu->enabled)
 		return 0;
@@ -379,7 +379,7 @@ static int remove_process_element(struct cxl_context *ctx)
 }
 
 
-static void assign_psn_space(struct cxl_context *ctx)
+void cxl_assign_psn_space(struct cxl_context *ctx)
 {
 	if (!ctx->afu->pp_size || ctx->master) {
 		ctx->psn_phys = ctx->afu->psn_phys;
@@ -430,34 +430,46 @@ err:
 #define set_endian(sr) ((sr) &= ~(CXL_PSL_SR_An_LE))
 #endif
 
+static u64 calculate_sr(struct cxl_context *ctx)
+{
+	u64 sr = 0;
+
+	if (ctx->master)
+		sr |= CXL_PSL_SR_An_MP;
+	if (mfspr(SPRN_LPCR) & LPCR_TC)
+		sr |= CXL_PSL_SR_An_TC;
+	if (ctx->kernel) {
+		sr |= CXL_PSL_SR_An_R | (mfmsr() & MSR_SF);
+		sr |= CXL_PSL_SR_An_HV;
+	} else {
+		sr |= CXL_PSL_SR_An_PR | CXL_PSL_SR_An_R;
+		set_endian(sr);
+		sr &= ~(CXL_PSL_SR_An_HV);
+		if (!test_tsk_thread_flag(current, TIF_32BIT))
+			sr |= CXL_PSL_SR_An_SF;
+	}
+	return sr;
+}
+
 static int attach_afu_directed(struct cxl_context *ctx, u64 wed, u64 amr)
 {
-	u64 sr;
+	u32 pid;
 	int r, result;
 
-	assign_psn_space(ctx);
+	cxl_assign_psn_space(ctx);
 
 	ctx->elem->ctxtime = 0; /* disable */
 	ctx->elem->lpid = cpu_to_be32(mfspr(SPRN_LPID));
 	ctx->elem->haurp = 0; /* disable */
 	ctx->elem->sdr = cpu_to_be64(mfspr(SPRN_SDR1));
 
-	sr = 0;
-	if (ctx->master)
-		sr |= CXL_PSL_SR_An_MP;
-	if (mfspr(SPRN_LPCR) & LPCR_TC)
-		sr |= CXL_PSL_SR_An_TC;
-	/* HV=0, PR=1, R=1 for userspace
-	 * For kernel contexts: this would need to change
-	 */
-	sr |= CXL_PSL_SR_An_PR | CXL_PSL_SR_An_R;
-	set_endian(sr);
-	sr &= ~(CXL_PSL_SR_An_HV);
-	if (!test_tsk_thread_flag(current, TIF_32BIT))
-		sr |= CXL_PSL_SR_An_SF;
-	ctx->elem->common.pid = cpu_to_be32(current->pid);
+	pid = current->pid;
+	if (ctx->kernel)
+		pid = 0;
 	ctx->elem->common.tid = 0;
-	ctx->elem->sr = cpu_to_be64(sr);
+	ctx->elem->common.pid = cpu_to_be32(pid);
+
+	ctx->elem->sr = cpu_to_be64(calculate_sr(ctx));
 
 	ctx->elem->common.csrp = 0; /* disable */
 	ctx->elem->common.aurp0 = 0; /* disable */
@@ -477,7 +489,7 @@ static int attach_afu_directed(struct cxl_context *ctx, u64 wed, u64 amr)
 	ctx->elem->common.wed = cpu_to_be64(wed);
 
 	/* first guy needs to enable */
-	if ((result = afu_check_and_enable(ctx->afu)))
+	if ((result = cxl_afu_check_and_enable(ctx->afu)))
 		return result;
 
 	add_process_element(ctx);
@@ -495,7 +507,7 @@ static int deactivate_afu_directed(struct cxl_afu *afu)
 	cxl_sysfs_afu_m_remove(afu);
 	cxl_chardev_afu_remove(afu);
 
-	cxl_afu_reset(afu);
+	__cxl_afu_reset(afu);
 	cxl_afu_disable(afu);
 	cxl_psl_purge(afu);
 
@@ -530,20 +542,15 @@ static int activate_dedicated_process(struct cxl_afu *afu)
 static int attach_dedicated(struct cxl_context *ctx, u64 wed, u64 amr)
 {
 	struct cxl_afu *afu = ctx->afu;
-	u64 sr;
+	u64 pid;
 	int rc;
 
-	sr = 0;
-	set_endian(sr);
-	if (ctx->master)
-		sr |= CXL_PSL_SR_An_MP;
-	if (mfspr(SPRN_LPCR) & LPCR_TC)
-		sr |= CXL_PSL_SR_An_TC;
-	sr |= CXL_PSL_SR_An_PR | CXL_PSL_SR_An_R;
-	if (!test_tsk_thread_flag(current, TIF_32BIT))
-		sr |= CXL_PSL_SR_An_SF;
-	cxl_p2n_write(afu, CXL_PSL_PID_TID_An, (u64)current->pid << 32);
-	cxl_p1n_write(afu, CXL_PSL_SR_An, sr);
+	pid = (u64)current->pid << 32;
+	if (ctx->kernel)
+		pid = 0;
+	cxl_p2n_write(afu, CXL_PSL_PID_TID_An, pid);
+
+	cxl_p1n_write(afu, CXL_PSL_SR_An, calculate_sr(ctx));
 
 	if ((rc = cxl_write_sstp(afu, ctx->sstp0, ctx->sstp1)))
 		return rc;
@@ -564,9 +571,9 @@ static int attach_dedicated(struct cxl_context *ctx, u64 wed, u64 amr)
 	cxl_p2n_write(afu, CXL_PSL_AMR_An, amr);
 
 	/* master only context for dedicated */
-	assign_psn_space(ctx);
+	cxl_assign_psn_space(ctx);
 
-	if ((rc = cxl_afu_reset(afu)))
+	if ((rc = __cxl_afu_reset(afu)))
 		return rc;
 
 	cxl_p2n_write(afu, CXL_PSL_WED_An, wed);
@@ -629,7 +636,7 @@ int cxl_attach_process(struct cxl_context *ctx, bool kernel, u64 wed, u64 amr)
 
 static inline int detach_process_native_dedicated(struct cxl_context *ctx)
 {
-	cxl_afu_reset(ctx->afu);
+	__cxl_afu_reset(ctx->afu);
 	cxl_afu_disable(ctx->afu);
 	cxl_psl_purge(ctx->afu);
 	return 0;
diff --git a/drivers/misc/cxl/pci.c b/drivers/misc/cxl/pci.c
index 1ef01647265f..32ad09705949 100644
--- a/drivers/misc/cxl/pci.c
+++ b/drivers/misc/cxl/pci.c
@@ -90,6 +90,7 @@
 /* This works a little different than the p1/p2 register accesses to make it
  * easier to pull out individual fields */
 #define AFUD_READ(afu, off)		in_be64(afu->afu_desc_mmio + off)
+#define AFUD_READ_LE(afu, off)		in_le64(afu->afu_desc_mmio + off)
 #define EXTRACT_PPC_BIT(val, bit)	(!!(val & PPC_BIT(bit)))
 #define EXTRACT_PPC_BITS(val, bs, be)	((val & PPC_BITMASK(bs, be)) >> PPC_BITLSHIFT(be))
 
@@ -204,7 +205,7 @@ static void dump_cxl_config_space(struct pci_dev *dev)
 	dev_info(&dev->dev, "p1 regs: %#llx, len: %#llx\n",
 		p1_base(dev), p1_size(dev));
 	dev_info(&dev->dev, "p2 regs: %#llx, len: %#llx\n",
-		p1_base(dev), p2_size(dev));
+		p2_base(dev), p2_size(dev));
 	dev_info(&dev->dev, "BAR 4/5: %#llx, len: %#llx\n",
 		pci_resource_start(dev, 4), pci_resource_len(dev, 4));
 
@@ -286,7 +287,8 @@ static void dump_cxl_config_space(struct pci_dev *dev)
 
 static void dump_afu_descriptor(struct cxl_afu *afu)
 {
-	u64 val;
+	u64 val, afu_cr_num, afu_cr_off, afu_cr_len;
+	int i;
 
 #define show_reg(name, what) \
 	dev_info(&afu->dev, "afu desc: %30s: %#llx\n", name, what)
@@ -296,6 +298,7 @@ static void dump_afu_descriptor(struct cxl_afu *afu)
 	show_reg("num_of_processes", AFUD_NUM_PROCS(val));
 	show_reg("num_of_afu_CRs", AFUD_NUM_CRS(val));
 	show_reg("req_prog_mode", val & 0xffffULL);
+	afu_cr_num = AFUD_NUM_CRS(val);
 
 	val = AFUD_READ(afu, 0x8);
 	show_reg("Reserved", val);
@@ -307,8 +310,10 @@ static void dump_afu_descriptor(struct cxl_afu *afu)
 	val = AFUD_READ_CR(afu);
 	show_reg("Reserved", (val >> (63-7)) & 0xff);
 	show_reg("AFU_CR_len", AFUD_CR_LEN(val));
+	afu_cr_len = AFUD_CR_LEN(val) * 256;
 
 	val = AFUD_READ_CR_OFF(afu);
+	afu_cr_off = val;
 	show_reg("AFU_CR_offset", val);
 
 	val = AFUD_READ_PPPSA(afu);
@@ -325,6 +330,11 @@ static void dump_afu_descriptor(struct cxl_afu *afu)
 	val = AFUD_READ_EB_OFF(afu);
 	show_reg("AFU_EB_offset", val);
 
+	for (i = 0; i < afu_cr_num; i++) {
+		val = AFUD_READ_LE(afu, afu_cr_off + i * afu_cr_len);
+		show_reg("CR Vendor", val & 0xffff);
+		show_reg("CR Device", (val >> 16) & 0xffff);
+	}
 #undef show_reg
 }
 
@@ -529,7 +539,7 @@ err:
 
 static void cxl_unmap_slice_regs(struct cxl_afu *afu)
 {
-	if (afu->p1n_mmio)
+	if (afu->p2n_mmio)
 		iounmap(afu->p2n_mmio);
 	if (afu->p1n_mmio)
 		iounmap(afu->p1n_mmio);
@@ -593,6 +603,22 @@ static int cxl_read_afu_descriptor(struct cxl_afu *afu)
 	afu->crs_len = AFUD_CR_LEN(val) * 256;
 	afu->crs_offset = AFUD_READ_CR_OFF(afu);
 
+
+	/* eb_len is in multiple of 4K */
+	afu->eb_len = AFUD_EB_LEN(AFUD_READ_EB(afu)) * 4096;
+	afu->eb_offset = AFUD_READ_EB_OFF(afu);
+
+	/* eb_off is 4K aligned so lower 12 bits are always zero */
+	if (EXTRACT_PPC_BITS(afu->eb_offset, 0, 11) != 0) {
+		dev_warn(&afu->dev,
+			 "Invalid AFU error buffer offset %Lx\n",
+			 afu->eb_offset);
+		dev_info(&afu->dev,
+			 "Ignoring AFU error buffer in the descriptor\n");
+		/* indicate that no afu buffer exists */
+		afu->eb_len = 0;
+	}
+
 	return 0;
 }
 
@@ -631,7 +657,7 @@ static int sanitise_afu_regs(struct cxl_afu *afu)
 	reg = cxl_p2n_read(afu, CXL_AFU_Cntl_An);
 	if ((reg & CXL_AFU_Cntl_An_ES_MASK) != CXL_AFU_Cntl_An_ES_Disabled) {
 		dev_warn(&afu->dev, "WARNING: AFU was not disabled: %#.16llx\n", reg);
-		if (cxl_afu_reset(afu))
+		if (__cxl_afu_reset(afu))
 			return -EIO;
 		if (cxl_afu_disable(afu))
 			return -EIO;
@@ -672,6 +698,50 @@ static int sanitise_afu_regs(struct cxl_afu *afu)
 	return 0;
 }
 
+#define ERR_BUFF_MAX_COPY_SIZE PAGE_SIZE
+/*
+ * afu_eb_read:
+ * Called from sysfs and reads the afu error info buffer. The h/w only supports
+ * 4/8 bytes aligned access. So in case the requested offset/count arent 8 byte
+ * aligned the function uses a bounce buffer which can be max PAGE_SIZE.
+ */
+ssize_t cxl_afu_read_err_buffer(struct cxl_afu *afu, char *buf,
+				loff_t off, size_t count)
+{
+	loff_t aligned_start, aligned_end;
+	size_t aligned_length;
+	void *tbuf;
+	const void __iomem *ebuf = afu->afu_desc_mmio + afu->eb_offset;
+
+	if (count == 0 || off < 0 || (size_t)off >= afu->eb_len)
+		return 0;
+
+	/* calculate aligned read window */
+	count = min((size_t)(afu->eb_len - off), count);
+	aligned_start = round_down(off, 8);
+	aligned_end = round_up(off + count, 8);
+	aligned_length = aligned_end - aligned_start;
+
+	/* max we can copy in one read is PAGE_SIZE */
+	if (aligned_length > ERR_BUFF_MAX_COPY_SIZE) {
+		aligned_length = ERR_BUFF_MAX_COPY_SIZE;
+		count = ERR_BUFF_MAX_COPY_SIZE - (off & 0x7);
+	}
+
+	/* use bounce buffer for copy */
+	tbuf = (void *)__get_free_page(GFP_TEMPORARY);
+	if (!tbuf)
+		return -ENOMEM;
+
+	/* perform aligned read from the mmio region */
+	memcpy_fromio(tbuf, ebuf + aligned_start, aligned_length);
+	memcpy(buf, tbuf + (off & 0x7), count);
+
+	free_page((unsigned long)tbuf);
+
+	return count;
+}
+
 static int cxl_init_afu(struct cxl *adapter, int slice, struct pci_dev *dev)
 {
 	struct cxl_afu *afu;
@@ -691,7 +761,7 @@ static int cxl_init_afu(struct cxl *adapter, int slice, struct pci_dev *dev)
 		goto err2;
 
 	/* We need to reset the AFU before we can read the AFU descriptor */
-	if ((rc = cxl_afu_reset(afu)))
+	if ((rc = __cxl_afu_reset(afu)))
 		goto err2;
 
 	if (cxl_verbose)
@@ -731,6 +801,9 @@ static int cxl_init_afu(struct cxl *adapter, int slice, struct pci_dev *dev)
 
 	adapter->afu[afu->slice] = afu;
 
+	if ((rc = cxl_pci_vphb_add(afu)))
+		dev_info(&afu->dev, "Can't register vPHB\n");
+
 	return 0;
 
 err_put2:
@@ -783,8 +856,10 @@ int cxl_reset(struct cxl *adapter)
 
 	dev_info(&dev->dev, "CXL reset\n");
 
-	for (i = 0; i < adapter->slices; i++)
+	for (i = 0; i < adapter->slices; i++) {
+		cxl_pci_vphb_remove(adapter->afu[i]);
 		cxl_remove_afu(adapter->afu[i]);
+	}
 
 	/* pcie_warm_reset requests a fundamental pci reset which includes a
 	 * PERST assert/deassert.  PERST triggers a loading of the image
@@ -857,13 +932,13 @@ static int cxl_read_vsec(struct cxl *adapter, struct pci_dev *dev)
 	u8 image_state;
 
 	if (!(vsec = find_cxl_vsec(dev))) {
-		dev_err(&adapter->dev, "ABORTING: CXL VSEC not found!\n");
+		dev_err(&dev->dev, "ABORTING: CXL VSEC not found!\n");
 		return -ENODEV;
 	}
 
 	CXL_READ_VSEC_LENGTH(dev, vsec, &vseclen);
 	if (vseclen < CXL_VSEC_MIN_SIZE) {
-		pr_err("ABORTING: CXL VSEC too short\n");
+		dev_err(&dev->dev, "ABORTING: CXL VSEC too short\n");
 		return -EINVAL;
 	}
 
@@ -902,24 +977,24 @@ static int cxl_vsec_looks_ok(struct cxl *adapter, struct pci_dev *dev)
 		return -EBUSY;
 
 	if (adapter->vsec_status & CXL_UNSUPPORTED_FEATURES) {
-		dev_err(&adapter->dev, "ABORTING: CXL requires unsupported features\n");
+		dev_err(&dev->dev, "ABORTING: CXL requires unsupported features\n");
 		return -EINVAL;
 	}
 
 	if (!adapter->slices) {
 		/* Once we support dynamic reprogramming we can use the card if
 		 * it supports loadable AFUs */
-		dev_err(&adapter->dev, "ABORTING: Device has no AFUs\n");
+		dev_err(&dev->dev, "ABORTING: Device has no AFUs\n");
 		return -EINVAL;
 	}
 
 	if (!adapter->afu_desc_off || !adapter->afu_desc_size) {
-		dev_err(&adapter->dev, "ABORTING: VSEC shows no AFU descriptors\n");
+		dev_err(&dev->dev, "ABORTING: VSEC shows no AFU descriptors\n");
 		return -EINVAL;
 	}
 
 	if (adapter->ps_size > p2_size(dev) - adapter->ps_off) {
-		dev_err(&adapter->dev, "ABORTING: Problem state size larger than "
+		dev_err(&dev->dev, "ABORTING: Problem state size larger than "
 				   "available in BAR2: 0x%llx > 0x%llx\n",
 			 adapter->ps_size, p2_size(dev) - adapter->ps_off);
 		return -EINVAL;
@@ -968,6 +1043,15 @@ static struct cxl *cxl_init_adapter(struct pci_dev *dev)
 	if (!(adapter = cxl_alloc_adapter(dev)))
 		return ERR_PTR(-ENOMEM);
 
+	if ((rc = cxl_read_vsec(adapter, dev)))
+		goto err1;
+
+	if ((rc = cxl_vsec_looks_ok(adapter, dev)))
+		goto err1;
+
+	if ((rc = setup_cxl_bars(dev)))
+		goto err1;
+
 	if ((rc = switch_card_to_cxl(dev)))
 		goto err1;
 
@@ -977,12 +1061,6 @@ static struct cxl *cxl_init_adapter(struct pci_dev *dev)
 	if ((rc = dev_set_name(&adapter->dev, "card%i", adapter->adapter_num)))
 		goto err2;
 
-	if ((rc = cxl_read_vsec(adapter, dev)))
-		goto err2;
-
-	if ((rc = cxl_vsec_looks_ok(adapter, dev)))
-		goto err2;
-
 	if ((rc = cxl_update_image_control(adapter)))
 		goto err2;
 
@@ -1067,9 +1145,6 @@ static int cxl_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	if (cxl_verbose)
 		dump_cxl_config_space(dev);
 
-	if ((rc = setup_cxl_bars(dev)))
-		return rc;
-
 	if ((rc = pci_enable_device(dev))) {
 		dev_err(&dev->dev, "pci_enable_device failed: %i\n", rc);
 		return rc;
@@ -1078,6 +1153,7 @@ static int cxl_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	adapter = cxl_init_adapter(dev);
 	if (IS_ERR(adapter)) {
 		dev_err(&dev->dev, "cxl_init_adapter failed: %li\n", PTR_ERR(adapter));
+		pci_disable_device(dev);
 		return PTR_ERR(adapter);
 	}
 
@@ -1092,16 +1168,18 @@ static int cxl_probe(struct pci_dev *dev, const struct pci_device_id *id)
 static void cxl_remove(struct pci_dev *dev)
 {
 	struct cxl *adapter = pci_get_drvdata(dev);
-	int afu;
-
-	dev_warn(&dev->dev, "pci remove\n");
+	struct cxl_afu *afu;
+	int i;
 
 	/*
 	 * Lock to prevent someone grabbing a ref through the adapter list as
 	 * we are removing it
 	 */
-	for (afu = 0; afu < adapter->slices; afu++)
-		cxl_remove_afu(adapter->afu[afu]);
+	for (i = 0; i < adapter->slices; i++) {
+		afu = adapter->afu[i];
+		cxl_pci_vphb_remove(afu);
+		cxl_remove_afu(afu);
+	}
 	cxl_remove_adapter(adapter);
 }
 
@@ -1110,4 +1188,5 @@ struct pci_driver cxl_pci_driver = {
 	.id_table = cxl_pci_tbl,
 	.probe = cxl_probe,
 	.remove = cxl_remove,
+	.shutdown = cxl_remove,
 };
diff --git a/drivers/misc/cxl/sysfs.c b/drivers/misc/cxl/sysfs.c
index d0c38c7bc0c4..31f38bc71a3d 100644
--- a/drivers/misc/cxl/sysfs.c
+++ b/drivers/misc/cxl/sysfs.c
@@ -185,7 +185,7 @@ static ssize_t reset_store_afu(struct device *device,
 		goto err;
 	}
 
-	if ((rc = cxl_afu_reset(afu)))
+	if ((rc = __cxl_afu_reset(afu)))
 		goto err;
 
 	rc = count;
@@ -356,6 +356,16 @@ static ssize_t api_version_compatible_show(struct device *device,
 	return scnprintf(buf, PAGE_SIZE, "%i\n", CXL_API_VERSION_COMPATIBLE);
 }
 
+static ssize_t afu_eb_read(struct file *filp, struct kobject *kobj,
+			       struct bin_attribute *bin_attr, char *buf,
+			       loff_t off, size_t count)
+{
+	struct cxl_afu *afu = to_cxl_afu(container_of(kobj,
+						      struct device, kobj));
+
+	return cxl_afu_read_err_buffer(afu, buf, off, count);
+}
+
 static struct device_attribute afu_attrs[] = {
 	__ATTR_RO(mmio_size),
 	__ATTR_RO(irqs_min),
@@ -534,6 +544,10 @@ void cxl_sysfs_afu_remove(struct cxl_afu *afu)
 	struct afu_config_record *cr, *tmp;
 	int i;
 
+	/* remove the err buffer bin attribute */
+	if (afu->eb_len)
+		device_remove_bin_file(&afu->dev, &afu->attr_eb);
+
 	for (i = 0; i < ARRAY_SIZE(afu_attrs); i++)
 		device_remove_file(&afu->dev, &afu_attrs[i]);
 
@@ -555,6 +569,22 @@ int cxl_sysfs_afu_add(struct cxl_afu *afu)
 			goto err;
 	}
 
+	/* conditionally create the add the binary file for error info buffer */
+	if (afu->eb_len) {
+		afu->attr_eb.attr.name = "afu_err_buff";
+		afu->attr_eb.attr.mode = S_IRUGO;
+		afu->attr_eb.size = afu->eb_len;
+		afu->attr_eb.read = afu_eb_read;
+
+		rc = device_create_bin_file(&afu->dev, &afu->attr_eb);
+		if (rc) {
+			dev_err(&afu->dev,
+				"Unable to create eb attr for the afu. Err(%d)\n",
+				rc);
+			goto err;
+		}
+	}
+
 	for (i = 0; i < afu->crs_num; i++) {
 		cr = cxl_sysfs_afu_new_cr(afu, i);
 		if (IS_ERR(cr)) {
@@ -570,6 +600,9 @@ err1:
 	cxl_sysfs_afu_remove(afu);
 	return rc;
 err:
+	/* reset the eb_len as we havent created the bin attr */
+	afu->eb_len = 0;
+
 	for (i--; i >= 0; i--)
 		device_remove_file(&afu->dev, &afu_attrs[i]);
 	return rc;
diff --git a/drivers/misc/cxl/vphb.c b/drivers/misc/cxl/vphb.c
new file mode 100644
index 000000000000..2eba002b580b
--- /dev/null
+++ b/drivers/misc/cxl/vphb.c
@@ -0,0 +1,271 @@
+/*
+ * 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.
+ */
+
+#include <linux/pci.h>
+#include <misc/cxl.h>
+#include "cxl.h"
+
+static int cxl_dma_set_mask(struct pci_dev *pdev, u64 dma_mask)
+{
+	if (dma_mask < DMA_BIT_MASK(64)) {
+		pr_info("%s only 64bit DMA supported on CXL", __func__);
+		return -EIO;
+	}
+
+	*(pdev->dev.dma_mask) = dma_mask;
+	return 0;
+}
+
+static int cxl_pci_probe_mode(struct pci_bus *bus)
+{
+	return PCI_PROBE_NORMAL;
+}
+
+static int cxl_setup_msi_irqs(struct pci_dev *pdev, int nvec, int type)
+{
+	return -ENODEV;
+}
+
+static void cxl_teardown_msi_irqs(struct pci_dev *pdev)
+{
+	/*
+	 * MSI should never be set but need still need to provide this call
+	 * back.
+	 */
+}
+
+static bool cxl_pci_enable_device_hook(struct pci_dev *dev)
+{
+	struct pci_controller *phb;
+	struct cxl_afu *afu;
+	struct cxl_context *ctx;
+
+	phb = pci_bus_to_host(dev->bus);
+	afu = (struct cxl_afu *)phb->private_data;
+	set_dma_ops(&dev->dev, &dma_direct_ops);
+	set_dma_offset(&dev->dev, PAGE_OFFSET);
+
+	/*
+	 * Allocate a context to do cxl things too.  If we eventually do real
+	 * DMA ops, we'll need a default context to attach them to
+	 */
+	ctx = cxl_dev_context_init(dev);
+	if (!ctx)
+		return false;
+	dev->dev.archdata.cxl_ctx = ctx;
+
+	return (cxl_afu_check_and_enable(afu) == 0);
+}
+
+static void cxl_pci_disable_device(struct pci_dev *dev)
+{
+	struct cxl_context *ctx = cxl_get_context(dev);
+
+	if (ctx) {
+		if (ctx->status == STARTED) {
+			dev_err(&dev->dev, "Default context started\n");
+			return;
+		}
+		dev->dev.archdata.cxl_ctx = NULL;
+		cxl_release_context(ctx);
+	}
+}
+
+static resource_size_t cxl_pci_window_alignment(struct pci_bus *bus,
+						unsigned long type)
+{
+	return 1;
+}
+
+static void cxl_pci_reset_secondary_bus(struct pci_dev *dev)
+{
+	/* Should we do an AFU reset here ? */
+}
+
+static int cxl_pcie_cfg_record(u8 bus, u8 devfn)
+{
+	return (bus << 8) + devfn;
+}
+
+static unsigned long cxl_pcie_cfg_addr(struct pci_controller* phb,
+				       u8 bus, u8 devfn, int offset)
+{
+	int record = cxl_pcie_cfg_record(bus, devfn);
+
+	return (unsigned long)phb->cfg_addr + ((unsigned long)phb->cfg_data * record) + offset;
+}
+
+
+static int cxl_pcie_config_info(struct pci_bus *bus, unsigned int devfn,
+				int offset, int len,
+				volatile void __iomem **ioaddr,
+				u32 *mask, int *shift)
+{
+	struct pci_controller *phb;
+	struct cxl_afu *afu;
+	unsigned long addr;
+
+	phb = pci_bus_to_host(bus);
+	if (phb == NULL)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+	afu = (struct cxl_afu *)phb->private_data;
+
+	if (cxl_pcie_cfg_record(bus->number, devfn) > afu->crs_num)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+	if (offset >= (unsigned long)phb->cfg_data)
+		return PCIBIOS_BAD_REGISTER_NUMBER;
+	addr = cxl_pcie_cfg_addr(phb, bus->number, devfn, offset);
+
+	*ioaddr = (void *)(addr & ~0x3ULL);
+	*shift = ((addr & 0x3) * 8);
+	switch (len) {
+	case 1:
+		*mask = 0xff;
+		break;
+	case 2:
+		*mask = 0xffff;
+		break;
+	default:
+		*mask = 0xffffffff;
+		break;
+	}
+	return 0;
+}
+
+static int cxl_pcie_read_config(struct pci_bus *bus, unsigned int devfn,
+				int offset, int len, u32 *val)
+{
+	volatile void __iomem *ioaddr;
+	int shift, rc;
+	u32 mask;
+
+	rc = cxl_pcie_config_info(bus, devfn, offset, len, &ioaddr,
+				  &mask, &shift);
+	if (rc)
+		return rc;
+
+	/* Can only read 32 bits */
+	*val = (in_le32(ioaddr) >> shift) & mask;
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static int cxl_pcie_write_config(struct pci_bus *bus, unsigned int devfn,
+				 int offset, int len, u32 val)
+{
+	volatile void __iomem *ioaddr;
+	u32 v, mask;
+	int shift, rc;
+
+	rc = cxl_pcie_config_info(bus, devfn, offset, len, &ioaddr,
+				  &mask, &shift);
+	if (rc)
+		return rc;
+
+	/* Can only write 32 bits so do read-modify-write */
+	mask <<= shift;
+	val <<= shift;
+
+	v = (in_le32(ioaddr) & ~mask) || (val & mask);
+
+	out_le32(ioaddr, v);
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static struct pci_ops cxl_pcie_pci_ops =
+{
+	.read = cxl_pcie_read_config,
+	.write = cxl_pcie_write_config,
+};
+
+
+static struct pci_controller_ops cxl_pci_controller_ops =
+{
+	.probe_mode = cxl_pci_probe_mode,
+	.enable_device_hook = cxl_pci_enable_device_hook,
+	.disable_device = cxl_pci_disable_device,
+	.release_device = cxl_pci_disable_device,
+	.window_alignment = cxl_pci_window_alignment,
+	.reset_secondary_bus = cxl_pci_reset_secondary_bus,
+	.setup_msi_irqs = cxl_setup_msi_irqs,
+	.teardown_msi_irqs = cxl_teardown_msi_irqs,
+	.dma_set_mask = cxl_dma_set_mask,
+};
+
+int cxl_pci_vphb_add(struct cxl_afu *afu)
+{
+	struct pci_dev *phys_dev;
+	struct pci_controller *phb, *phys_phb;
+
+	phys_dev = to_pci_dev(afu->adapter->dev.parent);
+	phys_phb = pci_bus_to_host(phys_dev->bus);
+
+	/* Alloc and setup PHB data structure */
+	phb = pcibios_alloc_controller(phys_phb->dn);
+
+	if (!phb)
+		return -ENODEV;
+
+	/* Setup parent in sysfs */
+	phb->parent = &phys_dev->dev;
+
+	/* Setup the PHB using arch provided callback */
+	phb->ops = &cxl_pcie_pci_ops;
+	phb->cfg_addr = afu->afu_desc_mmio + afu->crs_offset;
+	phb->cfg_data = (void *)(u64)afu->crs_len;
+	phb->private_data = afu;
+	phb->controller_ops = cxl_pci_controller_ops;
+
+	/* Scan the bus */
+	pcibios_scan_phb(phb);
+	if (phb->bus == NULL)
+		return -ENXIO;
+
+	/* Claim resources. This might need some rework as well depending
+	 * whether we are doing probe-only or not, like assigning unassigned
+	 * resources etc...
+	 */
+	pcibios_claim_one_bus(phb->bus);
+
+	/* Add probed PCI devices to the device model */
+	pci_bus_add_devices(phb->bus);
+
+	afu->phb = phb;
+
+	return 0;
+}
+
+
+void cxl_pci_vphb_remove(struct cxl_afu *afu)
+{
+	struct pci_controller *phb;
+
+	/* If there is no configuration record we won't have one of these */
+	if (!afu || !afu->phb)
+		return;
+
+	phb = afu->phb;
+
+	pci_remove_root_bus(phb->bus);
+}
+
+struct cxl_afu *cxl_pci_to_afu(struct pci_dev *dev)
+{
+	struct pci_controller *phb;
+
+	phb = pci_bus_to_host(dev->bus);
+
+	return (struct cxl_afu *)phb->private_data;
+}
+EXPORT_SYMBOL_GPL(cxl_pci_to_afu);
+
+unsigned int cxl_pci_to_cfg_record(struct pci_dev *dev)
+{
+	return cxl_pcie_cfg_record(dev->bus->number, dev->devfn);
+}
+EXPORT_SYMBOL_GPL(cxl_pci_to_cfg_record);
diff --git a/drivers/misc/kgdbts.c b/drivers/misc/kgdbts.c
index 36f5d52775a9..9a60bd4d3c49 100644
--- a/drivers/misc/kgdbts.c
+++ b/drivers/misc/kgdbts.c
@@ -220,7 +220,7 @@ static unsigned long lookup_addr(char *arg)
 	else if (!strcmp(arg, "sys_open"))
 		addr = (unsigned long)do_sys_open;
 	else if (!strcmp(arg, "do_fork"))
-		addr = (unsigned long)do_fork;
+		addr = (unsigned long)_do_fork;
 	else if (!strcmp(arg, "hw_break_val"))
 		addr = (unsigned long)&hw_break_val;
 	addr = (unsigned long) dereference_function_descriptor((void *)addr);
diff --git a/drivers/misc/lis3lv02d/lis3lv02d.c b/drivers/misc/lis3lv02d/lis3lv02d.c
index 4739689d23ad..fb8705fc3aca 100644
--- a/drivers/misc/lis3lv02d/lis3lv02d.c
+++ b/drivers/misc/lis3lv02d/lis3lv02d.c
@@ -115,7 +115,7 @@ static int param_set_axis(const char *val, const struct kernel_param *kp)
 	return ret;
 }
 
-static struct kernel_param_ops param_ops_axis = {
+static const struct kernel_param_ops param_ops_axis = {
 	.set = param_set_axis,
 	.get = param_get_int,
 };
diff --git a/drivers/misc/mei/amthif.c b/drivers/misc/mei/amthif.c
index d2cd53e3fac3..1e42781592d8 100644
--- a/drivers/misc/mei/amthif.c
+++ b/drivers/misc/mei/amthif.c
@@ -59,46 +59,29 @@ void mei_amthif_reset_params(struct mei_device *dev)
  * mei_amthif_host_init - mei initialization amthif client.
  *
  * @dev: the device structure
+ * @me_cl: me client
  *
  * Return: 0 on success, <0 on failure.
  */
-int mei_amthif_host_init(struct mei_device *dev)
+int mei_amthif_host_init(struct mei_device *dev, struct mei_me_client *me_cl)
 {
 	struct mei_cl *cl = &dev->iamthif_cl;
-	struct mei_me_client *me_cl;
 	int ret;
 
 	dev->iamthif_state = MEI_IAMTHIF_IDLE;
 
 	mei_cl_init(cl, dev);
 
-	me_cl = mei_me_cl_by_uuid(dev, &mei_amthif_guid);
-	if (!me_cl) {
-		dev_info(dev->dev, "amthif: failed to find the client");
-		return -ENOTTY;
-	}
-
-	cl->me_client_id = me_cl->client_id;
-	cl->cl_uuid = me_cl->props.protocol_name;
-
-	/* Assign iamthif_mtu to the value received from ME  */
-
-	dev->iamthif_mtu = me_cl->props.max_msg_length;
-	dev_dbg(dev->dev, "IAMTHIF_MTU = %d\n", dev->iamthif_mtu);
-
-
 	ret = mei_cl_link(cl, MEI_IAMTHIF_HOST_CLIENT_ID);
 	if (ret < 0) {
 		dev_err(dev->dev, "amthif: failed cl_link %d\n", ret);
-		goto out;
+		return ret;
 	}
 
-	ret = mei_cl_connect(cl, NULL);
+	ret = mei_cl_connect(cl, me_cl, NULL);
 
 	dev->iamthif_state = MEI_IAMTHIF_IDLE;
 
-out:
-	mei_me_cl_put(me_cl);
 	return ret;
 }
 
@@ -250,7 +233,6 @@ static int mei_amthif_read_start(struct mei_cl *cl, struct file *file)
 {
 	struct mei_device *dev = cl->dev;
 	struct mei_cl_cb *cb;
-	size_t length = dev->iamthif_mtu;
 	int rets;
 
 	cb = mei_io_cb_init(cl, MEI_FOP_READ, file);
@@ -259,7 +241,7 @@ static int mei_amthif_read_start(struct mei_cl *cl, struct file *file)
 		goto err;
 	}
 
-	rets = mei_io_cb_alloc_buf(cb, length);
+	rets = mei_io_cb_alloc_buf(cb, mei_cl_mtu(cl));
 	if (rets)
 		goto err;
 
diff --git a/drivers/misc/mei/bus.c b/drivers/misc/mei/bus.c
index 4cf38c39878a..458aa5a09c52 100644
--- a/drivers/misc/mei/bus.c
+++ b/drivers/misc/mei/bus.c
@@ -35,18 +35,30 @@ static int mei_cl_device_match(struct device *dev, struct device_driver *drv)
 	struct mei_cl_device *device = to_mei_cl_device(dev);
 	struct mei_cl_driver *driver = to_mei_cl_driver(drv);
 	const struct mei_cl_device_id *id;
+	const uuid_le *uuid;
+	const char *name;
 
 	if (!device)
 		return 0;
 
+	uuid = mei_me_cl_uuid(device->me_cl);
+	name = device->name;
+
 	if (!driver || !driver->id_table)
 		return 0;
 
 	id = driver->id_table;
 
-	while (id->name[0]) {
-		if (!strncmp(dev_name(dev), id->name, sizeof(id->name)))
-			return 1;
+	while (uuid_le_cmp(NULL_UUID_LE, id->uuid)) {
+
+		if (!uuid_le_cmp(*uuid, id->uuid)) {
+			if (id->name[0]) {
+				if (!strncmp(name, id->name, sizeof(id->name)))
+					return 1;
+			} else {
+				return 1;
+			}
+		}
 
 		id++;
 	}
@@ -69,7 +81,7 @@ static int mei_cl_device_probe(struct device *dev)
 
 	dev_dbg(dev, "Device probe\n");
 
-	strlcpy(id.name, dev_name(dev), sizeof(id.name));
+	strlcpy(id.name, device->name, sizeof(id.name));
 
 	return driver->probe(device, &id);
 }
@@ -97,18 +109,48 @@ static int mei_cl_device_remove(struct device *dev)
 	return driver->remove(device);
 }
 
+static ssize_t name_show(struct device *dev, struct device_attribute *a,
+			     char *buf)
+{
+	struct mei_cl_device *device = to_mei_cl_device(dev);
+	size_t len;
+
+	len = snprintf(buf, PAGE_SIZE, "%s", device->name);
+
+	return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
+}
+static DEVICE_ATTR_RO(name);
+
+static ssize_t uuid_show(struct device *dev, struct device_attribute *a,
+			     char *buf)
+{
+	struct mei_cl_device *device = to_mei_cl_device(dev);
+	const uuid_le *uuid = mei_me_cl_uuid(device->me_cl);
+	size_t len;
+
+	len = snprintf(buf, PAGE_SIZE, "%pUl", uuid);
+
+	return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
+}
+static DEVICE_ATTR_RO(uuid);
+
 static ssize_t modalias_show(struct device *dev, struct device_attribute *a,
 			     char *buf)
 {
-	int len;
+	struct mei_cl_device *device = to_mei_cl_device(dev);
+	const uuid_le *uuid = mei_me_cl_uuid(device->me_cl);
+	size_t len;
 
-	len = snprintf(buf, PAGE_SIZE, "mei:%s\n", dev_name(dev));
+	len = snprintf(buf, PAGE_SIZE, "mei:%s:" MEI_CL_UUID_FMT ":",
+		device->name, MEI_CL_UUID_ARGS(uuid->b));
 
 	return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
 }
 static DEVICE_ATTR_RO(modalias);
 
 static struct attribute *mei_cl_dev_attrs[] = {
+	&dev_attr_name.attr,
+	&dev_attr_uuid.attr,
 	&dev_attr_modalias.attr,
 	NULL,
 };
@@ -116,7 +158,17 @@ ATTRIBUTE_GROUPS(mei_cl_dev);
 
 static int mei_cl_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
-	if (add_uevent_var(env, "MODALIAS=mei:%s", dev_name(dev)))
+	struct mei_cl_device *device = to_mei_cl_device(dev);
+	const uuid_le *uuid = mei_me_cl_uuid(device->me_cl);
+
+	if (add_uevent_var(env, "MEI_CL_UUID=%pUl", uuid))
+		return -ENOMEM;
+
+	if (add_uevent_var(env, "MEI_CL_NAME=%s", device->name))
+		return -ENOMEM;
+
+	if (add_uevent_var(env, "MODALIAS=mei:%s:" MEI_CL_UUID_FMT ":",
+		device->name, MEI_CL_UUID_ARGS(uuid->b)))
 		return -ENOMEM;
 
 	return 0;
@@ -133,7 +185,13 @@ static struct bus_type mei_cl_bus_type = {
 
 static void mei_cl_dev_release(struct device *dev)
 {
-	kfree(to_mei_cl_device(dev));
+	struct mei_cl_device *device = to_mei_cl_device(dev);
+
+	if (!device)
+		return;
+
+	mei_me_cl_put(device->me_cl);
+	kfree(device);
 }
 
 static struct device_type mei_cl_device_type = {
@@ -141,45 +199,50 @@ static struct device_type mei_cl_device_type = {
 };
 
 struct mei_cl *mei_cl_bus_find_cl_by_uuid(struct mei_device *dev,
-						uuid_le uuid)
+					 uuid_le uuid)
 {
 	struct mei_cl *cl;
 
 	list_for_each_entry(cl, &dev->device_list, device_link) {
-		if (!uuid_le_cmp(uuid, cl->cl_uuid))
+		if (cl->device && cl->device->me_cl &&
+		    !uuid_le_cmp(uuid, *mei_me_cl_uuid(cl->device->me_cl)))
 			return cl;
 	}
 
 	return NULL;
 }
+
 struct mei_cl_device *mei_cl_add_device(struct mei_device *dev,
-					uuid_le uuid, char *name,
-					struct mei_cl_ops *ops)
+					struct mei_me_client *me_cl,
+					struct mei_cl *cl,
+					char *name)
 {
 	struct mei_cl_device *device;
-	struct mei_cl *cl;
 	int status;
 
-	cl = mei_cl_bus_find_cl_by_uuid(dev, uuid);
-	if (cl == NULL)
-		return NULL;
-
 	device = kzalloc(sizeof(struct mei_cl_device), GFP_KERNEL);
 	if (!device)
 		return NULL;
 
-	device->cl = cl;
-	device->ops = ops;
+	device->me_cl = mei_me_cl_get(me_cl);
+	if (!device->me_cl) {
+		kfree(device);
+		return NULL;
+	}
 
+	device->cl = cl;
 	device->dev.parent = dev->dev;
 	device->dev.bus = &mei_cl_bus_type;
 	device->dev.type = &mei_cl_device_type;
 
-	dev_set_name(&device->dev, "%s", name);
+	strlcpy(device->name, name, sizeof(device->name));
+
+	dev_set_name(&device->dev, "mei:%s:%pUl", name, mei_me_cl_uuid(me_cl));
 
 	status = device_register(&device->dev);
 	if (status) {
 		dev_err(dev->dev, "Failed to register MEI device\n");
+		mei_me_cl_put(device->me_cl);
 		kfree(device);
 		return NULL;
 	}
@@ -224,11 +287,10 @@ void mei_cl_driver_unregister(struct mei_cl_driver *driver)
 }
 EXPORT_SYMBOL_GPL(mei_cl_driver_unregister);
 
-static ssize_t ___mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
+ssize_t __mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
 			bool blocking)
 {
 	struct mei_device *dev;
-	struct mei_me_client *me_cl = NULL;
 	struct mei_cl_cb *cb = NULL;
 	ssize_t rets;
 
@@ -244,13 +306,12 @@ static ssize_t ___mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
 	}
 
 	/* Check if we have an ME client device */
-	me_cl = mei_me_cl_by_uuid_id(dev, &cl->cl_uuid, cl->me_client_id);
-	if (!me_cl) {
+	if (!mei_me_cl_is_active(cl->me_cl)) {
 		rets = -ENOTTY;
 		goto out;
 	}
 
-	if (length > me_cl->props.max_msg_length) {
+	if (length > mei_cl_mtu(cl)) {
 		rets = -EFBIG;
 		goto out;
 	}
@@ -266,7 +327,6 @@ static ssize_t ___mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
 	rets = mei_cl_write(cl, cb, blocking);
 
 out:
-	mei_me_cl_put(me_cl);
 	mutex_unlock(&dev->device_lock);
 	if (rets < 0)
 		mei_io_cb_free(cb);
@@ -341,16 +401,6 @@ out:
 	return rets;
 }
 
-inline ssize_t __mei_cl_async_send(struct mei_cl *cl, u8 *buf, size_t length)
-{
-	return ___mei_cl_send(cl, buf, length, 0);
-}
-
-inline ssize_t __mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length)
-{
-	return ___mei_cl_send(cl, buf, length, 1);
-}
-
 ssize_t mei_cl_send(struct mei_cl_device *device, u8 *buf, size_t length)
 {
 	struct mei_cl *cl = device->cl;
@@ -358,23 +408,17 @@ ssize_t mei_cl_send(struct mei_cl_device *device, u8 *buf, size_t length)
 	if (cl == NULL)
 		return -ENODEV;
 
-	if (device->ops && device->ops->send)
-		return device->ops->send(device, buf, length);
-
-	return __mei_cl_send(cl, buf, length);
+	return __mei_cl_send(cl, buf, length, 1);
 }
 EXPORT_SYMBOL_GPL(mei_cl_send);
 
 ssize_t mei_cl_recv(struct mei_cl_device *device, u8 *buf, size_t length)
 {
-	struct mei_cl *cl =  device->cl;
+	struct mei_cl *cl = device->cl;
 
 	if (cl == NULL)
 		return -ENODEV;
 
-	if (device->ops && device->ops->recv)
-		return device->ops->recv(device, buf, length);
-
 	return __mei_cl_recv(cl, buf, length);
 }
 EXPORT_SYMBOL_GPL(mei_cl_recv);
@@ -436,7 +480,13 @@ int mei_cl_enable_device(struct mei_cl_device *device)
 
 	mutex_lock(&dev->device_lock);
 
-	err = mei_cl_connect(cl, NULL);
+	if (mei_cl_is_connected(cl)) {
+		mutex_unlock(&dev->device_lock);
+		dev_warn(dev->dev, "Already connected");
+		return -EBUSY;
+	}
+
+	err = mei_cl_connect(cl, device->me_cl, NULL);
 	if (err < 0) {
 		mutex_unlock(&dev->device_lock);
 		dev_err(dev->dev, "Could not connect to the ME client");
@@ -449,10 +499,7 @@ int mei_cl_enable_device(struct mei_cl_device *device)
 	if (device->event_cb)
 		mei_cl_read_start(device->cl, 0, NULL);
 
-	if (!device->ops || !device->ops->enable)
-		return 0;
-
-	return device->ops->enable(device);
+	return 0;
 }
 EXPORT_SYMBOL_GPL(mei_cl_enable_device);
 
@@ -467,9 +514,6 @@ int mei_cl_disable_device(struct mei_cl_device *device)
 
 	dev = cl->dev;
 
-	if (device->ops && device->ops->disable)
-		device->ops->disable(device);
-
 	device->event_cb = NULL;
 
 	mutex_lock(&dev->device_lock);
@@ -480,8 +524,6 @@ int mei_cl_disable_device(struct mei_cl_device *device)
 		goto out;
 	}
 
-	cl->state = MEI_FILE_DISCONNECTING;
-
 	err = mei_cl_disconnect(cl);
 	if (err < 0) {
 		dev_err(dev->dev, "Could not disconnect from the ME client");
@@ -510,22 +552,6 @@ void mei_cl_bus_rx_event(struct mei_cl *cl)
 	schedule_work(&device->event_work);
 }
 
-void mei_cl_bus_remove_devices(struct mei_device *dev)
-{
-	struct mei_cl *cl, *next;
-
-	mutex_lock(&dev->device_lock);
-	list_for_each_entry_safe(cl, next, &dev->device_list, device_link) {
-		if (cl->device)
-			mei_cl_remove_device(cl->device);
-
-		list_del(&cl->device_link);
-		mei_cl_unlink(cl);
-		kfree(cl);
-	}
-	mutex_unlock(&dev->device_lock);
-}
-
 int __init mei_cl_bus_init(void)
 {
 	return bus_register(&mei_cl_bus_type);
diff --git a/drivers/misc/mei/client.c b/drivers/misc/mei/client.c
index 1e99ef6a54a2..6decbe136ea7 100644
--- a/drivers/misc/mei/client.c
+++ b/drivers/misc/mei/client.c
@@ -83,7 +83,7 @@ void mei_me_cl_put(struct mei_me_client *me_cl)
 }
 
 /**
- * __mei_me_cl_del  - delete me client form the list and decrease
+ * __mei_me_cl_del  - delete me client from the list and decrease
  *     reference counter
  *
  * @dev: mei device
@@ -96,11 +96,25 @@ static void __mei_me_cl_del(struct mei_device *dev, struct mei_me_client *me_cl)
 	if (!me_cl)
 		return;
 
-	list_del(&me_cl->list);
+	list_del_init(&me_cl->list);
 	mei_me_cl_put(me_cl);
 }
 
 /**
+ * mei_me_cl_del - delete me client from the list and decrease
+ *     reference counter
+ *
+ * @dev: mei device
+ * @me_cl: me client
+ */
+void mei_me_cl_del(struct mei_device *dev, struct mei_me_client *me_cl)
+{
+	down_write(&dev->me_clients_rwsem);
+	__mei_me_cl_del(dev, me_cl);
+	up_write(&dev->me_clients_rwsem);
+}
+
+/**
  * mei_me_cl_add - add me client to the list
  *
  * @dev: mei device
@@ -317,7 +331,7 @@ static inline bool mei_cl_cmp_id(const struct mei_cl *cl1,
 {
 	return cl1 && cl2 &&
 		(cl1->host_client_id == cl2->host_client_id) &&
-		(cl1->me_client_id == cl2->me_client_id);
+		(mei_cl_me_id(cl1) == mei_cl_me_id(cl2));
 }
 
 /**
@@ -546,6 +560,7 @@ void mei_cl_init(struct mei_cl *cl, struct mei_device *dev)
 	INIT_LIST_HEAD(&cl->link);
 	INIT_LIST_HEAD(&cl->device_link);
 	cl->writing_state = MEI_IDLE;
+	cl->state = MEI_FILE_INITIALIZING;
 	cl->dev = dev;
 }
 
@@ -619,7 +634,7 @@ int mei_cl_link(struct mei_cl *cl, int id)
 }
 
 /**
- * mei_cl_unlink - remove me_cl from the list
+ * mei_cl_unlink - remove host client from the list
  *
  * @cl: host client
  *
@@ -667,17 +682,17 @@ void mei_host_client_init(struct work_struct *work)
 
 	me_cl = mei_me_cl_by_uuid(dev, &mei_amthif_guid);
 	if (me_cl)
-		mei_amthif_host_init(dev);
+		mei_amthif_host_init(dev, me_cl);
 	mei_me_cl_put(me_cl);
 
 	me_cl = mei_me_cl_by_uuid(dev, &mei_wd_guid);
 	if (me_cl)
-		mei_wd_host_init(dev);
+		mei_wd_host_init(dev, me_cl);
 	mei_me_cl_put(me_cl);
 
 	me_cl = mei_me_cl_by_uuid(dev, &mei_nfc_guid);
 	if (me_cl)
-		mei_nfc_host_init(dev);
+		mei_nfc_host_init(dev, me_cl);
 	mei_me_cl_put(me_cl);
 
 
@@ -699,7 +714,7 @@ void mei_host_client_init(struct work_struct *work)
 bool mei_hbuf_acquire(struct mei_device *dev)
 {
 	if (mei_pg_state(dev) == MEI_PG_ON ||
-	    dev->pg_event == MEI_PG_EVENT_WAIT) {
+	    mei_pg_in_transition(dev)) {
 		dev_dbg(dev->dev, "device is in pg\n");
 		return false;
 	}
@@ -715,6 +730,120 @@ bool mei_hbuf_acquire(struct mei_device *dev)
 }
 
 /**
+ * mei_cl_set_disconnected - set disconnected state and clear
+ *   associated states and resources
+ *
+ * @cl: host client
+ */
+void mei_cl_set_disconnected(struct mei_cl *cl)
+{
+	struct mei_device *dev = cl->dev;
+
+	if (cl->state == MEI_FILE_DISCONNECTED ||
+	    cl->state == MEI_FILE_INITIALIZING)
+		return;
+
+	cl->state = MEI_FILE_DISCONNECTED;
+	mei_io_list_flush(&dev->ctrl_rd_list, cl);
+	mei_io_list_flush(&dev->ctrl_wr_list, cl);
+	cl->mei_flow_ctrl_creds = 0;
+	cl->timer_count = 0;
+
+	if (!cl->me_cl)
+		return;
+
+	if (!WARN_ON(cl->me_cl->connect_count == 0))
+		cl->me_cl->connect_count--;
+
+	if (cl->me_cl->connect_count == 0)
+		cl->me_cl->mei_flow_ctrl_creds = 0;
+
+	mei_me_cl_put(cl->me_cl);
+	cl->me_cl = NULL;
+}
+
+static int mei_cl_set_connecting(struct mei_cl *cl, struct mei_me_client *me_cl)
+{
+	if (!mei_me_cl_get(me_cl))
+		return -ENOENT;
+
+	/* only one connection is allowed for fixed address clients */
+	if (me_cl->props.fixed_address) {
+		if (me_cl->connect_count) {
+			mei_me_cl_put(me_cl);
+			return -EBUSY;
+		}
+	}
+
+	cl->me_cl = me_cl;
+	cl->state = MEI_FILE_CONNECTING;
+	cl->me_cl->connect_count++;
+
+	return 0;
+}
+
+/*
+ * mei_cl_send_disconnect - send disconnect request
+ *
+ * @cl: host client
+ * @cb: callback block
+ *
+ * Return: 0, OK; otherwise, error.
+ */
+static int mei_cl_send_disconnect(struct mei_cl *cl, struct mei_cl_cb *cb)
+{
+	struct mei_device *dev;
+	int ret;
+
+	dev = cl->dev;
+
+	ret = mei_hbm_cl_disconnect_req(dev, cl);
+	cl->status = ret;
+	if (ret) {
+		cl->state = MEI_FILE_DISCONNECT_REPLY;
+		return ret;
+	}
+
+	list_move_tail(&cb->list, &dev->ctrl_rd_list.list);
+	cl->timer_count = MEI_CONNECT_TIMEOUT;
+
+	return 0;
+}
+
+/**
+ * mei_cl_irq_disconnect - processes close related operation from
+ *	interrupt thread context - send disconnect request
+ *
+ * @cl: client
+ * @cb: callback block.
+ * @cmpl_list: complete list.
+ *
+ * Return: 0, OK; otherwise, error.
+ */
+int mei_cl_irq_disconnect(struct mei_cl *cl, struct mei_cl_cb *cb,
+			    struct mei_cl_cb *cmpl_list)
+{
+	struct mei_device *dev = cl->dev;
+	u32 msg_slots;
+	int slots;
+	int ret;
+
+	msg_slots = mei_data2slots(sizeof(struct hbm_client_connect_request));
+	slots = mei_hbuf_empty_slots(dev);
+
+	if (slots < msg_slots)
+		return -EMSGSIZE;
+
+	ret = mei_cl_send_disconnect(cl, cb);
+	if (ret)
+		list_move_tail(&cb->list, &cmpl_list->list);
+
+	return ret;
+}
+
+
+
+/**
  * mei_cl_disconnect - disconnect host client from the me one
  *
  * @cl: host client
@@ -736,8 +865,13 @@ int mei_cl_disconnect(struct mei_cl *cl)
 
 	cl_dbg(dev, cl, "disconnecting");
 
-	if (cl->state != MEI_FILE_DISCONNECTING)
+	if (!mei_cl_is_connected(cl))
+		return 0;
+
+	if (mei_cl_is_fixed_address(cl)) {
+		mei_cl_set_disconnected(cl);
 		return 0;
+	}
 
 	rets = pm_runtime_get(dev->dev);
 	if (rets < 0 && rets != -EINPROGRESS) {
@@ -746,44 +880,41 @@ int mei_cl_disconnect(struct mei_cl *cl)
 		return rets;
 	}
 
+	cl->state = MEI_FILE_DISCONNECTING;
+
 	cb = mei_io_cb_init(cl, MEI_FOP_DISCONNECT, NULL);
 	rets = cb ? 0 : -ENOMEM;
 	if (rets)
-		goto free;
+		goto out;
+
+	cl_dbg(dev, cl, "add disconnect cb to control write list\n");
+	list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
 
 	if (mei_hbuf_acquire(dev)) {
-		if (mei_hbm_cl_disconnect_req(dev, cl)) {
-			rets = -ENODEV;
+		rets = mei_cl_send_disconnect(cl, cb);
+		if (rets) {
 			cl_err(dev, cl, "failed to disconnect.\n");
-			goto free;
+			goto out;
 		}
-		cl->timer_count = MEI_CONNECT_TIMEOUT;
-		mdelay(10); /* Wait for hardware disconnection ready */
-		list_add_tail(&cb->list, &dev->ctrl_rd_list.list);
-	} else {
-		cl_dbg(dev, cl, "add disconnect cb to control write list\n");
-		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
-
 	}
-	mutex_unlock(&dev->device_lock);
-
-	wait_event_timeout(cl->wait,
-			MEI_FILE_DISCONNECTED == cl->state,
-			mei_secs_to_jiffies(MEI_CL_CONNECT_TIMEOUT));
 
+	mutex_unlock(&dev->device_lock);
+	wait_event_timeout(cl->wait, cl->state == MEI_FILE_DISCONNECT_REPLY,
+			   mei_secs_to_jiffies(MEI_CL_CONNECT_TIMEOUT));
 	mutex_lock(&dev->device_lock);
 
-	if (MEI_FILE_DISCONNECTED == cl->state) {
-		rets = 0;
-		cl_dbg(dev, cl, "successfully disconnected from FW client.\n");
-	} else {
+	rets = cl->status;
+	if (cl->state != MEI_FILE_DISCONNECT_REPLY) {
 		cl_dbg(dev, cl, "timeout on disconnect from FW client.\n");
 		rets = -ETIME;
 	}
 
-	mei_io_list_flush(&dev->ctrl_rd_list, cl);
-	mei_io_list_flush(&dev->ctrl_wr_list, cl);
-free:
+out:
+	/* we disconnect also on error */
+	mei_cl_set_disconnected(cl);
+	if (!rets)
+		cl_dbg(dev, cl, "successfully disconnected from FW client.\n");
+
 	cl_dbg(dev, cl, "rpm: autosuspend\n");
 	pm_runtime_mark_last_busy(dev->dev);
 	pm_runtime_put_autosuspend(dev->dev);
@@ -801,53 +932,119 @@ free:
  *
  * Return: true if other client is connected, false - otherwise.
  */
-bool mei_cl_is_other_connecting(struct mei_cl *cl)
+static bool mei_cl_is_other_connecting(struct mei_cl *cl)
 {
 	struct mei_device *dev;
-	struct mei_cl *ocl; /* the other client */
-
-	if (WARN_ON(!cl || !cl->dev))
-		return false;
+	struct mei_cl_cb *cb;
 
 	dev = cl->dev;
 
-	list_for_each_entry(ocl, &dev->file_list, link) {
-		if (ocl->state == MEI_FILE_CONNECTING &&
-		    ocl != cl &&
-		    cl->me_client_id == ocl->me_client_id)
+	list_for_each_entry(cb, &dev->ctrl_rd_list.list, list) {
+		if (cb->fop_type == MEI_FOP_CONNECT &&
+		    mei_cl_me_id(cl) == mei_cl_me_id(cb->cl))
 			return true;
-
 	}
 
 	return false;
 }
 
 /**
+ * mei_cl_send_connect - send connect request
+ *
+ * @cl: host client
+ * @cb: callback block
+ *
+ * Return: 0, OK; otherwise, error.
+ */
+static int mei_cl_send_connect(struct mei_cl *cl, struct mei_cl_cb *cb)
+{
+	struct mei_device *dev;
+	int ret;
+
+	dev = cl->dev;
+
+	ret = mei_hbm_cl_connect_req(dev, cl);
+	cl->status = ret;
+	if (ret) {
+		cl->state = MEI_FILE_DISCONNECT_REPLY;
+		return ret;
+	}
+
+	list_move_tail(&cb->list, &dev->ctrl_rd_list.list);
+	cl->timer_count = MEI_CONNECT_TIMEOUT;
+	return 0;
+}
+
+/**
+ * mei_cl_irq_connect - send connect request in irq_thread context
+ *
+ * @cl: host client
+ * @cb: callback block
+ * @cmpl_list: complete list
+ *
+ * Return: 0, OK; otherwise, error.
+ */
+int mei_cl_irq_connect(struct mei_cl *cl, struct mei_cl_cb *cb,
+			      struct mei_cl_cb *cmpl_list)
+{
+	struct mei_device *dev = cl->dev;
+	u32 msg_slots;
+	int slots;
+	int rets;
+
+	msg_slots = mei_data2slots(sizeof(struct hbm_client_connect_request));
+	slots = mei_hbuf_empty_slots(dev);
+
+	if (mei_cl_is_other_connecting(cl))
+		return 0;
+
+	if (slots < msg_slots)
+		return -EMSGSIZE;
+
+	rets = mei_cl_send_connect(cl, cb);
+	if (rets)
+		list_move_tail(&cb->list, &cmpl_list->list);
+
+	return rets;
+}
+
+/**
  * mei_cl_connect - connect host client to the me one
  *
  * @cl: host client
+ * @me_cl: me client
  * @file: pointer to file structure
  *
  * Locking: called under "dev->device_lock" lock
  *
  * Return: 0 on success, <0 on failure.
  */
-int mei_cl_connect(struct mei_cl *cl, struct file *file)
+int mei_cl_connect(struct mei_cl *cl, struct mei_me_client *me_cl,
+		   struct file *file)
 {
 	struct mei_device *dev;
 	struct mei_cl_cb *cb;
 	int rets;
 
-	if (WARN_ON(!cl || !cl->dev))
+	if (WARN_ON(!cl || !cl->dev || !me_cl))
 		return -ENODEV;
 
 	dev = cl->dev;
 
+	rets = mei_cl_set_connecting(cl, me_cl);
+	if (rets)
+		return rets;
+
+	if (mei_cl_is_fixed_address(cl)) {
+		cl->state = MEI_FILE_CONNECTED;
+		return 0;
+	}
+
 	rets = pm_runtime_get(dev->dev);
 	if (rets < 0 && rets != -EINPROGRESS) {
 		pm_runtime_put_noidle(dev->dev);
 		cl_err(dev, cl, "rpm: get failed %d\n", rets);
-		return rets;
+		goto nortpm;
 	}
 
 	cb = mei_io_cb_init(cl, MEI_FOP_CONNECT, file);
@@ -855,45 +1052,40 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 	if (rets)
 		goto out;
 
+	list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
+
 	/* run hbuf acquire last so we don't have to undo */
 	if (!mei_cl_is_other_connecting(cl) && mei_hbuf_acquire(dev)) {
-		cl->state = MEI_FILE_CONNECTING;
-		if (mei_hbm_cl_connect_req(dev, cl)) {
-			rets = -ENODEV;
+		rets = mei_cl_send_connect(cl, cb);
+		if (rets)
 			goto out;
-		}
-		cl->timer_count = MEI_CONNECT_TIMEOUT;
-		list_add_tail(&cb->list, &dev->ctrl_rd_list.list);
-	} else {
-		cl->state = MEI_FILE_INITIALIZING;
-		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
 	}
 
 	mutex_unlock(&dev->device_lock);
 	wait_event_timeout(cl->wait,
 			(cl->state == MEI_FILE_CONNECTED ||
-			 cl->state == MEI_FILE_DISCONNECTED),
+			 cl->state == MEI_FILE_DISCONNECT_REPLY),
 			mei_secs_to_jiffies(MEI_CL_CONNECT_TIMEOUT));
 	mutex_lock(&dev->device_lock);
 
 	if (!mei_cl_is_connected(cl)) {
-		cl->state = MEI_FILE_DISCONNECTED;
-		/* something went really wrong */
+		/* timeout or something went really wrong */
 		if (!cl->status)
 			cl->status = -EFAULT;
-
-		mei_io_list_flush(&dev->ctrl_rd_list, cl);
-		mei_io_list_flush(&dev->ctrl_wr_list, cl);
 	}
 
 	rets = cl->status;
-
 out:
 	cl_dbg(dev, cl, "rpm: autosuspend\n");
 	pm_runtime_mark_last_busy(dev->dev);
 	pm_runtime_put_autosuspend(dev->dev);
 
 	mei_io_cb_free(cb);
+
+nortpm:
+	if (!mei_cl_is_connected(cl))
+		mei_cl_set_disconnected(cl);
+
 	return rets;
 }
 
@@ -934,36 +1126,29 @@ err:
  * @cl: private data of the file object
  *
  * Return: 1 if mei_flow_ctrl_creds >0, 0 - otherwise.
- *	-ENOENT if mei_cl is not present
- *	-EINVAL if single_recv_buf == 0
  */
 int mei_cl_flow_ctrl_creds(struct mei_cl *cl)
 {
-	struct mei_device *dev;
-	struct mei_me_client *me_cl;
-	int rets = 0;
+	int rets;
 
-	if (WARN_ON(!cl || !cl->dev))
+	if (WARN_ON(!cl || !cl->me_cl))
 		return -EINVAL;
 
-	dev = cl->dev;
-
 	if (cl->mei_flow_ctrl_creds > 0)
 		return 1;
 
-	me_cl = mei_me_cl_by_uuid_id(dev, &cl->cl_uuid, cl->me_client_id);
-	if (!me_cl) {
-		cl_err(dev, cl, "no such me client %d\n", cl->me_client_id);
-		return -ENOENT;
+	if (mei_cl_is_fixed_address(cl)) {
+		rets = mei_cl_read_start(cl, mei_cl_mtu(cl), NULL);
+		if (rets && rets != -EBUSY)
+			return rets;
+		return 1;
 	}
 
-	if (me_cl->mei_flow_ctrl_creds > 0) {
-		rets = 1;
-		if (WARN_ON(me_cl->props.single_recv_buf == 0))
-			rets = -EINVAL;
+	if (mei_cl_is_single_recv_buf(cl)) {
+		if (cl->me_cl->mei_flow_ctrl_creds > 0)
+			return 1;
 	}
-	mei_me_cl_put(me_cl);
-	return rets;
+	return 0;
 }
 
 /**
@@ -973,43 +1158,26 @@ int mei_cl_flow_ctrl_creds(struct mei_cl *cl)
  *
  * Return:
  *	0 on success
- *	-ENOENT when me client is not found
  *	-EINVAL when ctrl credits are <= 0
  */
 int mei_cl_flow_ctrl_reduce(struct mei_cl *cl)
 {
-	struct mei_device *dev;
-	struct mei_me_client *me_cl;
-	int rets;
-
-	if (WARN_ON(!cl || !cl->dev))
+	if (WARN_ON(!cl || !cl->me_cl))
 		return -EINVAL;
 
-	dev = cl->dev;
-
-	me_cl = mei_me_cl_by_uuid_id(dev, &cl->cl_uuid, cl->me_client_id);
-	if (!me_cl) {
-		cl_err(dev, cl, "no such me client %d\n", cl->me_client_id);
-		return -ENOENT;
-	}
+	if (mei_cl_is_fixed_address(cl))
+		return 0;
 
-	if (me_cl->props.single_recv_buf) {
-		if (WARN_ON(me_cl->mei_flow_ctrl_creds <= 0)) {
-			rets = -EINVAL;
-			goto out;
-		}
-		me_cl->mei_flow_ctrl_creds--;
+	if (mei_cl_is_single_recv_buf(cl)) {
+		if (WARN_ON(cl->me_cl->mei_flow_ctrl_creds <= 0))
+			return -EINVAL;
+		cl->me_cl->mei_flow_ctrl_creds--;
 	} else {
-		if (WARN_ON(cl->mei_flow_ctrl_creds <= 0)) {
-			rets = -EINVAL;
-			goto out;
-		}
+		if (WARN_ON(cl->mei_flow_ctrl_creds <= 0))
+			return -EINVAL;
 		cl->mei_flow_ctrl_creds--;
 	}
-	rets = 0;
-out:
-	mei_me_cl_put(me_cl);
-	return rets;
+	return 0;
 }
 
 /**
@@ -1025,7 +1193,6 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length, struct file *fp)
 {
 	struct mei_device *dev;
 	struct mei_cl_cb *cb;
-	struct mei_me_client *me_cl;
 	int rets;
 
 	if (WARN_ON(!cl || !cl->dev))
@@ -1040,27 +1207,29 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length, struct file *fp)
 	if (!list_empty(&cl->rd_pending))
 		return -EBUSY;
 
-	me_cl = mei_me_cl_by_uuid_id(dev, &cl->cl_uuid, cl->me_client_id);
-	if (!me_cl) {
-		cl_err(dev, cl, "no such me client %d\n", cl->me_client_id);
+	if (!mei_me_cl_is_active(cl->me_cl)) {
+		cl_err(dev, cl, "no such me client\n");
 		return  -ENOTTY;
 	}
+
 	/* always allocate at least client max message */
-	length = max_t(size_t, length, me_cl->props.max_msg_length);
-	mei_me_cl_put(me_cl);
+	length = max_t(size_t, length, mei_cl_mtu(cl));
+	cb = mei_cl_alloc_cb(cl, length, MEI_FOP_READ, fp);
+	if (!cb)
+		return -ENOMEM;
+
+	if (mei_cl_is_fixed_address(cl)) {
+		list_add_tail(&cb->list, &cl->rd_pending);
+		return 0;
+	}
 
 	rets = pm_runtime_get(dev->dev);
 	if (rets < 0 && rets != -EINPROGRESS) {
 		pm_runtime_put_noidle(dev->dev);
 		cl_err(dev, cl, "rpm: get failed %d\n", rets);
-		return rets;
+		goto nortpm;
 	}
 
-	cb = mei_cl_alloc_cb(cl, length, MEI_FOP_READ, fp);
-	rets = cb ? 0 : -ENOMEM;
-	if (rets)
-		goto out;
-
 	if (mei_hbuf_acquire(dev)) {
 		rets = mei_hbm_cl_flow_control_req(dev, cl);
 		if (rets < 0)
@@ -1068,6 +1237,7 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length, struct file *fp)
 
 		list_add_tail(&cb->list, &cl->rd_pending);
 	} else {
+		rets = 0;
 		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
 	}
 
@@ -1075,7 +1245,7 @@ out:
 	cl_dbg(dev, cl, "rpm: autosuspend\n");
 	pm_runtime_mark_last_busy(dev->dev);
 	pm_runtime_put_autosuspend(dev->dev);
-
+nortpm:
 	if (rets)
 		mei_io_cb_free(cb);
 
@@ -1102,6 +1272,7 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 	u32 msg_slots;
 	int slots;
 	int rets;
+	bool first_chunk;
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -ENODEV;
@@ -1110,7 +1281,9 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 
 	buf = &cb->buf;
 
-	rets = mei_cl_flow_ctrl_creds(cl);
+	first_chunk = cb->buf_idx == 0;
+
+	rets = first_chunk ? mei_cl_flow_ctrl_creds(cl) : 1;
 	if (rets < 0)
 		return rets;
 
@@ -1123,8 +1296,8 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 	len = buf->size - cb->buf_idx;
 	msg_slots = mei_data2slots(len);
 
-	mei_hdr.host_addr = cl->host_client_id;
-	mei_hdr.me_addr = cl->me_client_id;
+	mei_hdr.host_addr = mei_cl_host_addr(cl);
+	mei_hdr.me_addr = mei_cl_me_id(cl);
 	mei_hdr.reserved = 0;
 	mei_hdr.internal = cb->internal;
 
@@ -1157,12 +1330,14 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 	cb->buf_idx += mei_hdr.length;
 	cb->completed = mei_hdr.msg_complete == 1;
 
-	if (mei_hdr.msg_complete) {
+	if (first_chunk) {
 		if (mei_cl_flow_ctrl_reduce(cl))
 			return -EIO;
-		list_move_tail(&cb->list, &dev->write_waiting_list.list);
 	}
 
+	if (mei_hdr.msg_complete)
+		list_move_tail(&cb->list, &dev->write_waiting_list.list);
+
 	return 0;
 }
 
@@ -1207,8 +1382,8 @@ int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking)
 	cb->buf_idx = 0;
 	cl->writing_state = MEI_IDLE;
 
-	mei_hdr.host_addr = cl->host_client_id;
-	mei_hdr.me_addr = cl->me_client_id;
+	mei_hdr.host_addr = mei_cl_host_addr(cl);
+	mei_hdr.me_addr = mei_cl_me_id(cl);
 	mei_hdr.reserved = 0;
 	mei_hdr.msg_complete = 0;
 	mei_hdr.internal = cb->internal;
@@ -1241,21 +1416,19 @@ int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking)
 	if (rets)
 		goto err;
 
+	rets = mei_cl_flow_ctrl_reduce(cl);
+	if (rets)
+		goto err;
+
 	cl->writing_state = MEI_WRITING;
 	cb->buf_idx = mei_hdr.length;
 	cb->completed = mei_hdr.msg_complete == 1;
 
 out:
-	if (mei_hdr.msg_complete) {
-		rets = mei_cl_flow_ctrl_reduce(cl);
-		if (rets < 0)
-			goto err;
-
+	if (mei_hdr.msg_complete)
 		list_add_tail(&cb->list, &dev->write_waiting_list.list);
-	} else {
+	else
 		list_add_tail(&cb->list, &dev->write_list.list);
-	}
-
 
 	if (blocking && cl->writing_state != MEI_WRITE_COMPLETE) {
 
@@ -1289,20 +1462,36 @@ err:
  */
 void mei_cl_complete(struct mei_cl *cl, struct mei_cl_cb *cb)
 {
-	if (cb->fop_type == MEI_FOP_WRITE) {
+	struct mei_device *dev = cl->dev;
+
+	switch (cb->fop_type) {
+	case MEI_FOP_WRITE:
 		mei_io_cb_free(cb);
-		cb = NULL;
 		cl->writing_state = MEI_WRITE_COMPLETE;
-		if (waitqueue_active(&cl->tx_wait))
+		if (waitqueue_active(&cl->tx_wait)) {
 			wake_up_interruptible(&cl->tx_wait);
+		} else {
+			pm_runtime_mark_last_busy(dev->dev);
+			pm_request_autosuspend(dev->dev);
+		}
+		break;
 
-	} else if (cb->fop_type == MEI_FOP_READ) {
+	case MEI_FOP_READ:
 		list_add_tail(&cb->list, &cl->rd_completed);
 		if (waitqueue_active(&cl->rx_wait))
 			wake_up_interruptible_all(&cl->rx_wait);
 		else
 			mei_cl_bus_rx_event(cl);
+		break;
+
+	case MEI_FOP_CONNECT:
+	case MEI_FOP_DISCONNECT:
+		if (waitqueue_active(&cl->wait))
+			wake_up(&cl->wait);
 
+		break;
+	default:
+		BUG_ON(0);
 	}
 }
 
@@ -1312,16 +1501,12 @@ void mei_cl_complete(struct mei_cl *cl, struct mei_cl_cb *cb)
  *
  * @dev: mei device
  */
-
 void mei_cl_all_disconnect(struct mei_device *dev)
 {
 	struct mei_cl *cl;
 
-	list_for_each_entry(cl, &dev->file_list, link) {
-		cl->state = MEI_FILE_DISCONNECTED;
-		cl->mei_flow_ctrl_creds = 0;
-		cl->timer_count = 0;
-	}
+	list_for_each_entry(cl, &dev->file_list, link)
+		mei_cl_set_disconnected(cl);
 }
 
 
diff --git a/drivers/misc/mei/client.h b/drivers/misc/mei/client.h
index 0a39e5d45171..8d7f057f1045 100644
--- a/drivers/misc/mei/client.h
+++ b/drivers/misc/mei/client.h
@@ -44,6 +44,30 @@ void mei_me_cl_rm_by_uuid_id(struct mei_device *dev,
 			     const uuid_le *uuid, u8 id);
 void mei_me_cl_rm_all(struct mei_device *dev);
 
+/**
+ * mei_me_cl_is_active - check whether me client is active in the fw
+ *
+ * @me_cl: me client
+ *
+ * Return: true if the me client is active in the firmware
+ */
+static inline bool mei_me_cl_is_active(const struct mei_me_client *me_cl)
+{
+	return !list_empty_careful(&me_cl->list);
+}
+
+/**
+ * mei_me_cl_uuid - return me client protocol name (uuid)
+ *
+ * @me_cl: me client
+ *
+ * Return: me client protocol name
+ */
+static inline const uuid_le *mei_me_cl_uuid(const struct mei_me_client *me_cl)
+{
+	return &me_cl->props.protocol_name;
+}
+
 /*
  * MEI IO Functions
  */
@@ -94,18 +118,96 @@ int mei_cl_flow_ctrl_reduce(struct mei_cl *cl);
 /**
  * mei_cl_is_connected - host client is connected
  *
- * @cl: host clinet
+ * @cl: host client
  *
- * Return: true if the host clinet is connected
+ * Return: true if the host client is connected
  */
 static inline bool mei_cl_is_connected(struct mei_cl *cl)
 {
 	return  cl->state == MEI_FILE_CONNECTED;
 }
 
-bool mei_cl_is_other_connecting(struct mei_cl *cl);
+/**
+ * mei_cl_me_id - me client id
+ *
+ * @cl: host client
+ *
+ * Return: me client id or 0 if client is not connected
+ */
+static inline u8 mei_cl_me_id(const struct mei_cl *cl)
+{
+	return cl->me_cl ? cl->me_cl->client_id : 0;
+}
+
+/**
+ * mei_cl_mtu - maximal message that client can send and receive
+ *
+ * @cl: host client
+ *
+ * Return: mtu
+ */
+static inline size_t mei_cl_mtu(const struct mei_cl *cl)
+{
+	return cl->me_cl->props.max_msg_length;
+}
+
+/**
+ * mei_cl_is_fixed_address - check whether the me client uses fixed address
+ *
+ * @cl: host client
+ *
+ * Return: true if the client is connected and it has fixed me address
+ */
+static inline bool mei_cl_is_fixed_address(const struct mei_cl *cl)
+{
+	return cl->me_cl && cl->me_cl->props.fixed_address;
+}
+
+/**
+ * mei_cl_is_single_recv_buf- check whether the me client
+ *       uses single receiving buffer
+ *
+ * @cl: host client
+ *
+ * Return: true if single_recv_buf == 1; 0 otherwise
+ */
+static inline bool mei_cl_is_single_recv_buf(const struct mei_cl *cl)
+{
+	return cl->me_cl->props.single_recv_buf;
+}
+
+/**
+ * mei_cl_uuid -  client's uuid
+ *
+ * @cl: host client
+ *
+ * Return: return uuid of connected me client
+ */
+static inline const uuid_le *mei_cl_uuid(const struct mei_cl *cl)
+{
+	return mei_me_cl_uuid(cl->me_cl);
+}
+
+/**
+ * mei_cl_host_addr - client's host address
+ *
+ * @cl: host client
+ *
+ * Return: 0 for fixed address client, host address for dynamic client
+ */
+static inline u8 mei_cl_host_addr(const struct mei_cl *cl)
+{
+	return  mei_cl_is_fixed_address(cl) ? 0 : cl->host_client_id;
+}
+
 int mei_cl_disconnect(struct mei_cl *cl);
-int mei_cl_connect(struct mei_cl *cl, struct file *file);
+void mei_cl_set_disconnected(struct mei_cl *cl);
+int mei_cl_irq_disconnect(struct mei_cl *cl, struct mei_cl_cb *cb,
+			  struct mei_cl_cb *cmpl_list);
+int mei_cl_connect(struct mei_cl *cl, struct mei_me_client *me_cl,
+		   struct file *file);
+int mei_cl_irq_connect(struct mei_cl *cl, struct mei_cl_cb *cb,
+			      struct mei_cl_cb *cmpl_list);
 int mei_cl_read_start(struct mei_cl *cl, size_t length, struct file *fp);
 int mei_cl_irq_read_msg(struct mei_cl *cl, struct mei_msg_hdr *hdr,
 			struct mei_cl_cb *cmpl_list);
@@ -117,14 +219,12 @@ void mei_cl_complete(struct mei_cl *cl, struct mei_cl_cb *cb);
 
 void mei_host_client_init(struct work_struct *work);
 
-
-
 void mei_cl_all_disconnect(struct mei_device *dev);
 void mei_cl_all_wakeup(struct mei_device *dev);
 void mei_cl_all_write_clear(struct mei_device *dev);
 
 #define MEI_CL_FMT "cl:host=%02d me=%02d "
-#define MEI_CL_PRM(cl) (cl)->host_client_id, (cl)->me_client_id
+#define MEI_CL_PRM(cl) (cl)->host_client_id, mei_cl_me_id(cl)
 
 #define cl_dbg(dev, cl, format, arg...) \
 	dev_dbg((dev)->dev, MEI_CL_FMT format, MEI_CL_PRM(cl), ##arg)
diff --git a/drivers/misc/mei/debugfs.c b/drivers/misc/mei/debugfs.c
index d9cd7e6ee484..eb868341247f 100644
--- a/drivers/misc/mei/debugfs.c
+++ b/drivers/misc/mei/debugfs.c
@@ -116,7 +116,7 @@ static ssize_t mei_dbgfs_read_active(struct file *fp, char __user *ubuf,
 
 		pos += scnprintf(buf + pos, bufsz - pos,
 			"%2d|%2d|%4d|%5d|%2d|%2d|\n",
-			i, cl->me_client_id, cl->host_client_id, cl->state,
+			i, mei_cl_me_id(cl), cl->host_client_id, cl->state,
 			!list_empty(&cl->rd_completed), cl->writing_state);
 		i++;
 	}
@@ -149,6 +149,13 @@ static ssize_t mei_dbgfs_read_devstate(struct file *fp, char __user *ubuf,
 			mei_dev_state_str(dev->dev_state));
 	pos += scnprintf(buf + pos, bufsz - pos, "hbm: %s\n",
 			mei_hbm_state_str(dev->hbm_state));
+
+	if (dev->hbm_state == MEI_HBM_STARTED) {
+		pos += scnprintf(buf + pos, bufsz - pos, "hbm features:\n");
+		pos += scnprintf(buf + pos, bufsz - pos, "\tPG: %01d\n",
+				 dev->hbm_f_pg_supported);
+	}
+
 	pos += scnprintf(buf + pos, bufsz - pos, "pg:  %s, %s\n",
 			mei_pg_is_enabled(dev) ? "ENABLED" : "DISABLED",
 			mei_pg_state_str(mei_pg_state(dev)));
@@ -209,6 +216,12 @@ int mei_dbgfs_register(struct mei_device *dev, const char *name)
 		dev_err(dev->dev, "devstate: registration failed\n");
 		goto err;
 	}
+	f = debugfs_create_bool("allow_fixed_address", S_IRUSR | S_IWUSR, dir,
+				&dev->allow_fixed_address);
+	if (!f) {
+		dev_err(dev->dev, "allow_fixed_address: registration failed\n");
+		goto err;
+	}
 	dev->dbgfs_dir = dir;
 	return 0;
 err:
diff --git a/drivers/misc/mei/hbm.c b/drivers/misc/mei/hbm.c
index 58da92565c5e..a4f283165a33 100644
--- a/drivers/misc/mei/hbm.c
+++ b/drivers/misc/mei/hbm.c
@@ -150,8 +150,8 @@ void mei_hbm_cl_hdr(struct mei_cl *cl, u8 hbm_cmd, void *buf, size_t len)
 	memset(cmd, 0, len);
 
 	cmd->hbm_cmd = hbm_cmd;
-	cmd->host_addr = cl->host_client_id;
-	cmd->me_addr = cl->me_client_id;
+	cmd->host_addr = mei_cl_host_addr(cl);
+	cmd->me_addr = mei_cl_me_id(cl);
 }
 
 /**
@@ -188,8 +188,8 @@ int mei_hbm_cl_write(struct mei_device *dev,
 static inline
 bool mei_hbm_cl_addr_equal(struct mei_cl *cl, struct mei_hbm_cl_cmd *cmd)
 {
-	return cl->host_client_id == cmd->host_addr &&
-		cl->me_client_id == cmd->me_addr;
+	return  mei_cl_host_addr(cl) == cmd->host_addr &&
+		mei_cl_me_id(cl) == cmd->me_addr;
 }
 
 /**
@@ -572,7 +572,7 @@ static void mei_hbm_cl_disconnect_res(struct mei_device *dev, struct mei_cl *cl,
 	cl_dbg(dev, cl, "hbm: disconnect response status=%d\n", rs->status);
 
 	if (rs->status == MEI_CL_DISCONN_SUCCESS)
-		cl->state = MEI_FILE_DISCONNECTED;
+		cl->state = MEI_FILE_DISCONNECT_REPLY;
 	cl->status = 0;
 }
 
@@ -611,7 +611,7 @@ static void mei_hbm_cl_connect_res(struct mei_device *dev, struct mei_cl *cl,
 	if (rs->status == MEI_CL_CONN_SUCCESS)
 		cl->state = MEI_FILE_CONNECTED;
 	else
-		cl->state = MEI_FILE_DISCONNECTED;
+		cl->state = MEI_FILE_DISCONNECT_REPLY;
 	cl->status = mei_cl_conn_status_to_errno(rs->status);
 }
 
@@ -680,8 +680,8 @@ static int mei_hbm_fw_disconnect_req(struct mei_device *dev,
 
 	cl = mei_hbm_cl_find_by_cmd(dev, disconnect_req);
 	if (cl) {
-		cl_dbg(dev, cl, "disconnect request received\n");
-		cl->state = MEI_FILE_DISCONNECTED;
+		cl_dbg(dev, cl, "fw disconnect request received\n");
+		cl->state = MEI_FILE_DISCONNECTING;
 		cl->timer_count = 0;
 
 		cb = mei_io_cb_init(cl, MEI_FOP_DISCONNECT_RSP, NULL);
diff --git a/drivers/misc/mei/hw-me.c b/drivers/misc/mei/hw-me.c
index 6fb75e62a764..43d7101ff993 100644
--- a/drivers/misc/mei/hw-me.c
+++ b/drivers/misc/mei/hw-me.c
@@ -663,11 +663,27 @@ int mei_me_pg_exit_sync(struct mei_device *dev)
 	mutex_lock(&dev->device_lock);
 
 reply:
-	if (dev->pg_event == MEI_PG_EVENT_RECEIVED)
-		ret = mei_hbm_pg(dev, MEI_PG_ISOLATION_EXIT_RES_CMD);
+	if (dev->pg_event != MEI_PG_EVENT_RECEIVED) {
+		ret = -ETIME;
+		goto out;
+	}
+
+	dev->pg_event = MEI_PG_EVENT_INTR_WAIT;
+	ret = mei_hbm_pg(dev, MEI_PG_ISOLATION_EXIT_RES_CMD);
+	if (ret)
+		return ret;
+
+	mutex_unlock(&dev->device_lock);
+	wait_event_timeout(dev->wait_pg,
+		dev->pg_event == MEI_PG_EVENT_INTR_RECEIVED, timeout);
+	mutex_lock(&dev->device_lock);
+
+	if (dev->pg_event == MEI_PG_EVENT_INTR_RECEIVED)
+		ret = 0;
 	else
 		ret = -ETIME;
 
+out:
 	dev->pg_event = MEI_PG_EVENT_IDLE;
 	hw->pg_state = MEI_PG_OFF;
 
@@ -675,6 +691,19 @@ reply:
 }
 
 /**
+ * mei_me_pg_in_transition - is device now in pg transition
+ *
+ * @dev: the device structure
+ *
+ * Return: true if in pg transition, false otherwise
+ */
+static bool mei_me_pg_in_transition(struct mei_device *dev)
+{
+	return dev->pg_event >= MEI_PG_EVENT_WAIT &&
+	       dev->pg_event <= MEI_PG_EVENT_INTR_WAIT;
+}
+
+/**
  * mei_me_pg_is_enabled - detect if PG is supported by HW
  *
  * @dev: the device structure
@@ -705,6 +734,24 @@ notsupported:
 }
 
 /**
+ * mei_me_pg_intr - perform pg processing in interrupt thread handler
+ *
+ * @dev: the device structure
+ */
+static void mei_me_pg_intr(struct mei_device *dev)
+{
+	struct mei_me_hw *hw = to_me_hw(dev);
+
+	if (dev->pg_event != MEI_PG_EVENT_INTR_WAIT)
+		return;
+
+	dev->pg_event = MEI_PG_EVENT_INTR_RECEIVED;
+	hw->pg_state = MEI_PG_OFF;
+	if (waitqueue_active(&dev->wait_pg))
+		wake_up(&dev->wait_pg);
+}
+
+/**
  * mei_me_irq_quick_handler - The ISR of the MEI device
  *
  * @irq: The irq number
@@ -761,6 +808,8 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 		goto end;
 	}
 
+	mei_me_pg_intr(dev);
+
 	/*  check if we need to start the dev */
 	if (!mei_host_is_ready(dev)) {
 		if (mei_hw_is_ready(dev)) {
@@ -797,9 +846,10 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 	/*
 	 * During PG handshake only allowed write is the replay to the
 	 * PG exit message, so block calling write function
-	 * if the pg state is not idle
+	 * if the pg event is in PG handshake
 	 */
-	if (dev->pg_event == MEI_PG_EVENT_IDLE) {
+	if (dev->pg_event != MEI_PG_EVENT_WAIT &&
+	    dev->pg_event != MEI_PG_EVENT_RECEIVED) {
 		rets = mei_irq_write_handler(dev, &complete_list);
 		dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
 	}
@@ -824,6 +874,7 @@ static const struct mei_hw_ops mei_me_hw_ops = {
 	.hw_config = mei_me_hw_config,
 	.hw_start = mei_me_hw_start,
 
+	.pg_in_transition = mei_me_pg_in_transition,
 	.pg_is_enabled = mei_me_pg_is_enabled,
 
 	.intr_clear = mei_me_intr_clear,
diff --git a/drivers/misc/mei/hw-txe.c b/drivers/misc/mei/hw-txe.c
index 7abafe7d120d..bae680c648ff 100644
--- a/drivers/misc/mei/hw-txe.c
+++ b/drivers/misc/mei/hw-txe.c
@@ -16,6 +16,7 @@
 
 #include <linux/pci.h>
 #include <linux/jiffies.h>
+#include <linux/ktime.h>
 #include <linux/delay.h>
 #include <linux/kthread.h>
 #include <linux/irqreturn.h>
@@ -218,26 +219,25 @@ static u32 mei_txe_aliveness_get(struct mei_device *dev)
  *
  * Polls for HICR_HOST_ALIVENESS_RESP.ALIVENESS_RESP to be set
  *
- * Return: > 0 if the expected value was received, -ETIME otherwise
+ * Return: 0 if the expected value was received, -ETIME otherwise
  */
 static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
-	int t = 0;
+	ktime_t stop, start;
 
+	start = ktime_get();
+	stop = ktime_add(start, ms_to_ktime(SEC_ALIVENESS_WAIT_TIMEOUT));
 	do {
 		hw->aliveness = mei_txe_aliveness_get(dev);
 		if (hw->aliveness == expected) {
 			dev->pg_event = MEI_PG_EVENT_IDLE;
-			dev_dbg(dev->dev,
-				"aliveness settled after %d msecs\n", t);
-			return t;
+			dev_dbg(dev->dev, "aliveness settled after %lld usecs\n",
+				ktime_to_us(ktime_sub(ktime_get(), start)));
+			return 0;
 		}
-		mutex_unlock(&dev->device_lock);
-		msleep(MSEC_PER_SEC / 5);
-		mutex_lock(&dev->device_lock);
-		t += MSEC_PER_SEC / 5;
-	} while (t < SEC_ALIVENESS_WAIT_TIMEOUT);
+		usleep_range(20, 50);
+	} while (ktime_before(ktime_get(), stop));
 
 	dev->pg_event = MEI_PG_EVENT_IDLE;
 	dev_err(dev->dev, "aliveness timed out\n");
@@ -302,6 +302,18 @@ int mei_txe_aliveness_set_sync(struct mei_device *dev, u32 req)
 }
 
 /**
+ * mei_txe_pg_in_transition - is device now in pg transition
+ *
+ * @dev: the device structure
+ *
+ * Return: true if in pg transition, false otherwise
+ */
+static bool mei_txe_pg_in_transition(struct mei_device *dev)
+{
+	return dev->pg_event == MEI_PG_EVENT_WAIT;
+}
+
+/**
  * mei_txe_pg_is_enabled - detect if PG is supported by HW
  *
  * @dev: the device structure
@@ -1138,6 +1150,7 @@ static const struct mei_hw_ops mei_txe_hw_ops = {
 	.hw_config = mei_txe_hw_config,
 	.hw_start = mei_txe_hw_start,
 
+	.pg_in_transition = mei_txe_pg_in_transition,
 	.pg_is_enabled = mei_txe_pg_is_enabled,
 
 	.intr_clear = mei_txe_intr_clear,
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index 97353cf8d9b6..00c3865ca3b1 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -333,8 +333,6 @@ void mei_stop(struct mei_device *dev)
 
 	mei_nfc_host_exit(dev);
 
-	mei_cl_bus_remove_devices(dev);
-
 	mutex_lock(&dev->device_lock);
 
 	mei_wd_stop(dev);
@@ -361,13 +359,15 @@ bool mei_write_is_idle(struct mei_device *dev)
 {
 	bool idle = (dev->dev_state == MEI_DEV_ENABLED &&
 		list_empty(&dev->ctrl_wr_list.list) &&
-		list_empty(&dev->write_list.list));
+		list_empty(&dev->write_list.list)   &&
+		list_empty(&dev->write_waiting_list.list));
 
-	dev_dbg(dev->dev, "write pg: is idle[%d] state=%s ctrl=%d write=%d\n",
+	dev_dbg(dev->dev, "write pg: is idle[%d] state=%s ctrl=%01d write=%01d wwait=%01d\n",
 		idle,
 		mei_dev_state_str(dev->dev_state),
 		list_empty(&dev->ctrl_wr_list.list),
-		list_empty(&dev->write_list.list));
+		list_empty(&dev->write_list.list),
+		list_empty(&dev->write_waiting_list.list));
 
 	return idle;
 }
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index 3f84d2edcde4..3f3405269c39 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -65,8 +65,8 @@ EXPORT_SYMBOL_GPL(mei_irq_compl_handler);
 static inline int mei_cl_hbm_equal(struct mei_cl *cl,
 			struct mei_msg_hdr *mei_hdr)
 {
-	return cl->host_client_id == mei_hdr->host_addr &&
-		cl->me_client_id == mei_hdr->me_addr;
+	return  mei_cl_host_addr(cl) == mei_hdr->host_addr &&
+		mei_cl_me_id(cl) == mei_hdr->me_addr;
 }
 
 /**
@@ -180,56 +180,14 @@ static int mei_cl_irq_disconnect_rsp(struct mei_cl *cl, struct mei_cl_cb *cb,
 		return -EMSGSIZE;
 
 	ret = mei_hbm_cl_disconnect_rsp(dev, cl);
-
-	cl->state = MEI_FILE_DISCONNECTED;
-	cl->status = 0;
+	mei_cl_set_disconnected(cl);
 	mei_io_cb_free(cb);
+	mei_me_cl_put(cl->me_cl);
+	cl->me_cl = NULL;
 
 	return ret;
 }
 
-
-
-/**
- * mei_cl_irq_disconnect - processes close related operation from
- *	interrupt thread context - send disconnect request
- *
- * @cl: client
- * @cb: callback block.
- * @cmpl_list: complete list.
- *
- * Return: 0, OK; otherwise, error.
- */
-static int mei_cl_irq_disconnect(struct mei_cl *cl, struct mei_cl_cb *cb,
-			    struct mei_cl_cb *cmpl_list)
-{
-	struct mei_device *dev = cl->dev;
-	u32 msg_slots;
-	int slots;
-
-	msg_slots = mei_data2slots(sizeof(struct hbm_client_connect_request));
-	slots = mei_hbuf_empty_slots(dev);
-
-	if (slots < msg_slots)
-		return -EMSGSIZE;
-
-	if (mei_hbm_cl_disconnect_req(dev, cl)) {
-		cl->status = 0;
-		cb->buf_idx = 0;
-		list_move_tail(&cb->list, &cmpl_list->list);
-		return -EIO;
-	}
-
-	cl->state = MEI_FILE_DISCONNECTING;
-	cl->status = 0;
-	cb->buf_idx = 0;
-	list_move_tail(&cb->list, &dev->ctrl_rd_list.list);
-	cl->timer_count = MEI_CONNECT_TIMEOUT;
-
-	return 0;
-}
-
-
 /**
  * mei_cl_irq_read - processes client read related operation from the
  *	interrupt thread context - request for flow control credits
@@ -267,49 +225,6 @@ static int mei_cl_irq_read(struct mei_cl *cl, struct mei_cl_cb *cb,
 	return 0;
 }
 
-
-/**
- * mei_cl_irq_connect - send connect request in irq_thread context
- *
- * @cl: client
- * @cb: callback block.
- * @cmpl_list: complete list.
- *
- * Return: 0, OK; otherwise, error.
- */
-static int mei_cl_irq_connect(struct mei_cl *cl, struct mei_cl_cb *cb,
-			      struct mei_cl_cb *cmpl_list)
-{
-	struct mei_device *dev = cl->dev;
-	u32 msg_slots;
-	int slots;
-	int ret;
-
-	msg_slots = mei_data2slots(sizeof(struct hbm_client_connect_request));
-	slots = mei_hbuf_empty_slots(dev);
-
-	if (mei_cl_is_other_connecting(cl))
-		return 0;
-
-	if (slots < msg_slots)
-		return -EMSGSIZE;
-
-	cl->state = MEI_FILE_CONNECTING;
-
-	ret = mei_hbm_cl_connect_req(dev, cl);
-	if (ret) {
-		cl->status = ret;
-		cb->buf_idx = 0;
-		list_del_init(&cb->list);
-		return ret;
-	}
-
-	list_move_tail(&cb->list, &dev->ctrl_rd_list.list);
-	cl->timer_count = MEI_CONNECT_TIMEOUT;
-	return 0;
-}
-
-
 /**
  * mei_irq_read_handler - bottom half read routine after ISR to
  * handle the read processing.
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index 3e2968159506..8eb0a9500a90 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -94,7 +94,7 @@ static int mei_release(struct inode *inode, struct file *file)
 {
 	struct mei_cl *cl = file->private_data;
 	struct mei_device *dev;
-	int rets = 0;
+	int rets;
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -ENODEV;
@@ -106,11 +106,8 @@ static int mei_release(struct inode *inode, struct file *file)
 		rets = mei_amthif_release(dev, file);
 		goto out;
 	}
-	if (mei_cl_is_connected(cl)) {
-		cl->state = MEI_FILE_DISCONNECTING;
-		cl_dbg(dev, cl, "disconnecting\n");
-		rets = mei_cl_disconnect(cl);
-	}
+	rets = mei_cl_disconnect(cl);
+
 	mei_cl_flush_queues(cl, file);
 	cl_dbg(dev, cl, "removing\n");
 
@@ -186,8 +183,7 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 
 	err = mei_cl_read_start(cl, length, file);
 	if (err && err != -EBUSY) {
-		dev_dbg(dev->dev,
-			"mei start read failure with status = %d\n", err);
+		cl_dbg(dev, cl, "mei start read failure status = %d\n", err);
 		rets = err;
 		goto out;
 	}
@@ -218,6 +214,11 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 
 	cb = mei_cl_read_cb(cl, file);
 	if (!cb) {
+		if (mei_cl_is_fixed_address(cl) && dev->allow_fixed_address) {
+			cb = mei_cl_read_cb(cl, NULL);
+			if (cb)
+				goto copy_buffer;
+		}
 		rets = 0;
 		goto out;
 	}
@@ -226,11 +227,11 @@ copy_buffer:
 	/* now copy the data to user space */
 	if (cb->status) {
 		rets = cb->status;
-		dev_dbg(dev->dev, "read operation failed %d\n", rets);
+		cl_dbg(dev, cl, "read operation failed %d\n", rets);
 		goto free;
 	}
 
-	dev_dbg(dev->dev, "buf.size = %d buf.idx= %ld\n",
+	cl_dbg(dev, cl, "buf.size = %d buf.idx = %ld\n",
 	    cb->buf.size, cb->buf_idx);
 	if (length == 0 || ubuf == NULL || *offset > cb->buf_idx) {
 		rets = -EMSGSIZE;
@@ -256,7 +257,7 @@ free:
 	mei_io_cb_free(cb);
 
 out:
-	dev_dbg(dev->dev, "end mei read rets= %d\n", rets);
+	cl_dbg(dev, cl, "end mei read rets = %d\n", rets);
 	mutex_unlock(&dev->device_lock);
 	return rets;
 }
@@ -274,7 +275,6 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 			 size_t length, loff_t *offset)
 {
 	struct mei_cl *cl = file->private_data;
-	struct mei_me_client *me_cl = NULL;
 	struct mei_cl_cb *write_cb = NULL;
 	struct mei_device *dev;
 	unsigned long timeout = 0;
@@ -292,27 +292,27 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 		goto out;
 	}
 
-	me_cl = mei_me_cl_by_uuid_id(dev, &cl->cl_uuid, cl->me_client_id);
-	if (!me_cl) {
-		rets = -ENOTTY;
+	if (!mei_cl_is_connected(cl)) {
+		cl_err(dev, cl, "is not connected");
+		rets = -ENODEV;
 		goto out;
 	}
 
-	if (length == 0) {
-		rets = 0;
+	if (!mei_me_cl_is_active(cl->me_cl)) {
+		rets = -ENOTTY;
 		goto out;
 	}
 
-	if (length > me_cl->props.max_msg_length) {
+	if (length > mei_cl_mtu(cl)) {
 		rets = -EFBIG;
 		goto out;
 	}
 
-	if (!mei_cl_is_connected(cl)) {
-		cl_err(dev, cl, "is not connected");
-		rets = -ENODEV;
+	if (length == 0) {
+		rets = 0;
 		goto out;
 	}
+
 	if (cl == &dev->iamthif_cl) {
 		write_cb = mei_amthif_find_read_list_entry(dev, file);
 
@@ -350,14 +350,12 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 				"amthif write failed with status = %d\n", rets);
 			goto out;
 		}
-		mei_me_cl_put(me_cl);
 		mutex_unlock(&dev->device_lock);
 		return length;
 	}
 
 	rets = mei_cl_write(cl, write_cb, false);
 out:
-	mei_me_cl_put(me_cl);
 	mutex_unlock(&dev->device_lock);
 	if (rets < 0)
 		mei_io_cb_free(write_cb);
@@ -395,17 +393,16 @@ static int mei_ioctl_connect_client(struct file *file,
 
 	/* find ME client we're trying to connect to */
 	me_cl = mei_me_cl_by_uuid(dev, &data->in_client_uuid);
-	if (!me_cl || me_cl->props.fixed_address) {
+	if (!me_cl ||
+	    (me_cl->props.fixed_address && !dev->allow_fixed_address)) {
 		dev_dbg(dev->dev, "Cannot connect to FW Client UUID = %pUl\n",
-				&data->in_client_uuid);
+			&data->in_client_uuid);
+		mei_me_cl_put(me_cl);
 		return  -ENOTTY;
 	}
 
-	cl->me_client_id = me_cl->client_id;
-	cl->cl_uuid = me_cl->props.protocol_name;
-
 	dev_dbg(dev->dev, "Connect to FW Client ID = %d\n",
-			cl->me_client_id);
+			me_cl->client_id);
 	dev_dbg(dev->dev, "FW Client - Protocol Version = %d\n",
 			me_cl->props.protocol_version);
 	dev_dbg(dev->dev, "FW Client - Max Msg Len = %d\n",
@@ -441,7 +438,7 @@ static int mei_ioctl_connect_client(struct file *file,
 	client->protocol_version = me_cl->props.protocol_version;
 	dev_dbg(dev->dev, "Can connect?\n");
 
-	rets = mei_cl_connect(cl, file);
+	rets = mei_cl_connect(cl, me_cl, file);
 
 end:
 	mei_me_cl_put(me_cl);
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index f066ecd71939..453f6a333b42 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -88,7 +88,8 @@ enum file_state {
 	MEI_FILE_CONNECTING,
 	MEI_FILE_CONNECTED,
 	MEI_FILE_DISCONNECTING,
-	MEI_FILE_DISCONNECTED
+	MEI_FILE_DISCONNECT_REPLY,
+	MEI_FILE_DISCONNECTED,
 };
 
 /* MEI device states */
@@ -176,6 +177,8 @@ struct mei_fw_status {
  * @props: client properties
  * @client_id: me client id
  * @mei_flow_ctrl_creds: flow control credits
+ * @connect_count: number connections to this client
+ * @reserved: reserved
  */
 struct mei_me_client {
 	struct list_head list;
@@ -183,6 +186,8 @@ struct mei_me_client {
 	struct mei_client_properties props;
 	u8 client_id;
 	u8 mei_flow_ctrl_creds;
+	u8 connect_count;
+	u8 reserved;
 };
 
 
@@ -226,11 +231,11 @@ struct mei_cl_cb {
  * @rx_wait: wait queue for rx completion
  * @wait:  wait queue for management operation
  * @status: connection status
- * @cl_uuid: client uuid name
+ * @me_cl: fw client connected
  * @host_client_id: host id
- * @me_client_id: me/fw id
  * @mei_flow_ctrl_creds: transmit flow credentials
  * @timer_count:  watchdog timer for operation completion
+ * @reserved: reserved for alignment
  * @writing_state: state of the tx
  * @rd_pending: pending read credits
  * @rd_completed: completed read
@@ -246,11 +251,11 @@ struct mei_cl {
 	wait_queue_head_t rx_wait;
 	wait_queue_head_t wait;
 	int status;
-	uuid_le cl_uuid;
+	struct mei_me_client *me_cl;
 	u8 host_client_id;
-	u8 me_client_id;
 	u8 mei_flow_ctrl_creds;
 	u8 timer_count;
+	u8 reserved;
 	enum mei_file_transaction_states writing_state;
 	struct list_head rd_pending;
 	struct list_head rd_completed;
@@ -271,6 +276,7 @@ struct mei_cl {
 
  * @fw_status        : get fw status registers
  * @pg_state         : power gating state of the device
+ * @pg_in_transition : is device now in pg transition
  * @pg_is_enabled    : is power gating enabled
 
  * @intr_clear       : clear pending interrupts
@@ -300,6 +306,7 @@ struct mei_hw_ops {
 
 	int (*fw_status)(struct mei_device *dev, struct mei_fw_status *fw_sts);
 	enum mei_pg_state (*pg_state)(struct mei_device *dev);
+	bool (*pg_in_transition)(struct mei_device *dev);
 	bool (*pg_is_enabled)(struct mei_device *dev);
 
 	void (*intr_clear)(struct mei_device *dev);
@@ -323,34 +330,14 @@ struct mei_hw_ops {
 
 /* MEI bus API*/
 
-/**
- * struct mei_cl_ops - MEI CL device ops
- * This structure allows ME host clients to implement technology
- * specific operations.
- *
- * @enable: Enable an MEI CL device. Some devices require specific
- *	HECI commands to initialize completely.
- * @disable: Disable an MEI CL device.
- * @send: Tx hook for the device. This allows ME host clients to trap
- *	the device driver buffers before actually physically
- *	pushing it to the ME.
- * @recv: Rx hook for the device. This allows ME host clients to trap the
- *	ME buffers before forwarding them to the device driver.
- */
-struct mei_cl_ops {
-	int (*enable)(struct mei_cl_device *device);
-	int (*disable)(struct mei_cl_device *device);
-	int (*send)(struct mei_cl_device *device, u8 *buf, size_t length);
-	int (*recv)(struct mei_cl_device *device, u8 *buf, size_t length);
-};
-
 struct mei_cl_device *mei_cl_add_device(struct mei_device *dev,
-					uuid_le uuid, char *name,
-					struct mei_cl_ops *ops);
+					struct mei_me_client *me_cl,
+					struct mei_cl *cl,
+					char *name);
 void mei_cl_remove_device(struct mei_cl_device *device);
 
-ssize_t __mei_cl_async_send(struct mei_cl *cl, u8 *buf, size_t length);
-ssize_t __mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length);
+ssize_t __mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
+			bool blocking);
 ssize_t __mei_cl_recv(struct mei_cl *cl, u8 *buf, size_t length);
 void mei_cl_bus_rx_event(struct mei_cl *cl);
 void mei_cl_bus_remove_devices(struct mei_device *dev);
@@ -358,51 +345,21 @@ 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);
 
-
-/**
- * struct mei_cl_device - MEI device handle
- * An mei_cl_device pointer is returned from mei_add_device()
- * and links MEI bus clients to their actual ME host client pointer.
- * Drivers for MEI devices will get an mei_cl_device pointer
- * when being probed and shall use it for doing ME bus I/O.
- *
- * @dev: linux driver model device pointer
- * @cl: mei client
- * @ops: ME transport ops
- * @event_work: async work to execute event callback
- * @event_cb: Drivers register this callback to get asynchronous ME
- *	events (e.g. Rx buffer pending) notifications.
- * @event_context: event callback run context
- * @events: Events bitmask sent to the driver.
- * @priv_data: client private data
- */
-struct mei_cl_device {
-	struct device dev;
-
-	struct mei_cl *cl;
-
-	const struct mei_cl_ops *ops;
-
-	struct work_struct event_work;
-	mei_cl_event_cb_t event_cb;
-	void *event_context;
-	unsigned long events;
-
-	void *priv_data;
-};
-
-
 /**
  * enum mei_pg_event - power gating transition events
  *
  * @MEI_PG_EVENT_IDLE: the driver is not in power gating transition
  * @MEI_PG_EVENT_WAIT: the driver is waiting for a pg event to complete
  * @MEI_PG_EVENT_RECEIVED: the driver received pg event
+ * @MEI_PG_EVENT_INTR_WAIT: the driver is waiting for a pg event interrupt
+ * @MEI_PG_EVENT_INTR_RECEIVED: the driver received pg event interrupt
  */
 enum mei_pg_event {
 	MEI_PG_EVENT_IDLE,
 	MEI_PG_EVENT_WAIT,
 	MEI_PG_EVENT_RECEIVED,
+	MEI_PG_EVENT_INTR_WAIT,
+	MEI_PG_EVENT_INTR_RECEIVED,
 };
 
 /**
@@ -467,6 +424,8 @@ const char *mei_pg_state_str(enum mei_pg_state state);
  * @host_clients_map : host clients id pool
  * @me_client_index : last FW client index in enumeration
  *
+ * @allow_fixed_address: allow user space to connect a fixed client
+ *
  * @wd_cl       : watchdog client
  * @wd_state    : watchdog client state
  * @wd_pending  : watchdog command is pending
@@ -479,7 +438,6 @@ const char *mei_pg_state_str(enum mei_pg_state state);
  * @iamthif_cl  : amthif host client
  * @iamthif_current_cb : amthif current operation callback
  * @iamthif_open_count : number of opened amthif connections
- * @iamthif_mtu : amthif client max message length
  * @iamthif_timer : time stamp of current amthif command completion
  * @iamthif_stall_timer : timer to detect amthif hang
  * @iamthif_state : amthif processor state
@@ -558,6 +516,8 @@ struct mei_device {
 	DECLARE_BITMAP(host_clients_map, MEI_CLIENTS_MAX);
 	unsigned long me_client_index;
 
+	u32 allow_fixed_address;
+
 	struct mei_cl wd_cl;
 	enum mei_wd_states wd_state;
 	bool wd_pending;
@@ -573,7 +533,6 @@ struct mei_device {
 	struct mei_cl iamthif_cl;
 	struct mei_cl_cb *iamthif_current_cb;
 	long iamthif_open_count;
-	int iamthif_mtu;
 	unsigned long iamthif_timer;
 	u32 iamthif_stall_timer;
 	enum iamthif_states iamthif_state;
@@ -652,7 +611,7 @@ void mei_irq_compl_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list);
  */
 void mei_amthif_reset_params(struct mei_device *dev);
 
-int mei_amthif_host_init(struct mei_device *dev);
+int mei_amthif_host_init(struct mei_device *dev, struct mei_me_client *me_cl);
 
 int mei_amthif_read(struct mei_device *dev, struct file *file,
 		char __user *ubuf, size_t length, loff_t *offset);
@@ -679,7 +638,7 @@ int mei_amthif_irq_read(struct mei_device *dev, s32 *slots);
 /*
  * NFC functions
  */
-int mei_nfc_host_init(struct mei_device *dev);
+int mei_nfc_host_init(struct mei_device *dev, struct mei_me_client *me_cl);
 void mei_nfc_host_exit(struct mei_device *dev);
 
 /*
@@ -689,7 +648,7 @@ extern const uuid_le mei_nfc_guid;
 
 int mei_wd_send(struct mei_device *dev);
 int mei_wd_stop(struct mei_device *dev);
-int mei_wd_host_init(struct mei_device *dev);
+int mei_wd_host_init(struct mei_device *dev, struct mei_me_client *me_cl);
 /*
  * mei_watchdog_register  - Registering watchdog interface
  *   once we got connection to the WD Client
@@ -717,6 +676,11 @@ static inline enum mei_pg_state mei_pg_state(struct mei_device *dev)
 	return dev->ops->pg_state(dev);
 }
 
+static inline bool mei_pg_in_transition(struct mei_device *dev)
+{
+	return dev->ops->pg_in_transition(dev);
+}
+
 static inline bool mei_pg_is_enabled(struct mei_device *dev)
 {
 	return dev->ops->pg_is_enabled(dev);
diff --git a/drivers/misc/mei/nfc.c b/drivers/misc/mei/nfc.c
index c3bcb63686d7..290ef3037437 100644
--- a/drivers/misc/mei/nfc.c
+++ b/drivers/misc/mei/nfc.c
@@ -91,30 +91,25 @@ struct mei_nfc_hci_hdr {
 /**
  * struct mei_nfc_dev - NFC mei device
  *
+ * @me_cl: NFC me client
  * @cl: NFC host client
  * @cl_info: NFC info host client
  * @init_work: perform connection to the info client
- * @send_wq: send completion wait queue
  * @fw_ivn: NFC Interface Version Number
  * @vendor_id: NFC manufacturer ID
  * @radio_type: NFC radio type
  * @bus_name: bus name
  *
- * @req_id:  message counter
- * @recv_req_id: reception message counter
  */
 struct mei_nfc_dev {
+	struct mei_me_client *me_cl;
 	struct mei_cl *cl;
 	struct mei_cl *cl_info;
 	struct work_struct init_work;
-	wait_queue_head_t send_wq;
 	u8 fw_ivn;
 	u8 vendor_id;
 	u8 radio_type;
 	char *bus_name;
-
-	u16 req_id;
-	u16 recv_req_id;
 };
 
 /* UUIDs for NFC F/W clients */
@@ -151,6 +146,7 @@ static void mei_nfc_free(struct mei_nfc_dev *ndev)
 		kfree(ndev->cl_info);
 	}
 
+	mei_me_cl_put(ndev->me_cl);
 	kfree(ndev);
 }
 
@@ -199,73 +195,6 @@ static int mei_nfc_build_bus_name(struct mei_nfc_dev *ndev)
 	return 0;
 }
 
-static int mei_nfc_connect(struct mei_nfc_dev *ndev)
-{
-	struct mei_device *dev;
-	struct mei_cl *cl;
-	struct mei_nfc_cmd *cmd, *reply;
-	struct mei_nfc_connect *connect;
-	struct mei_nfc_connect_resp *connect_resp;
-	size_t connect_length, connect_resp_length;
-	int bytes_recv, ret;
-
-	cl = ndev->cl;
-	dev = cl->dev;
-
-	connect_length = sizeof(struct mei_nfc_cmd) +
-			sizeof(struct mei_nfc_connect);
-
-	connect_resp_length = sizeof(struct mei_nfc_cmd) +
-			sizeof(struct mei_nfc_connect_resp);
-
-	cmd = kzalloc(connect_length, GFP_KERNEL);
-	if (!cmd)
-		return -ENOMEM;
-	connect = (struct mei_nfc_connect *)cmd->data;
-
-	reply = kzalloc(connect_resp_length, GFP_KERNEL);
-	if (!reply) {
-		kfree(cmd);
-		return -ENOMEM;
-	}
-
-	connect_resp = (struct mei_nfc_connect_resp *)reply->data;
-
-	cmd->command = MEI_NFC_CMD_MAINTENANCE;
-	cmd->data_size = 3;
-	cmd->sub_command = MEI_NFC_SUBCMD_CONNECT;
-	connect->fw_ivn = ndev->fw_ivn;
-	connect->vendor_id = ndev->vendor_id;
-
-	ret = __mei_cl_send(cl, (u8 *)cmd, connect_length);
-	if (ret < 0) {
-		dev_err(dev->dev, "Could not send connect cmd\n");
-		goto err;
-	}
-
-	bytes_recv = __mei_cl_recv(cl, (u8 *)reply, connect_resp_length);
-	if (bytes_recv < 0) {
-		dev_err(dev->dev, "Could not read connect response\n");
-		ret = bytes_recv;
-		goto err;
-	}
-
-	dev_info(dev->dev, "IVN 0x%x Vendor ID 0x%x\n",
-		 connect_resp->fw_ivn, connect_resp->vendor_id);
-
-	dev_info(dev->dev, "ME FW %d.%d.%d.%d\n",
-		connect_resp->me_major, connect_resp->me_minor,
-		connect_resp->me_hotfix, connect_resp->me_build);
-
-	ret = 0;
-
-err:
-	kfree(reply);
-	kfree(cmd);
-
-	return ret;
-}
-
 static int mei_nfc_if_version(struct mei_nfc_dev *ndev)
 {
 	struct mei_device *dev;
@@ -285,7 +214,7 @@ static int mei_nfc_if_version(struct mei_nfc_dev *ndev)
 	cmd.data_size = 1;
 	cmd.sub_command = MEI_NFC_SUBCMD_IF_VERSION;
 
-	ret = __mei_cl_send(cl, (u8 *)&cmd, sizeof(struct mei_nfc_cmd));
+	ret = __mei_cl_send(cl, (u8 *)&cmd, sizeof(struct mei_nfc_cmd), 1);
 	if (ret < 0) {
 		dev_err(dev->dev, "Could not send IF version cmd\n");
 		return ret;
@@ -317,106 +246,13 @@ err:
 	return ret;
 }
 
-static int mei_nfc_enable(struct mei_cl_device *cldev)
-{
-	struct mei_device *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);
-	if (ret < 0) {
-		dev_err(dev->dev, "Could not connect to NFC");
-		return ret;
-	}
-
-	return 0;
-}
-
-static int mei_nfc_disable(struct mei_cl_device *cldev)
-{
-	return 0;
-}
-
-static int mei_nfc_send(struct mei_cl_device *cldev, u8 *buf, size_t length)
-{
-	struct mei_device *dev;
-	struct mei_nfc_dev *ndev;
-	struct mei_nfc_hci_hdr *hdr;
-	u8 *mei_buf;
-	int err;
-
-	ndev = (struct mei_nfc_dev *) cldev->priv_data;
-	dev = ndev->cl->dev;
-
-	err = -ENOMEM;
-	mei_buf = kzalloc(length + MEI_NFC_HEADER_SIZE, GFP_KERNEL);
-	if (!mei_buf)
-		goto out;
-
-	hdr = (struct mei_nfc_hci_hdr *) mei_buf;
-	hdr->cmd = MEI_NFC_CMD_HCI_SEND;
-	hdr->status = 0;
-	hdr->req_id = ndev->req_id;
-	hdr->reserved = 0;
-	hdr->data_size = length;
-
-	memcpy(mei_buf + MEI_NFC_HEADER_SIZE, buf, length);
-	err = __mei_cl_send(ndev->cl, mei_buf, length + MEI_NFC_HEADER_SIZE);
-	if (err < 0)
-		goto out;
-
-	if (!wait_event_interruptible_timeout(ndev->send_wq,
-				ndev->recv_req_id == ndev->req_id, HZ)) {
-		dev_err(dev->dev, "NFC MEI command timeout\n");
-		err = -ETIME;
-	} else {
-		ndev->req_id++;
-	}
-out:
-	kfree(mei_buf);
-	return err;
-}
-
-static int mei_nfc_recv(struct mei_cl_device *cldev, u8 *buf, size_t length)
-{
-	struct mei_nfc_dev *ndev;
-	struct mei_nfc_hci_hdr *hci_hdr;
-	int received_length;
-
-	ndev = (struct mei_nfc_dev *)cldev->priv_data;
-
-	received_length = __mei_cl_recv(ndev->cl, buf, length);
-	if (received_length < 0)
-		return received_length;
-
-	hci_hdr = (struct mei_nfc_hci_hdr *) buf;
-
-	if (hci_hdr->cmd == MEI_NFC_CMD_HCI_SEND) {
-		ndev->recv_req_id = hci_hdr->req_id;
-		wake_up(&ndev->send_wq);
-
-		return 0;
-	}
-
-	return received_length;
-}
-
-static struct mei_cl_ops nfc_ops = {
-	.enable = mei_nfc_enable,
-	.disable = mei_nfc_disable,
-	.send = mei_nfc_send,
-	.recv = mei_nfc_recv,
-};
-
 static void mei_nfc_init(struct work_struct *work)
 {
 	struct mei_device *dev;
 	struct mei_cl_device *cldev;
 	struct mei_nfc_dev *ndev;
 	struct mei_cl *cl_info;
+	struct mei_me_client *me_cl_info;
 
 	ndev = container_of(work, struct mei_nfc_dev, init_work);
 
@@ -425,13 +261,22 @@ static void mei_nfc_init(struct work_struct *work)
 
 	mutex_lock(&dev->device_lock);
 
-	if (mei_cl_connect(cl_info, NULL) < 0) {
+	/* check for valid client id */
+	me_cl_info = mei_me_cl_by_uuid(dev, &mei_nfc_info_guid);
+	if (!me_cl_info) {
+		mutex_unlock(&dev->device_lock);
+		dev_info(dev->dev, "nfc: failed to find the info client\n");
+		goto err;
+	}
+
+	if (mei_cl_connect(cl_info, me_cl_info, NULL) < 0) {
+		mei_me_cl_put(me_cl_info);
 		mutex_unlock(&dev->device_lock);
 		dev_err(dev->dev, "Could not connect to the NFC INFO ME client");
 
 		goto err;
 	}
-
+	mei_me_cl_put(me_cl_info);
 	mutex_unlock(&dev->device_lock);
 
 	if (mei_nfc_if_version(ndev) < 0) {
@@ -459,7 +304,8 @@ static void mei_nfc_init(struct work_struct *work)
 		return;
 	}
 
-	cldev = mei_cl_add_device(dev, mei_nfc_guid, ndev->bus_name, &nfc_ops);
+	cldev = mei_cl_add_device(dev, ndev->me_cl, ndev->cl,
+				  ndev->bus_name);
 	if (!cldev) {
 		dev_err(dev->dev, "Could not add the NFC device to the MEI bus\n");
 
@@ -479,11 +325,10 @@ err:
 }
 
 
-int mei_nfc_host_init(struct mei_device *dev)
+int mei_nfc_host_init(struct mei_device *dev, struct mei_me_client *me_cl)
 {
 	struct mei_nfc_dev *ndev;
 	struct mei_cl *cl_info, *cl;
-	struct mei_me_client *me_cl = NULL;
 	int ret;
 
 
@@ -500,11 +345,9 @@ int mei_nfc_host_init(struct mei_device *dev)
 		goto err;
 	}
 
-	/* check for valid client id */
-	me_cl = mei_me_cl_by_uuid(dev, &mei_nfc_info_guid);
-	if (!me_cl) {
-		dev_info(dev->dev, "nfc: failed to find the client\n");
-		ret = -ENOTTY;
+	ndev->me_cl = mei_me_cl_get(me_cl);
+	if (!ndev->me_cl) {
+		ret = -ENODEV;
 		goto err;
 	}
 
@@ -514,48 +357,26 @@ int mei_nfc_host_init(struct mei_device *dev)
 		goto err;
 	}
 
-	cl_info->me_client_id = me_cl->client_id;
-	cl_info->cl_uuid = me_cl->props.protocol_name;
-	mei_me_cl_put(me_cl);
-	me_cl = NULL;
-
 	list_add_tail(&cl_info->device_link, &dev->device_list);
 
 	ndev->cl_info = cl_info;
 
-	/* check for valid client id */
-	me_cl = mei_me_cl_by_uuid(dev, &mei_nfc_guid);
-	if (!me_cl) {
-		dev_info(dev->dev, "nfc: failed to find the client\n");
-		ret = -ENOTTY;
-		goto err;
-	}
-
 	cl = mei_cl_alloc_linked(dev, MEI_HOST_CLIENT_ID_ANY);
 	if (IS_ERR(cl)) {
 		ret = PTR_ERR(cl);
 		goto err;
 	}
 
-	cl->me_client_id = me_cl->client_id;
-	cl->cl_uuid = me_cl->props.protocol_name;
-	mei_me_cl_put(me_cl);
-	me_cl = NULL;
-
 	list_add_tail(&cl->device_link, &dev->device_list);
 
 	ndev->cl = cl;
 
-	ndev->req_id = 1;
-
 	INIT_WORK(&ndev->init_work, mei_nfc_init);
-	init_waitqueue_head(&ndev->send_wq);
 	schedule_work(&ndev->init_work);
 
 	return 0;
 
 err:
-	mei_me_cl_put(me_cl);
 	mei_nfc_free(ndev);
 
 	return ret;
@@ -581,11 +402,12 @@ void mei_nfc_host_exit(struct mei_device *dev)
 
 	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);
+
+	mutex_lock(&dev->device_lock);
 	mei_nfc_free(ndev);
 	mutex_unlock(&dev->device_lock);
 }
diff --git a/drivers/misc/mei/pci-txe.c b/drivers/misc/mei/pci-txe.c
index dcfcba44b6f7..0882c0201907 100644
--- a/drivers/misc/mei/pci-txe.c
+++ b/drivers/misc/mei/pci-txe.c
@@ -338,7 +338,7 @@ static int mei_txe_pm_runtime_suspend(struct device *device)
 	 * However if device is not wakeable we do not enter
 	 * D-low state and we need to keep the interrupt kicking
 	 */
-	 if (!ret && pci_dev_run_wake(pdev))
+	if (!ret && pci_dev_run_wake(pdev))
 		mei_disable_interrupts(dev);
 
 	dev_dbg(&pdev->dev, "rpm: txe: runtime suspend ret=%d\n", ret);
diff --git a/drivers/misc/mei/wd.c b/drivers/misc/mei/wd.c
index 2725f865c3d6..2bc0f5089f82 100644
--- a/drivers/misc/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -50,15 +50,15 @@ static void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
  * mei_wd_host_init - connect to the watchdog client
  *
  * @dev: the device structure
+ * @me_cl: me client
  *
  * Return: -ENOTTY if wd client cannot be found
  *         -EIO if write has failed
  *         0 on success
  */
-int mei_wd_host_init(struct mei_device *dev)
+int mei_wd_host_init(struct mei_device *dev, struct mei_me_client *me_cl)
 {
 	struct mei_cl *cl = &dev->wd_cl;
-	struct mei_me_client *me_cl;
 	int ret;
 
 	mei_cl_init(cl, dev);
@@ -66,27 +66,13 @@ int mei_wd_host_init(struct mei_device *dev)
 	dev->wd_timeout = MEI_WD_DEFAULT_TIMEOUT;
 	dev->wd_state = MEI_WD_IDLE;
 
-
-	/* check for valid client id */
-	me_cl = mei_me_cl_by_uuid(dev, &mei_wd_guid);
-	if (!me_cl) {
-		dev_info(dev->dev, "wd: failed to find the client\n");
-		return -ENOTTY;
-	}
-
-	cl->me_client_id = me_cl->client_id;
-	cl->cl_uuid = me_cl->props.protocol_name;
-	mei_me_cl_put(me_cl);
-
 	ret = mei_cl_link(cl, MEI_WD_HOST_CLIENT_ID);
-
 	if (ret < 0) {
 		dev_info(dev->dev, "wd: failed link client\n");
 		return ret;
 	}
 
-	ret = mei_cl_connect(cl, NULL);
-
+	ret = mei_cl_connect(cl, me_cl, NULL);
 	if (ret) {
 		dev_err(dev->dev, "wd: failed to connect = %d\n", ret);
 		mei_cl_unlink(cl);
@@ -118,7 +104,7 @@ int mei_wd_send(struct mei_device *dev)
 	int ret;
 
 	hdr.host_addr = cl->host_client_id;
-	hdr.me_addr = cl->me_client_id;
+	hdr.me_addr = mei_cl_me_id(cl);
 	hdr.msg_complete = 1;
 	hdr.reserved = 0;
 	hdr.internal = 0;
diff --git a/drivers/misc/mic/Kconfig b/drivers/misc/mic/Kconfig
index cc4eef040c14..e9f2f56c370d 100644
--- a/drivers/misc/mic/Kconfig
+++ b/drivers/misc/mic/Kconfig
@@ -15,11 +15,28 @@ config INTEL_MIC_BUS
 	  OS and tools for MIC to use with this driver are available from
 	  <http://software.intel.com/en-us/mic-developer>.
 
+comment "SCIF Bus Driver"
+
+config SCIF_BUS
+	tristate "SCIF Bus Driver"
+	depends on 64BIT && PCI && X86 && X86_DEV_DMA_OPS
+	help
+	  This option is selected by any driver which registers a
+	  device or driver on the SCIF Bus, such as CONFIG_INTEL_MIC_HOST
+	  and CONFIG_INTEL_MIC_CARD.
+
+	  If you are building a host/card kernel with an Intel MIC device
+	  then say M (recommended) or Y, else say N. If unsure say N.
+
+	  More information about the Intel MIC family as well as the Linux
+	  OS and tools for MIC to use with this driver are available from
+	  <http://software.intel.com/en-us/mic-developer>.
+
 comment "Intel MIC Host Driver"
 
 config INTEL_MIC_HOST
 	tristate "Intel MIC Host Driver"
-	depends on 64BIT && PCI && X86 && INTEL_MIC_BUS
+	depends on 64BIT && PCI && X86 && INTEL_MIC_BUS && SCIF_BUS
 	select VHOST_RING
 	help
 	  This enables Host Driver support for the Intel Many Integrated
@@ -39,7 +56,7 @@ comment "Intel MIC Card Driver"
 
 config INTEL_MIC_CARD
 	tristate "Intel MIC Card Driver"
-	depends on 64BIT && X86 && INTEL_MIC_BUS
+	depends on 64BIT && X86 && INTEL_MIC_BUS && SCIF_BUS
 	select VIRTIO
 	help
 	  This enables card driver support for the Intel Many Integrated
@@ -52,3 +69,22 @@ config INTEL_MIC_CARD
 
 	  For more information see
 	  <http://software.intel.com/en-us/mic-developer>.
+
+comment "SCIF Driver"
+
+config SCIF
+	tristate "SCIF Driver"
+	depends on 64BIT && PCI && X86 && SCIF_BUS
+	help
+	  This enables SCIF Driver support for the Intel Many Integrated
+	  Core (MIC) family of PCIe form factor coprocessor devices that
+	  run a 64 bit Linux OS. The Symmetric Communication Interface
+	  (SCIF (pronounced as skiff)) is a low level communications API
+	  across PCIe currently implemented for MIC.
+
+	  If you are building a host kernel with an Intel MIC device then
+	  say M (recommended) or Y, else say N. If unsure say N.
+
+	  More information about the Intel MIC family as well as the Linux
+	  OS and tools for MIC to use with this driver are available from
+	  <http://software.intel.com/en-us/mic-developer>.
diff --git a/drivers/misc/mic/Makefile b/drivers/misc/mic/Makefile
index e9bf148755e2..a74042c58649 100644
--- a/drivers/misc/mic/Makefile
+++ b/drivers/misc/mic/Makefile
@@ -4,4 +4,5 @@
 #
 obj-$(CONFIG_INTEL_MIC_HOST) += host/
 obj-$(CONFIG_INTEL_MIC_CARD) += card/
-obj-$(CONFIG_INTEL_MIC_BUS) += bus/
+obj-y += bus/
+obj-$(CONFIG_SCIF) += scif/
diff --git a/drivers/misc/mic/bus/Makefile b/drivers/misc/mic/bus/Makefile
index d85c7f2a0af4..1ed37e234c96 100644
--- a/drivers/misc/mic/bus/Makefile
+++ b/drivers/misc/mic/bus/Makefile
@@ -3,3 +3,4 @@
 # Copyright(c) 2014, Intel Corporation.
 #
 obj-$(CONFIG_INTEL_MIC_BUS) += mic_bus.o
+obj-$(CONFIG_SCIF_BUS) += scif_bus.o
diff --git a/drivers/misc/mic/bus/scif_bus.c b/drivers/misc/mic/bus/scif_bus.c
new file mode 100644
index 000000000000..2da7ceed015d
--- /dev/null
+++ b/drivers/misc/mic/bus/scif_bus.c
@@ -0,0 +1,210 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel Symmetric Communications Interface Bus driver.
+ */
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/idr.h>
+#include <linux/dma-mapping.h>
+
+#include "scif_bus.h"
+
+static ssize_t device_show(struct device *d,
+			   struct device_attribute *attr, char *buf)
+{
+	struct scif_hw_dev *dev = dev_to_scif(d);
+
+	return sprintf(buf, "0x%04x\n", dev->id.device);
+}
+
+static DEVICE_ATTR_RO(device);
+
+static ssize_t vendor_show(struct device *d,
+			   struct device_attribute *attr, char *buf)
+{
+	struct scif_hw_dev *dev = dev_to_scif(d);
+
+	return sprintf(buf, "0x%04x\n", dev->id.vendor);
+}
+
+static DEVICE_ATTR_RO(vendor);
+
+static ssize_t modalias_show(struct device *d,
+			     struct device_attribute *attr, char *buf)
+{
+	struct scif_hw_dev *dev = dev_to_scif(d);
+
+	return sprintf(buf, "scif:d%08Xv%08X\n",
+		       dev->id.device, dev->id.vendor);
+}
+
+static DEVICE_ATTR_RO(modalias);
+
+static struct attribute *scif_dev_attrs[] = {
+	&dev_attr_device.attr,
+	&dev_attr_vendor.attr,
+	&dev_attr_modalias.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(scif_dev);
+
+static inline int scif_id_match(const struct scif_hw_dev *dev,
+				const struct scif_hw_dev_id *id)
+{
+	if (id->device != dev->id.device && id->device != SCIF_DEV_ANY_ID)
+		return 0;
+
+	return id->vendor == SCIF_DEV_ANY_ID || id->vendor == dev->id.vendor;
+}
+
+/*
+ * This looks through all the IDs a driver claims to support.  If any of them
+ * match, we return 1 and the kernel will call scif_dev_probe().
+ */
+static int scif_dev_match(struct device *dv, struct device_driver *dr)
+{
+	unsigned int i;
+	struct scif_hw_dev *dev = dev_to_scif(dv);
+	const struct scif_hw_dev_id *ids;
+
+	ids = drv_to_scif(dr)->id_table;
+	for (i = 0; ids[i].device; i++)
+		if (scif_id_match(dev, &ids[i]))
+			return 1;
+	return 0;
+}
+
+static int scif_uevent(struct device *dv, struct kobj_uevent_env *env)
+{
+	struct scif_hw_dev *dev = dev_to_scif(dv);
+
+	return add_uevent_var(env, "MODALIAS=scif:d%08Xv%08X",
+			      dev->id.device, dev->id.vendor);
+}
+
+static int scif_dev_probe(struct device *d)
+{
+	struct scif_hw_dev *dev = dev_to_scif(d);
+	struct scif_driver *drv = drv_to_scif(dev->dev.driver);
+
+	return drv->probe(dev);
+}
+
+static int scif_dev_remove(struct device *d)
+{
+	struct scif_hw_dev *dev = dev_to_scif(d);
+	struct scif_driver *drv = drv_to_scif(dev->dev.driver);
+
+	drv->remove(dev);
+	return 0;
+}
+
+static struct bus_type scif_bus = {
+	.name  = "scif_bus",
+	.match = scif_dev_match,
+	.dev_groups = scif_dev_groups,
+	.uevent = scif_uevent,
+	.probe = scif_dev_probe,
+	.remove = scif_dev_remove,
+};
+
+int scif_register_driver(struct scif_driver *driver)
+{
+	driver->driver.bus = &scif_bus;
+	return driver_register(&driver->driver);
+}
+EXPORT_SYMBOL_GPL(scif_register_driver);
+
+void scif_unregister_driver(struct scif_driver *driver)
+{
+	driver_unregister(&driver->driver);
+}
+EXPORT_SYMBOL_GPL(scif_unregister_driver);
+
+static void scif_release_dev(struct device *d)
+{
+	struct scif_hw_dev *sdev = dev_to_scif(d);
+
+	kfree(sdev);
+}
+
+struct scif_hw_dev *
+scif_register_device(struct device *pdev, int id, struct dma_map_ops *dma_ops,
+		     struct scif_hw_ops *hw_ops, u8 dnode, u8 snode,
+		     struct mic_mw *mmio, struct mic_mw *aper, void *dp,
+		     void __iomem *rdp, struct dma_chan **chan, int num_chan)
+{
+	int ret;
+	struct scif_hw_dev *sdev;
+
+	sdev = kzalloc(sizeof(*sdev), GFP_KERNEL);
+	if (!sdev)
+		return ERR_PTR(-ENOMEM);
+
+	sdev->dev.parent = pdev;
+	sdev->id.device = id;
+	sdev->id.vendor = SCIF_DEV_ANY_ID;
+	sdev->dev.archdata.dma_ops = dma_ops;
+	sdev->dev.release = scif_release_dev;
+	sdev->hw_ops = hw_ops;
+	sdev->dnode = dnode;
+	sdev->snode = snode;
+	dev_set_drvdata(&sdev->dev, sdev);
+	sdev->dev.bus = &scif_bus;
+	sdev->mmio = mmio;
+	sdev->aper = aper;
+	sdev->dp = dp;
+	sdev->rdp = rdp;
+	sdev->dev.dma_mask = &sdev->dev.coherent_dma_mask;
+	dma_set_mask(&sdev->dev, DMA_BIT_MASK(64));
+	sdev->dma_ch = chan;
+	sdev->num_dma_ch = num_chan;
+	dev_set_name(&sdev->dev, "scif-dev%u", sdev->dnode);
+	/*
+	 * device_register() causes the bus infrastructure to look for a
+	 * matching driver.
+	 */
+	ret = device_register(&sdev->dev);
+	if (ret)
+		goto free_sdev;
+	return sdev;
+free_sdev:
+	kfree(sdev);
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(scif_register_device);
+
+void scif_unregister_device(struct scif_hw_dev *sdev)
+{
+	device_unregister(&sdev->dev);
+}
+EXPORT_SYMBOL_GPL(scif_unregister_device);
+
+static int __init scif_init(void)
+{
+	return bus_register(&scif_bus);
+}
+
+static void __exit scif_exit(void)
+{
+	bus_unregister(&scif_bus);
+}
+
+core_initcall(scif_init);
+module_exit(scif_exit);
+
+MODULE_AUTHOR("Intel Corporation");
+MODULE_DESCRIPTION("Intel(R) SCIF Bus driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/misc/mic/bus/scif_bus.h b/drivers/misc/mic/bus/scif_bus.h
new file mode 100644
index 000000000000..335a228a8236
--- /dev/null
+++ b/drivers/misc/mic/bus/scif_bus.h
@@ -0,0 +1,129 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel Symmetric Communications Interface Bus driver.
+ */
+#ifndef _SCIF_BUS_H_
+#define _SCIF_BUS_H_
+/*
+ * Everything a scif driver needs to work with any particular scif
+ * hardware abstraction layer.
+ */
+#include <linux/dma-mapping.h>
+
+#include <linux/mic_common.h>
+#include "../common/mic_dev.h"
+
+struct scif_hw_dev_id {
+	u32 device;
+	u32 vendor;
+};
+
+#define MIC_SCIF_DEV 1
+#define SCIF_DEV_ANY_ID 0xffffffff
+
+/**
+ * scif_hw_dev - representation of a hardware device abstracted for scif
+ * @hw_ops: the hardware ops supported by this device
+ * @id: the device type identification (used to match it with a driver)
+ * @mmio: MMIO memory window
+ * @aper: Aperture memory window
+ * @dev: underlying device
+ * @dnode - The destination node which this device will communicate with.
+ * @snode - The source node for this device.
+ * @dp - Self device page
+ * @rdp - Remote device page
+ * @dma_ch - Array of DMA channels
+ * @num_dma_ch - Number of DMA channels available
+ */
+struct scif_hw_dev {
+	struct scif_hw_ops *hw_ops;
+	struct scif_hw_dev_id id;
+	struct mic_mw *mmio;
+	struct mic_mw *aper;
+	struct device dev;
+	u8 dnode;
+	u8 snode;
+	void *dp;
+	void __iomem *rdp;
+	struct dma_chan **dma_ch;
+	int num_dma_ch;
+};
+
+/**
+ * scif_driver - operations for a scif I/O driver
+ * @driver: underlying device driver (populate name and owner).
+ * @id_table: the ids serviced by this driver.
+ * @probe: the function to call when a device is found.  Returns 0 or -errno.
+ * @remove: the function to call when a device is removed.
+ */
+struct scif_driver {
+	struct device_driver driver;
+	const struct scif_hw_dev_id *id_table;
+	int (*probe)(struct scif_hw_dev *dev);
+	void (*remove)(struct scif_hw_dev *dev);
+};
+
+/**
+ * scif_hw_ops - Hardware operations for accessing a SCIF device on the SCIF bus.
+ *
+ * @next_db: Obtain the next available doorbell.
+ * @request_irq: Request an interrupt on a particular doorbell.
+ * @free_irq: Free an interrupt requested previously.
+ * @ack_interrupt: acknowledge an interrupt in the ISR.
+ * @send_intr: Send an interrupt to the remote node on a specified doorbell.
+ * @send_p2p_intr: Send an interrupt to the peer node on a specified doorbell
+ * which is specifically targeted for a peer to peer node.
+ * @ioremap: Map a buffer with the specified physical address and length.
+ * @iounmap: Unmap a buffer previously mapped.
+ */
+struct scif_hw_ops {
+	int (*next_db)(struct scif_hw_dev *sdev);
+	struct mic_irq * (*request_irq)(struct scif_hw_dev *sdev,
+					irqreturn_t (*func)(int irq,
+							    void *data),
+					const char *name, void *data,
+					int db);
+	void (*free_irq)(struct scif_hw_dev *sdev,
+			 struct mic_irq *cookie, void *data);
+	void (*ack_interrupt)(struct scif_hw_dev *sdev, int num);
+	void (*send_intr)(struct scif_hw_dev *sdev, int db);
+	void (*send_p2p_intr)(struct scif_hw_dev *sdev, int db,
+			      struct mic_mw *mw);
+	void __iomem * (*ioremap)(struct scif_hw_dev *sdev,
+				  phys_addr_t pa, size_t len);
+	void (*iounmap)(struct scif_hw_dev *sdev, void __iomem *va);
+};
+
+int scif_register_driver(struct scif_driver *driver);
+void scif_unregister_driver(struct scif_driver *driver);
+struct scif_hw_dev *
+scif_register_device(struct device *pdev, int id,
+		     struct dma_map_ops *dma_ops,
+		     struct scif_hw_ops *hw_ops, u8 dnode, u8 snode,
+		     struct mic_mw *mmio, struct mic_mw *aper,
+		     void *dp, void __iomem *rdp,
+		     struct dma_chan **chan, int num_chan);
+void scif_unregister_device(struct scif_hw_dev *sdev);
+
+static inline struct scif_hw_dev *dev_to_scif(struct device *dev)
+{
+	return container_of(dev, struct scif_hw_dev, dev);
+}
+
+static inline struct scif_driver *drv_to_scif(struct device_driver *drv)
+{
+	return container_of(drv, struct scif_driver, driver);
+}
+#endif /* _SCIF_BUS_H */
diff --git a/drivers/misc/mic/card/mic_device.c b/drivers/misc/mic/card/mic_device.c
index 83819eee553b..6338908b2252 100644
--- a/drivers/misc/mic/card/mic_device.c
+++ b/drivers/misc/mic/card/mic_device.c
@@ -28,6 +28,8 @@
 #include <linux/pci.h>
 #include <linux/interrupt.h>
 #include <linux/reboot.h>
+#include <linux/dmaengine.h>
+#include <linux/kmod.h>
 
 #include <linux/mic_common.h>
 #include "../common/mic_dev.h"
@@ -240,6 +242,111 @@ static void mic_uninit_irq(void)
 	kfree(mdrv->irq_info.irq_usage_count);
 }
 
+static inline struct mic_driver *scdev_to_mdrv(struct scif_hw_dev *scdev)
+{
+	return dev_get_drvdata(scdev->dev.parent);
+}
+
+static struct mic_irq *
+___mic_request_irq(struct scif_hw_dev *scdev,
+		   irqreturn_t (*func)(int irq, void *data),
+				       const char *name, void *data,
+				       int db)
+{
+	return mic_request_card_irq(func, NULL, name, data, db);
+}
+
+static void
+___mic_free_irq(struct scif_hw_dev *scdev,
+		struct mic_irq *cookie, void *data)
+{
+	return mic_free_card_irq(cookie, data);
+}
+
+static void ___mic_ack_interrupt(struct scif_hw_dev *scdev, int num)
+{
+	struct mic_driver *mdrv = scdev_to_mdrv(scdev);
+
+	mic_ack_interrupt(&mdrv->mdev);
+}
+
+static int ___mic_next_db(struct scif_hw_dev *scdev)
+{
+	return mic_next_card_db();
+}
+
+static void ___mic_send_intr(struct scif_hw_dev *scdev, int db)
+{
+	struct mic_driver *mdrv = scdev_to_mdrv(scdev);
+
+	mic_send_intr(&mdrv->mdev, db);
+}
+
+static void ___mic_send_p2p_intr(struct scif_hw_dev *scdev, int db,
+				 struct mic_mw *mw)
+{
+	mic_send_p2p_intr(db, mw);
+}
+
+static void __iomem *
+___mic_ioremap(struct scif_hw_dev *scdev,
+	       phys_addr_t pa, size_t len)
+{
+	struct mic_driver *mdrv = scdev_to_mdrv(scdev);
+
+	return mic_card_map(&mdrv->mdev, pa, len);
+}
+
+static void ___mic_iounmap(struct scif_hw_dev *scdev, void __iomem *va)
+{
+	struct mic_driver *mdrv = scdev_to_mdrv(scdev);
+
+	mic_card_unmap(&mdrv->mdev, va);
+}
+
+static struct scif_hw_ops scif_hw_ops = {
+	.request_irq = ___mic_request_irq,
+	.free_irq = ___mic_free_irq,
+	.ack_interrupt = ___mic_ack_interrupt,
+	.next_db = ___mic_next_db,
+	.send_intr = ___mic_send_intr,
+	.send_p2p_intr = ___mic_send_p2p_intr,
+	.ioremap = ___mic_ioremap,
+	.iounmap = ___mic_iounmap,
+};
+
+static int mic_request_dma_chans(struct mic_driver *mdrv)
+{
+	dma_cap_mask_t mask;
+	struct dma_chan *chan;
+
+	request_module("mic_x100_dma");
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_MEMCPY, mask);
+
+	do {
+		chan = dma_request_channel(mask, NULL, NULL);
+		if (chan) {
+			mdrv->dma_ch[mdrv->num_dma_ch++] = chan;
+			if (mdrv->num_dma_ch >= MIC_MAX_DMA_CHAN)
+				break;
+		}
+	} while (chan);
+	dev_info(mdrv->dev, "DMA channels # %d\n", mdrv->num_dma_ch);
+	return mdrv->num_dma_ch;
+}
+
+static void mic_free_dma_chans(struct mic_driver *mdrv)
+{
+	int i = 0;
+
+	for (i = 0; i < mdrv->num_dma_ch; i++) {
+		dma_release_channel(mdrv->dma_ch[i]);
+		mdrv->dma_ch[i] = NULL;
+	}
+	mdrv->num_dma_ch = 0;
+}
+
 /*
  * mic_driver_init - MIC driver initialization tasks.
  *
@@ -248,6 +355,8 @@ static void mic_uninit_irq(void)
 int __init mic_driver_init(struct mic_driver *mdrv)
 {
 	int rc;
+	struct mic_bootparam __iomem *bootparam;
+	u8 node_id;
 
 	g_drv = mdrv;
 	/*
@@ -268,13 +377,32 @@ int __init mic_driver_init(struct mic_driver *mdrv)
 	rc = mic_shutdown_init();
 	if (rc)
 		goto irq_uninit;
+	if (!mic_request_dma_chans(mdrv)) {
+		rc = -ENODEV;
+		goto shutdown_uninit;
+	}
 	rc = mic_devices_init(mdrv);
 	if (rc)
-		goto shutdown_uninit;
+		goto dma_free;
+	bootparam = mdrv->dp;
+	node_id = ioread8(&bootparam->node_id);
+	mdrv->scdev = scif_register_device(mdrv->dev, MIC_SCIF_DEV,
+					   NULL, &scif_hw_ops,
+					   0, node_id, &mdrv->mdev.mmio, NULL,
+					   NULL, mdrv->dp, mdrv->dma_ch,
+					   mdrv->num_dma_ch);
+	if (IS_ERR(mdrv->scdev)) {
+		rc = PTR_ERR(mdrv->scdev);
+		goto device_uninit;
+	}
 	mic_create_card_debug_dir(mdrv);
 	atomic_notifier_chain_register(&panic_notifier_list, &mic_panic);
 done:
 	return rc;
+device_uninit:
+	mic_devices_uninit(mdrv);
+dma_free:
+	mic_free_dma_chans(mdrv);
 shutdown_uninit:
 	mic_shutdown_uninit();
 irq_uninit:
@@ -294,7 +422,9 @@ put:
 void mic_driver_uninit(struct mic_driver *mdrv)
 {
 	mic_delete_card_debug_dir(mdrv);
+	scif_unregister_device(mdrv->scdev);
 	mic_devices_uninit(mdrv);
+	mic_free_dma_chans(mdrv);
 	/*
 	 * Inform the host about the shutdown status i.e. poweroff/restart etc.
 	 * The module cannot be unloaded so the only code path to call
diff --git a/drivers/misc/mic/card/mic_device.h b/drivers/misc/mic/card/mic_device.h
index 844be8fc9b22..1dbf83c41289 100644
--- a/drivers/misc/mic/card/mic_device.h
+++ b/drivers/misc/mic/card/mic_device.h
@@ -29,9 +29,9 @@
 
 #include <linux/workqueue.h>
 #include <linux/io.h>
-#include <linux/irqreturn.h>
 #include <linux/interrupt.h>
 #include <linux/mic_bus.h>
+#include "../bus/scif_bus.h"
 
 /**
  * struct mic_intr_info - Contains h/w specific interrupt sources info
@@ -73,6 +73,9 @@ struct mic_device {
  * @irq_info: The OS specific irq information
  * @intr_info: H/W specific interrupt information.
  * @dma_mbdev: dma device on the MIC virtual bus.
+ * @dma_ch - Array of DMA channels
+ * @num_dma_ch - Number of DMA channels available
+ * @scdev: SCIF device on the SCIF virtual bus.
  */
 struct mic_driver {
 	char name[20];
@@ -84,6 +87,9 @@ struct mic_driver {
 	struct mic_irq_info irq_info;
 	struct mic_intr_info intr_info;
 	struct mbus_device *dma_mbdev;
+	struct dma_chan *dma_ch[MIC_MAX_DMA_CHAN];
+	int num_dma_ch;
+	struct scif_hw_dev *scdev;
 };
 
 /**
@@ -122,10 +128,11 @@ void mic_driver_uninit(struct mic_driver *mdrv);
 int mic_next_card_db(void);
 struct mic_irq *
 mic_request_card_irq(irq_handler_t handler, irq_handler_t thread_fn,
-		     const char *name, void *data, int intr_src);
+		     const char *name, void *data, int db);
 void mic_free_card_irq(struct mic_irq *cookie, void *data);
 u32 mic_read_spad(struct mic_device *mdev, unsigned int idx);
 void mic_send_intr(struct mic_device *mdev, int doorbell);
+void mic_send_p2p_intr(int doorbell, struct mic_mw *mw);
 int mic_db_to_irq(struct mic_driver *mdrv, int db);
 u32 mic_ack_interrupt(struct mic_device *mdev);
 void mic_hw_intr_init(struct mic_driver *mdrv);
diff --git a/drivers/misc/mic/card/mic_x100.c b/drivers/misc/mic/card/mic_x100.c
index e98e537d68e3..77fd41781c2e 100644
--- a/drivers/misc/mic/card/mic_x100.c
+++ b/drivers/misc/mic/card/mic_x100.c
@@ -70,6 +70,41 @@ void mic_send_intr(struct mic_device *mdev, int doorbell)
 		       (MIC_X100_SBOX_SDBIC0 + (4 * doorbell)));
 }
 
+/*
+ * mic_x100_send_sbox_intr - Send an MIC_X100_SBOX interrupt to MIC.
+ */
+static void mic_x100_send_sbox_intr(struct mic_mw *mw, int doorbell)
+{
+	u64 apic_icr_offset = MIC_X100_SBOX_APICICR0 + doorbell * 8;
+	u32 apicicr_low = mic_mmio_read(mw, MIC_X100_SBOX_BASE_ADDRESS +
+					apic_icr_offset);
+
+	/* for MIC we need to make sure we "hit" the send_icr bit (13) */
+	apicicr_low = (apicicr_low | (1 << 13));
+	/*
+	 * Ensure that the interrupt is ordered w.r.t. previous stores
+	 * to main memory. Fence instructions are not implemented in X100
+	 * since execution is in order but a compiler barrier is still
+	 * required.
+	 */
+	wmb();
+	mic_mmio_write(mw, apicicr_low,
+		       MIC_X100_SBOX_BASE_ADDRESS + apic_icr_offset);
+}
+
+static void mic_x100_send_rdmasr_intr(struct mic_mw *mw, int doorbell)
+{
+	int rdmasr_offset = MIC_X100_SBOX_RDMASR0 + (doorbell << 2);
+	/*
+	 * Ensure that the interrupt is ordered w.r.t. previous stores
+	 * to main memory. Fence instructions are not implemented in X100
+	 * since execution is in order but a compiler barrier is still
+	 * required.
+	 */
+	wmb();
+	mic_mmio_write(mw, 0, MIC_X100_SBOX_BASE_ADDRESS + rdmasr_offset);
+}
+
 /**
  * mic_ack_interrupt - Device specific interrupt handling.
  * @mdev: pointer to mic_device instance
@@ -91,6 +126,18 @@ static inline int mic_get_rdmasr_irq(int index)
 	return  MIC_X100_RDMASR_IRQ_BASE + index;
 }
 
+void mic_send_p2p_intr(int db, struct mic_mw *mw)
+{
+	int rdmasr_index;
+
+	if (db < MIC_X100_NUM_SBOX_IRQ) {
+		mic_x100_send_sbox_intr(mw, db);
+	} else {
+		rdmasr_index = db - MIC_X100_NUM_SBOX_IRQ;
+		mic_x100_send_rdmasr_intr(mw, rdmasr_index);
+	}
+}
+
 /**
  * mic_hw_intr_init - Initialize h/w specific interrupt
  * information.
@@ -113,11 +160,15 @@ void mic_hw_intr_init(struct mic_driver *mdrv)
 int mic_db_to_irq(struct mic_driver *mdrv, int db)
 {
 	int rdmasr_index;
+
+	/*
+	 * The total number of doorbell interrupts on the card are 16. Indices
+	 * 0-8 falls in the SBOX category and 8-15 fall in the RDMASR category.
+	 */
 	if (db < MIC_X100_NUM_SBOX_IRQ) {
 		return mic_get_sbox_irq(db);
 	} else {
-		rdmasr_index = db - MIC_X100_NUM_SBOX_IRQ +
-			MIC_X100_RDMASR_IRQ_BASE;
+		rdmasr_index = db - MIC_X100_NUM_SBOX_IRQ;
 		return mic_get_rdmasr_irq(rdmasr_index);
 	}
 }
@@ -243,10 +294,16 @@ static void mic_platform_shutdown(struct platform_device *pdev)
 	mic_remove(pdev);
 }
 
+static u64 mic_dma_mask = DMA_BIT_MASK(64);
+
 static struct platform_device mic_platform_dev = {
 	.name = mic_driver_name,
 	.id   = 0,
 	.num_resources = 0,
+	.dev = {
+		.dma_mask = &mic_dma_mask,
+		.coherent_dma_mask = DMA_BIT_MASK(64),
+	},
 };
 
 static struct platform_driver __refdata mic_platform_driver = {
diff --git a/drivers/misc/mic/card/mic_x100.h b/drivers/misc/mic/card/mic_x100.h
index d66ea55639c3..7e2224934ba8 100644
--- a/drivers/misc/mic/card/mic_x100.h
+++ b/drivers/misc/mic/card/mic_x100.h
@@ -35,6 +35,7 @@
 #define MIC_X100_SBOX_SDBIC0 0x0000CC90
 #define MIC_X100_SBOX_SDBIC0_DBREQ_BIT 0x80000000
 #define MIC_X100_SBOX_RDMASR0	0x0000B180
+#define MIC_X100_SBOX_APICICR0 0x0000A9D0
 
 #define MIC_X100_MAX_DOORBELL_IDX 8
 
diff --git a/drivers/misc/mic/common/mic_dev.h b/drivers/misc/mic/common/mic_dev.h
index 92999c2bbf82..0b58c46045dc 100644
--- a/drivers/misc/mic/common/mic_dev.h
+++ b/drivers/misc/mic/common/mic_dev.h
@@ -48,4 +48,7 @@ struct mic_mw {
 #define MIC_VIRTIO_PARAM_DEV_REMOVE 0x1
 #define MIC_VIRTIO_PARAM_CONFIG_CHANGED 0x2
 
+/* Maximum number of DMA channels */
+#define MIC_MAX_DMA_CHAN 4
+
 #endif
diff --git a/drivers/misc/mic/host/mic_boot.c b/drivers/misc/mic/host/mic_boot.c
index d9fa609da061..e5f6a5e7bca1 100644
--- a/drivers/misc/mic/host/mic_boot.c
+++ b/drivers/misc/mic/host/mic_boot.c
@@ -21,6 +21,7 @@
 #include <linux/delay.h>
 #include <linux/firmware.h>
 #include <linux/pci.h>
+#include <linux/kmod.h>
 
 #include <linux/mic_common.h>
 #include <linux/mic_bus.h>
@@ -29,6 +30,188 @@
 #include "mic_smpt.h"
 #include "mic_virtio.h"
 
+static inline struct mic_device *scdev_to_mdev(struct scif_hw_dev *scdev)
+{
+	return dev_get_drvdata(scdev->dev.parent);
+}
+
+static void *__mic_dma_alloc(struct device *dev, size_t size,
+			     dma_addr_t *dma_handle, gfp_t gfp,
+			     struct dma_attrs *attrs)
+{
+	struct scif_hw_dev *scdev = dev_get_drvdata(dev);
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+	dma_addr_t tmp;
+	void *va = kmalloc(size, gfp);
+
+	if (va) {
+		tmp = mic_map_single(mdev, va, size);
+		if (dma_mapping_error(dev, tmp)) {
+			kfree(va);
+			va = NULL;
+		} else {
+			*dma_handle = tmp;
+		}
+	}
+	return va;
+}
+
+static void __mic_dma_free(struct device *dev, size_t size, void *vaddr,
+			   dma_addr_t dma_handle, struct dma_attrs *attrs)
+{
+	struct scif_hw_dev *scdev = dev_get_drvdata(dev);
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	mic_unmap_single(mdev, dma_handle, size);
+	kfree(vaddr);
+}
+
+static dma_addr_t
+__mic_dma_map_page(struct device *dev, struct page *page, unsigned long offset,
+		   size_t size, enum dma_data_direction dir,
+		   struct dma_attrs *attrs)
+{
+	void *va = phys_to_virt(page_to_phys(page)) + offset;
+	struct scif_hw_dev *scdev = dev_get_drvdata(dev);
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	return mic_map_single(mdev, va, size);
+}
+
+static void
+__mic_dma_unmap_page(struct device *dev, dma_addr_t dma_addr,
+		     size_t size, enum dma_data_direction dir,
+		     struct dma_attrs *attrs)
+{
+	struct scif_hw_dev *scdev = dev_get_drvdata(dev);
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	mic_unmap_single(mdev, dma_addr, size);
+}
+
+static int __mic_dma_map_sg(struct device *dev, struct scatterlist *sg,
+			    int nents, enum dma_data_direction dir,
+			    struct dma_attrs *attrs)
+{
+	struct scif_hw_dev *scdev = dev_get_drvdata(dev);
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+	struct scatterlist *s;
+	int i, j, ret;
+	dma_addr_t da;
+
+	ret = dma_map_sg(mdev->sdev->parent, sg, nents, dir);
+	if (ret <= 0)
+		return 0;
+
+	for_each_sg(sg, s, nents, i) {
+		da = mic_map(mdev, sg_dma_address(s) + s->offset, s->length);
+		if (!da)
+			goto err;
+		sg_dma_address(s) = da;
+	}
+	return nents;
+err:
+	for_each_sg(sg, s, i, j) {
+		mic_unmap(mdev, sg_dma_address(s), s->length);
+		sg_dma_address(s) = mic_to_dma_addr(mdev, sg_dma_address(s));
+	}
+	dma_unmap_sg(mdev->sdev->parent, sg, nents, dir);
+	return 0;
+}
+
+static void __mic_dma_unmap_sg(struct device *dev,
+			       struct scatterlist *sg, int nents,
+			       enum dma_data_direction dir,
+			       struct dma_attrs *attrs)
+{
+	struct scif_hw_dev *scdev = dev_get_drvdata(dev);
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+	struct scatterlist *s;
+	dma_addr_t da;
+	int i;
+
+	for_each_sg(sg, s, nents, i) {
+		da = mic_to_dma_addr(mdev, sg_dma_address(s));
+		mic_unmap(mdev, sg_dma_address(s), s->length);
+		sg_dma_address(s) = da;
+	}
+	dma_unmap_sg(mdev->sdev->parent, sg, nents, dir);
+}
+
+static struct dma_map_ops __mic_dma_ops = {
+	.alloc = __mic_dma_alloc,
+	.free = __mic_dma_free,
+	.map_page = __mic_dma_map_page,
+	.unmap_page = __mic_dma_unmap_page,
+	.map_sg = __mic_dma_map_sg,
+	.unmap_sg = __mic_dma_unmap_sg,
+};
+
+static struct mic_irq *
+___mic_request_irq(struct scif_hw_dev *scdev,
+		   irqreturn_t (*func)(int irq, void *data),
+				       const char *name,
+				       void *data, int db)
+{
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	return mic_request_threaded_irq(mdev, func, NULL, name, data,
+					db, MIC_INTR_DB);
+}
+
+static void
+___mic_free_irq(struct scif_hw_dev *scdev,
+		struct mic_irq *cookie, void *data)
+{
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	return mic_free_irq(mdev, cookie, data);
+}
+
+static void ___mic_ack_interrupt(struct scif_hw_dev *scdev, int num)
+{
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	mdev->ops->intr_workarounds(mdev);
+}
+
+static int ___mic_next_db(struct scif_hw_dev *scdev)
+{
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	return mic_next_db(mdev);
+}
+
+static void ___mic_send_intr(struct scif_hw_dev *scdev, int db)
+{
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	mdev->ops->send_intr(mdev, db);
+}
+
+static void __iomem *___mic_ioremap(struct scif_hw_dev *scdev,
+				    phys_addr_t pa, size_t len)
+{
+	struct mic_device *mdev = scdev_to_mdev(scdev);
+
+	return mdev->aper.va + pa;
+}
+
+static void ___mic_iounmap(struct scif_hw_dev *scdev, void __iomem *va)
+{
+	/* nothing to do */
+}
+
+static struct scif_hw_ops scif_hw_ops = {
+	.request_irq = ___mic_request_irq,
+	.free_irq = ___mic_free_irq,
+	.ack_interrupt = ___mic_ack_interrupt,
+	.next_db = ___mic_next_db,
+	.send_intr = ___mic_send_intr,
+	.ioremap = ___mic_ioremap,
+	.iounmap = ___mic_iounmap,
+};
+
 static inline struct mic_device *mbdev_to_mdev(struct mbus_device *mbdev)
 {
 	return dev_get_drvdata(mbdev->dev.parent);
@@ -127,6 +310,58 @@ void mic_bootparam_init(struct mic_device *mdev)
 	bootparam->h2c_config_db = -1;
 	bootparam->shutdown_status = 0;
 	bootparam->shutdown_card = 0;
+	/* Total nodes = number of MICs + 1 for self node */
+	bootparam->tot_nodes = atomic_read(&g_num_mics) + 1;
+	bootparam->node_id = mdev->id + 1;
+	bootparam->scif_host_dma_addr = 0x0;
+	bootparam->scif_card_dma_addr = 0x0;
+	bootparam->c2h_scif_db = -1;
+	bootparam->h2c_scif_db = -1;
+}
+
+/**
+ * mic_request_dma_chans - Request DMA channels
+ * @mdev: pointer to mic_device instance
+ *
+ * returns number of DMA channels acquired
+ */
+static int mic_request_dma_chans(struct mic_device *mdev)
+{
+	dma_cap_mask_t mask;
+	struct dma_chan *chan;
+
+	request_module("mic_x100_dma");
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_MEMCPY, mask);
+
+	do {
+		chan = dma_request_channel(mask, mdev->ops->dma_filter,
+					   mdev->sdev->parent);
+		if (chan) {
+			mdev->dma_ch[mdev->num_dma_ch++] = chan;
+			if (mdev->num_dma_ch >= MIC_MAX_DMA_CHAN)
+				break;
+		}
+	} while (chan);
+	dev_info(mdev->sdev->parent, "DMA channels # %d\n", mdev->num_dma_ch);
+	return mdev->num_dma_ch;
+}
+
+/**
+ * mic_free_dma_chans - release DMA channels
+ * @mdev: pointer to mic_device instance
+ *
+ * returns none
+ */
+static void mic_free_dma_chans(struct mic_device *mdev)
+{
+	int i = 0;
+
+	for (i = 0; i < mdev->num_dma_ch; i++) {
+		dma_release_channel(mdev->dma_ch[i]);
+		mdev->dma_ch[i] = NULL;
+	}
+	mdev->num_dma_ch = 0;
 }
 
 /**
@@ -141,6 +376,7 @@ int mic_start(struct mic_device *mdev, const char *buf)
 {
 	int rc;
 	mutex_lock(&mdev->mic_mutex);
+	mic_bootparam_init(mdev);
 retry:
 	if (MIC_OFFLINE != mdev->state) {
 		rc = -EINVAL;
@@ -161,14 +397,22 @@ retry:
 		rc = PTR_ERR(mdev->dma_mbdev);
 		goto unlock_ret;
 	}
-	mdev->dma_ch = mic_request_dma_chan(mdev);
-	if (!mdev->dma_ch) {
-		rc = -ENXIO;
+	if (!mic_request_dma_chans(mdev)) {
+		rc = -ENODEV;
 		goto dma_remove;
 	}
+	mdev->scdev = scif_register_device(mdev->sdev->parent, MIC_SCIF_DEV,
+					   &__mic_dma_ops, &scif_hw_ops,
+					   mdev->id + 1, 0, &mdev->mmio,
+					   &mdev->aper, mdev->dp, NULL,
+					   mdev->dma_ch, mdev->num_dma_ch);
+	if (IS_ERR(mdev->scdev)) {
+		rc = PTR_ERR(mdev->scdev);
+		goto dma_free;
+	}
 	rc = mdev->ops->load_mic_fw(mdev, buf);
 	if (rc)
-		goto dma_release;
+		goto scif_remove;
 	mic_smpt_restore(mdev);
 	mic_intr_restore(mdev);
 	mdev->intr_ops->enable_interrupts(mdev);
@@ -177,8 +421,10 @@ retry:
 	mdev->ops->send_firmware_intr(mdev);
 	mic_set_state(mdev, MIC_ONLINE);
 	goto unlock_ret;
-dma_release:
-	dma_release_channel(mdev->dma_ch);
+scif_remove:
+	scif_unregister_device(mdev->scdev);
+dma_free:
+	mic_free_dma_chans(mdev);
 dma_remove:
 	mbus_unregister_device(mdev->dma_mbdev);
 unlock_ret:
@@ -197,11 +443,9 @@ void mic_stop(struct mic_device *mdev, bool force)
 {
 	mutex_lock(&mdev->mic_mutex);
 	if (MIC_OFFLINE != mdev->state || force) {
+		scif_unregister_device(mdev->scdev);
 		mic_virtio_reset_devices(mdev);
-		if (mdev->dma_ch) {
-			dma_release_channel(mdev->dma_ch);
-			mdev->dma_ch = NULL;
-		}
+		mic_free_dma_chans(mdev);
 		mbus_unregister_device(mdev->dma_mbdev);
 		mic_bootparam_init(mdev);
 		mic_reset(mdev);
diff --git a/drivers/misc/mic/host/mic_debugfs.c b/drivers/misc/mic/host/mic_debugfs.c
index 687e9aacf3bb..3c9ea4896f3c 100644
--- a/drivers/misc/mic/host/mic_debugfs.c
+++ b/drivers/misc/mic/host/mic_debugfs.c
@@ -214,6 +214,19 @@ static int mic_dp_show(struct seq_file *s, void *pos)
 		   bootparam->shutdown_status);
 	seq_printf(s, "Bootparam: shutdown_card %d\n",
 		   bootparam->shutdown_card);
+	seq_printf(s, "Bootparam: tot_nodes %d\n",
+		   bootparam->tot_nodes);
+	seq_printf(s, "Bootparam: node_id %d\n",
+		   bootparam->node_id);
+	seq_printf(s, "Bootparam: c2h_scif_db %d\n",
+		   bootparam->c2h_scif_db);
+	seq_printf(s, "Bootparam: h2c_scif_db %d\n",
+		   bootparam->h2c_scif_db);
+	seq_printf(s, "Bootparam: scif_host_dma_addr 0x%llx\n",
+		   bootparam->scif_host_dma_addr);
+	seq_printf(s, "Bootparam: scif_card_dma_addr 0x%llx\n",
+		   bootparam->scif_card_dma_addr);
+
 
 	for (i = sizeof(*bootparam); i < MIC_DP_SIZE;
 	     i += mic_total_desc_size(d)) {
diff --git a/drivers/misc/mic/host/mic_device.h b/drivers/misc/mic/host/mic_device.h
index 016bd15a7bd1..01a7555aa648 100644
--- a/drivers/misc/mic/host/mic_device.h
+++ b/drivers/misc/mic/host/mic_device.h
@@ -27,7 +27,7 @@
 #include <linux/irqreturn.h>
 #include <linux/dmaengine.h>
 #include <linux/mic_bus.h>
-
+#include "../bus/scif_bus.h"
 #include "mic_intr.h"
 
 /* The maximum number of MIC devices supported in a single host system. */
@@ -90,7 +90,9 @@ enum mic_stepping {
  * @vdev_list: list of virtio devices.
  * @pm_notifier: Handles PM notifications from the OS.
  * @dma_mbdev: MIC BUS DMA device.
- * @dma_ch: DMA channel reserved by this driver for use by virtio devices.
+ * @dma_ch - Array of DMA channels
+ * @num_dma_ch - Number of DMA channels available
+ * @scdev: SCIF device on the SCIF virtual bus.
  */
 struct mic_device {
 	struct mic_mw mmio;
@@ -129,7 +131,9 @@ struct mic_device {
 	struct list_head vdev_list;
 	struct notifier_block pm_notifier;
 	struct mbus_device *dma_mbdev;
-	struct dma_chan *dma_ch;
+	struct dma_chan *dma_ch[MIC_MAX_DMA_CHAN];
+	int num_dma_ch;
+	struct scif_hw_dev *scdev;
 };
 
 /**
@@ -228,4 +232,5 @@ void mic_exit_debugfs(void);
 void mic_prepare_suspend(struct mic_device *mdev);
 void mic_complete_resume(struct mic_device *mdev);
 void mic_suspend(struct mic_device *mdev);
+extern atomic_t g_num_mics;
 #endif
diff --git a/drivers/misc/mic/host/mic_intr.h b/drivers/misc/mic/host/mic_intr.h
index 9f783d4ad7f1..cce28824db8a 100644
--- a/drivers/misc/mic/host/mic_intr.h
+++ b/drivers/misc/mic/host/mic_intr.h
@@ -28,8 +28,9 @@
  * 3 for virtio network, console and block devices.
  * 1 for card shutdown notifications.
  * 4 for host owned DMA channels.
+ * 1 for SCIF
  */
-#define MIC_MIN_MSIX 8
+#define MIC_MIN_MSIX 9
 #define MIC_NUM_OFFSETS 32
 
 /**
diff --git a/drivers/misc/mic/host/mic_main.c b/drivers/misc/mic/host/mic_main.c
index ab37a3117d23..456462932151 100644
--- a/drivers/misc/mic/host/mic_main.c
+++ b/drivers/misc/mic/host/mic_main.c
@@ -67,6 +67,8 @@ static struct ida g_mic_ida;
 static struct class *g_mic_class;
 /* Base device node number for MIC devices */
 static dev_t g_mic_devno;
+/* Track the total number of MIC devices */
+atomic_t g_num_mics;
 
 static const struct file_operations mic_fops = {
 	.open = mic_open,
@@ -408,6 +410,7 @@ static int mic_probe(struct pci_dev *pdev,
 		dev_err(&pdev->dev, "cdev_add err id %d rc %d\n", mdev->id, rc);
 		goto cleanup_debug_dir;
 	}
+	atomic_inc(&g_num_mics);
 	return 0;
 cleanup_debug_dir:
 	mic_delete_debug_dir(mdev);
@@ -459,6 +462,7 @@ static void mic_remove(struct pci_dev *pdev)
 		return;
 
 	mic_stop(mdev, false);
+	atomic_dec(&g_num_mics);
 	cdev_del(&mdev->cdev);
 	mic_delete_debug_dir(mdev);
 	mutex_lock(&mdev->mic_mutex);
@@ -478,6 +482,7 @@ static void mic_remove(struct pci_dev *pdev)
 	ida_simple_remove(&g_mic_ida, mdev->id);
 	kfree(mdev);
 }
+
 static struct pci_driver mic_driver = {
 	.name = mic_driver_name,
 	.id_table = mic_pci_tbl,
@@ -512,6 +517,7 @@ static int __init mic_init(void)
 	}
 	return ret;
 cleanup_debugfs:
+	ida_destroy(&g_mic_ida);
 	mic_exit_debugfs();
 	class_destroy(g_mic_class);
 cleanup_chrdev:
diff --git a/drivers/misc/mic/host/mic_smpt.c b/drivers/misc/mic/host/mic_smpt.c
index fae474c4899e..cec82034875f 100644
--- a/drivers/misc/mic/host/mic_smpt.c
+++ b/drivers/misc/mic/host/mic_smpt.c
@@ -174,8 +174,7 @@ static int mic_get_smpt_ref_count(struct mic_device *mdev, dma_addr_t dma_addr,
  *
  * returns a DMA address.
  */
-static dma_addr_t
-mic_to_dma_addr(struct mic_device *mdev, dma_addr_t mic_addr)
+dma_addr_t mic_to_dma_addr(struct mic_device *mdev, dma_addr_t mic_addr)
 {
 	struct mic_smpt_info *smpt_info = mdev->smpt;
 	int spt;
@@ -214,7 +213,7 @@ dma_addr_t mic_map(struct mic_device *mdev, dma_addr_t dma_addr, size_t size)
 	if (!size || size > mic_max_system_memory(mdev))
 		return mic_addr;
 
-	ref = kmalloc(mdev->smpt->info.num_reg * sizeof(s64), GFP_KERNEL);
+	ref = kmalloc_array(mdev->smpt->info.num_reg, sizeof(s64), GFP_ATOMIC);
 	if (!ref)
 		return mic_addr;
 
@@ -271,7 +270,7 @@ void mic_unmap(struct mic_device *mdev, dma_addr_t mic_addr, size_t size)
 	}
 
 	spt = mic_sys_addr_to_smpt(mdev, mic_addr);
-	ref = kmalloc(mdev->smpt->info.num_reg * sizeof(s64), GFP_KERNEL);
+	ref = kmalloc_array(mdev->smpt->info.num_reg, sizeof(s64), GFP_ATOMIC);
 	if (!ref)
 		return;
 
diff --git a/drivers/misc/mic/host/mic_smpt.h b/drivers/misc/mic/host/mic_smpt.h
index 51970abfe7df..68721c6e7455 100644
--- a/drivers/misc/mic/host/mic_smpt.h
+++ b/drivers/misc/mic/host/mic_smpt.h
@@ -78,6 +78,7 @@ void mic_unmap_single(struct mic_device *mdev,
 dma_addr_t mic_map(struct mic_device *mdev,
 	dma_addr_t dma_addr, size_t size);
 void mic_unmap(struct mic_device *mdev, dma_addr_t mic_addr, size_t size);
+dma_addr_t mic_to_dma_addr(struct mic_device *mdev, dma_addr_t mic_addr);
 
 /**
  * mic_map_error - Check a MIC address for errors.
diff --git a/drivers/misc/mic/host/mic_virtio.c b/drivers/misc/mic/host/mic_virtio.c
index a020e4eb435a..cc08e9f733c9 100644
--- a/drivers/misc/mic/host/mic_virtio.c
+++ b/drivers/misc/mic/host/mic_virtio.c
@@ -40,7 +40,7 @@ static int mic_sync_dma(struct mic_device *mdev, dma_addr_t dst,
 {
 	int err = 0;
 	struct dma_async_tx_descriptor *tx;
-	struct dma_chan *mic_ch = mdev->dma_ch;
+	struct dma_chan *mic_ch = mdev->dma_ch[0];
 
 	if (!mic_ch) {
 		err = -EBUSY;
@@ -80,7 +80,7 @@ static int mic_virtio_copy_to_user(struct mic_vdev *mvdev, void __user *ubuf,
 	struct mic_device *mdev = mvdev->mdev;
 	void __iomem *dbuf = mdev->aper.va + daddr;
 	struct mic_vringh *mvr = &mvdev->mvr[vr_idx];
-	size_t dma_alignment = 1 << mdev->dma_ch->device->copy_align;
+	size_t dma_alignment = 1 << mdev->dma_ch[0]->device->copy_align;
 	size_t dma_offset;
 	size_t partlen;
 	int err;
@@ -129,7 +129,7 @@ static int mic_virtio_copy_from_user(struct mic_vdev *mvdev, void __user *ubuf,
 	struct mic_device *mdev = mvdev->mdev;
 	void __iomem *dbuf = mdev->aper.va + daddr;
 	struct mic_vringh *mvr = &mvdev->mvr[vr_idx];
-	size_t dma_alignment = 1 << mdev->dma_ch->device->copy_align;
+	size_t dma_alignment = 1 << mdev->dma_ch[0]->device->copy_align;
 	size_t partlen;
 	int err;
 
diff --git a/drivers/misc/mic/host/mic_x100.c b/drivers/misc/mic/host/mic_x100.c
index b7a21e11dcdf..3341e90dede4 100644
--- a/drivers/misc/mic/host/mic_x100.c
+++ b/drivers/misc/mic/host/mic_x100.c
@@ -167,8 +167,7 @@ static void mic_x100_send_intr(struct mic_device *mdev, int doorbell)
 	if (doorbell < MIC_X100_NUM_SBOX_IRQ) {
 		mic_x100_send_sbox_intr(mdev, doorbell);
 	} else {
-		rdmasr_db = doorbell - MIC_X100_NUM_SBOX_IRQ +
-			MIC_X100_RDMASR_IRQ_BASE;
+		rdmasr_db = doorbell - MIC_X100_NUM_SBOX_IRQ;
 		mic_x100_send_rdmasr_intr(mdev, rdmasr_db);
 	}
 }
diff --git a/drivers/misc/mic/scif/Makefile b/drivers/misc/mic/scif/Makefile
new file mode 100644
index 000000000000..bf10bb7e2b91
--- /dev/null
+++ b/drivers/misc/mic/scif/Makefile
@@ -0,0 +1,15 @@
+#
+# Makefile - SCIF driver.
+# Copyright(c) 2014, Intel Corporation.
+#
+obj-$(CONFIG_SCIF) += scif.o
+scif-objs := scif_main.o
+scif-objs += scif_peer_bus.o
+scif-objs += scif_ports.o
+scif-objs += scif_debugfs.o
+scif-objs += scif_fd.o
+scif-objs += scif_api.o
+scif-objs += scif_epd.o
+scif-objs += scif_rb.o
+scif-objs += scif_nodeqp.o
+scif-objs += scif_nm.o
diff --git a/drivers/misc/mic/scif/scif_api.c b/drivers/misc/mic/scif/scif_api.c
new file mode 100644
index 000000000000..f39d3135a9ef
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_api.c
@@ -0,0 +1,1276 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include <linux/scif.h>
+#include "scif_main.h"
+#include "scif_map.h"
+
+static const char * const scif_ep_states[] = {
+	"Unbound",
+	"Bound",
+	"Listening",
+	"Connected",
+	"Connecting",
+	"Mapping",
+	"Closing",
+	"Close Listening",
+	"Disconnected",
+	"Zombie"};
+
+enum conn_async_state {
+	ASYNC_CONN_IDLE = 1,	/* ep setup for async connect */
+	ASYNC_CONN_INPROGRESS,	/* async connect in progress */
+	ASYNC_CONN_FLUSH_WORK	/* async work flush in progress  */
+};
+
+scif_epd_t scif_open(void)
+{
+	struct scif_endpt *ep;
+
+	might_sleep();
+	ep = kzalloc(sizeof(*ep), GFP_KERNEL);
+	if (!ep)
+		goto err_ep_alloc;
+
+	ep->qp_info.qp = kzalloc(sizeof(*ep->qp_info.qp), GFP_KERNEL);
+	if (!ep->qp_info.qp)
+		goto err_qp_alloc;
+
+	spin_lock_init(&ep->lock);
+	mutex_init(&ep->sendlock);
+	mutex_init(&ep->recvlock);
+
+	ep->state = SCIFEP_UNBOUND;
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI open: ep %p success\n", ep);
+	return ep;
+
+err_qp_alloc:
+	kfree(ep);
+err_ep_alloc:
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(scif_open);
+
+/*
+ * scif_disconnect_ep - Disconnects the endpoint if found
+ * @epd: The end point returned from scif_open()
+ */
+static struct scif_endpt *scif_disconnect_ep(struct scif_endpt *ep)
+{
+	struct scifmsg msg;
+	struct scif_endpt *fep = NULL;
+	struct scif_endpt *tmpep;
+	struct list_head *pos, *tmpq;
+	int err;
+
+	/*
+	 * Wake up any threads blocked in send()/recv() before closing
+	 * out the connection. Grabbing and releasing the send/recv lock
+	 * will ensure that any blocked senders/receivers have exited for
+	 * Ring 0 endpoints. It is a Ring 0 bug to call send/recv after
+	 * close. Ring 3 endpoints are not affected since close will not
+	 * be called while there are IOCTLs executing.
+	 */
+	wake_up_interruptible(&ep->sendwq);
+	wake_up_interruptible(&ep->recvwq);
+	mutex_lock(&ep->sendlock);
+	mutex_unlock(&ep->sendlock);
+	mutex_lock(&ep->recvlock);
+	mutex_unlock(&ep->recvlock);
+
+	/* Remove from the connected list */
+	mutex_lock(&scif_info.connlock);
+	list_for_each_safe(pos, tmpq, &scif_info.connected) {
+		tmpep = list_entry(pos, struct scif_endpt, list);
+		if (tmpep == ep) {
+			list_del(pos);
+			fep = tmpep;
+			spin_lock(&ep->lock);
+			break;
+		}
+	}
+
+	if (!fep) {
+		/*
+		 * The other side has completed the disconnect before
+		 * the end point can be removed from the list. Therefore
+		 * the ep lock is not locked, traverse the disconnected
+		 * list to find the endpoint and release the conn lock.
+		 */
+		list_for_each_safe(pos, tmpq, &scif_info.disconnected) {
+			tmpep = list_entry(pos, struct scif_endpt, list);
+			if (tmpep == ep) {
+				list_del(pos);
+				break;
+			}
+		}
+		mutex_unlock(&scif_info.connlock);
+		return NULL;
+	}
+
+	init_completion(&ep->discon);
+	msg.uop = SCIF_DISCNCT;
+	msg.src = ep->port;
+	msg.dst = ep->peer;
+	msg.payload[0] = (u64)ep;
+	msg.payload[1] = ep->remote_ep;
+
+	err = scif_nodeqp_send(ep->remote_dev, &msg);
+	spin_unlock(&ep->lock);
+	mutex_unlock(&scif_info.connlock);
+
+	if (!err)
+		/* Wait for the remote node to respond with SCIF_DISCNT_ACK */
+		wait_for_completion_timeout(&ep->discon,
+					    SCIF_NODE_ALIVE_TIMEOUT);
+	return ep;
+}
+
+int scif_close(scif_epd_t epd)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	struct scif_endpt *tmpep;
+	struct list_head *pos, *tmpq;
+	enum scif_epd_state oldstate;
+	bool flush_conn;
+
+	dev_dbg(scif_info.mdev.this_device, "SCIFAPI close: ep %p %s\n",
+		ep, scif_ep_states[ep->state]);
+	might_sleep();
+	spin_lock(&ep->lock);
+	flush_conn = (ep->conn_async_state == ASYNC_CONN_INPROGRESS);
+	spin_unlock(&ep->lock);
+
+	if (flush_conn)
+		flush_work(&scif_info.conn_work);
+
+	spin_lock(&ep->lock);
+	oldstate = ep->state;
+
+	ep->state = SCIFEP_CLOSING;
+
+	switch (oldstate) {
+	case SCIFEP_ZOMBIE:
+	case SCIFEP_DISCONNECTED:
+		spin_unlock(&ep->lock);
+		/* Remove from the disconnected list */
+		mutex_lock(&scif_info.connlock);
+		list_for_each_safe(pos, tmpq, &scif_info.disconnected) {
+			tmpep = list_entry(pos, struct scif_endpt, list);
+			if (tmpep == ep) {
+				list_del(pos);
+				break;
+			}
+		}
+		mutex_unlock(&scif_info.connlock);
+		break;
+	case SCIFEP_UNBOUND:
+	case SCIFEP_BOUND:
+	case SCIFEP_CONNECTING:
+		spin_unlock(&ep->lock);
+		break;
+	case SCIFEP_MAPPING:
+	case SCIFEP_CONNECTED:
+	case SCIFEP_CLOSING:
+	{
+		spin_unlock(&ep->lock);
+		scif_disconnect_ep(ep);
+		break;
+	}
+	case SCIFEP_LISTENING:
+	case SCIFEP_CLLISTEN:
+	{
+		struct scif_conreq *conreq;
+		struct scifmsg msg;
+		struct scif_endpt *aep;
+
+		spin_unlock(&ep->lock);
+		spin_lock(&scif_info.eplock);
+
+		/* remove from listen list */
+		list_for_each_safe(pos, tmpq, &scif_info.listen) {
+			tmpep = list_entry(pos, struct scif_endpt, list);
+			if (tmpep == ep)
+				list_del(pos);
+		}
+		/* Remove any dangling accepts */
+		while (ep->acceptcnt) {
+			aep = list_first_entry(&ep->li_accept,
+					       struct scif_endpt, liacceptlist);
+			list_del(&aep->liacceptlist);
+			scif_put_port(aep->port.port);
+			list_for_each_safe(pos, tmpq, &scif_info.uaccept) {
+				tmpep = list_entry(pos, struct scif_endpt,
+						   miacceptlist);
+				if (tmpep == aep) {
+					list_del(pos);
+					break;
+				}
+			}
+			spin_unlock(&scif_info.eplock);
+			mutex_lock(&scif_info.connlock);
+			list_for_each_safe(pos, tmpq, &scif_info.connected) {
+				tmpep = list_entry(pos,
+						   struct scif_endpt, list);
+				if (tmpep == aep) {
+					list_del(pos);
+					break;
+				}
+			}
+			list_for_each_safe(pos, tmpq, &scif_info.disconnected) {
+				tmpep = list_entry(pos,
+						   struct scif_endpt, list);
+				if (tmpep == aep) {
+					list_del(pos);
+					break;
+				}
+			}
+			mutex_unlock(&scif_info.connlock);
+			scif_teardown_ep(aep);
+			spin_lock(&scif_info.eplock);
+			scif_add_epd_to_zombie_list(aep, SCIF_EPLOCK_HELD);
+			ep->acceptcnt--;
+		}
+
+		spin_lock(&ep->lock);
+		spin_unlock(&scif_info.eplock);
+
+		/* Remove and reject any pending connection requests. */
+		while (ep->conreqcnt) {
+			conreq = list_first_entry(&ep->conlist,
+						  struct scif_conreq, list);
+			list_del(&conreq->list);
+
+			msg.uop = SCIF_CNCT_REJ;
+			msg.dst.node = conreq->msg.src.node;
+			msg.dst.port = conreq->msg.src.port;
+			msg.payload[0] = conreq->msg.payload[0];
+			msg.payload[1] = conreq->msg.payload[1];
+			/*
+			 * No Error Handling on purpose for scif_nodeqp_send().
+			 * If the remote node is lost we still want free the
+			 * connection requests on the self node.
+			 */
+			scif_nodeqp_send(&scif_dev[conreq->msg.src.node],
+					 &msg);
+			ep->conreqcnt--;
+			kfree(conreq);
+		}
+
+		spin_unlock(&ep->lock);
+		/* If a kSCIF accept is waiting wake it up */
+		wake_up_interruptible(&ep->conwq);
+		break;
+	}
+	}
+	scif_put_port(ep->port.port);
+	scif_teardown_ep(ep);
+	scif_add_epd_to_zombie_list(ep, !SCIF_EPLOCK_HELD);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(scif_close);
+
+/**
+ * scif_flush() - Wakes up any blocking accepts. The endpoint will no longer
+ *			accept new connections.
+ * @epd: The end point returned from scif_open()
+ */
+int __scif_flush(scif_epd_t epd)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+
+	switch (ep->state) {
+	case SCIFEP_LISTENING:
+	{
+		ep->state = SCIFEP_CLLISTEN;
+
+		/* If an accept is waiting wake it up */
+		wake_up_interruptible(&ep->conwq);
+		break;
+	}
+	default:
+		break;
+	}
+	return 0;
+}
+
+int scif_bind(scif_epd_t epd, u16 pn)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	int ret = 0;
+	int tmp;
+
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI bind: ep %p %s requested port number %d\n",
+		ep, scif_ep_states[ep->state], pn);
+	if (pn) {
+		/*
+		 * Similar to IETF RFC 1700, SCIF ports below
+		 * SCIF_ADMIN_PORT_END can only be bound by system (or root)
+		 * processes or by processes executed by privileged users.
+		 */
+		if (pn < SCIF_ADMIN_PORT_END && !capable(CAP_SYS_ADMIN)) {
+			ret = -EACCES;
+			goto scif_bind_admin_exit;
+		}
+	}
+
+	spin_lock(&ep->lock);
+	if (ep->state == SCIFEP_BOUND) {
+		ret = -EINVAL;
+		goto scif_bind_exit;
+	} else if (ep->state != SCIFEP_UNBOUND) {
+		ret = -EISCONN;
+		goto scif_bind_exit;
+	}
+
+	if (pn) {
+		tmp = scif_rsrv_port(pn);
+		if (tmp != pn) {
+			ret = -EINVAL;
+			goto scif_bind_exit;
+		}
+	} else {
+		pn = scif_get_new_port();
+		if (!pn) {
+			ret = -ENOSPC;
+			goto scif_bind_exit;
+		}
+	}
+
+	ep->state = SCIFEP_BOUND;
+	ep->port.node = scif_info.nodeid;
+	ep->port.port = pn;
+	ep->conn_async_state = ASYNC_CONN_IDLE;
+	ret = pn;
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI bind: bound to port number %d\n", pn);
+scif_bind_exit:
+	spin_unlock(&ep->lock);
+scif_bind_admin_exit:
+	return ret;
+}
+EXPORT_SYMBOL_GPL(scif_bind);
+
+int scif_listen(scif_epd_t epd, int backlog)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI listen: ep %p %s\n", ep, scif_ep_states[ep->state]);
+	spin_lock(&ep->lock);
+	switch (ep->state) {
+	case SCIFEP_ZOMBIE:
+	case SCIFEP_CLOSING:
+	case SCIFEP_CLLISTEN:
+	case SCIFEP_UNBOUND:
+	case SCIFEP_DISCONNECTED:
+		spin_unlock(&ep->lock);
+		return -EINVAL;
+	case SCIFEP_LISTENING:
+	case SCIFEP_CONNECTED:
+	case SCIFEP_CONNECTING:
+	case SCIFEP_MAPPING:
+		spin_unlock(&ep->lock);
+		return -EISCONN;
+	case SCIFEP_BOUND:
+		break;
+	}
+
+	ep->state = SCIFEP_LISTENING;
+	ep->backlog = backlog;
+
+	ep->conreqcnt = 0;
+	ep->acceptcnt = 0;
+	INIT_LIST_HEAD(&ep->conlist);
+	init_waitqueue_head(&ep->conwq);
+	INIT_LIST_HEAD(&ep->li_accept);
+	spin_unlock(&ep->lock);
+
+	/*
+	 * Listen status is complete so delete the qp information not needed
+	 * on a listen before placing on the list of listening ep's
+	 */
+	scif_teardown_ep(ep);
+	ep->qp_info.qp = NULL;
+
+	spin_lock(&scif_info.eplock);
+	list_add_tail(&ep->list, &scif_info.listen);
+	spin_unlock(&scif_info.eplock);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(scif_listen);
+
+/*
+ ************************************************************************
+ * SCIF connection flow:
+ *
+ * 1) A SCIF listening endpoint can call scif_accept(..) to wait for SCIF
+ *	connections via a SCIF_CNCT_REQ message
+ * 2) A SCIF endpoint can initiate a SCIF connection by calling
+ *	scif_connect(..) which calls scif_setup_qp_connect(..) which
+ *	allocates the local qp for the endpoint ring buffer and then sends
+ *	a SCIF_CNCT_REQ to the remote node and waits for a SCIF_CNCT_GNT or
+ *	a SCIF_CNCT_REJ message
+ * 3) The peer node handles a SCIF_CNCT_REQ via scif_cnctreq_resp(..) which
+ *	wakes up any threads blocked in step 1 or sends a SCIF_CNCT_REJ
+ *	message otherwise
+ * 4) A thread blocked waiting for incoming connections allocates its local
+ *	endpoint QP and ring buffer following which it sends a SCIF_CNCT_GNT
+ *	and waits for a SCIF_CNCT_GNT(N)ACK. If the allocation fails then
+ *	the node sends a SCIF_CNCT_REJ message
+ * 5) Upon receipt of a SCIF_CNCT_GNT or a SCIF_CNCT_REJ message the
+ *	connecting endpoint is woken up as part of handling
+ *	scif_cnctgnt_resp(..) following which it maps the remote endpoints'
+ *	QP, updates its outbound QP and sends a SCIF_CNCT_GNTACK message on
+ *	success or a SCIF_CNCT_GNTNACK message on failure and completes
+ *	the scif_connect(..) API
+ * 6) Upon receipt of a SCIF_CNCT_GNT(N)ACK the accepting endpoint blocked
+ *	in step 4 is woken up and completes the scif_accept(..) API
+ * 7) The SCIF connection is now established between the two SCIF endpoints.
+ */
+static int scif_conn_func(struct scif_endpt *ep)
+{
+	int err = 0;
+	struct scifmsg msg;
+	struct device *spdev;
+
+	/* Initiate the first part of the endpoint QP setup */
+	err = scif_setup_qp_connect(ep->qp_info.qp, &ep->qp_info.qp_offset,
+				    SCIF_ENDPT_QP_SIZE, ep->remote_dev);
+	if (err) {
+		dev_err(&ep->remote_dev->sdev->dev,
+			"%s err %d qp_offset 0x%llx\n",
+			__func__, err, ep->qp_info.qp_offset);
+		ep->state = SCIFEP_BOUND;
+		goto connect_error_simple;
+	}
+
+	spdev = scif_get_peer_dev(ep->remote_dev);
+	if (IS_ERR(spdev)) {
+		err = PTR_ERR(spdev);
+		goto cleanup_qp;
+	}
+	/* Format connect message and send it */
+	msg.src = ep->port;
+	msg.dst = ep->conn_port;
+	msg.uop = SCIF_CNCT_REQ;
+	msg.payload[0] = (u64)ep;
+	msg.payload[1] = ep->qp_info.qp_offset;
+	err = _scif_nodeqp_send(ep->remote_dev, &msg);
+	if (err)
+		goto connect_error_dec;
+	scif_put_peer_dev(spdev);
+	/*
+	 * Wait for the remote node to respond with SCIF_CNCT_GNT or
+	 * SCIF_CNCT_REJ message.
+	 */
+	err = wait_event_timeout(ep->conwq, ep->state != SCIFEP_CONNECTING,
+				 SCIF_NODE_ALIVE_TIMEOUT);
+	if (!err) {
+		dev_err(&ep->remote_dev->sdev->dev,
+			"%s %d timeout\n", __func__, __LINE__);
+		ep->state = SCIFEP_BOUND;
+	}
+	spdev = scif_get_peer_dev(ep->remote_dev);
+	if (IS_ERR(spdev)) {
+		err = PTR_ERR(spdev);
+		goto cleanup_qp;
+	}
+	if (ep->state == SCIFEP_MAPPING) {
+		err = scif_setup_qp_connect_response(ep->remote_dev,
+						     ep->qp_info.qp,
+						     ep->qp_info.gnt_pld);
+		/*
+		 * If the resource to map the queue are not available then
+		 * we need to tell the other side to terminate the accept
+		 */
+		if (err) {
+			dev_err(&ep->remote_dev->sdev->dev,
+				"%s %d err %d\n", __func__, __LINE__, err);
+			msg.uop = SCIF_CNCT_GNTNACK;
+			msg.payload[0] = ep->remote_ep;
+			_scif_nodeqp_send(ep->remote_dev, &msg);
+			ep->state = SCIFEP_BOUND;
+			goto connect_error_dec;
+		}
+
+		msg.uop = SCIF_CNCT_GNTACK;
+		msg.payload[0] = ep->remote_ep;
+		err = _scif_nodeqp_send(ep->remote_dev, &msg);
+		if (err) {
+			ep->state = SCIFEP_BOUND;
+			goto connect_error_dec;
+		}
+		ep->state = SCIFEP_CONNECTED;
+		mutex_lock(&scif_info.connlock);
+		list_add_tail(&ep->list, &scif_info.connected);
+		mutex_unlock(&scif_info.connlock);
+		dev_dbg(&ep->remote_dev->sdev->dev,
+			"SCIFAPI connect: ep %p connected\n", ep);
+	} else if (ep->state == SCIFEP_BOUND) {
+		dev_dbg(&ep->remote_dev->sdev->dev,
+			"SCIFAPI connect: ep %p connection refused\n", ep);
+		err = -ECONNREFUSED;
+		goto connect_error_dec;
+	}
+	scif_put_peer_dev(spdev);
+	return err;
+connect_error_dec:
+	scif_put_peer_dev(spdev);
+cleanup_qp:
+	scif_cleanup_ep_qp(ep);
+connect_error_simple:
+	return err;
+}
+
+/*
+ * scif_conn_handler:
+ *
+ * Workqueue handler for servicing non-blocking SCIF connect
+ *
+ */
+void scif_conn_handler(struct work_struct *work)
+{
+	struct scif_endpt *ep;
+
+	do {
+		ep = NULL;
+		spin_lock(&scif_info.nb_connect_lock);
+		if (!list_empty(&scif_info.nb_connect_list)) {
+			ep = list_first_entry(&scif_info.nb_connect_list,
+					      struct scif_endpt, conn_list);
+			list_del(&ep->conn_list);
+		}
+		spin_unlock(&scif_info.nb_connect_lock);
+		if (ep)
+			ep->conn_err = scif_conn_func(ep);
+	} while (ep);
+}
+
+int __scif_connect(scif_epd_t epd, struct scif_port_id *dst, bool non_block)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	int err = 0;
+	struct scif_dev *remote_dev;
+	struct device *spdev;
+
+	dev_dbg(scif_info.mdev.this_device, "SCIFAPI connect: ep %p %s\n", ep,
+		scif_ep_states[ep->state]);
+
+	if (!scif_dev || dst->node > scif_info.maxid)
+		return -ENODEV;
+
+	might_sleep();
+
+	remote_dev = &scif_dev[dst->node];
+	spdev = scif_get_peer_dev(remote_dev);
+	if (IS_ERR(spdev)) {
+		err = PTR_ERR(spdev);
+		return err;
+	}
+
+	spin_lock(&ep->lock);
+	switch (ep->state) {
+	case SCIFEP_ZOMBIE:
+	case SCIFEP_CLOSING:
+		err = -EINVAL;
+		break;
+	case SCIFEP_DISCONNECTED:
+		if (ep->conn_async_state == ASYNC_CONN_INPROGRESS)
+			ep->conn_async_state = ASYNC_CONN_FLUSH_WORK;
+		else
+			err = -EINVAL;
+		break;
+	case SCIFEP_LISTENING:
+	case SCIFEP_CLLISTEN:
+		err = -EOPNOTSUPP;
+		break;
+	case SCIFEP_CONNECTING:
+	case SCIFEP_MAPPING:
+		if (ep->conn_async_state == ASYNC_CONN_INPROGRESS)
+			err = -EINPROGRESS;
+		else
+			err = -EISCONN;
+		break;
+	case SCIFEP_CONNECTED:
+		if (ep->conn_async_state == ASYNC_CONN_INPROGRESS)
+			ep->conn_async_state = ASYNC_CONN_FLUSH_WORK;
+		else
+			err = -EISCONN;
+		break;
+	case SCIFEP_UNBOUND:
+		ep->port.port = scif_get_new_port();
+		if (!ep->port.port) {
+			err = -ENOSPC;
+		} else {
+			ep->port.node = scif_info.nodeid;
+			ep->conn_async_state = ASYNC_CONN_IDLE;
+		}
+		/* Fall through */
+	case SCIFEP_BOUND:
+		/*
+		 * If a non-blocking connect has been already initiated
+		 * (conn_async_state is either ASYNC_CONN_INPROGRESS or
+		 * ASYNC_CONN_FLUSH_WORK), the end point could end up in
+		 * SCIF_BOUND due an error in the connection process
+		 * (e.g., connection refused) If conn_async_state is
+		 * ASYNC_CONN_INPROGRESS - transition to ASYNC_CONN_FLUSH_WORK
+		 * so that the error status can be collected. If the state is
+		 * already ASYNC_CONN_FLUSH_WORK - then set the error to
+		 * EINPROGRESS since some other thread is waiting to collect
+		 * error status.
+		 */
+		if (ep->conn_async_state == ASYNC_CONN_INPROGRESS) {
+			ep->conn_async_state = ASYNC_CONN_FLUSH_WORK;
+		} else if (ep->conn_async_state == ASYNC_CONN_FLUSH_WORK) {
+			err = -EINPROGRESS;
+		} else {
+			ep->conn_port = *dst;
+			init_waitqueue_head(&ep->sendwq);
+			init_waitqueue_head(&ep->recvwq);
+			init_waitqueue_head(&ep->conwq);
+			ep->conn_async_state = 0;
+
+			if (unlikely(non_block))
+				ep->conn_async_state = ASYNC_CONN_INPROGRESS;
+		}
+		break;
+	}
+
+	if (err || ep->conn_async_state == ASYNC_CONN_FLUSH_WORK)
+			goto connect_simple_unlock1;
+
+	ep->state = SCIFEP_CONNECTING;
+	ep->remote_dev = &scif_dev[dst->node];
+	ep->qp_info.qp->magic = SCIFEP_MAGIC;
+	if (ep->conn_async_state == ASYNC_CONN_INPROGRESS) {
+		spin_lock(&scif_info.nb_connect_lock);
+		list_add_tail(&ep->conn_list, &scif_info.nb_connect_list);
+		spin_unlock(&scif_info.nb_connect_lock);
+		err = -EINPROGRESS;
+		schedule_work(&scif_info.conn_work);
+	}
+connect_simple_unlock1:
+	spin_unlock(&ep->lock);
+	scif_put_peer_dev(spdev);
+	if (err) {
+		return err;
+	} else if (ep->conn_async_state == ASYNC_CONN_FLUSH_WORK) {
+		flush_work(&scif_info.conn_work);
+		err = ep->conn_err;
+		spin_lock(&ep->lock);
+		ep->conn_async_state = ASYNC_CONN_IDLE;
+		spin_unlock(&ep->lock);
+	} else {
+		err = scif_conn_func(ep);
+	}
+	return err;
+}
+
+int scif_connect(scif_epd_t epd, struct scif_port_id *dst)
+{
+	return __scif_connect(epd, dst, false);
+}
+EXPORT_SYMBOL_GPL(scif_connect);
+
+/**
+ * scif_accept() - Accept a connection request from the remote node
+ *
+ * The function accepts a connection request from the remote node.  Successful
+ * complete is indicate by a new end point being created and passed back
+ * to the caller for future reference.
+ *
+ * Upon successful complete a zero will be returned and the peer information
+ * will be filled in.
+ *
+ * If the end point is not in the listening state -EINVAL will be returned.
+ *
+ * If during the connection sequence resource allocation fails the -ENOMEM
+ * will be returned.
+ *
+ * If the function is called with the ASYNC flag set and no connection requests
+ * are pending it will return -EAGAIN.
+ *
+ * If the remote side is not sending any connection requests the caller may
+ * terminate this function with a signal.  If so a -EINTR will be returned.
+ */
+int scif_accept(scif_epd_t epd, struct scif_port_id *peer,
+		scif_epd_t *newepd, int flags)
+{
+	struct scif_endpt *lep = (struct scif_endpt *)epd;
+	struct scif_endpt *cep;
+	struct scif_conreq *conreq;
+	struct scifmsg msg;
+	int err;
+	struct device *spdev;
+
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI accept: ep %p %s\n", lep, scif_ep_states[lep->state]);
+
+	if (flags & ~SCIF_ACCEPT_SYNC)
+		return -EINVAL;
+
+	if (!peer || !newepd)
+		return -EINVAL;
+
+	might_sleep();
+	spin_lock(&lep->lock);
+	if (lep->state != SCIFEP_LISTENING) {
+		spin_unlock(&lep->lock);
+		return -EINVAL;
+	}
+
+	if (!lep->conreqcnt && !(flags & SCIF_ACCEPT_SYNC)) {
+		/* No connection request present and we do not want to wait */
+		spin_unlock(&lep->lock);
+		return -EAGAIN;
+	}
+
+	lep->files = current->files;
+retry_connection:
+	spin_unlock(&lep->lock);
+	/* Wait for the remote node to send us a SCIF_CNCT_REQ */
+	err = wait_event_interruptible(lep->conwq,
+				       (lep->conreqcnt ||
+				       (lep->state != SCIFEP_LISTENING)));
+	if (err)
+		return err;
+
+	if (lep->state != SCIFEP_LISTENING)
+		return -EINTR;
+
+	spin_lock(&lep->lock);
+
+	if (!lep->conreqcnt)
+		goto retry_connection;
+
+	/* Get the first connect request off the list */
+	conreq = list_first_entry(&lep->conlist, struct scif_conreq, list);
+	list_del(&conreq->list);
+	lep->conreqcnt--;
+	spin_unlock(&lep->lock);
+
+	/* Fill in the peer information */
+	peer->node = conreq->msg.src.node;
+	peer->port = conreq->msg.src.port;
+
+	cep = kzalloc(sizeof(*cep), GFP_KERNEL);
+	if (!cep) {
+		err = -ENOMEM;
+		goto scif_accept_error_epalloc;
+	}
+	spin_lock_init(&cep->lock);
+	mutex_init(&cep->sendlock);
+	mutex_init(&cep->recvlock);
+	cep->state = SCIFEP_CONNECTING;
+	cep->remote_dev = &scif_dev[peer->node];
+	cep->remote_ep = conreq->msg.payload[0];
+
+	cep->qp_info.qp = kzalloc(sizeof(*cep->qp_info.qp), GFP_KERNEL);
+	if (!cep->qp_info.qp) {
+		err = -ENOMEM;
+		goto scif_accept_error_qpalloc;
+	}
+
+	cep->qp_info.qp->magic = SCIFEP_MAGIC;
+	spdev = scif_get_peer_dev(cep->remote_dev);
+	if (IS_ERR(spdev)) {
+		err = PTR_ERR(spdev);
+		goto scif_accept_error_map;
+	}
+	err = scif_setup_qp_accept(cep->qp_info.qp, &cep->qp_info.qp_offset,
+				   conreq->msg.payload[1], SCIF_ENDPT_QP_SIZE,
+				   cep->remote_dev);
+	if (err) {
+		dev_dbg(&cep->remote_dev->sdev->dev,
+			"SCIFAPI accept: ep %p new %p scif_setup_qp_accept %d qp_offset 0x%llx\n",
+			lep, cep, err, cep->qp_info.qp_offset);
+		scif_put_peer_dev(spdev);
+		goto scif_accept_error_map;
+	}
+
+	cep->port.node = lep->port.node;
+	cep->port.port = lep->port.port;
+	cep->peer.node = peer->node;
+	cep->peer.port = peer->port;
+	init_waitqueue_head(&cep->sendwq);
+	init_waitqueue_head(&cep->recvwq);
+	init_waitqueue_head(&cep->conwq);
+
+	msg.uop = SCIF_CNCT_GNT;
+	msg.src = cep->port;
+	msg.payload[0] = cep->remote_ep;
+	msg.payload[1] = cep->qp_info.qp_offset;
+	msg.payload[2] = (u64)cep;
+
+	err = _scif_nodeqp_send(cep->remote_dev, &msg);
+	scif_put_peer_dev(spdev);
+	if (err)
+		goto scif_accept_error_map;
+retry:
+	/* Wait for the remote node to respond with SCIF_CNCT_GNT(N)ACK */
+	err = wait_event_timeout(cep->conwq, cep->state != SCIFEP_CONNECTING,
+				 SCIF_NODE_ACCEPT_TIMEOUT);
+	if (!err && scifdev_alive(cep))
+		goto retry;
+	err = !err ? -ENODEV : 0;
+	if (err)
+		goto scif_accept_error_map;
+	kfree(conreq);
+
+	spin_lock(&cep->lock);
+
+	if (cep->state == SCIFEP_CLOSING) {
+		/*
+		 * Remote failed to allocate resources and NAKed the grant.
+		 * There is at this point nothing referencing the new end point.
+		 */
+		spin_unlock(&cep->lock);
+		scif_teardown_ep(cep);
+		kfree(cep);
+
+		/* If call with sync flag then go back and wait. */
+		if (flags & SCIF_ACCEPT_SYNC) {
+			spin_lock(&lep->lock);
+			goto retry_connection;
+		}
+		return -EAGAIN;
+	}
+
+	scif_get_port(cep->port.port);
+	*newepd = (scif_epd_t)cep;
+	spin_unlock(&cep->lock);
+	return 0;
+scif_accept_error_map:
+	scif_teardown_ep(cep);
+scif_accept_error_qpalloc:
+	kfree(cep);
+scif_accept_error_epalloc:
+	msg.uop = SCIF_CNCT_REJ;
+	msg.dst.node = conreq->msg.src.node;
+	msg.dst.port = conreq->msg.src.port;
+	msg.payload[0] = conreq->msg.payload[0];
+	msg.payload[1] = conreq->msg.payload[1];
+	scif_nodeqp_send(&scif_dev[conreq->msg.src.node], &msg);
+	kfree(conreq);
+	return err;
+}
+EXPORT_SYMBOL_GPL(scif_accept);
+
+/*
+ * scif_msg_param_check:
+ * @epd: The end point returned from scif_open()
+ * @len: Length to receive
+ * @flags: blocking or non blocking
+ *
+ * Validate parameters for messaging APIs scif_send(..)/scif_recv(..).
+ */
+static inline int scif_msg_param_check(scif_epd_t epd, int len, int flags)
+{
+	int ret = -EINVAL;
+
+	if (len < 0)
+		goto err_ret;
+	if (flags && (!(flags & SCIF_RECV_BLOCK)))
+		goto err_ret;
+	ret = 0;
+err_ret:
+	return ret;
+}
+
+static int _scif_send(scif_epd_t epd, void *msg, int len, int flags)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	struct scifmsg notif_msg;
+	int curr_xfer_len = 0, sent_len = 0, write_count;
+	int ret = 0;
+	struct scif_qp *qp = ep->qp_info.qp;
+
+	if (flags & SCIF_SEND_BLOCK)
+		might_sleep();
+
+	spin_lock(&ep->lock);
+	while (sent_len != len && SCIFEP_CONNECTED == ep->state) {
+		write_count = scif_rb_space(&qp->outbound_q);
+		if (write_count) {
+			/* Best effort to send as much data as possible */
+			curr_xfer_len = min(len - sent_len, write_count);
+			ret = scif_rb_write(&qp->outbound_q, msg,
+					    curr_xfer_len);
+			if (ret < 0)
+				break;
+			/* Success. Update write pointer */
+			scif_rb_commit(&qp->outbound_q);
+			/*
+			 * Send a notification to the peer about the
+			 * produced data message.
+			 */
+			notif_msg.src = ep->port;
+			notif_msg.uop = SCIF_CLIENT_SENT;
+			notif_msg.payload[0] = ep->remote_ep;
+			ret = _scif_nodeqp_send(ep->remote_dev, &notif_msg);
+			if (ret)
+				break;
+			sent_len += curr_xfer_len;
+			msg = msg + curr_xfer_len;
+			continue;
+		}
+		curr_xfer_len = min(len - sent_len, SCIF_ENDPT_QP_SIZE - 1);
+		/* Not enough RB space. return for the Non Blocking case */
+		if (!(flags & SCIF_SEND_BLOCK))
+			break;
+
+		spin_unlock(&ep->lock);
+		/* Wait for a SCIF_CLIENT_RCVD message in the Blocking case */
+		ret =
+		wait_event_interruptible(ep->sendwq,
+					 (SCIFEP_CONNECTED != ep->state) ||
+					 (scif_rb_space(&qp->outbound_q) >=
+					 curr_xfer_len));
+		spin_lock(&ep->lock);
+		if (ret)
+			break;
+	}
+	if (sent_len)
+		ret = sent_len;
+	else if (!ret && SCIFEP_CONNECTED != ep->state)
+		ret = SCIFEP_DISCONNECTED == ep->state ?
+			-ECONNRESET : -ENOTCONN;
+	spin_unlock(&ep->lock);
+	return ret;
+}
+
+static int _scif_recv(scif_epd_t epd, void *msg, int len, int flags)
+{
+	int read_size;
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	struct scifmsg notif_msg;
+	int curr_recv_len = 0, remaining_len = len, read_count;
+	int ret = 0;
+	struct scif_qp *qp = ep->qp_info.qp;
+
+	if (flags & SCIF_RECV_BLOCK)
+		might_sleep();
+	spin_lock(&ep->lock);
+	while (remaining_len && (SCIFEP_CONNECTED == ep->state ||
+				 SCIFEP_DISCONNECTED == ep->state)) {
+		read_count = scif_rb_count(&qp->inbound_q, remaining_len);
+		if (read_count) {
+			/*
+			 * Best effort to recv as much data as there
+			 * are bytes to read in the RB particularly
+			 * important for the Non Blocking case.
+			 */
+			curr_recv_len = min(remaining_len, read_count);
+			read_size = scif_rb_get_next(&qp->inbound_q,
+						     msg, curr_recv_len);
+			if (ep->state == SCIFEP_CONNECTED) {
+				/*
+				 * Update the read pointer only if the endpoint
+				 * is still connected else the read pointer
+				 * might no longer exist since the peer has
+				 * freed resources!
+				 */
+				scif_rb_update_read_ptr(&qp->inbound_q);
+				/*
+				 * Send a notification to the peer about the
+				 * consumed data message only if the EP is in
+				 * SCIFEP_CONNECTED state.
+				 */
+				notif_msg.src = ep->port;
+				notif_msg.uop = SCIF_CLIENT_RCVD;
+				notif_msg.payload[0] = ep->remote_ep;
+				ret = _scif_nodeqp_send(ep->remote_dev,
+							&notif_msg);
+				if (ret)
+					break;
+			}
+			remaining_len -= curr_recv_len;
+			msg = msg + curr_recv_len;
+			continue;
+		}
+		/*
+		 * Bail out now if the EP is in SCIFEP_DISCONNECTED state else
+		 * we will keep looping forever.
+		 */
+		if (ep->state == SCIFEP_DISCONNECTED)
+			break;
+		/*
+		 * Return in the Non Blocking case if there is no data
+		 * to read in this iteration.
+		 */
+		if (!(flags & SCIF_RECV_BLOCK))
+			break;
+		curr_recv_len = min(remaining_len, SCIF_ENDPT_QP_SIZE - 1);
+		spin_unlock(&ep->lock);
+		/*
+		 * Wait for a SCIF_CLIENT_SEND message in the blocking case
+		 * or until other side disconnects.
+		 */
+		ret =
+		wait_event_interruptible(ep->recvwq,
+					 SCIFEP_CONNECTED != ep->state ||
+					 scif_rb_count(&qp->inbound_q,
+						       curr_recv_len)
+					 >= curr_recv_len);
+		spin_lock(&ep->lock);
+		if (ret)
+			break;
+	}
+	if (len - remaining_len)
+		ret = len - remaining_len;
+	else if (!ret && ep->state != SCIFEP_CONNECTED)
+		ret = ep->state == SCIFEP_DISCONNECTED ?
+			-ECONNRESET : -ENOTCONN;
+	spin_unlock(&ep->lock);
+	return ret;
+}
+
+/**
+ * scif_user_send() - Send data to connection queue
+ * @epd: The end point returned from scif_open()
+ * @msg: Address to place data
+ * @len: Length to receive
+ * @flags: blocking or non blocking
+ *
+ * This function is called from the driver IOCTL entry point
+ * only and is a wrapper for _scif_send().
+ */
+int scif_user_send(scif_epd_t epd, void __user *msg, int len, int flags)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	int err = 0;
+	int sent_len = 0;
+	char *tmp;
+	int loop_len;
+	int chunk_len = min(len, (1 << (MAX_ORDER + PAGE_SHIFT - 1)));
+
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI send (U): ep %p %s\n", ep, scif_ep_states[ep->state]);
+	if (!len)
+		return 0;
+
+	err = scif_msg_param_check(epd, len, flags);
+	if (err)
+		goto send_err;
+
+	tmp = kmalloc(chunk_len, GFP_KERNEL);
+	if (!tmp) {
+		err = -ENOMEM;
+		goto send_err;
+	}
+	/*
+	 * Grabbing the lock before breaking up the transfer in
+	 * multiple chunks is required to ensure that messages do
+	 * not get fragmented and reordered.
+	 */
+	mutex_lock(&ep->sendlock);
+	while (sent_len != len) {
+		loop_len = len - sent_len;
+		loop_len = min(chunk_len, loop_len);
+		if (copy_from_user(tmp, msg, loop_len)) {
+			err = -EFAULT;
+			goto send_free_err;
+		}
+		err = _scif_send(epd, tmp, loop_len, flags);
+		if (err < 0)
+			goto send_free_err;
+		sent_len += err;
+		msg += err;
+		if (err != loop_len)
+			goto send_free_err;
+	}
+send_free_err:
+	mutex_unlock(&ep->sendlock);
+	kfree(tmp);
+send_err:
+	return err < 0 ? err : sent_len;
+}
+
+/**
+ * scif_user_recv() - Receive data from connection queue
+ * @epd: The end point returned from scif_open()
+ * @msg: Address to place data
+ * @len: Length to receive
+ * @flags: blocking or non blocking
+ *
+ * This function is called from the driver IOCTL entry point
+ * only and is a wrapper for _scif_recv().
+ */
+int scif_user_recv(scif_epd_t epd, void __user *msg, int len, int flags)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	int err = 0;
+	int recv_len = 0;
+	char *tmp;
+	int loop_len;
+	int chunk_len = min(len, (1 << (MAX_ORDER + PAGE_SHIFT - 1)));
+
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI recv (U): ep %p %s\n", ep, scif_ep_states[ep->state]);
+	if (!len)
+		return 0;
+
+	err = scif_msg_param_check(epd, len, flags);
+	if (err)
+		goto recv_err;
+
+	tmp = kmalloc(chunk_len, GFP_KERNEL);
+	if (!tmp) {
+		err = -ENOMEM;
+		goto recv_err;
+	}
+	/*
+	 * Grabbing the lock before breaking up the transfer in
+	 * multiple chunks is required to ensure that messages do
+	 * not get fragmented and reordered.
+	 */
+	mutex_lock(&ep->recvlock);
+	while (recv_len != len) {
+		loop_len = len - recv_len;
+		loop_len = min(chunk_len, loop_len);
+		err = _scif_recv(epd, tmp, loop_len, flags);
+		if (err < 0)
+			goto recv_free_err;
+		if (copy_to_user(msg, tmp, err)) {
+			err = -EFAULT;
+			goto recv_free_err;
+		}
+		recv_len += err;
+		msg += err;
+		if (err != loop_len)
+			goto recv_free_err;
+	}
+recv_free_err:
+	mutex_unlock(&ep->recvlock);
+	kfree(tmp);
+recv_err:
+	return err < 0 ? err : recv_len;
+}
+
+/**
+ * scif_send() - Send data to connection queue
+ * @epd: The end point returned from scif_open()
+ * @msg: Address to place data
+ * @len: Length to receive
+ * @flags: blocking or non blocking
+ *
+ * This function is called from the kernel mode only and is
+ * a wrapper for _scif_send().
+ */
+int scif_send(scif_epd_t epd, void *msg, int len, int flags)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	int ret;
+
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI send (K): ep %p %s\n", ep, scif_ep_states[ep->state]);
+	if (!len)
+		return 0;
+
+	ret = scif_msg_param_check(epd, len, flags);
+	if (ret)
+		return ret;
+	if (!ep->remote_dev)
+		return -ENOTCONN;
+	/*
+	 * Grab the mutex lock in the blocking case only
+	 * to ensure messages do not get fragmented/reordered.
+	 * The non blocking mode is protected using spin locks
+	 * in _scif_send().
+	 */
+	if (flags & SCIF_SEND_BLOCK)
+		mutex_lock(&ep->sendlock);
+
+	ret = _scif_send(epd, msg, len, flags);
+
+	if (flags & SCIF_SEND_BLOCK)
+		mutex_unlock(&ep->sendlock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(scif_send);
+
+/**
+ * scif_recv() - Receive data from connection queue
+ * @epd: The end point returned from scif_open()
+ * @msg: Address to place data
+ * @len: Length to receive
+ * @flags: blocking or non blocking
+ *
+ * This function is called from the kernel mode only and is
+ * a wrapper for _scif_recv().
+ */
+int scif_recv(scif_epd_t epd, void *msg, int len, int flags)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)epd;
+	int ret;
+
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI recv (K): ep %p %s\n", ep, scif_ep_states[ep->state]);
+	if (!len)
+		return 0;
+
+	ret = scif_msg_param_check(epd, len, flags);
+	if (ret)
+		return ret;
+	/*
+	 * Grab the mutex lock in the blocking case only
+	 * to ensure messages do not get fragmented/reordered.
+	 * The non blocking mode is protected using spin locks
+	 * in _scif_send().
+	 */
+	if (flags & SCIF_RECV_BLOCK)
+		mutex_lock(&ep->recvlock);
+
+	ret = _scif_recv(epd, msg, len, flags);
+
+	if (flags & SCIF_RECV_BLOCK)
+		mutex_unlock(&ep->recvlock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(scif_recv);
+
+int scif_get_node_ids(u16 *nodes, int len, u16 *self)
+{
+	int online = 0;
+	int offset = 0;
+	int node;
+
+	if (!scif_is_mgmt_node())
+		scif_get_node_info();
+
+	*self = scif_info.nodeid;
+	mutex_lock(&scif_info.conflock);
+	len = min_t(int, len, scif_info.total);
+	for (node = 0; node <= scif_info.maxid; node++) {
+		if (_scifdev_alive(&scif_dev[node])) {
+			online++;
+			if (offset < len)
+				nodes[offset++] = node;
+		}
+	}
+	dev_dbg(scif_info.mdev.this_device,
+		"SCIFAPI get_node_ids total %d online %d filled in %d nodes\n",
+		scif_info.total, online, offset);
+	mutex_unlock(&scif_info.conflock);
+
+	return online;
+}
+EXPORT_SYMBOL_GPL(scif_get_node_ids);
diff --git a/drivers/misc/mic/scif/scif_debugfs.c b/drivers/misc/mic/scif/scif_debugfs.c
new file mode 100644
index 000000000000..51f14e2a1196
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_debugfs.c
@@ -0,0 +1,85 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+#include "../common/mic_dev.h"
+#include "scif_main.h"
+
+/* Debugfs parent dir */
+static struct dentry *scif_dbg;
+
+static int scif_dev_test(struct seq_file *s, void *unused)
+{
+	int node;
+
+	seq_printf(s, "Total Nodes %d Self Node Id %d Maxid %d\n",
+		   scif_info.total, scif_info.nodeid,
+		   scif_info.maxid);
+
+	if (!scif_dev)
+		return 0;
+
+	seq_printf(s, "%-16s\t%-16s\n", "node_id", "state");
+
+	for (node = 0; node <= scif_info.maxid; node++)
+		seq_printf(s, "%-16d\t%-16s\n", scif_dev[node].node,
+			   _scifdev_alive(&scif_dev[node]) ?
+			   "Running" : "Offline");
+	return 0;
+}
+
+static int scif_dev_test_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, scif_dev_test, inode->i_private);
+}
+
+static int scif_dev_test_release(struct inode *inode, struct file *file)
+{
+	return single_release(inode, file);
+}
+
+static const struct file_operations scif_dev_ops = {
+	.owner   = THIS_MODULE,
+	.open    = scif_dev_test_open,
+	.read    = seq_read,
+	.llseek  = seq_lseek,
+	.release = scif_dev_test_release
+};
+
+void __init scif_init_debugfs(void)
+{
+	struct dentry *d;
+
+	scif_dbg = debugfs_create_dir(KBUILD_MODNAME, NULL);
+	if (!scif_dbg) {
+		dev_err(scif_info.mdev.this_device,
+			"can't create debugfs dir scif\n");
+		return;
+	}
+
+	d = debugfs_create_file("scif_dev", 0444, scif_dbg,
+				NULL, &scif_dev_ops);
+	debugfs_create_u8("en_msg_log", 0666, scif_dbg, &scif_info.en_msg_log);
+	debugfs_create_u8("p2p_enable", 0666, scif_dbg, &scif_info.p2p_enable);
+}
+
+void scif_exit_debugfs(void)
+{
+	debugfs_remove_recursive(scif_dbg);
+}
diff --git a/drivers/misc/mic/scif/scif_epd.c b/drivers/misc/mic/scif/scif_epd.c
new file mode 100644
index 000000000000..b4bfbb08a8e3
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_epd.c
@@ -0,0 +1,353 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include "scif_main.h"
+#include "scif_map.h"
+
+void scif_cleanup_ep_qp(struct scif_endpt *ep)
+{
+	struct scif_qp *qp = ep->qp_info.qp;
+
+	if (qp->outbound_q.rb_base) {
+		scif_iounmap((void *)qp->outbound_q.rb_base,
+			     qp->outbound_q.size, ep->remote_dev);
+		qp->outbound_q.rb_base = NULL;
+	}
+	if (qp->remote_qp) {
+		scif_iounmap((void *)qp->remote_qp,
+			     sizeof(struct scif_qp), ep->remote_dev);
+		qp->remote_qp = NULL;
+	}
+	if (qp->local_qp) {
+		scif_unmap_single(qp->local_qp, ep->remote_dev,
+				  sizeof(struct scif_qp));
+		qp->local_qp = 0x0;
+	}
+	if (qp->local_buf) {
+		scif_unmap_single(qp->local_buf, ep->remote_dev,
+				  SCIF_ENDPT_QP_SIZE);
+		qp->local_buf = 0;
+	}
+}
+
+void scif_teardown_ep(void *endpt)
+{
+	struct scif_endpt *ep = endpt;
+	struct scif_qp *qp = ep->qp_info.qp;
+
+	if (qp) {
+		spin_lock(&ep->lock);
+		scif_cleanup_ep_qp(ep);
+		spin_unlock(&ep->lock);
+		kfree(qp->inbound_q.rb_base);
+		kfree(qp);
+	}
+}
+
+/*
+ * Enqueue the endpoint to the zombie list for cleanup.
+ * The endpoint should not be accessed once this API returns.
+ */
+void scif_add_epd_to_zombie_list(struct scif_endpt *ep, bool eplock_held)
+{
+	if (!eplock_held)
+		spin_lock(&scif_info.eplock);
+	spin_lock(&ep->lock);
+	ep->state = SCIFEP_ZOMBIE;
+	spin_unlock(&ep->lock);
+	list_add_tail(&ep->list, &scif_info.zombie);
+	scif_info.nr_zombies++;
+	if (!eplock_held)
+		spin_unlock(&scif_info.eplock);
+	schedule_work(&scif_info.misc_work);
+}
+
+static struct scif_endpt *scif_find_listen_ep(u16 port)
+{
+	struct scif_endpt *ep = NULL;
+	struct list_head *pos, *tmpq;
+
+	spin_lock(&scif_info.eplock);
+	list_for_each_safe(pos, tmpq, &scif_info.listen) {
+		ep = list_entry(pos, struct scif_endpt, list);
+		if (ep->port.port == port) {
+			spin_lock(&ep->lock);
+			spin_unlock(&scif_info.eplock);
+			return ep;
+		}
+	}
+	spin_unlock(&scif_info.eplock);
+	return NULL;
+}
+
+void scif_cleanup_zombie_epd(void)
+{
+	struct list_head *pos, *tmpq;
+	struct scif_endpt *ep;
+
+	spin_lock(&scif_info.eplock);
+	list_for_each_safe(pos, tmpq, &scif_info.zombie) {
+		ep = list_entry(pos, struct scif_endpt, list);
+		list_del(pos);
+		scif_info.nr_zombies--;
+		kfree(ep);
+	}
+	spin_unlock(&scif_info.eplock);
+}
+
+/**
+ * scif_cnctreq() - Respond to SCIF_CNCT_REQ interrupt message
+ * @msg:        Interrupt message
+ *
+ * This message is initiated by the remote node to request a connection
+ * to the local node.  This function looks for an end point in the
+ * listen state on the requested port id.
+ *
+ * If it finds a listening port it places the connect request on the
+ * listening end points queue and wakes up any pending accept calls.
+ *
+ * If it does not find a listening end point it sends a connection
+ * reject message to the remote node.
+ */
+void scif_cnctreq(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = NULL;
+	struct scif_conreq *conreq;
+
+	conreq = kmalloc(sizeof(*conreq), GFP_KERNEL);
+	if (!conreq)
+		/* Lack of resources so reject the request. */
+		goto conreq_sendrej;
+
+	ep = scif_find_listen_ep(msg->dst.port);
+	if (!ep)
+		/*  Send reject due to no listening ports */
+		goto conreq_sendrej_free;
+
+	if (ep->backlog <= ep->conreqcnt) {
+		/*  Send reject due to too many pending requests */
+		spin_unlock(&ep->lock);
+		goto conreq_sendrej_free;
+	}
+
+	conreq->msg = *msg;
+	list_add_tail(&conreq->list, &ep->conlist);
+	ep->conreqcnt++;
+	wake_up_interruptible(&ep->conwq);
+	spin_unlock(&ep->lock);
+	return;
+
+conreq_sendrej_free:
+	kfree(conreq);
+conreq_sendrej:
+	msg->uop = SCIF_CNCT_REJ;
+	scif_nodeqp_send(&scif_dev[msg->src.node], msg);
+}
+
+/**
+ * scif_cnctgnt() - Respond to SCIF_CNCT_GNT interrupt message
+ * @msg:        Interrupt message
+ *
+ * An accept() on the remote node has occurred and sent this message
+ * to indicate success.  Place the end point in the MAPPING state and
+ * save the remote nodes memory information.  Then wake up the connect
+ * request so it can finish.
+ */
+void scif_cnctgnt(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)msg->payload[0];
+
+	spin_lock(&ep->lock);
+	if (SCIFEP_CONNECTING == ep->state) {
+		ep->peer.node = msg->src.node;
+		ep->peer.port = msg->src.port;
+		ep->qp_info.gnt_pld = msg->payload[1];
+		ep->remote_ep = msg->payload[2];
+		ep->state = SCIFEP_MAPPING;
+
+		wake_up(&ep->conwq);
+	}
+	spin_unlock(&ep->lock);
+}
+
+/**
+ * scif_cnctgnt_ack() - Respond to SCIF_CNCT_GNTACK interrupt message
+ * @msg:        Interrupt message
+ *
+ * The remote connection request has finished mapping the local memory.
+ * Place the connection in the connected state and wake up the pending
+ * accept() call.
+ */
+void scif_cnctgnt_ack(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)msg->payload[0];
+
+	mutex_lock(&scif_info.connlock);
+	spin_lock(&ep->lock);
+	/* New ep is now connected with all resources set. */
+	ep->state = SCIFEP_CONNECTED;
+	list_add_tail(&ep->list, &scif_info.connected);
+	wake_up(&ep->conwq);
+	spin_unlock(&ep->lock);
+	mutex_unlock(&scif_info.connlock);
+}
+
+/**
+ * scif_cnctgnt_nack() - Respond to SCIF_CNCT_GNTNACK interrupt message
+ * @msg:        Interrupt message
+ *
+ * The remote connection request failed to map the local memory it was sent.
+ * Place the end point in the CLOSING state to indicate it and wake up
+ * the pending accept();
+ */
+void scif_cnctgnt_nack(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)msg->payload[0];
+
+	spin_lock(&ep->lock);
+	ep->state = SCIFEP_CLOSING;
+	wake_up(&ep->conwq);
+	spin_unlock(&ep->lock);
+}
+
+/**
+ * scif_cnctrej() - Respond to SCIF_CNCT_REJ interrupt message
+ * @msg:        Interrupt message
+ *
+ * The remote end has rejected the connection request.  Set the end
+ * point back to the bound state and wake up the pending connect().
+ */
+void scif_cnctrej(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)msg->payload[0];
+
+	spin_lock(&ep->lock);
+	if (SCIFEP_CONNECTING == ep->state) {
+		ep->state = SCIFEP_BOUND;
+		wake_up(&ep->conwq);
+	}
+	spin_unlock(&ep->lock);
+}
+
+/**
+ * scif_discnct() - Respond to SCIF_DISCNCT interrupt message
+ * @msg:        Interrupt message
+ *
+ * The remote node has indicated close() has been called on its end
+ * point.  Remove the local end point from the connected list, set its
+ * state to disconnected and ensure accesses to the remote node are
+ * shutdown.
+ *
+ * When all accesses to the remote end have completed then send a
+ * DISCNT_ACK to indicate it can remove its resources and complete
+ * the close routine.
+ */
+void scif_discnct(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = NULL;
+	struct scif_endpt *tmpep;
+	struct list_head *pos, *tmpq;
+
+	mutex_lock(&scif_info.connlock);
+	list_for_each_safe(pos, tmpq, &scif_info.connected) {
+		tmpep = list_entry(pos, struct scif_endpt, list);
+		/*
+		 * The local ep may have sent a disconnect and and been closed
+		 * due to a message response time out. It may have been
+		 * allocated again and formed a new connection so we want to
+		 * check if the remote ep matches
+		 */
+		if (((u64)tmpep == msg->payload[1]) &&
+		    ((u64)tmpep->remote_ep == msg->payload[0])) {
+			list_del(pos);
+			ep = tmpep;
+			spin_lock(&ep->lock);
+			break;
+		}
+	}
+
+	/*
+	 * If the terminated end is not found then this side started closing
+	 * before the other side sent the disconnect.  If so the ep will no
+	 * longer be on the connected list.  Regardless the other side
+	 * needs to be acked to let it know close is complete.
+	 */
+	if (!ep) {
+		mutex_unlock(&scif_info.connlock);
+		goto discnct_ack;
+	}
+
+	ep->state = SCIFEP_DISCONNECTED;
+	list_add_tail(&ep->list, &scif_info.disconnected);
+
+	wake_up_interruptible(&ep->sendwq);
+	wake_up_interruptible(&ep->recvwq);
+	spin_unlock(&ep->lock);
+	mutex_unlock(&scif_info.connlock);
+
+discnct_ack:
+	msg->uop = SCIF_DISCNT_ACK;
+	scif_nodeqp_send(&scif_dev[msg->src.node], msg);
+}
+
+/**
+ * scif_discnct_ack() - Respond to SCIF_DISCNT_ACK interrupt message
+ * @msg:        Interrupt message
+ *
+ * Remote side has indicated it has not more references to local resources
+ */
+void scif_discnt_ack(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)msg->payload[0];
+
+	spin_lock(&ep->lock);
+	ep->state = SCIFEP_DISCONNECTED;
+	spin_unlock(&ep->lock);
+	complete(&ep->discon);
+}
+
+/**
+ * scif_clientsend() - Respond to SCIF_CLIENT_SEND interrupt message
+ * @msg:        Interrupt message
+ *
+ * Remote side is confirming send or receive interrupt handling is complete.
+ */
+void scif_clientsend(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)msg->payload[0];
+
+	spin_lock(&ep->lock);
+	if (SCIFEP_CONNECTED == ep->state)
+		wake_up_interruptible(&ep->recvwq);
+	spin_unlock(&ep->lock);
+}
+
+/**
+ * scif_clientrcvd() - Respond to SCIF_CLIENT_RCVD interrupt message
+ * @msg:        Interrupt message
+ *
+ * Remote side is confirming send or receive interrupt handling is complete.
+ */
+void scif_clientrcvd(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_endpt *ep = (struct scif_endpt *)msg->payload[0];
+
+	spin_lock(&ep->lock);
+	if (SCIFEP_CONNECTED == ep->state)
+		wake_up_interruptible(&ep->sendwq);
+	spin_unlock(&ep->lock);
+}
diff --git a/drivers/misc/mic/scif/scif_epd.h b/drivers/misc/mic/scif/scif_epd.h
new file mode 100644
index 000000000000..331322a25213
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_epd.h
@@ -0,0 +1,160 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#ifndef SCIF_EPD_H
+#define SCIF_EPD_H
+
+#include <linux/delay.h>
+#include <linux/scif.h>
+#include <linux/scif_ioctl.h>
+
+#define SCIF_EPLOCK_HELD true
+
+enum scif_epd_state {
+	SCIFEP_UNBOUND,
+	SCIFEP_BOUND,
+	SCIFEP_LISTENING,
+	SCIFEP_CONNECTED,
+	SCIFEP_CONNECTING,
+	SCIFEP_MAPPING,
+	SCIFEP_CLOSING,
+	SCIFEP_CLLISTEN,
+	SCIFEP_DISCONNECTED,
+	SCIFEP_ZOMBIE
+};
+
+/*
+ * struct scif_conreq - Data structure added to the connection list.
+ *
+ * @msg: connection request message received
+ * @list: link to list of connection requests
+ */
+struct scif_conreq {
+	struct scifmsg msg;
+	struct list_head list;
+};
+
+/* Size of the RB for the Endpoint QP */
+#define SCIF_ENDPT_QP_SIZE 0x1000
+
+/*
+ * scif_endpt_qp_info - SCIF endpoint queue pair
+ *
+ * @qp - Qpair for this endpoint
+ * @qp_offset - DMA address of the QP
+ * @gnt_pld - Payload in a SCIF_CNCT_GNT message containing the
+ * physical address of the remote_qp.
+ */
+struct scif_endpt_qp_info {
+	struct scif_qp *qp;
+	dma_addr_t qp_offset;
+	dma_addr_t gnt_pld;
+};
+
+/*
+ * struct scif_endpt - The SCIF endpoint data structure
+ *
+ * @state: end point state
+ * @lock: lock synchronizing access to endpoint fields like state etc
+ * @port: self port information
+ * @peer: peer port information
+ * @backlog: maximum pending connection requests
+ * @qp_info: Endpoint QP information for SCIF messaging
+ * @remote_dev: scifdev used by this endpt to communicate with remote node.
+ * @remote_ep: remote endpoint
+ * @conreqcnt: Keep track of number of connection requests.
+ * @files: Open file information used to match the id passed in with
+ *         the flush routine.
+ * @conlist: list of connection requests
+ * @conwq: waitqueue for connection processing
+ * @discon: completion used during disconnection
+ * @sendwq: waitqueue used during sending messages
+ * @recvwq: waitqueue used during message receipt
+ * @sendlock: Synchronize ordering of messages sent
+ * @recvlock: Synchronize ordering of messages received
+ * @list: link to list of various endpoints like connected, listening etc
+ * @li_accept: pending ACCEPTREG
+ * @acceptcnt: pending ACCEPTREG cnt
+ * @liacceptlist: link to listen accept
+ * @miacceptlist: link to uaccept
+ * @listenep: associated listen ep
+ * @conn_work: Non blocking connect work
+ * @conn_port: Connection port
+ * @conn_err: Errors during connection
+ * @conn_async_state: Async connection
+ * @conn_list: List of async connection requests
+ */
+struct scif_endpt {
+	enum scif_epd_state state;
+	spinlock_t lock;
+	struct scif_port_id port;
+	struct scif_port_id peer;
+	int backlog;
+	struct scif_endpt_qp_info qp_info;
+	struct scif_dev *remote_dev;
+	u64 remote_ep;
+	int conreqcnt;
+	struct files_struct *files;
+	struct list_head conlist;
+	wait_queue_head_t conwq;
+	struct completion discon;
+	wait_queue_head_t sendwq;
+	wait_queue_head_t recvwq;
+	struct mutex sendlock;
+	struct mutex recvlock;
+	struct list_head list;
+	struct list_head li_accept;
+	int acceptcnt;
+	struct list_head liacceptlist;
+	struct list_head miacceptlist;
+	struct scif_endpt *listenep;
+	struct scif_port_id conn_port;
+	int conn_err;
+	int conn_async_state;
+	struct list_head conn_list;
+};
+
+static inline int scifdev_alive(struct scif_endpt *ep)
+{
+	return _scifdev_alive(ep->remote_dev);
+}
+
+void scif_cleanup_zombie_epd(void);
+void scif_teardown_ep(void *endpt);
+void scif_cleanup_ep_qp(struct scif_endpt *ep);
+void scif_add_epd_to_zombie_list(struct scif_endpt *ep, bool eplock_held);
+void scif_get_node_info(void);
+void scif_send_acks(struct scif_dev *dev);
+void scif_conn_handler(struct work_struct *work);
+int scif_rsrv_port(u16 port);
+void scif_get_port(u16 port);
+int scif_get_new_port(void);
+void scif_put_port(u16 port);
+int scif_user_send(scif_epd_t epd, void __user *msg, int len, int flags);
+int scif_user_recv(scif_epd_t epd, void __user *msg, int len, int flags);
+void scif_cnctreq(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_cnctgnt(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_cnctgnt_ack(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_cnctgnt_nack(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_cnctrej(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_discnct(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_discnt_ack(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_clientsend(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_clientrcvd(struct scif_dev *scifdev, struct scifmsg *msg);
+int __scif_connect(scif_epd_t epd, struct scif_port_id *dst, bool non_block);
+int __scif_flush(scif_epd_t epd);
+#endif /* SCIF_EPD_H */
diff --git a/drivers/misc/mic/scif/scif_fd.c b/drivers/misc/mic/scif/scif_fd.c
new file mode 100644
index 000000000000..eccf7e7135f9
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_fd.c
@@ -0,0 +1,303 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include "scif_main.h"
+
+static int scif_fdopen(struct inode *inode, struct file *f)
+{
+	struct scif_endpt *priv = scif_open();
+
+	if (!priv)
+		return -ENOMEM;
+	f->private_data = priv;
+	return 0;
+}
+
+static int scif_fdclose(struct inode *inode, struct file *f)
+{
+	struct scif_endpt *priv = f->private_data;
+
+	return scif_close(priv);
+}
+
+static int scif_fdflush(struct file *f, fl_owner_t id)
+{
+	struct scif_endpt *ep = f->private_data;
+
+	spin_lock(&ep->lock);
+	/*
+	 * The listening endpoint stashes the open file information before
+	 * waiting for incoming connections. The release callback would never be
+	 * called if the application closed the endpoint, while waiting for
+	 * incoming connections from a separate thread since the file descriptor
+	 * reference count is bumped up in the accept IOCTL. Call the flush
+	 * routine if the id matches the endpoint open file information so that
+	 * the listening endpoint can be woken up and the fd released.
+	 */
+	if (ep->files == id)
+		__scif_flush(ep);
+	spin_unlock(&ep->lock);
+	return 0;
+}
+
+static __always_inline void scif_err_debug(int err, const char *str)
+{
+	/*
+	 * ENOTCONN is a common uninteresting error which is
+	 * flooding debug messages to the console unnecessarily.
+	 */
+	if (err < 0 && err != -ENOTCONN)
+		dev_dbg(scif_info.mdev.this_device, "%s err %d\n", str, err);
+}
+
+static long scif_fdioctl(struct file *f, unsigned int cmd, unsigned long arg)
+{
+	struct scif_endpt *priv = f->private_data;
+	void __user *argp = (void __user *)arg;
+	int err = 0;
+	struct scifioctl_msg request;
+	bool non_block = false;
+
+	non_block = !!(f->f_flags & O_NONBLOCK);
+
+	switch (cmd) {
+	case SCIF_BIND:
+	{
+		int pn;
+
+		if (copy_from_user(&pn, argp, sizeof(pn)))
+			return -EFAULT;
+
+		pn = scif_bind(priv, pn);
+		if (pn < 0)
+			return pn;
+
+		if (copy_to_user(argp, &pn, sizeof(pn)))
+			return -EFAULT;
+
+		return 0;
+	}
+	case SCIF_LISTEN:
+		return scif_listen(priv, arg);
+	case SCIF_CONNECT:
+	{
+		struct scifioctl_connect req;
+		struct scif_endpt *ep = (struct scif_endpt *)priv;
+
+		if (copy_from_user(&req, argp, sizeof(req)))
+			return -EFAULT;
+
+		err = __scif_connect(priv, &req.peer, non_block);
+		if (err < 0)
+			return err;
+
+		req.self.node = ep->port.node;
+		req.self.port = ep->port.port;
+
+		if (copy_to_user(argp, &req, sizeof(req)))
+			return -EFAULT;
+
+		return 0;
+	}
+	/*
+	 * Accept is done in two halves.  The request ioctl does the basic
+	 * functionality of accepting the request and returning the information
+	 * about it including the internal ID of the end point.  The register
+	 * is done with the internal ID on a new file descriptor opened by the
+	 * requesting process.
+	 */
+	case SCIF_ACCEPTREQ:
+	{
+		struct scifioctl_accept request;
+		scif_epd_t *ep = (scif_epd_t *)&request.endpt;
+
+		if (copy_from_user(&request, argp, sizeof(request)))
+			return -EFAULT;
+
+		err = scif_accept(priv, &request.peer, ep, request.flags);
+		if (err < 0)
+			return err;
+
+		if (copy_to_user(argp, &request, sizeof(request))) {
+			scif_close(*ep);
+			return -EFAULT;
+		}
+		/*
+		 * Add to the list of user mode eps where the second half
+		 * of the accept is not yet completed.
+		 */
+		spin_lock(&scif_info.eplock);
+		list_add_tail(&((*ep)->miacceptlist), &scif_info.uaccept);
+		list_add_tail(&((*ep)->liacceptlist), &priv->li_accept);
+		(*ep)->listenep = priv;
+		priv->acceptcnt++;
+		spin_unlock(&scif_info.eplock);
+
+		return 0;
+	}
+	case SCIF_ACCEPTREG:
+	{
+		struct scif_endpt *priv = f->private_data;
+		struct scif_endpt *newep;
+		struct scif_endpt *lisep;
+		struct scif_endpt *fep = NULL;
+		struct scif_endpt *tmpep;
+		struct list_head *pos, *tmpq;
+
+		/* Finally replace the pointer to the accepted endpoint */
+		if (copy_from_user(&newep, argp, sizeof(void *)))
+			return -EFAULT;
+
+		/* Remove form the user accept queue */
+		spin_lock(&scif_info.eplock);
+		list_for_each_safe(pos, tmpq, &scif_info.uaccept) {
+			tmpep = list_entry(pos,
+					   struct scif_endpt, miacceptlist);
+			if (tmpep == newep) {
+				list_del(pos);
+				fep = tmpep;
+				break;
+			}
+		}
+
+		if (!fep) {
+			spin_unlock(&scif_info.eplock);
+			return -ENOENT;
+		}
+
+		lisep = newep->listenep;
+		list_for_each_safe(pos, tmpq, &lisep->li_accept) {
+			tmpep = list_entry(pos,
+					   struct scif_endpt, liacceptlist);
+			if (tmpep == newep) {
+				list_del(pos);
+				lisep->acceptcnt--;
+				break;
+			}
+		}
+
+		spin_unlock(&scif_info.eplock);
+
+		/* Free the resources automatically created from the open. */
+		scif_teardown_ep(priv);
+		scif_add_epd_to_zombie_list(priv, !SCIF_EPLOCK_HELD);
+		f->private_data = newep;
+		return 0;
+	}
+	case SCIF_SEND:
+	{
+		struct scif_endpt *priv = f->private_data;
+
+		if (copy_from_user(&request, argp,
+				   sizeof(struct scifioctl_msg))) {
+			err = -EFAULT;
+			goto send_err;
+		}
+		err = scif_user_send(priv, (void __user *)request.msg,
+				     request.len, request.flags);
+		if (err < 0)
+			goto send_err;
+		if (copy_to_user(&
+				 ((struct scifioctl_msg __user *)argp)->out_len,
+				 &err, sizeof(err))) {
+			err = -EFAULT;
+			goto send_err;
+		}
+		err = 0;
+send_err:
+		scif_err_debug(err, "scif_send");
+		return err;
+	}
+	case SCIF_RECV:
+	{
+		struct scif_endpt *priv = f->private_data;
+
+		if (copy_from_user(&request, argp,
+				   sizeof(struct scifioctl_msg))) {
+			err = -EFAULT;
+			goto recv_err;
+		}
+
+		err = scif_user_recv(priv, (void __user *)request.msg,
+				     request.len, request.flags);
+		if (err < 0)
+			goto recv_err;
+
+		if (copy_to_user(&
+				 ((struct scifioctl_msg __user *)argp)->out_len,
+			&err, sizeof(err))) {
+			err = -EFAULT;
+			goto recv_err;
+		}
+		err = 0;
+recv_err:
+		scif_err_debug(err, "scif_recv");
+		return err;
+	}
+	case SCIF_GET_NODEIDS:
+	{
+		struct scifioctl_node_ids node_ids;
+		int entries;
+		u16 *nodes;
+		void __user *unodes, *uself;
+		u16 self;
+
+		if (copy_from_user(&node_ids, argp, sizeof(node_ids))) {
+			err = -EFAULT;
+			goto getnodes_err2;
+		}
+
+		entries = min_t(int, scif_info.maxid, node_ids.len);
+		nodes = kmalloc_array(entries, sizeof(u16), GFP_KERNEL);
+		if (entries && !nodes) {
+			err = -ENOMEM;
+			goto getnodes_err2;
+		}
+		node_ids.len = scif_get_node_ids(nodes, entries, &self);
+
+		unodes = (void __user *)node_ids.nodes;
+		if (copy_to_user(unodes, nodes, sizeof(u16) * entries)) {
+			err = -EFAULT;
+			goto getnodes_err1;
+		}
+
+		uself = (void __user *)node_ids.self;
+		if (copy_to_user(uself, &self, sizeof(u16))) {
+			err = -EFAULT;
+			goto getnodes_err1;
+		}
+
+		if (copy_to_user(argp, &node_ids, sizeof(node_ids))) {
+			err = -EFAULT;
+			goto getnodes_err1;
+		}
+getnodes_err1:
+		kfree(nodes);
+getnodes_err2:
+		return err;
+	}
+	}
+	return -EINVAL;
+}
+
+const struct file_operations scif_fops = {
+	.open = scif_fdopen,
+	.release = scif_fdclose,
+	.unlocked_ioctl = scif_fdioctl,
+	.flush = scif_fdflush,
+	.owner = THIS_MODULE,
+};
diff --git a/drivers/misc/mic/scif/scif_main.c b/drivers/misc/mic/scif/scif_main.c
new file mode 100644
index 000000000000..6ce851f5c7e6
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_main.c
@@ -0,0 +1,388 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include <linux/module.h>
+#include <linux/idr.h>
+
+#include <linux/mic_common.h>
+#include "../common/mic_dev.h"
+#include "../bus/scif_bus.h"
+#include "scif_peer_bus.h"
+#include "scif_main.h"
+#include "scif_map.h"
+
+struct scif_info scif_info = {
+	.mdev = {
+		.minor = MISC_DYNAMIC_MINOR,
+		.name = "scif",
+		.fops = &scif_fops,
+	}
+};
+
+struct scif_dev *scif_dev;
+static atomic_t g_loopb_cnt;
+
+/* Runs in the context of intr_wq */
+static void scif_intr_bh_handler(struct work_struct *work)
+{
+	struct scif_dev *scifdev =
+			container_of(work, struct scif_dev, intr_bh);
+
+	if (scifdev_self(scifdev))
+		scif_loopb_msg_handler(scifdev, scifdev->qpairs);
+	else
+		scif_nodeqp_intrhandler(scifdev, scifdev->qpairs);
+}
+
+int scif_setup_intr_wq(struct scif_dev *scifdev)
+{
+	if (!scifdev->intr_wq) {
+		snprintf(scifdev->intr_wqname, sizeof(scifdev->intr_wqname),
+			 "SCIF INTR %d", scifdev->node);
+		scifdev->intr_wq =
+			alloc_ordered_workqueue(scifdev->intr_wqname, 0);
+		if (!scifdev->intr_wq)
+			return -ENOMEM;
+		INIT_WORK(&scifdev->intr_bh, scif_intr_bh_handler);
+	}
+	return 0;
+}
+
+void scif_destroy_intr_wq(struct scif_dev *scifdev)
+{
+	if (scifdev->intr_wq) {
+		destroy_workqueue(scifdev->intr_wq);
+		scifdev->intr_wq = NULL;
+	}
+}
+
+irqreturn_t scif_intr_handler(int irq, void *data)
+{
+	struct scif_dev *scifdev = data;
+	struct scif_hw_dev *sdev = scifdev->sdev;
+
+	sdev->hw_ops->ack_interrupt(sdev, scifdev->db);
+	queue_work(scifdev->intr_wq, &scifdev->intr_bh);
+	return IRQ_HANDLED;
+}
+
+static int scif_peer_probe(struct scif_peer_dev *spdev)
+{
+	struct scif_dev *scifdev = &scif_dev[spdev->dnode];
+
+	mutex_lock(&scif_info.conflock);
+	scif_info.total++;
+	scif_info.maxid = max_t(u32, spdev->dnode, scif_info.maxid);
+	mutex_unlock(&scif_info.conflock);
+	rcu_assign_pointer(scifdev->spdev, spdev);
+
+	/* In the future SCIF kernel client devices will be added here */
+	return 0;
+}
+
+static void scif_peer_remove(struct scif_peer_dev *spdev)
+{
+	struct scif_dev *scifdev = &scif_dev[spdev->dnode];
+
+	/* In the future SCIF kernel client devices will be removed here */
+	spdev = rcu_dereference(scifdev->spdev);
+	if (spdev)
+		RCU_INIT_POINTER(scifdev->spdev, NULL);
+	synchronize_rcu();
+
+	mutex_lock(&scif_info.conflock);
+	scif_info.total--;
+	mutex_unlock(&scif_info.conflock);
+}
+
+static void scif_qp_setup_handler(struct work_struct *work)
+{
+	struct scif_dev *scifdev = container_of(work, struct scif_dev,
+						qp_dwork.work);
+	struct scif_hw_dev *sdev = scifdev->sdev;
+	dma_addr_t da = 0;
+	int err;
+
+	if (scif_is_mgmt_node()) {
+		struct mic_bootparam *bp = sdev->dp;
+
+		da = bp->scif_card_dma_addr;
+		scifdev->rdb = bp->h2c_scif_db;
+	} else {
+		struct mic_bootparam __iomem *bp = sdev->rdp;
+
+		da = readq(&bp->scif_host_dma_addr);
+		scifdev->rdb = ioread8(&bp->c2h_scif_db);
+	}
+	if (da) {
+		err = scif_qp_response(da, scifdev);
+		if (err)
+			dev_err(&scifdev->sdev->dev,
+				"scif_qp_response err %d\n", err);
+	} else {
+		schedule_delayed_work(&scifdev->qp_dwork,
+				      msecs_to_jiffies(1000));
+	}
+}
+
+static int scif_setup_scifdev(struct scif_hw_dev *sdev)
+{
+	int i;
+	u8 num_nodes;
+
+	if (sdev->snode) {
+		struct mic_bootparam __iomem *bp = sdev->rdp;
+
+		num_nodes = ioread8(&bp->tot_nodes);
+	} else {
+		struct mic_bootparam *bp = sdev->dp;
+
+		num_nodes = bp->tot_nodes;
+	}
+	scif_dev = kcalloc(num_nodes, sizeof(*scif_dev), GFP_KERNEL);
+	if (!scif_dev)
+		return -ENOMEM;
+	for (i = 0; i < num_nodes; i++) {
+		struct scif_dev *scifdev = &scif_dev[i];
+
+		scifdev->node = i;
+		scifdev->exit = OP_IDLE;
+		init_waitqueue_head(&scifdev->disconn_wq);
+		mutex_init(&scifdev->lock);
+		INIT_WORK(&scifdev->init_msg_work, scif_qp_response_ack);
+		INIT_DELAYED_WORK(&scifdev->p2p_dwork,
+				  scif_poll_qp_state);
+		INIT_DELAYED_WORK(&scifdev->qp_dwork,
+				  scif_qp_setup_handler);
+		INIT_LIST_HEAD(&scifdev->p2p);
+		RCU_INIT_POINTER(scifdev->spdev, NULL);
+	}
+	return 0;
+}
+
+static void scif_destroy_scifdev(void)
+{
+	kfree(scif_dev);
+}
+
+static int scif_probe(struct scif_hw_dev *sdev)
+{
+	struct scif_dev *scifdev;
+	int rc;
+
+	dev_set_drvdata(&sdev->dev, sdev);
+	if (1 == atomic_add_return(1, &g_loopb_cnt)) {
+		struct scif_dev *loopb_dev;
+
+		rc = scif_setup_scifdev(sdev);
+		if (rc)
+			goto exit;
+		scifdev = &scif_dev[sdev->dnode];
+		scifdev->sdev = sdev;
+		loopb_dev = &scif_dev[sdev->snode];
+		loopb_dev->sdev = sdev;
+		rc = scif_setup_loopback_qp(loopb_dev);
+		if (rc)
+			goto free_sdev;
+	} else {
+		scifdev = &scif_dev[sdev->dnode];
+		scifdev->sdev = sdev;
+	}
+	rc = scif_setup_intr_wq(scifdev);
+	if (rc)
+		goto destroy_loopb;
+	rc = scif_setup_qp(scifdev);
+	if (rc)
+		goto destroy_intr;
+	scifdev->db = sdev->hw_ops->next_db(sdev);
+	scifdev->cookie = sdev->hw_ops->request_irq(sdev, scif_intr_handler,
+						    "SCIF_INTR", scifdev,
+						    scifdev->db);
+	if (IS_ERR(scifdev->cookie)) {
+		rc = PTR_ERR(scifdev->cookie);
+		goto free_qp;
+	}
+	if (scif_is_mgmt_node()) {
+		struct mic_bootparam *bp = sdev->dp;
+
+		bp->c2h_scif_db = scifdev->db;
+		bp->scif_host_dma_addr = scifdev->qp_dma_addr;
+	} else {
+		struct mic_bootparam __iomem *bp = sdev->rdp;
+
+		iowrite8(scifdev->db, &bp->h2c_scif_db);
+		writeq(scifdev->qp_dma_addr, &bp->scif_card_dma_addr);
+	}
+	schedule_delayed_work(&scifdev->qp_dwork,
+			      msecs_to_jiffies(1000));
+	return rc;
+free_qp:
+	scif_free_qp(scifdev);
+destroy_intr:
+	scif_destroy_intr_wq(scifdev);
+destroy_loopb:
+	if (atomic_dec_and_test(&g_loopb_cnt))
+		scif_destroy_loopback_qp(&scif_dev[sdev->snode]);
+free_sdev:
+	scif_destroy_scifdev();
+exit:
+	return rc;
+}
+
+void scif_stop(struct scif_dev *scifdev)
+{
+	struct scif_dev *dev;
+	int i;
+
+	for (i = scif_info.maxid; i >= 0; i--) {
+		dev = &scif_dev[i];
+		if (scifdev_self(dev))
+			continue;
+		scif_handle_remove_node(i);
+	}
+}
+
+static void scif_remove(struct scif_hw_dev *sdev)
+{
+	struct scif_dev *scifdev = &scif_dev[sdev->dnode];
+
+	if (scif_is_mgmt_node()) {
+		struct mic_bootparam *bp = sdev->dp;
+
+		bp->c2h_scif_db = -1;
+		bp->scif_host_dma_addr = 0x0;
+	} else {
+		struct mic_bootparam __iomem *bp = sdev->rdp;
+
+		iowrite8(-1, &bp->h2c_scif_db);
+		writeq(0x0, &bp->scif_card_dma_addr);
+	}
+	if (scif_is_mgmt_node()) {
+		scif_disconnect_node(scifdev->node, true);
+	} else {
+		scif_info.card_initiated_exit = true;
+		scif_stop(scifdev);
+	}
+	if (atomic_dec_and_test(&g_loopb_cnt))
+		scif_destroy_loopback_qp(&scif_dev[sdev->snode]);
+	if (scifdev->cookie) {
+		sdev->hw_ops->free_irq(sdev, scifdev->cookie, scifdev);
+		scifdev->cookie = NULL;
+	}
+	scif_destroy_intr_wq(scifdev);
+	cancel_delayed_work(&scifdev->qp_dwork);
+	scif_free_qp(scifdev);
+	scifdev->rdb = -1;
+	scifdev->sdev = NULL;
+}
+
+static struct scif_peer_driver scif_peer_driver = {
+	.driver.name =	KBUILD_MODNAME,
+	.driver.owner =	THIS_MODULE,
+	.probe = scif_peer_probe,
+	.remove = scif_peer_remove,
+};
+
+static struct scif_hw_dev_id id_table[] = {
+	{ MIC_SCIF_DEV, SCIF_DEV_ANY_ID },
+	{ 0 },
+};
+
+static struct scif_driver scif_driver = {
+	.driver.name =	KBUILD_MODNAME,
+	.driver.owner =	THIS_MODULE,
+	.id_table = id_table,
+	.probe = scif_probe,
+	.remove = scif_remove,
+};
+
+static int _scif_init(void)
+{
+	spin_lock_init(&scif_info.eplock);
+	spin_lock_init(&scif_info.nb_connect_lock);
+	spin_lock_init(&scif_info.port_lock);
+	mutex_init(&scif_info.conflock);
+	mutex_init(&scif_info.connlock);
+	INIT_LIST_HEAD(&scif_info.uaccept);
+	INIT_LIST_HEAD(&scif_info.listen);
+	INIT_LIST_HEAD(&scif_info.zombie);
+	INIT_LIST_HEAD(&scif_info.connected);
+	INIT_LIST_HEAD(&scif_info.disconnected);
+	INIT_LIST_HEAD(&scif_info.nb_connect_list);
+	init_waitqueue_head(&scif_info.exitwq);
+	scif_info.en_msg_log = 0;
+	scif_info.p2p_enable = 1;
+	INIT_WORK(&scif_info.misc_work, scif_misc_handler);
+	INIT_WORK(&scif_info.conn_work, scif_conn_handler);
+	idr_init(&scif_ports);
+	return 0;
+}
+
+static void _scif_exit(void)
+{
+	idr_destroy(&scif_ports);
+	scif_destroy_scifdev();
+}
+
+static int __init scif_init(void)
+{
+	struct miscdevice *mdev = &scif_info.mdev;
+	int rc;
+
+	_scif_init();
+	rc = scif_peer_bus_init();
+	if (rc)
+		goto exit;
+	rc = scif_peer_register_driver(&scif_peer_driver);
+	if (rc)
+		goto peer_bus_exit;
+	rc = scif_register_driver(&scif_driver);
+	if (rc)
+		goto unreg_scif_peer;
+	rc = misc_register(mdev);
+	if (rc)
+		goto unreg_scif;
+	scif_init_debugfs();
+	return 0;
+unreg_scif:
+	scif_unregister_driver(&scif_driver);
+unreg_scif_peer:
+	scif_peer_unregister_driver(&scif_peer_driver);
+peer_bus_exit:
+	scif_peer_bus_exit();
+exit:
+	_scif_exit();
+	return rc;
+}
+
+static void __exit scif_exit(void)
+{
+	scif_exit_debugfs();
+	misc_deregister(&scif_info.mdev);
+	scif_unregister_driver(&scif_driver);
+	scif_peer_unregister_driver(&scif_peer_driver);
+	scif_peer_bus_exit();
+	_scif_exit();
+}
+
+module_init(scif_init);
+module_exit(scif_exit);
+
+MODULE_DEVICE_TABLE(scif, id_table);
+MODULE_AUTHOR("Intel Corporation");
+MODULE_DESCRIPTION("Intel(R) SCIF driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/misc/mic/scif/scif_main.h b/drivers/misc/mic/scif/scif_main.h
new file mode 100644
index 000000000000..580bc63e1b23
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_main.h
@@ -0,0 +1,254 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#ifndef SCIF_MAIN_H
+#define SCIF_MAIN_H
+
+#include <linux/sched.h>
+#include <linux/pci.h>
+#include <linux/miscdevice.h>
+#include <linux/dmaengine.h>
+#include <linux/file.h>
+#include <linux/scif.h>
+
+#include "../common/mic_dev.h"
+
+#define SCIF_MGMT_NODE 0
+#define SCIF_DEFAULT_WATCHDOG_TO 30
+#define SCIF_NODE_ACCEPT_TIMEOUT (3 * HZ)
+#define SCIF_NODE_ALIVE_TIMEOUT (SCIF_DEFAULT_WATCHDOG_TO * HZ)
+
+/*
+ * Generic state used for certain node QP message exchanges
+ * like Unregister, Alloc etc.
+ */
+enum scif_msg_state {
+	OP_IDLE = 1,
+	OP_IN_PROGRESS,
+	OP_COMPLETED,
+	OP_FAILED
+};
+
+/*
+ * struct scif_info - Global SCIF information
+ *
+ * @nodeid: Node ID this node is to others
+ * @maxid: Max known node ID
+ * @total: Total number of SCIF nodes
+ * @nr_zombies: number of zombie endpoints
+ * @eplock: Lock to synchronize listening, zombie endpoint lists
+ * @connlock: Lock to synchronize connected and disconnected lists
+ * @nb_connect_lock: Synchronize non blocking connect operations
+ * @port_lock: Synchronize access to SCIF ports
+ * @uaccept: List of user acceptreq waiting for acceptreg
+ * @listen: List of listening end points
+ * @zombie: List of zombie end points with pending RMA's
+ * @connected: List of end points in connected state
+ * @disconnected: List of end points in disconnected state
+ * @nb_connect_list: List for non blocking connections
+ * @misc_work: miscellaneous SCIF tasks
+ * @conflock: Lock to synchronize SCIF node configuration changes
+ * @en_msg_log: Enable debug message logging
+ * @p2p_enable: Enable P2P SCIF network
+ * @mdev: The MISC device
+ * @conn_work: Work for workqueue handling all connections
+ * @exitwq: Wait queue for waiting for an EXIT node QP message response
+ * @loopb_dev: Dummy SCIF device used for loopback
+ * @loopb_wq: Workqueue used for handling loopback messages
+ * @loopb_wqname[16]: Name of loopback workqueue
+ * @loopb_work: Used for submitting work to loopb_wq
+ * @loopb_recv_q: List of messages received on the loopb_wq
+ * @card_initiated_exit: set when the card has initiated the exit
+ */
+struct scif_info {
+	u8 nodeid;
+	u8 maxid;
+	u8 total;
+	u32 nr_zombies;
+	spinlock_t eplock;
+	struct mutex connlock;
+	spinlock_t nb_connect_lock;
+	spinlock_t port_lock;
+	struct list_head uaccept;
+	struct list_head listen;
+	struct list_head zombie;
+	struct list_head connected;
+	struct list_head disconnected;
+	struct list_head nb_connect_list;
+	struct work_struct misc_work;
+	struct mutex conflock;
+	u8 en_msg_log;
+	u8 p2p_enable;
+	struct miscdevice mdev;
+	struct work_struct conn_work;
+	wait_queue_head_t exitwq;
+	struct scif_dev *loopb_dev;
+	struct workqueue_struct *loopb_wq;
+	char loopb_wqname[16];
+	struct work_struct loopb_work;
+	struct list_head loopb_recv_q;
+	bool card_initiated_exit;
+};
+
+/*
+ * struct scif_p2p_info - SCIF mapping information used for P2P
+ *
+ * @ppi_peer_id - SCIF peer node id
+ * @ppi_sg - Scatter list for bar information (One for mmio and one for aper)
+ * @sg_nentries - Number of entries in the scatterlist
+ * @ppi_da: DMA address for MMIO and APER bars
+ * @ppi_len: Length of MMIO and APER bars
+ * @ppi_list: Link in list of mapping information
+ */
+struct scif_p2p_info {
+	u8 ppi_peer_id;
+	struct scatterlist *ppi_sg[2];
+	u64 sg_nentries[2];
+	dma_addr_t ppi_da[2];
+	u64 ppi_len[2];
+#define SCIF_PPI_MMIO 0
+#define SCIF_PPI_APER 1
+	struct list_head ppi_list;
+};
+
+/*
+ * struct scif_dev - SCIF remote device specific fields
+ *
+ * @node: Node id
+ * @p2p: List of P2P mapping information
+ * @qpairs: The node queue pair for exchanging control messages
+ * @intr_wq: Workqueue for handling Node QP messages
+ * @intr_wqname: Name of node QP workqueue for handling interrupts
+ * @intr_bh: Used for submitting work to intr_wq
+ * @lock: Lock used for synchronizing access to the scif device
+ * @sdev: SCIF hardware device on the SCIF hardware bus
+ * @db: doorbell the peer will trigger to generate an interrupt on self
+ * @rdb: Doorbell to trigger on the peer to generate an interrupt on the peer
+ * @cookie: Cookie received while registering the interrupt handler
+ * init_msg_work: work scheduled for SCIF_INIT message processing
+ * @p2p_dwork: Delayed work to enable polling for P2P state
+ * @qp_dwork: Delayed work for enabling polling for remote QP information
+ * @p2p_retry: Number of times to retry polling of P2P state
+ * @base_addr: P2P aperture bar base address
+ * @mic_mw mmio: The peer MMIO information used for P2P
+ * @spdev: SCIF peer device on the SCIF peer bus
+ * @node_remove_ack_pending: True if a node_remove_ack is pending
+ * @exit_ack_pending: true if an exit_ack is pending
+ * @disconn_wq: Used while waiting for a node remove response
+ * @disconn_rescnt: Keeps track of number of node remove requests sent
+ * @exit: Status of exit message
+ * @qp_dma_addr: Queue pair DMA address passed to the peer
+*/
+struct scif_dev {
+	u8 node;
+	struct list_head p2p;
+	struct scif_qp *qpairs;
+	struct workqueue_struct *intr_wq;
+	char intr_wqname[16];
+	struct work_struct intr_bh;
+	struct mutex lock;
+	struct scif_hw_dev *sdev;
+	int db;
+	int rdb;
+	struct mic_irq *cookie;
+	struct work_struct init_msg_work;
+	struct delayed_work p2p_dwork;
+	struct delayed_work qp_dwork;
+	int p2p_retry;
+	dma_addr_t base_addr;
+	struct mic_mw mmio;
+	struct scif_peer_dev __rcu *spdev;
+	bool node_remove_ack_pending;
+	bool exit_ack_pending;
+	wait_queue_head_t disconn_wq;
+	atomic_t disconn_rescnt;
+	enum scif_msg_state exit;
+	dma_addr_t qp_dma_addr;
+};
+
+extern struct scif_info scif_info;
+extern struct idr scif_ports;
+extern struct scif_dev *scif_dev;
+extern const struct file_operations scif_fops;
+
+/* Size of the RB for the Node QP */
+#define SCIF_NODE_QP_SIZE 0x10000
+
+#include "scif_nodeqp.h"
+
+/*
+ * scifdev_self:
+ * @dev: The remote SCIF Device
+ *
+ * Returns true if the SCIF Device passed is the self aka Loopback SCIF device.
+ */
+static inline int scifdev_self(struct scif_dev *dev)
+{
+	return dev->node == scif_info.nodeid;
+}
+
+static inline bool scif_is_mgmt_node(void)
+{
+	return !scif_info.nodeid;
+}
+
+/*
+ * scifdev_is_p2p:
+ * @dev: The remote SCIF Device
+ *
+ * Returns true if the SCIF Device is a MIC Peer to Peer SCIF device.
+ */
+static inline bool scifdev_is_p2p(struct scif_dev *dev)
+{
+	if (scif_is_mgmt_node())
+		return false;
+	else
+		return dev != &scif_dev[SCIF_MGMT_NODE] &&
+			!scifdev_self(dev);
+}
+
+/*
+ * scifdev_alive:
+ * @scifdev: The remote SCIF Device
+ *
+ * Returns true if the remote SCIF Device is running or sleeping for
+ * this endpoint.
+ */
+static inline int _scifdev_alive(struct scif_dev *scifdev)
+{
+	struct scif_peer_dev *spdev;
+
+	rcu_read_lock();
+	spdev = rcu_dereference(scifdev->spdev);
+	rcu_read_unlock();
+	return !!spdev;
+}
+
+#include "scif_epd.h"
+
+void __init scif_init_debugfs(void);
+void scif_exit_debugfs(void);
+int scif_setup_intr_wq(struct scif_dev *scifdev);
+void scif_destroy_intr_wq(struct scif_dev *scifdev);
+void scif_cleanup_scifdev(struct scif_dev *dev);
+void scif_handle_remove_node(int node);
+void scif_disconnect_node(u32 node_id, bool mgmt_initiated);
+void scif_free_qp(struct scif_dev *dev);
+void scif_misc_handler(struct work_struct *work);
+void scif_stop(struct scif_dev *scifdev);
+irqreturn_t scif_intr_handler(int irq, void *data);
+#endif /* SCIF_MAIN_H */
diff --git a/drivers/misc/mic/scif/scif_map.h b/drivers/misc/mic/scif/scif_map.h
new file mode 100644
index 000000000000..20e50b4e19b2
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_map.h
@@ -0,0 +1,113 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#ifndef SCIF_MAP_H
+#define SCIF_MAP_H
+
+#include "../bus/scif_bus.h"
+
+static __always_inline void *
+scif_alloc_coherent(dma_addr_t *dma_handle,
+		    struct scif_dev *scifdev, size_t size,
+		    gfp_t gfp)
+{
+	void *va;
+
+	if (scifdev_self(scifdev)) {
+		va = kmalloc(size, gfp);
+		if (va)
+			*dma_handle = virt_to_phys(va);
+	} else {
+		va = dma_alloc_coherent(&scifdev->sdev->dev,
+					size, dma_handle, gfp);
+		if (va && scifdev_is_p2p(scifdev))
+			*dma_handle = *dma_handle + scifdev->base_addr;
+	}
+	return va;
+}
+
+static __always_inline void
+scif_free_coherent(void *va, dma_addr_t local,
+		   struct scif_dev *scifdev, size_t size)
+{
+	if (scifdev_self(scifdev)) {
+		kfree(va);
+	} else {
+		if (scifdev_is_p2p(scifdev) && local > scifdev->base_addr)
+			local = local - scifdev->base_addr;
+		dma_free_coherent(&scifdev->sdev->dev,
+				  size, va, local);
+	}
+}
+
+static __always_inline int
+scif_map_single(dma_addr_t *dma_handle,
+		void *local, struct scif_dev *scifdev, size_t size)
+{
+	int err = 0;
+
+	if (scifdev_self(scifdev)) {
+		*dma_handle = virt_to_phys((local));
+	} else {
+		*dma_handle = dma_map_single(&scifdev->sdev->dev,
+					     local, size, DMA_BIDIRECTIONAL);
+		if (dma_mapping_error(&scifdev->sdev->dev, *dma_handle))
+			err = -ENOMEM;
+		else if (scifdev_is_p2p(scifdev))
+			*dma_handle = *dma_handle + scifdev->base_addr;
+	}
+	if (err)
+		*dma_handle = 0;
+	return err;
+}
+
+static __always_inline void
+scif_unmap_single(dma_addr_t local, struct scif_dev *scifdev,
+		  size_t size)
+{
+	if (!scifdev_self(scifdev)) {
+		if (scifdev_is_p2p(scifdev) && local > scifdev->base_addr)
+			local = local - scifdev->base_addr;
+		dma_unmap_single(&scifdev->sdev->dev, local,
+				 size, DMA_BIDIRECTIONAL);
+	}
+}
+
+static __always_inline void *
+scif_ioremap(dma_addr_t phys, size_t size, struct scif_dev *scifdev)
+{
+	void *out_virt;
+	struct scif_hw_dev *sdev = scifdev->sdev;
+
+	if (scifdev_self(scifdev))
+		out_virt = phys_to_virt(phys);
+	else
+		out_virt = (void __force *)
+			   sdev->hw_ops->ioremap(sdev, phys, size);
+	return out_virt;
+}
+
+static __always_inline void
+scif_iounmap(void *virt, size_t len, struct scif_dev *scifdev)
+{
+	if (!scifdev_self(scifdev)) {
+		struct scif_hw_dev *sdev = scifdev->sdev;
+
+		sdev->hw_ops->iounmap(sdev, (void __force __iomem *)virt);
+	}
+}
+#endif  /* SCIF_MAP_H */
diff --git a/drivers/misc/mic/scif/scif_nm.c b/drivers/misc/mic/scif/scif_nm.c
new file mode 100644
index 000000000000..9b4c5382d6a7
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_nm.c
@@ -0,0 +1,237 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include "scif_peer_bus.h"
+
+#include "scif_main.h"
+#include "scif_map.h"
+
+/**
+ * scif_invalidate_ep() - Set state for all connected endpoints
+ * to disconnected and wake up all send/recv waitqueues
+ */
+static void scif_invalidate_ep(int node)
+{
+	struct scif_endpt *ep;
+	struct list_head *pos, *tmpq;
+
+	flush_work(&scif_info.conn_work);
+	mutex_lock(&scif_info.connlock);
+	list_for_each_safe(pos, tmpq, &scif_info.disconnected) {
+		ep = list_entry(pos, struct scif_endpt, list);
+		if (ep->remote_dev->node == node) {
+			spin_lock(&ep->lock);
+			scif_cleanup_ep_qp(ep);
+			spin_unlock(&ep->lock);
+		}
+	}
+	list_for_each_safe(pos, tmpq, &scif_info.connected) {
+		ep = list_entry(pos, struct scif_endpt, list);
+		if (ep->remote_dev->node == node) {
+			list_del(pos);
+			spin_lock(&ep->lock);
+			ep->state = SCIFEP_DISCONNECTED;
+			list_add_tail(&ep->list, &scif_info.disconnected);
+			scif_cleanup_ep_qp(ep);
+			wake_up_interruptible(&ep->sendwq);
+			wake_up_interruptible(&ep->recvwq);
+			spin_unlock(&ep->lock);
+		}
+	}
+	mutex_unlock(&scif_info.connlock);
+}
+
+void scif_free_qp(struct scif_dev *scifdev)
+{
+	struct scif_qp *qp = scifdev->qpairs;
+
+	if (!qp)
+		return;
+	scif_free_coherent((void *)qp->inbound_q.rb_base,
+			   qp->local_buf, scifdev, qp->inbound_q.size);
+	scif_unmap_single(qp->local_qp, scifdev, sizeof(struct scif_qp));
+	kfree(scifdev->qpairs);
+	scifdev->qpairs = NULL;
+}
+
+static void scif_cleanup_qp(struct scif_dev *dev)
+{
+	struct scif_qp *qp = &dev->qpairs[0];
+
+	if (!qp)
+		return;
+	scif_iounmap((void *)qp->remote_qp, sizeof(struct scif_qp), dev);
+	scif_iounmap((void *)qp->outbound_q.rb_base,
+		     sizeof(struct scif_qp), dev);
+	qp->remote_qp = NULL;
+	qp->local_write = 0;
+	qp->inbound_q.current_write_offset = 0;
+	qp->inbound_q.current_read_offset = 0;
+	if (scifdev_is_p2p(dev))
+		scif_free_qp(dev);
+}
+
+void scif_send_acks(struct scif_dev *dev)
+{
+	struct scifmsg msg;
+
+	if (dev->node_remove_ack_pending) {
+		msg.uop = SCIF_NODE_REMOVE_ACK;
+		msg.src.node = scif_info.nodeid;
+		msg.dst.node = SCIF_MGMT_NODE;
+		msg.payload[0] = dev->node;
+		scif_nodeqp_send(&scif_dev[SCIF_MGMT_NODE], &msg);
+		dev->node_remove_ack_pending = false;
+	}
+	if (dev->exit_ack_pending) {
+		msg.uop = SCIF_EXIT_ACK;
+		msg.src.node = scif_info.nodeid;
+		msg.dst.node = dev->node;
+		scif_nodeqp_send(dev, &msg);
+		dev->exit_ack_pending = false;
+	}
+}
+
+/*
+ * scif_cleanup_scifdev
+ *
+ * @dev: Remote SCIF device.
+ * Uninitialize SCIF data structures for remote SCIF device.
+ */
+void scif_cleanup_scifdev(struct scif_dev *dev)
+{
+	struct scif_hw_dev *sdev = dev->sdev;
+
+	if (!dev->sdev)
+		return;
+	if (scifdev_is_p2p(dev)) {
+		if (dev->cookie) {
+			sdev->hw_ops->free_irq(sdev, dev->cookie, dev);
+			dev->cookie = NULL;
+		}
+		scif_destroy_intr_wq(dev);
+	}
+	scif_destroy_p2p(dev);
+	scif_invalidate_ep(dev->node);
+	scif_send_acks(dev);
+	if (!dev->node && scif_info.card_initiated_exit) {
+		/*
+		 * Send an SCIF_EXIT message which is the last message from MIC
+		 * to the Host and wait for a SCIF_EXIT_ACK
+		 */
+		scif_send_exit(dev);
+		scif_info.card_initiated_exit = false;
+	}
+	scif_cleanup_qp(dev);
+}
+
+/*
+ * scif_remove_node:
+ *
+ * @node: Node to remove
+ */
+void scif_handle_remove_node(int node)
+{
+	struct scif_dev *scifdev = &scif_dev[node];
+	struct scif_peer_dev *spdev;
+
+	rcu_read_lock();
+	spdev = rcu_dereference(scifdev->spdev);
+	rcu_read_unlock();
+	if (spdev)
+		scif_peer_unregister_device(spdev);
+	else
+		scif_send_acks(scifdev);
+}
+
+static int scif_send_rmnode_msg(int node, int remove_node)
+{
+	struct scifmsg notif_msg;
+	struct scif_dev *dev = &scif_dev[node];
+
+	notif_msg.uop = SCIF_NODE_REMOVE;
+	notif_msg.src.node = scif_info.nodeid;
+	notif_msg.dst.node = node;
+	notif_msg.payload[0] = remove_node;
+	return scif_nodeqp_send(dev, &notif_msg);
+}
+
+/**
+ * scif_node_disconnect:
+ *
+ * @node_id[in]: source node id.
+ * @mgmt_initiated: Disconnection initiated from the mgmt node
+ *
+ * Disconnect a node from the scif network.
+ */
+void scif_disconnect_node(u32 node_id, bool mgmt_initiated)
+{
+	int ret;
+	int msg_cnt = 0;
+	u32 i = 0;
+	struct scif_dev *scifdev = &scif_dev[node_id];
+
+	if (!node_id)
+		return;
+
+	atomic_set(&scifdev->disconn_rescnt, 0);
+
+	/* Destroy p2p network */
+	for (i = 1; i <= scif_info.maxid; i++) {
+		if (i == node_id)
+			continue;
+		ret = scif_send_rmnode_msg(i, node_id);
+		if (!ret)
+			msg_cnt++;
+	}
+	/* Wait for the remote nodes to respond with SCIF_NODE_REMOVE_ACK */
+	ret = wait_event_timeout(scifdev->disconn_wq,
+				 (atomic_read(&scifdev->disconn_rescnt)
+				 == msg_cnt), SCIF_NODE_ALIVE_TIMEOUT);
+	/* Tell the card to clean up */
+	if (mgmt_initiated && _scifdev_alive(scifdev))
+		/*
+		 * Send an SCIF_EXIT message which is the last message from Host
+		 * to the MIC and wait for a SCIF_EXIT_ACK
+		 */
+		scif_send_exit(scifdev);
+	atomic_set(&scifdev->disconn_rescnt, 0);
+	/* Tell the mgmt node to clean up */
+	ret = scif_send_rmnode_msg(SCIF_MGMT_NODE, node_id);
+	if (!ret)
+		/* Wait for mgmt node to respond with SCIF_NODE_REMOVE_ACK */
+		wait_event_timeout(scifdev->disconn_wq,
+				   (atomic_read(&scifdev->disconn_rescnt) == 1),
+				   SCIF_NODE_ALIVE_TIMEOUT);
+}
+
+void scif_get_node_info(void)
+{
+	struct scifmsg msg;
+	DECLARE_COMPLETION_ONSTACK(node_info);
+
+	msg.uop = SCIF_GET_NODE_INFO;
+	msg.src.node = scif_info.nodeid;
+	msg.dst.node = SCIF_MGMT_NODE;
+	msg.payload[3] = (u64)&node_info;
+
+	if ((scif_nodeqp_send(&scif_dev[SCIF_MGMT_NODE], &msg)))
+		return;
+
+	/* Wait for a response with SCIF_GET_NODE_INFO */
+	wait_for_completion(&node_info);
+}
diff --git a/drivers/misc/mic/scif/scif_nodeqp.c b/drivers/misc/mic/scif/scif_nodeqp.c
new file mode 100644
index 000000000000..41e3bdb10061
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_nodeqp.c
@@ -0,0 +1,1312 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include "../bus/scif_bus.h"
+#include "scif_peer_bus.h"
+#include "scif_main.h"
+#include "scif_nodeqp.h"
+#include "scif_map.h"
+
+/*
+ ************************************************************************
+ * SCIF node Queue Pair (QP) setup flow:
+ *
+ * 1) SCIF driver gets probed with a scif_hw_dev via the scif_hw_bus
+ * 2) scif_setup_qp(..) allocates the local qp and calls
+ *	scif_setup_qp_connect(..) which allocates and maps the local
+ *	buffer for the inbound QP
+ * 3) The local node updates the device page with the DMA address of the QP
+ * 4) A delayed work is scheduled (qp_dwork) which periodically reads if
+ *	the peer node has updated its QP DMA address
+ * 5) Once a valid non zero address is found in the QP DMA address field
+ *	in the device page, the local node maps the remote node's QP,
+ *	updates its outbound QP and sends a SCIF_INIT message to the peer
+ * 6) The SCIF_INIT message is received by the peer node QP interrupt bottom
+ *	half handler by calling scif_init(..)
+ * 7) scif_init(..) registers a new SCIF peer node by calling
+ *	scif_peer_register_device(..) which signifies the addition of a new
+ *	SCIF node
+ * 8) On the mgmt node, P2P network setup/teardown is initiated if all the
+ *	remote nodes are online via scif_p2p_setup(..)
+ * 9) For P2P setup, the host maps the remote nodes' aperture and memory
+ *	bars and sends a SCIF_NODE_ADD message to both nodes
+ * 10) As part of scif_nodeadd, both nodes set up their local inbound
+ *	QPs and send a SCIF_NODE_ADD_ACK to the mgmt node
+ * 11) As part of scif_node_add_ack(..) the mgmt node forwards the
+ *	SCIF_NODE_ADD_ACK to the remote nodes
+ * 12) As part of scif_node_add_ack(..) the remote nodes update their
+ *	outbound QPs, make sure they can access memory on the remote node
+ *	and then add a new SCIF peer node by calling
+ *	scif_peer_register_device(..) which signifies the addition of a new
+ *	SCIF node.
+ * 13) The SCIF network is now established across all nodes.
+ *
+ ************************************************************************
+ * SCIF node QP teardown flow (initiated by non mgmt node):
+ *
+ * 1) SCIF driver gets a remove callback with a scif_hw_dev via the scif_hw_bus
+ * 2) The device page QP DMA address field is updated with 0x0
+ * 3) A non mgmt node now cleans up all local data structures and sends a
+ *	SCIF_EXIT message to the peer and waits for a SCIF_EXIT_ACK
+ * 4) As part of scif_exit(..) handling scif_disconnect_node(..) is called
+ * 5) scif_disconnect_node(..) sends a SCIF_NODE_REMOVE message to all the
+ *	peers and waits for a SCIF_NODE_REMOVE_ACK
+ * 6) As part of scif_node_remove(..) a remote node unregisters the peer
+ *	node from the SCIF network and sends a SCIF_NODE_REMOVE_ACK
+ * 7) When the mgmt node has received all the SCIF_NODE_REMOVE_ACKs
+ *	it sends itself a node remove message whose handling cleans up local
+ *	data structures and unregisters the peer node from the SCIF network
+ * 8) The mgmt node sends a SCIF_EXIT_ACK
+ * 9) Upon receipt of the SCIF_EXIT_ACK the node initiating the teardown
+ *	completes the SCIF remove routine
+ * 10) The SCIF network is now torn down for the node initiating the
+ *	teardown sequence
+ *
+ ************************************************************************
+ * SCIF node QP teardown flow (initiated by mgmt node):
+ *
+ * 1) SCIF driver gets a remove callback with a scif_hw_dev via the scif_hw_bus
+ * 2) The device page QP DMA address field is updated with 0x0
+ * 3) The mgmt node calls scif_disconnect_node(..)
+ * 4) scif_disconnect_node(..) sends a SCIF_NODE_REMOVE message to all the peers
+ *	and waits for a SCIF_NODE_REMOVE_ACK
+ * 5) As part of scif_node_remove(..) a remote node unregisters the peer
+ *	node from the SCIF network and sends a SCIF_NODE_REMOVE_ACK
+ * 6) When the mgmt node has received all the SCIF_NODE_REMOVE_ACKs
+ *	it unregisters the peer node from the SCIF network
+ * 7) The mgmt node sends a SCIF_EXIT message and waits for a SCIF_EXIT_ACK.
+ * 8) A non mgmt node upon receipt of a SCIF_EXIT message calls scif_stop(..)
+ *	which would clean up local data structures for all SCIF nodes and
+ *	then send a SCIF_EXIT_ACK back to the mgmt node
+ * 9) Upon receipt of the SCIF_EXIT_ACK the the mgmt node sends itself a node
+ *	remove message whose handling cleans up local data structures and
+ *	destroys any P2P mappings.
+ * 10) The SCIF hardware device for which a remove callback was received is now
+ *	disconnected from the SCIF network.
+ */
+/*
+ * Initializes "local" data structures for the QP. Allocates the QP
+ * ring buffer (rb) and initializes the "in bound" queue.
+ */
+int scif_setup_qp_connect(struct scif_qp *qp, dma_addr_t *qp_offset,
+			  int local_size, struct scif_dev *scifdev)
+{
+	void *local_q = NULL;
+	int err = 0;
+	u32 tmp_rd = 0;
+
+	spin_lock_init(&qp->send_lock);
+	spin_lock_init(&qp->recv_lock);
+
+	local_q = kzalloc(local_size, GFP_KERNEL);
+	if (!local_q) {
+		err = -ENOMEM;
+		return err;
+	}
+	err = scif_map_single(&qp->local_buf, local_q, scifdev, local_size);
+	if (err)
+		goto kfree;
+	/*
+	 * To setup the inbound_q, the buffer lives locally, the read pointer
+	 * is remote and the write pointer is local.
+	 */
+	scif_rb_init(&qp->inbound_q,
+		     &tmp_rd,
+		     &qp->local_write,
+		     local_q, get_count_order(local_size));
+	/*
+	 * The read pointer is NULL initially and it is unsafe to use the ring
+	 * buffer til this changes!
+	 */
+	qp->inbound_q.read_ptr = NULL;
+	err = scif_map_single(qp_offset, qp,
+			      scifdev, sizeof(struct scif_qp));
+	if (err)
+		goto unmap;
+	qp->local_qp = *qp_offset;
+	return err;
+unmap:
+	scif_unmap_single(qp->local_buf, scifdev, local_size);
+	qp->local_buf = 0;
+kfree:
+	kfree(local_q);
+	return err;
+}
+
+/* When the other side has already done it's allocation, this is called */
+int scif_setup_qp_accept(struct scif_qp *qp, dma_addr_t *qp_offset,
+			 dma_addr_t phys, int local_size,
+			 struct scif_dev *scifdev)
+{
+	void *local_q;
+	void *remote_q;
+	struct scif_qp *remote_qp;
+	int remote_size;
+	int err = 0;
+
+	spin_lock_init(&qp->send_lock);
+	spin_lock_init(&qp->recv_lock);
+	/* Start by figuring out where we need to point */
+	remote_qp = scif_ioremap(phys, sizeof(struct scif_qp), scifdev);
+	if (!remote_qp)
+		return -EIO;
+	qp->remote_qp = remote_qp;
+	if (qp->remote_qp->magic != SCIFEP_MAGIC) {
+		err = -EIO;
+		goto iounmap;
+	}
+	qp->remote_buf = remote_qp->local_buf;
+	remote_size = qp->remote_qp->inbound_q.size;
+	remote_q = scif_ioremap(qp->remote_buf, remote_size, scifdev);
+	if (!remote_q) {
+		err = -EIO;
+		goto iounmap;
+	}
+	qp->remote_qp->local_write = 0;
+	/*
+	 * To setup the outbound_q, the buffer lives in remote memory,
+	 * the read pointer is local, the write pointer is remote
+	 */
+	scif_rb_init(&qp->outbound_q,
+		     &qp->local_read,
+		     &qp->remote_qp->local_write,
+		     remote_q,
+		     get_count_order(remote_size));
+	local_q = kzalloc(local_size, GFP_KERNEL);
+	if (!local_q) {
+		err = -ENOMEM;
+		goto iounmap_1;
+	}
+	err = scif_map_single(&qp->local_buf, local_q, scifdev, local_size);
+	if (err)
+		goto kfree;
+	qp->remote_qp->local_read = 0;
+	/*
+	 * To setup the inbound_q, the buffer lives locally, the read pointer
+	 * is remote and the write pointer is local
+	 */
+	scif_rb_init(&qp->inbound_q,
+		     &qp->remote_qp->local_read,
+		     &qp->local_write,
+		     local_q, get_count_order(local_size));
+	err = scif_map_single(qp_offset, qp, scifdev,
+			      sizeof(struct scif_qp));
+	if (err)
+		goto unmap;
+	qp->local_qp = *qp_offset;
+	return err;
+unmap:
+	scif_unmap_single(qp->local_buf, scifdev, local_size);
+	qp->local_buf = 0;
+kfree:
+	kfree(local_q);
+iounmap_1:
+	scif_iounmap(remote_q, remote_size, scifdev);
+	qp->outbound_q.rb_base = NULL;
+iounmap:
+	scif_iounmap(qp->remote_qp, sizeof(struct scif_qp), scifdev);
+	qp->remote_qp = NULL;
+	return err;
+}
+
+int scif_setup_qp_connect_response(struct scif_dev *scifdev,
+				   struct scif_qp *qp, u64 payload)
+{
+	int err = 0;
+	void *r_buf;
+	int remote_size;
+	phys_addr_t tmp_phys;
+
+	qp->remote_qp = scif_ioremap(payload, sizeof(struct scif_qp), scifdev);
+
+	if (!qp->remote_qp) {
+		err = -ENOMEM;
+		goto error;
+	}
+
+	if (qp->remote_qp->magic != SCIFEP_MAGIC) {
+		dev_err(&scifdev->sdev->dev,
+			"SCIFEP_MAGIC mismatch between self %d remote %d\n",
+			scif_dev[scif_info.nodeid].node, scifdev->node);
+		err = -ENODEV;
+		goto error;
+	}
+
+	tmp_phys = qp->remote_qp->local_buf;
+	remote_size = qp->remote_qp->inbound_q.size;
+	r_buf = scif_ioremap(tmp_phys, remote_size, scifdev);
+
+	if (!r_buf)
+		return -EIO;
+
+	qp->local_read = 0;
+	scif_rb_init(&qp->outbound_q,
+		     &qp->local_read,
+		     &qp->remote_qp->local_write,
+		     r_buf,
+		     get_count_order(remote_size));
+	/*
+	 * resetup the inbound_q now that we know where the
+	 * inbound_read really is.
+	 */
+	scif_rb_init(&qp->inbound_q,
+		     &qp->remote_qp->local_read,
+		     &qp->local_write,
+		     qp->inbound_q.rb_base,
+		     get_count_order(qp->inbound_q.size));
+error:
+	return err;
+}
+
+static __always_inline void
+scif_send_msg_intr(struct scif_dev *scifdev)
+{
+	struct scif_hw_dev *sdev = scifdev->sdev;
+
+	if (scifdev_is_p2p(scifdev))
+		sdev->hw_ops->send_p2p_intr(sdev, scifdev->rdb, &scifdev->mmio);
+	else
+		sdev->hw_ops->send_intr(sdev, scifdev->rdb);
+}
+
+int scif_qp_response(phys_addr_t phys, struct scif_dev *scifdev)
+{
+	int err = 0;
+	struct scifmsg msg;
+
+	err = scif_setup_qp_connect_response(scifdev, scifdev->qpairs, phys);
+	if (!err) {
+		/*
+		 * Now that everything is setup and mapped, we're ready
+		 * to tell the peer about our queue's location
+		 */
+		msg.uop = SCIF_INIT;
+		msg.dst.node = scifdev->node;
+		err = scif_nodeqp_send(scifdev, &msg);
+	}
+	return err;
+}
+
+void scif_send_exit(struct scif_dev *scifdev)
+{
+	struct scifmsg msg;
+	int ret;
+
+	scifdev->exit = OP_IN_PROGRESS;
+	msg.uop = SCIF_EXIT;
+	msg.src.node = scif_info.nodeid;
+	msg.dst.node = scifdev->node;
+	ret = scif_nodeqp_send(scifdev, &msg);
+	if (ret)
+		goto done;
+	/* Wait for a SCIF_EXIT_ACK message */
+	wait_event_timeout(scif_info.exitwq, scifdev->exit == OP_COMPLETED,
+			   SCIF_NODE_ALIVE_TIMEOUT);
+done:
+	scifdev->exit = OP_IDLE;
+}
+
+int scif_setup_qp(struct scif_dev *scifdev)
+{
+	int err = 0;
+	int local_size;
+	struct scif_qp *qp;
+
+	local_size = SCIF_NODE_QP_SIZE;
+
+	qp = kzalloc(sizeof(*qp), GFP_KERNEL);
+	if (!qp) {
+		err = -ENOMEM;
+		return err;
+	}
+	qp->magic = SCIFEP_MAGIC;
+	scifdev->qpairs = qp;
+	err = scif_setup_qp_connect(qp, &scifdev->qp_dma_addr,
+				    local_size, scifdev);
+	if (err)
+		goto free_qp;
+	/*
+	 * We're as setup as we can be. The inbound_q is setup, w/o a usable
+	 * outbound q.  When we get a message, the read_ptr will be updated,
+	 * and we will pull the message.
+	 */
+	return err;
+free_qp:
+	kfree(scifdev->qpairs);
+	scifdev->qpairs = NULL;
+	return err;
+}
+
+static void scif_p2p_freesg(struct scatterlist *sg)
+{
+	kfree(sg);
+}
+
+static struct scatterlist *
+scif_p2p_setsg(void __iomem *va, int page_size, int page_cnt)
+{
+	struct scatterlist *sg;
+	struct page *page;
+	int i;
+
+	sg = kcalloc(page_cnt, sizeof(struct scatterlist), GFP_KERNEL);
+	if (!sg)
+		return NULL;
+	sg_init_table(sg, page_cnt);
+	for (i = 0; i < page_cnt; i++) {
+		page = vmalloc_to_page((void __force *)va);
+		if (!page)
+			goto p2p_sg_err;
+		sg_set_page(&sg[i], page, page_size, 0);
+		va += page_size;
+	}
+	return sg;
+p2p_sg_err:
+	kfree(sg);
+	return NULL;
+}
+
+/* Init p2p mappings required to access peerdev from scifdev */
+static struct scif_p2p_info *
+scif_init_p2p_info(struct scif_dev *scifdev, struct scif_dev *peerdev)
+{
+	struct scif_p2p_info *p2p;
+	int num_mmio_pages, num_aper_pages, sg_page_shift, err, num_aper_chunks;
+	struct scif_hw_dev *psdev = peerdev->sdev;
+	struct scif_hw_dev *sdev = scifdev->sdev;
+
+	num_mmio_pages = psdev->mmio->len >> PAGE_SHIFT;
+	num_aper_pages = psdev->aper->len >> PAGE_SHIFT;
+
+	p2p = kzalloc(sizeof(*p2p), GFP_KERNEL);
+	if (!p2p)
+		return NULL;
+	p2p->ppi_sg[SCIF_PPI_MMIO] = scif_p2p_setsg(psdev->mmio->va,
+						    PAGE_SIZE, num_mmio_pages);
+	if (!p2p->ppi_sg[SCIF_PPI_MMIO])
+		goto free_p2p;
+	p2p->sg_nentries[SCIF_PPI_MMIO] = num_mmio_pages;
+	sg_page_shift = get_order(min(psdev->aper->len, (u64)(1 << 30)));
+	num_aper_chunks = num_aper_pages >> (sg_page_shift - PAGE_SHIFT);
+	p2p->ppi_sg[SCIF_PPI_APER] = scif_p2p_setsg(psdev->aper->va,
+						    1 << sg_page_shift,
+						    num_aper_chunks);
+	p2p->sg_nentries[SCIF_PPI_APER] = num_aper_chunks;
+	err = dma_map_sg(&sdev->dev, p2p->ppi_sg[SCIF_PPI_MMIO],
+			 num_mmio_pages, PCI_DMA_BIDIRECTIONAL);
+	if (err != num_mmio_pages)
+		goto scif_p2p_free;
+	err = dma_map_sg(&sdev->dev, p2p->ppi_sg[SCIF_PPI_APER],
+			 num_aper_chunks, PCI_DMA_BIDIRECTIONAL);
+	if (err != num_aper_chunks)
+		goto dma_unmap;
+	p2p->ppi_da[SCIF_PPI_MMIO] = sg_dma_address(p2p->ppi_sg[SCIF_PPI_MMIO]);
+	p2p->ppi_da[SCIF_PPI_APER] = sg_dma_address(p2p->ppi_sg[SCIF_PPI_APER]);
+	p2p->ppi_len[SCIF_PPI_MMIO] = num_mmio_pages;
+	p2p->ppi_len[SCIF_PPI_APER] = num_aper_pages;
+	p2p->ppi_peer_id = peerdev->node;
+	return p2p;
+dma_unmap:
+	dma_unmap_sg(&sdev->dev, p2p->ppi_sg[SCIF_PPI_MMIO],
+		     p2p->sg_nentries[SCIF_PPI_MMIO], DMA_BIDIRECTIONAL);
+scif_p2p_free:
+	scif_p2p_freesg(p2p->ppi_sg[SCIF_PPI_MMIO]);
+	scif_p2p_freesg(p2p->ppi_sg[SCIF_PPI_APER]);
+free_p2p:
+	kfree(p2p);
+	return NULL;
+}
+
+/**
+ * scif_node_connect: Respond to SCIF_NODE_CONNECT interrupt message
+ * @dst: Destination node
+ *
+ * Connect the src and dst node by setting up the p2p connection
+ * between them. Management node here acts like a proxy.
+ */
+static void scif_node_connect(struct scif_dev *scifdev, int dst)
+{
+	struct scif_dev *dev_j = scifdev;
+	struct scif_dev *dev_i = NULL;
+	struct scif_p2p_info *p2p_ij = NULL;    /* bus addr for j from i */
+	struct scif_p2p_info *p2p_ji = NULL;    /* bus addr for i from j */
+	struct scif_p2p_info *p2p;
+	struct list_head *pos, *tmp;
+	struct scifmsg msg;
+	int err;
+	u64 tmppayload;
+
+	if (dst < 1 || dst > scif_info.maxid)
+		return;
+
+	dev_i = &scif_dev[dst];
+
+	if (!_scifdev_alive(dev_i))
+		return;
+	/*
+	 * If the p2p connection is already setup or in the process of setting
+	 * up then just ignore this request. The requested node will get
+	 * informed by SCIF_NODE_ADD_ACK or SCIF_NODE_ADD_NACK
+	 */
+	if (!list_empty(&dev_i->p2p)) {
+		list_for_each_safe(pos, tmp, &dev_i->p2p) {
+			p2p = list_entry(pos, struct scif_p2p_info, ppi_list);
+			if (p2p->ppi_peer_id == dev_j->node)
+				return;
+		}
+	}
+	p2p_ij = scif_init_p2p_info(dev_i, dev_j);
+	if (!p2p_ij)
+		return;
+	p2p_ji = scif_init_p2p_info(dev_j, dev_i);
+	if (!p2p_ji)
+		return;
+	list_add_tail(&p2p_ij->ppi_list, &dev_i->p2p);
+	list_add_tail(&p2p_ji->ppi_list, &dev_j->p2p);
+
+	/*
+	 * Send a SCIF_NODE_ADD to dev_i, pass it its bus address
+	 * as seen from dev_j
+	 */
+	msg.uop = SCIF_NODE_ADD;
+	msg.src.node = dev_j->node;
+	msg.dst.node = dev_i->node;
+
+	msg.payload[0] = p2p_ji->ppi_da[SCIF_PPI_APER];
+	msg.payload[1] = p2p_ij->ppi_da[SCIF_PPI_MMIO];
+	msg.payload[2] = p2p_ij->ppi_da[SCIF_PPI_APER];
+	msg.payload[3] = p2p_ij->ppi_len[SCIF_PPI_APER] << PAGE_SHIFT;
+
+	err = scif_nodeqp_send(dev_i,  &msg);
+	if (err) {
+		dev_err(&scifdev->sdev->dev,
+			"%s %d error %d\n", __func__, __LINE__, err);
+		return;
+	}
+
+	/* Same as above but to dev_j */
+	msg.uop = SCIF_NODE_ADD;
+	msg.src.node = dev_i->node;
+	msg.dst.node = dev_j->node;
+
+	tmppayload = msg.payload[0];
+	msg.payload[0] = msg.payload[2];
+	msg.payload[2] = tmppayload;
+	msg.payload[1] = p2p_ji->ppi_da[SCIF_PPI_MMIO];
+	msg.payload[3] = p2p_ji->ppi_len[SCIF_PPI_APER] << PAGE_SHIFT;
+
+	scif_nodeqp_send(dev_j, &msg);
+}
+
+static void scif_p2p_setup(void)
+{
+	int i, j;
+
+	if (!scif_info.p2p_enable)
+		return;
+
+	for (i = 1; i <= scif_info.maxid; i++)
+		if (!_scifdev_alive(&scif_dev[i]))
+			return;
+
+	for (i = 1; i <= scif_info.maxid; i++) {
+		for (j = 1; j <= scif_info.maxid; j++) {
+			struct scif_dev *scifdev = &scif_dev[i];
+
+			if (i == j)
+				continue;
+			scif_node_connect(scifdev, j);
+		}
+	}
+}
+
+void scif_qp_response_ack(struct work_struct *work)
+{
+	struct scif_dev *scifdev = container_of(work, struct scif_dev,
+						init_msg_work);
+	struct scif_peer_dev *spdev;
+
+	/* Drop the INIT message if it has already been received */
+	if (_scifdev_alive(scifdev))
+		return;
+
+	spdev = scif_peer_register_device(scifdev);
+	if (IS_ERR(spdev))
+		return;
+
+	if (scif_is_mgmt_node()) {
+		mutex_lock(&scif_info.conflock);
+		scif_p2p_setup();
+		mutex_unlock(&scif_info.conflock);
+	}
+}
+
+static char *message_types[] = {"BAD",
+				"INIT",
+				"EXIT",
+				"SCIF_EXIT_ACK",
+				"SCIF_NODE_ADD",
+				"SCIF_NODE_ADD_ACK",
+				"SCIF_NODE_ADD_NACK",
+				"REMOVE_NODE",
+				"REMOVE_NODE_ACK",
+				"CNCT_REQ",
+				"CNCT_GNT",
+				"CNCT_GNTACK",
+				"CNCT_GNTNACK",
+				"CNCT_REJ",
+				"DISCNCT",
+				"DISCNT_ACK",
+				"CLIENT_SENT",
+				"CLIENT_RCVD",
+				"SCIF_GET_NODE_INFO"};
+
+static void
+scif_display_message(struct scif_dev *scifdev, struct scifmsg *msg,
+		     const char *label)
+{
+	if (!scif_info.en_msg_log)
+		return;
+	if (msg->uop > SCIF_MAX_MSG) {
+		dev_err(&scifdev->sdev->dev,
+			"%s: unknown msg type %d\n", label, msg->uop);
+		return;
+	}
+	dev_info(&scifdev->sdev->dev,
+		 "%s: msg type %s, src %d:%d, dest %d:%d payload 0x%llx:0x%llx:0x%llx:0x%llx\n",
+		 label, message_types[msg->uop], msg->src.node, msg->src.port,
+		 msg->dst.node, msg->dst.port, msg->payload[0], msg->payload[1],
+		 msg->payload[2], msg->payload[3]);
+}
+
+int _scif_nodeqp_send(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_qp *qp = scifdev->qpairs;
+	int err = -ENOMEM, loop_cnt = 0;
+
+	scif_display_message(scifdev, msg, "Sent");
+	if (!qp) {
+		err = -EINVAL;
+		goto error;
+	}
+	spin_lock(&qp->send_lock);
+
+	while ((err = scif_rb_write(&qp->outbound_q,
+				    msg, sizeof(struct scifmsg)))) {
+		mdelay(1);
+#define SCIF_NODEQP_SEND_TO_MSEC (3 * 1000)
+		if (loop_cnt++ > (SCIF_NODEQP_SEND_TO_MSEC)) {
+			err = -ENODEV;
+			break;
+		}
+	}
+	if (!err)
+		scif_rb_commit(&qp->outbound_q);
+	spin_unlock(&qp->send_lock);
+	if (!err) {
+		if (scifdev_self(scifdev))
+			/*
+			 * For loopback we need to emulate an interrupt by
+			 * queuing work for the queue handling real node
+			 * Qp interrupts.
+			 */
+			queue_work(scifdev->intr_wq, &scifdev->intr_bh);
+		else
+			scif_send_msg_intr(scifdev);
+	}
+error:
+	if (err)
+		dev_dbg(&scifdev->sdev->dev,
+			"%s %d error %d uop %d\n",
+			 __func__, __LINE__, err, msg->uop);
+	return err;
+}
+
+/**
+ * scif_nodeqp_send - Send a message on the node queue pair
+ * @scifdev: Scif Device.
+ * @msg: The message to be sent.
+ */
+int scif_nodeqp_send(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	int err;
+	struct device *spdev = NULL;
+
+	if (msg->uop > SCIF_EXIT_ACK) {
+		/* Dont send messages once the exit flow has begun */
+		if (OP_IDLE != scifdev->exit)
+			return -ENODEV;
+		spdev = scif_get_peer_dev(scifdev);
+		if (IS_ERR(spdev)) {
+			err = PTR_ERR(spdev);
+			return err;
+		}
+	}
+	err = _scif_nodeqp_send(scifdev, msg);
+	if (msg->uop > SCIF_EXIT_ACK)
+		scif_put_peer_dev(spdev);
+	return err;
+}
+
+/*
+ * scif_misc_handler:
+ *
+ * Work queue handler for servicing miscellaneous SCIF tasks.
+ * Examples include:
+ * 1) Cleanup of zombie endpoints.
+ */
+void scif_misc_handler(struct work_struct *work)
+{
+	scif_cleanup_zombie_epd();
+}
+
+/**
+ * scif_init() - Respond to SCIF_INIT interrupt message
+ * @scifdev:    Remote SCIF device node
+ * @msg:        Interrupt message
+ */
+static __always_inline void
+scif_init(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	/*
+	 * Allow the thread waiting for device page updates for the peer QP DMA
+	 * address to complete initializing the inbound_q.
+	 */
+	flush_delayed_work(&scifdev->qp_dwork);
+	/*
+	 * Delegate the peer device registration to a workqueue, otherwise if
+	 * SCIF client probe (called during peer device registration) calls
+	 * scif_connect(..), it will block the message processing thread causing
+	 * a deadlock.
+	 */
+	schedule_work(&scifdev->init_msg_work);
+}
+
+/**
+ * scif_exit() - Respond to SCIF_EXIT interrupt message
+ * @scifdev:    Remote SCIF device node
+ * @msg:        Interrupt message
+ *
+ * This function stops the SCIF interface for the node which sent
+ * the SCIF_EXIT message and starts waiting for that node to
+ * resetup the queue pair again.
+ */
+static __always_inline void
+scif_exit(struct scif_dev *scifdev, struct scifmsg *unused)
+{
+	scifdev->exit_ack_pending = true;
+	if (scif_is_mgmt_node())
+		scif_disconnect_node(scifdev->node, false);
+	else
+		scif_stop(scifdev);
+	schedule_delayed_work(&scifdev->qp_dwork,
+			      msecs_to_jiffies(1000));
+}
+
+/**
+ * scif_exitack() - Respond to SCIF_EXIT_ACK interrupt message
+ * @scifdev:    Remote SCIF device node
+ * @msg:        Interrupt message
+ *
+ */
+static __always_inline void
+scif_exit_ack(struct scif_dev *scifdev, struct scifmsg *unused)
+{
+	scifdev->exit = OP_COMPLETED;
+	wake_up(&scif_info.exitwq);
+}
+
+/**
+ * scif_node_add() - Respond to SCIF_NODE_ADD interrupt message
+ * @scifdev:    Remote SCIF device node
+ * @msg:        Interrupt message
+ *
+ * When the mgmt node driver has finished initializing a MIC node queue pair it
+ * marks the node as online. It then looks for all currently online MIC cards
+ * and send a SCIF_NODE_ADD message to identify the ID of the new card for
+ * peer to peer initialization
+ *
+ * The local node allocates its incoming queue and sends its address in the
+ * SCIF_NODE_ADD_ACK message back to the mgmt node, the mgmt node "reflects"
+ * this message to the new node
+ */
+static __always_inline void
+scif_node_add(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_dev *newdev;
+	dma_addr_t qp_offset;
+	int qp_connect;
+	struct scif_hw_dev *sdev;
+
+	dev_dbg(&scifdev->sdev->dev,
+		"Scifdev %d:%d received NODE_ADD msg for node %d\n",
+		scifdev->node, msg->dst.node, msg->src.node);
+	dev_dbg(&scifdev->sdev->dev,
+		"Remote address for this node's aperture %llx\n",
+		msg->payload[0]);
+	newdev = &scif_dev[msg->src.node];
+	newdev->node = msg->src.node;
+	newdev->sdev = scif_dev[SCIF_MGMT_NODE].sdev;
+	sdev = newdev->sdev;
+
+	if (scif_setup_intr_wq(newdev)) {
+		dev_err(&scifdev->sdev->dev,
+			"failed to setup interrupts for %d\n", msg->src.node);
+		goto interrupt_setup_error;
+	}
+	newdev->mmio.va = ioremap_nocache(msg->payload[1], sdev->mmio->len);
+	if (!newdev->mmio.va) {
+		dev_err(&scifdev->sdev->dev,
+			"failed to map mmio for %d\n", msg->src.node);
+		goto mmio_map_error;
+	}
+	newdev->qpairs = kzalloc(sizeof(*newdev->qpairs), GFP_KERNEL);
+	if (!newdev->qpairs)
+		goto qp_alloc_error;
+	/*
+	 * Set the base address of the remote node's memory since it gets
+	 * added to qp_offset
+	 */
+	newdev->base_addr = msg->payload[0];
+
+	qp_connect = scif_setup_qp_connect(newdev->qpairs, &qp_offset,
+					   SCIF_NODE_QP_SIZE, newdev);
+	if (qp_connect) {
+		dev_err(&scifdev->sdev->dev,
+			"failed to setup qp_connect %d\n", qp_connect);
+		goto qp_connect_error;
+	}
+
+	newdev->db = sdev->hw_ops->next_db(sdev);
+	newdev->cookie = sdev->hw_ops->request_irq(sdev, scif_intr_handler,
+						   "SCIF_INTR", newdev,
+						   newdev->db);
+	if (IS_ERR(newdev->cookie))
+		goto qp_connect_error;
+	newdev->qpairs->magic = SCIFEP_MAGIC;
+	newdev->qpairs->qp_state = SCIF_QP_OFFLINE;
+
+	msg->uop = SCIF_NODE_ADD_ACK;
+	msg->dst.node = msg->src.node;
+	msg->src.node = scif_info.nodeid;
+	msg->payload[0] = qp_offset;
+	msg->payload[2] = newdev->db;
+	scif_nodeqp_send(&scif_dev[SCIF_MGMT_NODE], msg);
+	return;
+qp_connect_error:
+	kfree(newdev->qpairs);
+	newdev->qpairs = NULL;
+qp_alloc_error:
+	iounmap(newdev->mmio.va);
+	newdev->mmio.va = NULL;
+mmio_map_error:
+interrupt_setup_error:
+	dev_err(&scifdev->sdev->dev,
+		"node add failed for node %d\n", msg->src.node);
+	msg->uop = SCIF_NODE_ADD_NACK;
+	msg->dst.node = msg->src.node;
+	msg->src.node = scif_info.nodeid;
+	scif_nodeqp_send(&scif_dev[SCIF_MGMT_NODE], msg);
+}
+
+void scif_poll_qp_state(struct work_struct *work)
+{
+#define SCIF_NODE_QP_RETRY 100
+#define SCIF_NODE_QP_TIMEOUT 100
+	struct scif_dev *peerdev = container_of(work, struct scif_dev,
+							p2p_dwork.work);
+	struct scif_qp *qp = &peerdev->qpairs[0];
+
+	if (qp->qp_state != SCIF_QP_ONLINE ||
+	    qp->remote_qp->qp_state != SCIF_QP_ONLINE) {
+		if (peerdev->p2p_retry++ == SCIF_NODE_QP_RETRY) {
+			dev_err(&peerdev->sdev->dev,
+				"Warning: QP check timeout with state %d\n",
+				qp->qp_state);
+			goto timeout;
+		}
+		schedule_delayed_work(&peerdev->p2p_dwork,
+				      msecs_to_jiffies(SCIF_NODE_QP_TIMEOUT));
+		return;
+	}
+	scif_peer_register_device(peerdev);
+	return;
+timeout:
+	dev_err(&peerdev->sdev->dev,
+		"%s %d remote node %d offline,  state = 0x%x\n",
+		__func__, __LINE__, peerdev->node, qp->qp_state);
+	qp->remote_qp->qp_state = SCIF_QP_OFFLINE;
+	scif_cleanup_scifdev(peerdev);
+}
+
+/**
+ * scif_node_add_ack() - Respond to SCIF_NODE_ADD_ACK interrupt message
+ * @scifdev:    Remote SCIF device node
+ * @msg:        Interrupt message
+ *
+ * After a MIC node receives the SCIF_NODE_ADD_ACK message it send this
+ * message to the mgmt node to confirm the sequence is finished.
+ *
+ */
+static __always_inline void
+scif_node_add_ack(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_dev *peerdev;
+	struct scif_qp *qp;
+	struct scif_dev *dst_dev = &scif_dev[msg->dst.node];
+
+	dev_dbg(&scifdev->sdev->dev,
+		"Scifdev %d received SCIF_NODE_ADD_ACK msg src %d dst %d\n",
+		scifdev->node, msg->src.node, msg->dst.node);
+	dev_dbg(&scifdev->sdev->dev,
+		"payload %llx %llx %llx %llx\n", msg->payload[0],
+		msg->payload[1], msg->payload[2], msg->payload[3]);
+	if (scif_is_mgmt_node()) {
+		/*
+		 * the lock serializes with scif_qp_response_ack. The mgmt node
+		 * is forwarding the NODE_ADD_ACK message from src to dst we
+		 * need to make sure that the dst has already received a
+		 * NODE_ADD for src and setup its end of the qp to dst
+		 */
+		mutex_lock(&scif_info.conflock);
+		msg->payload[1] = scif_info.maxid;
+		scif_nodeqp_send(dst_dev, msg);
+		mutex_unlock(&scif_info.conflock);
+		return;
+	}
+	peerdev = &scif_dev[msg->src.node];
+	peerdev->sdev = scif_dev[SCIF_MGMT_NODE].sdev;
+	peerdev->node = msg->src.node;
+
+	qp = &peerdev->qpairs[0];
+
+	if ((scif_setup_qp_connect_response(peerdev, &peerdev->qpairs[0],
+					    msg->payload[0])))
+		goto local_error;
+	peerdev->rdb = msg->payload[2];
+	qp->remote_qp->qp_state = SCIF_QP_ONLINE;
+	schedule_delayed_work(&peerdev->p2p_dwork, 0);
+	return;
+local_error:
+	scif_cleanup_scifdev(peerdev);
+}
+
+/**
+ * scif_node_add_nack: Respond to SCIF_NODE_ADD_NACK interrupt message
+ * @msg:        Interrupt message
+ *
+ * SCIF_NODE_ADD failed, so inform the waiting wq.
+ */
+static __always_inline void
+scif_node_add_nack(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	if (scif_is_mgmt_node()) {
+		struct scif_dev *dst_dev = &scif_dev[msg->dst.node];
+
+		dev_dbg(&scifdev->sdev->dev,
+			"SCIF_NODE_ADD_NACK received from %d\n", scifdev->node);
+		scif_nodeqp_send(dst_dev, msg);
+	}
+}
+
+/*
+ * scif_node_remove: Handle SCIF_NODE_REMOVE message
+ * @msg: Interrupt message
+ *
+ * Handle node removal.
+ */
+static __always_inline void
+scif_node_remove(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	int node = msg->payload[0];
+	struct scif_dev *scdev = &scif_dev[node];
+
+	scdev->node_remove_ack_pending = true;
+	scif_handle_remove_node(node);
+}
+
+/*
+ * scif_node_remove_ack: Handle SCIF_NODE_REMOVE_ACK message
+ * @msg: Interrupt message
+ *
+ * The peer has acked a SCIF_NODE_REMOVE message.
+ */
+static __always_inline void
+scif_node_remove_ack(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	struct scif_dev *sdev = &scif_dev[msg->payload[0]];
+
+	atomic_inc(&sdev->disconn_rescnt);
+	wake_up(&sdev->disconn_wq);
+}
+
+/**
+ * scif_get_node_info: Respond to SCIF_GET_NODE_INFO interrupt message
+ * @msg:        Interrupt message
+ *
+ * Retrieve node info i.e maxid and total from the mgmt node.
+ */
+static __always_inline void
+scif_get_node_info_resp(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	if (scif_is_mgmt_node()) {
+		swap(msg->dst.node, msg->src.node);
+		mutex_lock(&scif_info.conflock);
+		msg->payload[1] = scif_info.maxid;
+		msg->payload[2] = scif_info.total;
+		mutex_unlock(&scif_info.conflock);
+		scif_nodeqp_send(scifdev, msg);
+	} else {
+		struct completion *node_info =
+			(struct completion *)msg->payload[3];
+
+		mutex_lock(&scif_info.conflock);
+		scif_info.maxid = msg->payload[1];
+		scif_info.total = msg->payload[2];
+		complete_all(node_info);
+		mutex_unlock(&scif_info.conflock);
+	}
+}
+
+static void
+scif_msg_unknown(struct scif_dev *scifdev, struct scifmsg *msg)
+{
+	/* Bogus Node Qp Message? */
+	dev_err(&scifdev->sdev->dev,
+		"Unknown message 0x%xn scifdev->node 0x%x\n",
+		msg->uop, scifdev->node);
+}
+
+static void (*scif_intr_func[SCIF_MAX_MSG + 1])
+	    (struct scif_dev *, struct scifmsg *msg) = {
+	scif_msg_unknown,	/* Error */
+	scif_init,		/* SCIF_INIT */
+	scif_exit,		/* SCIF_EXIT */
+	scif_exit_ack,		/* SCIF_EXIT_ACK */
+	scif_node_add,		/* SCIF_NODE_ADD */
+	scif_node_add_ack,	/* SCIF_NODE_ADD_ACK */
+	scif_node_add_nack,	/* SCIF_NODE_ADD_NACK */
+	scif_node_remove,	/* SCIF_NODE_REMOVE */
+	scif_node_remove_ack,	/* SCIF_NODE_REMOVE_ACK */
+	scif_cnctreq,		/* SCIF_CNCT_REQ */
+	scif_cnctgnt,		/* SCIF_CNCT_GNT */
+	scif_cnctgnt_ack,	/* SCIF_CNCT_GNTACK */
+	scif_cnctgnt_nack,	/* SCIF_CNCT_GNTNACK */
+	scif_cnctrej,		/* SCIF_CNCT_REJ */
+	scif_discnct,		/* SCIF_DISCNCT */
+	scif_discnt_ack,	/* SCIF_DISCNT_ACK */
+	scif_clientsend,	/* SCIF_CLIENT_SENT */
+	scif_clientrcvd,	/* SCIF_CLIENT_RCVD */
+	scif_get_node_info_resp,/* SCIF_GET_NODE_INFO */
+};
+
+/**
+ * scif_nodeqp_msg_handler() - Common handler for node messages
+ * @scifdev: Remote device to respond to
+ * @qp: Remote memory pointer
+ * @msg: The message to be handled.
+ *
+ * This routine calls the appropriate routine to handle a Node Qp
+ * message receipt
+ */
+static int scif_max_msg_id = SCIF_MAX_MSG;
+
+static void
+scif_nodeqp_msg_handler(struct scif_dev *scifdev,
+			struct scif_qp *qp, struct scifmsg *msg)
+{
+	scif_display_message(scifdev, msg, "Rcvd");
+
+	if (msg->uop > (u32)scif_max_msg_id) {
+		/* Bogus Node Qp Message? */
+		dev_err(&scifdev->sdev->dev,
+			"Unknown message 0x%xn scifdev->node 0x%x\n",
+			msg->uop, scifdev->node);
+		return;
+	}
+
+	scif_intr_func[msg->uop](scifdev, msg);
+}
+
+/**
+ * scif_nodeqp_intrhandler() - Interrupt handler for node messages
+ * @scifdev:    Remote device to respond to
+ * @qp:         Remote memory pointer
+ *
+ * This routine is triggered by the interrupt mechanism.  It reads
+ * messages from the node queue RB and calls the Node QP Message handling
+ * routine.
+ */
+void scif_nodeqp_intrhandler(struct scif_dev *scifdev, struct scif_qp *qp)
+{
+	struct scifmsg msg;
+	int read_size;
+
+	do {
+		read_size = scif_rb_get_next(&qp->inbound_q, &msg, sizeof(msg));
+		if (!read_size)
+			break;
+		scif_nodeqp_msg_handler(scifdev, qp, &msg);
+		/*
+		 * The node queue pair is unmapped so skip the read pointer
+		 * update after receipt of a SCIF_EXIT_ACK
+		 */
+		if (SCIF_EXIT_ACK == msg.uop)
+			break;
+		scif_rb_update_read_ptr(&qp->inbound_q);
+	} while (1);
+}
+
+/**
+ * scif_loopb_wq_handler - Loopback Workqueue Handler.
+ * @work: loop back work
+ *
+ * This work queue routine is invoked by the loopback work queue handler.
+ * It grabs the recv lock, dequeues any available messages from the head
+ * of the loopback message list, calls the node QP message handler,
+ * waits for it to return, then frees up this message and dequeues more
+ * elements of the list if available.
+ */
+static void scif_loopb_wq_handler(struct work_struct *unused)
+{
+	struct scif_dev *scifdev = scif_info.loopb_dev;
+	struct scif_qp *qp = scifdev->qpairs;
+	struct scif_loopb_msg *msg;
+
+	do {
+		msg = NULL;
+		spin_lock(&qp->recv_lock);
+		if (!list_empty(&scif_info.loopb_recv_q)) {
+			msg = list_first_entry(&scif_info.loopb_recv_q,
+					       struct scif_loopb_msg,
+					       list);
+			list_del(&msg->list);
+		}
+		spin_unlock(&qp->recv_lock);
+
+		if (msg) {
+			scif_nodeqp_msg_handler(scifdev, qp, &msg->msg);
+			kfree(msg);
+		}
+	} while (msg);
+}
+
+/**
+ * scif_loopb_msg_handler() - Workqueue handler for loopback messages.
+ * @scifdev: SCIF device
+ * @qp: Queue pair.
+ *
+ * This work queue routine is triggered when a loopback message is received.
+ *
+ * We need special handling for receiving Node Qp messages on a loopback SCIF
+ * device via two workqueues for receiving messages.
+ *
+ * The reason we need the extra workqueue which is not required with *normal*
+ * non-loopback SCIF devices is the potential classic deadlock described below:
+ *
+ * Thread A tries to send a message on a loopback SCIF device and blocks since
+ * there is no space in the RB while it has the send_lock held or another
+ * lock called lock X for example.
+ *
+ * Thread B: The Loopback Node QP message receive workqueue receives the message
+ * and tries to send a message (eg an ACK) to the loopback SCIF device. It tries
+ * to grab the send lock again or lock X and deadlocks with Thread A. The RB
+ * cannot be drained any further due to this classic deadlock.
+ *
+ * In order to avoid deadlocks as mentioned above we have an extra level of
+ * indirection achieved by having two workqueues.
+ * 1) The first workqueue whose handler is scif_loopb_msg_handler reads
+ * messages from the Node QP RB, adds them to a list and queues work for the
+ * second workqueue.
+ *
+ * 2) The second workqueue whose handler is scif_loopb_wq_handler dequeues
+ * messages from the list, handles them, frees up the memory and dequeues
+ * more elements from the list if possible.
+ */
+int
+scif_loopb_msg_handler(struct scif_dev *scifdev, struct scif_qp *qp)
+{
+	int read_size;
+	struct scif_loopb_msg *msg;
+
+	do {
+		msg = kmalloc(sizeof(*msg), GFP_KERNEL);
+		if (!msg)
+			return -ENOMEM;
+		read_size = scif_rb_get_next(&qp->inbound_q, &msg->msg,
+					     sizeof(struct scifmsg));
+		if (read_size != sizeof(struct scifmsg)) {
+			kfree(msg);
+			scif_rb_update_read_ptr(&qp->inbound_q);
+			break;
+		}
+		spin_lock(&qp->recv_lock);
+		list_add_tail(&msg->list, &scif_info.loopb_recv_q);
+		spin_unlock(&qp->recv_lock);
+		queue_work(scif_info.loopb_wq, &scif_info.loopb_work);
+		scif_rb_update_read_ptr(&qp->inbound_q);
+	} while (read_size == sizeof(struct scifmsg));
+	return read_size;
+}
+
+/**
+ * scif_setup_loopback_qp - One time setup work for Loopback Node Qp.
+ * @scifdev: SCIF device
+ *
+ * Sets up the required loopback workqueues, queue pairs and ring buffers
+ */
+int scif_setup_loopback_qp(struct scif_dev *scifdev)
+{
+	int err = 0;
+	void *local_q;
+	struct scif_qp *qp;
+	struct scif_peer_dev *spdev;
+
+	err = scif_setup_intr_wq(scifdev);
+	if (err)
+		goto exit;
+	INIT_LIST_HEAD(&scif_info.loopb_recv_q);
+	snprintf(scif_info.loopb_wqname, sizeof(scif_info.loopb_wqname),
+		 "SCIF LOOPB %d", scifdev->node);
+	scif_info.loopb_wq =
+		alloc_ordered_workqueue(scif_info.loopb_wqname, 0);
+	if (!scif_info.loopb_wq) {
+		err = -ENOMEM;
+		goto destroy_intr;
+	}
+	INIT_WORK(&scif_info.loopb_work, scif_loopb_wq_handler);
+	/* Allocate Self Qpair */
+	scifdev->qpairs = kzalloc(sizeof(*scifdev->qpairs), GFP_KERNEL);
+	if (!scifdev->qpairs) {
+		err = -ENOMEM;
+		goto destroy_loopb_wq;
+	}
+
+	qp = scifdev->qpairs;
+	qp->magic = SCIFEP_MAGIC;
+	spin_lock_init(&qp->send_lock);
+	spin_lock_init(&qp->recv_lock);
+
+	local_q = kzalloc(SCIF_NODE_QP_SIZE, GFP_KERNEL);
+	if (!local_q) {
+		err = -ENOMEM;
+		goto free_qpairs;
+	}
+	/*
+	 * For loopback the inbound_q and outbound_q are essentially the same
+	 * since the Node sends a message on the loopback interface to the
+	 * outbound_q which is then received on the inbound_q.
+	 */
+	scif_rb_init(&qp->outbound_q,
+		     &qp->local_read,
+		     &qp->local_write,
+		     local_q, get_count_order(SCIF_NODE_QP_SIZE));
+
+	scif_rb_init(&qp->inbound_q,
+		     &qp->local_read,
+		     &qp->local_write,
+		     local_q, get_count_order(SCIF_NODE_QP_SIZE));
+	scif_info.nodeid = scifdev->node;
+	spdev = scif_peer_register_device(scifdev);
+	if (IS_ERR(spdev)) {
+		err = PTR_ERR(spdev);
+		goto free_local_q;
+	}
+	scif_info.loopb_dev = scifdev;
+	return err;
+free_local_q:
+	kfree(local_q);
+free_qpairs:
+	kfree(scifdev->qpairs);
+destroy_loopb_wq:
+	destroy_workqueue(scif_info.loopb_wq);
+destroy_intr:
+	scif_destroy_intr_wq(scifdev);
+exit:
+	return err;
+}
+
+/**
+ * scif_destroy_loopback_qp - One time uninit work for Loopback Node Qp
+ * @scifdev: SCIF device
+ *
+ * Destroys the workqueues and frees up the Ring Buffer and Queue Pair memory.
+ */
+int scif_destroy_loopback_qp(struct scif_dev *scifdev)
+{
+	struct scif_peer_dev *spdev;
+
+	rcu_read_lock();
+	spdev = rcu_dereference(scifdev->spdev);
+	rcu_read_unlock();
+	if (spdev)
+		scif_peer_unregister_device(spdev);
+	destroy_workqueue(scif_info.loopb_wq);
+	scif_destroy_intr_wq(scifdev);
+	kfree(scifdev->qpairs->outbound_q.rb_base);
+	kfree(scifdev->qpairs);
+	scifdev->sdev = NULL;
+	scif_info.loopb_dev = NULL;
+	return 0;
+}
+
+void scif_destroy_p2p(struct scif_dev *scifdev)
+{
+	struct scif_dev *peer_dev;
+	struct scif_p2p_info *p2p;
+	struct list_head *pos, *tmp;
+	int bd;
+
+	mutex_lock(&scif_info.conflock);
+	/* Free P2P mappings in the given node for all its peer nodes */
+	list_for_each_safe(pos, tmp, &scifdev->p2p) {
+		p2p = list_entry(pos, struct scif_p2p_info, ppi_list);
+		dma_unmap_sg(&scifdev->sdev->dev, p2p->ppi_sg[SCIF_PPI_MMIO],
+			     p2p->sg_nentries[SCIF_PPI_MMIO],
+			     DMA_BIDIRECTIONAL);
+		dma_unmap_sg(&scifdev->sdev->dev, p2p->ppi_sg[SCIF_PPI_APER],
+			     p2p->sg_nentries[SCIF_PPI_APER],
+			     DMA_BIDIRECTIONAL);
+		scif_p2p_freesg(p2p->ppi_sg[SCIF_PPI_MMIO]);
+		scif_p2p_freesg(p2p->ppi_sg[SCIF_PPI_APER]);
+		list_del(pos);
+		kfree(p2p);
+	}
+
+	/* Free P2P mapping created in the peer nodes for the given node */
+	for (bd = SCIF_MGMT_NODE + 1; bd <= scif_info.maxid; bd++) {
+		peer_dev = &scif_dev[bd];
+		list_for_each_safe(pos, tmp, &peer_dev->p2p) {
+			p2p = list_entry(pos, struct scif_p2p_info, ppi_list);
+			if (p2p->ppi_peer_id == scifdev->node) {
+				dma_unmap_sg(&peer_dev->sdev->dev,
+					     p2p->ppi_sg[SCIF_PPI_MMIO],
+					     p2p->sg_nentries[SCIF_PPI_MMIO],
+					     DMA_BIDIRECTIONAL);
+				dma_unmap_sg(&peer_dev->sdev->dev,
+					     p2p->ppi_sg[SCIF_PPI_APER],
+					     p2p->sg_nentries[SCIF_PPI_APER],
+					     DMA_BIDIRECTIONAL);
+				scif_p2p_freesg(p2p->ppi_sg[SCIF_PPI_MMIO]);
+				scif_p2p_freesg(p2p->ppi_sg[SCIF_PPI_APER]);
+				list_del(pos);
+				kfree(p2p);
+			}
+		}
+	}
+	mutex_unlock(&scif_info.conflock);
+}
diff --git a/drivers/misc/mic/scif/scif_nodeqp.h b/drivers/misc/mic/scif/scif_nodeqp.h
new file mode 100644
index 000000000000..6c0ed6783479
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_nodeqp.h
@@ -0,0 +1,183 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in
+ *   the documentation and/or other materials provided with the
+ *   distribution.
+ * * Neither the name of Intel Corporation nor the names of its
+ *   contributors may be used to endorse or promote products derived
+ *   from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#ifndef SCIF_NODEQP
+#define SCIF_NODEQP
+
+#include "scif_rb.h"
+#include "scif_peer_bus.h"
+
+#define SCIF_INIT 1  /* First message sent to the peer node for discovery */
+#define SCIF_EXIT 2  /* Last message from the peer informing intent to exit */
+#define SCIF_EXIT_ACK 3 /* Response to SCIF_EXIT message */
+#define SCIF_NODE_ADD 4  /* Tell Online nodes a new node exits */
+#define SCIF_NODE_ADD_ACK 5  /* Confirm to mgmt node sequence is finished */
+#define SCIF_NODE_ADD_NACK 6 /* SCIF_NODE_ADD failed */
+#define SCIF_NODE_REMOVE 7 /* Request to deactivate a SCIF node */
+#define SCIF_NODE_REMOVE_ACK 8 /* Response to a SCIF_NODE_REMOVE message */
+#define SCIF_CNCT_REQ 9  /* Phys addr of Request connection to a port */
+#define SCIF_CNCT_GNT 10  /* Phys addr of new Grant connection request */
+#define SCIF_CNCT_GNTACK 11  /* Error type Reject a connection request */
+#define SCIF_CNCT_GNTNACK 12  /* Error type Reject a connection request */
+#define SCIF_CNCT_REJ 13  /* Error type Reject a connection request */
+#define SCIF_DISCNCT 14 /* Notify peer that connection is being terminated */
+#define SCIF_DISCNT_ACK 15 /* Notify peer that connection is being terminated */
+#define SCIF_CLIENT_SENT 16 /* Notify the peer that data has been written */
+#define SCIF_CLIENT_RCVD 17 /* Notify the peer that data has been read */
+#define SCIF_GET_NODE_INFO 18 /* Get current node mask from the mgmt node*/
+#define SCIF_MAX_MSG SCIF_GET_NODE_INFO
+
+/*
+ * struct scifmsg - Node QP message format
+ *
+ * @src: Source information
+ * @dst: Destination information
+ * @uop: The message opcode
+ * @payload: Unique payload format for each message
+ */
+struct scifmsg {
+	struct scif_port_id src;
+	struct scif_port_id dst;
+	u32 uop;
+	u64 payload[4];
+} __packed;
+
+/*
+ * struct scif_qp - Node Queue Pair
+ *
+ * Interesting structure -- a little difficult because we can only
+ * write across the PCIe, so any r/w pointer we need to read is
+ * local. We only need to read the read pointer on the inbound_q
+ * and read the write pointer in the outbound_q
+ *
+ * @magic: Magic value to ensure the peer sees the QP correctly
+ * @outbound_q: The outbound ring buffer for sending messages
+ * @inbound_q: The inbound ring buffer for receiving messages
+ * @local_write: Local write index
+ * @local_read: Local read index
+ * @remote_qp: The remote queue pair
+ * @local_buf: DMA address of local ring buffer
+ * @local_qp: DMA address of the local queue pair data structure
+ * @remote_buf: DMA address of remote ring buffer
+ * @qp_state: QP state i.e. online or offline used for P2P
+ * @send_lock: synchronize access to outbound queue
+ * @recv_lock: Synchronize access to inbound queue
+ */
+struct scif_qp {
+	u64 magic;
+#define SCIFEP_MAGIC 0x5c1f000000005c1fULL
+	struct scif_rb outbound_q;
+	struct scif_rb inbound_q;
+
+	u32 local_write __aligned(64);
+	u32 local_read __aligned(64);
+	struct scif_qp *remote_qp;
+	dma_addr_t local_buf;
+	dma_addr_t local_qp;
+	dma_addr_t remote_buf;
+	u32 qp_state;
+#define SCIF_QP_OFFLINE 0xdead
+#define SCIF_QP_ONLINE 0xc0de
+	spinlock_t send_lock;
+	spinlock_t recv_lock;
+};
+
+/*
+ * struct scif_loopb_msg - An element in the loopback Node QP message list.
+ *
+ * @msg - The SCIF node QP message
+ * @list - link in the list of messages
+ */
+struct scif_loopb_msg {
+	struct scifmsg msg;
+	struct list_head list;
+};
+
+int scif_nodeqp_send(struct scif_dev *scifdev, struct scifmsg *msg);
+int _scif_nodeqp_send(struct scif_dev *scifdev, struct scifmsg *msg);
+void scif_nodeqp_intrhandler(struct scif_dev *scifdev, struct scif_qp *qp);
+int scif_loopb_msg_handler(struct scif_dev *scifdev, struct scif_qp *qp);
+int scif_setup_qp(struct scif_dev *scifdev);
+int scif_qp_response(phys_addr_t phys, struct scif_dev *dev);
+int scif_setup_qp_connect(struct scif_qp *qp, dma_addr_t *qp_offset,
+			  int local_size, struct scif_dev *scifdev);
+int scif_setup_qp_accept(struct scif_qp *qp, dma_addr_t *qp_offset,
+			 dma_addr_t phys, int local_size,
+			 struct scif_dev *scifdev);
+int scif_setup_qp_connect_response(struct scif_dev *scifdev,
+				   struct scif_qp *qp, u64 payload);
+int scif_setup_loopback_qp(struct scif_dev *scifdev);
+int scif_destroy_loopback_qp(struct scif_dev *scifdev);
+void scif_poll_qp_state(struct work_struct *work);
+void scif_qp_response_ack(struct work_struct *work);
+void scif_destroy_p2p(struct scif_dev *scifdev);
+void scif_send_exit(struct scif_dev *scifdev);
+static inline struct device *scif_get_peer_dev(struct scif_dev *scifdev)
+{
+	struct scif_peer_dev *spdev;
+	struct device *spdev_ret;
+
+	rcu_read_lock();
+	spdev = rcu_dereference(scifdev->spdev);
+	if (spdev)
+		spdev_ret = get_device(&spdev->dev);
+	else
+		spdev_ret = ERR_PTR(-ENODEV);
+	rcu_read_unlock();
+	return spdev_ret;
+}
+
+static inline void scif_put_peer_dev(struct device *dev)
+{
+	put_device(dev);
+}
+#endif  /* SCIF_NODEQP */
diff --git a/drivers/misc/mic/scif/scif_peer_bus.c b/drivers/misc/mic/scif/scif_peer_bus.c
new file mode 100644
index 000000000000..589ae9ad2501
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_peer_bus.c
@@ -0,0 +1,124 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ */
+#include "scif_main.h"
+#include "../bus/scif_bus.h"
+#include "scif_peer_bus.h"
+
+static inline struct scif_peer_dev *
+dev_to_scif_peer(struct device *dev)
+{
+	return container_of(dev, struct scif_peer_dev, dev);
+}
+
+static inline struct scif_peer_driver *
+drv_to_scif_peer(struct device_driver *drv)
+{
+	return container_of(drv, struct scif_peer_driver, driver);
+}
+
+static int scif_peer_dev_match(struct device *dv, struct device_driver *dr)
+{
+	return !strncmp(dev_name(dv), dr->name, 4);
+}
+
+static int scif_peer_dev_probe(struct device *d)
+{
+	struct scif_peer_dev *dev = dev_to_scif_peer(d);
+	struct scif_peer_driver *drv = drv_to_scif_peer(dev->dev.driver);
+
+	return drv->probe(dev);
+}
+
+static int scif_peer_dev_remove(struct device *d)
+{
+	struct scif_peer_dev *dev = dev_to_scif_peer(d);
+	struct scif_peer_driver *drv = drv_to_scif_peer(dev->dev.driver);
+
+	drv->remove(dev);
+	return 0;
+}
+
+static struct bus_type scif_peer_bus = {
+	.name  = "scif_peer_bus",
+	.match = scif_peer_dev_match,
+	.probe = scif_peer_dev_probe,
+	.remove = scif_peer_dev_remove,
+};
+
+int scif_peer_register_driver(struct scif_peer_driver *driver)
+{
+	driver->driver.bus = &scif_peer_bus;
+	return driver_register(&driver->driver);
+}
+
+void scif_peer_unregister_driver(struct scif_peer_driver *driver)
+{
+	driver_unregister(&driver->driver);
+}
+
+static void scif_peer_release_dev(struct device *d)
+{
+	struct scif_peer_dev *sdev = dev_to_scif_peer(d);
+	struct scif_dev *scifdev = &scif_dev[sdev->dnode];
+
+	scif_cleanup_scifdev(scifdev);
+	kfree(sdev);
+}
+
+struct scif_peer_dev *
+scif_peer_register_device(struct scif_dev *scifdev)
+{
+	int ret;
+	struct scif_peer_dev *spdev;
+
+	spdev = kzalloc(sizeof(*spdev), GFP_KERNEL);
+	if (!spdev)
+		return ERR_PTR(-ENOMEM);
+
+	spdev->dev.parent = scifdev->sdev->dev.parent;
+	spdev->dev.release = scif_peer_release_dev;
+	spdev->dnode = scifdev->node;
+	spdev->dev.bus = &scif_peer_bus;
+
+	dev_set_name(&spdev->dev, "scif_peer-dev%u", spdev->dnode);
+	/*
+	 * device_register() causes the bus infrastructure to look for a
+	 * matching driver.
+	 */
+	ret = device_register(&spdev->dev);
+	if (ret)
+		goto free_spdev;
+	return spdev;
+free_spdev:
+	kfree(spdev);
+	return ERR_PTR(ret);
+}
+
+void scif_peer_unregister_device(struct scif_peer_dev *sdev)
+{
+	device_unregister(&sdev->dev);
+}
+
+int scif_peer_bus_init(void)
+{
+	return bus_register(&scif_peer_bus);
+}
+
+void scif_peer_bus_exit(void)
+{
+	bus_unregister(&scif_peer_bus);
+}
diff --git a/drivers/misc/mic/scif/scif_peer_bus.h b/drivers/misc/mic/scif/scif_peer_bus.h
new file mode 100644
index 000000000000..33f0dbb30152
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_peer_bus.h
@@ -0,0 +1,65 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ */
+#ifndef _SCIF_PEER_BUS_H_
+#define _SCIF_PEER_BUS_H_
+
+#include <linux/device.h>
+#include <linux/mic_common.h>
+
+/*
+ * Peer devices show up as PCIe devices for the mgmt node but not the cards.
+ * The mgmt node discovers all the cards on the PCIe bus and informs the other
+ * cards about their peers. Upon notification of a peer a node adds a peer
+ * device to the peer bus to maintain symmetry in the way devices are
+ * discovered across all nodes in the SCIF network.
+ */
+/**
+ * scif_peer_dev - representation of a peer SCIF device
+ * @dev: underlying device
+ * @dnode - The destination node which this device will communicate with.
+ */
+struct scif_peer_dev {
+	struct device dev;
+	u8 dnode;
+};
+
+/**
+ * scif_peer_driver - operations for a scif_peer I/O driver
+ * @driver: underlying device driver (populate name and owner).
+ * @id_table: the ids serviced by this driver.
+ * @probe: the function to call when a device is found.  Returns 0 or -errno.
+ * @remove: the function to call when a device is removed.
+ */
+struct scif_peer_driver {
+	struct device_driver driver;
+	const struct scif_peer_dev_id *id_table;
+
+	int (*probe)(struct scif_peer_dev *dev);
+	void (*remove)(struct scif_peer_dev *dev);
+};
+
+struct scif_dev;
+
+int scif_peer_register_driver(struct scif_peer_driver *driver);
+void scif_peer_unregister_driver(struct scif_peer_driver *driver);
+
+struct scif_peer_dev *scif_peer_register_device(struct scif_dev *sdev);
+void scif_peer_unregister_device(struct scif_peer_dev *sdev);
+
+int scif_peer_bus_init(void);
+void scif_peer_bus_exit(void);
+#endif /* _SCIF_PEER_BUS_H */
diff --git a/drivers/misc/mic/scif/scif_ports.c b/drivers/misc/mic/scif/scif_ports.c
new file mode 100644
index 000000000000..594e18d279d8
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_ports.c
@@ -0,0 +1,124 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include <linux/idr.h>
+
+#include "scif_main.h"
+
+#define SCIF_PORT_COUNT	0x10000	/* Ports available */
+
+struct idr scif_ports;
+
+/*
+ * struct scif_port - SCIF port information
+ *
+ * @ref_cnt - Reference count since there can be multiple endpoints
+ *		created via scif_accept(..) simultaneously using a port.
+ */
+struct scif_port {
+	int ref_cnt;
+};
+
+/**
+ * __scif_get_port - Reserve a specified port # for SCIF and add it
+ * to the global list.
+ * @port : port # to be reserved.
+ *
+ * @return : Allocated SCIF port #, or -ENOSPC if port unavailable.
+ *		On memory allocation failure, returns -ENOMEM.
+ */
+static int __scif_get_port(int start, int end)
+{
+	int id;
+	struct scif_port *port = kzalloc(sizeof(*port), GFP_ATOMIC);
+
+	if (!port)
+		return -ENOMEM;
+	spin_lock(&scif_info.port_lock);
+	id = idr_alloc(&scif_ports, port, start, end, GFP_ATOMIC);
+	if (id >= 0)
+		port->ref_cnt++;
+	spin_unlock(&scif_info.port_lock);
+	return id;
+}
+
+/**
+ * scif_rsrv_port - Reserve a specified port # for SCIF.
+ * @port : port # to be reserved.
+ *
+ * @return : Allocated SCIF port #, or -ENOSPC if port unavailable.
+ *		On memory allocation failure, returns -ENOMEM.
+ */
+int scif_rsrv_port(u16 port)
+{
+	return __scif_get_port(port, port + 1);
+}
+
+/**
+ * scif_get_new_port - Get and reserve any port # for SCIF in the range
+ *			SCIF_PORT_RSVD + 1 to SCIF_PORT_COUNT - 1.
+ *
+ * @return : Allocated SCIF port #, or -ENOSPC if no ports available.
+ *		On memory allocation failure, returns -ENOMEM.
+ */
+int scif_get_new_port(void)
+{
+	return __scif_get_port(SCIF_PORT_RSVD + 1, SCIF_PORT_COUNT);
+}
+
+/**
+ * scif_get_port - Increment the reference count for a SCIF port
+ * @id : SCIF port
+ *
+ * @return : None
+ */
+void scif_get_port(u16 id)
+{
+	struct scif_port *port;
+
+	if (!id)
+		return;
+	spin_lock(&scif_info.port_lock);
+	port = idr_find(&scif_ports, id);
+	if (port)
+		port->ref_cnt++;
+	spin_unlock(&scif_info.port_lock);
+}
+
+/**
+ * scif_put_port - Release a reserved SCIF port
+ * @id : SCIF port to be released.
+ *
+ * @return : None
+ */
+void scif_put_port(u16 id)
+{
+	struct scif_port *port;
+
+	if (!id)
+		return;
+	spin_lock(&scif_info.port_lock);
+	port = idr_find(&scif_ports, id);
+	if (port) {
+		port->ref_cnt--;
+		if (!port->ref_cnt) {
+			idr_remove(&scif_ports, id);
+			kfree(port);
+		}
+	}
+	spin_unlock(&scif_info.port_lock);
+}
diff --git a/drivers/misc/mic/scif/scif_rb.c b/drivers/misc/mic/scif/scif_rb.c
new file mode 100644
index 000000000000..637cc4686742
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_rb.c
@@ -0,0 +1,249 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, version 2, as
+ * 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.
+ *
+ * Intel SCIF driver.
+ *
+ */
+#include <linux/circ_buf.h>
+#include <linux/types.h>
+#include <linux/io.h>
+#include <linux/errno.h>
+
+#include "scif_rb.h"
+
+#define scif_rb_ring_cnt(head, tail, size) CIRC_CNT(head, tail, size)
+#define scif_rb_ring_space(head, tail, size) CIRC_SPACE(head, tail, size)
+
+/**
+ * scif_rb_init - Initializes the ring buffer
+ * @rb: ring buffer
+ * @read_ptr: A pointer to the read offset
+ * @write_ptr: A pointer to the write offset
+ * @rb_base: A pointer to the base of the ring buffer
+ * @size: The size of the ring buffer in powers of two
+ */
+void scif_rb_init(struct scif_rb *rb, u32 *read_ptr, u32 *write_ptr,
+		  void *rb_base, u8 size)
+{
+	rb->rb_base = rb_base;
+	rb->size = (1 << size);
+	rb->read_ptr = read_ptr;
+	rb->write_ptr = write_ptr;
+	rb->current_read_offset = *read_ptr;
+	rb->current_write_offset = *write_ptr;
+}
+
+/* Copies a message to the ring buffer -- handles the wrap around case */
+static void memcpy_torb(struct scif_rb *rb, void *header,
+			void *msg, u32 size)
+{
+	u32 size1, size2;
+
+	if (header + size >= rb->rb_base + rb->size) {
+		/* Need to call two copies if it wraps around */
+		size1 = (u32)(rb->rb_base + rb->size - header);
+		size2 = size - size1;
+		memcpy_toio((void __iomem __force *)header, msg, size1);
+		memcpy_toio((void __iomem __force *)rb->rb_base,
+			    msg + size1, size2);
+	} else {
+		memcpy_toio((void __iomem __force *)header, msg, size);
+	}
+}
+
+/* Copies a message from the ring buffer -- handles the wrap around case */
+static void memcpy_fromrb(struct scif_rb *rb, void *header,
+			  void *msg, u32 size)
+{
+	u32 size1, size2;
+
+	if (header + size >= rb->rb_base + rb->size) {
+		/* Need to call two copies if it wraps around */
+		size1 = (u32)(rb->rb_base + rb->size - header);
+		size2 = size - size1;
+		memcpy_fromio(msg, (void __iomem __force *)header, size1);
+		memcpy_fromio(msg + size1,
+			      (void __iomem __force *)rb->rb_base, size2);
+	} else {
+		memcpy_fromio(msg, (void __iomem __force *)header, size);
+	}
+}
+
+/**
+ * scif_rb_space - Query space available for writing to the RB
+ * @rb: ring buffer
+ *
+ * Return: size available for writing to RB in bytes.
+ */
+u32 scif_rb_space(struct scif_rb *rb)
+{
+	rb->current_read_offset = *rb->read_ptr;
+	/*
+	 * Update from the HW read pointer only once the peer has exposed the
+	 * new empty slot. This barrier is paired with the memory barrier
+	 * scif_rb_update_read_ptr()
+	 */
+	mb();
+	return scif_rb_ring_space(rb->current_write_offset,
+				  rb->current_read_offset, rb->size);
+}
+
+/**
+ * scif_rb_write - Write a message to the RB
+ * @rb: ring buffer
+ * @msg: buffer to send the message.  Must be at least size bytes long
+ * @size: the size (in bytes) to be copied to the RB
+ *
+ * This API does not block if there isn't enough space in the RB.
+ * Returns: 0 on success or -ENOMEM on failure
+ */
+int scif_rb_write(struct scif_rb *rb, void *msg, u32 size)
+{
+	void *header;
+
+	if (scif_rb_space(rb) < size)
+		return -ENOMEM;
+	header = rb->rb_base + rb->current_write_offset;
+	memcpy_torb(rb, header, msg, size);
+	/*
+	 * Wait until scif_rb_commit(). Update the local ring
+	 * buffer data, not the shared data until commit.
+	 */
+	rb->current_write_offset =
+		(rb->current_write_offset + size) & (rb->size - 1);
+	return 0;
+}
+
+/**
+ * scif_rb_commit - To submit the message to let the peer fetch it
+ * @rb: ring buffer
+ */
+void scif_rb_commit(struct scif_rb *rb)
+{
+	/*
+	 * We must ensure ordering between the all the data committed
+	 * previously before we expose the new message to the peer by
+	 * updating the write_ptr. This write barrier is paired with
+	 * the read barrier in scif_rb_count(..)
+	 */
+	wmb();
+	ACCESS_ONCE(*rb->write_ptr) = rb->current_write_offset;
+#ifdef CONFIG_INTEL_MIC_CARD
+	/*
+	 * X100 Si bug: For the case where a Core is performing an EXT_WR
+	 * followed by a Doorbell Write, the Core must perform two EXT_WR to the
+	 * same address with the same data before it does the Doorbell Write.
+	 * This way, if ordering is violated for the Interrupt Message, it will
+	 * fall just behind the first Posted associated with the first EXT_WR.
+	 */
+	ACCESS_ONCE(*rb->write_ptr) = rb->current_write_offset;
+#endif
+}
+
+/**
+ * scif_rb_get - To get next message from the ring buffer
+ * @rb: ring buffer
+ * @size: Number of bytes to be read
+ *
+ * Return: NULL if no bytes to be read from the ring buffer, otherwise the
+ *	pointer to the next byte
+ */
+static void *scif_rb_get(struct scif_rb *rb, u32 size)
+{
+	void *header = NULL;
+
+	if (scif_rb_count(rb, size) >= size)
+		header = rb->rb_base + rb->current_read_offset;
+	return header;
+}
+
+/*
+ * scif_rb_get_next - Read from ring buffer.
+ * @rb: ring buffer
+ * @msg: buffer to hold the message.  Must be at least size bytes long
+ * @size: Number of bytes to be read
+ *
+ * Return: number of bytes read if available bytes are >= size, otherwise
+ * returns zero.
+ */
+u32 scif_rb_get_next(struct scif_rb *rb, void *msg, u32 size)
+{
+	void *header = NULL;
+	int read_size = 0;
+
+	header = scif_rb_get(rb, size);
+	if (header) {
+		u32 next_cmd_offset =
+			(rb->current_read_offset + size) & (rb->size - 1);
+
+		read_size = size;
+		rb->current_read_offset = next_cmd_offset;
+		memcpy_fromrb(rb, header, msg, size);
+	}
+	return read_size;
+}
+
+/**
+ * scif_rb_update_read_ptr
+ * @rb: ring buffer
+ */
+void scif_rb_update_read_ptr(struct scif_rb *rb)
+{
+	u32 new_offset;
+
+	new_offset = rb->current_read_offset;
+	/*
+	 * We must ensure ordering between the all the data committed or read
+	 * previously before we expose the empty slot to the peer by updating
+	 * the read_ptr. This barrier is paired with the memory barrier in
+	 * scif_rb_space(..)
+	 */
+	mb();
+	ACCESS_ONCE(*rb->read_ptr) = new_offset;
+#ifdef CONFIG_INTEL_MIC_CARD
+	/*
+	 * X100 Si Bug: For the case where a Core is performing an EXT_WR
+	 * followed by a Doorbell Write, the Core must perform two EXT_WR to the
+	 * same address with the same data before it does the Doorbell Write.
+	 * This way, if ordering is violated for the Interrupt Message, it will
+	 * fall just behind the first Posted associated with the first EXT_WR.
+	 */
+	ACCESS_ONCE(*rb->read_ptr) = new_offset;
+#endif
+}
+
+/**
+ * scif_rb_count
+ * @rb: ring buffer
+ * @size: Number of bytes expected to be read
+ *
+ * Return: number of bytes that can be read from the RB
+ */
+u32 scif_rb_count(struct scif_rb *rb, u32 size)
+{
+	if (scif_rb_ring_cnt(rb->current_write_offset,
+			     rb->current_read_offset,
+			     rb->size) < size) {
+		rb->current_write_offset = *rb->write_ptr;
+		/*
+		 * Update from the HW write pointer if empty only once the peer
+		 * has exposed the new message. This read barrier is paired
+		 * with the write barrier in scif_rb_commit(..)
+		 */
+		smp_rmb();
+	}
+	return scif_rb_ring_cnt(rb->current_write_offset,
+				rb->current_read_offset,
+				rb->size);
+}
diff --git a/drivers/misc/mic/scif/scif_rb.h b/drivers/misc/mic/scif/scif_rb.h
new file mode 100644
index 000000000000..166dffe3093d
--- /dev/null
+++ b/drivers/misc/mic/scif/scif_rb.h
@@ -0,0 +1,100 @@
+/*
+ * Intel MIC Platform Software Stack (MPSS)
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2014 Intel Corporation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in
+ *   the documentation and/or other materials provided with the
+ *   distribution.
+ * * Neither the name of Intel Corporation nor the names of its
+ *   contributors may be used to endorse or promote products derived
+ *   from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Intel SCIF driver.
+ */
+#ifndef SCIF_RB_H
+#define SCIF_RB_H
+/*
+ * This file describes a general purpose, byte based ring buffer. Writers to the
+ * ring buffer need to synchronize using a lock. The same is true for readers,
+ * although in practice, the ring buffer has a single reader. It is lockless
+ * between producer and consumer so it can handle being used across the PCIe
+ * bus. The ring buffer ensures that there are no reads across the PCIe bus for
+ * performance reasons. Two of these are used to form a single bidirectional
+ * queue-pair across PCIe.
+ */
+/*
+ * struct scif_rb - SCIF Ring Buffer
+ *
+ * @rb_base: The base of the memory used for storing RB messages
+ * @read_ptr: Pointer to the read offset
+ * @write_ptr: Pointer to the write offset
+ * @size: Size of the memory in rb_base
+ * @current_read_offset: Cached read offset for performance
+ * @current_write_offset: Cached write offset for performance
+ */
+struct scif_rb {
+	void *rb_base;
+	u32 *read_ptr;
+	u32 *write_ptr;
+	u32 size;
+	u32 current_read_offset;
+	u32 current_write_offset;
+};
+
+/* methods used by both */
+void scif_rb_init(struct scif_rb *rb, u32 *read_ptr, u32 *write_ptr,
+		  void *rb_base, u8 size);
+/* writer only methods */
+/* write a new command, then scif_rb_commit() */
+int scif_rb_write(struct scif_rb *rb, void *msg, u32 size);
+/* after write(), then scif_rb_commit() */
+void scif_rb_commit(struct scif_rb *rb);
+/* query space available for writing to a RB. */
+u32 scif_rb_space(struct scif_rb *rb);
+
+/* reader only methods */
+/* read a new message from the ring buffer of size bytes */
+u32 scif_rb_get_next(struct scif_rb *rb, void *msg, u32 size);
+/* update the read pointer so that the space can be reused */
+void scif_rb_update_read_ptr(struct scif_rb *rb);
+/* count the number of bytes that can be read */
+u32 scif_rb_count(struct scif_rb *rb, u32 size);
+#endif
diff --git a/drivers/misc/spear13xx_pcie_gadget.c b/drivers/misc/spear13xx_pcie_gadget.c
index fe3ad0ca9a3e..b8374cdaf9c9 100644
--- a/drivers/misc/spear13xx_pcie_gadget.c
+++ b/drivers/misc/spear13xx_pcie_gadget.c
@@ -2,7 +2,7 @@
  * drivers/misc/spear13xx_pcie_gadget.c
  *
  * Copyright (C) 2010 ST Microelectronics
- * Pratyush Anand<pratyush.anand@st.com>
+ * Pratyush Anand<pratyush.anand@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
diff --git a/drivers/misc/sram.c b/drivers/misc/sram.c
index eeaaf5fca105..15c33cc34a80 100644
--- a/drivers/misc/sram.c
+++ b/drivers/misc/sram.c
@@ -18,23 +18,20 @@
  * MA 02110-1301, USA.
  */
 
-#include <linux/kernel.h>
-#include <linux/init.h>
 #include <linux/clk.h>
-#include <linux/err.h>
+#include <linux/genalloc.h>
 #include <linux/io.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
-#include <linux/list.h>
 #include <linux/list_sort.h>
+#include <linux/of_address.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/spinlock.h>
-#include <linux/genalloc.h>
 
 #define SRAM_GRANULARITY	32
 
 struct sram_dev {
+	struct device *dev;
+	void __iomem *virt_base;
+
 	struct gen_pool *pool;
 	struct clk *clk;
 };
@@ -54,62 +51,27 @@ static int sram_reserve_cmp(void *priv, struct list_head *a,
 	return ra->start - rb->start;
 }
 
-static int sram_probe(struct platform_device *pdev)
+static int sram_reserve_regions(struct sram_dev *sram, struct resource *res)
 {
-	void __iomem *virt_base;
-	struct sram_dev *sram;
-	struct resource *res;
-	struct device_node *np = pdev->dev.of_node, *child;
+	struct device_node *np = sram->dev->of_node, *child;
 	unsigned long size, cur_start, cur_size;
 	struct sram_reserve *rblocks, *block;
 	struct list_head reserve_list;
 	unsigned int nblocks;
-	int ret;
+	int ret = 0;
 
 	INIT_LIST_HEAD(&reserve_list);
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "found no memory resource\n");
-		return -EINVAL;
-	}
-
 	size = resource_size(res);
 
-	if (!devm_request_mem_region(&pdev->dev,
-			res->start, size, pdev->name)) {
-		dev_err(&pdev->dev, "could not request region for resource\n");
-		return -EBUSY;
-	}
-
-	virt_base = devm_ioremap_wc(&pdev->dev, res->start, size);
-	if (IS_ERR(virt_base))
-		return PTR_ERR(virt_base);
-
-	sram = devm_kzalloc(&pdev->dev, sizeof(*sram), GFP_KERNEL);
-	if (!sram)
-		return -ENOMEM;
-
-	sram->clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(sram->clk))
-		sram->clk = NULL;
-	else
-		clk_prepare_enable(sram->clk);
-
-	sram->pool = devm_gen_pool_create(&pdev->dev, ilog2(SRAM_GRANULARITY), -1);
-	if (!sram->pool)
-		return -ENOMEM;
-
 	/*
 	 * We need an additional block to mark the end of the memory region
 	 * after the reserved blocks from the dt are processed.
 	 */
 	nblocks = (np) ? of_get_available_child_count(np) + 1 : 1;
 	rblocks = kmalloc((nblocks) * sizeof(*rblocks), GFP_KERNEL);
-	if (!rblocks) {
-		ret = -ENOMEM;
-		goto err_alloc;
-	}
+	if (!rblocks)
+		return -ENOMEM;
 
 	block = &rblocks[0];
 	for_each_available_child_of_node(np, child) {
@@ -117,17 +79,19 @@ static int sram_probe(struct platform_device *pdev)
 
 		ret = of_address_to_resource(child, 0, &child_res);
 		if (ret < 0) {
-			dev_err(&pdev->dev,
+			dev_err(sram->dev,
 				"could not get address for node %s\n",
 				child->full_name);
+			of_node_put(child);
 			goto err_chunks;
 		}
 
 		if (child_res.start < res->start || child_res.end > res->end) {
-			dev_err(&pdev->dev,
+			dev_err(sram->dev,
 				"reserved block %s outside the sram area\n",
 				child->full_name);
 			ret = -EINVAL;
+			of_node_put(child);
 			goto err_chunks;
 		}
 
@@ -135,9 +99,8 @@ static int sram_probe(struct platform_device *pdev)
 		block->size = resource_size(&child_res);
 		list_add_tail(&block->list, &reserve_list);
 
-		dev_dbg(&pdev->dev, "found reserved block 0x%x-0x%x\n",
-			block->start,
-			block->start + block->size);
+		dev_dbg(sram->dev, "found reserved block 0x%x-0x%x\n",
+			block->start, block->start + block->size);
 
 		block++;
 	}
@@ -154,7 +117,7 @@ static int sram_probe(struct platform_device *pdev)
 	list_for_each_entry(block, &reserve_list, list) {
 		/* can only happen if sections overlap */
 		if (block->start < cur_start) {
-			dev_err(&pdev->dev,
+			dev_err(sram->dev,
 				"block at 0x%x starts after current offset 0x%lx\n",
 				block->start, cur_start);
 			ret = -EINVAL;
@@ -174,10 +137,11 @@ static int sram_probe(struct platform_device *pdev)
 		 */
 		cur_size = block->start - cur_start;
 
-		dev_dbg(&pdev->dev, "adding chunk 0x%lx-0x%lx\n",
+		dev_dbg(sram->dev, "adding chunk 0x%lx-0x%lx\n",
 			cur_start, cur_start + cur_size);
+
 		ret = gen_pool_add_virt(sram->pool,
-				(unsigned long)virt_base + cur_start,
+				(unsigned long)sram->virt_base + cur_start,
 				res->start + cur_start, cur_size, -1);
 		if (ret < 0)
 			goto err_chunks;
@@ -186,20 +150,63 @@ static int sram_probe(struct platform_device *pdev)
 		cur_start = block->start + block->size;
 	}
 
+ err_chunks:
 	kfree(rblocks);
 
+	return ret;
+}
+
+static int sram_probe(struct platform_device *pdev)
+{
+	struct sram_dev *sram;
+	struct resource *res;
+	size_t size;
+	int ret;
+
+	sram = devm_kzalloc(&pdev->dev, sizeof(*sram), GFP_KERNEL);
+	if (!sram)
+		return -ENOMEM;
+
+	sram->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(sram->dev, "found no memory resource\n");
+		return -EINVAL;
+	}
+
+	size = resource_size(res);
+
+	if (!devm_request_mem_region(sram->dev, res->start, size, pdev->name)) {
+		dev_err(sram->dev, "could not request region for resource\n");
+		return -EBUSY;
+	}
+
+	sram->virt_base = devm_ioremap_wc(sram->dev, res->start, size);
+	if (IS_ERR(sram->virt_base))
+		return PTR_ERR(sram->virt_base);
+
+	sram->pool = devm_gen_pool_create(sram->dev,
+					  ilog2(SRAM_GRANULARITY), -1);
+	if (!sram->pool)
+		return -ENOMEM;
+
+	ret = sram_reserve_regions(sram, res);
+	if (ret)
+		return ret;
+
+	sram->clk = devm_clk_get(sram->dev, NULL);
+	if (IS_ERR(sram->clk))
+		sram->clk = NULL;
+	else
+		clk_prepare_enable(sram->clk);
+
 	platform_set_drvdata(pdev, sram);
 
-	dev_dbg(&pdev->dev, "SRAM pool: %ld KiB @ 0x%p\n", size / 1024, virt_base);
+	dev_dbg(sram->dev, "SRAM pool: %zu KiB @ 0x%p\n",
+		gen_pool_size(sram->pool) / 1024, sram->virt_base);
 
 	return 0;
-
-err_chunks:
-	kfree(rblocks);
-err_alloc:
-	if (sram->clk)
-		clk_disable_unprepare(sram->clk);
-	return ret;
 }
 
 static int sram_remove(struct platform_device *pdev)
@@ -207,7 +214,7 @@ static int sram_remove(struct platform_device *pdev)
 	struct sram_dev *sram = platform_get_drvdata(pdev);
 
 	if (gen_pool_avail(sram->pool) < gen_pool_size(sram->pool))
-		dev_dbg(&pdev->dev, "removed while SRAM allocated\n");
+		dev_err(sram->dev, "removed while SRAM allocated\n");
 
 	if (sram->clk)
 		clk_disable_unprepare(sram->clk);
diff --git a/drivers/misc/ti-st/st_kim.c b/drivers/misc/ti-st/st_kim.c
index 18e7a03985d4..5027b8ffae43 100644
--- a/drivers/misc/ti-st/st_kim.c
+++ b/drivers/misc/ti-st/st_kim.c
@@ -752,9 +752,8 @@ static struct ti_st_plat_data *get_platform_data(struct device *dev)
 	int len;
 
 	dt_pdata = kzalloc(sizeof(*dt_pdata), GFP_KERNEL);
-
 	if (!dt_pdata)
-		pr_err("Can't allocate device_tree platform data\n");
+		return NULL;
 
 	dt_property = of_get_property(np, "dev_name", &len);
 	if (dt_property)
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index 60f7141a6b02..c9c3d20b784b 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -913,6 +913,9 @@ static int mmc_blk_cmd_recovery(struct mmc_card *card, struct request *req,
 		if (!err)
 			break;
 
+		/* Re-tune if needed */
+		mmc_retune_recheck(card->host);
+
 		prev_cmd_status_valid = false;
 		pr_err("%s: error %d sending status command, %sing\n",
 		       req->rq_disk->disk_name, err, retry ? "retry" : "abort");
@@ -1204,6 +1207,7 @@ static int mmc_blk_err_check(struct mmc_card *card,
 						    mmc_active);
 	struct mmc_blk_request *brq = &mq_mrq->brq;
 	struct request *req = mq_mrq->req;
+	int need_retune = card->host->need_retune;
 	int ecc_err = 0, gen_err = 0;
 
 	/*
@@ -1271,6 +1275,12 @@ static int mmc_blk_err_check(struct mmc_card *card,
 	}
 
 	if (brq->data.error) {
+		if (need_retune && !brq->retune_retry_done) {
+			pr_info("%s: retrying because a re-tune was needed\n",
+				req->rq_disk->disk_name);
+			brq->retune_retry_done = 1;
+			return MMC_BLK_RETRY;
+		}
 		pr_err("%s: error %d transferring data, sector %u, nr %u, cmd response %#x, card status %#x\n",
 		       req->rq_disk->disk_name, brq->data.error,
 		       (unsigned)blk_rq_pos(req),
@@ -1830,7 +1840,7 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 	struct mmc_blk_data *md = mq->data;
 	struct mmc_card *card = md->queue.card;
 	struct mmc_blk_request *brq = &mq->mqrq_cur->brq;
-	int ret = 1, disable_multi = 0, retry = 0, type;
+	int ret = 1, disable_multi = 0, retry = 0, type, retune_retry_done = 0;
 	enum mmc_blk_status status;
 	struct mmc_queue_req *mq_rq;
 	struct request *req = rqc;
@@ -1910,10 +1920,13 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 			break;
 		case MMC_BLK_CMD_ERR:
 			ret = mmc_blk_cmd_err(md, card, brq, req, ret);
-			if (!mmc_blk_reset(md, card->host, type))
-				break;
-			goto cmd_abort;
+			if (mmc_blk_reset(md, card->host, type))
+				goto cmd_abort;
+			if (!ret)
+				goto start_new_req;
+			break;
 		case MMC_BLK_RETRY:
+			retune_retry_done = brq->retune_retry_done;
 			if (retry++ < 5)
 				break;
 			/* Fall through */
@@ -1976,6 +1989,7 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 				mmc_start_req(card->host,
 						&mq_rq->mmc_active, NULL);
 			}
+			mq_rq->brq.retune_retry_done = retune_retry_done;
 		}
 	} while (ret);
 
@@ -2217,7 +2231,8 @@ static struct mmc_blk_data *mmc_blk_alloc(struct mmc_card *card)
 		 * The CSD capacity field is in units of read_blkbits.
 		 * set_capacity takes units of 512 bytes.
 		 */
-		size = card->csd.capacity << (card->csd.read_blkbits - 9);
+		size = (typeof(sector_t))card->csd.capacity
+			<< (card->csd.read_blkbits - 9);
 	}
 
 	return mmc_blk_alloc_req(card, &card->dev, size, false, NULL,
diff --git a/drivers/mmc/card/mmc_test.c b/drivers/mmc/card/mmc_test.c
index 53b741398b93..b78cf5d403a3 100644
--- a/drivers/mmc/card/mmc_test.c
+++ b/drivers/mmc/card/mmc_test.c
@@ -268,8 +268,6 @@ static int mmc_test_wait_busy(struct mmc_test_card *test)
 static int mmc_test_buffer_transfer(struct mmc_test_card *test,
 	u8 *buffer, unsigned addr, unsigned blksz, int write)
 {
-	int ret;
-
 	struct mmc_request mrq = {0};
 	struct mmc_command cmd = {0};
 	struct mmc_command stop = {0};
@@ -292,11 +290,7 @@ static int mmc_test_buffer_transfer(struct mmc_test_card *test,
 	if (data.error)
 		return data.error;
 
-	ret = mmc_test_wait_busy(test);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_wait_busy(test);
 }
 
 static void mmc_test_free_mem(struct mmc_test_mem *mem)
@@ -826,9 +820,7 @@ static int mmc_test_nonblock_transfer(struct mmc_test_card *test,
 				mmc_test_nonblock_reset(&mrq1, &cmd1,
 							&stop1, &data1);
 		}
-		done_areq = cur_areq;
-		cur_areq = other_areq;
-		other_areq = done_areq;
+		swap(cur_areq, other_areq);
 		dev_addr += blocks;
 	}
 
@@ -994,11 +986,7 @@ static int mmc_test_basic_write(struct mmc_test_card *test)
 
 	sg_init_one(&sg, test->buffer, 512);
 
-	ret = mmc_test_simple_transfer(test, &sg, 1, 0, 1, 512, 1);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_simple_transfer(test, &sg, 1, 0, 1, 512, 1);
 }
 
 static int mmc_test_basic_read(struct mmc_test_card *test)
@@ -1012,44 +1000,29 @@ static int mmc_test_basic_read(struct mmc_test_card *test)
 
 	sg_init_one(&sg, test->buffer, 512);
 
-	ret = mmc_test_simple_transfer(test, &sg, 1, 0, 1, 512, 0);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_simple_transfer(test, &sg, 1, 0, 1, 512, 0);
 }
 
 static int mmc_test_verify_write(struct mmc_test_card *test)
 {
-	int ret;
 	struct scatterlist sg;
 
 	sg_init_one(&sg, test->buffer, 512);
 
-	ret = mmc_test_transfer(test, &sg, 1, 0, 1, 512, 1);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_transfer(test, &sg, 1, 0, 1, 512, 1);
 }
 
 static int mmc_test_verify_read(struct mmc_test_card *test)
 {
-	int ret;
 	struct scatterlist sg;
 
 	sg_init_one(&sg, test->buffer, 512);
 
-	ret = mmc_test_transfer(test, &sg, 1, 0, 1, 512, 0);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_transfer(test, &sg, 1, 0, 1, 512, 0);
 }
 
 static int mmc_test_multi_write(struct mmc_test_card *test)
 {
-	int ret;
 	unsigned int size;
 	struct scatterlist sg;
 
@@ -1066,16 +1039,11 @@ static int mmc_test_multi_write(struct mmc_test_card *test)
 
 	sg_init_one(&sg, test->buffer, size);
 
-	ret = mmc_test_transfer(test, &sg, 1, 0, size/512, 512, 1);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_transfer(test, &sg, 1, 0, size/512, 512, 1);
 }
 
 static int mmc_test_multi_read(struct mmc_test_card *test)
 {
-	int ret;
 	unsigned int size;
 	struct scatterlist sg;
 
@@ -1092,11 +1060,7 @@ static int mmc_test_multi_read(struct mmc_test_card *test)
 
 	sg_init_one(&sg, test->buffer, size);
 
-	ret = mmc_test_transfer(test, &sg, 1, 0, size/512, 512, 0);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_transfer(test, &sg, 1, 0, size/512, 512, 0);
 }
 
 static int mmc_test_pow2_write(struct mmc_test_card *test)
@@ -1263,11 +1227,7 @@ static int mmc_test_xfersize_write(struct mmc_test_card *test)
 	if (ret)
 		return ret;
 
-	ret = mmc_test_broken_transfer(test, 1, 512, 1);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_broken_transfer(test, 1, 512, 1);
 }
 
 static int mmc_test_xfersize_read(struct mmc_test_card *test)
@@ -1278,11 +1238,7 @@ static int mmc_test_xfersize_read(struct mmc_test_card *test)
 	if (ret)
 		return ret;
 
-	ret = mmc_test_broken_transfer(test, 1, 512, 0);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_broken_transfer(test, 1, 512, 0);
 }
 
 static int mmc_test_multi_xfersize_write(struct mmc_test_card *test)
@@ -1296,11 +1252,7 @@ static int mmc_test_multi_xfersize_write(struct mmc_test_card *test)
 	if (ret)
 		return ret;
 
-	ret = mmc_test_broken_transfer(test, 2, 512, 1);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_broken_transfer(test, 2, 512, 1);
 }
 
 static int mmc_test_multi_xfersize_read(struct mmc_test_card *test)
@@ -1314,48 +1266,33 @@ static int mmc_test_multi_xfersize_read(struct mmc_test_card *test)
 	if (ret)
 		return ret;
 
-	ret = mmc_test_broken_transfer(test, 2, 512, 0);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_broken_transfer(test, 2, 512, 0);
 }
 
 #ifdef CONFIG_HIGHMEM
 
 static int mmc_test_write_high(struct mmc_test_card *test)
 {
-	int ret;
 	struct scatterlist sg;
 
 	sg_init_table(&sg, 1);
 	sg_set_page(&sg, test->highmem, 512, 0);
 
-	ret = mmc_test_transfer(test, &sg, 1, 0, 1, 512, 1);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_transfer(test, &sg, 1, 0, 1, 512, 1);
 }
 
 static int mmc_test_read_high(struct mmc_test_card *test)
 {
-	int ret;
 	struct scatterlist sg;
 
 	sg_init_table(&sg, 1);
 	sg_set_page(&sg, test->highmem, 512, 0);
 
-	ret = mmc_test_transfer(test, &sg, 1, 0, 1, 512, 0);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_transfer(test, &sg, 1, 0, 1, 512, 0);
 }
 
 static int mmc_test_multi_write_high(struct mmc_test_card *test)
 {
-	int ret;
 	unsigned int size;
 	struct scatterlist sg;
 
@@ -1373,16 +1310,11 @@ static int mmc_test_multi_write_high(struct mmc_test_card *test)
 	sg_init_table(&sg, 1);
 	sg_set_page(&sg, test->highmem, size, 0);
 
-	ret = mmc_test_transfer(test, &sg, 1, 0, size/512, 512, 1);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_transfer(test, &sg, 1, 0, size/512, 512, 1);
 }
 
 static int mmc_test_multi_read_high(struct mmc_test_card *test)
 {
-	int ret;
 	unsigned int size;
 	struct scatterlist sg;
 
@@ -1400,11 +1332,7 @@ static int mmc_test_multi_read_high(struct mmc_test_card *test)
 	sg_init_table(&sg, 1);
 	sg_set_page(&sg, test->highmem, size, 0);
 
-	ret = mmc_test_transfer(test, &sg, 1, 0, size/512, 512, 0);
-	if (ret)
-		return ret;
-
-	return 0;
+	return mmc_test_transfer(test, &sg, 1, 0, size/512, 512, 0);
 }
 
 #else
diff --git a/drivers/mmc/card/queue.c b/drivers/mmc/card/queue.c
index 8efa3684aef8..b5a2b145d89f 100644
--- a/drivers/mmc/card/queue.c
+++ b/drivers/mmc/card/queue.c
@@ -56,7 +56,6 @@ static int mmc_queue_thread(void *d)
 	down(&mq->thread_sem);
 	do {
 		struct request *req = NULL;
-		struct mmc_queue_req *tmp;
 		unsigned int cmd_flags = 0;
 
 		spin_lock_irq(q->queue_lock);
@@ -69,6 +68,7 @@ static int mmc_queue_thread(void *d)
 			set_current_state(TASK_RUNNING);
 			cmd_flags = req ? req->cmd_flags : 0;
 			mq->issue_fn(mq, req);
+			cond_resched();
 			if (mq->flags & MMC_QUEUE_NEW_REQUEST) {
 				mq->flags &= ~MMC_QUEUE_NEW_REQUEST;
 				continue; /* fetch again */
@@ -86,9 +86,7 @@ static int mmc_queue_thread(void *d)
 
 			mq->mqrq_prev->brq.mrq.data = NULL;
 			mq->mqrq_prev->req = NULL;
-			tmp = mq->mqrq_prev;
-			mq->mqrq_prev = mq->mqrq_cur;
-			mq->mqrq_cur = tmp;
+			swap(mq->mqrq_prev, mq->mqrq_cur);
 		} else {
 			if (kthread_should_stop()) {
 				set_current_state(TASK_RUNNING);
diff --git a/drivers/mmc/card/queue.h b/drivers/mmc/card/queue.h
index 99e6521e6169..36cddab57d77 100644
--- a/drivers/mmc/card/queue.h
+++ b/drivers/mmc/card/queue.h
@@ -12,6 +12,7 @@ struct mmc_blk_request {
 	struct mmc_command	cmd;
 	struct mmc_command	stop;
 	struct mmc_data		data;
+	int			retune_retry_done;
 };
 
 enum mmc_packed_type {
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 92e7671426eb..9ad73f30f744 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -133,6 +133,12 @@ void mmc_request_done(struct mmc_host *host, struct mmc_request *mrq)
 	struct mmc_command *cmd = mrq->cmd;
 	int err = cmd->error;
 
+	/* Flag re-tuning needed on CRC errors */
+	if (err == -EILSEQ || (mrq->sbc && mrq->sbc->error == -EILSEQ) ||
+	    (mrq->data && mrq->data->error == -EILSEQ) ||
+	    (mrq->stop && mrq->stop->error == -EILSEQ))
+		mmc_retune_needed(host);
+
 	if (err && cmd->retries && mmc_host_is_spi(host)) {
 		if (cmd->resp[0] & R1_SPI_ILLEGAL_COMMAND)
 			cmd->retries = 0;
@@ -186,12 +192,29 @@ void mmc_request_done(struct mmc_host *host, struct mmc_request *mrq)
 
 EXPORT_SYMBOL(mmc_request_done);
 
+static void __mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
+{
+	int err;
+
+	/* Assumes host controller has been runtime resumed by mmc_claim_host */
+	err = mmc_retune(host);
+	if (err) {
+		mrq->cmd->error = err;
+		mmc_request_done(host, mrq);
+		return;
+	}
+
+	host->ops->request(host, mrq);
+}
+
 static int mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
 {
 #ifdef CONFIG_MMC_DEBUG
 	unsigned int i, sz;
 	struct scatterlist *sg;
 #endif
+	mmc_retune_hold(host);
+
 	if (mmc_card_removed(host->card))
 		return -ENOMEDIUM;
 
@@ -252,7 +275,7 @@ static int mmc_start_request(struct mmc_host *host, struct mmc_request *mrq)
 	}
 	mmc_host_clk_hold(host);
 	led_trigger_event(host->led, LED_FULL);
-	host->ops->request(host, mrq);
+	__mmc_start_request(host, mrq);
 
 	return 0;
 }
@@ -301,12 +324,15 @@ void mmc_start_bkops(struct mmc_card *card, bool from_exception)
 		use_busy_signal = false;
 	}
 
+	mmc_retune_hold(card->host);
+
 	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
 			EXT_CSD_BKOPS_START, 1, timeout,
 			use_busy_signal, true, false);
 	if (err) {
 		pr_warn("%s: Error %d starting bkops\n",
 			mmc_hostname(card->host), err);
+		mmc_retune_release(card->host);
 		goto out;
 	}
 
@@ -317,6 +343,8 @@ void mmc_start_bkops(struct mmc_card *card, bool from_exception)
 	 */
 	if (!use_busy_signal)
 		mmc_card_set_doing_bkops(card);
+	else
+		mmc_retune_release(card->host);
 out:
 	mmc_release_host(card->host);
 }
@@ -417,22 +445,22 @@ static int mmc_wait_for_data_req_done(struct mmc_host *host,
 							    host->areq);
 				break; /* return err */
 			} else {
+				mmc_retune_recheck(host);
 				pr_info("%s: req failed (CMD%u): %d, retrying...\n",
 					mmc_hostname(host),
 					cmd->opcode, cmd->error);
 				cmd->retries--;
 				cmd->error = 0;
-				host->ops->request(host, mrq);
+				__mmc_start_request(host, mrq);
 				continue; /* wait for done/new event again */
 			}
 		} else if (context_info->is_new_req) {
 			context_info->is_new_req = false;
-			if (!next_req) {
-				err = MMC_BLK_NEW_REQUEST;
-				break; /* return err */
-			}
+			if (!next_req)
+				return MMC_BLK_NEW_REQUEST;
 		}
 	}
+	mmc_retune_release(host);
 	return err;
 }
 
@@ -467,12 +495,16 @@ static void mmc_wait_for_req_done(struct mmc_host *host,
 		    mmc_card_removed(host->card))
 			break;
 
+		mmc_retune_recheck(host);
+
 		pr_debug("%s: req failed (CMD%u): %d, retrying...\n",
 			 mmc_hostname(host), cmd->opcode, cmd->error);
 		cmd->retries--;
 		cmd->error = 0;
-		host->ops->request(host, mrq);
+		__mmc_start_request(host, mrq);
 	}
+
+	mmc_retune_release(host);
 }
 
 /**
@@ -728,6 +760,7 @@ int mmc_stop_bkops(struct mmc_card *card)
 	 */
 	if (!err || (err == -EINVAL)) {
 		mmc_card_clr_doing_bkops(card);
+		mmc_retune_release(card->host);
 		err = 0;
 	}
 
@@ -1109,6 +1142,8 @@ int mmc_execute_tuning(struct mmc_card *card)
 
 	if (err)
 		pr_err("%s: tuning execution failed\n", mmc_hostname(host));
+	else
+		mmc_retune_enable(host);
 
 	return err;
 }
@@ -1140,6 +1175,8 @@ void mmc_set_bus_width(struct mmc_host *host, unsigned int width)
  */
 void mmc_set_initial_state(struct mmc_host *host)
 {
+	mmc_retune_disable(host);
+
 	if (mmc_host_is_spi(host))
 		host->ios.chip_select = MMC_CS_HIGH;
 	else
@@ -1147,6 +1184,7 @@ void mmc_set_initial_state(struct mmc_host *host)
 	host->ios.bus_mode = MMC_BUSMODE_PUSHPULL;
 	host->ios.bus_width = MMC_BUS_WIDTH_1;
 	host->ios.timing = MMC_TIMING_LEGACY;
+	host->ios.drv_type = 0;
 
 	mmc_set_ios(host);
 }
@@ -1551,8 +1589,8 @@ int mmc_set_signal_voltage(struct mmc_host *host, int signal_voltage, u32 ocr)
 		goto power_cycle;
 	}
 
-	/* Keep clock gated for at least 5 ms */
-	mmc_delay(5);
+	/* Keep clock gated for at least 10 ms, though spec only says 5 ms */
+	mmc_delay(10);
 	host->ios.clock = clock;
 	mmc_set_ios(host);
 
@@ -1601,6 +1639,44 @@ void mmc_set_driver_type(struct mmc_host *host, unsigned int drv_type)
 	mmc_host_clk_release(host);
 }
 
+int mmc_select_drive_strength(struct mmc_card *card, unsigned int max_dtr,
+			      int card_drv_type, int *drv_type)
+{
+	struct mmc_host *host = card->host;
+	int host_drv_type = SD_DRIVER_TYPE_B;
+	int drive_strength;
+
+	*drv_type = 0;
+
+	if (!host->ops->select_drive_strength)
+		return 0;
+
+	/* Use SD definition of driver strength for hosts */
+	if (host->caps & MMC_CAP_DRIVER_TYPE_A)
+		host_drv_type |= SD_DRIVER_TYPE_A;
+
+	if (host->caps & MMC_CAP_DRIVER_TYPE_C)
+		host_drv_type |= SD_DRIVER_TYPE_C;
+
+	if (host->caps & MMC_CAP_DRIVER_TYPE_D)
+		host_drv_type |= SD_DRIVER_TYPE_D;
+
+	/*
+	 * The drive strength that the hardware can support
+	 * depends on the board design.  Pass the appropriate
+	 * information and let the hardware specific code
+	 * return what is possible given the options
+	 */
+	mmc_host_clk_hold(host);
+	drive_strength = host->ops->select_drive_strength(card, max_dtr,
+							  host_drv_type,
+							  card_drv_type,
+							  drv_type);
+	mmc_host_clk_release(host);
+
+	return drive_strength;
+}
+
 /*
  * Apply power to the MMC stack.  This is a two-stage process.
  * First, we enable power to the card without the clock running.
@@ -1970,6 +2046,8 @@ static int mmc_do_erase(struct mmc_card *card, unsigned int from,
 	unsigned long timeout;
 	int err;
 
+	mmc_retune_hold(card->host);
+
 	/*
 	 * qty is used to calculate the erase timeout which depends on how many
 	 * erase groups (or allocation units in SD terminology) are affected.
@@ -2073,6 +2151,7 @@ static int mmc_do_erase(struct mmc_card *card, unsigned int from,
 	} while (!(cmd.resp[0] & R1_READY_FOR_DATA) ||
 		 (R1_CURRENT_STATE(cmd.resp[0]) == R1_STATE_PRG));
 out:
+	mmc_retune_release(card->host);
 	return err;
 }
 
@@ -2331,7 +2410,8 @@ int mmc_hw_reset(struct mmc_host *host)
 	ret = host->bus_ops->reset(host);
 	mmc_bus_put(host);
 
-	pr_warn("%s: tried to reset card\n", mmc_hostname(host));
+	if (ret != -EOPNOTSUPP)
+		pr_warn("%s: tried to reset card\n", mmc_hostname(host));
 
 	return ret;
 }
diff --git a/drivers/mmc/core/core.h b/drivers/mmc/core/core.h
index cfba3c05aab1..1a22a82209b2 100644
--- a/drivers/mmc/core/core.h
+++ b/drivers/mmc/core/core.h
@@ -50,6 +50,8 @@ int mmc_set_signal_voltage(struct mmc_host *host, int signal_voltage, u32 ocr);
 int __mmc_set_signal_voltage(struct mmc_host *host, int signal_voltage);
 void mmc_set_timing(struct mmc_host *host, unsigned int timing);
 void mmc_set_driver_type(struct mmc_host *host, unsigned int drv_type);
+int mmc_select_drive_strength(struct mmc_card *card, unsigned int max_dtr,
+			      int card_drv_type, int *drv_type);
 void mmc_power_up(struct mmc_host *host, u32 ocr);
 void mmc_power_off(struct mmc_host *host);
 void mmc_power_cycle(struct mmc_host *host, u32 ocr);
@@ -88,6 +90,8 @@ void mmc_remove_card_debugfs(struct mmc_card *card);
 void mmc_init_context_info(struct mmc_host *host);
 
 int mmc_execute_tuning(struct mmc_card *card);
+int mmc_hs200_to_hs400(struct mmc_card *card);
+int mmc_hs400_to_hs200(struct mmc_card *card);
 
 #endif
 
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 8be0df758e68..99a9c9011c50 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -301,6 +301,90 @@ static inline void mmc_host_clk_sysfs_init(struct mmc_host *host)
 
 #endif
 
+void mmc_retune_enable(struct mmc_host *host)
+{
+	host->can_retune = 1;
+	if (host->retune_period)
+		mod_timer(&host->retune_timer,
+			  jiffies + host->retune_period * HZ);
+}
+
+void mmc_retune_disable(struct mmc_host *host)
+{
+	host->can_retune = 0;
+	del_timer_sync(&host->retune_timer);
+	host->retune_now = 0;
+	host->need_retune = 0;
+}
+
+void mmc_retune_timer_stop(struct mmc_host *host)
+{
+	del_timer_sync(&host->retune_timer);
+}
+EXPORT_SYMBOL(mmc_retune_timer_stop);
+
+void mmc_retune_hold(struct mmc_host *host)
+{
+	if (!host->hold_retune)
+		host->retune_now = 1;
+	host->hold_retune += 1;
+}
+
+void mmc_retune_release(struct mmc_host *host)
+{
+	if (host->hold_retune)
+		host->hold_retune -= 1;
+	else
+		WARN_ON(1);
+}
+
+int mmc_retune(struct mmc_host *host)
+{
+	bool return_to_hs400 = false;
+	int err;
+
+	if (host->retune_now)
+		host->retune_now = 0;
+	else
+		return 0;
+
+	if (!host->need_retune || host->doing_retune || !host->card)
+		return 0;
+
+	host->need_retune = 0;
+
+	host->doing_retune = 1;
+
+	if (host->ios.timing == MMC_TIMING_MMC_HS400) {
+		err = mmc_hs400_to_hs200(host->card);
+		if (err)
+			goto out;
+
+		return_to_hs400 = true;
+
+		if (host->ops->prepare_hs400_tuning)
+			host->ops->prepare_hs400_tuning(host, &host->ios);
+	}
+
+	err = mmc_execute_tuning(host->card);
+	if (err)
+		goto out;
+
+	if (return_to_hs400)
+		err = mmc_hs200_to_hs400(host->card);
+out:
+	host->doing_retune = 0;
+
+	return err;
+}
+
+static void mmc_retune_timer(unsigned long data)
+{
+	struct mmc_host *host = (struct mmc_host *)data;
+
+	mmc_retune_needed(host);
+}
+
 /**
  *	mmc_of_parse() - parse host's device-tree node
  *	@host: host whose node should be parsed.
@@ -400,6 +484,9 @@ int mmc_of_parse(struct mmc_host *host)
 	else if (ret != -ENOENT)
 		return ret;
 
+	if (of_property_read_bool(np, "disable-wp"))
+		host->caps2 |= MMC_CAP2_NO_WRITE_PROTECT;
+
 	/* See the comment on CD inversion above */
 	if (ro_cap_invert ^ ro_gpio_invert)
 		host->caps2 |= MMC_CAP2_RO_ACTIVE_HIGH;
@@ -504,6 +591,7 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 #ifdef CONFIG_PM
 	host->pm_notify.notifier_call = mmc_pm_notify;
 #endif
+	setup_timer(&host->retune_timer, mmc_retune_timer, (unsigned long)host);
 
 	/*
 	 * By default, hosts do not support SGIO or large requests.
diff --git a/drivers/mmc/core/host.h b/drivers/mmc/core/host.h
index f2ab9e578126..992bf5397633 100644
--- a/drivers/mmc/core/host.h
+++ b/drivers/mmc/core/host.h
@@ -15,5 +15,11 @@
 int mmc_register_host_class(void);
 void mmc_unregister_host_class(void);
 
+void mmc_retune_enable(struct mmc_host *host);
+void mmc_retune_disable(struct mmc_host *host);
+void mmc_retune_hold(struct mmc_host *host);
+void mmc_retune_release(struct mmc_host *host);
+int mmc_retune(struct mmc_host *host);
+
 #endif
 
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index f36c76f8b232..e726903170a8 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -21,6 +21,7 @@
 #include <linux/mmc/mmc.h>
 
 #include "core.h"
+#include "host.h"
 #include "bus.h"
 #include "mmc_ops.h"
 #include "sd_ops.h"
@@ -266,8 +267,10 @@ static void mmc_manage_enhanced_area(struct mmc_card *card, u8 *ext_csd)
 			 * calculate the enhanced data area offset, in bytes
 			 */
 			card->ext_csd.enhanced_area_offset =
-				(ext_csd[139] << 24) + (ext_csd[138] << 16) +
-				(ext_csd[137] << 8) + ext_csd[136];
+				(((unsigned long long)ext_csd[139]) << 24) +
+				(((unsigned long long)ext_csd[138]) << 16) +
+				(((unsigned long long)ext_csd[137]) << 8) +
+				(((unsigned long long)ext_csd[136]));
 			if (mmc_card_blockaddr(card))
 				card->ext_csd.enhanced_area_offset <<= 9;
 			/*
@@ -434,6 +437,7 @@ static int mmc_decode_ext_csd(struct mmc_card *card, u8 *ext_csd)
 	card->ext_csd.raw_trim_mult =
 		ext_csd[EXT_CSD_TRIM_MULT];
 	card->ext_csd.raw_partition_support = ext_csd[EXT_CSD_PARTITION_SUPPORT];
+	card->ext_csd.raw_driver_strength = ext_csd[EXT_CSD_DRIVER_STRENGTH];
 	if (card->ext_csd.rev >= 4) {
 		if (ext_csd[EXT_CSD_PARTITION_SETTING_COMPLETED] &
 		    EXT_CSD_PART_SETTING_COMPLETED)
@@ -1040,6 +1044,7 @@ static int mmc_select_hs400(struct mmc_card *card)
 {
 	struct mmc_host *host = card->host;
 	int err = 0;
+	u8 val;
 
 	/*
 	 * HS400 mode requires 8-bit bus width
@@ -1055,8 +1060,10 @@ static int mmc_select_hs400(struct mmc_card *card)
 	mmc_set_timing(card->host, MMC_TIMING_MMC_HS);
 	mmc_set_bus_speed(card);
 
+	val = EXT_CSD_TIMING_HS |
+	      card->drive_strength << EXT_CSD_DRV_STR_SHIFT;
 	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-			   EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS,
+			   EXT_CSD_HS_TIMING, val,
 			   card->ext_csd.generic_cmd6_time,
 			   true, true, true);
 	if (err) {
@@ -1075,8 +1082,10 @@ static int mmc_select_hs400(struct mmc_card *card)
 		return err;
 	}
 
+	val = EXT_CSD_TIMING_HS400 |
+	      card->drive_strength << EXT_CSD_DRV_STR_SHIFT;
 	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-			   EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS400,
+			   EXT_CSD_HS_TIMING, val,
 			   card->ext_csd.generic_cmd6_time,
 			   true, true, true);
 	if (err) {
@@ -1091,6 +1100,115 @@ static int mmc_select_hs400(struct mmc_card *card)
 	return 0;
 }
 
+int mmc_hs200_to_hs400(struct mmc_card *card)
+{
+	return mmc_select_hs400(card);
+}
+
+/* Caller must hold re-tuning */
+static int mmc_switch_status(struct mmc_card *card)
+{
+	u32 status;
+	int err;
+
+	err = mmc_send_status(card, &status);
+	if (err)
+		return err;
+
+	return mmc_switch_status_error(card->host, status);
+}
+
+int mmc_hs400_to_hs200(struct mmc_card *card)
+{
+	struct mmc_host *host = card->host;
+	bool send_status = true;
+	unsigned int max_dtr;
+	int err;
+	u8 val;
+
+	if (host->caps & MMC_CAP_WAIT_WHILE_BUSY)
+		send_status = false;
+
+	/* Reduce frequency to HS */
+	max_dtr = card->ext_csd.hs_max_dtr;
+	mmc_set_clock(host, max_dtr);
+
+	/* Switch HS400 to HS DDR */
+	val = EXT_CSD_TIMING_HS |
+	      card->drive_strength << EXT_CSD_DRV_STR_SHIFT;
+	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_HS_TIMING,
+			   val, card->ext_csd.generic_cmd6_time,
+			   true, send_status, true);
+	if (err)
+		goto out_err;
+
+	mmc_set_timing(host, MMC_TIMING_MMC_DDR52);
+
+	if (!send_status) {
+		err = mmc_switch_status(card);
+		if (err)
+			goto out_err;
+	}
+
+	/* Switch HS DDR to HS */
+	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_BUS_WIDTH,
+			   EXT_CSD_BUS_WIDTH_8, card->ext_csd.generic_cmd6_time,
+			   true, send_status, true);
+	if (err)
+		goto out_err;
+
+	mmc_set_timing(host, MMC_TIMING_MMC_HS);
+
+	if (!send_status) {
+		err = mmc_switch_status(card);
+		if (err)
+			goto out_err;
+	}
+
+	/* Switch HS to HS200 */
+	val = EXT_CSD_TIMING_HS200 |
+	      card->drive_strength << EXT_CSD_DRV_STR_SHIFT;
+	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_HS_TIMING,
+			   val, card->ext_csd.generic_cmd6_time, true,
+			   send_status, true);
+	if (err)
+		goto out_err;
+
+	mmc_set_timing(host, MMC_TIMING_MMC_HS200);
+
+	if (!send_status) {
+		err = mmc_switch_status(card);
+		if (err)
+			goto out_err;
+	}
+
+	mmc_set_bus_speed(card);
+
+	return 0;
+
+out_err:
+	pr_err("%s: %s failed, error %d\n", mmc_hostname(card->host),
+	       __func__, err);
+	return err;
+}
+
+static void mmc_select_driver_type(struct mmc_card *card)
+{
+	int card_drv_type, drive_strength, drv_type;
+
+	card_drv_type = card->ext_csd.raw_driver_strength |
+			mmc_driver_type_mask(0);
+
+	drive_strength = mmc_select_drive_strength(card,
+						   card->ext_csd.hs200_max_dtr,
+						   card_drv_type, &drv_type);
+
+	card->drive_strength = drive_strength;
+
+	if (drv_type)
+		mmc_set_driver_type(card->host, drv_type);
+}
+
 /*
  * For device supporting HS200 mode, the following sequence
  * should be done before executing the tuning process.
@@ -1102,6 +1220,7 @@ static int mmc_select_hs200(struct mmc_card *card)
 {
 	struct mmc_host *host = card->host;
 	int err = -EINVAL;
+	u8 val;
 
 	if (card->mmc_avail_type & EXT_CSD_CARD_TYPE_HS200_1_2V)
 		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_120);
@@ -1113,14 +1232,18 @@ static int mmc_select_hs200(struct mmc_card *card)
 	if (err)
 		goto err;
 
+	mmc_select_driver_type(card);
+
 	/*
 	 * Set the bus width(4 or 8) with host's support and
 	 * switch to HS200 mode if bus width is set successfully.
 	 */
 	err = mmc_select_bus_width(card);
 	if (!IS_ERR_VALUE(err)) {
+		val = EXT_CSD_TIMING_HS200 |
+		      card->drive_strength << EXT_CSD_DRV_STR_SHIFT;
 		err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-				   EXT_CSD_HS_TIMING, EXT_CSD_TIMING_HS200,
+				   EXT_CSD_HS_TIMING, val,
 				   card->ext_csd.generic_cmd6_time,
 				   true, true, true);
 		if (!err)
@@ -1511,9 +1634,12 @@ static int mmc_sleep(struct mmc_host *host)
 	unsigned int timeout_ms = DIV_ROUND_UP(card->ext_csd.sa_timeout, 10000);
 	int err;
 
+	/* Re-tuning can't be done once the card is deselected */
+	mmc_retune_hold(host);
+
 	err = mmc_deselect_cards(host);
 	if (err)
-		return err;
+		goto out_release;
 
 	cmd.opcode = MMC_SLEEP_AWAKE;
 	cmd.arg = card->rca << 16;
@@ -1534,7 +1660,7 @@ static int mmc_sleep(struct mmc_host *host)
 
 	err = mmc_wait_for_cmd(host, &cmd, 0);
 	if (err)
-		return err;
+		goto out_release;
 
 	/*
 	 * If the host does not wait while the card signals busy, then we will
@@ -1545,6 +1671,8 @@ static int mmc_sleep(struct mmc_host *host)
 	if (!cmd.busy_timeout || !(host->caps & MMC_CAP_WAIT_WHILE_BUSY))
 		mmc_delay(timeout_ms);
 
+out_release:
+	mmc_retune_release(host);
 	return err;
 }
 
@@ -1782,17 +1910,6 @@ static int mmc_runtime_resume(struct mmc_host *host)
 	return 0;
 }
 
-static int mmc_power_restore(struct mmc_host *host)
-{
-	int ret;
-
-	mmc_claim_host(host);
-	ret = mmc_init_card(host, host->card->ocr, host->card);
-	mmc_release_host(host);
-
-	return ret;
-}
-
 int mmc_can_reset(struct mmc_card *card)
 {
 	u8 rst_n_function;
@@ -1830,7 +1947,7 @@ static int mmc_reset(struct mmc_host *host)
 	mmc_set_initial_state(host);
 	mmc_host_clk_release(host);
 
-	return mmc_power_restore(host);
+	return mmc_init_card(host, card->ocr, card);
 }
 
 static const struct mmc_bus_ops mmc_ops = {
@@ -1840,7 +1957,6 @@ static const struct mmc_bus_ops mmc_ops = {
 	.resume = mmc_resume,
 	.runtime_suspend = mmc_runtime_suspend,
 	.runtime_resume = mmc_runtime_resume,
-	.power_restore = mmc_power_restore,
 	.alive = mmc_alive,
 	.shutdown = mmc_shutdown,
 	.reset = mmc_reset,
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 0ea042dc7443..0e9ae1c276c8 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -19,6 +19,7 @@
 #include <linux/mmc/mmc.h>
 
 #include "core.h"
+#include "host.h"
 #include "mmc_ops.h"
 
 #define MMC_OPS_TIMEOUT_MS	(10 * 60 * 1000) /* 10 minute timeout */
@@ -449,6 +450,21 @@ int mmc_spi_set_crc(struct mmc_host *host, int use_crc)
 	return err;
 }
 
+int mmc_switch_status_error(struct mmc_host *host, u32 status)
+{
+	if (mmc_host_is_spi(host)) {
+		if (status & R1_SPI_ILLEGAL_COMMAND)
+			return -EBADMSG;
+	} else {
+		if (status & 0xFDFFA000)
+			pr_warn("%s: unexpected status %#x after switch\n",
+				mmc_hostname(host), status);
+		if (status & R1_SWITCH_ERROR)
+			return -EBADMSG;
+	}
+	return 0;
+}
+
 /**
  *	__mmc_switch - modify EXT_CSD register
  *	@card: the MMC card associated with the data transfer
@@ -474,6 +490,8 @@ int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 	u32 status = 0;
 	bool use_r1b_resp = use_busy_signal;
 
+	mmc_retune_hold(host);
+
 	/*
 	 * If the cmd timeout and the max_busy_timeout of the host are both
 	 * specified, let's validate them. A failure means we need to prevent
@@ -506,11 +524,11 @@ int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 
 	err = mmc_wait_for_cmd(host, &cmd, MMC_CMD_RETRIES);
 	if (err)
-		return err;
+		goto out;
 
 	/* No need to check card status in case of unblocking command */
 	if (!use_busy_signal)
-		return 0;
+		goto out;
 
 	/*
 	 * CRC errors shall only be ignored in cases were CMD13 is used to poll
@@ -529,7 +547,7 @@ int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 		if (send_status) {
 			err = __mmc_send_status(card, &status, ignore_crc);
 			if (err)
-				return err;
+				goto out;
 		}
 		if ((host->caps & MMC_CAP_WAIT_WHILE_BUSY) && use_r1b_resp)
 			break;
@@ -543,29 +561,23 @@ int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 		 */
 		if (!send_status) {
 			mmc_delay(timeout_ms);
-			return 0;
+			goto out;
 		}
 
 		/* Timeout if the device never leaves the program state. */
 		if (time_after(jiffies, timeout)) {
 			pr_err("%s: Card stuck in programming state! %s\n",
 				mmc_hostname(host), __func__);
-			return -ETIMEDOUT;
+			err = -ETIMEDOUT;
+			goto out;
 		}
 	} while (R1_CURRENT_STATE(status) == R1_STATE_PRG);
 
-	if (mmc_host_is_spi(host)) {
-		if (status & R1_SPI_ILLEGAL_COMMAND)
-			return -EBADMSG;
-	} else {
-		if (status & 0xFDFFA000)
-			pr_warn("%s: unexpected status %#x after switch\n",
-				mmc_hostname(host), status);
-		if (status & R1_SWITCH_ERROR)
-			return -EBADMSG;
-	}
+	err = mmc_switch_status_error(host, status);
+out:
+	mmc_retune_release(host);
 
-	return 0;
+	return err;
 }
 EXPORT_SYMBOL_GPL(__mmc_switch);
 
diff --git a/drivers/mmc/core/mmc_ops.h b/drivers/mmc/core/mmc_ops.h
index 6f4b00ed93de..f498f9ae21f0 100644
--- a/drivers/mmc/core/mmc_ops.h
+++ b/drivers/mmc/core/mmc_ops.h
@@ -27,6 +27,7 @@ int mmc_spi_set_crc(struct mmc_host *host, int use_crc);
 int mmc_bus_test(struct mmc_card *card, u8 bus_width);
 int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status);
 int mmc_can_ext_csd(struct mmc_card *card);
+int mmc_switch_status_error(struct mmc_host *host, u32 status);
 
 #endif
 
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index 31a9ef256d06..4e7366ab187f 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -386,64 +386,31 @@ out:
 
 static int sd_select_driver_type(struct mmc_card *card, u8 *status)
 {
-	int host_drv_type = SD_DRIVER_TYPE_B;
-	int card_drv_type = SD_DRIVER_TYPE_B;
-	int drive_strength;
+	int card_drv_type, drive_strength, drv_type;
 	int err;
 
-	/*
-	 * If the host doesn't support any of the Driver Types A,C or D,
-	 * or there is no board specific handler then default Driver
-	 * Type B is used.
-	 */
-	if (!(card->host->caps & (MMC_CAP_DRIVER_TYPE_A | MMC_CAP_DRIVER_TYPE_C
-	    | MMC_CAP_DRIVER_TYPE_D)))
-		return 0;
-
-	if (!card->host->ops->select_drive_strength)
-		return 0;
-
-	if (card->host->caps & MMC_CAP_DRIVER_TYPE_A)
-		host_drv_type |= SD_DRIVER_TYPE_A;
-
-	if (card->host->caps & MMC_CAP_DRIVER_TYPE_C)
-		host_drv_type |= SD_DRIVER_TYPE_C;
-
-	if (card->host->caps & MMC_CAP_DRIVER_TYPE_D)
-		host_drv_type |= SD_DRIVER_TYPE_D;
-
-	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_A)
-		card_drv_type |= SD_DRIVER_TYPE_A;
-
-	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_C)
-		card_drv_type |= SD_DRIVER_TYPE_C;
+	card->drive_strength = 0;
 
-	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_D)
-		card_drv_type |= SD_DRIVER_TYPE_D;
+	card_drv_type = card->sw_caps.sd3_drv_type | SD_DRIVER_TYPE_B;
 
-	/*
-	 * The drive strength that the hardware can support
-	 * depends on the board design.  Pass the appropriate
-	 * information and let the hardware specific code
-	 * return what is possible given the options
-	 */
-	mmc_host_clk_hold(card->host);
-	drive_strength = card->host->ops->select_drive_strength(
-		card->sw_caps.uhs_max_dtr,
-		host_drv_type, card_drv_type);
-	mmc_host_clk_release(card->host);
-
-	err = mmc_sd_switch(card, 1, 2, drive_strength, status);
-	if (err)
-		return err;
+	drive_strength = mmc_select_drive_strength(card,
+						   card->sw_caps.uhs_max_dtr,
+						   card_drv_type, &drv_type);
 
-	if ((status[15] & 0xF) != drive_strength) {
-		pr_warn("%s: Problem setting drive strength!\n",
-			mmc_hostname(card->host));
-		return 0;
+	if (drive_strength) {
+		err = mmc_sd_switch(card, 1, 2, drive_strength, status);
+		if (err)
+			return err;
+		if ((status[15] & 0xF) != drive_strength) {
+			pr_warn("%s: Problem setting drive strength!\n",
+				mmc_hostname(card->host));
+			return 0;
+		}
+		card->drive_strength = drive_strength;
 	}
 
-	mmc_set_driver_type(card->host, drive_strength);
+	if (drv_type)
+		mmc_set_driver_type(card->host, drv_type);
 
 	return 0;
 }
@@ -804,6 +771,28 @@ int mmc_sd_get_csd(struct mmc_host *host, struct mmc_card *card)
 	return 0;
 }
 
+static int mmc_sd_get_ro(struct mmc_host *host)
+{
+	int ro;
+
+	/*
+	 * Some systems don't feature a write-protect pin and don't need one.
+	 * E.g. because they only have micro-SD card slot. For those systems
+	 * assume that the SD card is always read-write.
+	 */
+	if (host->caps2 & MMC_CAP2_NO_WRITE_PROTECT)
+		return 0;
+
+	if (!host->ops->get_ro)
+		return -1;
+
+	mmc_host_clk_hold(host);
+	ro = host->ops->get_ro(host);
+	mmc_host_clk_release(host);
+
+	return ro;
+}
+
 int mmc_sd_setup_card(struct mmc_host *host, struct mmc_card *card,
 	bool reinit)
 {
@@ -855,13 +844,7 @@ int mmc_sd_setup_card(struct mmc_host *host, struct mmc_card *card,
 	 * Check if read-only switch is active.
 	 */
 	if (!reinit) {
-		int ro = -1;
-
-		if (host->ops->get_ro) {
-			mmc_host_clk_hold(card->host);
-			ro = host->ops->get_ro(host);
-			mmc_host_clk_release(card->host);
-		}
+		int ro = mmc_sd_get_ro(host);
 
 		if (ro < 0) {
 			pr_warn("%s: host does not support reading read-only switch, assuming write-enable\n",
@@ -1181,21 +1164,10 @@ static int mmc_sd_runtime_resume(struct mmc_host *host)
 	return 0;
 }
 
-static int mmc_sd_power_restore(struct mmc_host *host)
-{
-	int ret;
-
-	mmc_claim_host(host);
-	ret = mmc_sd_init_card(host, host->card->ocr, host->card);
-	mmc_release_host(host);
-
-	return ret;
-}
-
 static int mmc_sd_reset(struct mmc_host *host)
 {
 	mmc_power_cycle(host, host->card->ocr);
-	return mmc_sd_power_restore(host);
+	return mmc_sd_init_card(host, host->card->ocr, host->card);
 }
 
 static const struct mmc_bus_ops mmc_sd_ops = {
@@ -1205,7 +1177,6 @@ static const struct mmc_bus_ops mmc_sd_ops = {
 	.runtime_resume = mmc_sd_runtime_resume,
 	.suspend = mmc_sd_suspend,
 	.resume = mmc_sd_resume,
-	.power_restore = mmc_sd_power_restore,
 	.alive = mmc_sd_alive,
 	.shutdown = mmc_sd_suspend,
 	.reset = mmc_sd_reset,
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index 5bc6c7dbbd60..b91abedcfdca 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -402,69 +402,38 @@ static unsigned char host_drive_to_sdio_drive(int host_strength)
 
 static void sdio_select_driver_type(struct mmc_card *card)
 {
-	int host_drv_type = SD_DRIVER_TYPE_B;
-	int card_drv_type = SD_DRIVER_TYPE_B;
-	int drive_strength;
+	int card_drv_type, drive_strength, drv_type;
 	unsigned char card_strength;
 	int err;
 
-	/*
-	 * If the host doesn't support any of the Driver Types A,C or D,
-	 * or there is no board specific handler then default Driver
-	 * Type B is used.
-	 */
-	if (!(card->host->caps &
-		(MMC_CAP_DRIVER_TYPE_A |
-		 MMC_CAP_DRIVER_TYPE_C |
-		 MMC_CAP_DRIVER_TYPE_D)))
-		return;
-
-	if (!card->host->ops->select_drive_strength)
-		return;
-
-	if (card->host->caps & MMC_CAP_DRIVER_TYPE_A)
-		host_drv_type |= SD_DRIVER_TYPE_A;
-
-	if (card->host->caps & MMC_CAP_DRIVER_TYPE_C)
-		host_drv_type |= SD_DRIVER_TYPE_C;
+	card->drive_strength = 0;
 
-	if (card->host->caps & MMC_CAP_DRIVER_TYPE_D)
-		host_drv_type |= SD_DRIVER_TYPE_D;
+	card_drv_type = card->sw_caps.sd3_drv_type | SD_DRIVER_TYPE_B;
 
-	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_A)
-		card_drv_type |= SD_DRIVER_TYPE_A;
-
-	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_C)
-		card_drv_type |= SD_DRIVER_TYPE_C;
-
-	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_D)
-		card_drv_type |= SD_DRIVER_TYPE_D;
-
-	/*
-	 * The drive strength that the hardware can support
-	 * depends on the board design.  Pass the appropriate
-	 * information and let the hardware specific code
-	 * return what is possible given the options
-	 */
-	drive_strength = card->host->ops->select_drive_strength(
-		card->sw_caps.uhs_max_dtr,
-		host_drv_type, card_drv_type);
+	drive_strength = mmc_select_drive_strength(card,
+						   card->sw_caps.uhs_max_dtr,
+						   card_drv_type, &drv_type);
 
-	/* if error just use default for drive strength B */
-	err = mmc_io_rw_direct(card, 0, 0, SDIO_CCCR_DRIVE_STRENGTH, 0,
-		&card_strength);
-	if (err)
-		return;
+	if (drive_strength) {
+		/* if error just use default for drive strength B */
+		err = mmc_io_rw_direct(card, 0, 0, SDIO_CCCR_DRIVE_STRENGTH, 0,
+				       &card_strength);
+		if (err)
+			return;
 
-	card_strength &= ~(SDIO_DRIVE_DTSx_MASK<<SDIO_DRIVE_DTSx_SHIFT);
-	card_strength |= host_drive_to_sdio_drive(drive_strength);
+		card_strength &= ~(SDIO_DRIVE_DTSx_MASK<<SDIO_DRIVE_DTSx_SHIFT);
+		card_strength |= host_drive_to_sdio_drive(drive_strength);
 
-	err = mmc_io_rw_direct(card, 1, 0, SDIO_CCCR_DRIVE_STRENGTH,
-		card_strength, NULL);
+		/* if error default to drive strength B */
+		err = mmc_io_rw_direct(card, 1, 0, SDIO_CCCR_DRIVE_STRENGTH,
+				       card_strength, NULL);
+		if (err)
+			return;
+		card->drive_strength = drive_strength;
+	}
 
-	/* if error default to drive strength B */
-	if (!err)
-		mmc_set_driver_type(card->host, drive_strength);
+	if (drv_type)
+		mmc_set_driver_type(card->host, drv_type);
 }
 
 
@@ -934,8 +903,12 @@ static int mmc_sdio_suspend(struct mmc_host *host)
 		mmc_release_host(host);
 	}
 
-	if (!mmc_card_keep_power(host))
+	if (!mmc_card_keep_power(host)) {
 		mmc_power_off(host);
+	} else if (host->retune_period) {
+		mmc_retune_timer_stop(host);
+		mmc_retune_needed(host);
+	}
 
 	return 0;
 }
@@ -1056,6 +1029,12 @@ static int mmc_sdio_runtime_resume(struct mmc_host *host)
 	return mmc_sdio_power_restore(host);
 }
 
+static int mmc_sdio_reset(struct mmc_host *host)
+{
+	mmc_power_cycle(host, host->card->ocr);
+	return mmc_sdio_power_restore(host);
+}
+
 static const struct mmc_bus_ops mmc_sdio_ops = {
 	.remove = mmc_sdio_remove,
 	.detect = mmc_sdio_detect,
@@ -1066,6 +1045,7 @@ static const struct mmc_bus_ops mmc_sdio_ops = {
 	.runtime_resume = mmc_sdio_runtime_resume,
 	.power_restore = mmc_sdio_power_restore,
 	.alive = mmc_sdio_alive,
+	.reset = mmc_sdio_reset,
 };
 
 
diff --git a/drivers/mmc/core/sdio_bus.c b/drivers/mmc/core/sdio_bus.c
index bee02e644d62..7e327a6dd53d 100644
--- a/drivers/mmc/core/sdio_bus.c
+++ b/drivers/mmc/core/sdio_bus.c
@@ -137,6 +137,10 @@ static int sdio_bus_probe(struct device *dev)
 	if (!id)
 		return -ENODEV;
 
+	ret = dev_pm_domain_attach(dev, false);
+	if (ret == -EPROBE_DEFER)
+		return ret;
+
 	/* Unbound SDIO functions are always suspended.
 	 * During probe, the function is set active and the usage count
 	 * is incremented.  If the driver supports runtime PM,
@@ -166,6 +170,7 @@ static int sdio_bus_probe(struct device *dev)
 disable_runtimepm:
 	if (func->card->host->caps & MMC_CAP_POWER_OFF_CARD)
 		pm_runtime_put_noidle(dev);
+	dev_pm_domain_detach(dev, false);
 	return ret;
 }
 
@@ -197,6 +202,8 @@ static int sdio_bus_remove(struct device *dev)
 	if (func->card->host->caps & MMC_CAP_POWER_OFF_CARD)
 		pm_runtime_put_sync(dev);
 
+	dev_pm_domain_detach(dev, false);
+
 	return ret;
 }
 
@@ -316,10 +323,8 @@ int sdio_add_func(struct sdio_func *func)
 	sdio_set_of_node(func);
 	sdio_acpi_set_handle(func);
 	ret = device_add(&func->dev);
-	if (ret == 0) {
+	if (ret == 0)
 		sdio_func_set_present(func);
-		dev_pm_domain_attach(&func->dev, false);
-	}
 
 	return ret;
 }
@@ -335,7 +340,6 @@ void sdio_remove_func(struct sdio_func *func)
 	if (!sdio_func_present(func))
 		return;
 
-	dev_pm_domain_detach(&func->dev, false);
 	device_del(&func->dev);
 	of_node_put(func->dev.of_node);
 	put_device(&func->dev);
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index b1f837e749fe..fd9a58e216a5 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -219,6 +219,7 @@ config MMC_SDHCI_SIRF
 	tristate "SDHCI support on CSR SiRFprimaII and SiRFmarco SoCs"
 	depends on ARCH_SIRF
 	depends on MMC_SDHCI_PLTFM
+	select MMC_SDHCI_IO_ACCESSORS
 	help
 	  This selects the SDHCI support for SiRF System-on-Chip devices.
 
@@ -775,3 +776,11 @@ config MMC_TOSHIBA_PCI
 	tristate "Toshiba Type A SD/MMC Card Interface Driver"
 	depends on PCI
 	help
+
+config MMC_MTK
+	tristate "MediaTek SD/MMC Card Interface support"
+	help
+	  This selects the MediaTek(R) Secure digital and Multimedia card Interface.
+	  If you have a machine with a integrated SD/MMC card reader, say Y or M here.
+	  This is needed if support for any SD/SDIO/MMC devices is required.
+	  If unsure, say N.
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index e3ab5b968651..e928d61c5f4b 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_MMC_SDHCI_F_SDH30)	+= sdhci_f_sdh30.o
 obj-$(CONFIG_MMC_SDHCI_SPEAR)	+= sdhci-spear.o
 obj-$(CONFIG_MMC_WBSD)		+= wbsd.o
 obj-$(CONFIG_MMC_AU1X)		+= au1xmmc.o
+obj-$(CONFIG_MMC_MTK)		+= mtk-sd.o
 obj-$(CONFIG_MMC_OMAP)		+= omap.o
 obj-$(CONFIG_MMC_OMAP_HS)	+= omap_hsmmc.o
 obj-$(CONFIG_MMC_ATMELMCI)	+= atmel-mci.o
diff --git a/drivers/mmc/host/android-goldfish.c b/drivers/mmc/host/android-goldfish.c
index 8b4e20a3f16c..b1eac719a4cc 100644
--- a/drivers/mmc/host/android-goldfish.c
+++ b/drivers/mmc/host/android-goldfish.c
@@ -42,10 +42,10 @@
 #include <linux/spinlock.h>
 #include <linux/timer.h>
 #include <linux/clk.h>
+#include <linux/scatterlist.h>
 
 #include <asm/io.h>
 #include <asm/irq.h>
-#include <asm/scatterlist.h>
 
 #include <asm/types.h>
 #include <asm/io.h>
diff --git a/drivers/mmc/host/davinci_mmc.c b/drivers/mmc/host/davinci_mmc.c
index 1625f908dc70..ea2a2ebc6b91 100644
--- a/drivers/mmc/host/davinci_mmc.c
+++ b/drivers/mmc/host/davinci_mmc.c
@@ -1161,7 +1161,7 @@ static void __init init_mmcsd_host(struct mmc_davinci_host *host)
 	mmc_davinci_reset_ctrl(host, 0);
 }
 
-static struct platform_device_id davinci_mmc_devtype[] = {
+static const struct platform_device_id davinci_mmc_devtype[] = {
 	{
 		.name	= "dm6441-mmc",
 		.driver_data = MMC_CTLR_VERSION_1,
diff --git a/drivers/mmc/host/dw_mmc-exynos.c b/drivers/mmc/host/dw_mmc-exynos.c
index e761eb1b1441..1e75309898b7 100644
--- a/drivers/mmc/host/dw_mmc-exynos.c
+++ b/drivers/mmc/host/dw_mmc-exynos.c
@@ -556,4 +556,4 @@ module_platform_driver(dw_mci_exynos_pltfm_driver);
 MODULE_DESCRIPTION("Samsung Specific DW-MSHC Driver Extension");
 MODULE_AUTHOR("Thomas Abraham <thomas.ab@samsung.com");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:dwmmc-exynos");
+MODULE_ALIAS("platform:dwmmc_exynos");
diff --git a/drivers/mmc/host/dw_mmc-k3.c b/drivers/mmc/host/dw_mmc-k3.c
index 650f9cc3f7a6..63c2e2ed1288 100644
--- a/drivers/mmc/host/dw_mmc-k3.c
+++ b/drivers/mmc/host/dw_mmc-k3.c
@@ -8,16 +8,30 @@
  * (at your option) any later version.
  */
 
-#include <linux/module.h>
-#include <linux/platform_device.h>
 #include <linux/clk.h>
+#include <linux/mfd/syscon.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/dw_mmc.h>
+#include <linux/module.h>
 #include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
 
 #include "dw_mmc.h"
 #include "dw_mmc-pltfm.h"
 
+/*
+ * hi6220 sd only support io voltage 1.8v and 3v
+ * Also need config AO_SCTRL_SEL18 accordingly
+ */
+#define AO_SCTRL_SEL18		BIT(10)
+#define AO_SCTRL_CTRL3		0x40C
+
+struct k3_priv {
+	struct regmap	*reg;
+};
+
 static void dw_mci_k3_set_ios(struct dw_mci *host, struct mmc_ios *ios)
 {
 	int ret;
@@ -33,8 +47,93 @@ static const struct dw_mci_drv_data k3_drv_data = {
 	.set_ios		= dw_mci_k3_set_ios,
 };
 
+static int dw_mci_hi6220_parse_dt(struct dw_mci *host)
+{
+	struct k3_priv *priv;
+
+	priv = devm_kzalloc(host->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->reg = syscon_regmap_lookup_by_phandle(host->dev->of_node,
+					 "hisilicon,peripheral-syscon");
+	if (IS_ERR(priv->reg))
+		priv->reg = NULL;
+
+	host->priv = priv;
+	return 0;
+}
+
+static int dw_mci_hi6220_switch_voltage(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct dw_mci_slot *slot = mmc_priv(mmc);
+	struct k3_priv *priv;
+	struct dw_mci *host;
+	int min_uv, max_uv;
+	int ret;
+
+	host = slot->host;
+	priv = host->priv;
+
+	if (!priv || !priv->reg)
+		return 0;
+
+	if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_330) {
+		ret = regmap_update_bits(priv->reg, AO_SCTRL_CTRL3,
+					 AO_SCTRL_SEL18, 0);
+		min_uv = 3000000;
+		max_uv = 3000000;
+	} else if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_180) {
+		ret = regmap_update_bits(priv->reg, AO_SCTRL_CTRL3,
+					 AO_SCTRL_SEL18, AO_SCTRL_SEL18);
+		min_uv = 1800000;
+		max_uv = 1800000;
+	} else {
+		dev_dbg(host->dev, "voltage not supported\n");
+		return -EINVAL;
+	}
+
+	if (ret) {
+		dev_dbg(host->dev, "switch voltage failed\n");
+		return ret;
+	}
+
+	if (IS_ERR_OR_NULL(mmc->supply.vqmmc))
+		return 0;
+
+	ret = regulator_set_voltage(mmc->supply.vqmmc, min_uv, max_uv);
+	if (ret) {
+		dev_dbg(host->dev, "Regulator set error %d: %d - %d\n",
+				 ret, min_uv, max_uv);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void dw_mci_hi6220_set_ios(struct dw_mci *host, struct mmc_ios *ios)
+{
+	int ret;
+	unsigned int clock;
+
+	clock = (ios->clock <= 25000000) ? 25000000 : ios->clock;
+
+	ret = clk_set_rate(host->biu_clk, clock);
+	if (ret)
+		dev_warn(host->dev, "failed to set rate %uHz\n", clock);
+
+	host->bus_hz = clk_get_rate(host->biu_clk);
+}
+
+static const struct dw_mci_drv_data hi6220_data = {
+	.switch_voltage		= dw_mci_hi6220_switch_voltage,
+	.set_ios		= dw_mci_hi6220_set_ios,
+	.parse_dt		= dw_mci_hi6220_parse_dt,
+};
+
 static const struct of_device_id dw_mci_k3_match[] = {
 	{ .compatible = "hisilicon,hi4511-dw-mshc", .data = &k3_drv_data, },
+	{ .compatible = "hisilicon,hi6220-dw-mshc", .data = &hi6220_data, },
 	{},
 };
 MODULE_DEVICE_TABLE(of, dw_mci_k3_match);
@@ -94,4 +193,4 @@ module_platform_driver(dw_mci_k3_pltfm_driver);
 
 MODULE_DESCRIPTION("K3 Specific DW-MSHC Driver Extension");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:dwmmc-k3");
+MODULE_ALIAS("platform:dwmmc_k3");
diff --git a/drivers/mmc/host/dw_mmc-rockchip.c b/drivers/mmc/host/dw_mmc-rockchip.c
index dbf166f94f1b..de15121bba7d 100644
--- a/drivers/mmc/host/dw_mmc-rockchip.c
+++ b/drivers/mmc/host/dw_mmc-rockchip.c
@@ -153,5 +153,5 @@ module_platform_driver(dw_mci_rockchip_pltfm_driver);
 
 MODULE_AUTHOR("Addy Ke <addy.ke@rock-chips.com>");
 MODULE_DESCRIPTION("Rockchip Specific DW-MSHC Driver Extension");
-MODULE_ALIAS("platform:dwmmc-rockchip");
+MODULE_ALIAS("platform:dwmmc_rockchip");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index 5f5adafb253a..40e9d8e45f25 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -1236,11 +1236,15 @@ static int dw_mci_switch_voltage(struct mmc_host *mmc, struct mmc_ios *ios)
 {
 	struct dw_mci_slot *slot = mmc_priv(mmc);
 	struct dw_mci *host = slot->host;
+	const struct dw_mci_drv_data *drv_data = host->drv_data;
 	u32 uhs;
 	u32 v18 = SDMMC_UHS_18V << slot->id;
 	int min_uv, max_uv;
 	int ret;
 
+	if (drv_data && drv_data->switch_voltage)
+		return drv_data->switch_voltage(mmc, ios);
+
 	/*
 	 * Program the voltage.  Note that some instances of dw_mmc may use
 	 * the UHS_REG for this.  For other instances (like exynos) the UHS_REG
@@ -1278,10 +1282,7 @@ static int dw_mci_get_ro(struct mmc_host *mmc)
 	int gpio_ro = mmc_gpio_get_ro(mmc);
 
 	/* Use platform get_ro function, else try on board write protect */
-	if ((slot->quirks & DW_MCI_SLOT_QUIRK_NO_WRITE_PROTECT) ||
-			(slot->host->quirks & DW_MCI_QUIRK_NO_WRITE_PROTECT))
-		read_only = 0;
-	else if (!IS_ERR_VALUE(gpio_ro))
+	if (!IS_ERR_VALUE(gpio_ro))
 		read_only = gpio_ro;
 	else
 		read_only =
@@ -2280,9 +2281,10 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 }
 
 #ifdef CONFIG_OF
-/* given a slot id, find out the device node representing that slot */
-static struct device_node *dw_mci_of_find_slot_node(struct device *dev, u8 slot)
+/* given a slot, find out the device node representing that slot */
+static struct device_node *dw_mci_of_find_slot_node(struct dw_mci_slot *slot)
 {
+	struct device *dev = slot->mmc->parent;
 	struct device_node *np;
 	const __be32 *addr;
 	int len;
@@ -2294,42 +2296,28 @@ static struct device_node *dw_mci_of_find_slot_node(struct device *dev, u8 slot)
 		addr = of_get_property(np, "reg", &len);
 		if (!addr || (len < sizeof(int)))
 			continue;
-		if (be32_to_cpup(addr) == slot)
+		if (be32_to_cpup(addr) == slot->id)
 			return np;
 	}
 	return NULL;
 }
 
-static struct dw_mci_of_slot_quirks {
-	char *quirk;
-	int id;
-} of_slot_quirks[] = {
-	{
-		.quirk	= "disable-wp",
-		.id	= DW_MCI_SLOT_QUIRK_NO_WRITE_PROTECT,
-	},
-};
-
-static int dw_mci_of_get_slot_quirks(struct device *dev, u8 slot)
+static void dw_mci_slot_of_parse(struct dw_mci_slot *slot)
 {
-	struct device_node *np = dw_mci_of_find_slot_node(dev, slot);
-	int quirks = 0;
-	int idx;
+	struct device_node *np = dw_mci_of_find_slot_node(slot);
 
-	/* get quirks */
-	for (idx = 0; idx < ARRAY_SIZE(of_slot_quirks); idx++)
-		if (of_get_property(np, of_slot_quirks[idx].quirk, NULL)) {
-			dev_warn(dev, "Slot quirk %s is deprecated\n",
-					of_slot_quirks[idx].quirk);
-			quirks |= of_slot_quirks[idx].id;
-		}
+	if (!np)
+		return;
 
-	return quirks;
+	if (of_property_read_bool(np, "disable-wp")) {
+		slot->mmc->caps2 |= MMC_CAP2_NO_WRITE_PROTECT;
+		dev_warn(slot->mmc->parent,
+			"Slot quirk 'disable-wp' is deprecated\n");
+	}
 }
 #else /* CONFIG_OF */
-static int dw_mci_of_get_slot_quirks(struct device *dev, u8 slot)
+static void dw_mci_slot_of_parse(struct dw_mci_slot *slot)
 {
-	return 0;
 }
 #endif /* CONFIG_OF */
 
@@ -2352,8 +2340,6 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	slot->host = host;
 	host->slot[id] = slot;
 
-	slot->quirks = dw_mci_of_get_slot_quirks(host->dev, slot->id);
-
 	mmc->ops = &dw_mci_ops;
 	if (of_property_read_u32_array(host->dev->of_node,
 				       "clock-freq-min-max", freq, 2)) {
@@ -2391,6 +2377,8 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	if (host->pdata->caps2)
 		mmc->caps2 = host->pdata->caps2;
 
+	dw_mci_slot_of_parse(slot);
+
 	ret = mmc_of_parse(mmc);
 	if (ret)
 		goto err_host_allocated;
@@ -2618,9 +2606,6 @@ static struct dw_mci_of_quirks {
 	{
 		.quirk	= "broken-cd",
 		.id	= DW_MCI_QUIRK_BROKEN_CARD_DETECTION,
-	}, {
-		.quirk	= "disable-wp",
-		.id	= DW_MCI_QUIRK_NO_WRITE_PROTECT,
 	},
 };
 
@@ -2941,15 +2926,15 @@ void dw_mci_remove(struct dw_mci *host)
 {
 	int i;
 
-	mci_writel(host, RINTSTS, 0xFFFFFFFF);
-	mci_writel(host, INTMASK, 0); /* disable all mmc interrupt first */
-
 	for (i = 0; i < host->num_slots; i++) {
 		dev_dbg(host->dev, "remove slot %d\n", i);
 		if (host->slot[i])
 			dw_mci_cleanup_slot(host->slot[i], i);
 	}
 
+	mci_writel(host, RINTSTS, 0xFFFFFFFF);
+	mci_writel(host, INTMASK, 0); /* disable all mmc interrupt first */
+
 	/* disable clock to CIU */
 	mci_writel(host, CLKENA, 0);
 	mci_writel(host, CLKSRC, 0);
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index f45ab91de339..8ce4674730a6 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -227,7 +227,6 @@ extern int dw_mci_resume(struct dw_mci *host);
  * struct dw_mci_slot - MMC slot state
  * @mmc: The mmc_host representing this slot.
  * @host: The MMC controller this slot is using.
- * @quirks: Slot-level quirks (DW_MCI_SLOT_QUIRK_XXX)
  * @ctype: Card type for this slot.
  * @mrq: mmc_request currently being processed or waiting to be
  *	processed, or NULL when the slot is idle.
@@ -245,8 +244,6 @@ struct dw_mci_slot {
 	struct mmc_host		*mmc;
 	struct dw_mci		*host;
 
-	int			quirks;
-
 	u32			ctype;
 
 	struct mmc_request	*mrq;
@@ -287,5 +284,7 @@ struct dw_mci_drv_data {
 	int		(*execute_tuning)(struct dw_mci_slot *slot);
 	int		(*prepare_hs400_tuning)(struct dw_mci *host,
 						struct mmc_ios *ios);
+	int		(*switch_voltage)(struct mmc_host *mmc,
+					  struct mmc_ios *ios);
 };
 #endif /* _DW_MMC_H_ */
diff --git a/drivers/mmc/host/mtk-sd.c b/drivers/mmc/host/mtk-sd.c
new file mode 100644
index 000000000000..7153500dd007
--- /dev/null
+++ b/drivers/mmc/host/mtk-sd.c
@@ -0,0 +1,1462 @@
+/*
+ * Copyright (c) 2014-2015 MediaTek Inc.
+ * Author: Chaotian.Jing <chaotian.jing@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/module.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/ioport.h>
+#include <linux/irq.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/spinlock.h>
+
+#include <linux/mmc/card.h>
+#include <linux/mmc/core.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/sd.h>
+#include <linux/mmc/sdio.h>
+
+#define MAX_BD_NUM          1024
+
+/*--------------------------------------------------------------------------*/
+/* Common Definition                                                        */
+/*--------------------------------------------------------------------------*/
+#define MSDC_BUS_1BITS          0x0
+#define MSDC_BUS_4BITS          0x1
+#define MSDC_BUS_8BITS          0x2
+
+#define MSDC_BURST_64B          0x6
+
+/*--------------------------------------------------------------------------*/
+/* Register Offset                                                          */
+/*--------------------------------------------------------------------------*/
+#define MSDC_CFG         0x0
+#define MSDC_IOCON       0x04
+#define MSDC_PS          0x08
+#define MSDC_INT         0x0c
+#define MSDC_INTEN       0x10
+#define MSDC_FIFOCS      0x14
+#define SDC_CFG          0x30
+#define SDC_CMD          0x34
+#define SDC_ARG          0x38
+#define SDC_STS          0x3c
+#define SDC_RESP0        0x40
+#define SDC_RESP1        0x44
+#define SDC_RESP2        0x48
+#define SDC_RESP3        0x4c
+#define SDC_BLK_NUM      0x50
+#define SDC_ACMD_RESP    0x80
+#define MSDC_DMA_SA      0x90
+#define MSDC_DMA_CTRL    0x98
+#define MSDC_DMA_CFG     0x9c
+#define MSDC_PATCH_BIT   0xb0
+#define MSDC_PATCH_BIT1  0xb4
+#define MSDC_PAD_TUNE    0xec
+
+/*--------------------------------------------------------------------------*/
+/* Register Mask                                                            */
+/*--------------------------------------------------------------------------*/
+
+/* MSDC_CFG mask */
+#define MSDC_CFG_MODE           (0x1 << 0)	/* RW */
+#define MSDC_CFG_CKPDN          (0x1 << 1)	/* RW */
+#define MSDC_CFG_RST            (0x1 << 2)	/* RW */
+#define MSDC_CFG_PIO            (0x1 << 3)	/* RW */
+#define MSDC_CFG_CKDRVEN        (0x1 << 4)	/* RW */
+#define MSDC_CFG_BV18SDT        (0x1 << 5)	/* RW */
+#define MSDC_CFG_BV18PSS        (0x1 << 6)	/* R  */
+#define MSDC_CFG_CKSTB          (0x1 << 7)	/* R  */
+#define MSDC_CFG_CKDIV          (0xff << 8)	/* RW */
+#define MSDC_CFG_CKMOD          (0x3 << 16)	/* RW */
+
+/* MSDC_IOCON mask */
+#define MSDC_IOCON_SDR104CKS    (0x1 << 0)	/* RW */
+#define MSDC_IOCON_RSPL         (0x1 << 1)	/* RW */
+#define MSDC_IOCON_DSPL         (0x1 << 2)	/* RW */
+#define MSDC_IOCON_DDLSEL       (0x1 << 3)	/* RW */
+#define MSDC_IOCON_DDR50CKD     (0x1 << 4)	/* RW */
+#define MSDC_IOCON_DSPLSEL      (0x1 << 5)	/* RW */
+#define MSDC_IOCON_W_DSPL       (0x1 << 8)	/* RW */
+#define MSDC_IOCON_D0SPL        (0x1 << 16)	/* RW */
+#define MSDC_IOCON_D1SPL        (0x1 << 17)	/* RW */
+#define MSDC_IOCON_D2SPL        (0x1 << 18)	/* RW */
+#define MSDC_IOCON_D3SPL        (0x1 << 19)	/* RW */
+#define MSDC_IOCON_D4SPL        (0x1 << 20)	/* RW */
+#define MSDC_IOCON_D5SPL        (0x1 << 21)	/* RW */
+#define MSDC_IOCON_D6SPL        (0x1 << 22)	/* RW */
+#define MSDC_IOCON_D7SPL        (0x1 << 23)	/* RW */
+#define MSDC_IOCON_RISCSZ       (0x3 << 24)	/* RW */
+
+/* MSDC_PS mask */
+#define MSDC_PS_CDEN            (0x1 << 0)	/* RW */
+#define MSDC_PS_CDSTS           (0x1 << 1)	/* R  */
+#define MSDC_PS_CDDEBOUNCE      (0xf << 12)	/* RW */
+#define MSDC_PS_DAT             (0xff << 16)	/* R  */
+#define MSDC_PS_CMD             (0x1 << 24)	/* R  */
+#define MSDC_PS_WP              (0x1 << 31)	/* R  */
+
+/* MSDC_INT mask */
+#define MSDC_INT_MMCIRQ         (0x1 << 0)	/* W1C */
+#define MSDC_INT_CDSC           (0x1 << 1)	/* W1C */
+#define MSDC_INT_ACMDRDY        (0x1 << 3)	/* W1C */
+#define MSDC_INT_ACMDTMO        (0x1 << 4)	/* W1C */
+#define MSDC_INT_ACMDCRCERR     (0x1 << 5)	/* W1C */
+#define MSDC_INT_DMAQ_EMPTY     (0x1 << 6)	/* W1C */
+#define MSDC_INT_SDIOIRQ        (0x1 << 7)	/* W1C */
+#define MSDC_INT_CMDRDY         (0x1 << 8)	/* W1C */
+#define MSDC_INT_CMDTMO         (0x1 << 9)	/* W1C */
+#define MSDC_INT_RSPCRCERR      (0x1 << 10)	/* W1C */
+#define MSDC_INT_CSTA           (0x1 << 11)	/* R */
+#define MSDC_INT_XFER_COMPL     (0x1 << 12)	/* W1C */
+#define MSDC_INT_DXFER_DONE     (0x1 << 13)	/* W1C */
+#define MSDC_INT_DATTMO         (0x1 << 14)	/* W1C */
+#define MSDC_INT_DATCRCERR      (0x1 << 15)	/* W1C */
+#define MSDC_INT_ACMD19_DONE    (0x1 << 16)	/* W1C */
+#define MSDC_INT_DMA_BDCSERR    (0x1 << 17)	/* W1C */
+#define MSDC_INT_DMA_GPDCSERR   (0x1 << 18)	/* W1C */
+#define MSDC_INT_DMA_PROTECT    (0x1 << 19)	/* W1C */
+
+/* MSDC_INTEN mask */
+#define MSDC_INTEN_MMCIRQ       (0x1 << 0)	/* RW */
+#define MSDC_INTEN_CDSC         (0x1 << 1)	/* RW */
+#define MSDC_INTEN_ACMDRDY      (0x1 << 3)	/* RW */
+#define MSDC_INTEN_ACMDTMO      (0x1 << 4)	/* RW */
+#define MSDC_INTEN_ACMDCRCERR   (0x1 << 5)	/* RW */
+#define MSDC_INTEN_DMAQ_EMPTY   (0x1 << 6)	/* RW */
+#define MSDC_INTEN_SDIOIRQ      (0x1 << 7)	/* RW */
+#define MSDC_INTEN_CMDRDY       (0x1 << 8)	/* RW */
+#define MSDC_INTEN_CMDTMO       (0x1 << 9)	/* RW */
+#define MSDC_INTEN_RSPCRCERR    (0x1 << 10)	/* RW */
+#define MSDC_INTEN_CSTA         (0x1 << 11)	/* RW */
+#define MSDC_INTEN_XFER_COMPL   (0x1 << 12)	/* RW */
+#define MSDC_INTEN_DXFER_DONE   (0x1 << 13)	/* RW */
+#define MSDC_INTEN_DATTMO       (0x1 << 14)	/* RW */
+#define MSDC_INTEN_DATCRCERR    (0x1 << 15)	/* RW */
+#define MSDC_INTEN_ACMD19_DONE  (0x1 << 16)	/* RW */
+#define MSDC_INTEN_DMA_BDCSERR  (0x1 << 17)	/* RW */
+#define MSDC_INTEN_DMA_GPDCSERR (0x1 << 18)	/* RW */
+#define MSDC_INTEN_DMA_PROTECT  (0x1 << 19)	/* RW */
+
+/* MSDC_FIFOCS mask */
+#define MSDC_FIFOCS_RXCNT       (0xff << 0)	/* R */
+#define MSDC_FIFOCS_TXCNT       (0xff << 16)	/* R */
+#define MSDC_FIFOCS_CLR         (0x1 << 31)	/* RW */
+
+/* SDC_CFG mask */
+#define SDC_CFG_SDIOINTWKUP     (0x1 << 0)	/* RW */
+#define SDC_CFG_INSWKUP         (0x1 << 1)	/* RW */
+#define SDC_CFG_BUSWIDTH        (0x3 << 16)	/* RW */
+#define SDC_CFG_SDIO            (0x1 << 19)	/* RW */
+#define SDC_CFG_SDIOIDE         (0x1 << 20)	/* RW */
+#define SDC_CFG_INTATGAP        (0x1 << 21)	/* RW */
+#define SDC_CFG_DTOC            (0xff << 24)	/* RW */
+
+/* SDC_STS mask */
+#define SDC_STS_SDCBUSY         (0x1 << 0)	/* RW */
+#define SDC_STS_CMDBUSY         (0x1 << 1)	/* RW */
+#define SDC_STS_SWR_COMPL       (0x1 << 31)	/* RW */
+
+/* MSDC_DMA_CTRL mask */
+#define MSDC_DMA_CTRL_START     (0x1 << 0)	/* W */
+#define MSDC_DMA_CTRL_STOP      (0x1 << 1)	/* W */
+#define MSDC_DMA_CTRL_RESUME    (0x1 << 2)	/* W */
+#define MSDC_DMA_CTRL_MODE      (0x1 << 8)	/* RW */
+#define MSDC_DMA_CTRL_LASTBUF   (0x1 << 10)	/* RW */
+#define MSDC_DMA_CTRL_BRUSTSZ   (0x7 << 12)	/* RW */
+
+/* MSDC_DMA_CFG mask */
+#define MSDC_DMA_CFG_STS        (0x1 << 0)	/* R */
+#define MSDC_DMA_CFG_DECSEN     (0x1 << 1)	/* RW */
+#define MSDC_DMA_CFG_AHBHPROT2  (0x2 << 8)	/* RW */
+#define MSDC_DMA_CFG_ACTIVEEN   (0x2 << 12)	/* RW */
+#define MSDC_DMA_CFG_CS12B16B   (0x1 << 16)	/* RW */
+
+/* MSDC_PATCH_BIT mask */
+#define MSDC_PATCH_BIT_ODDSUPP    (0x1 <<  1)	/* RW */
+#define MSDC_INT_DAT_LATCH_CK_SEL (0x7 <<  7)
+#define MSDC_CKGEN_MSDC_DLY_SEL   (0x1f << 10)
+#define MSDC_PATCH_BIT_IODSSEL    (0x1 << 16)	/* RW */
+#define MSDC_PATCH_BIT_IOINTSEL   (0x1 << 17)	/* RW */
+#define MSDC_PATCH_BIT_BUSYDLY    (0xf << 18)	/* RW */
+#define MSDC_PATCH_BIT_WDOD       (0xf << 22)	/* RW */
+#define MSDC_PATCH_BIT_IDRTSEL    (0x1 << 26)	/* RW */
+#define MSDC_PATCH_BIT_CMDFSEL    (0x1 << 27)	/* RW */
+#define MSDC_PATCH_BIT_INTDLSEL   (0x1 << 28)	/* RW */
+#define MSDC_PATCH_BIT_SPCPUSH    (0x1 << 29)	/* RW */
+#define MSDC_PATCH_BIT_DECRCTMO   (0x1 << 30)	/* RW */
+
+#define REQ_CMD_EIO  (0x1 << 0)
+#define REQ_CMD_TMO  (0x1 << 1)
+#define REQ_DAT_ERR  (0x1 << 2)
+#define REQ_STOP_EIO (0x1 << 3)
+#define REQ_STOP_TMO (0x1 << 4)
+#define REQ_CMD_BUSY (0x1 << 5)
+
+#define MSDC_PREPARE_FLAG (0x1 << 0)
+#define MSDC_ASYNC_FLAG (0x1 << 1)
+#define MSDC_MMAP_FLAG (0x1 << 2)
+
+#define MTK_MMC_AUTOSUSPEND_DELAY	50
+#define CMD_TIMEOUT         (HZ/10 * 5)	/* 100ms x5 */
+#define DAT_TIMEOUT         (HZ    * 5)	/* 1000ms x5 */
+
+/*--------------------------------------------------------------------------*/
+/* Descriptor Structure                                                     */
+/*--------------------------------------------------------------------------*/
+struct mt_gpdma_desc {
+	u32 gpd_info;
+#define GPDMA_DESC_HWO		(0x1 << 0)
+#define GPDMA_DESC_BDP		(0x1 << 1)
+#define GPDMA_DESC_CHECKSUM	(0xff << 8) /* bit8 ~ bit15 */
+#define GPDMA_DESC_INT		(0x1 << 16)
+	u32 next;
+	u32 ptr;
+	u32 gpd_data_len;
+#define GPDMA_DESC_BUFLEN	(0xffff) /* bit0 ~ bit15 */
+#define GPDMA_DESC_EXTLEN	(0xff << 16) /* bit16 ~ bit23 */
+	u32 arg;
+	u32 blknum;
+	u32 cmd;
+};
+
+struct mt_bdma_desc {
+	u32 bd_info;
+#define BDMA_DESC_EOL		(0x1 << 0)
+#define BDMA_DESC_CHECKSUM	(0xff << 8) /* bit8 ~ bit15 */
+#define BDMA_DESC_BLKPAD	(0x1 << 17)
+#define BDMA_DESC_DWPAD		(0x1 << 18)
+	u32 next;
+	u32 ptr;
+	u32 bd_data_len;
+#define BDMA_DESC_BUFLEN	(0xffff) /* bit0 ~ bit15 */
+};
+
+struct msdc_dma {
+	struct scatterlist *sg;	/* I/O scatter list */
+	struct mt_gpdma_desc *gpd;		/* pointer to gpd array */
+	struct mt_bdma_desc *bd;		/* pointer to bd array */
+	dma_addr_t gpd_addr;	/* the physical address of gpd array */
+	dma_addr_t bd_addr;	/* the physical address of bd array */
+};
+
+struct msdc_save_para {
+	u32 msdc_cfg;
+	u32 iocon;
+	u32 sdc_cfg;
+	u32 pad_tune;
+	u32 patch_bit0;
+	u32 patch_bit1;
+};
+
+struct msdc_host {
+	struct device *dev;
+	struct mmc_host *mmc;	/* mmc structure */
+	int cmd_rsp;
+
+	spinlock_t lock;
+	struct mmc_request *mrq;
+	struct mmc_command *cmd;
+	struct mmc_data *data;
+	int error;
+
+	void __iomem *base;		/* host base address */
+
+	struct msdc_dma dma;	/* dma channel */
+	u64 dma_mask;
+
+	u32 timeout_ns;		/* data timeout ns */
+	u32 timeout_clks;	/* data timeout clks */
+
+	struct pinctrl *pinctrl;
+	struct pinctrl_state *pins_default;
+	struct pinctrl_state *pins_uhs;
+	struct delayed_work req_timeout;
+	int irq;		/* host interrupt */
+
+	struct clk *src_clk;	/* msdc source clock */
+	struct clk *h_clk;      /* msdc h_clk */
+	u32 mclk;		/* mmc subsystem clock frequency */
+	u32 src_clk_freq;	/* source clock frequency */
+	u32 sclk;		/* SD/MS bus clock frequency */
+	bool ddr;
+	bool vqmmc_enabled;
+	struct msdc_save_para save_para; /* used when gate HCLK */
+};
+
+static void sdr_set_bits(void __iomem *reg, u32 bs)
+{
+	u32 val = readl(reg);
+
+	val |= bs;
+	writel(val, reg);
+}
+
+static void sdr_clr_bits(void __iomem *reg, u32 bs)
+{
+	u32 val = readl(reg);
+
+	val &= ~bs;
+	writel(val, reg);
+}
+
+static void sdr_set_field(void __iomem *reg, u32 field, u32 val)
+{
+	unsigned int tv = readl(reg);
+
+	tv &= ~field;
+	tv |= ((val) << (ffs((unsigned int)field) - 1));
+	writel(tv, reg);
+}
+
+static void sdr_get_field(void __iomem *reg, u32 field, u32 *val)
+{
+	unsigned int tv = readl(reg);
+
+	*val = ((tv & field) >> (ffs((unsigned int)field) - 1));
+}
+
+static void msdc_reset_hw(struct msdc_host *host)
+{
+	u32 val;
+
+	sdr_set_bits(host->base + MSDC_CFG, MSDC_CFG_RST);
+	while (readl(host->base + MSDC_CFG) & MSDC_CFG_RST)
+		cpu_relax();
+
+	sdr_set_bits(host->base + MSDC_FIFOCS, MSDC_FIFOCS_CLR);
+	while (readl(host->base + MSDC_FIFOCS) & MSDC_FIFOCS_CLR)
+		cpu_relax();
+
+	val = readl(host->base + MSDC_INT);
+	writel(val, host->base + MSDC_INT);
+}
+
+static void msdc_cmd_next(struct msdc_host *host,
+		struct mmc_request *mrq, struct mmc_command *cmd);
+
+static u32 data_ints_mask = MSDC_INTEN_XFER_COMPL | MSDC_INTEN_DATTMO |
+			MSDC_INTEN_DATCRCERR | MSDC_INTEN_DMA_BDCSERR |
+			MSDC_INTEN_DMA_GPDCSERR | MSDC_INTEN_DMA_PROTECT;
+
+static u8 msdc_dma_calcs(u8 *buf, u32 len)
+{
+	u32 i, sum = 0;
+
+	for (i = 0; i < len; i++)
+		sum += buf[i];
+	return 0xff - (u8) sum;
+}
+
+static inline void msdc_dma_setup(struct msdc_host *host, struct msdc_dma *dma,
+		struct mmc_data *data)
+{
+	unsigned int j, dma_len;
+	dma_addr_t dma_address;
+	u32 dma_ctrl;
+	struct scatterlist *sg;
+	struct mt_gpdma_desc *gpd;
+	struct mt_bdma_desc *bd;
+
+	sg = data->sg;
+
+	gpd = dma->gpd;
+	bd = dma->bd;
+
+	/* modify gpd */
+	gpd->gpd_info |= GPDMA_DESC_HWO;
+	gpd->gpd_info |= GPDMA_DESC_BDP;
+	/* need to clear first. use these bits to calc checksum */
+	gpd->gpd_info &= ~GPDMA_DESC_CHECKSUM;
+	gpd->gpd_info |= msdc_dma_calcs((u8 *) gpd, 16) << 8;
+
+	/* modify bd */
+	for_each_sg(data->sg, sg, data->sg_count, j) {
+		dma_address = sg_dma_address(sg);
+		dma_len = sg_dma_len(sg);
+
+		/* init bd */
+		bd[j].bd_info &= ~BDMA_DESC_BLKPAD;
+		bd[j].bd_info &= ~BDMA_DESC_DWPAD;
+		bd[j].ptr = (u32)dma_address;
+		bd[j].bd_data_len &= ~BDMA_DESC_BUFLEN;
+		bd[j].bd_data_len |= (dma_len & BDMA_DESC_BUFLEN);
+
+		if (j == data->sg_count - 1) /* the last bd */
+			bd[j].bd_info |= BDMA_DESC_EOL;
+		else
+			bd[j].bd_info &= ~BDMA_DESC_EOL;
+
+		/* checksume need to clear first */
+		bd[j].bd_info &= ~BDMA_DESC_CHECKSUM;
+		bd[j].bd_info |= msdc_dma_calcs((u8 *)(&bd[j]), 16) << 8;
+	}
+
+	sdr_set_field(host->base + MSDC_DMA_CFG, MSDC_DMA_CFG_DECSEN, 1);
+	dma_ctrl = readl_relaxed(host->base + MSDC_DMA_CTRL);
+	dma_ctrl &= ~(MSDC_DMA_CTRL_BRUSTSZ | MSDC_DMA_CTRL_MODE);
+	dma_ctrl |= (MSDC_BURST_64B << 12 | 1 << 8);
+	writel_relaxed(dma_ctrl, host->base + MSDC_DMA_CTRL);
+	writel((u32)dma->gpd_addr, host->base + MSDC_DMA_SA);
+}
+
+static void msdc_prepare_data(struct msdc_host *host, struct mmc_request *mrq)
+{
+	struct mmc_data *data = mrq->data;
+
+	if (!(data->host_cookie & MSDC_PREPARE_FLAG)) {
+		bool read = (data->flags & MMC_DATA_READ) != 0;
+
+		data->host_cookie |= MSDC_PREPARE_FLAG;
+		data->sg_count = dma_map_sg(host->dev, data->sg, data->sg_len,
+					   read ? DMA_FROM_DEVICE : DMA_TO_DEVICE);
+	}
+}
+
+static void msdc_unprepare_data(struct msdc_host *host, struct mmc_request *mrq)
+{
+	struct mmc_data *data = mrq->data;
+
+	if (data->host_cookie & MSDC_ASYNC_FLAG)
+		return;
+
+	if (data->host_cookie & MSDC_PREPARE_FLAG) {
+		bool read = (data->flags & MMC_DATA_READ) != 0;
+
+		dma_unmap_sg(host->dev, data->sg, data->sg_len,
+			     read ? DMA_FROM_DEVICE : DMA_TO_DEVICE);
+		data->host_cookie &= ~MSDC_PREPARE_FLAG;
+	}
+}
+
+/* clock control primitives */
+static void msdc_set_timeout(struct msdc_host *host, u32 ns, u32 clks)
+{
+	u32 timeout, clk_ns;
+	u32 mode = 0;
+
+	host->timeout_ns = ns;
+	host->timeout_clks = clks;
+	if (host->sclk == 0) {
+		timeout = 0;
+	} else {
+		clk_ns  = 1000000000UL / host->sclk;
+		timeout = (ns + clk_ns - 1) / clk_ns + clks;
+		/* in 1048576 sclk cycle unit */
+		timeout = (timeout + (0x1 << 20) - 1) >> 20;
+		sdr_get_field(host->base + MSDC_CFG, MSDC_CFG_CKMOD, &mode);
+		/*DDR mode will double the clk cycles for data timeout */
+		timeout = mode >= 2 ? timeout * 2 : timeout;
+		timeout = timeout > 1 ? timeout - 1 : 0;
+		timeout = timeout > 255 ? 255 : timeout;
+	}
+	sdr_set_field(host->base + SDC_CFG, SDC_CFG_DTOC, timeout);
+}
+
+static void msdc_gate_clock(struct msdc_host *host)
+{
+	clk_disable_unprepare(host->src_clk);
+	clk_disable_unprepare(host->h_clk);
+}
+
+static void msdc_ungate_clock(struct msdc_host *host)
+{
+	clk_prepare_enable(host->h_clk);
+	clk_prepare_enable(host->src_clk);
+	while (!(readl(host->base + MSDC_CFG) & MSDC_CFG_CKSTB))
+		cpu_relax();
+}
+
+static void msdc_set_mclk(struct msdc_host *host, int ddr, u32 hz)
+{
+	u32 mode;
+	u32 flags;
+	u32 div;
+	u32 sclk;
+
+	if (!hz) {
+		dev_dbg(host->dev, "set mclk to 0\n");
+		host->mclk = 0;
+		sdr_clr_bits(host->base + MSDC_CFG, MSDC_CFG_CKPDN);
+		return;
+	}
+
+	flags = readl(host->base + MSDC_INTEN);
+	sdr_clr_bits(host->base + MSDC_INTEN, flags);
+	if (ddr) { /* may need to modify later */
+		mode = 0x2; /* ddr mode and use divisor */
+		if (hz >= (host->src_clk_freq >> 2)) {
+			div = 0; /* mean div = 1/4 */
+			sclk = host->src_clk_freq >> 2; /* sclk = clk / 4 */
+		} else {
+			div = (host->src_clk_freq + ((hz << 2) - 1)) / (hz << 2);
+			sclk = (host->src_clk_freq >> 2) / div;
+			div = (div >> 1);
+		}
+	} else if (hz >= host->src_clk_freq) {
+		mode = 0x1; /* no divisor */
+		div = 0;
+		sclk = host->src_clk_freq;
+	} else {
+		mode = 0x0; /* use divisor */
+		if (hz >= (host->src_clk_freq >> 1)) {
+			div = 0; /* mean div = 1/2 */
+			sclk = host->src_clk_freq >> 1; /* sclk = clk / 2 */
+		} else {
+			div = (host->src_clk_freq + ((hz << 2) - 1)) / (hz << 2);
+			sclk = (host->src_clk_freq >> 2) / div;
+		}
+	}
+	sdr_set_field(host->base + MSDC_CFG, MSDC_CFG_CKMOD | MSDC_CFG_CKDIV,
+			(mode << 8) | (div % 0xff));
+	sdr_set_bits(host->base + MSDC_CFG, MSDC_CFG_CKPDN);
+	while (!(readl(host->base + MSDC_CFG) & MSDC_CFG_CKSTB))
+		cpu_relax();
+	host->sclk = sclk;
+	host->mclk = hz;
+	host->ddr = ddr;
+	/* need because clk changed. */
+	msdc_set_timeout(host, host->timeout_ns, host->timeout_clks);
+	sdr_set_bits(host->base + MSDC_INTEN, flags);
+
+	dev_dbg(host->dev, "sclk: %d, ddr: %d\n", host->sclk, ddr);
+}
+
+static inline u32 msdc_cmd_find_resp(struct msdc_host *host,
+		struct mmc_request *mrq, struct mmc_command *cmd)
+{
+	u32 resp;
+
+	switch (mmc_resp_type(cmd)) {
+		/* Actually, R1, R5, R6, R7 are the same */
+	case MMC_RSP_R1:
+		resp = 0x1;
+		break;
+	case MMC_RSP_R1B:
+		resp = 0x7;
+		break;
+	case MMC_RSP_R2:
+		resp = 0x2;
+		break;
+	case MMC_RSP_R3:
+		resp = 0x3;
+		break;
+	case MMC_RSP_NONE:
+	default:
+		resp = 0x0;
+		break;
+	}
+
+	return resp;
+}
+
+static inline u32 msdc_cmd_prepare_raw_cmd(struct msdc_host *host,
+		struct mmc_request *mrq, struct mmc_command *cmd)
+{
+	/* rawcmd :
+	 * vol_swt << 30 | auto_cmd << 28 | blklen << 16 | go_irq << 15 |
+	 * stop << 14 | rw << 13 | dtype << 11 | rsptyp << 7 | brk << 6 | opcode
+	 */
+	u32 opcode = cmd->opcode;
+	u32 resp = msdc_cmd_find_resp(host, mrq, cmd);
+	u32 rawcmd = (opcode & 0x3f) | ((resp & 0x7) << 7);
+
+	host->cmd_rsp = resp;
+
+	if ((opcode == SD_IO_RW_DIRECT && cmd->flags == (unsigned int) -1) ||
+	    opcode == MMC_STOP_TRANSMISSION)
+		rawcmd |= (0x1 << 14);
+	else if (opcode == SD_SWITCH_VOLTAGE)
+		rawcmd |= (0x1 << 30);
+	else if (opcode == SD_APP_SEND_SCR ||
+		 opcode == SD_APP_SEND_NUM_WR_BLKS ||
+		 (opcode == SD_SWITCH && mmc_cmd_type(cmd) == MMC_CMD_ADTC) ||
+		 (opcode == SD_APP_SD_STATUS && mmc_cmd_type(cmd) == MMC_CMD_ADTC) ||
+		 (opcode == MMC_SEND_EXT_CSD && mmc_cmd_type(cmd) == MMC_CMD_ADTC))
+		rawcmd |= (0x1 << 11);
+
+	if (cmd->data) {
+		struct mmc_data *data = cmd->data;
+
+		if (mmc_op_multi(opcode)) {
+			if (mmc_card_mmc(host->mmc->card) && mrq->sbc &&
+			    !(mrq->sbc->arg & 0xFFFF0000))
+				rawcmd |= 0x2 << 28; /* AutoCMD23 */
+		}
+
+		rawcmd |= ((data->blksz & 0xFFF) << 16);
+		if (data->flags & MMC_DATA_WRITE)
+			rawcmd |= (0x1 << 13);
+		if (data->blocks > 1)
+			rawcmd |= (0x2 << 11);
+		else
+			rawcmd |= (0x1 << 11);
+		/* Always use dma mode */
+		sdr_clr_bits(host->base + MSDC_CFG, MSDC_CFG_PIO);
+
+		if (host->timeout_ns != data->timeout_ns ||
+		    host->timeout_clks != data->timeout_clks)
+			msdc_set_timeout(host, data->timeout_ns,
+					data->timeout_clks);
+
+		writel(data->blocks, host->base + SDC_BLK_NUM);
+	}
+	return rawcmd;
+}
+
+static void msdc_start_data(struct msdc_host *host, struct mmc_request *mrq,
+			    struct mmc_command *cmd, struct mmc_data *data)
+{
+	bool read;
+
+	WARN_ON(host->data);
+	host->data = data;
+	read = data->flags & MMC_DATA_READ;
+
+	mod_delayed_work(system_wq, &host->req_timeout, DAT_TIMEOUT);
+	msdc_dma_setup(host, &host->dma, data);
+	sdr_set_bits(host->base + MSDC_INTEN, data_ints_mask);
+	sdr_set_field(host->base + MSDC_DMA_CTRL, MSDC_DMA_CTRL_START, 1);
+	dev_dbg(host->dev, "DMA start\n");
+	dev_dbg(host->dev, "%s: cmd=%d DMA data: %d blocks; read=%d\n",
+			__func__, cmd->opcode, data->blocks, read);
+}
+
+static int msdc_auto_cmd_done(struct msdc_host *host, int events,
+		struct mmc_command *cmd)
+{
+	u32 *rsp = cmd->resp;
+
+	rsp[0] = readl(host->base + SDC_ACMD_RESP);
+
+	if (events & MSDC_INT_ACMDRDY) {
+		cmd->error = 0;
+	} else {
+		msdc_reset_hw(host);
+		if (events & MSDC_INT_ACMDCRCERR) {
+			cmd->error = -EILSEQ;
+			host->error |= REQ_STOP_EIO;
+		} else if (events & MSDC_INT_ACMDTMO) {
+			cmd->error = -ETIMEDOUT;
+			host->error |= REQ_STOP_TMO;
+		}
+		dev_err(host->dev,
+			"%s: AUTO_CMD%d arg=%08X; rsp %08X; cmd_error=%d\n",
+			__func__, cmd->opcode, cmd->arg, rsp[0], cmd->error);
+	}
+	return cmd->error;
+}
+
+static void msdc_track_cmd_data(struct msdc_host *host,
+				struct mmc_command *cmd, struct mmc_data *data)
+{
+	if (host->error)
+		dev_dbg(host->dev, "%s: cmd=%d arg=%08X; host->error=0x%08X\n",
+			__func__, cmd->opcode, cmd->arg, host->error);
+}
+
+static void msdc_request_done(struct msdc_host *host, struct mmc_request *mrq)
+{
+	unsigned long flags;
+	bool ret;
+
+	ret = cancel_delayed_work(&host->req_timeout);
+	if (!ret) {
+		/* delay work already running */
+		return;
+	}
+	spin_lock_irqsave(&host->lock, flags);
+	host->mrq = NULL;
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	msdc_track_cmd_data(host, mrq->cmd, mrq->data);
+	if (mrq->data)
+		msdc_unprepare_data(host, mrq);
+	mmc_request_done(host->mmc, mrq);
+
+	pm_runtime_mark_last_busy(host->dev);
+	pm_runtime_put_autosuspend(host->dev);
+}
+
+/* returns true if command is fully handled; returns false otherwise */
+static bool msdc_cmd_done(struct msdc_host *host, int events,
+			  struct mmc_request *mrq, struct mmc_command *cmd)
+{
+	bool done = false;
+	bool sbc_error;
+	unsigned long flags;
+	u32 *rsp = cmd->resp;
+
+	if (mrq->sbc && cmd == mrq->cmd &&
+	    (events & (MSDC_INT_ACMDRDY | MSDC_INT_ACMDCRCERR
+				   | MSDC_INT_ACMDTMO)))
+		msdc_auto_cmd_done(host, events, mrq->sbc);
+
+	sbc_error = mrq->sbc && mrq->sbc->error;
+
+	if (!sbc_error && !(events & (MSDC_INT_CMDRDY
+					| MSDC_INT_RSPCRCERR
+					| MSDC_INT_CMDTMO)))
+		return done;
+
+	spin_lock_irqsave(&host->lock, flags);
+	done = !host->cmd;
+	host->cmd = NULL;
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	if (done)
+		return true;
+
+	sdr_clr_bits(host->base + MSDC_INTEN, MSDC_INTEN_CMDRDY |
+			MSDC_INTEN_RSPCRCERR | MSDC_INTEN_CMDTMO |
+			MSDC_INTEN_ACMDRDY | MSDC_INTEN_ACMDCRCERR |
+			MSDC_INTEN_ACMDTMO);
+	writel(cmd->arg, host->base + SDC_ARG);
+
+	if (cmd->flags & MMC_RSP_PRESENT) {
+		if (cmd->flags & MMC_RSP_136) {
+			rsp[0] = readl(host->base + SDC_RESP3);
+			rsp[1] = readl(host->base + SDC_RESP2);
+			rsp[2] = readl(host->base + SDC_RESP1);
+			rsp[3] = readl(host->base + SDC_RESP0);
+		} else {
+			rsp[0] = readl(host->base + SDC_RESP0);
+		}
+	}
+
+	if (!sbc_error && !(events & MSDC_INT_CMDRDY)) {
+		msdc_reset_hw(host);
+		if (events & MSDC_INT_RSPCRCERR) {
+			cmd->error = -EILSEQ;
+			host->error |= REQ_CMD_EIO;
+		} else if (events & MSDC_INT_CMDTMO) {
+			cmd->error = -ETIMEDOUT;
+			host->error |= REQ_CMD_TMO;
+		}
+	}
+	if (cmd->error)
+		dev_dbg(host->dev,
+				"%s: cmd=%d arg=%08X; rsp %08X; cmd_error=%d\n",
+				__func__, cmd->opcode, cmd->arg, rsp[0],
+				cmd->error);
+
+	msdc_cmd_next(host, mrq, cmd);
+	return true;
+}
+
+/* It is the core layer's responsibility to ensure card status
+ * is correct before issue a request. but host design do below
+ * checks recommended.
+ */
+static inline bool msdc_cmd_is_ready(struct msdc_host *host,
+		struct mmc_request *mrq, struct mmc_command *cmd)
+{
+	/* The max busy time we can endure is 20ms */
+	unsigned long tmo = jiffies + msecs_to_jiffies(20);
+
+	while ((readl(host->base + SDC_STS) & SDC_STS_CMDBUSY) &&
+			time_before(jiffies, tmo))
+		cpu_relax();
+	if (readl(host->base + SDC_STS) & SDC_STS_CMDBUSY) {
+		dev_err(host->dev, "CMD bus busy detected\n");
+		host->error |= REQ_CMD_BUSY;
+		msdc_cmd_done(host, MSDC_INT_CMDTMO, mrq, cmd);
+		return false;
+	}
+
+	if (mmc_resp_type(cmd) == MMC_RSP_R1B || cmd->data) {
+		tmo = jiffies + msecs_to_jiffies(20);
+		/* R1B or with data, should check SDCBUSY */
+		while ((readl(host->base + SDC_STS) & SDC_STS_SDCBUSY) &&
+				time_before(jiffies, tmo))
+			cpu_relax();
+		if (readl(host->base + SDC_STS) & SDC_STS_SDCBUSY) {
+			dev_err(host->dev, "Controller busy detected\n");
+			host->error |= REQ_CMD_BUSY;
+			msdc_cmd_done(host, MSDC_INT_CMDTMO, mrq, cmd);
+			return false;
+		}
+	}
+	return true;
+}
+
+static void msdc_start_command(struct msdc_host *host,
+		struct mmc_request *mrq, struct mmc_command *cmd)
+{
+	u32 rawcmd;
+
+	WARN_ON(host->cmd);
+	host->cmd = cmd;
+
+	if (!msdc_cmd_is_ready(host, mrq, cmd))
+		return;
+
+	if ((readl(host->base + MSDC_FIFOCS) & MSDC_FIFOCS_TXCNT) >> 16 ||
+	    readl(host->base + MSDC_FIFOCS) & MSDC_FIFOCS_RXCNT) {
+		dev_err(host->dev, "TX/RX FIFO non-empty before start of IO. Reset\n");
+		msdc_reset_hw(host);
+	}
+
+	cmd->error = 0;
+	rawcmd = msdc_cmd_prepare_raw_cmd(host, mrq, cmd);
+	mod_delayed_work(system_wq, &host->req_timeout, DAT_TIMEOUT);
+
+	sdr_set_bits(host->base + MSDC_INTEN, MSDC_INTEN_CMDRDY |
+			MSDC_INTEN_RSPCRCERR | MSDC_INTEN_CMDTMO |
+			MSDC_INTEN_ACMDRDY | MSDC_INTEN_ACMDCRCERR |
+			MSDC_INTEN_ACMDTMO);
+	writel(cmd->arg, host->base + SDC_ARG);
+	writel(rawcmd, host->base + SDC_CMD);
+}
+
+static void msdc_cmd_next(struct msdc_host *host,
+		struct mmc_request *mrq, struct mmc_command *cmd)
+{
+	if (cmd->error || (mrq->sbc && mrq->sbc->error))
+		msdc_request_done(host, mrq);
+	else if (cmd == mrq->sbc)
+		msdc_start_command(host, mrq, mrq->cmd);
+	else if (!cmd->data)
+		msdc_request_done(host, mrq);
+	else
+		msdc_start_data(host, mrq, cmd, cmd->data);
+}
+
+static void msdc_ops_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+	struct msdc_host *host = mmc_priv(mmc);
+
+	host->error = 0;
+	WARN_ON(host->mrq);
+	host->mrq = mrq;
+
+	pm_runtime_get_sync(host->dev);
+
+	if (mrq->data)
+		msdc_prepare_data(host, mrq);
+
+	/* if SBC is required, we have HW option and SW option.
+	 * if HW option is enabled, and SBC does not have "special" flags,
+	 * use HW option,  otherwise use SW option
+	 */
+	if (mrq->sbc && (!mmc_card_mmc(mmc->card) ||
+	    (mrq->sbc->arg & 0xFFFF0000)))
+		msdc_start_command(host, mrq, mrq->sbc);
+	else
+		msdc_start_command(host, mrq, mrq->cmd);
+}
+
+static void msdc_pre_req(struct mmc_host *mmc, struct mmc_request *mrq,
+		bool is_first_req)
+{
+	struct msdc_host *host = mmc_priv(mmc);
+	struct mmc_data *data = mrq->data;
+
+	if (!data)
+		return;
+
+	msdc_prepare_data(host, mrq);
+	data->host_cookie |= MSDC_ASYNC_FLAG;
+}
+
+static void msdc_post_req(struct mmc_host *mmc, struct mmc_request *mrq,
+		int err)
+{
+	struct msdc_host *host = mmc_priv(mmc);
+	struct mmc_data *data;
+
+	data = mrq->data;
+	if (!data)
+		return;
+	if (data->host_cookie) {
+		data->host_cookie &= ~MSDC_ASYNC_FLAG;
+		msdc_unprepare_data(host, mrq);
+	}
+}
+
+static void msdc_data_xfer_next(struct msdc_host *host,
+				struct mmc_request *mrq, struct mmc_data *data)
+{
+	if (mmc_op_multi(mrq->cmd->opcode) && mrq->stop && !mrq->stop->error &&
+	    (!data->bytes_xfered || !mrq->sbc))
+		msdc_start_command(host, mrq, mrq->stop);
+	else
+		msdc_request_done(host, mrq);
+}
+
+static bool msdc_data_xfer_done(struct msdc_host *host, u32 events,
+				struct mmc_request *mrq, struct mmc_data *data)
+{
+	struct mmc_command *stop = data->stop;
+	unsigned long flags;
+	bool done;
+	unsigned int check_data = events &
+	    (MSDC_INT_XFER_COMPL | MSDC_INT_DATCRCERR | MSDC_INT_DATTMO
+	     | MSDC_INT_DMA_BDCSERR | MSDC_INT_DMA_GPDCSERR
+	     | MSDC_INT_DMA_PROTECT);
+
+	spin_lock_irqsave(&host->lock, flags);
+	done = !host->data;
+	if (check_data)
+		host->data = NULL;
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	if (done)
+		return true;
+
+	if (check_data || (stop && stop->error)) {
+		dev_dbg(host->dev, "DMA status: 0x%8X\n",
+				readl(host->base + MSDC_DMA_CFG));
+		sdr_set_field(host->base + MSDC_DMA_CTRL, MSDC_DMA_CTRL_STOP,
+				1);
+		while (readl(host->base + MSDC_DMA_CFG) & MSDC_DMA_CFG_STS)
+			cpu_relax();
+		sdr_clr_bits(host->base + MSDC_INTEN, data_ints_mask);
+		dev_dbg(host->dev, "DMA stop\n");
+
+		if ((events & MSDC_INT_XFER_COMPL) && (!stop || !stop->error)) {
+			data->bytes_xfered = data->blocks * data->blksz;
+		} else {
+			dev_err(host->dev, "interrupt events: %x\n", events);
+			msdc_reset_hw(host);
+			host->error |= REQ_DAT_ERR;
+			data->bytes_xfered = 0;
+
+			if (events & MSDC_INT_DATTMO)
+				data->error = -ETIMEDOUT;
+
+			dev_err(host->dev, "%s: cmd=%d; blocks=%d",
+				__func__, mrq->cmd->opcode, data->blocks);
+			dev_err(host->dev, "data_error=%d xfer_size=%d\n",
+					(int)data->error, data->bytes_xfered);
+		}
+
+		msdc_data_xfer_next(host, mrq, data);
+		done = true;
+	}
+	return done;
+}
+
+static void msdc_set_buswidth(struct msdc_host *host, u32 width)
+{
+	u32 val = readl(host->base + SDC_CFG);
+
+	val &= ~SDC_CFG_BUSWIDTH;
+
+	switch (width) {
+	default:
+	case MMC_BUS_WIDTH_1:
+		val |= (MSDC_BUS_1BITS << 16);
+		break;
+	case MMC_BUS_WIDTH_4:
+		val |= (MSDC_BUS_4BITS << 16);
+		break;
+	case MMC_BUS_WIDTH_8:
+		val |= (MSDC_BUS_8BITS << 16);
+		break;
+	}
+
+	writel(val, host->base + SDC_CFG);
+	dev_dbg(host->dev, "Bus Width = %d", width);
+}
+
+static int msdc_ops_switch_volt(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct msdc_host *host = mmc_priv(mmc);
+	int min_uv, max_uv;
+	int ret = 0;
+
+	if (!IS_ERR(mmc->supply.vqmmc)) {
+		if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_330) {
+			min_uv = 3300000;
+			max_uv = 3300000;
+		} else if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_180) {
+			min_uv = 1800000;
+			max_uv = 1800000;
+		} else {
+			dev_err(host->dev, "Unsupported signal voltage!\n");
+			return -EINVAL;
+		}
+
+		ret = regulator_set_voltage(mmc->supply.vqmmc, min_uv, max_uv);
+		if (ret) {
+			dev_err(host->dev,
+					"Regulator set error %d: %d - %d\n",
+					ret, min_uv, max_uv);
+		} else {
+			/* Apply different pinctrl settings for different signal voltage */
+			if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_180)
+				pinctrl_select_state(host->pinctrl, host->pins_uhs);
+			else
+				pinctrl_select_state(host->pinctrl, host->pins_default);
+		}
+	}
+	return ret;
+}
+
+static int msdc_card_busy(struct mmc_host *mmc)
+{
+	struct msdc_host *host = mmc_priv(mmc);
+	u32 status = readl(host->base + MSDC_PS);
+
+	/* check if any pin between dat[0:3] is low */
+	if (((status >> 16) & 0xf) != 0xf)
+		return 1;
+
+	return 0;
+}
+
+static void msdc_request_timeout(struct work_struct *work)
+{
+	struct msdc_host *host = container_of(work, struct msdc_host,
+			req_timeout.work);
+
+	/* simulate HW timeout status */
+	dev_err(host->dev, "%s: aborting cmd/data/mrq\n", __func__);
+	if (host->mrq) {
+		dev_err(host->dev, "%s: aborting mrq=%p cmd=%d\n", __func__,
+				host->mrq, host->mrq->cmd->opcode);
+		if (host->cmd) {
+			dev_err(host->dev, "%s: aborting cmd=%d\n",
+					__func__, host->cmd->opcode);
+			msdc_cmd_done(host, MSDC_INT_CMDTMO, host->mrq,
+					host->cmd);
+		} else if (host->data) {
+			dev_err(host->dev, "%s: abort data: cmd%d; %d blocks\n",
+					__func__, host->mrq->cmd->opcode,
+					host->data->blocks);
+			msdc_data_xfer_done(host, MSDC_INT_DATTMO, host->mrq,
+					host->data);
+		}
+	}
+}
+
+static irqreturn_t msdc_irq(int irq, void *dev_id)
+{
+	struct msdc_host *host = (struct msdc_host *) dev_id;
+
+	while (true) {
+		unsigned long flags;
+		struct mmc_request *mrq;
+		struct mmc_command *cmd;
+		struct mmc_data *data;
+		u32 events, event_mask;
+
+		spin_lock_irqsave(&host->lock, flags);
+		events = readl(host->base + MSDC_INT);
+		event_mask = readl(host->base + MSDC_INTEN);
+		/* clear interrupts */
+		writel(events & event_mask, host->base + MSDC_INT);
+
+		mrq = host->mrq;
+		cmd = host->cmd;
+		data = host->data;
+		spin_unlock_irqrestore(&host->lock, flags);
+
+		if (!(events & event_mask))
+			break;
+
+		if (!mrq) {
+			dev_err(host->dev,
+				"%s: MRQ=NULL; events=%08X; event_mask=%08X\n",
+				__func__, events, event_mask);
+			WARN_ON(1);
+			break;
+		}
+
+		dev_dbg(host->dev, "%s: events=%08X\n", __func__, events);
+
+		if (cmd)
+			msdc_cmd_done(host, events, mrq, cmd);
+		else if (data)
+			msdc_data_xfer_done(host, events, mrq, data);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void msdc_init_hw(struct msdc_host *host)
+{
+	u32 val;
+
+	/* Configure to MMC/SD mode, clock free running */
+	sdr_set_bits(host->base + MSDC_CFG, MSDC_CFG_MODE | MSDC_CFG_CKPDN);
+
+	/* Reset */
+	msdc_reset_hw(host);
+
+	/* Disable card detection */
+	sdr_clr_bits(host->base + MSDC_PS, MSDC_PS_CDEN);
+
+	/* Disable and clear all interrupts */
+	writel(0, host->base + MSDC_INTEN);
+	val = readl(host->base + MSDC_INT);
+	writel(val, host->base + MSDC_INT);
+
+	writel(0, host->base + MSDC_PAD_TUNE);
+	writel(0, host->base + MSDC_IOCON);
+	sdr_set_field(host->base + MSDC_IOCON, MSDC_IOCON_DDLSEL, 1);
+	writel(0x403c004f, host->base + MSDC_PATCH_BIT);
+	sdr_set_field(host->base + MSDC_PATCH_BIT, MSDC_CKGEN_MSDC_DLY_SEL, 1);
+	writel(0xffff0089, host->base + MSDC_PATCH_BIT1);
+	/* Configure to enable SDIO mode.
+	 * it's must otherwise sdio cmd5 failed
+	 */
+	sdr_set_bits(host->base + SDC_CFG, SDC_CFG_SDIO);
+
+	/* disable detect SDIO device interrupt function */
+	sdr_clr_bits(host->base + SDC_CFG, SDC_CFG_SDIOIDE);
+
+	/* Configure to default data timeout */
+	sdr_set_field(host->base + SDC_CFG, SDC_CFG_DTOC, 3);
+
+	dev_dbg(host->dev, "init hardware done!");
+}
+
+static void msdc_deinit_hw(struct msdc_host *host)
+{
+	u32 val;
+	/* Disable and clear all interrupts */
+	writel(0, host->base + MSDC_INTEN);
+
+	val = readl(host->base + MSDC_INT);
+	writel(val, host->base + MSDC_INT);
+}
+
+/* init gpd and bd list in msdc_drv_probe */
+static void msdc_init_gpd_bd(struct msdc_host *host, struct msdc_dma *dma)
+{
+	struct mt_gpdma_desc *gpd = dma->gpd;
+	struct mt_bdma_desc *bd = dma->bd;
+	int i;
+
+	memset(gpd, 0, sizeof(struct mt_gpdma_desc));
+
+	gpd->gpd_info = GPDMA_DESC_BDP; /* hwo, cs, bd pointer */
+	gpd->ptr = (u32)dma->bd_addr; /* physical address */
+
+	memset(bd, 0, sizeof(struct mt_bdma_desc) * MAX_BD_NUM);
+	for (i = 0; i < (MAX_BD_NUM - 1); i++)
+		bd[i].next = (u32)dma->bd_addr + sizeof(*bd) * (i + 1);
+}
+
+static void msdc_ops_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct msdc_host *host = mmc_priv(mmc);
+	int ret;
+	u32 ddr = 0;
+
+	pm_runtime_get_sync(host->dev);
+
+	if (ios->timing == MMC_TIMING_UHS_DDR50 ||
+	    ios->timing == MMC_TIMING_MMC_DDR52)
+		ddr = 1;
+
+	msdc_set_buswidth(host, ios->bus_width);
+
+	/* Suspend/Resume will do power off/on */
+	switch (ios->power_mode) {
+	case MMC_POWER_UP:
+		if (!IS_ERR(mmc->supply.vmmc)) {
+			ret = mmc_regulator_set_ocr(mmc, mmc->supply.vmmc,
+					ios->vdd);
+			if (ret) {
+				dev_err(host->dev, "Failed to set vmmc power!\n");
+				goto end;
+			}
+		}
+		break;
+	case MMC_POWER_ON:
+		if (!IS_ERR(mmc->supply.vqmmc) && !host->vqmmc_enabled) {
+			ret = regulator_enable(mmc->supply.vqmmc);
+			if (ret)
+				dev_err(host->dev, "Failed to set vqmmc power!\n");
+			else
+				host->vqmmc_enabled = true;
+		}
+		break;
+	case MMC_POWER_OFF:
+		if (!IS_ERR(mmc->supply.vmmc))
+			mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
+
+		if (!IS_ERR(mmc->supply.vqmmc) && host->vqmmc_enabled) {
+			regulator_disable(mmc->supply.vqmmc);
+			host->vqmmc_enabled = false;
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (host->mclk != ios->clock || host->ddr != ddr)
+		msdc_set_mclk(host, ddr, ios->clock);
+
+end:
+	pm_runtime_mark_last_busy(host->dev);
+	pm_runtime_put_autosuspend(host->dev);
+}
+
+static struct mmc_host_ops mt_msdc_ops = {
+	.post_req = msdc_post_req,
+	.pre_req = msdc_pre_req,
+	.request = msdc_ops_request,
+	.set_ios = msdc_ops_set_ios,
+	.start_signal_voltage_switch = msdc_ops_switch_volt,
+	.card_busy = msdc_card_busy,
+};
+
+static int msdc_drv_probe(struct platform_device *pdev)
+{
+	struct mmc_host *mmc;
+	struct msdc_host *host;
+	struct resource *res;
+	int ret;
+
+	if (!pdev->dev.of_node) {
+		dev_err(&pdev->dev, "No DT found\n");
+		return -EINVAL;
+	}
+	/* Allocate MMC host for this device */
+	mmc = mmc_alloc_host(sizeof(struct msdc_host), &pdev->dev);
+	if (!mmc)
+		return -ENOMEM;
+
+	host = mmc_priv(mmc);
+	ret = mmc_of_parse(mmc);
+	if (ret)
+		goto host_free;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	host->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(host->base)) {
+		ret = PTR_ERR(host->base);
+		goto host_free;
+	}
+
+	ret = mmc_regulator_get_supply(mmc);
+	if (ret == -EPROBE_DEFER)
+		goto host_free;
+
+	host->src_clk = devm_clk_get(&pdev->dev, "source");
+	if (IS_ERR(host->src_clk)) {
+		ret = PTR_ERR(host->src_clk);
+		goto host_free;
+	}
+
+	host->h_clk = devm_clk_get(&pdev->dev, "hclk");
+	if (IS_ERR(host->h_clk)) {
+		ret = PTR_ERR(host->h_clk);
+		goto host_free;
+	}
+
+	host->irq = platform_get_irq(pdev, 0);
+	if (host->irq < 0) {
+		ret = -EINVAL;
+		goto host_free;
+	}
+
+	host->pinctrl = devm_pinctrl_get(&pdev->dev);
+	if (IS_ERR(host->pinctrl)) {
+		ret = PTR_ERR(host->pinctrl);
+		dev_err(&pdev->dev, "Cannot find pinctrl!\n");
+		goto host_free;
+	}
+
+	host->pins_default = pinctrl_lookup_state(host->pinctrl, "default");
+	if (IS_ERR(host->pins_default)) {
+		ret = PTR_ERR(host->pins_default);
+		dev_err(&pdev->dev, "Cannot find pinctrl default!\n");
+		goto host_free;
+	}
+
+	host->pins_uhs = pinctrl_lookup_state(host->pinctrl, "state_uhs");
+	if (IS_ERR(host->pins_uhs)) {
+		ret = PTR_ERR(host->pins_uhs);
+		dev_err(&pdev->dev, "Cannot find pinctrl uhs!\n");
+		goto host_free;
+	}
+
+	host->dev = &pdev->dev;
+	host->mmc = mmc;
+	host->src_clk_freq = clk_get_rate(host->src_clk);
+	/* Set host parameters to mmc */
+	mmc->ops = &mt_msdc_ops;
+	mmc->f_min = host->src_clk_freq / (4 * 255);
+
+	mmc->caps |= MMC_CAP_ERASE | MMC_CAP_CMD23;
+	/* MMC core transfer sizes tunable parameters */
+	mmc->max_segs = MAX_BD_NUM;
+	mmc->max_seg_size = BDMA_DESC_BUFLEN;
+	mmc->max_blk_size = 2048;
+	mmc->max_req_size = 512 * 1024;
+	mmc->max_blk_count = mmc->max_req_size / 512;
+	host->dma_mask = DMA_BIT_MASK(32);
+	mmc_dev(mmc)->dma_mask = &host->dma_mask;
+
+	host->timeout_clks = 3 * 1048576;
+	host->dma.gpd = dma_alloc_coherent(&pdev->dev,
+				sizeof(struct mt_gpdma_desc),
+				&host->dma.gpd_addr, GFP_KERNEL);
+	host->dma.bd = dma_alloc_coherent(&pdev->dev,
+				MAX_BD_NUM * sizeof(struct mt_bdma_desc),
+				&host->dma.bd_addr, GFP_KERNEL);
+	if (!host->dma.gpd || !host->dma.bd) {
+		ret = -ENOMEM;
+		goto release_mem;
+	}
+	msdc_init_gpd_bd(host, &host->dma);
+	INIT_DELAYED_WORK(&host->req_timeout, msdc_request_timeout);
+	spin_lock_init(&host->lock);
+
+	platform_set_drvdata(pdev, mmc);
+	msdc_ungate_clock(host);
+	msdc_init_hw(host);
+
+	ret = devm_request_irq(&pdev->dev, host->irq, msdc_irq,
+		IRQF_TRIGGER_LOW | IRQF_ONESHOT, pdev->name, host);
+	if (ret)
+		goto release;
+
+	pm_runtime_set_active(host->dev);
+	pm_runtime_set_autosuspend_delay(host->dev, MTK_MMC_AUTOSUSPEND_DELAY);
+	pm_runtime_use_autosuspend(host->dev);
+	pm_runtime_enable(host->dev);
+	ret = mmc_add_host(mmc);
+
+	if (ret)
+		goto end;
+
+	return 0;
+end:
+	pm_runtime_disable(host->dev);
+release:
+	platform_set_drvdata(pdev, NULL);
+	msdc_deinit_hw(host);
+	msdc_gate_clock(host);
+release_mem:
+	if (host->dma.gpd)
+		dma_free_coherent(&pdev->dev,
+			sizeof(struct mt_gpdma_desc),
+			host->dma.gpd, host->dma.gpd_addr);
+	if (host->dma.bd)
+		dma_free_coherent(&pdev->dev,
+			MAX_BD_NUM * sizeof(struct mt_bdma_desc),
+			host->dma.bd, host->dma.bd_addr);
+host_free:
+	mmc_free_host(mmc);
+
+	return ret;
+}
+
+static int msdc_drv_remove(struct platform_device *pdev)
+{
+	struct mmc_host *mmc;
+	struct msdc_host *host;
+
+	mmc = platform_get_drvdata(pdev);
+	host = mmc_priv(mmc);
+
+	pm_runtime_get_sync(host->dev);
+
+	platform_set_drvdata(pdev, NULL);
+	mmc_remove_host(host->mmc);
+	msdc_deinit_hw(host);
+	msdc_gate_clock(host);
+
+	pm_runtime_disable(host->dev);
+	pm_runtime_put_noidle(host->dev);
+	dma_free_coherent(&pdev->dev,
+			sizeof(struct mt_gpdma_desc),
+			host->dma.gpd, host->dma.gpd_addr);
+	dma_free_coherent(&pdev->dev, MAX_BD_NUM * sizeof(struct mt_bdma_desc),
+			host->dma.bd, host->dma.bd_addr);
+
+	mmc_free_host(host->mmc);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static void msdc_save_reg(struct msdc_host *host)
+{
+	host->save_para.msdc_cfg = readl(host->base + MSDC_CFG);
+	host->save_para.iocon = readl(host->base + MSDC_IOCON);
+	host->save_para.sdc_cfg = readl(host->base + SDC_CFG);
+	host->save_para.pad_tune = readl(host->base + MSDC_PAD_TUNE);
+	host->save_para.patch_bit0 = readl(host->base + MSDC_PATCH_BIT);
+	host->save_para.patch_bit1 = readl(host->base + MSDC_PATCH_BIT1);
+}
+
+static void msdc_restore_reg(struct msdc_host *host)
+{
+	writel(host->save_para.msdc_cfg, host->base + MSDC_CFG);
+	writel(host->save_para.iocon, host->base + MSDC_IOCON);
+	writel(host->save_para.sdc_cfg, host->base + SDC_CFG);
+	writel(host->save_para.pad_tune, host->base + MSDC_PAD_TUNE);
+	writel(host->save_para.patch_bit0, host->base + MSDC_PATCH_BIT);
+	writel(host->save_para.patch_bit1, host->base + MSDC_PATCH_BIT1);
+}
+
+static int msdc_runtime_suspend(struct device *dev)
+{
+	struct mmc_host *mmc = dev_get_drvdata(dev);
+	struct msdc_host *host = mmc_priv(mmc);
+
+	msdc_save_reg(host);
+	msdc_gate_clock(host);
+	return 0;
+}
+
+static int msdc_runtime_resume(struct device *dev)
+{
+	struct mmc_host *mmc = dev_get_drvdata(dev);
+	struct msdc_host *host = mmc_priv(mmc);
+
+	msdc_ungate_clock(host);
+	msdc_restore_reg(host);
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops msdc_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(msdc_runtime_suspend, msdc_runtime_resume, NULL)
+};
+
+static const struct of_device_id msdc_of_ids[] = {
+	{   .compatible = "mediatek,mt8135-mmc", },
+	{}
+};
+
+static struct platform_driver mt_msdc_driver = {
+	.probe = msdc_drv_probe,
+	.remove = msdc_drv_remove,
+	.driver = {
+		.name = "mtk-msdc",
+		.of_match_table = msdc_of_ids,
+		.pm = &msdc_dev_pm_ops,
+	},
+};
+
+module_platform_driver(mt_msdc_driver);
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MediaTek SD/MMC Card Driver");
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index 317d709f7550..d110f9e98c4b 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -605,11 +605,7 @@ static int mxcmci_push(struct mxcmci_host *host, void *_buf, int bytes)
 		mxcmci_writel(host, cpu_to_le32(tmp), MMC_REG_BUFFER_ACCESS);
 	}
 
-	stat = mxcmci_poll_status(host, STATUS_BUF_WRITE_RDY);
-	if (stat)
-		return stat;
-
-	return 0;
+	return mxcmci_poll_status(host, STATUS_BUF_WRITE_RDY);
 }
 
 static int mxcmci_transfer_data(struct mxcmci_host *host)
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index a82411a2c024..d839147e591d 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -549,7 +549,7 @@ static const struct mmc_host_ops mxs_mmc_ops = {
 	.enable_sdio_irq = mxs_mmc_enable_sdio_irq,
 };
 
-static struct platform_device_id mxs_ssp_ids[] = {
+static const struct platform_device_id mxs_ssp_ids[] = {
 	{
 		.name = "imx23-mmc",
 		.driver_data = IMX23_SSP,
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 9df2b6801f76..b2b411da297b 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -43,6 +43,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_wakeirq.h>
 #include <linux/platform_data/hsmmc-omap.h>
 
 /* OMAP HSMMC Host Controller Registers */
@@ -218,7 +219,6 @@ struct omap_hsmmc_host {
 	unsigned int		flags;
 #define AUTO_CMD23		(1 << 0)        /* Auto CMD23 support */
 #define HSMMC_SDIO_IRQ_ENABLED	(1 << 1)        /* SDIO irq enabled */
-#define HSMMC_WAKE_IRQ_ENABLED	(1 << 2)
 	struct omap_hsmmc_next	next_data;
 	struct	omap_hsmmc_platform_data	*pdata;
 
@@ -1117,22 +1117,6 @@ static irqreturn_t omap_hsmmc_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t omap_hsmmc_wake_irq(int irq, void *dev_id)
-{
-	struct omap_hsmmc_host *host = dev_id;
-
-	/* cirq is level triggered, disable to avoid infinite loop */
-	spin_lock(&host->irq_lock);
-	if (host->flags & HSMMC_WAKE_IRQ_ENABLED) {
-		disable_irq_nosync(host->wake_irq);
-		host->flags &= ~HSMMC_WAKE_IRQ_ENABLED;
-	}
-	spin_unlock(&host->irq_lock);
-	pm_request_resume(host->dev); /* no use counter */
-
-	return IRQ_HANDLED;
-}
-
 static void set_sd_bus_power(struct omap_hsmmc_host *host)
 {
 	unsigned long i;
@@ -1665,7 +1649,6 @@ static void omap_hsmmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
 
 static int omap_hsmmc_configure_wake_irq(struct omap_hsmmc_host *host)
 {
-	struct mmc_host *mmc = host->mmc;
 	int ret;
 
 	/*
@@ -1677,11 +1660,7 @@ static int omap_hsmmc_configure_wake_irq(struct omap_hsmmc_host *host)
 	if (!host->dev->of_node || !host->wake_irq)
 		return -ENODEV;
 
-	/* Prevent auto-enabling of IRQ */
-	irq_set_status_flags(host->wake_irq, IRQ_NOAUTOEN);
-	ret = devm_request_irq(host->dev, host->wake_irq, omap_hsmmc_wake_irq,
-			       IRQF_TRIGGER_LOW | IRQF_ONESHOT,
-			       mmc_hostname(mmc), host);
+	ret = dev_pm_set_dedicated_wake_irq(host->dev, host->wake_irq);
 	if (ret) {
 		dev_err(mmc_dev(host->mmc), "Unable to request wake IRQ\n");
 		goto err;
@@ -1718,7 +1697,7 @@ static int omap_hsmmc_configure_wake_irq(struct omap_hsmmc_host *host)
 	return 0;
 
 err_free_irq:
-	devm_free_irq(host->dev, host->wake_irq, host);
+	dev_pm_clear_wake_irq(host->dev);
 err:
 	dev_warn(host->dev, "no SDIO IRQ support, falling back to polling\n");
 	host->wake_irq = 0;
@@ -2007,6 +1986,7 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 		omap_hsmmc_ops.multi_io_quirk = omap_hsmmc_multi_io_quirk;
 	}
 
+	device_init_wakeup(&pdev->dev, true);
 	pm_runtime_enable(host->dev);
 	pm_runtime_get_sync(host->dev);
 	pm_runtime_set_autosuspend_delay(host->dev, MMC_AUTOSUSPEND_DELAY);
@@ -2147,6 +2127,7 @@ err_slot_name:
 	if (host->use_reg)
 		omap_hsmmc_reg_put(host);
 err_irq:
+	device_init_wakeup(&pdev->dev, false);
 	if (host->tx_chan)
 		dma_release_channel(host->tx_chan);
 	if (host->rx_chan)
@@ -2178,6 +2159,7 @@ static int omap_hsmmc_remove(struct platform_device *pdev)
 
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
+	device_init_wakeup(&pdev->dev, false);
 	if (host->dbclk)
 		clk_disable_unprepare(host->dbclk);
 
@@ -2204,11 +2186,6 @@ static int omap_hsmmc_suspend(struct device *dev)
 				OMAP_HSMMC_READ(host->base, HCTL) & ~SDBP);
 	}
 
-	/* do not wake up due to sdio irq */
-	if ((host->mmc->caps & MMC_CAP_SDIO_IRQ) &&
-	    !(host->mmc->pm_flags & MMC_PM_WAKE_SDIO_IRQ))
-		disable_irq(host->wake_irq);
-
 	if (host->dbclk)
 		clk_disable_unprepare(host->dbclk);
 
@@ -2233,11 +2210,6 @@ static int omap_hsmmc_resume(struct device *dev)
 		omap_hsmmc_conf_bus_power(host);
 
 	omap_hsmmc_protect_card(host);
-
-	if ((host->mmc->caps & MMC_CAP_SDIO_IRQ) &&
-	    !(host->mmc->pm_flags & MMC_PM_WAKE_SDIO_IRQ))
-		enable_irq(host->wake_irq);
-
 	pm_runtime_mark_last_busy(host->dev);
 	pm_runtime_put_autosuspend(host->dev);
 	return 0;
@@ -2277,10 +2249,6 @@ static int omap_hsmmc_runtime_suspend(struct device *dev)
 		}
 
 		pinctrl_pm_select_idle_state(dev);
-
-		WARN_ON(host->flags & HSMMC_WAKE_IRQ_ENABLED);
-		enable_irq(host->wake_irq);
-		host->flags |= HSMMC_WAKE_IRQ_ENABLED;
 	} else {
 		pinctrl_pm_select_idle_state(dev);
 	}
@@ -2302,11 +2270,6 @@ static int omap_hsmmc_runtime_resume(struct device *dev)
 	spin_lock_irqsave(&host->irq_lock, flags);
 	if ((host->mmc->caps & MMC_CAP_SDIO_IRQ) &&
 	    (host->flags & HSMMC_SDIO_IRQ_ENABLED)) {
-		/* sdio irq flag can't change while in runtime suspend */
-		if (host->flags & HSMMC_WAKE_IRQ_ENABLED) {
-			disable_irq_nosync(host->wake_irq);
-			host->flags &= ~HSMMC_WAKE_IRQ_ENABLED;
-		}
 
 		pinctrl_pm_select_default_state(host->dev);
 
diff --git a/drivers/mmc/host/rtsx_pci_sdmmc.c b/drivers/mmc/host/rtsx_pci_sdmmc.c
index 1d3d6c4bfdc6..93137483ecde 100644
--- a/drivers/mmc/host/rtsx_pci_sdmmc.c
+++ b/drivers/mmc/host/rtsx_pci_sdmmc.c
@@ -1474,7 +1474,7 @@ static int rtsx_pci_sdmmc_drv_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id rtsx_pci_sdmmc_ids[] = {
+static const struct platform_device_id rtsx_pci_sdmmc_ids[] = {
 	{
 		.name = DRV_NAME_RTSX_PCI_SDMMC,
 	}, {
diff --git a/drivers/mmc/host/rtsx_usb_sdmmc.c b/drivers/mmc/host/rtsx_usb_sdmmc.c
index 88af827e086b..6c71fc9f76c7 100644
--- a/drivers/mmc/host/rtsx_usb_sdmmc.c
+++ b/drivers/mmc/host/rtsx_usb_sdmmc.c
@@ -1439,7 +1439,7 @@ static int rtsx_usb_sdmmc_drv_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id rtsx_usb_sdmmc_ids[] = {
+static const struct platform_device_id rtsx_usb_sdmmc_ids[] = {
 	{
 		.name = "rtsx_usb_sdmmc",
 	}, {
diff --git a/drivers/mmc/host/s3cmci.c b/drivers/mmc/host/s3cmci.c
index 94cddf381ba3..6291d5042ef2 100644
--- a/drivers/mmc/host/s3cmci.c
+++ b/drivers/mmc/host/s3cmci.c
@@ -1856,7 +1856,7 @@ static int s3cmci_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id s3cmci_driver_ids[] = {
+static const struct platform_device_id s3cmci_driver_ids[] = {
 	{
 		.name	= "s3c2410-sdi",
 		.driver_data	= 0,
diff --git a/drivers/mmc/host/sdhci-bcm2835.c b/drivers/mmc/host/sdhci-bcm2835.c
index 0ef0343c603a..1c65d4690e70 100644
--- a/drivers/mmc/host/sdhci-bcm2835.c
+++ b/drivers/mmc/host/sdhci-bcm2835.c
@@ -172,9 +172,19 @@ static int bcm2835_sdhci_probe(struct platform_device *pdev)
 		ret = PTR_ERR(pltfm_host->clk);
 		goto err;
 	}
+	ret = clk_prepare_enable(pltfm_host->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to enable host clk\n");
+		goto err;
+	}
 
-	return sdhci_add_host(host);
+	ret = sdhci_add_host(host);
+	if (ret)
+		goto err_clk;
 
+	return 0;
+err_clk:
+	clk_disable_unprepare(pltfm_host->clk);
 err:
 	sdhci_pltfm_free(pdev);
 	return ret;
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 82f512d87cb8..faf0cb910c96 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -4,7 +4,7 @@
  * derived from the OF-version.
  *
  * Copyright (c) 2010 Pengutronix e.K.
- *   Author: Wolfram Sang <w.sang@pengutronix.de>
+ *   Author: Wolfram Sang <kernel@pengutronix.de>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -112,6 +112,14 @@
 #define ESDHC_FLAG_STD_TUNING		BIT(5)
 /* The IP has SDHCI_CAPABILITIES_1 register */
 #define ESDHC_FLAG_HAVE_CAP1		BIT(6)
+/*
+ * The IP has errata ERR004536
+ * uSDHC: ADMA Length Mismatch Error occurs if the AHB read access is slow,
+ * when reading data from the card
+ */
+#define ESDHC_FLAG_ERR004536		BIT(7)
+/* The IP supports HS200 mode */
+#define ESDHC_FLAG_HS200		BIT(8)
 
 struct esdhc_soc_data {
 	u32 flags;
@@ -139,7 +147,13 @@ static struct esdhc_soc_data usdhc_imx6q_data = {
 
 static struct esdhc_soc_data usdhc_imx6sl_data = {
 	.flags = ESDHC_FLAG_USDHC | ESDHC_FLAG_STD_TUNING
-			| ESDHC_FLAG_HAVE_CAP1,
+			| ESDHC_FLAG_HAVE_CAP1 | ESDHC_FLAG_ERR004536
+			| ESDHC_FLAG_HS200,
+};
+
+static struct esdhc_soc_data usdhc_imx6sx_data = {
+	.flags = ESDHC_FLAG_USDHC | ESDHC_FLAG_STD_TUNING
+			| ESDHC_FLAG_HAVE_CAP1 | ESDHC_FLAG_HS200,
 };
 
 struct pltfm_imx_data {
@@ -161,7 +175,7 @@ struct pltfm_imx_data {
 	u32 is_ddr;
 };
 
-static struct platform_device_id imx_esdhc_devtype[] = {
+static const struct platform_device_id imx_esdhc_devtype[] = {
 	{
 		.name = "sdhci-esdhc-imx25",
 		.driver_data = (kernel_ulong_t) &esdhc_imx25_data,
@@ -182,6 +196,7 @@ static const struct of_device_id imx_esdhc_dt_ids[] = {
 	{ .compatible = "fsl,imx35-esdhc", .data = &esdhc_imx35_data, },
 	{ .compatible = "fsl,imx51-esdhc", .data = &esdhc_imx51_data, },
 	{ .compatible = "fsl,imx53-esdhc", .data = &esdhc_imx53_data, },
+	{ .compatible = "fsl,imx6sx-usdhc", .data = &usdhc_imx6sx_data, },
 	{ .compatible = "fsl,imx6sl-usdhc", .data = &usdhc_imx6sl_data, },
 	{ .compatible = "fsl,imx6q-usdhc", .data = &usdhc_imx6q_data, },
 	{ /* sentinel */ }
@@ -298,7 +313,7 @@ static void esdhc_writel_le(struct sdhci_host *host, u32 val, int reg)
 	u32 data;
 
 	if (unlikely(reg == SDHCI_INT_ENABLE || reg == SDHCI_SIGNAL_ENABLE)) {
-		if (val & SDHCI_INT_CARD_INT) {
+		if ((val & SDHCI_INT_CARD_INT) && !esdhc_is_usdhc(imx_data)) {
 			/*
 			 * Clear and then set D3CD bit to avoid missing the
 			 * card interrupt.  This is a eSDHC controller problem
@@ -313,6 +328,11 @@ static void esdhc_writel_le(struct sdhci_host *host, u32 val, int reg)
 			data |= ESDHC_CTRL_D3CD;
 			writel(data, host->ioaddr + SDHCI_HOST_CONTROL);
 		}
+
+		if (val & SDHCI_INT_ADMA_ERROR) {
+			val &= ~SDHCI_INT_ADMA_ERROR;
+			val |= ESDHC_INT_VENDOR_SPEC_DMA_ERR;
+		}
 	}
 
 	if (unlikely((imx_data->socdata->flags & ESDHC_FLAG_MULTIBLK_NO_INT)
@@ -333,13 +353,6 @@ static void esdhc_writel_le(struct sdhci_host *host, u32 val, int reg)
 			}
 	}
 
-	if (unlikely(reg == SDHCI_INT_ENABLE || reg == SDHCI_SIGNAL_ENABLE)) {
-		if (val & SDHCI_INT_ADMA_ERROR) {
-			val &= ~SDHCI_INT_ADMA_ERROR;
-			val |= ESDHC_INT_VENDOR_SPEC_DMA_ERR;
-		}
-	}
-
 	writel(val, host->ioaddr + reg);
 }
 
@@ -903,7 +916,8 @@ sdhci_esdhc_imx_probe_dt(struct platform_device *pdev,
 
 	mmc_of_parse_voltage(np, &host->ocr_mask);
 
-	return 0;
+	/* call to generic mmc_of_parse to support additional capabilities */
+	return mmc_of_parse(host->mmc);
 }
 #else
 static inline int
@@ -924,6 +938,7 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 	struct esdhc_platform_data *boarddata;
 	int err;
 	struct pltfm_imx_data *imx_data;
+	bool dt = true;
 
 	host = sdhci_pltfm_init(pdev, &sdhci_esdhc_imx_pdata, 0);
 	if (IS_ERR(host))
@@ -991,6 +1006,16 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 		writel(0x08100810, host->ioaddr + ESDHC_WTMK_LVL);
 		host->quirks2 |= SDHCI_QUIRK2_PRESET_VALUE_BROKEN;
 		host->mmc->caps |= MMC_CAP_1_8V_DDR;
+
+		if (!(imx_data->socdata->flags & ESDHC_FLAG_HS200))
+			host->quirks2 |= SDHCI_QUIRK2_BROKEN_HS200;
+
+		/*
+		* errata ESDHC_FLAG_ERR004536 fix for MX6Q TO1.2 and MX6DL
+		* TO1.1, it's harmless for MX6SL
+		*/
+		writel(readl(host->ioaddr + 0x6c) | BIT(7),
+			host->ioaddr + 0x6c);
 	}
 
 	if (imx_data->socdata->flags & ESDHC_FLAG_MAN_TUNING)
@@ -1002,6 +1027,9 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 			ESDHC_STD_TUNING_EN | ESDHC_TUNING_START_TAP,
 			host->ioaddr + ESDHC_TUNING_CTRL);
 
+	if (imx_data->socdata->flags & ESDHC_FLAG_ERR004536)
+		host->quirks |= SDHCI_QUIRK_BROKEN_ADMA;
+
 	boarddata = &imx_data->boarddata;
 	if (sdhci_esdhc_imx_probe_dt(pdev, host, boarddata) < 0) {
 		if (!host->mmc->parent->platform_data) {
@@ -1011,11 +1039,44 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 		}
 		imx_data->boarddata = *((struct esdhc_platform_data *)
 					host->mmc->parent->platform_data);
+		dt = false;
+	}
+	/* write_protect */
+	if (boarddata->wp_type == ESDHC_WP_GPIO && !dt) {
+		err = mmc_gpio_request_ro(host->mmc, boarddata->wp_gpio);
+		if (err) {
+			dev_err(mmc_dev(host->mmc),
+				"failed to request write-protect gpio!\n");
+			goto disable_clk;
+		}
+		host->mmc->caps2 |= MMC_CAP2_RO_ACTIVE_HIGH;
 	}
 
 	/* card_detect */
-	if (boarddata->cd_type == ESDHC_CD_CONTROLLER)
+	switch (boarddata->cd_type) {
+	case ESDHC_CD_GPIO:
+		if (dt)
+			break;
+		err = mmc_gpio_request_cd(host->mmc, boarddata->cd_gpio, 0);
+		if (err) {
+			dev_err(mmc_dev(host->mmc),
+				"failed to request card-detect gpio!\n");
+			goto disable_clk;
+		}
+		/* fall through */
+
+	case ESDHC_CD_CONTROLLER:
+		/* we have a working card_detect back */
 		host->quirks &= ~SDHCI_QUIRK_BROKEN_CARD_DETECTION;
+		break;
+
+	case ESDHC_CD_PERMANENT:
+		host->mmc->caps |= MMC_CAP_NONREMOVABLE;
+		break;
+
+	case ESDHC_CD_NONE:
+		break;
+	}
 
 	switch (boarddata->max_bus_width) {
 	case 8:
@@ -1048,11 +1109,6 @@ static int sdhci_esdhc_imx_probe(struct platform_device *pdev)
 		host->quirks2 |= SDHCI_QUIRK2_NO_1_8_V;
 	}
 
-	/* call to generic mmc_of_parse to support additional capabilities */
-	err = mmc_of_parse(host->mmc);
-	if (err)
-		goto disable_clk;
-
 	err = sdhci_add_host(host);
 	if (err)
 		goto disable_clk;
@@ -1151,5 +1207,5 @@ static struct platform_driver sdhci_esdhc_imx_driver = {
 module_platform_driver(sdhci_esdhc_imx_driver);
 
 MODULE_DESCRIPTION("SDHCI driver for Freescale i.MX eSDHC");
-MODULE_AUTHOR("Wolfram Sang <w.sang@pengutronix.de>");
+MODULE_AUTHOR("Wolfram Sang <kernel@pengutronix.de>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/host/sdhci-of-arasan.c b/drivers/mmc/host/sdhci-of-arasan.c
index 6287d426c96b..21c0c08dfe54 100644
--- a/drivers/mmc/host/sdhci-of-arasan.c
+++ b/drivers/mmc/host/sdhci-of-arasan.c
@@ -20,6 +20,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include "sdhci-pltfm.h"
 
 #define SDHCI_ARASAN_CLK_CTRL_OFFSET	0x2c
@@ -168,6 +169,11 @@ static int sdhci_arasan_probe(struct platform_device *pdev)
 		goto clk_disable_all;
 	}
 
+	if (of_device_is_compatible(pdev->dev.of_node, "arasan,sdhci-4.9a")) {
+		host->quirks |= SDHCI_QUIRK_NO_HISPD_BIT;
+		host->quirks2 |= SDHCI_QUIRK2_HOST_NO_CMD23;
+	}
+
 	sdhci_get_of_property(pdev);
 	pltfm_host = sdhci_priv(host);
 	pltfm_host->priv = sdhci_arasan;
@@ -208,6 +214,7 @@ static int sdhci_arasan_remove(struct platform_device *pdev)
 
 static const struct of_device_id sdhci_arasan_of_match[] = {
 	{ .compatible = "arasan,sdhci-8.9a" },
+	{ .compatible = "arasan,sdhci-4.9a" },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, sdhci_arasan_of_match);
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index 22e9111b11ff..797be7549a15 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -199,7 +199,7 @@ static unsigned int esdhc_of_get_min_clock(struct sdhci_host *host)
 
 static void esdhc_of_set_clock(struct sdhci_host *host, unsigned int clock)
 {
-	int pre_div = 2;
+	int pre_div = 1;
 	int div = 1;
 	u32 temp;
 
@@ -229,7 +229,7 @@ static void esdhc_of_set_clock(struct sdhci_host *host, unsigned int clock)
 
 	dev_dbg(mmc_dev(host->mmc), "desired SD clock: %d, actual: %d\n",
 		clock, host->max_clk / pre_div / div);
-
+	host->mmc->actual_clock = host->max_clk / pre_div / div;
 	pre_div >>= 1;
 	div--;
 
@@ -361,6 +361,13 @@ static int sdhci_esdhc_probe(struct platform_device *pdev)
 	sdhci_get_of_property(pdev);
 
 	np = pdev->dev.of_node;
+	if (of_device_is_compatible(np, "fsl,p5040-esdhc") ||
+	    of_device_is_compatible(np, "fsl,p5020-esdhc") ||
+	    of_device_is_compatible(np, "fsl,p4080-esdhc") ||
+	    of_device_is_compatible(np, "fsl,p1020-esdhc") ||
+	    of_device_is_compatible(np, "fsl,t1040-esdhc"))
+		host->quirks &= ~SDHCI_QUIRK_BROKEN_CARD_DETECTION;
+
 	if (of_device_is_compatible(np, "fsl,p2020-esdhc")) {
 		/*
 		 * Freescale messed up with P2020 as it has a non-standard
diff --git a/drivers/mmc/host/sdhci-pci-data.c b/drivers/mmc/host/sdhci-pci-data.c
index a611217769f5..56fddc622a54 100644
--- a/drivers/mmc/host/sdhci-pci-data.c
+++ b/drivers/mmc/host/sdhci-pci-data.c
@@ -3,3 +3,6 @@
 
 struct sdhci_pci_data *(*sdhci_pci_get_data)(struct pci_dev *pdev, int slotno);
 EXPORT_SYMBOL_GPL(sdhci_pci_get_data);
+
+int sdhci_pci_spt_drive_strength;
+EXPORT_SYMBOL_GPL(sdhci_pci_spt_drive_strength);
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index 7a3fc16d0a6c..94f54d2772e8 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -20,6 +20,7 @@
 #include <linux/slab.h>
 #include <linux/device.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
 #include <linux/scatterlist.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
@@ -266,6 +267,69 @@ static void sdhci_pci_int_hw_reset(struct sdhci_host *host)
 	usleep_range(300, 1000);
 }
 
+static int spt_select_drive_strength(struct sdhci_host *host,
+				     struct mmc_card *card,
+				     unsigned int max_dtr,
+				     int host_drv, int card_drv, int *drv_type)
+{
+	int drive_strength;
+
+	if (sdhci_pci_spt_drive_strength > 0)
+		drive_strength = sdhci_pci_spt_drive_strength & 0xf;
+	else
+		drive_strength = 1; /* 33-ohm */
+
+	if ((mmc_driver_type_mask(drive_strength) & card_drv) == 0)
+		drive_strength = 0; /* Default 50-ohm */
+
+	return drive_strength;
+}
+
+/* Try to read the drive strength from the card */
+static void spt_read_drive_strength(struct sdhci_host *host)
+{
+	u32 val, i, t;
+	u16 m;
+
+	if (sdhci_pci_spt_drive_strength)
+		return;
+
+	sdhci_pci_spt_drive_strength = -1;
+
+	m = sdhci_readw(host, SDHCI_HOST_CONTROL2) & 0x7;
+	if (m != 3 && m != 5)
+		return;
+	val = sdhci_readl(host, SDHCI_PRESENT_STATE);
+	if (val & 0x3)
+		return;
+	sdhci_writel(host, 0x007f0023, SDHCI_INT_ENABLE);
+	sdhci_writel(host, 0, SDHCI_SIGNAL_ENABLE);
+	sdhci_writew(host, 0x10, SDHCI_TRANSFER_MODE);
+	sdhci_writeb(host, 0xe, SDHCI_TIMEOUT_CONTROL);
+	sdhci_writew(host, 512, SDHCI_BLOCK_SIZE);
+	sdhci_writew(host, 1, SDHCI_BLOCK_COUNT);
+	sdhci_writel(host, 0, SDHCI_ARGUMENT);
+	sdhci_writew(host, 0x83b, SDHCI_COMMAND);
+	for (i = 0; i < 1000; i++) {
+		val = sdhci_readl(host, SDHCI_INT_STATUS);
+		if (val & 0xffff8000)
+			return;
+		if (val & 0x20)
+			break;
+		udelay(1);
+	}
+	val = sdhci_readl(host, SDHCI_PRESENT_STATE);
+	if (!(val & 0x800))
+		return;
+	for (i = 0; i < 47; i++)
+		val = sdhci_readl(host, SDHCI_BUFFER);
+	t = val & 0xf00;
+	if (t != 0x200 && t != 0x300)
+		return;
+
+	sdhci_pci_spt_drive_strength = 0x10 | ((val >> 12) & 0xf);
+}
+
 static int byt_emmc_probe_slot(struct sdhci_pci_slot *slot)
 {
 	slot->host->mmc->caps |= MMC_CAP_8_BIT_DATA | MMC_CAP_NONREMOVABLE |
@@ -276,6 +340,10 @@ static int byt_emmc_probe_slot(struct sdhci_pci_slot *slot)
 	slot->hw_reset = sdhci_pci_int_hw_reset;
 	if (slot->chip->pdev->device == PCI_DEVICE_ID_INTEL_BSW_EMMC)
 		slot->host->timeout_clk = 1000; /* 1000 kHz i.e. 1 MHz */
+	if (slot->chip->pdev->device == PCI_DEVICE_ID_INTEL_SPT_EMMC) {
+		spt_read_drive_strength(slot->host);
+		slot->select_drive_strength = spt_select_drive_strength;
+	}
 	return 0;
 }
 
@@ -302,6 +370,7 @@ static const struct sdhci_pci_fixes sdhci_intel_byt_emmc = {
 	.probe_slot	= byt_emmc_probe_slot,
 	.quirks		= SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC,
 	.quirks2	= SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
+			  SDHCI_QUIRK2_CAPS_BIT63_FOR_HS400 |
 			  SDHCI_QUIRK2_STOP_WITH_TC,
 };
 
@@ -655,14 +724,37 @@ static const struct sdhci_pci_fixes sdhci_rtsx = {
 	.probe_slot	= rtsx_probe_slot,
 };
 
+/*AMD chipset generation*/
+enum amd_chipset_gen {
+	AMD_CHIPSET_BEFORE_ML,
+	AMD_CHIPSET_CZ,
+	AMD_CHIPSET_NL,
+	AMD_CHIPSET_UNKNOWN,
+};
+
 static int amd_probe(struct sdhci_pci_chip *chip)
 {
 	struct pci_dev	*smbus_dev;
+	enum amd_chipset_gen gen;
 
 	smbus_dev = pci_get_device(PCI_VENDOR_ID_AMD,
 			PCI_DEVICE_ID_AMD_HUDSON2_SMBUS, NULL);
+	if (smbus_dev) {
+		gen = AMD_CHIPSET_BEFORE_ML;
+	} else {
+		smbus_dev = pci_get_device(PCI_VENDOR_ID_AMD,
+				PCI_DEVICE_ID_AMD_KERNCZ_SMBUS, NULL);
+		if (smbus_dev) {
+			if (smbus_dev->revision < 0x51)
+				gen = AMD_CHIPSET_CZ;
+			else
+				gen = AMD_CHIPSET_NL;
+		} else {
+			gen = AMD_CHIPSET_UNKNOWN;
+		}
+	}
 
-	if (smbus_dev && (smbus_dev->revision < 0x51)) {
+	if ((gen == AMD_CHIPSET_BEFORE_ML) || (gen == AMD_CHIPSET_CZ)) {
 		chip->quirks2 |= SDHCI_QUIRK2_CLEAR_TRANSFERMODE_REG_BEFORE_CMD;
 		chip->quirks2 |= SDHCI_QUIRK2_BROKEN_HS200;
 	}
@@ -1203,6 +1295,20 @@ static void sdhci_pci_hw_reset(struct sdhci_host *host)
 		slot->hw_reset(host);
 }
 
+static int sdhci_pci_select_drive_strength(struct sdhci_host *host,
+					   struct mmc_card *card,
+					   unsigned int max_dtr, int host_drv,
+					   int card_drv, int *drv_type)
+{
+	struct sdhci_pci_slot *slot = sdhci_priv(host);
+
+	if (!slot->select_drive_strength)
+		return 0;
+
+	return slot->select_drive_strength(host, card, max_dtr, host_drv,
+					   card_drv, drv_type);
+}
+
 static const struct sdhci_ops sdhci_pci_ops = {
 	.set_clock	= sdhci_set_clock,
 	.enable_dma	= sdhci_pci_enable_dma,
@@ -1210,6 +1316,7 @@ static const struct sdhci_ops sdhci_pci_ops = {
 	.reset		= sdhci_reset,
 	.set_uhs_signaling = sdhci_set_uhs_signaling,
 	.hw_reset		= sdhci_pci_hw_reset,
+	.select_drive_strength	= sdhci_pci_select_drive_strength,
 };
 
 /*****************************************************************************\
diff --git a/drivers/mmc/host/sdhci-pci.h b/drivers/mmc/host/sdhci-pci.h
index 1ec684d06d54..541f1cad5247 100644
--- a/drivers/mmc/host/sdhci-pci.h
+++ b/drivers/mmc/host/sdhci-pci.h
@@ -72,6 +72,10 @@ struct sdhci_pci_slot {
 	bool			cd_override_level;
 
 	void (*hw_reset)(struct sdhci_host *host);
+	int (*select_drive_strength)(struct sdhci_host *host,
+				     struct mmc_card *card,
+				     unsigned int max_dtr, int host_drv,
+				     int card_drv, int *drv_type);
 };
 
 struct sdhci_pci_chip {
diff --git a/drivers/mmc/host/sdhci-pxav2.c b/drivers/mmc/host/sdhci-pxav2.c
index f98008b5ea77..beffd8615489 100644
--- a/drivers/mmc/host/sdhci-pxav2.c
+++ b/drivers/mmc/host/sdhci-pxav2.c
@@ -252,9 +252,7 @@ static int sdhci_pxav2_remove(struct platform_device *pdev)
 static struct platform_driver sdhci_pxav2_driver = {
 	.driver		= {
 		.name	= "sdhci-pxav2",
-#ifdef CONFIG_OF
-		.of_match_table = sdhci_pxav2_of_match,
-#endif
+		.of_match_table = of_match_ptr(sdhci_pxav2_of_match),
 		.pm	= SDHCI_PLTFM_PMOPS,
 	},
 	.probe		= sdhci_pxav2_probe,
diff --git a/drivers/mmc/host/sdhci-pxav3.c b/drivers/mmc/host/sdhci-pxav3.c
index b5103a247bc1..9cd5fc62f130 100644
--- a/drivers/mmc/host/sdhci-pxav3.c
+++ b/drivers/mmc/host/sdhci-pxav3.c
@@ -457,12 +457,8 @@ static int sdhci_pxav3_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, host);
 
-	if (host->mmc->pm_caps & MMC_PM_KEEP_POWER) {
+	if (host->mmc->pm_caps & MMC_PM_WAKE_SDIO_IRQ)
 		device_init_wakeup(&pdev->dev, 1);
-		host->mmc->pm_flags |= MMC_PM_WAKE_SDIO_IRQ;
-	} else {
-		device_init_wakeup(&pdev->dev, 0);
-	}
 
 	pm_runtime_put_autosuspend(&pdev->dev);
 
@@ -578,9 +574,7 @@ static const struct dev_pm_ops sdhci_pxav3_pmops = {
 static struct platform_driver sdhci_pxav3_driver = {
 	.driver		= {
 		.name	= "sdhci-pxav3",
-#ifdef CONFIG_OF
-		.of_match_table = sdhci_pxav3_of_match,
-#endif
+		.of_match_table = of_match_ptr(sdhci_pxav3_of_match),
 		.pm	= SDHCI_PXAV3_PMOPS,
 	},
 	.probe		= sdhci_pxav3_probe,
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index c6d2dd7317c1..70c724bc6fc7 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -736,7 +736,7 @@ static struct sdhci_s3c_drv_data exynos4_sdhci_drv_data = {
 #define EXYNOS4_SDHCI_DRV_DATA ((kernel_ulong_t)NULL)
 #endif
 
-static struct platform_device_id sdhci_s3c_driver_ids[] = {
+static const struct platform_device_id sdhci_s3c_driver_ids[] = {
 	{
 		.name		= "s3c-sdhci",
 		.driver_data	= (kernel_ulong_t)NULL,
diff --git a/drivers/mmc/host/sdhci-sirf.c b/drivers/mmc/host/sdhci-sirf.c
index 32848eb7ad80..0110bae25b7e 100644
--- a/drivers/mmc/host/sdhci-sirf.c
+++ b/drivers/mmc/host/sdhci-sirf.c
@@ -17,7 +17,7 @@
 
 #define SDHCI_CLK_DELAY_SETTING 0x4C
 #define SDHCI_SIRF_8BITBUS BIT(3)
-#define SIRF_TUNING_COUNT 128
+#define SIRF_TUNING_COUNT 16384
 
 struct sdhci_sirf_priv {
 	int gpio_cd;
@@ -43,10 +43,43 @@ static void sdhci_sirf_set_bus_width(struct sdhci_host *host, int width)
 	sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
 }
 
+static u32 sdhci_sirf_readl_le(struct sdhci_host *host, int reg)
+{
+	u32 val = readl(host->ioaddr + reg);
+
+	if (unlikely((reg == SDHCI_CAPABILITIES_1) &&
+			(host->mmc->caps & MMC_CAP_UHS_SDR50))) {
+		/* fake CAP_1 register */
+		val = SDHCI_SUPPORT_SDR50 | SDHCI_USE_SDR50_TUNING;
+	}
+
+	if (unlikely(reg == SDHCI_SLOT_INT_STATUS)) {
+		u32 prss = val;
+		/* fake chips as V3.0 host conreoller */
+		prss &= ~(0xFF << 16);
+		val = prss | (SDHCI_SPEC_300 << 16);
+	}
+	return val;
+}
+
+static u16 sdhci_sirf_readw_le(struct sdhci_host *host, int reg)
+{
+	u16 ret = 0;
+
+	ret = readw(host->ioaddr + reg);
+
+	if (unlikely(reg == SDHCI_HOST_VERSION)) {
+		ret = readw(host->ioaddr + SDHCI_HOST_VERSION);
+		ret |= SDHCI_SPEC_300;
+	}
+
+	return ret;
+}
+
 static int sdhci_sirf_execute_tuning(struct sdhci_host *host, u32 opcode)
 {
 	int tuning_seq_cnt = 3;
-	u8 phase, tuned_phases[SIRF_TUNING_COUNT];
+	int phase;
 	u8 tuned_phase_cnt = 0;
 	int rc = 0, longest_range = 0;
 	int start = -1, end = 0, tuning_value = -1, range = 0;
@@ -58,6 +91,7 @@ static int sdhci_sirf_execute_tuning(struct sdhci_host *host, u32 opcode)
 
 retry:
 	phase = 0;
+	tuned_phase_cnt = 0;
 	do {
 		sdhci_writel(host,
 			clock_setting | phase,
@@ -65,7 +99,7 @@ retry:
 
 		if (!mmc_send_tuning(mmc)) {
 			/* Tuning is successful at this tuning point */
-			tuned_phases[tuned_phase_cnt++] = phase;
+			tuned_phase_cnt++;
 			dev_dbg(mmc_dev(mmc), "%s: Found good phase = %d\n",
 				 mmc_hostname(mmc), phase);
 			if (start == -1)
@@ -85,7 +119,7 @@ retry:
 			start = -1;
 			end = range = 0;
 		}
-	} while (++phase < ARRAY_SIZE(tuned_phases));
+	} while (++phase < SIRF_TUNING_COUNT);
 
 	if (tuned_phase_cnt && tuning_value > 0) {
 		/*
@@ -112,6 +146,8 @@ retry:
 }
 
 static struct sdhci_ops sdhci_sirf_ops = {
+	.read_l = sdhci_sirf_readl_le,
+	.read_w = sdhci_sirf_readw_le,
 	.platform_execute_tuning = sdhci_sirf_execute_tuning,
 	.set_clock = sdhci_set_clock,
 	.get_max_clock	= sdhci_pltfm_clk_get_max_clock,
diff --git a/drivers/mmc/host/sdhci-st.c b/drivers/mmc/host/sdhci-st.c
index 682f2bb0f4bf..969c2b0d57fd 100644
--- a/drivers/mmc/host/sdhci-st.c
+++ b/drivers/mmc/host/sdhci-st.c
@@ -509,4 +509,4 @@ module_platform_driver(sdhci_st_driver);
 MODULE_DESCRIPTION("SDHCI driver for STMicroelectronics SoCs");
 MODULE_AUTHOR("Giuseppe Cavallaro <peppe.cavallaro@st.com>");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:st-sdhci");
+MODULE_ALIAS("platform:sdhci-st");
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index c80287a02735..bc1445238fb3 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -52,7 +52,6 @@ static void sdhci_finish_data(struct sdhci_host *);
 
 static void sdhci_finish_command(struct sdhci_host *);
 static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode);
-static void sdhci_tuning_timer(unsigned long data);
 static void sdhci_enable_preset_value(struct sdhci_host *host, bool enable);
 static int sdhci_pre_dma_transfer(struct sdhci_host *host,
 					struct mmc_data *data,
@@ -254,17 +253,6 @@ static void sdhci_init(struct sdhci_host *host, int soft)
 static void sdhci_reinit(struct sdhci_host *host)
 {
 	sdhci_init(host, 0);
-	/*
-	 * Retuning stuffs are affected by different cards inserted and only
-	 * applicable to UHS-I cards. So reset these fields to their initial
-	 * value when card is removed.
-	 */
-	if (host->flags & SDHCI_USING_RETUNING_TIMER) {
-		host->flags &= ~SDHCI_USING_RETUNING_TIMER;
-
-		del_timer_sync(&host->tuning_timer);
-		host->flags &= ~SDHCI_NEEDS_RETUNING;
-	}
 	sdhci_enable_card_detection(host);
 }
 
@@ -328,8 +316,7 @@ static void sdhci_read_block_pio(struct sdhci_host *host)
 	local_irq_save(flags);
 
 	while (blksize) {
-		if (!sg_miter_next(&host->sg_miter))
-			BUG();
+		BUG_ON(!sg_miter_next(&host->sg_miter));
 
 		len = min(host->sg_miter.length, blksize);
 
@@ -374,8 +361,7 @@ static void sdhci_write_block_pio(struct sdhci_host *host)
 	local_irq_save(flags);
 
 	while (blksize) {
-		if (!sg_miter_next(&host->sg_miter))
-			BUG();
+		BUG_ON(!sg_miter_next(&host->sg_miter));
 
 		len = min(host->sg_miter.length, blksize);
 
@@ -848,7 +834,7 @@ static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_command *cmd)
 			int sg_cnt;
 
 			sg_cnt = sdhci_pre_dma_transfer(host, data, NULL);
-			if (sg_cnt == 0) {
+			if (sg_cnt <= 0) {
 				/*
 				 * This only happens when someone fed
 				 * us an invalid request.
@@ -1353,7 +1339,6 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	struct sdhci_host *host;
 	int present;
 	unsigned long flags;
-	u32 tuning_opcode;
 
 	host = mmc_priv(mmc);
 
@@ -1387,39 +1372,6 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 		host->mrq->cmd->error = -ENOMEDIUM;
 		tasklet_schedule(&host->finish_tasklet);
 	} else {
-		u32 present_state;
-
-		present_state = sdhci_readl(host, SDHCI_PRESENT_STATE);
-		/*
-		 * Check if the re-tuning timer has already expired and there
-		 * is no on-going data transfer and DAT0 is not busy. If so,
-		 * we need to execute tuning procedure before sending command.
-		 */
-		if ((host->flags & SDHCI_NEEDS_RETUNING) &&
-		    !(present_state & (SDHCI_DOING_WRITE | SDHCI_DOING_READ)) &&
-		    (present_state & SDHCI_DATA_0_LVL_MASK)) {
-			if (mmc->card) {
-				/* eMMC uses cmd21 but sd and sdio use cmd19 */
-				tuning_opcode =
-					mmc->card->type == MMC_TYPE_MMC ?
-					MMC_SEND_TUNING_BLOCK_HS200 :
-					MMC_SEND_TUNING_BLOCK;
-
-				/* Here we need to set the host->mrq to NULL,
-				 * in case the pending finish_tasklet
-				 * finishes it incorrectly.
-				 */
-				host->mrq = NULL;
-
-				spin_unlock_irqrestore(&host->lock, flags);
-				sdhci_execute_tuning(mmc, tuning_opcode);
-				spin_lock_irqsave(&host->lock, flags);
-
-				/* Restore original mmc_request structure */
-				host->mrq = mrq;
-			}
-		}
-
 		if (mrq->sbc && !(host->flags & SDHCI_AUTO_CMD23))
 			sdhci_send_command(host, mrq->sbc);
 		else
@@ -1562,8 +1514,17 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 			ctrl_2 &= ~SDHCI_CTRL_DRV_TYPE_MASK;
 			if (ios->drv_type == MMC_SET_DRIVER_TYPE_A)
 				ctrl_2 |= SDHCI_CTRL_DRV_TYPE_A;
+			else if (ios->drv_type == MMC_SET_DRIVER_TYPE_B)
+				ctrl_2 |= SDHCI_CTRL_DRV_TYPE_B;
 			else if (ios->drv_type == MMC_SET_DRIVER_TYPE_C)
 				ctrl_2 |= SDHCI_CTRL_DRV_TYPE_C;
+			else if (ios->drv_type == MMC_SET_DRIVER_TYPE_D)
+				ctrl_2 |= SDHCI_CTRL_DRV_TYPE_D;
+			else {
+				pr_warn("%s: invalid driver type, default to "
+					"driver type B\n", mmc_hostname(mmc));
+				ctrl_2 |= SDHCI_CTRL_DRV_TYPE_B;
+			}
 
 			sdhci_writew(host, ctrl_2, SDHCI_HOST_CONTROL2);
 		} else {
@@ -2065,23 +2026,18 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 	}
 
 out:
-	host->flags &= ~SDHCI_NEEDS_RETUNING;
-
 	if (tuning_count) {
-		host->flags |= SDHCI_USING_RETUNING_TIMER;
-		mod_timer(&host->tuning_timer, jiffies + tuning_count * HZ);
+		/*
+		 * In case tuning fails, host controllers which support
+		 * re-tuning can try tuning again at a later time, when the
+		 * re-tuning timer expires.  So for these controllers, we
+		 * return 0. Since there might be other controllers who do not
+		 * have this capability, we return error for them.
+		 */
+		err = 0;
 	}
 
-	/*
-	 * In case tuning fails, host controllers which support re-tuning can
-	 * try tuning again at a later time, when the re-tuning timer expires.
-	 * So for these controllers, we return 0. Since there might be other
-	 * controllers who do not have this capability, we return error for
-	 * them. SDHCI_USING_RETUNING_TIMER means the host is currently using
-	 * a retuning timer to do the retuning for the card.
-	 */
-	if (err && (host->flags & SDHCI_USING_RETUNING_TIMER))
-		err = 0;
+	host->mmc->retune_period = err ? 0 : tuning_count;
 
 	sdhci_writel(host, host->ier, SDHCI_INT_ENABLE);
 	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
@@ -2092,6 +2048,18 @@ out_unlock:
 	return err;
 }
 
+static int sdhci_select_drive_strength(struct mmc_card *card,
+				       unsigned int max_dtr, int host_drv,
+				       int card_drv, int *drv_type)
+{
+	struct sdhci_host *host = mmc_priv(card->host);
+
+	if (!host->ops->select_drive_strength)
+		return 0;
+
+	return host->ops->select_drive_strength(host, card, max_dtr, host_drv,
+						card_drv, drv_type);
+}
 
 static void sdhci_enable_preset_value(struct sdhci_host *host, bool enable)
 {
@@ -2236,6 +2204,7 @@ static const struct mmc_host_ops sdhci_ops = {
 	.start_signal_voltage_switch	= sdhci_start_signal_voltage_switch,
 	.prepare_hs400_tuning		= sdhci_prepare_hs400_tuning,
 	.execute_tuning			= sdhci_execute_tuning,
+	.select_drive_strength		= sdhci_select_drive_strength,
 	.card_event			= sdhci_card_event,
 	.card_busy	= sdhci_card_busy,
 };
@@ -2337,20 +2306,6 @@ static void sdhci_timeout_timer(unsigned long data)
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
-static void sdhci_tuning_timer(unsigned long data)
-{
-	struct sdhci_host *host;
-	unsigned long flags;
-
-	host = (struct sdhci_host *)data;
-
-	spin_lock_irqsave(&host->lock, flags);
-
-	host->flags |= SDHCI_NEEDS_RETUNING;
-
-	spin_unlock_irqrestore(&host->lock, flags);
-}
-
 /*****************************************************************************\
  *                                                                           *
  * Interrupt handling                                                        *
@@ -2728,11 +2683,8 @@ int sdhci_suspend_host(struct sdhci_host *host)
 {
 	sdhci_disable_card_detection(host);
 
-	/* Disable tuning since we are suspending */
-	if (host->flags & SDHCI_USING_RETUNING_TIMER) {
-		del_timer_sync(&host->tuning_timer);
-		host->flags &= ~SDHCI_NEEDS_RETUNING;
-	}
+	mmc_retune_timer_stop(host->mmc);
+	mmc_retune_needed(host->mmc);
 
 	if (!device_may_wakeup(mmc_dev(host->mmc))) {
 		host->ier = 0;
@@ -2782,10 +2734,6 @@ int sdhci_resume_host(struct sdhci_host *host)
 
 	sdhci_enable_card_detection(host);
 
-	/* Set the re-tuning expiration flag */
-	if (host->flags & SDHCI_USING_RETUNING_TIMER)
-		host->flags |= SDHCI_NEEDS_RETUNING;
-
 	return ret;
 }
 
@@ -2822,11 +2770,8 @@ int sdhci_runtime_suspend_host(struct sdhci_host *host)
 {
 	unsigned long flags;
 
-	/* Disable tuning since we are suspending */
-	if (host->flags & SDHCI_USING_RETUNING_TIMER) {
-		del_timer_sync(&host->tuning_timer);
-		host->flags &= ~SDHCI_NEEDS_RETUNING;
-	}
+	mmc_retune_timer_stop(host->mmc);
+	mmc_retune_needed(host->mmc);
 
 	spin_lock_irqsave(&host->lock, flags);
 	host->ier &= SDHCI_INT_CARD_INT;
@@ -2869,10 +2814,6 @@ int sdhci_runtime_resume_host(struct sdhci_host *host)
 		spin_unlock_irqrestore(&host->lock, flags);
 	}
 
-	/* Set the re-tuning expiration flag */
-	if (host->flags & SDHCI_USING_RETUNING_TIMER)
-		host->flags |= SDHCI_NEEDS_RETUNING;
-
 	spin_lock_irqsave(&host->lock, flags);
 
 	host->runtime_suspended = false;
@@ -3315,13 +3256,14 @@ int sdhci_add_host(struct sdhci_host *host)
 				   SDHCI_MAX_CURRENT_MULTIPLIER;
 	}
 
-	/* If OCR set by external regulators, use it instead */
+	/* If OCR set by host, use it instead. */
+	if (host->ocr_mask)
+		ocr_avail = host->ocr_mask;
+
+	/* If OCR set by external regulators, give it highest prio. */
 	if (mmc->ocr_avail)
 		ocr_avail = mmc->ocr_avail;
 
-	if (host->ocr_mask)
-		ocr_avail &= host->ocr_mask;
-
 	mmc->ocr_avail = ocr_avail;
 	mmc->ocr_avail_sdio = ocr_avail;
 	if (host->ocr_avail_sdio)
@@ -3408,13 +3350,6 @@ int sdhci_add_host(struct sdhci_host *host)
 
 	init_waitqueue_head(&host->buf_ready_int);
 
-	if (host->version >= SDHCI_SPEC_300) {
-		/* Initialize re-tuning timer */
-		init_timer(&host->tuning_timer);
-		host->tuning_timer.data = (unsigned long)host;
-		host->tuning_timer.function = sdhci_tuning_timer;
-	}
-
 	sdhci_init(host, 0);
 
 	ret = request_threaded_irq(host->irq, sdhci_irq, sdhci_thread_irq,
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index e639b7f435e5..5521d29368e4 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -432,13 +432,11 @@ struct sdhci_host {
 #define SDHCI_REQ_USE_DMA	(1<<2)	/* Use DMA for this req. */
 #define SDHCI_DEVICE_DEAD	(1<<3)	/* Device unresponsive */
 #define SDHCI_SDR50_NEEDS_TUNING (1<<4)	/* SDR50 needs tuning */
-#define SDHCI_NEEDS_RETUNING	(1<<5)	/* Host needs retuning */
 #define SDHCI_AUTO_CMD12	(1<<6)	/* Auto CMD12 support */
 #define SDHCI_AUTO_CMD23	(1<<7)	/* Auto CMD23 support */
 #define SDHCI_PV_ENABLED	(1<<8)	/* Preset value enabled */
 #define SDHCI_SDIO_IRQ_ENABLED	(1<<9)	/* SDIO irq enabled */
 #define SDHCI_SDR104_NEEDS_TUNING (1<<10)	/* SDR104/HS200 needs tuning */
-#define SDHCI_USING_RETUNING_TIMER (1<<11)	/* Host is using a retuning timer for the card */
 #define SDHCI_USE_64_BIT_DMA	(1<<12)	/* Use 64-bit DMA */
 #define SDHCI_HS400_TUNING	(1<<13)	/* Tuning for HS400 */
 
@@ -504,7 +502,6 @@ struct sdhci_host {
 	unsigned int		tuning_count;	/* Timer count for re-tuning */
 	unsigned int		tuning_mode;	/* Re-tuning mode supported by host */
 #define SDHCI_TUNING_MODE_1	0
-	struct timer_list	tuning_timer;	/* Timer for tuning */
 
 	struct sdhci_host_next	next_data;
 	unsigned long private[0] ____cacheline_aligned;
@@ -541,6 +538,10 @@ struct sdhci_ops {
 	void	(*platform_init)(struct sdhci_host *host);
 	void    (*card_event)(struct sdhci_host *host);
 	void	(*voltage_switch)(struct sdhci_host *host);
+	int	(*select_drive_strength)(struct sdhci_host *host,
+					 struct mmc_card *card,
+					 unsigned int max_dtr, int host_drv,
+					 int card_drv, int *drv_type);
 };
 
 #ifdef CONFIG_MMC_SDHCI_IO_ACCESSORS
diff --git a/drivers/mmc/host/sdhci_f_sdh30.c b/drivers/mmc/host/sdhci_f_sdh30.c
index 2fe8b91481b3..983b8b32ef96 100644
--- a/drivers/mmc/host/sdhci_f_sdh30.c
+++ b/drivers/mmc/host/sdhci_f_sdh30.c
@@ -49,7 +49,7 @@ struct f_sdhost_priv {
 	struct device *dev;
 };
 
-void sdhci_f_sdh30_soft_voltage_switch(struct sdhci_host *host)
+static void sdhci_f_sdh30_soft_voltage_switch(struct sdhci_host *host)
 {
 	struct f_sdhost_priv *priv = sdhci_priv(host);
 	u32 ctrl = 0;
@@ -77,12 +77,12 @@ void sdhci_f_sdh30_soft_voltage_switch(struct sdhci_host *host)
 	sdhci_writel(host, ctrl, F_SDH30_TUNING_SETTING);
 }
 
-unsigned int sdhci_f_sdh30_get_min_clock(struct sdhci_host *host)
+static unsigned int sdhci_f_sdh30_get_min_clock(struct sdhci_host *host)
 {
 	return F_SDH30_MIN_CLOCK;
 }
 
-void sdhci_f_sdh30_reset(struct sdhci_host *host, u8 mask)
+static void sdhci_f_sdh30_reset(struct sdhci_host *host, u8 mask)
 {
 	if (sdhci_readw(host, SDHCI_CLOCK_CONTROL) == 0)
 		sdhci_writew(host, 0xBC01, SDHCI_CLOCK_CONTROL);
@@ -114,8 +114,7 @@ static int sdhci_f_sdh30_probe(struct platform_device *pdev)
 		return irq;
 	}
 
-	host = sdhci_alloc_host(dev, sizeof(struct sdhci_host) +
-						sizeof(struct f_sdhost_priv));
+	host = sdhci_alloc_host(dev, sizeof(struct f_sdhost_priv));
 	if (IS_ERR(host))
 		return PTR_ERR(host);
 
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index 7eff087cf515..5a1fdd405b1a 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -57,6 +57,7 @@
 #include <linux/mmc/slot-gpio.h>
 #include <linux/mod_devicetable.h>
 #include <linux/mutex.h>
+#include <linux/of_device.h>
 #include <linux/pagemap.h>
 #include <linux/platform_device.h>
 #include <linux/pm_qos.h>
@@ -205,14 +206,14 @@
 #define CLKDEV_MMC_DATA		20000000 /* 20MHz */
 #define CLKDEV_INIT		400000   /* 400 KHz */
 
-enum mmcif_state {
+enum sh_mmcif_state {
 	STATE_IDLE,
 	STATE_REQUEST,
 	STATE_IOS,
 	STATE_TIMEOUT,
 };
 
-enum mmcif_wait_for {
+enum sh_mmcif_wait_for {
 	MMCIF_WAIT_FOR_REQUEST,
 	MMCIF_WAIT_FOR_CMD,
 	MMCIF_WAIT_FOR_MREAD,
@@ -224,12 +225,14 @@ enum mmcif_wait_for {
 	MMCIF_WAIT_FOR_STOP,
 };
 
+/*
+ * difference for each SoC
+ */
 struct sh_mmcif_host {
 	struct mmc_host *mmc;
 	struct mmc_request *mrq;
 	struct platform_device *pd;
-	struct clk *hclk;
-	unsigned int clk;
+	struct clk *clk;
 	int bus_width;
 	unsigned char timing;
 	bool sd_error;
@@ -238,8 +241,8 @@ struct sh_mmcif_host {
 	void __iomem *addr;
 	u32 *pio_ptr;
 	spinlock_t lock;		/* protect sh_mmcif_host::state */
-	enum mmcif_state state;
-	enum mmcif_wait_for wait_for;
+	enum sh_mmcif_state state;
+	enum sh_mmcif_wait_for wait_for;
 	struct delayed_work timeout_work;
 	size_t blocksize;
 	int sg_idx;
@@ -249,6 +252,7 @@ struct sh_mmcif_host {
 	bool ccs_enable;		/* Command Completion Signal support */
 	bool clk_ctrl2_enable;
 	struct mutex thread_lock;
+	u32 clkdiv_map;         /* see CE_CLK_CTRL::CLKDIV */
 
 	/* DMA support */
 	struct dma_chan		*chan_rx;
@@ -257,6 +261,14 @@ struct sh_mmcif_host {
 	bool			dma_active;
 };
 
+static const struct of_device_id sh_mmcif_of_match[] = {
+	{ .compatible = "renesas,sh-mmcif" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sh_mmcif_of_match);
+
+#define sh_mmcif_host_to_dev(host) (&host->pd->dev)
+
 static inline void sh_mmcif_bitset(struct sh_mmcif_host *host,
 					unsigned int reg, u32 val)
 {
@@ -269,15 +281,16 @@ static inline void sh_mmcif_bitclr(struct sh_mmcif_host *host,
 	writel(~val & readl(host->addr + reg), host->addr + reg);
 }
 
-static void mmcif_dma_complete(void *arg)
+static void sh_mmcif_dma_complete(void *arg)
 {
 	struct sh_mmcif_host *host = arg;
 	struct mmc_request *mrq = host->mrq;
+	struct device *dev = sh_mmcif_host_to_dev(host);
 
-	dev_dbg(&host->pd->dev, "Command completed\n");
+	dev_dbg(dev, "Command completed\n");
 
 	if (WARN(!mrq || !mrq->data, "%s: NULL data in DMA completion!\n",
-		 dev_name(&host->pd->dev)))
+		 dev_name(dev)))
 		return;
 
 	complete(&host->dma_complete);
@@ -289,6 +302,7 @@ static void sh_mmcif_start_dma_rx(struct sh_mmcif_host *host)
 	struct scatterlist *sg = data->sg;
 	struct dma_async_tx_descriptor *desc = NULL;
 	struct dma_chan *chan = host->chan_rx;
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	dma_cookie_t cookie = -EINVAL;
 	int ret;
 
@@ -301,13 +315,13 @@ static void sh_mmcif_start_dma_rx(struct sh_mmcif_host *host)
 	}
 
 	if (desc) {
-		desc->callback = mmcif_dma_complete;
+		desc->callback = sh_mmcif_dma_complete;
 		desc->callback_param = host;
 		cookie = dmaengine_submit(desc);
 		sh_mmcif_bitset(host, MMCIF_CE_BUF_ACC, BUF_ACC_DMAREN);
 		dma_async_issue_pending(chan);
 	}
-	dev_dbg(&host->pd->dev, "%s(): mapped %d -> %d, cookie %d\n",
+	dev_dbg(dev, "%s(): mapped %d -> %d, cookie %d\n",
 		__func__, data->sg_len, ret, cookie);
 
 	if (!desc) {
@@ -323,12 +337,12 @@ static void sh_mmcif_start_dma_rx(struct sh_mmcif_host *host)
 			host->chan_tx = NULL;
 			dma_release_channel(chan);
 		}
-		dev_warn(&host->pd->dev,
+		dev_warn(dev,
 			 "DMA failed: %d, falling back to PIO\n", ret);
 		sh_mmcif_bitclr(host, MMCIF_CE_BUF_ACC, BUF_ACC_DMAREN | BUF_ACC_DMAWEN);
 	}
 
-	dev_dbg(&host->pd->dev, "%s(): desc %p, cookie %d, sg[%d]\n", __func__,
+	dev_dbg(dev, "%s(): desc %p, cookie %d, sg[%d]\n", __func__,
 		desc, cookie, data->sg_len);
 }
 
@@ -338,6 +352,7 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 	struct scatterlist *sg = data->sg;
 	struct dma_async_tx_descriptor *desc = NULL;
 	struct dma_chan *chan = host->chan_tx;
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	dma_cookie_t cookie = -EINVAL;
 	int ret;
 
@@ -350,13 +365,13 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 	}
 
 	if (desc) {
-		desc->callback = mmcif_dma_complete;
+		desc->callback = sh_mmcif_dma_complete;
 		desc->callback_param = host;
 		cookie = dmaengine_submit(desc);
 		sh_mmcif_bitset(host, MMCIF_CE_BUF_ACC, BUF_ACC_DMAWEN);
 		dma_async_issue_pending(chan);
 	}
-	dev_dbg(&host->pd->dev, "%s(): mapped %d -> %d, cookie %d\n",
+	dev_dbg(dev, "%s(): mapped %d -> %d, cookie %d\n",
 		__func__, data->sg_len, ret, cookie);
 
 	if (!desc) {
@@ -372,12 +387,12 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 			host->chan_rx = NULL;
 			dma_release_channel(chan);
 		}
-		dev_warn(&host->pd->dev,
+		dev_warn(dev,
 			 "DMA failed: %d, falling back to PIO\n", ret);
 		sh_mmcif_bitclr(host, MMCIF_CE_BUF_ACC, BUF_ACC_DMAREN | BUF_ACC_DMAWEN);
 	}
 
-	dev_dbg(&host->pd->dev, "%s(): desc %p, cookie %d\n", __func__,
+	dev_dbg(dev, "%s(): desc %p, cookie %d\n", __func__,
 		desc, cookie);
 }
 
@@ -390,6 +405,7 @@ sh_mmcif_request_dma_one(struct sh_mmcif_host *host,
 	struct dma_chan *chan;
 	void *slave_data = NULL;
 	struct resource *res;
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	dma_cap_mask_t mask;
 	int ret;
 
@@ -402,10 +418,10 @@ sh_mmcif_request_dma_one(struct sh_mmcif_host *host,
 			(void *)pdata->slave_id_rx;
 
 	chan = dma_request_slave_channel_compat(mask, shdma_chan_filter,
-				slave_data, &host->pd->dev,
+				slave_data, dev,
 				direction == DMA_MEM_TO_DEV ? "tx" : "rx");
 
-	dev_dbg(&host->pd->dev, "%s: %s: got channel %p\n", __func__,
+	dev_dbg(dev, "%s: %s: got channel %p\n", __func__,
 		direction == DMA_MEM_TO_DEV ? "TX" : "RX", chan);
 
 	if (!chan)
@@ -435,12 +451,13 @@ sh_mmcif_request_dma_one(struct sh_mmcif_host *host,
 static void sh_mmcif_request_dma(struct sh_mmcif_host *host,
 				 struct sh_mmcif_plat_data *pdata)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	host->dma_active = false;
 
 	if (pdata) {
 		if (pdata->slave_id_tx <= 0 || pdata->slave_id_rx <= 0)
 			return;
-	} else if (!host->pd->dev.of_node) {
+	} else if (!dev->of_node) {
 		return;
 	}
 
@@ -476,21 +493,59 @@ static void sh_mmcif_release_dma(struct sh_mmcif_host *host)
 
 static void sh_mmcif_clock_control(struct sh_mmcif_host *host, unsigned int clk)
 {
-	struct sh_mmcif_plat_data *p = host->pd->dev.platform_data;
+	struct device *dev = sh_mmcif_host_to_dev(host);
+	struct sh_mmcif_plat_data *p = dev->platform_data;
 	bool sup_pclk = p ? p->sup_pclk : false;
+	unsigned int current_clk = clk_get_rate(host->clk);
+	unsigned int clkdiv;
 
 	sh_mmcif_bitclr(host, MMCIF_CE_CLK_CTRL, CLK_ENABLE);
 	sh_mmcif_bitclr(host, MMCIF_CE_CLK_CTRL, CLK_CLEAR);
 
 	if (!clk)
 		return;
-	if (sup_pclk && clk == host->clk)
-		sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_SUP_PCLK);
-	else
-		sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_CLEAR &
-				((fls(DIV_ROUND_UP(host->clk,
-						   clk) - 1) - 1) << 16));
 
+	if (host->clkdiv_map) {
+		unsigned int freq, best_freq, myclk, div, diff_min, diff;
+		int i;
+
+		clkdiv = 0;
+		diff_min = ~0;
+		best_freq = 0;
+		for (i = 31; i >= 0; i--) {
+			if (!((1 << i) & host->clkdiv_map))
+				continue;
+
+			/*
+			 * clk = parent_freq / div
+			 * -> parent_freq = clk x div
+			 */
+
+			div = 1 << (i + 1);
+			freq = clk_round_rate(host->clk, clk * div);
+			myclk = freq / div;
+			diff = (myclk > clk) ? myclk - clk : clk - myclk;
+
+			if (diff <= diff_min) {
+				best_freq = freq;
+				clkdiv = i;
+				diff_min = diff;
+			}
+		}
+
+		dev_dbg(dev, "clk %u/%u (%u, 0x%x)\n",
+			(best_freq / (1 << (clkdiv + 1))), clk,
+			best_freq, clkdiv);
+
+		clk_set_rate(host->clk, best_freq);
+		clkdiv = clkdiv << 16;
+	} else if (sup_pclk && clk == current_clk) {
+		clkdiv = CLK_SUP_PCLK;
+	} else {
+		clkdiv = (fls(DIV_ROUND_UP(current_clk, clk) - 1) - 1) << 16;
+	}
+
+	sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_CLEAR & clkdiv);
 	sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_ENABLE);
 }
 
@@ -514,6 +569,7 @@ static void sh_mmcif_sync_reset(struct sh_mmcif_host *host)
 
 static int sh_mmcif_error_manage(struct sh_mmcif_host *host)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	u32 state1, state2;
 	int ret, timeout;
 
@@ -521,8 +577,8 @@ static int sh_mmcif_error_manage(struct sh_mmcif_host *host)
 
 	state1 = sh_mmcif_readl(host->addr, MMCIF_CE_HOST_STS1);
 	state2 = sh_mmcif_readl(host->addr, MMCIF_CE_HOST_STS2);
-	dev_dbg(&host->pd->dev, "ERR HOST_STS1 = %08x\n", state1);
-	dev_dbg(&host->pd->dev, "ERR HOST_STS2 = %08x\n", state2);
+	dev_dbg(dev, "ERR HOST_STS1 = %08x\n", state1);
+	dev_dbg(dev, "ERR HOST_STS2 = %08x\n", state2);
 
 	if (state1 & STS1_CMDSEQ) {
 		sh_mmcif_bitset(host, MMCIF_CE_CMD_CTRL, CMD_CTRL_BREAK);
@@ -534,25 +590,25 @@ static int sh_mmcif_error_manage(struct sh_mmcif_host *host)
 			mdelay(1);
 		}
 		if (!timeout) {
-			dev_err(&host->pd->dev,
+			dev_err(dev,
 				"Forced end of command sequence timeout err\n");
 			return -EIO;
 		}
 		sh_mmcif_sync_reset(host);
-		dev_dbg(&host->pd->dev, "Forced end of command sequence\n");
+		dev_dbg(dev, "Forced end of command sequence\n");
 		return -EIO;
 	}
 
 	if (state2 & STS2_CRC_ERR) {
-		dev_err(&host->pd->dev, " CRC error: state %u, wait %u\n",
+		dev_err(dev, " CRC error: state %u, wait %u\n",
 			host->state, host->wait_for);
 		ret = -EIO;
 	} else if (state2 & STS2_TIMEOUT_ERR) {
-		dev_err(&host->pd->dev, " Timeout: state %u, wait %u\n",
+		dev_err(dev, " Timeout: state %u, wait %u\n",
 			host->state, host->wait_for);
 		ret = -ETIMEDOUT;
 	} else {
-		dev_dbg(&host->pd->dev, " End/Index error: state %u, wait %u\n",
+		dev_dbg(dev, " End/Index error: state %u, wait %u\n",
 			host->state, host->wait_for);
 		ret = -EIO;
 	}
@@ -593,13 +649,14 @@ static void sh_mmcif_single_read(struct sh_mmcif_host *host,
 
 static bool sh_mmcif_read_block(struct sh_mmcif_host *host)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	struct mmc_data *data = host->mrq->data;
 	u32 *p = sg_virt(data->sg);
 	int i;
 
 	if (host->sd_error) {
 		data->error = sh_mmcif_error_manage(host);
-		dev_dbg(&host->pd->dev, "%s(): %d\n", __func__, data->error);
+		dev_dbg(dev, "%s(): %d\n", __func__, data->error);
 		return false;
 	}
 
@@ -634,13 +691,14 @@ static void sh_mmcif_multi_read(struct sh_mmcif_host *host,
 
 static bool sh_mmcif_mread_block(struct sh_mmcif_host *host)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	struct mmc_data *data = host->mrq->data;
 	u32 *p = host->pio_ptr;
 	int i;
 
 	if (host->sd_error) {
 		data->error = sh_mmcif_error_manage(host);
-		dev_dbg(&host->pd->dev, "%s(): %d\n", __func__, data->error);
+		dev_dbg(dev, "%s(): %d\n", __func__, data->error);
 		return false;
 	}
 
@@ -671,13 +729,14 @@ static void sh_mmcif_single_write(struct sh_mmcif_host *host,
 
 static bool sh_mmcif_write_block(struct sh_mmcif_host *host)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	struct mmc_data *data = host->mrq->data;
 	u32 *p = sg_virt(data->sg);
 	int i;
 
 	if (host->sd_error) {
 		data->error = sh_mmcif_error_manage(host);
-		dev_dbg(&host->pd->dev, "%s(): %d\n", __func__, data->error);
+		dev_dbg(dev, "%s(): %d\n", __func__, data->error);
 		return false;
 	}
 
@@ -712,13 +771,14 @@ static void sh_mmcif_multi_write(struct sh_mmcif_host *host,
 
 static bool sh_mmcif_mwrite_block(struct sh_mmcif_host *host)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	struct mmc_data *data = host->mrq->data;
 	u32 *p = host->pio_ptr;
 	int i;
 
 	if (host->sd_error) {
 		data->error = sh_mmcif_error_manage(host);
-		dev_dbg(&host->pd->dev, "%s(): %d\n", __func__, data->error);
+		dev_dbg(dev, "%s(): %d\n", __func__, data->error);
 		return false;
 	}
 
@@ -756,6 +816,7 @@ static void sh_mmcif_get_cmd12response(struct sh_mmcif_host *host,
 static u32 sh_mmcif_set_cmd(struct sh_mmcif_host *host,
 			    struct mmc_request *mrq)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	struct mmc_data *data = mrq->data;
 	struct mmc_command *cmd = mrq->cmd;
 	u32 opc = cmd->opcode;
@@ -775,7 +836,7 @@ static u32 sh_mmcif_set_cmd(struct sh_mmcif_host *host,
 		tmp |= CMD_SET_RTYP_17B;
 		break;
 	default:
-		dev_err(&host->pd->dev, "Unsupported response type.\n");
+		dev_err(dev, "Unsupported response type.\n");
 		break;
 	}
 	switch (opc) {
@@ -803,7 +864,7 @@ static u32 sh_mmcif_set_cmd(struct sh_mmcif_host *host,
 			tmp |= CMD_SET_DATW_8;
 			break;
 		default:
-			dev_err(&host->pd->dev, "Unsupported bus width.\n");
+			dev_err(dev, "Unsupported bus width.\n");
 			break;
 		}
 		switch (host->timing) {
@@ -846,6 +907,8 @@ static u32 sh_mmcif_set_cmd(struct sh_mmcif_host *host,
 static int sh_mmcif_data_trans(struct sh_mmcif_host *host,
 			       struct mmc_request *mrq, u32 opc)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
+
 	switch (opc) {
 	case MMC_READ_MULTIPLE_BLOCK:
 		sh_mmcif_multi_read(host, mrq);
@@ -861,7 +924,7 @@ static int sh_mmcif_data_trans(struct sh_mmcif_host *host,
 		sh_mmcif_single_read(host, mrq);
 		return 0;
 	default:
-		dev_err(&host->pd->dev, "Unsupported CMD%d\n", opc);
+		dev_err(dev, "Unsupported CMD%d\n", opc);
 		return -EINVAL;
 	}
 }
@@ -918,6 +981,8 @@ static void sh_mmcif_start_cmd(struct sh_mmcif_host *host,
 static void sh_mmcif_stop_cmd(struct sh_mmcif_host *host,
 			      struct mmc_request *mrq)
 {
+	struct device *dev = sh_mmcif_host_to_dev(host);
+
 	switch (mrq->cmd->opcode) {
 	case MMC_READ_MULTIPLE_BLOCK:
 		sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MCMD12DRE);
@@ -926,7 +991,7 @@ static void sh_mmcif_stop_cmd(struct sh_mmcif_host *host,
 		sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MCMD12RBE);
 		break;
 	default:
-		dev_err(&host->pd->dev, "unsupported stop cmd\n");
+		dev_err(dev, "unsupported stop cmd\n");
 		mrq->stop->error = sh_mmcif_error_manage(host);
 		return;
 	}
@@ -937,11 +1002,13 @@ static void sh_mmcif_stop_cmd(struct sh_mmcif_host *host,
 static void sh_mmcif_request(struct mmc_host *mmc, struct mmc_request *mrq)
 {
 	struct sh_mmcif_host *host = mmc_priv(mmc);
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	unsigned long flags;
 
 	spin_lock_irqsave(&host->lock, flags);
 	if (host->state != STATE_IDLE) {
-		dev_dbg(&host->pd->dev, "%s() rejected, state %u\n", __func__, host->state);
+		dev_dbg(dev, "%s() rejected, state %u\n",
+			__func__, host->state);
 		spin_unlock_irqrestore(&host->lock, flags);
 		mrq->cmd->error = -EAGAIN;
 		mmc_request_done(mmc, mrq);
@@ -972,17 +1039,37 @@ static void sh_mmcif_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	sh_mmcif_start_cmd(host, mrq);
 }
 
-static int sh_mmcif_clk_update(struct sh_mmcif_host *host)
+static void sh_mmcif_clk_setup(struct sh_mmcif_host *host)
 {
-	int ret = clk_prepare_enable(host->hclk);
+	struct device *dev = sh_mmcif_host_to_dev(host);
+
+	if (host->mmc->f_max) {
+		unsigned int f_max, f_min = 0, f_min_old;
+
+		f_max = host->mmc->f_max;
+		for (f_min_old = f_max; f_min_old > 2;) {
+			f_min = clk_round_rate(host->clk, f_min_old / 2);
+			if (f_min == f_min_old)
+				break;
+			f_min_old = f_min;
+		}
+
+		/*
+		 * This driver assumes this SoC is R-Car Gen2 or later
+		 */
+		host->clkdiv_map = 0x3ff;
+
+		host->mmc->f_max = f_max / (1 << ffs(host->clkdiv_map));
+		host->mmc->f_min = f_min / (1 << fls(host->clkdiv_map));
+	} else {
+		unsigned int clk = clk_get_rate(host->clk);
 
-	if (!ret) {
-		host->clk = clk_get_rate(host->hclk);
-		host->mmc->f_max = host->clk / 2;
-		host->mmc->f_min = host->clk / 512;
+		host->mmc->f_max = clk / 2;
+		host->mmc->f_min = clk / 512;
 	}
 
-	return ret;
+	dev_dbg(dev, "clk max/min = %d/%d\n",
+		host->mmc->f_max, host->mmc->f_min);
 }
 
 static void sh_mmcif_set_power(struct sh_mmcif_host *host, struct mmc_ios *ios)
@@ -998,11 +1085,13 @@ static void sh_mmcif_set_power(struct sh_mmcif_host *host, struct mmc_ios *ios)
 static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
 	struct sh_mmcif_host *host = mmc_priv(mmc);
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	unsigned long flags;
 
 	spin_lock_irqsave(&host->lock, flags);
 	if (host->state != STATE_IDLE) {
-		dev_dbg(&host->pd->dev, "%s() rejected, state %u\n", __func__, host->state);
+		dev_dbg(dev, "%s() rejected, state %u\n",
+			__func__, host->state);
 		spin_unlock_irqrestore(&host->lock, flags);
 		return;
 	}
@@ -1013,7 +1102,7 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	if (ios->power_mode == MMC_POWER_UP) {
 		if (!host->card_present) {
 			/* See if we also get DMA */
-			sh_mmcif_request_dma(host, host->pd->dev.platform_data);
+			sh_mmcif_request_dma(host, dev->platform_data);
 			host->card_present = true;
 		}
 		sh_mmcif_set_power(host, ios);
@@ -1027,8 +1116,8 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 			}
 		}
 		if (host->power) {
-			pm_runtime_put_sync(&host->pd->dev);
-			clk_disable_unprepare(host->hclk);
+			pm_runtime_put_sync(dev);
+			clk_disable_unprepare(host->clk);
 			host->power = false;
 			if (ios->power_mode == MMC_POWER_OFF)
 				sh_mmcif_set_power(host, ios);
@@ -1039,8 +1128,9 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	if (ios->clock) {
 		if (!host->power) {
-			sh_mmcif_clk_update(host);
-			pm_runtime_get_sync(&host->pd->dev);
+			clk_prepare_enable(host->clk);
+
+			pm_runtime_get_sync(dev);
 			host->power = true;
 			sh_mmcif_sync_reset(host);
 		}
@@ -1055,7 +1145,8 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 static int sh_mmcif_get_cd(struct mmc_host *mmc)
 {
 	struct sh_mmcif_host *host = mmc_priv(mmc);
-	struct sh_mmcif_plat_data *p = host->pd->dev.platform_data;
+	struct device *dev = sh_mmcif_host_to_dev(host);
+	struct sh_mmcif_plat_data *p = dev->platform_data;
 	int ret = mmc_gpio_get_cd(mmc);
 
 	if (ret >= 0)
@@ -1077,6 +1168,7 @@ static bool sh_mmcif_end_cmd(struct sh_mmcif_host *host)
 {
 	struct mmc_command *cmd = host->mrq->cmd;
 	struct mmc_data *data = host->mrq->data;
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	long time;
 
 	if (host->sd_error) {
@@ -1090,7 +1182,7 @@ static bool sh_mmcif_end_cmd(struct sh_mmcif_host *host)
 			cmd->error = sh_mmcif_error_manage(host);
 			break;
 		}
-		dev_dbg(&host->pd->dev, "CMD%d error %d\n",
+		dev_dbg(dev, "CMD%d error %d\n",
 			cmd->opcode, cmd->error);
 		host->sd_error = false;
 		return false;
@@ -1170,6 +1262,7 @@ static irqreturn_t sh_mmcif_irqt(int irq, void *dev_id)
 {
 	struct sh_mmcif_host *host = dev_id;
 	struct mmc_request *mrq;
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	bool wait = false;
 	unsigned long flags;
 	int wait_work;
@@ -1184,7 +1277,7 @@ static irqreturn_t sh_mmcif_irqt(int irq, void *dev_id)
 
 	mrq = host->mrq;
 	if (!mrq) {
-		dev_dbg(&host->pd->dev, "IRQ thread state %u, wait %u: NULL mrq!\n",
+		dev_dbg(dev, "IRQ thread state %u, wait %u: NULL mrq!\n",
 			host->state, host->wait_for);
 		mutex_unlock(&host->thread_lock);
 		return IRQ_HANDLED;
@@ -1222,7 +1315,7 @@ static irqreturn_t sh_mmcif_irqt(int irq, void *dev_id)
 	case MMCIF_WAIT_FOR_STOP:
 		if (host->sd_error) {
 			mrq->stop->error = sh_mmcif_error_manage(host);
-			dev_dbg(&host->pd->dev, "%s(): %d\n", __func__, mrq->stop->error);
+			dev_dbg(dev, "%s(): %d\n", __func__, mrq->stop->error);
 			break;
 		}
 		sh_mmcif_get_cmd12response(host, mrq->stop);
@@ -1232,7 +1325,7 @@ static irqreturn_t sh_mmcif_irqt(int irq, void *dev_id)
 	case MMCIF_WAIT_FOR_WRITE_END:
 		if (host->sd_error) {
 			mrq->data->error = sh_mmcif_error_manage(host);
-			dev_dbg(&host->pd->dev, "%s(): %d\n", __func__, mrq->data->error);
+			dev_dbg(dev, "%s(): %d\n", __func__, mrq->data->error);
 		}
 		break;
 	default:
@@ -1275,6 +1368,7 @@ static irqreturn_t sh_mmcif_irqt(int irq, void *dev_id)
 static irqreturn_t sh_mmcif_intr(int irq, void *dev_id)
 {
 	struct sh_mmcif_host *host = dev_id;
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	u32 state, mask;
 
 	state = sh_mmcif_readl(host->addr, MMCIF_CE_INT);
@@ -1286,32 +1380,33 @@ static irqreturn_t sh_mmcif_intr(int irq, void *dev_id)
 	sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, state & MASK_CLEAN);
 
 	if (state & ~MASK_CLEAN)
-		dev_dbg(&host->pd->dev, "IRQ state = 0x%08x incompletely cleared\n",
+		dev_dbg(dev, "IRQ state = 0x%08x incompletely cleared\n",
 			state);
 
 	if (state & INT_ERR_STS || state & ~INT_ALL) {
 		host->sd_error = true;
-		dev_dbg(&host->pd->dev, "int err state = 0x%08x\n", state);
+		dev_dbg(dev, "int err state = 0x%08x\n", state);
 	}
 	if (state & ~(INT_CMD12RBE | INT_CMD12CRE)) {
 		if (!host->mrq)
-			dev_dbg(&host->pd->dev, "NULL IRQ state = 0x%08x\n", state);
+			dev_dbg(dev, "NULL IRQ state = 0x%08x\n", state);
 		if (!host->dma_active)
 			return IRQ_WAKE_THREAD;
 		else if (host->sd_error)
-			mmcif_dma_complete(host);
+			sh_mmcif_dma_complete(host);
 	} else {
-		dev_dbg(&host->pd->dev, "Unexpected IRQ 0x%x\n", state);
+		dev_dbg(dev, "Unexpected IRQ 0x%x\n", state);
 	}
 
 	return IRQ_HANDLED;
 }
 
-static void mmcif_timeout_work(struct work_struct *work)
+static void sh_mmcif_timeout_work(struct work_struct *work)
 {
 	struct delayed_work *d = container_of(work, struct delayed_work, work);
 	struct sh_mmcif_host *host = container_of(d, struct sh_mmcif_host, timeout_work);
 	struct mmc_request *mrq = host->mrq;
+	struct device *dev = sh_mmcif_host_to_dev(host);
 	unsigned long flags;
 
 	if (host->dying)
@@ -1324,7 +1419,7 @@ static void mmcif_timeout_work(struct work_struct *work)
 		return;
 	}
 
-	dev_err(&host->pd->dev, "Timeout waiting for %u on CMD%u\n",
+	dev_err(dev, "Timeout waiting for %u on CMD%u\n",
 		host->wait_for, mrq->cmd->opcode);
 
 	host->state = STATE_TIMEOUT;
@@ -1361,7 +1456,8 @@ static void mmcif_timeout_work(struct work_struct *work)
 
 static void sh_mmcif_init_ocr(struct sh_mmcif_host *host)
 {
-	struct sh_mmcif_plat_data *pd = host->pd->dev.platform_data;
+	struct device *dev = sh_mmcif_host_to_dev(host);
+	struct sh_mmcif_plat_data *pd = dev->platform_data;
 	struct mmc_host *mmc = host->mmc;
 
 	mmc_regulator_get_supply(mmc);
@@ -1380,7 +1476,8 @@ static int sh_mmcif_probe(struct platform_device *pdev)
 	int ret = 0, irq[2];
 	struct mmc_host *mmc;
 	struct sh_mmcif_host *host;
-	struct sh_mmcif_plat_data *pd = pdev->dev.platform_data;
+	struct device *dev = &pdev->dev;
+	struct sh_mmcif_plat_data *pd = dev->platform_data;
 	struct resource *res;
 	void __iomem *reg;
 	const char *name;
@@ -1388,16 +1485,16 @@ static int sh_mmcif_probe(struct platform_device *pdev)
 	irq[0] = platform_get_irq(pdev, 0);
 	irq[1] = platform_get_irq(pdev, 1);
 	if (irq[0] < 0) {
-		dev_err(&pdev->dev, "Get irq error\n");
+		dev_err(dev, "Get irq error\n");
 		return -ENXIO;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	reg = devm_ioremap_resource(&pdev->dev, res);
+	reg = devm_ioremap_resource(dev, res);
 	if (IS_ERR(reg))
 		return PTR_ERR(reg);
 
-	mmc = mmc_alloc_host(sizeof(struct sh_mmcif_host), &pdev->dev);
+	mmc = mmc_alloc_host(sizeof(struct sh_mmcif_host), dev);
 	if (!mmc)
 		return -ENOMEM;
 
@@ -1430,41 +1527,44 @@ static int sh_mmcif_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, host);
 
-	pm_runtime_enable(&pdev->dev);
+	pm_runtime_enable(dev);
 	host->power = false;
 
-	host->hclk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(host->hclk)) {
-		ret = PTR_ERR(host->hclk);
-		dev_err(&pdev->dev, "cannot get clock: %d\n", ret);
+	host->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(host->clk)) {
+		ret = PTR_ERR(host->clk);
+		dev_err(dev, "cannot get clock: %d\n", ret);
 		goto err_pm;
 	}
-	ret = sh_mmcif_clk_update(host);
+
+	ret = clk_prepare_enable(host->clk);
 	if (ret < 0)
 		goto err_pm;
 
-	ret = pm_runtime_resume(&pdev->dev);
+	sh_mmcif_clk_setup(host);
+
+	ret = pm_runtime_resume(dev);
 	if (ret < 0)
 		goto err_clk;
 
-	INIT_DELAYED_WORK(&host->timeout_work, mmcif_timeout_work);
+	INIT_DELAYED_WORK(&host->timeout_work, sh_mmcif_timeout_work);
 
 	sh_mmcif_sync_reset(host);
 	sh_mmcif_writel(host->addr, MMCIF_CE_INT_MASK, MASK_ALL);
 
-	name = irq[1] < 0 ? dev_name(&pdev->dev) : "sh_mmc:error";
-	ret = devm_request_threaded_irq(&pdev->dev, irq[0], sh_mmcif_intr,
+	name = irq[1] < 0 ? dev_name(dev) : "sh_mmc:error";
+	ret = devm_request_threaded_irq(dev, irq[0], sh_mmcif_intr,
 					sh_mmcif_irqt, 0, name, host);
 	if (ret) {
-		dev_err(&pdev->dev, "request_irq error (%s)\n", name);
+		dev_err(dev, "request_irq error (%s)\n", name);
 		goto err_clk;
 	}
 	if (irq[1] >= 0) {
-		ret = devm_request_threaded_irq(&pdev->dev, irq[1],
+		ret = devm_request_threaded_irq(dev, irq[1],
 						sh_mmcif_intr, sh_mmcif_irqt,
 						0, "sh_mmc:int", host);
 		if (ret) {
-			dev_err(&pdev->dev, "request_irq error (sh_mmc:int)\n");
+			dev_err(dev, "request_irq error (sh_mmc:int)\n");
 			goto err_clk;
 		}
 	}
@@ -1481,19 +1581,19 @@ static int sh_mmcif_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto err_clk;
 
-	dev_pm_qos_expose_latency_limit(&pdev->dev, 100);
+	dev_pm_qos_expose_latency_limit(dev, 100);
 
-	dev_info(&pdev->dev, "Chip version 0x%04x, clock rate %luMHz\n",
+	dev_info(dev, "Chip version 0x%04x, clock rate %luMHz\n",
 		 sh_mmcif_readl(host->addr, MMCIF_CE_VERSION) & 0xffff,
-		 clk_get_rate(host->hclk) / 1000000UL);
+		 clk_get_rate(host->clk) / 1000000UL);
 
-	clk_disable_unprepare(host->hclk);
+	clk_disable_unprepare(host->clk);
 	return ret;
 
 err_clk:
-	clk_disable_unprepare(host->hclk);
+	clk_disable_unprepare(host->clk);
 err_pm:
-	pm_runtime_disable(&pdev->dev);
+	pm_runtime_disable(dev);
 err_host:
 	mmc_free_host(mmc);
 	return ret;
@@ -1504,7 +1604,7 @@ static int sh_mmcif_remove(struct platform_device *pdev)
 	struct sh_mmcif_host *host = platform_get_drvdata(pdev);
 
 	host->dying = true;
-	clk_prepare_enable(host->hclk);
+	clk_prepare_enable(host->clk);
 	pm_runtime_get_sync(&pdev->dev);
 
 	dev_pm_qos_hide_latency_limit(&pdev->dev);
@@ -1519,7 +1619,7 @@ static int sh_mmcif_remove(struct platform_device *pdev)
 	 */
 	cancel_delayed_work_sync(&host->timeout_work);
 
-	clk_disable_unprepare(host->hclk);
+	clk_disable_unprepare(host->clk);
 	mmc_free_host(host->mmc);
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
@@ -1543,12 +1643,6 @@ static int sh_mmcif_resume(struct device *dev)
 }
 #endif
 
-static const struct of_device_id mmcif_of_match[] = {
-	{ .compatible = "renesas,sh-mmcif" },
-	{ }
-};
-MODULE_DEVICE_TABLE(of, mmcif_of_match);
-
 static const struct dev_pm_ops sh_mmcif_dev_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(sh_mmcif_suspend, sh_mmcif_resume)
 };
@@ -1559,7 +1653,7 @@ static struct platform_driver sh_mmcif_driver = {
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.pm	= &sh_mmcif_dev_pm_ops,
-		.of_match_table = mmcif_of_match,
+		.of_match_table = sh_mmcif_of_match,
 	},
 };
 
diff --git a/drivers/mmc/host/tmio_mmc.c b/drivers/mmc/host/tmio_mmc.c
index f746df493892..e897e7fc3b14 100644
--- a/drivers/mmc/host/tmio_mmc.c
+++ b/drivers/mmc/host/tmio_mmc.c
@@ -85,8 +85,10 @@ static int tmio_mmc_probe(struct platform_device *pdev)
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -EINVAL;
+	if (!res) {
+		ret = -EINVAL;
+		goto cell_disable;
+	}
 
 	pdata->flags |= TMIO_MMC_HAVE_HIGH_REG;
 
@@ -101,7 +103,8 @@ static int tmio_mmc_probe(struct platform_device *pdev)
 	if (ret)
 		goto host_free;
 
-	ret = request_irq(irq, tmio_mmc_irq, IRQF_TRIGGER_FALLING,
+	ret = devm_request_irq(&pdev->dev, irq, tmio_mmc_irq,
+				IRQF_TRIGGER_FALLING,
 				dev_name(&pdev->dev), host);
 	if (ret)
 		goto host_remove;
@@ -129,7 +132,6 @@ static int tmio_mmc_remove(struct platform_device *pdev)
 
 	if (mmc) {
 		struct tmio_mmc_host *host = mmc_priv(mmc);
-		free_irq(platform_get_irq(pdev, 0), host);
 		tmio_mmc_host_remove(host);
 		if (cell->disable)
 			cell->disable(pdev);
diff --git a/drivers/mmc/host/tmio_mmc_pio.c b/drivers/mmc/host/tmio_mmc_pio.c
index dba7e1c19dd7..e3dcf31a8bd6 100644
--- a/drivers/mmc/host/tmio_mmc_pio.c
+++ b/drivers/mmc/host/tmio_mmc_pio.c
@@ -1108,7 +1108,8 @@ int tmio_mmc_host_probe(struct tmio_mmc_host *_host,
 	if (ret < 0)
 		goto host_free;
 
-	_host->ctl = ioremap(res_ctl->start, resource_size(res_ctl));
+	_host->ctl = devm_ioremap(&pdev->dev,
+				  res_ctl->start, resource_size(res_ctl));
 	if (!_host->ctl) {
 		ret = -ENOMEM;
 		goto host_free;
@@ -1230,8 +1231,6 @@ void tmio_mmc_host_remove(struct tmio_mmc_host *host)
 
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
-
-	iounmap(host->ctl);
 }
 EXPORT_SYMBOL(tmio_mmc_host_remove);
 
diff --git a/drivers/mtd/chips/Kconfig b/drivers/mtd/chips/Kconfig
index 9f02c28c0204..54479c481a7a 100644
--- a/drivers/mtd/chips/Kconfig
+++ b/drivers/mtd/chips/Kconfig
@@ -16,6 +16,7 @@ config MTD_CFI
 config MTD_JEDECPROBE
 	tristate "Detect non-CFI AMD/JEDEC-compatible flash chips"
 	select MTD_GEN_PROBE
+	select MTD_CFI_UTIL
 	help
 	  This option enables JEDEC-style probing of flash chips which are not
 	  compatible with the Common Flash Interface, but will use the common
diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index c50d8cf0f60d..c3624eb571d1 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -1295,7 +1295,7 @@ static int do_otp_write(struct map_info *map, struct flchip *chip, loff_t adr,
 		unsigned long bus_ofs = adr & ~(map_bankwidth(map)-1);
 		int gap = adr - bus_ofs;
 		int n = min_t(int, len, map_bankwidth(map) - gap);
-		map_word datum;
+		map_word datum = map_word_ff(map);
 
 		if (n != map_bankwidth(map)) {
 			/* partial write of a word, load old contents */
diff --git a/drivers/mtd/chips/cfi_util.c b/drivers/mtd/chips/cfi_util.c
index 09c79bd0b4f4..6f16552cd59f 100644
--- a/drivers/mtd/chips/cfi_util.c
+++ b/drivers/mtd/chips/cfi_util.c
@@ -23,6 +23,194 @@
 #include <linux/mtd/map.h>
 #include <linux/mtd/cfi.h>
 
+void cfi_udelay(int us)
+{
+	if (us >= 1000) {
+		msleep((us+999)/1000);
+	} else {
+		udelay(us);
+		cond_resched();
+	}
+}
+EXPORT_SYMBOL(cfi_udelay);
+
+/*
+ * Returns the command address according to the given geometry.
+ */
+uint32_t cfi_build_cmd_addr(uint32_t cmd_ofs,
+				struct map_info *map, struct cfi_private *cfi)
+{
+	unsigned bankwidth = map_bankwidth(map);
+	unsigned interleave = cfi_interleave(cfi);
+	unsigned type = cfi->device_type;
+	uint32_t addr;
+
+	addr = (cmd_ofs * type) * interleave;
+
+	/* Modify the unlock address if we are in compatibility mode.
+	 * For 16bit devices on 8 bit busses
+	 * and 32bit devices on 16 bit busses
+	 * set the low bit of the alternating bit sequence of the address.
+	 */
+	if (((type * interleave) > bankwidth) && ((cmd_ofs & 0xff) == 0xaa))
+		addr |= (type >> 1)*interleave;
+
+	return  addr;
+}
+EXPORT_SYMBOL(cfi_build_cmd_addr);
+
+/*
+ * Transforms the CFI command for the given geometry (bus width & interleave).
+ * It looks too long to be inline, but in the common case it should almost all
+ * get optimised away.
+ */
+map_word cfi_build_cmd(u_long cmd, struct map_info *map, struct cfi_private *cfi)
+{
+	map_word val = { {0} };
+	int wordwidth, words_per_bus, chip_mode, chips_per_word;
+	unsigned long onecmd;
+	int i;
+
+	/* We do it this way to give the compiler a fighting chance
+	   of optimising away all the crap for 'bankwidth' larger than
+	   an unsigned long, in the common case where that support is
+	   disabled */
+	if (map_bankwidth_is_large(map)) {
+		wordwidth = sizeof(unsigned long);
+		words_per_bus = (map_bankwidth(map)) / wordwidth; // i.e. normally 1
+	} else {
+		wordwidth = map_bankwidth(map);
+		words_per_bus = 1;
+	}
+
+	chip_mode = map_bankwidth(map) / cfi_interleave(cfi);
+	chips_per_word = wordwidth * cfi_interleave(cfi) / map_bankwidth(map);
+
+	/* First, determine what the bit-pattern should be for a single
+	   device, according to chip mode and endianness... */
+	switch (chip_mode) {
+	default: BUG();
+	case 1:
+		onecmd = cmd;
+		break;
+	case 2:
+		onecmd = cpu_to_cfi16(map, cmd);
+		break;
+	case 4:
+		onecmd = cpu_to_cfi32(map, cmd);
+		break;
+	}
+
+	/* Now replicate it across the size of an unsigned long, or
+	   just to the bus width as appropriate */
+	switch (chips_per_word) {
+	default: BUG();
+#if BITS_PER_LONG >= 64
+	case 8:
+		onecmd |= (onecmd << (chip_mode * 32));
+#endif
+	case 4:
+		onecmd |= (onecmd << (chip_mode * 16));
+	case 2:
+		onecmd |= (onecmd << (chip_mode * 8));
+	case 1:
+		;
+	}
+
+	/* And finally, for the multi-word case, replicate it
+	   in all words in the structure */
+	for (i=0; i < words_per_bus; i++) {
+		val.x[i] = onecmd;
+	}
+
+	return val;
+}
+EXPORT_SYMBOL(cfi_build_cmd);
+
+unsigned long cfi_merge_status(map_word val, struct map_info *map,
+					   struct cfi_private *cfi)
+{
+	int wordwidth, words_per_bus, chip_mode, chips_per_word;
+	unsigned long onestat, res = 0;
+	int i;
+
+	/* We do it this way to give the compiler a fighting chance
+	   of optimising away all the crap for 'bankwidth' larger than
+	   an unsigned long, in the common case where that support is
+	   disabled */
+	if (map_bankwidth_is_large(map)) {
+		wordwidth = sizeof(unsigned long);
+		words_per_bus = (map_bankwidth(map)) / wordwidth; // i.e. normally 1
+	} else {
+		wordwidth = map_bankwidth(map);
+		words_per_bus = 1;
+	}
+
+	chip_mode = map_bankwidth(map) / cfi_interleave(cfi);
+	chips_per_word = wordwidth * cfi_interleave(cfi) / map_bankwidth(map);
+
+	onestat = val.x[0];
+	/* Or all status words together */
+	for (i=1; i < words_per_bus; i++) {
+		onestat |= val.x[i];
+	}
+
+	res = onestat;
+	switch(chips_per_word) {
+	default: BUG();
+#if BITS_PER_LONG >= 64
+	case 8:
+		res |= (onestat >> (chip_mode * 32));
+#endif
+	case 4:
+		res |= (onestat >> (chip_mode * 16));
+	case 2:
+		res |= (onestat >> (chip_mode * 8));
+	case 1:
+		;
+	}
+
+	/* Last, determine what the bit-pattern should be for a single
+	   device, according to chip mode and endianness... */
+	switch (chip_mode) {
+	case 1:
+		break;
+	case 2:
+		res = cfi16_to_cpu(map, res);
+		break;
+	case 4:
+		res = cfi32_to_cpu(map, res);
+		break;
+	default: BUG();
+	}
+	return res;
+}
+EXPORT_SYMBOL(cfi_merge_status);
+
+/*
+ * Sends a CFI command to a bank of flash for the given geometry.
+ *
+ * Returns the offset in flash where the command was written.
+ * If prev_val is non-null, it will be set to the value at the command address,
+ * before the command was written.
+ */
+uint32_t cfi_send_gen_cmd(u_char cmd, uint32_t cmd_addr, uint32_t base,
+				struct map_info *map, struct cfi_private *cfi,
+				int type, map_word *prev_val)
+{
+	map_word val;
+	uint32_t addr = base + cfi_build_cmd_addr(cmd_addr, map, cfi);
+	val = cfi_build_cmd(cmd, map, cfi);
+
+	if (prev_val)
+		*prev_val = map_read(map, addr);
+
+	map_write(map, val, addr);
+
+	return addr - base;
+}
+EXPORT_SYMBOL(cfi_send_gen_cmd);
+
 int __xipram cfi_qry_present(struct map_info *map, __u32 base,
 			     struct cfi_private *cfi)
 {
diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index c49d0b127fef..f73c41697a00 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -195,6 +195,14 @@ config MTD_BLOCK2MTD
 	  Testing MTD users (eg JFFS2) on large media and media that might
 	  be removed during a write (using the floppy drive).
 
+config MTD_POWERNV_FLASH
+	tristate "powernv flash MTD driver"
+	depends on PPC_POWERNV
+	help
+	  This provides an MTD device to access flash on powernv OPAL
+	  platforms from Linux. This device abstracts away the
+	  firmware interface for flash access.
+
 comment "Disk-On-Chip Device Drivers"
 
 config MTD_DOCG3
diff --git a/drivers/mtd/devices/Makefile b/drivers/mtd/devices/Makefile
index f0b0e611d1d6..7912d3a0ee34 100644
--- a/drivers/mtd/devices/Makefile
+++ b/drivers/mtd/devices/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_MTD_SPEAR_SMI)	+= spear_smi.o
 obj-$(CONFIG_MTD_SST25L)	+= sst25l.o
 obj-$(CONFIG_MTD_BCM47XXSFLASH)	+= bcm47xxsflash.o
 obj-$(CONFIG_MTD_ST_SPI_FSM)    += st_spi_fsm.o
+obj-$(CONFIG_MTD_POWERNV_FLASH)	+= powernv_flash.o
 
 
 CFLAGS_docg3.o			+= -I$(src)
diff --git a/drivers/mtd/devices/block2mtd.c b/drivers/mtd/devices/block2mtd.c
index b16f3cda97ff..e2c0057737e6 100644
--- a/drivers/mtd/devices/block2mtd.c
+++ b/drivers/mtd/devices/block2mtd.c
@@ -20,6 +20,7 @@
 #include <linux/delay.h>
 #include <linux/fs.h>
 #include <linux/blkdev.h>
+#include <linux/backing-dev.h>
 #include <linux/bio.h>
 #include <linux/pagemap.h>
 #include <linux/list.h>
diff --git a/drivers/mtd/devices/docg3.c b/drivers/mtd/devices/docg3.c
index 866d31904475..5e67b4acde78 100644
--- a/drivers/mtd/devices/docg3.c
+++ b/drivers/mtd/devices/docg3.c
@@ -1815,7 +1815,7 @@ static void doc_dbg_unregister(struct docg3 *docg3)
  * @chip_id: The chip ID of the supported chip
  * @mtd: The structure to fill
  */
-static void __init doc_set_driver_info(int chip_id, struct mtd_info *mtd)
+static int __init doc_set_driver_info(int chip_id, struct mtd_info *mtd)
 {
 	struct docg3 *docg3 = mtd->priv;
 	int cfg;
@@ -1828,6 +1828,8 @@ static void __init doc_set_driver_info(int chip_id, struct mtd_info *mtd)
 	case DOC_CHIPID_G3:
 		mtd->name = kasprintf(GFP_KERNEL, "docg3.%d",
 				      docg3->device_id);
+		if (!mtd->name)
+			return -ENOMEM;
 		docg3->max_block = 2047;
 		break;
 	}
@@ -1850,6 +1852,8 @@ static void __init doc_set_driver_info(int chip_id, struct mtd_info *mtd)
 	mtd->_block_isbad = doc_block_isbad;
 	mtd->ecclayout = &docg3_oobinfo;
 	mtd->ecc_strength = DOC_ECC_BCH_T;
+
+	return 0;
 }
 
 /**
@@ -1900,7 +1904,7 @@ doc_probe_device(struct docg3_cascade *cascade, int floor, struct device *dev)
 
 	ret = 0;
 	if (chip_id != (u16)(~chip_id_inv)) {
-		goto nomem3;
+		goto nomem4;
 	}
 
 	switch (chip_id) {
@@ -1910,15 +1914,19 @@ doc_probe_device(struct docg3_cascade *cascade, int floor, struct device *dev)
 		break;
 	default:
 		doc_err("Chip id %04x is not a DiskOnChip G3 chip\n", chip_id);
-		goto nomem3;
+		goto nomem4;
 	}
 
-	doc_set_driver_info(chip_id, mtd);
+	ret = doc_set_driver_info(chip_id, mtd);
+	if (ret)
+		goto nomem4;
 
 	doc_hamming_ecc_init(docg3, DOC_LAYOUT_OOB_PAGEINFO_SZ);
 	doc_reload_bbt(docg3);
 	return mtd;
 
+nomem4:
+	kfree(docg3->bbt);
 nomem3:
 	kfree(mtd);
 nomem2:
@@ -2117,7 +2125,7 @@ static int docg3_release(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id docg3_dt_ids[] = {
+static const struct of_device_id docg3_dt_ids[] = {
 	{ .compatible = "m-systems,diskonchip-g3" },
 	{}
 };
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 3af137f49ac9..d313f948b96c 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -261,45 +261,33 @@ static int m25p_remove(struct spi_device *spi)
  * keep them available as module aliases for existing platforms.
  */
 static const struct spi_device_id m25p_ids[] = {
-	{"at25fs010"},	{"at25fs040"},	{"at25df041a"},	{"at25df321a"},
-	{"at25df641"},	{"at26f004"},	{"at26df081a"},	{"at26df161a"},
-	{"at26df321"},	{"at45db081d"},
-	{"en25f32"},	{"en25p32"},	{"en25q32b"},	{"en25p64"},
-	{"en25q64"},	{"en25qh128"},	{"en25qh256"},
-	{"f25l32pa"},
-	{"mr25h256"},	{"mr25h10"},
-	{"gd25q32"},	{"gd25q64"},
-	{"160s33b"},	{"320s33b"},	{"640s33b"},
-	{"mx25l2005a"},	{"mx25l4005a"},	{"mx25l8005"},	{"mx25l1606e"},
-	{"mx25l3205d"},	{"mx25l3255e"},	{"mx25l6405d"},	{"mx25l12805d"},
-	{"mx25l12855e"},{"mx25l25635e"},{"mx25l25655e"},{"mx66l51235l"},
-	{"mx66l1g55g"},
-	{"n25q064"},	{"n25q128a11"},	{"n25q128a13"},	{"n25q256a"},
-	{"n25q512a"},	{"n25q512ax3"},	{"n25q00"},
-	{"pm25lv512"},	{"pm25lv010"},	{"pm25lq032"},
-	{"s25sl032p"},	{"s25sl064p"},	{"s25fl256s0"},	{"s25fl256s1"},
-	{"s25fl512s"},	{"s70fl01gs"},	{"s25sl12800"},	{"s25sl12801"},
-	{"s25fl129p0"},	{"s25fl129p1"},	{"s25sl004a"},	{"s25sl008a"},
-	{"s25sl016a"},	{"s25sl032a"},	{"s25sl064a"},	{"s25fl008k"},
-	{"s25fl016k"},	{"s25fl064k"},	{"s25fl132k"},
-	{"sst25vf040b"},{"sst25vf080b"},{"sst25vf016b"},{"sst25vf032b"},
-	{"sst25vf064c"},{"sst25wf512"},	{"sst25wf010"},	{"sst25wf020"},
-	{"sst25wf040"},
-	{"m25p05"},	{"m25p10"},	{"m25p20"},	{"m25p40"},
-	{"m25p80"},	{"m25p16"},	{"m25p32"},	{"m25p64"},
-	{"m25p128"},	{"n25q032"},
+	/*
+	 * Entries not used in DTs that should be safe to drop after replacing
+	 * them with "nor-jedec" in platform data.
+	 */
+	{"s25sl064a"},	{"w25x16"},	{"m25p10"},	{"m25px64"},
+
+	/*
+	 * Entries that were used in DTs without "nor-jedec" fallback and should
+	 * be kept for backward compatibility.
+	 */
+	{"at25df321a"},	{"at25df641"},	{"at26df081a"},
+	{"mr25h256"},
+	{"mx25l4005a"},	{"mx25l1606e"},	{"mx25l6405d"},	{"mx25l12805d"},
+	{"mx25l25635e"},{"mx66l51235l"},
+	{"n25q064"},	{"n25q128a11"},	{"n25q128a13"},	{"n25q512a"},
+	{"s25fl256s1"},	{"s25fl512s"},	{"s25sl12801"},	{"s25fl008k"},
+	{"s25fl064k"},
+	{"sst25vf040b"},{"sst25vf016b"},{"sst25vf032b"},{"sst25wf040"},
+	{"m25p40"},	{"m25p80"},	{"m25p16"},	{"m25p32"},
+	{"m25p64"},	{"m25p128"},
+	{"w25x80"},	{"w25x32"},	{"w25q32"},	{"w25q32dw"},
+	{"w25q80bl"},	{"w25q128"},	{"w25q256"},
+
+	/* Flashes that can't be detected using JEDEC */
 	{"m25p05-nonjedec"},	{"m25p10-nonjedec"},	{"m25p20-nonjedec"},
 	{"m25p40-nonjedec"},	{"m25p80-nonjedec"},	{"m25p16-nonjedec"},
 	{"m25p32-nonjedec"},	{"m25p64-nonjedec"},	{"m25p128-nonjedec"},
-	{"m45pe10"},	{"m45pe80"},	{"m45pe16"},
-	{"m25pe20"},	{"m25pe80"},	{"m25pe16"},
-	{"m25px16"},	{"m25px32"},	{"m25px32-s0"},	{"m25px32-s1"},
-	{"m25px64"},	{"m25px80"},
-	{"w25x10"},	{"w25x20"},	{"w25x40"},	{"w25x80"},
-	{"w25x16"},	{"w25x32"},	{"w25q32"},	{"w25q32dw"},
-	{"w25x64"},	{"w25q64"},	{"w25q80"},	{"w25q80bl"},
-	{"w25q128"},	{"w25q256"},	{"cat25c11"},
-	{"cat25c03"},	{"cat25c09"},	{"cat25c17"},	{"cat25128"},
 
 	/*
 	 * Generic support for SPI NOR that can be identified by the JEDEC READ
diff --git a/drivers/mtd/devices/powernv_flash.c b/drivers/mtd/devices/powernv_flash.c
new file mode 100644
index 000000000000..d5b870b3fd4e
--- /dev/null
+++ b/drivers/mtd/devices/powernv_flash.c
@@ -0,0 +1,285 @@
+/*
+ * OPAL PNOR flash MTD abstraction
+ *
+ * Copyright IBM 2015
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under 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/kernel.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+#include <asm/opal.h>
+
+
+/*
+ * This driver creates the a Linux MTD abstraction for platform PNOR flash
+ * backed by OPAL calls
+ */
+
+struct powernv_flash {
+	struct mtd_info	mtd;
+	u32 id;
+};
+
+enum flash_op {
+	FLASH_OP_READ,
+	FLASH_OP_WRITE,
+	FLASH_OP_ERASE,
+};
+
+static int powernv_flash_async_op(struct mtd_info *mtd, enum flash_op op,
+		loff_t offset, size_t len, size_t *retlen, u_char *buf)
+{
+	struct powernv_flash *info = (struct powernv_flash *)mtd->priv;
+	struct device *dev = &mtd->dev;
+	int token;
+	struct opal_msg msg;
+	int rc;
+
+	dev_dbg(dev, "%s(op=%d, offset=0x%llx, len=%zu)\n",
+			__func__, op, offset, len);
+
+	token = opal_async_get_token_interruptible();
+	if (token < 0) {
+		if (token != -ERESTARTSYS)
+			dev_err(dev, "Failed to get an async token\n");
+
+		return token;
+	}
+
+	switch (op) {
+	case FLASH_OP_READ:
+		rc = opal_flash_read(info->id, offset, __pa(buf), len, token);
+		break;
+	case FLASH_OP_WRITE:
+		rc = opal_flash_write(info->id, offset, __pa(buf), len, token);
+		break;
+	case FLASH_OP_ERASE:
+		rc = opal_flash_erase(info->id, offset, len, token);
+		break;
+	default:
+		BUG_ON(1);
+	}
+
+	if (rc != OPAL_ASYNC_COMPLETION) {
+		dev_err(dev, "opal_flash_async_op(op=%d) failed (rc %d)\n",
+				op, rc);
+		opal_async_release_token(token);
+		return -EIO;
+	}
+
+	rc = opal_async_wait_response(token, &msg);
+	opal_async_release_token(token);
+	if (rc) {
+		dev_err(dev, "opal async wait failed (rc %d)\n", rc);
+		return -EIO;
+	}
+
+	rc = be64_to_cpu(msg.params[1]);
+	if (rc == OPAL_SUCCESS) {
+		rc = 0;
+		if (retlen)
+			*retlen = len;
+	} else {
+		rc = -EIO;
+	}
+
+	return rc;
+}
+
+/**
+ * @mtd: the device
+ * @from: the offset to read from
+ * @len: the number of bytes to read
+ * @retlen: the number of bytes actually read
+ * @buf: the filled in buffer
+ *
+ * Returns 0 if read successful, or -ERRNO if an error occurred
+ */
+static int powernv_flash_read(struct mtd_info *mtd, loff_t from, size_t len,
+	     size_t *retlen, u_char *buf)
+{
+	return powernv_flash_async_op(mtd, FLASH_OP_READ, from,
+			len, retlen, buf);
+}
+
+/**
+ * @mtd: the device
+ * @to: the offset to write to
+ * @len: the number of bytes to write
+ * @retlen: the number of bytes actually written
+ * @buf: the buffer to get bytes from
+ *
+ * Returns 0 if write successful, -ERRNO if error occurred
+ */
+static int powernv_flash_write(struct mtd_info *mtd, loff_t to, size_t len,
+		     size_t *retlen, const u_char *buf)
+{
+	return powernv_flash_async_op(mtd, FLASH_OP_WRITE, to,
+			len, retlen, (u_char *)buf);
+}
+
+/**
+ * @mtd: the device
+ * @erase: the erase info
+ * Returns 0 if erase successful or -ERRNO if an error occurred
+ */
+static int powernv_flash_erase(struct mtd_info *mtd, struct erase_info *erase)
+{
+	int rc;
+
+	erase->state = MTD_ERASING;
+
+	/* todo: register our own notifier to do a true async implementation */
+	rc =  powernv_flash_async_op(mtd, FLASH_OP_ERASE, erase->addr,
+			erase->len, NULL, NULL);
+
+	if (rc) {
+		erase->fail_addr = erase->addr;
+		erase->state = MTD_ERASE_FAILED;
+	} else {
+		erase->state = MTD_ERASE_DONE;
+	}
+	mtd_erase_callback(erase);
+	return rc;
+}
+
+/**
+ * powernv_flash_set_driver_info - Fill the mtd_info structure and docg3
+ * structure @pdev: The platform device
+ * @mtd: The structure to fill
+ */
+static int powernv_flash_set_driver_info(struct device *dev,
+		struct mtd_info *mtd)
+{
+	u64 size;
+	u32 erase_size;
+	int rc;
+
+	rc = of_property_read_u32(dev->of_node, "ibm,flash-block-size",
+			&erase_size);
+	if (rc) {
+		dev_err(dev, "couldn't get resource block size information\n");
+		return rc;
+	}
+
+	rc = of_property_read_u64(dev->of_node, "reg", &size);
+	if (rc) {
+		dev_err(dev, "couldn't get resource size information\n");
+		return rc;
+	}
+
+	/*
+	 * Going to have to check what details I need to set and how to
+	 * get them
+	 */
+	mtd->name = of_get_property(dev->of_node, "name", NULL);
+	mtd->type = MTD_NORFLASH;
+	mtd->flags = MTD_WRITEABLE;
+	mtd->size = size;
+	mtd->erasesize = erase_size;
+	mtd->writebufsize = mtd->writesize = 1;
+	mtd->owner = THIS_MODULE;
+	mtd->_erase = powernv_flash_erase;
+	mtd->_read = powernv_flash_read;
+	mtd->_write = powernv_flash_write;
+	mtd->dev.parent = dev;
+	return 0;
+}
+
+/**
+ * powernv_flash_probe
+ * @pdev: platform device
+ *
+ * Returns 0 on success, -ENOMEM, -ENXIO on error
+ */
+static int powernv_flash_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct powernv_flash *data;
+	int ret;
+
+	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	data->mtd.priv = data;
+
+	ret = of_property_read_u32(dev->of_node, "ibm,opal-id", &(data->id));
+	if (ret) {
+		dev_err(dev, "no device property 'ibm,opal-id'\n");
+		goto out;
+	}
+
+	ret = powernv_flash_set_driver_info(dev, &data->mtd);
+	if (ret)
+		goto out;
+
+	dev_set_drvdata(dev, data);
+
+	/*
+	 * The current flash that skiboot exposes is one contiguous flash chip
+	 * with an ffs partition at the start, it should prove easier for users
+	 * to deal with partitions or not as they see fit
+	 */
+	ret = mtd_device_register(&data->mtd, NULL, 0);
+
+out:
+	return ret;
+}
+
+/**
+ * op_release - Release the driver
+ * @pdev: the platform device
+ *
+ * Returns 0
+ */
+static int powernv_flash_release(struct platform_device *pdev)
+{
+	struct powernv_flash *data = dev_get_drvdata(&(pdev->dev));
+
+	/* All resources should be freed automatically */
+	return mtd_device_unregister(&(data->mtd));
+}
+
+static const struct of_device_id powernv_flash_match[] = {
+	{ .compatible = "ibm,opal-flash" },
+	{}
+};
+
+static struct platform_driver powernv_flash_driver = {
+	.driver		= {
+		.name		= "powernv_flash",
+		.of_match_table	= powernv_flash_match,
+	},
+	.remove		= powernv_flash_release,
+	.probe		= powernv_flash_probe,
+};
+
+module_platform_driver(powernv_flash_driver);
+
+MODULE_DEVICE_TABLE(of, powernv_flash_match);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Cyril Bur <cyril.bur@au1.ibm.com>");
+MODULE_DESCRIPTION("MTD abstraction for OPAL flash");
diff --git a/drivers/mtd/devices/spear_smi.c b/drivers/mtd/devices/spear_smi.c
index 508bab3bd0c4..04b24d2b03f2 100644
--- a/drivers/mtd/devices/spear_smi.c
+++ b/drivers/mtd/devices/spear_smi.c
@@ -1,8 +1,8 @@
 /*
  * SMI (Serial Memory Controller) device driver for Serial NOR Flash on
  * SPEAr platform
- * The serial nor interface is largely based on drivers/mtd/m25p80.c,
- * however the SPI interface has been replaced by SMI.
+ * The serial nor interface is largely based on m25p80.c, however the SPI
+ * interface has been replaced by SMI.
  *
  * Copyright © 2010 STMicroelectronics.
  * Ashish Priyadarshi
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index e715ae90632f..7c95a656f9e4 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -326,7 +326,7 @@ config MTD_BFIN_ASYNC
 
 config MTD_GPIO_ADDR
 	tristate "GPIO-assisted Flash Chip Support"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	depends on MTD_COMPLEX_MAPPINGS
 	help
 	  Map driver which allows flashes to be partially physically addressed
diff --git a/drivers/mtd/maps/amd76xrom.c b/drivers/mtd/maps/amd76xrom.c
index f7207b0a76dc..f2b68667ea59 100644
--- a/drivers/mtd/maps/amd76xrom.c
+++ b/drivers/mtd/maps/amd76xrom.c
@@ -138,7 +138,7 @@ static int amd76xrom_init_one(struct pci_dev *pdev,
 	/*
 	 * Try to reserve the window mem region.  If this fails then
 	 * it is likely due to a fragment of the window being
-	 * "reseved" by the BIOS.  In the case that the
+	 * "reserved" by the BIOS.  In the case that the
 	 * request_mem_region() fails then once the rom size is
 	 * discovered we will try to reserve the unreserved fragment.
 	 */
diff --git a/drivers/mtd/maps/dc21285.c b/drivers/mtd/maps/dc21285.c
index f8a7dd14cee0..70a3db3ab856 100644
--- a/drivers/mtd/maps/dc21285.c
+++ b/drivers/mtd/maps/dc21285.c
@@ -38,9 +38,9 @@ static void nw_en_write(void)
 	 * we want to write a bit pattern XXX1 to Xilinx to enable
 	 * the write gate, which will be open for about the next 2ms.
 	 */
-	spin_lock_irqsave(&nw_gpio_lock, flags);
+	raw_spin_lock_irqsave(&nw_gpio_lock, flags);
 	nw_cpld_modify(CPLD_FLASH_WR_ENABLE, CPLD_FLASH_WR_ENABLE);
-	spin_unlock_irqrestore(&nw_gpio_lock, flags);
+	raw_spin_unlock_irqrestore(&nw_gpio_lock, flags);
 
 	/*
 	 * let the ISA bus to catch on...
diff --git a/drivers/mtd/maps/esb2rom.c b/drivers/mtd/maps/esb2rom.c
index f784cf0caa13..76ed651b515b 100644
--- a/drivers/mtd/maps/esb2rom.c
+++ b/drivers/mtd/maps/esb2rom.c
@@ -234,7 +234,7 @@ static int esb2rom_init_one(struct pci_dev *pdev,
 
 	/*
 	 * Try to reserve the window mem region.  If this fails then
-	 * it is likely due to the window being "reseved" by the BIOS.
+	 * it is likely due to the window being "reserved" by the BIOS.
 	 */
 	window->rsrc.name = MOD_NAME;
 	window->rsrc.start = window->phys;
diff --git a/drivers/mtd/maps/ichxrom.c b/drivers/mtd/maps/ichxrom.c
index c7478e18f485..8636bba42200 100644
--- a/drivers/mtd/maps/ichxrom.c
+++ b/drivers/mtd/maps/ichxrom.c
@@ -167,7 +167,7 @@ static int ichxrom_init_one(struct pci_dev *pdev,
 
 	/*
 	 * Try to reserve the window mem region.  If this fails then
-	 * it is likely due to the window being "reseved" by the BIOS.
+	 * it is likely due to the window being "reserved" by the BIOS.
 	 */
 	window->rsrc.name = MOD_NAME;
 	window->rsrc.start = window->phys;
diff --git a/drivers/mtd/maps/lantiq-flash.c b/drivers/mtd/maps/lantiq-flash.c
index 33d26f5bee54..e2f878216048 100644
--- a/drivers/mtd/maps/lantiq-flash.c
+++ b/drivers/mtd/maps/lantiq-flash.c
@@ -45,7 +45,6 @@ struct ltq_mtd {
 };
 
 static const char ltq_map_name[] = "ltq_nor";
-static const char * const ltq_probe_types[] = { "cmdlinepart", "ofpart", NULL };
 
 static map_word
 ltq_read16(struct map_info *map, unsigned long adr)
@@ -168,8 +167,7 @@ ltq_mtd_probe(struct platform_device *pdev)
 	cfi->addr_unlock2 ^= 1;
 
 	ppdata.of_node = pdev->dev.of_node;
-	err = mtd_device_parse_register(ltq_mtd->mtd, ltq_probe_types,
-					&ppdata, NULL, 0);
+	err = mtd_device_parse_register(ltq_mtd->mtd, NULL, &ppdata, NULL, 0);
 	if (err) {
 		dev_err(&pdev->dev, "failed to add partitions\n");
 		goto err_destroy;
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index ff26e979b1a1..774b32fd29e6 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -147,7 +147,7 @@ static void of_free_probes(const char * const *probes)
 		kfree(probes);
 }
 
-static struct of_device_id of_flash_match[];
+static const struct of_device_id of_flash_match[];
 static int of_flash_probe(struct platform_device *dev)
 {
 	const char * const *part_probe_types;
@@ -327,7 +327,7 @@ err_flash_remove:
 	return err;
 }
 
-static struct of_device_id of_flash_match[] = {
+static const struct of_device_id of_flash_match[] = {
 	{
 		.compatible	= "cfi-flash",
 		.data		= (void *)"cfi_probe",
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index 2b0c52870999..41acc507b22e 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -197,6 +197,7 @@ static int blktrans_open(struct block_device *bdev, fmode_t mode)
 		return -ERESTARTSYS; /* FIXME: busy loop! -arnd*/
 
 	mutex_lock(&dev->lock);
+	mutex_lock(&mtd_table_mutex);
 
 	if (dev->open)
 		goto unlock;
@@ -220,6 +221,7 @@ static int blktrans_open(struct block_device *bdev, fmode_t mode)
 
 unlock:
 	dev->open++;
+	mutex_unlock(&mtd_table_mutex);
 	mutex_unlock(&dev->lock);
 	blktrans_dev_put(dev);
 	return ret;
@@ -230,6 +232,7 @@ error_release:
 error_put:
 	module_put(dev->tr->owner);
 	kref_put(&dev->ref, blktrans_dev_release);
+	mutex_unlock(&mtd_table_mutex);
 	mutex_unlock(&dev->lock);
 	blktrans_dev_put(dev);
 	return ret;
@@ -243,6 +246,7 @@ static void blktrans_release(struct gendisk *disk, fmode_t mode)
 		return;
 
 	mutex_lock(&dev->lock);
+	mutex_lock(&mtd_table_mutex);
 
 	if (--dev->open)
 		goto unlock;
@@ -256,6 +260,7 @@ static void blktrans_release(struct gendisk *disk, fmode_t mode)
 		__put_mtd_device(dev->mtd);
 	}
 unlock:
+	mutex_unlock(&mtd_table_mutex);
 	mutex_unlock(&dev->lock);
 	blktrans_dev_put(dev);
 }
@@ -273,7 +278,7 @@ static int blktrans_getgeo(struct block_device *bdev, struct hd_geometry *geo)
 	if (!dev->mtd)
 		goto unlock;
 
-	ret = dev->tr->getgeo ? dev->tr->getgeo(dev, geo) : 0;
+	ret = dev->tr->getgeo ? dev->tr->getgeo(dev, geo) : -ENOTTY;
 unlock:
 	mutex_unlock(&dev->lock);
 	blktrans_dev_put(dev);
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index d172195fbd15..8bbbb751bf45 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -48,14 +48,34 @@
 static struct backing_dev_info mtd_bdi = {
 };
 
-static int mtd_cls_suspend(struct device *dev, pm_message_t state);
-static int mtd_cls_resume(struct device *dev);
+#ifdef CONFIG_PM_SLEEP
+
+static int mtd_cls_suspend(struct device *dev)
+{
+	struct mtd_info *mtd = dev_get_drvdata(dev);
+
+	return mtd ? mtd_suspend(mtd) : 0;
+}
+
+static int mtd_cls_resume(struct device *dev)
+{
+	struct mtd_info *mtd = dev_get_drvdata(dev);
+
+	if (mtd)
+		mtd_resume(mtd);
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(mtd_cls_pm_ops, mtd_cls_suspend, mtd_cls_resume);
+#define MTD_CLS_PM_OPS (&mtd_cls_pm_ops)
+#else
+#define MTD_CLS_PM_OPS NULL
+#endif
 
 static struct class mtd_class = {
 	.name = "mtd",
 	.owner = THIS_MODULE,
-	.suspend = mtd_cls_suspend,
-	.resume = mtd_cls_resume,
+	.pm = MTD_CLS_PM_OPS,
 };
 
 static DEFINE_IDR(mtd_idr);
@@ -88,22 +108,6 @@ static void mtd_release(struct device *dev)
 	device_destroy(&mtd_class, index + 1);
 }
 
-static int mtd_cls_suspend(struct device *dev, pm_message_t state)
-{
-	struct mtd_info *mtd = dev_get_drvdata(dev);
-
-	return mtd ? mtd_suspend(mtd) : 0;
-}
-
-static int mtd_cls_resume(struct device *dev)
-{
-	struct mtd_info *mtd = dev_get_drvdata(dev);
-
-	if (mtd)
-		mtd_resume(mtd);
-	return 0;
-}
-
 static ssize_t mtd_type_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -375,8 +379,7 @@ static int mtd_reboot_notifier(struct notifier_block *n, unsigned long state,
  *
  *	Add a device to the list of MTD devices present in the system, and
  *	notify each currently active MTD 'user' of its arrival. Returns
- *	zero on success or 1 on failure, which currently will only happen
- *	if there is insufficient memory or a sysfs error.
+ *	zero on success or non-zero on failure.
  */
 
 int add_mtd_device(struct mtd_info *mtd)
@@ -390,8 +393,10 @@ int add_mtd_device(struct mtd_info *mtd)
 	mutex_lock(&mtd_table_mutex);
 
 	i = idr_alloc(&mtd_idr, mtd, 0, 0, GFP_KERNEL);
-	if (i < 0)
+	if (i < 0) {
+		error = i;
 		goto fail_locked;
+	}
 
 	mtd->index = i;
 	mtd->usecount = 0;
@@ -420,6 +425,8 @@ int add_mtd_device(struct mtd_info *mtd)
 			printk(KERN_WARNING
 			       "%s: unlock failed, writes may not work\n",
 			       mtd->name);
+		/* Ignore unlock failures? */
+		error = 0;
 	}
 
 	/* Caller should have set dev.parent to match the
@@ -430,7 +437,8 @@ int add_mtd_device(struct mtd_info *mtd)
 	mtd->dev.devt = MTD_DEVT(i);
 	dev_set_name(&mtd->dev, "mtd%d", i);
 	dev_set_drvdata(&mtd->dev, mtd);
-	if (device_register(&mtd->dev) != 0)
+	error = device_register(&mtd->dev);
+	if (error)
 		goto fail_added;
 
 	device_create(&mtd_class, mtd->dev.parent, MTD_DEVT(i) + 1, NULL,
@@ -454,7 +462,7 @@ fail_added:
 	idr_remove(&mtd_idr, i);
 fail_locked:
 	mutex_unlock(&mtd_table_mutex);
-	return 1;
+	return error;
 }
 
 /**
@@ -510,8 +518,8 @@ static int mtd_add_device_partitions(struct mtd_info *mtd,
 
 	if (nbparts == 0 || IS_ENABLED(CONFIG_MTD_PARTITIONED_MASTER)) {
 		ret = add_mtd_device(mtd);
-		if (ret == 1)
-			return -ENODEV;
+		if (ret)
+			return ret;
 	}
 
 	if (nbparts > 0) {
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 5897d8d8fa5a..5b2806a7e5f7 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -76,7 +76,7 @@ config MTD_NAND_DENALI_SCRATCH_REG_ADDR
 
 config MTD_NAND_GPIO
 	tristate "GPIO assisted NAND Flash driver"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  This enables a NAND flash driver where control signals are
 	  connected to GPIO pins, and commands and data are communicated
@@ -394,6 +394,14 @@ config MTD_NAND_GPMI_NAND
 	 block, such as SD card. So pay attention to it when you enable
 	 the GPMI.
 
+config MTD_NAND_BRCMNAND
+	tristate "Broadcom STB NAND controller"
+	depends on ARM || MIPS
+	help
+	  Enables the Broadcom NAND controller driver. The controller was
+	  originally designed for Set-Top Box but is used on various BCM7xxx,
+	  BCM3xxx, BCM63xxx, iProc/Cygnus and more.
+
 config MTD_NAND_BCM47XXNFLASH
 	tristate "Support for NAND flash on BCM4706 BCMA bus"
 	depends on BCMA_NFLASH
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index 582bbd05aff7..1f897ec3c242 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -52,5 +52,6 @@ obj-$(CONFIG_MTD_NAND_XWAY)		+= xway_nand.o
 obj-$(CONFIG_MTD_NAND_BCM47XXNFLASH)	+= bcm47xxnflash/
 obj-$(CONFIG_MTD_NAND_SUNXI)		+= sunxi_nand.o
 obj-$(CONFIG_MTD_NAND_HISI504)	        += hisi504_nand.o
+obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= brcmnand/
 
 nand-objs := nand_base.o nand_bbt.o nand_timings.o
diff --git a/drivers/mtd/nand/brcmnand/Makefile b/drivers/mtd/nand/brcmnand/Makefile
new file mode 100644
index 000000000000..3b1fbfd27d4f
--- /dev/null
+++ b/drivers/mtd/nand/brcmnand/Makefile
@@ -0,0 +1,6 @@
+# link order matters; don't link the more generic brcmstb_nand.o before the
+# more specific iproc_nand.o, for instance
+obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= iproc_nand.o
+obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= bcm63138_nand.o
+obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= brcmstb_nand.o
+obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= brcmnand.o
diff --git a/drivers/mtd/nand/brcmnand/bcm63138_nand.c b/drivers/mtd/nand/brcmnand/bcm63138_nand.c
new file mode 100644
index 000000000000..3f4c44c24e14
--- /dev/null
+++ b/drivers/mtd/nand/brcmnand/bcm63138_nand.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright © 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "brcmnand.h"
+
+struct bcm63138_nand_soc_priv {
+	void __iomem *base;
+};
+
+#define BCM63138_NAND_INT_STATUS		0x00
+#define BCM63138_NAND_INT_EN			0x04
+
+enum {
+	BCM63138_CTLRDY		= BIT(4),
+};
+
+static bool bcm63138_nand_intc_ack(struct brcmnand_soc *soc)
+{
+	struct bcm63138_nand_soc_priv *priv = soc->priv;
+	void __iomem *mmio = priv->base + BCM63138_NAND_INT_STATUS;
+	u32 val = brcmnand_readl(mmio);
+
+	if (val & BCM63138_CTLRDY) {
+		brcmnand_writel(val & ~BCM63138_CTLRDY, mmio);
+		return true;
+	}
+
+	return false;
+}
+
+static void bcm63138_nand_intc_set(struct brcmnand_soc *soc, bool en)
+{
+	struct bcm63138_nand_soc_priv *priv = soc->priv;
+	void __iomem *mmio = priv->base + BCM63138_NAND_INT_EN;
+	u32 val = brcmnand_readl(mmio);
+
+	if (en)
+		val |= BCM63138_CTLRDY;
+	else
+		val &= ~BCM63138_CTLRDY;
+
+	brcmnand_writel(val, mmio);
+}
+
+static int bcm63138_nand_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct bcm63138_nand_soc_priv *priv;
+	struct brcmnand_soc *soc;
+	struct resource *res;
+
+	soc = devm_kzalloc(dev, sizeof(*soc), GFP_KERNEL);
+	if (!soc)
+		return -ENOMEM;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "nand-int-base");
+	priv->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
+
+	soc->pdev = pdev;
+	soc->priv = priv;
+	soc->ctlrdy_ack = bcm63138_nand_intc_ack;
+	soc->ctlrdy_set_enabled = bcm63138_nand_intc_set;
+
+	return brcmnand_probe(pdev, soc);
+}
+
+static const struct of_device_id bcm63138_nand_of_match[] = {
+	{ .compatible = "brcm,nand-bcm63138" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, bcm63138_nand_of_match);
+
+static struct platform_driver bcm63138_nand_driver = {
+	.probe			= bcm63138_nand_probe,
+	.remove			= brcmnand_remove,
+	.driver = {
+		.name		= "bcm63138_nand",
+		.pm		= &brcmnand_pm_ops,
+		.of_match_table	= bcm63138_nand_of_match,
+	}
+};
+module_platform_driver(bcm63138_nand_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Brian Norris");
+MODULE_DESCRIPTION("NAND driver for BCM63138");
diff --git a/drivers/mtd/nand/brcmnand/brcmnand.c b/drivers/mtd/nand/brcmnand/brcmnand.c
new file mode 100644
index 000000000000..fddb795eeb71
--- /dev/null
+++ b/drivers/mtd/nand/brcmnand/brcmnand.c
@@ -0,0 +1,2246 @@
+/*
+ * Copyright © 2010-2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/dma-mapping.h>
+#include <linux/ioport.h>
+#include <linux/bug.h>
+#include <linux/kernel.h>
+#include <linux/bitops.h>
+#include <linux/mm.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <linux/of.h>
+#include <linux/of_mtd.h>
+#include <linux/of_platform.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/log2.h>
+
+#include "brcmnand.h"
+
+/*
+ * This flag controls if WP stays on between erase/write commands to mitigate
+ * flash corruption due to power glitches. Values:
+ * 0: NAND_WP is not used or not available
+ * 1: NAND_WP is set by default, cleared for erase/write operations
+ * 2: NAND_WP is always cleared
+ */
+static int wp_on = 1;
+module_param(wp_on, int, 0444);
+
+/***********************************************************************
+ * Definitions
+ ***********************************************************************/
+
+#define DRV_NAME			"brcmnand"
+
+#define CMD_NULL			0x00
+#define CMD_PAGE_READ			0x01
+#define CMD_SPARE_AREA_READ		0x02
+#define CMD_STATUS_READ			0x03
+#define CMD_PROGRAM_PAGE		0x04
+#define CMD_PROGRAM_SPARE_AREA		0x05
+#define CMD_COPY_BACK			0x06
+#define CMD_DEVICE_ID_READ		0x07
+#define CMD_BLOCK_ERASE			0x08
+#define CMD_FLASH_RESET			0x09
+#define CMD_BLOCKS_LOCK			0x0a
+#define CMD_BLOCKS_LOCK_DOWN		0x0b
+#define CMD_BLOCKS_UNLOCK		0x0c
+#define CMD_READ_BLOCKS_LOCK_STATUS	0x0d
+#define CMD_PARAMETER_READ		0x0e
+#define CMD_PARAMETER_CHANGE_COL	0x0f
+#define CMD_LOW_LEVEL_OP		0x10
+
+struct brcm_nand_dma_desc {
+	u32 next_desc;
+	u32 next_desc_ext;
+	u32 cmd_irq;
+	u32 dram_addr;
+	u32 dram_addr_ext;
+	u32 tfr_len;
+	u32 total_len;
+	u32 flash_addr;
+	u32 flash_addr_ext;
+	u32 cs;
+	u32 pad2[5];
+	u32 status_valid;
+} __packed;
+
+/* Bitfields for brcm_nand_dma_desc::status_valid */
+#define FLASH_DMA_ECC_ERROR	(1 << 8)
+#define FLASH_DMA_CORR_ERROR	(1 << 9)
+
+/* 512B flash cache in the NAND controller HW */
+#define FC_SHIFT		9U
+#define FC_BYTES		512U
+#define FC_WORDS		(FC_BYTES >> 2)
+
+#define BRCMNAND_MIN_PAGESIZE	512
+#define BRCMNAND_MIN_BLOCKSIZE	(8 * 1024)
+#define BRCMNAND_MIN_DEVSIZE	(4ULL * 1024 * 1024)
+
+/* Controller feature flags */
+enum {
+	BRCMNAND_HAS_1K_SECTORS			= BIT(0),
+	BRCMNAND_HAS_PREFETCH			= BIT(1),
+	BRCMNAND_HAS_CACHE_MODE			= BIT(2),
+	BRCMNAND_HAS_WP				= BIT(3),
+};
+
+struct brcmnand_controller {
+	struct device		*dev;
+	struct nand_hw_control	controller;
+	void __iomem		*nand_base;
+	void __iomem		*nand_fc; /* flash cache */
+	void __iomem		*flash_dma_base;
+	unsigned int		irq;
+	unsigned int		dma_irq;
+	int			nand_version;
+
+	/* Some SoCs provide custom interrupt status register(s) */
+	struct brcmnand_soc	*soc;
+
+	int			cmd_pending;
+	bool			dma_pending;
+	struct completion	done;
+	struct completion	dma_done;
+
+	/* List of NAND hosts (one for each chip-select) */
+	struct list_head host_list;
+
+	struct brcm_nand_dma_desc *dma_desc;
+	dma_addr_t		dma_pa;
+
+	/* in-memory cache of the FLASH_CACHE, used only for some commands */
+	u32			flash_cache[FC_WORDS];
+
+	/* Controller revision details */
+	const u16		*reg_offsets;
+	unsigned int		reg_spacing; /* between CS1, CS2, ... regs */
+	const u8		*cs_offsets; /* within each chip-select */
+	const u8		*cs0_offsets; /* within CS0, if different */
+	unsigned int		max_block_size;
+	const unsigned int	*block_sizes;
+	unsigned int		max_page_size;
+	const unsigned int	*page_sizes;
+	unsigned int		max_oob;
+	u32			features;
+
+	/* for low-power standby/resume only */
+	u32			nand_cs_nand_select;
+	u32			nand_cs_nand_xor;
+	u32			corr_stat_threshold;
+	u32			flash_dma_mode;
+};
+
+struct brcmnand_cfg {
+	u64			device_size;
+	unsigned int		block_size;
+	unsigned int		page_size;
+	unsigned int		spare_area_size;
+	unsigned int		device_width;
+	unsigned int		col_adr_bytes;
+	unsigned int		blk_adr_bytes;
+	unsigned int		ful_adr_bytes;
+	unsigned int		sector_size_1k;
+	unsigned int		ecc_level;
+	/* use for low-power standby/resume only */
+	u32			acc_control;
+	u32			config;
+	u32			config_ext;
+	u32			timing_1;
+	u32			timing_2;
+};
+
+struct brcmnand_host {
+	struct list_head	node;
+	struct device_node	*of_node;
+
+	struct nand_chip	chip;
+	struct mtd_info		mtd;
+	struct platform_device	*pdev;
+	int			cs;
+
+	unsigned int		last_cmd;
+	unsigned int		last_byte;
+	u64			last_addr;
+	struct brcmnand_cfg	hwcfg;
+	struct brcmnand_controller *ctrl;
+};
+
+enum brcmnand_reg {
+	BRCMNAND_CMD_START = 0,
+	BRCMNAND_CMD_EXT_ADDRESS,
+	BRCMNAND_CMD_ADDRESS,
+	BRCMNAND_INTFC_STATUS,
+	BRCMNAND_CS_SELECT,
+	BRCMNAND_CS_XOR,
+	BRCMNAND_LL_OP,
+	BRCMNAND_CS0_BASE,
+	BRCMNAND_CS1_BASE,		/* CS1 regs, if non-contiguous */
+	BRCMNAND_CORR_THRESHOLD,
+	BRCMNAND_CORR_THRESHOLD_EXT,
+	BRCMNAND_UNCORR_COUNT,
+	BRCMNAND_CORR_COUNT,
+	BRCMNAND_CORR_EXT_ADDR,
+	BRCMNAND_CORR_ADDR,
+	BRCMNAND_UNCORR_EXT_ADDR,
+	BRCMNAND_UNCORR_ADDR,
+	BRCMNAND_SEMAPHORE,
+	BRCMNAND_ID,
+	BRCMNAND_ID_EXT,
+	BRCMNAND_LL_RDATA,
+	BRCMNAND_OOB_READ_BASE,
+	BRCMNAND_OOB_READ_10_BASE,	/* offset 0x10, if non-contiguous */
+	BRCMNAND_OOB_WRITE_BASE,
+	BRCMNAND_OOB_WRITE_10_BASE,	/* offset 0x10, if non-contiguous */
+	BRCMNAND_FC_BASE,
+};
+
+/* BRCMNAND v4.0 */
+static const u16 brcmnand_regs_v40[] = {
+	[BRCMNAND_CMD_START]		=  0x04,
+	[BRCMNAND_CMD_EXT_ADDRESS]	=  0x08,
+	[BRCMNAND_CMD_ADDRESS]		=  0x0c,
+	[BRCMNAND_INTFC_STATUS]		=  0x6c,
+	[BRCMNAND_CS_SELECT]		=  0x14,
+	[BRCMNAND_CS_XOR]		=  0x18,
+	[BRCMNAND_LL_OP]		= 0x178,
+	[BRCMNAND_CS0_BASE]		=  0x40,
+	[BRCMNAND_CS1_BASE]		=  0xd0,
+	[BRCMNAND_CORR_THRESHOLD]	=  0x84,
+	[BRCMNAND_CORR_THRESHOLD_EXT]	=     0,
+	[BRCMNAND_UNCORR_COUNT]		=     0,
+	[BRCMNAND_CORR_COUNT]		=     0,
+	[BRCMNAND_CORR_EXT_ADDR]	=  0x70,
+	[BRCMNAND_CORR_ADDR]		=  0x74,
+	[BRCMNAND_UNCORR_EXT_ADDR]	=  0x78,
+	[BRCMNAND_UNCORR_ADDR]		=  0x7c,
+	[BRCMNAND_SEMAPHORE]		=  0x58,
+	[BRCMNAND_ID]			=  0x60,
+	[BRCMNAND_ID_EXT]		=  0x64,
+	[BRCMNAND_LL_RDATA]		= 0x17c,
+	[BRCMNAND_OOB_READ_BASE]	=  0x20,
+	[BRCMNAND_OOB_READ_10_BASE]	= 0x130,
+	[BRCMNAND_OOB_WRITE_BASE]	=  0x30,
+	[BRCMNAND_OOB_WRITE_10_BASE]	=     0,
+	[BRCMNAND_FC_BASE]		= 0x200,
+};
+
+/* BRCMNAND v5.0 */
+static const u16 brcmnand_regs_v50[] = {
+	[BRCMNAND_CMD_START]		=  0x04,
+	[BRCMNAND_CMD_EXT_ADDRESS]	=  0x08,
+	[BRCMNAND_CMD_ADDRESS]		=  0x0c,
+	[BRCMNAND_INTFC_STATUS]		=  0x6c,
+	[BRCMNAND_CS_SELECT]		=  0x14,
+	[BRCMNAND_CS_XOR]		=  0x18,
+	[BRCMNAND_LL_OP]		= 0x178,
+	[BRCMNAND_CS0_BASE]		=  0x40,
+	[BRCMNAND_CS1_BASE]		=  0xd0,
+	[BRCMNAND_CORR_THRESHOLD]	=  0x84,
+	[BRCMNAND_CORR_THRESHOLD_EXT]	=     0,
+	[BRCMNAND_UNCORR_COUNT]		=     0,
+	[BRCMNAND_CORR_COUNT]		=     0,
+	[BRCMNAND_CORR_EXT_ADDR]	=  0x70,
+	[BRCMNAND_CORR_ADDR]		=  0x74,
+	[BRCMNAND_UNCORR_EXT_ADDR]	=  0x78,
+	[BRCMNAND_UNCORR_ADDR]		=  0x7c,
+	[BRCMNAND_SEMAPHORE]		=  0x58,
+	[BRCMNAND_ID]			=  0x60,
+	[BRCMNAND_ID_EXT]		=  0x64,
+	[BRCMNAND_LL_RDATA]		= 0x17c,
+	[BRCMNAND_OOB_READ_BASE]	=  0x20,
+	[BRCMNAND_OOB_READ_10_BASE]	= 0x130,
+	[BRCMNAND_OOB_WRITE_BASE]	=  0x30,
+	[BRCMNAND_OOB_WRITE_10_BASE]	= 0x140,
+	[BRCMNAND_FC_BASE]		= 0x200,
+};
+
+/* BRCMNAND v6.0 - v7.1 */
+static const u16 brcmnand_regs_v60[] = {
+	[BRCMNAND_CMD_START]		=  0x04,
+	[BRCMNAND_CMD_EXT_ADDRESS]	=  0x08,
+	[BRCMNAND_CMD_ADDRESS]		=  0x0c,
+	[BRCMNAND_INTFC_STATUS]		=  0x14,
+	[BRCMNAND_CS_SELECT]		=  0x18,
+	[BRCMNAND_CS_XOR]		=  0x1c,
+	[BRCMNAND_LL_OP]		=  0x20,
+	[BRCMNAND_CS0_BASE]		=  0x50,
+	[BRCMNAND_CS1_BASE]		=     0,
+	[BRCMNAND_CORR_THRESHOLD]	=  0xc0,
+	[BRCMNAND_CORR_THRESHOLD_EXT]	=  0xc4,
+	[BRCMNAND_UNCORR_COUNT]		=  0xfc,
+	[BRCMNAND_CORR_COUNT]		= 0x100,
+	[BRCMNAND_CORR_EXT_ADDR]	= 0x10c,
+	[BRCMNAND_CORR_ADDR]		= 0x110,
+	[BRCMNAND_UNCORR_EXT_ADDR]	= 0x114,
+	[BRCMNAND_UNCORR_ADDR]		= 0x118,
+	[BRCMNAND_SEMAPHORE]		= 0x150,
+	[BRCMNAND_ID]			= 0x194,
+	[BRCMNAND_ID_EXT]		= 0x198,
+	[BRCMNAND_LL_RDATA]		= 0x19c,
+	[BRCMNAND_OOB_READ_BASE]	= 0x200,
+	[BRCMNAND_OOB_READ_10_BASE]	=     0,
+	[BRCMNAND_OOB_WRITE_BASE]	= 0x280,
+	[BRCMNAND_OOB_WRITE_10_BASE]	=     0,
+	[BRCMNAND_FC_BASE]		= 0x400,
+};
+
+enum brcmnand_cs_reg {
+	BRCMNAND_CS_CFG_EXT = 0,
+	BRCMNAND_CS_CFG,
+	BRCMNAND_CS_ACC_CONTROL,
+	BRCMNAND_CS_TIMING1,
+	BRCMNAND_CS_TIMING2,
+};
+
+/* Per chip-select offsets for v7.1 */
+static const u8 brcmnand_cs_offsets_v71[] = {
+	[BRCMNAND_CS_ACC_CONTROL]	= 0x00,
+	[BRCMNAND_CS_CFG_EXT]		= 0x04,
+	[BRCMNAND_CS_CFG]		= 0x08,
+	[BRCMNAND_CS_TIMING1]		= 0x0c,
+	[BRCMNAND_CS_TIMING2]		= 0x10,
+};
+
+/* Per chip-select offsets for pre v7.1, except CS0 on <= v5.0 */
+static const u8 brcmnand_cs_offsets[] = {
+	[BRCMNAND_CS_ACC_CONTROL]	= 0x00,
+	[BRCMNAND_CS_CFG_EXT]		= 0x04,
+	[BRCMNAND_CS_CFG]		= 0x04,
+	[BRCMNAND_CS_TIMING1]		= 0x08,
+	[BRCMNAND_CS_TIMING2]		= 0x0c,
+};
+
+/* Per chip-select offset for <= v5.0 on CS0 only */
+static const u8 brcmnand_cs_offsets_cs0[] = {
+	[BRCMNAND_CS_ACC_CONTROL]	= 0x00,
+	[BRCMNAND_CS_CFG_EXT]		= 0x08,
+	[BRCMNAND_CS_CFG]		= 0x08,
+	[BRCMNAND_CS_TIMING1]		= 0x10,
+	[BRCMNAND_CS_TIMING2]		= 0x14,
+};
+
+/* BRCMNAND_INTFC_STATUS */
+enum {
+	INTFC_FLASH_STATUS		= GENMASK(7, 0),
+
+	INTFC_ERASED			= BIT(27),
+	INTFC_OOB_VALID			= BIT(28),
+	INTFC_CACHE_VALID		= BIT(29),
+	INTFC_FLASH_READY		= BIT(30),
+	INTFC_CTLR_READY		= BIT(31),
+};
+
+static inline u32 nand_readreg(struct brcmnand_controller *ctrl, u32 offs)
+{
+	return brcmnand_readl(ctrl->nand_base + offs);
+}
+
+static inline void nand_writereg(struct brcmnand_controller *ctrl, u32 offs,
+				 u32 val)
+{
+	brcmnand_writel(val, ctrl->nand_base + offs);
+}
+
+static int brcmnand_revision_init(struct brcmnand_controller *ctrl)
+{
+	static const unsigned int block_sizes_v6[] = { 8, 16, 128, 256, 512, 1024, 2048, 0 };
+	static const unsigned int block_sizes_v4[] = { 16, 128, 8, 512, 256, 1024, 2048, 0 };
+	static const unsigned int page_sizes[] = { 512, 2048, 4096, 8192, 0 };
+
+	ctrl->nand_version = nand_readreg(ctrl, 0) & 0xffff;
+
+	/* Only support v4.0+? */
+	if (ctrl->nand_version < 0x0400) {
+		dev_err(ctrl->dev, "version %#x not supported\n",
+			ctrl->nand_version);
+		return -ENODEV;
+	}
+
+	/* Register offsets */
+	if (ctrl->nand_version >= 0x0600)
+		ctrl->reg_offsets = brcmnand_regs_v60;
+	else if (ctrl->nand_version >= 0x0500)
+		ctrl->reg_offsets = brcmnand_regs_v50;
+	else if (ctrl->nand_version >= 0x0400)
+		ctrl->reg_offsets = brcmnand_regs_v40;
+
+	/* Chip-select stride */
+	if (ctrl->nand_version >= 0x0701)
+		ctrl->reg_spacing = 0x14;
+	else
+		ctrl->reg_spacing = 0x10;
+
+	/* Per chip-select registers */
+	if (ctrl->nand_version >= 0x0701) {
+		ctrl->cs_offsets = brcmnand_cs_offsets_v71;
+	} else {
+		ctrl->cs_offsets = brcmnand_cs_offsets;
+
+		/* v5.0 and earlier has a different CS0 offset layout */
+		if (ctrl->nand_version <= 0x0500)
+			ctrl->cs0_offsets = brcmnand_cs_offsets_cs0;
+	}
+
+	/* Page / block sizes */
+	if (ctrl->nand_version >= 0x0701) {
+		/* >= v7.1 use nice power-of-2 values! */
+		ctrl->max_page_size = 16 * 1024;
+		ctrl->max_block_size = 2 * 1024 * 1024;
+	} else {
+		ctrl->page_sizes = page_sizes;
+		if (ctrl->nand_version >= 0x0600)
+			ctrl->block_sizes = block_sizes_v6;
+		else
+			ctrl->block_sizes = block_sizes_v4;
+
+		if (ctrl->nand_version < 0x0400) {
+			ctrl->max_page_size = 4096;
+			ctrl->max_block_size = 512 * 1024;
+		}
+	}
+
+	/* Maximum spare area sector size (per 512B) */
+	if (ctrl->nand_version >= 0x0600)
+		ctrl->max_oob = 64;
+	else if (ctrl->nand_version >= 0x0500)
+		ctrl->max_oob = 32;
+	else
+		ctrl->max_oob = 16;
+
+	/* v6.0 and newer (except v6.1) have prefetch support */
+	if (ctrl->nand_version >= 0x0600 && ctrl->nand_version != 0x0601)
+		ctrl->features |= BRCMNAND_HAS_PREFETCH;
+
+	/*
+	 * v6.x has cache mode, but it's implemented differently. Ignore it for
+	 * now.
+	 */
+	if (ctrl->nand_version >= 0x0700)
+		ctrl->features |= BRCMNAND_HAS_CACHE_MODE;
+
+	if (ctrl->nand_version >= 0x0500)
+		ctrl->features |= BRCMNAND_HAS_1K_SECTORS;
+
+	if (ctrl->nand_version >= 0x0700)
+		ctrl->features |= BRCMNAND_HAS_WP;
+	else if (of_property_read_bool(ctrl->dev->of_node, "brcm,nand-has-wp"))
+		ctrl->features |= BRCMNAND_HAS_WP;
+
+	return 0;
+}
+
+static inline u32 brcmnand_read_reg(struct brcmnand_controller *ctrl,
+		enum brcmnand_reg reg)
+{
+	u16 offs = ctrl->reg_offsets[reg];
+
+	if (offs)
+		return nand_readreg(ctrl, offs);
+	else
+		return 0;
+}
+
+static inline void brcmnand_write_reg(struct brcmnand_controller *ctrl,
+				      enum brcmnand_reg reg, u32 val)
+{
+	u16 offs = ctrl->reg_offsets[reg];
+
+	if (offs)
+		nand_writereg(ctrl, offs, val);
+}
+
+static inline void brcmnand_rmw_reg(struct brcmnand_controller *ctrl,
+				    enum brcmnand_reg reg, u32 mask, unsigned
+				    int shift, u32 val)
+{
+	u32 tmp = brcmnand_read_reg(ctrl, reg);
+
+	tmp &= ~mask;
+	tmp |= val << shift;
+	brcmnand_write_reg(ctrl, reg, tmp);
+}
+
+static inline u32 brcmnand_read_fc(struct brcmnand_controller *ctrl, int word)
+{
+	return __raw_readl(ctrl->nand_fc + word * 4);
+}
+
+static inline void brcmnand_write_fc(struct brcmnand_controller *ctrl,
+				     int word, u32 val)
+{
+	__raw_writel(val, ctrl->nand_fc + word * 4);
+}
+
+static inline u16 brcmnand_cs_offset(struct brcmnand_controller *ctrl, int cs,
+				     enum brcmnand_cs_reg reg)
+{
+	u16 offs_cs0 = ctrl->reg_offsets[BRCMNAND_CS0_BASE];
+	u16 offs_cs1 = ctrl->reg_offsets[BRCMNAND_CS1_BASE];
+	u8 cs_offs;
+
+	if (cs == 0 && ctrl->cs0_offsets)
+		cs_offs = ctrl->cs0_offsets[reg];
+	else
+		cs_offs = ctrl->cs_offsets[reg];
+
+	if (cs && offs_cs1)
+		return offs_cs1 + (cs - 1) * ctrl->reg_spacing + cs_offs;
+
+	return offs_cs0 + cs * ctrl->reg_spacing + cs_offs;
+}
+
+static inline u32 brcmnand_count_corrected(struct brcmnand_controller *ctrl)
+{
+	if (ctrl->nand_version < 0x0600)
+		return 1;
+	return brcmnand_read_reg(ctrl, BRCMNAND_CORR_COUNT);
+}
+
+static void brcmnand_wr_corr_thresh(struct brcmnand_host *host, u8 val)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	unsigned int shift = 0, bits;
+	enum brcmnand_reg reg = BRCMNAND_CORR_THRESHOLD;
+	int cs = host->cs;
+
+	if (ctrl->nand_version >= 0x0600)
+		bits = 6;
+	else if (ctrl->nand_version >= 0x0500)
+		bits = 5;
+	else
+		bits = 4;
+
+	if (ctrl->nand_version >= 0x0600) {
+		if (cs >= 5)
+			reg = BRCMNAND_CORR_THRESHOLD_EXT;
+		shift = (cs % 5) * bits;
+	}
+	brcmnand_rmw_reg(ctrl, reg, (bits - 1) << shift, shift, val);
+}
+
+static inline int brcmnand_cmd_shift(struct brcmnand_controller *ctrl)
+{
+	if (ctrl->nand_version < 0x0700)
+		return 24;
+	return 0;
+}
+
+/***********************************************************************
+ * NAND ACC CONTROL bitfield
+ *
+ * Some bits have remained constant throughout hardware revision, while
+ * others have shifted around.
+ ***********************************************************************/
+
+/* Constant for all versions (where supported) */
+enum {
+	/* See BRCMNAND_HAS_CACHE_MODE */
+	ACC_CONTROL_CACHE_MODE				= BIT(22),
+
+	/* See BRCMNAND_HAS_PREFETCH */
+	ACC_CONTROL_PREFETCH				= BIT(23),
+
+	ACC_CONTROL_PAGE_HIT				= BIT(24),
+	ACC_CONTROL_WR_PREEMPT				= BIT(25),
+	ACC_CONTROL_PARTIAL_PAGE			= BIT(26),
+	ACC_CONTROL_RD_ERASED				= BIT(27),
+	ACC_CONTROL_FAST_PGM_RDIN			= BIT(28),
+	ACC_CONTROL_WR_ECC				= BIT(30),
+	ACC_CONTROL_RD_ECC				= BIT(31),
+};
+
+static inline u32 brcmnand_spare_area_mask(struct brcmnand_controller *ctrl)
+{
+	if (ctrl->nand_version >= 0x0600)
+		return GENMASK(6, 0);
+	else
+		return GENMASK(5, 0);
+}
+
+#define NAND_ACC_CONTROL_ECC_SHIFT	16
+
+static inline u32 brcmnand_ecc_level_mask(struct brcmnand_controller *ctrl)
+{
+	u32 mask = (ctrl->nand_version >= 0x0600) ? 0x1f : 0x0f;
+
+	return mask << NAND_ACC_CONTROL_ECC_SHIFT;
+}
+
+static void brcmnand_set_ecc_enabled(struct brcmnand_host *host, int en)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	u16 offs = brcmnand_cs_offset(ctrl, host->cs, BRCMNAND_CS_ACC_CONTROL);
+	u32 acc_control = nand_readreg(ctrl, offs);
+	u32 ecc_flags = ACC_CONTROL_WR_ECC | ACC_CONTROL_RD_ECC;
+
+	if (en) {
+		acc_control |= ecc_flags; /* enable RD/WR ECC */
+		acc_control |= host->hwcfg.ecc_level
+			       << NAND_ACC_CONTROL_ECC_SHIFT;
+	} else {
+		acc_control &= ~ecc_flags; /* disable RD/WR ECC */
+		acc_control &= ~brcmnand_ecc_level_mask(ctrl);
+	}
+
+	nand_writereg(ctrl, offs, acc_control);
+}
+
+static inline int brcmnand_sector_1k_shift(struct brcmnand_controller *ctrl)
+{
+	if (ctrl->nand_version >= 0x0600)
+		return 7;
+	else if (ctrl->nand_version >= 0x0500)
+		return 6;
+	else
+		return -1;
+}
+
+static int brcmnand_get_sector_size_1k(struct brcmnand_host *host)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	int shift = brcmnand_sector_1k_shift(ctrl);
+	u16 acc_control_offs = brcmnand_cs_offset(ctrl, host->cs,
+						  BRCMNAND_CS_ACC_CONTROL);
+
+	if (shift < 0)
+		return 0;
+
+	return (nand_readreg(ctrl, acc_control_offs) >> shift) & 0x1;
+}
+
+static void brcmnand_set_sector_size_1k(struct brcmnand_host *host, int val)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	int shift = brcmnand_sector_1k_shift(ctrl);
+	u16 acc_control_offs = brcmnand_cs_offset(ctrl, host->cs,
+						  BRCMNAND_CS_ACC_CONTROL);
+	u32 tmp;
+
+	if (shift < 0)
+		return;
+
+	tmp = nand_readreg(ctrl, acc_control_offs);
+	tmp &= ~(1 << shift);
+	tmp |= (!!val) << shift;
+	nand_writereg(ctrl, acc_control_offs, tmp);
+}
+
+/***********************************************************************
+ * CS_NAND_SELECT
+ ***********************************************************************/
+
+enum {
+	CS_SELECT_NAND_WP			= BIT(29),
+	CS_SELECT_AUTO_DEVICE_ID_CFG		= BIT(30),
+};
+
+static inline void brcmnand_set_wp(struct brcmnand_controller *ctrl, bool en)
+{
+	u32 val = en ? CS_SELECT_NAND_WP : 0;
+
+	brcmnand_rmw_reg(ctrl, BRCMNAND_CS_SELECT, CS_SELECT_NAND_WP, 0, val);
+}
+
+/***********************************************************************
+ * Flash DMA
+ ***********************************************************************/
+
+enum flash_dma_reg {
+	FLASH_DMA_REVISION		= 0x00,
+	FLASH_DMA_FIRST_DESC		= 0x04,
+	FLASH_DMA_FIRST_DESC_EXT	= 0x08,
+	FLASH_DMA_CTRL			= 0x0c,
+	FLASH_DMA_MODE			= 0x10,
+	FLASH_DMA_STATUS		= 0x14,
+	FLASH_DMA_INTERRUPT_DESC	= 0x18,
+	FLASH_DMA_INTERRUPT_DESC_EXT	= 0x1c,
+	FLASH_DMA_ERROR_STATUS		= 0x20,
+	FLASH_DMA_CURRENT_DESC		= 0x24,
+	FLASH_DMA_CURRENT_DESC_EXT	= 0x28,
+};
+
+static inline bool has_flash_dma(struct brcmnand_controller *ctrl)
+{
+	return ctrl->flash_dma_base;
+}
+
+static inline bool flash_dma_buf_ok(const void *buf)
+{
+	return buf && !is_vmalloc_addr(buf) &&
+		likely(IS_ALIGNED((uintptr_t)buf, 4));
+}
+
+static inline void flash_dma_writel(struct brcmnand_controller *ctrl, u8 offs,
+				    u32 val)
+{
+	brcmnand_writel(val, ctrl->flash_dma_base + offs);
+}
+
+static inline u32 flash_dma_readl(struct brcmnand_controller *ctrl, u8 offs)
+{
+	return brcmnand_readl(ctrl->flash_dma_base + offs);
+}
+
+/* Low-level operation types: command, address, write, or read */
+enum brcmnand_llop_type {
+	LL_OP_CMD,
+	LL_OP_ADDR,
+	LL_OP_WR,
+	LL_OP_RD,
+};
+
+/***********************************************************************
+ * Internal support functions
+ ***********************************************************************/
+
+static inline bool is_hamming_ecc(struct brcmnand_cfg *cfg)
+{
+	return cfg->sector_size_1k == 0 && cfg->spare_area_size == 16 &&
+		cfg->ecc_level == 15;
+}
+
+/*
+ * Returns a nand_ecclayout strucutre for the given layout/configuration.
+ * Returns NULL on failure.
+ */
+static struct nand_ecclayout *brcmnand_create_layout(int ecc_level,
+						     struct brcmnand_host *host)
+{
+	struct brcmnand_cfg *cfg = &host->hwcfg;
+	int i, j;
+	struct nand_ecclayout *layout;
+	int req;
+	int sectors;
+	int sas;
+	int idx1, idx2;
+
+	layout = devm_kzalloc(&host->pdev->dev, sizeof(*layout), GFP_KERNEL);
+	if (!layout)
+		return NULL;
+
+	sectors = cfg->page_size / (512 << cfg->sector_size_1k);
+	sas = cfg->spare_area_size << cfg->sector_size_1k;
+
+	/* Hamming */
+	if (is_hamming_ecc(cfg)) {
+		for (i = 0, idx1 = 0, idx2 = 0; i < sectors; i++) {
+			/* First sector of each page may have BBI */
+			if (i == 0) {
+				layout->oobfree[idx2].offset = i * sas + 1;
+				/* Small-page NAND use byte 6 for BBI */
+				if (cfg->page_size == 512)
+					layout->oobfree[idx2].offset--;
+				layout->oobfree[idx2].length = 5;
+			} else {
+				layout->oobfree[idx2].offset = i * sas;
+				layout->oobfree[idx2].length = 6;
+			}
+			idx2++;
+			layout->eccpos[idx1++] = i * sas + 6;
+			layout->eccpos[idx1++] = i * sas + 7;
+			layout->eccpos[idx1++] = i * sas + 8;
+			layout->oobfree[idx2].offset = i * sas + 9;
+			layout->oobfree[idx2].length = 7;
+			idx2++;
+			/* Leave zero-terminated entry for OOBFREE */
+			if (idx1 >= MTD_MAX_ECCPOS_ENTRIES_LARGE ||
+				    idx2 >= MTD_MAX_OOBFREE_ENTRIES_LARGE - 1)
+				break;
+		}
+		goto out;
+	}
+
+	/*
+	 * CONTROLLER_VERSION:
+	 *   < v5.0: ECC_REQ = ceil(BCH_T * 13/8)
+	 *  >= v5.0: ECC_REQ = ceil(BCH_T * 14/8)
+	 * But we will just be conservative.
+	 */
+	req = DIV_ROUND_UP(ecc_level * 14, 8);
+	if (req >= sas) {
+		dev_err(&host->pdev->dev,
+			"error: ECC too large for OOB (ECC bytes %d, spare sector %d)\n",
+			req, sas);
+		return NULL;
+	}
+
+	layout->eccbytes = req * sectors;
+	for (i = 0, idx1 = 0, idx2 = 0; i < sectors; i++) {
+		for (j = sas - req; j < sas && idx1 <
+				MTD_MAX_ECCPOS_ENTRIES_LARGE; j++, idx1++)
+			layout->eccpos[idx1] = i * sas + j;
+
+		/* First sector of each page may have BBI */
+		if (i == 0) {
+			if (cfg->page_size == 512 && (sas - req >= 6)) {
+				/* Small-page NAND use byte 6 for BBI */
+				layout->oobfree[idx2].offset = 0;
+				layout->oobfree[idx2].length = 5;
+				idx2++;
+				if (sas - req > 6) {
+					layout->oobfree[idx2].offset = 6;
+					layout->oobfree[idx2].length =
+						sas - req - 6;
+					idx2++;
+				}
+			} else if (sas > req + 1) {
+				layout->oobfree[idx2].offset = i * sas + 1;
+				layout->oobfree[idx2].length = sas - req - 1;
+				idx2++;
+			}
+		} else if (sas > req) {
+			layout->oobfree[idx2].offset = i * sas;
+			layout->oobfree[idx2].length = sas - req;
+			idx2++;
+		}
+		/* Leave zero-terminated entry for OOBFREE */
+		if (idx1 >= MTD_MAX_ECCPOS_ENTRIES_LARGE ||
+				idx2 >= MTD_MAX_OOBFREE_ENTRIES_LARGE - 1)
+			break;
+	}
+out:
+	/* Sum available OOB */
+	for (i = 0; i < MTD_MAX_OOBFREE_ENTRIES_LARGE; i++)
+		layout->oobavail += layout->oobfree[i].length;
+	return layout;
+}
+
+static struct nand_ecclayout *brcmstb_choose_ecc_layout(
+		struct brcmnand_host *host)
+{
+	struct nand_ecclayout *layout;
+	struct brcmnand_cfg *p = &host->hwcfg;
+	unsigned int ecc_level = p->ecc_level;
+
+	if (p->sector_size_1k)
+		ecc_level <<= 1;
+
+	layout = brcmnand_create_layout(ecc_level, host);
+	if (!layout) {
+		dev_err(&host->pdev->dev,
+				"no proper ecc_layout for this NAND cfg\n");
+		return NULL;
+	}
+
+	return layout;
+}
+
+static void brcmnand_wp(struct mtd_info *mtd, int wp)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_controller *ctrl = host->ctrl;
+
+	if ((ctrl->features & BRCMNAND_HAS_WP) && wp_on == 1) {
+		static int old_wp = -1;
+
+		if (old_wp != wp) {
+			dev_dbg(ctrl->dev, "WP %s\n", wp ? "on" : "off");
+			old_wp = wp;
+		}
+		brcmnand_set_wp(ctrl, wp);
+	}
+}
+
+/* Helper functions for reading and writing OOB registers */
+static inline u8 oob_reg_read(struct brcmnand_controller *ctrl, u32 offs)
+{
+	u16 offset0, offset10, reg_offs;
+
+	offset0 = ctrl->reg_offsets[BRCMNAND_OOB_READ_BASE];
+	offset10 = ctrl->reg_offsets[BRCMNAND_OOB_READ_10_BASE];
+
+	if (offs >= ctrl->max_oob)
+		return 0x77;
+
+	if (offs >= 16 && offset10)
+		reg_offs = offset10 + ((offs - 0x10) & ~0x03);
+	else
+		reg_offs = offset0 + (offs & ~0x03);
+
+	return nand_readreg(ctrl, reg_offs) >> (24 - ((offs & 0x03) << 3));
+}
+
+static inline void oob_reg_write(struct brcmnand_controller *ctrl, u32 offs,
+				 u32 data)
+{
+	u16 offset0, offset10, reg_offs;
+
+	offset0 = ctrl->reg_offsets[BRCMNAND_OOB_WRITE_BASE];
+	offset10 = ctrl->reg_offsets[BRCMNAND_OOB_WRITE_10_BASE];
+
+	if (offs >= ctrl->max_oob)
+		return;
+
+	if (offs >= 16 && offset10)
+		reg_offs = offset10 + ((offs - 0x10) & ~0x03);
+	else
+		reg_offs = offset0 + (offs & ~0x03);
+
+	nand_writereg(ctrl, reg_offs, data);
+}
+
+/*
+ * read_oob_from_regs - read data from OOB registers
+ * @ctrl: NAND controller
+ * @i: sub-page sector index
+ * @oob: buffer to read to
+ * @sas: spare area sector size (i.e., OOB size per FLASH_CACHE)
+ * @sector_1k: 1 for 1KiB sectors, 0 for 512B, other values are illegal
+ */
+static int read_oob_from_regs(struct brcmnand_controller *ctrl, int i, u8 *oob,
+			      int sas, int sector_1k)
+{
+	int tbytes = sas << sector_1k;
+	int j;
+
+	/* Adjust OOB values for 1K sector size */
+	if (sector_1k && (i & 0x01))
+		tbytes = max(0, tbytes - (int)ctrl->max_oob);
+	tbytes = min_t(int, tbytes, ctrl->max_oob);
+
+	for (j = 0; j < tbytes; j++)
+		oob[j] = oob_reg_read(ctrl, j);
+	return tbytes;
+}
+
+/*
+ * write_oob_to_regs - write data to OOB registers
+ * @i: sub-page sector index
+ * @oob: buffer to write from
+ * @sas: spare area sector size (i.e., OOB size per FLASH_CACHE)
+ * @sector_1k: 1 for 1KiB sectors, 0 for 512B, other values are illegal
+ */
+static int write_oob_to_regs(struct brcmnand_controller *ctrl, int i,
+			     const u8 *oob, int sas, int sector_1k)
+{
+	int tbytes = sas << sector_1k;
+	int j;
+
+	/* Adjust OOB values for 1K sector size */
+	if (sector_1k && (i & 0x01))
+		tbytes = max(0, tbytes - (int)ctrl->max_oob);
+	tbytes = min_t(int, tbytes, ctrl->max_oob);
+
+	for (j = 0; j < tbytes; j += 4)
+		oob_reg_write(ctrl, j,
+				(oob[j + 0] << 24) |
+				(oob[j + 1] << 16) |
+				(oob[j + 2] <<  8) |
+				(oob[j + 3] <<  0));
+	return tbytes;
+}
+
+static irqreturn_t brcmnand_ctlrdy_irq(int irq, void *data)
+{
+	struct brcmnand_controller *ctrl = data;
+
+	/* Discard all NAND_CTLRDY interrupts during DMA */
+	if (ctrl->dma_pending)
+		return IRQ_HANDLED;
+
+	complete(&ctrl->done);
+	return IRQ_HANDLED;
+}
+
+/* Handle SoC-specific interrupt hardware */
+static irqreturn_t brcmnand_irq(int irq, void *data)
+{
+	struct brcmnand_controller *ctrl = data;
+
+	if (ctrl->soc->ctlrdy_ack(ctrl->soc))
+		return brcmnand_ctlrdy_irq(irq, data);
+
+	return IRQ_NONE;
+}
+
+static irqreturn_t brcmnand_dma_irq(int irq, void *data)
+{
+	struct brcmnand_controller *ctrl = data;
+
+	complete(&ctrl->dma_done);
+
+	return IRQ_HANDLED;
+}
+
+static void brcmnand_send_cmd(struct brcmnand_host *host, int cmd)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	u32 intfc;
+
+	dev_dbg(ctrl->dev, "send native cmd %d addr_lo 0x%x\n", cmd,
+		brcmnand_read_reg(ctrl, BRCMNAND_CMD_ADDRESS));
+	BUG_ON(ctrl->cmd_pending != 0);
+	ctrl->cmd_pending = cmd;
+
+	intfc = brcmnand_read_reg(ctrl, BRCMNAND_INTFC_STATUS);
+	BUG_ON(!(intfc & INTFC_CTLR_READY));
+
+	mb(); /* flush previous writes */
+	brcmnand_write_reg(ctrl, BRCMNAND_CMD_START,
+			   cmd << brcmnand_cmd_shift(ctrl));
+}
+
+/***********************************************************************
+ * NAND MTD API: read/program/erase
+ ***********************************************************************/
+
+static void brcmnand_cmd_ctrl(struct mtd_info *mtd, int dat,
+	unsigned int ctrl)
+{
+	/* intentionally left blank */
+}
+
+static int brcmnand_waitfunc(struct mtd_info *mtd, struct nand_chip *this)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_controller *ctrl = host->ctrl;
+	unsigned long timeo = msecs_to_jiffies(100);
+
+	dev_dbg(ctrl->dev, "wait on native cmd %d\n", ctrl->cmd_pending);
+	if (ctrl->cmd_pending &&
+			wait_for_completion_timeout(&ctrl->done, timeo) <= 0) {
+		u32 cmd = brcmnand_read_reg(ctrl, BRCMNAND_CMD_START)
+					>> brcmnand_cmd_shift(ctrl);
+
+		dev_err_ratelimited(ctrl->dev,
+			"timeout waiting for command %#02x\n", cmd);
+		dev_err_ratelimited(ctrl->dev, "intfc status %08x\n",
+			brcmnand_read_reg(ctrl, BRCMNAND_INTFC_STATUS));
+	}
+	ctrl->cmd_pending = 0;
+	return brcmnand_read_reg(ctrl, BRCMNAND_INTFC_STATUS) &
+				 INTFC_FLASH_STATUS;
+}
+
+enum {
+	LLOP_RE				= BIT(16),
+	LLOP_WE				= BIT(17),
+	LLOP_ALE			= BIT(18),
+	LLOP_CLE			= BIT(19),
+	LLOP_RETURN_IDLE		= BIT(31),
+
+	LLOP_DATA_MASK			= GENMASK(15, 0),
+};
+
+static int brcmnand_low_level_op(struct brcmnand_host *host,
+				 enum brcmnand_llop_type type, u32 data,
+				 bool last_op)
+{
+	struct mtd_info *mtd = &host->mtd;
+	struct nand_chip *chip = &host->chip;
+	struct brcmnand_controller *ctrl = host->ctrl;
+	u32 tmp;
+
+	tmp = data & LLOP_DATA_MASK;
+	switch (type) {
+	case LL_OP_CMD:
+		tmp |= LLOP_WE | LLOP_CLE;
+		break;
+	case LL_OP_ADDR:
+		/* WE | ALE */
+		tmp |= LLOP_WE | LLOP_ALE;
+		break;
+	case LL_OP_WR:
+		/* WE */
+		tmp |= LLOP_WE;
+		break;
+	case LL_OP_RD:
+		/* RE */
+		tmp |= LLOP_RE;
+		break;
+	}
+	if (last_op)
+		/* RETURN_IDLE */
+		tmp |= LLOP_RETURN_IDLE;
+
+	dev_dbg(ctrl->dev, "ll_op cmd %#x\n", tmp);
+
+	brcmnand_write_reg(ctrl, BRCMNAND_LL_OP, tmp);
+	(void)brcmnand_read_reg(ctrl, BRCMNAND_LL_OP);
+
+	brcmnand_send_cmd(host, CMD_LOW_LEVEL_OP);
+	return brcmnand_waitfunc(mtd, chip);
+}
+
+static void brcmnand_cmdfunc(struct mtd_info *mtd, unsigned command,
+			     int column, int page_addr)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_controller *ctrl = host->ctrl;
+	u64 addr = (u64)page_addr << chip->page_shift;
+	int native_cmd = 0;
+
+	if (command == NAND_CMD_READID || command == NAND_CMD_PARAM ||
+			command == NAND_CMD_RNDOUT)
+		addr = (u64)column;
+	/* Avoid propagating a negative, don't-care address */
+	else if (page_addr < 0)
+		addr = 0;
+
+	dev_dbg(ctrl->dev, "cmd 0x%x addr 0x%llx\n", command,
+		(unsigned long long)addr);
+
+	host->last_cmd = command;
+	host->last_byte = 0;
+	host->last_addr = addr;
+
+	switch (command) {
+	case NAND_CMD_RESET:
+		native_cmd = CMD_FLASH_RESET;
+		break;
+	case NAND_CMD_STATUS:
+		native_cmd = CMD_STATUS_READ;
+		break;
+	case NAND_CMD_READID:
+		native_cmd = CMD_DEVICE_ID_READ;
+		break;
+	case NAND_CMD_READOOB:
+		native_cmd = CMD_SPARE_AREA_READ;
+		break;
+	case NAND_CMD_ERASE1:
+		native_cmd = CMD_BLOCK_ERASE;
+		brcmnand_wp(mtd, 0);
+		break;
+	case NAND_CMD_PARAM:
+		native_cmd = CMD_PARAMETER_READ;
+		break;
+	case NAND_CMD_SET_FEATURES:
+	case NAND_CMD_GET_FEATURES:
+		brcmnand_low_level_op(host, LL_OP_CMD, command, false);
+		brcmnand_low_level_op(host, LL_OP_ADDR, column, false);
+		break;
+	case NAND_CMD_RNDOUT:
+		native_cmd = CMD_PARAMETER_CHANGE_COL;
+		addr &= ~((u64)(FC_BYTES - 1));
+		/*
+		 * HW quirk: PARAMETER_CHANGE_COL requires SECTOR_SIZE_1K=0
+		 * NB: hwcfg.sector_size_1k may not be initialized yet
+		 */
+		if (brcmnand_get_sector_size_1k(host)) {
+			host->hwcfg.sector_size_1k =
+				brcmnand_get_sector_size_1k(host);
+			brcmnand_set_sector_size_1k(host, 0);
+		}
+		break;
+	}
+
+	if (!native_cmd)
+		return;
+
+	brcmnand_write_reg(ctrl, BRCMNAND_CMD_EXT_ADDRESS,
+		(host->cs << 16) | ((addr >> 32) & 0xffff));
+	(void)brcmnand_read_reg(ctrl, BRCMNAND_CMD_EXT_ADDRESS);
+	brcmnand_write_reg(ctrl, BRCMNAND_CMD_ADDRESS, lower_32_bits(addr));
+	(void)brcmnand_read_reg(ctrl, BRCMNAND_CMD_ADDRESS);
+
+	brcmnand_send_cmd(host, native_cmd);
+	brcmnand_waitfunc(mtd, chip);
+
+	if (native_cmd == CMD_PARAMETER_READ ||
+			native_cmd == CMD_PARAMETER_CHANGE_COL) {
+		int i;
+
+		brcmnand_soc_data_bus_prepare(ctrl->soc);
+
+		/*
+		 * Must cache the FLASH_CACHE now, since changes in
+		 * SECTOR_SIZE_1K may invalidate it
+		 */
+		for (i = 0; i < FC_WORDS; i++)
+			ctrl->flash_cache[i] = brcmnand_read_fc(ctrl, i);
+
+		brcmnand_soc_data_bus_unprepare(ctrl->soc);
+
+		/* Cleanup from HW quirk: restore SECTOR_SIZE_1K */
+		if (host->hwcfg.sector_size_1k)
+			brcmnand_set_sector_size_1k(host,
+						    host->hwcfg.sector_size_1k);
+	}
+
+	/* Re-enable protection is necessary only after erase */
+	if (command == NAND_CMD_ERASE1)
+		brcmnand_wp(mtd, 1);
+}
+
+static uint8_t brcmnand_read_byte(struct mtd_info *mtd)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_controller *ctrl = host->ctrl;
+	uint8_t ret = 0;
+	int addr, offs;
+
+	switch (host->last_cmd) {
+	case NAND_CMD_READID:
+		if (host->last_byte < 4)
+			ret = brcmnand_read_reg(ctrl, BRCMNAND_ID) >>
+				(24 - (host->last_byte << 3));
+		else if (host->last_byte < 8)
+			ret = brcmnand_read_reg(ctrl, BRCMNAND_ID_EXT) >>
+				(56 - (host->last_byte << 3));
+		break;
+
+	case NAND_CMD_READOOB:
+		ret = oob_reg_read(ctrl, host->last_byte);
+		break;
+
+	case NAND_CMD_STATUS:
+		ret = brcmnand_read_reg(ctrl, BRCMNAND_INTFC_STATUS) &
+					INTFC_FLASH_STATUS;
+		if (wp_on) /* hide WP status */
+			ret |= NAND_STATUS_WP;
+		break;
+
+	case NAND_CMD_PARAM:
+	case NAND_CMD_RNDOUT:
+		addr = host->last_addr + host->last_byte;
+		offs = addr & (FC_BYTES - 1);
+
+		/* At FC_BYTES boundary, switch to next column */
+		if (host->last_byte > 0 && offs == 0)
+			chip->cmdfunc(mtd, NAND_CMD_RNDOUT, addr, -1);
+
+		ret = ctrl->flash_cache[offs >> 2] >>
+					(24 - ((offs & 0x03) << 3));
+		break;
+	case NAND_CMD_GET_FEATURES:
+		if (host->last_byte >= ONFI_SUBFEATURE_PARAM_LEN) {
+			ret = 0;
+		} else {
+			bool last = host->last_byte ==
+				ONFI_SUBFEATURE_PARAM_LEN - 1;
+			brcmnand_low_level_op(host, LL_OP_RD, 0, last);
+			ret = brcmnand_read_reg(ctrl, BRCMNAND_LL_RDATA) & 0xff;
+		}
+	}
+
+	dev_dbg(ctrl->dev, "read byte = 0x%02x\n", ret);
+	host->last_byte++;
+
+	return ret;
+}
+
+static void brcmnand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++, buf++)
+		*buf = brcmnand_read_byte(mtd);
+}
+
+static void brcmnand_write_buf(struct mtd_info *mtd, const uint8_t *buf,
+				   int len)
+{
+	int i;
+	struct nand_chip *chip = mtd->priv;
+	struct brcmnand_host *host = chip->priv;
+
+	switch (host->last_cmd) {
+	case NAND_CMD_SET_FEATURES:
+		for (i = 0; i < len; i++)
+			brcmnand_low_level_op(host, LL_OP_WR, buf[i],
+						  (i + 1) == len);
+		break;
+	default:
+		BUG();
+		break;
+	}
+}
+
+/**
+ * Construct a FLASH_DMA descriptor as part of a linked list. You must know the
+ * following ahead of time:
+ *  - Is this descriptor the beginning or end of a linked list?
+ *  - What is the (DMA) address of the next descriptor in the linked list?
+ */
+static int brcmnand_fill_dma_desc(struct brcmnand_host *host,
+				  struct brcm_nand_dma_desc *desc, u64 addr,
+				  dma_addr_t buf, u32 len, u8 dma_cmd,
+				  bool begin, bool end,
+				  dma_addr_t next_desc)
+{
+	memset(desc, 0, sizeof(*desc));
+	/* Descriptors are written in native byte order (wordwise) */
+	desc->next_desc = lower_32_bits(next_desc);
+	desc->next_desc_ext = upper_32_bits(next_desc);
+	desc->cmd_irq = (dma_cmd << 24) |
+		(end ? (0x03 << 8) : 0) | /* IRQ | STOP */
+		(!!begin) | ((!!end) << 1); /* head, tail */
+#ifdef CONFIG_CPU_BIG_ENDIAN
+	desc->cmd_irq |= 0x01 << 12;
+#endif
+	desc->dram_addr = lower_32_bits(buf);
+	desc->dram_addr_ext = upper_32_bits(buf);
+	desc->tfr_len = len;
+	desc->total_len = len;
+	desc->flash_addr = lower_32_bits(addr);
+	desc->flash_addr_ext = upper_32_bits(addr);
+	desc->cs = host->cs;
+	desc->status_valid = 0x01;
+	return 0;
+}
+
+/**
+ * Kick the FLASH_DMA engine, with a given DMA descriptor
+ */
+static void brcmnand_dma_run(struct brcmnand_host *host, dma_addr_t desc)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	unsigned long timeo = msecs_to_jiffies(100);
+
+	flash_dma_writel(ctrl, FLASH_DMA_FIRST_DESC, lower_32_bits(desc));
+	(void)flash_dma_readl(ctrl, FLASH_DMA_FIRST_DESC);
+	flash_dma_writel(ctrl, FLASH_DMA_FIRST_DESC_EXT, upper_32_bits(desc));
+	(void)flash_dma_readl(ctrl, FLASH_DMA_FIRST_DESC_EXT);
+
+	/* Start FLASH_DMA engine */
+	ctrl->dma_pending = true;
+	mb(); /* flush previous writes */
+	flash_dma_writel(ctrl, FLASH_DMA_CTRL, 0x03); /* wake | run */
+
+	if (wait_for_completion_timeout(&ctrl->dma_done, timeo) <= 0) {
+		dev_err(ctrl->dev,
+				"timeout waiting for DMA; status %#x, error status %#x\n",
+				flash_dma_readl(ctrl, FLASH_DMA_STATUS),
+				flash_dma_readl(ctrl, FLASH_DMA_ERROR_STATUS));
+	}
+	ctrl->dma_pending = false;
+	flash_dma_writel(ctrl, FLASH_DMA_CTRL, 0); /* force stop */
+}
+
+static int brcmnand_dma_trans(struct brcmnand_host *host, u64 addr, u32 *buf,
+			      u32 len, u8 dma_cmd)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	dma_addr_t buf_pa;
+	int dir = dma_cmd == CMD_PAGE_READ ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+
+	buf_pa = dma_map_single(ctrl->dev, buf, len, dir);
+	if (dma_mapping_error(ctrl->dev, buf_pa)) {
+		dev_err(ctrl->dev, "unable to map buffer for DMA\n");
+		return -ENOMEM;
+	}
+
+	brcmnand_fill_dma_desc(host, ctrl->dma_desc, addr, buf_pa, len,
+				   dma_cmd, true, true, 0);
+
+	brcmnand_dma_run(host, ctrl->dma_pa);
+
+	dma_unmap_single(ctrl->dev, buf_pa, len, dir);
+
+	if (ctrl->dma_desc->status_valid & FLASH_DMA_ECC_ERROR)
+		return -EBADMSG;
+	else if (ctrl->dma_desc->status_valid & FLASH_DMA_CORR_ERROR)
+		return -EUCLEAN;
+
+	return 0;
+}
+
+/*
+ * Assumes proper CS is already set
+ */
+static int brcmnand_read_by_pio(struct mtd_info *mtd, struct nand_chip *chip,
+				u64 addr, unsigned int trans, u32 *buf,
+				u8 *oob, u64 *err_addr)
+{
+	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_controller *ctrl = host->ctrl;
+	int i, j, ret = 0;
+
+	/* Clear error addresses */
+	brcmnand_write_reg(ctrl, BRCMNAND_UNCORR_ADDR, 0);
+	brcmnand_write_reg(ctrl, BRCMNAND_CORR_ADDR, 0);
+
+	brcmnand_write_reg(ctrl, BRCMNAND_CMD_EXT_ADDRESS,
+			(host->cs << 16) | ((addr >> 32) & 0xffff));
+	(void)brcmnand_read_reg(ctrl, BRCMNAND_CMD_EXT_ADDRESS);
+
+	for (i = 0; i < trans; i++, addr += FC_BYTES) {
+		brcmnand_write_reg(ctrl, BRCMNAND_CMD_ADDRESS,
+				   lower_32_bits(addr));
+		(void)brcmnand_read_reg(ctrl, BRCMNAND_CMD_ADDRESS);
+		/* SPARE_AREA_READ does not use ECC, so just use PAGE_READ */
+		brcmnand_send_cmd(host, CMD_PAGE_READ);
+		brcmnand_waitfunc(mtd, chip);
+
+		if (likely(buf)) {
+			brcmnand_soc_data_bus_prepare(ctrl->soc);
+
+			for (j = 0; j < FC_WORDS; j++, buf++)
+				*buf = brcmnand_read_fc(ctrl, j);
+
+			brcmnand_soc_data_bus_unprepare(ctrl->soc);
+		}
+
+		if (oob)
+			oob += read_oob_from_regs(ctrl, i, oob,
+					mtd->oobsize / trans,
+					host->hwcfg.sector_size_1k);
+
+		if (!ret) {
+			*err_addr = brcmnand_read_reg(ctrl,
+					BRCMNAND_UNCORR_ADDR) |
+				((u64)(brcmnand_read_reg(ctrl,
+						BRCMNAND_UNCORR_EXT_ADDR)
+					& 0xffff) << 32);
+			if (*err_addr)
+				ret = -EBADMSG;
+		}
+
+		if (!ret) {
+			*err_addr = brcmnand_read_reg(ctrl,
+					BRCMNAND_CORR_ADDR) |
+				((u64)(brcmnand_read_reg(ctrl,
+						BRCMNAND_CORR_EXT_ADDR)
+					& 0xffff) << 32);
+			if (*err_addr)
+				ret = -EUCLEAN;
+		}
+	}
+
+	return ret;
+}
+
+static int brcmnand_read(struct mtd_info *mtd, struct nand_chip *chip,
+			 u64 addr, unsigned int trans, u32 *buf, u8 *oob)
+{
+	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_controller *ctrl = host->ctrl;
+	u64 err_addr = 0;
+	int err;
+
+	dev_dbg(ctrl->dev, "read %llx -> %p\n", (unsigned long long)addr, buf);
+
+	brcmnand_write_reg(ctrl, BRCMNAND_UNCORR_COUNT, 0);
+
+	if (has_flash_dma(ctrl) && !oob && flash_dma_buf_ok(buf)) {
+		err = brcmnand_dma_trans(host, addr, buf, trans * FC_BYTES,
+					     CMD_PAGE_READ);
+		if (err) {
+			if (mtd_is_bitflip_or_eccerr(err))
+				err_addr = addr;
+			else
+				return -EIO;
+		}
+	} else {
+		if (oob)
+			memset(oob, 0x99, mtd->oobsize);
+
+		err = brcmnand_read_by_pio(mtd, chip, addr, trans, buf,
+					       oob, &err_addr);
+	}
+
+	if (mtd_is_eccerr(err)) {
+		dev_dbg(ctrl->dev, "uncorrectable error at 0x%llx\n",
+			(unsigned long long)err_addr);
+		mtd->ecc_stats.failed++;
+		/* NAND layer expects zero on ECC errors */
+		return 0;
+	}
+
+	if (mtd_is_bitflip(err)) {
+		unsigned int corrected = brcmnand_count_corrected(ctrl);
+
+		dev_dbg(ctrl->dev, "corrected error at 0x%llx\n",
+			(unsigned long long)err_addr);
+		mtd->ecc_stats.corrected += corrected;
+		/* Always exceed the software-imposed threshold */
+		return max(mtd->bitflip_threshold, corrected);
+	}
+
+	return 0;
+}
+
+static int brcmnand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
+			      uint8_t *buf, int oob_required, int page)
+{
+	struct brcmnand_host *host = chip->priv;
+	u8 *oob = oob_required ? (u8 *)chip->oob_poi : NULL;
+
+	return brcmnand_read(mtd, chip, host->last_addr,
+			mtd->writesize >> FC_SHIFT, (u32 *)buf, oob);
+}
+
+static int brcmnand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+				  uint8_t *buf, int oob_required, int page)
+{
+	struct brcmnand_host *host = chip->priv;
+	u8 *oob = oob_required ? (u8 *)chip->oob_poi : NULL;
+	int ret;
+
+	brcmnand_set_ecc_enabled(host, 0);
+	ret = brcmnand_read(mtd, chip, host->last_addr,
+			mtd->writesize >> FC_SHIFT, (u32 *)buf, oob);
+	brcmnand_set_ecc_enabled(host, 1);
+	return ret;
+}
+
+static int brcmnand_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
+			     int page)
+{
+	return brcmnand_read(mtd, chip, (u64)page << chip->page_shift,
+			mtd->writesize >> FC_SHIFT,
+			NULL, (u8 *)chip->oob_poi);
+}
+
+static int brcmnand_read_oob_raw(struct mtd_info *mtd, struct nand_chip *chip,
+				 int page)
+{
+	struct brcmnand_host *host = chip->priv;
+
+	brcmnand_set_ecc_enabled(host, 0);
+	brcmnand_read(mtd, chip, (u64)page << chip->page_shift,
+		mtd->writesize >> FC_SHIFT,
+		NULL, (u8 *)chip->oob_poi);
+	brcmnand_set_ecc_enabled(host, 1);
+	return 0;
+}
+
+static int brcmnand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip,
+				 uint32_t data_offs, uint32_t readlen,
+				 uint8_t *bufpoi, int page)
+{
+	struct brcmnand_host *host = chip->priv;
+
+	return brcmnand_read(mtd, chip, host->last_addr + data_offs,
+			readlen >> FC_SHIFT, (u32 *)bufpoi, NULL);
+}
+
+static int brcmnand_write(struct mtd_info *mtd, struct nand_chip *chip,
+			  u64 addr, const u32 *buf, u8 *oob)
+{
+	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_controller *ctrl = host->ctrl;
+	unsigned int i, j, trans = mtd->writesize >> FC_SHIFT;
+	int status, ret = 0;
+
+	dev_dbg(ctrl->dev, "write %llx <- %p\n", (unsigned long long)addr, buf);
+
+	if (unlikely((u32)buf & 0x03)) {
+		dev_warn(ctrl->dev, "unaligned buffer: %p\n", buf);
+		buf = (u32 *)((u32)buf & ~0x03);
+	}
+
+	brcmnand_wp(mtd, 0);
+
+	for (i = 0; i < ctrl->max_oob; i += 4)
+		oob_reg_write(ctrl, i, 0xffffffff);
+
+	if (has_flash_dma(ctrl) && !oob && flash_dma_buf_ok(buf)) {
+		if (brcmnand_dma_trans(host, addr, (u32 *)buf,
+					mtd->writesize, CMD_PROGRAM_PAGE))
+			ret = -EIO;
+		goto out;
+	}
+
+	brcmnand_write_reg(ctrl, BRCMNAND_CMD_EXT_ADDRESS,
+			(host->cs << 16) | ((addr >> 32) & 0xffff));
+	(void)brcmnand_read_reg(ctrl, BRCMNAND_CMD_EXT_ADDRESS);
+
+	for (i = 0; i < trans; i++, addr += FC_BYTES) {
+		/* full address MUST be set before populating FC */
+		brcmnand_write_reg(ctrl, BRCMNAND_CMD_ADDRESS,
+				   lower_32_bits(addr));
+		(void)brcmnand_read_reg(ctrl, BRCMNAND_CMD_ADDRESS);
+
+		if (buf) {
+			brcmnand_soc_data_bus_prepare(ctrl->soc);
+
+			for (j = 0; j < FC_WORDS; j++, buf++)
+				brcmnand_write_fc(ctrl, j, *buf);
+
+			brcmnand_soc_data_bus_unprepare(ctrl->soc);
+		} else if (oob) {
+			for (j = 0; j < FC_WORDS; j++)
+				brcmnand_write_fc(ctrl, j, 0xffffffff);
+		}
+
+		if (oob) {
+			oob += write_oob_to_regs(ctrl, i, oob,
+					mtd->oobsize / trans,
+					host->hwcfg.sector_size_1k);
+		}
+
+		/* we cannot use SPARE_AREA_PROGRAM when PARTIAL_PAGE_EN=0 */
+		brcmnand_send_cmd(host, CMD_PROGRAM_PAGE);
+		status = brcmnand_waitfunc(mtd, chip);
+
+		if (status & NAND_STATUS_FAIL) {
+			dev_info(ctrl->dev, "program failed at %llx\n",
+				(unsigned long long)addr);
+			ret = -EIO;
+			goto out;
+		}
+	}
+out:
+	brcmnand_wp(mtd, 1);
+	return ret;
+}
+
+static int brcmnand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
+			       const uint8_t *buf, int oob_required)
+{
+	struct brcmnand_host *host = chip->priv;
+	void *oob = oob_required ? chip->oob_poi : NULL;
+
+	brcmnand_write(mtd, chip, host->last_addr, (const u32 *)buf, oob);
+	return 0;
+}
+
+static int brcmnand_write_page_raw(struct mtd_info *mtd,
+				   struct nand_chip *chip, const uint8_t *buf,
+				   int oob_required)
+{
+	struct brcmnand_host *host = chip->priv;
+	void *oob = oob_required ? chip->oob_poi : NULL;
+
+	brcmnand_set_ecc_enabled(host, 0);
+	brcmnand_write(mtd, chip, host->last_addr, (const u32 *)buf, oob);
+	brcmnand_set_ecc_enabled(host, 1);
+	return 0;
+}
+
+static int brcmnand_write_oob(struct mtd_info *mtd, struct nand_chip *chip,
+				  int page)
+{
+	return brcmnand_write(mtd, chip, (u64)page << chip->page_shift,
+				  NULL, chip->oob_poi);
+}
+
+static int brcmnand_write_oob_raw(struct mtd_info *mtd, struct nand_chip *chip,
+				  int page)
+{
+	struct brcmnand_host *host = chip->priv;
+	int ret;
+
+	brcmnand_set_ecc_enabled(host, 0);
+	ret = brcmnand_write(mtd, chip, (u64)page << chip->page_shift, NULL,
+				 (u8 *)chip->oob_poi);
+	brcmnand_set_ecc_enabled(host, 1);
+
+	return ret;
+}
+
+/***********************************************************************
+ * Per-CS setup (1 NAND device)
+ ***********************************************************************/
+
+static int brcmnand_set_cfg(struct brcmnand_host *host,
+			    struct brcmnand_cfg *cfg)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	struct nand_chip *chip = &host->chip;
+	u16 cfg_offs = brcmnand_cs_offset(ctrl, host->cs, BRCMNAND_CS_CFG);
+	u16 cfg_ext_offs = brcmnand_cs_offset(ctrl, host->cs,
+			BRCMNAND_CS_CFG_EXT);
+	u16 acc_control_offs = brcmnand_cs_offset(ctrl, host->cs,
+			BRCMNAND_CS_ACC_CONTROL);
+	u8 block_size = 0, page_size = 0, device_size = 0;
+	u32 tmp;
+
+	if (ctrl->block_sizes) {
+		int i, found;
+
+		for (i = 0, found = 0; ctrl->block_sizes[i]; i++)
+			if (ctrl->block_sizes[i] * 1024 == cfg->block_size) {
+				block_size = i;
+				found = 1;
+			}
+		if (!found) {
+			dev_warn(ctrl->dev, "invalid block size %u\n",
+					cfg->block_size);
+			return -EINVAL;
+		}
+	} else {
+		block_size = ffs(cfg->block_size) - ffs(BRCMNAND_MIN_BLOCKSIZE);
+	}
+
+	if (cfg->block_size < BRCMNAND_MIN_BLOCKSIZE || (ctrl->max_block_size &&
+				cfg->block_size > ctrl->max_block_size)) {
+		dev_warn(ctrl->dev, "invalid block size %u\n",
+				cfg->block_size);
+		block_size = 0;
+	}
+
+	if (ctrl->page_sizes) {
+		int i, found;
+
+		for (i = 0, found = 0; ctrl->page_sizes[i]; i++)
+			if (ctrl->page_sizes[i] == cfg->page_size) {
+				page_size = i;
+				found = 1;
+			}
+		if (!found) {
+			dev_warn(ctrl->dev, "invalid page size %u\n",
+					cfg->page_size);
+			return -EINVAL;
+		}
+	} else {
+		page_size = ffs(cfg->page_size) - ffs(BRCMNAND_MIN_PAGESIZE);
+	}
+
+	if (cfg->page_size < BRCMNAND_MIN_PAGESIZE || (ctrl->max_page_size &&
+				cfg->page_size > ctrl->max_page_size)) {
+		dev_warn(ctrl->dev, "invalid page size %u\n", cfg->page_size);
+		return -EINVAL;
+	}
+
+	if (fls64(cfg->device_size) < fls64(BRCMNAND_MIN_DEVSIZE)) {
+		dev_warn(ctrl->dev, "invalid device size 0x%llx\n",
+			(unsigned long long)cfg->device_size);
+		return -EINVAL;
+	}
+	device_size = fls64(cfg->device_size) - fls64(BRCMNAND_MIN_DEVSIZE);
+
+	tmp = (cfg->blk_adr_bytes << 8) |
+		(cfg->col_adr_bytes << 12) |
+		(cfg->ful_adr_bytes << 16) |
+		(!!(cfg->device_width == 16) << 23) |
+		(device_size << 24);
+	if (cfg_offs == cfg_ext_offs) {
+		tmp |= (page_size << 20) | (block_size << 28);
+		nand_writereg(ctrl, cfg_offs, tmp);
+	} else {
+		nand_writereg(ctrl, cfg_offs, tmp);
+		tmp = page_size | (block_size << 4);
+		nand_writereg(ctrl, cfg_ext_offs, tmp);
+	}
+
+	tmp = nand_readreg(ctrl, acc_control_offs);
+	tmp &= ~brcmnand_ecc_level_mask(ctrl);
+	tmp |= cfg->ecc_level << NAND_ACC_CONTROL_ECC_SHIFT;
+	tmp &= ~brcmnand_spare_area_mask(ctrl);
+	tmp |= cfg->spare_area_size;
+	nand_writereg(ctrl, acc_control_offs, tmp);
+
+	brcmnand_set_sector_size_1k(host, cfg->sector_size_1k);
+
+	/* threshold = ceil(BCH-level * 0.75) */
+	brcmnand_wr_corr_thresh(host, DIV_ROUND_UP(chip->ecc.strength * 3, 4));
+
+	return 0;
+}
+
+static void brcmnand_print_cfg(char *buf, struct brcmnand_cfg *cfg)
+{
+	buf += sprintf(buf,
+		"%lluMiB total, %uKiB blocks, %u%s pages, %uB OOB, %u-bit",
+		(unsigned long long)cfg->device_size >> 20,
+		cfg->block_size >> 10,
+		cfg->page_size >= 1024 ? cfg->page_size >> 10 : cfg->page_size,
+		cfg->page_size >= 1024 ? "KiB" : "B",
+		cfg->spare_area_size, cfg->device_width);
+
+	/* Account for Hamming ECC and for BCH 512B vs 1KiB sectors */
+	if (is_hamming_ecc(cfg))
+		sprintf(buf, ", Hamming ECC");
+	else if (cfg->sector_size_1k)
+		sprintf(buf, ", BCH-%u (1KiB sector)", cfg->ecc_level << 1);
+	else
+		sprintf(buf, ", BCH-%u", cfg->ecc_level);
+}
+
+/*
+ * Minimum number of bytes to address a page. Calculated as:
+ *     roundup(log2(size / page-size) / 8)
+ *
+ * NB: the following does not "round up" for non-power-of-2 'size'; but this is
+ *     OK because many other things will break if 'size' is irregular...
+ */
+static inline int get_blk_adr_bytes(u64 size, u32 writesize)
+{
+	return ALIGN(ilog2(size) - ilog2(writesize), 8) >> 3;
+}
+
+static int brcmnand_setup_dev(struct brcmnand_host *host)
+{
+	struct mtd_info *mtd = &host->mtd;
+	struct nand_chip *chip = &host->chip;
+	struct brcmnand_controller *ctrl = host->ctrl;
+	struct brcmnand_cfg *cfg = &host->hwcfg;
+	char msg[128];
+	u32 offs, tmp, oob_sector;
+	int ret;
+
+	memset(cfg, 0, sizeof(*cfg));
+
+	ret = of_property_read_u32(chip->dn, "brcm,nand-oob-sector-size",
+				   &oob_sector);
+	if (ret) {
+		/* Use detected size */
+		cfg->spare_area_size = mtd->oobsize /
+					(mtd->writesize >> FC_SHIFT);
+	} else {
+		cfg->spare_area_size = oob_sector;
+	}
+	if (cfg->spare_area_size > ctrl->max_oob)
+		cfg->spare_area_size = ctrl->max_oob;
+	/*
+	 * Set oobsize to be consistent with controller's spare_area_size, as
+	 * the rest is inaccessible.
+	 */
+	mtd->oobsize = cfg->spare_area_size * (mtd->writesize >> FC_SHIFT);
+
+	cfg->device_size = mtd->size;
+	cfg->block_size = mtd->erasesize;
+	cfg->page_size = mtd->writesize;
+	cfg->device_width = (chip->options & NAND_BUSWIDTH_16) ? 16 : 8;
+	cfg->col_adr_bytes = 2;
+	cfg->blk_adr_bytes = get_blk_adr_bytes(mtd->size, mtd->writesize);
+
+	switch (chip->ecc.size) {
+	case 512:
+		if (chip->ecc.strength == 1) /* Hamming */
+			cfg->ecc_level = 15;
+		else
+			cfg->ecc_level = chip->ecc.strength;
+		cfg->sector_size_1k = 0;
+		break;
+	case 1024:
+		if (!(ctrl->features & BRCMNAND_HAS_1K_SECTORS)) {
+			dev_err(ctrl->dev, "1KB sectors not supported\n");
+			return -EINVAL;
+		}
+		if (chip->ecc.strength & 0x1) {
+			dev_err(ctrl->dev,
+				"odd ECC not supported with 1KB sectors\n");
+			return -EINVAL;
+		}
+
+		cfg->ecc_level = chip->ecc.strength >> 1;
+		cfg->sector_size_1k = 1;
+		break;
+	default:
+		dev_err(ctrl->dev, "unsupported ECC size: %d\n",
+			chip->ecc.size);
+		return -EINVAL;
+	}
+
+	cfg->ful_adr_bytes = cfg->blk_adr_bytes;
+	if (mtd->writesize > 512)
+		cfg->ful_adr_bytes += cfg->col_adr_bytes;
+	else
+		cfg->ful_adr_bytes += 1;
+
+	ret = brcmnand_set_cfg(host, cfg);
+	if (ret)
+		return ret;
+
+	brcmnand_set_ecc_enabled(host, 1);
+
+	brcmnand_print_cfg(msg, cfg);
+	dev_info(ctrl->dev, "detected %s\n", msg);
+
+	/* Configure ACC_CONTROL */
+	offs = brcmnand_cs_offset(ctrl, host->cs, BRCMNAND_CS_ACC_CONTROL);
+	tmp = nand_readreg(ctrl, offs);
+	tmp &= ~ACC_CONTROL_PARTIAL_PAGE;
+	tmp &= ~ACC_CONTROL_RD_ERASED;
+	tmp &= ~ACC_CONTROL_FAST_PGM_RDIN;
+	if (ctrl->features & BRCMNAND_HAS_PREFETCH) {
+		/*
+		 * FIXME: Flash DMA + prefetch may see spurious erased-page ECC
+		 * errors
+		 */
+		if (has_flash_dma(ctrl))
+			tmp &= ~ACC_CONTROL_PREFETCH;
+		else
+			tmp |= ACC_CONTROL_PREFETCH;
+	}
+	nand_writereg(ctrl, offs, tmp);
+
+	return 0;
+}
+
+static int brcmnand_init_cs(struct brcmnand_host *host)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	struct device_node *dn = host->of_node;
+	struct platform_device *pdev = host->pdev;
+	struct mtd_info *mtd;
+	struct nand_chip *chip;
+	int ret;
+	struct mtd_part_parser_data ppdata = { .of_node = dn };
+
+	ret = of_property_read_u32(dn, "reg", &host->cs);
+	if (ret) {
+		dev_err(&pdev->dev, "can't get chip-select\n");
+		return -ENXIO;
+	}
+
+	mtd = &host->mtd;
+	chip = &host->chip;
+
+	chip->dn = dn;
+	chip->priv = host;
+	mtd->priv = chip;
+	mtd->name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "brcmnand.%d",
+				   host->cs);
+	mtd->owner = THIS_MODULE;
+	mtd->dev.parent = &pdev->dev;
+
+	chip->IO_ADDR_R = (void __iomem *)0xdeadbeef;
+	chip->IO_ADDR_W = (void __iomem *)0xdeadbeef;
+
+	chip->cmd_ctrl = brcmnand_cmd_ctrl;
+	chip->cmdfunc = brcmnand_cmdfunc;
+	chip->waitfunc = brcmnand_waitfunc;
+	chip->read_byte = brcmnand_read_byte;
+	chip->read_buf = brcmnand_read_buf;
+	chip->write_buf = brcmnand_write_buf;
+
+	chip->ecc.mode = NAND_ECC_HW;
+	chip->ecc.read_page = brcmnand_read_page;
+	chip->ecc.read_subpage = brcmnand_read_subpage;
+	chip->ecc.write_page = brcmnand_write_page;
+	chip->ecc.read_page_raw = brcmnand_read_page_raw;
+	chip->ecc.write_page_raw = brcmnand_write_page_raw;
+	chip->ecc.write_oob_raw = brcmnand_write_oob_raw;
+	chip->ecc.read_oob_raw = brcmnand_read_oob_raw;
+	chip->ecc.read_oob = brcmnand_read_oob;
+	chip->ecc.write_oob = brcmnand_write_oob;
+
+	chip->controller = &ctrl->controller;
+
+	if (nand_scan_ident(mtd, 1, NULL))
+		return -ENXIO;
+
+	chip->options |= NAND_NO_SUBPAGE_WRITE;
+	/*
+	 * Avoid (for instance) kmap()'d buffers from JFFS2, which we can't DMA
+	 * to/from, and have nand_base pass us a bounce buffer instead, as
+	 * needed.
+	 */
+	chip->options |= NAND_USE_BOUNCE_BUFFER;
+
+	if (of_get_nand_on_flash_bbt(dn))
+		chip->bbt_options |= NAND_BBT_USE_FLASH | NAND_BBT_NO_OOB;
+
+	if (brcmnand_setup_dev(host))
+		return -ENXIO;
+
+	chip->ecc.size = host->hwcfg.sector_size_1k ? 1024 : 512;
+	/* only use our internal HW threshold */
+	mtd->bitflip_threshold = 1;
+
+	chip->ecc.layout = brcmstb_choose_ecc_layout(host);
+	if (!chip->ecc.layout)
+		return -ENXIO;
+
+	if (nand_scan_tail(mtd))
+		return -ENXIO;
+
+	return mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+}
+
+static void brcmnand_save_restore_cs_config(struct brcmnand_host *host,
+					    int restore)
+{
+	struct brcmnand_controller *ctrl = host->ctrl;
+	u16 cfg_offs = brcmnand_cs_offset(ctrl, host->cs, BRCMNAND_CS_CFG);
+	u16 cfg_ext_offs = brcmnand_cs_offset(ctrl, host->cs,
+			BRCMNAND_CS_CFG_EXT);
+	u16 acc_control_offs = brcmnand_cs_offset(ctrl, host->cs,
+			BRCMNAND_CS_ACC_CONTROL);
+	u16 t1_offs = brcmnand_cs_offset(ctrl, host->cs, BRCMNAND_CS_TIMING1);
+	u16 t2_offs = brcmnand_cs_offset(ctrl, host->cs, BRCMNAND_CS_TIMING2);
+
+	if (restore) {
+		nand_writereg(ctrl, cfg_offs, host->hwcfg.config);
+		if (cfg_offs != cfg_ext_offs)
+			nand_writereg(ctrl, cfg_ext_offs,
+				      host->hwcfg.config_ext);
+		nand_writereg(ctrl, acc_control_offs, host->hwcfg.acc_control);
+		nand_writereg(ctrl, t1_offs, host->hwcfg.timing_1);
+		nand_writereg(ctrl, t2_offs, host->hwcfg.timing_2);
+	} else {
+		host->hwcfg.config = nand_readreg(ctrl, cfg_offs);
+		if (cfg_offs != cfg_ext_offs)
+			host->hwcfg.config_ext =
+				nand_readreg(ctrl, cfg_ext_offs);
+		host->hwcfg.acc_control = nand_readreg(ctrl, acc_control_offs);
+		host->hwcfg.timing_1 = nand_readreg(ctrl, t1_offs);
+		host->hwcfg.timing_2 = nand_readreg(ctrl, t2_offs);
+	}
+}
+
+static int brcmnand_suspend(struct device *dev)
+{
+	struct brcmnand_controller *ctrl = dev_get_drvdata(dev);
+	struct brcmnand_host *host;
+
+	list_for_each_entry(host, &ctrl->host_list, node)
+		brcmnand_save_restore_cs_config(host, 0);
+
+	ctrl->nand_cs_nand_select = brcmnand_read_reg(ctrl, BRCMNAND_CS_SELECT);
+	ctrl->nand_cs_nand_xor = brcmnand_read_reg(ctrl, BRCMNAND_CS_XOR);
+	ctrl->corr_stat_threshold =
+		brcmnand_read_reg(ctrl, BRCMNAND_CORR_THRESHOLD);
+
+	if (has_flash_dma(ctrl))
+		ctrl->flash_dma_mode = flash_dma_readl(ctrl, FLASH_DMA_MODE);
+
+	return 0;
+}
+
+static int brcmnand_resume(struct device *dev)
+{
+	struct brcmnand_controller *ctrl = dev_get_drvdata(dev);
+	struct brcmnand_host *host;
+
+	if (has_flash_dma(ctrl)) {
+		flash_dma_writel(ctrl, FLASH_DMA_MODE, ctrl->flash_dma_mode);
+		flash_dma_writel(ctrl, FLASH_DMA_ERROR_STATUS, 0);
+	}
+
+	brcmnand_write_reg(ctrl, BRCMNAND_CS_SELECT, ctrl->nand_cs_nand_select);
+	brcmnand_write_reg(ctrl, BRCMNAND_CS_XOR, ctrl->nand_cs_nand_xor);
+	brcmnand_write_reg(ctrl, BRCMNAND_CORR_THRESHOLD,
+			ctrl->corr_stat_threshold);
+	if (ctrl->soc) {
+		/* Clear/re-enable interrupt */
+		ctrl->soc->ctlrdy_ack(ctrl->soc);
+		ctrl->soc->ctlrdy_set_enabled(ctrl->soc, true);
+	}
+
+	list_for_each_entry(host, &ctrl->host_list, node) {
+		struct mtd_info *mtd = &host->mtd;
+		struct nand_chip *chip = mtd->priv;
+
+		brcmnand_save_restore_cs_config(host, 1);
+
+		/* Reset the chip, required by some chips after power-up */
+		chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+	}
+
+	return 0;
+}
+
+const struct dev_pm_ops brcmnand_pm_ops = {
+	.suspend		= brcmnand_suspend,
+	.resume			= brcmnand_resume,
+};
+EXPORT_SYMBOL_GPL(brcmnand_pm_ops);
+
+static const struct of_device_id brcmnand_of_match[] = {
+	{ .compatible = "brcm,brcmnand-v4.0" },
+	{ .compatible = "brcm,brcmnand-v5.0" },
+	{ .compatible = "brcm,brcmnand-v6.0" },
+	{ .compatible = "brcm,brcmnand-v6.1" },
+	{ .compatible = "brcm,brcmnand-v7.0" },
+	{ .compatible = "brcm,brcmnand-v7.1" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, brcmnand_of_match);
+
+/***********************************************************************
+ * Platform driver setup (per controller)
+ ***********************************************************************/
+
+int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *dn = dev->of_node, *child;
+	struct brcmnand_controller *ctrl;
+	struct resource *res;
+	int ret;
+
+	/* We only support device-tree instantiation */
+	if (!dn)
+		return -ENODEV;
+
+	if (!of_match_node(brcmnand_of_match, dn))
+		return -ENODEV;
+
+	ctrl = devm_kzalloc(dev, sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, ctrl);
+	ctrl->dev = dev;
+
+	init_completion(&ctrl->done);
+	init_completion(&ctrl->dma_done);
+	spin_lock_init(&ctrl->controller.lock);
+	init_waitqueue_head(&ctrl->controller.wq);
+	INIT_LIST_HEAD(&ctrl->host_list);
+
+	/* NAND register range */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ctrl->nand_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(ctrl->nand_base))
+		return PTR_ERR(ctrl->nand_base);
+
+	/* Initialize NAND revision */
+	ret = brcmnand_revision_init(ctrl);
+	if (ret)
+		return ret;
+
+	/*
+	 * Most chips have this cache at a fixed offset within 'nand' block.
+	 * Some must specify this region separately.
+	 */
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "nand-cache");
+	if (res) {
+		ctrl->nand_fc = devm_ioremap_resource(dev, res);
+		if (IS_ERR(ctrl->nand_fc))
+			return PTR_ERR(ctrl->nand_fc);
+	} else {
+		ctrl->nand_fc = ctrl->nand_base +
+				ctrl->reg_offsets[BRCMNAND_FC_BASE];
+	}
+
+	/* FLASH_DMA */
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "flash-dma");
+	if (res) {
+		ctrl->flash_dma_base = devm_ioremap_resource(dev, res);
+		if (IS_ERR(ctrl->flash_dma_base))
+			return PTR_ERR(ctrl->flash_dma_base);
+
+		flash_dma_writel(ctrl, FLASH_DMA_MODE, 1); /* linked-list */
+		flash_dma_writel(ctrl, FLASH_DMA_ERROR_STATUS, 0);
+
+		/* Allocate descriptor(s) */
+		ctrl->dma_desc = dmam_alloc_coherent(dev,
+						     sizeof(*ctrl->dma_desc),
+						     &ctrl->dma_pa, GFP_KERNEL);
+		if (!ctrl->dma_desc)
+			return -ENOMEM;
+
+		ctrl->dma_irq = platform_get_irq(pdev, 1);
+		if ((int)ctrl->dma_irq < 0) {
+			dev_err(dev, "missing FLASH_DMA IRQ\n");
+			return -ENODEV;
+		}
+
+		ret = devm_request_irq(dev, ctrl->dma_irq,
+				brcmnand_dma_irq, 0, DRV_NAME,
+				ctrl);
+		if (ret < 0) {
+			dev_err(dev, "can't allocate IRQ %d: error %d\n",
+					ctrl->dma_irq, ret);
+			return ret;
+		}
+
+		dev_info(dev, "enabling FLASH_DMA\n");
+	}
+
+	/* Disable automatic device ID config, direct addressing */
+	brcmnand_rmw_reg(ctrl, BRCMNAND_CS_SELECT,
+			 CS_SELECT_AUTO_DEVICE_ID_CFG | 0xff, 0, 0);
+	/* Disable XOR addressing */
+	brcmnand_rmw_reg(ctrl, BRCMNAND_CS_XOR, 0xff, 0, 0);
+
+	if (ctrl->features & BRCMNAND_HAS_WP) {
+		/* Permanently disable write protection */
+		if (wp_on == 2)
+			brcmnand_set_wp(ctrl, false);
+	} else {
+		wp_on = 0;
+	}
+
+	/* IRQ */
+	ctrl->irq = platform_get_irq(pdev, 0);
+	if ((int)ctrl->irq < 0) {
+		dev_err(dev, "no IRQ defined\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Some SoCs integrate this controller (e.g., its interrupt bits) in
+	 * interesting ways
+	 */
+	if (soc) {
+		ctrl->soc = soc;
+
+		ret = devm_request_irq(dev, ctrl->irq, brcmnand_irq, 0,
+				       DRV_NAME, ctrl);
+
+		/* Enable interrupt */
+		ctrl->soc->ctlrdy_ack(ctrl->soc);
+		ctrl->soc->ctlrdy_set_enabled(ctrl->soc, true);
+	} else {
+		/* Use standard interrupt infrastructure */
+		ret = devm_request_irq(dev, ctrl->irq, brcmnand_ctlrdy_irq, 0,
+				       DRV_NAME, ctrl);
+	}
+	if (ret < 0) {
+		dev_err(dev, "can't allocate IRQ %d: error %d\n",
+			ctrl->irq, ret);
+		return ret;
+	}
+
+	for_each_available_child_of_node(dn, child) {
+		if (of_device_is_compatible(child, "brcm,nandcs")) {
+			struct brcmnand_host *host;
+
+			host = devm_kzalloc(dev, sizeof(*host), GFP_KERNEL);
+			if (!host)
+				return -ENOMEM;
+			host->pdev = pdev;
+			host->ctrl = ctrl;
+			host->of_node = child;
+
+			ret = brcmnand_init_cs(host);
+			if (ret)
+				continue; /* Try all chip-selects */
+
+			list_add_tail(&host->node, &ctrl->host_list);
+		}
+	}
+
+	/* No chip-selects could initialize properly */
+	if (list_empty(&ctrl->host_list))
+		return -ENODEV;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(brcmnand_probe);
+
+int brcmnand_remove(struct platform_device *pdev)
+{
+	struct brcmnand_controller *ctrl = dev_get_drvdata(&pdev->dev);
+	struct brcmnand_host *host;
+
+	list_for_each_entry(host, &ctrl->host_list, node)
+		nand_release(&host->mtd);
+
+	dev_set_drvdata(&pdev->dev, NULL);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(brcmnand_remove);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Kevin Cernekee");
+MODULE_AUTHOR("Brian Norris");
+MODULE_DESCRIPTION("NAND driver for Broadcom chips");
+MODULE_ALIAS("platform:brcmnand");
diff --git a/drivers/mtd/nand/brcmnand/brcmnand.h b/drivers/mtd/nand/brcmnand/brcmnand.h
new file mode 100644
index 000000000000..a20c73630b7b
--- /dev/null
+++ b/drivers/mtd/nand/brcmnand/brcmnand.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright © 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __BRCMNAND_H__
+#define __BRCMNAND_H__
+
+#include <linux/types.h>
+#include <linux/io.h>
+
+struct platform_device;
+struct dev_pm_ops;
+
+struct brcmnand_soc {
+	struct platform_device *pdev;
+	void *priv;
+	bool (*ctlrdy_ack)(struct brcmnand_soc *soc);
+	void (*ctlrdy_set_enabled)(struct brcmnand_soc *soc, bool en);
+	void (*prepare_data_bus)(struct brcmnand_soc *soc, bool prepare);
+};
+
+static inline void brcmnand_soc_data_bus_prepare(struct brcmnand_soc *soc)
+{
+	if (soc && soc->prepare_data_bus)
+		soc->prepare_data_bus(soc, true);
+}
+
+static inline void brcmnand_soc_data_bus_unprepare(struct brcmnand_soc *soc)
+{
+	if (soc && soc->prepare_data_bus)
+		soc->prepare_data_bus(soc, false);
+}
+
+static inline u32 brcmnand_readl(void __iomem *addr)
+{
+	/*
+	 * MIPS endianness is configured by boot strap, which also reverses all
+	 * bus endianness (i.e., big-endian CPU + big endian bus ==> native
+	 * endian I/O).
+	 *
+	 * Other architectures (e.g., ARM) either do not support big endian, or
+	 * else leave I/O in little endian mode.
+	 */
+	if (IS_ENABLED(CONFIG_MIPS) && IS_ENABLED(__BIG_ENDIAN))
+		return __raw_readl(addr);
+	else
+		return readl_relaxed(addr);
+}
+
+static inline void brcmnand_writel(u32 val, void __iomem *addr)
+{
+	/* See brcmnand_readl() comments */
+	if (IS_ENABLED(CONFIG_MIPS) && IS_ENABLED(__BIG_ENDIAN))
+		__raw_writel(val, addr);
+	else
+		writel_relaxed(val, addr);
+}
+
+int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc);
+int brcmnand_remove(struct platform_device *pdev);
+
+extern const struct dev_pm_ops brcmnand_pm_ops;
+
+#endif /* __BRCMNAND_H__ */
diff --git a/drivers/mtd/nand/brcmnand/brcmstb_nand.c b/drivers/mtd/nand/brcmnand/brcmstb_nand.c
new file mode 100644
index 000000000000..5c271077ac87
--- /dev/null
+++ b/drivers/mtd/nand/brcmnand/brcmstb_nand.c
@@ -0,0 +1,44 @@
+/*
+ * Copyright © 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include "brcmnand.h"
+
+static const struct of_device_id brcmstb_nand_of_match[] = {
+	{ .compatible = "brcm,brcmnand" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, brcmstb_nand_of_match);
+
+static int brcmstb_nand_probe(struct platform_device *pdev)
+{
+	return brcmnand_probe(pdev, NULL);
+}
+
+static struct platform_driver brcmstb_nand_driver = {
+	.probe			= brcmstb_nand_probe,
+	.remove			= brcmnand_remove,
+	.driver = {
+		.name		= "brcmstb_nand",
+		.pm		= &brcmnand_pm_ops,
+		.of_match_table = brcmstb_nand_of_match,
+	}
+};
+module_platform_driver(brcmstb_nand_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Brian Norris");
+MODULE_DESCRIPTION("NAND driver for Broadcom STB chips");
diff --git a/drivers/mtd/nand/brcmnand/iproc_nand.c b/drivers/mtd/nand/brcmnand/iproc_nand.c
new file mode 100644
index 000000000000..683495c74620
--- /dev/null
+++ b/drivers/mtd/nand/brcmnand/iproc_nand.c
@@ -0,0 +1,150 @@
+/*
+ * Copyright © 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "brcmnand.h"
+
+struct iproc_nand_soc_priv {
+	void __iomem *idm_base;
+	void __iomem *ext_base;
+	spinlock_t idm_lock;
+};
+
+#define IPROC_NAND_CTLR_READY_OFFSET       0x10
+#define IPROC_NAND_CTLR_READY              BIT(0)
+
+#define IPROC_NAND_IO_CTRL_OFFSET          0x00
+#define IPROC_NAND_APB_LE_MODE             BIT(24)
+#define IPROC_NAND_INT_CTRL_READ_ENABLE    BIT(6)
+
+static bool iproc_nand_intc_ack(struct brcmnand_soc *soc)
+{
+	struct iproc_nand_soc_priv *priv = soc->priv;
+	void __iomem *mmio = priv->ext_base + IPROC_NAND_CTLR_READY_OFFSET;
+	u32 val = brcmnand_readl(mmio);
+
+	if (val & IPROC_NAND_CTLR_READY) {
+		brcmnand_writel(IPROC_NAND_CTLR_READY, mmio);
+		return true;
+	}
+
+	return false;
+}
+
+static void iproc_nand_intc_set(struct brcmnand_soc *soc, bool en)
+{
+	struct iproc_nand_soc_priv *priv = soc->priv;
+	void __iomem *mmio = priv->idm_base + IPROC_NAND_IO_CTRL_OFFSET;
+	u32 val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->idm_lock, flags);
+
+	val = brcmnand_readl(mmio);
+
+	if (en)
+		val |= IPROC_NAND_INT_CTRL_READ_ENABLE;
+	else
+		val &= ~IPROC_NAND_INT_CTRL_READ_ENABLE;
+
+	brcmnand_writel(val, mmio);
+
+	spin_unlock_irqrestore(&priv->idm_lock, flags);
+}
+
+static void iproc_nand_apb_access(struct brcmnand_soc *soc, bool prepare)
+{
+	struct iproc_nand_soc_priv *priv = soc->priv;
+	void __iomem *mmio = priv->idm_base + IPROC_NAND_IO_CTRL_OFFSET;
+	u32 val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->idm_lock, flags);
+
+	val = brcmnand_readl(mmio);
+
+	if (prepare)
+		val |= IPROC_NAND_APB_LE_MODE;
+	else
+		val &= ~IPROC_NAND_APB_LE_MODE;
+
+	brcmnand_writel(val, mmio);
+
+	spin_unlock_irqrestore(&priv->idm_lock, flags);
+}
+
+static int iproc_nand_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct iproc_nand_soc_priv *priv;
+	struct brcmnand_soc *soc;
+	struct resource *res;
+
+	soc = devm_kzalloc(dev, sizeof(*soc), GFP_KERNEL);
+	if (!soc)
+		return -ENOMEM;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	spin_lock_init(&priv->idm_lock);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "iproc-idm");
+	priv->idm_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->idm_base))
+		return PTR_ERR(priv->idm_base);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "iproc-ext");
+	priv->ext_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->ext_base))
+		return PTR_ERR(priv->ext_base);
+
+	soc->pdev = pdev;
+	soc->priv = priv;
+	soc->ctlrdy_ack = iproc_nand_intc_ack;
+	soc->ctlrdy_set_enabled = iproc_nand_intc_set;
+	soc->prepare_data_bus = iproc_nand_apb_access;
+
+	return brcmnand_probe(pdev, soc);
+}
+
+static const struct of_device_id iproc_nand_of_match[] = {
+	{ .compatible = "brcm,nand-iproc" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, iproc_nand_of_match);
+
+static struct platform_driver iproc_nand_driver = {
+	.probe			= iproc_nand_probe,
+	.remove			= brcmnand_remove,
+	.driver = {
+		.name		= "iproc_nand",
+		.pm		= &brcmnand_pm_ops,
+		.of_match_table	= iproc_nand_of_match,
+	}
+};
+module_platform_driver(iproc_nand_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Brian Norris");
+MODULE_AUTHOR("Ray Jui");
+MODULE_DESCRIPTION("NAND driver for Broadcom IPROC-based SoCs");
diff --git a/drivers/mtd/nand/cs553x_nand.c b/drivers/mtd/nand/cs553x_nand.c
index 88109d375ae7..aec6045058c7 100644
--- a/drivers/mtd/nand/cs553x_nand.c
+++ b/drivers/mtd/nand/cs553x_nand.c
@@ -237,17 +237,23 @@ static int __init cs553x_init_one(int cs, int mmio, unsigned long adr)
 	/* Enable the following for a flash based bad block table */
 	this->bbt_options = NAND_BBT_USE_FLASH;
 
+	new_mtd->name = kasprintf(GFP_KERNEL, "cs553x_nand_cs%d", cs);
+	if (!new_mtd->name) {
+		err = -ENOMEM;
+		goto out_ior;
+	}
+
 	/* Scan to find existence of the device */
 	if (nand_scan(new_mtd, 1)) {
 		err = -ENXIO;
-		goto out_ior;
+		goto out_free;
 	}
 
-	new_mtd->name = kasprintf(GFP_KERNEL, "cs553x_nand_cs%d", cs);
-
 	cs553x_mtd[cs] = new_mtd;
 	goto out;
 
+out_free:
+	kfree(new_mtd->name);
 out_ior:
 	iounmap(this->IO_ADDR_R);
 out_mtd:
diff --git a/drivers/mtd/nand/diskonchip.c b/drivers/mtd/nand/diskonchip.c
index f68a7bccecdc..7da266a53979 100644
--- a/drivers/mtd/nand/diskonchip.c
+++ b/drivers/mtd/nand/diskonchip.c
@@ -69,6 +69,9 @@ struct doc_priv {
 	int mh0_page;
 	int mh1_page;
 	struct mtd_info *nextdoc;
+
+	/* Handle the last stage of initialization (BBT scan, partitioning) */
+	int (*late_init)(struct mtd_info *mtd);
 };
 
 /* This is the syndrome computed by the HW ecc generator upon reading an empty
@@ -1294,14 +1297,11 @@ static int __init nftl_scan_bbt(struct mtd_info *mtd)
 		this->bbt_md = NULL;
 	}
 
-	/* It's safe to set bd=NULL below because NAND_BBT_CREATE is not set.
-	   At least as nand_bbt.c is currently written. */
-	if ((ret = nand_scan_bbt(mtd, NULL)))
+	ret = this->scan_bbt(mtd);
+	if (ret)
 		return ret;
-	mtd_device_register(mtd, NULL, 0);
-	if (!no_autopart)
-		mtd_device_register(mtd, parts, numparts);
-	return 0;
+
+	return mtd_device_register(mtd, parts, no_autopart ? 0 : numparts);
 }
 
 static int __init inftl_scan_bbt(struct mtd_info *mtd)
@@ -1344,10 +1344,10 @@ static int __init inftl_scan_bbt(struct mtd_info *mtd)
 		this->bbt_md->pattern = "TBB_SYSM";
 	}
 
-	/* It's safe to set bd=NULL below because NAND_BBT_CREATE is not set.
-	   At least as nand_bbt.c is currently written. */
-	if ((ret = nand_scan_bbt(mtd, NULL)))
+	ret = this->scan_bbt(mtd);
+	if (ret)
 		return ret;
+
 	memset((char *)parts, 0, sizeof(parts));
 	numparts = inftl_partscan(mtd, parts);
 	/* At least for now, require the INFTL Media Header.  We could probably
@@ -1355,10 +1355,7 @@ static int __init inftl_scan_bbt(struct mtd_info *mtd)
 	   autopartitioning, but I want to give it more thought. */
 	if (!numparts)
 		return -EIO;
-	mtd_device_register(mtd, NULL, 0);
-	if (!no_autopart)
-		mtd_device_register(mtd, parts, numparts);
-	return 0;
+	return mtd_device_register(mtd, parts, no_autopart ? 0 : numparts);
 }
 
 static inline int __init doc2000_init(struct mtd_info *mtd)
@@ -1369,7 +1366,7 @@ static inline int __init doc2000_init(struct mtd_info *mtd)
 	this->read_byte = doc2000_read_byte;
 	this->write_buf = doc2000_writebuf;
 	this->read_buf = doc2000_readbuf;
-	this->scan_bbt = nftl_scan_bbt;
+	doc->late_init = nftl_scan_bbt;
 
 	doc->CDSNControl = CDSN_CTRL_FLASH_IO | CDSN_CTRL_ECC_IO;
 	doc2000_count_chips(mtd);
@@ -1396,13 +1393,13 @@ static inline int __init doc2001_init(struct mtd_info *mtd)
 		   can have multiple chips. */
 		doc2000_count_chips(mtd);
 		mtd->name = "DiskOnChip 2000 (INFTL Model)";
-		this->scan_bbt = inftl_scan_bbt;
+		doc->late_init = inftl_scan_bbt;
 		return (4 * doc->chips_per_floor);
 	} else {
 		/* Bog-standard Millennium */
 		doc->chips_per_floor = 1;
 		mtd->name = "DiskOnChip Millennium";
-		this->scan_bbt = nftl_scan_bbt;
+		doc->late_init = nftl_scan_bbt;
 		return 1;
 	}
 }
@@ -1415,7 +1412,7 @@ static inline int __init doc2001plus_init(struct mtd_info *mtd)
 	this->read_byte = doc2001plus_read_byte;
 	this->write_buf = doc2001plus_writebuf;
 	this->read_buf = doc2001plus_readbuf;
-	this->scan_bbt = inftl_scan_bbt;
+	doc->late_init = inftl_scan_bbt;
 	this->cmd_ctrl = NULL;
 	this->select_chip = doc2001plus_select_chip;
 	this->cmdfunc = doc2001plus_command;
@@ -1591,6 +1588,8 @@ static int __init doc_probe(unsigned long physadr)
 	nand->ecc.bytes		= 6;
 	nand->ecc.strength	= 2;
 	nand->bbt_options	= NAND_BBT_USE_FLASH;
+	/* Skip the automatic BBT scan so we can run it manually */
+	nand->options		|= NAND_SKIP_BBTSCAN;
 
 	doc->physadr		= physadr;
 	doc->virtadr		= virtadr;
@@ -1608,7 +1607,7 @@ static int __init doc_probe(unsigned long physadr)
 	else
 		numchips = doc2001_init(mtd);
 
-	if ((ret = nand_scan(mtd, numchips))) {
+	if ((ret = nand_scan(mtd, numchips)) || (ret = doc->late_init(mtd))) {
 		/* DBB note: i believe nand_release is necessary here, as
 		   buffers may have been allocated in nand_base.  Check with
 		   Thomas. FIX ME! */
diff --git a/drivers/mtd/nand/fsmc_nand.c b/drivers/mtd/nand/fsmc_nand.c
index e58af4bfa8c8..793872f18065 100644
--- a/drivers/mtd/nand/fsmc_nand.c
+++ b/drivers/mtd/nand/fsmc_nand.c
@@ -562,6 +562,7 @@ static int dma_xfer(struct fsmc_nand_data *host, void *buffer, int len,
 	dma_cookie_t cookie;
 	unsigned long flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
 	int ret;
+	unsigned long time_left;
 
 	if (direction == DMA_TO_DEVICE)
 		chan = host->write_dma_chan;
@@ -601,14 +602,13 @@ static int dma_xfer(struct fsmc_nand_data *host, void *buffer, int len,
 
 	dma_async_issue_pending(chan);
 
-	ret =
+	time_left =
 	wait_for_completion_timeout(&host->dma_access_complete,
 				msecs_to_jiffies(3000));
-	if (ret <= 0) {
+	if (time_left == 0) {
 		dmaengine_terminate_all(chan);
 		dev_err(host->dev, "wait_for_completion_timeout\n");
-		if (!ret)
-			ret = -ETIMEDOUT;
+		ret = -ETIMEDOUT;
 		goto unmap_dma;
 	}
 
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index 1f12e5bfbced..2a49b53c8db9 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -837,7 +837,7 @@ static int mpc5121_nfc_remove(struct platform_device *op)
 	return 0;
 }
 
-static struct of_device_id mpc5121_nfc_match[] = {
+static const struct of_device_id mpc5121_nfc_match[] = {
 	{ .compatible = "fsl,mpc5121-nfc", },
 	{},
 };
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 372e0e38f59b..2426db88db36 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -189,6 +189,7 @@ struct mxc_nand_host {
 	int			clk_act;
 	int			irq;
 	int			eccsize;
+	int			used_oobsize;
 	int			active_cs;
 
 	struct completion	op_completion;
@@ -280,12 +281,44 @@ static void memcpy32_fromio(void *trg, const void __iomem  *src, size_t size)
 		*t++ = __raw_readl(s++);
 }
 
+static void memcpy16_fromio(void *trg, const void __iomem  *src, size_t size)
+{
+	int i;
+	u16 *t = trg;
+	const __iomem u16 *s = src;
+
+	/* We assume that src (IO) is always 32bit aligned */
+	if (PTR_ALIGN(trg, 4) == trg && IS_ALIGNED(size, 4)) {
+		memcpy32_fromio(trg, src, size);
+		return;
+	}
+
+	for (i = 0; i < (size >> 1); i++)
+		*t++ = __raw_readw(s++);
+}
+
 static inline void memcpy32_toio(void __iomem *trg, const void *src, int size)
 {
 	/* __iowrite32_copy use 32bit size values so divide by 4 */
 	__iowrite32_copy(trg, src, size / 4);
 }
 
+static void memcpy16_toio(void __iomem *trg, const void *src, int size)
+{
+	int i;
+	__iomem u16 *t = trg;
+	const u16 *s = src;
+
+	/* We assume that trg (IO) is always 32bit aligned */
+	if (PTR_ALIGN(src, 4) == src && IS_ALIGNED(size, 4)) {
+		memcpy32_toio(trg, src, size);
+		return;
+	}
+
+	for (i = 0; i < (size >> 1); i++)
+		__raw_writew(*s++, t++);
+}
+
 static int check_int_v3(struct mxc_nand_host *host)
 {
 	uint32_t tmp;
@@ -807,32 +840,48 @@ static void mxc_nand_select_chip_v2(struct mtd_info *mtd, int chip)
 }
 
 /*
- * Function to transfer data to/from spare area.
+ * The controller splits a page into data chunks of 512 bytes + partial oob.
+ * There are writesize / 512 such chunks, the size of the partial oob parts is
+ * oobsize / #chunks rounded down to a multiple of 2. The last oob chunk then
+ * contains additionally the byte lost by rounding (if any).
+ * This function handles the needed shuffling between host->data_buf (which
+ * holds a page in natural order, i.e. writesize bytes data + oobsize bytes
+ * spare) and the NFC buffer.
  */
 static void copy_spare(struct mtd_info *mtd, bool bfrom)
 {
 	struct nand_chip *this = mtd->priv;
 	struct mxc_nand_host *host = this->priv;
-	u16 i, j;
-	u16 n = mtd->writesize >> 9;
+	u16 i, oob_chunk_size;
+	u16 num_chunks = mtd->writesize / 512;
+
 	u8 *d = host->data_buf + mtd->writesize;
 	u8 __iomem *s = host->spare0;
-	u16 t = host->devtype_data->spare_len;
+	u16 sparebuf_size = host->devtype_data->spare_len;
 
-	j = (mtd->oobsize / n >> 1) << 1;
+	/* size of oob chunk for all but possibly the last one */
+	oob_chunk_size = (host->used_oobsize / num_chunks) & ~1;
 
 	if (bfrom) {
-		for (i = 0; i < n - 1; i++)
-			memcpy32_fromio(d + i * j, s + i * t, j);
-
-		/* the last section */
-		memcpy32_fromio(d + i * j, s + i * t, mtd->oobsize - i * j);
+		for (i = 0; i < num_chunks - 1; i++)
+			memcpy16_fromio(d + i * oob_chunk_size,
+					s + i * sparebuf_size,
+					oob_chunk_size);
+
+		/* the last chunk */
+		memcpy16_fromio(d + i * oob_chunk_size,
+				s + i * sparebuf_size,
+				host->used_oobsize - i * oob_chunk_size);
 	} else {
-		for (i = 0; i < n - 1; i++)
-			memcpy32_toio(&s[i * t], &d[i * j], j);
-
-		/* the last section */
-		memcpy32_toio(&s[i * t], &d[i * j], mtd->oobsize - i * j);
+		for (i = 0; i < num_chunks - 1; i++)
+			memcpy16_toio(&s[i * sparebuf_size],
+				      &d[i * oob_chunk_size],
+				      oob_chunk_size);
+
+		/* the last chunk */
+		memcpy16_toio(&s[oob_chunk_size * sparebuf_size],
+			      &d[i * oob_chunk_size],
+			      host->used_oobsize - i * oob_chunk_size);
 	}
 }
 
@@ -911,6 +960,23 @@ static int get_eccsize(struct mtd_info *mtd)
 		return 8;
 }
 
+static void ecc_8bit_layout_4k(struct nand_ecclayout *layout)
+{
+	int i, j;
+
+	layout->eccbytes = 8*18;
+	for (i = 0; i < 8; i++)
+		for (j = 0; j < 18; j++)
+			layout->eccpos[i*18 + j] = i*26 + j + 7;
+
+	layout->oobfree[0].offset = 2;
+	layout->oobfree[0].length = 4;
+	for (i = 1; i < 8; i++) {
+		layout->oobfree[i].offset = i*26;
+		layout->oobfree[i].length = 7;
+	}
+}
+
 static void preset_v1(struct mtd_info *mtd)
 {
 	struct nand_chip *nand_chip = mtd->priv;
@@ -1350,7 +1416,7 @@ static inline int is_imx53_nfc(struct mxc_nand_host *host)
 	return host->devtype_data == &imx53_nand_devtype_data;
 }
 
-static struct platform_device_id mxcnd_devtype[] = {
+static const struct platform_device_id mxcnd_devtype[] = {
 	{
 		.name = "imx21-nand",
 		.driver_data = (kernel_ulong_t) &imx21_nand_devtype_data,
@@ -1587,8 +1653,20 @@ static int mxcnd_probe(struct platform_device *pdev)
 
 	if (mtd->writesize == 2048)
 		this->ecc.layout = host->devtype_data->ecclayout_2k;
-	else if (mtd->writesize == 4096)
+	else if (mtd->writesize == 4096) {
 		this->ecc.layout = host->devtype_data->ecclayout_4k;
+		if (get_eccsize(mtd) == 8)
+			ecc_8bit_layout_4k(this->ecc.layout);
+	}
+
+	/*
+	 * Experimentation shows that i.MX NFC can only handle up to 218 oob
+	 * bytes. Limit used_oobsize to 218 so as to not confuse copy_spare()
+	 * into copying invalid data to/from the spare IO buffer, as this
+	 * might cause ECC data corruption when doing sub-page write to a
+	 * partially written page.
+	 */
+	host->used_oobsize = min(mtd->oobsize, 218U);
 
 	if (this->ecc.mode == NAND_ECC_HW) {
 		if (is_imx21_nfc(host) || is_imx27_nfc(host))
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index c2e1232cd45c..ceb68ca8277a 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/mtd/nand.c
- *
  *  Overview:
  *   This is the generic MTD driver for NAND flash devices. It should be
  *   capable of working with almost all NAND chips currently available.
@@ -48,6 +46,7 @@
 #include <linux/leds.h>
 #include <linux/io.h>
 #include <linux/mtd/partitions.h>
+#include <linux/of_mtd.h>
 
 /* Define default oob placement schemes for large and small page devices */
 static struct nand_ecclayout nand_oob_8 = {
@@ -2928,9 +2927,6 @@ static int nand_onfi_get_features(struct mtd_info *mtd, struct nand_chip *chip,
 	      & ONFI_OPT_CMD_SET_GET_FEATURES))
 		return -EINVAL;
 
-	/* clear the sub feature parameters */
-	memset(subfeature_param, 0, ONFI_SUBFEATURE_PARAM_LEN);
-
 	chip->cmdfunc(mtd, NAND_CMD_GET_FEATURES, addr, -1);
 	for (i = 0; i < ONFI_SUBFEATURE_PARAM_LEN; ++i)
 		*subfeature_param++ = chip->read_byte(mtd);
@@ -3689,7 +3685,7 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 			if (find_full_id_nand(mtd, chip, type, id_data, &busw))
 				goto ident_done;
 		} else if (*dev_id == type->dev_id) {
-				break;
+			break;
 		}
 	}
 
@@ -3798,6 +3794,39 @@ ident_done:
 	return type;
 }
 
+static int nand_dt_init(struct mtd_info *mtd, struct nand_chip *chip,
+			struct device_node *dn)
+{
+	int ecc_mode, ecc_strength, ecc_step;
+
+	if (of_get_nand_bus_width(dn) == 16)
+		chip->options |= NAND_BUSWIDTH_16;
+
+	if (of_get_nand_on_flash_bbt(dn))
+		chip->bbt_options |= NAND_BBT_USE_FLASH;
+
+	ecc_mode = of_get_nand_ecc_mode(dn);
+	ecc_strength = of_get_nand_ecc_strength(dn);
+	ecc_step = of_get_nand_ecc_step_size(dn);
+
+	if ((ecc_step >= 0 && !(ecc_strength >= 0)) ||
+	    (!(ecc_step >= 0) && ecc_strength >= 0)) {
+		pr_err("must set both strength and step size in DT\n");
+		return -EINVAL;
+	}
+
+	if (ecc_mode >= 0)
+		chip->ecc.mode = ecc_mode;
+
+	if (ecc_strength >= 0)
+		chip->ecc.strength = ecc_strength;
+
+	if (ecc_step > 0)
+		chip->ecc.size = ecc_step;
+
+	return 0;
+}
+
 /**
  * nand_scan_ident - [NAND Interface] Scan for the NAND device
  * @mtd: MTD device structure
@@ -3815,6 +3844,13 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 	int i, nand_maf_id, nand_dev_id;
 	struct nand_chip *chip = mtd->priv;
 	struct nand_flash_dev *type;
+	int ret;
+
+	if (chip->dn) {
+		ret = nand_dt_init(mtd, chip, chip->dn);
+		if (ret)
+			return ret;
+	}
 
 	/* Set the default functions */
 	nand_set_defaults(chip, chip->options & NAND_BUSWIDTH_16);
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 9bb8453d224e..63a1a36a3f4b 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/mtd/nand_bbt.c
- *
  *  Overview:
  *   Bad block table support for the NAND driver
  *
@@ -64,7 +62,6 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/bbm.h>
 #include <linux/mtd/nand.h>
-#include <linux/mtd/nand_ecc.h>
 #include <linux/bitops.h>
 #include <linux/delay.h>
 #include <linux/vmalloc.h>
@@ -720,7 +717,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 		/* Must we save the block contents? */
 		if (td->options & NAND_BBT_SAVECONTENT) {
 			/* Make it block aligned */
-			to &= ~((loff_t)((1 << this->bbt_erase_shift) - 1));
+			to &= ~(((loff_t)1 << this->bbt_erase_shift) - 1);
 			len = 1 << this->bbt_erase_shift;
 			res = mtd_read(mtd, to, len, &retlen, buf);
 			if (res < 0) {
@@ -1075,10 +1072,10 @@ static void verify_bbt_descr(struct mtd_info *mtd, struct nand_bbt_descr *bd)
  * The bad block table memory is allocated here. It must be freed by calling
  * the nand_free_bbt function.
  */
-int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
+static int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 {
 	struct nand_chip *this = mtd->priv;
-	int len, res = 0;
+	int len, res;
 	uint8_t *buf;
 	struct nand_bbt_descr *td = this->bbt_td;
 	struct nand_bbt_descr *md = this->bbt_md;
@@ -1099,10 +1096,9 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 	if (!td) {
 		if ((res = nand_memory_bbt(mtd, bd))) {
 			pr_err("nand_bbt: can't scan flash and build the RAM-based BBT\n");
-			kfree(this->bbt);
-			this->bbt = NULL;
+			goto err;
 		}
-		return res;
+		return 0;
 	}
 	verify_bbt_descr(mtd, td);
 	verify_bbt_descr(mtd, md);
@@ -1112,9 +1108,8 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 	len += (len >> this->page_shift) * mtd->oobsize;
 	buf = vmalloc(len);
 	if (!buf) {
-		kfree(this->bbt);
-		this->bbt = NULL;
-		return -ENOMEM;
+		res = -ENOMEM;
+		goto err;
 	}
 
 	/* Is the bbt at a given page? */
@@ -1126,6 +1121,8 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 	}
 
 	res = check_create(mtd, buf, bd);
+	if (res)
+		goto err;
 
 	/* Prevent the bbt regions from erasing / writing */
 	mark_bbt_region(mtd, td);
@@ -1133,6 +1130,11 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 		mark_bbt_region(mtd, md);
 
 	vfree(buf);
+	return 0;
+
+err:
+	kfree(this->bbt);
+	this->bbt = NULL;
 	return res;
 }
 
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index dd620c19c619..7124400d903b 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/mtd/nandids.c
- *
  *  Copyright (C) 2002 Thomas Gleixner (tglx@linutronix.de)
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index f2324271b94e..52c0c1a3899c 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -743,6 +743,11 @@ static int init_nandsim(struct mtd_info *mtd)
 			goto error;
 		}
 		ns->partitions[i].name   = get_partition_name(i);
+		if (!ns->partitions[i].name) {
+			NS_ERR("unable to allocate memory.\n");
+			ret = -ENOMEM;
+			goto error;
+		}
 		ns->partitions[i].offset = next_offset;
 		ns->partitions[i].size   = part_sz;
 		next_offset += ns->partitions[i].size;
@@ -756,6 +761,11 @@ static int init_nandsim(struct mtd_info *mtd)
 			goto error;
 		}
 		ns->partitions[i].name   = get_partition_name(i);
+		if (!ns->partitions[i].name) {
+			NS_ERR("unable to allocate memory.\n");
+			ret = -ENOMEM;
+			goto error;
+		}
 		ns->partitions[i].offset = next_offset;
 		ns->partitions[i].size   = remains;
 		ns->nbparts += 1;
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index 3187c6b92d9a..67a1b3f911cf 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/mtd/ndfc.c
- *
  *  Overview:
  *   Platform independent driver for NDFC (NanD Flash Controller)
  *   integrated into EP440 cores
diff --git a/drivers/mtd/nand/plat_nand.c b/drivers/mtd/nand/plat_nand.c
index 4535c263fae5..717cf623fcde 100644
--- a/drivers/mtd/nand/plat_nand.c
+++ b/drivers/mtd/nand/plat_nand.c
@@ -24,8 +24,6 @@ struct plat_nand_data {
 	void __iomem		*io_base;
 };
 
-static const char *part_probe_types[] = { "cmdlinepart", NULL };
-
 /*
  * Probe for the NAND device.
  */
@@ -95,7 +93,7 @@ static int plat_nand_probe(struct platform_device *pdev)
 		goto out;
 	}
 
-	part_types = pdata->chip.part_probe_types ? : part_probe_types;
+	part_types = pdata->chip.part_probe_types;
 
 	ppdata.of_node = pdev->dev.of_node;
 	err = mtd_device_parse_register(&data->mtd, part_types, &ppdata,
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index a4615fcc3d00..1259cc558ce9 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -22,13 +22,14 @@
 #include <linux/mtd/nand.h>
 #include <linux/mtd/partitions.h>
 #include <linux/io.h>
+#include <linux/iopoll.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_mtd.h>
 
-#if defined(CONFIG_ARCH_PXA) || defined(CONFIG_ARCH_MMP)
+#if defined(CONFIG_ARM) && (defined(CONFIG_ARCH_PXA) || defined(CONFIG_ARCH_MMP))
 #define ARCH_HAS_DMA
 #endif
 
@@ -483,7 +484,8 @@ static void disable_int(struct pxa3xx_nand_info *info, uint32_t int_mask)
 static void drain_fifo(struct pxa3xx_nand_info *info, void *data, int len)
 {
 	if (info->ecc_bch) {
-		int timeout;
+		u32 val;
+		int ret;
 
 		/*
 		 * According to the datasheet, when reading from NDDB
@@ -494,18 +496,14 @@ static void drain_fifo(struct pxa3xx_nand_info *info, void *data, int len)
 		 * the polling on the last read.
 		 */
 		while (len > 8) {
-			__raw_readsl(info->mmio_base + NDDB, data, 8);
-
-			for (timeout = 0;
-			     !(nand_readl(info, NDSR) & NDSR_RDDREQ);
-			     timeout++) {
-				if (timeout >= 5) {
-					dev_err(&info->pdev->dev,
-						"Timeout on RDDREQ while draining the FIFO\n");
-					return;
-				}
-
-				mdelay(1);
+			readsl(info->mmio_base + NDDB, data, 8);
+
+			ret = readl_relaxed_poll_timeout(info->mmio_base + NDSR, val,
+							 val & NDSR_RDDREQ, 1000, 5000);
+			if (ret) {
+				dev_err(&info->pdev->dev,
+					"Timeout on RDDREQ while draining the FIFO\n");
+				return;
 			}
 
 			data += 32;
@@ -513,7 +511,7 @@ static void drain_fifo(struct pxa3xx_nand_info *info, void *data, int len)
 		}
 	}
 
-	__raw_readsl(info->mmio_base + NDDB, data, len);
+	readsl(info->mmio_base + NDDB, data, len);
 }
 
 static void handle_data_pio(struct pxa3xx_nand_info *info)
@@ -522,14 +520,14 @@ static void handle_data_pio(struct pxa3xx_nand_info *info)
 
 	switch (info->state) {
 	case STATE_PIO_WRITING:
-		__raw_writesl(info->mmio_base + NDDB,
-			      info->data_buff + info->data_buff_pos,
-			      DIV_ROUND_UP(do_bytes, 4));
+		writesl(info->mmio_base + NDDB,
+			info->data_buff + info->data_buff_pos,
+			DIV_ROUND_UP(do_bytes, 4));
 
 		if (info->oob_size > 0)
-			__raw_writesl(info->mmio_base + NDDB,
-				      info->oob_buff + info->oob_buff_pos,
-				      DIV_ROUND_UP(info->oob_size, 4));
+			writesl(info->mmio_base + NDDB,
+				info->oob_buff + info->oob_buff_pos,
+				DIV_ROUND_UP(info->oob_size, 4));
 		break;
 	case STATE_PIO_READING:
 		drain_fifo(info,
@@ -1630,8 +1628,7 @@ static int alloc_nand_resource(struct platform_device *pdev)
 	info->pdev = pdev;
 	info->variant = pxa3xx_nand_get_variant(pdev);
 	for (cs = 0; cs < pdata->num_cs; cs++) {
-		mtd = (struct mtd_info *)((unsigned int)&info[1] +
-		      (sizeof(*mtd) + sizeof(*host)) * cs);
+		mtd = (void *)&info[1] + (sizeof(*mtd) + sizeof(*host)) * cs;
 		chip = (struct nand_chip *)(&mtd[1]);
 		host = (struct pxa3xx_nand_host *)chip;
 		info->host[cs] = host;
diff --git a/drivers/mtd/nand/r852.c b/drivers/mtd/nand/r852.c
index baea83f4dea8..77e96d2df96c 100644
--- a/drivers/mtd/nand/r852.c
+++ b/drivers/mtd/nand/r852.c
@@ -653,11 +653,15 @@ static int r852_register_nand_device(struct r852_device *dev)
 	if (sm_register_device(dev->mtd, dev->sm))
 		goto error2;
 
-	if (device_create_file(&dev->mtd->dev, &dev_attr_media_type))
+	if (device_create_file(&dev->mtd->dev, &dev_attr_media_type)) {
 		message("can't create media type sysfs attribute");
+		goto error3;
+	}
 
 	dev->card_registred = 1;
 	return 0;
+error3:
+	nand_release(dev->mtd);
 error2:
 	kfree(dev->mtd);
 error1:
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index 0e02be47ce1d..381f67ac6b5a 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -1105,7 +1105,7 @@ static int s3c24xx_nand_resume(struct platform_device *dev)
 
 /* driver device registration */
 
-static struct platform_device_id s3c24xx_driver_ids[] = {
+static const struct platform_device_id s3c24xx_driver_ids[] = {
 	{
 		.name		= "s3c2410-nand",
 		.driver_data	= TYPE_S3C2410,
diff --git a/drivers/mtd/nand/xway_nand.c b/drivers/mtd/nand/xway_nand.c
index 3f81dc8f214c..3b28db458ea0 100644
--- a/drivers/mtd/nand/xway_nand.c
+++ b/drivers/mtd/nand/xway_nand.c
@@ -160,14 +160,10 @@ static int xway_nand_probe(struct platform_device *pdev)
 	return 0;
 }
 
-/* allow users to override the partition in DT using the cmdline */
-static const char *part_probes[] = { "cmdlinepart", "ofpart", NULL };
-
 static struct platform_nand_data xway_nand_data = {
 	.chip = {
 		.nr_chips		= 1,
 		.chip_delay		= 30,
-		.part_probe_types	= part_probes,
 	},
 	.ctrl = {
 		.probe		= xway_nand_probe,
diff --git a/drivers/mtd/onenand/samsung.c b/drivers/mtd/onenand/samsung.c
index 19cfb97adbc0..739259513055 100644
--- a/drivers/mtd/onenand/samsung.c
+++ b/drivers/mtd/onenand/samsung.c
@@ -1083,7 +1083,7 @@ static const struct dev_pm_ops s3c_pm_ops = {
 	.resume		= s3c_pm_ops_resume,
 };
 
-static struct platform_device_id s3c_onenand_driver_ids[] = {
+static const struct platform_device_id s3c_onenand_driver_ids[] = {
 	{
 		.name		= "s3c6400-onenand",
 		.driver_data	= TYPE_S3C6400,
diff --git a/drivers/mtd/spi-nor/fsl-quadspi.c b/drivers/mtd/spi-nor/fsl-quadspi.c
index 5d5d36272bb5..52a872fa1b6e 100644
--- a/drivers/mtd/spi-nor/fsl-quadspi.c
+++ b/drivers/mtd/spi-nor/fsl-quadspi.c
@@ -662,7 +662,7 @@ static int fsl_qspi_nor_setup_last(struct fsl_qspi *q)
 	return 0;
 }
 
-static struct of_device_id fsl_qspi_dt_ids[] = {
+static const struct of_device_id fsl_qspi_dt_ids[] = {
 	{ .compatible = "fsl,vf610-qspi", .data = (void *)&vybrid_data, },
 	{ .compatible = "fsl,imx6sx-qspi", .data = (void *)&imx6sx_data, },
 	{ /* sentinel */ }
diff --git a/drivers/mtd/spi-nor/spi-nor.c b/drivers/mtd/spi-nor/spi-nor.c
index 14a5d2325dac..d78831b4422b 100644
--- a/drivers/mtd/spi-nor/spi-nor.c
+++ b/drivers/mtd/spi-nor/spi-nor.c
@@ -513,6 +513,13 @@ static int spi_nor_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 /* NOTE: double check command sets and memory organization when you add
  * more nor chips.  This current list focusses on newer chips, which
  * have been converging on command sets which including JEDEC ID.
+ *
+ * All newly added entries should describe *hardware* and should use SECT_4K
+ * (or SECT_4K_PMC) if hardware supports erasing 4 KiB sectors. For usage
+ * scenarios excluding small sectors there is config option that can be
+ * disabled: CONFIG_MTD_SPI_NOR_USE_4K_SECTORS.
+ * For historical (and compatibility) reasons (before we got above config) some
+ * old entries may be missing 4K flag.
  */
 static const struct spi_device_id spi_nor_ids[] = {
 	/* Atmel -- some are (confusingly) marketed as "DataFlash" */
@@ -538,7 +545,7 @@ static const struct spi_device_id spi_nor_ids[] = {
 	{ "en25q64",    INFO(0x1c3017, 0, 64 * 1024,  128, SECT_4K) },
 	{ "en25qh128",  INFO(0x1c7018, 0, 64 * 1024,  256, 0) },
 	{ "en25qh256",  INFO(0x1c7019, 0, 64 * 1024,  512, 0) },
-	{ "en25s64",	INFO(0x1c3817, 0, 64 * 1024,  128, 0) },
+	{ "en25s64",	INFO(0x1c3817, 0, 64 * 1024,  128, SECT_4K) },
 
 	/* ESMT */
 	{ "f25l32pa", INFO(0x8c2016, 0, 64 * 1024, 64, SECT_4K) },
@@ -560,7 +567,11 @@ static const struct spi_device_id spi_nor_ids[] = {
 	{ "320s33b",  INFO(0x898912, 0, 64 * 1024,  64, 0) },
 	{ "640s33b",  INFO(0x898913, 0, 64 * 1024, 128, 0) },
 
+	/* ISSI */
+	{ "is25cd512", INFO(0x7f9d20, 0, 32 * 1024,   2, SECT_4K) },
+
 	/* Macronix */
+	{ "mx25l512e",   INFO(0xc22010, 0, 64 * 1024,   1, SECT_4K) },
 	{ "mx25l2005a",  INFO(0xc22012, 0, 64 * 1024,   4, SECT_4K) },
 	{ "mx25l4005a",  INFO(0xc22013, 0, 64 * 1024,   8, SECT_4K) },
 	{ "mx25l8005",   INFO(0xc22014, 0, 64 * 1024,  16, 0) },
@@ -602,7 +613,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) },
+	{ "s25fl128s",	INFO6(0x012018, 0x4d0180, 64 * 1024, 256, SECT_4K | 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) },
@@ -613,7 +624,8 @@ 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) },
+	{ "s25fl132k",  INFO(0x014016,      0,  64 * 1024,  64, SECT_4K) },
+	{ "s25fl164k",  INFO(0x014017,      0,  64 * 1024, 128, SECT_4K) },
 
 	/* SST -- large erase sizes are "overlays", "sectors" are 4K */
 	{ "sst25vf040b", INFO(0xbf258d, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
diff --git a/drivers/mtd/ubi/block.c b/drivers/mtd/ubi/block.c
index c9eb78f10a0d..ebf46ad2d513 100644
--- a/drivers/mtd/ubi/block.c
+++ b/drivers/mtd/ubi/block.c
@@ -48,6 +48,7 @@
 #include <linux/blk-mq.h>
 #include <linux/hdreg.h>
 #include <linux/scatterlist.h>
+#include <linux/idr.h>
 #include <asm/div64.h>
 
 #include "ubi-media.h"
@@ -161,7 +162,7 @@ static int __init ubiblock_set_param(const char *val,
 	return 0;
 }
 
-static struct kernel_param_ops ubiblock_param_ops = {
+static const struct kernel_param_ops ubiblock_param_ops = {
 	.set    = ubiblock_set_param,
 };
 module_param_cb(block, &ubiblock_param_ops, NULL, 0);
@@ -353,6 +354,8 @@ static struct blk_mq_ops ubiblock_mq_ops = {
 	.map_queue      = blk_mq_map_queue,
 };
 
+static DEFINE_IDR(ubiblock_minor_idr);
+
 int ubiblock_create(struct ubi_volume_info *vi)
 {
 	struct ubiblock *dev;
@@ -390,7 +393,13 @@ int ubiblock_create(struct ubi_volume_info *vi)
 
 	gd->fops = &ubiblock_ops;
 	gd->major = ubiblock_major;
-	gd->first_minor = dev->ubi_num * UBI_MAX_VOLUMES + dev->vol_id;
+	gd->first_minor = idr_alloc(&ubiblock_minor_idr, dev, 0, 0, GFP_KERNEL);
+	if (gd->first_minor < 0) {
+		dev_err(disk_to_dev(gd),
+			"block: dynamic minor allocation failed");
+		ret = -ENODEV;
+		goto out_put_disk;
+	}
 	gd->private_data = dev;
 	sprintf(gd->disk_name, "ubiblock%d_%d", dev->ubi_num, dev->vol_id);
 	set_capacity(gd, disk_capacity);
@@ -407,7 +416,7 @@ int ubiblock_create(struct ubi_volume_info *vi)
 	ret = blk_mq_alloc_tag_set(&dev->tag_set);
 	if (ret) {
 		dev_err(disk_to_dev(dev->gd), "blk_mq_alloc_tag_set failed");
-		goto out_put_disk;
+		goto out_remove_minor;
 	}
 
 	dev->rq = blk_mq_init_queue(&dev->tag_set);
@@ -445,6 +454,8 @@ out_free_queue:
 	blk_cleanup_queue(dev->rq);
 out_free_tags:
 	blk_mq_free_tag_set(&dev->tag_set);
+out_remove_minor:
+	idr_remove(&ubiblock_minor_idr, gd->first_minor);
 out_put_disk:
 	put_disk(dev->gd);
 out_free_dev:
@@ -463,6 +474,7 @@ static void ubiblock_cleanup(struct ubiblock *dev)
 	blk_cleanup_queue(dev->rq);
 	blk_mq_free_tag_set(&dev->tag_set);
 	dev_info(disk_to_dev(dev->gd), "released");
+	idr_remove(&ubiblock_minor_idr, dev->gd->first_minor);
 	put_disk(dev->gd);
 }
 
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index b7f824d5ee88..22fd19c0c5d3 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -83,8 +83,6 @@ static struct mtd_dev_param __initdata mtd_dev_param[UBI_MAX_DEVICES];
 static bool fm_autoconvert;
 static bool fm_debug;
 #endif
-/* Root UBI "class" object (corresponds to '/<sysfs>/class/ubi/') */
-struct class *ubi_class;
 
 /* Slab cache for wear-leveling entries */
 struct kmem_cache *ubi_wl_entry_slab;
@@ -113,8 +111,17 @@ static ssize_t ubi_version_show(struct class *class,
 }
 
 /* UBI version attribute ('/<sysfs>/class/ubi/version') */
-static struct class_attribute ubi_version =
-	__ATTR(version, S_IRUGO, ubi_version_show, NULL);
+static struct class_attribute ubi_class_attrs[] = {
+	__ATTR(version, S_IRUGO, ubi_version_show, NULL),
+	__ATTR_NULL
+};
+
+/* Root UBI "class" object (corresponds to '/<sysfs>/class/ubi/') */
+struct class ubi_class = {
+	.name		= UBI_NAME_STR,
+	.owner		= THIS_MODULE,
+	.class_attrs	= ubi_class_attrs,
+};
 
 static ssize_t dev_attribute_show(struct device *dev,
 				  struct device_attribute *attr, char *buf);
@@ -385,6 +392,22 @@ static ssize_t dev_attribute_show(struct device *dev,
 	return ret;
 }
 
+static struct attribute *ubi_dev_attrs[] = {
+	&dev_eraseblock_size.attr,
+	&dev_avail_eraseblocks.attr,
+	&dev_total_eraseblocks.attr,
+	&dev_volumes_count.attr,
+	&dev_max_ec.attr,
+	&dev_reserved_for_bad.attr,
+	&dev_bad_peb_count.attr,
+	&dev_max_vol_count.attr,
+	&dev_min_io_size.attr,
+	&dev_bgt_enabled.attr,
+	&dev_mtd_num.attr,
+	NULL
+};
+ATTRIBUTE_GROUPS(ubi_dev);
+
 static void dev_release(struct device *dev)
 {
 	struct ubi_device *ubi = container_of(dev, struct ubi_device, dev);
@@ -407,45 +430,15 @@ static int ubi_sysfs_init(struct ubi_device *ubi, int *ref)
 
 	ubi->dev.release = dev_release;
 	ubi->dev.devt = ubi->cdev.dev;
-	ubi->dev.class = ubi_class;
+	ubi->dev.class = &ubi_class;
+	ubi->dev.groups = ubi_dev_groups;
 	dev_set_name(&ubi->dev, UBI_NAME_STR"%d", ubi->ubi_num);
 	err = device_register(&ubi->dev);
 	if (err)
 		return err;
 
 	*ref = 1;
-	err = device_create_file(&ubi->dev, &dev_eraseblock_size);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_avail_eraseblocks);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_total_eraseblocks);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_volumes_count);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_max_ec);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_reserved_for_bad);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_bad_peb_count);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_max_vol_count);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_min_io_size);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_bgt_enabled);
-	if (err)
-		return err;
-	err = device_create_file(&ubi->dev, &dev_mtd_num);
-	return err;
+	return 0;
 }
 
 /**
@@ -454,17 +447,6 @@ static int ubi_sysfs_init(struct ubi_device *ubi, int *ref)
  */
 static void ubi_sysfs_close(struct ubi_device *ubi)
 {
-	device_remove_file(&ubi->dev, &dev_mtd_num);
-	device_remove_file(&ubi->dev, &dev_bgt_enabled);
-	device_remove_file(&ubi->dev, &dev_min_io_size);
-	device_remove_file(&ubi->dev, &dev_max_vol_count);
-	device_remove_file(&ubi->dev, &dev_bad_peb_count);
-	device_remove_file(&ubi->dev, &dev_reserved_for_bad);
-	device_remove_file(&ubi->dev, &dev_max_ec);
-	device_remove_file(&ubi->dev, &dev_volumes_count);
-	device_remove_file(&ubi->dev, &dev_total_eraseblocks);
-	device_remove_file(&ubi->dev, &dev_avail_eraseblocks);
-	device_remove_file(&ubi->dev, &dev_eraseblock_size);
 	device_unregister(&ubi->dev);
 }
 
@@ -947,8 +929,8 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 	 */
 	ubi->fm_pool.max_size = min(((int)mtd_div_by_eb(ubi->mtd->size,
 		ubi->mtd) / 100) * 5, UBI_FM_MAX_POOL_SIZE);
-	if (ubi->fm_pool.max_size < UBI_FM_MIN_POOL_SIZE)
-		ubi->fm_pool.max_size = UBI_FM_MIN_POOL_SIZE;
+	ubi->fm_pool.max_size = max(ubi->fm_pool.max_size,
+		UBI_FM_MIN_POOL_SIZE);
 
 	ubi->fm_wl_pool.max_size = ubi->fm_pool.max_size / 2;
 	ubi->fm_disabled = !fm_autoconvert;
@@ -1233,23 +1215,14 @@ static int __init ubi_init(void)
 	}
 
 	/* Create base sysfs directory and sysfs files */
-	ubi_class = class_create(THIS_MODULE, UBI_NAME_STR);
-	if (IS_ERR(ubi_class)) {
-		err = PTR_ERR(ubi_class);
-		pr_err("UBI error: cannot create UBI class");
-		goto out;
-	}
-
-	err = class_create_file(ubi_class, &ubi_version);
-	if (err) {
-		pr_err("UBI error: cannot create sysfs file");
-		goto out_class;
-	}
+	err = class_register(&ubi_class);
+	if (err < 0)
+		return err;
 
 	err = misc_register(&ubi_ctrl_cdev);
 	if (err) {
 		pr_err("UBI error: cannot register device");
-		goto out_version;
+		goto out;
 	}
 
 	ubi_wl_entry_slab = kmem_cache_create("ubi_wl_entry_slab",
@@ -1333,11 +1306,8 @@ out_slab:
 	kmem_cache_destroy(ubi_wl_entry_slab);
 out_dev_unreg:
 	misc_deregister(&ubi_ctrl_cdev);
-out_version:
-	class_remove_file(ubi_class, &ubi_version);
-out_class:
-	class_destroy(ubi_class);
 out:
+	class_unregister(&ubi_class);
 	pr_err("UBI error: cannot initialize UBI, error %d", err);
 	return err;
 }
@@ -1358,8 +1328,7 @@ static void __exit ubi_exit(void)
 	ubi_debugfs_exit();
 	kmem_cache_destroy(ubi_wl_entry_slab);
 	misc_deregister(&ubi_ctrl_cdev);
-	class_remove_file(ubi_class, &ubi_version);
-	class_destroy(ubi_class);
+	class_unregister(&ubi_class);
 }
 module_exit(ubi_exit);
 
diff --git a/drivers/mtd/ubi/fastmap.c b/drivers/mtd/ubi/fastmap.c
index 02a6de2f53ee..4aa2fd8633e7 100644
--- a/drivers/mtd/ubi/fastmap.c
+++ b/drivers/mtd/ubi/fastmap.c
@@ -88,13 +88,13 @@ size_t ubi_calc_fm_size(struct ubi_device *ubi)
 {
 	size_t size;
 
-	size = sizeof(struct ubi_fm_sb) + \
-		sizeof(struct ubi_fm_hdr) + \
-		sizeof(struct ubi_fm_scan_pool) + \
-		sizeof(struct ubi_fm_scan_pool) + \
-		(ubi->peb_count * sizeof(struct ubi_fm_ec)) + \
-		(sizeof(struct ubi_fm_eba) + \
-		(ubi->peb_count * sizeof(__be32))) + \
+	size = sizeof(struct ubi_fm_sb) +
+		sizeof(struct ubi_fm_hdr) +
+		sizeof(struct ubi_fm_scan_pool) +
+		sizeof(struct ubi_fm_scan_pool) +
+		(ubi->peb_count * sizeof(struct ubi_fm_ec)) +
+		(sizeof(struct ubi_fm_eba) +
+		(ubi->peb_count * sizeof(__be32))) +
 		sizeof(struct ubi_fm_volhdr) * UBI_MAX_VOLUMES;
 	return roundup(size, ubi->leb_size);
 }
@@ -192,8 +192,10 @@ static struct ubi_ainf_volume *add_vol(struct ubi_attach_info *ai, int vol_id,
 
 		if (vol_id > av->vol_id)
 			p = &(*p)->rb_left;
-		else
+		else if (vol_id < av->vol_id)
 			p = &(*p)->rb_right;
+		else
+			return ERR_PTR(-EINVAL);
 	}
 
 	av = kmalloc(sizeof(struct ubi_ainf_volume), GFP_KERNEL);
@@ -314,7 +316,7 @@ static int update_vol(struct ubi_device *ubi, struct ubi_attach_info *ai,
 			list_add_tail(&victim->u.list, &ai->erase);
 
 			if (av->highest_lnum == be32_to_cpu(new_vh->lnum))
-				av->last_data_size = \
+				av->last_data_size =
 					be32_to_cpu(new_vh->data_size);
 
 			dbg_bld("vol %i: AEB %i's PEB %i is the newer",
@@ -601,7 +603,7 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 	struct ubi_ainf_peb *aeb, *tmp_aeb, *_tmp_aeb;
 	struct ubi_fm_sb *fmsb;
 	struct ubi_fm_hdr *fmhdr;
-	struct ubi_fm_scan_pool *fmpl1, *fmpl2;
+	struct ubi_fm_scan_pool *fmpl, *fmpl_wl;
 	struct ubi_fm_ec *fmec;
 	struct ubi_fm_volhdr *fmvhdr;
 	struct ubi_fm_eba *fm_eba;
@@ -631,30 +633,30 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 		goto fail_bad;
 	}
 
-	fmpl1 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
-	fm_pos += sizeof(*fmpl1);
+	fmpl = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmpl);
 	if (fm_pos >= fm_size)
 		goto fail_bad;
-	if (be32_to_cpu(fmpl1->magic) != UBI_FM_POOL_MAGIC) {
+	if (be32_to_cpu(fmpl->magic) != UBI_FM_POOL_MAGIC) {
 		ubi_err(ubi, "bad fastmap pool magic: 0x%x, expected: 0x%x",
-			be32_to_cpu(fmpl1->magic), UBI_FM_POOL_MAGIC);
+			be32_to_cpu(fmpl->magic), UBI_FM_POOL_MAGIC);
 		goto fail_bad;
 	}
 
-	fmpl2 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
-	fm_pos += sizeof(*fmpl2);
+	fmpl_wl = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmpl_wl);
 	if (fm_pos >= fm_size)
 		goto fail_bad;
-	if (be32_to_cpu(fmpl2->magic) != UBI_FM_POOL_MAGIC) {
-		ubi_err(ubi, "bad fastmap pool magic: 0x%x, expected: 0x%x",
-			be32_to_cpu(fmpl2->magic), UBI_FM_POOL_MAGIC);
+	if (be32_to_cpu(fmpl_wl->magic) != UBI_FM_POOL_MAGIC) {
+		ubi_err(ubi, "bad fastmap WL pool magic: 0x%x, expected: 0x%x",
+			be32_to_cpu(fmpl_wl->magic), UBI_FM_POOL_MAGIC);
 		goto fail_bad;
 	}
 
-	pool_size = be16_to_cpu(fmpl1->size);
-	wl_pool_size = be16_to_cpu(fmpl2->size);
-	fm->max_pool_size = be16_to_cpu(fmpl1->max_size);
-	fm->max_wl_pool_size = be16_to_cpu(fmpl2->max_size);
+	pool_size = be16_to_cpu(fmpl->size);
+	wl_pool_size = be16_to_cpu(fmpl_wl->size);
+	fm->max_pool_size = be16_to_cpu(fmpl->max_size);
+	fm->max_wl_pool_size = be16_to_cpu(fmpl_wl->max_size);
 
 	if (pool_size > UBI_FM_MAX_POOL_SIZE || pool_size < 0) {
 		ubi_err(ubi, "bad pool size: %i", pool_size);
@@ -748,6 +750,11 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 
 		if (!av)
 			goto fail_bad;
+		if (PTR_ERR(av) == -EINVAL) {
+			ubi_err(ubi, "volume (ID %i) already exists",
+				fmvhdr->vol_id);
+			goto fail_bad;
+		}
 
 		ai->vols_found++;
 		if (ai->highest_vol_id < be32_to_cpu(fmvhdr->vol_id))
@@ -796,11 +803,11 @@ static int ubi_attach_fastmap(struct ubi_device *ubi,
 		}
 	}
 
-	ret = scan_pool(ubi, ai, fmpl1->pebs, pool_size, &max_sqnum, &free);
+	ret = scan_pool(ubi, ai, fmpl->pebs, pool_size, &max_sqnum, &free);
 	if (ret)
 		goto fail;
 
-	ret = scan_pool(ubi, ai, fmpl2->pebs, wl_pool_size, &max_sqnum, &free);
+	ret = scan_pool(ubi, ai, fmpl_wl->pebs, wl_pool_size, &max_sqnum, &free);
 	if (ret)
 		goto fail;
 
@@ -1083,7 +1090,7 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	void *fm_raw;
 	struct ubi_fm_sb *fmsb;
 	struct ubi_fm_hdr *fmh;
-	struct ubi_fm_scan_pool *fmpl1, *fmpl2;
+	struct ubi_fm_scan_pool *fmpl, *fmpl_wl;
 	struct ubi_fm_ec *fec;
 	struct ubi_fm_volhdr *fvh;
 	struct ubi_fm_eba *feba;
@@ -1141,25 +1148,25 @@ static int ubi_write_fastmap(struct ubi_device *ubi,
 	erase_peb_count = 0;
 	vol_count = 0;
 
-	fmpl1 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
-	fm_pos += sizeof(*fmpl1);
-	fmpl1->magic = cpu_to_be32(UBI_FM_POOL_MAGIC);
-	fmpl1->size = cpu_to_be16(ubi->fm_pool.size);
-	fmpl1->max_size = cpu_to_be16(ubi->fm_pool.max_size);
+	fmpl = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmpl);
+	fmpl->magic = cpu_to_be32(UBI_FM_POOL_MAGIC);
+	fmpl->size = cpu_to_be16(ubi->fm_pool.size);
+	fmpl->max_size = cpu_to_be16(ubi->fm_pool.max_size);
 
 	for (i = 0; i < ubi->fm_pool.size; i++) {
-		fmpl1->pebs[i] = cpu_to_be32(ubi->fm_pool.pebs[i]);
+		fmpl->pebs[i] = cpu_to_be32(ubi->fm_pool.pebs[i]);
 		set_seen(ubi, ubi->fm_pool.pebs[i], seen_pebs);
 	}
 
-	fmpl2 = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
-	fm_pos += sizeof(*fmpl2);
-	fmpl2->magic = cpu_to_be32(UBI_FM_POOL_MAGIC);
-	fmpl2->size = cpu_to_be16(ubi->fm_wl_pool.size);
-	fmpl2->max_size = cpu_to_be16(ubi->fm_wl_pool.max_size);
+	fmpl_wl = (struct ubi_fm_scan_pool *)(fm_raw + fm_pos);
+	fm_pos += sizeof(*fmpl_wl);
+	fmpl_wl->magic = cpu_to_be32(UBI_FM_POOL_MAGIC);
+	fmpl_wl->size = cpu_to_be16(ubi->fm_wl_pool.size);
+	fmpl_wl->max_size = cpu_to_be16(ubi->fm_wl_pool.max_size);
 
 	for (i = 0; i < ubi->fm_wl_pool.size; i++) {
-		fmpl2->pebs[i] = cpu_to_be32(ubi->fm_wl_pool.pebs[i]);
+		fmpl_wl->pebs[i] = cpu_to_be32(ubi->fm_wl_pool.pebs[i]);
 		set_seen(ubi, ubi->fm_wl_pool.pebs[i], seen_pebs);
 	}
 
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index c998212fc680..2974b67f6c6c 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -775,7 +775,7 @@ extern struct kmem_cache *ubi_wl_entry_slab;
 extern const struct file_operations ubi_ctrl_cdev_operations;
 extern const struct file_operations ubi_cdev_operations;
 extern const struct file_operations ubi_vol_cdev_operations;
-extern struct class *ubi_class;
+extern struct class ubi_class;
 extern struct mutex ubi_devices_mutex;
 extern struct blocking_notifier_head ubi_notifiers;
 
diff --git a/drivers/mtd/ubi/vmt.c b/drivers/mtd/ubi/vmt.c
index ff4d97848d1c..1ae17bb9b889 100644
--- a/drivers/mtd/ubi/vmt.c
+++ b/drivers/mtd/ubi/vmt.c
@@ -120,6 +120,19 @@ static ssize_t vol_attribute_show(struct device *dev,
 	return ret;
 }
 
+static struct attribute *volume_dev_attrs[] = {
+	&attr_vol_reserved_ebs.attr,
+	&attr_vol_type.attr,
+	&attr_vol_name.attr,
+	&attr_vol_corrupted.attr,
+	&attr_vol_alignment.attr,
+	&attr_vol_usable_eb_size.attr,
+	&attr_vol_data_bytes.attr,
+	&attr_vol_upd_marker.attr,
+	NULL
+};
+ATTRIBUTE_GROUPS(volume_dev);
+
 /* Release method for volume devices */
 static void vol_release(struct device *dev)
 {
@@ -130,64 +143,6 @@ static void vol_release(struct device *dev)
 }
 
 /**
- * volume_sysfs_init - initialize sysfs for new volume.
- * @ubi: UBI device description object
- * @vol: volume description object
- *
- * This function returns zero in case of success and a negative error code in
- * case of failure.
- *
- * Note, this function does not free allocated resources in case of failure -
- * the caller does it. This is because this would cause release() here and the
- * caller would oops.
- */
-static int volume_sysfs_init(struct ubi_device *ubi, struct ubi_volume *vol)
-{
-	int err;
-
-	err = device_create_file(&vol->dev, &attr_vol_reserved_ebs);
-	if (err)
-		return err;
-	err = device_create_file(&vol->dev, &attr_vol_type);
-	if (err)
-		return err;
-	err = device_create_file(&vol->dev, &attr_vol_name);
-	if (err)
-		return err;
-	err = device_create_file(&vol->dev, &attr_vol_corrupted);
-	if (err)
-		return err;
-	err = device_create_file(&vol->dev, &attr_vol_alignment);
-	if (err)
-		return err;
-	err = device_create_file(&vol->dev, &attr_vol_usable_eb_size);
-	if (err)
-		return err;
-	err = device_create_file(&vol->dev, &attr_vol_data_bytes);
-	if (err)
-		return err;
-	err = device_create_file(&vol->dev, &attr_vol_upd_marker);
-	return err;
-}
-
-/**
- * volume_sysfs_close - close sysfs for a volume.
- * @vol: volume description object
- */
-static void volume_sysfs_close(struct ubi_volume *vol)
-{
-	device_remove_file(&vol->dev, &attr_vol_upd_marker);
-	device_remove_file(&vol->dev, &attr_vol_data_bytes);
-	device_remove_file(&vol->dev, &attr_vol_usable_eb_size);
-	device_remove_file(&vol->dev, &attr_vol_alignment);
-	device_remove_file(&vol->dev, &attr_vol_corrupted);
-	device_remove_file(&vol->dev, &attr_vol_name);
-	device_remove_file(&vol->dev, &attr_vol_type);
-	device_remove_file(&vol->dev, &attr_vol_reserved_ebs);
-	device_unregister(&vol->dev);
-}
-
-/**
  * ubi_create_volume - create volume.
  * @ubi: UBI device description object
  * @req: volume creation request
@@ -253,8 +208,8 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 
 	/* Calculate how many eraseblocks are requested */
 	vol->usable_leb_size = ubi->leb_size - ubi->leb_size % req->alignment;
-	vol->reserved_pebs += div_u64(req->bytes + vol->usable_leb_size - 1,
-				      vol->usable_leb_size);
+	vol->reserved_pebs = div_u64(req->bytes + vol->usable_leb_size - 1,
+				     vol->usable_leb_size);
 
 	/* Reserve physical eraseblocks */
 	if (vol->reserved_pebs > ubi->avail_pebs) {
@@ -323,7 +278,8 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	vol->dev.release = vol_release;
 	vol->dev.parent = &ubi->dev;
 	vol->dev.devt = dev;
-	vol->dev.class = ubi_class;
+	vol->dev.class = &ubi_class;
+	vol->dev.groups = volume_dev_groups;
 
 	dev_set_name(&vol->dev, "%s_%d", ubi->ubi_name, vol->vol_id);
 	err = device_register(&vol->dev);
@@ -332,10 +288,6 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 		goto out_cdev;
 	}
 
-	err = volume_sysfs_init(ubi, vol);
-	if (err)
-		goto out_sysfs;
-
 	/* Fill volume table record */
 	memset(&vtbl_rec, 0, sizeof(struct ubi_vtbl_record));
 	vtbl_rec.reserved_pebs = cpu_to_be32(vol->reserved_pebs);
@@ -372,7 +324,7 @@ out_sysfs:
 	 */
 	do_free = 0;
 	get_device(&vol->dev);
-	volume_sysfs_close(vol);
+	device_unregister(&vol->dev);
 out_cdev:
 	cdev_del(&vol->cdev);
 out_mapping:
@@ -440,7 +392,7 @@ int ubi_remove_volume(struct ubi_volume_desc *desc, int no_vtbl)
 	}
 
 	cdev_del(&vol->cdev);
-	volume_sysfs_close(vol);
+	device_unregister(&vol->dev);
 
 	spin_lock(&ubi->volumes_lock);
 	ubi->rsvd_pebs -= reserved_pebs;
@@ -653,19 +605,13 @@ int ubi_add_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 	vol->dev.release = vol_release;
 	vol->dev.parent = &ubi->dev;
 	vol->dev.devt = dev;
-	vol->dev.class = ubi_class;
+	vol->dev.class = &ubi_class;
+	vol->dev.groups = volume_dev_groups;
 	dev_set_name(&vol->dev, "%s_%d", ubi->ubi_name, vol->vol_id);
 	err = device_register(&vol->dev);
 	if (err)
 		goto out_cdev;
 
-	err = volume_sysfs_init(ubi, vol);
-	if (err) {
-		cdev_del(&vol->cdev);
-		volume_sysfs_close(vol);
-		return err;
-	}
-
 	self_check_volumes(ubi);
 	return err;
 
@@ -688,7 +634,7 @@ void ubi_free_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 
 	ubi->volumes[vol->vol_id] = NULL;
 	cdev_del(&vol->cdev);
-	volume_sysfs_close(vol);
+	device_unregister(&vol->dev);
 }
 
 /**
diff --git a/drivers/mtd/ubi/vtbl.c b/drivers/mtd/ubi/vtbl.c
index 68c9c5ea676f..80bdd5b88bac 100644
--- a/drivers/mtd/ubi/vtbl.c
+++ b/drivers/mtd/ubi/vtbl.c
@@ -70,6 +70,26 @@ static void self_vtbl_check(const struct ubi_device *ubi);
 static struct ubi_vtbl_record empty_vtbl_record;
 
 /**
+ * ubi_update_layout_vol - helper for updatting layout volumes on flash
+ * @ubi: UBI device description object
+ */
+static int ubi_update_layout_vol(struct ubi_device *ubi)
+{
+	struct ubi_volume *layout_vol;
+	int i, err;
+
+	layout_vol = ubi->volumes[vol_id2idx(ubi, UBI_LAYOUT_VOLUME_ID)];
+	for (i = 0; i < UBI_LAYOUT_VOLUME_EBS; i++) {
+		err = ubi_eba_atomic_leb_change(ubi, layout_vol, i, ubi->vtbl,
+						ubi->vtbl_size);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+/**
  * ubi_change_vtbl_record - change volume table record.
  * @ubi: UBI device description object
  * @idx: table index to change
@@ -83,12 +103,10 @@ static struct ubi_vtbl_record empty_vtbl_record;
 int ubi_change_vtbl_record(struct ubi_device *ubi, int idx,
 			   struct ubi_vtbl_record *vtbl_rec)
 {
-	int i, err;
+	int err;
 	uint32_t crc;
-	struct ubi_volume *layout_vol;
 
 	ubi_assert(idx >= 0 && idx < ubi->vtbl_slots);
-	layout_vol = ubi->volumes[vol_id2idx(ubi, UBI_LAYOUT_VOLUME_ID)];
 
 	if (!vtbl_rec)
 		vtbl_rec = &empty_vtbl_record;
@@ -98,15 +116,10 @@ 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_atomic_leb_change(ubi, layout_vol, i, ubi->vtbl,
-						ubi->vtbl_size);
-		if (err)
-			return err;
-	}
+	err = ubi_update_layout_vol(ubi);
 
 	self_vtbl_check(ubi);
-	return 0;
+	return err ? err : 0;
 }
 
 /**
@@ -121,9 +134,7 @@ int ubi_change_vtbl_record(struct ubi_device *ubi, int idx,
 int ubi_vtbl_rename_volumes(struct ubi_device *ubi,
 			    struct list_head *rename_list)
 {
-	int i, err;
 	struct ubi_rename_entry *re;
-	struct ubi_volume *layout_vol;
 
 	list_for_each_entry(re, rename_list, list) {
 		uint32_t crc;
@@ -145,15 +156,7 @@ int ubi_vtbl_rename_volumes(struct ubi_device *ubi,
 		vtbl_rec->crc = cpu_to_be32(crc);
 	}
 
-	layout_vol = ubi->volumes[vol_id2idx(ubi, UBI_LAYOUT_VOLUME_ID)];
-	for (i = 0; i < UBI_LAYOUT_VOLUME_EBS; i++) {
-		err = ubi_eba_atomic_leb_change(ubi, layout_vol, i, ubi->vtbl,
-						ubi->vtbl_size);
-		if (err)
-			return err;
-	}
-
-	return 0;
+	return ubi_update_layout_vol(ubi);
 }
 
 /**
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 16214d3d57a4..275d9fb6fe5c 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -1581,7 +1581,7 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	dbg_wl("found %i PEBs", found_pebs);
 
 	if (ubi->fm) {
-		ubi_assert(ubi->good_peb_count == \
+		ubi_assert(ubi->good_peb_count ==
 			   found_pebs + ubi->fm->used_blocks);
 
 		for (i = 0; i < ubi->fm->used_blocks; i++) {
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index df51d6025a90..c18f9e62a9fa 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -179,6 +179,20 @@ config VXLAN
 	  To compile this driver as a module, choose M here: the module
 	  will be called vxlan.
 
+config GENEVE
+       tristate "Generic Network Virtualization Encapsulation netdev"
+       depends on INET && GENEVE_CORE
+       select NET_IP_TUNNEL
+       ---help---
+	  This allows one to create geneve virtual interfaces that provide
+	  Layer 2 Networks over Layer 3 Networks. GENEVE is often used
+	  to tunnel virtual network infrastructure in virtualized environments.
+	  For more information see:
+	    http://tools.ietf.org/html/draft-gross-geneve-02
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called geneve.
+
 config NETCONSOLE
 	tristate "Network console logging support"
 	---help---
@@ -203,8 +217,8 @@ config NET_POLL_CONTROLLER
 	def_bool NETPOLL
 
 config NTB_NETDEV
-	tristate "Virtual Ethernet over NTB"
-	depends on NTB
+	tristate "Virtual Ethernet over NTB Transport"
+	depends on NTB_TRANSPORT
 
 config RIONET
 	tristate "RapidIO Ethernet over messaging driver support"
@@ -244,6 +258,20 @@ config TUN
 
 	  If you don't know what to use this for, you don't need it.
 
+config TUN_VNET_CROSS_LE
+	bool "Support for cross-endian vnet headers on little-endian kernels"
+	default n
+	---help---
+	  This option allows TUN/TAP and MACVTAP device drivers in a
+	  little-endian kernel to parse vnet headers that come from a
+	  big-endian legacy virtio device.
+
+	  Userspace programs can control the feature using the TUNSETVNETBE
+	  and TUNGETVNETBE ioctls.
+
+	  Unless you have a little-endian system hosting a big-endian virtual
+	  machine with a legacy virtio NIC, you should say N.
+
 config VETH
 	tristate "Virtual ethernet pair device"
 	---help---
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index e25fdd7d905e..c12cb22478a7 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -23,6 +23,7 @@ obj-$(CONFIG_TUN) += tun.o
 obj-$(CONFIG_VETH) += veth.o
 obj-$(CONFIG_VIRTIO_NET) += virtio_net.o
 obj-$(CONFIG_VXLAN) += vxlan.o
+obj-$(CONFIG_GENEVE) += geneve.o
 obj-$(CONFIG_NLMON) += nlmon.o
 
 #
diff --git a/drivers/net/arcnet/Kconfig b/drivers/net/arcnet/Kconfig
index 84fb6349a59a..2a9c3c3abe9b 100644
--- a/drivers/net/arcnet/Kconfig
+++ b/drivers/net/arcnet/Kconfig
@@ -15,10 +15,6 @@ menuconfig ARCNET
 	  COM90xx type card, so say Y (or M) to "ARCnet COM90xx chipset
 	  support" below.
 
-	  You might also want to have a look at the Ethernet-HOWTO, available
-	  from <http://www.tldp.org/docs.html#howto>(even though ARCnet
-	  is not really Ethernet).
-
 	  To compile this driver as a module, choose M here. The module will
 	  be called arcnet.
 
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index fbd54f0e32e8..7fde4d5c2b28 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -75,10 +75,10 @@
 /* Port Key definitions
  * key is determined according to the link speed, duplex and
  * user key (which is yet not supported)
- * --------------------------------------------------------------
- * Port key :	| User key	| Speed		| Duplex	|
- * --------------------------------------------------------------
- * 16		  6		  1		  0
+ *           --------------------------------------------------------------
+ * Port key  | User key (10 bits)           | Speed (5 bits)      | Duplex|
+ *           --------------------------------------------------------------
+ *           |15                           6|5                   1|0
  */
 #define  AD_DUPLEX_KEY_MASKS    0x1
 #define  AD_SPEED_KEY_MASKS     0x3E
@@ -1908,8 +1908,14 @@ void bond_3ad_initialize(struct bonding *bond, u16 tick_resolution)
 
 		BOND_AD_INFO(bond).aggregator_identifier = 0;
 
-		BOND_AD_INFO(bond).system.sys_priority = 0xFFFF;
-		BOND_AD_INFO(bond).system.sys_mac_addr = *((struct mac_addr *)bond->dev->dev_addr);
+		BOND_AD_INFO(bond).system.sys_priority =
+			bond->params.ad_actor_sys_prio;
+		if (is_zero_ether_addr(bond->params.ad_actor_system))
+			BOND_AD_INFO(bond).system.sys_mac_addr =
+			    *((struct mac_addr *)bond->dev->dev_addr);
+		else
+			BOND_AD_INFO(bond).system.sys_mac_addr =
+			    *((struct mac_addr *)bond->params.ad_actor_system);
 
 		/* initialize how many times this module is called in one
 		 * second (should be about every 100ms)
@@ -1945,10 +1951,10 @@ void bond_3ad_bind_slave(struct slave *slave)
 
 		port->slave = slave;
 		port->actor_port_number = SLAVE_AD_INFO(slave)->id;
-		/* key is determined according to the link speed, duplex and user key(which
-		 * is yet not supported)
+		/* key is determined according to the link speed, duplex and
+		 * user key
 		 */
-		port->actor_admin_port_key = 0;
+		port->actor_admin_port_key = bond->params.ad_user_port_key << 6;
 		port->actor_admin_port_key |= __get_duplex(port);
 		port->actor_admin_port_key |= (__get_link_speed(port) << 1);
 		port->actor_oper_port_key = port->actor_admin_port_key;
@@ -1959,6 +1965,8 @@ void bond_3ad_bind_slave(struct slave *slave)
 			port->sm_vars &= ~AD_PORT_LACP_ENABLED;
 		/* actor system is the bond's system */
 		port->actor_system = BOND_AD_INFO(bond).system.sys_mac_addr;
+		port->actor_system_priority =
+		    BOND_AD_INFO(bond).system.sys_priority;
 		/* tx timer(to verify that no more than MAX_TX_IN_SECOND
 		 * lacpdu's are sent in one second)
 		 */
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index d5fe5d5f490f..19eb990d398c 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -76,7 +76,7 @@
 #include <net/netns/generic.h>
 #include <net/pkt_sched.h>
 #include <linux/rculist.h>
-#include <net/flow_keys.h>
+#include <net/flow_dissector.h>
 #include <net/switchdev.h>
 #include <net/bonding.h>
 #include <net/bond_3ad.h>
@@ -1015,10 +1015,7 @@ static netdev_features_t bond_fix_features(struct net_device *dev,
 	netdev_features_t mask;
 	struct slave *slave;
 
-	/* If any slave has the offload feature flag set,
-	 * set the offload flag on the bond.
-	 */
-	mask = features | NETIF_F_HW_SWITCH_OFFLOAD;
+	mask = features;
 
 	features &= ~NETIF_F_ONE_FOR_ALL;
 	features |= NETIF_F_ALL_FOR_ALL;
@@ -3054,16 +3051,15 @@ static bool bond_flow_dissect(struct bonding *bond, struct sk_buff *skb,
 	int noff, proto = -1;
 
 	if (bond->params.xmit_policy > BOND_XMIT_POLICY_LAYER23)
-		return skb_flow_dissect(skb, fk);
+		return skb_flow_dissect_flow_keys(skb, fk);
 
-	fk->ports = 0;
+	fk->ports.ports = 0;
 	noff = skb_network_offset(skb);
 	if (skb->protocol == htons(ETH_P_IP)) {
 		if (unlikely(!pskb_may_pull(skb, noff + sizeof(*iph))))
 			return false;
 		iph = ip_hdr(skb);
-		fk->src = iph->saddr;
-		fk->dst = iph->daddr;
+		iph_to_flow_copy_v4addrs(fk, iph);
 		noff += iph->ihl << 2;
 		if (!ip_is_fragment(iph))
 			proto = iph->protocol;
@@ -3071,15 +3067,14 @@ static bool bond_flow_dissect(struct bonding *bond, struct sk_buff *skb,
 		if (unlikely(!pskb_may_pull(skb, noff + sizeof(*iph6))))
 			return false;
 		iph6 = ipv6_hdr(skb);
-		fk->src = (__force __be32)ipv6_addr_hash(&iph6->saddr);
-		fk->dst = (__force __be32)ipv6_addr_hash(&iph6->daddr);
+		iph_to_flow_copy_v6addrs(fk, iph6);
 		noff += sizeof(*iph6);
 		proto = iph6->nexthdr;
 	} else {
 		return false;
 	}
 	if (bond->params.xmit_policy == BOND_XMIT_POLICY_LAYER34 && proto >= 0)
-		fk->ports = skb_flow_get_ports(skb, noff, proto);
+		fk->ports.ports = skb_flow_get_ports(skb, noff, proto);
 
 	return true;
 }
@@ -3105,8 +3100,9 @@ u32 bond_xmit_hash(struct bonding *bond, struct sk_buff *skb)
 	    bond->params.xmit_policy == BOND_XMIT_POLICY_ENCAP23)
 		hash = bond_eth_hash(skb);
 	else
-		hash = (__force u32)flow.ports;
-	hash ^= (__force u32)flow.dst ^ (__force u32)flow.src;
+		hash = (__force u32)flow.ports.ports;
+	hash ^= (__force u32)flow_get_u32_dst(&flow) ^
+		(__force u32)flow_get_u32_src(&flow);
 	hash ^= (hash >> 16);
 	hash ^= (hash >> 8);
 
@@ -4039,8 +4035,12 @@ static const struct net_device_ops bond_netdev_ops = {
 	.ndo_add_slave		= bond_enslave,
 	.ndo_del_slave		= bond_release,
 	.ndo_fix_features	= bond_fix_features,
-	.ndo_bridge_setlink	= ndo_dflt_netdev_switch_port_bridge_setlink,
-	.ndo_bridge_dellink	= ndo_dflt_netdev_switch_port_bridge_dellink,
+	.ndo_bridge_setlink	= switchdev_port_bridge_setlink,
+	.ndo_bridge_getlink	= switchdev_port_bridge_getlink,
+	.ndo_bridge_dellink	= switchdev_port_bridge_dellink,
+	.ndo_fdb_add		= switchdev_port_fdb_add,
+	.ndo_fdb_del		= switchdev_port_fdb_del,
+	.ndo_fdb_dump		= switchdev_port_fdb_dump,
 	.ndo_features_check	= passthru_features_check,
 };
 
@@ -4140,6 +4140,8 @@ static int bond_check_params(struct bond_params *params)
 	struct bond_opt_value newval;
 	const struct bond_opt_value *valptr;
 	int arp_all_targets_value;
+	u16 ad_actor_sys_prio = 0;
+	u16 ad_user_port_key = 0;
 
 	/* Convert string parameters. */
 	if (mode) {
@@ -4434,6 +4436,24 @@ static int bond_check_params(struct bond_params *params)
 		fail_over_mac_value = BOND_FOM_NONE;
 	}
 
+	bond_opt_initstr(&newval, "default");
+	valptr = bond_opt_parse(
+			bond_opt_get(BOND_OPT_AD_ACTOR_SYS_PRIO),
+				     &newval);
+	if (!valptr) {
+		pr_err("Error: No ad_actor_sys_prio default value");
+		return -EINVAL;
+	}
+	ad_actor_sys_prio = valptr->value;
+
+	valptr = bond_opt_parse(bond_opt_get(BOND_OPT_AD_USER_PORT_KEY),
+				&newval);
+	if (!valptr) {
+		pr_err("Error: No ad_user_port_key default value");
+		return -EINVAL;
+	}
+	ad_user_port_key = valptr->value;
+
 	if (lp_interval == 0) {
 		pr_warn("Warning: ip_interval must be between 1 and %d, so it was reset to %d\n",
 			INT_MAX, BOND_ALB_DEFAULT_LP_INTERVAL);
@@ -4462,6 +4482,9 @@ static int bond_check_params(struct bond_params *params)
 	params->lp_interval = lp_interval;
 	params->packets_per_slave = packets_per_slave;
 	params->tlb_dynamic_lb = 1; /* Default value */
+	params->ad_actor_sys_prio = ad_actor_sys_prio;
+	eth_zero_addr(params->ad_actor_system);
+	params->ad_user_port_key = ad_user_port_key;
 	if (packets_per_slave > 0) {
 		params->reciprocal_packets_per_slave =
 			reciprocal_value(packets_per_slave);
diff --git a/drivers/net/bonding/bond_netlink.c b/drivers/net/bonding/bond_netlink.c
index 7b1124366011..1bda29249d12 100644
--- a/drivers/net/bonding/bond_netlink.c
+++ b/drivers/net/bonding/bond_netlink.c
@@ -28,6 +28,8 @@ static size_t bond_get_slave_size(const struct net_device *bond_dev,
 		nla_total_size(MAX_ADDR_LEN) +	/* IFLA_BOND_SLAVE_PERM_HWADDR */
 		nla_total_size(sizeof(u16)) +	/* IFLA_BOND_SLAVE_QUEUE_ID */
 		nla_total_size(sizeof(u16)) +	/* IFLA_BOND_SLAVE_AD_AGGREGATOR_ID */
+		nla_total_size(sizeof(u8)) +	/* IFLA_BOND_SLAVE_AD_ACTOR_OPER_PORT_STATE */
+		nla_total_size(sizeof(u16)) +	/* IFLA_BOND_SLAVE_AD_PARTNER_OPER_PORT_STATE */
 		0;
 }
 
@@ -56,12 +58,23 @@ static int bond_fill_slave_info(struct sk_buff *skb,
 
 	if (BOND_MODE(slave->bond) == BOND_MODE_8023AD) {
 		const struct aggregator *agg;
+		const struct port *ad_port;
 
+		ad_port = &SLAVE_AD_INFO(slave)->port;
 		agg = SLAVE_AD_INFO(slave)->port.aggregator;
-		if (agg)
+		if (agg) {
 			if (nla_put_u16(skb, IFLA_BOND_SLAVE_AD_AGGREGATOR_ID,
 					agg->aggregator_identifier))
 				goto nla_put_failure;
+			if (nla_put_u8(skb,
+				       IFLA_BOND_SLAVE_AD_ACTOR_OPER_PORT_STATE,
+				       ad_port->actor_oper_port_state))
+				goto nla_put_failure;
+			if (nla_put_u16(skb,
+					IFLA_BOND_SLAVE_AD_PARTNER_OPER_PORT_STATE,
+					ad_port->partner_oper.port_state))
+				goto nla_put_failure;
+		}
 	}
 
 	return 0;
@@ -94,6 +107,10 @@ static const struct nla_policy bond_policy[IFLA_BOND_MAX + 1] = {
 	[IFLA_BOND_AD_LACP_RATE]	= { .type = NLA_U8 },
 	[IFLA_BOND_AD_SELECT]		= { .type = NLA_U8 },
 	[IFLA_BOND_AD_INFO]		= { .type = NLA_NESTED },
+	[IFLA_BOND_AD_ACTOR_SYS_PRIO]	= { .type = NLA_U16 },
+	[IFLA_BOND_AD_USER_PORT_KEY]	= { .type = NLA_U16 },
+	[IFLA_BOND_AD_ACTOR_SYSTEM]	= { .type = NLA_BINARY,
+					    .len  = ETH_ALEN },
 };
 
 static const struct nla_policy bond_slave_policy[IFLA_BOND_SLAVE_MAX + 1] = {
@@ -379,6 +396,36 @@ static int bond_changelink(struct net_device *bond_dev,
 		if (err)
 			return err;
 	}
+	if (data[IFLA_BOND_AD_ACTOR_SYS_PRIO]) {
+		int actor_sys_prio =
+			nla_get_u16(data[IFLA_BOND_AD_ACTOR_SYS_PRIO]);
+
+		bond_opt_initval(&newval, actor_sys_prio);
+		err = __bond_opt_set(bond, BOND_OPT_AD_ACTOR_SYS_PRIO, &newval);
+		if (err)
+			return err;
+	}
+
+	if (data[IFLA_BOND_AD_USER_PORT_KEY]) {
+		int port_key =
+			nla_get_u16(data[IFLA_BOND_AD_USER_PORT_KEY]);
+
+		bond_opt_initval(&newval, port_key);
+		err = __bond_opt_set(bond, BOND_OPT_AD_USER_PORT_KEY, &newval);
+		if (err)
+			return err;
+	}
+
+	if (data[IFLA_BOND_AD_ACTOR_SYSTEM]) {
+		if (nla_len(data[IFLA_BOND_AD_ACTOR_SYSTEM]) != ETH_ALEN)
+			return -EINVAL;
+
+		bond_opt_initval(&newval,
+				 nla_get_be64(data[IFLA_BOND_AD_ACTOR_SYSTEM]));
+		err = __bond_opt_set(bond, BOND_OPT_AD_ACTOR_SYSTEM, &newval);
+		if (err)
+			return err;
+	}
 	return 0;
 }
 
@@ -426,6 +473,9 @@ static size_t bond_get_size(const struct net_device *bond_dev)
 		nla_total_size(sizeof(u16)) + /* IFLA_BOND_AD_INFO_ACTOR_KEY */
 		nla_total_size(sizeof(u16)) + /* IFLA_BOND_AD_INFO_PARTNER_KEY*/
 		nla_total_size(ETH_ALEN) +    /* IFLA_BOND_AD_INFO_PARTNER_MAC*/
+		nla_total_size(sizeof(u16)) + /* IFLA_BOND_AD_ACTOR_SYS_PRIO */
+		nla_total_size(sizeof(u16)) + /* IFLA_BOND_AD_USER_PORT_KEY */
+		nla_total_size(ETH_ALEN) + /* IFLA_BOND_AD_ACTOR_SYSTEM */
 		0;
 }
 
@@ -551,6 +601,20 @@ static int bond_fill_info(struct sk_buff *skb,
 	if (BOND_MODE(bond) == BOND_MODE_8023AD) {
 		struct ad_info info;
 
+		if (capable(CAP_NET_ADMIN)) {
+			if (nla_put_u16(skb, IFLA_BOND_AD_ACTOR_SYS_PRIO,
+					bond->params.ad_actor_sys_prio))
+				goto nla_put_failure;
+
+			if (nla_put_u16(skb, IFLA_BOND_AD_USER_PORT_KEY,
+					bond->params.ad_user_port_key))
+				goto nla_put_failure;
+
+			if (nla_put(skb, IFLA_BOND_AD_ACTOR_SYSTEM,
+				    sizeof(bond->params.ad_actor_system),
+				    &bond->params.ad_actor_system))
+				goto nla_put_failure;
+		}
 		if (!bond_3ad_get_active_agg_info(bond, &info)) {
 			struct nlattr *nest;
 
diff --git a/drivers/net/bonding/bond_options.c b/drivers/net/bonding/bond_options.c
index e8d3c1d35453..e9c624d54dd4 100644
--- a/drivers/net/bonding/bond_options.c
+++ b/drivers/net/bonding/bond_options.c
@@ -70,6 +70,12 @@ static int bond_option_slaves_set(struct bonding *bond,
 				  const struct bond_opt_value *newval);
 static int bond_option_tlb_dynamic_lb_set(struct bonding *bond,
 				  const struct bond_opt_value *newval);
+static int bond_option_ad_actor_sys_prio_set(struct bonding *bond,
+					     const struct bond_opt_value *newval);
+static int bond_option_ad_actor_system_set(struct bonding *bond,
+					   const struct bond_opt_value *newval);
+static int bond_option_ad_user_port_key_set(struct bonding *bond,
+					    const struct bond_opt_value *newval);
 
 
 static const struct bond_opt_value bond_mode_tbl[] = {
@@ -186,6 +192,18 @@ static const struct bond_opt_value bond_tlb_dynamic_lb_tbl[] = {
 	{ NULL,  -1, 0}
 };
 
+static const struct bond_opt_value bond_ad_actor_sys_prio_tbl[] = {
+	{ "minval",  1,     BOND_VALFLAG_MIN},
+	{ "maxval",  65535, BOND_VALFLAG_MAX | BOND_VALFLAG_DEFAULT},
+	{ NULL,      -1,    0},
+};
+
+static const struct bond_opt_value bond_ad_user_port_key_tbl[] = {
+	{ "minval",  0,     BOND_VALFLAG_MIN | BOND_VALFLAG_DEFAULT},
+	{ "maxval",  1023,  BOND_VALFLAG_MAX},
+	{ NULL,      -1,    0},
+};
+
 static const struct bond_option bond_opts[BOND_OPT_LAST] = {
 	[BOND_OPT_MODE] = {
 		.id = BOND_OPT_MODE,
@@ -379,6 +397,29 @@ static const struct bond_option bond_opts[BOND_OPT_LAST] = {
 		.values = bond_tlb_dynamic_lb_tbl,
 		.flags = BOND_OPTFLAG_IFDOWN,
 		.set = bond_option_tlb_dynamic_lb_set,
+	},
+	[BOND_OPT_AD_ACTOR_SYS_PRIO] = {
+		.id = BOND_OPT_AD_ACTOR_SYS_PRIO,
+		.name = "ad_actor_sys_prio",
+		.unsuppmodes = BOND_MODE_ALL_EX(BIT(BOND_MODE_8023AD)),
+		.flags = BOND_OPTFLAG_IFDOWN,
+		.values = bond_ad_actor_sys_prio_tbl,
+		.set = bond_option_ad_actor_sys_prio_set,
+	},
+	[BOND_OPT_AD_ACTOR_SYSTEM] = {
+		.id = BOND_OPT_AD_ACTOR_SYSTEM,
+		.name = "ad_actor_system",
+		.unsuppmodes = BOND_MODE_ALL_EX(BIT(BOND_MODE_8023AD)),
+		.flags = BOND_OPTFLAG_RAWVAL | BOND_OPTFLAG_IFDOWN,
+		.set = bond_option_ad_actor_system_set,
+	},
+	[BOND_OPT_AD_USER_PORT_KEY] = {
+		.id = BOND_OPT_AD_USER_PORT_KEY,
+		.name = "ad_user_port_key",
+		.unsuppmodes = BOND_MODE_ALL_EX(BIT(BOND_MODE_8023AD)),
+		.flags = BOND_OPTFLAG_IFDOWN,
+		.values = bond_ad_user_port_key_tbl,
+		.set = bond_option_ad_user_port_key_set,
 	}
 };
 
@@ -1349,3 +1390,53 @@ static int bond_option_tlb_dynamic_lb_set(struct bonding *bond,
 
 	return 0;
 }
+
+static int bond_option_ad_actor_sys_prio_set(struct bonding *bond,
+					     const struct bond_opt_value *newval)
+{
+	netdev_info(bond->dev, "Setting ad_actor_sys_prio to %llu\n",
+		    newval->value);
+
+	bond->params.ad_actor_sys_prio = newval->value;
+	return 0;
+}
+
+static int bond_option_ad_actor_system_set(struct bonding *bond,
+					   const struct bond_opt_value *newval)
+{
+	u8 macaddr[ETH_ALEN];
+	u8 *mac;
+	int i;
+
+	if (newval->string) {
+		i = sscanf(newval->string, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
+			   &macaddr[0], &macaddr[1], &macaddr[2],
+			   &macaddr[3], &macaddr[4], &macaddr[5]);
+		if (i != ETH_ALEN)
+			goto err;
+		mac = macaddr;
+	} else {
+		mac = (u8 *)&newval->value;
+	}
+
+	if (!is_valid_ether_addr(mac))
+		goto err;
+
+	netdev_info(bond->dev, "Setting ad_actor_system to %pM\n", mac);
+	ether_addr_copy(bond->params.ad_actor_system, mac);
+	return 0;
+
+err:
+	netdev_err(bond->dev, "Invalid MAC address.\n");
+	return -EINVAL;
+}
+
+static int bond_option_ad_user_port_key_set(struct bonding *bond,
+					    const struct bond_opt_value *newval)
+{
+	netdev_info(bond->dev, "Setting ad_user_port_key to %llu\n",
+		    newval->value);
+
+	bond->params.ad_user_port_key = newval->value;
+	return 0;
+}
diff --git a/drivers/net/bonding/bond_procfs.c b/drivers/net/bonding/bond_procfs.c
index b20b35acb47d..f514fe5e80a5 100644
--- a/drivers/net/bonding/bond_procfs.c
+++ b/drivers/net/bonding/bond_procfs.c
@@ -135,23 +135,30 @@ static void bond_info_show_master(struct seq_file *seq)
 					  bond->params.ad_select);
 		seq_printf(seq, "Aggregator selection policy (ad_select): %s\n",
 			   optval->string);
-
-		if (__bond_3ad_get_active_agg_info(bond, &ad_info)) {
-			seq_printf(seq, "bond %s has no active aggregator\n",
-				   bond->dev->name);
-		} else {
-			seq_printf(seq, "Active Aggregator Info:\n");
-
-			seq_printf(seq, "\tAggregator ID: %d\n",
-				   ad_info.aggregator_id);
-			seq_printf(seq, "\tNumber of ports: %d\n",
-				   ad_info.ports);
-			seq_printf(seq, "\tActor Key: %d\n",
-				   ad_info.actor_key);
-			seq_printf(seq, "\tPartner Key: %d\n",
-				   ad_info.partner_key);
-			seq_printf(seq, "\tPartner Mac Address: %pM\n",
-				   ad_info.partner_system);
+		if (capable(CAP_NET_ADMIN)) {
+			seq_printf(seq, "System priority: %d\n",
+				   BOND_AD_INFO(bond).system.sys_priority);
+			seq_printf(seq, "System MAC address: %pM\n",
+				   &BOND_AD_INFO(bond).system.sys_mac_addr);
+
+			if (__bond_3ad_get_active_agg_info(bond, &ad_info)) {
+				seq_printf(seq,
+					   "bond %s has no active aggregator\n",
+					   bond->dev->name);
+			} else {
+				seq_printf(seq, "Active Aggregator Info:\n");
+
+				seq_printf(seq, "\tAggregator ID: %d\n",
+					   ad_info.aggregator_id);
+				seq_printf(seq, "\tNumber of ports: %d\n",
+					   ad_info.ports);
+				seq_printf(seq, "\tActor Key: %d\n",
+					   ad_info.actor_key);
+				seq_printf(seq, "\tPartner Key: %d\n",
+					   ad_info.partner_key);
+				seq_printf(seq, "\tPartner Mac Address: %pM\n",
+					   ad_info.partner_system);
+			}
 		}
 	}
 }
@@ -195,29 +202,35 @@ static void bond_info_show_slave(struct seq_file *seq,
 			seq_printf(seq, "Partner Churned Count: %d\n",
 				   port->churn_partner_count);
 
-			seq_puts(seq, "details actor lacp pdu:\n");
-			seq_printf(seq, "    system priority: %d\n",
-				   port->actor_system_priority);
-			seq_printf(seq, "    port key: %d\n",
-				   port->actor_oper_port_key);
-			seq_printf(seq, "    port priority: %d\n",
-				   port->actor_port_priority);
-			seq_printf(seq, "    port number: %d\n",
-				   port->actor_port_number);
-			seq_printf(seq, "    port state: %d\n",
-				   port->actor_oper_port_state);
-
-			seq_puts(seq, "details partner lacp pdu:\n");
-			seq_printf(seq, "    system priority: %d\n",
-				   port->partner_oper.system_priority);
-			seq_printf(seq, "    oper key: %d\n",
-				   port->partner_oper.key);
-			seq_printf(seq, "    port priority: %d\n",
-				   port->partner_oper.port_priority);
-			seq_printf(seq, "    port number: %d\n",
-				   port->partner_oper.port_number);
-			seq_printf(seq, "    port state: %d\n",
-				   port->partner_oper.port_state);
+			if (capable(CAP_NET_ADMIN)) {
+				seq_puts(seq, "details actor lacp pdu:\n");
+				seq_printf(seq, "    system priority: %d\n",
+					   port->actor_system_priority);
+				seq_printf(seq, "    system mac address: %pM\n",
+					   &port->actor_system);
+				seq_printf(seq, "    port key: %d\n",
+					   port->actor_oper_port_key);
+				seq_printf(seq, "    port priority: %d\n",
+					   port->actor_port_priority);
+				seq_printf(seq, "    port number: %d\n",
+					   port->actor_port_number);
+				seq_printf(seq, "    port state: %d\n",
+					   port->actor_oper_port_state);
+
+				seq_puts(seq, "details partner lacp pdu:\n");
+				seq_printf(seq, "    system priority: %d\n",
+					   port->partner_oper.system_priority);
+				seq_printf(seq, "    system mac address: %pM\n",
+					   &port->partner_oper.system);
+				seq_printf(seq, "    oper key: %d\n",
+					   port->partner_oper.key);
+				seq_printf(seq, "    port priority: %d\n",
+					   port->partner_oper.port_priority);
+				seq_printf(seq, "    port number: %d\n",
+					   port->partner_oper.port_number);
+				seq_printf(seq, "    port state: %d\n",
+					   port->partner_oper.port_state);
+			}
 		} else {
 			seq_puts(seq, "Aggregator ID: N/A\n");
 		}
diff --git a/drivers/net/bonding/bond_sysfs.c b/drivers/net/bonding/bond_sysfs.c
index 7e9e151d4d61..31835a4dab57 100644
--- a/drivers/net/bonding/bond_sysfs.c
+++ b/drivers/net/bonding/bond_sysfs.c
@@ -549,7 +549,7 @@ static ssize_t bonding_show_ad_actor_key(struct device *d,
 	int count = 0;
 	struct bonding *bond = to_bond(d);
 
-	if (BOND_MODE(bond) == BOND_MODE_8023AD) {
+	if (BOND_MODE(bond) == BOND_MODE_8023AD && capable(CAP_NET_ADMIN)) {
 		struct ad_info ad_info;
 		count = sprintf(buf, "%d\n",
 				bond_3ad_get_active_agg_info(bond, &ad_info)
@@ -569,7 +569,7 @@ static ssize_t bonding_show_ad_partner_key(struct device *d,
 	int count = 0;
 	struct bonding *bond = to_bond(d);
 
-	if (BOND_MODE(bond) == BOND_MODE_8023AD) {
+	if (BOND_MODE(bond) == BOND_MODE_8023AD && capable(CAP_NET_ADMIN)) {
 		struct ad_info ad_info;
 		count = sprintf(buf, "%d\n",
 				bond_3ad_get_active_agg_info(bond, &ad_info)
@@ -589,7 +589,7 @@ static ssize_t bonding_show_ad_partner_mac(struct device *d,
 	int count = 0;
 	struct bonding *bond = to_bond(d);
 
-	if (BOND_MODE(bond) == BOND_MODE_8023AD) {
+	if (BOND_MODE(bond) == BOND_MODE_8023AD && capable(CAP_NET_ADMIN)) {
 		struct ad_info ad_info;
 		if (!bond_3ad_get_active_agg_info(bond, &ad_info))
 			count = sprintf(buf, "%pM\n", ad_info.partner_system);
@@ -692,6 +692,49 @@ static ssize_t bonding_show_packets_per_slave(struct device *d,
 static DEVICE_ATTR(packets_per_slave, S_IRUGO | S_IWUSR,
 		   bonding_show_packets_per_slave, bonding_sysfs_store_option);
 
+static ssize_t bonding_show_ad_actor_sys_prio(struct device *d,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct bonding *bond = to_bond(d);
+
+	if (BOND_MODE(bond) == BOND_MODE_8023AD && capable(CAP_NET_ADMIN))
+		return sprintf(buf, "%hu\n", bond->params.ad_actor_sys_prio);
+
+	return 0;
+}
+static DEVICE_ATTR(ad_actor_sys_prio, S_IRUGO | S_IWUSR,
+		   bonding_show_ad_actor_sys_prio, bonding_sysfs_store_option);
+
+static ssize_t bonding_show_ad_actor_system(struct device *d,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct bonding *bond = to_bond(d);
+
+	if (BOND_MODE(bond) == BOND_MODE_8023AD && capable(CAP_NET_ADMIN))
+		return sprintf(buf, "%pM\n", bond->params.ad_actor_system);
+
+	return 0;
+}
+
+static DEVICE_ATTR(ad_actor_system, S_IRUGO | S_IWUSR,
+		   bonding_show_ad_actor_system, bonding_sysfs_store_option);
+
+static ssize_t bonding_show_ad_user_port_key(struct device *d,
+					     struct device_attribute *attr,
+					     char *buf)
+{
+	struct bonding *bond = to_bond(d);
+
+	if (BOND_MODE(bond) == BOND_MODE_8023AD && capable(CAP_NET_ADMIN))
+		return sprintf(buf, "%hu\n", bond->params.ad_user_port_key);
+
+	return 0;
+}
+static DEVICE_ATTR(ad_user_port_key, S_IRUGO | S_IWUSR,
+		   bonding_show_ad_user_port_key, bonding_sysfs_store_option);
+
 static struct attribute *per_bond_attrs[] = {
 	&dev_attr_slaves.attr,
 	&dev_attr_mode.attr,
@@ -725,6 +768,9 @@ static struct attribute *per_bond_attrs[] = {
 	&dev_attr_lp_interval.attr,
 	&dev_attr_packets_per_slave.attr,
 	&dev_attr_tlb_dynamic_lb.attr,
+	&dev_attr_ad_actor_sys_prio.attr,
+	&dev_attr_ad_actor_system.attr,
+	&dev_attr_ad_user_port_key.attr,
 	NULL,
 };
 
diff --git a/drivers/net/bonding/bond_sysfs_slave.c b/drivers/net/bonding/bond_sysfs_slave.c
index 23618a831612..7d16c51e6913 100644
--- a/drivers/net/bonding/bond_sysfs_slave.c
+++ b/drivers/net/bonding/bond_sysfs_slave.c
@@ -80,6 +80,36 @@ static ssize_t ad_aggregator_id_show(struct slave *slave, char *buf)
 }
 static SLAVE_ATTR_RO(ad_aggregator_id);
 
+static ssize_t ad_actor_oper_port_state_show(struct slave *slave, char *buf)
+{
+	const struct port *ad_port;
+
+	if (BOND_MODE(slave->bond) == BOND_MODE_8023AD) {
+		ad_port = &SLAVE_AD_INFO(slave)->port;
+		if (ad_port->aggregator)
+			return sprintf(buf, "%u\n",
+				       ad_port->actor_oper_port_state);
+	}
+
+	return sprintf(buf, "N/A\n");
+}
+static SLAVE_ATTR_RO(ad_actor_oper_port_state);
+
+static ssize_t ad_partner_oper_port_state_show(struct slave *slave, char *buf)
+{
+	const struct port *ad_port;
+
+	if (BOND_MODE(slave->bond) == BOND_MODE_8023AD) {
+		ad_port = &SLAVE_AD_INFO(slave)->port;
+		if (ad_port->aggregator)
+			return sprintf(buf, "%u\n",
+				       ad_port->partner_oper.port_state);
+	}
+
+	return sprintf(buf, "N/A\n");
+}
+static SLAVE_ATTR_RO(ad_partner_oper_port_state);
+
 static const struct slave_attribute *slave_attrs[] = {
 	&slave_attr_state,
 	&slave_attr_mii_status,
@@ -87,6 +117,8 @@ static const struct slave_attribute *slave_attrs[] = {
 	&slave_attr_perm_hwaddr,
 	&slave_attr_queue_id,
 	&slave_attr_ad_aggregator_id,
+	&slave_attr_ad_actor_oper_port_state,
+	&slave_attr_ad_partner_oper_port_state,
 	NULL
 };
 
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index b0f69248cb71..e9b1810d319f 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -440,6 +440,9 @@ unsigned int can_get_echo_skb(struct net_device *dev, unsigned int idx)
 		struct can_frame *cf = (struct can_frame *)skb->data;
 		u8 dlc = cf->can_dlc;
 
+		if (!(skb->tstamp.tv64))
+			__net_timestamp(skb);
+
 		netif_rx(priv->echo_skb[idx]);
 		priv->echo_skb[idx] = NULL;
 
@@ -575,6 +578,7 @@ struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
 	if (unlikely(!skb))
 		return NULL;
 
+	__net_timestamp(skb);
 	skb->protocol = htons(ETH_P_CAN);
 	skb->pkt_type = PACKET_BROADCAST;
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
@@ -603,6 +607,7 @@ struct sk_buff *alloc_canfd_skb(struct net_device *dev,
 	if (unlikely(!skb))
 		return NULL;
 
+	__net_timestamp(skb);
 	skb->protocol = htons(ETH_P_CANFD);
 	skb->pkt_type = PACKET_BROADCAST;
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index ad0a7e8c2c2b..6201c5a1a884 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -93,13 +93,13 @@
 	(FLEXCAN_CTRL_ERR_BUS | FLEXCAN_CTRL_ERR_STATE)
 
 /* FLEXCAN control register 2 (CTRL2) bits */
-#define FLEXCAN_CRL2_ECRWRE		BIT(29)
-#define FLEXCAN_CRL2_WRMFRZ		BIT(28)
-#define FLEXCAN_CRL2_RFFN(x)		(((x) & 0x0f) << 24)
-#define FLEXCAN_CRL2_TASD(x)		(((x) & 0x1f) << 19)
-#define FLEXCAN_CRL2_MRP		BIT(18)
-#define FLEXCAN_CRL2_RRS		BIT(17)
-#define FLEXCAN_CRL2_EACEN		BIT(16)
+#define FLEXCAN_CTRL2_ECRWRE		BIT(29)
+#define FLEXCAN_CTRL2_WRMFRZ		BIT(28)
+#define FLEXCAN_CTRL2_RFFN(x)		(((x) & 0x0f) << 24)
+#define FLEXCAN_CTRL2_TASD(x)		(((x) & 0x1f) << 19)
+#define FLEXCAN_CTRL2_MRP		BIT(18)
+#define FLEXCAN_CTRL2_RRS		BIT(17)
+#define FLEXCAN_CTRL2_EACEN		BIT(16)
 
 /* FLEXCAN memory error control register (MECR) bits */
 #define FLEXCAN_MECR_ECRWRDIS		BIT(31)
@@ -158,7 +158,6 @@
 	 FLEXCAN_IFLAG_BUF(FLEXCAN_TX_BUF_ID))
 
 /* FLEXCAN message buffers */
-#define FLEXCAN_MB_CNT_CODE(x)		(((x) & 0xf) << 24)
 #define FLEXCAN_MB_CODE_RX_INACTIVE	(0x0 << 24)
 #define FLEXCAN_MB_CODE_RX_EMPTY	(0x4 << 24)
 #define FLEXCAN_MB_CODE_RX_FULL		(0x2 << 24)
@@ -184,14 +183,14 @@
  * FLEXCAN hardware feature flags
  *
  * Below is some version info we got:
- *    SOC   Version   IP-Version  Glitch-  [TR]WRN_INT  Memory err
- *                                Filter?   connected?  detection
- *   MX25  FlexCAN2  03.00.00.00     no         no         no
- *   MX28  FlexCAN2  03.00.04.00    yes        yes         no
- *   MX35  FlexCAN2  03.00.00.00     no         no         no
- *   MX53  FlexCAN2  03.00.00.00    yes         no         no
- *   MX6s  FlexCAN3  10.00.12.00    yes        yes         no
- *   VF610 FlexCAN3  ?               no        yes        yes
+ *    SOC   Version   IP-Version  Glitch- [TR]WRN_INT Memory err RTR re-
+ *                                Filter? connected?  detection  ception in MB
+ *   MX25  FlexCAN2  03.00.00.00     no        no         no        no
+ *   MX28  FlexCAN2  03.00.04.00    yes       yes         no        no
+ *   MX35  FlexCAN2  03.00.00.00     no        no         no        no
+ *   MX53  FlexCAN2  03.00.00.00    yes        no         no        no
+ *   MX6s  FlexCAN3  10.00.12.00    yes       yes         no       yes
+ *   VF610 FlexCAN3  ?               no       yes        yes       yes?
  *
  * Some SOCs do not have the RX_WARN & TX_WARN interrupt line connected.
  */
@@ -221,7 +220,7 @@ struct flexcan_regs {
 	u32 imask1;		/* 0x28 */
 	u32 iflag2;		/* 0x2c */
 	u32 iflag1;		/* 0x30 */
-	u32 crl2;		/* 0x34 */
+	u32 ctrl2;		/* 0x34 */
 	u32 esr2;		/* 0x38 */
 	u32 imeur;		/* 0x3c */
 	u32 lrfr;		/* 0x40 */
@@ -230,6 +229,16 @@ struct flexcan_regs {
 	u32 rxfir;		/* 0x4c */
 	u32 _reserved3[12];	/* 0x50 */
 	struct flexcan_mb cantxfg[64];	/* 0x80 */
+	/* FIFO-mode:
+	 *			MB
+	 * 0x080...0x08f	0	RX message buffer
+	 * 0x090...0x0df	1-5	reserverd
+	 * 0x0e0...0x0ff	6-7	8 entry ID table
+	 *				(mx25, mx28, mx35, mx53)
+	 * 0x0e0...0x2df	6-7..37	8..128 entry ID table
+	 *			  	size conf'ed via ctrl2::RFFN
+	 *				(mx6, vf610)
+	 */
 	u32 _reserved4[408];
 	u32 mecr;		/* 0xae0 */
 	u32 erriar;		/* 0xae4 */
@@ -468,7 +477,7 @@ static int flexcan_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct flexcan_regs __iomem *regs = priv->base;
 	struct can_frame *cf = (struct can_frame *)skb->data;
 	u32 can_id;
-	u32 ctrl = FLEXCAN_MB_CNT_CODE(0xc) | (cf->can_dlc << 16);
+	u32 ctrl = FLEXCAN_MB_CODE_TX_DATA | (cf->can_dlc << 16);
 
 	if (can_dropped_invalid_skb(dev, skb))
 		return NETDEV_TX_OK;
@@ -815,7 +824,7 @@ static int flexcan_chip_start(struct net_device *dev)
 {
 	struct flexcan_priv *priv = netdev_priv(dev);
 	struct flexcan_regs __iomem *regs = priv->base;
-	u32 reg_mcr, reg_ctrl, reg_crl2, reg_mecr;
+	u32 reg_mcr, reg_ctrl, reg_ctrl2, reg_mecr;
 	int err, i;
 
 	/* enable module */
@@ -918,9 +927,9 @@ static int flexcan_chip_start(struct net_device *dev)
 		 * and Correction of Memory Errors" to write to
 		 * MECR register
 		 */
-		reg_crl2 = flexcan_read(&regs->crl2);
-		reg_crl2 |= FLEXCAN_CRL2_ECRWRE;
-		flexcan_write(reg_crl2, &regs->crl2);
+		reg_ctrl2 = flexcan_read(&regs->ctrl2);
+		reg_ctrl2 |= FLEXCAN_CTRL2_ECRWRE;
+		flexcan_write(reg_ctrl2, &regs->ctrl2);
 
 		reg_mecr = flexcan_read(&regs->mecr);
 		reg_mecr &= ~FLEXCAN_MECR_ECRWRDIS;
diff --git a/drivers/net/can/janz-ican3.c b/drivers/net/can/janz-ican3.c
index 4dd183a3643a..c1e85368a198 100644
--- a/drivers/net/can/janz-ican3.c
+++ b/drivers/net/can/janz-ican3.c
@@ -40,6 +40,7 @@
 #define MSYNC_PEER		0x00		/* ICAN only */
 #define MSYNC_LOCL		0x01		/* host only */
 #define TARGET_RUNNING		0x02
+#define FIRMWARE_STAMP		0x60		/* big endian firmware stamp */
 
 #define MSYNC_RB0		0x01
 #define MSYNC_RB1		0x02
@@ -83,6 +84,7 @@
 #define MSG_COFFREQ		0x42
 #define MSG_CONREQ		0x43
 #define MSG_CCONFREQ		0x47
+#define MSG_LMTS		0xb4
 
 /*
  * Janz ICAN3 CAN Inquiry Message Types
@@ -165,6 +167,12 @@
 /* SJA1000 Clock Input */
 #define ICAN3_CAN_CLOCK		8000000
 
+/* Janz ICAN3 firmware types */
+enum ican3_fwtype {
+	ICAN3_FWTYPE_ICANOS,
+	ICAN3_FWTYPE_CAL_CANOPEN,
+};
+
 /* Driver Name */
 #define DRV_NAME "janz-ican3"
 
@@ -215,6 +223,10 @@ struct ican3_dev {
 	struct completion buserror_comp;
 	struct can_berr_counter bec;
 
+	/* firmware type */
+	enum ican3_fwtype fwtype;
+	char fwinfo[32];
+
 	/* old and new style host interface */
 	unsigned int iftype;
 
@@ -750,13 +762,61 @@ static int ican3_set_id_filter(struct ican3_dev *mod, bool accept)
  */
 static int ican3_set_bus_state(struct ican3_dev *mod, bool on)
 {
+	struct can_bittiming *bt = &mod->can.bittiming;
 	struct ican3_msg msg;
+	u8 btr0, btr1;
+	int res;
 
-	memset(&msg, 0, sizeof(msg));
-	msg.spec = on ? MSG_CONREQ : MSG_COFFREQ;
-	msg.len = cpu_to_le16(0);
+	/* This algorithm was stolen from drivers/net/can/sja1000/sja1000.c      */
+	/* The bittiming register command for the ICAN3 just sets the bit timing */
+	/* registers on the SJA1000 chip directly                                */
+	btr0 = ((bt->brp - 1) & 0x3f) | (((bt->sjw - 1) & 0x3) << 6);
+	btr1 = ((bt->prop_seg + bt->phase_seg1 - 1) & 0xf) |
+		(((bt->phase_seg2 - 1) & 0x7) << 4);
+	if (mod->can.ctrlmode & CAN_CTRLMODE_3_SAMPLES)
+		btr1 |= 0x80;
 
-	return ican3_send_msg(mod, &msg);
+	if (mod->fwtype == ICAN3_FWTYPE_ICANOS) {
+		if (on) {
+			/* set bittiming */
+			memset(&msg, 0, sizeof(msg));
+			msg.spec = MSG_CBTRREQ;
+			msg.len = cpu_to_le16(4);
+			msg.data[0] = 0x00;
+			msg.data[1] = 0x00;
+			msg.data[2] = btr0;
+			msg.data[3] = btr1;
+
+			res = ican3_send_msg(mod, &msg);
+			if (res)
+				return res;
+		}
+
+		/* can-on/off request */
+		memset(&msg, 0, sizeof(msg));
+		msg.spec = on ? MSG_CONREQ : MSG_COFFREQ;
+		msg.len = cpu_to_le16(0);
+
+		return ican3_send_msg(mod, &msg);
+
+	} else if (mod->fwtype == ICAN3_FWTYPE_CAL_CANOPEN) {
+		memset(&msg, 0, sizeof(msg));
+		msg.spec = MSG_LMTS;
+		if (on) {
+			msg.len = cpu_to_le16(4);
+			msg.data[0] = 0;
+			msg.data[1] = 0;
+			msg.data[2] = btr0;
+			msg.data[3] = btr1;
+		} else {
+			msg.len = cpu_to_le16(2);
+			msg.data[0] = 1;
+			msg.data[1] = 0;
+		}
+
+		return ican3_send_msg(mod, &msg);
+	}
+	return -ENOTSUPP;
 }
 
 static int ican3_set_termination(struct ican3_dev *mod, bool on)
@@ -1402,7 +1462,7 @@ static int ican3_reset_module(struct ican3_dev *mod)
 			return 0;
 
 		msleep(10);
-	} while (time_before(jiffies, start + HZ / 4));
+	} while (time_before(jiffies, start + HZ / 2));
 
 	netdev_err(mod->ndev, "failed to reset CAN module\n");
 	return -ETIMEDOUT;
@@ -1427,6 +1487,17 @@ static int ican3_startup_module(struct ican3_dev *mod)
 		return ret;
 	}
 
+	/* detect firmware */
+	memcpy_fromio(mod->fwinfo, mod->dpm + FIRMWARE_STAMP, sizeof(mod->fwinfo) - 1);
+	if (strncmp(mod->fwinfo, "JANZ-ICAN3", 10)) {
+		netdev_err(mod->ndev, "ICAN3 not detected (found %s)\n", mod->fwinfo);
+		return -ENODEV;
+	}
+	if (strstr(mod->fwinfo, "CAL/CANopen"))
+		mod->fwtype = ICAN3_FWTYPE_CAL_CANOPEN;
+	else
+		mod->fwtype = ICAN3_FWTYPE_ICANOS;
+
 	/* re-enable interrupts so we can send messages */
 	iowrite8(1 << mod->num, &mod->ctrl->int_enable);
 
@@ -1615,36 +1686,6 @@ static const struct can_bittiming_const ican3_bittiming_const = {
 	.brp_inc = 1,
 };
 
-/*
- * This routine was stolen from drivers/net/can/sja1000/sja1000.c
- *
- * The bittiming register command for the ICAN3 just sets the bit timing
- * registers on the SJA1000 chip directly
- */
-static int ican3_set_bittiming(struct net_device *ndev)
-{
-	struct ican3_dev *mod = netdev_priv(ndev);
-	struct can_bittiming *bt = &mod->can.bittiming;
-	struct ican3_msg msg;
-	u8 btr0, btr1;
-
-	btr0 = ((bt->brp - 1) & 0x3f) | (((bt->sjw - 1) & 0x3) << 6);
-	btr1 = ((bt->prop_seg + bt->phase_seg1 - 1) & 0xf) |
-		(((bt->phase_seg2 - 1) & 0x7) << 4);
-	if (mod->can.ctrlmode & CAN_CTRLMODE_3_SAMPLES)
-		btr1 |= 0x80;
-
-	memset(&msg, 0, sizeof(msg));
-	msg.spec = MSG_CBTRREQ;
-	msg.len = cpu_to_le16(4);
-	msg.data[0] = 0x00;
-	msg.data[1] = 0x00;
-	msg.data[2] = btr0;
-	msg.data[3] = btr1;
-
-	return ican3_send_msg(mod, &msg);
-}
-
 static int ican3_set_mode(struct net_device *ndev, enum can_mode mode)
 {
 	struct ican3_dev *mod = netdev_priv(ndev);
@@ -1730,11 +1771,22 @@ static ssize_t ican3_sysfs_set_term(struct device *dev,
 	return count;
 }
 
+static ssize_t ican3_sysfs_show_fwinfo(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf)
+{
+	struct ican3_dev *mod = netdev_priv(to_net_dev(dev));
+
+	return scnprintf(buf, PAGE_SIZE, "%s\n", mod->fwinfo);
+}
+
 static DEVICE_ATTR(termination, S_IWUSR | S_IRUGO, ican3_sysfs_show_term,
 						   ican3_sysfs_set_term);
+static DEVICE_ATTR(fwinfo, S_IRUSR | S_IRUGO, ican3_sysfs_show_fwinfo, NULL);
 
 static struct attribute *ican3_sysfs_attrs[] = {
 	&dev_attr_termination.attr,
+	&dev_attr_fwinfo.attr,
 	NULL,
 };
 
@@ -1794,7 +1846,6 @@ static int ican3_probe(struct platform_device *pdev)
 
 	mod->can.clock.freq = ICAN3_CAN_CLOCK;
 	mod->can.bittiming_const = &ican3_bittiming_const;
-	mod->can.do_set_bittiming = ican3_set_bittiming;
 	mod->can.do_set_mode = ican3_set_mode;
 	mod->can.do_get_berr_counter = ican3_get_berr_counter;
 	mod->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES
@@ -1866,7 +1917,7 @@ static int ican3_probe(struct platform_device *pdev)
 		goto out_free_irq;
 	}
 
-	dev_info(dev, "module %d: registered CAN device\n", pdata->modno);
+	netdev_info(mod->ndev, "module %d: registered CAN device\n", pdata->modno);
 	return 0;
 
 out_free_irq:
diff --git a/drivers/net/can/slcan.c b/drivers/net/can/slcan.c
index c837eb91d43e..f64f5290d6f8 100644
--- a/drivers/net/can/slcan.c
+++ b/drivers/net/can/slcan.c
@@ -207,6 +207,7 @@ static void slc_bump(struct slcan *sl)
 	if (!skb)
 		return;
 
+	__net_timestamp(skb);
 	skb->dev = sl->dev;
 	skb->protocol = htons(ETH_P_CAN);
 	skb->pkt_type = PACKET_BROADCAST;
diff --git a/drivers/net/can/spi/mcp251x.c b/drivers/net/can/spi/mcp251x.c
index bf63fee4e743..c1a95a34d62e 100644
--- a/drivers/net/can/spi/mcp251x.c
+++ b/drivers/net/can/spi/mcp251x.c
@@ -190,10 +190,11 @@
 #define RXBEID0_OFF 4
 #define RXBDLC_OFF  5
 #define RXBDAT_OFF  6
-#define RXFSIDH(n) ((n) * 4)
-#define RXFSIDL(n) ((n) * 4 + 1)
-#define RXFEID8(n) ((n) * 4 + 2)
-#define RXFEID0(n) ((n) * 4 + 3)
+#define RXFSID(n) ((n < 3) ? 0 : 4)
+#define RXFSIDH(n) ((n) * 4 + RXFSID(n))
+#define RXFSIDL(n) ((n) * 4 + 1 + RXFSID(n))
+#define RXFEID8(n) ((n) * 4 + 2 + RXFSID(n))
+#define RXFEID0(n) ((n) * 4 + 3 + RXFSID(n))
 #define RXMSIDH(n) ((n) * 4 + 0x20)
 #define RXMSIDL(n) ((n) * 4 + 0x21)
 #define RXMEID8(n) ((n) * 4 + 0x22)
diff --git a/drivers/net/can/vcan.c b/drivers/net/can/vcan.c
index 674f367087c5..0ce868de855d 100644
--- a/drivers/net/can/vcan.c
+++ b/drivers/net/can/vcan.c
@@ -78,6 +78,9 @@ static void vcan_rx(struct sk_buff *skb, struct net_device *dev)
 	skb->dev       = dev;
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
 
+	if (!(skb->tstamp.tv64))
+		__net_timestamp(skb);
+
 	netif_rx_ni(skb);
 }
 
diff --git a/drivers/net/dsa/Kconfig b/drivers/net/dsa/Kconfig
index 18550c7ebe6f..7ad0a4d8e475 100644
--- a/drivers/net/dsa/Kconfig
+++ b/drivers/net/dsa/Kconfig
@@ -37,22 +37,22 @@ config NET_DSA_MV88E6123_61_65
 	  ethernet switch chips.
 
 config NET_DSA_MV88E6171
-	tristate "Marvell 88E6171/6172 ethernet switch chip support"
+	tristate "Marvell 88E6171/6175/6350/6351 ethernet switch chip support"
 	depends on NET_DSA
 	select NET_DSA_MV88E6XXX
 	select NET_DSA_TAG_EDSA
 	---help---
-	  This enables support for the Marvell 88E6171/6172 ethernet switch
-	  chips.
+	  This enables support for the Marvell 88E6171/6175/6350/6351
+	  ethernet switches chips.
 
 config NET_DSA_MV88E6352
-	tristate "Marvell 88E6176/88E6352 ethernet switch chip support"
+	tristate "Marvell 88E6172/88E6176/88E6352 ethernet switch chip support"
 	depends on NET_DSA
 	select NET_DSA_MV88E6XXX
 	select NET_DSA_TAG_EDSA
 	---help---
-	  This enables support for the Marvell 88E6176 and 88E6352 ethernet
-	  switch chips.
+	  This enables support for the Marvell 88E6172, 88E6176 and 88E6352
+	  ethernet switch chips.
 
 config NET_DSA_BCM_SF2
 	tristate "Broadcom Starfighter 2 Ethernet switch support"
diff --git a/drivers/net/dsa/bcm_sf2.c b/drivers/net/dsa/bcm_sf2.c
index cedb572bf25a..972982f8bea7 100644
--- a/drivers/net/dsa/bcm_sf2.c
+++ b/drivers/net/dsa/bcm_sf2.c
@@ -24,6 +24,7 @@
 #include <net/dsa.h>
 #include <linux/ethtool.h>
 #include <linux/if_bridge.h>
+#include <linux/brcmphy.h>
 
 #include "bcm_sf2.h"
 #include "bcm_sf2_regs.h"
@@ -697,7 +698,7 @@ static int bcm_sf2_sw_setup(struct dsa_switch *ds)
 	/* Include the pseudo-PHY address and the broadcast PHY address to
 	 * divert reads towards our workaround
 	 */
-	ds->phys_mii_mask |= ((1 << 30) | (1 << 0));
+	ds->phys_mii_mask |= ((1 << BRCM_PSEUDO_PHY_ADDR) | (1 << 0));
 
 	rev = reg_readl(priv, REG_SWITCH_REVISION);
 	priv->hw_params.top_rev = (rev >> SWITCH_TOP_REV_SHIFT) &
@@ -782,7 +783,7 @@ static int bcm_sf2_sw_phy_read(struct dsa_switch *ds, int addr, int regnum)
 	 */
 	switch (addr) {
 	case 0:
-	case 30:
+	case BRCM_PSEUDO_PHY_ADDR:
 		return bcm_sf2_sw_indir_rw(ds, 1, addr, regnum, 0);
 	default:
 		return 0xffff;
@@ -797,7 +798,7 @@ static int bcm_sf2_sw_phy_write(struct dsa_switch *ds, int addr, int regnum,
 	 */
 	switch (addr) {
 	case 0:
-	case 30:
+	case BRCM_PSEUDO_PHY_ADDR:
 		bcm_sf2_sw_indir_rw(ds, 0, addr, regnum, val);
 		break;
 	}
@@ -911,6 +912,13 @@ static void bcm_sf2_sw_fixed_link_update(struct dsa_switch *ds, int port,
 	 */
 	if (port == 7) {
 		status->link = priv->port_sts[port].link;
+		/* For MoCA interfaces, also force a link down notification
+		 * since some version of the user-space daemon (mocad) use
+		 * cmd->autoneg to force the link, which messes up the PHY
+		 * state machine and make it go in PHY_FORCING state instead.
+		 */
+		if (!status->link)
+			netif_carrier_off(ds->ports[port]);
 		status->duplex = 1;
 	} else {
 		status->link = 1;
diff --git a/drivers/net/dsa/mv88e6123_61_65.c b/drivers/net/dsa/mv88e6123_61_65.c
index b4af6d5aff7c..71a29a7ce538 100644
--- a/drivers/net/dsa/mv88e6123_61_65.c
+++ b/drivers/net/dsa/mv88e6123_61_65.c
@@ -54,192 +54,40 @@ static char *mv88e6123_61_65_probe(struct device *host_dev, int sw_addr)
 
 static int mv88e6123_61_65_setup_global(struct dsa_switch *ds)
 {
+	u32 upstream_port = dsa_upstream_port(ds);
 	int ret;
-	int i;
+	u32 reg;
+
+	ret = mv88e6xxx_setup_global(ds);
+	if (ret)
+		return ret;
 
 	/* Disable the PHY polling unit (since there won't be any
 	 * external PHYs to poll), don't discard packets with
 	 * excessive collisions, and mask all interrupt sources.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x04, 0x0000);
-
-	/* Set the default address aging time to 5 minutes, and
-	 * enable address learn messages to be sent to all message
-	 * ports.
-	 */
-	REG_WRITE(REG_GLOBAL, 0x0a, 0x0148);
-
-	/* Configure the priority mapping registers. */
-	ret = mv88e6xxx_config_prio(ds);
-	if (ret < 0)
-		return ret;
+	REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL, 0x0000);
 
 	/* Configure the upstream port, and configure the upstream
 	 * port as the port to which ingress and egress monitor frames
 	 * are to be sent.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x1a, (dsa_upstream_port(ds) * 0x1110));
+	reg = upstream_port << GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT |
+		upstream_port << GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT |
+		upstream_port << GLOBAL_MONITOR_CONTROL_ARP_SHIFT;
+	REG_WRITE(REG_GLOBAL, GLOBAL_MONITOR_CONTROL, reg);
 
 	/* Disable remote management for now, and set the switch's
 	 * DSA device number.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x1c, ds->index & 0x1f);
-
-	/* Send all frames with destination addresses matching
-	 * 01:80:c2:00:00:2x to the CPU port.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x02, 0xffff);
-
-	/* Send all frames with destination addresses matching
-	 * 01:80:c2:00:00:0x to the CPU port.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x03, 0xffff);
-
-	/* Disable the loopback filter, disable flow control
-	 * messages, disable flood broadcast override, disable
-	 * removing of provider tags, disable ATU age violation
-	 * interrupts, disable tag flow control, force flow
-	 * control priority to the highest, and send all special
-	 * multicast frames to the CPU at the highest priority.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x05, 0x00ff);
-
-	/* Program the DSA routing table. */
-	for (i = 0; i < 32; i++) {
-		int nexthop;
-
-		nexthop = 0x1f;
-		if (i != ds->index && i < ds->dst->pd->nr_chips)
-			nexthop = ds->pd->rtable[i] & 0x1f;
-
-		REG_WRITE(REG_GLOBAL2, 0x06, 0x8000 | (i << 8) | nexthop);
-	}
-
-	/* Clear all trunk masks. */
-	for (i = 0; i < 8; i++)
-		REG_WRITE(REG_GLOBAL2, 0x07, 0x8000 | (i << 12) | 0xff);
-
-	/* Clear all trunk mappings. */
-	for (i = 0; i < 16; i++)
-		REG_WRITE(REG_GLOBAL2, 0x08, 0x8000 | (i << 11));
-
-	/* Disable ingress rate limiting by resetting all ingress
-	 * rate limit registers to their initial state.
-	 */
-	for (i = 0; i < 6; i++)
-		REG_WRITE(REG_GLOBAL2, 0x09, 0x9000 | (i << 8));
-
-	/* Initialise cross-chip port VLAN table to reset defaults. */
-	REG_WRITE(REG_GLOBAL2, 0x0b, 0x9000);
-
-	/* Clear the priority override table. */
-	for (i = 0; i < 16; i++)
-		REG_WRITE(REG_GLOBAL2, 0x0f, 0x8000 | (i << 8));
-
-	/* @@@ initialise AVB (22/23) watchdog (27) sdet (29) registers */
+	REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL_2, ds->index & 0x1f);
 
 	return 0;
 }
 
-static int mv88e6123_61_65_setup_port(struct dsa_switch *ds, int p)
-{
-	int addr = REG_PORT(p);
-	u16 val;
-
-	/* MAC Forcing register: don't force link, speed, duplex
-	 * or flow control state to any particular values on physical
-	 * ports, but force the CPU port and all DSA ports to 1000 Mb/s
-	 * full duplex.
-	 */
-	if (dsa_is_cpu_port(ds, p) || ds->dsa_port_mask & (1 << p))
-		REG_WRITE(addr, 0x01, 0x003e);
-	else
-		REG_WRITE(addr, 0x01, 0x0003);
-
-	/* Do not limit the period of time that this port can be
-	 * paused for by the remote end or the period of time that
-	 * this port can pause the remote end.
-	 */
-	REG_WRITE(addr, 0x02, 0x0000);
-
-	/* Port Control: disable Drop-on-Unlock, disable Drop-on-Lock,
-	 * disable Header mode, enable IGMP/MLD snooping, disable VLAN
-	 * tunneling, determine priority by looking at 802.1p and IP
-	 * priority fields (IP prio has precedence), and set STP state
-	 * to Forwarding.
-	 *
-	 * If this is the CPU link, use DSA or EDSA tagging depending
-	 * on which tagging mode was configured.
-	 *
-	 * If this is a link to another switch, use DSA tagging mode.
-	 *
-	 * If this is the upstream port for this switch, enable
-	 * forwarding of unknown unicasts and multicasts.
-	 */
-	val = 0x0433;
-	if (dsa_is_cpu_port(ds, p)) {
-		if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
-			val |= 0x3300;
-		else
-			val |= 0x0100;
-	}
-	if (ds->dsa_port_mask & (1 << p))
-		val |= 0x0100;
-	if (p == dsa_upstream_port(ds))
-		val |= 0x000c;
-	REG_WRITE(addr, 0x04, val);
-
-	/* Port Control 2: don't force a good FCS, set the maximum
-	 * frame size to 10240 bytes, don't let the switch add or
-	 * strip 802.1q tags, don't discard tagged or untagged frames
-	 * on this port, do a destination address lookup on all
-	 * received packets as usual, disable ARP mirroring and don't
-	 * send a copy of all transmitted/received frames on this port
-	 * to the CPU.
-	 */
-	REG_WRITE(addr, 0x08, 0x2080);
-
-	/* Egress rate control: disable egress rate control. */
-	REG_WRITE(addr, 0x09, 0x0001);
-
-	/* Egress rate control 2: disable egress rate control. */
-	REG_WRITE(addr, 0x0a, 0x0000);
-
-	/* Port Association Vector: when learning source addresses
-	 * of packets, add the address to the address database using
-	 * a port bitmap that has only the bit for this port set and
-	 * the other bits clear.
-	 */
-	REG_WRITE(addr, 0x0b, 1 << p);
-
-	/* Port ATU control: disable limiting the number of address
-	 * database entries that this port is allowed to use.
-	 */
-	REG_WRITE(addr, 0x0c, 0x0000);
-
-	/* Priority Override: disable DA, SA and VTU priority override. */
-	REG_WRITE(addr, 0x0d, 0x0000);
-
-	/* Port Ethertype: use the Ethertype DSA Ethertype value. */
-	REG_WRITE(addr, 0x0f, ETH_P_EDSA);
-
-	/* Tag Remap: use an identity 802.1p prio -> switch prio
-	 * mapping.
-	 */
-	REG_WRITE(addr, 0x18, 0x3210);
-
-	/* Tag Remap 2: use an identity 802.1p prio -> switch prio
-	 * mapping.
-	 */
-	REG_WRITE(addr, 0x19, 0x7654);
-
-	return mv88e6xxx_setup_port_common(ds, p);
-}
-
 static int mv88e6123_61_65_setup(struct dsa_switch *ds)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int i;
 	int ret;
 
 	ret = mv88e6xxx_setup_common(ds);
@@ -262,19 +110,11 @@ static int mv88e6123_61_65_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
-	/* @@@ initialise vtu and atu */
-
 	ret = mv88e6123_61_65_setup_global(ds);
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < ps->num_ports; i++) {
-		ret = mv88e6123_61_65_setup_port(ds, i);
-		if (ret < 0)
-			return ret;
-	}
-
-	return 0;
+	return mv88e6xxx_setup_ports(ds);
 }
 
 struct dsa_switch_driver mv88e6123_61_65_switch_driver = {
diff --git a/drivers/net/dsa/mv88e6131.c b/drivers/net/dsa/mv88e6131.c
index e54824fa0d95..32f4a08e9bc9 100644
--- a/drivers/net/dsa/mv88e6131.c
+++ b/drivers/net/dsa/mv88e6131.c
@@ -37,6 +37,8 @@ static char *mv88e6131_probe(struct device *host_dev, int sw_addr)
 			return "Marvell 88E6131 (B2)";
 		if (ret_masked == PORT_SWITCH_ID_6131)
 			return "Marvell 88E6131";
+		if (ret_masked == PORT_SWITCH_ID_6185)
+			return "Marvell 88E6185";
 	}
 
 	return NULL;
@@ -44,186 +46,62 @@ static char *mv88e6131_probe(struct device *host_dev, int sw_addr)
 
 static int mv88e6131_setup_global(struct dsa_switch *ds)
 {
+	u32 upstream_port = dsa_upstream_port(ds);
 	int ret;
-	int i;
+	u32 reg;
+
+	ret = mv88e6xxx_setup_global(ds);
+	if (ret)
+		return ret;
 
 	/* Enable the PHY polling unit, don't discard packets with
 	 * excessive collisions, use a weighted fair queueing scheme
 	 * to arbitrate between packet queues, set the maximum frame
 	 * size to 1632, and mask all interrupt sources.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x04, 0x4400);
-
-	/* Set the default address aging time to 5 minutes, and
-	 * enable address learn messages to be sent to all message
-	 * ports.
-	 */
-	REG_WRITE(REG_GLOBAL, 0x0a, 0x0148);
-
-	/* Configure the priority mapping registers. */
-	ret = mv88e6xxx_config_prio(ds);
-	if (ret < 0)
-		return ret;
+	REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL,
+		  GLOBAL_CONTROL_PPU_ENABLE | GLOBAL_CONTROL_MAX_FRAME_1632);
 
 	/* Set the VLAN ethertype to 0x8100. */
-	REG_WRITE(REG_GLOBAL, 0x19, 0x8100);
+	REG_WRITE(REG_GLOBAL, GLOBAL_CORE_TAG_TYPE, 0x8100);
 
 	/* Disable ARP mirroring, and configure the upstream port as
 	 * the port to which ingress and egress monitor frames are to
 	 * be sent.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x1a, (dsa_upstream_port(ds) * 0x1100) | 0x00f0);
+	reg = upstream_port << GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT |
+		upstream_port << GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT |
+		GLOBAL_MONITOR_CONTROL_ARP_DISABLED;
+	REG_WRITE(REG_GLOBAL, GLOBAL_MONITOR_CONTROL, reg);
 
 	/* Disable cascade port functionality unless this device
 	 * is used in a cascade configuration, and set the switch's
 	 * DSA device number.
 	 */
 	if (ds->dst->pd->nr_chips > 1)
-		REG_WRITE(REG_GLOBAL, 0x1c, 0xf000 | (ds->index & 0x1f));
+		REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL_2,
+			  GLOBAL_CONTROL_2_MULTIPLE_CASCADE |
+			  (ds->index & 0x1f));
 	else
-		REG_WRITE(REG_GLOBAL, 0x1c, 0xe000 | (ds->index & 0x1f));
-
-	/* Send all frames with destination addresses matching
-	 * 01:80:c2:00:00:0x to the CPU port.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x03, 0xffff);
-
-	/* Ignore removed tag data on doubly tagged packets, disable
-	 * flow control messages, force flow control priority to the
-	 * highest, and send all special multicast frames to the CPU
-	 * port at the highest priority.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x05, 0x00ff);
-
-	/* Program the DSA routing table. */
-	for (i = 0; i < 32; i++) {
-		int nexthop;
-
-		nexthop = 0x1f;
-		if (ds->pd->rtable &&
-		    i != ds->index && i < ds->dst->pd->nr_chips)
-			nexthop = ds->pd->rtable[i] & 0x1f;
-
-		REG_WRITE(REG_GLOBAL2, 0x06, 0x8000 | (i << 8) | nexthop);
-	}
-
-	/* Clear all trunk masks. */
-	for (i = 0; i < 8; i++)
-		REG_WRITE(REG_GLOBAL2, 0x07, 0x8000 | (i << 12) | 0x7ff);
-
-	/* Clear all trunk mappings. */
-	for (i = 0; i < 16; i++)
-		REG_WRITE(REG_GLOBAL2, 0x08, 0x8000 | (i << 11));
+		REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL_2,
+			  GLOBAL_CONTROL_2_NO_CASCADE |
+			  (ds->index & 0x1f));
 
 	/* Force the priority of IGMP/MLD snoop frames and ARP frames
 	 * to the highest setting.
 	 */
-	REG_WRITE(REG_GLOBAL2, 0x0f, 0x00ff);
+	REG_WRITE(REG_GLOBAL2, GLOBAL2_PRIO_OVERRIDE,
+		  GLOBAL2_PRIO_OVERRIDE_FORCE_SNOOP |
+		  7 << GLOBAL2_PRIO_OVERRIDE_SNOOP_SHIFT |
+		  GLOBAL2_PRIO_OVERRIDE_FORCE_ARP |
+		  7 << GLOBAL2_PRIO_OVERRIDE_ARP_SHIFT);
 
 	return 0;
 }
 
-static int mv88e6131_setup_port(struct dsa_switch *ds, int p)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int addr = REG_PORT(p);
-	u16 val;
-
-	/* MAC Forcing register: don't force link, speed, duplex
-	 * or flow control state to any particular values on physical
-	 * ports, but force the CPU port and all DSA ports to 1000 Mb/s
-	 * (100 Mb/s on 6085) full duplex.
-	 */
-	if (dsa_is_cpu_port(ds, p) || ds->dsa_port_mask & (1 << p))
-		if (ps->id == PORT_SWITCH_ID_6085)
-			REG_WRITE(addr, 0x01, 0x003d); /* 100 Mb/s */
-		else
-			REG_WRITE(addr, 0x01, 0x003e); /* 1000 Mb/s */
-	else
-		REG_WRITE(addr, 0x01, 0x0003);
-
-	/* Port Control: disable Core Tag, disable Drop-on-Lock,
-	 * transmit frames unmodified, disable Header mode,
-	 * enable IGMP/MLD snoop, disable DoubleTag, disable VLAN
-	 * tunneling, determine priority by looking at 802.1p and
-	 * IP priority fields (IP prio has precedence), and set STP
-	 * state to Forwarding.
-	 *
-	 * If this is the upstream port for this switch, enable
-	 * forwarding of unknown unicasts, and enable DSA tagging
-	 * mode.
-	 *
-	 * If this is the link to another switch, use DSA tagging
-	 * mode, but do not enable forwarding of unknown unicasts.
-	 */
-	val = 0x0433;
-	if (p == dsa_upstream_port(ds)) {
-		val |= 0x0104;
-		/* On 6085, unknown multicast forward is controlled
-		 * here rather than in Port Control 2 register.
-		 */
-		if (ps->id == PORT_SWITCH_ID_6085)
-			val |= 0x0008;
-	}
-	if (ds->dsa_port_mask & (1 << p))
-		val |= 0x0100;
-	REG_WRITE(addr, 0x04, val);
-
-	/* Port Control 2: don't force a good FCS, don't use
-	 * VLAN-based, source address-based or destination
-	 * address-based priority overrides, don't let the switch
-	 * add or strip 802.1q tags, don't discard tagged or
-	 * untagged frames on this port, do a destination address
-	 * lookup on received packets as usual, don't send a copy
-	 * of all transmitted/received frames on this port to the
-	 * CPU, and configure the upstream port number.
-	 *
-	 * If this is the upstream port for this switch, enable
-	 * forwarding of unknown multicast addresses.
-	 */
-	if (ps->id == PORT_SWITCH_ID_6085)
-		/* on 6085, bits 3:0 are reserved, bit 6 control ARP
-		 * mirroring, and multicast forward is handled in
-		 * Port Control register.
-		 */
-		REG_WRITE(addr, 0x08, 0x0080);
-	else {
-		val = 0x0080 | dsa_upstream_port(ds);
-		if (p == dsa_upstream_port(ds))
-			val |= 0x0040;
-		REG_WRITE(addr, 0x08, val);
-	}
-
-	/* Rate Control: disable ingress rate limiting. */
-	REG_WRITE(addr, 0x09, 0x0000);
-
-	/* Rate Control 2: disable egress rate limiting. */
-	REG_WRITE(addr, 0x0a, 0x0000);
-
-	/* Port Association Vector: when learning source addresses
-	 * of packets, add the address to the address database using
-	 * a port bitmap that has only the bit for this port set and
-	 * the other bits clear.
-	 */
-	REG_WRITE(addr, 0x0b, 1 << p);
-
-	/* Tag Remap: use an identity 802.1p prio -> switch prio
-	 * mapping.
-	 */
-	REG_WRITE(addr, 0x18, 0x3210);
-
-	/* Tag Remap 2: use an identity 802.1p prio -> switch prio
-	 * mapping.
-	 */
-	REG_WRITE(addr, 0x19, 0x7654);
-
-	return mv88e6xxx_setup_port_common(ds, p);
-}
-
 static int mv88e6131_setup(struct dsa_switch *ds)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int i;
 	int ret;
 
 	ret = mv88e6xxx_setup_common(ds);
@@ -234,6 +112,7 @@ static int mv88e6131_setup(struct dsa_switch *ds)
 
 	switch (ps->id) {
 	case PORT_SWITCH_ID_6085:
+	case PORT_SWITCH_ID_6185:
 		ps->num_ports = 10;
 		break;
 	case PORT_SWITCH_ID_6095:
@@ -251,19 +130,11 @@ static int mv88e6131_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
-	/* @@@ initialise vtu and atu */
-
 	ret = mv88e6131_setup_global(ds);
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < ps->num_ports; i++) {
-		ret = mv88e6131_setup_port(ds, i);
-		if (ret < 0)
-			return ret;
-	}
-
-	return 0;
+	return mv88e6xxx_setup_ports(ds);
 }
 
 static int mv88e6131_port_to_phy_addr(struct dsa_switch *ds, int port)
diff --git a/drivers/net/dsa/mv88e6171.c b/drivers/net/dsa/mv88e6171.c
index 9104efea0e3e..1c7808495a9d 100644
--- a/drivers/net/dsa/mv88e6171.c
+++ b/drivers/net/dsa/mv88e6171.c
@@ -1,4 +1,4 @@
-/* net/dsa/mv88e6171.c - Marvell 88e6171/8826172 switch chip support
+/* net/dsa/mv88e6171.c - Marvell 88e6171 switch chip support
  * Copyright (c) 2008-2009 Marvell Semiconductor
  * Copyright (c) 2014 Claudio Leite <leitec@staticky.com>
  *
@@ -29,8 +29,12 @@ static char *mv88e6171_probe(struct device *host_dev, int sw_addr)
 	if (ret >= 0) {
 		if ((ret & 0xfff0) == PORT_SWITCH_ID_6171)
 			return "Marvell 88E6171";
-		if ((ret & 0xfff0) == PORT_SWITCH_ID_6172)
-			return "Marvell 88E6172";
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6175)
+			return "Marvell 88E6175";
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6350)
+			return "Marvell 88E6350";
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6351)
+			return "Marvell 88E6351";
 	}
 
 	return NULL;
@@ -38,196 +42,41 @@ static char *mv88e6171_probe(struct device *host_dev, int sw_addr)
 
 static int mv88e6171_setup_global(struct dsa_switch *ds)
 {
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	u32 upstream_port = dsa_upstream_port(ds);
 	int ret;
-	int i;
+	u32 reg;
+
+	ret = mv88e6xxx_setup_global(ds);
+	if (ret)
+		return ret;
 
 	/* Discard packets with excessive collisions, mask all
 	 * interrupt sources, enable PPU.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x04, 0x6000);
-
-	/* Set the default address aging time to 5 minutes, and
-	 * enable address learn messages to be sent to all message
-	 * ports.
-	 */
-	REG_WRITE(REG_GLOBAL, 0x0a, 0x0148);
-
-	/* Configure the priority mapping registers. */
-	ret = mv88e6xxx_config_prio(ds);
-	if (ret < 0)
-		return ret;
+	REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL,
+		  GLOBAL_CONTROL_PPU_ENABLE | GLOBAL_CONTROL_DISCARD_EXCESS);
 
 	/* Configure the upstream port, and configure the upstream
 	 * port as the port to which ingress and egress monitor frames
 	 * are to be sent.
 	 */
-	if (REG_READ(REG_PORT(0), 0x03) == 0x1710)
-		REG_WRITE(REG_GLOBAL, 0x1a, (dsa_upstream_port(ds) * 0x1111));
-	else
-		REG_WRITE(REG_GLOBAL, 0x1a, (dsa_upstream_port(ds) * 0x1110));
+	reg = upstream_port << GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT |
+		upstream_port << GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT |
+		upstream_port << GLOBAL_MONITOR_CONTROL_ARP_SHIFT |
+		upstream_port << GLOBAL_MONITOR_CONTROL_MIRROR_SHIFT;
+	REG_WRITE(REG_GLOBAL, GLOBAL_MONITOR_CONTROL, reg);
 
 	/* Disable remote management for now, and set the switch's
 	 * DSA device number.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x1c, ds->index & 0x1f);
-
-	/* Send all frames with destination addresses matching
-	 * 01:80:c2:00:00:2x to the CPU port.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x02, 0xffff);
-
-	/* Send all frames with destination addresses matching
-	 * 01:80:c2:00:00:0x to the CPU port.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x03, 0xffff);
-
-	/* Disable the loopback filter, disable flow control
-	 * messages, disable flood broadcast override, disable
-	 * removing of provider tags, disable ATU age violation
-	 * interrupts, disable tag flow control, force flow
-	 * control priority to the highest, and send all special
-	 * multicast frames to the CPU at the highest priority.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x05, 0x00ff);
-
-	/* Program the DSA routing table. */
-	for (i = 0; i < 32; i++) {
-		int nexthop;
-
-		nexthop = 0x1f;
-		if (i != ds->index && i < ds->dst->pd->nr_chips)
-			nexthop = ds->pd->rtable[i] & 0x1f;
-
-		REG_WRITE(REG_GLOBAL2, 0x06, 0x8000 | (i << 8) | nexthop);
-	}
-
-	/* Clear all trunk masks. */
-	for (i = 0; i < ps->num_ports; i++)
-		REG_WRITE(REG_GLOBAL2, 0x07, 0x8000 | (i << 12) | 0xff);
-
-	/* Clear all trunk mappings. */
-	for (i = 0; i < 16; i++)
-		REG_WRITE(REG_GLOBAL2, 0x08, 0x8000 | (i << 11));
-
-	/* Disable ingress rate limiting by resetting all ingress
-	 * rate limit registers to their initial state.
-	 */
-	for (i = 0; i < 6; i++)
-		REG_WRITE(REG_GLOBAL2, 0x09, 0x9000 | (i << 8));
-
-	/* Initialise cross-chip port VLAN table to reset defaults. */
-	REG_WRITE(REG_GLOBAL2, 0x0b, 0x9000);
-
-	/* Clear the priority override table. */
-	for (i = 0; i < 16; i++)
-		REG_WRITE(REG_GLOBAL2, 0x0f, 0x8000 | (i << 8));
-
-	/* @@@ initialise AVB (22/23) watchdog (27) sdet (29) registers */
+	REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL_2, ds->index & 0x1f);
 
 	return 0;
 }
 
-static int mv88e6171_setup_port(struct dsa_switch *ds, int p)
-{
-	int addr = REG_PORT(p);
-	u16 val;
-
-	/* MAC Forcing register: don't force link, speed, duplex
-	 * or flow control state to any particular values on physical
-	 * ports, but force the CPU port and all DSA ports to 1000 Mb/s
-	 * full duplex.
-	 */
-	val = REG_READ(addr, 0x01);
-	if (dsa_is_cpu_port(ds, p) || ds->dsa_port_mask & (1 << p))
-		REG_WRITE(addr, 0x01, val | 0x003e);
-	else
-		REG_WRITE(addr, 0x01, val | 0x0003);
-
-	/* Do not limit the period of time that this port can be
-	 * paused for by the remote end or the period of time that
-	 * this port can pause the remote end.
-	 */
-	REG_WRITE(addr, 0x02, 0x0000);
-
-	/* Port Control: disable Drop-on-Unlock, disable Drop-on-Lock,
-	 * disable Header mode, enable IGMP/MLD snooping, disable VLAN
-	 * tunneling, determine priority by looking at 802.1p and IP
-	 * priority fields (IP prio has precedence), and set STP state
-	 * to Forwarding.
-	 *
-	 * If this is the CPU link, use DSA or EDSA tagging depending
-	 * on which tagging mode was configured.
-	 *
-	 * If this is a link to another switch, use DSA tagging mode.
-	 *
-	 * If this is the upstream port for this switch, enable
-	 * forwarding of unknown unicasts and multicasts.
-	 */
-	val = 0x0433;
-	if (dsa_is_cpu_port(ds, p)) {
-		if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
-			val |= 0x3300;
-		else
-			val |= 0x0100;
-	}
-	if (ds->dsa_port_mask & (1 << p))
-		val |= 0x0100;
-	if (p == dsa_upstream_port(ds))
-		val |= 0x000c;
-	REG_WRITE(addr, 0x04, val);
-
-	/* Port Control 2: don't force a good FCS, set the maximum
-	 * frame size to 10240 bytes, don't let the switch add or
-	 * strip 802.1q tags, don't discard tagged or untagged frames
-	 * on this port, do a destination address lookup on all
-	 * received packets as usual, disable ARP mirroring and don't
-	 * send a copy of all transmitted/received frames on this port
-	 * to the CPU.
-	 */
-	REG_WRITE(addr, 0x08, 0x2080);
-
-	/* Egress rate control: disable egress rate control. */
-	REG_WRITE(addr, 0x09, 0x0001);
-
-	/* Egress rate control 2: disable egress rate control. */
-	REG_WRITE(addr, 0x0a, 0x0000);
-
-	/* Port Association Vector: when learning source addresses
-	 * of packets, add the address to the address database using
-	 * a port bitmap that has only the bit for this port set and
-	 * the other bits clear.
-	 */
-	REG_WRITE(addr, 0x0b, 1 << p);
-
-	/* Port ATU control: disable limiting the number of address
-	 * database entries that this port is allowed to use.
-	 */
-	REG_WRITE(addr, 0x0c, 0x0000);
-
-	/* Priority Override: disable DA, SA and VTU priority override. */
-	REG_WRITE(addr, 0x0d, 0x0000);
-
-	/* Port Ethertype: use the Ethertype DSA Ethertype value. */
-	REG_WRITE(addr, 0x0f, ETH_P_EDSA);
-
-	/* Tag Remap: use an identity 802.1p prio -> switch prio
-	 * mapping.
-	 */
-	REG_WRITE(addr, 0x18, 0x3210);
-
-	/* Tag Remap 2: use an identity 802.1p prio -> switch prio
-	 * mapping.
-	 */
-	REG_WRITE(addr, 0x19, 0x7654);
-
-	return mv88e6xxx_setup_port_common(ds, p);
-}
-
 static int mv88e6171_setup(struct dsa_switch *ds)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int i;
 	int ret;
 
 	ret = mv88e6xxx_setup_common(ds);
@@ -240,44 +89,11 @@ static int mv88e6171_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
-	/* @@@ initialise vtu and atu */
-
 	ret = mv88e6171_setup_global(ds);
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < ps->num_ports; i++) {
-		if (!(dsa_is_cpu_port(ds, i) || ds->phys_port_mask & (1 << i)))
-			continue;
-
-		ret = mv88e6171_setup_port(ds, i);
-		if (ret < 0)
-			return ret;
-	}
-
-	return 0;
-}
-
-static int mv88e6171_get_eee(struct dsa_switch *ds, int port,
-			     struct ethtool_eee *e)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-
-	if (ps->id == PORT_SWITCH_ID_6172)
-		return mv88e6xxx_get_eee(ds, port, e);
-
-	return -EOPNOTSUPP;
-}
-
-static int mv88e6171_set_eee(struct dsa_switch *ds, int port,
-			     struct phy_device *phydev, struct ethtool_eee *e)
-{
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-
-	if (ps->id == PORT_SWITCH_ID_6172)
-		return mv88e6xxx_set_eee(ds, port, phydev, e);
-
-	return -EOPNOTSUPP;
+	return mv88e6xxx_setup_ports(ds);
 }
 
 struct dsa_switch_driver mv88e6171_switch_driver = {
@@ -292,8 +108,6 @@ struct dsa_switch_driver mv88e6171_switch_driver = {
 	.get_strings		= mv88e6xxx_get_strings,
 	.get_ethtool_stats	= mv88e6xxx_get_ethtool_stats,
 	.get_sset_count		= mv88e6xxx_get_sset_count,
-	.set_eee		= mv88e6171_set_eee,
-	.get_eee		= mv88e6171_get_eee,
 #ifdef CONFIG_NET_DSA_HWMON
 	.get_temp               = mv88e6xxx_get_temp,
 #endif
@@ -308,4 +122,6 @@ struct dsa_switch_driver mv88e6171_switch_driver = {
 };
 
 MODULE_ALIAS("platform:mv88e6171");
-MODULE_ALIAS("platform:mv88e6172");
+MODULE_ALIAS("platform:mv88e6175");
+MODULE_ALIAS("platform:mv88e6350");
+MODULE_ALIAS("platform:mv88e6351");
diff --git a/drivers/net/dsa/mv88e6352.c b/drivers/net/dsa/mv88e6352.c
index 126c11b81e75..632815c10a40 100644
--- a/drivers/net/dsa/mv88e6352.c
+++ b/drivers/net/dsa/mv88e6352.c
@@ -32,6 +32,8 @@ static char *mv88e6352_probe(struct device *host_dev, int sw_addr)
 
 	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), PORT_SWITCH_ID);
 	if (ret >= 0) {
+		if ((ret & 0xfff0) == PORT_SWITCH_ID_6172)
+			return "Marvell 88E6172";
 		if ((ret & 0xfff0) == PORT_SWITCH_ID_6176)
 			return "Marvell 88E6176";
 		if (ret == PORT_SWITCH_ID_6352_A0)
@@ -47,187 +49,37 @@ static char *mv88e6352_probe(struct device *host_dev, int sw_addr)
 
 static int mv88e6352_setup_global(struct dsa_switch *ds)
 {
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	u32 upstream_port = dsa_upstream_port(ds);
 	int ret;
-	int i;
+	u32 reg;
+
+	ret = mv88e6xxx_setup_global(ds);
+	if (ret)
+		return ret;
 
 	/* Discard packets with excessive collisions,
 	 * mask all interrupt sources, enable PPU (bit 14, undocumented).
 	 */
-	REG_WRITE(REG_GLOBAL, 0x04, 0x6000);
-
-	/* Set the default address aging time to 5 minutes, and
-	 * enable address learn messages to be sent to all message
-	 * ports.
-	 */
-	REG_WRITE(REG_GLOBAL, 0x0a, 0x0148);
-
-	/* Configure the priority mapping registers. */
-	ret = mv88e6xxx_config_prio(ds);
-	if (ret < 0)
-		return ret;
+	REG_WRITE(REG_GLOBAL, GLOBAL_CONTROL,
+		  GLOBAL_CONTROL_PPU_ENABLE | GLOBAL_CONTROL_DISCARD_EXCESS);
 
 	/* Configure the upstream port, and configure the upstream
 	 * port as the port to which ingress and egress monitor frames
 	 * are to be sent.
 	 */
-	REG_WRITE(REG_GLOBAL, 0x1a, (dsa_upstream_port(ds) * 0x1110));
+	reg = upstream_port << GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT |
+		upstream_port << GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT |
+		upstream_port << GLOBAL_MONITOR_CONTROL_ARP_SHIFT;
+	REG_WRITE(REG_GLOBAL, GLOBAL_MONITOR_CONTROL, reg);
 
 	/* Disable remote management for now, and set the switch's
 	 * DSA device number.
 	 */
 	REG_WRITE(REG_GLOBAL, 0x1c, ds->index & 0x1f);
 
-	/* Send all frames with destination addresses matching
-	 * 01:80:c2:00:00:2x to the CPU port.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x02, 0xffff);
-
-	/* Send all frames with destination addresses matching
-	 * 01:80:c2:00:00:0x to the CPU port.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x03, 0xffff);
-
-	/* Disable the loopback filter, disable flow control
-	 * messages, disable flood broadcast override, disable
-	 * removing of provider tags, disable ATU age violation
-	 * interrupts, disable tag flow control, force flow
-	 * control priority to the highest, and send all special
-	 * multicast frames to the CPU at the highest priority.
-	 */
-	REG_WRITE(REG_GLOBAL2, 0x05, 0x00ff);
-
-	/* Program the DSA routing table. */
-	for (i = 0; i < 32; i++) {
-		int nexthop = 0x1f;
-
-		if (i != ds->index && i < ds->dst->pd->nr_chips)
-			nexthop = ds->pd->rtable[i] & 0x1f;
-
-		REG_WRITE(REG_GLOBAL2, 0x06, 0x8000 | (i << 8) | nexthop);
-	}
-
-	/* Clear all trunk masks. */
-	for (i = 0; i < 8; i++)
-		REG_WRITE(REG_GLOBAL2, 0x07, 0x8000 | (i << 12) | 0x7f);
-
-	/* Clear all trunk mappings. */
-	for (i = 0; i < 16; i++)
-		REG_WRITE(REG_GLOBAL2, 0x08, 0x8000 | (i << 11));
-
-	/* Disable ingress rate limiting by resetting all ingress
-	 * rate limit registers to their initial state.
-	 */
-	for (i = 0; i < ps->num_ports; i++)
-		REG_WRITE(REG_GLOBAL2, 0x09, 0x9000 | (i << 8));
-
-	/* Initialise cross-chip port VLAN table to reset defaults. */
-	REG_WRITE(REG_GLOBAL2, 0x0b, 0x9000);
-
-	/* Clear the priority override table. */
-	for (i = 0; i < 16; i++)
-		REG_WRITE(REG_GLOBAL2, 0x0f, 0x8000 | (i << 8));
-
-	/* @@@ initialise AVB (22/23) watchdog (27) sdet (29) registers */
-
 	return 0;
 }
 
-static int mv88e6352_setup_port(struct dsa_switch *ds, int p)
-{
-	int addr = REG_PORT(p);
-	u16 val;
-
-	/* MAC Forcing register: don't force link, speed, duplex
-	 * or flow control state to any particular values on physical
-	 * ports, but force the CPU port and all DSA ports to 1000 Mb/s
-	 * full duplex.
-	 */
-	if (dsa_is_cpu_port(ds, p) || ds->dsa_port_mask & (1 << p))
-		REG_WRITE(addr, 0x01, 0x003e);
-	else
-		REG_WRITE(addr, 0x01, 0x0003);
-
-	/* Do not limit the period of time that this port can be
-	 * paused for by the remote end or the period of time that
-	 * this port can pause the remote end.
-	 */
-	REG_WRITE(addr, 0x02, 0x0000);
-
-	/* Port Control: disable Drop-on-Unlock, disable Drop-on-Lock,
-	 * disable Header mode, enable IGMP/MLD snooping, disable VLAN
-	 * tunneling, determine priority by looking at 802.1p and IP
-	 * priority fields (IP prio has precedence), and set STP state
-	 * to Forwarding.
-	 *
-	 * If this is the CPU link, use DSA or EDSA tagging depending
-	 * on which tagging mode was configured.
-	 *
-	 * If this is a link to another switch, use DSA tagging mode.
-	 *
-	 * If this is the upstream port for this switch, enable
-	 * forwarding of unknown unicasts and multicasts.
-	 */
-	val = 0x0433;
-	if (dsa_is_cpu_port(ds, p)) {
-		if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
-			val |= 0x3300;
-		else
-			val |= 0x0100;
-	}
-	if (ds->dsa_port_mask & (1 << p))
-		val |= 0x0100;
-	if (p == dsa_upstream_port(ds))
-		val |= 0x000c;
-	REG_WRITE(addr, 0x04, val);
-
-	/* Port Control 2: don't force a good FCS, set the maximum
-	 * frame size to 10240 bytes, don't let the switch add or
-	 * strip 802.1q tags, don't discard tagged or untagged frames
-	 * on this port, do a destination address lookup on all
-	 * received packets as usual, disable ARP mirroring and don't
-	 * send a copy of all transmitted/received frames on this port
-	 * to the CPU.
-	 */
-	REG_WRITE(addr, 0x08, 0x2080);
-
-	/* Egress rate control: disable egress rate control. */
-	REG_WRITE(addr, 0x09, 0x0001);
-
-	/* Egress rate control 2: disable egress rate control. */
-	REG_WRITE(addr, 0x0a, 0x0000);
-
-	/* Port Association Vector: when learning source addresses
-	 * of packets, add the address to the address database using
-	 * a port bitmap that has only the bit for this port set and
-	 * the other bits clear.
-	 */
-	REG_WRITE(addr, 0x0b, 1 << p);
-
-	/* Port ATU control: disable limiting the number of address
-	 * database entries that this port is allowed to use.
-	 */
-	REG_WRITE(addr, 0x0c, 0x0000);
-
-	/* Priority Override: disable DA, SA and VTU priority override. */
-	REG_WRITE(addr, 0x0d, 0x0000);
-
-	/* Port Ethertype: use the Ethertype DSA Ethertype value. */
-	REG_WRITE(addr, 0x0f, ETH_P_EDSA);
-
-	/* Tag Remap: use an identity 802.1p prio -> switch prio
-	 * mapping.
-	 */
-	REG_WRITE(addr, 0x18, 0x3210);
-
-	/* Tag Remap 2: use an identity 802.1p prio -> switch prio
-	 * mapping.
-	 */
-	REG_WRITE(addr, 0x19, 0x7654);
-
-	return mv88e6xxx_setup_port_common(ds, p);
-}
-
 #ifdef CONFIG_NET_DSA_HWMON
 
 static int mv88e6352_get_temp(struct dsa_switch *ds, int *temp)
@@ -292,7 +144,6 @@ static int mv88e6352_setup(struct dsa_switch *ds)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int ret;
-	int i;
 
 	ret = mv88e6xxx_setup_common(ds);
 	if (ret < 0)
@@ -306,19 +157,11 @@ static int mv88e6352_setup(struct dsa_switch *ds)
 	if (ret < 0)
 		return ret;
 
-	/* @@@ initialise vtu and atu */
-
 	ret = mv88e6352_setup_global(ds);
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < ps->num_ports; i++) {
-		ret = mv88e6352_setup_port(ds, i);
-		if (ret < 0)
-			return ret;
-	}
-
-	return 0;
+	return mv88e6xxx_setup_ports(ds);
 }
 
 static int mv88e6352_read_eeprom_word(struct dsa_switch *ds, int addr)
@@ -552,3 +395,4 @@ struct dsa_switch_driver mv88e6352_switch_driver = {
 };
 
 MODULE_ALIAS("platform:mv88e6352");
+MODULE_ALIAS("platform:mv88e6172");
diff --git a/drivers/net/dsa/mv88e6xxx.c b/drivers/net/dsa/mv88e6xxx.c
index cf309aa92802..fd8547c2b79d 100644
--- a/drivers/net/dsa/mv88e6xxx.c
+++ b/drivers/net/dsa/mv88e6xxx.c
@@ -8,6 +8,7 @@
  * (at your option) any later version.
  */
 
+#include <linux/debugfs.h>
 #include <linux/delay.h>
 #include <linux/etherdevice.h>
 #include <linux/if_bridge.h>
@@ -16,9 +17,38 @@
 #include <linux/module.h>
 #include <linux/netdevice.h>
 #include <linux/phy.h>
+#include <linux/seq_file.h>
 #include <net/dsa.h>
 #include "mv88e6xxx.h"
 
+/* MDIO bus access can be nested in the case of PHYs connected to the
+ * internal MDIO bus of the switch, which is accessed via MDIO bus of
+ * the Ethernet interface. Avoid lockdep false positives by using
+ * mutex_lock_nested().
+ */
+static int mv88e6xxx_mdiobus_read(struct mii_bus *bus, int addr, u32 regnum)
+{
+	int ret;
+
+	mutex_lock_nested(&bus->mdio_lock, SINGLE_DEPTH_NESTING);
+	ret = bus->read(bus, addr, regnum);
+	mutex_unlock(&bus->mdio_lock);
+
+	return ret;
+}
+
+static int mv88e6xxx_mdiobus_write(struct mii_bus *bus, int addr, u32 regnum,
+				   u16 val)
+{
+	int ret;
+
+	mutex_lock_nested(&bus->mdio_lock, SINGLE_DEPTH_NESTING);
+	ret = bus->write(bus, addr, regnum, val);
+	mutex_unlock(&bus->mdio_lock);
+
+	return ret;
+}
+
 /* If the switch's ADDR[4:0] strap pins are strapped to zero, it will
  * use all 32 SMI bus addresses on its SMI bus, and all switch registers
  * will be directly accessible on some {device address,register address}
@@ -33,7 +63,7 @@ static int mv88e6xxx_reg_wait_ready(struct mii_bus *bus, int sw_addr)
 	int i;
 
 	for (i = 0; i < 16; i++) {
-		ret = mdiobus_read(bus, sw_addr, SMI_CMD);
+		ret = mv88e6xxx_mdiobus_read(bus, sw_addr, SMI_CMD);
 		if (ret < 0)
 			return ret;
 
@@ -49,7 +79,7 @@ int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg)
 	int ret;
 
 	if (sw_addr == 0)
-		return mdiobus_read(bus, addr, reg);
+		return mv88e6xxx_mdiobus_read(bus, addr, reg);
 
 	/* Wait for the bus to become free. */
 	ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
@@ -57,8 +87,8 @@ int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg)
 		return ret;
 
 	/* Transmit the read command. */
-	ret = mdiobus_write(bus, sw_addr, SMI_CMD,
-			    SMI_CMD_OP_22_READ | (addr << 5) | reg);
+	ret = mv88e6xxx_mdiobus_write(bus, sw_addr, SMI_CMD,
+				      SMI_CMD_OP_22_READ | (addr << 5) | reg);
 	if (ret < 0)
 		return ret;
 
@@ -68,7 +98,7 @@ int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg)
 		return ret;
 
 	/* Read the data. */
-	ret = mdiobus_read(bus, sw_addr, SMI_DATA);
+	ret = mv88e6xxx_mdiobus_read(bus, sw_addr, SMI_DATA);
 	if (ret < 0)
 		return ret;
 
@@ -112,7 +142,7 @@ int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 	int ret;
 
 	if (sw_addr == 0)
-		return mdiobus_write(bus, addr, reg, val);
+		return mv88e6xxx_mdiobus_write(bus, addr, reg, val);
 
 	/* Wait for the bus to become free. */
 	ret = mv88e6xxx_reg_wait_ready(bus, sw_addr);
@@ -120,13 +150,13 @@ int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 		return ret;
 
 	/* Transmit the data to write. */
-	ret = mdiobus_write(bus, sw_addr, SMI_DATA, val);
+	ret = mv88e6xxx_mdiobus_write(bus, sw_addr, SMI_DATA, val);
 	if (ret < 0)
 		return ret;
 
 	/* Transmit the write command. */
-	ret = mdiobus_write(bus, sw_addr, SMI_CMD,
-			    SMI_CMD_OP_22_WRITE | (addr << 5) | reg);
+	ret = mv88e6xxx_mdiobus_write(bus, sw_addr, SMI_CMD,
+				      SMI_CMD_OP_22_WRITE | (addr << 5) | reg);
 	if (ret < 0)
 		return ret;
 
@@ -165,24 +195,6 @@ int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 	return ret;
 }
 
-int mv88e6xxx_config_prio(struct dsa_switch *ds)
-{
-	/* Configure the IP ToS mapping registers. */
-	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_0, 0x0000);
-	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_1, 0x0000);
-	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_2, 0x5555);
-	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_3, 0x5555);
-	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_4, 0xaaaa);
-	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_5, 0xaaaa);
-	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_6, 0xffff);
-	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_7, 0xffff);
-
-	/* Configure the IEEE 802.1p priority mapping register. */
-	REG_WRITE(REG_GLOBAL, GLOBAL_IEEE_PRI, 0xfa41);
-
-	return 0;
-}
-
 int mv88e6xxx_set_addr_direct(struct dsa_switch *ds, u8 *addr)
 {
 	REG_WRITE(REG_GLOBAL, GLOBAL_MAC_01, (addr[0] << 8) | addr[1]);
@@ -217,20 +229,20 @@ int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr)
 	return 0;
 }
 
-/* Must be called with phy mutex held */
+/* Must be called with SMI mutex held */
 static int _mv88e6xxx_phy_read(struct dsa_switch *ds, int addr, int regnum)
 {
 	if (addr >= 0)
-		return mv88e6xxx_reg_read(ds, addr, regnum);
+		return _mv88e6xxx_reg_read(ds, addr, regnum);
 	return 0xffff;
 }
 
-/* Must be called with phy mutex held */
+/* Must be called with SMI mutex held */
 static int _mv88e6xxx_phy_write(struct dsa_switch *ds, int addr, int regnum,
 				u16 val)
 {
 	if (addr >= 0)
-		return mv88e6xxx_reg_write(ds, addr, regnum, val);
+		return _mv88e6xxx_reg_write(ds, addr, regnum, val);
 	return 0;
 }
 
@@ -434,26 +446,113 @@ void mv88e6xxx_poll_link(struct dsa_switch *ds)
 	}
 }
 
+static bool mv88e6xxx_6065_family(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6031:
+	case PORT_SWITCH_ID_6061:
+	case PORT_SWITCH_ID_6035:
+	case PORT_SWITCH_ID_6065:
+		return true;
+	}
+	return false;
+}
+
+static bool mv88e6xxx_6095_family(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6092:
+	case PORT_SWITCH_ID_6095:
+		return true;
+	}
+	return false;
+}
+
+static bool mv88e6xxx_6097_family(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6046:
+	case PORT_SWITCH_ID_6085:
+	case PORT_SWITCH_ID_6096:
+	case PORT_SWITCH_ID_6097:
+		return true;
+	}
+	return false;
+}
+
+static bool mv88e6xxx_6165_family(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6123:
+	case PORT_SWITCH_ID_6161:
+	case PORT_SWITCH_ID_6165:
+		return true;
+	}
+	return false;
+}
+
+static bool mv88e6xxx_6185_family(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6121:
+	case PORT_SWITCH_ID_6122:
+	case PORT_SWITCH_ID_6152:
+	case PORT_SWITCH_ID_6155:
+	case PORT_SWITCH_ID_6182:
+	case PORT_SWITCH_ID_6185:
+	case PORT_SWITCH_ID_6108:
+	case PORT_SWITCH_ID_6131:
+		return true;
+	}
+	return false;
+}
+
+static bool mv88e6xxx_6351_family(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	switch (ps->id) {
+	case PORT_SWITCH_ID_6171:
+	case PORT_SWITCH_ID_6175:
+	case PORT_SWITCH_ID_6350:
+	case PORT_SWITCH_ID_6351:
+		return true;
+	}
+	return false;
+}
+
 static bool mv88e6xxx_6352_family(struct dsa_switch *ds)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 
 	switch (ps->id) {
-	case PORT_SWITCH_ID_6352:
 	case PORT_SWITCH_ID_6172:
 	case PORT_SWITCH_ID_6176:
+	case PORT_SWITCH_ID_6240:
+	case PORT_SWITCH_ID_6352:
 		return true;
 	}
 	return false;
 }
 
-static int mv88e6xxx_stats_wait(struct dsa_switch *ds)
+/* Must be called with SMI mutex held */
+static int _mv88e6xxx_stats_wait(struct dsa_switch *ds)
 {
 	int ret;
 	int i;
 
 	for (i = 0; i < 10; i++) {
-		ret = REG_READ(REG_GLOBAL, GLOBAL_STATS_OP);
+		ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_OP);
 		if ((ret & GLOBAL_STATS_OP_BUSY) == 0)
 			return 0;
 	}
@@ -461,7 +560,8 @@ static int mv88e6xxx_stats_wait(struct dsa_switch *ds)
 	return -ETIMEDOUT;
 }
 
-static int mv88e6xxx_stats_snapshot(struct dsa_switch *ds, int port)
+/* Must be called with SMI mutex held */
+static int _mv88e6xxx_stats_snapshot(struct dsa_switch *ds, int port)
 {
 	int ret;
 
@@ -469,42 +569,45 @@ static int mv88e6xxx_stats_snapshot(struct dsa_switch *ds, int port)
 		port = (port + 1) << 5;
 
 	/* Snapshot the hardware statistics counters for this port. */
-	REG_WRITE(REG_GLOBAL, GLOBAL_STATS_OP,
-		  GLOBAL_STATS_OP_CAPTURE_PORT |
-		  GLOBAL_STATS_OP_HIST_RX_TX | port);
+	ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_STATS_OP,
+				   GLOBAL_STATS_OP_CAPTURE_PORT |
+				   GLOBAL_STATS_OP_HIST_RX_TX | port);
+	if (ret < 0)
+		return ret;
 
 	/* Wait for the snapshotting to complete. */
-	ret = mv88e6xxx_stats_wait(ds);
+	ret = _mv88e6xxx_stats_wait(ds);
 	if (ret < 0)
 		return ret;
 
 	return 0;
 }
 
-static void mv88e6xxx_stats_read(struct dsa_switch *ds, int stat, u32 *val)
+/* Must be called with SMI mutex held */
+static void _mv88e6xxx_stats_read(struct dsa_switch *ds, int stat, u32 *val)
 {
 	u32 _val;
 	int ret;
 
 	*val = 0;
 
-	ret = mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_STATS_OP,
-				  GLOBAL_STATS_OP_READ_CAPTURED |
-				  GLOBAL_STATS_OP_HIST_RX_TX | stat);
+	ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL, GLOBAL_STATS_OP,
+				   GLOBAL_STATS_OP_READ_CAPTURED |
+				   GLOBAL_STATS_OP_HIST_RX_TX | stat);
 	if (ret < 0)
 		return;
 
-	ret = mv88e6xxx_stats_wait(ds);
+	ret = _mv88e6xxx_stats_wait(ds);
 	if (ret < 0)
 		return;
 
-	ret = mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_COUNTER_32);
+	ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_COUNTER_32);
 	if (ret < 0)
 		return;
 
 	_val = ret << 16;
 
-	ret = mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_COUNTER_01);
+	ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_STATS_COUNTER_01);
 	if (ret < 0)
 		return;
 
@@ -578,6 +681,40 @@ static void _mv88e6xxx_get_strings(struct dsa_switch *ds,
 	}
 }
 
+static uint64_t _mv88e6xxx_get_ethtool_stat(struct dsa_switch *ds,
+					    int stat,
+					    struct mv88e6xxx_hw_stat *stats,
+					    int port)
+{
+	struct mv88e6xxx_hw_stat *s = stats + stat;
+	u32 low;
+	u32 high = 0;
+	int ret;
+	u64 value;
+
+	if (s->reg >= 0x100) {
+		ret = _mv88e6xxx_reg_read(ds, REG_PORT(port),
+					  s->reg - 0x100);
+		if (ret < 0)
+			return UINT64_MAX;
+
+		low = ret;
+		if (s->sizeof_stat == 4) {
+			ret = _mv88e6xxx_reg_read(ds, REG_PORT(port),
+						  s->reg - 0x100 + 1);
+			if (ret < 0)
+				return UINT64_MAX;
+			high = ret;
+		}
+	} else {
+		_mv88e6xxx_stats_read(ds, s->reg, &low);
+		if (s->sizeof_stat == 8)
+			_mv88e6xxx_stats_read(ds, s->reg + 1, &high);
+	}
+	value = (((u64)high) << 16) | low;
+	return value;
+}
+
 static void _mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
 					 int nr_stats,
 					 struct mv88e6xxx_hw_stat *stats,
@@ -587,44 +724,19 @@ static void _mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
 	int ret;
 	int i;
 
-	mutex_lock(&ps->stats_mutex);
+	mutex_lock(&ps->smi_mutex);
 
-	ret = mv88e6xxx_stats_snapshot(ds, port);
+	ret = _mv88e6xxx_stats_snapshot(ds, port);
 	if (ret < 0) {
-		mutex_unlock(&ps->stats_mutex);
+		mutex_unlock(&ps->smi_mutex);
 		return;
 	}
 
 	/* Read each of the counters. */
-	for (i = 0; i < nr_stats; i++) {
-		struct mv88e6xxx_hw_stat *s = stats + i;
-		u32 low;
-		u32 high = 0;
+	for (i = 0; i < nr_stats; i++)
+		data[i] = _mv88e6xxx_get_ethtool_stat(ds, i, stats, port);
 
-		if (s->reg >= 0x100) {
-			ret = mv88e6xxx_reg_read(ds, REG_PORT(port),
-						 s->reg - 0x100);
-			if (ret < 0)
-				goto error;
-			low = ret;
-			if (s->sizeof_stat == 4) {
-				ret = mv88e6xxx_reg_read(ds, REG_PORT(port),
-							 s->reg - 0x100 + 1);
-				if (ret < 0)
-					goto error;
-				high = ret;
-			}
-			data[i] = (((u64)high) << 16) | low;
-			continue;
-		}
-		mv88e6xxx_stats_read(ds, s->reg, &low);
-		if (s->sizeof_stat == 8)
-			mv88e6xxx_stats_read(ds, s->reg + 1, &high);
-
-		data[i] = (((u64)high) << 32) | low;
-	}
-error:
-	mutex_unlock(&ps->stats_mutex);
+	mutex_unlock(&ps->smi_mutex);
 }
 
 /* All the statistics in the table */
@@ -694,7 +806,7 @@ int  mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
 
 	*temp = 0;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 
 	ret = _mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x6);
 	if (ret < 0)
@@ -727,19 +839,23 @@ int  mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
 
 error:
 	_mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x0);
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 	return ret;
 }
 #endif /* CONFIG_NET_DSA_HWMON */
 
-static int mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
+/* Must be called with SMI lock held */
+static int _mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset,
+			   u16 mask)
 {
 	unsigned long timeout = jiffies + HZ / 10;
 
 	while (time_before(jiffies, timeout)) {
 		int ret;
 
-		ret = REG_READ(reg, offset);
+		ret = _mv88e6xxx_reg_read(ds, reg, offset);
+		if (ret < 0)
+			return ret;
 		if (!(ret & mask))
 			return 0;
 
@@ -748,10 +864,22 @@ static int mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
 	return -ETIMEDOUT;
 }
 
-int mv88e6xxx_phy_wait(struct dsa_switch *ds)
+static int mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
 {
-	return mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_SMI_OP,
-			      GLOBAL2_SMI_OP_BUSY);
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+
+	mutex_lock(&ps->smi_mutex);
+	ret = _mv88e6xxx_wait(ds, reg, offset, mask);
+	mutex_unlock(&ps->smi_mutex);
+
+	return ret;
+}
+
+static int _mv88e6xxx_phy_wait(struct dsa_switch *ds)
+{
+	return _mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_SMI_OP,
+			       GLOBAL2_SMI_OP_BUSY);
 }
 
 int mv88e6xxx_eeprom_load_wait(struct dsa_switch *ds)
@@ -767,56 +895,53 @@ int mv88e6xxx_eeprom_busy_wait(struct dsa_switch *ds)
 }
 
 /* Must be called with SMI lock held */
-static int _mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
+static int _mv88e6xxx_atu_wait(struct dsa_switch *ds)
 {
-	unsigned long timeout = jiffies + HZ / 10;
-
-	while (time_before(jiffies, timeout)) {
-		int ret;
-
-		ret = _mv88e6xxx_reg_read(ds, reg, offset);
-		if (ret < 0)
-			return ret;
-		if (!(ret & mask))
-			return 0;
-
-		usleep_range(1000, 2000);
-	}
-	return -ETIMEDOUT;
+	return _mv88e6xxx_wait(ds, REG_GLOBAL, GLOBAL_ATU_OP,
+			       GLOBAL_ATU_OP_BUSY);
 }
 
 /* Must be called with SMI lock held */
-static int _mv88e6xxx_atu_wait(struct dsa_switch *ds)
+static int _mv88e6xxx_scratch_wait(struct dsa_switch *ds)
 {
-	return _mv88e6xxx_wait(ds, REG_GLOBAL, GLOBAL_ATU_OP,
-			       GLOBAL_ATU_OP_BUSY);
+	return _mv88e6xxx_wait(ds, REG_GLOBAL2, GLOBAL2_SCRATCH_MISC,
+			       GLOBAL2_SCRATCH_BUSY);
 }
 
-/* Must be called with phy mutex held */
+/* Must be called with SMI mutex held */
 static int _mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr,
 					int regnum)
 {
 	int ret;
 
-	REG_WRITE(REG_GLOBAL2, GLOBAL2_SMI_OP,
-		  GLOBAL2_SMI_OP_22_READ | (addr << 5) | regnum);
+	ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL2, GLOBAL2_SMI_OP,
+				   GLOBAL2_SMI_OP_22_READ | (addr << 5) |
+				   regnum);
+	if (ret < 0)
+		return ret;
 
-	ret = mv88e6xxx_phy_wait(ds);
+	ret = _mv88e6xxx_phy_wait(ds);
 	if (ret < 0)
 		return ret;
 
-	return REG_READ(REG_GLOBAL2, GLOBAL2_SMI_DATA);
+	return _mv88e6xxx_reg_read(ds, REG_GLOBAL2, GLOBAL2_SMI_DATA);
 }
 
-/* Must be called with phy mutex held */
+/* Must be called with SMI mutex held */
 static int _mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int addr,
 					 int regnum, u16 val)
 {
-	REG_WRITE(REG_GLOBAL2, GLOBAL2_SMI_DATA, val);
-	REG_WRITE(REG_GLOBAL2, GLOBAL2_SMI_OP,
-		  GLOBAL2_SMI_OP_22_WRITE | (addr << 5) | regnum);
+	int ret;
 
-	return mv88e6xxx_phy_wait(ds);
+	ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL2, GLOBAL2_SMI_DATA, val);
+	if (ret < 0)
+		return ret;
+
+	ret = _mv88e6xxx_reg_write(ds, REG_GLOBAL2, GLOBAL2_SMI_OP,
+				   GLOBAL2_SMI_OP_22_WRITE | (addr << 5) |
+				   regnum);
+
+	return _mv88e6xxx_phy_wait(ds);
 }
 
 int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
@@ -824,7 +949,7 @@ int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int reg;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 
 	reg = _mv88e6xxx_phy_read_indirect(ds, port, 16);
 	if (reg < 0)
@@ -833,7 +958,7 @@ int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
 	e->eee_enabled = !!(reg & 0x0200);
 	e->tx_lpi_enabled = !!(reg & 0x0100);
 
-	reg = mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_STATUS);
+	reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_STATUS);
 	if (reg < 0)
 		goto out;
 
@@ -841,7 +966,7 @@ int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
 	reg = 0;
 
 out:
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 	return reg;
 }
 
@@ -852,7 +977,7 @@ int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
 	int reg;
 	int ret;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 
 	ret = _mv88e6xxx_phy_read_indirect(ds, port, 16);
 	if (ret < 0)
@@ -866,7 +991,7 @@ int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
 
 	ret = _mv88e6xxx_phy_write_indirect(ds, port, 16, reg);
 out:
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 
 	return ret;
 }
@@ -1241,13 +1366,212 @@ static void mv88e6xxx_bridge_work(struct work_struct *work)
 	}
 }
 
-int mv88e6xxx_setup_port_common(struct dsa_switch *ds, int port)
+static int mv88e6xxx_setup_port(struct dsa_switch *ds, int port)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int ret, fid;
+	u16 reg;
 
 	mutex_lock(&ps->smi_mutex);
 
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
+	    mv88e6xxx_6185_family(ds) || mv88e6xxx_6095_family(ds) ||
+	    mv88e6xxx_6065_family(ds)) {
+		/* MAC Forcing register: don't force link, speed,
+		 * duplex or flow control state to any particular
+		 * values on physical ports, but force the CPU port
+		 * and all DSA ports to their maximum bandwidth and
+		 * full duplex.
+		 */
+		reg = _mv88e6xxx_reg_read(ds, REG_PORT(port), PORT_PCS_CTRL);
+		if (dsa_is_cpu_port(ds, port) ||
+		    ds->dsa_port_mask & (1 << port)) {
+			reg |= PORT_PCS_CTRL_FORCE_LINK |
+				PORT_PCS_CTRL_LINK_UP |
+				PORT_PCS_CTRL_DUPLEX_FULL |
+				PORT_PCS_CTRL_FORCE_DUPLEX;
+			if (mv88e6xxx_6065_family(ds))
+				reg |= PORT_PCS_CTRL_100;
+			else
+				reg |= PORT_PCS_CTRL_1000;
+		} else {
+			reg |= PORT_PCS_CTRL_UNFORCED;
+		}
+
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_PCS_CTRL, reg);
+		if (ret)
+			goto abort;
+	}
+
+	/* Port Control: disable Drop-on-Unlock, disable Drop-on-Lock,
+	 * disable Header mode, enable IGMP/MLD snooping, disable VLAN
+	 * tunneling, determine priority by looking at 802.1p and IP
+	 * priority fields (IP prio has precedence), and set STP state
+	 * to Forwarding.
+	 *
+	 * If this is the CPU link, use DSA or EDSA tagging depending
+	 * on which tagging mode was configured.
+	 *
+	 * If this is a link to another switch, use DSA tagging mode.
+	 *
+	 * If this is the upstream port for this switch, enable
+	 * forwarding of unknown unicasts and multicasts.
+	 */
+	reg = 0;
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
+	    mv88e6xxx_6095_family(ds) || mv88e6xxx_6065_family(ds) ||
+	    mv88e6xxx_6185_family(ds))
+		reg = PORT_CONTROL_IGMP_MLD_SNOOP |
+		PORT_CONTROL_USE_TAG | PORT_CONTROL_USE_IP |
+		PORT_CONTROL_STATE_FORWARDING;
+	if (dsa_is_cpu_port(ds, port)) {
+		if (mv88e6xxx_6095_family(ds) || mv88e6xxx_6185_family(ds))
+			reg |= PORT_CONTROL_DSA_TAG;
+		if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+		    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds)) {
+			if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
+				reg |= PORT_CONTROL_FRAME_ETHER_TYPE_DSA;
+			else
+				reg |= PORT_CONTROL_FRAME_MODE_DSA;
+		}
+
+		if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+		    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
+		    mv88e6xxx_6095_family(ds) || mv88e6xxx_6065_family(ds) ||
+		    mv88e6xxx_6185_family(ds)) {
+			if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
+				reg |= PORT_CONTROL_EGRESS_ADD_TAG;
+		}
+	}
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
+	    mv88e6xxx_6095_family(ds) || mv88e6xxx_6065_family(ds)) {
+		if (ds->dsa_port_mask & (1 << port))
+			reg |= PORT_CONTROL_FRAME_MODE_DSA;
+		if (port == dsa_upstream_port(ds))
+			reg |= PORT_CONTROL_FORWARD_UNKNOWN |
+				PORT_CONTROL_FORWARD_UNKNOWN_MC;
+	}
+	if (reg) {
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_CONTROL, reg);
+		if (ret)
+			goto abort;
+	}
+
+	/* Port Control 2: don't force a good FCS, set the maximum
+	 * frame size to 10240 bytes, don't let the switch add or
+	 * strip 802.1q tags, don't discard tagged or untagged frames
+	 * on this port, do a destination address lookup on all
+	 * received packets as usual, disable ARP mirroring and don't
+	 * send a copy of all transmitted/received frames on this port
+	 * to the CPU.
+	 */
+	reg = 0;
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
+	    mv88e6xxx_6095_family(ds))
+		reg = PORT_CONTROL_2_MAP_DA;
+
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds))
+		reg |= PORT_CONTROL_2_JUMBO_10240;
+
+	if (mv88e6xxx_6095_family(ds) || mv88e6xxx_6185_family(ds)) {
+		/* Set the upstream port this port should use */
+		reg |= dsa_upstream_port(ds);
+		/* enable forwarding of unknown multicast addresses to
+		 * the upstream port
+		 */
+		if (port == dsa_upstream_port(ds))
+			reg |= PORT_CONTROL_2_FORWARD_UNKNOWN;
+	}
+
+	if (reg) {
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_CONTROL_2, reg);
+		if (ret)
+			goto abort;
+	}
+
+	/* Port Association Vector: when learning source addresses
+	 * of packets, add the address to the address database using
+	 * a port bitmap that has only the bit for this port set and
+	 * the other bits clear.
+	 */
+	ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_ASSOC_VECTOR,
+				   1 << port);
+	if (ret)
+		goto abort;
+
+	/* Egress rate control 2: disable egress rate control. */
+	ret = _mv88e6xxx_reg_write(ds, REG_PORT(port), PORT_RATE_CONTROL_2,
+				   0x0000);
+	if (ret)
+		goto abort;
+
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds)) {
+		/* Do not limit the period of time that this port can
+		 * be paused for by the remote end or the period of
+		 * time that this port can pause the remote end.
+		 */
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_PAUSE_CTRL, 0x0000);
+		if (ret)
+			goto abort;
+
+		/* Port ATU control: disable limiting the number of
+		 * address database entries that this port is allowed
+		 * to use.
+		 */
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_ATU_CONTROL, 0x0000);
+		/* Priority Override: disable DA, SA and VTU priority
+		 * override.
+		 */
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_PRI_OVERRIDE, 0x0000);
+		if (ret)
+			goto abort;
+
+		/* Port Ethertype: use the Ethertype DSA Ethertype
+		 * value.
+		 */
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_ETH_TYPE, ETH_P_EDSA);
+		if (ret)
+			goto abort;
+		/* Tag Remap: use an identity 802.1p prio -> switch
+		 * prio mapping.
+		 */
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_TAG_REGMAP_0123, 0x3210);
+		if (ret)
+			goto abort;
+
+		/* Tag Remap 2: use an identity 802.1p prio -> switch
+		 * prio mapping.
+		 */
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_TAG_REGMAP_4567, 0x7654);
+		if (ret)
+			goto abort;
+	}
+
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
+	    mv88e6xxx_6185_family(ds) || mv88e6xxx_6095_family(ds)) {
+		/* Rate Control: disable ingress rate limiting. */
+		ret = _mv88e6xxx_reg_write(ds, REG_PORT(port),
+					   PORT_RATE_CONTROL, 0x0001);
+		if (ret)
+			goto abort;
+	}
+
 	/* Port Control 1: disable trunking, disable sending
 	 * learning messages to this port.
 	 */
@@ -1281,13 +1605,283 @@ abort:
 	return ret;
 }
 
+int mv88e6xxx_setup_ports(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int ret;
+	int i;
+
+	for (i = 0; i < ps->num_ports; i++) {
+		ret = mv88e6xxx_setup_port(ds, i);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int mv88e6xxx_regs_show(struct seq_file *s, void *p)
+{
+	struct dsa_switch *ds = s->private;
+
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int reg, port;
+
+	seq_puts(s, "    GLOBAL GLOBAL2 ");
+	for (port = 0 ; port < ps->num_ports; port++)
+		seq_printf(s, " %2d  ", port);
+	seq_puts(s, "\n");
+
+	for (reg = 0; reg < 32; reg++) {
+		seq_printf(s, "%2x: ", reg);
+		seq_printf(s, " %4x    %4x  ",
+			   mv88e6xxx_reg_read(ds, REG_GLOBAL, reg),
+			   mv88e6xxx_reg_read(ds, REG_GLOBAL2, reg));
+
+		for (port = 0 ; port < ps->num_ports; port++)
+			seq_printf(s, "%4x ",
+				   mv88e6xxx_reg_read(ds, REG_PORT(port), reg));
+		seq_puts(s, "\n");
+	}
+
+	return 0;
+}
+
+static int mv88e6xxx_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mv88e6xxx_regs_show, inode->i_private);
+}
+
+static const struct file_operations mv88e6xxx_regs_fops = {
+	.open   = mv88e6xxx_regs_open,
+	.read   = seq_read,
+	.llseek = no_llseek,
+	.release = single_release,
+	.owner  = THIS_MODULE,
+};
+
+static void mv88e6xxx_atu_show_header(struct seq_file *s)
+{
+	seq_puts(s, "DB   T/P  Vec State Addr\n");
+}
+
+static void mv88e6xxx_atu_show_entry(struct seq_file *s, int dbnum,
+				     unsigned char *addr, int data)
+{
+	bool trunk = !!(data & GLOBAL_ATU_DATA_TRUNK);
+	int portvec = ((data & GLOBAL_ATU_DATA_PORT_VECTOR_MASK) >>
+		       GLOBAL_ATU_DATA_PORT_VECTOR_SHIFT);
+	int state = data & GLOBAL_ATU_DATA_STATE_MASK;
+
+	seq_printf(s, "%03x %5s %10pb   %x   %pM\n",
+		   dbnum, (trunk ? "Trunk" : "Port"), &portvec, state, addr);
+}
+
+static int mv88e6xxx_atu_show_db(struct seq_file *s, struct dsa_switch *ds,
+				 int dbnum)
+{
+	unsigned char bcast[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	unsigned char addr[6];
+	int ret, data, state;
+
+	ret = __mv88e6xxx_write_addr(ds, bcast);
+	if (ret < 0)
+		return ret;
+
+	do {
+		ret = _mv88e6xxx_atu_cmd(ds, dbnum, GLOBAL_ATU_OP_GET_NEXT_DB);
+		if (ret < 0)
+			return ret;
+		data = _mv88e6xxx_reg_read(ds, REG_GLOBAL, GLOBAL_ATU_DATA);
+		if (data < 0)
+			return data;
+
+		state = data & GLOBAL_ATU_DATA_STATE_MASK;
+		if (state == GLOBAL_ATU_DATA_STATE_UNUSED)
+			break;
+		ret = __mv88e6xxx_read_addr(ds, addr);
+		if (ret < 0)
+			return ret;
+		mv88e6xxx_atu_show_entry(s, dbnum, addr, data);
+	} while (state != GLOBAL_ATU_DATA_STATE_UNUSED);
+
+	return 0;
+}
+
+static int mv88e6xxx_atu_show(struct seq_file *s, void *p)
+{
+	struct dsa_switch *ds = s->private;
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int dbnum;
+
+	mv88e6xxx_atu_show_header(s);
+
+	for (dbnum = 0; dbnum < 255; dbnum++) {
+		mutex_lock(&ps->smi_mutex);
+		mv88e6xxx_atu_show_db(s, ds, dbnum);
+		mutex_unlock(&ps->smi_mutex);
+	}
+
+	return 0;
+}
+
+static int mv88e6xxx_atu_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mv88e6xxx_atu_show, inode->i_private);
+}
+
+static const struct file_operations mv88e6xxx_atu_fops = {
+	.open   = mv88e6xxx_atu_open,
+	.read   = seq_read,
+	.llseek = no_llseek,
+	.release = single_release,
+	.owner  = THIS_MODULE,
+};
+
+static void mv88e6xxx_stats_show_header(struct seq_file *s,
+					struct mv88e6xxx_priv_state *ps)
+{
+	int port;
+
+	seq_puts(s, "      Statistic       ");
+	for (port = 0 ; port < ps->num_ports; port++)
+		seq_printf(s, "Port %2d  ", port);
+	seq_puts(s, "\n");
+}
+
+static int mv88e6xxx_stats_show(struct seq_file *s, void *p)
+{
+	struct dsa_switch *ds = s->private;
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	struct mv88e6xxx_hw_stat *stats = mv88e6xxx_hw_stats;
+	int port, stat, max_stats;
+	uint64_t value;
+
+	if (have_sw_in_discards(ds))
+		max_stats = ARRAY_SIZE(mv88e6xxx_hw_stats);
+	else
+		max_stats = ARRAY_SIZE(mv88e6xxx_hw_stats) - 3;
+
+	mv88e6xxx_stats_show_header(s, ps);
+
+	mutex_lock(&ps->smi_mutex);
+
+	for (stat = 0; stat < max_stats; stat++) {
+		seq_printf(s, "%19s: ", stats[stat].string);
+		for (port = 0 ; port < ps->num_ports; port++) {
+			_mv88e6xxx_stats_snapshot(ds, port);
+			value = _mv88e6xxx_get_ethtool_stat(ds, stat, stats,
+							    port);
+			seq_printf(s, "%8llu ", value);
+		}
+		seq_puts(s, "\n");
+	}
+	mutex_unlock(&ps->smi_mutex);
+
+	return 0;
+}
+
+static int mv88e6xxx_stats_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mv88e6xxx_stats_show, inode->i_private);
+}
+
+static const struct file_operations mv88e6xxx_stats_fops = {
+	.open   = mv88e6xxx_stats_open,
+	.read   = seq_read,
+	.llseek = no_llseek,
+	.release = single_release,
+	.owner  = THIS_MODULE,
+};
+
+static int mv88e6xxx_device_map_show(struct seq_file *s, void *p)
+{
+	struct dsa_switch *ds = s->private;
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int target, ret;
+
+	seq_puts(s, "Target Port\n");
+
+	mutex_lock(&ps->smi_mutex);
+	for (target = 0; target < 32; target++) {
+		ret = _mv88e6xxx_reg_write(
+			ds, REG_GLOBAL2, GLOBAL2_DEVICE_MAPPING,
+			target << GLOBAL2_DEVICE_MAPPING_TARGET_SHIFT);
+		if (ret < 0)
+			goto out;
+		ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL2,
+					  GLOBAL2_DEVICE_MAPPING);
+		seq_printf(s, "  %2d   %2d\n", target,
+			   ret & GLOBAL2_DEVICE_MAPPING_PORT_MASK);
+	}
+out:
+	mutex_unlock(&ps->smi_mutex);
+
+	return 0;
+}
+
+static int mv88e6xxx_device_map_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mv88e6xxx_device_map_show, inode->i_private);
+}
+
+static const struct file_operations mv88e6xxx_device_map_fops = {
+	.open   = mv88e6xxx_device_map_open,
+	.read   = seq_read,
+	.llseek = no_llseek,
+	.release = single_release,
+	.owner  = THIS_MODULE,
+};
+
+static int mv88e6xxx_scratch_show(struct seq_file *s, void *p)
+{
+	struct dsa_switch *ds = s->private;
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int reg, ret;
+
+	seq_puts(s, "Register Value\n");
+
+	mutex_lock(&ps->smi_mutex);
+	for (reg = 0; reg < 0x80; reg++) {
+		ret = _mv88e6xxx_reg_write(
+			ds, REG_GLOBAL2, GLOBAL2_SCRATCH_MISC,
+			reg << GLOBAL2_SCRATCH_REGISTER_SHIFT);
+		if (ret < 0)
+			goto out;
+
+		ret = _mv88e6xxx_scratch_wait(ds);
+		if (ret < 0)
+			goto out;
+
+		ret = _mv88e6xxx_reg_read(ds, REG_GLOBAL2,
+					  GLOBAL2_SCRATCH_MISC);
+		seq_printf(s, "  %2x   %2x\n", reg,
+			   ret & GLOBAL2_SCRATCH_VALUE_MASK);
+	}
+out:
+	mutex_unlock(&ps->smi_mutex);
+
+	return 0;
+}
+
+static int mv88e6xxx_scratch_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mv88e6xxx_scratch_show, inode->i_private);
+}
+
+static const struct file_operations mv88e6xxx_scratch_fops = {
+	.open   = mv88e6xxx_scratch_open,
+	.read   = seq_read,
+	.llseek = no_llseek,
+	.release = single_release,
+	.owner  = THIS_MODULE,
+};
+
 int mv88e6xxx_setup_common(struct dsa_switch *ds)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	char *name;
 
 	mutex_init(&ps->smi_mutex);
-	mutex_init(&ps->stats_mutex);
-	mutex_init(&ps->phy_mutex);
 
 	ps->id = REG_READ(REG_PORT(0), PORT_SWITCH_ID) & 0xfff0;
 
@@ -1295,6 +1889,128 @@ int mv88e6xxx_setup_common(struct dsa_switch *ds)
 
 	INIT_WORK(&ps->bridge_work, mv88e6xxx_bridge_work);
 
+	name = kasprintf(GFP_KERNEL, "dsa%d", ds->index);
+	ps->dbgfs = debugfs_create_dir(name, NULL);
+	kfree(name);
+
+	debugfs_create_file("regs", S_IRUGO, ps->dbgfs, ds,
+			    &mv88e6xxx_regs_fops);
+
+	debugfs_create_file("atu", S_IRUGO, ps->dbgfs, ds,
+			    &mv88e6xxx_atu_fops);
+
+	debugfs_create_file("stats", S_IRUGO, ps->dbgfs, ds,
+			    &mv88e6xxx_stats_fops);
+
+	debugfs_create_file("device_map", S_IRUGO, ps->dbgfs, ds,
+			    &mv88e6xxx_device_map_fops);
+
+	debugfs_create_file("scratch", S_IRUGO, ps->dbgfs, ds,
+			    &mv88e6xxx_scratch_fops);
+	return 0;
+}
+
+int mv88e6xxx_setup_global(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int i;
+
+	/* Set the default address aging time to 5 minutes, and
+	 * enable address learn messages to be sent to all message
+	 * ports.
+	 */
+	REG_WRITE(REG_GLOBAL, GLOBAL_ATU_CONTROL,
+		  0x0140 | GLOBAL_ATU_CONTROL_LEARN2ALL);
+
+	/* Configure the IP ToS mapping registers. */
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_0, 0x0000);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_1, 0x0000);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_2, 0x5555);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_3, 0x5555);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_4, 0xaaaa);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_5, 0xaaaa);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_6, 0xffff);
+	REG_WRITE(REG_GLOBAL, GLOBAL_IP_PRI_7, 0xffff);
+
+	/* Configure the IEEE 802.1p priority mapping register. */
+	REG_WRITE(REG_GLOBAL, GLOBAL_IEEE_PRI, 0xfa41);
+
+	/* Send all frames with destination addresses matching
+	 * 01:80:c2:00:00:0x to the CPU port.
+	 */
+	REG_WRITE(REG_GLOBAL2, GLOBAL2_MGMT_EN_0X, 0xffff);
+
+	/* Ignore removed tag data on doubly tagged packets, disable
+	 * flow control messages, force flow control priority to the
+	 * highest, and send all special multicast frames to the CPU
+	 * port at the highest priority.
+	 */
+	REG_WRITE(REG_GLOBAL2, GLOBAL2_SWITCH_MGMT,
+		  0x7 | GLOBAL2_SWITCH_MGMT_RSVD2CPU | 0x70 |
+		  GLOBAL2_SWITCH_MGMT_FORCE_FLOW_CTRL_PRI);
+
+	/* Program the DSA routing table. */
+	for (i = 0; i < 32; i++) {
+		int nexthop = 0x1f;
+
+		if (ds->pd->rtable &&
+		    i != ds->index && i < ds->dst->pd->nr_chips)
+			nexthop = ds->pd->rtable[i] & 0x1f;
+
+		REG_WRITE(REG_GLOBAL2, GLOBAL2_DEVICE_MAPPING,
+			  GLOBAL2_DEVICE_MAPPING_UPDATE |
+			  (i << GLOBAL2_DEVICE_MAPPING_TARGET_SHIFT) |
+			  nexthop);
+	}
+
+	/* Clear all trunk masks. */
+	for (i = 0; i < 8; i++)
+		REG_WRITE(REG_GLOBAL2, GLOBAL2_TRUNK_MASK,
+			  0x8000 | (i << GLOBAL2_TRUNK_MASK_NUM_SHIFT) |
+			  ((1 << ps->num_ports) - 1));
+
+	/* Clear all trunk mappings. */
+	for (i = 0; i < 16; i++)
+		REG_WRITE(REG_GLOBAL2, GLOBAL2_TRUNK_MAPPING,
+			  GLOBAL2_TRUNK_MAPPING_UPDATE |
+			  (i << GLOBAL2_TRUNK_MAPPING_ID_SHIFT));
+
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds)) {
+		/* Send all frames with destination addresses matching
+		 * 01:80:c2:00:00:2x to the CPU port.
+		 */
+		REG_WRITE(REG_GLOBAL2, GLOBAL2_MGMT_EN_2X, 0xffff);
+
+		/* Initialise cross-chip port VLAN table to reset
+		 * defaults.
+		 */
+		REG_WRITE(REG_GLOBAL2, GLOBAL2_PVT_ADDR, 0x9000);
+
+		/* Clear the priority override table. */
+		for (i = 0; i < 16; i++)
+			REG_WRITE(REG_GLOBAL2, GLOBAL2_PRIO_OVERRIDE,
+				  0x8000 | (i << 8));
+	}
+
+	if (mv88e6xxx_6352_family(ds) || mv88e6xxx_6351_family(ds) ||
+	    mv88e6xxx_6165_family(ds) || mv88e6xxx_6097_family(ds) ||
+	    mv88e6xxx_6185_family(ds) || mv88e6xxx_6095_family(ds)) {
+		/* Disable ingress rate limiting by resetting all
+		 * ingress rate limit registers to their initial
+		 * state.
+		 */
+		for (i = 0; i < ps->num_ports; i++)
+			REG_WRITE(REG_GLOBAL2, GLOBAL2_INGRESS_OP,
+				  0x9000 | (i << 8));
+	}
+
+	/* Clear the statistics counters for all ports */
+	REG_WRITE(REG_GLOBAL, GLOBAL_STATS_OP, GLOBAL_STATS_OP_FLUSH_ALL);
+
+	/* Wait for the flush to complete. */
+	_mv88e6xxx_stats_wait(ds);
+
 	return 0;
 }
 
@@ -1343,14 +2059,14 @@ int mv88e6xxx_phy_page_read(struct dsa_switch *ds, int port, int page, int reg)
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int ret;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 	ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
 	if (ret < 0)
 		goto error;
 	ret = _mv88e6xxx_phy_read_indirect(ds, port, reg);
 error:
 	_mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 	return ret;
 }
 
@@ -1360,7 +2076,7 @@ int mv88e6xxx_phy_page_write(struct dsa_switch *ds, int port, int page,
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	int ret;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 	ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
 	if (ret < 0)
 		goto error;
@@ -1368,7 +2084,7 @@ int mv88e6xxx_phy_page_write(struct dsa_switch *ds, int port, int page,
 	ret = _mv88e6xxx_phy_write_indirect(ds, port, reg, val);
 error:
 	_mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 	return ret;
 }
 
@@ -1391,9 +2107,9 @@ mv88e6xxx_phy_read(struct dsa_switch *ds, int port, int regnum)
 	if (addr < 0)
 		return addr;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 	ret = _mv88e6xxx_phy_read(ds, addr, regnum);
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 	return ret;
 }
 
@@ -1407,9 +2123,9 @@ mv88e6xxx_phy_write(struct dsa_switch *ds, int port, int regnum, u16 val)
 	if (addr < 0)
 		return addr;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 	ret = _mv88e6xxx_phy_write(ds, addr, regnum, val);
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 	return ret;
 }
 
@@ -1423,9 +2139,9 @@ mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int port, int regnum)
 	if (addr < 0)
 		return addr;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 	ret = _mv88e6xxx_phy_read_indirect(ds, addr, regnum);
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 	return ret;
 }
 
@@ -1440,9 +2156,9 @@ mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int port, int regnum,
 	if (addr < 0)
 		return addr;
 
-	mutex_lock(&ps->phy_mutex);
+	mutex_lock(&ps->smi_mutex);
 	ret = _mv88e6xxx_phy_write_indirect(ds, addr, regnum, val);
-	mutex_unlock(&ps->phy_mutex);
+	mutex_unlock(&ps->smi_mutex);
 	return ret;
 }
 
diff --git a/drivers/net/dsa/mv88e6xxx.h b/drivers/net/dsa/mv88e6xxx.h
index e045154f3364..a650b2656de9 100644
--- a/drivers/net/dsa/mv88e6xxx.h
+++ b/drivers/net/dsa/mv88e6xxx.h
@@ -11,6 +11,10 @@
 #ifndef __MV88E6XXX_H
 #define __MV88E6XXX_H
 
+#ifndef UINT64_MAX
+#define UINT64_MAX		(u64)(~((u64)0))
+#endif
+
 #define SMI_CMD			0x00
 #define SMI_CMD_BUSY		BIT(15)
 #define SMI_CMD_CLAUSE_22	BIT(12)
@@ -40,9 +44,31 @@
 #define PORT_STATUS_TX_PAUSED	BIT(5)
 #define PORT_STATUS_FLOW_CTRL	BIT(4)
 #define PORT_PCS_CTRL		0x01
+#define PORT_PCS_CTRL_FC		BIT(7)
+#define PORT_PCS_CTRL_FORCE_FC		BIT(6)
+#define PORT_PCS_CTRL_LINK_UP		BIT(5)
+#define PORT_PCS_CTRL_FORCE_LINK	BIT(4)
+#define PORT_PCS_CTRL_DUPLEX_FULL	BIT(3)
+#define PORT_PCS_CTRL_FORCE_DUPLEX	BIT(2)
+#define PORT_PCS_CTRL_10		0x00
+#define PORT_PCS_CTRL_100		0x01
+#define PORT_PCS_CTRL_1000		0x02
+#define PORT_PCS_CTRL_UNFORCED		0x03
+#define PORT_PAUSE_CTRL		0x02
 #define PORT_SWITCH_ID		0x03
+#define PORT_SWITCH_ID_6031	0x0310
+#define PORT_SWITCH_ID_6035	0x0350
+#define PORT_SWITCH_ID_6046	0x0480
+#define PORT_SWITCH_ID_6061	0x0610
+#define PORT_SWITCH_ID_6065	0x0650
 #define PORT_SWITCH_ID_6085	0x04a0
+#define PORT_SWITCH_ID_6092	0x0970
 #define PORT_SWITCH_ID_6095	0x0950
+#define PORT_SWITCH_ID_6096	0x0980
+#define PORT_SWITCH_ID_6097	0x0990
+#define PORT_SWITCH_ID_6108	0x1070
+#define PORT_SWITCH_ID_6121	0x1040
+#define PORT_SWITCH_ID_6122	0x1050
 #define PORT_SWITCH_ID_6123	0x1210
 #define PORT_SWITCH_ID_6123_A1	0x1212
 #define PORT_SWITCH_ID_6123_A2	0x1213
@@ -58,13 +84,38 @@
 #define PORT_SWITCH_ID_6165_A2	0x1653
 #define PORT_SWITCH_ID_6171	0x1710
 #define PORT_SWITCH_ID_6172	0x1720
+#define PORT_SWITCH_ID_6175	0x1750
 #define PORT_SWITCH_ID_6176	0x1760
 #define PORT_SWITCH_ID_6182	0x1a60
 #define PORT_SWITCH_ID_6185	0x1a70
+#define PORT_SWITCH_ID_6240	0x2400
+#define PORT_SWITCH_ID_6320	0x1250
+#define PORT_SWITCH_ID_6350	0x3710
+#define PORT_SWITCH_ID_6351	0x3750
 #define PORT_SWITCH_ID_6352	0x3520
 #define PORT_SWITCH_ID_6352_A0	0x3521
 #define PORT_SWITCH_ID_6352_A1	0x3522
 #define PORT_CONTROL		0x04
+#define PORT_CONTROL_USE_CORE_TAG	BIT(15)
+#define PORT_CONTROL_DROP_ON_LOCK	BIT(14)
+#define PORT_CONTROL_EGRESS_UNMODIFIED	(0x0 << 12)
+#define PORT_CONTROL_EGRESS_UNTAGGED	(0x1 << 12)
+#define PORT_CONTROL_EGRESS_TAGGED	(0x2 << 12)
+#define PORT_CONTROL_EGRESS_ADD_TAG	(0x3 << 12)
+#define PORT_CONTROL_HEADER		BIT(11)
+#define PORT_CONTROL_IGMP_MLD_SNOOP	BIT(10)
+#define PORT_CONTROL_DOUBLE_TAG		BIT(9)
+#define PORT_CONTROL_FRAME_MODE_NORMAL		(0x0 << 8)
+#define PORT_CONTROL_FRAME_MODE_DSA		(0x1 << 8)
+#define PORT_CONTROL_FRAME_MODE_PROVIDER	(0x2 << 8)
+#define PORT_CONTROL_FRAME_ETHER_TYPE_DSA	(0x3 << 8)
+#define PORT_CONTROL_DSA_TAG		BIT(8)
+#define PORT_CONTROL_VLAN_TUNNEL	BIT(7)
+#define PORT_CONTROL_TAG_IF_BOTH	BIT(6)
+#define PORT_CONTROL_USE_IP		BIT(5)
+#define PORT_CONTROL_USE_TAG		BIT(4)
+#define PORT_CONTROL_FORWARD_UNKNOWN_MC	BIT(3)
+#define PORT_CONTROL_FORWARD_UNKNOWN	BIT(2)
 #define PORT_CONTROL_STATE_MASK		0x03
 #define PORT_CONTROL_STATE_DISABLED	0x00
 #define PORT_CONTROL_STATE_BLOCKING	0x01
@@ -74,15 +125,32 @@
 #define PORT_BASE_VLAN		0x06
 #define PORT_DEFAULT_VLAN	0x07
 #define PORT_CONTROL_2		0x08
+#define PORT_CONTROL_2_IGNORE_FCS	BIT(15)
+#define PORT_CONTROL_2_VTU_PRI_OVERRIDE	BIT(14)
+#define PORT_CONTROL_2_SA_PRIO_OVERRIDE	BIT(13)
+#define PORT_CONTROL_2_DA_PRIO_OVERRIDE	BIT(12)
+#define PORT_CONTROL_2_JUMBO_1522	(0x00 << 12)
+#define PORT_CONTROL_2_JUMBO_2048	(0x01 << 12)
+#define PORT_CONTROL_2_JUMBO_10240	(0x02 << 12)
+#define PORT_CONTROL_2_DISCARD_TAGGED	BIT(9)
+#define PORT_CONTROL_2_DISCARD_UNTAGGED	BIT(8)
+#define PORT_CONTROL_2_MAP_DA		BIT(7)
+#define PORT_CONTROL_2_DEFAULT_FORWARD	BIT(6)
+#define PORT_CONTROL_2_FORWARD_UNKNOWN	BIT(6)
+#define PORT_CONTROL_2_EGRESS_MONITOR	BIT(5)
+#define PORT_CONTROL_2_INGRESS_MONITOR	BIT(4)
 #define PORT_RATE_CONTROL	0x09
 #define PORT_RATE_CONTROL_2	0x0a
 #define PORT_ASSOC_VECTOR	0x0b
+#define PORT_ATU_CONTROL	0x0c
+#define PORT_PRI_OVERRIDE	0x0d
+#define PORT_ETH_TYPE		0x0f
 #define PORT_IN_DISCARD_LO	0x10
 #define PORT_IN_DISCARD_HI	0x11
 #define PORT_IN_FILTERED	0x12
 #define PORT_OUT_FILTERED	0x13
-#define PORT_TAG_REGMAP_0123	0x19
-#define PORT_TAG_REGMAP_4567	0x1a
+#define PORT_TAG_REGMAP_0123	0x18
+#define PORT_TAG_REGMAP_4567	0x19
 
 #define REG_GLOBAL		0x1b
 #define GLOBAL_STATUS		0x00
@@ -102,7 +170,7 @@
 #define GLOBAL_CONTROL_DISCARD_EXCESS	BIT(13) /* 6352 */
 #define GLOBAL_CONTROL_SCHED_PRIO	BIT(11) /* 6152 */
 #define GLOBAL_CONTROL_MAX_FRAME_1632	BIT(10) /* 6152 */
-#define GLOBAL_CONTROL_RELOAD_EEPROM	BIT(9)  /* 6152 */
+#define GLOBAL_CONTROL_RELOAD_EEPROM	BIT(9)	/* 6152 */
 #define GLOBAL_CONTROL_DEVICE_EN	BIT(7)
 #define GLOBAL_CONTROL_STATS_DONE_EN	BIT(6)
 #define GLOBAL_CONTROL_VTU_PROBLEM_EN	BIT(5)
@@ -117,6 +185,7 @@
 #define GLOBAL_VTU_DATA_4_7	0x08
 #define GLOBAL_VTU_DATA_8_11	0x09
 #define GLOBAL_ATU_CONTROL	0x0a
+#define GLOBAL_ATU_CONTROL_LEARN2ALL	BIT(3)
 #define GLOBAL_ATU_OP		0x0b
 #define GLOBAL_ATU_OP_BUSY	BIT(15)
 #define GLOBAL_ATU_OP_NOP		(0 << 12)
@@ -128,6 +197,9 @@
 #define GLOBAL_ATU_OP_FLUSH_NON_STATIC_DB ((6 << 12) | GLOBAL_ATU_OP_BUSY)
 #define GLOBAL_ATU_OP_GET_CLR_VIOLATION	  ((7 << 12) | GLOBAL_ATU_OP_BUSY)
 #define GLOBAL_ATU_DATA		0x0c
+#define GLOBAL_ATU_DATA_TRUNK			BIT(15)
+#define GLOBAL_ATU_DATA_PORT_VECTOR_MASK	0x3ff0
+#define GLOBAL_ATU_DATA_PORT_VECTOR_SHIFT	4
 #define GLOBAL_ATU_DATA_STATE_MASK		0x0f
 #define GLOBAL_ATU_DATA_STATE_UNUSED		0x00
 #define GLOBAL_ATU_DATA_STATE_UC_MGMT		0x0d
@@ -151,7 +223,15 @@
 #define GLOBAL_IEEE_PRI		0x18
 #define GLOBAL_CORE_TAG_TYPE	0x19
 #define GLOBAL_MONITOR_CONTROL	0x1a
+#define GLOBAL_MONITOR_CONTROL_INGRESS_SHIFT	12
+#define GLOBAL_MONITOR_CONTROL_EGRESS_SHIFT	8
+#define GLOBAL_MONITOR_CONTROL_ARP_SHIFT	4
+#define GLOBAL_MONITOR_CONTROL_MIRROR_SHIFT	0
+#define GLOBAL_MONITOR_CONTROL_ARP_DISABLED	(0xf0)
 #define GLOBAL_CONTROL_2	0x1c
+#define GLOBAL_CONTROL_2_NO_CASCADE		0xe000
+#define GLOBAL_CONTROL_2_MULTIPLE_CASCADE	0xf000
+
 #define GLOBAL_STATS_OP		0x1d
 #define GLOBAL_STATS_OP_BUSY	BIT(15)
 #define GLOBAL_STATS_OP_NOP		(0 << 12)
@@ -172,9 +252,21 @@
 #define GLOBAL2_MGMT_EN_0X	0x03
 #define GLOBAL2_FLOW_CONTROL	0x04
 #define GLOBAL2_SWITCH_MGMT	0x05
+#define GLOBAL2_SWITCH_MGMT_USE_DOUBLE_TAG_DATA	BIT(15)
+#define GLOBAL2_SWITCH_MGMT_PREVENT_LOOPS	BIT(14)
+#define GLOBAL2_SWITCH_MGMT_FLOW_CONTROL_MSG	BIT(13)
+#define GLOBAL2_SWITCH_MGMT_FORCE_FLOW_CTRL_PRI	BIT(7)
+#define GLOBAL2_SWITCH_MGMT_RSVD2CPU		BIT(3)
 #define GLOBAL2_DEVICE_MAPPING	0x06
+#define GLOBAL2_DEVICE_MAPPING_UPDATE		BIT(15)
+#define GLOBAL2_DEVICE_MAPPING_TARGET_SHIFT	8
+#define GLOBAL2_DEVICE_MAPPING_PORT_MASK	0x0f
 #define GLOBAL2_TRUNK_MASK	0x07
+#define GLOBAL2_TRUNK_MASK_UPDATE		BIT(15)
+#define GLOBAL2_TRUNK_MASK_NUM_SHIFT		12
 #define GLOBAL2_TRUNK_MAPPING	0x08
+#define GLOBAL2_TRUNK_MAPPING_UPDATE		BIT(15)
+#define GLOBAL2_TRUNK_MAPPING_ID_SHIFT		11
 #define GLOBAL2_INGRESS_OP	0x09
 #define GLOBAL2_INGRESS_DATA	0x0a
 #define GLOBAL2_PVT_ADDR	0x0b
@@ -183,6 +275,10 @@
 #define GLOBAL2_SWITCH_MAC_BUSY BIT(15)
 #define GLOBAL2_ATU_STATS	0x0e
 #define GLOBAL2_PRIO_OVERRIDE	0x0f
+#define GLOBAL2_PRIO_OVERRIDE_FORCE_SNOOP	BIT(7)
+#define GLOBAL2_PRIO_OVERRIDE_SNOOP_SHIFT	4
+#define GLOBAL2_PRIO_OVERRIDE_FORCE_ARP		BIT(3)
+#define GLOBAL2_PRIO_OVERRIDE_ARP_SHIFT		0
 #define GLOBAL2_EEPROM_OP	0x14
 #define GLOBAL2_EEPROM_OP_BUSY	BIT(15)
 #define GLOBAL2_EEPROM_OP_LOAD	BIT(11)
@@ -201,6 +297,9 @@
 #define GLOBAL2_SMI_OP_45_READ_DATA	((2 << 10) | GLOBAL2_SMI_OP_BUSY)
 #define GLOBAL2_SMI_DATA	0x19
 #define GLOBAL2_SCRATCH_MISC	0x1a
+#define GLOBAL2_SCRATCH_BUSY		BIT(15)
+#define GLOBAL2_SCRATCH_REGISTER_SHIFT	8
+#define GLOBAL2_SCRATCH_VALUE_MASK	0xff
 #define GLOBAL2_WDOG_CONTROL	0x1b
 #define GLOBAL2_QOS_WEIGHT	0x1c
 #define GLOBAL2_MISC		0x1d
@@ -251,6 +350,8 @@ struct mv88e6xxx_priv_state {
 	u8 port_state[DSA_MAX_PORTS];
 
 	struct work_struct bridge_work;
+
+	struct dentry *dbgfs;
 };
 
 struct mv88e6xxx_hw_stat {
@@ -260,14 +361,14 @@ struct mv88e6xxx_hw_stat {
 };
 
 int mv88e6xxx_switch_reset(struct dsa_switch *ds, bool ppu_active);
-int mv88e6xxx_setup_port_common(struct dsa_switch *ds, int port);
+int mv88e6xxx_setup_ports(struct dsa_switch *ds);
 int mv88e6xxx_setup_common(struct dsa_switch *ds);
+int mv88e6xxx_setup_global(struct dsa_switch *ds);
 int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg);
 int mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg);
 int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 			  int reg, u16 val);
 int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val);
-int mv88e6xxx_config_prio(struct dsa_switch *ds);
 int mv88e6xxx_set_addr_direct(struct dsa_switch *ds, u8 *addr);
 int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr);
 int mv88e6xxx_phy_read(struct dsa_switch *ds, int port, int regnum);
@@ -289,7 +390,6 @@ int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port);
 void mv88e6xxx_get_regs(struct dsa_switch *ds, int port,
 			struct ethtool_regs *regs, void *_p);
 int  mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp);
-int mv88e6xxx_phy_wait(struct dsa_switch *ds);
 int mv88e6xxx_eeprom_load_wait(struct dsa_switch *ds);
 int mv88e6xxx_eeprom_busy_wait(struct dsa_switch *ds);
 int mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr, int regnum);
diff --git a/drivers/net/ethernet/3com/Kconfig b/drivers/net/ethernet/3com/Kconfig
index afaab4b2333f..5b7658bcf020 100644
--- a/drivers/net/ethernet/3com/Kconfig
+++ b/drivers/net/ethernet/3com/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_3COM
 	default y
 	depends on ISA || EISA || PCI || PCMCIA
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -23,8 +21,7 @@ config EL3
 	depends on (ISA || EISA)
 	---help---
 	  If you have a network (Ethernet) card belonging to the 3Com
-	  EtherLinkIII series, say Y and read the Ethernet-HOWTO, available
-	  from <http://www.tldp.org/docs.html#howto>.
+	  EtherLinkIII series, say Y here.
 
 	  If your card is not working you may need to use the DOS
 	  setup disk to disable Plug & Play mode, and to select the default
@@ -38,8 +35,7 @@ config 3C515
 	depends on ISA && ISA_DMA_API
 	---help---
 	  If you have a 3Com ISA EtherLink XL "Corkscrew" 3c515 Fast Ethernet
-	  network card, say Y and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  network card, say Y here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called 3c515.
@@ -78,9 +74,7 @@ config VORTEX
 	  "Tornado"   (3c905)                                  PCI
 	  "Hurricane" (3c555/3cSOHO)                           PCI
 
-	  If you have such a card, say Y and read the Ethernet-HOWTO,
-	  available from <http://www.tldp.org/docs.html#howto>. More
-	  specific information is in
+	  If you have such a card, say Y here.  More specific information is in
 	  <file:Documentation/networking/vortex.txt> and in the comments at
 	  the beginning of <file:drivers/net/ethernet/3com/3c59x.c>.
 
@@ -97,9 +91,7 @@ config TYPHOON
 	  3CR990SVR, 3CR990SVR95, 3CR990SVR97, 3CR990-FX-95 Server,
 	  3CR990-FX-97 Server, 3C990B-TX-M, 3C990BSVR
 
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card of this type, say Y here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called typhoon.
diff --git a/drivers/net/ethernet/8390/Kconfig b/drivers/net/ethernet/8390/Kconfig
index 2d89bd00de61..edf72258ab1d 100644
--- a/drivers/net/ethernet/8390/Kconfig
+++ b/drivers/net/ethernet/8390/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_8390
 	default y
 	depends on NET_VENDOR_NATSEMI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -71,9 +69,7 @@ config MAC8390
 	select CRC32
 	---help---
 	  If you want to include a driver to support Nubus or LC-PDS
-	  Ethernet cards using an NS8390 chipset or its equivalent, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  Ethernet cards using an NS8390 chipset or its equivalent, say Y.
 
 config MCF8390
 	tristate "ColdFire NS8390 based Ethernet support"
@@ -95,10 +91,9 @@ config NE2000
 		    ATARI_ETHERNEC)
 	select CRC32
 	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.  Many Ethernet cards
-	  without a specific driver are compatible with NE2000.
+	  If you have a network (Ethernet) card of this type, say Y here.
+	  Many Ethernet cards without a specific driver are compatible with
+	  the NE2000.
 
 	  If you have a PCI NE2000 card however, say N here and Y to "PCI
 	  NE2000 and clone support" below.
@@ -114,8 +109,7 @@ config NE2K_PCI
 	  This driver is for NE2000 compatible PCI cards. It will not work
 	  with ISA NE2000 cards (they have their own driver, "NE2000/NE1000
 	  support" below). If you have a PCI NE2000 network (Ethernet) card,
-	  say Y and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  say Y here.
 
 	  This driver also works for the following NE2000 clone cards:
 	  RealTek RTL-8029  Winbond 89C940  Compex RL2000  KTI ET32P2
@@ -164,9 +158,7 @@ config ULTRA
 	depends on ISA
 	select CRC32
 	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card of this type, say Y here.
 
 	  Important: There have been many reports that, with some motherboards
 	  mixing an SMC Ultra and an Adaptec AHA154x SCSI card (or compatible,
@@ -183,9 +175,7 @@ config WD80x3
 	depends on ISA
 	select CRC32
 	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card of this type, say Y here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called wd.
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index eadcb053807e..f3bb1784066b 100644
--- a/drivers/net/ethernet/Kconfig
+++ b/drivers/net/ethernet/Kconfig
@@ -34,6 +34,7 @@ source "drivers/net/ethernet/adi/Kconfig"
 source "drivers/net/ethernet/broadcom/Kconfig"
 source "drivers/net/ethernet/brocade/Kconfig"
 source "drivers/net/ethernet/calxeda/Kconfig"
+source "drivers/net/ethernet/cavium/Kconfig"
 source "drivers/net/ethernet/chelsio/Kconfig"
 source "drivers/net/ethernet/cirrus/Kconfig"
 source "drivers/net/ethernet/cisco/Kconfig"
@@ -66,6 +67,7 @@ config DNET
 source "drivers/net/ethernet/dec/Kconfig"
 source "drivers/net/ethernet/dlink/Kconfig"
 source "drivers/net/ethernet/emulex/Kconfig"
+source "drivers/net/ethernet/ezchip/Kconfig"
 source "drivers/net/ethernet/neterion/Kconfig"
 source "drivers/net/ethernet/faraday/Kconfig"
 source "drivers/net/ethernet/freescale/Kconfig"
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index 1367afcd0a8b..c51014b0464f 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_NET_BFIN) += adi/
 obj-$(CONFIG_NET_VENDOR_BROADCOM) += broadcom/
 obj-$(CONFIG_NET_VENDOR_BROCADE) += brocade/
 obj-$(CONFIG_NET_CALXEDA_XGMAC) += calxeda/
+obj-$(CONFIG_NET_VENDOR_CAVIUM) += cavium/
 obj-$(CONFIG_NET_VENDOR_CHELSIO) += chelsio/
 obj-$(CONFIG_NET_VENDOR_CIRRUS) += cirrus/
 obj-$(CONFIG_NET_VENDOR_CISCO) += cisco/
@@ -29,6 +30,7 @@ obj-$(CONFIG_DNET) += dnet.o
 obj-$(CONFIG_NET_VENDOR_DEC) += dec/
 obj-$(CONFIG_NET_VENDOR_DLINK) += dlink/
 obj-$(CONFIG_NET_VENDOR_EMULEX) += emulex/
+obj-$(CONFIG_NET_VENDOR_EZCHIP) += ezchip/
 obj-$(CONFIG_NET_VENDOR_EXAR) += neterion/
 obj-$(CONFIG_NET_VENDOR_FARADAY) += faraday/
 obj-$(CONFIG_NET_VENDOR_FREESCALE) += freescale/
diff --git a/drivers/net/ethernet/adaptec/Kconfig b/drivers/net/ethernet/adaptec/Kconfig
index 5c804bbe3dab..822cffb4174c 100644
--- a/drivers/net/ethernet/adaptec/Kconfig
+++ b/drivers/net/ethernet/adaptec/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_ADAPTEC
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/adi/Kconfig b/drivers/net/ethernet/adi/Kconfig
index c9cd3592ab73..6b94ba610399 100644
--- a/drivers/net/ethernet/adi/Kconfig
+++ b/drivers/net/ethernet/adi/Kconfig
@@ -7,8 +7,6 @@ config NET_BFIN
 	depends on BF516 || BF518 || BF526 || BF527 || BF536 || BF537
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y.
-	  Make sure you know the name of your card. Read the Ethernet-HOWTO,
-	  available from <http://www.tldp.org/docs.html#howto>.
 
 	  If unsure, say Y.
 
diff --git a/drivers/net/ethernet/agere/Kconfig b/drivers/net/ethernet/agere/Kconfig
index 63e805de619e..b6fe9200355a 100644
--- a/drivers/net/ethernet/agere/Kconfig
+++ b/drivers/net/ethernet/agere/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_AGERE
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/allwinner/Kconfig b/drivers/net/ethernet/allwinner/Kconfig
index d8d95d4cd45a..47da7e7a5b6a 100644
--- a/drivers/net/ethernet/allwinner/Kconfig
+++ b/drivers/net/ethernet/allwinner/Kconfig
@@ -9,8 +9,7 @@ config NET_VENDOR_ALLWINNER
 	depends on ARCH_SUNXI
 	---help---
 	  If you have a network (Ethernet) card belonging to this
-	  class, say Y and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  class, say Y here.
 
 	  Note that the answer to this question doesn't directly
 	  affect the kernel: saying N will just cause the configurator
diff --git a/drivers/net/ethernet/alteon/Kconfig b/drivers/net/ethernet/alteon/Kconfig
index 799a85282070..e06ccab354b5 100644
--- a/drivers/net/ethernet/alteon/Kconfig
+++ b/drivers/net/ethernet/alteon/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_ALTEON
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/amd/Kconfig b/drivers/net/ethernet/amd/Kconfig
index 426916036151..afc62ea804fc 100644
--- a/drivers/net/ethernet/amd/Kconfig
+++ b/drivers/net/ethernet/amd/Kconfig
@@ -37,8 +37,7 @@ config AMD8111_ETH
 	select MII
 	---help---
 	  If you have an AMD 8111-based PCI LANCE ethernet card,
-	  answer Y here and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  answer Y here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called amd8111e.
@@ -47,10 +46,8 @@ config LANCE
 	tristate "AMD LANCE and PCnet (AT1500 and NE2100) support"
 	depends on ISA && ISA_DMA_API && !ARM
 	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>. Some LinkSys cards are
-	  of this type.
+	  If you have a network (Ethernet) card of this type, say Y here.
+	  Some LinkSys cards are of this type.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called lance.  This is recommended.
@@ -62,8 +59,7 @@ config PCNET32
 	select MII
 	---help---
 	  If you have a PCnet32 or PCnetPCI based network (Ethernet) card,
-	  answer Y here and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  answer Y here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called pcnet32.
@@ -144,9 +140,7 @@ config NI65
 	tristate "NI6510 support"
 	depends on ISA && ISA_DMA_API && !ARM
 	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card of this type, say Y here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called ni65.
@@ -179,10 +173,8 @@ config SUNLANCE
 
 config AMD_XGBE
 	tristate "AMD 10GbE Ethernet driver"
-	depends on (OF_NET || ACPI) && HAS_IOMEM && HAS_DMA
+	depends on ((OF_NET && OF_ADDRESS) || ACPI) && HAS_IOMEM && HAS_DMA
 	depends on ARM64 || COMPILE_TEST
-	select PHYLIB
-	select AMD_XGBE_PHY
 	select BITREVERSE
 	select CRC32
 	select PTP_1588_CLOCK
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-common.h b/drivers/net/ethernet/amd/xgbe/xgbe-common.h
index 34c28aac767f..b6fa89102526 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-common.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-common.h
@@ -857,6 +857,48 @@
  */
 #define PCS_MMD_SELECT			0xff
 
+/* SerDes integration register offsets */
+#define SIR0_KR_RT_1			0x002c
+#define SIR0_STATUS			0x0040
+#define SIR1_SPEED			0x0000
+
+/* SerDes integration register entry bit positions and sizes */
+#define SIR0_KR_RT_1_RESET_INDEX	11
+#define SIR0_KR_RT_1_RESET_WIDTH	1
+#define SIR0_STATUS_RX_READY_INDEX	0
+#define SIR0_STATUS_RX_READY_WIDTH	1
+#define SIR0_STATUS_TX_READY_INDEX	8
+#define SIR0_STATUS_TX_READY_WIDTH	1
+#define SIR1_SPEED_CDR_RATE_INDEX	12
+#define SIR1_SPEED_CDR_RATE_WIDTH	4
+#define SIR1_SPEED_DATARATE_INDEX	4
+#define SIR1_SPEED_DATARATE_WIDTH	2
+#define SIR1_SPEED_PLLSEL_INDEX		3
+#define SIR1_SPEED_PLLSEL_WIDTH		1
+#define SIR1_SPEED_RATECHANGE_INDEX	6
+#define SIR1_SPEED_RATECHANGE_WIDTH	1
+#define SIR1_SPEED_TXAMP_INDEX		8
+#define SIR1_SPEED_TXAMP_WIDTH		4
+#define SIR1_SPEED_WORDMODE_INDEX	0
+#define SIR1_SPEED_WORDMODE_WIDTH	3
+
+/* SerDes RxTx register offsets */
+#define RXTX_REG6			0x0018
+#define RXTX_REG20			0x0050
+#define RXTX_REG22			0x0058
+#define RXTX_REG114			0x01c8
+#define RXTX_REG129			0x0204
+
+/* SerDes RxTx register entry bit positions and sizes */
+#define RXTX_REG6_RESETB_RXD_INDEX	8
+#define RXTX_REG6_RESETB_RXD_WIDTH	1
+#define RXTX_REG20_BLWC_ENA_INDEX	2
+#define RXTX_REG20_BLWC_ENA_WIDTH	1
+#define RXTX_REG114_PQ_REG_INDEX	9
+#define RXTX_REG114_PQ_REG_WIDTH	7
+#define RXTX_REG129_RXDFE_CONFIG_INDEX	14
+#define RXTX_REG129_RXDFE_CONFIG_WIDTH	2
+
 /* Descriptor/Packet entry bit positions and sizes */
 #define RX_PACKET_ERRORS_CRC_INDEX		2
 #define RX_PACKET_ERRORS_CRC_WIDTH		1
@@ -973,10 +1015,47 @@
 #define TX_NORMAL_DESC2_VLAN_INSERT		0x2
 
 /* MDIO undefined or vendor specific registers */
+#ifndef MDIO_PMA_10GBR_PMD_CTRL
+#define MDIO_PMA_10GBR_PMD_CTRL		0x0096
+#endif
+
+#ifndef MDIO_PMA_10GBR_FECCTRL
+#define MDIO_PMA_10GBR_FECCTRL		0x00ab
+#endif
+
+#ifndef MDIO_AN_XNP
+#define MDIO_AN_XNP			0x0016
+#endif
+
+#ifndef MDIO_AN_LPX
+#define MDIO_AN_LPX			0x0019
+#endif
+
 #ifndef MDIO_AN_COMP_STAT
 #define MDIO_AN_COMP_STAT		0x0030
 #endif
 
+#ifndef MDIO_AN_INTMASK
+#define MDIO_AN_INTMASK			0x8001
+#endif
+
+#ifndef MDIO_AN_INT
+#define MDIO_AN_INT			0x8002
+#endif
+
+#ifndef MDIO_CTRL1_SPEED1G
+#define MDIO_CTRL1_SPEED1G		(MDIO_CTRL1_SPEED10G & ~BMCR_SPEED100)
+#endif
+
+/* MDIO mask values */
+#define XGBE_XNP_MCF_NULL_MESSAGE	0x001
+#define XGBE_XNP_ACK_PROCESSED		BIT(12)
+#define XGBE_XNP_MP_FORMATTED		BIT(13)
+#define XGBE_XNP_NP_EXCHANGE		BIT(15)
+
+#define XGBE_KR_TRAINING_START		BIT(0)
+#define XGBE_KR_TRAINING_ENABLE		BIT(1)
+
 /* Bit setting and getting macros
  *  The get macro will extract the current bit field value from within
  *  the variable
@@ -1119,6 +1198,82 @@ do {									\
 	ioread32((_pdata)->xpcs_regs + (_off))
 
 /* Macros for building, reading or writing register values or bits
+ * within the register values of SerDes integration registers.
+ */
+#define XSIR_GET_BITS(_var, _prefix, _field)                            \
+	GET_BITS((_var),                                                \
+		 _prefix##_##_field##_INDEX,                            \
+		 _prefix##_##_field##_WIDTH)
+
+#define XSIR_SET_BITS(_var, _prefix, _field, _val)                      \
+	SET_BITS((_var),                                                \
+		 _prefix##_##_field##_INDEX,                            \
+		 _prefix##_##_field##_WIDTH, (_val))
+
+#define XSIR0_IOREAD(_pdata, _reg)					\
+	ioread16((_pdata)->sir0_regs + _reg)
+
+#define XSIR0_IOREAD_BITS(_pdata, _reg, _field)				\
+	GET_BITS(XSIR0_IOREAD((_pdata), _reg),				\
+		 _reg##_##_field##_INDEX,				\
+		 _reg##_##_field##_WIDTH)
+
+#define XSIR0_IOWRITE(_pdata, _reg, _val)				\
+	iowrite16((_val), (_pdata)->sir0_regs + _reg)
+
+#define XSIR0_IOWRITE_BITS(_pdata, _reg, _field, _val)			\
+do {									\
+	u16 reg_val = XSIR0_IOREAD((_pdata), _reg);			\
+	SET_BITS(reg_val,						\
+		 _reg##_##_field##_INDEX,				\
+		 _reg##_##_field##_WIDTH, (_val));			\
+	XSIR0_IOWRITE((_pdata), _reg, reg_val);				\
+} while (0)
+
+#define XSIR1_IOREAD(_pdata, _reg)					\
+	ioread16((_pdata)->sir1_regs + _reg)
+
+#define XSIR1_IOREAD_BITS(_pdata, _reg, _field)				\
+	GET_BITS(XSIR1_IOREAD((_pdata), _reg),				\
+		 _reg##_##_field##_INDEX,				\
+		 _reg##_##_field##_WIDTH)
+
+#define XSIR1_IOWRITE(_pdata, _reg, _val)				\
+	iowrite16((_val), (_pdata)->sir1_regs + _reg)
+
+#define XSIR1_IOWRITE_BITS(_pdata, _reg, _field, _val)			\
+do {									\
+	u16 reg_val = XSIR1_IOREAD((_pdata), _reg);			\
+	SET_BITS(reg_val,						\
+		 _reg##_##_field##_INDEX,				\
+		 _reg##_##_field##_WIDTH, (_val));			\
+	XSIR1_IOWRITE((_pdata), _reg, reg_val);				\
+} while (0)
+
+/* Macros for building, reading or writing register values or bits
+ * within the register values of SerDes RxTx registers.
+ */
+#define XRXTX_IOREAD(_pdata, _reg)					\
+	ioread16((_pdata)->rxtx_regs + _reg)
+
+#define XRXTX_IOREAD_BITS(_pdata, _reg, _field)				\
+	GET_BITS(XRXTX_IOREAD((_pdata), _reg),				\
+		 _reg##_##_field##_INDEX,				\
+		 _reg##_##_field##_WIDTH)
+
+#define XRXTX_IOWRITE(_pdata, _reg, _val)				\
+	iowrite16((_val), (_pdata)->rxtx_regs + _reg)
+
+#define XRXTX_IOWRITE_BITS(_pdata, _reg, _field, _val)			\
+do {									\
+	u16 reg_val = XRXTX_IOREAD((_pdata), _reg);			\
+	SET_BITS(reg_val,						\
+		 _reg##_##_field##_INDEX,				\
+		 _reg##_##_field##_WIDTH, (_val));			\
+	XRXTX_IOWRITE((_pdata), _reg, reg_val);				\
+} while (0)
+
+/* Macros for building, reading or writing register values or bits
  * using MDIO.  Different from above because of the use of standardized
  * Linux include values.  No shifting is performed with the bit
  * operations, everything works on mask values.
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-dcb.c b/drivers/net/ethernet/amd/xgbe/xgbe-dcb.c
index 8a50b01c2686..a6b9899e285f 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-dcb.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-dcb.c
@@ -150,9 +150,12 @@ static int xgbe_dcb_ieee_setets(struct net_device *netdev,
 	tc_ets = 0;
 	tc_ets_weight = 0;
 	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
-		DBGPR("  TC%u: tx_bw=%hhu, rx_bw=%hhu, tsa=%hhu\n", i,
-		      ets->tc_tx_bw[i], ets->tc_rx_bw[i], ets->tc_tsa[i]);
-		DBGPR("  PRIO%u: TC=%hhu\n", i, ets->prio_tc[i]);
+		netif_dbg(pdata, drv, netdev,
+			  "TC%u: tx_bw=%hhu, rx_bw=%hhu, tsa=%hhu\n", i,
+			  ets->tc_tx_bw[i], ets->tc_rx_bw[i],
+			  ets->tc_tsa[i]);
+		netif_dbg(pdata, drv, netdev, "PRIO%u: TC=%hhu\n", i,
+			  ets->prio_tc[i]);
 
 		if ((ets->tc_tx_bw[i] || ets->tc_tsa[i]) &&
 		    (i >= pdata->hw_feat.tc_cnt))
@@ -214,8 +217,9 @@ static int xgbe_dcb_ieee_setpfc(struct net_device *netdev,
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
 
-	DBGPR("  cap=%hhu, en=%hhx, mbc=%hhu, delay=%hhu\n",
-	      pfc->pfc_cap, pfc->pfc_en, pfc->mbc, pfc->delay);
+	netif_dbg(pdata, drv, netdev,
+		  "cap=%hhu, en=%#hhx, mbc=%hhu, delay=%hhu\n",
+		  pfc->pfc_cap, pfc->pfc_en, pfc->mbc, pfc->delay);
 
 	if (!pdata->pfc) {
 		pdata->pfc = devm_kzalloc(pdata->dev, sizeof(*pdata->pfc),
@@ -238,9 +242,10 @@ static u8 xgbe_dcb_getdcbx(struct net_device *netdev)
 
 static u8 xgbe_dcb_setdcbx(struct net_device *netdev, u8 dcbx)
 {
+	struct xgbe_prv_data *pdata = netdev_priv(netdev);
 	u8 support = xgbe_dcb_getdcbx(netdev);
 
-	DBGPR("  DCBX=%#hhx\n", dcbx);
+	netif_dbg(pdata, drv, netdev, "DCBX=%#hhx\n", dcbx);
 
 	if (dcbx & ~support)
 		return 1;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-desc.c b/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
index d81fc6bd4759..661cdaa7ea96 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
@@ -208,8 +208,9 @@ static int xgbe_init_ring(struct xgbe_prv_data *pdata,
 	if (!ring->rdata)
 		return -ENOMEM;
 
-	DBGPR("    rdesc=0x%p, rdesc_dma=0x%llx, rdata=0x%p\n",
-	      ring->rdesc, ring->rdesc_dma, ring->rdata);
+	netif_dbg(pdata, drv, pdata->netdev,
+		  "rdesc=%p, rdesc_dma=%pad, rdata=%p\n",
+		  ring->rdesc, &ring->rdesc_dma, ring->rdata);
 
 	DBGPR("<--xgbe_init_ring\n");
 
@@ -226,7 +227,9 @@ static int xgbe_alloc_ring_resources(struct xgbe_prv_data *pdata)
 
 	channel = pdata->channel;
 	for (i = 0; i < pdata->channel_count; i++, channel++) {
-		DBGPR("  %s - tx_ring:\n", channel->name);
+		netif_dbg(pdata, drv, pdata->netdev, "%s - Tx ring:\n",
+			  channel->name);
+
 		ret = xgbe_init_ring(pdata, channel->tx_ring,
 				     pdata->tx_desc_count);
 		if (ret) {
@@ -235,12 +238,14 @@ static int xgbe_alloc_ring_resources(struct xgbe_prv_data *pdata)
 			goto err_ring;
 		}
 
-		DBGPR("  %s - rx_ring:\n", channel->name);
+		netif_dbg(pdata, drv, pdata->netdev, "%s - Rx ring:\n",
+			  channel->name);
+
 		ret = xgbe_init_ring(pdata, channel->rx_ring,
 				     pdata->rx_desc_count);
 		if (ret) {
 			netdev_alert(pdata->netdev,
-				     "error initializing Tx ring\n");
+				     "error initializing Rx ring\n");
 			goto err_ring;
 		}
 	}
@@ -263,7 +268,7 @@ static int xgbe_alloc_pages(struct xgbe_prv_data *pdata,
 	int ret;
 
 	/* Try to obtain pages, decreasing order if necessary */
-	gfp |= __GFP_COLD | __GFP_COMP;
+	gfp |= __GFP_COLD | __GFP_COMP | __GFP_NOWARN;
 	while (order >= 0) {
 		pages = alloc_pages(gfp, order);
 		if (pages)
@@ -476,8 +481,6 @@ static void xgbe_unmap_rdata(struct xgbe_prv_data *pdata,
 
 	if (rdata->state_saved) {
 		rdata->state_saved = 0;
-		rdata->state.incomplete = 0;
-		rdata->state.context_next = 0;
 		rdata->state.skb = NULL;
 		rdata->state.len = 0;
 		rdata->state.error = 0;
@@ -518,8 +521,6 @@ static int xgbe_map_tx_skb(struct xgbe_channel *channel, struct sk_buff *skb)
 	rdata = XGBE_GET_DESC_DATA(ring, cur_index);
 
 	if (tso) {
-		DBGPR("  TSO packet\n");
-
 		/* Map the TSO header */
 		skb_dma = dma_map_single(pdata->dev, skb->data,
 					 packet->header_len, DMA_TO_DEVICE);
@@ -529,6 +530,9 @@ static int xgbe_map_tx_skb(struct xgbe_channel *channel, struct sk_buff *skb)
 		}
 		rdata->skb_dma = skb_dma;
 		rdata->skb_dma_len = packet->header_len;
+		netif_dbg(pdata, tx_queued, pdata->netdev,
+			  "skb header: index=%u, dma=%pad, len=%u\n",
+			  cur_index, &skb_dma, packet->header_len);
 
 		offset = packet->header_len;
 
@@ -550,8 +554,9 @@ static int xgbe_map_tx_skb(struct xgbe_channel *channel, struct sk_buff *skb)
 		}
 		rdata->skb_dma = skb_dma;
 		rdata->skb_dma_len = len;
-		DBGPR("  skb data: index=%u, dma=0x%llx, len=%u\n",
-		      cur_index, skb_dma, len);
+		netif_dbg(pdata, tx_queued, pdata->netdev,
+			  "skb data: index=%u, dma=%pad, len=%u\n",
+			  cur_index, &skb_dma, len);
 
 		datalen -= len;
 		offset += len;
@@ -563,7 +568,8 @@ static int xgbe_map_tx_skb(struct xgbe_channel *channel, struct sk_buff *skb)
 	}
 
 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
-		DBGPR("  mapping frag %u\n", i);
+		netif_dbg(pdata, tx_queued, pdata->netdev,
+			  "mapping frag %u\n", i);
 
 		frag = &skb_shinfo(skb)->frags[i];
 		offset = 0;
@@ -582,8 +588,9 @@ static int xgbe_map_tx_skb(struct xgbe_channel *channel, struct sk_buff *skb)
 			rdata->skb_dma = skb_dma;
 			rdata->skb_dma_len = len;
 			rdata->mapped_as_page = 1;
-			DBGPR("  skb data: index=%u, dma=0x%llx, len=%u\n",
-			      cur_index, skb_dma, len);
+			netif_dbg(pdata, tx_queued, pdata->netdev,
+				  "skb frag: index=%u, dma=%pad, len=%u\n",
+				  cur_index, &skb_dma, len);
 
 			datalen -= len;
 			offset += len;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-dev.c b/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
index 21d9497518fd..506e832c9e9a 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
@@ -710,7 +710,8 @@ static int xgbe_set_promiscuous_mode(struct xgbe_prv_data *pdata,
 	if (XGMAC_IOREAD_BITS(pdata, MAC_PFR, PR) == val)
 		return 0;
 
-	DBGPR("  %s promiscuous mode\n", enable ? "entering" : "leaving");
+	netif_dbg(pdata, drv, pdata->netdev, "%s promiscuous mode\n",
+		  enable ? "entering" : "leaving");
 	XGMAC_IOWRITE_BITS(pdata, MAC_PFR, PR, val);
 
 	return 0;
@@ -724,7 +725,8 @@ static int xgbe_set_all_multicast_mode(struct xgbe_prv_data *pdata,
 	if (XGMAC_IOREAD_BITS(pdata, MAC_PFR, PM) == val)
 		return 0;
 
-	DBGPR("  %s allmulti mode\n", enable ? "entering" : "leaving");
+	netif_dbg(pdata, drv, pdata->netdev, "%s allmulti mode\n",
+		  enable ? "entering" : "leaving");
 	XGMAC_IOWRITE_BITS(pdata, MAC_PFR, PM, val);
 
 	return 0;
@@ -749,8 +751,9 @@ static void xgbe_set_mac_reg(struct xgbe_prv_data *pdata,
 		mac_addr[0] = ha->addr[4];
 		mac_addr[1] = ha->addr[5];
 
-		DBGPR("  adding mac address %pM at 0x%04x\n", ha->addr,
-		      *mac_reg);
+		netif_dbg(pdata, drv, pdata->netdev,
+			  "adding mac address %pM at %#x\n",
+			  ha->addr, *mac_reg);
 
 		XGMAC_SET_BITS(mac_addr_hi, MAC_MACA1HR, AE, 1);
 	}
@@ -907,23 +910,6 @@ static void xgbe_write_mmd_regs(struct xgbe_prv_data *pdata, int prtad,
 	else
 		mmd_address = (pdata->mdio_mmd << 16) | (mmd_reg & 0xffff);
 
-	/* If the PCS is changing modes, match the MAC speed to it */
-	if (((mmd_address >> 16) == MDIO_MMD_PCS) &&
-	    ((mmd_address & 0xffff) == MDIO_CTRL2)) {
-		struct phy_device *phydev = pdata->phydev;
-
-		if (mmd_data & MDIO_PCS_CTRL2_TYPE) {
-			/* KX mode */
-			if (phydev->supported & SUPPORTED_1000baseKX_Full)
-				xgbe_set_gmii_speed(pdata);
-			else
-				xgbe_set_gmii_2500_speed(pdata);
-		} else {
-			/* KR mode */
-			xgbe_set_xgmii_speed(pdata);
-		}
-	}
-
 	/* The PCS registers are accessed using mmio. The underlying APB3
 	 * management interface uses indirect addressing to access the MMD
 	 * register sets. This requires accessing of the PCS register in two
@@ -1322,7 +1308,8 @@ static void xgbe_config_dcb_tc(struct xgbe_prv_data *pdata)
 	for (i = 0; i < pdata->hw_feat.tc_cnt; i++) {
 		switch (ets->tc_tsa[i]) {
 		case IEEE_8021QAZ_TSA_STRICT:
-			DBGPR("  TC%u using SP\n", i);
+			netif_dbg(pdata, drv, pdata->netdev,
+				  "TC%u using SP\n", i);
 			XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_TC_ETSCR, TSA,
 					       MTL_TSA_SP);
 			break;
@@ -1330,7 +1317,8 @@ static void xgbe_config_dcb_tc(struct xgbe_prv_data *pdata)
 			weight = total_weight * ets->tc_tx_bw[i] / 100;
 			weight = clamp(weight, min_weight, total_weight);
 
-			DBGPR("  TC%u using DWRR (weight %u)\n", i, weight);
+			netif_dbg(pdata, drv, pdata->netdev,
+				  "TC%u using DWRR (weight %u)\n", i, weight);
 			XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_TC_ETSCR, TSA,
 					       MTL_TSA_ETS);
 			XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_TC_QWR, QW,
@@ -1359,7 +1347,8 @@ static void xgbe_config_dcb_pfc(struct xgbe_prv_data *pdata)
 		}
 		mask &= 0xff;
 
-		DBGPR("  TC%u PFC mask=%#x\n", tc, mask);
+		netif_dbg(pdata, drv, pdata->netdev, "TC%u PFC mask=%#x\n",
+			  tc, mask);
 		reg = MTL_TCPM0R + (MTL_TCPM_INC * (tc / MTL_TCPM_TC_PER_REG));
 		reg_val = XGMAC_IOREAD(pdata, reg);
 
@@ -1457,8 +1446,9 @@ static void xgbe_dev_xmit(struct xgbe_channel *channel)
 	/* Create a context descriptor if this is a TSO packet */
 	if (tso_context || vlan_context) {
 		if (tso_context) {
-			DBGPR("  TSO context descriptor, mss=%u\n",
-			      packet->mss);
+			netif_dbg(pdata, tx_queued, pdata->netdev,
+				  "TSO context descriptor, mss=%u\n",
+				  packet->mss);
 
 			/* Set the MSS size */
 			XGMAC_SET_BITS_LE(rdesc->desc2, TX_CONTEXT_DESC2,
@@ -1476,8 +1466,9 @@ static void xgbe_dev_xmit(struct xgbe_channel *channel)
 		}
 
 		if (vlan_context) {
-			DBGPR("  VLAN context descriptor, ctag=%u\n",
-			      packet->vlan_ctag);
+			netif_dbg(pdata, tx_queued, pdata->netdev,
+				  "VLAN context descriptor, ctag=%u\n",
+				  packet->vlan_ctag);
 
 			/* Mark it as a CONTEXT descriptor */
 			XGMAC_SET_BITS_LE(rdesc->desc3, TX_CONTEXT_DESC3,
@@ -1533,6 +1524,8 @@ static void xgbe_dev_xmit(struct xgbe_channel *channel)
 				  packet->tcp_payload_len);
 		XGMAC_SET_BITS_LE(rdesc->desc3, TX_NORMAL_DESC3, TCPHDRLEN,
 				  packet->tcp_header_len / 4);
+
+		pdata->ext_stats.tx_tso_packets++;
 	} else {
 		/* Enable CRC and Pad Insertion */
 		XGMAC_SET_BITS_LE(rdesc->desc3, TX_NORMAL_DESC3, CPC, 0);
@@ -1594,9 +1587,9 @@ static void xgbe_dev_xmit(struct xgbe_channel *channel)
 	rdesc = rdata->rdesc;
 	XGMAC_SET_BITS_LE(rdesc->desc3, TX_NORMAL_DESC3, OWN, 1);
 
-#ifdef XGMAC_ENABLE_TX_DESC_DUMP
-	xgbe_dump_tx_desc(ring, start_index, packet->rdesc_count, 1);
-#endif
+	if (netif_msg_tx_queued(pdata))
+		xgbe_dump_tx_desc(pdata, ring, start_index,
+				  packet->rdesc_count, 1);
 
 	/* Make sure ownership is written to the descriptor */
 	dma_wmb();
@@ -1618,11 +1611,12 @@ static void xgbe_dev_xmit(struct xgbe_channel *channel)
 
 static int xgbe_dev_read(struct xgbe_channel *channel)
 {
+	struct xgbe_prv_data *pdata = channel->pdata;
 	struct xgbe_ring *ring = channel->rx_ring;
 	struct xgbe_ring_data *rdata;
 	struct xgbe_ring_desc *rdesc;
 	struct xgbe_packet_data *packet = &ring->packet_data;
-	struct net_device *netdev = channel->pdata->netdev;
+	struct net_device *netdev = pdata->netdev;
 	unsigned int err, etlt, l34t;
 
 	DBGPR("-->xgbe_dev_read: cur = %d\n", ring->cur);
@@ -1637,9 +1631,8 @@ static int xgbe_dev_read(struct xgbe_channel *channel)
 	/* Make sure descriptor fields are read after reading the OWN bit */
 	dma_rmb();
 
-#ifdef XGMAC_ENABLE_RX_DESC_DUMP
-	xgbe_dump_rx_desc(ring, rdesc, ring->cur);
-#endif
+	if (netif_msg_rx_status(pdata))
+		xgbe_dump_rx_desc(pdata, ring, ring->cur);
 
 	if (XGMAC_GET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, CTXT)) {
 		/* Timestamp Context Descriptor */
@@ -1661,9 +1654,12 @@ static int xgbe_dev_read(struct xgbe_channel *channel)
 			       CONTEXT_NEXT, 1);
 
 	/* Get the header length */
-	if (XGMAC_GET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, FD))
+	if (XGMAC_GET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, FD)) {
 		rdata->rx.hdr_len = XGMAC_GET_BITS_LE(rdesc->desc2,
 						      RX_NORMAL_DESC2, HL);
+		if (rdata->rx.hdr_len)
+			pdata->ext_stats.rx_split_header_packets++;
+	}
 
 	/* Get the RSS hash */
 	if (XGMAC_GET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, RSV)) {
@@ -1700,14 +1696,14 @@ static int xgbe_dev_read(struct xgbe_channel *channel)
 		       INCOMPLETE, 0);
 
 	/* Set checksum done indicator as appropriate */
-	if (channel->pdata->netdev->features & NETIF_F_RXCSUM)
+	if (netdev->features & NETIF_F_RXCSUM)
 		XGMAC_SET_BITS(packet->attributes, RX_PACKET_ATTRIBUTES,
 			       CSUM_DONE, 1);
 
 	/* Check for errors (only valid in last descriptor) */
 	err = XGMAC_GET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, ES);
 	etlt = XGMAC_GET_BITS_LE(rdesc->desc3, RX_NORMAL_DESC3, ETLT);
-	DBGPR("  err=%u, etlt=%#x\n", err, etlt);
+	netif_dbg(pdata, rx_status, netdev, "err=%u, etlt=%#x\n", err, etlt);
 
 	if (!err || !etlt) {
 		/* No error if err is 0 or etlt is 0 */
@@ -1718,7 +1714,8 @@ static int xgbe_dev_read(struct xgbe_channel *channel)
 			packet->vlan_ctag = XGMAC_GET_BITS_LE(rdesc->desc0,
 							      RX_NORMAL_DESC0,
 							      OVT);
-			DBGPR("  vlan-ctag=0x%04x\n", packet->vlan_ctag);
+			netif_dbg(pdata, rx_status, netdev, "vlan-ctag=%#06x\n",
+				  packet->vlan_ctag);
 		}
 	} else {
 		if ((etlt == 0x05) || (etlt == 0x06))
@@ -2026,9 +2023,9 @@ static void xgbe_config_tx_fifo_size(struct xgbe_prv_data *pdata)
 	for (i = 0; i < pdata->tx_q_count; i++)
 		XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_Q_TQOMR, TQS, fifo_size);
 
-	netdev_notice(pdata->netdev,
-		      "%d Tx hardware queues, %d byte fifo per queue\n",
-		      pdata->tx_q_count, ((fifo_size + 1) * 256));
+	netif_info(pdata, drv, pdata->netdev,
+		   "%d Tx hardware queues, %d byte fifo per queue\n",
+		   pdata->tx_q_count, ((fifo_size + 1) * 256));
 }
 
 static void xgbe_config_rx_fifo_size(struct xgbe_prv_data *pdata)
@@ -2042,9 +2039,9 @@ static void xgbe_config_rx_fifo_size(struct xgbe_prv_data *pdata)
 	for (i = 0; i < pdata->rx_q_count; i++)
 		XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_Q_RQOMR, RQS, fifo_size);
 
-	netdev_notice(pdata->netdev,
-		      "%d Rx hardware queues, %d byte fifo per queue\n",
-		      pdata->rx_q_count, ((fifo_size + 1) * 256));
+	netif_info(pdata, drv, pdata->netdev,
+		   "%d Rx hardware queues, %d byte fifo per queue\n",
+		   pdata->rx_q_count, ((fifo_size + 1) * 256));
 }
 
 static void xgbe_config_queue_mapping(struct xgbe_prv_data *pdata)
@@ -2063,14 +2060,16 @@ static void xgbe_config_queue_mapping(struct xgbe_prv_data *pdata)
 
 	for (i = 0, queue = 0; i < pdata->hw_feat.tc_cnt; i++) {
 		for (j = 0; j < qptc; j++) {
-			DBGPR("  TXq%u mapped to TC%u\n", queue, i);
+			netif_dbg(pdata, drv, pdata->netdev,
+				  "TXq%u mapped to TC%u\n", queue, i);
 			XGMAC_MTL_IOWRITE_BITS(pdata, queue, MTL_Q_TQOMR,
 					       Q2TCMAP, i);
 			pdata->q2tc_map[queue++] = i;
 		}
 
 		if (i < qptc_extra) {
-			DBGPR("  TXq%u mapped to TC%u\n", queue, i);
+			netif_dbg(pdata, drv, pdata->netdev,
+				  "TXq%u mapped to TC%u\n", queue, i);
 			XGMAC_MTL_IOWRITE_BITS(pdata, queue, MTL_Q_TQOMR,
 					       Q2TCMAP, i);
 			pdata->q2tc_map[queue++] = i;
@@ -2088,13 +2087,15 @@ static void xgbe_config_queue_mapping(struct xgbe_prv_data *pdata)
 	for (i = 0, prio = 0; i < prio_queues;) {
 		mask = 0;
 		for (j = 0; j < ppq; j++) {
-			DBGPR("  PRIO%u mapped to RXq%u\n", prio, i);
+			netif_dbg(pdata, drv, pdata->netdev,
+				  "PRIO%u mapped to RXq%u\n", prio, i);
 			mask |= (1 << prio);
 			pdata->prio2q_map[prio++] = i;
 		}
 
 		if (i < ppq_extra) {
-			DBGPR("  PRIO%u mapped to RXq%u\n", prio, i);
+			netif_dbg(pdata, drv, pdata->netdev,
+				  "PRIO%u mapped to RXq%u\n", prio, i);
 			mask |= (1 << prio);
 			pdata->prio2q_map[prio++] = i;
 		}
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
index db84ddcfec84..1e9c28d19ef8 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
@@ -183,9 +183,10 @@ static int xgbe_alloc_channels(struct xgbe_prv_data *pdata)
 			channel->rx_ring = rx_ring++;
 		}
 
-		DBGPR("  %s: queue=%u, dma_regs=%p, dma_irq=%d, tx=%p, rx=%p\n",
-		      channel->name, channel->queue_index, channel->dma_regs,
-		      channel->dma_irq, channel->tx_ring, channel->rx_ring);
+		netif_dbg(pdata, drv, pdata->netdev,
+			  "%s: dma_regs=%p, dma_irq=%d, tx=%p, rx=%p\n",
+			  channel->name, channel->dma_regs, channel->dma_irq,
+			  channel->tx_ring, channel->rx_ring);
 	}
 
 	pdata->channel = channel_mem;
@@ -235,7 +236,8 @@ static int xgbe_maybe_stop_tx_queue(struct xgbe_channel *channel,
 	struct xgbe_prv_data *pdata = channel->pdata;
 
 	if (count > xgbe_tx_avail_desc(ring)) {
-		DBGPR("  Tx queue stopped, not enough descriptors available\n");
+		netif_info(pdata, drv, pdata->netdev,
+			   "Tx queue stopped, not enough descriptors available\n");
 		netif_stop_subqueue(pdata->netdev, channel->queue_index);
 		ring->tx.queue_stopped = 1;
 
@@ -330,7 +332,7 @@ static irqreturn_t xgbe_isr(int irq, void *data)
 	if (!dma_isr)
 		goto isr_done;
 
-	DBGPR("  DMA_ISR = %08x\n", dma_isr);
+	netif_dbg(pdata, intr, pdata->netdev, "DMA_ISR=%#010x\n", dma_isr);
 
 	for (i = 0; i < pdata->channel_count; i++) {
 		if (!(dma_isr & (1 << i)))
@@ -339,7 +341,8 @@ static irqreturn_t xgbe_isr(int irq, void *data)
 		channel = pdata->channel + i;
 
 		dma_ch_isr = XGMAC_DMA_IOREAD(channel, DMA_CH_SR);
-		DBGPR("  DMA_CH%u_ISR = %08x\n", i, dma_ch_isr);
+		netif_dbg(pdata, intr, pdata->netdev, "DMA_CH%u_ISR=%#010x\n",
+			  i, dma_ch_isr);
 
 		/* The TI or RI interrupt bits may still be set even if using
 		 * per channel DMA interrupts. Check to be sure those are not
@@ -386,8 +389,6 @@ static irqreturn_t xgbe_isr(int irq, void *data)
 		}
 	}
 
-	DBGPR("  DMA_ISR = %08x\n", XGMAC_IOREAD(pdata, DMA_ISR));
-
 isr_done:
 	return IRQ_HANDLED;
 }
@@ -423,7 +424,7 @@ static void xgbe_tx_timer(unsigned long data)
 	if (napi_schedule_prep(napi)) {
 		/* Disable Tx and Rx interrupts */
 		if (pdata->per_channel_irq)
-			disable_irq(channel->dma_irq);
+			disable_irq_nosync(channel->dma_irq);
 		else
 			xgbe_disable_rx_tx_ints(pdata);
 
@@ -436,43 +437,61 @@ static void xgbe_tx_timer(unsigned long data)
 	DBGPR("<--xgbe_tx_timer\n");
 }
 
-static void xgbe_init_tx_timers(struct xgbe_prv_data *pdata)
+static void xgbe_service(struct work_struct *work)
+{
+	struct xgbe_prv_data *pdata = container_of(work,
+						   struct xgbe_prv_data,
+						   service_work);
+
+	pdata->phy_if.phy_status(pdata);
+}
+
+static void xgbe_service_timer(unsigned long data)
+{
+	struct xgbe_prv_data *pdata = (struct xgbe_prv_data *)data;
+
+	schedule_work(&pdata->service_work);
+
+	mod_timer(&pdata->service_timer, jiffies + HZ);
+}
+
+static void xgbe_init_timers(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_channel *channel;
 	unsigned int i;
 
-	DBGPR("-->xgbe_init_tx_timers\n");
+	setup_timer(&pdata->service_timer, xgbe_service_timer,
+		    (unsigned long)pdata);
 
 	channel = pdata->channel;
 	for (i = 0; i < pdata->channel_count; i++, channel++) {
 		if (!channel->tx_ring)
 			break;
 
-		DBGPR("  %s adding tx timer\n", channel->name);
 		setup_timer(&channel->tx_timer, xgbe_tx_timer,
 			    (unsigned long)channel);
 	}
+}
 
-	DBGPR("<--xgbe_init_tx_timers\n");
+static void xgbe_start_timers(struct xgbe_prv_data *pdata)
+{
+	mod_timer(&pdata->service_timer, jiffies + HZ);
 }
 
-static void xgbe_stop_tx_timers(struct xgbe_prv_data *pdata)
+static void xgbe_stop_timers(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_channel *channel;
 	unsigned int i;
 
-	DBGPR("-->xgbe_stop_tx_timers\n");
+	del_timer_sync(&pdata->service_timer);
 
 	channel = pdata->channel;
 	for (i = 0; i < pdata->channel_count; i++, channel++) {
 		if (!channel->tx_ring)
 			break;
 
-		DBGPR("  %s deleting tx timer\n", channel->name);
 		del_timer_sync(&channel->tx_timer);
 	}
-
-	DBGPR("<--xgbe_stop_tx_timers\n");
 }
 
 void xgbe_get_all_hw_features(struct xgbe_prv_data *pdata)
@@ -512,6 +531,7 @@ void xgbe_get_all_hw_features(struct xgbe_prv_data *pdata)
 						RXFIFOSIZE);
 	hw_feat->tx_fifo_size  = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R,
 						TXFIFOSIZE);
+	hw_feat->adv_ts_hi     = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R, ADVTHWORD);
 	hw_feat->dma_width     = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R, ADDR64);
 	hw_feat->dcb           = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R, DCBEN);
 	hw_feat->sph           = XGMAC_GET_BITS(mac_hfr1, MAC_HWF1R, SPHEN);
@@ -759,112 +779,12 @@ static void xgbe_free_rx_data(struct xgbe_prv_data *pdata)
 	DBGPR("<--xgbe_free_rx_data\n");
 }
 
-static void xgbe_adjust_link(struct net_device *netdev)
-{
-	struct xgbe_prv_data *pdata = netdev_priv(netdev);
-	struct xgbe_hw_if *hw_if = &pdata->hw_if;
-	struct phy_device *phydev = pdata->phydev;
-	int new_state = 0;
-
-	if (!phydev)
-		return;
-
-	if (phydev->link) {
-		/* Flow control support */
-		if (pdata->pause_autoneg) {
-			if (phydev->pause || phydev->asym_pause) {
-				pdata->tx_pause = 1;
-				pdata->rx_pause = 1;
-			} else {
-				pdata->tx_pause = 0;
-				pdata->rx_pause = 0;
-			}
-		}
-
-		if (pdata->tx_pause != pdata->phy_tx_pause) {
-			hw_if->config_tx_flow_control(pdata);
-			pdata->phy_tx_pause = pdata->tx_pause;
-		}
-
-		if (pdata->rx_pause != pdata->phy_rx_pause) {
-			hw_if->config_rx_flow_control(pdata);
-			pdata->phy_rx_pause = pdata->rx_pause;
-		}
-
-		/* Speed support */
-		if (phydev->speed != pdata->phy_speed) {
-			new_state = 1;
-
-			switch (phydev->speed) {
-			case SPEED_10000:
-				hw_if->set_xgmii_speed(pdata);
-				break;
-
-			case SPEED_2500:
-				hw_if->set_gmii_2500_speed(pdata);
-				break;
-
-			case SPEED_1000:
-				hw_if->set_gmii_speed(pdata);
-				break;
-			}
-			pdata->phy_speed = phydev->speed;
-		}
-
-		if (phydev->link != pdata->phy_link) {
-			new_state = 1;
-			pdata->phy_link = 1;
-		}
-	} else if (pdata->phy_link) {
-		new_state = 1;
-		pdata->phy_link = 0;
-		pdata->phy_speed = SPEED_UNKNOWN;
-	}
-
-	if (new_state)
-		phy_print_status(phydev);
-}
-
 static int xgbe_phy_init(struct xgbe_prv_data *pdata)
 {
-	struct net_device *netdev = pdata->netdev;
-	struct phy_device *phydev = pdata->phydev;
-	int ret;
-
 	pdata->phy_link = -1;
 	pdata->phy_speed = SPEED_UNKNOWN;
-	pdata->phy_tx_pause = pdata->tx_pause;
-	pdata->phy_rx_pause = pdata->rx_pause;
 
-	ret = phy_connect_direct(netdev, phydev, &xgbe_adjust_link,
-				 pdata->phy_mode);
-	if (ret) {
-		netdev_err(netdev, "phy_connect_direct failed\n");
-		return ret;
-	}
-
-	if (!phydev->drv || (phydev->drv->phy_id == 0)) {
-		netdev_err(netdev, "phy_id not valid\n");
-		ret = -ENODEV;
-		goto err_phy_connect;
-	}
-	DBGPR("  phy_connect_direct succeeded for PHY %s, link=%d\n",
-	      dev_name(&phydev->dev), phydev->link);
-
-	return 0;
-
-err_phy_connect:
-	phy_disconnect(phydev);
-
-	return ret;
-}
-
-static void xgbe_phy_exit(struct xgbe_prv_data *pdata)
-{
-	if (!pdata->phydev)
-		return;
-
-	phy_disconnect(pdata->phydev);
+	return pdata->phy_if.phy_reset(pdata);
 }
 
 int xgbe_powerdown(struct net_device *netdev, unsigned int caller)
@@ -889,13 +809,14 @@ int xgbe_powerdown(struct net_device *netdev, unsigned int caller)
 
 	netif_tx_stop_all_queues(netdev);
 
+	xgbe_stop_timers(pdata);
+	flush_workqueue(pdata->dev_workqueue);
+
 	hw_if->powerdown_tx(pdata);
 	hw_if->powerdown_rx(pdata);
 
 	xgbe_napi_disable(pdata, 0);
 
-	phy_stop(pdata->phydev);
-
 	pdata->power_down = 1;
 
 	spin_unlock_irqrestore(&pdata->lock, flags);
@@ -924,8 +845,6 @@ int xgbe_powerup(struct net_device *netdev, unsigned int caller)
 
 	pdata->power_down = 0;
 
-	phy_start(pdata->phydev);
-
 	xgbe_napi_enable(pdata, 0);
 
 	hw_if->powerup_tx(pdata);
@@ -936,6 +855,8 @@ int xgbe_powerup(struct net_device *netdev, unsigned int caller)
 
 	netif_tx_start_all_queues(netdev);
 
+	xgbe_start_timers(pdata);
+
 	spin_unlock_irqrestore(&pdata->lock, flags);
 
 	DBGPR("<--xgbe_powerup\n");
@@ -946,6 +867,7 @@ int xgbe_powerup(struct net_device *netdev, unsigned int caller)
 static int xgbe_start(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_hw_if *hw_if = &pdata->hw_if;
+	struct xgbe_phy_if *phy_if = &pdata->phy_if;
 	struct net_device *netdev = pdata->netdev;
 	int ret;
 
@@ -953,7 +875,9 @@ static int xgbe_start(struct xgbe_prv_data *pdata)
 
 	hw_if->init(pdata);
 
-	phy_start(pdata->phydev);
+	ret = phy_if->phy_start(pdata);
+	if (ret)
+		goto err_phy;
 
 	xgbe_napi_enable(pdata, 1);
 
@@ -964,10 +888,11 @@ static int xgbe_start(struct xgbe_prv_data *pdata)
 	hw_if->enable_tx(pdata);
 	hw_if->enable_rx(pdata);
 
-	xgbe_init_tx_timers(pdata);
-
 	netif_tx_start_all_queues(netdev);
 
+	xgbe_start_timers(pdata);
+	schedule_work(&pdata->service_work);
+
 	DBGPR("<--xgbe_start\n");
 
 	return 0;
@@ -975,8 +900,9 @@ static int xgbe_start(struct xgbe_prv_data *pdata)
 err_napi:
 	xgbe_napi_disable(pdata, 1);
 
-	phy_stop(pdata->phydev);
+	phy_if->phy_stop(pdata);
 
+err_phy:
 	hw_if->exit(pdata);
 
 	return ret;
@@ -985,6 +911,7 @@ err_napi:
 static void xgbe_stop(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_hw_if *hw_if = &pdata->hw_if;
+	struct xgbe_phy_if *phy_if = &pdata->phy_if;
 	struct xgbe_channel *channel;
 	struct net_device *netdev = pdata->netdev;
 	struct netdev_queue *txq;
@@ -994,7 +921,8 @@ static void xgbe_stop(struct xgbe_prv_data *pdata)
 
 	netif_tx_stop_all_queues(netdev);
 
-	xgbe_stop_tx_timers(pdata);
+	xgbe_stop_timers(pdata);
+	flush_workqueue(pdata->dev_workqueue);
 
 	hw_if->disable_tx(pdata);
 	hw_if->disable_rx(pdata);
@@ -1003,7 +931,7 @@ static void xgbe_stop(struct xgbe_prv_data *pdata)
 
 	xgbe_napi_disable(pdata, 1);
 
-	phy_stop(pdata->phydev);
+	phy_if->phy_stop(pdata);
 
 	hw_if->exit(pdata);
 
@@ -1374,7 +1302,7 @@ static int xgbe_open(struct net_device *netdev)
 	ret = clk_prepare_enable(pdata->sysclk);
 	if (ret) {
 		netdev_alert(netdev, "dma clk_prepare_enable failed\n");
-		goto err_phy_init;
+		return ret;
 	}
 
 	ret = clk_prepare_enable(pdata->ptpclk);
@@ -1399,14 +1327,17 @@ static int xgbe_open(struct net_device *netdev)
 	if (ret)
 		goto err_channels;
 
-	/* Initialize the device restart and Tx timestamp work struct */
+	INIT_WORK(&pdata->service_work, xgbe_service);
 	INIT_WORK(&pdata->restart_work, xgbe_restart);
 	INIT_WORK(&pdata->tx_tstamp_work, xgbe_tx_tstamp);
+	xgbe_init_timers(pdata);
 
 	ret = xgbe_start(pdata);
 	if (ret)
 		goto err_rings;
 
+	clear_bit(XGBE_DOWN, &pdata->dev_state);
+
 	DBGPR("<--xgbe_open\n");
 
 	return 0;
@@ -1423,9 +1354,6 @@ err_ptpclk:
 err_sysclk:
 	clk_disable_unprepare(pdata->sysclk);
 
-err_phy_init:
-	xgbe_phy_exit(pdata);
-
 	return ret;
 }
 
@@ -1449,8 +1377,7 @@ static int xgbe_close(struct net_device *netdev)
 	clk_disable_unprepare(pdata->ptpclk);
 	clk_disable_unprepare(pdata->sysclk);
 
-	/* Release the phy */
-	xgbe_phy_exit(pdata);
+	set_bit(XGBE_DOWN, &pdata->dev_state);
 
 	DBGPR("<--xgbe_close\n");
 
@@ -1478,7 +1405,8 @@ static int xgbe_xmit(struct sk_buff *skb, struct net_device *netdev)
 	ret = NETDEV_TX_OK;
 
 	if (skb->len == 0) {
-		netdev_err(netdev, "empty skb received from stack\n");
+		netif_err(pdata, tx_err, netdev,
+			  "empty skb received from stack\n");
 		dev_kfree_skb_any(skb);
 		goto tx_netdev_return;
 	}
@@ -1494,7 +1422,8 @@ static int xgbe_xmit(struct sk_buff *skb, struct net_device *netdev)
 
 	ret = xgbe_prep_tso(skb, packet);
 	if (ret) {
-		netdev_err(netdev, "error processing TSO packet\n");
+		netif_err(pdata, tx_err, netdev,
+			  "error processing TSO packet\n");
 		dev_kfree_skb_any(skb);
 		goto tx_netdev_return;
 	}
@@ -1513,9 +1442,8 @@ static int xgbe_xmit(struct sk_buff *skb, struct net_device *netdev)
 	/* Configure required descriptor fields for transmission */
 	hw_if->dev_xmit(channel);
 
-#ifdef XGMAC_ENABLE_TX_PKT_DUMP
-	xgbe_print_pkt(netdev, skb, true);
-#endif
+	if (netif_msg_pktdata(pdata))
+		xgbe_print_pkt(netdev, skb, true);
 
 	/* Stop the queue in advance if there may not be enough descriptors */
 	xgbe_maybe_stop_tx_queue(channel, ring, XGBE_TX_MAX_DESCS);
@@ -1710,7 +1638,8 @@ static int xgbe_setup_tc(struct net_device *netdev, u8 tc)
 			       (pdata->q2tc_map[queue] == i))
 				queue++;
 
-			DBGPR("  TC%u using TXq%u-%u\n", i, offset, queue - 1);
+			netif_dbg(pdata, drv, netdev, "TC%u using TXq%u-%u\n",
+				  i, offset, queue - 1);
 			netdev_set_tc_queue(netdev, i, queue - offset, offset);
 			offset = queue;
 		}
@@ -1820,9 +1749,10 @@ static void xgbe_rx_refresh(struct xgbe_channel *channel)
 			  lower_32_bits(rdata->rdesc_dma));
 }
 
-static struct sk_buff *xgbe_create_skb(struct napi_struct *napi,
+static struct sk_buff *xgbe_create_skb(struct xgbe_prv_data *pdata,
+				       struct napi_struct *napi,
 				       struct xgbe_ring_data *rdata,
-				       unsigned int *len)
+				       unsigned int len)
 {
 	struct sk_buff *skb;
 	u8 *packet;
@@ -1832,14 +1762,31 @@ static struct sk_buff *xgbe_create_skb(struct napi_struct *napi,
 	if (!skb)
 		return NULL;
 
+	/* Start with the header buffer which may contain just the header
+	 * or the header plus data
+	 */
+	dma_sync_single_for_cpu(pdata->dev, rdata->rx.hdr.dma,
+				rdata->rx.hdr.dma_len, DMA_FROM_DEVICE);
+
 	packet = page_address(rdata->rx.hdr.pa.pages) +
 		 rdata->rx.hdr.pa.pages_offset;
-	copy_len = (rdata->rx.hdr_len) ? rdata->rx.hdr_len : *len;
+	copy_len = (rdata->rx.hdr_len) ? rdata->rx.hdr_len : len;
 	copy_len = min(rdata->rx.hdr.dma_len, copy_len);
 	skb_copy_to_linear_data(skb, packet, copy_len);
 	skb_put(skb, copy_len);
 
-	*len -= copy_len;
+	len -= copy_len;
+	if (len) {
+		/* Add the remaining data as a frag */
+		dma_sync_single_for_cpu(pdata->dev, rdata->rx.buf.dma,
+					rdata->rx.buf.dma_len, DMA_FROM_DEVICE);
+
+		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
+				rdata->rx.buf.pa.pages,
+				rdata->rx.buf.pa.pages_offset,
+				len, rdata->rx.buf.dma_len);
+		rdata->rx.buf.pa.pages = NULL;
+	}
 
 	return skb;
 }
@@ -1877,9 +1824,8 @@ static int xgbe_tx_poll(struct xgbe_channel *channel)
 		 * bit */
 		dma_rmb();
 
-#ifdef XGMAC_ENABLE_TX_DESC_DUMP
-		xgbe_dump_tx_desc(ring, ring->dirty, 1, 0);
-#endif
+		if (netif_msg_tx_done(pdata))
+			xgbe_dump_tx_desc(pdata, ring, ring->dirty, 1, 0);
 
 		if (hw_if->is_last_desc(rdesc)) {
 			tx_packets += rdata->tx.packets;
@@ -1922,7 +1868,7 @@ static int xgbe_rx_poll(struct xgbe_channel *channel, int budget)
 	struct sk_buff *skb;
 	struct skb_shared_hwtstamps *hwtstamps;
 	unsigned int incomplete, error, context_next, context;
-	unsigned int len, put_len, max_len;
+	unsigned int len, rdesc_len, max_len;
 	unsigned int received = 0;
 	int packet_count = 0;
 
@@ -1932,6 +1878,9 @@ static int xgbe_rx_poll(struct xgbe_channel *channel, int budget)
 	if (!ring)
 		return 0;
 
+	incomplete = 0;
+	context_next = 0;
+
 	napi = (pdata->per_channel_irq) ? &channel->napi : &pdata->napi;
 
 	rdata = XGBE_GET_DESC_DATA(ring, ring->cur);
@@ -1941,15 +1890,11 @@ static int xgbe_rx_poll(struct xgbe_channel *channel, int budget)
 
 		/* First time in loop see if we need to restore state */
 		if (!received && rdata->state_saved) {
-			incomplete = rdata->state.incomplete;
-			context_next = rdata->state.context_next;
 			skb = rdata->state.skb;
 			error = rdata->state.error;
 			len = rdata->state.len;
 		} else {
 			memset(packet, 0, sizeof(*packet));
-			incomplete = 0;
-			context_next = 0;
 			skb = NULL;
 			error = 0;
 			len = 0;
@@ -1983,29 +1928,23 @@ read_again:
 
 		if (error || packet->errors) {
 			if (packet->errors)
-				DBGPR("Error in received packet\n");
+				netif_err(pdata, rx_err, netdev,
+					  "error in received packet\n");
 			dev_kfree_skb(skb);
 			goto next_packet;
 		}
 
 		if (!context) {
-			put_len = rdata->rx.len - len;
-			len += put_len;
-
-			if (!skb) {
-				dma_sync_single_for_cpu(pdata->dev,
-							rdata->rx.hdr.dma,
-							rdata->rx.hdr.dma_len,
-							DMA_FROM_DEVICE);
-
-				skb = xgbe_create_skb(napi, rdata, &put_len);
-				if (!skb) {
+			/* Length is cumulative, get this descriptor's length */
+			rdesc_len = rdata->rx.len - len;
+			len += rdesc_len;
+
+			if (rdesc_len && !skb) {
+				skb = xgbe_create_skb(pdata, napi, rdata,
+						      rdesc_len);
+				if (!skb)
 					error = 1;
-					goto skip_data;
-				}
-			}
-
-			if (put_len) {
+			} else if (rdesc_len) {
 				dma_sync_single_for_cpu(pdata->dev,
 							rdata->rx.buf.dma,
 							rdata->rx.buf.dma_len,
@@ -2014,12 +1953,12 @@ read_again:
 				skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
 						rdata->rx.buf.pa.pages,
 						rdata->rx.buf.pa.pages_offset,
-						put_len, rdata->rx.buf.dma_len);
+						rdesc_len,
+						rdata->rx.buf.dma_len);
 				rdata->rx.buf.pa.pages = NULL;
 			}
 		}
 
-skip_data:
 		if (incomplete || context_next)
 			goto read_again;
 
@@ -2033,14 +1972,14 @@ skip_data:
 			max_len += VLAN_HLEN;
 
 		if (skb->len > max_len) {
-			DBGPR("packet length exceeds configured MTU\n");
+			netif_err(pdata, rx_err, netdev,
+				  "packet length exceeds configured MTU\n");
 			dev_kfree_skb(skb);
 			goto next_packet;
 		}
 
-#ifdef XGMAC_ENABLE_RX_PKT_DUMP
-		xgbe_print_pkt(netdev, skb, false);
-#endif
+		if (netif_msg_pktdata(pdata))
+			xgbe_print_pkt(netdev, skb, false);
 
 		skb_checksum_none_assert(skb);
 		if (XGMAC_GET_BITS(packet->attributes,
@@ -2072,7 +2011,6 @@ skip_data:
 		skb_record_rx_queue(skb, channel->queue_index);
 		skb_mark_napi_id(skb, napi);
 
-		netdev->last_rx = jiffies;
 		napi_gro_receive(napi, skb);
 
 next_packet:
@@ -2083,8 +2021,6 @@ next_packet:
 	if (received && (incomplete || context_next)) {
 		rdata = XGBE_GET_DESC_DATA(ring, ring->cur);
 		rdata->state_saved = 1;
-		rdata->state.incomplete = incomplete;
-		rdata->state.context_next = context_next;
 		rdata->state.skb = skb;
 		rdata->state.len = len;
 		rdata->state.error = error;
@@ -2165,8 +2101,8 @@ static int xgbe_all_poll(struct napi_struct *napi, int budget)
 	return processed;
 }
 
-void xgbe_dump_tx_desc(struct xgbe_ring *ring, unsigned int idx,
-		       unsigned int count, unsigned int flag)
+void xgbe_dump_tx_desc(struct xgbe_prv_data *pdata, struct xgbe_ring *ring,
+		       unsigned int idx, unsigned int count, unsigned int flag)
 {
 	struct xgbe_ring_data *rdata;
 	struct xgbe_ring_desc *rdesc;
@@ -2174,20 +2110,29 @@ void xgbe_dump_tx_desc(struct xgbe_ring *ring, unsigned int idx,
 	while (count--) {
 		rdata = XGBE_GET_DESC_DATA(ring, idx);
 		rdesc = rdata->rdesc;
-		pr_alert("TX_NORMAL_DESC[%d %s] = %08x:%08x:%08x:%08x\n", idx,
-			 (flag == 1) ? "QUEUED FOR TX" : "TX BY DEVICE",
-			 le32_to_cpu(rdesc->desc0), le32_to_cpu(rdesc->desc1),
-			 le32_to_cpu(rdesc->desc2), le32_to_cpu(rdesc->desc3));
+		netdev_dbg(pdata->netdev,
+			   "TX_NORMAL_DESC[%d %s] = %08x:%08x:%08x:%08x\n", idx,
+			   (flag == 1) ? "QUEUED FOR TX" : "TX BY DEVICE",
+			   le32_to_cpu(rdesc->desc0),
+			   le32_to_cpu(rdesc->desc1),
+			   le32_to_cpu(rdesc->desc2),
+			   le32_to_cpu(rdesc->desc3));
 		idx++;
 	}
 }
 
-void xgbe_dump_rx_desc(struct xgbe_ring *ring, struct xgbe_ring_desc *desc,
+void xgbe_dump_rx_desc(struct xgbe_prv_data *pdata, struct xgbe_ring *ring,
 		       unsigned int idx)
 {
-	pr_alert("RX_NORMAL_DESC[%d RX BY DEVICE] = %08x:%08x:%08x:%08x\n", idx,
-		 le32_to_cpu(desc->desc0), le32_to_cpu(desc->desc1),
-		 le32_to_cpu(desc->desc2), le32_to_cpu(desc->desc3));
+	struct xgbe_ring_data *rdata;
+	struct xgbe_ring_desc *rdesc;
+
+	rdata = XGBE_GET_DESC_DATA(ring, idx);
+	rdesc = rdata->rdesc;
+	netdev_dbg(pdata->netdev,
+		   "RX_NORMAL_DESC[%d RX BY DEVICE] = %08x:%08x:%08x:%08x\n",
+		   idx, le32_to_cpu(rdesc->desc0), le32_to_cpu(rdesc->desc1),
+		   le32_to_cpu(rdesc->desc2), le32_to_cpu(rdesc->desc3));
 }
 
 void xgbe_print_pkt(struct net_device *netdev, struct sk_buff *skb, bool tx_rx)
@@ -2197,21 +2142,21 @@ void xgbe_print_pkt(struct net_device *netdev, struct sk_buff *skb, bool tx_rx)
 	unsigned char buffer[128];
 	unsigned int i, j;
 
-	netdev_alert(netdev, "\n************** SKB dump ****************\n");
+	netdev_dbg(netdev, "\n************** SKB dump ****************\n");
 
-	netdev_alert(netdev, "%s packet of %d bytes\n",
-		     (tx_rx ? "TX" : "RX"), skb->len);
+	netdev_dbg(netdev, "%s packet of %d bytes\n",
+		   (tx_rx ? "TX" : "RX"), skb->len);
 
-	netdev_alert(netdev, "Dst MAC addr: %pM\n", eth->h_dest);
-	netdev_alert(netdev, "Src MAC addr: %pM\n", eth->h_source);
-	netdev_alert(netdev, "Protocol: 0x%04hx\n", ntohs(eth->h_proto));
+	netdev_dbg(netdev, "Dst MAC addr: %pM\n", eth->h_dest);
+	netdev_dbg(netdev, "Src MAC addr: %pM\n", eth->h_source);
+	netdev_dbg(netdev, "Protocol: %#06hx\n", ntohs(eth->h_proto));
 
 	for (i = 0, j = 0; i < skb->len;) {
 		j += snprintf(buffer + j, sizeof(buffer) - j, "%02hhx",
 			      buf[i++]);
 
 		if ((i % 32) == 0) {
-			netdev_alert(netdev, "  0x%04x: %s\n", i - 32, buffer);
+			netdev_dbg(netdev, "  %#06x: %s\n", i - 32, buffer);
 			j = 0;
 		} else if ((i % 16) == 0) {
 			buffer[j++] = ' ';
@@ -2221,7 +2166,7 @@ void xgbe_print_pkt(struct net_device *netdev, struct sk_buff *skb, bool tx_rx)
 		}
 	}
 	if (i % 32)
-		netdev_alert(netdev, "  0x%04x: %s\n", i - (i % 32), buffer);
+		netdev_dbg(netdev, "  %#06x: %s\n", i - (i % 32), buffer);
 
-	netdev_alert(netdev, "\n************** SKB dump ****************\n");
+	netdev_dbg(netdev, "\n************** SKB dump ****************\n");
 }
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
index 5f149e8ee20f..59e090e95c0e 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
@@ -133,6 +133,12 @@ struct xgbe_stats {
 	  offsetof(struct xgbe_prv_data, mmc_stats._var),	\
 	}
 
+#define XGMAC_EXT_STAT(_string, _var)				\
+	{ _string,						\
+	  FIELD_SIZEOF(struct xgbe_ext_stats, _var),		\
+	  offsetof(struct xgbe_prv_data, ext_stats._var),	\
+	}
+
 static const struct xgbe_stats xgbe_gstring_stats[] = {
 	XGMAC_MMC_STAT("tx_bytes", txoctetcount_gb),
 	XGMAC_MMC_STAT("tx_packets", txframecount_gb),
@@ -140,6 +146,7 @@ static const struct xgbe_stats xgbe_gstring_stats[] = {
 	XGMAC_MMC_STAT("tx_broadcast_packets", txbroadcastframes_gb),
 	XGMAC_MMC_STAT("tx_multicast_packets", txmulticastframes_gb),
 	XGMAC_MMC_STAT("tx_vlan_packets", txvlanframes_g),
+	XGMAC_EXT_STAT("tx_tso_packets", tx_tso_packets),
 	XGMAC_MMC_STAT("tx_64_byte_packets", tx64octets_gb),
 	XGMAC_MMC_STAT("tx_65_to_127_byte_packets", tx65to127octets_gb),
 	XGMAC_MMC_STAT("tx_128_to_255_byte_packets", tx128to255octets_gb),
@@ -171,6 +178,7 @@ static const struct xgbe_stats xgbe_gstring_stats[] = {
 	XGMAC_MMC_STAT("rx_fifo_overflow_errors", rxfifooverflow),
 	XGMAC_MMC_STAT("rx_watchdog_errors", rxwatchdogerror),
 	XGMAC_MMC_STAT("rx_pause_frames", rxpauseframes),
+	XGMAC_EXT_STAT("rx_split_header_packets", rx_split_header_packets),
 };
 
 #define XGBE_STATS_COUNT	ARRAY_SIZE(xgbe_gstring_stats)
@@ -239,9 +247,9 @@ static void xgbe_get_pauseparam(struct net_device *netdev,
 
 	DBGPR("-->xgbe_get_pauseparam\n");
 
-	pause->autoneg = pdata->pause_autoneg;
-	pause->tx_pause = pdata->tx_pause;
-	pause->rx_pause = pdata->rx_pause;
+	pause->autoneg = pdata->phy.pause_autoneg;
+	pause->tx_pause = pdata->phy.tx_pause;
+	pause->rx_pause = pdata->phy.rx_pause;
 
 	DBGPR("<--xgbe_get_pauseparam\n");
 }
@@ -250,7 +258,6 @@ static int xgbe_set_pauseparam(struct net_device *netdev,
 			       struct ethtool_pauseparam *pause)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
-	struct phy_device *phydev = pdata->phydev;
 	int ret = 0;
 
 	DBGPR("-->xgbe_set_pauseparam\n");
@@ -258,21 +265,26 @@ static int xgbe_set_pauseparam(struct net_device *netdev,
 	DBGPR("  autoneg = %d, tx_pause = %d, rx_pause = %d\n",
 	      pause->autoneg, pause->tx_pause, pause->rx_pause);
 
-	pdata->pause_autoneg = pause->autoneg;
-	if (pause->autoneg) {
-		phydev->advertising |= ADVERTISED_Pause;
-		phydev->advertising |= ADVERTISED_Asym_Pause;
+	if (pause->autoneg && (pdata->phy.autoneg != AUTONEG_ENABLE))
+		return -EINVAL;
+
+	pdata->phy.pause_autoneg = pause->autoneg;
+	pdata->phy.tx_pause = pause->tx_pause;
+	pdata->phy.rx_pause = pause->rx_pause;
 
-	} else {
-		phydev->advertising &= ~ADVERTISED_Pause;
-		phydev->advertising &= ~ADVERTISED_Asym_Pause;
+	pdata->phy.advertising &= ~ADVERTISED_Pause;
+	pdata->phy.advertising &= ~ADVERTISED_Asym_Pause;
 
-		pdata->tx_pause = pause->tx_pause;
-		pdata->rx_pause = pause->rx_pause;
+	if (pause->rx_pause) {
+		pdata->phy.advertising |= ADVERTISED_Pause;
+		pdata->phy.advertising |= ADVERTISED_Asym_Pause;
 	}
 
+	if (pause->tx_pause)
+		pdata->phy.advertising ^= ADVERTISED_Asym_Pause;
+
 	if (netif_running(netdev))
-		ret = phy_start_aneg(phydev);
+		ret = pdata->phy_if.phy_config_aneg(pdata);
 
 	DBGPR("<--xgbe_set_pauseparam\n");
 
@@ -283,36 +295,39 @@ static int xgbe_get_settings(struct net_device *netdev,
 			     struct ethtool_cmd *cmd)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
-	int ret;
 
 	DBGPR("-->xgbe_get_settings\n");
 
-	if (!pdata->phydev)
-		return -ENODEV;
+	cmd->phy_address = pdata->phy.address;
+
+	cmd->supported = pdata->phy.supported;
+	cmd->advertising = pdata->phy.advertising;
+	cmd->lp_advertising = pdata->phy.lp_advertising;
+
+	cmd->autoneg = pdata->phy.autoneg;
+	ethtool_cmd_speed_set(cmd, pdata->phy.speed);
+	cmd->duplex = pdata->phy.duplex;
 
-	ret = phy_ethtool_gset(pdata->phydev, cmd);
+	cmd->port = PORT_NONE;
+	cmd->transceiver = XCVR_INTERNAL;
 
 	DBGPR("<--xgbe_get_settings\n");
 
-	return ret;
+	return 0;
 }
 
 static int xgbe_set_settings(struct net_device *netdev,
 			     struct ethtool_cmd *cmd)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
-	struct phy_device *phydev = pdata->phydev;
 	u32 speed;
 	int ret;
 
 	DBGPR("-->xgbe_set_settings\n");
 
-	if (!pdata->phydev)
-		return -ENODEV;
-
 	speed = ethtool_cmd_speed(cmd);
 
-	if (cmd->phy_address != phydev->addr)
+	if (cmd->phy_address != pdata->phy.address)
 		return -EINVAL;
 
 	if ((cmd->autoneg != AUTONEG_ENABLE) &&
@@ -333,23 +348,23 @@ static int xgbe_set_settings(struct net_device *netdev,
 			return -EINVAL;
 	}
 
-	cmd->advertising &= phydev->supported;
+	cmd->advertising &= pdata->phy.supported;
 	if ((cmd->autoneg == AUTONEG_ENABLE) && !cmd->advertising)
 		return -EINVAL;
 
 	ret = 0;
-	phydev->autoneg = cmd->autoneg;
-	phydev->speed = speed;
-	phydev->duplex = cmd->duplex;
-	phydev->advertising = cmd->advertising;
+	pdata->phy.autoneg = cmd->autoneg;
+	pdata->phy.speed = speed;
+	pdata->phy.duplex = cmd->duplex;
+	pdata->phy.advertising = cmd->advertising;
 
 	if (cmd->autoneg == AUTONEG_ENABLE)
-		phydev->advertising |= ADVERTISED_Autoneg;
+		pdata->phy.advertising |= ADVERTISED_Autoneg;
 	else
-		phydev->advertising &= ~ADVERTISED_Autoneg;
+		pdata->phy.advertising &= ~ADVERTISED_Autoneg;
 
 	if (netif_running(netdev))
-		ret = phy_start_aneg(phydev);
+		ret = pdata->phy_if.phy_config_aneg(pdata);
 
 	DBGPR("<--xgbe_set_settings\n");
 
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-main.c b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
index 714905384900..e83bd76abce6 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-main.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
@@ -124,9 +124,11 @@
 #include <linux/of.h>
 #include <linux/of_net.h>
 #include <linux/of_address.h>
+#include <linux/of_platform.h>
 #include <linux/clk.h>
 #include <linux/property.h>
 #include <linux/acpi.h>
+#include <linux/mdio.h>
 
 #include "xgbe.h"
 #include "xgbe-common.h"
@@ -136,6 +138,49 @@ MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION(XGBE_DRV_VERSION);
 MODULE_DESCRIPTION(XGBE_DRV_DESC);
 
+static int debug = -1;
+module_param(debug, int, S_IWUSR | S_IRUGO);
+MODULE_PARM_DESC(debug, " Network interface message level setting");
+
+static const u32 default_msg_level = (NETIF_MSG_LINK | NETIF_MSG_IFDOWN |
+				      NETIF_MSG_IFUP);
+
+static const u32 xgbe_serdes_blwc[] = {
+	XGBE_SPEED_1000_BLWC,
+	XGBE_SPEED_2500_BLWC,
+	XGBE_SPEED_10000_BLWC,
+};
+
+static const u32 xgbe_serdes_cdr_rate[] = {
+	XGBE_SPEED_1000_CDR,
+	XGBE_SPEED_2500_CDR,
+	XGBE_SPEED_10000_CDR,
+};
+
+static const u32 xgbe_serdes_pq_skew[] = {
+	XGBE_SPEED_1000_PQ,
+	XGBE_SPEED_2500_PQ,
+	XGBE_SPEED_10000_PQ,
+};
+
+static const u32 xgbe_serdes_tx_amp[] = {
+	XGBE_SPEED_1000_TXAMP,
+	XGBE_SPEED_2500_TXAMP,
+	XGBE_SPEED_10000_TXAMP,
+};
+
+static const u32 xgbe_serdes_dfe_tap_cfg[] = {
+	XGBE_SPEED_1000_DFE_TAP_CONFIG,
+	XGBE_SPEED_2500_DFE_TAP_CONFIG,
+	XGBE_SPEED_10000_DFE_TAP_CONFIG,
+};
+
+static const u32 xgbe_serdes_dfe_tap_ena[] = {
+	XGBE_SPEED_1000_DFE_TAP_ENABLE,
+	XGBE_SPEED_2500_DFE_TAP_ENABLE,
+	XGBE_SPEED_10000_DFE_TAP_ENABLE,
+};
+
 static void xgbe_default_config(struct xgbe_prv_data *pdata)
 {
 	DBGPR("-->xgbe_default_config\n");
@@ -153,8 +198,6 @@ static void xgbe_default_config(struct xgbe_prv_data *pdata)
 	pdata->rx_pause = 1;
 	pdata->phy_speed = SPEED_UNKNOWN;
 	pdata->power_down = 0;
-	pdata->default_autoneg = AUTONEG_ENABLE;
-	pdata->default_speed = SPEED_10000;
 
 	DBGPR("<--xgbe_default_config\n");
 }
@@ -162,19 +205,15 @@ static void xgbe_default_config(struct xgbe_prv_data *pdata)
 static void xgbe_init_all_fptrs(struct xgbe_prv_data *pdata)
 {
 	xgbe_init_function_ptrs_dev(&pdata->hw_if);
+	xgbe_init_function_ptrs_phy(&pdata->phy_if);
 	xgbe_init_function_ptrs_desc(&pdata->desc_if);
 }
 
 #ifdef CONFIG_ACPI
 static int xgbe_acpi_support(struct xgbe_prv_data *pdata)
 {
-	struct acpi_device *adev = pdata->adev;
 	struct device *dev = pdata->dev;
 	u32 property;
-	acpi_handle handle;
-	acpi_status status;
-	unsigned long long data;
-	int cca;
 	int ret;
 
 	/* Obtain the system clock setting */
@@ -195,24 +234,6 @@ static int xgbe_acpi_support(struct xgbe_prv_data *pdata)
 	}
 	pdata->ptpclk_rate = property;
 
-	/* Retrieve the device cache coherency value */
-	handle = adev->handle;
-	do {
-		status = acpi_evaluate_integer(handle, "_CCA", NULL, &data);
-		if (!ACPI_FAILURE(status)) {
-			cca = data;
-			break;
-		}
-
-		status = acpi_get_parent(handle, &handle);
-	} while (!ACPI_FAILURE(status));
-
-	if (ACPI_FAILURE(status)) {
-		dev_err(dev, "error obtaining acpi coherency value\n");
-		return -EINVAL;
-	}
-	pdata->coherent = !!cca;
-
 	return 0;
 }
 #else   /* CONFIG_ACPI */
@@ -243,28 +264,84 @@ static int xgbe_of_support(struct xgbe_prv_data *pdata)
 	}
 	pdata->ptpclk_rate = clk_get_rate(pdata->ptpclk);
 
-	/* Retrieve the device cache coherency value */
-	pdata->coherent = of_dma_is_coherent(dev->of_node);
-
 	return 0;
 }
+
+static struct platform_device *xgbe_of_get_phy_pdev(struct xgbe_prv_data *pdata)
+{
+	struct device *dev = pdata->dev;
+	struct device_node *phy_node;
+	struct platform_device *phy_pdev;
+
+	phy_node = of_parse_phandle(dev->of_node, "phy-handle", 0);
+	if (phy_node) {
+		/* Old style device tree:
+		 *   The XGBE and PHY resources are separate
+		 */
+		phy_pdev = of_find_device_by_node(phy_node);
+		of_node_put(phy_node);
+	} else {
+		/* New style device tree:
+		 *   The XGBE and PHY resources are grouped together with
+		 *   the PHY resources listed last
+		 */
+		get_device(dev);
+		phy_pdev = pdata->pdev;
+	}
+
+	return phy_pdev;
+}
 #else   /* CONFIG_OF */
 static int xgbe_of_support(struct xgbe_prv_data *pdata)
 {
 	return -EINVAL;
 }
-#endif  /*CONFIG_OF */
+
+static struct platform_device *xgbe_of_get_phy_pdev(struct xgbe_prv_data *pdata)
+{
+	return NULL;
+}
+#endif  /* CONFIG_OF */
+
+static unsigned int xgbe_resource_count(struct platform_device *pdev,
+					unsigned int type)
+{
+	unsigned int count;
+	int i;
+
+	for (i = 0, count = 0; i < pdev->num_resources; i++) {
+		struct resource *res = &pdev->resource[i];
+
+		if (type == resource_type(res))
+			count++;
+	}
+
+	return count;
+}
+
+static struct platform_device *xgbe_get_phy_pdev(struct xgbe_prv_data *pdata)
+{
+	struct platform_device *phy_pdev;
+
+	if (pdata->use_acpi) {
+		get_device(pdata->dev);
+		phy_pdev = pdata->pdev;
+	} else {
+		phy_pdev = xgbe_of_get_phy_pdev(pdata);
+	}
+
+	return phy_pdev;
+}
 
 static int xgbe_probe(struct platform_device *pdev)
 {
 	struct xgbe_prv_data *pdata;
-	struct xgbe_hw_if *hw_if;
-	struct xgbe_desc_if *desc_if;
 	struct net_device *netdev;
-	struct device *dev = &pdev->dev;
+	struct device *dev = &pdev->dev, *phy_dev;
+	struct platform_device *phy_pdev;
 	struct resource *res;
 	const char *phy_mode;
-	unsigned int i;
+	unsigned int i, phy_memnum, phy_irqnum;
 	int ret;
 
 	DBGPR("--> xgbe_probe\n");
@@ -289,9 +366,36 @@ static int xgbe_probe(struct platform_device *pdev)
 	mutex_init(&pdata->rss_mutex);
 	spin_lock_init(&pdata->tstamp_lock);
 
+	pdata->msg_enable = netif_msg_init(debug, default_msg_level);
+
+	set_bit(XGBE_DOWN, &pdata->dev_state);
+
 	/* Check if we should use ACPI or DT */
 	pdata->use_acpi = (!pdata->adev || acpi_disabled) ? 0 : 1;
 
+	phy_pdev = xgbe_get_phy_pdev(pdata);
+	if (!phy_pdev) {
+		dev_err(dev, "unable to obtain phy device\n");
+		ret = -EINVAL;
+		goto err_phydev;
+	}
+	phy_dev = &phy_pdev->dev;
+
+	if (pdev == phy_pdev) {
+		/* New style device tree or ACPI:
+		 *   The XGBE and PHY resources are grouped together with
+		 *   the PHY resources listed last
+		 */
+		phy_memnum = xgbe_resource_count(pdev, IORESOURCE_MEM) - 3;
+		phy_irqnum = xgbe_resource_count(pdev, IORESOURCE_IRQ) - 1;
+	} else {
+		/* Old style device tree:
+		 *   The XGBE and PHY resources are separate
+		 */
+		phy_memnum = 0;
+		phy_irqnum = 0;
+	}
+
 	/* Set and validate the number of descriptors for a ring */
 	BUILD_BUG_ON_NOT_POWER_OF_2(XGBE_TX_DESC_CNT);
 	pdata->tx_desc_count = XGBE_TX_DESC_CNT;
@@ -318,7 +422,8 @@ static int xgbe_probe(struct platform_device *pdev)
 		ret = PTR_ERR(pdata->xgmac_regs);
 		goto err_io;
 	}
-	DBGPR("  xgmac_regs = %p\n", pdata->xgmac_regs);
+	if (netif_msg_probe(pdata))
+		dev_dbg(dev, "xgmac_regs = %p\n", pdata->xgmac_regs);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 	pdata->xpcs_regs = devm_ioremap_resource(dev, res);
@@ -327,7 +432,38 @@ static int xgbe_probe(struct platform_device *pdev)
 		ret = PTR_ERR(pdata->xpcs_regs);
 		goto err_io;
 	}
-	DBGPR("  xpcs_regs  = %p\n", pdata->xpcs_regs);
+	if (netif_msg_probe(pdata))
+		dev_dbg(dev, "xpcs_regs  = %p\n", pdata->xpcs_regs);
+
+	res = platform_get_resource(phy_pdev, IORESOURCE_MEM, phy_memnum++);
+	pdata->rxtx_regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(pdata->rxtx_regs)) {
+		dev_err(dev, "rxtx ioremap failed\n");
+		ret = PTR_ERR(pdata->rxtx_regs);
+		goto err_io;
+	}
+	if (netif_msg_probe(pdata))
+		dev_dbg(dev, "rxtx_regs  = %p\n", pdata->rxtx_regs);
+
+	res = platform_get_resource(phy_pdev, IORESOURCE_MEM, phy_memnum++);
+	pdata->sir0_regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(pdata->sir0_regs)) {
+		dev_err(dev, "sir0 ioremap failed\n");
+		ret = PTR_ERR(pdata->sir0_regs);
+		goto err_io;
+	}
+	if (netif_msg_probe(pdata))
+		dev_dbg(dev, "sir0_regs  = %p\n", pdata->sir0_regs);
+
+	res = platform_get_resource(phy_pdev, IORESOURCE_MEM, phy_memnum++);
+	pdata->sir1_regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(pdata->sir1_regs)) {
+		dev_err(dev, "sir1 ioremap failed\n");
+		ret = PTR_ERR(pdata->sir1_regs);
+		goto err_io;
+	}
+	if (netif_msg_probe(pdata))
+		dev_dbg(dev, "sir1_regs  = %p\n", pdata->sir1_regs);
 
 	/* Retrieve the MAC address */
 	ret = device_property_read_u8_array(dev, XGBE_MAC_ADDR_PROPERTY,
@@ -355,6 +491,115 @@ static int xgbe_probe(struct platform_device *pdev)
 	if (device_property_present(dev, XGBE_DMA_IRQS_PROPERTY))
 		pdata->per_channel_irq = 1;
 
+	/* Retrieve the PHY speedset */
+	ret = device_property_read_u32(phy_dev, XGBE_SPEEDSET_PROPERTY,
+				       &pdata->speed_set);
+	if (ret) {
+		dev_err(dev, "invalid %s property\n", XGBE_SPEEDSET_PROPERTY);
+		goto err_io;
+	}
+
+	switch (pdata->speed_set) {
+	case XGBE_SPEEDSET_1000_10000:
+	case XGBE_SPEEDSET_2500_10000:
+		break;
+	default:
+		dev_err(dev, "invalid %s property\n", XGBE_SPEEDSET_PROPERTY);
+		ret = -EINVAL;
+		goto err_io;
+	}
+
+	/* Retrieve the PHY configuration properties */
+	if (device_property_present(phy_dev, XGBE_BLWC_PROPERTY)) {
+		ret = device_property_read_u32_array(phy_dev,
+						     XGBE_BLWC_PROPERTY,
+						     pdata->serdes_blwc,
+						     XGBE_SPEEDS);
+		if (ret) {
+			dev_err(dev, "invalid %s property\n",
+				XGBE_BLWC_PROPERTY);
+			goto err_io;
+		}
+	} else {
+		memcpy(pdata->serdes_blwc, xgbe_serdes_blwc,
+		       sizeof(pdata->serdes_blwc));
+	}
+
+	if (device_property_present(phy_dev, XGBE_CDR_RATE_PROPERTY)) {
+		ret = device_property_read_u32_array(phy_dev,
+						     XGBE_CDR_RATE_PROPERTY,
+						     pdata->serdes_cdr_rate,
+						     XGBE_SPEEDS);
+		if (ret) {
+			dev_err(dev, "invalid %s property\n",
+				XGBE_CDR_RATE_PROPERTY);
+			goto err_io;
+		}
+	} else {
+		memcpy(pdata->serdes_cdr_rate, xgbe_serdes_cdr_rate,
+		       sizeof(pdata->serdes_cdr_rate));
+	}
+
+	if (device_property_present(phy_dev, XGBE_PQ_SKEW_PROPERTY)) {
+		ret = device_property_read_u32_array(phy_dev,
+						     XGBE_PQ_SKEW_PROPERTY,
+						     pdata->serdes_pq_skew,
+						     XGBE_SPEEDS);
+		if (ret) {
+			dev_err(dev, "invalid %s property\n",
+				XGBE_PQ_SKEW_PROPERTY);
+			goto err_io;
+		}
+	} else {
+		memcpy(pdata->serdes_pq_skew, xgbe_serdes_pq_skew,
+		       sizeof(pdata->serdes_pq_skew));
+	}
+
+	if (device_property_present(phy_dev, XGBE_TX_AMP_PROPERTY)) {
+		ret = device_property_read_u32_array(phy_dev,
+						     XGBE_TX_AMP_PROPERTY,
+						     pdata->serdes_tx_amp,
+						     XGBE_SPEEDS);
+		if (ret) {
+			dev_err(dev, "invalid %s property\n",
+				XGBE_TX_AMP_PROPERTY);
+			goto err_io;
+		}
+	} else {
+		memcpy(pdata->serdes_tx_amp, xgbe_serdes_tx_amp,
+		       sizeof(pdata->serdes_tx_amp));
+	}
+
+	if (device_property_present(phy_dev, XGBE_DFE_CFG_PROPERTY)) {
+		ret = device_property_read_u32_array(phy_dev,
+						     XGBE_DFE_CFG_PROPERTY,
+						     pdata->serdes_dfe_tap_cfg,
+						     XGBE_SPEEDS);
+		if (ret) {
+			dev_err(dev, "invalid %s property\n",
+				XGBE_DFE_CFG_PROPERTY);
+			goto err_io;
+		}
+	} else {
+		memcpy(pdata->serdes_dfe_tap_cfg, xgbe_serdes_dfe_tap_cfg,
+		       sizeof(pdata->serdes_dfe_tap_cfg));
+	}
+
+	if (device_property_present(phy_dev, XGBE_DFE_ENA_PROPERTY)) {
+		ret = device_property_read_u32_array(phy_dev,
+						     XGBE_DFE_ENA_PROPERTY,
+						     pdata->serdes_dfe_tap_ena,
+						     XGBE_SPEEDS);
+		if (ret) {
+			dev_err(dev, "invalid %s property\n",
+				XGBE_DFE_ENA_PROPERTY);
+			goto err_io;
+		}
+	} else {
+		memcpy(pdata->serdes_dfe_tap_ena, xgbe_serdes_dfe_tap_ena,
+		       sizeof(pdata->serdes_dfe_tap_ena));
+	}
+
 	/* Obtain device settings unique to ACPI/OF */
 	if (pdata->use_acpi)
 		ret = xgbe_acpi_support(pdata);
@@ -364,6 +609,7 @@ static int xgbe_probe(struct platform_device *pdev)
 		goto err_io;
 
 	/* Set the DMA coherency values */
+	pdata->coherent = device_dma_is_coherent(pdata->dev);
 	if (pdata->coherent) {
 		pdata->axdomain = XGBE_DMA_OS_AXDOMAIN;
 		pdata->arcache = XGBE_DMA_OS_ARCACHE;
@@ -382,17 +628,23 @@ static int xgbe_probe(struct platform_device *pdev)
 	}
 	pdata->dev_irq = ret;
 
+	/* Get the auto-negotiation interrupt */
+	ret = platform_get_irq(phy_pdev, phy_irqnum++);
+	if (ret < 0) {
+		dev_err(dev, "platform_get_irq phy 0 failed\n");
+		goto err_io;
+	}
+	pdata->an_irq = ret;
+
 	netdev->irq = pdata->dev_irq;
 	netdev->base_addr = (unsigned long)pdata->xgmac_regs;
 	memcpy(netdev->dev_addr, pdata->mac_addr, netdev->addr_len);
 
 	/* Set all the function pointers */
 	xgbe_init_all_fptrs(pdata);
-	hw_if = &pdata->hw_if;
-	desc_if = &pdata->desc_if;
 
 	/* Issue software reset to device */
-	hw_if->exit(pdata);
+	pdata->hw_if.exit(pdata);
 
 	/* Populate the hardware features */
 	xgbe_get_all_hw_features(pdata);
@@ -401,8 +653,6 @@ static int xgbe_probe(struct platform_device *pdev)
 	xgbe_default_config(pdata);
 
 	/* Set the DMA mask */
-	if (!dev->dma_mask)
-		dev->dma_mask = &dev->coherent_dma_mask;
 	ret = dma_set_mask_and_coherent(dev,
 					DMA_BIT_MASK(pdata->hw_feat.dma_width));
 	if (ret) {
@@ -447,16 +697,8 @@ static int xgbe_probe(struct platform_device *pdev)
 	XGMAC_SET_BITS(pdata->rss_options, MAC_RSSCR, TCP4TE, 1);
 	XGMAC_SET_BITS(pdata->rss_options, MAC_RSSCR, UDP4TE, 1);
 
-	/* Prepare to regsiter with MDIO */
-	pdata->mii_bus_id = kasprintf(GFP_KERNEL, "%s", pdev->name);
-	if (!pdata->mii_bus_id) {
-		dev_err(dev, "failed to allocate mii bus id\n");
-		ret = -ENOMEM;
-		goto err_io;
-	}
-	ret = xgbe_mdio_register(pdata);
-	if (ret)
-		goto err_bus_id;
+	/* Call MDIO/PHY initialization routine */
+	pdata->phy_if.phy_init(pdata);
 
 	/* Set device operations */
 	netdev->netdev_ops = xgbe_get_netdev_ops();
@@ -501,26 +743,52 @@ static int xgbe_probe(struct platform_device *pdev)
 	ret = register_netdev(netdev);
 	if (ret) {
 		dev_err(dev, "net device registration failed\n");
-		goto err_reg_netdev;
+		goto err_io;
+	}
+
+	/* Create the PHY/ANEG name based on netdev name */
+	snprintf(pdata->an_name, sizeof(pdata->an_name) - 1, "%s-pcs",
+		 netdev_name(netdev));
+
+	/* Create workqueues */
+	pdata->dev_workqueue =
+		create_singlethread_workqueue(netdev_name(netdev));
+	if (!pdata->dev_workqueue) {
+		netdev_err(netdev, "device workqueue creation failed\n");
+		ret = -ENOMEM;
+		goto err_netdev;
+	}
+
+	pdata->an_workqueue =
+		create_singlethread_workqueue(pdata->an_name);
+	if (!pdata->an_workqueue) {
+		netdev_err(netdev, "phy workqueue creation failed\n");
+		ret = -ENOMEM;
+		goto err_wq;
 	}
 
 	xgbe_ptp_register(pdata);
 
 	xgbe_debugfs_init(pdata);
 
+	platform_device_put(phy_pdev);
+
 	netdev_notice(netdev, "net device enabled\n");
 
 	DBGPR("<-- xgbe_probe\n");
 
 	return 0;
 
-err_reg_netdev:
-	xgbe_mdio_unregister(pdata);
+err_wq:
+	destroy_workqueue(pdata->dev_workqueue);
 
-err_bus_id:
-	kfree(pdata->mii_bus_id);
+err_netdev:
+	unregister_netdev(netdev);
 
 err_io:
+	platform_device_put(phy_pdev);
+
+err_phydev:
 	free_netdev(netdev);
 
 err_alloc:
@@ -540,11 +808,13 @@ static int xgbe_remove(struct platform_device *pdev)
 
 	xgbe_ptp_unregister(pdata);
 
-	unregister_netdev(netdev);
+	flush_workqueue(pdata->an_workqueue);
+	destroy_workqueue(pdata->an_workqueue);
 
-	xgbe_mdio_unregister(pdata);
+	flush_workqueue(pdata->dev_workqueue);
+	destroy_workqueue(pdata->dev_workqueue);
 
-	kfree(pdata->mii_bus_id);
+	unregister_netdev(netdev);
 
 	free_netdev(netdev);
 
@@ -557,16 +827,17 @@ static int xgbe_remove(struct platform_device *pdev)
 static int xgbe_suspend(struct device *dev)
 {
 	struct net_device *netdev = dev_get_drvdata(dev);
-	int ret;
+	struct xgbe_prv_data *pdata = netdev_priv(netdev);
+	int ret = 0;
 
 	DBGPR("-->xgbe_suspend\n");
 
-	if (!netif_running(netdev)) {
-		DBGPR("<--xgbe_dev_suspend\n");
-		return -EINVAL;
-	}
+	if (netif_running(netdev))
+		ret = xgbe_powerdown(netdev, XGMAC_DRIVER_CONTEXT);
 
-	ret = xgbe_powerdown(netdev, XGMAC_DRIVER_CONTEXT);
+	pdata->lpm_ctrl = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1);
+	pdata->lpm_ctrl |= MDIO_CTRL1_LPOWER;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, pdata->lpm_ctrl);
 
 	DBGPR("<--xgbe_suspend\n");
 
@@ -576,16 +847,16 @@ static int xgbe_suspend(struct device *dev)
 static int xgbe_resume(struct device *dev)
 {
 	struct net_device *netdev = dev_get_drvdata(dev);
-	int ret;
+	struct xgbe_prv_data *pdata = netdev_priv(netdev);
+	int ret = 0;
 
 	DBGPR("-->xgbe_resume\n");
 
-	if (!netif_running(netdev)) {
-		DBGPR("<--xgbe_dev_resume\n");
-		return -EINVAL;
-	}
+	pdata->lpm_ctrl &= ~MDIO_CTRL1_LPOWER;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, pdata->lpm_ctrl);
 
-	ret = xgbe_powerup(netdev, XGMAC_DRIVER_CONTEXT);
+	if (netif_running(netdev))
+		ret = xgbe_powerup(netdev, XGMAC_DRIVER_CONTEXT);
 
 	DBGPR("<--xgbe_resume\n");
 
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c b/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
index 59e267f3f1b7..9088c3a35a20 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
@@ -119,194 +119,1246 @@
 #include <linux/mdio.h>
 #include <linux/phy.h>
 #include <linux/of.h>
+#include <linux/bitops.h>
+#include <linux/jiffies.h>
 
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-static int xgbe_mdio_read(struct mii_bus *mii, int prtad, int mmd_reg)
+static void xgbe_an_enable_kr_training(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_prv_data *pdata = mii->priv;
-	struct xgbe_hw_if *hw_if = &pdata->hw_if;
-	int mmd_data;
+	unsigned int reg;
 
-	DBGPR_MDIO("-->xgbe_mdio_read: prtad=%#x mmd_reg=%#x\n",
-		   prtad, mmd_reg);
+	reg = XMDIO_READ(pdata, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL);
 
-	mmd_data = hw_if->read_mmd_regs(pdata, prtad, mmd_reg);
+	reg |= XGBE_KR_TRAINING_ENABLE;
+	XMDIO_WRITE(pdata, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL, reg);
+}
+
+static void xgbe_an_disable_kr_training(struct xgbe_prv_data *pdata)
+{
+	unsigned int reg;
 
-	DBGPR_MDIO("<--xgbe_mdio_read: mmd_data=%#x\n", mmd_data);
+	reg = XMDIO_READ(pdata, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL);
 
-	return mmd_data;
+	reg &= ~XGBE_KR_TRAINING_ENABLE;
+	XMDIO_WRITE(pdata, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL, reg);
 }
 
-static int xgbe_mdio_write(struct mii_bus *mii, int prtad, int mmd_reg,
-			   u16 mmd_val)
+static void xgbe_pcs_power_cycle(struct xgbe_prv_data *pdata)
 {
-	struct xgbe_prv_data *pdata = mii->priv;
-	struct xgbe_hw_if *hw_if = &pdata->hw_if;
-	int mmd_data = mmd_val;
+	unsigned int reg;
 
-	DBGPR_MDIO("-->xgbe_mdio_write: prtad=%#x mmd_reg=%#x mmd_data=%#x\n",
-		   prtad, mmd_reg, mmd_data);
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1);
 
-	hw_if->write_mmd_regs(pdata, prtad, mmd_reg, mmd_data);
+	reg |= MDIO_CTRL1_LPOWER;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, reg);
 
-	DBGPR_MDIO("<--xgbe_mdio_write\n");
+	usleep_range(75, 100);
 
-	return 0;
+	reg &= ~MDIO_CTRL1_LPOWER;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, reg);
 }
 
-void xgbe_dump_phy_registers(struct xgbe_prv_data *pdata)
+static void xgbe_serdes_start_ratechange(struct xgbe_prv_data *pdata)
 {
-	struct device *dev = pdata->dev;
-	struct phy_device *phydev = pdata->mii->phy_map[XGBE_PRTAD];
-	int i;
-
-	dev_alert(dev, "\n************* PHY Reg dump **********************\n");
-
-	dev_alert(dev, "PCS Control Reg (%#04x) = %#04x\n", MDIO_CTRL1,
-		  XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1));
-	dev_alert(dev, "PCS Status Reg (%#04x) = %#04x\n", MDIO_STAT1,
-		  XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_STAT1));
-	dev_alert(dev, "Phy Id (PHYS ID 1 %#04x)= %#04x\n", MDIO_DEVID1,
-		  XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_DEVID1));
-	dev_alert(dev, "Phy Id (PHYS ID 2 %#04x)= %#04x\n", MDIO_DEVID2,
-		  XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_DEVID2));
-	dev_alert(dev, "Devices in Package (%#04x)= %#04x\n", MDIO_DEVS1,
-		  XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_DEVS1));
-	dev_alert(dev, "Devices in Package (%#04x)= %#04x\n", MDIO_DEVS2,
-		  XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_DEVS2));
-
-	dev_alert(dev, "Auto-Neg Control Reg (%#04x) = %#04x\n", MDIO_CTRL1,
-		  XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_CTRL1));
-	dev_alert(dev, "Auto-Neg Status Reg (%#04x) = %#04x\n", MDIO_STAT1,
-		  XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_STAT1));
-	dev_alert(dev, "Auto-Neg Ad Reg 1 (%#04x) = %#04x\n",
-		  MDIO_AN_ADVERTISE,
-		  XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE));
-	dev_alert(dev, "Auto-Neg Ad Reg 2 (%#04x) = %#04x\n",
-		  MDIO_AN_ADVERTISE + 1,
-		  XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 1));
-	dev_alert(dev, "Auto-Neg Ad Reg 3 (%#04x) = %#04x\n",
-		  MDIO_AN_ADVERTISE + 2,
-		  XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2));
-	dev_alert(dev, "Auto-Neg Completion Reg (%#04x) = %#04x\n",
-		  MDIO_AN_COMP_STAT,
-		  XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_COMP_STAT));
-
-	dev_alert(dev, "MMD Device Mask = %#x\n",
-		  phydev->c45_ids.devices_in_package);
-	for (i = 0; i < ARRAY_SIZE(phydev->c45_ids.device_ids); i++)
-		dev_alert(dev, "  MMD %d: ID = %#08x\n", i,
-			  phydev->c45_ids.device_ids[i]);
-
-	dev_alert(dev, "\n*************************************************\n");
-}
-
-int xgbe_mdio_register(struct xgbe_prv_data *pdata)
-{
-	struct mii_bus *mii;
-	struct phy_device *phydev;
-	int ret = 0;
-
-	DBGPR("-->xgbe_mdio_register\n");
-
-	mii = mdiobus_alloc();
-	if (!mii) {
-		dev_err(pdata->dev, "mdiobus_alloc failed\n");
-		return -ENOMEM;
+	/* Assert Rx and Tx ratechange */
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, RATECHANGE, 1);
+}
+
+static void xgbe_serdes_complete_ratechange(struct xgbe_prv_data *pdata)
+{
+	unsigned int wait;
+	u16 status;
+
+	/* Release Rx and Tx ratechange */
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, RATECHANGE, 0);
+
+	/* Wait for Rx and Tx ready */
+	wait = XGBE_RATECHANGE_COUNT;
+	while (wait--) {
+		usleep_range(50, 75);
+
+		status = XSIR0_IOREAD(pdata, SIR0_STATUS);
+		if (XSIR_GET_BITS(status, SIR0_STATUS, RX_READY) &&
+		    XSIR_GET_BITS(status, SIR0_STATUS, TX_READY))
+			goto rx_reset;
 	}
 
-	/* Register on the MDIO bus (don't probe any PHYs) */
-	mii->name = XGBE_PHY_NAME;
-	mii->read = xgbe_mdio_read;
-	mii->write = xgbe_mdio_write;
-	snprintf(mii->id, sizeof(mii->id), "%s", pdata->mii_bus_id);
-	mii->priv = pdata;
-	mii->phy_mask = ~0;
-	mii->parent = pdata->dev;
-	ret = mdiobus_register(mii);
-	if (ret) {
-		dev_err(pdata->dev, "mdiobus_register failed\n");
-		goto err_mdiobus_alloc;
+	netif_dbg(pdata, link, pdata->netdev, "SerDes rx/tx not ready (%#hx)\n",
+		  status);
+
+rx_reset:
+	/* Perform Rx reset for the DFE changes */
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG6, RESETB_RXD, 0);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG6, RESETB_RXD, 1);
+}
+
+static void xgbe_xgmii_mode(struct xgbe_prv_data *pdata)
+{
+	unsigned int reg;
+
+	/* Enable KR training */
+	xgbe_an_enable_kr_training(pdata);
+
+	/* Set MAC to 10G speed */
+	pdata->hw_if.set_xgmii_speed(pdata);
+
+	/* Set PCS to KR/10G speed */
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL2);
+	reg &= ~MDIO_PCS_CTRL2_TYPE;
+	reg |= MDIO_PCS_CTRL2_10GBR;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL2, reg);
+
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1);
+	reg &= ~MDIO_CTRL1_SPEEDSEL;
+	reg |= MDIO_CTRL1_SPEED10G;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, reg);
+
+	xgbe_pcs_power_cycle(pdata);
+
+	/* Set SerDes to 10G speed */
+	xgbe_serdes_start_ratechange(pdata);
+
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, DATARATE, XGBE_SPEED_10000_RATE);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, WORDMODE, XGBE_SPEED_10000_WORD);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, PLLSEL, XGBE_SPEED_10000_PLL);
+
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, CDR_RATE,
+			   pdata->serdes_cdr_rate[XGBE_SPEED_10000]);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, TXAMP,
+			   pdata->serdes_tx_amp[XGBE_SPEED_10000]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG20, BLWC_ENA,
+			   pdata->serdes_blwc[XGBE_SPEED_10000]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG114, PQ_REG,
+			   pdata->serdes_pq_skew[XGBE_SPEED_10000]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG129, RXDFE_CONFIG,
+			   pdata->serdes_dfe_tap_cfg[XGBE_SPEED_10000]);
+	XRXTX_IOWRITE(pdata, RXTX_REG22,
+		      pdata->serdes_dfe_tap_ena[XGBE_SPEED_10000]);
+
+	xgbe_serdes_complete_ratechange(pdata);
+
+	netif_dbg(pdata, link, pdata->netdev, "10GbE KR mode set\n");
+}
+
+static void xgbe_gmii_2500_mode(struct xgbe_prv_data *pdata)
+{
+	unsigned int reg;
+
+	/* Disable KR training */
+	xgbe_an_disable_kr_training(pdata);
+
+	/* Set MAC to 2.5G speed */
+	pdata->hw_if.set_gmii_2500_speed(pdata);
+
+	/* Set PCS to KX/1G speed */
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL2);
+	reg &= ~MDIO_PCS_CTRL2_TYPE;
+	reg |= MDIO_PCS_CTRL2_10GBX;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL2, reg);
+
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1);
+	reg &= ~MDIO_CTRL1_SPEEDSEL;
+	reg |= MDIO_CTRL1_SPEED1G;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, reg);
+
+	xgbe_pcs_power_cycle(pdata);
+
+	/* Set SerDes to 2.5G speed */
+	xgbe_serdes_start_ratechange(pdata);
+
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, DATARATE, XGBE_SPEED_2500_RATE);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, WORDMODE, XGBE_SPEED_2500_WORD);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, PLLSEL, XGBE_SPEED_2500_PLL);
+
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, CDR_RATE,
+			   pdata->serdes_cdr_rate[XGBE_SPEED_2500]);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, TXAMP,
+			   pdata->serdes_tx_amp[XGBE_SPEED_2500]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG20, BLWC_ENA,
+			   pdata->serdes_blwc[XGBE_SPEED_2500]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG114, PQ_REG,
+			   pdata->serdes_pq_skew[XGBE_SPEED_2500]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG129, RXDFE_CONFIG,
+			   pdata->serdes_dfe_tap_cfg[XGBE_SPEED_2500]);
+	XRXTX_IOWRITE(pdata, RXTX_REG22,
+		      pdata->serdes_dfe_tap_ena[XGBE_SPEED_2500]);
+
+	xgbe_serdes_complete_ratechange(pdata);
+
+	netif_dbg(pdata, link, pdata->netdev, "2.5GbE KX mode set\n");
+}
+
+static void xgbe_gmii_mode(struct xgbe_prv_data *pdata)
+{
+	unsigned int reg;
+
+	/* Disable KR training */
+	xgbe_an_disable_kr_training(pdata);
+
+	/* Set MAC to 1G speed */
+	pdata->hw_if.set_gmii_speed(pdata);
+
+	/* Set PCS to KX/1G speed */
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL2);
+	reg &= ~MDIO_PCS_CTRL2_TYPE;
+	reg |= MDIO_PCS_CTRL2_10GBX;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL2, reg);
+
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1);
+	reg &= ~MDIO_CTRL1_SPEEDSEL;
+	reg |= MDIO_CTRL1_SPEED1G;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, reg);
+
+	xgbe_pcs_power_cycle(pdata);
+
+	/* Set SerDes to 1G speed */
+	xgbe_serdes_start_ratechange(pdata);
+
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, DATARATE, XGBE_SPEED_1000_RATE);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, WORDMODE, XGBE_SPEED_1000_WORD);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, PLLSEL, XGBE_SPEED_1000_PLL);
+
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, CDR_RATE,
+			   pdata->serdes_cdr_rate[XGBE_SPEED_1000]);
+	XSIR1_IOWRITE_BITS(pdata, SIR1_SPEED, TXAMP,
+			   pdata->serdes_tx_amp[XGBE_SPEED_1000]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG20, BLWC_ENA,
+			   pdata->serdes_blwc[XGBE_SPEED_1000]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG114, PQ_REG,
+			   pdata->serdes_pq_skew[XGBE_SPEED_1000]);
+	XRXTX_IOWRITE_BITS(pdata, RXTX_REG129, RXDFE_CONFIG,
+			   pdata->serdes_dfe_tap_cfg[XGBE_SPEED_1000]);
+	XRXTX_IOWRITE(pdata, RXTX_REG22,
+		      pdata->serdes_dfe_tap_ena[XGBE_SPEED_1000]);
+
+	xgbe_serdes_complete_ratechange(pdata);
+
+	netif_dbg(pdata, link, pdata->netdev, "1GbE KX mode set\n");
+}
+
+static void xgbe_cur_mode(struct xgbe_prv_data *pdata,
+			  enum xgbe_mode *mode)
+{
+	unsigned int reg;
+
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL2);
+	if ((reg & MDIO_PCS_CTRL2_TYPE) == MDIO_PCS_CTRL2_10GBR)
+		*mode = XGBE_MODE_KR;
+	else
+		*mode = XGBE_MODE_KX;
+}
+
+static bool xgbe_in_kr_mode(struct xgbe_prv_data *pdata)
+{
+	enum xgbe_mode mode;
+
+	xgbe_cur_mode(pdata, &mode);
+
+	return (mode == XGBE_MODE_KR);
+}
+
+static void xgbe_switch_mode(struct xgbe_prv_data *pdata)
+{
+	/* If we are in KR switch to KX, and vice-versa */
+	if (xgbe_in_kr_mode(pdata)) {
+		if (pdata->speed_set == XGBE_SPEEDSET_1000_10000)
+			xgbe_gmii_mode(pdata);
+		else
+			xgbe_gmii_2500_mode(pdata);
+	} else {
+		xgbe_xgmii_mode(pdata);
 	}
-	DBGPR("  mdiobus_register succeeded for %s\n", pdata->mii_bus_id);
-
-	/* Probe the PCS using Clause 45 */
-	phydev = get_phy_device(mii, XGBE_PRTAD, true);
-	if (IS_ERR(phydev) || !phydev ||
-	    !phydev->c45_ids.device_ids[MDIO_MMD_PCS]) {
-		dev_err(pdata->dev, "get_phy_device failed\n");
-		ret = phydev ? PTR_ERR(phydev) : -ENOLINK;
-		goto err_mdiobus_register;
+}
+
+static void xgbe_set_mode(struct xgbe_prv_data *pdata,
+			  enum xgbe_mode mode)
+{
+	enum xgbe_mode cur_mode;
+
+	xgbe_cur_mode(pdata, &cur_mode);
+	if (mode != cur_mode)
+		xgbe_switch_mode(pdata);
+}
+
+static bool xgbe_use_xgmii_mode(struct xgbe_prv_data *pdata)
+{
+	if (pdata->phy.autoneg == AUTONEG_ENABLE) {
+		if (pdata->phy.advertising & ADVERTISED_10000baseKR_Full)
+			return true;
+	} else {
+		if (pdata->phy.speed == SPEED_10000)
+			return true;
 	}
-	request_module(MDIO_MODULE_PREFIX MDIO_ID_FMT,
-		       MDIO_ID_ARGS(phydev->c45_ids.device_ids[MDIO_MMD_PCS]));
 
-	ret = phy_device_register(phydev);
-	if (ret) {
-		dev_err(pdata->dev, "phy_device_register failed\n");
-		goto err_phy_device;
+	return false;
+}
+
+static bool xgbe_use_gmii_2500_mode(struct xgbe_prv_data *pdata)
+{
+	if (pdata->phy.autoneg == AUTONEG_ENABLE) {
+		if (pdata->phy.advertising & ADVERTISED_2500baseX_Full)
+			return true;
+	} else {
+		if (pdata->phy.speed == SPEED_2500)
+			return true;
 	}
-	if (!phydev->dev.driver) {
-		dev_err(pdata->dev, "phy driver probe failed\n");
-		ret = -EIO;
-		goto err_phy_device;
+
+	return false;
+}
+
+static bool xgbe_use_gmii_mode(struct xgbe_prv_data *pdata)
+{
+	if (pdata->phy.autoneg == AUTONEG_ENABLE) {
+		if (pdata->phy.advertising & ADVERTISED_1000baseKX_Full)
+			return true;
+	} else {
+		if (pdata->phy.speed == SPEED_1000)
+			return true;
 	}
 
-	/* Add a reference to the PHY driver so it can't be unloaded */
-	pdata->phy_module = phydev->dev.driver->owner;
-	if (!try_module_get(pdata->phy_module)) {
-		dev_err(pdata->dev, "try_module_get failed\n");
-		ret = -EIO;
-		goto err_phy_device;
+	return false;
+}
+
+static void xgbe_set_an(struct xgbe_prv_data *pdata, bool enable, bool restart)
+{
+	unsigned int reg;
+
+	reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_CTRL1);
+	reg &= ~MDIO_AN_CTRL1_ENABLE;
+
+	if (enable)
+		reg |= MDIO_AN_CTRL1_ENABLE;
+
+	if (restart)
+		reg |= MDIO_AN_CTRL1_RESTART;
+
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_CTRL1, reg);
+}
+
+static void xgbe_restart_an(struct xgbe_prv_data *pdata)
+{
+	xgbe_set_an(pdata, true, true);
+
+	netif_dbg(pdata, link, pdata->netdev, "AN enabled/restarted\n");
+}
+
+static void xgbe_disable_an(struct xgbe_prv_data *pdata)
+{
+	xgbe_set_an(pdata, false, false);
+
+	netif_dbg(pdata, link, pdata->netdev, "AN disabled\n");
+}
+
+static enum xgbe_an xgbe_an_tx_training(struct xgbe_prv_data *pdata,
+					enum xgbe_rx *state)
+{
+	unsigned int ad_reg, lp_reg, reg;
+
+	*state = XGBE_RX_COMPLETE;
+
+	/* If we're not in KR mode then we're done */
+	if (!xgbe_in_kr_mode(pdata))
+		return XGBE_AN_PAGE_RECEIVED;
+
+	/* Enable/Disable FEC */
+	ad_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2);
+	lp_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_LPA + 2);
+
+	reg = XMDIO_READ(pdata, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_FECCTRL);
+	reg &= ~(MDIO_PMA_10GBR_FECABLE_ABLE | MDIO_PMA_10GBR_FECABLE_ERRABLE);
+	if ((ad_reg & 0xc000) && (lp_reg & 0xc000))
+		reg |= pdata->fec_ability;
+
+	XMDIO_WRITE(pdata, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_FECCTRL, reg);
+
+	/* Start KR training */
+	reg = XMDIO_READ(pdata, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL);
+	if (reg & XGBE_KR_TRAINING_ENABLE) {
+		XSIR0_IOWRITE_BITS(pdata, SIR0_KR_RT_1, RESET, 1);
+
+		reg |= XGBE_KR_TRAINING_START;
+		XMDIO_WRITE(pdata, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL,
+			    reg);
+
+		XSIR0_IOWRITE_BITS(pdata, SIR0_KR_RT_1, RESET, 0);
+
+		netif_dbg(pdata, link, pdata->netdev,
+			  "KR training initiated\n");
 	}
 
-	pdata->mii = mii;
-	pdata->mdio_mmd = MDIO_MMD_PCS;
+	return XGBE_AN_PAGE_RECEIVED;
+}
+
+static enum xgbe_an xgbe_an_tx_xnp(struct xgbe_prv_data *pdata,
+				   enum xgbe_rx *state)
+{
+	u16 msg;
+
+	*state = XGBE_RX_XNP;
+
+	msg = XGBE_XNP_MCF_NULL_MESSAGE;
+	msg |= XGBE_XNP_MP_FORMATTED;
+
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_XNP + 2, 0);
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_XNP + 1, 0);
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_XNP, msg);
+
+	return XGBE_AN_PAGE_RECEIVED;
+}
+
+static enum xgbe_an xgbe_an_rx_bpa(struct xgbe_prv_data *pdata,
+				   enum xgbe_rx *state)
+{
+	unsigned int link_support;
+	unsigned int reg, ad_reg, lp_reg;
+
+	/* Read Base Ability register 2 first */
+	reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_LPA + 1);
+
+	/* Check for a supported mode, otherwise restart in a different one */
+	link_support = xgbe_in_kr_mode(pdata) ? 0x80 : 0x20;
+	if (!(reg & link_support))
+		return XGBE_AN_INCOMPAT_LINK;
+
+	/* Check Extended Next Page support */
+	ad_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE);
+	lp_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_LPA);
+
+	return ((ad_reg & XGBE_XNP_NP_EXCHANGE) ||
+		(lp_reg & XGBE_XNP_NP_EXCHANGE))
+	       ? xgbe_an_tx_xnp(pdata, state)
+	       : xgbe_an_tx_training(pdata, state);
+}
+
+static enum xgbe_an xgbe_an_rx_xnp(struct xgbe_prv_data *pdata,
+				   enum xgbe_rx *state)
+{
+	unsigned int ad_reg, lp_reg;
+
+	/* Check Extended Next Page support */
+	ad_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_XNP);
+	lp_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_LPX);
+
+	return ((ad_reg & XGBE_XNP_NP_EXCHANGE) ||
+		(lp_reg & XGBE_XNP_NP_EXCHANGE))
+	       ? xgbe_an_tx_xnp(pdata, state)
+	       : xgbe_an_tx_training(pdata, state);
+}
+
+static enum xgbe_an xgbe_an_page_received(struct xgbe_prv_data *pdata)
+{
+	enum xgbe_rx *state;
+	unsigned long an_timeout;
+	enum xgbe_an ret;
+
+	if (!pdata->an_start) {
+		pdata->an_start = jiffies;
+	} else {
+		an_timeout = pdata->an_start +
+			     msecs_to_jiffies(XGBE_AN_MS_TIMEOUT);
+		if (time_after(jiffies, an_timeout)) {
+			/* Auto-negotiation timed out, reset state */
+			pdata->kr_state = XGBE_RX_BPA;
+			pdata->kx_state = XGBE_RX_BPA;
+
+			pdata->an_start = jiffies;
+
+			netif_dbg(pdata, link, pdata->netdev,
+				  "AN timed out, resetting state\n");
+		}
+	}
 
-	phydev->autoneg = pdata->default_autoneg;
-	if (phydev->autoneg == AUTONEG_DISABLE) {
-		phydev->speed = pdata->default_speed;
-		phydev->duplex = DUPLEX_FULL;
+	state = xgbe_in_kr_mode(pdata) ? &pdata->kr_state
+					   : &pdata->kx_state;
 
-		phydev->advertising &= ~ADVERTISED_Autoneg;
+	switch (*state) {
+	case XGBE_RX_BPA:
+		ret = xgbe_an_rx_bpa(pdata, state);
+		break;
+
+	case XGBE_RX_XNP:
+		ret = xgbe_an_rx_xnp(pdata, state);
+		break;
+
+	default:
+		ret = XGBE_AN_ERROR;
+	}
+
+	return ret;
+}
+
+static enum xgbe_an xgbe_an_incompat_link(struct xgbe_prv_data *pdata)
+{
+	/* Be sure we aren't looping trying to negotiate */
+	if (xgbe_in_kr_mode(pdata)) {
+		pdata->kr_state = XGBE_RX_ERROR;
+
+		if (!(pdata->phy.advertising & ADVERTISED_1000baseKX_Full) &&
+		    !(pdata->phy.advertising & ADVERTISED_2500baseX_Full))
+			return XGBE_AN_NO_LINK;
+
+		if (pdata->kx_state != XGBE_RX_BPA)
+			return XGBE_AN_NO_LINK;
+	} else {
+		pdata->kx_state = XGBE_RX_ERROR;
+
+		if (!(pdata->phy.advertising & ADVERTISED_10000baseKR_Full))
+			return XGBE_AN_NO_LINK;
+
+		if (pdata->kr_state != XGBE_RX_BPA)
+			return XGBE_AN_NO_LINK;
+	}
+
+	xgbe_disable_an(pdata);
+
+	xgbe_switch_mode(pdata);
+
+	xgbe_restart_an(pdata);
+
+	return XGBE_AN_INCOMPAT_LINK;
+}
+
+static irqreturn_t xgbe_an_isr(int irq, void *data)
+{
+	struct xgbe_prv_data *pdata = (struct xgbe_prv_data *)data;
+
+	netif_dbg(pdata, intr, pdata->netdev, "AN interrupt received\n");
+
+	/* Interrupt reason must be read and cleared outside of IRQ context */
+	disable_irq_nosync(pdata->an_irq);
+
+	queue_work(pdata->an_workqueue, &pdata->an_irq_work);
+
+	return IRQ_HANDLED;
+}
+
+static void xgbe_an_irq_work(struct work_struct *work)
+{
+	struct xgbe_prv_data *pdata = container_of(work,
+						   struct xgbe_prv_data,
+						   an_irq_work);
+
+	/* Avoid a race between enabling the IRQ and exiting the work by
+	 * waiting for the work to finish and then queueing it
+	 */
+	flush_work(&pdata->an_work);
+	queue_work(pdata->an_workqueue, &pdata->an_work);
+}
+
+static const char *xgbe_state_as_string(enum xgbe_an state)
+{
+	switch (state) {
+	case XGBE_AN_READY:
+		return "Ready";
+	case XGBE_AN_PAGE_RECEIVED:
+		return "Page-Received";
+	case XGBE_AN_INCOMPAT_LINK:
+		return "Incompatible-Link";
+	case XGBE_AN_COMPLETE:
+		return "Complete";
+	case XGBE_AN_NO_LINK:
+		return "No-Link";
+	case XGBE_AN_ERROR:
+		return "Error";
+	default:
+		return "Undefined";
+	}
+}
+
+static void xgbe_an_state_machine(struct work_struct *work)
+{
+	struct xgbe_prv_data *pdata = container_of(work,
+						   struct xgbe_prv_data,
+						   an_work);
+	enum xgbe_an cur_state = pdata->an_state;
+	unsigned int int_reg, int_mask;
+
+	mutex_lock(&pdata->an_mutex);
+
+	/* Read the interrupt */
+	int_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_INT);
+	if (!int_reg)
+		goto out;
+
+next_int:
+	if (int_reg & XGBE_AN_PG_RCV) {
+		pdata->an_state = XGBE_AN_PAGE_RECEIVED;
+		int_mask = XGBE_AN_PG_RCV;
+	} else if (int_reg & XGBE_AN_INC_LINK) {
+		pdata->an_state = XGBE_AN_INCOMPAT_LINK;
+		int_mask = XGBE_AN_INC_LINK;
+	} else if (int_reg & XGBE_AN_INT_CMPLT) {
+		pdata->an_state = XGBE_AN_COMPLETE;
+		int_mask = XGBE_AN_INT_CMPLT;
+	} else {
+		pdata->an_state = XGBE_AN_ERROR;
+		int_mask = 0;
 	}
 
-	pdata->phydev = phydev;
+	/* Clear the interrupt to be processed */
+	int_reg &= ~int_mask;
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_INT, int_reg);
+
+	pdata->an_result = pdata->an_state;
+
+again:
+	netif_dbg(pdata, link, pdata->netdev, "AN %s\n",
+		  xgbe_state_as_string(pdata->an_state));
+
+	cur_state = pdata->an_state;
+
+	switch (pdata->an_state) {
+	case XGBE_AN_READY:
+		pdata->an_supported = 0;
+		break;
+
+	case XGBE_AN_PAGE_RECEIVED:
+		pdata->an_state = xgbe_an_page_received(pdata);
+		pdata->an_supported++;
+		break;
+
+	case XGBE_AN_INCOMPAT_LINK:
+		pdata->an_supported = 0;
+		pdata->parallel_detect = 0;
+		pdata->an_state = xgbe_an_incompat_link(pdata);
+		break;
 
-	DBGPHY_REGS(pdata);
+	case XGBE_AN_COMPLETE:
+		pdata->parallel_detect = pdata->an_supported ? 0 : 1;
+		netif_dbg(pdata, link, pdata->netdev, "%s successful\n",
+			  pdata->an_supported ? "Auto negotiation"
+					      : "Parallel detection");
+		break;
 
-	DBGPR("<--xgbe_mdio_register\n");
+	case XGBE_AN_NO_LINK:
+		break;
+
+	default:
+		pdata->an_state = XGBE_AN_ERROR;
+	}
+
+	if (pdata->an_state == XGBE_AN_NO_LINK) {
+		int_reg = 0;
+		XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_INT, 0);
+	} else if (pdata->an_state == XGBE_AN_ERROR) {
+		netdev_err(pdata->netdev,
+			   "error during auto-negotiation, state=%u\n",
+			   cur_state);
+
+		int_reg = 0;
+		XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_INT, 0);
+	}
+
+	if (pdata->an_state >= XGBE_AN_COMPLETE) {
+		pdata->an_result = pdata->an_state;
+		pdata->an_state = XGBE_AN_READY;
+		pdata->kr_state = XGBE_RX_BPA;
+		pdata->kx_state = XGBE_RX_BPA;
+		pdata->an_start = 0;
+
+		netif_dbg(pdata, link, pdata->netdev, "AN result: %s\n",
+			  xgbe_state_as_string(pdata->an_result));
+	}
+
+	if (cur_state != pdata->an_state)
+		goto again;
+
+	if (int_reg)
+		goto next_int;
+
+out:
+	enable_irq(pdata->an_irq);
+
+	mutex_unlock(&pdata->an_mutex);
+}
+
+static void xgbe_an_init(struct xgbe_prv_data *pdata)
+{
+	unsigned int reg;
+
+	/* Set up Advertisement register 3 first */
+	reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2);
+	if (pdata->phy.advertising & ADVERTISED_10000baseR_FEC)
+		reg |= 0xc000;
+	else
+		reg &= ~0xc000;
+
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2, reg);
+
+	/* Set up Advertisement register 2 next */
+	reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 1);
+	if (pdata->phy.advertising & ADVERTISED_10000baseKR_Full)
+		reg |= 0x80;
+	else
+		reg &= ~0x80;
+
+	if ((pdata->phy.advertising & ADVERTISED_1000baseKX_Full) ||
+	    (pdata->phy.advertising & ADVERTISED_2500baseX_Full))
+		reg |= 0x20;
+	else
+		reg &= ~0x20;
+
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 1, reg);
+
+	/* Set up Advertisement register 1 last */
+	reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE);
+	if (pdata->phy.advertising & ADVERTISED_Pause)
+		reg |= 0x400;
+	else
+		reg &= ~0x400;
+
+	if (pdata->phy.advertising & ADVERTISED_Asym_Pause)
+		reg |= 0x800;
+	else
+		reg &= ~0x800;
+
+	/* We don't intend to perform XNP */
+	reg &= ~XGBE_XNP_NP_EXCHANGE;
+
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE, reg);
+
+	netif_dbg(pdata, link, pdata->netdev, "AN initialized\n");
+}
+
+static const char *xgbe_phy_fc_string(struct xgbe_prv_data *pdata)
+{
+	if (pdata->tx_pause && pdata->rx_pause)
+		return "rx/tx";
+	else if (pdata->rx_pause)
+		return "rx";
+	else if (pdata->tx_pause)
+		return "tx";
+	else
+		return "off";
+}
+
+static const char *xgbe_phy_speed_string(int speed)
+{
+	switch (speed) {
+	case SPEED_1000:
+		return "1Gbps";
+	case SPEED_2500:
+		return "2.5Gbps";
+	case SPEED_10000:
+		return "10Gbps";
+	case SPEED_UNKNOWN:
+		return "Unknown";
+	default:
+		return "Unsupported";
+	}
+}
+
+static void xgbe_phy_print_status(struct xgbe_prv_data *pdata)
+{
+	if (pdata->phy.link)
+		netdev_info(pdata->netdev,
+			    "Link is Up - %s/%s - flow control %s\n",
+			    xgbe_phy_speed_string(pdata->phy.speed),
+			    pdata->phy.duplex == DUPLEX_FULL ? "Full" : "Half",
+			    xgbe_phy_fc_string(pdata));
+	else
+		netdev_info(pdata->netdev, "Link is Down\n");
+}
+
+static void xgbe_phy_adjust_link(struct xgbe_prv_data *pdata)
+{
+	int new_state = 0;
+
+	if (pdata->phy.link) {
+		/* Flow control support */
+		pdata->pause_autoneg = pdata->phy.pause_autoneg;
+
+		if (pdata->tx_pause != pdata->phy.tx_pause) {
+			new_state = 1;
+			pdata->hw_if.config_tx_flow_control(pdata);
+			pdata->tx_pause = pdata->phy.tx_pause;
+		}
+
+		if (pdata->rx_pause != pdata->phy.rx_pause) {
+			new_state = 1;
+			pdata->hw_if.config_rx_flow_control(pdata);
+			pdata->rx_pause = pdata->phy.rx_pause;
+		}
+
+		/* Speed support */
+		if (pdata->phy_speed != pdata->phy.speed) {
+			new_state = 1;
+			pdata->phy_speed = pdata->phy.speed;
+		}
+
+		if (pdata->phy_link != pdata->phy.link) {
+			new_state = 1;
+			pdata->phy_link = pdata->phy.link;
+		}
+	} else if (pdata->phy_link) {
+		new_state = 1;
+		pdata->phy_link = 0;
+		pdata->phy_speed = SPEED_UNKNOWN;
+	}
+
+	if (new_state && netif_msg_link(pdata))
+		xgbe_phy_print_status(pdata);
+}
+
+static int xgbe_phy_config_fixed(struct xgbe_prv_data *pdata)
+{
+	netif_dbg(pdata, link, pdata->netdev, "fixed PHY configuration\n");
+
+	/* Disable auto-negotiation */
+	xgbe_disable_an(pdata);
+
+	/* Validate/Set specified speed */
+	switch (pdata->phy.speed) {
+	case SPEED_10000:
+		xgbe_set_mode(pdata, XGBE_MODE_KR);
+		break;
+
+	case SPEED_2500:
+	case SPEED_1000:
+		xgbe_set_mode(pdata, XGBE_MODE_KX);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	/* Validate duplex mode */
+	if (pdata->phy.duplex != DUPLEX_FULL)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int __xgbe_phy_config_aneg(struct xgbe_prv_data *pdata)
+{
+	set_bit(XGBE_LINK_INIT, &pdata->dev_state);
+	pdata->link_check = jiffies;
+
+	if (pdata->phy.autoneg != AUTONEG_ENABLE)
+		return xgbe_phy_config_fixed(pdata);
+
+	netif_dbg(pdata, link, pdata->netdev, "AN PHY configuration\n");
+
+	/* Disable auto-negotiation interrupt */
+	disable_irq(pdata->an_irq);
+
+	/* Start auto-negotiation in a supported mode */
+	if (pdata->phy.advertising & ADVERTISED_10000baseKR_Full) {
+		xgbe_set_mode(pdata, XGBE_MODE_KR);
+	} else if ((pdata->phy.advertising & ADVERTISED_1000baseKX_Full) ||
+		   (pdata->phy.advertising & ADVERTISED_2500baseX_Full)) {
+		xgbe_set_mode(pdata, XGBE_MODE_KX);
+	} else {
+		enable_irq(pdata->an_irq);
+		return -EINVAL;
+	}
+
+	/* Disable and stop any in progress auto-negotiation */
+	xgbe_disable_an(pdata);
+
+	/* Clear any auto-negotitation interrupts */
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_INT, 0);
+
+	pdata->an_result = XGBE_AN_READY;
+	pdata->an_state = XGBE_AN_READY;
+	pdata->kr_state = XGBE_RX_BPA;
+	pdata->kx_state = XGBE_RX_BPA;
+
+	/* Re-enable auto-negotiation interrupt */
+	enable_irq(pdata->an_irq);
+
+	/* Set up advertisement registers based on current settings */
+	xgbe_an_init(pdata);
+
+	/* Enable and start auto-negotiation */
+	xgbe_restart_an(pdata);
 
 	return 0;
+}
+
+static int xgbe_phy_config_aneg(struct xgbe_prv_data *pdata)
+{
+	int ret;
+
+	mutex_lock(&pdata->an_mutex);
+
+	ret = __xgbe_phy_config_aneg(pdata);
+	if (ret)
+		set_bit(XGBE_LINK_ERR, &pdata->dev_state);
+	else
+		clear_bit(XGBE_LINK_ERR, &pdata->dev_state);
+
+	mutex_unlock(&pdata->an_mutex);
+
+	return ret;
+}
+
+static bool xgbe_phy_aneg_done(struct xgbe_prv_data *pdata)
+{
+	return (pdata->an_result == XGBE_AN_COMPLETE);
+}
+
+static void xgbe_check_link_timeout(struct xgbe_prv_data *pdata)
+{
+	unsigned long link_timeout;
+
+	link_timeout = pdata->link_check + (XGBE_LINK_TIMEOUT * HZ);
+	if (time_after(jiffies, link_timeout)) {
+		netif_dbg(pdata, link, pdata->netdev, "AN link timeout\n");
+		xgbe_phy_config_aneg(pdata);
+	}
+}
+
+static void xgbe_phy_status_force(struct xgbe_prv_data *pdata)
+{
+	if (xgbe_in_kr_mode(pdata)) {
+		pdata->phy.speed = SPEED_10000;
+	} else {
+		switch (pdata->speed_set) {
+		case XGBE_SPEEDSET_1000_10000:
+			pdata->phy.speed = SPEED_1000;
+			break;
+
+		case XGBE_SPEEDSET_2500_10000:
+			pdata->phy.speed = SPEED_2500;
+			break;
+		}
+	}
+	pdata->phy.duplex = DUPLEX_FULL;
+}
+
+static void xgbe_phy_status_aneg(struct xgbe_prv_data *pdata)
+{
+	unsigned int ad_reg, lp_reg;
+
+	pdata->phy.lp_advertising = 0;
+
+	if ((pdata->phy.autoneg != AUTONEG_ENABLE) || pdata->parallel_detect)
+		return xgbe_phy_status_force(pdata);
+
+	pdata->phy.lp_advertising |= ADVERTISED_Autoneg;
+	pdata->phy.lp_advertising |= ADVERTISED_Backplane;
+
+	/* Compare Advertisement and Link Partner register 1 */
+	ad_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE);
+	lp_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_LPA);
+	if (lp_reg & 0x400)
+		pdata->phy.lp_advertising |= ADVERTISED_Pause;
+	if (lp_reg & 0x800)
+		pdata->phy.lp_advertising |= ADVERTISED_Asym_Pause;
+
+	if (pdata->phy.pause_autoneg) {
+		/* Set flow control based on auto-negotiation result */
+		pdata->phy.tx_pause = 0;
+		pdata->phy.rx_pause = 0;
+
+		if (ad_reg & lp_reg & 0x400) {
+			pdata->phy.tx_pause = 1;
+			pdata->phy.rx_pause = 1;
+		} else if (ad_reg & lp_reg & 0x800) {
+			if (ad_reg & 0x400)
+				pdata->phy.rx_pause = 1;
+			else if (lp_reg & 0x400)
+				pdata->phy.tx_pause = 1;
+		}
+	}
+
+	/* Compare Advertisement and Link Partner register 2 */
+	ad_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 1);
+	lp_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_LPA + 1);
+	if (lp_reg & 0x80)
+		pdata->phy.lp_advertising |= ADVERTISED_10000baseKR_Full;
+	if (lp_reg & 0x20) {
+		switch (pdata->speed_set) {
+		case XGBE_SPEEDSET_1000_10000:
+			pdata->phy.lp_advertising |= ADVERTISED_1000baseKX_Full;
+			break;
+		case XGBE_SPEEDSET_2500_10000:
+			pdata->phy.lp_advertising |= ADVERTISED_2500baseX_Full;
+			break;
+		}
+	}
+
+	ad_reg &= lp_reg;
+	if (ad_reg & 0x80) {
+		pdata->phy.speed = SPEED_10000;
+		xgbe_set_mode(pdata, XGBE_MODE_KR);
+	} else if (ad_reg & 0x20) {
+		switch (pdata->speed_set) {
+		case XGBE_SPEEDSET_1000_10000:
+			pdata->phy.speed = SPEED_1000;
+			break;
+
+		case XGBE_SPEEDSET_2500_10000:
+			pdata->phy.speed = SPEED_2500;
+			break;
+		}
+
+		xgbe_set_mode(pdata, XGBE_MODE_KX);
+	} else {
+		pdata->phy.speed = SPEED_UNKNOWN;
+	}
+
+	/* Compare Advertisement and Link Partner register 3 */
+	ad_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2);
+	lp_reg = XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_LPA + 2);
+	if (lp_reg & 0xc000)
+		pdata->phy.lp_advertising |= ADVERTISED_10000baseR_FEC;
+
+	pdata->phy.duplex = DUPLEX_FULL;
+}
+
+static void xgbe_phy_status(struct xgbe_prv_data *pdata)
+{
+	unsigned int reg, link_aneg;
+
+	if (test_bit(XGBE_LINK_ERR, &pdata->dev_state)) {
+		if (test_and_clear_bit(XGBE_LINK, &pdata->dev_state))
+			netif_carrier_off(pdata->netdev);
+
+		pdata->phy.link = 0;
+		goto adjust_link;
+	}
+
+	link_aneg = (pdata->phy.autoneg == AUTONEG_ENABLE);
+
+	/* Get the link status. Link status is latched low, so read
+	 * once to clear and then read again to get current state
+	 */
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_STAT1);
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_STAT1);
+	pdata->phy.link = (reg & MDIO_STAT1_LSTATUS) ? 1 : 0;
+
+	if (pdata->phy.link) {
+		if (link_aneg && !xgbe_phy_aneg_done(pdata)) {
+			xgbe_check_link_timeout(pdata);
+			return;
+		}
+
+		xgbe_phy_status_aneg(pdata);
+
+		if (test_bit(XGBE_LINK_INIT, &pdata->dev_state))
+			clear_bit(XGBE_LINK_INIT, &pdata->dev_state);
+
+		if (!test_bit(XGBE_LINK, &pdata->dev_state)) {
+			set_bit(XGBE_LINK, &pdata->dev_state);
+			netif_carrier_on(pdata->netdev);
+		}
+	} else {
+		if (test_bit(XGBE_LINK_INIT, &pdata->dev_state)) {
+			xgbe_check_link_timeout(pdata);
+
+			if (link_aneg)
+				return;
+		}
+
+		xgbe_phy_status_aneg(pdata);
+
+		if (test_bit(XGBE_LINK, &pdata->dev_state)) {
+			clear_bit(XGBE_LINK, &pdata->dev_state);
+			netif_carrier_off(pdata->netdev);
+		}
+	}
+
+adjust_link:
+	xgbe_phy_adjust_link(pdata);
+}
+
+static void xgbe_phy_stop(struct xgbe_prv_data *pdata)
+{
+	netif_dbg(pdata, link, pdata->netdev, "stopping PHY\n");
+
+	/* Disable auto-negotiation */
+	xgbe_disable_an(pdata);
+
+	/* Disable auto-negotiation interrupts */
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_INTMASK, 0);
+
+	devm_free_irq(pdata->dev, pdata->an_irq, pdata);
 
-err_phy_device:
-	phy_device_free(phydev);
+	pdata->phy.link = 0;
+	if (test_and_clear_bit(XGBE_LINK, &pdata->dev_state))
+		netif_carrier_off(pdata->netdev);
 
-err_mdiobus_register:
-	mdiobus_unregister(mii);
+	xgbe_phy_adjust_link(pdata);
+}
+
+static int xgbe_phy_start(struct xgbe_prv_data *pdata)
+{
+	struct net_device *netdev = pdata->netdev;
+	int ret;
+
+	netif_dbg(pdata, link, pdata->netdev, "starting PHY\n");
+
+	ret = devm_request_irq(pdata->dev, pdata->an_irq,
+			       xgbe_an_isr, 0, pdata->an_name,
+			       pdata);
+	if (ret) {
+		netdev_err(netdev, "phy irq request failed\n");
+		return ret;
+	}
+
+	/* Set initial mode - call the mode setting routines
+	 * directly to insure we are properly configured
+	 */
+	if (xgbe_use_xgmii_mode(pdata)) {
+		xgbe_xgmii_mode(pdata);
+	} else if (xgbe_use_gmii_mode(pdata)) {
+		xgbe_gmii_mode(pdata);
+	} else if (xgbe_use_gmii_2500_mode(pdata)) {
+		xgbe_gmii_2500_mode(pdata);
+	} else {
+		ret = -EINVAL;
+		goto err_irq;
+	}
+
+	/* Set up advertisement registers based on current settings */
+	xgbe_an_init(pdata);
+
+	/* Enable auto-negotiation interrupts */
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_INTMASK, 0x07);
 
-err_mdiobus_alloc:
-	mdiobus_free(mii);
+	return xgbe_phy_config_aneg(pdata);
+
+err_irq:
+	devm_free_irq(pdata->dev, pdata->an_irq, pdata);
 
 	return ret;
 }
 
-void xgbe_mdio_unregister(struct xgbe_prv_data *pdata)
+static int xgbe_phy_reset(struct xgbe_prv_data *pdata)
+{
+	unsigned int count, reg;
+
+	reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1);
+	reg |= MDIO_CTRL1_RESET;
+	XMDIO_WRITE(pdata, MDIO_MMD_PCS, MDIO_CTRL1, reg);
+
+	count = 50;
+	do {
+		msleep(20);
+		reg = XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1);
+	} while ((reg & MDIO_CTRL1_RESET) && --count);
+
+	if (reg & MDIO_CTRL1_RESET)
+		return -ETIMEDOUT;
+
+	/* Disable auto-negotiation for now */
+	xgbe_disable_an(pdata);
+
+	/* Clear auto-negotiation interrupts */
+	XMDIO_WRITE(pdata, MDIO_MMD_AN, MDIO_AN_INT, 0);
+
+	return 0;
+}
+
+static void xgbe_dump_phy_registers(struct xgbe_prv_data *pdata)
 {
-	DBGPR("-->xgbe_mdio_unregister\n");
+	struct device *dev = pdata->dev;
+
+	dev_dbg(dev, "\n************* PHY Reg dump **********************\n");
+
+	dev_dbg(dev, "PCS Control Reg (%#04x) = %#04x\n", MDIO_CTRL1,
+		XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_CTRL1));
+	dev_dbg(dev, "PCS Status Reg (%#04x) = %#04x\n", MDIO_STAT1,
+		XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_STAT1));
+	dev_dbg(dev, "Phy Id (PHYS ID 1 %#04x)= %#04x\n", MDIO_DEVID1,
+		XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_DEVID1));
+	dev_dbg(dev, "Phy Id (PHYS ID 2 %#04x)= %#04x\n", MDIO_DEVID2,
+		XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_DEVID2));
+	dev_dbg(dev, "Devices in Package (%#04x)= %#04x\n", MDIO_DEVS1,
+		XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_DEVS1));
+	dev_dbg(dev, "Devices in Package (%#04x)= %#04x\n", MDIO_DEVS2,
+		XMDIO_READ(pdata, MDIO_MMD_PCS, MDIO_DEVS2));
+
+	dev_dbg(dev, "Auto-Neg Control Reg (%#04x) = %#04x\n", MDIO_CTRL1,
+		XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_CTRL1));
+	dev_dbg(dev, "Auto-Neg Status Reg (%#04x) = %#04x\n", MDIO_STAT1,
+		XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_STAT1));
+	dev_dbg(dev, "Auto-Neg Ad Reg 1 (%#04x) = %#04x\n",
+		MDIO_AN_ADVERTISE,
+		XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE));
+	dev_dbg(dev, "Auto-Neg Ad Reg 2 (%#04x) = %#04x\n",
+		MDIO_AN_ADVERTISE + 1,
+		XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 1));
+	dev_dbg(dev, "Auto-Neg Ad Reg 3 (%#04x) = %#04x\n",
+		MDIO_AN_ADVERTISE + 2,
+		XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2));
+	dev_dbg(dev, "Auto-Neg Completion Reg (%#04x) = %#04x\n",
+		MDIO_AN_COMP_STAT,
+		XMDIO_READ(pdata, MDIO_MMD_AN, MDIO_AN_COMP_STAT));
+
+	dev_dbg(dev, "\n*************************************************\n");
+}
+
+static void xgbe_phy_init(struct xgbe_prv_data *pdata)
+{
+	mutex_init(&pdata->an_mutex);
+	INIT_WORK(&pdata->an_irq_work, xgbe_an_irq_work);
+	INIT_WORK(&pdata->an_work, xgbe_an_state_machine);
+	pdata->mdio_mmd = MDIO_MMD_PCS;
+
+	/* Initialize supported features */
+	pdata->phy.supported = SUPPORTED_Autoneg;
+	pdata->phy.supported |= SUPPORTED_Pause | SUPPORTED_Asym_Pause;
+	pdata->phy.supported |= SUPPORTED_Backplane;
+	pdata->phy.supported |= SUPPORTED_10000baseKR_Full;
+	switch (pdata->speed_set) {
+	case XGBE_SPEEDSET_1000_10000:
+		pdata->phy.supported |= SUPPORTED_1000baseKX_Full;
+		break;
+	case XGBE_SPEEDSET_2500_10000:
+		pdata->phy.supported |= SUPPORTED_2500baseX_Full;
+		break;
+	}
 
-	pdata->phydev = NULL;
+	pdata->fec_ability = XMDIO_READ(pdata, MDIO_MMD_PMAPMD,
+					MDIO_PMA_10GBR_FECABLE);
+	pdata->fec_ability &= (MDIO_PMA_10GBR_FECABLE_ABLE |
+			       MDIO_PMA_10GBR_FECABLE_ERRABLE);
+	if (pdata->fec_ability & MDIO_PMA_10GBR_FECABLE_ABLE)
+		pdata->phy.supported |= SUPPORTED_10000baseR_FEC;
 
-	module_put(pdata->phy_module);
-	pdata->phy_module = NULL;
+	pdata->phy.advertising = pdata->phy.supported;
 
-	mdiobus_unregister(pdata->mii);
-	pdata->mii->priv = NULL;
+	pdata->phy.address = 0;
+
+	pdata->phy.autoneg = AUTONEG_ENABLE;
+	pdata->phy.speed = SPEED_UNKNOWN;
+	pdata->phy.duplex = DUPLEX_UNKNOWN;
+
+	pdata->phy.link = 0;
+
+	pdata->phy.pause_autoneg = pdata->pause_autoneg;
+	pdata->phy.tx_pause = pdata->tx_pause;
+	pdata->phy.rx_pause = pdata->rx_pause;
+
+	/* Fix up Flow Control advertising */
+	pdata->phy.advertising &= ~ADVERTISED_Pause;
+	pdata->phy.advertising &= ~ADVERTISED_Asym_Pause;
+
+	if (pdata->rx_pause) {
+		pdata->phy.advertising |= ADVERTISED_Pause;
+		pdata->phy.advertising |= ADVERTISED_Asym_Pause;
+	}
+
+	if (pdata->tx_pause)
+		pdata->phy.advertising ^= ADVERTISED_Asym_Pause;
+
+	if (netif_msg_drv(pdata))
+		xgbe_dump_phy_registers(pdata);
+}
+
+void xgbe_init_function_ptrs_phy(struct xgbe_phy_if *phy_if)
+{
+	phy_if->phy_init        = xgbe_phy_init;
 
-	mdiobus_free(pdata->mii);
-	pdata->mii = NULL;
+	phy_if->phy_reset       = xgbe_phy_reset;
+	phy_if->phy_start       = xgbe_phy_start;
+	phy_if->phy_stop        = xgbe_phy_stop;
 
-	DBGPR("<--xgbe_mdio_unregister\n");
+	phy_if->phy_status      = xgbe_phy_status;
+	phy_if->phy_config_aneg = xgbe_phy_config_aneg;
 }
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe.h b/drivers/net/ethernet/amd/xgbe/xgbe.h
index e62dfa2deab6..63d72a140053 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe.h
@@ -129,7 +129,7 @@
 #include <net/dcbnl.h>
 
 #define XGBE_DRV_NAME		"amd-xgbe"
-#define XGBE_DRV_VERSION	"1.0.0-a"
+#define XGBE_DRV_VERSION	"1.0.2"
 #define XGBE_DRV_DESC		"AMD 10 Gigabit Ethernet Driver"
 
 /* Descriptor related defines */
@@ -178,14 +178,17 @@
 #define XGMAC_JUMBO_PACKET_MTU	9000
 #define XGMAC_MAX_JUMBO_PACKET	9018
 
-/* MDIO bus phy name */
-#define XGBE_PHY_NAME		"amd_xgbe_phy"
-#define XGBE_PRTAD		0
-
 /* Common property names */
 #define XGBE_MAC_ADDR_PROPERTY	"mac-address"
 #define XGBE_PHY_MODE_PROPERTY	"phy-mode"
 #define XGBE_DMA_IRQS_PROPERTY	"amd,per-channel-interrupt"
+#define XGBE_SPEEDSET_PROPERTY	"amd,speed-set"
+#define XGBE_BLWC_PROPERTY	"amd,serdes-blwc"
+#define XGBE_CDR_RATE_PROPERTY	"amd,serdes-cdr-rate"
+#define XGBE_PQ_SKEW_PROPERTY	"amd,serdes-pq-skew"
+#define XGBE_TX_AMP_PROPERTY	"amd,serdes-tx-amp"
+#define XGBE_DFE_CFG_PROPERTY	"amd,serdes-dfe-tap-config"
+#define XGBE_DFE_ENA_PROPERTY	"amd,serdes-dfe-tap-enable"
 
 /* Device-tree clock names */
 #define XGBE_DMA_CLOCK		"dma_clk"
@@ -241,6 +244,49 @@
 #define XGBE_RSS_LOOKUP_TABLE_TYPE	0
 #define XGBE_RSS_HASH_KEY_TYPE		1
 
+/* Auto-negotiation */
+#define XGBE_AN_MS_TIMEOUT		500
+#define XGBE_LINK_TIMEOUT		10
+
+#define XGBE_AN_INT_CMPLT		0x01
+#define XGBE_AN_INC_LINK		0x02
+#define XGBE_AN_PG_RCV			0x04
+#define XGBE_AN_INT_MASK		0x07
+
+/* Rate-change complete wait/retry count */
+#define XGBE_RATECHANGE_COUNT		500
+
+/* Default SerDes settings */
+#define XGBE_SPEED_10000_BLWC		0
+#define XGBE_SPEED_10000_CDR		0x7
+#define XGBE_SPEED_10000_PLL		0x1
+#define XGBE_SPEED_10000_PQ		0x12
+#define XGBE_SPEED_10000_RATE		0x0
+#define XGBE_SPEED_10000_TXAMP		0xa
+#define XGBE_SPEED_10000_WORD		0x7
+#define XGBE_SPEED_10000_DFE_TAP_CONFIG	0x1
+#define XGBE_SPEED_10000_DFE_TAP_ENABLE	0x7f
+
+#define XGBE_SPEED_2500_BLWC		1
+#define XGBE_SPEED_2500_CDR		0x2
+#define XGBE_SPEED_2500_PLL		0x0
+#define XGBE_SPEED_2500_PQ		0xa
+#define XGBE_SPEED_2500_RATE		0x1
+#define XGBE_SPEED_2500_TXAMP		0xf
+#define XGBE_SPEED_2500_WORD		0x1
+#define XGBE_SPEED_2500_DFE_TAP_CONFIG	0x3
+#define XGBE_SPEED_2500_DFE_TAP_ENABLE	0x0
+
+#define XGBE_SPEED_1000_BLWC		1
+#define XGBE_SPEED_1000_CDR		0x2
+#define XGBE_SPEED_1000_PLL		0x0
+#define XGBE_SPEED_1000_PQ		0xa
+#define XGBE_SPEED_1000_RATE		0x3
+#define XGBE_SPEED_1000_TXAMP		0xf
+#define XGBE_SPEED_1000_WORD		0x1
+#define XGBE_SPEED_1000_DFE_TAP_CONFIG	0x3
+#define XGBE_SPEED_1000_DFE_TAP_ENABLE	0x0
+
 struct xgbe_prv_data;
 
 struct xgbe_packet_data {
@@ -334,8 +380,6 @@ struct xgbe_ring_data {
 	 */
 	unsigned int state_saved;
 	struct {
-		unsigned int incomplete;
-		unsigned int context_next;
 		struct sk_buff *skb;
 		unsigned int len;
 		unsigned int error;
@@ -414,6 +458,13 @@ struct xgbe_channel {
 	struct xgbe_ring *rx_ring;
 } ____cacheline_aligned;
 
+enum xgbe_state {
+	XGBE_DOWN,
+	XGBE_LINK,
+	XGBE_LINK_INIT,
+	XGBE_LINK_ERR,
+};
+
 enum xgbe_int {
 	XGMAC_INT_DMA_CH_SR_TI,
 	XGMAC_INT_DMA_CH_SR_TPS,
@@ -445,6 +496,57 @@ enum xgbe_mtl_fifo_size {
 	XGMAC_MTL_FIFO_SIZE_256K = 0x3ff,
 };
 
+enum xgbe_speed {
+	XGBE_SPEED_1000 = 0,
+	XGBE_SPEED_2500,
+	XGBE_SPEED_10000,
+	XGBE_SPEEDS,
+};
+
+enum xgbe_an {
+	XGBE_AN_READY = 0,
+	XGBE_AN_PAGE_RECEIVED,
+	XGBE_AN_INCOMPAT_LINK,
+	XGBE_AN_COMPLETE,
+	XGBE_AN_NO_LINK,
+	XGBE_AN_ERROR,
+};
+
+enum xgbe_rx {
+	XGBE_RX_BPA = 0,
+	XGBE_RX_XNP,
+	XGBE_RX_COMPLETE,
+	XGBE_RX_ERROR,
+};
+
+enum xgbe_mode {
+	XGBE_MODE_KR = 0,
+	XGBE_MODE_KX,
+};
+
+enum xgbe_speedset {
+	XGBE_SPEEDSET_1000_10000 = 0,
+	XGBE_SPEEDSET_2500_10000,
+};
+
+struct xgbe_phy {
+	u32 supported;
+	u32 advertising;
+	u32 lp_advertising;
+
+	int address;
+
+	int autoneg;
+	int speed;
+	int duplex;
+
+	int link;
+
+	int pause_autoneg;
+	int tx_pause;
+	int rx_pause;
+};
+
 struct xgbe_mmc_stats {
 	/* Tx Stats */
 	u64 txoctetcount_gb;
@@ -492,6 +594,11 @@ struct xgbe_mmc_stats {
 	u64 rxwatchdogerror;
 };
 
+struct xgbe_ext_stats {
+	u64 tx_tso_packets;
+	u64 rx_split_header_packets;
+};
+
 struct xgbe_hw_if {
 	int (*tx_complete)(struct xgbe_ring_desc *);
 
@@ -591,6 +698,20 @@ struct xgbe_hw_if {
 	int (*set_rss_lookup_table)(struct xgbe_prv_data *, const u32 *);
 };
 
+struct xgbe_phy_if {
+	/* For initial PHY setup */
+	void (*phy_init)(struct xgbe_prv_data *);
+
+	/* For PHY support when setting device up/down */
+	int (*phy_reset)(struct xgbe_prv_data *);
+	int (*phy_start)(struct xgbe_prv_data *);
+	void (*phy_stop)(struct xgbe_prv_data *);
+
+	/* For PHY support while device is up */
+	void (*phy_status)(struct xgbe_prv_data *);
+	int (*phy_config_aneg)(struct xgbe_prv_data *);
+};
+
 struct xgbe_desc_if {
 	int (*alloc_ring_resources)(struct xgbe_prv_data *);
 	void (*free_ring_resources)(struct xgbe_prv_data *);
@@ -660,6 +781,9 @@ struct xgbe_prv_data {
 	/* XGMAC/XPCS related mmio registers */
 	void __iomem *xgmac_regs;	/* XGMAC CSRs */
 	void __iomem *xpcs_regs;	/* XPCS MMD registers */
+	void __iomem *rxtx_regs;	/* SerDes Rx/Tx CSRs */
+	void __iomem *sir0_regs;	/* SerDes integration registers (1/2) */
+	void __iomem *sir1_regs;	/* SerDes integration registers (2/2) */
 
 	/* Overall device lock */
 	spinlock_t lock;
@@ -670,10 +794,14 @@ struct xgbe_prv_data {
 	/* RSS addressing mutex */
 	struct mutex rss_mutex;
 
+	/* Flags representing xgbe_state */
+	unsigned long dev_state;
+
 	int dev_irq;
 	unsigned int per_channel_irq;
 
 	struct xgbe_hw_if hw_if;
+	struct xgbe_phy_if phy_if;
 	struct xgbe_desc_if desc_if;
 
 	/* AXI DMA settings */
@@ -682,6 +810,11 @@ struct xgbe_prv_data {
 	unsigned int arcache;
 	unsigned int awcache;
 
+	/* Service routine support */
+	struct workqueue_struct *dev_workqueue;
+	struct work_struct service_work;
+	struct timer_list service_timer;
+
 	/* Rings for Tx/Rx on a DMA channel */
 	struct xgbe_channel *channel;
 	unsigned int channel_count;
@@ -729,27 +862,12 @@ struct xgbe_prv_data {
 	u32 rss_table[XGBE_RSS_MAX_TABLE_SIZE];
 	u32 rss_options;
 
-	/* MDIO settings */
-	struct module *phy_module;
-	char *mii_bus_id;
-	struct mii_bus *mii;
-	int mdio_mmd;
-	struct phy_device *phydev;
-	int default_autoneg;
-	int default_speed;
-
-	/* Current PHY settings */
-	phy_interface_t phy_mode;
-	int phy_link;
-	int phy_speed;
-	unsigned int phy_tx_pause;
-	unsigned int phy_rx_pause;
-
 	/* Netdev related settings */
 	unsigned char mac_addr[ETH_ALEN];
 	netdev_features_t netdev_features;
 	struct napi_struct napi;
 	struct xgbe_mmc_stats mmc_stats;
+	struct xgbe_ext_stats ext_stats;
 
 	/* Filtering support */
 	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
@@ -787,6 +905,54 @@ struct xgbe_prv_data {
 	/* Keeps track of power mode */
 	unsigned int power_down;
 
+	/* Network interface message level setting */
+	u32 msg_enable;
+
+	/* Current PHY settings */
+	phy_interface_t phy_mode;
+	int phy_link;
+	int phy_speed;
+
+	/* MDIO/PHY related settings */
+	struct xgbe_phy phy;
+	int mdio_mmd;
+	unsigned long link_check;
+
+	char an_name[IFNAMSIZ + 32];
+	struct workqueue_struct *an_workqueue;
+
+	int an_irq;
+	struct work_struct an_irq_work;
+
+	unsigned int speed_set;
+
+	/* SerDes UEFI configurable settings.
+	 *   Switching between modes/speeds requires new values for some
+	 *   SerDes settings.  The values can be supplied as device
+	 *   properties in array format.  The first array entry is for
+	 *   1GbE, second for 2.5GbE and third for 10GbE
+	 */
+	u32 serdes_blwc[XGBE_SPEEDS];
+	u32 serdes_cdr_rate[XGBE_SPEEDS];
+	u32 serdes_pq_skew[XGBE_SPEEDS];
+	u32 serdes_tx_amp[XGBE_SPEEDS];
+	u32 serdes_dfe_tap_cfg[XGBE_SPEEDS];
+	u32 serdes_dfe_tap_ena[XGBE_SPEEDS];
+
+	/* Auto-negotiation state machine support */
+	struct mutex an_mutex;
+	enum xgbe_an an_result;
+	enum xgbe_an an_state;
+	enum xgbe_rx kr_state;
+	enum xgbe_rx kx_state;
+	struct work_struct an_work;
+	unsigned int an_supported;
+	unsigned int parallel_detect;
+	unsigned int fec_ability;
+	unsigned long an_start;
+
+	unsigned int lpm_ctrl;		/* CTRL1 for resume */
+
 #ifdef CONFIG_DEBUG_FS
 	struct dentry *xgbe_debugfs;
 
@@ -800,6 +966,7 @@ struct xgbe_prv_data {
 /* Function prototypes*/
 
 void xgbe_init_function_ptrs_dev(struct xgbe_hw_if *);
+void xgbe_init_function_ptrs_phy(struct xgbe_phy_if *);
 void xgbe_init_function_ptrs_desc(struct xgbe_desc_if *);
 struct net_device_ops *xgbe_get_netdev_ops(void);
 struct ethtool_ops *xgbe_get_ethtool_ops(void);
@@ -807,14 +974,11 @@ struct ethtool_ops *xgbe_get_ethtool_ops(void);
 const struct dcbnl_rtnl_ops *xgbe_get_dcbnl_ops(void);
 #endif
 
-int xgbe_mdio_register(struct xgbe_prv_data *);
-void xgbe_mdio_unregister(struct xgbe_prv_data *);
-void xgbe_dump_phy_registers(struct xgbe_prv_data *);
 void xgbe_ptp_register(struct xgbe_prv_data *);
 void xgbe_ptp_unregister(struct xgbe_prv_data *);
-void xgbe_dump_tx_desc(struct xgbe_ring *, unsigned int, unsigned int,
-		       unsigned int);
-void xgbe_dump_rx_desc(struct xgbe_ring *, struct xgbe_ring_desc *,
+void xgbe_dump_tx_desc(struct xgbe_prv_data *, struct xgbe_ring *,
+		       unsigned int, unsigned int, unsigned int);
+void xgbe_dump_rx_desc(struct xgbe_prv_data *, struct xgbe_ring *,
 		       unsigned int);
 void xgbe_print_pkt(struct net_device *, struct sk_buff *, bool);
 void xgbe_get_all_hw_features(struct xgbe_prv_data *);
@@ -831,18 +995,6 @@ static inline void xgbe_debugfs_init(struct xgbe_prv_data *pdata) {}
 static inline void xgbe_debugfs_exit(struct xgbe_prv_data *pdata) {}
 #endif /* CONFIG_DEBUG_FS */
 
-/* NOTE: Uncomment for TX and RX DESCRIPTOR DUMP in KERNEL LOG */
-#if 0
-#define XGMAC_ENABLE_TX_DESC_DUMP
-#define XGMAC_ENABLE_RX_DESC_DUMP
-#endif
-
-/* NOTE: Uncomment for TX and RX PACKET DUMP in KERNEL LOG */
-#if 0
-#define XGMAC_ENABLE_TX_PKT_DUMP
-#define XGMAC_ENABLE_RX_PKT_DUMP
-#endif
-
 /* NOTE: Uncomment for function trace log messages in KERNEL LOG */
 #if 0
 #define YDEBUG
@@ -852,10 +1004,8 @@ static inline void xgbe_debugfs_exit(struct xgbe_prv_data *pdata) {}
 /* For debug prints */
 #ifdef YDEBUG
 #define DBGPR(x...) pr_alert(x)
-#define DBGPHY_REGS(x...) xgbe_dump_phy_registers(x)
 #else
 #define DBGPR(x...) do { } while (0)
-#define DBGPHY_REGS(x...) do { } while (0)
 #endif
 
 #ifdef YDEBUG_MDIO
diff --git a/drivers/net/ethernet/apm/xgene/Makefile b/drivers/net/ethernet/apm/xgene/Makefile
index 68be565548c0..700b5abe5de5 100644
--- a/drivers/net/ethernet/apm/xgene/Makefile
+++ b/drivers/net/ethernet/apm/xgene/Makefile
@@ -3,5 +3,5 @@
 #
 
 xgene-enet-objs := xgene_enet_hw.o xgene_enet_sgmac.o xgene_enet_xgmac.o \
-		   xgene_enet_main.o xgene_enet_ethtool.o
+		   xgene_enet_main.o xgene_enet_ring2.o xgene_enet_ethtool.o
 obj-$(CONFIG_NET_XGENE) += xgene-enet.o
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
index b927021c6c40..a626c4315a89 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
@@ -87,10 +87,11 @@ static void xgene_enet_ring_rd32(struct xgene_enet_desc_ring *ring,
 
 static void xgene_enet_write_ring_state(struct xgene_enet_desc_ring *ring)
 {
+	struct xgene_enet_pdata *pdata = netdev_priv(ring->ndev);
 	int i;
 
 	xgene_enet_ring_wr32(ring, CSR_RING_CONFIG, ring->num);
-	for (i = 0; i < NUM_RING_CONFIG; i++) {
+	for (i = 0; i < pdata->ring_ops->num_ring_config; i++) {
 		xgene_enet_ring_wr32(ring, CSR_RING_WR_BASE + (i * 4),
 				     ring->state[i]);
 	}
@@ -98,7 +99,7 @@ static void xgene_enet_write_ring_state(struct xgene_enet_desc_ring *ring)
 
 static void xgene_enet_clr_ring_state(struct xgene_enet_desc_ring *ring)
 {
-	memset(ring->state, 0, sizeof(u32) * NUM_RING_CONFIG);
+	memset(ring->state, 0, sizeof(ring->state));
 	xgene_enet_write_ring_state(ring);
 }
 
@@ -141,8 +142,8 @@ static void xgene_enet_clr_desc_ring_id(struct xgene_enet_desc_ring *ring)
 	xgene_enet_ring_wr32(ring, CSR_RING_ID_BUF, 0);
 }
 
-struct xgene_enet_desc_ring *xgene_enet_setup_ring(
-					struct xgene_enet_desc_ring *ring)
+static struct xgene_enet_desc_ring *xgene_enet_setup_ring(
+				    struct xgene_enet_desc_ring *ring)
 {
 	u32 size = ring->size;
 	u32 i, data;
@@ -168,7 +169,7 @@ struct xgene_enet_desc_ring *xgene_enet_setup_ring(
 	return ring;
 }
 
-void xgene_enet_clear_ring(struct xgene_enet_desc_ring *ring)
+static void xgene_enet_clear_ring(struct xgene_enet_desc_ring *ring)
 {
 	u32 data;
 	bool is_bufpool;
@@ -186,6 +187,22 @@ out:
 	xgene_enet_clr_ring_state(ring);
 }
 
+static void xgene_enet_wr_cmd(struct xgene_enet_desc_ring *ring, int count)
+{
+	iowrite32(count, ring->cmd);
+}
+
+static u32 xgene_enet_ring_len(struct xgene_enet_desc_ring *ring)
+{
+	u32 __iomem *cmd_base = ring->cmd_base;
+	u32 ring_state, num_msgs;
+
+	ring_state = ioread32(&cmd_base[1]);
+	num_msgs = GET_VAL(NUMMSGSINQ, ring_state);
+
+	return num_msgs;
+}
+
 void xgene_enet_parse_error(struct xgene_enet_desc_ring *ring,
 			    struct xgene_enet_pdata *pdata,
 			    enum xgene_enet_err_code status)
@@ -593,7 +610,7 @@ static int xgene_enet_reset(struct xgene_enet_pdata *pdata)
 	if (!xgene_ring_mgr_init(pdata))
 		return -ENODEV;
 
-	if (pdata->clk) {
+	if (!IS_ERR(pdata->clk)) {
 		clk_prepare_enable(pdata->clk);
 		clk_disable_unprepare(pdata->clk);
 		clk_prepare_enable(pdata->clk);
@@ -612,7 +629,8 @@ static int xgene_enet_reset(struct xgene_enet_pdata *pdata)
 
 static void xgene_gport_shutdown(struct xgene_enet_pdata *pdata)
 {
-	clk_disable_unprepare(pdata->clk);
+	if (!IS_ERR(pdata->clk))
+		clk_disable_unprepare(pdata->clk);
 }
 
 static int xgene_enet_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
@@ -734,7 +752,7 @@ static int xgene_mdiobus_register(struct xgene_enet_pdata *pdata,
 	if (ret)
 		return -EINVAL;
 
-	phy = get_phy_device(mdio, phy_id, true);
+	phy = get_phy_device(mdio, phy_id, false);
 	if (!phy || IS_ERR(phy))
 		return -EIO;
 
@@ -803,3 +821,12 @@ struct xgene_port_ops xgene_gport_ops = {
 	.cle_bypass = xgene_enet_cle_bypass,
 	.shutdown = xgene_gport_shutdown,
 };
+
+struct xgene_ring_ops xgene_ring1_ops = {
+	.num_ring_config = NUM_RING_CONFIG,
+	.num_ring_id_shift = 6,
+	.setup = xgene_enet_setup_ring,
+	.clear = xgene_enet_clear_ring,
+	.wr_cmd = xgene_enet_wr_cmd,
+	.len = xgene_enet_ring_len,
+};
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
index d9bc89d69266..541bed056012 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
@@ -26,6 +26,7 @@
 
 struct xgene_enet_pdata;
 struct xgene_enet_stats;
+struct xgene_enet_desc_ring;
 
 /* clears and then set bits */
 static inline void xgene_set_bits(u32 *dst, u32 val, u32 start, u32 len)
@@ -101,8 +102,8 @@ enum xgene_enet_rm {
 
 #define BLOCK_ETH_CSR_OFFSET		0x2000
 #define BLOCK_ETH_RING_IF_OFFSET	0x9000
+#define BLOCK_ETH_CLKRST_CSR_OFFSET	0xc000
 #define BLOCK_ETH_DIAG_CSR_OFFSET	0xD000
-
 #define BLOCK_ETH_MAC_OFFSET		0x0000
 #define BLOCK_ETH_MAC_CSR_OFFSET	0x2800
 
@@ -261,6 +262,7 @@ enum xgene_enet_ring_type {
 
 enum xgene_ring_owner {
 	RING_OWNER_ETH0,
+	RING_OWNER_ETH1,
 	RING_OWNER_CPU = 15,
 	RING_OWNER_INVALID
 };
@@ -314,9 +316,6 @@ static inline u16 xgene_enet_get_numslots(u16 id, u32 size)
 		      size / WORK_DESC_SIZE;
 }
 
-struct xgene_enet_desc_ring *xgene_enet_setup_ring(
-		struct xgene_enet_desc_ring *ring);
-void xgene_enet_clear_ring(struct xgene_enet_desc_ring *ring);
 void xgene_enet_parse_error(struct xgene_enet_desc_ring *ring,
 			    struct xgene_enet_pdata *pdata,
 			    enum xgene_enet_err_code status);
@@ -327,5 +326,6 @@ bool xgene_ring_mgr_init(struct xgene_enet_pdata *p);
 
 extern struct xgene_mac_ops xgene_gmac_ops;
 extern struct xgene_port_ops xgene_gport_ops;
+extern struct xgene_ring_ops xgene_ring1_ops;
 
 #endif /* __XGENE_ENET_HW_H__ */
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
index 40d3530d7f30..299eb4315fe6 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
@@ -28,6 +28,9 @@
 #define RES_RING_CSR	1
 #define RES_RING_CMD	2
 
+static const struct of_device_id xgene_enet_of_match[];
+static const struct acpi_device_id xgene_enet_acpi_match[];
+
 static void xgene_enet_init_bufpool(struct xgene_enet_desc_ring *buf_pool)
 {
 	struct xgene_enet_raw_desc16 *raw_desc;
@@ -48,6 +51,7 @@ static int xgene_enet_refill_bufpool(struct xgene_enet_desc_ring *buf_pool,
 {
 	struct sk_buff *skb;
 	struct xgene_enet_raw_desc16 *raw_desc;
+	struct xgene_enet_pdata *pdata;
 	struct net_device *ndev;
 	struct device *dev;
 	dma_addr_t dma_addr;
@@ -58,6 +62,7 @@ static int xgene_enet_refill_bufpool(struct xgene_enet_desc_ring *buf_pool,
 
 	ndev = buf_pool->ndev;
 	dev = ndev_to_dev(buf_pool->ndev);
+	pdata = netdev_priv(ndev);
 	bufdatalen = BUF_LEN_CODE_2K | (SKB_BUFFER_SIZE & GENMASK(11, 0));
 	len = XGENE_ENET_MAX_MTU;
 
@@ -82,7 +87,7 @@ static int xgene_enet_refill_bufpool(struct xgene_enet_desc_ring *buf_pool,
 		tail = (tail + 1) & slots;
 	}
 
-	iowrite32(nbuf, buf_pool->cmd);
+	pdata->ring_ops->wr_cmd(buf_pool, nbuf);
 	buf_pool->tail = tail;
 
 	return 0;
@@ -102,26 +107,16 @@ static u8 xgene_enet_hdr_len(const void *data)
 	return (eth->h_proto == htons(ETH_P_8021Q)) ? VLAN_ETH_HLEN : ETH_HLEN;
 }
 
-static u32 xgene_enet_ring_len(struct xgene_enet_desc_ring *ring)
-{
-	u32 __iomem *cmd_base = ring->cmd_base;
-	u32 ring_state, num_msgs;
-
-	ring_state = ioread32(&cmd_base[1]);
-	num_msgs = ring_state & CREATE_MASK(NUMMSGSINQ_POS, NUMMSGSINQ_LEN);
-
-	return num_msgs >> NUMMSGSINQ_POS;
-}
-
 static void xgene_enet_delete_bufpool(struct xgene_enet_desc_ring *buf_pool)
 {
+	struct xgene_enet_pdata *pdata = netdev_priv(buf_pool->ndev);
 	struct xgene_enet_raw_desc16 *raw_desc;
 	u32 slots = buf_pool->slots - 1;
 	u32 tail = buf_pool->tail;
 	u32 userinfo;
 	int i, len;
 
-	len = xgene_enet_ring_len(buf_pool);
+	len = pdata->ring_ops->len(buf_pool);
 	for (i = 0; i < len; i++) {
 		tail = (tail - 1) & slots;
 		raw_desc = &buf_pool->raw_desc16[tail];
@@ -131,7 +126,7 @@ static void xgene_enet_delete_bufpool(struct xgene_enet_desc_ring *buf_pool)
 		dev_kfree_skb_any(buf_pool->rx_skb[userinfo]);
 	}
 
-	iowrite32(-len, buf_pool->cmd);
+	pdata->ring_ops->wr_cmd(buf_pool, -len);
 	buf_pool->tail = tail;
 }
 
@@ -263,8 +258,8 @@ static netdev_tx_t xgene_enet_start_xmit(struct sk_buff *skb,
 	struct xgene_enet_desc_ring *cp_ring = tx_ring->cp_ring;
 	u32 tx_level, cq_level;
 
-	tx_level = xgene_enet_ring_len(tx_ring);
-	cq_level = xgene_enet_ring_len(cp_ring);
+	tx_level = pdata->ring_ops->len(tx_ring);
+	cq_level = pdata->ring_ops->len(cp_ring);
 	if (unlikely(tx_level > pdata->tx_qcnt_hi ||
 		     cq_level > pdata->cp_qcnt_hi)) {
 		netif_stop_queue(ndev);
@@ -276,7 +271,7 @@ static netdev_tx_t xgene_enet_start_xmit(struct sk_buff *skb,
 		return NETDEV_TX_OK;
 	}
 
-	iowrite32(1, tx_ring->cmd);
+	pdata->ring_ops->wr_cmd(tx_ring, 1);
 	skb_tx_timestamp(skb);
 	tx_ring->tail = (tx_ring->tail + 1) & (tx_ring->slots - 1);
 
@@ -389,11 +384,11 @@ static int xgene_enet_process_ring(struct xgene_enet_desc_ring *ring,
 	} while (--budget);
 
 	if (likely(count)) {
-		iowrite32(-count, ring->cmd);
+		pdata->ring_ops->wr_cmd(ring, -count);
 		ring->head = head;
 
 		if (netif_queue_stopped(ring->ndev)) {
-			if (xgene_enet_ring_len(ring) < pdata->cp_qcnt_low)
+			if (pdata->ring_ops->len(ring) < pdata->cp_qcnt_low)
 				netif_wake_queue(ring->ndev);
 		}
 	}
@@ -510,6 +505,7 @@ static int xgene_enet_open(struct net_device *ndev)
 	else
 		schedule_delayed_work(&pdata->link_work, PHY_POLL_LINK_OFF);
 
+	netif_carrier_off(ndev);
 	netif_start_queue(ndev);
 
 	return ret;
@@ -545,7 +541,7 @@ static void xgene_enet_delete_ring(struct xgene_enet_desc_ring *ring)
 	pdata = netdev_priv(ring->ndev);
 	dev = ndev_to_dev(ring->ndev);
 
-	xgene_enet_clear_ring(ring);
+	pdata->ring_ops->clear(ring);
 	dma_free_coherent(dev, ring->size, ring->desc_addr, ring->dma);
 }
 
@@ -598,15 +594,17 @@ static int xgene_enet_get_ring_size(struct device *dev,
 
 static void xgene_enet_free_desc_ring(struct xgene_enet_desc_ring *ring)
 {
+	struct xgene_enet_pdata *pdata;
 	struct device *dev;
 
 	if (!ring)
 		return;
 
 	dev = ndev_to_dev(ring->ndev);
+	pdata = netdev_priv(ring->ndev);
 
 	if (ring->desc_addr) {
-		xgene_enet_clear_ring(ring);
+		pdata->ring_ops->clear(ring);
 		dma_free_coherent(dev, ring->size, ring->desc_addr, ring->dma);
 	}
 	devm_kfree(dev, ring);
@@ -637,6 +635,25 @@ static void xgene_enet_free_desc_rings(struct xgene_enet_pdata *pdata)
 	}
 }
 
+static bool is_irq_mbox_required(struct xgene_enet_pdata *pdata,
+				 struct xgene_enet_desc_ring *ring)
+{
+	if ((pdata->enet_id == XGENE_ENET2) &&
+	    (xgene_enet_ring_owner(ring->id) == RING_OWNER_CPU)) {
+		return true;
+	}
+
+	return false;
+}
+
+static void __iomem *xgene_enet_ring_cmd_base(struct xgene_enet_pdata *pdata,
+					      struct xgene_enet_desc_ring *ring)
+{
+	u8 num_ring_id_shift = pdata->ring_ops->num_ring_id_shift;
+
+	return pdata->ring_cmd_addr + (ring->num << num_ring_id_shift);
+}
+
 static struct xgene_enet_desc_ring *xgene_enet_create_desc_ring(
 			struct net_device *ndev, u32 ring_num,
 			enum xgene_enet_ring_cfgsize cfgsize, u32 ring_id)
@@ -668,9 +685,20 @@ static struct xgene_enet_desc_ring *xgene_enet_create_desc_ring(
 	}
 	ring->size = size;
 
-	ring->cmd_base = pdata->ring_cmd_addr + (ring->num << 6);
+	if (is_irq_mbox_required(pdata, ring)) {
+		ring->irq_mbox_addr = dma_zalloc_coherent(dev, INTR_MBOX_SIZE,
+				&ring->irq_mbox_dma, GFP_KERNEL);
+		if (!ring->irq_mbox_addr) {
+			dma_free_coherent(dev, size, ring->desc_addr,
+					  ring->dma);
+			devm_kfree(dev, ring);
+			return NULL;
+		}
+	}
+
+	ring->cmd_base = xgene_enet_ring_cmd_base(pdata, ring);
 	ring->cmd = ring->cmd_base + INC_DEC_CMD_ADDR;
-	ring = xgene_enet_setup_ring(ring);
+	ring = pdata->ring_ops->setup(ring);
 	netdev_dbg(ndev, "ring info: num=%d  size=%d  id=%d  slots=%d\n",
 		   ring->num, ring->size, ring->id, ring->slots);
 
@@ -682,12 +710,34 @@ static u16 xgene_enet_get_ring_id(enum xgene_ring_owner owner, u8 bufnum)
 	return (owner << 6) | (bufnum & GENMASK(5, 0));
 }
 
+static enum xgene_ring_owner xgene_derive_ring_owner(struct xgene_enet_pdata *p)
+{
+	enum xgene_ring_owner owner;
+
+	if (p->enet_id == XGENE_ENET1) {
+		switch (p->phy_mode) {
+		case PHY_INTERFACE_MODE_SGMII:
+			owner = RING_OWNER_ETH0;
+			break;
+		default:
+			owner = (!p->port_id) ? RING_OWNER_ETH0 :
+						RING_OWNER_ETH1;
+			break;
+		}
+	} else {
+		owner = (!p->port_id) ? RING_OWNER_ETH0 : RING_OWNER_ETH1;
+	}
+
+	return owner;
+}
+
 static int xgene_enet_create_desc_rings(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
 	struct device *dev = ndev_to_dev(ndev);
 	struct xgene_enet_desc_ring *rx_ring, *tx_ring, *cp_ring;
 	struct xgene_enet_desc_ring *buf_pool = NULL;
+	enum xgene_ring_owner owner;
 	u8 cpu_bufnum = pdata->cpu_bufnum;
 	u8 eth_bufnum = pdata->eth_bufnum;
 	u8 bp_bufnum = pdata->bp_bufnum;
@@ -696,6 +746,7 @@ static int xgene_enet_create_desc_rings(struct net_device *ndev)
 	int ret;
 
 	/* allocate rx descriptor ring */
+	owner = xgene_derive_ring_owner(pdata);
 	ring_id = xgene_enet_get_ring_id(RING_OWNER_CPU, cpu_bufnum++);
 	rx_ring = xgene_enet_create_desc_ring(ndev, ring_num++,
 					      RING_CFGSIZE_16KB, ring_id);
@@ -705,7 +756,8 @@ static int xgene_enet_create_desc_rings(struct net_device *ndev)
 	}
 
 	/* allocate buffer pool for receiving packets */
-	ring_id = xgene_enet_get_ring_id(RING_OWNER_ETH0, bp_bufnum++);
+	owner = xgene_derive_ring_owner(pdata);
+	ring_id = xgene_enet_get_ring_id(owner, bp_bufnum++);
 	buf_pool = xgene_enet_create_desc_ring(ndev, ring_num++,
 					       RING_CFGSIZE_2KB, ring_id);
 	if (!buf_pool) {
@@ -734,7 +786,8 @@ static int xgene_enet_create_desc_rings(struct net_device *ndev)
 	pdata->rx_ring = rx_ring;
 
 	/* allocate tx descriptor ring */
-	ring_id = xgene_enet_get_ring_id(RING_OWNER_ETH0, eth_bufnum++);
+	owner = xgene_derive_ring_owner(pdata);
+	ring_id = xgene_enet_get_ring_id(owner, eth_bufnum++);
 	tx_ring = xgene_enet_create_desc_ring(ndev, ring_num++,
 					      RING_CFGSIZE_16KB, ring_id);
 	if (!tx_ring) {
@@ -818,20 +871,38 @@ static const struct net_device_ops xgene_ndev_ops = {
 	.ndo_set_mac_address = xgene_enet_set_mac_address,
 };
 
-static int xgene_get_port_id(struct device *dev, struct xgene_enet_pdata *pdata)
+#ifdef CONFIG_ACPI
+static int xgene_get_port_id_acpi(struct device *dev,
+				  struct xgene_enet_pdata *pdata)
+{
+	acpi_status status;
+	u64 temp;
+
+	status = acpi_evaluate_integer(ACPI_HANDLE(dev), "_SUN", NULL, &temp);
+	if (ACPI_FAILURE(status)) {
+		pdata->port_id = 0;
+	} else {
+		pdata->port_id = temp;
+	}
+
+	return 0;
+}
+#endif
+
+static int xgene_get_port_id_dt(struct device *dev, struct xgene_enet_pdata *pdata)
 {
 	u32 id = 0;
 	int ret;
 
-	ret = device_property_read_u32(dev, "port-id", &id);
-	if (!ret && id > 1) {
-		dev_err(dev, "Incorrect port-id specified\n");
-		return -ENODEV;
+	ret = of_property_read_u32(dev->of_node, "port-id", &id);
+	if (ret) {
+		pdata->port_id = 0;
+		ret = 0;
+	} else {
+		pdata->port_id = id & BIT(0);
 	}
 
-	pdata->port_id = id;
-
-	return 0;
+	return ret;
 }
 
 static int xgene_get_mac_address(struct device *dev,
@@ -876,7 +947,8 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 	struct device *dev;
 	struct resource *res;
 	void __iomem *base_addr;
-	int ret;
+	u32 offset;
+	int ret = 0;
 
 	pdev = pdata->pdev;
 	dev = &pdev->dev;
@@ -917,7 +989,12 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 		return -ENOMEM;
 	}
 
-	ret = xgene_get_port_id(dev, pdata);
+	if (dev->of_node)
+		ret = xgene_get_port_id_dt(dev, pdata);
+#ifdef CONFIG_ACPI
+	else
+		ret = xgene_get_port_id_acpi(dev, pdata);
+#endif
 	if (ret)
 		return ret;
 
@@ -949,27 +1026,35 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 	if (pdata->phy_mode != PHY_INTERFACE_MODE_RGMII) {
 		ret = platform_get_irq(pdev, 1);
 		if (ret <= 0) {
-			dev_err(dev, "Unable to get ENET Tx completion IRQ\n");
-			ret = ret ? : -ENXIO;
-			return ret;
+			pdata->cq_cnt = 0;
+			dev_info(dev, "Unable to get Tx completion IRQ,"
+				 "using Rx IRQ instead\n");
+		} else {
+			pdata->cq_cnt = XGENE_MAX_TXC_RINGS;
+			pdata->txc_irq = ret;
 		}
-		pdata->txc_irq = ret;
 	}
 
 	pdata->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(pdata->clk)) {
 		/* Firmware may have set up the clock already. */
-		pdata->clk = NULL;
+		dev_info(dev, "clocks have been setup already\n");
 	}
 
-	base_addr = pdata->base_addr - (pdata->port_id * MAC_OFFSET);
+	if (pdata->phy_mode != PHY_INTERFACE_MODE_XGMII)
+		base_addr = pdata->base_addr - (pdata->port_id * MAC_OFFSET);
+	else
+		base_addr = pdata->base_addr;
 	pdata->eth_csr_addr = base_addr + BLOCK_ETH_CSR_OFFSET;
 	pdata->eth_ring_if_addr = base_addr + BLOCK_ETH_RING_IF_OFFSET;
 	pdata->eth_diag_csr_addr = base_addr + BLOCK_ETH_DIAG_CSR_OFFSET;
 	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII ||
 	    pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
 		pdata->mcx_mac_addr = pdata->base_addr + BLOCK_ETH_MAC_OFFSET;
-		pdata->mcx_mac_csr_addr = base_addr + BLOCK_ETH_MAC_CSR_OFFSET;
+		offset = (pdata->enet_id == XGENE_ENET1) ?
+			  BLOCK_ETH_MAC_CSR_OFFSET :
+			  X2_BLOCK_ETH_MAC_CSR_OFFSET;
+		pdata->mcx_mac_csr_addr = base_addr + offset;
 	} else {
 		pdata->mcx_mac_addr = base_addr + BLOCK_AXG_MAC_OFFSET;
 		pdata->mcx_mac_csr_addr = base_addr + BLOCK_AXG_MAC_CSR_OFFSET;
@@ -1024,33 +1109,52 @@ static void xgene_enet_setup_ops(struct xgene_enet_pdata *pdata)
 		pdata->mac_ops = &xgene_sgmac_ops;
 		pdata->port_ops = &xgene_sgport_ops;
 		pdata->rm = RM1;
-		pdata->cq_cnt = XGENE_MAX_TXC_RINGS;
 		break;
 	default:
 		pdata->mac_ops = &xgene_xgmac_ops;
 		pdata->port_ops = &xgene_xgport_ops;
 		pdata->rm = RM0;
-		pdata->cq_cnt = XGENE_MAX_TXC_RINGS;
 		break;
 	}
 
-	switch (pdata->port_id) {
-	case 0:
-		pdata->cpu_bufnum = START_CPU_BUFNUM_0;
-		pdata->eth_bufnum = START_ETH_BUFNUM_0;
-		pdata->bp_bufnum = START_BP_BUFNUM_0;
-		pdata->ring_num = START_RING_NUM_0;
-		break;
-	case 1:
-		pdata->cpu_bufnum = START_CPU_BUFNUM_1;
-		pdata->eth_bufnum = START_ETH_BUFNUM_1;
-		pdata->bp_bufnum = START_BP_BUFNUM_1;
-		pdata->ring_num = START_RING_NUM_1;
-		break;
-	default:
-		break;
+	if (pdata->enet_id == XGENE_ENET1) {
+		switch (pdata->port_id) {
+		case 0:
+			pdata->cpu_bufnum = START_CPU_BUFNUM_0;
+			pdata->eth_bufnum = START_ETH_BUFNUM_0;
+			pdata->bp_bufnum = START_BP_BUFNUM_0;
+			pdata->ring_num = START_RING_NUM_0;
+			break;
+		case 1:
+			pdata->cpu_bufnum = START_CPU_BUFNUM_1;
+			pdata->eth_bufnum = START_ETH_BUFNUM_1;
+			pdata->bp_bufnum = START_BP_BUFNUM_1;
+			pdata->ring_num = START_RING_NUM_1;
+			break;
+		default:
+			break;
+		}
+		pdata->ring_ops = &xgene_ring1_ops;
+	} else {
+		switch (pdata->port_id) {
+		case 0:
+			pdata->cpu_bufnum = X2_START_CPU_BUFNUM_0;
+			pdata->eth_bufnum = X2_START_ETH_BUFNUM_0;
+			pdata->bp_bufnum = X2_START_BP_BUFNUM_0;
+			pdata->ring_num = X2_START_RING_NUM_0;
+			break;
+		case 1:
+			pdata->cpu_bufnum = X2_START_CPU_BUFNUM_1;
+			pdata->eth_bufnum = X2_START_ETH_BUFNUM_1;
+			pdata->bp_bufnum = X2_START_BP_BUFNUM_1;
+			pdata->ring_num = X2_START_RING_NUM_1;
+			break;
+		default:
+			break;
+		}
+		pdata->rm = RM0;
+		pdata->ring_ops = &xgene_ring2_ops;
 	}
-
 }
 
 static void xgene_enet_napi_add(struct xgene_enet_pdata *pdata)
@@ -1086,6 +1190,7 @@ static int xgene_enet_probe(struct platform_device *pdev)
 	struct xgene_enet_pdata *pdata;
 	struct device *dev = &pdev->dev;
 	struct xgene_mac_ops *mac_ops;
+	const struct of_device_id *of_id;
 	int ret;
 
 	ndev = alloc_etherdev(sizeof(struct xgene_enet_pdata));
@@ -1104,6 +1209,24 @@ static int xgene_enet_probe(struct platform_device *pdev)
 			  NETIF_F_GSO |
 			  NETIF_F_GRO;
 
+	of_id = of_match_device(xgene_enet_of_match, &pdev->dev);
+	if (of_id) {
+		pdata->enet_id = (enum xgene_enet_id)of_id->data;
+	}
+#ifdef CONFIG_ACPI
+	else {
+		const struct acpi_device_id *acpi_id;
+
+		acpi_id = acpi_match_device(xgene_enet_acpi_match, &pdev->dev);
+		if (acpi_id)
+			pdata->enet_id = (enum xgene_enet_id) acpi_id->driver_data;
+	}
+#endif
+	if (!pdata->enet_id) {
+		free_netdev(ndev);
+		return -ENODEV;
+	}
+
 	ret = xgene_enet_get_resources(pdata);
 	if (ret)
 		goto err;
@@ -1165,9 +1288,11 @@ static int xgene_enet_remove(struct platform_device *pdev)
 
 #ifdef CONFIG_ACPI
 static const struct acpi_device_id xgene_enet_acpi_match[] = {
-	{ "APMC0D05", },
-	{ "APMC0D30", },
-	{ "APMC0D31", },
+	{ "APMC0D05", XGENE_ENET1},
+	{ "APMC0D30", XGENE_ENET1},
+	{ "APMC0D31", XGENE_ENET1},
+	{ "APMC0D26", XGENE_ENET2},
+	{ "APMC0D25", XGENE_ENET2},
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, xgene_enet_acpi_match);
@@ -1175,9 +1300,11 @@ MODULE_DEVICE_TABLE(acpi, xgene_enet_acpi_match);
 
 #ifdef CONFIG_OF
 static const struct of_device_id xgene_enet_of_match[] = {
-	{.compatible = "apm,xgene-enet",},
-	{.compatible = "apm,xgene1-sgenet",},
-	{.compatible = "apm,xgene1-xgenet",},
+	{.compatible = "apm,xgene-enet",    .data = (void *)XGENE_ENET1},
+	{.compatible = "apm,xgene1-sgenet", .data = (void *)XGENE_ENET1},
+	{.compatible = "apm,xgene1-xgenet", .data = (void *)XGENE_ENET1},
+	{.compatible = "apm,xgene2-sgenet", .data = (void *)XGENE_ENET2},
+	{.compatible = "apm,xgene2-xgenet", .data = (void *)XGENE_ENET2},
 	{},
 };
 
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
index 8f3d232b09bc..1c85fc87703a 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
@@ -35,6 +35,7 @@
 #include <linux/if_vlan.h>
 #include <linux/phy.h>
 #include "xgene_enet_hw.h"
+#include "xgene_enet_ring2.h"
 
 #define XGENE_DRV_VERSION	"v1.0"
 #define XGENE_ENET_MAX_MTU	1536
@@ -51,12 +52,26 @@
 #define START_BP_BUFNUM_1	0x2A
 #define START_RING_NUM_1	264
 
+#define X2_START_CPU_BUFNUM_0	0
+#define X2_START_ETH_BUFNUM_0	0
+#define X2_START_BP_BUFNUM_0	0x20
+#define X2_START_RING_NUM_0	0
+#define X2_START_CPU_BUFNUM_1	0xc
+#define X2_START_ETH_BUFNUM_1	0
+#define X2_START_BP_BUFNUM_1	0x20
+#define X2_START_RING_NUM_1	256
+
 #define IRQ_ID_SIZE		16
 #define XGENE_MAX_TXC_RINGS	1
 
 #define PHY_POLL_LINK_ON	(10 * HZ)
 #define PHY_POLL_LINK_OFF	(PHY_POLL_LINK_ON / 5)
 
+enum xgene_enet_id {
+	XGENE_ENET1 = 1,
+	XGENE_ENET2
+};
+
 /* software context of a descriptor ring */
 struct xgene_enet_desc_ring {
 	struct net_device *ndev;
@@ -68,10 +83,12 @@ struct xgene_enet_desc_ring {
 	u16 irq;
 	char irq_name[IRQ_ID_SIZE];
 	u32 size;
-	u32 state[NUM_RING_CONFIG];
+	u32 state[X2_NUM_RING_CONFIG];
 	void __iomem *cmd_base;
 	void __iomem *cmd;
 	dma_addr_t dma;
+	dma_addr_t irq_mbox_dma;
+	void *irq_mbox_addr;
 	u16 dst_ring_num;
 	u8 nbufpool;
 	struct sk_buff *(*rx_skb);
@@ -105,6 +122,15 @@ struct xgene_port_ops {
 	void (*shutdown)(struct xgene_enet_pdata *pdata);
 };
 
+struct xgene_ring_ops {
+	u8 num_ring_config;
+	u8 num_ring_id_shift;
+	struct xgene_enet_desc_ring * (*setup)(struct xgene_enet_desc_ring *);
+	void (*clear)(struct xgene_enet_desc_ring *);
+	void (*wr_cmd)(struct xgene_enet_desc_ring *, int);
+	u32 (*len)(struct xgene_enet_desc_ring *);
+};
+
 /* ethernet private data */
 struct xgene_enet_pdata {
 	struct net_device *ndev;
@@ -113,6 +139,7 @@ struct xgene_enet_pdata {
 	int phy_speed;
 	struct clk *clk;
 	struct platform_device *pdev;
+	enum xgene_enet_id enet_id;
 	struct xgene_enet_desc_ring *tx_ring;
 	struct xgene_enet_desc_ring *rx_ring;
 	char *dev_name;
@@ -136,6 +163,7 @@ struct xgene_enet_pdata {
 	struct rtnl_link_stats64 stats;
 	struct xgene_mac_ops *mac_ops;
 	struct xgene_port_ops *port_ops;
+	struct xgene_ring_ops *ring_ops;
 	struct delayed_work link_work;
 	u32 port_id;
 	u8 cpu_bufnum;
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_ring2.c b/drivers/net/ethernet/apm/xgene/xgene_enet_ring2.c
new file mode 100644
index 000000000000..0b6896bb351e
--- /dev/null
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_ring2.c
@@ -0,0 +1,200 @@
+/* Applied Micro X-Gene SoC Ethernet Driver
+ *
+ * Copyright (c) 2015, Applied Micro Circuits Corporation
+ * Author: Iyappan Subramanian <isubramanian@apm.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "xgene_enet_main.h"
+#include "xgene_enet_hw.h"
+#include "xgene_enet_ring2.h"
+
+static void xgene_enet_ring_init(struct xgene_enet_desc_ring *ring)
+{
+	u32 *ring_cfg = ring->state;
+	u64 addr = ring->dma;
+
+	if (xgene_enet_ring_owner(ring->id) == RING_OWNER_CPU) {
+		ring_cfg[0] |= SET_VAL(X2_INTLINE, ring->id & RING_BUFNUM_MASK);
+		ring_cfg[3] |= SET_BIT(X2_DEQINTEN);
+	}
+	ring_cfg[0] |= SET_VAL(X2_CFGCRID, 1);
+
+	addr >>= 8;
+	ring_cfg[2] |= QCOHERENT | SET_VAL(RINGADDRL, addr);
+
+	addr >>= 27;
+	ring_cfg[3] |= SET_VAL(RINGSIZE, ring->cfgsize)
+		    | ACCEPTLERR
+		    | SET_VAL(RINGADDRH, addr);
+	ring_cfg[4] |= SET_VAL(X2_SELTHRSH, 1);
+	ring_cfg[5] |= SET_BIT(X2_QBASE_AM) | SET_BIT(X2_MSG_AM);
+}
+
+static void xgene_enet_ring_set_type(struct xgene_enet_desc_ring *ring)
+{
+	u32 *ring_cfg = ring->state;
+	bool is_bufpool;
+	u32 val;
+
+	is_bufpool = xgene_enet_is_bufpool(ring->id);
+	val = (is_bufpool) ? RING_BUFPOOL : RING_REGULAR;
+	ring_cfg[4] |= SET_VAL(X2_RINGTYPE, val);
+	if (is_bufpool)
+		ring_cfg[3] |= SET_VAL(RINGMODE, BUFPOOL_MODE);
+}
+
+static void xgene_enet_ring_set_recombbuf(struct xgene_enet_desc_ring *ring)
+{
+	u32 *ring_cfg = ring->state;
+
+	ring_cfg[3] |= RECOMBBUF;
+	ring_cfg[4] |= SET_VAL(X2_RECOMTIMEOUT, 0x7);
+}
+
+static void xgene_enet_ring_wr32(struct xgene_enet_desc_ring *ring,
+				 u32 offset, u32 data)
+{
+	struct xgene_enet_pdata *pdata = netdev_priv(ring->ndev);
+
+	iowrite32(data, pdata->ring_csr_addr + offset);
+}
+
+static void xgene_enet_write_ring_state(struct xgene_enet_desc_ring *ring)
+{
+	struct xgene_enet_pdata *pdata = netdev_priv(ring->ndev);
+	int i;
+
+	xgene_enet_ring_wr32(ring, CSR_RING_CONFIG, ring->num);
+	for (i = 0; i < pdata->ring_ops->num_ring_config; i++) {
+		xgene_enet_ring_wr32(ring, CSR_RING_WR_BASE + (i * 4),
+				     ring->state[i]);
+	}
+}
+
+static void xgene_enet_clr_ring_state(struct xgene_enet_desc_ring *ring)
+{
+	memset(ring->state, 0, sizeof(ring->state));
+	xgene_enet_write_ring_state(ring);
+}
+
+static void xgene_enet_set_ring_state(struct xgene_enet_desc_ring *ring)
+{
+	enum xgene_ring_owner owner;
+
+	xgene_enet_ring_set_type(ring);
+
+	owner = xgene_enet_ring_owner(ring->id);
+	if (owner == RING_OWNER_ETH0 || owner == RING_OWNER_ETH1)
+		xgene_enet_ring_set_recombbuf(ring);
+
+	xgene_enet_ring_init(ring);
+	xgene_enet_write_ring_state(ring);
+}
+
+static void xgene_enet_set_ring_id(struct xgene_enet_desc_ring *ring)
+{
+	u32 ring_id_val, ring_id_buf;
+	bool is_bufpool;
+
+	if (xgene_enet_ring_owner(ring->id) == RING_OWNER_CPU)
+		return;
+
+	is_bufpool = xgene_enet_is_bufpool(ring->id);
+
+	ring_id_val = ring->id & GENMASK(9, 0);
+	ring_id_val |= OVERWRITE;
+
+	ring_id_buf = (ring->num << 9) & GENMASK(18, 9);
+	ring_id_buf |= PREFETCH_BUF_EN;
+	if (is_bufpool)
+		ring_id_buf |= IS_BUFFER_POOL;
+
+	xgene_enet_ring_wr32(ring, CSR_RING_ID, ring_id_val);
+	xgene_enet_ring_wr32(ring, CSR_RING_ID_BUF, ring_id_buf);
+}
+
+static void xgene_enet_clr_desc_ring_id(struct xgene_enet_desc_ring *ring)
+{
+	u32 ring_id;
+
+	ring_id = ring->id | OVERWRITE;
+	xgene_enet_ring_wr32(ring, CSR_RING_ID, ring_id);
+	xgene_enet_ring_wr32(ring, CSR_RING_ID_BUF, 0);
+}
+
+static struct xgene_enet_desc_ring *xgene_enet_setup_ring(
+				    struct xgene_enet_desc_ring *ring)
+{
+	bool is_bufpool;
+	u32 addr, i;
+
+	xgene_enet_clr_ring_state(ring);
+	xgene_enet_set_ring_state(ring);
+	xgene_enet_set_ring_id(ring);
+
+	ring->slots = xgene_enet_get_numslots(ring->id, ring->size);
+
+	is_bufpool = xgene_enet_is_bufpool(ring->id);
+	if (is_bufpool || xgene_enet_ring_owner(ring->id) != RING_OWNER_CPU)
+		return ring;
+
+	addr = CSR_VMID0_INTR_MBOX + (4 * (ring->id & RING_BUFNUM_MASK));
+	xgene_enet_ring_wr32(ring, addr, ring->irq_mbox_dma >> 10);
+
+	for (i = 0; i < ring->slots; i++)
+		xgene_enet_mark_desc_slot_empty(&ring->raw_desc[i]);
+
+	return ring;
+}
+
+static void xgene_enet_clear_ring(struct xgene_enet_desc_ring *ring)
+{
+	xgene_enet_clr_desc_ring_id(ring);
+	xgene_enet_clr_ring_state(ring);
+}
+
+static void xgene_enet_wr_cmd(struct xgene_enet_desc_ring *ring, int count)
+{
+	u32 data = 0;
+
+	if (xgene_enet_ring_owner(ring->id) == RING_OWNER_CPU) {
+		data = SET_VAL(X2_INTLINE, ring->id & RING_BUFNUM_MASK) |
+		       INTR_CLEAR;
+	}
+	data |= (count & GENMASK(16, 0));
+
+	iowrite32(data, ring->cmd);
+}
+
+static u32 xgene_enet_ring_len(struct xgene_enet_desc_ring *ring)
+{
+	u32 __iomem *cmd_base = ring->cmd_base;
+	u32 ring_state, num_msgs;
+
+	ring_state = ioread32(&cmd_base[1]);
+	num_msgs = GET_VAL(X2_NUMMSGSINQ, ring_state);
+
+	return num_msgs;
+}
+
+struct xgene_ring_ops xgene_ring2_ops = {
+	.num_ring_config = X2_NUM_RING_CONFIG,
+	.num_ring_id_shift = 13,
+	.setup = xgene_enet_setup_ring,
+	.clear = xgene_enet_clear_ring,
+	.wr_cmd = xgene_enet_wr_cmd,
+	.len = xgene_enet_ring_len,
+};
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_ring2.h b/drivers/net/ethernet/apm/xgene/xgene_enet_ring2.h
new file mode 100644
index 000000000000..8b235db23c42
--- /dev/null
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_ring2.h
@@ -0,0 +1,49 @@
+/* Applied Micro X-Gene SoC Ethernet Driver
+ *
+ * Copyright (c) 2015, Applied Micro Circuits Corporation
+ * Author: Iyappan Subramanian <isubramanian@apm.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __XGENE_ENET_RING2_H__
+#define __XGENE_ENET_RING2_H__
+
+#include "xgene_enet_main.h"
+
+#define X2_NUM_RING_CONFIG	6
+
+#define INTR_MBOX_SIZE		1024
+#define CSR_VMID0_INTR_MBOX	0x0270
+#define INTR_CLEAR		BIT(23)
+
+#define X2_MSG_AM_POS		10
+#define X2_QBASE_AM_POS		11
+#define X2_INTLINE_POS		24
+#define X2_INTLINE_LEN		5
+#define X2_CFGCRID_POS		29
+#define X2_CFGCRID_LEN		3
+#define X2_SELTHRSH_POS		7
+#define X2_SELTHRSH_LEN		3
+#define X2_RINGTYPE_POS		23
+#define X2_RINGTYPE_LEN		2
+#define X2_DEQINTEN_POS		29
+#define X2_RECOMTIMEOUT_POS	0
+#define X2_RECOMTIMEOUT_LEN	7
+#define X2_NUMMSGSINQ_POS	0
+#define X2_NUMMSGSINQ_LEN	17
+
+extern struct xgene_ring_ops xgene_ring2_ops;
+
+#endif /* __XGENE_ENET_RING2_H__ */
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
index f27fb6f2a93b..05b817e56fde 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
@@ -21,6 +21,7 @@
 #include "xgene_enet_main.h"
 #include "xgene_enet_hw.h"
 #include "xgene_enet_sgmac.h"
+#include "xgene_enet_xgmac.h"
 
 static void xgene_enet_wr_csr(struct xgene_enet_pdata *p, u32 offset, u32 val)
 {
@@ -39,6 +40,14 @@ static void xgene_enet_wr_diag_csr(struct xgene_enet_pdata *p,
 	iowrite32(val, p->eth_diag_csr_addr + offset);
 }
 
+static void xgene_enet_wr_mcx_csr(struct xgene_enet_pdata *pdata,
+				  u32 offset, u32 val)
+{
+	void __iomem *addr = pdata->mcx_mac_csr_addr + offset;
+
+	iowrite32(val, addr);
+}
+
 static bool xgene_enet_wr_indirect(struct xgene_indirect_ctl *ctl,
 				   u32 wr_addr, u32 wr_data)
 {
@@ -140,8 +149,9 @@ static int xgene_enet_ecc_init(struct xgene_enet_pdata *p)
 
 static void xgene_enet_config_ring_if_assoc(struct xgene_enet_pdata *p)
 {
-	u32 val = 0xffffffff;
+	u32 val;
 
+	val = (p->enet_id == XGENE_ENET1) ? 0xffffffff : 0;
 	xgene_enet_wr_ring_if(p, ENET_CFGSSQMIWQASSOC_ADDR, val);
 	xgene_enet_wr_ring_if(p, ENET_CFGSSQMIFPQASSOC_ADDR, val);
 }
@@ -227,6 +237,8 @@ static void xgene_sgmac_init(struct xgene_enet_pdata *p)
 {
 	u32 data, loop = 10;
 	u32 offset = p->port_id * 4;
+	u32 enet_spare_cfg_reg, rsif_config_reg;
+	u32 cfg_bypass_reg, rx_dv_gate_reg;
 
 	xgene_sgmac_reset(p);
 
@@ -239,7 +251,7 @@ static void xgene_sgmac_init(struct xgene_enet_pdata *p)
 					  SGMII_STATUS_ADDR >> 2);
 		if ((data & AUTO_NEG_COMPLETE) && (data & LINK_STATUS))
 			break;
-		usleep_range(10, 20);
+		usleep_range(1000, 2000);
 	}
 	if (!(data & AUTO_NEG_COMPLETE) || !(data & LINK_STATUS))
 		netdev_err(p->ndev, "Auto-negotiation failed\n");
@@ -249,33 +261,38 @@ static void xgene_sgmac_init(struct xgene_enet_pdata *p)
 	xgene_enet_wr_mac(p, MAC_CONFIG_2_ADDR, data | FULL_DUPLEX2);
 	xgene_enet_wr_mac(p, INTERFACE_CONTROL_ADDR, ENET_GHD_MODE);
 
-	data = xgene_enet_rd_csr(p, ENET_SPARE_CFG_REG_ADDR);
+	if (p->enet_id == XGENE_ENET1) {
+		enet_spare_cfg_reg = ENET_SPARE_CFG_REG_ADDR;
+		rsif_config_reg = RSIF_CONFIG_REG_ADDR;
+		cfg_bypass_reg = CFG_BYPASS_ADDR;
+		rx_dv_gate_reg = SG_RX_DV_GATE_REG_0_ADDR;
+	} else {
+		enet_spare_cfg_reg = XG_ENET_SPARE_CFG_REG_ADDR;
+		rsif_config_reg = XG_RSIF_CONFIG_REG_ADDR;
+		cfg_bypass_reg = XG_CFG_BYPASS_ADDR;
+		rx_dv_gate_reg = XG_MCX_RX_DV_GATE_REG_0_ADDR;
+	}
+
+	data = xgene_enet_rd_csr(p, enet_spare_cfg_reg);
 	data |= MPA_IDLE_WITH_QMI_EMPTY;
-	xgene_enet_wr_csr(p, ENET_SPARE_CFG_REG_ADDR, data);
+	xgene_enet_wr_csr(p, enet_spare_cfg_reg, data);
 
 	xgene_sgmac_set_mac_addr(p);
 
-	data = xgene_enet_rd_csr(p, DEBUG_REG_ADDR);
-	data |= CFG_BYPASS_UNISEC_TX | CFG_BYPASS_UNISEC_RX;
-	xgene_enet_wr_csr(p, DEBUG_REG_ADDR, data);
-
 	/* Adjust MDC clock frequency */
 	data = xgene_enet_rd_mac(p, MII_MGMT_CONFIG_ADDR);
 	MGMT_CLOCK_SEL_SET(&data, 7);
 	xgene_enet_wr_mac(p, MII_MGMT_CONFIG_ADDR, data);
 
 	/* Enable drop if bufpool not available */
-	data = xgene_enet_rd_csr(p, RSIF_CONFIG_REG_ADDR);
+	data = xgene_enet_rd_csr(p, rsif_config_reg);
 	data |= CFG_RSIF_FPBUFF_TIMEOUT_EN;
-	xgene_enet_wr_csr(p, RSIF_CONFIG_REG_ADDR, data);
-
-	/* Rtype should be copied from FP */
-	xgene_enet_wr_csr(p, RSIF_RAM_DBG_REG0_ADDR, 0);
+	xgene_enet_wr_csr(p, rsif_config_reg, data);
 
 	/* Bypass traffic gating */
-	xgene_enet_wr_csr(p, CFG_LINK_AGGR_RESUME_0_ADDR + offset, TX_PORT0);
-	xgene_enet_wr_csr(p, CFG_BYPASS_ADDR, RESUME_TX);
-	xgene_enet_wr_csr(p, SG_RX_DV_GATE_REG_0_ADDR + offset, RESUME_RX0);
+	xgene_enet_wr_csr(p, XG_ENET_SPARE_CFG_REG_1_ADDR, 0x84);
+	xgene_enet_wr_csr(p, cfg_bypass_reg, RESUME_TX);
+	xgene_enet_wr_mcx_csr(p, rx_dv_gate_reg + offset, RESUME_RX0);
 }
 
 static void xgene_sgmac_rxtx(struct xgene_enet_pdata *p, u32 bits, bool set)
@@ -317,9 +334,11 @@ static int xgene_enet_reset(struct xgene_enet_pdata *p)
 	if (!xgene_ring_mgr_init(p))
 		return -ENODEV;
 
-	clk_prepare_enable(p->clk);
-	clk_disable_unprepare(p->clk);
-	clk_prepare_enable(p->clk);
+	if (!IS_ERR(p->clk)) {
+		clk_prepare_enable(p->clk);
+		clk_disable_unprepare(p->clk);
+		clk_prepare_enable(p->clk);
+	}
 
 	xgene_enet_ecc_init(p);
 	xgene_enet_config_ring_if_assoc(p);
@@ -331,19 +350,29 @@ static void xgene_enet_cle_bypass(struct xgene_enet_pdata *p,
 				  u32 dst_ring_num, u16 bufpool_id)
 {
 	u32 data, fpsel;
+	u32 cle_bypass_reg0, cle_bypass_reg1;
 	u32 offset = p->port_id * MAC_OFFSET;
 
+	if (p->enet_id == XGENE_ENET1) {
+		cle_bypass_reg0 = CLE_BYPASS_REG0_0_ADDR;
+		cle_bypass_reg1 = CLE_BYPASS_REG1_0_ADDR;
+	} else {
+		cle_bypass_reg0 = XCLE_BYPASS_REG0_ADDR;
+		cle_bypass_reg1 = XCLE_BYPASS_REG1_ADDR;
+	}
+
 	data = CFG_CLE_BYPASS_EN0;
-	xgene_enet_wr_csr(p, CLE_BYPASS_REG0_0_ADDR + offset, data);
+	xgene_enet_wr_csr(p, cle_bypass_reg0 + offset, data);
 
 	fpsel = xgene_enet_ring_bufnum(bufpool_id) - 0x20;
 	data = CFG_CLE_DSTQID0(dst_ring_num) | CFG_CLE_FPSEL0(fpsel);
-	xgene_enet_wr_csr(p, CLE_BYPASS_REG1_0_ADDR + offset, data);
+	xgene_enet_wr_csr(p, cle_bypass_reg1 + offset, data);
 }
 
 static void xgene_enet_shutdown(struct xgene_enet_pdata *p)
 {
-	clk_disable_unprepare(p->clk);
+	if (!IS_ERR(p->clk))
+		clk_disable_unprepare(p->clk);
 }
 
 static void xgene_enet_link_state(struct work_struct *work)
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
index a18a9d1f1143..05edb847cf26 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
@@ -122,7 +122,6 @@ static bool xgene_enet_rd_indirect(void __iomem *addr, void __iomem *rd,
 
 	return true;
 }
-
 static void xgene_enet_rd_mac(struct xgene_enet_pdata *pdata,
 			      u32 rd_addr, u32 *rd_data)
 {
@@ -257,9 +256,11 @@ static int xgene_enet_reset(struct xgene_enet_pdata *pdata)
 	if (!xgene_ring_mgr_init(pdata))
 		return -ENODEV;
 
-	clk_prepare_enable(pdata->clk);
-	clk_disable_unprepare(pdata->clk);
-	clk_prepare_enable(pdata->clk);
+	if (!IS_ERR(pdata->clk)) {
+		clk_prepare_enable(pdata->clk);
+		clk_disable_unprepare(pdata->clk);
+		clk_prepare_enable(pdata->clk);
+	}
 
 	xgene_enet_ecc_init(pdata);
 	xgene_enet_config_ring_if_assoc(pdata);
@@ -286,7 +287,8 @@ static void xgene_enet_xgcle_bypass(struct xgene_enet_pdata *pdata,
 
 static void xgene_enet_shutdown(struct xgene_enet_pdata *pdata)
 {
-	clk_disable_unprepare(pdata->clk);
+	if (!IS_ERR(pdata->clk))
+		clk_disable_unprepare(pdata->clk);
 }
 
 static void xgene_enet_link_state(struct work_struct *work)
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
index 5a5296a6d1df..bf0a99435737 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
@@ -21,9 +21,28 @@
 #ifndef __XGENE_ENET_XGMAC_H__
 #define __XGENE_ENET_XGMAC_H__
 
+#define X2_BLOCK_ETH_MAC_CSR_OFFSET	0x3000
 #define BLOCK_AXG_MAC_OFFSET		0x0800
 #define BLOCK_AXG_MAC_CSR_OFFSET	0x2000
 
+#define XGENET_CONFIG_REG_ADDR		0x20
+#define XGENET_SRST_ADDR		0x00
+#define XGENET_CLKEN_ADDR		0x08
+
+#define CSR_CLK		BIT(0)
+#define XGENET_CLK	BIT(1)
+#define PCS_CLK		BIT(3)
+#define AN_REF_CLK	BIT(4)
+#define AN_CLK		BIT(5)
+#define AD_CLK		BIT(6)
+
+#define CSR_RST		BIT(0)
+#define XGENET_RST	BIT(1)
+#define PCS_RST		BIT(3)
+#define AN_REF_RST	BIT(4)
+#define AN_RST		BIT(5)
+#define AD_RST		BIT(6)
+
 #define AXGMAC_CONFIG_0			0x0000
 #define AXGMAC_CONFIG_1			0x0004
 #define HSTMACRST			BIT(31)
@@ -38,6 +57,7 @@
 #define HSTMACADR_MSW_ADDR		0x0014
 #define HSTMAXFRAME_LENGTH_ADDR		0x0020
 
+#define XG_MCX_RX_DV_GATE_REG_0_ADDR	0x0004
 #define XG_RSIF_CONFIG_REG_ADDR		0x00a0
 #define XCLE_BYPASS_REG0_ADDR           0x0160
 #define XCLE_BYPASS_REG1_ADDR           0x0164
diff --git a/drivers/net/ethernet/apple/Kconfig b/drivers/net/ethernet/apple/Kconfig
index 1375e2dc9468..d19a41b0c6d2 100644
--- a/drivers/net/ethernet/apple/Kconfig
+++ b/drivers/net/ethernet/apple/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_APPLE
 	default y
 	depends on (PPC_PMAC && PPC32) || MAC
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -59,7 +57,6 @@ config MACMACE
 	---help---
 	  Support for the onboard AMD 79C940 MACE Ethernet controller used in
 	  the 660AV and 840AV Macintosh.  If you have one of these Macintoshes
-	  say Y and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  say Y here.
 
 endif # NET_VENDOR_APPLE
diff --git a/drivers/net/ethernet/arc/Kconfig b/drivers/net/ethernet/arc/Kconfig
index dea29ee24da4..52a6b16f57d2 100644
--- a/drivers/net/ethernet/arc/Kconfig
+++ b/drivers/net/ethernet/arc/Kconfig
@@ -6,9 +6,7 @@ config NET_VENDOR_ARC
 	bool "ARC devices"
 	default y
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/atheros/Kconfig b/drivers/net/ethernet/atheros/Kconfig
index 58ad37c733bc..e05b25675333 100644
--- a/drivers/net/ethernet/atheros/Kconfig
+++ b/drivers/net/ethernet/atheros/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_ATHEROS
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/broadcom/Kconfig b/drivers/net/ethernet/broadcom/Kconfig
index a6f9142b9048..8be9eab73320 100644
--- a/drivers/net/ethernet/broadcom/Kconfig
+++ b/drivers/net/ethernet/broadcom/Kconfig
@@ -26,8 +26,7 @@ config B44
 	select PHYLIB
 	---help---
 	  If you have a network (Ethernet) controller of this type, say Y
-	  or M and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  or M here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called b44.
diff --git a/drivers/net/ethernet/broadcom/b44.c b/drivers/net/ethernet/broadcom/b44.c
index 77363d680532..a3b1c07ae0af 100644
--- a/drivers/net/ethernet/broadcom/b44.c
+++ b/drivers/net/ethernet/broadcom/b44.c
@@ -2464,6 +2464,7 @@ err_out_powerdown:
 	ssb_bus_may_powerdown(sdev->bus);
 
 err_out_free_dev:
+	netif_napi_del(&bp->napi);
 	free_netdev(dev);
 
 out:
@@ -2480,6 +2481,7 @@ static void b44_remove_one(struct ssb_device *sdev)
 		b44_unregister_phy_one(bp);
 	ssb_device_disable(sdev, 0);
 	ssb_bus_may_powerdown(sdev->bus);
+	netif_napi_del(&bp->napi);
 	free_netdev(dev);
 	ssb_pcihost_set_power_state(sdev, PCI_D3hot);
 	ssb_set_drvdata(sdev, NULL);
diff --git a/drivers/net/ethernet/broadcom/b44.h b/drivers/net/ethernet/broadcom/b44.h
index 3e9c3fc7591b..65d88d7c5581 100644
--- a/drivers/net/ethernet/broadcom/b44.h
+++ b/drivers/net/ethernet/broadcom/b44.h
@@ -1,6 +1,8 @@
 #ifndef _B44_H
 #define _B44_H
 
+#include <linux/brcmphy.h>
+
 /* Register layout. (These correspond to struct _bcmenettregs in bcm4400.) */
 #define	B44_DEVCTRL	0x0000UL /* Device Control */
 #define  DEVCTRL_MPM		0x00000040 /* Magic Packet PME Enable (B0 only) */
@@ -281,8 +283,10 @@ struct ring_info {
 };
 
 #define B44_MCAST_TABLE_SIZE		32
-#define B44_PHY_ADDR_NO_LOCAL_PHY	30 /* no local phy regs */
-#define B44_PHY_ADDR_NO_PHY		31 /* no phy present at all */
+/* no local phy regs, e.g: Broadcom switches pseudo-PHY */
+#define B44_PHY_ADDR_NO_LOCAL_PHY	BRCM_PSEUDO_PHY_ADDR
+/* no phy present at all */
+#define B44_PHY_ADDR_NO_PHY		31
 #define B44_MDC_RATIO			5000000
 
 #define	B44_STAT_REG_DECLARE		\
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index 783543ad1fcf..909ad7a0d480 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -456,6 +456,67 @@ static int bcm_sysport_set_wol(struct net_device *dev,
 	return 0;
 }
 
+static int bcm_sysport_get_coalesce(struct net_device *dev,
+				    struct ethtool_coalesce *ec)
+{
+	struct bcm_sysport_priv *priv = netdev_priv(dev);
+	u32 reg;
+
+	reg = tdma_readl(priv, TDMA_DESC_RING_INTR_CONTROL(0));
+
+	ec->tx_coalesce_usecs = (reg >> RING_TIMEOUT_SHIFT) * 8192 / 1000;
+	ec->tx_max_coalesced_frames = reg & RING_INTR_THRESH_MASK;
+
+	reg = rdma_readl(priv, RDMA_MBDONE_INTR);
+
+	ec->rx_coalesce_usecs = (reg >> RDMA_TIMEOUT_SHIFT) * 8192 / 1000;
+	ec->rx_max_coalesced_frames = reg & RDMA_INTR_THRESH_MASK;
+
+	return 0;
+}
+
+static int bcm_sysport_set_coalesce(struct net_device *dev,
+				    struct ethtool_coalesce *ec)
+{
+	struct bcm_sysport_priv *priv = netdev_priv(dev);
+	unsigned int i;
+	u32 reg;
+
+	/* Base system clock is 125Mhz, DMA timeout is this reference clock
+	 * divided by 1024, which yield roughly 8.192 us, our maximum value has
+	 * to fit in the RING_TIMEOUT_MASK (16 bits).
+	 */
+	if (ec->tx_max_coalesced_frames > RING_INTR_THRESH_MASK ||
+	    ec->tx_coalesce_usecs > (RING_TIMEOUT_MASK * 8) + 1 ||
+	    ec->rx_max_coalesced_frames > RDMA_INTR_THRESH_MASK ||
+	    ec->rx_coalesce_usecs > (RDMA_TIMEOUT_MASK * 8) + 1)
+		return -EINVAL;
+
+	if ((ec->tx_coalesce_usecs == 0 && ec->tx_max_coalesced_frames == 0) ||
+	    (ec->rx_coalesce_usecs == 0 && ec->rx_max_coalesced_frames == 0))
+		return -EINVAL;
+
+	for (i = 0; i < dev->num_tx_queues; i++) {
+		reg = tdma_readl(priv, TDMA_DESC_RING_INTR_CONTROL(i));
+		reg &= ~(RING_INTR_THRESH_MASK |
+			 RING_TIMEOUT_MASK << RING_TIMEOUT_SHIFT);
+		reg |= ec->tx_max_coalesced_frames;
+		reg |= DIV_ROUND_UP(ec->tx_coalesce_usecs * 1000, 8192) <<
+			 RING_TIMEOUT_SHIFT;
+		tdma_writel(priv, reg, TDMA_DESC_RING_INTR_CONTROL(i));
+	}
+
+	reg = rdma_readl(priv, RDMA_MBDONE_INTR);
+	reg &= ~(RDMA_INTR_THRESH_MASK |
+		 RDMA_TIMEOUT_MASK << RDMA_TIMEOUT_SHIFT);
+	reg |= ec->rx_max_coalesced_frames;
+	reg |= DIV_ROUND_UP(ec->rx_coalesce_usecs * 1000, 8192) <<
+			    RDMA_TIMEOUT_SHIFT;
+	rdma_writel(priv, reg, RDMA_MBDONE_INTR);
+
+	return 0;
+}
+
 static void bcm_sysport_free_cb(struct bcm_sysport_cb *cb)
 {
 	dev_kfree_skb_any(cb->skb);
@@ -463,67 +524,70 @@ static void bcm_sysport_free_cb(struct bcm_sysport_cb *cb)
 	dma_unmap_addr_set(cb, dma_addr, 0);
 }
 
-static int bcm_sysport_rx_refill(struct bcm_sysport_priv *priv,
-				 struct bcm_sysport_cb *cb)
+static struct sk_buff *bcm_sysport_rx_refill(struct bcm_sysport_priv *priv,
+					     struct bcm_sysport_cb *cb)
 {
 	struct device *kdev = &priv->pdev->dev;
 	struct net_device *ndev = priv->netdev;
+	struct sk_buff *skb, *rx_skb;
 	dma_addr_t mapping;
-	int ret;
 
-	cb->skb = netdev_alloc_skb(priv->netdev, RX_BUF_LENGTH);
-	if (!cb->skb) {
+	/* Allocate a new SKB for a new packet */
+	skb = netdev_alloc_skb(priv->netdev, RX_BUF_LENGTH);
+	if (!skb) {
+		priv->mib.alloc_rx_buff_failed++;
 		netif_err(priv, rx_err, ndev, "SKB alloc failed\n");
-		return -ENOMEM;
+		return NULL;
 	}
 
-	mapping = dma_map_single(kdev, cb->skb->data,
+	mapping = dma_map_single(kdev, skb->data,
 				 RX_BUF_LENGTH, DMA_FROM_DEVICE);
-	ret = dma_mapping_error(kdev, mapping);
-	if (ret) {
+	if (dma_mapping_error(kdev, mapping)) {
 		priv->mib.rx_dma_failed++;
-		bcm_sysport_free_cb(cb);
+		dev_kfree_skb_any(skb);
 		netif_err(priv, rx_err, ndev, "DMA mapping failure\n");
-		return ret;
+		return NULL;
 	}
 
-	dma_unmap_addr_set(cb, dma_addr, mapping);
-	dma_desc_set_addr(priv, priv->rx_bd_assign_ptr, mapping);
+	/* Grab the current SKB on the ring */
+	rx_skb = cb->skb;
+	if (likely(rx_skb))
+		dma_unmap_single(kdev, dma_unmap_addr(cb, dma_addr),
+				 RX_BUF_LENGTH, DMA_FROM_DEVICE);
 
-	priv->rx_bd_assign_index++;
-	priv->rx_bd_assign_index &= (priv->num_rx_bds - 1);
-	priv->rx_bd_assign_ptr = priv->rx_bds +
-		(priv->rx_bd_assign_index * DESC_SIZE);
+	/* Put the new SKB on the ring */
+	cb->skb = skb;
+	dma_unmap_addr_set(cb, dma_addr, mapping);
+	dma_desc_set_addr(priv, cb->bd_addr, mapping);
 
 	netif_dbg(priv, rx_status, ndev, "RX refill\n");
 
-	return 0;
+	/* Return the current SKB to the caller */
+	return rx_skb;
 }
 
 static int bcm_sysport_alloc_rx_bufs(struct bcm_sysport_priv *priv)
 {
 	struct bcm_sysport_cb *cb;
-	int ret = 0;
+	struct sk_buff *skb;
 	unsigned int i;
 
 	for (i = 0; i < priv->num_rx_bds; i++) {
-		cb = &priv->rx_cbs[priv->rx_bd_assign_index];
-		if (cb->skb)
-			continue;
-
-		ret = bcm_sysport_rx_refill(priv, cb);
-		if (ret)
-			break;
+		cb = &priv->rx_cbs[i];
+		skb = bcm_sysport_rx_refill(priv, cb);
+		if (skb)
+			dev_kfree_skb(skb);
+		if (!cb->skb)
+			return -ENOMEM;
 	}
 
-	return ret;
+	return 0;
 }
 
 /* Poll the hardware for up to budget packets to process */
 static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 					unsigned int budget)
 {
-	struct device *kdev = &priv->pdev->dev;
 	struct net_device *ndev = priv->netdev;
 	unsigned int processed = 0, to_process;
 	struct bcm_sysport_cb *cb;
@@ -531,7 +595,6 @@ static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 	unsigned int p_index;
 	u16 len, status;
 	struct bcm_rsb *rsb;
-	int ret;
 
 	/* Determine how much we should process since last call */
 	p_index = rdma_readl(priv, RDMA_PROD_INDEX);
@@ -549,13 +612,8 @@ static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 
 	while ((processed < to_process) && (processed < budget)) {
 		cb = &priv->rx_cbs[priv->rx_read_ptr];
-		skb = cb->skb;
+		skb = bcm_sysport_rx_refill(priv, cb);
 
-		processed++;
-		priv->rx_read_ptr++;
-
-		if (priv->rx_read_ptr == priv->num_rx_bds)
-			priv->rx_read_ptr = 0;
 
 		/* We do not have a backing SKB, so we do not a corresponding
 		 * DMA mapping for this incoming packet since
@@ -566,12 +624,9 @@ static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 			netif_err(priv, rx_err, ndev, "out of memory!\n");
 			ndev->stats.rx_dropped++;
 			ndev->stats.rx_errors++;
-			goto refill;
+			goto next;
 		}
 
-		dma_unmap_single(kdev, dma_unmap_addr(cb, dma_addr),
-				 RX_BUF_LENGTH, DMA_FROM_DEVICE);
-
 		/* Extract the Receive Status Block prepended */
 		rsb = (struct bcm_rsb *)skb->data;
 		len = (rsb->rx_status_len >> DESC_LEN_SHIFT) & DESC_LEN_MASK;
@@ -583,12 +638,20 @@ static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 			  p_index, priv->rx_c_index, priv->rx_read_ptr,
 			  len, status);
 
+		if (unlikely(len > RX_BUF_LENGTH)) {
+			netif_err(priv, rx_status, ndev, "oversized packet\n");
+			ndev->stats.rx_length_errors++;
+			ndev->stats.rx_errors++;
+			dev_kfree_skb_any(skb);
+			goto next;
+		}
+
 		if (unlikely(!(status & DESC_EOP) || !(status & DESC_SOP))) {
 			netif_err(priv, rx_status, ndev, "fragmented packet!\n");
 			ndev->stats.rx_dropped++;
 			ndev->stats.rx_errors++;
-			bcm_sysport_free_cb(cb);
-			goto refill;
+			dev_kfree_skb_any(skb);
+			goto next;
 		}
 
 		if (unlikely(status & (RX_STATUS_ERR | RX_STATUS_OVFLOW))) {
@@ -597,8 +660,8 @@ static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 				ndev->stats.rx_over_errors++;
 			ndev->stats.rx_dropped++;
 			ndev->stats.rx_errors++;
-			bcm_sysport_free_cb(cb);
-			goto refill;
+			dev_kfree_skb_any(skb);
+			goto next;
 		}
 
 		skb_put(skb, len);
@@ -625,10 +688,12 @@ static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 		ndev->stats.rx_bytes += len;
 
 		napi_gro_receive(&priv->napi, skb);
-refill:
-		ret = bcm_sysport_rx_refill(priv, cb);
-		if (ret)
-			priv->mib.alloc_rx_buff_failed++;
+next:
+		processed++;
+		priv->rx_read_ptr++;
+
+		if (priv->rx_read_ptr == priv->num_rx_bds)
+			priv->rx_read_ptr = 0;
 	}
 
 	return processed;
@@ -1269,14 +1334,14 @@ static inline int tdma_enable_set(struct bcm_sysport_priv *priv,
 
 static int bcm_sysport_init_rx_ring(struct bcm_sysport_priv *priv)
 {
+	struct bcm_sysport_cb *cb;
 	u32 reg;
 	int ret;
+	int i;
 
 	/* Initialize SW view of the RX ring */
 	priv->num_rx_bds = NUM_RX_DESC;
 	priv->rx_bds = priv->base + SYS_PORT_RDMA_OFFSET;
-	priv->rx_bd_assign_ptr = priv->rx_bds;
-	priv->rx_bd_assign_index = 0;
 	priv->rx_c_index = 0;
 	priv->rx_read_ptr = 0;
 	priv->rx_cbs = kcalloc(priv->num_rx_bds, sizeof(struct bcm_sysport_cb),
@@ -1286,6 +1351,11 @@ static int bcm_sysport_init_rx_ring(struct bcm_sysport_priv *priv)
 		return -ENOMEM;
 	}
 
+	for (i = 0; i < priv->num_rx_bds; i++) {
+		cb = priv->rx_cbs + i;
+		cb->bd_addr = priv->rx_bds + i * DESC_SIZE;
+	}
+
 	ret = bcm_sysport_alloc_rx_bufs(priv);
 	if (ret) {
 		netif_err(priv, hw, priv->netdev, "SKB allocation failed\n");
@@ -1641,6 +1711,8 @@ static struct ethtool_ops bcm_sysport_ethtool_ops = {
 	.get_sset_count		= bcm_sysport_get_sset_count,
 	.get_wol		= bcm_sysport_get_wol,
 	.set_wol		= bcm_sysport_set_wol,
+	.get_coalesce		= bcm_sysport_get_coalesce,
+	.set_coalesce		= bcm_sysport_set_coalesce,
 };
 
 static const struct net_device_ops bcm_sysport_netdev_ops = {
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.h b/drivers/net/ethernet/broadcom/bcmsysport.h
index e2c043eabbf3..f28bf545d7f4 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.h
+++ b/drivers/net/ethernet/broadcom/bcmsysport.h
@@ -292,7 +292,7 @@ struct bcm_rsb {
 #define RDMA_END_ADDR_LO		0x102c
 
 #define RDMA_MBDONE_INTR		0x1030
-#define  RDMA_INTR_THRESH_MASK		0xff
+#define  RDMA_INTR_THRESH_MASK		0x1ff
 #define  RDMA_TIMEOUT_SHIFT		16
 #define  RDMA_TIMEOUT_MASK		0xffff
 
@@ -663,8 +663,6 @@ struct bcm_sysport_priv {
 
 	/* Receive queue */
 	void __iomem		*rx_bds;
-	void __iomem		*rx_bd_assign_ptr;
-	unsigned int		rx_bd_assign_index;
 	struct bcm_sysport_cb	*rx_cbs;
 	unsigned int		num_rx_bds;
 	unsigned int		rx_read_ptr;
diff --git a/drivers/net/ethernet/broadcom/bgmac.h b/drivers/net/ethernet/broadcom/bgmac.h
index db27febbb215..4fbb093e0d84 100644
--- a/drivers/net/ethernet/broadcom/bgmac.h
+++ b/drivers/net/ethernet/broadcom/bgmac.h
@@ -13,6 +13,7 @@
 	dev_dbg(&(bgmac)->core->dev, fmt, ##__VA_ARGS__)
 
 #include <linux/bcma/bcma.h>
+#include <linux/brcmphy.h>
 #include <linux/netdevice.h>
 
 #define BGMAC_DEV_CTL				0x000
@@ -349,7 +350,7 @@
 #define BGMAC_DESC_CTL0_SOF			0x80000000	/* Start of frame */
 #define BGMAC_DESC_CTL1_LEN			0x00001FFF
 
-#define BGMAC_PHY_NOREGS			0x1E
+#define BGMAC_PHY_NOREGS			BRCM_PSEUDO_PHY_ADDR
 #define BGMAC_PHY_MASK				0x1F
 
 #define BGMAC_MAX_TX_RINGS			4
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index a3b0f7a0c61e..cd4ae76bbff2 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -357,6 +357,7 @@ struct sw_tx_bd {
 struct sw_rx_page {
 	struct page	*page;
 	DEFINE_DMA_UNMAP_ADDR(mapping);
+	unsigned int	offset;
 };
 
 union db_prod {
@@ -381,9 +382,10 @@ union db_prod {
 
 #define PAGES_PER_SGE_SHIFT	0
 #define PAGES_PER_SGE		(1 << PAGES_PER_SGE_SHIFT)
-#define SGE_PAGE_SIZE		PAGE_SIZE
-#define SGE_PAGE_SHIFT		PAGE_SHIFT
-#define SGE_PAGE_ALIGN(addr)	PAGE_ALIGN((typeof(PAGE_SIZE))(addr))
+#define SGE_PAGE_SHIFT		12
+#define SGE_PAGE_SIZE		(1 << SGE_PAGE_SHIFT)
+#define SGE_PAGE_MASK		(~(SGE_PAGE_SIZE - 1))
+#define SGE_PAGE_ALIGN(addr)	(((addr) + SGE_PAGE_SIZE - 1) & SGE_PAGE_MASK)
 #define SGE_PAGES		(SGE_PAGE_SIZE * PAGES_PER_SGE)
 #define TPA_AGG_SIZE		min_t(u32, (min_t(u32, 8, MAX_SKB_FRAGS) * \
 					    SGE_PAGES), 0xffff)
@@ -526,6 +528,11 @@ enum bnx2x_tpa_mode_t {
 	TPA_MODE_GRO
 };
 
+struct bnx2x_alloc_pool {
+	struct page	*page;
+	unsigned int	offset;
+};
+
 struct bnx2x_fastpath {
 	struct bnx2x		*bp; /* parent */
 
@@ -599,6 +606,8 @@ struct bnx2x_fastpath {
 	     4 (for the digits and to make it DWORD aligned) */
 #define FP_NAME_SIZE		(sizeof(((struct net_device *)0)->name) + 8)
 	char			name[FP_NAME_SIZE];
+
+	struct bnx2x_alloc_pool	page_pool;
 };
 
 #define bnx2x_fp(bp, nr, var)	((bp)->fp[(nr)].var)
@@ -1774,7 +1783,7 @@ struct bnx2x {
 	int			stats_state;
 
 	/* used for synchronization of concurrent threads statistics handling */
-	struct mutex		stats_lock;
+	struct semaphore	stats_lock;
 
 	/* used by dmae command loader */
 	struct dmae_command	stats_dmae;
@@ -2408,10 +2417,13 @@ void bnx2x_igu_clear_sb_gen(struct bnx2x *bp, u8 func, u8 idu_sb_id,
 				 AEU_INPUTS_ATTN_BITS_IGU_PARITY_ERROR | \
 				 AEU_INPUTS_ATTN_BITS_MISC_PARITY_ERROR)
 
-#define HW_PRTY_ASSERT_SET_3 (AEU_INPUTS_ATTN_BITS_MCP_LATCHED_ROM_PARITY | \
-		AEU_INPUTS_ATTN_BITS_MCP_LATCHED_UMP_RX_PARITY | \
-		AEU_INPUTS_ATTN_BITS_MCP_LATCHED_UMP_TX_PARITY | \
-		AEU_INPUTS_ATTN_BITS_MCP_LATCHED_SCPAD_PARITY)
+#define HW_PRTY_ASSERT_SET_3_WITHOUT_SCPAD \
+		(AEU_INPUTS_ATTN_BITS_MCP_LATCHED_ROM_PARITY | \
+		 AEU_INPUTS_ATTN_BITS_MCP_LATCHED_UMP_RX_PARITY | \
+		 AEU_INPUTS_ATTN_BITS_MCP_LATCHED_UMP_TX_PARITY)
+
+#define HW_PRTY_ASSERT_SET_3 (HW_PRTY_ASSERT_SET_3_WITHOUT_SCPAD | \
+			      AEU_INPUTS_ATTN_BITS_MCP_LATCHED_SCPAD_PARITY)
 
 #define HW_PRTY_ASSERT_SET_4 (AEU_INPUTS_ATTN_BITS_PGLUE_PARITY_ERROR | \
 			      AEU_INPUTS_ATTN_BITS_ATC_PARITY_ERROR)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index ec56a9b65dc3..a90d7364334f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -544,30 +544,46 @@ static void bnx2x_set_gro_params(struct sk_buff *skb, u16 parsing_flags,
 static int bnx2x_alloc_rx_sge(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 			      u16 index, gfp_t gfp_mask)
 {
-	struct page *page = alloc_pages(gfp_mask, PAGES_PER_SGE_SHIFT);
 	struct sw_rx_page *sw_buf = &fp->rx_page_ring[index];
 	struct eth_rx_sge *sge = &fp->rx_sge_ring[index];
+	struct bnx2x_alloc_pool *pool = &fp->page_pool;
 	dma_addr_t mapping;
 
-	if (unlikely(page == NULL)) {
-		BNX2X_ERR("Can't alloc sge\n");
-		return -ENOMEM;
+	if (!pool->page || (PAGE_SIZE - pool->offset) < SGE_PAGE_SIZE) {
+
+		/* put page reference used by the memory pool, since we
+		 * won't be using this page as the mempool anymore.
+		 */
+		if (pool->page)
+			put_page(pool->page);
+
+		pool->page = alloc_pages(gfp_mask, PAGES_PER_SGE_SHIFT);
+		if (unlikely(!pool->page)) {
+			BNX2X_ERR("Can't alloc sge\n");
+			return -ENOMEM;
+		}
+
+		pool->offset = 0;
 	}
 
-	mapping = dma_map_page(&bp->pdev->dev, page, 0,
-			       SGE_PAGES, DMA_FROM_DEVICE);
+	mapping = dma_map_page(&bp->pdev->dev, pool->page,
+			       pool->offset, SGE_PAGE_SIZE, DMA_FROM_DEVICE);
 	if (unlikely(dma_mapping_error(&bp->pdev->dev, mapping))) {
-		__free_pages(page, PAGES_PER_SGE_SHIFT);
 		BNX2X_ERR("Can't map sge\n");
 		return -ENOMEM;
 	}
 
-	sw_buf->page = page;
+	get_page(pool->page);
+	sw_buf->page = pool->page;
+	sw_buf->offset = pool->offset;
+
 	dma_unmap_addr_set(sw_buf, mapping, mapping);
 
 	sge->addr_hi = cpu_to_le32(U64_HI(mapping));
 	sge->addr_lo = cpu_to_le32(U64_LO(mapping));
 
+	pool->offset += SGE_PAGE_SIZE;
+
 	return 0;
 }
 
@@ -629,20 +645,22 @@ static int bnx2x_fill_frag_skb(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 			return err;
 		}
 
-		/* Unmap the page as we're going to pass it to the stack */
 		dma_unmap_page(&bp->pdev->dev,
 			       dma_unmap_addr(&old_rx_pg, mapping),
-			       SGE_PAGES, DMA_FROM_DEVICE);
+			       SGE_PAGE_SIZE, DMA_FROM_DEVICE);
 		/* Add one frag and update the appropriate fields in the skb */
 		if (fp->mode == TPA_MODE_LRO)
-			skb_fill_page_desc(skb, j, old_rx_pg.page, 0, frag_len);
+			skb_fill_page_desc(skb, j, old_rx_pg.page,
+					   old_rx_pg.offset, frag_len);
 		else { /* GRO */
 			int rem;
 			int offset = 0;
 			for (rem = frag_len; rem > 0; rem -= gro_size) {
 				int len = rem > gro_size ? gro_size : rem;
 				skb_fill_page_desc(skb, frag_id++,
-						   old_rx_pg.page, offset, len);
+						   old_rx_pg.page,
+						   old_rx_pg.offset + offset,
+						   len);
 				if (offset)
 					get_page(old_rx_pg.page);
 				offset += len;
@@ -662,7 +680,7 @@ static int bnx2x_fill_frag_skb(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 static void bnx2x_frag_free(const struct bnx2x_fastpath *fp, void *data)
 {
 	if (fp->rx_frag_size)
-		put_page(virt_to_head_page(data));
+		skb_free_frag(data);
 	else
 		kfree(data);
 }
@@ -3400,8 +3418,13 @@ static int bnx2x_pkt_req_lin(struct bnx2x *bp, struct sk_buff *skb,
 			u32 wnd_sum = 0;
 
 			/* Headers length */
-			hlen = (int)(skb_transport_header(skb) - skb->data) +
-				tcp_hdrlen(skb);
+			if (xmit_type & XMIT_GSO_ENC)
+				hlen = (int)(skb_inner_transport_header(skb) -
+					     skb->data) +
+					     inner_tcp_hdrlen(skb);
+			else
+				hlen = (int)(skb_transport_header(skb) -
+					     skb->data) + tcp_hdrlen(skb);
 
 			/* Amount of data (w/o headers) on linear part of SKB*/
 			first_bd_sz = skb_headlen(skb) - hlen;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
index d7a71758e876..03b7404d5b9b 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
@@ -804,9 +804,13 @@ static inline void bnx2x_free_rx_sge(struct bnx2x *bp,
 	if (!page)
 		return;
 
+	/* Since many fragments can share the same page, make sure to
+	 * only unmap and free the page once.
+	 */
 	dma_unmap_page(&bp->pdev->dev, dma_unmap_addr(sw_buf, mapping),
-		       SGE_PAGES, DMA_FROM_DEVICE);
-	__free_pages(page, PAGES_PER_SGE_SHIFT);
+		       SGE_PAGE_SIZE, DMA_FROM_DEVICE);
+
+	put_page(page);
 
 	sw_buf->page = NULL;
 	sge->addr_hi = 0;
@@ -964,6 +968,17 @@ static inline void bnx2x_set_fw_mac_addr(__le16 *fw_hi, __le16 *fw_mid,
 	((u8 *)fw_lo)[1]  = mac[4];
 }
 
+static inline void bnx2x_free_rx_mem_pool(struct bnx2x *bp,
+					  struct bnx2x_alloc_pool *pool)
+{
+	if (!pool->page)
+		return;
+
+	put_page(pool->page);
+
+	pool->page = NULL;
+}
+
 static inline void bnx2x_free_rx_sge_range(struct bnx2x *bp,
 					   struct bnx2x_fastpath *fp, int last)
 {
@@ -974,6 +989,8 @@ static inline void bnx2x_free_rx_sge_range(struct bnx2x *bp,
 
 	for (i = 0; i < last; i++)
 		bnx2x_free_rx_sge(bp, fp, i);
+
+	bnx2x_free_rx_mem_pool(bp, &fp->page_pool);
 }
 
 static inline void bnx2x_set_next_page_rx_bd(struct bnx2x_fastpath *fp)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
index 48ed005ba73f..76b9052a961c 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
@@ -257,14 +257,15 @@ static int bnx2x_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
 	struct bnx2x *bp = netdev_priv(dev);
 	int cfg_idx = bnx2x_get_link_cfg_idx(bp);
+	u32 media_type;
 
 	/* Dual Media boards present all available port types */
 	cmd->supported = bp->port.supported[cfg_idx] |
 		(bp->port.supported[cfg_idx ^ 1] &
 		 (SUPPORTED_TP | SUPPORTED_FIBRE));
 	cmd->advertising = bp->port.advertising[cfg_idx];
-	if (bp->link_params.phy[bnx2x_get_cur_phy_idx(bp)].media_type ==
-	    ETH_PHY_SFP_1G_FIBER) {
+	media_type = bp->link_params.phy[bnx2x_get_cur_phy_idx(bp)].media_type;
+	if (media_type == ETH_PHY_SFP_1G_FIBER) {
 		cmd->supported &= ~(SUPPORTED_10000baseT_Full);
 		cmd->advertising &= ~(ADVERTISED_10000baseT_Full);
 	}
@@ -312,12 +313,26 @@ static int bnx2x_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 			cmd->lp_advertising |= ADVERTISED_100baseT_Full;
 		if (status & LINK_STATUS_LINK_PARTNER_1000THD_CAPABLE)
 			cmd->lp_advertising |= ADVERTISED_1000baseT_Half;
-		if (status & LINK_STATUS_LINK_PARTNER_1000TFD_CAPABLE)
-			cmd->lp_advertising |= ADVERTISED_1000baseT_Full;
+		if (status & LINK_STATUS_LINK_PARTNER_1000TFD_CAPABLE) {
+			if (media_type == ETH_PHY_KR) {
+				cmd->lp_advertising |=
+					ADVERTISED_1000baseKX_Full;
+			} else {
+				cmd->lp_advertising |=
+					ADVERTISED_1000baseT_Full;
+			}
+		}
 		if (status & LINK_STATUS_LINK_PARTNER_2500XFD_CAPABLE)
 			cmd->lp_advertising |= ADVERTISED_2500baseX_Full;
-		if (status & LINK_STATUS_LINK_PARTNER_10GXFD_CAPABLE)
-			cmd->lp_advertising |= ADVERTISED_10000baseT_Full;
+		if (status & LINK_STATUS_LINK_PARTNER_10GXFD_CAPABLE) {
+			if (media_type == ETH_PHY_KR) {
+				cmd->lp_advertising |=
+					ADVERTISED_10000baseKR_Full;
+			} else {
+				cmd->lp_advertising |=
+					ADVERTISED_10000baseT_Full;
+			}
+		}
 		if (status & LINK_STATUS_LINK_PARTNER_20GXFD_CAPABLE)
 			cmd->lp_advertising |= ADVERTISED_20000baseKR2_Full;
 	}
@@ -564,15 +579,20 @@ static int bnx2x_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 				return -EINVAL;
 			}
 
-			if (!(bp->port.supported[cfg_idx] &
-			      SUPPORTED_1000baseT_Full)) {
+			if (bp->port.supported[cfg_idx] &
+			     SUPPORTED_1000baseT_Full) {
+				advertising = (ADVERTISED_1000baseT_Full |
+					       ADVERTISED_TP);
+
+			} else if (bp->port.supported[cfg_idx] &
+				   SUPPORTED_1000baseKX_Full) {
+				advertising = ADVERTISED_1000baseKX_Full;
+			} else {
 				DP(BNX2X_MSG_ETHTOOL,
 				   "1G full not supported\n");
 				return -EINVAL;
 			}
 
-			advertising = (ADVERTISED_1000baseT_Full |
-				       ADVERTISED_TP);
 			break;
 
 		case SPEED_2500:
@@ -600,17 +620,22 @@ static int bnx2x_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 				return -EINVAL;
 			}
 			phy_idx = bnx2x_get_cur_phy_idx(bp);
-			if (!(bp->port.supported[cfg_idx]
-			      & SUPPORTED_10000baseT_Full) ||
-			    (bp->link_params.phy[phy_idx].media_type ==
+			if ((bp->port.supported[cfg_idx] &
+			     SUPPORTED_10000baseT_Full) &&
+			    (bp->link_params.phy[phy_idx].media_type !=
 			     ETH_PHY_SFP_1G_FIBER)) {
+				advertising = (ADVERTISED_10000baseT_Full |
+					       ADVERTISED_FIBRE);
+			} else if (bp->port.supported[cfg_idx] &
+			       SUPPORTED_10000baseKR_Full) {
+				advertising = (ADVERTISED_10000baseKR_Full |
+					       ADVERTISED_FIBRE);
+			} else {
 				DP(BNX2X_MSG_ETHTOOL,
 				   "10G full not supported\n");
 				return -EINVAL;
 			}
 
-			advertising = (ADVERTISED_10000baseT_Full |
-				       ADVERTISED_FIBRE);
 			break;
 
 		default:
@@ -633,6 +658,7 @@ static int bnx2x_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 	bp->link_params.multi_phy_config = new_multi_phy_config;
 	if (netif_running(dev)) {
 		bnx2x_stats_handle(bp, STATS_EVENT_STOP);
+		bnx2x_force_link_reset(bp);
 		bnx2x_link_set(bp);
 	}
 
@@ -1204,6 +1230,7 @@ static int bnx2x_acquire_nvram_lock(struct bnx2x *bp)
 	if (!(val & (MCPR_NVM_SW_ARB_ARB_ARB1 << port))) {
 		DP(BNX2X_MSG_ETHTOOL | BNX2X_MSG_NVM,
 		   "cannot get access to nvram interface\n");
+		bnx2x_release_hw_lock(bp, HW_LOCK_RESOURCE_NVRAM);
 		return -EBUSY;
 	}
 
@@ -1944,6 +1971,7 @@ static int bnx2x_set_pauseparam(struct net_device *dev,
 
 	if (netif_running(dev)) {
 		bnx2x_stats_handle(bp, STATS_EVENT_STOP);
+		bnx2x_force_link_reset(bp);
 		bnx2x_link_set(bp);
 	}
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index 21a0d6afca4a..a0b03c27e0a3 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -3392,9 +3392,9 @@ static void bnx2x_calc_ieee_aneg_adv(struct bnx2x_phy *phy,
 	case BNX2X_FLOW_CTRL_AUTO:
 		switch (params->req_fc_auto_adv) {
 		case BNX2X_FLOW_CTRL_BOTH:
+		case BNX2X_FLOW_CTRL_RX:
 			*ieee_fc |= MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_BOTH;
 			break;
-		case BNX2X_FLOW_CTRL_RX:
 		case BNX2X_FLOW_CTRL_TX:
 			*ieee_fc |=
 				MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_ASYMMETRIC;
@@ -3488,14 +3488,21 @@ static void bnx2x_ext_phy_set_pause(struct link_params *params,
 	bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD, MDIO_AN_REG_ADV_PAUSE, val);
 }
 
-static void bnx2x_pause_resolve(struct link_vars *vars, u32 pause_result)
-{						/*  LD	    LP	 */
+static void bnx2x_pause_resolve(struct bnx2x_phy *phy,
+				struct link_params *params,
+				struct link_vars *vars,
+				u32 pause_result)
+{
+	struct bnx2x *bp = params->bp;
+						/*  LD	    LP	 */
 	switch (pause_result) {			/* ASYM P ASYM P */
 	case 0xb:				/*   1  0   1  1 */
+		DP(NETIF_MSG_LINK, "Flow Control: TX only\n");
 		vars->flow_ctrl = BNX2X_FLOW_CTRL_TX;
 		break;
 
 	case 0xe:				/*   1  1   1  0 */
+		DP(NETIF_MSG_LINK, "Flow Control: RX only\n");
 		vars->flow_ctrl = BNX2X_FLOW_CTRL_RX;
 		break;
 
@@ -3503,10 +3510,22 @@ static void bnx2x_pause_resolve(struct link_vars *vars, u32 pause_result)
 	case 0x7:				/*   0  1   1  1 */
 	case 0xd:				/*   1  1   0  1 */
 	case 0xf:				/*   1  1   1  1 */
-		vars->flow_ctrl = BNX2X_FLOW_CTRL_BOTH;
+		/* If the user selected to advertise RX ONLY,
+		 * although we advertised both, need to enable
+		 * RX only.
+		 */
+		if (params->req_fc_auto_adv == BNX2X_FLOW_CTRL_BOTH) {
+			DP(NETIF_MSG_LINK, "Flow Control: RX & TX\n");
+			vars->flow_ctrl = BNX2X_FLOW_CTRL_BOTH;
+		} else {
+			DP(NETIF_MSG_LINK, "Flow Control: RX only\n");
+			vars->flow_ctrl = BNX2X_FLOW_CTRL_RX;
+		}
 		break;
 
 	default:
+		DP(NETIF_MSG_LINK, "Flow Control: None\n");
+		vars->flow_ctrl = BNX2X_FLOW_CTRL_NONE;
 		break;
 	}
 	if (pause_result & (1<<0))
@@ -3567,7 +3586,7 @@ static void bnx2x_ext_phy_update_adv_fc(struct bnx2x_phy *phy,
 	pause_result |= (lp_pause &
 			 MDIO_AN_REG_ADV_PAUSE_MASK) >> 10;
 	DP(NETIF_MSG_LINK, "Ext PHY pause result 0x%x\n", pause_result);
-	bnx2x_pause_resolve(vars, pause_result);
+	bnx2x_pause_resolve(phy, params, vars, pause_result);
 
 }
 
@@ -5396,7 +5415,7 @@ static void bnx2x_update_adv_fc(struct bnx2x_phy *phy,
 				 MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_MASK)>>7;
 		DP(NETIF_MSG_LINK, "pause_result CL37 0x%x\n", pause_result);
 	}
-	bnx2x_pause_resolve(vars, pause_result);
+	bnx2x_pause_resolve(phy, params, vars, pause_result);
 
 }
 
@@ -7129,7 +7148,7 @@ static void bnx2x_8073_resolve_fc(struct bnx2x_phy *phy,
 		pause_result |= (lp_pause &
 				 MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_BOTH) >> 7;
 
-		bnx2x_pause_resolve(vars, pause_result);
+		bnx2x_pause_resolve(phy, params, vars, pause_result);
 		DP(NETIF_MSG_LINK, "Ext PHY CL37 pause result 0x%x\n",
 			   pause_result);
 	}
@@ -11474,7 +11493,9 @@ static const struct bnx2x_phy phy_warpcore = {
 			   SUPPORTED_100baseT_Half |
 			   SUPPORTED_100baseT_Full |
 			   SUPPORTED_1000baseT_Full |
+			   SUPPORTED_1000baseKX_Full |
 			   SUPPORTED_10000baseT_Full |
+			   SUPPORTED_10000baseKR_Full |
 			   SUPPORTED_20000baseKR2_Full |
 			   SUPPORTED_20000baseMLD2_Full |
 			   SUPPORTED_FIBRE |
@@ -11980,8 +12001,8 @@ static int bnx2x_populate_int_phy(struct bnx2x *bp, u32 shmem_base, u8 port,
 			break;
 		case PORT_HW_CFG_NET_SERDES_IF_KR:
 			phy->media_type = ETH_PHY_KR;
-			phy->supported &= (SUPPORTED_1000baseT_Full |
-					   SUPPORTED_10000baseT_Full |
+			phy->supported &= (SUPPORTED_1000baseKX_Full |
+					   SUPPORTED_10000baseKR_Full |
 					   SUPPORTED_FIBRE |
 					   SUPPORTED_Autoneg |
 					   SUPPORTED_Pause |
@@ -11999,8 +12020,8 @@ static int bnx2x_populate_int_phy(struct bnx2x *bp, u32 shmem_base, u8 port,
 			phy->media_type = ETH_PHY_KR;
 			phy->flags |= FLAGS_WC_DUAL_MODE;
 			phy->supported &= (SUPPORTED_20000baseKR2_Full |
-					   SUPPORTED_10000baseT_Full |
-					   SUPPORTED_1000baseT_Full |
+					   SUPPORTED_10000baseKR_Full |
+					   SUPPORTED_1000baseKX_Full |
 					   SUPPORTED_Autoneg |
 					   SUPPORTED_FIBRE |
 					   SUPPORTED_Pause |
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index fd52ce95127e..c27af12314ed 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -2287,13 +2287,11 @@ static int bnx2x_set_spio(struct bnx2x *bp, int spio, u32 mode)
 void bnx2x_calc_fc_adv(struct bnx2x *bp)
 {
 	u8 cfg_idx = bnx2x_get_link_cfg_idx(bp);
+
+	bp->port.advertising[cfg_idx] &= ~(ADVERTISED_Asym_Pause |
+					   ADVERTISED_Pause);
 	switch (bp->link_vars.ieee_fc &
 		MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_MASK) {
-	case MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_NONE:
-		bp->port.advertising[cfg_idx] &= ~(ADVERTISED_Asym_Pause |
-						   ADVERTISED_Pause);
-		break;
-
 	case MDIO_COMBO_IEEE0_AUTO_NEG_ADV_PAUSE_BOTH:
 		bp->port.advertising[cfg_idx] |= (ADVERTISED_Asym_Pause |
 						  ADVERTISED_Pause);
@@ -2304,8 +2302,6 @@ void bnx2x_calc_fc_adv(struct bnx2x *bp)
 		break;
 
 	default:
-		bp->port.advertising[cfg_idx] &= ~(ADVERTISED_Asym_Pause |
-						   ADVERTISED_Pause);
 		break;
 	}
 }
@@ -2351,12 +2347,16 @@ int bnx2x_initial_phy_init(struct bnx2x *bp, int load_mode)
 		if (load_mode == LOAD_DIAG) {
 			struct link_params *lp = &bp->link_params;
 			lp->loopback_mode = LOOPBACK_XGXS;
-			/* do PHY loopback at 10G speed, if possible */
-			if (lp->req_line_speed[cfx_idx] < SPEED_10000) {
+			/* Prefer doing PHY loopback at highest speed */
+			if (lp->req_line_speed[cfx_idx] < SPEED_20000) {
 				if (lp->speed_cap_mask[cfx_idx] &
-				    PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)
+				    PORT_HW_CFG_SPEED_CAPABILITY_D0_20G)
 					lp->req_line_speed[cfx_idx] =
-					SPEED_10000;
+					SPEED_20000;
+				else if (lp->speed_cap_mask[cfx_idx] &
+					    PORT_HW_CFG_SPEED_CAPABILITY_D0_10G)
+						lp->req_line_speed[cfx_idx] =
+						SPEED_10000;
 				else
 					lp->req_line_speed[cfx_idx] =
 					SPEED_1000;
@@ -4867,9 +4867,7 @@ static bool bnx2x_check_blocks_with_parity3(struct bnx2x *bp, u32 sig,
 				res = true;
 				break;
 			case AEU_INPUTS_ATTN_BITS_MCP_LATCHED_SCPAD_PARITY:
-				if (print)
-					_print_next_block((*par_num)++,
-							  "MCP SCPAD");
+				(*par_num)++;
 				/* clear latched SCPAD PATIRY from MCP */
 				REG_WR(bp, MISC_REG_AEU_CLR_LATCH_SIGNAL,
 				       1UL << 10);
@@ -4931,6 +4929,7 @@ static bool bnx2x_parity_attn(struct bnx2x *bp, bool *global, bool print,
 	    (sig[3] & HW_PRTY_ASSERT_SET_3) ||
 	    (sig[4] & HW_PRTY_ASSERT_SET_4)) {
 		int par_num = 0;
+
 		DP(NETIF_MSG_HW, "Was parity error: HW block parity attention:\n"
 				 "[0]:0x%08x [1]:0x%08x [2]:0x%08x [3]:0x%08x [4]:0x%08x\n",
 			  sig[0] & HW_PRTY_ASSERT_SET_0,
@@ -4938,9 +4937,18 @@ static bool bnx2x_parity_attn(struct bnx2x *bp, bool *global, bool print,
 			  sig[2] & HW_PRTY_ASSERT_SET_2,
 			  sig[3] & HW_PRTY_ASSERT_SET_3,
 			  sig[4] & HW_PRTY_ASSERT_SET_4);
-		if (print)
-			netdev_err(bp->dev,
-				   "Parity errors detected in blocks: ");
+		if (print) {
+			if (((sig[0] & HW_PRTY_ASSERT_SET_0) ||
+			     (sig[1] & HW_PRTY_ASSERT_SET_1) ||
+			     (sig[2] & HW_PRTY_ASSERT_SET_2) ||
+			     (sig[4] & HW_PRTY_ASSERT_SET_4)) ||
+			     (sig[3] & HW_PRTY_ASSERT_SET_3_WITHOUT_SCPAD)) {
+				netdev_err(bp->dev,
+					   "Parity errors detected in blocks: ");
+			} else {
+				print = false;
+			}
+		}
 		res |= bnx2x_check_blocks_with_parity0(bp,
 			sig[0] & HW_PRTY_ASSERT_SET_0, &par_num, print);
 		res |= bnx2x_check_blocks_with_parity1(bp,
@@ -8431,7 +8439,7 @@ int bnx2x_set_eth_mac(struct bnx2x *bp, bool set)
 					 BNX2X_ETH_MAC, &ramrod_flags);
 	} else { /* vf */
 		return bnx2x_vfpf_config_mac(bp, bp->dev->dev_addr,
-					     bp->fp->index, true);
+					     bp->fp->index, set);
 	}
 }
 
@@ -9323,7 +9331,8 @@ unload_error:
 	 * function stop ramrod is sent, since as part of this ramrod FW access
 	 * PTP registers.
 	 */
-	bnx2x_stop_ptp(bp);
+	if (bp->flags & PTP_SUPPORTED)
+		bnx2x_stop_ptp(bp);
 
 	/* Disable HW interrupts, NAPI */
 	bnx2x_netif_stop(bp, 1);
@@ -11147,6 +11156,12 @@ static void bnx2x_link_settings_requested(struct bnx2x *bp)
 				bp->port.advertising[idx] |=
 					(ADVERTISED_1000baseT_Full |
 					 ADVERTISED_TP);
+			} else if (bp->port.supported[idx] &
+				   SUPPORTED_1000baseKX_Full) {
+				bp->link_params.req_line_speed[idx] =
+					SPEED_1000;
+				bp->port.advertising[idx] |=
+					ADVERTISED_1000baseKX_Full;
 			} else {
 				BNX2X_ERR("NVRAM config error. Invalid link_config 0x%x  speed_cap_mask 0x%x\n",
 				    link_config,
@@ -11179,6 +11194,13 @@ static void bnx2x_link_settings_requested(struct bnx2x *bp)
 				bp->port.advertising[idx] |=
 					(ADVERTISED_10000baseT_Full |
 						ADVERTISED_FIBRE);
+			} else if (bp->port.supported[idx] &
+				   SUPPORTED_10000baseKR_Full) {
+				bp->link_params.req_line_speed[idx] =
+					SPEED_10000;
+				bp->port.advertising[idx] |=
+					(ADVERTISED_10000baseKR_Full |
+						ADVERTISED_FIBRE);
 			} else {
 				BNX2X_ERR("NVRAM config error. Invalid link_config 0x%x  speed_cap_mask 0x%x\n",
 				    link_config,
@@ -12054,7 +12076,7 @@ static int bnx2x_init_bp(struct bnx2x *bp)
 	mutex_init(&bp->port.phy_mutex);
 	mutex_init(&bp->fw_mb_mutex);
 	mutex_init(&bp->drv_info_mutex);
-	mutex_init(&bp->stats_lock);
+	sema_init(&bp->stats_lock, 1);
 	bp->drv_info_mng_owner = false;
 
 	INIT_DELAYED_WORK(&bp->sp_task, bnx2x_sp_task);
@@ -13690,9 +13712,10 @@ static int bnx2x_eeh_nic_unload(struct bnx2x *bp)
 	cancel_delayed_work_sync(&bp->sp_task);
 	cancel_delayed_work_sync(&bp->period_task);
 
-	mutex_lock(&bp->stats_lock);
-	bp->stats_state = STATS_STATE_DISABLED;
-	mutex_unlock(&bp->stats_lock);
+	if (!down_timeout(&bp->stats_lock, HZ / 10)) {
+		bp->stats_state = STATS_STATE_DISABLED;
+		up(&bp->stats_lock);
+	}
 
 	bnx2x_save_statistics(bp);
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
index 07cdf9bbffef..4ad415ac8cfe 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
@@ -424,7 +424,7 @@ static void __bnx2x_vlan_mac_h_exec_pending(struct bnx2x *bp,
 	o->head_exe_request = false;
 	o->saved_ramrod_flags = 0;
 	rc = bnx2x_exe_queue_step(bp, &o->exe_queue, &ramrod_flags);
-	if (rc != 0) {
+	if ((rc != 0) && (rc != 1)) {
 		BNX2X_ERR("execution of pending commands failed with rc %d\n",
 			  rc);
 #ifdef BNX2X_STOP_ON_ERROR
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
index 266b055c2360..69d699f0730a 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
@@ -1372,19 +1372,23 @@ void bnx2x_stats_handle(struct bnx2x *bp, enum bnx2x_stats_event event)
 	 * that context in case someone is in the middle of a transition.
 	 * For other events, wait a bit until lock is taken.
 	 */
-	if (!mutex_trylock(&bp->stats_lock)) {
+	if (down_trylock(&bp->stats_lock)) {
 		if (event == STATS_EVENT_UPDATE)
 			return;
 
 		DP(BNX2X_MSG_STATS,
 		   "Unlikely stats' lock contention [event %d]\n", event);
-		mutex_lock(&bp->stats_lock);
+		if (unlikely(down_timeout(&bp->stats_lock, HZ / 10))) {
+			BNX2X_ERR("Failed to take stats lock [event %d]\n",
+				  event);
+			return;
+		}
 	}
 
 	bnx2x_stats_stm[state][event].action(bp);
 	bp->stats_state = bnx2x_stats_stm[state][event].next_state;
 
-	mutex_unlock(&bp->stats_lock);
+	up(&bp->stats_lock);
 
 	if ((event != STATS_EVENT_UPDATE) || netif_msg_timer(bp))
 		DP(BNX2X_MSG_STATS, "state %d -> event %d -> state %d\n",
@@ -1970,7 +1974,11 @@ int bnx2x_stats_safe_exec(struct bnx2x *bp,
 	/* Wait for statistics to end [while blocking further requests],
 	 * then run supplied function 'safely'.
 	 */
-	mutex_lock(&bp->stats_lock);
+	rc = down_timeout(&bp->stats_lock, HZ / 10);
+	if (unlikely(rc)) {
+		BNX2X_ERR("Failed to take statistics lock for safe execution\n");
+		goto out_no_lock;
+	}
 
 	bnx2x_stats_comp(bp);
 	while (bp->stats_pending && cnt--)
@@ -1988,7 +1996,7 @@ out:
 	/* No need to restart statistics - if they're enabled, the timer
 	 * will restart the statistics.
 	 */
-	mutex_unlock(&bp->stats_lock);
-
+	up(&bp->stats_lock);
+out_no_lock:
 	return rc;
 }
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.c b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
index 6043734ea613..b43b2cb9b830 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
@@ -2770,12 +2770,79 @@ static int bcmgenet_close(struct net_device *dev)
 	return ret;
 }
 
+static void bcmgenet_dump_tx_queue(struct bcmgenet_tx_ring *ring)
+{
+	struct bcmgenet_priv *priv = ring->priv;
+	u32 p_index, c_index, intsts, intmsk;
+	struct netdev_queue *txq;
+	unsigned int free_bds;
+	unsigned long flags;
+	bool txq_stopped;
+
+	if (!netif_msg_tx_err(priv))
+		return;
+
+	txq = netdev_get_tx_queue(priv->dev, ring->queue);
+
+	spin_lock_irqsave(&ring->lock, flags);
+	if (ring->index == DESC_INDEX) {
+		intsts = ~bcmgenet_intrl2_0_readl(priv, INTRL2_CPU_MASK_STATUS);
+		intmsk = UMAC_IRQ_TXDMA_DONE | UMAC_IRQ_TXDMA_MBDONE;
+	} else {
+		intsts = ~bcmgenet_intrl2_1_readl(priv, INTRL2_CPU_MASK_STATUS);
+		intmsk = 1 << ring->index;
+	}
+	c_index = bcmgenet_tdma_ring_readl(priv, ring->index, TDMA_CONS_INDEX);
+	p_index = bcmgenet_tdma_ring_readl(priv, ring->index, TDMA_PROD_INDEX);
+	txq_stopped = netif_tx_queue_stopped(txq);
+	free_bds = ring->free_bds;
+	spin_unlock_irqrestore(&ring->lock, flags);
+
+	netif_err(priv, tx_err, priv->dev, "Ring %d queue %d status summary\n"
+		  "TX queue status: %s, interrupts: %s\n"
+		  "(sw)free_bds: %d (sw)size: %d\n"
+		  "(sw)p_index: %d (hw)p_index: %d\n"
+		  "(sw)c_index: %d (hw)c_index: %d\n"
+		  "(sw)clean_p: %d (sw)write_p: %d\n"
+		  "(sw)cb_ptr: %d (sw)end_ptr: %d\n",
+		  ring->index, ring->queue,
+		  txq_stopped ? "stopped" : "active",
+		  intsts & intmsk ? "enabled" : "disabled",
+		  free_bds, ring->size,
+		  ring->prod_index, p_index & DMA_P_INDEX_MASK,
+		  ring->c_index, c_index & DMA_C_INDEX_MASK,
+		  ring->clean_ptr, ring->write_ptr,
+		  ring->cb_ptr, ring->end_ptr);
+}
+
 static void bcmgenet_timeout(struct net_device *dev)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
+	u32 int0_enable = 0;
+	u32 int1_enable = 0;
+	unsigned int q;
 
 	netif_dbg(priv, tx_err, dev, "bcmgenet_timeout\n");
 
+	bcmgenet_disable_tx_napi(priv);
+
+	for (q = 0; q < priv->hw_params->tx_queues; q++)
+		bcmgenet_dump_tx_queue(&priv->tx_rings[q]);
+	bcmgenet_dump_tx_queue(&priv->tx_rings[DESC_INDEX]);
+
+	bcmgenet_tx_reclaim_all(dev);
+
+	for (q = 0; q < priv->hw_params->tx_queues; q++)
+		int1_enable |= (1 << q);
+
+	int0_enable = UMAC_IRQ_TXDMA_DONE;
+
+	/* Re-enable TX interrupts if disabled */
+	bcmgenet_intrl2_0_writel(priv, int0_enable, INTRL2_CPU_MASK_CLEAR);
+	bcmgenet_intrl2_1_writel(priv, int1_enable, INTRL2_CPU_MASK_CLEAR);
+
+	bcmgenet_enable_tx_napi(priv);
+
 	dev->trans_start = jiffies;
 
 	dev->stats.tx_errors++;
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.h b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
index 6f2887a5e0be..6159deab8c98 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.h
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
@@ -594,6 +594,7 @@ struct bcmgenet_priv {
 	wait_queue_head_t wq;
 	struct phy_device *phydev;
 	struct device_node *phy_dn;
+	struct device_node *mdio_dn;
 	struct mii_bus *mii_bus;
 	u16 gphy_rev;
 	struct clk *clk_eee;
diff --git a/drivers/net/ethernet/broadcom/genet/bcmmii.c b/drivers/net/ethernet/broadcom/genet/bcmmii.c
index e7651b3c6c57..adf23d2ac488 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmmii.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmmii.c
@@ -47,7 +47,12 @@ static int bcmgenet_mii_read(struct mii_bus *bus, int phy_id, int location)
 			   HZ / 100);
 	ret = bcmgenet_umac_readl(priv, UMAC_MDIO_CMD);
 
-	if (ret & MDIO_READ_FAIL)
+	/* Some broken devices are known not to release the line during
+	 * turn-around, e.g: Broadcom BCM53125 external switches, so check for
+	 * that condition here and ignore the MDIO controller read failure
+	 * indication.
+	 */
+	if (!(bus->phy_ignore_ta_mask & 1 << phy_id) && (ret & MDIO_READ_FAIL))
 		return -EIO;
 
 	return ret & 0xffff;
@@ -299,9 +304,6 @@ int bcmgenet_mii_config(struct net_device *dev, bool init)
 			phy_name = "external RGMII (no delay)";
 		else
 			phy_name = "external RGMII (TX delay)";
-		reg = bcmgenet_ext_readl(priv, EXT_RGMII_OOB_CTRL);
-		reg |= RGMII_MODE_EN | id_mode_dis;
-		bcmgenet_ext_writel(priv, reg, EXT_RGMII_OOB_CTRL);
 		bcmgenet_sys_writel(priv,
 				    PORT_MODE_EXT_GPHY, SYS_PORT_CTRL);
 		break;
@@ -310,6 +312,15 @@ int bcmgenet_mii_config(struct net_device *dev, bool init)
 		return -EINVAL;
 	}
 
+	/* This is an external PHY (xMII), so we need to enable the RGMII
+	 * block for the interface to work
+	 */
+	if (priv->ext_phy) {
+		reg = bcmgenet_ext_readl(priv, EXT_RGMII_OOB_CTRL);
+		reg |= RGMII_MODE_EN | id_mode_dis;
+		bcmgenet_ext_writel(priv, reg, EXT_RGMII_OOB_CTRL);
+	}
+
 	if (init)
 		dev_info(kdev, "configuring instance for %s\n", phy_name);
 
@@ -397,6 +408,52 @@ static int bcmgenet_mii_probe(struct net_device *dev)
 	return 0;
 }
 
+/* Workaround for integrated BCM7xxx Gigabit PHYs which have a problem with
+ * their internal MDIO management controller making them fail to successfully
+ * be read from or written to for the first transaction.  We insert a dummy
+ * BMSR read here to make sure that phy_get_device() and get_phy_id() can
+ * correctly read the PHY MII_PHYSID1/2 registers and successfully register a
+ * PHY device for this peripheral.
+ *
+ * Once the PHY driver is registered, we can workaround subsequent reads from
+ * there (e.g: during system-wide power management).
+ *
+ * bus->reset is invoked before mdiobus_scan during mdiobus_register and is
+ * therefore the right location to stick that workaround. Since we do not want
+ * to read from non-existing PHYs, we either use bus->phy_mask or do a manual
+ * Device Tree scan to limit the search area.
+ */
+static int bcmgenet_mii_bus_reset(struct mii_bus *bus)
+{
+	struct net_device *dev = bus->priv;
+	struct bcmgenet_priv *priv = netdev_priv(dev);
+	struct device_node *np = priv->mdio_dn;
+	struct device_node *child = NULL;
+	u32 read_mask = 0;
+	int addr = 0;
+
+	if (!np) {
+		read_mask = 1 << priv->phy_addr;
+	} else {
+		for_each_available_child_of_node(np, child) {
+			addr = of_mdio_parse_addr(&dev->dev, child);
+			if (addr < 0)
+				continue;
+
+			read_mask |= 1 << addr;
+		}
+	}
+
+	for (addr = 0; addr < PHY_MAX_ADDR; addr++) {
+		if (read_mask & 1 << addr) {
+			dev_dbg(&dev->dev, "Workaround for PHY @ %d\n", addr);
+			mdiobus_read(bus, addr, MII_BMSR);
+		}
+	}
+
+	return 0;
+}
+
 static int bcmgenet_mii_alloc(struct bcmgenet_priv *priv)
 {
 	struct mii_bus *bus;
@@ -416,6 +473,7 @@ static int bcmgenet_mii_alloc(struct bcmgenet_priv *priv)
 	bus->parent = &priv->pdev->dev;
 	bus->read = bcmgenet_mii_read;
 	bus->write = bcmgenet_mii_write;
+	bus->reset = bcmgenet_mii_bus_reset;
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%s-%d",
 		 priv->pdev->name, priv->pdev->id);
 
@@ -432,7 +490,6 @@ static int bcmgenet_mii_of_init(struct bcmgenet_priv *priv)
 {
 	struct device_node *dn = priv->pdev->dev.of_node;
 	struct device *kdev = &priv->pdev->dev;
-	struct device_node *mdio_dn;
 	char *compat;
 	int ret;
 
@@ -440,14 +497,14 @@ static int bcmgenet_mii_of_init(struct bcmgenet_priv *priv)
 	if (!compat)
 		return -ENOMEM;
 
-	mdio_dn = of_find_compatible_node(dn, NULL, compat);
+	priv->mdio_dn = of_find_compatible_node(dn, NULL, compat);
 	kfree(compat);
-	if (!mdio_dn) {
+	if (!priv->mdio_dn) {
 		dev_err(kdev, "unable to find MDIO bus node\n");
 		return -ENODEV;
 	}
 
-	ret = of_mdiobus_register(priv->mii_bus, mdio_dn);
+	ret = of_mdiobus_register(priv->mii_bus, priv->mdio_dn);
 	if (ret) {
 		dev_err(kdev, "failed to register MDIO bus\n");
 		return ret;
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index 069952fa5d64..73c934cf6c61 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -6618,7 +6618,7 @@ static void tg3_tx(struct tg3_napi *tnapi)
 static void tg3_frag_free(bool is_frag, void *data)
 {
 	if (is_frag)
-		put_page(virt_to_head_page(data));
+		skb_free_frag(data);
 	else
 		kfree(data);
 }
diff --git a/drivers/net/ethernet/brocade/Kconfig b/drivers/net/ethernet/brocade/Kconfig
index 4e8c0b6c57d0..c4bbe54e2cad 100644
--- a/drivers/net/ethernet/brocade/Kconfig
+++ b/drivers/net/ethernet/brocade/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_BROCADE
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/brocade/bna/Makefile b/drivers/net/ethernet/brocade/bna/Makefile
index 6e10b99733a2..8584abcf5366 100644
--- a/drivers/net/ethernet/brocade/bna/Makefile
+++ b/drivers/net/ethernet/brocade/bna/Makefile
@@ -9,5 +9,3 @@ obj-$(CONFIG_BNA) += bna.o
 bna-objs := bnad.o bnad_ethtool.o bnad_debugfs.o bna_enet.o bna_tx_rx.o
 bna-objs += bfa_msgq.o bfa_ioc.o bfa_ioc_ct.o bfa_cee.o
 bna-objs += cna_fwimg.o
-
-EXTRA_CFLAGS := -Idrivers/net/bna
diff --git a/drivers/net/ethernet/brocade/bna/bfa_cee.c b/drivers/net/ethernet/brocade/bna/bfa_cee.c
index cf9f3956f198..95bc8b644a5d 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_cee.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_cee.c
@@ -282,7 +282,6 @@ bfa_nw_cee_attach(struct bfa_cee *cee, struct bfa_ioc *ioc,
 	cee->ioc = ioc;
 
 	bfa_nw_ioc_mbox_regisr(cee->ioc, BFI_MC_CEE, bfa_cee_isr, cee);
-	bfa_q_qe_init(&cee->ioc_notify);
 	bfa_ioc_notify_init(&cee->ioc_notify, bfa_cee_notify, cee);
 	bfa_nw_ioc_notify_register(cee->ioc, &cee->ioc_notify);
 }
diff --git a/drivers/net/ethernet/brocade/bna/bfa_cs.h b/drivers/net/ethernet/brocade/bna/bfa_cs.h
index af25d8e8fae0..1d11d666d408 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_cs.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_cs.h
@@ -28,19 +28,6 @@
 
 typedef void (*bfa_sm_t)(void *sm, int event);
 
-/* oc - object class eg. bfa_ioc
- * st - state, eg. reset
- * otype - object type, eg. struct bfa_ioc
- * etype - object type, eg. enum ioc_event
- */
-#define bfa_sm_state_decl(oc, st, otype, etype)			\
-	static void oc ## _sm_ ## st(otype * fsm, etype event)
-
-#define bfa_sm_set_state(_sm, _state)	((_sm)->sm = (bfa_sm_t)(_state))
-#define bfa_sm_send_event(_sm, _event)	((_sm)->sm((_sm), (_event)))
-#define bfa_sm_get_state(_sm)		((_sm)->sm)
-#define bfa_sm_cmp_state(_sm, _state)	((_sm)->sm == (bfa_sm_t)(_state))
-
 /* For converting from state machine function to state encoding. */
 struct bfa_sm_table {
 	bfa_sm_t	sm;	/*!< state machine function	*/
@@ -67,7 +54,6 @@ typedef void (*bfa_fsm_t)(void *fsm, int event);
 } while (0)
 
 #define bfa_fsm_send_event(_fsm, _event)	((_fsm)->fsm((_fsm), (_event)))
-#define bfa_fsm_get_state(_fsm)			((_fsm)->fsm)
 #define bfa_fsm_cmp_state(_fsm, _state)					\
 	((_fsm)->fsm == (bfa_fsm_t)(_state))
 
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs.h b/drivers/net/ethernet/brocade/bna/bfa_defs.h
index 3bfd9da92630..d152b3fa6c54 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs.h
@@ -24,7 +24,6 @@
 #include "bfa_defs_status.h"
 #include "bfa_defs_mfg_comm.h"
 
-#define BFA_STRING_32	32
 #define BFA_VERSION_LEN 64
 
 /* ---------------------- adapter definitions ------------ */
@@ -55,7 +54,7 @@ struct bfa_adapter_attr {
 	char		optrom_ver[BFA_VERSION_LEN];
 	char		os_type[BFA_ADAPTER_OS_TYPE_LEN];
 	struct bfa_mfg_vpd vpd;
-	struct mac mac;
+	u8		mac[ETH_ALEN];
 
 	u8		nports;
 	u8		max_speed;
@@ -187,8 +186,6 @@ enum {
 #define BFA_MFG_SUPPLIER_SERIALNUM_SIZE		20
 #define BFA_MFG_SUPPLIER_REVISION_SIZE		4
 
-#pragma pack(1)
-
 /* BFA adapter manufacturing block definition.
  *
  * All numerical fields are in big-endian format.
@@ -211,7 +208,7 @@ struct bfa_mfg_block {
 	char	supplier_partnum[STRSZ(BFA_MFG_SUPPLIER_PARTNUM_SIZE)];
 	char	supplier_serialnum[STRSZ(BFA_MFG_SUPPLIER_SERIALNUM_SIZE)];
 	char	supplier_revision[STRSZ(BFA_MFG_SUPPLIER_REVISION_SIZE)];
-	mac_t	mfg_mac;	/* base mac address */
+	u8	mfg_mac[ETH_ALEN]; /* base mac address */
 	u8	num_mac;	/* number of mac addresses */
 	u8	rsv2;
 	u32	card_type;	/* card type          */
@@ -227,9 +224,7 @@ struct bfa_mfg_block {
 	char	initial_mode[8]; /* initial mode: hba/cna/nic */
 	u8	rsv4[84];
 	u8	md5_chksum[BFA_MFG_CHKSUM_SIZE]; /* md5 checksum */
-};
-
-#pragma pack()
+} __packed;
 
 /* ---------------------- pci definitions ------------ */
 
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h b/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
index a37326d44fbb..f048887cbb81 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs_cna.h
@@ -109,8 +109,6 @@ union bfa_port_stats_u {
 	struct bfa_port_eth_stats eth;
 };
 
-#pragma pack(1)
-
 #define BFA_CEE_LLDP_MAX_STRING_LEN (128)
 #define BFA_CEE_DCBX_MAX_PRIORITY	(8)
 #define BFA_CEE_DCBX_MAX_PGID		(8)
@@ -133,7 +131,7 @@ struct bfa_cee_lldp_str {
 	u8 len;
 	u8 rsvd[2];
 	u8 value[BFA_CEE_LLDP_MAX_STRING_LEN];
-};
+} __packed;
 
 /* LLDP parameters */
 struct bfa_cee_lldp_cfg {
@@ -145,7 +143,7 @@ struct bfa_cee_lldp_cfg {
 	struct bfa_cee_lldp_str mgmt_addr;
 	u16 time_to_live;
 	u16 enabled_system_cap;
-};
+} __packed;
 
 enum bfa_cee_dcbx_version {
 	DCBX_PROTOCOL_PRECEE	= 1,
@@ -171,7 +169,7 @@ struct bfa_cee_dcbx_cfg {
 	u8 lls_fcoe; /* FCoE Logical Link Status */
 	u8 lls_lan; /* LAN Logical Link Status */
 	u8 rsvd[2];
-};
+} __packed;
 
 /* CEE status */
 /* Making this to tri-state for the benefit of port list command */
@@ -188,11 +186,11 @@ struct bfa_cee_attr {
 	u8 error_reason;
 	struct bfa_cee_lldp_cfg lldp_remote;
 	struct bfa_cee_dcbx_cfg dcbx_remote;
-	mac_t src_mac;
+	u8 src_mac[ETH_ALEN];
 	u8 link_speed;
 	u8 nw_priority;
 	u8 filler[2];
-};
+} __packed;
 
 /* LLDP/DCBX/CEE Statistics */
 struct bfa_cee_stats {
@@ -214,8 +212,6 @@ struct bfa_cee_stats {
 	u32	cee_status_up;		/*!< CEE status up */
 	u32	cee_hw_cfg_changed;	/*!< CEE hw cfg changed */
 	u32	cee_rx_invalid_cfg;	/*!< CEE invalid cfg */
-};
-
-#pragma pack()
+} __packed;
 
 #endif	/* __BFA_DEFS_CNA_H__ */
diff --git a/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h b/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
index 7a45cd0b594d..7e17451c94d1 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_defs_mfg_comm.h
@@ -59,8 +59,6 @@ enum {
 	BFA_MFG_TYPE_INVALID = 0,	 /*!< Invalid card type		*/
 };
 
-#pragma pack(1)
-
 /* Check if Mezz card */
 #define bfa_mfg_is_mezz(type) (( \
 	(type) == BFA_MFG_TYPE_JAYHAWK || \
@@ -77,7 +75,7 @@ enum {
 	CB_GPIO_FC4P2   = (4),		/*!< 4G 2port FC card		*/
 	CB_GPIO_FC4P1   = (5),		/*!< 4G 1port FC card		*/
 	CB_GPIO_DFLY    = (6),		/*!< 8G 2port FC mezzanine card	*/
-	CB_GPIO_PROTO   = (1 << 7)	/*!< 8G 2port FC prototypes	*/
+	CB_GPIO_PROTO   = BIT(7)	/*!< 8G 2port FC prototypes	*/
 };
 
 #define bfa_mfg_adapter_prop_init_gpio(gpio, card_type, prop)	\
@@ -86,7 +84,7 @@ do {								\
 		(prop) |= BFI_ADAPTER_PROTO;			\
 		(gpio) &= ~CB_GPIO_PROTO;			\
 	}							\
-	switch ((gpio)) {					\
+	switch (gpio) {						\
 	case CB_GPIO_TTV:					\
 		(prop) |= BFI_ADAPTER_TTV;			\
 	case CB_GPIO_DFLY:					\
@@ -148,8 +146,6 @@ struct bfa_mfg_vpd {
 	u8	len;		/*!< vpd data length excluding header */
 	u8	rsv;
 	u8		data[BFA_MFG_VPD_LEN];	/*!< vpd data */
-};
-
-#pragma pack()
+} __packed;
 
 #endif /* __BFA_DEFS_MFG_H__ */
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc.c b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
index 594a2ab36d31..b7a0f7879de2 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc.c
@@ -23,14 +23,6 @@
 
 /* IOC local definitions */
 
-#define bfa_ioc_state_disabled(__sm)			\
-	(((__sm) == BFI_IOC_UNINIT) ||			\
-	 ((__sm) == BFI_IOC_INITING) ||			\
-	 ((__sm) == BFI_IOC_HWINIT) ||			\
-	 ((__sm) == BFI_IOC_DISABLED) ||		\
-	 ((__sm) == BFI_IOC_FAIL) ||			\
-	 ((__sm) == BFI_IOC_CFG_DISABLED))
-
 /* Asic specific macros : see bfa_hw_cb.c and bfa_hw_ct.c for details. */
 
 #define bfa_ioc_firmware_lock(__ioc)			\
@@ -57,12 +49,6 @@
 			((__ioc)->ioc_hwif->ioc_get_fwstate(__ioc))
 #define bfa_ioc_set_alt_ioc_fwstate(__ioc, __fwstate)		\
 		((__ioc)->ioc_hwif->ioc_set_alt_fwstate(__ioc, __fwstate))
-#define bfa_ioc_get_alt_ioc_fwstate(__ioc)		\
-			((__ioc)->ioc_hwif->ioc_get_alt_fwstate(__ioc))
-
-#define bfa_ioc_mbox_cmd_pending(__ioc)		\
-			(!list_empty(&((__ioc)->mbox_mod.cmd_q)) || \
-			readl((__ioc)->ioc_regs.hfn_mbox_cmd))
 
 static bool bfa_nw_auto_recover = true;
 
@@ -1105,12 +1091,9 @@ static void
 bfa_ioc_event_notify(struct bfa_ioc *ioc, enum bfa_ioc_event event)
 {
 	struct bfa_ioc_notify *notify;
-	struct list_head			*qe;
 
-	list_for_each(qe, &ioc->notify_q) {
-		notify = (struct bfa_ioc_notify *)qe;
+	list_for_each_entry(notify, &ioc->notify_q, qe)
 		notify->cbfn(notify->cbarg, event);
-	}
 }
 
 static void
@@ -1321,7 +1304,7 @@ bfa_nw_ioc_fwver_get(struct bfa_ioc *ioc, struct bfi_ioc_image_hdr *fwhdr)
 	for (i = 0; i < (sizeof(struct bfi_ioc_image_hdr) / sizeof(u32));
 	     i++) {
 		fwsig[i] =
-			swab32(readl((loff) + (ioc->ioc_regs.smem_page_start)));
+			swab32(readl(loff + ioc->ioc_regs.smem_page_start));
 		loff += sizeof(u32);
 	}
 }
@@ -1387,7 +1370,7 @@ static enum bfi_ioc_img_ver_cmp
 bfa_ioc_fw_ver_patch_cmp(struct bfi_ioc_image_hdr *base_fwhdr,
 			 struct bfi_ioc_image_hdr *fwhdr_to_cmp)
 {
-	if (bfa_ioc_fw_ver_compatible(base_fwhdr, fwhdr_to_cmp) == false)
+	if (!bfa_ioc_fw_ver_compatible(base_fwhdr, fwhdr_to_cmp))
 		return BFI_IOC_IMG_VER_INCOMP;
 
 	if (fwhdr_to_cmp->fwver.patch > base_fwhdr->fwver.patch)
@@ -1398,7 +1381,7 @@ bfa_ioc_fw_ver_patch_cmp(struct bfi_ioc_image_hdr *base_fwhdr,
 	/* GA takes priority over internal builds of the same patch stream.
 	 * At this point major minor maint and patch numbers are same.
 	 */
-	if (fwhdr_is_ga(base_fwhdr) == true)
+	if (fwhdr_is_ga(base_fwhdr))
 		if (fwhdr_is_ga(fwhdr_to_cmp))
 			return BFI_IOC_IMG_VER_SAME;
 		else
@@ -1692,7 +1675,7 @@ bfa_raw_sem_get(void __iomem *bar)
 {
 	int	locked;
 
-	locked = readl((bar + FLASH_SEM_LOCK_REG));
+	locked = readl(bar + FLASH_SEM_LOCK_REG);
 
 	return !locked;
 }
@@ -1912,10 +1895,8 @@ bfa_ioc_hwinit(struct bfa_ioc *ioc, bool force)
 }
 
 void
-bfa_nw_ioc_timeout(void *ioc_arg)
+bfa_nw_ioc_timeout(struct bfa_ioc *ioc)
 {
-	struct bfa_ioc *ioc = (struct bfa_ioc *) ioc_arg;
-
 	bfa_fsm_send_event(ioc, IOC_E_TIMEOUT);
 }
 
@@ -1980,10 +1961,9 @@ bfa_ioc_send_getattr(struct bfa_ioc *ioc)
 }
 
 void
-bfa_nw_ioc_hb_check(void *cbarg)
+bfa_nw_ioc_hb_check(struct bfa_ioc *ioc)
 {
-	struct bfa_ioc *ioc = cbarg;
-	u32	hb_count;
+	u32 hb_count;
 
 	hb_count = readl(ioc->ioc_regs.heartbeat);
 	if (ioc->hb_count == hb_count) {
@@ -2069,8 +2049,8 @@ bfa_ioc_download_fw(struct bfa_ioc *ioc, u32 boot_type,
 		/**
 		 * write smem
 		 */
-		writel((swab32(fwimg[BFA_IOC_FLASH_OFFSET_IN_CHUNK(i)])),
-			      ((ioc->ioc_regs.smem_page_start) + (loff)));
+		writel(swab32(fwimg[BFA_IOC_FLASH_OFFSET_IN_CHUNK(i)]),
+		       ioc->ioc_regs.smem_page_start + loff);
 
 		loff += sizeof(u32);
 
@@ -2177,7 +2157,8 @@ bfa_ioc_mbox_poll(struct bfa_ioc *ioc)
 	/**
 	 * Enqueue command to firmware.
 	 */
-	bfa_q_deq(&mod->cmd_q, &cmd);
+	cmd = list_first_entry(&mod->cmd_q, struct bfa_mbox_cmd, qe);
+	list_del(&cmd->qe);
 	bfa_ioc_mbox_send(ioc, cmd->msg, sizeof(cmd->msg));
 
 	/**
@@ -2198,8 +2179,10 @@ bfa_ioc_mbox_flush(struct bfa_ioc *ioc)
 	struct bfa_ioc_mbox_mod *mod = &ioc->mbox_mod;
 	struct bfa_mbox_cmd *cmd;
 
-	while (!list_empty(&mod->cmd_q))
-		bfa_q_deq(&mod->cmd_q, &cmd);
+	while (!list_empty(&mod->cmd_q)) {
+		cmd = list_first_entry(&mod->cmd_q, struct bfa_mbox_cmd, qe);
+		list_del(&cmd->qe);
+	}
 }
 
 /**
@@ -2223,14 +2206,14 @@ bfa_nw_ioc_smem_read(struct bfa_ioc *ioc, void *tbuf, u32 soff, u32 sz)
 	/*
 	 *  Hold semaphore to serialize pll init and fwtrc.
 	*/
-	if (bfa_nw_ioc_sem_get(ioc->ioc_regs.ioc_init_sem_reg) == 0)
+	if (!bfa_nw_ioc_sem_get(ioc->ioc_regs.ioc_init_sem_reg))
 		return 1;
 
 	writel(pgnum, ioc->ioc_regs.host_page_num_fn);
 
 	len = sz/sizeof(u32);
 	for (i = 0; i < len; i++) {
-		r32 = swab32(readl((loff) + (ioc->ioc_regs.smem_page_start)));
+		r32 = swab32(readl(loff + ioc->ioc_regs.smem_page_start));
 		buf[i] = be32_to_cpu(r32);
 		loff += sizeof(u32);
 
@@ -2278,7 +2261,7 @@ bfa_nw_ioc_debug_save_ftrc(struct bfa_ioc *ioc)
 	int tlen;
 
 	if (ioc->dbg_fwsave_once) {
-		ioc->dbg_fwsave_once = 0;
+		ioc->dbg_fwsave_once = false;
 		if (ioc->dbg_fwsave_len) {
 			tlen = ioc->dbg_fwsave_len;
 			bfa_nw_ioc_debug_fwtrc(ioc, ioc->dbg_fwsave, &tlen);
@@ -2414,7 +2397,7 @@ bfa_ioc_boot(struct bfa_ioc *ioc, enum bfi_fwboot_type boot_type,
 	if (status == BFA_STATUS_OK)
 		bfa_ioc_lpu_start(ioc);
 	else
-		bfa_nw_iocpf_timeout(ioc);
+		bfa_fsm_send_event(&ioc->iocpf, IOCPF_E_TIMEOUT);
 
 	return status;
 }
@@ -2796,7 +2779,7 @@ bfa_ioc_get_adapter_attr(struct bfa_ioc *ioc,
 		ad_attr->prototype = 0;
 
 	ad_attr->pwwn = bfa_ioc_get_pwwn(ioc);
-	ad_attr->mac  = bfa_nw_ioc_get_mac(ioc);
+	bfa_nw_ioc_get_mac(ioc, ad_attr->mac);
 
 	ad_attr->pcie_gen = ioc_attr->pcie_gen;
 	ad_attr->pcie_lanes = ioc_attr->pcie_lanes;
@@ -2942,10 +2925,10 @@ bfa_ioc_get_pwwn(struct bfa_ioc *ioc)
 	return ioc->attr->pwwn;
 }
 
-mac_t
-bfa_nw_ioc_get_mac(struct bfa_ioc *ioc)
+void
+bfa_nw_ioc_get_mac(struct bfa_ioc *ioc, u8 *mac)
 {
-	return ioc->attr->mac;
+	ether_addr_copy(mac, ioc->attr->mac);
 }
 
 /* Firmware failure detected. Start recovery actions. */
@@ -2997,9 +2980,8 @@ bfa_iocpf_stop(struct bfa_ioc *ioc)
 }
 
 void
-bfa_nw_iocpf_timeout(void *ioc_arg)
+bfa_nw_iocpf_timeout(struct bfa_ioc *ioc)
 {
-	struct bfa_ioc  *ioc = (struct bfa_ioc *) ioc_arg;
 	enum bfa_iocpf_state iocpf_st;
 
 	iocpf_st = bfa_sm_to_state(iocpf_sm_table, ioc->iocpf.fsm);
@@ -3011,10 +2993,8 @@ bfa_nw_iocpf_timeout(void *ioc_arg)
 }
 
 void
-bfa_nw_iocpf_sem_timeout(void *ioc_arg)
+bfa_nw_iocpf_sem_timeout(struct bfa_ioc *ioc)
 {
-	struct bfa_ioc  *ioc = (struct bfa_ioc *) ioc_arg;
-
 	bfa_ioc_hw_sem_get(ioc);
 }
 
@@ -3029,7 +3009,7 @@ bfa_ioc_poll_fwinit(struct bfa_ioc *ioc)
 	}
 
 	if (ioc->iocpf.poll_time >= BFA_IOC_TOV) {
-		bfa_nw_iocpf_timeout(ioc);
+		bfa_fsm_send_event(&ioc->iocpf, IOCPF_E_TIMEOUT);
 	} else {
 		ioc->iocpf.poll_time += BFA_IOC_POLL_TOV;
 		mod_timer(&ioc->iocpf_timer, jiffies +
@@ -3245,7 +3225,6 @@ bfa_nw_flash_attach(struct bfa_flash *flash, struct bfa_ioc *ioc, void *dev)
 	flash->op_busy = 0;
 
 	bfa_nw_ioc_mbox_regisr(flash->ioc, BFI_MC_FLASH, bfa_flash_intr, flash);
-	bfa_q_qe_init(&flash->ioc_notify);
 	bfa_ioc_notify_init(&flash->ioc_notify, bfa_flash_notify, flash);
 	list_add_tail(&flash->ioc_notify.qe, &flash->ioc->notify_q);
 }
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc.h b/drivers/net/ethernet/brocade/bna/bfa_ioc.h
index effb7156e7a4..2c0b4c076355 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc.h
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc.h
@@ -232,12 +232,6 @@ struct bfa_ioc_hwif {
 #define bfa_ioc_asic_gen(__ioc)		((__ioc)->asic_gen)
 #define bfa_ioc_is_default(__ioc)	\
 	(bfa_ioc_pcifn(__ioc) == bfa_ioc_portid(__ioc))
-#define bfa_ioc_fetch_stats(__ioc, __stats) \
-		(((__stats)->drv_stats) = (__ioc)->stats)
-#define bfa_ioc_clr_stats(__ioc)	\
-		memset(&(__ioc)->stats, 0, sizeof((__ioc)->stats))
-#define bfa_ioc_maxfrsize(__ioc)	((__ioc)->attr->maxfrsize)
-#define bfa_ioc_rx_bbcredit(__ioc)	((__ioc)->attr->rx_bbcredit)
 #define bfa_ioc_speed_sup(__ioc)	\
 	BFI_ADAPTER_GETP(SPEED, (__ioc)->attr->adapter_prop)
 #define bfa_ioc_get_nports(__ioc)	\
@@ -268,13 +262,6 @@ void bfa_nw_ioc_mbox_regisr(struct bfa_ioc *ioc, enum bfi_mclass mc,
 	((__ioc)->ioc_hwif->ioc_pll_init((__ioc)->pcidev.pci_bar_kva, \
 			   (__ioc)->asic_mode))
 
-#define	bfa_ioc_isr_mode_set(__ioc, __msix) do {			\
-	if ((__ioc)->ioc_hwif->ioc_isr_mode_set)			\
-		((__ioc)->ioc_hwif->ioc_isr_mode_set(__ioc, __msix));	\
-} while (0)
-#define	bfa_ioc_ownership_reset(__ioc)				\
-			((__ioc)->ioc_hwif->ioc_ownership_reset(__ioc))
-
 #define bfa_ioc_lpu_read_stat(__ioc) do {				\
 		if ((__ioc)->ioc_hwif->ioc_lpu_read_stat)		\
 			((__ioc)->ioc_hwif->ioc_lpu_read_stat(__ioc));	\
@@ -309,7 +296,7 @@ void bfa_nw_ioc_fwver_get(struct bfa_ioc *ioc,
 			struct bfi_ioc_image_hdr *fwhdr);
 bool bfa_nw_ioc_fwver_cmp(struct bfa_ioc *ioc,
 			struct bfi_ioc_image_hdr *fwhdr);
-mac_t bfa_nw_ioc_get_mac(struct bfa_ioc *ioc);
+void bfa_nw_ioc_get_mac(struct bfa_ioc *ioc, u8 *mac);
 void bfa_nw_ioc_debug_memclaim(struct bfa_ioc *ioc, void *dbg_fwsave);
 int bfa_nw_ioc_debug_fwtrc(struct bfa_ioc *ioc, void *trcdata, int *trclen);
 int bfa_nw_ioc_debug_fwsave(struct bfa_ioc *ioc, void *trcdata, int *trclen);
@@ -317,10 +304,10 @@ int bfa_nw_ioc_debug_fwsave(struct bfa_ioc *ioc, void *trcdata, int *trclen);
 /*
  * Timeout APIs
  */
-void bfa_nw_ioc_timeout(void *ioc);
-void bfa_nw_ioc_hb_check(void *ioc);
-void bfa_nw_iocpf_timeout(void *ioc);
-void bfa_nw_iocpf_sem_timeout(void *ioc);
+void bfa_nw_ioc_timeout(struct bfa_ioc *ioc);
+void bfa_nw_ioc_hb_check(struct bfa_ioc *ioc);
+void bfa_nw_iocpf_timeout(struct bfa_ioc *ioc);
+void bfa_nw_iocpf_sem_timeout(struct bfa_ioc *ioc);
 
 /*
  * F/W Image Size & Chunk
diff --git a/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c b/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
index 2e72445dbb4f..74e5ed55ac01 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_ioc_ct.c
@@ -23,8 +23,7 @@
 #include "bfi_reg.h"
 #include "bfa_defs.h"
 
-#define bfa_ioc_ct_sync_pos(__ioc)	\
-		((u32) (1 << bfa_ioc_pcifn(__ioc)))
+#define bfa_ioc_ct_sync_pos(__ioc)	BIT(bfa_ioc_pcifn(__ioc))
 #define BFA_IOC_SYNC_REQD_SH		16
 #define bfa_ioc_ct_get_sync_ackd(__val) (__val & 0x0000ffff)
 #define bfa_ioc_ct_clear_sync_ackd(__val) (__val & 0xffff0000)
@@ -536,7 +535,7 @@ bfa_ioc_ct_sync_ack(struct bfa_ioc *ioc)
 {
 	u32 r32 = readl(ioc->ioc_regs.ioc_fail_sync);
 
-	writel((r32 | bfa_ioc_ct_sync_pos(ioc)), ioc->ioc_regs.ioc_fail_sync);
+	writel(r32 | bfa_ioc_ct_sync_pos(ioc), ioc->ioc_regs.ioc_fail_sync);
 }
 
 static bool
@@ -667,7 +666,7 @@ bfa_ioc_ct_pll_init(void __iomem *rb, enum bfi_asic_mode asic_mode)
 		writel(__PMM_1T_RESET_P, (rb + PMM_1T_RESET_REG_P0));
 		writel(__PMM_1T_RESET_P, (rb + PMM_1T_RESET_REG_P1));
 	}
-	r32 = readl((rb + PSS_CTL_REG));
+	r32 = readl(rb + PSS_CTL_REG);
 	r32 &= ~__PSS_LMEM_RESET;
 	writel(r32, (rb + PSS_CTL_REG));
 	udelay(1000);
@@ -678,7 +677,7 @@ bfa_ioc_ct_pll_init(void __iomem *rb, enum bfi_asic_mode asic_mode)
 
 	writel(__EDRAM_BISTR_START, (rb + MBIST_CTL_REG));
 	udelay(1000);
-	r32 = readl((rb + MBIST_STAT_REG));
+	r32 = readl(rb + MBIST_STAT_REG);
 	writel(0, (rb + MBIST_CTL_REG));
 	return BFA_STATUS_OK;
 }
@@ -691,7 +690,7 @@ bfa_ioc_ct2_sclk_init(void __iomem *rb)
 	/*
 	 * put s_clk PLL and PLL FSM in reset
 	 */
-	r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
+	r32 = readl(rb + CT2_APP_PLL_SCLK_CTL_REG);
 	r32 &= ~(__APP_PLL_SCLK_ENABLE | __APP_PLL_SCLK_LRESETN);
 	r32 |= (__APP_PLL_SCLK_ENARST | __APP_PLL_SCLK_BYPASS |
 		__APP_PLL_SCLK_LOGIC_SOFT_RESET);
@@ -701,28 +700,28 @@ bfa_ioc_ct2_sclk_init(void __iomem *rb)
 	 * Ignore mode and program for the max clock (which is FC16)
 	 * Firmware/NFC will do the PLL init appropriately
 	 */
-	r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
+	r32 = readl(rb + CT2_APP_PLL_SCLK_CTL_REG);
 	r32 &= ~(__APP_PLL_SCLK_REFCLK_SEL | __APP_PLL_SCLK_CLK_DIV2);
 	writel(r32, (rb + CT2_APP_PLL_SCLK_CTL_REG));
 
 	/*
 	 * while doing PLL init dont clock gate ethernet subsystem
 	 */
-	r32 = readl((rb + CT2_CHIP_MISC_PRG));
-	writel((r32 | __ETH_CLK_ENABLE_PORT0),
-				(rb + CT2_CHIP_MISC_PRG));
+	r32 = readl(rb + CT2_CHIP_MISC_PRG);
+	writel(r32 | __ETH_CLK_ENABLE_PORT0,
+	       rb + CT2_CHIP_MISC_PRG);
 
-	r32 = readl((rb + CT2_PCIE_MISC_REG));
-	writel((r32 | __ETH_CLK_ENABLE_PORT1),
-				(rb + CT2_PCIE_MISC_REG));
+	r32 = readl(rb + CT2_PCIE_MISC_REG);
+	writel(r32 | __ETH_CLK_ENABLE_PORT1,
+	       rb + CT2_PCIE_MISC_REG);
 
 	/*
 	 * set sclk value
 	 */
-	r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
+	r32 = readl(rb + CT2_APP_PLL_SCLK_CTL_REG);
 	r32 &= (__P_SCLK_PLL_LOCK | __APP_PLL_SCLK_REFCLK_SEL |
 		__APP_PLL_SCLK_CLK_DIV2);
-	writel(r32 | 0x1061731b, (rb + CT2_APP_PLL_SCLK_CTL_REG));
+	writel(r32 | 0x1061731b, rb + CT2_APP_PLL_SCLK_CTL_REG);
 
 	/*
 	 * poll for s_clk lock or delay 1ms
@@ -743,28 +742,28 @@ bfa_ioc_ct2_lclk_init(void __iomem *rb)
 	/*
 	 * put l_clk PLL and PLL FSM in reset
 	 */
-	r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
+	r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
 	r32 &= ~(__APP_PLL_LCLK_ENABLE | __APP_PLL_LCLK_LRESETN);
 	r32 |= (__APP_PLL_LCLK_ENARST | __APP_PLL_LCLK_BYPASS |
 		__APP_PLL_LCLK_LOGIC_SOFT_RESET);
-	writel(r32, (rb + CT2_APP_PLL_LCLK_CTL_REG));
+	writel(r32, rb + CT2_APP_PLL_LCLK_CTL_REG);
 
 	/*
 	 * set LPU speed (set for FC16 which will work for other modes)
 	 */
-	r32 = readl((rb + CT2_CHIP_MISC_PRG));
+	r32 = readl(rb + CT2_CHIP_MISC_PRG);
 	writel(r32, (rb + CT2_CHIP_MISC_PRG));
 
 	/*
 	 * set LPU half speed (set for FC16 which will work for other modes)
 	 */
-	r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
-	writel(r32, (rb + CT2_APP_PLL_LCLK_CTL_REG));
+	r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
+	writel(r32, rb + CT2_APP_PLL_LCLK_CTL_REG);
 
 	/*
 	 * set lclk for mode (set for FC16)
 	 */
-	r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
+	r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
 	r32 &= (__P_LCLK_PLL_LOCK | __APP_LPUCLK_HALFSPEED);
 	r32 |= 0x20c1731b;
 	writel(r32, (rb + CT2_APP_PLL_LCLK_CTL_REG));
@@ -780,14 +779,14 @@ bfa_ioc_ct2_mem_init(void __iomem *rb)
 {
 	u32 r32;
 
-	r32 = readl((rb + PSS_CTL_REG));
+	r32 = readl(rb + PSS_CTL_REG);
 	r32 &= ~__PSS_LMEM_RESET;
-	writel(r32, (rb + PSS_CTL_REG));
+	writel(r32, rb + PSS_CTL_REG);
 	udelay(1000);
 
-	writel(__EDRAM_BISTR_START, (rb + CT2_MBIST_CTL_REG));
+	writel(__EDRAM_BISTR_START, rb + CT2_MBIST_CTL_REG);
 	udelay(1000);
-	writel(0, (rb + CT2_MBIST_CTL_REG));
+	writel(0, rb + CT2_MBIST_CTL_REG);
 }
 
 static void
@@ -801,22 +800,22 @@ bfa_ioc_ct2_mac_reset(void __iomem *rb)
 	/*
 	 * release soft reset on s_clk & l_clk
 	 */
-	r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
-	writel((r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET),
-			(rb + CT2_APP_PLL_SCLK_CTL_REG));
+	r32 = readl(rb + CT2_APP_PLL_SCLK_CTL_REG);
+	writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
+	       rb + CT2_APP_PLL_SCLK_CTL_REG);
 
 	/*
 	 * release soft reset on s_clk & l_clk
 	 */
-	r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
-	writel((r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET),
-			(rb + CT2_APP_PLL_LCLK_CTL_REG));
+	r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
+	writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
+	       rb + CT2_APP_PLL_LCLK_CTL_REG);
 
 	/* put port0, port1 MAC & AHB in reset */
-	writel((__CSI_MAC_RESET | __CSI_MAC_AHB_RESET),
-			(rb + CT2_CSI_MAC_CONTROL_REG(0)));
-	writel((__CSI_MAC_RESET | __CSI_MAC_AHB_RESET),
-			(rb + CT2_CSI_MAC_CONTROL_REG(1)));
+	writel(__CSI_MAC_RESET | __CSI_MAC_AHB_RESET,
+	       rb + CT2_CSI_MAC_CONTROL_REG(0));
+	writel(__CSI_MAC_RESET | __CSI_MAC_AHB_RESET,
+	       rb + CT2_CSI_MAC_CONTROL_REG(1));
 }
 
 #define CT2_NFC_MAX_DELAY       1000
@@ -861,8 +860,8 @@ bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode asic_mode)
 
 	nfc_ver = readl(rb + CT2_RSC_GPR15_REG);
 
-	if ((wgn == (__A2T_AHB_LOAD | __WGN_READY)) &&
-		(nfc_ver >= CT2_NFC_VER_VALID)) {
+	if (wgn == (__A2T_AHB_LOAD | __WGN_READY) &&
+	    nfc_ver >= CT2_NFC_VER_VALID) {
 		if (bfa_ioc_ct2_nfc_halted(rb))
 			bfa_ioc_ct2_nfc_resume(rb);
 		writel(__RESET_AND_START_SCLK_LCLK_PLLS,
@@ -899,19 +898,19 @@ bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode asic_mode)
 		bfa_ioc_ct2_lclk_init(rb);
 
 		/* release soft reset on s_clk & l_clk */
-		r32 = readl((rb + CT2_APP_PLL_SCLK_CTL_REG));
+		r32 = readl(rb + CT2_APP_PLL_SCLK_CTL_REG);
 		writel(r32 & ~__APP_PLL_SCLK_LOGIC_SOFT_RESET,
 				rb + CT2_APP_PLL_SCLK_CTL_REG);
-		r32 = readl((rb + CT2_APP_PLL_LCLK_CTL_REG));
+		r32 = readl(rb + CT2_APP_PLL_LCLK_CTL_REG);
 		writel(r32 & ~__APP_PLL_LCLK_LOGIC_SOFT_RESET,
 				rb + CT2_APP_PLL_LCLK_CTL_REG);
 	}
 
 	/* Announce flash device presence, if flash was corrupted. */
 	if (wgn == (__WGN_READY | __GLBL_PF_VF_CFG_RDY)) {
-		r32 = readl((rb + PSS_GPIO_OUT_REG));
+		r32 = readl(rb + PSS_GPIO_OUT_REG);
 		writel(r32 & ~1, rb + PSS_GPIO_OUT_REG);
-		r32 = readl((rb + PSS_GPIO_OE_REG));
+		r32 = readl(rb + PSS_GPIO_OE_REG);
 		writel(r32 | 1, rb + PSS_GPIO_OE_REG);
 	}
 
@@ -919,27 +918,27 @@ bfa_ioc_ct2_pll_init(void __iomem *rb, enum bfi_asic_mode asic_mode)
 	 * Mask the interrupts and clear any
 	 * pending interrupts left by BIOS/EFI
 	 */
-	writel(1, (rb + CT2_LPU0_HOSTFN_MBOX0_MSK));
-	writel(1, (rb + CT2_LPU1_HOSTFN_MBOX0_MSK));
+	writel(1, rb + CT2_LPU0_HOSTFN_MBOX0_MSK);
+	writel(1, rb + CT2_LPU1_HOSTFN_MBOX0_MSK);
 
 	/* For first time initialization, no need to clear interrupts */
 	r32 = readl(rb + HOST_SEM5_REG);
 	if (r32 & 0x1) {
-		r32 = readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
+		r32 = readl(rb + CT2_LPU0_HOSTFN_CMD_STAT);
 		if (r32 == 1) {
-			writel(1, (rb + CT2_LPU0_HOSTFN_CMD_STAT));
-			readl((rb + CT2_LPU0_HOSTFN_CMD_STAT));
+			writel(1, rb + CT2_LPU0_HOSTFN_CMD_STAT);
+			readl(rb + CT2_LPU0_HOSTFN_CMD_STAT);
 		}
-		r32 = readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
+		r32 = readl(rb + CT2_LPU1_HOSTFN_CMD_STAT);
 		if (r32 == 1) {
-			writel(1, (rb + CT2_LPU1_HOSTFN_CMD_STAT));
-			readl((rb + CT2_LPU1_HOSTFN_CMD_STAT));
+			writel(1, rb + CT2_LPU1_HOSTFN_CMD_STAT);
+			readl(rb + CT2_LPU1_HOSTFN_CMD_STAT);
 		}
 	}
 
 	bfa_ioc_ct2_mem_init(rb);
 
-	writel(BFI_IOC_UNINIT, (rb + CT2_BFA_IOC0_STATE_REG));
-	writel(BFI_IOC_UNINIT, (rb + CT2_BFA_IOC1_STATE_REG));
+	writel(BFI_IOC_UNINIT, rb + CT2_BFA_IOC0_STATE_REG);
+	writel(BFI_IOC_UNINIT, rb + CT2_BFA_IOC1_STATE_REG);
 	return BFA_STATUS_OK;
 }
diff --git a/drivers/net/ethernet/brocade/bna/bfa_msgq.c b/drivers/net/ethernet/brocade/bna/bfa_msgq.c
index c07d5b9372f4..9c5bb24e8abb 100644
--- a/drivers/net/ethernet/brocade/bna/bfa_msgq.c
+++ b/drivers/net/ethernet/brocade/bna/bfa_msgq.c
@@ -66,8 +66,9 @@ cmdq_sm_stopped_entry(struct bfa_msgq_cmdq *cmdq)
 	cmdq->offset = 0;
 	cmdq->bytes_to_copy = 0;
 	while (!list_empty(&cmdq->pending_q)) {
-		bfa_q_deq(&cmdq->pending_q, &cmdq_ent);
-		bfa_q_qe_init(&cmdq_ent->qe);
+		cmdq_ent = list_first_entry(&cmdq->pending_q,
+					    struct bfa_msgq_cmd_entry, qe);
+		list_del(&cmdq_ent->qe);
 		call_cmdq_ent_cbfn(cmdq_ent, BFA_STATUS_FAILED);
 	}
 }
@@ -242,8 +243,8 @@ bfa_msgq_cmdq_ci_update(struct bfa_msgq_cmdq *cmdq, struct bfi_mbmsg *mb)
 
 	/* Walk through pending list to see if the command can be posted */
 	while (!list_empty(&cmdq->pending_q)) {
-		cmd =
-		(struct bfa_msgq_cmd_entry *)bfa_q_first(&cmdq->pending_q);
+		cmd = list_first_entry(&cmdq->pending_q,
+				       struct bfa_msgq_cmd_entry, qe);
 		if (ntohs(cmd->msg_hdr->num_entries) <=
 			BFA_MSGQ_FREE_CNT(cmdq)) {
 			list_del(&cmd->qe);
@@ -615,7 +616,6 @@ bfa_msgq_attach(struct bfa_msgq *msgq, struct bfa_ioc *ioc)
 	bfa_msgq_rspq_attach(&msgq->rspq, msgq);
 
 	bfa_nw_ioc_mbox_regisr(msgq->ioc, BFI_MC_MSGQ, bfa_msgq_isr, msgq);
-	bfa_q_qe_init(&msgq->ioc_notify);
 	bfa_ioc_notify_init(&msgq->ioc_notify, bfa_msgq_notify, msgq);
 	bfa_nw_ioc_notify_register(msgq->ioc, &msgq->ioc_notify);
 }
diff --git a/drivers/net/ethernet/brocade/bna/bfi.h b/drivers/net/ethernet/brocade/bna/bfi.h
index 2bcde4042268..81e59ea8b4f2 100644
--- a/drivers/net/ethernet/brocade/bna/bfi.h
+++ b/drivers/net/ethernet/brocade/bna/bfi.h
@@ -21,8 +21,6 @@
 
 #include "bfa_defs.h"
 
-#pragma pack(1)
-
 /* BFI FW image type */
 #define	BFI_FLASH_CHUNK_SZ			256	/*!< Flash chunk size */
 #define	BFI_FLASH_CHUNK_SZ_WORDS	(BFI_FLASH_CHUNK_SZ/sizeof(u32))
@@ -36,10 +34,10 @@ struct bfi_mhdr {
 		struct {
 			u8	qid;
 			u8	fn_lpu;	/*!< msg destination		    */
-		} h2i;
+		} __packed h2i;
 		u16	i2htok;	/*!< token in msgs to host	    */
-	} mtag;
-};
+	} __packed mtag;
+} __packed;
 
 #define bfi_fn_lpu(__fn, __lpu)	((__fn) << 1 | (__lpu))
 #define bfi_mhdr_2_fn(_mh)	((_mh)->mtag.h2i.fn_lpu >> 1)
@@ -75,14 +73,14 @@ union bfi_addr_u {
 	struct {
 		u32	addr_lo;
 		u32	addr_hi;
-	} a32;
-};
+	} __packed a32;
+} __packed;
 
 /* Generic DMA addr-len pair. */
 struct bfi_alen {
 	union bfi_addr_u	al_addr;	/* DMA addr of buffer	*/
 	u32			al_len;		/* length of buffer */
-};
+} __packed;
 
 /*
  * Large Message structure - 128 Bytes size Msgs
@@ -96,7 +94,7 @@ struct bfi_alen {
 struct bfi_mbmsg {
 	struct bfi_mhdr mh;
 	u32		pl[BFI_MBMSG_SZ];
-};
+} __packed;
 
 /* Supported PCI function class codes (personality) */
 enum bfi_pcifn_class {
@@ -184,19 +182,19 @@ enum bfi_ioc_i2h_msgs {
 struct bfi_ioc_getattr_req {
 	struct bfi_mhdr mh;
 	union bfi_addr_u	attr_addr;
-};
+} __packed;
 
 struct bfi_ioc_attr {
 	u64		mfg_pwwn;	/*!< Mfg port wwn	   */
 	u64		mfg_nwwn;	/*!< Mfg node wwn	   */
-	mac_t		mfg_mac;	/*!< Mfg mac		   */
+	u8		mfg_mac[ETH_ALEN]; /*!< Mfg mac		   */
 	u8		port_mode;	/* enum bfi_port_mode	   */
 	u8		rsvd_a;
 	u64		pwwn;
 	u64		nwwn;
-	mac_t		mac;		/*!< PBC or Mfg mac	   */
+	u8		mac[ETH_ALEN];	/*!< PBC or Mfg mac	   */
 	u16	rsvd_b;
-	mac_t		fcoe_mac;
+	u8		fcoe_mac[ETH_ALEN];
 	u16	rsvd_c;
 	char		brcd_serialnum[STRSZ(BFA_MFG_SERIALNUM_SIZE)];
 	u8		pcie_gen;
@@ -211,14 +209,14 @@ struct bfi_ioc_attr {
 	char		optrom_version[BFA_VERSION_LEN];
 	struct bfa_mfg_vpd vpd;
 	u32	card_type;	/*!< card type			*/
-};
+} __packed;
 
 /* BFI_IOC_I2H_GETATTR_REPLY message */
 struct bfi_ioc_getattr_reply {
 	struct bfi_mhdr mh;	/*!< Common msg header		*/
 	u8			status;	/*!< cfg reply status		*/
 	u8			rsvd[3];
-};
+} __packed;
 
 /* Firmware memory page offsets */
 #define BFI_IOC_SMEM_PG0_CB	(0x40)
@@ -256,7 +254,7 @@ struct bfi_ioc_fwver {
 	u8 build;
 	u8 rsvd[2];
 #endif
-};
+} __packed;
 
 struct bfi_ioc_image_hdr {
 	u32	signature;	/*!< constant signature */
@@ -269,7 +267,7 @@ struct bfi_ioc_image_hdr {
 	u32	rsvd_b[2];
 	struct bfi_ioc_fwver fwver;
 	u32	md5sum[BFI_IOC_MD5SUM_SZ];
-};
+} __packed;
 
 enum bfi_ioc_img_ver_cmp {
 	BFI_IOC_IMG_VER_INCOMP,
@@ -301,7 +299,7 @@ enum bfi_port_mode {
 struct bfi_ioc_hbeat {
 	struct bfi_mhdr mh;		/*!< common msg header		*/
 	u32	   hb_count;	/*!< current heart beat count	*/
-};
+} __packed;
 
 /* IOC hardware/firmware state */
 enum bfi_ioc_state {
@@ -317,8 +315,6 @@ enum bfi_ioc_state {
 	BFI_IOC_MEMTEST		= 9,	/*!< IOC is doing memtest	     */
 };
 
-#define BFI_IOC_ENDIAN_SIG  0x12345678
-
 enum {
 	BFI_ADAPTER_TYPE_FC	= 0x01,		/*!< FC adapters	   */
 	BFI_ADAPTER_TYPE_MK	= 0x0f0000,	/*!< adapter type mask     */
@@ -337,12 +333,6 @@ enum {
 		BFI_ADAPTER_ ## __prop ## _SH)
 #define BFI_ADAPTER_SETP(__prop, __val)				\
 	((__val) << BFI_ADAPTER_ ## __prop ## _SH)
-#define BFI_ADAPTER_IS_PROTO(__adap_type)			\
-	((__adap_type) & BFI_ADAPTER_PROTO)
-#define BFI_ADAPTER_IS_TTV(__adap_type)				\
-	((__adap_type) & BFI_ADAPTER_TTV)
-#define BFI_ADAPTER_IS_UNSUPP(__adap_type)			\
-	((__adap_type) & BFI_ADAPTER_UNSUPP)
 #define BFI_ADAPTER_IS_SPECIAL(__adap_type)			\
 	((__adap_type) & (BFI_ADAPTER_TTV | BFI_ADAPTER_PROTO |	\
 			BFI_ADAPTER_UNSUPP))
@@ -353,7 +343,7 @@ struct bfi_ioc_ctrl_req {
 	u16			clscode;
 	u16			rsvd;
 	u32		tv_sec;
-};
+} __packed;
 
 /* BFI_IOC_I2H_ENABLE_REPLY & BFI_IOC_I2H_DISABLE_REPLY messages */
 struct bfi_ioc_ctrl_reply {
@@ -362,7 +352,7 @@ struct bfi_ioc_ctrl_reply {
 	u8			port_mode;	/*!< enum bfa_mode */
 	u8			cap_bm;		/*!< capability bit mask */
 	u8			rsvd;
-};
+} __packed;
 
 #define BFI_IOC_MSGSZ   8
 /* H2I Messages */
@@ -372,14 +362,14 @@ union bfi_ioc_h2i_msg_u {
 	struct bfi_ioc_ctrl_req disable_req;
 	struct bfi_ioc_getattr_req getattr_req;
 	u32			mboxmsg[BFI_IOC_MSGSZ];
-};
+} __packed;
 
 /* I2H Messages */
 union bfi_ioc_i2h_msg_u {
 	struct bfi_mhdr mh;
 	struct bfi_ioc_ctrl_reply fw_event;
 	u32			mboxmsg[BFI_IOC_MSGSZ];
-};
+} __packed;
 
 /*----------------------------------------------------------------------
  *				MSGQ
@@ -408,7 +398,7 @@ struct bfi_msgq_mhdr {
 	u16	num_entries;
 	u8	enet_id;
 	u8	rsvd[1];
-};
+} __packed;
 
 #define bfi_msgq_mhdr_set(_mh, _mc, _mid, _tok, _enet_id) do {	\
 	(_mh).msg_class	 = (_mc);	\
@@ -430,21 +420,21 @@ struct bfi_msgq {
 	union bfi_addr_u addr;
 	u16 q_depth;     /* Total num of entries in the queue */
 	u8 rsvd[2];
-};
+} __packed;
 
 /* BFI_ENET_MSGQ_CFG_REQ TBD init or cfg? */
 struct bfi_msgq_cfg_req {
 	struct bfi_mhdr mh;
 	struct bfi_msgq cmdq;
 	struct bfi_msgq rspq;
-};
+} __packed;
 
 /* BFI_ENET_MSGQ_CFG_RSP */
 struct bfi_msgq_cfg_rsp {
 	struct bfi_mhdr mh;
 	u8 cmd_status;
 	u8 rsvd[3];
-};
+} __packed;
 
 /* BFI_MSGQ_H2I_DOORBELL */
 struct bfi_msgq_h2i_db {
@@ -452,8 +442,8 @@ struct bfi_msgq_h2i_db {
 	union {
 		u16 cmdq_pi;
 		u16 rspq_ci;
-	} idx;
-};
+	} __packed idx;
+} __packed;
 
 /* BFI_MSGQ_I2H_DOORBELL */
 struct bfi_msgq_i2h_db {
@@ -461,8 +451,8 @@ struct bfi_msgq_i2h_db {
 	union {
 		u16 rspq_pi;
 		u16 cmdq_ci;
-	} idx;
-};
+	} __packed idx;
+} __packed;
 
 #define BFI_CMD_COPY_SZ 28
 
@@ -470,14 +460,14 @@ struct bfi_msgq_i2h_db {
 struct bfi_msgq_h2i_cmdq_copy_rsp {
 	struct bfi_mhdr mh;
 	u8	      data[BFI_CMD_COPY_SZ];
-};
+} __packed;
 
 /* BFI_MSGQ_I2H_CMD_COPY_REQ */
 struct bfi_msgq_i2h_cmdq_copy_req {
 	struct bfi_mhdr mh;
 	u16     offset;
 	u16     len;
-};
+} __packed;
 
 /*
  *      FLASH module specific
@@ -505,7 +495,7 @@ enum bfi_flash_i2h_msgs {
 struct bfi_flash_query_req {
 	struct bfi_mhdr mh;   /* Common msg header */
 	struct bfi_alen alen;
-};
+} __packed;
 
 /*
  * Flash write request
@@ -519,7 +509,7 @@ struct bfi_flash_write_req {
 	u8	rsv[2];
 	u32	offset;
 	u32	length;
-};
+} __packed;
 
 /*
  * Flash read request
@@ -532,7 +522,7 @@ struct bfi_flash_read_req {
 	u32	offset;
 	u32	length;
 	struct bfi_alen alen;
-};
+} __packed;
 
 /*
  * Flash query response
@@ -540,7 +530,7 @@ struct bfi_flash_read_req {
 struct bfi_flash_query_rsp {
 	struct bfi_mhdr mh;	/* Common msg header */
 	u32	status;
-};
+} __packed;
 
 /*
  * Flash read response
@@ -552,7 +542,7 @@ struct bfi_flash_read_rsp {
 	u8	rsv[3];
 	u32	status;
 	u32	length;
-};
+} __packed;
 
 /*
  * Flash write response
@@ -564,8 +554,6 @@ struct bfi_flash_write_rsp {
 	u8	rsv[3];
 	u32	status;
 	u32	length;
-};
-
-#pragma pack()
+} __packed;
 
 #endif /* __BFI_H__ */
diff --git a/drivers/net/ethernet/brocade/bna/bfi_cna.h b/drivers/net/ethernet/brocade/bna/bfi_cna.h
index bd605bee72ee..fad651101c48 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_cna.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_cna.h
@@ -22,8 +22,6 @@
 #include "bfi.h"
 #include "bfa_defs_cna.h"
 
-#pragma pack(1)
-
 enum bfi_port_h2i {
 	BFI_PORT_H2I_ENABLE_REQ		= (1),
 	BFI_PORT_H2I_DISABLE_REQ	= (2),
@@ -43,7 +41,7 @@ struct bfi_port_generic_req {
 	struct bfi_mhdr mh;		/*!< msg header			    */
 	u32	msgtag;		/*!< msgtag for reply		    */
 	u32	rsvd;
-};
+} __packed;
 
 /* Generic RSP type */
 struct bfi_port_generic_rsp {
@@ -51,13 +49,13 @@ struct bfi_port_generic_rsp {
 	u8		status;		/*!< port enable status		    */
 	u8		rsvd[3];
 	u32	msgtag;		/*!< msgtag for reply		    */
-};
+} __packed;
 
 /* BFI_PORT_H2I_GET_STATS_REQ */
 struct bfi_port_get_stats_req {
 	struct bfi_mhdr mh;		/*!< common msg header		    */
 	union bfi_addr_u   dma_addr;
-};
+} __packed;
 
 union bfi_port_h2i_msg_u {
 	struct bfi_mhdr mh;
@@ -65,7 +63,7 @@ union bfi_port_h2i_msg_u {
 	struct bfi_port_generic_req disable_req;
 	struct bfi_port_get_stats_req getstats_req;
 	struct bfi_port_generic_req clearstats_req;
-};
+} __packed;
 
 union bfi_port_i2h_msg_u {
 	struct bfi_mhdr mh;
@@ -73,7 +71,7 @@ union bfi_port_i2h_msg_u {
 	struct bfi_port_generic_rsp disable_rsp;
 	struct bfi_port_generic_rsp getstats_rsp;
 	struct bfi_port_generic_rsp clearstats_rsp;
-};
+} __packed;
 
 /* @brief Mailbox commands from host to (DCBX/LLDP) firmware */
 enum bfi_cee_h2i_msgs {
@@ -97,7 +95,7 @@ enum bfi_cee_i2h_msgs {
  */
 struct bfi_lldp_reset_stats {
 	struct bfi_mhdr mh;
-};
+} __packed;
 
 /*
  * @brief H2I command structure for resetting the stats.
@@ -105,7 +103,7 @@ struct bfi_lldp_reset_stats {
  */
 struct bfi_cee_reset_stats {
 	struct bfi_mhdr mh;
-};
+} __packed;
 
 /*
  * @brief  get configuration  command from host
@@ -114,7 +112,7 @@ struct bfi_cee_reset_stats {
 struct bfi_cee_get_req {
 	struct bfi_mhdr mh;
 	union bfi_addr_u   dma_addr;
-};
+} __packed;
 
 /*
  * @brief reply message from firmware
@@ -124,7 +122,7 @@ struct bfi_cee_get_rsp {
 	struct bfi_mhdr mh;
 	u8			cmd_status;
 	u8			rsvd[3];
-};
+} __packed;
 
 /*
  * @brief  get configuration  command from host
@@ -133,7 +131,7 @@ struct bfi_cee_get_rsp {
 struct bfi_cee_stats_req {
 	struct bfi_mhdr mh;
 	union bfi_addr_u   dma_addr;
-};
+} __packed;
 
 /*
  * @brief reply message from firmware
@@ -143,22 +141,20 @@ struct bfi_cee_stats_rsp {
 	struct bfi_mhdr mh;
 	u8			cmd_status;
 	u8			rsvd[3];
-};
+} __packed;
 
 /* @brief mailbox command structures from host to firmware */
 union bfi_cee_h2i_msg_u {
 	struct bfi_mhdr mh;
 	struct bfi_cee_get_req get_req;
 	struct bfi_cee_stats_req stats_req;
-};
+} __packed;
 
 /* @brief mailbox message structures from firmware to host	*/
 union bfi_cee_i2h_msg_u {
 	struct bfi_mhdr mh;
 	struct bfi_cee_get_rsp get_rsp;
 	struct bfi_cee_stats_rsp stats_rsp;
-};
-
-#pragma pack()
+} __packed;
 
 #endif /* __BFI_CNA_H__ */
diff --git a/drivers/net/ethernet/brocade/bna/bfi_enet.h b/drivers/net/ethernet/brocade/bna/bfi_enet.h
index bccca3bbadb8..d7be7ea8c7f5 100644
--- a/drivers/net/ethernet/brocade/bna/bfi_enet.h
+++ b/drivers/net/ethernet/brocade/bna/bfi_enet.h
@@ -36,8 +36,6 @@
 #include "bfa_defs.h"
 #include "bfi.h"
 
-#pragma pack(1)
-
 #define BFI_ENET_CFG_MAX		32	/* Max resources per PF */
 
 #define BFI_ENET_TXQ_PRIO_MAX		8
@@ -59,8 +57,8 @@ union bfi_addr_be_u {
 	struct {
 		u32	addr_hi;	/* Most Significant 32-bits */
 		u32	addr_lo;	/* Least Significant 32-Bits */
-	} a32;
-};
+	} __packed a32;
+} __packed;
 
 /*	T X   Q U E U E   D E F I N E S      */
 /* TxQ Vector (a.k.a. Tx-Buffer Descriptor) */
@@ -70,13 +68,13 @@ union bfi_addr_be_u {
 #define BFI_ENET_TXQ_WI_EXTENSION	(0x104)	/* Extension WI */
 
 /* TxQ Entry Control Flags */
-#define BFI_ENET_TXQ_WI_CF_FCOE_CRC	(1 << 8)
-#define BFI_ENET_TXQ_WI_CF_IPID_MODE	(1 << 5)
-#define BFI_ENET_TXQ_WI_CF_INS_PRIO	(1 << 4)
-#define BFI_ENET_TXQ_WI_CF_INS_VLAN	(1 << 3)
-#define BFI_ENET_TXQ_WI_CF_UDP_CKSUM	(1 << 2)
-#define BFI_ENET_TXQ_WI_CF_TCP_CKSUM	(1 << 1)
-#define BFI_ENET_TXQ_WI_CF_IP_CKSUM	(1 << 0)
+#define BFI_ENET_TXQ_WI_CF_FCOE_CRC	BIT(8)
+#define BFI_ENET_TXQ_WI_CF_IPID_MODE	BIT(5)
+#define BFI_ENET_TXQ_WI_CF_INS_PRIO	BIT(4)
+#define BFI_ENET_TXQ_WI_CF_INS_VLAN	BIT(3)
+#define BFI_ENET_TXQ_WI_CF_UDP_CKSUM	BIT(2)
+#define BFI_ENET_TXQ_WI_CF_TCP_CKSUM	BIT(1)
+#define BFI_ENET_TXQ_WI_CF_IP_CKSUM	BIT(0)
 
 struct bfi_enet_txq_wi_base {
 	u8			reserved;
@@ -88,28 +86,28 @@ struct bfi_enet_txq_wi_base {
 	u16			vlan_tag;
 	u16			lso_mss;	/* Only 14 LSB are valid */
 	u32			frame_length;	/* Only 24 LSB are valid */
-};
+} __packed;
 
 struct bfi_enet_txq_wi_ext {
 	u16			reserved;
 	u16			opcode;		/* BFI_ENET_TXQ_WI_EXTENSION */
 	u32			reserved2[3];
-};
+} __packed;
 
 struct bfi_enet_txq_wi_vector {			/* Tx Buffer Descriptor */
 	u16			reserved;
 	u16			length;		/* Only 14 LSB are valid */
 	union bfi_addr_be_u	addr;
-};
+} __packed;
 
 /*  TxQ Entry Structure  */
 struct bfi_enet_txq_entry {
 	union {
 		struct bfi_enet_txq_wi_base	base;
 		struct bfi_enet_txq_wi_ext	ext;
-	} wi;
+	} __packed wi;
 	struct bfi_enet_txq_wi_vector vector[BFI_ENET_TXQ_WI_VECT_MAX];
-};
+} __packed;
 
 #define wi_hdr		wi.base
 #define wi_ext_hdr	wi.ext
@@ -120,36 +118,36 @@ struct bfi_enet_txq_entry {
 /*   R X   Q U E U E   D E F I N E S   */
 struct bfi_enet_rxq_entry {
 	union bfi_addr_be_u  rx_buffer;
-};
+} __packed;
 
 /*   R X   C O M P L E T I O N   Q U E U E   D E F I N E S   */
 /* CQ Entry Flags */
-#define	BFI_ENET_CQ_EF_MAC_ERROR	(1 <<  0)
-#define	BFI_ENET_CQ_EF_FCS_ERROR	(1 <<  1)
-#define	BFI_ENET_CQ_EF_TOO_LONG		(1 <<  2)
-#define	BFI_ENET_CQ_EF_FC_CRC_OK	(1 <<  3)
+#define BFI_ENET_CQ_EF_MAC_ERROR	BIT(0)
+#define BFI_ENET_CQ_EF_FCS_ERROR	BIT(1)
+#define BFI_ENET_CQ_EF_TOO_LONG		BIT(2)
+#define BFI_ENET_CQ_EF_FC_CRC_OK	BIT(3)
 
-#define	BFI_ENET_CQ_EF_RSVD1		(1 <<  4)
-#define	BFI_ENET_CQ_EF_L4_CKSUM_OK	(1 <<  5)
-#define	BFI_ENET_CQ_EF_L3_CKSUM_OK	(1 <<  6)
-#define	BFI_ENET_CQ_EF_HDS_HEADER	(1 <<  7)
+#define BFI_ENET_CQ_EF_RSVD1		BIT(4)
+#define BFI_ENET_CQ_EF_L4_CKSUM_OK	BIT(5)
+#define BFI_ENET_CQ_EF_L3_CKSUM_OK	BIT(6)
+#define BFI_ENET_CQ_EF_HDS_HEADER	BIT(7)
 
-#define	BFI_ENET_CQ_EF_UDP		(1 <<  8)
-#define	BFI_ENET_CQ_EF_TCP		(1 <<  9)
-#define	BFI_ENET_CQ_EF_IP_OPTIONS	(1 << 10)
-#define	BFI_ENET_CQ_EF_IPV6		(1 << 11)
+#define BFI_ENET_CQ_EF_UDP		BIT(8)
+#define BFI_ENET_CQ_EF_TCP		BIT(9)
+#define BFI_ENET_CQ_EF_IP_OPTIONS	BIT(10)
+#define BFI_ENET_CQ_EF_IPV6		BIT(11)
 
-#define	BFI_ENET_CQ_EF_IPV4		(1 << 12)
-#define	BFI_ENET_CQ_EF_VLAN		(1 << 13)
-#define	BFI_ENET_CQ_EF_RSS		(1 << 14)
-#define	BFI_ENET_CQ_EF_RSVD2		(1 << 15)
+#define BFI_ENET_CQ_EF_IPV4		BIT(12)
+#define BFI_ENET_CQ_EF_VLAN		BIT(13)
+#define BFI_ENET_CQ_EF_RSS		BIT(14)
+#define BFI_ENET_CQ_EF_RSVD2		BIT(15)
 
-#define	BFI_ENET_CQ_EF_MCAST_MATCH	(1 << 16)
-#define	BFI_ENET_CQ_EF_MCAST		(1 << 17)
-#define BFI_ENET_CQ_EF_BCAST		(1 << 18)
-#define	BFI_ENET_CQ_EF_REMOTE		(1 << 19)
+#define BFI_ENET_CQ_EF_MCAST_MATCH	BIT(16)
+#define BFI_ENET_CQ_EF_MCAST		BIT(17)
+#define BFI_ENET_CQ_EF_BCAST		BIT(18)
+#define BFI_ENET_CQ_EF_REMOTE		BIT(19)
 
-#define	BFI_ENET_CQ_EF_LOCAL		(1 << 20)
+#define BFI_ENET_CQ_EF_LOCAL		BIT(20)
 
 /* CQ Entry Structure */
 struct bfi_enet_cq_entry {
@@ -161,7 +159,7 @@ struct bfi_enet_cq_entry {
 	u8	reserved1;
 	u8	reserved2;
 	u8	rxq_id;
-};
+} __packed;
 
 /*   E N E T   C O N T R O L   P A T H   C O M M A N D S   */
 struct bfi_enet_q {
@@ -169,23 +167,23 @@ struct bfi_enet_q {
 	union bfi_addr_u	first_entry;
 	u16		pages;	/* # of pages */
 	u16		page_sz;
-};
+} __packed;
 
 struct bfi_enet_txq {
 	struct bfi_enet_q	q;
 	u8			priority;
 	u8			rsvd[3];
-};
+} __packed;
 
 struct bfi_enet_rxq {
 	struct bfi_enet_q	q;
 	u16		rx_buffer_size;
 	u16		rsvd;
-};
+} __packed;
 
 struct bfi_enet_cq {
 	struct bfi_enet_q	q;
-};
+} __packed;
 
 struct bfi_enet_ib_cfg {
 	u8		int_pkt_dma;
@@ -198,16 +196,16 @@ struct bfi_enet_ib_cfg {
 	u32	inter_pkt_timeout;
 	u8		inter_pkt_count;
 	u8		rsvd1[3];
-};
+} __packed;
 
 struct bfi_enet_ib {
 	union bfi_addr_u	index_addr;
 	union {
 		u16	msix_index;
 		u16	intx_bitmask;
-	} intr;
+	} __packed intr;
 	u16		rsvd;
-};
+} __packed;
 
 /* ENET command messages */
 enum bfi_enet_h2i_msgs {
@@ -355,7 +353,7 @@ enum bfi_enet_err {
  */
 struct bfi_enet_req {
 	struct bfi_msgq_mhdr mh;
-};
+} __packed;
 
 /* Enable/Disable Request
  *
@@ -370,7 +368,7 @@ struct bfi_enet_enable_req {
 	struct		bfi_msgq_mhdr mh;
 	u8		enable;		/* 1 = enable;  0 = disable */
 	u8		rsvd[3];
-};
+} __packed;
 
 /* Generic Response */
 struct bfi_enet_rsp {
@@ -378,7 +376,7 @@ struct bfi_enet_rsp {
 	u8		error;		/*!< if error see cmd_offset */
 	u8		rsvd;
 	u16		cmd_offset;	/*!< offset to invalid parameter */
-};
+} __packed;
 
 /* GLOBAL CONFIGURATION */
 
@@ -387,7 +385,7 @@ struct bfi_enet_rsp {
  */
 struct bfi_enet_attr_req {
 	struct bfi_msgq_mhdr	mh;
-};
+} __packed;
 
 /* bfi_enet_attr_rsp is used by:
  *	BFI_ENET_I2H_GET_ATTR_RSP
@@ -400,7 +398,7 @@ struct bfi_enet_attr_rsp {
 	u32		max_cfg;
 	u32		max_ucmac;
 	u32		rit_size;
-};
+} __packed;
 
 /* Tx Configuration
  *
@@ -421,7 +419,7 @@ struct bfi_enet_tx_cfg {
 	u8		apply_vlan_filter;
 	u8		add_to_vswitch;
 	u8		rsvd1[1];
-};
+} __packed;
 
 struct bfi_enet_tx_cfg_req {
 	struct bfi_msgq_mhdr mh;
@@ -431,7 +429,7 @@ struct bfi_enet_tx_cfg_req {
 	struct {
 		struct bfi_enet_txq	q;
 		struct bfi_enet_ib	ib;
-	} q_cfg[BFI_ENET_TXQ_PRIO_MAX];
+	} __packed q_cfg[BFI_ENET_TXQ_PRIO_MAX];
 
 	struct bfi_enet_ib_cfg	ib_cfg;
 
@@ -448,7 +446,7 @@ struct bfi_enet_tx_cfg_rsp {
 		u32	i_dbell;	/* PCI base address offset */
 		u8	hw_qid;		/* For debugging */
 		u8	rsvd[3];
-	} q_handles[BFI_ENET_TXQ_PRIO_MAX];
+	} __packed q_handles[BFI_ENET_TXQ_PRIO_MAX];
 };
 
 /* Rx Configuration
@@ -481,13 +479,13 @@ struct bfi_enet_rx_cfg {
 		u8			force_offset;
 		u8			type;
 		u8			rsvd1;
-	} hds;
+	} __packed hds;
 
 	u8		multi_buffer;
 	u8		strip_vlan;
 	u8		drop_untagged;
 	u8		rsvd2;
-};
+} __packed;
 
 /*
  * Multicast frames are received on the ql of q-set index zero.
@@ -504,12 +502,12 @@ struct bfi_enet_rx_cfg_req {
 		struct bfi_enet_rxq	qs;	/* small/header buffers */
 		struct bfi_enet_cq	cq;
 		struct bfi_enet_ib	ib;
-	} q_cfg[BFI_ENET_RX_QSET_MAX];
+	} __packed q_cfg[BFI_ENET_RX_QSET_MAX];
 
 	struct bfi_enet_ib_cfg	ib_cfg;
 
 	struct bfi_enet_rx_cfg	rx_cfg;
-};
+} __packed;
 
 struct bfi_enet_rx_cfg_rsp {
 	struct bfi_msgq_mhdr mh;
@@ -524,8 +522,8 @@ struct bfi_enet_rx_cfg_rsp {
 		u8		hw_sqid;  /* For debugging */
 		u8		hw_cqid;  /* For debugging */
 		u8		rsvd;
-	} q_handles[BFI_ENET_RX_QSET_MAX];
-};
+	} __packed q_handles[BFI_ENET_RX_QSET_MAX];
+} __packed;
 
 /* RIT
  *
@@ -537,7 +535,7 @@ struct bfi_enet_rit_req {
 	u16	size;			/* number of table-entries used */
 	u8	rsvd[2];
 	u8	table[BFI_ENET_RSS_RIT_MAX];
-};
+} __packed;
 
 /* RSS
  *
@@ -556,12 +554,12 @@ struct bfi_enet_rss_cfg {
 	u8	mask;
 	u8	rsvd[2];
 	u32	key[BFI_ENET_RSS_KEY_LEN];
-};
+} __packed;
 
 struct bfi_enet_rss_cfg_req {
 	struct bfi_msgq_mhdr	mh;
 	struct bfi_enet_rss_cfg	cfg;
-};
+} __packed;
 
 /* MAC Unicast
  *
@@ -573,16 +571,16 @@ struct bfi_enet_rss_cfg_req {
  */
 struct bfi_enet_ucast_req {
 	struct bfi_msgq_mhdr	mh;
-	mac_t			mac_addr;
+	u8			mac_addr[ETH_ALEN];
 	u8			rsvd[2];
-};
+} __packed;
 
 /* MAC Unicast + VLAN */
 struct bfi_enet_mac_n_vlan_req {
 	struct bfi_msgq_mhdr	mh;
 	u16			vlan_id;
-	mac_t			mac_addr;
-};
+	u8			mac_addr[ETH_ALEN];
+} __packed;
 
 /* MAC Multicast
  *
@@ -591,9 +589,9 @@ struct bfi_enet_mac_n_vlan_req {
  */
 struct bfi_enet_mcast_add_req {
 	struct bfi_msgq_mhdr	mh;
-	mac_t			mac_addr;
+	u8			mac_addr[ETH_ALEN];
 	u8			rsvd[2];
-};
+} __packed;
 
 /* bfi_enet_mac_mfilter_add_rsp is used by:
  *	BFI_ENET_I2H_MAC_MCAST_ADD_RSP
@@ -605,7 +603,7 @@ struct bfi_enet_mcast_add_rsp {
 	u16			cmd_offset;
 	u16			handle;
 	u8			rsvd1[2];
-};
+} __packed;
 
 /* bfi_enet_mac_mfilter_del_req is used by:
  *	BFI_ENET_H2I_MAC_MCAST_DEL_REQ
@@ -614,7 +612,7 @@ struct bfi_enet_mcast_del_req {
 	struct bfi_msgq_mhdr	mh;
 	u16			handle;
 	u8			rsvd[2];
-};
+} __packed;
 
 /* VLAN
  *
@@ -626,7 +624,7 @@ struct bfi_enet_rx_vlan_req {
 	u8			block_idx;
 	u8			rsvd[3];
 	u32			bit_mask[BFI_ENET_VLAN_WORDS_MAX];
-};
+} __packed;
 
 /* PAUSE
  *
@@ -638,7 +636,7 @@ struct bfi_enet_set_pause_req {
 	u8			rsvd[2];
 	u8			tx_pause;	/* 1 = enable;  0 = disable */
 	u8			rx_pause;	/* 1 = enable;  0 = disable */
-};
+} __packed;
 
 /* DIAGNOSTICS
  *
@@ -650,7 +648,7 @@ struct bfi_enet_diag_lb_req {
 	u8			rsvd[2];
 	u8			mode;		/* cable or Serdes */
 	u8			enable;		/* 1 = enable;  0 = disable */
-};
+} __packed;
 
 /* enum for Loopback opmodes */
 enum {
@@ -671,14 +669,14 @@ struct bfi_enet_stats_req {
 	u32			rx_enet_mask;
 	u32			tx_enet_mask;
 	union bfi_addr_u	host_buffer;
-};
+} __packed;
 
 /* defines for "stats_mask" above. */
-#define BFI_ENET_STATS_MAC    (1 << 0)    /* !< MAC Statistics */
-#define BFI_ENET_STATS_BPC    (1 << 1)    /* !< Pause Stats from BPC */
-#define BFI_ENET_STATS_RAD    (1 << 2)    /* !< Rx Admission Statistics */
-#define BFI_ENET_STATS_RX_FC  (1 << 3)    /* !< Rx FC Stats from RxA */
-#define BFI_ENET_STATS_TX_FC  (1 << 4)    /* !< Tx FC Stats from TxA */
+#define BFI_ENET_STATS_MAC    BIT(0)    /* !< MAC Statistics */
+#define BFI_ENET_STATS_BPC    BIT(1)    /* !< Pause Stats from BPC */
+#define BFI_ENET_STATS_RAD    BIT(2)    /* !< Rx Admission Statistics */
+#define BFI_ENET_STATS_RX_FC  BIT(3)    /* !< Rx FC Stats from RxA */
+#define BFI_ENET_STATS_TX_FC  BIT(4)    /* !< Tx FC Stats from TxA */
 
 #define BFI_ENET_STATS_ALL    0x1f
 
@@ -699,7 +697,7 @@ struct bfi_enet_stats_txf {
 	u64 errors;
 	u64 filter_vlan;      /* frames filtered due to VLAN */
 	u64 filter_mac_sa;    /* frames filtered due to SA check */
-};
+} __packed;
 
 /* RxF Frame Statistics */
 struct bfi_enet_stats_rxf {
@@ -715,7 +713,7 @@ struct bfi_enet_stats_rxf {
 	u64 bcast;
 	u64 bcast_vlan;
 	u64 frame_drops;
-};
+} __packed;
 
 /* FC Tx Frame Statistics */
 struct bfi_enet_stats_fc_tx {
@@ -734,7 +732,7 @@ struct bfi_enet_stats_fc_tx {
 	u64 txf_parity_errors;
 	u64 txf_timeout;
 	u64 txf_fid_parity_errors;
-};
+} __packed;
 
 /* FC Rx Frame Statistics */
 struct bfi_enet_stats_fc_rx {
@@ -749,7 +747,7 @@ struct bfi_enet_stats_fc_rx {
 	u64 rxf_bcast_octets;
 	u64 rxf_bcast;
 	u64 rxf_bcast_vlan;
-};
+} __packed;
 
 /* RAD Frame Statistics */
 struct bfi_enet_stats_rad {
@@ -770,7 +768,7 @@ struct bfi_enet_stats_rad {
 	u64 rx_bcast_vlan;
 
 	u64 rx_drops;
-};
+} __packed;
 
 /* BPC Tx Registers */
 struct bfi_enet_stats_bpc {
@@ -785,7 +783,7 @@ struct bfi_enet_stats_bpc {
 	u64 rx_zero_pause[8];	/*!< Pause cancellation */
 	/*!<Pause initiation rather than retention */
 	u64 rx_first_pause[8];
-};
+} __packed;
 
 /* MAC Rx Statistics */
 struct bfi_enet_stats_mac {
@@ -838,7 +836,7 @@ struct bfi_enet_stats_mac {
 	u64 tx_oversize;
 	u64 tx_undersize;
 	u64 tx_fragments;
-};
+} __packed;
 
 /* Complete statistics, DMAed from fw to host followed by
  * BFI_ENET_I2H_STATS_GET_RSP
@@ -852,8 +850,6 @@ struct bfi_enet_stats {
 	struct bfi_enet_stats_fc_tx	fc_tx_stats;
 	struct bfi_enet_stats_rxf	rxf_stats[BFI_ENET_CFG_MAX];
 	struct bfi_enet_stats_txf	txf_stats[BFI_ENET_CFG_MAX];
-};
-
-#pragma pack()
+} __packed;
 
 #endif  /* __BFI_ENET_H__ */
diff --git a/drivers/net/ethernet/brocade/bna/bna.h b/drivers/net/ethernet/brocade/bna/bna.h
index 8ba72b1f36d9..006dcad9a260 100644
--- a/drivers/net/ethernet/brocade/bna/bna.h
+++ b/drivers/net/ethernet/brocade/bna/bna.h
@@ -28,36 +28,8 @@ extern const u32 bna_napi_dim_vector[][BNA_BIAS_T_MAX];
 
 /*  Macros and constants  */
 
-#define BNA_IOC_TIMER_FREQ		200
-
-/* Log string size */
-#define BNA_MESSAGE_SIZE		256
-
 #define bna_is_small_rxq(_id) ((_id) & 0x1)
 
-#define BNA_MAC_IS_EQUAL(_mac1, _mac2)					\
-	(!memcmp((_mac1), (_mac2), sizeof(mac_t)))
-
-#define BNA_POWER_OF_2(x) (((x) & ((x) - 1)) == 0)
-
-#define BNA_TO_POWER_OF_2(x)						\
-do {									\
-	int _shift = 0;							\
-	while ((x) && (x) != 1) {					\
-		(x) >>= 1;						\
-		_shift++;						\
-	}								\
-	(x) <<= _shift;							\
-} while (0)
-
-#define BNA_TO_POWER_OF_2_HIGH(x)					\
-do {									\
-	int n = 1;							\
-	while (n < (x))							\
-		n <<= 1;						\
-	(x) = n;							\
-} while (0)
-
 /*
  * input : _addr-> os dma addr in host endian format,
  * output : _bna_dma_addr-> pointer to hw dma addr
@@ -80,62 +52,8 @@ do {								\
 	| ((ntohl((_bna_dma_addr)->lsb) & 0xffffffff));	\
 } while (0)
 
-#define	containing_rec(addr, type, field)				\
-	((type *)((unsigned char *)(addr) -				\
-	(unsigned char *)(&((type *)0)->field)))
-
 #define BNA_TXQ_WI_NEEDED(_vectors)	(((_vectors) + 3) >> 2)
 
-/* TxQ element is 64 bytes */
-#define BNA_TXQ_PAGE_INDEX_MAX		(PAGE_SIZE >> 6)
-#define BNA_TXQ_PAGE_INDEX_MAX_SHIFT	(PAGE_SHIFT - 6)
-
-#define BNA_TXQ_QPGE_PTR_GET(_qe_idx, _qpt_ptr, _qe_ptr, _qe_ptr_range) \
-{									\
-	unsigned int page_index;	/* index within a page */	\
-	void *page_addr;						\
-	page_index = (_qe_idx) & (BNA_TXQ_PAGE_INDEX_MAX - 1);		\
-	(_qe_ptr_range) = (BNA_TXQ_PAGE_INDEX_MAX - page_index);	\
-	page_addr = (_qpt_ptr)[((_qe_idx) >>  BNA_TXQ_PAGE_INDEX_MAX_SHIFT)];\
-	(_qe_ptr) = &((struct bna_txq_entry *)(page_addr))[page_index]; \
-}
-
-/* RxQ element is 8 bytes */
-#define BNA_RXQ_PAGE_INDEX_MAX		(PAGE_SIZE >> 3)
-#define BNA_RXQ_PAGE_INDEX_MAX_SHIFT	(PAGE_SHIFT - 3)
-
-#define BNA_RXQ_QPGE_PTR_GET(_qe_idx, _qpt_ptr, _qe_ptr, _qe_ptr_range) \
-{									\
-	unsigned int page_index;	/* index within a page */	\
-	void *page_addr;						\
-	page_index = (_qe_idx) & (BNA_RXQ_PAGE_INDEX_MAX - 1);		\
-	(_qe_ptr_range) = (BNA_RXQ_PAGE_INDEX_MAX - page_index);	\
-	page_addr = (_qpt_ptr)[((_qe_idx) >>				\
-				BNA_RXQ_PAGE_INDEX_MAX_SHIFT)];		\
-	(_qe_ptr) = &((struct bna_rxq_entry *)(page_addr))[page_index]; \
-}
-
-/* CQ element is 16 bytes */
-#define BNA_CQ_PAGE_INDEX_MAX		(PAGE_SIZE >> 4)
-#define BNA_CQ_PAGE_INDEX_MAX_SHIFT	(PAGE_SHIFT - 4)
-
-#define BNA_CQ_QPGE_PTR_GET(_qe_idx, _qpt_ptr, _qe_ptr, _qe_ptr_range)	\
-{									\
-	unsigned int page_index;	  /* index within a page */	\
-	void *page_addr;						\
-									\
-	page_index = (_qe_idx) & (BNA_CQ_PAGE_INDEX_MAX - 1);		\
-	(_qe_ptr_range) = (BNA_CQ_PAGE_INDEX_MAX - page_index);		\
-	page_addr = (_qpt_ptr)[((_qe_idx) >>				\
-				    BNA_CQ_PAGE_INDEX_MAX_SHIFT)];	\
-	(_qe_ptr) = &((struct bna_cq_entry *)(page_addr))[page_index];\
-}
-
-#define BNA_QE_INDX_2_PTR(_cast, _qe_idx, _q_base)			\
-	(&((_cast *)(_q_base))[(_qe_idx)])
-
-#define BNA_QE_INDX_RANGE(_qe_idx, _q_depth) ((_q_depth) - (_qe_idx))
-
 #define BNA_QE_INDX_ADD(_qe_idx, _qe_num, _q_depth)			\
 	((_qe_idx) = ((_qe_idx) + (_qe_num)) & ((_q_depth) - 1))
 
@@ -147,31 +65,10 @@ do {								\
 #define BNA_QE_FREE_CNT(_q_ptr, _q_depth)				\
 	(((_q_ptr)->consumer_index - (_q_ptr)->producer_index - 1) &	\
 	 ((_q_depth) - 1))
-
 #define BNA_QE_IN_USE_CNT(_q_ptr, _q_depth)				\
 	((((_q_ptr)->producer_index - (_q_ptr)->consumer_index)) &	\
 	 (_q_depth - 1))
 
-#define BNA_Q_GET_CI(_q_ptr)		((_q_ptr)->q.consumer_index)
-
-#define BNA_Q_GET_PI(_q_ptr)		((_q_ptr)->q.producer_index)
-
-#define BNA_Q_PI_ADD(_q_ptr, _num)					\
-	(_q_ptr)->q.producer_index =					\
-		(((_q_ptr)->q.producer_index + (_num)) &		\
-		((_q_ptr)->q.q_depth - 1))
-
-#define BNA_Q_CI_ADD(_q_ptr, _num)					\
-	(_q_ptr)->q.consumer_index =					\
-		(((_q_ptr)->q.consumer_index + (_num))			\
-		& ((_q_ptr)->q.q_depth - 1))
-
-#define BNA_Q_FREE_COUNT(_q_ptr)					\
-	(BNA_QE_FREE_CNT(&((_q_ptr)->q), (_q_ptr)->q.q_depth))
-
-#define BNA_Q_IN_USE_COUNT(_q_ptr)					\
-	(BNA_QE_IN_USE_CNT(&(_q_ptr)->q, (_q_ptr)->q.q_depth))
-
 #define BNA_LARGE_PKT_SIZE		1000
 
 #define BNA_UPDATE_PKT_CNT(_pkt, _len)					\
@@ -222,21 +119,6 @@ do {									\
 	}								\
 } while (0)
 
-#define	call_rxf_pause_cbfn(rxf)					\
-do {									\
-	if ((rxf)->oper_state_cbfn) {					\
-		void (*cbfn)(struct bnad *, struct bna_rx *);	\
-		struct bnad *cbarg;					\
-		cbfn = (rxf)->oper_state_cbfn;				\
-		cbarg = (rxf)->oper_state_cbarg;			\
-		(rxf)->oper_state_cbfn = NULL;				\
-		(rxf)->oper_state_cbarg = NULL;				\
-		cbfn(cbarg, rxf->rx);					\
-	}								\
-} while (0)
-
-#define	call_rxf_resume_cbfn(rxf) call_rxf_pause_cbfn(rxf)
-
 #define is_xxx_enable(mode, bitmask, xxx) ((bitmask & xxx) && (mode & xxx))
 
 #define is_xxx_disable(mode, bitmask, xxx) ((bitmask & xxx) && !(mode & xxx))
@@ -326,28 +208,24 @@ do {									\
 #define bna_rx_rid_mask(_bna) ((_bna)->rx_mod.rid_mask)
 
 #define bna_tx_from_rid(_bna, _rid, _tx)				\
-do {								    \
-	struct bna_tx_mod *__tx_mod = &(_bna)->tx_mod;	  \
-	struct bna_tx *__tx;					    \
-	struct list_head *qe;					   \
-	_tx = NULL;						     \
-	list_for_each(qe, &__tx_mod->tx_active_q) {		     \
-		__tx = (struct bna_tx *)qe;			     \
-		if (__tx->rid == (_rid)) {			      \
-			(_tx) = __tx;				   \
-			break;					  \
-		}						       \
-	}							       \
+do {									\
+	struct bna_tx_mod *__tx_mod = &(_bna)->tx_mod;			\
+	struct bna_tx *__tx;						\
+	_tx = NULL;							\
+	list_for_each_entry(__tx, &__tx_mod->tx_active_q, qe) {		\
+		if (__tx->rid == (_rid)) {				\
+			(_tx) = __tx;					\
+			break;						\
+		}							\
+	}								\
 } while (0)
 
 #define bna_rx_from_rid(_bna, _rid, _rx)				\
 do {									\
 	struct bna_rx_mod *__rx_mod = &(_bna)->rx_mod;			\
 	struct bna_rx *__rx;						\
-	struct list_head *qe;						\
 	_rx = NULL;							\
-	list_for_each(qe, &__rx_mod->rx_active_q) {			\
-		__rx = (struct bna_rx *)qe;				\
+	list_for_each_entry(__rx, &__rx_mod->rx_active_q, qe) {		\
 		if (__rx->rid == (_rid)) {				\
 			(_rx) = __rx;					\
 			break;						\
@@ -365,17 +243,14 @@ do {									\
 
 /*  Inline functions  */
 
-static inline struct bna_mac *bna_mac_find(struct list_head *q, u8 *addr)
+static inline struct bna_mac *bna_mac_find(struct list_head *q, const u8 *addr)
 {
-	struct bna_mac *mac = NULL;
-	struct list_head *qe;
-	list_for_each(qe, q) {
-		if (BNA_MAC_IS_EQUAL(((struct bna_mac *)qe)->addr, addr)) {
-			mac = (struct bna_mac *)qe;
-			break;
-		}
-	}
-	return mac;
+	struct bna_mac *mac;
+
+	list_for_each_entry(mac, q, qe)
+		if (ether_addr_equal(mac->addr, addr))
+			return mac;
+	return NULL;
 }
 
 #define bna_attr(_bna) (&(_bna)->ioceth.attr)
@@ -401,7 +276,6 @@ void bna_hw_stats_get(struct bna *bna);
 
 /* APIs for RxF */
 struct bna_mac *bna_cam_mod_mac_get(struct list_head *head);
-void bna_cam_mod_mac_put(struct list_head *tail, struct bna_mac *mac);
 struct bna_mcam_handle *bna_mcam_mod_handle_get(struct bna_mcam_mod *mod);
 void bna_mcam_mod_handle_put(struct bna_mcam_mod *mcam_mod,
 			  struct bna_mcam_handle *handle);
@@ -488,31 +362,19 @@ void bna_rx_cleanup_complete(struct bna_rx *rx);
 void bna_rx_coalescing_timeo_set(struct bna_rx *rx, int coalescing_timeo);
 void bna_rx_dim_reconfig(struct bna *bna, const u32 vector[][BNA_BIAS_T_MAX]);
 void bna_rx_dim_update(struct bna_ccb *ccb);
-enum bna_cb_status
-bna_rx_ucast_set(struct bna_rx *rx, u8 *ucmac,
-		 void (*cbfn)(struct bnad *, struct bna_rx *));
-enum bna_cb_status
-bna_rx_ucast_add(struct bna_rx *rx, u8* ucmac,
-		 void (*cbfn)(struct bnad *, struct bna_rx *));
-enum bna_cb_status
-bna_rx_ucast_del(struct bna_rx *rx, u8 *ucmac,
-		 void (*cbfn)(struct bnad *, struct bna_rx *));
-enum bna_cb_status
-bna_rx_ucast_listset(struct bna_rx *rx, int count, u8 *uclist,
-		     void (*cbfn)(struct bnad *, struct bna_rx *));
-enum bna_cb_status
-bna_rx_mcast_add(struct bna_rx *rx, u8 *mcmac,
-		 void (*cbfn)(struct bnad *, struct bna_rx *));
-enum bna_cb_status
-bna_rx_mcast_listset(struct bna_rx *rx, int count, u8 *mcmac,
-		     void (*cbfn)(struct bnad *, struct bna_rx *));
+enum bna_cb_status bna_rx_ucast_set(struct bna_rx *rx, const u8 *ucmac);
+enum bna_cb_status bna_rx_ucast_listset(struct bna_rx *rx, int count,
+					const u8 *uclist);
+enum bna_cb_status bna_rx_mcast_add(struct bna_rx *rx, const u8 *mcmac,
+				    void (*cbfn)(struct bnad *,
+						 struct bna_rx *));
+enum bna_cb_status bna_rx_mcast_listset(struct bna_rx *rx, int count,
+					const u8 *mcmac);
 void
-bna_rx_mcast_delall(struct bna_rx *rx,
-		    void (*cbfn)(struct bnad *, struct bna_rx *));
+bna_rx_mcast_delall(struct bna_rx *rx);
 enum bna_cb_status
 bna_rx_mode_set(struct bna_rx *rx, enum bna_rxmode rxmode,
-		enum bna_rxmode bitmask,
-		void (*cbfn)(struct bnad *, struct bna_rx *));
+		enum bna_rxmode bitmask);
 void bna_rx_vlan_add(struct bna_rx *rx, int vlan_id);
 void bna_rx_vlan_del(struct bna_rx *rx, int vlan_id);
 void bna_rx_vlanfilter_enable(struct bna_rx *rx);
@@ -532,11 +394,10 @@ void bna_enet_enable(struct bna_enet *enet);
 void bna_enet_disable(struct bna_enet *enet, enum bna_cleanup_type type,
 		      void (*cbfn)(void *));
 void bna_enet_pause_config(struct bna_enet *enet,
-			   struct bna_pause_config *pause_config,
-			   void (*cbfn)(struct bnad *));
+			   struct bna_pause_config *pause_config);
 void bna_enet_mtu_set(struct bna_enet *enet, int mtu,
 		      void (*cbfn)(struct bnad *));
-void bna_enet_perm_mac_get(struct bna_enet *enet, mac_t *mac);
+void bna_enet_perm_mac_get(struct bna_enet *enet, u8 *mac);
 
 /* IOCETH */
 
diff --git a/drivers/net/ethernet/brocade/bna/bna_enet.c b/drivers/net/ethernet/brocade/bna/bna_enet.c
index deb8da6ab9cc..4e5c3874a50f 100644
--- a/drivers/net/ethernet/brocade/bna/bna_enet.c
+++ b/drivers/net/ethernet/brocade/bna/bna_enet.c
@@ -207,7 +207,7 @@ bna_bfi_stats_get_rsp(struct bna *bna, struct bfi_msgq_mhdr *msghdr)
 	for (i = 0; i < BFI_ENET_CFG_MAX; i++) {
 		stats_dst = (u64 *)&(bna->stats.hw_stats.rxf_stats[i]);
 		memset(stats_dst, 0, sizeof(struct bfi_enet_stats_rxf));
-		if (rx_enet_mask & ((u32)(1 << i))) {
+		if (rx_enet_mask & BIT(i)) {
 			int k;
 			count = sizeof(struct bfi_enet_stats_rxf) /
 				sizeof(u64);
@@ -222,7 +222,7 @@ bna_bfi_stats_get_rsp(struct bna *bna, struct bfi_msgq_mhdr *msghdr)
 	for (i = 0; i < BFI_ENET_CFG_MAX; i++) {
 		stats_dst = (u64 *)&(bna->stats.hw_stats.txf_stats[i]);
 		memset(stats_dst, 0, sizeof(struct bfi_enet_stats_txf));
-		if (tx_enet_mask & ((u32)(1 << i))) {
+		if (tx_enet_mask & BIT(i)) {
 			int k;
 			count = sizeof(struct bfi_enet_stats_txf) /
 				sizeof(u64);
@@ -884,16 +884,6 @@ do {									\
 	}								\
 } while (0)
 
-#define call_enet_pause_cbfn(enet)					\
-do {									\
-	if ((enet)->pause_cbfn) {					\
-		void (*cbfn)(struct bnad *);				\
-		cbfn = (enet)->pause_cbfn;				\
-		(enet)->pause_cbfn = NULL;				\
-		cbfn((enet)->bna->bnad);				\
-	}								\
-} while (0)
-
 #define call_enet_mtu_cbfn(enet)					\
 do {									\
 	if ((enet)->mtu_cbfn) {						\
@@ -925,7 +915,6 @@ bfa_fsm_state_decl(bna_enet, chld_stop_wait, struct bna_enet,
 static void
 bna_enet_sm_stopped_entry(struct bna_enet *enet)
 {
-	call_enet_pause_cbfn(enet);
 	call_enet_mtu_cbfn(enet);
 	call_enet_stop_cbfn(enet);
 }
@@ -947,7 +936,6 @@ bna_enet_sm_stopped(struct bna_enet *enet, enum bna_enet_event event)
 		break;
 
 	case ENET_E_PAUSE_CFG:
-		call_enet_pause_cbfn(enet);
 		break;
 
 	case ENET_E_MTU_CFG:
@@ -1039,7 +1027,6 @@ bna_enet_sm_started_entry(struct bna_enet *enet)
 	 * NOTE: Do not call bna_enet_chld_start() here, since it will be
 	 * inadvertently called during cfg_wait->started transition as well
 	 */
-	call_enet_pause_cbfn(enet);
 	call_enet_mtu_cbfn(enet);
 }
 
@@ -1211,8 +1198,6 @@ bna_enet_init(struct bna_enet *enet, struct bna *bna)
 	enet->stop_cbfn = NULL;
 	enet->stop_cbarg = NULL;
 
-	enet->pause_cbfn = NULL;
-
 	enet->mtu_cbfn = NULL;
 
 	bfa_fsm_set_state(enet, bna_enet_sm_stopped);
@@ -1308,13 +1293,10 @@ bna_enet_disable(struct bna_enet *enet, enum bna_cleanup_type type,
 
 void
 bna_enet_pause_config(struct bna_enet *enet,
-		      struct bna_pause_config *pause_config,
-		      void (*cbfn)(struct bnad *))
+		      struct bna_pause_config *pause_config)
 {
 	enet->pause_config = *pause_config;
 
-	enet->pause_cbfn = cbfn;
-
 	bfa_fsm_send_event(enet, ENET_E_PAUSE_CFG);
 }
 
@@ -1330,9 +1312,9 @@ bna_enet_mtu_set(struct bna_enet *enet, int mtu,
 }
 
 void
-bna_enet_perm_mac_get(struct bna_enet *enet, mac_t *mac)
+bna_enet_perm_mac_get(struct bna_enet *enet, u8 *mac)
 {
-	*mac = bfa_nw_ioc_get_mac(&enet->bna->ioceth.ioc);
+	bfa_nw_ioc_get_mac(&enet->bna->ioceth.ioc, mac);
 }
 
 /* IOCETH */
@@ -1810,17 +1792,13 @@ bna_ucam_mod_init(struct bna_ucam_mod *ucam_mod, struct bna *bna,
 	res_info[BNA_MOD_RES_MEM_T_UCMAC_ARRAY].res_u.mem_info.mdl[0].kva;
 
 	INIT_LIST_HEAD(&ucam_mod->free_q);
-	for (i = 0; i < bna->ioceth.attr.num_ucmac; i++) {
-		bfa_q_qe_init(&ucam_mod->ucmac[i].qe);
+	for (i = 0; i < bna->ioceth.attr.num_ucmac; i++)
 		list_add_tail(&ucam_mod->ucmac[i].qe, &ucam_mod->free_q);
-	}
 
 	/* A separate queue to allow synchronous setting of a list of MACs */
 	INIT_LIST_HEAD(&ucam_mod->del_q);
-	for (i = i; i < (bna->ioceth.attr.num_ucmac * 2); i++) {
-		bfa_q_qe_init(&ucam_mod->ucmac[i].qe);
+	for (i = i; i < (bna->ioceth.attr.num_ucmac * 2); i++)
 		list_add_tail(&ucam_mod->ucmac[i].qe, &ucam_mod->del_q);
-	}
 
 	ucam_mod->bna = bna;
 }
@@ -1828,17 +1806,6 @@ bna_ucam_mod_init(struct bna_ucam_mod *ucam_mod, struct bna *bna,
 static void
 bna_ucam_mod_uninit(struct bna_ucam_mod *ucam_mod)
 {
-	struct list_head *qe;
-	int i;
-
-	i = 0;
-	list_for_each(qe, &ucam_mod->free_q)
-		i++;
-
-	i = 0;
-	list_for_each(qe, &ucam_mod->del_q)
-		i++;
-
 	ucam_mod->bna = NULL;
 }
 
@@ -1852,27 +1819,21 @@ bna_mcam_mod_init(struct bna_mcam_mod *mcam_mod, struct bna *bna,
 	res_info[BNA_MOD_RES_MEM_T_MCMAC_ARRAY].res_u.mem_info.mdl[0].kva;
 
 	INIT_LIST_HEAD(&mcam_mod->free_q);
-	for (i = 0; i < bna->ioceth.attr.num_mcmac; i++) {
-		bfa_q_qe_init(&mcam_mod->mcmac[i].qe);
+	for (i = 0; i < bna->ioceth.attr.num_mcmac; i++)
 		list_add_tail(&mcam_mod->mcmac[i].qe, &mcam_mod->free_q);
-	}
 
 	mcam_mod->mchandle = (struct bna_mcam_handle *)
 	res_info[BNA_MOD_RES_MEM_T_MCHANDLE_ARRAY].res_u.mem_info.mdl[0].kva;
 
 	INIT_LIST_HEAD(&mcam_mod->free_handle_q);
-	for (i = 0; i < bna->ioceth.attr.num_mcmac; i++) {
-		bfa_q_qe_init(&mcam_mod->mchandle[i].qe);
+	for (i = 0; i < bna->ioceth.attr.num_mcmac; i++)
 		list_add_tail(&mcam_mod->mchandle[i].qe,
-				&mcam_mod->free_handle_q);
-	}
+			      &mcam_mod->free_handle_q);
 
 	/* A separate queue to allow synchronous setting of a list of MACs */
 	INIT_LIST_HEAD(&mcam_mod->del_q);
-	for (i = i; i < (bna->ioceth.attr.num_mcmac * 2); i++) {
-		bfa_q_qe_init(&mcam_mod->mcmac[i].qe);
+	for (i = i; i < (bna->ioceth.attr.num_mcmac * 2); i++)
 		list_add_tail(&mcam_mod->mcmac[i].qe, &mcam_mod->del_q);
-	}
 
 	mcam_mod->bna = bna;
 }
@@ -1880,18 +1841,6 @@ bna_mcam_mod_init(struct bna_mcam_mod *mcam_mod, struct bna *bna,
 static void
 bna_mcam_mod_uninit(struct bna_mcam_mod *mcam_mod)
 {
-	struct list_head *qe;
-	int i;
-
-	i = 0;
-	list_for_each(qe, &mcam_mod->free_q) i++;
-
-	i = 0;
-	list_for_each(qe, &mcam_mod->del_q) i++;
-
-	i = 0;
-	list_for_each(qe, &mcam_mod->free_handle_q) i++;
-
 	mcam_mod->bna = NULL;
 }
 
@@ -2108,32 +2057,26 @@ bna_num_rxp_set(struct bna *bna, int num_rxp)
 struct bna_mac *
 bna_cam_mod_mac_get(struct list_head *head)
 {
-	struct list_head *qe;
-
-	if (list_empty(head))
-		return NULL;
+	struct bna_mac *mac;
 
-	bfa_q_deq(head, &qe);
-	return (struct bna_mac *)qe;
-}
+	mac = list_first_entry_or_null(head, struct bna_mac, qe);
+	if (mac)
+		list_del(&mac->qe);
 
-void
-bna_cam_mod_mac_put(struct list_head *tail, struct bna_mac *mac)
-{
-	list_add_tail(&mac->qe, tail);
+	return mac;
 }
 
 struct bna_mcam_handle *
 bna_mcam_mod_handle_get(struct bna_mcam_mod *mcam_mod)
 {
-	struct list_head *qe;
-
-	if (list_empty(&mcam_mod->free_handle_q))
-		return NULL;
+	struct bna_mcam_handle *handle;
 
-	bfa_q_deq(&mcam_mod->free_handle_q, &qe);
+	handle = list_first_entry_or_null(&mcam_mod->free_handle_q,
+					  struct bna_mcam_handle, qe);
+	if (handle)
+		list_del(&handle->qe);
 
-	return (struct bna_mcam_handle *)qe;
+	return handle;
 }
 
 void
diff --git a/drivers/net/ethernet/brocade/bna/bna_hw_defs.h b/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
index 174af0e9d056..52b45c9935aa 100644
--- a/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
+++ b/drivers/net/ethernet/brocade/bna/bna_hw_defs.h
@@ -213,7 +213,7 @@ do {									\
  * 15 bits (32K) should  be large enough to accumulate, anyways, and the max.
  * acked events to h/w can be (32K + max poll weight) (currently 64).
  */
-#define	BNA_IB_MAX_ACK_EVENTS		(1 << 15)
+#define BNA_IB_MAX_ACK_EVENTS		BIT(15)
 
 /* These macros build the data portion of the TxQ/RxQ doorbell */
 #define BNA_DOORBELL_Q_PRD_IDX(_pi)	(0x80000000 | (_pi))
@@ -282,13 +282,13 @@ do {									\
 #define BNA_TXQ_WI_EXTENSION		(0x104)	/* Extension WI */
 
 /* TxQ Entry Control Flags */
-#define BNA_TXQ_WI_CF_FCOE_CRC		(1 << 8)
-#define BNA_TXQ_WI_CF_IPID_MODE		(1 << 5)
-#define BNA_TXQ_WI_CF_INS_PRIO		(1 << 4)
-#define BNA_TXQ_WI_CF_INS_VLAN		(1 << 3)
-#define BNA_TXQ_WI_CF_UDP_CKSUM		(1 << 2)
-#define BNA_TXQ_WI_CF_TCP_CKSUM		(1 << 1)
-#define BNA_TXQ_WI_CF_IP_CKSUM		(1 << 0)
+#define BNA_TXQ_WI_CF_FCOE_CRC		BIT(8)
+#define BNA_TXQ_WI_CF_IPID_MODE		BIT(5)
+#define BNA_TXQ_WI_CF_INS_PRIO		BIT(4)
+#define BNA_TXQ_WI_CF_INS_VLAN		BIT(3)
+#define BNA_TXQ_WI_CF_UDP_CKSUM		BIT(2)
+#define BNA_TXQ_WI_CF_TCP_CKSUM		BIT(1)
+#define BNA_TXQ_WI_CF_IP_CKSUM		BIT(0)
 
 #define BNA_TXQ_WI_L4_HDR_N_OFFSET(_hdr_size, _offset) \
 		(((_hdr_size) << 10) | ((_offset) & 0x3FF))
@@ -297,36 +297,36 @@ do {									\
  * Completion Q defines
  */
 /* CQ Entry Flags */
-#define	BNA_CQ_EF_MAC_ERROR	(1 <<  0)
-#define	BNA_CQ_EF_FCS_ERROR	(1 <<  1)
-#define	BNA_CQ_EF_TOO_LONG	(1 <<  2)
-#define	BNA_CQ_EF_FC_CRC_OK	(1 <<  3)
-
-#define	BNA_CQ_EF_RSVD1		(1 <<  4)
-#define	BNA_CQ_EF_L4_CKSUM_OK	(1 <<  5)
-#define	BNA_CQ_EF_L3_CKSUM_OK	(1 <<  6)
-#define	BNA_CQ_EF_HDS_HEADER	(1 <<  7)
-
-#define	BNA_CQ_EF_UDP		(1 <<  8)
-#define	BNA_CQ_EF_TCP		(1 <<  9)
-#define	BNA_CQ_EF_IP_OPTIONS	(1 << 10)
-#define	BNA_CQ_EF_IPV6		(1 << 11)
-
-#define	BNA_CQ_EF_IPV4		(1 << 12)
-#define	BNA_CQ_EF_VLAN		(1 << 13)
-#define	BNA_CQ_EF_RSS		(1 << 14)
-#define	BNA_CQ_EF_RSVD2		(1 << 15)
-
-#define	BNA_CQ_EF_MCAST_MATCH   (1 << 16)
-#define	BNA_CQ_EF_MCAST		(1 << 17)
-#define BNA_CQ_EF_BCAST		(1 << 18)
-#define	BNA_CQ_EF_REMOTE	(1 << 19)
-
-#define	BNA_CQ_EF_LOCAL		(1 << 20)
+#define BNA_CQ_EF_MAC_ERROR	BIT(0)
+#define BNA_CQ_EF_FCS_ERROR	BIT(1)
+#define BNA_CQ_EF_TOO_LONG	BIT(2)
+#define BNA_CQ_EF_FC_CRC_OK	BIT(3)
+
+#define BNA_CQ_EF_RSVD1		BIT(4)
+#define BNA_CQ_EF_L4_CKSUM_OK	BIT(5)
+#define BNA_CQ_EF_L3_CKSUM_OK	BIT(6)
+#define BNA_CQ_EF_HDS_HEADER	BIT(7)
+
+#define BNA_CQ_EF_UDP		BIT(8)
+#define BNA_CQ_EF_TCP		BIT(9)
+#define BNA_CQ_EF_IP_OPTIONS	BIT(10)
+#define BNA_CQ_EF_IPV6		BIT(11)
+
+#define BNA_CQ_EF_IPV4		BIT(12)
+#define BNA_CQ_EF_VLAN		BIT(13)
+#define BNA_CQ_EF_RSS		BIT(14)
+#define BNA_CQ_EF_RSVD2		BIT(15)
+
+#define BNA_CQ_EF_MCAST_MATCH   BIT(16)
+#define BNA_CQ_EF_MCAST		BIT(17)
+#define BNA_CQ_EF_BCAST		BIT(18)
+#define BNA_CQ_EF_REMOTE	BIT(19)
+
+#define BNA_CQ_EF_LOCAL		BIT(20)
 /* CAT2 ASIC does not use bit 21 as per the SPEC.
  * Bit 31 is set in every end of frame completion
  */
-#define BNA_CQ_EF_EOP		(1 << 31)
+#define BNA_CQ_EF_EOP		BIT(31)
 
 /* Data structures */
 
diff --git a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
index 8ab3a5f62706..5d0753cc7e73 100644
--- a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
+++ b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
@@ -46,7 +46,6 @@ do {									\
 
 static int bna_rxf_cfg_apply(struct bna_rxf *rxf);
 static void bna_rxf_cfg_reset(struct bna_rxf *rxf);
-static int bna_rxf_fltr_clear(struct bna_rxf *rxf);
 static int bna_rxf_ucast_cfg_apply(struct bna_rxf *rxf);
 static int bna_rxf_promisc_cfg_apply(struct bna_rxf *rxf);
 static int bna_rxf_allmulti_cfg_apply(struct bna_rxf *rxf);
@@ -60,14 +59,10 @@ static int bna_rxf_allmulti_cfg_reset(struct bna_rxf *rxf,
 
 bfa_fsm_state_decl(bna_rxf, stopped, struct bna_rxf,
 			enum bna_rxf_event);
-bfa_fsm_state_decl(bna_rxf, paused, struct bna_rxf,
-			enum bna_rxf_event);
 bfa_fsm_state_decl(bna_rxf, cfg_wait, struct bna_rxf,
 			enum bna_rxf_event);
 bfa_fsm_state_decl(bna_rxf, started, struct bna_rxf,
 			enum bna_rxf_event);
-bfa_fsm_state_decl(bna_rxf, fltr_clr_wait, struct bna_rxf,
-			enum bna_rxf_event);
 bfa_fsm_state_decl(bna_rxf, last_resp_wait, struct bna_rxf,
 			enum bna_rxf_event);
 
@@ -82,11 +77,7 @@ bna_rxf_sm_stopped(struct bna_rxf *rxf, enum bna_rxf_event event)
 {
 	switch (event) {
 	case RXF_E_START:
-		if (rxf->flags & BNA_RXF_F_PAUSED) {
-			bfa_fsm_set_state(rxf, bna_rxf_sm_paused);
-			call_rxf_start_cbfn(rxf);
-		} else
-			bfa_fsm_set_state(rxf, bna_rxf_sm_cfg_wait);
+		bfa_fsm_set_state(rxf, bna_rxf_sm_cfg_wait);
 		break;
 
 	case RXF_E_STOP:
@@ -101,45 +92,6 @@ bna_rxf_sm_stopped(struct bna_rxf *rxf, enum bna_rxf_event event)
 		call_rxf_cam_fltr_cbfn(rxf);
 		break;
 
-	case RXF_E_PAUSE:
-		rxf->flags |= BNA_RXF_F_PAUSED;
-		call_rxf_pause_cbfn(rxf);
-		break;
-
-	case RXF_E_RESUME:
-		rxf->flags &= ~BNA_RXF_F_PAUSED;
-		call_rxf_resume_cbfn(rxf);
-		break;
-
-	default:
-		bfa_sm_fault(event);
-	}
-}
-
-static void
-bna_rxf_sm_paused_entry(struct bna_rxf *rxf)
-{
-	call_rxf_pause_cbfn(rxf);
-}
-
-static void
-bna_rxf_sm_paused(struct bna_rxf *rxf, enum bna_rxf_event event)
-{
-	switch (event) {
-	case RXF_E_STOP:
-	case RXF_E_FAIL:
-		bfa_fsm_set_state(rxf, bna_rxf_sm_stopped);
-		break;
-
-	case RXF_E_CONFIG:
-		call_rxf_cam_fltr_cbfn(rxf);
-		break;
-
-	case RXF_E_RESUME:
-		rxf->flags &= ~BNA_RXF_F_PAUSED;
-		bfa_fsm_set_state(rxf, bna_rxf_sm_cfg_wait);
-		break;
-
 	default:
 		bfa_sm_fault(event);
 	}
@@ -166,7 +118,6 @@ bna_rxf_sm_cfg_wait(struct bna_rxf *rxf, enum bna_rxf_event event)
 		bna_rxf_cfg_reset(rxf);
 		call_rxf_start_cbfn(rxf);
 		call_rxf_cam_fltr_cbfn(rxf);
-		call_rxf_resume_cbfn(rxf);
 		bfa_fsm_set_state(rxf, bna_rxf_sm_stopped);
 		break;
 
@@ -174,12 +125,6 @@ bna_rxf_sm_cfg_wait(struct bna_rxf *rxf, enum bna_rxf_event event)
 		/* No-op */
 		break;
 
-	case RXF_E_PAUSE:
-		rxf->flags |= BNA_RXF_F_PAUSED;
-		call_rxf_start_cbfn(rxf);
-		bfa_fsm_set_state(rxf, bna_rxf_sm_fltr_clr_wait);
-		break;
-
 	case RXF_E_FW_RESP:
 		if (!bna_rxf_cfg_apply(rxf)) {
 			/* No more pending config updates */
@@ -197,7 +142,6 @@ bna_rxf_sm_started_entry(struct bna_rxf *rxf)
 {
 	call_rxf_start_cbfn(rxf);
 	call_rxf_cam_fltr_cbfn(rxf);
-	call_rxf_resume_cbfn(rxf);
 }
 
 static void
@@ -214,41 +158,6 @@ bna_rxf_sm_started(struct bna_rxf *rxf, enum bna_rxf_event event)
 		bfa_fsm_set_state(rxf, bna_rxf_sm_cfg_wait);
 		break;
 
-	case RXF_E_PAUSE:
-		rxf->flags |= BNA_RXF_F_PAUSED;
-		if (!bna_rxf_fltr_clear(rxf))
-			bfa_fsm_set_state(rxf, bna_rxf_sm_paused);
-		else
-			bfa_fsm_set_state(rxf, bna_rxf_sm_fltr_clr_wait);
-		break;
-
-	default:
-		bfa_sm_fault(event);
-	}
-}
-
-static void
-bna_rxf_sm_fltr_clr_wait_entry(struct bna_rxf *rxf)
-{
-}
-
-static void
-bna_rxf_sm_fltr_clr_wait(struct bna_rxf *rxf, enum bna_rxf_event event)
-{
-	switch (event) {
-	case RXF_E_FAIL:
-		bna_rxf_cfg_reset(rxf);
-		call_rxf_pause_cbfn(rxf);
-		bfa_fsm_set_state(rxf, bna_rxf_sm_stopped);
-		break;
-
-	case RXF_E_FW_RESP:
-		if (!bna_rxf_fltr_clear(rxf)) {
-			/* No more pending CAM entries to clear */
-			bfa_fsm_set_state(rxf, bna_rxf_sm_paused);
-		}
-		break;
-
 	default:
 		bfa_sm_fault(event);
 	}
@@ -283,7 +192,7 @@ bna_bfi_ucast_req(struct bna_rxf *rxf, struct bna_mac *mac,
 	bfi_msgq_mhdr_set(req->mh, BFI_MC_ENET, req_type, 0, rxf->rx->rid);
 	req->mh.num_entries = htons(
 	bfi_msgq_num_cmd_entries(sizeof(struct bfi_enet_ucast_req)));
-	memcpy(&req->mac_addr, &mac->addr, sizeof(mac_t));
+	ether_addr_copy(req->mac_addr, mac->addr);
 	bfa_msgq_cmd_set(&rxf->msgq_cmd, NULL, NULL,
 		sizeof(struct bfi_enet_ucast_req), &req->mh);
 	bfa_msgq_cmd_post(&rxf->rx->bna->msgq, &rxf->msgq_cmd);
@@ -299,7 +208,7 @@ bna_bfi_mcast_add_req(struct bna_rxf *rxf, struct bna_mac *mac)
 		0, rxf->rx->rid);
 	req->mh.num_entries = htons(
 	bfi_msgq_num_cmd_entries(sizeof(struct bfi_enet_mcast_add_req)));
-	memcpy(&req->mac_addr, &mac->addr, sizeof(mac_t));
+	ether_addr_copy(req->mac_addr, mac->addr);
 	bfa_msgq_cmd_set(&rxf->msgq_cmd, NULL, NULL,
 		sizeof(struct bfi_enet_mcast_add_req), &req->mh);
 	bfa_msgq_cmd_post(&rxf->rx->bna->msgq, &rxf->msgq_cmd);
@@ -444,22 +353,17 @@ bna_bfi_rss_enable(struct bna_rxf *rxf)
 
 /* This function gets the multicast MAC that has already been added to CAM */
 static struct bna_mac *
-bna_rxf_mcmac_get(struct bna_rxf *rxf, u8 *mac_addr)
+bna_rxf_mcmac_get(struct bna_rxf *rxf, const u8 *mac_addr)
 {
 	struct bna_mac *mac;
-	struct list_head *qe;
 
-	list_for_each(qe, &rxf->mcast_active_q) {
-		mac = (struct bna_mac *)qe;
-		if (BNA_MAC_IS_EQUAL(&mac->addr, mac_addr))
+	list_for_each_entry(mac, &rxf->mcast_active_q, qe)
+		if (ether_addr_equal(mac->addr, mac_addr))
 			return mac;
-	}
 
-	list_for_each(qe, &rxf->mcast_pending_del_q) {
-		mac = (struct bna_mac *)qe;
-		if (BNA_MAC_IS_EQUAL(&mac->addr, mac_addr))
+	list_for_each_entry(mac, &rxf->mcast_pending_del_q, qe)
+		if (ether_addr_equal(mac->addr, mac_addr))
 			return mac;
-	}
 
 	return NULL;
 }
@@ -468,13 +372,10 @@ static struct bna_mcam_handle *
 bna_rxf_mchandle_get(struct bna_rxf *rxf, int handle)
 {
 	struct bna_mcam_handle *mchandle;
-	struct list_head *qe;
 
-	list_for_each(qe, &rxf->mcast_handle_q) {
-		mchandle = (struct bna_mcam_handle *)qe;
+	list_for_each_entry(mchandle, &rxf->mcast_handle_q, qe)
 		if (mchandle->handle == handle)
 			return mchandle;
-	}
 
 	return NULL;
 }
@@ -515,7 +416,6 @@ bna_rxf_mcast_del(struct bna_rxf *rxf, struct bna_mac *mac,
 			ret = 1;
 		}
 		list_del(&mchandle->qe);
-		bfa_q_qe_init(&mchandle->qe);
 		bna_mcam_mod_handle_put(&rxf->rx->bna->mcam_mod, mchandle);
 	}
 	mac->handle = NULL;
@@ -527,26 +427,23 @@ static int
 bna_rxf_mcast_cfg_apply(struct bna_rxf *rxf)
 {
 	struct bna_mac *mac = NULL;
-	struct list_head *qe;
 	int ret;
 
 	/* First delete multicast entries to maintain the count */
 	while (!list_empty(&rxf->mcast_pending_del_q)) {
-		bfa_q_deq(&rxf->mcast_pending_del_q, &qe);
-		bfa_q_qe_init(qe);
-		mac = (struct bna_mac *)qe;
+		mac = list_first_entry(&rxf->mcast_pending_del_q,
+				       struct bna_mac, qe);
 		ret = bna_rxf_mcast_del(rxf, mac, BNA_HARD_CLEANUP);
-		bna_cam_mod_mac_put(bna_mcam_mod_del_q(rxf->rx->bna), mac);
+		list_move_tail(&mac->qe, bna_mcam_mod_del_q(rxf->rx->bna));
 		if (ret)
 			return ret;
 	}
 
 	/* Add multicast entries */
 	if (!list_empty(&rxf->mcast_pending_add_q)) {
-		bfa_q_deq(&rxf->mcast_pending_add_q, &qe);
-		bfa_q_qe_init(qe);
-		mac = (struct bna_mac *)qe;
-		list_add_tail(&mac->qe, &rxf->mcast_active_q);
+		mac = list_first_entry(&rxf->mcast_pending_add_q,
+				       struct bna_mac, qe);
+		list_move_tail(&mac->qe, &rxf->mcast_active_q);
 		bna_bfi_mcast_add_req(rxf, mac);
 		return 1;
 	}
@@ -566,7 +463,7 @@ bna_rxf_vlan_cfg_apply(struct bna_rxf *rxf)
 			block_idx++;
 			vlan_pending_bitmask >>= 1;
 		}
-		rxf->vlan_pending_bitmask &= ~(1 << block_idx);
+		rxf->vlan_pending_bitmask &= ~BIT(block_idx);
 		bna_bfi_rx_vlan_filter_set(rxf, block_idx);
 		return 1;
 	}
@@ -577,27 +474,24 @@ bna_rxf_vlan_cfg_apply(struct bna_rxf *rxf)
 static int
 bna_rxf_mcast_cfg_reset(struct bna_rxf *rxf, enum bna_cleanup_type cleanup)
 {
-	struct list_head *qe;
 	struct bna_mac *mac;
 	int ret;
 
 	/* Throw away delete pending mcast entries */
 	while (!list_empty(&rxf->mcast_pending_del_q)) {
-		bfa_q_deq(&rxf->mcast_pending_del_q, &qe);
-		bfa_q_qe_init(qe);
-		mac = (struct bna_mac *)qe;
+		mac = list_first_entry(&rxf->mcast_pending_del_q,
+				       struct bna_mac, qe);
 		ret = bna_rxf_mcast_del(rxf, mac, cleanup);
-		bna_cam_mod_mac_put(bna_mcam_mod_del_q(rxf->rx->bna), mac);
+		list_move_tail(&mac->qe, bna_mcam_mod_del_q(rxf->rx->bna));
 		if (ret)
 			return ret;
 	}
 
 	/* Move active mcast entries to pending_add_q */
 	while (!list_empty(&rxf->mcast_active_q)) {
-		bfa_q_deq(&rxf->mcast_active_q, &qe);
-		bfa_q_qe_init(qe);
-		list_add_tail(qe, &rxf->mcast_pending_add_q);
-		mac = (struct bna_mac *)qe;
+		mac = list_first_entry(&rxf->mcast_active_q,
+				       struct bna_mac, qe);
+		list_move_tail(&mac->qe, &rxf->mcast_pending_add_q);
 		if (bna_rxf_mcast_del(rxf, mac, cleanup))
 			return 1;
 	}
@@ -658,25 +552,6 @@ bna_rxf_cfg_apply(struct bna_rxf *rxf)
 	return 0;
 }
 
-/* Only software reset */
-static int
-bna_rxf_fltr_clear(struct bna_rxf *rxf)
-{
-	if (bna_rxf_ucast_cfg_reset(rxf, BNA_HARD_CLEANUP))
-		return 1;
-
-	if (bna_rxf_mcast_cfg_reset(rxf, BNA_HARD_CLEANUP))
-		return 1;
-
-	if (bna_rxf_promisc_cfg_reset(rxf, BNA_HARD_CLEANUP))
-		return 1;
-
-	if (bna_rxf_allmulti_cfg_reset(rxf, BNA_HARD_CLEANUP))
-		return 1;
-
-	return 0;
-}
-
 static void
 bna_rxf_cfg_reset(struct bna_rxf *rxf)
 {
@@ -693,16 +568,13 @@ bna_rit_init(struct bna_rxf *rxf, int rit_size)
 {
 	struct bna_rx *rx = rxf->rx;
 	struct bna_rxp *rxp;
-	struct list_head *qe;
 	int offset = 0;
 
 	rxf->rit_size = rit_size;
-	list_for_each(qe, &rx->rxp_q) {
-		rxp = (struct bna_rxp *)qe;
+	list_for_each_entry(rxp, &rx->rxp_q, qe) {
 		rxf->rit[offset] = rxp->cq.ccb->id;
 		offset++;
 	}
-
 }
 
 void
@@ -760,9 +632,6 @@ bna_rxf_init(struct bna_rxf *rxf,
 	INIT_LIST_HEAD(&rxf->mcast_active_q);
 	INIT_LIST_HEAD(&rxf->mcast_handle_q);
 
-	if (q_config->paused)
-		rxf->flags |= BNA_RXF_F_PAUSED;
-
 	rxf->rit = (u8 *)
 		res_info[BNA_RX_RES_MEM_T_RIT].res_u.mem_info.mdl[0].kva;
 	bna_rit_init(rxf, q_config->num_paths);
@@ -795,22 +664,21 @@ bna_rxf_uninit(struct bna_rxf *rxf)
 	rxf->ucast_active_set = 0;
 
 	while (!list_empty(&rxf->ucast_pending_add_q)) {
-		bfa_q_deq(&rxf->ucast_pending_add_q, &mac);
-		bfa_q_qe_init(&mac->qe);
-		bna_cam_mod_mac_put(bna_ucam_mod_free_q(rxf->rx->bna), mac);
+		mac = list_first_entry(&rxf->ucast_pending_add_q,
+				       struct bna_mac, qe);
+		list_move_tail(&mac->qe, bna_ucam_mod_free_q(rxf->rx->bna));
 	}
 
 	if (rxf->ucast_pending_mac) {
-		bfa_q_qe_init(&rxf->ucast_pending_mac->qe);
-		bna_cam_mod_mac_put(bna_ucam_mod_free_q(rxf->rx->bna),
-				    rxf->ucast_pending_mac);
+		list_add_tail(&rxf->ucast_pending_mac->qe,
+			      bna_ucam_mod_free_q(rxf->rx->bna));
 		rxf->ucast_pending_mac = NULL;
 	}
 
 	while (!list_empty(&rxf->mcast_pending_add_q)) {
-		bfa_q_deq(&rxf->mcast_pending_add_q, &mac);
-		bfa_q_qe_init(&mac->qe);
-		bna_cam_mod_mac_put(bna_mcam_mod_free_q(rxf->rx->bna), mac);
+		mac = list_first_entry(&rxf->mcast_pending_add_q,
+				       struct bna_mac, qe);
+		list_move_tail(&mac->qe, bna_mcam_mod_free_q(rxf->rx->bna));
 	}
 
 	rxf->rxmode_pending = 0;
@@ -823,8 +691,6 @@ bna_rxf_uninit(struct bna_rxf *rxf)
 	rxf->rss_pending = 0;
 	rxf->vlan_strip_pending = false;
 
-	rxf->flags = 0;
-
 	rxf->rx = NULL;
 }
 
@@ -863,8 +729,7 @@ bna_rxf_fail(struct bna_rxf *rxf)
 }
 
 enum bna_cb_status
-bna_rx_ucast_set(struct bna_rx *rx, u8 *ucmac,
-		 void (*cbfn)(struct bnad *, struct bna_rx *))
+bna_rx_ucast_set(struct bna_rx *rx, const u8 *ucmac)
 {
 	struct bna_rxf *rxf = &rx->rxf;
 
@@ -873,12 +738,11 @@ bna_rx_ucast_set(struct bna_rx *rx, u8 *ucmac,
 			bna_cam_mod_mac_get(bna_ucam_mod_free_q(rxf->rx->bna));
 		if (rxf->ucast_pending_mac == NULL)
 			return BNA_CB_UCAST_CAM_FULL;
-		bfa_q_qe_init(&rxf->ucast_pending_mac->qe);
 	}
 
-	memcpy(rxf->ucast_pending_mac->addr, ucmac, ETH_ALEN);
+	ether_addr_copy(rxf->ucast_pending_mac->addr, ucmac);
 	rxf->ucast_pending_set = 1;
-	rxf->cam_fltr_cbfn = cbfn;
+	rxf->cam_fltr_cbfn = NULL;
 	rxf->cam_fltr_cbarg = rx->bna->bnad;
 
 	bfa_fsm_send_event(rxf, RXF_E_CONFIG);
@@ -887,7 +751,7 @@ bna_rx_ucast_set(struct bna_rx *rx, u8 *ucmac,
 }
 
 enum bna_cb_status
-bna_rx_mcast_add(struct bna_rx *rx, u8 *addr,
+bna_rx_mcast_add(struct bna_rx *rx, const u8 *addr,
 		 void (*cbfn)(struct bnad *, struct bna_rx *))
 {
 	struct bna_rxf *rxf = &rx->rxf;
@@ -904,8 +768,7 @@ bna_rx_mcast_add(struct bna_rx *rx, u8 *addr,
 	mac = bna_cam_mod_mac_get(bna_mcam_mod_free_q(rxf->rx->bna));
 	if (mac == NULL)
 		return BNA_CB_MCAST_LIST_FULL;
-	bfa_q_qe_init(&mac->qe);
-	memcpy(mac->addr, addr, ETH_ALEN);
+	ether_addr_copy(mac->addr, addr);
 	list_add_tail(&mac->qe, &rxf->mcast_pending_add_q);
 
 	rxf->cam_fltr_cbfn = cbfn;
@@ -917,35 +780,31 @@ bna_rx_mcast_add(struct bna_rx *rx, u8 *addr,
 }
 
 enum bna_cb_status
-bna_rx_ucast_listset(struct bna_rx *rx, int count, u8 *uclist,
-		     void (*cbfn)(struct bnad *, struct bna_rx *))
+bna_rx_ucast_listset(struct bna_rx *rx, int count, const u8 *uclist)
 {
 	struct bna_ucam_mod *ucam_mod = &rx->bna->ucam_mod;
 	struct bna_rxf *rxf = &rx->rxf;
 	struct list_head list_head;
-	struct list_head *qe;
-	u8 *mcaddr;
+	const u8 *mcaddr;
 	struct bna_mac *mac, *del_mac;
 	int i;
 
 	/* Purge the pending_add_q */
 	while (!list_empty(&rxf->ucast_pending_add_q)) {
-		bfa_q_deq(&rxf->ucast_pending_add_q, &qe);
-		bfa_q_qe_init(qe);
-		mac = (struct bna_mac *)qe;
-		bna_cam_mod_mac_put(&ucam_mod->free_q, mac);
+		mac = list_first_entry(&rxf->ucast_pending_add_q,
+				       struct bna_mac, qe);
+		list_move_tail(&mac->qe, &ucam_mod->free_q);
 	}
 
 	/* Schedule active_q entries for deletion */
 	while (!list_empty(&rxf->ucast_active_q)) {
-		bfa_q_deq(&rxf->ucast_active_q, &qe);
-		mac = (struct bna_mac *)qe;
-		bfa_q_qe_init(&mac->qe);
-
+		mac = list_first_entry(&rxf->ucast_active_q,
+				       struct bna_mac, qe);
 		del_mac = bna_cam_mod_mac_get(&ucam_mod->del_q);
-		memcpy(del_mac, mac, sizeof(*del_mac));
+		ether_addr_copy(del_mac->addr, mac->addr);
+		del_mac->handle = mac->handle;
 		list_add_tail(&del_mac->qe, &rxf->ucast_pending_del_q);
-		bna_cam_mod_mac_put(&ucam_mod->free_q, mac);
+		list_move_tail(&mac->qe, &ucam_mod->free_q);
 	}
 
 	/* Allocate nodes */
@@ -954,69 +813,57 @@ bna_rx_ucast_listset(struct bna_rx *rx, int count, u8 *uclist,
 		mac = bna_cam_mod_mac_get(&ucam_mod->free_q);
 		if (mac == NULL)
 			goto err_return;
-		bfa_q_qe_init(&mac->qe);
-		memcpy(mac->addr, mcaddr, ETH_ALEN);
+		ether_addr_copy(mac->addr, mcaddr);
 		list_add_tail(&mac->qe, &list_head);
 		mcaddr += ETH_ALEN;
 	}
 
 	/* Add the new entries */
 	while (!list_empty(&list_head)) {
-		bfa_q_deq(&list_head, &qe);
-		mac = (struct bna_mac *)qe;
-		bfa_q_qe_init(&mac->qe);
-		list_add_tail(&mac->qe, &rxf->ucast_pending_add_q);
+		mac = list_first_entry(&list_head, struct bna_mac, qe);
+		list_move_tail(&mac->qe, &rxf->ucast_pending_add_q);
 	}
 
-	rxf->cam_fltr_cbfn = cbfn;
-	rxf->cam_fltr_cbarg = rx->bna->bnad;
 	bfa_fsm_send_event(rxf, RXF_E_CONFIG);
 
 	return BNA_CB_SUCCESS;
 
 err_return:
 	while (!list_empty(&list_head)) {
-		bfa_q_deq(&list_head, &qe);
-		mac = (struct bna_mac *)qe;
-		bfa_q_qe_init(&mac->qe);
-		bna_cam_mod_mac_put(&ucam_mod->free_q, mac);
+		mac = list_first_entry(&list_head, struct bna_mac, qe);
+		list_move_tail(&mac->qe, &ucam_mod->free_q);
 	}
 
 	return BNA_CB_UCAST_CAM_FULL;
 }
 
 enum bna_cb_status
-bna_rx_mcast_listset(struct bna_rx *rx, int count, u8 *mclist,
-		     void (*cbfn)(struct bnad *, struct bna_rx *))
+bna_rx_mcast_listset(struct bna_rx *rx, int count, const u8 *mclist)
 {
 	struct bna_mcam_mod *mcam_mod = &rx->bna->mcam_mod;
 	struct bna_rxf *rxf = &rx->rxf;
 	struct list_head list_head;
-	struct list_head *qe;
-	u8 *mcaddr;
+	const u8 *mcaddr;
 	struct bna_mac *mac, *del_mac;
 	int i;
 
 	/* Purge the pending_add_q */
 	while (!list_empty(&rxf->mcast_pending_add_q)) {
-		bfa_q_deq(&rxf->mcast_pending_add_q, &qe);
-		bfa_q_qe_init(qe);
-		mac = (struct bna_mac *)qe;
-		bna_cam_mod_mac_put(&mcam_mod->free_q, mac);
+		mac = list_first_entry(&rxf->mcast_pending_add_q,
+				       struct bna_mac, qe);
+		list_move_tail(&mac->qe, &mcam_mod->free_q);
 	}
 
 	/* Schedule active_q entries for deletion */
 	while (!list_empty(&rxf->mcast_active_q)) {
-		bfa_q_deq(&rxf->mcast_active_q, &qe);
-		mac = (struct bna_mac *)qe;
-		bfa_q_qe_init(&mac->qe);
-
+		mac = list_first_entry(&rxf->mcast_active_q,
+				       struct bna_mac, qe);
 		del_mac = bna_cam_mod_mac_get(&mcam_mod->del_q);
-
-		memcpy(del_mac, mac, sizeof(*del_mac));
+		ether_addr_copy(del_mac->addr, mac->addr);
+		del_mac->handle = mac->handle;
 		list_add_tail(&del_mac->qe, &rxf->mcast_pending_del_q);
 		mac->handle = NULL;
-		bna_cam_mod_mac_put(&mcam_mod->free_q, mac);
+		list_move_tail(&mac->qe, &mcam_mod->free_q);
 	}
 
 	/* Allocate nodes */
@@ -1025,8 +872,7 @@ bna_rx_mcast_listset(struct bna_rx *rx, int count, u8 *mclist,
 		mac = bna_cam_mod_mac_get(&mcam_mod->free_q);
 		if (mac == NULL)
 			goto err_return;
-		bfa_q_qe_init(&mac->qe);
-		memcpy(mac->addr, mcaddr, ETH_ALEN);
+		ether_addr_copy(mac->addr, mcaddr);
 		list_add_tail(&mac->qe, &list_head);
 
 		mcaddr += ETH_ALEN;
@@ -1034,70 +880,52 @@ bna_rx_mcast_listset(struct bna_rx *rx, int count, u8 *mclist,
 
 	/* Add the new entries */
 	while (!list_empty(&list_head)) {
-		bfa_q_deq(&list_head, &qe);
-		mac = (struct bna_mac *)qe;
-		bfa_q_qe_init(&mac->qe);
-		list_add_tail(&mac->qe, &rxf->mcast_pending_add_q);
+		mac = list_first_entry(&list_head, struct bna_mac, qe);
+		list_move_tail(&mac->qe, &rxf->mcast_pending_add_q);
 	}
 
-	rxf->cam_fltr_cbfn = cbfn;
-	rxf->cam_fltr_cbarg = rx->bna->bnad;
 	bfa_fsm_send_event(rxf, RXF_E_CONFIG);
 
 	return BNA_CB_SUCCESS;
 
 err_return:
 	while (!list_empty(&list_head)) {
-		bfa_q_deq(&list_head, &qe);
-		mac = (struct bna_mac *)qe;
-		bfa_q_qe_init(&mac->qe);
-		bna_cam_mod_mac_put(&mcam_mod->free_q, mac);
+		mac = list_first_entry(&list_head, struct bna_mac, qe);
+		list_move_tail(&mac->qe, &mcam_mod->free_q);
 	}
 
 	return BNA_CB_MCAST_LIST_FULL;
 }
 
 void
-bna_rx_mcast_delall(struct bna_rx *rx,
-		    void (*cbfn)(struct bnad *, struct bna_rx *))
+bna_rx_mcast_delall(struct bna_rx *rx)
 {
 	struct bna_rxf *rxf = &rx->rxf;
-	struct list_head *qe;
 	struct bna_mac *mac, *del_mac;
 	int need_hw_config = 0;
 
 	/* Purge all entries from pending_add_q */
 	while (!list_empty(&rxf->mcast_pending_add_q)) {
-		bfa_q_deq(&rxf->mcast_pending_add_q, &qe);
-		mac = (struct bna_mac *)qe;
-		bfa_q_qe_init(&mac->qe);
-		bna_cam_mod_mac_put(bna_mcam_mod_free_q(rxf->rx->bna), mac);
+		mac = list_first_entry(&rxf->mcast_pending_add_q,
+				       struct bna_mac, qe);
+		list_move_tail(&mac->qe, bna_mcam_mod_free_q(rxf->rx->bna));
 	}
 
 	/* Schedule all entries in active_q for deletion */
 	while (!list_empty(&rxf->mcast_active_q)) {
-		bfa_q_deq(&rxf->mcast_active_q, &qe);
-		mac = (struct bna_mac *)qe;
-		bfa_q_qe_init(&mac->qe);
-
+		mac = list_first_entry(&rxf->mcast_active_q,
+				       struct bna_mac, qe);
+		list_del(&mac->qe);
 		del_mac = bna_cam_mod_mac_get(bna_mcam_mod_del_q(rxf->rx->bna));
-
 		memcpy(del_mac, mac, sizeof(*del_mac));
 		list_add_tail(&del_mac->qe, &rxf->mcast_pending_del_q);
 		mac->handle = NULL;
-		bna_cam_mod_mac_put(bna_mcam_mod_free_q(rxf->rx->bna), mac);
+		list_add_tail(&mac->qe, bna_mcam_mod_free_q(rxf->rx->bna));
 		need_hw_config = 1;
 	}
 
-	if (need_hw_config) {
-		rxf->cam_fltr_cbfn = cbfn;
-		rxf->cam_fltr_cbarg = rx->bna->bnad;
+	if (need_hw_config)
 		bfa_fsm_send_event(rxf, RXF_E_CONFIG);
-		return;
-	}
-
-	if (cbfn)
-		(*cbfn)(rx->bna->bnad, rx);
 }
 
 void
@@ -1105,12 +933,12 @@ bna_rx_vlan_add(struct bna_rx *rx, int vlan_id)
 {
 	struct bna_rxf *rxf = &rx->rxf;
 	int index = (vlan_id >> BFI_VLAN_WORD_SHIFT);
-	int bit = (1 << (vlan_id & BFI_VLAN_WORD_MASK));
+	int bit = BIT(vlan_id & BFI_VLAN_WORD_MASK);
 	int group_id = (vlan_id >> BFI_VLAN_BLOCK_SHIFT);
 
 	rxf->vlan_filter_table[index] |= bit;
 	if (rxf->vlan_filter_status == BNA_STATUS_T_ENABLED) {
-		rxf->vlan_pending_bitmask |= (1 << group_id);
+		rxf->vlan_pending_bitmask |= BIT(group_id);
 		bfa_fsm_send_event(rxf, RXF_E_CONFIG);
 	}
 }
@@ -1120,12 +948,12 @@ bna_rx_vlan_del(struct bna_rx *rx, int vlan_id)
 {
 	struct bna_rxf *rxf = &rx->rxf;
 	int index = (vlan_id >> BFI_VLAN_WORD_SHIFT);
-	int bit = (1 << (vlan_id & BFI_VLAN_WORD_MASK));
+	int bit = BIT(vlan_id & BFI_VLAN_WORD_MASK);
 	int group_id = (vlan_id >> BFI_VLAN_BLOCK_SHIFT);
 
 	rxf->vlan_filter_table[index] &= ~bit;
 	if (rxf->vlan_filter_status == BNA_STATUS_T_ENABLED) {
-		rxf->vlan_pending_bitmask |= (1 << group_id);
+		rxf->vlan_pending_bitmask |= BIT(group_id);
 		bfa_fsm_send_event(rxf, RXF_E_CONFIG);
 	}
 }
@@ -1134,23 +962,21 @@ static int
 bna_rxf_ucast_cfg_apply(struct bna_rxf *rxf)
 {
 	struct bna_mac *mac = NULL;
-	struct list_head *qe;
 
 	/* Delete MAC addresses previousely added */
 	if (!list_empty(&rxf->ucast_pending_del_q)) {
-		bfa_q_deq(&rxf->ucast_pending_del_q, &qe);
-		bfa_q_qe_init(qe);
-		mac = (struct bna_mac *)qe;
+		mac = list_first_entry(&rxf->ucast_pending_del_q,
+				       struct bna_mac, qe);
 		bna_bfi_ucast_req(rxf, mac, BFI_ENET_H2I_MAC_UCAST_DEL_REQ);
-		bna_cam_mod_mac_put(bna_ucam_mod_del_q(rxf->rx->bna), mac);
+		list_move_tail(&mac->qe, bna_ucam_mod_del_q(rxf->rx->bna));
 		return 1;
 	}
 
 	/* Set default unicast MAC */
 	if (rxf->ucast_pending_set) {
 		rxf->ucast_pending_set = 0;
-		memcpy(rxf->ucast_active_mac.addr,
-			rxf->ucast_pending_mac->addr, ETH_ALEN);
+		ether_addr_copy(rxf->ucast_active_mac.addr,
+				rxf->ucast_pending_mac->addr);
 		rxf->ucast_active_set = 1;
 		bna_bfi_ucast_req(rxf, &rxf->ucast_active_mac,
 			BFI_ENET_H2I_MAC_UCAST_SET_REQ);
@@ -1159,9 +985,8 @@ bna_rxf_ucast_cfg_apply(struct bna_rxf *rxf)
 
 	/* Add additional MAC entries */
 	if (!list_empty(&rxf->ucast_pending_add_q)) {
-		bfa_q_deq(&rxf->ucast_pending_add_q, &qe);
-		bfa_q_qe_init(qe);
-		mac = (struct bna_mac *)qe;
+		mac = list_first_entry(&rxf->ucast_pending_add_q,
+				       struct bna_mac, qe);
 		list_add_tail(&mac->qe, &rxf->ucast_active_q);
 		bna_bfi_ucast_req(rxf, mac, BFI_ENET_H2I_MAC_UCAST_ADD_REQ);
 		return 1;
@@ -1173,33 +998,30 @@ bna_rxf_ucast_cfg_apply(struct bna_rxf *rxf)
 static int
 bna_rxf_ucast_cfg_reset(struct bna_rxf *rxf, enum bna_cleanup_type cleanup)
 {
-	struct list_head *qe;
 	struct bna_mac *mac;
 
 	/* Throw away delete pending ucast entries */
 	while (!list_empty(&rxf->ucast_pending_del_q)) {
-		bfa_q_deq(&rxf->ucast_pending_del_q, &qe);
-		bfa_q_qe_init(qe);
-		mac = (struct bna_mac *)qe;
+		mac = list_first_entry(&rxf->ucast_pending_del_q,
+				       struct bna_mac, qe);
 		if (cleanup == BNA_SOFT_CLEANUP)
-			bna_cam_mod_mac_put(bna_ucam_mod_del_q(rxf->rx->bna),
-					    mac);
+			list_move_tail(&mac->qe,
+				       bna_ucam_mod_del_q(rxf->rx->bna));
 		else {
 			bna_bfi_ucast_req(rxf, mac,
-				BFI_ENET_H2I_MAC_UCAST_DEL_REQ);
-			bna_cam_mod_mac_put(bna_ucam_mod_del_q(rxf->rx->bna),
-					    mac);
+					  BFI_ENET_H2I_MAC_UCAST_DEL_REQ);
+			list_move_tail(&mac->qe,
+				       bna_ucam_mod_del_q(rxf->rx->bna));
 			return 1;
 		}
 	}
 
 	/* Move active ucast entries to pending_add_q */
 	while (!list_empty(&rxf->ucast_active_q)) {
-		bfa_q_deq(&rxf->ucast_active_q, &qe);
-		bfa_q_qe_init(qe);
-		list_add_tail(qe, &rxf->ucast_pending_add_q);
+		mac = list_first_entry(&rxf->ucast_active_q,
+				       struct bna_mac, qe);
+		list_move_tail(&mac->qe, &rxf->ucast_pending_add_q);
 		if (cleanup == BNA_HARD_CLEANUP) {
-			mac = (struct bna_mac *)qe;
 			bna_bfi_ucast_req(rxf, mac,
 				BFI_ENET_H2I_MAC_UCAST_DEL_REQ);
 			return 1;
@@ -1654,14 +1476,11 @@ static void
 bna_rx_sm_started_entry(struct bna_rx *rx)
 {
 	struct bna_rxp *rxp;
-	struct list_head *qe_rxp;
 	int is_regular = (rx->type == BNA_RX_T_REGULAR);
 
 	/* Start IB */
-	list_for_each(qe_rxp, &rx->rxp_q) {
-		rxp = (struct bna_rxp *)qe_rxp;
+	list_for_each_entry(rxp, &rx->rxp_q, qe)
 		bna_ib_start(rx->bna, &rxp->cq.ib, is_regular);
-	}
 
 	bna_ethport_cb_rx_started(&rx->bna->ethport);
 }
@@ -1804,7 +1623,6 @@ bna_bfi_rx_enet_start(struct bna_rx *rx)
 	struct bfi_enet_rx_cfg_req *cfg_req = &rx->bfi_enet_cmd.cfg_req;
 	struct bna_rxp *rxp = NULL;
 	struct bna_rxq *q0 = NULL, *q1 = NULL;
-	struct list_head *rxp_qe;
 	int i;
 
 	bfi_msgq_mhdr_set(cfg_req->mh, BFI_MC_ENET,
@@ -1814,11 +1632,9 @@ bna_bfi_rx_enet_start(struct bna_rx *rx)
 
 	cfg_req->rx_cfg.frame_size = bna_enet_mtu_get(&rx->bna->enet);
 	cfg_req->num_queue_sets = rx->num_paths;
-	for (i = 0, rxp_qe = bfa_q_first(&rx->rxp_q);
-		i < rx->num_paths;
-		i++, rxp_qe = bfa_q_next(rxp_qe)) {
-		rxp = (struct bna_rxp *)rxp_qe;
-
+	for (i = 0; i < rx->num_paths; i++) {
+		rxp = rxp ? list_next_entry(rxp, qe)
+			: list_first_entry(&rx->rxp_q, struct bna_rxp, qe);
 		GET_RXQS(rxp, q0, q1);
 		switch (rxp->type) {
 		case BNA_RXP_SLR:
@@ -1921,13 +1737,10 @@ static void
 bna_rx_enet_stop(struct bna_rx *rx)
 {
 	struct bna_rxp *rxp;
-	struct list_head		 *qe_rxp;
 
 	/* Stop IB */
-	list_for_each(qe_rxp, &rx->rxp_q) {
-		rxp = (struct bna_rxp *)qe_rxp;
+	list_for_each_entry(rxp, &rx->rxp_q, qe)
 		bna_ib_stop(rx->bna, &rxp->cq.ib);
-	}
 
 	bna_bfi_rx_enet_stop(rx);
 }
@@ -1957,12 +1770,10 @@ static struct bna_rxq *
 bna_rxq_get(struct bna_rx_mod *rx_mod)
 {
 	struct bna_rxq *rxq = NULL;
-	struct list_head	*qe = NULL;
 
-	bfa_q_deq(&rx_mod->rxq_free_q, &qe);
+	rxq = list_first_entry(&rx_mod->rxq_free_q, struct bna_rxq, qe);
+	list_del(&rxq->qe);
 	rx_mod->rxq_free_count--;
-	rxq = (struct bna_rxq *)qe;
-	bfa_q_qe_init(&rxq->qe);
 
 	return rxq;
 }
@@ -1970,7 +1781,6 @@ bna_rxq_get(struct bna_rx_mod *rx_mod)
 static void
 bna_rxq_put(struct bna_rx_mod *rx_mod, struct bna_rxq *rxq)
 {
-	bfa_q_qe_init(&rxq->qe);
 	list_add_tail(&rxq->qe, &rx_mod->rxq_free_q);
 	rx_mod->rxq_free_count++;
 }
@@ -1978,13 +1788,11 @@ bna_rxq_put(struct bna_rx_mod *rx_mod, struct bna_rxq *rxq)
 static struct bna_rxp *
 bna_rxp_get(struct bna_rx_mod *rx_mod)
 {
-	struct list_head	*qe = NULL;
 	struct bna_rxp *rxp = NULL;
 
-	bfa_q_deq(&rx_mod->rxp_free_q, &qe);
+	rxp = list_first_entry(&rx_mod->rxp_free_q, struct bna_rxp, qe);
+	list_del(&rxp->qe);
 	rx_mod->rxp_free_count--;
-	rxp = (struct bna_rxp *)qe;
-	bfa_q_qe_init(&rxp->qe);
 
 	return rxp;
 }
@@ -1992,7 +1800,6 @@ bna_rxp_get(struct bna_rx_mod *rx_mod)
 static void
 bna_rxp_put(struct bna_rx_mod *rx_mod, struct bna_rxp *rxp)
 {
-	bfa_q_qe_init(&rxp->qe);
 	list_add_tail(&rxp->qe, &rx_mod->rxp_free_q);
 	rx_mod->rxp_free_count++;
 }
@@ -2000,18 +1807,16 @@ bna_rxp_put(struct bna_rx_mod *rx_mod, struct bna_rxp *rxp)
 static struct bna_rx *
 bna_rx_get(struct bna_rx_mod *rx_mod, enum bna_rx_type type)
 {
-	struct list_head	*qe = NULL;
 	struct bna_rx *rx = NULL;
 
-	if (type == BNA_RX_T_REGULAR) {
-		bfa_q_deq(&rx_mod->rx_free_q, &qe);
-	} else
-		bfa_q_deq_tail(&rx_mod->rx_free_q, &qe);
+	BUG_ON(list_empty(&rx_mod->rx_free_q));
+	if (type == BNA_RX_T_REGULAR)
+		rx = list_first_entry(&rx_mod->rx_free_q, struct bna_rx, qe);
+	else
+		rx = list_last_entry(&rx_mod->rx_free_q, struct bna_rx, qe);
 
 	rx_mod->rx_free_count--;
-	rx = (struct bna_rx *)qe;
-	bfa_q_qe_init(&rx->qe);
-	list_add_tail(&rx->qe, &rx_mod->rx_active_q);
+	list_move_tail(&rx->qe, &rx_mod->rx_active_q);
 	rx->type = type;
 
 	return rx;
@@ -2020,32 +1825,13 @@ bna_rx_get(struct bna_rx_mod *rx_mod, enum bna_rx_type type)
 static void
 bna_rx_put(struct bna_rx_mod *rx_mod, struct bna_rx *rx)
 {
-	struct list_head *prev_qe = NULL;
 	struct list_head *qe;
 
-	bfa_q_qe_init(&rx->qe);
-
-	list_for_each(qe, &rx_mod->rx_free_q) {
+	list_for_each_prev(qe, &rx_mod->rx_free_q)
 		if (((struct bna_rx *)qe)->rid < rx->rid)
-			prev_qe = qe;
-		else
 			break;
-	}
-
-	if (prev_qe == NULL) {
-		/* This is the first entry */
-		bfa_q_enq_head(&rx_mod->rx_free_q, &rx->qe);
-	} else if (bfa_q_next(prev_qe) == &rx_mod->rx_free_q) {
-		/* This is the last entry */
-		list_add_tail(&rx->qe, &rx_mod->rx_free_q);
-	} else {
-		/* Somewhere in the middle */
-		bfa_q_next(&rx->qe) = bfa_q_next(prev_qe);
-		bfa_q_prev(&rx->qe) = prev_qe;
-		bfa_q_next(prev_qe) = &rx->qe;
-		bfa_q_prev(bfa_q_next(&rx->qe)) = &rx->qe;
-	}
 
+	list_add(&rx->qe, qe);
 	rx_mod->rx_free_count++;
 }
 
@@ -2199,24 +1985,20 @@ void
 bna_rx_mod_start(struct bna_rx_mod *rx_mod, enum bna_rx_type type)
 {
 	struct bna_rx *rx;
-	struct list_head *qe;
 
 	rx_mod->flags |= BNA_RX_MOD_F_ENET_STARTED;
 	if (type == BNA_RX_T_LOOPBACK)
 		rx_mod->flags |= BNA_RX_MOD_F_ENET_LOOPBACK;
 
-	list_for_each(qe, &rx_mod->rx_active_q) {
-		rx = (struct bna_rx *)qe;
+	list_for_each_entry(rx, &rx_mod->rx_active_q, qe)
 		if (rx->type == type)
 			bna_rx_start(rx);
-	}
 }
 
 void
 bna_rx_mod_stop(struct bna_rx_mod *rx_mod, enum bna_rx_type type)
 {
 	struct bna_rx *rx;
-	struct list_head *qe;
 
 	rx_mod->flags &= ~BNA_RX_MOD_F_ENET_STARTED;
 	rx_mod->flags &= ~BNA_RX_MOD_F_ENET_LOOPBACK;
@@ -2225,13 +2007,11 @@ bna_rx_mod_stop(struct bna_rx_mod *rx_mod, enum bna_rx_type type)
 
 	bfa_wc_init(&rx_mod->rx_stop_wc, bna_rx_mod_cb_rx_stopped_all, rx_mod);
 
-	list_for_each(qe, &rx_mod->rx_active_q) {
-		rx = (struct bna_rx *)qe;
+	list_for_each_entry(rx, &rx_mod->rx_active_q, qe)
 		if (rx->type == type) {
 			bfa_wc_up(&rx_mod->rx_stop_wc);
 			bna_rx_stop(rx);
 		}
-	}
 
 	bfa_wc_wait(&rx_mod->rx_stop_wc);
 }
@@ -2240,15 +2020,12 @@ void
 bna_rx_mod_fail(struct bna_rx_mod *rx_mod)
 {
 	struct bna_rx *rx;
-	struct list_head *qe;
 
 	rx_mod->flags &= ~BNA_RX_MOD_F_ENET_STARTED;
 	rx_mod->flags &= ~BNA_RX_MOD_F_ENET_LOOPBACK;
 
-	list_for_each(qe, &rx_mod->rx_active_q) {
-		rx = (struct bna_rx *)qe;
+	list_for_each_entry(rx, &rx_mod->rx_active_q, qe)
 		bna_rx_fail(rx);
-	}
 }
 
 void bna_rx_mod_init(struct bna_rx_mod *rx_mod, struct bna *bna,
@@ -2282,7 +2059,6 @@ void bna_rx_mod_init(struct bna_rx_mod *rx_mod, struct bna *bna,
 	for (index = 0; index < bna->ioceth.attr.num_rxp; index++) {
 		rx_ptr = &rx_mod->rx[index];
 
-		bfa_q_qe_init(&rx_ptr->qe);
 		INIT_LIST_HEAD(&rx_ptr->rxp_q);
 		rx_ptr->bna = NULL;
 		rx_ptr->rid = index;
@@ -2296,7 +2072,6 @@ void bna_rx_mod_init(struct bna_rx_mod *rx_mod, struct bna *bna,
 	/* build RX-path queue */
 	for (index = 0; index < bna->ioceth.attr.num_rxp; index++) {
 		rxp_ptr = &rx_mod->rxp[index];
-		bfa_q_qe_init(&rxp_ptr->qe);
 		list_add_tail(&rxp_ptr->qe, &rx_mod->rxp_free_q);
 		rx_mod->rxp_free_count++;
 	}
@@ -2304,7 +2079,6 @@ void bna_rx_mod_init(struct bna_rx_mod *rx_mod, struct bna *bna,
 	/* build RXQ queue */
 	for (index = 0; index < (bna->ioceth.attr.num_rxp * 2); index++) {
 		rxq_ptr = &rx_mod->rxq[index];
-		bfa_q_qe_init(&rxq_ptr->qe);
 		list_add_tail(&rxq_ptr->qe, &rx_mod->rxq_free_q);
 		rx_mod->rxq_free_count++;
 	}
@@ -2313,21 +2087,6 @@ void bna_rx_mod_init(struct bna_rx_mod *rx_mod, struct bna *bna,
 void
 bna_rx_mod_uninit(struct bna_rx_mod *rx_mod)
 {
-	struct list_head		*qe;
-	int i;
-
-	i = 0;
-	list_for_each(qe, &rx_mod->rx_free_q)
-		i++;
-
-	i = 0;
-	list_for_each(qe, &rx_mod->rxp_free_q)
-		i++;
-
-	i = 0;
-	list_for_each(qe, &rx_mod->rxq_free_q)
-		i++;
-
 	rx_mod->bna = NULL;
 }
 
@@ -2337,7 +2096,6 @@ bna_bfi_rx_enet_start_rsp(struct bna_rx *rx, struct bfi_msgq_mhdr *msghdr)
 	struct bfi_enet_rx_cfg_rsp *cfg_rsp = &rx->bfi_enet_cmd.cfg_rsp;
 	struct bna_rxp *rxp = NULL;
 	struct bna_rxq *q0 = NULL, *q1 = NULL;
-	struct list_head *rxp_qe;
 	int i;
 
 	bfa_msgq_rsp_copy(&rx->bna->msgq, (u8 *)cfg_rsp,
@@ -2345,10 +2103,8 @@ bna_bfi_rx_enet_start_rsp(struct bna_rx *rx, struct bfi_msgq_mhdr *msghdr)
 
 	rx->hw_id = cfg_rsp->hw_id;
 
-	for (i = 0, rxp_qe = bfa_q_first(&rx->rxp_q);
-		i < rx->num_paths;
-		i++, rxp_qe = bfa_q_next(rxp_qe)) {
-		rxp = (struct bna_rxp *)rxp_qe;
+	for (i = 0, rxp = list_first_entry(&rx->rxp_q, struct bna_rxp, qe);
+	     i < rx->num_paths; i++, rxp = list_next_entry(rxp, qe)) {
 		GET_RXQS(rxp, q0, q1);
 
 		/* Setup doorbells */
@@ -2396,20 +2152,19 @@ bna_rx_res_req(struct bna_rx_config *q_cfg, struct bna_res_info *res_info)
 
 	dq_depth = q_cfg->q0_depth;
 	hq_depth = ((q_cfg->rxp_type == BNA_RXP_SINGLE) ? 0 : q_cfg->q1_depth);
-	cq_depth = dq_depth + hq_depth;
+	cq_depth = roundup_pow_of_two(dq_depth + hq_depth);
 
-	BNA_TO_POWER_OF_2_HIGH(cq_depth);
 	cq_size = cq_depth * BFI_CQ_WI_SIZE;
 	cq_size = ALIGN(cq_size, PAGE_SIZE);
 	cpage_count = SIZE_TO_PAGES(cq_size);
 
-	BNA_TO_POWER_OF_2_HIGH(dq_depth);
+	dq_depth = roundup_pow_of_two(dq_depth);
 	dq_size = dq_depth * BFI_RXQ_WI_SIZE;
 	dq_size = ALIGN(dq_size, PAGE_SIZE);
 	dpage_count = SIZE_TO_PAGES(dq_size);
 
 	if (BNA_RXP_SINGLE != q_cfg->rxp_type) {
-		BNA_TO_POWER_OF_2_HIGH(hq_depth);
+		hq_depth = roundup_pow_of_two(hq_depth);
 		hq_size = hq_depth * BFI_RXQ_WI_SIZE;
 		hq_size = ALIGN(hq_size, PAGE_SIZE);
 		hpage_count = SIZE_TO_PAGES(hq_size);
@@ -2620,7 +2375,7 @@ bna_rx_create(struct bna *bna, struct bnad *bnad,
 		if (intr_info->intr_type == BNA_INTR_T_MSIX)
 			rxp->cq.ib.intr_vector = rxp->vector;
 		else
-			rxp->cq.ib.intr_vector = (1 << rxp->vector);
+			rxp->cq.ib.intr_vector = BIT(rxp->vector);
 		rxp->cq.ib.coalescing_timeo = rx_cfg->coalescing_timeo;
 		rxp->cq.ib.interpkt_count = BFI_RX_INTERPKT_COUNT;
 		rxp->cq.ib.interpkt_timeo = BFI_RX_INTERPKT_TIMEO;
@@ -2691,7 +2446,7 @@ bna_rx_create(struct bna *bna, struct bnad *bnad,
 		/* if multi-buffer is enabled sum of q0_depth
 		 * and q1_depth need not be a power of 2
 		 */
-		BNA_TO_POWER_OF_2_HIGH(cq_depth);
+		cq_depth = roundup_pow_of_two(cq_depth);
 		rxp->cq.ccb->q_depth = cq_depth;
 		rxp->cq.ccb->cq = &rxp->cq;
 		rxp->cq.ccb->rcb[0] = q0->rcb;
@@ -2725,7 +2480,7 @@ bna_rx_create(struct bna *bna, struct bnad *bnad,
 
 	bfa_fsm_set_state(rx, bna_rx_sm_stopped);
 
-	rx_mod->rid_mask |= (1 << rx->rid);
+	rx_mod->rid_mask |= BIT(rx->rid);
 
 	return rx;
 }
@@ -2742,7 +2497,8 @@ bna_rx_destroy(struct bna_rx *rx)
 	bna_rxf_uninit(&rx->rxf);
 
 	while (!list_empty(&rx->rxp_q)) {
-		bfa_q_deq(&rx->rxp_q, &rxp);
+		rxp = list_first_entry(&rx->rxp_q, struct bna_rxp, qe);
+		list_del(&rxp->qe);
 		GET_RXQS(rxp, q0, q1);
 		if (rx->rcb_destroy_cbfn)
 			rx->rcb_destroy_cbfn(rx->bna->bnad, q0->rcb);
@@ -2769,15 +2525,13 @@ bna_rx_destroy(struct bna_rx *rx)
 		bna_rxp_put(rx_mod, rxp);
 	}
 
-	list_for_each(qe, &rx_mod->rx_active_q) {
+	list_for_each(qe, &rx_mod->rx_active_q)
 		if (qe == &rx->qe) {
 			list_del(&rx->qe);
-			bfa_q_qe_init(&rx->qe);
 			break;
 		}
-	}
 
-	rx_mod->rid_mask &= ~(1 << rx->rid);
+	rx_mod->rid_mask &= ~BIT(rx->rid);
 
 	rx->bna = NULL;
 	rx->priv = NULL;
@@ -2844,8 +2598,7 @@ bna_rx_vlan_strip_disable(struct bna_rx *rx)
 
 enum bna_cb_status
 bna_rx_mode_set(struct bna_rx *rx, enum bna_rxmode new_mode,
-		enum bna_rxmode bitmask,
-		void (*cbfn)(struct bnad *, struct bna_rx *))
+		enum bna_rxmode bitmask)
 {
 	struct bna_rxf *rxf = &rx->rxf;
 	int need_hw_config = 0;
@@ -2900,11 +2653,10 @@ bna_rx_mode_set(struct bna_rx *rx, enum bna_rxmode new_mode,
 	/* Trigger h/w if needed */
 
 	if (need_hw_config) {
-		rxf->cam_fltr_cbfn = cbfn;
+		rxf->cam_fltr_cbfn = NULL;
 		rxf->cam_fltr_cbarg = rx->bna->bnad;
 		bfa_fsm_send_event(rxf, RXF_E_CONFIG);
-	} else if (cbfn)
-		(*cbfn)(rx->bna->bnad, rx);
+	}
 
 	return BNA_CB_SUCCESS;
 
@@ -2928,10 +2680,8 @@ void
 bna_rx_coalescing_timeo_set(struct bna_rx *rx, int coalescing_timeo)
 {
 	struct bna_rxp *rxp;
-	struct list_head *qe;
 
-	list_for_each(qe, &rx->rxp_q) {
-		rxp = (struct bna_rxp *)qe;
+	list_for_each_entry(rxp, &rx->rxp_q, qe) {
 		rxp->cq.ccb->rx_coalescing_timeo = coalescing_timeo;
 		bna_ib_coalescing_timeo_set(&rxp->cq.ib, coalescing_timeo);
 	}
@@ -3024,16 +2774,6 @@ do {									\
 	}								\
 } while (0)
 
-#define call_tx_prio_change_cbfn(tx)					\
-do {									\
-	if ((tx)->prio_change_cbfn) {					\
-		void (*cbfn)(struct bnad *, struct bna_tx *);	\
-		cbfn = (tx)->prio_change_cbfn;				\
-		(tx)->prio_change_cbfn = NULL;				\
-		cbfn((tx)->bna->bnad, (tx));				\
-	}								\
-} while (0)
-
 static void bna_tx_mod_cb_tx_stopped(void *tx_mod, struct bna_tx *tx);
 static void bna_bfi_tx_enet_start(struct bna_tx *tx);
 static void bna_tx_enet_stop(struct bna_tx *tx);
@@ -3044,7 +2784,6 @@ enum bna_tx_event {
 	TX_E_FAIL			= 3,
 	TX_E_STARTED			= 4,
 	TX_E_STOPPED			= 5,
-	TX_E_PRIO_CHANGE		= 6,
 	TX_E_CLEANUP_DONE		= 7,
 	TX_E_BW_UPDATE			= 8,
 };
@@ -3085,10 +2824,6 @@ bna_tx_sm_stopped(struct bna_tx *tx, enum bna_tx_event event)
 		/* No-op */
 		break;
 
-	case TX_E_PRIO_CHANGE:
-		call_tx_prio_change_cbfn(tx);
-		break;
-
 	case TX_E_BW_UPDATE:
 		/* No-op */
 		break;
@@ -3109,28 +2844,23 @@ bna_tx_sm_start_wait(struct bna_tx *tx, enum bna_tx_event event)
 {
 	switch (event) {
 	case TX_E_STOP:
-		tx->flags &= ~(BNA_TX_F_PRIO_CHANGED | BNA_TX_F_BW_UPDATED);
+		tx->flags &= ~BNA_TX_F_BW_UPDATED;
 		bfa_fsm_set_state(tx, bna_tx_sm_stop_wait);
 		break;
 
 	case TX_E_FAIL:
-		tx->flags &= ~(BNA_TX_F_PRIO_CHANGED | BNA_TX_F_BW_UPDATED);
+		tx->flags &= ~BNA_TX_F_BW_UPDATED;
 		bfa_fsm_set_state(tx, bna_tx_sm_stopped);
 		break;
 
 	case TX_E_STARTED:
-		if (tx->flags & (BNA_TX_F_PRIO_CHANGED | BNA_TX_F_BW_UPDATED)) {
-			tx->flags &= ~(BNA_TX_F_PRIO_CHANGED |
-				BNA_TX_F_BW_UPDATED);
+		if (tx->flags & BNA_TX_F_BW_UPDATED) {
+			tx->flags &= ~BNA_TX_F_BW_UPDATED;
 			bfa_fsm_set_state(tx, bna_tx_sm_prio_stop_wait);
 		} else
 			bfa_fsm_set_state(tx, bna_tx_sm_started);
 		break;
 
-	case TX_E_PRIO_CHANGE:
-		tx->flags |=  BNA_TX_F_PRIO_CHANGED;
-		break;
-
 	case TX_E_BW_UPDATE:
 		tx->flags |= BNA_TX_F_BW_UPDATED;
 		break;
@@ -3144,11 +2874,9 @@ static void
 bna_tx_sm_started_entry(struct bna_tx *tx)
 {
 	struct bna_txq *txq;
-	struct list_head		 *qe;
 	int is_regular = (tx->type == BNA_TX_T_REGULAR);
 
-	list_for_each(qe, &tx->txq_q) {
-		txq = (struct bna_txq *)qe;
+	list_for_each_entry(txq, &tx->txq_q, qe) {
 		txq->tcb->priority = txq->priority;
 		/* Start IB */
 		bna_ib_start(tx->bna, &txq->ib, is_regular);
@@ -3172,7 +2900,6 @@ bna_tx_sm_started(struct bna_tx *tx, enum bna_tx_event event)
 		tx->tx_cleanup_cbfn(tx->bna->bnad, tx);
 		break;
 
-	case TX_E_PRIO_CHANGE:
 	case TX_E_BW_UPDATE:
 		bfa_fsm_set_state(tx, bna_tx_sm_prio_stop_wait);
 		break;
@@ -3205,7 +2932,6 @@ bna_tx_sm_stop_wait(struct bna_tx *tx, enum bna_tx_event event)
 		bna_tx_enet_stop(tx);
 		break;
 
-	case TX_E_PRIO_CHANGE:
 	case TX_E_BW_UPDATE:
 		/* No-op */
 		break;
@@ -3225,7 +2951,6 @@ bna_tx_sm_cleanup_wait(struct bna_tx *tx, enum bna_tx_event event)
 {
 	switch (event) {
 	case TX_E_FAIL:
-	case TX_E_PRIO_CHANGE:
 	case TX_E_BW_UPDATE:
 		/* No-op */
 		break;
@@ -3256,7 +2981,6 @@ bna_tx_sm_prio_stop_wait(struct bna_tx *tx, enum bna_tx_event event)
 
 	case TX_E_FAIL:
 		bfa_fsm_set_state(tx, bna_tx_sm_failed);
-		call_tx_prio_change_cbfn(tx);
 		tx->tx_cleanup_cbfn(tx->bna->bnad, tx);
 		break;
 
@@ -3264,7 +2988,6 @@ bna_tx_sm_prio_stop_wait(struct bna_tx *tx, enum bna_tx_event event)
 		bfa_fsm_set_state(tx, bna_tx_sm_prio_cleanup_wait);
 		break;
 
-	case TX_E_PRIO_CHANGE:
 	case TX_E_BW_UPDATE:
 		/* No-op */
 		break;
@@ -3277,7 +3000,6 @@ bna_tx_sm_prio_stop_wait(struct bna_tx *tx, enum bna_tx_event event)
 static void
 bna_tx_sm_prio_cleanup_wait_entry(struct bna_tx *tx)
 {
-	call_tx_prio_change_cbfn(tx);
 	tx->tx_cleanup_cbfn(tx->bna->bnad, tx);
 }
 
@@ -3293,7 +3015,6 @@ bna_tx_sm_prio_cleanup_wait(struct bna_tx *tx, enum bna_tx_event event)
 		bfa_fsm_set_state(tx, bna_tx_sm_failed);
 		break;
 
-	case TX_E_PRIO_CHANGE:
 	case TX_E_BW_UPDATE:
 		/* No-op */
 		break;
@@ -3372,7 +3093,6 @@ bna_bfi_tx_enet_start(struct bna_tx *tx)
 {
 	struct bfi_enet_tx_cfg_req *cfg_req = &tx->bfi_enet_cmd.cfg_req;
 	struct bna_txq *txq = NULL;
-	struct list_head *qe;
 	int i;
 
 	bfi_msgq_mhdr_set(cfg_req->mh, BFI_MC_ENET,
@@ -3381,11 +3101,9 @@ bna_bfi_tx_enet_start(struct bna_tx *tx)
 		bfi_msgq_num_cmd_entries(sizeof(struct bfi_enet_tx_cfg_req)));
 
 	cfg_req->num_queues = tx->num_txq;
-	for (i = 0, qe = bfa_q_first(&tx->txq_q);
-		i < tx->num_txq;
-		i++, qe = bfa_q_next(qe)) {
-		txq = (struct bna_txq *)qe;
-
+	for (i = 0; i < tx->num_txq; i++) {
+		txq = txq ? list_next_entry(txq, qe)
+			: list_first_entry(&tx->txq_q, struct bna_txq, qe);
 		bfi_enet_datapath_q_init(&cfg_req->q_cfg[i].q.q, &txq->qpt);
 		cfg_req->q_cfg[i].q.priority = txq->priority;
 
@@ -3437,13 +3155,10 @@ static void
 bna_tx_enet_stop(struct bna_tx *tx)
 {
 	struct bna_txq *txq;
-	struct list_head		 *qe;
 
 	/* Stop IB */
-	list_for_each(qe, &tx->txq_q) {
-		txq = (struct bna_txq *)qe;
+	list_for_each_entry(txq, &tx->txq_q, qe)
 		bna_ib_stop(tx->bna, &txq->ib);
-	}
 
 	bna_bfi_tx_enet_stop(tx);
 }
@@ -3487,18 +3202,15 @@ bna_txq_qpt_setup(struct bna_txq *txq, int page_count, int page_size,
 static struct bna_tx *
 bna_tx_get(struct bna_tx_mod *tx_mod, enum bna_tx_type type)
 {
-	struct list_head	*qe = NULL;
 	struct bna_tx *tx = NULL;
 
 	if (list_empty(&tx_mod->tx_free_q))
 		return NULL;
-	if (type == BNA_TX_T_REGULAR) {
-		bfa_q_deq(&tx_mod->tx_free_q, &qe);
-	} else {
-		bfa_q_deq_tail(&tx_mod->tx_free_q, &qe);
-	}
-	tx = (struct bna_tx *)qe;
-	bfa_q_qe_init(&tx->qe);
+	if (type == BNA_TX_T_REGULAR)
+		tx = list_first_entry(&tx_mod->tx_free_q, struct bna_tx, qe);
+	else
+		tx = list_last_entry(&tx_mod->tx_free_q, struct bna_tx, qe);
+	list_del(&tx->qe);
 	tx->type = type;
 
 	return tx;
@@ -3509,21 +3221,18 @@ bna_tx_free(struct bna_tx *tx)
 {
 	struct bna_tx_mod *tx_mod = &tx->bna->tx_mod;
 	struct bna_txq *txq;
-	struct list_head *prev_qe;
 	struct list_head *qe;
 
 	while (!list_empty(&tx->txq_q)) {
-		bfa_q_deq(&tx->txq_q, &txq);
-		bfa_q_qe_init(&txq->qe);
+		txq = list_first_entry(&tx->txq_q, struct bna_txq, qe);
 		txq->tcb = NULL;
 		txq->tx = NULL;
-		list_add_tail(&txq->qe, &tx_mod->txq_free_q);
+		list_move_tail(&txq->qe, &tx_mod->txq_free_q);
 	}
 
 	list_for_each(qe, &tx_mod->tx_active_q) {
 		if (qe == &tx->qe) {
 			list_del(&tx->qe);
-			bfa_q_qe_init(&tx->qe);
 			break;
 		}
 	}
@@ -3531,28 +3240,11 @@ bna_tx_free(struct bna_tx *tx)
 	tx->bna = NULL;
 	tx->priv = NULL;
 
-	prev_qe = NULL;
-	list_for_each(qe, &tx_mod->tx_free_q) {
+	list_for_each_prev(qe, &tx_mod->tx_free_q)
 		if (((struct bna_tx *)qe)->rid < tx->rid)
-			prev_qe = qe;
-		else {
 			break;
-		}
-	}
 
-	if (prev_qe == NULL) {
-		/* This is the first entry */
-		bfa_q_enq_head(&tx_mod->tx_free_q, &tx->qe);
-	} else if (bfa_q_next(prev_qe) == &tx_mod->tx_free_q) {
-		/* This is the last entry */
-		list_add_tail(&tx->qe, &tx_mod->tx_free_q);
-	} else {
-		/* Somewhere in the middle */
-		bfa_q_next(&tx->qe) = bfa_q_next(prev_qe);
-		bfa_q_prev(&tx->qe) = prev_qe;
-		bfa_q_next(prev_qe) = &tx->qe;
-		bfa_q_prev(bfa_q_next(&tx->qe)) = &tx->qe;
-	}
+	list_add(&tx->qe, qe);
 }
 
 static void
@@ -3585,7 +3277,6 @@ bna_bfi_tx_enet_start_rsp(struct bna_tx *tx, struct bfi_msgq_mhdr *msghdr)
 {
 	struct bfi_enet_tx_cfg_rsp *cfg_rsp = &tx->bfi_enet_cmd.cfg_rsp;
 	struct bna_txq *txq = NULL;
-	struct list_head *qe;
 	int i;
 
 	bfa_msgq_rsp_copy(&tx->bna->msgq, (u8 *)cfg_rsp,
@@ -3593,10 +3284,8 @@ bna_bfi_tx_enet_start_rsp(struct bna_tx *tx, struct bfi_msgq_mhdr *msghdr)
 
 	tx->hw_id = cfg_rsp->hw_id;
 
-	for (i = 0, qe = bfa_q_first(&tx->txq_q);
-		i < tx->num_txq; i++, qe = bfa_q_next(qe)) {
-		txq = (struct bna_txq *)qe;
-
+	for (i = 0, txq = list_first_entry(&tx->txq_q, struct bna_txq, qe);
+	     i < tx->num_txq; i++, txq = list_next_entry(txq, qe)) {
 		/* Setup doorbells */
 		txq->tcb->i_dbell->doorbell_addr =
 			tx->bna->pcidev.pci_bar_kva
@@ -3624,12 +3313,9 @@ void
 bna_bfi_bw_update_aen(struct bna_tx_mod *tx_mod)
 {
 	struct bna_tx *tx;
-	struct list_head		*qe;
 
-	list_for_each(qe, &tx_mod->tx_active_q) {
-		tx = (struct bna_tx *)qe;
+	list_for_each_entry(tx, &tx_mod->tx_active_q, qe)
 		bfa_fsm_send_event(tx, TX_E_BW_UPDATE);
-	}
 }
 
 void
@@ -3689,7 +3375,6 @@ bna_tx_create(struct bna *bna, struct bnad *bnad,
 	struct bna_tx_mod *tx_mod = &bna->tx_mod;
 	struct bna_tx *tx;
 	struct bna_txq *txq;
-	struct list_head *qe;
 	int page_count;
 	int i;
 
@@ -3719,9 +3404,8 @@ bna_tx_create(struct bna *bna, struct bnad *bnad,
 		if (list_empty(&tx_mod->txq_free_q))
 			goto err_return;
 
-		bfa_q_deq(&tx_mod->txq_free_q, &txq);
-		bfa_q_qe_init(&txq->qe);
-		list_add_tail(&txq->qe, &tx->txq_q);
+		txq = list_first_entry(&tx_mod->txq_free_q, struct bna_txq, qe);
+		list_move_tail(&txq->qe, &tx->txq_q);
 		txq->tx = tx;
 	}
 
@@ -3760,8 +3444,7 @@ bna_tx_create(struct bna *bna, struct bnad *bnad,
 	/* TxQ */
 
 	i = 0;
-	list_for_each(qe, &tx->txq_q) {
-		txq = (struct bna_txq *)qe;
+	list_for_each_entry(txq, &tx->txq_q, qe) {
 		txq->tcb = (struct bna_tcb *)
 		res_info[BNA_TX_RES_MEM_T_TCB].res_u.mem_info.mdl[i].kva;
 		txq->tx_packets = 0;
@@ -3779,7 +3462,7 @@ bna_tx_create(struct bna *bna, struct bnad *bnad,
 					intr_info->idl[0].vector :
 					intr_info->idl[i].vector;
 		if (intr_info->intr_type == BNA_INTR_T_INTX)
-			txq->ib.intr_vector = (1 <<  txq->ib.intr_vector);
+			txq->ib.intr_vector = BIT(txq->ib.intr_vector);
 		txq->ib.coalescing_timeo = tx_cfg->coalescing_timeo;
 		txq->ib.interpkt_timeo = BFI_TX_INTERPKT_TIMEO;
 		txq->ib.interpkt_count = BFI_TX_INTERPKT_COUNT;
@@ -3821,7 +3504,7 @@ bna_tx_create(struct bna *bna, struct bnad *bnad,
 
 	bfa_fsm_set_state(tx, bna_tx_sm_stopped);
 
-	tx_mod->rid_mask |= (1 << tx->rid);
+	tx_mod->rid_mask |= BIT(tx->rid);
 
 	return tx;
 
@@ -3834,15 +3517,12 @@ void
 bna_tx_destroy(struct bna_tx *tx)
 {
 	struct bna_txq *txq;
-	struct list_head *qe;
 
-	list_for_each(qe, &tx->txq_q) {
-		txq = (struct bna_txq *)qe;
+	list_for_each_entry(txq, &tx->txq_q, qe)
 		if (tx->tcb_destroy_cbfn)
 			(tx->tcb_destroy_cbfn)(tx->bna->bnad, txq->tcb);
-	}
 
-	tx->bna->tx_mod.rid_mask &= ~(1 << tx->rid);
+	tx->bna->tx_mod.rid_mask &= ~BIT(tx->rid);
 	bna_tx_free(tx);
 }
 
@@ -3920,9 +3600,7 @@ bna_tx_mod_init(struct bna_tx_mod *tx_mod, struct bna *bna,
 
 	for (i = 0; i < bna->ioceth.attr.num_txq; i++) {
 		tx_mod->tx[i].rid = i;
-		bfa_q_qe_init(&tx_mod->tx[i].qe);
 		list_add_tail(&tx_mod->tx[i].qe, &tx_mod->tx_free_q);
-		bfa_q_qe_init(&tx_mod->txq[i].qe);
 		list_add_tail(&tx_mod->txq[i].qe, &tx_mod->txq_free_q);
 	}
 
@@ -3935,17 +3613,6 @@ bna_tx_mod_init(struct bna_tx_mod *tx_mod, struct bna *bna,
 void
 bna_tx_mod_uninit(struct bna_tx_mod *tx_mod)
 {
-	struct list_head		*qe;
-	int i;
-
-	i = 0;
-	list_for_each(qe, &tx_mod->tx_free_q)
-		i++;
-
-	i = 0;
-	list_for_each(qe, &tx_mod->txq_free_q)
-		i++;
-
 	tx_mod->bna = NULL;
 }
 
@@ -3953,24 +3620,20 @@ void
 bna_tx_mod_start(struct bna_tx_mod *tx_mod, enum bna_tx_type type)
 {
 	struct bna_tx *tx;
-	struct list_head		*qe;
 
 	tx_mod->flags |= BNA_TX_MOD_F_ENET_STARTED;
 	if (type == BNA_TX_T_LOOPBACK)
 		tx_mod->flags |= BNA_TX_MOD_F_ENET_LOOPBACK;
 
-	list_for_each(qe, &tx_mod->tx_active_q) {
-		tx = (struct bna_tx *)qe;
+	list_for_each_entry(tx, &tx_mod->tx_active_q, qe)
 		if (tx->type == type)
 			bna_tx_start(tx);
-	}
 }
 
 void
 bna_tx_mod_stop(struct bna_tx_mod *tx_mod, enum bna_tx_type type)
 {
 	struct bna_tx *tx;
-	struct list_head		*qe;
 
 	tx_mod->flags &= ~BNA_TX_MOD_F_ENET_STARTED;
 	tx_mod->flags &= ~BNA_TX_MOD_F_ENET_LOOPBACK;
@@ -3979,13 +3642,11 @@ bna_tx_mod_stop(struct bna_tx_mod *tx_mod, enum bna_tx_type type)
 
 	bfa_wc_init(&tx_mod->tx_stop_wc, bna_tx_mod_cb_tx_stopped_all, tx_mod);
 
-	list_for_each(qe, &tx_mod->tx_active_q) {
-		tx = (struct bna_tx *)qe;
+	list_for_each_entry(tx, &tx_mod->tx_active_q, qe)
 		if (tx->type == type) {
 			bfa_wc_up(&tx_mod->tx_stop_wc);
 			bna_tx_stop(tx);
 		}
-	}
 
 	bfa_wc_wait(&tx_mod->tx_stop_wc);
 }
@@ -3994,25 +3655,19 @@ void
 bna_tx_mod_fail(struct bna_tx_mod *tx_mod)
 {
 	struct bna_tx *tx;
-	struct list_head		*qe;
 
 	tx_mod->flags &= ~BNA_TX_MOD_F_ENET_STARTED;
 	tx_mod->flags &= ~BNA_TX_MOD_F_ENET_LOOPBACK;
 
-	list_for_each(qe, &tx_mod->tx_active_q) {
-		tx = (struct bna_tx *)qe;
+	list_for_each_entry(tx, &tx_mod->tx_active_q, qe)
 		bna_tx_fail(tx);
-	}
 }
 
 void
 bna_tx_coalescing_timeo_set(struct bna_tx *tx, int coalescing_timeo)
 {
 	struct bna_txq *txq;
-	struct list_head *qe;
 
-	list_for_each(qe, &tx->txq_q) {
-		txq = (struct bna_txq *)qe;
+	list_for_each_entry(txq, &tx->txq_q, qe)
 		bna_ib_coalescing_timeo_set(&txq->ib, coalescing_timeo);
-	}
 }
diff --git a/drivers/net/ethernet/brocade/bna/bna_types.h b/drivers/net/ethernet/brocade/bna/bna_types.h
index d0a7a566f5d6..e0e797f2ea14 100644
--- a/drivers/net/ethernet/brocade/bna/bna_types.h
+++ b/drivers/net/ethernet/brocade/bna/bna_types.h
@@ -135,7 +135,6 @@ enum bna_tx_type {
 enum bna_tx_flags {
 	BNA_TX_F_ENET_STARTED	= 1,
 	BNA_TX_F_ENABLED	= 2,
-	BNA_TX_F_PRIO_CHANGED	= 4,
 	BNA_TX_F_BW_UPDATED	= 8,
 };
 
@@ -182,17 +181,11 @@ enum bna_rx_mod_flags {
 	BNA_RX_MOD_F_ENET_LOOPBACK	= 2,
 };
 
-enum bna_rxf_flags {
-	BNA_RXF_F_PAUSED		= 1,
-};
-
 enum bna_rxf_event {
 	RXF_E_START			= 1,
 	RXF_E_STOP			= 2,
 	RXF_E_FAIL			= 3,
 	RXF_E_CONFIG			= 4,
-	RXF_E_PAUSE			= 5,
-	RXF_E_RESUME			= 6,
 	RXF_E_FW_RESP			= 7,
 };
 
@@ -362,9 +355,6 @@ struct bna_enet {
 	void (*stop_cbfn)(void *);
 	void			*stop_cbarg;
 
-	/* Callback for bna_enet_pause_config() */
-	void (*pause_cbfn)(struct bnad *);
-
 	/* Callback for bna_enet_mtu_set() */
 	void (*mtu_cbfn)(struct bnad *);
 
@@ -498,9 +488,6 @@ struct bna_tx {
 	void (*stop_cbfn)(void *arg, struct bna_tx *tx);
 	void			*stop_cbarg;
 
-	/* callback for bna_tx_prio_set() */
-	void (*prio_change_cbfn)(struct bnad *bnad, struct bna_tx *tx);
-
 	struct bfa_msgq_cmd_entry msgq_cmd;
 	union {
 		struct bfi_enet_tx_cfg_req	cfg_req;
@@ -676,7 +663,6 @@ struct bna_rx_config {
 	enum bna_rx_type rx_type;
 	int			num_paths;
 	enum bna_rxp_type rxp_type;
-	int			paused;
 	int			coalescing_timeo;
 	/*
 	 * Small/Large (or Header/Data) buffer size to be configured
@@ -721,7 +707,6 @@ struct bna_rxp {
 /* RxF structure (hardware Rx Function) */
 struct bna_rxf {
 	bfa_fsm_t		fsm;
-	enum bna_rxf_flags flags;
 
 	struct bfa_msgq_cmd_entry msgq_cmd;
 	union {
@@ -742,10 +727,6 @@ struct bna_rxf {
 	void (*stop_cbfn) (struct bna_rx *rx);
 	struct bna_rx *stop_cbarg;
 
-	/* callback for bna_rx_receive_pause() / bna_rx_receive_resume() */
-	void (*oper_state_cbfn) (struct bnad *bnad, struct bna_rx *rx);
-	struct bnad *oper_state_cbarg;
-
 	/**
 	 * callback for:
 	 *	bna_rxf_ucast_set()
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index 37072a83f9d6..0612b19f6313 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -57,7 +57,8 @@ static u32 bnad_rxqs_per_cq = 2;
 static u32 bna_id;
 static struct mutex bnad_list_mutex;
 static LIST_HEAD(bnad_list);
-static const u8 bnad_bcast_addr[] =  {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+static const u8 bnad_bcast_addr[] __aligned(2) =
+	{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 
 /*
  * Local MACROS
@@ -308,7 +309,7 @@ bnad_rxq_alloc_init(struct bnad *bnad, struct bna_rcb *rcb)
 		}
 	}
 
-	BUG_ON(((PAGE_SIZE << order) % unmap_q->map_size));
+	BUG_ON((PAGE_SIZE << order) % unmap_q->map_size);
 
 	return 0;
 }
@@ -724,7 +725,6 @@ next:
 			cmpl->valid = 0;
 			BNA_QE_INDX_INC(ccb->producer_index, ccb->q_depth);
 		}
-		cmpl = &cq[ccb->producer_index];
 	}
 
 	napi_gro_flush(&rx_ctrl->napi, false);
@@ -757,7 +757,7 @@ bnad_msix_rx(int irq, void *data)
 	struct bna_ccb *ccb = (struct bna_ccb *)data;
 
 	if (ccb) {
-		((struct bnad_rx_ctrl *)(ccb->ctrl))->rx_intr_ctr++;
+		((struct bnad_rx_ctrl *)ccb->ctrl)->rx_intr_ctr++;
 		bnad_netif_rx_schedule_poll(ccb->bnad, ccb);
 	}
 
@@ -875,9 +875,9 @@ bnad_set_netdev_perm_addr(struct bnad *bnad)
 {
 	struct net_device *netdev = bnad->netdev;
 
-	memcpy(netdev->perm_addr, &bnad->perm_addr, netdev->addr_len);
+	ether_addr_copy(netdev->perm_addr, bnad->perm_addr);
 	if (is_zero_ether_addr(netdev->dev_addr))
-		memcpy(netdev->dev_addr, &bnad->perm_addr, netdev->addr_len);
+		ether_addr_copy(netdev->dev_addr, bnad->perm_addr);
 }
 
 /* Control Path Handlers */
@@ -946,8 +946,7 @@ bnad_cb_ethport_link_status(struct bnad *bnad,
 	if (link_up) {
 		if (!netif_carrier_ok(bnad->netdev)) {
 			uint tx_id, tcb_id;
-			printk(KERN_WARNING "bna: %s link up\n",
-				bnad->netdev->name);
+			netdev_info(bnad->netdev, "link up\n");
 			netif_carrier_on(bnad->netdev);
 			BNAD_UPDATE_CTR(bnad, link_toggle);
 			for (tx_id = 0; tx_id < bnad->num_tx; tx_id++) {
@@ -966,10 +965,6 @@ bnad_cb_ethport_link_status(struct bnad *bnad,
 						/*
 						 * Force an immediate
 						 * Transmit Schedule */
-						printk(KERN_INFO "bna: %s %d "
-						      "TXQ_STARTED\n",
-						       bnad->netdev->name,
-						       txq_id);
 						netif_wake_subqueue(
 								bnad->netdev,
 								txq_id);
@@ -987,8 +982,7 @@ bnad_cb_ethport_link_status(struct bnad *bnad,
 		}
 	} else {
 		if (netif_carrier_ok(bnad->netdev)) {
-			printk(KERN_WARNING "bna: %s link down\n",
-				bnad->netdev->name);
+			netdev_info(bnad->netdev, "link down\n");
 			netif_carrier_off(bnad->netdev);
 			BNAD_UPDATE_CTR(bnad, link_toggle);
 		}
@@ -1058,8 +1052,6 @@ bnad_cb_tx_stall(struct bnad *bnad, struct bna_tx *tx)
 		txq_id = tcb->id;
 		clear_bit(BNAD_TXQ_TX_STARTED, &tcb->flags);
 		netif_stop_subqueue(bnad->netdev, txq_id);
-		printk(KERN_INFO "bna: %s %d TXQ_STOPPED\n",
-			bnad->netdev->name, txq_id);
 	}
 }
 
@@ -1082,8 +1074,6 @@ bnad_cb_tx_resume(struct bnad *bnad, struct bna_tx *tx)
 		BUG_ON(*(tcb->hw_consumer_index) != 0);
 
 		if (netif_carrier_ok(bnad->netdev)) {
-			printk(KERN_INFO "bna: %s %d TXQ_STARTED\n",
-				bnad->netdev->name, txq_id);
 			netif_wake_subqueue(bnad->netdev, txq_id);
 			BNAD_UPDATE_CTR(bnad, netif_queue_wakeup);
 		}
@@ -1094,8 +1084,8 @@ bnad_cb_tx_resume(struct bnad *bnad, struct bna_tx *tx)
 	 * get a 0 MAC address. We try to get the MAC address
 	 * again here.
 	 */
-	if (is_zero_ether_addr(&bnad->perm_addr.mac[0])) {
-		bna_enet_perm_mac_get(&bnad->bna.enet, &bnad->perm_addr);
+	if (is_zero_ether_addr(bnad->perm_addr)) {
+		bna_enet_perm_mac_get(&bnad->bna.enet, bnad->perm_addr);
 		bnad_set_netdev_perm_addr(bnad);
 	}
 }
@@ -1703,7 +1693,7 @@ bnad_ioc_timeout(unsigned long data)
 	unsigned long flags;
 
 	spin_lock_irqsave(&bnad->bna_lock, flags);
-	bfa_nw_ioc_timeout((void *) &bnad->bna.ioceth.ioc);
+	bfa_nw_ioc_timeout(&bnad->bna.ioceth.ioc);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 }
 
@@ -1714,7 +1704,7 @@ bnad_ioc_hb_check(unsigned long data)
 	unsigned long flags;
 
 	spin_lock_irqsave(&bnad->bna_lock, flags);
-	bfa_nw_ioc_hb_check((void *) &bnad->bna.ioceth.ioc);
+	bfa_nw_ioc_hb_check(&bnad->bna.ioceth.ioc);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 }
 
@@ -1725,7 +1715,7 @@ bnad_iocpf_timeout(unsigned long data)
 	unsigned long flags;
 
 	spin_lock_irqsave(&bnad->bna_lock, flags);
-	bfa_nw_iocpf_timeout((void *) &bnad->bna.ioceth.ioc);
+	bfa_nw_iocpf_timeout(&bnad->bna.ioceth.ioc);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 }
 
@@ -1736,7 +1726,7 @@ bnad_iocpf_sem_timeout(unsigned long data)
 	unsigned long flags;
 
 	spin_lock_irqsave(&bnad->bna_lock, flags);
-	bfa_nw_iocpf_sem_timeout((void *) &bnad->bna.ioceth.ioc);
+	bfa_nw_iocpf_sem_timeout(&bnad->bna.ioceth.ioc);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 }
 
@@ -1862,8 +1852,7 @@ bnad_netdev_mc_list_get(struct net_device *netdev, u8 *mc_list)
 	struct netdev_hw_addr *mc_addr;
 
 	netdev_for_each_mc_addr(mc_addr, netdev) {
-		memcpy(&mc_list[i * ETH_ALEN], &mc_addr->addr[0],
-							ETH_ALEN);
+		ether_addr_copy(&mc_list[i * ETH_ALEN], &mc_addr->addr[0]);
 		i++;
 	}
 }
@@ -2137,7 +2126,7 @@ bnad_reinit_rx(struct bnad *bnad)
 		current_err = bnad_setup_rx(bnad, rx_id);
 		if (current_err && !err) {
 			err = current_err;
-			pr_err("RXQ:%u setup failed\n", rx_id);
+			netdev_err(netdev, "RXQ:%u setup failed\n", rx_id);
 		}
 	}
 
@@ -2338,7 +2327,7 @@ bnad_rx_coalescing_timeo_set(struct bnad *bnad)
  * Called with bnad->bna_lock held
  */
 int
-bnad_mac_addr_set_locked(struct bnad *bnad, u8 *mac_addr)
+bnad_mac_addr_set_locked(struct bnad *bnad, const u8 *mac_addr)
 {
 	int ret;
 
@@ -2349,7 +2338,7 @@ bnad_mac_addr_set_locked(struct bnad *bnad, u8 *mac_addr)
 	if (!bnad->rx_info[0].rx)
 		return 0;
 
-	ret = bna_rx_ucast_set(bnad->rx_info[0].rx, mac_addr, NULL);
+	ret = bna_rx_ucast_set(bnad->rx_info[0].rx, mac_addr);
 	if (ret != BNA_CB_SUCCESS)
 		return -EADDRNOTAVAIL;
 
@@ -2367,8 +2356,8 @@ bnad_enable_default_bcast(struct bnad *bnad)
 	init_completion(&bnad->bnad_completions.mcast_comp);
 
 	spin_lock_irqsave(&bnad->bna_lock, flags);
-	ret = bna_rx_mcast_add(rx_info->rx, (u8 *)bnad_bcast_addr,
-				bnad_cb_rx_mcast_add);
+	ret = bna_rx_mcast_add(rx_info->rx, bnad_bcast_addr,
+			       bnad_cb_rx_mcast_add);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 
 	if (ret == BNA_CB_SUCCESS)
@@ -2673,8 +2662,9 @@ bnad_enable_msix(struct bnad *bnad)
 	if (ret < 0) {
 		goto intx_mode;
 	} else if (ret < bnad->msix_num) {
-		pr_warn("BNA: %d MSI-X vectors allocated < %d requested\n",
-			ret, bnad->msix_num);
+		dev_warn(&bnad->pcidev->dev,
+			 "%d MSI-X vectors allocated < %d requested\n",
+			 ret, bnad->msix_num);
 
 		spin_lock_irqsave(&bnad->bna_lock, flags);
 		/* ret = #of vectors that we got */
@@ -2696,7 +2686,8 @@ bnad_enable_msix(struct bnad *bnad)
 	return;
 
 intx_mode:
-	pr_warn("BNA: MSI-X enable failed - operating in INTx mode\n");
+	dev_warn(&bnad->pcidev->dev,
+		 "MSI-X enable failed - operating in INTx mode\n");
 
 	kfree(bnad->msix_table);
 	bnad->msix_table = NULL;
@@ -2754,7 +2745,7 @@ bnad_open(struct net_device *netdev)
 	spin_lock_irqsave(&bnad->bna_lock, flags);
 	bna_enet_mtu_set(&bnad->bna.enet,
 			 BNAD_FRAME_SIZE(bnad->netdev->mtu), NULL);
-	bna_enet_pause_config(&bnad->bna.enet, &pause_config, NULL);
+	bna_enet_pause_config(&bnad->bna.enet, &pause_config);
 	bna_enet_enable(&bnad->bna.enet);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 
@@ -3128,7 +3119,7 @@ bnad_set_rx_ucast_fltr(struct bnad *bnad)
 	int entry;
 
 	if (netdev_uc_empty(bnad->netdev)) {
-		bna_rx_ucast_listset(bnad->rx_info[0].rx, 0, NULL, NULL);
+		bna_rx_ucast_listset(bnad->rx_info[0].rx, 0, NULL);
 		return;
 	}
 
@@ -3141,13 +3132,11 @@ bnad_set_rx_ucast_fltr(struct bnad *bnad)
 
 	entry = 0;
 	netdev_for_each_uc_addr(ha, netdev) {
-		memcpy(&mac_list[entry * ETH_ALEN],
-		       &ha->addr[0], ETH_ALEN);
+		ether_addr_copy(&mac_list[entry * ETH_ALEN], &ha->addr[0]);
 		entry++;
 	}
 
-	ret = bna_rx_ucast_listset(bnad->rx_info[0].rx, entry,
-			mac_list, NULL);
+	ret = bna_rx_ucast_listset(bnad->rx_info[0].rx, entry, mac_list);
 	kfree(mac_list);
 
 	if (ret != BNA_CB_SUCCESS)
@@ -3158,7 +3147,7 @@ bnad_set_rx_ucast_fltr(struct bnad *bnad)
 	/* ucast packets not in UCAM are routed to default function */
 mode_default:
 	bnad->cfg_flags |= BNAD_CF_DEFAULT;
-	bna_rx_ucast_listset(bnad->rx_info[0].rx, 0, NULL, NULL);
+	bna_rx_ucast_listset(bnad->rx_info[0].rx, 0, NULL);
 }
 
 static void
@@ -3183,12 +3172,11 @@ bnad_set_rx_mcast_fltr(struct bnad *bnad)
 	if (mac_list == NULL)
 		goto mode_allmulti;
 
-	memcpy(&mac_list[0], &bnad_bcast_addr[0], ETH_ALEN);
+	ether_addr_copy(&mac_list[0], &bnad_bcast_addr[0]);
 
 	/* copy rest of the MCAST addresses */
 	bnad_netdev_mc_list_get(netdev, mac_list);
-	ret = bna_rx_mcast_listset(bnad->rx_info[0].rx, mc_count + 1,
-			mac_list, NULL);
+	ret = bna_rx_mcast_listset(bnad->rx_info[0].rx, mc_count + 1, mac_list);
 	kfree(mac_list);
 
 	if (ret != BNA_CB_SUCCESS)
@@ -3198,7 +3186,7 @@ bnad_set_rx_mcast_fltr(struct bnad *bnad)
 
 mode_allmulti:
 	bnad->cfg_flags |= BNAD_CF_ALLMULTI;
-	bna_rx_mcast_delall(bnad->rx_info[0].rx, NULL);
+	bna_rx_mcast_delall(bnad->rx_info[0].rx);
 }
 
 void
@@ -3237,7 +3225,7 @@ bnad_set_rx_mode(struct net_device *netdev)
 
 	mode_mask = BNA_RXMODE_PROMISC | BNA_RXMODE_DEFAULT |
 			BNA_RXMODE_ALLMULTI;
-	bna_rx_mode_set(bnad->rx_info[0].rx, new_mode, mode_mask, NULL);
+	bna_rx_mode_set(bnad->rx_info[0].rx, new_mode, mode_mask);
 
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 }
@@ -3248,19 +3236,18 @@ bnad_set_rx_mode(struct net_device *netdev)
  * in a non-blocking context.
  */
 static int
-bnad_set_mac_address(struct net_device *netdev, void *mac_addr)
+bnad_set_mac_address(struct net_device *netdev, void *addr)
 {
 	int err;
 	struct bnad *bnad = netdev_priv(netdev);
-	struct sockaddr *sa = (struct sockaddr *)mac_addr;
+	struct sockaddr *sa = (struct sockaddr *)addr;
 	unsigned long flags;
 
 	spin_lock_irqsave(&bnad->bna_lock, flags);
 
 	err = bnad_mac_addr_set_locked(bnad, sa->sa_data);
-
 	if (!err)
-		memcpy(netdev->dev_addr, sa->sa_data, netdev->addr_len);
+		ether_addr_copy(netdev->dev_addr, sa->sa_data);
 
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 
@@ -3487,8 +3474,8 @@ bnad_init(struct bnad *bnad,
 		dev_err(&pdev->dev, "ioremap for bar0 failed\n");
 		return -ENOMEM;
 	}
-	pr_info("bar0 mapped to %p, len %llu\n", bnad->bar0,
-	       (unsigned long long) bnad->mmio_len);
+	dev_info(&pdev->dev, "bar0 mapped to %p, len %llu\n", bnad->bar0,
+		 (unsigned long long) bnad->mmio_len);
 
 	spin_lock_irqsave(&bnad->bna_lock, flags);
 	if (!bnad_msix_disable)
@@ -3609,13 +3596,10 @@ bnad_pci_probe(struct pci_dev *pdev,
 	struct bfa_pcidev pcidev_info;
 	unsigned long flags;
 
-	pr_info("bnad_pci_probe : (0x%p, 0x%p) PCI Func : (%d)\n",
-	       pdev, pcidev_id, PCI_FUNC(pdev->devfn));
-
 	mutex_lock(&bnad_fwimg_mutex);
 	if (!cna_get_firmware_buf(pdev)) {
 		mutex_unlock(&bnad_fwimg_mutex);
-		pr_warn("Failed to load Firmware Image!\n");
+		dev_err(&pdev->dev, "failed to load firmware image!\n");
 		return -ENODEV;
 	}
 	mutex_unlock(&bnad_fwimg_mutex);
@@ -3693,17 +3677,13 @@ bnad_pci_probe(struct pci_dev *pdev,
 
 	/* Set up timers */
 	setup_timer(&bnad->bna.ioceth.ioc.ioc_timer, bnad_ioc_timeout,
-				((unsigned long)bnad));
+		    (unsigned long)bnad);
 	setup_timer(&bnad->bna.ioceth.ioc.hb_timer, bnad_ioc_hb_check,
-				((unsigned long)bnad));
+		    (unsigned long)bnad);
 	setup_timer(&bnad->bna.ioceth.ioc.iocpf_timer, bnad_iocpf_timeout,
-				((unsigned long)bnad));
+		    (unsigned long)bnad);
 	setup_timer(&bnad->bna.ioceth.ioc.sem_timer, bnad_iocpf_sem_timeout,
-				((unsigned long)bnad));
-
-	/* Now start the timer before calling IOC */
-	mod_timer(&bnad->bna.ioceth.ioc.iocpf_timer,
-		  jiffies + msecs_to_jiffies(BNA_IOC_TIMER_FREQ));
+		    (unsigned long)bnad);
 
 	/*
 	 * Start the chip
@@ -3712,8 +3692,7 @@ bnad_pci_probe(struct pci_dev *pdev,
 	 */
 	err = bnad_ioceth_enable(bnad);
 	if (err) {
-		pr_err("BNA: Initialization failed err=%d\n",
-		       err);
+		dev_err(&pdev->dev, "initialization failed err=%d\n", err);
 		goto probe_success;
 	}
 
@@ -3746,7 +3725,7 @@ bnad_pci_probe(struct pci_dev *pdev,
 
 	/* Get the burnt-in mac */
 	spin_lock_irqsave(&bnad->bna_lock, flags);
-	bna_enet_perm_mac_get(&bna->enet, &bnad->perm_addr);
+	bna_enet_perm_mac_get(&bna->enet, bnad->perm_addr);
 	bnad_set_netdev_perm_addr(bnad);
 	spin_unlock_irqrestore(&bnad->bna_lock, flags);
 
@@ -3755,7 +3734,7 @@ bnad_pci_probe(struct pci_dev *pdev,
 	/* Finally, reguister with net_device layer */
 	err = register_netdev(netdev);
 	if (err) {
-		pr_err("BNA : Registering with netdev failed\n");
+		dev_err(&pdev->dev, "registering net device failed\n");
 		goto probe_uninit;
 	}
 	set_bit(BNAD_RF_NETDEV_REGISTERED, &bnad->run_flags);
@@ -3807,7 +3786,6 @@ bnad_pci_remove(struct pci_dev *pdev)
 	if (!netdev)
 		return;
 
-	pr_info("%s bnad_pci_remove\n", netdev->name);
 	bnad = netdev_priv(netdev);
 	bna = &bnad->bna;
 
@@ -3868,15 +3846,14 @@ bnad_module_init(void)
 {
 	int err;
 
-	pr_info("QLogic BR-series 10G Ethernet driver - version: %s\n",
-			BNAD_VERSION);
+	pr_info("bna: QLogic BR-series 10G Ethernet driver - version: %s\n",
+		BNAD_VERSION);
 
 	bfa_nw_ioc_auto_recover(bnad_ioc_auto_recover);
 
 	err = pci_register_driver(&bnad_pci_driver);
 	if (err < 0) {
-		pr_err("bna : PCI registration failed in module init "
-		       "(%d)\n", err);
+		pr_err("bna: PCI driver registration failed err=%d\n", err);
 		return err;
 	}
 
diff --git a/drivers/net/ethernet/brocade/bna/bnad.h b/drivers/net/ethernet/brocade/bna/bnad.h
index 7ead6c23edb6..faedbf24777e 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.h
+++ b/drivers/net/ethernet/brocade/bna/bnad.h
@@ -344,7 +344,7 @@ struct bnad {
 	struct bnad_completion bnad_completions;
 
 	/* Burnt in MAC address */
-	mac_t			perm_addr;
+	u8			perm_addr[ETH_ALEN];
 
 	struct workqueue_struct *work_q;
 
@@ -385,7 +385,7 @@ u32 *cna_get_firmware_buf(struct pci_dev *pdev);
 /* Netdev entry point prototypes */
 void bnad_set_rx_mode(struct net_device *netdev);
 struct net_device_stats *bnad_get_netdev_stats(struct net_device *netdev);
-int bnad_mac_addr_set_locked(struct bnad *bnad, u8 *mac_addr);
+int bnad_mac_addr_set_locked(struct bnad *bnad, const u8 *mac_addr);
 int bnad_enable_default_bcast(struct bnad *bnad);
 void bnad_restore_vlans(struct bnad *bnad, u32 rx_id);
 void bnad_set_ethtool_ops(struct net_device *netdev);
diff --git a/drivers/net/ethernet/brocade/bna/bnad_debugfs.c b/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
index 72c89550417c..8fc246ea1fb8 100644
--- a/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
+++ b/drivers/net/ethernet/brocade/bna/bnad_debugfs.c
@@ -76,8 +76,7 @@ bnad_debugfs_open_fwtrc(struct inode *inode, struct file *file)
 		fw_debug->debug_buffer = NULL;
 		kfree(fw_debug);
 		fw_debug = NULL;
-		pr_warn("bnad %s: Failed to collect fwtrc\n",
-			pci_name(bnad->pcidev));
+		netdev_warn(bnad->netdev, "failed to collect fwtrc\n");
 		return -ENOMEM;
 	}
 
@@ -117,8 +116,7 @@ bnad_debugfs_open_fwsave(struct inode *inode, struct file *file)
 		fw_debug->debug_buffer = NULL;
 		kfree(fw_debug);
 		fw_debug = NULL;
-		pr_warn("bna %s: Failed to collect fwsave\n",
-			pci_name(bnad->pcidev));
+		netdev_warn(bnad->netdev, "failed to collect fwsave\n");
 		return -ENOMEM;
 	}
 
@@ -217,8 +215,7 @@ bnad_debugfs_open_drvinfo(struct inode *inode, struct file *file)
 		drv_info->debug_buffer = NULL;
 		kfree(drv_info);
 		drv_info = NULL;
-		pr_warn("bna %s: Failed to collect drvinfo\n",
-			pci_name(bnad->pcidev));
+		netdev_warn(bnad->netdev, "failed to collect drvinfo\n");
 		return -ENOMEM;
 	}
 
@@ -271,15 +268,15 @@ bna_reg_offset_check(struct bfa_ioc *ioc, u32 offset, u32 len)
 	area = (offset >> 15) & 0x7;
 	if (area == 0) {
 		/* PCIe core register */
-		if ((offset + (len<<2)) > 0x8000)	/* 8k dwords or 32KB */
+		if (offset + (len << 2) > 0x8000)	/* 8k dwords or 32KB */
 			return BFA_STATUS_EINVAL;
 	} else if (area == 0x1) {
 		/* CB 32 KB memory page */
-		if ((offset + (len<<2)) > 0x10000)	/* 8k dwords or 32KB */
+		if (offset + (len << 2) > 0x10000)	/* 8k dwords or 32KB */
 			return BFA_STATUS_EINVAL;
 	} else {
 		/* CB register space 64KB */
-		if ((offset + (len<<2)) > BFA_REG_ADDRMSK(ioc))
+		if (offset + (len << 2) > BFA_REG_ADDRMSK(ioc))
 			return BFA_STATUS_EINVAL;
 	}
 	return BFA_STATUS_OK;
@@ -321,27 +318,20 @@ bnad_debugfs_write_regrd(struct file *file, const char __user *buf,
 	unsigned long flags;
 	void *kern_buf;
 
-	/* Allocate memory to store the user space buf */
-	kern_buf = kzalloc(nbytes, GFP_KERNEL);
-	if (!kern_buf)
-		return -ENOMEM;
-
-	if (copy_from_user(kern_buf, (void  __user *)buf, nbytes)) {
-		kfree(kern_buf);
-		return -ENOMEM;
-	}
+	/* Copy the user space buf */
+	kern_buf = memdup_user(buf, nbytes);
+	if (IS_ERR(kern_buf))
+		return PTR_ERR(kern_buf);
 
 	rc = sscanf(kern_buf, "%x:%x", &addr, &len);
 	if (rc < 2) {
-		pr_warn("bna %s: Failed to read user buffer\n",
-			pci_name(bnad->pcidev));
+		netdev_warn(bnad->netdev, "failed to read user buffer\n");
 		kfree(kern_buf);
 		return -EINVAL;
 	}
 
 	kfree(kern_buf);
 	kfree(bnad->regdata);
-	bnad->regdata = NULL;
 	bnad->reglen = 0;
 
 	bnad->regdata = kzalloc(len << 2, GFP_KERNEL);
@@ -355,8 +345,7 @@ bnad_debugfs_write_regrd(struct file *file, const char __user *buf,
 	/* offset and len sanity check */
 	rc = bna_reg_offset_check(ioc, addr, len);
 	if (rc) {
-		pr_warn("bna %s: Failed reg offset check\n",
-			pci_name(bnad->pcidev));
+		netdev_warn(bnad->netdev, "failed reg offset check\n");
 		kfree(bnad->regdata);
 		bnad->regdata = NULL;
 		bnad->reglen = 0;
@@ -388,20 +377,14 @@ bnad_debugfs_write_regwr(struct file *file, const char __user *buf,
 	unsigned long flags;
 	void *kern_buf;
 
-	/* Allocate memory to store the user space buf */
-	kern_buf = kzalloc(nbytes, GFP_KERNEL);
-	if (!kern_buf)
-		return -ENOMEM;
-
-	if (copy_from_user(kern_buf, (void  __user *)buf, nbytes)) {
-		kfree(kern_buf);
-		return -ENOMEM;
-	}
+	/* Copy the user space buf */
+	kern_buf = memdup_user(buf, nbytes);
+	if (IS_ERR(kern_buf))
+		return PTR_ERR(kern_buf);
 
 	rc = sscanf(kern_buf, "%x:%x", &addr, &val);
 	if (rc < 2) {
-		pr_warn("bna %s: Failed to read user buffer\n",
-			pci_name(bnad->pcidev));
+		netdev_warn(bnad->netdev, "failed to read user buffer\n");
 		kfree(kern_buf);
 		return -EINVAL;
 	}
@@ -412,8 +395,7 @@ bnad_debugfs_write_regwr(struct file *file, const char __user *buf,
 	/* offset and len sanity check */
 	rc = bna_reg_offset_check(ioc, addr, 1);
 	if (rc) {
-		pr_warn("bna %s: Failed reg offset check\n",
-			pci_name(bnad->pcidev));
+		netdev_warn(bnad->netdev, "failed reg offset check\n");
 		return -EINVAL;
 	}
 
@@ -525,7 +507,8 @@ bnad_debugfs_init(struct bnad *bnad)
 		bna_debugfs_root = debugfs_create_dir("bna", NULL);
 		atomic_set(&bna_debugfs_port_count, 0);
 		if (!bna_debugfs_root) {
-			pr_warn("BNA: debugfs root dir creation failed\n");
+			netdev_warn(bnad->netdev,
+				    "debugfs root dir creation failed\n");
 			return;
 		}
 	}
@@ -536,8 +519,8 @@ bnad_debugfs_init(struct bnad *bnad)
 		bnad->port_debugfs_root =
 			debugfs_create_dir(name, bna_debugfs_root);
 		if (!bnad->port_debugfs_root) {
-			pr_warn("bna pci_dev %s: root dir creation failed\n",
-				pci_name(bnad->pcidev));
+			netdev_warn(bnad->netdev,
+				    "debugfs root dir creation failed\n");
 			return;
 		}
 
@@ -552,9 +535,9 @@ bnad_debugfs_init(struct bnad *bnad)
 							bnad,
 							file->fops);
 			if (!bnad->bnad_dentry_files[i]) {
-				pr_warn(
-				     "BNA pci_dev:%s: create %s entry failed\n",
-				     pci_name(bnad->pcidev), file->name);
+				netdev_warn(bnad->netdev,
+					    "create %s entry failed\n",
+					    file->name);
 				return;
 			}
 		}
diff --git a/drivers/net/ethernet/brocade/bna/bnad_ethtool.c b/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
index 12f344debd1c..2bdfc5dff4b1 100644
--- a/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
+++ b/drivers/net/ethernet/brocade/bna/bnad_ethtool.c
@@ -445,13 +445,13 @@ bnad_set_ringparam(struct net_device *netdev,
 
 	if (ringparam->rx_pending < BNAD_MIN_Q_DEPTH ||
 	    ringparam->rx_pending > BNAD_MAX_RXQ_DEPTH ||
-	    !BNA_POWER_OF_2(ringparam->rx_pending)) {
+	    !is_power_of_2(ringparam->rx_pending)) {
 		mutex_unlock(&bnad->conf_mutex);
 		return -EINVAL;
 	}
 	if (ringparam->tx_pending < BNAD_MIN_Q_DEPTH ||
 	    ringparam->tx_pending > BNAD_MAX_TXQ_DEPTH ||
-	    !BNA_POWER_OF_2(ringparam->tx_pending)) {
+	    !is_power_of_2(ringparam->tx_pending)) {
 		mutex_unlock(&bnad->conf_mutex);
 		return -EINVAL;
 	}
@@ -533,7 +533,7 @@ bnad_set_pauseparam(struct net_device *netdev,
 		pause_config.rx_pause = pauseparam->rx_pause;
 		pause_config.tx_pause = pauseparam->tx_pause;
 		spin_lock_irqsave(&bnad->bna_lock, flags);
-		bna_enet_pause_config(&bnad->bna.enet, &pause_config, NULL);
+		bna_enet_pause_config(&bnad->bna.enet, &pause_config);
 		spin_unlock_irqrestore(&bnad->bna_lock, flags);
 	}
 	mutex_unlock(&bnad->conf_mutex);
@@ -1080,7 +1080,7 @@ bnad_flash_device(struct net_device *netdev, struct ethtool_flash *eflash)
 
 	ret = request_firmware(&fw, eflash->data, &bnad->pcidev->dev);
 	if (ret) {
-		pr_err("BNA: Can't locate firmware %s\n", eflash->data);
+		netdev_err(netdev, "can't load firmware %s\n", eflash->data);
 		goto out;
 	}
 
@@ -1093,7 +1093,7 @@ bnad_flash_device(struct net_device *netdev, struct ethtool_flash *eflash)
 				bnad->id, (u8 *)fw->data, fw->size, 0,
 				bnad_cb_completion, &fcomp);
 	if (ret != BFA_STATUS_OK) {
-		pr_warn("BNA: Flash update failed with err: %d\n", ret);
+		netdev_warn(netdev, "flash update failed with err=%d\n", ret);
 		ret = -EIO;
 		spin_unlock_irq(&bnad->bna_lock);
 		goto out;
@@ -1103,8 +1103,9 @@ bnad_flash_device(struct net_device *netdev, struct ethtool_flash *eflash)
 	wait_for_completion(&fcomp.comp);
 	if (fcomp.comp_status != BFA_STATUS_OK) {
 		ret = -EIO;
-		pr_warn("BNA: Firmware image update to flash failed with: %d\n",
-			fcomp.comp_status);
+		netdev_warn(netdev,
+			    "firmware image update failed with err=%d\n",
+			    fcomp.comp_status);
 	}
 out:
 	release_firmware(fw);
diff --git a/drivers/net/ethernet/brocade/bna/cna.h b/drivers/net/ethernet/brocade/bna/cna.h
index 28e7d0ffeab1..75f8f1ac9fb7 100644
--- a/drivers/net/ethernet/brocade/bna/cna.h
+++ b/drivers/net/ethernet/brocade/bna/cna.h
@@ -42,66 +42,4 @@ extern char bfa_version[];
 #define CNA_FW_FILE_CT2	"ct2fw-3.2.5.1.bin"
 #define FC_SYMNAME_MAX	256	/*!< max name server symbolic name size */
 
-#pragma pack(1)
-
-typedef struct mac { u8 mac[ETH_ALEN]; } mac_t;
-
-#pragma pack()
-
-#define bfa_q_first(_q) ((void *)(((struct list_head *) (_q))->next))
-#define bfa_q_next(_qe)	(((struct list_head *) (_qe))->next)
-#define bfa_q_prev(_qe) (((struct list_head *) (_qe))->prev)
-
-/*
- * bfa_q_qe_init - to initialize a queue element
- */
-#define bfa_q_qe_init(_qe) {						\
-	bfa_q_next(_qe) = (struct list_head *) NULL;			\
-	bfa_q_prev(_qe) = (struct list_head *) NULL;			\
-}
-
-/*
- * bfa_q_deq - dequeue an element from head of the queue
- */
-#define bfa_q_deq(_q, _qe) {						\
-	if (!list_empty(_q)) {						\
-		(*((struct list_head **) (_qe))) = bfa_q_next(_q);	\
-		bfa_q_prev(bfa_q_next(*((struct list_head **) _qe))) =	\
-						(struct list_head *) (_q); \
-		bfa_q_next(_q) = bfa_q_next(*((struct list_head **) _qe)); \
-		bfa_q_qe_init(*((struct list_head **) _qe));		\
-	} else {							\
-		*((struct list_head **)(_qe)) = NULL;			\
-	}								\
-}
-
-/*
- * bfa_q_deq_tail - dequeue an element from tail of the queue
- */
-#define bfa_q_deq_tail(_q, _qe) {					\
-	if (!list_empty(_q)) {						\
-		*((struct list_head **) (_qe)) = bfa_q_prev(_q);	\
-		bfa_q_next(bfa_q_prev(*((struct list_head **) _qe))) =  \
-						(struct list_head *) (_q); \
-		bfa_q_prev(_q) = bfa_q_prev(*(struct list_head **) _qe);\
-		bfa_q_qe_init(*((struct list_head **) _qe));		\
-	} else {							\
-		*((struct list_head **) (_qe)) = (struct list_head *) NULL; \
-	}								\
-}
-
-/*
- * bfa_add_tail_head - enqueue an element at the head of queue
- */
-#define bfa_q_enq_head(_q, _qe) {					\
-	if (!(bfa_q_next(_qe) == NULL) && (bfa_q_prev(_qe) == NULL))	\
-		pr_err("Assertion failure: %s:%d: %d",			\
-			__FILE__, __LINE__,				\
-		(bfa_q_next(_qe) == NULL) && (bfa_q_prev(_qe) == NULL));\
-	bfa_q_next(_qe) = bfa_q_next(_q);				\
-	bfa_q_prev(_qe) = (struct list_head *) (_q);			\
-	bfa_q_prev(bfa_q_next(_q)) = (struct list_head *) (_qe);	\
-	bfa_q_next(_q) = (struct list_head *) (_qe);			\
-}
-
 #endif /* __CNA_H__ */
diff --git a/drivers/net/ethernet/brocade/bna/cna_fwimg.c b/drivers/net/ethernet/brocade/bna/cna_fwimg.c
index ebf462d8082f..2e7fb97883dc 100644
--- a/drivers/net/ethernet/brocade/bna/cna_fwimg.c
+++ b/drivers/net/ethernet/brocade/bna/cna_fwimg.c
@@ -30,9 +30,10 @@ cna_read_firmware(struct pci_dev *pdev, u32 **bfi_image,
 			u32 *bfi_image_size, char *fw_name)
 {
 	const struct firmware *fw;
+	u32 n;
 
 	if (request_firmware(&fw, fw_name, &pdev->dev)) {
-		pr_alert("Can't locate firmware %s\n", fw_name);
+		dev_alert(&pdev->dev, "can't load firmware %s\n", fw_name);
 		goto error;
 	}
 
@@ -40,6 +41,12 @@ cna_read_firmware(struct pci_dev *pdev, u32 **bfi_image,
 	*bfi_image_size = fw->size/sizeof(u32);
 	bfi_fw = fw;
 
+	/* Convert loaded firmware to host order as it is stored in file
+	 * as sequence of LE32 integers.
+	 */
+	for (n = 0; n < *bfi_image_size; n++)
+		le32_to_cpus(*bfi_image + n);
+
 	return *bfi_image;
 error:
 	return NULL;
diff --git a/drivers/net/ethernet/cadence/Kconfig b/drivers/net/ethernet/cadence/Kconfig
index 1ba3e3a67389..f0bcb15d3fec 100644
--- a/drivers/net/ethernet/cadence/Kconfig
+++ b/drivers/net/ethernet/cadence/Kconfig
@@ -8,8 +8,6 @@ config NET_CADENCE
 	default y
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y.
-	  Make sure you know the name of your card. Read the Ethernet-HOWTO,
-	  available from <http://www.tldp.org/docs.html#howto>.
 
 	  If unsure, say Y.
 
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index fc646a41d548..caeb39561567 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -54,6 +54,8 @@
 #define MACB_MAX_TX_LEN		((unsigned int)((1 << MACB_TX_FRMLEN_SIZE) - 1))
 #define GEM_MAX_TX_LEN		((unsigned int)((1 << GEM_TX_FRMLEN_SIZE) - 1))
 
+#define GEM_MTU_MIN_SIZE	68
+
 /*
  * Graceful stop timeouts in us. We should allow up to
  * 1 frame time (10 Mbits/s, full-duplex, ignoring collisions)
@@ -785,7 +787,7 @@ static int gem_rx(struct macb *bp, int budget)
 		}
 		/* now everything is ready for receiving packet */
 		bp->rx_skbuff[entry] = NULL;
-		len = MACB_BFEXT(RX_FRMLEN, ctrl);
+		len = ctrl & bp->rx_frm_len_mask;
 
 		netdev_vdbg(bp->dev, "gem_rx %u (len %u)\n", entry, len);
 
@@ -831,7 +833,7 @@ static int macb_rx_frame(struct macb *bp, unsigned int first_frag,
 	struct macb_dma_desc *desc;
 
 	desc = macb_rx_desc(bp, last_frag);
-	len = MACB_BFEXT(RX_FRMLEN, desc->ctrl);
+	len = desc->ctrl & bp->rx_frm_len_mask;
 
 	netdev_vdbg(bp->dev, "macb_rx_frame frags %u - %u (len %u)\n",
 		macb_rx_ring_wrap(first_frag),
@@ -1651,7 +1653,10 @@ static void macb_init_hw(struct macb *bp)
 	config |= MACB_BF(RBOF, NET_IP_ALIGN);	/* Make eth data aligned */
 	config |= MACB_BIT(PAE);		/* PAuse Enable */
 	config |= MACB_BIT(DRFCS);		/* Discard Rx FCS */
-	config |= MACB_BIT(BIG);		/* Receive oversized frames */
+	if (bp->caps & MACB_CAPS_JUMBO)
+		config |= MACB_BIT(JFRAME);	/* Enable jumbo frames */
+	else
+		config |= MACB_BIT(BIG);	/* Receive oversized frames */
 	if (bp->dev->flags & IFF_PROMISC)
 		config |= MACB_BIT(CAF);	/* Copy All Frames */
 	else if (macb_is_gem(bp) && bp->dev->features & NETIF_F_RXCSUM)
@@ -1660,8 +1665,13 @@ static void macb_init_hw(struct macb *bp)
 		config |= MACB_BIT(NBC);	/* No BroadCast */
 	config |= macb_dbw(bp);
 	macb_writel(bp, NCFGR, config);
+	if ((bp->caps & MACB_CAPS_JUMBO) && bp->jumbo_max_len)
+		gem_writel(bp, JML, bp->jumbo_max_len);
 	bp->speed = SPEED_10;
 	bp->duplex = DUPLEX_HALF;
+	bp->rx_frm_len_mask = MACB_RX_FRMLEN_MASK;
+	if (bp->caps & MACB_CAPS_JUMBO)
+		bp->rx_frm_len_mask = MACB_RX_JFRMLEN_MASK;
 
 	macb_configure_dma(bp);
 
@@ -1865,6 +1875,26 @@ static int macb_close(struct net_device *dev)
 	return 0;
 }
 
+static int macb_change_mtu(struct net_device *dev, int new_mtu)
+{
+	struct macb *bp = netdev_priv(dev);
+	u32 max_mtu;
+
+	if (netif_running(dev))
+		return -EBUSY;
+
+	max_mtu = ETH_DATA_LEN;
+	if (bp->caps & MACB_CAPS_JUMBO)
+		max_mtu = gem_readl(bp, JML) - ETH_HLEN - ETH_FCS_LEN;
+
+	if ((new_mtu > max_mtu) || (new_mtu < GEM_MTU_MIN_SIZE))
+		return -EINVAL;
+
+	dev->mtu = new_mtu;
+
+	return 0;
+}
+
 static void gem_update_stats(struct macb *bp)
 {
 	int i;
@@ -2141,7 +2171,7 @@ static const struct net_device_ops macb_netdev_ops = {
 	.ndo_get_stats		= macb_get_stats,
 	.ndo_do_ioctl		= macb_ioctl,
 	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_change_mtu		= eth_change_mtu,
+	.ndo_change_mtu		= macb_change_mtu,
 	.ndo_set_mac_address	= eth_mac_addr,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= macb_poll_controller,
@@ -2683,6 +2713,13 @@ static const struct macb_config pc302gem_config = {
 	.init = macb_init,
 };
 
+static const struct macb_config sama5d2_config = {
+	.caps = 0,
+	.dma_burst_length = 16,
+	.clk_init = macb_clk_init,
+	.init = macb_init,
+};
+
 static const struct macb_config sama5d3_config = {
 	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE,
 	.dma_burst_length = 16,
@@ -2702,6 +2739,16 @@ static const struct macb_config emac_config = {
 	.init = at91ether_init,
 };
 
+
+static const struct macb_config zynqmp_config = {
+	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE |
+		MACB_CAPS_JUMBO,
+	.dma_burst_length = 16,
+	.clk_init = macb_clk_init,
+	.init = macb_init,
+	.jumbo_max_len = 10240,
+};
+
 static const struct macb_config zynq_config = {
 	.caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE |
 		MACB_CAPS_NO_GIGABIT_HALF,
@@ -2716,10 +2763,12 @@ static const struct of_device_id macb_dt_ids[] = {
 	{ .compatible = "cdns,macb" },
 	{ .compatible = "cdns,pc302-gem", .data = &pc302gem_config },
 	{ .compatible = "cdns,gem", .data = &pc302gem_config },
+	{ .compatible = "atmel,sama5d2-gem", .data = &sama5d2_config },
 	{ .compatible = "atmel,sama5d3-gem", .data = &sama5d3_config },
 	{ .compatible = "atmel,sama5d4-gem", .data = &sama5d4_config },
 	{ .compatible = "cdns,at91rm9200-emac", .data = &emac_config },
 	{ .compatible = "cdns,emac", .data = &emac_config },
+	{ .compatible = "cdns,zynqmp-gem", .data = &zynqmp_config},
 	{ .compatible = "cdns,zynq-gem", .data = &zynq_config },
 	{ /* sentinel */ }
 };
@@ -2789,6 +2838,10 @@ static int macb_probe(struct platform_device *pdev)
 	bp->pclk = pclk;
 	bp->hclk = hclk;
 	bp->tx_clk = tx_clk;
+	if (macb_config->jumbo_max_len) {
+		bp->jumbo_max_len = macb_config->jumbo_max_len;
+	}
+
 	spin_lock_init(&bp->lock);
 
 	/* setup capabilities */
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index 24b1d9bcd865..d74655993d4b 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -71,6 +71,7 @@
 #define GEM_NCFGR		0x0004 /* Network Config */
 #define GEM_USRIO		0x000c /* User IO */
 #define GEM_DMACFG		0x0010 /* DMA Configuration */
+#define GEM_JML			0x0048 /* Jumbo Max Length */
 #define GEM_HRB			0x0080 /* Hash Bottom */
 #define GEM_HRT			0x0084 /* Hash Top */
 #define GEM_SA1B		0x0088 /* Specific1 Bottom */
@@ -398,6 +399,7 @@
 #define MACB_CAPS_GIGABIT_MODE_AVAILABLE	0x20000000
 #define MACB_CAPS_SG_DISABLED			0x40000000
 #define MACB_CAPS_MACB_IS_GEM			0x80000000
+#define MACB_CAPS_JUMBO				0x00000008
 
 /* Bit manipulation macros */
 #define MACB_BIT(name)					\
@@ -515,6 +517,9 @@ struct macb_dma_desc {
 #define MACB_RX_BROADCAST_OFFSET		31
 #define MACB_RX_BROADCAST_SIZE			1
 
+#define MACB_RX_FRMLEN_MASK			0xFFF
+#define MACB_RX_JFRMLEN_MASK			0x3FFF
+
 /* RX checksum offload disabled: bit 24 clear in NCFGR */
 #define GEM_RX_TYPEID_MATCH_OFFSET		22
 #define GEM_RX_TYPEID_MATCH_SIZE		2
@@ -758,6 +763,7 @@ struct macb_config {
 	int	(*clk_init)(struct platform_device *pdev, struct clk **pclk,
 			    struct clk **hclk, struct clk **tx_clk);
 	int	(*init)(struct platform_device *pdev);
+	int	jumbo_max_len;
 };
 
 struct macb_queue {
@@ -827,6 +833,9 @@ struct macb {
 	unsigned int		max_tx_length;
 
 	u64			ethtool_stats[GEM_STATS_LEN];
+
+	unsigned int		rx_frm_len_mask;
+	unsigned int		jumbo_max_len;
 };
 
 static inline bool macb_is_gem(struct macb *bp)
diff --git a/drivers/net/ethernet/cavium/Kconfig b/drivers/net/ethernet/cavium/Kconfig
new file mode 100644
index 000000000000..c4d6bbe9458d
--- /dev/null
+++ b/drivers/net/ethernet/cavium/Kconfig
@@ -0,0 +1,57 @@
+#
+# Cavium ethernet device configuration
+#
+
+config NET_VENDOR_CAVIUM
+	tristate "Cavium ethernet drivers"
+	depends on PCI
+	default y
+	---help---
+	  Select this option if you want enable Cavium network support.
+
+	  If you have a Cavium SoC or network adapter, say Y.
+
+if NET_VENDOR_CAVIUM
+
+config THUNDER_NIC_PF
+	tristate "Thunder Physical function driver"
+	depends on 64BIT
+	default ARCH_THUNDER
+	select THUNDER_NIC_BGX
+	---help---
+	  This driver supports Thunder's NIC physical function.
+	  The NIC provides the controller and DMA engines to
+	  move network traffic to/from the memory. The NIC
+	  works closely with TNS, BGX and SerDes to implement the
+	  functions replacing and virtualizing those of a typical
+	  standalone PCIe NIC chip.
+
+config THUNDER_NIC_VF
+	tristate "Thunder Virtual function driver"
+	depends on 64BIT
+	default ARCH_THUNDER
+	---help---
+	  This driver supports Thunder's NIC virtual function
+
+config	THUNDER_NIC_BGX
+	tristate "Thunder MAC interface driver (BGX)"
+	depends on 64BIT
+	default ARCH_THUNDER
+	---help---
+	  This driver supports programming and controlling of MAC
+	  interface from NIC physical function driver.
+
+config LIQUIDIO
+	tristate "Cavium LiquidIO support"
+	depends on 64BIT
+	select PTP_1588_CLOCK
+	select FW_LOADER
+	select LIBCRC32C
+	---help---
+	  This driver supports Cavium LiquidIO Intelligent Server Adapters
+	  based on CN66XX and CN68XX chips.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called liquidio.  This is recommended.
+
+endif # NET_VENDOR_CAVIUM
diff --git a/drivers/net/ethernet/cavium/Makefile b/drivers/net/ethernet/cavium/Makefile
new file mode 100644
index 000000000000..d22f886ac291
--- /dev/null
+++ b/drivers/net/ethernet/cavium/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the Cavium ethernet device drivers.
+#
+obj-$(CONFIG_NET_VENDOR_CAVIUM) += thunder/
+obj-$(CONFIG_NET_VENDOR_CAVIUM) += liquidio/
diff --git a/drivers/net/ethernet/cavium/liquidio/Makefile b/drivers/net/ethernet/cavium/liquidio/Makefile
new file mode 100644
index 000000000000..2f366806835d
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/Makefile
@@ -0,0 +1,16 @@
+#
+# Cavium Liquidio ethernet device driver
+#
+obj-$(CONFIG_LIQUIDIO) += liquidio.o
+
+liquidio-objs := lio_main.o  \
+	      lio_ethtool.o      \
+	      request_manager.o  \
+	      response_manager.o \
+	      octeon_device.o    \
+	      cn66xx_device.o    \
+	      cn68xx_device.o    \
+	      octeon_mem_ops.o   \
+	      octeon_droq.o      \
+	      octeon_console.o   \
+	      octeon_nic.o
diff --git a/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c
new file mode 100644
index 000000000000..8ad7425f89bf
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.c
@@ -0,0 +1,796 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "liquidio_image.h"
+#include "octeon_mem_ops.h"
+
+int lio_cn6xxx_soft_reset(struct octeon_device *oct)
+{
+	octeon_write_csr64(oct, CN6XXX_WIN_WR_MASK_REG, 0xFF);
+
+	dev_dbg(&oct->pci_dev->dev, "BIST enabled for soft reset\n");
+
+	lio_pci_writeq(oct, 1, CN6XXX_CIU_SOFT_BIST);
+	octeon_write_csr64(oct, CN6XXX_SLI_SCRATCH1, 0x1234ULL);
+
+	lio_pci_readq(oct, CN6XXX_CIU_SOFT_RST);
+	lio_pci_writeq(oct, 1, CN6XXX_CIU_SOFT_RST);
+
+	/* make sure that the reset is written before starting timer */
+	mmiowb();
+
+	/* Wait for 10ms as Octeon resets. */
+	mdelay(100);
+
+	if (octeon_read_csr64(oct, CN6XXX_SLI_SCRATCH1) == 0x1234ULL) {
+		dev_err(&oct->pci_dev->dev, "Soft reset failed\n");
+		return 1;
+	}
+
+	dev_dbg(&oct->pci_dev->dev, "Reset completed\n");
+	octeon_write_csr64(oct, CN6XXX_WIN_WR_MASK_REG, 0xFF);
+
+	return 0;
+}
+
+void lio_cn6xxx_enable_error_reporting(struct octeon_device *oct)
+{
+	u32 val;
+
+	pci_read_config_dword(oct->pci_dev, CN6XXX_PCIE_DEVCTL, &val);
+	if (val & 0x000f0000) {
+		dev_err(&oct->pci_dev->dev, "PCI-E Link error detected: 0x%08x\n",
+			val & 0x000f0000);
+	}
+
+	val |= 0xf;          /* Enable Link error reporting */
+
+	dev_dbg(&oct->pci_dev->dev, "Enabling PCI-E error reporting..\n");
+	pci_write_config_dword(oct->pci_dev, CN6XXX_PCIE_DEVCTL, val);
+}
+
+void lio_cn6xxx_setup_pcie_mps(struct octeon_device *oct,
+			       enum octeon_pcie_mps mps)
+{
+	u32 val;
+	u64 r64;
+
+	/* Read config register for MPS */
+	pci_read_config_dword(oct->pci_dev, CN6XXX_PCIE_DEVCTL, &val);
+
+	if (mps == PCIE_MPS_DEFAULT) {
+		mps = ((val & (0x7 << 5)) >> 5);
+	} else {
+		val &= ~(0x7 << 5);  /* Turn off any MPS bits */
+		val |= (mps << 5);   /* Set MPS */
+		pci_write_config_dword(oct->pci_dev, CN6XXX_PCIE_DEVCTL, val);
+	}
+
+	/* Set MPS in DPI_SLI_PRT0_CFG to the same value. */
+	r64 = lio_pci_readq(oct, CN6XXX_DPI_SLI_PRTX_CFG(oct->pcie_port));
+	r64 |= (mps << 4);
+	lio_pci_writeq(oct, r64, CN6XXX_DPI_SLI_PRTX_CFG(oct->pcie_port));
+}
+
+void lio_cn6xxx_setup_pcie_mrrs(struct octeon_device *oct,
+				enum octeon_pcie_mrrs mrrs)
+{
+	u32 val;
+	u64 r64;
+
+	/* Read config register for MRRS */
+	pci_read_config_dword(oct->pci_dev, CN6XXX_PCIE_DEVCTL, &val);
+
+	if (mrrs == PCIE_MRRS_DEFAULT) {
+		mrrs = ((val & (0x7 << 12)) >> 12);
+	} else {
+		val &= ~(0x7 << 12); /* Turn off any MRRS bits */
+		val |= (mrrs << 12); /* Set MRRS */
+		pci_write_config_dword(oct->pci_dev, CN6XXX_PCIE_DEVCTL, val);
+	}
+
+	/* Set MRRS in SLI_S2M_PORT0_CTL to the same value. */
+	r64 = octeon_read_csr64(oct, CN6XXX_SLI_S2M_PORTX_CTL(oct->pcie_port));
+	r64 |= mrrs;
+	octeon_write_csr64(oct, CN6XXX_SLI_S2M_PORTX_CTL(oct->pcie_port), r64);
+
+	/* Set MRRS in DPI_SLI_PRT0_CFG to the same value. */
+	r64 = lio_pci_readq(oct, CN6XXX_DPI_SLI_PRTX_CFG(oct->pcie_port));
+	r64 |= mrrs;
+	lio_pci_writeq(oct, r64, CN6XXX_DPI_SLI_PRTX_CFG(oct->pcie_port));
+}
+
+u32 lio_cn6xxx_coprocessor_clock(struct octeon_device *oct)
+{
+	/* Bits 29:24 of MIO_RST_BOOT holds the ref. clock multiplier
+	 * for SLI.
+	 */
+	return ((lio_pci_readq(oct, CN6XXX_MIO_RST_BOOT) >> 24) & 0x3f) * 50;
+}
+
+u32 lio_cn6xxx_get_oq_ticks(struct octeon_device *oct,
+			    u32 time_intr_in_us)
+{
+	/* This gives the SLI clock per microsec */
+	u32 oqticks_per_us = lio_cn6xxx_coprocessor_clock(oct);
+
+	/* core clock per us / oq ticks will be fractional. TO avoid that
+	 * we use the method below.
+	 */
+
+	/* This gives the clock cycles per millisecond */
+	oqticks_per_us *= 1000;
+
+	/* This gives the oq ticks (1024 core clock cycles) per millisecond */
+	oqticks_per_us /= 1024;
+
+	/* time_intr is in microseconds. The next 2 steps gives the oq ticks
+	 * corressponding to time_intr.
+	 */
+	oqticks_per_us *= time_intr_in_us;
+	oqticks_per_us /= 1000;
+
+	return oqticks_per_us;
+}
+
+void lio_cn6xxx_setup_global_input_regs(struct octeon_device *oct)
+{
+	/* Select Round-Robin Arb, ES, RO, NS for Input Queues */
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_INPUT_CONTROL,
+			 CN6XXX_INPUT_CTL_MASK);
+
+	/* Instruction Read Size - Max 4 instructions per PCIE Read */
+	octeon_write_csr64(oct, CN6XXX_SLI_PKT_INSTR_RD_SIZE,
+			   0xFFFFFFFFFFFFFFFFULL);
+
+	/* Select PCIE Port for all Input rings. */
+	octeon_write_csr64(oct, CN6XXX_SLI_IN_PCIE_PORT,
+			   (oct->pcie_port * 0x5555555555555555ULL));
+}
+
+static void lio_cn66xx_setup_pkt_ctl_regs(struct octeon_device *oct)
+{
+	u64 pktctl;
+
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->chip;
+
+	pktctl = octeon_read_csr64(oct, CN6XXX_SLI_PKT_CTL);
+
+	/* 66XX SPECIFIC */
+	if (CFG_GET_OQ_MAX_Q(cn6xxx->conf) <= 4)
+		/* Disable RING_EN if only upto 4 rings are used. */
+		pktctl &= ~(1 << 4);
+	else
+		pktctl |= (1 << 4);
+
+	if (CFG_GET_IS_SLI_BP_ON(cn6xxx->conf))
+		pktctl |= 0xF;
+	else
+		/* Disable per-port backpressure. */
+		pktctl &= ~0xF;
+	octeon_write_csr64(oct, CN6XXX_SLI_PKT_CTL, pktctl);
+}
+
+void lio_cn6xxx_setup_global_output_regs(struct octeon_device *oct)
+{
+	u32 time_threshold;
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->chip;
+
+	/* / Select PCI-E Port for all Output queues */
+	octeon_write_csr64(oct, CN6XXX_SLI_PKT_PCIE_PORT64,
+			   (oct->pcie_port * 0x5555555555555555ULL));
+
+	if (CFG_GET_IS_SLI_BP_ON(cn6xxx->conf)) {
+		octeon_write_csr64(oct, CN6XXX_SLI_OQ_WMARK, 32);
+	} else {
+		/* / Set Output queue watermark to 0 to disable backpressure */
+		octeon_write_csr64(oct, CN6XXX_SLI_OQ_WMARK, 0);
+	}
+
+	/* / Select Info Ptr for length & data */
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_IPTR, 0xFFFFFFFF);
+
+	/* / Select Packet count instead of bytes for SLI_PKTi_CNTS[CNT] */
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_OUT_BMODE, 0);
+
+	/* / Select ES,RO,NS setting from register for Output Queue Packet
+	 * Address
+	 */
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_DPADDR, 0xFFFFFFFF);
+
+	/* No Relaxed Ordering, No Snoop, 64-bit swap for Output
+	 * Queue ScatterList
+	 */
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_SLIST_ROR, 0);
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_SLIST_NS, 0);
+
+	/* / ENDIAN_SPECIFIC CHANGES - 0 works for LE. */
+#ifdef __BIG_ENDIAN_BITFIELD
+	octeon_write_csr64(oct, CN6XXX_SLI_PKT_SLIST_ES64,
+			   0x5555555555555555ULL);
+#else
+	octeon_write_csr64(oct, CN6XXX_SLI_PKT_SLIST_ES64, 0ULL);
+#endif
+
+	/* / No Relaxed Ordering, No Snoop, 64-bit swap for Output Queue Data */
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_DATA_OUT_ROR, 0);
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_DATA_OUT_NS, 0);
+	octeon_write_csr64(oct, CN6XXX_SLI_PKT_DATA_OUT_ES64,
+			   0x5555555555555555ULL);
+
+	/* / Set up interrupt packet and time threshold */
+	octeon_write_csr(oct, CN6XXX_SLI_OQ_INT_LEVEL_PKTS,
+			 (u32)CFG_GET_OQ_INTR_PKT(cn6xxx->conf));
+	time_threshold =
+		lio_cn6xxx_get_oq_ticks(oct, (u32)
+					CFG_GET_OQ_INTR_TIME(cn6xxx->conf));
+
+	octeon_write_csr(oct, CN6XXX_SLI_OQ_INT_LEVEL_TIME, time_threshold);
+}
+
+static int lio_cn6xxx_setup_device_regs(struct octeon_device *oct)
+{
+	lio_cn6xxx_setup_pcie_mps(oct, PCIE_MPS_DEFAULT);
+	lio_cn6xxx_setup_pcie_mrrs(oct, PCIE_MRRS_512B);
+	lio_cn6xxx_enable_error_reporting(oct);
+
+	lio_cn6xxx_setup_global_input_regs(oct);
+	lio_cn66xx_setup_pkt_ctl_regs(oct);
+	lio_cn6xxx_setup_global_output_regs(oct);
+
+	/* Default error timeout value should be 0x200000 to avoid host hang
+	 * when reads invalid register
+	 */
+	octeon_write_csr64(oct, CN6XXX_SLI_WINDOW_CTL, 0x200000ULL);
+	return 0;
+}
+
+void lio_cn6xxx_setup_iq_regs(struct octeon_device *oct, u32 iq_no)
+{
+	struct octeon_instr_queue *iq = oct->instr_queue[iq_no];
+
+	/* Disable Packet-by-Packet mode; No Parse Mode or Skip length */
+	octeon_write_csr64(oct, CN6XXX_SLI_IQ_PKT_INSTR_HDR64(iq_no), 0);
+
+	/* Write the start of the input queue's ring and its size  */
+	octeon_write_csr64(oct, CN6XXX_SLI_IQ_BASE_ADDR64(iq_no),
+			   iq->base_addr_dma);
+	octeon_write_csr(oct, CN6XXX_SLI_IQ_SIZE(iq_no), iq->max_count);
+
+	/* Remember the doorbell & instruction count register addr for this
+	 * queue
+	 */
+	iq->doorbell_reg = oct->mmio[0].hw_addr + CN6XXX_SLI_IQ_DOORBELL(iq_no);
+	iq->inst_cnt_reg = oct->mmio[0].hw_addr
+			   + CN6XXX_SLI_IQ_INSTR_COUNT(iq_no);
+	dev_dbg(&oct->pci_dev->dev, "InstQ[%d]:dbell reg @ 0x%p instcnt_reg @ 0x%p\n",
+		iq_no, iq->doorbell_reg, iq->inst_cnt_reg);
+
+	/* Store the current instruction counter
+	 * (used in flush_iq calculation)
+	 */
+	iq->reset_instr_cnt = readl(iq->inst_cnt_reg);
+}
+
+static void lio_cn66xx_setup_iq_regs(struct octeon_device *oct, u32 iq_no)
+{
+	lio_cn6xxx_setup_iq_regs(oct, iq_no);
+
+	/* Backpressure for this queue - WMARK set to all F's. This effectively
+	 * disables the backpressure mechanism.
+	 */
+	octeon_write_csr64(oct, CN66XX_SLI_IQ_BP64(iq_no),
+			   (0xFFFFFFFFULL << 32));
+}
+
+void lio_cn6xxx_setup_oq_regs(struct octeon_device *oct, u32 oq_no)
+{
+	u32 intr;
+	struct octeon_droq *droq = oct->droq[oq_no];
+
+	octeon_write_csr64(oct, CN6XXX_SLI_OQ_BASE_ADDR64(oq_no),
+			   droq->desc_ring_dma);
+	octeon_write_csr(oct, CN6XXX_SLI_OQ_SIZE(oq_no), droq->max_count);
+
+	octeon_write_csr(oct, CN6XXX_SLI_OQ_BUFF_INFO_SIZE(oq_no),
+			 (droq->buffer_size | (OCT_RH_SIZE << 16)));
+
+	/* Get the mapped address of the pkt_sent and pkts_credit regs */
+	droq->pkts_sent_reg =
+		oct->mmio[0].hw_addr + CN6XXX_SLI_OQ_PKTS_SENT(oq_no);
+	droq->pkts_credit_reg =
+		oct->mmio[0].hw_addr + CN6XXX_SLI_OQ_PKTS_CREDIT(oq_no);
+
+	/* Enable this output queue to generate Packet Timer Interrupt */
+	intr = octeon_read_csr(oct, CN6XXX_SLI_PKT_TIME_INT_ENB);
+	intr |= (1 << oq_no);
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_TIME_INT_ENB, intr);
+
+	/* Enable this output queue to generate Packet Timer Interrupt */
+	intr = octeon_read_csr(oct, CN6XXX_SLI_PKT_CNT_INT_ENB);
+	intr |= (1 << oq_no);
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_CNT_INT_ENB, intr);
+}
+
+void lio_cn6xxx_enable_io_queues(struct octeon_device *oct)
+{
+	u32 mask;
+
+	mask = octeon_read_csr(oct, CN6XXX_SLI_PKT_INSTR_SIZE);
+	mask |= oct->io_qmask.iq64B;
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_INSTR_SIZE, mask);
+
+	mask = octeon_read_csr(oct, CN6XXX_SLI_PKT_INSTR_ENB);
+	mask |= oct->io_qmask.iq;
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_INSTR_ENB, mask);
+
+	mask = octeon_read_csr(oct, CN6XXX_SLI_PKT_OUT_ENB);
+	mask |= oct->io_qmask.oq;
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_OUT_ENB, mask);
+}
+
+void lio_cn6xxx_disable_io_queues(struct octeon_device *oct)
+{
+	u32 mask, i, loop = HZ;
+	u32 d32;
+
+	/* Reset the Enable bits for Input Queues. */
+	mask = octeon_read_csr(oct, CN6XXX_SLI_PKT_INSTR_ENB);
+	mask ^= oct->io_qmask.iq;
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_INSTR_ENB, mask);
+
+	/* Wait until hardware indicates that the queues are out of reset. */
+	mask = oct->io_qmask.iq;
+	d32 = octeon_read_csr(oct, CN6XXX_SLI_PORT_IN_RST_IQ);
+	while (((d32 & mask) != mask) && loop--) {
+		d32 = octeon_read_csr(oct, CN6XXX_SLI_PORT_IN_RST_IQ);
+		schedule_timeout_uninterruptible(1);
+	}
+
+	/* Reset the doorbell register for each Input queue. */
+	for (i = 0; i < MAX_OCTEON_INSTR_QUEUES; i++) {
+		if (!(oct->io_qmask.iq & (1UL << i)))
+			continue;
+		octeon_write_csr(oct, CN6XXX_SLI_IQ_DOORBELL(i), 0xFFFFFFFF);
+		d32 = octeon_read_csr(oct, CN6XXX_SLI_IQ_DOORBELL(i));
+	}
+
+	/* Reset the Enable bits for Output Queues. */
+	mask = octeon_read_csr(oct, CN6XXX_SLI_PKT_OUT_ENB);
+	mask ^= oct->io_qmask.oq;
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_OUT_ENB, mask);
+
+	/* Wait until hardware indicates that the queues are out of reset. */
+	loop = HZ;
+	mask = oct->io_qmask.oq;
+	d32 = octeon_read_csr(oct, CN6XXX_SLI_PORT_IN_RST_OQ);
+	while (((d32 & mask) != mask) && loop--) {
+		d32 = octeon_read_csr(oct, CN6XXX_SLI_PORT_IN_RST_OQ);
+		schedule_timeout_uninterruptible(1);
+	}
+	;
+
+	/* Reset the doorbell register for each Output queue. */
+	/* for (i = 0; i < oct->num_oqs; i++) { */
+	for (i = 0; i < MAX_OCTEON_OUTPUT_QUEUES; i++) {
+		if (!(oct->io_qmask.oq & (1UL << i)))
+			continue;
+		octeon_write_csr(oct, CN6XXX_SLI_OQ_PKTS_CREDIT(i), 0xFFFFFFFF);
+		d32 = octeon_read_csr(oct, CN6XXX_SLI_OQ_PKTS_CREDIT(i));
+
+		d32 = octeon_read_csr(oct, CN6XXX_SLI_OQ_PKTS_SENT(i));
+		octeon_write_csr(oct, CN6XXX_SLI_OQ_PKTS_SENT(i), d32);
+	}
+
+	d32 = octeon_read_csr(oct, CN6XXX_SLI_PKT_CNT_INT);
+	if (d32)
+		octeon_write_csr(oct, CN6XXX_SLI_PKT_CNT_INT, d32);
+
+	d32 = octeon_read_csr(oct, CN6XXX_SLI_PKT_TIME_INT);
+	if (d32)
+		octeon_write_csr(oct, CN6XXX_SLI_PKT_TIME_INT, d32);
+}
+
+void lio_cn6xxx_reinit_regs(struct octeon_device *oct)
+{
+	u32 i;
+
+	for (i = 0; i < MAX_OCTEON_INSTR_QUEUES; i++) {
+		if (!(oct->io_qmask.iq & (1UL << i)))
+			continue;
+		oct->fn_list.setup_iq_regs(oct, i);
+	}
+
+	for (i = 0; i < MAX_OCTEON_OUTPUT_QUEUES; i++) {
+		if (!(oct->io_qmask.oq & (1UL << i)))
+			continue;
+		oct->fn_list.setup_oq_regs(oct, i);
+	}
+
+	oct->fn_list.setup_device_regs(oct);
+
+	oct->fn_list.enable_interrupt(oct->chip);
+
+	oct->fn_list.enable_io_queues(oct);
+
+	/* for (i = 0; i < oct->num_oqs; i++) { */
+	for (i = 0; i < MAX_OCTEON_OUTPUT_QUEUES; i++) {
+		if (!(oct->io_qmask.oq & (1UL << i)))
+			continue;
+		writel(oct->droq[i]->max_count, oct->droq[i]->pkts_credit_reg);
+	}
+}
+
+void
+lio_cn6xxx_bar1_idx_setup(struct octeon_device *oct,
+			  u64 core_addr,
+			  u32 idx,
+			  int valid)
+{
+	u64 bar1;
+
+	if (valid == 0) {
+		bar1 = lio_pci_readq(oct, CN6XXX_BAR1_REG(idx, oct->pcie_port));
+		lio_pci_writeq(oct, (bar1 & 0xFFFFFFFEULL),
+			       CN6XXX_BAR1_REG(idx, oct->pcie_port));
+		bar1 = lio_pci_readq(oct, CN6XXX_BAR1_REG(idx, oct->pcie_port));
+		return;
+	}
+
+	/* Bits 17:4 of the PCI_BAR1_INDEXx stores bits 35:22 of
+	 * the Core Addr
+	 */
+	lio_pci_writeq(oct, (((core_addr >> 22) << 4) | PCI_BAR1_MASK),
+		       CN6XXX_BAR1_REG(idx, oct->pcie_port));
+
+	bar1 = lio_pci_readq(oct, CN6XXX_BAR1_REG(idx, oct->pcie_port));
+}
+
+void lio_cn6xxx_bar1_idx_write(struct octeon_device *oct,
+			       u32 idx,
+			       u32 mask)
+{
+	lio_pci_writeq(oct, mask, CN6XXX_BAR1_REG(idx, oct->pcie_port));
+}
+
+u32 lio_cn6xxx_bar1_idx_read(struct octeon_device *oct, u32 idx)
+{
+	return (u32)lio_pci_readq(oct, CN6XXX_BAR1_REG(idx, oct->pcie_port));
+}
+
+u32
+lio_cn6xxx_update_read_index(struct octeon_device *oct __attribute__((unused)),
+			     struct octeon_instr_queue *iq)
+{
+	u32 new_idx = readl(iq->inst_cnt_reg);
+
+	/* The new instr cnt reg is a 32-bit counter that can roll over. We have
+	 * noted the counter's initial value at init time into
+	 * reset_instr_cnt
+	 */
+	if (iq->reset_instr_cnt < new_idx)
+		new_idx -= iq->reset_instr_cnt;
+	else
+		new_idx += (0xffffffff - iq->reset_instr_cnt) + 1;
+
+	/* Modulo of the new index with the IQ size will give us
+	 * the new index.
+	 */
+	new_idx %= iq->max_count;
+
+	return new_idx;
+}
+
+void lio_cn6xxx_enable_interrupt(void *chip)
+{
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)chip;
+	u64 mask = cn6xxx->intr_mask64 | CN6XXX_INTR_DMA0_FORCE;
+
+	/* Enable Interrupt */
+	writeq(mask, cn6xxx->intr_enb_reg64);
+}
+
+void lio_cn6xxx_disable_interrupt(void *chip)
+{
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)chip;
+
+	/* Disable Interrupts */
+	writeq(0, cn6xxx->intr_enb_reg64);
+
+	/* make sure interrupts are really disabled */
+	mmiowb();
+}
+
+static void lio_cn6xxx_get_pcie_qlmport(struct octeon_device *oct)
+{
+	/* CN63xx Pass2 and newer parts implements the SLI_MAC_NUMBER register
+	 * to determine the PCIE port #
+	 */
+	oct->pcie_port = octeon_read_csr(oct, CN6XXX_SLI_MAC_NUMBER) & 0xff;
+
+	dev_dbg(&oct->pci_dev->dev, "Using PCIE Port %d\n", oct->pcie_port);
+}
+
+void
+lio_cn6xxx_process_pcie_error_intr(struct octeon_device *oct, u64 intr64)
+{
+	dev_err(&oct->pci_dev->dev, "Error Intr: 0x%016llx\n",
+		CVM_CAST64(intr64));
+}
+
+int lio_cn6xxx_process_droq_intr_regs(struct octeon_device *oct)
+{
+	struct octeon_droq *droq;
+	u32 oq_no, pkt_count, droq_time_mask, droq_mask, droq_int_enb;
+	u32 droq_cnt_enb, droq_cnt_mask;
+
+	droq_cnt_enb = octeon_read_csr(oct, CN6XXX_SLI_PKT_CNT_INT_ENB);
+	droq_cnt_mask = octeon_read_csr(oct, CN6XXX_SLI_PKT_CNT_INT);
+	droq_mask = droq_cnt_mask & droq_cnt_enb;
+
+	droq_time_mask = octeon_read_csr(oct, CN6XXX_SLI_PKT_TIME_INT);
+	droq_int_enb = octeon_read_csr(oct, CN6XXX_SLI_PKT_TIME_INT_ENB);
+	droq_mask |= (droq_time_mask & droq_int_enb);
+
+	droq_mask &= oct->io_qmask.oq;
+
+	oct->droq_intr = 0;
+
+	/* for (oq_no = 0; oq_no < oct->num_oqs; oq_no++) { */
+	for (oq_no = 0; oq_no < MAX_OCTEON_OUTPUT_QUEUES; oq_no++) {
+		if (!(droq_mask & (1 << oq_no)))
+			continue;
+
+		droq = oct->droq[oq_no];
+		pkt_count = octeon_droq_check_hw_for_pkts(oct, droq);
+		if (pkt_count) {
+			oct->droq_intr |= (1ULL << oq_no);
+			if (droq->ops.poll_mode) {
+				u32 value;
+				u32 reg;
+
+				struct octeon_cn6xxx *cn6xxx =
+					(struct octeon_cn6xxx *)oct->chip;
+
+				/* disable interrupts for this droq */
+				spin_lock
+					(&cn6xxx->lock_for_droq_int_enb_reg);
+				reg = CN6XXX_SLI_PKT_TIME_INT_ENB;
+				value = octeon_read_csr(oct, reg);
+				value &= ~(1 << oq_no);
+				octeon_write_csr(oct, reg, value);
+				reg = CN6XXX_SLI_PKT_CNT_INT_ENB;
+				value = octeon_read_csr(oct, reg);
+				value &= ~(1 << oq_no);
+				octeon_write_csr(oct, reg, value);
+
+				/* Ensure that the enable register is written.
+				 */
+				mmiowb();
+
+				spin_unlock(&cn6xxx->lock_for_droq_int_enb_reg);
+			}
+		}
+	}
+
+	droq_time_mask &= oct->io_qmask.oq;
+	droq_cnt_mask &= oct->io_qmask.oq;
+
+	/* Reset the PKT_CNT/TIME_INT registers. */
+	if (droq_time_mask)
+		octeon_write_csr(oct, CN6XXX_SLI_PKT_TIME_INT, droq_time_mask);
+
+	if (droq_cnt_mask)      /* reset PKT_CNT register:66xx */
+		octeon_write_csr(oct, CN6XXX_SLI_PKT_CNT_INT, droq_cnt_mask);
+
+	return 0;
+}
+
+irqreturn_t lio_cn6xxx_process_interrupt_regs(void *dev)
+{
+	struct octeon_device *oct = (struct octeon_device *)dev;
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->chip;
+	u64 intr64;
+
+	intr64 = readq(cn6xxx->intr_sum_reg64);
+
+	/* If our device has interrupted, then proceed.
+	 * Also check for all f's if interrupt was triggered on an error
+	 * and the PCI read fails.
+	 */
+	if (!intr64 || (intr64 == 0xFFFFFFFFFFFFFFFFULL))
+		return IRQ_NONE;
+
+	oct->int_status = 0;
+
+	if (intr64 & CN6XXX_INTR_ERR)
+		lio_cn6xxx_process_pcie_error_intr(oct, intr64);
+
+	if (intr64 & CN6XXX_INTR_PKT_DATA) {
+		lio_cn6xxx_process_droq_intr_regs(oct);
+		oct->int_status |= OCT_DEV_INTR_PKT_DATA;
+	}
+
+	if (intr64 & CN6XXX_INTR_DMA0_FORCE)
+		oct->int_status |= OCT_DEV_INTR_DMA0_FORCE;
+
+	if (intr64 & CN6XXX_INTR_DMA1_FORCE)
+		oct->int_status |= OCT_DEV_INTR_DMA1_FORCE;
+
+	/* Clear the current interrupts */
+	writeq(intr64, cn6xxx->intr_sum_reg64);
+
+	return IRQ_HANDLED;
+}
+
+void lio_cn6xxx_setup_reg_address(struct octeon_device *oct,
+				  void *chip,
+				  struct octeon_reg_list *reg_list)
+{
+	u8 __iomem *bar0_pciaddr = oct->mmio[0].hw_addr;
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)chip;
+
+	reg_list->pci_win_wr_addr_hi =
+		(u32 __iomem *)(bar0_pciaddr + CN6XXX_WIN_WR_ADDR_HI);
+	reg_list->pci_win_wr_addr_lo =
+		(u32 __iomem *)(bar0_pciaddr + CN6XXX_WIN_WR_ADDR_LO);
+	reg_list->pci_win_wr_addr =
+		(u64 __iomem *)(bar0_pciaddr + CN6XXX_WIN_WR_ADDR64);
+
+	reg_list->pci_win_rd_addr_hi =
+		(u32 __iomem *)(bar0_pciaddr + CN6XXX_WIN_RD_ADDR_HI);
+	reg_list->pci_win_rd_addr_lo =
+		(u32 __iomem *)(bar0_pciaddr + CN6XXX_WIN_RD_ADDR_LO);
+	reg_list->pci_win_rd_addr =
+		(u64 __iomem *)(bar0_pciaddr + CN6XXX_WIN_RD_ADDR64);
+
+	reg_list->pci_win_wr_data_hi =
+		(u32 __iomem *)(bar0_pciaddr + CN6XXX_WIN_WR_DATA_HI);
+	reg_list->pci_win_wr_data_lo =
+		(u32 __iomem *)(bar0_pciaddr + CN6XXX_WIN_WR_DATA_LO);
+	reg_list->pci_win_wr_data =
+		(u64 __iomem *)(bar0_pciaddr + CN6XXX_WIN_WR_DATA64);
+
+	reg_list->pci_win_rd_data_hi =
+		(u32 __iomem *)(bar0_pciaddr + CN6XXX_WIN_RD_DATA_HI);
+	reg_list->pci_win_rd_data_lo =
+		(u32 __iomem *)(bar0_pciaddr + CN6XXX_WIN_RD_DATA_LO);
+	reg_list->pci_win_rd_data =
+		(u64 __iomem *)(bar0_pciaddr + CN6XXX_WIN_RD_DATA64);
+
+	lio_cn6xxx_get_pcie_qlmport(oct);
+
+	cn6xxx->intr_sum_reg64 = bar0_pciaddr + CN6XXX_SLI_INT_SUM64;
+	cn6xxx->intr_mask64 = CN6XXX_INTR_MASK;
+	cn6xxx->intr_enb_reg64 =
+		bar0_pciaddr + CN6XXX_SLI_INT_ENB64(oct->pcie_port);
+}
+
+int lio_setup_cn66xx_octeon_device(struct octeon_device *oct)
+{
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->chip;
+
+	if (octeon_map_pci_barx(oct, 0, 0))
+		return 1;
+
+	if (octeon_map_pci_barx(oct, 1, MAX_BAR1_IOREMAP_SIZE)) {
+		dev_err(&oct->pci_dev->dev, "%s CN66XX BAR1 map failed\n",
+			__func__);
+		octeon_unmap_pci_barx(oct, 0);
+		return 1;
+	}
+
+	spin_lock_init(&cn6xxx->lock_for_droq_int_enb_reg);
+
+	oct->fn_list.setup_iq_regs = lio_cn66xx_setup_iq_regs;
+	oct->fn_list.setup_oq_regs = lio_cn6xxx_setup_oq_regs;
+
+	oct->fn_list.soft_reset = lio_cn6xxx_soft_reset;
+	oct->fn_list.setup_device_regs = lio_cn6xxx_setup_device_regs;
+	oct->fn_list.reinit_regs = lio_cn6xxx_reinit_regs;
+	oct->fn_list.update_iq_read_idx = lio_cn6xxx_update_read_index;
+
+	oct->fn_list.bar1_idx_setup = lio_cn6xxx_bar1_idx_setup;
+	oct->fn_list.bar1_idx_write = lio_cn6xxx_bar1_idx_write;
+	oct->fn_list.bar1_idx_read = lio_cn6xxx_bar1_idx_read;
+
+	oct->fn_list.process_interrupt_regs = lio_cn6xxx_process_interrupt_regs;
+	oct->fn_list.enable_interrupt = lio_cn6xxx_enable_interrupt;
+	oct->fn_list.disable_interrupt = lio_cn6xxx_disable_interrupt;
+
+	oct->fn_list.enable_io_queues = lio_cn6xxx_enable_io_queues;
+	oct->fn_list.disable_io_queues = lio_cn6xxx_disable_io_queues;
+
+	lio_cn6xxx_setup_reg_address(oct, oct->chip, &oct->reg_list);
+
+	cn6xxx->conf = (struct octeon_config *)
+		       oct_get_config_info(oct, LIO_210SV);
+	if (!cn6xxx->conf) {
+		dev_err(&oct->pci_dev->dev, "%s No Config found for CN66XX\n",
+			__func__);
+		octeon_unmap_pci_barx(oct, 0);
+		octeon_unmap_pci_barx(oct, 1);
+		return 1;
+	}
+
+	oct->coproc_clock_rate = 1000000ULL * lio_cn6xxx_coprocessor_clock(oct);
+
+	return 0;
+}
+
+int lio_validate_cn6xxx_config_info(struct octeon_device *oct,
+				    struct octeon_config *conf6xxx)
+{
+	/* int total_instrs = 0; */
+
+	if (CFG_GET_IQ_MAX_Q(conf6xxx) > CN6XXX_MAX_INPUT_QUEUES) {
+		dev_err(&oct->pci_dev->dev, "%s: Num IQ (%d) exceeds Max (%d)\n",
+			__func__, CFG_GET_IQ_MAX_Q(conf6xxx),
+			CN6XXX_MAX_INPUT_QUEUES);
+		return 1;
+	}
+
+	if (CFG_GET_OQ_MAX_Q(conf6xxx) > CN6XXX_MAX_OUTPUT_QUEUES) {
+		dev_err(&oct->pci_dev->dev, "%s: Num OQ (%d) exceeds Max (%d)\n",
+			__func__, CFG_GET_OQ_MAX_Q(conf6xxx),
+			CN6XXX_MAX_OUTPUT_QUEUES);
+		return 1;
+	}
+
+	if (CFG_GET_IQ_INSTR_TYPE(conf6xxx) != OCTEON_32BYTE_INSTR &&
+	    CFG_GET_IQ_INSTR_TYPE(conf6xxx) != OCTEON_64BYTE_INSTR) {
+		dev_err(&oct->pci_dev->dev, "%s: Invalid instr type for IQ\n",
+			__func__);
+		return 1;
+	}
+	if (!(CFG_GET_OQ_INFO_PTR(conf6xxx)) ||
+	    !(CFG_GET_OQ_REFILL_THRESHOLD(conf6xxx))) {
+		dev_err(&oct->pci_dev->dev, "%s: Invalid parameter for OQ\n",
+			__func__);
+		return 1;
+	}
+
+	if (!(CFG_GET_OQ_INTR_TIME(conf6xxx))) {
+		dev_err(&oct->pci_dev->dev, "%s: No Time Interrupt for OQ\n",
+			__func__);
+		return 1;
+	}
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/cn66xx_device.h b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.h
new file mode 100644
index 000000000000..f77918779355
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn66xx_device.h
@@ -0,0 +1,107 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+
+/*! \file  cn66xx_device.h
+ *  \brief Host Driver: Routines that perform CN66XX specific operations.
+ */
+
+#ifndef __CN66XX_DEVICE_H__
+#define  __CN66XX_DEVICE_H__
+
+/* Register address and configuration for a CN6XXX devices.
+ * If device specific changes need to be made then add a struct to include
+ * device specific fields as shown in the commented section
+ */
+struct octeon_cn6xxx {
+	/** PCI interrupt summary register */
+	u8 __iomem *intr_sum_reg64;
+
+	/** PCI interrupt enable register */
+	u8 __iomem *intr_enb_reg64;
+
+	/** The PCI interrupt mask used by interrupt handler */
+	u64 intr_mask64;
+
+	struct octeon_config *conf;
+
+	/* Example additional fields - not used currently
+	 *  struct {
+	 *  }cn6xyz;
+	 */
+
+	/* For the purpose of atomic access to interrupt enable reg */
+	spinlock_t lock_for_droq_int_enb_reg;
+
+};
+
+enum octeon_pcie_mps {
+	PCIE_MPS_DEFAULT = -1,	/* Use the default setup by BIOS */
+	PCIE_MPS_128B = 0,
+	PCIE_MPS_256B = 1
+};
+
+enum octeon_pcie_mrrs {
+	PCIE_MRRS_DEFAULT = -1,	/* Use the default setup by BIOS */
+	PCIE_MRRS_128B = 0,
+	PCIE_MRRS_256B = 1,
+	PCIE_MRRS_512B = 2,
+	PCIE_MRRS_1024B = 3,
+	PCIE_MRRS_2048B = 4,
+	PCIE_MRRS_4096B = 5
+};
+
+/* Common functions for 66xx and 68xx */
+int lio_cn6xxx_soft_reset(struct octeon_device *oct);
+void lio_cn6xxx_enable_error_reporting(struct octeon_device *oct);
+void lio_cn6xxx_setup_pcie_mps(struct octeon_device *oct,
+			       enum octeon_pcie_mps mps);
+void lio_cn6xxx_setup_pcie_mrrs(struct octeon_device *oct,
+				enum octeon_pcie_mrrs mrrs);
+void lio_cn6xxx_setup_global_input_regs(struct octeon_device *oct);
+void lio_cn6xxx_setup_global_output_regs(struct octeon_device *oct);
+void lio_cn6xxx_setup_iq_regs(struct octeon_device *oct, u32 iq_no);
+void lio_cn6xxx_setup_oq_regs(struct octeon_device *oct, u32 oq_no);
+void lio_cn6xxx_enable_io_queues(struct octeon_device *oct);
+void lio_cn6xxx_disable_io_queues(struct octeon_device *oct);
+void lio_cn6xxx_process_pcie_error_intr(struct octeon_device *oct, u64 intr64);
+int lio_cn6xxx_process_droq_intr_regs(struct octeon_device *oct);
+irqreturn_t lio_cn6xxx_process_interrupt_regs(void *dev);
+void lio_cn6xxx_reinit_regs(struct octeon_device *oct);
+void lio_cn6xxx_bar1_idx_setup(struct octeon_device *oct, u64 core_addr,
+			       u32 idx, int valid);
+void lio_cn6xxx_bar1_idx_write(struct octeon_device *oct, u32 idx, u32 mask);
+u32 lio_cn6xxx_bar1_idx_read(struct octeon_device *oct, u32 idx);
+u32
+lio_cn6xxx_update_read_index(struct octeon_device *oct __attribute__((unused)),
+			     struct octeon_instr_queue *iq);
+void lio_cn6xxx_enable_interrupt(void *chip);
+void lio_cn6xxx_disable_interrupt(void *chip);
+void cn6xxx_get_pcie_qlmport(struct octeon_device *oct);
+void lio_cn6xxx_setup_reg_address(struct octeon_device *oct, void *chip,
+				  struct octeon_reg_list *reg_list);
+u32 lio_cn6xxx_coprocessor_clock(struct octeon_device *oct);
+u32 lio_cn6xxx_get_oq_ticks(struct octeon_device *oct, u32 time_intr_in_us);
+int lio_setup_cn66xx_octeon_device(struct octeon_device *);
+int lio_validate_cn6xxx_config_info(struct octeon_device *oct,
+				    struct octeon_config *);
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/cn66xx_regs.h b/drivers/net/ethernet/cavium/liquidio/cn66xx_regs.h
new file mode 100644
index 000000000000..5e3aff242ad3
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn66xx_regs.h
@@ -0,0 +1,535 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+
+/*! \file cn66xx_regs.h
+ *  \brief Host Driver: Register Address and Register Mask values for
+ *  Octeon CN66XX devices.
+ */
+
+#ifndef __CN66XX_REGS_H__
+#define __CN66XX_REGS_H__
+
+#define     CN6XXX_XPANSION_BAR             0x30
+
+#define     CN6XXX_MSI_CAP                  0x50
+#define     CN6XXX_MSI_ADDR_LO              0x54
+#define     CN6XXX_MSI_ADDR_HI              0x58
+#define     CN6XXX_MSI_DATA                 0x5C
+
+#define     CN6XXX_PCIE_CAP                 0x70
+#define     CN6XXX_PCIE_DEVCAP              0x74
+#define     CN6XXX_PCIE_DEVCTL              0x78
+#define     CN6XXX_PCIE_LINKCAP             0x7C
+#define     CN6XXX_PCIE_LINKCTL             0x80
+#define     CN6XXX_PCIE_SLOTCAP             0x84
+#define     CN6XXX_PCIE_SLOTCTL             0x88
+
+#define     CN6XXX_PCIE_ENH_CAP             0x100
+#define     CN6XXX_PCIE_UNCORR_ERR_STATUS   0x104
+#define     CN6XXX_PCIE_UNCORR_ERR_MASK     0x108
+#define     CN6XXX_PCIE_UNCORR_ERR          0x10C
+#define     CN6XXX_PCIE_CORR_ERR_STATUS     0x110
+#define     CN6XXX_PCIE_CORR_ERR_MASK       0x114
+#define     CN6XXX_PCIE_ADV_ERR_CAP         0x118
+
+#define     CN6XXX_PCIE_ACK_REPLAY_TIMER    0x700
+#define     CN6XXX_PCIE_OTHER_MSG           0x704
+#define     CN6XXX_PCIE_PORT_FORCE_LINK     0x708
+#define     CN6XXX_PCIE_ACK_FREQ            0x70C
+#define     CN6XXX_PCIE_PORT_LINK_CTL       0x710
+#define     CN6XXX_PCIE_LANE_SKEW           0x714
+#define     CN6XXX_PCIE_SYM_NUM             0x718
+#define     CN6XXX_PCIE_FLTMSK              0x720
+
+/* ##############  BAR0 Registers ################  */
+
+#define    CN6XXX_SLI_CTL_PORT0                    0x0050
+#define    CN6XXX_SLI_CTL_PORT1                    0x0060
+
+#define    CN6XXX_SLI_WINDOW_CTL                   0x02E0
+#define    CN6XXX_SLI_DBG_DATA                     0x0310
+#define    CN6XXX_SLI_SCRATCH1                     0x03C0
+#define    CN6XXX_SLI_SCRATCH2                     0x03D0
+#define    CN6XXX_SLI_CTL_STATUS                   0x0570
+
+#define    CN6XXX_WIN_WR_ADDR_LO                   0x0000
+#define    CN6XXX_WIN_WR_ADDR_HI                   0x0004
+#define    CN6XXX_WIN_WR_ADDR64                    CN6XXX_WIN_WR_ADDR_LO
+
+#define    CN6XXX_WIN_RD_ADDR_LO                   0x0010
+#define    CN6XXX_WIN_RD_ADDR_HI                   0x0014
+#define    CN6XXX_WIN_RD_ADDR64                    CN6XXX_WIN_RD_ADDR_LO
+
+#define    CN6XXX_WIN_WR_DATA_LO                   0x0020
+#define    CN6XXX_WIN_WR_DATA_HI                   0x0024
+#define    CN6XXX_WIN_WR_DATA64                    CN6XXX_WIN_WR_DATA_LO
+
+#define    CN6XXX_WIN_RD_DATA_LO                   0x0040
+#define    CN6XXX_WIN_RD_DATA_HI                   0x0044
+#define    CN6XXX_WIN_RD_DATA64                    CN6XXX_WIN_RD_DATA_LO
+
+#define    CN6XXX_WIN_WR_MASK_LO                   0x0030
+#define    CN6XXX_WIN_WR_MASK_HI                   0x0034
+#define    CN6XXX_WIN_WR_MASK_REG                  CN6XXX_WIN_WR_MASK_LO
+
+/* 1 register (32-bit) to enable Input queues */
+#define    CN6XXX_SLI_PKT_INSTR_ENB               0x1000
+
+/* 1 register (32-bit) to enable Output queues */
+#define    CN6XXX_SLI_PKT_OUT_ENB                 0x1010
+
+/* 1 register (32-bit) to determine whether Output queues are in reset. */
+#define    CN6XXX_SLI_PORT_IN_RST_OQ              0x11F0
+
+/* 1 register (32-bit) to determine whether Input queues are in reset. */
+#define    CN6XXX_SLI_PORT_IN_RST_IQ              0x11F4
+
+/*###################### REQUEST QUEUE #########################*/
+
+/* 1 register (32-bit) - instr. size of each input queue. */
+#define    CN6XXX_SLI_PKT_INSTR_SIZE             0x1020
+
+/* 32 registers for Input Queue Instr Count - SLI_PKT_IN_DONE0_CNTS */
+#define    CN6XXX_SLI_IQ_INSTR_COUNT_START       0x2000
+
+/* 32 registers for Input Queue Start Addr - SLI_PKT0_INSTR_BADDR */
+#define    CN6XXX_SLI_IQ_BASE_ADDR_START64       0x2800
+
+/* 32 registers for Input Doorbell - SLI_PKT0_INSTR_BAOFF_DBELL */
+#define    CN6XXX_SLI_IQ_DOORBELL_START          0x2C00
+
+/* 32 registers for Input Queue size - SLI_PKT0_INSTR_FIFO_RSIZE */
+#define    CN6XXX_SLI_IQ_SIZE_START              0x3000
+
+/* 32 registers for Instruction Header Options - SLI_PKT0_INSTR_HEADER */
+#define    CN6XXX_SLI_IQ_PKT_INSTR_HDR_START64   0x3400
+
+/* 1 register (64-bit) - Back Pressure for each input queue - SLI_PKT0_IN_BP */
+#define    CN66XX_SLI_INPUT_BP_START64           0x3800
+
+/* Each Input Queue register is at a 16-byte Offset in BAR0 */
+#define    CN6XXX_IQ_OFFSET                      0x10
+
+/* 1 register (32-bit) - ES, RO, NS, Arbitration for Input Queue Data &
+ * gather list fetches. SLI_PKT_INPUT_CONTROL.
+ */
+#define    CN6XXX_SLI_PKT_INPUT_CONTROL          0x1170
+
+/* 1 register (64-bit) - Number of instructions to read at one time
+ * - 2 bits for each input ring. SLI_PKT_INSTR_RD_SIZE.
+ */
+#define    CN6XXX_SLI_PKT_INSTR_RD_SIZE          0x11A0
+
+/* 1 register (64-bit) - Assign Input ring to MAC port
+ * - 2 bits for each input ring. SLI_PKT_IN_PCIE_PORT.
+ */
+#define    CN6XXX_SLI_IN_PCIE_PORT               0x11B0
+
+/*------- Request Queue Macros ---------*/
+#define    CN6XXX_SLI_IQ_BASE_ADDR64(iq)          \
+	(CN6XXX_SLI_IQ_BASE_ADDR_START64 + ((iq) * CN6XXX_IQ_OFFSET))
+
+#define    CN6XXX_SLI_IQ_SIZE(iq)                 \
+	(CN6XXX_SLI_IQ_SIZE_START + ((iq) * CN6XXX_IQ_OFFSET))
+
+#define    CN6XXX_SLI_IQ_PKT_INSTR_HDR64(iq)      \
+	(CN6XXX_SLI_IQ_PKT_INSTR_HDR_START64 + ((iq) * CN6XXX_IQ_OFFSET))
+
+#define    CN6XXX_SLI_IQ_DOORBELL(iq)             \
+	(CN6XXX_SLI_IQ_DOORBELL_START + ((iq) * CN6XXX_IQ_OFFSET))
+
+#define    CN6XXX_SLI_IQ_INSTR_COUNT(iq)          \
+	(CN6XXX_SLI_IQ_INSTR_COUNT_START + ((iq) * CN6XXX_IQ_OFFSET))
+
+#define    CN66XX_SLI_IQ_BP64(iq)                 \
+	(CN66XX_SLI_INPUT_BP_START64 + ((iq) * CN6XXX_IQ_OFFSET))
+
+/*------------------ Masks ----------------*/
+#define    CN6XXX_INPUT_CTL_ROUND_ROBIN_ARB         BIT(22)
+#define    CN6XXX_INPUT_CTL_DATA_NS                 BIT(8)
+#define    CN6XXX_INPUT_CTL_DATA_ES_64B_SWAP        BIT(6)
+#define    CN6XXX_INPUT_CTL_DATA_RO                 BIT(5)
+#define    CN6XXX_INPUT_CTL_USE_CSR                 BIT(4)
+#define    CN6XXX_INPUT_CTL_GATHER_NS               BIT(3)
+#define    CN6XXX_INPUT_CTL_GATHER_ES_64B_SWAP      BIT(2)
+#define    CN6XXX_INPUT_CTL_GATHER_RO               BIT(1)
+
+#ifdef __BIG_ENDIAN_BITFIELD
+#define    CN6XXX_INPUT_CTL_MASK                    \
+	(CN6XXX_INPUT_CTL_DATA_ES_64B_SWAP      \
+	  | CN6XXX_INPUT_CTL_USE_CSR              \
+	  | CN6XXX_INPUT_CTL_GATHER_ES_64B_SWAP)
+#else
+#define    CN6XXX_INPUT_CTL_MASK                    \
+	(CN6XXX_INPUT_CTL_DATA_ES_64B_SWAP     \
+	  | CN6XXX_INPUT_CTL_USE_CSR)
+#endif
+
+/*############################ OUTPUT QUEUE #########################*/
+
+/* 32 registers for Output queue buffer and info size - SLI_PKT0_OUT_SIZE */
+#define    CN6XXX_SLI_OQ0_BUFF_INFO_SIZE         0x0C00
+
+/* 32 registers for Output Queue Start Addr - SLI_PKT0_SLIST_BADDR */
+#define    CN6XXX_SLI_OQ_BASE_ADDR_START64       0x1400
+
+/* 32 registers for Output Queue Packet Credits - SLI_PKT0_SLIST_BAOFF_DBELL */
+#define    CN6XXX_SLI_OQ_PKT_CREDITS_START       0x1800
+
+/* 32 registers for Output Queue size - SLI_PKT0_SLIST_FIFO_RSIZE */
+#define    CN6XXX_SLI_OQ_SIZE_START              0x1C00
+
+/* 32 registers for Output Queue Packet Count - SLI_PKT0_CNTS */
+#define    CN6XXX_SLI_OQ_PKT_SENT_START          0x2400
+
+/* Each Output Queue register is at a 16-byte Offset in BAR0 */
+#define    CN6XXX_OQ_OFFSET                      0x10
+
+/* 1 register (32-bit) - 1 bit for each output queue
+ * - Relaxed Ordering setting for reading Output Queues descriptors
+ * - SLI_PKT_SLIST_ROR
+ */
+#define    CN6XXX_SLI_PKT_SLIST_ROR              0x1030
+
+/* 1 register (32-bit) - 1 bit for each output queue
+ * - No Snoop mode for reading Output Queues descriptors
+ * - SLI_PKT_SLIST_NS
+ */
+#define    CN6XXX_SLI_PKT_SLIST_NS               0x1040
+
+/* 1 register (64-bit) - 2 bits for each output queue
+ * - Endian-Swap mode for reading Output Queue descriptors
+ * - SLI_PKT_SLIST_ES
+ */
+#define    CN6XXX_SLI_PKT_SLIST_ES64             0x1050
+
+/* 1 register (32-bit) - 1 bit for each output queue
+ * - InfoPtr mode for Output Queues.
+ * - SLI_PKT_IPTR
+ */
+#define    CN6XXX_SLI_PKT_IPTR                   0x1070
+
+/* 1 register (32-bit) - 1 bit for each output queue
+ * - DPTR format selector for Output queues.
+ * - SLI_PKT_DPADDR
+ */
+#define    CN6XXX_SLI_PKT_DPADDR                 0x1080
+
+/* 1 register (32-bit) - 1 bit for each output queue
+ * - Relaxed Ordering setting for reading Output Queues data
+ * - SLI_PKT_DATA_OUT_ROR
+ */
+#define    CN6XXX_SLI_PKT_DATA_OUT_ROR           0x1090
+
+/* 1 register (32-bit) - 1 bit for each output queue
+ * - No Snoop mode for reading Output Queues data
+ * - SLI_PKT_DATA_OUT_NS
+ */
+#define    CN6XXX_SLI_PKT_DATA_OUT_NS            0x10A0
+
+/* 1 register (64-bit)  - 2 bits for each output queue
+ * - Endian-Swap mode for reading Output Queue data
+ * - SLI_PKT_DATA_OUT_ES
+ */
+#define    CN6XXX_SLI_PKT_DATA_OUT_ES64          0x10B0
+
+/* 1 register (32-bit) - 1 bit for each output queue
+ * - Controls whether SLI_PKTn_CNTS is incremented for bytes or for packets.
+ * - SLI_PKT_OUT_BMODE
+ */
+#define    CN6XXX_SLI_PKT_OUT_BMODE              0x10D0
+
+/* 1 register (64-bit) - 2 bits for each output queue
+ * - Assign PCIE port for Output queues
+ * - SLI_PKT_PCIE_PORT.
+ */
+#define    CN6XXX_SLI_PKT_PCIE_PORT64            0x10E0
+
+/* 1 (64-bit) register for Output Queue Packet Count Interrupt Threshold
+ * & Time Threshold. The same setting applies to all 32 queues.
+ * The register is defined as a 64-bit registers, but we use the
+ * 32-bit offsets to define distinct addresses.
+ */
+#define    CN6XXX_SLI_OQ_INT_LEVEL_PKTS          0x1120
+#define    CN6XXX_SLI_OQ_INT_LEVEL_TIME          0x1124
+
+/* 1 (64-bit register) for Output Queue backpressure across all rings. */
+#define    CN6XXX_SLI_OQ_WMARK                   0x1180
+
+/* 1 register to control output queue global backpressure & ring enable. */
+#define    CN6XXX_SLI_PKT_CTL                    0x1220
+
+/*------- Output Queue Macros ---------*/
+#define    CN6XXX_SLI_OQ_BASE_ADDR64(oq)          \
+	(CN6XXX_SLI_OQ_BASE_ADDR_START64 + ((oq) * CN6XXX_OQ_OFFSET))
+
+#define    CN6XXX_SLI_OQ_SIZE(oq)                 \
+	(CN6XXX_SLI_OQ_SIZE_START + ((oq) * CN6XXX_OQ_OFFSET))
+
+#define    CN6XXX_SLI_OQ_BUFF_INFO_SIZE(oq)                 \
+	(CN6XXX_SLI_OQ0_BUFF_INFO_SIZE + ((oq) * CN6XXX_OQ_OFFSET))
+
+#define    CN6XXX_SLI_OQ_PKTS_SENT(oq)            \
+	(CN6XXX_SLI_OQ_PKT_SENT_START + ((oq) * CN6XXX_OQ_OFFSET))
+
+#define    CN6XXX_SLI_OQ_PKTS_CREDIT(oq)          \
+	(CN6XXX_SLI_OQ_PKT_CREDITS_START + ((oq) * CN6XXX_OQ_OFFSET))
+
+/*######################### DMA Counters #########################*/
+
+/* 2 registers (64-bit) - DMA Count - 1 for each DMA counter 0/1. */
+#define    CN6XXX_DMA_CNT_START                   0x0400
+
+/* 2 registers (64-bit) - DMA Timer 0/1, contains DMA timer values
+ * SLI_DMA_0_TIM
+ */
+#define    CN6XXX_DMA_TIM_START                   0x0420
+
+/* 2 registers (64-bit) - DMA count & Time Interrupt threshold -
+ * SLI_DMA_0_INT_LEVEL
+ */
+#define    CN6XXX_DMA_INT_LEVEL_START             0x03E0
+
+/* Each DMA register is at a 16-byte Offset in BAR0 */
+#define    CN6XXX_DMA_OFFSET                      0x10
+
+/*---------- DMA Counter Macros ---------*/
+#define    CN6XXX_DMA_CNT(dq)                      \
+	(CN6XXX_DMA_CNT_START + ((dq) * CN6XXX_DMA_OFFSET))
+
+#define    CN6XXX_DMA_INT_LEVEL(dq)                \
+	(CN6XXX_DMA_INT_LEVEL_START + ((dq) * CN6XXX_DMA_OFFSET))
+
+#define    CN6XXX_DMA_PKT_INT_LEVEL(dq)            \
+	(CN6XXX_DMA_INT_LEVEL_START + ((dq) * CN6XXX_DMA_OFFSET))
+
+#define    CN6XXX_DMA_TIME_INT_LEVEL(dq)           \
+	(CN6XXX_DMA_INT_LEVEL_START + 4 + ((dq) * CN6XXX_DMA_OFFSET))
+
+#define    CN6XXX_DMA_TIM(dq)                      \
+	(CN6XXX_DMA_TIM_START + ((dq) * CN6XXX_DMA_OFFSET))
+
+/*######################## INTERRUPTS #########################*/
+
+/* 1 register (64-bit) for Interrupt Summary */
+#define    CN6XXX_SLI_INT_SUM64                  0x0330
+
+/* 1 register (64-bit) for Interrupt Enable */
+#define    CN6XXX_SLI_INT_ENB64_PORT0            0x0340
+#define    CN6XXX_SLI_INT_ENB64_PORT1            0x0350
+
+/* 1 register (32-bit) to enable Output Queue Packet/Byte Count Interrupt */
+#define    CN6XXX_SLI_PKT_CNT_INT_ENB            0x1150
+
+/* 1 register (32-bit) to enable Output Queue Packet Timer Interrupt */
+#define    CN6XXX_SLI_PKT_TIME_INT_ENB           0x1160
+
+/* 1 register (32-bit) to indicate which Output Queue reached pkt threshold */
+#define    CN6XXX_SLI_PKT_CNT_INT                0x1130
+
+/* 1 register (32-bit) to indicate which Output Queue reached time threshold */
+#define    CN6XXX_SLI_PKT_TIME_INT               0x1140
+
+/*------------------ Interrupt Masks ----------------*/
+
+#define    CN6XXX_INTR_RML_TIMEOUT_ERR           BIT(1)
+#define    CN6XXX_INTR_BAR0_RW_TIMEOUT_ERR       BIT(2)
+#define    CN6XXX_INTR_IO2BIG_ERR                BIT(3)
+#define    CN6XXX_INTR_PKT_COUNT                 BIT(4)
+#define    CN6XXX_INTR_PKT_TIME                  BIT(5)
+#define    CN6XXX_INTR_M0UPB0_ERR                BIT(8)
+#define    CN6XXX_INTR_M0UPWI_ERR                BIT(9)
+#define    CN6XXX_INTR_M0UNB0_ERR                BIT(10)
+#define    CN6XXX_INTR_M0UNWI_ERR                BIT(11)
+#define    CN6XXX_INTR_M1UPB0_ERR                BIT(12)
+#define    CN6XXX_INTR_M1UPWI_ERR                BIT(13)
+#define    CN6XXX_INTR_M1UNB0_ERR                BIT(14)
+#define    CN6XXX_INTR_M1UNWI_ERR                BIT(15)
+#define    CN6XXX_INTR_MIO_INT0                  BIT(16)
+#define    CN6XXX_INTR_MIO_INT1                  BIT(17)
+#define    CN6XXX_INTR_MAC_INT0                  BIT(18)
+#define    CN6XXX_INTR_MAC_INT1                  BIT(19)
+
+#define    CN6XXX_INTR_DMA0_FORCE                BIT_ULL(32)
+#define    CN6XXX_INTR_DMA1_FORCE                BIT_ULL(33)
+#define    CN6XXX_INTR_DMA0_COUNT                BIT_ULL(34)
+#define    CN6XXX_INTR_DMA1_COUNT                BIT_ULL(35)
+#define    CN6XXX_INTR_DMA0_TIME                 BIT_ULL(36)
+#define    CN6XXX_INTR_DMA1_TIME                 BIT_ULL(37)
+#define    CN6XXX_INTR_INSTR_DB_OF_ERR           BIT_ULL(48)
+#define    CN6XXX_INTR_SLIST_DB_OF_ERR           BIT_ULL(49)
+#define    CN6XXX_INTR_POUT_ERR                  BIT_ULL(50)
+#define    CN6XXX_INTR_PIN_BP_ERR                BIT_ULL(51)
+#define    CN6XXX_INTR_PGL_ERR                   BIT_ULL(52)
+#define    CN6XXX_INTR_PDI_ERR                   BIT_ULL(53)
+#define    CN6XXX_INTR_POP_ERR                   BIT_ULL(54)
+#define    CN6XXX_INTR_PINS_ERR                  BIT_ULL(55)
+#define    CN6XXX_INTR_SPRT0_ERR                 BIT_ULL(56)
+#define    CN6XXX_INTR_SPRT1_ERR                 BIT_ULL(57)
+#define    CN6XXX_INTR_ILL_PAD_ERR               BIT_ULL(60)
+
+#define    CN6XXX_INTR_DMA0_DATA                 (CN6XXX_INTR_DMA0_TIME)
+
+#define    CN6XXX_INTR_DMA1_DATA                 (CN6XXX_INTR_DMA1_TIME)
+
+#define    CN6XXX_INTR_DMA_DATA                  \
+	(CN6XXX_INTR_DMA0_DATA | CN6XXX_INTR_DMA1_DATA)
+
+#define    CN6XXX_INTR_PKT_DATA                  (CN6XXX_INTR_PKT_TIME | \
+						  CN6XXX_INTR_PKT_COUNT)
+
+/* Sum of interrupts for all PCI-Express Data Interrupts */
+#define    CN6XXX_INTR_PCIE_DATA                 \
+	(CN6XXX_INTR_DMA_DATA | CN6XXX_INTR_PKT_DATA)
+
+#define    CN6XXX_INTR_MIO                       \
+	(CN6XXX_INTR_MIO_INT0 | CN6XXX_INTR_MIO_INT1)
+
+#define    CN6XXX_INTR_MAC                       \
+	(CN6XXX_INTR_MAC_INT0 | CN6XXX_INTR_MAC_INT1)
+
+/* Sum of interrupts for error events */
+#define    CN6XXX_INTR_ERR                       \
+	(CN6XXX_INTR_BAR0_RW_TIMEOUT_ERR    \
+	   | CN6XXX_INTR_IO2BIG_ERR             \
+	   | CN6XXX_INTR_M0UPB0_ERR             \
+	   | CN6XXX_INTR_M0UPWI_ERR             \
+	   | CN6XXX_INTR_M0UNB0_ERR             \
+	   | CN6XXX_INTR_M0UNWI_ERR             \
+	   | CN6XXX_INTR_M1UPB0_ERR             \
+	   | CN6XXX_INTR_M1UPWI_ERR             \
+	   | CN6XXX_INTR_M1UPB0_ERR             \
+	   | CN6XXX_INTR_M1UNWI_ERR             \
+	   | CN6XXX_INTR_INSTR_DB_OF_ERR        \
+	   | CN6XXX_INTR_SLIST_DB_OF_ERR        \
+	   | CN6XXX_INTR_POUT_ERR               \
+	   | CN6XXX_INTR_PIN_BP_ERR             \
+	   | CN6XXX_INTR_PGL_ERR                \
+	   | CN6XXX_INTR_PDI_ERR                \
+	   | CN6XXX_INTR_POP_ERR                \
+	   | CN6XXX_INTR_PINS_ERR               \
+	   | CN6XXX_INTR_SPRT0_ERR              \
+	   | CN6XXX_INTR_SPRT1_ERR              \
+	   | CN6XXX_INTR_ILL_PAD_ERR)
+
+/* Programmed Mask for Interrupt Sum */
+#define    CN6XXX_INTR_MASK                      \
+	(CN6XXX_INTR_PCIE_DATA              \
+	   | CN6XXX_INTR_DMA0_FORCE             \
+	   | CN6XXX_INTR_DMA1_FORCE             \
+	   | CN6XXX_INTR_MIO                    \
+	   | CN6XXX_INTR_MAC                    \
+	   | CN6XXX_INTR_ERR)
+
+#define    CN6XXX_SLI_S2M_PORT0_CTL              0x3D80
+#define    CN6XXX_SLI_S2M_PORT1_CTL              0x3D90
+#define    CN6XXX_SLI_S2M_PORTX_CTL(port)        \
+	(CN6XXX_SLI_S2M_PORT0_CTL + (port * 0x10))
+
+#define    CN6XXX_SLI_INT_ENB64(port)            \
+	(CN6XXX_SLI_INT_ENB64_PORT0 + (port * 0x10))
+
+#define    CN6XXX_SLI_MAC_NUMBER                 0x3E00
+
+/* CN6XXX BAR1 Index registers. */
+#define    CN6XXX_PEM_BAR1_INDEX000                0x00011800C00000A8ULL
+#define    CN6XXX_PEM_OFFSET                       0x0000000001000000ULL
+
+#define    CN6XXX_BAR1_INDEX_START                 CN6XXX_PEM_BAR1_INDEX000
+#define    CN6XXX_PCI_BAR1_OFFSET                  0x8
+
+#define    CN6XXX_BAR1_REG(idx, port) \
+		(CN6XXX_BAR1_INDEX_START + (port * CN6XXX_PEM_OFFSET) + \
+		(CN6XXX_PCI_BAR1_OFFSET * (idx)))
+
+/*############################ DPI #########################*/
+
+#define    CN6XXX_DPI_CTL                 0x0001df0000000040ULL
+
+#define    CN6XXX_DPI_DMA_CONTROL         0x0001df0000000048ULL
+
+#define    CN6XXX_DPI_REQ_GBL_ENB         0x0001df0000000050ULL
+
+#define    CN6XXX_DPI_REQ_ERR_RSP         0x0001df0000000058ULL
+
+#define    CN6XXX_DPI_REQ_ERR_RST         0x0001df0000000060ULL
+
+#define    CN6XXX_DPI_DMA_ENG0_ENB        0x0001df0000000080ULL
+
+#define    CN6XXX_DPI_DMA_ENG_ENB(q_no)   \
+	(CN6XXX_DPI_DMA_ENG0_ENB + (q_no * 8))
+
+#define    CN6XXX_DPI_DMA_ENG0_BUF        0x0001df0000000880ULL
+
+#define    CN6XXX_DPI_DMA_ENG_BUF(q_no)   \
+	(CN6XXX_DPI_DMA_ENG0_BUF + (q_no * 8))
+
+#define    CN6XXX_DPI_SLI_PRT0_CFG        0x0001df0000000900ULL
+#define    CN6XXX_DPI_SLI_PRT1_CFG        0x0001df0000000908ULL
+#define    CN6XXX_DPI_SLI_PRTX_CFG(port)        \
+	(CN6XXX_DPI_SLI_PRT0_CFG + (port * 0x10))
+
+#define    CN6XXX_DPI_DMA_COMMIT_MODE     BIT_ULL(58)
+#define    CN6XXX_DPI_DMA_PKT_HP          BIT_ULL(57)
+#define    CN6XXX_DPI_DMA_PKT_EN          BIT_ULL(56)
+#define    CN6XXX_DPI_DMA_O_ES            BIT_ULL(15)
+#define    CN6XXX_DPI_DMA_O_MODE          BIT_ULL(14)
+
+#define    CN6XXX_DPI_DMA_CTL_MASK             \
+	(CN6XXX_DPI_DMA_COMMIT_MODE    |    \
+	 CN6XXX_DPI_DMA_PKT_HP         |    \
+	 CN6XXX_DPI_DMA_PKT_EN         |    \
+	 CN6XXX_DPI_DMA_O_ES           |    \
+	 CN6XXX_DPI_DMA_O_MODE)
+
+/*############################ CIU #########################*/
+
+#define    CN6XXX_CIU_SOFT_BIST           0x0001070000000738ULL
+#define    CN6XXX_CIU_SOFT_RST            0x0001070000000740ULL
+
+/*############################ MIO #########################*/
+#define    CN6XXX_MIO_PTP_CLOCK_CFG       0x0001070000000f00ULL
+#define    CN6XXX_MIO_PTP_CLOCK_LO        0x0001070000000f08ULL
+#define    CN6XXX_MIO_PTP_CLOCK_HI        0x0001070000000f10ULL
+#define    CN6XXX_MIO_PTP_CLOCK_COMP      0x0001070000000f18ULL
+#define    CN6XXX_MIO_PTP_TIMESTAMP       0x0001070000000f20ULL
+#define    CN6XXX_MIO_PTP_EVT_CNT         0x0001070000000f28ULL
+#define    CN6XXX_MIO_PTP_CKOUT_THRESH_LO 0x0001070000000f30ULL
+#define    CN6XXX_MIO_PTP_CKOUT_THRESH_HI 0x0001070000000f38ULL
+#define    CN6XXX_MIO_PTP_CKOUT_HI_INCR   0x0001070000000f40ULL
+#define    CN6XXX_MIO_PTP_CKOUT_LO_INCR   0x0001070000000f48ULL
+#define    CN6XXX_MIO_PTP_PPS_THRESH_LO   0x0001070000000f50ULL
+#define    CN6XXX_MIO_PTP_PPS_THRESH_HI   0x0001070000000f58ULL
+#define    CN6XXX_MIO_PTP_PPS_HI_INCR     0x0001070000000f60ULL
+#define    CN6XXX_MIO_PTP_PPS_LO_INCR     0x0001070000000f68ULL
+
+#define    CN6XXX_MIO_QLM4_CFG            0x00011800000015B0ULL
+#define    CN6XXX_MIO_RST_BOOT            0x0001180000001600ULL
+
+#define    CN6XXX_MIO_QLM_CFG_MASK        0x7
+
+/*############################ LMC #########################*/
+
+#define    CN6XXX_LMC0_RESET_CTL               0x0001180088000180ULL
+#define    CN6XXX_LMC0_RESET_CTL_DDR3RST_MASK  0x0000000000000001ULL
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/cn68xx_device.c b/drivers/net/ethernet/cavium/liquidio/cn68xx_device.c
new file mode 100644
index 000000000000..8e830d0c0754
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn68xx_device.c
@@ -0,0 +1,198 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+#include "octeon_mem_ops.h"
+
+static void lio_cn68xx_set_dpi_regs(struct octeon_device *oct)
+{
+	u32 i;
+	u32 fifo_sizes[6] = { 3, 3, 1, 1, 1, 8 };
+
+	lio_pci_writeq(oct, CN6XXX_DPI_DMA_CTL_MASK, CN6XXX_DPI_DMA_CONTROL);
+	dev_dbg(&oct->pci_dev->dev, "DPI_DMA_CONTROL: 0x%016llx\n",
+		lio_pci_readq(oct, CN6XXX_DPI_DMA_CONTROL));
+
+	for (i = 0; i < 6; i++) {
+		/* Prevent service of instruction queue for all DMA engines
+		 * Engine 5 will remain 0. Engines 0 - 4 will be setup by
+		 * core.
+		 */
+		lio_pci_writeq(oct, 0, CN6XXX_DPI_DMA_ENG_ENB(i));
+		lio_pci_writeq(oct, fifo_sizes[i], CN6XXX_DPI_DMA_ENG_BUF(i));
+		dev_dbg(&oct->pci_dev->dev, "DPI_ENG_BUF%d: 0x%016llx\n", i,
+			lio_pci_readq(oct, CN6XXX_DPI_DMA_ENG_BUF(i)));
+	}
+
+	/* DPI_SLI_PRT_CFG has MPS and MRRS settings that will be set
+	 * separately.
+	 */
+
+	lio_pci_writeq(oct, 1, CN6XXX_DPI_CTL);
+	dev_dbg(&oct->pci_dev->dev, "DPI_CTL: 0x%016llx\n",
+		lio_pci_readq(oct, CN6XXX_DPI_CTL));
+}
+
+static int lio_cn68xx_soft_reset(struct octeon_device *oct)
+{
+	lio_cn6xxx_soft_reset(oct);
+	lio_cn68xx_set_dpi_regs(oct);
+
+	return 0;
+}
+
+static void lio_cn68xx_setup_pkt_ctl_regs(struct octeon_device *oct)
+{
+	struct octeon_cn6xxx *cn68xx = (struct octeon_cn6xxx *)oct->chip;
+	u64 pktctl, tx_pipe, max_oqs;
+
+	pktctl = octeon_read_csr64(oct, CN6XXX_SLI_PKT_CTL);
+
+	/* 68XX specific */
+	max_oqs = CFG_GET_OQ_MAX_Q(CHIP_FIELD(oct, cn6xxx, conf));
+	tx_pipe  = octeon_read_csr64(oct, CN68XX_SLI_TX_PIPE);
+	tx_pipe &= 0xffffffffff00ffffULL; /* clear out NUMP field */
+	tx_pipe |= max_oqs << 16; /* put max_oqs in NUMP field */
+	octeon_write_csr64(oct, CN68XX_SLI_TX_PIPE, tx_pipe);
+
+	if (CFG_GET_IS_SLI_BP_ON(cn68xx->conf))
+		pktctl |= 0xF;
+	else
+		/* Disable per-port backpressure. */
+		pktctl &= ~0xF;
+	octeon_write_csr64(oct, CN6XXX_SLI_PKT_CTL, pktctl);
+}
+
+static int lio_cn68xx_setup_device_regs(struct octeon_device *oct)
+{
+	lio_cn6xxx_setup_pcie_mps(oct, PCIE_MPS_DEFAULT);
+	lio_cn6xxx_setup_pcie_mrrs(oct, PCIE_MRRS_256B);
+	lio_cn6xxx_enable_error_reporting(oct);
+
+	lio_cn6xxx_setup_global_input_regs(oct);
+	lio_cn68xx_setup_pkt_ctl_regs(oct);
+	lio_cn6xxx_setup_global_output_regs(oct);
+
+	/* Default error timeout value should be 0x200000 to avoid host hang
+	 * when reads invalid register
+	 */
+	octeon_write_csr64(oct, CN6XXX_SLI_WINDOW_CTL, 0x200000ULL);
+
+	return 0;
+}
+
+static inline void lio_cn68xx_vendor_message_fix(struct octeon_device *oct)
+{
+	u32 val = 0;
+
+	/* Set M_VEND1_DRP and M_VEND0_DRP bits */
+	pci_read_config_dword(oct->pci_dev, CN6XXX_PCIE_FLTMSK, &val);
+	val |= 0x3;
+	pci_write_config_dword(oct->pci_dev, CN6XXX_PCIE_FLTMSK, val);
+}
+
+int lio_is_210nv(struct octeon_device *oct)
+{
+	u64 mio_qlm4_cfg = lio_pci_readq(oct, CN6XXX_MIO_QLM4_CFG);
+
+	return ((mio_qlm4_cfg & CN6XXX_MIO_QLM_CFG_MASK) == 0);
+}
+
+int lio_setup_cn68xx_octeon_device(struct octeon_device *oct)
+{
+	struct octeon_cn6xxx *cn68xx = (struct octeon_cn6xxx *)oct->chip;
+	u16 card_type = LIO_410NV;
+
+	if (octeon_map_pci_barx(oct, 0, 0))
+		return 1;
+
+	if (octeon_map_pci_barx(oct, 1, MAX_BAR1_IOREMAP_SIZE)) {
+		dev_err(&oct->pci_dev->dev, "%s CN68XX BAR1 map failed\n",
+			__func__);
+		octeon_unmap_pci_barx(oct, 0);
+		return 1;
+	}
+
+	spin_lock_init(&cn68xx->lock_for_droq_int_enb_reg);
+
+	oct->fn_list.setup_iq_regs = lio_cn6xxx_setup_iq_regs;
+	oct->fn_list.setup_oq_regs = lio_cn6xxx_setup_oq_regs;
+
+	oct->fn_list.process_interrupt_regs = lio_cn6xxx_process_interrupt_regs;
+	oct->fn_list.soft_reset = lio_cn68xx_soft_reset;
+	oct->fn_list.setup_device_regs = lio_cn68xx_setup_device_regs;
+	oct->fn_list.reinit_regs = lio_cn6xxx_reinit_regs;
+	oct->fn_list.update_iq_read_idx = lio_cn6xxx_update_read_index;
+
+	oct->fn_list.bar1_idx_setup = lio_cn6xxx_bar1_idx_setup;
+	oct->fn_list.bar1_idx_write = lio_cn6xxx_bar1_idx_write;
+	oct->fn_list.bar1_idx_read = lio_cn6xxx_bar1_idx_read;
+
+	oct->fn_list.enable_interrupt = lio_cn6xxx_enable_interrupt;
+	oct->fn_list.disable_interrupt = lio_cn6xxx_disable_interrupt;
+
+	oct->fn_list.enable_io_queues = lio_cn6xxx_enable_io_queues;
+	oct->fn_list.disable_io_queues = lio_cn6xxx_disable_io_queues;
+
+	lio_cn6xxx_setup_reg_address(oct, oct->chip, &oct->reg_list);
+
+	/* Determine variant of card */
+	if (lio_is_210nv(oct))
+		card_type = LIO_210NV;
+
+	cn68xx->conf = (struct octeon_config *)
+		       oct_get_config_info(oct, card_type);
+	if (!cn68xx->conf) {
+		dev_err(&oct->pci_dev->dev, "%s No Config found for CN68XX %s\n",
+			__func__,
+			(card_type == LIO_410NV) ? LIO_410NV_NAME :
+			LIO_210NV_NAME);
+		octeon_unmap_pci_barx(oct, 0);
+		octeon_unmap_pci_barx(oct, 1);
+		return 1;
+	}
+
+	oct->coproc_clock_rate = 1000000ULL * lio_cn6xxx_coprocessor_clock(oct);
+
+	lio_cn68xx_vendor_message_fix(oct);
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/cn68xx_device.h b/drivers/net/ethernet/cavium/liquidio/cn68xx_device.h
new file mode 100644
index 000000000000..d4e1c9fb0bf2
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn68xx_device.h
@@ -0,0 +1,33 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+
+/*! \file  cn68xx_device.h
+ *  \brief Host Driver: Routines that perform CN68XX specific operations.
+ */
+
+#ifndef __CN68XX_DEVICE_H__
+#define  __CN68XX_DEVICE_H__
+
+int lio_setup_cn68xx_octeon_device(struct octeon_device *oct);
+int lio_is_210nv(struct octeon_device *oct);
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/cn68xx_regs.h b/drivers/net/ethernet/cavium/liquidio/cn68xx_regs.h
new file mode 100644
index 000000000000..38cddbd107b6
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/cn68xx_regs.h
@@ -0,0 +1,51 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+
+/*! \file cn68xx_regs.h
+ *  \brief Host Driver: Register Address and Register Mask values for
+ *  Octeon CN68XX devices. The register map for CN66XX is the same
+ *  for most registers. This file has the other registers that are
+ *  68XX-specific.
+ */
+
+#ifndef __CN68XX_REGS_H__
+#define __CN68XX_REGS_H__
+#include "cn66xx_regs.h"
+
+/*###################### REQUEST QUEUE #########################*/
+
+#define    CN68XX_SLI_IQ_PORT0_PKIND             0x0800
+
+#define    CN68XX_SLI_IQ_PORT_PKIND(iq)           \
+	(CN68XX_SLI_IQ_PORT0_PKIND + ((iq) * CN6XXX_IQ_OFFSET))
+
+/*############################ OUTPUT QUEUE #########################*/
+
+/* Starting pipe number and number of pipes used by the SLI packet output. */
+#define    CN68XX_SLI_TX_PIPE                    0x1230
+
+/*######################## INTERRUPTS #########################*/
+
+/*------------------ Interrupt Masks ----------------*/
+#define    CN68XX_INTR_PIPE_ERR                  BIT_ULL(61)
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c b/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c
new file mode 100644
index 000000000000..29f330831784
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/lio_ethtool.c
@@ -0,0 +1,1217 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+#include <linux/version.h>
+#include <linux/netdevice.h>
+#include <linux/net_tstamp.h>
+#include <linux/ethtool.h>
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+
+struct oct_mdio_cmd_context {
+	int octeon_id;
+	wait_queue_head_t wc;
+	int cond;
+};
+
+struct oct_mdio_cmd_resp {
+	u64 rh;
+	struct oct_mdio_cmd resp;
+	u64 status;
+};
+
+#define OCT_MDIO45_RESP_SIZE   (sizeof(struct oct_mdio_cmd_resp))
+
+/* Octeon's interface mode of operation */
+enum {
+	INTERFACE_MODE_DISABLED,
+	INTERFACE_MODE_RGMII,
+	INTERFACE_MODE_GMII,
+	INTERFACE_MODE_SPI,
+	INTERFACE_MODE_PCIE,
+	INTERFACE_MODE_XAUI,
+	INTERFACE_MODE_SGMII,
+	INTERFACE_MODE_PICMG,
+	INTERFACE_MODE_NPI,
+	INTERFACE_MODE_LOOP,
+	INTERFACE_MODE_SRIO,
+	INTERFACE_MODE_ILK,
+	INTERFACE_MODE_RXAUI,
+	INTERFACE_MODE_QSGMII,
+	INTERFACE_MODE_AGL,
+};
+
+#define ARRAY_LENGTH(a) (sizeof(a) / sizeof((a)[0]))
+#define OCT_ETHTOOL_REGDUMP_LEN  4096
+#define OCT_ETHTOOL_REGSVER  1
+
+static const char oct_iq_stats_strings[][ETH_GSTRING_LEN] = {
+	"Instr posted",
+	"Instr processed",
+	"Instr dropped",
+	"Bytes Sent",
+	"Sgentry_sent",
+	"Inst cntreg",
+	"Tx done",
+	"Tx Iq busy",
+	"Tx dropped",
+	"Tx bytes",
+};
+
+static const char oct_droq_stats_strings[][ETH_GSTRING_LEN] = {
+	"OQ Pkts Received",
+	"OQ Bytes Received",
+	"Dropped no dispatch",
+	"Dropped nomem",
+	"Dropped toomany",
+	"Stack RX cnt",
+	"Stack RX Bytes",
+	"RX dropped",
+};
+
+#define OCTNIC_NCMD_AUTONEG_ON  0x1
+#define OCTNIC_NCMD_PHY_ON      0x2
+
+static int lio_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct oct_link_info *linfo;
+
+	linfo = &lio->linfo;
+
+	if (linfo->link.s.interface == INTERFACE_MODE_XAUI ||
+	    linfo->link.s.interface == INTERFACE_MODE_RXAUI) {
+		ecmd->port = PORT_FIBRE;
+		ecmd->supported =
+			(SUPPORTED_10000baseT_Full | SUPPORTED_FIBRE |
+			 SUPPORTED_Pause);
+		ecmd->advertising =
+			(ADVERTISED_10000baseT_Full | ADVERTISED_Pause);
+		ecmd->transceiver = XCVR_EXTERNAL;
+		ecmd->autoneg = AUTONEG_DISABLE;
+
+	} else {
+		dev_err(&oct->pci_dev->dev, "Unknown link interface reported\n");
+	}
+
+	if (linfo->link.s.status) {
+		ethtool_cmd_speed_set(ecmd, linfo->link.s.speed);
+		ecmd->duplex = linfo->link.s.duplex;
+	} else {
+		ethtool_cmd_speed_set(ecmd, SPEED_UNKNOWN);
+		ecmd->duplex = DUPLEX_UNKNOWN;
+	}
+
+	return 0;
+}
+
+static void
+lio_get_drvinfo(struct net_device *netdev, struct ethtool_drvinfo *drvinfo)
+{
+	struct lio *lio;
+	struct octeon_device *oct;
+
+	lio = GET_LIO(netdev);
+	oct = lio->oct_dev;
+
+	memset(drvinfo, 0, sizeof(struct ethtool_drvinfo));
+	strcpy(drvinfo->driver, "liquidio");
+	strcpy(drvinfo->version, LIQUIDIO_VERSION);
+	strncpy(drvinfo->fw_version, oct->fw_info.liquidio_firmware_version,
+		ETHTOOL_FWVERS_LEN);
+	strncpy(drvinfo->bus_info, pci_name(oct->pci_dev), 32);
+	drvinfo->regdump_len = OCT_ETHTOOL_REGDUMP_LEN;
+}
+
+static void
+lio_ethtool_get_channels(struct net_device *dev,
+			 struct ethtool_channels *channel)
+{
+	struct lio *lio = GET_LIO(dev);
+	struct octeon_device *oct = lio->oct_dev;
+	u32 max_rx = 0, max_tx = 0, tx_count = 0, rx_count = 0;
+
+	if (OCTEON_CN6XXX(oct)) {
+		struct octeon_config *conf6x = CHIP_FIELD(oct, cn6xxx, conf);
+
+		max_rx = CFG_GET_OQ_MAX_Q(conf6x);
+		max_tx = CFG_GET_IQ_MAX_Q(conf6x);
+		rx_count = CFG_GET_NUM_RXQS_NIC_IF(conf6x, lio->ifidx);
+		tx_count = CFG_GET_NUM_TXQS_NIC_IF(conf6x, lio->ifidx);
+	}
+
+	channel->max_rx = max_rx;
+	channel->max_tx = max_tx;
+	channel->rx_count = rx_count;
+	channel->tx_count = tx_count;
+}
+
+static int lio_get_eeprom_len(struct net_device *netdev)
+{
+	u8 buf[128];
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct_dev = lio->oct_dev;
+	struct octeon_board_info *board_info;
+	int len;
+
+	board_info = (struct octeon_board_info *)(&oct_dev->boardinfo);
+	len = sprintf(buf, "boardname:%s serialnum:%s maj:%lld min:%lld\n",
+		      board_info->name, board_info->serial_number,
+		      board_info->major, board_info->minor);
+
+	return len;
+}
+
+static int
+lio_get_eeprom(struct net_device *netdev, struct ethtool_eeprom *eeprom,
+	       u8 *bytes)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct_dev = lio->oct_dev;
+	struct octeon_board_info *board_info;
+	int len;
+
+	if (eeprom->offset != 0)
+		return -EINVAL;
+
+	eeprom->magic = oct_dev->pci_dev->vendor;
+	board_info = (struct octeon_board_info *)(&oct_dev->boardinfo);
+	len =
+		sprintf((char *)bytes,
+			"boardname:%s serialnum:%s maj:%lld min:%lld\n",
+			board_info->name, board_info->serial_number,
+			board_info->major, board_info->minor);
+
+	return 0;
+}
+
+static int octnet_gpio_access(struct net_device *netdev, int addr, int val)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct octnic_ctrl_pkt nctrl;
+	struct octnic_ctrl_params nparams;
+	int ret = 0;
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = OCTNET_CMD_GPIO_ACCESS;
+	nctrl.ncmd.s.param1 = lio->linfo.ifidx;
+	nctrl.ncmd.s.param2 = addr;
+	nctrl.ncmd.s.param3 = val;
+	nctrl.wait_time = 100;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+
+	nparams.resp_order = OCTEON_RESP_ORDERED;
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl, nparams);
+	if (ret < 0) {
+		dev_err(&oct->pci_dev->dev, "Failed to configure gpio value\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Callback for when mdio command response arrives
+ */
+static void octnet_mdio_resp_callback(struct octeon_device *oct,
+				      u32 status,
+				      void *buf)
+{
+	struct oct_mdio_cmd_resp *mdio_cmd_rsp;
+	struct oct_mdio_cmd_context *mdio_cmd_ctx;
+	struct octeon_soft_command *sc = (struct octeon_soft_command *)buf;
+
+	mdio_cmd_rsp = (struct oct_mdio_cmd_resp *)sc->virtrptr;
+	mdio_cmd_ctx = (struct oct_mdio_cmd_context *)sc->ctxptr;
+
+	oct = lio_get_device(mdio_cmd_ctx->octeon_id);
+	if (status) {
+		dev_err(&oct->pci_dev->dev, "MIDO instruction failed. Status: %llx\n",
+			CVM_CAST64(status));
+		ACCESS_ONCE(mdio_cmd_ctx->cond) = -1;
+	} else {
+		ACCESS_ONCE(mdio_cmd_ctx->cond) = 1;
+	}
+	wake_up_interruptible(&mdio_cmd_ctx->wc);
+}
+
+/* This routine provides PHY access routines for
+ * mdio  clause45 .
+ */
+static int
+octnet_mdio45_access(struct lio *lio, int op, int loc, int *value)
+{
+	struct octeon_device *oct_dev = lio->oct_dev;
+	struct octeon_soft_command *sc;
+	struct oct_mdio_cmd_resp *mdio_cmd_rsp;
+	struct oct_mdio_cmd_context *mdio_cmd_ctx;
+	struct oct_mdio_cmd *mdio_cmd;
+	int retval = 0;
+
+	sc = (struct octeon_soft_command *)
+		octeon_alloc_soft_command(oct_dev,
+					  sizeof(struct oct_mdio_cmd),
+					  sizeof(struct oct_mdio_cmd_resp),
+					  sizeof(struct oct_mdio_cmd_context));
+
+	if (!sc)
+		return -ENOMEM;
+
+	mdio_cmd_ctx = (struct oct_mdio_cmd_context *)sc->ctxptr;
+	mdio_cmd_rsp = (struct oct_mdio_cmd_resp *)sc->virtrptr;
+	mdio_cmd = (struct oct_mdio_cmd *)sc->virtdptr;
+
+	ACCESS_ONCE(mdio_cmd_ctx->cond) = 0;
+	mdio_cmd_ctx->octeon_id = lio_get_device_id(oct_dev);
+	mdio_cmd->op = op;
+	mdio_cmd->mdio_addr = loc;
+	if (op)
+		mdio_cmd->value1 = *value;
+	mdio_cmd->value2 = lio->linfo.ifidx;
+	octeon_swap_8B_data((u64 *)mdio_cmd, sizeof(struct oct_mdio_cmd) / 8);
+
+	octeon_prepare_soft_command(oct_dev, sc, OPCODE_NIC, OPCODE_NIC_MDIO45,
+				    0, 0, 0);
+
+	sc->wait_time = 1000;
+	sc->callback = octnet_mdio_resp_callback;
+	sc->callback_arg = sc;
+
+	init_waitqueue_head(&mdio_cmd_ctx->wc);
+
+	retval = octeon_send_soft_command(oct_dev, sc);
+
+	if (retval) {
+		dev_err(&oct_dev->pci_dev->dev,
+			"octnet_mdio45_access instruction failed status: %x\n",
+			retval);
+		retval =  -EBUSY;
+	} else {
+		/* Sleep on a wait queue till the cond flag indicates that the
+		 * response arrived
+		 */
+		sleep_cond(&mdio_cmd_ctx->wc, &mdio_cmd_ctx->cond);
+		retval = mdio_cmd_rsp->status;
+		if (retval) {
+			dev_err(&oct_dev->pci_dev->dev, "octnet mdio45 access failed\n");
+			retval = -EBUSY;
+		} else {
+			octeon_swap_8B_data((u64 *)(&mdio_cmd_rsp->resp),
+					    sizeof(struct oct_mdio_cmd) / 8);
+
+			if (ACCESS_ONCE(mdio_cmd_ctx->cond) == 1) {
+				if (!op)
+					*value = mdio_cmd_rsp->resp.value1;
+			} else {
+				retval = -EINVAL;
+			}
+		}
+	}
+
+	octeon_free_soft_command(oct_dev, sc);
+
+	return retval;
+}
+
+static int lio_set_phys_id(struct net_device *netdev,
+			   enum ethtool_phys_id_state state)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	int value, ret;
+
+	switch (state) {
+	case ETHTOOL_ID_ACTIVE:
+		if (oct->chip_id == OCTEON_CN66XX) {
+			octnet_gpio_access(netdev, VITESSE_PHY_GPIO_CFG,
+					   VITESSE_PHY_GPIO_DRIVEON);
+			return 2;
+
+		} else if (oct->chip_id == OCTEON_CN68XX) {
+			/* Save the current LED settings */
+			ret = octnet_mdio45_access(lio, 0,
+						   LIO68XX_LED_BEACON_ADDR,
+						   &lio->phy_beacon_val);
+			if (ret)
+				return ret;
+
+			ret = octnet_mdio45_access(lio, 0,
+						   LIO68XX_LED_CTRL_ADDR,
+						   &lio->led_ctrl_val);
+			if (ret)
+				return ret;
+
+			/* Configure Beacon values */
+			value = LIO68XX_LED_BEACON_CFGON;
+			ret =
+				octnet_mdio45_access(lio, 1,
+						     LIO68XX_LED_BEACON_ADDR,
+						     &value);
+			if (ret)
+				return ret;
+
+			value = LIO68XX_LED_CTRL_CFGON;
+			ret =
+				octnet_mdio45_access(lio, 1,
+						     LIO68XX_LED_CTRL_ADDR,
+						     &value);
+			if (ret)
+				return ret;
+		} else {
+			return -EINVAL;
+		}
+		break;
+
+	case ETHTOOL_ID_ON:
+		if (oct->chip_id == OCTEON_CN66XX) {
+			octnet_gpio_access(netdev, VITESSE_PHY_GPIO_CFG,
+					   VITESSE_PHY_GPIO_HIGH);
+
+		} else if (oct->chip_id == OCTEON_CN68XX) {
+			return -EINVAL;
+		} else {
+			return -EINVAL;
+		}
+		break;
+
+	case ETHTOOL_ID_OFF:
+		if (oct->chip_id == OCTEON_CN66XX)
+			octnet_gpio_access(netdev, VITESSE_PHY_GPIO_CFG,
+					   VITESSE_PHY_GPIO_LOW);
+		else if (oct->chip_id == OCTEON_CN68XX)
+			return -EINVAL;
+		else
+			return -EINVAL;
+
+		break;
+
+	case ETHTOOL_ID_INACTIVE:
+		if (oct->chip_id == OCTEON_CN66XX) {
+			octnet_gpio_access(netdev, VITESSE_PHY_GPIO_CFG,
+					   VITESSE_PHY_GPIO_DRIVEOFF);
+		} else if (oct->chip_id == OCTEON_CN68XX) {
+			/* Restore LED settings */
+			ret = octnet_mdio45_access(lio, 1,
+						   LIO68XX_LED_CTRL_ADDR,
+						   &lio->led_ctrl_val);
+			if (ret)
+				return ret;
+
+			ret = octnet_mdio45_access(lio, 1,
+						   LIO68XX_LED_BEACON_ADDR,
+						   &lio->phy_beacon_val);
+			if (ret)
+				return ret;
+
+		} else {
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void
+lio_ethtool_get_ringparam(struct net_device *netdev,
+			  struct ethtool_ringparam *ering)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	u32 tx_max_pending = 0, rx_max_pending = 0, tx_pending = 0,
+	    rx_pending = 0;
+
+	if (OCTEON_CN6XXX(oct)) {
+		struct octeon_config *conf6x = CHIP_FIELD(oct, cn6xxx, conf);
+
+		tx_max_pending = CN6XXX_MAX_IQ_DESCRIPTORS;
+		rx_max_pending = CN6XXX_MAX_OQ_DESCRIPTORS;
+		rx_pending = CFG_GET_NUM_RX_DESCS_NIC_IF(conf6x, lio->ifidx);
+		tx_pending = CFG_GET_NUM_TX_DESCS_NIC_IF(conf6x, lio->ifidx);
+	}
+
+	if (lio->mtu > OCTNET_DEFAULT_FRM_SIZE) {
+		ering->rx_pending = 0;
+		ering->rx_max_pending = 0;
+		ering->rx_mini_pending = 0;
+		ering->rx_jumbo_pending = rx_pending;
+		ering->rx_mini_max_pending = 0;
+		ering->rx_jumbo_max_pending = rx_max_pending;
+	} else {
+		ering->rx_pending = rx_pending;
+		ering->rx_max_pending = rx_max_pending;
+		ering->rx_mini_pending = 0;
+		ering->rx_jumbo_pending = 0;
+		ering->rx_mini_max_pending = 0;
+		ering->rx_jumbo_max_pending = 0;
+	}
+
+	ering->tx_pending = tx_pending;
+	ering->tx_max_pending = tx_max_pending;
+}
+
+static u32 lio_get_msglevel(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+
+	return lio->msg_enable;
+}
+
+static void lio_set_msglevel(struct net_device *netdev, u32 msglvl)
+{
+	struct lio *lio = GET_LIO(netdev);
+
+	if ((msglvl ^ lio->msg_enable) & NETIF_MSG_HW) {
+		if (msglvl & NETIF_MSG_HW)
+			liquidio_set_feature(netdev,
+					     OCTNET_CMD_VERBOSE_ENABLE);
+		else
+			liquidio_set_feature(netdev,
+					     OCTNET_CMD_VERBOSE_DISABLE);
+	}
+
+	lio->msg_enable = msglvl;
+}
+
+static void
+lio_get_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *pause)
+{
+	/* Notes: Not supporting any auto negotiation in these
+	 * drivers. Just report pause frame support.
+	 */
+	pause->tx_pause = 1;
+	pause->rx_pause = 1;    /* TODO: Need to support RX pause frame!!. */
+}
+
+static void
+lio_get_ethtool_stats(struct net_device *netdev,
+		      struct ethtool_stats *stats, u64 *data)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct_dev = lio->oct_dev;
+	int i = 0, j;
+
+	for (j = 0; j < MAX_OCTEON_INSTR_QUEUES; j++) {
+		if (!(oct_dev->io_qmask.iq & (1UL << j)))
+			continue;
+		data[i++] =
+			CVM_CAST64(oct_dev->instr_queue[j]->stats.instr_posted);
+		data[i++] =
+			CVM_CAST64(
+				oct_dev->instr_queue[j]->stats.instr_processed);
+		data[i++] =
+			CVM_CAST64(
+				oct_dev->instr_queue[j]->stats.instr_dropped);
+		data[i++] =
+			CVM_CAST64(oct_dev->instr_queue[j]->stats.bytes_sent);
+		data[i++] =
+			CVM_CAST64(oct_dev->instr_queue[j]->stats.sgentry_sent);
+		data[i++] =
+			readl(oct_dev->instr_queue[j]->inst_cnt_reg);
+		data[i++] =
+			CVM_CAST64(oct_dev->instr_queue[j]->stats.tx_done);
+		data[i++] =
+			CVM_CAST64(oct_dev->instr_queue[j]->stats.tx_iq_busy);
+		data[i++] =
+			CVM_CAST64(oct_dev->instr_queue[j]->stats.tx_dropped);
+		data[i++] =
+			CVM_CAST64(oct_dev->instr_queue[j]->stats.tx_tot_bytes);
+	}
+
+	/* for (j = 0; j < oct_dev->num_oqs; j++){ */
+	for (j = 0; j < MAX_OCTEON_OUTPUT_QUEUES; j++) {
+		if (!(oct_dev->io_qmask.oq & (1UL << j)))
+			continue;
+		data[i++] = CVM_CAST64(oct_dev->droq[j]->stats.pkts_received);
+		data[i++] = CVM_CAST64(oct_dev->droq[j]->stats.bytes_received);
+		data[i++] =
+			CVM_CAST64(oct_dev->droq[j]->stats.dropped_nodispatch);
+		data[i++] = CVM_CAST64(oct_dev->droq[j]->stats.dropped_nomem);
+		data[i++] = CVM_CAST64(oct_dev->droq[j]->stats.dropped_toomany);
+		data[i++] =
+			CVM_CAST64(oct_dev->droq[j]->stats.rx_pkts_received);
+		data[i++] =
+			CVM_CAST64(oct_dev->droq[j]->stats.rx_bytes_received);
+		data[i++] =
+			CVM_CAST64(oct_dev->droq[j]->stats.rx_dropped);
+	}
+}
+
+static void lio_get_strings(struct net_device *netdev, u32 stringset, u8 *data)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct_dev = lio->oct_dev;
+	int num_iq_stats, num_oq_stats, i, j;
+
+	num_iq_stats = ARRAY_SIZE(oct_iq_stats_strings);
+	for (i = 0; i < MAX_OCTEON_INSTR_QUEUES; i++) {
+		if (!(oct_dev->io_qmask.iq & (1UL << i)))
+			continue;
+		for (j = 0; j < num_iq_stats; j++) {
+			sprintf(data, "IQ%d %s", i, oct_iq_stats_strings[j]);
+			data += ETH_GSTRING_LEN;
+		}
+	}
+
+	num_oq_stats = ARRAY_SIZE(oct_droq_stats_strings);
+	/* for (i = 0; i < oct_dev->num_oqs; i++) { */
+	for (i = 0; i < MAX_OCTEON_OUTPUT_QUEUES; i++) {
+		if (!(oct_dev->io_qmask.oq & (1UL << i)))
+			continue;
+		for (j = 0; j < num_oq_stats; j++) {
+			sprintf(data, "OQ%d %s", i, oct_droq_stats_strings[j]);
+			data += ETH_GSTRING_LEN;
+		}
+	}
+}
+
+static int lio_get_sset_count(struct net_device *netdev, int sset)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct_dev = lio->oct_dev;
+
+	return (ARRAY_SIZE(oct_iq_stats_strings) * oct_dev->num_iqs) +
+	       (ARRAY_SIZE(oct_droq_stats_strings) * oct_dev->num_oqs);
+}
+
+static int lio_get_intr_coalesce(struct net_device *netdev,
+				 struct ethtool_coalesce *intr_coal)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->chip;
+	struct octeon_instr_queue *iq;
+	struct oct_intrmod_cfg *intrmod_cfg;
+
+	intrmod_cfg = &oct->intrmod;
+
+	switch (oct->chip_id) {
+	/* case OCTEON_CN73XX: Todo */
+	/*      break; */
+	case OCTEON_CN68XX:
+	case OCTEON_CN66XX:
+		if (!intrmod_cfg->intrmod_enable) {
+			intr_coal->rx_coalesce_usecs =
+				CFG_GET_OQ_INTR_TIME(cn6xxx->conf);
+			intr_coal->rx_max_coalesced_frames =
+				CFG_GET_OQ_INTR_PKT(cn6xxx->conf);
+		} else {
+			intr_coal->use_adaptive_rx_coalesce =
+				intrmod_cfg->intrmod_enable;
+			intr_coal->rate_sample_interval =
+				intrmod_cfg->intrmod_check_intrvl;
+			intr_coal->pkt_rate_high =
+				intrmod_cfg->intrmod_maxpkt_ratethr;
+			intr_coal->pkt_rate_low =
+				intrmod_cfg->intrmod_minpkt_ratethr;
+			intr_coal->rx_max_coalesced_frames_high =
+				intrmod_cfg->intrmod_maxcnt_trigger;
+			intr_coal->rx_coalesce_usecs_high =
+				intrmod_cfg->intrmod_maxtmr_trigger;
+			intr_coal->rx_coalesce_usecs_low =
+				intrmod_cfg->intrmod_mintmr_trigger;
+			intr_coal->rx_max_coalesced_frames_low =
+				intrmod_cfg->intrmod_mincnt_trigger;
+		}
+
+		iq = oct->instr_queue[lio->linfo.txpciq[0]];
+		intr_coal->tx_max_coalesced_frames = iq->fill_threshold;
+		break;
+
+	default:
+		netif_info(lio, drv, lio->netdev, "Unknown Chip !!\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Callback function for intrmod */
+static void octnet_intrmod_callback(struct octeon_device *oct_dev,
+				    u32 status,
+				    void *ptr)
+{
+	struct oct_intrmod_cmd *cmd = ptr;
+	struct octeon_soft_command *sc = cmd->sc;
+
+	oct_dev = cmd->oct_dev;
+
+	if (status)
+		dev_err(&oct_dev->pci_dev->dev, "intrmod config failed. Status: %llx\n",
+			CVM_CAST64(status));
+	else
+		dev_info(&oct_dev->pci_dev->dev,
+			 "Rx-Adaptive Interrupt moderation enabled:%llx\n",
+			 oct_dev->intrmod.intrmod_enable);
+
+	octeon_free_soft_command(oct_dev, sc);
+}
+
+/*  Configure interrupt moderation parameters */
+static int octnet_set_intrmod_cfg(void *oct, struct oct_intrmod_cfg *intr_cfg)
+{
+	struct octeon_soft_command *sc;
+	struct oct_intrmod_cmd *cmd;
+	struct oct_intrmod_cfg *cfg;
+	int retval;
+	struct octeon_device *oct_dev = (struct octeon_device *)oct;
+
+	/* Alloc soft command */
+	sc = (struct octeon_soft_command *)
+		octeon_alloc_soft_command(oct_dev,
+					  sizeof(struct oct_intrmod_cfg),
+					  0,
+					  sizeof(struct oct_intrmod_cmd));
+
+	if (!sc)
+		return -ENOMEM;
+
+	cmd = (struct oct_intrmod_cmd *)sc->ctxptr;
+	cfg = (struct oct_intrmod_cfg *)sc->virtdptr;
+
+	memcpy(cfg, intr_cfg, sizeof(struct oct_intrmod_cfg));
+	octeon_swap_8B_data((u64 *)cfg, (sizeof(struct oct_intrmod_cfg)) / 8);
+	cmd->sc = sc;
+	cmd->cfg = cfg;
+	cmd->oct_dev = oct_dev;
+
+	octeon_prepare_soft_command(oct_dev, sc, OPCODE_NIC,
+				    OPCODE_NIC_INTRMOD_CFG, 0, 0, 0);
+
+	sc->callback = octnet_intrmod_callback;
+	sc->callback_arg = cmd;
+	sc->wait_time = 1000;
+
+	retval = octeon_send_soft_command(oct_dev, sc);
+	if (retval) {
+		octeon_free_soft_command(oct_dev, sc);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Enable/Disable auto interrupt Moderation */
+static int oct_cfg_adaptive_intr(struct lio *lio, struct ethtool_coalesce
+				 *intr_coal, int adaptive)
+{
+	int ret = 0;
+	struct octeon_device *oct = lio->oct_dev;
+	struct oct_intrmod_cfg *intrmod_cfg;
+
+	intrmod_cfg = &oct->intrmod;
+
+	if (adaptive) {
+		if (intr_coal->rate_sample_interval)
+			intrmod_cfg->intrmod_check_intrvl =
+				intr_coal->rate_sample_interval;
+		else
+			intrmod_cfg->intrmod_check_intrvl =
+				LIO_INTRMOD_CHECK_INTERVAL;
+
+		if (intr_coal->pkt_rate_high)
+			intrmod_cfg->intrmod_maxpkt_ratethr =
+				intr_coal->pkt_rate_high;
+		else
+			intrmod_cfg->intrmod_maxpkt_ratethr =
+				LIO_INTRMOD_MAXPKT_RATETHR;
+
+		if (intr_coal->pkt_rate_low)
+			intrmod_cfg->intrmod_minpkt_ratethr =
+				intr_coal->pkt_rate_low;
+		else
+			intrmod_cfg->intrmod_minpkt_ratethr =
+				LIO_INTRMOD_MINPKT_RATETHR;
+
+		if (intr_coal->rx_max_coalesced_frames_high)
+			intrmod_cfg->intrmod_maxcnt_trigger =
+				intr_coal->rx_max_coalesced_frames_high;
+		else
+			intrmod_cfg->intrmod_maxcnt_trigger =
+				LIO_INTRMOD_MAXCNT_TRIGGER;
+
+		if (intr_coal->rx_coalesce_usecs_high)
+			intrmod_cfg->intrmod_maxtmr_trigger =
+				intr_coal->rx_coalesce_usecs_high;
+		else
+			intrmod_cfg->intrmod_maxtmr_trigger =
+				LIO_INTRMOD_MAXTMR_TRIGGER;
+
+		if (intr_coal->rx_coalesce_usecs_low)
+			intrmod_cfg->intrmod_mintmr_trigger =
+				intr_coal->rx_coalesce_usecs_low;
+		else
+			intrmod_cfg->intrmod_mintmr_trigger =
+				LIO_INTRMOD_MINTMR_TRIGGER;
+
+		if (intr_coal->rx_max_coalesced_frames_low)
+			intrmod_cfg->intrmod_mincnt_trigger =
+				intr_coal->rx_max_coalesced_frames_low;
+		else
+			intrmod_cfg->intrmod_mincnt_trigger =
+				LIO_INTRMOD_MINCNT_TRIGGER;
+	}
+
+	intrmod_cfg->intrmod_enable = adaptive;
+	ret = octnet_set_intrmod_cfg(oct, intrmod_cfg);
+
+	return ret;
+}
+
+static int
+oct_cfg_rx_intrcnt(struct lio *lio, struct ethtool_coalesce *intr_coal)
+{
+	int ret;
+	struct octeon_device *oct = lio->oct_dev;
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->chip;
+	u32 rx_max_coalesced_frames;
+
+	if (!intr_coal->rx_max_coalesced_frames)
+		rx_max_coalesced_frames = CN6XXX_OQ_INTR_PKT;
+	else
+		rx_max_coalesced_frames = intr_coal->rx_max_coalesced_frames;
+
+	/* Disable adaptive interrupt modulation */
+	ret = oct_cfg_adaptive_intr(lio, intr_coal, 0);
+	if (ret)
+		return ret;
+
+	/* Config Cnt based interrupt values */
+	octeon_write_csr(oct, CN6XXX_SLI_OQ_INT_LEVEL_PKTS,
+			 rx_max_coalesced_frames);
+	CFG_SET_OQ_INTR_PKT(cn6xxx->conf, rx_max_coalesced_frames);
+	return 0;
+}
+
+static int oct_cfg_rx_intrtime(struct lio *lio, struct ethtool_coalesce
+			       *intr_coal)
+{
+	int ret;
+	struct octeon_device *oct = lio->oct_dev;
+	struct octeon_cn6xxx *cn6xxx = (struct octeon_cn6xxx *)oct->chip;
+	u32 time_threshold, rx_coalesce_usecs;
+
+	if (!intr_coal->rx_coalesce_usecs)
+		rx_coalesce_usecs = CN6XXX_OQ_INTR_TIME;
+	else
+		rx_coalesce_usecs = intr_coal->rx_coalesce_usecs;
+
+	/* Disable adaptive interrupt modulation */
+	ret = oct_cfg_adaptive_intr(lio, intr_coal, 0);
+	if (ret)
+		return ret;
+
+	/* Config Time based interrupt values */
+	time_threshold = lio_cn6xxx_get_oq_ticks(oct, rx_coalesce_usecs);
+	octeon_write_csr(oct, CN6XXX_SLI_OQ_INT_LEVEL_TIME, time_threshold);
+	CFG_SET_OQ_INTR_TIME(cn6xxx->conf, rx_coalesce_usecs);
+
+	return 0;
+}
+
+static int lio_set_intr_coalesce(struct net_device *netdev,
+				 struct ethtool_coalesce *intr_coal)
+{
+	struct lio *lio = GET_LIO(netdev);
+	int ret;
+	struct octeon_device *oct = lio->oct_dev;
+	u32 j, q_no;
+
+	if ((intr_coal->tx_max_coalesced_frames >= CN6XXX_DB_MIN) &&
+	    (intr_coal->tx_max_coalesced_frames <= CN6XXX_DB_MAX)) {
+		for (j = 0; j < lio->linfo.num_txpciq; j++) {
+			q_no = lio->linfo.txpciq[j];
+			oct->instr_queue[q_no]->fill_threshold =
+				intr_coal->tx_max_coalesced_frames;
+		}
+	} else {
+		dev_err(&oct->pci_dev->dev,
+			"LIQUIDIO: Invalid tx-frames:%d. Range is min:%d max:%d\n",
+			intr_coal->tx_max_coalesced_frames, CN6XXX_DB_MIN,
+			CN6XXX_DB_MAX);
+		return -EINVAL;
+	}
+
+	/* User requested adaptive-rx on */
+	if (intr_coal->use_adaptive_rx_coalesce) {
+		ret = oct_cfg_adaptive_intr(lio, intr_coal, 1);
+		if (ret)
+			goto ret_intrmod;
+	}
+
+	/* User requested adaptive-rx off and rx coalesce */
+	if ((intr_coal->rx_coalesce_usecs) &&
+	    (!intr_coal->use_adaptive_rx_coalesce)) {
+		ret = oct_cfg_rx_intrtime(lio, intr_coal);
+		if (ret)
+			goto ret_intrmod;
+	}
+
+	/* User requested adaptive-rx off and rx coalesce */
+	if ((intr_coal->rx_max_coalesced_frames) &&
+	    (!intr_coal->use_adaptive_rx_coalesce)) {
+		ret = oct_cfg_rx_intrcnt(lio, intr_coal);
+		if (ret)
+			goto ret_intrmod;
+	}
+
+	/* User requested adaptive-rx off, so use default coalesce params */
+	if ((!intr_coal->rx_max_coalesced_frames) &&
+	    (!intr_coal->use_adaptive_rx_coalesce) &&
+	    (!intr_coal->rx_coalesce_usecs)) {
+		dev_info(&oct->pci_dev->dev,
+			 "Turning off adaptive-rx interrupt moderation\n");
+		dev_info(&oct->pci_dev->dev,
+			 "Using RX Coalesce Default values rx_coalesce_usecs:%d rx_max_coalesced_frames:%d\n",
+			 CN6XXX_OQ_INTR_TIME, CN6XXX_OQ_INTR_PKT);
+		ret = oct_cfg_rx_intrtime(lio, intr_coal);
+		if (ret)
+			goto ret_intrmod;
+
+		ret = oct_cfg_rx_intrcnt(lio, intr_coal);
+		if (ret)
+			goto ret_intrmod;
+	}
+
+	return 0;
+ret_intrmod:
+	return ret;
+}
+
+static int lio_get_ts_info(struct net_device *netdev,
+			   struct ethtool_ts_info *info)
+{
+	struct lio *lio = GET_LIO(netdev);
+
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_TX_SOFTWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RX_SOFTWARE |
+		SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_RAW_HARDWARE;
+
+	if (lio->ptp_clock)
+		info->phc_index = ptp_clock_index(lio->ptp_clock);
+	else
+		info->phc_index = -1;
+
+	info->tx_types = (1 << HWTSTAMP_TX_OFF) | (1 << HWTSTAMP_TX_ON);
+
+	info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
+			   (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
+			   (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
+			   (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
+
+	return 0;
+}
+
+static int lio_set_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct oct_link_info *linfo;
+	struct octnic_ctrl_pkt nctrl;
+	struct octnic_ctrl_params nparams;
+	int ret = 0;
+
+	/* get the link info */
+	linfo = &lio->linfo;
+
+	if (ecmd->autoneg != AUTONEG_ENABLE && ecmd->autoneg != AUTONEG_DISABLE)
+		return -EINVAL;
+
+	if (ecmd->autoneg == AUTONEG_DISABLE && ((ecmd->speed != SPEED_100 &&
+						  ecmd->speed != SPEED_10) ||
+						 (ecmd->duplex != DUPLEX_HALF &&
+						  ecmd->duplex != DUPLEX_FULL)))
+		return -EINVAL;
+
+	/* Ethtool Support is not provided for XAUI and RXAUI Interfaces
+	 * as they operate at fixed Speed and Duplex settings
+	 */
+	if (linfo->link.s.interface == INTERFACE_MODE_XAUI ||
+	    linfo->link.s.interface == INTERFACE_MODE_RXAUI) {
+		dev_info(&oct->pci_dev->dev, "XAUI IFs settings cannot be modified.\n");
+		return -EINVAL;
+	}
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = OCTNET_CMD_SET_SETTINGS;
+	nctrl.wait_time = 1000;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.ncmd.s.param1 = lio->linfo.ifidx;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+
+	/* Passing the parameters sent by ethtool like Speed, Autoneg & Duplex
+	 * to SE core application using ncmd.s.more & ncmd.s.param
+	 */
+	if (ecmd->autoneg == AUTONEG_ENABLE) {
+		/* Autoneg ON */
+		nctrl.ncmd.s.more = OCTNIC_NCMD_PHY_ON |
+				     OCTNIC_NCMD_AUTONEG_ON;
+		nctrl.ncmd.s.param2 = ecmd->advertising;
+	} else {
+		/* Autoneg OFF */
+		nctrl.ncmd.s.more = OCTNIC_NCMD_PHY_ON;
+
+		nctrl.ncmd.s.param3 = ecmd->duplex;
+
+		nctrl.ncmd.s.param2 = ecmd->speed;
+	}
+
+	nparams.resp_order = OCTEON_RESP_ORDERED;
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl, nparams);
+	if (ret < 0) {
+		dev_err(&oct->pci_dev->dev, "Failed to set settings\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int lio_nway_reset(struct net_device *netdev)
+{
+	if (netif_running(netdev)) {
+		struct ethtool_cmd ecmd;
+
+		memset(&ecmd, 0, sizeof(struct ethtool_cmd));
+		ecmd.autoneg = 0;
+		ecmd.speed = 0;
+		ecmd.duplex = 0;
+		lio_set_settings(netdev, &ecmd);
+	}
+	return 0;
+}
+
+/* Return register dump len. */
+static int lio_get_regs_len(struct net_device *dev)
+{
+	return OCT_ETHTOOL_REGDUMP_LEN;
+}
+
+static int cn6xxx_read_csr_reg(char *s, struct octeon_device *oct)
+{
+	u32 reg;
+	int i, len = 0;
+
+	/* PCI  Window Registers */
+
+	len += sprintf(s + len, "\n\t Octeon CSR Registers\n\n");
+	reg = CN6XXX_WIN_WR_ADDR_LO;
+	len += sprintf(s + len, "\n[%02x] (WIN_WR_ADDR_LO): %08x\n",
+		       CN6XXX_WIN_WR_ADDR_LO, octeon_read_csr(oct, reg));
+	reg = CN6XXX_WIN_WR_ADDR_HI;
+	len += sprintf(s + len, "[%02x] (WIN_WR_ADDR_HI): %08x\n",
+		       CN6XXX_WIN_WR_ADDR_HI, octeon_read_csr(oct, reg));
+	reg = CN6XXX_WIN_RD_ADDR_LO;
+	len += sprintf(s + len, "[%02x] (WIN_RD_ADDR_LO): %08x\n",
+		       CN6XXX_WIN_RD_ADDR_LO, octeon_read_csr(oct, reg));
+	reg = CN6XXX_WIN_RD_ADDR_HI;
+	len += sprintf(s + len, "[%02x] (WIN_RD_ADDR_HI): %08x\n",
+		       CN6XXX_WIN_RD_ADDR_HI, octeon_read_csr(oct, reg));
+	reg = CN6XXX_WIN_WR_DATA_LO;
+	len += sprintf(s + len, "[%02x] (WIN_WR_DATA_LO): %08x\n",
+		       CN6XXX_WIN_WR_DATA_LO, octeon_read_csr(oct, reg));
+	reg = CN6XXX_WIN_WR_DATA_HI;
+	len += sprintf(s + len, "[%02x] (WIN_WR_DATA_HI): %08x\n",
+		       CN6XXX_WIN_WR_DATA_HI, octeon_read_csr(oct, reg));
+	len += sprintf(s + len, "[%02x] (WIN_WR_MASK_REG): %08x\n",
+		       CN6XXX_WIN_WR_MASK_REG,
+		       octeon_read_csr(oct, CN6XXX_WIN_WR_MASK_REG));
+
+	/* PCI  Interrupt Register */
+	len += sprintf(s + len, "\n[%x] (INT_ENABLE PORT 0): %08x\n",
+		       CN6XXX_SLI_INT_ENB64_PORT0, octeon_read_csr(oct,
+						CN6XXX_SLI_INT_ENB64_PORT0));
+	len += sprintf(s + len, "\n[%x] (INT_ENABLE PORT 1): %08x\n",
+		       CN6XXX_SLI_INT_ENB64_PORT1,
+		       octeon_read_csr(oct, CN6XXX_SLI_INT_ENB64_PORT1));
+	len += sprintf(s + len, "[%x] (INT_SUM): %08x\n", CN6XXX_SLI_INT_SUM64,
+		       octeon_read_csr(oct, CN6XXX_SLI_INT_SUM64));
+
+	/* PCI  Output queue registers */
+	for (i = 0; i < oct->num_oqs; i++) {
+		reg = CN6XXX_SLI_OQ_PKTS_SENT(i);
+		len += sprintf(s + len, "\n[%x] (PKTS_SENT_%d): %08x\n",
+			       reg, i, octeon_read_csr(oct, reg));
+		reg = CN6XXX_SLI_OQ_PKTS_CREDIT(i);
+		len += sprintf(s + len, "[%x] (PKT_CREDITS_%d): %08x\n",
+			       reg, i, octeon_read_csr(oct, reg));
+	}
+	reg = CN6XXX_SLI_OQ_INT_LEVEL_PKTS;
+	len += sprintf(s + len, "\n[%x] (PKTS_SENT_INT_LEVEL): %08x\n",
+		       reg, octeon_read_csr(oct, reg));
+	reg = CN6XXX_SLI_OQ_INT_LEVEL_TIME;
+	len += sprintf(s + len, "[%x] (PKTS_SENT_TIME): %08x\n",
+		       reg, octeon_read_csr(oct, reg));
+
+	/* PCI  Input queue registers */
+	for (i = 0; i <= 3; i++) {
+		u32 reg;
+
+		reg = CN6XXX_SLI_IQ_DOORBELL(i);
+		len += sprintf(s + len, "\n[%x] (INSTR_DOORBELL_%d): %08x\n",
+			       reg, i, octeon_read_csr(oct, reg));
+		reg = CN6XXX_SLI_IQ_INSTR_COUNT(i);
+		len += sprintf(s + len, "[%x] (INSTR_COUNT_%d): %08x\n",
+			       reg, i, octeon_read_csr(oct, reg));
+	}
+
+	/* PCI  DMA registers */
+
+	len += sprintf(s + len, "\n[%x] (DMA_CNT_0): %08x\n",
+		       CN6XXX_DMA_CNT(0),
+		       octeon_read_csr(oct, CN6XXX_DMA_CNT(0)));
+	reg = CN6XXX_DMA_PKT_INT_LEVEL(0);
+	len += sprintf(s + len, "[%x] (DMA_INT_LEV_0): %08x\n",
+		       CN6XXX_DMA_PKT_INT_LEVEL(0), octeon_read_csr(oct, reg));
+	reg = CN6XXX_DMA_TIME_INT_LEVEL(0);
+	len += sprintf(s + len, "[%x] (DMA_TIME_0): %08x\n",
+		       CN6XXX_DMA_TIME_INT_LEVEL(0),
+		       octeon_read_csr(oct, reg));
+
+	len += sprintf(s + len, "\n[%x] (DMA_CNT_1): %08x\n",
+		       CN6XXX_DMA_CNT(1),
+		       octeon_read_csr(oct, CN6XXX_DMA_CNT(1)));
+	reg = CN6XXX_DMA_PKT_INT_LEVEL(1);
+	len += sprintf(s + len, "[%x] (DMA_INT_LEV_1): %08x\n",
+		       CN6XXX_DMA_PKT_INT_LEVEL(1),
+		       octeon_read_csr(oct, reg));
+	reg = CN6XXX_DMA_PKT_INT_LEVEL(1);
+	len += sprintf(s + len, "[%x] (DMA_TIME_1): %08x\n",
+		       CN6XXX_DMA_TIME_INT_LEVEL(1),
+		       octeon_read_csr(oct, reg));
+
+	/* PCI  Index registers */
+
+	len += sprintf(s + len, "\n");
+
+	for (i = 0; i < 16; i++) {
+		reg = lio_pci_readq(oct, CN6XXX_BAR1_REG(i, oct->pcie_port));
+		len += sprintf(s + len, "[%llx] (BAR1_INDEX_%02d): %08x\n",
+			       CN6XXX_BAR1_REG(i, oct->pcie_port), i, reg);
+	}
+
+	return len;
+}
+
+static int cn6xxx_read_config_reg(char *s, struct octeon_device *oct)
+{
+	u32 val;
+	int i, len = 0;
+
+	/* PCI CONFIG Registers */
+
+	len += sprintf(s + len,
+		       "\n\t Octeon Config space Registers\n\n");
+
+	for (i = 0; i <= 13; i++) {
+		pci_read_config_dword(oct->pci_dev, (i * 4), &val);
+		len += sprintf(s + len, "[0x%x] (Config[%d]): 0x%08x\n",
+			       (i * 4), i, val);
+	}
+
+	for (i = 30; i <= 34; i++) {
+		pci_read_config_dword(oct->pci_dev, (i * 4), &val);
+		len += sprintf(s + len, "[0x%x] (Config[%d]): 0x%08x\n",
+			       (i * 4), i, val);
+	}
+
+	return len;
+}
+
+/*  Return register dump user app.  */
+static void lio_get_regs(struct net_device *dev,
+			 struct ethtool_regs *regs, void *regbuf)
+{
+	struct lio *lio = GET_LIO(dev);
+	int len = 0;
+	struct octeon_device *oct = lio->oct_dev;
+
+	memset(regbuf, 0, OCT_ETHTOOL_REGDUMP_LEN);
+	regs->version = OCT_ETHTOOL_REGSVER;
+
+	switch (oct->chip_id) {
+	/* case OCTEON_CN73XX: Todo */
+	case OCTEON_CN68XX:
+	case OCTEON_CN66XX:
+		len += cn6xxx_read_csr_reg(regbuf + len, oct);
+		len += cn6xxx_read_config_reg(regbuf + len, oct);
+		break;
+	default:
+		dev_err(&oct->pci_dev->dev, "%s Unknown chipid: %d\n",
+			__func__, oct->chip_id);
+	}
+}
+
+static const struct ethtool_ops lio_ethtool_ops = {
+	.get_settings		= lio_get_settings,
+	.get_link		= ethtool_op_get_link,
+	.get_drvinfo		= lio_get_drvinfo,
+	.get_ringparam		= lio_ethtool_get_ringparam,
+	.get_channels		= lio_ethtool_get_channels,
+	.set_phys_id		= lio_set_phys_id,
+	.get_eeprom_len		= lio_get_eeprom_len,
+	.get_eeprom		= lio_get_eeprom,
+	.get_strings		= lio_get_strings,
+	.get_ethtool_stats	= lio_get_ethtool_stats,
+	.get_pauseparam		= lio_get_pauseparam,
+	.get_regs_len		= lio_get_regs_len,
+	.get_regs		= lio_get_regs,
+	.get_msglevel		= lio_get_msglevel,
+	.set_msglevel		= lio_set_msglevel,
+	.get_sset_count		= lio_get_sset_count,
+	.nway_reset		= lio_nway_reset,
+	.set_settings		= lio_set_settings,
+	.get_coalesce		= lio_get_intr_coalesce,
+	.set_coalesce		= lio_set_intr_coalesce,
+	.get_ts_info		= lio_get_ts_info,
+};
+
+void liquidio_set_ethtool_ops(struct net_device *netdev)
+{
+	netdev->ethtool_ops = &lio_ethtool_ops;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_main.c b/drivers/net/ethernet/cavium/liquidio/lio_main.c
new file mode 100644
index 000000000000..0660deecc2c9
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/lio_main.c
@@ -0,0 +1,3668 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/crc32.h>
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+#include <linux/ip.h>
+#include <net/ip.h>
+#include <linux/ipv6.h>
+#include <linux/net_tstamp.h>
+#include <linux/if_vlan.h>
+#include <linux/firmware.h>
+#include <linux/ethtool.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/workqueue.h>
+#include <linux/interrupt.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+
+MODULE_AUTHOR("Cavium Networks, <support@cavium.com>");
+MODULE_DESCRIPTION("Cavium LiquidIO Intelligent Server Adapter Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(LIQUIDIO_VERSION);
+MODULE_FIRMWARE(LIO_FW_DIR LIO_FW_BASE_NAME LIO_210SV_NAME LIO_FW_NAME_SUFFIX);
+MODULE_FIRMWARE(LIO_FW_DIR LIO_FW_BASE_NAME LIO_210NV_NAME LIO_FW_NAME_SUFFIX);
+MODULE_FIRMWARE(LIO_FW_DIR LIO_FW_BASE_NAME LIO_410NV_NAME LIO_FW_NAME_SUFFIX);
+
+static int ddr_timeout = 10000;
+module_param(ddr_timeout, int, 0644);
+MODULE_PARM_DESC(ddr_timeout,
+		 "Number of milliseconds to wait for DDR initialization. 0 waits for ddr_timeout to be set to non-zero value before starting to check");
+
+static u32 console_bitmask;
+module_param(console_bitmask, int, 0644);
+MODULE_PARM_DESC(console_bitmask,
+		 "Bitmask indicating which consoles have debug output redirected to syslog.");
+
+#define DEFAULT_MSG_ENABLE (NETIF_MSG_DRV | NETIF_MSG_PROBE | NETIF_MSG_LINK)
+
+static int debug = -1;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "NETIF_MSG debug bits");
+
+static char fw_type[LIO_MAX_FW_TYPE_LEN];
+module_param_string(fw_type, fw_type, sizeof(fw_type), 0000);
+MODULE_PARM_DESC(fw_type, "Type of firmware to be loaded. Default \"nic\"");
+
+static int conf_type;
+module_param(conf_type, int, 0);
+MODULE_PARM_DESC(conf_type, "select octeon configuration 0 default 1 ovs");
+
+/* Bit mask values for lio->ifstate */
+#define   LIO_IFSTATE_DROQ_OPS             0x01
+#define   LIO_IFSTATE_REGISTERED           0x02
+#define   LIO_IFSTATE_RUNNING              0x04
+#define   LIO_IFSTATE_RX_TIMESTAMP_ENABLED 0x08
+
+/* Polling interval for determining when NIC application is alive */
+#define LIQUIDIO_STARTER_POLL_INTERVAL_MS 100
+
+/* runtime link query interval */
+#define LIQUIDIO_LINK_QUERY_INTERVAL_MS         1000
+
+struct liquidio_if_cfg_context {
+	int octeon_id;
+
+	wait_queue_head_t wc;
+
+	int cond;
+};
+
+struct liquidio_if_cfg_resp {
+	u64 rh;
+	struct liquidio_if_cfg_info cfg_info;
+	u64 status;
+};
+
+struct oct_link_status_resp {
+	u64 rh;
+	struct oct_link_info link_info;
+	u64 status;
+};
+
+struct oct_timestamp_resp {
+	u64 rh;
+	u64 timestamp;
+	u64 status;
+};
+
+#define OCT_TIMESTAMP_RESP_SIZE (sizeof(struct oct_timestamp_resp))
+
+union tx_info {
+	u64 u64;
+	struct {
+#ifdef __BIG_ENDIAN_BITFIELD
+		u16 gso_size;
+		u16 gso_segs;
+		u32 reserved;
+#else
+		u32 reserved;
+		u16 gso_segs;
+		u16 gso_size;
+#endif
+	} s;
+};
+
+/** Octeon device properties to be used by the NIC module.
+ * Each octeon device in the system will be represented
+ * by this structure in the NIC module.
+ */
+
+#define OCTNIC_MAX_SG  (MAX_SKB_FRAGS)
+
+#define OCTNIC_GSO_MAX_HEADER_SIZE 128
+#define OCTNIC_GSO_MAX_SIZE (GSO_MAX_SIZE - OCTNIC_GSO_MAX_HEADER_SIZE)
+
+/** Structure of a node in list of gather components maintained by
+ * NIC driver for each network device.
+ */
+struct octnic_gather {
+	/** List manipulation. Next and prev pointers. */
+	struct list_head list;
+
+	/** Size of the gather component at sg in bytes. */
+	int sg_size;
+
+	/** Number of bytes that sg was adjusted to make it 8B-aligned. */
+	int adjust;
+
+	/** Gather component that can accommodate max sized fragment list
+	 *  received from the IP layer.
+	 */
+	struct octeon_sg_entry *sg;
+};
+
+/** This structure is used by NIC driver to store information required
+ * to free the sk_buff when the packet has been fetched by Octeon.
+ * Bytes offset below assume worst-case of a 64-bit system.
+ */
+struct octnet_buf_free_info {
+	/** Bytes 1-8.  Pointer to network device private structure. */
+	struct lio *lio;
+
+	/** Bytes 9-16.  Pointer to sk_buff. */
+	struct sk_buff *skb;
+
+	/** Bytes 17-24.  Pointer to gather list. */
+	struct octnic_gather *g;
+
+	/** Bytes 25-32. Physical address of skb->data or gather list. */
+	u64 dptr;
+
+	/** Bytes 33-47. Piggybacked soft command, if any */
+	struct octeon_soft_command *sc;
+};
+
+struct handshake {
+	struct completion init;
+	struct completion started;
+	struct pci_dev *pci_dev;
+	int init_ok;
+	int started_ok;
+};
+
+struct octeon_device_priv {
+	/** Tasklet structures for this device. */
+	struct tasklet_struct droq_tasklet;
+	unsigned long napi_mask;
+};
+
+static int octeon_device_init(struct octeon_device *);
+static void liquidio_remove(struct pci_dev *pdev);
+static int liquidio_probe(struct pci_dev *pdev,
+			  const struct pci_device_id *ent);
+
+static struct handshake handshake[MAX_OCTEON_DEVICES];
+static struct completion first_stage;
+
+static void octeon_droq_bh(unsigned long pdev)
+{
+	int q_no;
+	int reschedule = 0;
+	struct octeon_device *oct = (struct octeon_device *)pdev;
+	struct octeon_device_priv *oct_priv =
+		(struct octeon_device_priv *)oct->priv;
+
+	/* for (q_no = 0; q_no < oct->num_oqs; q_no++) { */
+	for (q_no = 0; q_no < MAX_OCTEON_OUTPUT_QUEUES; q_no++) {
+		if (!(oct->io_qmask.oq & (1UL << q_no)))
+			continue;
+		reschedule |= octeon_droq_process_packets(oct, oct->droq[q_no],
+							  MAX_PACKET_BUDGET);
+	}
+
+	if (reschedule)
+		tasklet_schedule(&oct_priv->droq_tasklet);
+}
+
+static int lio_wait_for_oq_pkts(struct octeon_device *oct)
+{
+	struct octeon_device_priv *oct_priv =
+		(struct octeon_device_priv *)oct->priv;
+	int retry = 100, pkt_cnt = 0, pending_pkts = 0;
+	int i;
+
+	do {
+		pending_pkts = 0;
+
+		for (i = 0; i < MAX_OCTEON_OUTPUT_QUEUES; i++) {
+			if (!(oct->io_qmask.oq & (1UL << i)))
+				continue;
+			pkt_cnt += octeon_droq_check_hw_for_pkts(oct,
+								 oct->droq[i]);
+		}
+		if (pkt_cnt > 0) {
+			pending_pkts += pkt_cnt;
+			tasklet_schedule(&oct_priv->droq_tasklet);
+		}
+		pkt_cnt = 0;
+		schedule_timeout_uninterruptible(1);
+
+	} while (retry-- && pending_pkts);
+
+	return pkt_cnt;
+}
+
+void octeon_report_tx_completion_to_bql(void *txq, unsigned int pkts_compl,
+					unsigned int bytes_compl)
+{
+	struct netdev_queue *netdev_queue = txq;
+
+	netdev_tx_completed_queue(netdev_queue, pkts_compl, bytes_compl);
+}
+
+void octeon_update_tx_completion_counters(void *buf, int reqtype,
+					  unsigned int *pkts_compl,
+					  unsigned int *bytes_compl)
+{
+	struct octnet_buf_free_info *finfo;
+	struct sk_buff *skb = NULL;
+	struct octeon_soft_command *sc;
+
+	switch (reqtype) {
+	case REQTYPE_NORESP_NET:
+	case REQTYPE_NORESP_NET_SG:
+		finfo = buf;
+		skb = finfo->skb;
+		break;
+
+	case REQTYPE_RESP_NET_SG:
+	case REQTYPE_RESP_NET:
+		sc = buf;
+		skb = sc->callback_arg;
+		break;
+
+	default:
+		return;
+	}
+
+	(*pkts_compl)++;
+	*bytes_compl += skb->len;
+}
+
+void octeon_report_sent_bytes_to_bql(void *buf, int reqtype)
+{
+	struct octnet_buf_free_info *finfo;
+	struct sk_buff *skb;
+	struct octeon_soft_command *sc;
+	struct netdev_queue *txq;
+
+	switch (reqtype) {
+	case REQTYPE_NORESP_NET:
+	case REQTYPE_NORESP_NET_SG:
+		finfo = buf;
+		skb = finfo->skb;
+		break;
+
+	case REQTYPE_RESP_NET_SG:
+	case REQTYPE_RESP_NET:
+		sc = buf;
+		skb = sc->callback_arg;
+		break;
+
+	default:
+		return;
+	}
+
+	txq = netdev_get_tx_queue(skb->dev, skb_get_queue_mapping(skb));
+	netdev_tx_sent_queue(txq, skb->len);
+}
+
+int octeon_console_debug_enabled(u32 console)
+{
+	return (console_bitmask >> (console)) & 0x1;
+}
+
+/**
+ * \brief Forces all IO queues off on a given device
+ * @param oct Pointer to Octeon device
+ */
+static void force_io_queues_off(struct octeon_device *oct)
+{
+	if ((oct->chip_id == OCTEON_CN66XX) ||
+	    (oct->chip_id == OCTEON_CN68XX)) {
+		/* Reset the Enable bits for Input Queues. */
+		octeon_write_csr(oct, CN6XXX_SLI_PKT_INSTR_ENB, 0);
+
+		/* Reset the Enable bits for Output Queues. */
+		octeon_write_csr(oct, CN6XXX_SLI_PKT_OUT_ENB, 0);
+	}
+}
+
+/**
+ * \brief wait for all pending requests to complete
+ * @param oct Pointer to Octeon device
+ *
+ * Called during shutdown sequence
+ */
+static int wait_for_pending_requests(struct octeon_device *oct)
+{
+	int i, pcount = 0;
+
+	for (i = 0; i < 100; i++) {
+		pcount =
+			atomic_read(&oct->response_list
+				[OCTEON_ORDERED_SC_LIST].pending_req_count);
+		if (pcount)
+			schedule_timeout_uninterruptible(HZ / 10);
+		 else
+			break;
+	}
+
+	if (pcount)
+		return 1;
+
+	return 0;
+}
+
+/**
+ * \brief Cause device to go quiet so it can be safely removed/reset/etc
+ * @param oct Pointer to Octeon device
+ */
+static inline void pcierror_quiesce_device(struct octeon_device *oct)
+{
+	int i;
+
+	/* Disable the input and output queues now. No more packets will
+	 * arrive from Octeon, but we should wait for all packet processing
+	 * to finish.
+	 */
+	force_io_queues_off(oct);
+
+	/* To allow for in-flight requests */
+	schedule_timeout_uninterruptible(100);
+
+	if (wait_for_pending_requests(oct))
+		dev_err(&oct->pci_dev->dev, "There were pending requests\n");
+
+	/* Force all requests waiting to be fetched by OCTEON to complete. */
+	for (i = 0; i < MAX_OCTEON_INSTR_QUEUES; i++) {
+		struct octeon_instr_queue *iq;
+
+		if (!(oct->io_qmask.iq & (1UL << i)))
+			continue;
+		iq = oct->instr_queue[i];
+
+		if (atomic_read(&iq->instr_pending)) {
+			spin_lock_bh(&iq->lock);
+			iq->fill_cnt = 0;
+			iq->octeon_read_index = iq->host_write_index;
+			iq->stats.instr_processed +=
+				atomic_read(&iq->instr_pending);
+			lio_process_iq_request_list(oct, iq);
+			spin_unlock_bh(&iq->lock);
+		}
+	}
+
+	/* Force all pending ordered list requests to time out. */
+	lio_process_ordered_list(oct, 1);
+
+	/* We do not need to wait for output queue packets to be processed. */
+}
+
+/**
+ * \brief Cleanup PCI AER uncorrectable error status
+ * @param dev Pointer to PCI device
+ */
+static void cleanup_aer_uncorrect_error_status(struct pci_dev *dev)
+{
+	int pos = 0x100;
+	u32 status, mask;
+
+	pr_info("%s :\n", __func__);
+
+	pci_read_config_dword(dev, pos + PCI_ERR_UNCOR_STATUS, &status);
+	pci_read_config_dword(dev, pos + PCI_ERR_UNCOR_SEVER, &mask);
+	if (dev->error_state == pci_channel_io_normal)
+		status &= ~mask;        /* Clear corresponding nonfatal bits */
+	else
+		status &= mask;         /* Clear corresponding fatal bits */
+	pci_write_config_dword(dev, pos + PCI_ERR_UNCOR_STATUS, status);
+}
+
+/**
+ * \brief Stop all PCI IO to a given device
+ * @param dev Pointer to Octeon device
+ */
+static void stop_pci_io(struct octeon_device *oct)
+{
+	/* No more instructions will be forwarded. */
+	atomic_set(&oct->status, OCT_DEV_IN_RESET);
+
+	pci_disable_device(oct->pci_dev);
+
+	/* Disable interrupts  */
+	oct->fn_list.disable_interrupt(oct->chip);
+
+	pcierror_quiesce_device(oct);
+
+	/* Release the interrupt line */
+	free_irq(oct->pci_dev->irq, oct);
+
+	if (oct->flags & LIO_FLAG_MSI_ENABLED)
+		pci_disable_msi(oct->pci_dev);
+
+	dev_dbg(&oct->pci_dev->dev, "Device state is now %s\n",
+		lio_get_state_string(&oct->status));
+
+	/* cn63xx_cleanup_aer_uncorrect_error_status(oct->pci_dev); */
+	/* making it a common function for all OCTEON models */
+	cleanup_aer_uncorrect_error_status(oct->pci_dev);
+}
+
+/**
+ * \brief called when PCI error is detected
+ * @param pdev Pointer to PCI device
+ * @param state The current pci connection state
+ *
+ * This function is called after a PCI bus error affecting
+ * this device has been detected.
+ */
+static pci_ers_result_t liquidio_pcie_error_detected(struct pci_dev *pdev,
+						     pci_channel_state_t state)
+{
+	struct octeon_device *oct = pci_get_drvdata(pdev);
+
+	/* Non-correctable Non-fatal errors */
+	if (state == pci_channel_io_normal) {
+		dev_err(&oct->pci_dev->dev, "Non-correctable non-fatal error reported:\n");
+		cleanup_aer_uncorrect_error_status(oct->pci_dev);
+		return PCI_ERS_RESULT_CAN_RECOVER;
+	}
+
+	/* Non-correctable Fatal errors */
+	dev_err(&oct->pci_dev->dev, "Non-correctable FATAL reported by PCI AER driver\n");
+	stop_pci_io(oct);
+
+	/* Always return a DISCONNECT. There is no support for recovery but only
+	 * for a clean shutdown.
+	 */
+	return PCI_ERS_RESULT_DISCONNECT;
+}
+
+/**
+ * \brief mmio handler
+ * @param pdev Pointer to PCI device
+ */
+static pci_ers_result_t liquidio_pcie_mmio_enabled(struct pci_dev *pdev)
+{
+	/* We should never hit this since we never ask for a reset for a Fatal
+	 * Error. We always return DISCONNECT in io_error above.
+	 * But play safe and return RECOVERED for now.
+	 */
+	return PCI_ERS_RESULT_RECOVERED;
+}
+
+/**
+ * \brief called after the pci bus has been reset.
+ * @param pdev Pointer to PCI device
+ *
+ * Restart the card from scratch, as if from a cold-boot. Implementation
+ * resembles the first-half of the octeon_resume routine.
+ */
+static pci_ers_result_t liquidio_pcie_slot_reset(struct pci_dev *pdev)
+{
+	/* We should never hit this since we never ask for a reset for a Fatal
+	 * Error. We always return DISCONNECT in io_error above.
+	 * But play safe and return RECOVERED for now.
+	 */
+	return PCI_ERS_RESULT_RECOVERED;
+}
+
+/**
+ * \brief called when traffic can start flowing again.
+ * @param pdev Pointer to PCI device
+ *
+ * This callback is called when the error recovery driver tells us that
+ * its OK to resume normal operation. Implementation resembles the
+ * second-half of the octeon_resume routine.
+ */
+static void liquidio_pcie_resume(struct pci_dev *pdev)
+{
+	/* Nothing to be done here. */
+}
+
+#ifdef CONFIG_PM
+/**
+ * \brief called when suspending
+ * @param pdev Pointer to PCI device
+ * @param state state to suspend to
+ */
+static int liquidio_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	return 0;
+}
+
+/**
+ * \brief called when resuming
+ * @param pdev Pointer to PCI device
+ */
+static int liquidio_resume(struct pci_dev *pdev)
+{
+	return 0;
+}
+#endif
+
+/* For PCI-E Advanced Error Recovery (AER) Interface */
+static struct pci_error_handlers liquidio_err_handler = {
+	.error_detected = liquidio_pcie_error_detected,
+	.mmio_enabled	= liquidio_pcie_mmio_enabled,
+	.slot_reset	= liquidio_pcie_slot_reset,
+	.resume		= liquidio_pcie_resume,
+};
+
+static const struct pci_device_id liquidio_pci_tbl[] = {
+	{       /* 68xx */
+		PCI_VENDOR_ID_CAVIUM, 0x91, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0
+	},
+	{       /* 66xx */
+		PCI_VENDOR_ID_CAVIUM, 0x92, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0
+	},
+	{
+		0, 0, 0, 0, 0, 0, 0
+	}
+};
+MODULE_DEVICE_TABLE(pci, liquidio_pci_tbl);
+
+static struct pci_driver liquidio_pci_driver = {
+	.name		= "LiquidIO",
+	.id_table	= liquidio_pci_tbl,
+	.probe		= liquidio_probe,
+	.remove		= liquidio_remove,
+	.err_handler	= &liquidio_err_handler,    /* For AER */
+
+#ifdef CONFIG_PM
+	.suspend	= liquidio_suspend,
+	.resume		= liquidio_resume,
+#endif
+
+};
+
+/**
+ * \brief register PCI driver
+ */
+static int liquidio_init_pci(void)
+{
+	return pci_register_driver(&liquidio_pci_driver);
+}
+
+/**
+ * \brief unregister PCI driver
+ */
+static void liquidio_deinit_pci(void)
+{
+	pci_unregister_driver(&liquidio_pci_driver);
+}
+
+/**
+ * \brief check interface state
+ * @param lio per-network private data
+ * @param state_flag flag state to check
+ */
+static inline int ifstate_check(struct lio *lio, int state_flag)
+{
+	return atomic_read(&lio->ifstate) & state_flag;
+}
+
+/**
+ * \brief set interface state
+ * @param lio per-network private data
+ * @param state_flag flag state to set
+ */
+static inline void ifstate_set(struct lio *lio, int state_flag)
+{
+	atomic_set(&lio->ifstate, (atomic_read(&lio->ifstate) | state_flag));
+}
+
+/**
+ * \brief clear interface state
+ * @param lio per-network private data
+ * @param state_flag flag state to clear
+ */
+static inline void ifstate_reset(struct lio *lio, int state_flag)
+{
+	atomic_set(&lio->ifstate, (atomic_read(&lio->ifstate) & ~(state_flag)));
+}
+
+/**
+ * \brief Stop Tx queues
+ * @param netdev network device
+ */
+static inline void txqs_stop(struct net_device *netdev)
+{
+	if (netif_is_multiqueue(netdev)) {
+		int i;
+
+		for (i = 0; i < netdev->num_tx_queues; i++)
+			netif_stop_subqueue(netdev, i);
+	} else {
+		netif_stop_queue(netdev);
+	}
+}
+
+/**
+ * \brief Start Tx queues
+ * @param netdev network device
+ */
+static inline void txqs_start(struct net_device *netdev)
+{
+	if (netif_is_multiqueue(netdev)) {
+		int i;
+
+		for (i = 0; i < netdev->num_tx_queues; i++)
+			netif_start_subqueue(netdev, i);
+	} else {
+		netif_start_queue(netdev);
+	}
+}
+
+/**
+ * \brief Wake Tx queues
+ * @param netdev network device
+ */
+static inline void txqs_wake(struct net_device *netdev)
+{
+	if (netif_is_multiqueue(netdev)) {
+		int i;
+
+		for (i = 0; i < netdev->num_tx_queues; i++)
+			netif_wake_subqueue(netdev, i);
+	} else {
+		netif_wake_queue(netdev);
+	}
+}
+
+/**
+ * \brief Stop Tx queue
+ * @param netdev network device
+ */
+static void stop_txq(struct net_device *netdev)
+{
+	txqs_stop(netdev);
+}
+
+/**
+ * \brief Start Tx queue
+ * @param netdev network device
+ */
+static void start_txq(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+
+	if (lio->linfo.link.s.status) {
+		txqs_start(netdev);
+		return;
+	}
+}
+
+/**
+ * \brief Wake a queue
+ * @param netdev network device
+ * @param q which queue to wake
+ */
+static inline void wake_q(struct net_device *netdev, int q)
+{
+	if (netif_is_multiqueue(netdev))
+		netif_wake_subqueue(netdev, q);
+	else
+		netif_wake_queue(netdev);
+}
+
+/**
+ * \brief Stop a queue
+ * @param netdev network device
+ * @param q which queue to stop
+ */
+static inline void stop_q(struct net_device *netdev, int q)
+{
+	if (netif_is_multiqueue(netdev))
+		netif_stop_subqueue(netdev, q);
+	else
+		netif_stop_queue(netdev);
+}
+
+/**
+ * \brief Check Tx queue status, and take appropriate action
+ * @param lio per-network private data
+ * @returns 0 if full, number of queues woken up otherwise
+ */
+static inline int check_txq_status(struct lio *lio)
+{
+	int ret_val = 0;
+
+	if (netif_is_multiqueue(lio->netdev)) {
+		int numqs = lio->netdev->num_tx_queues;
+		int q, iq = 0;
+
+		/* check each sub-queue state */
+		for (q = 0; q < numqs; q++) {
+			iq = lio->linfo.txpciq[q & (lio->linfo.num_txpciq - 1)];
+			if (octnet_iq_is_full(lio->oct_dev, iq))
+				continue;
+			wake_q(lio->netdev, q);
+			ret_val++;
+		}
+	} else {
+		if (octnet_iq_is_full(lio->oct_dev, lio->txq))
+			return 0;
+		wake_q(lio->netdev, lio->txq);
+		ret_val = 1;
+	}
+	return ret_val;
+}
+
+/**
+ * Remove the node at the head of the list. The list would be empty at
+ * the end of this call if there are no more nodes in the list.
+ */
+static inline struct list_head *list_delete_head(struct list_head *root)
+{
+	struct list_head *node;
+
+	if ((root->prev == root) && (root->next == root))
+		node = NULL;
+	else
+		node = root->next;
+
+	if (node)
+		list_del(node);
+
+	return node;
+}
+
+/**
+ * \brief Delete gather list
+ * @param lio per-network private data
+ */
+static void delete_glist(struct lio *lio)
+{
+	struct octnic_gather *g;
+
+	do {
+		g = (struct octnic_gather *)
+		    list_delete_head(&lio->glist);
+		if (g) {
+			if (g->sg)
+				kfree((void *)((unsigned long)g->sg -
+						g->adjust));
+			kfree(g);
+		}
+	} while (g);
+}
+
+/**
+ * \brief Setup gather list
+ * @param lio per-network private data
+ */
+static int setup_glist(struct lio *lio)
+{
+	int i;
+	struct octnic_gather *g;
+
+	INIT_LIST_HEAD(&lio->glist);
+
+	for (i = 0; i < lio->tx_qsize; i++) {
+		g = kmalloc(sizeof(*g), GFP_KERNEL);
+		if (!g)
+			break;
+		memset(g, 0, sizeof(struct octnic_gather));
+
+		g->sg_size =
+			((ROUNDUP4(OCTNIC_MAX_SG) >> 2) * OCT_SG_ENTRY_SIZE);
+
+		g->sg = kmalloc(g->sg_size + 8, GFP_KERNEL);
+		if (!g->sg) {
+			kfree(g);
+			break;
+		}
+
+		/* The gather component should be aligned on 64-bit boundary */
+		if (((unsigned long)g->sg) & 7) {
+			g->adjust = 8 - (((unsigned long)g->sg) & 7);
+			g->sg = (struct octeon_sg_entry *)
+				((unsigned long)g->sg + g->adjust);
+		}
+		list_add_tail(&g->list, &lio->glist);
+	}
+
+	if (i == lio->tx_qsize)
+		return 0;
+
+	delete_glist(lio);
+	return 1;
+}
+
+/**
+ * \brief Print link information
+ * @param netdev network device
+ */
+static void print_link_info(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+
+	if (atomic_read(&lio->ifstate) & LIO_IFSTATE_REGISTERED) {
+		struct oct_link_info *linfo = &lio->linfo;
+
+		if (linfo->link.s.status) {
+			netif_info(lio, link, lio->netdev, "%d Mbps %s Duplex UP\n",
+				   linfo->link.s.speed,
+				   (linfo->link.s.duplex) ? "Full" : "Half");
+		} else {
+			netif_info(lio, link, lio->netdev, "Link Down\n");
+		}
+	}
+}
+
+/**
+ * \brief Update link status
+ * @param netdev network device
+ * @param ls link status structure
+ *
+ * Called on receipt of a link status response from the core application to
+ * update each interface's link status.
+ */
+static inline void update_link_status(struct net_device *netdev,
+				      union oct_link_status *ls)
+{
+	struct lio *lio = GET_LIO(netdev);
+
+	if ((lio->intf_open) && (lio->linfo.link.u64 != ls->u64)) {
+		lio->linfo.link.u64 = ls->u64;
+
+		print_link_info(netdev);
+
+		if (lio->linfo.link.s.status) {
+			netif_carrier_on(netdev);
+			/* start_txq(netdev); */
+			txqs_wake(netdev);
+		} else {
+			netif_carrier_off(netdev);
+			stop_txq(netdev);
+		}
+	}
+}
+
+/**
+ * \brief Droq packet processor sceduler
+ * @param oct octeon device
+ */
+static
+void liquidio_schedule_droq_pkt_handlers(struct octeon_device *oct)
+{
+	struct octeon_device_priv *oct_priv =
+		(struct octeon_device_priv *)oct->priv;
+	u64 oq_no;
+	struct octeon_droq *droq;
+
+	if (oct->int_status & OCT_DEV_INTR_PKT_DATA) {
+		for (oq_no = 0; oq_no < MAX_OCTEON_OUTPUT_QUEUES; oq_no++) {
+			if (!(oct->droq_intr & (1 << oq_no)))
+				continue;
+
+			droq = oct->droq[oq_no];
+
+			if (droq->ops.poll_mode) {
+				droq->ops.napi_fn(droq);
+				oct_priv->napi_mask |= (1 << oq_no);
+			} else {
+				tasklet_schedule(&oct_priv->droq_tasklet);
+			}
+		}
+	}
+}
+
+/**
+ * \brief Interrupt handler for octeon
+ * @param irq unused
+ * @param dev octeon device
+ */
+static
+irqreturn_t liquidio_intr_handler(int irq __attribute__((unused)), void *dev)
+{
+	struct octeon_device *oct = (struct octeon_device *)dev;
+	irqreturn_t ret;
+
+	/* Disable our interrupts for the duration of ISR */
+	oct->fn_list.disable_interrupt(oct->chip);
+
+	ret = oct->fn_list.process_interrupt_regs(oct);
+
+	if (ret == IRQ_HANDLED)
+		liquidio_schedule_droq_pkt_handlers(oct);
+
+	/* Re-enable our interrupts  */
+	if (!(atomic_read(&oct->status) == OCT_DEV_IN_RESET))
+		oct->fn_list.enable_interrupt(oct->chip);
+
+	return ret;
+}
+
+/**
+ * \brief Setup interrupt for octeon device
+ * @param oct octeon device
+ *
+ *  Enable interrupt in Octeon device as given in the PCI interrupt mask.
+ */
+static int octeon_setup_interrupt(struct octeon_device *oct)
+{
+	int irqret, err;
+
+	err = pci_enable_msi(oct->pci_dev);
+	if (err)
+		dev_warn(&oct->pci_dev->dev, "Reverting to legacy interrupts. Error: %d\n",
+			 err);
+	else
+		oct->flags |= LIO_FLAG_MSI_ENABLED;
+
+	irqret = request_irq(oct->pci_dev->irq, liquidio_intr_handler,
+			     IRQF_SHARED, "octeon", oct);
+	if (irqret) {
+		if (oct->flags & LIO_FLAG_MSI_ENABLED)
+			pci_disable_msi(oct->pci_dev);
+		dev_err(&oct->pci_dev->dev, "Request IRQ failed with code: %d\n",
+			irqret);
+		return 1;
+	}
+
+	return 0;
+}
+
+/**
+ * \brief PCI probe handler
+ * @param pdev PCI device structure
+ * @param ent unused
+ */
+static int liquidio_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct octeon_device *oct_dev = NULL;
+	struct handshake *hs;
+
+	oct_dev = octeon_allocate_device(pdev->device,
+					 sizeof(struct octeon_device_priv));
+	if (!oct_dev) {
+		dev_err(&pdev->dev, "Unable to allocate device\n");
+		return -ENOMEM;
+	}
+
+	dev_info(&pdev->dev, "Initializing device %x:%x.\n",
+		 (u32)pdev->vendor, (u32)pdev->device);
+
+	/* Assign octeon_device for this device to the private data area. */
+	pci_set_drvdata(pdev, oct_dev);
+
+	/* set linux specific device pointer */
+	oct_dev->pci_dev = (void *)pdev;
+
+	hs = &handshake[oct_dev->octeon_id];
+	init_completion(&hs->init);
+	init_completion(&hs->started);
+	hs->pci_dev = pdev;
+
+	if (oct_dev->octeon_id == 0)
+		/* first LiquidIO NIC is detected */
+		complete(&first_stage);
+
+	if (octeon_device_init(oct_dev)) {
+		liquidio_remove(pdev);
+		return -ENOMEM;
+	}
+
+	dev_dbg(&oct_dev->pci_dev->dev, "Device is ready\n");
+
+	return 0;
+}
+
+/**
+ *\brief Destroy resources associated with octeon device
+ * @param pdev PCI device structure
+ * @param ent unused
+ */
+static void octeon_destroy_resources(struct octeon_device *oct)
+{
+	int i;
+	struct octeon_device_priv *oct_priv =
+		(struct octeon_device_priv *)oct->priv;
+
+	struct handshake *hs;
+
+	switch (atomic_read(&oct->status)) {
+	case OCT_DEV_RUNNING:
+	case OCT_DEV_CORE_OK:
+
+		/* No more instructions will be forwarded. */
+		atomic_set(&oct->status, OCT_DEV_IN_RESET);
+
+		oct->app_mode = CVM_DRV_INVALID_APP;
+		dev_dbg(&oct->pci_dev->dev, "Device state is now %s\n",
+			lio_get_state_string(&oct->status));
+
+		schedule_timeout_uninterruptible(HZ / 10);
+
+		/* fallthrough */
+	case OCT_DEV_HOST_OK:
+
+		/* fallthrough */
+	case OCT_DEV_CONSOLE_INIT_DONE:
+		/* Remove any consoles */
+		octeon_remove_consoles(oct);
+
+		/* fallthrough */
+	case OCT_DEV_IO_QUEUES_DONE:
+		if (wait_for_pending_requests(oct))
+			dev_err(&oct->pci_dev->dev, "There were pending requests\n");
+
+		if (lio_wait_for_instr_fetch(oct))
+			dev_err(&oct->pci_dev->dev, "IQ had pending instructions\n");
+
+		/* Disable the input and output queues now. No more packets will
+		 * arrive from Octeon, but we should wait for all packet
+		 * processing to finish.
+		 */
+		oct->fn_list.disable_io_queues(oct);
+
+		if (lio_wait_for_oq_pkts(oct))
+			dev_err(&oct->pci_dev->dev, "OQ had pending packets\n");
+
+		/* Disable interrupts  */
+		oct->fn_list.disable_interrupt(oct->chip);
+
+		/* Release the interrupt line */
+		free_irq(oct->pci_dev->irq, oct);
+
+		if (oct->flags & LIO_FLAG_MSI_ENABLED)
+			pci_disable_msi(oct->pci_dev);
+
+		/* Soft reset the octeon device before exiting */
+		oct->fn_list.soft_reset(oct);
+
+		/* Disable the device, releasing the PCI INT */
+		pci_disable_device(oct->pci_dev);
+
+		/* fallthrough */
+	case OCT_DEV_IN_RESET:
+	case OCT_DEV_DROQ_INIT_DONE:
+		/*atomic_set(&oct->status, OCT_DEV_DROQ_INIT_DONE);*/
+		mdelay(100);
+		for (i = 0; i < MAX_OCTEON_OUTPUT_QUEUES; i++) {
+			if (!(oct->io_qmask.oq & (1UL << i)))
+				continue;
+			octeon_delete_droq(oct, i);
+		}
+
+		/* Force any pending handshakes to complete */
+		for (i = 0; i < MAX_OCTEON_DEVICES; i++) {
+			hs = &handshake[i];
+
+			if (hs->pci_dev) {
+				handshake[oct->octeon_id].init_ok = 0;
+				complete(&handshake[oct->octeon_id].init);
+				handshake[oct->octeon_id].started_ok = 0;
+				complete(&handshake[oct->octeon_id].started);
+			}
+		}
+
+		/* fallthrough */
+	case OCT_DEV_RESP_LIST_INIT_DONE:
+		octeon_delete_response_list(oct);
+
+		/* fallthrough */
+	case OCT_DEV_SC_BUFF_POOL_INIT_DONE:
+		octeon_free_sc_buffer_pool(oct);
+
+		/* fallthrough */
+	case OCT_DEV_INSTR_QUEUE_INIT_DONE:
+		for (i = 0; i < MAX_OCTEON_INSTR_QUEUES; i++) {
+			if (!(oct->io_qmask.iq & (1UL << i)))
+				continue;
+			octeon_delete_instr_queue(oct, i);
+		}
+
+		/* fallthrough */
+	case OCT_DEV_DISPATCH_INIT_DONE:
+		octeon_delete_dispatch_list(oct);
+		cancel_delayed_work_sync(&oct->nic_poll_work.work);
+
+		/* fallthrough */
+	case OCT_DEV_PCI_MAP_DONE:
+		octeon_unmap_pci_barx(oct, 0);
+		octeon_unmap_pci_barx(oct, 1);
+
+		/* fallthrough */
+	case OCT_DEV_BEGIN_STATE:
+		/* Nothing to be done here either */
+		break;
+	}                       /* end switch(oct->status) */
+
+	tasklet_kill(&oct_priv->droq_tasklet);
+}
+
+/**
+ * \brief Send Rx control command
+ * @param lio per-network private data
+ * @param start_stop whether to start or stop
+ */
+static void send_rx_ctrl_cmd(struct lio *lio, int start_stop)
+{
+	struct octnic_ctrl_pkt nctrl;
+	struct octnic_ctrl_params nparams;
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.s.cmd = OCTNET_CMD_RX_CTL;
+	nctrl.ncmd.s.param1 = lio->linfo.ifidx;
+	nctrl.ncmd.s.param2 = start_stop;
+	nctrl.netpndev = (u64)lio->netdev;
+
+	nparams.resp_order = OCTEON_RESP_NORESPONSE;
+
+	if (octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl, nparams) < 0)
+		netif_info(lio, rx_err, lio->netdev, "Failed to send RX Control message\n");
+}
+
+/**
+ * \brief Destroy NIC device interface
+ * @param oct octeon device
+ * @param ifidx which interface to destroy
+ *
+ * Cleanup associated with each interface for an Octeon device  when NIC
+ * module is being unloaded or if initialization fails during load.
+ */
+static void liquidio_destroy_nic_device(struct octeon_device *oct, int ifidx)
+{
+	struct net_device *netdev = oct->props[ifidx].netdev;
+	struct lio *lio;
+
+	if (!netdev) {
+		dev_err(&oct->pci_dev->dev, "%s No netdevice ptr for index %d\n",
+			__func__, ifidx);
+		return;
+	}
+
+	lio = GET_LIO(netdev);
+
+	dev_dbg(&oct->pci_dev->dev, "NIC device cleanup\n");
+
+	send_rx_ctrl_cmd(lio, 0);
+
+	if (atomic_read(&lio->ifstate) & LIO_IFSTATE_RUNNING)
+		txqs_stop(netdev);
+
+	if (atomic_read(&lio->ifstate) & LIO_IFSTATE_REGISTERED)
+		unregister_netdev(netdev);
+
+	delete_glist(lio);
+
+	free_netdev(netdev);
+
+	oct->props[ifidx].netdev = NULL;
+}
+
+/**
+ * \brief Stop complete NIC functionality
+ * @param oct octeon device
+ */
+static int liquidio_stop_nic_module(struct octeon_device *oct)
+{
+	int i, j;
+	struct lio *lio;
+
+	dev_dbg(&oct->pci_dev->dev, "Stopping network interfaces\n");
+	if (!oct->ifcount) {
+		dev_err(&oct->pci_dev->dev, "Init for Octeon was not completed\n");
+		return 1;
+	}
+
+	for (i = 0; i < oct->ifcount; i++) {
+		lio = GET_LIO(oct->props[i].netdev);
+		for (j = 0; j < lio->linfo.num_rxpciq; j++)
+			octeon_unregister_droq_ops(oct, lio->linfo.rxpciq[j]);
+	}
+
+	for (i = 0; i < oct->ifcount; i++)
+		liquidio_destroy_nic_device(oct, i);
+
+	dev_dbg(&oct->pci_dev->dev, "Network interfaces stopped\n");
+	return 0;
+}
+
+/**
+ * \brief Cleans up resources at unload time
+ * @param pdev PCI device structure
+ */
+static void liquidio_remove(struct pci_dev *pdev)
+{
+	struct octeon_device *oct_dev = pci_get_drvdata(pdev);
+
+	dev_dbg(&oct_dev->pci_dev->dev, "Stopping device\n");
+
+	if (oct_dev->app_mode && (oct_dev->app_mode == CVM_DRV_NIC_APP))
+		liquidio_stop_nic_module(oct_dev);
+
+	/* Reset the octeon device and cleanup all memory allocated for
+	 * the octeon device by driver.
+	 */
+	octeon_destroy_resources(oct_dev);
+
+	dev_info(&oct_dev->pci_dev->dev, "Device removed\n");
+
+	/* This octeon device has been removed. Update the global
+	 * data structure to reflect this. Free the device structure.
+	 */
+	octeon_free_device_mem(oct_dev);
+}
+
+/**
+ * \brief Identify the Octeon device and to map the BAR address space
+ * @param oct octeon device
+ */
+static int octeon_chip_specific_setup(struct octeon_device *oct)
+{
+	u32 dev_id, rev_id;
+	int ret = 1;
+
+	pci_read_config_dword(oct->pci_dev, 0, &dev_id);
+	pci_read_config_dword(oct->pci_dev, 8, &rev_id);
+	oct->rev_id = rev_id & 0xff;
+
+	switch (dev_id) {
+	case OCTEON_CN68XX_PCIID:
+		oct->chip_id = OCTEON_CN68XX;
+		ret = lio_setup_cn68xx_octeon_device(oct);
+		break;
+
+	case OCTEON_CN66XX_PCIID:
+		oct->chip_id = OCTEON_CN66XX;
+		ret = lio_setup_cn66xx_octeon_device(oct);
+		break;
+	default:
+		dev_err(&oct->pci_dev->dev, "Unknown device found (dev_id: %x)\n",
+			dev_id);
+	}
+
+	if (!ret)
+		dev_info(&oct->pci_dev->dev, "CN68XX PASS%d.%d %s\n",
+			 OCTEON_MAJOR_REV(oct),
+			 OCTEON_MINOR_REV(oct),
+			 octeon_get_conf(oct)->card_name);
+
+	return ret;
+}
+
+/**
+ * \brief PCI initialization for each Octeon device.
+ * @param oct octeon device
+ */
+static int octeon_pci_os_setup(struct octeon_device *oct)
+{
+	/* setup PCI stuff first */
+	if (pci_enable_device(oct->pci_dev)) {
+		dev_err(&oct->pci_dev->dev, "pci_enable_device failed\n");
+		return 1;
+	}
+
+	if (dma_set_mask_and_coherent(&oct->pci_dev->dev, DMA_BIT_MASK(64))) {
+		dev_err(&oct->pci_dev->dev, "Unexpected DMA device capability\n");
+		return 1;
+	}
+
+	/* Enable PCI DMA Master. */
+	pci_set_master(oct->pci_dev);
+
+	return 0;
+}
+
+/**
+ * \brief Check Tx queue state for a given network buffer
+ * @param lio per-network private data
+ * @param skb network buffer
+ */
+static inline int check_txq_state(struct lio *lio, struct sk_buff *skb)
+{
+	int q = 0, iq = 0;
+
+	if (netif_is_multiqueue(lio->netdev)) {
+		q = skb->queue_mapping;
+		iq = lio->linfo.txpciq[(q & (lio->linfo.num_txpciq - 1))];
+	} else {
+		iq = lio->txq;
+	}
+
+	if (octnet_iq_is_full(lio->oct_dev, iq))
+		return 0;
+	wake_q(lio->netdev, q);
+	return 1;
+}
+
+/**
+ * \brief Unmap and free network buffer
+ * @param buf buffer
+ */
+static void free_netbuf(void *buf)
+{
+	struct sk_buff *skb;
+	struct octnet_buf_free_info *finfo;
+	struct lio *lio;
+
+	finfo = (struct octnet_buf_free_info *)buf;
+	skb = finfo->skb;
+	lio = finfo->lio;
+
+	dma_unmap_single(&lio->oct_dev->pci_dev->dev, finfo->dptr, skb->len,
+			 DMA_TO_DEVICE);
+
+	check_txq_state(lio, skb);
+
+	recv_buffer_free((struct sk_buff *)skb);
+}
+
+/**
+ * \brief Unmap and free gather buffer
+ * @param buf buffer
+ */
+static void free_netsgbuf(void *buf)
+{
+	struct octnet_buf_free_info *finfo;
+	struct sk_buff *skb;
+	struct lio *lio;
+	struct octnic_gather *g;
+	int i, frags;
+
+	finfo = (struct octnet_buf_free_info *)buf;
+	skb = finfo->skb;
+	lio = finfo->lio;
+	g = finfo->g;
+	frags = skb_shinfo(skb)->nr_frags;
+
+	dma_unmap_single(&lio->oct_dev->pci_dev->dev,
+			 g->sg[0].ptr[0], (skb->len - skb->data_len),
+			 DMA_TO_DEVICE);
+
+	i = 1;
+	while (frags--) {
+		struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i - 1];
+
+		pci_unmap_page((lio->oct_dev)->pci_dev,
+			       g->sg[(i >> 2)].ptr[(i & 3)],
+			       frag->size, DMA_TO_DEVICE);
+		i++;
+	}
+
+	dma_unmap_single(&lio->oct_dev->pci_dev->dev,
+			 finfo->dptr, g->sg_size,
+			 DMA_TO_DEVICE);
+
+	spin_lock(&lio->lock);
+	list_add_tail(&g->list, &lio->glist);
+	spin_unlock(&lio->lock);
+
+	check_txq_state(lio, skb);     /* mq support: sub-queue state check */
+
+	recv_buffer_free((struct sk_buff *)skb);
+}
+
+/**
+ * \brief Unmap and free gather buffer with response
+ * @param buf buffer
+ */
+static void free_netsgbuf_with_resp(void *buf)
+{
+	struct octeon_soft_command *sc;
+	struct octnet_buf_free_info *finfo;
+	struct sk_buff *skb;
+	struct lio *lio;
+	struct octnic_gather *g;
+	int i, frags;
+
+	sc = (struct octeon_soft_command *)buf;
+	skb = (struct sk_buff *)sc->callback_arg;
+	finfo = (struct octnet_buf_free_info *)&skb->cb;
+
+	lio = finfo->lio;
+	g = finfo->g;
+	frags = skb_shinfo(skb)->nr_frags;
+
+	dma_unmap_single(&lio->oct_dev->pci_dev->dev,
+			 g->sg[0].ptr[0], (skb->len - skb->data_len),
+			 DMA_TO_DEVICE);
+
+	i = 1;
+	while (frags--) {
+		struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i - 1];
+
+		pci_unmap_page((lio->oct_dev)->pci_dev,
+			       g->sg[(i >> 2)].ptr[(i & 3)],
+			       frag->size, DMA_TO_DEVICE);
+		i++;
+	}
+
+	dma_unmap_single(&lio->oct_dev->pci_dev->dev,
+			 finfo->dptr, g->sg_size,
+			 DMA_TO_DEVICE);
+
+	spin_lock(&lio->lock);
+	list_add_tail(&g->list, &lio->glist);
+	spin_unlock(&lio->lock);
+
+	/* Don't free the skb yet */
+
+	check_txq_state(lio, skb);
+}
+
+/**
+ * \brief Adjust ptp frequency
+ * @param ptp PTP clock info
+ * @param ppb how much to adjust by, in parts-per-billion
+ */
+static int liquidio_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct lio *lio = container_of(ptp, struct lio, ptp_info);
+	struct octeon_device *oct = (struct octeon_device *)lio->oct_dev;
+	u64 comp, delta;
+	unsigned long flags;
+	bool neg_adj = false;
+
+	if (ppb < 0) {
+		neg_adj = true;
+		ppb = -ppb;
+	}
+
+	/* The hardware adds the clock compensation value to the
+	 * PTP clock on every coprocessor clock cycle, so we
+	 * compute the delta in terms of coprocessor clocks.
+	 */
+	delta = (u64)ppb << 32;
+	do_div(delta, oct->coproc_clock_rate);
+
+	spin_lock_irqsave(&lio->ptp_lock, flags);
+	comp = lio_pci_readq(oct, CN6XXX_MIO_PTP_CLOCK_COMP);
+	if (neg_adj)
+		comp -= delta;
+	else
+		comp += delta;
+	lio_pci_writeq(oct, comp, CN6XXX_MIO_PTP_CLOCK_COMP);
+	spin_unlock_irqrestore(&lio->ptp_lock, flags);
+
+	return 0;
+}
+
+/**
+ * \brief Adjust ptp time
+ * @param ptp PTP clock info
+ * @param delta how much to adjust by, in nanosecs
+ */
+static int liquidio_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	unsigned long flags;
+	struct lio *lio = container_of(ptp, struct lio, ptp_info);
+
+	spin_lock_irqsave(&lio->ptp_lock, flags);
+	lio->ptp_adjust += delta;
+	spin_unlock_irqrestore(&lio->ptp_lock, flags);
+
+	return 0;
+}
+
+/**
+ * \brief Get hardware clock time, including any adjustment
+ * @param ptp PTP clock info
+ * @param ts timespec
+ */
+static int liquidio_ptp_gettime(struct ptp_clock_info *ptp,
+				struct timespec64 *ts)
+{
+	u64 ns;
+	u32 remainder;
+	unsigned long flags;
+	struct lio *lio = container_of(ptp, struct lio, ptp_info);
+	struct octeon_device *oct = (struct octeon_device *)lio->oct_dev;
+
+	spin_lock_irqsave(&lio->ptp_lock, flags);
+	ns = lio_pci_readq(oct, CN6XXX_MIO_PTP_CLOCK_HI);
+	ns += lio->ptp_adjust;
+	spin_unlock_irqrestore(&lio->ptp_lock, flags);
+
+	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder);
+	ts->tv_nsec = remainder;
+
+	return 0;
+}
+
+/**
+ * \brief Set hardware clock time. Reset adjustment
+ * @param ptp PTP clock info
+ * @param ts timespec
+ */
+static int liquidio_ptp_settime(struct ptp_clock_info *ptp,
+				const struct timespec64 *ts)
+{
+	u64 ns;
+	unsigned long flags;
+	struct lio *lio = container_of(ptp, struct lio, ptp_info);
+	struct octeon_device *oct = (struct octeon_device *)lio->oct_dev;
+
+	ns = timespec_to_ns(ts);
+
+	spin_lock_irqsave(&lio->ptp_lock, flags);
+	lio_pci_writeq(oct, ns, CN6XXX_MIO_PTP_CLOCK_HI);
+	lio->ptp_adjust = 0;
+	spin_unlock_irqrestore(&lio->ptp_lock, flags);
+
+	return 0;
+}
+
+/**
+ * \brief Check if PTP is enabled
+ * @param ptp PTP clock info
+ * @param rq request
+ * @param on is it on
+ */
+static int liquidio_ptp_enable(struct ptp_clock_info *ptp,
+			       struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+/**
+ * \brief Open PTP clock source
+ * @param netdev network device
+ */
+static void oct_ptp_open(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = (struct octeon_device *)lio->oct_dev;
+
+	spin_lock_init(&lio->ptp_lock);
+
+	snprintf(lio->ptp_info.name, 16, "%s", netdev->name);
+	lio->ptp_info.owner = THIS_MODULE;
+	lio->ptp_info.max_adj = 250000000;
+	lio->ptp_info.n_alarm = 0;
+	lio->ptp_info.n_ext_ts = 0;
+	lio->ptp_info.n_per_out = 0;
+	lio->ptp_info.pps = 0;
+	lio->ptp_info.adjfreq = liquidio_ptp_adjfreq;
+	lio->ptp_info.adjtime = liquidio_ptp_adjtime;
+	lio->ptp_info.gettime64 = liquidio_ptp_gettime;
+	lio->ptp_info.settime64 = liquidio_ptp_settime;
+	lio->ptp_info.enable = liquidio_ptp_enable;
+
+	lio->ptp_adjust = 0;
+
+	lio->ptp_clock = ptp_clock_register(&lio->ptp_info,
+					     &oct->pci_dev->dev);
+
+	if (IS_ERR(lio->ptp_clock))
+		lio->ptp_clock = NULL;
+}
+
+/**
+ * \brief Init PTP clock
+ * @param oct octeon device
+ */
+static void liquidio_ptp_init(struct octeon_device *oct)
+{
+	u64 clock_comp, cfg;
+
+	clock_comp = (u64)NSEC_PER_SEC << 32;
+	do_div(clock_comp, oct->coproc_clock_rate);
+	lio_pci_writeq(oct, clock_comp, CN6XXX_MIO_PTP_CLOCK_COMP);
+
+	/* Enable */
+	cfg = lio_pci_readq(oct, CN6XXX_MIO_PTP_CLOCK_CFG);
+	lio_pci_writeq(oct, cfg | 0x01, CN6XXX_MIO_PTP_CLOCK_CFG);
+}
+
+/**
+ * \brief Load firmware to device
+ * @param oct octeon device
+ *
+ * Maps device to firmware filename, requests firmware, and downloads it
+ */
+static int load_firmware(struct octeon_device *oct)
+{
+	int ret = 0;
+	const struct firmware *fw;
+	char fw_name[LIO_MAX_FW_FILENAME_LEN];
+	char *tmp_fw_type;
+
+	if (strncmp(fw_type, LIO_FW_NAME_TYPE_NONE,
+		    sizeof(LIO_FW_NAME_TYPE_NONE)) == 0) {
+		dev_info(&oct->pci_dev->dev, "Skipping firmware load\n");
+		return ret;
+	}
+
+	if (fw_type[0] == '\0')
+		tmp_fw_type = LIO_FW_NAME_TYPE_NIC;
+	else
+		tmp_fw_type = fw_type;
+
+	sprintf(fw_name, "%s%s%s_%s%s", LIO_FW_DIR, LIO_FW_BASE_NAME,
+		octeon_get_conf(oct)->card_name, tmp_fw_type,
+		LIO_FW_NAME_SUFFIX);
+
+	ret = request_firmware(&fw, fw_name, &oct->pci_dev->dev);
+	if (ret) {
+		dev_err(&oct->pci_dev->dev, "Request firmware failed. Could not find file %s.\n.",
+			fw_name);
+		return ret;
+	}
+
+	ret = octeon_download_firmware(oct, fw->data, fw->size);
+
+	release_firmware(fw);
+
+	return ret;
+}
+
+/**
+ * \brief Setup output queue
+ * @param oct octeon device
+ * @param q_no which queue
+ * @param num_descs how many descriptors
+ * @param desc_size size of each descriptor
+ * @param app_ctx application context
+ */
+static int octeon_setup_droq(struct octeon_device *oct, int q_no, int num_descs,
+			     int desc_size, void *app_ctx)
+{
+	int ret_val = 0;
+
+	dev_dbg(&oct->pci_dev->dev, "Creating Droq: %d\n", q_no);
+	/* droq creation and local register settings. */
+	ret_val = octeon_create_droq(oct, q_no, num_descs, desc_size, app_ctx);
+	if (ret_val == -1)
+		return ret_val;
+
+	if (ret_val == 1) {
+		dev_dbg(&oct->pci_dev->dev, "Using default droq %d\n", q_no);
+		return 0;
+	}
+	/* tasklet creation for the droq */
+
+	/* Enable the droq queues */
+	octeon_set_droq_pkt_op(oct, q_no, 1);
+
+	/* Send Credit for Octeon Output queues. Credits are always
+	 * sent after the output queue is enabled.
+	 */
+	writel(oct->droq[q_no]->max_count,
+	       oct->droq[q_no]->pkts_credit_reg);
+
+	return ret_val;
+}
+
+/**
+ * \brief Callback for getting interface configuration
+ * @param status status of request
+ * @param buf pointer to resp structure
+ */
+static void if_cfg_callback(struct octeon_device *oct,
+			    u32 status,
+			    void *buf)
+{
+	struct octeon_soft_command *sc = (struct octeon_soft_command *)buf;
+	struct liquidio_if_cfg_resp *resp;
+	struct liquidio_if_cfg_context *ctx;
+
+	resp = (struct liquidio_if_cfg_resp *)sc->virtrptr;
+	ctx  = (struct liquidio_if_cfg_context *)sc->ctxptr;
+
+	oct = lio_get_device(ctx->octeon_id);
+	if (resp->status)
+		dev_err(&oct->pci_dev->dev, "nic if cfg instruction failed. Status: %llx\n",
+			CVM_CAST64(resp->status));
+	ACCESS_ONCE(ctx->cond) = 1;
+
+	/* This barrier is required to be sure that the response has been
+	 * written fully before waking up the handler
+	 */
+	wmb();
+
+	wake_up_interruptible(&ctx->wc);
+}
+
+/**
+ * \brief Select queue based on hash
+ * @param dev Net device
+ * @param skb sk_buff structure
+ * @returns selected queue number
+ */
+static u16 select_q(struct net_device *dev, struct sk_buff *skb,
+		    void *accel_priv, select_queue_fallback_t fallback)
+{
+	int qindex;
+	struct lio *lio;
+
+	lio = GET_LIO(dev);
+	/* select queue on chosen queue_mapping or core */
+	qindex = skb_rx_queue_recorded(skb) ?
+		 skb_get_rx_queue(skb) : smp_processor_id();
+	return (u16)(qindex & (lio->linfo.num_txpciq - 1));
+}
+
+/** Routine to push packets arriving on Octeon interface upto network layer.
+ * @param oct_id   - octeon device id.
+ * @param skbuff   - skbuff struct to be passed to network layer.
+ * @param len      - size of total data received.
+ * @param rh       - Control header associated with the packet
+ * @param param    - additional control data with the packet
+ */
+static void
+liquidio_push_packet(u32 octeon_id,
+		     void *skbuff,
+		     u32 len,
+		     union octeon_rh *rh,
+		     void *param)
+{
+	struct napi_struct *napi = param;
+	struct octeon_device *oct = lio_get_device(octeon_id);
+	struct sk_buff *skb = (struct sk_buff *)skbuff;
+	struct skb_shared_hwtstamps *shhwtstamps;
+	u64 ns;
+	struct net_device *netdev =
+		(struct net_device *)oct->props[rh->r_dh.link].netdev;
+	struct octeon_droq *droq = container_of(param, struct octeon_droq,
+						napi);
+	if (netdev) {
+		int packet_was_received;
+		struct lio *lio = GET_LIO(netdev);
+
+		/* Do not proceed if the interface is not in RUNNING state. */
+		if (!ifstate_check(lio, LIO_IFSTATE_RUNNING)) {
+			recv_buffer_free(skb);
+			droq->stats.rx_dropped++;
+			return;
+		}
+
+		skb->dev = netdev;
+
+		if (rh->r_dh.has_hwtstamp) {
+			/* timestamp is included from the hardware at the
+			 * beginning of the packet.
+			 */
+			if (ifstate_check(lio,
+					  LIO_IFSTATE_RX_TIMESTAMP_ENABLED)) {
+				/* Nanoseconds are in the first 64-bits
+				 * of the packet.
+				 */
+				memcpy(&ns, (skb->data), sizeof(ns));
+				shhwtstamps = skb_hwtstamps(skb);
+				shhwtstamps->hwtstamp =
+					ns_to_ktime(ns + lio->ptp_adjust);
+			}
+			skb_pull(skb, sizeof(ns));
+		}
+
+		skb->protocol = eth_type_trans(skb, skb->dev);
+
+		if ((netdev->features & NETIF_F_RXCSUM) &&
+		    (rh->r_dh.csum_verified == CNNIC_CSUM_VERIFIED))
+			/* checksum has already been verified */
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+		else
+			skb->ip_summed = CHECKSUM_NONE;
+
+		packet_was_received = napi_gro_receive(napi, skb) != GRO_DROP;
+
+		if (packet_was_received) {
+			droq->stats.rx_bytes_received += len;
+			droq->stats.rx_pkts_received++;
+			netdev->last_rx = jiffies;
+		} else {
+			droq->stats.rx_dropped++;
+			netif_info(lio, rx_err, lio->netdev,
+				   "droq:%d  error rx_dropped:%llu\n",
+				   droq->q_no, droq->stats.rx_dropped);
+		}
+
+	} else {
+		recv_buffer_free(skb);
+	}
+}
+
+/**
+ * \brief wrapper for calling napi_schedule
+ * @param param parameters to pass to napi_schedule
+ *
+ * Used when scheduling on different CPUs
+ */
+static void napi_schedule_wrapper(void *param)
+{
+	struct napi_struct *napi = param;
+
+	napi_schedule(napi);
+}
+
+/**
+ * \brief callback when receive interrupt occurs and we are in NAPI mode
+ * @param arg pointer to octeon output queue
+ */
+static void liquidio_napi_drv_callback(void *arg)
+{
+	struct octeon_droq *droq = arg;
+	int this_cpu = smp_processor_id();
+
+	if (droq->cpu_id == this_cpu) {
+		napi_schedule(&droq->napi);
+	} else {
+		struct call_single_data *csd = &droq->csd;
+
+		csd->func = napi_schedule_wrapper;
+		csd->info = &droq->napi;
+		csd->flags = 0;
+
+		smp_call_function_single_async(droq->cpu_id, csd);
+	}
+}
+
+/**
+ * \brief Main NAPI poll function
+ * @param droq octeon output queue
+ * @param budget maximum number of items to process
+ */
+static int liquidio_napi_do_rx(struct octeon_droq *droq, int budget)
+{
+	int work_done;
+	struct lio *lio = GET_LIO(droq->napi.dev);
+	struct octeon_device *oct = lio->oct_dev;
+
+	work_done = octeon_process_droq_poll_cmd(oct, droq->q_no,
+						 POLL_EVENT_PROCESS_PKTS,
+						 budget);
+	if (work_done < 0) {
+		netif_info(lio, rx_err, lio->netdev,
+			   "Receive work_done < 0, rxq:%d\n", droq->q_no);
+		goto octnet_napi_finish;
+	}
+
+	if (work_done > budget)
+		dev_err(&oct->pci_dev->dev, ">>>> %s work_done: %d budget: %d\n",
+			__func__, work_done, budget);
+
+	return work_done;
+
+octnet_napi_finish:
+	napi_complete(&droq->napi);
+	octeon_process_droq_poll_cmd(oct, droq->q_no, POLL_EVENT_ENABLE_INTR,
+				     0);
+	return 0;
+}
+
+/**
+ * \brief Entry point for NAPI polling
+ * @param napi NAPI structure
+ * @param budget maximum number of items to process
+ */
+static int liquidio_napi_poll(struct napi_struct *napi, int budget)
+{
+	struct octeon_droq *droq;
+	int work_done;
+
+	droq = container_of(napi, struct octeon_droq, napi);
+
+	work_done = liquidio_napi_do_rx(droq, budget);
+
+	if (work_done < budget) {
+		napi_complete(napi);
+		octeon_process_droq_poll_cmd(droq->oct_dev, droq->q_no,
+					     POLL_EVENT_ENABLE_INTR, 0);
+		return 0;
+	}
+
+	return work_done;
+}
+
+/**
+ * \brief Setup input and output queues
+ * @param octeon_dev octeon device
+ * @param net_device Net device
+ *
+ * Note: Queues are with respect to the octeon device. Thus
+ * an input queue is for egress packets, and output queues
+ * are for ingress packets.
+ */
+static inline int setup_io_queues(struct octeon_device *octeon_dev,
+				  struct net_device *net_device)
+{
+	static int first_time = 1;
+	static struct octeon_droq_ops droq_ops;
+	static int cpu_id;
+	static int cpu_id_modulus;
+	struct octeon_droq *droq;
+	struct napi_struct *napi;
+	int q, q_no, retval = 0;
+	struct lio *lio;
+	int num_tx_descs;
+
+	lio = GET_LIO(net_device);
+	if (first_time) {
+		first_time = 0;
+		memset(&droq_ops, 0, sizeof(struct octeon_droq_ops));
+
+		droq_ops.fptr = liquidio_push_packet;
+
+		droq_ops.poll_mode = 1;
+		droq_ops.napi_fn = liquidio_napi_drv_callback;
+		cpu_id = 0;
+		cpu_id_modulus = num_present_cpus();
+	}
+
+	/* set up DROQs. */
+	for (q = 0; q < lio->linfo.num_rxpciq; q++) {
+		q_no = lio->linfo.rxpciq[q];
+
+		retval = octeon_setup_droq(octeon_dev, q_no,
+					   CFG_GET_NUM_RX_DESCS_NIC_IF
+						   (octeon_get_conf(octeon_dev),
+						   lio->ifidx),
+					   CFG_GET_NUM_RX_BUF_SIZE_NIC_IF
+						   (octeon_get_conf(octeon_dev),
+						   lio->ifidx), NULL);
+		if (retval) {
+			dev_err(&octeon_dev->pci_dev->dev,
+				" %s : Runtime DROQ(RxQ) creation failed.\n",
+				__func__);
+			return 1;
+		}
+
+		droq = octeon_dev->droq[q_no];
+		napi = &droq->napi;
+		netif_napi_add(net_device, napi, liquidio_napi_poll, 64);
+
+		/* designate a CPU for this droq */
+		droq->cpu_id = cpu_id;
+		cpu_id++;
+		if (cpu_id >= cpu_id_modulus)
+			cpu_id = 0;
+
+		octeon_register_droq_ops(octeon_dev, q_no, &droq_ops);
+	}
+
+	/* set up IQs. */
+	for (q = 0; q < lio->linfo.num_txpciq; q++) {
+		num_tx_descs = CFG_GET_NUM_TX_DESCS_NIC_IF(octeon_get_conf
+							   (octeon_dev),
+							   lio->ifidx);
+		retval = octeon_setup_iq(octeon_dev, lio->linfo.txpciq[q],
+					 num_tx_descs,
+					 netdev_get_tx_queue(net_device, q));
+		if (retval) {
+			dev_err(&octeon_dev->pci_dev->dev,
+				" %s : Runtime IQ(TxQ) creation failed.\n",
+				__func__);
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * \brief Poll routine for checking transmit queue status
+ * @param work work_struct data structure
+ */
+static void octnet_poll_check_txq_status(struct work_struct *work)
+{
+	struct cavium_wk *wk = (struct cavium_wk *)work;
+	struct lio *lio = (struct lio *)wk->ctxptr;
+
+	if (!ifstate_check(lio, LIO_IFSTATE_RUNNING))
+		return;
+
+	check_txq_status(lio);
+	queue_delayed_work(lio->txq_status_wq.wq,
+			   &lio->txq_status_wq.wk.work, msecs_to_jiffies(1));
+}
+
+/**
+ * \brief Sets up the txq poll check
+ * @param netdev network device
+ */
+static inline void setup_tx_poll_fn(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+
+	lio->txq_status_wq.wq = create_workqueue("txq-status");
+	if (!lio->txq_status_wq.wq) {
+		dev_err(&oct->pci_dev->dev, "unable to create cavium txq status wq\n");
+		return;
+	}
+	INIT_DELAYED_WORK(&lio->txq_status_wq.wk.work,
+			  octnet_poll_check_txq_status);
+	lio->txq_status_wq.wk.ctxptr = lio;
+	queue_delayed_work(lio->txq_status_wq.wq,
+			   &lio->txq_status_wq.wk.work, msecs_to_jiffies(1));
+}
+
+/**
+ * \brief Net device open for LiquidIO
+ * @param netdev network device
+ */
+static int liquidio_open(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct napi_struct *napi, *n;
+
+	list_for_each_entry_safe(napi, n, &netdev->napi_list, dev_list)
+		napi_enable(napi);
+
+	oct_ptp_open(netdev);
+
+	ifstate_set(lio, LIO_IFSTATE_RUNNING);
+	setup_tx_poll_fn(netdev);
+	start_txq(netdev);
+
+	netif_info(lio, ifup, lio->netdev, "Interface Open, ready for traffic\n");
+	try_module_get(THIS_MODULE);
+
+	/* tell Octeon to start forwarding packets to host */
+	send_rx_ctrl_cmd(lio, 1);
+
+	/* Ready for link status updates */
+	lio->intf_open = 1;
+
+	dev_info(&oct->pci_dev->dev, "%s interface is opened\n",
+		 netdev->name);
+
+	return 0;
+}
+
+/**
+ * \brief Net device stop for LiquidIO
+ * @param netdev network device
+ */
+static int liquidio_stop(struct net_device *netdev)
+{
+	struct napi_struct *napi, *n;
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+
+	netif_info(lio, ifdown, lio->netdev, "Stopping interface!\n");
+	/* Inform that netif carrier is down */
+	lio->intf_open = 0;
+	lio->linfo.link.s.status = 0;
+
+	netif_carrier_off(netdev);
+
+	/* tell Octeon to stop forwarding packets to host */
+	send_rx_ctrl_cmd(lio, 0);
+
+	cancel_delayed_work_sync(&lio->txq_status_wq.wk.work);
+	flush_workqueue(lio->txq_status_wq.wq);
+	destroy_workqueue(lio->txq_status_wq.wq);
+
+	if (lio->ptp_clock) {
+		ptp_clock_unregister(lio->ptp_clock);
+		lio->ptp_clock = NULL;
+	}
+
+	ifstate_reset(lio, LIO_IFSTATE_RUNNING);
+
+	/* This is a hack that allows DHCP to continue working. */
+	set_bit(__LINK_STATE_START, &lio->netdev->state);
+
+	list_for_each_entry_safe(napi, n, &netdev->napi_list, dev_list)
+		napi_disable(napi);
+
+	txqs_stop(netdev);
+
+	dev_info(&oct->pci_dev->dev, "%s interface is stopped\n", netdev->name);
+	module_put(THIS_MODULE);
+
+	return 0;
+}
+
+void liquidio_link_ctrl_cmd_completion(void *nctrl_ptr)
+{
+	struct octnic_ctrl_pkt *nctrl = (struct octnic_ctrl_pkt *)nctrl_ptr;
+	struct net_device *netdev = (struct net_device *)nctrl->netpndev;
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+
+	switch (nctrl->ncmd.s.cmd) {
+	case OCTNET_CMD_CHANGE_DEVFLAGS:
+	case OCTNET_CMD_SET_MULTI_LIST:
+		break;
+
+	case OCTNET_CMD_CHANGE_MACADDR:
+		/* If command is successful, change the MACADDR. */
+		netif_info(lio, probe, lio->netdev, " MACAddr changed to 0x%llx\n",
+			   CVM_CAST64(nctrl->udd[0]));
+		dev_info(&oct->pci_dev->dev, "%s MACAddr changed to 0x%llx\n",
+			 netdev->name, CVM_CAST64(nctrl->udd[0]));
+		memcpy(netdev->dev_addr, ((u8 *)&nctrl->udd[0]) + 2, ETH_ALEN);
+		break;
+
+	case OCTNET_CMD_CHANGE_MTU:
+		/* If command is successful, change the MTU. */
+		netif_info(lio, probe, lio->netdev, " MTU Changed from %d to %d\n",
+			   netdev->mtu, nctrl->ncmd.s.param2);
+		dev_info(&oct->pci_dev->dev, "%s MTU Changed from %d to %d\n",
+			 netdev->name, netdev->mtu,
+			 nctrl->ncmd.s.param2);
+		netdev->mtu = nctrl->ncmd.s.param2;
+		break;
+
+	case OCTNET_CMD_GPIO_ACCESS:
+		netif_info(lio, probe, lio->netdev, "LED Flashing visual identification\n");
+
+		break;
+
+	case OCTNET_CMD_LRO_ENABLE:
+		dev_info(&oct->pci_dev->dev, "%s LRO Enabled\n", netdev->name);
+		break;
+
+	case OCTNET_CMD_LRO_DISABLE:
+		dev_info(&oct->pci_dev->dev, "%s LRO Disabled\n",
+			 netdev->name);
+		break;
+
+	case OCTNET_CMD_VERBOSE_ENABLE:
+		dev_info(&oct->pci_dev->dev, "%s LRO Enabled\n", netdev->name);
+		break;
+
+	case OCTNET_CMD_VERBOSE_DISABLE:
+		dev_info(&oct->pci_dev->dev, "%s LRO Disabled\n",
+			 netdev->name);
+		break;
+
+	case OCTNET_CMD_SET_SETTINGS:
+		dev_info(&oct->pci_dev->dev, "%s settings changed\n",
+			 netdev->name);
+
+		break;
+
+	default:
+		dev_err(&oct->pci_dev->dev, "%s Unknown cmd %d\n", __func__,
+			nctrl->ncmd.s.cmd);
+	}
+}
+
+/**
+ * \brief Converts a mask based on net device flags
+ * @param netdev network device
+ *
+ * This routine generates a octnet_ifflags mask from the net device flags
+ * received from the OS.
+ */
+static inline enum octnet_ifflags get_new_flags(struct net_device *netdev)
+{
+	enum octnet_ifflags f = OCTNET_IFFLAG_UNICAST;
+
+	if (netdev->flags & IFF_PROMISC)
+		f |= OCTNET_IFFLAG_PROMISC;
+
+	if (netdev->flags & IFF_ALLMULTI)
+		f |= OCTNET_IFFLAG_ALLMULTI;
+
+	if (netdev->flags & IFF_MULTICAST) {
+		f |= OCTNET_IFFLAG_MULTICAST;
+
+		/* Accept all multicast addresses if there are more than we
+		 * can handle
+		 */
+		if (netdev_mc_count(netdev) > MAX_OCTEON_MULTICAST_ADDR)
+			f |= OCTNET_IFFLAG_ALLMULTI;
+	}
+
+	if (netdev->flags & IFF_BROADCAST)
+		f |= OCTNET_IFFLAG_BROADCAST;
+
+	return f;
+}
+
+/**
+ * \brief Net device set_multicast_list
+ * @param netdev network device
+ */
+static void liquidio_set_mcast_list(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct octnic_ctrl_pkt nctrl;
+	struct octnic_ctrl_params nparams;
+	struct netdev_hw_addr *ha;
+	u64 *mc;
+	int ret, i;
+	int mc_count = min(netdev_mc_count(netdev), MAX_OCTEON_MULTICAST_ADDR);
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	/* Create a ctrl pkt command to be sent to core app. */
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = OCTNET_CMD_SET_MULTI_LIST;
+	nctrl.ncmd.s.param1 = lio->linfo.ifidx;
+	nctrl.ncmd.s.param2 = get_new_flags(netdev);
+	nctrl.ncmd.s.param3 = mc_count;
+	nctrl.ncmd.s.more = mc_count;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+
+	/* copy all the addresses into the udd */
+	i = 0;
+	mc = &nctrl.udd[0];
+	netdev_for_each_mc_addr(ha, netdev) {
+		*mc = 0;
+		memcpy(((u8 *)mc) + 2, ha->addr, ETH_ALEN);
+		/* no need to swap bytes */
+
+		if (++mc > &nctrl.udd[mc_count])
+			break;
+	}
+
+	/* Apparently, any activity in this call from the kernel has to
+	 * be atomic. So we won't wait for response.
+	 */
+	nctrl.wait_time = 0;
+
+	nparams.resp_order = OCTEON_RESP_NORESPONSE;
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl, nparams);
+	if (ret < 0) {
+		dev_err(&oct->pci_dev->dev, "DEVFLAGS change failed in core (ret: 0x%x)\n",
+			ret);
+	}
+}
+
+/**
+ * \brief Net device set_mac_address
+ * @param netdev network device
+ */
+static int liquidio_set_mac(struct net_device *netdev, void *p)
+{
+	int ret = 0;
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct sockaddr *addr = (struct sockaddr *)p;
+	struct octnic_ctrl_pkt nctrl;
+	struct octnic_ctrl_params nparams;
+
+	if ((!is_valid_ether_addr(addr->sa_data)) ||
+	    (ifstate_check(lio, LIO_IFSTATE_RUNNING)))
+		return -EADDRNOTAVAIL;
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = OCTNET_CMD_CHANGE_MACADDR;
+	nctrl.ncmd.s.param1 = lio->linfo.ifidx;
+	nctrl.ncmd.s.param2 = 0;
+	nctrl.ncmd.s.more = 1;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+	nctrl.wait_time = 100;
+
+	nctrl.udd[0] = 0;
+	/* The MAC Address is presented in network byte order. */
+	memcpy((u8 *)&nctrl.udd[0] + 2, addr->sa_data, ETH_ALEN);
+
+	nparams.resp_order = OCTEON_RESP_ORDERED;
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl, nparams);
+	if (ret < 0) {
+		dev_err(&oct->pci_dev->dev, "MAC Address change failed\n");
+		return -ENOMEM;
+	}
+	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
+	memcpy(((u8 *)&lio->linfo.hw_addr) + 2, addr->sa_data, ETH_ALEN);
+
+	return 0;
+}
+
+/**
+ * \brief Net device get_stats
+ * @param netdev network device
+ */
+static struct net_device_stats *liquidio_get_stats(struct net_device *netdev)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct net_device_stats *stats = &netdev->stats;
+	struct octeon_device *oct;
+	u64 pkts = 0, drop = 0, bytes = 0;
+	struct oct_droq_stats *oq_stats;
+	struct oct_iq_stats *iq_stats;
+	int i, iq_no, oq_no;
+
+	oct = lio->oct_dev;
+
+	for (i = 0; i < lio->linfo.num_txpciq; i++) {
+		iq_no = lio->linfo.txpciq[i];
+		iq_stats = &oct->instr_queue[iq_no]->stats;
+		pkts += iq_stats->tx_done;
+		drop += iq_stats->tx_dropped;
+		bytes += iq_stats->tx_tot_bytes;
+	}
+
+	stats->tx_packets = pkts;
+	stats->tx_bytes = bytes;
+	stats->tx_dropped = drop;
+
+	pkts = 0;
+	drop = 0;
+	bytes = 0;
+
+	for (i = 0; i < lio->linfo.num_rxpciq; i++) {
+		oq_no = lio->linfo.rxpciq[i];
+		oq_stats = &oct->droq[oq_no]->stats;
+		pkts += oq_stats->rx_pkts_received;
+		drop += (oq_stats->rx_dropped +
+			 oq_stats->dropped_nodispatch +
+			 oq_stats->dropped_toomany +
+			 oq_stats->dropped_nomem);
+		bytes += oq_stats->rx_bytes_received;
+	}
+
+	stats->rx_bytes = bytes;
+	stats->rx_packets = pkts;
+	stats->rx_dropped = drop;
+
+	return stats;
+}
+
+/**
+ * \brief Net device change_mtu
+ * @param netdev network device
+ */
+static int liquidio_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct octnic_ctrl_pkt nctrl;
+	struct octnic_ctrl_params nparams;
+	int max_frm_size = new_mtu + OCTNET_FRM_HEADER_SIZE;
+	int ret = 0;
+
+	/* Limit the MTU to make sure the ethernet packets are between 64 bytes
+	 * and 65535 bytes
+	 */
+	if ((max_frm_size < OCTNET_MIN_FRM_SIZE) ||
+	    (max_frm_size > OCTNET_MAX_FRM_SIZE)) {
+		dev_err(&oct->pci_dev->dev, "Invalid MTU: %d\n", new_mtu);
+		dev_err(&oct->pci_dev->dev, "Valid range %d and %d\n",
+			(OCTNET_MIN_FRM_SIZE - OCTNET_FRM_HEADER_SIZE),
+			(OCTNET_MAX_FRM_SIZE - OCTNET_FRM_HEADER_SIZE));
+		return -EINVAL;
+	}
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = OCTNET_CMD_CHANGE_MTU;
+	nctrl.ncmd.s.param1 = lio->linfo.ifidx;
+	nctrl.ncmd.s.param2 = new_mtu;
+	nctrl.wait_time = 100;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+
+	nparams.resp_order = OCTEON_RESP_ORDERED;
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl, nparams);
+	if (ret < 0) {
+		dev_err(&oct->pci_dev->dev, "Failed to set MTU\n");
+		return -1;
+	}
+
+	lio->mtu = new_mtu;
+
+	return 0;
+}
+
+/**
+ * \brief Handler for SIOCSHWTSTAMP ioctl
+ * @param netdev network device
+ * @param ifr interface request
+ * @param cmd command
+ */
+static int hwtstamp_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
+{
+	struct hwtstamp_config conf;
+	struct lio *lio = GET_LIO(netdev);
+
+	if (copy_from_user(&conf, ifr->ifr_data, sizeof(conf)))
+		return -EFAULT;
+
+	if (conf.flags)
+		return -EINVAL;
+
+	switch (conf.tx_type) {
+	case HWTSTAMP_TX_ON:
+	case HWTSTAMP_TX_OFF:
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (conf.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		break;
+	case HWTSTAMP_FILTER_ALL:
+	case HWTSTAMP_FILTER_SOME:
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+		conf.rx_filter = HWTSTAMP_FILTER_ALL;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	if (conf.rx_filter == HWTSTAMP_FILTER_ALL)
+		ifstate_set(lio, LIO_IFSTATE_RX_TIMESTAMP_ENABLED);
+
+	else
+		ifstate_reset(lio, LIO_IFSTATE_RX_TIMESTAMP_ENABLED);
+
+	return copy_to_user(ifr->ifr_data, &conf, sizeof(conf)) ? -EFAULT : 0;
+}
+
+/**
+ * \brief ioctl handler
+ * @param netdev network device
+ * @param ifr interface request
+ * @param cmd command
+ */
+static int liquidio_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
+{
+	switch (cmd) {
+	case SIOCSHWTSTAMP:
+		return hwtstamp_ioctl(netdev, ifr, cmd);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+/**
+ * \brief handle a Tx timestamp response
+ * @param status response status
+ * @param buf pointer to skb
+ */
+static void handle_timestamp(struct octeon_device *oct,
+			     u32 status,
+			     void *buf)
+{
+	struct octnet_buf_free_info *finfo;
+	struct octeon_soft_command *sc;
+	struct oct_timestamp_resp *resp;
+	struct lio *lio;
+	struct sk_buff *skb = (struct sk_buff *)buf;
+
+	finfo = (struct octnet_buf_free_info *)skb->cb;
+	lio = finfo->lio;
+	sc = finfo->sc;
+	oct = lio->oct_dev;
+	resp = (struct oct_timestamp_resp *)sc->virtrptr;
+
+	if (status != OCTEON_REQUEST_DONE) {
+		dev_err(&oct->pci_dev->dev, "Tx timestamp instruction failed. Status: %llx\n",
+			CVM_CAST64(status));
+		resp->timestamp = 0;
+	}
+
+	octeon_swap_8B_data(&resp->timestamp, 1);
+
+	if (unlikely((skb_shinfo(skb)->tx_flags | SKBTX_IN_PROGRESS) != 0)) {
+		struct skb_shared_hwtstamps ts;
+		u64 ns = resp->timestamp;
+
+		netif_info(lio, tx_done, lio->netdev,
+			   "Got resulting SKBTX_HW_TSTAMP skb=%p ns=%016llu\n",
+			   skb, (unsigned long long)ns);
+		ts.hwtstamp = ns_to_ktime(ns + lio->ptp_adjust);
+		skb_tstamp_tx(skb, &ts);
+	}
+
+	octeon_free_soft_command(oct, sc);
+	recv_buffer_free(skb);
+}
+
+/* \brief Send a data packet that will be timestamped
+ * @param oct octeon device
+ * @param ndata pointer to network data
+ * @param finfo pointer to private network data
+ */
+static inline int send_nic_timestamp_pkt(struct octeon_device *oct,
+					 struct octnic_data_pkt *ndata,
+					 struct octnet_buf_free_info *finfo,
+					 int xmit_more)
+{
+	int retval;
+	struct octeon_soft_command *sc;
+	struct octeon_instr_ih *ih;
+	struct octeon_instr_rdp *rdp;
+	struct lio *lio;
+	int ring_doorbell;
+
+	lio = finfo->lio;
+
+	sc = octeon_alloc_soft_command_resp(oct, &ndata->cmd,
+					    sizeof(struct oct_timestamp_resp));
+	finfo->sc = sc;
+
+	if (!sc) {
+		dev_err(&oct->pci_dev->dev, "No memory for timestamped data packet\n");
+		return IQ_SEND_FAILED;
+	}
+
+	if (ndata->reqtype == REQTYPE_NORESP_NET)
+		ndata->reqtype = REQTYPE_RESP_NET;
+	else if (ndata->reqtype == REQTYPE_NORESP_NET_SG)
+		ndata->reqtype = REQTYPE_RESP_NET_SG;
+
+	sc->callback = handle_timestamp;
+	sc->callback_arg = finfo->skb;
+	sc->iq_no = ndata->q_no;
+
+	ih = (struct octeon_instr_ih *)&sc->cmd.ih;
+	rdp = (struct octeon_instr_rdp *)&sc->cmd.rdp;
+
+	ring_doorbell = !xmit_more;
+	retval = octeon_send_command(oct, sc->iq_no, ring_doorbell, &sc->cmd,
+				     sc, ih->dlengsz, ndata->reqtype);
+
+	if (retval) {
+		dev_err(&oct->pci_dev->dev, "timestamp data packet failed status: %x\n",
+			retval);
+		octeon_free_soft_command(oct, sc);
+	} else {
+		netif_info(lio, tx_queued, lio->netdev, "Queued timestamp packet\n");
+	}
+
+	return retval;
+}
+
+static inline int is_ipv4(struct sk_buff *skb)
+{
+	return (skb->protocol == htons(ETH_P_IP)) &&
+	       (ip_hdr(skb)->version == 4);
+}
+
+static inline int is_vlan(struct sk_buff *skb)
+{
+	return skb->protocol == htons(ETH_P_8021Q);
+}
+
+static inline int is_ip_fragmented(struct sk_buff *skb)
+{
+	/* The Don't fragment and Reserved flag fields are ignored.
+	 * IP is fragmented if
+	 * -  the More fragments bit is set (indicating this IP is a fragment
+	 * with more to follow; the current offset could be 0 ).
+	 * -  ths offset field is non-zero.
+	 */
+	return (ip_hdr(skb)->frag_off & htons(IP_MF | IP_OFFSET)) ? 1 : 0;
+}
+
+static inline int is_ipv6(struct sk_buff *skb)
+{
+	return (skb->protocol == htons(ETH_P_IPV6)) &&
+	       (ipv6_hdr(skb)->version == 6);
+}
+
+static inline int is_with_extn_hdr(struct sk_buff *skb)
+{
+	return (ipv6_hdr(skb)->nexthdr != IPPROTO_TCP) &&
+	       (ipv6_hdr(skb)->nexthdr != IPPROTO_UDP);
+}
+
+static inline int is_tcpudp(struct sk_buff *skb)
+{
+	return (ip_hdr(skb)->protocol == IPPROTO_TCP) ||
+	       (ip_hdr(skb)->protocol == IPPROTO_UDP);
+}
+
+static inline u32 get_ipv4_5tuple_tag(struct sk_buff *skb)
+{
+	u32 tag;
+	struct iphdr *iphdr = ip_hdr(skb);
+
+	tag = crc32(0, &iphdr->protocol, 1);
+	tag = crc32(tag, (u8 *)&iphdr->saddr, 8);
+	tag = crc32(tag, skb_transport_header(skb), 4);
+	return tag;
+}
+
+static inline u32 get_ipv6_5tuple_tag(struct sk_buff *skb)
+{
+	u32 tag;
+	struct ipv6hdr *ipv6hdr = ipv6_hdr(skb);
+
+	tag = crc32(0, &ipv6hdr->nexthdr, 1);
+	tag = crc32(tag, (u8 *)&ipv6hdr->saddr, 32);
+	tag = crc32(tag, skb_transport_header(skb), 4);
+	return tag;
+}
+
+/** \brief Transmit networks packets to the Octeon interface
+ * @param skbuff   skbuff struct to be passed to network layer.
+ * @param netdev    pointer to network device
+ * @returns whether the packet was transmitted to the device okay or not
+ *             (NETDEV_TX_OK or NETDEV_TX_BUSY)
+ */
+static int liquidio_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct lio *lio;
+	struct octnet_buf_free_info *finfo;
+	union octnic_cmd_setup cmdsetup;
+	struct octnic_data_pkt ndata;
+	struct octeon_device *oct;
+	struct oct_iq_stats *stats;
+	int cpu = 0, status = 0;
+	int q_idx = 0, iq_no = 0;
+	int xmit_more;
+	u32 tag = 0;
+
+	lio = GET_LIO(netdev);
+	oct = lio->oct_dev;
+
+	if (netif_is_multiqueue(netdev)) {
+		cpu = skb->queue_mapping;
+		q_idx = (cpu & (lio->linfo.num_txpciq - 1));
+		iq_no = lio->linfo.txpciq[q_idx];
+	} else {
+		iq_no = lio->txq;
+	}
+
+	stats = &oct->instr_queue[iq_no]->stats;
+
+	/* Check for all conditions in which the current packet cannot be
+	 * transmitted.
+	 */
+	if (!(atomic_read(&lio->ifstate) & LIO_IFSTATE_RUNNING) ||
+	    (!lio->linfo.link.s.status) ||
+	    (skb->len <= 0)) {
+		netif_info(lio, tx_err, lio->netdev,
+			   "Transmit failed link_status : %d\n",
+			   lio->linfo.link.s.status);
+		goto lio_xmit_failed;
+	}
+
+	/* Use space in skb->cb to store info used to unmap and
+	 * free the buffers.
+	 */
+	finfo = (struct octnet_buf_free_info *)skb->cb;
+	finfo->lio = lio;
+	finfo->skb = skb;
+	finfo->sc = NULL;
+
+	/* Prepare the attributes for the data to be passed to OSI. */
+	memset(&ndata, 0, sizeof(struct octnic_data_pkt));
+
+	ndata.buf = (void *)finfo;
+
+	ndata.q_no = iq_no;
+
+	if (netif_is_multiqueue(netdev)) {
+		if (octnet_iq_is_full(oct, ndata.q_no)) {
+			/* defer sending if queue is full */
+			netif_info(lio, tx_err, lio->netdev, "Transmit failed iq:%d full\n",
+				   ndata.q_no);
+			stats->tx_iq_busy++;
+			return NETDEV_TX_BUSY;
+		}
+	} else {
+		if (octnet_iq_is_full(oct, lio->txq)) {
+			/* defer sending if queue is full */
+			stats->tx_iq_busy++;
+			netif_info(lio, tx_err, lio->netdev, "Transmit failed iq:%d full\n",
+				   ndata.q_no);
+			return NETDEV_TX_BUSY;
+		}
+	}
+	/* pr_info(" XMIT - valid Qs: %d, 1st Q no: %d, cpu:  %d, q_no:%d\n",
+	 *	lio->linfo.num_txpciq, lio->txq, cpu, ndata.q_no );
+	 */
+
+	ndata.datasize = skb->len;
+
+	cmdsetup.u64 = 0;
+	cmdsetup.s.ifidx = lio->linfo.ifidx;
+
+	if (skb->ip_summed == CHECKSUM_PARTIAL) {
+		if (is_ipv4(skb) && !is_ip_fragmented(skb) && is_tcpudp(skb)) {
+			tag = get_ipv4_5tuple_tag(skb);
+
+			cmdsetup.s.cksum_offset = sizeof(struct ethhdr) + 1;
+
+			if (ip_hdr(skb)->ihl > 5)
+				cmdsetup.s.ipv4opts_ipv6exthdr =
+						OCT_PKT_PARAM_IPV4OPTS;
+
+		} else if (is_ipv6(skb)) {
+			tag = get_ipv6_5tuple_tag(skb);
+
+			cmdsetup.s.cksum_offset = sizeof(struct ethhdr) + 1;
+
+			if (is_with_extn_hdr(skb))
+				cmdsetup.s.ipv4opts_ipv6exthdr =
+						OCT_PKT_PARAM_IPV6EXTHDR;
+
+		} else if (is_vlan(skb)) {
+			if (vlan_eth_hdr(skb)->h_vlan_encapsulated_proto
+				== htons(ETH_P_IP) &&
+				!is_ip_fragmented(skb) && is_tcpudp(skb)) {
+				tag = get_ipv4_5tuple_tag(skb);
+
+				cmdsetup.s.cksum_offset =
+					sizeof(struct vlan_ethhdr) + 1;
+
+				if (ip_hdr(skb)->ihl > 5)
+					cmdsetup.s.ipv4opts_ipv6exthdr =
+						OCT_PKT_PARAM_IPV4OPTS;
+
+			} else if (vlan_eth_hdr(skb)->h_vlan_encapsulated_proto
+				== htons(ETH_P_IPV6)) {
+				tag = get_ipv6_5tuple_tag(skb);
+
+				cmdsetup.s.cksum_offset =
+					sizeof(struct vlan_ethhdr) + 1;
+
+				if (is_with_extn_hdr(skb))
+					cmdsetup.s.ipv4opts_ipv6exthdr =
+						OCT_PKT_PARAM_IPV6EXTHDR;
+			}
+		}
+	}
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
+		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+		cmdsetup.s.timestamp = 1;
+	}
+
+	if (skb_shinfo(skb)->nr_frags == 0) {
+		cmdsetup.s.u.datasize = skb->len;
+		octnet_prepare_pci_cmd(&ndata.cmd, &cmdsetup, tag);
+		/* Offload checksum calculation for TCP/UDP packets */
+		ndata.cmd.dptr = dma_map_single(&oct->pci_dev->dev,
+						skb->data,
+						skb->len,
+						DMA_TO_DEVICE);
+		if (dma_mapping_error(&oct->pci_dev->dev, ndata.cmd.dptr)) {
+			dev_err(&oct->pci_dev->dev, "%s DMA mapping error 1\n",
+				__func__);
+			return NETDEV_TX_BUSY;
+		}
+
+		finfo->dptr = ndata.cmd.dptr;
+
+		ndata.reqtype = REQTYPE_NORESP_NET;
+
+	} else {
+		int i, frags;
+		struct skb_frag_struct *frag;
+		struct octnic_gather *g;
+
+		spin_lock(&lio->lock);
+		g = (struct octnic_gather *)list_delete_head(&lio->glist);
+		spin_unlock(&lio->lock);
+
+		if (!g) {
+			netif_info(lio, tx_err, lio->netdev,
+				   "Transmit scatter gather: glist null!\n");
+			goto lio_xmit_failed;
+		}
+
+		cmdsetup.s.gather = 1;
+		cmdsetup.s.u.gatherptrs = (skb_shinfo(skb)->nr_frags + 1);
+		octnet_prepare_pci_cmd(&ndata.cmd, &cmdsetup, tag);
+
+		memset(g->sg, 0, g->sg_size);
+
+		g->sg[0].ptr[0] = dma_map_single(&oct->pci_dev->dev,
+						 skb->data,
+						 (skb->len - skb->data_len),
+						 DMA_TO_DEVICE);
+		if (dma_mapping_error(&oct->pci_dev->dev, g->sg[0].ptr[0])) {
+			dev_err(&oct->pci_dev->dev, "%s DMA mapping error 2\n",
+				__func__);
+			return NETDEV_TX_BUSY;
+		}
+		add_sg_size(&g->sg[0], (skb->len - skb->data_len), 0);
+
+		frags = skb_shinfo(skb)->nr_frags;
+		i = 1;
+		while (frags--) {
+			frag = &skb_shinfo(skb)->frags[i - 1];
+
+			g->sg[(i >> 2)].ptr[(i & 3)] =
+				dma_map_page(&oct->pci_dev->dev,
+					     frag->page.p,
+					     frag->page_offset,
+					     frag->size,
+					     DMA_TO_DEVICE);
+
+			add_sg_size(&g->sg[(i >> 2)], frag->size, (i & 3));
+			i++;
+		}
+
+		ndata.cmd.dptr = dma_map_single(&oct->pci_dev->dev,
+						g->sg, g->sg_size,
+						DMA_TO_DEVICE);
+		if (dma_mapping_error(&oct->pci_dev->dev, ndata.cmd.dptr)) {
+			dev_err(&oct->pci_dev->dev, "%s DMA mapping error 3\n",
+				__func__);
+			dma_unmap_single(&oct->pci_dev->dev, g->sg[0].ptr[0],
+					 skb->len - skb->data_len,
+					 DMA_TO_DEVICE);
+			return NETDEV_TX_BUSY;
+		}
+
+		finfo->dptr = ndata.cmd.dptr;
+		finfo->g = g;
+
+		ndata.reqtype = REQTYPE_NORESP_NET_SG;
+	}
+
+	if (skb_shinfo(skb)->gso_size) {
+		struct octeon_instr_irh *irh =
+			(struct octeon_instr_irh *)&ndata.cmd.irh;
+		union tx_info *tx_info = (union tx_info *)&ndata.cmd.ossp[0];
+
+		irh->len = 1;   /* to indicate that ossp[0] contains tx_info */
+		tx_info->s.gso_size = skb_shinfo(skb)->gso_size;
+		tx_info->s.gso_segs = skb_shinfo(skb)->gso_segs;
+	}
+
+	xmit_more = skb->xmit_more;
+
+	if (unlikely(cmdsetup.s.timestamp))
+		status = send_nic_timestamp_pkt(oct, &ndata, finfo, xmit_more);
+	else
+		status = octnet_send_nic_data_pkt(oct, &ndata, xmit_more);
+	if (status == IQ_SEND_FAILED)
+		goto lio_xmit_failed;
+
+	netif_info(lio, tx_queued, lio->netdev, "Transmit queued successfully\n");
+
+	if (status == IQ_SEND_STOP)
+		stop_q(lio->netdev, q_idx);
+
+	netdev->trans_start = jiffies;
+
+	stats->tx_done++;
+	stats->tx_tot_bytes += skb->len;
+
+	return NETDEV_TX_OK;
+
+lio_xmit_failed:
+	stats->tx_dropped++;
+	netif_info(lio, tx_err, lio->netdev, "IQ%d Transmit dropped:%llu\n",
+		   iq_no, stats->tx_dropped);
+	dma_unmap_single(&oct->pci_dev->dev, ndata.cmd.dptr,
+			 ndata.datasize, DMA_TO_DEVICE);
+	recv_buffer_free(skb);
+	return NETDEV_TX_OK;
+}
+
+/** \brief Network device Tx timeout
+ * @param netdev    pointer to network device
+ */
+static void liquidio_tx_timeout(struct net_device *netdev)
+{
+	struct lio *lio;
+
+	lio = GET_LIO(netdev);
+
+	netif_info(lio, tx_err, lio->netdev,
+		   "Transmit timeout tx_dropped:%ld, waking up queues now!!\n",
+		   netdev->stats.tx_dropped);
+	netdev->trans_start = jiffies;
+	txqs_wake(netdev);
+}
+
+int liquidio_set_feature(struct net_device *netdev, int cmd)
+{
+	struct lio *lio = GET_LIO(netdev);
+	struct octeon_device *oct = lio->oct_dev;
+	struct octnic_ctrl_pkt nctrl;
+	struct octnic_ctrl_params nparams;
+	int ret = 0;
+
+	memset(&nctrl, 0, sizeof(struct octnic_ctrl_pkt));
+
+	nctrl.ncmd.u64 = 0;
+	nctrl.ncmd.s.cmd = cmd;
+	nctrl.ncmd.s.param1 = lio->linfo.ifidx;
+	nctrl.ncmd.s.param2 = OCTNIC_LROIPV4 | OCTNIC_LROIPV6;
+	nctrl.wait_time = 100;
+	nctrl.netpndev = (u64)netdev;
+	nctrl.cb_fn = liquidio_link_ctrl_cmd_completion;
+
+	nparams.resp_order = OCTEON_RESP_NORESPONSE;
+
+	ret = octnet_send_nic_ctrl_pkt(lio->oct_dev, &nctrl, nparams);
+	if (ret < 0) {
+		dev_err(&oct->pci_dev->dev, "Feature change failed in core (ret: 0x%x)\n",
+			ret);
+	}
+	return ret;
+}
+
+/** \brief Net device fix features
+ * @param netdev  pointer to network device
+ * @param request features requested
+ * @returns updated features list
+ */
+static netdev_features_t liquidio_fix_features(struct net_device *netdev,
+					       netdev_features_t request)
+{
+	struct lio *lio = netdev_priv(netdev);
+
+	if ((request & NETIF_F_RXCSUM) &&
+	    !(lio->dev_capability & NETIF_F_RXCSUM))
+		request &= ~NETIF_F_RXCSUM;
+
+	if ((request & NETIF_F_HW_CSUM) &&
+	    !(lio->dev_capability & NETIF_F_HW_CSUM))
+		request &= ~NETIF_F_HW_CSUM;
+
+	if ((request & NETIF_F_TSO) && !(lio->dev_capability & NETIF_F_TSO))
+		request &= ~NETIF_F_TSO;
+
+	if ((request & NETIF_F_TSO6) && !(lio->dev_capability & NETIF_F_TSO6))
+		request &= ~NETIF_F_TSO6;
+
+	if ((request & NETIF_F_LRO) && !(lio->dev_capability & NETIF_F_LRO))
+		request &= ~NETIF_F_LRO;
+
+	/*Disable LRO if RXCSUM is off */
+	if (!(request & NETIF_F_RXCSUM) && (netdev->features & NETIF_F_LRO) &&
+	    (lio->dev_capability & NETIF_F_LRO))
+		request &= ~NETIF_F_LRO;
+
+	return request;
+}
+
+/** \brief Net device set features
+ * @param netdev  pointer to network device
+ * @param features features to enable/disable
+ */
+static int liquidio_set_features(struct net_device *netdev,
+				 netdev_features_t features)
+{
+	struct lio *lio = netdev_priv(netdev);
+
+	if (!((netdev->features ^ features) & NETIF_F_LRO))
+		return 0;
+
+	if ((features & NETIF_F_LRO) && (lio->dev_capability & NETIF_F_LRO))
+		liquidio_set_feature(netdev, OCTNET_CMD_LRO_ENABLE);
+	else if (!(features & NETIF_F_LRO) &&
+		 (lio->dev_capability & NETIF_F_LRO))
+		liquidio_set_feature(netdev, OCTNET_CMD_LRO_DISABLE);
+
+	return 0;
+}
+
+static struct net_device_ops lionetdevops = {
+	.ndo_open		= liquidio_open,
+	.ndo_stop		= liquidio_stop,
+	.ndo_start_xmit		= liquidio_xmit,
+	.ndo_get_stats		= liquidio_get_stats,
+	.ndo_set_mac_address	= liquidio_set_mac,
+	.ndo_set_rx_mode	= liquidio_set_mcast_list,
+	.ndo_tx_timeout		= liquidio_tx_timeout,
+	.ndo_change_mtu		= liquidio_change_mtu,
+	.ndo_do_ioctl		= liquidio_ioctl,
+	.ndo_fix_features	= liquidio_fix_features,
+	.ndo_set_features	= liquidio_set_features,
+};
+
+/** \brief Entry point for the liquidio module
+ */
+static int __init liquidio_init(void)
+{
+	int i;
+	struct handshake *hs;
+
+	init_completion(&first_stage);
+
+	octeon_init_device_list(conf_type);
+
+	if (liquidio_init_pci())
+		return -EINVAL;
+
+	wait_for_completion_timeout(&first_stage, msecs_to_jiffies(1000));
+
+	for (i = 0; i < MAX_OCTEON_DEVICES; i++) {
+		hs = &handshake[i];
+		if (hs->pci_dev) {
+			wait_for_completion(&hs->init);
+			if (!hs->init_ok) {
+				/* init handshake failed */
+				dev_err(&hs->pci_dev->dev,
+					"Failed to init device\n");
+				liquidio_deinit_pci();
+				return -EIO;
+			}
+		}
+	}
+
+	for (i = 0; i < MAX_OCTEON_DEVICES; i++) {
+		hs = &handshake[i];
+		if (hs->pci_dev) {
+			wait_for_completion_timeout(&hs->started,
+						    msecs_to_jiffies(30000));
+			if (!hs->started_ok) {
+				/* starter handshake failed */
+				dev_err(&hs->pci_dev->dev,
+					"Firmware failed to start\n");
+				liquidio_deinit_pci();
+				return -EIO;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int lio_nic_info(struct octeon_recv_info *recv_info, void *buf)
+{
+	struct octeon_device *oct = (struct octeon_device *)buf;
+	struct octeon_recv_pkt *recv_pkt = recv_info->recv_pkt;
+	int ifidx = 0;
+	union oct_link_status *ls;
+	int i;
+
+	if ((recv_pkt->buffer_size[0] != sizeof(*ls)) ||
+	    (recv_pkt->rh.r_nic_info.ifidx > oct->ifcount)) {
+		dev_err(&oct->pci_dev->dev, "Malformed NIC_INFO, len=%d, ifidx=%d\n",
+			recv_pkt->buffer_size[0],
+			recv_pkt->rh.r_nic_info.ifidx);
+		goto nic_info_err;
+	}
+
+	ifidx = recv_pkt->rh.r_nic_info.ifidx;
+	ls = (union oct_link_status *)get_rbd(recv_pkt->buffer_ptr[0]);
+
+	octeon_swap_8B_data((u64 *)ls, (sizeof(union oct_link_status)) >> 3);
+
+	update_link_status(oct->props[ifidx].netdev, ls);
+
+nic_info_err:
+	for (i = 0; i < recv_pkt->buffer_count; i++)
+		recv_buffer_free(recv_pkt->buffer_ptr[i]);
+	octeon_free_recv_info(recv_info);
+	return 0;
+}
+
+/**
+ * \brief Setup network interfaces
+ * @param octeon_dev  octeon device
+ *
+ * Called during init time for each device. It assumes the NIC
+ * is already up and running.  The link information for each
+ * interface is passed in link_info.
+ */
+static int setup_nic_devices(struct octeon_device *octeon_dev)
+{
+	struct lio *lio = NULL;
+	struct net_device *netdev;
+	u8 mac[6], i, j;
+	struct octeon_soft_command *sc;
+	struct liquidio_if_cfg_context *ctx;
+	struct liquidio_if_cfg_resp *resp;
+	struct octdev_props *props;
+	int retval, num_iqueues, num_oqueues, q_no;
+	u64 q_mask;
+	int num_cpus = num_online_cpus();
+	union oct_nic_if_cfg if_cfg;
+	unsigned int base_queue;
+	unsigned int gmx_port_id;
+	u32 resp_size, ctx_size;
+
+	/* This is to handle link status changes */
+	octeon_register_dispatch_fn(octeon_dev, OPCODE_NIC,
+				    OPCODE_NIC_INFO,
+				    lio_nic_info, octeon_dev);
+
+	/* REQTYPE_RESP_NET and REQTYPE_SOFT_COMMAND do not have free functions.
+	 * They are handled directly.
+	 */
+	octeon_register_reqtype_free_fn(octeon_dev, REQTYPE_NORESP_NET,
+					free_netbuf);
+
+	octeon_register_reqtype_free_fn(octeon_dev, REQTYPE_NORESP_NET_SG,
+					free_netsgbuf);
+
+	octeon_register_reqtype_free_fn(octeon_dev, REQTYPE_RESP_NET_SG,
+					free_netsgbuf_with_resp);
+
+	for (i = 0; i < octeon_dev->ifcount; i++) {
+		resp_size = sizeof(struct liquidio_if_cfg_resp);
+		ctx_size = sizeof(struct liquidio_if_cfg_context);
+		sc = (struct octeon_soft_command *)
+			octeon_alloc_soft_command(octeon_dev, 0,
+						  resp_size, ctx_size);
+		resp = (struct liquidio_if_cfg_resp *)sc->virtrptr;
+		ctx  = (struct liquidio_if_cfg_context *)sc->ctxptr;
+
+		num_iqueues =
+			CFG_GET_NUM_TXQS_NIC_IF(octeon_get_conf(octeon_dev), i);
+		num_oqueues =
+			CFG_GET_NUM_RXQS_NIC_IF(octeon_get_conf(octeon_dev), i);
+		base_queue =
+			CFG_GET_BASE_QUE_NIC_IF(octeon_get_conf(octeon_dev), i);
+		gmx_port_id =
+			CFG_GET_GMXID_NIC_IF(octeon_get_conf(octeon_dev), i);
+		if (num_iqueues > num_cpus)
+			num_iqueues = num_cpus;
+		if (num_oqueues > num_cpus)
+			num_oqueues = num_cpus;
+		dev_dbg(&octeon_dev->pci_dev->dev,
+			"requesting config for interface %d, iqs %d, oqs %d\n",
+			i, num_iqueues, num_oqueues);
+		ACCESS_ONCE(ctx->cond) = 0;
+		ctx->octeon_id = lio_get_device_id(octeon_dev);
+		init_waitqueue_head(&ctx->wc);
+
+		if_cfg.u64 = 0;
+		if_cfg.s.num_iqueues = num_iqueues;
+		if_cfg.s.num_oqueues = num_oqueues;
+		if_cfg.s.base_queue = base_queue;
+		if_cfg.s.gmx_port_id = gmx_port_id;
+		octeon_prepare_soft_command(octeon_dev, sc, OPCODE_NIC,
+					    OPCODE_NIC_IF_CFG, i,
+					    if_cfg.u64, 0);
+
+		sc->callback = if_cfg_callback;
+		sc->callback_arg = sc;
+		sc->wait_time = 1000;
+
+		retval = octeon_send_soft_command(octeon_dev, sc);
+		if (retval) {
+			dev_err(&octeon_dev->pci_dev->dev,
+				"iq/oq config failed status: %x\n",
+				retval);
+			/* Soft instr is freed by driver in case of failure. */
+			goto setup_nic_dev_fail;
+		}
+
+		/* Sleep on a wait queue till the cond flag indicates that the
+		 * response arrived or timed-out.
+		 */
+		sleep_cond(&ctx->wc, &ctx->cond);
+		retval = resp->status;
+		if (retval) {
+			dev_err(&octeon_dev->pci_dev->dev, "iq/oq config failed\n");
+			goto setup_nic_dev_fail;
+		}
+
+		octeon_swap_8B_data((u64 *)(&resp->cfg_info),
+				    (sizeof(struct liquidio_if_cfg_info)) >> 3);
+
+		num_iqueues = hweight64(resp->cfg_info.iqmask);
+		num_oqueues = hweight64(resp->cfg_info.oqmask);
+
+		if (!(num_iqueues) || !(num_oqueues)) {
+			dev_err(&octeon_dev->pci_dev->dev,
+				"Got bad iqueues (%016llx) or oqueues (%016llx) from firmware.\n",
+				resp->cfg_info.iqmask,
+				resp->cfg_info.oqmask);
+			goto setup_nic_dev_fail;
+		}
+		dev_dbg(&octeon_dev->pci_dev->dev,
+			"interface %d, iqmask %016llx, oqmask %016llx, numiqueues %d, numoqueues %d\n",
+			i, resp->cfg_info.iqmask, resp->cfg_info.oqmask,
+			num_iqueues, num_oqueues);
+		netdev = alloc_etherdev_mq(LIO_SIZE, num_iqueues);
+
+		if (!netdev) {
+			dev_err(&octeon_dev->pci_dev->dev, "Device allocation failed\n");
+			goto setup_nic_dev_fail;
+		}
+
+		props = &octeon_dev->props[i];
+		props->netdev = netdev;
+
+		if (num_iqueues > 1)
+			lionetdevops.ndo_select_queue = select_q;
+
+		/* Associate the routines that will handle different
+		 * netdev tasks.
+		 */
+		netdev->netdev_ops = &lionetdevops;
+
+		lio = GET_LIO(netdev);
+
+		memset(lio, 0, sizeof(struct lio));
+
+		lio->linfo.ifidx = resp->cfg_info.ifidx;
+		lio->ifidx = resp->cfg_info.ifidx;
+
+		lio->linfo.num_rxpciq = num_oqueues;
+		lio->linfo.num_txpciq = num_iqueues;
+		q_mask = resp->cfg_info.oqmask;
+		/* q_mask is 0-based and already verified mask is nonzero */
+		for (j = 0; j < num_oqueues; j++) {
+			q_no = __ffs64(q_mask);
+			q_mask &= (~(1UL << q_no));
+			lio->linfo.rxpciq[j] = q_no;
+		}
+		q_mask = resp->cfg_info.iqmask;
+		for (j = 0; j < num_iqueues; j++) {
+			q_no = __ffs64(q_mask);
+			q_mask &= (~(1UL << q_no));
+			lio->linfo.txpciq[j] = q_no;
+		}
+		lio->linfo.hw_addr = resp->cfg_info.linfo.hw_addr;
+		lio->linfo.gmxport = resp->cfg_info.linfo.gmxport;
+		lio->linfo.link.u64 = resp->cfg_info.linfo.link.u64;
+
+		lio->msg_enable = netif_msg_init(debug, DEFAULT_MSG_ENABLE);
+
+		lio->dev_capability = NETIF_F_HIGHDMA
+				      | NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM
+				      | NETIF_F_SG | NETIF_F_RXCSUM
+				      | NETIF_F_TSO | NETIF_F_TSO6
+				      | NETIF_F_LRO;
+		netif_set_gso_max_size(netdev, OCTNIC_GSO_MAX_SIZE);
+
+		netdev->features = lio->dev_capability;
+		netdev->vlan_features = lio->dev_capability;
+
+		netdev->hw_features = lio->dev_capability;
+
+		/* Point to the  properties for octeon device to which this
+		 * interface belongs.
+		 */
+		lio->oct_dev = octeon_dev;
+		lio->octprops = props;
+		lio->netdev = netdev;
+		spin_lock_init(&lio->lock);
+
+		dev_dbg(&octeon_dev->pci_dev->dev,
+			"if%d gmx: %d hw_addr: 0x%llx\n", i,
+			lio->linfo.gmxport, CVM_CAST64(lio->linfo.hw_addr));
+
+		/* 64-bit swap required on LE machines */
+		octeon_swap_8B_data(&lio->linfo.hw_addr, 1);
+		for (j = 0; j < 6; j++)
+			mac[j] = *((u8 *)(((u8 *)&lio->linfo.hw_addr) + 2 + j));
+
+		/* Copy MAC Address to OS network device structure */
+
+		ether_addr_copy(netdev->dev_addr, mac);
+
+		if (setup_io_queues(octeon_dev, netdev)) {
+			dev_err(&octeon_dev->pci_dev->dev, "I/O queues creation failed\n");
+			goto setup_nic_dev_fail;
+		}
+
+		ifstate_set(lio, LIO_IFSTATE_DROQ_OPS);
+
+		/* By default all interfaces on a single Octeon uses the same
+		 * tx and rx queues
+		 */
+		lio->txq = lio->linfo.txpciq[0];
+		lio->rxq = lio->linfo.rxpciq[0];
+
+		lio->tx_qsize = octeon_get_tx_qsize(octeon_dev, lio->txq);
+		lio->rx_qsize = octeon_get_rx_qsize(octeon_dev, lio->rxq);
+
+		if (setup_glist(lio)) {
+			dev_err(&octeon_dev->pci_dev->dev,
+				"Gather list allocation failed\n");
+			goto setup_nic_dev_fail;
+		}
+
+		/* Register ethtool support */
+		liquidio_set_ethtool_ops(netdev);
+
+		liquidio_set_feature(netdev, OCTNET_CMD_LRO_ENABLE);
+
+		if ((debug != -1) && (debug & NETIF_MSG_HW))
+			liquidio_set_feature(netdev, OCTNET_CMD_VERBOSE_ENABLE);
+
+		/* Register the network device with the OS */
+		if (register_netdev(netdev)) {
+			dev_err(&octeon_dev->pci_dev->dev, "Device registration failed\n");
+			goto setup_nic_dev_fail;
+		}
+
+		dev_dbg(&octeon_dev->pci_dev->dev,
+			"Setup NIC ifidx:%d mac:%02x%02x%02x%02x%02x%02x\n",
+			i, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+		netif_carrier_off(netdev);
+
+		if (lio->linfo.link.s.status) {
+			netif_carrier_on(netdev);
+			start_txq(netdev);
+		} else {
+			netif_carrier_off(netdev);
+		}
+
+		ifstate_set(lio, LIO_IFSTATE_REGISTERED);
+
+		dev_dbg(&octeon_dev->pci_dev->dev,
+			"NIC ifidx:%d Setup successful\n", i);
+
+		octeon_free_soft_command(octeon_dev, sc);
+	}
+
+	return 0;
+
+setup_nic_dev_fail:
+
+	octeon_free_soft_command(octeon_dev, sc);
+
+	while (i--) {
+		dev_err(&octeon_dev->pci_dev->dev,
+			"NIC ifidx:%d Setup failed\n", i);
+		liquidio_destroy_nic_device(octeon_dev, i);
+	}
+	return -ENODEV;
+}
+
+/**
+ * \brief initialize the NIC
+ * @param oct octeon device
+ *
+ * This initialization routine is called once the Octeon device application is
+ * up and running
+ */
+static int liquidio_init_nic_module(struct octeon_device *oct)
+{
+	struct oct_intrmod_cfg *intrmod_cfg;
+	int retval = 0;
+	int num_nic_ports = CFG_GET_NUM_NIC_PORTS(octeon_get_conf(oct));
+
+	dev_dbg(&oct->pci_dev->dev, "Initializing network interfaces\n");
+
+	/* only default iq and oq were initialized
+	 * initialize the rest as well
+	 */
+	/* run port_config command for each port */
+	oct->ifcount = num_nic_ports;
+
+	memset(oct->props, 0,
+	       sizeof(struct octdev_props) * num_nic_ports);
+
+	retval = setup_nic_devices(oct);
+	if (retval) {
+		dev_err(&oct->pci_dev->dev, "Setup NIC devices failed\n");
+		goto octnet_init_failure;
+	}
+
+	liquidio_ptp_init(oct);
+
+	/* Initialize interrupt moderation params */
+	intrmod_cfg = &((struct octeon_device *)oct)->intrmod;
+	intrmod_cfg->intrmod_enable = 1;
+	intrmod_cfg->intrmod_check_intrvl = LIO_INTRMOD_CHECK_INTERVAL;
+	intrmod_cfg->intrmod_maxpkt_ratethr = LIO_INTRMOD_MAXPKT_RATETHR;
+	intrmod_cfg->intrmod_minpkt_ratethr = LIO_INTRMOD_MINPKT_RATETHR;
+	intrmod_cfg->intrmod_maxcnt_trigger = LIO_INTRMOD_MAXCNT_TRIGGER;
+	intrmod_cfg->intrmod_maxtmr_trigger = LIO_INTRMOD_MAXTMR_TRIGGER;
+	intrmod_cfg->intrmod_mintmr_trigger = LIO_INTRMOD_MINTMR_TRIGGER;
+	intrmod_cfg->intrmod_mincnt_trigger = LIO_INTRMOD_MINCNT_TRIGGER;
+
+	dev_dbg(&oct->pci_dev->dev, "Network interfaces ready\n");
+
+	return retval;
+
+octnet_init_failure:
+
+	oct->ifcount = 0;
+
+	return retval;
+}
+
+/**
+ * \brief starter callback that invokes the remaining initialization work after
+ * the NIC is up and running.
+ * @param octptr  work struct work_struct
+ */
+static void nic_starter(struct work_struct *work)
+{
+	struct octeon_device *oct;
+	struct cavium_wk *wk = (struct cavium_wk *)work;
+
+	oct = (struct octeon_device *)wk->ctxptr;
+
+	if (atomic_read(&oct->status) == OCT_DEV_RUNNING)
+		return;
+
+	/* If the status of the device is CORE_OK, the core
+	 * application has reported its application type. Call
+	 * any registered handlers now and move to the RUNNING
+	 * state.
+	 */
+	if (atomic_read(&oct->status) != OCT_DEV_CORE_OK) {
+		schedule_delayed_work(&oct->nic_poll_work.work,
+				      LIQUIDIO_STARTER_POLL_INTERVAL_MS);
+		return;
+	}
+
+	atomic_set(&oct->status, OCT_DEV_RUNNING);
+
+	if (oct->app_mode && oct->app_mode == CVM_DRV_NIC_APP) {
+		dev_dbg(&oct->pci_dev->dev, "Starting NIC module\n");
+
+		if (liquidio_init_nic_module(oct))
+			dev_err(&oct->pci_dev->dev, "NIC initialization failed\n");
+		else
+			handshake[oct->octeon_id].started_ok = 1;
+	} else {
+		dev_err(&oct->pci_dev->dev,
+			"Unexpected application running on NIC (%d). Check firmware.\n",
+			oct->app_mode);
+	}
+
+	complete(&handshake[oct->octeon_id].started);
+}
+
+/**
+ * \brief Device initialization for each Octeon device that is probed
+ * @param octeon_dev  octeon device
+ */
+static int octeon_device_init(struct octeon_device *octeon_dev)
+{
+	int j, ret;
+	struct octeon_device_priv *oct_priv =
+		(struct octeon_device_priv *)octeon_dev->priv;
+	atomic_set(&octeon_dev->status, OCT_DEV_BEGIN_STATE);
+
+	/* Enable access to the octeon device and make its DMA capability
+	 * known to the OS.
+	 */
+	if (octeon_pci_os_setup(octeon_dev))
+		return 1;
+
+	/* Identify the Octeon type and map the BAR address space. */
+	if (octeon_chip_specific_setup(octeon_dev)) {
+		dev_err(&octeon_dev->pci_dev->dev, "Chip specific setup failed\n");
+		return 1;
+	}
+
+	atomic_set(&octeon_dev->status, OCT_DEV_PCI_MAP_DONE);
+
+	octeon_dev->app_mode = CVM_DRV_INVALID_APP;
+
+	/* Do a soft reset of the Octeon device. */
+	if (octeon_dev->fn_list.soft_reset(octeon_dev))
+		return 1;
+
+	/* Initialize the dispatch mechanism used to push packets arriving on
+	 * Octeon Output queues.
+	 */
+	if (octeon_init_dispatch_list(octeon_dev))
+		return 1;
+
+	octeon_register_dispatch_fn(octeon_dev, OPCODE_NIC,
+				    OPCODE_NIC_CORE_DRV_ACTIVE,
+				    octeon_core_drv_init,
+				    octeon_dev);
+
+	INIT_DELAYED_WORK(&octeon_dev->nic_poll_work.work, nic_starter);
+	octeon_dev->nic_poll_work.ctxptr = (void *)octeon_dev;
+	schedule_delayed_work(&octeon_dev->nic_poll_work.work,
+			      LIQUIDIO_STARTER_POLL_INTERVAL_MS);
+
+	atomic_set(&octeon_dev->status, OCT_DEV_DISPATCH_INIT_DONE);
+
+	octeon_set_io_queues_off(octeon_dev);
+
+	/*  Setup the data structures that manage this Octeon's Input queues. */
+	if (octeon_setup_instr_queues(octeon_dev)) {
+		dev_err(&octeon_dev->pci_dev->dev,
+			"instruction queue initialization failed\n");
+		/* On error, release any previously allocated queues */
+		for (j = 0; j < octeon_dev->num_iqs; j++)
+			octeon_delete_instr_queue(octeon_dev, j);
+		return 1;
+	}
+	atomic_set(&octeon_dev->status, OCT_DEV_INSTR_QUEUE_INIT_DONE);
+
+	/* Initialize soft command buffer pool
+	 */
+	if (octeon_setup_sc_buffer_pool(octeon_dev)) {
+		dev_err(&octeon_dev->pci_dev->dev, "sc buffer pool allocation failed\n");
+		return 1;
+	}
+	atomic_set(&octeon_dev->status, OCT_DEV_SC_BUFF_POOL_INIT_DONE);
+
+	/* Initialize lists to manage the requests of different types that
+	 * arrive from user & kernel applications for this octeon device.
+	 */
+	if (octeon_setup_response_list(octeon_dev)) {
+		dev_err(&octeon_dev->pci_dev->dev, "Response list allocation failed\n");
+		return 1;
+	}
+	atomic_set(&octeon_dev->status, OCT_DEV_RESP_LIST_INIT_DONE);
+
+	if (octeon_setup_output_queues(octeon_dev)) {
+		dev_err(&octeon_dev->pci_dev->dev, "Output queue initialization failed\n");
+		/* Release any previously allocated queues */
+		for (j = 0; j < octeon_dev->num_oqs; j++)
+			octeon_delete_droq(octeon_dev, j);
+	}
+
+	atomic_set(&octeon_dev->status, OCT_DEV_DROQ_INIT_DONE);
+
+	/* The input and output queue registers were setup earlier (the queues
+	 * were not enabled). Any additional registers that need to be
+	 * programmed should be done now.
+	 */
+	ret = octeon_dev->fn_list.setup_device_regs(octeon_dev);
+	if (ret) {
+		dev_err(&octeon_dev->pci_dev->dev,
+			"Failed to configure device registers\n");
+		return ret;
+	}
+
+	/* Initialize the tasklet that handles output queue packet processing.*/
+	dev_dbg(&octeon_dev->pci_dev->dev, "Initializing droq tasklet\n");
+	tasklet_init(&oct_priv->droq_tasklet, octeon_droq_bh,
+		     (unsigned long)octeon_dev);
+
+	/* Setup the interrupt handler and record the INT SUM register address
+	 */
+	octeon_setup_interrupt(octeon_dev);
+
+	/* Enable Octeon device interrupts */
+	octeon_dev->fn_list.enable_interrupt(octeon_dev->chip);
+
+	/* Enable the input and output queues for this Octeon device */
+	octeon_dev->fn_list.enable_io_queues(octeon_dev);
+
+	atomic_set(&octeon_dev->status, OCT_DEV_IO_QUEUES_DONE);
+
+	dev_dbg(&octeon_dev->pci_dev->dev, "Waiting for DDR initialization...\n");
+
+	if (ddr_timeout == 0) {
+		dev_info(&octeon_dev->pci_dev->dev,
+			 "WAITING. Set ddr_timeout to non-zero value to proceed with initialization.\n");
+	}
+
+	schedule_timeout_uninterruptible(HZ * LIO_RESET_SECS);
+
+	/* Wait for the octeon to initialize DDR after the soft-reset. */
+	ret = octeon_wait_for_ddr_init(octeon_dev, &ddr_timeout);
+	if (ret) {
+		dev_err(&octeon_dev->pci_dev->dev,
+			"DDR not initialized. Please confirm that board is configured to boot from Flash, ret: %d\n",
+			ret);
+		return 1;
+	}
+
+	if (octeon_wait_for_bootloader(octeon_dev, 1000) != 0) {
+		dev_err(&octeon_dev->pci_dev->dev, "Board not responding\n");
+		return 1;
+	}
+
+	dev_dbg(&octeon_dev->pci_dev->dev, "Initializing consoles\n");
+	ret = octeon_init_consoles(octeon_dev);
+	if (ret) {
+		dev_err(&octeon_dev->pci_dev->dev, "Could not access board consoles\n");
+		return 1;
+	}
+	ret = octeon_add_console(octeon_dev, 0);
+	if (ret) {
+		dev_err(&octeon_dev->pci_dev->dev, "Could not access board console\n");
+		return 1;
+	}
+
+	atomic_set(&octeon_dev->status, OCT_DEV_CONSOLE_INIT_DONE);
+
+	dev_dbg(&octeon_dev->pci_dev->dev, "Loading firmware\n");
+	ret = load_firmware(octeon_dev);
+	if (ret) {
+		dev_err(&octeon_dev->pci_dev->dev, "Could not load firmware to board\n");
+		return 1;
+	}
+
+	handshake[octeon_dev->octeon_id].init_ok = 1;
+	complete(&handshake[octeon_dev->octeon_id].init);
+
+	atomic_set(&octeon_dev->status, OCT_DEV_HOST_OK);
+
+	/* Send Credit for Octeon Output queues. Credits are always sent after
+	 * the output queue is enabled.
+	 */
+	for (j = 0; j < octeon_dev->num_oqs; j++)
+		writel(octeon_dev->droq[j]->max_count,
+		       octeon_dev->droq[j]->pkts_credit_reg);
+
+	/* Packets can start arriving on the output queues from this point. */
+
+	return 0;
+}
+
+/**
+ * \brief Exits the module
+ */
+static void __exit liquidio_exit(void)
+{
+	liquidio_deinit_pci();
+
+	pr_info("LiquidIO network module is now unloaded\n");
+}
+
+module_init(liquidio_init);
+module_exit(liquidio_exit);
diff --git a/drivers/net/ethernet/cavium/liquidio/liquidio_common.h b/drivers/net/ethernet/cavium/liquidio/liquidio_common.h
new file mode 100644
index 000000000000..0ac347ccc8ba
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/liquidio_common.h
@@ -0,0 +1,673 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+
+/*!  \file  liquidio_common.h
+ *   \brief Common: Structures and macros used in PCI-NIC package by core and
+ *   host driver.
+ */
+
+#ifndef __LIQUIDIO_COMMON_H__
+#define __LIQUIDIO_COMMON_H__
+
+#include "octeon_config.h"
+
+#define LIQUIDIO_VERSION        "1.1.9"
+#define LIQUIDIO_MAJOR_VERSION  1
+#define LIQUIDIO_MINOR_VERSION  1
+#define LIQUIDIO_MICRO_VERSION  9
+
+#define CONTROL_IQ 0
+/** Tag types used by Octeon cores in its work. */
+enum octeon_tag_type {
+	ORDERED_TAG = 0,
+	ATOMIC_TAG = 1,
+	NULL_TAG = 2,
+	NULL_NULL_TAG = 3
+};
+
+/* pre-defined host->NIC tag values */
+#define LIO_CONTROL  (0x11111110)
+#define LIO_DATA(i)  (0x11111111 + (i))
+
+/* Opcodes used by host driver/apps to perform operations on the core.
+ * These are used to identify the major subsystem that the operation
+ * is for.
+ */
+#define OPCODE_CORE 0           /* used for generic core operations */
+#define OPCODE_NIC  1           /* used for NIC operations */
+#define OPCODE_LAST OPCODE_NIC
+
+/* Subcodes are used by host driver/apps to identify the sub-operation
+ * for the core. They only need to by unique for a given subsystem.
+ */
+#define OPCODE_SUBCODE(op, sub)       (((op & 0x0f) << 8) | ((sub) & 0x7f))
+
+/** OPCODE_CORE subcodes. For future use. */
+
+/** OPCODE_NIC subcodes */
+
+/* This subcode is sent by core PCI driver to indicate cores are ready. */
+#define OPCODE_NIC_CORE_DRV_ACTIVE     0x01
+#define OPCODE_NIC_NW_DATA             0x02     /* network packet data */
+#define OPCODE_NIC_CMD                 0x03
+#define OPCODE_NIC_INFO                0x04
+#define OPCODE_NIC_PORT_STATS          0x05
+#define OPCODE_NIC_MDIO45              0x06
+#define OPCODE_NIC_TIMESTAMP           0x07
+#define OPCODE_NIC_INTRMOD_CFG         0x08
+#define OPCODE_NIC_IF_CFG              0x09
+
+#define CORE_DRV_TEST_SCATTER_OP    0xFFF5
+
+#define OPCODE_SLOW_PATH(rh)  \
+	(OPCODE_SUBCODE(rh->r.opcode, rh->r.subcode) != \
+		OPCODE_SUBCODE(OPCODE_NIC, OPCODE_NIC_NW_DATA))
+
+/* Application codes advertised by the core driver initialization packet. */
+#define CVM_DRV_APP_START           0x0
+#define CVM_DRV_NO_APP              0
+#define CVM_DRV_APP_COUNT           0x2
+#define CVM_DRV_BASE_APP            (CVM_DRV_APP_START + 0x0)
+#define CVM_DRV_NIC_APP             (CVM_DRV_APP_START + 0x1)
+#define CVM_DRV_INVALID_APP         (CVM_DRV_APP_START + 0x2)
+#define CVM_DRV_APP_END             (CVM_DRV_INVALID_APP - 1)
+
+/* Macro to increment index.
+ * Index is incremented by count; if the sum exceeds
+ * max, index is wrapped-around to the start.
+ */
+#define INCR_INDEX(index, count, max)                \
+do {                                                 \
+	if (((index) + (count)) >= (max))            \
+		index = ((index) + (count)) - (max); \
+	else                                         \
+		index += (count);                    \
+} while (0)
+
+#define INCR_INDEX_BY1(index, max)	\
+do {                                    \
+	if ((++(index)) == (max))       \
+		index = 0;	        \
+} while (0)
+
+#define DECR_INDEX(index, count, max)                  \
+do {						       \
+	if ((count) > (index))                         \
+		index = ((max) - ((count - index)));   \
+	else                                           \
+		index -= count;			       \
+} while (0)
+
+#define OCT_BOARD_NAME 32
+#define OCT_SERIAL_LEN 64
+
+/* Structure used by core driver to send indication that the Octeon
+ * application is ready.
+ */
+struct octeon_core_setup {
+	u64 corefreq;
+
+	char boardname[OCT_BOARD_NAME];
+
+	char board_serial_number[OCT_SERIAL_LEN];
+
+	u64 board_rev_major;
+
+	u64 board_rev_minor;
+
+};
+
+/*---------------------------  SCATTER GATHER ENTRY  -----------------------*/
+
+/* The Scatter-Gather List Entry. The scatter or gather component used with
+ * a Octeon input instruction has this format.
+ */
+struct octeon_sg_entry {
+	/** The first 64 bit gives the size of data in each dptr.*/
+	union {
+		u16 size[4];
+		u64 size64;
+	} u;
+
+	/** The 4 dptr pointers for this entry. */
+	u64 ptr[4];
+
+};
+
+#define OCT_SG_ENTRY_SIZE    (sizeof(struct octeon_sg_entry))
+
+/* \brief Add size to gather list
+ * @param sg_entry scatter/gather entry
+ * @param size size to add
+ * @param pos position to add it.
+ */
+static inline void add_sg_size(struct octeon_sg_entry *sg_entry,
+			       u16 size,
+			       u32 pos)
+{
+#ifdef __BIG_ENDIAN_BITFIELD
+	sg_entry->u.size[pos] = size;
+#else
+	sg_entry->u.size[3 - pos] = size;
+#endif
+}
+
+/*------------------------- End Scatter/Gather ---------------------------*/
+
+#define   OCTNET_FRM_PTP_HEADER_SIZE  8
+#define   OCTNET_FRM_HEADER_SIZE     30 /* PTP timestamp + VLAN + Ethernet */
+
+#define   OCTNET_MIN_FRM_SIZE        (64  + OCTNET_FRM_PTP_HEADER_SIZE)
+#define   OCTNET_MAX_FRM_SIZE        (16000 + OCTNET_FRM_HEADER_SIZE)
+
+#define   OCTNET_DEFAULT_FRM_SIZE    (1500 + OCTNET_FRM_HEADER_SIZE)
+
+/** NIC Commands are sent using this Octeon Input Queue */
+#define   OCTNET_CMD_Q                0
+
+/* NIC Command types */
+#define   OCTNET_CMD_CHANGE_MTU       0x1
+#define   OCTNET_CMD_CHANGE_MACADDR   0x2
+#define   OCTNET_CMD_CHANGE_DEVFLAGS  0x3
+#define   OCTNET_CMD_RX_CTL           0x4
+
+#define	  OCTNET_CMD_SET_MULTI_LIST   0x5
+#define   OCTNET_CMD_CLEAR_STATS      0x6
+
+/* command for setting the speed, duplex & autoneg */
+#define   OCTNET_CMD_SET_SETTINGS     0x7
+#define   OCTNET_CMD_SET_FLOW_CTL     0x8
+
+#define   OCTNET_CMD_MDIO_READ_WRITE  0x9
+#define   OCTNET_CMD_GPIO_ACCESS      0xA
+#define   OCTNET_CMD_LRO_ENABLE       0xB
+#define   OCTNET_CMD_LRO_DISABLE      0xC
+#define   OCTNET_CMD_SET_RSS          0xD
+#define   OCTNET_CMD_WRITE_SA         0xE
+#define   OCTNET_CMD_DELETE_SA        0xF
+#define   OCTNET_CMD_UPDATE_SA        0x12
+
+#define   OCTNET_CMD_TNL_RX_CSUM_CTL 0x10
+#define   OCTNET_CMD_TNL_TX_CSUM_CTL 0x11
+#define   OCTNET_CMD_IPSECV2_AH_ESP_CTL 0x13
+#define   OCTNET_CMD_VERBOSE_ENABLE   0x14
+#define   OCTNET_CMD_VERBOSE_DISABLE  0x15
+
+/* RX(packets coming from wire) Checksum verification flags */
+/* TCP/UDP csum */
+#define   CNNIC_L4SUM_VERIFIED             0x1
+#define   CNNIC_IPSUM_VERIFIED             0x2
+#define   CNNIC_TUN_CSUM_VERIFIED          0x4
+#define   CNNIC_CSUM_VERIFIED (CNNIC_IPSUM_VERIFIED | CNNIC_L4SUM_VERIFIED)
+
+/*LROIPV4 and LROIPV6 Flags*/
+#define   OCTNIC_LROIPV4    0x1
+#define   OCTNIC_LROIPV6    0x2
+
+/* Interface flags communicated between host driver and core app. */
+enum octnet_ifflags {
+	OCTNET_IFFLAG_PROMISC   = 0x01,
+	OCTNET_IFFLAG_ALLMULTI  = 0x02,
+	OCTNET_IFFLAG_MULTICAST = 0x04,
+	OCTNET_IFFLAG_BROADCAST = 0x08,
+	OCTNET_IFFLAG_UNICAST   = 0x10
+};
+
+/*   wqe
+ *  ---------------  0
+ * |  wqe  word0-3 |
+ *  ---------------  32
+ * |    PCI IH     |
+ *  ---------------  40
+ * |     RPTR      |
+ *  ---------------  48
+ * |    PCI IRH    |
+ *  ---------------  56
+ * |  OCT_NET_CMD  |
+ *  ---------------  64
+ * | Addtl 8-BData |
+ * |               |
+ *  ---------------
+ */
+
+union octnet_cmd {
+	u64 u64;
+
+	struct {
+#ifdef __BIG_ENDIAN_BITFIELD
+		u64 cmd:5;
+
+		u64 more:6; /* How many udd words follow the command */
+
+		u64 param1:29;
+
+		u64 param2:16;
+
+		u64 param3:8;
+
+#else
+
+		u64 param3:8;
+
+		u64 param2:16;
+
+		u64 param1:29;
+
+		u64 more:6;
+
+		u64 cmd:5;
+
+#endif
+	} s;
+
+};
+
+#define   OCTNET_CMD_SIZE     (sizeof(union octnet_cmd))
+
+/** Instruction Header */
+struct octeon_instr_ih {
+#ifdef __BIG_ENDIAN_BITFIELD
+	/** Raw mode indicator 1 = RAW */
+	u64 raw:1;
+
+	/** Gather indicator 1=gather*/
+	u64 gather:1;
+
+	/** Data length OR no. of entries in gather list */
+	u64 dlengsz:14;
+
+	/** Front Data size */
+	u64 fsz:6;
+
+	/** Packet Order / Work Unit selection (1 of 8)*/
+	u64 qos:3;
+
+	/** Core group selection (1 of 16) */
+	u64 grp:4;
+
+	/** Short Raw Packet Indicator 1=short raw pkt */
+	u64 rs:1;
+
+	/** Tag type */
+	u64 tagtype:2;
+
+	/** Tag Value */
+	u64 tag:32;
+#else
+	/** Tag Value */
+	u64 tag:32;
+
+	/** Tag type */
+	u64 tagtype:2;
+
+	/** Short Raw Packet Indicator 1=short raw pkt */
+	u64 rs:1;
+
+	/** Core group selection (1 of 16) */
+	u64 grp:4;
+
+	/** Packet Order / Work Unit selection (1 of 8)*/
+	u64 qos:3;
+
+	/** Front Data size */
+	u64 fsz:6;
+
+	/** Data length OR no. of entries in gather list */
+	u64 dlengsz:14;
+
+	/** Gather indicator 1=gather*/
+	u64 gather:1;
+
+	/** Raw mode indicator 1 = RAW */
+	u64 raw:1;
+#endif
+};
+
+/** Input Request Header */
+struct octeon_instr_irh {
+#ifdef __BIG_ENDIAN_BITFIELD
+	u64 opcode:4;
+	u64 rflag:1;
+	u64 subcode:7;
+	u64 len:3;
+	u64 rid:13;
+	u64 reserved:4;
+	u64 ossp:32;             /* opcode/subcode specific parameters */
+#else
+	u64 ossp:32;             /* opcode/subcode specific parameters */
+	u64 reserved:4;
+	u64 rid:13;
+	u64 len:3;
+	u64 subcode:7;
+	u64 rflag:1;
+	u64 opcode:4;
+#endif
+};
+
+/** Return Data Parameters */
+struct octeon_instr_rdp {
+#ifdef __BIG_ENDIAN_BITFIELD
+	u64 reserved:49;
+	u64 pcie_port:3;
+	u64 rlen:12;
+#else
+	u64 rlen:12;
+	u64 pcie_port:3;
+	u64 reserved:49;
+#endif
+};
+
+/** Receive Header */
+union octeon_rh {
+#ifdef __BIG_ENDIAN_BITFIELD
+	u64 u64;
+	struct {
+		u64 opcode:4;
+		u64 subcode:8;
+		u64 len:3;       /** additional 64-bit words */
+		u64 rid:13;      /** request id in response to pkt sent by host */
+		u64 reserved:4;
+		u64 ossp:32;     /** opcode/subcode specific parameters */
+	} r;
+	struct {
+		u64 opcode:4;
+		u64 subcode:8;
+		u64 len:3;       /** additional 64-bit words */
+		u64 rid:13;      /** request id in response to pkt sent by host */
+		u64 extra:24;
+		u64 link:8;
+		u64 csum_verified:3;     /** checksum verified. */
+		u64 has_hwtstamp:1;      /** Has hardware timestamp. 1 = yes. */
+	} r_dh;
+	struct {
+		u64 opcode:4;
+		u64 subcode:8;
+		u64 len:3;       /** additional 64-bit words */
+		u64 rid:13;      /** request id in response to pkt sent by host */
+		u64 num_gmx_ports:8;
+		u64 max_nic_ports:8;
+		u64 app_cap_flags:4;
+		u64 app_mode:16;
+	} r_core_drv_init;
+	struct {
+		u64 opcode:4;
+		u64 subcode:8;
+		u64 len:3;       /** additional 64-bit words */
+		u64 rid:13;
+		u64 reserved:4;
+		u64 extra:25;
+		u64 ifidx:7;
+	} r_nic_info;
+#else
+	u64 u64;
+	struct {
+		u64 ossp:32;  /** opcode/subcode specific parameters */
+		u64 reserved:4;
+		u64 rid:13;   /** req id in response to pkt sent by host */
+		u64 len:3;    /** additional 64-bit words */
+		u64 subcode:8;
+		u64 opcode:4;
+	} r;
+	struct {
+		u64 has_hwtstamp:1;      /** 1 = has hwtstamp */
+		u64 csum_verified:3;     /** checksum verified. */
+		u64 link:8;
+		u64 extra:24;
+		u64 rid:13;   /** req id in response to pkt sent by host */
+		u64 len:3;    /** additional 64-bit words */
+		u64 subcode:8;
+		u64 opcode:4;
+	} r_dh;
+	struct {
+		u64 app_mode:16;
+		u64 app_cap_flags:4;
+		u64 max_nic_ports:8;
+		u64 num_gmx_ports:8;
+		u64 rid:13;
+		u64 len:3;       /** additional 64-bit words */
+		u64 subcode:8;
+		u64 opcode:4;
+	} r_core_drv_init;
+	struct {
+		u64 ifidx:7;
+		u64 extra:25;
+		u64 reserved:4;
+		u64 rid:13;
+		u64 len:3;       /** additional 64-bit words */
+		u64 subcode:8;
+		u64 opcode:4;
+	} r_nic_info;
+#endif
+};
+
+#define  OCT_RH_SIZE   (sizeof(union  octeon_rh))
+
+#define OCT_PKT_PARAM_IPV4OPTS   1
+#define OCT_PKT_PARAM_IPV6EXTHDR 2
+
+union octnic_packet_params {
+	u32 u32;
+	struct {
+#ifdef __BIG_ENDIAN_BITFIELD
+		u32 reserved:6;
+		u32 tnl_csum:1;
+		u32 ip_csum:1;
+		u32 ipv4opts_ipv6exthdr:2;
+		u32 ipsec_ops:4;
+		u32 tsflag:1;
+		u32 csoffset:9;
+		u32 ifidx:8;
+#else
+		u32 ifidx:8;
+		u32 csoffset:9;
+		u32 tsflag:1;
+		u32 ipsec_ops:4;
+		u32 ipv4opts_ipv6exthdr:2;
+		u32 ip_csum:1;
+		u32 tnl_csum:1;
+		u32 reserved:6;
+#endif
+	} s;
+};
+
+/** Status of a RGMII Link on Octeon as seen by core driver. */
+union oct_link_status {
+	u64 u64;
+
+	struct {
+#ifdef __BIG_ENDIAN_BITFIELD
+		u64 duplex:8;
+		u64 status:8;
+		u64 mtu:16;
+		u64 speed:16;
+		u64 autoneg:1;
+		u64 interface:4;
+		u64 pause:1;
+		u64 reserved:10;
+#else
+		u64 reserved:10;
+		u64 pause:1;
+		u64 interface:4;
+		u64 autoneg:1;
+		u64 speed:16;
+		u64 mtu:16;
+		u64 status:8;
+		u64 duplex:8;
+#endif
+	} s;
+};
+
+/** Information for a OCTEON ethernet interface shared between core & host. */
+struct oct_link_info {
+	union oct_link_status link;
+	u64 hw_addr;
+
+#ifdef __BIG_ENDIAN_BITFIELD
+	u16 gmxport;
+	u8 rsvd[3];
+	u8 num_txpciq;
+	u8 num_rxpciq;
+	u8 ifidx;
+#else
+	u8 ifidx;
+	u8 num_rxpciq;
+	u8 num_txpciq;
+	u8 rsvd[3];
+	u16 gmxport;
+#endif
+
+	u8 txpciq[MAX_IOQS_PER_NICIF];
+	u8 rxpciq[MAX_IOQS_PER_NICIF];
+};
+
+#define OCT_LINK_INFO_SIZE   (sizeof(struct oct_link_info))
+
+struct liquidio_if_cfg_info {
+	u64 ifidx;
+	u64 iqmask; /** mask for IQs enabled for  the port */
+	u64 oqmask; /** mask for OQs enabled for the port */
+	struct oct_link_info linfo; /** initial link information */
+};
+
+/** Stats for each NIC port in RX direction. */
+struct nic_rx_stats {
+	/* link-level stats */
+	u64 total_rcvd;
+	u64 bytes_rcvd;
+	u64 total_bcst;
+	u64 total_mcst;
+	u64 runts;
+	u64 ctl_rcvd;
+	u64 fifo_err;      /* Accounts for over/under-run of buffers */
+	u64 dmac_drop;
+	u64 fcs_err;
+	u64 jabber_err;
+	u64 l2_err;
+	u64 frame_err;
+
+	/* firmware stats */
+	u64 fw_total_rcvd;
+	u64 fw_total_fwd;
+	u64 fw_err_pko;
+	u64 fw_err_link;
+	u64 fw_err_drop;
+	u64 fw_lro_pkts;   /* Number of packets that are LROed      */
+	u64 fw_lro_octs;   /* Number of octets that are LROed       */
+	u64 fw_total_lro;  /* Number of LRO packets formed          */
+	u64 fw_lro_aborts; /* Number of times lRO of packet aborted */
+	/* intrmod: packet forward rate */
+	u64 fwd_rate;
+};
+
+/** Stats for each NIC port in RX direction. */
+struct nic_tx_stats {
+	/* link-level stats */
+	u64 total_pkts_sent;
+	u64 total_bytes_sent;
+	u64 mcast_pkts_sent;
+	u64 bcast_pkts_sent;
+	u64 ctl_sent;
+	u64 one_collision_sent;   /* Packets sent after one collision*/
+	u64 multi_collision_sent; /* Packets sent after multiple collision*/
+	u64 max_collision_fail;   /* Packets not sent due to max collisions */
+	u64 max_deferral_fail;   /* Packets not sent due to max deferrals */
+	u64 fifo_err;       /* Accounts for over/under-run of buffers */
+	u64 runts;
+	u64 total_collisions; /* Total number of collisions detected */
+
+	/* firmware stats */
+	u64 fw_total_sent;
+	u64 fw_total_fwd;
+	u64 fw_err_pko;
+	u64 fw_err_link;
+	u64 fw_err_drop;
+};
+
+struct oct_link_stats {
+	struct nic_rx_stats fromwire;
+	struct nic_tx_stats fromhost;
+
+};
+
+#define LIO68XX_LED_CTRL_ADDR     0x3501
+#define LIO68XX_LED_CTRL_CFGON    0x1f
+#define LIO68XX_LED_CTRL_CFGOFF   0x100
+#define LIO68XX_LED_BEACON_ADDR   0x3508
+#define LIO68XX_LED_BEACON_CFGON  0x47fd
+#define LIO68XX_LED_BEACON_CFGOFF 0x11fc
+#define VITESSE_PHY_GPIO_DRIVEON  0x1
+#define VITESSE_PHY_GPIO_CFG      0x8
+#define VITESSE_PHY_GPIO_DRIVEOFF 0x4
+#define VITESSE_PHY_GPIO_HIGH     0x2
+#define VITESSE_PHY_GPIO_LOW      0x3
+
+struct oct_mdio_cmd {
+	u64 op;
+	u64 mdio_addr;
+	u64 value1;
+	u64 value2;
+	u64 value3;
+};
+
+#define OCT_LINK_STATS_SIZE   (sizeof(struct oct_link_stats))
+
+#define LIO_INTRMOD_CHECK_INTERVAL  1
+#define LIO_INTRMOD_MAXPKT_RATETHR  196608 /* max pkt rate threshold */
+#define LIO_INTRMOD_MINPKT_RATETHR  9216   /* min pkt rate threshold */
+#define LIO_INTRMOD_MAXCNT_TRIGGER  384    /* max pkts to trigger interrupt */
+#define LIO_INTRMOD_MINCNT_TRIGGER  1      /* min pkts to trigger interrupt */
+#define LIO_INTRMOD_MAXTMR_TRIGGER  128    /* max time to trigger interrupt */
+#define LIO_INTRMOD_MINTMR_TRIGGER  32     /* min time to trigger interrupt */
+
+struct oct_intrmod_cfg {
+	u64 intrmod_enable;
+	u64 intrmod_check_intrvl;
+	u64 intrmod_maxpkt_ratethr;
+	u64 intrmod_minpkt_ratethr;
+	u64 intrmod_maxcnt_trigger;
+	u64 intrmod_maxtmr_trigger;
+	u64 intrmod_mincnt_trigger;
+	u64 intrmod_mintmr_trigger;
+};
+
+#define BASE_QUEUE_NOT_REQUESTED 65535
+
+union oct_nic_if_cfg {
+	u64 u64;
+	struct {
+#ifdef __BIG_ENDIAN_BITFIELD
+		u64 base_queue:16;
+		u64 num_iqueues:16;
+		u64 num_oqueues:16;
+		u64 gmx_port_id:8;
+		u64 reserved:8;
+#else
+		u64 reserved:8;
+		u64 gmx_port_id:8;
+		u64 num_oqueues:16;
+		u64 num_iqueues:16;
+		u64 base_queue:16;
+#endif
+	} s;
+};
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/liquidio_image.h b/drivers/net/ethernet/cavium/liquidio/liquidio_image.h
new file mode 100644
index 000000000000..93819bd8602b
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/liquidio_image.h
@@ -0,0 +1,57 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+#ifndef _LIQUIDIO_IMAGE_H_
+#define _LIQUIDIO_IMAGE_H_
+
+#define LIO_MAX_FW_TYPE_LEN     (8)
+#define LIO_MAX_FW_FILENAME_LEN (256)
+#define LIO_FW_DIR              "liquidio/"
+#define LIO_FW_BASE_NAME        "lio_"
+#define LIO_FW_NAME_SUFFIX      ".bin"
+#define LIO_FW_NAME_TYPE_NIC    "nic"
+#define LIO_FW_NAME_TYPE_NONE   "none"
+#define LIO_MAX_FIRMWARE_VERSION_LEN 16
+
+#define LIO_MAX_BOOTCMD_LEN 1024
+#define LIO_MAX_IMAGES 16
+#define LIO_NIC_MAGIC 0x434E4943     /* "CNIC" */
+struct octeon_firmware_desc {
+	__be64 addr;
+	__be32 len;
+	__be32 crc32;         /* crc32 of image */
+};
+
+/* Following the header is a list of 64-bit aligned binary images,
+ * as described by the desc field.
+ * Numeric fields are in network byte order.
+ */
+struct octeon_firmware_file_header {
+	__be32 magic;
+	char version[LIO_MAX_FIRMWARE_VERSION_LEN];
+	char bootcmd[LIO_MAX_BOOTCMD_LEN];
+	__be32 num_images;
+	struct octeon_firmware_desc desc[LIO_MAX_IMAGES];
+	__be32 pad;
+	__be32 crc32;         /* header checksum */
+};
+
+#endif /* _LIQUIDIO_IMAGE_H_ */
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_config.h b/drivers/net/ethernet/cavium/liquidio/octeon_config.h
new file mode 100644
index 000000000000..62a8dd5cd3dc
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_config.h
@@ -0,0 +1,424 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+
+/*! \file  octeon_config.h
+ *  \brief Host Driver: Configuration data structures for the host driver.
+ */
+
+#ifndef __OCTEON_CONFIG_H__
+#define __OCTEON_CONFIG_H__
+
+/*--------------------------CONFIG VALUES------------------------*/
+
+/* The following macros affect the way the driver data structures
+ * are generated for Octeon devices.
+ * They can be modified.
+ */
+
+/* Maximum octeon devices defined as MAX_OCTEON_NICIF to support
+ * multiple(<= MAX_OCTEON_NICIF) Miniports
+ */
+#define   MAX_OCTEON_NICIF             32
+#define   MAX_OCTEON_DEVICES           MAX_OCTEON_NICIF
+#define   MAX_OCTEON_LINKS	       MAX_OCTEON_NICIF
+#define   MAX_OCTEON_MULTICAST_ADDR    32
+
+/* CN6xxx IQ configuration macros */
+#define   CN6XXX_MAX_INPUT_QUEUES      32
+#define   CN6XXX_MAX_IQ_DESCRIPTORS    2048
+#define   CN6XXX_DB_MIN                1
+#define   CN6XXX_DB_MAX                8
+#define   CN6XXX_DB_TIMEOUT            1
+
+/* CN6xxx OQ configuration macros */
+#define   CN6XXX_MAX_OUTPUT_QUEUES     32
+#define   CN6XXX_MAX_OQ_DESCRIPTORS    2048
+#define   CN6XXX_OQ_BUF_SIZE           1536
+#define   CN6XXX_OQ_PKTSPER_INTR       ((CN6XXX_MAX_OQ_DESCRIPTORS < 512) ? \
+					(CN6XXX_MAX_OQ_DESCRIPTORS / 4) : 128)
+#define   CN6XXX_OQ_REFIL_THRESHOLD    ((CN6XXX_MAX_OQ_DESCRIPTORS < 512) ? \
+					(CN6XXX_MAX_OQ_DESCRIPTORS / 4) : 128)
+
+#define   CN6XXX_OQ_INTR_PKT           64
+#define   CN6XXX_OQ_INTR_TIME          100
+#define   DEFAULT_NUM_NIC_PORTS_66XX   2
+#define   DEFAULT_NUM_NIC_PORTS_68XX   4
+#define   DEFAULT_NUM_NIC_PORTS_68XX_210NV  2
+
+/* common OCTEON configuration macros */
+#define   CN6XXX_CFG_IO_QUEUES         32
+#define   OCTEON_32BYTE_INSTR          32
+#define   OCTEON_64BYTE_INSTR          64
+#define   OCTEON_MAX_BASE_IOQ          4
+#define   OCTEON_OQ_BUFPTR_MODE        0
+#define   OCTEON_OQ_INFOPTR_MODE       1
+
+#define   OCTEON_DMA_INTR_PKT          64
+#define   OCTEON_DMA_INTR_TIME         1000
+
+#define MAX_TXQS_PER_INTF  8
+#define MAX_RXQS_PER_INTF  8
+#define DEF_TXQS_PER_INTF  4
+#define DEF_RXQS_PER_INTF  4
+
+#define INVALID_IOQ_NO          0xff
+
+#define   DEFAULT_POW_GRP       0
+
+/* Macros to get octeon config params */
+#define CFG_GET_IQ_CFG(cfg)                      ((cfg)->iq)
+#define CFG_GET_IQ_MAX_Q(cfg)                    ((cfg)->iq.max_iqs)
+#define CFG_GET_IQ_PENDING_LIST_SIZE(cfg)        ((cfg)->iq.pending_list_size)
+#define CFG_GET_IQ_INSTR_TYPE(cfg)               ((cfg)->iq.instr_type)
+#define CFG_GET_IQ_DB_MIN(cfg)                   ((cfg)->iq.db_min)
+#define CFG_GET_IQ_DB_TIMEOUT(cfg)               ((cfg)->iq.db_timeout)
+
+#define CFG_GET_OQ_MAX_Q(cfg)                    ((cfg)->oq.max_oqs)
+#define CFG_GET_OQ_INFO_PTR(cfg)                 ((cfg)->oq.info_ptr)
+#define CFG_GET_OQ_PKTS_PER_INTR(cfg)            ((cfg)->oq.pkts_per_intr)
+#define CFG_GET_OQ_REFILL_THRESHOLD(cfg)         ((cfg)->oq.refill_threshold)
+#define CFG_GET_OQ_INTR_PKT(cfg)                 ((cfg)->oq.oq_intr_pkt)
+#define CFG_GET_OQ_INTR_TIME(cfg)                ((cfg)->oq.oq_intr_time)
+#define CFG_SET_OQ_INTR_PKT(cfg, val)            (cfg)->oq.oq_intr_pkt = val
+#define CFG_SET_OQ_INTR_TIME(cfg, val)           (cfg)->oq.oq_intr_time = val
+
+#define CFG_GET_DMA_INTR_PKT(cfg)                ((cfg)->dma.dma_intr_pkt)
+#define CFG_GET_DMA_INTR_TIME(cfg)               ((cfg)->dma.dma_intr_time)
+#define CFG_GET_NUM_NIC_PORTS(cfg)               ((cfg)->num_nic_ports)
+#define CFG_GET_NUM_DEF_TX_DESCS(cfg)            ((cfg)->num_def_tx_descs)
+#define CFG_GET_NUM_DEF_RX_DESCS(cfg)            ((cfg)->num_def_rx_descs)
+#define CFG_GET_DEF_RX_BUF_SIZE(cfg)             ((cfg)->def_rx_buf_size)
+
+#define CFG_GET_MAX_TXQS_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].max_txqs)
+#define CFG_GET_NUM_TXQS_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].num_txqs)
+#define CFG_GET_MAX_RXQS_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].max_rxqs)
+#define CFG_GET_NUM_RXQS_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].num_rxqs)
+#define CFG_GET_NUM_RX_DESCS_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].num_rx_descs)
+#define CFG_GET_NUM_TX_DESCS_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].num_tx_descs)
+#define CFG_GET_NUM_RX_BUF_SIZE_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].rx_buf_size)
+#define CFG_GET_BASE_QUE_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].base_queue)
+#define CFG_GET_GMXID_NIC_IF(cfg, idx) \
+				((cfg)->nic_if_cfg[idx].gmx_port_id)
+
+#define CFG_GET_CTRL_Q_GRP(cfg)                  ((cfg)->misc.ctrlq_grp)
+#define CFG_GET_HOST_LINK_QUERY_INTERVAL(cfg) \
+				((cfg)->misc.host_link_query_interval)
+#define CFG_GET_OCT_LINK_QUERY_INTERVAL(cfg) \
+				((cfg)->misc.oct_link_query_interval)
+#define CFG_GET_IS_SLI_BP_ON(cfg)                ((cfg)->misc.enable_sli_oq_bp)
+
+/* Max IOQs per OCTEON Link */
+#define MAX_IOQS_PER_NICIF              32
+
+enum lio_card_type {
+	LIO_210SV = 0, /* Two port, 66xx */
+	LIO_210NV,     /* Two port, 68xx */
+	LIO_410NV      /* Four port, 68xx */
+};
+
+#define LIO_210SV_NAME "210sv"
+#define LIO_210NV_NAME "210nv"
+#define LIO_410NV_NAME "410nv"
+
+/** Structure to define the configuration attributes for each Input queue.
+ *  Applicable to all Octeon processors
+ **/
+struct octeon_iq_config {
+#ifdef __BIG_ENDIAN_BITFIELD
+	u64 reserved:32;
+
+	/** Minimum ticks to wait before checking for pending instructions. */
+	u64 db_timeout:16;
+
+	/** Minimum number of commands pending to be posted to Octeon
+	 *  before driver hits the Input queue doorbell.
+	 */
+	u64 db_min:8;
+
+	/** Command size - 32 or 64 bytes */
+	u64 instr_type:32;
+
+	/** Pending list size (usually set to the sum of the size of all Input
+	 *  queues)
+	 */
+	u64 pending_list_size:32;
+
+	/* Max number of IQs available */
+	u64 max_iqs:8;
+#else
+	/* Max number of IQs available */
+	u64 max_iqs:8;
+
+	/** Pending list size (usually set to the sum of the size of all Input
+	 *  queues)
+	 */
+	u64 pending_list_size:32;
+
+	/** Command size - 32 or 64 bytes */
+	u64 instr_type:32;
+
+	/** Minimum number of commands pending to be posted to Octeon
+	 *  before driver hits the Input queue doorbell.
+	 */
+	u64 db_min:8;
+
+	/** Minimum ticks to wait before checking for pending instructions. */
+	u64 db_timeout:16;
+
+	u64 reserved:32;
+#endif
+};
+
+/** Structure to define the configuration attributes for each Output queue.
+ *  Applicable to all Octeon processors
+ **/
+struct octeon_oq_config {
+#ifdef __BIG_ENDIAN_BITFIELD
+	u64 reserved:16;
+
+	u64 pkts_per_intr:16;
+
+	/** Interrupt Coalescing (Time Interval). Octeon will interrupt the
+	 *  host if atleast one packet was sent in the time interval specified
+	 *  by this field. The driver uses time interval interrupt coalescing
+	 *  by default. The time is specified in microseconds.
+	 */
+	u64 oq_intr_time:16;
+
+	/** Interrupt Coalescing (Packet Count). Octeon will interrupt the host
+	 *  only if it sent as many packets as specified by this field.
+	 *  The driver
+	 *  usually does not use packet count interrupt coalescing.
+	 */
+	u64 oq_intr_pkt:16;
+
+	/** The number of buffers that were consumed during packet processing by
+	 *   the driver on this Output queue before the driver attempts to
+	 *   replenish
+	 *   the descriptor ring with new buffers.
+	 */
+	u64 refill_threshold:16;
+
+	/** If set, the Output queue uses info-pointer mode. (Default: 1 ) */
+	u64 info_ptr:32;
+
+	/* Max number of OQs available */
+	u64 max_oqs:8;
+
+#else
+	/* Max number of OQs available */
+	u64 max_oqs:8;
+
+	/** If set, the Output queue uses info-pointer mode. (Default: 1 ) */
+	u64 info_ptr:32;
+
+	/** The number of buffers that were consumed during packet processing by
+	 *   the driver on this Output queue before the driver attempts to
+	 *   replenish
+	 *   the descriptor ring with new buffers.
+	 */
+	u64 refill_threshold:16;
+
+	/** Interrupt Coalescing (Packet Count). Octeon will interrupt the host
+	 *  only if it sent as many packets as specified by this field.
+	 *  The driver
+	 *  usually does not use packet count interrupt coalescing.
+	 */
+	u64 oq_intr_pkt:16;
+
+	/** Interrupt Coalescing (Time Interval). Octeon will interrupt the
+	 *  host if atleast one packet was sent in the time interval specified
+	 *  by this field. The driver uses time interval interrupt coalescing
+	 *  by default.  The time is specified in microseconds.
+	 */
+	u64 oq_intr_time:16;
+
+	u64 pkts_per_intr:16;
+
+	u64 reserved:16;
+#endif
+
+};
+
+/** This structure conatins the NIC link configuration attributes,
+ *  common for all the OCTEON Modles.
+ */
+struct octeon_nic_if_config {
+#ifdef __BIG_ENDIAN_BITFIELD
+	u64 reserved:56;
+
+	u64 base_queue:16;
+
+	u64 gmx_port_id:8;
+
+	/* SKB size, We need not change buf size even for Jumbo frames.
+	 * Octeon can send jumbo frames in 4 consecutive descriptors,
+	 */
+	u64 rx_buf_size:16;
+
+	/* Num of desc for tx rings */
+	u64 num_tx_descs:16;
+
+	/* Num of desc for rx rings */
+	u64 num_rx_descs:16;
+
+	/* Actual configured value. Range could be: 1...max_rxqs */
+	u64 num_rxqs:16;
+
+	/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+	u64 max_rxqs:16;
+
+	/* Actual configured value. Range could be: 1...max_txqs */
+	u64 num_txqs:16;
+
+	/* Max Txqs: Half for each of the two ports :max_iq/2 */
+	u64 max_txqs:16;
+#else
+	/* Max Txqs: Half for each of the two ports :max_iq/2 */
+	u64 max_txqs:16;
+
+	/* Actual configured value. Range could be: 1...max_txqs */
+	u64 num_txqs:16;
+
+	/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+	u64 max_rxqs:16;
+
+	/* Actual configured value. Range could be: 1...max_rxqs */
+	u64 num_rxqs:16;
+
+	/* Num of desc for rx rings */
+	u64 num_rx_descs:16;
+
+	/* Num of desc for tx rings */
+	u64 num_tx_descs:16;
+
+	/* SKB size, We need not change buf size even for Jumbo frames.
+	 * Octeon can send jumbo frames in 4 consecutive descriptors,
+	 */
+	u64 rx_buf_size:16;
+
+	u64 gmx_port_id:8;
+
+	u64 base_queue:16;
+
+	u64 reserved:56;
+#endif
+
+};
+
+/** Structure to define the configuration attributes for meta data.
+ *  Applicable to all Octeon processors.
+ */
+
+struct octeon_misc_config {
+#ifdef __BIG_ENDIAN_BITFIELD
+	/** Host link status polling period */
+	u64 host_link_query_interval:32;
+	/** Oct link status polling period */
+	u64 oct_link_query_interval:32;
+
+	u64 enable_sli_oq_bp:1;
+	/** Control IQ Group */
+	u64 ctrlq_grp:4;
+#else
+	/** Control IQ Group */
+	u64 ctrlq_grp:4;
+	/** BP for SLI OQ */
+	u64 enable_sli_oq_bp:1;
+	/** Host link status polling period */
+	u64 oct_link_query_interval:32;
+	/** Oct link status polling period */
+	u64 host_link_query_interval:32;
+#endif
+};
+
+/** Structure to define the configuration for all OCTEON processors. */
+struct octeon_config {
+	u16 card_type;
+	char *card_name;
+
+	/** Input Queue attributes. */
+	struct octeon_iq_config iq;
+
+	/** Output Queue attributes. */
+	struct octeon_oq_config oq;
+
+	/** NIC Port Configuration */
+	struct octeon_nic_if_config nic_if_cfg[MAX_OCTEON_NICIF];
+
+	/** Miscellaneous attributes */
+	struct octeon_misc_config misc;
+
+	int num_nic_ports;
+
+	int num_def_tx_descs;
+
+	/* Num of desc for rx rings */
+	int num_def_rx_descs;
+
+	int def_rx_buf_size;
+
+};
+
+/* The following config values are fixed and should not be modified. */
+
+/* Maximum address space to be mapped for Octeon's BAR1 index-based access. */
+#define  MAX_BAR1_MAP_INDEX                     2
+#define  OCTEON_BAR1_ENTRY_SIZE         (4 * 1024 * 1024)
+
+/* BAR1 Index 0 to (MAX_BAR1_MAP_INDEX - 1) for normal mapped memory access.
+ * Bar1 register at MAX_BAR1_MAP_INDEX used by driver for dynamic access.
+ */
+#define  MAX_BAR1_IOREMAP_SIZE  ((MAX_BAR1_MAP_INDEX + 1) * \
+				 OCTEON_BAR1_ENTRY_SIZE)
+
+/* Response lists - 1 ordered, 1 unordered-blocking, 1 unordered-nonblocking
+ * NoResponse Lists are now maintained with each IQ. (Dec' 2007).
+ */
+#define MAX_RESPONSE_LISTS           4
+
+/* Opcode hash bits. The opcode is hashed on the lower 6-bits to lookup the
+ * dispatch table.
+ */
+#define OPCODE_MASK_BITS             6
+
+/* Mask for the 6-bit lookup hash */
+#define OCTEON_OPCODE_MASK           0x3f
+
+/* Size of the dispatch table. The 6-bit hash can index into 2^6 entries */
+#define DISPATCH_LIST_SIZE                      BIT(OPCODE_MASK_BITS)
+
+/* Maximum number of Octeon Instruction (command) queues */
+#define MAX_OCTEON_INSTR_QUEUES         CN6XXX_MAX_INPUT_QUEUES
+
+/* Maximum number of Octeon Instruction (command) queues */
+#define MAX_OCTEON_OUTPUT_QUEUES        CN6XXX_MAX_OUTPUT_QUEUES
+
+#endif /* __OCTEON_CONFIG_H__  */
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_console.c b/drivers/net/ethernet/cavium/liquidio/octeon_console.c
new file mode 100644
index 000000000000..466147e409c9
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_console.c
@@ -0,0 +1,723 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+
+/**
+ * @file octeon_console.c
+ */
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+#include "octeon_mem_ops.h"
+
+static void octeon_remote_lock(void);
+static void octeon_remote_unlock(void);
+static u64 cvmx_bootmem_phy_named_block_find(struct octeon_device *oct,
+					     const char *name,
+					     u32 flags);
+
+#define MIN(a, b) min((a), (b))
+#define CAST_ULL(v) ((u64)(v))
+
+#define BOOTLOADER_PCI_READ_BUFFER_DATA_ADDR    0x0006c008
+#define BOOTLOADER_PCI_READ_BUFFER_LEN_ADDR     0x0006c004
+#define BOOTLOADER_PCI_READ_BUFFER_OWNER_ADDR   0x0006c000
+#define BOOTLOADER_PCI_READ_DESC_ADDR           0x0006c100
+#define BOOTLOADER_PCI_WRITE_BUFFER_STR_LEN     248
+
+#define OCTEON_PCI_IO_BUF_OWNER_OCTEON    0x00000001
+#define OCTEON_PCI_IO_BUF_OWNER_HOST      0x00000002
+
+/** Can change without breaking ABI */
+#define CVMX_BOOTMEM_NUM_NAMED_BLOCKS 64
+
+/** minimum alignment of bootmem alloced blocks */
+#define CVMX_BOOTMEM_ALIGNMENT_SIZE     (16ull)
+
+/** CVMX bootmem descriptor major version */
+#define CVMX_BOOTMEM_DESC_MAJ_VER   3
+/* CVMX bootmem descriptor minor version */
+#define CVMX_BOOTMEM_DESC_MIN_VER   0
+
+/* Current versions */
+#define OCTEON_PCI_CONSOLE_MAJOR_VERSION    1
+#define OCTEON_PCI_CONSOLE_MINOR_VERSION    0
+#define OCTEON_PCI_CONSOLE_BLOCK_NAME   "__pci_console"
+#define OCTEON_CONSOLE_POLL_INTERVAL_MS  100    /* 10 times per second */
+
+/* First three members of cvmx_bootmem_desc are left in original
+** positions for backwards compatibility.
+** Assumes big endian target
+*/
+struct cvmx_bootmem_desc {
+	/** spinlock to control access to list */
+	u32 lock;
+
+	/** flags for indicating various conditions */
+	u32 flags;
+
+	u64 head_addr;
+
+	/** incremented changed when incompatible changes made */
+	u32 major_version;
+
+	/** incremented changed when compatible changes made,
+	 *  reset to zero when major incremented
+	 */
+	u32 minor_version;
+
+	u64 app_data_addr;
+	u64 app_data_size;
+
+	/** number of elements in named blocks array */
+	u32 nb_num_blocks;
+
+	/** length of name array in bootmem blocks */
+	u32 named_block_name_len;
+
+	/** address of named memory block descriptors */
+	u64 named_block_array_addr;
+};
+
+/* Structure that defines a single console.
+ *
+ * Note: when read_index == write_index, the buffer is empty.
+ * The actual usable size of each console is console_buf_size -1;
+ */
+struct octeon_pci_console {
+	u64 input_base_addr;
+	u32 input_read_index;
+	u32 input_write_index;
+	u64 output_base_addr;
+	u32 output_read_index;
+	u32 output_write_index;
+	u32 lock;
+	u32 buf_size;
+};
+
+/* This is the main container structure that contains all the information
+ * about all PCI consoles.  The address of this structure is passed to various
+ * routines that operation on PCI consoles.
+ */
+struct octeon_pci_console_desc {
+	u32 major_version;
+	u32 minor_version;
+	u32 lock;
+	u32 flags;
+	u32 num_consoles;
+	u32 pad;
+	/* must be 64 bit aligned here... */
+	/* Array of addresses of octeon_pci_console structures */
+	u64 console_addr_array[0];
+	/* Implicit storage for console_addr_array */
+};
+
+/**
+ * This macro returns the size of a member of a structure.
+ * Logically it is the same as "sizeof(s::field)" in C++, but
+ * C lacks the "::" operator.
+ */
+#define SIZEOF_FIELD(s, field) sizeof(((s *)NULL)->field)
+
+/**
+ * This macro returns a member of the cvmx_bootmem_desc
+ * structure. These members can't be directly addressed as
+ * they might be in memory not directly reachable. In the case
+ * where bootmem is compiled with LINUX_HOST, the structure
+ * itself might be located on a remote Octeon. The argument
+ * "field" is the member name of the cvmx_bootmem_desc to read.
+ * Regardless of the type of the field, the return type is always
+ * a u64.
+ */
+#define CVMX_BOOTMEM_DESC_GET_FIELD(oct, field)                              \
+	__cvmx_bootmem_desc_get(oct, oct->bootmem_desc_addr,                 \
+				offsetof(struct cvmx_bootmem_desc, field),   \
+				SIZEOF_FIELD(struct cvmx_bootmem_desc, field))
+
+#define __cvmx_bootmem_lock(flags)
+#define __cvmx_bootmem_unlock(flags)
+
+/**
+ * This macro returns a member of the
+ * cvmx_bootmem_named_block_desc structure. These members can't
+ * be directly addressed as they might be in memory not directly
+ * reachable. In the case where bootmem is compiled with
+ * LINUX_HOST, the structure itself might be located on a remote
+ * Octeon. The argument "field" is the member name of the
+ * cvmx_bootmem_named_block_desc to read. Regardless of the type
+ * of the field, the return type is always a u64. The "addr"
+ * parameter is the physical address of the structure.
+ */
+#define CVMX_BOOTMEM_NAMED_GET_FIELD(oct, addr, field)                   \
+	__cvmx_bootmem_desc_get(oct, addr,                               \
+		offsetof(struct cvmx_bootmem_named_block_desc, field),   \
+		SIZEOF_FIELD(struct cvmx_bootmem_named_block_desc, field))
+
+/**
+ * This function is the implementation of the get macros defined
+ * for individual structure members. The argument are generated
+ * by the macros inorder to read only the needed memory.
+ *
+ * @param oct    Pointer to current octeon device
+ * @param base   64bit physical address of the complete structure
+ * @param offset Offset from the beginning of the structure to the member being
+ *               accessed.
+ * @param size   Size of the structure member.
+ *
+ * @return Value of the structure member promoted into a u64.
+ */
+static inline u64 __cvmx_bootmem_desc_get(struct octeon_device *oct,
+					  u64 base,
+					  u32 offset,
+					  u32 size)
+{
+	base = (1ull << 63) | (base + offset);
+	switch (size) {
+	case 4:
+		return octeon_read_device_mem32(oct, base);
+	case 8:
+		return octeon_read_device_mem64(oct, base);
+	default:
+		return 0;
+	}
+}
+
+/**
+ * This function retrieves the string name of a named block. It is
+ * more complicated than a simple memcpy() since the named block
+ * descriptor may not be directly accessible.
+ *
+ * @param addr   Physical address of the named block descriptor
+ * @param str    String to receive the named block string name
+ * @param len    Length of the string buffer, which must match the length
+ *               stored in the bootmem descriptor.
+ */
+static void CVMX_BOOTMEM_NAMED_GET_NAME(struct octeon_device *oct,
+					u64 addr,
+					char *str,
+					u32 len)
+{
+	addr += offsetof(struct cvmx_bootmem_named_block_desc, name);
+	octeon_pci_read_core_mem(oct, addr, str, len);
+	str[len] = 0;
+}
+
+/* See header file for descriptions of functions */
+
+/**
+ * Check the version information on the bootmem descriptor
+ *
+ * @param exact_match
+ *               Exact major version to check against. A zero means
+ *               check that the version supports named blocks.
+ *
+ * @return Zero if the version is correct. Negative if the version is
+ *         incorrect. Failures also cause a message to be displayed.
+ */
+static int __cvmx_bootmem_check_version(struct octeon_device *oct,
+					u32 exact_match)
+{
+	u32 major_version;
+	u32 minor_version;
+
+	if (!oct->bootmem_desc_addr)
+		oct->bootmem_desc_addr =
+			octeon_read_device_mem64(oct,
+						 BOOTLOADER_PCI_READ_DESC_ADDR);
+	major_version =
+		(u32)CVMX_BOOTMEM_DESC_GET_FIELD(oct, major_version);
+	minor_version =
+		(u32)CVMX_BOOTMEM_DESC_GET_FIELD(oct, minor_version);
+	dev_dbg(&oct->pci_dev->dev, "%s: major_version=%d\n", __func__,
+		major_version);
+	if ((major_version > 3) ||
+	    (exact_match && major_version != exact_match)) {
+		dev_err(&oct->pci_dev->dev, "bootmem ver mismatch %d.%d addr:0x%llx\n",
+			major_version, minor_version,
+			CAST_ULL(oct->bootmem_desc_addr));
+		return -1;
+	} else {
+		return 0;
+	}
+}
+
+static const struct cvmx_bootmem_named_block_desc
+*__cvmx_bootmem_find_named_block_flags(struct octeon_device *oct,
+					const char *name, u32 flags)
+{
+	struct cvmx_bootmem_named_block_desc *desc =
+		&oct->bootmem_named_block_desc;
+	u64 named_addr = cvmx_bootmem_phy_named_block_find(oct, name, flags);
+
+	if (named_addr) {
+		desc->base_addr = CVMX_BOOTMEM_NAMED_GET_FIELD(oct, named_addr,
+							       base_addr);
+		desc->size =
+			CVMX_BOOTMEM_NAMED_GET_FIELD(oct, named_addr, size);
+		strncpy(desc->name, name, sizeof(desc->name));
+		desc->name[sizeof(desc->name) - 1] = 0;
+		return &oct->bootmem_named_block_desc;
+	} else {
+		return NULL;
+	}
+}
+
+static u64 cvmx_bootmem_phy_named_block_find(struct octeon_device *oct,
+					     const char *name,
+					     u32 flags)
+{
+	u64 result = 0;
+
+	__cvmx_bootmem_lock(flags);
+	if (!__cvmx_bootmem_check_version(oct, 3)) {
+		u32 i;
+		u64 named_block_array_addr =
+			CVMX_BOOTMEM_DESC_GET_FIELD(oct,
+						    named_block_array_addr);
+		u32 num_blocks = (u32)
+			CVMX_BOOTMEM_DESC_GET_FIELD(oct, nb_num_blocks);
+		u32 name_length = (u32)
+			CVMX_BOOTMEM_DESC_GET_FIELD(oct, named_block_name_len);
+		u64 named_addr = named_block_array_addr;
+
+		for (i = 0; i < num_blocks; i++) {
+			u64 named_size =
+				CVMX_BOOTMEM_NAMED_GET_FIELD(oct, named_addr,
+							     size);
+			if (name && named_size) {
+				char *name_tmp =
+					kmalloc(name_length + 1, GFP_KERNEL);
+				CVMX_BOOTMEM_NAMED_GET_NAME(oct, named_addr,
+							    name_tmp,
+							    name_length);
+				if (!strncmp(name, name_tmp, name_length)) {
+					result = named_addr;
+					kfree(name_tmp);
+					break;
+				}
+				kfree(name_tmp);
+			} else if (!name && !named_size) {
+				result = named_addr;
+				break;
+			}
+
+			named_addr +=
+				sizeof(struct cvmx_bootmem_named_block_desc);
+		}
+	}
+	__cvmx_bootmem_unlock(flags);
+	return result;
+}
+
+/**
+ * Find a named block on the remote Octeon
+ *
+ * @param name      Name of block to find
+ * @param base_addr Address the block is at (OUTPUT)
+ * @param size      The size of the block (OUTPUT)
+ *
+ * @return Zero on success, One on failure.
+ */
+static int octeon_named_block_find(struct octeon_device *oct, const char *name,
+				   u64 *base_addr, u64 *size)
+{
+	const struct cvmx_bootmem_named_block_desc *named_block;
+
+	octeon_remote_lock();
+	named_block = __cvmx_bootmem_find_named_block_flags(oct, name, 0);
+	octeon_remote_unlock();
+	if (named_block) {
+		*base_addr = named_block->base_addr;
+		*size = named_block->size;
+		return 0;
+	}
+	return 1;
+}
+
+static void octeon_remote_lock(void)
+{
+	/* fill this in if any sharing is needed */
+}
+
+static void octeon_remote_unlock(void)
+{
+	/* fill this in if any sharing is needed */
+}
+
+int octeon_console_send_cmd(struct octeon_device *oct, char *cmd_str,
+			    u32 wait_hundredths)
+{
+	u32 len = strlen(cmd_str);
+
+	dev_dbg(&oct->pci_dev->dev, "sending \"%s\" to bootloader\n", cmd_str);
+
+	if (len > BOOTLOADER_PCI_WRITE_BUFFER_STR_LEN - 1) {
+		dev_err(&oct->pci_dev->dev, "Command string too long, max length is: %d\n",
+			BOOTLOADER_PCI_WRITE_BUFFER_STR_LEN - 1);
+		return -1;
+	}
+
+	if (octeon_wait_for_bootloader(oct, wait_hundredths) != 0) {
+		dev_err(&oct->pci_dev->dev, "Bootloader not ready for command.\n");
+		return -1;
+	}
+
+	/* Write command to bootloader */
+	octeon_remote_lock();
+	octeon_pci_write_core_mem(oct, BOOTLOADER_PCI_READ_BUFFER_DATA_ADDR,
+				  (u8 *)cmd_str, len);
+	octeon_write_device_mem32(oct, BOOTLOADER_PCI_READ_BUFFER_LEN_ADDR,
+				  len);
+	octeon_write_device_mem32(oct, BOOTLOADER_PCI_READ_BUFFER_OWNER_ADDR,
+				  OCTEON_PCI_IO_BUF_OWNER_OCTEON);
+
+	/* Bootloader should accept command very quickly
+	 * if it really was ready
+	 */
+	if (octeon_wait_for_bootloader(oct, 200) != 0) {
+		octeon_remote_unlock();
+		dev_err(&oct->pci_dev->dev, "Bootloader did not accept command.\n");
+		return -1;
+	}
+	octeon_remote_unlock();
+	return 0;
+}
+
+int octeon_wait_for_bootloader(struct octeon_device *oct,
+			       u32 wait_time_hundredths)
+{
+	dev_dbg(&oct->pci_dev->dev, "waiting %d0 ms for bootloader\n",
+		wait_time_hundredths);
+
+	if (octeon_mem_access_ok(oct))
+		return -1;
+
+	while (wait_time_hundredths > 0 &&
+	       octeon_read_device_mem32(oct,
+					BOOTLOADER_PCI_READ_BUFFER_OWNER_ADDR)
+	       != OCTEON_PCI_IO_BUF_OWNER_HOST) {
+		if (--wait_time_hundredths <= 0)
+			return -1;
+		schedule_timeout_uninterruptible(HZ / 100);
+	}
+	return 0;
+}
+
+static void octeon_console_handle_result(struct octeon_device *oct,
+					 size_t console_num,
+					 char *buffer, s32 bytes_read)
+{
+	struct octeon_console *console;
+
+	console = &oct->console[console_num];
+
+	console->waiting = 0;
+}
+
+static char console_buffer[OCTEON_CONSOLE_MAX_READ_BYTES];
+
+static void output_console_line(struct octeon_device *oct,
+				struct octeon_console *console,
+				size_t console_num,
+				char *console_buffer,
+				s32 bytes_read)
+{
+	char *line;
+	s32 i;
+
+	line = console_buffer;
+	for (i = 0; i < bytes_read; i++) {
+		/* Output a line at a time, prefixed */
+		if (console_buffer[i] == '\n') {
+			console_buffer[i] = '\0';
+			if (console->leftover[0]) {
+				dev_info(&oct->pci_dev->dev, "%lu: %s%s\n",
+					 console_num, console->leftover,
+					 line);
+				console->leftover[0] = '\0';
+			} else {
+				dev_info(&oct->pci_dev->dev, "%lu: %s\n",
+					 console_num, line);
+			}
+			line = &console_buffer[i + 1];
+		}
+	}
+
+	/* Save off any leftovers */
+	if (line != &console_buffer[bytes_read]) {
+		console_buffer[bytes_read] = '\0';
+		strcpy(console->leftover, line);
+	}
+}
+
+static void check_console(struct work_struct *work)
+{
+	s32 bytes_read, tries, total_read;
+	struct octeon_console *console;
+	struct cavium_wk *wk = (struct cavium_wk *)work;
+	struct octeon_device *oct = (struct octeon_device *)wk->ctxptr;
+	size_t console_num = wk->ctxul;
+	u32 delay;
+
+	console = &oct->console[console_num];
+	tries = 0;
+	total_read = 0;
+
+	do {
+		/* Take console output regardless of whether it will
+		 * be logged
+		 */
+		bytes_read =
+			octeon_console_read(oct, console_num, console_buffer,
+					    sizeof(console_buffer) - 1, 0);
+		if (bytes_read > 0) {
+			total_read += bytes_read;
+			if (console->waiting) {
+				octeon_console_handle_result(oct, console_num,
+							     console_buffer,
+							     bytes_read);
+			}
+			if (octeon_console_debug_enabled(console_num)) {
+				output_console_line(oct, console, console_num,
+						    console_buffer, bytes_read);
+			}
+		} else if (bytes_read < 0) {
+			dev_err(&oct->pci_dev->dev, "Error reading console %lu, ret=%d\n",
+				console_num, bytes_read);
+		}
+
+		tries++;
+	} while ((bytes_read > 0) && (tries < 16));
+
+	/* If nothing is read after polling the console,
+	 * output any leftovers if any
+	 */
+	if (octeon_console_debug_enabled(console_num) &&
+	    (total_read == 0) && (console->leftover[0])) {
+		dev_info(&oct->pci_dev->dev, "%lu: %s\n",
+			 console_num, console->leftover);
+		console->leftover[0] = '\0';
+	}
+
+	delay = OCTEON_CONSOLE_POLL_INTERVAL_MS;
+
+	schedule_delayed_work(&wk->work, msecs_to_jiffies(delay));
+}
+
+int octeon_init_consoles(struct octeon_device *oct)
+{
+	int ret = 0;
+	u64 addr, size;
+
+	ret = octeon_mem_access_ok(oct);
+	if (ret) {
+		dev_err(&oct->pci_dev->dev, "Memory access not okay'\n");
+		return ret;
+	}
+
+	ret = octeon_named_block_find(oct, OCTEON_PCI_CONSOLE_BLOCK_NAME, &addr,
+				      &size);
+	if (ret) {
+		dev_err(&oct->pci_dev->dev, "Could not find console '%s'\n",
+			OCTEON_PCI_CONSOLE_BLOCK_NAME);
+		return ret;
+	}
+
+	/* num_consoles > 0, is an indication that the consoles
+	 * are accessible
+	 */
+	oct->num_consoles = octeon_read_device_mem32(oct,
+		addr + offsetof(struct octeon_pci_console_desc,
+			num_consoles));
+	oct->console_desc_addr = addr;
+
+	dev_dbg(&oct->pci_dev->dev, "Initialized consoles. %d available\n",
+		oct->num_consoles);
+
+	return ret;
+}
+
+int octeon_add_console(struct octeon_device *oct, u32 console_num)
+{
+	int ret = 0;
+	u32 delay;
+	u64 coreaddr;
+	struct delayed_work *work;
+	struct octeon_console *console;
+
+	if (console_num >= oct->num_consoles) {
+		dev_err(&oct->pci_dev->dev,
+			"trying to read from console number %d when only 0 to %d exist\n",
+			console_num, oct->num_consoles);
+	} else {
+		console = &oct->console[console_num];
+
+		console->waiting = 0;
+
+		coreaddr = oct->console_desc_addr + console_num * 8 +
+			offsetof(struct octeon_pci_console_desc,
+				 console_addr_array);
+		console->addr = octeon_read_device_mem64(oct, coreaddr);
+		coreaddr = console->addr + offsetof(struct octeon_pci_console,
+						    buf_size);
+		console->buffer_size = octeon_read_device_mem32(oct, coreaddr);
+		coreaddr = console->addr + offsetof(struct octeon_pci_console,
+						    input_base_addr);
+		console->input_base_addr =
+			octeon_read_device_mem64(oct, coreaddr);
+		coreaddr = console->addr + offsetof(struct octeon_pci_console,
+						    output_base_addr);
+		console->output_base_addr =
+			octeon_read_device_mem64(oct, coreaddr);
+		console->leftover[0] = '\0';
+
+		work = &oct->console_poll_work[console_num].work;
+
+		INIT_DELAYED_WORK(work, check_console);
+		oct->console_poll_work[console_num].ctxptr = (void *)oct;
+		oct->console_poll_work[console_num].ctxul = console_num;
+		delay = OCTEON_CONSOLE_POLL_INTERVAL_MS;
+		schedule_delayed_work(work, msecs_to_jiffies(delay));
+
+		if (octeon_console_debug_enabled(console_num)) {
+			ret = octeon_console_send_cmd(oct,
+						      "setenv pci_console_active 1",
+						      2000);
+		}
+
+		console->active = 1;
+	}
+
+	return ret;
+}
+
+/**
+ * Removes all consoles
+ *
+ * @param oct         octeon device
+ */
+void octeon_remove_consoles(struct octeon_device *oct)
+{
+	u32 i;
+	struct octeon_console *console;
+
+	for (i = 0; i < oct->num_consoles; i++) {
+		console = &oct->console[i];
+
+		if (!console->active)
+			continue;
+
+		cancel_delayed_work_sync(&oct->console_poll_work[i].
+						work);
+		console->addr = 0;
+		console->buffer_size = 0;
+		console->input_base_addr = 0;
+		console->output_base_addr = 0;
+	}
+
+	oct->num_consoles = 0;
+}
+
+static inline int octeon_console_free_bytes(u32 buffer_size,
+					    u32 wr_idx,
+					    u32 rd_idx)
+{
+	if (rd_idx >= buffer_size || wr_idx >= buffer_size)
+		return -1;
+
+	return ((buffer_size - 1) - (wr_idx - rd_idx)) % buffer_size;
+}
+
+static inline int octeon_console_avail_bytes(u32 buffer_size,
+					     u32 wr_idx,
+					     u32 rd_idx)
+{
+	if (rd_idx >= buffer_size || wr_idx >= buffer_size)
+		return -1;
+
+	return buffer_size - 1 -
+	       octeon_console_free_bytes(buffer_size, wr_idx, rd_idx);
+}
+
+int octeon_console_read(struct octeon_device *oct, u32 console_num,
+			char *buffer, u32 buf_size, u32 flags)
+{
+	int bytes_to_read;
+	u32 rd_idx, wr_idx;
+	struct octeon_console *console;
+
+	if (console_num >= oct->num_consoles) {
+		dev_err(&oct->pci_dev->dev, "Attempted to read from disabled console %d\n",
+			console_num);
+		return 0;
+	}
+
+	console = &oct->console[console_num];
+
+	/* Check to see if any data is available.
+	 * Maybe optimize this with 64-bit read.
+	 */
+	rd_idx = octeon_read_device_mem32(oct, console->addr +
+		offsetof(struct octeon_pci_console, output_read_index));
+	wr_idx = octeon_read_device_mem32(oct, console->addr +
+		offsetof(struct octeon_pci_console, output_write_index));
+
+	bytes_to_read = octeon_console_avail_bytes(console->buffer_size,
+						   wr_idx, rd_idx);
+	if (bytes_to_read <= 0)
+		return bytes_to_read;
+
+	bytes_to_read = MIN(bytes_to_read, (s32)buf_size);
+
+	/* Check to see if what we want to read is not contiguous, and limit
+	 * ourselves to the contiguous block
+	 */
+	if (rd_idx + bytes_to_read >= console->buffer_size)
+		bytes_to_read = console->buffer_size - rd_idx;
+
+	octeon_pci_read_core_mem(oct, console->output_base_addr + rd_idx,
+				 buffer, bytes_to_read);
+	octeon_write_device_mem32(oct, console->addr +
+				  offsetof(struct octeon_pci_console,
+					   output_read_index),
+				  (rd_idx + bytes_to_read) %
+				  console->buffer_size);
+
+	return bytes_to_read;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_device.c b/drivers/net/ethernet/cavium/liquidio/octeon_device.c
new file mode 100644
index 000000000000..f67641a2ff9e
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_device.c
@@ -0,0 +1,1304 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/crc32.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include <linux/vmalloc.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+#include "octeon_mem_ops.h"
+
+/** Default configuration
+ *  for CN66XX OCTEON Models.
+ */
+static struct octeon_config default_cn66xx_conf = {
+	.card_type                              = LIO_210SV,
+	.card_name                              = LIO_210SV_NAME,
+
+	/** IQ attributes */
+	.iq					= {
+		.max_iqs			= CN6XXX_CFG_IO_QUEUES,
+		.pending_list_size		=
+			(CN6XXX_MAX_IQ_DESCRIPTORS * CN6XXX_CFG_IO_QUEUES),
+		.instr_type			= OCTEON_64BYTE_INSTR,
+		.db_min				= CN6XXX_DB_MIN,
+		.db_timeout			= CN6XXX_DB_TIMEOUT,
+	}
+	,
+
+	/** OQ attributes */
+	.oq					= {
+		.max_oqs			= CN6XXX_CFG_IO_QUEUES,
+		.info_ptr			= OCTEON_OQ_INFOPTR_MODE,
+		.refill_threshold		= CN6XXX_OQ_REFIL_THRESHOLD,
+		.oq_intr_pkt			= CN6XXX_OQ_INTR_PKT,
+		.oq_intr_time			= CN6XXX_OQ_INTR_TIME,
+		.pkts_per_intr			= CN6XXX_OQ_PKTSPER_INTR,
+	}
+	,
+
+	.num_nic_ports				= DEFAULT_NUM_NIC_PORTS_66XX,
+	.num_def_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+	.num_def_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+	.def_rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+	/* For ethernet interface 0:  Port cfg Attributes */
+	.nic_if_cfg[0] = {
+		/* Max Txqs: Half for each of the two ports :max_iq/2 */
+		.max_txqs			= MAX_TXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_txqs */
+		.num_txqs			= DEF_TXQS_PER_INTF,
+
+		/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+		.max_rxqs			= MAX_RXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_rxqs */
+		.num_rxqs			= DEF_RXQS_PER_INTF,
+
+		/* Num of desc for rx rings */
+		.num_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+
+		/* SKB size, We need not change buf size even for Jumbo frames.
+		 * Octeon can send jumbo frames in 4 consecutive descriptors,
+		 */
+		.rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 0,
+	},
+
+	.nic_if_cfg[1] = {
+		/* Max Txqs: Half for each of the two ports :max_iq/2 */
+		.max_txqs			= MAX_TXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_txqs */
+		.num_txqs			= DEF_TXQS_PER_INTF,
+
+		/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+		.max_rxqs			= MAX_RXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_rxqs */
+		.num_rxqs			= DEF_RXQS_PER_INTF,
+
+		/* Num of desc for rx rings */
+		.num_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+
+		/* SKB size, We need not change buf size even for Jumbo frames.
+		 * Octeon can send jumbo frames in 4 consecutive descriptors,
+		 */
+		.rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 1,
+	},
+
+	/** Miscellaneous attributes */
+	.misc					= {
+		/* Host driver link query interval */
+		.oct_link_query_interval	= 100,
+
+		/* Octeon link query interval */
+		.host_link_query_interval	= 500,
+
+		.enable_sli_oq_bp		= 0,
+
+		/* Control queue group */
+		.ctrlq_grp			= 1,
+	}
+	,
+};
+
+/** Default configuration
+ *  for CN68XX OCTEON Model.
+ */
+
+static struct octeon_config default_cn68xx_conf = {
+	.card_type                              = LIO_410NV,
+	.card_name                              = LIO_410NV_NAME,
+
+	/** IQ attributes */
+	.iq					= {
+		.max_iqs			= CN6XXX_CFG_IO_QUEUES,
+		.pending_list_size		=
+			(CN6XXX_MAX_IQ_DESCRIPTORS * CN6XXX_CFG_IO_QUEUES),
+		.instr_type			= OCTEON_64BYTE_INSTR,
+		.db_min				= CN6XXX_DB_MIN,
+		.db_timeout			= CN6XXX_DB_TIMEOUT,
+	}
+	,
+
+	/** OQ attributes */
+	.oq					= {
+		.max_oqs			= CN6XXX_CFG_IO_QUEUES,
+		.info_ptr			= OCTEON_OQ_INFOPTR_MODE,
+		.refill_threshold		= CN6XXX_OQ_REFIL_THRESHOLD,
+		.oq_intr_pkt			= CN6XXX_OQ_INTR_PKT,
+		.oq_intr_time			= CN6XXX_OQ_INTR_TIME,
+		.pkts_per_intr			= CN6XXX_OQ_PKTSPER_INTR,
+	}
+	,
+
+	.num_nic_ports				= DEFAULT_NUM_NIC_PORTS_68XX,
+	.num_def_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+	.num_def_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+	.def_rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+	.nic_if_cfg[0] = {
+		/* Max Txqs: Half for each of the two ports :max_iq/2 */
+		.max_txqs			= MAX_TXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_txqs */
+		.num_txqs			= DEF_TXQS_PER_INTF,
+
+		/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+		.max_rxqs			= MAX_RXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_rxqs */
+		.num_rxqs			= DEF_RXQS_PER_INTF,
+
+		/* Num of desc for rx rings */
+		.num_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+
+		/* SKB size, We need not change buf size even for Jumbo frames.
+		 * Octeon can send jumbo frames in 4 consecutive descriptors,
+		 */
+		.rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 0,
+	},
+
+	.nic_if_cfg[1] = {
+		/* Max Txqs: Half for each of the two ports :max_iq/2 */
+		.max_txqs			= MAX_TXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_txqs */
+		.num_txqs			= DEF_TXQS_PER_INTF,
+
+		/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+		.max_rxqs			= MAX_RXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_rxqs */
+		.num_rxqs			= DEF_RXQS_PER_INTF,
+
+		/* Num of desc for rx rings */
+		.num_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+
+		/* SKB size, We need not change buf size even for Jumbo frames.
+		 * Octeon can send jumbo frames in 4 consecutive descriptors,
+		 */
+		.rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 1,
+	},
+
+	.nic_if_cfg[2] = {
+		/* Max Txqs: Half for each of the two ports :max_iq/2 */
+		.max_txqs			= MAX_TXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_txqs */
+		.num_txqs			= DEF_TXQS_PER_INTF,
+
+		/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+		.max_rxqs			= MAX_RXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_rxqs */
+		.num_rxqs			= DEF_RXQS_PER_INTF,
+
+		/* Num of desc for rx rings */
+		.num_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+
+		/* SKB size, We need not change buf size even for Jumbo frames.
+		 * Octeon can send jumbo frames in 4 consecutive descriptors,
+		 */
+		.rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 2,
+	},
+
+	.nic_if_cfg[3] = {
+		/* Max Txqs: Half for each of the two ports :max_iq/2 */
+		.max_txqs			= MAX_TXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_txqs */
+		.num_txqs			= DEF_TXQS_PER_INTF,
+
+		/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+		.max_rxqs			= MAX_RXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_rxqs */
+		.num_rxqs			= DEF_RXQS_PER_INTF,
+
+		/* Num of desc for rx rings */
+		.num_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+
+		/* SKB size, We need not change buf size even for Jumbo frames.
+		 * Octeon can send jumbo frames in 4 consecutive descriptors,
+		 */
+		.rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 3,
+	},
+
+	/** Miscellaneous attributes */
+	.misc					= {
+		/* Host driver link query interval */
+		.oct_link_query_interval	= 100,
+
+		/* Octeon link query interval */
+		.host_link_query_interval	= 500,
+
+		.enable_sli_oq_bp		= 0,
+
+		/* Control queue group */
+		.ctrlq_grp			= 1,
+	}
+	,
+};
+
+/** Default configuration
+ *  for CN68XX OCTEON Model.
+ */
+static struct octeon_config default_cn68xx_210nv_conf = {
+	.card_type                              = LIO_210NV,
+	.card_name                              = LIO_210NV_NAME,
+
+	/** IQ attributes */
+
+	.iq					= {
+		.max_iqs			= CN6XXX_CFG_IO_QUEUES,
+		.pending_list_size		=
+			(CN6XXX_MAX_IQ_DESCRIPTORS * CN6XXX_CFG_IO_QUEUES),
+		.instr_type			= OCTEON_64BYTE_INSTR,
+		.db_min				= CN6XXX_DB_MIN,
+		.db_timeout			= CN6XXX_DB_TIMEOUT,
+	}
+	,
+
+	/** OQ attributes */
+	.oq					= {
+		.max_oqs			= CN6XXX_CFG_IO_QUEUES,
+		.info_ptr			= OCTEON_OQ_INFOPTR_MODE,
+		.refill_threshold		= CN6XXX_OQ_REFIL_THRESHOLD,
+		.oq_intr_pkt			= CN6XXX_OQ_INTR_PKT,
+		.oq_intr_time			= CN6XXX_OQ_INTR_TIME,
+		.pkts_per_intr			= CN6XXX_OQ_PKTSPER_INTR,
+	}
+	,
+
+	.num_nic_ports			= DEFAULT_NUM_NIC_PORTS_68XX_210NV,
+	.num_def_rx_descs		= CN6XXX_MAX_OQ_DESCRIPTORS,
+	.num_def_tx_descs		= CN6XXX_MAX_IQ_DESCRIPTORS,
+	.def_rx_buf_size		= CN6XXX_OQ_BUF_SIZE,
+
+	.nic_if_cfg[0] = {
+		/* Max Txqs: Half for each of the two ports :max_iq/2 */
+		.max_txqs			= MAX_TXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_txqs */
+		.num_txqs			= DEF_TXQS_PER_INTF,
+
+		/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+		.max_rxqs			= MAX_RXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_rxqs */
+		.num_rxqs			= DEF_RXQS_PER_INTF,
+
+		/* Num of desc for rx rings */
+		.num_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+
+		/* SKB size, We need not change buf size even for Jumbo frames.
+		 * Octeon can send jumbo frames in 4 consecutive descriptors,
+		 */
+		.rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 0,
+	},
+
+	.nic_if_cfg[1] = {
+		/* Max Txqs: Half for each of the two ports :max_iq/2 */
+		.max_txqs			= MAX_TXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_txqs */
+		.num_txqs			= DEF_TXQS_PER_INTF,
+
+		/* Max Rxqs: Half for each of the two ports :max_oq/2  */
+		.max_rxqs			= MAX_RXQS_PER_INTF,
+
+		/* Actual configured value. Range could be: 1...max_rxqs */
+		.num_rxqs			= DEF_RXQS_PER_INTF,
+
+		/* Num of desc for rx rings */
+		.num_rx_descs			= CN6XXX_MAX_OQ_DESCRIPTORS,
+
+		/* Num of desc for tx rings */
+		.num_tx_descs			= CN6XXX_MAX_IQ_DESCRIPTORS,
+
+		/* SKB size, We need not change buf size even for Jumbo frames.
+		 * Octeon can send jumbo frames in 4 consecutive descriptors,
+		 */
+		.rx_buf_size			= CN6XXX_OQ_BUF_SIZE,
+
+		.base_queue			= BASE_QUEUE_NOT_REQUESTED,
+
+		.gmx_port_id			= 1,
+	},
+
+	/** Miscellaneous attributes */
+	.misc					= {
+		/* Host driver link query interval */
+		.oct_link_query_interval	= 100,
+
+		/* Octeon link query interval */
+		.host_link_query_interval	= 500,
+
+		.enable_sli_oq_bp		= 0,
+
+		/* Control queue group */
+		.ctrlq_grp			= 1,
+	}
+	,
+};
+
+enum {
+	OCTEON_CONFIG_TYPE_DEFAULT = 0,
+	NUM_OCTEON_CONFS,
+};
+
+static struct octeon_config_ptr {
+	u32 conf_type;
+} oct_conf_info[MAX_OCTEON_DEVICES] = {
+	{
+		OCTEON_CONFIG_TYPE_DEFAULT,
+	}, {
+		OCTEON_CONFIG_TYPE_DEFAULT,
+	}, {
+		OCTEON_CONFIG_TYPE_DEFAULT,
+	}, {
+		OCTEON_CONFIG_TYPE_DEFAULT,
+	},
+};
+
+static char oct_dev_state_str[OCT_DEV_STATES + 1][32] = {
+	"BEGIN",	"PCI-MAP-DONE",	      "DISPATCH-INIT-DONE",
+	"IQ-INIT-DONE", "SCBUFF-POOL-INIT-DONE", "RESPLIST-INIT-DONE",
+	"DROQ-INIT-DONE", "IO-QUEUES-INIT-DONE", "CONSOLE-INIT-DONE",
+	"HOST-READY",	"CORE-READY",	      "RUNNING",	   "IN-RESET",
+	"INVALID"
+};
+
+static char oct_dev_app_str[CVM_DRV_APP_COUNT + 1][32] = {
+	"BASE", "NIC", "UNKNOWN"};
+
+static struct octeon_device *octeon_device[MAX_OCTEON_DEVICES];
+static u32 octeon_device_count;
+
+static struct octeon_core_setup core_setup[MAX_OCTEON_DEVICES];
+
+static void oct_set_config_info(int oct_id, int conf_type)
+{
+	if (conf_type < 0 || conf_type > (NUM_OCTEON_CONFS - 1))
+		conf_type = OCTEON_CONFIG_TYPE_DEFAULT;
+	oct_conf_info[oct_id].conf_type = conf_type;
+}
+
+void octeon_init_device_list(int conf_type)
+{
+	int i;
+
+	memset(octeon_device, 0, (sizeof(void *) * MAX_OCTEON_DEVICES));
+	for (i = 0; i <  MAX_OCTEON_DEVICES; i++)
+		oct_set_config_info(i, conf_type);
+}
+
+static void *__retrieve_octeon_config_info(struct octeon_device *oct,
+					   u16 card_type)
+{
+	u32 oct_id = oct->octeon_id;
+	void *ret = NULL;
+
+	switch (oct_conf_info[oct_id].conf_type) {
+	case OCTEON_CONFIG_TYPE_DEFAULT:
+		if (oct->chip_id == OCTEON_CN66XX) {
+			ret = (void *)&default_cn66xx_conf;
+		} else if ((oct->chip_id == OCTEON_CN68XX) &&
+			   (card_type == LIO_210NV)) {
+			ret =  (void *)&default_cn68xx_210nv_conf;
+		} else if ((oct->chip_id == OCTEON_CN68XX) &&
+			   (card_type == LIO_410NV)) {
+			ret =  (void *)&default_cn68xx_conf;
+		}
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+static int __verify_octeon_config_info(struct octeon_device *oct, void *conf)
+{
+	switch (oct->chip_id) {
+	case OCTEON_CN66XX:
+	case OCTEON_CN68XX:
+		return lio_validate_cn6xxx_config_info(oct, conf);
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+void *oct_get_config_info(struct octeon_device *oct, u16 card_type)
+{
+	void *conf = NULL;
+
+	conf = __retrieve_octeon_config_info(oct, card_type);
+	if (!conf)
+		return NULL;
+
+	if (__verify_octeon_config_info(oct, conf)) {
+		dev_err(&oct->pci_dev->dev, "Configuration verification failed\n");
+		return NULL;
+	}
+
+	return conf;
+}
+
+char *lio_get_state_string(atomic_t *state_ptr)
+{
+	s32 istate = (s32)atomic_read(state_ptr);
+
+	if (istate > OCT_DEV_STATES || istate < 0)
+		return oct_dev_state_str[OCT_DEV_STATE_INVALID];
+	return oct_dev_state_str[istate];
+}
+
+static char *get_oct_app_string(u32 app_mode)
+{
+	if (app_mode <= CVM_DRV_APP_END)
+		return oct_dev_app_str[app_mode - CVM_DRV_APP_START];
+	return oct_dev_app_str[CVM_DRV_INVALID_APP - CVM_DRV_APP_START];
+}
+
+int octeon_download_firmware(struct octeon_device *oct, const u8 *data,
+			     size_t size)
+{
+	int ret = 0;
+	u8 *p;
+	u8 *buffer;
+	u32 crc32_result;
+	u64 load_addr;
+	u32 image_len;
+	struct octeon_firmware_file_header *h;
+	u32 i;
+
+	if (size < sizeof(struct octeon_firmware_file_header)) {
+		dev_err(&oct->pci_dev->dev, "Firmware file too small (%d < %d).\n",
+			(u32)size,
+			(u32)sizeof(struct octeon_firmware_file_header));
+		return -EINVAL;
+	}
+
+	h = (struct octeon_firmware_file_header *)data;
+
+	if (be32_to_cpu(h->magic) != LIO_NIC_MAGIC) {
+		dev_err(&oct->pci_dev->dev, "Unrecognized firmware file.\n");
+		return -EINVAL;
+	}
+
+	crc32_result =
+		crc32(~0, data,
+		      sizeof(struct octeon_firmware_file_header) -
+		      sizeof(u32)) ^ ~0U;
+	if (crc32_result != be32_to_cpu(h->crc32)) {
+		dev_err(&oct->pci_dev->dev, "Firmware CRC mismatch (0x%08x != 0x%08x).\n",
+			crc32_result, be32_to_cpu(h->crc32));
+		return -EINVAL;
+	}
+
+	if (memcmp(LIQUIDIO_VERSION, h->version, strlen(LIQUIDIO_VERSION))) {
+		dev_err(&oct->pci_dev->dev, "Unmatched firmware version. Expected %s, got %s.\n",
+			LIQUIDIO_VERSION, h->version);
+		return -EINVAL;
+	}
+
+	if (be32_to_cpu(h->num_images) > LIO_MAX_IMAGES) {
+		dev_err(&oct->pci_dev->dev, "Too many images in firmware file (%d).\n",
+			be32_to_cpu(h->num_images));
+		return -EINVAL;
+	}
+
+	dev_info(&oct->pci_dev->dev, "Firmware version: %s\n", h->version);
+	snprintf(oct->fw_info.liquidio_firmware_version, 32, "LIQUIDIO: %s",
+		 h->version);
+
+	buffer = kmalloc(size, GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	memcpy(buffer, data, size);
+
+	p = buffer + sizeof(struct octeon_firmware_file_header);
+
+	/* load all images */
+	for (i = 0; i < be32_to_cpu(h->num_images); i++) {
+		load_addr = be64_to_cpu(h->desc[i].addr);
+		image_len = be32_to_cpu(h->desc[i].len);
+
+		/* validate the image */
+		crc32_result = crc32(~0, p, image_len) ^ ~0U;
+		if (crc32_result != be32_to_cpu(h->desc[i].crc32)) {
+			dev_err(&oct->pci_dev->dev,
+				"Firmware CRC mismatch in image %d (0x%08x != 0x%08x).\n",
+				i, crc32_result,
+				be32_to_cpu(h->desc[i].crc32));
+			ret = -EINVAL;
+			goto done_downloading;
+		}
+
+		/* download the image */
+		octeon_pci_write_core_mem(oct, load_addr, p, image_len);
+
+		p += image_len;
+		dev_dbg(&oct->pci_dev->dev,
+			"Downloaded image %d (%d bytes) to address 0x%016llx\n",
+			i, image_len, load_addr);
+	}
+
+	/* Invoke the bootcmd */
+	ret = octeon_console_send_cmd(oct, h->bootcmd, 50);
+
+done_downloading:
+	kfree(buffer);
+
+	return ret;
+}
+
+void octeon_free_device_mem(struct octeon_device *oct)
+{
+	u32 i;
+
+	for (i = 0; i < MAX_OCTEON_OUTPUT_QUEUES; i++) {
+		/* could check  mask as well */
+		vfree(oct->droq[i]);
+	}
+
+	for (i = 0; i < MAX_OCTEON_INSTR_QUEUES; i++) {
+		/* could check mask as well */
+		vfree(oct->instr_queue[i]);
+	}
+
+	i = oct->octeon_id;
+	vfree(oct);
+
+	octeon_device[i] = NULL;
+	octeon_device_count--;
+}
+
+static struct octeon_device *octeon_allocate_device_mem(u32 pci_id,
+							u32 priv_size)
+{
+	struct octeon_device *oct;
+	u8 *buf = NULL;
+	u32 octdevsize = 0, configsize = 0, size;
+
+	switch (pci_id) {
+	case OCTEON_CN68XX:
+	case OCTEON_CN66XX:
+		configsize = sizeof(struct octeon_cn6xxx);
+		break;
+
+	default:
+		pr_err("%s: Unknown PCI Device: 0x%x\n",
+		       __func__,
+		       pci_id);
+		return NULL;
+	}
+
+	if (configsize & 0x7)
+		configsize += (8 - (configsize & 0x7));
+
+	octdevsize = sizeof(struct octeon_device);
+	if (octdevsize & 0x7)
+		octdevsize += (8 - (octdevsize & 0x7));
+
+	if (priv_size & 0x7)
+		priv_size += (8 - (priv_size & 0x7));
+
+	size = octdevsize + priv_size + configsize +
+		(sizeof(struct octeon_dispatch) * DISPATCH_LIST_SIZE);
+
+	buf = vmalloc(size);
+	if (!buf)
+		return NULL;
+
+	memset(buf, 0, size);
+
+	oct = (struct octeon_device *)buf;
+	oct->priv = (void *)(buf + octdevsize);
+	oct->chip = (void *)(buf + octdevsize + priv_size);
+	oct->dispatch.dlist = (struct octeon_dispatch *)
+		(buf + octdevsize + priv_size + configsize);
+
+	return oct;
+}
+
+struct octeon_device *octeon_allocate_device(u32 pci_id,
+					     u32 priv_size)
+{
+	u32 oct_idx = 0;
+	struct octeon_device *oct = NULL;
+
+	for (oct_idx = 0; oct_idx < MAX_OCTEON_DEVICES; oct_idx++)
+		if (!octeon_device[oct_idx])
+			break;
+
+	if (oct_idx == MAX_OCTEON_DEVICES)
+		return NULL;
+
+	oct = octeon_allocate_device_mem(pci_id, priv_size);
+	if (!oct)
+		return NULL;
+
+	spin_lock_init(&oct->pci_win_lock);
+	spin_lock_init(&oct->mem_access_lock);
+
+	octeon_device_count++;
+	octeon_device[oct_idx] = oct;
+
+	oct->octeon_id = oct_idx;
+	snprintf((oct->device_name), sizeof(oct->device_name),
+		 "LiquidIO%d", (oct->octeon_id));
+
+	return oct;
+}
+
+int octeon_setup_instr_queues(struct octeon_device *oct)
+{
+	u32 i, num_iqs = 0;
+	u32 num_descs = 0;
+
+	/* this causes queue 0 to be default queue */
+	if (OCTEON_CN6XXX(oct)) {
+		num_iqs = 1;
+		num_descs =
+			CFG_GET_NUM_DEF_TX_DESCS(CHIP_FIELD(oct, cn6xxx, conf));
+	}
+
+	oct->num_iqs = 0;
+
+	for (i = 0; i < num_iqs; i++) {
+		oct->instr_queue[i] =
+			vmalloc(sizeof(struct octeon_instr_queue));
+		if (!oct->instr_queue[i])
+			return 1;
+
+		memset(oct->instr_queue[i], 0,
+		       sizeof(struct octeon_instr_queue));
+
+		oct->instr_queue[i]->app_ctx = (void *)(size_t)i;
+		if (octeon_init_instr_queue(oct, i, num_descs))
+			return 1;
+
+		oct->num_iqs++;
+	}
+
+	return 0;
+}
+
+int octeon_setup_output_queues(struct octeon_device *oct)
+{
+	u32 i, num_oqs = 0;
+	u32 num_descs = 0;
+	u32 desc_size = 0;
+
+	/* this causes queue 0 to be default queue */
+	if (OCTEON_CN6XXX(oct)) {
+		/* CFG_GET_OQ_MAX_BASE_Q(CHIP_FIELD(oct, cn6xxx, conf)); */
+		num_oqs = 1;
+		num_descs =
+			CFG_GET_NUM_DEF_RX_DESCS(CHIP_FIELD(oct, cn6xxx, conf));
+		desc_size =
+			CFG_GET_DEF_RX_BUF_SIZE(CHIP_FIELD(oct, cn6xxx, conf));
+	}
+
+	oct->num_oqs = 0;
+
+	for (i = 0; i < num_oqs; i++) {
+		oct->droq[i] = vmalloc(sizeof(*oct->droq[i]));
+		if (!oct->droq[i])
+			return 1;
+
+		memset(oct->droq[i], 0, sizeof(struct octeon_droq));
+
+		if (octeon_init_droq(oct, i, num_descs, desc_size, NULL))
+			return 1;
+
+		oct->num_oqs++;
+	}
+
+	return 0;
+}
+
+void octeon_set_io_queues_off(struct octeon_device *oct)
+{
+	/* Disable the i/p and o/p queues for this Octeon. */
+
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_INSTR_ENB, 0);
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_OUT_ENB, 0);
+}
+
+void octeon_set_droq_pkt_op(struct octeon_device *oct,
+			    u32 q_no,
+			    u32 enable)
+{
+	u32 reg_val = 0;
+
+	/* Disable the i/p and o/p queues for this Octeon. */
+	reg_val = octeon_read_csr(oct, CN6XXX_SLI_PKT_OUT_ENB);
+
+	if (enable)
+		reg_val = reg_val | (1 << q_no);
+	else
+		reg_val = reg_val & (~(1 << q_no));
+
+	octeon_write_csr(oct, CN6XXX_SLI_PKT_OUT_ENB, reg_val);
+}
+
+int octeon_init_dispatch_list(struct octeon_device *oct)
+{
+	u32 i;
+
+	oct->dispatch.count = 0;
+
+	for (i = 0; i < DISPATCH_LIST_SIZE; i++) {
+		oct->dispatch.dlist[i].opcode = 0;
+		INIT_LIST_HEAD(&oct->dispatch.dlist[i].list);
+	}
+
+	for (i = 0; i <= REQTYPE_LAST; i++)
+		octeon_register_reqtype_free_fn(oct, i, NULL);
+
+	spin_lock_init(&oct->dispatch.lock);
+
+	return 0;
+}
+
+void octeon_delete_dispatch_list(struct octeon_device *oct)
+{
+	u32 i;
+	struct list_head freelist, *temp, *tmp2;
+
+	INIT_LIST_HEAD(&freelist);
+
+	spin_lock_bh(&oct->dispatch.lock);
+
+	for (i = 0; i < DISPATCH_LIST_SIZE; i++) {
+		struct list_head *dispatch;
+
+		dispatch = &oct->dispatch.dlist[i].list;
+		while (dispatch->next != dispatch) {
+			temp = dispatch->next;
+			list_del(temp);
+			list_add_tail(temp, &freelist);
+		}
+
+		oct->dispatch.dlist[i].opcode = 0;
+	}
+
+	oct->dispatch.count = 0;
+
+	spin_unlock_bh(&oct->dispatch.lock);
+
+	list_for_each_safe(temp, tmp2, &freelist) {
+		list_del(temp);
+		vfree(temp);
+	}
+}
+
+octeon_dispatch_fn_t
+octeon_get_dispatch(struct octeon_device *octeon_dev, u16 opcode,
+		    u16 subcode)
+{
+	u32 idx;
+	struct list_head *dispatch;
+	octeon_dispatch_fn_t fn = NULL;
+	u16 combined_opcode = OPCODE_SUBCODE(opcode, subcode);
+
+	idx = combined_opcode & OCTEON_OPCODE_MASK;
+
+	spin_lock_bh(&octeon_dev->dispatch.lock);
+
+	if (octeon_dev->dispatch.count == 0) {
+		spin_unlock_bh(&octeon_dev->dispatch.lock);
+		return NULL;
+	}
+
+	if (!(octeon_dev->dispatch.dlist[idx].opcode)) {
+		spin_unlock_bh(&octeon_dev->dispatch.lock);
+		return NULL;
+	}
+
+	if (octeon_dev->dispatch.dlist[idx].opcode == combined_opcode) {
+		fn = octeon_dev->dispatch.dlist[idx].dispatch_fn;
+	} else {
+		list_for_each(dispatch,
+			      &octeon_dev->dispatch.dlist[idx].list) {
+			if (((struct octeon_dispatch *)dispatch)->opcode ==
+			    combined_opcode) {
+				fn = ((struct octeon_dispatch *)
+				      dispatch)->dispatch_fn;
+				break;
+			}
+		}
+	}
+
+	spin_unlock_bh(&octeon_dev->dispatch.lock);
+	return fn;
+}
+
+/* octeon_register_dispatch_fn
+ * Parameters:
+ *   octeon_id - id of the octeon device.
+ *   opcode    - opcode for which driver should call the registered function
+ *   subcode   - subcode for which driver should call the registered function
+ *   fn        - The function to call when a packet with "opcode" arrives in
+ *		  octeon output queues.
+ *   fn_arg    - The argument to be passed when calling function "fn".
+ * Description:
+ *   Registers a function and its argument to be called when a packet
+ *   arrives in Octeon output queues with "opcode".
+ * Returns:
+ *   Success: 0
+ *   Failure: 1
+ * Locks:
+ *   No locks are held.
+ */
+int
+octeon_register_dispatch_fn(struct octeon_device *oct,
+			    u16 opcode,
+			    u16 subcode,
+			    octeon_dispatch_fn_t fn, void *fn_arg)
+{
+	u32 idx;
+	octeon_dispatch_fn_t pfn;
+	u16 combined_opcode = OPCODE_SUBCODE(opcode, subcode);
+
+	idx = combined_opcode & OCTEON_OPCODE_MASK;
+
+	spin_lock_bh(&oct->dispatch.lock);
+	/* Add dispatch function to first level of lookup table */
+	if (oct->dispatch.dlist[idx].opcode == 0) {
+		oct->dispatch.dlist[idx].opcode = combined_opcode;
+		oct->dispatch.dlist[idx].dispatch_fn = fn;
+		oct->dispatch.dlist[idx].arg = fn_arg;
+		oct->dispatch.count++;
+		spin_unlock_bh(&oct->dispatch.lock);
+		return 0;
+	}
+
+	spin_unlock_bh(&oct->dispatch.lock);
+
+	/* Check if there was a function already registered for this
+	 * opcode/subcode.
+	 */
+	pfn = octeon_get_dispatch(oct, opcode, subcode);
+	if (!pfn) {
+		struct octeon_dispatch *dispatch;
+
+		dev_dbg(&oct->pci_dev->dev,
+			"Adding opcode to dispatch list linked list\n");
+		dispatch = (struct octeon_dispatch *)
+			   vmalloc(sizeof(struct octeon_dispatch));
+		if (!dispatch) {
+			dev_err(&oct->pci_dev->dev,
+				"No memory to add dispatch function\n");
+			return 1;
+		}
+		dispatch->opcode = combined_opcode;
+		dispatch->dispatch_fn = fn;
+		dispatch->arg = fn_arg;
+
+		/* Add dispatch function to linked list of fn ptrs
+		 * at the hashed index.
+		 */
+		spin_lock_bh(&oct->dispatch.lock);
+		list_add(&dispatch->list, &oct->dispatch.dlist[idx].list);
+		oct->dispatch.count++;
+		spin_unlock_bh(&oct->dispatch.lock);
+
+	} else {
+		dev_err(&oct->pci_dev->dev,
+			"Found previously registered dispatch fn for opcode/subcode: %x/%x\n",
+			opcode, subcode);
+		return 1;
+	}
+
+	return 0;
+}
+
+/* octeon_unregister_dispatch_fn
+ * Parameters:
+ *   oct       - octeon device
+ *   opcode    - driver should unregister the function for this opcode
+ *   subcode   - driver should unregister the function for this subcode
+ * Description:
+ *   Unregister the function set for this opcode+subcode.
+ * Returns:
+ *   Success: 0
+ *   Failure: 1
+ * Locks:
+ *   No locks are held.
+ */
+int
+octeon_unregister_dispatch_fn(struct octeon_device *oct, u16 opcode,
+			      u16 subcode)
+{
+	int retval = 0;
+	u32 idx;
+	struct list_head *dispatch, *dfree = NULL, *tmp2;
+	u16 combined_opcode = OPCODE_SUBCODE(opcode, subcode);
+
+	idx = combined_opcode & OCTEON_OPCODE_MASK;
+
+	spin_lock_bh(&oct->dispatch.lock);
+
+	if (oct->dispatch.count == 0) {
+		spin_unlock_bh(&oct->dispatch.lock);
+		dev_err(&oct->pci_dev->dev,
+			"No dispatch functions registered for this device\n");
+		return 1;
+	}
+
+	if (oct->dispatch.dlist[idx].opcode == combined_opcode) {
+		dispatch = &oct->dispatch.dlist[idx].list;
+		if (dispatch->next != dispatch) {
+			dispatch = dispatch->next;
+			oct->dispatch.dlist[idx].opcode =
+				((struct octeon_dispatch *)dispatch)->opcode;
+			oct->dispatch.dlist[idx].dispatch_fn =
+				((struct octeon_dispatch *)
+				 dispatch)->dispatch_fn;
+			oct->dispatch.dlist[idx].arg =
+				((struct octeon_dispatch *)dispatch)->arg;
+			list_del(dispatch);
+			dfree = dispatch;
+		} else {
+			oct->dispatch.dlist[idx].opcode = 0;
+			oct->dispatch.dlist[idx].dispatch_fn = NULL;
+			oct->dispatch.dlist[idx].arg = NULL;
+		}
+	} else {
+		retval = 1;
+		list_for_each_safe(dispatch, tmp2,
+				   &(oct->dispatch.dlist[idx].
+				     list)) {
+			if (((struct octeon_dispatch *)dispatch)->opcode ==
+			    combined_opcode) {
+				list_del(dispatch);
+				dfree = dispatch;
+				retval = 0;
+			}
+		}
+	}
+
+	if (!retval)
+		oct->dispatch.count--;
+
+	spin_unlock_bh(&oct->dispatch.lock);
+	vfree(dfree);
+	return retval;
+}
+
+int octeon_core_drv_init(struct octeon_recv_info *recv_info, void *buf)
+{
+	u32 i;
+	char app_name[16];
+	struct octeon_device *oct = (struct octeon_device *)buf;
+	struct octeon_recv_pkt *recv_pkt = recv_info->recv_pkt;
+	struct octeon_core_setup *cs = NULL;
+	u32 num_nic_ports = 0;
+
+	if (OCTEON_CN6XXX(oct))
+		num_nic_ports =
+			CFG_GET_NUM_NIC_PORTS(CHIP_FIELD(oct, cn6xxx, conf));
+
+	if (atomic_read(&oct->status) >= OCT_DEV_RUNNING) {
+		dev_err(&oct->pci_dev->dev, "Received CORE OK when device state is 0x%x\n",
+			atomic_read(&oct->status));
+		goto core_drv_init_err;
+	}
+
+	strncpy(app_name,
+		get_oct_app_string(
+		(u32)recv_pkt->rh.r_core_drv_init.app_mode),
+		sizeof(app_name) - 1);
+	oct->app_mode = (u32)recv_pkt->rh.r_core_drv_init.app_mode;
+	if (recv_pkt->rh.r_core_drv_init.app_mode == CVM_DRV_NIC_APP) {
+		oct->fw_info.max_nic_ports =
+			(u32)recv_pkt->rh.r_core_drv_init.max_nic_ports;
+		oct->fw_info.num_gmx_ports =
+			(u32)recv_pkt->rh.r_core_drv_init.num_gmx_ports;
+	}
+
+	if (oct->fw_info.max_nic_ports < num_nic_ports) {
+		dev_err(&oct->pci_dev->dev,
+			"Config has more ports than firmware allows (%d > %d).\n",
+			num_nic_ports, oct->fw_info.max_nic_ports);
+		goto core_drv_init_err;
+	}
+	oct->fw_info.app_cap_flags = recv_pkt->rh.r_core_drv_init.app_cap_flags;
+	oct->fw_info.app_mode = (u32)recv_pkt->rh.r_core_drv_init.app_mode;
+
+	atomic_set(&oct->status, OCT_DEV_CORE_OK);
+
+	cs = &core_setup[oct->octeon_id];
+
+	if (recv_pkt->buffer_size[0] != sizeof(*cs)) {
+		dev_dbg(&oct->pci_dev->dev, "Core setup bytes expected %u found %d\n",
+			(u32)sizeof(*cs),
+			recv_pkt->buffer_size[0]);
+	}
+
+	memcpy(cs, get_rbd(recv_pkt->buffer_ptr[0]), sizeof(*cs));
+	strncpy(oct->boardinfo.name, cs->boardname, OCT_BOARD_NAME);
+	strncpy(oct->boardinfo.serial_number, cs->board_serial_number,
+		OCT_SERIAL_LEN);
+
+	octeon_swap_8B_data((u64 *)cs, (sizeof(*cs) >> 3));
+
+	oct->boardinfo.major = cs->board_rev_major;
+	oct->boardinfo.minor = cs->board_rev_minor;
+
+	dev_info(&oct->pci_dev->dev,
+		 "Running %s (%llu Hz)\n",
+		 app_name, CVM_CAST64(cs->corefreq));
+
+core_drv_init_err:
+	for (i = 0; i < recv_pkt->buffer_count; i++)
+		recv_buffer_free(recv_pkt->buffer_ptr[i]);
+	octeon_free_recv_info(recv_info);
+	return 0;
+}
+
+int octeon_get_tx_qsize(struct octeon_device *oct, u32 q_no)
+
+{
+	if (oct && (q_no < MAX_OCTEON_INSTR_QUEUES) &&
+	    (oct->io_qmask.iq & (1UL << q_no)))
+		return oct->instr_queue[q_no]->max_count;
+
+	return -1;
+}
+
+int octeon_get_rx_qsize(struct octeon_device *oct, u32 q_no)
+{
+	if (oct && (q_no < MAX_OCTEON_OUTPUT_QUEUES) &&
+	    (oct->io_qmask.oq & (1UL << q_no)))
+		return oct->droq[q_no]->max_count;
+	return -1;
+}
+
+/* Retruns the host firmware handshake OCTEON specific configuration */
+struct octeon_config *octeon_get_conf(struct octeon_device *oct)
+{
+	struct octeon_config *default_oct_conf = NULL;
+
+	/* check the OCTEON Device model & return the corresponding octeon
+	 * configuration
+	 */
+
+	if (OCTEON_CN6XXX(oct)) {
+		default_oct_conf =
+			(struct octeon_config *)(CHIP_FIELD(oct, cn6xxx, conf));
+	}
+
+	return default_oct_conf;
+}
+
+/* scratch register address is same in all the OCT-II and CN70XX models */
+#define CNXX_SLI_SCRATCH1   0x3C0
+
+/** Get the octeon device pointer.
+ *  @param octeon_id  - The id for which the octeon device pointer is required.
+ *  @return Success: Octeon device pointer.
+ *  @return Failure: NULL.
+ */
+struct octeon_device *lio_get_device(u32 octeon_id)
+{
+	if (octeon_id >= MAX_OCTEON_DEVICES)
+		return NULL;
+	else
+		return octeon_device[octeon_id];
+}
+
+u64 lio_pci_readq(struct octeon_device *oct, u64 addr)
+{
+	u64 val64;
+	unsigned long flags;
+	u32 val32, addrhi;
+
+	spin_lock_irqsave(&oct->pci_win_lock, flags);
+
+	/* The windowed read happens when the LSB of the addr is written.
+	 * So write MSB first
+	 */
+	addrhi = (addr >> 32);
+	if ((oct->chip_id == OCTEON_CN66XX) || (oct->chip_id == OCTEON_CN68XX))
+		addrhi |= 0x00060000;
+	writel(addrhi, oct->reg_list.pci_win_rd_addr_hi);
+
+	/* Read back to preserve ordering of writes */
+	val32 = readl(oct->reg_list.pci_win_rd_addr_hi);
+
+	writel(addr & 0xffffffff, oct->reg_list.pci_win_rd_addr_lo);
+	val32 = readl(oct->reg_list.pci_win_rd_addr_lo);
+
+	val64 = readq(oct->reg_list.pci_win_rd_data);
+
+	spin_unlock_irqrestore(&oct->pci_win_lock, flags);
+
+	return val64;
+}
+
+void lio_pci_writeq(struct octeon_device *oct,
+		    u64 val,
+		    u64 addr)
+{
+	u32 val32;
+	unsigned long flags;
+
+	spin_lock_irqsave(&oct->pci_win_lock, flags);
+
+	writeq(addr, oct->reg_list.pci_win_wr_addr);
+
+	/* The write happens when the LSB is written. So write MSB first. */
+	writel(val >> 32, oct->reg_list.pci_win_wr_data_hi);
+	/* Read the MSB to ensure ordering of writes. */
+	val32 = readl(oct->reg_list.pci_win_wr_data_hi);
+
+	writel(val & 0xffffffff, oct->reg_list.pci_win_wr_data_lo);
+
+	spin_unlock_irqrestore(&oct->pci_win_lock, flags);
+}
+
+int octeon_mem_access_ok(struct octeon_device *oct)
+{
+	u64 access_okay = 0;
+
+	/* Check to make sure a DDR interface is enabled */
+	u64 lmc0_reset_ctl = lio_pci_readq(oct, CN6XXX_LMC0_RESET_CTL);
+
+	access_okay = (lmc0_reset_ctl & CN6XXX_LMC0_RESET_CTL_DDR3RST_MASK);
+
+	return access_okay ? 0 : 1;
+}
+
+int octeon_wait_for_ddr_init(struct octeon_device *oct, u32 *timeout)
+{
+	int ret = 1;
+	u32 ms;
+
+	if (!timeout)
+		return ret;
+
+	while (*timeout == 0)
+		schedule_timeout_uninterruptible(HZ / 10);
+
+	for (ms = 0; (ret != 0) && ((*timeout == 0) || (ms <= *timeout));
+	     ms += HZ / 10) {
+		ret = octeon_mem_access_ok(oct);
+
+		/* wait 100 ms */
+		if (ret)
+			schedule_timeout_uninterruptible(HZ / 10);
+	}
+
+	return ret;
+}
+
+/** Get the octeon id assigned to the octeon device passed as argument.
+ *  This function is exported to other modules.
+ *  @param dev - octeon device pointer passed as a void *.
+ *  @return octeon device id
+ */
+int lio_get_device_id(void *dev)
+{
+	struct octeon_device *octeon_dev = (struct octeon_device *)dev;
+	u32 i;
+
+	for (i = 0; i < MAX_OCTEON_DEVICES; i++)
+		if (octeon_device[i] == octeon_dev)
+			return octeon_dev->octeon_id;
+	return -1;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_device.h b/drivers/net/ethernet/cavium/liquidio/octeon_device.h
new file mode 100644
index 000000000000..36e1f85df8c4
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_device.h
@@ -0,0 +1,649 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+
+/*! \file octeon_device.h
+ *  \brief Host Driver: This file defines the octeon device structure.
+ */
+
+#ifndef _OCTEON_DEVICE_H_
+#define  _OCTEON_DEVICE_H_
+
+/** PCI VendorId Device Id */
+#define  OCTEON_CN68XX_PCIID          0x91177d
+#define  OCTEON_CN66XX_PCIID          0x92177d
+
+/** Driver identifies chips by these Ids, created by clubbing together
+ *  DeviceId+RevisionId; Where Revision Id is not used to distinguish
+ *  between chips, a value of 0 is used for revision id.
+ */
+#define  OCTEON_CN68XX                0x0091
+#define  OCTEON_CN66XX                0x0092
+
+/** Endian-swap modes supported by Octeon. */
+enum octeon_pci_swap_mode {
+	OCTEON_PCI_PASSTHROUGH = 0,
+	OCTEON_PCI_64BIT_SWAP = 1,
+	OCTEON_PCI_32BIT_BYTE_SWAP = 2,
+	OCTEON_PCI_32BIT_LW_SWAP = 3
+};
+
+/*---------------   PCI BAR1 index registers -------------*/
+
+/* BAR1 Mask */
+#define    PCI_BAR1_ENABLE_CA            1
+#define    PCI_BAR1_ENDIAN_MODE          OCTEON_PCI_64BIT_SWAP
+#define    PCI_BAR1_ENTRY_VALID          1
+#define    PCI_BAR1_MASK                 ((PCI_BAR1_ENABLE_CA << 3)   \
+					    | (PCI_BAR1_ENDIAN_MODE << 1) \
+					    | PCI_BAR1_ENTRY_VALID)
+
+/** Octeon Device state.
+ *  Each octeon device goes through each of these states
+ *  as it is initialized.
+ */
+#define    OCT_DEV_BEGIN_STATE            0x0
+#define    OCT_DEV_PCI_MAP_DONE           0x1
+#define    OCT_DEV_DISPATCH_INIT_DONE     0x2
+#define    OCT_DEV_INSTR_QUEUE_INIT_DONE  0x3
+#define    OCT_DEV_SC_BUFF_POOL_INIT_DONE 0x4
+#define    OCT_DEV_RESP_LIST_INIT_DONE    0x5
+#define    OCT_DEV_DROQ_INIT_DONE         0x6
+#define    OCT_DEV_IO_QUEUES_DONE         0x7
+#define    OCT_DEV_CONSOLE_INIT_DONE      0x8
+#define    OCT_DEV_HOST_OK                0x9
+#define    OCT_DEV_CORE_OK                0xa
+#define    OCT_DEV_RUNNING                0xb
+#define    OCT_DEV_IN_RESET               0xc
+#define    OCT_DEV_STATE_INVALID          0xd
+
+#define    OCT_DEV_STATES                 OCT_DEV_STATE_INVALID
+
+/** Octeon Device interrupts
+  *  These interrupt bits are set in int_status filed of
+  *  octeon_device structure
+  */
+#define	   OCT_DEV_INTR_DMA0_FORCE	  0x01
+#define	   OCT_DEV_INTR_DMA1_FORCE	  0x02
+#define	   OCT_DEV_INTR_PKT_DATA	  0x04
+
+#define LIO_RESET_SECS (3)
+
+/*---------------------------DISPATCH LIST-------------------------------*/
+
+/** The dispatch list entry.
+ *  The driver keeps a record of functions registered for each
+ *  response header opcode in this structure. Since the opcode is
+ *  hashed to index into the driver's list, more than one opcode
+ *  can hash to the same entry, in which case the list field points
+ *  to a linked list with the other entries.
+ */
+struct octeon_dispatch {
+	/** List head for this entry */
+	struct list_head list;
+
+	/** The opcode for which the dispatch function & arg should be used */
+	u16 opcode;
+
+	/** The function to be called for a packet received by the driver */
+	octeon_dispatch_fn_t dispatch_fn;
+
+	/* The application specified argument to be passed to the above
+	 * function along with the received packet
+	 */
+	void *arg;
+};
+
+/** The dispatch list structure. */
+struct octeon_dispatch_list {
+	/** access to dispatch list must be atomic */
+	spinlock_t lock;
+
+	/** Count of dispatch functions currently registered */
+	u32 count;
+
+	/** The list of dispatch functions */
+	struct octeon_dispatch *dlist;
+};
+
+/*-----------------------  THE OCTEON DEVICE  ---------------------------*/
+
+#define OCT_MEM_REGIONS     3
+/** PCI address space mapping information.
+ *  Each of the 3 address spaces given by BAR0, BAR2 and BAR4 of
+ *  Octeon gets mapped to different physical address spaces in
+ *  the kernel.
+ */
+struct octeon_mmio {
+	/** PCI address to which the BAR is mapped. */
+	u64 start;
+
+	/** Length of this PCI address space. */
+	u32 len;
+
+	/** Length that has been mapped to phys. address space. */
+	u32 mapped_len;
+
+	/** The physical address to which the PCI address space is mapped. */
+	u8 __iomem *hw_addr;
+
+	/** Flag indicating the mapping was successful. */
+	u32 done;
+};
+
+#define   MAX_OCTEON_MAPS    32
+
+struct octeon_io_enable {
+	u32 iq;
+	u32 oq;
+	u32 iq64B;
+};
+
+struct octeon_reg_list {
+	u32 __iomem *pci_win_wr_addr_hi;
+	u32 __iomem *pci_win_wr_addr_lo;
+	u64 __iomem *pci_win_wr_addr;
+
+	u32 __iomem *pci_win_rd_addr_hi;
+	u32 __iomem *pci_win_rd_addr_lo;
+	u64 __iomem *pci_win_rd_addr;
+
+	u32 __iomem *pci_win_wr_data_hi;
+	u32 __iomem *pci_win_wr_data_lo;
+	u64 __iomem *pci_win_wr_data;
+
+	u32 __iomem *pci_win_rd_data_hi;
+	u32 __iomem *pci_win_rd_data_lo;
+	u64 __iomem *pci_win_rd_data;
+};
+
+#define OCTEON_CONSOLE_MAX_READ_BYTES 512
+struct octeon_console {
+	u32 active;
+	u32 waiting;
+	u64 addr;
+	u32 buffer_size;
+	u64 input_base_addr;
+	u64 output_base_addr;
+	char leftover[OCTEON_CONSOLE_MAX_READ_BYTES];
+};
+
+struct octeon_board_info {
+	char name[OCT_BOARD_NAME];
+	char serial_number[OCT_SERIAL_LEN];
+	u64 major;
+	u64 minor;
+};
+
+struct octeon_fn_list {
+	void (*setup_iq_regs)(struct octeon_device *, u32);
+	void (*setup_oq_regs)(struct octeon_device *, u32);
+
+	irqreturn_t (*process_interrupt_regs)(void *);
+	int (*soft_reset)(struct octeon_device *);
+	int (*setup_device_regs)(struct octeon_device *);
+	void (*reinit_regs)(struct octeon_device *);
+	void (*bar1_idx_setup)(struct octeon_device *, u64, u32, int);
+	void (*bar1_idx_write)(struct octeon_device *, u32, u32);
+	u32 (*bar1_idx_read)(struct octeon_device *, u32);
+	u32 (*update_iq_read_idx)(struct octeon_device *,
+				  struct octeon_instr_queue *);
+
+	void (*enable_oq_pkt_time_intr)(struct octeon_device *, u32);
+	void (*disable_oq_pkt_time_intr)(struct octeon_device *, u32);
+
+	void (*enable_interrupt)(void *);
+	void (*disable_interrupt)(void *);
+
+	void (*enable_io_queues)(struct octeon_device *);
+	void (*disable_io_queues)(struct octeon_device *);
+};
+
+/* Must be multiple of 8, changing breaks ABI */
+#define CVMX_BOOTMEM_NAME_LEN 128
+
+/* Structure for named memory blocks
+ * Number of descriptors
+ * available can be changed without affecting compatiblity,
+ * but name length changes require a bump in the bootmem
+ * descriptor version
+ * Note: This structure must be naturally 64 bit aligned, as a single
+ * memory image will be used by both 32 and 64 bit programs.
+ */
+struct cvmx_bootmem_named_block_desc {
+	/** Base address of named block */
+	u64 base_addr;
+
+	/** Size actually allocated for named block */
+	u64 size;
+
+	/** name of named block */
+	char name[CVMX_BOOTMEM_NAME_LEN];
+};
+
+struct oct_fw_info {
+	u32 max_nic_ports;      /** max nic ports for the device */
+	u32 num_gmx_ports;      /** num gmx ports */
+	u64 app_cap_flags;      /** firmware cap flags */
+
+	/** The core application is running in this mode.
+	 * See octeon-drv-opcodes.h for values.
+	 */
+	u32 app_mode;
+	char   liquidio_firmware_version[32];
+};
+
+/* wrappers around work structs */
+struct cavium_wk {
+	struct delayed_work work;
+	void *ctxptr;
+	size_t ctxul;
+};
+
+struct cavium_wq {
+	struct workqueue_struct *wq;
+	struct cavium_wk wk;
+};
+
+struct octdev_props {
+	/* Each interface in the Octeon device has a network
+	 * device pointer (used for OS specific calls).
+	 */
+	struct net_device *netdev;
+};
+
+/** The Octeon device.
+ *  Each Octeon device has this structure to represent all its
+ *  components.
+ */
+struct octeon_device {
+	/** Lock for PCI window configuration accesses */
+	spinlock_t pci_win_lock;
+
+	/** Lock for memory accesses */
+	spinlock_t mem_access_lock;
+
+	/** PCI device pointer */
+	struct pci_dev *pci_dev;
+
+	/** Chip specific information. */
+	void *chip;
+
+	/** Number of interfaces detected in this octeon device. */
+	u32 ifcount;
+
+	struct octdev_props props[MAX_OCTEON_LINKS];
+
+	/** Octeon Chip type. */
+	u16 chip_id;
+	u16 rev_id;
+
+	/** This device's id - set by the driver. */
+	u32 octeon_id;
+
+	/** This device's PCIe port used for traffic. */
+	u16 pcie_port;
+
+	u16 flags;
+#define LIO_FLAG_MSI_ENABLED                  (u32)(1 << 1)
+#define LIO_FLAG_MSIX_ENABLED                 (u32)(1 << 2)
+
+	/** The state of this device */
+	atomic_t status;
+
+	/** memory mapped io range */
+	struct octeon_mmio mmio[OCT_MEM_REGIONS];
+
+	struct octeon_reg_list reg_list;
+
+	struct octeon_fn_list fn_list;
+
+	struct octeon_board_info boardinfo;
+
+	u32 num_iqs;
+
+	/* The pool containing pre allocated buffers used for soft commands */
+	struct octeon_sc_buffer_pool	sc_buf_pool;
+
+	/** The input instruction queues */
+	struct octeon_instr_queue *instr_queue[MAX_OCTEON_INSTR_QUEUES];
+
+	/** The doubly-linked list of instruction response */
+	struct octeon_response_list response_list[MAX_RESPONSE_LISTS];
+
+	u32 num_oqs;
+
+	/** The DROQ output queues  */
+	struct octeon_droq *droq[MAX_OCTEON_OUTPUT_QUEUES];
+
+	struct octeon_io_enable io_qmask;
+
+	/** List of dispatch functions */
+	struct octeon_dispatch_list dispatch;
+
+	/* Interrupt Moderation */
+	struct oct_intrmod_cfg intrmod;
+
+	u32 int_status;
+
+	u64 droq_intr;
+
+	/** Physical location of the cvmx_bootmem_desc_t in octeon memory */
+	u64 bootmem_desc_addr;
+
+	/** Placeholder memory for named blocks.
+	 * Assumes single-threaded access
+	 */
+	struct cvmx_bootmem_named_block_desc bootmem_named_block_desc;
+
+	/** Address of consoles descriptor */
+	u64 console_desc_addr;
+
+	/** Number of consoles available. 0 means they are inaccessible */
+	u32 num_consoles;
+
+	/* Console caches */
+	struct octeon_console console[MAX_OCTEON_MAPS];
+
+	/* Coprocessor clock rate. */
+	u64 coproc_clock_rate;
+
+	/** The core application is running in this mode. See liquidio_common.h
+	 * for values.
+	 */
+	u32 app_mode;
+
+	struct oct_fw_info fw_info;
+
+	/** The name given to this device. */
+	char device_name[32];
+
+	/** Application Context */
+	void *app_ctx;
+
+	struct cavium_wq dma_comp_wq;
+
+	struct cavium_wq check_db_wq[MAX_OCTEON_INSTR_QUEUES];
+
+	struct cavium_wk nic_poll_work;
+
+	struct cavium_wk console_poll_work[MAX_OCTEON_MAPS];
+
+	void *priv;
+};
+
+#define  OCTEON_CN6XXX(oct)           ((oct->chip_id == OCTEON_CN66XX) || \
+				       (oct->chip_id == OCTEON_CN68XX))
+#define CHIP_FIELD(oct, TYPE, field)             \
+	(((struct octeon_ ## TYPE  *)(oct->chip))->field)
+
+struct oct_intrmod_cmd {
+	struct octeon_device *oct_dev;
+	struct octeon_soft_command *sc;
+	struct oct_intrmod_cfg *cfg;
+};
+
+/*------------------ Function Prototypes ----------------------*/
+
+/** Initialize device list memory */
+void octeon_init_device_list(int conf_type);
+
+/** Free memory for Input and Output queue structures for a octeon device */
+void octeon_free_device_mem(struct octeon_device *);
+
+/* Look up a free entry in the octeon_device table and allocate resources
+ * for the octeon_device structure for an octeon device. Called at init
+ * time.
+ */
+struct octeon_device *octeon_allocate_device(u32 pci_id,
+					     u32 priv_size);
+
+/**  Initialize the driver's dispatch list which is a mix of a hash table
+ *  and a linked list. This is done at driver load time.
+ *  @param octeon_dev - pointer to the octeon device structure.
+ *  @return 0 on success, else -ve error value
+ */
+int octeon_init_dispatch_list(struct octeon_device *octeon_dev);
+
+/**  Delete the driver's dispatch list and all registered entries.
+ * This is done at driver unload time.
+ *  @param octeon_dev - pointer to the octeon device structure.
+ */
+void octeon_delete_dispatch_list(struct octeon_device *octeon_dev);
+
+/** Initialize the core device fields with the info returned by the FW.
+ * @param recv_info - Receive info structure
+ * @param buf       - Receive buffer
+ */
+int octeon_core_drv_init(struct octeon_recv_info *recv_info, void *buf);
+
+/** Gets the dispatch function registered to receive packets with a
+ *  given opcode/subcode.
+ *  @param  octeon_dev  - the octeon device pointer.
+ *  @param  opcode      - the opcode for which the dispatch function
+ *                        is to checked.
+ *  @param  subcode     - the subcode for which the dispatch function
+ *                        is to checked.
+ *
+ *  @return Success: octeon_dispatch_fn_t (dispatch function pointer)
+ *  @return Failure: NULL
+ *
+ *  Looks up the dispatch list to get the dispatch function for a
+ *  given opcode.
+ */
+octeon_dispatch_fn_t
+octeon_get_dispatch(struct octeon_device *octeon_dev, u16 opcode,
+		    u16 subcode);
+
+/** Get the octeon device pointer.
+ *  @param octeon_id  - The id for which the octeon device pointer is required.
+ *  @return Success: Octeon device pointer.
+ *  @return Failure: NULL.
+ */
+struct octeon_device *lio_get_device(u32 octeon_id);
+
+/** Get the octeon id assigned to the octeon device passed as argument.
+ *  This function is exported to other modules.
+ *  @param dev - octeon device pointer passed as a void *.
+ *  @return octeon device id
+ */
+int lio_get_device_id(void *dev);
+
+static inline u16 OCTEON_MAJOR_REV(struct octeon_device *oct)
+{
+	u16 rev = (oct->rev_id & 0xC) >> 2;
+
+	return (rev == 0) ? 1 : rev;
+}
+
+static inline u16 OCTEON_MINOR_REV(struct octeon_device *oct)
+{
+	return oct->rev_id & 0x3;
+}
+
+/** Read windowed register.
+ *  @param  oct   -  pointer to the Octeon device.
+ *  @param  addr  -  Address of the register to read.
+ *
+ *  This routine is called to read from the indirectly accessed
+ *  Octeon registers that are visible through a PCI BAR0 mapped window
+ *  register.
+ *  @return  - 64 bit value read from the register.
+ */
+
+u64 lio_pci_readq(struct octeon_device *oct, u64 addr);
+
+/** Write windowed register.
+ *  @param  oct  -  pointer to the Octeon device.
+ *  @param  val  -  Value to write
+ *  @param  addr -  Address of the register to write
+ *
+ *  This routine is called to write to the indirectly accessed
+ *  Octeon registers that are visible through a PCI BAR0 mapped window
+ *  register.
+ *  @return   Nothing.
+ */
+void lio_pci_writeq(struct octeon_device *oct, u64 val, u64 addr);
+
+/* Routines for reading and writing CSRs */
+#define   octeon_write_csr(oct_dev, reg_off, value) \
+		writel(value, oct_dev->mmio[0].hw_addr + reg_off)
+
+#define   octeon_write_csr64(oct_dev, reg_off, val64) \
+		writeq(val64, oct_dev->mmio[0].hw_addr + reg_off)
+
+#define   octeon_read_csr(oct_dev, reg_off)         \
+		readl(oct_dev->mmio[0].hw_addr + reg_off)
+
+#define   octeon_read_csr64(oct_dev, reg_off)         \
+		readq(oct_dev->mmio[0].hw_addr + reg_off)
+
+/**
+ * Checks if memory access is okay
+ *
+ * @param oct which octeon to send to
+ * @return Zero on success, negative on failure.
+ */
+int octeon_mem_access_ok(struct octeon_device *oct);
+
+/**
+ * Waits for DDR initialization.
+ *
+ * @param oct which octeon to send to
+ * @param timeout_in_ms pointer to how long to wait until DDR is initialized
+ * in ms.
+ *                      If contents are 0, it waits until contents are non-zero
+ *                      before starting to check.
+ * @return Zero on success, negative on failure.
+ */
+int octeon_wait_for_ddr_init(struct octeon_device *oct,
+			     u32 *timeout_in_ms);
+
+/**
+ * Wait for u-boot to boot and be waiting for a command.
+ *
+ * @param wait_time_hundredths
+ *               Maximum time to wait
+ *
+ * @return Zero on success, negative on failure.
+ */
+int octeon_wait_for_bootloader(struct octeon_device *oct,
+			       u32 wait_time_hundredths);
+
+/**
+ * Initialize console access
+ *
+ * @param oct which octeon initialize
+ * @return Zero on success, negative on failure.
+ */
+int octeon_init_consoles(struct octeon_device *oct);
+
+/**
+ * Adds access to a console to the device.
+ *
+ * @param oct which octeon to add to
+ * @param console_num which console
+ * @return Zero on success, negative on failure.
+ */
+int octeon_add_console(struct octeon_device *oct, u32 console_num);
+
+/** write or read from a console */
+int octeon_console_write(struct octeon_device *oct, u32 console_num,
+			 char *buffer, u32 write_request_size, u32 flags);
+int octeon_console_write_avail(struct octeon_device *oct, u32 console_num);
+int octeon_console_read(struct octeon_device *oct, u32 console_num,
+			char *buffer, u32 buf_size, u32 flags);
+int octeon_console_read_avail(struct octeon_device *oct, u32 console_num);
+
+/** Removes all attached consoles. */
+void octeon_remove_consoles(struct octeon_device *oct);
+
+/**
+ * Send a string to u-boot on console 0 as a command.
+ *
+ * @param oct which octeon to send to
+ * @param cmd_str String to send
+ * @param wait_hundredths Time to wait for u-boot to accept the command.
+ *
+ * @return Zero on success, negative on failure.
+ */
+int octeon_console_send_cmd(struct octeon_device *oct, char *cmd_str,
+			    u32 wait_hundredths);
+
+/** Parses, validates, and downloads firmware, then boots associated cores.
+ *  @param oct which octeon to download firmware to
+ *  @param data  - The complete firmware file image
+ *  @param size  - The size of the data
+ *
+ *  @return 0 if success.
+ *         -EINVAL if file is incompatible or badly formatted.
+ *         -ENODEV if no handler was found for the application type or an
+ *         invalid octeon id was passed.
+ */
+int octeon_download_firmware(struct octeon_device *oct, const u8 *data,
+			     size_t size);
+
+char *lio_get_state_string(atomic_t *state_ptr);
+
+/** Sets up instruction queues for the device
+ *  @param oct which octeon to setup
+ *
+ *  @return 0 if success. 1 if fails
+ */
+int octeon_setup_instr_queues(struct octeon_device *oct);
+
+/** Sets up output queues for the device
+ *  @param oct which octeon to setup
+ *
+ *  @return 0 if success. 1 if fails
+ */
+int octeon_setup_output_queues(struct octeon_device *oct);
+
+int octeon_get_tx_qsize(struct octeon_device *oct, u32 q_no);
+
+int octeon_get_rx_qsize(struct octeon_device *oct, u32 q_no);
+
+/** Turns off the input and output queues for the device
+ *  @param oct which octeon to disable
+ */
+void octeon_set_io_queues_off(struct octeon_device *oct);
+
+/** Turns on or off the given output queue for the device
+ *  @param oct which octeon to change
+ *  @param q_no which queue
+ *  @param enable 1 to enable, 0 to disable
+ */
+void octeon_set_droq_pkt_op(struct octeon_device *oct, u32 q_no, u32 enable);
+
+/** Retrieve the config for the device
+ *  @param oct which octeon
+ *  @param card_type type of card
+ *
+ *  @returns pointer to configuration
+ */
+void *oct_get_config_info(struct octeon_device *oct, u16 card_type);
+
+/** Gets the octeon device configuration
+ *  @return - pointer to the octeon configuration struture
+ */
+struct octeon_config *octeon_get_conf(struct octeon_device *oct);
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_droq.c b/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
new file mode 100644
index 000000000000..4dba86eaa045
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
@@ -0,0 +1,987 @@
+/**********************************************************************
+* Author: Cavium, Inc.
+*
+* Contact: support@cavium.com
+*          Please include "LiquidIO" in the subject.
+*
+* Copyright (c) 2003-2015 Cavium, Inc.
+*
+* This file is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License, Version 2, as
+* published by the Free Software Foundation.
+*
+* This file is distributed in the hope that it will be useful, but
+* AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+* NONINFRINGEMENT.  See the GNU General Public License for more
+* details.
+*
+* This file may also be available under a different license from Cavium.
+* Contact Cavium, Inc. for more information
+**********************************************************************/
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include <linux/vmalloc.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+#include "octeon_mem_ops.h"
+
+/* #define CAVIUM_ONLY_PERF_MODE */
+
+#define     CVM_MIN(d1, d2)           (((d1) < (d2)) ? (d1) : (d2))
+#define     CVM_MAX(d1, d2)           (((d1) > (d2)) ? (d1) : (d2))
+
+struct niclist {
+	struct list_head list;
+	void *ptr;
+};
+
+struct __dispatch {
+	struct list_head list;
+	struct octeon_recv_info *rinfo;
+	octeon_dispatch_fn_t disp_fn;
+};
+
+/** Get the argument that the user set when registering dispatch
+ *  function for a given opcode/subcode.
+ *  @param  octeon_dev - the octeon device pointer.
+ *  @param  opcode     - the opcode for which the dispatch argument
+ *                       is to be checked.
+ *  @param  subcode    - the subcode for which the dispatch argument
+ *                       is to be checked.
+ *  @return  Success: void * (argument to the dispatch function)
+ *  @return  Failure: NULL
+ *
+ */
+static inline void *octeon_get_dispatch_arg(struct octeon_device *octeon_dev,
+					    u16 opcode, u16 subcode)
+{
+	int idx;
+	struct list_head *dispatch;
+	void *fn_arg = NULL;
+	u16 combined_opcode = OPCODE_SUBCODE(opcode, subcode);
+
+	idx = combined_opcode & OCTEON_OPCODE_MASK;
+
+	spin_lock_bh(&octeon_dev->dispatch.lock);
+
+	if (octeon_dev->dispatch.count == 0) {
+		spin_unlock_bh(&octeon_dev->dispatch.lock);
+		return NULL;
+	}
+
+	if (octeon_dev->dispatch.dlist[idx].opcode == combined_opcode) {
+		fn_arg = octeon_dev->dispatch.dlist[idx].arg;
+	} else {
+		list_for_each(dispatch,
+			      &octeon_dev->dispatch.dlist[idx].list) {
+			if (((struct octeon_dispatch *)dispatch)->opcode ==
+			    combined_opcode) {
+				fn_arg = ((struct octeon_dispatch *)
+					  dispatch)->arg;
+				break;
+			}
+		}
+	}
+
+	spin_unlock_bh(&octeon_dev->dispatch.lock);
+	return fn_arg;
+}
+
+u32 octeon_droq_check_hw_for_pkts(struct octeon_device *oct,
+				  struct octeon_droq *droq)
+{
+	u32 pkt_count = 0;
+
+	pkt_count = readl(droq->pkts_sent_reg);
+	if (pkt_count) {
+		atomic_add(pkt_count, &droq->pkts_pending);
+		writel(pkt_count, droq->pkts_sent_reg);
+	}
+
+	return pkt_count;
+}
+
+static void octeon_droq_compute_max_packet_bufs(struct octeon_droq *droq)
+{
+	u32 count = 0;
+
+	/* max_empty_descs is the max. no. of descs that can have no buffers.
+	 * If the empty desc count goes beyond this value, we cannot safely
+	 * read in a 64K packet sent by Octeon
+	 * (64K is max pkt size from Octeon)
+	 */
+	droq->max_empty_descs = 0;
+
+	do {
+		droq->max_empty_descs++;
+		count += droq->buffer_size;
+	} while (count < (64 * 1024));
+
+	droq->max_empty_descs = droq->max_count - droq->max_empty_descs;
+}
+
+static void octeon_droq_reset_indices(struct octeon_droq *droq)
+{
+	droq->read_idx = 0;
+	droq->write_idx = 0;
+	droq->refill_idx = 0;
+	droq->refill_count = 0;
+	atomic_set(&droq->pkts_pending, 0);
+}
+
+static void
+octeon_droq_destroy_ring_buffers(struct octeon_device *oct,
+				 struct octeon_droq *droq)
+{
+	u32 i;
+
+	for (i = 0; i < droq->max_count; i++) {
+		if (droq->recv_buf_list[i].buffer) {
+			if (droq->desc_ring) {
+				lio_unmap_ring_info(oct->pci_dev,
+						    (u64)droq->
+						    desc_ring[i].info_ptr,
+						    OCT_DROQ_INFO_SIZE);
+				lio_unmap_ring(oct->pci_dev,
+					       (u64)droq->desc_ring[i].
+					       buffer_ptr,
+					       droq->buffer_size);
+			}
+			recv_buffer_free(droq->recv_buf_list[i].buffer);
+			droq->recv_buf_list[i].buffer = NULL;
+		}
+	}
+
+	octeon_droq_reset_indices(droq);
+}
+
+static int
+octeon_droq_setup_ring_buffers(struct octeon_device *oct,
+			       struct octeon_droq *droq)
+{
+	u32 i;
+	void *buf;
+	struct octeon_droq_desc *desc_ring = droq->desc_ring;
+
+	for (i = 0; i < droq->max_count; i++) {
+		buf = recv_buffer_alloc(oct, droq->q_no, droq->buffer_size);
+
+		if (!buf) {
+			dev_err(&oct->pci_dev->dev, "%s buffer alloc failed\n",
+				__func__);
+			return -ENOMEM;
+		}
+
+		droq->recv_buf_list[i].buffer = buf;
+		droq->recv_buf_list[i].data = get_rbd(buf);
+
+		droq->info_list[i].length = 0;
+
+		/* map ring buffers into memory */
+		desc_ring[i].info_ptr = lio_map_ring_info(droq, i);
+		desc_ring[i].buffer_ptr =
+			lio_map_ring(oct->pci_dev,
+				     droq->recv_buf_list[i].buffer,
+				     droq->buffer_size);
+	}
+
+	octeon_droq_reset_indices(droq);
+
+	octeon_droq_compute_max_packet_bufs(droq);
+
+	return 0;
+}
+
+int octeon_delete_droq(struct octeon_device *oct, u32 q_no)
+{
+	struct octeon_droq *droq = oct->droq[q_no];
+
+	dev_dbg(&oct->pci_dev->dev, "%s[%d]\n", __func__, q_no);
+
+	octeon_droq_destroy_ring_buffers(oct, droq);
+	vfree(droq->recv_buf_list);
+
+	if (droq->info_base_addr)
+		cnnic_free_aligned_dma(oct->pci_dev, droq->info_list,
+				       droq->info_alloc_size,
+				       droq->info_base_addr,
+				       droq->info_list_dma);
+
+	if (droq->desc_ring)
+		lio_dma_free(oct, (droq->max_count * OCT_DROQ_DESC_SIZE),
+			     droq->desc_ring, droq->desc_ring_dma);
+
+	memset(droq, 0, OCT_DROQ_SIZE);
+
+	return 0;
+}
+
+int octeon_init_droq(struct octeon_device *oct,
+		     u32 q_no,
+		     u32 num_descs,
+		     u32 desc_size,
+		     void *app_ctx)
+{
+	struct octeon_droq *droq;
+	u32 desc_ring_size = 0, c_num_descs = 0, c_buf_size = 0;
+	u32 c_pkts_per_intr = 0, c_refill_threshold = 0;
+
+	dev_dbg(&oct->pci_dev->dev, "%s[%d]\n", __func__, q_no);
+
+	droq = oct->droq[q_no];
+	memset(droq, 0, OCT_DROQ_SIZE);
+
+	droq->oct_dev = oct;
+	droq->q_no = q_no;
+	if (app_ctx)
+		droq->app_ctx = app_ctx;
+	else
+		droq->app_ctx = (void *)(size_t)q_no;
+
+	c_num_descs = num_descs;
+	c_buf_size = desc_size;
+	if (OCTEON_CN6XXX(oct)) {
+		struct octeon_config *conf6x = CHIP_FIELD(oct, cn6xxx, conf);
+
+		c_pkts_per_intr = (u32)CFG_GET_OQ_PKTS_PER_INTR(conf6x);
+		c_refill_threshold = (u32)CFG_GET_OQ_REFILL_THRESHOLD(conf6x);
+	}
+
+	droq->max_count = c_num_descs;
+	droq->buffer_size = c_buf_size;
+
+	desc_ring_size = droq->max_count * OCT_DROQ_DESC_SIZE;
+	droq->desc_ring = lio_dma_alloc(oct, desc_ring_size,
+					(dma_addr_t *)&droq->desc_ring_dma);
+
+	if (!droq->desc_ring) {
+		dev_err(&oct->pci_dev->dev,
+			"Output queue %d ring alloc failed\n", q_no);
+		return 1;
+	}
+
+	dev_dbg(&oct->pci_dev->dev, "droq[%d]: desc_ring: virt: 0x%p, dma: %lx\n",
+		q_no, droq->desc_ring, droq->desc_ring_dma);
+	dev_dbg(&oct->pci_dev->dev, "droq[%d]: num_desc: %d\n", q_no,
+		droq->max_count);
+
+	droq->info_list =
+		cnnic_alloc_aligned_dma(oct->pci_dev,
+					(droq->max_count * OCT_DROQ_INFO_SIZE),
+					&droq->info_alloc_size,
+					&droq->info_base_addr,
+					&droq->info_list_dma);
+
+	if (!droq->info_list) {
+		dev_err(&oct->pci_dev->dev, "Cannot allocate memory for info list.\n");
+		lio_dma_free(oct, (droq->max_count * OCT_DROQ_DESC_SIZE),
+			     droq->desc_ring, droq->desc_ring_dma);
+		return 1;
+	}
+
+	droq->recv_buf_list = (struct octeon_recv_buffer *)
+			      vmalloc(droq->max_count *
+						OCT_DROQ_RECVBUF_SIZE);
+	if (!droq->recv_buf_list) {
+		dev_err(&oct->pci_dev->dev, "Output queue recv buf list alloc failed\n");
+		goto init_droq_fail;
+	}
+
+	if (octeon_droq_setup_ring_buffers(oct, droq))
+		goto init_droq_fail;
+
+	droq->pkts_per_intr = c_pkts_per_intr;
+	droq->refill_threshold = c_refill_threshold;
+
+	dev_dbg(&oct->pci_dev->dev, "DROQ INIT: max_empty_descs: %d\n",
+		droq->max_empty_descs);
+
+	spin_lock_init(&droq->lock);
+
+	INIT_LIST_HEAD(&droq->dispatch_list);
+
+	/* For 56xx Pass1, this function won't be called, so no checks. */
+	oct->fn_list.setup_oq_regs(oct, q_no);
+
+	oct->io_qmask.oq |= (1 << q_no);
+
+	return 0;
+
+init_droq_fail:
+	octeon_delete_droq(oct, q_no);
+	return 1;
+}
+
+/* octeon_create_recv_info
+ * Parameters:
+ *  octeon_dev - pointer to the octeon device structure
+ *  droq       - droq in which the packet arrived.
+ *  buf_cnt    - no. of buffers used by the packet.
+ *  idx        - index in the descriptor for the first buffer in the packet.
+ * Description:
+ *  Allocates a recv_info_t and copies the buffer addresses for packet data
+ *  into the recv_pkt space which starts at an 8B offset from recv_info_t.
+ *  Flags the descriptors for refill later. If available descriptors go
+ *  below the threshold to receive a 64K pkt, new buffers are first allocated
+ *  before the recv_pkt_t is created.
+ *  This routine will be called in interrupt context.
+ * Returns:
+ *  Success: Pointer to recv_info_t
+ *  Failure: NULL.
+ * Locks:
+ *  The droq->lock is held when this routine is called.
+ */
+static inline struct octeon_recv_info *octeon_create_recv_info(
+		struct octeon_device *octeon_dev,
+		struct octeon_droq *droq,
+		u32 buf_cnt,
+		u32 idx)
+{
+	struct octeon_droq_info *info;
+	struct octeon_recv_pkt *recv_pkt;
+	struct octeon_recv_info *recv_info;
+	u32 i, bytes_left;
+
+	info = &droq->info_list[idx];
+
+	recv_info = octeon_alloc_recv_info(sizeof(struct __dispatch));
+	if (!recv_info)
+		return NULL;
+
+	recv_pkt = recv_info->recv_pkt;
+	recv_pkt->rh = info->rh;
+	recv_pkt->length = (u32)info->length;
+	recv_pkt->buffer_count = (u16)buf_cnt;
+	recv_pkt->octeon_id = (u16)octeon_dev->octeon_id;
+
+	i = 0;
+	bytes_left = (u32)info->length;
+
+	while (buf_cnt) {
+		lio_unmap_ring(octeon_dev->pci_dev,
+			       (u64)droq->desc_ring[idx].buffer_ptr,
+			       droq->buffer_size);
+
+		recv_pkt->buffer_size[i] =
+			(bytes_left >=
+			 droq->buffer_size) ? droq->buffer_size : bytes_left;
+
+		recv_pkt->buffer_ptr[i] = droq->recv_buf_list[idx].buffer;
+		droq->recv_buf_list[idx].buffer = NULL;
+
+		INCR_INDEX_BY1(idx, droq->max_count);
+		bytes_left -= droq->buffer_size;
+		i++;
+		buf_cnt--;
+	}
+
+	return recv_info;
+}
+
+/* If we were not able to refill all buffers, try to move around
+ * the buffers that were not dispatched.
+ */
+static inline u32
+octeon_droq_refill_pullup_descs(struct octeon_droq *droq,
+				struct octeon_droq_desc *desc_ring)
+{
+	u32 desc_refilled = 0;
+
+	u32 refill_index = droq->refill_idx;
+
+	while (refill_index != droq->read_idx) {
+		if (droq->recv_buf_list[refill_index].buffer) {
+			droq->recv_buf_list[droq->refill_idx].buffer =
+				droq->recv_buf_list[refill_index].buffer;
+			droq->recv_buf_list[droq->refill_idx].data =
+				droq->recv_buf_list[refill_index].data;
+			desc_ring[droq->refill_idx].buffer_ptr =
+				desc_ring[refill_index].buffer_ptr;
+			droq->recv_buf_list[refill_index].buffer = NULL;
+			desc_ring[refill_index].buffer_ptr = 0;
+			do {
+				INCR_INDEX_BY1(droq->refill_idx,
+					       droq->max_count);
+				desc_refilled++;
+				droq->refill_count--;
+			} while (droq->recv_buf_list[droq->refill_idx].
+				 buffer);
+		}
+		INCR_INDEX_BY1(refill_index, droq->max_count);
+	}                       /* while */
+	return desc_refilled;
+}
+
+/* octeon_droq_refill
+ * Parameters:
+ *  droq       - droq in which descriptors require new buffers.
+ * Description:
+ *  Called during normal DROQ processing in interrupt mode or by the poll
+ *  thread to refill the descriptors from which buffers were dispatched
+ *  to upper layers. Attempts to allocate new buffers. If that fails, moves
+ *  up buffers (that were not dispatched) to form a contiguous ring.
+ * Returns:
+ *  No of descriptors refilled.
+ * Locks:
+ *  This routine is called with droq->lock held.
+ */
+static u32
+octeon_droq_refill(struct octeon_device *octeon_dev, struct octeon_droq *droq)
+{
+	struct octeon_droq_desc *desc_ring;
+	void *buf = NULL;
+	u8 *data;
+	u32 desc_refilled = 0;
+
+	desc_ring = droq->desc_ring;
+
+	while (droq->refill_count && (desc_refilled < droq->max_count)) {
+		/* If a valid buffer exists (happens if there is no dispatch),
+		 * reuse
+		 * the buffer, else allocate.
+		 */
+		if (!droq->recv_buf_list[droq->refill_idx].buffer) {
+			buf = recv_buffer_alloc(octeon_dev, droq->q_no,
+						droq->buffer_size);
+			/* If a buffer could not be allocated, no point in
+			 * continuing
+			 */
+			if (!buf)
+				break;
+			droq->recv_buf_list[droq->refill_idx].buffer =
+				buf;
+			data = get_rbd(buf);
+		} else {
+			data = get_rbd(droq->recv_buf_list
+				       [droq->refill_idx].buffer);
+		}
+
+		droq->recv_buf_list[droq->refill_idx].data = data;
+
+		desc_ring[droq->refill_idx].buffer_ptr =
+			lio_map_ring(octeon_dev->pci_dev,
+				     droq->recv_buf_list[droq->
+				     refill_idx].buffer,
+				     droq->buffer_size);
+
+		/* Reset any previous values in the length field. */
+		droq->info_list[droq->refill_idx].length = 0;
+
+		INCR_INDEX_BY1(droq->refill_idx, droq->max_count);
+		desc_refilled++;
+		droq->refill_count--;
+	}
+
+	if (droq->refill_count)
+		desc_refilled +=
+			octeon_droq_refill_pullup_descs(droq, desc_ring);
+
+	/* if droq->refill_count
+	 * The refill count would not change in pass two. We only moved buffers
+	 * to close the gap in the ring, but we would still have the same no. of
+	 * buffers to refill.
+	 */
+	return desc_refilled;
+}
+
+static inline u32
+octeon_droq_get_bufcount(u32 buf_size, u32 total_len)
+{
+	u32 buf_cnt = 0;
+
+	while (total_len > (buf_size * buf_cnt))
+		buf_cnt++;
+	return buf_cnt;
+}
+
+static int
+octeon_droq_dispatch_pkt(struct octeon_device *oct,
+			 struct octeon_droq *droq,
+			 union octeon_rh *rh,
+			 struct octeon_droq_info *info)
+{
+	u32 cnt;
+	octeon_dispatch_fn_t disp_fn;
+	struct octeon_recv_info *rinfo;
+
+	cnt = octeon_droq_get_bufcount(droq->buffer_size, (u32)info->length);
+
+	disp_fn = octeon_get_dispatch(oct, (u16)rh->r.opcode,
+				      (u16)rh->r.subcode);
+	if (disp_fn) {
+		rinfo = octeon_create_recv_info(oct, droq, cnt, droq->read_idx);
+		if (rinfo) {
+			struct __dispatch *rdisp = rinfo->rsvd;
+
+			rdisp->rinfo = rinfo;
+			rdisp->disp_fn = disp_fn;
+			rinfo->recv_pkt->rh = *rh;
+			list_add_tail(&rdisp->list,
+				      &droq->dispatch_list);
+		} else {
+			droq->stats.dropped_nomem++;
+		}
+	} else {
+		dev_err(&oct->pci_dev->dev, "DROQ: No dispatch function\n");
+		droq->stats.dropped_nodispatch++;
+	}                       /* else (dispatch_fn ... */
+
+	return cnt;
+}
+
+static inline void octeon_droq_drop_packets(struct octeon_device *oct,
+					    struct octeon_droq *droq,
+					    u32 cnt)
+{
+	u32 i = 0, buf_cnt;
+	struct octeon_droq_info *info;
+
+	for (i = 0; i < cnt; i++) {
+		info = &droq->info_list[droq->read_idx];
+		octeon_swap_8B_data((u64 *)info, 2);
+
+		if (info->length) {
+			info->length -= OCT_RH_SIZE;
+			droq->stats.bytes_received += info->length;
+			buf_cnt = octeon_droq_get_bufcount(droq->buffer_size,
+							   (u32)info->length);
+		} else {
+			dev_err(&oct->pci_dev->dev, "DROQ: In drop: pkt with len 0\n");
+			buf_cnt = 1;
+		}
+
+		INCR_INDEX(droq->read_idx, buf_cnt, droq->max_count);
+		droq->refill_count += buf_cnt;
+	}
+}
+
+static u32
+octeon_droq_fast_process_packets(struct octeon_device *oct,
+				 struct octeon_droq *droq,
+				 u32 pkts_to_process)
+{
+	struct octeon_droq_info *info;
+	union octeon_rh *rh;
+	u32 pkt, total_len = 0, pkt_count;
+
+	pkt_count = pkts_to_process;
+
+	for (pkt = 0; pkt < pkt_count; pkt++) {
+		u32 pkt_len = 0;
+		struct sk_buff *nicbuf = NULL;
+
+		info = &droq->info_list[droq->read_idx];
+		octeon_swap_8B_data((u64 *)info, 2);
+
+		if (!info->length) {
+			dev_err(&oct->pci_dev->dev,
+				"DROQ[%d] idx: %d len:0, pkt_cnt: %d\n",
+				droq->q_no, droq->read_idx, pkt_count);
+			print_hex_dump_bytes("", DUMP_PREFIX_ADDRESS,
+					     (u8 *)info,
+					     OCT_DROQ_INFO_SIZE);
+			break;
+		}
+
+		/* Len of resp hdr in included in the received data len. */
+		info->length -= OCT_RH_SIZE;
+		rh = &info->rh;
+
+		total_len += (u32)info->length;
+
+		if (OPCODE_SLOW_PATH(rh)) {
+			u32 buf_cnt;
+
+			buf_cnt = octeon_droq_dispatch_pkt(oct, droq, rh, info);
+			INCR_INDEX(droq->read_idx, buf_cnt, droq->max_count);
+			droq->refill_count += buf_cnt;
+		} else {
+			if (info->length <= droq->buffer_size) {
+				lio_unmap_ring(oct->pci_dev,
+					       (u64)droq->desc_ring[
+					       droq->read_idx].buffer_ptr,
+					       droq->buffer_size);
+				pkt_len = (u32)info->length;
+				nicbuf = droq->recv_buf_list[
+					droq->read_idx].buffer;
+				droq->recv_buf_list[droq->read_idx].buffer =
+					NULL;
+				INCR_INDEX_BY1(droq->read_idx, droq->max_count);
+				skb_put(nicbuf, pkt_len);
+				droq->refill_count++;
+			} else {
+				nicbuf = octeon_fast_packet_alloc(oct, droq,
+								  droq->q_no,
+								  (u32)
+								  info->length);
+				pkt_len = 0;
+				/* nicbuf allocation can fail. We'll handle it
+				 * inside the loop.
+				 */
+				while (pkt_len < info->length) {
+					int cpy_len;
+
+					cpy_len = ((pkt_len +
+						droq->buffer_size) >
+						info->length) ?
+						((u32)info->length - pkt_len) :
+						droq->buffer_size;
+
+					if (nicbuf) {
+						lio_unmap_ring(oct->pci_dev,
+							       (u64)
+							       droq->desc_ring
+							       [droq->read_idx].
+							       buffer_ptr,
+							       droq->
+							       buffer_size);
+						octeon_fast_packet_next(droq,
+									nicbuf,
+									cpy_len,
+									droq->
+									read_idx
+									);
+					}
+
+					pkt_len += cpy_len;
+					INCR_INDEX_BY1(droq->read_idx,
+						       droq->max_count);
+					droq->refill_count++;
+				}
+			}
+
+			if (nicbuf) {
+				if (droq->ops.fptr)
+					droq->ops.fptr(oct->octeon_id,
+					nicbuf, pkt_len,
+					rh, &droq->napi);
+				else
+					recv_buffer_free(nicbuf);
+			}
+		}
+
+		if (droq->refill_count >= droq->refill_threshold) {
+			int desc_refilled = octeon_droq_refill(oct, droq);
+
+			/* Flush the droq descriptor data to memory to be sure
+			* that when we update the credits the data in memory
+			* is accurate.
+			*/
+			wmb();
+			writel((desc_refilled), droq->pkts_credit_reg);
+			/* make sure mmio write completes */
+			mmiowb();
+		}
+
+	}                       /* for ( each packet )... */
+
+	/* Increment refill_count by the number of buffers processed. */
+	droq->stats.pkts_received += pkt;
+	droq->stats.bytes_received += total_len;
+
+	if ((droq->ops.drop_on_max) && (pkts_to_process - pkt)) {
+		octeon_droq_drop_packets(oct, droq, (pkts_to_process - pkt));
+
+		droq->stats.dropped_toomany += (pkts_to_process - pkt);
+		return pkts_to_process;
+	}
+
+	return pkt;
+}
+
+int
+octeon_droq_process_packets(struct octeon_device *oct,
+			    struct octeon_droq *droq,
+			    u32 budget)
+{
+	u32 pkt_count = 0, pkts_processed = 0;
+	struct list_head *tmp, *tmp2;
+
+	pkt_count = atomic_read(&droq->pkts_pending);
+	if (!pkt_count)
+		return 0;
+
+	if (pkt_count > budget)
+		pkt_count = budget;
+
+	/* Grab the lock */
+	spin_lock(&droq->lock);
+
+	pkts_processed = octeon_droq_fast_process_packets(oct, droq, pkt_count);
+
+	atomic_sub(pkts_processed, &droq->pkts_pending);
+
+	/* Release the spin lock */
+	spin_unlock(&droq->lock);
+
+	list_for_each_safe(tmp, tmp2, &droq->dispatch_list) {
+		struct __dispatch *rdisp = (struct __dispatch *)tmp;
+
+		list_del(tmp);
+		rdisp->disp_fn(rdisp->rinfo,
+			       octeon_get_dispatch_arg
+			       (oct,
+				(u16)rdisp->rinfo->recv_pkt->rh.r.opcode,
+				(u16)rdisp->rinfo->recv_pkt->rh.r.subcode));
+	}
+
+	/* If there are packets pending. schedule tasklet again */
+	if (atomic_read(&droq->pkts_pending))
+		return 1;
+
+	return 0;
+}
+
+/**
+ * Utility function to poll for packets. check_hw_for_packets must be
+ * called before calling this routine.
+ */
+
+static int
+octeon_droq_process_poll_pkts(struct octeon_device *oct,
+			      struct octeon_droq *droq, u32 budget)
+{
+	struct list_head *tmp, *tmp2;
+	u32 pkts_available = 0, pkts_processed = 0;
+	u32 total_pkts_processed = 0;
+
+	if (budget > droq->max_count)
+		budget = droq->max_count;
+
+	spin_lock(&droq->lock);
+
+	while (total_pkts_processed < budget) {
+		pkts_available =
+			CVM_MIN((budget - total_pkts_processed),
+				(u32)(atomic_read(&droq->pkts_pending)));
+
+		if (pkts_available == 0)
+			break;
+
+		pkts_processed =
+			octeon_droq_fast_process_packets(oct, droq,
+							 pkts_available);
+
+		atomic_sub(pkts_processed, &droq->pkts_pending);
+
+		total_pkts_processed += pkts_processed;
+
+		octeon_droq_check_hw_for_pkts(oct, droq);
+	}
+
+	spin_unlock(&droq->lock);
+
+	list_for_each_safe(tmp, tmp2, &droq->dispatch_list) {
+		struct __dispatch *rdisp = (struct __dispatch *)tmp;
+
+		list_del(tmp);
+		rdisp->disp_fn(rdisp->rinfo,
+			       octeon_get_dispatch_arg
+			       (oct,
+				(u16)rdisp->rinfo->recv_pkt->rh.r.opcode,
+				(u16)rdisp->rinfo->recv_pkt->rh.r.subcode));
+	}
+
+	return total_pkts_processed;
+}
+
+int
+octeon_process_droq_poll_cmd(struct octeon_device *oct, u32 q_no, int cmd,
+			     u32 arg)
+{
+	struct octeon_droq *droq;
+	struct octeon_config *oct_cfg = NULL;
+
+	oct_cfg = octeon_get_conf(oct);
+
+	if (!oct_cfg)
+		return -EINVAL;
+
+	if (q_no >= CFG_GET_OQ_MAX_Q(oct_cfg)) {
+		dev_err(&oct->pci_dev->dev, "%s: droq id (%d) exceeds MAX (%d)\n",
+			__func__, q_no, (oct->num_oqs - 1));
+		return -EINVAL;
+	}
+
+	droq = oct->droq[q_no];
+
+	if (cmd == POLL_EVENT_PROCESS_PKTS)
+		return octeon_droq_process_poll_pkts(oct, droq, arg);
+
+	if (cmd == POLL_EVENT_PENDING_PKTS) {
+		u32 pkt_cnt = atomic_read(&droq->pkts_pending);
+
+		return  octeon_droq_process_packets(oct, droq, pkt_cnt);
+	}
+
+	if (cmd == POLL_EVENT_ENABLE_INTR) {
+		u32 value;
+		unsigned long flags;
+
+		/* Enable Pkt Interrupt */
+		switch (oct->chip_id) {
+		case OCTEON_CN66XX:
+		case OCTEON_CN68XX: {
+			struct octeon_cn6xxx *cn6xxx =
+				(struct octeon_cn6xxx *)oct->chip;
+			spin_lock_irqsave
+				(&cn6xxx->lock_for_droq_int_enb_reg, flags);
+			value =
+				octeon_read_csr(oct,
+						CN6XXX_SLI_PKT_TIME_INT_ENB);
+			value |= (1 << q_no);
+			octeon_write_csr(oct,
+					 CN6XXX_SLI_PKT_TIME_INT_ENB,
+					 value);
+			value =
+				octeon_read_csr(oct,
+						CN6XXX_SLI_PKT_CNT_INT_ENB);
+			value |= (1 << q_no);
+			octeon_write_csr(oct,
+					 CN6XXX_SLI_PKT_CNT_INT_ENB,
+					 value);
+
+			/* don't bother flushing the enables */
+
+			spin_unlock_irqrestore
+				(&cn6xxx->lock_for_droq_int_enb_reg, flags);
+			return 0;
+		}
+		break;
+		}
+
+		return 0;
+	}
+
+	dev_err(&oct->pci_dev->dev, "%s Unknown command: %d\n", __func__, cmd);
+	return -EINVAL;
+}
+
+int octeon_register_droq_ops(struct octeon_device *oct, u32 q_no,
+			     struct octeon_droq_ops *ops)
+{
+	struct octeon_droq *droq;
+	unsigned long flags;
+	struct octeon_config *oct_cfg = NULL;
+
+	oct_cfg = octeon_get_conf(oct);
+
+	if (!oct_cfg)
+		return -EINVAL;
+
+	if (!(ops)) {
+		dev_err(&oct->pci_dev->dev, "%s: droq_ops pointer is NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (q_no >= CFG_GET_OQ_MAX_Q(oct_cfg)) {
+		dev_err(&oct->pci_dev->dev, "%s: droq id (%d) exceeds MAX (%d)\n",
+			__func__, q_no, (oct->num_oqs - 1));
+		return -EINVAL;
+	}
+
+	droq = oct->droq[q_no];
+
+	spin_lock_irqsave(&droq->lock, flags);
+
+	memcpy(&droq->ops, ops, sizeof(struct octeon_droq_ops));
+
+	spin_unlock_irqrestore(&droq->lock, flags);
+
+	return 0;
+}
+
+int octeon_unregister_droq_ops(struct octeon_device *oct, u32 q_no)
+{
+	unsigned long flags;
+	struct octeon_droq *droq;
+	struct octeon_config *oct_cfg = NULL;
+
+	oct_cfg = octeon_get_conf(oct);
+
+	if (!oct_cfg)
+		return -EINVAL;
+
+	if (q_no >= CFG_GET_OQ_MAX_Q(oct_cfg)) {
+		dev_err(&oct->pci_dev->dev, "%s: droq id (%d) exceeds MAX (%d)\n",
+			__func__, q_no, oct->num_oqs - 1);
+		return -EINVAL;
+	}
+
+	droq = oct->droq[q_no];
+
+	if (!droq) {
+		dev_info(&oct->pci_dev->dev,
+			 "Droq id (%d) not available.\n", q_no);
+		return 0;
+	}
+
+	spin_lock_irqsave(&droq->lock, flags);
+
+	droq->ops.fptr = NULL;
+	droq->ops.drop_on_max = 0;
+
+	spin_unlock_irqrestore(&droq->lock, flags);
+
+	return 0;
+}
+
+int octeon_create_droq(struct octeon_device *oct,
+		       u32 q_no, u32 num_descs,
+		       u32 desc_size, void *app_ctx)
+{
+	struct octeon_droq *droq;
+
+	if (oct->droq[q_no]) {
+		dev_dbg(&oct->pci_dev->dev, "Droq already in use. Cannot create droq %d again\n",
+			q_no);
+		return 1;
+	}
+
+	/* Allocate the DS for the new droq. */
+	droq = vmalloc(sizeof(*droq));
+	if (!droq)
+		goto create_droq_fail;
+	memset(droq, 0, sizeof(struct octeon_droq));
+
+	/*Disable the pkt o/p for this Q  */
+	octeon_set_droq_pkt_op(oct, q_no, 0);
+	oct->droq[q_no] = droq;
+
+	/* Initialize the Droq */
+	octeon_init_droq(oct, q_no, num_descs, desc_size, app_ctx);
+
+	oct->num_oqs++;
+
+	dev_dbg(&oct->pci_dev->dev, "%s: Total number of OQ: %d\n", __func__,
+		oct->num_oqs);
+
+	/* Global Droq register settings */
+
+	/* As of now not required, as setting are done for all 32 Droqs at
+	 * the same time.
+	 */
+	return 0;
+
+create_droq_fail:
+	octeon_delete_droq(oct, q_no);
+	return -1;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_droq.h b/drivers/net/ethernet/cavium/liquidio/octeon_droq.h
new file mode 100644
index 000000000000..7940ccee12d9
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_droq.h
@@ -0,0 +1,426 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+
+/*!  \file  octeon_droq.h
+ *   \brief Implementation of Octeon Output queues. "Output" is with
+ *   respect to the Octeon device on the NIC. From this driver's point of
+ *   view they are ingress queues.
+ */
+
+#ifndef __OCTEON_DROQ_H__
+#define __OCTEON_DROQ_H__
+
+/* Default number of packets that will be processed in one iteration. */
+#define MAX_PACKET_BUDGET 0xFFFFFFFF
+
+/** Octeon descriptor format.
+ *  The descriptor ring is made of descriptors which have 2 64-bit values:
+ *  -# Physical (bus) address of the data buffer.
+ *  -# Physical (bus) address of a octeon_droq_info structure.
+ *  The Octeon device DMA's incoming packets and its information at the address
+ *  given by these descriptor fields.
+ */
+struct octeon_droq_desc {
+	/** The buffer pointer */
+	u64 buffer_ptr;
+
+	/** The Info pointer */
+	u64 info_ptr;
+};
+
+#define OCT_DROQ_DESC_SIZE    (sizeof(struct octeon_droq_desc))
+
+/** Information about packet DMA'ed by Octeon.
+ *  The format of the information available at Info Pointer after Octeon
+ *  has posted a packet. Not all descriptors have valid information. Only
+ *  the Info field of the first descriptor for a packet has information
+ *  about the packet.
+ */
+struct octeon_droq_info {
+	/** The Output Receive Header. */
+	union octeon_rh rh;
+
+	/** The Length of the packet. */
+	u64 length;
+};
+
+#define OCT_DROQ_INFO_SIZE   (sizeof(struct octeon_droq_info))
+
+/** Pointer to data buffer.
+ *  Driver keeps a pointer to the data buffer that it made available to
+ *  the Octeon device. Since the descriptor ring keeps physical (bus)
+ *  addresses, this field is required for the driver to keep track of
+ *  the virtual address pointers.
+*/
+struct octeon_recv_buffer {
+	/** Packet buffer, including metadata. */
+	void *buffer;
+
+	/** Data in the packet buffer.  */
+	u8 *data;
+};
+
+#define OCT_DROQ_RECVBUF_SIZE    (sizeof(struct octeon_recv_buffer))
+
+/** Output Queue statistics. Each output queue has four stats fields. */
+struct oct_droq_stats {
+	/** Number of packets received in this queue. */
+	u64 pkts_received;
+
+	/** Bytes received by this queue. */
+	u64 bytes_received;
+
+	/** Packets dropped due to no dispatch function. */
+	u64 dropped_nodispatch;
+
+	/** Packets dropped due to no memory available. */
+	u64 dropped_nomem;
+
+	/** Packets dropped due to large number of pkts to process. */
+	u64 dropped_toomany;
+
+	/** Number of packets  sent to stack from this queue. */
+	u64 rx_pkts_received;
+
+	/** Number of Bytes sent to stack from this queue. */
+	u64 rx_bytes_received;
+
+	/** Num of Packets dropped due to receive path failures. */
+	u64 rx_dropped;
+};
+
+#define POLL_EVENT_INTR_ARRIVED  1
+#define POLL_EVENT_PROCESS_PKTS  2
+#define POLL_EVENT_PENDING_PKTS  3
+#define POLL_EVENT_ENABLE_INTR   4
+
+/* The maximum number of buffers that can be dispatched from the
+ * output/dma queue. Set to 64 assuming 1K buffers in DROQ and the fact that
+ * max packet size from DROQ is 64K.
+ */
+#define    MAX_RECV_BUFS    64
+
+/** Receive Packet format used when dispatching output queue packets
+ *  with non-raw opcodes.
+ *  The received packet will be sent to the upper layers using this
+ *  structure which is passed as a parameter to the dispatch function
+ */
+struct octeon_recv_pkt {
+	/**  Number of buffers in this received packet */
+	u16 buffer_count;
+
+	/** Id of the device that is sending the packet up */
+	u16 octeon_id;
+
+	/** Length of data in the packet buffer */
+	u32 length;
+
+	/** The receive header */
+	union octeon_rh rh;
+
+	/** Pointer to the OS-specific packet buffer */
+	void *buffer_ptr[MAX_RECV_BUFS];
+
+	/** Size of the buffers pointed to by ptr's in buffer_ptr */
+	u32 buffer_size[MAX_RECV_BUFS];
+};
+
+#define OCT_RECV_PKT_SIZE    (sizeof(struct octeon_recv_pkt))
+
+/** The first parameter of a dispatch function.
+ *  For a raw mode opcode, the driver dispatches with the device
+ *  pointer in this structure.
+ *  For non-raw mode opcode, the driver dispatches the recv_pkt
+ *  created to contain the buffers with data received from Octeon.
+ *  ---------------------
+ *  |     *recv_pkt ----|---
+ *  |-------------------|   |
+ *  | 0 or more bytes   |   |
+ *  | reserved by driver|   |
+ *  |-------------------|<-/
+ *  | octeon_recv_pkt   |
+ *  |                   |
+ *  |___________________|
+ */
+struct octeon_recv_info {
+	void *rsvd;
+	struct octeon_recv_pkt *recv_pkt;
+};
+
+#define  OCT_RECV_INFO_SIZE    (sizeof(struct octeon_recv_info))
+
+/** Allocate a recv_info structure. The recv_pkt pointer in the recv_info
+ *  structure is filled in before this call returns.
+ *  @param extra_bytes - extra bytes to be allocated at the end of the recv info
+ *                       structure.
+ *  @return - pointer to a newly allocated recv_info structure.
+ */
+static inline struct octeon_recv_info *octeon_alloc_recv_info(int extra_bytes)
+{
+	struct octeon_recv_info *recv_info;
+	u8 *buf;
+
+	buf = kmalloc(OCT_RECV_PKT_SIZE + OCT_RECV_INFO_SIZE +
+		      extra_bytes, GFP_ATOMIC);
+	if (!buf)
+		return NULL;
+
+	recv_info = (struct octeon_recv_info *)buf;
+	recv_info->recv_pkt =
+		(struct octeon_recv_pkt *)(buf + OCT_RECV_INFO_SIZE);
+	recv_info->rsvd = NULL;
+	if (extra_bytes)
+		recv_info->rsvd = buf + OCT_RECV_INFO_SIZE + OCT_RECV_PKT_SIZE;
+
+	return recv_info;
+}
+
+/** Free a recv_info structure.
+ *  @param recv_info - Pointer to receive_info to be freed
+ */
+static inline void octeon_free_recv_info(struct octeon_recv_info *recv_info)
+{
+	kfree(recv_info);
+}
+
+typedef int (*octeon_dispatch_fn_t)(struct octeon_recv_info *, void *);
+
+/** Used by NIC module to register packet handler and to get device
+ * information for each octeon device.
+ */
+struct octeon_droq_ops {
+	/** This registered function will be called by the driver with
+	 *  the octeon id, pointer to buffer from droq and length of
+	 *  data in the buffer. The receive header gives the port
+	 *  number to the caller.  Function pointer is set by caller.
+	 */
+	void (*fptr)(u32, void *, u32, union octeon_rh *, void *);
+
+	/* This function will be called by the driver for all NAPI related
+	 * events. The first param is the octeon id. The second param is the
+	 * output queue number. The third is the NAPI event that occurred.
+	 */
+	void (*napi_fn)(void *);
+
+	u32 poll_mode;
+
+	/** Flag indicating if the DROQ handler should drop packets that
+	 *  it cannot handle in one iteration. Set by caller.
+	 */
+	u32 drop_on_max;
+};
+
+/** The Descriptor Ring Output Queue structure.
+ *  This structure has all the information required to implement a
+ *  Octeon DROQ.
+ */
+struct octeon_droq {
+	/** A spinlock to protect access to this ring. */
+	spinlock_t lock;
+
+	u32 q_no;
+
+	struct octeon_droq_ops ops;
+
+	struct octeon_device *oct_dev;
+
+	/** The 8B aligned descriptor ring starts at this address. */
+	struct octeon_droq_desc *desc_ring;
+
+	/** Index in the ring where the driver should read the next packet */
+	u32 read_idx;
+
+	/** Index in the ring where Octeon will write the next packet */
+	u32 write_idx;
+
+	/** Index in the ring where the driver will refill the descriptor's
+	 * buffer
+	 */
+	u32 refill_idx;
+
+	/** Packets pending to be processed */
+	atomic_t pkts_pending;
+
+	/** Number of  descriptors in this ring. */
+	u32 max_count;
+
+	/** The number of descriptors pending refill. */
+	u32 refill_count;
+
+	u32 pkts_per_intr;
+	u32 refill_threshold;
+
+	/** The max number of descriptors in DROQ without a buffer.
+	 * This field is used to keep track of empty space threshold. If the
+	 * refill_count reaches this value, the DROQ cannot accept a max-sized
+	 * (64K) packet.
+	 */
+	u32 max_empty_descs;
+
+	/** The 8B aligned info ptrs begin from this address. */
+	struct octeon_droq_info *info_list;
+
+	/** The receive buffer list. This list has the virtual addresses of the
+	 * buffers.
+	 */
+	struct octeon_recv_buffer *recv_buf_list;
+
+	/** The size of each buffer pointed by the buffer pointer. */
+	u32 buffer_size;
+
+	/** Pointer to the mapped packet credit register.
+	 * Host writes number of info/buffer ptrs available to this register
+	 */
+	void  __iomem *pkts_credit_reg;
+
+	/** Pointer to the mapped packet sent register.
+	 * Octeon writes the number of packets DMA'ed to host memory
+	 * in this register.
+	 */
+	void __iomem *pkts_sent_reg;
+
+	struct list_head dispatch_list;
+
+	/** Statistics for this DROQ. */
+	struct oct_droq_stats stats;
+
+	/** DMA mapped address of the DROQ descriptor ring. */
+	size_t desc_ring_dma;
+
+	/** Info ptr list are allocated at this virtual address. */
+	size_t info_base_addr;
+
+	/** DMA mapped address of the info list */
+	size_t info_list_dma;
+
+	/** Allocated size of info list. */
+	u32 info_alloc_size;
+
+	/** application context */
+	void *app_ctx;
+
+	struct napi_struct napi;
+
+	u32 cpu_id;
+
+	struct call_single_data csd;
+};
+
+#define OCT_DROQ_SIZE   (sizeof(struct octeon_droq))
+
+/**
+ *  Allocates space for the descriptor ring for the droq and sets the
+ *   base addr, num desc etc in Octeon registers.
+ *
+ * @param  oct_dev    - pointer to the octeon device structure
+ * @param  q_no       - droq no. ranges from 0 - 3.
+ * @param app_ctx     - pointer to application context
+ * @return Success: 0    Failure: 1
+*/
+int octeon_init_droq(struct octeon_device *oct_dev,
+		     u32 q_no,
+		     u32 num_descs,
+		     u32 desc_size,
+		     void *app_ctx);
+
+/**
+ *  Frees the space for descriptor ring for the droq.
+ *
+ *  @param oct_dev - pointer to the octeon device structure
+ *  @param q_no    - droq no. ranges from 0 - 3.
+ *  @return:    Success: 0    Failure: 1
+*/
+int octeon_delete_droq(struct octeon_device *oct_dev, u32 q_no);
+
+/** Register a change in droq operations. The ops field has a pointer to a
+ * function which will called by the DROQ handler for all packets arriving
+ * on output queues given by q_no irrespective of the type of packet.
+ * The ops field also has a flag which if set tells the DROQ handler to
+ * drop packets if it receives more than what it can process in one
+ * invocation of the handler.
+ * @param oct       - octeon device
+ * @param q_no      - octeon output queue number (0 <= q_no <= MAX_OCTEON_DROQ-1
+ * @param ops       - the droq_ops settings for this queue
+ * @return          - 0 on success, -ENODEV or -EINVAL on error.
+ */
+int
+octeon_register_droq_ops(struct octeon_device *oct,
+			 u32 q_no,
+			 struct octeon_droq_ops *ops);
+
+/** Resets the function pointer and flag settings made by
+ * octeon_register_droq_ops(). After this routine is called, the DROQ handler
+ * will lookup dispatch function for each arriving packet on the output queue
+ * given by q_no.
+ * @param oct       - octeon device
+ * @param q_no      - octeon output queue number (0 <= q_no <= MAX_OCTEON_DROQ-1
+ * @return          - 0 on success, -ENODEV or -EINVAL on error.
+ */
+int octeon_unregister_droq_ops(struct octeon_device *oct, u32 q_no);
+
+/**   Register a dispatch function for a opcode/subcode. The driver will call
+ *    this dispatch function when it receives a packet with the given
+ *    opcode/subcode in its output queues along with the user specified
+ *    argument.
+ *    @param  oct        - the octeon device to register with.
+ *    @param  opcode     - the opcode for which the dispatch will be registered.
+ *    @param  subcode    - the subcode for which the dispatch will be registered
+ *    @param  fn         - the dispatch function.
+ *    @param  fn_arg     - user specified that will be passed along with the
+ *                         dispatch function by the driver.
+ *    @return Success: 0; Failure: 1
+ */
+int octeon_register_dispatch_fn(struct octeon_device *oct,
+				u16 opcode,
+				u16 subcode,
+				octeon_dispatch_fn_t fn, void *fn_arg);
+
+/**  Remove registration for an opcode/subcode. This will delete the mapping for
+ *   an opcode/subcode. The dispatch function will be unregistered and will no
+ *   longer be called if a packet with the opcode/subcode arrives in the driver
+ *   output queues.
+ *   @param  oct        -  the octeon device to unregister from.
+ *   @param  opcode     -  the opcode to be unregistered.
+ *   @param  subcode    -  the subcode to be unregistered.
+ *
+ *   @return Success: 0; Failure: 1
+ */
+int octeon_unregister_dispatch_fn(struct octeon_device *oct,
+				  u16 opcode,
+				  u16 subcode);
+
+void octeon_droq_print_stats(void);
+
+u32 octeon_droq_check_hw_for_pkts(struct octeon_device *oct,
+				  struct octeon_droq *droq);
+
+int octeon_create_droq(struct octeon_device *oct, u32 q_no,
+		       u32 num_descs, u32 desc_size, void *app_ctx);
+
+int octeon_droq_process_packets(struct octeon_device *oct,
+				struct octeon_droq *droq,
+				u32 budget);
+
+int octeon_process_droq_poll_cmd(struct octeon_device *oct, u32 q_no,
+				 int cmd, u32 arg);
+
+#endif	/*__OCTEON_DROQ_H__ */
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_iq.h b/drivers/net/ethernet/cavium/liquidio/octeon_iq.h
new file mode 100644
index 000000000000..592fe49b589d
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_iq.h
@@ -0,0 +1,319 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+
+/*!  \file  octeon_iq.h
+ *   \brief Host Driver: Implementation of Octeon input queues. "Input" is
+ *   with respect to the Octeon device on the NIC. From this driver's
+ *   point of view they are egress queues.
+ */
+
+#ifndef __OCTEON_IQ_H__
+#define  __OCTEON_IQ_H__
+
+#define IQ_STATUS_RUNNING   1
+
+#define IQ_SEND_OK          0
+#define IQ_SEND_STOP        1
+#define IQ_SEND_FAILED     -1
+
+/*-------------------------  INSTRUCTION QUEUE --------------------------*/
+
+/* \cond */
+
+#define REQTYPE_NONE                 0
+#define REQTYPE_NORESP_NET           1
+#define REQTYPE_NORESP_NET_SG        2
+#define REQTYPE_RESP_NET             3
+#define REQTYPE_RESP_NET_SG          4
+#define REQTYPE_SOFT_COMMAND         5
+#define REQTYPE_LAST                 5
+
+struct octeon_request_list {
+	u32 reqtype;
+	void *buf;
+};
+
+/* \endcond */
+
+/** Input Queue statistics. Each input queue has four stats fields. */
+struct oct_iq_stats {
+	u64 instr_posted; /**< Instructions posted to this queue. */
+	u64 instr_processed; /**< Instructions processed in this queue. */
+	u64 instr_dropped; /**< Instructions that could not be processed */
+	u64 bytes_sent;  /**< Bytes sent through this queue. */
+	u64 sgentry_sent;/**< Gather entries sent through this queue. */
+	u64 tx_done;/**< Num of packets sent to network. */
+	u64 tx_iq_busy;/**< Numof times this iq was found to be full. */
+	u64 tx_dropped;/**< Numof pkts dropped dueto xmitpath errors. */
+	u64 tx_tot_bytes;/**< Total count of bytes sento to network. */
+};
+
+#define OCT_IQ_STATS_SIZE   (sizeof(struct oct_iq_stats))
+
+/** The instruction (input) queue.
+ *  The input queue is used to post raw (instruction) mode data or packet
+ *  data to Octeon device from the host. Each input queue (upto 4) for
+ *  a Octeon device has one such structure to represent it.
+*/
+struct octeon_instr_queue {
+	/** A spinlock to protect access to the input ring.  */
+	spinlock_t lock;
+
+	/** Flag that indicates if the queue uses 64 byte commands. */
+	u32 iqcmd_64B:1;
+
+	/** Queue Number. */
+	u32 iq_no:5;
+
+	u32 rsvd:17;
+
+	/* Controls the periodic flushing of iq */
+	u32 do_auto_flush:1;
+
+	u32 status:8;
+
+	/** Maximum no. of instructions in this queue. */
+	u32 max_count;
+
+	/** Index in input ring where the driver should write the next packet */
+	u32 host_write_index;
+
+	/** Index in input ring where Octeon is expected to read the next
+	 * packet.
+	 */
+	u32 octeon_read_index;
+
+	/** This index aids in finding the window in the queue where Octeon
+	  * has read the commands.
+	  */
+	u32 flush_index;
+
+	/** This field keeps track of the instructions pending in this queue. */
+	atomic_t instr_pending;
+
+	u32 reset_instr_cnt;
+
+	/** Pointer to the Virtual Base addr of the input ring. */
+	u8 *base_addr;
+
+	struct octeon_request_list *request_list;
+
+	/** Octeon doorbell register for the ring. */
+	void __iomem *doorbell_reg;
+
+	/** Octeon instruction count register for this ring. */
+	void __iomem *inst_cnt_reg;
+
+	/** Number of instructions pending to be posted to Octeon. */
+	u32 fill_cnt;
+
+	/** The max. number of instructions that can be held pending by the
+	 * driver.
+	 */
+	u32 fill_threshold;
+
+	/** The last time that the doorbell was rung. */
+	u64 last_db_time;
+
+	/** The doorbell timeout. If the doorbell was not rung for this time and
+	  * fill_cnt is non-zero, ring the doorbell again.
+	  */
+	u32 db_timeout;
+
+	/** Statistics for this input queue. */
+	struct oct_iq_stats stats;
+
+	/** DMA mapped base address of the input descriptor ring. */
+	u64 base_addr_dma;
+
+	/** Application context */
+	void *app_ctx;
+};
+
+/*----------------------  INSTRUCTION FORMAT ----------------------------*/
+
+/** 32-byte instruction format.
+ *  Format of instruction for a 32-byte mode input queue.
+ */
+struct octeon_instr_32B {
+	/** Pointer where the input data is available. */
+	u64 dptr;
+
+	/** Instruction Header.  */
+	u64 ih;
+
+	/** Pointer where the response for a RAW mode packet will be written
+	 * by Octeon.
+	 */
+	u64 rptr;
+
+	/** Input Request Header. Additional info about the input. */
+	u64 irh;
+
+};
+
+#define OCT_32B_INSTR_SIZE     (sizeof(struct octeon_instr_32B))
+
+/** 64-byte instruction format.
+ *  Format of instruction for a 64-byte mode input queue.
+ */
+struct octeon_instr_64B {
+	/** Pointer where the input data is available. */
+	u64 dptr;
+
+	/** Instruction Header. */
+	u64 ih;
+
+	/** Input Request Header. */
+	u64 irh;
+
+	/** opcode/subcode specific parameters */
+	u64 ossp[2];
+
+	/** Return Data Parameters */
+	u64 rdp;
+
+	/** Pointer where the response for a RAW mode packet will be written
+	 * by Octeon.
+	 */
+	u64 rptr;
+
+	u64 reserved;
+
+};
+
+#define OCT_64B_INSTR_SIZE     (sizeof(struct octeon_instr_64B))
+
+/** The size of each buffer in soft command buffer pool
+ */
+#define  SOFT_COMMAND_BUFFER_SIZE	1024
+
+struct octeon_soft_command {
+	/** Soft command buffer info. */
+	struct list_head node;
+	u64 dma_addr;
+	u32 size;
+
+	/** Command and return status */
+	struct octeon_instr_64B cmd;
+#define COMPLETION_WORD_INIT    0xffffffffffffffffULL
+	u64 *status_word;
+
+	/** Data buffer info */
+	void *virtdptr;
+	u64 dmadptr;
+	u32 datasize;
+
+	/** Return buffer info */
+	void *virtrptr;
+	u64 dmarptr;
+	u32 rdatasize;
+
+	/** Context buffer info */
+	void *ctxptr;
+	u32  ctxsize;
+
+	/** Time out and callback */
+	size_t wait_time;
+	size_t timeout;
+	u32 iq_no;
+	void (*callback)(struct octeon_device *, u32, void *);
+	void *callback_arg;
+};
+
+/** Maximum number of buffers to allocate into soft command buffer pool
+ */
+#define  MAX_SOFT_COMMAND_BUFFERS	16
+
+/** Head of a soft command buffer pool.
+ */
+struct octeon_sc_buffer_pool {
+	/** List structure to add delete pending entries to */
+	struct list_head head;
+
+	/** A lock for this response list */
+	spinlock_t lock;
+
+	atomic_t alloc_buf_count;
+};
+
+int octeon_setup_sc_buffer_pool(struct octeon_device *oct);
+int octeon_free_sc_buffer_pool(struct octeon_device *oct);
+struct octeon_soft_command *
+	octeon_alloc_soft_command(struct octeon_device *oct,
+				  u32 datasize, u32 rdatasize,
+				  u32 ctxsize);
+void octeon_free_soft_command(struct octeon_device *oct,
+			      struct octeon_soft_command *sc);
+
+/**
+ *  octeon_init_instr_queue()
+ *  @param octeon_dev      - pointer to the octeon device structure.
+ *  @param iq_no           - queue to be initialized (0 <= q_no <= 3).
+ *
+ *  Called at driver init time for each input queue. iq_conf has the
+ *  configuration parameters for the queue.
+ *
+ *  @return  Success: 0   Failure: 1
+ */
+int octeon_init_instr_queue(struct octeon_device *octeon_dev, u32 iq_no,
+			    u32 num_descs);
+
+/**
+ *  octeon_delete_instr_queue()
+ *  @param octeon_dev      - pointer to the octeon device structure.
+ *  @param iq_no           - queue to be deleted (0 <= q_no <= 3).
+ *
+ *  Called at driver unload time for each input queue. Deletes all
+ *  allocated resources for the input queue.
+ *
+ *  @return  Success: 0   Failure: 1
+ */
+int octeon_delete_instr_queue(struct octeon_device *octeon_dev, u32 iq_no);
+
+int lio_wait_for_instr_fetch(struct octeon_device *oct);
+
+int
+octeon_register_reqtype_free_fn(struct octeon_device *oct, int reqtype,
+				void (*fn)(void *));
+
+int
+lio_process_iq_request_list(struct octeon_device *oct,
+			    struct octeon_instr_queue *iq);
+
+int octeon_send_command(struct octeon_device *oct, u32 iq_no,
+			u32 force_db, void *cmd, void *buf,
+			u32 datasize, u32 reqtype);
+
+void octeon_prepare_soft_command(struct octeon_device *oct,
+				 struct octeon_soft_command *sc,
+				 u8 opcode, u8 subcode,
+				 u32 irh_ossp, u64 ossp0,
+				 u64 ossp1);
+
+int octeon_send_soft_command(struct octeon_device *oct,
+			     struct octeon_soft_command *sc);
+
+int octeon_setup_iq(struct octeon_device *oct, u32 iq_no,
+		    u32 num_descs, void *app_ctx);
+
+#endif				/* __OCTEON_IQ_H__ */
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_main.h b/drivers/net/ethernet/cavium/liquidio/octeon_main.h
new file mode 100644
index 000000000000..cbd081981180
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_main.h
@@ -0,0 +1,237 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+
+/*! \file octeon_main.h
+ *  \brief Host Driver: This file is included by all host driver source files
+ *  to include common definitions.
+ */
+
+#ifndef _OCTEON_MAIN_H_
+#define  _OCTEON_MAIN_H_
+
+#if BITS_PER_LONG == 32
+#define CVM_CAST64(v) ((long long)(v))
+#elif BITS_PER_LONG == 64
+#define CVM_CAST64(v) ((long long)(long)(v))
+#else
+#error "Unknown system architecture"
+#endif
+
+#define DRV_NAME "LiquidIO"
+
+/**
+ * \brief determines if a given console has debug enabled.
+ * @param console console to check
+ * @returns  1 = enabled. 0 otherwise
+ */
+int octeon_console_debug_enabled(u32 console);
+
+/* BQL-related functions */
+void octeon_report_sent_bytes_to_bql(void *buf, int reqtype);
+void octeon_update_tx_completion_counters(void *buf, int reqtype,
+					  unsigned int *pkts_compl,
+					  unsigned int *bytes_compl);
+void octeon_report_tx_completion_to_bql(void *txq, unsigned int pkts_compl,
+					unsigned int bytes_compl);
+
+/** Swap 8B blocks */
+static inline void octeon_swap_8B_data(u64 *data, u32 blocks)
+{
+	while (blocks) {
+		cpu_to_be64s(data);
+		blocks--;
+		data++;
+	}
+}
+
+/**
+  * \brief unmaps a PCI BAR
+  * @param oct Pointer to Octeon device
+  * @param baridx bar index
+  */
+static inline void octeon_unmap_pci_barx(struct octeon_device *oct, int baridx)
+{
+	dev_dbg(&oct->pci_dev->dev, "Freeing PCI mapped regions for Bar%d\n",
+		baridx);
+
+	if (oct->mmio[baridx].done)
+		iounmap(oct->mmio[baridx].hw_addr);
+
+	if (oct->mmio[baridx].start)
+		pci_release_region(oct->pci_dev, baridx * 2);
+}
+
+/**
+ * \brief maps a PCI BAR
+ * @param oct Pointer to Octeon device
+ * @param baridx bar index
+ * @param max_map_len maximum length of mapped memory
+ */
+static inline int octeon_map_pci_barx(struct octeon_device *oct,
+				      int baridx, int max_map_len)
+{
+	u32 mapped_len = 0;
+
+	if (pci_request_region(oct->pci_dev, baridx * 2, DRV_NAME)) {
+		dev_err(&oct->pci_dev->dev, "pci_request_region failed for bar %d\n",
+			baridx);
+		return 1;
+	}
+
+	oct->mmio[baridx].start = pci_resource_start(oct->pci_dev, baridx * 2);
+	oct->mmio[baridx].len = pci_resource_len(oct->pci_dev, baridx * 2);
+
+	mapped_len = oct->mmio[baridx].len;
+	if (!mapped_len)
+		return 1;
+
+	if (max_map_len && (mapped_len > max_map_len))
+		mapped_len = max_map_len;
+
+	oct->mmio[baridx].hw_addr =
+		ioremap(oct->mmio[baridx].start, mapped_len);
+	oct->mmio[baridx].mapped_len = mapped_len;
+
+	dev_dbg(&oct->pci_dev->dev, "BAR%d start: 0x%llx mapped %u of %u bytes\n",
+		baridx, oct->mmio[baridx].start, mapped_len,
+		oct->mmio[baridx].len);
+
+	if (!oct->mmio[baridx].hw_addr) {
+		dev_err(&oct->pci_dev->dev, "error ioremap for bar %d\n",
+			baridx);
+		return 1;
+	}
+	oct->mmio[baridx].done = 1;
+
+	return 0;
+}
+
+static inline void *
+cnnic_alloc_aligned_dma(struct pci_dev *pci_dev,
+			u32 size,
+			u32 *alloc_size,
+			size_t *orig_ptr,
+			size_t *dma_addr __attribute__((unused)))
+{
+	int retries = 0;
+	void *ptr = NULL;
+
+#define OCTEON_MAX_ALLOC_RETRIES     1
+	do {
+		ptr =
+		    (void *)__get_free_pages(GFP_KERNEL,
+					     get_order(size));
+		if ((unsigned long)ptr & 0x07) {
+			free_pages((unsigned long)ptr, get_order(size));
+			ptr = NULL;
+			/* Increment the size required if the first
+			 * attempt failed.
+			 */
+			if (!retries)
+				size += 7;
+		}
+		retries++;
+	} while ((retries <= OCTEON_MAX_ALLOC_RETRIES) && !ptr);
+
+	*alloc_size = size;
+	*orig_ptr = (unsigned long)ptr;
+	if ((unsigned long)ptr & 0x07)
+		ptr = (void *)(((unsigned long)ptr + 7) & ~(7UL));
+	return ptr;
+}
+
+#define cnnic_free_aligned_dma(pci_dev, ptr, size, orig_ptr, dma_addr) \
+		free_pages(orig_ptr, get_order(size))
+
+static inline void
+sleep_cond(wait_queue_head_t *wait_queue, int *condition)
+{
+	wait_queue_t we;
+
+	init_waitqueue_entry(&we, current);
+	add_wait_queue(wait_queue, &we);
+	while (!(ACCESS_ONCE(*condition))) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (signal_pending(current))
+			goto out;
+		schedule();
+	}
+out:
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(wait_queue, &we);
+}
+
+static inline void
+sleep_atomic_cond(wait_queue_head_t *waitq, atomic_t *pcond)
+{
+	wait_queue_t we;
+
+	init_waitqueue_entry(&we, current);
+	add_wait_queue(waitq, &we);
+	while (!atomic_read(pcond)) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (signal_pending(current))
+			goto out;
+		schedule();
+	}
+out:
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(waitq, &we);
+}
+
+/* Gives up the CPU for a timeout period.
+ * Check that the condition is not true before we go to sleep for a
+ * timeout period.
+ */
+static inline void
+sleep_timeout_cond(wait_queue_head_t *wait_queue,
+		   int *condition,
+		   int timeout)
+{
+	wait_queue_t we;
+
+	init_waitqueue_entry(&we, current);
+	add_wait_queue(wait_queue, &we);
+	set_current_state(TASK_INTERRUPTIBLE);
+	if (!(*condition))
+		schedule_timeout(timeout);
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(wait_queue, &we);
+}
+
+#ifndef ROUNDUP4
+#define ROUNDUP4(val) (((val) + 3) & 0xfffffffc)
+#endif
+
+#ifndef ROUNDUP8
+#define ROUNDUP8(val) (((val) + 7) & 0xfffffff8)
+#endif
+
+#ifndef ROUNDUP16
+#define ROUNDUP16(val) (((val) + 15) & 0xfffffff0)
+#endif
+
+#ifndef ROUNDUP128
+#define ROUNDUP128(val) (((val) + 127) & 0xffffff80)
+#endif
+
+#endif /* _OCTEON_MAIN_H_ */
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c
new file mode 100644
index 000000000000..5aecef870377
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.c
@@ -0,0 +1,199 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+#include "octeon_mem_ops.h"
+
+#define MEMOPS_IDX   MAX_BAR1_MAP_INDEX
+
+static inline void
+octeon_toggle_bar1_swapmode(struct octeon_device *oct __attribute__((unused)),
+			    u32 idx __attribute__((unused)))
+{
+#ifdef __BIG_ENDIAN_BITFIELD
+	u32 mask;
+
+	mask = oct->fn_list.bar1_idx_read(oct, idx);
+	mask = (mask & 0x2) ? (mask & ~2) : (mask | 2);
+	oct->fn_list.bar1_idx_write(oct, idx, mask);
+#endif
+}
+
+static void
+octeon_pci_fastwrite(struct octeon_device *oct, u8 __iomem *mapped_addr,
+		     u8 *hostbuf, u32 len)
+{
+	while ((len) && ((unsigned long)mapped_addr) & 7) {
+		writeb(*(hostbuf++), mapped_addr++);
+		len--;
+	}
+
+	octeon_toggle_bar1_swapmode(oct, MEMOPS_IDX);
+
+	while (len >= 8) {
+		writeq(*((u64 *)hostbuf), mapped_addr);
+		mapped_addr += 8;
+		hostbuf += 8;
+		len -= 8;
+	}
+
+	octeon_toggle_bar1_swapmode(oct, MEMOPS_IDX);
+
+	while (len--)
+		writeb(*(hostbuf++), mapped_addr++);
+}
+
+static void
+octeon_pci_fastread(struct octeon_device *oct, u8 __iomem *mapped_addr,
+		    u8 *hostbuf, u32 len)
+{
+	while ((len) && ((unsigned long)mapped_addr) & 7) {
+		*(hostbuf++) = readb(mapped_addr++);
+		len--;
+	}
+
+	octeon_toggle_bar1_swapmode(oct, MEMOPS_IDX);
+
+	while (len >= 8) {
+		*((u64 *)hostbuf) = readq(mapped_addr);
+		mapped_addr += 8;
+		hostbuf += 8;
+		len -= 8;
+	}
+
+	octeon_toggle_bar1_swapmode(oct, MEMOPS_IDX);
+
+	while (len--)
+		*(hostbuf++) = readb(mapped_addr++);
+}
+
+/* Core mem read/write with temporary bar1 settings. */
+/* op = 1 to read, op = 0 to write. */
+static void
+__octeon_pci_rw_core_mem(struct octeon_device *oct, u64 addr,
+			 u8 *hostbuf, u32 len, u32 op)
+{
+	u32 copy_len = 0, index_reg_val = 0;
+	unsigned long flags;
+	u8 __iomem *mapped_addr;
+
+	spin_lock_irqsave(&oct->mem_access_lock, flags);
+
+	/* Save the original index reg value. */
+	index_reg_val = oct->fn_list.bar1_idx_read(oct, MEMOPS_IDX);
+	do {
+		oct->fn_list.bar1_idx_setup(oct, addr, MEMOPS_IDX, 1);
+		mapped_addr = oct->mmio[1].hw_addr
+		    + (MEMOPS_IDX << 22) + (addr & 0x3fffff);
+
+		/* If operation crosses a 4MB boundary, split the transfer
+		 * at the 4MB
+		 * boundary.
+		 */
+		if (((addr + len - 1) & ~(0x3fffff)) != (addr & ~(0x3fffff))) {
+			copy_len = (u32)(((addr & ~(0x3fffff)) +
+				   (MEMOPS_IDX << 22)) - addr);
+		} else {
+			copy_len = len;
+		}
+
+		if (op) {	/* read from core */
+			octeon_pci_fastread(oct, mapped_addr, hostbuf,
+					    copy_len);
+		} else {
+			octeon_pci_fastwrite(oct, mapped_addr, hostbuf,
+					     copy_len);
+		}
+
+		len -= copy_len;
+		addr += copy_len;
+		hostbuf += copy_len;
+
+	} while (len);
+
+	oct->fn_list.bar1_idx_write(oct, MEMOPS_IDX, index_reg_val);
+
+	spin_unlock_irqrestore(&oct->mem_access_lock, flags);
+}
+
+void
+octeon_pci_read_core_mem(struct octeon_device *oct,
+			 u64 coreaddr,
+			 u8 *buf,
+			 u32 len)
+{
+	__octeon_pci_rw_core_mem(oct, coreaddr, buf, len, 1);
+}
+
+void
+octeon_pci_write_core_mem(struct octeon_device *oct,
+			  u64 coreaddr,
+			  u8 *buf,
+			  u32 len)
+{
+	__octeon_pci_rw_core_mem(oct, coreaddr, buf, len, 0);
+}
+
+u64 octeon_read_device_mem64(struct octeon_device *oct, u64 coreaddr)
+{
+	__be64 ret;
+
+	__octeon_pci_rw_core_mem(oct, coreaddr, (u8 *)&ret, 8, 1);
+
+	return be64_to_cpu(ret);
+}
+
+u32 octeon_read_device_mem32(struct octeon_device *oct, u64 coreaddr)
+{
+	__be32 ret;
+
+	__octeon_pci_rw_core_mem(oct, coreaddr, (u8 *)&ret, 4, 1);
+
+	return be32_to_cpu(ret);
+}
+
+void octeon_write_device_mem32(struct octeon_device *oct, u64 coreaddr,
+			       u32 val)
+{
+	__be32 t = cpu_to_be32(val);
+
+	__octeon_pci_rw_core_mem(oct, coreaddr, (u8 *)&t, 4, 0);
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.h b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.h
new file mode 100644
index 000000000000..11b183377b44
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_mem_ops.h
@@ -0,0 +1,75 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+
+/*!  \file octeon_mem_ops.h
+ *   \brief Host Driver: Routines used to read/write Octeon memory.
+ */
+
+#ifndef __OCTEON_MEM_OPS_H__
+#define __OCTEON_MEM_OPS_H__
+
+/**  Read a 64-bit value from a BAR1 mapped core memory address.
+ *   @param  oct        -  pointer to the octeon device.
+ *   @param  core_addr  -  the address to read from.
+ *
+ *   The range_idx gives the BAR1 index register for the range of address
+ *   in which core_addr is mapped.
+ *
+ *   @return  64-bit value read from Core memory
+ */
+u64 octeon_read_device_mem64(struct octeon_device *oct, u64 core_addr);
+
+/**  Read a 32-bit value from a BAR1 mapped core memory address.
+ *   @param  oct        -  pointer to the octeon device.
+ *   @param  core_addr  -  the address to read from.
+ *
+ *   @return  32-bit value read from Core memory
+ */
+u32 octeon_read_device_mem32(struct octeon_device *oct, u64 core_addr);
+
+/**  Write a 32-bit value to a BAR1 mapped core memory address.
+ *   @param  oct        -  pointer to the octeon device.
+ *   @param  core_addr  -  the address to write to.
+ *   @param  val        -  32-bit value to write.
+ */
+void
+octeon_write_device_mem32(struct octeon_device *oct,
+			  u64 core_addr,
+			  u32 val);
+
+/** Read multiple bytes from Octeon memory.
+ */
+void
+octeon_pci_read_core_mem(struct octeon_device *oct,
+			 u64 coreaddr,
+			 u8 *buf,
+			 u32 len);
+
+/** Write multiple bytes into Octeon memory.
+ */
+void
+octeon_pci_write_core_mem(struct octeon_device *oct,
+			  u64 coreaddr,
+			  u8 *buf,
+			  u32 len);
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_network.h b/drivers/net/ethernet/cavium/liquidio/octeon_network.h
new file mode 100644
index 000000000000..b3abe5818fd3
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_network.h
@@ -0,0 +1,224 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+
+/*!  \file  octeon_network.h
+ *   \brief Host NIC Driver: Structure and Macro definitions used by NIC Module.
+ */
+
+#ifndef __OCTEON_NETWORK_H__
+#define __OCTEON_NETWORK_H__
+#include <linux/version.h>
+#include <linux/dma-mapping.h>
+#include <linux/ptp_clock_kernel.h>
+
+/** LiquidIO per-interface network private data */
+struct lio {
+	/** State of the interface. Rx/Tx happens only in the RUNNING state.  */
+	atomic_t ifstate;
+
+	/** Octeon Interface index number. This device will be represented as
+	 *  oct<ifidx> in the system.
+	 */
+	int ifidx;
+
+	/** Octeon Input queue to use to transmit for this network interface. */
+	int txq;
+
+	/** Octeon Output queue from which pkts arrive
+	 * for this network interface.
+	 */
+	int rxq;
+
+	/** Guards the glist */
+	spinlock_t lock;
+
+	/** Linked list of gather components */
+	struct list_head glist;
+
+	/** Pointer to the NIC properties for the Octeon device this network
+	 *  interface is associated with.
+	 */
+	struct octdev_props *octprops;
+
+	/** Pointer to the octeon device structure. */
+	struct octeon_device *oct_dev;
+
+	struct net_device *netdev;
+
+	/** Link information sent by the core application for this interface. */
+	struct oct_link_info linfo;
+
+	/** Size of Tx queue for this octeon device. */
+	u32 tx_qsize;
+
+	/** Size of Rx queue for this octeon device. */
+	u32 rx_qsize;
+
+	/** Size of MTU this octeon device. */
+	u32 mtu;
+
+	/** msg level flag per interface. */
+	u32 msg_enable;
+
+	/** Copy of Interface capabilities: TSO, TSO6, LRO, Chescksums . */
+	u64 dev_capability;
+
+	/** Copy of beacaon reg in phy */
+	u32 phy_beacon_val;
+
+	/** Copy of ctrl reg in phy */
+	u32 led_ctrl_val;
+
+	/* PTP clock information */
+	struct ptp_clock_info ptp_info;
+	struct ptp_clock *ptp_clock;
+	s64 ptp_adjust;
+
+	/* for atomic access to Octeon PTP reg and data struct */
+	spinlock_t ptp_lock;
+
+	/* Interface info */
+	u32	intf_open;
+
+	/* work queue for  txq status */
+	struct cavium_wq	txq_status_wq;
+
+};
+
+#define LIO_SIZE         (sizeof(struct lio))
+#define GET_LIO(netdev)  ((struct lio *)netdev_priv(netdev))
+
+/**
+ * \brief Enable or disable feature
+ * @param netdev    pointer to network device
+ * @param cmd       Command that just requires acknowledgment
+ */
+int liquidio_set_feature(struct net_device *netdev, int cmd);
+
+/**
+ * \brief Link control command completion callback
+ * @param nctrl_ptr pointer to control packet structure
+ *
+ * This routine is called by the callback function when a ctrl pkt sent to
+ * core app completes. The nctrl_ptr contains a copy of the command type
+ * and data sent to the core app. This routine is only called if the ctrl
+ * pkt was sent successfully to the core app.
+ */
+void liquidio_link_ctrl_cmd_completion(void *nctrl_ptr);
+
+/**
+ * \brief Register ethtool operations
+ * @param netdev    pointer to network device
+ */
+void liquidio_set_ethtool_ops(struct net_device *netdev);
+
+static inline void
+*recv_buffer_alloc(struct octeon_device *oct __attribute__((unused)),
+		   u32 q_no __attribute__((unused)), u32 size)
+{
+#define SKB_ADJ_MASK  0x3F
+#define SKB_ADJ       (SKB_ADJ_MASK + 1)
+
+	struct sk_buff *skb = dev_alloc_skb(size + SKB_ADJ);
+
+	if ((unsigned long)skb->data & SKB_ADJ_MASK) {
+		u32 r = SKB_ADJ - ((unsigned long)skb->data & SKB_ADJ_MASK);
+
+		skb_reserve(skb, r);
+	}
+
+	return (void *)skb;
+}
+
+static inline void recv_buffer_free(void *buffer)
+{
+	dev_kfree_skb_any((struct sk_buff *)buffer);
+}
+
+#define lio_dma_alloc(oct, size, dma_addr) \
+	dma_alloc_coherent(&oct->pci_dev->dev, size, dma_addr, GFP_KERNEL)
+#define lio_dma_free(oct, size, virt_addr, dma_addr) \
+	dma_free_coherent(&oct->pci_dev->dev, size, virt_addr, dma_addr)
+
+#define   get_rbd(ptr)      (((struct sk_buff *)(ptr))->data)
+
+static inline u64
+lio_map_ring_info(struct octeon_droq *droq, u32 i)
+{
+	dma_addr_t dma_addr;
+	struct octeon_device *oct = droq->oct_dev;
+
+	dma_addr = dma_map_single(&oct->pci_dev->dev, &droq->info_list[i],
+				  OCT_DROQ_INFO_SIZE, DMA_FROM_DEVICE);
+
+	BUG_ON(dma_mapping_error(&oct->pci_dev->dev, dma_addr));
+
+	return (u64)dma_addr;
+}
+
+static inline void
+lio_unmap_ring_info(struct pci_dev *pci_dev,
+		    u64 info_ptr, u32 size)
+{
+	dma_unmap_single(&pci_dev->dev, info_ptr, size, DMA_FROM_DEVICE);
+}
+
+static inline u64
+lio_map_ring(struct pci_dev *pci_dev,
+	     void *buf, u32 size)
+{
+	dma_addr_t dma_addr;
+
+	dma_addr = dma_map_single(&pci_dev->dev, get_rbd(buf), size,
+				  DMA_FROM_DEVICE);
+
+	BUG_ON(dma_mapping_error(&pci_dev->dev, dma_addr));
+
+	return (u64)dma_addr;
+}
+
+static inline void
+lio_unmap_ring(struct pci_dev *pci_dev,
+	       u64 buf_ptr, u32 size)
+{
+	dma_unmap_single(&pci_dev->dev,
+			 buf_ptr, size,
+			 DMA_FROM_DEVICE);
+}
+
+static inline void *octeon_fast_packet_alloc(struct octeon_device *oct,
+					     struct octeon_droq *droq,
+					     u32 q_no, u32 size)
+{
+	return recv_buffer_alloc(oct, q_no, size);
+}
+
+static inline void octeon_fast_packet_next(struct octeon_droq *droq,
+					   struct sk_buff *nicbuf,
+					   int copy_len,
+					   int idx)
+{
+	memcpy(skb_put(nicbuf, copy_len),
+	       get_rbd(droq->recv_buf_list[idx].buffer), copy_len);
+}
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_nic.c b/drivers/net/ethernet/cavium/liquidio/octeon_nic.c
new file mode 100644
index 000000000000..1a0191549cb3
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_nic.c
@@ -0,0 +1,189 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+#include "octeon_mem_ops.h"
+
+void *
+octeon_alloc_soft_command_resp(struct octeon_device    *oct,
+			       struct octeon_instr_64B *cmd,
+			       size_t		       rdatasize)
+{
+	struct octeon_soft_command *sc;
+	struct octeon_instr_ih  *ih;
+	struct octeon_instr_irh *irh;
+	struct octeon_instr_rdp *rdp;
+
+	sc = (struct octeon_soft_command *)
+		octeon_alloc_soft_command(oct, 0, rdatasize, 0);
+
+	if (!sc)
+		return NULL;
+
+	/* Copy existing command structure into the soft command */
+	memcpy(&sc->cmd, cmd, sizeof(struct octeon_instr_64B));
+
+	/* Add in the response related fields. Opcode and Param are already
+	 * there.
+	 */
+	ih      = (struct octeon_instr_ih *)&sc->cmd.ih;
+	ih->fsz = 40; /* irh + ossp[0] + ossp[1] + rdp + rptr = 40 bytes */
+
+	irh        = (struct octeon_instr_irh *)&sc->cmd.irh;
+	irh->rflag = 1; /* a response is required */
+	irh->len   = 4; /* means four 64-bit words immediately follow irh */
+
+	rdp            = (struct octeon_instr_rdp *)&sc->cmd.rdp;
+	rdp->pcie_port = oct->pcie_port;
+	rdp->rlen      = rdatasize;
+
+	*sc->status_word = COMPLETION_WORD_INIT;
+
+	sc->wait_time = 1000;
+	sc->timeout = jiffies + sc->wait_time;
+
+	return sc;
+}
+
+int octnet_send_nic_data_pkt(struct octeon_device *oct,
+			     struct octnic_data_pkt *ndata,
+			     u32 xmit_more)
+{
+	int ring_doorbell;
+
+	ring_doorbell = !xmit_more;
+
+	return octeon_send_command(oct, ndata->q_no, ring_doorbell, &ndata->cmd,
+				   ndata->buf, ndata->datasize,
+				   ndata->reqtype);
+}
+
+static void octnet_link_ctrl_callback(struct octeon_device *oct,
+				      u32 status,
+				      void *sc_ptr)
+{
+	struct octeon_soft_command *sc = (struct octeon_soft_command *)sc_ptr;
+	struct octnic_ctrl_pkt *nctrl;
+
+	nctrl = (struct octnic_ctrl_pkt *)sc->ctxptr;
+
+	/* Call the callback function if status is OK.
+	 * Status is OK only if a response was expected and core returned
+	 * success.
+	 * If no response was expected, status is OK if the command was posted
+	 * successfully.
+	 */
+	if (!status && nctrl->cb_fn)
+		nctrl->cb_fn(nctrl);
+
+	octeon_free_soft_command(oct, sc);
+}
+
+static inline struct octeon_soft_command
+*octnic_alloc_ctrl_pkt_sc(struct octeon_device *oct,
+			  struct octnic_ctrl_pkt *nctrl,
+			  struct octnic_ctrl_params nparams)
+{
+	struct octeon_soft_command *sc = NULL;
+	u8 *data;
+	size_t rdatasize;
+	u32 uddsize = 0, datasize = 0;
+
+	uddsize = (u32)(nctrl->ncmd.s.more * 8);
+
+	datasize = OCTNET_CMD_SIZE + uddsize;
+	rdatasize = (nctrl->wait_time) ? 16 : 0;
+
+	sc = (struct octeon_soft_command *)
+		octeon_alloc_soft_command(oct, datasize, rdatasize,
+					  sizeof(struct octnic_ctrl_pkt));
+
+	if (!sc)
+		return NULL;
+
+	memcpy(sc->ctxptr, nctrl, sizeof(struct octnic_ctrl_pkt));
+
+	data = (u8 *)sc->virtdptr;
+
+	memcpy(data, &nctrl->ncmd,  OCTNET_CMD_SIZE);
+
+	octeon_swap_8B_data((u64 *)data, (OCTNET_CMD_SIZE >> 3));
+
+	if (uddsize) {
+		/* Endian-Swap for UDD should have been done by caller. */
+		memcpy(data + OCTNET_CMD_SIZE, nctrl->udd, uddsize);
+	}
+
+	octeon_prepare_soft_command(oct, sc, OPCODE_NIC, OPCODE_NIC_CMD,
+				    0, 0, 0);
+
+	sc->callback = octnet_link_ctrl_callback;
+	sc->callback_arg = sc;
+	sc->wait_time = nctrl->wait_time;
+
+	return sc;
+}
+
+int
+octnet_send_nic_ctrl_pkt(struct octeon_device *oct,
+			 struct octnic_ctrl_pkt *nctrl,
+			 struct octnic_ctrl_params nparams)
+{
+	int retval;
+	struct octeon_soft_command *sc = NULL;
+
+	sc = octnic_alloc_ctrl_pkt_sc(oct, nctrl, nparams);
+	if (!sc) {
+		dev_err(&oct->pci_dev->dev, "%s soft command alloc failed\n",
+			__func__);
+		return -1;
+	}
+
+	retval = octeon_send_soft_command(oct, sc);
+	if (retval) {
+		octeon_free_soft_command(oct, sc);
+		dev_err(&oct->pci_dev->dev, "%s soft command send failed status: %x\n",
+			__func__, retval);
+		return -1;
+	}
+
+	return retval;
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_nic.h b/drivers/net/ethernet/cavium/liquidio/octeon_nic.h
new file mode 100644
index 000000000000..0238857c8105
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_nic.h
@@ -0,0 +1,227 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+
+/*!  \file octeon_nic.h
+ *   \brief Host NIC Driver: Routine to send network data &
+ *   control packet to Octeon.
+ */
+
+#ifndef __OCTEON_NIC_H__
+#define  __OCTEON_NIC_H__
+
+/* Maximum number of 8-byte words can be sent in a NIC control message.
+ */
+#define  MAX_NCTRL_UDD  32
+
+typedef void (*octnic_ctrl_pkt_cb_fn_t) (void *);
+
+/* Structure of control information passed by the NIC module to the OSI
+ * layer when sending control commands to Octeon device software.
+ */
+struct octnic_ctrl_pkt {
+	/** Command to be passed to the Octeon device software. */
+	union octnet_cmd ncmd;
+
+	/** Send buffer  */
+	void *data;
+	u64 dmadata;
+
+	/** Response buffer */
+	void *rdata;
+	u64 dmardata;
+
+	/** Additional data that may be needed by some commands. */
+	u64 udd[MAX_NCTRL_UDD];
+
+	/** Time to wait for Octeon software to respond to this control command.
+	 *  If wait_time is 0, OSI assumes no response is expected.
+	 */
+	size_t wait_time;
+
+	/** The network device that issued the control command. */
+	u64 netpndev;
+
+	/** Callback function called when the command has been fetched */
+	octnic_ctrl_pkt_cb_fn_t cb_fn;
+};
+
+#define MAX_UDD_SIZE(nctrl) (sizeof(nctrl->udd))
+
+/** Structure of data information passed by the NIC module to the OSI
+ * layer when forwarding data to Octeon device software.
+ */
+struct octnic_data_pkt {
+	/** Pointer to information maintained by NIC module for this packet. The
+	 *  OSI layer passes this as-is to the driver.
+	 */
+	void *buf;
+
+	/** Type of buffer passed in "buf" above. */
+	u32 reqtype;
+
+	/** Total data bytes to be transferred in this command. */
+	u32 datasize;
+
+	/** Command to be passed to the Octeon device software. */
+	struct octeon_instr_64B cmd;
+
+	/** Input queue to use to send this command. */
+	u32 q_no;
+
+};
+
+/** Structure passed by NIC module to OSI layer to prepare a command to send
+ * network data to Octeon.
+ */
+union octnic_cmd_setup {
+	struct {
+		u32 ifidx:8;
+		u32 cksum_offset:7;
+		u32 gather:1;
+		u32 timestamp:1;
+		u32 ipv4opts_ipv6exthdr:2;
+		u32 ip_csum:1;
+		u32 tnl_csum:1;
+
+		u32 rsvd:11;
+		union {
+			u32 datasize;
+			u32 gatherptrs;
+		} u;
+	} s;
+
+	u64 u64;
+
+};
+
+struct octnic_ctrl_params {
+	u32 resp_order;
+};
+
+static inline int octnet_iq_is_full(struct octeon_device *oct, u32 q_no)
+{
+	return ((u32)atomic_read(&oct->instr_queue[q_no]->instr_pending)
+		>= (oct->instr_queue[q_no]->max_count - 2));
+}
+
+/** Utility function to prepare a 64B NIC instruction based on a setup command
+ * @param cmd - pointer to instruction to be filled in.
+ * @param setup - pointer to the setup structure
+ * @param q_no - which queue for back pressure
+ *
+ * Assumes the cmd instruction is pre-allocated, but no fields are filled in.
+ */
+static inline void
+octnet_prepare_pci_cmd(struct octeon_instr_64B *cmd,
+		       union octnic_cmd_setup *setup, u32 tag)
+{
+	struct octeon_instr_ih *ih;
+	struct octeon_instr_irh *irh;
+	union octnic_packet_params packet_params;
+
+	memset(cmd, 0, sizeof(struct octeon_instr_64B));
+
+	ih = (struct octeon_instr_ih *)&cmd->ih;
+
+	/* assume that rflag is cleared so therefore front data will only have
+	 * irh and ossp[1] and ossp[2] for a total of 24 bytes
+	 */
+	ih->fsz = 24;
+
+	ih->tagtype = ORDERED_TAG;
+	ih->grp = DEFAULT_POW_GRP;
+
+	if (tag)
+		ih->tag = tag;
+	else
+		ih->tag = LIO_DATA(setup->s.ifidx);
+
+	ih->raw = 1;
+	ih->qos = (setup->s.ifidx & 3) + 4;	/* map qos based on interface */
+
+	if (!setup->s.gather) {
+		ih->dlengsz = setup->s.u.datasize;
+	} else {
+		ih->gather = 1;
+		ih->dlengsz = setup->s.u.gatherptrs;
+	}
+
+	irh = (struct octeon_instr_irh *)&cmd->irh;
+
+	irh->opcode = OPCODE_NIC;
+	irh->subcode = OPCODE_NIC_NW_DATA;
+
+	packet_params.u32 = 0;
+
+	if (setup->s.cksum_offset) {
+		packet_params.s.csoffset = setup->s.cksum_offset;
+		packet_params.s.ipv4opts_ipv6exthdr =
+						setup->s.ipv4opts_ipv6exthdr;
+	}
+
+	packet_params.s.ip_csum = setup->s.ip_csum;
+	packet_params.s.tnl_csum = setup->s.tnl_csum;
+	packet_params.s.ifidx = setup->s.ifidx;
+	packet_params.s.tsflag = setup->s.timestamp;
+
+	irh->ossp = packet_params.u32;
+}
+
+/** Allocate and a soft command with space for a response immediately following
+ * the commnad.
+ * @param oct - octeon device pointer
+ * @param cmd - pointer to the command structure, pre-filled for everything
+ * except the response.
+ * @param rdatasize - size in bytes of the response.
+ *
+ * @returns pointer to allocated buffer with command copied into it, and
+ * response space immediately following.
+ */
+void *
+octeon_alloc_soft_command_resp(struct octeon_device    *oct,
+			       struct octeon_instr_64B *cmd,
+			       size_t		       rdatasize);
+
+/** Send a NIC data packet to the device
+ * @param oct - octeon device pointer
+ * @param ndata - control structure with queueing, and buffer information
+ *
+ * @returns IQ_FAILED if it failed to add to the input queue. IQ_STOP if it the
+ * queue should be stopped, and IQ_SEND_OK if it sent okay.
+ */
+int octnet_send_nic_data_pkt(struct octeon_device *oct,
+			     struct octnic_data_pkt *ndata, u32 xmit_more);
+
+/** Send a NIC control packet to the device
+ * @param oct - octeon device pointer
+ * @param nctrl - control structure with command, timout, and callback info
+ * @param nparams - response control structure
+ *
+ * @returns IQ_FAILED if it failed to add to the input queue. IQ_STOP if it the
+ * queue should be stopped, and IQ_SEND_OK if it sent okay.
+ */
+int
+octnet_send_nic_ctrl_pkt(struct octeon_device *oct,
+			 struct octnic_ctrl_pkt *nctrl,
+			 struct octnic_ctrl_params nparams);
+
+#endif
diff --git a/drivers/net/ethernet/cavium/liquidio/request_manager.c b/drivers/net/ethernet/cavium/liquidio/request_manager.c
new file mode 100644
index 000000000000..a2a24652c8f3
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/request_manager.c
@@ -0,0 +1,765 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include <linux/vmalloc.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+
+#define INCR_INSTRQUEUE_PKT_COUNT(octeon_dev_ptr, iq_no, field, count)  \
+	(octeon_dev_ptr->instr_queue[iq_no]->stats.field += count)
+
+struct iq_post_status {
+	int status;
+	int index;
+};
+
+static void check_db_timeout(struct work_struct *work);
+static void  __check_db_timeout(struct octeon_device *oct, unsigned long iq_no);
+
+static void (*reqtype_free_fn[MAX_OCTEON_DEVICES][REQTYPE_LAST + 1]) (void *);
+
+static inline int IQ_INSTR_MODE_64B(struct octeon_device *oct, int iq_no)
+{
+	struct octeon_instr_queue *iq =
+	    (struct octeon_instr_queue *)oct->instr_queue[iq_no];
+	return iq->iqcmd_64B;
+}
+
+#define IQ_INSTR_MODE_32B(oct, iq_no)  (!IQ_INSTR_MODE_64B(oct, iq_no))
+
+/* Define this to return the request status comaptible to old code */
+/*#define OCTEON_USE_OLD_REQ_STATUS*/
+
+/* Return 0 on success, 1 on failure */
+int octeon_init_instr_queue(struct octeon_device *oct,
+			    u32 iq_no, u32 num_descs)
+{
+	struct octeon_instr_queue *iq;
+	struct octeon_iq_config *conf = NULL;
+	u32 q_size;
+	struct cavium_wq *db_wq;
+
+	if (OCTEON_CN6XXX(oct))
+		conf = &(CFG_GET_IQ_CFG(CHIP_FIELD(oct, cn6xxx, conf)));
+
+	if (!conf) {
+		dev_err(&oct->pci_dev->dev, "Unsupported Chip %x\n",
+			oct->chip_id);
+		return 1;
+	}
+
+	if (num_descs & (num_descs - 1)) {
+		dev_err(&oct->pci_dev->dev,
+			"Number of descriptors for instr queue %d not in power of 2.\n",
+			iq_no);
+		return 1;
+	}
+
+	q_size = (u32)conf->instr_type * num_descs;
+
+	iq = oct->instr_queue[iq_no];
+
+	iq->base_addr = lio_dma_alloc(oct, q_size,
+				      (dma_addr_t *)&iq->base_addr_dma);
+	if (!iq->base_addr) {
+		dev_err(&oct->pci_dev->dev, "Cannot allocate memory for instr queue %d\n",
+			iq_no);
+		return 1;
+	}
+
+	iq->max_count = num_descs;
+
+	/* Initialize a list to holds requests that have been posted to Octeon
+	 * but has yet to be fetched by octeon
+	 */
+	iq->request_list = vmalloc(sizeof(*iq->request_list) * num_descs);
+	if (!iq->request_list) {
+		lio_dma_free(oct, q_size, iq->base_addr, iq->base_addr_dma);
+		dev_err(&oct->pci_dev->dev, "Alloc failed for IQ[%d] nr free list\n",
+			iq_no);
+		return 1;
+	}
+
+	memset(iq->request_list, 0, sizeof(*iq->request_list) * num_descs);
+
+	dev_dbg(&oct->pci_dev->dev, "IQ[%d]: base: %p basedma: %llx count: %d\n",
+		iq_no, iq->base_addr, iq->base_addr_dma, iq->max_count);
+
+	iq->iq_no = iq_no;
+	iq->fill_threshold = (u32)conf->db_min;
+	iq->fill_cnt = 0;
+	iq->host_write_index = 0;
+	iq->octeon_read_index = 0;
+	iq->flush_index = 0;
+	iq->last_db_time = 0;
+	iq->do_auto_flush = 1;
+	iq->db_timeout = (u32)conf->db_timeout;
+	atomic_set(&iq->instr_pending, 0);
+
+	/* Initialize the spinlock for this instruction queue */
+	spin_lock_init(&iq->lock);
+
+	oct->io_qmask.iq |= (1 << iq_no);
+
+	/* Set the 32B/64B mode for each input queue */
+	oct->io_qmask.iq64B |= ((conf->instr_type == 64) << iq_no);
+	iq->iqcmd_64B = (conf->instr_type == 64);
+
+	oct->fn_list.setup_iq_regs(oct, iq_no);
+
+	oct->check_db_wq[iq_no].wq = create_workqueue("check_iq_db");
+	if (!oct->check_db_wq[iq_no].wq) {
+		lio_dma_free(oct, q_size, iq->base_addr, iq->base_addr_dma);
+		dev_err(&oct->pci_dev->dev, "check db wq create failed for iq %d\n",
+			iq_no);
+		return 1;
+	}
+
+	db_wq = &oct->check_db_wq[iq_no];
+
+	INIT_DELAYED_WORK(&db_wq->wk.work, check_db_timeout);
+	db_wq->wk.ctxptr = oct;
+	db_wq->wk.ctxul = iq_no;
+	queue_delayed_work(db_wq->wq, &db_wq->wk.work, msecs_to_jiffies(1));
+
+	return 0;
+}
+
+int octeon_delete_instr_queue(struct octeon_device *oct, u32 iq_no)
+{
+	u64 desc_size = 0, q_size;
+	struct octeon_instr_queue *iq = oct->instr_queue[iq_no];
+
+	cancel_delayed_work_sync(&oct->check_db_wq[iq_no].wk.work);
+	flush_workqueue(oct->check_db_wq[iq_no].wq);
+	destroy_workqueue(oct->check_db_wq[iq_no].wq);
+
+	if (OCTEON_CN6XXX(oct))
+		desc_size =
+		    CFG_GET_IQ_INSTR_TYPE(CHIP_FIELD(oct, cn6xxx, conf));
+
+	vfree(iq->request_list);
+
+	if (iq->base_addr) {
+		q_size = iq->max_count * desc_size;
+		lio_dma_free(oct, (u32)q_size, iq->base_addr,
+			     iq->base_addr_dma);
+		return 0;
+	}
+	return 1;
+}
+
+/* Return 0 on success, 1 on failure */
+int octeon_setup_iq(struct octeon_device *oct,
+		    u32 iq_no,
+		    u32 num_descs,
+		    void *app_ctx)
+{
+	if (oct->instr_queue[iq_no]) {
+		dev_dbg(&oct->pci_dev->dev, "IQ is in use. Cannot create the IQ: %d again\n",
+			iq_no);
+		oct->instr_queue[iq_no]->app_ctx = app_ctx;
+		return 0;
+	}
+	oct->instr_queue[iq_no] =
+	    vmalloc(sizeof(struct octeon_instr_queue));
+	if (!oct->instr_queue[iq_no])
+		return 1;
+
+	memset(oct->instr_queue[iq_no], 0,
+	       sizeof(struct octeon_instr_queue));
+
+	oct->instr_queue[iq_no]->app_ctx = app_ctx;
+	if (octeon_init_instr_queue(oct, iq_no, num_descs)) {
+		vfree(oct->instr_queue[iq_no]);
+		oct->instr_queue[iq_no] = NULL;
+		return 1;
+	}
+
+	oct->num_iqs++;
+	oct->fn_list.enable_io_queues(oct);
+	return 0;
+}
+
+int lio_wait_for_instr_fetch(struct octeon_device *oct)
+{
+	int i, retry = 1000, pending, instr_cnt = 0;
+
+	do {
+		instr_cnt = 0;
+
+		/*for (i = 0; i < oct->num_iqs; i++) {*/
+		for (i = 0; i < MAX_OCTEON_INSTR_QUEUES; i++) {
+			if (!(oct->io_qmask.iq & (1UL << i)))
+				continue;
+			pending =
+			    atomic_read(&oct->
+					       instr_queue[i]->instr_pending);
+			if (pending)
+				__check_db_timeout(oct, i);
+			instr_cnt += pending;
+		}
+
+		if (instr_cnt == 0)
+			break;
+
+		schedule_timeout_uninterruptible(1);
+
+	} while (retry-- && instr_cnt);
+
+	return instr_cnt;
+}
+
+static inline void
+ring_doorbell(struct octeon_device *oct, struct octeon_instr_queue *iq)
+{
+	if (atomic_read(&oct->status) == OCT_DEV_RUNNING) {
+		writel(iq->fill_cnt, iq->doorbell_reg);
+		/* make sure doorbell write goes through */
+		mmiowb();
+		iq->fill_cnt = 0;
+		iq->last_db_time = jiffies;
+		return;
+	}
+}
+
+static inline void __copy_cmd_into_iq(struct octeon_instr_queue *iq,
+				      u8 *cmd)
+{
+	u8 *iqptr, cmdsize;
+
+	cmdsize = ((iq->iqcmd_64B) ? 64 : 32);
+	iqptr = iq->base_addr + (cmdsize * iq->host_write_index);
+
+	memcpy(iqptr, cmd, cmdsize);
+}
+
+static inline int
+__post_command(struct octeon_device *octeon_dev __attribute__((unused)),
+	       struct octeon_instr_queue *iq,
+	       u32 force_db __attribute__((unused)), u8 *cmd)
+{
+	u32 index = -1;
+
+	/* This ensures that the read index does not wrap around to the same
+	 * position if queue gets full before Octeon could fetch any instr.
+	 */
+	if (atomic_read(&iq->instr_pending) >= (s32)(iq->max_count - 1))
+		return -1;
+
+	__copy_cmd_into_iq(iq, cmd);
+
+	/* "index" is returned, host_write_index is modified. */
+	index = iq->host_write_index;
+	INCR_INDEX_BY1(iq->host_write_index, iq->max_count);
+	iq->fill_cnt++;
+
+	/* Flush the command into memory. We need to be sure the data is in
+	 * memory before indicating that the instruction is pending.
+	 */
+	wmb();
+
+	atomic_inc(&iq->instr_pending);
+
+	return index;
+}
+
+static inline struct iq_post_status
+__post_command2(struct octeon_device *octeon_dev __attribute__((unused)),
+		struct octeon_instr_queue *iq,
+		u32 force_db __attribute__((unused)), u8 *cmd)
+{
+	struct iq_post_status st;
+
+	st.status = IQ_SEND_OK;
+
+	/* This ensures that the read index does not wrap around to the same
+	 * position if queue gets full before Octeon could fetch any instr.
+	 */
+	if (atomic_read(&iq->instr_pending) >= (s32)(iq->max_count - 1)) {
+		st.status = IQ_SEND_FAILED;
+		st.index = -1;
+		return st;
+	}
+
+	if (atomic_read(&iq->instr_pending) >= (s32)(iq->max_count - 2))
+		st.status = IQ_SEND_STOP;
+
+	__copy_cmd_into_iq(iq, cmd);
+
+	/* "index" is returned, host_write_index is modified. */
+	st.index = iq->host_write_index;
+	INCR_INDEX_BY1(iq->host_write_index, iq->max_count);
+	iq->fill_cnt++;
+
+	/* Flush the command into memory. We need to be sure the data is in
+	 * memory before indicating that the instruction is pending.
+	 */
+	wmb();
+
+	atomic_inc(&iq->instr_pending);
+
+	return st;
+}
+
+int
+octeon_register_reqtype_free_fn(struct octeon_device *oct, int reqtype,
+				void (*fn)(void *))
+{
+	if (reqtype > REQTYPE_LAST) {
+		dev_err(&oct->pci_dev->dev, "%s: Invalid reqtype: %d\n",
+			__func__, reqtype);
+		return -EINVAL;
+	}
+
+	reqtype_free_fn[oct->octeon_id][reqtype] = fn;
+
+	return 0;
+}
+
+static inline void
+__add_to_request_list(struct octeon_instr_queue *iq,
+		      int idx, void *buf, int reqtype)
+{
+	iq->request_list[idx].buf = buf;
+	iq->request_list[idx].reqtype = reqtype;
+}
+
+int
+lio_process_iq_request_list(struct octeon_device *oct,
+			    struct octeon_instr_queue *iq)
+{
+	int reqtype;
+	void *buf;
+	u32 old = iq->flush_index;
+	u32 inst_count = 0;
+	unsigned pkts_compl = 0, bytes_compl = 0;
+	struct octeon_soft_command *sc;
+	struct octeon_instr_irh *irh;
+
+	while (old != iq->octeon_read_index) {
+		reqtype = iq->request_list[old].reqtype;
+		buf     = iq->request_list[old].buf;
+
+		if (reqtype == REQTYPE_NONE)
+			goto skip_this;
+
+		octeon_update_tx_completion_counters(buf, reqtype, &pkts_compl,
+						     &bytes_compl);
+
+		switch (reqtype) {
+		case REQTYPE_NORESP_NET:
+		case REQTYPE_NORESP_NET_SG:
+		case REQTYPE_RESP_NET_SG:
+			reqtype_free_fn[oct->octeon_id][reqtype](buf);
+			break;
+		case REQTYPE_RESP_NET:
+		case REQTYPE_SOFT_COMMAND:
+			sc = buf;
+
+			irh = (struct octeon_instr_irh *)&sc->cmd.irh;
+			if (irh->rflag) {
+				/* We're expecting a response from Octeon.
+				 * It's up to lio_process_ordered_list() to
+				 * process  sc. Add sc to the ordered soft
+				 * command response list because we expect
+				 * a response from Octeon.
+				 */
+				spin_lock_bh(&oct->response_list
+					[OCTEON_ORDERED_SC_LIST].lock);
+				atomic_inc(&oct->response_list
+					[OCTEON_ORDERED_SC_LIST].
+					pending_req_count);
+				list_add_tail(&sc->node, &oct->response_list
+					[OCTEON_ORDERED_SC_LIST].head);
+				spin_unlock_bh(&oct->response_list
+					[OCTEON_ORDERED_SC_LIST].lock);
+			} else {
+				if (sc->callback) {
+					sc->callback(oct, OCTEON_REQUEST_DONE,
+						     sc->callback_arg);
+				}
+			}
+			break;
+		default:
+			dev_err(&oct->pci_dev->dev,
+				"%s Unknown reqtype: %d buf: %p at idx %d\n",
+				__func__, reqtype, buf, old);
+		}
+
+		iq->request_list[old].buf = NULL;
+		iq->request_list[old].reqtype = 0;
+
+ skip_this:
+		inst_count++;
+		INCR_INDEX_BY1(old, iq->max_count);
+	}
+	if (bytes_compl)
+		octeon_report_tx_completion_to_bql(iq->app_ctx, pkts_compl,
+						   bytes_compl);
+	iq->flush_index = old;
+
+	return inst_count;
+}
+
+static inline void
+update_iq_indices(struct octeon_device *oct, struct octeon_instr_queue *iq)
+{
+	u32 inst_processed = 0;
+
+	/* Calculate how many commands Octeon has read and move the read index
+	 * accordingly.
+	 */
+	iq->octeon_read_index = oct->fn_list.update_iq_read_idx(oct, iq);
+
+	/* Move the NORESPONSE requests to the per-device completion list. */
+	if (iq->flush_index != iq->octeon_read_index)
+		inst_processed = lio_process_iq_request_list(oct, iq);
+
+	if (inst_processed) {
+		atomic_sub(inst_processed, &iq->instr_pending);
+		iq->stats.instr_processed += inst_processed;
+	}
+}
+
+static void
+octeon_flush_iq(struct octeon_device *oct, struct octeon_instr_queue *iq,
+		u32 pending_thresh)
+{
+	if (atomic_read(&iq->instr_pending) >= (s32)pending_thresh) {
+		spin_lock_bh(&iq->lock);
+		update_iq_indices(oct, iq);
+		spin_unlock_bh(&iq->lock);
+	}
+}
+
+static void __check_db_timeout(struct octeon_device *oct, unsigned long iq_no)
+{
+	struct octeon_instr_queue *iq;
+	u64 next_time;
+
+	if (!oct)
+		return;
+	iq = oct->instr_queue[iq_no];
+	if (!iq)
+		return;
+
+	/* If jiffies - last_db_time < db_timeout do nothing  */
+	next_time = iq->last_db_time + iq->db_timeout;
+	if (!time_after(jiffies, (unsigned long)next_time))
+		return;
+	iq->last_db_time = jiffies;
+
+	/* Get the lock and prevent tasklets. This routine gets called from
+	 * the poll thread. Instructions can now be posted in tasklet context
+	 */
+	spin_lock_bh(&iq->lock);
+	if (iq->fill_cnt != 0)
+		ring_doorbell(oct, iq);
+
+	spin_unlock_bh(&iq->lock);
+
+	/* Flush the instruction queue */
+	if (iq->do_auto_flush)
+		octeon_flush_iq(oct, iq, 1);
+}
+
+/* Called by the Poll thread at regular intervals to check the instruction
+ * queue for commands to be posted and for commands that were fetched by Octeon.
+ */
+static void check_db_timeout(struct work_struct *work)
+{
+	struct cavium_wk *wk = (struct cavium_wk *)work;
+	struct octeon_device *oct = (struct octeon_device *)wk->ctxptr;
+	unsigned long iq_no = wk->ctxul;
+	struct cavium_wq *db_wq = &oct->check_db_wq[iq_no];
+
+	__check_db_timeout(oct, iq_no);
+	queue_delayed_work(db_wq->wq, &db_wq->wk.work, msecs_to_jiffies(1));
+}
+
+int
+octeon_send_command(struct octeon_device *oct, u32 iq_no,
+		    u32 force_db, void *cmd, void *buf,
+		    u32 datasize, u32 reqtype)
+{
+	struct iq_post_status st;
+	struct octeon_instr_queue *iq = oct->instr_queue[iq_no];
+
+	spin_lock_bh(&iq->lock);
+
+	st = __post_command2(oct, iq, force_db, cmd);
+
+	if (st.status != IQ_SEND_FAILED) {
+		octeon_report_sent_bytes_to_bql(buf, reqtype);
+		__add_to_request_list(iq, st.index, buf, reqtype);
+		INCR_INSTRQUEUE_PKT_COUNT(oct, iq_no, bytes_sent, datasize);
+		INCR_INSTRQUEUE_PKT_COUNT(oct, iq_no, instr_posted, 1);
+
+		if (iq->fill_cnt >= iq->fill_threshold || force_db)
+			ring_doorbell(oct, iq);
+	} else {
+		INCR_INSTRQUEUE_PKT_COUNT(oct, iq_no, instr_dropped, 1);
+	}
+
+	spin_unlock_bh(&iq->lock);
+
+	if (iq->do_auto_flush)
+		octeon_flush_iq(oct, iq, 2);
+
+	return st.status;
+}
+
+void
+octeon_prepare_soft_command(struct octeon_device *oct,
+			    struct octeon_soft_command *sc,
+			    u8 opcode,
+			    u8 subcode,
+			    u32 irh_ossp,
+			    u64 ossp0,
+			    u64 ossp1)
+{
+	struct octeon_config *oct_cfg;
+	struct octeon_instr_ih *ih;
+	struct octeon_instr_irh *irh;
+	struct octeon_instr_rdp *rdp;
+
+	BUG_ON(opcode > 15);
+	BUG_ON(subcode > 127);
+
+	oct_cfg = octeon_get_conf(oct);
+
+	ih          = (struct octeon_instr_ih *)&sc->cmd.ih;
+	ih->tagtype = ATOMIC_TAG;
+	ih->tag     = LIO_CONTROL;
+	ih->raw     = 1;
+	ih->grp     = CFG_GET_CTRL_Q_GRP(oct_cfg);
+
+	if (sc->datasize) {
+		ih->dlengsz = sc->datasize;
+		ih->rs = 1;
+	}
+
+	irh            = (struct octeon_instr_irh *)&sc->cmd.irh;
+	irh->opcode    = opcode;
+	irh->subcode   = subcode;
+
+	/* opcode/subcode specific parameters (ossp) */
+	irh->ossp       = irh_ossp;
+	sc->cmd.ossp[0] = ossp0;
+	sc->cmd.ossp[1] = ossp1;
+
+	if (sc->rdatasize) {
+		rdp            = (struct octeon_instr_rdp *)&sc->cmd.rdp;
+		rdp->pcie_port = oct->pcie_port;
+		rdp->rlen      = sc->rdatasize;
+
+		irh->rflag =  1;
+		irh->len   =  4;
+		ih->fsz    = 40; /* irh+ossp[0]+ossp[1]+rdp+rptr = 40 bytes */
+	} else {
+		irh->rflag =  0;
+		irh->len   =  2;
+		ih->fsz    = 24; /* irh + ossp[0] + ossp[1] = 24 bytes */
+	}
+
+	while (!(oct->io_qmask.iq & (1 << sc->iq_no)))
+		sc->iq_no++;
+}
+
+int octeon_send_soft_command(struct octeon_device *oct,
+			     struct octeon_soft_command *sc)
+{
+	struct octeon_instr_ih *ih;
+	struct octeon_instr_irh *irh;
+	struct octeon_instr_rdp *rdp;
+
+	ih = (struct octeon_instr_ih *)&sc->cmd.ih;
+	if (ih->dlengsz) {
+		BUG_ON(!sc->dmadptr);
+		sc->cmd.dptr = sc->dmadptr;
+	}
+
+	irh = (struct octeon_instr_irh *)&sc->cmd.irh;
+	if (irh->rflag) {
+		BUG_ON(!sc->dmarptr);
+		BUG_ON(!sc->status_word);
+		*sc->status_word = COMPLETION_WORD_INIT;
+
+		rdp = (struct octeon_instr_rdp *)&sc->cmd.rdp;
+
+		sc->cmd.rptr = sc->dmarptr;
+	}
+
+	if (sc->wait_time)
+		sc->timeout = jiffies + sc->wait_time;
+
+	return octeon_send_command(oct, sc->iq_no, 1, &sc->cmd, sc,
+				   (u32)ih->dlengsz, REQTYPE_SOFT_COMMAND);
+}
+
+int octeon_setup_sc_buffer_pool(struct octeon_device *oct)
+{
+	int i;
+	u64 dma_addr;
+	struct octeon_soft_command *sc;
+
+	INIT_LIST_HEAD(&oct->sc_buf_pool.head);
+	spin_lock_init(&oct->sc_buf_pool.lock);
+	atomic_set(&oct->sc_buf_pool.alloc_buf_count, 0);
+
+	for (i = 0; i < MAX_SOFT_COMMAND_BUFFERS; i++) {
+		sc = (struct octeon_soft_command *)
+			lio_dma_alloc(oct,
+				      SOFT_COMMAND_BUFFER_SIZE,
+					  (dma_addr_t *)&dma_addr);
+		if (!sc)
+			return 1;
+
+		sc->dma_addr = dma_addr;
+		sc->size = SOFT_COMMAND_BUFFER_SIZE;
+
+		list_add_tail(&sc->node, &oct->sc_buf_pool.head);
+	}
+
+	return 0;
+}
+
+int octeon_free_sc_buffer_pool(struct octeon_device *oct)
+{
+	struct list_head *tmp, *tmp2;
+	struct octeon_soft_command *sc;
+
+	spin_lock(&oct->sc_buf_pool.lock);
+
+	list_for_each_safe(tmp, tmp2, &oct->sc_buf_pool.head) {
+		list_del(tmp);
+
+		sc = (struct octeon_soft_command *)tmp;
+
+		lio_dma_free(oct, sc->size, sc, sc->dma_addr);
+	}
+
+	INIT_LIST_HEAD(&oct->sc_buf_pool.head);
+
+	spin_unlock(&oct->sc_buf_pool.lock);
+
+	return 0;
+}
+
+struct octeon_soft_command *octeon_alloc_soft_command(struct octeon_device *oct,
+						      u32 datasize,
+						      u32 rdatasize,
+						      u32 ctxsize)
+{
+	u64 dma_addr;
+	u32 size;
+	u32 offset = sizeof(struct octeon_soft_command);
+	struct octeon_soft_command *sc = NULL;
+	struct list_head *tmp;
+
+	BUG_ON((offset + datasize + rdatasize + ctxsize) >
+	       SOFT_COMMAND_BUFFER_SIZE);
+
+	spin_lock(&oct->sc_buf_pool.lock);
+
+	if (list_empty(&oct->sc_buf_pool.head)) {
+		spin_unlock(&oct->sc_buf_pool.lock);
+		return NULL;
+	}
+
+	list_for_each(tmp, &oct->sc_buf_pool.head)
+		break;
+
+	list_del(tmp);
+
+	atomic_inc(&oct->sc_buf_pool.alloc_buf_count);
+
+	spin_unlock(&oct->sc_buf_pool.lock);
+
+	sc = (struct octeon_soft_command *)tmp;
+
+	dma_addr = sc->dma_addr;
+	size = sc->size;
+
+	memset(sc, 0, sc->size);
+
+	sc->dma_addr = dma_addr;
+	sc->size = size;
+
+	if (ctxsize) {
+		sc->ctxptr = (u8 *)sc + offset;
+		sc->ctxsize = ctxsize;
+	}
+
+	/* Start data at 128 byte boundary */
+	offset = (offset + ctxsize + 127) & 0xffffff80;
+
+	if (datasize) {
+		sc->virtdptr = (u8 *)sc + offset;
+		sc->dmadptr = dma_addr + offset;
+		sc->datasize = datasize;
+	}
+
+	/* Start rdata at 128 byte boundary */
+	offset = (offset + datasize + 127) & 0xffffff80;
+
+	if (rdatasize) {
+		BUG_ON(rdatasize < 16);
+		sc->virtrptr = (u8 *)sc + offset;
+		sc->dmarptr = dma_addr + offset;
+		sc->rdatasize = rdatasize;
+		sc->status_word = (u64 *)((u8 *)(sc->virtrptr) + rdatasize - 8);
+	}
+
+	return sc;
+}
+
+void octeon_free_soft_command(struct octeon_device *oct,
+			      struct octeon_soft_command *sc)
+{
+	spin_lock(&oct->sc_buf_pool.lock);
+
+	list_add_tail(&sc->node, &oct->sc_buf_pool.head);
+
+	atomic_dec(&oct->sc_buf_pool.alloc_buf_count);
+
+	spin_unlock(&oct->sc_buf_pool.lock);
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/response_manager.c b/drivers/net/ethernet/cavium/liquidio/response_manager.c
new file mode 100644
index 000000000000..091f537a946e
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/response_manager.c
@@ -0,0 +1,178 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include "octeon_config.h"
+#include "liquidio_common.h"
+#include "octeon_droq.h"
+#include "octeon_iq.h"
+#include "response_manager.h"
+#include "octeon_device.h"
+#include "octeon_nic.h"
+#include "octeon_main.h"
+#include "octeon_network.h"
+#include "cn66xx_regs.h"
+#include "cn66xx_device.h"
+#include "cn68xx_regs.h"
+#include "cn68xx_device.h"
+#include "liquidio_image.h"
+
+static void oct_poll_req_completion(struct work_struct *work);
+
+int octeon_setup_response_list(struct octeon_device *oct)
+{
+	int i, ret = 0;
+	struct cavium_wq *cwq;
+
+	for (i = 0; i < MAX_RESPONSE_LISTS; i++) {
+		INIT_LIST_HEAD(&oct->response_list[i].head);
+		spin_lock_init(&oct->response_list[i].lock);
+		atomic_set(&oct->response_list[i].pending_req_count, 0);
+	}
+
+	oct->dma_comp_wq.wq = create_workqueue("dma-comp");
+	if (!oct->dma_comp_wq.wq) {
+		dev_err(&oct->pci_dev->dev, "failed to create wq thread\n");
+		return -ENOMEM;
+	}
+
+	cwq = &oct->dma_comp_wq;
+	INIT_DELAYED_WORK(&cwq->wk.work, oct_poll_req_completion);
+	cwq->wk.ctxptr = oct;
+	queue_delayed_work(cwq->wq, &cwq->wk.work, msecs_to_jiffies(100));
+
+	return ret;
+}
+
+void octeon_delete_response_list(struct octeon_device *oct)
+{
+	cancel_delayed_work_sync(&oct->dma_comp_wq.wk.work);
+	flush_workqueue(oct->dma_comp_wq.wq);
+	destroy_workqueue(oct->dma_comp_wq.wq);
+}
+
+int lio_process_ordered_list(struct octeon_device *octeon_dev,
+			     u32 force_quit)
+{
+	struct octeon_response_list *ordered_sc_list;
+	struct octeon_soft_command *sc;
+	int request_complete = 0;
+	int resp_to_process = MAX_ORD_REQS_TO_PROCESS;
+	u32 status;
+	u64 status64;
+	struct octeon_instr_rdp *rdp;
+
+	ordered_sc_list = &octeon_dev->response_list[OCTEON_ORDERED_SC_LIST];
+
+	do {
+		spin_lock_bh(&ordered_sc_list->lock);
+
+		if (ordered_sc_list->head.next == &ordered_sc_list->head) {
+			/* ordered_sc_list is empty; there is
+			 * nothing to process
+			 */
+			spin_unlock_bh
+			    (&ordered_sc_list->lock);
+			return 1;
+		}
+
+		sc = (struct octeon_soft_command *)ordered_sc_list->
+		    head.next;
+		rdp = (struct octeon_instr_rdp *)&sc->cmd.rdp;
+
+		status = OCTEON_REQUEST_PENDING;
+
+		/* check if octeon has finished DMA'ing a response
+		 * to where rptr is pointing to
+		 */
+		dma_sync_single_for_cpu(&octeon_dev->pci_dev->dev,
+					sc->cmd.rptr, rdp->rlen,
+					DMA_FROM_DEVICE);
+		status64 = *sc->status_word;
+
+		if (status64 != COMPLETION_WORD_INIT) {
+			if ((status64 & 0xff) != 0xff) {
+				octeon_swap_8B_data(&status64, 1);
+				if (((status64 & 0xff) != 0xff)) {
+					status = (u32)(status64 &
+						       0xffffffffULL);
+				}
+			}
+		} else if (force_quit || (sc->timeout &&
+			time_after(jiffies, (unsigned long)sc->timeout))) {
+			status = OCTEON_REQUEST_TIMEOUT;
+		}
+
+		if (status != OCTEON_REQUEST_PENDING) {
+			/* we have received a response or we have timed out */
+			/* remove node from linked list */
+			list_del(&sc->node);
+			atomic_dec(&octeon_dev->response_list
+					  [OCTEON_ORDERED_SC_LIST].
+					  pending_req_count);
+			spin_unlock_bh
+			    (&ordered_sc_list->lock);
+
+			if (sc->callback)
+				sc->callback(octeon_dev, status,
+					     sc->callback_arg);
+
+			request_complete++;
+
+		} else {
+			/* no response yet */
+			request_complete = 0;
+			spin_unlock_bh
+			    (&ordered_sc_list->lock);
+		}
+
+		/* If we hit the Max Ordered requests to process every loop,
+		 * we quit
+		 * and let this function be invoked the next time the poll
+		 * thread runs
+		 * to process the remaining requests. This function can take up
+		 * the entire CPU if there is no upper limit to the requests
+		 * processed.
+		 */
+		if (request_complete >= resp_to_process)
+			break;
+	} while (request_complete);
+
+	return 0;
+}
+
+static void oct_poll_req_completion(struct work_struct *work)
+{
+	struct cavium_wk *wk = (struct cavium_wk *)work;
+	struct octeon_device *oct = (struct octeon_device *)wk->ctxptr;
+	struct cavium_wq *cwq = &oct->dma_comp_wq;
+
+	lio_process_ordered_list(oct, 0);
+
+	queue_delayed_work(cwq->wq, &cwq->wk.work, msecs_to_jiffies(100));
+}
diff --git a/drivers/net/ethernet/cavium/liquidio/response_manager.h b/drivers/net/ethernet/cavium/liquidio/response_manager.h
new file mode 100644
index 000000000000..7a48752dcb10
--- /dev/null
+++ b/drivers/net/ethernet/cavium/liquidio/response_manager.h
@@ -0,0 +1,140 @@
+/**********************************************************************
+ * Author: Cavium, Inc.
+ *
+ * Contact: support@cavium.com
+ *          Please include "LiquidIO" in the subject.
+ *
+ * Copyright (c) 2003-2015 Cavium, Inc.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License, Version 2, as
+ * published by the Free Software Foundation.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
+ * NONINFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ *
+ * This file may also be available under a different license from Cavium.
+ * Contact Cavium, Inc. for more information
+ **********************************************************************/
+
+/*! \file response_manager.h
+ *  \brief Host Driver:  Response queues for host instructions.
+ */
+
+#ifndef __RESPONSE_MANAGER_H__
+#define __RESPONSE_MANAGER_H__
+
+/** Maximum ordered requests to process in every invocation of
+ * lio_process_ordered_list(). The function will continue to process requests
+ * as long as it can find one that has finished processing. If it keeps
+ * finding requests that have completed, the function can run for ever. The
+ * value defined here sets an upper limit on the number of requests it can
+ * process before it returns control to the poll thread.
+ */
+#define  MAX_ORD_REQS_TO_PROCESS   4096
+
+/** Head of a response list. There are several response lists in the
+ *  system. One for each response order- Unordered, ordered
+ *  and 1 for noresponse entries on each instruction queue.
+ */
+struct octeon_response_list {
+	/** List structure to add delete pending entries to */
+	struct list_head head;
+
+	/** A lock for this response list */
+	spinlock_t lock;
+
+	atomic_t pending_req_count;
+};
+
+/** The type of response list.
+ */
+enum {
+	OCTEON_ORDERED_LIST = 0,
+	OCTEON_UNORDERED_NONBLOCKING_LIST = 1,
+	OCTEON_UNORDERED_BLOCKING_LIST = 2,
+	OCTEON_ORDERED_SC_LIST = 3
+};
+
+/** Response Order values for a Octeon Request. */
+enum {
+	OCTEON_RESP_ORDERED = 0,
+	OCTEON_RESP_UNORDERED = 1,
+	OCTEON_RESP_NORESPONSE = 2
+};
+
+/** Error codes  used in Octeon Host-Core communication.
+ *
+ *   31            16 15            0
+ *   ---------------------------------
+ *   |               |               |
+ *   ---------------------------------
+ *   Error codes are 32-bit wide. The upper 16-bits, called Major Error Number,
+ *   are reserved to identify the group to which the error code belongs. The
+ *   lower 16-bits, called Minor Error Number, carry the actual code.
+ *
+ *   So error codes are (MAJOR NUMBER << 16)| MINOR_NUMBER.
+ */
+
+/*------------   Error codes used by host driver   -----------------*/
+#define DRIVER_MAJOR_ERROR_CODE           0x0000
+
+/**  A value of 0x00000000 indicates no error i.e. success */
+#define DRIVER_ERROR_NONE                 0x00000000
+
+/**  (Major number: 0x0000; Minor Number: 0x0001) */
+#define DRIVER_ERROR_REQ_PENDING          0x00000001
+#define DRIVER_ERROR_REQ_TIMEOUT          0x00000003
+#define DRIVER_ERROR_REQ_EINTR            0x00000004
+#define DRIVER_ERROR_REQ_ENXIO            0x00000006
+#define DRIVER_ERROR_REQ_ENOMEM           0x0000000C
+#define DRIVER_ERROR_REQ_EINVAL           0x00000016
+#define DRIVER_ERROR_REQ_FAILED           0x000000ff
+
+/** Status for a request.
+ * If a request is not queued to Octeon by the driver, the driver returns
+ * an error condition that's describe by one of the OCTEON_REQ_ERR_* value
+ * below. If the request is successfully queued, the driver will return
+ * a OCTEON_REQUEST_PENDING status. OCTEON_REQUEST_TIMEOUT and
+ * OCTEON_REQUEST_INTERRUPTED are only returned by the driver if the
+ * response for request failed to arrive before a time-out period or if
+ * the request processing * got interrupted due to a signal respectively.
+ */
+enum {
+	OCTEON_REQUEST_DONE = (DRIVER_ERROR_NONE),
+	OCTEON_REQUEST_PENDING = (DRIVER_ERROR_REQ_PENDING),
+	OCTEON_REQUEST_TIMEOUT = (DRIVER_ERROR_REQ_TIMEOUT),
+	OCTEON_REQUEST_INTERRUPTED = (DRIVER_ERROR_REQ_EINTR),
+	OCTEON_REQUEST_NO_DEVICE = (0x00000021),
+	OCTEON_REQUEST_NOT_RUNNING,
+	OCTEON_REQUEST_INVALID_IQ,
+	OCTEON_REQUEST_INVALID_BUFCNT,
+	OCTEON_REQUEST_INVALID_RESP_ORDER,
+	OCTEON_REQUEST_NO_MEMORY,
+	OCTEON_REQUEST_INVALID_BUFSIZE,
+	OCTEON_REQUEST_NO_PENDING_ENTRY,
+	OCTEON_REQUEST_NO_IQ_SPACE = (0x7FFFFFFF)
+
+};
+
+/** Initialize the response lists. The number of response lists to create is
+ * given by count.
+ * @param octeon_dev      - the octeon device structure.
+ */
+int octeon_setup_response_list(struct octeon_device *octeon_dev);
+
+void octeon_delete_response_list(struct octeon_device *octeon_dev);
+
+/** Check the status of first entry in the ordered list. If the instruction at
+ * that entry finished processing or has timed-out, the entry is cleaned.
+ * @param octeon_dev  - the octeon device structure.
+ * @param force_quit - the request is forced to timeout if this is 1
+ * @return 1 if the ordered list is empty, 0 otherwise.
+ */
+int lio_process_ordered_list(struct octeon_device *octeon_dev,
+			     u32 force_quit);
+
+#endif
diff --git a/drivers/net/ethernet/cavium/thunder/Makefile b/drivers/net/ethernet/cavium/thunder/Makefile
new file mode 100644
index 000000000000..5c4615ccaa14
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for Cavium's Thunder ethernet device
+#
+
+obj-$(CONFIG_THUNDER_NIC_BGX) += thunder_bgx.o
+obj-$(CONFIG_THUNDER_NIC_PF) += nicpf.o
+obj-$(CONFIG_THUNDER_NIC_VF) += nicvf.o
+
+nicpf-y := nic_main.o
+nicvf-y := nicvf_main.o nicvf_queues.o
+nicvf-y += nicvf_ethtool.o
diff --git a/drivers/net/ethernet/cavium/thunder/nic.h b/drivers/net/ethernet/cavium/thunder/nic.h
new file mode 100644
index 000000000000..dda8a02b7322
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/nic.h
@@ -0,0 +1,423 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * 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.
+ */
+
+#ifndef NIC_H
+#define	NIC_H
+
+#include <linux/netdevice.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include "thunder_bgx.h"
+
+/* PCI device IDs */
+#define	PCI_DEVICE_ID_THUNDER_NIC_PF		0xA01E
+#define	PCI_DEVICE_ID_THUNDER_PASS1_NIC_VF	0x0011
+#define	PCI_DEVICE_ID_THUNDER_NIC_VF		0xA034
+#define	PCI_DEVICE_ID_THUNDER_BGX		0xA026
+
+/* PCI BAR nos */
+#define	PCI_CFG_REG_BAR_NUM		0
+#define	PCI_MSIX_REG_BAR_NUM		4
+
+/* NIC SRIOV VF count */
+#define	MAX_NUM_VFS_SUPPORTED		128
+#define	DEFAULT_NUM_VF_ENABLED		8
+
+#define	NIC_TNS_BYPASS_MODE		0
+#define	NIC_TNS_MODE			1
+
+/* NIC priv flags */
+#define	NIC_SRIOV_ENABLED		BIT(0)
+
+/* Min/Max packet size */
+#define	NIC_HW_MIN_FRS			64
+#define	NIC_HW_MAX_FRS			9200 /* 9216 max packet including FCS */
+
+/* Max pkinds */
+#define	NIC_MAX_PKIND			16
+
+/* Rx Channels */
+/* Receive channel configuration in TNS bypass mode
+ * Below is configuration in TNS bypass mode
+ * BGX0-LMAC0-CHAN0 - VNIC CHAN0
+ * BGX0-LMAC1-CHAN0 - VNIC CHAN16
+ * ...
+ * BGX1-LMAC0-CHAN0 - VNIC CHAN128
+ * ...
+ * BGX1-LMAC3-CHAN0 - VNIC CHAN174
+ */
+#define	NIC_INTF_COUNT			2  /* Interfaces btw VNIC and TNS/BGX */
+#define	NIC_CHANS_PER_INF		128
+#define	NIC_MAX_CHANS			(NIC_INTF_COUNT * NIC_CHANS_PER_INF)
+#define	NIC_CPI_COUNT			2048 /* No of channel parse indices */
+
+/* TNS bypass mode: 1-1 mapping between VNIC and BGX:LMAC */
+#define NIC_MAX_BGX			MAX_BGX_PER_CN88XX
+#define	NIC_CPI_PER_BGX			(NIC_CPI_COUNT / NIC_MAX_BGX)
+#define	NIC_MAX_CPI_PER_LMAC		64 /* Max when CPI_ALG is IP diffserv */
+#define	NIC_RSSI_PER_BGX		(NIC_RSSI_COUNT / NIC_MAX_BGX)
+
+/* Tx scheduling */
+#define	NIC_MAX_TL4			1024
+#define	NIC_MAX_TL4_SHAPERS		256 /* 1 shaper for 4 TL4s */
+#define	NIC_MAX_TL3			256
+#define	NIC_MAX_TL3_SHAPERS		64  /* 1 shaper for 4 TL3s */
+#define	NIC_MAX_TL2			64
+#define	NIC_MAX_TL2_SHAPERS		2  /* 1 shaper for 32 TL2s */
+#define	NIC_MAX_TL1			2
+
+/* TNS bypass mode */
+#define	NIC_TL2_PER_BGX			32
+#define	NIC_TL4_PER_BGX			(NIC_MAX_TL4 / NIC_MAX_BGX)
+#define	NIC_TL4_PER_LMAC		(NIC_MAX_TL4 / NIC_CHANS_PER_INF)
+
+/* NIC VF Interrupts */
+#define	NICVF_INTR_CQ			0
+#define	NICVF_INTR_SQ			1
+#define	NICVF_INTR_RBDR			2
+#define	NICVF_INTR_PKT_DROP		3
+#define	NICVF_INTR_TCP_TIMER		4
+#define	NICVF_INTR_MBOX			5
+#define	NICVF_INTR_QS_ERR		6
+
+#define	NICVF_INTR_CQ_SHIFT		0
+#define	NICVF_INTR_SQ_SHIFT		8
+#define	NICVF_INTR_RBDR_SHIFT		16
+#define	NICVF_INTR_PKT_DROP_SHIFT	20
+#define	NICVF_INTR_TCP_TIMER_SHIFT	21
+#define	NICVF_INTR_MBOX_SHIFT		22
+#define	NICVF_INTR_QS_ERR_SHIFT		23
+
+#define	NICVF_INTR_CQ_MASK		(0xFF << NICVF_INTR_CQ_SHIFT)
+#define	NICVF_INTR_SQ_MASK		(0xFF << NICVF_INTR_SQ_SHIFT)
+#define	NICVF_INTR_RBDR_MASK		(0x03 << NICVF_INTR_RBDR_SHIFT)
+#define	NICVF_INTR_PKT_DROP_MASK	BIT(NICVF_INTR_PKT_DROP_SHIFT)
+#define	NICVF_INTR_TCP_TIMER_MASK	BIT(NICVF_INTR_TCP_TIMER_SHIFT)
+#define	NICVF_INTR_MBOX_MASK		BIT(NICVF_INTR_MBOX_SHIFT)
+#define	NICVF_INTR_QS_ERR_MASK		BIT(NICVF_INTR_QS_ERR_SHIFT)
+
+/* MSI-X interrupts */
+#define	NIC_PF_MSIX_VECTORS		10
+#define	NIC_VF_MSIX_VECTORS		20
+
+#define NIC_PF_INTR_ID_ECC0_SBE		0
+#define NIC_PF_INTR_ID_ECC0_DBE		1
+#define NIC_PF_INTR_ID_ECC1_SBE		2
+#define NIC_PF_INTR_ID_ECC1_DBE		3
+#define NIC_PF_INTR_ID_ECC2_SBE		4
+#define NIC_PF_INTR_ID_ECC2_DBE		5
+#define NIC_PF_INTR_ID_ECC3_SBE		6
+#define NIC_PF_INTR_ID_ECC3_DBE		7
+#define NIC_PF_INTR_ID_MBOX0		8
+#define NIC_PF_INTR_ID_MBOX1		9
+
+/* Global timer for CQ timer thresh interrupts
+ * Calculated for SCLK of 700Mhz
+ * value written should be a 1/16th of what is expected
+ *
+ * 1 tick per 0.05usec = value of 2.2
+ * This 10% would be covered in CQ timer thresh value
+ */
+#define NICPF_CLK_PER_INT_TICK		2
+
+struct nicvf_cq_poll {
+	u8	cq_idx;		/* Completion queue index */
+	struct	napi_struct napi;
+};
+
+#define	NIC_RSSI_COUNT			4096 /* Total no of RSS indices */
+#define NIC_MAX_RSS_HASH_BITS		8
+#define NIC_MAX_RSS_IDR_TBL_SIZE	(1 << NIC_MAX_RSS_HASH_BITS)
+#define RSS_HASH_KEY_SIZE		5 /* 320 bit key */
+
+struct nicvf_rss_info {
+	bool enable;
+#define	RSS_L2_EXTENDED_HASH_ENA	BIT(0)
+#define	RSS_IP_HASH_ENA			BIT(1)
+#define	RSS_TCP_HASH_ENA		BIT(2)
+#define	RSS_TCP_SYN_DIS			BIT(3)
+#define	RSS_UDP_HASH_ENA		BIT(4)
+#define RSS_L4_EXTENDED_HASH_ENA	BIT(5)
+#define	RSS_ROCE_ENA			BIT(6)
+#define	RSS_L3_BI_DIRECTION_ENA		BIT(7)
+#define	RSS_L4_BI_DIRECTION_ENA		BIT(8)
+	u64 cfg;
+	u8  hash_bits;
+	u16 rss_size;
+	u8  ind_tbl[NIC_MAX_RSS_IDR_TBL_SIZE];
+	u64 key[RSS_HASH_KEY_SIZE];
+} ____cacheline_aligned_in_smp;
+
+enum rx_stats_reg_offset {
+	RX_OCTS = 0x0,
+	RX_UCAST = 0x1,
+	RX_BCAST = 0x2,
+	RX_MCAST = 0x3,
+	RX_RED = 0x4,
+	RX_RED_OCTS = 0x5,
+	RX_ORUN = 0x6,
+	RX_ORUN_OCTS = 0x7,
+	RX_FCS = 0x8,
+	RX_L2ERR = 0x9,
+	RX_DRP_BCAST = 0xa,
+	RX_DRP_MCAST = 0xb,
+	RX_DRP_L3BCAST = 0xc,
+	RX_DRP_L3MCAST = 0xd,
+	RX_STATS_ENUM_LAST,
+};
+
+enum tx_stats_reg_offset {
+	TX_OCTS = 0x0,
+	TX_UCAST = 0x1,
+	TX_BCAST = 0x2,
+	TX_MCAST = 0x3,
+	TX_DROP = 0x4,
+	TX_STATS_ENUM_LAST,
+};
+
+struct nicvf_hw_stats {
+	u64 rx_bytes_ok;
+	u64 rx_ucast_frames_ok;
+	u64 rx_bcast_frames_ok;
+	u64 rx_mcast_frames_ok;
+	u64 rx_fcs_errors;
+	u64 rx_l2_errors;
+	u64 rx_drop_red;
+	u64 rx_drop_red_bytes;
+	u64 rx_drop_overrun;
+	u64 rx_drop_overrun_bytes;
+	u64 rx_drop_bcast;
+	u64 rx_drop_mcast;
+	u64 rx_drop_l3_bcast;
+	u64 rx_drop_l3_mcast;
+	u64 tx_bytes_ok;
+	u64 tx_ucast_frames_ok;
+	u64 tx_bcast_frames_ok;
+	u64 tx_mcast_frames_ok;
+	u64 tx_drops;
+};
+
+struct nicvf_drv_stats {
+	/* Rx */
+	u64 rx_frames_ok;
+	u64 rx_frames_64;
+	u64 rx_frames_127;
+	u64 rx_frames_255;
+	u64 rx_frames_511;
+	u64 rx_frames_1023;
+	u64 rx_frames_1518;
+	u64 rx_frames_jumbo;
+	u64 rx_drops;
+	/* Tx */
+	u64 tx_frames_ok;
+	u64 tx_drops;
+	u64 tx_busy;
+	u64 tx_tso;
+};
+
+struct nicvf {
+	struct net_device	*netdev;
+	struct pci_dev		*pdev;
+	u8			vf_id;
+	u8			node;
+	u8			tns_mode;
+	u16			mtu;
+	struct queue_set	*qs;
+	void __iomem		*reg_base;
+	bool			link_up;
+	u8			duplex;
+	u32			speed;
+	struct page		*rb_page;
+	u32			rb_page_offset;
+	bool			rb_alloc_fail;
+	bool			rb_work_scheduled;
+	struct delayed_work	rbdr_work;
+	struct tasklet_struct	rbdr_task;
+	struct tasklet_struct	qs_err_task;
+	struct tasklet_struct	cq_task;
+	struct nicvf_cq_poll	*napi[8];
+	struct nicvf_rss_info	rss_info;
+	u8			cpi_alg;
+	/* Interrupt coalescing settings */
+	u32			cq_coalesce_usecs;
+
+	u32			msg_enable;
+	struct nicvf_hw_stats   stats;
+	struct nicvf_drv_stats  drv_stats;
+	struct bgx_stats	bgx_stats;
+	struct work_struct	reset_task;
+
+	/* MSI-X  */
+	bool			msix_enabled;
+	u8			num_vec;
+	struct msix_entry	msix_entries[NIC_VF_MSIX_VECTORS];
+	char			irq_name[NIC_VF_MSIX_VECTORS][20];
+	bool			irq_allocated[NIC_VF_MSIX_VECTORS];
+
+	bool			pf_ready_to_rcv_msg;
+	bool			pf_acked;
+	bool			pf_nacked;
+	bool			bgx_stats_acked;
+	bool			set_mac_pending;
+} ____cacheline_aligned_in_smp;
+
+/* PF <--> VF Mailbox communication
+ * Eight 64bit registers are shared between PF and VF.
+ * Separate set for each VF.
+ * Writing '1' into last register mbx7 means end of message.
+ */
+
+/* PF <--> VF mailbox communication */
+#define	NIC_PF_VF_MAILBOX_SIZE		2
+#define	NIC_MBOX_MSG_TIMEOUT		2000 /* ms */
+
+/* Mailbox message types */
+#define	NIC_MBOX_MSG_READY		0x01	/* Is PF ready to rcv msgs */
+#define	NIC_MBOX_MSG_ACK		0x02	/* ACK the message received */
+#define	NIC_MBOX_MSG_NACK		0x03	/* NACK the message received */
+#define	NIC_MBOX_MSG_QS_CFG		0x04	/* Configure Qset */
+#define	NIC_MBOX_MSG_RQ_CFG		0x05	/* Configure receive queue */
+#define	NIC_MBOX_MSG_SQ_CFG		0x06	/* Configure Send queue */
+#define	NIC_MBOX_MSG_RQ_DROP_CFG	0x07	/* Configure receive queue */
+#define	NIC_MBOX_MSG_SET_MAC		0x08	/* Add MAC ID to DMAC filter */
+#define	NIC_MBOX_MSG_SET_MAX_FRS	0x09	/* Set max frame size */
+#define	NIC_MBOX_MSG_CPI_CFG		0x0A	/* Config CPI, RSSI */
+#define	NIC_MBOX_MSG_RSS_SIZE		0x0B	/* Get RSS indir_tbl size */
+#define	NIC_MBOX_MSG_RSS_CFG		0x0C	/* Config RSS table */
+#define	NIC_MBOX_MSG_RSS_CFG_CONT	0x0D	/* RSS config continuation */
+#define	NIC_MBOX_MSG_RQ_BP_CFG		0x0E	/* RQ backpressure config */
+#define	NIC_MBOX_MSG_RQ_SW_SYNC		0x0F	/* Flush inflight pkts to RQ */
+#define	NIC_MBOX_MSG_BGX_STATS		0x10	/* Get stats from BGX */
+#define	NIC_MBOX_MSG_BGX_LINK_CHANGE	0x11	/* BGX:LMAC link status */
+#define NIC_MBOX_MSG_CFG_DONE		0x12	/* VF configuration done */
+#define NIC_MBOX_MSG_SHUTDOWN		0x13	/* VF is being shutdown */
+
+struct nic_cfg_msg {
+	u8    msg;
+	u8    vf_id;
+	u8    tns_mode;
+	u8    node_id;
+	u8    mac_addr[ETH_ALEN];
+};
+
+/* Qset configuration */
+struct qs_cfg_msg {
+	u8    msg;
+	u8    num;
+	u64   cfg;
+};
+
+/* Receive queue configuration */
+struct rq_cfg_msg {
+	u8    msg;
+	u8    qs_num;
+	u8    rq_num;
+	u64   cfg;
+};
+
+/* Send queue configuration */
+struct sq_cfg_msg {
+	u8    msg;
+	u8    qs_num;
+	u8    sq_num;
+	u64   cfg;
+};
+
+/* Set VF's MAC address */
+struct set_mac_msg {
+	u8    msg;
+	u8    vf_id;
+	u8    mac_addr[ETH_ALEN];
+};
+
+/* Set Maximum frame size */
+struct set_frs_msg {
+	u8    msg;
+	u8    vf_id;
+	u16   max_frs;
+};
+
+/* Set CPI algorithm type */
+struct cpi_cfg_msg {
+	u8    msg;
+	u8    vf_id;
+	u8    rq_cnt;
+	u8    cpi_alg;
+};
+
+/* Get RSS table size */
+struct rss_sz_msg {
+	u8    msg;
+	u8    vf_id;
+	u16   ind_tbl_size;
+};
+
+/* Set RSS configuration */
+struct rss_cfg_msg {
+	u8    msg;
+	u8    vf_id;
+	u8    hash_bits;
+	u8    tbl_len;
+	u8    tbl_offset;
+#define RSS_IND_TBL_LEN_PER_MBX_MSG	8
+	u8    ind_tbl[RSS_IND_TBL_LEN_PER_MBX_MSG];
+};
+
+struct bgx_stats_msg {
+	u8    msg;
+	u8    vf_id;
+	u8    rx;
+	u8    idx;
+	u64   stats;
+};
+
+/* Physical interface link status */
+struct bgx_link_status {
+	u8    msg;
+	u8    link_up;
+	u8    duplex;
+	u32   speed;
+};
+
+/* 128 bit shared memory between PF and each VF */
+union nic_mbx {
+	struct { u8 msg; }	msg;
+	struct nic_cfg_msg	nic_cfg;
+	struct qs_cfg_msg	qs;
+	struct rq_cfg_msg	rq;
+	struct sq_cfg_msg	sq;
+	struct set_mac_msg	mac;
+	struct set_frs_msg	frs;
+	struct cpi_cfg_msg	cpi_cfg;
+	struct rss_sz_msg	rss_size;
+	struct rss_cfg_msg	rss_cfg;
+	struct bgx_stats_msg    bgx_stats;
+	struct bgx_link_status  link_status;
+};
+
+#define NIC_NODE_ID_MASK	0x03
+#define NIC_NODE_ID_SHIFT	44
+
+static inline int nic_get_node_id(struct pci_dev *pdev)
+{
+	u64 addr = pci_resource_start(pdev, PCI_CFG_REG_BAR_NUM);
+	return ((addr >> NIC_NODE_ID_SHIFT) & NIC_NODE_ID_MASK);
+}
+
+int nicvf_set_real_num_queues(struct net_device *netdev,
+			      int tx_queues, int rx_queues);
+int nicvf_open(struct net_device *netdev);
+int nicvf_stop(struct net_device *netdev);
+int nicvf_send_msg_to_pf(struct nicvf *vf, union nic_mbx *mbx);
+void nicvf_config_rss(struct nicvf *nic);
+void nicvf_set_rss_key(struct nicvf *nic);
+void nicvf_set_ethtool_ops(struct net_device *netdev);
+void nicvf_update_stats(struct nicvf *nic);
+void nicvf_update_lmac_stats(struct nicvf *nic);
+
+#endif /* NIC_H */
diff --git a/drivers/net/ethernet/cavium/thunder/nic_main.c b/drivers/net/ethernet/cavium/thunder/nic_main.c
new file mode 100644
index 000000000000..6e0c03169a55
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/nic_main.c
@@ -0,0 +1,932 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/etherdevice.h>
+#include <linux/of.h>
+
+#include "nic_reg.h"
+#include "nic.h"
+#include "q_struct.h"
+#include "thunder_bgx.h"
+
+#define DRV_NAME	"thunder-nic"
+#define DRV_VERSION	"1.0"
+
+struct nicpf {
+	struct pci_dev		*pdev;
+	u8			rev_id;
+	u8			node;
+	unsigned int		flags;
+	u8			num_vf_en;      /* No of VF enabled */
+	bool			vf_enabled[MAX_NUM_VFS_SUPPORTED];
+	void __iomem		*reg_base;       /* Register start address */
+	struct pkind_cfg	pkind;
+#define	NIC_SET_VF_LMAC_MAP(bgx, lmac)	(((bgx & 0xF) << 4) | (lmac & 0xF))
+#define	NIC_GET_BGX_FROM_VF_LMAC_MAP(map)	((map >> 4) & 0xF)
+#define	NIC_GET_LMAC_FROM_VF_LMAC_MAP(map)	(map & 0xF)
+	u8			vf_lmac_map[MAX_LMAC];
+	struct delayed_work     dwork;
+	struct workqueue_struct *check_link;
+	u8			link[MAX_LMAC];
+	u8			duplex[MAX_LMAC];
+	u32			speed[MAX_LMAC];
+	u16			cpi_base[MAX_NUM_VFS_SUPPORTED];
+	u16			rss_ind_tbl_size;
+	bool			mbx_lock[MAX_NUM_VFS_SUPPORTED];
+
+	/* MSI-X */
+	bool			msix_enabled;
+	u8			num_vec;
+	struct msix_entry	msix_entries[NIC_PF_MSIX_VECTORS];
+	bool			irq_allocated[NIC_PF_MSIX_VECTORS];
+};
+
+/* Supported devices */
+static const struct pci_device_id nic_id_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CAVIUM, PCI_DEVICE_ID_THUNDER_NIC_PF) },
+	{ 0, }  /* end of table */
+};
+
+MODULE_AUTHOR("Sunil Goutham");
+MODULE_DESCRIPTION("Cavium Thunder NIC Physical Function Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DRV_VERSION);
+MODULE_DEVICE_TABLE(pci, nic_id_table);
+
+/* The Cavium ThunderX network controller can *only* be found in SoCs
+ * containing the ThunderX ARM64 CPU implementation.  All accesses to the device
+ * registers on this platform are implicitly strongly ordered with respect
+ * to memory accesses. So writeq_relaxed() and readq_relaxed() are safe to use
+ * with no memory barriers in this driver.  The readq()/writeq() functions add
+ * explicit ordering operation which in this case are redundant, and only
+ * add overhead.
+ */
+
+/* Register read/write APIs */
+static void nic_reg_write(struct nicpf *nic, u64 offset, u64 val)
+{
+	writeq_relaxed(val, nic->reg_base + offset);
+}
+
+static u64 nic_reg_read(struct nicpf *nic, u64 offset)
+{
+	return readq_relaxed(nic->reg_base + offset);
+}
+
+/* PF -> VF mailbox communication APIs */
+static void nic_enable_mbx_intr(struct nicpf *nic)
+{
+	/* Enable mailbox interrupt for all 128 VFs */
+	nic_reg_write(nic, NIC_PF_MAILBOX_ENA_W1S, ~0ull);
+	nic_reg_write(nic, NIC_PF_MAILBOX_ENA_W1S + sizeof(u64), ~0ull);
+}
+
+static void nic_clear_mbx_intr(struct nicpf *nic, int vf, int mbx_reg)
+{
+	nic_reg_write(nic, NIC_PF_MAILBOX_INT + (mbx_reg << 3), BIT_ULL(vf));
+}
+
+static u64 nic_get_mbx_addr(int vf)
+{
+	return NIC_PF_VF_0_127_MAILBOX_0_1 + (vf << NIC_VF_NUM_SHIFT);
+}
+
+/* Send a mailbox message to VF
+ * @vf: vf to which this message to be sent
+ * @mbx: Message to be sent
+ */
+static void nic_send_msg_to_vf(struct nicpf *nic, int vf, union nic_mbx *mbx)
+{
+	void __iomem *mbx_addr = nic->reg_base + nic_get_mbx_addr(vf);
+	u64 *msg = (u64 *)mbx;
+
+	/* In first revision HW, mbox interrupt is triggerred
+	 * when PF writes to MBOX(1), in next revisions when
+	 * PF writes to MBOX(0)
+	 */
+	if (nic->rev_id == 0) {
+		/* see the comment for nic_reg_write()/nic_reg_read()
+		 * functions above
+		 */
+		writeq_relaxed(msg[0], mbx_addr);
+		writeq_relaxed(msg[1], mbx_addr + 8);
+	} else {
+		writeq_relaxed(msg[1], mbx_addr + 8);
+		writeq_relaxed(msg[0], mbx_addr);
+	}
+}
+
+/* Responds to VF's READY message with VF's
+ * ID, node, MAC address e.t.c
+ * @vf: VF which sent READY message
+ */
+static void nic_mbx_send_ready(struct nicpf *nic, int vf)
+{
+	union nic_mbx mbx = {};
+	int bgx_idx, lmac;
+	const char *mac;
+
+	mbx.nic_cfg.msg = NIC_MBOX_MSG_READY;
+	mbx.nic_cfg.vf_id = vf;
+
+	mbx.nic_cfg.tns_mode = NIC_TNS_BYPASS_MODE;
+
+	bgx_idx = NIC_GET_BGX_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vf]);
+	lmac = NIC_GET_LMAC_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vf]);
+
+	mac = bgx_get_lmac_mac(nic->node, bgx_idx, lmac);
+	if (mac)
+		ether_addr_copy((u8 *)&mbx.nic_cfg.mac_addr, mac);
+
+	mbx.nic_cfg.node_id = nic->node;
+	nic_send_msg_to_vf(nic, vf, &mbx);
+}
+
+/* ACKs VF's mailbox message
+ * @vf: VF to which ACK to be sent
+ */
+static void nic_mbx_send_ack(struct nicpf *nic, int vf)
+{
+	union nic_mbx mbx = {};
+
+	mbx.msg.msg = NIC_MBOX_MSG_ACK;
+	nic_send_msg_to_vf(nic, vf, &mbx);
+}
+
+/* NACKs VF's mailbox message that PF is not able to
+ * complete the action
+ * @vf: VF to which ACK to be sent
+ */
+static void nic_mbx_send_nack(struct nicpf *nic, int vf)
+{
+	union nic_mbx mbx = {};
+
+	mbx.msg.msg = NIC_MBOX_MSG_NACK;
+	nic_send_msg_to_vf(nic, vf, &mbx);
+}
+
+/* Flush all in flight receive packets to memory and
+ * bring down an active RQ
+ */
+static int nic_rcv_queue_sw_sync(struct nicpf *nic)
+{
+	u16 timeout = ~0x00;
+
+	nic_reg_write(nic, NIC_PF_SW_SYNC_RX, 0x01);
+	/* Wait till sync cycle is finished */
+	while (timeout) {
+		if (nic_reg_read(nic, NIC_PF_SW_SYNC_RX_DONE) & 0x1)
+			break;
+		timeout--;
+	}
+	nic_reg_write(nic, NIC_PF_SW_SYNC_RX, 0x00);
+	if (!timeout) {
+		dev_err(&nic->pdev->dev, "Receive queue software sync failed");
+		return 1;
+	}
+	return 0;
+}
+
+/* Get BGX Rx/Tx stats and respond to VF's request */
+static void nic_get_bgx_stats(struct nicpf *nic, struct bgx_stats_msg *bgx)
+{
+	int bgx_idx, lmac;
+	union nic_mbx mbx = {};
+
+	bgx_idx = NIC_GET_BGX_FROM_VF_LMAC_MAP(nic->vf_lmac_map[bgx->vf_id]);
+	lmac = NIC_GET_LMAC_FROM_VF_LMAC_MAP(nic->vf_lmac_map[bgx->vf_id]);
+
+	mbx.bgx_stats.msg = NIC_MBOX_MSG_BGX_STATS;
+	mbx.bgx_stats.vf_id = bgx->vf_id;
+	mbx.bgx_stats.rx = bgx->rx;
+	mbx.bgx_stats.idx = bgx->idx;
+	if (bgx->rx)
+		mbx.bgx_stats.stats = bgx_get_rx_stats(nic->node, bgx_idx,
+							    lmac, bgx->idx);
+	else
+		mbx.bgx_stats.stats = bgx_get_tx_stats(nic->node, bgx_idx,
+							    lmac, bgx->idx);
+	nic_send_msg_to_vf(nic, bgx->vf_id, &mbx);
+}
+
+/* Update hardware min/max frame size */
+static int nic_update_hw_frs(struct nicpf *nic, int new_frs, int vf)
+{
+	if ((new_frs > NIC_HW_MAX_FRS) || (new_frs < NIC_HW_MIN_FRS)) {
+		dev_err(&nic->pdev->dev,
+			"Invalid MTU setting from VF%d rejected, should be between %d and %d\n",
+			   vf, NIC_HW_MIN_FRS, NIC_HW_MAX_FRS);
+		return 1;
+	}
+	new_frs += ETH_HLEN;
+	if (new_frs <= nic->pkind.maxlen)
+		return 0;
+
+	nic->pkind.maxlen = new_frs;
+	nic_reg_write(nic, NIC_PF_PKIND_0_15_CFG, *(u64 *)&nic->pkind);
+	return 0;
+}
+
+/* Set minimum transmit packet size */
+static void nic_set_tx_pkt_pad(struct nicpf *nic, int size)
+{
+	int lmac;
+	u64 lmac_cfg;
+
+	/* Max value that can be set is 60 */
+	if (size > 60)
+		size = 60;
+
+	for (lmac = 0; lmac < (MAX_BGX_PER_CN88XX * MAX_LMAC_PER_BGX); lmac++) {
+		lmac_cfg = nic_reg_read(nic, NIC_PF_LMAC_0_7_CFG | (lmac << 3));
+		lmac_cfg &= ~(0xF << 2);
+		lmac_cfg |= ((size / 4) << 2);
+		nic_reg_write(nic, NIC_PF_LMAC_0_7_CFG | (lmac << 3), lmac_cfg);
+	}
+}
+
+/* Function to check number of LMACs present and set VF::LMAC mapping.
+ * Mapping will be used while initializing channels.
+ */
+static void nic_set_lmac_vf_mapping(struct nicpf *nic)
+{
+	unsigned bgx_map = bgx_get_map(nic->node);
+	int bgx, next_bgx_lmac = 0;
+	int lmac, lmac_cnt = 0;
+	u64 lmac_credit;
+
+	nic->num_vf_en = 0;
+
+	for (bgx = 0; bgx < NIC_MAX_BGX; bgx++) {
+		if (!(bgx_map & (1 << bgx)))
+			continue;
+		lmac_cnt = bgx_get_lmac_count(nic->node, bgx);
+		for (lmac = 0; lmac < lmac_cnt; lmac++)
+			nic->vf_lmac_map[next_bgx_lmac++] =
+						NIC_SET_VF_LMAC_MAP(bgx, lmac);
+		nic->num_vf_en += lmac_cnt;
+
+		/* Program LMAC credits */
+		lmac_credit = (1ull << 1); /* channel credit enable */
+		lmac_credit |= (0x1ff << 2); /* Max outstanding pkt count */
+		/* 48KB BGX Tx buffer size, each unit is of size 16bytes */
+		lmac_credit |= (((((48 * 1024) / lmac_cnt) -
+				NIC_HW_MAX_FRS) / 16) << 12);
+		lmac = bgx * MAX_LMAC_PER_BGX;
+		for (; lmac < lmac_cnt + (bgx * MAX_LMAC_PER_BGX); lmac++)
+			nic_reg_write(nic,
+				      NIC_PF_LMAC_0_7_CREDIT + (lmac * 8),
+				      lmac_credit);
+	}
+}
+
+#define BGX0_BLOCK 8
+#define BGX1_BLOCK 9
+
+static void nic_init_hw(struct nicpf *nic)
+{
+	int i;
+
+	/* Reset NIC, in case the driver is repeatedly inserted and removed */
+	nic_reg_write(nic, NIC_PF_SOFT_RESET, 1);
+
+	/* Enable NIC HW block */
+	nic_reg_write(nic, NIC_PF_CFG, 0x3);
+
+	/* Enable backpressure */
+	nic_reg_write(nic, NIC_PF_BP_CFG, (1ULL << 6) | 0x03);
+
+	/* Disable TNS mode on both interfaces */
+	nic_reg_write(nic, NIC_PF_INTF_0_1_SEND_CFG,
+		      (NIC_TNS_BYPASS_MODE << 7) | BGX0_BLOCK);
+	nic_reg_write(nic, NIC_PF_INTF_0_1_SEND_CFG | (1 << 8),
+		      (NIC_TNS_BYPASS_MODE << 7) | BGX1_BLOCK);
+	nic_reg_write(nic, NIC_PF_INTF_0_1_BP_CFG,
+		      (1ULL << 63) | BGX0_BLOCK);
+	nic_reg_write(nic, NIC_PF_INTF_0_1_BP_CFG + (1 << 8),
+		      (1ULL << 63) | BGX1_BLOCK);
+
+	/* PKIND configuration */
+	nic->pkind.minlen = 0;
+	nic->pkind.maxlen = NIC_HW_MAX_FRS + ETH_HLEN;
+	nic->pkind.lenerr_en = 1;
+	nic->pkind.rx_hdr = 0;
+	nic->pkind.hdr_sl = 0;
+
+	for (i = 0; i < NIC_MAX_PKIND; i++)
+		nic_reg_write(nic, NIC_PF_PKIND_0_15_CFG | (i << 3),
+			      *(u64 *)&nic->pkind);
+
+	nic_set_tx_pkt_pad(nic, NIC_HW_MIN_FRS);
+
+	/* Timer config */
+	nic_reg_write(nic, NIC_PF_INTR_TIMER_CFG, NICPF_CLK_PER_INT_TICK);
+}
+
+/* Channel parse index configuration */
+static void nic_config_cpi(struct nicpf *nic, struct cpi_cfg_msg *cfg)
+{
+	u32 vnic, bgx, lmac, chan;
+	u32 padd, cpi_count = 0;
+	u64 cpi_base, cpi, rssi_base, rssi;
+	u8  qset, rq_idx = 0;
+
+	vnic = cfg->vf_id;
+	bgx = NIC_GET_BGX_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vnic]);
+	lmac = NIC_GET_LMAC_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vnic]);
+
+	chan = (lmac * MAX_BGX_CHANS_PER_LMAC) + (bgx * NIC_CHANS_PER_INF);
+	cpi_base = (lmac * NIC_MAX_CPI_PER_LMAC) + (bgx * NIC_CPI_PER_BGX);
+	rssi_base = (lmac * nic->rss_ind_tbl_size) + (bgx * NIC_RSSI_PER_BGX);
+
+	/* Rx channel configuration */
+	nic_reg_write(nic, NIC_PF_CHAN_0_255_RX_BP_CFG | (chan << 3),
+		      (1ull << 63) | (vnic << 0));
+	nic_reg_write(nic, NIC_PF_CHAN_0_255_RX_CFG | (chan << 3),
+		      ((u64)cfg->cpi_alg << 62) | (cpi_base << 48));
+
+	if (cfg->cpi_alg == CPI_ALG_NONE)
+		cpi_count = 1;
+	else if (cfg->cpi_alg == CPI_ALG_VLAN) /* 3 bits of PCP */
+		cpi_count = 8;
+	else if (cfg->cpi_alg == CPI_ALG_VLAN16) /* 3 bits PCP + DEI */
+		cpi_count = 16;
+	else if (cfg->cpi_alg == CPI_ALG_DIFF) /* 6bits DSCP */
+		cpi_count = NIC_MAX_CPI_PER_LMAC;
+
+	/* RSS Qset, Qidx mapping */
+	qset = cfg->vf_id;
+	rssi = rssi_base;
+	for (; rssi < (rssi_base + cfg->rq_cnt); rssi++) {
+		nic_reg_write(nic, NIC_PF_RSSI_0_4097_RQ | (rssi << 3),
+			      (qset << 3) | rq_idx);
+		rq_idx++;
+	}
+
+	rssi = 0;
+	cpi = cpi_base;
+	for (; cpi < (cpi_base + cpi_count); cpi++) {
+		/* Determine port to channel adder */
+		if (cfg->cpi_alg != CPI_ALG_DIFF)
+			padd = cpi % cpi_count;
+		else
+			padd = cpi % 8; /* 3 bits CS out of 6bits DSCP */
+
+		/* Leave RSS_SIZE as '0' to disable RSS */
+		nic_reg_write(nic, NIC_PF_CPI_0_2047_CFG | (cpi << 3),
+			      (vnic << 24) | (padd << 16) | (rssi_base + rssi));
+
+		if ((rssi + 1) >= cfg->rq_cnt)
+			continue;
+
+		if (cfg->cpi_alg == CPI_ALG_VLAN)
+			rssi++;
+		else if (cfg->cpi_alg == CPI_ALG_VLAN16)
+			rssi = ((cpi - cpi_base) & 0xe) >> 1;
+		else if (cfg->cpi_alg == CPI_ALG_DIFF)
+			rssi = ((cpi - cpi_base) & 0x38) >> 3;
+	}
+	nic->cpi_base[cfg->vf_id] = cpi_base;
+}
+
+/* Responsds to VF with its RSS indirection table size */
+static void nic_send_rss_size(struct nicpf *nic, int vf)
+{
+	union nic_mbx mbx = {};
+	u64  *msg;
+
+	msg = (u64 *)&mbx;
+
+	mbx.rss_size.msg = NIC_MBOX_MSG_RSS_SIZE;
+	mbx.rss_size.ind_tbl_size = nic->rss_ind_tbl_size;
+	nic_send_msg_to_vf(nic, vf, &mbx);
+}
+
+/* Receive side scaling configuration
+ * configure:
+ * - RSS index
+ * - indir table i.e hash::RQ mapping
+ * - no of hash bits to consider
+ */
+static void nic_config_rss(struct nicpf *nic, struct rss_cfg_msg *cfg)
+{
+	u8  qset, idx = 0;
+	u64 cpi_cfg, cpi_base, rssi_base, rssi;
+
+	cpi_base = nic->cpi_base[cfg->vf_id];
+	cpi_cfg = nic_reg_read(nic, NIC_PF_CPI_0_2047_CFG | (cpi_base << 3));
+	rssi_base = (cpi_cfg & 0x0FFF) + cfg->tbl_offset;
+
+	rssi = rssi_base;
+	qset = cfg->vf_id;
+
+	for (; rssi < (rssi_base + cfg->tbl_len); rssi++) {
+		nic_reg_write(nic, NIC_PF_RSSI_0_4097_RQ | (rssi << 3),
+			      (qset << 3) | (cfg->ind_tbl[idx] & 0x7));
+		idx++;
+	}
+
+	cpi_cfg &= ~(0xFULL << 20);
+	cpi_cfg |= (cfg->hash_bits << 20);
+	nic_reg_write(nic, NIC_PF_CPI_0_2047_CFG | (cpi_base << 3), cpi_cfg);
+}
+
+/* 4 level transmit side scheduler configutation
+ * for TNS bypass mode
+ *
+ * Sample configuration for SQ0
+ * VNIC0-SQ0 -> TL4(0)   -> TL3[0]   -> TL2[0]  -> TL1[0] -> BGX0
+ * VNIC1-SQ0 -> TL4(8)   -> TL3[2]   -> TL2[0]  -> TL1[0] -> BGX0
+ * VNIC2-SQ0 -> TL4(16)  -> TL3[4]   -> TL2[1]  -> TL1[0] -> BGX0
+ * VNIC3-SQ0 -> TL4(24)  -> TL3[6]   -> TL2[1]  -> TL1[0] -> BGX0
+ * VNIC4-SQ0 -> TL4(512) -> TL3[128] -> TL2[32] -> TL1[1] -> BGX1
+ * VNIC5-SQ0 -> TL4(520) -> TL3[130] -> TL2[32] -> TL1[1] -> BGX1
+ * VNIC6-SQ0 -> TL4(528) -> TL3[132] -> TL2[33] -> TL1[1] -> BGX1
+ * VNIC7-SQ0 -> TL4(536) -> TL3[134] -> TL2[33] -> TL1[1] -> BGX1
+ */
+static void nic_tx_channel_cfg(struct nicpf *nic, u8 vnic, u8 sq_idx)
+{
+	u32 bgx, lmac, chan;
+	u32 tl2, tl3, tl4;
+	u32 rr_quantum;
+
+	bgx = NIC_GET_BGX_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vnic]);
+	lmac = NIC_GET_LMAC_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vnic]);
+	/* 24 bytes for FCS, IPG and preamble */
+	rr_quantum = ((NIC_HW_MAX_FRS + 24) / 4);
+
+	tl4 = (lmac * NIC_TL4_PER_LMAC) + (bgx * NIC_TL4_PER_BGX);
+	tl4 += sq_idx;
+	tl3 = tl4 / (NIC_MAX_TL4 / NIC_MAX_TL3);
+	nic_reg_write(nic, NIC_PF_QSET_0_127_SQ_0_7_CFG2 |
+		      ((u64)vnic << NIC_QS_ID_SHIFT) |
+		      ((u32)sq_idx << NIC_Q_NUM_SHIFT), tl4);
+	nic_reg_write(nic, NIC_PF_TL4_0_1023_CFG | (tl4 << 3),
+		      ((u64)vnic << 27) | ((u32)sq_idx << 24) | rr_quantum);
+
+	nic_reg_write(nic, NIC_PF_TL3_0_255_CFG | (tl3 << 3), rr_quantum);
+	chan = (lmac * MAX_BGX_CHANS_PER_LMAC) + (bgx * NIC_CHANS_PER_INF);
+	nic_reg_write(nic, NIC_PF_TL3_0_255_CHAN | (tl3 << 3), chan);
+	/* Enable backpressure on the channel */
+	nic_reg_write(nic, NIC_PF_CHAN_0_255_TX_CFG | (chan << 3), 1);
+
+	tl2 = tl3 >> 2;
+	nic_reg_write(nic, NIC_PF_TL3A_0_63_CFG | (tl2 << 3), tl2);
+	nic_reg_write(nic, NIC_PF_TL2_0_63_CFG | (tl2 << 3), rr_quantum);
+	/* No priorities as of now */
+	nic_reg_write(nic, NIC_PF_TL2_0_63_PRI | (tl2 << 3), 0x00);
+}
+
+/* Interrupt handler to handle mailbox messages from VFs */
+static void nic_handle_mbx_intr(struct nicpf *nic, int vf)
+{
+	union nic_mbx mbx = {};
+	u64 *mbx_data;
+	u64 mbx_addr;
+	u64 reg_addr;
+	int bgx, lmac;
+	int i;
+	int ret = 0;
+
+	nic->mbx_lock[vf] = true;
+
+	mbx_addr = nic_get_mbx_addr(vf);
+	mbx_data = (u64 *)&mbx;
+
+	for (i = 0; i < NIC_PF_VF_MAILBOX_SIZE; i++) {
+		*mbx_data = nic_reg_read(nic, mbx_addr);
+		mbx_data++;
+		mbx_addr += sizeof(u64);
+	}
+
+	dev_dbg(&nic->pdev->dev, "%s: Mailbox msg %d from VF%d\n",
+		__func__, mbx.msg.msg, vf);
+	switch (mbx.msg.msg) {
+	case NIC_MBOX_MSG_READY:
+		nic_mbx_send_ready(nic, vf);
+		nic->link[vf] = 0;
+		nic->duplex[vf] = 0;
+		nic->speed[vf] = 0;
+		ret = 1;
+		break;
+	case NIC_MBOX_MSG_QS_CFG:
+		reg_addr = NIC_PF_QSET_0_127_CFG |
+			   (mbx.qs.num << NIC_QS_ID_SHIFT);
+		nic_reg_write(nic, reg_addr, mbx.qs.cfg);
+		break;
+	case NIC_MBOX_MSG_RQ_CFG:
+		reg_addr = NIC_PF_QSET_0_127_RQ_0_7_CFG |
+			   (mbx.rq.qs_num << NIC_QS_ID_SHIFT) |
+			   (mbx.rq.rq_num << NIC_Q_NUM_SHIFT);
+		nic_reg_write(nic, reg_addr, mbx.rq.cfg);
+		break;
+	case NIC_MBOX_MSG_RQ_BP_CFG:
+		reg_addr = NIC_PF_QSET_0_127_RQ_0_7_BP_CFG |
+			   (mbx.rq.qs_num << NIC_QS_ID_SHIFT) |
+			   (mbx.rq.rq_num << NIC_Q_NUM_SHIFT);
+		nic_reg_write(nic, reg_addr, mbx.rq.cfg);
+		break;
+	case NIC_MBOX_MSG_RQ_SW_SYNC:
+		ret = nic_rcv_queue_sw_sync(nic);
+		break;
+	case NIC_MBOX_MSG_RQ_DROP_CFG:
+		reg_addr = NIC_PF_QSET_0_127_RQ_0_7_DROP_CFG |
+			   (mbx.rq.qs_num << NIC_QS_ID_SHIFT) |
+			   (mbx.rq.rq_num << NIC_Q_NUM_SHIFT);
+		nic_reg_write(nic, reg_addr, mbx.rq.cfg);
+		break;
+	case NIC_MBOX_MSG_SQ_CFG:
+		reg_addr = NIC_PF_QSET_0_127_SQ_0_7_CFG |
+			   (mbx.sq.qs_num << NIC_QS_ID_SHIFT) |
+			   (mbx.sq.sq_num << NIC_Q_NUM_SHIFT);
+		nic_reg_write(nic, reg_addr, mbx.sq.cfg);
+		nic_tx_channel_cfg(nic, mbx.qs.num, mbx.sq.sq_num);
+		break;
+	case NIC_MBOX_MSG_SET_MAC:
+		lmac = mbx.mac.vf_id;
+		bgx = NIC_GET_BGX_FROM_VF_LMAC_MAP(nic->vf_lmac_map[lmac]);
+		lmac = NIC_GET_LMAC_FROM_VF_LMAC_MAP(nic->vf_lmac_map[lmac]);
+		bgx_set_lmac_mac(nic->node, bgx, lmac, mbx.mac.mac_addr);
+		break;
+	case NIC_MBOX_MSG_SET_MAX_FRS:
+		ret = nic_update_hw_frs(nic, mbx.frs.max_frs,
+					mbx.frs.vf_id);
+		break;
+	case NIC_MBOX_MSG_CPI_CFG:
+		nic_config_cpi(nic, &mbx.cpi_cfg);
+		break;
+	case NIC_MBOX_MSG_RSS_SIZE:
+		nic_send_rss_size(nic, vf);
+		goto unlock;
+	case NIC_MBOX_MSG_RSS_CFG:
+	case NIC_MBOX_MSG_RSS_CFG_CONT:
+		nic_config_rss(nic, &mbx.rss_cfg);
+		break;
+	case NIC_MBOX_MSG_CFG_DONE:
+		/* Last message of VF config msg sequence */
+		nic->vf_enabled[vf] = true;
+		goto unlock;
+	case NIC_MBOX_MSG_SHUTDOWN:
+		/* First msg in VF teardown sequence */
+		nic->vf_enabled[vf] = false;
+		break;
+	case NIC_MBOX_MSG_BGX_STATS:
+		nic_get_bgx_stats(nic, &mbx.bgx_stats);
+		goto unlock;
+	default:
+		dev_err(&nic->pdev->dev,
+			"Invalid msg from VF%d, msg 0x%x\n", vf, mbx.msg.msg);
+		break;
+	}
+
+	if (!ret)
+		nic_mbx_send_ack(nic, vf);
+	else if (mbx.msg.msg != NIC_MBOX_MSG_READY)
+		nic_mbx_send_nack(nic, vf);
+unlock:
+	nic->mbx_lock[vf] = false;
+}
+
+static void nic_mbx_intr_handler (struct nicpf *nic, int mbx)
+{
+	u64 intr;
+	u8  vf, vf_per_mbx_reg = 64;
+
+	intr = nic_reg_read(nic, NIC_PF_MAILBOX_INT + (mbx << 3));
+	dev_dbg(&nic->pdev->dev, "PF interrupt Mbox%d 0x%llx\n", mbx, intr);
+	for (vf = 0; vf < vf_per_mbx_reg; vf++) {
+		if (intr & (1ULL << vf)) {
+			dev_dbg(&nic->pdev->dev, "Intr from VF %d\n",
+				vf + (mbx * vf_per_mbx_reg));
+			if ((vf + (mbx * vf_per_mbx_reg)) > nic->num_vf_en)
+				break;
+			nic_handle_mbx_intr(nic, vf + (mbx * vf_per_mbx_reg));
+			nic_clear_mbx_intr(nic, vf, mbx);
+		}
+	}
+}
+
+static irqreturn_t nic_mbx0_intr_handler (int irq, void *nic_irq)
+{
+	struct nicpf *nic = (struct nicpf *)nic_irq;
+
+	nic_mbx_intr_handler(nic, 0);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t nic_mbx1_intr_handler (int irq, void *nic_irq)
+{
+	struct nicpf *nic = (struct nicpf *)nic_irq;
+
+	nic_mbx_intr_handler(nic, 1);
+
+	return IRQ_HANDLED;
+}
+
+static int nic_enable_msix(struct nicpf *nic)
+{
+	int i, ret;
+
+	nic->num_vec = NIC_PF_MSIX_VECTORS;
+
+	for (i = 0; i < nic->num_vec; i++)
+		nic->msix_entries[i].entry = i;
+
+	ret = pci_enable_msix(nic->pdev, nic->msix_entries, nic->num_vec);
+	if (ret) {
+		dev_err(&nic->pdev->dev,
+			"Request for #%d msix vectors failed\n",
+			   nic->num_vec);
+		return ret;
+	}
+
+	nic->msix_enabled = 1;
+	return 0;
+}
+
+static void nic_disable_msix(struct nicpf *nic)
+{
+	if (nic->msix_enabled) {
+		pci_disable_msix(nic->pdev);
+		nic->msix_enabled = 0;
+		nic->num_vec = 0;
+	}
+}
+
+static void nic_free_all_interrupts(struct nicpf *nic)
+{
+	int irq;
+
+	for (irq = 0; irq < nic->num_vec; irq++) {
+		if (nic->irq_allocated[irq])
+			free_irq(nic->msix_entries[irq].vector, nic);
+		nic->irq_allocated[irq] = false;
+	}
+}
+
+static int nic_register_interrupts(struct nicpf *nic)
+{
+	int ret;
+
+	/* Enable MSI-X */
+	ret = nic_enable_msix(nic);
+	if (ret)
+		return ret;
+
+	/* Register mailbox interrupt handlers */
+	ret = request_irq(nic->msix_entries[NIC_PF_INTR_ID_MBOX0].vector,
+			  nic_mbx0_intr_handler, 0, "NIC Mbox0", nic);
+	if (ret)
+		goto fail;
+
+	nic->irq_allocated[NIC_PF_INTR_ID_MBOX0] = true;
+
+	ret = request_irq(nic->msix_entries[NIC_PF_INTR_ID_MBOX1].vector,
+			  nic_mbx1_intr_handler, 0, "NIC Mbox1", nic);
+	if (ret)
+		goto fail;
+
+	nic->irq_allocated[NIC_PF_INTR_ID_MBOX1] = true;
+
+	/* Enable mailbox interrupt */
+	nic_enable_mbx_intr(nic);
+	return 0;
+
+fail:
+	dev_err(&nic->pdev->dev, "Request irq failed\n");
+	nic_free_all_interrupts(nic);
+	return ret;
+}
+
+static void nic_unregister_interrupts(struct nicpf *nic)
+{
+	nic_free_all_interrupts(nic);
+	nic_disable_msix(nic);
+}
+
+static int nic_sriov_init(struct pci_dev *pdev, struct nicpf *nic)
+{
+	int pos = 0;
+	int err;
+	u16 total_vf_cnt;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_SRIOV);
+	if (!pos) {
+		dev_err(&pdev->dev, "SRIOV capability is not found in PCIe config space\n");
+		return -ENODEV;
+	}
+
+	pci_read_config_word(pdev, (pos + PCI_SRIOV_TOTAL_VF), &total_vf_cnt);
+	if (total_vf_cnt < nic->num_vf_en)
+		nic->num_vf_en = total_vf_cnt;
+
+	if (!total_vf_cnt)
+		return 0;
+
+	err = pci_enable_sriov(pdev, nic->num_vf_en);
+	if (err) {
+		dev_err(&pdev->dev, "SRIOV enable failed, num VF is %d\n",
+			nic->num_vf_en);
+		nic->num_vf_en = 0;
+		return err;
+	}
+
+	dev_info(&pdev->dev, "SRIOV enabled, number of VF available %d\n",
+		 nic->num_vf_en);
+
+	nic->flags |= NIC_SRIOV_ENABLED;
+	return 0;
+}
+
+/* Poll for BGX LMAC link status and update corresponding VF
+ * if there is a change, valid only if internal L2 switch
+ * is not present otherwise VF link is always treated as up
+ */
+static void nic_poll_for_link(struct work_struct *work)
+{
+	union nic_mbx mbx = {};
+	struct nicpf *nic;
+	struct bgx_link_status link;
+	u8 vf, bgx, lmac;
+
+	nic = container_of(work, struct nicpf, dwork.work);
+
+	mbx.link_status.msg = NIC_MBOX_MSG_BGX_LINK_CHANGE;
+
+	for (vf = 0; vf < nic->num_vf_en; vf++) {
+		/* Poll only if VF is UP */
+		if (!nic->vf_enabled[vf])
+			continue;
+
+		/* Get BGX, LMAC indices for the VF */
+		bgx = NIC_GET_BGX_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vf]);
+		lmac = NIC_GET_LMAC_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vf]);
+		/* Get interface link status */
+		bgx_get_lmac_link_state(nic->node, bgx, lmac, &link);
+
+		/* Inform VF only if link status changed */
+		if (nic->link[vf] == link.link_up)
+			continue;
+
+		if (!nic->mbx_lock[vf]) {
+			nic->link[vf] = link.link_up;
+			nic->duplex[vf] = link.duplex;
+			nic->speed[vf] = link.speed;
+
+			/* Send a mbox message to VF with current link status */
+			mbx.link_status.link_up = link.link_up;
+			mbx.link_status.duplex = link.duplex;
+			mbx.link_status.speed = link.speed;
+			nic_send_msg_to_vf(nic, vf, &mbx);
+		}
+	}
+	queue_delayed_work(nic->check_link, &nic->dwork, HZ * 2);
+}
+
+static int nic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct device *dev = &pdev->dev;
+	struct nicpf *nic;
+	int    err;
+
+	BUILD_BUG_ON(sizeof(union nic_mbx) > 16);
+
+	nic = devm_kzalloc(dev, sizeof(*nic), GFP_KERNEL);
+	if (!nic)
+		return -ENOMEM;
+
+	pci_set_drvdata(pdev, nic);
+
+	nic->pdev = pdev;
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		dev_err(dev, "Failed to enable PCI device\n");
+		pci_set_drvdata(pdev, NULL);
+		return err;
+	}
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err) {
+		dev_err(dev, "PCI request regions failed 0x%x\n", err);
+		goto err_disable_device;
+	}
+
+	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(48));
+	if (err) {
+		dev_err(dev, "Unable to get usable DMA configuration\n");
+		goto err_release_regions;
+	}
+
+	err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(48));
+	if (err) {
+		dev_err(dev, "Unable to get 48-bit DMA for consistent allocations\n");
+		goto err_release_regions;
+	}
+
+	/* MAP PF's configuration registers */
+	nic->reg_base = pcim_iomap(pdev, PCI_CFG_REG_BAR_NUM, 0);
+	if (!nic->reg_base) {
+		dev_err(dev, "Cannot map config register space, aborting\n");
+		err = -ENOMEM;
+		goto err_release_regions;
+	}
+
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &nic->rev_id);
+
+	nic->node = nic_get_node_id(pdev);
+
+	nic_set_lmac_vf_mapping(nic);
+
+	/* Initialize hardware */
+	nic_init_hw(nic);
+
+	/* Set RSS TBL size for each VF */
+	nic->rss_ind_tbl_size = NIC_MAX_RSS_IDR_TBL_SIZE;
+
+	/* Register interrupts */
+	err = nic_register_interrupts(nic);
+	if (err)
+		goto err_release_regions;
+
+	/* Configure SRIOV */
+	err = nic_sriov_init(pdev, nic);
+	if (err)
+		goto err_unregister_interrupts;
+
+	/* Register a physical link status poll fn() */
+	nic->check_link = alloc_workqueue("check_link_status",
+					  WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+	if (!nic->check_link) {
+		err = -ENOMEM;
+		goto err_disable_sriov;
+	}
+
+	INIT_DELAYED_WORK(&nic->dwork, nic_poll_for_link);
+	queue_delayed_work(nic->check_link, &nic->dwork, 0);
+
+	return 0;
+
+err_disable_sriov:
+	if (nic->flags & NIC_SRIOV_ENABLED)
+		pci_disable_sriov(pdev);
+err_unregister_interrupts:
+	nic_unregister_interrupts(nic);
+err_release_regions:
+	pci_release_regions(pdev);
+err_disable_device:
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+	return err;
+}
+
+static void nic_remove(struct pci_dev *pdev)
+{
+	struct nicpf *nic = pci_get_drvdata(pdev);
+
+	if (nic->flags & NIC_SRIOV_ENABLED)
+		pci_disable_sriov(pdev);
+
+	if (nic->check_link) {
+		/* Destroy work Queue */
+		cancel_delayed_work(&nic->dwork);
+		flush_workqueue(nic->check_link);
+		destroy_workqueue(nic->check_link);
+	}
+
+	nic_unregister_interrupts(nic);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+}
+
+static struct pci_driver nic_driver = {
+	.name = DRV_NAME,
+	.id_table = nic_id_table,
+	.probe = nic_probe,
+	.remove = nic_remove,
+};
+
+static int __init nic_init_module(void)
+{
+	pr_info("%s, ver %s\n", DRV_NAME, DRV_VERSION);
+
+	return pci_register_driver(&nic_driver);
+}
+
+static void __exit nic_cleanup_module(void)
+{
+	pci_unregister_driver(&nic_driver);
+}
+
+module_init(nic_init_module);
+module_exit(nic_cleanup_module);
diff --git a/drivers/net/ethernet/cavium/thunder/nic_reg.h b/drivers/net/ethernet/cavium/thunder/nic_reg.h
new file mode 100644
index 000000000000..58197bb2f805
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/nic_reg.h
@@ -0,0 +1,213 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * 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.
+ */
+
+#ifndef NIC_REG_H
+#define NIC_REG_H
+
+#define   NIC_PF_REG_COUNT			29573
+#define   NIC_VF_REG_COUNT			249
+
+/* Physical function register offsets */
+#define   NIC_PF_CFG				(0x0000)
+#define   NIC_PF_STATUS				(0x0010)
+#define   NIC_PF_INTR_TIMER_CFG			(0x0030)
+#define   NIC_PF_BIST_STATUS			(0x0040)
+#define   NIC_PF_SOFT_RESET			(0x0050)
+#define   NIC_PF_TCP_TIMER			(0x0060)
+#define   NIC_PF_BP_CFG				(0x0080)
+#define   NIC_PF_RRM_CFG			(0x0088)
+#define   NIC_PF_CQM_CF				(0x00A0)
+#define   NIC_PF_CNM_CF				(0x00A8)
+#define   NIC_PF_CNM_STATUS			(0x00B0)
+#define   NIC_PF_CQ_AVG_CFG			(0x00C0)
+#define   NIC_PF_RRM_AVG_CFG			(0x00C8)
+#define   NIC_PF_INTF_0_1_SEND_CFG		(0x0200)
+#define   NIC_PF_INTF_0_1_BP_CFG		(0x0208)
+#define   NIC_PF_INTF_0_1_BP_DIS_0_1		(0x0210)
+#define   NIC_PF_INTF_0_1_BP_SW_0_1		(0x0220)
+#define   NIC_PF_RBDR_BP_STATE_0_3		(0x0240)
+#define   NIC_PF_MAILBOX_INT			(0x0410)
+#define   NIC_PF_MAILBOX_INT_W1S		(0x0430)
+#define   NIC_PF_MAILBOX_ENA_W1C		(0x0450)
+#define   NIC_PF_MAILBOX_ENA_W1S		(0x0470)
+#define   NIC_PF_RX_ETYPE_0_7			(0x0500)
+#define   NIC_PF_PKIND_0_15_CFG			(0x0600)
+#define   NIC_PF_ECC0_FLIP0			(0x1000)
+#define   NIC_PF_ECC1_FLIP0			(0x1008)
+#define   NIC_PF_ECC2_FLIP0			(0x1010)
+#define   NIC_PF_ECC3_FLIP0			(0x1018)
+#define   NIC_PF_ECC0_FLIP1			(0x1080)
+#define   NIC_PF_ECC1_FLIP1			(0x1088)
+#define   NIC_PF_ECC2_FLIP1			(0x1090)
+#define   NIC_PF_ECC3_FLIP1			(0x1098)
+#define   NIC_PF_ECC0_CDIS			(0x1100)
+#define   NIC_PF_ECC1_CDIS			(0x1108)
+#define   NIC_PF_ECC2_CDIS			(0x1110)
+#define   NIC_PF_ECC3_CDIS			(0x1118)
+#define   NIC_PF_BIST0_STATUS			(0x1280)
+#define   NIC_PF_BIST1_STATUS			(0x1288)
+#define   NIC_PF_BIST2_STATUS			(0x1290)
+#define   NIC_PF_BIST3_STATUS			(0x1298)
+#define   NIC_PF_ECC0_SBE_INT			(0x2000)
+#define   NIC_PF_ECC0_SBE_INT_W1S		(0x2008)
+#define   NIC_PF_ECC0_SBE_ENA_W1C		(0x2010)
+#define   NIC_PF_ECC0_SBE_ENA_W1S		(0x2018)
+#define   NIC_PF_ECC0_DBE_INT			(0x2100)
+#define   NIC_PF_ECC0_DBE_INT_W1S		(0x2108)
+#define   NIC_PF_ECC0_DBE_ENA_W1C		(0x2110)
+#define   NIC_PF_ECC0_DBE_ENA_W1S		(0x2118)
+#define   NIC_PF_ECC1_SBE_INT			(0x2200)
+#define   NIC_PF_ECC1_SBE_INT_W1S		(0x2208)
+#define   NIC_PF_ECC1_SBE_ENA_W1C		(0x2210)
+#define   NIC_PF_ECC1_SBE_ENA_W1S		(0x2218)
+#define   NIC_PF_ECC1_DBE_INT			(0x2300)
+#define   NIC_PF_ECC1_DBE_INT_W1S		(0x2308)
+#define   NIC_PF_ECC1_DBE_ENA_W1C		(0x2310)
+#define   NIC_PF_ECC1_DBE_ENA_W1S		(0x2318)
+#define   NIC_PF_ECC2_SBE_INT			(0x2400)
+#define   NIC_PF_ECC2_SBE_INT_W1S		(0x2408)
+#define   NIC_PF_ECC2_SBE_ENA_W1C		(0x2410)
+#define   NIC_PF_ECC2_SBE_ENA_W1S		(0x2418)
+#define   NIC_PF_ECC2_DBE_INT			(0x2500)
+#define   NIC_PF_ECC2_DBE_INT_W1S		(0x2508)
+#define   NIC_PF_ECC2_DBE_ENA_W1C		(0x2510)
+#define   NIC_PF_ECC2_DBE_ENA_W1S		(0x2518)
+#define   NIC_PF_ECC3_SBE_INT			(0x2600)
+#define   NIC_PF_ECC3_SBE_INT_W1S		(0x2608)
+#define   NIC_PF_ECC3_SBE_ENA_W1C		(0x2610)
+#define   NIC_PF_ECC3_SBE_ENA_W1S		(0x2618)
+#define   NIC_PF_ECC3_DBE_INT			(0x2700)
+#define   NIC_PF_ECC3_DBE_INT_W1S		(0x2708)
+#define   NIC_PF_ECC3_DBE_ENA_W1C		(0x2710)
+#define   NIC_PF_ECC3_DBE_ENA_W1S		(0x2718)
+#define   NIC_PF_CPI_0_2047_CFG			(0x200000)
+#define   NIC_PF_RSSI_0_4097_RQ			(0x220000)
+#define   NIC_PF_LMAC_0_7_CFG			(0x240000)
+#define   NIC_PF_LMAC_0_7_SW_XOFF		(0x242000)
+#define   NIC_PF_LMAC_0_7_CREDIT		(0x244000)
+#define   NIC_PF_CHAN_0_255_TX_CFG		(0x400000)
+#define   NIC_PF_CHAN_0_255_RX_CFG		(0x420000)
+#define   NIC_PF_CHAN_0_255_SW_XOFF		(0x440000)
+#define   NIC_PF_CHAN_0_255_CREDIT		(0x460000)
+#define   NIC_PF_CHAN_0_255_RX_BP_CFG		(0x480000)
+#define   NIC_PF_SW_SYNC_RX			(0x490000)
+#define   NIC_PF_SW_SYNC_RX_DONE		(0x490008)
+#define   NIC_PF_TL2_0_63_CFG			(0x500000)
+#define   NIC_PF_TL2_0_63_PRI			(0x520000)
+#define   NIC_PF_TL2_0_63_SH_STATUS		(0x580000)
+#define   NIC_PF_TL3A_0_63_CFG			(0x5F0000)
+#define   NIC_PF_TL3_0_255_CFG			(0x600000)
+#define   NIC_PF_TL3_0_255_CHAN			(0x620000)
+#define   NIC_PF_TL3_0_255_PIR			(0x640000)
+#define   NIC_PF_TL3_0_255_SW_XOFF		(0x660000)
+#define   NIC_PF_TL3_0_255_CNM_RATE		(0x680000)
+#define   NIC_PF_TL3_0_255_SH_STATUS		(0x6A0000)
+#define   NIC_PF_TL4A_0_255_CFG			(0x6F0000)
+#define   NIC_PF_TL4_0_1023_CFG			(0x800000)
+#define   NIC_PF_TL4_0_1023_SW_XOFF		(0x820000)
+#define   NIC_PF_TL4_0_1023_SH_STATUS		(0x840000)
+#define   NIC_PF_TL4A_0_1023_CNM_RATE		(0x880000)
+#define   NIC_PF_TL4A_0_1023_CNM_STATUS		(0x8A0000)
+#define   NIC_PF_VF_0_127_MAILBOX_0_1		(0x20002030)
+#define   NIC_PF_VNIC_0_127_TX_STAT_0_4		(0x20004000)
+#define   NIC_PF_VNIC_0_127_RX_STAT_0_13	(0x20004100)
+#define   NIC_PF_QSET_0_127_LOCK_0_15		(0x20006000)
+#define   NIC_PF_QSET_0_127_CFG			(0x20010000)
+#define   NIC_PF_QSET_0_127_RQ_0_7_CFG		(0x20010400)
+#define   NIC_PF_QSET_0_127_RQ_0_7_DROP_CFG	(0x20010420)
+#define   NIC_PF_QSET_0_127_RQ_0_7_BP_CFG	(0x20010500)
+#define   NIC_PF_QSET_0_127_RQ_0_7_STAT_0_1	(0x20010600)
+#define   NIC_PF_QSET_0_127_SQ_0_7_CFG		(0x20010C00)
+#define   NIC_PF_QSET_0_127_SQ_0_7_CFG2		(0x20010C08)
+#define   NIC_PF_QSET_0_127_SQ_0_7_STAT_0_1	(0x20010D00)
+
+#define   NIC_PF_MSIX_VEC_0_18_ADDR		(0x000000)
+#define   NIC_PF_MSIX_VEC_0_CTL			(0x000008)
+#define   NIC_PF_MSIX_PBA_0			(0x0F0000)
+
+/* Virtual function register offsets */
+#define   NIC_VNIC_CFG				(0x000020)
+#define   NIC_VF_PF_MAILBOX_0_1			(0x000130)
+#define   NIC_VF_INT				(0x000200)
+#define   NIC_VF_INT_W1S			(0x000220)
+#define   NIC_VF_ENA_W1C			(0x000240)
+#define   NIC_VF_ENA_W1S			(0x000260)
+
+#define   NIC_VNIC_RSS_CFG			(0x0020E0)
+#define   NIC_VNIC_RSS_KEY_0_4			(0x002200)
+#define   NIC_VNIC_TX_STAT_0_4			(0x004000)
+#define   NIC_VNIC_RX_STAT_0_13			(0x004100)
+#define   NIC_QSET_RQ_GEN_CFG			(0x010010)
+
+#define   NIC_QSET_CQ_0_7_CFG			(0x010400)
+#define   NIC_QSET_CQ_0_7_CFG2			(0x010408)
+#define   NIC_QSET_CQ_0_7_THRESH		(0x010410)
+#define   NIC_QSET_CQ_0_7_BASE			(0x010420)
+#define   NIC_QSET_CQ_0_7_HEAD			(0x010428)
+#define   NIC_QSET_CQ_0_7_TAIL			(0x010430)
+#define   NIC_QSET_CQ_0_7_DOOR			(0x010438)
+#define   NIC_QSET_CQ_0_7_STATUS		(0x010440)
+#define   NIC_QSET_CQ_0_7_STATUS2		(0x010448)
+#define   NIC_QSET_CQ_0_7_DEBUG			(0x010450)
+
+#define   NIC_QSET_RQ_0_7_CFG			(0x010600)
+#define   NIC_QSET_RQ_0_7_STAT_0_1		(0x010700)
+
+#define   NIC_QSET_SQ_0_7_CFG			(0x010800)
+#define   NIC_QSET_SQ_0_7_THRESH		(0x010810)
+#define   NIC_QSET_SQ_0_7_BASE			(0x010820)
+#define   NIC_QSET_SQ_0_7_HEAD			(0x010828)
+#define   NIC_QSET_SQ_0_7_TAIL			(0x010830)
+#define   NIC_QSET_SQ_0_7_DOOR			(0x010838)
+#define   NIC_QSET_SQ_0_7_STATUS		(0x010840)
+#define   NIC_QSET_SQ_0_7_DEBUG			(0x010848)
+#define   NIC_QSET_SQ_0_7_CNM_CHG		(0x010860)
+#define   NIC_QSET_SQ_0_7_STAT_0_1		(0x010900)
+
+#define   NIC_QSET_RBDR_0_1_CFG			(0x010C00)
+#define   NIC_QSET_RBDR_0_1_THRESH		(0x010C10)
+#define   NIC_QSET_RBDR_0_1_BASE		(0x010C20)
+#define   NIC_QSET_RBDR_0_1_HEAD		(0x010C28)
+#define   NIC_QSET_RBDR_0_1_TAIL		(0x010C30)
+#define   NIC_QSET_RBDR_0_1_DOOR		(0x010C38)
+#define   NIC_QSET_RBDR_0_1_STATUS0		(0x010C40)
+#define   NIC_QSET_RBDR_0_1_STATUS1		(0x010C48)
+#define   NIC_QSET_RBDR_0_1_PREFETCH_STATUS	(0x010C50)
+
+#define   NIC_VF_MSIX_VECTOR_0_19_ADDR		(0x000000)
+#define   NIC_VF_MSIX_VECTOR_0_19_CTL		(0x000008)
+#define   NIC_VF_MSIX_PBA			(0x0F0000)
+
+/* Offsets within registers */
+#define   NIC_MSIX_VEC_SHIFT			4
+#define   NIC_Q_NUM_SHIFT			18
+#define   NIC_QS_ID_SHIFT			21
+#define   NIC_VF_NUM_SHIFT			21
+
+/* Port kind configuration register */
+struct pkind_cfg {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64 reserved_42_63:22;
+	u64 hdr_sl:5;	/* Header skip length */
+	u64 rx_hdr:3;	/* TNS Receive header present */
+	u64 lenerr_en:1;/* L2 length error check enable */
+	u64 reserved_32_32:1;
+	u64 maxlen:16;	/* Max frame size */
+	u64 minlen:16;	/* Min frame size */
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64 minlen:16;
+	u64 maxlen:16;
+	u64 reserved_32_32:1;
+	u64 lenerr_en:1;
+	u64 rx_hdr:3;
+	u64 hdr_sl:5;
+	u64 reserved_42_63:22;
+#endif
+};
+
+#endif /* NIC_REG_H */
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_ethtool.c b/drivers/net/ethernet/cavium/thunder/nicvf_ethtool.c
new file mode 100644
index 000000000000..16bd2d772db9
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_ethtool.c
@@ -0,0 +1,600 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * 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.
+ */
+
+/* ETHTOOL Support for VNIC_VF Device*/
+
+#include <linux/pci.h>
+
+#include "nic_reg.h"
+#include "nic.h"
+#include "nicvf_queues.h"
+#include "q_struct.h"
+#include "thunder_bgx.h"
+
+#define DRV_NAME	"thunder-nicvf"
+#define DRV_VERSION     "1.0"
+
+struct nicvf_stat {
+	char name[ETH_GSTRING_LEN];
+	unsigned int index;
+};
+
+#define NICVF_HW_STAT(stat) { \
+	.name = #stat, \
+	.index = offsetof(struct nicvf_hw_stats, stat) / sizeof(u64), \
+}
+
+#define NICVF_DRV_STAT(stat) { \
+	.name = #stat, \
+	.index = offsetof(struct nicvf_drv_stats, stat) / sizeof(u64), \
+}
+
+static const struct nicvf_stat nicvf_hw_stats[] = {
+	NICVF_HW_STAT(rx_bytes_ok),
+	NICVF_HW_STAT(rx_ucast_frames_ok),
+	NICVF_HW_STAT(rx_bcast_frames_ok),
+	NICVF_HW_STAT(rx_mcast_frames_ok),
+	NICVF_HW_STAT(rx_fcs_errors),
+	NICVF_HW_STAT(rx_l2_errors),
+	NICVF_HW_STAT(rx_drop_red),
+	NICVF_HW_STAT(rx_drop_red_bytes),
+	NICVF_HW_STAT(rx_drop_overrun),
+	NICVF_HW_STAT(rx_drop_overrun_bytes),
+	NICVF_HW_STAT(rx_drop_bcast),
+	NICVF_HW_STAT(rx_drop_mcast),
+	NICVF_HW_STAT(rx_drop_l3_bcast),
+	NICVF_HW_STAT(rx_drop_l3_mcast),
+	NICVF_HW_STAT(tx_bytes_ok),
+	NICVF_HW_STAT(tx_ucast_frames_ok),
+	NICVF_HW_STAT(tx_bcast_frames_ok),
+	NICVF_HW_STAT(tx_mcast_frames_ok),
+};
+
+static const struct nicvf_stat nicvf_drv_stats[] = {
+	NICVF_DRV_STAT(rx_frames_ok),
+	NICVF_DRV_STAT(rx_frames_64),
+	NICVF_DRV_STAT(rx_frames_127),
+	NICVF_DRV_STAT(rx_frames_255),
+	NICVF_DRV_STAT(rx_frames_511),
+	NICVF_DRV_STAT(rx_frames_1023),
+	NICVF_DRV_STAT(rx_frames_1518),
+	NICVF_DRV_STAT(rx_frames_jumbo),
+	NICVF_DRV_STAT(rx_drops),
+	NICVF_DRV_STAT(tx_frames_ok),
+	NICVF_DRV_STAT(tx_busy),
+	NICVF_DRV_STAT(tx_tso),
+	NICVF_DRV_STAT(tx_drops),
+};
+
+static const struct nicvf_stat nicvf_queue_stats[] = {
+	{ "bytes", 0 },
+	{ "frames", 1 },
+};
+
+static const unsigned int nicvf_n_hw_stats = ARRAY_SIZE(nicvf_hw_stats);
+static const unsigned int nicvf_n_drv_stats = ARRAY_SIZE(nicvf_drv_stats);
+static const unsigned int nicvf_n_queue_stats = ARRAY_SIZE(nicvf_queue_stats);
+
+static int nicvf_get_settings(struct net_device *netdev,
+			      struct ethtool_cmd *cmd)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+
+	cmd->supported = 0;
+	cmd->transceiver = XCVR_EXTERNAL;
+	if (nic->speed <= 1000) {
+		cmd->port = PORT_MII;
+		cmd->autoneg = AUTONEG_ENABLE;
+	} else {
+		cmd->port = PORT_FIBRE;
+		cmd->autoneg = AUTONEG_DISABLE;
+	}
+	cmd->duplex = nic->duplex;
+	ethtool_cmd_speed_set(cmd, nic->speed);
+
+	return 0;
+}
+
+static void nicvf_get_drvinfo(struct net_device *netdev,
+			      struct ethtool_drvinfo *info)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+
+	strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
+	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
+	strlcpy(info->bus_info, pci_name(nic->pdev), sizeof(info->bus_info));
+}
+
+static u32 nicvf_get_msglevel(struct net_device *netdev)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+
+	return nic->msg_enable;
+}
+
+static void nicvf_set_msglevel(struct net_device *netdev, u32 lvl)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+
+	nic->msg_enable = lvl;
+}
+
+static void nicvf_get_strings(struct net_device *netdev, u32 sset, u8 *data)
+{
+	int stats, qidx;
+
+	if (sset != ETH_SS_STATS)
+		return;
+
+	for (stats = 0; stats < nicvf_n_hw_stats; stats++) {
+		memcpy(data, nicvf_hw_stats[stats].name, ETH_GSTRING_LEN);
+		data += ETH_GSTRING_LEN;
+	}
+
+	for (stats = 0; stats < nicvf_n_drv_stats; stats++) {
+		memcpy(data, nicvf_drv_stats[stats].name, ETH_GSTRING_LEN);
+		data += ETH_GSTRING_LEN;
+	}
+
+	for (qidx = 0; qidx < MAX_RCV_QUEUES_PER_QS; qidx++) {
+		for (stats = 0; stats < nicvf_n_queue_stats; stats++) {
+			sprintf(data, "rxq%d: %s", qidx,
+				nicvf_queue_stats[stats].name);
+			data += ETH_GSTRING_LEN;
+		}
+	}
+
+	for (qidx = 0; qidx < MAX_SND_QUEUES_PER_QS; qidx++) {
+		for (stats = 0; stats < nicvf_n_queue_stats; stats++) {
+			sprintf(data, "txq%d: %s", qidx,
+				nicvf_queue_stats[stats].name);
+			data += ETH_GSTRING_LEN;
+		}
+	}
+
+	for (stats = 0; stats < BGX_RX_STATS_COUNT; stats++) {
+		sprintf(data, "bgx_rxstat%d: ", stats);
+		data += ETH_GSTRING_LEN;
+	}
+
+	for (stats = 0; stats < BGX_TX_STATS_COUNT; stats++) {
+		sprintf(data, "bgx_txstat%d: ", stats);
+		data += ETH_GSTRING_LEN;
+	}
+}
+
+static int nicvf_get_sset_count(struct net_device *netdev, int sset)
+{
+	if (sset != ETH_SS_STATS)
+		return -EINVAL;
+
+	return nicvf_n_hw_stats + nicvf_n_drv_stats +
+		(nicvf_n_queue_stats *
+		 (MAX_RCV_QUEUES_PER_QS + MAX_SND_QUEUES_PER_QS)) +
+		BGX_RX_STATS_COUNT + BGX_TX_STATS_COUNT;
+}
+
+static void nicvf_get_ethtool_stats(struct net_device *netdev,
+				    struct ethtool_stats *stats, u64 *data)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+	int stat, qidx;
+
+	nicvf_update_stats(nic);
+
+	/* Update LMAC stats */
+	nicvf_update_lmac_stats(nic);
+
+	for (stat = 0; stat < nicvf_n_hw_stats; stat++)
+		*(data++) = ((u64 *)&nic->stats)
+				[nicvf_hw_stats[stat].index];
+	for (stat = 0; stat < nicvf_n_drv_stats; stat++)
+		*(data++) = ((u64 *)&nic->drv_stats)
+				[nicvf_drv_stats[stat].index];
+
+	for (qidx = 0; qidx < MAX_RCV_QUEUES_PER_QS; qidx++) {
+		for (stat = 0; stat < nicvf_n_queue_stats; stat++)
+			*(data++) = ((u64 *)&nic->qs->rq[qidx].stats)
+					[nicvf_queue_stats[stat].index];
+	}
+
+	for (qidx = 0; qidx < MAX_SND_QUEUES_PER_QS; qidx++) {
+		for (stat = 0; stat < nicvf_n_queue_stats; stat++)
+			*(data++) = ((u64 *)&nic->qs->sq[qidx].stats)
+					[nicvf_queue_stats[stat].index];
+	}
+
+	for (stat = 0; stat < BGX_RX_STATS_COUNT; stat++)
+		*(data++) = nic->bgx_stats.rx_stats[stat];
+	for (stat = 0; stat < BGX_TX_STATS_COUNT; stat++)
+		*(data++) = nic->bgx_stats.tx_stats[stat];
+}
+
+static int nicvf_get_regs_len(struct net_device *dev)
+{
+	return sizeof(u64) * NIC_VF_REG_COUNT;
+}
+
+static void nicvf_get_regs(struct net_device *dev,
+			   struct ethtool_regs *regs, void *reg)
+{
+	struct nicvf *nic = netdev_priv(dev);
+	u64 *p = (u64 *)reg;
+	u64 reg_offset;
+	int mbox, key, stat, q;
+	int i = 0;
+
+	regs->version = 0;
+	memset(p, 0, NIC_VF_REG_COUNT);
+
+	p[i++] = nicvf_reg_read(nic, NIC_VNIC_CFG);
+	/* Mailbox registers */
+	for (mbox = 0; mbox < NIC_PF_VF_MAILBOX_SIZE; mbox++)
+		p[i++] = nicvf_reg_read(nic,
+					NIC_VF_PF_MAILBOX_0_1 | (mbox << 3));
+
+	p[i++] = nicvf_reg_read(nic, NIC_VF_INT);
+	p[i++] = nicvf_reg_read(nic, NIC_VF_INT_W1S);
+	p[i++] = nicvf_reg_read(nic, NIC_VF_ENA_W1C);
+	p[i++] = nicvf_reg_read(nic, NIC_VF_ENA_W1S);
+	p[i++] = nicvf_reg_read(nic, NIC_VNIC_RSS_CFG);
+
+	for (key = 0; key < RSS_HASH_KEY_SIZE; key++)
+		p[i++] = nicvf_reg_read(nic, NIC_VNIC_RSS_KEY_0_4 | (key << 3));
+
+	/* Tx/Rx statistics */
+	for (stat = 0; stat < TX_STATS_ENUM_LAST; stat++)
+		p[i++] = nicvf_reg_read(nic,
+					NIC_VNIC_TX_STAT_0_4 | (stat << 3));
+
+	for (i = 0; i < RX_STATS_ENUM_LAST; i++)
+		p[i++] = nicvf_reg_read(nic,
+					NIC_VNIC_RX_STAT_0_13 | (stat << 3));
+
+	p[i++] = nicvf_reg_read(nic, NIC_QSET_RQ_GEN_CFG);
+
+	/* All completion queue's registers */
+	for (q = 0; q < MAX_CMP_QUEUES_PER_QS; q++) {
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_CFG, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_CFG2, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_THRESH, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_BASE, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_HEAD, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_TAIL, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_DOOR, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_STATUS, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_STATUS2, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_DEBUG, q);
+	}
+
+	/* All receive queue's registers */
+	for (q = 0; q < MAX_RCV_QUEUES_PER_QS; q++) {
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_RQ_0_7_CFG, q);
+		p[i++] = nicvf_queue_reg_read(nic,
+						  NIC_QSET_RQ_0_7_STAT_0_1, q);
+		reg_offset = NIC_QSET_RQ_0_7_STAT_0_1 | (1 << 3);
+		p[i++] = nicvf_queue_reg_read(nic, reg_offset, q);
+	}
+
+	for (q = 0; q < MAX_SND_QUEUES_PER_QS; q++) {
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_CFG, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_THRESH, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_BASE, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_HEAD, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_TAIL, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_DOOR, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_STATUS, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_DEBUG, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_CNM_CHG, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_STAT_0_1, q);
+		reg_offset = NIC_QSET_SQ_0_7_STAT_0_1 | (1 << 3);
+		p[i++] = nicvf_queue_reg_read(nic, reg_offset, q);
+	}
+
+	for (q = 0; q < MAX_RCV_BUF_DESC_RINGS_PER_QS; q++) {
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_CFG, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_THRESH, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_BASE, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_HEAD, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_TAIL, q);
+		p[i++] = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_DOOR, q);
+		p[i++] = nicvf_queue_reg_read(nic,
+					      NIC_QSET_RBDR_0_1_STATUS0, q);
+		p[i++] = nicvf_queue_reg_read(nic,
+					      NIC_QSET_RBDR_0_1_STATUS1, q);
+		reg_offset = NIC_QSET_RBDR_0_1_PREFETCH_STATUS;
+		p[i++] = nicvf_queue_reg_read(nic, reg_offset, q);
+	}
+}
+
+static int nicvf_get_coalesce(struct net_device *netdev,
+			      struct ethtool_coalesce *cmd)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+
+	cmd->rx_coalesce_usecs = nic->cq_coalesce_usecs;
+	return 0;
+}
+
+static void nicvf_get_ringparam(struct net_device *netdev,
+				struct ethtool_ringparam *ring)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+	struct queue_set *qs = nic->qs;
+
+	ring->rx_max_pending = MAX_RCV_BUF_COUNT;
+	ring->rx_pending = qs->rbdr_len;
+	ring->tx_max_pending = MAX_SND_QUEUE_LEN;
+	ring->tx_pending = qs->sq_len;
+}
+
+static int nicvf_get_rss_hash_opts(struct nicvf *nic,
+				   struct ethtool_rxnfc *info)
+{
+	info->data = 0;
+
+	switch (info->flow_type) {
+	case TCP_V4_FLOW:
+	case TCP_V6_FLOW:
+	case UDP_V4_FLOW:
+	case UDP_V6_FLOW:
+	case SCTP_V4_FLOW:
+	case SCTP_V6_FLOW:
+		info->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+	case IPV4_FLOW:
+	case IPV6_FLOW:
+		info->data |= RXH_IP_SRC | RXH_IP_DST;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int nicvf_get_rxnfc(struct net_device *dev,
+			   struct ethtool_rxnfc *info, u32 *rules)
+{
+	struct nicvf *nic = netdev_priv(dev);
+	int ret = -EOPNOTSUPP;
+
+	switch (info->cmd) {
+	case ETHTOOL_GRXRINGS:
+		info->data = nic->qs->rq_cnt;
+		ret = 0;
+		break;
+	case ETHTOOL_GRXFH:
+		return nicvf_get_rss_hash_opts(nic, info);
+	default:
+		break;
+	}
+	return ret;
+}
+
+static int nicvf_set_rss_hash_opts(struct nicvf *nic,
+				   struct ethtool_rxnfc *info)
+{
+	struct nicvf_rss_info *rss = &nic->rss_info;
+	u64 rss_cfg = nicvf_reg_read(nic, NIC_VNIC_RSS_CFG);
+
+	if (!rss->enable)
+		netdev_err(nic->netdev,
+			   "RSS is disabled, hash cannot be set\n");
+
+	netdev_info(nic->netdev, "Set RSS flow type = %d, data = %lld\n",
+		    info->flow_type, info->data);
+
+	if (!(info->data & RXH_IP_SRC) || !(info->data & RXH_IP_DST))
+		return -EINVAL;
+
+	switch (info->flow_type) {
+	case TCP_V4_FLOW:
+	case TCP_V6_FLOW:
+		switch (info->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+		case 0:
+			rss_cfg &= ~(1ULL << RSS_HASH_TCP);
+			break;
+		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
+			rss_cfg |= (1ULL << RSS_HASH_TCP);
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case UDP_V4_FLOW:
+	case UDP_V6_FLOW:
+		switch (info->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+		case 0:
+			rss_cfg &= ~(1ULL << RSS_HASH_UDP);
+			break;
+		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
+			rss_cfg |= (1ULL << RSS_HASH_UDP);
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case SCTP_V4_FLOW:
+	case SCTP_V6_FLOW:
+		switch (info->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+		case 0:
+			rss_cfg &= ~(1ULL << RSS_HASH_L4ETC);
+			break;
+		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
+			rss_cfg |= (1ULL << RSS_HASH_L4ETC);
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case IPV4_FLOW:
+	case IPV6_FLOW:
+		rss_cfg = RSS_HASH_IP;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	nicvf_reg_write(nic, NIC_VNIC_RSS_CFG, rss_cfg);
+	return 0;
+}
+
+static int nicvf_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info)
+{
+	struct nicvf *nic = netdev_priv(dev);
+
+	switch (info->cmd) {
+	case ETHTOOL_SRXFH:
+		return nicvf_set_rss_hash_opts(nic, info);
+	default:
+		break;
+	}
+	return -EOPNOTSUPP;
+}
+
+static u32 nicvf_get_rxfh_key_size(struct net_device *netdev)
+{
+	return RSS_HASH_KEY_SIZE * sizeof(u64);
+}
+
+static u32 nicvf_get_rxfh_indir_size(struct net_device *dev)
+{
+	struct nicvf *nic = netdev_priv(dev);
+
+	return nic->rss_info.rss_size;
+}
+
+static int nicvf_get_rxfh(struct net_device *dev, u32 *indir, u8 *hkey,
+			  u8 *hfunc)
+{
+	struct nicvf *nic = netdev_priv(dev);
+	struct nicvf_rss_info *rss = &nic->rss_info;
+	int idx;
+
+	if (indir) {
+		for (idx = 0; idx < rss->rss_size; idx++)
+			indir[idx] = rss->ind_tbl[idx];
+	}
+
+	if (hkey)
+		memcpy(hkey, rss->key, RSS_HASH_KEY_SIZE * sizeof(u64));
+
+	if (hfunc)
+		*hfunc = ETH_RSS_HASH_TOP;
+
+	return 0;
+}
+
+static int nicvf_set_rxfh(struct net_device *dev, const u32 *indir,
+			  const u8 *hkey, u8 hfunc)
+{
+	struct nicvf *nic = netdev_priv(dev);
+	struct nicvf_rss_info *rss = &nic->rss_info;
+	int idx;
+
+	if ((nic->qs->rq_cnt <= 1) || (nic->cpi_alg != CPI_ALG_NONE)) {
+		rss->enable = false;
+		rss->hash_bits = 0;
+		return -EIO;
+	}
+
+	/* We do not allow change in unsupported parameters */
+	if (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_TOP)
+		return -EOPNOTSUPP;
+
+	rss->enable = true;
+	if (indir) {
+		for (idx = 0; idx < rss->rss_size; idx++)
+			rss->ind_tbl[idx] = indir[idx];
+	}
+
+	if (hkey) {
+		memcpy(rss->key, hkey, RSS_HASH_KEY_SIZE * sizeof(u64));
+		nicvf_set_rss_key(nic);
+	}
+
+	nicvf_config_rss(nic);
+	return 0;
+}
+
+/* Get no of queues device supports and current queue count */
+static void nicvf_get_channels(struct net_device *dev,
+			       struct ethtool_channels *channel)
+{
+	struct nicvf *nic = netdev_priv(dev);
+
+	memset(channel, 0, sizeof(*channel));
+
+	channel->max_rx = MAX_RCV_QUEUES_PER_QS;
+	channel->max_tx = MAX_SND_QUEUES_PER_QS;
+
+	channel->rx_count = nic->qs->rq_cnt;
+	channel->tx_count = nic->qs->sq_cnt;
+}
+
+/* Set no of Tx, Rx queues to be used */
+static int nicvf_set_channels(struct net_device *dev,
+			      struct ethtool_channels *channel)
+{
+	struct nicvf *nic = netdev_priv(dev);
+	int err = 0;
+
+	if (!channel->rx_count || !channel->tx_count)
+		return -EINVAL;
+	if (channel->rx_count > MAX_RCV_QUEUES_PER_QS)
+		return -EINVAL;
+	if (channel->tx_count > MAX_SND_QUEUES_PER_QS)
+		return -EINVAL;
+
+	nic->qs->rq_cnt = channel->rx_count;
+	nic->qs->sq_cnt = channel->tx_count;
+	nic->qs->cq_cnt = max(nic->qs->rq_cnt, nic->qs->sq_cnt);
+
+	err = nicvf_set_real_num_queues(dev, nic->qs->sq_cnt, nic->qs->rq_cnt);
+	if (err)
+		return err;
+
+	if (!netif_running(dev))
+		return err;
+
+	nicvf_stop(dev);
+	nicvf_open(dev);
+	netdev_info(dev, "Setting num Tx rings to %d, Rx rings to %d success\n",
+		    nic->qs->sq_cnt, nic->qs->rq_cnt);
+
+	return err;
+}
+
+static const struct ethtool_ops nicvf_ethtool_ops = {
+	.get_settings		= nicvf_get_settings,
+	.get_link		= ethtool_op_get_link,
+	.get_drvinfo		= nicvf_get_drvinfo,
+	.get_msglevel		= nicvf_get_msglevel,
+	.set_msglevel		= nicvf_set_msglevel,
+	.get_strings		= nicvf_get_strings,
+	.get_sset_count		= nicvf_get_sset_count,
+	.get_ethtool_stats	= nicvf_get_ethtool_stats,
+	.get_regs_len		= nicvf_get_regs_len,
+	.get_regs		= nicvf_get_regs,
+	.get_coalesce		= nicvf_get_coalesce,
+	.get_ringparam		= nicvf_get_ringparam,
+	.get_rxnfc		= nicvf_get_rxnfc,
+	.set_rxnfc		= nicvf_set_rxnfc,
+	.get_rxfh_key_size	= nicvf_get_rxfh_key_size,
+	.get_rxfh_indir_size	= nicvf_get_rxfh_indir_size,
+	.get_rxfh		= nicvf_get_rxfh,
+	.set_rxfh		= nicvf_set_rxfh,
+	.get_channels		= nicvf_get_channels,
+	.set_channels		= nicvf_set_channels,
+	.get_ts_info		= ethtool_op_get_ts_info,
+};
+
+void nicvf_set_ethtool_ops(struct net_device *netdev)
+{
+	netdev->ethtool_ops = &nicvf_ethtool_ops;
+}
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_main.c b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
new file mode 100644
index 000000000000..8b119a035b7e
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
@@ -0,0 +1,1341 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/log2.h>
+#include <linux/prefetch.h>
+#include <linux/irq.h>
+
+#include "nic_reg.h"
+#include "nic.h"
+#include "nicvf_queues.h"
+#include "thunder_bgx.h"
+
+#define DRV_NAME	"thunder-nicvf"
+#define DRV_VERSION	"1.0"
+
+/* Supported devices */
+static const struct pci_device_id nicvf_id_table[] = {
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_CAVIUM,
+			 PCI_DEVICE_ID_THUNDER_NIC_VF,
+			 PCI_VENDOR_ID_CAVIUM, 0xA11E) },
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_CAVIUM,
+			 PCI_DEVICE_ID_THUNDER_PASS1_NIC_VF,
+			 PCI_VENDOR_ID_CAVIUM, 0xA11E) },
+	{ 0, }  /* end of table */
+};
+
+MODULE_AUTHOR("Sunil Goutham");
+MODULE_DESCRIPTION("Cavium Thunder NIC Virtual Function Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DRV_VERSION);
+MODULE_DEVICE_TABLE(pci, nicvf_id_table);
+
+static int debug = 0x00;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "Debug message level bitmap");
+
+static int cpi_alg = CPI_ALG_NONE;
+module_param(cpi_alg, int, S_IRUGO);
+MODULE_PARM_DESC(cpi_alg,
+		 "PFC algorithm (0=none, 1=VLAN, 2=VLAN16, 3=IP Diffserv)");
+
+static inline void nicvf_set_rx_frame_cnt(struct nicvf *nic,
+					  struct sk_buff *skb)
+{
+	if (skb->len <= 64)
+		nic->drv_stats.rx_frames_64++;
+	else if (skb->len <= 127)
+		nic->drv_stats.rx_frames_127++;
+	else if (skb->len <= 255)
+		nic->drv_stats.rx_frames_255++;
+	else if (skb->len <= 511)
+		nic->drv_stats.rx_frames_511++;
+	else if (skb->len <= 1023)
+		nic->drv_stats.rx_frames_1023++;
+	else if (skb->len <= 1518)
+		nic->drv_stats.rx_frames_1518++;
+	else
+		nic->drv_stats.rx_frames_jumbo++;
+}
+
+/* The Cavium ThunderX network controller can *only* be found in SoCs
+ * containing the ThunderX ARM64 CPU implementation.  All accesses to the device
+ * registers on this platform are implicitly strongly ordered with respect
+ * to memory accesses. So writeq_relaxed() and readq_relaxed() are safe to use
+ * with no memory barriers in this driver.  The readq()/writeq() functions add
+ * explicit ordering operation which in this case are redundant, and only
+ * add overhead.
+ */
+
+/* Register read/write APIs */
+void nicvf_reg_write(struct nicvf *nic, u64 offset, u64 val)
+{
+	writeq_relaxed(val, nic->reg_base + offset);
+}
+
+u64 nicvf_reg_read(struct nicvf *nic, u64 offset)
+{
+	return readq_relaxed(nic->reg_base + offset);
+}
+
+void nicvf_queue_reg_write(struct nicvf *nic, u64 offset,
+			   u64 qidx, u64 val)
+{
+	void __iomem *addr = nic->reg_base + offset;
+
+	writeq_relaxed(val, addr + (qidx << NIC_Q_NUM_SHIFT));
+}
+
+u64 nicvf_queue_reg_read(struct nicvf *nic, u64 offset, u64 qidx)
+{
+	void __iomem *addr = nic->reg_base + offset;
+
+	return readq_relaxed(addr + (qidx << NIC_Q_NUM_SHIFT));
+}
+
+/* VF -> PF mailbox communication */
+
+static void nicvf_write_to_mbx(struct nicvf *nic, union nic_mbx *mbx)
+{
+	u64 *msg = (u64 *)mbx;
+
+	nicvf_reg_write(nic, NIC_VF_PF_MAILBOX_0_1 + 0, msg[0]);
+	nicvf_reg_write(nic, NIC_VF_PF_MAILBOX_0_1 + 8, msg[1]);
+}
+
+int nicvf_send_msg_to_pf(struct nicvf *nic, union nic_mbx *mbx)
+{
+	int timeout = NIC_MBOX_MSG_TIMEOUT;
+	int sleep = 10;
+
+	nic->pf_acked = false;
+	nic->pf_nacked = false;
+
+	nicvf_write_to_mbx(nic, mbx);
+
+	/* Wait for previous message to be acked, timeout 2sec */
+	while (!nic->pf_acked) {
+		if (nic->pf_nacked)
+			return -EINVAL;
+		msleep(sleep);
+		if (nic->pf_acked)
+			break;
+		timeout -= sleep;
+		if (!timeout) {
+			netdev_err(nic->netdev,
+				   "PF didn't ack to mbox msg %d from VF%d\n",
+				   (mbx->msg.msg & 0xFF), nic->vf_id);
+			return -EBUSY;
+		}
+	}
+	return 0;
+}
+
+/* Checks if VF is able to comminicate with PF
+* and also gets the VNIC number this VF is associated to.
+*/
+static int nicvf_check_pf_ready(struct nicvf *nic)
+{
+	int timeout = 5000, sleep = 20;
+	union nic_mbx mbx = {};
+
+	mbx.msg.msg = NIC_MBOX_MSG_READY;
+
+	nic->pf_ready_to_rcv_msg = false;
+
+	nicvf_write_to_mbx(nic, &mbx);
+
+	while (!nic->pf_ready_to_rcv_msg) {
+		msleep(sleep);
+		if (nic->pf_ready_to_rcv_msg)
+			break;
+		timeout -= sleep;
+		if (!timeout) {
+			netdev_err(nic->netdev,
+				   "PF didn't respond to READY msg\n");
+			return 0;
+		}
+	}
+	return 1;
+}
+
+static void nicvf_read_bgx_stats(struct nicvf *nic, struct bgx_stats_msg *bgx)
+{
+	if (bgx->rx)
+		nic->bgx_stats.rx_stats[bgx->idx] = bgx->stats;
+	else
+		nic->bgx_stats.tx_stats[bgx->idx] = bgx->stats;
+}
+
+static void  nicvf_handle_mbx_intr(struct nicvf *nic)
+{
+	union nic_mbx mbx = {};
+	u64 *mbx_data;
+	u64 mbx_addr;
+	int i;
+
+	mbx_addr = NIC_VF_PF_MAILBOX_0_1;
+	mbx_data = (u64 *)&mbx;
+
+	for (i = 0; i < NIC_PF_VF_MAILBOX_SIZE; i++) {
+		*mbx_data = nicvf_reg_read(nic, mbx_addr);
+		mbx_data++;
+		mbx_addr += sizeof(u64);
+	}
+
+	netdev_dbg(nic->netdev, "Mbox message: msg: 0x%x\n", mbx.msg.msg);
+	switch (mbx.msg.msg) {
+	case NIC_MBOX_MSG_READY:
+		nic->pf_ready_to_rcv_msg = true;
+		nic->vf_id = mbx.nic_cfg.vf_id & 0x7F;
+		nic->tns_mode = mbx.nic_cfg.tns_mode & 0x7F;
+		nic->node = mbx.nic_cfg.node_id;
+		if (!nic->set_mac_pending)
+			ether_addr_copy(nic->netdev->dev_addr,
+					mbx.nic_cfg.mac_addr);
+		nic->link_up = false;
+		nic->duplex = 0;
+		nic->speed = 0;
+		break;
+	case NIC_MBOX_MSG_ACK:
+		nic->pf_acked = true;
+		break;
+	case NIC_MBOX_MSG_NACK:
+		nic->pf_nacked = true;
+		break;
+	case NIC_MBOX_MSG_RSS_SIZE:
+		nic->rss_info.rss_size = mbx.rss_size.ind_tbl_size;
+		nic->pf_acked = true;
+		break;
+	case NIC_MBOX_MSG_BGX_STATS:
+		nicvf_read_bgx_stats(nic, &mbx.bgx_stats);
+		nic->pf_acked = true;
+		nic->bgx_stats_acked = true;
+		break;
+	case NIC_MBOX_MSG_BGX_LINK_CHANGE:
+		nic->pf_acked = true;
+		nic->link_up = mbx.link_status.link_up;
+		nic->duplex = mbx.link_status.duplex;
+		nic->speed = mbx.link_status.speed;
+		if (nic->link_up) {
+			netdev_info(nic->netdev, "%s: Link is Up %d Mbps %s\n",
+				    nic->netdev->name, nic->speed,
+				    nic->duplex == DUPLEX_FULL ?
+				"Full duplex" : "Half duplex");
+			netif_carrier_on(nic->netdev);
+			netif_tx_wake_all_queues(nic->netdev);
+		} else {
+			netdev_info(nic->netdev, "%s: Link is Down\n",
+				    nic->netdev->name);
+			netif_carrier_off(nic->netdev);
+			netif_tx_stop_all_queues(nic->netdev);
+		}
+		break;
+	default:
+		netdev_err(nic->netdev,
+			   "Invalid message from PF, msg 0x%x\n", mbx.msg.msg);
+		break;
+	}
+	nicvf_clear_intr(nic, NICVF_INTR_MBOX, 0);
+}
+
+static int nicvf_hw_set_mac_addr(struct nicvf *nic, struct net_device *netdev)
+{
+	union nic_mbx mbx = {};
+
+	mbx.mac.msg = NIC_MBOX_MSG_SET_MAC;
+	mbx.mac.vf_id = nic->vf_id;
+	ether_addr_copy(mbx.mac.mac_addr, netdev->dev_addr);
+
+	return nicvf_send_msg_to_pf(nic, &mbx);
+}
+
+static void nicvf_config_cpi(struct nicvf *nic)
+{
+	union nic_mbx mbx = {};
+
+	mbx.cpi_cfg.msg = NIC_MBOX_MSG_CPI_CFG;
+	mbx.cpi_cfg.vf_id = nic->vf_id;
+	mbx.cpi_cfg.cpi_alg = nic->cpi_alg;
+	mbx.cpi_cfg.rq_cnt = nic->qs->rq_cnt;
+
+	nicvf_send_msg_to_pf(nic, &mbx);
+}
+
+static void nicvf_get_rss_size(struct nicvf *nic)
+{
+	union nic_mbx mbx = {};
+
+	mbx.rss_size.msg = NIC_MBOX_MSG_RSS_SIZE;
+	mbx.rss_size.vf_id = nic->vf_id;
+	nicvf_send_msg_to_pf(nic, &mbx);
+}
+
+void nicvf_config_rss(struct nicvf *nic)
+{
+	union nic_mbx mbx = {};
+	struct nicvf_rss_info *rss = &nic->rss_info;
+	int ind_tbl_len = rss->rss_size;
+	int i, nextq = 0;
+
+	mbx.rss_cfg.vf_id = nic->vf_id;
+	mbx.rss_cfg.hash_bits = rss->hash_bits;
+	while (ind_tbl_len) {
+		mbx.rss_cfg.tbl_offset = nextq;
+		mbx.rss_cfg.tbl_len = min(ind_tbl_len,
+					       RSS_IND_TBL_LEN_PER_MBX_MSG);
+		mbx.rss_cfg.msg = mbx.rss_cfg.tbl_offset ?
+			  NIC_MBOX_MSG_RSS_CFG_CONT : NIC_MBOX_MSG_RSS_CFG;
+
+		for (i = 0; i < mbx.rss_cfg.tbl_len; i++)
+			mbx.rss_cfg.ind_tbl[i] = rss->ind_tbl[nextq++];
+
+		nicvf_send_msg_to_pf(nic, &mbx);
+
+		ind_tbl_len -= mbx.rss_cfg.tbl_len;
+	}
+}
+
+void nicvf_set_rss_key(struct nicvf *nic)
+{
+	struct nicvf_rss_info *rss = &nic->rss_info;
+	u64 key_addr = NIC_VNIC_RSS_KEY_0_4;
+	int idx;
+
+	for (idx = 0; idx < RSS_HASH_KEY_SIZE; idx++) {
+		nicvf_reg_write(nic, key_addr, rss->key[idx]);
+		key_addr += sizeof(u64);
+	}
+}
+
+static int nicvf_rss_init(struct nicvf *nic)
+{
+	struct nicvf_rss_info *rss = &nic->rss_info;
+	int idx;
+
+	nicvf_get_rss_size(nic);
+
+	if ((nic->qs->rq_cnt <= 1) || (cpi_alg != CPI_ALG_NONE)) {
+		rss->enable = false;
+		rss->hash_bits = 0;
+		return 0;
+	}
+
+	rss->enable = true;
+
+	/* Using the HW reset value for now */
+	rss->key[0] = 0xFEED0BADFEED0BADULL;
+	rss->key[1] = 0xFEED0BADFEED0BADULL;
+	rss->key[2] = 0xFEED0BADFEED0BADULL;
+	rss->key[3] = 0xFEED0BADFEED0BADULL;
+	rss->key[4] = 0xFEED0BADFEED0BADULL;
+
+	nicvf_set_rss_key(nic);
+
+	rss->cfg = RSS_IP_HASH_ENA | RSS_TCP_HASH_ENA | RSS_UDP_HASH_ENA;
+	nicvf_reg_write(nic, NIC_VNIC_RSS_CFG, rss->cfg);
+
+	rss->hash_bits =  ilog2(rounddown_pow_of_two(rss->rss_size));
+
+	for (idx = 0; idx < rss->rss_size; idx++)
+		rss->ind_tbl[idx] = ethtool_rxfh_indir_default(idx,
+							       nic->qs->rq_cnt);
+	nicvf_config_rss(nic);
+	return 1;
+}
+
+int nicvf_set_real_num_queues(struct net_device *netdev,
+			      int tx_queues, int rx_queues)
+{
+	int err = 0;
+
+	err = netif_set_real_num_tx_queues(netdev, tx_queues);
+	if (err) {
+		netdev_err(netdev,
+			   "Failed to set no of Tx queues: %d\n", tx_queues);
+		return err;
+	}
+
+	err = netif_set_real_num_rx_queues(netdev, rx_queues);
+	if (err)
+		netdev_err(netdev,
+			   "Failed to set no of Rx queues: %d\n", rx_queues);
+	return err;
+}
+
+static int nicvf_init_resources(struct nicvf *nic)
+{
+	int err;
+	union nic_mbx mbx = {};
+
+	mbx.msg.msg = NIC_MBOX_MSG_CFG_DONE;
+
+	/* Enable Qset */
+	nicvf_qset_config(nic, true);
+
+	/* Initialize queues and HW for data transfer */
+	err = nicvf_config_data_transfer(nic, true);
+	if (err) {
+		netdev_err(nic->netdev,
+			   "Failed to alloc/config VF's QSet resources\n");
+		return err;
+	}
+
+	/* Send VF config done msg to PF */
+	nicvf_write_to_mbx(nic, &mbx);
+
+	return 0;
+}
+
+static void nicvf_snd_pkt_handler(struct net_device *netdev,
+				  struct cmp_queue *cq,
+				  struct cqe_send_t *cqe_tx, int cqe_type)
+{
+	struct sk_buff *skb = NULL;
+	struct nicvf *nic = netdev_priv(netdev);
+	struct snd_queue *sq;
+	struct sq_hdr_subdesc *hdr;
+
+	sq = &nic->qs->sq[cqe_tx->sq_idx];
+
+	hdr = (struct sq_hdr_subdesc *)GET_SQ_DESC(sq, cqe_tx->sqe_ptr);
+	if (hdr->subdesc_type != SQ_DESC_TYPE_HEADER)
+		return;
+
+	netdev_dbg(nic->netdev,
+		   "%s Qset #%d SQ #%d SQ ptr #%d subdesc count %d\n",
+		   __func__, cqe_tx->sq_qs, cqe_tx->sq_idx,
+		   cqe_tx->sqe_ptr, hdr->subdesc_cnt);
+
+	nicvf_put_sq_desc(sq, hdr->subdesc_cnt + 1);
+	nicvf_check_cqe_tx_errs(nic, cq, cqe_tx);
+	skb = (struct sk_buff *)sq->skbuff[cqe_tx->sqe_ptr];
+	/* For TSO offloaded packets only one head SKB needs to be freed */
+	if (skb) {
+		prefetch(skb);
+		dev_consume_skb_any(skb);
+	}
+}
+
+static void nicvf_rcv_pkt_handler(struct net_device *netdev,
+				  struct napi_struct *napi,
+				  struct cmp_queue *cq,
+				  struct cqe_rx_t *cqe_rx, int cqe_type)
+{
+	struct sk_buff *skb;
+	struct nicvf *nic = netdev_priv(netdev);
+	int err = 0;
+
+	/* Check for errors */
+	err = nicvf_check_cqe_rx_errs(nic, cq, cqe_rx);
+	if (err && !cqe_rx->rb_cnt)
+		return;
+
+	skb = nicvf_get_rcv_skb(nic, cqe_rx);
+	if (!skb) {
+		netdev_dbg(nic->netdev, "Packet not received\n");
+		return;
+	}
+
+	if (netif_msg_pktdata(nic)) {
+		netdev_info(nic->netdev, "%s: skb 0x%p, len=%d\n", netdev->name,
+			    skb, skb->len);
+		print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 1,
+			       skb->data, skb->len, true);
+	}
+
+	nicvf_set_rx_frame_cnt(nic, skb);
+
+	skb_record_rx_queue(skb, cqe_rx->rq_idx);
+	if (netdev->hw_features & NETIF_F_RXCSUM) {
+		/* HW by default verifies TCP/UDP/SCTP checksums */
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+	} else {
+		skb_checksum_none_assert(skb);
+	}
+
+	skb->protocol = eth_type_trans(skb, netdev);
+
+	if (napi && (netdev->features & NETIF_F_GRO))
+		napi_gro_receive(napi, skb);
+	else
+		netif_receive_skb(skb);
+}
+
+static int nicvf_cq_intr_handler(struct net_device *netdev, u8 cq_idx,
+				 struct napi_struct *napi, int budget)
+{
+	int processed_cqe, work_done = 0;
+	int cqe_count, cqe_head;
+	struct nicvf *nic = netdev_priv(netdev);
+	struct queue_set *qs = nic->qs;
+	struct cmp_queue *cq = &qs->cq[cq_idx];
+	struct cqe_rx_t *cq_desc;
+
+	spin_lock_bh(&cq->lock);
+loop:
+	processed_cqe = 0;
+	/* Get no of valid CQ entries to process */
+	cqe_count = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_STATUS, cq_idx);
+	cqe_count &= CQ_CQE_COUNT;
+	if (!cqe_count)
+		goto done;
+
+	/* Get head of the valid CQ entries */
+	cqe_head = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_HEAD, cq_idx) >> 9;
+	cqe_head &= 0xFFFF;
+
+	netdev_dbg(nic->netdev, "%s cqe_count %d cqe_head %d\n",
+		   __func__, cqe_count, cqe_head);
+	while (processed_cqe < cqe_count) {
+		/* Get the CQ descriptor */
+		cq_desc = (struct cqe_rx_t *)GET_CQ_DESC(cq, cqe_head);
+		cqe_head++;
+		cqe_head &= (cq->dmem.q_len - 1);
+		/* Initiate prefetch for next descriptor */
+		prefetch((struct cqe_rx_t *)GET_CQ_DESC(cq, cqe_head));
+
+		if ((work_done >= budget) && napi &&
+		    (cq_desc->cqe_type != CQE_TYPE_SEND)) {
+			break;
+		}
+
+		netdev_dbg(nic->netdev, "cq_desc->cqe_type %d\n",
+			   cq_desc->cqe_type);
+		switch (cq_desc->cqe_type) {
+		case CQE_TYPE_RX:
+			nicvf_rcv_pkt_handler(netdev, napi, cq,
+					      cq_desc, CQE_TYPE_RX);
+			work_done++;
+		break;
+		case CQE_TYPE_SEND:
+			nicvf_snd_pkt_handler(netdev, cq,
+					      (void *)cq_desc, CQE_TYPE_SEND);
+		break;
+		case CQE_TYPE_INVALID:
+		case CQE_TYPE_RX_SPLIT:
+		case CQE_TYPE_RX_TCP:
+		case CQE_TYPE_SEND_PTP:
+			/* Ignore for now */
+		break;
+		}
+		processed_cqe++;
+	}
+	netdev_dbg(nic->netdev, "%s processed_cqe %d work_done %d budget %d\n",
+		   __func__, processed_cqe, work_done, budget);
+
+	/* Ring doorbell to inform H/W to reuse processed CQEs */
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_DOOR,
+			      cq_idx, processed_cqe);
+
+	if ((work_done < budget) && napi)
+		goto loop;
+
+done:
+	spin_unlock_bh(&cq->lock);
+	return work_done;
+}
+
+static int nicvf_poll(struct napi_struct *napi, int budget)
+{
+	u64  cq_head;
+	int  work_done = 0;
+	struct net_device *netdev = napi->dev;
+	struct nicvf *nic = netdev_priv(netdev);
+	struct nicvf_cq_poll *cq;
+	struct netdev_queue *txq;
+
+	cq = container_of(napi, struct nicvf_cq_poll, napi);
+	work_done = nicvf_cq_intr_handler(netdev, cq->cq_idx, napi, budget);
+
+	txq = netdev_get_tx_queue(netdev, cq->cq_idx);
+	if (netif_tx_queue_stopped(txq))
+		netif_tx_wake_queue(txq);
+
+	if (work_done < budget) {
+		/* Slow packet rate, exit polling */
+		napi_complete(napi);
+		/* Re-enable interrupts */
+		cq_head = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_HEAD,
+					       cq->cq_idx);
+		nicvf_clear_intr(nic, NICVF_INTR_CQ, cq->cq_idx);
+		nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_HEAD,
+				      cq->cq_idx, cq_head);
+		nicvf_enable_intr(nic, NICVF_INTR_CQ, cq->cq_idx);
+	}
+	return work_done;
+}
+
+/* Qset error interrupt handler
+ *
+ * As of now only CQ errors are handled
+ */
+static void nicvf_handle_qs_err(unsigned long data)
+{
+	struct nicvf *nic = (struct nicvf *)data;
+	struct queue_set *qs = nic->qs;
+	int qidx;
+	u64 status;
+
+	netif_tx_disable(nic->netdev);
+
+	/* Check if it is CQ err */
+	for (qidx = 0; qidx < qs->cq_cnt; qidx++) {
+		status = nicvf_queue_reg_read(nic, NIC_QSET_CQ_0_7_STATUS,
+					      qidx);
+		if (!(status & CQ_ERR_MASK))
+			continue;
+		/* Process already queued CQEs and reconfig CQ */
+		nicvf_disable_intr(nic, NICVF_INTR_CQ, qidx);
+		nicvf_sq_disable(nic, qidx);
+		nicvf_cq_intr_handler(nic->netdev, qidx, NULL, 0);
+		nicvf_cmp_queue_config(nic, qs, qidx, true);
+		nicvf_sq_free_used_descs(nic->netdev, &qs->sq[qidx], qidx);
+		nicvf_sq_enable(nic, &qs->sq[qidx], qidx);
+
+		nicvf_enable_intr(nic, NICVF_INTR_CQ, qidx);
+	}
+
+	netif_tx_start_all_queues(nic->netdev);
+	/* Re-enable Qset error interrupt */
+	nicvf_enable_intr(nic, NICVF_INTR_QS_ERR, 0);
+}
+
+static irqreturn_t nicvf_misc_intr_handler(int irq, void *nicvf_irq)
+{
+	struct nicvf *nic = (struct nicvf *)nicvf_irq;
+	u64 intr;
+
+	intr = nicvf_reg_read(nic, NIC_VF_INT);
+	/* Check for spurious interrupt */
+	if (!(intr & NICVF_INTR_MBOX_MASK))
+		return IRQ_HANDLED;
+
+	nicvf_handle_mbx_intr(nic);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t nicvf_intr_handler(int irq, void *nicvf_irq)
+{
+	u64 qidx, intr, clear_intr = 0;
+	u64 cq_intr, rbdr_intr, qs_err_intr;
+	struct nicvf *nic = (struct nicvf *)nicvf_irq;
+	struct queue_set *qs = nic->qs;
+	struct nicvf_cq_poll *cq_poll = NULL;
+
+	intr = nicvf_reg_read(nic, NIC_VF_INT);
+	if (netif_msg_intr(nic))
+		netdev_info(nic->netdev, "%s: interrupt status 0x%llx\n",
+			    nic->netdev->name, intr);
+
+	qs_err_intr = intr & NICVF_INTR_QS_ERR_MASK;
+	if (qs_err_intr) {
+		/* Disable Qset err interrupt and schedule softirq */
+		nicvf_disable_intr(nic, NICVF_INTR_QS_ERR, 0);
+		tasklet_hi_schedule(&nic->qs_err_task);
+		clear_intr |= qs_err_intr;
+	}
+
+	/* Disable interrupts and start polling */
+	cq_intr = (intr & NICVF_INTR_CQ_MASK) >> NICVF_INTR_CQ_SHIFT;
+	for (qidx = 0; qidx < qs->cq_cnt; qidx++) {
+		if (!(cq_intr & (1 << qidx)))
+			continue;
+		if (!nicvf_is_intr_enabled(nic, NICVF_INTR_CQ, qidx))
+			continue;
+
+		nicvf_disable_intr(nic, NICVF_INTR_CQ, qidx);
+		clear_intr |= ((1 << qidx) << NICVF_INTR_CQ_SHIFT);
+
+		cq_poll = nic->napi[qidx];
+		/* Schedule NAPI */
+		if (cq_poll)
+			napi_schedule(&cq_poll->napi);
+	}
+
+	/* Handle RBDR interrupts */
+	rbdr_intr = (intr & NICVF_INTR_RBDR_MASK) >> NICVF_INTR_RBDR_SHIFT;
+	if (rbdr_intr) {
+		/* Disable RBDR interrupt and schedule softirq */
+		for (qidx = 0; qidx < qs->rbdr_cnt; qidx++) {
+			if (!nicvf_is_intr_enabled(nic, NICVF_INTR_RBDR, qidx))
+				continue;
+			nicvf_disable_intr(nic, NICVF_INTR_RBDR, qidx);
+			tasklet_hi_schedule(&nic->rbdr_task);
+			clear_intr |= ((1 << qidx) << NICVF_INTR_RBDR_SHIFT);
+		}
+	}
+
+	/* Clear interrupts */
+	nicvf_reg_write(nic, NIC_VF_INT, clear_intr);
+	return IRQ_HANDLED;
+}
+
+static int nicvf_enable_msix(struct nicvf *nic)
+{
+	int ret, vec;
+
+	nic->num_vec = NIC_VF_MSIX_VECTORS;
+
+	for (vec = 0; vec < nic->num_vec; vec++)
+		nic->msix_entries[vec].entry = vec;
+
+	ret = pci_enable_msix(nic->pdev, nic->msix_entries, nic->num_vec);
+	if (ret) {
+		netdev_err(nic->netdev,
+			   "Req for #%d msix vectors failed\n", nic->num_vec);
+		return 0;
+	}
+	nic->msix_enabled = 1;
+	return 1;
+}
+
+static void nicvf_disable_msix(struct nicvf *nic)
+{
+	if (nic->msix_enabled) {
+		pci_disable_msix(nic->pdev);
+		nic->msix_enabled = 0;
+		nic->num_vec = 0;
+	}
+}
+
+static int nicvf_register_interrupts(struct nicvf *nic)
+{
+	int irq, free, ret = 0;
+	int vector;
+
+	for_each_cq_irq(irq)
+		sprintf(nic->irq_name[irq], "NICVF%d CQ%d",
+			nic->vf_id, irq);
+
+	for_each_sq_irq(irq)
+		sprintf(nic->irq_name[irq], "NICVF%d SQ%d",
+			nic->vf_id, irq - NICVF_INTR_ID_SQ);
+
+	for_each_rbdr_irq(irq)
+		sprintf(nic->irq_name[irq], "NICVF%d RBDR%d",
+			nic->vf_id, irq - NICVF_INTR_ID_RBDR);
+
+	/* Register all interrupts except mailbox */
+	for (irq = 0; irq < NICVF_INTR_ID_SQ; irq++) {
+		vector = nic->msix_entries[irq].vector;
+		ret = request_irq(vector, nicvf_intr_handler,
+				  0, nic->irq_name[irq], nic);
+		if (ret)
+			break;
+		nic->irq_allocated[irq] = true;
+	}
+
+	for (irq = NICVF_INTR_ID_SQ; irq < NICVF_INTR_ID_MISC; irq++) {
+		vector = nic->msix_entries[irq].vector;
+		ret = request_irq(vector, nicvf_intr_handler,
+				  0, nic->irq_name[irq], nic);
+		if (ret)
+			break;
+		nic->irq_allocated[irq] = true;
+	}
+
+	sprintf(nic->irq_name[NICVF_INTR_ID_QS_ERR],
+		"NICVF%d Qset error", nic->vf_id);
+	if (!ret) {
+		vector = nic->msix_entries[NICVF_INTR_ID_QS_ERR].vector;
+		irq = NICVF_INTR_ID_QS_ERR;
+		ret = request_irq(vector, nicvf_intr_handler,
+				  0, nic->irq_name[irq], nic);
+		if (!ret)
+			nic->irq_allocated[irq] = true;
+	}
+
+	if (ret) {
+		netdev_err(nic->netdev, "Request irq failed\n");
+		for (free = 0; free < irq; free++)
+			free_irq(nic->msix_entries[free].vector, nic);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void nicvf_unregister_interrupts(struct nicvf *nic)
+{
+	int irq;
+
+	/* Free registered interrupts */
+	for (irq = 0; irq < nic->num_vec; irq++) {
+		if (nic->irq_allocated[irq])
+			free_irq(nic->msix_entries[irq].vector, nic);
+		nic->irq_allocated[irq] = false;
+	}
+
+	/* Disable MSI-X */
+	nicvf_disable_msix(nic);
+}
+
+/* Initialize MSIX vectors and register MISC interrupt.
+ * Send READY message to PF to check if its alive
+ */
+static int nicvf_register_misc_interrupt(struct nicvf *nic)
+{
+	int ret = 0;
+	int irq = NICVF_INTR_ID_MISC;
+
+	/* Return if mailbox interrupt is already registered */
+	if (nic->msix_enabled)
+		return 0;
+
+	/* Enable MSI-X */
+	if (!nicvf_enable_msix(nic))
+		return 1;
+
+	sprintf(nic->irq_name[irq], "%s Mbox", "NICVF");
+	/* Register Misc interrupt */
+	ret = request_irq(nic->msix_entries[irq].vector,
+			  nicvf_misc_intr_handler, 0, nic->irq_name[irq], nic);
+
+	if (ret)
+		return ret;
+	nic->irq_allocated[irq] = true;
+
+	/* Enable mailbox interrupt */
+	nicvf_enable_intr(nic, NICVF_INTR_MBOX, 0);
+
+	/* Check if VF is able to communicate with PF */
+	if (!nicvf_check_pf_ready(nic)) {
+		nicvf_disable_intr(nic, NICVF_INTR_MBOX, 0);
+		nicvf_unregister_interrupts(nic);
+		return 1;
+	}
+
+	return 0;
+}
+
+static netdev_tx_t nicvf_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+	int qid = skb_get_queue_mapping(skb);
+	struct netdev_queue *txq = netdev_get_tx_queue(netdev, qid);
+
+	/* Check for minimum packet length */
+	if (skb->len <= ETH_HLEN) {
+		dev_kfree_skb(skb);
+		return NETDEV_TX_OK;
+	}
+
+	if (!nicvf_sq_append_skb(nic, skb) && !netif_tx_queue_stopped(txq)) {
+		netif_tx_stop_queue(txq);
+		nic->drv_stats.tx_busy++;
+		if (netif_msg_tx_err(nic))
+			netdev_warn(netdev,
+				    "%s: Transmit ring full, stopping SQ%d\n",
+				    netdev->name, qid);
+
+		return NETDEV_TX_BUSY;
+	}
+
+	return NETDEV_TX_OK;
+}
+
+int nicvf_stop(struct net_device *netdev)
+{
+	int irq, qidx;
+	struct nicvf *nic = netdev_priv(netdev);
+	struct queue_set *qs = nic->qs;
+	struct nicvf_cq_poll *cq_poll = NULL;
+	union nic_mbx mbx = {};
+
+	mbx.msg.msg = NIC_MBOX_MSG_SHUTDOWN;
+	nicvf_send_msg_to_pf(nic, &mbx);
+
+	netif_carrier_off(netdev);
+	netif_tx_disable(netdev);
+
+	/* Disable RBDR & QS error interrupts */
+	for (qidx = 0; qidx < qs->rbdr_cnt; qidx++) {
+		nicvf_disable_intr(nic, NICVF_INTR_RBDR, qidx);
+		nicvf_clear_intr(nic, NICVF_INTR_RBDR, qidx);
+	}
+	nicvf_disable_intr(nic, NICVF_INTR_QS_ERR, 0);
+	nicvf_clear_intr(nic, NICVF_INTR_QS_ERR, 0);
+
+	/* Wait for pending IRQ handlers to finish */
+	for (irq = 0; irq < nic->num_vec; irq++)
+		synchronize_irq(nic->msix_entries[irq].vector);
+
+	tasklet_kill(&nic->rbdr_task);
+	tasklet_kill(&nic->qs_err_task);
+	if (nic->rb_work_scheduled)
+		cancel_delayed_work_sync(&nic->rbdr_work);
+
+	for (qidx = 0; qidx < nic->qs->cq_cnt; qidx++) {
+		cq_poll = nic->napi[qidx];
+		if (!cq_poll)
+			continue;
+		nic->napi[qidx] = NULL;
+		napi_synchronize(&cq_poll->napi);
+		/* CQ intr is enabled while napi_complete,
+		 * so disable it now
+		 */
+		nicvf_disable_intr(nic, NICVF_INTR_CQ, qidx);
+		nicvf_clear_intr(nic, NICVF_INTR_CQ, qidx);
+		napi_disable(&cq_poll->napi);
+		netif_napi_del(&cq_poll->napi);
+		kfree(cq_poll);
+	}
+
+	/* Free resources */
+	nicvf_config_data_transfer(nic, false);
+
+	/* Disable HW Qset */
+	nicvf_qset_config(nic, false);
+
+	/* disable mailbox interrupt */
+	nicvf_disable_intr(nic, NICVF_INTR_MBOX, 0);
+
+	nicvf_unregister_interrupts(nic);
+
+	return 0;
+}
+
+int nicvf_open(struct net_device *netdev)
+{
+	int err, qidx;
+	struct nicvf *nic = netdev_priv(netdev);
+	struct queue_set *qs = nic->qs;
+	struct nicvf_cq_poll *cq_poll = NULL;
+
+	nic->mtu = netdev->mtu;
+
+	netif_carrier_off(netdev);
+
+	err = nicvf_register_misc_interrupt(nic);
+	if (err)
+		return err;
+
+	/* Register NAPI handler for processing CQEs */
+	for (qidx = 0; qidx < qs->cq_cnt; qidx++) {
+		cq_poll = kzalloc(sizeof(*cq_poll), GFP_KERNEL);
+		if (!cq_poll) {
+			err = -ENOMEM;
+			goto napi_del;
+		}
+		cq_poll->cq_idx = qidx;
+		netif_napi_add(netdev, &cq_poll->napi, nicvf_poll,
+			       NAPI_POLL_WEIGHT);
+		napi_enable(&cq_poll->napi);
+		nic->napi[qidx] = cq_poll;
+	}
+
+	/* Check if we got MAC address from PF or else generate a radom MAC */
+	if (is_zero_ether_addr(netdev->dev_addr)) {
+		eth_hw_addr_random(netdev);
+		nicvf_hw_set_mac_addr(nic, netdev);
+	}
+
+	if (nic->set_mac_pending) {
+		nic->set_mac_pending = false;
+		nicvf_hw_set_mac_addr(nic, netdev);
+	}
+
+	/* Init tasklet for handling Qset err interrupt */
+	tasklet_init(&nic->qs_err_task, nicvf_handle_qs_err,
+		     (unsigned long)nic);
+
+	/* Init RBDR tasklet which will refill RBDR */
+	tasklet_init(&nic->rbdr_task, nicvf_rbdr_task,
+		     (unsigned long)nic);
+	INIT_DELAYED_WORK(&nic->rbdr_work, nicvf_rbdr_work);
+
+	/* Configure CPI alorithm */
+	nic->cpi_alg = cpi_alg;
+	nicvf_config_cpi(nic);
+
+	/* Configure receive side scaling */
+	nicvf_rss_init(nic);
+
+	err = nicvf_register_interrupts(nic);
+	if (err)
+		goto cleanup;
+
+	/* Initialize the queues */
+	err = nicvf_init_resources(nic);
+	if (err)
+		goto cleanup;
+
+	/* Make sure queue initialization is written */
+	wmb();
+
+	nicvf_reg_write(nic, NIC_VF_INT, -1);
+	/* Enable Qset err interrupt */
+	nicvf_enable_intr(nic, NICVF_INTR_QS_ERR, 0);
+
+	/* Enable completion queue interrupt */
+	for (qidx = 0; qidx < qs->cq_cnt; qidx++)
+		nicvf_enable_intr(nic, NICVF_INTR_CQ, qidx);
+
+	/* Enable RBDR threshold interrupt */
+	for (qidx = 0; qidx < qs->rbdr_cnt; qidx++)
+		nicvf_enable_intr(nic, NICVF_INTR_RBDR, qidx);
+
+	netif_carrier_on(netdev);
+	netif_tx_start_all_queues(netdev);
+
+	return 0;
+cleanup:
+	nicvf_disable_intr(nic, NICVF_INTR_MBOX, 0);
+	nicvf_unregister_interrupts(nic);
+napi_del:
+	for (qidx = 0; qidx < qs->cq_cnt; qidx++) {
+		cq_poll = nic->napi[qidx];
+		if (!cq_poll)
+			continue;
+		napi_disable(&cq_poll->napi);
+		netif_napi_del(&cq_poll->napi);
+		kfree(cq_poll);
+		nic->napi[qidx] = NULL;
+	}
+	return err;
+}
+
+static int nicvf_update_hw_max_frs(struct nicvf *nic, int mtu)
+{
+	union nic_mbx mbx = {};
+
+	mbx.frs.msg = NIC_MBOX_MSG_SET_MAX_FRS;
+	mbx.frs.max_frs = mtu;
+	mbx.frs.vf_id = nic->vf_id;
+
+	return nicvf_send_msg_to_pf(nic, &mbx);
+}
+
+static int nicvf_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+
+	if (new_mtu > NIC_HW_MAX_FRS)
+		return -EINVAL;
+
+	if (new_mtu < NIC_HW_MIN_FRS)
+		return -EINVAL;
+
+	if (nicvf_update_hw_max_frs(nic, new_mtu))
+		return -EINVAL;
+	netdev->mtu = new_mtu;
+	nic->mtu = new_mtu;
+
+	return 0;
+}
+
+static int nicvf_set_mac_address(struct net_device *netdev, void *p)
+{
+	struct sockaddr *addr = p;
+	struct nicvf *nic = netdev_priv(netdev);
+
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+
+	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
+
+	if (nic->msix_enabled) {
+		if (nicvf_hw_set_mac_addr(nic, netdev))
+			return -EBUSY;
+	} else {
+		nic->set_mac_pending = true;
+	}
+
+	return 0;
+}
+
+void nicvf_update_lmac_stats(struct nicvf *nic)
+{
+	int stat = 0;
+	union nic_mbx mbx = {};
+	int timeout;
+
+	if (!netif_running(nic->netdev))
+		return;
+
+	mbx.bgx_stats.msg = NIC_MBOX_MSG_BGX_STATS;
+	mbx.bgx_stats.vf_id = nic->vf_id;
+	/* Rx stats */
+	mbx.bgx_stats.rx = 1;
+	while (stat < BGX_RX_STATS_COUNT) {
+		nic->bgx_stats_acked = 0;
+		mbx.bgx_stats.idx = stat;
+		nicvf_send_msg_to_pf(nic, &mbx);
+		timeout = 0;
+		while ((!nic->bgx_stats_acked) && (timeout < 10)) {
+			msleep(2);
+			timeout++;
+		}
+		stat++;
+	}
+
+	stat = 0;
+
+	/* Tx stats */
+	mbx.bgx_stats.rx = 0;
+	while (stat < BGX_TX_STATS_COUNT) {
+		nic->bgx_stats_acked = 0;
+		mbx.bgx_stats.idx = stat;
+		nicvf_send_msg_to_pf(nic, &mbx);
+		timeout = 0;
+		while ((!nic->bgx_stats_acked) && (timeout < 10)) {
+			msleep(2);
+			timeout++;
+		}
+		stat++;
+	}
+}
+
+void nicvf_update_stats(struct nicvf *nic)
+{
+	int qidx;
+	struct nicvf_hw_stats *stats = &nic->stats;
+	struct nicvf_drv_stats *drv_stats = &nic->drv_stats;
+	struct queue_set *qs = nic->qs;
+
+#define GET_RX_STATS(reg) \
+	nicvf_reg_read(nic, NIC_VNIC_RX_STAT_0_13 | (reg << 3))
+#define GET_TX_STATS(reg) \
+	nicvf_reg_read(nic, NIC_VNIC_TX_STAT_0_4 | (reg << 3))
+
+	stats->rx_bytes_ok = GET_RX_STATS(RX_OCTS);
+	stats->rx_ucast_frames_ok = GET_RX_STATS(RX_UCAST);
+	stats->rx_bcast_frames_ok = GET_RX_STATS(RX_BCAST);
+	stats->rx_mcast_frames_ok = GET_RX_STATS(RX_MCAST);
+	stats->rx_fcs_errors = GET_RX_STATS(RX_FCS);
+	stats->rx_l2_errors = GET_RX_STATS(RX_L2ERR);
+	stats->rx_drop_red = GET_RX_STATS(RX_RED);
+	stats->rx_drop_overrun = GET_RX_STATS(RX_ORUN);
+	stats->rx_drop_bcast = GET_RX_STATS(RX_DRP_BCAST);
+	stats->rx_drop_mcast = GET_RX_STATS(RX_DRP_MCAST);
+	stats->rx_drop_l3_bcast = GET_RX_STATS(RX_DRP_L3BCAST);
+	stats->rx_drop_l3_mcast = GET_RX_STATS(RX_DRP_L3MCAST);
+
+	stats->tx_bytes_ok = GET_TX_STATS(TX_OCTS);
+	stats->tx_ucast_frames_ok = GET_TX_STATS(TX_UCAST);
+	stats->tx_bcast_frames_ok = GET_TX_STATS(TX_BCAST);
+	stats->tx_mcast_frames_ok = GET_TX_STATS(TX_MCAST);
+	stats->tx_drops = GET_TX_STATS(TX_DROP);
+
+	drv_stats->rx_frames_ok = stats->rx_ucast_frames_ok +
+				  stats->rx_bcast_frames_ok +
+				  stats->rx_mcast_frames_ok;
+	drv_stats->tx_frames_ok = stats->tx_ucast_frames_ok +
+				  stats->tx_bcast_frames_ok +
+				  stats->tx_mcast_frames_ok;
+	drv_stats->rx_drops = stats->rx_drop_red +
+			      stats->rx_drop_overrun;
+	drv_stats->tx_drops = stats->tx_drops;
+
+	/* Update RQ and SQ stats */
+	for (qidx = 0; qidx < qs->rq_cnt; qidx++)
+		nicvf_update_rq_stats(nic, qidx);
+	for (qidx = 0; qidx < qs->sq_cnt; qidx++)
+		nicvf_update_sq_stats(nic, qidx);
+}
+
+static struct rtnl_link_stats64 *nicvf_get_stats64(struct net_device *netdev,
+					    struct rtnl_link_stats64 *stats)
+{
+	struct nicvf *nic = netdev_priv(netdev);
+	struct nicvf_hw_stats *hw_stats = &nic->stats;
+	struct nicvf_drv_stats *drv_stats = &nic->drv_stats;
+
+	nicvf_update_stats(nic);
+
+	stats->rx_bytes = hw_stats->rx_bytes_ok;
+	stats->rx_packets = drv_stats->rx_frames_ok;
+	stats->rx_dropped = drv_stats->rx_drops;
+
+	stats->tx_bytes = hw_stats->tx_bytes_ok;
+	stats->tx_packets = drv_stats->tx_frames_ok;
+	stats->tx_dropped = drv_stats->tx_drops;
+
+	return stats;
+}
+
+static void nicvf_tx_timeout(struct net_device *dev)
+{
+	struct nicvf *nic = netdev_priv(dev);
+
+	if (netif_msg_tx_err(nic))
+		netdev_warn(dev, "%s: Transmit timed out, resetting\n",
+			    dev->name);
+
+	schedule_work(&nic->reset_task);
+}
+
+static void nicvf_reset_task(struct work_struct *work)
+{
+	struct nicvf *nic;
+
+	nic = container_of(work, struct nicvf, reset_task);
+
+	if (!netif_running(nic->netdev))
+		return;
+
+	nicvf_stop(nic->netdev);
+	nicvf_open(nic->netdev);
+	nic->netdev->trans_start = jiffies;
+}
+
+static const struct net_device_ops nicvf_netdev_ops = {
+	.ndo_open		= nicvf_open,
+	.ndo_stop		= nicvf_stop,
+	.ndo_start_xmit		= nicvf_xmit,
+	.ndo_change_mtu		= nicvf_change_mtu,
+	.ndo_set_mac_address	= nicvf_set_mac_address,
+	.ndo_get_stats64	= nicvf_get_stats64,
+	.ndo_tx_timeout         = nicvf_tx_timeout,
+};
+
+static int nicvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct device *dev = &pdev->dev;
+	struct net_device *netdev;
+	struct nicvf *nic;
+	struct queue_set *qs;
+	int    err;
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		dev_err(dev, "Failed to enable PCI device\n");
+		return err;
+	}
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err) {
+		dev_err(dev, "PCI request regions failed 0x%x\n", err);
+		goto err_disable_device;
+	}
+
+	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(48));
+	if (err) {
+		dev_err(dev, "Unable to get usable DMA configuration\n");
+		goto err_release_regions;
+	}
+
+	err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(48));
+	if (err) {
+		dev_err(dev, "unable to get 48-bit DMA for consistent allocations\n");
+		goto err_release_regions;
+	}
+
+	netdev = alloc_etherdev_mqs(sizeof(struct nicvf),
+				    MAX_RCV_QUEUES_PER_QS,
+				    MAX_SND_QUEUES_PER_QS);
+	if (!netdev) {
+		err = -ENOMEM;
+		goto err_release_regions;
+	}
+
+	pci_set_drvdata(pdev, netdev);
+
+	SET_NETDEV_DEV(netdev, &pdev->dev);
+
+	nic = netdev_priv(netdev);
+	nic->netdev = netdev;
+	nic->pdev = pdev;
+
+	/* MAP VF's configuration registers */
+	nic->reg_base = pcim_iomap(pdev, PCI_CFG_REG_BAR_NUM, 0);
+	if (!nic->reg_base) {
+		dev_err(dev, "Cannot map config register space, aborting\n");
+		err = -ENOMEM;
+		goto err_free_netdev;
+	}
+
+	err = nicvf_set_qset_resources(nic);
+	if (err)
+		goto err_free_netdev;
+
+	qs = nic->qs;
+
+	err = nicvf_set_real_num_queues(netdev, qs->sq_cnt, qs->rq_cnt);
+	if (err)
+		goto err_free_netdev;
+
+	/* Check if PF is alive and get MAC address for this VF */
+	err = nicvf_register_misc_interrupt(nic);
+	if (err)
+		goto err_free_netdev;
+
+	netdev->features |= (NETIF_F_RXCSUM | NETIF_F_IP_CSUM | NETIF_F_SG |
+			     NETIF_F_TSO | NETIF_F_GRO);
+	netdev->hw_features = netdev->features;
+
+	netdev->netdev_ops = &nicvf_netdev_ops;
+
+	INIT_WORK(&nic->reset_task, nicvf_reset_task);
+
+	err = register_netdev(netdev);
+	if (err) {
+		dev_err(dev, "Failed to register netdevice\n");
+		goto err_unregister_interrupts;
+	}
+
+	nic->msg_enable = debug;
+
+	nicvf_set_ethtool_ops(netdev);
+
+	return 0;
+
+err_unregister_interrupts:
+	nicvf_unregister_interrupts(nic);
+err_free_netdev:
+	pci_set_drvdata(pdev, NULL);
+	free_netdev(netdev);
+err_release_regions:
+	pci_release_regions(pdev);
+err_disable_device:
+	pci_disable_device(pdev);
+	return err;
+}
+
+static void nicvf_remove(struct pci_dev *pdev)
+{
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct nicvf *nic = netdev_priv(netdev);
+
+	unregister_netdev(netdev);
+	nicvf_unregister_interrupts(nic);
+	pci_set_drvdata(pdev, NULL);
+	free_netdev(netdev);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+static struct pci_driver nicvf_driver = {
+	.name = DRV_NAME,
+	.id_table = nicvf_id_table,
+	.probe = nicvf_probe,
+	.remove = nicvf_remove,
+};
+
+static int __init nicvf_init_module(void)
+{
+	pr_info("%s, ver %s\n", DRV_NAME, DRV_VERSION);
+
+	return pci_register_driver(&nicvf_driver);
+}
+
+static void __exit nicvf_cleanup_module(void)
+{
+	pci_unregister_driver(&nicvf_driver);
+}
+
+module_init(nicvf_init_module);
+module_exit(nicvf_cleanup_module);
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
new file mode 100644
index 000000000000..d69d228d11a0
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
@@ -0,0 +1,1545 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ */
+
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/ip.h>
+#include <linux/etherdevice.h>
+#include <net/ip.h>
+#include <net/tso.h>
+
+#include "nic_reg.h"
+#include "nic.h"
+#include "q_struct.h"
+#include "nicvf_queues.h"
+
+struct rbuf_info {
+	struct page *page;
+	void	*data;
+	u64	offset;
+};
+
+#define GET_RBUF_INFO(x) ((struct rbuf_info *)(x - NICVF_RCV_BUF_ALIGN_BYTES))
+
+/* Poll a register for a specific value */
+static int nicvf_poll_reg(struct nicvf *nic, int qidx,
+			  u64 reg, int bit_pos, int bits, int val)
+{
+	u64 bit_mask;
+	u64 reg_val;
+	int timeout = 10;
+
+	bit_mask = (1ULL << bits) - 1;
+	bit_mask = (bit_mask << bit_pos);
+
+	while (timeout) {
+		reg_val = nicvf_queue_reg_read(nic, reg, qidx);
+		if (((reg_val & bit_mask) >> bit_pos) == val)
+			return 0;
+		usleep_range(1000, 2000);
+		timeout--;
+	}
+	netdev_err(nic->netdev, "Poll on reg 0x%llx failed\n", reg);
+	return 1;
+}
+
+/* Allocate memory for a queue's descriptors */
+static int nicvf_alloc_q_desc_mem(struct nicvf *nic, struct q_desc_mem *dmem,
+				  int q_len, int desc_size, int align_bytes)
+{
+	dmem->q_len = q_len;
+	dmem->size = (desc_size * q_len) + align_bytes;
+	/* Save address, need it while freeing */
+	dmem->unalign_base = dma_zalloc_coherent(&nic->pdev->dev, dmem->size,
+						&dmem->dma, GFP_KERNEL);
+	if (!dmem->unalign_base)
+		return -ENOMEM;
+
+	/* Align memory address for 'align_bytes' */
+	dmem->phys_base = NICVF_ALIGNED_ADDR((u64)dmem->dma, align_bytes);
+	dmem->base = dmem->unalign_base + (dmem->phys_base - dmem->dma);
+	return 0;
+}
+
+/* Free queue's descriptor memory */
+static void nicvf_free_q_desc_mem(struct nicvf *nic, struct q_desc_mem *dmem)
+{
+	if (!dmem)
+		return;
+
+	dma_free_coherent(&nic->pdev->dev, dmem->size,
+			  dmem->unalign_base, dmem->dma);
+	dmem->unalign_base = NULL;
+	dmem->base = NULL;
+}
+
+/* Allocate buffer for packet reception
+ * HW returns memory address where packet is DMA'ed but not a pointer
+ * into RBDR ring, so save buffer address at the start of fragment and
+ * align the start address to a cache aligned address
+ */
+static inline int nicvf_alloc_rcv_buffer(struct nicvf *nic, gfp_t gfp,
+					 u32 buf_len, u64 **rbuf)
+{
+	u64 data;
+	struct rbuf_info *rinfo;
+	int order = get_order(buf_len);
+
+	/* Check if request can be accomodated in previous allocated page */
+	if (nic->rb_page) {
+		if ((nic->rb_page_offset + buf_len + buf_len) >
+		    (PAGE_SIZE << order)) {
+			nic->rb_page = NULL;
+		} else {
+			nic->rb_page_offset += buf_len;
+			get_page(nic->rb_page);
+		}
+	}
+
+	/* Allocate a new page */
+	if (!nic->rb_page) {
+		nic->rb_page = alloc_pages(gfp | __GFP_COMP, order);
+		if (!nic->rb_page) {
+			netdev_err(nic->netdev, "Failed to allocate new rcv buffer\n");
+			return -ENOMEM;
+		}
+		nic->rb_page_offset = 0;
+	}
+
+	data = (u64)page_address(nic->rb_page) + nic->rb_page_offset;
+
+	/* Align buffer addr to cache line i.e 128 bytes */
+	rinfo = (struct rbuf_info *)(data + NICVF_RCV_BUF_ALIGN_LEN(data));
+	/* Save page address for reference updation */
+	rinfo->page = nic->rb_page;
+	/* Store start address for later retrieval */
+	rinfo->data = (void *)data;
+	/* Store alignment offset */
+	rinfo->offset = NICVF_RCV_BUF_ALIGN_LEN(data);
+
+	data += rinfo->offset;
+
+	/* Give next aligned address to hw for DMA */
+	*rbuf = (u64 *)(data + NICVF_RCV_BUF_ALIGN_BYTES);
+	return 0;
+}
+
+/* Retrieve actual buffer start address and build skb for received packet */
+static struct sk_buff *nicvf_rb_ptr_to_skb(struct nicvf *nic,
+					   u64 rb_ptr, int len)
+{
+	struct sk_buff *skb;
+	struct rbuf_info *rinfo;
+
+	rb_ptr = (u64)phys_to_virt(rb_ptr);
+	/* Get buffer start address and alignment offset */
+	rinfo = GET_RBUF_INFO(rb_ptr);
+
+	/* Now build an skb to give to stack */
+	skb = build_skb(rinfo->data, RCV_FRAG_LEN);
+	if (!skb) {
+		put_page(rinfo->page);
+		return NULL;
+	}
+
+	/* Set correct skb->data */
+	skb_reserve(skb, rinfo->offset + NICVF_RCV_BUF_ALIGN_BYTES);
+
+	prefetch((void *)rb_ptr);
+	return skb;
+}
+
+/* Allocate RBDR ring and populate receive buffers */
+static int  nicvf_init_rbdr(struct nicvf *nic, struct rbdr *rbdr,
+			    int ring_len, int buf_size)
+{
+	int idx;
+	u64 *rbuf;
+	struct rbdr_entry_t *desc;
+	int err;
+
+	err = nicvf_alloc_q_desc_mem(nic, &rbdr->dmem, ring_len,
+				     sizeof(struct rbdr_entry_t),
+				     NICVF_RCV_BUF_ALIGN_BYTES);
+	if (err)
+		return err;
+
+	rbdr->desc = rbdr->dmem.base;
+	/* Buffer size has to be in multiples of 128 bytes */
+	rbdr->dma_size = buf_size;
+	rbdr->enable = true;
+	rbdr->thresh = RBDR_THRESH;
+
+	nic->rb_page = NULL;
+	for (idx = 0; idx < ring_len; idx++) {
+		err = nicvf_alloc_rcv_buffer(nic, GFP_KERNEL, RCV_FRAG_LEN,
+					     &rbuf);
+		if (err)
+			return err;
+
+		desc = GET_RBDR_DESC(rbdr, idx);
+		desc->buf_addr = virt_to_phys(rbuf) >> NICVF_RCV_BUF_ALIGN;
+	}
+	return 0;
+}
+
+/* Free RBDR ring and its receive buffers */
+static void nicvf_free_rbdr(struct nicvf *nic, struct rbdr *rbdr)
+{
+	int head, tail;
+	u64 buf_addr;
+	struct rbdr_entry_t *desc;
+	struct rbuf_info *rinfo;
+
+	if (!rbdr)
+		return;
+
+	rbdr->enable = false;
+	if (!rbdr->dmem.base)
+		return;
+
+	head = rbdr->head;
+	tail = rbdr->tail;
+
+	/* Free SKBs */
+	while (head != tail) {
+		desc = GET_RBDR_DESC(rbdr, head);
+		buf_addr = desc->buf_addr << NICVF_RCV_BUF_ALIGN;
+		rinfo = GET_RBUF_INFO((u64)phys_to_virt(buf_addr));
+		put_page(rinfo->page);
+		head++;
+		head &= (rbdr->dmem.q_len - 1);
+	}
+	/* Free SKB of tail desc */
+	desc = GET_RBDR_DESC(rbdr, tail);
+	buf_addr = desc->buf_addr << NICVF_RCV_BUF_ALIGN;
+	rinfo = GET_RBUF_INFO((u64)phys_to_virt(buf_addr));
+	put_page(rinfo->page);
+
+	/* Free RBDR ring */
+	nicvf_free_q_desc_mem(nic, &rbdr->dmem);
+}
+
+/* Refill receive buffer descriptors with new buffers.
+ */
+static void nicvf_refill_rbdr(struct nicvf *nic, gfp_t gfp)
+{
+	struct queue_set *qs = nic->qs;
+	int rbdr_idx = qs->rbdr_cnt;
+	int tail, qcount;
+	int refill_rb_cnt;
+	struct rbdr *rbdr;
+	struct rbdr_entry_t *desc;
+	u64 *rbuf;
+	int new_rb = 0;
+
+refill:
+	if (!rbdr_idx)
+		return;
+	rbdr_idx--;
+	rbdr = &qs->rbdr[rbdr_idx];
+	/* Check if it's enabled */
+	if (!rbdr->enable)
+		goto next_rbdr;
+
+	/* Get no of desc's to be refilled */
+	qcount = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_STATUS0, rbdr_idx);
+	qcount &= 0x7FFFF;
+	/* Doorbell can be ringed with a max of ring size minus 1 */
+	if (qcount >= (qs->rbdr_len - 1))
+		goto next_rbdr;
+	else
+		refill_rb_cnt = qs->rbdr_len - qcount - 1;
+
+	/* Start filling descs from tail */
+	tail = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_TAIL, rbdr_idx) >> 3;
+	while (refill_rb_cnt) {
+		tail++;
+		tail &= (rbdr->dmem.q_len - 1);
+
+		if (nicvf_alloc_rcv_buffer(nic, gfp, RCV_FRAG_LEN, &rbuf))
+			break;
+
+		desc = GET_RBDR_DESC(rbdr, tail);
+		desc->buf_addr = virt_to_phys(rbuf) >> NICVF_RCV_BUF_ALIGN;
+		refill_rb_cnt--;
+		new_rb++;
+	}
+
+	/* make sure all memory stores are done before ringing doorbell */
+	smp_wmb();
+
+	/* Check if buffer allocation failed */
+	if (refill_rb_cnt)
+		nic->rb_alloc_fail = true;
+	else
+		nic->rb_alloc_fail = false;
+
+	/* Notify HW */
+	nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_DOOR,
+			      rbdr_idx, new_rb);
+next_rbdr:
+	/* Re-enable RBDR interrupts only if buffer allocation is success */
+	if (!nic->rb_alloc_fail && rbdr->enable)
+		nicvf_enable_intr(nic, NICVF_INTR_RBDR, rbdr_idx);
+
+	if (rbdr_idx)
+		goto refill;
+}
+
+/* Alloc rcv buffers in non-atomic mode for better success */
+void nicvf_rbdr_work(struct work_struct *work)
+{
+	struct nicvf *nic = container_of(work, struct nicvf, rbdr_work.work);
+
+	nicvf_refill_rbdr(nic, GFP_KERNEL);
+	if (nic->rb_alloc_fail)
+		schedule_delayed_work(&nic->rbdr_work, msecs_to_jiffies(10));
+	else
+		nic->rb_work_scheduled = false;
+}
+
+/* In Softirq context, alloc rcv buffers in atomic mode */
+void nicvf_rbdr_task(unsigned long data)
+{
+	struct nicvf *nic = (struct nicvf *)data;
+
+	nicvf_refill_rbdr(nic, GFP_ATOMIC);
+	if (nic->rb_alloc_fail) {
+		nic->rb_work_scheduled = true;
+		schedule_delayed_work(&nic->rbdr_work, msecs_to_jiffies(10));
+	}
+}
+
+/* Initialize completion queue */
+static int nicvf_init_cmp_queue(struct nicvf *nic,
+				struct cmp_queue *cq, int q_len)
+{
+	int err;
+
+	err = nicvf_alloc_q_desc_mem(nic, &cq->dmem, q_len, CMP_QUEUE_DESC_SIZE,
+				     NICVF_CQ_BASE_ALIGN_BYTES);
+	if (err)
+		return err;
+
+	cq->desc = cq->dmem.base;
+	cq->thresh = CMP_QUEUE_CQE_THRESH;
+	nic->cq_coalesce_usecs = (CMP_QUEUE_TIMER_THRESH * 0.05) - 1;
+
+	return 0;
+}
+
+static void nicvf_free_cmp_queue(struct nicvf *nic, struct cmp_queue *cq)
+{
+	if (!cq)
+		return;
+	if (!cq->dmem.base)
+		return;
+
+	nicvf_free_q_desc_mem(nic, &cq->dmem);
+}
+
+/* Initialize transmit queue */
+static int nicvf_init_snd_queue(struct nicvf *nic,
+				struct snd_queue *sq, int q_len)
+{
+	int err;
+
+	err = nicvf_alloc_q_desc_mem(nic, &sq->dmem, q_len, SND_QUEUE_DESC_SIZE,
+				     NICVF_SQ_BASE_ALIGN_BYTES);
+	if (err)
+		return err;
+
+	sq->desc = sq->dmem.base;
+	sq->skbuff = kcalloc(q_len, sizeof(u64), GFP_KERNEL);
+	if (!sq->skbuff)
+		return -ENOMEM;
+	sq->head = 0;
+	sq->tail = 0;
+	atomic_set(&sq->free_cnt, q_len - 1);
+	sq->thresh = SND_QUEUE_THRESH;
+
+	/* Preallocate memory for TSO segment's header */
+	sq->tso_hdrs = dma_alloc_coherent(&nic->pdev->dev,
+					  q_len * TSO_HEADER_SIZE,
+					  &sq->tso_hdrs_phys, GFP_KERNEL);
+	if (!sq->tso_hdrs)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void nicvf_free_snd_queue(struct nicvf *nic, struct snd_queue *sq)
+{
+	if (!sq)
+		return;
+	if (!sq->dmem.base)
+		return;
+
+	if (sq->tso_hdrs)
+		dma_free_coherent(&nic->pdev->dev, sq->dmem.q_len,
+				  sq->tso_hdrs, sq->tso_hdrs_phys);
+
+	kfree(sq->skbuff);
+	nicvf_free_q_desc_mem(nic, &sq->dmem);
+}
+
+static void nicvf_reclaim_snd_queue(struct nicvf *nic,
+				    struct queue_set *qs, int qidx)
+{
+	/* Disable send queue */
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_CFG, qidx, 0);
+	/* Check if SQ is stopped */
+	if (nicvf_poll_reg(nic, qidx, NIC_QSET_SQ_0_7_STATUS, 21, 1, 0x01))
+		return;
+	/* Reset send queue */
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_CFG, qidx, NICVF_SQ_RESET);
+}
+
+static void nicvf_reclaim_rcv_queue(struct nicvf *nic,
+				    struct queue_set *qs, int qidx)
+{
+	union nic_mbx mbx = {};
+
+	/* Make sure all packets in the pipeline are written back into mem */
+	mbx.msg.msg = NIC_MBOX_MSG_RQ_SW_SYNC;
+	nicvf_send_msg_to_pf(nic, &mbx);
+}
+
+static void nicvf_reclaim_cmp_queue(struct nicvf *nic,
+				    struct queue_set *qs, int qidx)
+{
+	/* Disable timer threshold (doesn't get reset upon CQ reset */
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_CFG2, qidx, 0);
+	/* Disable completion queue */
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_CFG, qidx, 0);
+	/* Reset completion queue */
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_CFG, qidx, NICVF_CQ_RESET);
+}
+
+static void nicvf_reclaim_rbdr(struct nicvf *nic,
+			       struct rbdr *rbdr, int qidx)
+{
+	u64 tmp, fifo_state;
+	int timeout = 10;
+
+	/* Save head and tail pointers for feeing up buffers */
+	rbdr->head = nicvf_queue_reg_read(nic,
+					  NIC_QSET_RBDR_0_1_HEAD,
+					  qidx) >> 3;
+	rbdr->tail = nicvf_queue_reg_read(nic,
+					  NIC_QSET_RBDR_0_1_TAIL,
+					  qidx) >> 3;
+
+	/* If RBDR FIFO is in 'FAIL' state then do a reset first
+	 * before relaiming.
+	 */
+	fifo_state = nicvf_queue_reg_read(nic, NIC_QSET_RBDR_0_1_STATUS0, qidx);
+	if (((fifo_state >> 62) & 0x03) == 0x3)
+		nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_CFG,
+				      qidx, NICVF_RBDR_RESET);
+
+	/* Disable RBDR */
+	nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_CFG, qidx, 0);
+	if (nicvf_poll_reg(nic, qidx, NIC_QSET_RBDR_0_1_STATUS0, 62, 2, 0x00))
+		return;
+	while (1) {
+		tmp = nicvf_queue_reg_read(nic,
+					   NIC_QSET_RBDR_0_1_PREFETCH_STATUS,
+					   qidx);
+		if ((tmp & 0xFFFFFFFF) == ((tmp >> 32) & 0xFFFFFFFF))
+			break;
+		usleep_range(1000, 2000);
+		timeout--;
+		if (!timeout) {
+			netdev_err(nic->netdev,
+				   "Failed polling on prefetch status\n");
+			return;
+		}
+	}
+	nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_CFG,
+			      qidx, NICVF_RBDR_RESET);
+
+	if (nicvf_poll_reg(nic, qidx, NIC_QSET_RBDR_0_1_STATUS0, 62, 2, 0x02))
+		return;
+	nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_CFG, qidx, 0x00);
+	if (nicvf_poll_reg(nic, qidx, NIC_QSET_RBDR_0_1_STATUS0, 62, 2, 0x00))
+		return;
+}
+
+/* Configures receive queue */
+static void nicvf_rcv_queue_config(struct nicvf *nic, struct queue_set *qs,
+				   int qidx, bool enable)
+{
+	union nic_mbx mbx = {};
+	struct rcv_queue *rq;
+	struct rq_cfg rq_cfg;
+
+	rq = &qs->rq[qidx];
+	rq->enable = enable;
+
+	/* Disable receive queue */
+	nicvf_queue_reg_write(nic, NIC_QSET_RQ_0_7_CFG, qidx, 0);
+
+	if (!rq->enable) {
+		nicvf_reclaim_rcv_queue(nic, qs, qidx);
+		return;
+	}
+
+	rq->cq_qs = qs->vnic_id;
+	rq->cq_idx = qidx;
+	rq->start_rbdr_qs = qs->vnic_id;
+	rq->start_qs_rbdr_idx = qs->rbdr_cnt - 1;
+	rq->cont_rbdr_qs = qs->vnic_id;
+	rq->cont_qs_rbdr_idx = qs->rbdr_cnt - 1;
+	/* all writes of RBDR data to be loaded into L2 Cache as well*/
+	rq->caching = 1;
+
+	/* Send a mailbox msg to PF to config RQ */
+	mbx.rq.msg = NIC_MBOX_MSG_RQ_CFG;
+	mbx.rq.qs_num = qs->vnic_id;
+	mbx.rq.rq_num = qidx;
+	mbx.rq.cfg = (rq->caching << 26) | (rq->cq_qs << 19) |
+			  (rq->cq_idx << 16) | (rq->cont_rbdr_qs << 9) |
+			  (rq->cont_qs_rbdr_idx << 8) |
+			  (rq->start_rbdr_qs << 1) | (rq->start_qs_rbdr_idx);
+	nicvf_send_msg_to_pf(nic, &mbx);
+
+	mbx.rq.msg = NIC_MBOX_MSG_RQ_BP_CFG;
+	mbx.rq.cfg = (1ULL << 63) | (1ULL << 62) | (qs->vnic_id << 0);
+	nicvf_send_msg_to_pf(nic, &mbx);
+
+	/* RQ drop config
+	 * Enable CQ drop to reserve sufficient CQEs for all tx packets
+	 */
+	mbx.rq.msg = NIC_MBOX_MSG_RQ_DROP_CFG;
+	mbx.rq.cfg = (1ULL << 62) | (RQ_CQ_DROP << 8);
+	nicvf_send_msg_to_pf(nic, &mbx);
+
+	nicvf_queue_reg_write(nic, NIC_QSET_RQ_GEN_CFG, qidx, 0x00);
+
+	/* Enable Receive queue */
+	rq_cfg.ena = 1;
+	rq_cfg.tcp_ena = 0;
+	nicvf_queue_reg_write(nic, NIC_QSET_RQ_0_7_CFG, qidx, *(u64 *)&rq_cfg);
+}
+
+/* Configures completion queue */
+void nicvf_cmp_queue_config(struct nicvf *nic, struct queue_set *qs,
+			    int qidx, bool enable)
+{
+	struct cmp_queue *cq;
+	struct cq_cfg cq_cfg;
+
+	cq = &qs->cq[qidx];
+	cq->enable = enable;
+
+	if (!cq->enable) {
+		nicvf_reclaim_cmp_queue(nic, qs, qidx);
+		return;
+	}
+
+	/* Reset completion queue */
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_CFG, qidx, NICVF_CQ_RESET);
+
+	if (!cq->enable)
+		return;
+
+	spin_lock_init(&cq->lock);
+	/* Set completion queue base address */
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_BASE,
+			      qidx, (u64)(cq->dmem.phys_base));
+
+	/* Enable Completion queue */
+	cq_cfg.ena = 1;
+	cq_cfg.reset = 0;
+	cq_cfg.caching = 0;
+	cq_cfg.qsize = CMP_QSIZE;
+	cq_cfg.avg_con = 0;
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_CFG, qidx, *(u64 *)&cq_cfg);
+
+	/* Set threshold value for interrupt generation */
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_THRESH, qidx, cq->thresh);
+	nicvf_queue_reg_write(nic, NIC_QSET_CQ_0_7_CFG2,
+			      qidx, nic->cq_coalesce_usecs);
+}
+
+/* Configures transmit queue */
+static void nicvf_snd_queue_config(struct nicvf *nic, struct queue_set *qs,
+				   int qidx, bool enable)
+{
+	union nic_mbx mbx = {};
+	struct snd_queue *sq;
+	struct sq_cfg sq_cfg;
+
+	sq = &qs->sq[qidx];
+	sq->enable = enable;
+
+	if (!sq->enable) {
+		nicvf_reclaim_snd_queue(nic, qs, qidx);
+		return;
+	}
+
+	/* Reset send queue */
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_CFG, qidx, NICVF_SQ_RESET);
+
+	sq->cq_qs = qs->vnic_id;
+	sq->cq_idx = qidx;
+
+	/* Send a mailbox msg to PF to config SQ */
+	mbx.sq.msg = NIC_MBOX_MSG_SQ_CFG;
+	mbx.sq.qs_num = qs->vnic_id;
+	mbx.sq.sq_num = qidx;
+	mbx.sq.cfg = (sq->cq_qs << 3) | sq->cq_idx;
+	nicvf_send_msg_to_pf(nic, &mbx);
+
+	/* Set queue base address */
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_BASE,
+			      qidx, (u64)(sq->dmem.phys_base));
+
+	/* Enable send queue  & set queue size */
+	sq_cfg.ena = 1;
+	sq_cfg.reset = 0;
+	sq_cfg.ldwb = 0;
+	sq_cfg.qsize = SND_QSIZE;
+	sq_cfg.tstmp_bgx_intf = 0;
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_CFG, qidx, *(u64 *)&sq_cfg);
+
+	/* Set threshold value for interrupt generation */
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_THRESH, qidx, sq->thresh);
+
+	/* Set queue:cpu affinity for better load distribution */
+	if (cpu_online(qidx)) {
+		cpumask_set_cpu(qidx, &sq->affinity_mask);
+		netif_set_xps_queue(nic->netdev,
+				    &sq->affinity_mask, qidx);
+	}
+}
+
+/* Configures receive buffer descriptor ring */
+static void nicvf_rbdr_config(struct nicvf *nic, struct queue_set *qs,
+			      int qidx, bool enable)
+{
+	struct rbdr *rbdr;
+	struct rbdr_cfg rbdr_cfg;
+
+	rbdr = &qs->rbdr[qidx];
+	nicvf_reclaim_rbdr(nic, rbdr, qidx);
+	if (!enable)
+		return;
+
+	/* Set descriptor base address */
+	nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_BASE,
+			      qidx, (u64)(rbdr->dmem.phys_base));
+
+	/* Enable RBDR  & set queue size */
+	/* Buffer size should be in multiples of 128 bytes */
+	rbdr_cfg.ena = 1;
+	rbdr_cfg.reset = 0;
+	rbdr_cfg.ldwb = 0;
+	rbdr_cfg.qsize = RBDR_SIZE;
+	rbdr_cfg.avg_con = 0;
+	rbdr_cfg.lines = rbdr->dma_size / 128;
+	nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_CFG,
+			      qidx, *(u64 *)&rbdr_cfg);
+
+	/* Notify HW */
+	nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_DOOR,
+			      qidx, qs->rbdr_len - 1);
+
+	/* Set threshold value for interrupt generation */
+	nicvf_queue_reg_write(nic, NIC_QSET_RBDR_0_1_THRESH,
+			      qidx, rbdr->thresh - 1);
+}
+
+/* Requests PF to assign and enable Qset */
+void nicvf_qset_config(struct nicvf *nic, bool enable)
+{
+	union nic_mbx mbx = {};
+	struct queue_set *qs = nic->qs;
+	struct qs_cfg *qs_cfg;
+
+	if (!qs) {
+		netdev_warn(nic->netdev,
+			    "Qset is still not allocated, don't init queues\n");
+		return;
+	}
+
+	qs->enable = enable;
+	qs->vnic_id = nic->vf_id;
+
+	/* Send a mailbox msg to PF to config Qset */
+	mbx.qs.msg = NIC_MBOX_MSG_QS_CFG;
+	mbx.qs.num = qs->vnic_id;
+
+	mbx.qs.cfg = 0;
+	qs_cfg = (struct qs_cfg *)&mbx.qs.cfg;
+	if (qs->enable) {
+		qs_cfg->ena = 1;
+#ifdef __BIG_ENDIAN
+		qs_cfg->be = 1;
+#endif
+		qs_cfg->vnic = qs->vnic_id;
+	}
+	nicvf_send_msg_to_pf(nic, &mbx);
+}
+
+static void nicvf_free_resources(struct nicvf *nic)
+{
+	int qidx;
+	struct queue_set *qs = nic->qs;
+
+	/* Free receive buffer descriptor ring */
+	for (qidx = 0; qidx < qs->rbdr_cnt; qidx++)
+		nicvf_free_rbdr(nic, &qs->rbdr[qidx]);
+
+	/* Free completion queue */
+	for (qidx = 0; qidx < qs->cq_cnt; qidx++)
+		nicvf_free_cmp_queue(nic, &qs->cq[qidx]);
+
+	/* Free send queue */
+	for (qidx = 0; qidx < qs->sq_cnt; qidx++)
+		nicvf_free_snd_queue(nic, &qs->sq[qidx]);
+}
+
+static int nicvf_alloc_resources(struct nicvf *nic)
+{
+	int qidx;
+	struct queue_set *qs = nic->qs;
+
+	/* Alloc receive buffer descriptor ring */
+	for (qidx = 0; qidx < qs->rbdr_cnt; qidx++) {
+		if (nicvf_init_rbdr(nic, &qs->rbdr[qidx], qs->rbdr_len,
+				    DMA_BUFFER_LEN))
+			goto alloc_fail;
+	}
+
+	/* Alloc send queue */
+	for (qidx = 0; qidx < qs->sq_cnt; qidx++) {
+		if (nicvf_init_snd_queue(nic, &qs->sq[qidx], qs->sq_len))
+			goto alloc_fail;
+	}
+
+	/* Alloc completion queue */
+	for (qidx = 0; qidx < qs->cq_cnt; qidx++) {
+		if (nicvf_init_cmp_queue(nic, &qs->cq[qidx], qs->cq_len))
+			goto alloc_fail;
+	}
+
+	return 0;
+alloc_fail:
+	nicvf_free_resources(nic);
+	return -ENOMEM;
+}
+
+int nicvf_set_qset_resources(struct nicvf *nic)
+{
+	struct queue_set *qs;
+
+	qs = devm_kzalloc(&nic->pdev->dev, sizeof(*qs), GFP_KERNEL);
+	if (!qs)
+		return -ENOMEM;
+	nic->qs = qs;
+
+	/* Set count of each queue */
+	qs->rbdr_cnt = RBDR_CNT;
+	qs->rq_cnt = RCV_QUEUE_CNT;
+	qs->sq_cnt = SND_QUEUE_CNT;
+	qs->cq_cnt = CMP_QUEUE_CNT;
+
+	/* Set queue lengths */
+	qs->rbdr_len = RCV_BUF_COUNT;
+	qs->sq_len = SND_QUEUE_LEN;
+	qs->cq_len = CMP_QUEUE_LEN;
+	return 0;
+}
+
+int nicvf_config_data_transfer(struct nicvf *nic, bool enable)
+{
+	bool disable = false;
+	struct queue_set *qs = nic->qs;
+	int qidx;
+
+	if (!qs)
+		return 0;
+
+	if (enable) {
+		if (nicvf_alloc_resources(nic))
+			return -ENOMEM;
+
+		for (qidx = 0; qidx < qs->sq_cnt; qidx++)
+			nicvf_snd_queue_config(nic, qs, qidx, enable);
+		for (qidx = 0; qidx < qs->cq_cnt; qidx++)
+			nicvf_cmp_queue_config(nic, qs, qidx, enable);
+		for (qidx = 0; qidx < qs->rbdr_cnt; qidx++)
+			nicvf_rbdr_config(nic, qs, qidx, enable);
+		for (qidx = 0; qidx < qs->rq_cnt; qidx++)
+			nicvf_rcv_queue_config(nic, qs, qidx, enable);
+	} else {
+		for (qidx = 0; qidx < qs->rq_cnt; qidx++)
+			nicvf_rcv_queue_config(nic, qs, qidx, disable);
+		for (qidx = 0; qidx < qs->rbdr_cnt; qidx++)
+			nicvf_rbdr_config(nic, qs, qidx, disable);
+		for (qidx = 0; qidx < qs->sq_cnt; qidx++)
+			nicvf_snd_queue_config(nic, qs, qidx, disable);
+		for (qidx = 0; qidx < qs->cq_cnt; qidx++)
+			nicvf_cmp_queue_config(nic, qs, qidx, disable);
+
+		nicvf_free_resources(nic);
+	}
+
+	return 0;
+}
+
+/* Get a free desc from SQ
+ * returns descriptor ponter & descriptor number
+ */
+static inline int nicvf_get_sq_desc(struct snd_queue *sq, int desc_cnt)
+{
+	int qentry;
+
+	qentry = sq->tail;
+	atomic_sub(desc_cnt, &sq->free_cnt);
+	sq->tail += desc_cnt;
+	sq->tail &= (sq->dmem.q_len - 1);
+
+	return qentry;
+}
+
+/* Free descriptor back to SQ for future use */
+void nicvf_put_sq_desc(struct snd_queue *sq, int desc_cnt)
+{
+	atomic_add(desc_cnt, &sq->free_cnt);
+	sq->head += desc_cnt;
+	sq->head &= (sq->dmem.q_len - 1);
+}
+
+static inline int nicvf_get_nxt_sqentry(struct snd_queue *sq, int qentry)
+{
+	qentry++;
+	qentry &= (sq->dmem.q_len - 1);
+	return qentry;
+}
+
+void nicvf_sq_enable(struct nicvf *nic, struct snd_queue *sq, int qidx)
+{
+	u64 sq_cfg;
+
+	sq_cfg = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_CFG, qidx);
+	sq_cfg |= NICVF_SQ_EN;
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_CFG, qidx, sq_cfg);
+	/* Ring doorbell so that H/W restarts processing SQEs */
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_DOOR, qidx, 0);
+}
+
+void nicvf_sq_disable(struct nicvf *nic, int qidx)
+{
+	u64 sq_cfg;
+
+	sq_cfg = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_CFG, qidx);
+	sq_cfg &= ~NICVF_SQ_EN;
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_CFG, qidx, sq_cfg);
+}
+
+void nicvf_sq_free_used_descs(struct net_device *netdev, struct snd_queue *sq,
+			      int qidx)
+{
+	u64 head, tail;
+	struct sk_buff *skb;
+	struct nicvf *nic = netdev_priv(netdev);
+	struct sq_hdr_subdesc *hdr;
+
+	head = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_HEAD, qidx) >> 4;
+	tail = nicvf_queue_reg_read(nic, NIC_QSET_SQ_0_7_TAIL, qidx) >> 4;
+	while (sq->head != head) {
+		hdr = (struct sq_hdr_subdesc *)GET_SQ_DESC(sq, sq->head);
+		if (hdr->subdesc_type != SQ_DESC_TYPE_HEADER) {
+			nicvf_put_sq_desc(sq, 1);
+			continue;
+		}
+		skb = (struct sk_buff *)sq->skbuff[sq->head];
+		atomic64_add(1, (atomic64_t *)&netdev->stats.tx_packets);
+		atomic64_add(hdr->tot_len,
+			     (atomic64_t *)&netdev->stats.tx_bytes);
+		dev_kfree_skb_any(skb);
+		nicvf_put_sq_desc(sq, hdr->subdesc_cnt + 1);
+	}
+}
+
+/* Calculate no of SQ subdescriptors needed to transmit all
+ * segments of this TSO packet.
+ * Taken from 'Tilera network driver' with a minor modification.
+ */
+static int nicvf_tso_count_subdescs(struct sk_buff *skb)
+{
+	struct skb_shared_info *sh = skb_shinfo(skb);
+	unsigned int sh_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
+	unsigned int data_len = skb->len - sh_len;
+	unsigned int p_len = sh->gso_size;
+	long f_id = -1;    /* id of the current fragment */
+	long f_size = skb_headlen(skb) - sh_len;  /* current fragment size */
+	long f_used = 0;  /* bytes used from the current fragment */
+	long n;            /* size of the current piece of payload */
+	int num_edescs = 0;
+	int segment;
+
+	for (segment = 0; segment < sh->gso_segs; segment++) {
+		unsigned int p_used = 0;
+
+		/* One edesc for header and for each piece of the payload. */
+		for (num_edescs++; p_used < p_len; num_edescs++) {
+			/* Advance as needed. */
+			while (f_used >= f_size) {
+				f_id++;
+				f_size = skb_frag_size(&sh->frags[f_id]);
+				f_used = 0;
+			}
+
+			/* Use bytes from the current fragment. */
+			n = p_len - p_used;
+			if (n > f_size - f_used)
+				n = f_size - f_used;
+			f_used += n;
+			p_used += n;
+		}
+
+		/* The last segment may be less than gso_size. */
+		data_len -= p_len;
+		if (data_len < p_len)
+			p_len = data_len;
+	}
+
+	/* '+ gso_segs' for SQ_HDR_SUDESCs for each segment */
+	return num_edescs + sh->gso_segs;
+}
+
+/* Get the number of SQ descriptors needed to xmit this skb */
+static int nicvf_sq_subdesc_required(struct nicvf *nic, struct sk_buff *skb)
+{
+	int subdesc_cnt = MIN_SQ_DESC_PER_PKT_XMIT;
+
+	if (skb_shinfo(skb)->gso_size) {
+		subdesc_cnt = nicvf_tso_count_subdescs(skb);
+		return subdesc_cnt;
+	}
+
+	if (skb_shinfo(skb)->nr_frags)
+		subdesc_cnt += skb_shinfo(skb)->nr_frags;
+
+	return subdesc_cnt;
+}
+
+/* Add SQ HEADER subdescriptor.
+ * First subdescriptor for every send descriptor.
+ */
+static inline void
+nicvf_sq_add_hdr_subdesc(struct snd_queue *sq, int qentry,
+			 int subdesc_cnt, struct sk_buff *skb, int len)
+{
+	int proto;
+	struct sq_hdr_subdesc *hdr;
+
+	hdr = (struct sq_hdr_subdesc *)GET_SQ_DESC(sq, qentry);
+	sq->skbuff[qentry] = (u64)skb;
+
+	memset(hdr, 0, SND_QUEUE_DESC_SIZE);
+	hdr->subdesc_type = SQ_DESC_TYPE_HEADER;
+	/* Enable notification via CQE after processing SQE */
+	hdr->post_cqe = 1;
+	/* No of subdescriptors following this */
+	hdr->subdesc_cnt = subdesc_cnt;
+	hdr->tot_len = len;
+
+	/* Offload checksum calculation to HW */
+	if (skb->ip_summed == CHECKSUM_PARTIAL) {
+		if (skb->protocol != htons(ETH_P_IP))
+			return;
+
+		hdr->csum_l3 = 1; /* Enable IP csum calculation */
+		hdr->l3_offset = skb_network_offset(skb);
+		hdr->l4_offset = skb_transport_offset(skb);
+
+		proto = ip_hdr(skb)->protocol;
+		switch (proto) {
+		case IPPROTO_TCP:
+			hdr->csum_l4 = SEND_L4_CSUM_TCP;
+			break;
+		case IPPROTO_UDP:
+			hdr->csum_l4 = SEND_L4_CSUM_UDP;
+			break;
+		case IPPROTO_SCTP:
+			hdr->csum_l4 = SEND_L4_CSUM_SCTP;
+			break;
+		}
+	}
+}
+
+/* SQ GATHER subdescriptor
+ * Must follow HDR descriptor
+ */
+static inline void nicvf_sq_add_gather_subdesc(struct snd_queue *sq, int qentry,
+					       int size, u64 data)
+{
+	struct sq_gather_subdesc *gather;
+
+	qentry &= (sq->dmem.q_len - 1);
+	gather = (struct sq_gather_subdesc *)GET_SQ_DESC(sq, qentry);
+
+	memset(gather, 0, SND_QUEUE_DESC_SIZE);
+	gather->subdesc_type = SQ_DESC_TYPE_GATHER;
+	gather->ld_type = NIC_SEND_LD_TYPE_E_LDWB;
+	gather->size = size;
+	gather->addr = data;
+}
+
+/* Segment a TSO packet into 'gso_size' segments and append
+ * them to SQ for transfer
+ */
+static int nicvf_sq_append_tso(struct nicvf *nic, struct snd_queue *sq,
+			       int qentry, struct sk_buff *skb)
+{
+	struct tso_t tso;
+	int seg_subdescs = 0, desc_cnt = 0;
+	int seg_len, total_len, data_left;
+	int hdr_qentry = qentry;
+	int hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
+
+	tso_start(skb, &tso);
+	total_len = skb->len - hdr_len;
+	while (total_len > 0) {
+		char *hdr;
+
+		/* Save Qentry for adding HDR_SUBDESC at the end */
+		hdr_qentry = qentry;
+
+		data_left = min_t(int, skb_shinfo(skb)->gso_size, total_len);
+		total_len -= data_left;
+
+		/* Add segment's header */
+		qentry = nicvf_get_nxt_sqentry(sq, qentry);
+		hdr = sq->tso_hdrs + qentry * TSO_HEADER_SIZE;
+		tso_build_hdr(skb, hdr, &tso, data_left, total_len == 0);
+		nicvf_sq_add_gather_subdesc(sq, qentry, hdr_len,
+					    sq->tso_hdrs_phys +
+					    qentry * TSO_HEADER_SIZE);
+		/* HDR_SUDESC + GATHER */
+		seg_subdescs = 2;
+		seg_len = hdr_len;
+
+		/* Add segment's payload fragments */
+		while (data_left > 0) {
+			int size;
+
+			size = min_t(int, tso.size, data_left);
+
+			qentry = nicvf_get_nxt_sqentry(sq, qentry);
+			nicvf_sq_add_gather_subdesc(sq, qentry, size,
+						    virt_to_phys(tso.data));
+			seg_subdescs++;
+			seg_len += size;
+
+			data_left -= size;
+			tso_build_data(skb, &tso, size);
+		}
+		nicvf_sq_add_hdr_subdesc(sq, hdr_qentry,
+					 seg_subdescs - 1, skb, seg_len);
+		sq->skbuff[hdr_qentry] = 0;
+		qentry = nicvf_get_nxt_sqentry(sq, qentry);
+
+		desc_cnt += seg_subdescs;
+	}
+	/* Save SKB in the last segment for freeing */
+	sq->skbuff[hdr_qentry] = (u64)skb;
+
+	/* make sure all memory stores are done before ringing doorbell */
+	smp_wmb();
+
+	/* Inform HW to xmit all TSO segments */
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_DOOR,
+			      skb_get_queue_mapping(skb), desc_cnt);
+	return 1;
+}
+
+/* Append an skb to a SQ for packet transfer. */
+int nicvf_sq_append_skb(struct nicvf *nic, struct sk_buff *skb)
+{
+	int i, size;
+	int subdesc_cnt;
+	int sq_num, qentry;
+	struct queue_set *qs = nic->qs;
+	struct snd_queue *sq;
+
+	sq_num = skb_get_queue_mapping(skb);
+	sq = &qs->sq[sq_num];
+
+	subdesc_cnt = nicvf_sq_subdesc_required(nic, skb);
+	if (subdesc_cnt > atomic_read(&sq->free_cnt))
+		goto append_fail;
+
+	qentry = nicvf_get_sq_desc(sq, subdesc_cnt);
+
+	/* Check if its a TSO packet */
+	if (skb_shinfo(skb)->gso_size)
+		return nicvf_sq_append_tso(nic, sq, qentry, skb);
+
+	/* Add SQ header subdesc */
+	nicvf_sq_add_hdr_subdesc(sq, qentry, subdesc_cnt - 1, skb, skb->len);
+
+	/* Add SQ gather subdescs */
+	qentry = nicvf_get_nxt_sqentry(sq, qentry);
+	size = skb_is_nonlinear(skb) ? skb_headlen(skb) : skb->len;
+	nicvf_sq_add_gather_subdesc(sq, qentry, size, virt_to_phys(skb->data));
+
+	/* Check for scattered buffer */
+	if (!skb_is_nonlinear(skb))
+		goto doorbell;
+
+	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+		const struct skb_frag_struct *frag;
+
+		frag = &skb_shinfo(skb)->frags[i];
+
+		qentry = nicvf_get_nxt_sqentry(sq, qentry);
+		size = skb_frag_size(frag);
+		nicvf_sq_add_gather_subdesc(sq, qentry, size,
+					    virt_to_phys(
+					    skb_frag_address(frag)));
+	}
+
+doorbell:
+	/* make sure all memory stores are done before ringing doorbell */
+	smp_wmb();
+
+	/* Inform HW to xmit new packet */
+	nicvf_queue_reg_write(nic, NIC_QSET_SQ_0_7_DOOR,
+			      sq_num, subdesc_cnt);
+	return 1;
+
+append_fail:
+	netdev_dbg(nic->netdev, "Not enough SQ descriptors to xmit pkt\n");
+	return 0;
+}
+
+static inline unsigned frag_num(unsigned i)
+{
+#ifdef __BIG_ENDIAN
+	return (i & ~3) + 3 - (i & 3);
+#else
+	return i;
+#endif
+}
+
+/* Returns SKB for a received packet */
+struct sk_buff *nicvf_get_rcv_skb(struct nicvf *nic, struct cqe_rx_t *cqe_rx)
+{
+	int frag;
+	int payload_len = 0;
+	struct sk_buff *skb = NULL;
+	struct sk_buff *skb_frag = NULL;
+	struct sk_buff *prev_frag = NULL;
+	u16 *rb_lens = NULL;
+	u64 *rb_ptrs = NULL;
+
+	rb_lens = (void *)cqe_rx + (3 * sizeof(u64));
+	rb_ptrs = (void *)cqe_rx + (6 * sizeof(u64));
+
+	netdev_dbg(nic->netdev, "%s rb_cnt %d rb0_ptr %llx rb0_sz %d\n",
+		   __func__, cqe_rx->rb_cnt, cqe_rx->rb0_ptr, cqe_rx->rb0_sz);
+
+	for (frag = 0; frag < cqe_rx->rb_cnt; frag++) {
+		payload_len = rb_lens[frag_num(frag)];
+		if (!frag) {
+			/* First fragment */
+			skb = nicvf_rb_ptr_to_skb(nic,
+						  *rb_ptrs - cqe_rx->align_pad,
+						  payload_len);
+			if (!skb)
+				return NULL;
+			skb_reserve(skb, cqe_rx->align_pad);
+			skb_put(skb, payload_len);
+		} else {
+			/* Add fragments */
+			skb_frag = nicvf_rb_ptr_to_skb(nic, *rb_ptrs,
+						       payload_len);
+			if (!skb_frag) {
+				dev_kfree_skb(skb);
+				return NULL;
+			}
+
+			if (!skb_shinfo(skb)->frag_list)
+				skb_shinfo(skb)->frag_list = skb_frag;
+			else
+				prev_frag->next = skb_frag;
+
+			prev_frag = skb_frag;
+			skb->len += payload_len;
+			skb->data_len += payload_len;
+			skb_frag->len = payload_len;
+		}
+		/* Next buffer pointer */
+		rb_ptrs++;
+	}
+	return skb;
+}
+
+/* Enable interrupt */
+void nicvf_enable_intr(struct nicvf *nic, int int_type, int q_idx)
+{
+	u64 reg_val;
+
+	reg_val = nicvf_reg_read(nic, NIC_VF_ENA_W1S);
+
+	switch (int_type) {
+	case NICVF_INTR_CQ:
+		reg_val |= ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
+		break;
+	case NICVF_INTR_SQ:
+		reg_val |= ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
+		break;
+	case NICVF_INTR_RBDR:
+		reg_val |= ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
+		break;
+	case NICVF_INTR_PKT_DROP:
+		reg_val |= (1ULL << NICVF_INTR_PKT_DROP_SHIFT);
+		break;
+	case NICVF_INTR_TCP_TIMER:
+		reg_val |= (1ULL << NICVF_INTR_TCP_TIMER_SHIFT);
+		break;
+	case NICVF_INTR_MBOX:
+		reg_val |= (1ULL << NICVF_INTR_MBOX_SHIFT);
+		break;
+	case NICVF_INTR_QS_ERR:
+		reg_val |= (1ULL << NICVF_INTR_QS_ERR_SHIFT);
+		break;
+	default:
+		netdev_err(nic->netdev,
+			   "Failed to enable interrupt: unknown type\n");
+		break;
+	}
+
+	nicvf_reg_write(nic, NIC_VF_ENA_W1S, reg_val);
+}
+
+/* Disable interrupt */
+void nicvf_disable_intr(struct nicvf *nic, int int_type, int q_idx)
+{
+	u64 reg_val = 0;
+
+	switch (int_type) {
+	case NICVF_INTR_CQ:
+		reg_val |= ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
+		break;
+	case NICVF_INTR_SQ:
+		reg_val |= ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
+		break;
+	case NICVF_INTR_RBDR:
+		reg_val |= ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
+		break;
+	case NICVF_INTR_PKT_DROP:
+		reg_val |= (1ULL << NICVF_INTR_PKT_DROP_SHIFT);
+		break;
+	case NICVF_INTR_TCP_TIMER:
+		reg_val |= (1ULL << NICVF_INTR_TCP_TIMER_SHIFT);
+		break;
+	case NICVF_INTR_MBOX:
+		reg_val |= (1ULL << NICVF_INTR_MBOX_SHIFT);
+		break;
+	case NICVF_INTR_QS_ERR:
+		reg_val |= (1ULL << NICVF_INTR_QS_ERR_SHIFT);
+		break;
+	default:
+		netdev_err(nic->netdev,
+			   "Failed to disable interrupt: unknown type\n");
+		break;
+	}
+
+	nicvf_reg_write(nic, NIC_VF_ENA_W1C, reg_val);
+}
+
+/* Clear interrupt */
+void nicvf_clear_intr(struct nicvf *nic, int int_type, int q_idx)
+{
+	u64 reg_val = 0;
+
+	switch (int_type) {
+	case NICVF_INTR_CQ:
+		reg_val = ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
+		break;
+	case NICVF_INTR_SQ:
+		reg_val = ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
+		break;
+	case NICVF_INTR_RBDR:
+		reg_val = ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
+		break;
+	case NICVF_INTR_PKT_DROP:
+		reg_val = (1ULL << NICVF_INTR_PKT_DROP_SHIFT);
+		break;
+	case NICVF_INTR_TCP_TIMER:
+		reg_val = (1ULL << NICVF_INTR_TCP_TIMER_SHIFT);
+		break;
+	case NICVF_INTR_MBOX:
+		reg_val = (1ULL << NICVF_INTR_MBOX_SHIFT);
+		break;
+	case NICVF_INTR_QS_ERR:
+		reg_val |= (1ULL << NICVF_INTR_QS_ERR_SHIFT);
+		break;
+	default:
+		netdev_err(nic->netdev,
+			   "Failed to clear interrupt: unknown type\n");
+		break;
+	}
+
+	nicvf_reg_write(nic, NIC_VF_INT, reg_val);
+}
+
+/* Check if interrupt is enabled */
+int nicvf_is_intr_enabled(struct nicvf *nic, int int_type, int q_idx)
+{
+	u64 reg_val;
+	u64 mask = 0xff;
+
+	reg_val = nicvf_reg_read(nic, NIC_VF_ENA_W1S);
+
+	switch (int_type) {
+	case NICVF_INTR_CQ:
+		mask = ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
+		break;
+	case NICVF_INTR_SQ:
+		mask = ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
+		break;
+	case NICVF_INTR_RBDR:
+		mask = ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
+		break;
+	case NICVF_INTR_PKT_DROP:
+		mask = NICVF_INTR_PKT_DROP_MASK;
+		break;
+	case NICVF_INTR_TCP_TIMER:
+		mask = NICVF_INTR_TCP_TIMER_MASK;
+		break;
+	case NICVF_INTR_MBOX:
+		mask = NICVF_INTR_MBOX_MASK;
+		break;
+	case NICVF_INTR_QS_ERR:
+		mask = NICVF_INTR_QS_ERR_MASK;
+		break;
+	default:
+		netdev_err(nic->netdev,
+			   "Failed to check interrupt enable: unknown type\n");
+		break;
+	}
+
+	return (reg_val & mask);
+}
+
+void nicvf_update_rq_stats(struct nicvf *nic, int rq_idx)
+{
+	struct rcv_queue *rq;
+
+#define GET_RQ_STATS(reg) \
+	nicvf_reg_read(nic, NIC_QSET_RQ_0_7_STAT_0_1 |\
+			    (rq_idx << NIC_Q_NUM_SHIFT) | (reg << 3))
+
+	rq = &nic->qs->rq[rq_idx];
+	rq->stats.bytes = GET_RQ_STATS(RQ_SQ_STATS_OCTS);
+	rq->stats.pkts = GET_RQ_STATS(RQ_SQ_STATS_PKTS);
+}
+
+void nicvf_update_sq_stats(struct nicvf *nic, int sq_idx)
+{
+	struct snd_queue *sq;
+
+#define GET_SQ_STATS(reg) \
+	nicvf_reg_read(nic, NIC_QSET_SQ_0_7_STAT_0_1 |\
+			    (sq_idx << NIC_Q_NUM_SHIFT) | (reg << 3))
+
+	sq = &nic->qs->sq[sq_idx];
+	sq->stats.bytes = GET_SQ_STATS(RQ_SQ_STATS_OCTS);
+	sq->stats.pkts = GET_SQ_STATS(RQ_SQ_STATS_PKTS);
+}
+
+/* Check for errors in the receive cmp.queue entry */
+int nicvf_check_cqe_rx_errs(struct nicvf *nic,
+			    struct cmp_queue *cq, struct cqe_rx_t *cqe_rx)
+{
+	struct cmp_queue_stats *stats = &cq->stats;
+
+	if (!cqe_rx->err_level && !cqe_rx->err_opcode) {
+		stats->rx.errop.good++;
+		return 0;
+	}
+
+	if (netif_msg_rx_err(nic))
+		netdev_err(nic->netdev,
+			   "%s: RX error CQE err_level 0x%x err_opcode 0x%x\n",
+			   nic->netdev->name,
+			   cqe_rx->err_level, cqe_rx->err_opcode);
+
+	switch (cqe_rx->err_level) {
+	case CQ_ERRLVL_MAC:
+		stats->rx.errlvl.mac_errs++;
+		break;
+	case CQ_ERRLVL_L2:
+		stats->rx.errlvl.l2_errs++;
+		break;
+	case CQ_ERRLVL_L3:
+		stats->rx.errlvl.l3_errs++;
+		break;
+	case CQ_ERRLVL_L4:
+		stats->rx.errlvl.l4_errs++;
+		break;
+	}
+
+	switch (cqe_rx->err_opcode) {
+	case CQ_RX_ERROP_RE_PARTIAL:
+		stats->rx.errop.partial_pkts++;
+		break;
+	case CQ_RX_ERROP_RE_JABBER:
+		stats->rx.errop.jabber_errs++;
+		break;
+	case CQ_RX_ERROP_RE_FCS:
+		stats->rx.errop.fcs_errs++;
+		break;
+	case CQ_RX_ERROP_RE_TERMINATE:
+		stats->rx.errop.terminate_errs++;
+		break;
+	case CQ_RX_ERROP_RE_RX_CTL:
+		stats->rx.errop.bgx_rx_errs++;
+		break;
+	case CQ_RX_ERROP_PREL2_ERR:
+		stats->rx.errop.prel2_errs++;
+		break;
+	case CQ_RX_ERROP_L2_FRAGMENT:
+		stats->rx.errop.l2_frags++;
+		break;
+	case CQ_RX_ERROP_L2_OVERRUN:
+		stats->rx.errop.l2_overruns++;
+		break;
+	case CQ_RX_ERROP_L2_PFCS:
+		stats->rx.errop.l2_pfcs++;
+		break;
+	case CQ_RX_ERROP_L2_PUNY:
+		stats->rx.errop.l2_puny++;
+		break;
+	case CQ_RX_ERROP_L2_MAL:
+		stats->rx.errop.l2_hdr_malformed++;
+		break;
+	case CQ_RX_ERROP_L2_OVERSIZE:
+		stats->rx.errop.l2_oversize++;
+		break;
+	case CQ_RX_ERROP_L2_UNDERSIZE:
+		stats->rx.errop.l2_undersize++;
+		break;
+	case CQ_RX_ERROP_L2_LENMISM:
+		stats->rx.errop.l2_len_mismatch++;
+		break;
+	case CQ_RX_ERROP_L2_PCLP:
+		stats->rx.errop.l2_pclp++;
+		break;
+	case CQ_RX_ERROP_IP_NOT:
+		stats->rx.errop.non_ip++;
+		break;
+	case CQ_RX_ERROP_IP_CSUM_ERR:
+		stats->rx.errop.ip_csum_err++;
+		break;
+	case CQ_RX_ERROP_IP_MAL:
+		stats->rx.errop.ip_hdr_malformed++;
+		break;
+	case CQ_RX_ERROP_IP_MALD:
+		stats->rx.errop.ip_payload_malformed++;
+		break;
+	case CQ_RX_ERROP_IP_HOP:
+		stats->rx.errop.ip_hop_errs++;
+		break;
+	case CQ_RX_ERROP_L3_ICRC:
+		stats->rx.errop.l3_icrc_errs++;
+		break;
+	case CQ_RX_ERROP_L3_PCLP:
+		stats->rx.errop.l3_pclp++;
+		break;
+	case CQ_RX_ERROP_L4_MAL:
+		stats->rx.errop.l4_malformed++;
+		break;
+	case CQ_RX_ERROP_L4_CHK:
+		stats->rx.errop.l4_csum_errs++;
+		break;
+	case CQ_RX_ERROP_UDP_LEN:
+		stats->rx.errop.udp_len_err++;
+		break;
+	case CQ_RX_ERROP_L4_PORT:
+		stats->rx.errop.bad_l4_port++;
+		break;
+	case CQ_RX_ERROP_TCP_FLAG:
+		stats->rx.errop.bad_tcp_flag++;
+		break;
+	case CQ_RX_ERROP_TCP_OFFSET:
+		stats->rx.errop.tcp_offset_errs++;
+		break;
+	case CQ_RX_ERROP_L4_PCLP:
+		stats->rx.errop.l4_pclp++;
+		break;
+	case CQ_RX_ERROP_RBDR_TRUNC:
+		stats->rx.errop.pkt_truncated++;
+		break;
+	}
+
+	return 1;
+}
+
+/* Check for errors in the send cmp.queue entry */
+int nicvf_check_cqe_tx_errs(struct nicvf *nic,
+			    struct cmp_queue *cq, struct cqe_send_t *cqe_tx)
+{
+	struct cmp_queue_stats *stats = &cq->stats;
+
+	switch (cqe_tx->send_status) {
+	case CQ_TX_ERROP_GOOD:
+		stats->tx.good++;
+		return 0;
+	case CQ_TX_ERROP_DESC_FAULT:
+		stats->tx.desc_fault++;
+		break;
+	case CQ_TX_ERROP_HDR_CONS_ERR:
+		stats->tx.hdr_cons_err++;
+		break;
+	case CQ_TX_ERROP_SUBDC_ERR:
+		stats->tx.subdesc_err++;
+		break;
+	case CQ_TX_ERROP_IMM_SIZE_OFLOW:
+		stats->tx.imm_size_oflow++;
+		break;
+	case CQ_TX_ERROP_DATA_SEQUENCE_ERR:
+		stats->tx.data_seq_err++;
+		break;
+	case CQ_TX_ERROP_MEM_SEQUENCE_ERR:
+		stats->tx.mem_seq_err++;
+		break;
+	case CQ_TX_ERROP_LOCK_VIOL:
+		stats->tx.lock_viol++;
+		break;
+	case CQ_TX_ERROP_DATA_FAULT:
+		stats->tx.data_fault++;
+		break;
+	case CQ_TX_ERROP_TSTMP_CONFLICT:
+		stats->tx.tstmp_conflict++;
+		break;
+	case CQ_TX_ERROP_TSTMP_TIMEOUT:
+		stats->tx.tstmp_timeout++;
+		break;
+	case CQ_TX_ERROP_MEM_FAULT:
+		stats->tx.mem_fault++;
+		break;
+	case CQ_TX_ERROP_CK_OVERLAP:
+		stats->tx.csum_overlap++;
+		break;
+	case CQ_TX_ERROP_CK_OFLOW:
+		stats->tx.csum_overflow++;
+		break;
+	}
+
+	return 1;
+}
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_queues.h b/drivers/net/ethernet/cavium/thunder/nicvf_queues.h
new file mode 100644
index 000000000000..8341bdf755d1
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_queues.h
@@ -0,0 +1,381 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * 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.
+ */
+
+#ifndef NICVF_QUEUES_H
+#define NICVF_QUEUES_H
+
+#include <linux/netdevice.h>
+#include "q_struct.h"
+
+#define MAX_QUEUE_SET			128
+#define MAX_RCV_QUEUES_PER_QS		8
+#define MAX_RCV_BUF_DESC_RINGS_PER_QS	2
+#define MAX_SND_QUEUES_PER_QS		8
+#define MAX_CMP_QUEUES_PER_QS		8
+
+/* VF's queue interrupt ranges */
+#define	NICVF_INTR_ID_CQ		0
+#define	NICVF_INTR_ID_SQ		8
+#define	NICVF_INTR_ID_RBDR		16
+#define	NICVF_INTR_ID_MISC		18
+#define	NICVF_INTR_ID_QS_ERR		19
+
+#define	for_each_cq_irq(irq)	\
+	for (irq = NICVF_INTR_ID_CQ; irq < NICVF_INTR_ID_SQ; irq++)
+#define	for_each_sq_irq(irq)	\
+	for (irq = NICVF_INTR_ID_SQ; irq < NICVF_INTR_ID_RBDR; irq++)
+#define	for_each_rbdr_irq(irq)	\
+	for (irq = NICVF_INTR_ID_RBDR; irq < NICVF_INTR_ID_MISC; irq++)
+
+#define RBDR_SIZE0		0ULL /* 8K entries */
+#define RBDR_SIZE1		1ULL /* 16K entries */
+#define RBDR_SIZE2		2ULL /* 32K entries */
+#define RBDR_SIZE3		3ULL /* 64K entries */
+#define RBDR_SIZE4		4ULL /* 126K entries */
+#define RBDR_SIZE5		5ULL /* 256K entries */
+#define RBDR_SIZE6		6ULL /* 512K entries */
+
+#define SND_QUEUE_SIZE0		0ULL /* 1K entries */
+#define SND_QUEUE_SIZE1		1ULL /* 2K entries */
+#define SND_QUEUE_SIZE2		2ULL /* 4K entries */
+#define SND_QUEUE_SIZE3		3ULL /* 8K entries */
+#define SND_QUEUE_SIZE4		4ULL /* 16K entries */
+#define SND_QUEUE_SIZE5		5ULL /* 32K entries */
+#define SND_QUEUE_SIZE6		6ULL /* 64K entries */
+
+#define CMP_QUEUE_SIZE0		0ULL /* 1K entries */
+#define CMP_QUEUE_SIZE1		1ULL /* 2K entries */
+#define CMP_QUEUE_SIZE2		2ULL /* 4K entries */
+#define CMP_QUEUE_SIZE3		3ULL /* 8K entries */
+#define CMP_QUEUE_SIZE4		4ULL /* 16K entries */
+#define CMP_QUEUE_SIZE5		5ULL /* 32K entries */
+#define CMP_QUEUE_SIZE6		6ULL /* 64K entries */
+
+/* Default queue count per QS, its lengths and threshold values */
+#define RBDR_CNT		1
+#define RCV_QUEUE_CNT		8
+#define SND_QUEUE_CNT		8
+#define CMP_QUEUE_CNT		8 /* Max of RCV and SND qcount */
+
+#define SND_QSIZE		SND_QUEUE_SIZE4
+#define SND_QUEUE_LEN		(1ULL << (SND_QSIZE + 10))
+#define MAX_SND_QUEUE_LEN	(1ULL << (SND_QUEUE_SIZE6 + 10))
+#define SND_QUEUE_THRESH	2ULL
+#define MIN_SQ_DESC_PER_PKT_XMIT	2
+/* Since timestamp not enabled, otherwise 2 */
+#define MAX_CQE_PER_PKT_XMIT		1
+
+#define CMP_QSIZE		CMP_QUEUE_SIZE4
+#define CMP_QUEUE_LEN		(1ULL << (CMP_QSIZE + 10))
+#define CMP_QUEUE_CQE_THRESH	0
+#define CMP_QUEUE_TIMER_THRESH	220 /* 10usec */
+
+#define RBDR_SIZE		RBDR_SIZE0
+#define RCV_BUF_COUNT		(1ULL << (RBDR_SIZE + 13))
+#define MAX_RCV_BUF_COUNT	(1ULL << (RBDR_SIZE6 + 13))
+#define RBDR_THRESH		(RCV_BUF_COUNT / 2)
+#define DMA_BUFFER_LEN		2048 /* In multiples of 128bytes */
+#define RCV_FRAG_LEN	(SKB_DATA_ALIGN(DMA_BUFFER_LEN + NET_SKB_PAD) + \
+			 SKB_DATA_ALIGN(sizeof(struct skb_shared_info)) + \
+			 (NICVF_RCV_BUF_ALIGN_BYTES * 2))
+#define RCV_DATA_OFFSET		NICVF_RCV_BUF_ALIGN_BYTES
+
+#define MAX_CQES_FOR_TX		((SND_QUEUE_LEN / MIN_SQ_DESC_PER_PKT_XMIT) * \
+				 MAX_CQE_PER_PKT_XMIT)
+#define RQ_CQ_DROP		((CMP_QUEUE_LEN - MAX_CQES_FOR_TX) / 256)
+
+/* Descriptor size in bytes */
+#define SND_QUEUE_DESC_SIZE	16
+#define CMP_QUEUE_DESC_SIZE	512
+
+/* Buffer / descriptor alignments */
+#define NICVF_RCV_BUF_ALIGN		7
+#define NICVF_RCV_BUF_ALIGN_BYTES	(1ULL << NICVF_RCV_BUF_ALIGN)
+#define NICVF_CQ_BASE_ALIGN_BYTES	512  /* 9 bits */
+#define NICVF_SQ_BASE_ALIGN_BYTES	128  /* 7 bits */
+
+#define NICVF_ALIGNED_ADDR(ADDR, ALIGN_BYTES)	ALIGN(ADDR, ALIGN_BYTES)
+#define NICVF_ADDR_ALIGN_LEN(ADDR, BYTES)\
+	(NICVF_ALIGNED_ADDR(ADDR, BYTES) - BYTES)
+#define NICVF_RCV_BUF_ALIGN_LEN(X)\
+	(NICVF_ALIGNED_ADDR(X, NICVF_RCV_BUF_ALIGN_BYTES) - X)
+
+/* Queue enable/disable */
+#define NICVF_SQ_EN		BIT_ULL(19)
+
+/* Queue reset */
+#define NICVF_CQ_RESET		BIT_ULL(41)
+#define NICVF_SQ_RESET		BIT_ULL(17)
+#define NICVF_RBDR_RESET	BIT_ULL(43)
+
+enum CQ_RX_ERRLVL_E {
+	CQ_ERRLVL_MAC,
+	CQ_ERRLVL_L2,
+	CQ_ERRLVL_L3,
+	CQ_ERRLVL_L4,
+};
+
+enum CQ_RX_ERROP_E {
+	CQ_RX_ERROP_RE_NONE = 0x0,
+	CQ_RX_ERROP_RE_PARTIAL = 0x1,
+	CQ_RX_ERROP_RE_JABBER = 0x2,
+	CQ_RX_ERROP_RE_FCS = 0x7,
+	CQ_RX_ERROP_RE_TERMINATE = 0x9,
+	CQ_RX_ERROP_RE_RX_CTL = 0xb,
+	CQ_RX_ERROP_PREL2_ERR = 0x1f,
+	CQ_RX_ERROP_L2_FRAGMENT = 0x20,
+	CQ_RX_ERROP_L2_OVERRUN = 0x21,
+	CQ_RX_ERROP_L2_PFCS = 0x22,
+	CQ_RX_ERROP_L2_PUNY = 0x23,
+	CQ_RX_ERROP_L2_MAL = 0x24,
+	CQ_RX_ERROP_L2_OVERSIZE = 0x25,
+	CQ_RX_ERROP_L2_UNDERSIZE = 0x26,
+	CQ_RX_ERROP_L2_LENMISM = 0x27,
+	CQ_RX_ERROP_L2_PCLP = 0x28,
+	CQ_RX_ERROP_IP_NOT = 0x41,
+	CQ_RX_ERROP_IP_CSUM_ERR = 0x42,
+	CQ_RX_ERROP_IP_MAL = 0x43,
+	CQ_RX_ERROP_IP_MALD = 0x44,
+	CQ_RX_ERROP_IP_HOP = 0x45,
+	CQ_RX_ERROP_L3_ICRC = 0x46,
+	CQ_RX_ERROP_L3_PCLP = 0x47,
+	CQ_RX_ERROP_L4_MAL = 0x61,
+	CQ_RX_ERROP_L4_CHK = 0x62,
+	CQ_RX_ERROP_UDP_LEN = 0x63,
+	CQ_RX_ERROP_L4_PORT = 0x64,
+	CQ_RX_ERROP_TCP_FLAG = 0x65,
+	CQ_RX_ERROP_TCP_OFFSET = 0x66,
+	CQ_RX_ERROP_L4_PCLP = 0x67,
+	CQ_RX_ERROP_RBDR_TRUNC = 0x70,
+};
+
+enum CQ_TX_ERROP_E {
+	CQ_TX_ERROP_GOOD = 0x0,
+	CQ_TX_ERROP_DESC_FAULT = 0x10,
+	CQ_TX_ERROP_HDR_CONS_ERR = 0x11,
+	CQ_TX_ERROP_SUBDC_ERR = 0x12,
+	CQ_TX_ERROP_IMM_SIZE_OFLOW = 0x80,
+	CQ_TX_ERROP_DATA_SEQUENCE_ERR = 0x81,
+	CQ_TX_ERROP_MEM_SEQUENCE_ERR = 0x82,
+	CQ_TX_ERROP_LOCK_VIOL = 0x83,
+	CQ_TX_ERROP_DATA_FAULT = 0x84,
+	CQ_TX_ERROP_TSTMP_CONFLICT = 0x85,
+	CQ_TX_ERROP_TSTMP_TIMEOUT = 0x86,
+	CQ_TX_ERROP_MEM_FAULT = 0x87,
+	CQ_TX_ERROP_CK_OVERLAP = 0x88,
+	CQ_TX_ERROP_CK_OFLOW = 0x89,
+	CQ_TX_ERROP_ENUM_LAST = 0x8a,
+};
+
+struct cmp_queue_stats {
+	struct rx_stats {
+		struct {
+			u64 mac_errs;
+			u64 l2_errs;
+			u64 l3_errs;
+			u64 l4_errs;
+		} errlvl;
+		struct {
+			u64 good;
+			u64 partial_pkts;
+			u64 jabber_errs;
+			u64 fcs_errs;
+			u64 terminate_errs;
+			u64 bgx_rx_errs;
+			u64 prel2_errs;
+			u64 l2_frags;
+			u64 l2_overruns;
+			u64 l2_pfcs;
+			u64 l2_puny;
+			u64 l2_hdr_malformed;
+			u64 l2_oversize;
+			u64 l2_undersize;
+			u64 l2_len_mismatch;
+			u64 l2_pclp;
+			u64 non_ip;
+			u64 ip_csum_err;
+			u64 ip_hdr_malformed;
+			u64 ip_payload_malformed;
+			u64 ip_hop_errs;
+			u64 l3_icrc_errs;
+			u64 l3_pclp;
+			u64 l4_malformed;
+			u64 l4_csum_errs;
+			u64 udp_len_err;
+			u64 bad_l4_port;
+			u64 bad_tcp_flag;
+			u64 tcp_offset_errs;
+			u64 l4_pclp;
+			u64 pkt_truncated;
+		} errop;
+	} rx;
+	struct tx_stats {
+		u64 good;
+		u64 desc_fault;
+		u64 hdr_cons_err;
+		u64 subdesc_err;
+		u64 imm_size_oflow;
+		u64 data_seq_err;
+		u64 mem_seq_err;
+		u64 lock_viol;
+		u64 data_fault;
+		u64 tstmp_conflict;
+		u64 tstmp_timeout;
+		u64 mem_fault;
+		u64 csum_overlap;
+		u64 csum_overflow;
+	} tx;
+} ____cacheline_aligned_in_smp;
+
+enum RQ_SQ_STATS {
+	RQ_SQ_STATS_OCTS,
+	RQ_SQ_STATS_PKTS,
+};
+
+struct rx_tx_queue_stats {
+	u64	bytes;
+	u64	pkts;
+} ____cacheline_aligned_in_smp;
+
+struct q_desc_mem {
+	dma_addr_t	dma;
+	u64		size;
+	u16		q_len;
+	dma_addr_t	phys_base;
+	void		*base;
+	void		*unalign_base;
+};
+
+struct rbdr {
+	bool		enable;
+	u32		dma_size;
+	u32		frag_len;
+	u32		thresh;		/* Threshold level for interrupt */
+	void		*desc;
+	u32		head;
+	u32		tail;
+	struct q_desc_mem   dmem;
+} ____cacheline_aligned_in_smp;
+
+struct rcv_queue {
+	bool		enable;
+	struct	rbdr	*rbdr_start;
+	struct	rbdr	*rbdr_cont;
+	bool		en_tcp_reassembly;
+	u8		cq_qs;  /* CQ's QS to which this RQ is assigned */
+	u8		cq_idx; /* CQ index (0 to 7) in the QS */
+	u8		cont_rbdr_qs;      /* Continue buffer ptrs - QS num */
+	u8		cont_qs_rbdr_idx;  /* RBDR idx in the cont QS */
+	u8		start_rbdr_qs;     /* First buffer ptrs - QS num */
+	u8		start_qs_rbdr_idx; /* RBDR idx in the above QS */
+	u8		caching;
+	struct		rx_tx_queue_stats stats;
+} ____cacheline_aligned_in_smp;
+
+struct cmp_queue {
+	bool		enable;
+	u16		thresh;
+	spinlock_t	lock;  /* lock to serialize processing CQEs */
+	void		*desc;
+	struct q_desc_mem   dmem;
+	struct cmp_queue_stats	stats;
+} ____cacheline_aligned_in_smp;
+
+struct snd_queue {
+	bool		enable;
+	u8		cq_qs;  /* CQ's QS to which this SQ is pointing */
+	u8		cq_idx; /* CQ index (0 to 7) in the above QS */
+	u16		thresh;
+	atomic_t	free_cnt;
+	u32		head;
+	u32		tail;
+	u64		*skbuff;
+	void		*desc;
+
+#define	TSO_HEADER_SIZE	128
+	/* For TSO segment's header */
+	char		*tso_hdrs;
+	dma_addr_t	tso_hdrs_phys;
+
+	cpumask_t	affinity_mask;
+	struct q_desc_mem   dmem;
+	struct rx_tx_queue_stats stats;
+} ____cacheline_aligned_in_smp;
+
+struct queue_set {
+	bool		enable;
+	bool		be_en;
+	u8		vnic_id;
+	u8		rq_cnt;
+	u8		cq_cnt;
+	u64		cq_len;
+	u8		sq_cnt;
+	u64		sq_len;
+	u8		rbdr_cnt;
+	u64		rbdr_len;
+	struct	rcv_queue	rq[MAX_RCV_QUEUES_PER_QS];
+	struct	cmp_queue	cq[MAX_CMP_QUEUES_PER_QS];
+	struct	snd_queue	sq[MAX_SND_QUEUES_PER_QS];
+	struct	rbdr		rbdr[MAX_RCV_BUF_DESC_RINGS_PER_QS];
+} ____cacheline_aligned_in_smp;
+
+#define GET_RBDR_DESC(RING, idx)\
+		(&(((struct rbdr_entry_t *)((RING)->desc))[idx]))
+#define GET_SQ_DESC(RING, idx)\
+		(&(((struct sq_hdr_subdesc *)((RING)->desc))[idx]))
+#define GET_CQ_DESC(RING, idx)\
+		(&(((union cq_desc_t *)((RING)->desc))[idx]))
+
+/* CQ status bits */
+#define	CQ_WR_FULL	BIT(26)
+#define	CQ_WR_DISABLE	BIT(25)
+#define	CQ_WR_FAULT	BIT(24)
+#define	CQ_CQE_COUNT	(0xFFFF << 0)
+
+#define	CQ_ERR_MASK	(CQ_WR_FULL | CQ_WR_DISABLE | CQ_WR_FAULT)
+
+int nicvf_set_qset_resources(struct nicvf *nic);
+int nicvf_config_data_transfer(struct nicvf *nic, bool enable);
+void nicvf_qset_config(struct nicvf *nic, bool enable);
+void nicvf_cmp_queue_config(struct nicvf *nic, struct queue_set *qs,
+			    int qidx, bool enable);
+
+void nicvf_sq_enable(struct nicvf *nic, struct snd_queue *sq, int qidx);
+void nicvf_sq_disable(struct nicvf *nic, int qidx);
+void nicvf_put_sq_desc(struct snd_queue *sq, int desc_cnt);
+void nicvf_sq_free_used_descs(struct net_device *netdev,
+			      struct snd_queue *sq, int qidx);
+int nicvf_sq_append_skb(struct nicvf *nic, struct sk_buff *skb);
+
+struct sk_buff *nicvf_get_rcv_skb(struct nicvf *nic, struct cqe_rx_t *cqe_rx);
+void nicvf_rbdr_task(unsigned long data);
+void nicvf_rbdr_work(struct work_struct *work);
+
+void nicvf_enable_intr(struct nicvf *nic, int int_type, int q_idx);
+void nicvf_disable_intr(struct nicvf *nic, int int_type, int q_idx);
+void nicvf_clear_intr(struct nicvf *nic, int int_type, int q_idx);
+int nicvf_is_intr_enabled(struct nicvf *nic, int int_type, int q_idx);
+
+/* Register access APIs */
+void nicvf_reg_write(struct nicvf *nic, u64 offset, u64 val);
+u64  nicvf_reg_read(struct nicvf *nic, u64 offset);
+void nicvf_qset_reg_write(struct nicvf *nic, u64 offset, u64 val);
+u64 nicvf_qset_reg_read(struct nicvf *nic, u64 offset);
+void nicvf_queue_reg_write(struct nicvf *nic, u64 offset,
+			   u64 qidx, u64 val);
+u64  nicvf_queue_reg_read(struct nicvf *nic,
+			  u64 offset, u64 qidx);
+
+/* Stats */
+void nicvf_update_rq_stats(struct nicvf *nic, int rq_idx);
+void nicvf_update_sq_stats(struct nicvf *nic, int sq_idx);
+int nicvf_check_cqe_rx_errs(struct nicvf *nic,
+			    struct cmp_queue *cq, struct cqe_rx_t *cqe_rx);
+int nicvf_check_cqe_tx_errs(struct nicvf *nic,
+			    struct cmp_queue *cq, struct cqe_send_t *cqe_tx);
+#endif /* NICVF_QUEUES_H */
diff --git a/drivers/net/ethernet/cavium/thunder/q_struct.h b/drivers/net/ethernet/cavium/thunder/q_struct.h
new file mode 100644
index 000000000000..3c1de97b1add
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/q_struct.h
@@ -0,0 +1,701 @@
+/*
+ * This file contains HW queue descriptor formats, config register
+ * structures etc
+ *
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * 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.
+ */
+
+#ifndef Q_STRUCT_H
+#define Q_STRUCT_H
+
+/* Load transaction types for reading segment bytes specified by
+ * NIC_SEND_GATHER_S[LD_TYPE].
+ */
+enum nic_send_ld_type_e {
+	NIC_SEND_LD_TYPE_E_LDD = 0x0,
+	NIC_SEND_LD_TYPE_E_LDT = 0x1,
+	NIC_SEND_LD_TYPE_E_LDWB = 0x2,
+	NIC_SEND_LD_TYPE_E_ENUM_LAST = 0x3,
+};
+
+enum ether_type_algorithm {
+	ETYPE_ALG_NONE = 0x0,
+	ETYPE_ALG_SKIP = 0x1,
+	ETYPE_ALG_ENDPARSE = 0x2,
+	ETYPE_ALG_VLAN = 0x3,
+	ETYPE_ALG_VLAN_STRIP = 0x4,
+};
+
+enum layer3_type {
+	L3TYPE_NONE = 0x00,
+	L3TYPE_GRH = 0x01,
+	L3TYPE_IPV4 = 0x04,
+	L3TYPE_IPV4_OPTIONS = 0x05,
+	L3TYPE_IPV6 = 0x06,
+	L3TYPE_IPV6_OPTIONS = 0x07,
+	L3TYPE_ET_STOP = 0x0D,
+	L3TYPE_OTHER = 0x0E,
+};
+
+enum layer4_type {
+	L4TYPE_NONE = 0x00,
+	L4TYPE_IPSEC_ESP = 0x01,
+	L4TYPE_IPFRAG = 0x02,
+	L4TYPE_IPCOMP = 0x03,
+	L4TYPE_TCP = 0x04,
+	L4TYPE_UDP = 0x05,
+	L4TYPE_SCTP = 0x06,
+	L4TYPE_GRE = 0x07,
+	L4TYPE_ROCE_BTH = 0x08,
+	L4TYPE_OTHER = 0x0E,
+};
+
+/* CPI and RSSI configuration */
+enum cpi_algorithm_type {
+	CPI_ALG_NONE = 0x0,
+	CPI_ALG_VLAN = 0x1,
+	CPI_ALG_VLAN16 = 0x2,
+	CPI_ALG_DIFF = 0x3,
+};
+
+enum rss_algorithm_type {
+	RSS_ALG_NONE = 0x00,
+	RSS_ALG_PORT = 0x01,
+	RSS_ALG_IP = 0x02,
+	RSS_ALG_TCP_IP = 0x03,
+	RSS_ALG_UDP_IP = 0x04,
+	RSS_ALG_SCTP_IP = 0x05,
+	RSS_ALG_GRE_IP = 0x06,
+	RSS_ALG_ROCE = 0x07,
+};
+
+enum rss_hash_cfg {
+	RSS_HASH_L2ETC = 0x00,
+	RSS_HASH_IP = 0x01,
+	RSS_HASH_TCP = 0x02,
+	RSS_HASH_TCP_SYN_DIS = 0x03,
+	RSS_HASH_UDP = 0x04,
+	RSS_HASH_L4ETC = 0x05,
+	RSS_HASH_ROCE = 0x06,
+	RSS_L3_BIDI = 0x07,
+	RSS_L4_BIDI = 0x08,
+};
+
+/* Completion queue entry types */
+enum cqe_type {
+	CQE_TYPE_INVALID = 0x0,
+	CQE_TYPE_RX = 0x2,
+	CQE_TYPE_RX_SPLIT = 0x3,
+	CQE_TYPE_RX_TCP = 0x4,
+	CQE_TYPE_SEND = 0x8,
+	CQE_TYPE_SEND_PTP = 0x9,
+};
+
+enum cqe_rx_tcp_status {
+	CQE_RX_STATUS_VALID_TCP_CNXT = 0x00,
+	CQE_RX_STATUS_INVALID_TCP_CNXT = 0x0F,
+};
+
+enum cqe_send_status {
+	CQE_SEND_STATUS_GOOD = 0x00,
+	CQE_SEND_STATUS_DESC_FAULT = 0x01,
+	CQE_SEND_STATUS_HDR_CONS_ERR = 0x11,
+	CQE_SEND_STATUS_SUBDESC_ERR = 0x12,
+	CQE_SEND_STATUS_IMM_SIZE_OFLOW = 0x80,
+	CQE_SEND_STATUS_CRC_SEQ_ERR = 0x81,
+	CQE_SEND_STATUS_DATA_SEQ_ERR = 0x82,
+	CQE_SEND_STATUS_MEM_SEQ_ERR = 0x83,
+	CQE_SEND_STATUS_LOCK_VIOL = 0x84,
+	CQE_SEND_STATUS_LOCK_UFLOW = 0x85,
+	CQE_SEND_STATUS_DATA_FAULT = 0x86,
+	CQE_SEND_STATUS_TSTMP_CONFLICT = 0x87,
+	CQE_SEND_STATUS_TSTMP_TIMEOUT = 0x88,
+	CQE_SEND_STATUS_MEM_FAULT = 0x89,
+	CQE_SEND_STATUS_CSUM_OVERLAP = 0x8A,
+	CQE_SEND_STATUS_CSUM_OVERFLOW = 0x8B,
+};
+
+enum cqe_rx_tcp_end_reason {
+	CQE_RX_TCP_END_FIN_FLAG_DET = 0,
+	CQE_RX_TCP_END_INVALID_FLAG = 1,
+	CQE_RX_TCP_END_TIMEOUT = 2,
+	CQE_RX_TCP_END_OUT_OF_SEQ = 3,
+	CQE_RX_TCP_END_PKT_ERR = 4,
+	CQE_RX_TCP_END_QS_DISABLED = 0x0F,
+};
+
+/* Packet protocol level error enumeration */
+enum cqe_rx_err_level {
+	CQE_RX_ERRLVL_RE = 0x0,
+	CQE_RX_ERRLVL_L2 = 0x1,
+	CQE_RX_ERRLVL_L3 = 0x2,
+	CQE_RX_ERRLVL_L4 = 0x3,
+};
+
+/* Packet protocol level error type enumeration */
+enum cqe_rx_err_opcode {
+	CQE_RX_ERR_RE_NONE = 0x0,
+	CQE_RX_ERR_RE_PARTIAL = 0x1,
+	CQE_RX_ERR_RE_JABBER = 0x2,
+	CQE_RX_ERR_RE_FCS = 0x7,
+	CQE_RX_ERR_RE_TERMINATE = 0x9,
+	CQE_RX_ERR_RE_RX_CTL = 0xb,
+	CQE_RX_ERR_PREL2_ERR = 0x1f,
+	CQE_RX_ERR_L2_FRAGMENT = 0x20,
+	CQE_RX_ERR_L2_OVERRUN = 0x21,
+	CQE_RX_ERR_L2_PFCS = 0x22,
+	CQE_RX_ERR_L2_PUNY = 0x23,
+	CQE_RX_ERR_L2_MAL = 0x24,
+	CQE_RX_ERR_L2_OVERSIZE = 0x25,
+	CQE_RX_ERR_L2_UNDERSIZE = 0x26,
+	CQE_RX_ERR_L2_LENMISM = 0x27,
+	CQE_RX_ERR_L2_PCLP = 0x28,
+	CQE_RX_ERR_IP_NOT = 0x41,
+	CQE_RX_ERR_IP_CHK = 0x42,
+	CQE_RX_ERR_IP_MAL = 0x43,
+	CQE_RX_ERR_IP_MALD = 0x44,
+	CQE_RX_ERR_IP_HOP = 0x45,
+	CQE_RX_ERR_L3_ICRC = 0x46,
+	CQE_RX_ERR_L3_PCLP = 0x47,
+	CQE_RX_ERR_L4_MAL = 0x61,
+	CQE_RX_ERR_L4_CHK = 0x62,
+	CQE_RX_ERR_UDP_LEN = 0x63,
+	CQE_RX_ERR_L4_PORT = 0x64,
+	CQE_RX_ERR_TCP_FLAG = 0x65,
+	CQE_RX_ERR_TCP_OFFSET = 0x66,
+	CQE_RX_ERR_L4_PCLP = 0x67,
+	CQE_RX_ERR_RBDR_TRUNC = 0x70,
+};
+
+struct cqe_rx_t {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64   cqe_type:4; /* W0 */
+	u64   stdn_fault:1;
+	u64   rsvd0:1;
+	u64   rq_qs:7;
+	u64   rq_idx:3;
+	u64   rsvd1:12;
+	u64   rss_alg:4;
+	u64   rsvd2:4;
+	u64   rb_cnt:4;
+	u64   vlan_found:1;
+	u64   vlan_stripped:1;
+	u64   vlan2_found:1;
+	u64   vlan2_stripped:1;
+	u64   l4_type:4;
+	u64   l3_type:4;
+	u64   l2_present:1;
+	u64   err_level:3;
+	u64   err_opcode:8;
+
+	u64   pkt_len:16; /* W1 */
+	u64   l2_ptr:8;
+	u64   l3_ptr:8;
+	u64   l4_ptr:8;
+	u64   cq_pkt_len:8;
+	u64   align_pad:3;
+	u64   rsvd3:1;
+	u64   chan:12;
+
+	u64   rss_tag:32; /* W2 */
+	u64   vlan_tci:16;
+	u64   vlan_ptr:8;
+	u64   vlan2_ptr:8;
+
+	u64   rb3_sz:16; /* W3 */
+	u64   rb2_sz:16;
+	u64   rb1_sz:16;
+	u64   rb0_sz:16;
+
+	u64   rb7_sz:16; /* W4 */
+	u64   rb6_sz:16;
+	u64   rb5_sz:16;
+	u64   rb4_sz:16;
+
+	u64   rb11_sz:16; /* W5 */
+	u64   rb10_sz:16;
+	u64   rb9_sz:16;
+	u64   rb8_sz:16;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64   err_opcode:8;
+	u64   err_level:3;
+	u64   l2_present:1;
+	u64   l3_type:4;
+	u64   l4_type:4;
+	u64   vlan2_stripped:1;
+	u64   vlan2_found:1;
+	u64   vlan_stripped:1;
+	u64   vlan_found:1;
+	u64   rb_cnt:4;
+	u64   rsvd2:4;
+	u64   rss_alg:4;
+	u64   rsvd1:12;
+	u64   rq_idx:3;
+	u64   rq_qs:7;
+	u64   rsvd0:1;
+	u64   stdn_fault:1;
+	u64   cqe_type:4; /* W0 */
+	u64   chan:12;
+	u64   rsvd3:1;
+	u64   align_pad:3;
+	u64   cq_pkt_len:8;
+	u64   l4_ptr:8;
+	u64   l3_ptr:8;
+	u64   l2_ptr:8;
+	u64   pkt_len:16; /* W1 */
+	u64   vlan2_ptr:8;
+	u64   vlan_ptr:8;
+	u64   vlan_tci:16;
+	u64   rss_tag:32; /* W2 */
+	u64   rb0_sz:16;
+	u64   rb1_sz:16;
+	u64   rb2_sz:16;
+	u64   rb3_sz:16; /* W3 */
+	u64   rb4_sz:16;
+	u64   rb5_sz:16;
+	u64   rb6_sz:16;
+	u64   rb7_sz:16; /* W4 */
+	u64   rb8_sz:16;
+	u64   rb9_sz:16;
+	u64   rb10_sz:16;
+	u64   rb11_sz:16; /* W5 */
+#endif
+	u64   rb0_ptr:64;
+	u64   rb1_ptr:64;
+	u64   rb2_ptr:64;
+	u64   rb3_ptr:64;
+	u64   rb4_ptr:64;
+	u64   rb5_ptr:64;
+	u64   rb6_ptr:64;
+	u64   rb7_ptr:64;
+	u64   rb8_ptr:64;
+	u64   rb9_ptr:64;
+	u64   rb10_ptr:64;
+	u64   rb11_ptr:64;
+};
+
+struct cqe_rx_tcp_err_t {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64   cqe_type:4; /* W0 */
+	u64   rsvd0:60;
+
+	u64   rsvd1:4; /* W1 */
+	u64   partial_first:1;
+	u64   rsvd2:27;
+	u64   rbdr_bytes:8;
+	u64   rsvd3:24;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64   rsvd0:60;
+	u64   cqe_type:4;
+
+	u64   rsvd3:24;
+	u64   rbdr_bytes:8;
+	u64   rsvd2:27;
+	u64   partial_first:1;
+	u64   rsvd1:4;
+#endif
+};
+
+struct cqe_rx_tcp_t {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64   cqe_type:4; /* W0 */
+	u64   rsvd0:52;
+	u64   cq_tcp_status:8;
+
+	u64   rsvd1:32; /* W1 */
+	u64   tcp_cntx_bytes:8;
+	u64   rsvd2:8;
+	u64   tcp_err_bytes:16;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64   cq_tcp_status:8;
+	u64   rsvd0:52;
+	u64   cqe_type:4; /* W0 */
+
+	u64   tcp_err_bytes:16;
+	u64   rsvd2:8;
+	u64   tcp_cntx_bytes:8;
+	u64   rsvd1:32; /* W1 */
+#endif
+};
+
+struct cqe_send_t {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64   cqe_type:4; /* W0 */
+	u64   rsvd0:4;
+	u64   sqe_ptr:16;
+	u64   rsvd1:4;
+	u64   rsvd2:10;
+	u64   sq_qs:7;
+	u64   sq_idx:3;
+	u64   rsvd3:8;
+	u64   send_status:8;
+
+	u64   ptp_timestamp:64; /* W1 */
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64   send_status:8;
+	u64   rsvd3:8;
+	u64   sq_idx:3;
+	u64   sq_qs:7;
+	u64   rsvd2:10;
+	u64   rsvd1:4;
+	u64   sqe_ptr:16;
+	u64   rsvd0:4;
+	u64   cqe_type:4; /* W0 */
+
+	u64   ptp_timestamp:64; /* W1 */
+#endif
+};
+
+union cq_desc_t {
+	u64    u[64];
+	struct cqe_send_t snd_hdr;
+	struct cqe_rx_t rx_hdr;
+	struct cqe_rx_tcp_t rx_tcp_hdr;
+	struct cqe_rx_tcp_err_t rx_tcp_err_hdr;
+};
+
+struct rbdr_entry_t {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64   rsvd0:15;
+	u64   buf_addr:42;
+	u64   cache_align:7;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64   cache_align:7;
+	u64   buf_addr:42;
+	u64   rsvd0:15;
+#endif
+};
+
+/* TCP reassembly context */
+struct rbe_tcp_cnxt_t {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64   tcp_pkt_cnt:12;
+	u64   rsvd1:4;
+	u64   align_hdr_bytes:4;
+	u64   align_ptr_bytes:4;
+	u64   ptr_bytes:16;
+	u64   rsvd2:24;
+	u64   cqe_type:4;
+	u64   rsvd0:54;
+	u64   tcp_end_reason:2;
+	u64   tcp_status:4;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64   tcp_status:4;
+	u64   tcp_end_reason:2;
+	u64   rsvd0:54;
+	u64   cqe_type:4;
+	u64   rsvd2:24;
+	u64   ptr_bytes:16;
+	u64   align_ptr_bytes:4;
+	u64   align_hdr_bytes:4;
+	u64   rsvd1:4;
+	u64   tcp_pkt_cnt:12;
+#endif
+};
+
+/* Always Big endian */
+struct rx_hdr_t {
+	u64   opaque:32;
+	u64   rss_flow:8;
+	u64   skip_length:6;
+	u64   disable_rss:1;
+	u64   disable_tcp_reassembly:1;
+	u64   nodrop:1;
+	u64   dest_alg:2;
+	u64   rsvd0:2;
+	u64   dest_rq:11;
+};
+
+enum send_l4_csum_type {
+	SEND_L4_CSUM_DISABLE = 0x00,
+	SEND_L4_CSUM_UDP = 0x01,
+	SEND_L4_CSUM_TCP = 0x02,
+	SEND_L4_CSUM_SCTP = 0x03,
+};
+
+enum send_crc_alg {
+	SEND_CRCALG_CRC32 = 0x00,
+	SEND_CRCALG_CRC32C = 0x01,
+	SEND_CRCALG_ICRC = 0x02,
+};
+
+enum send_load_type {
+	SEND_LD_TYPE_LDD = 0x00,
+	SEND_LD_TYPE_LDT = 0x01,
+	SEND_LD_TYPE_LDWB = 0x02,
+};
+
+enum send_mem_alg_type {
+	SEND_MEMALG_SET = 0x00,
+	SEND_MEMALG_ADD = 0x08,
+	SEND_MEMALG_SUB = 0x09,
+	SEND_MEMALG_ADDLEN = 0x0A,
+	SEND_MEMALG_SUBLEN = 0x0B,
+};
+
+enum send_mem_dsz_type {
+	SEND_MEMDSZ_B64 = 0x00,
+	SEND_MEMDSZ_B32 = 0x01,
+	SEND_MEMDSZ_B8 = 0x03,
+};
+
+enum sq_subdesc_type {
+	SQ_DESC_TYPE_INVALID = 0x00,
+	SQ_DESC_TYPE_HEADER = 0x01,
+	SQ_DESC_TYPE_CRC = 0x02,
+	SQ_DESC_TYPE_IMMEDIATE = 0x03,
+	SQ_DESC_TYPE_GATHER = 0x04,
+	SQ_DESC_TYPE_MEMORY = 0x05,
+};
+
+struct sq_crc_subdesc {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64    rsvd1:32;
+	u64    crc_ival:32;
+	u64    subdesc_type:4;
+	u64    crc_alg:2;
+	u64    rsvd0:10;
+	u64    crc_insert_pos:16;
+	u64    hdr_start:16;
+	u64    crc_len:16;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64    crc_len:16;
+	u64    hdr_start:16;
+	u64    crc_insert_pos:16;
+	u64    rsvd0:10;
+	u64    crc_alg:2;
+	u64    subdesc_type:4;
+	u64    crc_ival:32;
+	u64    rsvd1:32;
+#endif
+};
+
+struct sq_gather_subdesc {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64    subdesc_type:4; /* W0 */
+	u64    ld_type:2;
+	u64    rsvd0:42;
+	u64    size:16;
+
+	u64    rsvd1:15; /* W1 */
+	u64    addr:49;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64    size:16;
+	u64    rsvd0:42;
+	u64    ld_type:2;
+	u64    subdesc_type:4; /* W0 */
+
+	u64    addr:49;
+	u64    rsvd1:15; /* W1 */
+#endif
+};
+
+/* SQ immediate subdescriptor */
+struct sq_imm_subdesc {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64    subdesc_type:4; /* W0 */
+	u64    rsvd0:46;
+	u64    len:14;
+
+	u64    data:64; /* W1 */
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64    len:14;
+	u64    rsvd0:46;
+	u64    subdesc_type:4; /* W0 */
+
+	u64    data:64; /* W1 */
+#endif
+};
+
+struct sq_mem_subdesc {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64    subdesc_type:4; /* W0 */
+	u64    mem_alg:4;
+	u64    mem_dsz:2;
+	u64    wmem:1;
+	u64    rsvd0:21;
+	u64    offset:32;
+
+	u64    rsvd1:15; /* W1 */
+	u64    addr:49;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64    offset:32;
+	u64    rsvd0:21;
+	u64    wmem:1;
+	u64    mem_dsz:2;
+	u64    mem_alg:4;
+	u64    subdesc_type:4; /* W0 */
+
+	u64    addr:49;
+	u64    rsvd1:15; /* W1 */
+#endif
+};
+
+struct sq_hdr_subdesc {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64    subdesc_type:4;
+	u64    tso:1;
+	u64    post_cqe:1; /* Post CQE on no error also */
+	u64    dont_send:1;
+	u64    tstmp:1;
+	u64    subdesc_cnt:8;
+	u64    csum_l4:2;
+	u64    csum_l3:1;
+	u64    rsvd0:5;
+	u64    l4_offset:8;
+	u64    l3_offset:8;
+	u64    rsvd1:4;
+	u64    tot_len:20; /* W0 */
+
+	u64    tso_sdc_cont:8;
+	u64    tso_sdc_first:8;
+	u64    tso_l4_offset:8;
+	u64    tso_flags_last:12;
+	u64    tso_flags_first:12;
+	u64    rsvd2:2;
+	u64    tso_max_paysize:14; /* W1 */
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64    tot_len:20;
+	u64    rsvd1:4;
+	u64    l3_offset:8;
+	u64    l4_offset:8;
+	u64    rsvd0:5;
+	u64    csum_l3:1;
+	u64    csum_l4:2;
+	u64    subdesc_cnt:8;
+	u64    tstmp:1;
+	u64    dont_send:1;
+	u64    post_cqe:1; /* Post CQE on no error also */
+	u64    tso:1;
+	u64    subdesc_type:4; /* W0 */
+
+	u64    tso_max_paysize:14;
+	u64    rsvd2:2;
+	u64    tso_flags_first:12;
+	u64    tso_flags_last:12;
+	u64    tso_l4_offset:8;
+	u64    tso_sdc_first:8;
+	u64    tso_sdc_cont:8; /* W1 */
+#endif
+};
+
+/* Queue config register formats */
+struct rq_cfg {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64 reserved_2_63:62;
+	u64 ena:1;
+	u64 tcp_ena:1;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64 tcp_ena:1;
+	u64 ena:1;
+	u64 reserved_2_63:62;
+#endif
+};
+
+struct cq_cfg {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64 reserved_43_63:21;
+	u64 ena:1;
+	u64 reset:1;
+	u64 caching:1;
+	u64 reserved_35_39:5;
+	u64 qsize:3;
+	u64 reserved_25_31:7;
+	u64 avg_con:9;
+	u64 reserved_0_15:16;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64 reserved_0_15:16;
+	u64 avg_con:9;
+	u64 reserved_25_31:7;
+	u64 qsize:3;
+	u64 reserved_35_39:5;
+	u64 caching:1;
+	u64 reset:1;
+	u64 ena:1;
+	u64 reserved_43_63:21;
+#endif
+};
+
+struct sq_cfg {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64 reserved_20_63:44;
+	u64 ena:1;
+	u64 reserved_18_18:1;
+	u64 reset:1;
+	u64 ldwb:1;
+	u64 reserved_11_15:5;
+	u64 qsize:3;
+	u64 reserved_3_7:5;
+	u64 tstmp_bgx_intf:3;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64 tstmp_bgx_intf:3;
+	u64 reserved_3_7:5;
+	u64 qsize:3;
+	u64 reserved_11_15:5;
+	u64 ldwb:1;
+	u64 reset:1;
+	u64 reserved_18_18:1;
+	u64 ena:1;
+	u64 reserved_20_63:44;
+#endif
+};
+
+struct rbdr_cfg {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64 reserved_45_63:19;
+	u64 ena:1;
+	u64 reset:1;
+	u64 ldwb:1;
+	u64 reserved_36_41:6;
+	u64 qsize:4;
+	u64 reserved_25_31:7;
+	u64 avg_con:9;
+	u64 reserved_12_15:4;
+	u64 lines:12;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64 lines:12;
+	u64 reserved_12_15:4;
+	u64 avg_con:9;
+	u64 reserved_25_31:7;
+	u64 qsize:4;
+	u64 reserved_36_41:6;
+	u64 ldwb:1;
+	u64 reset:1;
+	u64 ena: 1;
+	u64 reserved_45_63:19;
+#endif
+};
+
+struct qs_cfg {
+#if defined(__BIG_ENDIAN_BITFIELD)
+	u64 reserved_32_63:32;
+	u64 ena:1;
+	u64 reserved_27_30:4;
+	u64 sq_ins_ena:1;
+	u64 sq_ins_pos:6;
+	u64 lock_ena:1;
+	u64 lock_viol_cqe_ena:1;
+	u64 send_tstmp_ena:1;
+	u64 be:1;
+	u64 reserved_7_15:9;
+	u64 vnic:7;
+#elif defined(__LITTLE_ENDIAN_BITFIELD)
+	u64 vnic:7;
+	u64 reserved_7_15:9;
+	u64 be:1;
+	u64 send_tstmp_ena:1;
+	u64 lock_viol_cqe_ena:1;
+	u64 lock_ena:1;
+	u64 sq_ins_pos:6;
+	u64 sq_ins_ena:1;
+	u64 reserved_27_30:4;
+	u64 ena:1;
+	u64 reserved_32_63:32;
+#endif
+};
+
+#endif /* Q_STRUCT_H */
diff --git a/drivers/net/ethernet/cavium/thunder/thunder_bgx.c b/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
new file mode 100644
index 000000000000..633ec05dfe05
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
@@ -0,0 +1,966 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License
+ * as published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/phy.h>
+#include <linux/of.h>
+#include <linux/of_mdio.h>
+#include <linux/of_net.h>
+
+#include "nic_reg.h"
+#include "nic.h"
+#include "thunder_bgx.h"
+
+#define DRV_NAME	"thunder-BGX"
+#define DRV_VERSION	"1.0"
+
+struct lmac {
+	struct bgx		*bgx;
+	int			dmac;
+	unsigned char		mac[ETH_ALEN];
+	bool			link_up;
+	int			lmacid; /* ID within BGX */
+	int			lmacid_bd; /* ID on board */
+	struct net_device       netdev;
+	struct phy_device       *phydev;
+	unsigned int            last_duplex;
+	unsigned int            last_link;
+	unsigned int            last_speed;
+	bool			is_sgmii;
+	struct delayed_work	dwork;
+	struct workqueue_struct *check_link;
+};
+
+struct bgx {
+	u8			bgx_id;
+	u8			qlm_mode;
+	struct	lmac		lmac[MAX_LMAC_PER_BGX];
+	int			lmac_count;
+	int                     lmac_type;
+	int                     lane_to_sds;
+	int			use_training;
+	void __iomem		*reg_base;
+	struct pci_dev		*pdev;
+};
+
+static struct bgx *bgx_vnic[MAX_BGX_THUNDER];
+static int lmac_count; /* Total no of LMACs in system */
+
+static int bgx_xaui_check_link(struct lmac *lmac);
+
+/* Supported devices */
+static const struct pci_device_id bgx_id_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_CAVIUM, PCI_DEVICE_ID_THUNDER_BGX) },
+	{ 0, }  /* end of table */
+};
+
+MODULE_AUTHOR("Cavium Inc");
+MODULE_DESCRIPTION("Cavium Thunder BGX/MAC Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DRV_VERSION);
+MODULE_DEVICE_TABLE(pci, bgx_id_table);
+
+/* The Cavium ThunderX network controller can *only* be found in SoCs
+ * containing the ThunderX ARM64 CPU implementation.  All accesses to the device
+ * registers on this platform are implicitly strongly ordered with respect
+ * to memory accesses. So writeq_relaxed() and readq_relaxed() are safe to use
+ * with no memory barriers in this driver.  The readq()/writeq() functions add
+ * explicit ordering operation which in this case are redundant, and only
+ * add overhead.
+ */
+
+/* Register read/write APIs */
+static u64 bgx_reg_read(struct bgx *bgx, u8 lmac, u64 offset)
+{
+	void __iomem *addr = bgx->reg_base + ((u32)lmac << 20) + offset;
+
+	return readq_relaxed(addr);
+}
+
+static void bgx_reg_write(struct bgx *bgx, u8 lmac, u64 offset, u64 val)
+{
+	void __iomem *addr = bgx->reg_base + ((u32)lmac << 20) + offset;
+
+	writeq_relaxed(val, addr);
+}
+
+static void bgx_reg_modify(struct bgx *bgx, u8 lmac, u64 offset, u64 val)
+{
+	void __iomem *addr = bgx->reg_base + ((u32)lmac << 20) + offset;
+
+	writeq_relaxed(val | readq_relaxed(addr), addr);
+}
+
+static int bgx_poll_reg(struct bgx *bgx, u8 lmac, u64 reg, u64 mask, bool zero)
+{
+	int timeout = 100;
+	u64 reg_val;
+
+	while (timeout) {
+		reg_val = bgx_reg_read(bgx, lmac, reg);
+		if (zero && !(reg_val & mask))
+			return 0;
+		if (!zero && (reg_val & mask))
+			return 0;
+		usleep_range(1000, 2000);
+		timeout--;
+	}
+	return 1;
+}
+
+/* Return number of BGX present in HW */
+unsigned bgx_get_map(int node)
+{
+	int i;
+	unsigned map = 0;
+
+	for (i = 0; i < MAX_BGX_PER_CN88XX; i++) {
+		if (bgx_vnic[(node * MAX_BGX_PER_CN88XX) + i])
+			map |= (1 << i);
+	}
+
+	return map;
+}
+EXPORT_SYMBOL(bgx_get_map);
+
+/* Return number of LMAC configured for this BGX */
+int bgx_get_lmac_count(int node, int bgx_idx)
+{
+	struct bgx *bgx;
+
+	bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+	if (bgx)
+		return bgx->lmac_count;
+
+	return 0;
+}
+EXPORT_SYMBOL(bgx_get_lmac_count);
+
+/* Returns the current link status of LMAC */
+void bgx_get_lmac_link_state(int node, int bgx_idx, int lmacid, void *status)
+{
+	struct bgx_link_status *link = (struct bgx_link_status *)status;
+	struct bgx *bgx;
+	struct lmac *lmac;
+
+	bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+	if (!bgx)
+		return;
+
+	lmac = &bgx->lmac[lmacid];
+	link->link_up = lmac->link_up;
+	link->duplex = lmac->last_duplex;
+	link->speed = lmac->last_speed;
+}
+EXPORT_SYMBOL(bgx_get_lmac_link_state);
+
+const u8 *bgx_get_lmac_mac(int node, int bgx_idx, int lmacid)
+{
+	struct bgx *bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+
+	if (bgx)
+		return bgx->lmac[lmacid].mac;
+
+	return NULL;
+}
+EXPORT_SYMBOL(bgx_get_lmac_mac);
+
+void bgx_set_lmac_mac(int node, int bgx_idx, int lmacid, const u8 *mac)
+{
+	struct bgx *bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+
+	if (!bgx)
+		return;
+
+	ether_addr_copy(bgx->lmac[lmacid].mac, mac);
+}
+EXPORT_SYMBOL(bgx_set_lmac_mac);
+
+static void bgx_sgmii_change_link_state(struct lmac *lmac)
+{
+	struct bgx *bgx = lmac->bgx;
+	u64 cmr_cfg;
+	u64 port_cfg = 0;
+	u64 misc_ctl = 0;
+
+	cmr_cfg = bgx_reg_read(bgx, lmac->lmacid, BGX_CMRX_CFG);
+	cmr_cfg &= ~CMR_EN;
+	bgx_reg_write(bgx, lmac->lmacid, BGX_CMRX_CFG, cmr_cfg);
+
+	port_cfg = bgx_reg_read(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG);
+	misc_ctl = bgx_reg_read(bgx, lmac->lmacid, BGX_GMP_PCS_MISCX_CTL);
+
+	if (lmac->link_up) {
+		misc_ctl &= ~PCS_MISC_CTL_GMX_ENO;
+		port_cfg &= ~GMI_PORT_CFG_DUPLEX;
+		port_cfg |=  (lmac->last_duplex << 2);
+	} else {
+		misc_ctl |= PCS_MISC_CTL_GMX_ENO;
+	}
+
+	switch (lmac->last_speed) {
+	case 10:
+		port_cfg &= ~GMI_PORT_CFG_SPEED; /* speed 0 */
+		port_cfg |= GMI_PORT_CFG_SPEED_MSB;  /* speed_msb 1 */
+		port_cfg &= ~GMI_PORT_CFG_SLOT_TIME; /* slottime 0 */
+		misc_ctl &= ~PCS_MISC_CTL_SAMP_PT_MASK;
+		misc_ctl |= 50; /* samp_pt */
+		bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_GMI_TXX_SLOT, 64);
+		bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_GMI_TXX_BURST, 0);
+		break;
+	case 100:
+		port_cfg &= ~GMI_PORT_CFG_SPEED; /* speed 0 */
+		port_cfg &= ~GMI_PORT_CFG_SPEED_MSB; /* speed_msb 0 */
+		port_cfg &= ~GMI_PORT_CFG_SLOT_TIME; /* slottime 0 */
+		misc_ctl &= ~PCS_MISC_CTL_SAMP_PT_MASK;
+		misc_ctl |= 5; /* samp_pt */
+		bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_GMI_TXX_SLOT, 64);
+		bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_GMI_TXX_BURST, 0);
+		break;
+	case 1000:
+		port_cfg |= GMI_PORT_CFG_SPEED; /* speed 1 */
+		port_cfg &= ~GMI_PORT_CFG_SPEED_MSB; /* speed_msb 0 */
+		port_cfg |= GMI_PORT_CFG_SLOT_TIME; /* slottime 1 */
+		misc_ctl &= ~PCS_MISC_CTL_SAMP_PT_MASK;
+		misc_ctl |= 1; /* samp_pt */
+		bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_GMI_TXX_SLOT, 512);
+		if (lmac->last_duplex)
+			bgx_reg_write(bgx, lmac->lmacid,
+				      BGX_GMP_GMI_TXX_BURST, 0);
+		else
+			bgx_reg_write(bgx, lmac->lmacid,
+				      BGX_GMP_GMI_TXX_BURST, 8192);
+		break;
+	default:
+		break;
+	}
+	bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_PCS_MISCX_CTL, misc_ctl);
+	bgx_reg_write(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG, port_cfg);
+
+	port_cfg = bgx_reg_read(bgx, lmac->lmacid, BGX_GMP_GMI_PRTX_CFG);
+
+	/* renable lmac */
+	cmr_cfg |= CMR_EN;
+	bgx_reg_write(bgx, lmac->lmacid, BGX_CMRX_CFG, cmr_cfg);
+}
+
+static void bgx_lmac_handler(struct net_device *netdev)
+{
+	struct lmac *lmac = container_of(netdev, struct lmac, netdev);
+	struct phy_device *phydev = lmac->phydev;
+	int link_changed = 0;
+
+	if (!lmac)
+		return;
+
+	if (!phydev->link && lmac->last_link)
+		link_changed = -1;
+
+	if (phydev->link &&
+	    (lmac->last_duplex != phydev->duplex ||
+	     lmac->last_link != phydev->link ||
+	     lmac->last_speed != phydev->speed)) {
+			link_changed = 1;
+	}
+
+	lmac->last_link = phydev->link;
+	lmac->last_speed = phydev->speed;
+	lmac->last_duplex = phydev->duplex;
+
+	if (!link_changed)
+		return;
+
+	if (link_changed > 0)
+		lmac->link_up = true;
+	else
+		lmac->link_up = false;
+
+	if (lmac->is_sgmii)
+		bgx_sgmii_change_link_state(lmac);
+	else
+		bgx_xaui_check_link(lmac);
+}
+
+u64 bgx_get_rx_stats(int node, int bgx_idx, int lmac, int idx)
+{
+	struct bgx *bgx;
+
+	bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+	if (!bgx)
+		return 0;
+
+	if (idx > 8)
+		lmac = 0;
+	return bgx_reg_read(bgx, lmac, BGX_CMRX_RX_STAT0 + (idx * 8));
+}
+EXPORT_SYMBOL(bgx_get_rx_stats);
+
+u64 bgx_get_tx_stats(int node, int bgx_idx, int lmac, int idx)
+{
+	struct bgx *bgx;
+
+	bgx = bgx_vnic[(node * MAX_BGX_PER_CN88XX) + bgx_idx];
+	if (!bgx)
+		return 0;
+
+	return bgx_reg_read(bgx, lmac, BGX_CMRX_TX_STAT0 + (idx * 8));
+}
+EXPORT_SYMBOL(bgx_get_tx_stats);
+
+static void bgx_flush_dmac_addrs(struct bgx *bgx, int lmac)
+{
+	u64 offset;
+
+	while (bgx->lmac[lmac].dmac > 0) {
+		offset = ((bgx->lmac[lmac].dmac - 1) * sizeof(u64)) +
+			(lmac * MAX_DMAC_PER_LMAC * sizeof(u64));
+		bgx_reg_write(bgx, 0, BGX_CMR_RX_DMACX_CAM + offset, 0);
+		bgx->lmac[lmac].dmac--;
+	}
+}
+
+static int bgx_lmac_sgmii_init(struct bgx *bgx, int lmacid)
+{
+	u64 cfg;
+
+	bgx_reg_modify(bgx, lmacid, BGX_GMP_GMI_TXX_THRESH, 0x30);
+	/* max packet size */
+	bgx_reg_modify(bgx, lmacid, BGX_GMP_GMI_RXX_JABBER, MAX_FRAME_SIZE);
+
+	/* Disable frame alignment if using preamble */
+	cfg = bgx_reg_read(bgx, lmacid, BGX_GMP_GMI_TXX_APPEND);
+	if (cfg & 1)
+		bgx_reg_write(bgx, lmacid, BGX_GMP_GMI_TXX_SGMII_CTL, 0);
+
+	/* Enable lmac */
+	bgx_reg_modify(bgx, lmacid, BGX_CMRX_CFG, CMR_EN);
+
+	/* PCS reset */
+	bgx_reg_modify(bgx, lmacid, BGX_GMP_PCS_MRX_CTL, PCS_MRX_CTL_RESET);
+	if (bgx_poll_reg(bgx, lmacid, BGX_GMP_PCS_MRX_CTL,
+			 PCS_MRX_CTL_RESET, true)) {
+		dev_err(&bgx->pdev->dev, "BGX PCS reset not completed\n");
+		return -1;
+	}
+
+	/* power down, reset autoneg, autoneg enable */
+	cfg = bgx_reg_read(bgx, lmacid, BGX_GMP_PCS_MRX_CTL);
+	cfg &= ~PCS_MRX_CTL_PWR_DN;
+	cfg |= (PCS_MRX_CTL_RST_AN | PCS_MRX_CTL_AN_EN);
+	bgx_reg_write(bgx, lmacid, BGX_GMP_PCS_MRX_CTL, cfg);
+
+	if (bgx_poll_reg(bgx, lmacid, BGX_GMP_PCS_MRX_STATUS,
+			 PCS_MRX_STATUS_AN_CPT, false)) {
+		dev_err(&bgx->pdev->dev, "BGX AN_CPT not completed\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int bgx_lmac_xaui_init(struct bgx *bgx, int lmacid, int lmac_type)
+{
+	u64 cfg;
+
+	/* Reset SPU */
+	bgx_reg_modify(bgx, lmacid, BGX_SPUX_CONTROL1, SPU_CTL_RESET);
+	if (bgx_poll_reg(bgx, lmacid, BGX_SPUX_CONTROL1, SPU_CTL_RESET, true)) {
+		dev_err(&bgx->pdev->dev, "BGX SPU reset not completed\n");
+		return -1;
+	}
+
+	/* Disable LMAC */
+	cfg = bgx_reg_read(bgx, lmacid, BGX_CMRX_CFG);
+	cfg &= ~CMR_EN;
+	bgx_reg_write(bgx, lmacid, BGX_CMRX_CFG, cfg);
+
+	bgx_reg_modify(bgx, lmacid, BGX_SPUX_CONTROL1, SPU_CTL_LOW_POWER);
+	/* Set interleaved running disparity for RXAUI */
+	if (bgx->lmac_type != BGX_MODE_RXAUI)
+		bgx_reg_modify(bgx, lmacid,
+			       BGX_SPUX_MISC_CONTROL, SPU_MISC_CTL_RX_DIS);
+	else
+		bgx_reg_modify(bgx, lmacid, BGX_SPUX_MISC_CONTROL,
+			       SPU_MISC_CTL_RX_DIS | SPU_MISC_CTL_INTLV_RDISP);
+
+	/* clear all interrupts */
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SMUX_RX_INT);
+	bgx_reg_write(bgx, lmacid, BGX_SMUX_RX_INT, cfg);
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SMUX_TX_INT);
+	bgx_reg_write(bgx, lmacid, BGX_SMUX_TX_INT, cfg);
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_INT);
+	bgx_reg_write(bgx, lmacid, BGX_SPUX_INT, cfg);
+
+	if (bgx->use_training) {
+		bgx_reg_write(bgx, lmacid, BGX_SPUX_BR_PMD_LP_CUP, 0x00);
+		bgx_reg_write(bgx, lmacid, BGX_SPUX_BR_PMD_LD_CUP, 0x00);
+		bgx_reg_write(bgx, lmacid, BGX_SPUX_BR_PMD_LD_REP, 0x00);
+		/* training enable */
+		bgx_reg_modify(bgx, lmacid,
+			       BGX_SPUX_BR_PMD_CRTL, SPU_PMD_CRTL_TRAIN_EN);
+	}
+
+	/* Append FCS to each packet */
+	bgx_reg_modify(bgx, lmacid, BGX_SMUX_TX_APPEND, SMU_TX_APPEND_FCS_D);
+
+	/* Disable forward error correction */
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_FEC_CONTROL);
+	cfg &= ~SPU_FEC_CTL_FEC_EN;
+	bgx_reg_write(bgx, lmacid, BGX_SPUX_FEC_CONTROL, cfg);
+
+	/* Disable autoneg */
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_AN_CONTROL);
+	cfg = cfg & ~(SPU_AN_CTL_AN_EN | SPU_AN_CTL_XNP_EN);
+	bgx_reg_write(bgx, lmacid, BGX_SPUX_AN_CONTROL, cfg);
+
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_AN_ADV);
+	if (bgx->lmac_type == BGX_MODE_10G_KR)
+		cfg |= (1 << 23);
+	else if (bgx->lmac_type == BGX_MODE_40G_KR)
+		cfg |= (1 << 24);
+	else
+		cfg &= ~((1 << 23) | (1 << 24));
+	cfg = cfg & (~((1ULL << 25) | (1ULL << 22) | (1ULL << 12)));
+	bgx_reg_write(bgx, lmacid, BGX_SPUX_AN_ADV, cfg);
+
+	cfg = bgx_reg_read(bgx, 0, BGX_SPU_DBG_CONTROL);
+	cfg &= ~SPU_DBG_CTL_AN_ARB_LINK_CHK_EN;
+	bgx_reg_write(bgx, 0, BGX_SPU_DBG_CONTROL, cfg);
+
+	/* Enable lmac */
+	bgx_reg_modify(bgx, lmacid, BGX_CMRX_CFG, CMR_EN);
+
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_CONTROL1);
+	cfg &= ~SPU_CTL_LOW_POWER;
+	bgx_reg_write(bgx, lmacid, BGX_SPUX_CONTROL1, cfg);
+
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SMUX_TX_CTL);
+	cfg &= ~SMU_TX_CTL_UNI_EN;
+	cfg |= SMU_TX_CTL_DIC_EN;
+	bgx_reg_write(bgx, lmacid, BGX_SMUX_TX_CTL, cfg);
+
+	/* take lmac_count into account */
+	bgx_reg_modify(bgx, lmacid, BGX_SMUX_TX_THRESH, (0x100 - 1));
+	/* max packet size */
+	bgx_reg_modify(bgx, lmacid, BGX_SMUX_RX_JABBER, MAX_FRAME_SIZE);
+
+	return 0;
+}
+
+static int bgx_xaui_check_link(struct lmac *lmac)
+{
+	struct bgx *bgx = lmac->bgx;
+	int lmacid = lmac->lmacid;
+	int lmac_type = bgx->lmac_type;
+	u64 cfg;
+
+	bgx_reg_modify(bgx, lmacid, BGX_SPUX_MISC_CONTROL, SPU_MISC_CTL_RX_DIS);
+	if (bgx->use_training) {
+		cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_INT);
+		if (!(cfg & (1ull << 13))) {
+			cfg = (1ull << 13) | (1ull << 14);
+			bgx_reg_write(bgx, lmacid, BGX_SPUX_INT, cfg);
+			cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_BR_PMD_CRTL);
+			cfg |= (1ull << 0);
+			bgx_reg_write(bgx, lmacid, BGX_SPUX_BR_PMD_CRTL, cfg);
+			return -1;
+		}
+	}
+
+	/* wait for PCS to come out of reset */
+	if (bgx_poll_reg(bgx, lmacid, BGX_SPUX_CONTROL1, SPU_CTL_RESET, true)) {
+		dev_err(&bgx->pdev->dev, "BGX SPU reset not completed\n");
+		return -1;
+	}
+
+	if ((lmac_type == BGX_MODE_10G_KR) || (lmac_type == BGX_MODE_XFI) ||
+	    (lmac_type == BGX_MODE_40G_KR) || (lmac_type == BGX_MODE_XLAUI)) {
+		if (bgx_poll_reg(bgx, lmacid, BGX_SPUX_BR_STATUS1,
+				 SPU_BR_STATUS_BLK_LOCK, false)) {
+			dev_err(&bgx->pdev->dev,
+				"SPU_BR_STATUS_BLK_LOCK not completed\n");
+			return -1;
+		}
+	} else {
+		if (bgx_poll_reg(bgx, lmacid, BGX_SPUX_BX_STATUS,
+				 SPU_BX_STATUS_RX_ALIGN, false)) {
+			dev_err(&bgx->pdev->dev,
+				"SPU_BX_STATUS_RX_ALIGN not completed\n");
+			return -1;
+		}
+	}
+
+	/* Clear rcvflt bit (latching high) and read it back */
+	bgx_reg_modify(bgx, lmacid, BGX_SPUX_STATUS2, SPU_STATUS2_RCVFLT);
+	if (bgx_reg_read(bgx, lmacid, BGX_SPUX_STATUS2) & SPU_STATUS2_RCVFLT) {
+		dev_err(&bgx->pdev->dev, "Receive fault, retry training\n");
+		if (bgx->use_training) {
+			cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_INT);
+			if (!(cfg & (1ull << 13))) {
+				cfg = (1ull << 13) | (1ull << 14);
+				bgx_reg_write(bgx, lmacid, BGX_SPUX_INT, cfg);
+				cfg = bgx_reg_read(bgx, lmacid,
+						   BGX_SPUX_BR_PMD_CRTL);
+				cfg |= (1ull << 0);
+				bgx_reg_write(bgx, lmacid,
+					      BGX_SPUX_BR_PMD_CRTL, cfg);
+				return -1;
+			}
+		}
+		return -1;
+	}
+
+	/* Wait for MAC RX to be ready */
+	if (bgx_poll_reg(bgx, lmacid, BGX_SMUX_RX_CTL,
+			 SMU_RX_CTL_STATUS, true)) {
+		dev_err(&bgx->pdev->dev, "SMU RX link not okay\n");
+		return -1;
+	}
+
+	/* Wait for BGX RX to be idle */
+	if (bgx_poll_reg(bgx, lmacid, BGX_SMUX_CTL, SMU_CTL_RX_IDLE, false)) {
+		dev_err(&bgx->pdev->dev, "SMU RX not idle\n");
+		return -1;
+	}
+
+	/* Wait for BGX TX to be idle */
+	if (bgx_poll_reg(bgx, lmacid, BGX_SMUX_CTL, SMU_CTL_TX_IDLE, false)) {
+		dev_err(&bgx->pdev->dev, "SMU TX not idle\n");
+		return -1;
+	}
+
+	if (bgx_reg_read(bgx, lmacid, BGX_SPUX_STATUS2) & SPU_STATUS2_RCVFLT) {
+		dev_err(&bgx->pdev->dev, "Receive fault\n");
+		return -1;
+	}
+
+	/* Receive link is latching low. Force it high and verify it */
+	bgx_reg_modify(bgx, lmacid, BGX_SPUX_STATUS1, SPU_STATUS1_RCV_LNK);
+	if (bgx_poll_reg(bgx, lmacid, BGX_SPUX_STATUS1,
+			 SPU_STATUS1_RCV_LNK, false)) {
+		dev_err(&bgx->pdev->dev, "SPU receive link down\n");
+		return -1;
+	}
+
+	cfg = bgx_reg_read(bgx, lmacid, BGX_SPUX_MISC_CONTROL);
+	cfg &= ~SPU_MISC_CTL_RX_DIS;
+	bgx_reg_write(bgx, lmacid, BGX_SPUX_MISC_CONTROL, cfg);
+	return 0;
+}
+
+static void bgx_poll_for_link(struct work_struct *work)
+{
+	struct lmac *lmac;
+	u64 link;
+
+	lmac = container_of(work, struct lmac, dwork.work);
+
+	/* Receive link is latching low. Force it high and verify it */
+	bgx_reg_modify(lmac->bgx, lmac->lmacid,
+		       BGX_SPUX_STATUS1, SPU_STATUS1_RCV_LNK);
+	bgx_poll_reg(lmac->bgx, lmac->lmacid, BGX_SPUX_STATUS1,
+		     SPU_STATUS1_RCV_LNK, false);
+
+	link = bgx_reg_read(lmac->bgx, lmac->lmacid, BGX_SPUX_STATUS1);
+	if (link & SPU_STATUS1_RCV_LNK) {
+		lmac->link_up = 1;
+		if (lmac->bgx->lmac_type == BGX_MODE_XLAUI)
+			lmac->last_speed = 40000;
+		else
+			lmac->last_speed = 10000;
+		lmac->last_duplex = 1;
+	} else {
+		lmac->link_up = 0;
+	}
+
+	if (lmac->last_link != lmac->link_up) {
+		lmac->last_link = lmac->link_up;
+		if (lmac->link_up)
+			bgx_xaui_check_link(lmac);
+	}
+
+	queue_delayed_work(lmac->check_link, &lmac->dwork, HZ * 2);
+}
+
+static int bgx_lmac_enable(struct bgx *bgx, u8 lmacid)
+{
+	struct lmac *lmac;
+	u64 cfg;
+
+	lmac = &bgx->lmac[lmacid];
+	lmac->bgx = bgx;
+
+	if (bgx->lmac_type == BGX_MODE_SGMII) {
+		lmac->is_sgmii = 1;
+		if (bgx_lmac_sgmii_init(bgx, lmacid))
+			return -1;
+	} else {
+		lmac->is_sgmii = 0;
+		if (bgx_lmac_xaui_init(bgx, lmacid, bgx->lmac_type))
+			return -1;
+	}
+
+	if (lmac->is_sgmii) {
+		cfg = bgx_reg_read(bgx, lmacid, BGX_GMP_GMI_TXX_APPEND);
+		cfg |= ((1ull << 2) | (1ull << 1)); /* FCS and PAD */
+		bgx_reg_modify(bgx, lmacid, BGX_GMP_GMI_TXX_APPEND, cfg);
+		bgx_reg_write(bgx, lmacid, BGX_GMP_GMI_TXX_MIN_PKT, 60 - 1);
+	} else {
+		cfg = bgx_reg_read(bgx, lmacid, BGX_SMUX_TX_APPEND);
+		cfg |= ((1ull << 2) | (1ull << 1)); /* FCS and PAD */
+		bgx_reg_modify(bgx, lmacid, BGX_SMUX_TX_APPEND, cfg);
+		bgx_reg_write(bgx, lmacid, BGX_SMUX_TX_MIN_PKT, 60 + 4);
+	}
+
+	/* Enable lmac */
+	bgx_reg_modify(bgx, lmacid, BGX_CMRX_CFG,
+		       CMR_EN | CMR_PKT_RX_EN | CMR_PKT_TX_EN);
+
+	/* Restore default cfg, incase low level firmware changed it */
+	bgx_reg_write(bgx, lmacid, BGX_CMRX_RX_DMAC_CTL, 0x03);
+
+	if ((bgx->lmac_type != BGX_MODE_XFI) &&
+	    (bgx->lmac_type != BGX_MODE_XLAUI) &&
+	    (bgx->lmac_type != BGX_MODE_40G_KR) &&
+	    (bgx->lmac_type != BGX_MODE_10G_KR)) {
+		if (!lmac->phydev)
+			return -ENODEV;
+
+		lmac->phydev->dev_flags = 0;
+
+		if (phy_connect_direct(&lmac->netdev, lmac->phydev,
+				       bgx_lmac_handler,
+				       PHY_INTERFACE_MODE_SGMII))
+			return -ENODEV;
+
+		phy_start_aneg(lmac->phydev);
+	} else {
+		lmac->check_link = alloc_workqueue("check_link", WQ_UNBOUND |
+						   WQ_MEM_RECLAIM, 1);
+		if (!lmac->check_link)
+			return -ENOMEM;
+		INIT_DELAYED_WORK(&lmac->dwork, bgx_poll_for_link);
+		queue_delayed_work(lmac->check_link, &lmac->dwork, 0);
+	}
+
+	return 0;
+}
+
+static void bgx_lmac_disable(struct bgx *bgx, u8 lmacid)
+{
+	struct lmac *lmac;
+	u64 cmrx_cfg;
+
+	lmac = &bgx->lmac[lmacid];
+	if (lmac->check_link) {
+		/* Destroy work queue */
+		cancel_delayed_work(&lmac->dwork);
+		flush_workqueue(lmac->check_link);
+		destroy_workqueue(lmac->check_link);
+	}
+
+	cmrx_cfg = bgx_reg_read(bgx, lmacid, BGX_CMRX_CFG);
+	cmrx_cfg &= ~(1 << 15);
+	bgx_reg_write(bgx, lmacid, BGX_CMRX_CFG, cmrx_cfg);
+	bgx_flush_dmac_addrs(bgx, lmacid);
+
+	if (lmac->phydev)
+		phy_disconnect(lmac->phydev);
+
+	lmac->phydev = NULL;
+}
+
+static void bgx_set_num_ports(struct bgx *bgx)
+{
+	u64 lmac_count;
+
+	switch (bgx->qlm_mode) {
+	case QLM_MODE_SGMII:
+		bgx->lmac_count = 4;
+		bgx->lmac_type = BGX_MODE_SGMII;
+		bgx->lane_to_sds = 0;
+		break;
+	case QLM_MODE_XAUI_1X4:
+		bgx->lmac_count = 1;
+		bgx->lmac_type = BGX_MODE_XAUI;
+		bgx->lane_to_sds = 0xE4;
+			break;
+	case QLM_MODE_RXAUI_2X2:
+		bgx->lmac_count = 2;
+		bgx->lmac_type = BGX_MODE_RXAUI;
+		bgx->lane_to_sds = 0xE4;
+			break;
+	case QLM_MODE_XFI_4X1:
+		bgx->lmac_count = 4;
+		bgx->lmac_type = BGX_MODE_XFI;
+		bgx->lane_to_sds = 0;
+		break;
+	case QLM_MODE_XLAUI_1X4:
+		bgx->lmac_count = 1;
+		bgx->lmac_type = BGX_MODE_XLAUI;
+		bgx->lane_to_sds = 0xE4;
+		break;
+	case QLM_MODE_10G_KR_4X1:
+		bgx->lmac_count = 4;
+		bgx->lmac_type = BGX_MODE_10G_KR;
+		bgx->lane_to_sds = 0;
+		bgx->use_training = 1;
+		break;
+	case QLM_MODE_40G_KR4_1X4:
+		bgx->lmac_count = 1;
+		bgx->lmac_type = BGX_MODE_40G_KR;
+		bgx->lane_to_sds = 0xE4;
+		bgx->use_training = 1;
+		break;
+	default:
+		bgx->lmac_count = 0;
+		break;
+	}
+
+	/* Check if low level firmware has programmed LMAC count
+	 * based on board type, if yes consider that otherwise
+	 * the default static values
+	 */
+	lmac_count = bgx_reg_read(bgx, 0, BGX_CMR_RX_LMACS) & 0x7;
+	if (lmac_count != 4)
+		bgx->lmac_count = lmac_count;
+}
+
+static void bgx_init_hw(struct bgx *bgx)
+{
+	int i;
+
+	bgx_set_num_ports(bgx);
+
+	bgx_reg_modify(bgx, 0, BGX_CMR_GLOBAL_CFG, CMR_GLOBAL_CFG_FCS_STRIP);
+	if (bgx_reg_read(bgx, 0, BGX_CMR_BIST_STATUS))
+		dev_err(&bgx->pdev->dev, "BGX%d BIST failed\n", bgx->bgx_id);
+
+	/* Set lmac type and lane2serdes mapping */
+	for (i = 0; i < bgx->lmac_count; i++) {
+		if (bgx->lmac_type == BGX_MODE_RXAUI) {
+			if (i)
+				bgx->lane_to_sds = 0x0e;
+			else
+				bgx->lane_to_sds = 0x04;
+			bgx_reg_write(bgx, i, BGX_CMRX_CFG,
+				      (bgx->lmac_type << 8) | bgx->lane_to_sds);
+			continue;
+		}
+		bgx_reg_write(bgx, i, BGX_CMRX_CFG,
+			      (bgx->lmac_type << 8) | (bgx->lane_to_sds + i));
+		bgx->lmac[i].lmacid_bd = lmac_count;
+		lmac_count++;
+	}
+
+	bgx_reg_write(bgx, 0, BGX_CMR_TX_LMACS, bgx->lmac_count);
+	bgx_reg_write(bgx, 0, BGX_CMR_RX_LMACS, bgx->lmac_count);
+
+	/* Set the backpressure AND mask */
+	for (i = 0; i < bgx->lmac_count; i++)
+		bgx_reg_modify(bgx, 0, BGX_CMR_CHAN_MSK_AND,
+			       ((1ULL << MAX_BGX_CHANS_PER_LMAC) - 1) <<
+			       (i * MAX_BGX_CHANS_PER_LMAC));
+
+	/* Disable all MAC filtering */
+	for (i = 0; i < RX_DMAC_COUNT; i++)
+		bgx_reg_write(bgx, 0, BGX_CMR_RX_DMACX_CAM + (i * 8), 0x00);
+
+	/* Disable MAC steering (NCSI traffic) */
+	for (i = 0; i < RX_TRAFFIC_STEER_RULE_COUNT; i++)
+		bgx_reg_write(bgx, 0, BGX_CMR_RX_STREERING + (i * 8), 0x00);
+}
+
+static void bgx_get_qlm_mode(struct bgx *bgx)
+{
+	struct device *dev = &bgx->pdev->dev;
+	int lmac_type;
+	int train_en;
+
+	/* Read LMAC0 type to figure out QLM mode
+	 * This is configured by low level firmware
+	 */
+	lmac_type = bgx_reg_read(bgx, 0, BGX_CMRX_CFG);
+	lmac_type = (lmac_type >> 8) & 0x07;
+
+	train_en = bgx_reg_read(bgx, 0, BGX_SPUX_BR_PMD_CRTL) &
+				SPU_PMD_CRTL_TRAIN_EN;
+
+	switch (lmac_type) {
+	case BGX_MODE_SGMII:
+		bgx->qlm_mode = QLM_MODE_SGMII;
+		dev_info(dev, "BGX%d QLM mode: SGMII\n", bgx->bgx_id);
+		break;
+	case BGX_MODE_XAUI:
+		bgx->qlm_mode = QLM_MODE_XAUI_1X4;
+		dev_info(dev, "BGX%d QLM mode: XAUI\n", bgx->bgx_id);
+		break;
+	case BGX_MODE_RXAUI:
+		bgx->qlm_mode = QLM_MODE_RXAUI_2X2;
+		dev_info(dev, "BGX%d QLM mode: RXAUI\n", bgx->bgx_id);
+		break;
+	case BGX_MODE_XFI:
+		if (!train_en) {
+			bgx->qlm_mode = QLM_MODE_XFI_4X1;
+			dev_info(dev, "BGX%d QLM mode: XFI\n", bgx->bgx_id);
+		} else {
+			bgx->qlm_mode = QLM_MODE_10G_KR_4X1;
+			dev_info(dev, "BGX%d QLM mode: 10G_KR\n", bgx->bgx_id);
+		}
+		break;
+	case BGX_MODE_XLAUI:
+		if (!train_en) {
+			bgx->qlm_mode = QLM_MODE_XLAUI_1X4;
+			dev_info(dev, "BGX%d QLM mode: XLAUI\n", bgx->bgx_id);
+		} else {
+			bgx->qlm_mode = QLM_MODE_40G_KR4_1X4;
+			dev_info(dev, "BGX%d QLM mode: 40G_KR4\n", bgx->bgx_id);
+		}
+		break;
+	default:
+		bgx->qlm_mode = QLM_MODE_SGMII;
+		dev_info(dev, "BGX%d QLM default mode: SGMII\n", bgx->bgx_id);
+	}
+}
+
+static void bgx_init_of(struct bgx *bgx, struct device_node *np)
+{
+	struct device_node *np_child;
+	u8 lmac = 0;
+
+	for_each_child_of_node(np, np_child) {
+		struct device_node *phy_np;
+		const char *mac;
+
+		phy_np = of_parse_phandle(np_child, "phy-handle", 0);
+		if (phy_np)
+			bgx->lmac[lmac].phydev = of_phy_find_device(phy_np);
+
+		mac = of_get_mac_address(np_child);
+		if (mac)
+			ether_addr_copy(bgx->lmac[lmac].mac, mac);
+
+		SET_NETDEV_DEV(&bgx->lmac[lmac].netdev, &bgx->pdev->dev);
+		bgx->lmac[lmac].lmacid = lmac;
+		lmac++;
+		if (lmac == MAX_LMAC_PER_BGX)
+			break;
+	}
+}
+
+static int bgx_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	int err;
+	struct device *dev = &pdev->dev;
+	struct bgx *bgx = NULL;
+	struct device_node *np;
+	char bgx_sel[5];
+	u8 lmac;
+
+	bgx = devm_kzalloc(dev, sizeof(*bgx), GFP_KERNEL);
+	if (!bgx)
+		return -ENOMEM;
+	bgx->pdev = pdev;
+
+	pci_set_drvdata(pdev, bgx);
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		dev_err(dev, "Failed to enable PCI device\n");
+		pci_set_drvdata(pdev, NULL);
+		return err;
+	}
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err) {
+		dev_err(dev, "PCI request regions failed 0x%x\n", err);
+		goto err_disable_device;
+	}
+
+	/* MAP configuration registers */
+	bgx->reg_base = pcim_iomap(pdev, PCI_CFG_REG_BAR_NUM, 0);
+	if (!bgx->reg_base) {
+		dev_err(dev, "BGX: Cannot map CSR memory space, aborting\n");
+		err = -ENOMEM;
+		goto err_release_regions;
+	}
+	bgx->bgx_id = (pci_resource_start(pdev, PCI_CFG_REG_BAR_NUM) >> 24) & 1;
+	bgx->bgx_id += nic_get_node_id(pdev) * MAX_BGX_PER_CN88XX;
+
+	bgx_vnic[bgx->bgx_id] = bgx;
+	bgx_get_qlm_mode(bgx);
+
+	snprintf(bgx_sel, 5, "bgx%d", bgx->bgx_id);
+	np = of_find_node_by_name(NULL, bgx_sel);
+	if (np)
+		bgx_init_of(bgx, np);
+
+	bgx_init_hw(bgx);
+
+	/* Enable all LMACs */
+	for (lmac = 0; lmac < bgx->lmac_count; lmac++) {
+		err = bgx_lmac_enable(bgx, lmac);
+		if (err) {
+			dev_err(dev, "BGX%d failed to enable lmac%d\n",
+				bgx->bgx_id, lmac);
+			goto err_enable;
+		}
+	}
+
+	return 0;
+
+err_enable:
+	bgx_vnic[bgx->bgx_id] = NULL;
+err_release_regions:
+	pci_release_regions(pdev);
+err_disable_device:
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+	return err;
+}
+
+static void bgx_remove(struct pci_dev *pdev)
+{
+	struct bgx *bgx = pci_get_drvdata(pdev);
+	u8 lmac;
+
+	/* Disable all LMACs */
+	for (lmac = 0; lmac < bgx->lmac_count; lmac++)
+		bgx_lmac_disable(bgx, lmac);
+
+	bgx_vnic[bgx->bgx_id] = NULL;
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+}
+
+static struct pci_driver bgx_driver = {
+	.name = DRV_NAME,
+	.id_table = bgx_id_table,
+	.probe = bgx_probe,
+	.remove = bgx_remove,
+};
+
+static int __init bgx_init_module(void)
+{
+	pr_info("%s, ver %s\n", DRV_NAME, DRV_VERSION);
+
+	return pci_register_driver(&bgx_driver);
+}
+
+static void __exit bgx_cleanup_module(void)
+{
+	pci_unregister_driver(&bgx_driver);
+}
+
+module_init(bgx_init_module);
+module_exit(bgx_cleanup_module);
diff --git a/drivers/net/ethernet/cavium/thunder/thunder_bgx.h b/drivers/net/ethernet/cavium/thunder/thunder_bgx.h
new file mode 100644
index 000000000000..ba4f53b7cc2c
--- /dev/null
+++ b/drivers/net/ethernet/cavium/thunder/thunder_bgx.h
@@ -0,0 +1,220 @@
+/*
+ * Copyright (C) 2015 Cavium, Inc.
+ *
+ * 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.
+ */
+
+#ifndef THUNDER_BGX_H
+#define THUNDER_BGX_H
+
+#define    MAX_BGX_THUNDER			8 /* Max 4 nodes, 2 per node */
+#define    MAX_BGX_PER_CN88XX			2
+#define    MAX_LMAC_PER_BGX			4
+#define    MAX_BGX_CHANS_PER_LMAC		16
+#define    MAX_DMAC_PER_LMAC			8
+#define    MAX_FRAME_SIZE			9216
+
+#define    MAX_DMAC_PER_LMAC_TNS_BYPASS_MODE	2
+
+#define    MAX_LMAC	(MAX_BGX_PER_CN88XX * MAX_LMAC_PER_BGX)
+
+/* Registers */
+#define BGX_CMRX_CFG			0x00
+#define  CMR_PKT_TX_EN				BIT_ULL(13)
+#define  CMR_PKT_RX_EN				BIT_ULL(14)
+#define  CMR_EN					BIT_ULL(15)
+#define BGX_CMR_GLOBAL_CFG		0x08
+#define  CMR_GLOBAL_CFG_FCS_STRIP		BIT_ULL(6)
+#define BGX_CMRX_RX_ID_MAP		0x60
+#define BGX_CMRX_RX_STAT0		0x70
+#define BGX_CMRX_RX_STAT1		0x78
+#define BGX_CMRX_RX_STAT2		0x80
+#define BGX_CMRX_RX_STAT3		0x88
+#define BGX_CMRX_RX_STAT4		0x90
+#define BGX_CMRX_RX_STAT5		0x98
+#define BGX_CMRX_RX_STAT6		0xA0
+#define BGX_CMRX_RX_STAT7		0xA8
+#define BGX_CMRX_RX_STAT8		0xB0
+#define BGX_CMRX_RX_STAT9		0xB8
+#define BGX_CMRX_RX_STAT10		0xC0
+#define BGX_CMRX_RX_BP_DROP		0xC8
+#define BGX_CMRX_RX_DMAC_CTL		0x0E8
+#define BGX_CMR_RX_DMACX_CAM		0x200
+#define  RX_DMACX_CAM_EN			BIT_ULL(48)
+#define  RX_DMACX_CAM_LMACID(x)			(x << 49)
+#define  RX_DMAC_COUNT				32
+#define BGX_CMR_RX_STREERING		0x300
+#define  RX_TRAFFIC_STEER_RULE_COUNT		8
+#define BGX_CMR_CHAN_MSK_AND		0x450
+#define BGX_CMR_BIST_STATUS		0x460
+#define BGX_CMR_RX_LMACS		0x468
+#define BGX_CMRX_TX_STAT0		0x600
+#define BGX_CMRX_TX_STAT1		0x608
+#define BGX_CMRX_TX_STAT2		0x610
+#define BGX_CMRX_TX_STAT3		0x618
+#define BGX_CMRX_TX_STAT4		0x620
+#define BGX_CMRX_TX_STAT5		0x628
+#define BGX_CMRX_TX_STAT6		0x630
+#define BGX_CMRX_TX_STAT7		0x638
+#define BGX_CMRX_TX_STAT8		0x640
+#define BGX_CMRX_TX_STAT9		0x648
+#define BGX_CMRX_TX_STAT10		0x650
+#define BGX_CMRX_TX_STAT11		0x658
+#define BGX_CMRX_TX_STAT12		0x660
+#define BGX_CMRX_TX_STAT13		0x668
+#define BGX_CMRX_TX_STAT14		0x670
+#define BGX_CMRX_TX_STAT15		0x678
+#define BGX_CMRX_TX_STAT16		0x680
+#define BGX_CMRX_TX_STAT17		0x688
+#define BGX_CMR_TX_LMACS		0x1000
+
+#define BGX_SPUX_CONTROL1		0x10000
+#define  SPU_CTL_LOW_POWER			BIT_ULL(11)
+#define  SPU_CTL_RESET				BIT_ULL(15)
+#define BGX_SPUX_STATUS1		0x10008
+#define  SPU_STATUS1_RCV_LNK			BIT_ULL(2)
+#define BGX_SPUX_STATUS2		0x10020
+#define  SPU_STATUS2_RCVFLT			BIT_ULL(10)
+#define BGX_SPUX_BX_STATUS		0x10028
+#define  SPU_BX_STATUS_RX_ALIGN			BIT_ULL(12)
+#define BGX_SPUX_BR_STATUS1		0x10030
+#define  SPU_BR_STATUS_BLK_LOCK			BIT_ULL(0)
+#define  SPU_BR_STATUS_RCV_LNK			BIT_ULL(12)
+#define BGX_SPUX_BR_PMD_CRTL		0x10068
+#define  SPU_PMD_CRTL_TRAIN_EN			BIT_ULL(1)
+#define BGX_SPUX_BR_PMD_LP_CUP		0x10078
+#define BGX_SPUX_BR_PMD_LD_CUP		0x10088
+#define BGX_SPUX_BR_PMD_LD_REP		0x10090
+#define BGX_SPUX_FEC_CONTROL		0x100A0
+#define  SPU_FEC_CTL_FEC_EN			BIT_ULL(0)
+#define  SPU_FEC_CTL_ERR_EN			BIT_ULL(1)
+#define BGX_SPUX_AN_CONTROL		0x100C8
+#define  SPU_AN_CTL_AN_EN			BIT_ULL(12)
+#define  SPU_AN_CTL_XNP_EN			BIT_ULL(13)
+#define BGX_SPUX_AN_ADV			0x100D8
+#define BGX_SPUX_MISC_CONTROL		0x10218
+#define  SPU_MISC_CTL_INTLV_RDISP		BIT_ULL(10)
+#define  SPU_MISC_CTL_RX_DIS			BIT_ULL(12)
+#define BGX_SPUX_INT			0x10220	/* +(0..3) << 20 */
+#define BGX_SPUX_INT_W1S		0x10228
+#define BGX_SPUX_INT_ENA_W1C		0x10230
+#define BGX_SPUX_INT_ENA_W1S		0x10238
+#define BGX_SPU_DBG_CONTROL		0x10300
+#define  SPU_DBG_CTL_AN_ARB_LINK_CHK_EN		BIT_ULL(18)
+#define  SPU_DBG_CTL_AN_NONCE_MCT_DIS		BIT_ULL(29)
+
+#define BGX_SMUX_RX_INT			0x20000
+#define BGX_SMUX_RX_JABBER		0x20030
+#define BGX_SMUX_RX_CTL			0x20048
+#define  SMU_RX_CTL_STATUS			(3ull << 0)
+#define BGX_SMUX_TX_APPEND		0x20100
+#define  SMU_TX_APPEND_FCS_D			BIT_ULL(2)
+#define BGX_SMUX_TX_MIN_PKT		0x20118
+#define BGX_SMUX_TX_INT			0x20140
+#define BGX_SMUX_TX_CTL			0x20178
+#define  SMU_TX_CTL_DIC_EN			BIT_ULL(0)
+#define  SMU_TX_CTL_UNI_EN			BIT_ULL(1)
+#define  SMU_TX_CTL_LNK_STATUS			(3ull << 4)
+#define BGX_SMUX_TX_THRESH		0x20180
+#define BGX_SMUX_CTL			0x20200
+#define  SMU_CTL_RX_IDLE			BIT_ULL(0)
+#define  SMU_CTL_TX_IDLE			BIT_ULL(1)
+
+#define BGX_GMP_PCS_MRX_CTL		0x30000
+#define	 PCS_MRX_CTL_RST_AN			BIT_ULL(9)
+#define	 PCS_MRX_CTL_PWR_DN			BIT_ULL(11)
+#define	 PCS_MRX_CTL_AN_EN			BIT_ULL(12)
+#define	 PCS_MRX_CTL_RESET			BIT_ULL(15)
+#define BGX_GMP_PCS_MRX_STATUS		0x30008
+#define	 PCS_MRX_STATUS_AN_CPT			BIT_ULL(5)
+#define BGX_GMP_PCS_ANX_AN_RESULTS	0x30020
+#define BGX_GMP_PCS_SGM_AN_ADV		0x30068
+#define BGX_GMP_PCS_MISCX_CTL		0x30078
+#define  PCS_MISC_CTL_GMX_ENO			BIT_ULL(11)
+#define  PCS_MISC_CTL_SAMP_PT_MASK	0x7Full
+#define BGX_GMP_GMI_PRTX_CFG		0x38020
+#define  GMI_PORT_CFG_SPEED			BIT_ULL(1)
+#define  GMI_PORT_CFG_DUPLEX			BIT_ULL(2)
+#define  GMI_PORT_CFG_SLOT_TIME			BIT_ULL(3)
+#define  GMI_PORT_CFG_SPEED_MSB			BIT_ULL(8)
+#define BGX_GMP_GMI_RXX_JABBER		0x38038
+#define BGX_GMP_GMI_TXX_THRESH		0x38210
+#define BGX_GMP_GMI_TXX_APPEND		0x38218
+#define BGX_GMP_GMI_TXX_SLOT		0x38220
+#define BGX_GMP_GMI_TXX_BURST		0x38228
+#define BGX_GMP_GMI_TXX_MIN_PKT		0x38240
+#define BGX_GMP_GMI_TXX_SGMII_CTL	0x38300
+
+#define BGX_MSIX_VEC_0_29_ADDR		0x400000 /* +(0..29) << 4 */
+#define BGX_MSIX_VEC_0_29_CTL		0x400008
+#define BGX_MSIX_PBA_0			0x4F0000
+
+/* MSI-X interrupts */
+#define BGX_MSIX_VECTORS	30
+#define BGX_LMAC_VEC_OFFSET	7
+#define BGX_MSIX_VEC_SHIFT	4
+
+#define CMRX_INT		0
+#define SPUX_INT		1
+#define SMUX_RX_INT		2
+#define SMUX_TX_INT		3
+#define GMPX_PCS_INT		4
+#define GMPX_GMI_RX_INT		5
+#define GMPX_GMI_TX_INT		6
+#define CMR_MEM_INT		28
+#define SPU_MEM_INT		29
+
+#define LMAC_INTR_LINK_UP	BIT(0)
+#define LMAC_INTR_LINK_DOWN	BIT(1)
+
+/*  RX_DMAC_CTL configuration*/
+enum MCAST_MODE {
+		MCAST_MODE_REJECT,
+		MCAST_MODE_ACCEPT,
+		MCAST_MODE_CAM_FILTER,
+		RSVD
+};
+
+#define BCAST_ACCEPT	1
+#define CAM_ACCEPT	1
+
+void bgx_add_dmac_addr(u64 dmac, int node, int bgx_idx, int lmac);
+unsigned bgx_get_map(int node);
+int bgx_get_lmac_count(int node, int bgx);
+const u8 *bgx_get_lmac_mac(int node, int bgx_idx, int lmacid);
+void bgx_set_lmac_mac(int node, int bgx_idx, int lmacid, const u8 *mac);
+void bgx_get_lmac_link_state(int node, int bgx_idx, int lmacid, void *status);
+u64 bgx_get_rx_stats(int node, int bgx_idx, int lmac, int idx);
+u64 bgx_get_tx_stats(int node, int bgx_idx, int lmac, int idx);
+#define BGX_RX_STATS_COUNT 11
+#define BGX_TX_STATS_COUNT 18
+
+struct bgx_stats {
+	u64 rx_stats[BGX_RX_STATS_COUNT];
+	u64 tx_stats[BGX_TX_STATS_COUNT];
+};
+
+enum LMAC_TYPE {
+	BGX_MODE_SGMII = 0, /* 1 lane, 1.250 Gbaud */
+	BGX_MODE_XAUI = 1,  /* 4 lanes, 3.125 Gbaud */
+	BGX_MODE_DXAUI = 1, /* 4 lanes, 6.250 Gbaud */
+	BGX_MODE_RXAUI = 2, /* 2 lanes, 6.250 Gbaud */
+	BGX_MODE_XFI = 3,   /* 1 lane, 10.3125 Gbaud */
+	BGX_MODE_XLAUI = 4, /* 4 lanes, 10.3125 Gbaud */
+	BGX_MODE_10G_KR = 3,/* 1 lane, 10.3125 Gbaud */
+	BGX_MODE_40G_KR = 4,/* 4 lanes, 10.3125 Gbaud */
+};
+
+enum qlm_mode {
+	QLM_MODE_SGMII,         /* SGMII, each lane independent */
+	QLM_MODE_XAUI_1X4,      /* 1 XAUI or DXAUI, 4 lanes */
+	QLM_MODE_RXAUI_2X2,     /* 2 RXAUI, 2 lanes each */
+	QLM_MODE_XFI_4X1,       /* 4 XFI, 1 lane each */
+	QLM_MODE_XLAUI_1X4,     /* 1 XLAUI, 4 lanes each */
+	QLM_MODE_10G_KR_4X1,    /* 4 10GBASE-KR, 1 lane each */
+	QLM_MODE_40G_KR4_1X4,   /* 1 40GBASE-KR4, 4 lanes each */
+};
+
+#endif /* THUNDER_BGX_H */
diff --git a/drivers/net/ethernet/chelsio/Kconfig b/drivers/net/ethernet/chelsio/Kconfig
index 7daa088a9bb7..a79813a17b6e 100644
--- a/drivers/net/ethernet/chelsio/Kconfig
+++ b/drivers/net/ethernet/chelsio/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_CHELSIO
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
index b96e4bfcac41..8f7aa53a4c4b 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
@@ -1025,19 +1025,19 @@ int t3_get_edc_fw(struct cphy *phy, int edc_idx, int size)
 {
 	struct adapter *adapter = phy->adapter;
 	const struct firmware *fw;
-	char buf[64];
+	const char *fw_name;
 	u32 csum;
 	const __be32 *p;
 	u16 *cache = phy->phy_cache;
-	int i, ret;
-
-	snprintf(buf, sizeof(buf), get_edc_fw_name(edc_idx));
+	int i, ret = -EINVAL;
 
-	ret = request_firmware(&fw, buf, &adapter->pdev->dev);
+	fw_name = get_edc_fw_name(edc_idx);
+	if (fw_name)
+		ret = request_firmware(&fw, fw_name, &adapter->pdev->dev);
 	if (ret < 0) {
 		dev_err(&adapter->pdev->dev,
 			"could not upgrade firmware: unable to load %s\n",
-			buf);
+			fw_name);
 		return ret;
 	}
 
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
index b0cbb2b7fd48..76684dcb874c 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_offload.c
@@ -1169,10 +1169,7 @@ void *cxgb_alloc_mem(unsigned long size)
  */
 void cxgb_free_mem(void *addr)
 {
-	if (is_vmalloc_addr(addr))
-		vfree(addr);
-	else
-		kfree(addr);
+	kvfree(addr);
 }
 
 /*
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index 524d11098c56..629f75d70353 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -46,17 +46,19 @@
 #include <linux/spinlock.h>
 #include <linux/timer.h>
 #include <linux/vmalloc.h>
+#include <linux/etherdevice.h>
 #include <asm/io.h>
 #include "cxgb4_uld.h"
 
 #define CH_WARN(adap, fmt, ...) dev_warn(adap->pdev_dev, fmt, ## __VA_ARGS__)
 
 enum {
-	MAX_NPORTS = 4,     /* max # of ports */
-	SERNUM_LEN = 24,    /* Serial # length */
-	EC_LEN     = 16,    /* E/C length */
-	ID_LEN     = 16,    /* ID length */
-	PN_LEN     = 16,    /* Part Number length */
+	MAX_NPORTS	= 4,     /* max # of ports */
+	SERNUM_LEN	= 24,    /* Serial # length */
+	EC_LEN		= 16,    /* E/C length */
+	ID_LEN		= 16,    /* ID length */
+	PN_LEN		= 16,    /* Part Number length */
+	MACADDR_LEN	= 12,    /* MAC Address length */
 };
 
 enum {
@@ -198,23 +200,45 @@ struct lb_port_stats {
 };
 
 struct tp_tcp_stats {
-	u32 tcpOutRsts;
-	u64 tcpInSegs;
-	u64 tcpOutSegs;
-	u64 tcpRetransSegs;
+	u32 tcp_out_rsts;
+	u64 tcp_in_segs;
+	u64 tcp_out_segs;
+	u64 tcp_retrans_segs;
+};
+
+struct tp_usm_stats {
+	u32 frames;
+	u32 drops;
+	u64 octets;
+};
+
+struct tp_fcoe_stats {
+	u32 frames_ddp;
+	u32 frames_drop;
+	u64 octets_ddp;
 };
 
 struct tp_err_stats {
-	u32 macInErrs[4];
-	u32 hdrInErrs[4];
-	u32 tcpInErrs[4];
-	u32 tnlCongDrops[4];
-	u32 ofldChanDrops[4];
-	u32 tnlTxDrops[4];
-	u32 ofldVlanDrops[4];
-	u32 tcp6InErrs[4];
-	u32 ofldNoNeigh;
-	u32 ofldCongDefer;
+	u32 mac_in_errs[4];
+	u32 hdr_in_errs[4];
+	u32 tcp_in_errs[4];
+	u32 tnl_cong_drops[4];
+	u32 ofld_chan_drops[4];
+	u32 tnl_tx_drops[4];
+	u32 ofld_vlan_drops[4];
+	u32 tcp6_in_errs[4];
+	u32 ofld_no_neigh;
+	u32 ofld_cong_defer;
+};
+
+struct tp_cpl_stats {
+	u32 req[4];
+	u32 rsp[4];
+};
+
+struct tp_rdma_stats {
+	u32 rqe_dfr_pkt;
+	u32 rqe_dfr_mod;
 };
 
 struct sge_params {
@@ -224,7 +248,6 @@ struct sge_params {
 };
 
 struct tp_params {
-	unsigned int ntxchan;        /* # of Tx channels */
 	unsigned int tre;            /* log2 of core clocks per TP tick */
 	unsigned int la_mask;        /* what events are recorded by TP LA */
 	unsigned short tx_modq_map;  /* TX modulation scheduler queue to */
@@ -259,6 +282,7 @@ struct vpd_params {
 	u8 sn[SERNUM_LEN + 1];
 	u8 id[ID_LEN + 1];
 	u8 pn[PN_LEN + 1];
+	u8 na[MACADDR_LEN + 1];
 };
 
 struct pci_params {
@@ -273,6 +297,7 @@ struct pci_params {
 
 #define CHELSIO_T4		0x4
 #define CHELSIO_T5		0x5
+#define CHELSIO_T6		0x6
 
 enum chip_type {
 	T4_A1 = CHELSIO_CHIP_CODE(CHELSIO_T4, 1),
@@ -284,6 +309,10 @@ enum chip_type {
 	T5_A1 = CHELSIO_CHIP_CODE(CHELSIO_T5, 1),
 	T5_FIRST_REV	= T5_A0,
 	T5_LAST_REV	= T5_A1,
+
+	T6_A0 = CHELSIO_CHIP_CODE(CHELSIO_T6, 0),
+	T6_FIRST_REV    = T6_A0,
+	T6_LAST_REV     = T6_A0,
 };
 
 struct devlog_params {
@@ -292,6 +321,15 @@ struct devlog_params {
 	u32 size;                       /* size of log */
 };
 
+/* Stores chip specific parameters */
+struct arch_specific_params {
+	u8 nchan;
+	u16 mps_rplc_size;
+	u16 vfcount;
+	u32 sge_fl_db;
+	u16 mps_tcam_size;
+};
+
 struct adapter_params {
 	struct sge_params sge;
 	struct tp_params  tp;
@@ -317,6 +355,7 @@ struct adapter_params {
 	unsigned char nports;             /* # of ethernet ports */
 	unsigned char portvec;
 	enum chip_type chip;               /* chip code */
+	struct arch_specific_params arch;  /* chip specific params */
 	unsigned char offload;
 
 	unsigned char bypass;
@@ -328,6 +367,17 @@ struct adapter_params {
 	unsigned int max_ird_adapter;     /* Max read depth per adapter */
 };
 
+/* State needed to monitor the forward progress of SGE Ingress DMA activities
+ * and possible hangs.
+ */
+struct sge_idma_monitor_state {
+	unsigned int idma_1s_thresh;	/* 1s threshold in Core Clock ticks */
+	unsigned int idma_stalled[2];	/* synthesized stalled timers in HZ */
+	unsigned int idma_state[2];	/* IDMA Hang detect state */
+	unsigned int idma_qid[2];	/* IDMA Hung Ingress Queue ID */
+	unsigned int idma_warn[2];	/* time to warning in HZ */
+};
+
 #include "t4fw_api.h"
 
 #define FW_VERSION(chip) ( \
@@ -421,6 +471,7 @@ struct port_info {
 	u8     rss_mode;
 	struct link_config link_cfg;
 	u16   *rss;
+	struct port_stats stats_base;
 #ifdef CONFIG_CHELSIO_T4_DCB
 	struct port_dcb_info dcb;     /* Data Center Bridging support */
 #endif
@@ -630,12 +681,7 @@ struct sge {
 	u32 fl_align;               /* response queue message alignment */
 	u32 fl_starve_thres;        /* Free List starvation threshold */
 
-	/* State variables for detecting an SGE Ingress DMA hang */
-	unsigned int idma_1s_thresh;/* SGE same State Counter 1s threshold */
-	unsigned int idma_stalled[2];/* SGE synthesized stalled timers in HZ */
-	unsigned int idma_state[2]; /* SGE IDMA Hang detect state */
-	unsigned int idma_qid[2];   /* SGE IDMA Hung Ingress Queue ID */
-
+	struct sge_idma_monitor_state idma_monitor;
 	unsigned int egr_start;
 	unsigned int egr_sz;
 	unsigned int ingr_start;
@@ -644,6 +690,7 @@ struct sge {
 	struct sge_rspq **ingr_map; /* qid->queue ingress queue map */
 	unsigned long *starving_fl;
 	unsigned long *txq_maperr;
+	unsigned long *blocked_fl;
 	struct timer_list rx_timer; /* refills starving FLs */
 	struct timer_list tx_timer; /* checks Tx queues */
 };
@@ -665,6 +712,12 @@ struct l2t_data;
 
 #endif
 
+struct doorbell_stats {
+	u32 db_drop;
+	u32 db_empty;
+	u32 db_full;
+};
+
 struct adapter {
 	void __iomem *regs;
 	void __iomem *bar2;
@@ -672,7 +725,7 @@ struct adapter {
 	struct pci_dev *pdev;
 	struct device *pdev_dev;
 	unsigned int mbox;
-	unsigned int fn;
+	unsigned int pf;
 	unsigned int flags;
 	enum chip_type chip;
 
@@ -682,13 +735,12 @@ struct adapter {
 	struct cxgb4_virt_res vres;
 	unsigned int swintr;
 
-	unsigned int wol;
-
 	struct {
 		unsigned short vec;
 		char desc[IFNAMSIZ + 10];
 	} msix_info[MAX_INGQ + 1];
 
+	struct doorbell_stats db_stats;
 	struct sge sge;
 
 	struct net_device *port[MAX_NPORTS];
@@ -843,6 +895,16 @@ enum {
 	VLAN_REWRITE
 };
 
+static inline int is_offload(const struct adapter *adap)
+{
+	return adap->params.offload;
+}
+
+static inline int is_t6(enum chip_type chip)
+{
+	return CHELSIO_CHIP_VERSION(chip) == CHELSIO_T6;
+}
+
 static inline int is_t5(enum chip_type chip)
 {
 	return CHELSIO_CHIP_VERSION(chip) == CHELSIO_T5;
@@ -887,6 +949,22 @@ static inline void t4_write_reg64(struct adapter *adap, u32 reg_addr, u64 val)
 }
 
 /**
+ * t4_set_hw_addr - store a port's MAC address in SW
+ * @adapter: the adapter
+ * @port_idx: the port index
+ * @hw_addr: the Ethernet address
+ *
+ * Store the Ethernet address of the given port in SW.  Called by the common
+ * code when it retrieves a port's Ethernet address from EEPROM.
+ */
+static inline void t4_set_hw_addr(struct adapter *adapter, int port_idx,
+				  u8 hw_addr[])
+{
+	ether_addr_copy(adapter->port[port_idx]->dev_addr, hw_addr);
+	ether_addr_copy(adapter->port[port_idx]->perm_addr, hw_addr);
+}
+
+/**
  * netdev2pinfo - return the port_info structure associated with a net_device
  * @dev: the netdev
  *
@@ -1055,7 +1133,7 @@ int t4_mgmt_tx(struct adapter *adap, struct sk_buff *skb);
 int t4_ofld_send(struct adapter *adap, struct sk_buff *skb);
 int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 		     struct net_device *dev, int intr_idx,
-		     struct sge_fl *fl, rspq_handler_t hnd);
+		     struct sge_fl *fl, rspq_handler_t hnd, int cong);
 int t4_sge_alloc_eth_txq(struct adapter *adap, struct sge_eth_txq *txq,
 			 struct net_device *dev, struct netdev_queue *netdevq,
 			 unsigned int iqid);
@@ -1095,6 +1173,19 @@ static inline int is_bypass_device(int device)
 	}
 }
 
+static inline int is_10gbt_device(int device)
+{
+	/* this should be set based upon device capabilities */
+	switch (device) {
+	case 0x4409:
+	case 0x4486:
+		return 1;
+
+	default:
+		return 0;
+	}
+}
+
 static inline unsigned int core_ticks_per_usec(const struct adapter *adap)
 {
 	return adap->params.vpd.cclk / 1000;
@@ -1117,9 +1208,19 @@ static inline unsigned int core_ticks_to_us(const struct adapter *adapter,
 void t4_set_reg_field(struct adapter *adap, unsigned int addr, u32 mask,
 		      u32 val);
 
+int t4_wr_mbox_meat_timeout(struct adapter *adap, int mbox, const void *cmd,
+			    int size, void *rpl, bool sleep_ok, int timeout);
 int t4_wr_mbox_meat(struct adapter *adap, int mbox, const void *cmd, int size,
 		    void *rpl, bool sleep_ok);
 
+static inline int t4_wr_mbox_timeout(struct adapter *adap, int mbox,
+				     const void *cmd, int size, void *rpl,
+				     int timeout)
+{
+	return t4_wr_mbox_meat_timeout(adap, mbox, cmd, size, rpl, true,
+				       timeout);
+}
+
 static inline int t4_wr_mbox(struct adapter *adap, int mbox, const void *cmd,
 			     int size, void *rpl)
 {
@@ -1147,10 +1248,14 @@ void t4_intr_disable(struct adapter *adapter);
 int t4_slow_intr_handler(struct adapter *adapter);
 
 int t4_wait_dev_ready(void __iomem *regs);
-int t4_link_start(struct adapter *adap, unsigned int mbox, unsigned int port,
+int t4_link_l1cfg(struct adapter *adap, unsigned int mbox, unsigned int port,
 		  struct link_config *lc);
 int t4_restart_aneg(struct adapter *adap, unsigned int mbox, unsigned int port);
 
+u32 t4_read_pcie_cfg4(struct adapter *adap, int reg);
+u32 t4_get_util_window(struct adapter *adap);
+void t4_setup_memwin(struct adapter *adap, u32 memwin_base, u32 window);
+
 #define T4_MEMORY_WRITE	0
 #define T4_MEMORY_READ	1
 int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr, u32 len,
@@ -1165,10 +1270,16 @@ unsigned int t4_get_regs_len(struct adapter *adapter);
 void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size);
 
 int t4_seeprom_wp(struct adapter *adapter, bool enable);
-int get_vpd_params(struct adapter *adapter, struct vpd_params *p);
+int t4_get_raw_vpd_params(struct adapter *adapter, struct vpd_params *p);
+int t4_get_vpd_params(struct adapter *adapter, struct vpd_params *p);
 int t4_read_flash(struct adapter *adapter, unsigned int addr,
 		  unsigned int nwords, u32 *data, int byte_oriented);
 int t4_load_fw(struct adapter *adapter, const u8 *fw_data, unsigned int size);
+int t4_load_phy_fw(struct adapter *adap,
+		   int win, spinlock_t *lock,
+		   int (*phy_fw_version)(const u8 *, size_t),
+		   const u8 *phy_fw_data, size_t phy_fw_size);
+int t4_phy_fw_ver(struct adapter *adap, int *phy_fw_ver);
 int t4_fwcache(struct adapter *adap, enum fw_params_param_dev_fwcache op);
 int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
 		  const u8 *fw_data, unsigned int size, int force);
@@ -1182,9 +1293,10 @@ int t4_prep_fw(struct adapter *adap, struct fw_info *fw_info,
 int t4_prep_adapter(struct adapter *adapter);
 
 enum t4_bar2_qtype { T4_BAR2_QTYPE_EGRESS, T4_BAR2_QTYPE_INGRESS };
-int cxgb4_t4_bar2_sge_qregs(struct adapter *adapter,
+int t4_bar2_sge_qregs(struct adapter *adapter,
 		      unsigned int qid,
 		      enum t4_bar2_qtype qtype,
+		      int user,
 		      u64 *pbar2_qoffset,
 		      unsigned int *pbar2_qid);
 
@@ -1195,12 +1307,15 @@ int t4_init_devlog_params(struct adapter *adapter);
 int t4_init_sge_params(struct adapter *adapter);
 int t4_init_tp_params(struct adapter *adap);
 int t4_filter_field_shift(const struct adapter *adap, int filter_sel);
+int t4_init_rss_mode(struct adapter *adap, int mbox);
 int t4_port_init(struct adapter *adap, int mbox, int pf, int vf);
 void t4_fatal_err(struct adapter *adapter);
 int t4_config_rss_range(struct adapter *adapter, int mbox, unsigned int viid,
 			int start, int n, const u16 *rspq, unsigned int nrspq);
 int t4_config_glbl_rss(struct adapter *adapter, int mbox, unsigned int mode,
 		       unsigned int flags);
+int t4_config_vi_rss(struct adapter *adapter, int mbox, unsigned int viid,
+		     unsigned int flags, unsigned int defq);
 int t4_read_rss(struct adapter *adapter, u16 *entries);
 void t4_read_rss_key(struct adapter *adapter, u32 *key);
 void t4_write_rss_key(struct adapter *adap, const u32 *key, int idx);
@@ -1211,10 +1326,7 @@ void t4_read_rss_vf_config(struct adapter *adapter, unsigned int index,
 u32 t4_read_rss_pf_map(struct adapter *adapter);
 u32 t4_read_rss_pf_mask(struct adapter *adapter);
 
-int t4_mc_read(struct adapter *adap, int idx, u32 addr, __be32 *data,
-	       u64 *parity);
-int t4_edc_read(struct adapter *adap, int idx, u32 addr, __be32 *data,
-		u64 *parity);
+unsigned int t4_get_mps_bg_map(struct adapter *adapter, int idx);
 void t4_pmtx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[]);
 void t4_pmrx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[]);
 int t4_read_cim_ibq(struct adapter *adap, unsigned int qid, u32 *data,
@@ -1226,21 +1338,36 @@ int t4_cim_read(struct adapter *adap, unsigned int addr, unsigned int n,
 int t4_cim_write(struct adapter *adap, unsigned int addr, unsigned int n,
 		 const unsigned int *valp);
 int t4_cim_read_la(struct adapter *adap, u32 *la_buf, unsigned int *wrptr);
+void t4_cim_read_pif_la(struct adapter *adap, u32 *pif_req, u32 *pif_rsp,
+			unsigned int *pif_req_wrptr,
+			unsigned int *pif_rsp_wrptr);
+void t4_cim_read_ma_la(struct adapter *adap, u32 *ma_req, u32 *ma_rsp);
 void t4_read_cimq_cfg(struct adapter *adap, u16 *base, u16 *size, u16 *thres);
 const char *t4_get_port_type_description(enum fw_port_type port_type);
 void t4_get_port_stats(struct adapter *adap, int idx, struct port_stats *p);
+void t4_get_port_stats_offset(struct adapter *adap, int idx,
+			      struct port_stats *stats,
+			      struct port_stats *offset);
+void t4_get_lb_stats(struct adapter *adap, int idx, struct lb_port_stats *p);
 void t4_read_mtu_tbl(struct adapter *adap, u16 *mtus, u8 *mtu_log);
 void t4_read_cong_tbl(struct adapter *adap, u16 incr[NMTUS][NCCTRL_WIN]);
 void t4_tp_wr_bits_indirect(struct adapter *adap, unsigned int addr,
 			    unsigned int mask, unsigned int val);
 void t4_tp_read_la(struct adapter *adap, u64 *la_buf, unsigned int *wrptr);
+void t4_tp_get_err_stats(struct adapter *adap, struct tp_err_stats *st);
+void t4_tp_get_cpl_stats(struct adapter *adap, struct tp_cpl_stats *st);
+void t4_tp_get_rdma_stats(struct adapter *adap, struct tp_rdma_stats *st);
+void t4_get_usm_stats(struct adapter *adap, struct tp_usm_stats *st);
 void t4_tp_get_tcp_stats(struct adapter *adap, struct tp_tcp_stats *v4,
 			 struct tp_tcp_stats *v6);
+void t4_get_fcoe_stats(struct adapter *adap, unsigned int idx,
+		       struct tp_fcoe_stats *st);
 void t4_load_mtus(struct adapter *adap, const unsigned short *mtus,
 		  const unsigned short *alpha, const unsigned short *beta);
 
 void t4_ulprx_read_la(struct adapter *adap, u32 *la_buf);
 
+void t4_get_chan_txrate(struct adapter *adap, u64 *nic_rate, u64 *ofld_rate);
 void t4_mk_filtdelwr(unsigned int ftid, struct fw_filter_wr *wr, int qid);
 
 void t4_wol_magic_enable(struct adapter *adap, unsigned int port,
@@ -1259,13 +1386,16 @@ int t4_fw_initialize(struct adapter *adap, unsigned int mbox);
 int t4_query_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		    unsigned int vf, unsigned int nparams, const u32 *params,
 		    u32 *val);
+int t4_query_params_rw(struct adapter *adap, unsigned int mbox, unsigned int pf,
+		       unsigned int vf, unsigned int nparams, const u32 *params,
+		       u32 *val, int rw);
+int t4_set_params_timeout(struct adapter *adap, unsigned int mbox,
+			  unsigned int pf, unsigned int vf,
+			  unsigned int nparams, const u32 *params,
+			  const u32 *val, int timeout);
 int t4_set_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		  unsigned int vf, unsigned int nparams, const u32 *params,
 		  const u32 *val);
-int t4_set_params_nosleep(struct adapter *adap, unsigned int mbox,
-			  unsigned int pf, unsigned int vf,
-			  unsigned int nparams, const u32 *params,
-			  const u32 *val);
 int t4_cfg_pfvf(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		unsigned int vf, unsigned int txq, unsigned int txq_eth_ctrl,
 		unsigned int rxqi, unsigned int rxq, unsigned int tc,
@@ -1274,6 +1404,9 @@ int t4_cfg_pfvf(struct adapter *adap, unsigned int mbox, unsigned int pf,
 int t4_alloc_vi(struct adapter *adap, unsigned int mbox, unsigned int port,
 		unsigned int pf, unsigned int vf, unsigned int nmac, u8 *mac,
 		unsigned int *rss_size);
+int t4_free_vi(struct adapter *adap, unsigned int mbox,
+	       unsigned int pf, unsigned int vf,
+	       unsigned int viid);
 int t4_set_rxmode(struct adapter *adap, unsigned int mbox, unsigned int viid,
 		int mtu, int promisc, int all_multi, int bcast, int vlanex,
 		bool sleep_ok);
@@ -1303,6 +1436,7 @@ int t4_ctrl_eq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		    unsigned int vf, unsigned int eqid);
 int t4_ofld_eq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		    unsigned int vf, unsigned int eqid);
+int t4_sge_ctxt_flush(struct adapter *adap, unsigned int mbox);
 int t4_handle_fw_rpl(struct adapter *adap, const __be64 *rpl);
 void t4_db_full(struct adapter *adapter);
 void t4_db_dropped(struct adapter *adapter);
@@ -1310,4 +1444,9 @@ int t4_fwaddrspace_write(struct adapter *adap, unsigned int mbox,
 			 u32 addr, u32 val);
 void t4_sge_decode_idma_state(struct adapter *adapter, int state);
 void t4_free_mem(void *addr);
+void t4_idma_monitor_init(struct adapter *adapter,
+			  struct sge_idma_monitor_state *idma);
+void t4_idma_monitor(struct adapter *adapter,
+		     struct sge_idma_monitor_state *idma,
+		     int hz, int ticks);
 #endif /* __CXGB4_H__ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
index 371f75e782e5..484eb8c37489 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
@@ -182,6 +182,95 @@ static const struct file_operations cim_la_fops = {
 	.release = seq_release_private
 };
 
+static int cim_pif_la_show(struct seq_file *seq, void *v, int idx)
+{
+	const u32 *p = v;
+
+	if (v == SEQ_START_TOKEN) {
+		seq_puts(seq, "Cntl ID DataBE   Addr                 Data\n");
+	} else if (idx < CIM_PIFLA_SIZE) {
+		seq_printf(seq, " %02x  %02x  %04x  %08x %08x%08x%08x%08x\n",
+			   (p[5] >> 22) & 0xff, (p[5] >> 16) & 0x3f,
+			   p[5] & 0xffff, p[4], p[3], p[2], p[1], p[0]);
+	} else {
+		if (idx == CIM_PIFLA_SIZE)
+			seq_puts(seq, "\nCntl ID               Data\n");
+		seq_printf(seq, " %02x  %02x %08x%08x%08x%08x\n",
+			   (p[4] >> 6) & 0xff, p[4] & 0x3f,
+			   p[3], p[2], p[1], p[0]);
+	}
+	return 0;
+}
+
+static int cim_pif_la_open(struct inode *inode, struct file *file)
+{
+	struct seq_tab *p;
+	struct adapter *adap = inode->i_private;
+
+	p = seq_open_tab(file, 2 * CIM_PIFLA_SIZE, 6 * sizeof(u32), 1,
+			 cim_pif_la_show);
+	if (!p)
+		return -ENOMEM;
+
+	t4_cim_read_pif_la(adap, (u32 *)p->data,
+			   (u32 *)p->data + 6 * CIM_PIFLA_SIZE, NULL, NULL);
+	return 0;
+}
+
+static const struct file_operations cim_pif_la_fops = {
+	.owner   = THIS_MODULE,
+	.open    = cim_pif_la_open,
+	.read    = seq_read,
+	.llseek  = seq_lseek,
+	.release = seq_release_private
+};
+
+static int cim_ma_la_show(struct seq_file *seq, void *v, int idx)
+{
+	const u32 *p = v;
+
+	if (v == SEQ_START_TOKEN) {
+		seq_puts(seq, "\n");
+	} else if (idx < CIM_MALA_SIZE) {
+		seq_printf(seq, "%02x%08x%08x%08x%08x\n",
+			   p[4], p[3], p[2], p[1], p[0]);
+	} else {
+		if (idx == CIM_MALA_SIZE)
+			seq_puts(seq,
+				 "\nCnt ID Tag UE       Data       RDY VLD\n");
+		seq_printf(seq, "%3u %2u  %x   %u %08x%08x  %u   %u\n",
+			   (p[2] >> 10) & 0xff, (p[2] >> 7) & 7,
+			   (p[2] >> 3) & 0xf, (p[2] >> 2) & 1,
+			   (p[1] >> 2) | ((p[2] & 3) << 30),
+			   (p[0] >> 2) | ((p[1] & 3) << 30), (p[0] >> 1) & 1,
+			   p[0] & 1);
+	}
+	return 0;
+}
+
+static int cim_ma_la_open(struct inode *inode, struct file *file)
+{
+	struct seq_tab *p;
+	struct adapter *adap = inode->i_private;
+
+	p = seq_open_tab(file, 2 * CIM_MALA_SIZE, 5 * sizeof(u32), 1,
+			 cim_ma_la_show);
+	if (!p)
+		return -ENOMEM;
+
+	t4_cim_read_ma_la(adap, (u32 *)p->data,
+			  (u32 *)p->data + 5 * CIM_MALA_SIZE);
+	return 0;
+}
+
+static const struct file_operations cim_ma_la_fops = {
+	.owner   = THIS_MODULE,
+	.open    = cim_ma_la_open,
+	.read    = seq_read,
+	.llseek  = seq_lseek,
+	.release = seq_release_private
+};
+
 static int cim_qcfg_show(struct seq_file *seq, void *v)
 {
 	static const char * const qname[] = {
@@ -663,6 +752,39 @@ static const struct file_operations pm_stats_debugfs_fops = {
 	.write   = pm_stats_clear
 };
 
+static int tx_rate_show(struct seq_file *seq, void *v)
+{
+	u64 nrate[NCHAN], orate[NCHAN];
+	struct adapter *adap = seq->private;
+
+	t4_get_chan_txrate(adap, nrate, orate);
+	if (adap->params.arch.nchan == NCHAN) {
+		seq_puts(seq, "              channel 0   channel 1   "
+			 "channel 2   channel 3\n");
+		seq_printf(seq, "NIC B/s:     %10llu  %10llu  %10llu  %10llu\n",
+			   (unsigned long long)nrate[0],
+			   (unsigned long long)nrate[1],
+			   (unsigned long long)nrate[2],
+			   (unsigned long long)nrate[3]);
+		seq_printf(seq, "Offload B/s: %10llu  %10llu  %10llu  %10llu\n",
+			   (unsigned long long)orate[0],
+			   (unsigned long long)orate[1],
+			   (unsigned long long)orate[2],
+			   (unsigned long long)orate[3]);
+	} else {
+		seq_puts(seq, "              channel 0   channel 1\n");
+		seq_printf(seq, "NIC B/s:     %10llu  %10llu\n",
+			   (unsigned long long)nrate[0],
+			   (unsigned long long)nrate[1]);
+		seq_printf(seq, "Offload B/s: %10llu  %10llu\n",
+			   (unsigned long long)orate[0],
+			   (unsigned long long)orate[1]);
+	}
+	return 0;
+}
+
+DEFINE_SIMPLE_DEBUGFS_FILE(tx_rate);
+
 static int cctrl_tbl_show(struct seq_file *seq, void *v)
 {
 	static const char * const dec_fac[] = {
@@ -1084,41 +1206,89 @@ static inline void tcamxy2valmask(u64 x, u64 y, u8 *addr, u64 *mask)
 
 static int mps_tcam_show(struct seq_file *seq, void *v)
 {
-	if (v == SEQ_START_TOKEN)
-		seq_puts(seq, "Idx  Ethernet address     Mask     Vld Ports PF"
-			 "  VF              Replication             "
-			 "P0 P1 P2 P3  ML\n");
-	else {
+	struct adapter *adap = seq->private;
+	unsigned int chip_ver = CHELSIO_CHIP_VERSION(adap->params.chip);
+
+	if (v == SEQ_START_TOKEN) {
+		if (adap->params.arch.mps_rplc_size > 128)
+			seq_puts(seq, "Idx  Ethernet address     Mask     "
+				 "Vld Ports PF  VF                           "
+				 "Replication                                "
+				 "    P0 P1 P2 P3  ML\n");
+		else
+			seq_puts(seq, "Idx  Ethernet address     Mask     "
+				 "Vld Ports PF  VF              Replication"
+				 "	         P0 P1 P2 P3  ML\n");
+	} else {
 		u64 mask;
 		u8 addr[ETH_ALEN];
-		struct adapter *adap = seq->private;
+		bool replicate;
 		unsigned int idx = (uintptr_t)v - 2;
-		u64 tcamy = t4_read_reg64(adap, MPS_CLS_TCAM_Y_L(idx));
-		u64 tcamx = t4_read_reg64(adap, MPS_CLS_TCAM_X_L(idx));
-		u32 cls_lo = t4_read_reg(adap, MPS_CLS_SRAM_L(idx));
-		u32 cls_hi = t4_read_reg(adap, MPS_CLS_SRAM_H(idx));
-		u32 rplc[4] = {0, 0, 0, 0};
+		u64 tcamy, tcamx, val;
+		u32 cls_lo, cls_hi, ctl;
+		u32 rplc[8] = {0};
+
+		if (chip_ver > CHELSIO_T5) {
+			/* CtlCmdType - 0: Read, 1: Write
+			 * CtlTcamSel - 0: TCAM0, 1: TCAM1
+			 * CtlXYBitSel- 0: Y bit, 1: X bit
+			 */
+
+			/* Read tcamy */
+			ctl = CTLCMDTYPE_V(0) | CTLXYBITSEL_V(0);
+			if (idx < 256)
+				ctl |= CTLTCAMINDEX_V(idx) | CTLTCAMSEL_V(0);
+			else
+				ctl |= CTLTCAMINDEX_V(idx - 256) |
+				       CTLTCAMSEL_V(1);
+			t4_write_reg(adap, MPS_CLS_TCAM_DATA2_CTL_A, ctl);
+			val = t4_read_reg(adap, MPS_CLS_TCAM_DATA1_A);
+			tcamy = DMACH_G(val) << 32;
+			tcamy |= t4_read_reg(adap, MPS_CLS_TCAM_DATA0_A);
+
+			/* Read tcamx. Change the control param */
+			ctl |= CTLXYBITSEL_V(1);
+			t4_write_reg(adap, MPS_CLS_TCAM_DATA2_CTL_A, ctl);
+			val = t4_read_reg(adap, MPS_CLS_TCAM_DATA1_A);
+			tcamx = DMACH_G(val) << 32;
+			tcamx |= t4_read_reg(adap, MPS_CLS_TCAM_DATA0_A);
+		} else {
+			tcamy = t4_read_reg64(adap, MPS_CLS_TCAM_Y_L(idx));
+			tcamx = t4_read_reg64(adap, MPS_CLS_TCAM_X_L(idx));
+		}
+
+		cls_lo = t4_read_reg(adap, MPS_CLS_SRAM_L(idx));
+		cls_hi = t4_read_reg(adap, MPS_CLS_SRAM_H(idx));
 
 		if (tcamx & tcamy) {
 			seq_printf(seq, "%3u         -\n", idx);
 			goto out;
 		}
 
-		if (cls_lo & REPLICATE_F) {
+		rplc[0] = rplc[1] = rplc[2] = rplc[3] = 0;
+		if (chip_ver > CHELSIO_T5)
+			replicate = (cls_lo & T6_REPLICATE_F);
+		else
+			replicate = (cls_lo & REPLICATE_F);
+
+		if (replicate) {
 			struct fw_ldst_cmd ldst_cmd;
 			int ret;
+			struct fw_ldst_mps_rplc mps_rplc;
+			u32 ldst_addrspc;
 
 			memset(&ldst_cmd, 0, sizeof(ldst_cmd));
+			ldst_addrspc =
+				FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_MPS);
 			ldst_cmd.op_to_addrspace =
 				htonl(FW_CMD_OP_V(FW_LDST_CMD) |
 				      FW_CMD_REQUEST_F |
 				      FW_CMD_READ_F |
-				      FW_LDST_CMD_ADDRSPACE_V(
-					      FW_LDST_ADDRSPC_MPS));
+				      ldst_addrspc);
 			ldst_cmd.cycles_to_len16 = htonl(FW_LEN16(ldst_cmd));
-			ldst_cmd.u.mps.fid_ctl =
+			ldst_cmd.u.mps.rplc.fid_idx =
 				htons(FW_LDST_CMD_FID_V(FW_LDST_MPS_RPLC) |
-				      FW_LDST_CMD_CTL_V(idx));
+				      FW_LDST_CMD_IDX_V(idx));
 			ret = t4_wr_mbox(adap, adap->mbox, &ldst_cmd,
 					 sizeof(ldst_cmd), &ldst_cmd);
 			if (ret)
@@ -1126,30 +1296,69 @@ static int mps_tcam_show(struct seq_file *seq, void *v)
 					 "replication map for idx %d: %d\n",
 					 idx, -ret);
 			else {
-				rplc[0] = ntohl(ldst_cmd.u.mps.rplc31_0);
-				rplc[1] = ntohl(ldst_cmd.u.mps.rplc63_32);
-				rplc[2] = ntohl(ldst_cmd.u.mps.rplc95_64);
-				rplc[3] = ntohl(ldst_cmd.u.mps.rplc127_96);
+				mps_rplc = ldst_cmd.u.mps.rplc;
+				rplc[0] = ntohl(mps_rplc.rplc31_0);
+				rplc[1] = ntohl(mps_rplc.rplc63_32);
+				rplc[2] = ntohl(mps_rplc.rplc95_64);
+				rplc[3] = ntohl(mps_rplc.rplc127_96);
+				if (adap->params.arch.mps_rplc_size > 128) {
+					rplc[4] = ntohl(mps_rplc.rplc159_128);
+					rplc[5] = ntohl(mps_rplc.rplc191_160);
+					rplc[6] = ntohl(mps_rplc.rplc223_192);
+					rplc[7] = ntohl(mps_rplc.rplc255_224);
+				}
 			}
 		}
 
 		tcamxy2valmask(tcamx, tcamy, addr, &mask);
-		seq_printf(seq, "%3u %02x:%02x:%02x:%02x:%02x:%02x %012llx"
-			   "%3c   %#x%4u%4d",
-			   idx, addr[0], addr[1], addr[2], addr[3], addr[4],
-			   addr[5], (unsigned long long)mask,
-			   (cls_lo & SRAM_VLD_F) ? 'Y' : 'N', PORTMAP_G(cls_hi),
-			   PF_G(cls_lo),
-			   (cls_lo & VF_VALID_F) ? VF_G(cls_lo) : -1);
-		if (cls_lo & REPLICATE_F)
-			seq_printf(seq, " %08x %08x %08x %08x",
-				   rplc[3], rplc[2], rplc[1], rplc[0]);
+		if (chip_ver > CHELSIO_T5)
+			seq_printf(seq, "%3u %02x:%02x:%02x:%02x:%02x:%02x "
+				   "%012llx%3c   %#x%4u%4d",
+				   idx, addr[0], addr[1], addr[2], addr[3],
+				   addr[4], addr[5], (unsigned long long)mask,
+				   (cls_lo & T6_SRAM_VLD_F) ? 'Y' : 'N',
+				   PORTMAP_G(cls_hi),
+				   T6_PF_G(cls_lo),
+				   (cls_lo & T6_VF_VALID_F) ?
+				   T6_VF_G(cls_lo) : -1);
 		else
-			seq_printf(seq, "%36c", ' ');
-		seq_printf(seq, "%4u%3u%3u%3u %#x\n",
-			   SRAM_PRIO0_G(cls_lo), SRAM_PRIO1_G(cls_lo),
-			   SRAM_PRIO2_G(cls_lo), SRAM_PRIO3_G(cls_lo),
-			   (cls_lo >> MULTILISTEN0_S) & 0xf);
+			seq_printf(seq, "%3u %02x:%02x:%02x:%02x:%02x:%02x "
+				   "%012llx%3c   %#x%4u%4d",
+				   idx, addr[0], addr[1], addr[2], addr[3],
+				   addr[4], addr[5], (unsigned long long)mask,
+				   (cls_lo & SRAM_VLD_F) ? 'Y' : 'N',
+				   PORTMAP_G(cls_hi),
+				   PF_G(cls_lo),
+				   (cls_lo & VF_VALID_F) ? VF_G(cls_lo) : -1);
+
+		if (replicate) {
+			if (adap->params.arch.mps_rplc_size > 128)
+				seq_printf(seq, " %08x %08x %08x %08x "
+					   "%08x %08x %08x %08x",
+					   rplc[7], rplc[6], rplc[5], rplc[4],
+					   rplc[3], rplc[2], rplc[1], rplc[0]);
+			else
+				seq_printf(seq, " %08x %08x %08x %08x",
+					   rplc[3], rplc[2], rplc[1], rplc[0]);
+		} else {
+			if (adap->params.arch.mps_rplc_size > 128)
+				seq_printf(seq, "%72c", ' ');
+			else
+				seq_printf(seq, "%36c", ' ');
+		}
+
+		if (chip_ver > CHELSIO_T5)
+			seq_printf(seq, "%4u%3u%3u%3u %#x\n",
+				   T6_SRAM_PRIO0_G(cls_lo),
+				   T6_SRAM_PRIO1_G(cls_lo),
+				   T6_SRAM_PRIO2_G(cls_lo),
+				   T6_SRAM_PRIO3_G(cls_lo),
+				   (cls_lo >> T6_MULTILISTEN0_S) & 0xf);
+		else
+			seq_printf(seq, "%4u%3u%3u%3u %#x\n",
+				   SRAM_PRIO0_G(cls_lo), SRAM_PRIO1_G(cls_lo),
+				   SRAM_PRIO2_G(cls_lo), SRAM_PRIO3_G(cls_lo),
+				   (cls_lo >> MULTILISTEN0_S) & 0xf);
 	}
 out:	return 0;
 }
@@ -1222,7 +1431,7 @@ static int sensors_show(struct seq_file *seq, void *v)
 	param[1] = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
 		    FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_DIAG) |
 		    FW_PARAMS_PARAM_Y_V(FW_PARAM_DEV_DIAG_VDD));
-	ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2,
+	ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 2,
 			      param, val);
 
 	if (ret < 0 || val[0] == 0)
@@ -1416,6 +1625,9 @@ static int rss_config_show(struct seq_file *seq, void *v)
 	seq_printf(seq, "  HashDelay:     %3d\n", HASHDELAY_G(rssconf));
 	if (CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5)
 		seq_printf(seq, "  VfWrAddr:      %3d\n", VFWRADDR_G(rssconf));
+	else
+		seq_printf(seq, "  VfWrAddr:      %3d\n",
+			   T6_VFWRADDR_G(rssconf));
 	seq_printf(seq, "  KeyMode:       %s\n", keymode[KEYMODE_G(rssconf)]);
 	seq_printf(seq, "  VfWrEn:        %3s\n", yesno(rssconf & VFWREN_F));
 	seq_printf(seq, "  KeyWrEn:       %3s\n", yesno(rssconf & KEYWREN_F));
@@ -1634,14 +1846,14 @@ static int rss_vf_config_open(struct inode *inode, struct file *file)
 	struct adapter *adapter = inode->i_private;
 	struct seq_tab *p;
 	struct rss_vf_conf *vfconf;
-	int vf;
+	int vf, vfcount = adapter->params.arch.vfcount;
 
-	p = seq_open_tab(file, 128, sizeof(*vfconf), 1, rss_vf_config_show);
+	p = seq_open_tab(file, vfcount, sizeof(*vfconf), 1, rss_vf_config_show);
 	if (!p)
 		return -ENOMEM;
 
 	vfconf = (struct rss_vf_conf *)p->data;
-	for (vf = 0; vf < 128; vf++) {
+	for (vf = 0; vf < vfcount; vf++) {
 		t4_read_rss_vf_config(adapter, vf, &vfconf[vf].rss_vf_vfl,
 				      &vfconf[vf].rss_vf_vfh);
 	}
@@ -1959,6 +2171,61 @@ static void add_debugfs_mem(struct adapter *adap, const char *name,
 				 size_mb << 20);
 }
 
+static int blocked_fl_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t blocked_fl_read(struct file *filp, char __user *ubuf,
+			       size_t count, loff_t *ppos)
+{
+	int len;
+	const struct adapter *adap = filp->private_data;
+	char *buf;
+	ssize_t size = (adap->sge.egr_sz + 3) / 4 +
+			adap->sge.egr_sz / 32 + 2; /* includes ,/\n/\0 */
+
+	buf = kzalloc(size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	len = snprintf(buf, size - 1, "%*pb\n",
+		       adap->sge.egr_sz, adap->sge.blocked_fl);
+	len += sprintf(buf + len, "\n");
+	size = simple_read_from_buffer(ubuf, count, ppos, buf, len);
+	t4_free_mem(buf);
+	return size;
+}
+
+static ssize_t blocked_fl_write(struct file *filp, const char __user *ubuf,
+				size_t count, loff_t *ppos)
+{
+	int err;
+	unsigned long *t;
+	struct adapter *adap = filp->private_data;
+
+	t = kcalloc(BITS_TO_LONGS(adap->sge.egr_sz), sizeof(long), GFP_KERNEL);
+	if (!t)
+		return -ENOMEM;
+
+	err = bitmap_parse_user(ubuf, count, t, adap->sge.egr_sz);
+	if (err)
+		return err;
+
+	bitmap_copy(adap->sge.blocked_fl, t, adap->sge.egr_sz);
+	t4_free_mem(t);
+	return count;
+}
+
+static const struct file_operations blocked_fl_fops = {
+	.owner   = THIS_MODULE,
+	.open    = blocked_fl_open,
+	.read    = blocked_fl_read,
+	.write   = blocked_fl_write,
+	.llseek  = generic_file_llseek,
+};
+
 /* Add an array of Debug FS files.
  */
 void add_debugfs_files(struct adapter *adap,
@@ -1978,11 +2245,13 @@ void add_debugfs_files(struct adapter *adap,
 int t4_setup_debugfs(struct adapter *adap)
 {
 	int i;
-	u32 size;
+	u32 size = 0;
 	struct dentry *de;
 
 	static struct t4_debugfs_entry t4_debugfs_files[] = {
 		{ "cim_la", &cim_la_fops, S_IRUSR, 0 },
+		{ "cim_pif_la", &cim_pif_la_fops, S_IRUSR, 0 },
+		{ "cim_ma_la", &cim_ma_la_fops, S_IRUSR, 0 },
 		{ "cim_qcfg", &cim_qcfg_fops, S_IRUSR, 0 },
 		{ "clk", &clk_debugfs_fops, S_IRUSR, 0 },
 		{ "devlog", &devlog_fops, S_IRUSR, 0 },
@@ -2018,10 +2287,12 @@ int t4_setup_debugfs(struct adapter *adap)
 		{ "ulprx_la", &ulprx_la_fops, S_IRUSR, 0 },
 		{ "sensors", &sensors_debugfs_fops, S_IRUSR, 0 },
 		{ "pm_stats", &pm_stats_debugfs_fops, S_IRUSR, 0 },
+		{ "tx_rate", &tx_rate_debugfs_fops, S_IRUSR, 0 },
 		{ "cctrl", &cctrl_tbl_debugfs_fops, S_IRUSR, 0 },
 #if IS_ENABLED(CONFIG_IPV6)
 		{ "clip_tbl", &clip_tbl_debugfs_fops, S_IRUSR, 0 },
 #endif
+		{ "blocked_fl", &blocked_fl_fops, S_IRUSR | S_IWUSR, 0 },
 	};
 
 	/* Debug FS nodes common to all T5 and later adapters.
@@ -2048,12 +2319,7 @@ int t4_setup_debugfs(struct adapter *adap)
 		size = t4_read_reg(adap, MA_EDRAM1_BAR_A);
 		add_debugfs_mem(adap, "edc1", MEM_EDC1, EDRAM1_SIZE_G(size));
 	}
-	if (is_t4(adap->params.chip)) {
-		size = t4_read_reg(adap, MA_EXT_MEMORY_BAR_A);
-		if (i & EXT_MEM_ENABLE_F)
-			add_debugfs_mem(adap, "mc", MEM_MC,
-					EXT_MEM_SIZE_G(size));
-	} else {
+	if (is_t5(adap->params.chip)) {
 		if (i & EXT_MEM0_ENABLE_F) {
 			size = t4_read_reg(adap, MA_EXT_MEMORY0_BAR_A);
 			add_debugfs_mem(adap, "mc0", MEM_MC0,
@@ -2064,6 +2330,11 @@ int t4_setup_debugfs(struct adapter *adap)
 			add_debugfs_mem(adap, "mc1", MEM_MC1,
 					EXT_MEM1_SIZE_G(size));
 		}
+	} else {
+		if (i & EXT_MEM_ENABLE_F)
+			size = t4_read_reg(adap, MA_EXT_MEMORY_BAR_A);
+			add_debugfs_mem(adap, "mc", MEM_MC,
+					EXT_MEM_SIZE_G(size));
 	}
 
 	de = debugfs_create_file_size("flash", S_IRUSR, adap->debugfs_root, adap,
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
index 10d82b51d7ef..687acf71fa15 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
@@ -108,15 +108,82 @@ static const char stats_strings[][ETH_GSTRING_LEN] = {
 	"VLANinsertions     ",
 	"GROpackets         ",
 	"GROmerged          ",
-	"WriteCoalSuccess   ",
-	"WriteCoalFail      ",
+};
+
+static char adapter_stats_strings[][ETH_GSTRING_LEN] = {
+	"db_drop                ",
+	"db_full                ",
+	"db_empty               ",
+	"tcp_ipv4_out_rsts      ",
+	"tcp_ipv4_in_segs       ",
+	"tcp_ipv4_out_segs      ",
+	"tcp_ipv4_retrans_segs  ",
+	"tcp_ipv6_out_rsts      ",
+	"tcp_ipv6_in_segs       ",
+	"tcp_ipv6_out_segs      ",
+	"tcp_ipv6_retrans_segs  ",
+	"usm_ddp_frames         ",
+	"usm_ddp_octets         ",
+	"usm_ddp_drops          ",
+	"rdma_no_rqe_mod_defer  ",
+	"rdma_no_rqe_pkt_defer  ",
+	"tp_err_ofld_no_neigh   ",
+	"tp_err_ofld_cong_defer ",
+	"write_coal_success     ",
+	"write_coal_fail        ",
+};
+
+static char channel_stats_strings[][ETH_GSTRING_LEN] = {
+	"--------Channel--------- ",
+	"tp_cpl_requests        ",
+	"tp_cpl_responses       ",
+	"tp_mac_in_errs         ",
+	"tp_hdr_in_errs         ",
+	"tp_tcp_in_errs         ",
+	"tp_tcp6_in_errs        ",
+	"tp_tnl_cong_drops      ",
+	"tp_tnl_tx_drops        ",
+	"tp_ofld_vlan_drops     ",
+	"tp_ofld_chan_drops     ",
+	"fcoe_octets_ddp        ",
+	"fcoe_frames_ddp        ",
+	"fcoe_frames_drop       ",
+};
+
+static char loopback_stats_strings[][ETH_GSTRING_LEN] = {
+	"-------Loopback----------- ",
+	"octets_ok              ",
+	"frames_ok              ",
+	"bcast_frames           ",
+	"mcast_frames           ",
+	"ucast_frames           ",
+	"error_frames           ",
+	"frames_64              ",
+	"frames_65_to_127       ",
+	"frames_128_to_255      ",
+	"frames_256_to_511      ",
+	"frames_512_to_1023     ",
+	"frames_1024_to_1518    ",
+	"frames_1519_to_max     ",
+	"frames_dropped         ",
+	"bg0_frames_dropped     ",
+	"bg1_frames_dropped     ",
+	"bg2_frames_dropped     ",
+	"bg3_frames_dropped     ",
+	"bg0_frames_trunc       ",
+	"bg1_frames_trunc       ",
+	"bg2_frames_trunc       ",
+	"bg3_frames_trunc       ",
 };
 
 static int get_sset_count(struct net_device *dev, int sset)
 {
 	switch (sset) {
 	case ETH_SS_STATS:
-		return ARRAY_SIZE(stats_strings);
+		return ARRAY_SIZE(stats_strings) +
+		       ARRAY_SIZE(adapter_stats_strings) +
+		       ARRAY_SIZE(channel_stats_strings) +
+		       ARRAY_SIZE(loopback_stats_strings);
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -168,8 +235,18 @@ static void get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *info)
 
 static void get_strings(struct net_device *dev, u32 stringset, u8 *data)
 {
-	if (stringset == ETH_SS_STATS)
+	if (stringset == ETH_SS_STATS) {
 		memcpy(data, stats_strings, sizeof(stats_strings));
+		data += sizeof(stats_strings);
+		memcpy(data, adapter_stats_strings,
+		       sizeof(adapter_stats_strings));
+		data += sizeof(adapter_stats_strings);
+		memcpy(data, channel_stats_strings,
+		       sizeof(channel_stats_strings));
+		data += sizeof(channel_stats_strings);
+		memcpy(data, loopback_stats_strings,
+		       sizeof(loopback_stats_strings));
+	}
 }
 
 /* port stats maintained per queue of the port. They should be in the same
@@ -185,6 +262,45 @@ struct queue_port_stats {
 	u64 gro_merged;
 };
 
+struct adapter_stats {
+	u64 db_drop;
+	u64 db_full;
+	u64 db_empty;
+	u64 tcp_v4_out_rsts;
+	u64 tcp_v4_in_segs;
+	u64 tcp_v4_out_segs;
+	u64 tcp_v4_retrans_segs;
+	u64 tcp_v6_out_rsts;
+	u64 tcp_v6_in_segs;
+	u64 tcp_v6_out_segs;
+	u64 tcp_v6_retrans_segs;
+	u64 frames;
+	u64 octets;
+	u64 drops;
+	u64 rqe_dfr_mod;
+	u64 rqe_dfr_pkt;
+	u64 ofld_no_neigh;
+	u64 ofld_cong_defer;
+	u64 wc_success;
+	u64 wc_fail;
+};
+
+struct channel_stats {
+	u64 cpl_req;
+	u64 cpl_rsp;
+	u64 mac_in_errs;
+	u64 hdr_in_errs;
+	u64 tcp_in_errs;
+	u64 tcp6_in_errs;
+	u64 tnl_cong_drops;
+	u64 tnl_tx_drops;
+	u64 ofld_vlan_drops;
+	u64 ofld_chan_drops;
+	u64 octets_ddp;
+	u64 frames_ddp;
+	u64 frames_drop;
+};
+
 static void collect_sge_port_stats(const struct adapter *adap,
 				   const struct port_info *p,
 				   struct queue_port_stats *s)
@@ -205,30 +321,121 @@ static void collect_sge_port_stats(const struct adapter *adap,
 	}
 }
 
+static void collect_adapter_stats(struct adapter *adap, struct adapter_stats *s)
+{
+	struct tp_tcp_stats v4, v6;
+	struct tp_rdma_stats rdma_stats;
+	struct tp_err_stats err_stats;
+	struct tp_usm_stats usm_stats;
+	u64 val1, val2;
+
+	memset(s, 0, sizeof(*s));
+
+	spin_lock(&adap->stats_lock);
+	t4_tp_get_tcp_stats(adap, &v4, &v6);
+	t4_tp_get_rdma_stats(adap, &rdma_stats);
+	t4_get_usm_stats(adap, &usm_stats);
+	t4_tp_get_err_stats(adap, &err_stats);
+	spin_unlock(&adap->stats_lock);
+
+	s->db_drop = adap->db_stats.db_drop;
+	s->db_full = adap->db_stats.db_full;
+	s->db_empty = adap->db_stats.db_empty;
+
+	s->tcp_v4_out_rsts = v4.tcp_out_rsts;
+	s->tcp_v4_in_segs = v4.tcp_in_segs;
+	s->tcp_v4_out_segs = v4.tcp_out_segs;
+	s->tcp_v4_retrans_segs = v4.tcp_retrans_segs;
+	s->tcp_v6_out_rsts = v6.tcp_out_rsts;
+	s->tcp_v6_in_segs = v6.tcp_in_segs;
+	s->tcp_v6_out_segs = v6.tcp_out_segs;
+	s->tcp_v6_retrans_segs = v6.tcp_retrans_segs;
+
+	if (is_offload(adap)) {
+		s->frames = usm_stats.frames;
+		s->octets = usm_stats.octets;
+		s->drops = usm_stats.drops;
+		s->rqe_dfr_mod = rdma_stats.rqe_dfr_mod;
+		s->rqe_dfr_pkt = rdma_stats.rqe_dfr_pkt;
+	}
+
+	s->ofld_no_neigh = err_stats.ofld_no_neigh;
+	s->ofld_cong_defer = err_stats.ofld_cong_defer;
+
+	if (!is_t4(adap->params.chip)) {
+		int v;
+
+		v = t4_read_reg(adap, SGE_STAT_CFG_A);
+		if (STATSOURCE_T5_G(v) == 7) {
+			val2 = t4_read_reg(adap, SGE_STAT_MATCH_A);
+			val1 = t4_read_reg(adap, SGE_STAT_TOTAL_A);
+			s->wc_success = val1 - val2;
+			s->wc_fail = val2;
+		}
+	}
+}
+
+static void collect_channel_stats(struct adapter *adap, struct channel_stats *s,
+				  u8 i)
+{
+	struct tp_cpl_stats cpl_stats;
+	struct tp_err_stats err_stats;
+	struct tp_fcoe_stats fcoe_stats;
+
+	memset(s, 0, sizeof(*s));
+
+	spin_lock(&adap->stats_lock);
+	t4_tp_get_cpl_stats(adap, &cpl_stats);
+	t4_tp_get_err_stats(adap, &err_stats);
+	t4_get_fcoe_stats(adap, i, &fcoe_stats);
+	spin_unlock(&adap->stats_lock);
+
+	s->cpl_req = cpl_stats.req[i];
+	s->cpl_rsp = cpl_stats.rsp[i];
+	s->mac_in_errs = err_stats.mac_in_errs[i];
+	s->hdr_in_errs = err_stats.hdr_in_errs[i];
+	s->tcp_in_errs = err_stats.tcp_in_errs[i];
+	s->tcp6_in_errs = err_stats.tcp6_in_errs[i];
+	s->tnl_cong_drops = err_stats.tnl_cong_drops[i];
+	s->tnl_tx_drops = err_stats.tnl_tx_drops[i];
+	s->ofld_vlan_drops = err_stats.ofld_vlan_drops[i];
+	s->ofld_chan_drops = err_stats.ofld_chan_drops[i];
+	s->octets_ddp = fcoe_stats.octets_ddp;
+	s->frames_ddp = fcoe_stats.frames_ddp;
+	s->frames_drop = fcoe_stats.frames_drop;
+}
+
 static void get_stats(struct net_device *dev, struct ethtool_stats *stats,
 		      u64 *data)
 {
 	struct port_info *pi = netdev_priv(dev);
 	struct adapter *adapter = pi->adapter;
-	u32 val1, val2;
+	struct lb_port_stats s;
+	int i;
+	u64 *p0;
 
-	t4_get_port_stats(adapter, pi->tx_chan, (struct port_stats *)data);
+	t4_get_port_stats_offset(adapter, pi->tx_chan,
+				 (struct port_stats *)data,
+				 &pi->stats_base);
 
 	data += sizeof(struct port_stats) / sizeof(u64);
 	collect_sge_port_stats(adapter, pi, (struct queue_port_stats *)data);
 	data += sizeof(struct queue_port_stats) / sizeof(u64);
-	if (!is_t4(adapter->params.chip)) {
-		t4_write_reg(adapter, SGE_STAT_CFG_A, STATSOURCE_T5_V(7));
-		val1 = t4_read_reg(adapter, SGE_STAT_TOTAL_A);
-		val2 = t4_read_reg(adapter, SGE_STAT_MATCH_A);
-		*data = val1 - val2;
-		data++;
-		*data = val2;
-		data++;
-	} else {
-		memset(data, 0, 2 * sizeof(u64));
-		*data += 2;
-	}
+	collect_adapter_stats(adapter, (struct adapter_stats *)data);
+	data += sizeof(struct adapter_stats) / sizeof(u64);
+
+	*data++ = (u64)pi->port_id;
+	collect_channel_stats(adapter, (struct channel_stats *)data,
+			      pi->port_id);
+	data += sizeof(struct channel_stats) / sizeof(u64);
+
+	*data++ = (u64)pi->port_id;
+	memset(&s, 0, sizeof(s));
+	t4_get_lb_stats(adapter, pi->port_id, &s);
+
+	p0 = &s.octets;
+	for (i = 0; i < ARRAY_SIZE(loopback_stats_strings) - 1; i++)
+		*data++ = (unsigned long long)*p0++;
 }
 
 static void get_regs(struct net_device *dev, struct ethtool_regs *regs,
@@ -250,7 +457,7 @@ static int restart_autoneg(struct net_device *dev)
 		return -EAGAIN;
 	if (p->link_cfg.autoneg != AUTONEG_ENABLE)
 		return -EINVAL;
-	t4_restart_aneg(p->adapter, p->adapter->fn, p->tx_chan);
+	t4_restart_aneg(p->adapter, p->adapter->pf, p->tx_chan);
 	return 0;
 }
 
@@ -267,7 +474,7 @@ static int identify_port(struct net_device *dev,
 	else
 		return -EINVAL;
 
-	return t4_identify_port(adap, adap->fn, netdev2pinfo(dev)->viid, val);
+	return t4_identify_port(adap, adap->pf, netdev2pinfo(dev)->viid, val);
 }
 
 static unsigned int from_fw_linkcaps(enum fw_port_type type, unsigned int caps)
@@ -439,7 +646,7 @@ static int set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 	lc->autoneg = cmd->autoneg;
 
 	if (netif_running(dev))
-		return t4_link_start(p->adapter, p->adapter->fn, p->tx_chan,
+		return t4_link_l1cfg(p->adapter, p->adapter->pf, p->tx_chan,
 				     lc);
 	return 0;
 }
@@ -472,7 +679,7 @@ static int set_pauseparam(struct net_device *dev,
 	if (epause->tx_pause)
 		lc->requested_fc |= PAUSE_TX;
 	if (netif_running(dev))
-		return t4_link_start(p->adapter, p->adapter->fn, p->tx_chan,
+		return t4_link_l1cfg(p->adapter, p->adapter->pf, p->tx_chan,
 				     lc);
 	return 0;
 }
@@ -578,7 +785,7 @@ static int get_coalesce(struct net_device *dev, struct ethtool_coalesce *c)
 	const struct sge_rspq *rq = &adap->sge.ethrxq[pi->first_qset].rspq;
 
 	c->rx_coalesce_usecs = qtimer_val(adap, rq);
-	c->rx_max_coalesced_frames = (rq->intr_params & QINTR_CNT_EN) ?
+	c->rx_max_coalesced_frames = (rq->intr_params & QINTR_CNT_EN_F) ?
 		adap->sge.counter_val[rq->pktcnt_idx] : 0;
 	c->use_adaptive_rx_coalesce = get_adaptive_rx_setting(dev);
 	return 0;
@@ -617,7 +824,7 @@ static int eeprom_ptov(unsigned int phys_addr, unsigned int fn, unsigned int sz)
  */
 static int eeprom_rd_phys(struct adapter *adap, unsigned int phys_addr, u32 *v)
 {
-	int vaddr = eeprom_ptov(phys_addr, adap->fn, EEPROMPFSIZE);
+	int vaddr = eeprom_ptov(phys_addr, adap->pf, EEPROMPFSIZE);
 
 	if (vaddr >= 0)
 		vaddr = pci_read_vpd(adap->pdev, vaddr, sizeof(u32), v);
@@ -626,7 +833,7 @@ static int eeprom_rd_phys(struct adapter *adap, unsigned int phys_addr, u32 *v)
 
 static int eeprom_wr_phys(struct adapter *adap, unsigned int phys_addr, u32 v)
 {
-	int vaddr = eeprom_ptov(phys_addr, adap->fn, EEPROMPFSIZE);
+	int vaddr = eeprom_ptov(phys_addr, adap->pf, EEPROMPFSIZE);
 
 	if (vaddr >= 0)
 		vaddr = pci_write_vpd(adap->pdev, vaddr, sizeof(u32), &v);
@@ -669,8 +876,8 @@ static int set_eeprom(struct net_device *dev, struct ethtool_eeprom *eeprom,
 	aligned_offset = eeprom->offset & ~3;
 	aligned_len = (eeprom->len + (eeprom->offset & 3) + 3) & ~3;
 
-	if (adapter->fn > 0) {
-		u32 start = 1024 + adapter->fn * EEPROMPFSIZE;
+	if (adapter->pf > 0) {
+		u32 start = 1024 + adapter->pf * EEPROMPFSIZE;
 
 		if (aligned_offset < start ||
 		    aligned_offset + aligned_len > start + EEPROMPFSIZE)
@@ -740,37 +947,6 @@ static int set_flash(struct net_device *netdev, struct ethtool_flash *ef)
 	return ret;
 }
 
-#define WOL_SUPPORTED (WAKE_BCAST | WAKE_MAGIC)
-#define BCAST_CRC 0xa0ccc1a6
-
-static void get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
-{
-	wol->supported = WAKE_BCAST | WAKE_MAGIC;
-	wol->wolopts = netdev2adap(dev)->wol;
-	memset(&wol->sopass, 0, sizeof(wol->sopass));
-}
-
-static int set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
-{
-	int err = 0;
-	struct port_info *pi = netdev_priv(dev);
-
-	if (wol->wolopts & ~WOL_SUPPORTED)
-		return -EINVAL;
-	t4_wol_magic_enable(pi->adapter, pi->tx_chan,
-			    (wol->wolopts & WAKE_MAGIC) ? dev->dev_addr : NULL);
-	if (wol->wolopts & WAKE_BCAST) {
-		err = t4_wol_pat_enable(pi->adapter, pi->tx_chan, 0xfe, ~0ULL,
-					~0ULL, 0, false);
-		if (!err)
-			err = t4_wol_pat_enable(pi->adapter, pi->tx_chan, 1,
-						~6ULL, ~0ULL, BCAST_CRC, true);
-	} else {
-		t4_wol_pat_enable(pi->adapter, pi->tx_chan, 0, 0, 0, 0, false);
-	}
-	return err;
-}
-
 static u32 get_rss_table_size(struct net_device *dev)
 {
 	const struct port_info *pi = netdev_priv(dev);
@@ -900,8 +1076,6 @@ static const struct ethtool_ops cxgb_ethtool_ops = {
 	.get_ethtool_stats = get_stats,
 	.get_regs_len      = get_regs_len,
 	.get_regs          = get_regs,
-	.get_wol           = get_wol,
-	.set_wol           = set_wol,
 	.get_rxnfc         = get_rxnfc,
 	.get_rxfh_indir_size = get_rss_table_size,
 	.get_rxfh	   = get_rss_table,
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 803d91beec6f..351f3b1bf800 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -135,8 +135,14 @@ struct filter_entry {
 
 #define FW4_FNAME "cxgb4/t4fw.bin"
 #define FW5_FNAME "cxgb4/t5fw.bin"
+#define FW6_FNAME "cxgb4/t6fw.bin"
 #define FW4_CFNAME "cxgb4/t4-config.txt"
 #define FW5_CFNAME "cxgb4/t5-config.txt"
+#define FW6_CFNAME "cxgb4/t6-config.txt"
+#define PHY_AQ1202_FIRMWARE "cxgb4/aq1202_fw.cld"
+#define PHY_BCM84834_FIRMWARE "cxgb4/bcm8483.bin"
+#define PHY_AQ1202_DEVICEID 0x4409
+#define PHY_BCM84834_DEVICEID 0x4486
 
 MODULE_DESCRIPTION(DRV_DESC);
 MODULE_AUTHOR("Chelsio Communications");
@@ -318,8 +324,9 @@ static void dcb_tx_queue_prio_enable(struct net_device *dev, int enable)
 		 * level") we need to issue the Set Parameters Commannd
 		 * without sleeping (timeout < 0).
 		 */
-		err = t4_set_params_nosleep(adap, adap->mbox, adap->fn, 0, 1,
-					    &name, &value);
+		err = t4_set_params_timeout(adap, adap->mbox, adap->pf, 0, 1,
+					    &name, &value,
+					    -FW_CMD_MAX_TIMEOUT);
 
 		if (err)
 			dev_err(adap->pdev_dev,
@@ -382,7 +389,7 @@ static int set_addr_filters(const struct net_device *dev, bool sleep)
 	int uc_cnt = netdev_uc_count(dev);
 	int mc_cnt = netdev_mc_count(dev);
 	const struct port_info *pi = netdev_priv(dev);
-	unsigned int mb = pi->adapter->fn;
+	unsigned int mb = pi->adapter->pf;
 
 	/* first do the secondary unicast addresses */
 	netdev_for_each_uc_addr(ha, dev) {
@@ -439,7 +446,7 @@ static int set_rxmode(struct net_device *dev, int mtu, bool sleep_ok)
 
 	ret = set_addr_filters(dev, sleep_ok);
 	if (ret == 0)
-		ret = t4_set_rxmode(pi->adapter, pi->adapter->fn, pi->viid, mtu,
+		ret = t4_set_rxmode(pi->adapter, pi->adapter->pf, pi->viid, mtu,
 				    (dev->flags & IFF_PROMISC) ? 1 : 0,
 				    (dev->flags & IFF_ALLMULTI) ? 1 : 0, 1, -1,
 				    sleep_ok);
@@ -456,7 +463,7 @@ static int link_start(struct net_device *dev)
 {
 	int ret;
 	struct port_info *pi = netdev_priv(dev);
-	unsigned int mb = pi->adapter->fn;
+	unsigned int mb = pi->adapter->pf;
 
 	/*
 	 * We do not set address filters and promiscuity here, the stack does
@@ -474,7 +481,7 @@ static int link_start(struct net_device *dev)
 		}
 	}
 	if (ret == 0)
-		ret = t4_link_start(pi->adapter, mb, pi->tx_chan,
+		ret = t4_link_l1cfg(pi->adapter, mb, pi->tx_chan,
 				    &pi->link_cfg);
 	if (ret == 0) {
 		local_bh_disable();
@@ -856,23 +863,39 @@ static void free_msix_queue_irqs(struct adapter *adap)
  *
  *	Sets up the portion of the HW RSS table for the port's VI to distribute
  *	packets to the Rx queues in @queues.
+ *	Should never be called before setting up sge eth rx queues
  */
 int cxgb4_write_rss(const struct port_info *pi, const u16 *queues)
 {
 	u16 *rss;
 	int i, err;
-	const struct sge_eth_rxq *q = &pi->adapter->sge.ethrxq[pi->first_qset];
+	struct adapter *adapter = pi->adapter;
+	const struct sge_eth_rxq *rxq;
 
+	rxq = &adapter->sge.ethrxq[pi->first_qset];
 	rss = kmalloc(pi->rss_size * sizeof(u16), GFP_KERNEL);
 	if (!rss)
 		return -ENOMEM;
 
 	/* map the queue indices to queue ids */
 	for (i = 0; i < pi->rss_size; i++, queues++)
-		rss[i] = q[*queues].rspq.abs_id;
+		rss[i] = rxq[*queues].rspq.abs_id;
 
-	err = t4_config_rss_range(pi->adapter, pi->adapter->fn, pi->viid, 0,
+	err = t4_config_rss_range(adapter, adapter->pf, pi->viid, 0,
 				  pi->rss_size, rss, pi->rss_size);
+	/* If Tunnel All Lookup isn't specified in the global RSS
+	 * Configuration, then we need to specify a default Ingress
+	 * Queue for any ingress packets which aren't hashed.  We'll
+	 * use our first ingress queue ...
+	 */
+	if (!err)
+		err = t4_config_vi_rss(adapter, adapter->mbox, pi->viid,
+				       FW_RSS_VI_CONFIG_CMD_IP6FOURTUPEN_F |
+				       FW_RSS_VI_CONFIG_CMD_IP6TWOTUPEN_F |
+				       FW_RSS_VI_CONFIG_CMD_IP4FOURTUPEN_F |
+				       FW_RSS_VI_CONFIG_CMD_IP4TWOTUPEN_F |
+				       FW_RSS_VI_CONFIG_CMD_UDPEN_F,
+				       rss[0]);
 	kfree(rss);
 	return err;
 }
@@ -885,11 +908,15 @@ int cxgb4_write_rss(const struct port_info *pi, const u16 *queues)
  */
 static int setup_rss(struct adapter *adap)
 {
-	int i, err;
+	int i, j, err;
 
 	for_each_port(adap, i) {
 		const struct port_info *pi = adap2pinfo(adap, i);
 
+		/* Fill default values with equal distribution */
+		for (j = 0; j < pi->rss_size; j++)
+			pi->rss[j] = j % pi->nqsets;
+
 		err = cxgb4_write_rss(pi, pi->rss);
 		if (err)
 			return err;
@@ -977,7 +1004,7 @@ static int alloc_ofld_rxqs(struct adapter *adap, struct sge_ofld_rxq *q,
 		err = t4_sge_alloc_rxq(adap, &q->rspq, false,
 				       adap->port[i / per_chan],
 				       msi_idx, q->fl.size ? &q->fl : NULL,
-				       uldrx_handler);
+				       uldrx_handler, 0);
 		if (err)
 			return err;
 		memset(&q->stats, 0, sizeof(q->stats));
@@ -1007,7 +1034,7 @@ static int setup_sge_queues(struct adapter *adap)
 		msi_idx = 1;         /* vector 0 is for non-queue interrupts */
 	else {
 		err = t4_sge_alloc_rxq(adap, &s->intrq, false, adap->port[0], 0,
-				       NULL, NULL);
+				       NULL, NULL, -1);
 		if (err)
 			return err;
 		msi_idx = -((int)s->intrq.abs_id + 1);
@@ -1027,7 +1054,7 @@ static int setup_sge_queues(struct adapter *adap)
 	 *    new/deleted queues.
 	 */
 	err = t4_sge_alloc_rxq(adap, &s->fw_evtq, true, adap->port[0],
-			       msi_idx, NULL, fwevtq_handler);
+			       msi_idx, NULL, fwevtq_handler, -1);
 	if (err) {
 freeout:	t4_free_sge_resources(adap);
 		return err;
@@ -1044,7 +1071,9 @@ freeout:	t4_free_sge_resources(adap);
 				msi_idx++;
 			err = t4_sge_alloc_rxq(adap, &q->rspq, false, dev,
 					       msi_idx, &q->fl,
-					       t4_ethrx_handler);
+					       t4_ethrx_handler,
+					       t4_get_mps_bg_map(adap,
+								 pi->tx_chan));
 			if (err)
 				goto freeout;
 			q->rspq.idx = j;
@@ -1121,10 +1150,7 @@ void *t4_alloc_mem(size_t size)
  */
 void t4_free_mem(void *addr)
 {
-	if (is_vmalloc_addr(addr))
-		vfree(addr);
-	else
-		kfree(addr);
+	kvfree(addr);
 }
 
 /* Send a Work Request to write the filter at a specified index.  We construct
@@ -1324,11 +1350,6 @@ static u16 cxgb_select_queue(struct net_device *dev, struct sk_buff *skb,
 	return fallback(dev, skb) % dev->real_num_tx_queues;
 }
 
-static inline int is_offload(const struct adapter *adap)
-{
-	return adap->params.offload;
-}
-
 static int closest_timer(const struct sge *s, int time)
 {
 	int i, delta, match = 0, min_delta = INT_MAX;
@@ -1389,8 +1410,8 @@ int cxgb4_set_rspq_intr_params(struct sge_rspq *q,
 			    FW_PARAMS_PARAM_X_V(
 					FW_PARAMS_PARAM_DMAQ_IQ_INTCNTTHRESH) |
 			    FW_PARAMS_PARAM_YZ_V(q->cntxt_id);
-			err = t4_set_params(adap, adap->fn, adap->fn, 0, 1, &v,
-					    &new_idx);
+			err = t4_set_params(adap, adap->mbox, adap->pf, 0, 1,
+					    &v, &new_idx);
 			if (err)
 				return err;
 		}
@@ -1398,7 +1419,7 @@ int cxgb4_set_rspq_intr_params(struct sge_rspq *q,
 	}
 
 	us = us == 0 ? 6 : closest_timer(&adap->sge, us);
-	q->intr_params = QINTR_TIMER_IDX(us) | (cnt > 0 ? QINTR_CNT_EN : 0);
+	q->intr_params = QINTR_TIMER_IDX_V(us) | QINTR_CNT_EN_V(cnt > 0);
 	return 0;
 }
 
@@ -1411,7 +1432,7 @@ static int cxgb_set_features(struct net_device *dev, netdev_features_t features)
 	if (!(changed & NETIF_F_HW_VLAN_CTAG_RX))
 		return 0;
 
-	err = t4_set_rxmode(pi->adapter, pi->adapter->fn, pi->viid, -1,
+	err = t4_set_rxmode(pi->adapter, pi->adapter->pf, pi->viid, -1,
 			    -1, -1, -1,
 			    !!(features & NETIF_F_HW_VLAN_CTAG_RX), true);
 	if (unlikely(err))
@@ -1694,7 +1715,7 @@ static int tid_init(struct tid_info *t)
 	bitmap_zero(t->stid_bmap, t->nstids + t->nsftids);
 	/* Reserve stid 0 for T4/T5 adapters */
 	if (!t->stid_base &&
-	    (is_t4(adap->params.chip) || is_t5(adap->params.chip)))
+	    (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5))
 		__set_bit(0, t->stid_bmap);
 
 	return 0;
@@ -1983,11 +2004,8 @@ EXPORT_SYMBOL(cxgb4_iscsi_init);
 int cxgb4_flush_eq_cache(struct net_device *dev)
 {
 	struct adapter *adap = netdev2adap(dev);
-	int ret;
 
-	ret = t4_fwaddrspace_write(adap, adap->mbox,
-				   0xe1000000 + SGE_CTXT_CMD_A, 0x20000000);
-	return ret;
+	return t4_sge_ctxt_flush(adap, adap->mbox);
 }
 EXPORT_SYMBOL(cxgb4_flush_eq_cache);
 
@@ -2042,25 +2060,6 @@ out:
 }
 EXPORT_SYMBOL(cxgb4_sync_txq_pidx);
 
-void cxgb4_disable_db_coalescing(struct net_device *dev)
-{
-	struct adapter *adap;
-
-	adap = netdev2adap(dev);
-	t4_set_reg_field(adap, SGE_DOORBELL_CONTROL_A, NOCOALESCE_F,
-			 NOCOALESCE_F);
-}
-EXPORT_SYMBOL(cxgb4_disable_db_coalescing);
-
-void cxgb4_enable_db_coalescing(struct net_device *dev)
-{
-	struct adapter *adap;
-
-	adap = netdev2adap(dev);
-	t4_set_reg_field(adap, SGE_DOORBELL_CONTROL_A, NOCOALESCE_F, 0);
-}
-EXPORT_SYMBOL(cxgb4_enable_db_coalescing);
-
 int cxgb4_read_tpte(struct net_device *dev, u32 stag, __be32 *tpte)
 {
 	struct adapter *adap;
@@ -2100,10 +2099,7 @@ int cxgb4_read_tpte(struct net_device *dev, u32 stag, __be32 *tpte)
 		if (offset < mc0_end) {
 			memtype = MEM_MC0;
 			memaddr = offset - edc1_end;
-		} else if (is_t4(adap->params.chip)) {
-			/* T4 only has a single memory channel */
-			goto err;
-		} else {
+		} else if (is_t5(adap->params.chip)) {
 			size = t4_read_reg(adap, MA_EXT_MEMORY1_BAR_A);
 			mc1_size = EXT_MEM1_SIZE_G(size) << 20;
 			mc1_end = mc0_end + mc1_size;
@@ -2114,6 +2110,9 @@ int cxgb4_read_tpte(struct net_device *dev, u32 stag, __be32 *tpte)
 				/* offset beyond the end of any memory */
 				goto err;
 			}
+		} else {
+			/* T4/T6 only has a single memory channel */
+			goto err;
 		}
 	}
 
@@ -2145,14 +2144,16 @@ EXPORT_SYMBOL(cxgb4_read_sge_timestamp);
 int cxgb4_bar2_sge_qregs(struct net_device *dev,
 			 unsigned int qid,
 			 enum cxgb4_bar2_qtype qtype,
+			 int user,
 			 u64 *pbar2_qoffset,
 			 unsigned int *pbar2_qid)
 {
-	return cxgb4_t4_bar2_sge_qregs(netdev2adap(dev),
+	return t4_bar2_sge_qregs(netdev2adap(dev),
 				 qid,
 				 (qtype == CXGB4_BAR2_QTYPE_EGRESS
 				  ? T4_BAR2_QTYPE_EGRESS
 				  : T4_BAR2_QTYPE_INGRESS),
+				 user,
 				 pbar2_qoffset,
 				 pbar2_qid);
 }
@@ -2278,9 +2279,13 @@ static void process_db_full(struct work_struct *work)
 	drain_db_fifo(adap, dbfifo_drain_delay);
 	enable_dbs(adap);
 	notify_rdma_uld(adap, CXGB4_CONTROL_DB_EMPTY);
-	t4_set_reg_field(adap, SGE_INT_ENABLE3_A,
-			 DBFIFO_HP_INT_F | DBFIFO_LP_INT_F,
-			 DBFIFO_HP_INT_F | DBFIFO_LP_INT_F);
+	if (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5)
+		t4_set_reg_field(adap, SGE_INT_ENABLE3_A,
+				 DBFIFO_HP_INT_F | DBFIFO_LP_INT_F,
+				 DBFIFO_HP_INT_F | DBFIFO_LP_INT_F);
+	else
+		t4_set_reg_field(adap, SGE_INT_ENABLE3_A,
+				 DBFIFO_LP_INT_F, DBFIFO_LP_INT_F);
 }
 
 static void sync_txq_pidx(struct adapter *adap, struct sge_txq *q)
@@ -2342,7 +2347,7 @@ static void process_db_drop(struct work_struct *work)
 		drain_db_fifo(adap, dbfifo_drain_delay);
 		enable_dbs(adap);
 		notify_rdma_uld(adap, CXGB4_CONTROL_DB_EMPTY);
-	} else {
+	} else if (is_t5(adap->params.chip)) {
 		u32 dropped_db = t4_read_reg(adap, 0x010ac);
 		u16 qid = (dropped_db >> 15) & 0x1ffff;
 		u16 pidx_inc = dropped_db & 0x1fff;
@@ -2350,8 +2355,8 @@ static void process_db_drop(struct work_struct *work)
 		unsigned int bar2_qid;
 		int ret;
 
-		ret = cxgb4_t4_bar2_sge_qregs(adap, qid, T4_BAR2_QTYPE_EGRESS,
-					&bar2_qoffset, &bar2_qid);
+		ret = t4_bar2_sge_qregs(adap, qid, T4_BAR2_QTYPE_EGRESS,
+					0, &bar2_qoffset, &bar2_qid);
 		if (ret)
 			dev_err(adap->pdev_dev, "doorbell drop recovery: "
 				"qid=%d, pidx_inc=%d\n", qid, pidx_inc);
@@ -2363,7 +2368,8 @@ static void process_db_drop(struct work_struct *work)
 		t4_set_reg_field(adap, 0x10b0, 1<<15, 1<<15);
 	}
 
-	t4_set_reg_field(adap, SGE_DOORBELL_CONTROL_A, DROPPED_DB_F, 0);
+	if (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5)
+		t4_set_reg_field(adap, SGE_DOORBELL_CONTROL_A, DROPPED_DB_F, 0);
 }
 
 void t4_db_full(struct adapter *adap)
@@ -2393,7 +2399,7 @@ static void uld_attach(struct adapter *adap, unsigned int uld)
 	unsigned short i;
 
 	lli.pdev = adap->pdev;
-	lli.pf = adap->fn;
+	lli.pf = adap->pf;
 	lli.l2t = adap->l2t;
 	lli.tids = &adap->tids;
 	lli.ports = adap->port;
@@ -2432,6 +2438,7 @@ static void uld_attach(struct adapter *adap, unsigned int uld)
 	lli.max_ordird_qp = adap->params.max_ordird_qp;
 	lli.max_ird_adapter = adap->params.max_ird_adapter;
 	lli.ulptx_memwrite_dsgl = adap->params.ulptx_memwrite_dsgl;
+	lli.nodeid = dev_to_node(adap->pdev_dev);
 
 	handle = ulds[uld].add(&lli);
 	if (IS_ERR(handle)) {
@@ -2729,7 +2736,7 @@ static int cxgb_close(struct net_device *dev)
 
 	netif_tx_stop_all_queues(dev);
 	netif_carrier_off(dev);
-	return t4_enable_vi(adapter, adapter->fn, pi->viid, false, false);
+	return t4_enable_vi(adapter, adapter->pf, pi->viid, false, false);
 }
 
 /* Return an error number if the indicated filter isn't writable ...
@@ -2873,7 +2880,8 @@ static struct rtnl_link_stats64 *cxgb_get_stats(struct net_device *dev,
 		spin_unlock(&adapter->stats_lock);
 		return ns;
 	}
-	t4_get_port_stats(adapter, p->tx_chan, &stats);
+	t4_get_port_stats_offset(adapter, p->tx_chan, &stats,
+				 &p->stats_base);
 	spin_unlock(&adapter->stats_lock);
 
 	ns->tx_bytes   = stats.tx_octets;
@@ -2932,7 +2940,7 @@ static int cxgb_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
 		} else
 			return -EINVAL;
 
-		mbox = pi->adapter->fn;
+		mbox = pi->adapter->pf;
 		if (cmd == SIOCGMIIREG)
 			ret = t4_mdio_rd(pi->adapter, mbox, prtad, devad,
 					 data->reg_num, &data->val_out);
@@ -2959,7 +2967,7 @@ static int cxgb_change_mtu(struct net_device *dev, int new_mtu)
 
 	if (new_mtu < 81 || new_mtu > MAX_MTU)         /* accommodate SACK */
 		return -EINVAL;
-	ret = t4_set_rxmode(pi->adapter, pi->adapter->fn, pi->viid, new_mtu, -1,
+	ret = t4_set_rxmode(pi->adapter, pi->adapter->pf, pi->viid, new_mtu, -1,
 			    -1, -1, -1, true);
 	if (!ret)
 		dev->mtu = new_mtu;
@@ -2975,7 +2983,7 @@ static int cxgb_set_mac_addr(struct net_device *dev, void *p)
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
-	ret = t4_change_mac(pi->adapter, pi->adapter->fn, pi->viid,
+	ret = t4_change_mac(pi->adapter, pi->adapter->pf, pi->viid,
 			    pi->xact_addr_filt, addr->sa_data, true, true);
 	if (ret < 0)
 		return ret;
@@ -3034,86 +3042,11 @@ void t4_fatal_err(struct adapter *adap)
 	dev_alert(adap->pdev_dev, "encountered fatal error, adapter stopped\n");
 }
 
-/* Return the specified PCI-E Configuration Space register from our Physical
- * Function.  We try first via a Firmware LDST Command since we prefer to let
- * the firmware own all of these registers, but if that fails we go for it
- * directly ourselves.
- */
-static u32 t4_read_pcie_cfg4(struct adapter *adap, int reg)
-{
-	struct fw_ldst_cmd ldst_cmd;
-	u32 val;
-	int ret;
-
-	/* Construct and send the Firmware LDST Command to retrieve the
-	 * specified PCI-E Configuration Space register.
-	 */
-	memset(&ldst_cmd, 0, sizeof(ldst_cmd));
-	ldst_cmd.op_to_addrspace =
-		htonl(FW_CMD_OP_V(FW_LDST_CMD) |
-		      FW_CMD_REQUEST_F |
-		      FW_CMD_READ_F |
-		      FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_FUNC_PCIE));
-	ldst_cmd.cycles_to_len16 = htonl(FW_LEN16(ldst_cmd));
-	ldst_cmd.u.pcie.select_naccess = FW_LDST_CMD_NACCESS_V(1);
-	ldst_cmd.u.pcie.ctrl_to_fn =
-		(FW_LDST_CMD_LC_F | FW_LDST_CMD_FN_V(adap->fn));
-	ldst_cmd.u.pcie.r = reg;
-	ret = t4_wr_mbox(adap, adap->mbox, &ldst_cmd, sizeof(ldst_cmd),
-			 &ldst_cmd);
-
-	/* If the LDST Command suucceeded, exctract the returned register
-	 * value.  Otherwise read it directly ourself.
-	 */
-	if (ret == 0)
-		val = ntohl(ldst_cmd.u.pcie.data[0]);
-	else
-		t4_hw_pci_read_cfg4(adap, reg, &val);
-
-	return val;
-}
-
 static void setup_memwin(struct adapter *adap)
 {
-	u32 mem_win0_base, mem_win1_base, mem_win2_base, mem_win2_aperture;
+	u32 nic_win_base = t4_get_util_window(adap);
 
-	if (is_t4(adap->params.chip)) {
-		u32 bar0;
-
-		/* Truncation intentional: we only read the bottom 32-bits of
-		 * the 64-bit BAR0/BAR1 ...  We use the hardware backdoor
-		 * mechanism to read BAR0 instead of using
-		 * pci_resource_start() because we could be operating from
-		 * within a Virtual Machine which is trapping our accesses to
-		 * our Configuration Space and we need to set up the PCI-E
-		 * Memory Window decoders with the actual addresses which will
-		 * be coming across the PCI-E link.
-		 */
-		bar0 = t4_read_pcie_cfg4(adap, PCI_BASE_ADDRESS_0);
-		bar0 &= PCI_BASE_ADDRESS_MEM_MASK;
-		adap->t4_bar0 = bar0;
-
-		mem_win0_base = bar0 + MEMWIN0_BASE;
-		mem_win1_base = bar0 + MEMWIN1_BASE;
-		mem_win2_base = bar0 + MEMWIN2_BASE;
-		mem_win2_aperture = MEMWIN2_APERTURE;
-	} else {
-		/* For T5, only relative offset inside the PCIe BAR is passed */
-		mem_win0_base = MEMWIN0_BASE;
-		mem_win1_base = MEMWIN1_BASE;
-		mem_win2_base = MEMWIN2_BASE_T5;
-		mem_win2_aperture = MEMWIN2_APERTURE_T5;
-	}
-	t4_write_reg(adap, PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN_A, 0),
-		     mem_win0_base | BIR_V(0) |
-		     WINDOW_V(ilog2(MEMWIN0_APERTURE) - 10));
-	t4_write_reg(adap, PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN_A, 1),
-		     mem_win1_base | BIR_V(0) |
-		     WINDOW_V(ilog2(MEMWIN1_APERTURE) - 10));
-	t4_write_reg(adap, PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN_A, 2),
-		     mem_win2_base | BIR_V(0) |
-		     WINDOW_V(ilog2(mem_win2_aperture) - 10));
-	t4_read_reg(adap, PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN_A, 2));
+	t4_setup_memwin(adap, nic_win_base, MEMWIN_NIC);
 }
 
 static void setup_memwin_rdma(struct adapter *adap)
@@ -3147,7 +3080,7 @@ static int adap_init1(struct adapter *adap, struct fw_caps_config_cmd *c)
 	c->op_to_write = htonl(FW_CMD_OP_V(FW_CAPS_CONFIG_CMD) |
 			       FW_CMD_REQUEST_F | FW_CMD_READ_F);
 	c->cfvalid_to_len16 = htonl(FW_LEN16(*c));
-	ret = t4_wr_mbox(adap, adap->fn, c, sizeof(*c), c);
+	ret = t4_wr_mbox(adap, adap->mbox, c, sizeof(*c), c);
 	if (ret < 0)
 		return ret;
 
@@ -3163,18 +3096,18 @@ static int adap_init1(struct adapter *adap, struct fw_caps_config_cmd *c)
 	}
 	c->op_to_write = htonl(FW_CMD_OP_V(FW_CAPS_CONFIG_CMD) |
 			       FW_CMD_REQUEST_F | FW_CMD_WRITE_F);
-	ret = t4_wr_mbox(adap, adap->fn, c, sizeof(*c), NULL);
+	ret = t4_wr_mbox(adap, adap->mbox, c, sizeof(*c), NULL);
 	if (ret < 0)
 		return ret;
 
-	ret = t4_config_glbl_rss(adap, adap->fn,
+	ret = t4_config_glbl_rss(adap, adap->pf,
 				 FW_RSS_GLB_CONFIG_CMD_MODE_BASICVIRTUAL,
 				 FW_RSS_GLB_CONFIG_CMD_TNLMAPEN_F |
 				 FW_RSS_GLB_CONFIG_CMD_TNLALLLKP_F);
 	if (ret < 0)
 		return ret;
 
-	ret = t4_cfg_pfvf(adap, adap->fn, adap->fn, 0, adap->sge.egr_sz, 64,
+	ret = t4_cfg_pfvf(adap, adap->mbox, adap->pf, 0, adap->sge.egr_sz, 64,
 			  MAX_INGQ, 0, 0, 4, 0xf, 0xf, 16, FW_CMD_CAP_PF,
 			  FW_CMD_CAP_PF);
 	if (ret < 0)
@@ -3218,7 +3151,7 @@ static int adap_init1(struct adapter *adap, struct fw_caps_config_cmd *c)
 	}
 
 	/* get basic stuff going */
-	return t4_early_init(adap, adap->fn);
+	return t4_early_init(adap, adap->pf);
 }
 
 /*
@@ -3274,6 +3207,142 @@ static int adap_init0_tweaks(struct adapter *adapter)
 	return 0;
 }
 
+/* 10Gb/s-BT PHY Support. chip-external 10Gb/s-BT PHYs are complex chips
+ * unto themselves and they contain their own firmware to perform their
+ * tasks ...
+ */
+static int phy_aq1202_version(const u8 *phy_fw_data,
+			      size_t phy_fw_size)
+{
+	int offset;
+
+	/* At offset 0x8 you're looking for the primary image's
+	 * starting offset which is 3 Bytes wide
+	 *
+	 * At offset 0xa of the primary image, you look for the offset
+	 * of the DRAM segment which is 3 Bytes wide.
+	 *
+	 * The FW version is at offset 0x27e of the DRAM and is 2 Bytes
+	 * wide
+	 */
+	#define be16(__p) (((__p)[0] << 8) | (__p)[1])
+	#define le16(__p) ((__p)[0] | ((__p)[1] << 8))
+	#define le24(__p) (le16(__p) | ((__p)[2] << 16))
+
+	offset = le24(phy_fw_data + 0x8) << 12;
+	offset = le24(phy_fw_data + offset + 0xa);
+	return be16(phy_fw_data + offset + 0x27e);
+
+	#undef be16
+	#undef le16
+	#undef le24
+}
+
+static struct info_10gbt_phy_fw {
+	unsigned int phy_fw_id;		/* PCI Device ID */
+	char *phy_fw_file;		/* /lib/firmware/ PHY Firmware file */
+	int (*phy_fw_version)(const u8 *phy_fw_data, size_t phy_fw_size);
+	int phy_flash;			/* Has FLASH for PHY Firmware */
+} phy_info_array[] = {
+	{
+		PHY_AQ1202_DEVICEID,
+		PHY_AQ1202_FIRMWARE,
+		phy_aq1202_version,
+		1,
+	},
+	{
+		PHY_BCM84834_DEVICEID,
+		PHY_BCM84834_FIRMWARE,
+		NULL,
+		0,
+	},
+	{ 0, NULL, NULL },
+};
+
+static struct info_10gbt_phy_fw *find_phy_info(int devid)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(phy_info_array); i++) {
+		if (phy_info_array[i].phy_fw_id == devid)
+			return &phy_info_array[i];
+	}
+	return NULL;
+}
+
+/* Handle updating of chip-external 10Gb/s-BT PHY firmware.  This needs to
+ * happen after the FW_RESET_CMD but before the FW_INITIALIZE_CMD.  On error
+ * we return a negative error number.  If we transfer new firmware we return 1
+ * (from t4_load_phy_fw()).  If we don't do anything we return 0.
+ */
+static int adap_init0_phy(struct adapter *adap)
+{
+	const struct firmware *phyf;
+	int ret;
+	struct info_10gbt_phy_fw *phy_info;
+
+	/* Use the device ID to determine which PHY file to flash.
+	 */
+	phy_info = find_phy_info(adap->pdev->device);
+	if (!phy_info) {
+		dev_warn(adap->pdev_dev,
+			 "No PHY Firmware file found for this PHY\n");
+		return -EOPNOTSUPP;
+	}
+
+	/* If we have a T4 PHY firmware file under /lib/firmware/cxgb4/, then
+	 * use that. The adapter firmware provides us with a memory buffer
+	 * where we can load a PHY firmware file from the host if we want to
+	 * override the PHY firmware File in flash.
+	 */
+	ret = request_firmware_direct(&phyf, phy_info->phy_fw_file,
+				      adap->pdev_dev);
+	if (ret < 0) {
+		/* For adapters without FLASH attached to PHY for their
+		 * firmware, it's obviously a fatal error if we can't get the
+		 * firmware to the adapter.  For adapters with PHY firmware
+		 * FLASH storage, it's worth a warning if we can't find the
+		 * PHY Firmware but we'll neuter the error ...
+		 */
+		dev_err(adap->pdev_dev, "unable to find PHY Firmware image "
+			"/lib/firmware/%s, error %d\n",
+			phy_info->phy_fw_file, -ret);
+		if (phy_info->phy_flash) {
+			int cur_phy_fw_ver = 0;
+
+			t4_phy_fw_ver(adap, &cur_phy_fw_ver);
+			dev_warn(adap->pdev_dev, "continuing with, on-adapter "
+				 "FLASH copy, version %#x\n", cur_phy_fw_ver);
+			ret = 0;
+		}
+
+		return ret;
+	}
+
+	/* Load PHY Firmware onto adapter.
+	 */
+	ret = t4_load_phy_fw(adap, MEMWIN_NIC, &adap->win0_lock,
+			     phy_info->phy_fw_version,
+			     (u8 *)phyf->data, phyf->size);
+	if (ret < 0)
+		dev_err(adap->pdev_dev, "PHY Firmware transfer error %d\n",
+			-ret);
+	else if (ret > 0) {
+		int new_phy_fw_ver = 0;
+
+		if (phy_info->phy_fw_version)
+			new_phy_fw_ver = phy_info->phy_fw_version(phyf->data,
+								  phyf->size);
+		dev_info(adap->pdev_dev, "Successfully transferred PHY "
+			 "Firmware /lib/firmware/%s, version %#x\n",
+			 phy_info->phy_fw_file, new_phy_fw_ver);
+	}
+
+	release_firmware(phyf);
+
+	return ret;
+}
+
 /*
  * Attempt to initialize the adapter via a Firmware Configuration File.
  */
@@ -3298,6 +3367,16 @@ static int adap_init0_config(struct adapter *adapter, int reset)
 			goto bye;
 	}
 
+	/* If this is a 10Gb/s-BT adapter make sure the chip-external
+	 * 10Gb/s-BT PHYs have up-to-date firmware.  Note that this step needs
+	 * to be performed after any global adapter RESET above since some
+	 * PHYs only have local RAM copies of the PHY firmware.
+	 */
+	if (is_10gbt_device(adapter->pdev->device)) {
+		ret = adap_init0_phy(adapter);
+		if (ret < 0)
+			goto bye;
+	}
 	/*
 	 * If we have a T4 configuration file under /lib/firmware/cxgb4/,
 	 * then use that.  Otherwise, use the configuration file stored
@@ -3310,6 +3389,9 @@ static int adap_init0_config(struct adapter *adapter, int reset)
 	case CHELSIO_T5:
 		fw_config_file = FW5_CFNAME;
 		break;
+	case CHELSIO_T6:
+		fw_config_file = FW6_CFNAME;
+		break;
 	default:
 		dev_err(adapter->pdev_dev, "Device %d is not supported\n",
 		       adapter->pdev->device);
@@ -3335,7 +3417,7 @@ static int adap_init0_config(struct adapter *adapter, int reset)
 			params[0] = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
 			     FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_CF));
 			ret = t4_query_params(adapter, adapter->mbox,
-					      adapter->fn, 0, 1, params, val);
+					      adapter->pf, 0, 1, params, val);
 			if (ret == 0) {
 				/*
 				 * For t4_memory_rw() below addresses and
@@ -3506,7 +3588,24 @@ static struct fw_info fw_info_array[] = {
 			.intfver_iscsi = FW_INTFVER(T5, ISCSI),
 			.intfver_fcoe = FW_INTFVER(T5, FCOE),
 		},
+	}, {
+		.chip = CHELSIO_T6,
+		.fs_name = FW6_CFNAME,
+		.fw_mod_name = FW6_FNAME,
+		.fw_hdr = {
+			.chip = FW_HDR_CHIP_T6,
+			.fw_ver = __cpu_to_be32(FW_VERSION(T6)),
+			.intfver_nic = FW_INTFVER(T6, NIC),
+			.intfver_vnic = FW_INTFVER(T6, VNIC),
+			.intfver_ofld = FW_INTFVER(T6, OFLD),
+			.intfver_ri = FW_INTFVER(T6, RI),
+			.intfver_iscsipdu = FW_INTFVER(T6, ISCSIPDU),
+			.intfver_iscsi = FW_INTFVER(T6, ISCSI),
+			.intfver_fcoepdu = FW_INTFVER(T6, FCOEPDU),
+			.intfver_fcoe = FW_INTFVER(T6, FCOE),
+		},
 	}
+
 };
 
 static struct fw_info *find_fw_info(int chip)
@@ -3612,7 +3711,7 @@ static int adap_init0(struct adapter *adap)
 	 * the firmware.  On the other hand, we need these fairly early on
 	 * so we do this right after getting ahold of the firmware.
 	 */
-	ret = get_vpd_params(adap, &adap->params.vpd);
+	ret = t4_get_vpd_params(adap, &adap->params.vpd);
 	if (ret < 0)
 		goto bye;
 
@@ -3624,7 +3723,7 @@ static int adap_init0(struct adapter *adap)
 	v =
 	    FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
 	    FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_PORTVEC);
-	ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 1, &v, &port_vec);
+	ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 1, &v, &port_vec);
 	if (ret < 0)
 		goto bye;
 
@@ -3647,7 +3746,7 @@ static int adap_init0(struct adapter *adap)
 		 */
 		params[0] = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
 			     FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_CF));
-		ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 1,
+		ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 1,
 				      params, val);
 
 		/* If the firmware doesn't support Configuration Files,
@@ -3706,7 +3805,7 @@ static int adap_init0(struct adapter *adap)
 	params[3] = FW_PARAM_PFVF(FILTER_START);
 	params[4] = FW_PARAM_PFVF(FILTER_END);
 	params[5] = FW_PARAM_PFVF(IQFLINT_START);
-	ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 6, params, val);
+	ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 6, params, val);
 	if (ret < 0)
 		goto bye;
 	adap->sge.egr_start = val[0];
@@ -3724,7 +3823,7 @@ static int adap_init0(struct adapter *adap)
 	 */
 	params[0] = FW_PARAM_PFVF(EQ_END);
 	params[1] = FW_PARAM_PFVF(IQFLINT_END);
-	ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2, params, val);
+	ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 2, params, val);
 	if (ret < 0)
 		goto bye;
 	adap->sge.egr_sz = val[0] - adap->sge.egr_start + 1;
@@ -3745,7 +3844,7 @@ static int adap_init0(struct adapter *adap)
 	}
 
 	/* Allocate the memory for the vaious egress queue bitmaps
-	 * ie starving_fl and txq_maperr.
+	 * ie starving_fl, txq_maperr and blocked_fl.
 	 */
 	adap->sge.starving_fl =	kcalloc(BITS_TO_LONGS(adap->sge.egr_sz),
 					sizeof(long), GFP_KERNEL);
@@ -3761,9 +3860,18 @@ static int adap_init0(struct adapter *adap)
 		goto bye;
 	}
 
+#ifdef CONFIG_DEBUG_FS
+	adap->sge.blocked_fl = kcalloc(BITS_TO_LONGS(adap->sge.egr_sz),
+				       sizeof(long), GFP_KERNEL);
+	if (!adap->sge.blocked_fl) {
+		ret = -ENOMEM;
+		goto bye;
+	}
+#endif
+
 	params[0] = FW_PARAM_PFVF(CLIP_START);
 	params[1] = FW_PARAM_PFVF(CLIP_END);
-	ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2, params, val);
+	ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 2, params, val);
 	if (ret < 0)
 		goto bye;
 	adap->clipt_start = val[0];
@@ -3772,7 +3880,7 @@ static int adap_init0(struct adapter *adap)
 	/* query params related to active filter region */
 	params[0] = FW_PARAM_PFVF(ACTIVE_FILTER_START);
 	params[1] = FW_PARAM_PFVF(ACTIVE_FILTER_END);
-	ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2, params, val);
+	ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 2, params, val);
 	/* If Active filter size is set we enable establishing
 	 * offload connection through firmware work request
 	 */
@@ -3789,7 +3897,7 @@ static int adap_init0(struct adapter *adap)
 	 */
 	params[0] = FW_PARAM_PFVF(CPLFW4MSG_ENCAP);
 	val[0] = 1;
-	(void) t4_set_params(adap, adap->mbox, adap->fn, 0, 1, params, val);
+	(void)t4_set_params(adap, adap->mbox, adap->pf, 0, 1, params, val);
 
 	/*
 	 * Find out whether we're allowed to use the T5+ ULPTX MEMWRITE DSGL
@@ -3801,7 +3909,7 @@ static int adap_init0(struct adapter *adap)
 		adap->params.ulptx_memwrite_dsgl = false;
 	} else {
 		params[0] = FW_PARAM_DEV(ULPTX_MEMWRITE_DSGL);
-		ret = t4_query_params(adap, adap->mbox, adap->fn, 0,
+		ret = t4_query_params(adap, adap->mbox, adap->pf, 0,
 				      1, params, val);
 		adap->params.ulptx_memwrite_dsgl = (ret == 0 && val[0] != 0);
 	}
@@ -3827,7 +3935,7 @@ static int adap_init0(struct adapter *adap)
 		params[3] = FW_PARAM_PFVF(TDDP_START);
 		params[4] = FW_PARAM_PFVF(TDDP_END);
 		params[5] = FW_PARAM_DEV(FLOWC_BUFFIFO_SZ);
-		ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 6,
+		ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 6,
 				      params, val);
 		if (ret < 0)
 			goto bye;
@@ -3865,7 +3973,7 @@ static int adap_init0(struct adapter *adap)
 		params[3] = FW_PARAM_PFVF(RQ_END);
 		params[4] = FW_PARAM_PFVF(PBL_START);
 		params[5] = FW_PARAM_PFVF(PBL_END);
-		ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 6,
+		ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 6,
 				      params, val);
 		if (ret < 0)
 			goto bye;
@@ -3882,7 +3990,7 @@ static int adap_init0(struct adapter *adap)
 		params[3] = FW_PARAM_PFVF(CQ_END);
 		params[4] = FW_PARAM_PFVF(OCQ_START);
 		params[5] = FW_PARAM_PFVF(OCQ_END);
-		ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 6, params,
+		ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 6, params,
 				      val);
 		if (ret < 0)
 			goto bye;
@@ -3895,7 +4003,7 @@ static int adap_init0(struct adapter *adap)
 
 		params[0] = FW_PARAM_DEV(MAXORDIRD_QP);
 		params[1] = FW_PARAM_DEV(MAXIRD_ADAPTER);
-		ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2, params,
+		ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 2, params,
 				      val);
 		if (ret < 0) {
 			adap->params.max_ordird_qp = 8;
@@ -3913,7 +4021,7 @@ static int adap_init0(struct adapter *adap)
 	if (caps_cmd.iscsicaps) {
 		params[0] = FW_PARAM_PFVF(ISCSI_START);
 		params[1] = FW_PARAM_PFVF(ISCSI_END);
-		ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2,
+		ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 2,
 				      params, val);
 		if (ret < 0)
 			goto bye;
@@ -3959,8 +4067,8 @@ static int adap_init0(struct adapter *adap)
 			     adap->params.b_wnd);
 	}
 	t4_init_sge_params(adap);
-	t4_init_tp_params(adap);
 	adap->flags |= FW_OK;
+	t4_init_tp_params(adap);
 	return 0;
 
 	/*
@@ -3973,6 +4081,9 @@ bye:
 	kfree(adap->sge.ingr_map);
 	kfree(adap->sge.starving_fl);
 	kfree(adap->sge.txq_maperr);
+#ifdef CONFIG_DEBUG_FS
+	kfree(adap->sge.blocked_fl);
+#endif
 	if (ret != -ETIMEDOUT && ret != -EIO)
 		t4_fw_bye(adap, adap->mbox);
 	return ret;
@@ -4040,7 +4151,7 @@ static pci_ers_result_t eeh_slot_reset(struct pci_dev *pdev)
 
 	if (t4_wait_dev_ready(adap->regs) < 0)
 		return PCI_ERS_RESULT_DISCONNECT;
-	if (t4_fw_hello(adap, adap->fn, adap->fn, MASTER_MUST, NULL) < 0)
+	if (t4_fw_hello(adap, adap->mbox, adap->pf, MASTER_MUST, NULL) < 0)
 		return PCI_ERS_RESULT_DISCONNECT;
 	adap->flags |= FW_OK;
 	if (adap_init1(adap, &c))
@@ -4049,7 +4160,7 @@ static pci_ers_result_t eeh_slot_reset(struct pci_dev *pdev)
 	for_each_port(adap, i) {
 		struct port_info *p = adap2pinfo(adap, i);
 
-		ret = t4_alloc_vi(adap, adap->fn, p->tx_chan, adap->fn, 0, 1,
+		ret = t4_alloc_vi(adap, adap->mbox, p->tx_chan, adap->pf, 0, 1,
 				  NULL, NULL);
 		if (ret < 0)
 			return PCI_ERS_RESULT_DISCONNECT;
@@ -4340,7 +4451,12 @@ static int enable_msix(struct adapter *adap)
 
 static int init_rss(struct adapter *adap)
 {
-	unsigned int i, j;
+	unsigned int i;
+	int err;
+
+	err = t4_init_rss_mode(adap, adap->mbox);
+	if (err)
+		return err;
 
 	for_each_port(adap, i) {
 		struct port_info *pi = adap2pinfo(adap, i);
@@ -4348,8 +4464,6 @@ static int init_rss(struct adapter *adap)
 		pi->rss = kcalloc(pi->rss_size, sizeof(u16), GFP_KERNEL);
 		if (!pi->rss)
 			return -ENOMEM;
-		for (j = 0; j < pi->rss_size; j++)
-			pi->rss[j] = ethtool_rxfh_indir_default(j, pi->nqsets);
 	}
 	return 0;
 }
@@ -4413,15 +4527,23 @@ static void free_some_resources(struct adapter *adapter)
 	kfree(adapter->sge.ingr_map);
 	kfree(adapter->sge.starving_fl);
 	kfree(adapter->sge.txq_maperr);
+#ifdef CONFIG_DEBUG_FS
+	kfree(adapter->sge.blocked_fl);
+#endif
 	disable_msi(adapter);
 
 	for_each_port(adapter, i)
 		if (adapter->port[i]) {
+			struct port_info *pi = adap2pinfo(adapter, i);
+
+			if (pi->viid != 0)
+				t4_free_vi(adapter, adapter->mbox, adapter->pf,
+					   0, pi->viid);
 			kfree(adap2pinfo(adapter, i)->rss);
 			free_netdev(adapter->port[i]);
 		}
 	if (adapter->flags & FW_OK)
-		t4_fw_bye(adapter, adapter->fn);
+		t4_fw_bye(adapter, adapter->pf);
 }
 
 #define TSO_FLAGS (NETIF_F_TSO | NETIF_F_TSO6 | NETIF_F_TSO_ECN)
@@ -4512,7 +4634,7 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	adapter->pdev = pdev;
 	adapter->pdev_dev = &pdev->dev;
 	adapter->mbox = func;
-	adapter->fn = func;
+	adapter->pf = func;
 	adapter->msg_enable = dflt_msg_enable;
 	memset(adapter->chan_map, 0xff, sizeof(adapter->chan_map));
 
@@ -4532,7 +4654,7 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (!is_t4(adapter->params.chip)) {
 		s_qpp = (QUEUESPERPAGEPF0_S +
 			(QUEUESPERPAGEPF1_S - QUEUESPERPAGEPF0_S) *
-			adapter->fn);
+			adapter->pf);
 		qpp = 1 << QUEUESPERPAGEPF0_G(t4_read_reg(adapter,
 		      SGE_EGRESS_QUEUES_PER_PAGE_PF_A) >> s_qpp);
 		num_seg = PAGE_SIZE / SEGMENT_SIZE;
@@ -4555,10 +4677,15 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			err = -ENOMEM;
 			goto out_free_adapter;
 		}
+		t4_write_reg(adapter, SGE_STAT_CFG_A,
+			     STATSOURCE_T5_V(7) | STATMODE_V(0));
 	}
 
 	setup_memwin(adapter);
 	err = adap_init0(adapter);
+#ifdef CONFIG_DEBUG_FS
+	bitmap_zero(adapter->sge.blocked_fl, adapter->sge.egr_sz);
+#endif
 	setup_memwin_rdma(adapter);
 	if (err)
 		goto out_unmap_bar;
@@ -4607,10 +4734,25 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		err = t4_port_init(adapter, func, func, 0);
 		if (err)
 			goto out_free_dev;
+	} else if (adapter->params.nports == 1) {
+		/* If we don't have a connection to the firmware -- possibly
+		 * because of an error -- grab the raw VPD parameters so we
+		 * can set the proper MAC Address on the debug network
+		 * interface that we've created.
+		 */
+		u8 hw_addr[ETH_ALEN];
+		u8 *na = adapter->params.vpd.na;
+
+		err = t4_get_raw_vpd_params(adapter, &adapter->params.vpd);
+		if (!err) {
+			for (i = 0; i < ETH_ALEN; i++)
+				hw_addr[i] = (hex2val(na[2 * i + 0]) * 16 +
+					      hex2val(na[2 * i + 1]));
+			t4_set_hw_addr(adapter, 0, hw_addr);
+		}
 	}
 
-	/*
-	 * Configure queues and allocate tables now, they can be needed as
+	/* Configure queues and allocate tables now, they can be needed as
 	 * soon as the first register_netdev completes.
 	 */
 	cfg_queues(adapter);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
index 78ab4d406ce2..b27897d4f787 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
@@ -264,6 +264,7 @@ struct cxgb4_lld_info {
 	unsigned int max_ordird_qp;          /* Max ORD/IRD depth per RDMA QP */
 	unsigned int max_ird_adapter;        /* Max IRD memory per adapter */
 	bool ulptx_memwrite_dsgl;            /* use of T5 DSGL allowed */
+	int nodeid;			     /* device numa node id */
 };
 
 struct cxgb4_uld_info {
@@ -297,8 +298,6 @@ struct sk_buff *cxgb4_pktgl_to_skb(const struct pkt_gl *gl,
 				   unsigned int skb_len, unsigned int pull_len);
 int cxgb4_sync_txq_pidx(struct net_device *dev, u16 qid, u16 pidx, u16 size);
 int cxgb4_flush_eq_cache(struct net_device *dev);
-void cxgb4_disable_db_coalescing(struct net_device *dev);
-void cxgb4_enable_db_coalescing(struct net_device *dev);
 int cxgb4_read_tpte(struct net_device *dev, u32 stag, __be32 *tpte);
 u64 cxgb4_read_sge_timestamp(struct net_device *dev);
 
@@ -306,6 +305,7 @@ enum cxgb4_bar2_qtype { CXGB4_BAR2_QTYPE_EGRESS, CXGB4_BAR2_QTYPE_INGRESS };
 int cxgb4_bar2_sge_qregs(struct net_device *dev,
 			 unsigned int qid,
 			 enum cxgb4_bar2_qtype qtype,
+			 int user,
 			 u64 *pbar2_qoffset,
 			 unsigned int *pbar2_qid);
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index 0d2eddab04ef..942db078f33a 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -100,16 +100,6 @@
  */
 #define TX_QCHECK_PERIOD (HZ / 2)
 
-/* SGE Hung Ingress DMA Threshold Warning time (in Hz) and Warning Repeat Rate
- * (in RX_QCHECK_PERIOD multiples).  If we find one of the SGE Ingress DMA
- * State Machines in the same state for this amount of time (in HZ) then we'll
- * issue a warning about a potential hang.  We'll repeat the warning as the
- * SGE Ingress DMA Channel appears to be hung every N RX_QCHECK_PERIODs till
- * the situation clears.  If the situation clears, we'll note that as well.
- */
-#define SGE_IDMA_WARN_THRESH (1 * HZ)
-#define SGE_IDMA_WARN_REPEAT (20 * RX_QCHECK_PERIOD)
-
 /*
  * Max number of Tx descriptors to be reclaimed by the Tx timer.
  */
@@ -532,14 +522,17 @@ static void unmap_rx_buf(struct adapter *adap, struct sge_fl *q)
 
 static inline void ring_fl_db(struct adapter *adap, struct sge_fl *q)
 {
-	u32 val;
 	if (q->pend_cred >= 8) {
+		u32 val = adap->params.arch.sge_fl_db;
+
 		if (is_t4(adap->params.chip))
-			val = PIDX_V(q->pend_cred / 8);
+			val |= PIDX_V(q->pend_cred / 8);
 		else
-			val = PIDX_T5_V(q->pend_cred / 8) |
-				DBTYPE_F;
-		val |= DBPRIO_F;
+			val |= PIDX_T5_V(q->pend_cred / 8);
+
+		/* Make sure all memory writes to the Free List queue are
+		 * committed before we tell the hardware about them.
+		 */
 		wmb();
 
 		/* If we don't have access to the new User Doorbell (T5+), use
@@ -594,6 +587,11 @@ static unsigned int refill_fl(struct adapter *adap, struct sge_fl *q, int n,
 	struct rx_sw_desc *sd = &q->sdesc[q->pidx];
 	int node;
 
+#ifdef CONFIG_DEBUG_FS
+	if (test_bit(q->cntxt_id - adap->sge.egr_start, adap->sge.blocked_fl))
+		goto out;
+#endif
+
 	gfp |= __GFP_NOWARN;
 	node = dev_to_node(adap->pdev_dev);
 
@@ -930,7 +928,10 @@ static void cxgb_pio_copy(u64 __iomem *dst, u64 *src)
  */
 static inline void ring_tx_db(struct adapter *adap, struct sge_txq *q, int n)
 {
-	wmb();            /* write descriptors before telling HW */
+	/* Make sure that all writes to the TX Descriptors are committed
+	 * before we tell the hardware about them.
+	 */
+	wmb();
 
 	/* If we don't have access to the new User Doorbell (T5+), use the old
 	 * doorbell mechanism; otherwise use the new BAR2 mechanism.
@@ -1032,7 +1033,7 @@ static void inline_tx_skb(const struct sk_buff *skb, const struct sge_txq *q,
  * Figure out what HW csum a packet wants and return the appropriate control
  * bits.
  */
-static u64 hwcsum(const struct sk_buff *skb)
+static u64 hwcsum(enum chip_type chip, const struct sk_buff *skb)
 {
 	int csum_type;
 	const struct iphdr *iph = ip_hdr(skb);
@@ -1047,7 +1048,7 @@ nocsum:			/*
 			 * unknown protocol, disable HW csum
 			 * and hope a bad packet is detected
 			 */
-			return TXPKT_L4CSUM_DIS;
+			return TXPKT_L4CSUM_DIS_F;
 		}
 	} else {
 		/*
@@ -1063,15 +1064,21 @@ nocsum:			/*
 			goto nocsum;
 	}
 
-	if (likely(csum_type >= TX_CSUM_TCPIP))
-		return TXPKT_CSUM_TYPE(csum_type) |
-			TXPKT_IPHDR_LEN(skb_network_header_len(skb)) |
-			TXPKT_ETHHDR_LEN(skb_network_offset(skb) - ETH_HLEN);
-	else {
+	if (likely(csum_type >= TX_CSUM_TCPIP)) {
+		u64 hdr_len = TXPKT_IPHDR_LEN_V(skb_network_header_len(skb));
+		int eth_hdr_len = skb_network_offset(skb) - ETH_HLEN;
+
+		if (CHELSIO_CHIP_VERSION(chip) <= CHELSIO_T5)
+			hdr_len |= TXPKT_ETHHDR_LEN_V(eth_hdr_len);
+		else
+			hdr_len |= T6_TXPKT_ETHHDR_LEN_V(eth_hdr_len);
+		return TXPKT_CSUM_TYPE_V(csum_type) | hdr_len;
+	} else {
 		int start = skb_transport_offset(skb);
 
-		return TXPKT_CSUM_TYPE(csum_type) | TXPKT_CSUM_START(start) |
-			TXPKT_CSUM_LOC(start + skb->csum_offset);
+		return TXPKT_CSUM_TYPE_V(csum_type) |
+			TXPKT_CSUM_START_V(start) |
+			TXPKT_CSUM_LOC_V(start + skb->csum_offset);
 	}
 }
 
@@ -1112,11 +1119,11 @@ cxgb_fcoe_offload(struct sk_buff *skb, struct adapter *adap,
 		return -ENOTSUPP;
 
 	/* FC CRC offload */
-	*cntrl = TXPKT_CSUM_TYPE(TX_CSUM_FCOE) |
-		     TXPKT_L4CSUM_DIS | TXPKT_IPCSUM_DIS |
-		     TXPKT_CSUM_START(CXGB_FCOE_TXPKT_CSUM_START) |
-		     TXPKT_CSUM_END(CXGB_FCOE_TXPKT_CSUM_END) |
-		     TXPKT_CSUM_LOC(CXGB_FCOE_TXPKT_CSUM_END);
+	*cntrl = TXPKT_CSUM_TYPE_V(TX_CSUM_FCOE) |
+		     TXPKT_L4CSUM_DIS_F | TXPKT_IPCSUM_DIS_F |
+		     TXPKT_CSUM_START_V(CXGB_FCOE_TXPKT_CSUM_START) |
+		     TXPKT_CSUM_END_V(CXGB_FCOE_TXPKT_CSUM_END) |
+		     TXPKT_CSUM_LOC_V(CXGB_FCOE_TXPKT_CSUM_END);
 	return 0;
 }
 #endif /* CONFIG_CHELSIO_T4_FCOE */
@@ -1130,7 +1137,6 @@ cxgb_fcoe_offload(struct sk_buff *skb, struct adapter *adap,
  */
 netdev_tx_t t4_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 {
-	int len;
 	u32 wr_mid;
 	u64 cntrl, *end;
 	int qidx, credits;
@@ -1143,6 +1149,7 @@ netdev_tx_t t4_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 	const struct skb_shared_info *ssi;
 	dma_addr_t addr[MAX_SKB_FRAGS + 1];
 	bool immediate = false;
+	int len, max_pkt_len;
 #ifdef CONFIG_CHELSIO_T4_FCOE
 	int err;
 #endif /* CONFIG_CHELSIO_T4_FCOE */
@@ -1156,13 +1163,20 @@ out_free:	dev_kfree_skb_any(skb);
 		return NETDEV_TX_OK;
 	}
 
+	/* Discard the packet if the length is greater than mtu */
+	max_pkt_len = ETH_HLEN + dev->mtu;
+	if (skb_vlan_tag_present(skb))
+		max_pkt_len += VLAN_HLEN;
+	if (!skb_shinfo(skb)->gso_size && (unlikely(skb->len > max_pkt_len)))
+		goto out_free;
+
 	pi = netdev_priv(dev);
 	adap = pi->adapter;
 	qidx = skb_get_queue_mapping(skb);
 	q = &adap->sge.ethtxq[qidx + pi->first_qset];
 
 	reclaim_completed_tx(adap, &q->q, true);
-	cntrl = TXPKT_L4CSUM_DIS | TXPKT_IPCSUM_DIS;
+	cntrl = TXPKT_L4CSUM_DIS_F | TXPKT_IPCSUM_DIS_F;
 
 #ifdef CONFIG_CHELSIO_T4_FCOE
 	err = cxgb_fcoe_offload(skb, adap, pi, &cntrl);
@@ -1213,23 +1227,29 @@ out_free:	dev_kfree_skb_any(skb);
 		len += sizeof(*lso);
 		wr->op_immdlen = htonl(FW_WR_OP_V(FW_ETH_TX_PKT_WR) |
 				       FW_WR_IMMDLEN_V(len));
-		lso->c.lso_ctrl = htonl(LSO_OPCODE(CPL_TX_PKT_LSO) |
-					LSO_FIRST_SLICE | LSO_LAST_SLICE |
-					LSO_IPV6(v6) |
-					LSO_ETHHDR_LEN(eth_xtra_len / 4) |
-					LSO_IPHDR_LEN(l3hdr_len / 4) |
-					LSO_TCPHDR_LEN(tcp_hdr(skb)->doff));
+		lso->c.lso_ctrl = htonl(LSO_OPCODE_V(CPL_TX_PKT_LSO) |
+					LSO_FIRST_SLICE_F | LSO_LAST_SLICE_F |
+					LSO_IPV6_V(v6) |
+					LSO_ETHHDR_LEN_V(eth_xtra_len / 4) |
+					LSO_IPHDR_LEN_V(l3hdr_len / 4) |
+					LSO_TCPHDR_LEN_V(tcp_hdr(skb)->doff));
 		lso->c.ipid_ofst = htons(0);
 		lso->c.mss = htons(ssi->gso_size);
 		lso->c.seqno_offset = htonl(0);
 		if (is_t4(adap->params.chip))
 			lso->c.len = htonl(skb->len);
 		else
-			lso->c.len = htonl(LSO_T5_XFER_SIZE(skb->len));
+			lso->c.len = htonl(LSO_T5_XFER_SIZE_V(skb->len));
 		cpl = (void *)(lso + 1);
-		cntrl = TXPKT_CSUM_TYPE(v6 ? TX_CSUM_TCPIP6 : TX_CSUM_TCPIP) |
-			TXPKT_IPHDR_LEN(l3hdr_len) |
-			TXPKT_ETHHDR_LEN(eth_xtra_len);
+
+		if (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5)
+			cntrl =	TXPKT_ETHHDR_LEN_V(eth_xtra_len);
+		else
+			cntrl = T6_TXPKT_ETHHDR_LEN_V(eth_xtra_len);
+
+		cntrl |= TXPKT_CSUM_TYPE_V(v6 ?
+					   TX_CSUM_TCPIP6 : TX_CSUM_TCPIP) |
+			 TXPKT_IPHDR_LEN_V(l3hdr_len);
 		q->tso++;
 		q->tx_cso += ssi->gso_segs;
 	} else {
@@ -1238,23 +1258,25 @@ out_free:	dev_kfree_skb_any(skb);
 				       FW_WR_IMMDLEN_V(len));
 		cpl = (void *)(wr + 1);
 		if (skb->ip_summed == CHECKSUM_PARTIAL) {
-			cntrl = hwcsum(skb) | TXPKT_IPCSUM_DIS;
+			cntrl = hwcsum(adap->params.chip, skb) |
+				TXPKT_IPCSUM_DIS_F;
 			q->tx_cso++;
 		}
 	}
 
 	if (skb_vlan_tag_present(skb)) {
 		q->vlan_ins++;
-		cntrl |= TXPKT_VLAN_VLD | TXPKT_VLAN(skb_vlan_tag_get(skb));
+		cntrl |= TXPKT_VLAN_VLD_F | TXPKT_VLAN_V(skb_vlan_tag_get(skb));
 #ifdef CONFIG_CHELSIO_T4_FCOE
 		if (skb->protocol == htons(ETH_P_FCOE))
-			cntrl |= TXPKT_VLAN(
+			cntrl |= TXPKT_VLAN_V(
 				 ((skb->priority & 0x7) << VLAN_PRIO_SHIFT));
 #endif /* CONFIG_CHELSIO_T4_FCOE */
 	}
 
-	cpl->ctrl0 = htonl(TXPKT_OPCODE(CPL_TX_PKT_XT) |
-			   TXPKT_INTF(pi->tx_chan) | TXPKT_PF(adap->fn));
+	cpl->ctrl0 = htonl(TXPKT_OPCODE_V(CPL_TX_PKT_XT) |
+			   TXPKT_INTF_V(pi->tx_chan) |
+			   TXPKT_PF_V(adap->pf));
 	cpl->pack = htons(0);
 	cpl->len = htons(skb->len);
 	cpl->ctrl1 = cpu_to_be64(cntrl);
@@ -1964,7 +1986,7 @@ static void restore_rx_bufs(const struct pkt_gl *si, struct sge_fl *q,
 static inline bool is_new_response(const struct rsp_ctrl *r,
 				   const struct sge_rspq *q)
 {
-	return RSPD_GEN(r->type_gen) == q->gen;
+	return (r->type_gen >> RSPD_GEN_S) == q->gen;
 }
 
 /**
@@ -2011,19 +2033,19 @@ static int process_responses(struct sge_rspq *q, int budget)
 			break;
 
 		dma_rmb();
-		rsp_type = RSPD_TYPE(rc->type_gen);
-		if (likely(rsp_type == RSP_TYPE_FLBUF)) {
+		rsp_type = RSPD_TYPE_G(rc->type_gen);
+		if (likely(rsp_type == RSPD_TYPE_FLBUF_X)) {
 			struct page_frag *fp;
 			struct pkt_gl si;
 			const struct rx_sw_desc *rsd;
 			u32 len = ntohl(rc->pldbuflen_qid), bufsz, frags;
 
-			if (len & RSPD_NEWBUF) {
+			if (len & RSPD_NEWBUF_F) {
 				if (likely(q->offset > 0)) {
 					free_rx_bufs(q->adap, &rxq->fl, 1);
 					q->offset = 0;
 				}
-				len = RSPD_LEN(len);
+				len = RSPD_LEN_G(len);
 			}
 			si.tot_len = len;
 
@@ -2058,7 +2080,7 @@ static int process_responses(struct sge_rspq *q, int budget)
 				q->offset += ALIGN(fp->size, s->fl_align);
 			else
 				restore_rx_bufs(&si, &rxq->fl, frags);
-		} else if (likely(rsp_type == RSP_TYPE_CPL)) {
+		} else if (likely(rsp_type == RSPD_TYPE_CPL_X)) {
 			ret = q->handler(q, q->cur_desc, NULL);
 		} else {
 			ret = q->handler(q, (const __be64 *)rc, CXGB4_MSG_AN);
@@ -2066,7 +2088,7 @@ static int process_responses(struct sge_rspq *q, int budget)
 
 		if (unlikely(ret)) {
 			/* couldn't process descriptor, back off for recovery */
-			q->next_intr_params = QINTR_TIMER_IDX(NOMEM_TMR_IDX);
+			q->next_intr_params = QINTR_TIMER_IDX_V(NOMEM_TMR_IDX);
 			break;
 		}
 
@@ -2090,7 +2112,7 @@ int cxgb_busy_poll(struct napi_struct *napi)
 		return LL_FLUSH_BUSY;
 
 	work_done = process_responses(q, 4);
-	params = QINTR_TIMER_IDX(TIMERREG_COUNTER0_X) | QINTR_CNT_EN;
+	params = QINTR_TIMER_IDX_V(TIMERREG_COUNTER0_X) | QINTR_CNT_EN_V(1);
 	q->next_intr_params = params;
 	val = CIDXINC_V(work_done) | SEINTARM_V(params);
 
@@ -2137,7 +2159,7 @@ static int napi_rx_handler(struct napi_struct *napi, int budget)
 		int timer_index;
 
 		napi_complete(napi);
-		timer_index = QINTR_TIMER_IDX_GET(q->next_intr_params);
+		timer_index = QINTR_TIMER_IDX_G(q->next_intr_params);
 
 		if (q->adaptive_rx) {
 			if (work_done > max(timer_pkt_quota[timer_index],
@@ -2147,15 +2169,16 @@ static int napi_rx_handler(struct napi_struct *napi, int budget)
 				timer_index = timer_index - 1;
 
 			timer_index = clamp(timer_index, 0, SGE_TIMERREGS - 1);
-			q->next_intr_params = QINTR_TIMER_IDX(timer_index) |
-							      V_QINTR_CNT_EN;
+			q->next_intr_params =
+					QINTR_TIMER_IDX_V(timer_index) |
+					QINTR_CNT_EN_V(0);
 			params = q->next_intr_params;
 		} else {
 			params = q->next_intr_params;
 			q->next_intr_params = q->intr_params;
 		}
 	} else
-		params = QINTR_TIMER_IDX(7);
+		params = QINTR_TIMER_IDX_V(7);
 
 	val = CIDXINC_V(work_done) | SEINTARM_V(params);
 
@@ -2203,7 +2226,7 @@ static unsigned int process_intrq(struct adapter *adap)
 			break;
 
 		dma_rmb();
-		if (RSPD_TYPE(rc->type_gen) == RSP_TYPE_INTR) {
+		if (RSPD_TYPE_G(rc->type_gen) == RSPD_TYPE_INTR_X) {
 			unsigned int qid = ntohl(rc->pldbuflen_qid);
 
 			qid -= adap->sge.ingr_start;
@@ -2279,7 +2302,7 @@ irq_handler_t t4_intr_handler(struct adapter *adap)
 static void sge_rx_timer_cb(unsigned long data)
 {
 	unsigned long m;
-	unsigned int i, idma_same_state_cnt[2];
+	unsigned int i;
 	struct adapter *adap = (struct adapter *)data;
 	struct sge *s = &adap->sge;
 
@@ -2300,67 +2323,16 @@ static void sge_rx_timer_cb(unsigned long data)
 					set_bit(id, s->starving_fl);
 			}
 		}
+	/* The remainder of the SGE RX Timer Callback routine is dedicated to
+	 * global Master PF activities like checking for chip ingress stalls,
+	 * etc.
+	 */
+	if (!(adap->flags & MASTER_PF))
+		goto done;
 
-	t4_write_reg(adap, SGE_DEBUG_INDEX_A, 13);
-	idma_same_state_cnt[0] = t4_read_reg(adap, SGE_DEBUG_DATA_HIGH_A);
-	idma_same_state_cnt[1] = t4_read_reg(adap, SGE_DEBUG_DATA_LOW_A);
-
-	for (i = 0; i < 2; i++) {
-		u32 debug0, debug11;
-
-		/* If the Ingress DMA Same State Counter ("timer") is less
-		 * than 1s, then we can reset our synthesized Stall Timer and
-		 * continue.  If we have previously emitted warnings about a
-		 * potential stalled Ingress Queue, issue a note indicating
-		 * that the Ingress Queue has resumed forward progress.
-		 */
-		if (idma_same_state_cnt[i] < s->idma_1s_thresh) {
-			if (s->idma_stalled[i] >= SGE_IDMA_WARN_THRESH)
-				CH_WARN(adap, "SGE idma%d, queue%u,resumed after %d sec\n",
-					i, s->idma_qid[i],
-					s->idma_stalled[i]/HZ);
-			s->idma_stalled[i] = 0;
-			continue;
-		}
-
-		/* Synthesize an SGE Ingress DMA Same State Timer in the Hz
-		 * domain.  The first time we get here it'll be because we
-		 * passed the 1s Threshold; each additional time it'll be
-		 * because the RX Timer Callback is being fired on its regular
-		 * schedule.
-		 *
-		 * If the stall is below our Potential Hung Ingress Queue
-		 * Warning Threshold, continue.
-		 */
-		if (s->idma_stalled[i] == 0)
-			s->idma_stalled[i] = HZ;
-		else
-			s->idma_stalled[i] += RX_QCHECK_PERIOD;
-
-		if (s->idma_stalled[i] < SGE_IDMA_WARN_THRESH)
-			continue;
-
-		/* We'll issue a warning every SGE_IDMA_WARN_REPEAT Hz */
-		if (((s->idma_stalled[i] - HZ) % SGE_IDMA_WARN_REPEAT) != 0)
-			continue;
-
-		/* Read and save the SGE IDMA State and Queue ID information.
-		 * We do this every time in case it changes across time ...
-		 */
-		t4_write_reg(adap, SGE_DEBUG_INDEX_A, 0);
-		debug0 = t4_read_reg(adap, SGE_DEBUG_DATA_LOW_A);
-		s->idma_state[i] = (debug0 >> (i * 9)) & 0x3f;
-
-		t4_write_reg(adap, SGE_DEBUG_INDEX_A, 11);
-		debug11 = t4_read_reg(adap, SGE_DEBUG_DATA_LOW_A);
-		s->idma_qid[i] = (debug11 >> (i * 16)) & 0xffff;
-
-		CH_WARN(adap, "SGE idma%u, queue%u, maybe stuck state%u %dsecs (debug0=%#x, debug11=%#x)\n",
-			i, s->idma_qid[i], s->idma_state[i],
-			s->idma_stalled[i]/HZ, debug0, debug11);
-		t4_sge_decode_idma_state(adap, s->idma_state[i]);
-	}
+	t4_idma_monitor(adap, &s->idma_monitor, HZ, RX_QCHECK_PERIOD);
 
+done:
 	mod_timer(&s->rx_timer, jiffies + RX_QCHECK_PERIOD);
 }
 
@@ -2429,7 +2401,7 @@ static void __iomem *bar2_address(struct adapter *adapter,
 	u64 bar2_qoffset;
 	int ret;
 
-	ret = cxgb4_t4_bar2_sge_qregs(adapter, qid, qtype,
+	ret = t4_bar2_sge_qregs(adapter, qid, qtype, 0,
 				&bar2_qoffset, pbar2_qid);
 	if (ret)
 		return NULL;
@@ -2437,9 +2409,12 @@ static void __iomem *bar2_address(struct adapter *adapter,
 	return adapter->bar2 + bar2_qoffset;
 }
 
+/* @intr_idx: MSI/MSI-X vector if >=0, -(absolute qid + 1) if < 0
+ * @cong: < 0 -> no congestion feedback, >= 0 -> congestion channel map
+ */
 int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 		     struct net_device *dev, int intr_idx,
-		     struct sge_fl *fl, rspq_handler_t hnd)
+		     struct sge_fl *fl, rspq_handler_t hnd, int cong)
 {
 	int ret, flsz = 0;
 	struct fw_iq_cmd c;
@@ -2457,12 +2432,13 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 	memset(&c, 0, sizeof(c));
 	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_IQ_CMD) | FW_CMD_REQUEST_F |
 			    FW_CMD_WRITE_F | FW_CMD_EXEC_F |
-			    FW_IQ_CMD_PFN_V(adap->fn) | FW_IQ_CMD_VFN_V(0));
+			    FW_IQ_CMD_PFN_V(adap->pf) | FW_IQ_CMD_VFN_V(0));
 	c.alloc_to_len16 = htonl(FW_IQ_CMD_ALLOC_F | FW_IQ_CMD_IQSTART_F |
 				 FW_LEN16(c));
 	c.type_to_iqandstindex = htonl(FW_IQ_CMD_TYPE_V(FW_IQ_TYPE_FL_INT_CAP) |
 		FW_IQ_CMD_IQASYNCH_V(fwevtq) | FW_IQ_CMD_VIID_V(pi->viid) |
-		FW_IQ_CMD_IQANDST_V(intr_idx < 0) | FW_IQ_CMD_IQANUD_V(1) |
+		FW_IQ_CMD_IQANDST_V(intr_idx < 0) |
+		FW_IQ_CMD_IQANUD_V(UPDATEDELIVERY_INTERRUPT_X) |
 		FW_IQ_CMD_IQANDSTINDEX_V(intr_idx >= 0 ? intr_idx :
 							-intr_idx - 1));
 	c.iqdroprss_to_iqesize = htons(FW_IQ_CMD_IQPCIECH_V(pi->tx_chan) |
@@ -2471,8 +2447,21 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 		FW_IQ_CMD_IQESIZE_V(ilog2(iq->iqe_len) - 4));
 	c.iqsize = htons(iq->size);
 	c.iqaddr = cpu_to_be64(iq->phys_addr);
+	if (cong >= 0)
+		c.iqns_to_fl0congen = htonl(FW_IQ_CMD_IQFLINTCONGEN_F);
 
 	if (fl) {
+		enum chip_type chip = CHELSIO_CHIP_VERSION(adap->params.chip);
+
+		/* Allocate the ring for the hardware free list (with space
+		 * for its status page) along with the associated software
+		 * descriptor ring.  The free list size needs to be a multiple
+		 * of the Egress Queue Unit and at least 2 Egress Units larger
+		 * than the SGE's Egress Congrestion Threshold
+		 * (fl_starve_thres - 1).
+		 */
+		if (fl->size < s->fl_starve_thres - 1 + 2 * 8)
+			fl->size = s->fl_starve_thres - 1 + 2 * 8;
 		fl->size = roundup(fl->size, 8);
 		fl->desc = alloc_ring(adap->pdev_dev, fl->size, sizeof(__be64),
 				      sizeof(struct rx_sw_desc), &fl->addr,
@@ -2481,17 +2470,25 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 			goto fl_nomem;
 
 		flsz = fl->size / 8 + s->stat_len / sizeof(struct tx_desc);
-		c.iqns_to_fl0congen = htonl(FW_IQ_CMD_FL0PACKEN_F |
-					    FW_IQ_CMD_FL0FETCHRO_F |
-					    FW_IQ_CMD_FL0DATARO_F |
-					    FW_IQ_CMD_FL0PADEN_F);
-		c.fl0dcaen_to_fl0cidxfthresh = htons(FW_IQ_CMD_FL0FBMIN_V(2) |
-				FW_IQ_CMD_FL0FBMAX_V(3));
+		c.iqns_to_fl0congen |= htonl(FW_IQ_CMD_FL0PACKEN_F |
+					     FW_IQ_CMD_FL0FETCHRO_F |
+					     FW_IQ_CMD_FL0DATARO_F |
+					     FW_IQ_CMD_FL0PADEN_F);
+		if (cong >= 0)
+			c.iqns_to_fl0congen |=
+				htonl(FW_IQ_CMD_FL0CNGCHMAP_V(cong) |
+				      FW_IQ_CMD_FL0CONGCIF_F |
+				      FW_IQ_CMD_FL0CONGEN_F);
+		c.fl0dcaen_to_fl0cidxfthresh =
+			htons(FW_IQ_CMD_FL0FBMIN_V(FETCHBURSTMIN_64B_X) |
+			      FW_IQ_CMD_FL0FBMAX_V((chip <= CHELSIO_T5) ?
+						   FETCHBURSTMAX_512B_X :
+						   FETCHBURSTMAX_256B_X));
 		c.fl0size = htons(flsz);
 		c.fl0addr = cpu_to_be64(fl->addr);
 	}
 
-	ret = t4_wr_mbox(adap, adap->fn, &c, sizeof(c), &c);
+	ret = t4_wr_mbox(adap, adap->mbox, &c, sizeof(c), &c);
 	if (ret)
 		goto err;
 
@@ -2532,6 +2529,41 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 					     &fl->bar2_qid);
 		refill_fl(adap, fl, fl_cap(fl), GFP_KERNEL);
 	}
+
+	/* For T5 and later we attempt to set up the Congestion Manager values
+	 * of the new RX Ethernet Queue.  This should really be handled by
+	 * firmware because it's more complex than any host driver wants to
+	 * get involved with and it's different per chip and this is almost
+	 * certainly wrong.  Firmware would be wrong as well, but it would be
+	 * a lot easier to fix in one place ...  For now we do something very
+	 * simple (and hopefully less wrong).
+	 */
+	if (!is_t4(adap->params.chip) && cong >= 0) {
+		u32 param, val;
+		int i;
+
+		param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DMAQ) |
+			 FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DMAQ_CONM_CTXT) |
+			 FW_PARAMS_PARAM_YZ_V(iq->cntxt_id));
+		if (cong == 0) {
+			val = CONMCTXT_CNGTPMODE_V(CONMCTXT_CNGTPMODE_QUEUE_X);
+		} else {
+			val =
+			    CONMCTXT_CNGTPMODE_V(CONMCTXT_CNGTPMODE_CHANNEL_X);
+			for (i = 0; i < 4; i++) {
+				if (cong & (1 << i))
+					val |=
+					     CONMCTXT_CNGCHMAP_V(1 << (i << 2));
+			}
+		}
+		ret = t4_set_params(adap, adap->mbox, adap->pf, 0, 1,
+				    &param, &val);
+		if (ret)
+			dev_warn(adap->pdev_dev, "Failed to set Congestion"
+				 " Manager Context for Ingress Queue %d: %d\n",
+				 iq->cntxt_id, -ret);
+	}
+
 	return 0;
 
 fl_nomem:
@@ -2589,23 +2621,24 @@ int t4_sge_alloc_eth_txq(struct adapter *adap, struct sge_eth_txq *txq,
 	memset(&c, 0, sizeof(c));
 	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_EQ_ETH_CMD) | FW_CMD_REQUEST_F |
 			    FW_CMD_WRITE_F | FW_CMD_EXEC_F |
-			    FW_EQ_ETH_CMD_PFN_V(adap->fn) |
+			    FW_EQ_ETH_CMD_PFN_V(adap->pf) |
 			    FW_EQ_ETH_CMD_VFN_V(0));
 	c.alloc_to_len16 = htonl(FW_EQ_ETH_CMD_ALLOC_F |
 				 FW_EQ_ETH_CMD_EQSTART_F | FW_LEN16(c));
 	c.viid_pkd = htonl(FW_EQ_ETH_CMD_AUTOEQUEQE_F |
 			   FW_EQ_ETH_CMD_VIID_V(pi->viid));
-	c.fetchszm_to_iqid = htonl(FW_EQ_ETH_CMD_HOSTFCMODE_V(2) |
-				   FW_EQ_ETH_CMD_PCIECHN_V(pi->tx_chan) |
-				   FW_EQ_ETH_CMD_FETCHRO_V(1) |
-				   FW_EQ_ETH_CMD_IQID_V(iqid));
-	c.dcaen_to_eqsize = htonl(FW_EQ_ETH_CMD_FBMIN_V(2) |
-				  FW_EQ_ETH_CMD_FBMAX_V(3) |
-				  FW_EQ_ETH_CMD_CIDXFTHRESH_V(5) |
-				  FW_EQ_ETH_CMD_EQSIZE_V(nentries));
+	c.fetchszm_to_iqid =
+		htonl(FW_EQ_ETH_CMD_HOSTFCMODE_V(HOSTFCMODE_STATUS_PAGE_X) |
+		      FW_EQ_ETH_CMD_PCIECHN_V(pi->tx_chan) |
+		      FW_EQ_ETH_CMD_FETCHRO_F | FW_EQ_ETH_CMD_IQID_V(iqid));
+	c.dcaen_to_eqsize =
+		htonl(FW_EQ_ETH_CMD_FBMIN_V(FETCHBURSTMIN_64B_X) |
+		      FW_EQ_ETH_CMD_FBMAX_V(FETCHBURSTMAX_512B_X) |
+		      FW_EQ_ETH_CMD_CIDXFTHRESH_V(CIDXFLUSHTHRESH_32_X) |
+		      FW_EQ_ETH_CMD_EQSIZE_V(nentries));
 	c.eqaddr = cpu_to_be64(txq->q.phys_addr);
 
-	ret = t4_wr_mbox(adap, adap->fn, &c, sizeof(c), &c);
+	ret = t4_wr_mbox(adap, adap->mbox, &c, sizeof(c), &c);
 	if (ret) {
 		kfree(txq->q.sdesc);
 		txq->q.sdesc = NULL;
@@ -2637,29 +2670,30 @@ int t4_sge_alloc_ctrl_txq(struct adapter *adap, struct sge_ctrl_txq *txq,
 
 	txq->q.desc = alloc_ring(adap->pdev_dev, nentries,
 				 sizeof(struct tx_desc), 0, &txq->q.phys_addr,
-				 NULL, 0, NUMA_NO_NODE);
+				 NULL, 0, dev_to_node(adap->pdev_dev));
 	if (!txq->q.desc)
 		return -ENOMEM;
 
 	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_EQ_CTRL_CMD) | FW_CMD_REQUEST_F |
 			    FW_CMD_WRITE_F | FW_CMD_EXEC_F |
-			    FW_EQ_CTRL_CMD_PFN_V(adap->fn) |
+			    FW_EQ_CTRL_CMD_PFN_V(adap->pf) |
 			    FW_EQ_CTRL_CMD_VFN_V(0));
 	c.alloc_to_len16 = htonl(FW_EQ_CTRL_CMD_ALLOC_F |
 				 FW_EQ_CTRL_CMD_EQSTART_F | FW_LEN16(c));
 	c.cmpliqid_eqid = htonl(FW_EQ_CTRL_CMD_CMPLIQID_V(cmplqid));
 	c.physeqid_pkd = htonl(0);
-	c.fetchszm_to_iqid = htonl(FW_EQ_CTRL_CMD_HOSTFCMODE_V(2) |
-				   FW_EQ_CTRL_CMD_PCIECHN_V(pi->tx_chan) |
-				   FW_EQ_CTRL_CMD_FETCHRO_F |
-				   FW_EQ_CTRL_CMD_IQID_V(iqid));
-	c.dcaen_to_eqsize = htonl(FW_EQ_CTRL_CMD_FBMIN_V(2) |
-				  FW_EQ_CTRL_CMD_FBMAX_V(3) |
-				  FW_EQ_CTRL_CMD_CIDXFTHRESH_V(5) |
-				  FW_EQ_CTRL_CMD_EQSIZE_V(nentries));
+	c.fetchszm_to_iqid =
+		htonl(FW_EQ_CTRL_CMD_HOSTFCMODE_V(HOSTFCMODE_STATUS_PAGE_X) |
+		      FW_EQ_CTRL_CMD_PCIECHN_V(pi->tx_chan) |
+		      FW_EQ_CTRL_CMD_FETCHRO_F | FW_EQ_CTRL_CMD_IQID_V(iqid));
+	c.dcaen_to_eqsize =
+		htonl(FW_EQ_CTRL_CMD_FBMIN_V(FETCHBURSTMIN_64B_X) |
+		      FW_EQ_CTRL_CMD_FBMAX_V(FETCHBURSTMAX_512B_X) |
+		      FW_EQ_CTRL_CMD_CIDXFTHRESH_V(CIDXFLUSHTHRESH_32_X) |
+		      FW_EQ_CTRL_CMD_EQSIZE_V(nentries));
 	c.eqaddr = cpu_to_be64(txq->q.phys_addr);
 
-	ret = t4_wr_mbox(adap, adap->fn, &c, sizeof(c), &c);
+	ret = t4_wr_mbox(adap, adap->mbox, &c, sizeof(c), &c);
 	if (ret) {
 		dma_free_coherent(adap->pdev_dev,
 				  nentries * sizeof(struct tx_desc),
@@ -2697,21 +2731,22 @@ int t4_sge_alloc_ofld_txq(struct adapter *adap, struct sge_ofld_txq *txq,
 	memset(&c, 0, sizeof(c));
 	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_EQ_OFLD_CMD) | FW_CMD_REQUEST_F |
 			    FW_CMD_WRITE_F | FW_CMD_EXEC_F |
-			    FW_EQ_OFLD_CMD_PFN_V(adap->fn) |
+			    FW_EQ_OFLD_CMD_PFN_V(adap->pf) |
 			    FW_EQ_OFLD_CMD_VFN_V(0));
 	c.alloc_to_len16 = htonl(FW_EQ_OFLD_CMD_ALLOC_F |
 				 FW_EQ_OFLD_CMD_EQSTART_F | FW_LEN16(c));
-	c.fetchszm_to_iqid = htonl(FW_EQ_OFLD_CMD_HOSTFCMODE_V(2) |
-				   FW_EQ_OFLD_CMD_PCIECHN_V(pi->tx_chan) |
-				   FW_EQ_OFLD_CMD_FETCHRO_F |
-				   FW_EQ_OFLD_CMD_IQID_V(iqid));
-	c.dcaen_to_eqsize = htonl(FW_EQ_OFLD_CMD_FBMIN_V(2) |
-				  FW_EQ_OFLD_CMD_FBMAX_V(3) |
-				  FW_EQ_OFLD_CMD_CIDXFTHRESH_V(5) |
-				  FW_EQ_OFLD_CMD_EQSIZE_V(nentries));
+	c.fetchszm_to_iqid =
+		htonl(FW_EQ_OFLD_CMD_HOSTFCMODE_V(HOSTFCMODE_STATUS_PAGE_X) |
+		      FW_EQ_OFLD_CMD_PCIECHN_V(pi->tx_chan) |
+		      FW_EQ_OFLD_CMD_FETCHRO_F | FW_EQ_OFLD_CMD_IQID_V(iqid));
+	c.dcaen_to_eqsize =
+		htonl(FW_EQ_OFLD_CMD_FBMIN_V(FETCHBURSTMIN_64B_X) |
+		      FW_EQ_OFLD_CMD_FBMAX_V(FETCHBURSTMAX_512B_X) |
+		      FW_EQ_OFLD_CMD_CIDXFTHRESH_V(CIDXFLUSHTHRESH_32_X) |
+		      FW_EQ_OFLD_CMD_EQSIZE_V(nentries));
 	c.eqaddr = cpu_to_be64(txq->q.phys_addr);
 
-	ret = t4_wr_mbox(adap, adap->fn, &c, sizeof(c), &c);
+	ret = t4_wr_mbox(adap, adap->mbox, &c, sizeof(c), &c);
 	if (ret) {
 		kfree(txq->q.sdesc);
 		txq->q.sdesc = NULL;
@@ -2750,7 +2785,7 @@ static void free_rspq_fl(struct adapter *adap, struct sge_rspq *rq,
 	unsigned int fl_id = fl ? fl->cntxt_id : 0xffff;
 
 	adap->sge.ingr_map[rq->cntxt_id - adap->sge.ingr_start] = NULL;
-	t4_iq_free(adap, adap->fn, adap->fn, 0, FW_IQ_TYPE_FL_INT_CAP,
+	t4_iq_free(adap, adap->mbox, adap->pf, 0, FW_IQ_TYPE_FL_INT_CAP,
 		   rq->cntxt_id, fl_id, 0xffff);
 	dma_free_coherent(adap->pdev_dev, (rq->size + 1) * rq->iqe_len,
 			  rq->desc, rq->phys_addr);
@@ -2805,7 +2840,7 @@ void t4_free_sge_resources(struct adapter *adap)
 			free_rspq_fl(adap, &eq->rspq,
 				     eq->fl.size ? &eq->fl : NULL);
 		if (etq->q.desc) {
-			t4_eth_eq_free(adap, adap->fn, adap->fn, 0,
+			t4_eth_eq_free(adap, adap->mbox, adap->pf, 0,
 				       etq->q.cntxt_id);
 			free_tx_desc(adap, &etq->q, etq->q.in_use, true);
 			kfree(etq->q.sdesc);
@@ -2824,7 +2859,7 @@ void t4_free_sge_resources(struct adapter *adap)
 
 		if (q->q.desc) {
 			tasklet_kill(&q->qresume_tsk);
-			t4_ofld_eq_free(adap, adap->fn, adap->fn, 0,
+			t4_ofld_eq_free(adap, adap->mbox, adap->pf, 0,
 					q->q.cntxt_id);
 			free_tx_desc(adap, &q->q, q->q.in_use, false);
 			kfree(q->q.sdesc);
@@ -2839,7 +2874,7 @@ void t4_free_sge_resources(struct adapter *adap)
 
 		if (cq->q.desc) {
 			tasklet_kill(&cq->qresume_tsk);
-			t4_ctrl_eq_free(adap, adap->fn, adap->fn, 0,
+			t4_ctrl_eq_free(adap, adap->mbox, adap->pf, 0,
 					cq->q.cntxt_id);
 			__skb_queue_purge(&cq->sendq);
 			free_txq(adap, &cq->q);
@@ -3023,7 +3058,11 @@ int t4_sge_init(struct adapter *adap)
 	 * Packing Boundary.  T5 introduced the ability to specify these
 	 * separately.  The actual Ingress Packet Data alignment boundary
 	 * within Packed Buffer Mode is the maximum of these two
-	 * specifications.
+	 * specifications.  (Note that it makes no real practical sense to
+	 * have the Pading Boudary be larger than the Packing Boundary but you
+	 * could set the chip up that way and, in fact, legacy T4 code would
+	 * end doing this because it would initialize the Padding Boundary and
+	 * leave the Packing Boundary initialized to 0 (16 bytes).)
 	 */
 	ingpadboundary = 1 << (INGPADBOUNDARY_G(sge_control) +
 			       INGPADBOUNDARY_SHIFT_X);
@@ -3067,11 +3106,14 @@ int t4_sge_init(struct adapter *adap)
 		egress_threshold = EGRTHRESHOLDPACKING_G(sge_conm_ctrl);
 	s->fl_starve_thres = 2*egress_threshold + 1;
 
+	t4_idma_monitor_init(adap, &s->idma_monitor);
+
+	/* Set up timers used for recuring callbacks to process RX and TX
+	 * administrative tasks.
+	 */
 	setup_timer(&s->rx_timer, sge_rx_timer_cb, (unsigned long)adap);
 	setup_timer(&s->tx_timer, sge_tx_timer_cb, (unsigned long)adap);
-	s->idma_1s_thresh = core_ticks_per_usec(adap) * 1000000;  /* 1 s */
-	s->idma_stalled[0] = 0;
-	s->idma_stalled[1] = 0;
+
 	spin_lock_init(&s->intrq_lock);
 
 	return 0;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index e8578a742f2a..2b52aae7ec86 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -150,7 +150,12 @@ void t4_write_indirect(struct adapter *adap, unsigned int addr_reg,
  */
 void t4_hw_pci_read_cfg4(struct adapter *adap, int reg, u32 *val)
 {
-	u32 req = ENABLE_F | FUNCTION_V(adap->fn) | REGISTER_V(reg);
+	u32 req = FUNCTION_V(adap->pf) | REGISTER_V(reg);
+
+	if (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5)
+		req |= ENABLE_F;
+	else
+		req |= T6_ENABLE_F;
 
 	if (is_t4(adap->params.chip))
 		req |= LOCALCFG_F;
@@ -214,8 +219,8 @@ static void fw_asrt(struct adapter *adap, u32 mbox_addr)
 	get_mbox_rpl(adap, (__be64 *)&asrt, sizeof(asrt) / 8, mbox_addr);
 	dev_alert(adap->pdev_dev,
 		  "FW assertion at %.16s:%u, val0 %#x, val1 %#x\n",
-		  asrt.u.assert.filename_0_7, ntohl(asrt.u.assert.line),
-		  ntohl(asrt.u.assert.x), ntohl(asrt.u.assert.y));
+		  asrt.u.assert.filename_0_7, be32_to_cpu(asrt.u.assert.line),
+		  be32_to_cpu(asrt.u.assert.x), be32_to_cpu(asrt.u.assert.y));
 }
 
 static void dump_mbox(struct adapter *adap, int mbox, u32 data_reg)
@@ -233,13 +238,14 @@ static void dump_mbox(struct adapter *adap, int mbox, u32 data_reg)
 }
 
 /**
- *	t4_wr_mbox_meat - send a command to FW through the given mailbox
+ *	t4_wr_mbox_meat_timeout - send a command to FW through the given mailbox
  *	@adap: the adapter
  *	@mbox: index of the mailbox to use
  *	@cmd: the command to write
  *	@size: command length in bytes
  *	@rpl: where to optionally store the reply
  *	@sleep_ok: if true we may sleep while awaiting command completion
+ *	@timeout: time to wait for command to finish before timing out
  *
  *	Sends the given command to FW through the selected mailbox and waits
  *	for the FW to execute the command.  If @rpl is not %NULL it is used to
@@ -254,8 +260,8 @@ static void dump_mbox(struct adapter *adap, int mbox, u32 data_reg)
  *	command or FW executes it but signals an error.  In the latter case
  *	the return value is the error code indicated by FW (negated).
  */
-int t4_wr_mbox_meat(struct adapter *adap, int mbox, const void *cmd, int size,
-		    void *rpl, bool sleep_ok)
+int t4_wr_mbox_meat_timeout(struct adapter *adap, int mbox, const void *cmd,
+			    int size, void *rpl, bool sleep_ok, int timeout)
 {
 	static const int delay[] = {
 		1, 1, 3, 5, 10, 10, 20, 50, 100, 200
@@ -294,7 +300,7 @@ int t4_wr_mbox_meat(struct adapter *adap, int mbox, const void *cmd, int size,
 	delay_idx = 0;
 	ms = delay[0];
 
-	for (i = 0; i < FW_CMD_MAX_TIMEOUT; i += ms) {
+	for (i = 0; i < timeout; i += ms) {
 		if (sleep_ok) {
 			ms = delay[delay_idx];  /* last element may repeat */
 			if (delay_idx < ARRAY_SIZE(delay) - 1)
@@ -332,114 +338,11 @@ int t4_wr_mbox_meat(struct adapter *adap, int mbox, const void *cmd, int size,
 	return -ETIMEDOUT;
 }
 
-/**
- *	t4_mc_read - read from MC through backdoor accesses
- *	@adap: the adapter
- *	@addr: address of first byte requested
- *	@idx: which MC to access
- *	@data: 64 bytes of data containing the requested address
- *	@ecc: where to store the corresponding 64-bit ECC word
- *
- *	Read 64 bytes of data from MC starting at a 64-byte-aligned address
- *	that covers the requested address @addr.  If @parity is not %NULL it
- *	is assigned the 64-bit ECC word for the read data.
- */
-int t4_mc_read(struct adapter *adap, int idx, u32 addr, __be32 *data, u64 *ecc)
-{
-	int i;
-	u32 mc_bist_cmd, mc_bist_cmd_addr, mc_bist_cmd_len;
-	u32 mc_bist_status_rdata, mc_bist_data_pattern;
-
-	if (is_t4(adap->params.chip)) {
-		mc_bist_cmd = MC_BIST_CMD_A;
-		mc_bist_cmd_addr = MC_BIST_CMD_ADDR_A;
-		mc_bist_cmd_len = MC_BIST_CMD_LEN_A;
-		mc_bist_status_rdata = MC_BIST_STATUS_RDATA_A;
-		mc_bist_data_pattern = MC_BIST_DATA_PATTERN_A;
-	} else {
-		mc_bist_cmd = MC_REG(MC_P_BIST_CMD_A, idx);
-		mc_bist_cmd_addr = MC_REG(MC_P_BIST_CMD_ADDR_A, idx);
-		mc_bist_cmd_len = MC_REG(MC_P_BIST_CMD_LEN_A, idx);
-		mc_bist_status_rdata = MC_REG(MC_P_BIST_STATUS_RDATA_A, idx);
-		mc_bist_data_pattern = MC_REG(MC_P_BIST_DATA_PATTERN_A, idx);
-	}
-
-	if (t4_read_reg(adap, mc_bist_cmd) & START_BIST_F)
-		return -EBUSY;
-	t4_write_reg(adap, mc_bist_cmd_addr, addr & ~0x3fU);
-	t4_write_reg(adap, mc_bist_cmd_len, 64);
-	t4_write_reg(adap, mc_bist_data_pattern, 0xc);
-	t4_write_reg(adap, mc_bist_cmd, BIST_OPCODE_V(1) | START_BIST_F |
-		     BIST_CMD_GAP_V(1));
-	i = t4_wait_op_done(adap, mc_bist_cmd, START_BIST_F, 0, 10, 1);
-	if (i)
-		return i;
-
-#define MC_DATA(i) MC_BIST_STATUS_REG(mc_bist_status_rdata, i)
-
-	for (i = 15; i >= 0; i--)
-		*data++ = htonl(t4_read_reg(adap, MC_DATA(i)));
-	if (ecc)
-		*ecc = t4_read_reg64(adap, MC_DATA(16));
-#undef MC_DATA
-	return 0;
-}
-
-/**
- *	t4_edc_read - read from EDC through backdoor accesses
- *	@adap: the adapter
- *	@idx: which EDC to access
- *	@addr: address of first byte requested
- *	@data: 64 bytes of data containing the requested address
- *	@ecc: where to store the corresponding 64-bit ECC word
- *
- *	Read 64 bytes of data from EDC starting at a 64-byte-aligned address
- *	that covers the requested address @addr.  If @parity is not %NULL it
- *	is assigned the 64-bit ECC word for the read data.
- */
-int t4_edc_read(struct adapter *adap, int idx, u32 addr, __be32 *data, u64 *ecc)
+int t4_wr_mbox_meat(struct adapter *adap, int mbox, const void *cmd, int size,
+		    void *rpl, bool sleep_ok)
 {
-	int i;
-	u32 edc_bist_cmd, edc_bist_cmd_addr, edc_bist_cmd_len;
-	u32 edc_bist_cmd_data_pattern, edc_bist_status_rdata;
-
-	if (is_t4(adap->params.chip)) {
-		edc_bist_cmd = EDC_REG(EDC_BIST_CMD_A, idx);
-		edc_bist_cmd_addr = EDC_REG(EDC_BIST_CMD_ADDR_A, idx);
-		edc_bist_cmd_len = EDC_REG(EDC_BIST_CMD_LEN_A, idx);
-		edc_bist_cmd_data_pattern = EDC_REG(EDC_BIST_DATA_PATTERN_A,
-						    idx);
-		edc_bist_status_rdata = EDC_REG(EDC_BIST_STATUS_RDATA_A,
-						idx);
-	} else {
-		edc_bist_cmd = EDC_REG_T5(EDC_H_BIST_CMD_A, idx);
-		edc_bist_cmd_addr = EDC_REG_T5(EDC_H_BIST_CMD_ADDR_A, idx);
-		edc_bist_cmd_len = EDC_REG_T5(EDC_H_BIST_CMD_LEN_A, idx);
-		edc_bist_cmd_data_pattern =
-			EDC_REG_T5(EDC_H_BIST_DATA_PATTERN_A, idx);
-		edc_bist_status_rdata =
-			 EDC_REG_T5(EDC_H_BIST_STATUS_RDATA_A, idx);
-	}
-
-	if (t4_read_reg(adap, edc_bist_cmd) & START_BIST_F)
-		return -EBUSY;
-	t4_write_reg(adap, edc_bist_cmd_addr, addr & ~0x3fU);
-	t4_write_reg(adap, edc_bist_cmd_len, 64);
-	t4_write_reg(adap, edc_bist_cmd_data_pattern, 0xc);
-	t4_write_reg(adap, edc_bist_cmd,
-		     BIST_OPCODE_V(1) | BIST_CMD_GAP_V(1) | START_BIST_F);
-	i = t4_wait_op_done(adap, edc_bist_cmd, START_BIST_F, 0, 10, 1);
-	if (i)
-		return i;
-
-#define EDC_DATA(i) (EDC_BIST_STATUS_REG(edc_bist_status_rdata, i))
-
-	for (i = 15; i >= 0; i--)
-		*data++ = htonl(t4_read_reg(adap, EDC_DATA(i)));
-	if (ecc)
-		*ecc = t4_read_reg64(adap, EDC_DATA(16));
-#undef EDC_DATA
-	return 0;
+	return t4_wr_mbox_meat_timeout(adap, mbox, cmd, size, rpl, sleep_ok,
+				       FW_CMD_MAX_TIMEOUT);
 }
 
 /**
@@ -483,9 +386,8 @@ int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
 	/* Offset into the region of memory which is being accessed
 	 * MEM_EDC0 = 0
 	 * MEM_EDC1 = 1
-	 * MEM_MC   = 2 -- T4
-	 * MEM_MC0  = 2 -- For T5
-	 * MEM_MC1  = 3 -- For T5
+	 * MEM_MC   = 2 -- MEM_MC for chips with only 1 memory controller
+	 * MEM_MC1  = 3 -- for chips with 2 memory controllers (e.g. T5)
 	 */
 	edc_size  = EDRAM0_SIZE_G(t4_read_reg(adap, MA_EDRAM0_BAR_A));
 	if (mtype != MEM_MC1)
@@ -514,7 +416,7 @@ int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
 	mem_base = PCIEOFST_G(mem_reg) << PCIEOFST_SHIFT_X;
 	if (is_t4(adap->params.chip))
 		mem_base -= adap->t4_bar0;
-	win_pf = is_t4(adap->params.chip) ? 0 : PFNUM_V(adap->fn);
+	win_pf = is_t4(adap->params.chip) ? 0 : PFNUM_V(adap->pf);
 
 	/* Calculate our initial PCI-E Memory Window Position and Offset into
 	 * that Window.
@@ -625,6 +527,102 @@ int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
 	return 0;
 }
 
+/* Return the specified PCI-E Configuration Space register from our Physical
+ * Function.  We try first via a Firmware LDST Command since we prefer to let
+ * the firmware own all of these registers, but if that fails we go for it
+ * directly ourselves.
+ */
+u32 t4_read_pcie_cfg4(struct adapter *adap, int reg)
+{
+	u32 val, ldst_addrspace;
+
+	/* If fw_attach != 0, construct and send the Firmware LDST Command to
+	 * retrieve the specified PCI-E Configuration Space register.
+	 */
+	struct fw_ldst_cmd ldst_cmd;
+	int ret;
+
+	memset(&ldst_cmd, 0, sizeof(ldst_cmd));
+	ldst_addrspace = FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_FUNC_PCIE);
+	ldst_cmd.op_to_addrspace = cpu_to_be32(FW_CMD_OP_V(FW_LDST_CMD) |
+					       FW_CMD_REQUEST_F |
+					       FW_CMD_READ_F |
+					       ldst_addrspace);
+	ldst_cmd.cycles_to_len16 = cpu_to_be32(FW_LEN16(ldst_cmd));
+	ldst_cmd.u.pcie.select_naccess = FW_LDST_CMD_NACCESS_V(1);
+	ldst_cmd.u.pcie.ctrl_to_fn =
+		(FW_LDST_CMD_LC_F | FW_LDST_CMD_FN_V(adap->pf));
+	ldst_cmd.u.pcie.r = reg;
+
+	/* If the LDST Command succeeds, return the result, otherwise
+	 * fall through to reading it directly ourselves ...
+	 */
+	ret = t4_wr_mbox(adap, adap->mbox, &ldst_cmd, sizeof(ldst_cmd),
+			 &ldst_cmd);
+	if (ret == 0)
+		val = be32_to_cpu(ldst_cmd.u.pcie.data[0]);
+	else
+		/* Read the desired Configuration Space register via the PCI-E
+		 * Backdoor mechanism.
+		 */
+		t4_hw_pci_read_cfg4(adap, reg, &val);
+	return val;
+}
+
+/* Get the window based on base passed to it.
+ * Window aperture is currently unhandled, but there is no use case for it
+ * right now
+ */
+static u32 t4_get_window(struct adapter *adap, u32 pci_base, u64 pci_mask,
+			 u32 memwin_base)
+{
+	u32 ret;
+
+	if (is_t4(adap->params.chip)) {
+		u32 bar0;
+
+		/* Truncation intentional: we only read the bottom 32-bits of
+		 * the 64-bit BAR0/BAR1 ...  We use the hardware backdoor
+		 * mechanism to read BAR0 instead of using
+		 * pci_resource_start() because we could be operating from
+		 * within a Virtual Machine which is trapping our accesses to
+		 * our Configuration Space and we need to set up the PCI-E
+		 * Memory Window decoders with the actual addresses which will
+		 * be coming across the PCI-E link.
+		 */
+		bar0 = t4_read_pcie_cfg4(adap, pci_base);
+		bar0 &= pci_mask;
+		adap->t4_bar0 = bar0;
+
+		ret = bar0 + memwin_base;
+	} else {
+		/* For T5, only relative offset inside the PCIe BAR is passed */
+		ret = memwin_base;
+	}
+	return ret;
+}
+
+/* Get the default utility window (win0) used by everyone */
+u32 t4_get_util_window(struct adapter *adap)
+{
+	return t4_get_window(adap, PCI_BASE_ADDRESS_0,
+			     PCI_BASE_ADDRESS_MEM_MASK, MEMWIN0_BASE);
+}
+
+/* Set up memory window for accessing adapter memory ranges.  (Read
+ * back MA register to ensure that changes propagate before we attempt
+ * to use the new values.)
+ */
+void t4_setup_memwin(struct adapter *adap, u32 memwin_base, u32 window)
+{
+	t4_write_reg(adap,
+		     PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN_A, window),
+		     memwin_base | BIR_V(0) |
+		     WINDOW_V(ilog2(MEMWIN0_APERTURE) - WINDOW_SHIFT_X));
+	t4_read_reg(adap,
+		    PCIE_MEM_ACCESS_REG(PCIE_MEM_ACCESS_BASE_WIN_A, window));
+}
+
 /**
  *	t4_get_regs_len - return the size of the chips register set
  *	@adapter: the adapter
@@ -640,6 +638,7 @@ unsigned int t4_get_regs_len(struct adapter *adapter)
 		return T4_REGMAP_SIZE;
 
 	case CHELSIO_T5:
+	case CHELSIO_T6:
 		return T5_REGMAP_SIZE;
 	}
 
@@ -666,7 +665,8 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x11fc, 0x123c,
 		0x1300, 0x173c,
 		0x1800, 0x18fc,
-		0x3000, 0x30d8,
+		0x3000, 0x305c,
+		0x3068, 0x30d8,
 		0x30e0, 0x5924,
 		0x5960, 0x59d4,
 		0x5a00, 0x5af8,
@@ -729,7 +729,7 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x19238, 0x1924c,
 		0x193f8, 0x19474,
 		0x19490, 0x194f8,
-		0x19800, 0x19f30,
+		0x19800, 0x19f4c,
 		0x1a000, 0x1a06c,
 		0x1a0b0, 0x1a120,
 		0x1a128, 0x1a138,
@@ -878,7 +878,7 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x27780, 0x2778c,
 		0x27800, 0x27c38,
 		0x27c80, 0x27d7c,
-		0x27e00, 0x27e04
+		0x27e00, 0x27e04,
 	};
 
 	static const unsigned int t5_reg_ranges[] = {
@@ -888,7 +888,7 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x1280, 0x173c,
 		0x1800, 0x18fc,
 		0x3000, 0x3028,
-		0x3060, 0x30d8,
+		0x3068, 0x30d8,
 		0x30e0, 0x30fc,
 		0x3140, 0x357c,
 		0x35a8, 0x35cc,
@@ -900,7 +900,7 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x5940, 0x59dc,
 		0x59fc, 0x5a18,
 		0x5a60, 0x5a9c,
-		0x5b9c, 0x5bfc,
+		0x5b94, 0x5bfc,
 		0x6000, 0x6040,
 		0x6058, 0x614c,
 		0x7700, 0x7798,
@@ -1014,27 +1014,30 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x30800, 0x30834,
 		0x308c0, 0x30908,
 		0x30910, 0x309ac,
-		0x30a00, 0x30a04,
-		0x30a0c, 0x30a2c,
+		0x30a00, 0x30a2c,
 		0x30a44, 0x30a50,
 		0x30a74, 0x30c24,
+		0x30d00, 0x30d00,
 		0x30d08, 0x30d14,
 		0x30d1c, 0x30d20,
 		0x30d3c, 0x30d50,
 		0x31200, 0x3120c,
 		0x31220, 0x31220,
 		0x31240, 0x31240,
-		0x31600, 0x31600,
-		0x31608, 0x3160c,
+		0x31600, 0x3160c,
 		0x31a00, 0x31a1c,
-		0x31e04, 0x31e20,
+		0x31e00, 0x31e20,
 		0x31e38, 0x31e3c,
 		0x31e80, 0x31e80,
 		0x31e88, 0x31ea8,
 		0x31eb0, 0x31eb4,
 		0x31ec8, 0x31ed4,
 		0x31fb8, 0x32004,
-		0x32208, 0x3223c,
+		0x32200, 0x32200,
+		0x32208, 0x32240,
+		0x32248, 0x32280,
+		0x32288, 0x322c0,
+		0x322c8, 0x322fc,
 		0x32600, 0x32630,
 		0x32a00, 0x32abc,
 		0x32b00, 0x32b70,
@@ -1074,27 +1077,30 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x34800, 0x34834,
 		0x348c0, 0x34908,
 		0x34910, 0x349ac,
-		0x34a00, 0x34a04,
-		0x34a0c, 0x34a2c,
+		0x34a00, 0x34a2c,
 		0x34a44, 0x34a50,
 		0x34a74, 0x34c24,
+		0x34d00, 0x34d00,
 		0x34d08, 0x34d14,
 		0x34d1c, 0x34d20,
 		0x34d3c, 0x34d50,
 		0x35200, 0x3520c,
 		0x35220, 0x35220,
 		0x35240, 0x35240,
-		0x35600, 0x35600,
-		0x35608, 0x3560c,
+		0x35600, 0x3560c,
 		0x35a00, 0x35a1c,
-		0x35e04, 0x35e20,
+		0x35e00, 0x35e20,
 		0x35e38, 0x35e3c,
 		0x35e80, 0x35e80,
 		0x35e88, 0x35ea8,
 		0x35eb0, 0x35eb4,
 		0x35ec8, 0x35ed4,
 		0x35fb8, 0x36004,
-		0x36208, 0x3623c,
+		0x36200, 0x36200,
+		0x36208, 0x36240,
+		0x36248, 0x36280,
+		0x36288, 0x362c0,
+		0x362c8, 0x362fc,
 		0x36600, 0x36630,
 		0x36a00, 0x36abc,
 		0x36b00, 0x36b70,
@@ -1134,27 +1140,30 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x38800, 0x38834,
 		0x388c0, 0x38908,
 		0x38910, 0x389ac,
-		0x38a00, 0x38a04,
-		0x38a0c, 0x38a2c,
+		0x38a00, 0x38a2c,
 		0x38a44, 0x38a50,
 		0x38a74, 0x38c24,
+		0x38d00, 0x38d00,
 		0x38d08, 0x38d14,
 		0x38d1c, 0x38d20,
 		0x38d3c, 0x38d50,
 		0x39200, 0x3920c,
 		0x39220, 0x39220,
 		0x39240, 0x39240,
-		0x39600, 0x39600,
-		0x39608, 0x3960c,
+		0x39600, 0x3960c,
 		0x39a00, 0x39a1c,
-		0x39e04, 0x39e20,
+		0x39e00, 0x39e20,
 		0x39e38, 0x39e3c,
 		0x39e80, 0x39e80,
 		0x39e88, 0x39ea8,
 		0x39eb0, 0x39eb4,
 		0x39ec8, 0x39ed4,
 		0x39fb8, 0x3a004,
-		0x3a208, 0x3a23c,
+		0x3a200, 0x3a200,
+		0x3a208, 0x3a240,
+		0x3a248, 0x3a280,
+		0x3a288, 0x3a2c0,
+		0x3a2c8, 0x3a2fc,
 		0x3a600, 0x3a630,
 		0x3aa00, 0x3aabc,
 		0x3ab00, 0x3ab70,
@@ -1194,27 +1203,30 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x3c800, 0x3c834,
 		0x3c8c0, 0x3c908,
 		0x3c910, 0x3c9ac,
-		0x3ca00, 0x3ca04,
-		0x3ca0c, 0x3ca2c,
+		0x3ca00, 0x3ca2c,
 		0x3ca44, 0x3ca50,
 		0x3ca74, 0x3cc24,
+		0x3cd00, 0x3cd00,
 		0x3cd08, 0x3cd14,
 		0x3cd1c, 0x3cd20,
 		0x3cd3c, 0x3cd50,
 		0x3d200, 0x3d20c,
 		0x3d220, 0x3d220,
 		0x3d240, 0x3d240,
-		0x3d600, 0x3d600,
-		0x3d608, 0x3d60c,
+		0x3d600, 0x3d60c,
 		0x3da00, 0x3da1c,
-		0x3de04, 0x3de20,
+		0x3de00, 0x3de20,
 		0x3de38, 0x3de3c,
 		0x3de80, 0x3de80,
 		0x3de88, 0x3dea8,
 		0x3deb0, 0x3deb4,
 		0x3dec8, 0x3ded4,
 		0x3dfb8, 0x3e004,
-		0x3e208, 0x3e23c,
+		0x3e200, 0x3e200,
+		0x3e208, 0x3e240,
+		0x3e248, 0x3e280,
+		0x3e288, 0x3e2c0,
+		0x3e2c8, 0x3e2fc,
 		0x3e600, 0x3e630,
 		0x3ea00, 0x3eabc,
 		0x3eb00, 0x3eb70,
@@ -1247,7 +1259,7 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x3fcf0, 0x3fcfc,
 		0x40000, 0x4000c,
 		0x40040, 0x40068,
-		0x40080, 0x40144,
+		0x4007c, 0x40144,
 		0x40180, 0x4018c,
 		0x40200, 0x40298,
 		0x402ac, 0x4033c,
@@ -1275,7 +1287,7 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x47800, 0x47814,
 		0x48000, 0x4800c,
 		0x48040, 0x48068,
-		0x48080, 0x48144,
+		0x4807c, 0x48144,
 		0x48180, 0x4818c,
 		0x48200, 0x48298,
 		0x482ac, 0x4833c,
@@ -1309,6 +1321,344 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x51300, 0x51308,
 	};
 
+	static const unsigned int t6_reg_ranges[] = {
+		0x1008, 0x114c,
+		0x1180, 0x11b4,
+		0x11fc, 0x1250,
+		0x1280, 0x133c,
+		0x1800, 0x18fc,
+		0x3000, 0x302c,
+		0x3060, 0x30d8,
+		0x30e0, 0x30fc,
+		0x3140, 0x357c,
+		0x35a8, 0x35cc,
+		0x35ec, 0x35ec,
+		0x3600, 0x5624,
+		0x56cc, 0x575c,
+		0x580c, 0x5814,
+		0x5890, 0x58bc,
+		0x5940, 0x595c,
+		0x5980, 0x598c,
+		0x59b0, 0x59dc,
+		0x59fc, 0x5a18,
+		0x5a60, 0x5a6c,
+		0x5a80, 0x5a9c,
+		0x5b94, 0x5bfc,
+		0x5c10, 0x5ec0,
+		0x5ec8, 0x5ec8,
+		0x6000, 0x6040,
+		0x6058, 0x6154,
+		0x7700, 0x7798,
+		0x77c0, 0x7880,
+		0x78cc, 0x78fc,
+		0x7b00, 0x7c54,
+		0x7d00, 0x7efc,
+		0x8dc0, 0x8de0,
+		0x8df8, 0x8e84,
+		0x8ea0, 0x8f88,
+		0x8fb8, 0x911c,
+		0x9400, 0x9470,
+		0x9600, 0x971c,
+		0x9800, 0x9808,
+		0x9820, 0x983c,
+		0x9850, 0x9864,
+		0x9c00, 0x9c6c,
+		0x9c80, 0x9cec,
+		0x9d00, 0x9d6c,
+		0x9d80, 0x9dec,
+		0x9e00, 0x9e6c,
+		0x9e80, 0x9eec,
+		0x9f00, 0x9f6c,
+		0x9f80, 0xa020,
+		0xd004, 0xd03c,
+		0xdfc0, 0xdfe0,
+		0xe000, 0xf008,
+		0x11000, 0x11014,
+		0x11048, 0x11110,
+		0x11118, 0x1117c,
+		0x11190, 0x11260,
+		0x11300, 0x1130c,
+		0x12000, 0x1205c,
+		0x19040, 0x1906c,
+		0x19078, 0x19080,
+		0x1908c, 0x19124,
+		0x19150, 0x191b0,
+		0x191d0, 0x191e8,
+		0x19238, 0x192b8,
+		0x193f8, 0x19474,
+		0x19490, 0x194cc,
+		0x194f0, 0x194f8,
+		0x19c00, 0x19c80,
+		0x19c94, 0x19cbc,
+		0x19ce4, 0x19d28,
+		0x19d50, 0x19d78,
+		0x19d94, 0x19dc8,
+		0x19df0, 0x19e10,
+		0x19e50, 0x19e6c,
+		0x19ea0, 0x19f34,
+		0x19f40, 0x19f50,
+		0x19f90, 0x19fac,
+		0x19fc4, 0x19fe4,
+		0x1a000, 0x1a06c,
+		0x1a0b0, 0x1a120,
+		0x1a128, 0x1a138,
+		0x1a190, 0x1a1c4,
+		0x1a1fc, 0x1a1fc,
+		0x1e008, 0x1e00c,
+		0x1e040, 0x1e04c,
+		0x1e284, 0x1e290,
+		0x1e2c0, 0x1e2c0,
+		0x1e2e0, 0x1e2e0,
+		0x1e300, 0x1e384,
+		0x1e3c0, 0x1e3c8,
+		0x1e408, 0x1e40c,
+		0x1e440, 0x1e44c,
+		0x1e684, 0x1e690,
+		0x1e6c0, 0x1e6c0,
+		0x1e6e0, 0x1e6e0,
+		0x1e700, 0x1e784,
+		0x1e7c0, 0x1e7c8,
+		0x1e808, 0x1e80c,
+		0x1e840, 0x1e84c,
+		0x1ea84, 0x1ea90,
+		0x1eac0, 0x1eac0,
+		0x1eae0, 0x1eae0,
+		0x1eb00, 0x1eb84,
+		0x1ebc0, 0x1ebc8,
+		0x1ec08, 0x1ec0c,
+		0x1ec40, 0x1ec4c,
+		0x1ee84, 0x1ee90,
+		0x1eec0, 0x1eec0,
+		0x1eee0, 0x1eee0,
+		0x1ef00, 0x1ef84,
+		0x1efc0, 0x1efc8,
+		0x1f008, 0x1f00c,
+		0x1f040, 0x1f04c,
+		0x1f284, 0x1f290,
+		0x1f2c0, 0x1f2c0,
+		0x1f2e0, 0x1f2e0,
+		0x1f300, 0x1f384,
+		0x1f3c0, 0x1f3c8,
+		0x1f408, 0x1f40c,
+		0x1f440, 0x1f44c,
+		0x1f684, 0x1f690,
+		0x1f6c0, 0x1f6c0,
+		0x1f6e0, 0x1f6e0,
+		0x1f700, 0x1f784,
+		0x1f7c0, 0x1f7c8,
+		0x1f808, 0x1f80c,
+		0x1f840, 0x1f84c,
+		0x1fa84, 0x1fa90,
+		0x1fac0, 0x1fac0,
+		0x1fae0, 0x1fae0,
+		0x1fb00, 0x1fb84,
+		0x1fbc0, 0x1fbc8,
+		0x1fc08, 0x1fc0c,
+		0x1fc40, 0x1fc4c,
+		0x1fe84, 0x1fe90,
+		0x1fec0, 0x1fec0,
+		0x1fee0, 0x1fee0,
+		0x1ff00, 0x1ff84,
+		0x1ffc0, 0x1ffc8,
+		0x30000, 0x30070,
+		0x30100, 0x3015c,
+		0x30190, 0x301d0,
+		0x30200, 0x30318,
+		0x30400, 0x3052c,
+		0x30540, 0x3061c,
+		0x30800, 0x3088c,
+		0x308c0, 0x30908,
+		0x30910, 0x309b8,
+		0x30a00, 0x30a04,
+		0x30a0c, 0x30a2c,
+		0x30a44, 0x30a50,
+		0x30a74, 0x30c24,
+		0x30d00, 0x30d3c,
+		0x30d44, 0x30d7c,
+		0x30de0, 0x30de0,
+		0x30e00, 0x30ed4,
+		0x30f00, 0x30fa4,
+		0x30fc0, 0x30fc4,
+		0x31000, 0x31004,
+		0x31080, 0x310fc,
+		0x31208, 0x31220,
+		0x3123c, 0x31254,
+		0x31300, 0x31300,
+		0x31308, 0x3131c,
+		0x31338, 0x3133c,
+		0x31380, 0x31380,
+		0x31388, 0x313a8,
+		0x313b4, 0x313b4,
+		0x31400, 0x31420,
+		0x31438, 0x3143c,
+		0x31480, 0x31480,
+		0x314a8, 0x314a8,
+		0x314b0, 0x314b4,
+		0x314c8, 0x314d4,
+		0x31a40, 0x31a4c,
+		0x31af0, 0x31b20,
+		0x31b38, 0x31b3c,
+		0x31b80, 0x31b80,
+		0x31ba8, 0x31ba8,
+		0x31bb0, 0x31bb4,
+		0x31bc8, 0x31bd4,
+		0x32140, 0x3218c,
+		0x321f0, 0x32200,
+		0x32218, 0x32218,
+		0x32400, 0x32400,
+		0x32408, 0x3241c,
+		0x32618, 0x32620,
+		0x32664, 0x32664,
+		0x326a8, 0x326a8,
+		0x326ec, 0x326ec,
+		0x32a00, 0x32abc,
+		0x32b00, 0x32b78,
+		0x32c00, 0x32c00,
+		0x32c08, 0x32c3c,
+		0x32e00, 0x32e2c,
+		0x32f00, 0x32f2c,
+		0x33000, 0x330ac,
+		0x330c0, 0x331ac,
+		0x331c0, 0x332c4,
+		0x332e4, 0x333c4,
+		0x333e4, 0x334ac,
+		0x334c0, 0x335ac,
+		0x335c0, 0x336c4,
+		0x336e4, 0x337c4,
+		0x337e4, 0x337fc,
+		0x33814, 0x33814,
+		0x33854, 0x33868,
+		0x33880, 0x3388c,
+		0x338c0, 0x338d0,
+		0x338e8, 0x338ec,
+		0x33900, 0x339ac,
+		0x339c0, 0x33ac4,
+		0x33ae4, 0x33b10,
+		0x33b24, 0x33b50,
+		0x33bf0, 0x33c10,
+		0x33c24, 0x33c50,
+		0x33cf0, 0x33cfc,
+		0x34000, 0x34070,
+		0x34100, 0x3415c,
+		0x34190, 0x341d0,
+		0x34200, 0x34318,
+		0x34400, 0x3452c,
+		0x34540, 0x3461c,
+		0x34800, 0x3488c,
+		0x348c0, 0x34908,
+		0x34910, 0x349b8,
+		0x34a00, 0x34a04,
+		0x34a0c, 0x34a2c,
+		0x34a44, 0x34a50,
+		0x34a74, 0x34c24,
+		0x34d00, 0x34d3c,
+		0x34d44, 0x34d7c,
+		0x34de0, 0x34de0,
+		0x34e00, 0x34ed4,
+		0x34f00, 0x34fa4,
+		0x34fc0, 0x34fc4,
+		0x35000, 0x35004,
+		0x35080, 0x350fc,
+		0x35208, 0x35220,
+		0x3523c, 0x35254,
+		0x35300, 0x35300,
+		0x35308, 0x3531c,
+		0x35338, 0x3533c,
+		0x35380, 0x35380,
+		0x35388, 0x353a8,
+		0x353b4, 0x353b4,
+		0x35400, 0x35420,
+		0x35438, 0x3543c,
+		0x35480, 0x35480,
+		0x354a8, 0x354a8,
+		0x354b0, 0x354b4,
+		0x354c8, 0x354d4,
+		0x35a40, 0x35a4c,
+		0x35af0, 0x35b20,
+		0x35b38, 0x35b3c,
+		0x35b80, 0x35b80,
+		0x35ba8, 0x35ba8,
+		0x35bb0, 0x35bb4,
+		0x35bc8, 0x35bd4,
+		0x36140, 0x3618c,
+		0x361f0, 0x36200,
+		0x36218, 0x36218,
+		0x36400, 0x36400,
+		0x36408, 0x3641c,
+		0x36618, 0x36620,
+		0x36664, 0x36664,
+		0x366a8, 0x366a8,
+		0x366ec, 0x366ec,
+		0x36a00, 0x36abc,
+		0x36b00, 0x36b78,
+		0x36c00, 0x36c00,
+		0x36c08, 0x36c3c,
+		0x36e00, 0x36e2c,
+		0x36f00, 0x36f2c,
+		0x37000, 0x370ac,
+		0x370c0, 0x371ac,
+		0x371c0, 0x372c4,
+		0x372e4, 0x373c4,
+		0x373e4, 0x374ac,
+		0x374c0, 0x375ac,
+		0x375c0, 0x376c4,
+		0x376e4, 0x377c4,
+		0x377e4, 0x377fc,
+		0x37814, 0x37814,
+		0x37854, 0x37868,
+		0x37880, 0x3788c,
+		0x378c0, 0x378d0,
+		0x378e8, 0x378ec,
+		0x37900, 0x379ac,
+		0x379c0, 0x37ac4,
+		0x37ae4, 0x37b10,
+		0x37b24, 0x37b50,
+		0x37bf0, 0x37c10,
+		0x37c24, 0x37c50,
+		0x37cf0, 0x37cfc,
+		0x40040, 0x40040,
+		0x40080, 0x40084,
+		0x40100, 0x40100,
+		0x40140, 0x401bc,
+		0x40200, 0x40214,
+		0x40228, 0x40228,
+		0x40240, 0x40258,
+		0x40280, 0x40280,
+		0x40304, 0x40304,
+		0x40330, 0x4033c,
+		0x41304, 0x413dc,
+		0x41400, 0x4141c,
+		0x41480, 0x414d0,
+		0x44000, 0x4407c,
+		0x440c0, 0x4427c,
+		0x442c0, 0x4447c,
+		0x444c0, 0x4467c,
+		0x446c0, 0x4487c,
+		0x448c0, 0x44a7c,
+		0x44ac0, 0x44c7c,
+		0x44cc0, 0x44e7c,
+		0x44ec0, 0x4507c,
+		0x450c0, 0x451fc,
+		0x45800, 0x45868,
+		0x45880, 0x45884,
+		0x458a0, 0x458b0,
+		0x45a00, 0x45a68,
+		0x45a80, 0x45a84,
+		0x45aa0, 0x45ab0,
+		0x460c0, 0x460e4,
+		0x47000, 0x4708c,
+		0x47200, 0x47250,
+		0x47400, 0x47420,
+		0x47600, 0x47618,
+		0x47800, 0x4782c,
+		0x50000, 0x500cc,
+		0x50400, 0x50400,
+		0x50800, 0x508cc,
+		0x50c00, 0x50c00,
+		0x51000, 0x510b0,
+		0x51300, 0x51324,
+	};
+
 	u32 *buf_end = (u32 *)((char *)buf + buf_size);
 	const unsigned int *reg_ranges;
 	int reg_ranges_size, range;
@@ -1328,6 +1678,11 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		reg_ranges_size = ARRAY_SIZE(t5_reg_ranges);
 		break;
 
+	case CHELSIO_T6:
+		reg_ranges = t6_reg_ranges;
+		reg_ranges_size = ARRAY_SIZE(t6_reg_ranges);
+		break;
+
 	default:
 		dev_err(adap->pdev_dev,
 			"Unsupported chip version %d\n", chip_version);
@@ -1374,17 +1729,16 @@ int t4_seeprom_wp(struct adapter *adapter, bool enable)
 }
 
 /**
- *	get_vpd_params - read VPD parameters from VPD EEPROM
+ *	t4_get_raw_vpd_params - read VPD parameters from VPD EEPROM
  *	@adapter: adapter to read
  *	@p: where to store the parameters
  *
  *	Reads card parameters stored in VPD EEPROM.
  */
-int get_vpd_params(struct adapter *adapter, struct vpd_params *p)
+int t4_get_raw_vpd_params(struct adapter *adapter, struct vpd_params *p)
 {
-	u32 cclk_param, cclk_val;
-	int i, ret, addr;
-	int ec, sn, pn;
+	int i, ret = 0, addr;
+	int ec, sn, pn, na;
 	u8 *vpd, csum;
 	unsigned int vpdr_len, kw_offset, id_len;
 
@@ -1392,6 +1746,9 @@ int get_vpd_params(struct adapter *adapter, struct vpd_params *p)
 	if (!vpd)
 		return -ENOMEM;
 
+	/* Card information normally starts at VPD_BASE but early cards had
+	 * it at 0.
+	 */
 	ret = pci_read_vpd(adapter->pdev, VPD_BASE, sizeof(u32), vpd);
 	if (ret < 0)
 		goto out;
@@ -1457,6 +1814,7 @@ int get_vpd_params(struct adapter *adapter, struct vpd_params *p)
 	FIND_VPD_KW(ec, "EC");
 	FIND_VPD_KW(sn, "SN");
 	FIND_VPD_KW(pn, "PN");
+	FIND_VPD_KW(na, "NA");
 #undef FIND_VPD_KW
 
 	memcpy(p->id, vpd + PCI_VPD_LRDT_TAG_SIZE, id_len);
@@ -1469,18 +1827,42 @@ int get_vpd_params(struct adapter *adapter, struct vpd_params *p)
 	i = pci_vpd_info_field_size(vpd + pn - PCI_VPD_INFO_FLD_HDR_SIZE);
 	memcpy(p->pn, vpd + pn, min(i, PN_LEN));
 	strim(p->pn);
+	memcpy(p->na, vpd + na, min(i, MACADDR_LEN));
+	strim((char *)p->na);
 
-	/*
-	 * Ask firmware for the Core Clock since it knows how to translate the
+out:
+	vfree(vpd);
+	return ret;
+}
+
+/**
+ *	t4_get_vpd_params - read VPD parameters & retrieve Core Clock
+ *	@adapter: adapter to read
+ *	@p: where to store the parameters
+ *
+ *	Reads card parameters stored in VPD EEPROM and retrieves the Core
+ *	Clock.  This can only be called after a connection to the firmware
+ *	is established.
+ */
+int t4_get_vpd_params(struct adapter *adapter, struct vpd_params *p)
+{
+	u32 cclk_param, cclk_val;
+	int ret;
+
+	/* Grab the raw VPD parameters.
+	 */
+	ret = t4_get_raw_vpd_params(adapter, p);
+	if (ret)
+		return ret;
+
+	/* Ask firmware for the Core Clock since it knows how to translate the
 	 * Reference Clock ('V2') VPD field into a Core Clock value ...
 	 */
 	cclk_param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
 		      FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_CCLK));
-	ret = t4_query_params(adapter, adapter->mbox, 0, 0,
+	ret = t4_query_params(adapter, adapter->mbox, adapter->pf, 0,
 			      1, &cclk_param, &cclk_val);
 
-out:
-	vfree(vpd);
 	if (ret)
 		return ret;
 	p->cclk = cclk_val;
@@ -1618,7 +2000,7 @@ int t4_read_flash(struct adapter *adapter, unsigned int addr,
 		if (ret)
 			return ret;
 		if (byte_oriented)
-			*data = (__force __u32) (htonl(*data));
+			*data = (__force __u32)(cpu_to_be32(*data));
 	}
 	return 0;
 }
@@ -1941,7 +2323,8 @@ static bool t4_fw_matches_chip(const struct adapter *adap,
 	 * which will keep us "honest" in the future ...
 	 */
 	if ((is_t4(adap->params.chip) && hdr->chip == FW_HDR_CHIP_T4) ||
-	    (is_t5(adap->params.chip) && hdr->chip == FW_HDR_CHIP_T5))
+	    (is_t5(adap->params.chip) && hdr->chip == FW_HDR_CHIP_T5) ||
+	    (is_t6(adap->params.chip) && hdr->chip == FW_HDR_CHIP_T6))
 		return true;
 
 	dev_err(adap->pdev_dev,
@@ -1979,7 +2362,7 @@ int t4_load_fw(struct adapter *adap, const u8 *fw_data, unsigned int size)
 			"FW image size not multiple of 512 bytes\n");
 		return -EINVAL;
 	}
-	if (ntohs(hdr->len512) * 512 != size) {
+	if ((unsigned int)be16_to_cpu(hdr->len512) * 512 != size) {
 		dev_err(adap->pdev_dev,
 			"FW image size differs from size in FW header\n");
 		return -EINVAL;
@@ -1993,7 +2376,7 @@ int t4_load_fw(struct adapter *adap, const u8 *fw_data, unsigned int size)
 		return -EINVAL;
 
 	for (csum = 0, i = 0; i < size / sizeof(csum); i++)
-		csum += ntohl(p[i]);
+		csum += be32_to_cpu(p[i]);
 
 	if (csum != 0xffffffff) {
 		dev_err(adap->pdev_dev,
@@ -2012,7 +2395,7 @@ int t4_load_fw(struct adapter *adap, const u8 *fw_data, unsigned int size)
 	 * first page with a bad version.
 	 */
 	memcpy(first_page, fw_data, SF_PAGE_SIZE);
-	((struct fw_hdr *)first_page)->fw_ver = htonl(0xffffffff);
+	((struct fw_hdr *)first_page)->fw_ver = cpu_to_be32(0xffffffff);
 	ret = t4_write_flash(adap, fw_img_start, SF_PAGE_SIZE, first_page);
 	if (ret)
 		goto out;
@@ -2039,6 +2422,147 @@ out:
 }
 
 /**
+ *	t4_phy_fw_ver - return current PHY firmware version
+ *	@adap: the adapter
+ *	@phy_fw_ver: return value buffer for PHY firmware version
+ *
+ *	Returns the current version of external PHY firmware on the
+ *	adapter.
+ */
+int t4_phy_fw_ver(struct adapter *adap, int *phy_fw_ver)
+{
+	u32 param, val;
+	int ret;
+
+	param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
+		 FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_PHYFW) |
+		 FW_PARAMS_PARAM_Y_V(adap->params.portvec) |
+		 FW_PARAMS_PARAM_Z_V(FW_PARAMS_PARAM_DEV_PHYFW_VERSION));
+	ret = t4_query_params(adap, adap->mbox, adap->pf, 0, 1,
+			      &param, &val);
+	if (ret < 0)
+		return ret;
+	*phy_fw_ver = val;
+	return 0;
+}
+
+/**
+ *	t4_load_phy_fw - download port PHY firmware
+ *	@adap: the adapter
+ *	@win: the PCI-E Memory Window index to use for t4_memory_rw()
+ *	@win_lock: the lock to use to guard the memory copy
+ *	@phy_fw_version: function to check PHY firmware versions
+ *	@phy_fw_data: the PHY firmware image to write
+ *	@phy_fw_size: image size
+ *
+ *	Transfer the specified PHY firmware to the adapter.  If a non-NULL
+ *	@phy_fw_version is supplied, then it will be used to determine if
+ *	it's necessary to perform the transfer by comparing the version
+ *	of any existing adapter PHY firmware with that of the passed in
+ *	PHY firmware image.  If @win_lock is non-NULL then it will be used
+ *	around the call to t4_memory_rw() which transfers the PHY firmware
+ *	to the adapter.
+ *
+ *	A negative error number will be returned if an error occurs.  If
+ *	version number support is available and there's no need to upgrade
+ *	the firmware, 0 will be returned.  If firmware is successfully
+ *	transferred to the adapter, 1 will be retured.
+ *
+ *	NOTE: some adapters only have local RAM to store the PHY firmware.  As
+ *	a result, a RESET of the adapter would cause that RAM to lose its
+ *	contents.  Thus, loading PHY firmware on such adapters must happen
+ *	after any FW_RESET_CMDs ...
+ */
+int t4_load_phy_fw(struct adapter *adap,
+		   int win, spinlock_t *win_lock,
+		   int (*phy_fw_version)(const u8 *, size_t),
+		   const u8 *phy_fw_data, size_t phy_fw_size)
+{
+	unsigned long mtype = 0, maddr = 0;
+	u32 param, val;
+	int cur_phy_fw_ver = 0, new_phy_fw_vers = 0;
+	int ret;
+
+	/* If we have version number support, then check to see if the adapter
+	 * already has up-to-date PHY firmware loaded.
+	 */
+	 if (phy_fw_version) {
+		new_phy_fw_vers = phy_fw_version(phy_fw_data, phy_fw_size);
+		ret = t4_phy_fw_ver(adap, &cur_phy_fw_ver);
+		if (ret < 0)
+			return ret;
+
+		if (cur_phy_fw_ver >= new_phy_fw_vers) {
+			CH_WARN(adap, "PHY Firmware already up-to-date, "
+				"version %#x\n", cur_phy_fw_ver);
+			return 0;
+		}
+	}
+
+	/* Ask the firmware where it wants us to copy the PHY firmware image.
+	 * The size of the file requires a special version of the READ coommand
+	 * which will pass the file size via the values field in PARAMS_CMD and
+	 * retrieve the return value from firmware and place it in the same
+	 * buffer values
+	 */
+	param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
+		 FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_PHYFW) |
+		 FW_PARAMS_PARAM_Y_V(adap->params.portvec) |
+		 FW_PARAMS_PARAM_Z_V(FW_PARAMS_PARAM_DEV_PHYFW_DOWNLOAD));
+	val = phy_fw_size;
+	ret = t4_query_params_rw(adap, adap->mbox, adap->pf, 0, 1,
+				 &param, &val, 1);
+	if (ret < 0)
+		return ret;
+	mtype = val >> 8;
+	maddr = (val & 0xff) << 16;
+
+	/* Copy the supplied PHY Firmware image to the adapter memory location
+	 * allocated by the adapter firmware.
+	 */
+	if (win_lock)
+		spin_lock_bh(win_lock);
+	ret = t4_memory_rw(adap, win, mtype, maddr,
+			   phy_fw_size, (__be32 *)phy_fw_data,
+			   T4_MEMORY_WRITE);
+	if (win_lock)
+		spin_unlock_bh(win_lock);
+	if (ret)
+		return ret;
+
+	/* Tell the firmware that the PHY firmware image has been written to
+	 * RAM and it can now start copying it over to the PHYs.  The chip
+	 * firmware will RESET the affected PHYs as part of this operation
+	 * leaving them running the new PHY firmware image.
+	 */
+	param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DEV) |
+		 FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DEV_PHYFW) |
+		 FW_PARAMS_PARAM_Y_V(adap->params.portvec) |
+		 FW_PARAMS_PARAM_Z_V(FW_PARAMS_PARAM_DEV_PHYFW_DOWNLOAD));
+	ret = t4_set_params_timeout(adap, adap->mbox, adap->pf, 0, 1,
+				    &param, &val, 30000);
+
+	/* If we have version number support, then check to see that the new
+	 * firmware got loaded properly.
+	 */
+	if (phy_fw_version) {
+		ret = t4_phy_fw_ver(adap, &cur_phy_fw_ver);
+		if (ret < 0)
+			return ret;
+
+		if (cur_phy_fw_ver != new_phy_fw_vers) {
+			CH_WARN(adap, "PHY Firmware did not update: "
+				"version on adapter %#x, "
+				"version flashed %#x\n",
+				cur_phy_fw_ver, new_phy_fw_vers);
+			return -ENXIO;
+		}
+	}
+
+	return 1;
+}
+
+/**
  *	t4_fwcache - firmware cache operation
  *	@adap: the adapter
  *	@op  : the operation (flush or flush and invalidate)
@@ -2051,7 +2575,7 @@ int t4_fwcache(struct adapter *adap, enum fw_params_param_dev_fwcache op)
 	c.op_to_vfn =
 		cpu_to_be32(FW_CMD_OP_V(FW_PARAMS_CMD) |
 			    FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
-			    FW_PARAMS_CMD_PFN_V(adap->fn) |
+			    FW_PARAMS_CMD_PFN_V(adap->pf) |
 			    FW_PARAMS_CMD_VFN_V(0));
 	c.retval_len16 = cpu_to_be32(FW_LEN16(c));
 	c.param[0].mnem =
@@ -2062,6 +2586,61 @@ int t4_fwcache(struct adapter *adap, enum fw_params_param_dev_fwcache op)
 	return t4_wr_mbox(adap, adap->mbox, &c, sizeof(c), NULL);
 }
 
+void t4_cim_read_pif_la(struct adapter *adap, u32 *pif_req, u32 *pif_rsp,
+			unsigned int *pif_req_wrptr,
+			unsigned int *pif_rsp_wrptr)
+{
+	int i, j;
+	u32 cfg, val, req, rsp;
+
+	cfg = t4_read_reg(adap, CIM_DEBUGCFG_A);
+	if (cfg & LADBGEN_F)
+		t4_write_reg(adap, CIM_DEBUGCFG_A, cfg ^ LADBGEN_F);
+
+	val = t4_read_reg(adap, CIM_DEBUGSTS_A);
+	req = POLADBGWRPTR_G(val);
+	rsp = PILADBGWRPTR_G(val);
+	if (pif_req_wrptr)
+		*pif_req_wrptr = req;
+	if (pif_rsp_wrptr)
+		*pif_rsp_wrptr = rsp;
+
+	for (i = 0; i < CIM_PIFLA_SIZE; i++) {
+		for (j = 0; j < 6; j++) {
+			t4_write_reg(adap, CIM_DEBUGCFG_A, POLADBGRDPTR_V(req) |
+				     PILADBGRDPTR_V(rsp));
+			*pif_req++ = t4_read_reg(adap, CIM_PO_LA_DEBUGDATA_A);
+			*pif_rsp++ = t4_read_reg(adap, CIM_PI_LA_DEBUGDATA_A);
+			req++;
+			rsp++;
+		}
+		req = (req + 2) & POLADBGRDPTR_M;
+		rsp = (rsp + 2) & PILADBGRDPTR_M;
+	}
+	t4_write_reg(adap, CIM_DEBUGCFG_A, cfg);
+}
+
+void t4_cim_read_ma_la(struct adapter *adap, u32 *ma_req, u32 *ma_rsp)
+{
+	u32 cfg;
+	int i, j, idx;
+
+	cfg = t4_read_reg(adap, CIM_DEBUGCFG_A);
+	if (cfg & LADBGEN_F)
+		t4_write_reg(adap, CIM_DEBUGCFG_A, cfg ^ LADBGEN_F);
+
+	for (i = 0; i < CIM_MALA_SIZE; i++) {
+		for (j = 0; j < 5; j++) {
+			idx = 8 * i + j;
+			t4_write_reg(adap, CIM_DEBUGCFG_A, POLADBGRDPTR_V(idx) |
+				     PILADBGRDPTR_V(idx));
+			*ma_req++ = t4_read_reg(adap, CIM_PO_LA_MADEBUGDATA_A);
+			*ma_rsp++ = t4_read_reg(adap, CIM_PI_LA_MADEBUGDATA_A);
+		}
+	}
+	t4_write_reg(adap, CIM_DEBUGCFG_A, cfg);
+}
+
 void t4_ulprx_read_la(struct adapter *adap, u32 *la_buf)
 {
 	unsigned int i, j;
@@ -2082,7 +2661,7 @@ void t4_ulprx_read_la(struct adapter *adap, u32 *la_buf)
 		     FW_PORT_CAP_ANEG)
 
 /**
- *	t4_link_start - apply link configuration to MAC/PHY
+ *	t4_link_l1cfg - apply link configuration to MAC/PHY
  *	@phy: the PHY to setup
  *	@mac: the MAC to setup
  *	@lc: the requested link configuration
@@ -2094,7 +2673,7 @@ void t4_ulprx_read_la(struct adapter *adap, u32 *la_buf)
  *	- If auto-negotiation is off set the MAC to the proper speed/duplex/FC,
  *	  otherwise do it later based on the outcome of auto-negotiation.
  */
-int t4_link_start(struct adapter *adap, unsigned int mbox, unsigned int port,
+int t4_link_l1cfg(struct adapter *adap, unsigned int mbox, unsigned int port,
 		  struct link_config *lc)
 {
 	struct fw_port_cmd c;
@@ -2107,19 +2686,22 @@ int t4_link_start(struct adapter *adap, unsigned int mbox, unsigned int port,
 		fc |= FW_PORT_CAP_FC_TX;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_portid = htonl(FW_CMD_OP_V(FW_PORT_CMD) | FW_CMD_REQUEST_F |
-			       FW_CMD_EXEC_F | FW_PORT_CMD_PORTID_V(port));
-	c.action_to_len16 = htonl(FW_PORT_CMD_ACTION_V(FW_PORT_ACTION_L1_CFG) |
-				  FW_LEN16(c));
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PORT_CMD) |
+				     FW_CMD_REQUEST_F | FW_CMD_EXEC_F |
+				     FW_PORT_CMD_PORTID_V(port));
+	c.action_to_len16 =
+		cpu_to_be32(FW_PORT_CMD_ACTION_V(FW_PORT_ACTION_L1_CFG) |
+			    FW_LEN16(c));
 
 	if (!(lc->supported & FW_PORT_CAP_ANEG)) {
-		c.u.l1cfg.rcap = htonl((lc->supported & ADVERT_MASK) | fc);
+		c.u.l1cfg.rcap = cpu_to_be32((lc->supported & ADVERT_MASK) |
+					     fc);
 		lc->fc = lc->requested_fc & (PAUSE_RX | PAUSE_TX);
 	} else if (lc->autoneg == AUTONEG_DISABLE) {
-		c.u.l1cfg.rcap = htonl(lc->requested_speed | fc | mdi);
+		c.u.l1cfg.rcap = cpu_to_be32(lc->requested_speed | fc | mdi);
 		lc->fc = lc->requested_fc & (PAUSE_RX | PAUSE_TX);
 	} else
-		c.u.l1cfg.rcap = htonl(lc->advertising | fc | mdi);
+		c.u.l1cfg.rcap = cpu_to_be32(lc->advertising | fc | mdi);
 
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
@@ -2137,11 +2719,13 @@ int t4_restart_aneg(struct adapter *adap, unsigned int mbox, unsigned int port)
 	struct fw_port_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_portid = htonl(FW_CMD_OP_V(FW_PORT_CMD) | FW_CMD_REQUEST_F |
-			       FW_CMD_EXEC_F | FW_PORT_CMD_PORTID_V(port));
-	c.action_to_len16 = htonl(FW_PORT_CMD_ACTION_V(FW_PORT_ACTION_L1_CFG) |
-				  FW_LEN16(c));
-	c.u.l1cfg.rcap = htonl(FW_PORT_CAP_ANEG);
+	c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PORT_CMD) |
+				     FW_CMD_REQUEST_F | FW_CMD_EXEC_F |
+				     FW_PORT_CMD_PORTID_V(port));
+	c.action_to_len16 =
+		cpu_to_be32(FW_PORT_CMD_ACTION_V(FW_PORT_ACTION_L1_CFG) |
+			    FW_LEN16(c));
+	c.u.l1cfg.rcap = cpu_to_be32(FW_PORT_CAP_ANEG);
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -2335,6 +2919,7 @@ static void tp_intr_handler(struct adapter *adapter)
 static void sge_intr_handler(struct adapter *adapter)
 {
 	u64 v;
+	u32 err;
 
 	static const struct intr_info sge_intr_info[] = {
 		{ ERR_CPL_EXCEED_IQE_SIZE_F,
@@ -2343,8 +2928,6 @@ static void sge_intr_handler(struct adapter *adapter)
 		  "SGE GTS CIDX increment too large", -1, 0 },
 		{ ERR_CPL_OPCODE_0_F, "SGE received 0-length CPL", -1, 0 },
 		{ DBFIFO_LP_INT_F, NULL, -1, 0, t4_db_full },
-		{ DBFIFO_HP_INT_F, NULL, -1, 0, t4_db_full },
-		{ ERR_DROPPED_DB_F, NULL, -1, 0, t4_db_dropped },
 		{ ERR_DATA_CPL_ON_HIGH_QID1_F | ERR_DATA_CPL_ON_HIGH_QID0_F,
 		  "SGE IQID > 1023 received CPL for FL", -1, 0 },
 		{ ERR_BAD_DB_PIDX3_F, "SGE DBP 3 pidx increment too large", -1,
@@ -2357,13 +2940,19 @@ static void sge_intr_handler(struct adapter *adapter)
 		  0 },
 		{ ERR_ING_CTXT_PRIO_F,
 		  "SGE too many priority ingress contexts", -1, 0 },
-		{ ERR_EGR_CTXT_PRIO_F,
-		  "SGE too many priority egress contexts", -1, 0 },
 		{ INGRESS_SIZE_ERR_F, "SGE illegal ingress QID", -1, 0 },
 		{ EGRESS_SIZE_ERR_F, "SGE illegal egress QID", -1, 0 },
 		{ 0 }
 	};
 
+	static struct intr_info t4t5_sge_intr_info[] = {
+		{ ERR_DROPPED_DB_F, NULL, -1, 0, t4_db_dropped },
+		{ DBFIFO_HP_INT_F, NULL, -1, 0, t4_db_full },
+		{ ERR_EGR_CTXT_PRIO_F,
+		  "SGE too many priority egress contexts", -1, 0 },
+		{ 0 }
+	};
+
 	v = (u64)t4_read_reg(adapter, SGE_INT_CAUSE1_A) |
 		((u64)t4_read_reg(adapter, SGE_INT_CAUSE2_A) << 32);
 	if (v) {
@@ -2373,8 +2962,23 @@ static void sge_intr_handler(struct adapter *adapter)
 		t4_write_reg(adapter, SGE_INT_CAUSE2_A, v >> 32);
 	}
 
-	if (t4_handle_intr_status(adapter, SGE_INT_CAUSE3_A, sge_intr_info) ||
-	    v != 0)
+	v |= t4_handle_intr_status(adapter, SGE_INT_CAUSE3_A, sge_intr_info);
+	if (CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5)
+		v |= t4_handle_intr_status(adapter, SGE_INT_CAUSE3_A,
+					   t4t5_sge_intr_info);
+
+	err = t4_read_reg(adapter, SGE_ERROR_STATS_A);
+	if (err & ERROR_QID_VALID_F) {
+		dev_err(adapter->pdev_dev, "SGE error for queue %u\n",
+			ERROR_QID_G(err));
+		if (err & UNCAPTURED_ERROR_F)
+			dev_err(adapter->pdev_dev,
+				"SGE UNCAPTURED_ERROR set (clearing)\n");
+		t4_write_reg(adapter, SGE_ERROR_STATS_A, ERROR_QID_VALID_F |
+			     UNCAPTURED_ERROR_F);
+	}
+
+	if (v != 0)
 		t4_fatal_err(adapter);
 }
 
@@ -2547,6 +3151,7 @@ static void cplsw_intr_handler(struct adapter *adapter)
  */
 static void le_intr_handler(struct adapter *adap)
 {
+	enum chip_type chip = CHELSIO_CHIP_VERSION(adap->params.chip);
 	static const struct intr_info le_intr_info[] = {
 		{ LIPMISS_F, "LE LIP miss", -1, 0 },
 		{ LIP0_F, "LE 0 LIP error", -1, 0 },
@@ -2556,7 +3161,18 @@ static void le_intr_handler(struct adapter *adap)
 		{ 0 }
 	};
 
-	if (t4_handle_intr_status(adap, LE_DB_INT_CAUSE_A, le_intr_info))
+	static struct intr_info t6_le_intr_info[] = {
+		{ T6_LIPMISS_F, "LE LIP miss", -1, 0 },
+		{ T6_LIP0_F, "LE 0 LIP error", -1, 0 },
+		{ TCAMINTPERR_F, "LE parity error", -1, 1 },
+		{ T6_UNKNOWNCMD_F, "LE unknown command", -1, 1 },
+		{ SSRAMINTPERR_F, "LE request queue parity error", -1, 1 },
+		{ 0 }
+	};
+
+	if (t4_handle_intr_status(adap, LE_DB_INT_CAUSE_A,
+				  (chip <= CHELSIO_T5) ?
+				  le_intr_info : t6_le_intr_info))
 		t4_fatal_err(adap);
 }
 
@@ -2825,7 +3441,7 @@ int t4_slow_intr_handler(struct adapter *adapter)
 		pcie_intr_handler(adapter);
 	if (cause & MC_F)
 		mem_intr_handler(adapter, MEM_MC);
-	if (!is_t4(adapter->params.chip) && (cause & MC1_S))
+	if (is_t5(adapter->params.chip) && (cause & MC1_F))
 		mem_intr_handler(adapter, MEM_MC1);
 	if (cause & EDC0_F)
 		mem_intr_handler(adapter, MEM_EDC0);
@@ -2871,17 +3487,18 @@ int t4_slow_intr_handler(struct adapter *adapter)
  */
 void t4_intr_enable(struct adapter *adapter)
 {
+	u32 val = 0;
 	u32 pf = SOURCEPF_G(t4_read_reg(adapter, PL_WHOAMI_A));
 
+	if (CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5)
+		val = ERR_DROPPED_DB_F | ERR_EGR_CTXT_PRIO_F | DBFIFO_HP_INT_F;
 	t4_write_reg(adapter, SGE_INT_ENABLE3_A, ERR_CPL_EXCEED_IQE_SIZE_F |
 		     ERR_INVALID_CIDX_INC_F | ERR_CPL_OPCODE_0_F |
-		     ERR_DROPPED_DB_F | ERR_DATA_CPL_ON_HIGH_QID1_F |
+		     ERR_DATA_CPL_ON_HIGH_QID1_F | INGRESS_SIZE_ERR_F |
 		     ERR_DATA_CPL_ON_HIGH_QID0_F | ERR_BAD_DB_PIDX3_F |
 		     ERR_BAD_DB_PIDX2_F | ERR_BAD_DB_PIDX1_F |
 		     ERR_BAD_DB_PIDX0_F | ERR_ING_CTXT_PRIO_F |
-		     ERR_EGR_CTXT_PRIO_F | INGRESS_SIZE_ERR_F |
-		     DBFIFO_HP_INT_F | DBFIFO_LP_INT_F |
-		     EGRESS_SIZE_ERR_F);
+		     DBFIFO_LP_INT_F | EGRESS_SIZE_ERR_F | val);
 	t4_write_reg(adapter, MYPF_REG(PL_PF_INT_ENABLE_A), PF_INTR_MASK);
 	t4_set_reg_field(adapter, PL_INT_MAP0_A, 0, 1 << pf);
 }
@@ -2945,18 +3562,18 @@ int t4_config_rss_range(struct adapter *adapter, int mbox, unsigned int viid,
 	struct fw_rss_ind_tbl_cmd cmd;
 
 	memset(&cmd, 0, sizeof(cmd));
-	cmd.op_to_viid = htonl(FW_CMD_OP_V(FW_RSS_IND_TBL_CMD) |
+	cmd.op_to_viid = cpu_to_be32(FW_CMD_OP_V(FW_RSS_IND_TBL_CMD) |
 			       FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
 			       FW_RSS_IND_TBL_CMD_VIID_V(viid));
-	cmd.retval_len16 = htonl(FW_LEN16(cmd));
+	cmd.retval_len16 = cpu_to_be32(FW_LEN16(cmd));
 
 	/* each fw_rss_ind_tbl_cmd takes up to 32 entries */
 	while (n > 0) {
 		int nq = min(n, 32);
 		__be32 *qp = &cmd.iq0_to_iq2;
 
-		cmd.niqid = htons(nq);
-		cmd.startidx = htons(start);
+		cmd.niqid = cpu_to_be16(nq);
+		cmd.startidx = cpu_to_be16(start);
 
 		start += nq;
 		n -= nq;
@@ -2974,7 +3591,7 @@ int t4_config_rss_range(struct adapter *adapter, int mbox, unsigned int viid,
 			if (++rsp >= rsp_end)
 				rsp = rspq;
 
-			*qp++ = htonl(v);
+			*qp++ = cpu_to_be32(v);
 			nq -= 3;
 		}
 
@@ -3000,20 +3617,46 @@ int t4_config_glbl_rss(struct adapter *adapter, int mbox, unsigned int mode,
 	struct fw_rss_glb_config_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_write = htonl(FW_CMD_OP_V(FW_RSS_GLB_CONFIG_CMD) |
-			      FW_CMD_REQUEST_F | FW_CMD_WRITE_F);
-	c.retval_len16 = htonl(FW_LEN16(c));
+	c.op_to_write = cpu_to_be32(FW_CMD_OP_V(FW_RSS_GLB_CONFIG_CMD) |
+				    FW_CMD_REQUEST_F | FW_CMD_WRITE_F);
+	c.retval_len16 = cpu_to_be32(FW_LEN16(c));
 	if (mode == FW_RSS_GLB_CONFIG_CMD_MODE_MANUAL) {
-		c.u.manual.mode_pkd = htonl(FW_RSS_GLB_CONFIG_CMD_MODE_V(mode));
+		c.u.manual.mode_pkd =
+			cpu_to_be32(FW_RSS_GLB_CONFIG_CMD_MODE_V(mode));
 	} else if (mode == FW_RSS_GLB_CONFIG_CMD_MODE_BASICVIRTUAL) {
 		c.u.basicvirtual.mode_pkd =
-			htonl(FW_RSS_GLB_CONFIG_CMD_MODE_V(mode));
-		c.u.basicvirtual.synmapen_to_hashtoeplitz = htonl(flags);
+			cpu_to_be32(FW_RSS_GLB_CONFIG_CMD_MODE_V(mode));
+		c.u.basicvirtual.synmapen_to_hashtoeplitz = cpu_to_be32(flags);
 	} else
 		return -EINVAL;
 	return t4_wr_mbox(adapter, mbox, &c, sizeof(c), NULL);
 }
 
+/**
+ *	t4_config_vi_rss - configure per VI RSS settings
+ *	@adapter: the adapter
+ *	@mbox: mbox to use for the FW command
+ *	@viid: the VI id
+ *	@flags: RSS flags
+ *	@defq: id of the default RSS queue for the VI.
+ *
+ *	Configures VI-specific RSS properties.
+ */
+int t4_config_vi_rss(struct adapter *adapter, int mbox, unsigned int viid,
+		     unsigned int flags, unsigned int defq)
+{
+	struct fw_rss_vi_config_cmd c;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_viid = cpu_to_be32(FW_CMD_OP_V(FW_RSS_VI_CONFIG_CMD) |
+				   FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
+				   FW_RSS_VI_CONFIG_CMD_VIID_V(viid));
+	c.retval_len16 = cpu_to_be32(FW_LEN16(c));
+	c.u.basicvirtual.defaultq_to_udpen = cpu_to_be32(flags |
+					FW_RSS_VI_CONFIG_CMD_DEFAULTQ_V(defq));
+	return t4_wr_mbox(adapter, mbox, &c, sizeof(c), NULL);
+}
+
 /* Read an RSS table row */
 static int rd_rss_row(struct adapter *adap, int row, u32 *val)
 {
@@ -3045,6 +3688,40 @@ int t4_read_rss(struct adapter *adapter, u16 *map)
 }
 
 /**
+ *	t4_fw_tp_pio_rw - Access TP PIO through LDST
+ *	@adap: the adapter
+ *	@vals: where the indirect register values are stored/written
+ *	@nregs: how many indirect registers to read/write
+ *	@start_idx: index of first indirect register to read/write
+ *	@rw: Read (1) or Write (0)
+ *
+ *	Access TP PIO registers through LDST
+ */
+static void t4_fw_tp_pio_rw(struct adapter *adap, u32 *vals, unsigned int nregs,
+			    unsigned int start_index, unsigned int rw)
+{
+	int ret, i;
+	int cmd = FW_LDST_ADDRSPC_TP_PIO;
+	struct fw_ldst_cmd c;
+
+	for (i = 0 ; i < nregs; i++) {
+		memset(&c, 0, sizeof(c));
+		c.op_to_addrspace = cpu_to_be32(FW_CMD_OP_V(FW_LDST_CMD) |
+						FW_CMD_REQUEST_F |
+						(rw ? FW_CMD_READ_F :
+						      FW_CMD_WRITE_F) |
+						FW_LDST_CMD_ADDRSPACE_V(cmd));
+		c.cycles_to_len16 = cpu_to_be32(FW_LEN16(c));
+
+		c.u.addrval.addr = cpu_to_be32(start_index + i);
+		c.u.addrval.val  = rw ? 0 : cpu_to_be32(vals[i]);
+		ret = t4_wr_mbox(adap, adap->mbox, &c, sizeof(c), &c);
+		if (!ret && rw)
+			vals[i] = be32_to_cpu(c.u.addrval.val);
+	}
+}
+
+/**
  *	t4_read_rss_key - read the global RSS key
  *	@adap: the adapter
  *	@key: 10-entry array holding the 320-bit RSS key
@@ -3053,8 +3730,11 @@ int t4_read_rss(struct adapter *adapter, u16 *map)
  */
 void t4_read_rss_key(struct adapter *adap, u32 *key)
 {
-	t4_read_indirect(adap, TP_PIO_ADDR_A, TP_PIO_DATA_A, key, 10,
-			 TP_RSS_SECRET_KEY0_A);
+	if (adap->flags & FW_OK)
+		t4_fw_tp_pio_rw(adap, key, 10, TP_RSS_SECRET_KEY0_A, 1);
+	else
+		t4_read_indirect(adap, TP_PIO_ADDR_A, TP_PIO_DATA_A, key, 10,
+				 TP_RSS_SECRET_KEY0_A);
 }
 
 /**
@@ -3069,11 +3749,32 @@ void t4_read_rss_key(struct adapter *adap, u32 *key)
  */
 void t4_write_rss_key(struct adapter *adap, const u32 *key, int idx)
 {
-	t4_write_indirect(adap, TP_PIO_ADDR_A, TP_PIO_DATA_A, key, 10,
-			  TP_RSS_SECRET_KEY0_A);
-	if (idx >= 0 && idx < 16)
-		t4_write_reg(adap, TP_RSS_CONFIG_VRT_A,
-			     KEYWRADDR_V(idx) | KEYWREN_F);
+	u8 rss_key_addr_cnt = 16;
+	u32 vrt = t4_read_reg(adap, TP_RSS_CONFIG_VRT_A);
+
+	/* T6 and later: for KeyMode 3 (per-vf and per-vf scramble),
+	 * allows access to key addresses 16-63 by using KeyWrAddrX
+	 * as index[5:4](upper 2) into key table
+	 */
+	if ((CHELSIO_CHIP_VERSION(adap->params.chip) > CHELSIO_T5) &&
+	    (vrt & KEYEXTEND_F) && (KEYMODE_G(vrt) == 3))
+		rss_key_addr_cnt = 32;
+
+	if (adap->flags & FW_OK)
+		t4_fw_tp_pio_rw(adap, (void *)key, 10, TP_RSS_SECRET_KEY0_A, 0);
+	else
+		t4_write_indirect(adap, TP_PIO_ADDR_A, TP_PIO_DATA_A, key, 10,
+				  TP_RSS_SECRET_KEY0_A);
+
+	if (idx >= 0 && idx < rss_key_addr_cnt) {
+		if (rss_key_addr_cnt > 16)
+			t4_write_reg(adap, TP_RSS_CONFIG_VRT_A,
+				     KEYWRADDRX_V(idx >> 4) |
+				     T6_VFWRADDR_V(idx) | KEYWREN_F);
+		else
+			t4_write_reg(adap, TP_RSS_CONFIG_VRT_A,
+				     KEYWRADDR_V(idx) | KEYWREN_F);
+	}
 }
 
 /**
@@ -3088,8 +3789,12 @@ void t4_write_rss_key(struct adapter *adap, const u32 *key, int idx)
 void t4_read_rss_pf_config(struct adapter *adapter, unsigned int index,
 			   u32 *valp)
 {
-	t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
-			 valp, 1, TP_RSS_PF0_CONFIG_A + index);
+	if (adapter->flags & FW_OK)
+		t4_fw_tp_pio_rw(adapter, valp, 1,
+				TP_RSS_PF0_CONFIG_A + index, 1);
+	else
+		t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
+				 valp, 1, TP_RSS_PF0_CONFIG_A + index);
 }
 
 /**
@@ -3107,8 +3812,13 @@ void t4_read_rss_vf_config(struct adapter *adapter, unsigned int index,
 {
 	u32 vrt, mask, data;
 
-	mask = VFWRADDR_V(VFWRADDR_M);
-	data = VFWRADDR_V(index);
+	if (CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5) {
+		mask = VFWRADDR_V(VFWRADDR_M);
+		data = VFWRADDR_V(index);
+	} else {
+		 mask =  T6_VFWRADDR_V(T6_VFWRADDR_M);
+		 data = T6_VFWRADDR_V(index);
+	}
 
 	/* Request that the index'th VF Table values be read into VFL/VFH.
 	 */
@@ -3119,10 +3829,15 @@ void t4_read_rss_vf_config(struct adapter *adapter, unsigned int index,
 
 	/* Grab the VFL/VFH values ...
 	 */
-	t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
-			 vfl, 1, TP_RSS_VFL_CONFIG_A);
-	t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
-			 vfh, 1, TP_RSS_VFH_CONFIG_A);
+	if (adapter->flags & FW_OK) {
+		t4_fw_tp_pio_rw(adapter, vfl, 1, TP_RSS_VFL_CONFIG_A, 1);
+		t4_fw_tp_pio_rw(adapter, vfh, 1, TP_RSS_VFH_CONFIG_A, 1);
+	} else {
+		t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
+				 vfl, 1, TP_RSS_VFL_CONFIG_A);
+		t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
+				 vfh, 1, TP_RSS_VFH_CONFIG_A);
+	}
 }
 
 /**
@@ -3135,8 +3850,11 @@ u32 t4_read_rss_pf_map(struct adapter *adapter)
 {
 	u32 pfmap;
 
-	t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
-			 &pfmap, 1, TP_RSS_PF_MAP_A);
+	if (adapter->flags & FW_OK)
+		t4_fw_tp_pio_rw(adapter, &pfmap, 1, TP_RSS_PF_MAP_A, 1);
+	else
+		t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
+				 &pfmap, 1, TP_RSS_PF_MAP_A);
 	return pfmap;
 }
 
@@ -3150,8 +3868,11 @@ u32 t4_read_rss_pf_mask(struct adapter *adapter)
 {
 	u32 pfmask;
 
-	t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
-			 &pfmask, 1, TP_RSS_PF_MSK_A);
+	if (adapter->flags & FW_OK)
+		t4_fw_tp_pio_rw(adapter, &pfmask, 1, TP_RSS_PF_MSK_A, 1);
+	else
+		t4_read_indirect(adapter, TP_PIO_ADDR_A, TP_PIO_DATA_A,
+				 &pfmask, 1, TP_RSS_PF_MSK_A);
 	return pfmask;
 }
 
@@ -3176,18 +3897,18 @@ void t4_tp_get_tcp_stats(struct adapter *adap, struct tp_tcp_stats *v4,
 	if (v4) {
 		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, val,
 				 ARRAY_SIZE(val), TP_MIB_TCP_OUT_RST_A);
-		v4->tcpOutRsts = STAT(OUT_RST);
-		v4->tcpInSegs  = STAT64(IN_SEG);
-		v4->tcpOutSegs = STAT64(OUT_SEG);
-		v4->tcpRetransSegs = STAT64(RXT_SEG);
+		v4->tcp_out_rsts = STAT(OUT_RST);
+		v4->tcp_in_segs  = STAT64(IN_SEG);
+		v4->tcp_out_segs = STAT64(OUT_SEG);
+		v4->tcp_retrans_segs = STAT64(RXT_SEG);
 	}
 	if (v6) {
 		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, val,
 				 ARRAY_SIZE(val), TP_MIB_TCP_V6OUT_RST_A);
-		v6->tcpOutRsts = STAT(OUT_RST);
-		v6->tcpInSegs  = STAT64(IN_SEG);
-		v6->tcpOutSegs = STAT64(OUT_SEG);
-		v6->tcpRetransSegs = STAT64(RXT_SEG);
+		v6->tcp_out_rsts = STAT(OUT_RST);
+		v6->tcp_in_segs  = STAT64(IN_SEG);
+		v6->tcp_out_segs = STAT64(OUT_SEG);
+		v6->tcp_retrans_segs = STAT64(RXT_SEG);
 	}
 #undef STAT64
 #undef STAT
@@ -3195,6 +3916,130 @@ void t4_tp_get_tcp_stats(struct adapter *adap, struct tp_tcp_stats *v4,
 }
 
 /**
+ *	t4_tp_get_err_stats - read TP's error MIB counters
+ *	@adap: the adapter
+ *	@st: holds the counter values
+ *
+ *	Returns the values of TP's error counters.
+ */
+void t4_tp_get_err_stats(struct adapter *adap, struct tp_err_stats *st)
+{
+	/* T6 and later has 2 channels */
+	if (adap->params.arch.nchan == NCHAN) {
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->mac_in_errs, 12, TP_MIB_MAC_IN_ERR_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->tnl_cong_drops, 8,
+				 TP_MIB_TNL_CNG_DROP_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->tnl_tx_drops, 4,
+				 TP_MIB_TNL_DROP_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->ofld_vlan_drops, 4,
+				 TP_MIB_OFD_VLN_DROP_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->tcp6_in_errs, 4,
+				 TP_MIB_TCP_V6IN_ERR_0_A);
+	} else {
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->mac_in_errs, 2, TP_MIB_MAC_IN_ERR_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->hdr_in_errs, 2, TP_MIB_HDR_IN_ERR_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->tcp_in_errs, 2, TP_MIB_TCP_IN_ERR_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->tnl_cong_drops, 2,
+				 TP_MIB_TNL_CNG_DROP_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->ofld_chan_drops, 2,
+				 TP_MIB_OFD_CHN_DROP_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->tnl_tx_drops, 2, TP_MIB_TNL_DROP_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->ofld_vlan_drops, 2,
+				 TP_MIB_OFD_VLN_DROP_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+				 st->tcp6_in_errs, 2, TP_MIB_TCP_V6IN_ERR_0_A);
+	}
+	t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A,
+			 &st->ofld_no_neigh, 2, TP_MIB_OFD_ARP_DROP_A);
+}
+
+/**
+ *	t4_tp_get_cpl_stats - read TP's CPL MIB counters
+ *	@adap: the adapter
+ *	@st: holds the counter values
+ *
+ *	Returns the values of TP's CPL counters.
+ */
+void t4_tp_get_cpl_stats(struct adapter *adap, struct tp_cpl_stats *st)
+{
+	/* T6 and later has 2 channels */
+	if (adap->params.arch.nchan == NCHAN) {
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, st->req,
+				 8, TP_MIB_CPL_IN_REQ_0_A);
+	} else {
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, st->req,
+				 2, TP_MIB_CPL_IN_REQ_0_A);
+		t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, st->rsp,
+				 2, TP_MIB_CPL_OUT_RSP_0_A);
+	}
+}
+
+/**
+ *	t4_tp_get_rdma_stats - read TP's RDMA MIB counters
+ *	@adap: the adapter
+ *	@st: holds the counter values
+ *
+ *	Returns the values of TP's RDMA counters.
+ */
+void t4_tp_get_rdma_stats(struct adapter *adap, struct tp_rdma_stats *st)
+{
+	t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, &st->rqe_dfr_pkt,
+			 2, TP_MIB_RQE_DFR_PKT_A);
+}
+
+/**
+ *	t4_get_fcoe_stats - read TP's FCoE MIB counters for a port
+ *	@adap: the adapter
+ *	@idx: the port index
+ *	@st: holds the counter values
+ *
+ *	Returns the values of TP's FCoE counters for the selected port.
+ */
+void t4_get_fcoe_stats(struct adapter *adap, unsigned int idx,
+		       struct tp_fcoe_stats *st)
+{
+	u32 val[2];
+
+	t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, &st->frames_ddp,
+			 1, TP_MIB_FCOE_DDP_0_A + idx);
+	t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, &st->frames_drop,
+			 1, TP_MIB_FCOE_DROP_0_A + idx);
+	t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, val,
+			 2, TP_MIB_FCOE_BYTE_0_HI_A + 2 * idx);
+	st->octets_ddp = ((u64)val[0] << 32) | val[1];
+}
+
+/**
+ *	t4_get_usm_stats - read TP's non-TCP DDP MIB counters
+ *	@adap: the adapter
+ *	@st: holds the counter values
+ *
+ *	Returns the values of TP's counters for non-TCP directly-placed packets.
+ */
+void t4_get_usm_stats(struct adapter *adap, struct tp_usm_stats *st)
+{
+	u32 val[4];
+
+	t4_read_indirect(adap, TP_MIB_INDEX_A, TP_MIB_DATA_A, val, 4,
+			 TP_MIB_USM_PKTS_A);
+	st->frames = val[0];
+	st->drops = val[1];
+	st->octets = ((u64)val[2] << 32) | val[3];
+}
+
+/**
  *	t4_read_mtu_tbl - returns the values in the HW path MTU table
  *	@adap: the adapter
  *	@mtus: where to store the MTU values
@@ -3346,6 +4191,52 @@ void t4_load_mtus(struct adapter *adap, const unsigned short *mtus,
 	}
 }
 
+/* Calculates a rate in bytes/s given the number of 256-byte units per 4K core
+ * clocks.  The formula is
+ *
+ * bytes/s = bytes256 * 256 * ClkFreq / 4096
+ *
+ * which is equivalent to
+ *
+ * bytes/s = 62.5 * bytes256 * ClkFreq_ms
+ */
+static u64 chan_rate(struct adapter *adap, unsigned int bytes256)
+{
+	u64 v = bytes256 * adap->params.vpd.cclk;
+
+	return v * 62 + v / 2;
+}
+
+/**
+ *	t4_get_chan_txrate - get the current per channel Tx rates
+ *	@adap: the adapter
+ *	@nic_rate: rates for NIC traffic
+ *	@ofld_rate: rates for offloaded traffic
+ *
+ *	Return the current Tx rates in bytes/s for NIC and offloaded traffic
+ *	for each channel.
+ */
+void t4_get_chan_txrate(struct adapter *adap, u64 *nic_rate, u64 *ofld_rate)
+{
+	u32 v;
+
+	v = t4_read_reg(adap, TP_TX_TRATE_A);
+	nic_rate[0] = chan_rate(adap, TNLRATE0_G(v));
+	nic_rate[1] = chan_rate(adap, TNLRATE1_G(v));
+	if (adap->params.arch.nchan == NCHAN) {
+		nic_rate[2] = chan_rate(adap, TNLRATE2_G(v));
+		nic_rate[3] = chan_rate(adap, TNLRATE3_G(v));
+	}
+
+	v = t4_read_reg(adap, TP_TX_ORATE_A);
+	ofld_rate[0] = chan_rate(adap, OFDRATE0_G(v));
+	ofld_rate[1] = chan_rate(adap, OFDRATE1_G(v));
+	if (adap->params.arch.nchan == NCHAN) {
+		ofld_rate[2] = chan_rate(adap, OFDRATE2_G(v));
+		ofld_rate[3] = chan_rate(adap, OFDRATE3_G(v));
+	}
+}
+
 /**
  *	t4_pmtx_get_stats - returns the HW stats from PMTX
  *	@adap: the adapter
@@ -3401,7 +4292,7 @@ void t4_pmrx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[])
 }
 
 /**
- *	get_mps_bg_map - return the buffer groups associated with a port
+ *	t4_get_mps_bg_map - return the buffer groups associated with a port
  *	@adap: the adapter
  *	@idx: the port index
  *
@@ -3409,7 +4300,7 @@ void t4_pmrx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[])
  *	with the given port.  Bit i is set if buffer group i is used by the
  *	port.
  */
-static unsigned int get_mps_bg_map(struct adapter *adap, int idx)
+unsigned int t4_get_mps_bg_map(struct adapter *adap, int idx)
 {
 	u32 n = NUMPORTS_G(t4_read_reg(adap, MPS_CMN_CTL_A));
 
@@ -3451,6 +4342,28 @@ const char *t4_get_port_type_description(enum fw_port_type port_type)
 }
 
 /**
+ *      t4_get_port_stats_offset - collect port stats relative to a previous
+ *                                 snapshot
+ *      @adap: The adapter
+ *      @idx: The port
+ *      @stats: Current stats to fill
+ *      @offset: Previous stats snapshot
+ */
+void t4_get_port_stats_offset(struct adapter *adap, int idx,
+			      struct port_stats *stats,
+			      struct port_stats *offset)
+{
+	u64 *s, *o;
+	int i;
+
+	t4_get_port_stats(adap, idx, stats);
+	for (i = 0, s = (u64 *)stats, o = (u64 *)offset;
+			i < (sizeof(struct port_stats) / sizeof(u64));
+			i++, s++, o++)
+		*s -= *o;
+}
+
+/**
  *	t4_get_port_stats - collect port statistics
  *	@adap: the adapter
  *	@idx: the port index
@@ -3460,7 +4373,7 @@ const char *t4_get_port_type_description(enum fw_port_type port_type)
  */
 void t4_get_port_stats(struct adapter *adap, int idx, struct port_stats *p)
 {
-	u32 bgmap = get_mps_bg_map(adap, idx);
+	u32 bgmap = t4_get_mps_bg_map(adap, idx);
 
 #define GET_STAT(name) \
 	t4_read_reg64(adap, \
@@ -3534,103 +4447,51 @@ void t4_get_port_stats(struct adapter *adap, int idx, struct port_stats *p)
 }
 
 /**
- *	t4_wol_magic_enable - enable/disable magic packet WoL
- *	@adap: the adapter
- *	@port: the physical port index
- *	@addr: MAC address expected in magic packets, %NULL to disable
- *
- *	Enables/disables magic packet wake-on-LAN for the selected port.
- */
-void t4_wol_magic_enable(struct adapter *adap, unsigned int port,
-			 const u8 *addr)
-{
-	u32 mag_id_reg_l, mag_id_reg_h, port_cfg_reg;
-
-	if (is_t4(adap->params.chip)) {
-		mag_id_reg_l = PORT_REG(port, XGMAC_PORT_MAGIC_MACID_LO);
-		mag_id_reg_h = PORT_REG(port, XGMAC_PORT_MAGIC_MACID_HI);
-		port_cfg_reg = PORT_REG(port, XGMAC_PORT_CFG2_A);
-	} else {
-		mag_id_reg_l = T5_PORT_REG(port, MAC_PORT_MAGIC_MACID_LO);
-		mag_id_reg_h = T5_PORT_REG(port, MAC_PORT_MAGIC_MACID_HI);
-		port_cfg_reg = T5_PORT_REG(port, MAC_PORT_CFG2_A);
-	}
-
-	if (addr) {
-		t4_write_reg(adap, mag_id_reg_l,
-			     (addr[2] << 24) | (addr[3] << 16) |
-			     (addr[4] << 8) | addr[5]);
-		t4_write_reg(adap, mag_id_reg_h,
-			     (addr[0] << 8) | addr[1]);
-	}
-	t4_set_reg_field(adap, port_cfg_reg, MAGICEN_F,
-			 addr ? MAGICEN_F : 0);
-}
-
-/**
- *	t4_wol_pat_enable - enable/disable pattern-based WoL
+ *	t4_get_lb_stats - collect loopback port statistics
  *	@adap: the adapter
- *	@port: the physical port index
- *	@map: bitmap of which HW pattern filters to set
- *	@mask0: byte mask for bytes 0-63 of a packet
- *	@mask1: byte mask for bytes 64-127 of a packet
- *	@crc: Ethernet CRC for selected bytes
- *	@enable: enable/disable switch
+ *	@idx: the loopback port index
+ *	@p: the stats structure to fill
  *
- *	Sets the pattern filters indicated in @map to mask out the bytes
- *	specified in @mask0/@mask1 in received packets and compare the CRC of
- *	the resulting packet against @crc.  If @enable is %true pattern-based
- *	WoL is enabled, otherwise disabled.
+ *	Return HW statistics for the given loopback port.
  */
-int t4_wol_pat_enable(struct adapter *adap, unsigned int port, unsigned int map,
-		      u64 mask0, u64 mask1, unsigned int crc, bool enable)
+void t4_get_lb_stats(struct adapter *adap, int idx, struct lb_port_stats *p)
 {
-	int i;
-	u32 port_cfg_reg;
-
-	if (is_t4(adap->params.chip))
-		port_cfg_reg = PORT_REG(port, XGMAC_PORT_CFG2_A);
-	else
-		port_cfg_reg = T5_PORT_REG(port, MAC_PORT_CFG2_A);
-
-	if (!enable) {
-		t4_set_reg_field(adap, port_cfg_reg, PATEN_F, 0);
-		return 0;
-	}
-	if (map > 0xff)
-		return -EINVAL;
+	u32 bgmap = t4_get_mps_bg_map(adap, idx);
 
-#define EPIO_REG(name) \
+#define GET_STAT(name) \
+	t4_read_reg64(adap, \
 	(is_t4(adap->params.chip) ? \
-	 PORT_REG(port, XGMAC_PORT_EPIO_##name##_A) : \
-	 T5_PORT_REG(port, MAC_PORT_EPIO_##name##_A))
-
-	t4_write_reg(adap, EPIO_REG(DATA1), mask0 >> 32);
-	t4_write_reg(adap, EPIO_REG(DATA2), mask1);
-	t4_write_reg(adap, EPIO_REG(DATA3), mask1 >> 32);
-
-	for (i = 0; i < NWOL_PAT; i++, map >>= 1) {
-		if (!(map & 1))
-			continue;
+	PORT_REG(idx, MPS_PORT_STAT_LB_PORT_##name##_L) : \
+	T5_PORT_REG(idx, MPS_PORT_STAT_LB_PORT_##name##_L)))
+#define GET_STAT_COM(name) t4_read_reg64(adap, MPS_STAT_##name##_L)
 
-		/* write byte masks */
-		t4_write_reg(adap, EPIO_REG(DATA0), mask0);
-		t4_write_reg(adap, EPIO_REG(OP), ADDRESS_V(i) | EPIOWR_F);
-		t4_read_reg(adap, EPIO_REG(OP));                /* flush */
-		if (t4_read_reg(adap, EPIO_REG(OP)) & SF_BUSY_F)
-			return -ETIMEDOUT;
-
-		/* write CRC */
-		t4_write_reg(adap, EPIO_REG(DATA0), crc);
-		t4_write_reg(adap, EPIO_REG(OP), ADDRESS_V(i + 32) | EPIOWR_F);
-		t4_read_reg(adap, EPIO_REG(OP));                /* flush */
-		if (t4_read_reg(adap, EPIO_REG(OP)) & SF_BUSY_F)
-			return -ETIMEDOUT;
-	}
-#undef EPIO_REG
+	p->octets           = GET_STAT(BYTES);
+	p->frames           = GET_STAT(FRAMES);
+	p->bcast_frames     = GET_STAT(BCAST);
+	p->mcast_frames     = GET_STAT(MCAST);
+	p->ucast_frames     = GET_STAT(UCAST);
+	p->error_frames     = GET_STAT(ERROR);
+
+	p->frames_64        = GET_STAT(64B);
+	p->frames_65_127    = GET_STAT(65B_127B);
+	p->frames_128_255   = GET_STAT(128B_255B);
+	p->frames_256_511   = GET_STAT(256B_511B);
+	p->frames_512_1023  = GET_STAT(512B_1023B);
+	p->frames_1024_1518 = GET_STAT(1024B_1518B);
+	p->frames_1519_max  = GET_STAT(1519B_MAX);
+	p->drop             = GET_STAT(DROP_FRAMES);
+
+	p->ovflow0 = (bgmap & 1) ? GET_STAT_COM(RX_BG_0_LB_DROP_FRAME) : 0;
+	p->ovflow1 = (bgmap & 2) ? GET_STAT_COM(RX_BG_1_LB_DROP_FRAME) : 0;
+	p->ovflow2 = (bgmap & 4) ? GET_STAT_COM(RX_BG_2_LB_DROP_FRAME) : 0;
+	p->ovflow3 = (bgmap & 8) ? GET_STAT_COM(RX_BG_3_LB_DROP_FRAME) : 0;
+	p->trunc0 = (bgmap & 1) ? GET_STAT_COM(RX_BG_0_LB_TRUNC_FRAME) : 0;
+	p->trunc1 = (bgmap & 2) ? GET_STAT_COM(RX_BG_1_LB_TRUNC_FRAME) : 0;
+	p->trunc2 = (bgmap & 4) ? GET_STAT_COM(RX_BG_2_LB_TRUNC_FRAME) : 0;
+	p->trunc3 = (bgmap & 8) ? GET_STAT_COM(RX_BG_3_LB_TRUNC_FRAME) : 0;
 
-	t4_set_reg_field(adap, PORT_REG(port, XGMAC_PORT_CFG2_A), 0, PATEN_F);
-	return 0;
+#undef GET_STAT
+#undef GET_STAT_COM
 }
 
 /*     t4_mk_filtdelwr - create a delete filter WR
@@ -3644,33 +4505,38 @@ int t4_wol_pat_enable(struct adapter *adap, unsigned int port, unsigned int map,
 void t4_mk_filtdelwr(unsigned int ftid, struct fw_filter_wr *wr, int qid)
 {
 	memset(wr, 0, sizeof(*wr));
-	wr->op_pkd = htonl(FW_WR_OP_V(FW_FILTER_WR));
-	wr->len16_pkd = htonl(FW_WR_LEN16_V(sizeof(*wr) / 16));
-	wr->tid_to_iq = htonl(FW_FILTER_WR_TID_V(ftid) |
-			FW_FILTER_WR_NOREPLY_V(qid < 0));
-	wr->del_filter_to_l2tix = htonl(FW_FILTER_WR_DEL_FILTER_F);
+	wr->op_pkd = cpu_to_be32(FW_WR_OP_V(FW_FILTER_WR));
+	wr->len16_pkd = cpu_to_be32(FW_WR_LEN16_V(sizeof(*wr) / 16));
+	wr->tid_to_iq = cpu_to_be32(FW_FILTER_WR_TID_V(ftid) |
+				    FW_FILTER_WR_NOREPLY_V(qid < 0));
+	wr->del_filter_to_l2tix = cpu_to_be32(FW_FILTER_WR_DEL_FILTER_F);
 	if (qid >= 0)
-		wr->rx_chan_rx_rpl_iq = htons(FW_FILTER_WR_RX_RPL_IQ_V(qid));
+		wr->rx_chan_rx_rpl_iq =
+			cpu_to_be16(FW_FILTER_WR_RX_RPL_IQ_V(qid));
 }
 
 #define INIT_CMD(var, cmd, rd_wr) do { \
-	(var).op_to_write = htonl(FW_CMD_OP_V(FW_##cmd##_CMD) | \
-				  FW_CMD_REQUEST_F | FW_CMD_##rd_wr##_F); \
-	(var).retval_len16 = htonl(FW_LEN16(var)); \
+	(var).op_to_write = cpu_to_be32(FW_CMD_OP_V(FW_##cmd##_CMD) | \
+					FW_CMD_REQUEST_F | \
+					FW_CMD_##rd_wr##_F); \
+	(var).retval_len16 = cpu_to_be32(FW_LEN16(var)); \
 } while (0)
 
 int t4_fwaddrspace_write(struct adapter *adap, unsigned int mbox,
 			  u32 addr, u32 val)
 {
+	u32 ldst_addrspace;
 	struct fw_ldst_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_addrspace = htonl(FW_CMD_OP_V(FW_LDST_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_WRITE_F |
-			    FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_FIRMWARE));
-	c.cycles_to_len16 = htonl(FW_LEN16(c));
-	c.u.addrval.addr = htonl(addr);
-	c.u.addrval.val = htonl(val);
+	ldst_addrspace = FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_FIRMWARE);
+	c.op_to_addrspace = cpu_to_be32(FW_CMD_OP_V(FW_LDST_CMD) |
+					FW_CMD_REQUEST_F |
+					FW_CMD_WRITE_F |
+					ldst_addrspace);
+	c.cycles_to_len16 = cpu_to_be32(FW_LEN16(c));
+	c.u.addrval.addr = cpu_to_be32(addr);
+	c.u.addrval.val = cpu_to_be32(val);
 
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
@@ -3690,19 +4556,22 @@ int t4_mdio_rd(struct adapter *adap, unsigned int mbox, unsigned int phy_addr,
 	       unsigned int mmd, unsigned int reg, u16 *valp)
 {
 	int ret;
+	u32 ldst_addrspace;
 	struct fw_ldst_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_addrspace = htonl(FW_CMD_OP_V(FW_LDST_CMD) | FW_CMD_REQUEST_F |
-		FW_CMD_READ_F | FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_MDIO));
-	c.cycles_to_len16 = htonl(FW_LEN16(c));
-	c.u.mdio.paddr_mmd = htons(FW_LDST_CMD_PADDR_V(phy_addr) |
-				   FW_LDST_CMD_MMD_V(mmd));
-	c.u.mdio.raddr = htons(reg);
+	ldst_addrspace = FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_MDIO);
+	c.op_to_addrspace = cpu_to_be32(FW_CMD_OP_V(FW_LDST_CMD) |
+					FW_CMD_REQUEST_F | FW_CMD_READ_F |
+					ldst_addrspace);
+	c.cycles_to_len16 = cpu_to_be32(FW_LEN16(c));
+	c.u.mdio.paddr_mmd = cpu_to_be16(FW_LDST_CMD_PADDR_V(phy_addr) |
+					 FW_LDST_CMD_MMD_V(mmd));
+	c.u.mdio.raddr = cpu_to_be16(reg);
 
 	ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
 	if (ret == 0)
-		*valp = ntohs(c.u.mdio.rval);
+		*valp = be16_to_cpu(c.u.mdio.rval);
 	return ret;
 }
 
@@ -3720,16 +4589,19 @@ int t4_mdio_rd(struct adapter *adap, unsigned int mbox, unsigned int phy_addr,
 int t4_mdio_wr(struct adapter *adap, unsigned int mbox, unsigned int phy_addr,
 	       unsigned int mmd, unsigned int reg, u16 val)
 {
+	u32 ldst_addrspace;
 	struct fw_ldst_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_addrspace = htonl(FW_CMD_OP_V(FW_LDST_CMD) | FW_CMD_REQUEST_F |
-		FW_CMD_WRITE_F | FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_MDIO));
-	c.cycles_to_len16 = htonl(FW_LEN16(c));
-	c.u.mdio.paddr_mmd = htons(FW_LDST_CMD_PADDR_V(phy_addr) |
-				   FW_LDST_CMD_MMD_V(mmd));
-	c.u.mdio.raddr = htons(reg);
-	c.u.mdio.rval = htons(val);
+	ldst_addrspace = FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_MDIO);
+	c.op_to_addrspace = cpu_to_be32(FW_CMD_OP_V(FW_LDST_CMD) |
+					FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
+					ldst_addrspace);
+	c.cycles_to_len16 = cpu_to_be32(FW_LEN16(c));
+	c.u.mdio.paddr_mmd = cpu_to_be16(FW_LDST_CMD_PADDR_V(phy_addr) |
+					 FW_LDST_CMD_MMD_V(mmd));
+	c.u.mdio.raddr = cpu_to_be16(reg);
+	c.u.mdio.rval = cpu_to_be16(val);
 
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
@@ -3841,6 +4713,32 @@ void t4_sge_decode_idma_state(struct adapter *adapter, int state)
 }
 
 /**
+ *      t4_sge_ctxt_flush - flush the SGE context cache
+ *      @adap: the adapter
+ *      @mbox: mailbox to use for the FW command
+ *
+ *      Issues a FW command through the given mailbox to flush the
+ *      SGE context cache.
+ */
+int t4_sge_ctxt_flush(struct adapter *adap, unsigned int mbox)
+{
+	int ret;
+	u32 ldst_addrspace;
+	struct fw_ldst_cmd c;
+
+	memset(&c, 0, sizeof(c));
+	ldst_addrspace = FW_LDST_CMD_ADDRSPACE_V(FW_LDST_ADDRSPC_SGE_EGRC);
+	c.op_to_addrspace = cpu_to_be32(FW_CMD_OP_V(FW_LDST_CMD) |
+					FW_CMD_REQUEST_F | FW_CMD_READ_F |
+					ldst_addrspace);
+	c.cycles_to_len16 = cpu_to_be32(FW_LEN16(c));
+	c.u.idctxt.msg_ctxtflush = cpu_to_be32(FW_LDST_CMD_CTXTFLUSH_F);
+
+	ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
+	return ret;
+}
+
+/**
  *      t4_fw_hello - establish communication with FW
  *      @adap: the adapter
  *      @mbox: mailbox to use for the FW command
@@ -3863,11 +4761,11 @@ int t4_fw_hello(struct adapter *adap, unsigned int mbox, unsigned int evt_mbox,
 retry:
 	memset(&c, 0, sizeof(c));
 	INIT_CMD(c, HELLO, WRITE);
-	c.err_to_clearinit = htonl(
+	c.err_to_clearinit = cpu_to_be32(
 		FW_HELLO_CMD_MASTERDIS_V(master == MASTER_CANT) |
 		FW_HELLO_CMD_MASTERFORCE_V(master == MASTER_MUST) |
-		FW_HELLO_CMD_MBMASTER_V(master == MASTER_MUST ? mbox :
-				      FW_HELLO_CMD_MBMASTER_M) |
+		FW_HELLO_CMD_MBMASTER_V(master == MASTER_MUST ?
+					mbox : FW_HELLO_CMD_MBMASTER_M) |
 		FW_HELLO_CMD_MBASYNCNOT_V(evt_mbox) |
 		FW_HELLO_CMD_STAGE_V(fw_hello_cmd_stage_os) |
 		FW_HELLO_CMD_CLEARINIT_F);
@@ -3888,7 +4786,7 @@ retry:
 		return ret;
 	}
 
-	v = ntohl(c.err_to_clearinit);
+	v = be32_to_cpu(c.err_to_clearinit);
 	master_mbox = FW_HELLO_CMD_MBMASTER_G(v);
 	if (state) {
 		if (v & FW_HELLO_CMD_ERR_F)
@@ -4017,7 +4915,7 @@ int t4_fw_reset(struct adapter *adap, unsigned int mbox, int reset)
 
 	memset(&c, 0, sizeof(c));
 	INIT_CMD(c, RESET, WRITE);
-	c.val = htonl(reset);
+	c.val = cpu_to_be32(reset);
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -4050,8 +4948,8 @@ static int t4_fw_halt(struct adapter *adap, unsigned int mbox, int force)
 
 		memset(&c, 0, sizeof(c));
 		INIT_CMD(c, RESET, WRITE);
-		c.val = htonl(PIORST_F | PIORSTMODE_F);
-		c.halt_pkd = htonl(FW_RESET_CMD_HALT_F);
+		c.val = cpu_to_be32(PIORST_F | PIORSTMODE_F);
+		c.halt_pkd = cpu_to_be32(FW_RESET_CMD_HALT_F);
 		ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 	}
 
@@ -4190,7 +5088,7 @@ int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
 	 * the newly loaded firmware will handle this right by checking
 	 * its header flags to see if it advertises the capability.
 	 */
-	reset = ((ntohl(fw_hdr->flags) & FW_HDR_FLAGS_RESET_HALT) == 0);
+	reset = ((be32_to_cpu(fw_hdr->flags) & FW_HDR_FLAGS_RESET_HALT) == 0);
 	return t4_fw_restart(adap, mbox, reset);
 }
 
@@ -4321,7 +5219,7 @@ int t4_fw_initialize(struct adapter *adap, unsigned int mbox)
 }
 
 /**
- *	t4_query_params - query FW or device parameters
+ *	t4_query_params_rw - query FW or device parameters
  *	@adap: the adapter
  *	@mbox: mailbox to use for the FW command
  *	@pf: the PF
@@ -4329,13 +5227,14 @@ int t4_fw_initialize(struct adapter *adap, unsigned int mbox)
  *	@nparams: the number of parameters
  *	@params: the parameter names
  *	@val: the parameter values
+ *	@rw: Write and read flag
  *
  *	Reads the value of FW or device parameters.  Up to 7 parameters can be
  *	queried at once.
  */
-int t4_query_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
-		    unsigned int vf, unsigned int nparams, const u32 *params,
-		    u32 *val)
+int t4_query_params_rw(struct adapter *adap, unsigned int mbox, unsigned int pf,
+		       unsigned int vf, unsigned int nparams, const u32 *params,
+		       u32 *val, int rw)
 {
 	int i, ret;
 	struct fw_params_cmd c;
@@ -4345,22 +5244,35 @@ int t4_query_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		return -EINVAL;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_PARAMS_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_READ_F | FW_PARAMS_CMD_PFN_V(pf) |
-			    FW_PARAMS_CMD_VFN_V(vf));
-	c.retval_len16 = htonl(FW_LEN16(c));
-	for (i = 0; i < nparams; i++, p += 2)
-		*p = htonl(*params++);
+	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_PARAMS_CMD) |
+				  FW_CMD_REQUEST_F | FW_CMD_READ_F |
+				  FW_PARAMS_CMD_PFN_V(pf) |
+				  FW_PARAMS_CMD_VFN_V(vf));
+	c.retval_len16 = cpu_to_be32(FW_LEN16(c));
+
+	for (i = 0; i < nparams; i++) {
+		*p++ = cpu_to_be32(*params++);
+		if (rw)
+			*p = cpu_to_be32(*(val + i));
+		p++;
+	}
 
 	ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
 	if (ret == 0)
 		for (i = 0, p = &c.param[0].val; i < nparams; i++, p += 2)
-			*val++ = ntohl(*p);
+			*val++ = be32_to_cpu(*p);
 	return ret;
 }
 
+int t4_query_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
+		    unsigned int vf, unsigned int nparams, const u32 *params,
+		    u32 *val)
+{
+	return t4_query_params_rw(adap, mbox, pf, vf, nparams, params, val, 0);
+}
+
 /**
- *      t4_set_params_nosleep - sets FW or device parameters
+ *      t4_set_params_timeout - sets FW or device parameters
  *      @adap: the adapter
  *      @mbox: mailbox to use for the FW command
  *      @pf: the PF
@@ -4368,15 +5280,15 @@ int t4_query_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
  *      @nparams: the number of parameters
  *      @params: the parameter names
  *      @val: the parameter values
+ *      @timeout: the timeout time
  *
- *	 Does not ever sleep
  *      Sets the value of FW or device parameters.  Up to 7 parameters can be
  *      specified at once.
  */
-int t4_set_params_nosleep(struct adapter *adap, unsigned int mbox,
+int t4_set_params_timeout(struct adapter *adap, unsigned int mbox,
 			  unsigned int pf, unsigned int vf,
 			  unsigned int nparams, const u32 *params,
-			  const u32 *val)
+			  const u32 *val, int timeout)
 {
 	struct fw_params_cmd c;
 	__be32 *p = &c.param[0].mnem;
@@ -4386,9 +5298,9 @@ int t4_set_params_nosleep(struct adapter *adap, unsigned int mbox,
 
 	memset(&c, 0, sizeof(c));
 	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_PARAMS_CMD) |
-				FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
-				FW_PARAMS_CMD_PFN_V(pf) |
-				FW_PARAMS_CMD_VFN_V(vf));
+				  FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
+				  FW_PARAMS_CMD_PFN_V(pf) |
+				  FW_PARAMS_CMD_VFN_V(vf));
 	c.retval_len16 = cpu_to_be32(FW_LEN16(c));
 
 	while (nparams--) {
@@ -4396,7 +5308,7 @@ int t4_set_params_nosleep(struct adapter *adap, unsigned int mbox,
 		*p++ = cpu_to_be32(*val++);
 	}
 
-	return t4_wr_mbox_ns(adap, mbox, &c, sizeof(c), NULL);
+	return t4_wr_mbox_timeout(adap, mbox, &c, sizeof(c), NULL, timeout);
 }
 
 /**
@@ -4416,23 +5328,8 @@ int t4_set_params(struct adapter *adap, unsigned int mbox, unsigned int pf,
 		  unsigned int vf, unsigned int nparams, const u32 *params,
 		  const u32 *val)
 {
-	struct fw_params_cmd c;
-	__be32 *p = &c.param[0].mnem;
-
-	if (nparams > 7)
-		return -EINVAL;
-
-	memset(&c, 0, sizeof(c));
-	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_PARAMS_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_WRITE_F | FW_PARAMS_CMD_PFN_V(pf) |
-			    FW_PARAMS_CMD_VFN_V(vf));
-	c.retval_len16 = htonl(FW_LEN16(c));
-	while (nparams--) {
-		*p++ = htonl(*params++);
-		*p++ = htonl(*val++);
-	}
-
-	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
+	return t4_set_params_timeout(adap, mbox, pf, vf, nparams, params, val,
+				     FW_CMD_MAX_TIMEOUT);
 }
 
 /**
@@ -4465,20 +5362,21 @@ int t4_cfg_pfvf(struct adapter *adap, unsigned int mbox, unsigned int pf,
 	struct fw_pfvf_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_PFVF_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_WRITE_F | FW_PFVF_CMD_PFN_V(pf) |
-			    FW_PFVF_CMD_VFN_V(vf));
-	c.retval_len16 = htonl(FW_LEN16(c));
-	c.niqflint_niq = htonl(FW_PFVF_CMD_NIQFLINT_V(rxqi) |
-			       FW_PFVF_CMD_NIQ_V(rxq));
-	c.type_to_neq = htonl(FW_PFVF_CMD_CMASK_V(cmask) |
-			       FW_PFVF_CMD_PMASK_V(pmask) |
-			       FW_PFVF_CMD_NEQ_V(txq));
-	c.tc_to_nexactf = htonl(FW_PFVF_CMD_TC_V(tc) | FW_PFVF_CMD_NVI_V(vi) |
-				FW_PFVF_CMD_NEXACTF_V(nexact));
-	c.r_caps_to_nethctrl = htonl(FW_PFVF_CMD_R_CAPS_V(rcaps) |
-				     FW_PFVF_CMD_WX_CAPS_V(wxcaps) |
-				     FW_PFVF_CMD_NETHCTRL_V(txq_eth_ctrl));
+	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_PFVF_CMD) | FW_CMD_REQUEST_F |
+				  FW_CMD_WRITE_F | FW_PFVF_CMD_PFN_V(pf) |
+				  FW_PFVF_CMD_VFN_V(vf));
+	c.retval_len16 = cpu_to_be32(FW_LEN16(c));
+	c.niqflint_niq = cpu_to_be32(FW_PFVF_CMD_NIQFLINT_V(rxqi) |
+				     FW_PFVF_CMD_NIQ_V(rxq));
+	c.type_to_neq = cpu_to_be32(FW_PFVF_CMD_CMASK_V(cmask) |
+				    FW_PFVF_CMD_PMASK_V(pmask) |
+				    FW_PFVF_CMD_NEQ_V(txq));
+	c.tc_to_nexactf = cpu_to_be32(FW_PFVF_CMD_TC_V(tc) |
+				      FW_PFVF_CMD_NVI_V(vi) |
+				      FW_PFVF_CMD_NEXACTF_V(nexact));
+	c.r_caps_to_nethctrl = cpu_to_be32(FW_PFVF_CMD_R_CAPS_V(rcaps) |
+					FW_PFVF_CMD_WX_CAPS_V(wxcaps) |
+					FW_PFVF_CMD_NETHCTRL_V(txq_eth_ctrl));
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -4507,10 +5405,10 @@ int t4_alloc_vi(struct adapter *adap, unsigned int mbox, unsigned int port,
 	struct fw_vi_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_VI_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_WRITE_F | FW_CMD_EXEC_F |
-			    FW_VI_CMD_PFN_V(pf) | FW_VI_CMD_VFN_V(vf));
-	c.alloc_to_len16 = htonl(FW_VI_CMD_ALLOC_F | FW_LEN16(c));
+	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_VI_CMD) | FW_CMD_REQUEST_F |
+				  FW_CMD_WRITE_F | FW_CMD_EXEC_F |
+				  FW_VI_CMD_PFN_V(pf) | FW_VI_CMD_VFN_V(vf));
+	c.alloc_to_len16 = cpu_to_be32(FW_VI_CMD_ALLOC_F | FW_LEN16(c));
 	c.portid_pkd = FW_VI_CMD_PORTID_V(port);
 	c.nmac = nmac - 1;
 
@@ -4532,8 +5430,35 @@ int t4_alloc_vi(struct adapter *adap, unsigned int mbox, unsigned int port,
 		}
 	}
 	if (rss_size)
-		*rss_size = FW_VI_CMD_RSSSIZE_G(ntohs(c.rsssize_pkd));
-	return FW_VI_CMD_VIID_G(ntohs(c.type_viid));
+		*rss_size = FW_VI_CMD_RSSSIZE_G(be16_to_cpu(c.rsssize_pkd));
+	return FW_VI_CMD_VIID_G(be16_to_cpu(c.type_viid));
+}
+
+/**
+ *	t4_free_vi - free a virtual interface
+ *	@adap: the adapter
+ *	@mbox: mailbox to use for the FW command
+ *	@pf: the PF owning the VI
+ *	@vf: the VF owning the VI
+ *	@viid: virtual interface identifiler
+ *
+ *	Free a previously allocated virtual interface.
+ */
+int t4_free_vi(struct adapter *adap, unsigned int mbox, unsigned int pf,
+	       unsigned int vf, unsigned int viid)
+{
+	struct fw_vi_cmd c;
+
+	memset(&c, 0, sizeof(c));
+	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_VI_CMD) |
+				  FW_CMD_REQUEST_F |
+				  FW_CMD_EXEC_F |
+				  FW_VI_CMD_PFN_V(pf) |
+				  FW_VI_CMD_VFN_V(vf));
+	c.alloc_to_len16 = cpu_to_be32(FW_VI_CMD_FREE_F | FW_LEN16(c));
+	c.type_viid = cpu_to_be16(FW_VI_CMD_VIID_V(viid));
+
+	return t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
 }
 
 /**
@@ -4569,14 +5494,16 @@ int t4_set_rxmode(struct adapter *adap, unsigned int mbox, unsigned int viid,
 		vlanex = FW_VI_RXMODE_CMD_VLANEXEN_M;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_viid = htonl(FW_CMD_OP_V(FW_VI_RXMODE_CMD) | FW_CMD_REQUEST_F |
-			     FW_CMD_WRITE_F | FW_VI_RXMODE_CMD_VIID_V(viid));
-	c.retval_len16 = htonl(FW_LEN16(c));
-	c.mtu_to_vlanexen = htonl(FW_VI_RXMODE_CMD_MTU_V(mtu) |
-				  FW_VI_RXMODE_CMD_PROMISCEN_V(promisc) |
-				  FW_VI_RXMODE_CMD_ALLMULTIEN_V(all_multi) |
-				  FW_VI_RXMODE_CMD_BROADCASTEN_V(bcast) |
-				  FW_VI_RXMODE_CMD_VLANEXEN_V(vlanex));
+	c.op_to_viid = cpu_to_be32(FW_CMD_OP_V(FW_VI_RXMODE_CMD) |
+				   FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
+				   FW_VI_RXMODE_CMD_VIID_V(viid));
+	c.retval_len16 = cpu_to_be32(FW_LEN16(c));
+	c.mtu_to_vlanexen =
+		cpu_to_be32(FW_VI_RXMODE_CMD_MTU_V(mtu) |
+			    FW_VI_RXMODE_CMD_PROMISCEN_V(promisc) |
+			    FW_VI_RXMODE_CMD_ALLMULTIEN_V(all_multi) |
+			    FW_VI_RXMODE_CMD_BROADCASTEN_V(bcast) |
+			    FW_VI_RXMODE_CMD_VLANEXEN_V(vlanex));
 	return t4_wr_mbox_meat(adap, mbox, &c, sizeof(c), NULL, sleep_ok);
 }
 
@@ -4606,43 +5533,71 @@ int t4_alloc_mac_filt(struct adapter *adap, unsigned int mbox,
 		      unsigned int viid, bool free, unsigned int naddr,
 		      const u8 **addr, u16 *idx, u64 *hash, bool sleep_ok)
 {
-	int i, ret;
+	int offset, ret = 0;
 	struct fw_vi_mac_cmd c;
-	struct fw_vi_mac_exact *p;
-	unsigned int max_naddr = is_t4(adap->params.chip) ?
-				       NUM_MPS_CLS_SRAM_L_INSTANCES :
-				       NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
+	unsigned int nfilters = 0;
+	unsigned int max_naddr = adap->params.arch.mps_tcam_size;
+	unsigned int rem = naddr;
 
-	if (naddr > 7)
+	if (naddr > max_naddr)
 		return -EINVAL;
 
-	memset(&c, 0, sizeof(c));
-	c.op_to_viid = htonl(FW_CMD_OP_V(FW_VI_MAC_CMD) | FW_CMD_REQUEST_F |
-			     FW_CMD_WRITE_F | (free ? FW_CMD_EXEC_F : 0) |
-			     FW_VI_MAC_CMD_VIID_V(viid));
-	c.freemacs_to_len16 = htonl(FW_VI_MAC_CMD_FREEMACS_V(free) |
-				    FW_CMD_LEN16_V((naddr + 2) / 2));
-
-	for (i = 0, p = c.u.exact; i < naddr; i++, p++) {
-		p->valid_to_idx = htons(FW_VI_MAC_CMD_VALID_F |
-				      FW_VI_MAC_CMD_IDX_V(FW_VI_MAC_ADD_MAC));
-		memcpy(p->macaddr, addr[i], sizeof(p->macaddr));
-	}
+	for (offset = 0; offset < naddr ; /**/) {
+		unsigned int fw_naddr = (rem < ARRAY_SIZE(c.u.exact) ?
+					 rem : ARRAY_SIZE(c.u.exact));
+		size_t len16 = DIV_ROUND_UP(offsetof(struct fw_vi_mac_cmd,
+						     u.exact[fw_naddr]), 16);
+		struct fw_vi_mac_exact *p;
+		int i;
 
-	ret = t4_wr_mbox_meat(adap, mbox, &c, sizeof(c), &c, sleep_ok);
-	if (ret)
-		return ret;
+		memset(&c, 0, sizeof(c));
+		c.op_to_viid = cpu_to_be32(FW_CMD_OP_V(FW_VI_MAC_CMD) |
+					   FW_CMD_REQUEST_F |
+					   FW_CMD_WRITE_F |
+					   FW_CMD_EXEC_V(free) |
+					   FW_VI_MAC_CMD_VIID_V(viid));
+		c.freemacs_to_len16 =
+			cpu_to_be32(FW_VI_MAC_CMD_FREEMACS_V(free) |
+				    FW_CMD_LEN16_V(len16));
+
+		for (i = 0, p = c.u.exact; i < fw_naddr; i++, p++) {
+			p->valid_to_idx =
+				cpu_to_be16(FW_VI_MAC_CMD_VALID_F |
+					    FW_VI_MAC_CMD_IDX_V(
+						    FW_VI_MAC_ADD_MAC));
+			memcpy(p->macaddr, addr[offset + i],
+			       sizeof(p->macaddr));
+		}
 
-	for (i = 0, p = c.u.exact; i < naddr; i++, p++) {
-		u16 index = FW_VI_MAC_CMD_IDX_G(ntohs(p->valid_to_idx));
+		/* It's okay if we run out of space in our MAC address arena.
+		 * Some of the addresses we submit may get stored so we need
+		 * to run through the reply to see what the results were ...
+		 */
+		ret = t4_wr_mbox_meat(adap, mbox, &c, sizeof(c), &c, sleep_ok);
+		if (ret && ret != -FW_ENOMEM)
+			break;
 
-		if (idx)
-			idx[i] = index >= max_naddr ? 0xffff : index;
-		if (index < max_naddr)
-			ret++;
-		else if (hash)
-			*hash |= (1ULL << hash_mac_addr(addr[i]));
+		for (i = 0, p = c.u.exact; i < fw_naddr; i++, p++) {
+			u16 index = FW_VI_MAC_CMD_IDX_G(
+					be16_to_cpu(p->valid_to_idx));
+
+			if (idx)
+				idx[offset + i] = (index >= max_naddr ?
+						   0xffff : index);
+			if (index < max_naddr)
+				nfilters++;
+			else if (hash)
+				*hash |= (1ULL <<
+					  hash_mac_addr(addr[offset + i]));
+		}
+
+		free = false;
+		offset += fw_naddr;
+		rem -= fw_naddr;
 	}
+
+	if (ret == 0 || ret == -FW_ENOMEM)
+		ret = nfilters;
 	return ret;
 }
 
@@ -4671,26 +5626,25 @@ int t4_change_mac(struct adapter *adap, unsigned int mbox, unsigned int viid,
 	int ret, mode;
 	struct fw_vi_mac_cmd c;
 	struct fw_vi_mac_exact *p = c.u.exact;
-	unsigned int max_mac_addr = is_t4(adap->params.chip) ?
-				    NUM_MPS_CLS_SRAM_L_INSTANCES :
-				    NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
+	unsigned int max_mac_addr = adap->params.arch.mps_tcam_size;
 
 	if (idx < 0)                             /* new allocation */
 		idx = persist ? FW_VI_MAC_ADD_PERSIST_MAC : FW_VI_MAC_ADD_MAC;
 	mode = add_smt ? FW_VI_MAC_SMT_AND_MPSTCAM : FW_VI_MAC_MPS_TCAM_ENTRY;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_viid = htonl(FW_CMD_OP_V(FW_VI_MAC_CMD) | FW_CMD_REQUEST_F |
-			     FW_CMD_WRITE_F | FW_VI_MAC_CMD_VIID_V(viid));
-	c.freemacs_to_len16 = htonl(FW_CMD_LEN16_V(1));
-	p->valid_to_idx = htons(FW_VI_MAC_CMD_VALID_F |
-				FW_VI_MAC_CMD_SMAC_RESULT_V(mode) |
-				FW_VI_MAC_CMD_IDX_V(idx));
+	c.op_to_viid = cpu_to_be32(FW_CMD_OP_V(FW_VI_MAC_CMD) |
+				   FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
+				   FW_VI_MAC_CMD_VIID_V(viid));
+	c.freemacs_to_len16 = cpu_to_be32(FW_CMD_LEN16_V(1));
+	p->valid_to_idx = cpu_to_be16(FW_VI_MAC_CMD_VALID_F |
+				      FW_VI_MAC_CMD_SMAC_RESULT_V(mode) |
+				      FW_VI_MAC_CMD_IDX_V(idx));
 	memcpy(p->macaddr, addr, sizeof(p->macaddr));
 
 	ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
 	if (ret == 0) {
-		ret = FW_VI_MAC_CMD_IDX_G(ntohs(p->valid_to_idx));
+		ret = FW_VI_MAC_CMD_IDX_G(be16_to_cpu(p->valid_to_idx));
 		if (ret >= max_mac_addr)
 			ret = -ENOMEM;
 	}
@@ -4714,11 +5668,12 @@ int t4_set_addr_hash(struct adapter *adap, unsigned int mbox, unsigned int viid,
 	struct fw_vi_mac_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_viid = htonl(FW_CMD_OP_V(FW_VI_MAC_CMD) | FW_CMD_REQUEST_F |
-			     FW_CMD_WRITE_F | FW_VI_ENABLE_CMD_VIID_V(viid));
-	c.freemacs_to_len16 = htonl(FW_VI_MAC_CMD_HASHVECEN_F |
-				    FW_VI_MAC_CMD_HASHUNIEN_V(ucast) |
-				    FW_CMD_LEN16_V(1));
+	c.op_to_viid = cpu_to_be32(FW_CMD_OP_V(FW_VI_MAC_CMD) |
+				   FW_CMD_REQUEST_F | FW_CMD_WRITE_F |
+				   FW_VI_ENABLE_CMD_VIID_V(viid));
+	c.freemacs_to_len16 = cpu_to_be32(FW_VI_MAC_CMD_HASHVECEN_F |
+					  FW_VI_MAC_CMD_HASHUNIEN_V(ucast) |
+					  FW_CMD_LEN16_V(1));
 	c.u.hash.hashvec = cpu_to_be64(vec);
 	return t4_wr_mbox_meat(adap, mbox, &c, sizeof(c), NULL, sleep_ok);
 }
@@ -4741,12 +5696,13 @@ int t4_enable_vi_params(struct adapter *adap, unsigned int mbox,
 	struct fw_vi_enable_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_viid = htonl(FW_CMD_OP_V(FW_VI_ENABLE_CMD) | FW_CMD_REQUEST_F |
-			     FW_CMD_EXEC_F | FW_VI_ENABLE_CMD_VIID_V(viid));
-
-	c.ien_to_len16 = htonl(FW_VI_ENABLE_CMD_IEN_V(rx_en) |
-			       FW_VI_ENABLE_CMD_EEN_V(tx_en) | FW_LEN16(c) |
-			       FW_VI_ENABLE_CMD_DCB_INFO_V(dcb_en));
+	c.op_to_viid = cpu_to_be32(FW_CMD_OP_V(FW_VI_ENABLE_CMD) |
+				   FW_CMD_REQUEST_F | FW_CMD_EXEC_F |
+				   FW_VI_ENABLE_CMD_VIID_V(viid));
+	c.ien_to_len16 = cpu_to_be32(FW_VI_ENABLE_CMD_IEN_V(rx_en) |
+				     FW_VI_ENABLE_CMD_EEN_V(tx_en) |
+				     FW_VI_ENABLE_CMD_DCB_INFO_V(dcb_en) |
+				     FW_LEN16(c));
 	return t4_wr_mbox_ns(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -4781,10 +5737,11 @@ int t4_identify_port(struct adapter *adap, unsigned int mbox, unsigned int viid,
 	struct fw_vi_enable_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_viid = htonl(FW_CMD_OP_V(FW_VI_ENABLE_CMD) | FW_CMD_REQUEST_F |
-			     FW_CMD_EXEC_F | FW_VI_ENABLE_CMD_VIID_V(viid));
-	c.ien_to_len16 = htonl(FW_VI_ENABLE_CMD_LED_F | FW_LEN16(c));
-	c.blinkdur = htons(nblinks);
+	c.op_to_viid = cpu_to_be32(FW_CMD_OP_V(FW_VI_ENABLE_CMD) |
+				   FW_CMD_REQUEST_F | FW_CMD_EXEC_F |
+				   FW_VI_ENABLE_CMD_VIID_V(viid));
+	c.ien_to_len16 = cpu_to_be32(FW_VI_ENABLE_CMD_LED_F | FW_LEN16(c));
+	c.blinkdur = cpu_to_be16(nblinks);
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -4808,14 +5765,14 @@ int t4_iq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 	struct fw_iq_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_IQ_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_EXEC_F | FW_IQ_CMD_PFN_V(pf) |
-			    FW_IQ_CMD_VFN_V(vf));
-	c.alloc_to_len16 = htonl(FW_IQ_CMD_FREE_F | FW_LEN16(c));
-	c.type_to_iqandstindex = htonl(FW_IQ_CMD_TYPE_V(iqtype));
-	c.iqid = htons(iqid);
-	c.fl0id = htons(fl0id);
-	c.fl1id = htons(fl1id);
+	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_IQ_CMD) | FW_CMD_REQUEST_F |
+				  FW_CMD_EXEC_F | FW_IQ_CMD_PFN_V(pf) |
+				  FW_IQ_CMD_VFN_V(vf));
+	c.alloc_to_len16 = cpu_to_be32(FW_IQ_CMD_FREE_F | FW_LEN16(c));
+	c.type_to_iqandstindex = cpu_to_be32(FW_IQ_CMD_TYPE_V(iqtype));
+	c.iqid = cpu_to_be16(iqid);
+	c.fl0id = cpu_to_be16(fl0id);
+	c.fl1id = cpu_to_be16(fl1id);
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -4835,11 +5792,12 @@ int t4_eth_eq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 	struct fw_eq_eth_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_EQ_ETH_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_EXEC_F | FW_EQ_ETH_CMD_PFN_V(pf) |
-			    FW_EQ_ETH_CMD_VFN_V(vf));
-	c.alloc_to_len16 = htonl(FW_EQ_ETH_CMD_FREE_F | FW_LEN16(c));
-	c.eqid_pkd = htonl(FW_EQ_ETH_CMD_EQID_V(eqid));
+	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_EQ_ETH_CMD) |
+				  FW_CMD_REQUEST_F | FW_CMD_EXEC_F |
+				  FW_EQ_ETH_CMD_PFN_V(pf) |
+				  FW_EQ_ETH_CMD_VFN_V(vf));
+	c.alloc_to_len16 = cpu_to_be32(FW_EQ_ETH_CMD_FREE_F | FW_LEN16(c));
+	c.eqid_pkd = cpu_to_be32(FW_EQ_ETH_CMD_EQID_V(eqid));
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -4859,11 +5817,12 @@ int t4_ctrl_eq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 	struct fw_eq_ctrl_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_EQ_CTRL_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_EXEC_F | FW_EQ_CTRL_CMD_PFN_V(pf) |
-			    FW_EQ_CTRL_CMD_VFN_V(vf));
-	c.alloc_to_len16 = htonl(FW_EQ_CTRL_CMD_FREE_F | FW_LEN16(c));
-	c.cmpliqid_eqid = htonl(FW_EQ_CTRL_CMD_EQID_V(eqid));
+	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_EQ_CTRL_CMD) |
+				  FW_CMD_REQUEST_F | FW_CMD_EXEC_F |
+				  FW_EQ_CTRL_CMD_PFN_V(pf) |
+				  FW_EQ_CTRL_CMD_VFN_V(vf));
+	c.alloc_to_len16 = cpu_to_be32(FW_EQ_CTRL_CMD_FREE_F | FW_LEN16(c));
+	c.cmpliqid_eqid = cpu_to_be32(FW_EQ_CTRL_CMD_EQID_V(eqid));
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -4883,11 +5842,12 @@ int t4_ofld_eq_free(struct adapter *adap, unsigned int mbox, unsigned int pf,
 	struct fw_eq_ofld_cmd c;
 
 	memset(&c, 0, sizeof(c));
-	c.op_to_vfn = htonl(FW_CMD_OP_V(FW_EQ_OFLD_CMD) | FW_CMD_REQUEST_F |
-			    FW_CMD_EXEC_F | FW_EQ_OFLD_CMD_PFN_V(pf) |
-			    FW_EQ_OFLD_CMD_VFN_V(vf));
-	c.alloc_to_len16 = htonl(FW_EQ_OFLD_CMD_FREE_F | FW_LEN16(c));
-	c.eqid_pkd = htonl(FW_EQ_OFLD_CMD_EQID_V(eqid));
+	c.op_to_vfn = cpu_to_be32(FW_CMD_OP_V(FW_EQ_OFLD_CMD) |
+				  FW_CMD_REQUEST_F | FW_CMD_EXEC_F |
+				  FW_EQ_OFLD_CMD_PFN_V(pf) |
+				  FW_EQ_OFLD_CMD_VFN_V(vf));
+	c.alloc_to_len16 = cpu_to_be32(FW_EQ_OFLD_CMD_FREE_F | FW_LEN16(c));
+	c.eqid_pkd = cpu_to_be32(FW_EQ_OFLD_CMD_EQID_V(eqid));
 	return t4_wr_mbox(adap, mbox, &c, sizeof(c), NULL);
 }
 
@@ -4905,11 +5865,11 @@ int t4_handle_fw_rpl(struct adapter *adap, const __be64 *rpl)
 	if (opcode == FW_PORT_CMD) {    /* link/module state change message */
 		int speed = 0, fc = 0;
 		const struct fw_port_cmd *p = (void *)rpl;
-		int chan = FW_PORT_CMD_PORTID_G(ntohl(p->op_to_portid));
+		int chan = FW_PORT_CMD_PORTID_G(be32_to_cpu(p->op_to_portid));
 		int port = adap->chan_map[chan];
 		struct port_info *pi = adap2pinfo(adap, port);
 		struct link_config *lc = &pi->link_cfg;
-		u32 stat = ntohl(p->u.info.lstatus_to_modtype);
+		u32 stat = be32_to_cpu(p->u.info.lstatus_to_modtype);
 		int link_ok = (stat & FW_PORT_CMD_LSTATUS_F) != 0;
 		u32 mod = FW_PORT_CMD_MODTYPE_G(stat);
 
@@ -5043,6 +6003,22 @@ static int get_flash_params(struct adapter *adap)
 	return 0;
 }
 
+static void set_pcie_completion_timeout(struct adapter *adapter, u8 range)
+{
+	u16 val;
+	u32 pcie_cap;
+
+	pcie_cap = pci_find_capability(adapter->pdev, PCI_CAP_ID_EXP);
+	if (pcie_cap) {
+		pci_read_config_word(adapter->pdev,
+				     pcie_cap + PCI_EXP_DEVCTL2, &val);
+		val &= ~PCI_EXP_DEVCTL2_COMP_TIMEOUT;
+		val |= range;
+		pci_write_config_word(adapter->pdev,
+				      pcie_cap + PCI_EXP_DEVCTL2, val);
+	}
+}
+
 /**
  *	t4_prep_adapter - prepare SW and HW for operation
  *	@adapter: the adapter
@@ -5075,9 +6051,30 @@ int t4_prep_adapter(struct adapter *adapter)
 	switch (ver) {
 	case CHELSIO_T4:
 		adapter->params.chip |= CHELSIO_CHIP_CODE(CHELSIO_T4, pl_rev);
+		adapter->params.arch.sge_fl_db = DBPRIO_F;
+		adapter->params.arch.mps_tcam_size =
+				 NUM_MPS_CLS_SRAM_L_INSTANCES;
+		adapter->params.arch.mps_rplc_size = 128;
+		adapter->params.arch.nchan = NCHAN;
+		adapter->params.arch.vfcount = 128;
 		break;
 	case CHELSIO_T5:
 		adapter->params.chip |= CHELSIO_CHIP_CODE(CHELSIO_T5, pl_rev);
+		adapter->params.arch.sge_fl_db = DBPRIO_F | DBTYPE_F;
+		adapter->params.arch.mps_tcam_size =
+				 NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
+		adapter->params.arch.mps_rplc_size = 128;
+		adapter->params.arch.nchan = NCHAN;
+		adapter->params.arch.vfcount = 128;
+		break;
+	case CHELSIO_T6:
+		adapter->params.chip |= CHELSIO_CHIP_CODE(CHELSIO_T6, pl_rev);
+		adapter->params.arch.sge_fl_db = 0;
+		adapter->params.arch.mps_tcam_size =
+				 NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
+		adapter->params.arch.mps_rplc_size = 256;
+		adapter->params.arch.nchan = 2;
+		adapter->params.arch.vfcount = 256;
 		break;
 	default:
 		dev_err(adapter->pdev_dev, "Device %d is not supported\n",
@@ -5094,14 +6091,18 @@ int t4_prep_adapter(struct adapter *adapter)
 	adapter->params.nports = 1;
 	adapter->params.portvec = 1;
 	adapter->params.vpd.cclk = 50000;
+
+	/* Set pci completion timeout value to 4 seconds. */
+	set_pcie_completion_timeout(adapter, 0xd);
 	return 0;
 }
 
 /**
- *	cxgb4_t4_bar2_sge_qregs - return BAR2 SGE Queue register information
+ *	t4_bar2_sge_qregs - return BAR2 SGE Queue register information
  *	@adapter: the adapter
  *	@qid: the Queue ID
  *	@qtype: the Ingress or Egress type for @qid
+ *	@user: true if this request is for a user mode queue
  *	@pbar2_qoffset: BAR2 Queue Offset
  *	@pbar2_qid: BAR2 Queue ID or 0 for Queue ID inferred SGE Queues
  *
@@ -5122,9 +6123,10 @@ int t4_prep_adapter(struct adapter *adapter)
  *	Write Combining Doorbell Buffer. If the BAR2 Queue ID is not 0,
  *	then these "Inferred Queue ID" register may not be used.
  */
-int cxgb4_t4_bar2_sge_qregs(struct adapter *adapter,
+int t4_bar2_sge_qregs(struct adapter *adapter,
 		      unsigned int qid,
 		      enum t4_bar2_qtype qtype,
+		      int user,
 		      u64 *pbar2_qoffset,
 		      unsigned int *pbar2_qid)
 {
@@ -5132,9 +6134,8 @@ int cxgb4_t4_bar2_sge_qregs(struct adapter *adapter,
 	u64 bar2_page_offset, bar2_qoffset;
 	unsigned int bar2_qid, bar2_qid_offset, bar2_qinferred;
 
-	/* T4 doesn't support BAR2 SGE Queue registers.
-	 */
-	if (is_t4(adapter->params.chip))
+	/* T4 doesn't support BAR2 SGE Queue registers for kernel mode queues */
+	if (!user && is_t4(adapter->params.chip))
 		return -EINVAL;
 
 	/* Get our SGE Page Size parameters.
@@ -5154,7 +6155,7 @@ int cxgb4_t4_bar2_sge_qregs(struct adapter *adapter,
 	 *  o The BAR2 Queue ID.
 	 *  o The BAR2 Queue ID Offset into the BAR2 page.
 	 */
-	bar2_page_offset = ((qid >> qpp_shift) << page_shift);
+	bar2_page_offset = ((u64)(qid >> qpp_shift) << page_shift);
 	bar2_qid = qid & qpp_mask;
 	bar2_qid_offset = bar2_qid * SGE_UDB_SIZE;
 
@@ -5223,18 +6224,19 @@ int t4_init_devlog_params(struct adapter *adap)
 	/* Otherwise, ask the firmware for it's Device Log Parameters.
 	 */
 	memset(&devlog_cmd, 0, sizeof(devlog_cmd));
-	devlog_cmd.op_to_write = htonl(FW_CMD_OP_V(FW_DEVLOG_CMD) |
-				       FW_CMD_REQUEST_F | FW_CMD_READ_F);
-	devlog_cmd.retval_len16 = htonl(FW_LEN16(devlog_cmd));
+	devlog_cmd.op_to_write = cpu_to_be32(FW_CMD_OP_V(FW_DEVLOG_CMD) |
+					     FW_CMD_REQUEST_F | FW_CMD_READ_F);
+	devlog_cmd.retval_len16 = cpu_to_be32(FW_LEN16(devlog_cmd));
 	ret = t4_wr_mbox(adap, adap->mbox, &devlog_cmd, sizeof(devlog_cmd),
 			 &devlog_cmd);
 	if (ret)
 		return ret;
 
-	devlog_meminfo = ntohl(devlog_cmd.memtype_devlog_memaddr16_devlog);
+	devlog_meminfo =
+		be32_to_cpu(devlog_cmd.memtype_devlog_memaddr16_devlog);
 	dparams->memtype = FW_DEVLOG_CMD_MEMTYPE_DEVLOG_G(devlog_meminfo);
 	dparams->start = FW_DEVLOG_CMD_MEMADDR16_DEVLOG_G(devlog_meminfo) << 4;
-	dparams->size = ntohl(devlog_cmd.memsize_devlog);
+	dparams->size = be32_to_cpu(devlog_cmd.memsize_devlog);
 
 	return 0;
 }
@@ -5255,13 +6257,13 @@ int t4_init_sge_params(struct adapter *adapter)
 	 */
 	hps = t4_read_reg(adapter, SGE_HOST_PAGE_SIZE_A);
 	s_hps = (HOSTPAGESIZEPF0_S +
-		 (HOSTPAGESIZEPF1_S - HOSTPAGESIZEPF0_S) * adapter->fn);
+		 (HOSTPAGESIZEPF1_S - HOSTPAGESIZEPF0_S) * adapter->pf);
 	sge_params->hps = ((hps >> s_hps) & HOSTPAGESIZEPF0_M);
 
 	/* Extract the SGE Egress and Ingess Queues Per Page for our PF.
 	 */
 	s_qpp = (QUEUESPERPAGEPF0_S +
-		(QUEUESPERPAGEPF1_S - QUEUESPERPAGEPF0_S) * adapter->fn);
+		(QUEUESPERPAGEPF1_S - QUEUESPERPAGEPF0_S) * adapter->pf);
 	qpp = t4_read_reg(adapter, SGE_EGRESS_QUEUES_PER_PAGE_PF_A);
 	sge_params->eq_qpp = ((qpp >> s_qpp) & QUEUESPERPAGEPF0_M);
 	qpp = t4_read_reg(adapter, SGE_INGRESS_QUEUES_PER_PAGE_PF_A);
@@ -5292,12 +6294,19 @@ int t4_init_tp_params(struct adapter *adap)
 	/* Cache the adapter's Compressed Filter Mode and global Incress
 	 * Configuration.
 	 */
-	t4_read_indirect(adap, TP_PIO_ADDR_A, TP_PIO_DATA_A,
-			 &adap->params.tp.vlan_pri_map, 1,
-			 TP_VLAN_PRI_MAP_A);
-	t4_read_indirect(adap, TP_PIO_ADDR_A, TP_PIO_DATA_A,
-			 &adap->params.tp.ingress_config, 1,
-			 TP_INGRESS_CONFIG_A);
+	if (adap->flags & FW_OK) {
+		t4_fw_tp_pio_rw(adap, &adap->params.tp.vlan_pri_map, 1,
+				TP_VLAN_PRI_MAP_A, 1);
+		t4_fw_tp_pio_rw(adap, &adap->params.tp.ingress_config, 1,
+				TP_INGRESS_CONFIG_A, 1);
+	} else {
+		t4_read_indirect(adap, TP_PIO_ADDR_A, TP_PIO_DATA_A,
+				 &adap->params.tp.vlan_pri_map, 1,
+				 TP_VLAN_PRI_MAP_A);
+		t4_read_indirect(adap, TP_PIO_ADDR_A, TP_PIO_DATA_A,
+				 &adap->params.tp.ingress_config, 1,
+				 TP_INGRESS_CONFIG_A);
+	}
 
 	/* Now that we have TP_VLAN_PRI_MAP cached, we can calculate the field
 	 * shift positions of several elements of the Compressed Filter Tuple
@@ -5373,6 +6382,29 @@ int t4_filter_field_shift(const struct adapter *adap, int filter_sel)
 	return field_shift;
 }
 
+int t4_init_rss_mode(struct adapter *adap, int mbox)
+{
+	int i, ret;
+	struct fw_rss_vi_config_cmd rvc;
+
+	memset(&rvc, 0, sizeof(rvc));
+
+	for_each_port(adap, i) {
+		struct port_info *p = adap2pinfo(adap, i);
+
+		rvc.op_to_viid =
+			cpu_to_be32(FW_CMD_OP_V(FW_RSS_VI_CONFIG_CMD) |
+				    FW_CMD_REQUEST_F | FW_CMD_READ_F |
+				    FW_RSS_VI_CONFIG_CMD_VIID_V(p->viid));
+		rvc.retval_len16 = cpu_to_be32(FW_LEN16(rvc));
+		ret = t4_wr_mbox(adap, mbox, &rvc, sizeof(rvc), &rvc);
+		if (ret)
+			return ret;
+		p->rss_mode = be32_to_cpu(rvc.u.basicvirtual.defaultq_to_udpen);
+	}
+	return 0;
+}
+
 int t4_port_init(struct adapter *adap, int mbox, int pf, int vf)
 {
 	u8 addr[6];
@@ -5390,10 +6422,10 @@ int t4_port_init(struct adapter *adap, int mbox, int pf, int vf)
 		while ((adap->params.portvec & (1 << j)) == 0)
 			j++;
 
-		c.op_to_portid = htonl(FW_CMD_OP_V(FW_PORT_CMD) |
-				       FW_CMD_REQUEST_F | FW_CMD_READ_F |
-				       FW_PORT_CMD_PORTID_V(j));
-		c.action_to_len16 = htonl(
+		c.op_to_portid = cpu_to_be32(FW_CMD_OP_V(FW_PORT_CMD) |
+					     FW_CMD_REQUEST_F | FW_CMD_READ_F |
+					     FW_PORT_CMD_PORTID_V(j));
+		c.action_to_len16 = cpu_to_be32(
 			FW_PORT_CMD_ACTION_V(FW_PORT_ACTION_GET_PORT_INFO) |
 			FW_LEN16(c));
 		ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
@@ -5411,22 +6443,23 @@ int t4_port_init(struct adapter *adap, int mbox, int pf, int vf)
 		memcpy(adap->port[i]->dev_addr, addr, ETH_ALEN);
 		adap->port[i]->dev_port = j;
 
-		ret = ntohl(c.u.info.lstatus_to_modtype);
+		ret = be32_to_cpu(c.u.info.lstatus_to_modtype);
 		p->mdio_addr = (ret & FW_PORT_CMD_MDIOCAP_F) ?
 			FW_PORT_CMD_MDIOADDR_G(ret) : -1;
 		p->port_type = FW_PORT_CMD_PTYPE_G(ret);
 		p->mod_type = FW_PORT_MOD_TYPE_NA;
 
-		rvc.op_to_viid = htonl(FW_CMD_OP_V(FW_RSS_VI_CONFIG_CMD) |
-				       FW_CMD_REQUEST_F | FW_CMD_READ_F |
-				       FW_RSS_VI_CONFIG_CMD_VIID(p->viid));
-		rvc.retval_len16 = htonl(FW_LEN16(rvc));
+		rvc.op_to_viid =
+			cpu_to_be32(FW_CMD_OP_V(FW_RSS_VI_CONFIG_CMD) |
+				    FW_CMD_REQUEST_F | FW_CMD_READ_F |
+				    FW_RSS_VI_CONFIG_CMD_VIID(p->viid));
+		rvc.retval_len16 = cpu_to_be32(FW_LEN16(rvc));
 		ret = t4_wr_mbox(adap, mbox, &rvc, sizeof(rvc), &rvc);
 		if (ret)
 			return ret;
-		p->rss_mode = ntohl(rvc.u.basicvirtual.defaultq_to_udpen);
+		p->rss_mode = be32_to_cpu(rvc.u.basicvirtual.defaultq_to_udpen);
 
-		init_link_config(&p->link_cfg, ntohs(c.u.info.pcap));
+		init_link_config(&p->link_cfg, be16_to_cpu(c.u.info.pcap));
 		j++;
 	}
 	return 0;
@@ -5717,3 +6750,130 @@ void t4_tp_read_la(struct adapter *adap, u64 *la_buf, unsigned int *wrptr)
 		t4_write_reg(adap, TP_DBG_LA_CONFIG_A,
 			     cfg | adap->params.tp.la_mask);
 }
+
+/* SGE Hung Ingress DMA Warning Threshold time and Warning Repeat Rate (in
+ * seconds).  If we find one of the SGE Ingress DMA State Machines in the same
+ * state for more than the Warning Threshold then we'll issue a warning about
+ * a potential hang.  We'll repeat the warning as the SGE Ingress DMA Channel
+ * appears to be hung every Warning Repeat second till the situation clears.
+ * If the situation clears, we'll note that as well.
+ */
+#define SGE_IDMA_WARN_THRESH 1
+#define SGE_IDMA_WARN_REPEAT 300
+
+/**
+ *	t4_idma_monitor_init - initialize SGE Ingress DMA Monitor
+ *	@adapter: the adapter
+ *	@idma: the adapter IDMA Monitor state
+ *
+ *	Initialize the state of an SGE Ingress DMA Monitor.
+ */
+void t4_idma_monitor_init(struct adapter *adapter,
+			  struct sge_idma_monitor_state *idma)
+{
+	/* Initialize the state variables for detecting an SGE Ingress DMA
+	 * hang.  The SGE has internal counters which count up on each clock
+	 * tick whenever the SGE finds its Ingress DMA State Engines in the
+	 * same state they were on the previous clock tick.  The clock used is
+	 * the Core Clock so we have a limit on the maximum "time" they can
+	 * record; typically a very small number of seconds.  For instance,
+	 * with a 600MHz Core Clock, we can only count up to a bit more than
+	 * 7s.  So we'll synthesize a larger counter in order to not run the
+	 * risk of having the "timers" overflow and give us the flexibility to
+	 * maintain a Hung SGE State Machine of our own which operates across
+	 * a longer time frame.
+	 */
+	idma->idma_1s_thresh = core_ticks_per_usec(adapter) * 1000000; /* 1s */
+	idma->idma_stalled[0] = 0;
+	idma->idma_stalled[1] = 0;
+}
+
+/**
+ *	t4_idma_monitor - monitor SGE Ingress DMA state
+ *	@adapter: the adapter
+ *	@idma: the adapter IDMA Monitor state
+ *	@hz: number of ticks/second
+ *	@ticks: number of ticks since the last IDMA Monitor call
+ */
+void t4_idma_monitor(struct adapter *adapter,
+		     struct sge_idma_monitor_state *idma,
+		     int hz, int ticks)
+{
+	int i, idma_same_state_cnt[2];
+
+	 /* Read the SGE Debug Ingress DMA Same State Count registers.  These
+	  * are counters inside the SGE which count up on each clock when the
+	  * SGE finds its Ingress DMA State Engines in the same states they
+	  * were in the previous clock.  The counters will peg out at
+	  * 0xffffffff without wrapping around so once they pass the 1s
+	  * threshold they'll stay above that till the IDMA state changes.
+	  */
+	t4_write_reg(adapter, SGE_DEBUG_INDEX_A, 13);
+	idma_same_state_cnt[0] = t4_read_reg(adapter, SGE_DEBUG_DATA_HIGH_A);
+	idma_same_state_cnt[1] = t4_read_reg(adapter, SGE_DEBUG_DATA_LOW_A);
+
+	for (i = 0; i < 2; i++) {
+		u32 debug0, debug11;
+
+		/* If the Ingress DMA Same State Counter ("timer") is less
+		 * than 1s, then we can reset our synthesized Stall Timer and
+		 * continue.  If we have previously emitted warnings about a
+		 * potential stalled Ingress Queue, issue a note indicating
+		 * that the Ingress Queue has resumed forward progress.
+		 */
+		if (idma_same_state_cnt[i] < idma->idma_1s_thresh) {
+			if (idma->idma_stalled[i] >= SGE_IDMA_WARN_THRESH * hz)
+				dev_warn(adapter->pdev_dev, "SGE idma%d, queue %u, "
+					 "resumed after %d seconds\n",
+					 i, idma->idma_qid[i],
+					 idma->idma_stalled[i] / hz);
+			idma->idma_stalled[i] = 0;
+			continue;
+		}
+
+		/* Synthesize an SGE Ingress DMA Same State Timer in the Hz
+		 * domain.  The first time we get here it'll be because we
+		 * passed the 1s Threshold; each additional time it'll be
+		 * because the RX Timer Callback is being fired on its regular
+		 * schedule.
+		 *
+		 * If the stall is below our Potential Hung Ingress Queue
+		 * Warning Threshold, continue.
+		 */
+		if (idma->idma_stalled[i] == 0) {
+			idma->idma_stalled[i] = hz;
+			idma->idma_warn[i] = 0;
+		} else {
+			idma->idma_stalled[i] += ticks;
+			idma->idma_warn[i] -= ticks;
+		}
+
+		if (idma->idma_stalled[i] < SGE_IDMA_WARN_THRESH * hz)
+			continue;
+
+		/* We'll issue a warning every SGE_IDMA_WARN_REPEAT seconds.
+		 */
+		if (idma->idma_warn[i] > 0)
+			continue;
+		idma->idma_warn[i] = SGE_IDMA_WARN_REPEAT * hz;
+
+		/* Read and save the SGE IDMA State and Queue ID information.
+		 * We do this every time in case it changes across time ...
+		 * can't be too careful ...
+		 */
+		t4_write_reg(adapter, SGE_DEBUG_INDEX_A, 0);
+		debug0 = t4_read_reg(adapter, SGE_DEBUG_DATA_LOW_A);
+		idma->idma_state[i] = (debug0 >> (i * 9)) & 0x3f;
+
+		t4_write_reg(adapter, SGE_DEBUG_INDEX_A, 11);
+		debug11 = t4_read_reg(adapter, SGE_DEBUG_DATA_LOW_A);
+		idma->idma_qid[i] = (debug11 >> (i * 16)) & 0xffff;
+
+		dev_warn(adapter->pdev_dev, "SGE idma%u, queue %u, potentially stuck in "
+			 "state %u for %d seconds (debug0=%#x, debug11=%#x)\n",
+			 i, idma->idma_qid[i], idma->idma_state[i],
+			 idma->idma_stalled[i] / hz,
+			 debug0, debug11);
+		t4_sge_decode_idma_state(adapter, idma->idma_state[i]);
+	}
+}
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
index 380b15c0417a..c8488f430d19 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
@@ -52,8 +52,6 @@ enum {
 	MBOX_LEN       = 64,    /* mailbox size in bytes */
 	TRACE_LEN      = 112,   /* length of trace data and mask */
 	FILTER_OPT_LEN = 36,    /* filter tuple width for optional components */
-	NWOL_PAT       = 8,     /* # of WoL patterns */
-	WOL_PAT_LEN    = 128,   /* length of WoL patterns */
 };
 
 enum {
@@ -61,6 +59,8 @@ enum {
 	CIM_NUM_OBQ    = 6,     /* # of CIM OBQs */
 	CIM_NUM_OBQ_T5 = 8,     /* # of CIM OBQs for T5 adapter */
 	CIMLA_SIZE     = 2048,  /* # of 32-bit words in CIM LA */
+	CIM_PIFLA_SIZE = 64,    /* # of 192-bit words in CIM PIF LA */
+	CIM_MALA_SIZE  = 64,    /* # of 160-bit words in CIM MA LA */
 	CIM_IBQ_SIZE   = 128,   /* # of 128-bit words in a CIM IBQ */
 	CIM_OBQ_SIZE   = 128,   /* # of 128-bit words in a CIM OBQ */
 	TPLA_SIZE      = 128,   /* # of 64-bit words in TP LA */
@@ -152,17 +152,33 @@ struct rsp_ctrl {
 	};
 };
 
-#define RSPD_NEWBUF 0x80000000U
-#define RSPD_LEN(x) (((x) >> 0) & 0x7fffffffU)
-#define RSPD_QID(x) RSPD_LEN(x)
+#define RSPD_NEWBUF_S    31
+#define RSPD_NEWBUF_V(x) ((x) << RSPD_NEWBUF_S)
+#define RSPD_NEWBUF_F    RSPD_NEWBUF_V(1U)
 
-#define RSPD_GEN(x)  ((x) >> 7)
-#define RSPD_TYPE(x) (((x) >> 4) & 3)
+#define RSPD_LEN_S    0
+#define RSPD_LEN_M    0x7fffffff
+#define RSPD_LEN_G(x) (((x) >> RSPD_LEN_S) & RSPD_LEN_M)
 
-#define V_QINTR_CNT_EN	   0x0
-#define QINTR_CNT_EN       0x1
-#define QINTR_TIMER_IDX(x) ((x) << 1)
-#define QINTR_TIMER_IDX_GET(x) (((x) >> 1) & 0x7)
+#define RSPD_QID_S    RSPD_LEN_S
+#define RSPD_QID_M    RSPD_LEN_M
+#define RSPD_QID_G(x) RSPD_LEN_G(x)
+
+#define RSPD_GEN_S    7
+
+#define RSPD_TYPE_S    4
+#define RSPD_TYPE_M    0x3
+#define RSPD_TYPE_G(x) (((x) >> RSPD_TYPE_S) & RSPD_TYPE_M)
+
+/* Rx queue interrupt deferral fields: counter enable and timer index */
+#define QINTR_CNT_EN_S    0
+#define QINTR_CNT_EN_V(x) ((x) << QINTR_CNT_EN_S)
+#define QINTR_CNT_EN_F    QINTR_CNT_EN_V(1U)
+
+#define QINTR_TIMER_IDX_S    1
+#define QINTR_TIMER_IDX_M    0x7
+#define QINTR_TIMER_IDX_V(x) ((x) << QINTR_TIMER_IDX_S)
+#define QINTR_TIMER_IDX_G(x) (((x) >> QINTR_TIMER_IDX_S) & QINTR_TIMER_IDX_M)
 
 /*
  * Flash layout.
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
index 30a2f56e99c2..132cb8fc0bf7 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
@@ -634,26 +634,9 @@ struct cpl_tid_release {
 
 struct cpl_tx_pkt_core {
 	__be32 ctrl0;
-#define TXPKT_VF(x)        ((x) << 0)
-#define TXPKT_PF(x)        ((x) << 8)
-#define TXPKT_VF_VLD       (1 << 11)
-#define TXPKT_OVLAN_IDX(x) ((x) << 12)
-#define TXPKT_INTF(x)      ((x) << 16)
-#define TXPKT_INS_OVLAN    (1 << 21)
-#define TXPKT_OPCODE(x)    ((x) << 24)
 	__be16 pack;
 	__be16 len;
 	__be64 ctrl1;
-#define TXPKT_CSUM_END(x)   ((x) << 12)
-#define TXPKT_CSUM_START(x) ((x) << 20)
-#define TXPKT_IPHDR_LEN(x)  ((u64)(x) << 20)
-#define TXPKT_CSUM_LOC(x)   ((u64)(x) << 30)
-#define TXPKT_ETHHDR_LEN(x) ((u64)(x) << 34)
-#define TXPKT_CSUM_TYPE(x)  ((u64)(x) << 40)
-#define TXPKT_VLAN(x)       ((u64)(x) << 44)
-#define TXPKT_VLAN_VLD      (1ULL << 60)
-#define TXPKT_IPCSUM_DIS    (1ULL << 62)
-#define TXPKT_L4CSUM_DIS    (1ULL << 63)
 };
 
 struct cpl_tx_pkt {
@@ -663,16 +646,69 @@ struct cpl_tx_pkt {
 
 #define cpl_tx_pkt_xt cpl_tx_pkt
 
+/* cpl_tx_pkt_core.ctrl0 fields */
+#define TXPKT_VF_S    0
+#define TXPKT_VF_V(x) ((x) << TXPKT_VF_S)
+
+#define TXPKT_PF_S    8
+#define TXPKT_PF_V(x) ((x) << TXPKT_PF_S)
+
+#define TXPKT_VF_VLD_S    11
+#define TXPKT_VF_VLD_V(x) ((x) << TXPKT_VF_VLD_S)
+#define TXPKT_VF_VLD_F    TXPKT_VF_VLD_V(1U)
+
+#define TXPKT_OVLAN_IDX_S    12
+#define TXPKT_OVLAN_IDX_V(x) ((x) << TXPKT_OVLAN_IDX_S)
+
+#define TXPKT_INTF_S    16
+#define TXPKT_INTF_V(x) ((x) << TXPKT_INTF_S)
+
+#define TXPKT_INS_OVLAN_S    21
+#define TXPKT_INS_OVLAN_V(x) ((x) << TXPKT_INS_OVLAN_S)
+#define TXPKT_INS_OVLAN_F    TXPKT_INS_OVLAN_V(1U)
+
+#define TXPKT_OPCODE_S    24
+#define TXPKT_OPCODE_V(x) ((x) << TXPKT_OPCODE_S)
+
+/* cpl_tx_pkt_core.ctrl1 fields */
+#define TXPKT_CSUM_END_S    12
+#define TXPKT_CSUM_END_V(x) ((x) << TXPKT_CSUM_END_S)
+
+#define TXPKT_CSUM_START_S    20
+#define TXPKT_CSUM_START_V(x) ((x) << TXPKT_CSUM_START_S)
+
+#define TXPKT_IPHDR_LEN_S    20
+#define TXPKT_IPHDR_LEN_V(x) ((__u64)(x) << TXPKT_IPHDR_LEN_S)
+
+#define TXPKT_CSUM_LOC_S    30
+#define TXPKT_CSUM_LOC_V(x) ((__u64)(x) << TXPKT_CSUM_LOC_S)
+
+#define TXPKT_ETHHDR_LEN_S    34
+#define TXPKT_ETHHDR_LEN_V(x) ((__u64)(x) << TXPKT_ETHHDR_LEN_S)
+
+#define T6_TXPKT_ETHHDR_LEN_S    32
+#define T6_TXPKT_ETHHDR_LEN_V(x) ((__u64)(x) << T6_TXPKT_ETHHDR_LEN_S)
+
+#define TXPKT_CSUM_TYPE_S    40
+#define TXPKT_CSUM_TYPE_V(x) ((__u64)(x) << TXPKT_CSUM_TYPE_S)
+
+#define TXPKT_VLAN_S    44
+#define TXPKT_VLAN_V(x) ((__u64)(x) << TXPKT_VLAN_S)
+
+#define TXPKT_VLAN_VLD_S    60
+#define TXPKT_VLAN_VLD_V(x) ((__u64)(x) << TXPKT_VLAN_VLD_S)
+#define TXPKT_VLAN_VLD_F    TXPKT_VLAN_VLD_V(1ULL)
+
+#define TXPKT_IPCSUM_DIS_S    62
+#define TXPKT_IPCSUM_DIS_V(x) ((__u64)(x) << TXPKT_IPCSUM_DIS_S)
+#define TXPKT_IPCSUM_DIS_F    TXPKT_IPCSUM_DIS_V(1ULL)
+
+#define TXPKT_L4CSUM_DIS_S    63
+#define TXPKT_L4CSUM_DIS_V(x) ((__u64)(x) << TXPKT_L4CSUM_DIS_S)
+#define TXPKT_L4CSUM_DIS_F    TXPKT_L4CSUM_DIS_V(1ULL)
+
 struct cpl_tx_pkt_lso_core {
 	__be32 lso_ctrl;
-#define LSO_TCPHDR_LEN(x) ((x) << 0)
-#define LSO_IPHDR_LEN(x)  ((x) << 4)
-#define LSO_ETHHDR_LEN(x) ((x) << 16)
-#define LSO_IPV6(x)       ((x) << 20)
-#define LSO_LAST_SLICE    (1 << 22)
-#define LSO_FIRST_SLICE   (1 << 23)
-#define LSO_OPCODE(x)     ((x) << 24)
-#define LSO_T5_XFER_SIZE(x) ((x) << 0)
 	__be16 ipid_ofst;
 	__be16 mss;
 	__be32 seqno_offset;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h b/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
index 1a9a6f334d2d..d7ca106927b0 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
@@ -153,6 +153,8 @@ CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN
 	CH_PCI_ID_TABLE_FENTRY(0x5088),	/* Custom T570-CR */
 	CH_PCI_ID_TABLE_FENTRY(0x5089),	/* Custom T520-CR */
 	CH_PCI_ID_TABLE_FENTRY(0x5090),	/* Custom T540-CR */
+	CH_PCI_ID_TABLE_FENTRY(0x5091),	/* Custom T522-CR */
+	CH_PCI_ID_TABLE_FENTRY(0x5092),	/* Custom T520-CR */
 CH_PCI_DEVICE_ID_TABLE_DEFINE_END;
 
 #endif /* __T4_PCI_ID_TBL_H__ */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
index 326674b19983..375a825573b0 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
@@ -418,6 +418,20 @@
 #define SGE_INGRESS_QUEUES_PER_PAGE_PF_A 0x10f4
 #define SGE_INGRESS_QUEUES_PER_PAGE_VF_A 0x10f8
 
+#define SGE_ERROR_STATS_A 0x1100
+
+#define UNCAPTURED_ERROR_S    18
+#define UNCAPTURED_ERROR_V(x) ((x) << UNCAPTURED_ERROR_S)
+#define UNCAPTURED_ERROR_F    UNCAPTURED_ERROR_V(1U)
+
+#define ERROR_QID_VALID_S    17
+#define ERROR_QID_VALID_V(x) ((x) << ERROR_QID_VALID_S)
+#define ERROR_QID_VALID_F    ERROR_QID_VALID_V(1U)
+
+#define ERROR_QID_S    0
+#define ERROR_QID_M    0x1ffffU
+#define ERROR_QID_G(x) (((x) >> ERROR_QID_S) & ERROR_QID_M)
+
 #define HP_INT_THRESH_S    28
 #define HP_INT_THRESH_M    0xfU
 #define HP_INT_THRESH_V(x) ((x) << HP_INT_THRESH_S)
@@ -448,8 +462,13 @@
 #define SGE_STAT_MATCH_A	0x10e8
 #define SGE_STAT_CFG_A		0x10ec
 
+#define STATMODE_S    2
+#define STATMODE_V(x) ((x) << STATMODE_S)
+
 #define STATSOURCE_T5_S    9
+#define STATSOURCE_T5_M    0xfU
 #define STATSOURCE_T5_V(x) ((x) << STATSOURCE_T5_S)
+#define STATSOURCE_T5_G(x) (((x) >> STATSOURCE_T5_S) & STATSOURCE_T5_M)
 
 #define SGE_DBFIFO_STATUS2_A 0x1118
 
@@ -705,6 +724,10 @@
 #define REGISTER_S    0
 #define REGISTER_V(x) ((x) << REGISTER_S)
 
+#define T6_ENABLE_S    31
+#define T6_ENABLE_V(x) ((x) << T6_ENABLE_S)
+#define T6_ENABLE_F    T6_ENABLE_V(1U)
+
 #define PFNUM_S    0
 #define PFNUM_V(x) ((x) << PFNUM_S)
 
@@ -1338,6 +1361,42 @@
 #define FLMTXFLSTEMPTY_V(x) ((x) << FLMTXFLSTEMPTY_S)
 #define FLMTXFLSTEMPTY_F    FLMTXFLSTEMPTY_V(1U)
 
+#define TP_TX_ORATE_A 0x7ebc
+
+#define OFDRATE3_S    24
+#define OFDRATE3_M    0xffU
+#define OFDRATE3_G(x) (((x) >> OFDRATE3_S) & OFDRATE3_M)
+
+#define OFDRATE2_S    16
+#define OFDRATE2_M    0xffU
+#define OFDRATE2_G(x) (((x) >> OFDRATE2_S) & OFDRATE2_M)
+
+#define OFDRATE1_S    8
+#define OFDRATE1_M    0xffU
+#define OFDRATE1_G(x) (((x) >> OFDRATE1_S) & OFDRATE1_M)
+
+#define OFDRATE0_S    0
+#define OFDRATE0_M    0xffU
+#define OFDRATE0_G(x) (((x) >> OFDRATE0_S) & OFDRATE0_M)
+
+#define TP_TX_TRATE_A 0x7ed0
+
+#define TNLRATE3_S    24
+#define TNLRATE3_M    0xffU
+#define TNLRATE3_G(x) (((x) >> TNLRATE3_S) & TNLRATE3_M)
+
+#define TNLRATE2_S    16
+#define TNLRATE2_M    0xffU
+#define TNLRATE2_G(x) (((x) >> TNLRATE2_S) & TNLRATE2_M)
+
+#define TNLRATE1_S    8
+#define TNLRATE1_M    0xffU
+#define TNLRATE1_G(x) (((x) >> TNLRATE1_S) & TNLRATE1_M)
+
+#define TNLRATE0_S    0
+#define TNLRATE0_M    0xffU
+#define TNLRATE0_G(x) (((x) >> TNLRATE0_S) & TNLRATE0_M)
+
 #define TP_VLAN_PRI_MAP_A 0x140
 
 #define FRAGMENTATION_S    9
@@ -1399,6 +1458,8 @@
 #define CSUM_HAS_PSEUDO_HDR_F    CSUM_HAS_PSEUDO_HDR_V(1U)
 
 #define TP_MIB_MAC_IN_ERR_0_A	0x0
+#define TP_MIB_HDR_IN_ERR_0_A	0x4
+#define TP_MIB_TCP_IN_ERR_0_A	0x8
 #define TP_MIB_TCP_OUT_RST_A	0xc
 #define TP_MIB_TCP_IN_SEG_HI_A	0x10
 #define TP_MIB_TCP_IN_SEG_LO_A	0x11
@@ -1407,11 +1468,19 @@
 #define TP_MIB_TCP_RXT_SEG_HI_A	0x14
 #define TP_MIB_TCP_RXT_SEG_LO_A	0x15
 #define TP_MIB_TNL_CNG_DROP_0_A 0x18
+#define TP_MIB_OFD_CHN_DROP_0_A 0x1c
 #define TP_MIB_TCP_V6IN_ERR_0_A 0x28
 #define TP_MIB_TCP_V6OUT_RST_A	0x2c
 #define TP_MIB_OFD_ARP_DROP_A	0x36
+#define TP_MIB_CPL_IN_REQ_0_A	0x38
+#define TP_MIB_CPL_OUT_RSP_0_A	0x3c
 #define TP_MIB_TNL_DROP_0_A	0x44
+#define TP_MIB_FCOE_DDP_0_A	0x48
+#define TP_MIB_FCOE_DROP_0_A	0x4c
+#define TP_MIB_FCOE_BYTE_0_HI_A	0x50
 #define TP_MIB_OFD_VLN_DROP_0_A	0x58
+#define TP_MIB_USM_PKTS_A	0x5c
+#define TP_MIB_RQE_DFR_PKT_A	0x64
 
 #define ULP_TX_INT_CAUSE_A	0x8dcc
 
@@ -1572,6 +1641,7 @@
 #define MPS_PORT_STAT_LB_PORT_1519B_MAX_L 0x520
 #define MPS_PORT_STAT_LB_PORT_1519B_MAX_H 0x524
 #define MPS_PORT_STAT_LB_PORT_DROP_FRAMES 0x528
+#define MPS_PORT_STAT_LB_PORT_DROP_FRAMES_L 0x528
 #define MPS_PORT_STAT_RX_PORT_BYTES_L 0x540
 #define MPS_PORT_STAT_RX_PORT_BYTES_H 0x544
 #define MPS_PORT_STAT_RX_PORT_FRAMES_L 0x548
@@ -2054,6 +2124,11 @@
 #define VFLKPIDX_M    0xffU
 #define VFLKPIDX_G(x) (((x) >> VFLKPIDX_S) & VFLKPIDX_M)
 
+#define T6_VFWRADDR_S    8
+#define T6_VFWRADDR_M    0xffU
+#define T6_VFWRADDR_V(x) ((x) << T6_VFWRADDR_S)
+#define T6_VFWRADDR_G(x) (((x) >> T6_VFWRADDR_S) & T6_VFWRADDR_M)
+
 #define TP_RSS_CONFIG_CNG_A 0x7e04
 #define TP_RSS_SECRET_KEY0_A 0x40
 #define TP_RSS_PF0_CONFIG_A 0x30
@@ -2175,7 +2250,28 @@
 #define MPS_RX_PERR_INT_CAUSE_A 0x11074
 
 #define MPS_CLS_TCAM_Y_L_A 0xf000
+#define MPS_CLS_TCAM_DATA0_A 0xf000
+#define MPS_CLS_TCAM_DATA1_A 0xf004
+
+#define DMACH_S    0
+#define DMACH_M    0xffffU
+#define DMACH_G(x) (((x) >> DMACH_S) & DMACH_M)
+
 #define MPS_CLS_TCAM_X_L_A 0xf008
+#define MPS_CLS_TCAM_DATA2_CTL_A 0xf008
+
+#define CTLCMDTYPE_S    31
+#define CTLCMDTYPE_V(x) ((x) << CTLCMDTYPE_S)
+#define CTLCMDTYPE_F    CTLCMDTYPE_V(1U)
+
+#define CTLTCAMSEL_S    25
+#define CTLTCAMSEL_V(x) ((x) << CTLTCAMSEL_S)
+
+#define CTLTCAMINDEX_S    17
+#define CTLTCAMINDEX_V(x) ((x) << CTLTCAMINDEX_S)
+
+#define CTLXYBITSEL_S    16
+#define CTLXYBITSEL_V(x) ((x) << CTLXYBITSEL_S)
 
 #define MPS_CLS_TCAM_Y_L(idx) (MPS_CLS_TCAM_Y_L_A + (idx) * 16)
 #define NUM_MPS_CLS_TCAM_Y_L_INSTANCES 512
@@ -2184,6 +2280,45 @@
 #define NUM_MPS_CLS_TCAM_X_L_INSTANCES 512
 
 #define MPS_CLS_SRAM_L_A 0xe000
+
+#define T6_MULTILISTEN0_S    26
+
+#define T6_SRAM_PRIO3_S    23
+#define T6_SRAM_PRIO3_M    0x7U
+#define T6_SRAM_PRIO3_G(x) (((x) >> T6_SRAM_PRIO3_S) & T6_SRAM_PRIO3_M)
+
+#define T6_SRAM_PRIO2_S    20
+#define T6_SRAM_PRIO2_M    0x7U
+#define T6_SRAM_PRIO2_G(x) (((x) >> T6_SRAM_PRIO2_S) & T6_SRAM_PRIO2_M)
+
+#define T6_SRAM_PRIO1_S    17
+#define T6_SRAM_PRIO1_M    0x7U
+#define T6_SRAM_PRIO1_G(x) (((x) >> T6_SRAM_PRIO1_S) & T6_SRAM_PRIO1_M)
+
+#define T6_SRAM_PRIO0_S    14
+#define T6_SRAM_PRIO0_M    0x7U
+#define T6_SRAM_PRIO0_G(x) (((x) >> T6_SRAM_PRIO0_S) & T6_SRAM_PRIO0_M)
+
+#define T6_SRAM_VLD_S    13
+#define T6_SRAM_VLD_V(x) ((x) << T6_SRAM_VLD_S)
+#define T6_SRAM_VLD_F    T6_SRAM_VLD_V(1U)
+
+#define T6_REPLICATE_S    12
+#define T6_REPLICATE_V(x) ((x) << T6_REPLICATE_S)
+#define T6_REPLICATE_F    T6_REPLICATE_V(1U)
+
+#define T6_PF_S    9
+#define T6_PF_M    0x7U
+#define T6_PF_G(x) (((x) >> T6_PF_S) & T6_PF_M)
+
+#define T6_VF_VALID_S    8
+#define T6_VF_VALID_V(x) ((x) << T6_VF_VALID_S)
+#define T6_VF_VALID_F    T6_VF_VALID_V(1U)
+
+#define T6_VF_S    0
+#define T6_VF_M    0xffU
+#define T6_VF_G(x) (((x) >> T6_VF_S) & T6_VF_M)
+
 #define MPS_CLS_SRAM_H_A 0xe004
 
 #define MPS_CLS_SRAM_L(idx) (MPS_CLS_SRAM_L_A + (idx) * 8)
@@ -2433,6 +2568,8 @@
 #define CIM_F    CIM_V(1U)
 
 #define MC1_S    31
+#define MC1_V(x) ((x) << MC1_S)
+#define MC1_F    MC1_V(1U)
 
 #define PL_INT_ENABLE_A 0x19410
 #define PL_INT_MAP0_A 0x19414
@@ -2463,6 +2600,18 @@
 #define REV_V(x) ((x) << REV_S)
 #define REV_G(x) (((x) >> REV_S) & REV_M)
 
+#define T6_UNKNOWNCMD_S    3
+#define T6_UNKNOWNCMD_V(x) ((x) << T6_UNKNOWNCMD_S)
+#define T6_UNKNOWNCMD_F    T6_UNKNOWNCMD_V(1U)
+
+#define T6_LIP0_S    2
+#define T6_LIP0_V(x) ((x) << T6_LIP0_S)
+#define T6_LIP0_F    T6_LIP0_V(1U)
+
+#define T6_LIPMISS_S    1
+#define T6_LIPMISS_V(x) ((x) << T6_LIPMISS_S)
+#define T6_LIPMISS_F    T6_LIPMISS_V(1U)
+
 #define LE_DB_INT_CAUSE_A 0x19c3c
 
 #define REQQPARERR_S    16
@@ -2485,6 +2634,14 @@
 #define LIP0_V(x) ((x) << LIP0_S)
 #define LIP0_F    LIP0_V(1U)
 
+#define TCAMINTPERR_S    13
+#define TCAMINTPERR_V(x) ((x) << TCAMINTPERR_S)
+#define TCAMINTPERR_F    TCAMINTPERR_V(1U)
+
+#define SSRAMINTPERR_S    10
+#define SSRAMINTPERR_V(x) ((x) << SSRAMINTPERR_S)
+#define SSRAMINTPERR_F    SSRAMINTPERR_V(1U)
+
 #define NCSI_INT_CAUSE_A 0x1a0d8
 
 #define CIM_DM_PRTY_ERR_S    8
@@ -2638,6 +2795,33 @@
 
 #define CIM_IBQ_DBG_DATA_A 0x7b68
 #define CIM_OBQ_DBG_DATA_A 0x7b6c
+#define CIM_DEBUGCFG_A 0x7b70
+#define CIM_DEBUGSTS_A 0x7b74
+
+#define POLADBGRDPTR_S		23
+#define POLADBGRDPTR_M		0x1ffU
+#define POLADBGRDPTR_V(x)	((x) << POLADBGRDPTR_S)
+
+#define POLADBGWRPTR_S		16
+#define POLADBGWRPTR_M		0x1ffU
+#define POLADBGWRPTR_G(x)	(((x) >> POLADBGWRPTR_S) & POLADBGWRPTR_M)
+
+#define PILADBGRDPTR_S		14
+#define PILADBGRDPTR_M		0x1ffU
+#define PILADBGRDPTR_V(x)	((x) << PILADBGRDPTR_S)
+
+#define PILADBGWRPTR_S		0
+#define PILADBGWRPTR_M		0x1ffU
+#define PILADBGWRPTR_G(x)	(((x) >> PILADBGWRPTR_S) & PILADBGWRPTR_M)
+
+#define LADBGEN_S	12
+#define LADBGEN_V(x)	((x) << LADBGEN_S)
+#define LADBGEN_F	LADBGEN_V(1U)
+
+#define CIM_PO_LA_DEBUGDATA_A 0x7b78
+#define CIM_PI_LA_DEBUGDATA_A 0x7b7c
+#define CIM_PO_LA_MADEBUGDATA_A	0x7b80
+#define CIM_PI_LA_MADEBUGDATA_A	0x7b84
 
 #define UPDBGLARDEN_S		1
 #define UPDBGLARDEN_V(x)	((x) << UPDBGLARDEN_S)
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_values.h b/drivers/net/ethernet/chelsio/cxgb4/t4_values.h
index 19b2dcf6acde..7bdee3bf75ec 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_values.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_values.h
@@ -61,6 +61,30 @@
 #define SGE_TIMERREGS			6
 #define TIMERREG_COUNTER0_X		0
 
+#define FETCHBURSTMIN_64B_X		2
+
+#define FETCHBURSTMAX_256B_X		2
+#define FETCHBURSTMAX_512B_X		3
+
+#define HOSTFCMODE_STATUS_PAGE_X	2
+
+#define CIDXFLUSHTHRESH_32_X		5
+
+#define UPDATEDELIVERY_INTERRUPT_X	1
+
+#define RSPD_TYPE_FLBUF_X		0
+#define RSPD_TYPE_CPL_X			1
+#define RSPD_TYPE_INTR_X		2
+
+/* Congestion Manager Definitions.
+ */
+#define CONMCTXT_CNGTPMODE_S		19
+#define CONMCTXT_CNGTPMODE_V(x)		((x) << CONMCTXT_CNGTPMODE_S)
+#define CONMCTXT_CNGCHMAP_S		0
+#define CONMCTXT_CNGCHMAP_V(x)		((x) << CONMCTXT_CNGCHMAP_S)
+#define CONMCTXT_CNGTPMODE_CHANNEL_X	2
+#define CONMCTXT_CNGTPMODE_QUEUE_X	1
+
 /* T5 and later support a new BAR2-based doorbell mechanism for Egress Queues.
  * The User Doorbells are each 128 bytes in length with a Simple Doorbell at
  * offsets 8x and a Write Combining single 64-byte Egress Queue Unit
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index 03fbfd1fb3df..ab4674684acc 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -772,7 +772,7 @@ struct fw_ldst_cmd {
 		} addrval;
 		struct fw_ldst_idctxt {
 			__be32 physid;
-			__be32 msg_pkd;
+			__be32 msg_ctxtflush;
 			__be32 ctxt_data7;
 			__be32 ctxt_data6;
 			__be32 ctxt_data5;
@@ -788,15 +788,27 @@ struct fw_ldst_cmd {
 			__be16 vctl;
 			__be16 rval;
 		} mdio;
-		struct fw_ldst_mps {
-			__be16 fid_ctl;
-			__be16 rplcpf_pkd;
-			__be32 rplc127_96;
-			__be32 rplc95_64;
-			__be32 rplc63_32;
-			__be32 rplc31_0;
-			__be32 atrb;
-			__be16 vlan[16];
+		union fw_ldst_mps {
+			struct fw_ldst_mps_rplc {
+				__be16 fid_idx;
+				__be16 rplcpf_pkd;
+				__be32 rplc255_224;
+				__be32 rplc223_192;
+				__be32 rplc191_160;
+				__be32 rplc159_128;
+				__be32 rplc127_96;
+				__be32 rplc95_64;
+				__be32 rplc63_32;
+				__be32 rplc31_0;
+			} rplc;
+			struct fw_ldst_mps_atrb {
+				__be16 fid_mpsid;
+				__be16 r2[3];
+				__be32 r3[2];
+				__be32 r4;
+				__be32 atrb;
+				__be16 vlan[16];
+			} atrb;
 		} mps;
 		struct fw_ldst_func {
 			u8 access_ctl;
@@ -822,6 +834,10 @@ struct fw_ldst_cmd {
 #define FW_LDST_CMD_MSG_S       31
 #define FW_LDST_CMD_MSG_V(x)	((x) << FW_LDST_CMD_MSG_S)
 
+#define FW_LDST_CMD_CTXTFLUSH_S		30
+#define FW_LDST_CMD_CTXTFLUSH_V(x)	((x) << FW_LDST_CMD_CTXTFLUSH_S)
+#define FW_LDST_CMD_CTXTFLUSH_F		FW_LDST_CMD_CTXTFLUSH_V(1U)
+
 #define FW_LDST_CMD_PADDR_S     8
 #define FW_LDST_CMD_PADDR_V(x)	((x) << FW_LDST_CMD_PADDR_S)
 
@@ -831,8 +847,8 @@ struct fw_ldst_cmd {
 #define FW_LDST_CMD_FID_S       15
 #define FW_LDST_CMD_FID_V(x)	((x) << FW_LDST_CMD_FID_S)
 
-#define FW_LDST_CMD_CTL_S       0
-#define FW_LDST_CMD_CTL_V(x)	((x) << FW_LDST_CMD_CTL_S)
+#define FW_LDST_CMD_IDX_S	0
+#define FW_LDST_CMD_IDX_V(x)	((x) << FW_LDST_CMD_IDX_S)
 
 #define FW_LDST_CMD_RPLCPF_S    0
 #define FW_LDST_CMD_RPLCPF_V(x)	((x) << FW_LDST_CMD_RPLCPF_S)
@@ -1061,6 +1077,7 @@ enum fw_params_param_dev {
 	FW_PARAMS_PARAM_DEV_FWREV = 0x0B,
 	FW_PARAMS_PARAM_DEV_TPREV = 0x0C,
 	FW_PARAMS_PARAM_DEV_CF = 0x0D,
+	FW_PARAMS_PARAM_DEV_PHYFW = 0x0F,
 	FW_PARAMS_PARAM_DEV_DIAG = 0x11,
 	FW_PARAMS_PARAM_DEV_MAXORDIRD_QP = 0x13, /* max supported QP IRD/ORD */
 	FW_PARAMS_PARAM_DEV_MAXIRD_ADAPTER = 0x14, /* max supported adap IRD */
@@ -1123,6 +1140,12 @@ enum fw_params_param_dmaq {
 	FW_PARAMS_PARAM_DMAQ_EQ_CMPLIQID_CTRL = 0x11,
 	FW_PARAMS_PARAM_DMAQ_EQ_SCHEDCLASS_ETH = 0x12,
 	FW_PARAMS_PARAM_DMAQ_EQ_DCBPRIO_ETH = 0x13,
+	FW_PARAMS_PARAM_DMAQ_CONM_CTXT = 0x20,
+};
+
+enum fw_params_param_dev_phyfw {
+	FW_PARAMS_PARAM_DEV_PHYFW_DOWNLOAD = 0x00,
+	FW_PARAMS_PARAM_DEV_PHYFW_VERSION = 0x01,
 };
 
 enum fw_params_param_dev_diag {
@@ -1377,6 +1400,7 @@ struct fw_iq_cmd {
 
 #define FW_IQ_CMD_IQFLINTCONGEN_S	27
 #define FW_IQ_CMD_IQFLINTCONGEN_V(x)	((x) << FW_IQ_CMD_IQFLINTCONGEN_S)
+#define FW_IQ_CMD_IQFLINTCONGEN_F	FW_IQ_CMD_IQFLINTCONGEN_V(1U)
 
 #define FW_IQ_CMD_IQFLINTISCSIC_S	26
 #define FW_IQ_CMD_IQFLINTISCSIC_V(x)	((x) << FW_IQ_CMD_IQFLINTISCSIC_S)
@@ -1399,6 +1423,7 @@ struct fw_iq_cmd {
 
 #define FW_IQ_CMD_FL0CONGCIF_S		11
 #define FW_IQ_CMD_FL0CONGCIF_V(x)	((x) << FW_IQ_CMD_FL0CONGCIF_S)
+#define FW_IQ_CMD_FL0CONGCIF_F		FW_IQ_CMD_FL0CONGCIF_V(1U)
 
 #define FW_IQ_CMD_FL0ONCHIP_S		10
 #define FW_IQ_CMD_FL0ONCHIP_V(x)	((x) << FW_IQ_CMD_FL0ONCHIP_S)
@@ -1589,6 +1614,7 @@ struct fw_eq_eth_cmd {
 
 #define FW_EQ_ETH_CMD_FETCHRO_S		22
 #define FW_EQ_ETH_CMD_FETCHRO_V(x)	((x) << FW_EQ_ETH_CMD_FETCHRO_S)
+#define FW_EQ_ETH_CMD_FETCHRO_F		FW_EQ_ETH_CMD_FETCHRO_V(1U)
 
 #define FW_EQ_ETH_CMD_HOSTFCMODE_S	20
 #define FW_EQ_ETH_CMD_HOSTFCMODE_V(x)	((x) << FW_EQ_ETH_CMD_HOSTFCMODE_S)
@@ -2526,13 +2552,8 @@ enum fw_port_mod_sub_type {
 	FW_PORT_MOD_SUB_TYPE_TWINAX_7 = 0xC,
 };
 
-/* port stats */
-#define FW_NUM_PORT_STATS 50
-#define FW_NUM_PORT_TX_STATS 23
-#define FW_NUM_PORT_RX_STATS 27
-
 enum fw_port_stats_tx_index {
-	FW_STAT_TX_PORT_BYTES_IX,
+	FW_STAT_TX_PORT_BYTES_IX = 0,
 	FW_STAT_TX_PORT_FRAMES_IX,
 	FW_STAT_TX_PORT_BCAST_IX,
 	FW_STAT_TX_PORT_MCAST_IX,
@@ -2554,11 +2575,12 @@ enum fw_port_stats_tx_index {
 	FW_STAT_TX_PORT_PPP4_IX,
 	FW_STAT_TX_PORT_PPP5_IX,
 	FW_STAT_TX_PORT_PPP6_IX,
-	FW_STAT_TX_PORT_PPP7_IX
+	FW_STAT_TX_PORT_PPP7_IX,
+	FW_NUM_PORT_TX_STATS
 };
 
 enum fw_port_stat_rx_index {
-	FW_STAT_RX_PORT_BYTES_IX,
+	FW_STAT_RX_PORT_BYTES_IX = 0,
 	FW_STAT_RX_PORT_FRAMES_IX,
 	FW_STAT_RX_PORT_BCAST_IX,
 	FW_STAT_RX_PORT_MCAST_IX,
@@ -2584,9 +2606,14 @@ enum fw_port_stat_rx_index {
 	FW_STAT_RX_PORT_PPP5_IX,
 	FW_STAT_RX_PORT_PPP6_IX,
 	FW_STAT_RX_PORT_PPP7_IX,
-	FW_STAT_RX_PORT_LESS_64B_IX
+	FW_STAT_RX_PORT_LESS_64B_IX,
+	FW_STAT_RX_PORT_MAC_ERROR_IX,
+	FW_NUM_PORT_RX_STATS
 };
 
+/* port stats */
+#define FW_NUM_PORT_STATS (FW_NUM_PORT_TX_STATS + FW_NUM_PORT_RX_STATS)
+
 struct fw_port_stats_cmd {
 	__be32 op_to_portid;
 	__be32 retval_len16;
@@ -3015,7 +3042,8 @@ struct fw_hdr {
 
 enum fw_hdr_chip {
 	FW_HDR_CHIP_T4,
-	FW_HDR_CHIP_T5
+	FW_HDR_CHIP_T5,
+	FW_HDR_CHIP_T6
 };
 
 #define FW_HDR_FW_VER_MAJOR_S	24
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
index b9d1cbac0eee..32b213559b02 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
@@ -45,4 +45,9 @@
 #define T5FW_VERSION_MICRO 0x20
 #define T5FW_VERSION_BUILD 0x00
 
+#define T6FW_VERSION_MAJOR 0x01
+#define T6FW_VERSION_MINOR 0x0D
+#define T6FW_VERSION_MICRO 0x2D
+#define T6FW_VERSION_BUILD 0x00
+
 #endif
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
index 1d893b0b7ddf..b2b5e5bbe04c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
@@ -1021,7 +1021,7 @@ static int closest_thres(const struct sge *s, int thres)
 static unsigned int qtimer_val(const struct adapter *adapter,
 			       const struct sge_rspq *rspq)
 {
-	unsigned int timer_idx = QINTR_TIMER_IDX_GET(rspq->intr_params);
+	unsigned int timer_idx = QINTR_TIMER_IDX_G(rspq->intr_params);
 
 	return timer_idx < SGE_NTIMERS
 		? adapter->sge.timer_val[timer_idx]
@@ -1086,8 +1086,8 @@ static int set_rxq_intr_params(struct adapter *adapter, struct sge_rspq *rspq,
 	 * Update the response queue's interrupt coalescing parameters and
 	 * return success.
 	 */
-	rspq->intr_params = (QINTR_TIMER_IDX(timer_idx) |
-			     (cnt > 0 ? QINTR_CNT_EN : 0));
+	rspq->intr_params = (QINTR_TIMER_IDX_V(timer_idx) |
+			     QINTR_CNT_EN_V(cnt > 0));
 	return 0;
 }
 
@@ -1439,7 +1439,7 @@ static int cxgb4vf_get_coalesce(struct net_device *dev,
 
 	coalesce->rx_coalesce_usecs = qtimer_val(adapter, rspq);
 	coalesce->rx_max_coalesced_frames =
-		((rspq->intr_params & QINTR_CNT_EN)
+		((rspq->intr_params & QINTR_CNT_EN_F)
 		 ? adapter->sge.counter_val[rspq->pktcnt_idx]
 		 : 0);
 	return 0;
@@ -2393,8 +2393,9 @@ static inline void init_rspq(struct sge_rspq *rspq, u8 timer_idx,
 			     u8 pkt_cnt_idx, unsigned int size,
 			     unsigned int iqe_size)
 {
-	rspq->intr_params = (QINTR_TIMER_IDX(timer_idx) |
-			     (pkt_cnt_idx < SGE_NCOUNTERS ? QINTR_CNT_EN : 0));
+	rspq->intr_params = (QINTR_TIMER_IDX_V(timer_idx) |
+			     (pkt_cnt_idx < SGE_NCOUNTERS ?
+			      QINTR_CNT_EN_F : 0));
 	rspq->pktcnt_idx = (pkt_cnt_idx < SGE_NCOUNTERS
 			    ? pkt_cnt_idx
 			    : 0);
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index 482f6de6817d..ad53e5ad2acd 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -524,7 +524,7 @@ static void unmap_rx_buf(struct adapter *adapter, struct sge_fl *fl)
  */
 static inline void ring_fl_db(struct adapter *adapter, struct sge_fl *fl)
 {
-	u32 val;
+	u32 val = adapter->params.arch.sge_fl_db;
 
 	/* The SGE keeps track of its Producer and Consumer Indices in terms
 	 * of Egress Queue Units so we can only tell it about integral numbers
@@ -532,11 +532,9 @@ static inline void ring_fl_db(struct adapter *adapter, struct sge_fl *fl)
 	 */
 	if (fl->pend_cred >= FL_PER_EQ_UNIT) {
 		if (is_t4(adapter->params.chip))
-			val = PIDX_V(fl->pend_cred / FL_PER_EQ_UNIT);
+			val |= PIDX_V(fl->pend_cred / FL_PER_EQ_UNIT);
 		else
-			val = PIDX_T5_V(fl->pend_cred / FL_PER_EQ_UNIT) |
-			      DBTYPE_F;
-		val |= DBPRIO_F;
+			val |= PIDX_T5_V(fl->pend_cred / FL_PER_EQ_UNIT);
 
 		/* Make sure all memory writes to the Free List queue are
 		 * committed before we tell the hardware about them.
@@ -1084,7 +1082,7 @@ static void inline_tx_skb(const struct sk_buff *skb, const struct sge_txq *tq,
  * Figure out what HW csum a packet wants and return the appropriate control
  * bits.
  */
-static u64 hwcsum(const struct sk_buff *skb)
+static u64 hwcsum(enum chip_type chip, const struct sk_buff *skb)
 {
 	int csum_type;
 	const struct iphdr *iph = ip_hdr(skb);
@@ -1100,7 +1098,7 @@ nocsum:
 			 * unknown protocol, disable HW csum
 			 * and hope a bad packet is detected
 			 */
-			return TXPKT_L4CSUM_DIS;
+			return TXPKT_L4CSUM_DIS_F;
 		}
 	} else {
 		/*
@@ -1116,16 +1114,21 @@ nocsum:
 			goto nocsum;
 	}
 
-	if (likely(csum_type >= TX_CSUM_TCPIP))
-		return TXPKT_CSUM_TYPE(csum_type) |
-			TXPKT_IPHDR_LEN(skb_network_header_len(skb)) |
-			TXPKT_ETHHDR_LEN(skb_network_offset(skb) - ETH_HLEN);
-	else {
+	if (likely(csum_type >= TX_CSUM_TCPIP)) {
+		u64 hdr_len = TXPKT_IPHDR_LEN_V(skb_network_header_len(skb));
+		int eth_hdr_len = skb_network_offset(skb) - ETH_HLEN;
+
+		if (chip <= CHELSIO_T5)
+			hdr_len |= TXPKT_ETHHDR_LEN_V(eth_hdr_len);
+		else
+			hdr_len |= T6_TXPKT_ETHHDR_LEN_V(eth_hdr_len);
+		return TXPKT_CSUM_TYPE_V(csum_type) | hdr_len;
+	} else {
 		int start = skb_transport_offset(skb);
 
-		return TXPKT_CSUM_TYPE(csum_type) |
-			TXPKT_CSUM_START(start) |
-			TXPKT_CSUM_LOC(start + skb->csum_offset);
+		return TXPKT_CSUM_TYPE_V(csum_type) |
+			TXPKT_CSUM_START_V(start) |
+			TXPKT_CSUM_LOC_V(start + skb->csum_offset);
 	}
 }
 
@@ -1160,7 +1163,7 @@ int t4vf_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	u32 wr_mid;
 	u64 cntrl, *end;
-	int qidx, credits;
+	int qidx, credits, max_pkt_len;
 	unsigned int flits, ndesc;
 	struct adapter *adapter;
 	struct sge_eth_txq *txq;
@@ -1183,6 +1186,13 @@ int t4vf_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (unlikely(skb->len < fw_hdr_copy_len))
 		goto out_free;
 
+	/* Discard the packet if the length is greater than mtu */
+	max_pkt_len = ETH_HLEN + dev->mtu;
+	if (skb_vlan_tag_present(skb))
+		max_pkt_len += VLAN_HLEN;
+	if (!skb_shinfo(skb)->gso_size && (unlikely(skb->len > max_pkt_len)))
+		goto out_free;
+
 	/*
 	 * Figure out which TX Queue we're going to use.
 	 */
@@ -1281,29 +1291,35 @@ int t4vf_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 		 * Fill in the LSO CPL message.
 		 */
 		lso->lso_ctrl =
-			cpu_to_be32(LSO_OPCODE(CPL_TX_PKT_LSO) |
-				    LSO_FIRST_SLICE |
-				    LSO_LAST_SLICE |
-				    LSO_IPV6(v6) |
-				    LSO_ETHHDR_LEN(eth_xtra_len/4) |
-				    LSO_IPHDR_LEN(l3hdr_len/4) |
-				    LSO_TCPHDR_LEN(tcp_hdr(skb)->doff));
+			cpu_to_be32(LSO_OPCODE_V(CPL_TX_PKT_LSO) |
+				    LSO_FIRST_SLICE_F |
+				    LSO_LAST_SLICE_F |
+				    LSO_IPV6_V(v6) |
+				    LSO_ETHHDR_LEN_V(eth_xtra_len / 4) |
+				    LSO_IPHDR_LEN_V(l3hdr_len / 4) |
+				    LSO_TCPHDR_LEN_V(tcp_hdr(skb)->doff));
 		lso->ipid_ofst = cpu_to_be16(0);
 		lso->mss = cpu_to_be16(ssi->gso_size);
 		lso->seqno_offset = cpu_to_be32(0);
 		if (is_t4(adapter->params.chip))
 			lso->len = cpu_to_be32(skb->len);
 		else
-			lso->len = cpu_to_be32(LSO_T5_XFER_SIZE(skb->len));
+			lso->len = cpu_to_be32(LSO_T5_XFER_SIZE_V(skb->len));
 
 		/*
 		 * Set up TX Packet CPL pointer, control word and perform
 		 * accounting.
 		 */
 		cpl = (void *)(lso + 1);
-		cntrl = (TXPKT_CSUM_TYPE(v6 ? TX_CSUM_TCPIP6 : TX_CSUM_TCPIP) |
-			 TXPKT_IPHDR_LEN(l3hdr_len) |
-			 TXPKT_ETHHDR_LEN(eth_xtra_len));
+
+		if (CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5)
+			cntrl = TXPKT_ETHHDR_LEN_V(eth_xtra_len);
+		else
+			cntrl = T6_TXPKT_ETHHDR_LEN_V(eth_xtra_len);
+
+		cntrl |= TXPKT_CSUM_TYPE_V(v6 ?
+					   TX_CSUM_TCPIP6 : TX_CSUM_TCPIP) |
+			 TXPKT_IPHDR_LEN_V(l3hdr_len);
 		txq->tso++;
 		txq->tx_cso += ssi->gso_segs;
 	} else {
@@ -1320,10 +1336,11 @@ int t4vf_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 		 */
 		cpl = (void *)(wr + 1);
 		if (skb->ip_summed == CHECKSUM_PARTIAL) {
-			cntrl = hwcsum(skb) | TXPKT_IPCSUM_DIS;
+			cntrl = hwcsum(adapter->params.chip, skb) |
+				TXPKT_IPCSUM_DIS_F;
 			txq->tx_cso++;
 		} else
-			cntrl = TXPKT_L4CSUM_DIS | TXPKT_IPCSUM_DIS;
+			cntrl = TXPKT_L4CSUM_DIS_F | TXPKT_IPCSUM_DIS_F;
 	}
 
 	/*
@@ -1332,15 +1349,15 @@ int t4vf_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 	 */
 	if (skb_vlan_tag_present(skb)) {
 		txq->vlan_ins++;
-		cntrl |= TXPKT_VLAN_VLD | TXPKT_VLAN(skb_vlan_tag_get(skb));
+		cntrl |= TXPKT_VLAN_VLD_F | TXPKT_VLAN_V(skb_vlan_tag_get(skb));
 	}
 
 	/*
 	 * Fill in the TX Packet CPL message header.
 	 */
-	cpl->ctrl0 = cpu_to_be32(TXPKT_OPCODE(CPL_TX_PKT_XT) |
-				 TXPKT_INTF(pi->port_id) |
-				 TXPKT_PF(0));
+	cpl->ctrl0 = cpu_to_be32(TXPKT_OPCODE_V(CPL_TX_PKT_XT) |
+				 TXPKT_INTF_V(pi->port_id) |
+				 TXPKT_PF_V(0));
 	cpl->pack = cpu_to_be16(0);
 	cpl->len = cpu_to_be16(skb->len);
 	cpl->ctrl1 = cpu_to_be64(cntrl);
@@ -1663,7 +1680,7 @@ int t4vf_ethrx_handler(struct sge_rspq *rspq, const __be64 *rsp,
 static inline bool is_new_response(const struct rsp_ctrl *rc,
 				   const struct sge_rspq *rspq)
 {
-	return RSPD_GEN(rc->type_gen) == rspq->gen;
+	return ((rc->type_gen >> RSPD_GEN_S) & 0x1) == rspq->gen;
 }
 
 /**
@@ -1752,8 +1769,8 @@ static int process_responses(struct sge_rspq *rspq, int budget)
 		 * SGE.
 		 */
 		dma_rmb();
-		rsp_type = RSPD_TYPE(rc->type_gen);
-		if (likely(rsp_type == RSP_TYPE_FLBUF)) {
+		rsp_type = RSPD_TYPE_G(rc->type_gen);
+		if (likely(rsp_type == RSPD_TYPE_FLBUF_X)) {
 			struct page_frag *fp;
 			struct pkt_gl gl;
 			const struct rx_sw_desc *sdesc;
@@ -1764,7 +1781,7 @@ static int process_responses(struct sge_rspq *rspq, int budget)
 			 * If we get a "new buffer" message from the SGE we
 			 * need to move on to the next Free List buffer.
 			 */
-			if (len & RSPD_NEWBUF) {
+			if (len & RSPD_NEWBUF_F) {
 				/*
 				 * We get one "new buffer" message when we
 				 * first start up a queue so we need to ignore
@@ -1775,7 +1792,7 @@ static int process_responses(struct sge_rspq *rspq, int budget)
 						     1);
 					rspq->offset = 0;
 				}
-				len = RSPD_LEN(len);
+				len = RSPD_LEN_G(len);
 			}
 			gl.tot_len = len;
 
@@ -1818,10 +1835,10 @@ static int process_responses(struct sge_rspq *rspq, int budget)
 				rspq->offset += ALIGN(fp->size, s->fl_align);
 			else
 				restore_rx_bufs(&gl, &rxq->fl, frag);
-		} else if (likely(rsp_type == RSP_TYPE_CPL)) {
+		} else if (likely(rsp_type == RSPD_TYPE_CPL_X)) {
 			ret = rspq->handler(rspq, rspq->cur_desc, NULL);
 		} else {
-			WARN_ON(rsp_type > RSP_TYPE_CPL);
+			WARN_ON(rsp_type > RSPD_TYPE_CPL_X);
 			ret = 0;
 		}
 
@@ -1833,7 +1850,7 @@ static int process_responses(struct sge_rspq *rspq, int budget)
 			 */
 			const int NOMEM_TIMER_IDX = SGE_NTIMERS-1;
 			rspq->next_intr_params =
-				QINTR_TIMER_IDX(NOMEM_TIMER_IDX);
+				QINTR_TIMER_IDX_V(NOMEM_TIMER_IDX);
 			break;
 		}
 
@@ -1875,7 +1892,7 @@ static int napi_rx_handler(struct napi_struct *napi, int budget)
 		intr_params = rspq->next_intr_params;
 		rspq->next_intr_params = rspq->intr_params;
 	} else
-		intr_params = QINTR_TIMER_IDX(SGE_TIMER_UPD_CIDX);
+		intr_params = QINTR_TIMER_IDX_V(SGE_TIMER_UPD_CIDX);
 
 	if (unlikely(work_done == 0))
 		rspq->unhandled_irqs++;
@@ -1936,10 +1953,10 @@ static unsigned int process_intrq(struct adapter *adapter)
 		 * never happen ...
 		 */
 		dma_rmb();
-		if (unlikely(RSPD_TYPE(rc->type_gen) != RSP_TYPE_INTR)) {
+		if (unlikely(RSPD_TYPE_G(rc->type_gen) != RSPD_TYPE_INTR_X)) {
 			dev_err(adapter->pdev_dev,
 				"Unexpected INTRQ response type %d\n",
-				RSPD_TYPE(rc->type_gen));
+				RSPD_TYPE_G(rc->type_gen));
 			continue;
 		}
 
@@ -1951,7 +1968,7 @@ static unsigned int process_intrq(struct adapter *adapter)
 		 * want to either make them fatal and/or conditionalized under
 		 * DEBUG.
 		 */
-		qid = RSPD_QID(be32_to_cpu(rc->pldbuflen_qid));
+		qid = RSPD_QID_G(be32_to_cpu(rc->pldbuflen_qid));
 		iq_idx = IQ_IDX(s, qid);
 		if (unlikely(iq_idx >= MAX_INGQ)) {
 			dev_err(adapter->pdev_dev,
@@ -2154,8 +2171,8 @@ static void __iomem *bar2_address(struct adapter *adapter,
 	u64 bar2_qoffset;
 	int ret;
 
-	ret = t4_bar2_sge_qregs(adapter, qid, qtype,
-				&bar2_qoffset, pbar2_qid);
+	ret = t4vf_bar2_sge_qregs(adapter, qid, qtype,
+				  &bar2_qoffset, pbar2_qid);
 	if (ret)
 		return NULL;
 
@@ -2239,12 +2256,18 @@ int t4vf_sge_alloc_rxq(struct adapter *adapter, struct sge_rspq *rspq,
 	cmd.iqaddr = cpu_to_be64(rspq->phys_addr);
 
 	if (fl) {
+		enum chip_type chip =
+			CHELSIO_CHIP_VERSION(adapter->params.chip);
 		/*
 		 * Allocate the ring for the hardware free list (with space
 		 * for its status page) along with the associated software
 		 * descriptor ring.  The free list size needs to be a multiple
-		 * of the Egress Queue Unit.
+		 * of the Egress Queue Unit and at least 2 Egress Units larger
+		 * than the SGE's Egress Congrestion Threshold
+		 * (fl_starve_thres - 1).
 		 */
+		if (fl->size < s->fl_starve_thres - 1 + 2 * FL_PER_EQ_UNIT)
+			fl->size = s->fl_starve_thres - 1 + 2 * FL_PER_EQ_UNIT;
 		fl->size = roundup(fl->size, FL_PER_EQ_UNIT);
 		fl->desc = alloc_ring(adapter->pdev_dev, fl->size,
 				      sizeof(__be64), sizeof(struct rx_sw_desc),
@@ -2274,7 +2297,9 @@ int t4vf_sge_alloc_rxq(struct adapter *adapter, struct sge_rspq *rspq,
 		cmd.fl0dcaen_to_fl0cidxfthresh =
 			cpu_to_be16(
 				FW_IQ_CMD_FL0FBMIN_V(SGE_FETCHBURSTMIN_64B) |
-				FW_IQ_CMD_FL0FBMAX_V(SGE_FETCHBURSTMAX_512B));
+				FW_IQ_CMD_FL0FBMAX_V((chip <= CHELSIO_T5) ?
+						     FETCHBURSTMAX_512B_X :
+						     FETCHBURSTMAX_256B_X));
 		cmd.fl0size = cpu_to_be16(flsz);
 		cmd.fl0addr = cpu_to_be64(fl->addr);
 	}
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
index b9debb4f29a3..88b8981b4751 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
@@ -51,6 +51,7 @@
  */
 #define CHELSIO_T4		0x4
 #define CHELSIO_T5		0x5
+#define CHELSIO_T6		0x6
 
 enum chip_type {
 	T4_A1 = CHELSIO_CHIP_CODE(CHELSIO_T4, 1),
@@ -156,6 +157,12 @@ struct vpd_params {
 	u32 cclk;			/* Core Clock (KHz) */
 };
 
+/* Stores chip specific parameters */
+struct arch_specific_params {
+	u32 sge_fl_db;
+	u16 mps_tcam_size;
+};
+
 /*
  * Global Receive Side Scaling (RSS) parameters in host-native format.
  */
@@ -215,6 +222,7 @@ struct adapter_params {
 	struct vpd_params vpd;		/* Vital Product Data */
 	struct rss_params rss;		/* Receive Side Scaling */
 	struct vf_resources vfres;	/* Virtual Function Resource limits */
+	struct arch_specific_params arch; /* chip specific params */
 	enum chip_type chip;		/* chip code */
 	u8 nports;			/* # of Ethernet "ports" */
 };
@@ -284,11 +292,11 @@ int t4vf_fw_reset(struct adapter *);
 int t4vf_set_params(struct adapter *, unsigned int, const u32 *, const u32 *);
 
 enum t4_bar2_qtype { T4_BAR2_QTYPE_EGRESS, T4_BAR2_QTYPE_INGRESS };
-int t4_bar2_sge_qregs(struct adapter *adapter,
-		      unsigned int qid,
-		      enum t4_bar2_qtype qtype,
-		      u64 *pbar2_qoffset,
-		      unsigned int *pbar2_qid);
+int t4vf_bar2_sge_qregs(struct adapter *adapter,
+			unsigned int qid,
+			enum t4_bar2_qtype qtype,
+			u64 *pbar2_qoffset,
+			unsigned int *pbar2_qid);
 
 int t4vf_get_sge_params(struct adapter *);
 int t4vf_get_vpd_params(struct adapter *);
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
index 966ee900ed00..0db6dc9e9ed2 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
@@ -428,7 +428,7 @@ int t4vf_set_params(struct adapter *adapter, unsigned int nparams,
 }
 
 /**
- *	t4_bar2_sge_qregs - return BAR2 SGE Queue register information
+ *	t4vf_bar2_sge_qregs - return BAR2 SGE Queue register information
  *	@adapter: the adapter
  *	@qid: the Queue ID
  *	@qtype: the Ingress or Egress type for @qid
@@ -452,11 +452,11 @@ int t4vf_set_params(struct adapter *adapter, unsigned int nparams,
  *	Write Combining Doorbell Buffer. If the BAR2 Queue ID is not 0,
  *	then these "Inferred Queue ID" register may not be used.
  */
-int t4_bar2_sge_qregs(struct adapter *adapter,
-		      unsigned int qid,
-		      enum t4_bar2_qtype qtype,
-		      u64 *pbar2_qoffset,
-		      unsigned int *pbar2_qid)
+int t4vf_bar2_sge_qregs(struct adapter *adapter,
+			unsigned int qid,
+			enum t4_bar2_qtype qtype,
+			u64 *pbar2_qoffset,
+			unsigned int *pbar2_qid)
 {
 	unsigned int page_shift, page_size, qpp_shift, qpp_mask;
 	u64 bar2_page_offset, bar2_qoffset;
@@ -1191,9 +1191,7 @@ int t4vf_alloc_mac_filt(struct adapter *adapter, unsigned int viid, bool free,
 	unsigned nfilters = 0;
 	unsigned int rem = naddr;
 	struct fw_vi_mac_cmd cmd, rpl;
-	unsigned int max_naddr = is_t4(adapter->params.chip) ?
-				 NUM_MPS_CLS_SRAM_L_INSTANCES :
-				 NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
+	unsigned int max_naddr = adapter->params.arch.mps_tcam_size;
 
 	if (naddr > max_naddr)
 		return -EINVAL;
@@ -1285,9 +1283,7 @@ int t4vf_change_mac(struct adapter *adapter, unsigned int viid,
 	struct fw_vi_mac_exact *p = &cmd.u.exact[0];
 	size_t len16 = DIV_ROUND_UP(offsetof(struct fw_vi_mac_cmd,
 					     u.exact[1]), 16);
-	unsigned int max_naddr = is_t4(adapter->params.chip) ?
-				 NUM_MPS_CLS_SRAM_L_INSTANCES :
-				 NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
+	unsigned int max_mac_addr = adapter->params.arch.mps_tcam_size;
 
 	/*
 	 * If this is a new allocation, determine whether it should be
@@ -1310,7 +1306,7 @@ int t4vf_change_mac(struct adapter *adapter, unsigned int viid,
 	if (ret == 0) {
 		p = &rpl.u.exact[0];
 		ret = FW_VI_MAC_CMD_IDX_G(be16_to_cpu(p->valid_to_idx));
-		if (ret >= max_naddr)
+		if (ret >= max_mac_addr)
 			ret = -ENOMEM;
 	}
 	return ret;
@@ -1590,11 +1586,25 @@ int t4vf_prep_adapter(struct adapter *adapter)
 	switch (CHELSIO_PCI_ID_VER(adapter->pdev->device)) {
 	case CHELSIO_T4:
 		adapter->params.chip |= CHELSIO_CHIP_CODE(CHELSIO_T4, 0);
+		adapter->params.arch.sge_fl_db = DBPRIO_F;
+		adapter->params.arch.mps_tcam_size =
+				NUM_MPS_CLS_SRAM_L_INSTANCES;
 		break;
 
 	case CHELSIO_T5:
 		chipid = REV_G(t4_read_reg(adapter, PL_VF_REV_A));
 		adapter->params.chip |= CHELSIO_CHIP_CODE(CHELSIO_T5, chipid);
+		adapter->params.arch.sge_fl_db = DBPRIO_F | DBTYPE_F;
+		adapter->params.arch.mps_tcam_size =
+				NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
+		break;
+
+	case CHELSIO_T6:
+		chipid = REV_G(t4_read_reg(adapter, PL_VF_REV_A));
+		adapter->params.chip |= CHELSIO_CHIP_CODE(CHELSIO_T6, chipid);
+		adapter->params.arch.sge_fl_db = 0;
+		adapter->params.arch.mps_tcam_size =
+				NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
 		break;
 	}
 
diff --git a/drivers/net/ethernet/cirrus/Kconfig b/drivers/net/ethernet/cirrus/Kconfig
index 905ac5f5d9a6..5ab912937aff 100644
--- a/drivers/net/ethernet/cirrus/Kconfig
+++ b/drivers/net/ethernet/cirrus/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_CIRRUS
 	default y
 	depends on ISA || EISA || ARM || MAC
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -23,9 +21,7 @@ config CS89x0
 	depends on ISA || EISA || ARM
 	---help---
 	  Support for CS89x0 chipset based Ethernet cards. If you have a
-	  network (Ethernet) card of this type, say Y and read the
-	  Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto> as well as
+	  network (Ethernet) card of this type, say Y and read the file
 	  <file:Documentation/networking/cs89x0.txt>.
 
 	  To compile this driver as a module, choose M here. The module
@@ -55,9 +51,7 @@ config MAC89x0
 	depends on MAC
 	---help---
 	  Support for CS89x0 chipset based Ethernet cards.  If you have a
-	  Nubus or LC-PDS network (Ethernet) card of this type, say Y and
-	  read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  Nubus or LC-PDS network (Ethernet) card of this type, say Y here.
 
 	  To compile this driver as a module, choose M here. This module will
 	  be called mac89x0.
diff --git a/drivers/net/ethernet/cisco/Kconfig b/drivers/net/ethernet/cisco/Kconfig
index 1c7b884e3371..15b713a89620 100644
--- a/drivers/net/ethernet/cisco/Kconfig
+++ b/drivers/net/ethernet/cisco/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_CISCO
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/cisco/enic/enic_clsf.c b/drivers/net/ethernet/cisco/enic/enic_clsf.c
index 0be6850be8a2..d106186f4f4a 100644
--- a/drivers/net/ethernet/cisco/enic/enic_clsf.c
+++ b/drivers/net/ethernet/cisco/enic/enic_clsf.c
@@ -5,7 +5,7 @@
 #include <linux/in.h>
 #include <linux/types.h>
 #include <linux/skbuff.h>
-#include <net/flow_keys.h>
+#include <net/flow_dissector.h>
 #include "enic_res.h"
 #include "enic_clsf.h"
 
@@ -15,14 +15,14 @@
  *	@rq: rq number to steer to
  *
  * This function returns filter_id(hardware_id) of the filter
- * added. In case of error it returns an negative number.
+ * added. In case of error it returns a negative number.
  */
 int enic_addfltr_5t(struct enic *enic, struct flow_keys *keys, u16 rq)
 {
 	int res;
 	struct filter data;
 
-	switch (keys->ip_proto) {
+	switch (keys->basic.ip_proto) {
 	case IPPROTO_TCP:
 		data.u.ipv4.protocol = PROTO_TCP;
 		break;
@@ -33,10 +33,10 @@ int enic_addfltr_5t(struct enic *enic, struct flow_keys *keys, u16 rq)
 		return -EPROTONOSUPPORT;
 	};
 	data.type = FILTER_IPV4_5TUPLE;
-	data.u.ipv4.src_addr = ntohl(keys->src);
-	data.u.ipv4.dst_addr = ntohl(keys->dst);
-	data.u.ipv4.src_port = ntohs(keys->port16[0]);
-	data.u.ipv4.dst_port = ntohs(keys->port16[1]);
+	data.u.ipv4.src_addr = ntohl(keys->addrs.v4addrs.src);
+	data.u.ipv4.dst_addr = ntohl(keys->addrs.v4addrs.dst);
+	data.u.ipv4.src_port = ntohs(keys->ports.src);
+	data.u.ipv4.dst_port = ntohs(keys->ports.dst);
 	data.u.ipv4.flags = FILTER_FIELDS_IPV4_5TUPLE;
 
 	spin_lock_bh(&enic->devcmd_lock);
@@ -158,11 +158,11 @@ static struct enic_rfs_fltr_node *htbl_key_search(struct hlist_head *h,
 	struct enic_rfs_fltr_node *tpos;
 
 	hlist_for_each_entry(tpos, h, node)
-		if (tpos->keys.src == k->src &&
-		    tpos->keys.dst == k->dst &&
-		    tpos->keys.ports == k->ports &&
-		    tpos->keys.ip_proto == k->ip_proto &&
-		    tpos->keys.n_proto == k->n_proto)
+		if (tpos->keys.addrs.v4addrs.src == k->addrs.v4addrs.src &&
+		    tpos->keys.addrs.v4addrs.dst == k->addrs.v4addrs.dst &&
+		    tpos->keys.ports.ports == k->ports.ports &&
+		    tpos->keys.basic.ip_proto == k->basic.ip_proto &&
+		    tpos->keys.basic.n_proto == k->basic.n_proto)
 			return tpos;
 	return NULL;
 }
@@ -177,9 +177,10 @@ int enic_rx_flow_steer(struct net_device *dev, const struct sk_buff *skb,
 	int res, i;
 
 	enic = netdev_priv(dev);
-	res = skb_flow_dissect(skb, &keys);
-	if (!res || keys.n_proto != htons(ETH_P_IP) ||
-	    (keys.ip_proto != IPPROTO_TCP && keys.ip_proto != IPPROTO_UDP))
+	res = skb_flow_dissect_flow_keys(skb, &keys);
+	if (!res || keys.basic.n_proto != htons(ETH_P_IP) ||
+	    (keys.basic.ip_proto != IPPROTO_TCP &&
+	     keys.basic.ip_proto != IPPROTO_UDP))
 		return -EPROTONOSUPPORT;
 
 	tbl_idx = skb_get_hash_raw(skb) & ENIC_RFS_FLW_MASK;
diff --git a/drivers/net/ethernet/cisco/enic/enic_ethtool.c b/drivers/net/ethernet/cisco/enic/enic_ethtool.c
index 28d9ca675a27..f3f1601a76f3 100644
--- a/drivers/net/ethernet/cisco/enic/enic_ethtool.c
+++ b/drivers/net/ethernet/cisco/enic/enic_ethtool.c
@@ -131,8 +131,15 @@ static void enic_get_drvinfo(struct net_device *netdev,
 {
 	struct enic *enic = netdev_priv(netdev);
 	struct vnic_devcmd_fw_info *fw_info;
+	int err;
 
-	enic_dev_fw_info(enic, &fw_info);
+	err = enic_dev_fw_info(enic, &fw_info);
+	/* return only when pci_zalloc_consistent fails in vnic_dev_fw_info
+	 * For other failures, like devcmd failure, we return previously
+	 * recorded info.
+	 */
+	if (err == -ENOMEM)
+		return;
 
 	strlcpy(drvinfo->driver, DRV_NAME, sizeof(drvinfo->driver));
 	strlcpy(drvinfo->version, DRV_VERSION, sizeof(drvinfo->version));
@@ -181,8 +188,15 @@ static void enic_get_ethtool_stats(struct net_device *netdev,
 	struct enic *enic = netdev_priv(netdev);
 	struct vnic_stats *vstats;
 	unsigned int i;
+	int err;
 
-	enic_dev_stats_dump(enic, &vstats);
+	err = enic_dev_stats_dump(enic, &vstats);
+	/* return only when pci_zalloc_consistent fails in vnic_dev_stats_dump
+	 * For other failures, like devcmd failure, we return previously
+	 * recorded stats.
+	 */
+	if (err == -ENOMEM)
+		return;
 
 	for (i = 0; i < enic_n_tx_stats; i++)
 		*(data++) = ((u64 *)&vstats->tx)[enic_tx_stats[i].index];
@@ -334,7 +348,7 @@ static int enic_grxclsrule(struct enic *enic, struct ethtool_rxnfc *cmd)
 	n = htbl_fltr_search(enic, (u16)fsp->location);
 	if (!n)
 		return -EINVAL;
-	switch (n->keys.ip_proto) {
+	switch (n->keys.basic.ip_proto) {
 	case IPPROTO_TCP:
 		fsp->flow_type = TCP_V4_FLOW;
 		break;
@@ -346,16 +360,16 @@ static int enic_grxclsrule(struct enic *enic, struct ethtool_rxnfc *cmd)
 		break;
 	}
 
-	fsp->h_u.tcp_ip4_spec.ip4src = n->keys.src;
+	fsp->h_u.tcp_ip4_spec.ip4src = flow_get_u32_src(&n->keys);
 	fsp->m_u.tcp_ip4_spec.ip4src = (__u32)~0;
 
-	fsp->h_u.tcp_ip4_spec.ip4dst = n->keys.dst;
+	fsp->h_u.tcp_ip4_spec.ip4dst = flow_get_u32_dst(&n->keys);
 	fsp->m_u.tcp_ip4_spec.ip4dst = (__u32)~0;
 
-	fsp->h_u.tcp_ip4_spec.psrc = n->keys.port16[0];
+	fsp->h_u.tcp_ip4_spec.psrc = n->keys.ports.src;
 	fsp->m_u.tcp_ip4_spec.psrc = (__u16)~0;
 
-	fsp->h_u.tcp_ip4_spec.pdst = n->keys.port16[1];
+	fsp->h_u.tcp_ip4_spec.pdst = n->keys.ports.dst;
 	fsp->m_u.tcp_ip4_spec.pdst = (__u16)~0;
 
 	fsp->ring_cookie = n->rq_id;
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index 204bd182473b..da2004e2a741 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -615,8 +615,15 @@ static struct rtnl_link_stats64 *enic_get_stats(struct net_device *netdev,
 {
 	struct enic *enic = netdev_priv(netdev);
 	struct vnic_stats *stats;
+	int err;
 
-	enic_dev_stats_dump(enic, &stats);
+	err = enic_dev_stats_dump(enic, &stats);
+	/* return only when pci_zalloc_consistent fails in vnic_dev_stats_dump
+	 * For other failures, like devcmd failure, we return previously
+	 * recorded stats.
+	 */
+	if (err == -ENOMEM)
+		return net_stats;
 
 	net_stats->tx_packets = stats->tx.tx_frames_ok;
 	net_stats->tx_bytes = stats->tx.tx_bytes_ok;
@@ -1201,7 +1208,7 @@ static int enic_poll(struct napi_struct *napi, int budget)
 		napi_complete(napi);
 		vnic_intr_unmask(&enic->intr[intr]);
 	}
-	enic_poll_unlock_napi(&enic->rq[cq_rq]);
+	enic_poll_unlock_napi(&enic->rq[cq_rq], napi);
 
 	return rq_work_done;
 }
@@ -1407,6 +1414,7 @@ static int enic_poll_msix_rq(struct napi_struct *napi, int budget)
 		 */
 		enic_calc_int_moderation(enic, &enic->rq[rq]);
 
+	enic_poll_unlock_napi(&enic->rq[rq], napi);
 	if (work_done < work_to_do) {
 
 		/* Some work done, but not enough to stay in polling,
@@ -1418,7 +1426,6 @@ static int enic_poll_msix_rq(struct napi_struct *napi, int budget)
 			enic_set_int_moderation(enic, &enic->rq[rq]);
 		vnic_intr_unmask(&enic->intr[intr]);
 	}
-	enic_poll_unlock_napi(&enic->rq[rq]);
 
 	return work_done;
 }
diff --git a/drivers/net/ethernet/cisco/enic/vnic_rq.c b/drivers/net/ethernet/cisco/enic/vnic_rq.c
index 36a2ed606c91..c4b2183bf352 100644
--- a/drivers/net/ethernet/cisco/enic/vnic_rq.c
+++ b/drivers/net/ethernet/cisco/enic/vnic_rq.c
@@ -188,16 +188,15 @@ void vnic_rq_clean(struct vnic_rq *rq,
 	struct vnic_rq_buf *buf;
 	u32 fetch_index;
 	unsigned int count = rq->ring.desc_count;
+	int i;
 
 	buf = rq->to_clean;
 
-	while (vnic_rq_desc_used(rq) > 0) {
-
+	for (i = 0; i < rq->ring.desc_count; i++) {
 		(*buf_clean)(rq, buf);
-
-		buf = rq->to_clean = buf->next;
-		rq->ring.desc_avail++;
+		buf = buf->next;
 	}
+	rq->ring.desc_avail = rq->ring.desc_count - 1;
 
 	/* Use current fetch_index as the ring starting point */
 	fetch_index = ioread32(&rq->ctrl->fetch_index);
diff --git a/drivers/net/ethernet/cisco/enic/vnic_rq.h b/drivers/net/ethernet/cisco/enic/vnic_rq.h
index 8111d5202df2..b9c82f143d7e 100644
--- a/drivers/net/ethernet/cisco/enic/vnic_rq.h
+++ b/drivers/net/ethernet/cisco/enic/vnic_rq.h
@@ -21,6 +21,7 @@
 #define _VNIC_RQ_H_
 
 #include <linux/pci.h>
+#include <linux/netdevice.h>
 
 #include "vnic_dev.h"
 #include "vnic_cq.h"
@@ -75,6 +76,12 @@ struct vnic_rq_buf {
 	uint64_t wr_id;
 };
 
+enum enic_poll_state {
+	ENIC_POLL_STATE_IDLE,
+	ENIC_POLL_STATE_NAPI,
+	ENIC_POLL_STATE_POLL
+};
+
 struct vnic_rq {
 	unsigned int index;
 	struct vnic_dev *vdev;
@@ -86,19 +93,7 @@ struct vnic_rq {
 	void *os_buf_head;
 	unsigned int pkts_outstanding;
 #ifdef CONFIG_NET_RX_BUSY_POLL
-#define ENIC_POLL_STATE_IDLE		0
-#define ENIC_POLL_STATE_NAPI		(1 << 0) /* NAPI owns this poll */
-#define ENIC_POLL_STATE_POLL		(1 << 1) /* poll owns this poll */
-#define ENIC_POLL_STATE_NAPI_YIELD	(1 << 2) /* NAPI yielded this poll */
-#define ENIC_POLL_STATE_POLL_YIELD	(1 << 3) /* poll yielded this poll */
-#define ENIC_POLL_YIELD			(ENIC_POLL_STATE_NAPI_YIELD |	\
-					 ENIC_POLL_STATE_POLL_YIELD)
-#define ENIC_POLL_LOCKED		(ENIC_POLL_STATE_NAPI |		\
-					 ENIC_POLL_STATE_POLL)
-#define ENIC_POLL_USER_PEND		(ENIC_POLL_STATE_POLL |		\
-					 ENIC_POLL_STATE_POLL_YIELD)
-	unsigned int bpoll_state;
-	spinlock_t bpoll_lock;
+	atomic_t bpoll_state;
 #endif /* CONFIG_NET_RX_BUSY_POLL */
 };
 
@@ -215,76 +210,43 @@ static inline int vnic_rq_fill(struct vnic_rq *rq,
 #ifdef CONFIG_NET_RX_BUSY_POLL
 static inline void enic_busy_poll_init_lock(struct vnic_rq *rq)
 {
-	spin_lock_init(&rq->bpoll_lock);
-	rq->bpoll_state = ENIC_POLL_STATE_IDLE;
+	atomic_set(&rq->bpoll_state, ENIC_POLL_STATE_IDLE);
 }
 
 static inline bool enic_poll_lock_napi(struct vnic_rq *rq)
 {
-	bool rc = true;
-
-	spin_lock(&rq->bpoll_lock);
-	if (rq->bpoll_state & ENIC_POLL_LOCKED) {
-		WARN_ON(rq->bpoll_state & ENIC_POLL_STATE_NAPI);
-		rq->bpoll_state |= ENIC_POLL_STATE_NAPI_YIELD;
-		rc = false;
-	} else {
-		rq->bpoll_state = ENIC_POLL_STATE_NAPI;
-	}
-	spin_unlock(&rq->bpoll_lock);
+	int rc = atomic_cmpxchg(&rq->bpoll_state, ENIC_POLL_STATE_IDLE,
+				ENIC_POLL_STATE_NAPI);
 
-	return rc;
+	return (rc == ENIC_POLL_STATE_IDLE);
 }
 
-static inline bool enic_poll_unlock_napi(struct vnic_rq *rq)
+static inline void enic_poll_unlock_napi(struct vnic_rq *rq,
+					 struct napi_struct *napi)
 {
-	bool rc = false;
-
-	spin_lock(&rq->bpoll_lock);
-	WARN_ON(rq->bpoll_state &
-		(ENIC_POLL_STATE_POLL | ENIC_POLL_STATE_NAPI_YIELD));
-	if (rq->bpoll_state & ENIC_POLL_STATE_POLL_YIELD)
-		rc = true;
-	rq->bpoll_state = ENIC_POLL_STATE_IDLE;
-	spin_unlock(&rq->bpoll_lock);
-
-	return rc;
+	WARN_ON(atomic_read(&rq->bpoll_state) != ENIC_POLL_STATE_NAPI);
+	napi_gro_flush(napi, false);
+	atomic_set(&rq->bpoll_state, ENIC_POLL_STATE_IDLE);
 }
 
 static inline bool enic_poll_lock_poll(struct vnic_rq *rq)
 {
-	bool rc = true;
-
-	spin_lock_bh(&rq->bpoll_lock);
-	if (rq->bpoll_state & ENIC_POLL_LOCKED) {
-		rq->bpoll_state |= ENIC_POLL_STATE_POLL_YIELD;
-		rc = false;
-	} else {
-		rq->bpoll_state |= ENIC_POLL_STATE_POLL;
-	}
-	spin_unlock_bh(&rq->bpoll_lock);
+	int rc = atomic_cmpxchg(&rq->bpoll_state, ENIC_POLL_STATE_IDLE,
+				ENIC_POLL_STATE_POLL);
 
-	return rc;
+	return (rc == ENIC_POLL_STATE_IDLE);
 }
 
-static inline bool enic_poll_unlock_poll(struct vnic_rq *rq)
-{
-	bool rc = false;
 
-	spin_lock_bh(&rq->bpoll_lock);
-	WARN_ON(rq->bpoll_state & ENIC_POLL_STATE_NAPI);
-	if (rq->bpoll_state & ENIC_POLL_STATE_POLL_YIELD)
-		rc = true;
-	rq->bpoll_state = ENIC_POLL_STATE_IDLE;
-	spin_unlock_bh(&rq->bpoll_lock);
-
-	return rc;
+static inline void enic_poll_unlock_poll(struct vnic_rq *rq)
+{
+	WARN_ON(atomic_read(&rq->bpoll_state) != ENIC_POLL_STATE_POLL);
+	atomic_set(&rq->bpoll_state, ENIC_POLL_STATE_IDLE);
 }
 
 static inline bool enic_poll_busy_polling(struct vnic_rq *rq)
 {
-	WARN_ON(!(rq->bpoll_state & ENIC_POLL_LOCKED));
-	return rq->bpoll_state & ENIC_POLL_USER_PEND;
+	return atomic_read(&rq->bpoll_state) & ENIC_POLL_STATE_POLL;
 }
 
 #else
@@ -298,7 +260,8 @@ static inline bool enic_poll_lock_napi(struct vnic_rq *rq)
 	return true;
 }
 
-static inline bool enic_poll_unlock_napi(struct vnic_rq *rq)
+static inline bool enic_poll_unlock_napi(struct vnic_rq *rq,
+					 struct napi_struct *napi)
 {
 	return false;
 }
diff --git a/drivers/net/ethernet/dec/Kconfig b/drivers/net/ethernet/dec/Kconfig
index 68262aa57d01..740bbad5ed38 100644
--- a/drivers/net/ethernet/dec/Kconfig
+++ b/drivers/net/ethernet/dec/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_DEC
 	default y
 	depends on PCI || EISA || CARDBUS
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/dec/tulip/Kconfig b/drivers/net/ethernet/dec/tulip/Kconfig
index eb9ba6e97d04..1003201b5d80 100644
--- a/drivers/net/ethernet/dec/tulip/Kconfig
+++ b/drivers/net/ethernet/dec/tulip/Kconfig
@@ -21,8 +21,7 @@ config DE2104X
 	  of this type.  (If your card is NOT SMC EtherPower 10/100 PCI
 	  (smc9332dst), you can also try the driver for "Generic DECchip"
 	  cards, below.  However, most people with a network card of this type
-	  will say Y here.) Do read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  will say Y here.)
 
 	  To compile this driver as a module, choose M here. The module will
 	  be called de2104x.
@@ -50,8 +49,7 @@ config TULIP
 	  of this type.  (If your card is NOT SMC EtherPower 10/100 PCI
 	  (smc9332dst), you can also try the driver for "Generic DECchip"
 	  cards, above.  However, most people with a network card of this type
-	  will say Y here.) Do read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  will say Y here.)
 
 	  To compile this driver as a module, choose M here. The module will
 	  be called tulip.
@@ -113,9 +111,7 @@ config DE4X5
 	---help---
 	  This is support for the DIGITAL series of PCI/EISA Ethernet cards.
 	  These include the DE425, DE434, DE435, DE450 and DE500 models.  If
-	  you have a network card of this type, say Y and read the
-	  Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>. More specific
+	  you have a network card of this type, say Y.  More specific
 	  information is contained in
 	  <file:Documentation/networking/de4x5.txt>.
 
diff --git a/drivers/net/ethernet/dec/tulip/de4x5.c b/drivers/net/ethernet/dec/tulip/de4x5.c
index badff181e719..8966f3159bb2 100644
--- a/drivers/net/ethernet/dec/tulip/de4x5.c
+++ b/drivers/net/ethernet/dec/tulip/de4x5.c
@@ -5189,16 +5189,16 @@ de4x5_parse_params(struct net_device *dev)
 	if (strstr(p, "fdx") || strstr(p, "FDX")) lp->params.fdx = true;
 
 	if (strstr(p, "autosense") || strstr(p, "AUTOSENSE")) {
-	    if (strstr(p, "TP")) {
-		lp->params.autosense = TP;
-	    } else if (strstr(p, "TP_NW")) {
+	    if (strstr(p, "TP_NW")) {
 		lp->params.autosense = TP_NW;
+	    } else if (strstr(p, "TP")) {
+		lp->params.autosense = TP;
+	    } else if (strstr(p, "BNC_AUI")) {
+		lp->params.autosense = BNC;
 	    } else if (strstr(p, "BNC")) {
 		lp->params.autosense = BNC;
 	    } else if (strstr(p, "AUI")) {
 		lp->params.autosense = AUI;
-	    } else if (strstr(p, "BNC_AUI")) {
-		lp->params.autosense = BNC;
 	    } else if (strstr(p, "10Mb")) {
 		lp->params.autosense = _10Mb;
 	    } else if (strstr(p, "100Mb")) {
diff --git a/drivers/net/ethernet/dec/tulip/uli526x.c b/drivers/net/ethernet/dec/tulip/uli526x.c
index 2c30c0c83f98..447d09272ab7 100644
--- a/drivers/net/ethernet/dec/tulip/uli526x.c
+++ b/drivers/net/ethernet/dec/tulip/uli526x.c
@@ -1115,7 +1115,7 @@ static void uli526x_timer(unsigned long data)
 				netif_carrier_off(dev);
 			}
 		}
-		db->init=0;
+	db->init = 0;
 
 	/* Timer active again */
 	db->timer.expires = ULI526X_TIMER_WUT;
diff --git a/drivers/net/ethernet/dlink/Kconfig b/drivers/net/ethernet/dlink/Kconfig
index c543ac11ce08..f6e858d0b9d4 100644
--- a/drivers/net/ethernet/dlink/Kconfig
+++ b/drivers/net/ethernet/dlink/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_DLINK
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/dlink/dl2k.c b/drivers/net/ethernet/dlink/dl2k.c
index 1274b6fdac8a..cf0a5fcdaaaf 100644
--- a/drivers/net/ethernet/dlink/dl2k.c
+++ b/drivers/net/ethernet/dlink/dl2k.c
@@ -463,10 +463,8 @@ rio_open (struct net_device *dev)
 		dw32(MACCtrl, dr32(MACCtrl) | AutoVLANuntagging);
 	}
 
-	init_timer (&np->timer);
+	setup_timer(&np->timer, rio_timer, (unsigned long)dev);
 	np->timer.expires = jiffies + 1*HZ;
-	np->timer.data = (unsigned long) dev;
-	np->timer.function = rio_timer;
 	add_timer (&np->timer);
 
 	/* Start Tx/Rx */
diff --git a/drivers/net/ethernet/emulex/Kconfig b/drivers/net/ethernet/emulex/Kconfig
index 1b8d638c6cb1..fdbb27ceb02f 100644
--- a/drivers/net/ethernet/emulex/Kconfig
+++ b/drivers/net/ethernet/emulex/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_EMULEX
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/emulex/benet/Kconfig b/drivers/net/ethernet/emulex/benet/Kconfig
index ea94a8eb6b35..7108563260ae 100644
--- a/drivers/net/ethernet/emulex/benet/Kconfig
+++ b/drivers/net/ethernet/emulex/benet/Kconfig
@@ -5,6 +5,15 @@ config BE2NET
 	  This driver implements the NIC functionality for ServerEngines'
 	  10Gbps network adapter - BladeEngine.
 
+config BE2NET_HWMON
+	bool "HWMON support for be2net driver"
+	depends on BE2NET && HWMON
+	depends on !(BE2NET=y && HWMON=m)
+	default y
+	---help---
+	  Say Y here if you want to expose thermal sensor data on
+	  be2net network adapter.
+
 config BE2NET_VXLAN
         bool "VXLAN offload support on be2net driver"
         default y
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 1bf1cdce74ac..8d12b41b3b19 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2014 Emulex
+ * Copyright (C) 2005 - 2015 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -31,11 +31,13 @@
 #include <linux/slab.h>
 #include <linux/u64_stats_sync.h>
 #include <linux/cpumask.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
 
 #include "be_hw.h"
 #include "be_roce.h"
 
-#define DRV_VER			"10.6.0.1"
+#define DRV_VER			"10.6.0.2"
 #define DRV_NAME		"be2net"
 #define BE_NAME			"Emulex BladeEngine2"
 #define BE3_NAME		"Emulex BladeEngine3"
@@ -314,7 +316,6 @@ struct be_rx_obj {
 } ____cacheline_aligned_in_smp;
 
 struct be_drv_stats {
-	u32 be_on_die_temperature;
 	u32 eth_red_drops;
 	u32 dma_map_errors;
 	u32 rx_drops_no_pbuf;
@@ -366,6 +367,7 @@ struct be_vf_cfg {
 	u32 tx_rate;
 	u32 plink_tracking;
 	u32 privileges;
+	bool spoofchk;
 };
 
 enum vf_state {
@@ -382,6 +384,7 @@ enum vf_state {
 #define BE_FLAGS_SETUP_DONE			BIT(9)
 #define BE_FLAGS_EVT_INCOMPATIBLE_SFP		BIT(10)
 #define BE_FLAGS_ERR_DETECTION_SCHEDULED	BIT(11)
+#define BE_FLAGS_OS2BMC				BIT(12)
 
 #define BE_UC_PMAC_COUNT			30
 #define BE_VF_UC_PMAC_COUNT			2
@@ -426,6 +429,8 @@ struct be_resources {
 	u32 vf_if_cap_flags;	/* VF if capability flags */
 };
 
+#define be_is_os2bmc_enabled(adapter) (adapter->flags & BE_FLAGS_OS2BMC)
+
 struct rss_info {
 	u64 rss_flags;
 	u8 rsstable[RSS_INDIR_TABLE_LEN];
@@ -433,6 +438,12 @@ struct rss_info {
 	u8 rss_hkey[RSS_HASH_KEY_LEN];
 };
 
+#define BE_INVALID_DIE_TEMP	0xFF
+struct be_hwmon {
+	struct device *hwmon_dev;
+	u8 be_on_die_temp;  /* Unit: millidegree Celsius */
+};
+
 /* Macros to read/write the 'features' word of be_wrb_params structure.
  */
 #define	BE_WRB_F_BIT(name)			BE_WRB_F_##name##_BIT
@@ -453,7 +464,8 @@ enum {
 	BE_WRB_F_LSO_BIT,		/* LSO */
 	BE_WRB_F_LSO6_BIT,		/* LSO6 */
 	BE_WRB_F_VLAN_BIT,		/* VLAN */
-	BE_WRB_F_VLAN_SKIP_HW_BIT	/* Skip VLAN tag (workaround) */
+	BE_WRB_F_VLAN_SKIP_HW_BIT,	/* Skip VLAN tag (workaround) */
+	BE_WRB_F_OS2BMC_BIT		/* Send packet to the management ring */
 };
 
 /* The structure below provides a HW-agnostic abstraction of WRB params
@@ -514,6 +526,7 @@ struct be_adapter {
 	u16 work_counter;
 
 	struct delayed_work be_err_detection_work;
+	u8 err_flags;
 	u32 flags;
 	u32 cmd_privileges;
 	/* Ethtool knobs and info */
@@ -572,8 +585,11 @@ struct be_adapter {
 	u16 qnq_vid;
 	u32 msg_enable;
 	int be_get_temp_freq;
+	struct be_hwmon hwmon_info;
 	u8 pf_number;
 	struct rss_info rss_info;
+	/* Filters for packets that need to be sent to BMC */
+	u32 bmc_filt_mask;
 };
 
 #define be_physfn(adapter)		(!adapter->virtfn)
@@ -772,26 +788,36 @@ static inline bool is_ipv4_pkt(struct sk_buff *skb)
 	return skb->protocol == htons(ETH_P_IP) && ip_hdr(skb)->version == 4;
 }
 
-static inline bool be_multi_rxq(const struct be_adapter *adapter)
+#define BE_ERROR_EEH		1
+#define BE_ERROR_UE		BIT(1)
+#define BE_ERROR_FW		BIT(2)
+#define BE_ERROR_HW		(BE_ERROR_EEH | BE_ERROR_UE)
+#define BE_ERROR_ANY		(BE_ERROR_EEH | BE_ERROR_UE | BE_ERROR_FW)
+#define BE_CLEAR_ALL		0xFF
+
+static inline u8 be_check_error(struct be_adapter *adapter, u32 err_type)
 {
-	return adapter->num_rx_qs > 1;
+	return (adapter->err_flags & err_type);
 }
 
-static inline bool be_error(struct be_adapter *adapter)
+static inline void be_set_error(struct be_adapter *adapter, int err_type)
 {
-	return adapter->eeh_error || adapter->hw_error || adapter->fw_timeout;
+	struct net_device *netdev = adapter->netdev;
+
+	adapter->err_flags |= err_type;
+	netif_carrier_off(netdev);
+
+	dev_info(&adapter->pdev->dev, "%s: Link down\n", netdev->name);
 }
 
-static inline bool be_hw_error(struct be_adapter *adapter)
+static inline void  be_clear_error(struct be_adapter *adapter, int err_type)
 {
-	return adapter->eeh_error || adapter->hw_error;
+	adapter->err_flags &= ~err_type;
 }
 
-static inline void  be_clear_all_error(struct be_adapter *adapter)
+static inline bool be_multi_rxq(const struct be_adapter *adapter)
 {
-	adapter->eeh_error = false;
-	adapter->hw_error = false;
-	adapter->fw_timeout = false;
+	return adapter->num_rx_qs > 1;
 }
 
 void be_cq_notify(struct be_adapter *adapter, u16 qid, bool arm,
@@ -804,6 +830,7 @@ bool be_pause_supported(struct be_adapter *adapter);
 u32 be_get_fw_log_level(struct be_adapter *adapter);
 int be_update_queues(struct be_adapter *adapter);
 int be_poll(struct napi_struct *napi, int budget);
+void be_eqd_update(struct be_adapter *adapter, bool force_update);
 
 /*
  * internal function to initialize-cleanup roce device.
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index fb140faeafb1..9eac3227d2ca 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2014 Emulex
+ * Copyright (C) 2005 - 2015 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -93,7 +93,7 @@ static void be_mcc_notify(struct be_adapter *adapter)
 	struct be_queue_info *mccq = &adapter->mcc_obj.q;
 	u32 val = 0;
 
-	if (be_error(adapter))
+	if (be_check_error(adapter, BE_ERROR_ANY))
 		return;
 
 	val |= mccq->id & DB_MCCQ_RING_ID_MASK;
@@ -140,6 +140,7 @@ static bool be_skip_err_log(u8 opcode, u16 base_status, u16 addl_status)
 	if (base_status == MCC_STATUS_NOT_SUPPORTED ||
 	    base_status == MCC_STATUS_ILLEGAL_REQUEST ||
 	    addl_status == MCC_ADDL_STATUS_TOO_MANY_INTERFACES ||
+	    addl_status == MCC_ADDL_STATUS_INSUFFICIENT_VLANS ||
 	    (opcode == OPCODE_COMMON_WRITE_FLASHROM &&
 	    (base_status == MCC_STATUS_ILLEGAL_FIELD ||
 	     addl_status == MCC_ADDL_STATUS_FLASH_IMAGE_CRC_MISMATCH)))
@@ -191,10 +192,12 @@ static void be_async_cmd_process(struct be_adapter *adapter,
 		if (base_status == MCC_STATUS_SUCCESS) {
 			struct be_cmd_resp_get_cntl_addnl_attribs *resp =
 							(void *)resp_hdr;
-			adapter->drv_stats.be_on_die_temperature =
+			adapter->hwmon_info.be_on_die_temp =
 						resp->on_die_temperature;
 		} else {
 			adapter->be_get_temp_freq = 0;
+			adapter->hwmon_info.be_on_die_temp =
+						BE_INVALID_DIE_TEMP;
 		}
 		return;
 	}
@@ -330,6 +333,21 @@ static void be_async_grp5_pvid_state_process(struct be_adapter *adapter,
 	}
 }
 
+#define MGMT_ENABLE_MASK	0x4
+static void be_async_grp5_fw_control_process(struct be_adapter *adapter,
+					     struct be_mcc_compl *compl)
+{
+	struct be_async_fw_control *evt = (struct be_async_fw_control *)compl;
+	u32 evt_dw1 = le32_to_cpu(evt->event_data_word1);
+
+	if (evt_dw1 & MGMT_ENABLE_MASK) {
+		adapter->flags |= BE_FLAGS_OS2BMC;
+		adapter->bmc_filt_mask = le32_to_cpu(evt->event_data_word2);
+	} else {
+		adapter->flags &= ~BE_FLAGS_OS2BMC;
+	}
+}
+
 static void be_async_grp5_evt_process(struct be_adapter *adapter,
 				      struct be_mcc_compl *compl)
 {
@@ -346,6 +364,10 @@ static void be_async_grp5_evt_process(struct be_adapter *adapter,
 	case ASYNC_EVENT_PVID_STATE:
 		be_async_grp5_pvid_state_process(adapter, compl);
 		break;
+	/* Async event to disable/enable os2bmc and/or mac-learning */
+	case ASYNC_EVENT_FW_CONTROL:
+		be_async_grp5_fw_control_process(adapter, compl);
+		break;
 	default:
 		break;
 	}
@@ -486,7 +508,7 @@ static int be_mcc_wait_compl(struct be_adapter *adapter)
 	struct be_mcc_obj *mcc_obj = &adapter->mcc_obj;
 
 	for (i = 0; i < mcc_timeout; i++) {
-		if (be_error(adapter))
+		if (be_check_error(adapter, BE_ERROR_ANY))
 			return -EIO;
 
 		local_bh_disable();
@@ -499,7 +521,7 @@ static int be_mcc_wait_compl(struct be_adapter *adapter)
 	}
 	if (i == mcc_timeout) {
 		dev_err(&adapter->pdev->dev, "FW not responding\n");
-		adapter->fw_timeout = true;
+		be_set_error(adapter, BE_ERROR_FW);
 		return -EIO;
 	}
 	return status;
@@ -538,7 +560,7 @@ static int be_mbox_db_ready_wait(struct be_adapter *adapter, void __iomem *db)
 	u32 ready;
 
 	do {
-		if (be_error(adapter))
+		if (be_check_error(adapter, BE_ERROR_ANY))
 			return -EIO;
 
 		ready = ioread32(db);
@@ -551,7 +573,7 @@ static int be_mbox_db_ready_wait(struct be_adapter *adapter, void __iomem *db)
 
 		if (msecs > 4000) {
 			dev_err(&adapter->pdev->dev, "FW not responding\n");
-			adapter->fw_timeout = true;
+			be_set_error(adapter, BE_ERROR_FW);
 			be_detect_error(adapter);
 			return -1;
 		}
@@ -1457,7 +1479,7 @@ int be_cmd_if_create(struct be_adapter *adapter, u32 cap_flags, u32 en_flags,
 		*if_handle = le32_to_cpu(resp->interface_id);
 
 		/* Hack to retrieve VF's pmac-id on BE3 */
-		if (BE3_chip(adapter) && !be_physfn(adapter))
+		if (BE3_chip(adapter) && be_virtfn(adapter))
 			adapter->pmac_id[0] = le32_to_cpu(resp->pmac_id);
 	}
 	return status;
@@ -1720,9 +1742,9 @@ int be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf)
 	total_size = buf_len;
 
 	get_fat_cmd.size = sizeof(struct be_cmd_req_get_fat) + 60*1024;
-	get_fat_cmd.va = pci_alloc_consistent(adapter->pdev,
-					      get_fat_cmd.size,
-					      &get_fat_cmd.dma);
+	get_fat_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev,
+					     get_fat_cmd.size,
+					     &get_fat_cmd.dma, GFP_ATOMIC);
 	if (!get_fat_cmd.va) {
 		dev_err(&adapter->pdev->dev,
 			"Memory allocation failure while reading FAT data\n");
@@ -1767,8 +1789,8 @@ int be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf)
 		log_offset += buf_size;
 	}
 err:
-	pci_free_consistent(adapter->pdev, get_fat_cmd.size,
-			    get_fat_cmd.va, get_fat_cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, get_fat_cmd.size,
+			  get_fat_cmd.va, get_fat_cmd.dma);
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
@@ -2215,12 +2237,12 @@ int be_cmd_read_port_transceiver_data(struct be_adapter *adapter,
 		return -EINVAL;
 
 	cmd.size = sizeof(struct be_cmd_resp_port_type);
-	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size, &cmd.dma);
+	cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
+				     GFP_ATOMIC);
 	if (!cmd.va) {
 		dev_err(&adapter->pdev->dev, "Memory allocation failed\n");
 		return -ENOMEM;
 	}
-	memset(cmd.va, 0, cmd.size);
 
 	spin_lock_bh(&adapter->mcc_lock);
 
@@ -2245,7 +2267,7 @@ int be_cmd_read_port_transceiver_data(struct be_adapter *adapter,
 	}
 err:
 	spin_unlock_bh(&adapter->mcc_lock);
-	pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va, cmd.dma);
 	return status;
 }
 
@@ -2720,7 +2742,8 @@ int be_cmd_get_phy_info(struct be_adapter *adapter)
 		goto err;
 	}
 	cmd.size = sizeof(struct be_cmd_req_get_phy_info);
-	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size, &cmd.dma);
+	cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
+				     GFP_ATOMIC);
 	if (!cmd.va) {
 		dev_err(&adapter->pdev->dev, "Memory alloc failure\n");
 		status = -ENOMEM;
@@ -2754,7 +2777,7 @@ int be_cmd_get_phy_info(struct be_adapter *adapter)
 				BE_SUPPORTED_SPEED_1GBPS;
 		}
 	}
-	pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va, cmd.dma);
 err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
@@ -2805,8 +2828,9 @@ int be_cmd_get_cntl_attributes(struct be_adapter *adapter)
 
 	memset(&attribs_cmd, 0, sizeof(struct be_dma_mem));
 	attribs_cmd.size = sizeof(struct be_cmd_resp_cntl_attribs);
-	attribs_cmd.va = pci_alloc_consistent(adapter->pdev, attribs_cmd.size,
-					      &attribs_cmd.dma);
+	attribs_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev,
+					     attribs_cmd.size,
+					     &attribs_cmd.dma, GFP_ATOMIC);
 	if (!attribs_cmd.va) {
 		dev_err(&adapter->pdev->dev, "Memory allocation failure\n");
 		status = -ENOMEM;
@@ -2833,8 +2857,8 @@ int be_cmd_get_cntl_attributes(struct be_adapter *adapter)
 err:
 	mutex_unlock(&adapter->mbox_lock);
 	if (attribs_cmd.va)
-		pci_free_consistent(adapter->pdev, attribs_cmd.size,
-				    attribs_cmd.va, attribs_cmd.dma);
+		dma_free_coherent(&adapter->pdev->dev, attribs_cmd.size,
+				  attribs_cmd.va, attribs_cmd.dma);
 	return status;
 }
 
@@ -2972,9 +2996,10 @@ int be_cmd_get_mac_from_list(struct be_adapter *adapter, u8 *mac,
 
 	memset(&get_mac_list_cmd, 0, sizeof(struct be_dma_mem));
 	get_mac_list_cmd.size = sizeof(struct be_cmd_resp_get_mac_list);
-	get_mac_list_cmd.va = pci_alloc_consistent(adapter->pdev,
-						   get_mac_list_cmd.size,
-						   &get_mac_list_cmd.dma);
+	get_mac_list_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev,
+						  get_mac_list_cmd.size,
+						  &get_mac_list_cmd.dma,
+						  GFP_ATOMIC);
 
 	if (!get_mac_list_cmd.va) {
 		dev_err(&adapter->pdev->dev,
@@ -3047,8 +3072,8 @@ int be_cmd_get_mac_from_list(struct be_adapter *adapter, u8 *mac,
 
 out:
 	spin_unlock_bh(&adapter->mcc_lock);
-	pci_free_consistent(adapter->pdev, get_mac_list_cmd.size,
-			    get_mac_list_cmd.va, get_mac_list_cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, get_mac_list_cmd.size,
+			  get_mac_list_cmd.va, get_mac_list_cmd.dma);
 	return status;
 }
 
@@ -3101,8 +3126,8 @@ int be_cmd_set_mac_list(struct be_adapter *adapter, u8 *mac_array,
 
 	memset(&cmd, 0, sizeof(struct be_dma_mem));
 	cmd.size = sizeof(struct be_cmd_req_set_mac_list);
-	cmd.va = dma_alloc_coherent(&adapter->pdev->dev, cmd.size,
-				    &cmd.dma, GFP_KERNEL);
+	cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
+				     GFP_KERNEL);
 	if (!cmd.va)
 		return -ENOMEM;
 
@@ -3153,7 +3178,7 @@ int be_cmd_set_mac(struct be_adapter *adapter, u8 *mac, int if_id, u32 dom)
 }
 
 int be_cmd_set_hsw_config(struct be_adapter *adapter, u16 pvid,
-			  u32 domain, u16 intf_id, u16 hsw_mode)
+			  u32 domain, u16 intf_id, u16 hsw_mode, u8 spoofchk)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_set_hsw_config *req;
@@ -3189,6 +3214,14 @@ int be_cmd_set_hsw_config(struct be_adapter *adapter, u16 pvid,
 			      ctxt, hsw_mode);
 	}
 
+	/* Enable/disable both mac and vlan spoof checking */
+	if (!BEx_chip(adapter) && spoofchk) {
+		AMAP_SET_BITS(struct amap_set_hsw_context, mac_spoofchk,
+			      ctxt, spoofchk);
+		AMAP_SET_BITS(struct amap_set_hsw_context, vlan_spoofchk,
+			      ctxt, spoofchk);
+	}
+
 	be_dws_cpu_to_le(req->context, sizeof(req->context));
 	status = be_mcc_notify_wait(adapter);
 
@@ -3199,7 +3232,7 @@ err:
 
 /* Get Hyper switch config */
 int be_cmd_get_hsw_config(struct be_adapter *adapter, u16 *pvid,
-			  u32 domain, u16 intf_id, u8 *mode)
+			  u32 domain, u16 intf_id, u8 *mode, bool *spoofchk)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_get_hsw_config *req;
@@ -3247,6 +3280,10 @@ int be_cmd_get_hsw_config(struct be_adapter *adapter, u16 *pvid,
 		if (mode)
 			*mode = AMAP_GET_BITS(struct amap_get_hsw_resp_context,
 					      port_fwd_type, &resp->context);
+		if (spoofchk)
+			*spoofchk =
+				AMAP_GET_BITS(struct amap_get_hsw_resp_context,
+					      spoofchk, &resp->context);
 	}
 
 err:
@@ -3258,7 +3295,7 @@ static bool be_is_wol_excluded(struct be_adapter *adapter)
 {
 	struct pci_dev *pdev = adapter->pdev;
 
-	if (!be_physfn(adapter))
+	if (be_virtfn(adapter))
 		return true;
 
 	switch (pdev->subsystem_device) {
@@ -3291,7 +3328,8 @@ int be_cmd_get_acpi_wol_cap(struct be_adapter *adapter)
 
 	memset(&cmd, 0, sizeof(struct be_dma_mem));
 	cmd.size = sizeof(struct be_cmd_resp_acpi_wol_magic_config_v1);
-	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size, &cmd.dma);
+	cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
+				     GFP_ATOMIC);
 	if (!cmd.va) {
 		dev_err(&adapter->pdev->dev, "Memory allocation failure\n");
 		status = -ENOMEM;
@@ -3326,7 +3364,8 @@ int be_cmd_get_acpi_wol_cap(struct be_adapter *adapter)
 err:
 	mutex_unlock(&adapter->mbox_lock);
 	if (cmd.va)
-		pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+		dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va,
+				  cmd.dma);
 	return status;
 
 }
@@ -3340,8 +3379,9 @@ int be_cmd_set_fw_log_level(struct be_adapter *adapter, u32 level)
 
 	memset(&extfat_cmd, 0, sizeof(struct be_dma_mem));
 	extfat_cmd.size = sizeof(struct be_cmd_resp_get_ext_fat_caps);
-	extfat_cmd.va = pci_alloc_consistent(adapter->pdev, extfat_cmd.size,
-					     &extfat_cmd.dma);
+	extfat_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev,
+					    extfat_cmd.size, &extfat_cmd.dma,
+					    GFP_ATOMIC);
 	if (!extfat_cmd.va)
 		return -ENOMEM;
 
@@ -3363,8 +3403,8 @@ int be_cmd_set_fw_log_level(struct be_adapter *adapter, u32 level)
 
 	status = be_cmd_set_ext_fat_capabilites(adapter, &extfat_cmd, cfgs);
 err:
-	pci_free_consistent(adapter->pdev, extfat_cmd.size, extfat_cmd.va,
-			    extfat_cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, extfat_cmd.size, extfat_cmd.va,
+			  extfat_cmd.dma);
 	return status;
 }
 
@@ -3377,8 +3417,9 @@ int be_cmd_get_fw_log_level(struct be_adapter *adapter)
 
 	memset(&extfat_cmd, 0, sizeof(struct be_dma_mem));
 	extfat_cmd.size = sizeof(struct be_cmd_resp_get_ext_fat_caps);
-	extfat_cmd.va = pci_alloc_consistent(adapter->pdev, extfat_cmd.size,
-					     &extfat_cmd.dma);
+	extfat_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev,
+					    extfat_cmd.size, &extfat_cmd.dma,
+					    GFP_ATOMIC);
 
 	if (!extfat_cmd.va) {
 		dev_err(&adapter->pdev->dev, "%s: Memory allocation failure\n",
@@ -3396,8 +3437,8 @@ int be_cmd_get_fw_log_level(struct be_adapter *adapter)
 				level = cfgs->module[0].trace_lvl[j].dbg_lvl;
 		}
 	}
-	pci_free_consistent(adapter->pdev, extfat_cmd.size, extfat_cmd.va,
-			    extfat_cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, extfat_cmd.size, extfat_cmd.va,
+			  extfat_cmd.dma);
 err:
 	return level;
 }
@@ -3595,7 +3636,8 @@ int be_cmd_get_func_config(struct be_adapter *adapter, struct be_resources *res)
 
 	memset(&cmd, 0, sizeof(struct be_dma_mem));
 	cmd.size = sizeof(struct be_cmd_resp_get_func_config);
-	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size, &cmd.dma);
+	cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
+				     GFP_ATOMIC);
 	if (!cmd.va) {
 		dev_err(&adapter->pdev->dev, "Memory alloc failure\n");
 		status = -ENOMEM;
@@ -3635,7 +3677,8 @@ int be_cmd_get_func_config(struct be_adapter *adapter, struct be_resources *res)
 err:
 	mutex_unlock(&adapter->mbox_lock);
 	if (cmd.va)
-		pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+		dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va,
+				  cmd.dma);
 	return status;
 }
 
@@ -3656,7 +3699,8 @@ int be_cmd_get_profile_config(struct be_adapter *adapter,
 
 	memset(&cmd, 0, sizeof(struct be_dma_mem));
 	cmd.size = sizeof(struct be_cmd_resp_get_profile_config);
-	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size, &cmd.dma);
+	cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
+				     GFP_ATOMIC);
 	if (!cmd.va)
 		return -ENOMEM;
 
@@ -3702,7 +3746,8 @@ int be_cmd_get_profile_config(struct be_adapter *adapter,
 		res->vf_if_cap_flags = vf_res->cap_flags;
 err:
 	if (cmd.va)
-		pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+		dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va,
+				  cmd.dma);
 	return status;
 }
 
@@ -3717,7 +3762,8 @@ static int be_cmd_set_profile_config(struct be_adapter *adapter, void *desc,
 
 	memset(&cmd, 0, sizeof(struct be_dma_mem));
 	cmd.size = sizeof(struct be_cmd_req_set_profile_config);
-	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size, &cmd.dma);
+	cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, cmd.size, &cmd.dma,
+				     GFP_ATOMIC);
 	if (!cmd.va)
 		return -ENOMEM;
 
@@ -3733,7 +3779,8 @@ static int be_cmd_set_profile_config(struct be_adapter *adapter, void *desc,
 	status = be_cmd_notify_wait(adapter, &wrb);
 
 	if (cmd.va)
-		pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+		dma_free_coherent(&adapter->pdev->dev, cmd.size, cmd.va,
+				  cmd.dma);
 	return status;
 }
 
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index 1ec22300e254..2716e6f30d9a 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2014 Emulex
+ * Copyright (C) 2005 - 2015 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -65,7 +65,8 @@ enum mcc_base_status {
 enum mcc_addl_status {
 	MCC_ADDL_STATUS_INSUFFICIENT_RESOURCES = 0x16,
 	MCC_ADDL_STATUS_FLASH_IMAGE_CRC_MISMATCH = 0x4d,
-	MCC_ADDL_STATUS_TOO_MANY_INTERFACES = 0x4a
+	MCC_ADDL_STATUS_TOO_MANY_INTERFACES = 0x4a,
+	MCC_ADDL_STATUS_INSUFFICIENT_VLANS = 0xab
 };
 
 #define CQE_BASE_STATUS_MASK		0xFFFF
@@ -104,6 +105,7 @@ struct be_mcc_compl {
 #define ASYNC_DEBUG_EVENT_TYPE_QNQ	1
 #define ASYNC_EVENT_CODE_SLIPORT	0x11
 #define ASYNC_EVENT_PORT_MISCONFIG	0x9
+#define ASYNC_EVENT_FW_CONTROL		0x5
 
 enum {
 	LINK_DOWN	= 0x0,
@@ -180,6 +182,22 @@ struct be_async_event_misconfig_port {
 	u32 flags;
 } __packed;
 
+#define BMC_FILT_BROADCAST_ARP				BIT(0)
+#define BMC_FILT_BROADCAST_DHCP_CLIENT			BIT(1)
+#define BMC_FILT_BROADCAST_DHCP_SERVER			BIT(2)
+#define BMC_FILT_BROADCAST_NET_BIOS			BIT(3)
+#define BMC_FILT_BROADCAST				BIT(7)
+#define BMC_FILT_MULTICAST_IPV6_NEIGH_ADVER		BIT(8)
+#define BMC_FILT_MULTICAST_IPV6_RA			BIT(9)
+#define BMC_FILT_MULTICAST_IPV6_RAS			BIT(10)
+#define BMC_FILT_MULTICAST				BIT(15)
+struct be_async_fw_control {
+	u32 event_data_word1;
+	u32 event_data_word2;
+	u32 evt_tag;
+	u32 event_data_word4;
+} __packed;
+
 struct be_mcc_mailbox {
 	struct be_mcc_wrb wrb;
 	struct be_mcc_compl compl;
@@ -1109,10 +1127,6 @@ struct be_cmd_req_query_fw_cfg {
 	u32 rsvd[31];
 };
 
-/* ASIC revisions */
-#define ASIC_REV_B0		0x10
-#define ASIC_REV_P2		0x11
-
 struct be_cmd_resp_query_fw_cfg {
 	struct be_cmd_resp_hdr hdr;
 	u32 be_config_number;
@@ -1745,18 +1759,24 @@ struct be_cmd_req_set_mac_list {
 #define PORT_FWD_TYPE_VEPA		0x3
 #define PORT_FWD_TYPE_VEB		0x2
 
+#define ENABLE_MAC_SPOOFCHK		0x2
+#define DISABLE_MAC_SPOOFCHK		0x3
+
 struct amap_set_hsw_context {
 	u8 interface_id[16];
-	u8 rsvd0[14];
+	u8 rsvd0[8];
+	u8 mac_spoofchk[2];
+	u8 rsvd1[4];
 	u8 pvid_valid;
 	u8 pport;
-	u8 rsvd1[6];
+	u8 rsvd2[6];
 	u8 port_fwd_type[3];
-	u8 rsvd2[7];
+	u8 rsvd3[5];
+	u8 vlan_spoofchk[2];
 	u8 pvid[16];
-	u8 rsvd3[32];
 	u8 rsvd4[32];
 	u8 rsvd5[32];
+	u8 rsvd6[32];
 } __packed;
 
 struct be_cmd_req_set_hsw_config {
@@ -1774,11 +1794,13 @@ struct amap_get_hsw_req_context {
 struct amap_get_hsw_resp_context {
 	u8 rsvd0[6];
 	u8 port_fwd_type[3];
-	u8 rsvd1[7];
+	u8 rsvd1[5];
+	u8 spoofchk;
+	u8 rsvd2;
 	u8 pvid[16];
-	u8 rsvd2[32];
 	u8 rsvd3[32];
 	u8 rsvd4[32];
+	u8 rsvd5[32];
 } __packed;
 
 struct be_cmd_req_get_hsw_config {
@@ -2334,9 +2356,9 @@ int be_cmd_set_mac_list(struct be_adapter *adapter, u8 *mac_array, u8 mac_count,
 			u32 domain);
 int be_cmd_set_mac(struct be_adapter *adapter, u8 *mac, int if_id, u32 dom);
 int be_cmd_set_hsw_config(struct be_adapter *adapter, u16 pvid, u32 domain,
-			  u16 intf_id, u16 hsw_mode);
+			  u16 intf_id, u16 hsw_mode, u8 spoofchk);
 int be_cmd_get_hsw_config(struct be_adapter *adapter, u16 *pvid, u32 domain,
-			  u16 intf_id, u8 *mode);
+			  u16 intf_id, u8 *mode, bool *spoofchk);
 int be_cmd_get_acpi_wol_cap(struct be_adapter *adapter);
 int be_cmd_set_fw_log_level(struct be_adapter *adapter, u32 level);
 int be_cmd_get_fw_log_level(struct be_adapter *adapter);
diff --git a/drivers/net/ethernet/emulex/benet/be_ethtool.c b/drivers/net/ethernet/emulex/benet/be_ethtool.c
index b765c24625bf..b2476dbfd103 100644
--- a/drivers/net/ethernet/emulex/benet/be_ethtool.c
+++ b/drivers/net/ethernet/emulex/benet/be_ethtool.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2014 Emulex
+ * Copyright (C) 2005 - 2015 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -123,7 +123,6 @@ static const struct be_ethtool_stat et_stats[] = {
 	{DRVSTAT_INFO(dma_map_errors)},
 	/* Number of packets dropped due to random early drop function */
 	{DRVSTAT_INFO(eth_red_drops)},
-	{DRVSTAT_INFO(be_on_die_temperature)},
 	{DRVSTAT_INFO(rx_roce_bytes_lsd)},
 	{DRVSTAT_INFO(rx_roce_bytes_msd)},
 	{DRVSTAT_INFO(rx_roce_frames)},
@@ -264,8 +263,8 @@ static int lancer_cmd_read_file(struct be_adapter *adapter, u8 *file_name,
 	int status = 0;
 
 	read_cmd.size = LANCER_READ_FILE_CHUNK;
-	read_cmd.va = pci_alloc_consistent(adapter->pdev, read_cmd.size,
-					   &read_cmd.dma);
+	read_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev, read_cmd.size,
+					  &read_cmd.dma, GFP_ATOMIC);
 
 	if (!read_cmd.va) {
 		dev_err(&adapter->pdev->dev,
@@ -289,8 +288,8 @@ static int lancer_cmd_read_file(struct be_adapter *adapter, u8 *file_name,
 			break;
 		}
 	}
-	pci_free_consistent(adapter->pdev, read_cmd.size, read_cmd.va,
-			    read_cmd.dma);
+	dma_free_coherent(&adapter->pdev->dev, read_cmd.size, read_cmd.va,
+			  read_cmd.dma);
 
 	return status;
 }
@@ -368,6 +367,14 @@ static int be_set_coalesce(struct net_device *netdev,
 		aic++;
 	}
 
+	/* For Skyhawk, the EQD setting happens via EQ_DB when AIC is enabled.
+	 * When AIC is disabled, persistently force set EQD value via the
+	 * FW cmd, so that we don't have to calculate the delay multiplier
+	 * encode value each time EQ_DB is rung
+	 */
+	if (!et->use_adaptive_rx_coalesce && skyhawk_chip(adapter))
+		be_eqd_update(adapter, true);
+
 	return 0;
 }
 
@@ -818,8 +825,9 @@ static int be_test_ddr_dma(struct be_adapter *adapter)
 	};
 
 	ddrdma_cmd.size = sizeof(struct be_cmd_req_ddrdma_test);
-	ddrdma_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, ddrdma_cmd.size,
-					   &ddrdma_cmd.dma, GFP_KERNEL);
+	ddrdma_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev,
+					    ddrdma_cmd.size, &ddrdma_cmd.dma,
+					    GFP_KERNEL);
 	if (!ddrdma_cmd.va)
 		return -ENOMEM;
 
@@ -941,8 +949,9 @@ static int be_read_eeprom(struct net_device *netdev,
 
 	memset(&eeprom_cmd, 0, sizeof(struct be_dma_mem));
 	eeprom_cmd.size = sizeof(struct be_cmd_req_seeprom_read);
-	eeprom_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, eeprom_cmd.size,
-					   &eeprom_cmd.dma, GFP_KERNEL);
+	eeprom_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev,
+					    eeprom_cmd.size, &eeprom_cmd.dma,
+					    GFP_KERNEL);
 
 	if (!eeprom_cmd.va)
 		return -ENOMEM;
diff --git a/drivers/net/ethernet/emulex/benet/be_hw.h b/drivers/net/ethernet/emulex/benet/be_hw.h
index 48840889db62..c684bb32b487 100644
--- a/drivers/net/ethernet/emulex/benet/be_hw.h
+++ b/drivers/net/ethernet/emulex/benet/be_hw.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2014 Emulex
+ * Copyright (C) 2005 - 2015 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -132,6 +132,18 @@
 #define DB_EQ_NUM_POPPED_SHIFT		(16)	/* bits 16 - 28 */
 /* Rearm bit */
 #define DB_EQ_REARM_SHIFT		(29)	/* bit 29 */
+/* Rearm to interrupt delay encoding */
+#define DB_EQ_R2I_DLY_SHIFT		(30)    /* bits 30 - 31 */
+
+/* Rearm to interrupt (R2I) delay multiplier encoding represents 3 different
+ * values configured in CEV_REARM2IRPT_DLY_MULT_CSR register. This value is
+ * programmed by host driver while ringing an EQ doorbell(EQ_DB) if a delay
+ * between rearming the EQ and next interrupt on this EQ is desired.
+ */
+#define	R2I_DLY_ENC_0			0	/* No delay */
+#define	R2I_DLY_ENC_1			1	/* maps to 160us EQ delay */
+#define	R2I_DLY_ENC_2			2	/* maps to 96us EQ delay */
+#define	R2I_DLY_ENC_3			3	/* maps to 48us EQ delay */
 
 /********* Compl Q door bell *************/
 #define DB_CQ_OFFSET 			0x120
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 6f9ffb9026cd..6f642426308c 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2014 Emulex
+ * Copyright (C) 2005 - 2015 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -179,7 +179,7 @@ static void be_intr_set(struct be_adapter *adapter, bool enable)
 	if (lancer_chip(adapter))
 		return;
 
-	if (adapter->eeh_error)
+	if (be_check_error(adapter, BE_ERROR_EEH))
 		return;
 
 	status = be_cmd_intr_set(adapter, enable);
@@ -191,6 +191,9 @@ static void be_rxq_notify(struct be_adapter *adapter, u16 qid, u16 posted)
 {
 	u32 val = 0;
 
+	if (be_check_error(adapter, BE_ERROR_HW))
+		return;
+
 	val |= qid & DB_RQ_RING_ID_MASK;
 	val |= posted << DB_RQ_NUM_POSTED_SHIFT;
 
@@ -203,6 +206,9 @@ static void be_txq_notify(struct be_adapter *adapter, struct be_tx_obj *txo,
 {
 	u32 val = 0;
 
+	if (be_check_error(adapter, BE_ERROR_HW))
+		return;
+
 	val |= txo->q.id & DB_TXULP_RING_ID_MASK;
 	val |= (posted & DB_TXULP_NUM_POSTED_MASK) << DB_TXULP_NUM_POSTED_SHIFT;
 
@@ -211,14 +217,15 @@ static void be_txq_notify(struct be_adapter *adapter, struct be_tx_obj *txo,
 }
 
 static void be_eq_notify(struct be_adapter *adapter, u16 qid,
-			 bool arm, bool clear_int, u16 num_popped)
+			 bool arm, bool clear_int, u16 num_popped,
+			 u32 eq_delay_mult_enc)
 {
 	u32 val = 0;
 
 	val |= qid & DB_EQ_RING_ID_MASK;
 	val |= ((qid & DB_EQ_RING_ID_EXT_MASK) << DB_EQ_RING_ID_EXT_MASK_SHIFT);
 
-	if (adapter->eeh_error)
+	if (be_check_error(adapter, BE_ERROR_HW))
 		return;
 
 	if (arm)
@@ -227,6 +234,7 @@ static void be_eq_notify(struct be_adapter *adapter, u16 qid,
 		val |= 1 << DB_EQ_CLR_SHIFT;
 	val |= 1 << DB_EQ_EVNT_SHIFT;
 	val |= num_popped << DB_EQ_NUM_POPPED_SHIFT;
+	val |= eq_delay_mult_enc << DB_EQ_R2I_DLY_SHIFT;
 	iowrite32(val, adapter->db + DB_EQ_OFFSET);
 }
 
@@ -238,7 +246,7 @@ void be_cq_notify(struct be_adapter *adapter, u16 qid, bool arm, u16 num_popped)
 	val |= ((qid & DB_CQ_RING_ID_EXT_MASK) <<
 			DB_CQ_RING_ID_EXT_MASK_SHIFT);
 
-	if (adapter->eeh_error)
+	if (be_check_error(adapter, BE_ERROR_HW))
 		return;
 
 	if (arm)
@@ -662,6 +670,8 @@ void be_link_status_update(struct be_adapter *adapter, u8 link_status)
 		netif_carrier_on(netdev);
 	else
 		netif_carrier_off(netdev);
+
+	netdev_info(netdev, "Link is %s\n", link_status ? "Up" : "Down");
 }
 
 static void be_tx_stats_update(struct be_tx_obj *txo, struct sk_buff *skb)
@@ -810,6 +820,8 @@ static void wrb_fill_hdr(struct be_adapter *adapter,
 
 	SET_TX_WRB_HDR_BITS(num_wrb, hdr, skb_wrb_cnt(skb));
 	SET_TX_WRB_HDR_BITS(len, hdr, skb->len);
+	SET_TX_WRB_HDR_BITS(mgmt, hdr,
+			    BE_WRB_F_GET(wrb_params->features, OS2BMC));
 }
 
 static void unmap_tx_frag(struct device *dev, struct be_eth_wrb *wrb,
@@ -1146,6 +1158,130 @@ static void be_xmit_flush(struct be_adapter *adapter, struct be_tx_obj *txo)
 	txo->pend_wrb_cnt = 0;
 }
 
+/* OS2BMC related */
+
+#define DHCP_CLIENT_PORT	68
+#define DHCP_SERVER_PORT	67
+#define NET_BIOS_PORT1		137
+#define NET_BIOS_PORT2		138
+#define DHCPV6_RAS_PORT		547
+
+#define is_mc_allowed_on_bmc(adapter, eh)	\
+	(!is_multicast_filt_enabled(adapter) &&	\
+	 is_multicast_ether_addr(eh->h_dest) &&	\
+	 !is_broadcast_ether_addr(eh->h_dest))
+
+#define is_bc_allowed_on_bmc(adapter, eh)	\
+	(!is_broadcast_filt_enabled(adapter) &&	\
+	 is_broadcast_ether_addr(eh->h_dest))
+
+#define is_arp_allowed_on_bmc(adapter, skb)	\
+	(is_arp(skb) && is_arp_filt_enabled(adapter))
+
+#define is_broadcast_packet(eh, adapter)	\
+		(is_multicast_ether_addr(eh->h_dest) && \
+		!compare_ether_addr(eh->h_dest, adapter->netdev->broadcast))
+
+#define is_arp(skb)	(skb->protocol == htons(ETH_P_ARP))
+
+#define is_arp_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask & (BMC_FILT_BROADCAST_ARP))
+
+#define is_dhcp_client_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask & BMC_FILT_BROADCAST_DHCP_CLIENT)
+
+#define is_dhcp_srvr_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask & BMC_FILT_BROADCAST_DHCP_SERVER)
+
+#define is_nbios_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask & BMC_FILT_BROADCAST_NET_BIOS)
+
+#define is_ipv6_na_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask &	\
+			BMC_FILT_MULTICAST_IPV6_NEIGH_ADVER)
+
+#define is_ipv6_ra_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask & BMC_FILT_MULTICAST_IPV6_RA)
+
+#define is_ipv6_ras_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask & BMC_FILT_MULTICAST_IPV6_RAS)
+
+#define is_broadcast_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask & BMC_FILT_BROADCAST)
+
+#define is_multicast_filt_enabled(adapter)	\
+		(adapter->bmc_filt_mask & BMC_FILT_MULTICAST)
+
+static bool be_send_pkt_to_bmc(struct be_adapter *adapter,
+			       struct sk_buff **skb)
+{
+	struct ethhdr *eh = (struct ethhdr *)(*skb)->data;
+	bool os2bmc = false;
+
+	if (!be_is_os2bmc_enabled(adapter))
+		goto done;
+
+	if (!is_multicast_ether_addr(eh->h_dest))
+		goto done;
+
+	if (is_mc_allowed_on_bmc(adapter, eh) ||
+	    is_bc_allowed_on_bmc(adapter, eh) ||
+	    is_arp_allowed_on_bmc(adapter, (*skb))) {
+		os2bmc = true;
+		goto done;
+	}
+
+	if ((*skb)->protocol == htons(ETH_P_IPV6)) {
+		struct ipv6hdr *hdr = ipv6_hdr((*skb));
+		u8 nexthdr = hdr->nexthdr;
+
+		if (nexthdr == IPPROTO_ICMPV6) {
+			struct icmp6hdr *icmp6 = icmp6_hdr((*skb));
+
+			switch (icmp6->icmp6_type) {
+			case NDISC_ROUTER_ADVERTISEMENT:
+				os2bmc = is_ipv6_ra_filt_enabled(adapter);
+				goto done;
+			case NDISC_NEIGHBOUR_ADVERTISEMENT:
+				os2bmc = is_ipv6_na_filt_enabled(adapter);
+				goto done;
+			default:
+				break;
+			}
+		}
+	}
+
+	if (is_udp_pkt((*skb))) {
+		struct udphdr *udp = udp_hdr((*skb));
+
+		switch (udp->dest) {
+		case DHCP_CLIENT_PORT:
+			os2bmc = is_dhcp_client_filt_enabled(adapter);
+			goto done;
+		case DHCP_SERVER_PORT:
+			os2bmc = is_dhcp_srvr_filt_enabled(adapter);
+			goto done;
+		case NET_BIOS_PORT1:
+		case NET_BIOS_PORT2:
+			os2bmc = is_nbios_filt_enabled(adapter);
+			goto done;
+		case DHCPV6_RAS_PORT:
+			os2bmc = is_ipv6_ras_filt_enabled(adapter);
+			goto done;
+		default:
+			break;
+		}
+	}
+done:
+	/* For packets over a vlan, which are destined
+	 * to BMC, asic expects the vlan to be inline in the packet.
+	 */
+	if (os2bmc)
+		*skb = be_insert_vlan_in_pkt(adapter, *skb, NULL);
+
+	return os2bmc;
+}
+
 static netdev_tx_t be_xmit(struct sk_buff *skb, struct net_device *netdev)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
@@ -1167,6 +1303,18 @@ static netdev_tx_t be_xmit(struct sk_buff *skb, struct net_device *netdev)
 		goto drop;
 	}
 
+	/* if os2bmc is enabled and if the pkt is destined to bmc,
+	 * enqueue the pkt a 2nd time with mgmt bit set.
+	 */
+	if (be_send_pkt_to_bmc(adapter, &skb)) {
+		BE_WRB_F_SET(wrb_params.features, OS2BMC, 1);
+		wrb_cnt = be_xmit_enqueue(adapter, txo, skb, &wrb_params);
+		if (unlikely(!wrb_cnt))
+			goto drop;
+		else
+			skb_get(skb);
+	}
+
 	if (be_is_txq_full(txo)) {
 		netif_stop_subqueue(netdev, q_idx);
 		tx_stats(txo)->tx_stops++;
@@ -1265,7 +1413,8 @@ static int be_vid_config(struct be_adapter *adapter)
 	if (status) {
 		dev_err(dev, "Setting HW VLAN filtering failed\n");
 		/* Set to VLAN promisc mode as setting VLAN filter failed */
-		if (addl_status(status) ==
+		if (addl_status(status) == MCC_ADDL_STATUS_INSUFFICIENT_VLANS ||
+		    addl_status(status) ==
 				MCC_ADDL_STATUS_INSUFFICIENT_RESOURCES)
 			return be_set_vlan_promisc(adapter);
 	} else if (adapter->if_flags & BE_IF_FLAGS_VLAN_PROMISCUOUS) {
@@ -1466,6 +1615,7 @@ static int be_get_vf_config(struct net_device *netdev, int vf,
 	vi->qos = vf_cfg->vlan_tag >> VLAN_PRIO_SHIFT;
 	memcpy(&vi->mac, vf_cfg->mac_addr, ETH_ALEN);
 	vi->linkstate = adapter->vf_cfg[vf].plink_tracking;
+	vi->spoofchk = adapter->vf_cfg[vf].spoofchk;
 
 	return 0;
 }
@@ -1478,7 +1628,7 @@ static int be_set_vf_tvt(struct be_adapter *adapter, int vf, u16 vlan)
 	int status;
 
 	/* Enable Transparent VLAN Tagging */
-	status = be_cmd_set_hsw_config(adapter, vlan, vf + 1, vf_if_id, 0);
+	status = be_cmd_set_hsw_config(adapter, vlan, vf + 1, vf_if_id, 0, 0);
 	if (status)
 		return status;
 
@@ -1507,7 +1657,7 @@ static int be_clear_vf_tvt(struct be_adapter *adapter, int vf)
 
 	/* Reset Transparent VLAN Tagging. */
 	status = be_cmd_set_hsw_config(adapter, BE_RESET_VLAN_TAG_ID, vf + 1,
-				       vf_cfg->if_handle, 0);
+				       vf_cfg->if_handle, 0, 0);
 	if (status)
 		return status;
 
@@ -1642,6 +1792,39 @@ static int be_set_vf_link_state(struct net_device *netdev, int vf,
 	return 0;
 }
 
+static int be_set_vf_spoofchk(struct net_device *netdev, int vf, bool enable)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+	struct be_vf_cfg *vf_cfg = &adapter->vf_cfg[vf];
+	u8 spoofchk;
+	int status;
+
+	if (!sriov_enabled(adapter))
+		return -EPERM;
+
+	if (vf >= adapter->num_vfs)
+		return -EINVAL;
+
+	if (BEx_chip(adapter))
+		return -EOPNOTSUPP;
+
+	if (enable == vf_cfg->spoofchk)
+		return 0;
+
+	spoofchk = enable ? ENABLE_MAC_SPOOFCHK : DISABLE_MAC_SPOOFCHK;
+
+	status = be_cmd_set_hsw_config(adapter, 0, vf + 1, vf_cfg->if_handle,
+				       0, spoofchk);
+	if (status) {
+		dev_err(&adapter->pdev->dev,
+			"Spoofchk change on VF %d failed: %#x\n", vf, status);
+		return be_cmd_status(status);
+	}
+
+	vf_cfg->spoofchk = enable;
+	return 0;
+}
+
 static void be_aic_update(struct be_aic_obj *aic, u64 rx_pkts, u64 tx_pkts,
 			  ulong now)
 {
@@ -1650,61 +1833,110 @@ static void be_aic_update(struct be_aic_obj *aic, u64 rx_pkts, u64 tx_pkts,
 	aic->jiffies = now;
 }
 
-static void be_eqd_update(struct be_adapter *adapter)
+static int be_get_new_eqd(struct be_eq_obj *eqo)
 {
-	struct be_set_eqd set_eqd[MAX_EVT_QS];
-	int eqd, i, num = 0, start;
+	struct be_adapter *adapter = eqo->adapter;
+	int eqd, start;
 	struct be_aic_obj *aic;
-	struct be_eq_obj *eqo;
 	struct be_rx_obj *rxo;
 	struct be_tx_obj *txo;
-	u64 rx_pkts, tx_pkts;
+	u64 rx_pkts = 0, tx_pkts = 0;
 	ulong now;
 	u32 pps, delta;
+	int i;
 
-	for_all_evt_queues(adapter, eqo, i) {
-		aic = &adapter->aic_obj[eqo->idx];
-		if (!aic->enable) {
-			if (aic->jiffies)
-				aic->jiffies = 0;
-			eqd = aic->et_eqd;
-			goto modify_eqd;
-		}
+	aic = &adapter->aic_obj[eqo->idx];
+	if (!aic->enable) {
+		if (aic->jiffies)
+			aic->jiffies = 0;
+		eqd = aic->et_eqd;
+		return eqd;
+	}
 
-		rxo = &adapter->rx_obj[eqo->idx];
+	for_all_rx_queues_on_eq(adapter, eqo, rxo, i) {
 		do {
 			start = u64_stats_fetch_begin_irq(&rxo->stats.sync);
-			rx_pkts = rxo->stats.rx_pkts;
+			rx_pkts += rxo->stats.rx_pkts;
 		} while (u64_stats_fetch_retry_irq(&rxo->stats.sync, start));
+	}
 
-		txo = &adapter->tx_obj[eqo->idx];
+	for_all_tx_queues_on_eq(adapter, eqo, txo, i) {
 		do {
 			start = u64_stats_fetch_begin_irq(&txo->stats.sync);
-			tx_pkts = txo->stats.tx_reqs;
+			tx_pkts += txo->stats.tx_reqs;
 		} while (u64_stats_fetch_retry_irq(&txo->stats.sync, start));
+	}
 
-		/* Skip, if wrapped around or first calculation */
-		now = jiffies;
-		if (!aic->jiffies || time_before(now, aic->jiffies) ||
-		    rx_pkts < aic->rx_pkts_prev ||
-		    tx_pkts < aic->tx_reqs_prev) {
-			be_aic_update(aic, rx_pkts, tx_pkts, now);
-			continue;
-		}
+	/* Skip, if wrapped around or first calculation */
+	now = jiffies;
+	if (!aic->jiffies || time_before(now, aic->jiffies) ||
+	    rx_pkts < aic->rx_pkts_prev ||
+	    tx_pkts < aic->tx_reqs_prev) {
+		be_aic_update(aic, rx_pkts, tx_pkts, now);
+		return aic->prev_eqd;
+	}
 
-		delta = jiffies_to_msecs(now - aic->jiffies);
-		pps = (((u32)(rx_pkts - aic->rx_pkts_prev) * 1000) / delta) +
-			(((u32)(tx_pkts - aic->tx_reqs_prev) * 1000) / delta);
-		eqd = (pps / 15000) << 2;
+	delta = jiffies_to_msecs(now - aic->jiffies);
+	if (delta == 0)
+		return aic->prev_eqd;
 
-		if (eqd < 8)
-			eqd = 0;
-		eqd = min_t(u32, eqd, aic->max_eqd);
-		eqd = max_t(u32, eqd, aic->min_eqd);
+	pps = (((u32)(rx_pkts - aic->rx_pkts_prev) * 1000) / delta) +
+		(((u32)(tx_pkts - aic->tx_reqs_prev) * 1000) / delta);
+	eqd = (pps / 15000) << 2;
 
-		be_aic_update(aic, rx_pkts, tx_pkts, now);
-modify_eqd:
-		if (eqd != aic->prev_eqd) {
+	if (eqd < 8)
+		eqd = 0;
+	eqd = min_t(u32, eqd, aic->max_eqd);
+	eqd = max_t(u32, eqd, aic->min_eqd);
+
+	be_aic_update(aic, rx_pkts, tx_pkts, now);
+
+	return eqd;
+}
+
+/* For Skyhawk-R only */
+static u32 be_get_eq_delay_mult_enc(struct be_eq_obj *eqo)
+{
+	struct be_adapter *adapter = eqo->adapter;
+	struct be_aic_obj *aic = &adapter->aic_obj[eqo->idx];
+	ulong now = jiffies;
+	int eqd;
+	u32 mult_enc;
+
+	if (!aic->enable)
+		return 0;
+
+	if (time_before_eq(now, aic->jiffies) ||
+	    jiffies_to_msecs(now - aic->jiffies) < 1)
+		eqd = aic->prev_eqd;
+	else
+		eqd = be_get_new_eqd(eqo);
+
+	if (eqd > 100)
+		mult_enc = R2I_DLY_ENC_1;
+	else if (eqd > 60)
+		mult_enc = R2I_DLY_ENC_2;
+	else if (eqd > 20)
+		mult_enc = R2I_DLY_ENC_3;
+	else
+		mult_enc = R2I_DLY_ENC_0;
+
+	aic->prev_eqd = eqd;
+
+	return mult_enc;
+}
+
+void be_eqd_update(struct be_adapter *adapter, bool force_update)
+{
+	struct be_set_eqd set_eqd[MAX_EVT_QS];
+	struct be_aic_obj *aic;
+	struct be_eq_obj *eqo;
+	int i, num = 0, eqd;
+
+	for_all_evt_queues(adapter, eqo, i) {
+		aic = &adapter->aic_obj[eqo->idx];
+		eqd = be_get_new_eqd(eqo);
+		if (force_update || eqd != aic->prev_eqd) {
 			set_eqd[num].delay_multiplier = (eqd * 65)/100;
 			set_eqd[num].eq_id = eqo->q.id;
 			aic->prev_eqd = eqd;
@@ -2212,7 +2444,7 @@ static void be_eq_clean(struct be_eq_obj *eqo)
 {
 	int num = events_get(eqo);
 
-	be_eq_notify(eqo->adapter, eqo->q.id, false, true, num);
+	be_eq_notify(eqo->adapter, eqo->q.id, false, true, num, 0);
 }
 
 static void be_rx_cq_clean(struct be_rx_obj *rxo)
@@ -2236,7 +2468,9 @@ static void be_rx_cq_clean(struct be_rx_obj *rxo)
 			if (lancer_chip(adapter))
 				break;
 
-			if (flush_wait++ > 10 || be_hw_error(adapter)) {
+			if (flush_wait++ > 50 ||
+			    be_check_error(adapter,
+					   BE_ERROR_HW)) {
 				dev_warn(&adapter->pdev->dev,
 					 "did not receive flush compl\n");
 				break;
@@ -2297,7 +2531,8 @@ static void be_tx_compl_clean(struct be_adapter *adapter)
 				pending_txqs--;
 		}
 
-		if (pending_txqs == 0 || ++timeo > 10 || be_hw_error(adapter))
+		if (pending_txqs == 0 || ++timeo > 10 ||
+		    be_check_error(adapter, BE_ERROR_HW))
 			break;
 
 		mdelay(1);
@@ -2573,7 +2808,7 @@ static irqreturn_t be_intx(int irq, void *dev)
 		if (num_evts)
 			eqo->spurious_intr = 0;
 	}
-	be_eq_notify(adapter, eqo->q.id, false, true, num_evts);
+	be_eq_notify(adapter, eqo->q.id, false, true, num_evts, 0);
 
 	/* Return IRQ_HANDLED only for the the first spurious intr
 	 * after a valid intr to stop the kernel from branding
@@ -2589,7 +2824,7 @@ static irqreturn_t be_msix(int irq, void *dev)
 {
 	struct be_eq_obj *eqo = dev;
 
-	be_eq_notify(eqo->adapter, eqo->q.id, false, true, 0);
+	be_eq_notify(eqo->adapter, eqo->q.id, false, true, 0, 0);
 	napi_schedule(&eqo->napi);
 	return IRQ_HANDLED;
 }
@@ -2838,6 +3073,7 @@ int be_poll(struct napi_struct *napi, int budget)
 	int max_work = 0, work, i, num_evts;
 	struct be_rx_obj *rxo;
 	struct be_tx_obj *txo;
+	u32 mult_enc = 0;
 
 	num_evts = events_get(eqo);
 
@@ -2863,10 +3099,18 @@ int be_poll(struct napi_struct *napi, int budget)
 
 	if (max_work < budget) {
 		napi_complete(napi);
-		be_eq_notify(adapter, eqo->q.id, true, false, num_evts);
+
+		/* Skyhawk EQ_DB has a provision to set the rearm to interrupt
+		 * delay via a delay multiplier encoding value
+		 */
+		if (skyhawk_chip(adapter))
+			mult_enc = be_get_eq_delay_mult_enc(eqo);
+
+		be_eq_notify(adapter, eqo->q.id, true, false, num_evts,
+			     mult_enc);
 	} else {
 		/* As we'll continue in polling mode, count and clear events */
-		be_eq_notify(adapter, eqo->q.id, false, false, num_evts);
+		be_eq_notify(adapter, eqo->q.id, false, false, num_evts, 0);
 	}
 	return max_work;
 }
@@ -2898,22 +3142,19 @@ void be_detect_error(struct be_adapter *adapter)
 	u32 ue_lo = 0, ue_hi = 0, ue_lo_mask = 0, ue_hi_mask = 0;
 	u32 sliport_status = 0, sliport_err1 = 0, sliport_err2 = 0;
 	u32 i;
-	bool error_detected = false;
 	struct device *dev = &adapter->pdev->dev;
-	struct net_device *netdev = adapter->netdev;
 
-	if (be_hw_error(adapter))
+	if (be_check_error(adapter, BE_ERROR_HW))
 		return;
 
 	if (lancer_chip(adapter)) {
 		sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
 		if (sliport_status & SLIPORT_STATUS_ERR_MASK) {
+			be_set_error(adapter, BE_ERROR_UE);
 			sliport_err1 = ioread32(adapter->db +
 						SLIPORT_ERROR1_OFFSET);
 			sliport_err2 = ioread32(adapter->db +
 						SLIPORT_ERROR2_OFFSET);
-			adapter->hw_error = true;
-			error_detected = true;
 			/* Do not log error messages if its a FW reset */
 			if (sliport_err1 == SLIPORT_ERROR_FW_RESET1 &&
 			    sliport_err2 == SLIPORT_ERROR_FW_RESET2) {
@@ -2945,12 +3186,12 @@ void be_detect_error(struct be_adapter *adapter)
 		 */
 
 		if (ue_lo || ue_hi) {
-			error_detected = true;
 			dev_err(dev,
 				"Unrecoverable Error detected in the adapter");
 			dev_err(dev, "Please reboot server to recover");
 			if (skyhawk_chip(adapter))
-				adapter->hw_error = true;
+				be_set_error(adapter, BE_ERROR_UE);
+
 			for (i = 0; ue_lo; ue_lo >>= 1, i++) {
 				if (ue_lo & 1)
 					dev_err(dev, "UE: %s bit set\n",
@@ -2963,8 +3204,6 @@ void be_detect_error(struct be_adapter *adapter)
 			}
 		}
 	}
-	if (error_detected)
-		netif_carrier_off(netdev);
 }
 
 static void be_msix_disable(struct be_adapter *adapter)
@@ -3015,7 +3254,7 @@ fail:
 	dev_warn(dev, "MSIx enable failed\n");
 
 	/* INTx is not supported in VFs, so fail probe if enable_msix fails */
-	if (!be_physfn(adapter))
+	if (be_virtfn(adapter))
 		return num_vec;
 	return 0;
 }
@@ -3062,7 +3301,7 @@ static int be_irq_register(struct be_adapter *adapter)
 		if (status == 0)
 			goto done;
 		/* INTx is not supported for VF */
-		if (!be_physfn(adapter))
+		if (be_virtfn(adapter))
 			return status;
 	}
 
@@ -3229,9 +3468,12 @@ static int be_rx_qs_create(struct be_adapter *adapter)
 
 	memcpy(rss->rss_hkey, rss_key, RSS_HASH_KEY_LEN);
 
-	/* First time posting */
+	/* Post 1 less than RXQ-len to avoid head being equal to tail,
+	 * which is a queue empty condition
+	 */
 	for_all_rx_queues(adapter, rxo, i)
-		be_post_rx_frags(rxo, GFP_KERNEL, MAX_RX_POST);
+		be_post_rx_frags(rxo, GFP_KERNEL, RX_Q_LEN - 1);
+
 	return 0;
 }
 
@@ -3263,7 +3505,7 @@ static int be_open(struct net_device *netdev)
 	for_all_evt_queues(adapter, eqo, i) {
 		napi_enable(&eqo->napi);
 		be_enable_busy_poll(eqo);
-		be_eq_notify(adapter, eqo->q.id, true, true, 0);
+		be_eq_notify(adapter, eqo->q.id, true, true, 0, 0);
 	}
 	adapter->flags |= BE_FLAGS_NAPI_ENABLED;
 
@@ -3563,7 +3805,7 @@ static int be_vfs_if_create(struct be_adapter *adapter)
 
 	/* If a FW profile exists, then cap_flags are updated */
 	cap_flags = BE_IF_FLAGS_UNTAGGED | BE_IF_FLAGS_BROADCAST |
-		    BE_IF_FLAGS_MULTICAST;
+		    BE_IF_FLAGS_MULTICAST | BE_IF_FLAGS_PASS_L3L4_ERRORS;
 
 	for_all_vfs(adapter, vf_cfg, vf) {
 		if (!BE3_chip(adapter)) {
@@ -3610,6 +3852,7 @@ static int be_vf_setup(struct be_adapter *adapter)
 	struct device *dev = &adapter->pdev->dev;
 	struct be_vf_cfg *vf_cfg;
 	int status, old_vfs, vf;
+	bool spoofchk;
 
 	old_vfs = pci_num_vf(adapter->pdev);
 
@@ -3657,6 +3900,12 @@ static int be_vf_setup(struct be_adapter *adapter)
 		if (!old_vfs)
 			be_cmd_config_qos(adapter, 0, 0, vf + 1);
 
+		status = be_cmd_get_hsw_config(adapter, NULL, vf + 1,
+					       vf_cfg->if_handle, NULL,
+					       &spoofchk);
+		if (!status)
+			vf_cfg->spoofchk = spoofchk;
+
 		if (!old_vfs) {
 			be_cmd_enable_vf(adapter, vf + 1);
 			be_cmd_set_logical_link_config(adapter,
@@ -3733,8 +3982,9 @@ static void BEx_get_resources(struct be_adapter *adapter,
 	 *    *only* if it is RSS-capable.
 	 */
 	if (BE2_chip(adapter) || use_sriov ||  (adapter->port_num > 1) ||
-	    !be_physfn(adapter) || (be_is_mc(adapter) &&
-	    !(adapter->function_caps & BE_FUNCTION_CAPS_RSS))) {
+	    be_virtfn(adapter) ||
+	    (be_is_mc(adapter) &&
+	     !(adapter->function_caps & BE_FUNCTION_CAPS_RSS))) {
 		res->max_tx_qs = 1;
 	} else if (adapter->function_caps & BE_FUNCTION_CAPS_SUPER_NIC) {
 		struct be_resources super_nic_res = {0};
@@ -4075,7 +4325,7 @@ static int be_func_init(struct be_adapter *adapter)
 		msleep(100);
 
 		/* We can clear all errors when function reset succeeds */
-		be_clear_all_error(adapter);
+		be_clear_error(adapter, BE_CLEAR_ALL);
 	}
 
 	/* Tell FW we're ready to fire cmds */
@@ -4182,7 +4432,7 @@ static void be_netpoll(struct net_device *netdev)
 	int i;
 
 	for_all_evt_queues(adapter, eqo, i) {
-		be_eq_notify(eqo->adapter, eqo->q.id, false, true, 0);
+		be_eq_notify(eqo->adapter, eqo->q.id, false, true, 0, 0);
 		napi_schedule(&eqo->napi);
 	}
 }
@@ -4605,8 +4855,8 @@ static int lancer_fw_download(struct be_adapter *adapter,
 
 	flash_cmd.size = sizeof(struct lancer_cmd_req_write_object)
 				+ LANCER_FW_DOWNLOAD_CHUNK;
-	flash_cmd.va = dma_alloc_coherent(dev, flash_cmd.size,
-					  &flash_cmd.dma, GFP_KERNEL);
+	flash_cmd.va = dma_zalloc_coherent(dev, flash_cmd.size,
+					   &flash_cmd.dma, GFP_KERNEL);
 	if (!flash_cmd.va)
 		return -ENOMEM;
 
@@ -4666,14 +4916,11 @@ static int lancer_fw_download(struct be_adapter *adapter,
 	return 0;
 }
 
-#define BE2_UFI		2
-#define BE3_UFI		3
-#define BE3R_UFI	10
-#define SH_UFI		4
-#define SH_P2_UFI	11
-
-static int be_get_ufi_type(struct be_adapter *adapter,
-			   struct flash_file_hdr_g3 *fhdr)
+/* Check if the flash image file is compatible with the adapter that
+ * is being flashed.
+ */
+static bool be_check_ufi_compatibility(struct be_adapter *adapter,
+				       struct flash_file_hdr_g3 *fhdr)
 {
 	if (!fhdr) {
 		dev_err(&adapter->pdev->dev, "Invalid FW UFI file");
@@ -4685,43 +4932,22 @@ static int be_get_ufi_type(struct be_adapter *adapter,
 	 */
 	switch (fhdr->build[0]) {
 	case BLD_STR_UFI_TYPE_SH:
-		return (fhdr->asic_type_rev == ASIC_REV_P2) ? SH_P2_UFI :
-								SH_UFI;
+		if (!skyhawk_chip(adapter))
+			return false;
+		break;
 	case BLD_STR_UFI_TYPE_BE3:
-		return (fhdr->asic_type_rev == ASIC_REV_B0) ? BE3R_UFI :
-								BE3_UFI;
+		if (!BE3_chip(adapter))
+			return false;
+		break;
 	case BLD_STR_UFI_TYPE_BE2:
-		return BE2_UFI;
-	default:
-		return -1;
-	}
-}
-
-/* Check if the flash image file is compatible with the adapter that
- * is being flashed.
- * BE3 chips with asic-rev B0 must be flashed only with BE3R_UFI type.
- * Skyhawk chips with asic-rev P2 must be flashed only with SH_P2_UFI type.
- */
-static bool be_check_ufi_compatibility(struct be_adapter *adapter,
-				       struct flash_file_hdr_g3 *fhdr)
-{
-	int ufi_type = be_get_ufi_type(adapter, fhdr);
-
-	switch (ufi_type) {
-	case SH_P2_UFI:
-		return skyhawk_chip(adapter);
-	case SH_UFI:
-		return (skyhawk_chip(adapter) &&
-			adapter->asic_rev < ASIC_REV_P2);
-	case BE3R_UFI:
-		return BE3_chip(adapter);
-	case BE3_UFI:
-		return (BE3_chip(adapter) && adapter->asic_rev < ASIC_REV_B0);
-	case BE2_UFI:
-		return BE2_chip(adapter);
+		if (!BE2_chip(adapter))
+			return false;
+		break;
 	default:
 		return false;
 	}
+
+	return (fhdr->asic_type_rev >= adapter->asic_rev);
 }
 
 static int be_fw_download(struct be_adapter *adapter, const struct firmware* fw)
@@ -4739,8 +4965,8 @@ static int be_fw_download(struct be_adapter *adapter, const struct firmware* fw)
 	}
 
 	flash_cmd.size = sizeof(struct be_cmd_write_flashrom);
-	flash_cmd.va = dma_alloc_coherent(dev, flash_cmd.size, &flash_cmd.dma,
-					  GFP_KERNEL);
+	flash_cmd.va = dma_zalloc_coherent(dev, flash_cmd.size, &flash_cmd.dma,
+					   GFP_KERNEL);
 	if (!flash_cmd.va)
 		return -ENOMEM;
 
@@ -4829,7 +5055,7 @@ static int be_ndo_bridge_setlink(struct net_device *dev, struct nlmsghdr *nlh,
 					       adapter->if_handle,
 					       mode == BRIDGE_MODE_VEPA ?
 					       PORT_FWD_TYPE_VEPA :
-					       PORT_FWD_TYPE_VEB);
+					       PORT_FWD_TYPE_VEB, 0);
 		if (status)
 			goto err;
 
@@ -4861,7 +5087,8 @@ static int be_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 		hsw_mode = PORT_FWD_TYPE_VEB;
 	} else {
 		status = be_cmd_get_hsw_config(adapter, NULL, 0,
-					       adapter->if_handle, &hsw_mode);
+					       adapter->if_handle, &hsw_mode,
+					       NULL);
 		if (status)
 			return 0;
 	}
@@ -4869,7 +5096,7 @@ static int be_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 	return ndo_dflt_bridge_getlink(skb, pid, seq, dev,
 				       hsw_mode == PORT_FWD_TYPE_VEPA ?
 				       BRIDGE_MODE_VEPA : BRIDGE_MODE_VEB,
-				       0, 0, nlflags);
+				       0, 0, nlflags, filter_mask, NULL);
 }
 
 #ifdef CONFIG_BE2NET_VXLAN
@@ -5014,6 +5241,7 @@ static const struct net_device_ops be_netdev_ops = {
 	.ndo_set_vf_rate	= be_set_vf_tx_rate,
 	.ndo_get_vf_config	= be_get_vf_config,
 	.ndo_set_vf_link_state  = be_set_vf_link_state,
+	.ndo_set_vf_spoofchk    = be_set_vf_spoofchk,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= be_netpoll,
 #endif
@@ -5118,7 +5346,7 @@ static void be_err_detection_task(struct work_struct *work)
 
 	be_detect_error(adapter);
 
-	if (adapter->hw_error) {
+	if (be_check_error(adapter, BE_ERROR_HW)) {
 		be_cleanup(adapter);
 
 		/* As of now error recovery support is in Lancer only */
@@ -5182,7 +5410,9 @@ static void be_worker(struct work_struct *work)
 			be_post_rx_frags(rxo, GFP_KERNEL, MAX_RX_POST);
 	}
 
-	be_eqd_update(adapter);
+	/* EQ-delay update for Skyhawk is done while notifying EQ */
+	if (!skyhawk_chip(adapter))
+		be_eqd_update(adapter, false);
 
 	if (adapter->flags & BE_FLAGS_EVT_INCOMPATIBLE_SFP)
 		be_log_sfp_info(adapter);
@@ -5202,7 +5432,7 @@ static void be_unmap_pci_bars(struct be_adapter *adapter)
 
 static int db_bar(struct be_adapter *adapter)
 {
-	if (lancer_chip(adapter) || !be_physfn(adapter))
+	if (lancer_chip(adapter) || be_virtfn(adapter))
 		return 0;
 	else
 		return 4;
@@ -5291,16 +5521,15 @@ static int be_drv_init(struct be_adapter *adapter)
 	int status = 0;
 
 	mbox_mem_alloc->size = sizeof(struct be_mcc_mailbox) + 16;
-	mbox_mem_alloc->va = dma_alloc_coherent(dev, mbox_mem_alloc->size,
-						&mbox_mem_alloc->dma,
-						GFP_KERNEL);
+	mbox_mem_alloc->va = dma_zalloc_coherent(dev, mbox_mem_alloc->size,
+						 &mbox_mem_alloc->dma,
+						 GFP_KERNEL);
 	if (!mbox_mem_alloc->va)
 		return -ENOMEM;
 
 	mbox_mem_align->size = sizeof(struct be_mcc_mailbox);
 	mbox_mem_align->va = PTR_ALIGN(mbox_mem_alloc->va, 16);
 	mbox_mem_align->dma = PTR_ALIGN(mbox_mem_alloc->dma, 16);
-	memset(mbox_mem_align->va, 0, sizeof(struct be_mcc_mailbox));
 
 	rx_filter->size = sizeof(struct be_cmd_req_rx_filter);
 	rx_filter->va = dma_zalloc_coherent(dev, rx_filter->size,
@@ -5382,6 +5611,30 @@ static void be_remove(struct pci_dev *pdev)
 	free_netdev(adapter->netdev);
 }
 
+static ssize_t be_hwmon_show_temp(struct device *dev,
+				  struct device_attribute *dev_attr,
+				  char *buf)
+{
+	struct be_adapter *adapter = dev_get_drvdata(dev);
+
+	/* Unit: millidegree Celsius */
+	if (adapter->hwmon_info.be_on_die_temp == BE_INVALID_DIE_TEMP)
+		return -EIO;
+	else
+		return sprintf(buf, "%u\n",
+			       adapter->hwmon_info.be_on_die_temp * 1000);
+}
+
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO,
+			  be_hwmon_show_temp, NULL, 1);
+
+static struct attribute *be_hwmon_attrs[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	NULL
+};
+
+ATTRIBUTE_GROUPS(be_hwmon);
+
 static char *mc_name(struct be_adapter *adapter)
 {
 	char *str = "";	/* default */
@@ -5501,6 +5754,16 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 
 	be_schedule_err_detection(adapter);
 
+	/* On Die temperature not supported for VF. */
+	if (be_physfn(adapter) && IS_ENABLED(CONFIG_BE2NET_HWMON)) {
+		adapter->hwmon_info.hwmon_dev =
+			devm_hwmon_device_register_with_groups(&pdev->dev,
+							       DRV_NAME,
+							       adapter,
+							       be_hwmon_groups);
+		adapter->hwmon_info.be_on_die_temp = BE_INVALID_DIE_TEMP;
+	}
+
 	dev_info(&pdev->dev, "%s: %s %s port %c\n", nic_name(pdev),
 		 func_name(adapter), mc_name(adapter), adapter->port_name);
 
@@ -5593,8 +5856,8 @@ static pci_ers_result_t be_eeh_err_detected(struct pci_dev *pdev,
 
 	dev_err(&adapter->pdev->dev, "EEH error detected\n");
 
-	if (!adapter->eeh_error) {
-		adapter->eeh_error = true;
+	if (!be_check_error(adapter, BE_ERROR_EEH)) {
+		be_set_error(adapter, BE_ERROR_EEH);
 
 		be_cancel_err_detection(adapter);
 
@@ -5641,7 +5904,7 @@ static pci_ers_result_t be_eeh_reset(struct pci_dev *pdev)
 		return PCI_ERS_RESULT_DISCONNECT;
 
 	pci_cleanup_aer_uncorrect_error_status(pdev);
-	be_clear_all_error(adapter);
+	be_clear_error(adapter, BE_CLEAR_ALL);
 	return PCI_ERS_RESULT_RECOVERED;
 }
 
diff --git a/drivers/net/ethernet/emulex/benet/be_roce.c b/drivers/net/ethernet/emulex/benet/be_roce.c
index 132866433a25..60368207bf58 100644
--- a/drivers/net/ethernet/emulex/benet/be_roce.c
+++ b/drivers/net/ethernet/emulex/benet/be_roce.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2014 Emulex
+ * Copyright (C) 2005 - 2015 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/net/ethernet/emulex/benet/be_roce.h b/drivers/net/ethernet/emulex/benet/be_roce.h
index e6f7eb1a7d87..cde6ef905ec4 100644
--- a/drivers/net/ethernet/emulex/benet/be_roce.h
+++ b/drivers/net/ethernet/emulex/benet/be_roce.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2005 - 2014 Emulex
+ * Copyright (C) 2005 - 2015 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/net/ethernet/ezchip/Kconfig b/drivers/net/ethernet/ezchip/Kconfig
new file mode 100644
index 000000000000..48ecbc8aaaea
--- /dev/null
+++ b/drivers/net/ethernet/ezchip/Kconfig
@@ -0,0 +1,26 @@
+#
+# EZchip network device configuration
+#
+
+config NET_VENDOR_EZCHIP
+	bool "EZchip devices"
+	default y
+	---help---
+	  If you have a network (Ethernet) device belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about EZchip devices. If you say Y, you will be asked for
+	  your specific device in the following questions.
+
+if NET_VENDOR_EZCHIP
+
+config EZCHIP_NPS_MANAGEMENT_ENET
+	tristate "EZchip NPS management enet support"
+	depends on OF_IRQ && OF_NET
+	---help---
+	  Simple LAN device for debug or management purposes.
+	  Device supports interrupts for RX and TX(completion).
+	  Device does not have DMA ability.
+
+endif
diff --git a/drivers/net/ethernet/ezchip/Makefile b/drivers/net/ethernet/ezchip/Makefile
new file mode 100644
index 000000000000..e490176a8137
--- /dev/null
+++ b/drivers/net/ethernet/ezchip/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_EZCHIP_NPS_MANAGEMENT_ENET) += nps_enet.o
diff --git a/drivers/net/ethernet/ezchip/nps_enet.c b/drivers/net/ethernet/ezchip/nps_enet.c
new file mode 100644
index 000000000000..24a85b292007
--- /dev/null
+++ b/drivers/net/ethernet/ezchip/nps_enet.c
@@ -0,0 +1,658 @@
+/*
+ * Copyright(c) 2015 EZchip Technologies.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ */
+
+#include <linux/module.h>
+#include <linux/etherdevice.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_net.h>
+#include <linux/of_platform.h>
+#include "nps_enet.h"
+
+#define DRV_NAME			"nps_mgt_enet"
+
+static void nps_enet_clean_rx_fifo(struct net_device *ndev, u32 frame_len)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	u32 i, len = DIV_ROUND_UP(frame_len, sizeof(u32));
+
+	/* Empty Rx FIFO buffer by reading all words */
+	for (i = 0; i < len; i++)
+		nps_enet_reg_get(priv, NPS_ENET_REG_RX_BUF);
+}
+
+static void nps_enet_read_rx_fifo(struct net_device *ndev,
+				  unsigned char *dst, u32 length)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	s32 i, last = length & (sizeof(u32) - 1);
+	u32 *reg = (u32 *)dst, len = length / sizeof(u32);
+	bool dst_is_aligned = IS_ALIGNED((unsigned long)dst, sizeof(u32));
+
+	/* In case dst is not aligned we need an intermediate buffer */
+	if (dst_is_aligned)
+		for (i = 0; i < len; i++, reg++)
+			*reg = nps_enet_reg_get(priv, NPS_ENET_REG_RX_BUF);
+	else { /* !dst_is_aligned */
+		for (i = 0; i < len; i++, reg++) {
+			u32 buf =
+				nps_enet_reg_get(priv, NPS_ENET_REG_RX_BUF);
+
+			/* to accommodate word-unaligned address of "reg"
+			 * we have to do memcpy_toio() instead of simple "=".
+			 */
+			memcpy_toio((void __iomem *)reg, &buf, sizeof(buf));
+		}
+	}
+
+	/* copy last bytes (if any) */
+	if (last) {
+		u32 buf = nps_enet_reg_get(priv, NPS_ENET_REG_RX_BUF);
+
+		memcpy_toio((void __iomem *)reg, &buf, last);
+	}
+}
+
+static u32 nps_enet_rx_handler(struct net_device *ndev)
+{
+	u32 frame_len, err = 0;
+	u32 work_done = 0;
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct sk_buff *skb;
+	struct nps_enet_rx_ctl rx_ctrl;
+
+	rx_ctrl.value = nps_enet_reg_get(priv, NPS_ENET_REG_RX_CTL);
+	frame_len = rx_ctrl.nr;
+
+	/* Check if we got RX */
+	if (!rx_ctrl.cr)
+		return work_done;
+
+	/* If we got here there is a work for us */
+	work_done++;
+
+	/* Check Rx error */
+	if (rx_ctrl.er) {
+		ndev->stats.rx_errors++;
+		err = 1;
+	}
+
+	/* Check Rx CRC error */
+	if (rx_ctrl.crc) {
+		ndev->stats.rx_crc_errors++;
+		ndev->stats.rx_dropped++;
+		err = 1;
+	}
+
+	/* Check Frame length Min 64b */
+	if (unlikely(frame_len < ETH_ZLEN)) {
+		ndev->stats.rx_length_errors++;
+		ndev->stats.rx_dropped++;
+		err = 1;
+	}
+
+	if (err)
+		goto rx_irq_clean;
+
+	/* Skb allocation */
+	skb = netdev_alloc_skb_ip_align(ndev, frame_len);
+	if (unlikely(!skb)) {
+		ndev->stats.rx_errors++;
+		ndev->stats.rx_dropped++;
+		goto rx_irq_clean;
+	}
+
+	/* Copy frame from Rx fifo into the skb */
+	nps_enet_read_rx_fifo(ndev, skb->data, frame_len);
+
+	skb_put(skb, frame_len);
+	skb->protocol = eth_type_trans(skb, ndev);
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+	ndev->stats.rx_packets++;
+	ndev->stats.rx_bytes += frame_len;
+	netif_receive_skb(skb);
+
+	goto rx_irq_frame_done;
+
+rx_irq_clean:
+	/* Clean Rx fifo */
+	nps_enet_clean_rx_fifo(ndev, frame_len);
+
+rx_irq_frame_done:
+	/* Ack Rx ctrl register */
+	nps_enet_reg_set(priv, NPS_ENET_REG_RX_CTL, 0);
+
+	return work_done;
+}
+
+static void nps_enet_tx_handler(struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct nps_enet_tx_ctl tx_ctrl;
+
+	tx_ctrl.value = nps_enet_reg_get(priv, NPS_ENET_REG_TX_CTL);
+
+	/* Check if we got TX */
+	if (!priv->tx_packet_sent || tx_ctrl.ct)
+		return;
+
+	/* Check Tx transmit error */
+	if (unlikely(tx_ctrl.et)) {
+		ndev->stats.tx_errors++;
+	} else {
+		ndev->stats.tx_packets++;
+		ndev->stats.tx_bytes += tx_ctrl.nt;
+	}
+
+	if (priv->tx_skb) {
+		dev_kfree_skb(priv->tx_skb);
+		priv->tx_skb = NULL;
+	}
+
+	priv->tx_packet_sent = false;
+
+	if (netif_queue_stopped(ndev))
+		netif_wake_queue(ndev);
+}
+
+/**
+ * nps_enet_poll - NAPI poll handler.
+ * @napi:       Pointer to napi_struct structure.
+ * @budget:     How many frames to process on one call.
+ *
+ * returns:     Number of processed frames
+ */
+static int nps_enet_poll(struct napi_struct *napi, int budget)
+{
+	struct net_device *ndev = napi->dev;
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct nps_enet_buf_int_enable buf_int_enable;
+	u32 work_done;
+
+	buf_int_enable.rx_rdy = NPS_ENET_ENABLE;
+	buf_int_enable.tx_done = NPS_ENET_ENABLE;
+	nps_enet_tx_handler(ndev);
+	work_done = nps_enet_rx_handler(ndev);
+	if (work_done < budget) {
+		napi_complete(napi);
+		nps_enet_reg_set(priv, NPS_ENET_REG_BUF_INT_ENABLE,
+				 buf_int_enable.value);
+	}
+
+	return work_done;
+}
+
+/**
+ * nps_enet_irq_handler - Global interrupt handler for ENET.
+ * @irq:                irq number.
+ * @dev_instance:       device instance.
+ *
+ * returns: IRQ_HANDLED for all cases.
+ *
+ * EZchip ENET has 2 interrupt causes, and depending on bits raised in
+ * CTRL registers we may tell what is a reason for interrupt to fire up.
+ * We got one for RX and the other for TX (completion).
+ */
+static irqreturn_t nps_enet_irq_handler(s32 irq, void *dev_instance)
+{
+	struct net_device *ndev = dev_instance;
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct nps_enet_buf_int_cause buf_int_cause;
+
+	buf_int_cause.value =
+			nps_enet_reg_get(priv, NPS_ENET_REG_BUF_INT_CAUSE);
+
+	if (buf_int_cause.tx_done || buf_int_cause.rx_rdy)
+		if (likely(napi_schedule_prep(&priv->napi))) {
+			nps_enet_reg_set(priv, NPS_ENET_REG_BUF_INT_ENABLE, 0);
+			__napi_schedule(&priv->napi);
+		}
+
+	return IRQ_HANDLED;
+}
+
+static void nps_enet_set_hw_mac_address(struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct nps_enet_ge_mac_cfg_1 ge_mac_cfg_1;
+	struct nps_enet_ge_mac_cfg_2 *ge_mac_cfg_2 = &priv->ge_mac_cfg_2;
+
+	/* set MAC address in HW */
+	ge_mac_cfg_1.octet_0 = ndev->dev_addr[0];
+	ge_mac_cfg_1.octet_1 = ndev->dev_addr[1];
+	ge_mac_cfg_1.octet_2 = ndev->dev_addr[2];
+	ge_mac_cfg_1.octet_3 = ndev->dev_addr[3];
+	ge_mac_cfg_2->octet_4 = ndev->dev_addr[4];
+	ge_mac_cfg_2->octet_5 = ndev->dev_addr[5];
+
+	nps_enet_reg_set(priv, NPS_ENET_REG_GE_MAC_CFG_1,
+			 ge_mac_cfg_1.value);
+
+	nps_enet_reg_set(priv, NPS_ENET_REG_GE_MAC_CFG_2,
+			 ge_mac_cfg_2->value);
+}
+
+/**
+ * nps_enet_hw_reset - Reset the network device.
+ * @ndev:       Pointer to the network device.
+ *
+ * This function reset the PCS and TX fifo.
+ * The programming model is to set the relevant reset bits
+ * wait for some time for this to propagate and then unset
+ * the reset bits. This way we ensure that reset procedure
+ * is done successfully by device.
+ */
+static void nps_enet_hw_reset(struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct nps_enet_ge_rst ge_rst;
+	struct nps_enet_phase_fifo_ctl phase_fifo_ctl;
+
+	ge_rst.value = 0;
+	phase_fifo_ctl.value = 0;
+	/* Pcs reset sequence*/
+	ge_rst.gmac_0 = NPS_ENET_ENABLE;
+	nps_enet_reg_set(priv, NPS_ENET_REG_GE_RST, ge_rst.value);
+	usleep_range(10, 20);
+	ge_rst.value = 0;
+	nps_enet_reg_set(priv, NPS_ENET_REG_GE_RST, ge_rst.value);
+
+	/* Tx fifo reset sequence */
+	phase_fifo_ctl.rst = NPS_ENET_ENABLE;
+	phase_fifo_ctl.init = NPS_ENET_ENABLE;
+	nps_enet_reg_set(priv, NPS_ENET_REG_PHASE_FIFO_CTL,
+			 phase_fifo_ctl.value);
+	usleep_range(10, 20);
+	phase_fifo_ctl.value = 0;
+	nps_enet_reg_set(priv, NPS_ENET_REG_PHASE_FIFO_CTL,
+			 phase_fifo_ctl.value);
+}
+
+static void nps_enet_hw_enable_control(struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct nps_enet_ge_mac_cfg_0 ge_mac_cfg_0;
+	struct nps_enet_buf_int_enable buf_int_enable;
+	struct nps_enet_ge_mac_cfg_2 *ge_mac_cfg_2 = &priv->ge_mac_cfg_2;
+	struct nps_enet_ge_mac_cfg_3 *ge_mac_cfg_3 = &priv->ge_mac_cfg_3;
+	s32 max_frame_length;
+
+	ge_mac_cfg_0.value = 0;
+	buf_int_enable.value = 0;
+	/* Enable Rx and Tx statistics */
+	ge_mac_cfg_2->stat_en = NPS_ENET_GE_MAC_CFG_2_STAT_EN;
+
+	/* Discard packets with different MAC address */
+	ge_mac_cfg_2->disc_da = NPS_ENET_ENABLE;
+
+	/* Discard multicast packets */
+	ge_mac_cfg_2->disc_mc = NPS_ENET_ENABLE;
+
+	nps_enet_reg_set(priv, NPS_ENET_REG_GE_MAC_CFG_2,
+			 ge_mac_cfg_2->value);
+
+	/* Discard Packets bigger than max frame length */
+	max_frame_length = ETH_HLEN + ndev->mtu + ETH_FCS_LEN;
+	if (max_frame_length <= NPS_ENET_MAX_FRAME_LENGTH) {
+		ge_mac_cfg_3->max_len = max_frame_length;
+		nps_enet_reg_set(priv, NPS_ENET_REG_GE_MAC_CFG_3,
+				 ge_mac_cfg_3->value);
+	}
+
+	/* Enable interrupts */
+	buf_int_enable.rx_rdy = NPS_ENET_ENABLE;
+	buf_int_enable.tx_done = NPS_ENET_ENABLE;
+	nps_enet_reg_set(priv, NPS_ENET_REG_BUF_INT_ENABLE,
+			 buf_int_enable.value);
+
+	/* Write device MAC address to HW */
+	nps_enet_set_hw_mac_address(ndev);
+
+	/* Rx and Tx HW features */
+	ge_mac_cfg_0.tx_pad_en = NPS_ENET_ENABLE;
+	ge_mac_cfg_0.tx_crc_en = NPS_ENET_ENABLE;
+	ge_mac_cfg_0.rx_crc_strip = NPS_ENET_ENABLE;
+
+	/* IFG configuration */
+	ge_mac_cfg_0.rx_ifg = NPS_ENET_GE_MAC_CFG_0_RX_IFG;
+	ge_mac_cfg_0.tx_ifg = NPS_ENET_GE_MAC_CFG_0_TX_IFG;
+
+	/* preamble configuration */
+	ge_mac_cfg_0.rx_pr_check_en = NPS_ENET_ENABLE;
+	ge_mac_cfg_0.tx_pr_len = NPS_ENET_GE_MAC_CFG_0_TX_PR_LEN;
+
+	/* enable flow control frames */
+	ge_mac_cfg_0.tx_fc_en = NPS_ENET_ENABLE;
+	ge_mac_cfg_0.rx_fc_en = NPS_ENET_ENABLE;
+	ge_mac_cfg_0.tx_fc_retr = NPS_ENET_GE_MAC_CFG_0_TX_FC_RETR;
+
+	/* Enable Rx and Tx */
+	ge_mac_cfg_0.rx_en = NPS_ENET_ENABLE;
+	ge_mac_cfg_0.tx_en = NPS_ENET_ENABLE;
+
+	nps_enet_reg_set(priv, NPS_ENET_REG_GE_MAC_CFG_0,
+			 ge_mac_cfg_0.value);
+}
+
+static void nps_enet_hw_disable_control(struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+
+	/* Disable interrupts */
+	nps_enet_reg_set(priv, NPS_ENET_REG_BUF_INT_ENABLE, 0);
+
+	/* Disable Rx and Tx */
+	nps_enet_reg_set(priv, NPS_ENET_REG_GE_MAC_CFG_0, 0);
+}
+
+static void nps_enet_send_frame(struct net_device *ndev,
+				struct sk_buff *skb)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct nps_enet_tx_ctl tx_ctrl;
+	short length = skb->len;
+	u32 i, len = DIV_ROUND_UP(length, sizeof(u32));
+	u32 *src = (u32 *)virt_to_phys(skb->data);
+	bool src_is_aligned = IS_ALIGNED((unsigned long)src, sizeof(u32));
+
+	tx_ctrl.value = 0;
+	/* In case src is not aligned we need an intermediate buffer */
+	if (src_is_aligned)
+		for (i = 0; i < len; i++, src++)
+			nps_enet_reg_set(priv, NPS_ENET_REG_TX_BUF, *src);
+	else { /* !src_is_aligned */
+		for (i = 0; i < len; i++, src++) {
+			u32 buf;
+
+			/* to accommodate word-unaligned address of "src"
+			 * we have to do memcpy_fromio() instead of simple "="
+			 */
+			memcpy_fromio(&buf, (void __iomem *)src, sizeof(buf));
+			nps_enet_reg_set(priv, NPS_ENET_REG_TX_BUF, buf);
+		}
+	}
+	/* Write the length of the Frame */
+	tx_ctrl.nt = length;
+
+	/* Indicate SW is done */
+	priv->tx_packet_sent = true;
+	tx_ctrl.ct = NPS_ENET_ENABLE;
+
+	/* Send Frame */
+	nps_enet_reg_set(priv, NPS_ENET_REG_TX_CTL, tx_ctrl.value);
+}
+
+/**
+ * nps_enet_set_mac_address - Set the MAC address for this device.
+ * @ndev:       Pointer to net_device structure.
+ * @p:          6 byte Address to be written as MAC address.
+ *
+ * This function copies the HW address from the sockaddr structure to the
+ * net_device structure and updates the address in HW.
+ *
+ * returns:     -EBUSY if the net device is busy or 0 if the address is set
+ *              successfully.
+ */
+static s32 nps_enet_set_mac_address(struct net_device *ndev, void *p)
+{
+	struct sockaddr *addr = p;
+	s32 res;
+
+	if (netif_running(ndev))
+		return -EBUSY;
+
+	res = eth_mac_addr(ndev, p);
+	if (!res) {
+		ether_addr_copy(ndev->dev_addr, addr->sa_data);
+		nps_enet_set_hw_mac_address(ndev);
+	}
+
+	return res;
+}
+
+/**
+ * nps_enet_set_rx_mode - Change the receive filtering mode.
+ * @ndev:       Pointer to the network device.
+ *
+ * This function enables/disables promiscuous mode
+ */
+static void nps_enet_set_rx_mode(struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	struct nps_enet_ge_mac_cfg_2 ge_mac_cfg_2;
+
+	ge_mac_cfg_2.value = priv->ge_mac_cfg_2.value;
+
+	if (ndev->flags & IFF_PROMISC) {
+		ge_mac_cfg_2.disc_da = NPS_ENET_DISABLE;
+		ge_mac_cfg_2.disc_mc = NPS_ENET_DISABLE;
+	} else {
+		ge_mac_cfg_2.disc_da = NPS_ENET_ENABLE;
+		ge_mac_cfg_2.disc_mc = NPS_ENET_ENABLE;
+	}
+
+	nps_enet_reg_set(priv, NPS_ENET_REG_GE_MAC_CFG_2, ge_mac_cfg_2.value);
+}
+
+/**
+ * nps_enet_open - Open the network device.
+ * @ndev:       Pointer to the network device.
+ *
+ * returns: 0, on success or non-zero error value on failure.
+ *
+ * This function sets the MAC address, requests and enables an IRQ
+ * for the ENET device and starts the Tx queue.
+ */
+static s32 nps_enet_open(struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+	s32 err;
+
+	/* Reset private variables */
+	priv->tx_packet_sent = false;
+	priv->ge_mac_cfg_2.value = 0;
+	priv->ge_mac_cfg_3.value = 0;
+
+	/* ge_mac_cfg_3 default values */
+	priv->ge_mac_cfg_3.rx_ifg_th = NPS_ENET_GE_MAC_CFG_3_RX_IFG_TH;
+	priv->ge_mac_cfg_3.max_len = NPS_ENET_GE_MAC_CFG_3_MAX_LEN;
+
+	/* Disable HW device */
+	nps_enet_hw_disable_control(ndev);
+
+	/* irq Rx allocation */
+	err = request_irq(priv->irq, nps_enet_irq_handler,
+			  0, "enet-rx-tx", ndev);
+	if (err)
+		return err;
+
+	napi_enable(&priv->napi);
+
+	/* Enable HW device */
+	nps_enet_hw_reset(ndev);
+	nps_enet_hw_enable_control(ndev);
+
+	netif_start_queue(ndev);
+
+	return 0;
+}
+
+/**
+ * nps_enet_stop - Close the network device.
+ * @ndev:       Pointer to the network device.
+ *
+ * This function stops the Tx queue, disables interrupts for the ENET device.
+ */
+static s32 nps_enet_stop(struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+
+	napi_disable(&priv->napi);
+	netif_stop_queue(ndev);
+	nps_enet_hw_disable_control(ndev);
+	free_irq(priv->irq, ndev);
+
+	return 0;
+}
+
+/**
+ * nps_enet_start_xmit - Starts the data transmission.
+ * @skb:        sk_buff pointer that contains data to be Transmitted.
+ * @ndev:       Pointer to net_device structure.
+ *
+ * returns: NETDEV_TX_OK, on success
+ *              NETDEV_TX_BUSY, if any of the descriptors are not free.
+ *
+ * This function is invoked from upper layers to initiate transmission.
+ */
+static netdev_tx_t nps_enet_start_xmit(struct sk_buff *skb,
+				       struct net_device *ndev)
+{
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+
+	/* This driver handles one frame at a time  */
+	netif_stop_queue(ndev);
+
+	nps_enet_send_frame(ndev, skb);
+
+	priv->tx_skb = skb;
+
+	return NETDEV_TX_OK;
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void nps_enet_poll_controller(struct net_device *ndev)
+{
+	disable_irq(ndev->irq);
+	nps_enet_irq_handler(ndev->irq, ndev);
+	enable_irq(ndev->irq);
+}
+#endif
+
+static const struct net_device_ops nps_netdev_ops = {
+	.ndo_open		= nps_enet_open,
+	.ndo_stop		= nps_enet_stop,
+	.ndo_start_xmit		= nps_enet_start_xmit,
+	.ndo_set_mac_address	= nps_enet_set_mac_address,
+	.ndo_set_rx_mode        = nps_enet_set_rx_mode,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= nps_enet_poll_controller,
+#endif
+};
+
+static s32 nps_enet_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct net_device *ndev;
+	struct nps_enet_priv *priv;
+	s32 err = 0;
+	const char *mac_addr;
+	struct resource *res_regs;
+
+	if (!dev->of_node)
+		return -ENODEV;
+
+	ndev = alloc_etherdev(sizeof(struct nps_enet_priv));
+	if (!ndev)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ndev);
+	SET_NETDEV_DEV(ndev, dev);
+	priv = netdev_priv(ndev);
+
+	/* The EZ NET specific entries in the device structure. */
+	ndev->netdev_ops = &nps_netdev_ops;
+	ndev->watchdog_timeo = (400 * HZ / 1000);
+	/* FIXME :: no multicast support yet */
+	ndev->flags &= ~IFF_MULTICAST;
+
+	res_regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->regs_base = devm_ioremap_resource(dev, res_regs);
+	if (IS_ERR(priv->regs_base)) {
+		err = PTR_ERR(priv->regs_base);
+		goto out_netdev;
+	}
+	dev_dbg(dev, "Registers base address is 0x%p\n", priv->regs_base);
+
+	/* set kernel MAC address to dev */
+	mac_addr = of_get_mac_address(dev->of_node);
+	if (mac_addr)
+		ether_addr_copy(ndev->dev_addr, mac_addr);
+	else
+		eth_hw_addr_random(ndev);
+
+	/* Get IRQ number */
+	priv->irq = platform_get_irq(pdev, 0);
+	if (!priv->irq) {
+		dev_err(dev, "failed to retrieve <irq Rx-Tx> value from device tree\n");
+		err = -ENODEV;
+		goto out_netdev;
+	}
+
+	netif_napi_add(ndev, &priv->napi, nps_enet_poll,
+		       NPS_ENET_NAPI_POLL_WEIGHT);
+
+	/* Register the driver. Should be the last thing in probe */
+	err = register_netdev(ndev);
+	if (err) {
+		dev_err(dev, "Failed to register ndev for %s, err = 0x%08x\n",
+			ndev->name, (s32)err);
+		goto out_netif_api;
+	}
+
+	dev_info(dev, "(rx/tx=%d)\n", priv->irq);
+	return 0;
+
+out_netif_api:
+	netif_napi_del(&priv->napi);
+out_netdev:
+	if (err)
+		free_netdev(ndev);
+
+	return err;
+}
+
+static s32 nps_enet_remove(struct platform_device *pdev)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct nps_enet_priv *priv = netdev_priv(ndev);
+
+	unregister_netdev(ndev);
+	free_netdev(ndev);
+	netif_napi_del(&priv->napi);
+
+	return 0;
+}
+
+static const struct of_device_id nps_enet_dt_ids[] = {
+	{ .compatible = "ezchip,nps-mgt-enet" },
+	{ /* Sentinel */ }
+};
+
+static struct platform_driver nps_enet_driver = {
+	.probe = nps_enet_probe,
+	.remove = nps_enet_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table  = nps_enet_dt_ids,
+	},
+};
+
+module_platform_driver(nps_enet_driver);
+
+MODULE_AUTHOR("EZchip Semiconductor");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/ethernet/ezchip/nps_enet.h b/drivers/net/ethernet/ezchip/nps_enet.h
new file mode 100644
index 000000000000..fc45c9daa1c2
--- /dev/null
+++ b/drivers/net/ethernet/ezchip/nps_enet.h
@@ -0,0 +1,336 @@
+/*
+ * Copyright(c) 2015 EZchip Technologies.
+ *
+ * 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.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ */
+
+#ifndef _NPS_ENET_H
+#define _NPS_ENET_H
+
+/* default values */
+#define NPS_ENET_NAPI_POLL_WEIGHT		0x2
+#define NPS_ENET_MAX_FRAME_LENGTH		0x3FFF
+#define NPS_ENET_GE_MAC_CFG_0_TX_FC_RETR	0x7
+#define NPS_ENET_GE_MAC_CFG_0_RX_IFG		0x5
+#define NPS_ENET_GE_MAC_CFG_0_TX_IFG		0xC
+#define NPS_ENET_GE_MAC_CFG_0_TX_PR_LEN		0x7
+#define NPS_ENET_GE_MAC_CFG_2_STAT_EN		0x3
+#define NPS_ENET_GE_MAC_CFG_3_RX_IFG_TH		0x14
+#define NPS_ENET_GE_MAC_CFG_3_MAX_LEN		0x3FFC
+#define NPS_ENET_ENABLE				1
+#define NPS_ENET_DISABLE			0
+
+/* register definitions  */
+#define NPS_ENET_REG_TX_CTL		0x800
+#define NPS_ENET_REG_TX_BUF		0x808
+#define NPS_ENET_REG_RX_CTL		0x810
+#define NPS_ENET_REG_RX_BUF		0x818
+#define NPS_ENET_REG_BUF_INT_ENABLE	0x8C0
+#define NPS_ENET_REG_BUF_INT_CAUSE	0x8C4
+#define NPS_ENET_REG_GE_MAC_CFG_0	0x1000
+#define NPS_ENET_REG_GE_MAC_CFG_1	0x1004
+#define NPS_ENET_REG_GE_MAC_CFG_2	0x1008
+#define NPS_ENET_REG_GE_MAC_CFG_3	0x100C
+#define NPS_ENET_REG_GE_RST		0x1400
+#define NPS_ENET_REG_PHASE_FIFO_CTL	0x1404
+
+/* Tx control register */
+struct nps_enet_tx_ctl {
+	union {
+		/* ct: SW sets to indicate frame ready in Tx buffer for
+		 *     transmission. HW resets to when transmission done
+		 * et: Transmit error
+		 * nt: Length in bytes of Tx frame loaded to Tx buffer
+		 */
+		struct {
+			u32
+			__reserved_1:16,
+			ct:1,
+			et:1,
+			__reserved_2:3,
+			nt:11;
+		};
+
+		u32 value;
+	};
+};
+
+/* Rx control register */
+struct nps_enet_rx_ctl {
+	union {
+		/* cr:  HW sets to indicate frame ready in Rx buffer.
+		 *      SW resets to indicate host read received frame
+		 *      and new frames can be written to Rx buffer
+		 * er:  Rx error indication
+		 * crc: Rx CRC error indication
+		 * nr:  Length in bytes of Rx frame loaded by MAC to Rx buffer
+		 */
+		struct {
+			u32
+			__reserved_1:16,
+			cr:1,
+			er:1,
+			crc:1,
+			__reserved_2:2,
+			nr:11;
+		};
+
+		u32 value;
+	};
+};
+
+/* Interrupt enable for data buffer events register */
+struct nps_enet_buf_int_enable {
+	union {
+		/* tx_done: Interrupt generation in the case when new frame
+		 *          is ready in Rx buffer
+		 * rx_rdy:  Interrupt generation in the case when current frame
+		 *          was read from TX buffer
+		 */
+		struct {
+			u32
+			__reserved:30,
+			tx_done:1,
+			rx_rdy:1;
+		};
+
+		u32 value;
+	};
+};
+
+/* Interrupt cause for data buffer events register */
+struct nps_enet_buf_int_cause {
+	union {
+		/* tx_done: Interrupt in the case when current frame was
+		 *          read from TX buffer.
+		 * rx_rdy:  Interrupt in the case when new frame is ready
+		 *          in RX buffer.
+		 */
+		struct {
+			u32
+			__reserved:30,
+			tx_done:1,
+			rx_rdy:1;
+		};
+
+		u32 value;
+	};
+};
+
+/* Gbps Eth MAC Configuration 0 register */
+struct nps_enet_ge_mac_cfg_0 {
+	union {
+		/* tx_pr_len:          Transmit preamble length in bytes
+		 * tx_ifg_nib:         Tx idle pattern
+		 * nib_mode:           Nibble (4-bit) Mode
+		 * rx_pr_check_en:     Receive preamble Check Enable
+		 * tx_ifg:             Transmit inter-Frame Gap
+		 * rx_ifg:             Receive inter-Frame Gap
+		 * tx_fc_retr:         Transmit Flow Control Retransmit Mode
+		 * rx_length_check_en: Receive Length Check Enable
+		 * rx_crc_ignore:      Results of the CRC check are ignored
+		 * rx_crc_strip:       MAC strips the CRC from received frames
+		 * rx_fc_en:           Receive Flow Control Enable
+		 * tx_crc_en:          Transmit CRC Enabled
+		 * tx_pad_en:          Transmit Padding Enable
+		 * tx_cf_en:           Transmit Flow Control Enable
+		 * tx_en:              Transmit Enable
+		 * rx_en:              Receive Enable
+		 */
+		struct {
+			u32
+			tx_pr_len:4,
+			tx_ifg_nib:4,
+			nib_mode:1,
+			rx_pr_check_en:1,
+			tx_ifg:6,
+			rx_ifg:4,
+			tx_fc_retr:3,
+			rx_length_check_en:1,
+			rx_crc_ignore:1,
+			rx_crc_strip:1,
+			rx_fc_en:1,
+			tx_crc_en:1,
+			tx_pad_en:1,
+			tx_fc_en:1,
+			tx_en:1,
+			rx_en:1;
+		};
+
+		u32 value;
+	};
+};
+
+/* Gbps Eth MAC Configuration 1 register */
+struct nps_enet_ge_mac_cfg_1 {
+	union {
+		/* octet_3: MAC address octet 3
+		 * octet_2: MAC address octet 2
+		 * octet_1: MAC address octet 1
+		 * octet_0: MAC address octet 0
+		 */
+		struct {
+			u32
+			octet_3:8,
+			octet_2:8,
+			octet_1:8,
+			octet_0:8;
+		};
+
+		u32 value;
+	};
+};
+
+/* Gbps Eth MAC Configuration 2 register */
+struct nps_enet_ge_mac_cfg_2 {
+	union {
+		/* transmit_flush_en: MAC flush enable
+		 * stat_en:           RMON statistics interface enable
+		 * disc_da:           Discard frames with DA different
+		 *                    from MAC address
+		 * disc_bc:           Discard broadcast frames
+		 * disc_mc:           Discard multicast frames
+		 * octet_5:           MAC address octet 5
+		 * octet_4:           MAC address octet 4
+		 */
+		struct {
+			u32
+			transmit_flush_en:1,
+			__reserved_1:5,
+			stat_en:2,
+			__reserved_2:1,
+			disc_da:1,
+			disc_bc:1,
+			disc_mc:1,
+			__reserved_3:4,
+			octet_5:8,
+			octet_4:8;
+		};
+
+		u32 value;
+	};
+};
+
+/* Gbps Eth MAC Configuration 3 register */
+struct nps_enet_ge_mac_cfg_3 {
+	union {
+		/* ext_oob_cbfc_sel:  Selects one of the 4 profiles for
+		 *                    extended OOB in-flow-control indication
+		 * max_len:           Maximum receive frame length in bytes
+		 * tx_cbfc_en:        Enable transmission of class-based
+		 *                    flow control packets
+		 * rx_ifg_th:         Threshold for IFG status reporting via OOB
+		 * cf_timeout:        Configurable time to decrement FC counters
+		 * cf_drop:           Drop control frames
+		 * redirect_cbfc_sel: Selects one of CBFC redirect profiles
+		 * rx_cbfc_redir_en:  Enable Rx class-based flow
+		 *                    control redirect
+		 * rx_cbfc_en:        Enable Rx class-based flow control
+		 * tm_hd_mode:        TM header mode
+		 */
+		struct {
+			u32
+			ext_oob_cbfc_sel:2,
+			max_len:14,
+			tx_cbfc_en:1,
+			rx_ifg_th:5,
+			cf_timeout:4,
+			cf_drop:1,
+			redirect_cbfc_sel:2,
+			rx_cbfc_redir_en:1,
+			rx_cbfc_en:1,
+			tm_hd_mode:1;
+		};
+
+		u32 value;
+	};
+};
+
+/* GE MAC, PCS reset control register */
+struct nps_enet_ge_rst {
+	union {
+		/* gmac_0: GE MAC reset
+		 * spcs_0: SGMII PCS reset
+		 */
+		struct {
+			u32
+			__reserved_1:23,
+			gmac_0:1,
+			__reserved_2:7,
+			spcs_0:1;
+		};
+
+		u32 value;
+	};
+};
+
+/* Tx phase sync FIFO control register */
+struct nps_enet_phase_fifo_ctl {
+	union {
+		/* init: initialize serdes TX phase sync FIFO pointers
+		 * rst:  reset serdes TX phase sync FIFO
+		 */
+		struct {
+			u32
+			__reserved:30,
+			init:1,
+			rst:1;
+		};
+
+		u32 value;
+	};
+};
+
+/**
+ * struct nps_enet_priv - Storage of ENET's private information.
+ * @regs_base:      Base address of ENET memory-mapped control registers.
+ * @irq:            For RX/TX IRQ number.
+ * @tx_packet_sent: SW indication if frame is being sent.
+ * @tx_skb:         socket buffer of sent frame.
+ * @napi:           Structure for NAPI.
+ */
+struct nps_enet_priv {
+	void __iomem *regs_base;
+	s32 irq;
+	bool tx_packet_sent;
+	struct sk_buff *tx_skb;
+	struct napi_struct napi;
+	struct nps_enet_ge_mac_cfg_2 ge_mac_cfg_2;
+	struct nps_enet_ge_mac_cfg_3 ge_mac_cfg_3;
+};
+
+/**
+ * nps_reg_set - Sets ENET register with provided value.
+ * @priv:       Pointer to EZchip ENET private data structure.
+ * @reg:        Register offset from base address.
+ * @value:      Value to set in register.
+ */
+static inline void nps_enet_reg_set(struct nps_enet_priv *priv,
+				    s32 reg, s32 value)
+{
+	iowrite32be(value, priv->regs_base + reg);
+}
+
+/**
+ * nps_reg_get - Gets value of specified ENET register.
+ * @priv:       Pointer to EZchip ENET private data structure.
+ * @reg:        Register offset from base address.
+ *
+ * returns:     Value of requested register.
+ */
+static inline u32 nps_enet_reg_get(struct nps_enet_priv *priv, s32 reg)
+{
+	return ioread32be(priv->regs_base + reg);
+}
+
+#endif /* _NPS_ENET_H */
diff --git a/drivers/net/ethernet/faraday/Kconfig b/drivers/net/ethernet/faraday/Kconfig
index 5918c6891694..040c7f163325 100644
--- a/drivers/net/ethernet/faraday/Kconfig
+++ b/drivers/net/ethernet/faraday/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_FARADAY
 	default y
 	depends on ARM
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/freescale/Kconfig b/drivers/net/ethernet/freescale/Kconfig
index 25e3425729d0..ff76d4e9dc1b 100644
--- a/drivers/net/ethernet/freescale/Kconfig
+++ b/drivers/net/ethernet/freescale/Kconfig
@@ -9,9 +9,7 @@ config NET_VENDOR_FREESCALE
 		   M523x || M527x || M5272 || M528x || M520x || M532x || \
 		   ARCH_MXC || ARCH_MXS || (PPC_MPC52xx && PPC_BESTCOMM)
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -85,12 +83,12 @@ config UGETH_TX_ON_DEMAND
 
 config GIANFAR
 	tristate "Gianfar Ethernet"
-	depends on FSL_SOC
 	select FSL_PQ_MDIO
 	select PHYLIB
 	select CRC32
 	---help---
 	  This driver supports the Gigabit TSEC on the MPC83xx, MPC85xx,
-	  and MPC86xx family of chips, and the FEC on the 8540.
+	  and MPC86xx family of chips, the eTSEC on LS1021A and the FEC
+	  on the 8540.
 
 endif # NET_VENDOR_FREESCALE
diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index a86af8a7485d..1eee73cccdf5 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -428,6 +428,8 @@ struct bufdesc_ex {
 #define FEC_QUIRK_BUG_CAPTURE		(1 << 10)
 /* Controller has only one MDIO bus */
 #define FEC_QUIRK_SINGLE_MDIO		(1 << 11)
+/* Controller supports RACC register */
+#define FEC_QUIRK_HAS_RACC		(1 << 12)
 
 struct fec_enet_priv_tx_q {
 	int index;
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 66d47e448e4d..1f89c59b4353 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -85,28 +85,30 @@ static struct platform_device_id fec_devtype[] = {
 		.driver_data = 0,
 	}, {
 		.name = "imx25-fec",
-		.driver_data = FEC_QUIRK_USE_GASKET,
+		.driver_data = FEC_QUIRK_USE_GASKET | FEC_QUIRK_HAS_RACC,
 	}, {
 		.name = "imx27-fec",
-		.driver_data = 0,
+		.driver_data = FEC_QUIRK_HAS_RACC,
 	}, {
 		.name = "imx28-fec",
 		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_SWAP_FRAME |
-				FEC_QUIRK_SINGLE_MDIO,
+				FEC_QUIRK_SINGLE_MDIO | FEC_QUIRK_HAS_RACC,
 	}, {
 		.name = "imx6q-fec",
 		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_HAS_GBIT |
 				FEC_QUIRK_HAS_BUFDESC_EX | FEC_QUIRK_HAS_CSUM |
-				FEC_QUIRK_HAS_VLAN | FEC_QUIRK_ERR006358,
+				FEC_QUIRK_HAS_VLAN | FEC_QUIRK_ERR006358 |
+				FEC_QUIRK_HAS_RACC,
 	}, {
 		.name = "mvf600-fec",
-		.driver_data = FEC_QUIRK_ENET_MAC,
+		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_HAS_RACC,
 	}, {
 		.name = "imx6sx-fec",
 		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_HAS_GBIT |
 				FEC_QUIRK_HAS_BUFDESC_EX | FEC_QUIRK_HAS_CSUM |
 				FEC_QUIRK_HAS_VLAN | FEC_QUIRK_HAS_AVB |
-				FEC_QUIRK_ERR007885 | FEC_QUIRK_BUG_CAPTURE,
+				FEC_QUIRK_ERR007885 | FEC_QUIRK_BUG_CAPTURE |
+				FEC_QUIRK_HAS_RACC,
 	}, {
 		/* sentinel */
 	}
@@ -970,13 +972,15 @@ fec_restart(struct net_device *ndev)
 	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
 
 #if !defined(CONFIG_M5272)
-	/* set RX checksum */
-	val = readl(fep->hwp + FEC_RACC);
-	if (fep->csum_flags & FLAG_RX_CSUM_ENABLED)
-		val |= FEC_RACC_OPTIONS;
-	else
-		val &= ~FEC_RACC_OPTIONS;
-	writel(val, fep->hwp + FEC_RACC);
+	if (fep->quirks & FEC_QUIRK_HAS_RACC) {
+		/* set RX checksum */
+		val = readl(fep->hwp + FEC_RACC);
+		if (fep->csum_flags & FLAG_RX_CSUM_ENABLED)
+			val |= FEC_RACC_OPTIONS;
+		else
+			val &= ~FEC_RACC_OPTIONS;
+		writel(val, fep->hwp + FEC_RACC);
+	}
 #endif
 
 	/*
@@ -2118,6 +2122,82 @@ static void fec_enet_get_drvinfo(struct net_device *ndev,
 	strlcpy(info->bus_info, dev_name(&ndev->dev), sizeof(info->bus_info));
 }
 
+static int fec_enet_get_regs_len(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	struct resource *r;
+	int s = 0;
+
+	r = platform_get_resource(fep->pdev, IORESOURCE_MEM, 0);
+	if (r)
+		s = resource_size(r);
+
+	return s;
+}
+
+/* List of registers that can be safety be read to dump them with ethtool */
+#if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
+	defined(CONFIG_M520x) || defined(CONFIG_M532x) ||		\
+	defined(CONFIG_ARCH_MXC) || defined(CONFIG_SOC_IMX28)
+static u32 fec_enet_register_offset[] = {
+	FEC_IEVENT, FEC_IMASK, FEC_R_DES_ACTIVE_0, FEC_X_DES_ACTIVE_0,
+	FEC_ECNTRL, FEC_MII_DATA, FEC_MII_SPEED, FEC_MIB_CTRLSTAT, FEC_R_CNTRL,
+	FEC_X_CNTRL, FEC_ADDR_LOW, FEC_ADDR_HIGH, FEC_OPD, FEC_TXIC0, FEC_TXIC1,
+	FEC_TXIC2, FEC_RXIC0, FEC_RXIC1, FEC_RXIC2, FEC_HASH_TABLE_HIGH,
+	FEC_HASH_TABLE_LOW, FEC_GRP_HASH_TABLE_HIGH, FEC_GRP_HASH_TABLE_LOW,
+	FEC_X_WMRK, FEC_R_BOUND, FEC_R_FSTART, FEC_R_DES_START_1,
+	FEC_X_DES_START_1, FEC_R_BUFF_SIZE_1, FEC_R_DES_START_2,
+	FEC_X_DES_START_2, FEC_R_BUFF_SIZE_2, FEC_R_DES_START_0,
+	FEC_X_DES_START_0, FEC_R_BUFF_SIZE_0, FEC_R_FIFO_RSFL, FEC_R_FIFO_RSEM,
+	FEC_R_FIFO_RAEM, FEC_R_FIFO_RAFL, FEC_RACC, FEC_RCMR_1, FEC_RCMR_2,
+	FEC_DMA_CFG_1, FEC_DMA_CFG_2, FEC_R_DES_ACTIVE_1, FEC_X_DES_ACTIVE_1,
+	FEC_R_DES_ACTIVE_2, FEC_X_DES_ACTIVE_2, FEC_QOS_SCHEME,
+	RMON_T_DROP, RMON_T_PACKETS, RMON_T_BC_PKT, RMON_T_MC_PKT,
+	RMON_T_CRC_ALIGN, RMON_T_UNDERSIZE, RMON_T_OVERSIZE, RMON_T_FRAG,
+	RMON_T_JAB, RMON_T_COL, RMON_T_P64, RMON_T_P65TO127, RMON_T_P128TO255,
+	RMON_T_P256TO511, RMON_T_P512TO1023, RMON_T_P1024TO2047,
+	RMON_T_P_GTE2048, RMON_T_OCTETS,
+	IEEE_T_DROP, IEEE_T_FRAME_OK, IEEE_T_1COL, IEEE_T_MCOL, IEEE_T_DEF,
+	IEEE_T_LCOL, IEEE_T_EXCOL, IEEE_T_MACERR, IEEE_T_CSERR, IEEE_T_SQE,
+	IEEE_T_FDXFC, IEEE_T_OCTETS_OK,
+	RMON_R_PACKETS, RMON_R_BC_PKT, RMON_R_MC_PKT, RMON_R_CRC_ALIGN,
+	RMON_R_UNDERSIZE, RMON_R_OVERSIZE, RMON_R_FRAG, RMON_R_JAB,
+	RMON_R_RESVD_O, RMON_R_P64, RMON_R_P65TO127, RMON_R_P128TO255,
+	RMON_R_P256TO511, RMON_R_P512TO1023, RMON_R_P1024TO2047,
+	RMON_R_P_GTE2048, RMON_R_OCTETS,
+	IEEE_R_DROP, IEEE_R_FRAME_OK, IEEE_R_CRC, IEEE_R_ALIGN, IEEE_R_MACERR,
+	IEEE_R_FDXFC, IEEE_R_OCTETS_OK
+};
+#else
+static u32 fec_enet_register_offset[] = {
+	FEC_ECNTRL, FEC_IEVENT, FEC_IMASK, FEC_IVEC, FEC_R_DES_ACTIVE_0,
+	FEC_R_DES_ACTIVE_1, FEC_R_DES_ACTIVE_2, FEC_X_DES_ACTIVE_0,
+	FEC_X_DES_ACTIVE_1, FEC_X_DES_ACTIVE_2, FEC_MII_DATA, FEC_MII_SPEED,
+	FEC_R_BOUND, FEC_R_FSTART, FEC_X_WMRK, FEC_X_FSTART, FEC_R_CNTRL,
+	FEC_MAX_FRM_LEN, FEC_X_CNTRL, FEC_ADDR_LOW, FEC_ADDR_HIGH,
+	FEC_GRP_HASH_TABLE_HIGH, FEC_GRP_HASH_TABLE_LOW, FEC_R_DES_START_0,
+	FEC_R_DES_START_1, FEC_R_DES_START_2, FEC_X_DES_START_0,
+	FEC_X_DES_START_1, FEC_X_DES_START_2, FEC_R_BUFF_SIZE_0,
+	FEC_R_BUFF_SIZE_1, FEC_R_BUFF_SIZE_2
+};
+#endif
+
+static void fec_enet_get_regs(struct net_device *ndev,
+			      struct ethtool_regs *regs, void *regbuf)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	u32 __iomem *theregs = (u32 __iomem *)fep->hwp;
+	u32 *buf = (u32 *)regbuf;
+	u32 i, off;
+
+	memset(buf, 0, regs->len);
+
+	for (i = 0; i < ARRAY_SIZE(fec_enet_register_offset); i++) {
+		off = fec_enet_register_offset[i] / 4;
+		buf[off] = readl(&theregs[off]);
+	}
+}
+
 static int fec_enet_get_ts_info(struct net_device *ndev,
 				struct ethtool_ts_info *info)
 {
@@ -2515,6 +2595,8 @@ static const struct ethtool_ops fec_enet_ethtool_ops = {
 	.get_settings		= fec_enet_get_settings,
 	.set_settings		= fec_enet_set_settings,
 	.get_drvinfo		= fec_enet_get_drvinfo,
+	.get_regs_len		= fec_enet_get_regs_len,
+	.get_regs		= fec_enet_get_regs,
 	.nway_reset		= fec_enet_nway_reset,
 	.get_link		= ethtool_op_get_link,
 	.get_coalesce		= fec_enet_get_coalesce,
@@ -2778,12 +2860,14 @@ fec_enet_open(struct net_device *ndev)
 	if (ret)
 		goto err_enet_alloc;
 
+	/* Init MAC prior to mii bus probe */
+	fec_restart(ndev);
+
 	/* Probe and connect to PHY when open the interface */
 	ret = fec_enet_mii_probe(ndev);
 	if (ret)
 		goto err_enet_mii_probe;
 
-	fec_restart(ndev);
 	napi_enable(&fep->napi);
 	phy_start(fep->phy_dev);
 	netif_tx_start_all_queues(ndev);
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index a583d89b13c4..a15663ad7f5e 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -353,6 +353,7 @@ static int fec_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 	tmp = readl(fep->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK;
 	tmp |= corr_ns << FEC_T_INC_CORR_OFFSET;
 	writel(tmp, fep->hwp + FEC_ATIME_INC);
+	corr_period = corr_period > 1 ? corr_period - 1 : corr_period;
 	writel(corr_period, fep->hwp + FEC_ATIME_CORR);
 	/* dummy read to update the timer. */
 	timecounter_read(&fep->tc);
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 9b3639eae676..56316db6c5a6 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
@@ -86,7 +86,7 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 	struct net_device *dev = fep->ndev;
 	const struct fs_platform_info *fpi = fep->fpi;
 	cbd_t __iomem *bdp;
-	struct sk_buff *skb, *skbn, *skbt;
+	struct sk_buff *skb, *skbn;
 	int received = 0;
 	u16 pkt_len, sc;
 	int curidx;
@@ -161,10 +161,7 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 					skb_reserve(skbn, 2);	/* align IP header */
 					skb_copy_from_linear_data(skb,
 						      skbn->data, pkt_len);
-					/* swap */
-					skbt = skb;
-					skb = skbn;
-					skbn = skbt;
+					swap(skb, skbn);
 				}
 			} else {
 				skbn = netdev_alloc_skb(dev, ENET_RX_FRSIZE);
@@ -490,6 +487,9 @@ static struct sk_buff *tx_skb_align_workaround(struct net_device *dev,
 {
 	struct sk_buff *new_skb;
 
+	if (skb_linearize(skb))
+		return NULL;
+
 	/* Alloc new skb */
 	new_skb = netdev_alloc_skb(dev, skb->len + 4);
 	if (!new_skb)
@@ -515,12 +515,27 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	cbd_t __iomem *bdp;
 	int curidx;
 	u16 sc;
-	int nr_frags = skb_shinfo(skb)->nr_frags;
+	int nr_frags;
 	skb_frag_t *frag;
 	int len;
-
 #ifdef CONFIG_FS_ENET_MPC5121_FEC
-	if (((unsigned long)skb->data) & 0x3) {
+	int is_aligned = 1;
+	int i;
+
+	if (!IS_ALIGNED((unsigned long)skb->data, 4)) {
+		is_aligned = 0;
+	} else {
+		nr_frags = skb_shinfo(skb)->nr_frags;
+		frag = skb_shinfo(skb)->frags;
+		for (i = 0; i < nr_frags; i++, frag++) {
+			if (!IS_ALIGNED(frag->page_offset, 4)) {
+				is_aligned = 0;
+				break;
+			}
+		}
+	}
+
+	if (!is_aligned) {
 		skb = tx_skb_align_workaround(dev, skb);
 		if (!skb) {
 			/*
@@ -532,6 +547,7 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		}
 	}
 #endif
+
 	spin_lock(&fep->tx_lock);
 
 	/*
@@ -539,6 +555,7 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	 */
 	bdp = fep->cur_tx;
 
+	nr_frags = skb_shinfo(skb)->nr_frags;
 	if (fep->tx_free <= nr_frags || (CBDR_SC(bdp) & BD_ENET_TX_READY)) {
 		netif_stop_queue(dev);
 		spin_unlock(&fep->tx_lock);
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index 4ee080d49bc0..ff875028fdff 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -516,6 +516,15 @@ static struct net_device_stats *gfar_get_stats(struct net_device *dev)
 	return &dev->stats;
 }
 
+static int gfar_set_mac_addr(struct net_device *dev, void *p)
+{
+	eth_mac_addr(dev, p);
+
+	gfar_set_mac_for_addr(dev, 0, dev->dev_addr);
+
+	return 0;
+}
+
 static const struct net_device_ops gfar_netdev_ops = {
 	.ndo_open = gfar_enet_open,
 	.ndo_start_xmit = gfar_start_xmit,
@@ -526,7 +535,7 @@ static const struct net_device_ops gfar_netdev_ops = {
 	.ndo_tx_timeout = gfar_timeout,
 	.ndo_do_ioctl = gfar_ioctl,
 	.ndo_get_stats = gfar_get_stats,
-	.ndo_set_mac_address = eth_mac_addr,
+	.ndo_set_mac_address = gfar_set_mac_addr,
 	.ndo_validate_addr = eth_validate_addr,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller = gfar_netpoll,
@@ -1411,6 +1420,8 @@ static int gfar_probe(struct platform_device *ofdev)
 		dev->features |= NETIF_F_HW_VLAN_CTAG_RX;
 	}
 
+	dev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
+
 	gfar_init_addr_hash_table(priv);
 
 	/* Insert receive time stamps into padding alignment bytes */
@@ -2254,7 +2265,6 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	int i, rq = 0;
 	int do_tstamp, do_csum, do_vlan;
 	u32 bufaddr;
-	unsigned long flags;
 	unsigned int nr_frags, nr_txbds, bytes_sent, fcb_len = 0;
 
 	rq = skb->queue_mapping;
@@ -2434,19 +2444,6 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	netdev_tx_sent_queue(txq, bytes_sent);
 
-	/* We can work in parallel with gfar_clean_tx_ring(), except
-	 * when modifying num_txbdfree. Note that we didn't grab the lock
-	 * when we were reading the num_txbdfree and checking for available
-	 * space, that's because outside of this function it can only grow,
-	 * and once we've got needed space, it cannot suddenly disappear.
-	 *
-	 * The lock also protects us from gfar_error(), which can modify
-	 * regs->tstat and thus retrigger the transfers, which is why we
-	 * also must grab the lock before setting ready bit for the first
-	 * to be transmitted BD.
-	 */
-	spin_lock_irqsave(&tx_queue->txlock, flags);
-
 	gfar_wmb();
 
 	txbdp_start->lstatus = cpu_to_be32(lstatus);
@@ -2463,8 +2460,15 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	tx_queue->cur_tx = next_txbd(txbdp, base, tx_queue->tx_ring_size);
 
+	/* We can work in parallel with gfar_clean_tx_ring(), except
+	 * when modifying num_txbdfree. Note that we didn't grab the lock
+	 * when we were reading the num_txbdfree and checking for available
+	 * space, that's because outside of this function it can only grow.
+	 */
+	spin_lock_bh(&tx_queue->txlock);
 	/* reduce TxBD free count */
 	tx_queue->num_txbdfree -= (nr_txbds);
+	spin_unlock_bh(&tx_queue->txlock);
 
 	/* If the next BD still needs to be cleaned up, then the bds
 	 * are full.  We need to tell the kernel to stop sending us stuff.
@@ -2478,9 +2482,6 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	/* Tell the DMA to go go go */
 	gfar_write(&regs->tstat, TSTAT_CLEAR_THALT >> tx_queue->qindex);
 
-	/* Unlock priv */
-	spin_unlock_irqrestore(&tx_queue->txlock, flags);
-
 	return NETDEV_TX_OK;
 
 dma_map_err:
@@ -2622,7 +2623,6 @@ static void gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 	skb_dirtytx = tx_queue->skb_dirtytx;
 
 	while ((skb = tx_queue->tx_skbuff[skb_dirtytx])) {
-		unsigned long flags;
 
 		frags = skb_shinfo(skb)->nr_frags;
 
@@ -2686,9 +2686,9 @@ static void gfar_clean_tx_ring(struct gfar_priv_tx_q *tx_queue)
 			      TX_RING_MOD_MASK(tx_ring_size);
 
 		howmany++;
-		spin_lock_irqsave(&tx_queue->txlock, flags);
+		spin_lock(&tx_queue->txlock);
 		tx_queue->num_txbdfree += nr_txbds;
-		spin_unlock_irqrestore(&tx_queue->txlock, flags);
+		spin_unlock(&tx_queue->txlock);
 	}
 
 	/* If we freed a buffer, we can restart transmission, if necessary */
@@ -3411,21 +3411,12 @@ static irqreturn_t gfar_error(int irq, void *grp_id)
 		if (events & IEVENT_CRL)
 			dev->stats.tx_aborted_errors++;
 		if (events & IEVENT_XFUN) {
-			unsigned long flags;
-
 			netif_dbg(priv, tx_err, dev,
 				  "TX FIFO underrun, packet dropped\n");
 			dev->stats.tx_dropped++;
 			atomic64_inc(&priv->extra_stats.tx_underrun);
 
-			local_irq_save(flags);
-			lock_tx_qs(priv);
-
-			/* Reactivate the Tx Queues */
-			gfar_write(&regs->tstat, gfargrp->tstat);
-
-			unlock_tx_qs(priv);
-			local_irq_restore(flags);
+			schedule_work(&priv->reset_task);
 		}
 		netif_dbg(priv, tx_err, dev, "Transmit Error\n");
 	}
diff --git a/drivers/net/ethernet/fujitsu/Kconfig b/drivers/net/ethernet/fujitsu/Kconfig
index 1085257385d2..faee34e44a35 100644
--- a/drivers/net/ethernet/fujitsu/Kconfig
+++ b/drivers/net/ethernet/fujitsu/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_FUJITSU
 	default y
 	depends on PCMCIA
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  the questions about Fujitsu cards. If you say Y, you will be asked for
diff --git a/drivers/net/ethernet/hisilicon/Kconfig b/drivers/net/ethernet/hisilicon/Kconfig
index a54d89791311..dead17b5d769 100644
--- a/drivers/net/ethernet/hisilicon/Kconfig
+++ b/drivers/net/ethernet/hisilicon/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_HISILICON
 	default y
 	depends on ARM
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/hisilicon/hip04_eth.c b/drivers/net/ethernet/hisilicon/hip04_eth.c
index 3b39fdddeb57..d49bee38cd31 100644
--- a/drivers/net/ethernet/hisilicon/hip04_eth.c
+++ b/drivers/net/ethernet/hisilicon/hip04_eth.c
@@ -798,7 +798,7 @@ static void hip04_free_ring(struct net_device *ndev, struct device *d)
 
 	for (i = 0; i < RX_DESC_NUM; i++)
 		if (priv->rx_buf[i])
-			put_page(virt_to_head_page(priv->rx_buf[i]));
+			skb_free_frag(priv->rx_buf[i]);
 
 	for (i = 0; i < TX_DESC_NUM; i++)
 		if (priv->tx_skb[i])
diff --git a/drivers/net/ethernet/hisilicon/hix5hd2_gmac.c b/drivers/net/ethernet/hisilicon/hix5hd2_gmac.c
index 0ffdcd381fdd..a5e077eac99a 100644
--- a/drivers/net/ethernet/hisilicon/hix5hd2_gmac.c
+++ b/drivers/net/ethernet/hisilicon/hix5hd2_gmac.c
@@ -500,7 +500,6 @@ static int hix5hd2_rx(struct net_device *dev, int limit)
 		napi_gro_receive(&priv->napi, skb);
 		dev->stats.rx_packets++;
 		dev->stats.rx_bytes += skb->len;
-		dev->last_rx = jiffies;
 next:
 		pos = dma_ring_incr(pos, RX_DESC_NUM);
 	}
diff --git a/drivers/net/ethernet/hp/Kconfig b/drivers/net/ethernet/hp/Kconfig
index a0b8ece1e3bc..d4df78c2abce 100644
--- a/drivers/net/ethernet/hp/Kconfig
+++ b/drivers/net/ethernet/hp/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_HP
 	default y
 	depends on ISA || EISA || PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -22,9 +20,7 @@ config HP100
 	tristate "HP 10/100VG PCLAN (ISA, EISA, PCI) support"
 	depends on (ISA || EISA || PCI)
 	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card of this type, say Y here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called hp100.
diff --git a/drivers/net/ethernet/i825xx/Kconfig b/drivers/net/ethernet/i825xx/Kconfig
index 9521e68aa3b3..e8d61f670479 100644
--- a/drivers/net/ethernet/i825xx/Kconfig
+++ b/drivers/net/ethernet/i825xx/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_I825XX
 	default y
 	depends on NET_VENDOR_INTEL
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question does not directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/ibm/Kconfig b/drivers/net/ethernet/ibm/Kconfig
index 563a1ac71dbc..99c1cebd002d 100644
--- a/drivers/net/ethernet/ibm/Kconfig
+++ b/drivers/net/ethernet/ibm/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_IBM
 	default y
 	depends on PPC_PSERIES || PPC_DCR || (IBMEBUS && SPARSEMEM)
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/ibm/emac/core.c b/drivers/net/ethernet/ibm/emac/core.c
index b9df0cbd0a38..b60a34d982a9 100644
--- a/drivers/net/ethernet/ibm/emac/core.c
+++ b/drivers/net/ethernet/ibm/emac/core.c
@@ -2999,7 +2999,7 @@ static struct platform_driver emac_driver = {
 static void __init emac_make_bootlist(void)
 {
 	struct device_node *np = NULL;
-	int j, max, i = 0, k;
+	int j, max, i = 0;
 	int cell_indices[EMAC_BOOT_LIST_SIZE];
 
 	/* Collect EMACs */
@@ -3026,12 +3026,8 @@ static void __init emac_make_bootlist(void)
 	for (i = 0; max > 1 && (i < (max - 1)); i++)
 		for (j = i; j < max; j++) {
 			if (cell_indices[i] > cell_indices[j]) {
-				np = emac_boot_list[i];
-				emac_boot_list[i] = emac_boot_list[j];
-				emac_boot_list[j] = np;
-				k = cell_indices[i];
-				cell_indices[i] = cell_indices[j];
-				cell_indices[j] = k;
+				swap(emac_boot_list[i], emac_boot_list[j]);
+				swap(cell_indices[i], cell_indices[j]);
 			}
 		}
 }
diff --git a/drivers/net/ethernet/ibm/ibmveth.c b/drivers/net/ethernet/ibm/ibmveth.c
index 18134766a114..29bbb628d712 100644
--- a/drivers/net/ethernet/ibm/ibmveth.c
+++ b/drivers/net/ethernet/ibm/ibmveth.c
@@ -58,7 +58,7 @@ static struct kobj_type ktype_veth_pool;
 
 static const char ibmveth_driver_name[] = "ibmveth";
 static const char ibmveth_driver_string[] = "IBM Power Virtual Ethernet Driver";
-#define ibmveth_driver_version "1.04"
+#define ibmveth_driver_version "1.05"
 
 MODULE_AUTHOR("Santiago Leon <santil@linux.vnet.ibm.com>");
 MODULE_DESCRIPTION("IBM Power Virtual Ethernet Driver");
@@ -100,6 +100,8 @@ struct ibmveth_stat ibmveth_stats[] = {
 	{ "tx_send_failed", IBMVETH_STAT_OFF(tx_send_failed) },
 	{ "fw_enabled_ipv4_csum", IBMVETH_STAT_OFF(fw_ipv4_csum_support) },
 	{ "fw_enabled_ipv6_csum", IBMVETH_STAT_OFF(fw_ipv6_csum_support) },
+	{ "tx_large_packets", IBMVETH_STAT_OFF(tx_large_packets) },
+	{ "rx_large_packets", IBMVETH_STAT_OFF(rx_large_packets) }
 };
 
 /* simple methods of getting data from the current rxq entry */
@@ -852,6 +854,10 @@ static int ibmveth_set_features(struct net_device *dev,
 	struct ibmveth_adapter *adapter = netdev_priv(dev);
 	int rx_csum = !!(features & NETIF_F_RXCSUM);
 	int rc;
+	netdev_features_t changed = features ^ dev->features;
+
+	if (features & NETIF_F_TSO & changed)
+		netdev_info(dev, "TSO feature requires all partitions to have updated driver");
 
 	if (rx_csum == adapter->rx_csum)
 		return 0;
@@ -1035,6 +1041,15 @@ retry_bounce:
 		descs[i+1].fields.address = dma_addr;
 	}
 
+	if (skb_is_gso(skb) && !skb_is_gso_v6(skb)) {
+		/* Put -1 in the IP checksum to tell phyp it
+		 *  is a largesend packet and put the mss in the TCP checksum.
+		 */
+		ip_hdr(skb)->check = 0xffff;
+		tcp_hdr(skb)->check = cpu_to_be16(skb_shinfo(skb)->gso_size);
+		adapter->tx_large_packets++;
+	}
+
 	if (ibmveth_send(adapter, descs)) {
 		adapter->tx_send_failed++;
 		netdev->stats.tx_dropped++;
@@ -1080,6 +1095,7 @@ static int ibmveth_poll(struct napi_struct *napi, int budget)
 	struct net_device *netdev = adapter->netdev;
 	int frames_processed = 0;
 	unsigned long lpar_rc;
+	struct iphdr *iph;
 
 restart_poll:
 	while (frames_processed < budget) {
@@ -1122,10 +1138,23 @@ restart_poll:
 			skb_put(skb, length);
 			skb->protocol = eth_type_trans(skb, netdev);
 
-			if (csum_good)
+			if (csum_good) {
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
+				if (be16_to_cpu(skb->protocol) == ETH_P_IP) {
+					iph = (struct iphdr *)skb->data;
+
+					/* If the IP checksum is not offloaded and if the packet
+					 *  is large send, the checksum must be rebuilt.
+					 */
+					if (iph->check == 0xffff) {
+						iph->check = 0;
+						iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
+						adapter->rx_large_packets++;
+					}
+				}
+			}
 
-			netif_receive_skb(skb);	/* send it up */
+			napi_gro_receive(napi, skb);	/* send it up */
 
 			netdev->stats.rx_packets++;
 			netdev->stats.rx_bytes += length;
@@ -1422,8 +1451,14 @@ static int ibmveth_probe(struct vio_dev *dev, const struct vio_device_id *id)
 		NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
 	netdev->features |= netdev->hw_features;
 
+	/* TSO is disabled by default */
+	netdev->hw_features |= NETIF_F_TSO;
+
 	memcpy(netdev->dev_addr, mac_addr_p, ETH_ALEN);
 
+	if (firmware_has_feature(FW_FEATURE_CMO))
+		memcpy(pool_count, pool_count_cmo, sizeof(pool_count));
+
 	for (i = 0; i < IBMVETH_NUM_BUFF_POOLS; i++) {
 		struct kobject *kobj = &adapter->rx_buff_pool[i].kobj;
 		int error;
diff --git a/drivers/net/ethernet/ibm/ibmveth.h b/drivers/net/ethernet/ibm/ibmveth.h
index 1f37499d4398..41dedb1fb2ae 100644
--- a/drivers/net/ethernet/ibm/ibmveth.h
+++ b/drivers/net/ethernet/ibm/ibmveth.h
@@ -104,7 +104,8 @@ static inline long h_illan_attributes(unsigned long unit_address,
 
 static int pool_size[] = { 512, 1024 * 2, 1024 * 16, 1024 * 32, 1024 * 64 };
 static int pool_count[] = { 256, 512, 256, 256, 256 };
-static int pool_active[] = { 1, 1, 0, 0, 0};
+static int pool_count_cmo[] = { 256, 512, 256, 256, 64 };
+static int pool_active[] = { 1, 1, 0, 0, 1};
 
 #define IBM_VETH_INVALID_MAP ((u16)0xffff)
 
@@ -160,6 +161,8 @@ struct ibmveth_adapter {
     u64 rx_no_buffer;
     u64 tx_map_failed;
     u64 tx_send_failed;
+    u64 tx_large_packets;
+    u64 rx_large_packets;
 };
 
 /*
diff --git a/drivers/net/ethernet/icplus/ipg.c b/drivers/net/ethernet/icplus/ipg.c
index ff2903652f4b..c3b6af83f070 100644
--- a/drivers/net/ethernet/icplus/ipg.c
+++ b/drivers/net/ethernet/icplus/ipg.c
@@ -1028,7 +1028,7 @@ static struct net_device_stats *ipg_nic_get_stats(struct net_device *dev)
 
 	/* detailed rx_errors */
 	sp->stats.rx_length_errors += ipg_r16(IPG_INRANGELENGTHERRORS) +
-		ipg_r16(IPG_FRAMETOOLONGERRRORS);
+		ipg_r16(IPG_FRAMETOOLONGERRORS);
 	sp->stats.rx_crc_errors += ipg_r16(IPG_FRAMECHECKSEQERRORS);
 
 	/* Unutilized IPG statistic registers. */
diff --git a/drivers/net/ethernet/icplus/ipg.h b/drivers/net/ethernet/icplus/ipg.h
index a21e4f5702b5..de606281f97b 100644
--- a/drivers/net/ethernet/icplus/ipg.h
+++ b/drivers/net/ethernet/icplus/ipg.h
@@ -102,7 +102,7 @@ enum ipg_regs {
 #define	IPG_MCSTFRAMESRCVDOK		0xB8
 #define	IPG_BCSTFRAMESRCVDOK		0xBE
 #define	IPG_MACCONTROLFRAMESRCVD	0xC6
-#define	IPG_FRAMETOOLONGERRRORS		0xC8
+#define	IPG_FRAMETOOLONGERRORS		0xC8
 #define	IPG_INRANGELENGTHERRORS		0xCA
 #define	IPG_FRAMECHECKSEQERRORS		0xCC
 #define	IPG_FRAMESLOSTRXERRORS		0xCE
diff --git a/drivers/net/ethernet/intel/Kconfig b/drivers/net/ethernet/intel/Kconfig
index f4ff465584a0..4163b16489b3 100644
--- a/drivers/net/ethernet/intel/Kconfig
+++ b/drivers/net/ethernet/intel/Kconfig
@@ -6,9 +6,7 @@ config NET_VENDOR_INTEL
 	bool "Intel devices"
 	default y
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/intel/e100.c b/drivers/net/ethernet/intel/e100.c
index 1a450f4b6b12..d2657a412768 100644
--- a/drivers/net/ethernet/intel/e100.c
+++ b/drivers/net/ethernet/intel/e100.c
@@ -874,7 +874,7 @@ static int e100_exec_cb(struct nic *nic, struct sk_buff *skb,
 {
 	struct cb *cb;
 	unsigned long flags;
-	int err = 0;
+	int err;
 
 	spin_lock_irqsave(&nic->cb_lock, flags);
 
@@ -2922,9 +2922,7 @@ static int e100_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	pci_set_master(pdev);
 
-	init_timer(&nic->watchdog);
-	nic->watchdog.function = e100_watchdog;
-	nic->watchdog.data = (unsigned long)nic;
+	setup_timer(&nic->watchdog, e100_watchdog, (unsigned long)nic);
 
 	INIT_WORK(&nic->tx_timeout_task, e100_tx_timeout_task);
 
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index 983eb4e6f7aa..74dc15055971 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -2079,11 +2079,6 @@ static void *e1000_alloc_frag(const struct e1000_adapter *a)
 	return data;
 }
 
-static void e1000_free_frag(const void *data)
-{
-	put_page(virt_to_head_page(data));
-}
-
 /**
  * e1000_clean_rx_ring - Free Rx Buffers per Queue
  * @adapter: board private structure
@@ -2107,7 +2102,7 @@ static void e1000_clean_rx_ring(struct e1000_adapter *adapter,
 						 adapter->rx_buffer_len,
 						 DMA_FROM_DEVICE);
 			if (buffer_info->rxbuf.data) {
-				e1000_free_frag(buffer_info->rxbuf.data);
+				skb_free_frag(buffer_info->rxbuf.data);
 				buffer_info->rxbuf.data = NULL;
 			}
 		} else if (adapter->clean_rx == e1000_clean_jumbo_rx_irq) {
@@ -4594,28 +4589,28 @@ static void e1000_alloc_rx_buffers(struct e1000_adapter *adapter,
 			data = e1000_alloc_frag(adapter);
 			/* Failed allocation, critical failure */
 			if (!data) {
-				e1000_free_frag(olddata);
+				skb_free_frag(olddata);
 				adapter->alloc_rx_buff_failed++;
 				break;
 			}
 
 			if (!e1000_check_64k_bound(adapter, data, bufsz)) {
 				/* give up */
-				e1000_free_frag(data);
-				e1000_free_frag(olddata);
+				skb_free_frag(data);
+				skb_free_frag(olddata);
 				adapter->alloc_rx_buff_failed++;
 				break;
 			}
 
 			/* Use new allocation */
-			e1000_free_frag(olddata);
+			skb_free_frag(olddata);
 		}
 		buffer_info->dma = dma_map_single(&pdev->dev,
 						  data,
 						  adapter->rx_buffer_len,
 						  DMA_FROM_DEVICE);
 		if (dma_mapping_error(&pdev->dev, buffer_info->dma)) {
-			e1000_free_frag(data);
+			skb_free_frag(data);
 			buffer_info->dma = 0;
 			adapter->alloc_rx_buff_failed++;
 			break;
@@ -4637,7 +4632,7 @@ static void e1000_alloc_rx_buffers(struct e1000_adapter *adapter,
 					 adapter->rx_buffer_len,
 					 DMA_FROM_DEVICE);
 
-			e1000_free_frag(data);
+			skb_free_frag(data);
 			buffer_info->rxbuf.data = NULL;
 			buffer_info->dma = 0;
 
diff --git a/drivers/net/ethernet/intel/e1000e/80003es2lan.c b/drivers/net/ethernet/intel/e1000e/80003es2lan.c
index 08f22f348800..2af603f3e418 100644
--- a/drivers/net/ethernet/intel/e1000e/80003es2lan.c
+++ b/drivers/net/ethernet/intel/e1000e/80003es2lan.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/80003es2lan.h b/drivers/net/ethernet/intel/e1000e/80003es2lan.h
index 535a9430976d..a2162e11673e 100644
--- a/drivers/net/ethernet/intel/e1000e/80003es2lan.h
+++ b/drivers/net/ethernet/intel/e1000e/80003es2lan.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/82571.c b/drivers/net/ethernet/intel/e1000e/82571.c
index dc79ed85030b..5f7016442ec4 100644
--- a/drivers/net/ethernet/intel/e1000e/82571.c
+++ b/drivers/net/ethernet/intel/e1000e/82571.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
@@ -2010,7 +2010,7 @@ const struct e1000_info e1000_82573_info = {
 	.flags2			= FLAG2_DISABLE_ASPM_L1
 				  | FLAG2_DISABLE_ASPM_L0S,
 	.pba			= 20,
-	.max_hw_frame_size	= ETH_FRAME_LEN + ETH_FCS_LEN,
+	.max_hw_frame_size	= VLAN_ETH_FRAME_LEN + ETH_FCS_LEN,
 	.get_variants		= e1000_get_variants_82571,
 	.mac_ops		= &e82571_mac_ops,
 	.phy_ops		= &e82_phy_ops_m88,
diff --git a/drivers/net/ethernet/intel/e1000e/82571.h b/drivers/net/ethernet/intel/e1000e/82571.h
index 2e758f796d60..abc6a9abff98 100644
--- a/drivers/net/ethernet/intel/e1000e/82571.h
+++ b/drivers/net/ethernet/intel/e1000e/82571.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/defines.h b/drivers/net/ethernet/intel/e1000e/defines.h
index 0570c668ec3d..133d4074dbe4 100644
--- a/drivers/net/ethernet/intel/e1000e/defines.h
+++ b/drivers/net/ethernet/intel/e1000e/defines.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index 0abc942c966e..0b748d1959d9 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
@@ -98,6 +98,8 @@ struct e1000_info;
 #define DEFAULT_RADV			8
 #define BURST_RDTR			0x20
 #define BURST_RADV			0x20
+#define PCICFG_DESC_RING_STATUS		0xe4
+#define FLUSH_DESC_REQUIRED		0x100
 
 /* in the case of WTHRESH, it appears at least the 82571/2 hardware
  * writes back 4 descriptors when WTHRESH=5, and 3 descriptors when
@@ -384,6 +386,10 @@ s32 e1000e_get_base_timinca(struct e1000_adapter *adapter, u32 *timinca);
 #define INCVALUE_SHIFT_25MHz	18
 #define INCPERIOD_25MHz		1
 
+#define INCVALUE_24MHz		125
+#define INCVALUE_SHIFT_24MHz	14
+#define INCPERIOD_24MHz		3
+
 /* Another drawback of scaling the incvalue by a large factor is the
  * 64-bit SYSTIM register overflows more quickly.  This is dealt with
  * by simply reading the clock before it overflows.
diff --git a/drivers/net/ethernet/intel/e1000e/ethtool.c b/drivers/net/ethernet/intel/e1000e/ethtool.c
index 11f486e4ff7b..ad6daa656d3e 100644
--- a/drivers/net/ethernet/intel/e1000e/ethtool.c
+++ b/drivers/net/ethernet/intel/e1000e/ethtool.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
@@ -1516,8 +1516,19 @@ static int e1000_set_es2lan_mac_loopback(struct e1000_adapter *adapter)
 static int e1000_setup_loopback_test(struct e1000_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	u32 rctl;
-
+	u32 rctl, fext_nvm11, tarc0;
+
+	if (hw->mac.type == e1000_pch_spt) {
+		fext_nvm11 = er32(FEXTNVM11);
+		fext_nvm11 |= E1000_FEXTNVM11_DISABLE_MULR_FIX;
+		ew32(FEXTNVM11, fext_nvm11);
+		tarc0 = er32(TARC(0));
+		/* clear bits 28 & 29 (control of MULR concurrent requests) */
+		tarc0 &= 0xcfffffff;
+		/* set bit 29 (value of MULR requests is now 2) */
+		tarc0 |= 0x20000000;
+		ew32(TARC(0), tarc0);
+	}
 	if (hw->phy.media_type == e1000_media_type_fiber ||
 	    hw->phy.media_type == e1000_media_type_internal_serdes) {
 		switch (hw->mac.type) {
@@ -1542,7 +1553,7 @@ static int e1000_setup_loopback_test(struct e1000_adapter *adapter)
 static void e1000_loopback_cleanup(struct e1000_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	u32 rctl;
+	u32 rctl, fext_nvm11, tarc0;
 	u16 phy_reg;
 
 	rctl = er32(RCTL);
@@ -1550,6 +1561,16 @@ static void e1000_loopback_cleanup(struct e1000_adapter *adapter)
 	ew32(RCTL, rctl);
 
 	switch (hw->mac.type) {
+	case e1000_pch_spt:
+		fext_nvm11 = er32(FEXTNVM11);
+		fext_nvm11 &= ~E1000_FEXTNVM11_DISABLE_MULR_FIX;
+		ew32(FEXTNVM11, fext_nvm11);
+		tarc0 = er32(TARC(0));
+		/* clear bits 28 & 29 (control of MULR concurrent requests) */
+		/* set bit 29 (value of MULR requests is now 0) */
+		tarc0 &= 0xcfffffff;
+		ew32(TARC(0), tarc0);
+		/* fall through */
 	case e1000_80003es2lan:
 		if (hw->phy.media_type == e1000_media_type_fiber ||
 		    hw->phy.media_type == e1000_media_type_internal_serdes) {
diff --git a/drivers/net/ethernet/intel/e1000e/hw.h b/drivers/net/ethernet/intel/e1000e/hw.h
index 19e8c487db06..c9da4654e9ca 100644
--- a/drivers/net/ethernet/intel/e1000e/hw.h
+++ b/drivers/net/ethernet/intel/e1000e/hw.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.c b/drivers/net/ethernet/intel/e1000e/ich8lan.c
index 9d81c0317433..91a5a0ae9cd7 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.c
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
@@ -237,17 +237,19 @@ static bool e1000_phy_is_accessible_pchlan(struct e1000_hw *hw)
 	if (ret_val)
 		return false;
 out:
-	if ((hw->mac.type == e1000_pch_lpt) ||
-	    (hw->mac.type == e1000_pch_spt)) {
-		/* Unforce SMBus mode in PHY */
-		e1e_rphy_locked(hw, CV_SMB_CTRL, &phy_reg);
-		phy_reg &= ~CV_SMB_CTRL_FORCE_SMBUS;
-		e1e_wphy_locked(hw, CV_SMB_CTRL, phy_reg);
+	if ((hw->mac.type == e1000_pch_lpt) || (hw->mac.type == e1000_pch_spt)) {
+		/* Only unforce SMBus if ME is not active */
+		if (!(er32(FWSM) & E1000_ICH_FWSM_FW_VALID)) {
+			/* Unforce SMBus mode in PHY */
+			e1e_rphy_locked(hw, CV_SMB_CTRL, &phy_reg);
+			phy_reg &= ~CV_SMB_CTRL_FORCE_SMBUS;
+			e1e_wphy_locked(hw, CV_SMB_CTRL, phy_reg);
 
-		/* Unforce SMBus mode in MAC */
-		mac_reg = er32(CTRL_EXT);
-		mac_reg &= ~E1000_CTRL_EXT_FORCE_SMBUS;
-		ew32(CTRL_EXT, mac_reg);
+			/* Unforce SMBus mode in MAC */
+			mac_reg = er32(CTRL_EXT);
+			mac_reg &= ~E1000_CTRL_EXT_FORCE_SMBUS;
+			ew32(CTRL_EXT, mac_reg);
+		}
 	}
 
 	return true;
@@ -1014,8 +1016,7 @@ static s32 e1000_platform_pm_pch_lpt(struct e1000_hw *hw, bool link)
 		u16 speed, duplex, scale = 0;
 		u16 max_snoop, max_nosnoop;
 		u16 max_ltr_enc;	/* max LTR latency encoded */
-		s64 lat_ns;	/* latency (ns) */
-		s64 value;
+		u64 value;
 		u32 rxa;
 
 		if (!hw->adapter->max_frame_size) {
@@ -1040,14 +1041,11 @@ static s32 e1000_platform_pm_pch_lpt(struct e1000_hw *hw, bool link)
 		 * 2^25*(2^10-1) ns.  The scale is encoded as 0=2^0ns,
 		 * 1=2^5ns, 2=2^10ns,...5=2^25ns.
 		 */
-		lat_ns = ((s64)rxa * 1024 -
-			  (2 * (s64)hw->adapter->max_frame_size)) * 8 * 1000;
-		if (lat_ns < 0)
-			lat_ns = 0;
-		else
-			do_div(lat_ns, speed);
+		rxa *= 512;
+		value = (rxa > hw->adapter->max_frame_size) ?
+			(rxa - hw->adapter->max_frame_size) * (16000 / speed) :
+			0;
 
-		value = lat_ns;
 		while (value > PCI_LTR_VALUE_MASK) {
 			scale++;
 			value = DIV_ROUND_UP(value, (1 << 5));
@@ -1091,6 +1089,7 @@ s32 e1000_enable_ulp_lpt_lp(struct e1000_hw *hw, bool to_sx)
 	u32 mac_reg;
 	s32 ret_val = 0;
 	u16 phy_reg;
+	u16 oem_reg = 0;
 
 	if ((hw->mac.type < e1000_pch_lpt) ||
 	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_LPT_I217_LM) ||
@@ -1132,33 +1131,37 @@ s32 e1000_enable_ulp_lpt_lp(struct e1000_hw *hw, bool to_sx)
 	if (ret_val)
 		goto out;
 
+	/* Force SMBus mode in PHY */
+	ret_val = e1000_read_phy_reg_hv_locked(hw, CV_SMB_CTRL, &phy_reg);
+	if (ret_val)
+		goto release;
+	phy_reg |= CV_SMB_CTRL_FORCE_SMBUS;
+	e1000_write_phy_reg_hv_locked(hw, CV_SMB_CTRL, phy_reg);
+
+	/* Force SMBus mode in MAC */
+	mac_reg = er32(CTRL_EXT);
+	mac_reg |= E1000_CTRL_EXT_FORCE_SMBUS;
+	ew32(CTRL_EXT, mac_reg);
+
 	/* Si workaround for ULP entry flow on i127/rev6 h/w.  Enable
 	 * LPLU and disable Gig speed when entering ULP
 	 */
 	if ((hw->phy.type == e1000_phy_i217) && (hw->phy.revision == 6)) {
 		ret_val = e1000_read_phy_reg_hv_locked(hw, HV_OEM_BITS,
-						       &phy_reg);
+						       &oem_reg);
 		if (ret_val)
 			goto release;
+
+		phy_reg = oem_reg;
 		phy_reg |= HV_OEM_BITS_LPLU | HV_OEM_BITS_GBE_DIS;
+
 		ret_val = e1000_write_phy_reg_hv_locked(hw, HV_OEM_BITS,
 							phy_reg);
+
 		if (ret_val)
 			goto release;
 	}
 
-	/* Force SMBus mode in PHY */
-	ret_val = e1000_read_phy_reg_hv_locked(hw, CV_SMB_CTRL, &phy_reg);
-	if (ret_val)
-		goto release;
-	phy_reg |= CV_SMB_CTRL_FORCE_SMBUS;
-	e1000_write_phy_reg_hv_locked(hw, CV_SMB_CTRL, phy_reg);
-
-	/* Force SMBus mode in MAC */
-	mac_reg = er32(CTRL_EXT);
-	mac_reg |= E1000_CTRL_EXT_FORCE_SMBUS;
-	ew32(CTRL_EXT, mac_reg);
-
 	/* Set Inband ULP Exit, Reset to SMBus mode and
 	 * Disable SMBus Release on PERST# in PHY
 	 */
@@ -1170,10 +1173,15 @@ s32 e1000_enable_ulp_lpt_lp(struct e1000_hw *hw, bool to_sx)
 	if (to_sx) {
 		if (er32(WUFC) & E1000_WUFC_LNKC)
 			phy_reg |= I218_ULP_CONFIG1_WOL_HOST;
+		else
+			phy_reg &= ~I218_ULP_CONFIG1_WOL_HOST;
 
 		phy_reg |= I218_ULP_CONFIG1_STICKY_ULP;
+		phy_reg &= ~I218_ULP_CONFIG1_INBAND_EXIT;
 	} else {
 		phy_reg |= I218_ULP_CONFIG1_INBAND_EXIT;
+		phy_reg &= ~I218_ULP_CONFIG1_STICKY_ULP;
+		phy_reg &= ~I218_ULP_CONFIG1_WOL_HOST;
 	}
 	e1000_write_phy_reg_hv_locked(hw, I218_ULP_CONFIG1, phy_reg);
 
@@ -1185,6 +1193,15 @@ s32 e1000_enable_ulp_lpt_lp(struct e1000_hw *hw, bool to_sx)
 	/* Commit ULP changes in PHY by starting auto ULP configuration */
 	phy_reg |= I218_ULP_CONFIG1_START;
 	e1000_write_phy_reg_hv_locked(hw, I218_ULP_CONFIG1, phy_reg);
+
+	if ((hw->phy.type == e1000_phy_i217) && (hw->phy.revision == 6) &&
+	    to_sx && (er32(STATUS) & E1000_STATUS_LU)) {
+		ret_val = e1000_write_phy_reg_hv_locked(hw, HV_OEM_BITS,
+							oem_reg);
+		if (ret_val)
+			goto release;
+	}
+
 release:
 	hw->phy.ops.release(hw);
 out:
@@ -1383,16 +1400,20 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 	if (((hw->mac.type == e1000_pch2lan) ||
 	     (hw->mac.type == e1000_pch_lpt) ||
 	     (hw->mac.type == e1000_pch_spt)) && link) {
-		u32 reg;
+		u16 speed, duplex;
 
-		reg = er32(STATUS);
+		e1000e_get_speed_and_duplex_copper(hw, &speed, &duplex);
 		tipg_reg = er32(TIPG);
 		tipg_reg &= ~E1000_TIPG_IPGT_MASK;
 
-		if (!(reg & (E1000_STATUS_FD | E1000_STATUS_SPEED_MASK))) {
+		if (duplex == HALF_DUPLEX && speed == SPEED_10) {
 			tipg_reg |= 0xFF;
 			/* Reduce Rx latency in analog PHY */
 			emi_val = 0;
+		} else if (hw->mac.type == e1000_pch_spt &&
+			   duplex == FULL_DUPLEX && speed != SPEED_1000) {
+			tipg_reg |= 0xC;
+			emi_val = 1;
 		} else {
 
 			/* Roll back the default values */
@@ -1416,14 +1437,59 @@ static s32 e1000_check_for_copper_link_ich8lan(struct e1000_hw *hw)
 
 		if (ret_val)
 			return ret_val;
+
+		if (hw->mac.type == e1000_pch_spt) {
+			u16 data;
+			u16 ptr_gap;
+
+			if (speed == SPEED_1000) {
+				ret_val = hw->phy.ops.acquire(hw);
+				if (ret_val)
+					return ret_val;
+
+				ret_val = e1e_rphy_locked(hw,
+							  PHY_REG(776, 20),
+							  &data);
+				if (ret_val) {
+					hw->phy.ops.release(hw);
+					return ret_val;
+				}
+
+				ptr_gap = (data & (0x3FF << 2)) >> 2;
+				if (ptr_gap < 0x18) {
+					data &= ~(0x3FF << 2);
+					data |= (0x18 << 2);
+					ret_val =
+					    e1e_wphy_locked(hw,
+							    PHY_REG(776, 20),
+							    data);
+				}
+				hw->phy.ops.release(hw);
+				if (ret_val)
+					return ret_val;
+			}
+		}
+	}
+
+	/* I217 Packet Loss issue:
+	 * ensure that FEXTNVM4 Beacon Duration is set correctly
+	 * on power up.
+	 * Set the Beacon Duration for I217 to 8 usec
+	 */
+	if ((hw->mac.type == e1000_pch_lpt) || (hw->mac.type == e1000_pch_spt)) {
+		u32 mac_reg;
+
+		mac_reg = er32(FEXTNVM4);
+		mac_reg &= ~E1000_FEXTNVM4_BEACON_DURATION_MASK;
+		mac_reg |= E1000_FEXTNVM4_BEACON_DURATION_8USEC;
+		ew32(FEXTNVM4, mac_reg);
 	}
 
 	/* Work-around I218 hang issue */
 	if ((hw->adapter->pdev->device == E1000_DEV_ID_PCH_LPTLP_I218_LM) ||
 	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_LPTLP_I218_V) ||
 	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_I218_LM3) ||
-	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_I218_V3) ||
-	    (hw->mac.type == e1000_pch_spt)) {
+	    (hw->adapter->pdev->device == E1000_DEV_ID_PCH_I218_V3)) {
 		ret_val = e1000_k1_workaround_lpt_lp(hw, link);
 		if (ret_val)
 			return ret_val;
@@ -1563,7 +1629,7 @@ static s32 e1000_get_variants_ich8lan(struct e1000_adapter *adapter)
 	    ((adapter->hw.mac.type >= e1000_pch2lan) &&
 	     (!(er32(CTRL_EXT) & E1000_CTRL_EXT_LSECCK)))) {
 		adapter->flags &= ~FLAG_HAS_JUMBO_FRAMES;
-		adapter->max_hw_frame_size = ETH_FRAME_LEN + ETH_FCS_LEN;
+		adapter->max_hw_frame_size = VLAN_ETH_FRAME_LEN + ETH_FCS_LEN;
 
 		hw->mac.ops.blink_led = NULL;
 	}
@@ -5681,7 +5747,7 @@ const struct e1000_info e1000_ich8_info = {
 				  | FLAG_HAS_FLASH
 				  | FLAG_APME_IN_WUC,
 	.pba			= 8,
-	.max_hw_frame_size	= ETH_FRAME_LEN + ETH_FCS_LEN,
+	.max_hw_frame_size	= VLAN_ETH_FRAME_LEN + ETH_FCS_LEN,
 	.get_variants		= e1000_get_variants_ich8lan,
 	.mac_ops		= &ich8_mac_ops,
 	.phy_ops		= &ich8_phy_ops,
@@ -5754,7 +5820,7 @@ const struct e1000_info e1000_pch2_info = {
 	.flags2			= FLAG2_HAS_PHY_STATS
 				  | FLAG2_HAS_EEE,
 	.pba			= 26,
-	.max_hw_frame_size	= 9018,
+	.max_hw_frame_size	= 9022,
 	.get_variants		= e1000_get_variants_ich8lan,
 	.mac_ops		= &ich8_mac_ops,
 	.phy_ops		= &ich8_phy_ops,
@@ -5774,7 +5840,7 @@ const struct e1000_info e1000_pch_lpt_info = {
 	.flags2			= FLAG2_HAS_PHY_STATS
 				  | FLAG2_HAS_EEE,
 	.pba			= 26,
-	.max_hw_frame_size	= 9018,
+	.max_hw_frame_size	= 9022,
 	.get_variants		= e1000_get_variants_ich8lan,
 	.mac_ops		= &ich8_mac_ops,
 	.phy_ops		= &ich8_phy_ops,
@@ -5794,7 +5860,7 @@ const struct e1000_info e1000_pch_spt_info = {
 	.flags2			= FLAG2_HAS_PHY_STATS
 				  | FLAG2_HAS_EEE,
 	.pba			= 26,
-	.max_hw_frame_size	= 9018,
+	.max_hw_frame_size	= 9022,
 	.get_variants		= e1000_get_variants_ich8lan,
 	.mac_ops		= &ich8_mac_ops,
 	.phy_ops		= &ich8_phy_ops,
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.h b/drivers/net/ethernet/intel/e1000e/ich8lan.h
index 770a573b9eea..26459853c6be 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.h
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
@@ -98,8 +98,15 @@
 #define E1000_FEXTNVM6_K1_OFF_ENABLE	0x80000000
 /* bit for disabling packet buffer read */
 #define E1000_FEXTNVM7_DISABLE_PB_READ	0x00040000
-
+#define E1000_FEXTNVM7_SIDE_CLK_UNGATE	0x00000004
 #define E1000_FEXTNVM7_DISABLE_SMB_PERST	0x00000020
+#define E1000_FEXTNVM9_IOSFSB_CLKGATE_DIS	0x00000800
+#define E1000_FEXTNVM9_IOSFSB_CLKREQ_DIS	0x00001000
+#define E1000_FEXTNVM11_DISABLE_PB_READ		0x00000200
+#define E1000_FEXTNVM11_DISABLE_MULR_FIX	0x00002000
+
+/* bit24: RXDCTL thresholds granularity: 0 - cache lines, 1 - descriptors */
+#define E1000_RXDCTL_THRESH_UNIT_DESC 0x01000000
 
 #define K1_ENTRY_LATENCY	0
 #define K1_MIN_TIME		1
diff --git a/drivers/net/ethernet/intel/e1000e/mac.c b/drivers/net/ethernet/intel/e1000e/mac.c
index 30b74d590bee..e59d7c283cd4 100644
--- a/drivers/net/ethernet/intel/e1000e/mac.c
+++ b/drivers/net/ethernet/intel/e1000e/mac.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/mac.h b/drivers/net/ethernet/intel/e1000e/mac.h
index 0513d90cdeea..8284618af9ff 100644
--- a/drivers/net/ethernet/intel/e1000e/mac.h
+++ b/drivers/net/ethernet/intel/e1000e/mac.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/manage.c b/drivers/net/ethernet/intel/e1000e/manage.c
index 06edfca1a35e..cc9b3befc2bc 100644
--- a/drivers/net/ethernet/intel/e1000e/manage.c
+++ b/drivers/net/ethernet/intel/e1000e/manage.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/manage.h b/drivers/net/ethernet/intel/e1000e/manage.h
index a8c27f98f7b0..0b9ea5952b07 100644
--- a/drivers/net/ethernet/intel/e1000e/manage.h
+++ b/drivers/net/ethernet/intel/e1000e/manage.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index c509a5c900f5..89d788d8f263 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
@@ -48,7 +48,7 @@
 
 #define DRV_EXTRAVERSION "-k"
 
-#define DRV_VERSION "2.3.2" DRV_EXTRAVERSION
+#define DRV_VERSION "3.2.5" DRV_EXTRAVERSION
 char e1000e_driver_name[] = "e1000e";
 const char e1000e_driver_version[] = DRV_VERSION;
 
@@ -3525,22 +3525,30 @@ s32 e1000e_get_base_timinca(struct e1000_adapter *adapter, u32 *timinca)
 	switch (hw->mac.type) {
 	case e1000_pch2lan:
 	case e1000_pch_lpt:
-	case e1000_pch_spt:
-		/* On I217, I218 and I219, the clock frequency is 25MHz
-		 * or 96MHz as indicated by the System Clock Frequency
-		 * Indication
-		 */
-		if (((hw->mac.type != e1000_pch_lpt) &&
-		     (hw->mac.type != e1000_pch_spt)) ||
-		    (er32(TSYNCRXCTL) & E1000_TSYNCRXCTL_SYSCFI)) {
+		if (er32(TSYNCRXCTL) & E1000_TSYNCRXCTL_SYSCFI) {
 			/* Stable 96MHz frequency */
 			incperiod = INCPERIOD_96MHz;
 			incvalue = INCVALUE_96MHz;
 			shift = INCVALUE_SHIFT_96MHz;
 			adapter->cc.shift = shift + INCPERIOD_SHIFT_96MHz;
+		} else {
+			/* Stable 25MHz frequency */
+			incperiod = INCPERIOD_25MHz;
+			incvalue = INCVALUE_25MHz;
+			shift = INCVALUE_SHIFT_25MHz;
+			adapter->cc.shift = shift;
+		}
+		break;
+	case e1000_pch_spt:
+		if (er32(TSYNCRXCTL) & E1000_TSYNCRXCTL_SYSCFI) {
+			/* Stable 24MHz frequency */
+			incperiod = INCPERIOD_24MHz;
+			incvalue = INCVALUE_24MHz;
+			shift = INCVALUE_SHIFT_24MHz;
+			adapter->cc.shift = shift;
 			break;
 		}
-		/* fall-through */
+		return -EINVAL;
 	case e1000_82574:
 	case e1000_82583:
 		/* Stable 25MHz frequency */
@@ -3788,6 +3796,108 @@ static void e1000_power_down_phy(struct e1000_adapter *adapter)
 }
 
 /**
+ * e1000_flush_tx_ring - remove all descriptors from the tx_ring
+ *
+ * We want to clear all pending descriptors from the TX ring.
+ * zeroing happens when the HW reads the regs. We  assign the ring itself as
+ * the data of the next descriptor. We don't care about the data we are about
+ * to reset the HW.
+ */
+static void e1000_flush_tx_ring(struct e1000_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	struct e1000_ring *tx_ring = adapter->tx_ring;
+	struct e1000_tx_desc *tx_desc = NULL;
+	u32 tdt, tctl, txd_lower = E1000_TXD_CMD_IFCS;
+	u16 size = 512;
+
+	tctl = er32(TCTL);
+	ew32(TCTL, tctl | E1000_TCTL_EN);
+	tdt = er32(TDT(0));
+	BUG_ON(tdt != tx_ring->next_to_use);
+	tx_desc =  E1000_TX_DESC(*tx_ring, tx_ring->next_to_use);
+	tx_desc->buffer_addr = tx_ring->dma;
+
+	tx_desc->lower.data = cpu_to_le32(txd_lower | size);
+	tx_desc->upper.data = 0;
+	/* flush descriptors to memory before notifying the HW */
+	wmb();
+	tx_ring->next_to_use++;
+	if (tx_ring->next_to_use == tx_ring->count)
+		tx_ring->next_to_use = 0;
+	ew32(TDT(0), tx_ring->next_to_use);
+	mmiowb();
+	usleep_range(200, 250);
+}
+
+/**
+ * e1000_flush_rx_ring - remove all descriptors from the rx_ring
+ *
+ * Mark all descriptors in the RX ring as consumed and disable the rx ring
+ */
+static void e1000_flush_rx_ring(struct e1000_adapter *adapter)
+{
+	u32 rctl, rxdctl;
+	struct e1000_hw *hw = &adapter->hw;
+
+	rctl = er32(RCTL);
+	ew32(RCTL, rctl & ~E1000_RCTL_EN);
+	e1e_flush();
+	usleep_range(100, 150);
+
+	rxdctl = er32(RXDCTL(0));
+	/* zero the lower 14 bits (prefetch and host thresholds) */
+	rxdctl &= 0xffffc000;
+
+	/* update thresholds: prefetch threshold to 31, host threshold to 1
+	 * and make sure the granularity is "descriptors" and not "cache lines"
+	 */
+	rxdctl |= (0x1F | (1 << 8) | E1000_RXDCTL_THRESH_UNIT_DESC);
+
+	ew32(RXDCTL(0), rxdctl);
+	/* momentarily enable the RX ring for the changes to take effect */
+	ew32(RCTL, rctl | E1000_RCTL_EN);
+	e1e_flush();
+	usleep_range(100, 150);
+	ew32(RCTL, rctl & ~E1000_RCTL_EN);
+}
+
+/**
+ * e1000_flush_desc_rings - remove all descriptors from the descriptor rings
+ *
+ * In i219, the descriptor rings must be emptied before resetting the HW
+ * or before changing the device state to D3 during runtime (runtime PM).
+ *
+ * Failure to do this will cause the HW to enter a unit hang state which can
+ * only be released by PCI reset on the device
+ *
+ */
+
+static void e1000_flush_desc_rings(struct e1000_adapter *adapter)
+{
+	u16 hang_state;
+	u32 fext_nvm11, tdlen;
+	struct e1000_hw *hw = &adapter->hw;
+
+	/* First, disable MULR fix in FEXTNVM11 */
+	fext_nvm11 = er32(FEXTNVM11);
+	fext_nvm11 |= E1000_FEXTNVM11_DISABLE_MULR_FIX;
+	ew32(FEXTNVM11, fext_nvm11);
+	/* do nothing if we're not in faulty state, or if the queue is empty */
+	tdlen = er32(TDLEN(0));
+	pci_read_config_word(adapter->pdev, PCICFG_DESC_RING_STATUS,
+			     &hang_state);
+	if (!(hang_state & FLUSH_DESC_REQUIRED) || !tdlen)
+		return;
+	e1000_flush_tx_ring(adapter);
+	/* recheck, maybe the fault is caused by the rx ring */
+	pci_read_config_word(adapter->pdev, PCICFG_DESC_RING_STATUS,
+			     &hang_state);
+	if (hang_state & FLUSH_DESC_REQUIRED)
+		e1000_flush_rx_ring(adapter);
+}
+
+/**
  * e1000e_reset - bring the hardware into a known good state
  *
  * This function boots the hardware and enables some settings that
@@ -3807,7 +3917,7 @@ void e1000e_reset(struct e1000_adapter *adapter)
 	/* reset Packet Buffer Allocation to default */
 	ew32(PBA, pba);
 
-	if (adapter->max_frame_size > ETH_FRAME_LEN + ETH_FCS_LEN) {
+	if (adapter->max_frame_size > (VLAN_ETH_FRAME_LEN + ETH_FCS_LEN)) {
 		/* To maintain wire speed transmits, the Tx FIFO should be
 		 * large enough to accommodate two full transmit packets,
 		 * rounded up to the next 1KB and expressed in KB.  Likewise,
@@ -3943,6 +4053,8 @@ void e1000e_reset(struct e1000_adapter *adapter)
 		}
 	}
 
+	if (hw->mac.type == e1000_pch_spt)
+		e1000_flush_desc_rings(adapter);
 	/* Allow time for pending master requests to run */
 	mac->ops.reset_hw(hw);
 
@@ -4016,6 +4128,20 @@ void e1000e_reset(struct e1000_adapter *adapter)
 		phy_data &= ~IGP02E1000_PM_SPD;
 		e1e_wphy(hw, IGP02E1000_PHY_POWER_MGMT, phy_data);
 	}
+	if (hw->mac.type == e1000_pch_spt && adapter->int_mode == 0) {
+		u32 reg;
+
+		/* Fextnvm7 @ 0xe4[2] = 1 */
+		reg = er32(FEXTNVM7);
+		reg |= E1000_FEXTNVM7_SIDE_CLK_UNGATE;
+		ew32(FEXTNVM7, reg);
+		/* Fextnvm9 @ 0x5bb4[13:12] = 11 */
+		reg = er32(FEXTNVM9);
+		reg |= E1000_FEXTNVM9_IOSFSB_CLKGATE_DIS |
+		       E1000_FEXTNVM9_IOSFSB_CLKREQ_DIS;
+		ew32(FEXTNVM9, reg);
+	}
+
 }
 
 int e1000e_up(struct e1000_adapter *adapter)
@@ -4115,8 +4241,6 @@ void e1000e_down(struct e1000_adapter *adapter, bool reset)
 	spin_unlock(&adapter->stats64_lock);
 
 	e1000e_flush_descriptors(adapter);
-	e1000_clean_tx_ring(adapter->tx_ring);
-	e1000_clean_rx_ring(adapter->rx_ring);
 
 	adapter->link_speed = 0;
 	adapter->link_duplex = 0;
@@ -4127,8 +4251,14 @@ void e1000e_down(struct e1000_adapter *adapter, bool reset)
 	    e1000_lv_jumbo_workaround_ich8lan(hw, false))
 		e_dbg("failed to disable jumbo frame workaround mode\n");
 
-	if (reset && !pci_channel_offline(adapter->pdev))
-		e1000e_reset(adapter);
+	if (!pci_channel_offline(adapter->pdev)) {
+		if (reset)
+			e1000e_reset(adapter);
+		else if (hw->mac.type == e1000_pch_spt)
+			e1000_flush_desc_rings(adapter);
+	}
+	e1000_clean_tx_ring(adapter->tx_ring);
+	e1000_clean_rx_ring(adapter->rx_ring);
 }
 
 void e1000e_reinit_locked(struct e1000_adapter *adapter)
@@ -4151,9 +4281,16 @@ static cycle_t e1000e_cyclecounter_read(const struct cyclecounter *cc)
 						     cc);
 	struct e1000_hw *hw = &adapter->hw;
 	cycle_t systim, systim_next;
+	/* SYSTIMH latching upon SYSTIML read does not work well. To fix that
+	 * we don't want to allow overflow of SYSTIML and a change to SYSTIMH
+	 * to occur between reads, so if we read a vale close to overflow, we
+	 * wait for overflow to occur and read both registers when its safe.
+	 */
+	u32 systim_overflow_latch_fix = 0x3FFFFFFF;
 
-	/* latch SYSTIMH on read of SYSTIML */
-	systim = (cycle_t)er32(SYSTIML);
+	do {
+		systim = (cycle_t)er32(SYSTIML);
+	} while (systim > systim_overflow_latch_fix);
 	systim |= (cycle_t)er32(SYSTIMH) << 32;
 
 	if ((hw->mac.type == e1000_82574) || (hw->mac.type == e1000_82583)) {
@@ -4196,9 +4333,9 @@ static int e1000_sw_init(struct e1000_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
 
-	adapter->rx_buffer_len = ETH_FRAME_LEN + VLAN_HLEN + ETH_FCS_LEN;
+	adapter->rx_buffer_len = VLAN_ETH_FRAME_LEN + ETH_FCS_LEN;
 	adapter->rx_ps_bsize0 = 128;
-	adapter->max_frame_size = netdev->mtu + ETH_HLEN + ETH_FCS_LEN;
+	adapter->max_frame_size = netdev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN;
 	adapter->min_frame_size = ETH_ZLEN + ETH_FCS_LEN;
 	adapter->tx_ring_count = E1000_DEFAULT_TXD;
 	adapter->rx_ring_count = E1000_DEFAULT_RXD;
@@ -5781,17 +5918,17 @@ struct rtnl_link_stats64 *e1000e_get_stats64(struct net_device *netdev,
 static int e1000_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
-	int max_frame = new_mtu + VLAN_HLEN + ETH_HLEN + ETH_FCS_LEN;
+	int max_frame = new_mtu + VLAN_ETH_HLEN + ETH_FCS_LEN;
 
 	/* Jumbo frame support */
-	if ((max_frame > ETH_FRAME_LEN + ETH_FCS_LEN) &&
+	if ((max_frame > (VLAN_ETH_FRAME_LEN + ETH_FCS_LEN)) &&
 	    !(adapter->flags & FLAG_HAS_JUMBO_FRAMES)) {
 		e_err("Jumbo Frames not supported.\n");
 		return -EINVAL;
 	}
 
 	/* Supported frame sizes */
-	if ((new_mtu < ETH_ZLEN + ETH_FCS_LEN + VLAN_HLEN) ||
+	if ((new_mtu < (VLAN_ETH_ZLEN + ETH_FCS_LEN)) ||
 	    (max_frame > adapter->max_hw_frame_size)) {
 		e_err("Unsupported MTU setting\n");
 		return -EINVAL;
@@ -5831,10 +5968,8 @@ static int e1000_change_mtu(struct net_device *netdev, int new_mtu)
 		adapter->rx_buffer_len = 4096;
 
 	/* adjust allocation if LPE protects us, and we aren't using SBP */
-	if ((max_frame == ETH_FRAME_LEN + ETH_FCS_LEN) ||
-	    (max_frame == ETH_FRAME_LEN + VLAN_HLEN + ETH_FCS_LEN))
-		adapter->rx_buffer_len = ETH_FRAME_LEN + VLAN_HLEN
-		    + ETH_FCS_LEN;
+	if (max_frame <= (VLAN_ETH_FRAME_LEN + ETH_FCS_LEN))
+		adapter->rx_buffer_len = VLAN_ETH_FRAME_LEN + ETH_FCS_LEN;
 
 	if (netif_running(netdev))
 		e1000e_up(adapter);
@@ -6219,13 +6354,14 @@ static int __e1000_shutdown(struct pci_dev *pdev, bool runtime)
 }
 
 /**
- * e1000e_disable_aspm - Disable ASPM states
+ * __e1000e_disable_aspm - Disable ASPM states
  * @pdev: pointer to PCI device struct
  * @state: bit-mask of ASPM states to disable
+ * @locked: indication if this context holds pci_bus_sem locked.
  *
  * Some devices *must* have certain ASPM states disabled per hardware errata.
  **/
-static void e1000e_disable_aspm(struct pci_dev *pdev, u16 state)
+static void __e1000e_disable_aspm(struct pci_dev *pdev, u16 state, int locked)
 {
 	struct pci_dev *parent = pdev->bus->self;
 	u16 aspm_dis_mask = 0;
@@ -6264,7 +6400,10 @@ static void e1000e_disable_aspm(struct pci_dev *pdev, u16 state)
 		 "L1" : "");
 
 #ifdef CONFIG_PCIEASPM
-	pci_disable_link_state_locked(pdev, state);
+	if (locked)
+		pci_disable_link_state_locked(pdev, state);
+	else
+		pci_disable_link_state(pdev, state);
 
 	/* Double-check ASPM control.  If not disabled by the above, the
 	 * BIOS is preventing that from happening (or CONFIG_PCIEASPM is
@@ -6287,6 +6426,32 @@ static void e1000e_disable_aspm(struct pci_dev *pdev, u16 state)
 					   aspm_dis_mask);
 }
 
+/**
+ * e1000e_disable_aspm - Disable ASPM states.
+ * @pdev: pointer to PCI device struct
+ * @state: bit-mask of ASPM states to disable
+ *
+ * This function acquires the pci_bus_sem!
+ * Some devices *must* have certain ASPM states disabled per hardware errata.
+ **/
+static void e1000e_disable_aspm(struct pci_dev *pdev, u16 state)
+{
+	__e1000e_disable_aspm(pdev, state, 0);
+}
+
+/**
+ * e1000e_disable_aspm_locked   Disable ASPM states.
+ * @pdev: pointer to PCI device struct
+ * @state: bit-mask of ASPM states to disable
+ *
+ * This function must be called with pci_bus_sem acquired!
+ * Some devices *must* have certain ASPM states disabled per hardware errata.
+ **/
+static void e1000e_disable_aspm_locked(struct pci_dev *pdev, u16 state)
+{
+	__e1000e_disable_aspm(pdev, state, 1);
+}
+
 #ifdef CONFIG_PM
 static int __e1000_resume(struct pci_dev *pdev)
 {
@@ -6300,7 +6465,7 @@ static int __e1000_resume(struct pci_dev *pdev)
 	if (adapter->flags2 & FLAG2_DISABLE_ASPM_L1)
 		aspm_disable_flag |= PCIE_LINK_STATE_L1;
 	if (aspm_disable_flag)
-		e1000e_disable_aspm(pdev, aspm_disable_flag);
+		e1000e_disable_aspm_locked(pdev, aspm_disable_flag);
 
 	pci_set_master(pdev);
 
@@ -6678,6 +6843,19 @@ static void e1000_eeprom_checks(struct e1000_adapter *adapter)
 	}
 }
 
+static netdev_features_t e1000_fix_features(struct net_device *netdev,
+					    netdev_features_t features)
+{
+	struct e1000_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+
+	/* Jumbo frame workaround on 82579 and newer requires CRC be stripped */
+	if ((hw->mac.type >= e1000_pch2lan) && (netdev->mtu > ETH_DATA_LEN))
+		features &= ~NETIF_F_RXFCS;
+
+	return features;
+}
+
 static int e1000_set_features(struct net_device *netdev,
 			      netdev_features_t features)
 {
@@ -6734,6 +6912,7 @@ static const struct net_device_ops e1000e_netdev_ops = {
 	.ndo_poll_controller	= e1000_netpoll,
 #endif
 	.ndo_set_features = e1000_set_features,
+	.ndo_fix_features = e1000_fix_features,
 };
 
 /**
@@ -7289,7 +7468,7 @@ static int __init e1000_init_module(void)
 
 	pr_info("Intel(R) PRO/1000 Network Driver - %s\n",
 		e1000e_driver_version);
-	pr_info("Copyright(c) 1999 - 2014 Intel Corporation.\n");
+	pr_info("Copyright(c) 1999 - 2015 Intel Corporation.\n");
 	ret = pci_register_driver(&e1000_driver);
 
 	return ret;
diff --git a/drivers/net/ethernet/intel/e1000e/nvm.c b/drivers/net/ethernet/intel/e1000e/nvm.c
index fa6b1036a327..49f205c023bf 100644
--- a/drivers/net/ethernet/intel/e1000e/nvm.c
+++ b/drivers/net/ethernet/intel/e1000e/nvm.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/nvm.h b/drivers/net/ethernet/intel/e1000e/nvm.h
index 342bf69efab5..5d46967e0d1f 100644
--- a/drivers/net/ethernet/intel/e1000e/nvm.h
+++ b/drivers/net/ethernet/intel/e1000e/nvm.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/param.c b/drivers/net/ethernet/intel/e1000e/param.c
index aa1923f7ebdd..6d8c39abee16 100644
--- a/drivers/net/ethernet/intel/e1000e/param.c
+++ b/drivers/net/ethernet/intel/e1000e/param.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/phy.c b/drivers/net/ethernet/intel/e1000e/phy.c
index b2005e13fb01..de13aeacae97 100644
--- a/drivers/net/ethernet/intel/e1000e/phy.c
+++ b/drivers/net/ethernet/intel/e1000e/phy.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/phy.h b/drivers/net/ethernet/intel/e1000e/phy.h
index 537d2780b408..55bfe473514d 100644
--- a/drivers/net/ethernet/intel/e1000e/phy.h
+++ b/drivers/net/ethernet/intel/e1000e/phy.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/ptp.c b/drivers/net/ethernet/intel/e1000e/ptp.c
index 8d7b21dc7e19..25a0ad5102d6 100644
--- a/drivers/net/ethernet/intel/e1000e/ptp.c
+++ b/drivers/net/ethernet/intel/e1000e/ptp.c
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
diff --git a/drivers/net/ethernet/intel/e1000e/regs.h b/drivers/net/ethernet/intel/e1000e/regs.h
index 85eefc4832ba..b24e5fee17f2 100644
--- a/drivers/net/ethernet/intel/e1000e/regs.h
+++ b/drivers/net/ethernet/intel/e1000e/regs.h
@@ -1,5 +1,5 @@
 /* Intel PRO/1000 Linux driver
- * Copyright(c) 1999 - 2014 Intel Corporation.
+ * Copyright(c) 1999 - 2015 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,
@@ -38,6 +38,8 @@
 #define E1000_FEXTNVM4	0x00024	/* Future Extended NVM 4 - RW */
 #define E1000_FEXTNVM6	0x00010	/* Future Extended NVM 6 - RW */
 #define E1000_FEXTNVM7	0x000E4	/* Future Extended NVM 7 - RW */
+#define E1000_FEXTNVM9	0x5BB4  /* Future Extended NVM 9 - RW */
+#define E1000_FEXTNVM11	0x5BBC  /* Future Extended NVM 11 - RW */
 #define E1000_PCIEANACFG	0x00F18	/* PCIE Analog Config */
 #define E1000_FCT	0x00030	/* Flow Control Type - RW */
 #define E1000_VET	0x00038	/* VLAN Ether Type - RW */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
index 4b9d9f88af70..c6dc9683429e 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
@@ -124,7 +124,7 @@ static void fm10k_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 {
 	struct fm10k_intfc *interface = netdev_priv(dev);
 	char *p = (char *)data;
-	int i;
+	unsigned int i;
 
 	switch (stringset) {
 	case ETH_SS_TEST:
@@ -143,12 +143,13 @@ static void fm10k_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 			p += ETH_GSTRING_LEN;
 		}
 
-		if (interface->hw.mac.type != fm10k_mac_vf)
+		if (interface->hw.mac.type != fm10k_mac_vf) {
 			for (i = 0; i < FM10K_PF_STATS_LEN; i++) {
 				memcpy(p, fm10k_gstrings_pf_stats[i].stat_string,
 				       ETH_GSTRING_LEN);
 				p += ETH_GSTRING_LEN;
 			}
+		}
 
 		for (i = 0; i < interface->hw.mac.max_queues; i++) {
 			sprintf(p, "tx_queue_%u_packets", i);
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_iov.c b/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
index 5b08e6284a3c..94571e6e790c 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
@@ -400,11 +400,31 @@ int fm10k_iov_configure(struct pci_dev *pdev, int num_vfs)
 	return num_vfs;
 }
 
+static inline void fm10k_reset_vf_info(struct fm10k_intfc *interface,
+				       struct fm10k_vf_info *vf_info)
+{
+	struct fm10k_hw *hw = &interface->hw;
+
+	/* assigning the MAC address will send a mailbox message */
+	fm10k_mbx_lock(interface);
+
+	/* disable LPORT for this VF which clears switch rules */
+	hw->iov.ops.reset_lport(hw, vf_info);
+
+	/* assign new MAC+VLAN for this VF */
+	hw->iov.ops.assign_default_mac_vlan(hw, vf_info);
+
+	/* re-enable the LPORT for this VF */
+	hw->iov.ops.set_lport(hw, vf_info, vf_info->vf_idx,
+			      FM10K_VF_FLAG_MULTI_CAPABLE);
+
+	fm10k_mbx_unlock(interface);
+}
+
 int fm10k_ndo_set_vf_mac(struct net_device *netdev, int vf_idx, u8 *mac)
 {
 	struct fm10k_intfc *interface = netdev_priv(netdev);
 	struct fm10k_iov_data *iov_data = interface->iov_data;
-	struct fm10k_hw *hw = &interface->hw;
 	struct fm10k_vf_info *vf_info;
 
 	/* verify SR-IOV is active and that vf idx is valid */
@@ -419,13 +439,7 @@ int fm10k_ndo_set_vf_mac(struct net_device *netdev, int vf_idx, u8 *mac)
 	vf_info = &iov_data->vf_info[vf_idx];
 	ether_addr_copy(vf_info->mac, mac);
 
-	/* assigning the MAC will send a mailbox message so lock is needed */
-	fm10k_mbx_lock(interface);
-
-	/* assign MAC address to VF */
-	hw->iov.ops.assign_default_mac_vlan(hw, vf_info);
-
-	fm10k_mbx_unlock(interface);
+	fm10k_reset_vf_info(interface, vf_info);
 
 	return 0;
 }
@@ -455,16 +469,10 @@ int fm10k_ndo_set_vf_vlan(struct net_device *netdev, int vf_idx, u16 vid,
 	/* record default VLAN ID for VF */
 	vf_info->pf_vid = vid;
 
-	/* assigning the VLAN will send a mailbox message so lock is needed */
-	fm10k_mbx_lock(interface);
-
 	/* Clear the VLAN table for the VF */
 	hw->mac.ops.update_vlan(hw, FM10K_VLAN_ALL, vf_info->vsi, false);
 
-	/* Update VF assignment and trigger reset */
-	hw->iov.ops.assign_default_mac_vlan(hw, vf_info);
-
-	fm10k_mbx_unlock(interface);
+	fm10k_reset_vf_info(interface, vf_info);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_main.c b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
index c754b2027281..982fdcdc795b 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_main.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
@@ -269,16 +269,19 @@ static bool fm10k_add_rx_frag(struct fm10k_rx_buffer *rx_buffer,
 			      struct sk_buff *skb)
 {
 	struct page *page = rx_buffer->page;
+	unsigned char *va = page_address(page) + rx_buffer->page_offset;
 	unsigned int size = le16_to_cpu(rx_desc->w.length);
 #if (PAGE_SIZE < 8192)
 	unsigned int truesize = FM10K_RX_BUFSZ;
 #else
-	unsigned int truesize = ALIGN(size, L1_CACHE_BYTES);
+	unsigned int truesize = SKB_DATA_ALIGN(size);
 #endif
+	unsigned int pull_len;
 
-	if ((size <= FM10K_RX_HDR_LEN) && !skb_is_nonlinear(skb)) {
-		unsigned char *va = page_address(page) + rx_buffer->page_offset;
+	if (unlikely(skb_is_nonlinear(skb)))
+		goto add_tail_frag;
 
+	if (likely(size <= FM10K_RX_HDR_LEN)) {
 		memcpy(__skb_put(skb, size), va, ALIGN(size, sizeof(long)));
 
 		/* page is not reserved, we can reuse buffer as-is */
@@ -290,8 +293,21 @@ static bool fm10k_add_rx_frag(struct fm10k_rx_buffer *rx_buffer,
 		return false;
 	}
 
+	/* we need the header to contain the greater of either ETH_HLEN or
+	 * 60 bytes if the skb->len is less than 60 for skb_pad.
+	 */
+	pull_len = eth_get_headlen(va, FM10K_RX_HDR_LEN);
+
+	/* align pull length to size of long to optimize memcpy performance */
+	memcpy(__skb_put(skb, pull_len), va, ALIGN(pull_len, sizeof(long)));
+
+	/* update all of the pointers */
+	va += pull_len;
+	size -= pull_len;
+
+add_tail_frag:
 	skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
-			rx_buffer->page_offset, size, truesize);
+			(unsigned long)va & ~PAGE_MASK, size, truesize);
 
 	return fm10k_can_reuse_rx_page(rx_buffer, page, truesize);
 }
@@ -518,44 +534,6 @@ static bool fm10k_is_non_eop(struct fm10k_ring *rx_ring,
 }
 
 /**
- * fm10k_pull_tail - fm10k specific version of skb_pull_tail
- * @skb: pointer to current skb being adjusted
- *
- * This function is an fm10k specific version of __pskb_pull_tail.  The
- * main difference between this version and the original function is that
- * this function can make several assumptions about the state of things
- * that allow for significant optimizations versus the standard function.
- * As a result we can do things like drop a frag and maintain an accurate
- * truesize for the skb.
- */
-static void fm10k_pull_tail(struct sk_buff *skb)
-{
-	struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[0];
-	unsigned char *va;
-	unsigned int pull_len;
-
-	/* it is valid to use page_address instead of kmap since we are
-	 * working with pages allocated out of the lomem pool per
-	 * alloc_page(GFP_ATOMIC)
-	 */
-	va = skb_frag_address(frag);
-
-	/* we need the header to contain the greater of either ETH_HLEN or
-	 * 60 bytes if the skb->len is less than 60 for skb_pad.
-	 */
-	pull_len = eth_get_headlen(va, FM10K_RX_HDR_LEN);
-
-	/* align pull length to size of long to optimize memcpy performance */
-	skb_copy_to_linear_data(skb, va, ALIGN(pull_len, sizeof(long)));
-
-	/* update all of the pointers */
-	skb_frag_size_sub(frag, pull_len);
-	frag->page_offset += pull_len;
-	skb->data_len -= pull_len;
-	skb->tail += pull_len;
-}
-
-/**
  * fm10k_cleanup_headers - Correct corrupted or empty headers
  * @rx_ring: rx descriptor ring packet is being transacted on
  * @rx_desc: pointer to the EOP Rx descriptor
@@ -580,10 +558,6 @@ static bool fm10k_cleanup_headers(struct fm10k_ring *rx_ring,
 		return true;
 	}
 
-	/* place header in linear portion of buffer */
-	if (skb_is_nonlinear(skb))
-		fm10k_pull_tail(skb);
-
 	/* if eth_skb_pad returns an error the skb was freed */
 	if (eth_skb_pad(skb))
 		return true;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
index 1b2738380518..1a4b52637de9 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
@@ -1259,16 +1259,11 @@ static s32 fm10k_mbx_process_error(struct fm10k_hw *hw,
 				   struct fm10k_mbx_info *mbx)
 {
 	const u32 *hdr = &mbx->mbx_hdr;
-	s32 err_no;
 	u16 head;
 
 	/* we will need to pull all of the fields for verification */
 	head = FM10K_MSG_HDR_FIELD_GET(*hdr, HEAD);
 
-	/* we only have lower 10 bits of error number so add upper bits */
-	err_no = FM10K_MSG_HDR_FIELD_GET(*hdr, ERR_NO);
-	err_no |= ~FM10K_MSG_HDR_MASK(ERR_NO);
-
 	switch (mbx->state) {
 	case FM10K_STATE_OPEN:
 	case FM10K_STATE_DISCONNECT:
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
index 2f4f41b7eae7..99228bf46c12 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
@@ -923,18 +923,12 @@ static int __fm10k_mc_sync(struct net_device *dev,
 	struct fm10k_intfc *interface = netdev_priv(dev);
 	struct fm10k_hw *hw = &interface->hw;
 	u16 vid, glort = interface->glort;
-	s32 err;
-
-	if (!is_multicast_ether_addr(addr))
-		return -EADDRNOTAVAIL;
 
 	/* update table with current entries */
 	for (vid = hw->mac.default_vid ? fm10k_find_next_vlan(interface, 0) : 0;
 	     vid < VLAN_N_VID;
 	     vid = fm10k_find_next_vlan(interface, vid)) {
-		err = hw->mac.ops.update_mc_addr(hw, glort, addr, vid, sync);
-		if (err)
-			return err;
+		hw->mac.ops.update_mc_addr(hw, glort, addr, vid, sync);
 	}
 
 	return 0;
@@ -1339,8 +1333,7 @@ static void fm10k_dfwd_del_station(struct net_device *dev, void *priv)
 	dglort.rss_l = fls(interface->ring_feature[RING_F_RSS].mask);
 	dglort.pc_l = fls(interface->ring_feature[RING_F_QOS].mask);
 	dglort.glort = interface->glort;
-	if (l2_accel)
-		dglort.shared_l = fls(l2_accel->size);
+	dglort.shared_l = fls(l2_accel->size);
 	hw->mac.ops.configure_dglort_map(hw, &dglort);
 
 	/* If table is empty remove it */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
index df9fda38bdd1..ce53ff25f88d 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
@@ -1559,6 +1559,7 @@ void fm10k_down(struct fm10k_intfc *interface)
 
 	/* free any buffers still on the rings */
 	fm10k_clean_all_tx_rings(interface);
+	fm10k_clean_all_rx_rings(interface);
 }
 
 /**
@@ -1740,30 +1741,18 @@ static int fm10k_probe(struct pci_dev *pdev,
 	struct fm10k_intfc *interface;
 	struct fm10k_hw *hw;
 	int err;
-	u64 dma_mask;
 
 	err = pci_enable_device_mem(pdev);
 	if (err)
 		return err;
 
-	/* By default fm10k only supports a 48 bit DMA mask */
-	dma_mask = DMA_BIT_MASK(48) | dma_get_required_mask(&pdev->dev);
-
-	if ((dma_mask <= DMA_BIT_MASK(32)) ||
-	    dma_set_mask_and_coherent(&pdev->dev, dma_mask)) {
-		dma_mask &= DMA_BIT_MASK(32);
-
+	err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(48));
+	if (err)
 		err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
-		err = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
-		if (err) {
-			err = dma_set_coherent_mask(&pdev->dev,
-						    DMA_BIT_MASK(32));
-			if (err) {
-				dev_err(&pdev->dev,
-					"No usable DMA configuration, aborting\n");
-				goto err_dma;
-			}
-		}
+	if (err) {
+		dev_err(&pdev->dev,
+			"DMA configuration failed: %d\n", err);
+		goto err_dma;
 	}
 
 	err = pci_request_selected_regions(pdev,
@@ -1772,7 +1761,7 @@ static int fm10k_probe(struct pci_dev *pdev,
 					   fm10k_driver_name);
 	if (err) {
 		dev_err(&pdev->dev,
-			"pci_request_selected_regions failed 0x%x\n", err);
+			"pci_request_selected_regions failed: %d\n", err);
 		goto err_pci_reg;
 	}
 
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
index 891e21874b2a..3ca0233b3ea2 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
@@ -1046,6 +1046,12 @@ static s32 fm10k_iov_reset_resources_pf(struct fm10k_hw *hw,
 		fm10k_write_reg(hw, FM10K_RQMAP(qmap_idx + i), vf_q_idx + i);
 	}
 
+	/* repeat the first ring for all the remaining VF rings */
+	for (i = queues_per_pool; i < qmap_stride; i++) {
+		fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx + i), vf_q_idx);
+		fm10k_write_reg(hw, FM10K_RQMAP(qmap_idx + i), vf_q_idx);
+	}
+
 	return 0;
 }
 
@@ -1345,6 +1351,14 @@ s32 fm10k_iov_msg_lport_state_pf(struct fm10k_hw *hw, u32 **results,
 			err = fm10k_update_lport_state_pf(hw, vf_info->glort,
 							  1, false);
 
+		/* we need to clear VF_FLAG_ENABLED flags in order to ensure
+		 * that we actually re-enable the LPORT state below. Note that
+		 * this has no impact if the VF is already disabled, as the
+		 * flags are already cleared.
+		 */
+		if (!err)
+			vf_info->vf_flags = FM10K_VF_FLAG_CAPABLE(vf_info);
+
 		/* when enabling the port we should reset the rate limiters */
 		hw->iov.ops.configure_tc(hw, vf_info->vf_idx, vf_info->rate);
 
@@ -1786,8 +1800,8 @@ static s32 fm10k_adjust_systime_pf(struct fm10k_hw *hw, s32 ppb)
 	if (systime_adjust > FM10K_SW_SYSTIME_ADJUST_MASK)
 		return FM10K_ERR_PARAM;
 
-	if (ppb < 0)
-		systime_adjust |= FM10K_SW_SYSTIME_ADJUST_DIR_NEGATIVE;
+	if (ppb > 0)
+		systime_adjust |= FM10K_SW_SYSTIME_ADJUST_DIR_POSITIVE;
 
 	fm10k_write_sw_reg(hw, FM10K_SW_SYSTIME_ADJUST, (u32)systime_adjust);
 
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pf.h b/drivers/net/ethernet/intel/fm10k/fm10k_pf.h
index 7ab1db4fff32..40a0dbc62a04 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pf.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pf.h
@@ -81,26 +81,26 @@ struct fm10k_mac_update {
 	__le16	glort;
 	u8	flags;
 	u8	action;
-};
+} __packed;
 
 struct fm10k_global_table_data {
 	__le32	used;
 	__le32	avail;
-};
+} __packed;
 
 struct fm10k_swapi_error {
 	__le32				status;
 	struct fm10k_global_table_data	mac;
 	struct fm10k_global_table_data	nexthop;
 	struct fm10k_global_table_data	ffu;
-};
+} __packed;
 
 struct fm10k_swapi_1588_timestamp {
 	__le64 egress;
 	__le64 ingress;
 	__le16 dglort;
 	__le16 sglort;
-};
+} __packed;
 
 s32 fm10k_msg_lport_map_pf(struct fm10k_hw *, u32 **, struct fm10k_mbx_info *);
 extern const struct fm10k_tlv_attr fm10k_lport_map_msg_attr[];
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c b/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c
index 9043633c3e50..b4945e8abe03 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c
@@ -70,16 +70,16 @@ void fm10k_ts_tx_enqueue(struct fm10k_intfc *interface, struct sk_buff *skb)
 	 * if none are present then insert skb in tail of list
 	 */
 	skb = fm10k_ts_tx_skb(interface, FM10K_CB(clone)->fi.w.dglort);
-	if (!skb)
+	if (!skb) {
+		skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
 		__skb_queue_tail(list, clone);
+	}
 
 	spin_unlock_irqrestore(&list->lock, flags);
 
 	/* if list is already has one then we just free the clone */
 	if (skb)
-		kfree_skb(skb);
-	else
-		skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
+		dev_kfree_skb(clone);
 }
 
 void fm10k_ts_tx_hwtstamp(struct fm10k_intfc *interface, __le16 dglort,
@@ -103,9 +103,10 @@ void fm10k_ts_tx_hwtstamp(struct fm10k_intfc *interface, __le16 dglort,
 	if (!skb)
 		return;
 
-	/* timestamp the sk_buff and return it to the socket */
+	/* timestamp the sk_buff and free out copy */
 	fm10k_systime_to_hwtstamp(interface, &shhwtstamps, systime);
-	skb_complete_tx_timestamp(skb, &shhwtstamps);
+	skb_tstamp_tx(skb, &shhwtstamps);
+	dev_kfree_skb_any(skb);
 }
 
 void fm10k_ts_tx_subtask(struct fm10k_intfc *interface)
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_type.h b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
index 4af96686c584..2a17d82fa37d 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_type.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
@@ -369,7 +369,7 @@ struct fm10k_hw;
 /* Registers contained in BAR 4 for Switch management */
 #define FM10K_SW_SYSTIME_ADJUST	0x0224D
 #define FM10K_SW_SYSTIME_ADJUST_MASK		0x3FFFFFFF
-#define FM10K_SW_SYSTIME_ADJUST_DIR_NEGATIVE	0x80000000
+#define FM10K_SW_SYSTIME_ADJUST_DIR_POSITIVE	0x80000000
 #define FM10K_SW_SYSTIME_PULSE(_n)	((_n) + 0x02252)
 
 enum fm10k_int_source {
diff --git a/drivers/net/ethernet/intel/i40e/i40e.h b/drivers/net/ethernet/intel/i40e/i40e.h
index 33c35d3b7420..ec76c3fa3a04 100644
--- a/drivers/net/ethernet/intel/i40e/i40e.h
+++ b/drivers/net/ethernet/intel/i40e/i40e.h
@@ -182,6 +182,7 @@ struct i40e_lump_tracking {
 enum i40e_fd_stat_idx {
 	I40E_FD_STAT_ATR,
 	I40E_FD_STAT_SB,
+	I40E_FD_STAT_ATR_TUNNEL,
 	I40E_FD_STAT_PF_COUNT
 };
 #define I40E_FD_STAT_PF_IDX(pf_id) ((pf_id) * I40E_FD_STAT_PF_COUNT)
@@ -189,6 +190,8 @@ enum i40e_fd_stat_idx {
 			(I40E_FD_STAT_PF_IDX(pf_id) + I40E_FD_STAT_ATR)
 #define I40E_FD_SB_STAT_IDX(pf_id)  \
 			(I40E_FD_STAT_PF_IDX(pf_id) + I40E_FD_STAT_SB)
+#define I40E_FD_ATR_TUNNEL_STAT_IDX(pf_id) \
+			(I40E_FD_STAT_PF_IDX(pf_id) + I40E_FD_STAT_ATR_TUNNEL)
 
 struct i40e_fdir_filter {
 	struct hlist_node fdir_node;
@@ -263,8 +266,6 @@ struct i40e_pf {
 
 	struct hlist_head fdir_filter_list;
 	u16 fdir_pf_active_filters;
-	u16 fd_sb_cnt_idx;
-	u16 fd_atr_cnt_idx;
 	unsigned long fd_flush_timestamp;
 	u32 fd_flush_cnt;
 	u32 fd_add_err;
@@ -317,6 +318,7 @@ struct i40e_pf {
 #endif
 #define I40E_FLAG_PORT_ID_VALID                (u64)(1 << 28)
 #define I40E_FLAG_DCB_CAPABLE                  (u64)(1 << 29)
+#define I40E_FLAG_VEB_MODE_ENABLED		BIT_ULL(40)
 
 	/* tracks features that get auto disabled by errors */
 	u64 auto_disable_flags;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
index 34170eabca7d..da0faf478af0 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
@@ -1021,6 +1021,15 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 			goto command_write_done;
 		}
 
+		/* By default we are in VEPA mode, if this is the first VF/VMDq
+		 * VSI to be added switch to VEB mode.
+		 */
+		if (!(pf->flags & I40E_FLAG_VEB_MODE_ENABLED)) {
+			pf->flags |= I40E_FLAG_VEB_MODE_ENABLED;
+			i40e_do_reset_safe(pf,
+					   BIT_ULL(__I40E_PF_RESET_REQUESTED));
+		}
+
 		vsi = i40e_vsi_setup(pf, I40E_VSI_VMDQ2, vsi_seid, 0);
 		if (vsi)
 			dev_info(&pf->pdev->dev, "added VSI %d to relay %d\n",
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
index 4cbaaeb902c4..9a68c65b17ea 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
@@ -147,6 +147,7 @@ static struct i40e_stats i40e_gstrings_stats[] = {
 	I40E_PF_STAT("rx_hwtstamp_cleared", rx_hwtstamp_cleared),
 	I40E_PF_STAT("fdir_flush_cnt", fd_flush_cnt),
 	I40E_PF_STAT("fdir_atr_match", stats.fd_atr_match),
+	I40E_PF_STAT("fdir_atr_tunnel_match", stats.fd_atr_tunnel_match),
 	I40E_PF_STAT("fdir_sb_match", stats.fd_sb_match),
 
 	/* LPI stats */
@@ -1548,6 +1549,17 @@ static int i40e_loopback_test(struct net_device *netdev, u64 *data)
 	return *data;
 }
 
+static inline bool i40e_active_vfs(struct i40e_pf *pf)
+{
+	struct i40e_vf *vfs = pf->vf;
+	int i;
+
+	for (i = 0; i < pf->num_alloc_vfs; i++)
+		if (vfs[i].vf_states & I40E_VF_STAT_ACTIVE)
+			return true;
+	return false;
+}
+
 static void i40e_diag_test(struct net_device *netdev,
 			   struct ethtool_test *eth_test, u64 *data)
 {
@@ -1560,6 +1572,20 @@ static void i40e_diag_test(struct net_device *netdev,
 		netif_info(pf, drv, netdev, "offline testing starting\n");
 
 		set_bit(__I40E_TESTING, &pf->state);
+
+		if (i40e_active_vfs(pf)) {
+			dev_warn(&pf->pdev->dev,
+				 "Please take active VFS offline and restart the adapter before running NIC diagnostics\n");
+			data[I40E_ETH_TEST_REG]		= 1;
+			data[I40E_ETH_TEST_EEPROM]	= 1;
+			data[I40E_ETH_TEST_INTR]	= 1;
+			data[I40E_ETH_TEST_LOOPBACK]	= 1;
+			data[I40E_ETH_TEST_LINK]	= 1;
+			eth_test->flags |= ETH_TEST_FL_FAILED;
+			clear_bit(__I40E_TESTING, &pf->state);
+			goto skip_ol_tests;
+		}
+
 		/* If the device is online then take it offline */
 		if (if_running)
 			/* indicate we're in test mode */
@@ -1605,6 +1631,8 @@ static void i40e_diag_test(struct net_device *netdev,
 		data[I40E_ETH_TEST_LOOPBACK] = 0;
 	}
 
+skip_ol_tests:
+
 	netif_info(pf, drv, netdev, "testing finished\n");
 }
 
@@ -2265,7 +2293,7 @@ static int i40e_add_fdir_ethtool(struct i40e_vsi *vsi,
 	input->pctype = 0;
 	input->dest_vsi = vsi->id;
 	input->fd_status = I40E_FILTER_PROGRAM_DESC_FD_STATUS_FD_ID;
-	input->cnt_index  = pf->fd_sb_cnt_idx;
+	input->cnt_index  = I40E_FD_SB_STAT_IDX(pf->hw.pf_id);
 	input->flow_type = fsp->flow_type;
 	input->ip4_proto = fsp->h_u.usr_ip4_spec.proto;
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_fcoe.c b/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
index 1803afeef23e..c8b621e0e7cd 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
@@ -118,7 +118,7 @@ static inline int i40e_fcoe_fc_eof(struct sk_buff *skb, u8 *eof)
  *
  * The FC EOF is converted to the value understood by HW for descriptor
  * programming. Never call this w/o calling i40e_fcoe_eof_is_supported()
- * first.
+ * first and that already checks for all supported valid eof values.
  **/
 static inline u32 i40e_fcoe_ctxt_eof(u8 eof)
 {
@@ -132,9 +132,12 @@ static inline u32 i40e_fcoe_ctxt_eof(u8 eof)
 	case FC_EOF_A:
 		return I40E_TX_DESC_CMD_L4T_EOFT_EOF_A;
 	default:
-		/* FIXME: still returns 0 */
-		pr_err("Unrecognized EOF %x\n", eof);
-		return 0;
+		/* Supported valid eof shall be already checked by
+		 * calling i40e_fcoe_eof_is_supported() first,
+		 * therefore this default case shall never hit.
+		 */
+		WARN_ON(1);
+		return -EINVAL;
 	}
 }
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index a54c14491e3b..48a52b35b614 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -39,7 +39,7 @@ static const char i40e_driver_string[] =
 
 #define DRV_VERSION_MAJOR 1
 #define DRV_VERSION_MINOR 3
-#define DRV_VERSION_BUILD 2
+#define DRV_VERSION_BUILD 4
 #define DRV_VERSION __stringify(DRV_VERSION_MAJOR) "." \
 	     __stringify(DRV_VERSION_MINOR) "." \
 	     __stringify(DRV_VERSION_BUILD)    DRV_KERN
@@ -772,9 +772,8 @@ static void i40e_update_prio_xoff_rx(struct i40e_pf *pf)
 
 	dcb_cfg = &hw->local_dcbx_config;
 
-	/* See if DCB enabled with PFC TC */
-	if (!(pf->flags & I40E_FLAG_DCB_ENABLED) ||
-	    !(dcb_cfg->pfc.pfcenable)) {
+	/* Collect Link XOFF stats when PFC is disabled */
+	if (!dcb_cfg->pfc.pfcenable) {
 		i40e_update_link_xoff_rx(pf);
 		return;
 	}
@@ -1097,12 +1096,18 @@ static void i40e_update_pf_stats(struct i40e_pf *pf)
 			   &osd->rx_jabber, &nsd->rx_jabber);
 
 	/* FDIR stats */
-	i40e_stat_update32(hw, I40E_GLQF_PCNT(pf->fd_atr_cnt_idx),
+	i40e_stat_update32(hw,
+			   I40E_GLQF_PCNT(I40E_FD_ATR_STAT_IDX(pf->hw.pf_id)),
 			   pf->stat_offsets_loaded,
 			   &osd->fd_atr_match, &nsd->fd_atr_match);
-	i40e_stat_update32(hw, I40E_GLQF_PCNT(pf->fd_sb_cnt_idx),
+	i40e_stat_update32(hw,
+			   I40E_GLQF_PCNT(I40E_FD_SB_STAT_IDX(pf->hw.pf_id)),
 			   pf->stat_offsets_loaded,
 			   &osd->fd_sb_match, &nsd->fd_sb_match);
+	i40e_stat_update32(hw,
+		      I40E_GLQF_PCNT(I40E_FD_ATR_TUNNEL_STAT_IDX(pf->hw.pf_id)),
+		      pf->stat_offsets_loaded,
+		      &osd->fd_atr_tunnel_match, &nsd->fd_atr_tunnel_match);
 
 	val = rd32(hw, I40E_PRTPM_EEE_STAT);
 	nsd->tx_lpi_status =
@@ -4739,7 +4744,8 @@ static int i40e_up_complete(struct i40e_vsi *vsi)
 		pf->fd_add_err = pf->fd_atr_cnt = 0;
 		if (pf->fd_tcp_rule > 0) {
 			pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
-			dev_info(&pf->pdev->dev, "Forcing ATR off, sideband rules for TCP/IPv4 exist\n");
+			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+				dev_info(&pf->pdev->dev, "Forcing ATR off, sideband rules for TCP/IPv4 exist\n");
 			pf->fd_tcp_rule = 0;
 		}
 		i40e_fdir_filter_restore(vsi);
@@ -5428,7 +5434,8 @@ void i40e_fdir_check_and_reenable(struct i40e_pf *pf)
 		if ((pf->flags & I40E_FLAG_FD_SB_ENABLED) &&
 		    (pf->auto_disable_flags & I40E_FLAG_FD_SB_ENABLED)) {
 			pf->auto_disable_flags &= ~I40E_FLAG_FD_SB_ENABLED;
-			dev_info(&pf->pdev->dev, "FD Sideband/ntuple is being enabled since we have space in the table now\n");
+			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+				dev_info(&pf->pdev->dev, "FD Sideband/ntuple is being enabled since we have space in the table now\n");
 		}
 	}
 	/* Wait for some more space to be available to turn on ATR */
@@ -5436,7 +5443,8 @@ void i40e_fdir_check_and_reenable(struct i40e_pf *pf)
 		if ((pf->flags & I40E_FLAG_FD_ATR_ENABLED) &&
 		    (pf->auto_disable_flags & I40E_FLAG_FD_ATR_ENABLED)) {
 			pf->auto_disable_flags &= ~I40E_FLAG_FD_ATR_ENABLED;
-			dev_info(&pf->pdev->dev, "ATR is being enabled since we have space in the table now\n");
+			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+				dev_info(&pf->pdev->dev, "ATR is being enabled since we have space in the table now\n");
 		}
 	}
 }
@@ -5469,7 +5477,8 @@ static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
 
 		if (!(time_after(jiffies, min_flush_time)) &&
 		    (fd_room < I40E_FDIR_BUFFER_HEAD_ROOM_FOR_ATR)) {
-			dev_info(&pf->pdev->dev, "ATR disabled, not enough FD filter space.\n");
+			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+				dev_info(&pf->pdev->dev, "ATR disabled, not enough FD filter space.\n");
 			disable_atr = true;
 		}
 
@@ -5496,7 +5505,8 @@ static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
 			if (!disable_atr)
 				pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
 			clear_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state);
-			dev_info(&pf->pdev->dev, "FD Filter table flushed and FD-SB replayed.\n");
+			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+				dev_info(&pf->pdev->dev, "FD Filter table flushed and FD-SB replayed.\n");
 		}
 	}
 }
@@ -6097,6 +6107,10 @@ static int i40e_reconstitute_veb(struct i40e_veb *veb)
 	if (ret)
 		goto end_reconstitute;
 
+	if (pf->flags & I40E_FLAG_VEB_MODE_ENABLED)
+		veb->bridge_mode = BRIDGE_MODE_VEB;
+	else
+		veb->bridge_mode = BRIDGE_MODE_VEPA;
 	i40e_config_bridge_mode(veb);
 
 	/* create the remaining VSIs attached to this VEB */
@@ -7676,12 +7690,8 @@ static int i40e_sw_init(struct i40e_pf *pf)
 	    (pf->hw.func_caps.fd_filters_best_effort > 0)) {
 		pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
 		pf->atr_sample_rate = I40E_DEFAULT_ATR_SAMPLE_RATE;
-		/* Setup a counter for fd_atr per PF */
-		pf->fd_atr_cnt_idx = I40E_FD_ATR_STAT_IDX(pf->hw.pf_id);
 		if (!(pf->flags & I40E_FLAG_MFP_ENABLED)) {
 			pf->flags |= I40E_FLAG_FD_SB_ENABLED;
-			/* Setup a counter for fd_sb per PF */
-			pf->fd_sb_cnt_idx = I40E_FD_SB_STAT_IDX(pf->hw.pf_id);
 		} else {
 			dev_info(&pf->pdev->dev,
 				 "Flow Director Sideband mode Disabled in MFP mode\n");
@@ -7771,7 +7781,8 @@ bool i40e_set_ntuple(struct i40e_pf *pf, netdev_features_t features)
 		pf->fd_add_err = pf->fd_atr_cnt = pf->fd_tcp_rule = 0;
 		pf->fdir_pf_active_filters = 0;
 		pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
-		dev_info(&pf->pdev->dev, "ATR re-enabled.\n");
+		if (I40E_DEBUG_FD & pf->hw.debug_mask)
+			dev_info(&pf->pdev->dev, "ATR re-enabled.\n");
 		/* if ATR was auto disabled it can be re-enabled. */
 		if ((pf->flags & I40E_FLAG_FD_ATR_ENABLED) &&
 		    (pf->auto_disable_flags & I40E_FLAG_FD_ATR_ENABLED))
@@ -8031,7 +8042,12 @@ static int i40e_ndo_bridge_setlink(struct net_device *dev,
 		} else if (mode != veb->bridge_mode) {
 			/* Existing HW bridge but different mode needs reset */
 			veb->bridge_mode = mode;
-			i40e_do_reset(pf, (1 << __I40E_PF_RESET_REQUESTED));
+			/* TODO: If no VFs or VMDq VSIs, disallow VEB mode */
+			if (mode == BRIDGE_MODE_VEB)
+				pf->flags |= I40E_FLAG_VEB_MODE_ENABLED;
+			else
+				pf->flags &= ~I40E_FLAG_VEB_MODE_ENABLED;
+			i40e_do_reset(pf, BIT_ULL(__I40E_PF_RESET_REQUESTED));
 			break;
 		}
 	}
@@ -8053,7 +8069,7 @@ static int i40e_ndo_bridge_setlink(struct net_device *dev,
 #ifdef HAVE_BRIDGE_FILTER
 static int i40e_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 				   struct net_device *dev,
-				   u32 __always_unused filter_mask, int nlflags)
+				   u32 filter_mask, int nlflags)
 #else
 static int i40e_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 				   struct net_device *dev, int nlflags)
@@ -8079,7 +8095,7 @@ static int i40e_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 		return 0;
 
 	return ndo_dflt_bridge_getlink(skb, pid, seq, dev, veb->bridge_mode,
-				       nlflags);
+				       nlflags, 0, 0, filter_mask, NULL);
 }
 #endif /* HAVE_BRIDGE_ATTRIBS */
 
@@ -8343,11 +8359,12 @@ static int i40e_add_vsi(struct i40e_vsi *vsi)
 		ctxt.uplink_seid = vsi->uplink_seid;
 		ctxt.connection_type = I40E_AQ_VSI_CONN_TYPE_NORMAL;
 		ctxt.flags = I40E_AQ_VSI_TYPE_PF;
-		if (i40e_is_vsi_uplink_mode_veb(vsi)) {
+		if ((pf->flags & I40E_FLAG_VEB_MODE_ENABLED) &&
+		    (i40e_is_vsi_uplink_mode_veb(vsi))) {
 			ctxt.info.valid_sections |=
-				cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
+			     cpu_to_le16(I40E_AQ_VSI_PROP_SWITCH_VALID);
 			ctxt.info.switch_id =
-				cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
+			   cpu_to_le16(I40E_AQ_VSI_SW_ID_FLAG_ALLOW_LB);
 		}
 		i40e_vsi_setup_queue_map(vsi, &ctxt, enabled_tc, true);
 		break;
@@ -8746,6 +8763,14 @@ struct i40e_vsi *i40e_vsi_setup(struct i40e_pf *pf, u8 type,
 					 __func__);
 				return NULL;
 			}
+			/* We come up by default in VEPA mode if SRIOV is not
+			 * already enabled, in which case we can't force VEPA
+			 * mode.
+			 */
+			if (!(pf->flags & I40E_FLAG_VEB_MODE_ENABLED)) {
+				veb->bridge_mode = BRIDGE_MODE_VEPA;
+				pf->flags &= ~I40E_FLAG_VEB_MODE_ENABLED;
+			}
 			i40e_config_bridge_mode(veb);
 		}
 		for (i = 0; i < I40E_MAX_VEB && !veb; i++) {
@@ -9856,6 +9881,15 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_switch_setup;
 	}
 
+#ifdef CONFIG_PCI_IOV
+	/* prep for VF support */
+	if ((pf->flags & I40E_FLAG_SRIOV_ENABLED) &&
+	    (pf->flags & I40E_FLAG_MSIX_ENABLED) &&
+	    !test_bit(__I40E_BAD_EEPROM, &pf->state)) {
+		if (pci_num_vf(pdev))
+			pf->flags |= I40E_FLAG_VEB_MODE_ENABLED;
+	}
+#endif
 	err = i40e_setup_pf_switch(pf, false);
 	if (err) {
 		dev_info(&pdev->dev, "setup_pf_switch failed: %d\n", err);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.c b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
index 4bd3a80aba82..9a4f2bc70cd2 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -165,9 +165,6 @@ int i40e_program_fdir_filter(struct i40e_fdir_filter *fdir_data, u8 *raw_packet,
 	tx_desc->cmd_type_offset_bsz =
 		build_ctob(td_cmd, 0, I40E_FDIR_MAX_RAW_PACKET_SIZE, 0);
 
-	/* set the timestamp */
-	tx_buf->time_stamp = jiffies;
-
 	/* Force memory writes to complete before letting h/w
 	 * know there are new descriptors to fetch.
 	 */
@@ -283,7 +280,8 @@ static int i40e_add_del_fdir_tcpv4(struct i40e_vsi *vsi,
 	if (add) {
 		pf->fd_tcp_rule++;
 		if (pf->flags & I40E_FLAG_FD_ATR_ENABLED) {
-			dev_info(&pf->pdev->dev, "Forcing ATR off, sideband rules for TCP/IPv4 flow being applied\n");
+			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+				dev_info(&pf->pdev->dev, "Forcing ATR off, sideband rules for TCP/IPv4 flow being applied\n");
 			pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
 		}
 	} else {
@@ -291,7 +289,8 @@ static int i40e_add_del_fdir_tcpv4(struct i40e_vsi *vsi,
 				  (pf->fd_tcp_rule - 1) : 0;
 		if (pf->fd_tcp_rule == 0) {
 			pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
-			dev_info(&pf->pdev->dev, "ATR re-enabled due to no sideband TCP/IPv4 rules\n");
+			if (I40E_DEBUG_FD & pf->hw.debug_mask)
+				dev_info(&pf->pdev->dev, "ATR re-enabled due to no sideband TCP/IPv4 rules\n");
 		}
 	}
 
@@ -501,7 +500,8 @@ static void i40e_fd_handle_status(struct i40e_ring *rx_ring,
 			if ((pf->flags & I40E_FLAG_FD_SB_ENABLED) &&
 			    !(pf->auto_disable_flags &
 				     I40E_FLAG_FD_SB_ENABLED)) {
-				dev_warn(&pdev->dev, "FD filter space full, new ntuple rules will not be added\n");
+				if (I40E_DEBUG_FD & pf->hw.debug_mask)
+					dev_warn(&pdev->dev, "FD filter space full, new ntuple rules will not be added\n");
 				pf->auto_disable_flags |=
 							I40E_FLAG_FD_SB_ENABLED;
 			}
@@ -807,10 +807,6 @@ static bool i40e_clean_tx_irq(struct i40e_ring *tx_ring, int budget)
 			 tx_ring->vsi->seid,
 			 tx_ring->queue_index,
 			 tx_ring->next_to_use, i);
-		dev_info(tx_ring->dev, "tx_bi[next_to_clean]\n"
-			 "  time_stamp           <%lx>\n"
-			 "  jiffies              <%lx>\n",
-			 tx_ring->tx_bi[i].time_stamp, jiffies);
 
 		netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
 
@@ -1653,9 +1649,6 @@ static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 		/* ERR_MASK will only have valid bits if EOP set */
 		if (unlikely(rx_error & (1 << I40E_RX_DESC_ERROR_RXE_SHIFT))) {
 			dev_kfree_skb_any(skb);
-			/* TODO: shouldn't we increment a counter indicating the
-			 * drop?
-			 */
 			continue;
 		}
 
@@ -1688,7 +1681,6 @@ static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 		skb_mark_napi_id(skb, &rx_ring->q_vector->napi);
 		i40e_receive_skb(rx_ring, skb, vlan_tag);
 
-		rx_ring->netdev->last_rx = jiffies;
 		rx_desc->wb.qword1.status_error_len = 0;
 
 	} while (likely(total_rx_packets < budget));
@@ -1821,7 +1813,6 @@ static int i40e_clean_rx_irq_1buf(struct i40e_ring *rx_ring, int budget)
 #endif
 		i40e_receive_skb(rx_ring, skb, vlan_tag);
 
-		rx_ring->netdev->last_rx = jiffies;
 		rx_desc->wb.qword1.status_error_len = 0;
 	} while (likely(total_rx_packets < budget));
 
@@ -1925,11 +1916,11 @@ int i40e_napi_poll(struct napi_struct *napi, int budget)
  * i40e_atr - Add a Flow Director ATR filter
  * @tx_ring:  ring to add programming descriptor to
  * @skb:      send buffer
- * @flags:    send flags
+ * @tx_flags: send tx flags
  * @protocol: wire protocol
  **/
 static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
-		     u32 flags, __be16 protocol)
+		     u32 tx_flags, __be16 protocol)
 {
 	struct i40e_filter_program_desc *fdir_desc;
 	struct i40e_pf *pf = tx_ring->vsi->back;
@@ -1954,25 +1945,38 @@ static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	if (!tx_ring->atr_sample_rate)
 		return;
 
-	/* snag network header to get L4 type and address */
-	hdr.network = skb_network_header(skb);
+	if (!(tx_flags & (I40E_TX_FLAGS_IPV4 | I40E_TX_FLAGS_IPV6)))
+		return;
 
-	/* Currently only IPv4/IPv6 with TCP is supported */
-	if (protocol == htons(ETH_P_IP)) {
-		if (hdr.ipv4->protocol != IPPROTO_TCP)
-			return;
+	if (!(tx_flags & I40E_TX_FLAGS_VXLAN_TUNNEL)) {
+		/* snag network header to get L4 type and address */
+		hdr.network = skb_network_header(skb);
 
-		/* access ihl as a u8 to avoid unaligned access on ia64 */
-		hlen = (hdr.network[0] & 0x0F) << 2;
-	} else if (protocol == htons(ETH_P_IPV6)) {
-		if (hdr.ipv6->nexthdr != IPPROTO_TCP)
+		/* Currently only IPv4/IPv6 with TCP is supported
+		 * access ihl as u8 to avoid unaligned access on ia64
+		 */
+		if (tx_flags & I40E_TX_FLAGS_IPV4)
+			hlen = (hdr.network[0] & 0x0F) << 2;
+		else if (protocol == htons(ETH_P_IPV6))
+			hlen = sizeof(struct ipv6hdr);
+		else
 			return;
-
-		hlen = sizeof(struct ipv6hdr);
 	} else {
-		return;
+		hdr.network = skb_inner_network_header(skb);
+		hlen = skb_inner_network_header_len(skb);
 	}
 
+	/* Currently only IPv4/IPv6 with TCP is supported
+	 * Note: tx_flags gets modified to reflect inner protocols in
+	 * tx_enable_csum function if encap is enabled.
+	 */
+	if ((tx_flags & I40E_TX_FLAGS_IPV4) &&
+	    (hdr.ipv4->protocol != IPPROTO_TCP))
+		return;
+	else if ((tx_flags & I40E_TX_FLAGS_IPV6) &&
+		 (hdr.ipv6->nexthdr != IPPROTO_TCP))
+		return;
+
 	th = (struct tcphdr *)(hdr.network + hlen);
 
 	/* Due to lack of space, no more new filters can be programmed */
@@ -2022,9 +2026,16 @@ static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
 		     I40E_TXD_FLTR_QW1_FD_STATUS_SHIFT;
 
 	dtype_cmd |= I40E_TXD_FLTR_QW1_CNT_ENA_MASK;
-	dtype_cmd |=
-		((u32)pf->fd_atr_cnt_idx << I40E_TXD_FLTR_QW1_CNTINDEX_SHIFT) &
-		I40E_TXD_FLTR_QW1_CNTINDEX_MASK;
+	if (!(tx_flags & I40E_TX_FLAGS_VXLAN_TUNNEL))
+		dtype_cmd |=
+			((u32)I40E_FD_ATR_STAT_IDX(pf->hw.pf_id) <<
+			I40E_TXD_FLTR_QW1_CNTINDEX_SHIFT) &
+			I40E_TXD_FLTR_QW1_CNTINDEX_MASK;
+	else
+		dtype_cmd |=
+			((u32)I40E_FD_ATR_TUNNEL_STAT_IDX(pf->hw.pf_id) <<
+			I40E_TXD_FLTR_QW1_CNTINDEX_SHIFT) &
+			I40E_TXD_FLTR_QW1_CNTINDEX_MASK;
 
 	fdir_desc->qindex_flex_ptype_vsi = cpu_to_le32(flex_ptype);
 	fdir_desc->rsvd = cpu_to_le32(0);
@@ -2045,13 +2056,13 @@ static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
  * otherwise  returns 0 to indicate the flags has been set properly.
  **/
 #ifdef I40E_FCOE
-int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
-			       struct i40e_ring *tx_ring,
-			       u32 *flags)
-#else
-static int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
+inline int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
 				      struct i40e_ring *tx_ring,
 				      u32 *flags)
+#else
+static inline int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
+					     struct i40e_ring *tx_ring,
+					     u32 *flags)
 #endif
 {
 	__be16 protocol = skb->protocol;
@@ -2119,16 +2130,14 @@ out:
  * i40e_tso - set up the tso context descriptor
  * @tx_ring:  ptr to the ring to send
  * @skb:      ptr to the skb we're sending
- * @tx_flags: the collected send information
- * @protocol: the send protocol
  * @hdr_len:  ptr to the size of the packet header
  * @cd_tunneling: ptr to context descriptor bits
  *
  * Returns 0 if no TSO can happen, 1 if tso is going, or error
  **/
 static int i40e_tso(struct i40e_ring *tx_ring, struct sk_buff *skb,
-		    u32 tx_flags, __be16 protocol, u8 *hdr_len,
-		    u64 *cd_type_cmd_tso_mss, u32 *cd_tunneling)
+		    u8 *hdr_len, u64 *cd_type_cmd_tso_mss,
+		    u32 *cd_tunneling)
 {
 	u32 cd_cmd, cd_tso_len, cd_mss;
 	struct ipv6hdr *ipv6h;
@@ -2220,12 +2229,12 @@ static int i40e_tsyn(struct i40e_ring *tx_ring, struct sk_buff *skb,
 /**
  * i40e_tx_enable_csum - Enable Tx checksum offloads
  * @skb: send buffer
- * @tx_flags: Tx flags currently set
+ * @tx_flags: pointer to Tx flags currently set
  * @td_cmd: Tx descriptor command bits to set
  * @td_offset: Tx descriptor header offsets to set
  * @cd_tunneling: ptr to context desc bits
  **/
-static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
+static void i40e_tx_enable_csum(struct sk_buff *skb, u32 *tx_flags,
 				u32 *td_cmd, u32 *td_offset,
 				struct i40e_ring *tx_ring,
 				u32 *cd_tunneling)
@@ -2241,6 +2250,7 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 		switch (ip_hdr(skb)->protocol) {
 		case IPPROTO_UDP:
 			l4_tunnel = I40E_TXD_CTX_UDP_TUNNELING;
+			*tx_flags |= I40E_TX_FLAGS_VXLAN_TUNNEL;
 			break;
 		default:
 			return;
@@ -2250,18 +2260,17 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 		this_ipv6_hdr = inner_ipv6_hdr(skb);
 		this_tcp_hdrlen = inner_tcp_hdrlen(skb);
 
-		if (tx_flags & I40E_TX_FLAGS_IPV4) {
-
-			if (tx_flags & I40E_TX_FLAGS_TSO) {
+		if (*tx_flags & I40E_TX_FLAGS_IPV4) {
+			if (*tx_flags & I40E_TX_FLAGS_TSO) {
 				*cd_tunneling |= I40E_TX_CTX_EXT_IP_IPV4;
 				ip_hdr(skb)->check = 0;
 			} else {
 				*cd_tunneling |=
 					 I40E_TX_CTX_EXT_IP_IPV4_NO_CSUM;
 			}
-		} else if (tx_flags & I40E_TX_FLAGS_IPV6) {
+		} else if (*tx_flags & I40E_TX_FLAGS_IPV6) {
 			*cd_tunneling |= I40E_TX_CTX_EXT_IP_IPV6;
-			if (tx_flags & I40E_TX_FLAGS_TSO)
+			if (*tx_flags & I40E_TX_FLAGS_TSO)
 				ip_hdr(skb)->check = 0;
 		}
 
@@ -2273,8 +2282,8 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 					skb_transport_offset(skb)) >> 1) <<
 				   I40E_TXD_CTX_QW0_NATLEN_SHIFT;
 		if (this_ip_hdr->version == 6) {
-			tx_flags &= ~I40E_TX_FLAGS_IPV4;
-			tx_flags |= I40E_TX_FLAGS_IPV6;
+			*tx_flags &= ~I40E_TX_FLAGS_IPV4;
+			*tx_flags |= I40E_TX_FLAGS_IPV6;
 		}
 	} else {
 		network_hdr_len = skb_network_header_len(skb);
@@ -2284,12 +2293,12 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 	}
 
 	/* Enable IP checksum offloads */
-	if (tx_flags & I40E_TX_FLAGS_IPV4) {
+	if (*tx_flags & I40E_TX_FLAGS_IPV4) {
 		l4_hdr = this_ip_hdr->protocol;
 		/* the stack computes the IP header already, the only time we
 		 * need the hardware to recompute it is in the case of TSO.
 		 */
-		if (tx_flags & I40E_TX_FLAGS_TSO) {
+		if (*tx_flags & I40E_TX_FLAGS_TSO) {
 			*td_cmd |= I40E_TX_DESC_CMD_IIPT_IPV4_CSUM;
 			this_ip_hdr->check = 0;
 		} else {
@@ -2298,7 +2307,7 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 		/* Now set the td_offset for IP header length */
 		*td_offset = (network_hdr_len >> 2) <<
 			      I40E_TX_DESC_LENGTH_IPLEN_SHIFT;
-	} else if (tx_flags & I40E_TX_FLAGS_IPV6) {
+	} else if (*tx_flags & I40E_TX_FLAGS_IPV6) {
 		l4_hdr = this_ipv6_hdr->nexthdr;
 		*td_cmd |= I40E_TX_DESC_CMD_IIPT_IPV6;
 		/* Now set the td_offset for IP header length */
@@ -2396,9 +2405,9 @@ static inline int __i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
  * Returns 0 if stop is not needed
  **/
 #ifdef I40E_FCOE
-int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
+inline int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
 #else
-static int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
+static inline int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
 #endif
 {
 	if (likely(I40E_DESC_UNUSED(tx_ring) >= size))
@@ -2410,14 +2419,12 @@ static int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
  * i40e_chk_linearize - Check if there are more than 8 fragments per packet
  * @skb:      send buffer
  * @tx_flags: collected send information
- * @hdr_len:  size of the packet header
  *
  * Note: Our HW can't scatter-gather more than 8 fragments to build
  * a packet on the wire and so we need to figure out the cases where we
  * need to linearize the skb.
  **/
-static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags,
-			       const u8 hdr_len)
+static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags)
 {
 	struct skb_frag_struct *frag;
 	bool linearize = false;
@@ -2429,7 +2436,7 @@ static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags,
 	gso_segs = skb_shinfo(skb)->gso_segs;
 
 	if (tx_flags & (I40E_TX_FLAGS_TSO | I40E_TX_FLAGS_FSO)) {
-		u16 j = 1;
+		u16 j = 0;
 
 		if (num_frags < (I40E_MAX_BUFFER_TXD))
 			goto linearize_chk_done;
@@ -2440,21 +2447,18 @@ static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags,
 			goto linearize_chk_done;
 		}
 		frag = &skb_shinfo(skb)->frags[0];
-		size = hdr_len;
 		/* we might still have more fragments per segment */
 		do {
 			size += skb_frag_size(frag);
 			frag++; j++;
+			if ((size >= skb_shinfo(skb)->gso_size) &&
+			    (j < I40E_MAX_BUFFER_TXD)) {
+				size = (size % skb_shinfo(skb)->gso_size);
+				j = (size) ? 1 : 0;
+			}
 			if (j == I40E_MAX_BUFFER_TXD) {
-				if (size < skb_shinfo(skb)->gso_size) {
-					linearize = true;
-					break;
-				}
-				j = 1;
-				size -= skb_shinfo(skb)->gso_size;
-				if (size)
-					j++;
-				size += hdr_len;
+				linearize = true;
+				break;
 			}
 			num_frags--;
 		} while (num_frags);
@@ -2478,13 +2482,13 @@ linearize_chk_done:
  * @td_offset: offset for checksum or crc
  **/
 #ifdef I40E_FCOE
-void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
-		 struct i40e_tx_buffer *first, u32 tx_flags,
-		 const u8 hdr_len, u32 td_cmd, u32 td_offset)
-#else
-static void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
+inline void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 			struct i40e_tx_buffer *first, u32 tx_flags,
 			const u8 hdr_len, u32 td_cmd, u32 td_offset)
+#else
+static inline void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
+			       struct i40e_tx_buffer *first, u32 tx_flags,
+			       const u8 hdr_len, u32 td_cmd, u32 td_offset)
 #endif
 {
 	unsigned int data_len = skb->data_len;
@@ -2590,9 +2594,6 @@ static void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 						 tx_ring->queue_index),
 			     first->bytecount);
 
-	/* set the timestamp */
-	first->time_stamp = jiffies;
-
 	/* Force memory writes to complete before letting h/w
 	 * know there are new descriptors to fetch.  (Only
 	 * applicable for weak-ordered memory model archs,
@@ -2645,11 +2646,11 @@ dma_error:
  * one descriptor.
  **/
 #ifdef I40E_FCOE
-int i40e_xmit_descriptor_count(struct sk_buff *skb,
-			       struct i40e_ring *tx_ring)
-#else
-static int i40e_xmit_descriptor_count(struct sk_buff *skb,
+inline int i40e_xmit_descriptor_count(struct sk_buff *skb,
 				      struct i40e_ring *tx_ring)
+#else
+static inline int i40e_xmit_descriptor_count(struct sk_buff *skb,
+					     struct i40e_ring *tx_ring)
 #endif
 {
 	unsigned int f;
@@ -2711,7 +2712,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	else if (protocol == htons(ETH_P_IPV6))
 		tx_flags |= I40E_TX_FLAGS_IPV6;
 
-	tso = i40e_tso(tx_ring, skb, tx_flags, protocol, &hdr_len,
+	tso = i40e_tso(tx_ring, skb, &hdr_len,
 		       &cd_type_cmd_tso_mss, &cd_tunneling);
 
 	if (tso < 0)
@@ -2724,7 +2725,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	if (tsyn)
 		tx_flags |= I40E_TX_FLAGS_TSYN;
 
-	if (i40e_chk_linearize(skb, tx_flags, hdr_len))
+	if (i40e_chk_linearize(skb, tx_flags))
 		if (skb_linearize(skb))
 			goto out_drop;
 
@@ -2737,7 +2738,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
 		tx_flags |= I40E_TX_FLAGS_CSUM;
 
-		i40e_tx_enable_csum(skb, tx_flags, &td_cmd, &td_offset,
+		i40e_tx_enable_csum(skb, &tx_flags, &td_cmd, &td_offset,
 				    tx_ring, &cd_tunneling);
 	}
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.h b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
index 4b0b8102cdc3..0dc48dc9ca61 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
@@ -139,6 +139,7 @@ enum i40e_dyn_idx_t {
 #define I40E_TX_FLAGS_FSO		(u32)(1 << 7)
 #define I40E_TX_FLAGS_TSYN		(u32)(1 << 8)
 #define I40E_TX_FLAGS_FD_SB		(u32)(1 << 9)
+#define I40E_TX_FLAGS_VXLAN_TUNNEL	(u32)(1 << 10)
 #define I40E_TX_FLAGS_VLAN_MASK		0xffff0000
 #define I40E_TX_FLAGS_VLAN_PRIO_MASK	0xe0000000
 #define I40E_TX_FLAGS_VLAN_PRIO_SHIFT	29
@@ -146,7 +147,6 @@ enum i40e_dyn_idx_t {
 
 struct i40e_tx_buffer {
 	struct i40e_tx_desc *next_to_watch;
-	unsigned long time_stamp;
 	union {
 		struct sk_buff *skb;
 		void *raw_buf;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_type.h b/drivers/net/ethernet/intel/i40e/i40e_type.h
index 568e855da0f3..9a5a75b1e2bc 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_type.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_type.h
@@ -1133,6 +1133,7 @@ struct i40e_hw_port_stats {
 	/* flow director stats */
 	u64 fd_atr_match;
 	u64 fd_sb_match;
+	u64 fd_atr_tunnel_match;
 	/* EEE LPI */
 	u32 tx_lpi_status;
 	u32 rx_lpi_status;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
index 78d1c4ff565e..23f95cdbdfcc 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
@@ -980,6 +980,13 @@ static int i40e_pci_sriov_enable(struct pci_dev *pdev, int num_vfs)
 	int pre_existing_vfs = pci_num_vf(pdev);
 	int err = 0;
 
+	if (pf->state & __I40E_TESTING) {
+		dev_warn(&pdev->dev,
+			 "Cannot enable SR-IOV virtual functions while the device is undergoing diagnostic testing\n");
+		err = -EPERM;
+		goto err_out;
+	}
+
 	dev_info(&pdev->dev, "Allocating %d VFs.\n", num_vfs);
 	if (pre_existing_vfs && pre_existing_vfs != num_vfs)
 		i40e_free_vfs(pf);
@@ -1018,11 +1025,19 @@ int i40e_pci_sriov_configure(struct pci_dev *pdev, int num_vfs)
 {
 	struct i40e_pf *pf = pci_get_drvdata(pdev);
 
-	if (num_vfs)
+	if (num_vfs) {
+		if (!(pf->flags & I40E_FLAG_VEB_MODE_ENABLED)) {
+			pf->flags |= I40E_FLAG_VEB_MODE_ENABLED;
+			i40e_do_reset_safe(pf,
+					   BIT_ULL(__I40E_PF_RESET_REQUESTED));
+		}
 		return i40e_pci_sriov_enable(pdev, num_vfs);
+	}
 
 	if (!pci_vfs_assigned(pf->pdev)) {
 		i40e_free_vfs(pf);
+		pf->flags &= ~I40E_FLAG_VEB_MODE_ENABLED;
+		i40e_do_reset_safe(pf, BIT_ULL(__I40E_PF_RESET_REQUESTED));
 	} else {
 		dev_warn(&pdev->dev, "Unable to free VFs because some are assigned to VMs.\n");
 		return -EINVAL;
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
index b077e02a0cc7..395f32f226c0 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
@@ -322,10 +322,6 @@ static bool i40e_clean_tx_irq(struct i40e_ring *tx_ring, int budget)
 			 tx_ring->vsi->seid,
 			 tx_ring->queue_index,
 			 tx_ring->next_to_use, i);
-		dev_info(tx_ring->dev, "tx_bi[next_to_clean]\n"
-			 "  time_stamp           <%lx>\n"
-			 "  jiffies              <%lx>\n",
-			 tx_ring->tx_bi[i].time_stamp, jiffies);
 
 		netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
 
@@ -488,6 +484,8 @@ int i40evf_setup_tx_descriptors(struct i40e_ring *tx_ring)
 	if (!dev)
 		return -ENOMEM;
 
+	/* warn if we are about to overwrite the pointer */
+	WARN_ON(tx_ring->tx_bi);
 	bi_size = sizeof(struct i40e_tx_buffer) * tx_ring->count;
 	tx_ring->tx_bi = kzalloc(bi_size, GFP_KERNEL);
 	if (!tx_ring->tx_bi)
@@ -648,6 +646,8 @@ int i40evf_setup_rx_descriptors(struct i40e_ring *rx_ring)
 	struct device *dev = rx_ring->dev;
 	int bi_size;
 
+	/* warn if we are about to overwrite the pointer */
+	WARN_ON(rx_ring->rx_bi);
 	bi_size = sizeof(struct i40e_rx_buffer) * rx_ring->count;
 	rx_ring->rx_bi = kzalloc(bi_size, GFP_KERNEL);
 	if (!rx_ring->rx_bi)
@@ -1128,9 +1128,6 @@ static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 		/* ERR_MASK will only have valid bits if EOP set */
 		if (unlikely(rx_error & (1 << I40E_RX_DESC_ERROR_RXE_SHIFT))) {
 			dev_kfree_skb_any(skb);
-			/* TODO: shouldn't we increment a counter indicating the
-			 * drop?
-			 */
 			continue;
 		}
 
@@ -1156,7 +1153,6 @@ static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 		skb_mark_napi_id(skb, &rx_ring->q_vector->napi);
 		i40e_receive_skb(rx_ring, skb, vlan_tag);
 
-		rx_ring->netdev->last_rx = jiffies;
 		rx_desc->wb.qword1.status_error_len = 0;
 
 	} while (likely(total_rx_packets < budget));
@@ -1271,7 +1267,6 @@ static int i40e_clean_rx_irq_1buf(struct i40e_ring *rx_ring, int budget)
 			 : 0;
 		i40e_receive_skb(rx_ring, skb, vlan_tag);
 
-		rx_ring->netdev->last_rx = jiffies;
 		rx_desc->wb.qword1.status_error_len = 0;
 	} while (likely(total_rx_packets < budget));
 
@@ -1352,7 +1347,7 @@ int i40evf_napi_poll(struct napi_struct *napi, int budget)
 }
 
 /**
- * i40e_tx_prepare_vlan_flags - prepare generic TX VLAN tagging flags for HW
+ * i40evf_tx_prepare_vlan_flags - prepare generic TX VLAN tagging flags for HW
  * @skb:     send buffer
  * @tx_ring: ring to send buffer on
  * @flags:   the tx flags to be set
@@ -1363,9 +1358,9 @@ int i40evf_napi_poll(struct napi_struct *napi, int budget)
  * Returns error code indicate the frame should be dropped upon error and the
  * otherwise  returns 0 to indicate the flags has been set properly.
  **/
-static int i40e_tx_prepare_vlan_flags(struct sk_buff *skb,
-				      struct i40e_ring *tx_ring,
-				      u32 *flags)
+static inline int i40evf_tx_prepare_vlan_flags(struct sk_buff *skb,
+					       struct i40e_ring *tx_ring,
+					       u32 *flags)
 {
 	__be16 protocol = skb->protocol;
 	u32  tx_flags = 0;
@@ -1408,16 +1403,14 @@ out:
  * i40e_tso - set up the tso context descriptor
  * @tx_ring:  ptr to the ring to send
  * @skb:      ptr to the skb we're sending
- * @tx_flags: the collected send information
- * @protocol: the send protocol
  * @hdr_len:  ptr to the size of the packet header
  * @cd_tunneling: ptr to context descriptor bits
  *
  * Returns 0 if no TSO can happen, 1 if tso is going, or error
  **/
 static int i40e_tso(struct i40e_ring *tx_ring, struct sk_buff *skb,
-		    u32 tx_flags, __be16 protocol, u8 *hdr_len,
-		    u64 *cd_type_cmd_tso_mss, u32 *cd_tunneling)
+		    u8 *hdr_len, u64 *cd_type_cmd_tso_mss,
+		    u32 *cd_tunneling)
 {
 	u32 cd_cmd, cd_tso_len, cd_mss;
 	struct ipv6hdr *ipv6h;
@@ -1468,12 +1461,12 @@ static int i40e_tso(struct i40e_ring *tx_ring, struct sk_buff *skb,
 /**
  * i40e_tx_enable_csum - Enable Tx checksum offloads
  * @skb: send buffer
- * @tx_flags: Tx flags currently set
+ * @tx_flags: pointer to Tx flags currently set
  * @td_cmd: Tx descriptor command bits to set
  * @td_offset: Tx descriptor header offsets to set
  * @cd_tunneling: ptr to context desc bits
  **/
-static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
+static void i40e_tx_enable_csum(struct sk_buff *skb, u32 *tx_flags,
 				u32 *td_cmd, u32 *td_offset,
 				struct i40e_ring *tx_ring,
 				u32 *cd_tunneling)
@@ -1489,6 +1482,7 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 		switch (ip_hdr(skb)->protocol) {
 		case IPPROTO_UDP:
 			l4_tunnel = I40E_TXD_CTX_UDP_TUNNELING;
+			*tx_flags |= I40E_TX_FLAGS_VXLAN_TUNNEL;
 			break;
 		default:
 			return;
@@ -1498,18 +1492,17 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 		this_ipv6_hdr = inner_ipv6_hdr(skb);
 		this_tcp_hdrlen = inner_tcp_hdrlen(skb);
 
-		if (tx_flags & I40E_TX_FLAGS_IPV4) {
-
-			if (tx_flags & I40E_TX_FLAGS_TSO) {
+		if (*tx_flags & I40E_TX_FLAGS_IPV4) {
+			if (*tx_flags & I40E_TX_FLAGS_TSO) {
 				*cd_tunneling |= I40E_TX_CTX_EXT_IP_IPV4;
 				ip_hdr(skb)->check = 0;
 			} else {
 				*cd_tunneling |=
 					 I40E_TX_CTX_EXT_IP_IPV4_NO_CSUM;
 			}
-		} else if (tx_flags & I40E_TX_FLAGS_IPV6) {
+		} else if (*tx_flags & I40E_TX_FLAGS_IPV6) {
 			*cd_tunneling |= I40E_TX_CTX_EXT_IP_IPV6;
-			if (tx_flags & I40E_TX_FLAGS_TSO)
+			if (*tx_flags & I40E_TX_FLAGS_TSO)
 				ip_hdr(skb)->check = 0;
 		}
 
@@ -1521,8 +1514,8 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 					skb_transport_offset(skb)) >> 1) <<
 				   I40E_TXD_CTX_QW0_NATLEN_SHIFT;
 		if (this_ip_hdr->version == 6) {
-			tx_flags &= ~I40E_TX_FLAGS_IPV4;
-			tx_flags |= I40E_TX_FLAGS_IPV6;
+			*tx_flags &= ~I40E_TX_FLAGS_IPV4;
+			*tx_flags |= I40E_TX_FLAGS_IPV6;
 		}
 
 
@@ -1534,12 +1527,12 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 	}
 
 	/* Enable IP checksum offloads */
-	if (tx_flags & I40E_TX_FLAGS_IPV4) {
+	if (*tx_flags & I40E_TX_FLAGS_IPV4) {
 		l4_hdr = this_ip_hdr->protocol;
 		/* the stack computes the IP header already, the only time we
 		 * need the hardware to recompute it is in the case of TSO.
 		 */
-		if (tx_flags & I40E_TX_FLAGS_TSO) {
+		if (*tx_flags & I40E_TX_FLAGS_TSO) {
 			*td_cmd |= I40E_TX_DESC_CMD_IIPT_IPV4_CSUM;
 			this_ip_hdr->check = 0;
 		} else {
@@ -1548,7 +1541,7 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 tx_flags,
 		/* Now set the td_offset for IP header length */
 		*td_offset = (network_hdr_len >> 2) <<
 			      I40E_TX_DESC_LENGTH_IPLEN_SHIFT;
-	} else if (tx_flags & I40E_TX_FLAGS_IPV6) {
+	} else if (*tx_flags & I40E_TX_FLAGS_IPV6) {
 		l4_hdr = this_ipv6_hdr->nexthdr;
 		*td_cmd |= I40E_TX_DESC_CMD_IIPT_IPV6;
 		/* Now set the td_offset for IP header length */
@@ -1619,14 +1612,12 @@ static void i40e_create_tx_ctx(struct i40e_ring *tx_ring,
  * i40e_chk_linearize - Check if there are more than 8 fragments per packet
  * @skb:      send buffer
  * @tx_flags: collected send information
- * @hdr_len:  size of the packet header
  *
  * Note: Our HW can't scatter-gather more than 8 fragments to build
  * a packet on the wire and so we need to figure out the cases where we
  * need to linearize the skb.
  **/
-static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags,
-			       const u8 hdr_len)
+static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags)
 {
 	struct skb_frag_struct *frag;
 	bool linearize = false;
@@ -1638,7 +1629,7 @@ static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags,
 	gso_segs = skb_shinfo(skb)->gso_segs;
 
 	if (tx_flags & (I40E_TX_FLAGS_TSO | I40E_TX_FLAGS_FSO)) {
-		u16 j = 1;
+		u16 j = 0;
 
 		if (num_frags < (I40E_MAX_BUFFER_TXD))
 			goto linearize_chk_done;
@@ -1649,21 +1640,18 @@ static bool i40e_chk_linearize(struct sk_buff *skb, u32 tx_flags,
 			goto linearize_chk_done;
 		}
 		frag = &skb_shinfo(skb)->frags[0];
-		size = hdr_len;
 		/* we might still have more fragments per segment */
 		do {
 			size += skb_frag_size(frag);
 			frag++; j++;
+			if ((size >= skb_shinfo(skb)->gso_size) &&
+			    (j < I40E_MAX_BUFFER_TXD)) {
+				size = (size % skb_shinfo(skb)->gso_size);
+				j = (size) ? 1 : 0;
+			}
 			if (j == I40E_MAX_BUFFER_TXD) {
-				if (size < skb_shinfo(skb)->gso_size) {
-					linearize = true;
-					break;
-				}
-				j = 1;
-				size -= skb_shinfo(skb)->gso_size;
-				if (size)
-					j++;
-				size += hdr_len;
+				linearize = true;
+				break;
 			}
 			num_frags--;
 		} while (num_frags);
@@ -1677,7 +1665,44 @@ linearize_chk_done:
 }
 
 /**
- * i40e_tx_map - Build the Tx descriptor
+ * __i40evf_maybe_stop_tx - 2nd level check for tx stop conditions
+ * @tx_ring: the ring to be checked
+ * @size:    the size buffer we want to assure is available
+ *
+ * Returns -EBUSY if a stop is needed, else 0
+ **/
+static inline int __i40evf_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
+{
+	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
+	/* Memory barrier before checking head and tail */
+	smp_mb();
+
+	/* Check again in a case another CPU has just made room available. */
+	if (likely(I40E_DESC_UNUSED(tx_ring) < size))
+		return -EBUSY;
+
+	/* A reprieve! - use start_queue because it doesn't call schedule */
+	netif_start_subqueue(tx_ring->netdev, tx_ring->queue_index);
+	++tx_ring->tx_stats.restart_queue;
+	return 0;
+}
+
+/**
+ * i40evf_maybe_stop_tx - 1st level check for tx stop conditions
+ * @tx_ring: the ring to be checked
+ * @size:    the size buffer we want to assure is available
+ *
+ * Returns 0 if stop is not needed
+ **/
+static inline int i40evf_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
+{
+	if (likely(I40E_DESC_UNUSED(tx_ring) >= size))
+		return 0;
+	return __i40evf_maybe_stop_tx(tx_ring, size);
+}
+
+/**
+ * i40evf_tx_map - Build the Tx descriptor
  * @tx_ring:  ring to send buffer on
  * @skb:      send buffer
  * @first:    first buffer info buffer to use
@@ -1686,9 +1711,9 @@ linearize_chk_done:
  * @td_cmd:   the command field in the descriptor
  * @td_offset: offset for checksum or crc
  **/
-static void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
-			struct i40e_tx_buffer *first, u32 tx_flags,
-			const u8 hdr_len, u32 td_cmd, u32 td_offset)
+static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
+				 struct i40e_tx_buffer *first, u32 tx_flags,
+				 const u8 hdr_len, u32 td_cmd, u32 td_offset)
 {
 	unsigned int data_len = skb->data_len;
 	unsigned int size = skb_headlen(skb);
@@ -1794,9 +1819,6 @@ static void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 						 tx_ring->queue_index),
 			     first->bytecount);
 
-	/* set the timestamp */
-	first->time_stamp = jiffies;
-
 	/* Force memory writes to complete before letting h/w
 	 * know there are new descriptors to fetch.  (Only
 	 * applicable for weak-ordered memory model archs,
@@ -1813,8 +1835,12 @@ static void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 
 	tx_ring->next_to_use = i;
 
+	i40evf_maybe_stop_tx(tx_ring, DESC_NEEDED);
 	/* notify HW of packet */
-	writel(i, tx_ring->tail);
+	if (!skb->xmit_more ||
+	    netif_xmit_stopped(netdev_get_tx_queue(tx_ring->netdev,
+						   tx_ring->queue_index)))
+		writel(i, tx_ring->tail);
 
 	return;
 
@@ -1836,44 +1862,7 @@ dma_error:
 }
 
 /**
- * __i40e_maybe_stop_tx - 2nd level check for tx stop conditions
- * @tx_ring: the ring to be checked
- * @size:    the size buffer we want to assure is available
- *
- * Returns -EBUSY if a stop is needed, else 0
- **/
-static inline int __i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
-{
-	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
-	/* Memory barrier before checking head and tail */
-	smp_mb();
-
-	/* Check again in a case another CPU has just made room available. */
-	if (likely(I40E_DESC_UNUSED(tx_ring) < size))
-		return -EBUSY;
-
-	/* A reprieve! - use start_queue because it doesn't call schedule */
-	netif_start_subqueue(tx_ring->netdev, tx_ring->queue_index);
-	++tx_ring->tx_stats.restart_queue;
-	return 0;
-}
-
-/**
- * i40e_maybe_stop_tx - 1st level check for tx stop conditions
- * @tx_ring: the ring to be checked
- * @size:    the size buffer we want to assure is available
- *
- * Returns 0 if stop is not needed
- **/
-static int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
-{
-	if (likely(I40E_DESC_UNUSED(tx_ring) >= size))
-		return 0;
-	return __i40e_maybe_stop_tx(tx_ring, size);
-}
-
-/**
- * i40e_xmit_descriptor_count - calculate number of tx descriptors needed
+ * i40evf_xmit_descriptor_count - calculate number of tx descriptors needed
  * @skb:     send buffer
  * @tx_ring: ring to send buffer on
  *
@@ -1881,8 +1870,8 @@ static int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
  * there is not enough descriptors available in this ring since we need at least
  * one descriptor.
  **/
-static int i40e_xmit_descriptor_count(struct sk_buff *skb,
-				      struct i40e_ring *tx_ring)
+static inline int i40evf_xmit_descriptor_count(struct sk_buff *skb,
+					       struct i40e_ring *tx_ring)
 {
 	unsigned int f;
 	int count = 0;
@@ -1897,7 +1886,7 @@ static int i40e_xmit_descriptor_count(struct sk_buff *skb,
 		count += TXD_USE_COUNT(skb_shinfo(skb)->frags[f].size);
 
 	count += TXD_USE_COUNT(skb_headlen(skb));
-	if (i40e_maybe_stop_tx(tx_ring, count + 4 + 1)) {
+	if (i40evf_maybe_stop_tx(tx_ring, count + 4 + 1)) {
 		tx_ring->tx_stats.tx_busy++;
 		return 0;
 	}
@@ -1923,11 +1912,11 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	u32 td_cmd = 0;
 	u8 hdr_len = 0;
 	int tso;
-	if (0 == i40e_xmit_descriptor_count(skb, tx_ring))
+	if (0 == i40evf_xmit_descriptor_count(skb, tx_ring))
 		return NETDEV_TX_BUSY;
 
 	/* prepare the xmit flags */
-	if (i40e_tx_prepare_vlan_flags(skb, tx_ring, &tx_flags))
+	if (i40evf_tx_prepare_vlan_flags(skb, tx_ring, &tx_flags))
 		goto out_drop;
 
 	/* obtain protocol of skb */
@@ -1942,7 +1931,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	else if (protocol == htons(ETH_P_IPV6))
 		tx_flags |= I40E_TX_FLAGS_IPV6;
 
-	tso = i40e_tso(tx_ring, skb, tx_flags, protocol, &hdr_len,
+	tso = i40e_tso(tx_ring, skb, &hdr_len,
 		       &cd_type_cmd_tso_mss, &cd_tunneling);
 
 	if (tso < 0)
@@ -1950,7 +1939,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	else if (tso)
 		tx_flags |= I40E_TX_FLAGS_TSO;
 
-	if (i40e_chk_linearize(skb, tx_flags, hdr_len))
+	if (i40e_chk_linearize(skb, tx_flags))
 		if (skb_linearize(skb))
 			goto out_drop;
 
@@ -1963,17 +1952,15 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
 		tx_flags |= I40E_TX_FLAGS_CSUM;
 
-		i40e_tx_enable_csum(skb, tx_flags, &td_cmd, &td_offset,
+		i40e_tx_enable_csum(skb, &tx_flags, &td_cmd, &td_offset,
 				    tx_ring, &cd_tunneling);
 	}
 
 	i40e_create_tx_ctx(tx_ring, cd_type_cmd_tso_mss,
 			   cd_tunneling, cd_l2tag2);
 
-	i40e_tx_map(tx_ring, skb, first, tx_flags, hdr_len,
-		    td_cmd, td_offset);
-
-	i40e_maybe_stop_tx(tx_ring, DESC_NEEDED);
+	i40evf_tx_map(tx_ring, skb, first, tx_flags, hdr_len,
+		      td_cmd, td_offset);
 
 	return NETDEV_TX_OK;
 
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
index 1e49bb1fbac1..e7a34f899f2c 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
@@ -138,6 +138,7 @@ enum i40e_dyn_idx_t {
 #define I40E_TX_FLAGS_FCCRC		(u32)(1 << 6)
 #define I40E_TX_FLAGS_FSO		(u32)(1 << 7)
 #define I40E_TX_FLAGS_FD_SB		(u32)(1 << 9)
+#define I40E_TX_FLAGS_VXLAN_TUNNEL	(u32)(1 << 10)
 #define I40E_TX_FLAGS_VLAN_MASK		0xffff0000
 #define I40E_TX_FLAGS_VLAN_PRIO_MASK	0xe0000000
 #define I40E_TX_FLAGS_VLAN_PRIO_SHIFT	29
@@ -145,7 +146,6 @@ enum i40e_dyn_idx_t {
 
 struct i40e_tx_buffer {
 	struct i40e_tx_desc *next_to_watch;
-	unsigned long time_stamp;
 	union {
 		struct sk_buff *skb;
 		void *raw_buf;
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_type.h b/drivers/net/ethernet/intel/i40evf/i40e_type.h
index ec9d83a93379..c463ec41579c 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_type.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_type.h
@@ -1108,6 +1108,7 @@ struct i40e_hw_port_stats {
 	/* flow director stats */
 	u64 fd_atr_match;
 	u64 fd_sb_match;
+	u64 fd_atr_tunnel_match;
 	/* EEE LPI */
 	u32 tx_lpi_status;
 	u32 rx_lpi_status;
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf.h b/drivers/net/ethernet/intel/i40evf/i40evf.h
index 1b98c25b3092..fea3b75a9a35 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf.h
+++ b/drivers/net/ethernet/intel/i40evf/i40evf.h
@@ -264,7 +264,6 @@ extern const char i40evf_driver_version[];
 
 int i40evf_up(struct i40evf_adapter *adapter);
 void i40evf_down(struct i40evf_adapter *adapter);
-void i40evf_reinit_locked(struct i40evf_adapter *adapter);
 void i40evf_reset(struct i40evf_adapter *adapter);
 void i40evf_set_ethtool_ops(struct net_device *netdev);
 void i40evf_update_stats(struct i40evf_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c b/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
index f4e77665bc54..2b53c870e7f1 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
@@ -267,8 +267,10 @@ static int i40evf_set_ringparam(struct net_device *netdev,
 	adapter->tx_desc_count = new_tx_count;
 	adapter->rx_desc_count = new_rx_count;
 
-	if (netif_running(netdev))
-		i40evf_reinit_locked(adapter);
+	if (netif_running(netdev)) {
+		adapter->flags |= I40EVF_FLAG_RESET_NEEDED;
+		schedule_work(&adapter->reset_task);
+	}
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_main.c b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
index 7c53aca4b5a6..4ab4ebba07a1 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_main.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
@@ -170,7 +170,8 @@ static void i40evf_tx_timeout(struct net_device *netdev)
 	struct i40evf_adapter *adapter = netdev_priv(netdev);
 
 	adapter->tx_timeout_count++;
-	if (!(adapter->flags & I40EVF_FLAG_RESET_PENDING)) {
+	if (!(adapter->flags & (I40EVF_FLAG_RESET_PENDING |
+				I40EVF_FLAG_RESET_NEEDED))) {
 		adapter->flags |= I40EVF_FLAG_RESET_NEEDED;
 		schedule_work(&adapter->reset_task);
 	}
@@ -1460,7 +1461,7 @@ static void i40evf_configure_rss(struct i40evf_adapter *adapter)
 	for (i = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++) {
 		lut = 0;
 		for (j = 0; j < 4; j++) {
-			if (cqueue == adapter->vsi_res->num_queue_pairs)
+			if (cqueue == adapter->num_active_queues)
 				cqueue = 0;
 			lut |= ((cqueue) << (8 * j));
 			cqueue++;
@@ -1470,8 +1471,8 @@ static void i40evf_configure_rss(struct i40evf_adapter *adapter)
 	i40e_flush(hw);
 }
 
-#define I40EVF_RESET_WAIT_MS 100
-#define I40EVF_RESET_WAIT_COUNT 200
+#define I40EVF_RESET_WAIT_MS 10
+#define I40EVF_RESET_WAIT_COUNT 500
 /**
  * i40evf_reset_task - Call-back task to handle hardware reset
  * @work: pointer to work_struct
@@ -1495,10 +1496,17 @@ static void i40evf_reset_task(struct work_struct *work)
 				&adapter->crit_section))
 		usleep_range(500, 1000);
 
+	i40evf_misc_irq_disable(adapter);
 	if (adapter->flags & I40EVF_FLAG_RESET_NEEDED) {
-		dev_info(&adapter->pdev->dev, "Requesting reset from PF\n");
+		adapter->flags &= ~I40EVF_FLAG_RESET_NEEDED;
+		/* Restart the AQ here. If we have been reset but didn't
+		 * detect it, or if the PF had to reinit, our AQ will be hosed.
+		 */
+		i40evf_shutdown_adminq(hw);
+		i40evf_init_adminq(hw);
 		i40evf_request_reset(adapter);
 	}
+	adapter->flags |= I40EVF_FLAG_RESET_PENDING;
 
 	/* poll until we see the reset actually happen */
 	for (i = 0; i < I40EVF_RESET_WAIT_COUNT; i++) {
@@ -1507,10 +1515,10 @@ static void i40evf_reset_task(struct work_struct *work)
 		if ((rstat_val != I40E_VFR_VFACTIVE) &&
 		    (rstat_val != I40E_VFR_COMPLETED))
 			break;
-		msleep(I40EVF_RESET_WAIT_MS);
+		usleep_range(500, 1000);
 	}
 	if (i == I40EVF_RESET_WAIT_COUNT) {
-		adapter->flags &= ~I40EVF_FLAG_RESET_PENDING;
+		dev_info(&adapter->pdev->dev, "Never saw reset\n");
 		goto continue_reset; /* act like the reset happened */
 	}
 
@@ -1518,11 +1526,12 @@ static void i40evf_reset_task(struct work_struct *work)
 	for (i = 0; i < I40EVF_RESET_WAIT_COUNT; i++) {
 		rstat_val = rd32(hw, I40E_VFGEN_RSTAT) &
 			    I40E_VFGEN_RSTAT_VFR_STATE_MASK;
-		if ((rstat_val == I40E_VFR_VFACTIVE) ||
-		    (rstat_val == I40E_VFR_COMPLETED))
+		if (rstat_val == I40E_VFR_VFACTIVE)
 			break;
 		msleep(I40EVF_RESET_WAIT_MS);
 	}
+	/* extra wait to make sure minimum wait is met */
+	msleep(I40EVF_RESET_WAIT_MS);
 	if (i == I40EVF_RESET_WAIT_COUNT) {
 		struct i40evf_mac_filter *f, *ftmp;
 		struct i40evf_vlan_filter *fv, *fvtmp;
@@ -1534,11 +1543,10 @@ static void i40evf_reset_task(struct work_struct *work)
 
 		if (netif_running(adapter->netdev)) {
 			set_bit(__I40E_DOWN, &adapter->vsi.state);
-			i40evf_irq_disable(adapter);
-			i40evf_napi_disable_all(adapter);
-			netif_tx_disable(netdev);
-			netif_tx_stop_all_queues(netdev);
 			netif_carrier_off(netdev);
+			netif_tx_disable(netdev);
+			i40evf_napi_disable_all(adapter);
+			i40evf_irq_disable(adapter);
 			i40evf_free_traffic_irqs(adapter);
 			i40evf_free_all_tx_resources(adapter);
 			i40evf_free_all_rx_resources(adapter);
@@ -1550,6 +1558,7 @@ static void i40evf_reset_task(struct work_struct *work)
 			list_del(&f->list);
 			kfree(f);
 		}
+
 		list_for_each_entry_safe(fv, fvtmp, &adapter->vlan_filter_list,
 					 list) {
 			list_del(&fv->list);
@@ -1564,22 +1573,27 @@ static void i40evf_reset_task(struct work_struct *work)
 		i40evf_shutdown_adminq(hw);
 		adapter->netdev->flags &= ~IFF_UP;
 		clear_bit(__I40EVF_IN_CRITICAL_TASK, &adapter->crit_section);
+		adapter->flags &= ~I40EVF_FLAG_RESET_PENDING;
+		dev_info(&adapter->pdev->dev, "Reset task did not complete, VF disabled\n");
 		return; /* Do not attempt to reinit. It's dead, Jim. */
 	}
 
 continue_reset:
-	adapter->flags &= ~I40EVF_FLAG_RESET_PENDING;
-
-	i40evf_irq_disable(adapter);
-
 	if (netif_running(adapter->netdev)) {
-		i40evf_napi_disable_all(adapter);
-		netif_tx_disable(netdev);
-		netif_tx_stop_all_queues(netdev);
 		netif_carrier_off(netdev);
+		netif_tx_stop_all_queues(netdev);
+		i40evf_napi_disable_all(adapter);
 	}
+	i40evf_irq_disable(adapter);
 
 	adapter->state = __I40EVF_RESETTING;
+	adapter->flags &= ~I40EVF_FLAG_RESET_PENDING;
+
+	/* free the Tx/Rx rings and descriptors, might be better to just
+	 * re-use them sometime in the future
+	 */
+	i40evf_free_all_rx_resources(adapter);
+	i40evf_free_all_tx_resources(adapter);
 
 	/* kill and reinit the admin queue */
 	if (i40evf_shutdown_adminq(hw))
@@ -1603,6 +1617,7 @@ continue_reset:
 	adapter->aq_required = I40EVF_FLAG_AQ_ADD_MAC_FILTER;
 	adapter->aq_required |= I40EVF_FLAG_AQ_ADD_VLAN_FILTER;
 	clear_bit(__I40EVF_IN_CRITICAL_TASK, &adapter->crit_section);
+	i40evf_misc_irq_enable(adapter);
 
 	mod_timer(&adapter->watchdog_timer, jiffies + 2);
 
@@ -1624,7 +1639,10 @@ continue_reset:
 			goto reset_err;
 
 		i40evf_irq_enable(adapter, true);
+	} else {
+		adapter->state = __I40EVF_DOWN;
 	}
+
 	return;
 reset_err:
 	dev_err(&adapter->pdev->dev, "failed to allocate resources during reinit\n");
@@ -1667,6 +1685,11 @@ static void i40evf_adminq_task(struct work_struct *work)
 			memset(event.msg_buf, 0, I40EVF_MAX_AQ_BUF_SIZE);
 	} while (pending);
 
+	if ((adapter->flags &
+	     (I40EVF_FLAG_RESET_PENDING | I40EVF_FLAG_RESET_NEEDED)) ||
+	    adapter->state == __I40EVF_RESETTING)
+		goto freedom;
+
 	/* check for error indications */
 	val = rd32(hw, hw->aq.arq.len);
 	oldval = val;
@@ -1702,6 +1725,7 @@ static void i40evf_adminq_task(struct work_struct *work)
 	if (oldval != val)
 		wr32(hw, hw->aq.asq.len, val);
 
+freedom:
 	kfree(event.msg_buf);
 out:
 	/* re-enable Admin queue interrupt cause */
@@ -1897,47 +1921,6 @@ static struct net_device_stats *i40evf_get_stats(struct net_device *netdev)
 }
 
 /**
- * i40evf_reinit_locked - Software reinit
- * @adapter: board private structure
- *
- * Reinititalizes the ring structures in response to a software configuration
- * change. Roughly the same as close followed by open, but skips releasing
- * and reallocating the interrupts.
- **/
-void i40evf_reinit_locked(struct i40evf_adapter *adapter)
-{
-	struct net_device *netdev = adapter->netdev;
-	int err;
-
-	WARN_ON(in_interrupt());
-
-	i40evf_down(adapter);
-
-	/* allocate transmit descriptors */
-	err = i40evf_setup_all_tx_resources(adapter);
-	if (err)
-		goto err_reinit;
-
-	/* allocate receive descriptors */
-	err = i40evf_setup_all_rx_resources(adapter);
-	if (err)
-		goto err_reinit;
-
-	i40evf_configure(adapter);
-
-	err = i40evf_up_complete(adapter);
-	if (err)
-		goto err_reinit;
-
-	i40evf_irq_enable(adapter, true);
-	return;
-
-err_reinit:
-	dev_err(&adapter->pdev->dev, "failed to allocate resources during reinit\n");
-	i40evf_close(netdev);
-}
-
-/**
  * i40evf_change_mtu - Change the Maximum Transfer Unit
  * @netdev: network interface device structure
  * @new_mtu: new value for maximum frame size
@@ -1952,9 +1935,10 @@ static int i40evf_change_mtu(struct net_device *netdev, int new_mtu)
 	if ((new_mtu < 68) || (max_frame > I40E_MAX_RXBUFFER))
 		return -EINVAL;
 
-	/* must set new MTU before calling down or up */
 	netdev->mtu = new_mtu;
-	i40evf_reinit_locked(adapter);
+	adapter->flags |= I40EVF_FLAG_RESET_NEEDED;
+	schedule_work(&adapter->reset_task);
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.c b/drivers/net/ethernet/intel/igb/e1000_82575.c
index 0f69ef81751a..b0182dd31346 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.c
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.c
@@ -1,5 +1,5 @@
 /* Intel(R) Gigabit Ethernet Linux driver
- * Copyright(c) 2007-2014 Intel Corporation.
+ * Copyright(c) 2007-2015 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,
@@ -1900,8 +1900,8 @@ static void igb_clear_hw_cntrs_82575(struct e1000_hw *hw)
  *  igb_rx_fifo_flush_82575 - Clean rx fifo after RX enable
  *  @hw: pointer to the HW structure
  *
- *  After rx enable if managability is enabled then there is likely some
- *  bad data at the start of the fifo and possibly in the DMA fifo.  This
+ *  After rx enable if manageability is enabled then there is likely some
+ *  bad data at the start of the fifo and possibly in the DMA fifo. This
  *  function clears the fifos and flushes any packets that came in as rx was
  *  being enabled.
  **/
@@ -1910,6 +1910,11 @@ void igb_rx_fifo_flush_82575(struct e1000_hw *hw)
 	u32 rctl, rlpml, rxdctl[4], rfctl, temp_rctl, rx_enabled;
 	int i, ms_wait;
 
+	/* disable IPv6 options as per hardware errata */
+	rfctl = rd32(E1000_RFCTL);
+	rfctl |= E1000_RFCTL_IPV6_EX_DIS;
+	wr32(E1000_RFCTL, rfctl);
+
 	if (hw->mac.type != e1000_82575 ||
 	    !(rd32(E1000_MANC) & E1000_MANC_RCV_TCO_EN))
 		return;
@@ -1937,7 +1942,6 @@ void igb_rx_fifo_flush_82575(struct e1000_hw *hw)
 	 * incoming packets are rejected.  Set enable and wait 2ms so that
 	 * any packet that was coming in as RCTL.EN was set is flushed
 	 */
-	rfctl = rd32(E1000_RFCTL);
 	wr32(E1000_RFCTL, rfctl & ~E1000_RFCTL_LEF);
 
 	rlpml = rd32(E1000_RLPML);
diff --git a/drivers/net/ethernet/intel/igb/e1000_defines.h b/drivers/net/ethernet/intel/igb/e1000_defines.h
index 217f8138851b..f8684aa285be 100644
--- a/drivers/net/ethernet/intel/igb/e1000_defines.h
+++ b/drivers/net/ethernet/intel/igb/e1000_defines.h
@@ -344,7 +344,8 @@
 #define E1000_RXCSUM_PCSD      0x00002000   /* packet checksum disabled */
 
 /* Header split receive */
-#define E1000_RFCTL_LEF        0x00040000
+#define E1000_RFCTL_IPV6_EX_DIS         0x00010000
+#define E1000_RFCTL_LEF                 0x00040000
 
 /* Collision related configuration parameters */
 #define E1000_COLLISION_THRESHOLD       15
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index a0a9b1fcb5e8..2f70a9b152bd 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -58,7 +58,7 @@
 
 #define MAJ 5
 #define MIN 2
-#define BUILD 15
+#define BUILD 18
 #define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." \
 __stringify(BUILD) "-k"
 char igb_driver_name[] = "igb";
@@ -1836,31 +1836,19 @@ void igb_reinit_locked(struct igb_adapter *adapter)
  *
  * @adapter: adapter struct
  **/
-static s32 igb_enable_mas(struct igb_adapter *adapter)
+static void igb_enable_mas(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	u32 connsw;
-	s32 ret_val = 0;
-
-	connsw = rd32(E1000_CONNSW);
-	if (!(hw->phy.media_type == e1000_media_type_copper))
-		return ret_val;
+	u32 connsw = rd32(E1000_CONNSW);
 
 	/* configure for SerDes media detect */
-	if (!(connsw & E1000_CONNSW_SERDESD)) {
+	if ((hw->phy.media_type == e1000_media_type_copper) &&
+	    (!(connsw & E1000_CONNSW_SERDESD))) {
 		connsw |= E1000_CONNSW_ENRGSRC;
 		connsw |= E1000_CONNSW_AUTOSENSE_EN;
 		wr32(E1000_CONNSW, connsw);
 		wrfl();
-	} else if (connsw & E1000_CONNSW_SERDESD) {
-		/* already SerDes, no need to enable anything */
-		return ret_val;
-	} else {
-		netdev_info(adapter->netdev,
-			"MAS: Unable to configure feature, disabling..\n");
-		adapter->flags &= ~IGB_FLAG_MAS_ENABLE;
 	}
-	return ret_val;
 }
 
 void igb_reset(struct igb_adapter *adapter)
@@ -1980,10 +1968,9 @@ void igb_reset(struct igb_adapter *adapter)
 		adapter->ei.get_invariants(hw);
 		adapter->flags &= ~IGB_FLAG_MEDIA_RESET;
 	}
-	if (adapter->flags & IGB_FLAG_MAS_ENABLE) {
-		if (igb_enable_mas(adapter))
-			dev_err(&pdev->dev,
-				"Error enabling Media Auto Sense\n");
+	if ((mac->type == e1000_82575) &&
+	    (adapter->flags & IGB_FLAG_MAS_ENABLE)) {
+		igb_enable_mas(adapter);
 	}
 	if (hw->mac.ops.init_hw(hw))
 		dev_err(&pdev->dev, "Hardware Error\n");
@@ -4989,6 +4976,7 @@ netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
 	struct igb_tx_buffer *first;
 	int tso;
 	u32 tx_flags = 0;
+	unsigned short f;
 	u16 count = TXD_USE_COUNT(skb_headlen(skb));
 	__be16 protocol = vlan_get_protocol(skb);
 	u8 hdr_len = 0;
@@ -4999,14 +4987,8 @@ netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
 	 *       + 1 desc for context descriptor,
 	 * otherwise try next time
 	 */
-	if (NETDEV_FRAG_PAGE_MAX_SIZE > IGB_MAX_DATA_PER_TXD) {
-		unsigned short f;
-
-		for (f = 0; f < skb_shinfo(skb)->nr_frags; f++)
-			count += TXD_USE_COUNT(skb_shinfo(skb)->frags[f].size);
-	} else {
-		count += skb_shinfo(skb)->nr_frags;
-	}
+	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++)
+		count += TXD_USE_COUNT(skb_shinfo(skb)->frags[f].size);
 
 	if (igb_maybe_stop_tx(tx_ring, count + 3)) {
 		/* this is a hard error */
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index e3b9b63ad010..c3a9392cbc19 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -538,8 +538,8 @@ static int igb_ptp_feature_enable_i210(struct ptp_clock_info *ptp,
 			igb->perout[i].start.tv_nsec = rq->perout.start.nsec;
 			igb->perout[i].period.tv_sec = ts.tv_sec;
 			igb->perout[i].period.tv_nsec = ts.tv_nsec;
-			wr32(trgttiml, rq->perout.start.sec);
-			wr32(trgttimh, rq->perout.start.nsec);
+			wr32(trgttimh, rq->perout.start.sec);
+			wr32(trgttiml, rq->perout.start.nsec);
 			tsauxc |= tsauxc_mask;
 			tsim |= tsim_mask;
 		} else {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 636f9e350162..ac3ac2a20386 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -643,6 +643,7 @@ struct ixgbe_adapter {
 #define IXGBE_FLAG2_RSS_FIELD_IPV4_UDP		(u32)(1 << 8)
 #define IXGBE_FLAG2_RSS_FIELD_IPV6_UDP		(u32)(1 << 9)
 #define IXGBE_FLAG2_PTP_PPS_ENABLED		(u32)(1 << 10)
+#define IXGBE_FLAG2_PHY_INTERRUPT		(u32)(1 << 11)
 
 	/* Tx fast path data */
 	int num_tx_queues;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
index 824a7ab79ab6..65db69b862fb 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
@@ -1225,7 +1225,7 @@ static struct ixgbe_phy_operations phy_ops_82598 = {
 	.setup_link_speed	= &ixgbe_setup_phy_link_speed_generic,
 	.read_i2c_sff8472	= &ixgbe_read_i2c_sff8472_82598,
 	.read_i2c_eeprom	= &ixgbe_read_i2c_eeprom_82598,
-	.check_overtemp   = &ixgbe_tn_check_overtemp,
+	.check_overtemp		= &ixgbe_tn_check_overtemp,
 };
 
 struct ixgbe_info ixgbe_82598_info = {
@@ -1234,4 +1234,5 @@ struct ixgbe_info ixgbe_82598_info = {
 	.mac_ops		= &mac_ops_82598,
 	.eeprom_ops		= &eeprom_ops_82598,
 	.phy_ops		= &phy_ops_82598,
+	.mvals			= ixgbe_mvals_8259X,
 };
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
index e0c363948bf4..6b87d9634614 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
@@ -71,7 +71,7 @@ bool ixgbe_mng_enabled(struct ixgbe_hw *hw)
 {
 	u32 fwsm, manc, factps;
 
-	fwsm = IXGBE_READ_REG(hw, IXGBE_FWSM);
+	fwsm = IXGBE_READ_REG(hw, IXGBE_FWSM(hw));
 	if ((fwsm & IXGBE_FWSM_MODE_MASK) != IXGBE_FWSM_FW_MODE_PT)
 		return false;
 
@@ -79,7 +79,7 @@ bool ixgbe_mng_enabled(struct ixgbe_hw *hw)
 	if (!(manc & IXGBE_MANC_RCV_TCO_EN))
 		return false;
 
-	factps = IXGBE_READ_REG(hw, IXGBE_FACTPS);
+	factps = IXGBE_READ_REG(hw, IXGBE_FACTPS(hw));
 	if (factps & IXGBE_FACTPS_MNGCG)
 		return false;
 
@@ -510,7 +510,7 @@ static void ixgbe_stop_mac_link_on_d3_82599(struct ixgbe_hw *hw)
 	hw->eeprom.ops.read(hw, IXGBE_EEPROM_CTRL_2, &ee_ctrl_2);
 
 	/* Check to see if MNG FW could be enabled */
-	fwsm = IXGBE_READ_REG(hw, IXGBE_FWSM);
+	fwsm = IXGBE_READ_REG(hw, IXGBE_FWSM(hw));
 
 	if (((fwsm & IXGBE_FWSM_MODE_MASK) != IXGBE_FWSM_FW_MODE_PT) &&
 	    !hw->wol_enabled &&
@@ -2378,4 +2378,5 @@ struct ixgbe_info ixgbe_82599_info = {
 	.eeprom_ops             = &eeprom_ops_82599,
 	.phy_ops                = &phy_ops_82599,
 	.mbx_ops                = &mbx_ops_generic,
+	.mvals			= ixgbe_mvals_8259X,
 };
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index 06d8f3cfa099..4c1c26732b67 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -57,6 +57,11 @@ static s32 ixgbe_detect_eeprom_page_size_generic(struct ixgbe_hw *hw,
 						 u16 offset);
 static s32 ixgbe_disable_pcie_master(struct ixgbe_hw *hw);
 
+/* Base table for registers values that change by MAC */
+const u32 ixgbe_mvals_8259X[IXGBE_MVALS_IDX_LIMIT] = {
+	IXGBE_MVALS_INIT(8259X)
+};
+
 /**
  *  ixgbe_device_supports_autoneg_fc - Check if phy supports autoneg flow
  *  control
@@ -91,6 +96,8 @@ bool ixgbe_device_supports_autoneg_fc(struct ixgbe_hw *hw)
 		case IXGBE_DEV_ID_82599_T3_LOM:
 		case IXGBE_DEV_ID_X540T:
 		case IXGBE_DEV_ID_X540T1:
+		case IXGBE_DEV_ID_X550T:
+		case IXGBE_DEV_ID_X550EM_X_10G_T:
 			supported = true;
 			break;
 		default:
@@ -463,7 +470,7 @@ s32 ixgbe_clear_hw_cntrs_generic(struct ixgbe_hw *hw)
 		}
 	}
 
-	if (hw->mac.type == ixgbe_mac_X540) {
+	if (hw->mac.type == ixgbe_mac_X550 || hw->mac.type == ixgbe_mac_X540) {
 		if (hw->phy.id == 0)
 			hw->phy.ops.identify(hw);
 		hw->phy.ops.read_reg(hw, IXGBE_PCRC8ECL, MDIO_MMD_PCS, &i);
@@ -681,7 +688,7 @@ void ixgbe_set_lan_id_multi_port_pcie(struct ixgbe_hw *hw)
 	bus->lan_id = bus->func;
 
 	/* check for a port swap */
-	reg = IXGBE_READ_REG(hw, IXGBE_FACTPS);
+	reg = IXGBE_READ_REG(hw, IXGBE_FACTPS(hw));
 	if (reg & IXGBE_FACTPS_LFS)
 		bus->func ^= 0x1;
 }
@@ -799,7 +806,7 @@ s32 ixgbe_init_eeprom_params_generic(struct ixgbe_hw *hw)
 		 * Check for EEPROM present first.
 		 * If not present leave as none
 		 */
-		eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+		eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 		if (eec & IXGBE_EEC_PRES) {
 			eeprom->type = ixgbe_eeprom_spi;
 
@@ -1283,14 +1290,14 @@ static s32 ixgbe_acquire_eeprom(struct ixgbe_hw *hw)
 	if (hw->mac.ops.acquire_swfw_sync(hw, IXGBE_GSSR_EEP_SM) != 0)
 		return IXGBE_ERR_SWFW_SYNC;
 
-	eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+	eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 
 	/* Request EEPROM Access */
 	eec |= IXGBE_EEC_REQ;
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 
 	for (i = 0; i < IXGBE_EEPROM_GRANT_ATTEMPTS; i++) {
-		eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+		eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 		if (eec & IXGBE_EEC_GNT)
 			break;
 		udelay(5);
@@ -1299,7 +1306,7 @@ static s32 ixgbe_acquire_eeprom(struct ixgbe_hw *hw)
 	/* Release if grant not acquired */
 	if (!(eec & IXGBE_EEC_GNT)) {
 		eec &= ~IXGBE_EEC_REQ;
-		IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+		IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 		hw_dbg(hw, "Could not acquire EEPROM grant\n");
 
 		hw->mac.ops.release_swfw_sync(hw, IXGBE_GSSR_EEP_SM);
@@ -1309,7 +1316,7 @@ static s32 ixgbe_acquire_eeprom(struct ixgbe_hw *hw)
 	/* Setup EEPROM for Read/Write */
 	/* Clear CS and SK */
 	eec &= ~(IXGBE_EEC_CS | IXGBE_EEC_SK);
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 	IXGBE_WRITE_FLUSH(hw);
 	udelay(1);
 	return 0;
@@ -1333,7 +1340,7 @@ static s32 ixgbe_get_eeprom_semaphore(struct ixgbe_hw *hw)
 		 * If the SMBI bit is 0 when we read it, then the bit will be
 		 * set and we have the semaphore
 		 */
-		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM);
+		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM(hw));
 		if (!(swsm & IXGBE_SWSM_SMBI))
 			break;
 		usleep_range(50, 100);
@@ -1353,7 +1360,7 @@ static s32 ixgbe_get_eeprom_semaphore(struct ixgbe_hw *hw)
 		 * If the SMBI bit is 0 when we read it, then the bit will be
 		 * set and we have the semaphore
 		 */
-		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM);
+		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM(hw));
 		if (swsm & IXGBE_SWSM_SMBI) {
 			hw_dbg(hw, "Software semaphore SMBI between device drivers not granted.\n");
 			return IXGBE_ERR_EEPROM;
@@ -1362,16 +1369,16 @@ static s32 ixgbe_get_eeprom_semaphore(struct ixgbe_hw *hw)
 
 	/* Now get the semaphore between SW/FW through the SWESMBI bit */
 	for (i = 0; i < timeout; i++) {
-		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM);
+		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM(hw));
 
 		/* Set the SW EEPROM semaphore bit to request access */
 		swsm |= IXGBE_SWSM_SWESMBI;
-		IXGBE_WRITE_REG(hw, IXGBE_SWSM, swsm);
+		IXGBE_WRITE_REG(hw, IXGBE_SWSM(hw), swsm);
 
 		/* If we set the bit successfully then we got the
 		 * semaphore.
 		 */
-		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM);
+		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM(hw));
 		if (swsm & IXGBE_SWSM_SWESMBI)
 			break;
 
@@ -1400,11 +1407,11 @@ static void ixgbe_release_eeprom_semaphore(struct ixgbe_hw *hw)
 {
 	u32 swsm;
 
-	swsm = IXGBE_READ_REG(hw, IXGBE_SWSM);
+	swsm = IXGBE_READ_REG(hw, IXGBE_SWSM(hw));
 
 	/* Release both semaphores by writing 0 to the bits SWESMBI and SMBI */
 	swsm &= ~(IXGBE_SWSM_SWESMBI | IXGBE_SWSM_SMBI);
-	IXGBE_WRITE_REG(hw, IXGBE_SWSM, swsm);
+	IXGBE_WRITE_REG(hw, IXGBE_SWSM(hw), swsm);
 	IXGBE_WRITE_FLUSH(hw);
 }
 
@@ -1454,15 +1461,15 @@ static void ixgbe_standby_eeprom(struct ixgbe_hw *hw)
 {
 	u32 eec;
 
-	eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+	eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 
 	/* Toggle CS to flush commands */
 	eec |= IXGBE_EEC_CS;
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 	IXGBE_WRITE_FLUSH(hw);
 	udelay(1);
 	eec &= ~IXGBE_EEC_CS;
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 	IXGBE_WRITE_FLUSH(hw);
 	udelay(1);
 }
@@ -1480,7 +1487,7 @@ static void ixgbe_shift_out_eeprom_bits(struct ixgbe_hw *hw, u16 data,
 	u32 mask;
 	u32 i;
 
-	eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+	eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 
 	/*
 	 * Mask is used to shift "count" bits of "data" out to the EEPROM
@@ -1501,7 +1508,7 @@ static void ixgbe_shift_out_eeprom_bits(struct ixgbe_hw *hw, u16 data,
 		else
 			eec &= ~IXGBE_EEC_DI;
 
-		IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+		IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 		IXGBE_WRITE_FLUSH(hw);
 
 		udelay(1);
@@ -1518,7 +1525,7 @@ static void ixgbe_shift_out_eeprom_bits(struct ixgbe_hw *hw, u16 data,
 
 	/* We leave the "DI" bit set to "0" when we leave this routine. */
 	eec &= ~IXGBE_EEC_DI;
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 	IXGBE_WRITE_FLUSH(hw);
 }
 
@@ -1539,7 +1546,7 @@ static u16 ixgbe_shift_in_eeprom_bits(struct ixgbe_hw *hw, u16 count)
 	 * the value of the "DO" bit.  During this "shifting in" process the
 	 * "DI" bit should always be clear.
 	 */
-	eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+	eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 
 	eec &= ~(IXGBE_EEC_DO | IXGBE_EEC_DI);
 
@@ -1547,7 +1554,7 @@ static u16 ixgbe_shift_in_eeprom_bits(struct ixgbe_hw *hw, u16 count)
 		data = data << 1;
 		ixgbe_raise_eeprom_clk(hw, &eec);
 
-		eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+		eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 
 		eec &= ~(IXGBE_EEC_DI);
 		if (eec & IXGBE_EEC_DO)
@@ -1571,7 +1578,7 @@ static void ixgbe_raise_eeprom_clk(struct ixgbe_hw *hw, u32 *eec)
 	 * (setting the SK bit), then delay
 	 */
 	*eec = *eec | IXGBE_EEC_SK;
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, *eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), *eec);
 	IXGBE_WRITE_FLUSH(hw);
 	udelay(1);
 }
@@ -1588,7 +1595,7 @@ static void ixgbe_lower_eeprom_clk(struct ixgbe_hw *hw, u32 *eec)
 	 * delay
 	 */
 	*eec = *eec & ~IXGBE_EEC_SK;
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, *eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), *eec);
 	IXGBE_WRITE_FLUSH(hw);
 	udelay(1);
 }
@@ -1601,19 +1608,19 @@ static void ixgbe_release_eeprom(struct ixgbe_hw *hw)
 {
 	u32 eec;
 
-	eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+	eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 
 	eec |= IXGBE_EEC_CS;  /* Pull CS high */
 	eec &= ~IXGBE_EEC_SK; /* Lower SCK */
 
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 	IXGBE_WRITE_FLUSH(hw);
 
 	udelay(1);
 
 	/* Stop requesting EEPROM access */
 	eec &= ~IXGBE_EEC_REQ;
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, eec);
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), eec);
 
 	hw->mac.ops.release_swfw_sync(hw, IXGBE_GSSR_EEP_SM);
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
index f21f8a165ec4..ec015fed8fa7 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
@@ -118,6 +118,8 @@ bool ixgbe_mng_enabled(struct ixgbe_hw *hw);
 void ixgbe_set_rxpba_generic(struct ixgbe_hw *hw, int num_pb,
 			     u32 headroom, int strategy);
 
+extern const u32 ixgbe_mvals_8259X[IXGBE_MVALS_IDX_LIMIT];
+
 #define IXGBE_I2C_THERMAL_SENSOR_ADDR	0xF8
 #define IXGBE_EMC_INTERNAL_DATA		0x00
 #define IXGBE_EMC_INTERNAL_THERM_LIMIT	0x20
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index eafa9ec802ba..ec7b2324b77b 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -207,6 +207,7 @@ static int ixgbe_get_settings(struct net_device *netdev,
 	switch (adapter->hw.phy.type) {
 	case ixgbe_phy_tn:
 	case ixgbe_phy_aq:
+	case ixgbe_phy_x550em_ext_t:
 	case ixgbe_phy_cu_unknown:
 		ecmd->supported |= SUPPORTED_TP;
 		ecmd->advertising |= ADVERTISED_TP;
@@ -470,16 +471,16 @@ static void ixgbe_get_regs(struct net_device *netdev,
 	regs_buff[7] = IXGBE_READ_REG(hw, IXGBE_TCPTIMER);
 
 	/* NVM Register */
-	regs_buff[8] = IXGBE_READ_REG(hw, IXGBE_EEC);
+	regs_buff[8] = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 	regs_buff[9] = IXGBE_READ_REG(hw, IXGBE_EERD);
-	regs_buff[10] = IXGBE_READ_REG(hw, IXGBE_FLA);
+	regs_buff[10] = IXGBE_READ_REG(hw, IXGBE_FLA(hw));
 	regs_buff[11] = IXGBE_READ_REG(hw, IXGBE_EEMNGCTL);
 	regs_buff[12] = IXGBE_READ_REG(hw, IXGBE_EEMNGDATA);
 	regs_buff[13] = IXGBE_READ_REG(hw, IXGBE_FLMNGCTL);
 	regs_buff[14] = IXGBE_READ_REG(hw, IXGBE_FLMNGDATA);
 	regs_buff[15] = IXGBE_READ_REG(hw, IXGBE_FLMNGCNT);
 	regs_buff[16] = IXGBE_READ_REG(hw, IXGBE_FLOP);
-	regs_buff[17] = IXGBE_READ_REG(hw, IXGBE_GRC);
+	regs_buff[17] = IXGBE_READ_REG(hw, IXGBE_GRC(hw));
 
 	/* Interrupt */
 	/* don't read EICR because it can clear interrupt causes, instead
@@ -2594,18 +2595,35 @@ static int ixgbe_add_ethtool_fdir_entry(struct ixgbe_adapter *adapter,
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct ixgbe_fdir_filter *input;
 	union ixgbe_atr_input mask;
+	u8 queue;
 	int err;
 
 	if (!(adapter->flags & IXGBE_FLAG_FDIR_PERFECT_CAPABLE))
 		return -EOPNOTSUPP;
 
-	/*
-	 * Don't allow programming if the action is a queue greater than
-	 * the number of online Rx queues.
+	/* ring_cookie is a masked into a set of queues and ixgbe pools or
+	 * we use the drop index.
 	 */
-	if ((fsp->ring_cookie != RX_CLS_FLOW_DISC) &&
-	    (fsp->ring_cookie >= adapter->num_rx_queues))
-		return -EINVAL;
+	if (fsp->ring_cookie == RX_CLS_FLOW_DISC) {
+		queue = IXGBE_FDIR_DROP_QUEUE;
+	} else {
+		u32 ring = ethtool_get_flow_spec_ring(fsp->ring_cookie);
+		u8 vf = ethtool_get_flow_spec_ring_vf(fsp->ring_cookie);
+
+		if (!vf && (ring >= adapter->num_rx_queues))
+			return -EINVAL;
+		else if (vf &&
+			 ((vf > adapter->num_vfs) ||
+			   ring >= adapter->num_rx_queues_per_pool))
+			return -EINVAL;
+
+		/* Map the ring onto the absolute queue index */
+		if (!vf)
+			queue = adapter->rx_ring[ring]->reg_idx;
+		else
+			queue = ((vf - 1) *
+				adapter->num_rx_queues_per_pool) + ring;
+	}
 
 	/* Don't allow indexes to exist outside of available space */
 	if (fsp->location >= ((1024 << adapter->fdir_pballoc) - 2)) {
@@ -2683,10 +2701,7 @@ static int ixgbe_add_ethtool_fdir_entry(struct ixgbe_adapter *adapter,
 
 	/* program filters to filter memory */
 	err = ixgbe_fdir_write_perfect_filter_82599(hw,
-				&input->filter, input->sw_idx,
-				(input->action == IXGBE_FDIR_DROP_QUEUE) ?
-				IXGBE_FDIR_DROP_QUEUE :
-				adapter->rx_ring[input->action]->reg_idx);
+				&input->filter, input->sw_idx, queue);
 	if (err)
 		goto err_out_w_lock;
 
@@ -3053,7 +3068,7 @@ static int ixgbe_get_module_info(struct net_device *dev,
 {
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
 	struct ixgbe_hw *hw = &adapter->hw;
-	u32 status;
+	s32 status;
 	u8 sff8472_rev, addr_mode;
 	bool page_swap = false;
 
@@ -3061,14 +3076,14 @@ static int ixgbe_get_module_info(struct net_device *dev,
 	status = hw->phy.ops.read_i2c_eeprom(hw,
 					     IXGBE_SFF_SFF_8472_COMP,
 					     &sff8472_rev);
-	if (status != 0)
+	if (status)
 		return -EIO;
 
 	/* addressing mode is not supported */
 	status = hw->phy.ops.read_i2c_eeprom(hw,
 					     IXGBE_SFF_SFF_8472_SWAP,
 					     &addr_mode);
-	if (status != 0)
+	if (status)
 		return -EIO;
 
 	if (addr_mode & IXGBE_SFF_ADDRESSING_MODE) {
@@ -3095,7 +3110,7 @@ static int ixgbe_get_module_eeprom(struct net_device *dev,
 {
 	struct ixgbe_adapter *adapter = netdev_priv(dev);
 	struct ixgbe_hw *hw = &adapter->hw;
-	u32 status = IXGBE_ERR_PHY_ADDR_INVALID;
+	s32 status = IXGBE_ERR_PHY_ADDR_INVALID;
 	u8 databyte = 0xFF;
 	int i = 0;
 
@@ -3112,7 +3127,7 @@ static int ixgbe_get_module_eeprom(struct net_device *dev,
 		else
 			status = hw->phy.ops.read_i2c_sff8472(hw, i, &databyte);
 
-		if (status != 0)
+		if (status)
 			return -EIO;
 
 		data[i - ee->offset] = databyte;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index 5be12a00e1f4..9aa6104e34ea 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -81,6 +81,8 @@ const char ixgbe_driver_version[] = DRV_VERSION;
 static const char ixgbe_copyright[] =
 				"Copyright (c) 1999-2014 Intel Corporation.";
 
+static const char ixgbe_overheat_msg[] = "Network adapter has been stopped because it has over heated. Restart the computer. If the problem persists, power off the system and replace the adapter";
+
 static const struct ixgbe_info *ixgbe_info_tbl[] = {
 	[board_82598]		= &ixgbe_82598_info,
 	[board_82599]		= &ixgbe_82599_info,
@@ -131,6 +133,7 @@ static const struct pci_device_id ixgbe_pci_tbl[] = {
 	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550T), board_X550},
 	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550EM_X_KX4), board_X550EM_x},
 	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550EM_X_KR), board_X550EM_x},
+	{PCI_VDEVICE(INTEL, IXGBE_DEV_ID_X550EM_X_10G_T), board_X550EM_x},
 	/* required last entry */
 	{0, }
 };
@@ -2366,7 +2369,7 @@ static void ixgbe_check_overtemp_subtask(struct ixgbe_adapter *adapter)
 		 *  - We may have missed the interrupt so always have to
 		 *    check if we  got a LSC
 		 */
-		if (!(eicr & IXGBE_EICR_GPI_SDP0) &&
+		if (!(eicr & IXGBE_EICR_GPI_SDP0_8259X) &&
 		    !(eicr & IXGBE_EICR_LSC))
 			return;
 
@@ -2386,14 +2389,13 @@ static void ixgbe_check_overtemp_subtask(struct ixgbe_adapter *adapter)
 
 		break;
 	default:
-		if (!(eicr & IXGBE_EICR_GPI_SDP0))
+		if (adapter->hw.mac.type >= ixgbe_mac_X540)
+			return;
+		if (!(eicr & IXGBE_EICR_GPI_SDP0(hw)))
 			return;
 		break;
 	}
-	e_crit(drv,
-	       "Network adapter has been stopped because it has over heated. "
-	       "Restart the computer. If the problem persists, "
-	       "power off the system and replace the adapter\n");
+	e_crit(drv, "%s\n", ixgbe_overheat_msg);
 
 	adapter->interrupt_event = 0;
 }
@@ -2403,15 +2405,17 @@ static void ixgbe_check_fan_failure(struct ixgbe_adapter *adapter, u32 eicr)
 	struct ixgbe_hw *hw = &adapter->hw;
 
 	if ((adapter->flags & IXGBE_FLAG_FAN_FAIL_CAPABLE) &&
-	    (eicr & IXGBE_EICR_GPI_SDP1)) {
+	    (eicr & IXGBE_EICR_GPI_SDP1(hw))) {
 		e_crit(probe, "Fan has stopped, replace the adapter\n");
 		/* write to clear the interrupt */
-		IXGBE_WRITE_REG(hw, IXGBE_EICR, IXGBE_EICR_GPI_SDP1);
+		IXGBE_WRITE_REG(hw, IXGBE_EICR, IXGBE_EICR_GPI_SDP1(hw));
 	}
 }
 
 static void ixgbe_check_overtemp_event(struct ixgbe_adapter *adapter, u32 eicr)
 {
+	struct ixgbe_hw *hw = &adapter->hw;
+
 	if (!(adapter->flags2 & IXGBE_FLAG2_TEMP_SENSOR_CAPABLE))
 		return;
 
@@ -2421,7 +2425,8 @@ static void ixgbe_check_overtemp_event(struct ixgbe_adapter *adapter, u32 eicr)
 		 * Need to check link state so complete overtemp check
 		 * on service task
 		 */
-		if (((eicr & IXGBE_EICR_GPI_SDP0) || (eicr & IXGBE_EICR_LSC)) &&
+		if (((eicr & IXGBE_EICR_GPI_SDP0(hw)) ||
+		     (eicr & IXGBE_EICR_LSC)) &&
 		    (!test_bit(__IXGBE_DOWN, &adapter->state))) {
 			adapter->interrupt_event = eicr;
 			adapter->flags2 |= IXGBE_FLAG2_TEMP_SENSOR_EVENT;
@@ -2437,28 +2442,46 @@ static void ixgbe_check_overtemp_event(struct ixgbe_adapter *adapter, u32 eicr)
 		return;
 	}
 
-	e_crit(drv,
-	       "Network adapter has been stopped because it has over heated. "
-	       "Restart the computer. If the problem persists, "
-	       "power off the system and replace the adapter\n");
+	e_crit(drv, "%s\n", ixgbe_overheat_msg);
+}
+
+static inline bool ixgbe_is_sfp(struct ixgbe_hw *hw)
+{
+	switch (hw->mac.type) {
+	case ixgbe_mac_82598EB:
+		if (hw->phy.type == ixgbe_phy_nl)
+			return true;
+		return false;
+	case ixgbe_mac_82599EB:
+	case ixgbe_mac_X550EM_x:
+		switch (hw->mac.ops.get_media_type(hw)) {
+		case ixgbe_media_type_fiber:
+		case ixgbe_media_type_fiber_qsfp:
+			return true;
+		default:
+			return false;
+		}
+	default:
+		return false;
+	}
 }
 
 static void ixgbe_check_sfp_event(struct ixgbe_adapter *adapter, u32 eicr)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 
-	if (eicr & IXGBE_EICR_GPI_SDP2) {
+	if (eicr & IXGBE_EICR_GPI_SDP2(hw)) {
 		/* Clear the interrupt */
-		IXGBE_WRITE_REG(hw, IXGBE_EICR, IXGBE_EICR_GPI_SDP2);
+		IXGBE_WRITE_REG(hw, IXGBE_EICR, IXGBE_EICR_GPI_SDP2(hw));
 		if (!test_bit(__IXGBE_DOWN, &adapter->state)) {
 			adapter->flags2 |= IXGBE_FLAG2_SFP_NEEDS_RESET;
 			ixgbe_service_event_schedule(adapter);
 		}
 	}
 
-	if (eicr & IXGBE_EICR_GPI_SDP1) {
+	if (eicr & IXGBE_EICR_GPI_SDP1(hw)) {
 		/* Clear the interrupt */
-		IXGBE_WRITE_REG(hw, IXGBE_EICR, IXGBE_EICR_GPI_SDP1);
+		IXGBE_WRITE_REG(hw, IXGBE_EICR, IXGBE_EICR_GPI_SDP1(hw));
 		if (!test_bit(__IXGBE_DOWN, &adapter->state)) {
 			adapter->flags |= IXGBE_FLAG_NEED_LINK_CONFIG;
 			ixgbe_service_event_schedule(adapter);
@@ -2543,6 +2566,7 @@ static inline void ixgbe_irq_disable_queues(struct ixgbe_adapter *adapter,
 static inline void ixgbe_irq_enable(struct ixgbe_adapter *adapter, bool queues,
 				    bool flush)
 {
+	struct ixgbe_hw *hw = &adapter->hw;
 	u32 mask = (IXGBE_EIMS_ENABLE_MASK & ~IXGBE_EIMS_RTX_QUEUE);
 
 	/* don't reenable LSC while waiting for link */
@@ -2552,7 +2576,7 @@ static inline void ixgbe_irq_enable(struct ixgbe_adapter *adapter, bool queues,
 	if (adapter->flags2 & IXGBE_FLAG2_TEMP_SENSOR_CAPABLE)
 		switch (adapter->hw.mac.type) {
 		case ixgbe_mac_82599EB:
-			mask |= IXGBE_EIMS_GPI_SDP0;
+			mask |= IXGBE_EIMS_GPI_SDP0(hw);
 			break;
 		case ixgbe_mac_X540:
 		case ixgbe_mac_X550:
@@ -2563,15 +2587,17 @@ static inline void ixgbe_irq_enable(struct ixgbe_adapter *adapter, bool queues,
 			break;
 		}
 	if (adapter->flags & IXGBE_FLAG_FAN_FAIL_CAPABLE)
-		mask |= IXGBE_EIMS_GPI_SDP1;
+		mask |= IXGBE_EIMS_GPI_SDP1(hw);
 	switch (adapter->hw.mac.type) {
 	case ixgbe_mac_82599EB:
-		mask |= IXGBE_EIMS_GPI_SDP1;
-		mask |= IXGBE_EIMS_GPI_SDP2;
+		mask |= IXGBE_EIMS_GPI_SDP1(hw);
+		mask |= IXGBE_EIMS_GPI_SDP2(hw);
 		/* fall through */
 	case ixgbe_mac_X540:
 	case ixgbe_mac_X550:
 	case ixgbe_mac_X550EM_x:
+		if (adapter->hw.phy.type == ixgbe_phy_x550em_ext_t)
+			mask |= IXGBE_EICR_GPI_SDP0_X540;
 		mask |= IXGBE_EIMS_ECC;
 		mask |= IXGBE_EIMS_MAILBOX;
 		break;
@@ -2626,6 +2652,13 @@ static irqreturn_t ixgbe_msix_other(int irq, void *data)
 	case ixgbe_mac_X540:
 	case ixgbe_mac_X550:
 	case ixgbe_mac_X550EM_x:
+		if (hw->phy.type == ixgbe_phy_x550em_ext_t &&
+		    (eicr & IXGBE_EICR_GPI_SDP0_X540)) {
+			adapter->flags2 |= IXGBE_FLAG2_PHY_INTERRUPT;
+			ixgbe_service_event_schedule(adapter);
+			IXGBE_WRITE_REG(hw, IXGBE_EICR,
+					IXGBE_EICR_GPI_SDP0_X540);
+		}
 		if (eicr & IXGBE_EICR_ECC) {
 			e_info(link, "Received ECC Err, initiating reset\n");
 			adapter->flags2 |= IXGBE_FLAG2_RESET_REQUESTED;
@@ -4703,32 +4736,6 @@ static void ixgbe_configure(struct ixgbe_adapter *adapter)
 	ixgbe_configure_dfwd(adapter);
 }
 
-static inline bool ixgbe_is_sfp(struct ixgbe_hw *hw)
-{
-	switch (hw->phy.type) {
-	case ixgbe_phy_sfp_avago:
-	case ixgbe_phy_sfp_ftl:
-	case ixgbe_phy_sfp_intel:
-	case ixgbe_phy_sfp_unknown:
-	case ixgbe_phy_sfp_passive_tyco:
-	case ixgbe_phy_sfp_passive_unknown:
-	case ixgbe_phy_sfp_active_unknown:
-	case ixgbe_phy_sfp_ftl_active:
-	case ixgbe_phy_qsfp_passive_unknown:
-	case ixgbe_phy_qsfp_active_unknown:
-	case ixgbe_phy_qsfp_intel:
-	case ixgbe_phy_qsfp_unknown:
-	/* ixgbe_phy_none is set when no SFP module is present */
-	case ixgbe_phy_none:
-		return true;
-	case ixgbe_phy_nl:
-		if (hw->mac.type == ixgbe_mac_82598EB)
-			return true;
-	default:
-		return false;
-	}
-}
-
 /**
  * ixgbe_sfp_link_config - set up SFP+ link
  * @adapter: pointer to private adapter struct
@@ -4757,7 +4764,7 @@ static int ixgbe_non_sfp_link_config(struct ixgbe_hw *hw)
 {
 	u32 speed;
 	bool autoneg, link_up = false;
-	u32 ret = IXGBE_ERR_LINK_SETUP;
+	int ret = IXGBE_ERR_LINK_SETUP;
 
 	if (hw->mac.ops.check_link)
 		ret = hw->mac.ops.check_link(hw, &speed, &link_up, false);
@@ -4833,7 +4840,7 @@ static void ixgbe_setup_gpie(struct ixgbe_adapter *adapter)
 	if (adapter->flags2 & IXGBE_FLAG2_TEMP_SENSOR_CAPABLE) {
 		switch (adapter->hw.mac.type) {
 		case ixgbe_mac_82599EB:
-			gpie |= IXGBE_SDP0_GPIEN;
+			gpie |= IXGBE_SDP0_GPIEN_8259X;
 			break;
 		case ixgbe_mac_X540:
 			gpie |= IXGBE_EIMS_TS;
@@ -4845,11 +4852,11 @@ static void ixgbe_setup_gpie(struct ixgbe_adapter *adapter)
 
 	/* Enable fan failure interrupt */
 	if (adapter->flags & IXGBE_FLAG_FAN_FAIL_CAPABLE)
-		gpie |= IXGBE_SDP1_GPIEN;
+		gpie |= IXGBE_SDP1_GPIEN(hw);
 
 	if (hw->mac.type == ixgbe_mac_82599EB) {
-		gpie |= IXGBE_SDP1_GPIEN;
-		gpie |= IXGBE_SDP2_GPIEN;
+		gpie |= IXGBE_SDP1_GPIEN_8259X;
+		gpie |= IXGBE_SDP2_GPIEN_8259X;
 	}
 
 	IXGBE_WRITE_REG(hw, IXGBE_GPIE, gpie);
@@ -4873,6 +4880,9 @@ static void ixgbe_up_complete(struct ixgbe_adapter *adapter)
 	if (hw->mac.ops.enable_tx_laser)
 		hw->mac.ops.enable_tx_laser(hw);
 
+	if (hw->phy.ops.set_phy_power)
+		hw->phy.ops.set_phy_power(hw, true);
+
 	smp_mb__before_atomic();
 	clear_bit(__IXGBE_DOWN, &adapter->state);
 	ixgbe_napi_enable_all(adapter);
@@ -4992,6 +5002,13 @@ void ixgbe_reset(struct ixgbe_adapter *adapter)
 
 	if (test_bit(__IXGBE_PTP_RUNNING, &adapter->state))
 		ixgbe_ptp_reset(adapter);
+
+	if (hw->phy.ops.set_phy_power) {
+		if (!netif_running(adapter->netdev) && !adapter->wol)
+			hw->phy.ops.set_phy_power(hw, false);
+		else
+			hw->phy.ops.set_phy_power(hw, true);
+	}
 }
 
 /**
@@ -5260,7 +5277,7 @@ static int ixgbe_sw_init(struct ixgbe_adapter *adapter)
 			adapter->flags2 |= IXGBE_FLAG2_TEMP_SENSOR_CAPABLE;
 		break;
 	case ixgbe_mac_X540:
-		fwsm = IXGBE_READ_REG(hw, IXGBE_FWSM);
+		fwsm = IXGBE_READ_REG(hw, IXGBE_FWSM(hw));
 		if (fwsm & IXGBE_FWSM_TS_ENABLED)
 			adapter->flags2 |= IXGBE_FLAG2_TEMP_SENSOR_CAPABLE;
 		break;
@@ -5672,6 +5689,7 @@ static int ixgbe_change_mtu(struct net_device *netdev, int new_mtu)
 static int ixgbe_open(struct net_device *netdev)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+	struct ixgbe_hw *hw = &adapter->hw;
 	int err, queues;
 
 	/* disallow open during test */
@@ -5729,6 +5747,8 @@ err_set_queues:
 	ixgbe_free_irq(adapter);
 err_req_irq:
 	ixgbe_free_all_rx_resources(adapter);
+	if (hw->phy.ops.set_phy_power && !adapter->wol)
+		hw->phy.ops.set_phy_power(&adapter->hw, false);
 err_setup_rx:
 	ixgbe_free_all_tx_resources(adapter);
 err_setup_tx:
@@ -5889,6 +5909,8 @@ static int __ixgbe_shutdown(struct pci_dev *pdev, bool *enable_wake)
 	}
 
 	*enable_wake = !!wufc;
+	if (hw->phy.ops.set_phy_power && !*enable_wake)
+		hw->phy.ops.set_phy_power(hw, false);
 
 	ixgbe_release_hw_control(adapter);
 
@@ -6718,6 +6740,26 @@ static void ixgbe_service_timer(unsigned long data)
 	ixgbe_service_event_schedule(adapter);
 }
 
+static void ixgbe_phy_interrupt_subtask(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 status;
+
+	if (!(adapter->flags2 & IXGBE_FLAG2_PHY_INTERRUPT))
+		return;
+
+	adapter->flags2 &= ~IXGBE_FLAG2_PHY_INTERRUPT;
+
+	if (!hw->phy.ops.handle_lasi)
+		return;
+
+	status = hw->phy.ops.handle_lasi(&adapter->hw);
+	if (status != IXGBE_ERR_OVERTEMP)
+		return;
+
+	e_crit(drv, "%s\n", ixgbe_overheat_msg);
+}
+
 static void ixgbe_reset_subtask(struct ixgbe_adapter *adapter)
 {
 	if (!(adapter->flags2 & IXGBE_FLAG2_RESET_REQUESTED))
@@ -6759,6 +6801,7 @@ static void ixgbe_service_task(struct work_struct *work)
 		return;
 	}
 	ixgbe_reset_subtask(adapter);
+	ixgbe_phy_interrupt_subtask(adapter);
 	ixgbe_sfp_detection_subtask(adapter);
 	ixgbe_sfp_link_config_subtask(adapter);
 	ixgbe_check_overtemp_subtask(adapter);
@@ -8022,7 +8065,7 @@ static int ixgbe_ndo_bridge_setlink(struct net_device *dev,
 		return -EINVAL;
 
 	nla_for_each_nested(attr, br_spec, rem) {
-		u32 status;
+		int status;
 		__u16 mode;
 
 		if (nla_type(attr) != IFLA_BRIDGE_MODE)
@@ -8052,7 +8095,8 @@ static int ixgbe_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 		return 0;
 
 	return ndo_dflt_bridge_getlink(skb, pid, seq, dev,
-				       adapter->bridge_mode, 0, 0, nlflags);
+				       adapter->bridge_mode, 0, 0, nlflags,
+				       filter_mask, NULL);
 }
 
 static void *ixgbe_fwd_add(struct net_device *pdev, struct net_device *vdev)
@@ -8291,6 +8335,10 @@ int ixgbe_wol_supported(struct ixgbe_adapter *adapter, u16 device_id,
 		break;
 	case IXGBE_DEV_ID_X540T:
 	case IXGBE_DEV_ID_X540T1:
+	case IXGBE_DEV_ID_X550T:
+	case IXGBE_DEV_ID_X550EM_X_KX4:
+	case IXGBE_DEV_ID_X550EM_X_KR:
+	case IXGBE_DEV_ID_X550EM_X_10G_T:
 		/* check eeprom to see if enabled wol */
 		if ((wol_cap == IXGBE_DEVICE_CAPS_WOL_PORT0_1) ||
 		    ((wol_cap == IXGBE_DEVICE_CAPS_WOL_PORT0) &&
@@ -8431,10 +8479,11 @@ static int ixgbe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* Setup hw api */
 	memcpy(&hw->mac.ops, ii->mac_ops, sizeof(hw->mac.ops));
 	hw->mac.type  = ii->mac;
+	hw->mvals     = ii->mvals;
 
 	/* EEPROM */
 	memcpy(&hw->eeprom.ops, ii->eeprom_ops, sizeof(hw->eeprom.ops));
-	eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+	eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 	if (ixgbe_removed(hw->hw_addr)) {
 		err = -EIO;
 		goto err_ioremap;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
index 8a2be444113d..526a20bf7488 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
@@ -317,14 +317,14 @@ bool ixgbe_check_reset_blocked(struct ixgbe_hw *hw)
  **/
 static s32 ixgbe_get_phy_id(struct ixgbe_hw *hw)
 {
-	u32 status;
+	s32 status;
 	u16 phy_id_high = 0;
 	u16 phy_id_low = 0;
 
 	status = hw->phy.ops.read_reg(hw, MDIO_DEVID1, MDIO_MMD_PMAPMD,
 				      &phy_id_high);
 
-	if (status == 0) {
+	if (!status) {
 		hw->phy.id = (u32)(phy_id_high << 16);
 		status = hw->phy.ops.read_reg(hw, MDIO_DEVID2, MDIO_MMD_PMAPMD,
 					      &phy_id_low);
@@ -347,6 +347,7 @@ static enum ixgbe_phy_type ixgbe_get_phy_type_from_id(u32 phy_id)
 	case TN1010_PHY_ID:
 		phy_type = ixgbe_phy_tn;
 		break;
+	case X550_PHY_ID:
 	case X540_PHY_ID:
 		phy_type = ixgbe_phy_aq;
 		break;
@@ -356,6 +357,9 @@ static enum ixgbe_phy_type ixgbe_get_phy_type_from_id(u32 phy_id)
 	case ATH_PHY_ID:
 		phy_type = ixgbe_phy_nl;
 		break;
+	case X557_PHY_ID:
+		phy_type = ixgbe_phy_x550em_ext_t;
+		break;
 	default:
 		phy_type = ixgbe_phy_unknown;
 		break;
@@ -1348,6 +1352,9 @@ static s32 ixgbe_identify_qsfp_module_generic(struct ixgbe_hw *hw)
 		return IXGBE_ERR_SFP_NOT_PRESENT;
 	}
 
+	/* LAN ID is needed for sfp_type determination */
+	hw->mac.ops.set_lan_id(hw);
+
 	status = hw->phy.ops.read_i2c_eeprom(hw, IXGBE_SFF_IDENTIFIER,
 					     &identifier);
 
@@ -1361,9 +1368,6 @@ static s32 ixgbe_identify_qsfp_module_generic(struct ixgbe_hw *hw)
 
 	hw->phy.id = identifier;
 
-	/* LAN ID is needed for sfp_type determination */
-	hw->mac.ops.set_lan_id(hw);
-
 	status = hw->phy.ops.read_i2c_eeprom(hw, IXGBE_SFF_QSFP_10GBE_COMP,
 					     &comp_codes_10g);
 
@@ -1793,7 +1797,7 @@ fail:
  **/
 static void ixgbe_i2c_start(struct ixgbe_hw *hw)
 {
-	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 
 	/* Start condition must begin with data and clock high */
 	ixgbe_set_i2c_data(hw, &i2cctl, 1);
@@ -1822,7 +1826,7 @@ static void ixgbe_i2c_start(struct ixgbe_hw *hw)
  **/
 static void ixgbe_i2c_stop(struct ixgbe_hw *hw)
 {
-	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 
 	/* Stop condition must begin with data low and clock high */
 	ixgbe_set_i2c_data(hw, &i2cctl, 0);
@@ -1880,9 +1884,9 @@ static s32 ixgbe_clock_out_i2c_byte(struct ixgbe_hw *hw, u8 data)
 	}
 
 	/* Release SDA line (set high) */
-	i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
-	i2cctl |= IXGBE_I2C_DATA_OUT_BY_MAC(hw);
-	IXGBE_WRITE_REG(hw, IXGBE_I2CCTL_BY_MAC(hw), i2cctl);
+	i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
+	i2cctl |= IXGBE_I2C_DATA_OUT(hw);
+	IXGBE_WRITE_REG(hw, IXGBE_I2CCTL(hw), i2cctl);
 	IXGBE_WRITE_FLUSH(hw);
 
 	return status;
@@ -1898,7 +1902,7 @@ static s32 ixgbe_get_i2c_ack(struct ixgbe_hw *hw)
 {
 	s32 status = 0;
 	u32 i = 0;
-	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 	u32 timeout = 10;
 	bool ack = true;
 
@@ -1911,7 +1915,7 @@ static s32 ixgbe_get_i2c_ack(struct ixgbe_hw *hw)
 	/* Poll for ACK.  Note that ACK in I2C spec is
 	 * transition from 1 to 0 */
 	for (i = 0; i < timeout; i++) {
-		i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+		i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 		ack = ixgbe_get_i2c_data(hw, &i2cctl);
 
 		udelay(1);
@@ -1941,14 +1945,14 @@ static s32 ixgbe_get_i2c_ack(struct ixgbe_hw *hw)
  **/
 static s32 ixgbe_clock_in_i2c_bit(struct ixgbe_hw *hw, bool *data)
 {
-	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 
 	ixgbe_raise_i2c_clk(hw, &i2cctl);
 
 	/* Minimum high period of clock is 4us */
 	udelay(IXGBE_I2C_T_HIGH);
 
-	i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+	i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 	*data = ixgbe_get_i2c_data(hw, &i2cctl);
 
 	ixgbe_lower_i2c_clk(hw, &i2cctl);
@@ -1969,7 +1973,7 @@ static s32 ixgbe_clock_in_i2c_bit(struct ixgbe_hw *hw, bool *data)
 static s32 ixgbe_clock_out_i2c_bit(struct ixgbe_hw *hw, bool data)
 {
 	s32 status;
-	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 
 	status = ixgbe_set_i2c_data(hw, &i2cctl, data);
 	if (status == 0) {
@@ -2005,14 +2009,14 @@ static void ixgbe_raise_i2c_clk(struct ixgbe_hw *hw, u32 *i2cctl)
 	u32 i2cctl_r = 0;
 
 	for (i = 0; i < timeout; i++) {
-		*i2cctl |= IXGBE_I2C_CLK_OUT_BY_MAC(hw);
-		IXGBE_WRITE_REG(hw, IXGBE_I2CCTL_BY_MAC(hw), *i2cctl);
+		*i2cctl |= IXGBE_I2C_CLK_OUT(hw);
+		IXGBE_WRITE_REG(hw, IXGBE_I2CCTL(hw), *i2cctl);
 		IXGBE_WRITE_FLUSH(hw);
 		/* SCL rise time (1000ns) */
 		udelay(IXGBE_I2C_T_RISE);
 
-		i2cctl_r = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
-		if (i2cctl_r & IXGBE_I2C_CLK_IN_BY_MAC(hw))
+		i2cctl_r = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
+		if (i2cctl_r & IXGBE_I2C_CLK_IN(hw))
 			break;
 	}
 }
@@ -2027,9 +2031,9 @@ static void ixgbe_raise_i2c_clk(struct ixgbe_hw *hw, u32 *i2cctl)
 static void ixgbe_lower_i2c_clk(struct ixgbe_hw *hw, u32 *i2cctl)
 {
 
-	*i2cctl &= ~IXGBE_I2C_CLK_OUT_BY_MAC(hw);
+	*i2cctl &= ~IXGBE_I2C_CLK_OUT(hw);
 
-	IXGBE_WRITE_REG(hw, IXGBE_I2CCTL_BY_MAC(hw), *i2cctl);
+	IXGBE_WRITE_REG(hw, IXGBE_I2CCTL(hw), *i2cctl);
 	IXGBE_WRITE_FLUSH(hw);
 
 	/* SCL fall time (300ns) */
@@ -2047,18 +2051,18 @@ static void ixgbe_lower_i2c_clk(struct ixgbe_hw *hw, u32 *i2cctl)
 static s32 ixgbe_set_i2c_data(struct ixgbe_hw *hw, u32 *i2cctl, bool data)
 {
 	if (data)
-		*i2cctl |= IXGBE_I2C_DATA_OUT_BY_MAC(hw);
+		*i2cctl |= IXGBE_I2C_DATA_OUT(hw);
 	else
-		*i2cctl &= ~IXGBE_I2C_DATA_OUT_BY_MAC(hw);
+		*i2cctl &= ~IXGBE_I2C_DATA_OUT(hw);
 
-	IXGBE_WRITE_REG(hw, IXGBE_I2CCTL_BY_MAC(hw), *i2cctl);
+	IXGBE_WRITE_REG(hw, IXGBE_I2CCTL(hw), *i2cctl);
 	IXGBE_WRITE_FLUSH(hw);
 
 	/* Data rise/fall (1000ns/300ns) and set-up time (250ns) */
 	udelay(IXGBE_I2C_T_RISE + IXGBE_I2C_T_FALL + IXGBE_I2C_T_SU_DATA);
 
 	/* Verify data was set correctly */
-	*i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+	*i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 	if (data != ixgbe_get_i2c_data(hw, i2cctl)) {
 		hw_dbg(hw, "Error - I2C data was not set to %X.\n", data);
 		return IXGBE_ERR_I2C;
@@ -2076,7 +2080,7 @@ static s32 ixgbe_set_i2c_data(struct ixgbe_hw *hw, u32 *i2cctl, bool data)
  **/
 static bool ixgbe_get_i2c_data(struct ixgbe_hw *hw, u32 *i2cctl)
 {
-	if (*i2cctl & IXGBE_I2C_DATA_IN_BY_MAC(hw))
+	if (*i2cctl & IXGBE_I2C_DATA_IN(hw))
 		return true;
 	return false;
 }
@@ -2090,7 +2094,7 @@ static bool ixgbe_get_i2c_data(struct ixgbe_hw *hw, u32 *i2cctl)
  **/
 static void ixgbe_i2c_bus_clear(struct ixgbe_hw *hw)
 {
-	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL_BY_MAC(hw));
+	u32 i2cctl = IXGBE_READ_REG(hw, IXGBE_I2CCTL(hw));
 	u32 i;
 
 	ixgbe_i2c_start(hw);
@@ -2137,3 +2141,36 @@ s32 ixgbe_tn_check_overtemp(struct ixgbe_hw *hw)
 
 	return IXGBE_ERR_OVERTEMP;
 }
+
+/** ixgbe_set_copper_phy_power - Control power for copper phy
+ *  @hw: pointer to hardware structure
+ *  @on: true for on, false for off
+ **/
+s32 ixgbe_set_copper_phy_power(struct ixgbe_hw *hw, bool on)
+{
+	u32 status;
+	u16 reg;
+
+	/* Bail if we don't have copper phy */
+	if (hw->mac.ops.get_media_type(hw) != ixgbe_media_type_copper)
+		return 0;
+
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_VENDOR_SPECIFIC_1_CONTROL,
+				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				      &reg);
+	if (status)
+		return status;
+
+	if (on) {
+		reg &= ~IXGBE_MDIO_PHY_SET_LOW_POWER_MODE;
+	} else {
+		if (ixgbe_check_reset_blocked(hw))
+			return 0;
+		reg |= IXGBE_MDIO_PHY_SET_LOW_POWER_MODE;
+	}
+
+	status = hw->phy.ops.write_reg(hw, IXGBE_MDIO_VENDOR_SPECIFIC_1_CONTROL,
+				       IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				       reg);
+	return status;
+}
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.h
index 434643881287..e45988c4dad5 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.h
@@ -145,6 +145,7 @@ s32 ixgbe_get_phy_firmware_version_generic(struct ixgbe_hw *hw,
 					   u16 *firmware_version);
 
 s32 ixgbe_reset_phy_nl(struct ixgbe_hw *hw);
+s32 ixgbe_set_copper_phy_power(struct ixgbe_hw *hw, bool on);
 s32 ixgbe_identify_module_generic(struct ixgbe_hw *hw);
 s32 ixgbe_identify_sfp_module_generic(struct ixgbe_hw *hw);
 s32 ixgbe_get_sfp_init_sequence_offsets(struct ixgbe_hw *hw,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
index dd6ba5916dfe..b6f424f3b1a8 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
@@ -91,14 +91,24 @@
 #define IXGBE_DEV_ID_X550_VF		0x1565
 #define IXGBE_DEV_ID_X550EM_X_VF	0x15A8
 
+#define IXGBE_CAT(r, m)	IXGBE_##r##_##m
+
+#define IXGBE_BY_MAC(_hw, r)	((_hw)->mvals[IXGBE_CAT(r, IDX)])
+
 /* General Registers */
 #define IXGBE_CTRL      0x00000
 #define IXGBE_STATUS    0x00008
 #define IXGBE_CTRL_EXT  0x00018
 #define IXGBE_ESDP      0x00020
 #define IXGBE_EODSDP    0x00028
-#define IXGBE_I2CCTL_BY_MAC(_hw)((((_hw)->mac.type >= ixgbe_mac_X550) ? \
-					0x15F5C : 0x00028))
+
+#define IXGBE_I2CCTL_8259X	0x00028
+#define IXGBE_I2CCTL_X540	IXGBE_I2CCTL_8259X
+#define IXGBE_I2CCTL_X550	0x15F5C
+#define IXGBE_I2CCTL_X550EM_x	IXGBE_I2CCTL_X550
+#define IXGBE_I2CCTL_X550EM_a	IXGBE_I2CCTL_X550
+#define IXGBE_I2CCTL(_hw)	IXGBE_BY_MAC((_hw), I2CCTL)
+
 #define IXGBE_LEDCTL    0x00200
 #define IXGBE_FRTIMER   0x00048
 #define IXGBE_TCPTIMER  0x0004C
@@ -106,17 +116,39 @@
 #define IXGBE_EXVET     0x05078
 
 /* NVM Registers */
-#define IXGBE_EEC       0x10010
+#define IXGBE_EEC_8259X		0x10010
+#define IXGBE_EEC_X540		IXGBE_EEC_8259X
+#define IXGBE_EEC_X550		IXGBE_EEC_8259X
+#define IXGBE_EEC_X550EM_x	IXGBE_EEC_8259X
+#define IXGBE_EEC_X550EM_a	0x15FF8
+#define IXGBE_EEC(_hw)		IXGBE_BY_MAC((_hw), EEC)
 #define IXGBE_EERD      0x10014
 #define IXGBE_EEWR      0x10018
-#define IXGBE_FLA       0x1001C
+#define IXGBE_FLA_8259X		0x1001C
+#define IXGBE_FLA_X540		IXGBE_FLA_8259X
+#define IXGBE_FLA_X550		IXGBE_FLA_8259X
+#define IXGBE_FLA_X550EM_x	IXGBE_FLA_8259X
+#define IXGBE_FLA_X550EM_a	0x15F6C
+#define IXGBE_FLA(_hw)		IXGBE_BY_MAC((_hw), FLA)
 #define IXGBE_EEMNGCTL  0x10110
 #define IXGBE_EEMNGDATA 0x10114
 #define IXGBE_FLMNGCTL  0x10118
 #define IXGBE_FLMNGDATA 0x1011C
 #define IXGBE_FLMNGCNT  0x10120
 #define IXGBE_FLOP      0x1013C
-#define IXGBE_GRC       0x10200
+#define IXGBE_GRC_8259X		0x10200
+#define IXGBE_GRC_X540		IXGBE_GRC_8259X
+#define IXGBE_GRC_X550		IXGBE_GRC_8259X
+#define IXGBE_GRC_X550EM_x	IXGBE_GRC_8259X
+#define IXGBE_GRC_X550EM_a	0x15F64
+#define IXGBE_GRC(_hw)		IXGBE_BY_MAC((_hw), GRC)
+
+#define IXGBE_SRAMREL_8259X	0x10210
+#define IXGBE_SRAMREL_X540	IXGBE_SRAMREL_8259X
+#define IXGBE_SRAMREL_X550	IXGBE_SRAMREL_8259X
+#define IXGBE_SRAMREL_X550EM_x	IXGBE_SRAMREL_8259X
+#define IXGBE_SRAMREL_X550EM_a	0x15F6C
+#define IXGBE_SRAMREL(_hw)	IXGBE_BY_MAC((_hw), SRAMREL)
 
 /* General Receive Control */
 #define IXGBE_GRC_MNG  0x00000001 /* Manageability Enable */
@@ -126,14 +158,55 @@
 #define IXGBE_VPDDIAG1  0x10208
 
 /* I2CCTL Bit Masks */
-#define IXGBE_I2C_CLK_IN_BY_MAC(_hw)(((_hw)->mac.type) >= ixgbe_mac_X550 ? \
-					0x00004000 : 0x00000001)
-#define IXGBE_I2C_CLK_OUT_BY_MAC(_hw)(((_hw)->mac.type) >= ixgbe_mac_X550 ? \
-					0x00000200 : 0x00000002)
-#define IXGBE_I2C_DATA_IN_BY_MAC(_hw)(((_hw)->mac.type) >= ixgbe_mac_X550 ? \
-					0x00001000 : 0x00000004)
-#define IXGBE_I2C_DATA_OUT_BY_MAC(_hw)(((_hw)->mac.type) >= ixgbe_mac_X550 ? \
-					0x00000400 : 0x00000008)
+#define IXGBE_I2C_CLK_IN_8259X		0x00000001
+#define IXGBE_I2C_CLK_IN_X540		IXGBE_I2C_CLK_IN_8259X
+#define IXGBE_I2C_CLK_IN_X550		0x00004000
+#define IXGBE_I2C_CLK_IN_X550EM_x	IXGBE_I2C_CLK_IN_X550
+#define IXGBE_I2C_CLK_IN_X550EM_a	IXGBE_I2C_CLK_IN_X550
+#define IXGBE_I2C_CLK_IN(_hw)		IXGBE_BY_MAC((_hw), I2C_CLK_IN)
+
+#define IXGBE_I2C_CLK_OUT_8259X		0x00000002
+#define IXGBE_I2C_CLK_OUT_X540		IXGBE_I2C_CLK_OUT_8259X
+#define IXGBE_I2C_CLK_OUT_X550		0x00000200
+#define IXGBE_I2C_CLK_OUT_X550EM_x	IXGBE_I2C_CLK_OUT_X550
+#define IXGBE_I2C_CLK_OUT_X550EM_a	IXGBE_I2C_CLK_OUT_X550
+#define IXGBE_I2C_CLK_OUT(_hw)		IXGBE_BY_MAC((_hw), I2C_CLK_OUT)
+
+#define IXGBE_I2C_DATA_IN_8259X		0x00000004
+#define IXGBE_I2C_DATA_IN_X540		IXGBE_I2C_DATA_IN_8259X
+#define IXGBE_I2C_DATA_IN_X550		0x00001000
+#define IXGBE_I2C_DATA_IN_X550EM_x	IXGBE_I2C_DATA_IN_X550
+#define IXGBE_I2C_DATA_IN_X550EM_a	IXGBE_I2C_DATA_IN_X550
+#define IXGBE_I2C_DATA_IN(_hw)		IXGBE_BY_MAC((_hw), I2C_DATA_IN)
+
+#define IXGBE_I2C_DATA_OUT_8259X	0x00000008
+#define IXGBE_I2C_DATA_OUT_X540		IXGBE_I2C_DATA_OUT_8259X
+#define IXGBE_I2C_DATA_OUT_X550		0x00000400
+#define IXGBE_I2C_DATA_OUT_X550EM_x	IXGBE_I2C_DATA_OUT_X550
+#define IXGBE_I2C_DATA_OUT_X550EM_a	IXGBE_I2C_DATA_OUT_X550
+#define IXGBE_I2C_DATA_OUT(_hw)		IXGBE_BY_MAC((_hw), I2C_DATA_OUT)
+
+#define IXGBE_I2C_DATA_OE_N_EN_8259X	0
+#define IXGBE_I2C_DATA_OE_N_EN_X540	IXGBE_I2C_DATA_OE_N_EN_8259X
+#define IXGBE_I2C_DATA_OE_N_EN_X550	0x00000800
+#define IXGBE_I2C_DATA_OE_N_EN_X550EM_x	IXGBE_I2C_DATA_OE_N_EN_X550
+#define IXGBE_I2C_DATA_OE_N_EN_X550EM_a	IXGBE_I2C_DATA_OE_N_EN_X550
+#define IXGBE_I2C_DATA_OE_N_EN(_hw)	IXGBE_BY_MAC((_hw), I2C_DATA_OE_N_EN)
+
+#define IXGBE_I2C_BB_EN_8259X		0
+#define IXGBE_I2C_BB_EN_X540		IXGBE_I2C_BB_EN_8259X
+#define IXGBE_I2C_BB_EN_X550		0x00000100
+#define IXGBE_I2C_BB_EN_X550EM_x	IXGBE_I2C_BB_EN_X550
+#define IXGBE_I2C_BB_EN_X550EM_a	IXGBE_I2C_BB_EN_X550
+#define IXGBE_I2C_BB_EN(_hw)		IXGBE_BY_MAC((_hw), I2C_BB_EN)
+
+#define IXGBE_I2C_CLK_OE_N_EN_8259X	0
+#define IXGBE_I2C_CLK_OE_N_EN_X540	IXGBE_I2C_CLK_OE_N_EN_8259X
+#define IXGBE_I2C_CLK_OE_N_EN_X550	0x00002000
+#define IXGBE_I2C_CLK_OE_N_EN_X550EM_x	IXGBE_I2C_CLK_OE_N_EN_X550
+#define IXGBE_I2C_CLK_OE_N_EN_X550EM_a	IXGBE_I2C_CLK_OE_N_EN_X550
+#define IXGBE_I2C_CLK_OE_N_EN(_hw)	 IXGBE_BY_MAC((_hw), I2C_CLK_OE_N_EN)
+
 #define IXGBE_I2C_CLOCK_STRETCHING_TIMEOUT	500
 
 #define IXGBE_I2C_THERMAL_SENSOR_ADDR	0xF8
@@ -835,15 +908,36 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_GSCN_1    0x11024
 #define IXGBE_GSCN_2    0x11028
 #define IXGBE_GSCN_3    0x1102C
-#define IXGBE_FACTPS    0x10150
+#define IXGBE_FACTPS_8259X	0x10150
+#define IXGBE_FACTPS_X540	IXGBE_FACTPS_8259X
+#define IXGBE_FACTPS_X550	IXGBE_FACTPS_8259X
+#define IXGBE_FACTPS_X550EM_x	IXGBE_FACTPS_8259X
+#define IXGBE_FACTPS_X550EM_a	0x15FEC
+#define IXGBE_FACTPS(_hw)	IXGBE_BY_MAC((_hw), FACTPS)
+
 #define IXGBE_PCIEANACTL  0x11040
-#define IXGBE_SWSM      0x10140
-#define IXGBE_FWSM      0x10148
+#define IXGBE_SWSM_8259X	0x10140
+#define IXGBE_SWSM_X540		IXGBE_SWSM_8259X
+#define IXGBE_SWSM_X550		IXGBE_SWSM_8259X
+#define IXGBE_SWSM_X550EM_x	IXGBE_SWSM_8259X
+#define IXGBE_SWSM_X550EM_a	0x15F70
+#define IXGBE_SWSM(_hw)		IXGBE_BY_MAC((_hw), SWSM)
+#define IXGBE_FWSM_8259X	0x10148
+#define IXGBE_FWSM_X540		IXGBE_FWSM_8259X
+#define IXGBE_FWSM_X550		IXGBE_FWSM_8259X
+#define IXGBE_FWSM_X550EM_x	IXGBE_FWSM_8259X
+#define IXGBE_FWSM_X550EM_a	0x15F74
+#define IXGBE_FWSM(_hw)		IXGBE_BY_MAC((_hw), FWSM)
 #define IXGBE_GSSR      0x10160
 #define IXGBE_MREVID    0x11064
 #define IXGBE_DCA_ID    0x11070
 #define IXGBE_DCA_CTRL  0x11074
-#define IXGBE_SWFW_SYNC IXGBE_GSSR
+#define IXGBE_SWFW_SYNC_8259X		IXGBE_GSSR
+#define IXGBE_SWFW_SYNC_X540		IXGBE_SWFW_SYNC_8259X
+#define IXGBE_SWFW_SYNC_X550		IXGBE_SWFW_SYNC_8259X
+#define IXGBE_SWFW_SYNC_X550EM_x	IXGBE_SWFW_SYNC_8259X
+#define IXGBE_SWFW_SYNC_X550EM_a	0x15F78
+#define IXGBE_SWFW_SYNC(_hw)		IXGBE_BY_MAC((_hw), SWFW_SYNC)
 
 /* PCIe registers 82599-specific */
 #define IXGBE_GCR_EXT           0x11050
@@ -855,14 +949,21 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_PHYDAT_82599      0x11044
 #define IXGBE_PHYCTL_82599      0x11048
 #define IXGBE_PBACLR_82599      0x11068
-#define IXGBE_CIAA_82599        0x11088
-#define IXGBE_CIAD_82599        0x1108C
-#define IXGBE_CIAA_X550         0x11508
-#define IXGBE_CIAD_X550         0x11510
-#define IXGBE_CIAA_BY_MAC(_hw)  ((((_hw)->mac.type >= ixgbe_mac_X550) ? \
-				IXGBE_CIAA_X550 : IXGBE_CIAA_82599))
-#define IXGBE_CIAD_BY_MAC(_hw)  ((((_hw)->mac.type >= ixgbe_mac_X550) ? \
-				IXGBE_CIAD_X550 : IXGBE_CIAD_82599))
+
+#define IXGBE_CIAA_8259X	0x11088
+#define IXGBE_CIAA_X540		IXGBE_CIAA_8259X
+#define IXGBE_CIAA_X550		0x11508
+#define IXGBE_CIAA_X550EM_x	IXGBE_CIAA_X550
+#define IXGBE_CIAA_X550EM_a	IXGBE_CIAA_X550
+#define IXGBE_CIAA(_hw)		IXGBE_BY_MAC((_hw), CIAA)
+
+#define IXGBE_CIAD_8259X	0x1108C
+#define IXGBE_CIAD_X540		IXGBE_CIAD_8259X
+#define IXGBE_CIAD_X550		0x11510
+#define IXGBE_CIAD_X550EM_x	IXGBE_CIAD_X550
+#define IXGBE_CIAD_X550EM_a	IXGBE_CIAD_X550
+#define IXGBE_CIAD(_hw)		IXGBE_BY_MAC((_hw), CIAD)
+
 #define IXGBE_PICAUSE           0x110B0
 #define IXGBE_PIENA             0x110B8
 #define IXGBE_CDQ_MBR_82599     0x110B4
@@ -1204,18 +1305,37 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_MDIO_AUTO_NEG_CONTROL	0x0 /* AUTO_NEG Control Reg */
 #define IXGBE_MDIO_AUTO_NEG_STATUS	0x1 /* AUTO_NEG Status Reg */
 #define IXGBE_MDIO_AUTO_NEG_VENDOR_STAT	0xC800 /* AUTO_NEG Vendor Status Reg */
+#define IXGBE_MDIO_AUTO_NEG_VENDOR_TX_ALARM2 0xCC01 /* AUTO_NEG Vendor Tx Reg */
+#define IXGBE_MDIO_AUTO_NEG_VEN_LSC	0x1 /* AUTO_NEG Vendor Tx LSC */
 #define IXGBE_MDIO_AUTO_NEG_ADVT	0x10 /* AUTO_NEG Advt Reg */
 #define IXGBE_MDIO_AUTO_NEG_LP		0x13 /* AUTO_NEG LP Status Reg */
 #define IXGBE_MDIO_AUTO_NEG_EEE_ADVT	0x3C /* AUTO_NEG EEE Advt Reg */
 
+#define IXGBE_MDIO_PHY_SET_LOW_POWER_MODE	 0x0800 /* Set low power mode */
+
 #define IXGBE_MDIO_TX_VENDOR_ALARMS_3	0xCC02 /* Vendor Alarms 3 Reg */
 #define IXGBE_MDIO_TX_VENDOR_ALARMS_3_RST_MASK 0x3 /* PHY Reset Complete Mask */
 #define IXGBE_MDIO_GLOBAL_RES_PR_10 0xC479 /* Global Resv Provisioning 10 Reg */
 #define IXGBE_MDIO_POWER_UP_STALL	0x8000 /* Power Up Stall */
+#define IXGBE_MDIO_GLOBAL_INT_CHIP_STD_MASK	0xFF00 /* int std mask */
+#define IXGBE_MDIO_GLOBAL_CHIP_STD_INT_FLAG	0xFC00 /* chip std int flag */
+#define IXGBE_MDIO_GLOBAL_INT_CHIP_VEN_MASK	0xFF01 /* int chip-wide mask */
+#define IXGBE_MDIO_GLOBAL_INT_CHIP_VEN_FLAG	0xFC01 /* int chip-wide mask */
+#define IXGBE_MDIO_GLOBAL_ALARM_1		0xCC00 /* Global alarm 1 */
+#define IXGBE_MDIO_GLOBAL_ALM_1_HI_TMP_FAIL	0x4000 /* high temp failure */
+#define IXGBE_MDIO_GLOBAL_INT_MASK		0xD400 /* Global int mask */
+/* autoneg vendor alarm int enable */
+#define IXGBE_MDIO_GLOBAL_AN_VEN_ALM_INT_EN	0x1000
+#define IXGBE_MDIO_GLOBAL_ALARM_1_INT		0x4 /* int in Global alarm 1 */
+#define IXGBE_MDIO_GLOBAL_VEN_ALM_INT_EN	0x1 /* vendor alarm int enable */
+#define IXGBE_MDIO_GLOBAL_STD_ALM2_INT		0x200 /* vendor alarm2 int mask */
+#define IXGBE_MDIO_GLOBAL_INT_HI_TEMP_EN	0x4000 /* int high temp enable */
 
 #define IXGBE_MDIO_PMA_PMD_SDA_SCL_ADDR	0xC30A /* PHY_XS SDA/SCL Addr Reg */
 #define IXGBE_MDIO_PMA_PMD_SDA_SCL_DATA	0xC30B /* PHY_XS SDA/SCL Data Reg */
 #define IXGBE_MDIO_PMA_PMD_SDA_SCL_STAT	0xC30C /* PHY_XS SDA/SCL Stat Reg */
+#define IXGBE_MDIO_PMA_TX_VEN_LASI_INT_MASK	0xD401 /* PHY TX Vendor LASI */
+#define IXGBE_MDIO_PMA_TX_VEN_LASI_INT_EN	0x1 /* PHY TX Vendor LASI enable */
 #define IXGBE_MDIO_PMD_STD_TX_DISABLE_CNTR	0x9 /* Standard Tx Dis Reg */
 #define IXGBE_MDIO_PMD_GLOBAL_TX_DISABLE	0x0001 /* PMD Global Tx Dis */
 
@@ -1233,6 +1353,8 @@ struct ixgbe_thermal_sensor_data {
 #define TN1010_PHY_ID    0x00A19410
 #define TNX_FW_REV       0xB
 #define X540_PHY_ID      0x01540200
+#define X550_PHY_ID      0x01540220
+#define X557_PHY_ID      0x01540240
 #define QT2022_PHY_ID    0x0043A400
 #define ATH_PHY_ID       0x03429050
 #define AQ_FW_REV        0x20
@@ -1253,9 +1375,25 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_CONTROL_SOL_NL     0x0000
 
 /* General purpose Interrupt Enable */
-#define IXGBE_SDP0_GPIEN         0x00000001 /* SDP0 */
-#define IXGBE_SDP1_GPIEN         0x00000002 /* SDP1 */
-#define IXGBE_SDP2_GPIEN         0x00000004 /* SDP2 */
+#define IXGBE_SDP0_GPIEN_8259X		0x00000001 /* SDP0 */
+#define IXGBE_SDP1_GPIEN_8259X		0x00000002 /* SDP1 */
+#define IXGBE_SDP2_GPIEN_8259X		0x00000004 /* SDP2 */
+#define IXGBE_SDP0_GPIEN_X540		0x00000002 /* SDP0 on X540 and X550 */
+#define IXGBE_SDP1_GPIEN_X540		0x00000004 /* SDP1 on X540 and X550 */
+#define IXGBE_SDP2_GPIEN_X540		0x00000008 /* SDP2 on X540 and X550 */
+#define IXGBE_SDP0_GPIEN_X550		IXGBE_SDP0_GPIEN_X540
+#define IXGBE_SDP1_GPIEN_X550		IXGBE_SDP1_GPIEN_X540
+#define IXGBE_SDP2_GPIEN_X550		IXGBE_SDP2_GPIEN_X540
+#define IXGBE_SDP0_GPIEN_X550EM_x	IXGBE_SDP0_GPIEN_X540
+#define IXGBE_SDP1_GPIEN_X550EM_x	IXGBE_SDP1_GPIEN_X540
+#define IXGBE_SDP2_GPIEN_X550EM_x	IXGBE_SDP2_GPIEN_X540
+#define IXGBE_SDP0_GPIEN_X550EM_a	IXGBE_SDP0_GPIEN_X540
+#define IXGBE_SDP1_GPIEN_X550EM_a	IXGBE_SDP1_GPIEN_X540
+#define IXGBE_SDP2_GPIEN_X550EM_a	IXGBE_SDP2_GPIEN_X540
+#define IXGBE_SDP0_GPIEN(_hw)		IXGBE_BY_MAC((_hw), SDP0_GPIEN)
+#define IXGBE_SDP1_GPIEN(_hw)		IXGBE_BY_MAC((_hw), SDP1_GPIEN)
+#define IXGBE_SDP2_GPIEN(_hw)		IXGBE_BY_MAC((_hw), SDP2_GPIEN)
+
 #define IXGBE_GPIE_MSIX_MODE     0x00000010 /* MSI-X mode */
 #define IXGBE_GPIE_OCD           0x00000020 /* Other Clear Disable */
 #define IXGBE_GPIE_EIMEN         0x00000040 /* Immediate Interrupt Enable */
@@ -1417,9 +1555,25 @@ enum {
 #define IXGBE_EICR_MNG          0x00400000 /* Manageability Event Interrupt */
 #define IXGBE_EICR_TS           0x00800000 /* Thermal Sensor Event */
 #define IXGBE_EICR_TIMESYNC     0x01000000 /* Timesync Event */
-#define IXGBE_EICR_GPI_SDP0     0x01000000 /* Gen Purpose Interrupt on SDP0 */
-#define IXGBE_EICR_GPI_SDP1     0x02000000 /* Gen Purpose Interrupt on SDP1 */
-#define IXGBE_EICR_GPI_SDP2     0x04000000 /* Gen Purpose Interrupt on SDP2 */
+#define IXGBE_EICR_GPI_SDP0_8259X	0x01000000 /* Gen Purpose INT on SDP0 */
+#define IXGBE_EICR_GPI_SDP1_8259X	0x02000000 /* Gen Purpose INT on SDP1 */
+#define IXGBE_EICR_GPI_SDP2_8259X	0x04000000 /* Gen Purpose INT on SDP2 */
+#define IXGBE_EICR_GPI_SDP0_X540	0x02000000
+#define IXGBE_EICR_GPI_SDP1_X540	0x04000000
+#define IXGBE_EICR_GPI_SDP2_X540	0x08000000
+#define IXGBE_EICR_GPI_SDP0_X550	IXGBE_EICR_GPI_SDP0_X540
+#define IXGBE_EICR_GPI_SDP1_X550	IXGBE_EICR_GPI_SDP1_X540
+#define IXGBE_EICR_GPI_SDP2_X550	IXGBE_EICR_GPI_SDP2_X540
+#define IXGBE_EICR_GPI_SDP0_X550EM_x	IXGBE_EICR_GPI_SDP0_X540
+#define IXGBE_EICR_GPI_SDP1_X550EM_x	IXGBE_EICR_GPI_SDP1_X540
+#define IXGBE_EICR_GPI_SDP2_X550EM_x	IXGBE_EICR_GPI_SDP2_X540
+#define IXGBE_EICR_GPI_SDP0_X550EM_a	IXGBE_EICR_GPI_SDP0_X540
+#define IXGBE_EICR_GPI_SDP1_X550EM_a	IXGBE_EICR_GPI_SDP1_X540
+#define IXGBE_EICR_GPI_SDP2_X550EM_a	IXGBE_EICR_GPI_SDP2_X540
+#define IXGBE_EICR_GPI_SDP0(_hw)	IXGBE_BY_MAC((_hw), EICR_GPI_SDP0)
+#define IXGBE_EICR_GPI_SDP1(_hw)	IXGBE_BY_MAC((_hw), EICR_GPI_SDP1)
+#define IXGBE_EICR_GPI_SDP2(_hw)	IXGBE_BY_MAC((_hw), EICR_GPI_SDP2)
+
 #define IXGBE_EICR_ECC          0x10000000 /* ECC Error */
 #define IXGBE_EICR_PBUR         0x10000000 /* Packet Buffer Handler Error */
 #define IXGBE_EICR_DHER         0x20000000 /* Descriptor Handler Error */
@@ -1435,9 +1589,9 @@ enum {
 #define IXGBE_EICS_LSC          IXGBE_EICR_LSC       /* Link Status Change */
 #define IXGBE_EICS_MNG          IXGBE_EICR_MNG       /* MNG Event Interrupt */
 #define IXGBE_EICS_TIMESYNC     IXGBE_EICR_TIMESYNC  /* Timesync Event */
-#define IXGBE_EICS_GPI_SDP0     IXGBE_EICR_GPI_SDP0  /* SDP0 Gen Purpose Int */
-#define IXGBE_EICS_GPI_SDP1     IXGBE_EICR_GPI_SDP1  /* SDP1 Gen Purpose Int */
-#define IXGBE_EICS_GPI_SDP2     IXGBE_EICR_GPI_SDP2  /* SDP2 Gen Purpose Int */
+#define IXGBE_EICS_GPI_SDP0(_hw)	IXGBE_EICR_GPI_SDP0(_hw)
+#define IXGBE_EICS_GPI_SDP1(_hw)	IXGBE_EICR_GPI_SDP1(_hw)
+#define IXGBE_EICS_GPI_SDP2(_hw)	IXGBE_EICR_GPI_SDP2(_hw)
 #define IXGBE_EICS_ECC          IXGBE_EICR_ECC       /* ECC Error */
 #define IXGBE_EICS_PBUR         IXGBE_EICR_PBUR      /* Pkt Buf Handler Err */
 #define IXGBE_EICS_DHER         IXGBE_EICR_DHER      /* Desc Handler Error */
@@ -1454,9 +1608,9 @@ enum {
 #define IXGBE_EIMS_MNG          IXGBE_EICR_MNG       /* MNG Event Interrupt */
 #define IXGBE_EIMS_TS           IXGBE_EICR_TS        /* Thermel Sensor Event */
 #define IXGBE_EIMS_TIMESYNC     IXGBE_EICR_TIMESYNC  /* Timesync Event */
-#define IXGBE_EIMS_GPI_SDP0     IXGBE_EICR_GPI_SDP0  /* SDP0 Gen Purpose Int */
-#define IXGBE_EIMS_GPI_SDP1     IXGBE_EICR_GPI_SDP1  /* SDP1 Gen Purpose Int */
-#define IXGBE_EIMS_GPI_SDP2     IXGBE_EICR_GPI_SDP2  /* SDP2 Gen Purpose Int */
+#define IXGBE_EIMS_GPI_SDP0(_hw)	IXGBE_EICR_GPI_SDP0(_hw)
+#define IXGBE_EIMS_GPI_SDP1(_hw)	IXGBE_EICR_GPI_SDP1(_hw)
+#define IXGBE_EIMS_GPI_SDP2(_hw)	IXGBE_EICR_GPI_SDP2(_hw)
 #define IXGBE_EIMS_ECC          IXGBE_EICR_ECC       /* ECC Error */
 #define IXGBE_EIMS_PBUR         IXGBE_EICR_PBUR      /* Pkt Buf Handler Err */
 #define IXGBE_EIMS_DHER         IXGBE_EICR_DHER      /* Descr Handler Error */
@@ -1472,9 +1626,9 @@ enum {
 #define IXGBE_EIMC_LSC          IXGBE_EICR_LSC       /* Link Status Change */
 #define IXGBE_EIMC_MNG          IXGBE_EICR_MNG       /* MNG Event Interrupt */
 #define IXGBE_EIMC_TIMESYNC     IXGBE_EICR_TIMESYNC  /* Timesync Event */
-#define IXGBE_EIMC_GPI_SDP0     IXGBE_EICR_GPI_SDP0  /* SDP0 Gen Purpose Int */
-#define IXGBE_EIMC_GPI_SDP1     IXGBE_EICR_GPI_SDP1  /* SDP1 Gen Purpose Int */
-#define IXGBE_EIMC_GPI_SDP2     IXGBE_EICR_GPI_SDP2  /* SDP2 Gen Purpose Int */
+#define IXGBE_EIMC_GPI_SDP0(_hw)	IXGBE_EICR_GPI_SDP0(_hw)
+#define IXGBE_EIMC_GPI_SDP1(_hw)	IXGBE_EICR_GPI_SDP1(_hw)
+#define IXGBE_EIMC_GPI_SDP2(_hw)	IXGBE_EICR_GPI_SDP2(_hw)
 #define IXGBE_EIMC_ECC          IXGBE_EICR_ECC       /* ECC Error */
 #define IXGBE_EIMC_PBUR         IXGBE_EICR_PBUR      /* Pkt Buf Handler Err */
 #define IXGBE_EIMC_DHER         IXGBE_EICR_DHER      /* Desc Handler Err */
@@ -2741,6 +2895,37 @@ union ixgbe_atr_hash_dword {
 	__be32 dword;
 };
 
+#define IXGBE_MVALS_INIT(m)		\
+	IXGBE_CAT(EEC, m),		\
+	IXGBE_CAT(FLA, m),		\
+	IXGBE_CAT(GRC, m),		\
+	IXGBE_CAT(SRAMREL, m),		\
+	IXGBE_CAT(FACTPS, m),		\
+	IXGBE_CAT(SWSM, m),		\
+	IXGBE_CAT(SWFW_SYNC, m),	\
+	IXGBE_CAT(FWSM, m),		\
+	IXGBE_CAT(SDP0_GPIEN, m),	\
+	IXGBE_CAT(SDP1_GPIEN, m),	\
+	IXGBE_CAT(SDP2_GPIEN, m),	\
+	IXGBE_CAT(EICR_GPI_SDP0, m),	\
+	IXGBE_CAT(EICR_GPI_SDP1, m),	\
+	IXGBE_CAT(EICR_GPI_SDP2, m),	\
+	IXGBE_CAT(CIAA, m),		\
+	IXGBE_CAT(CIAD, m),		\
+	IXGBE_CAT(I2C_CLK_IN, m),	\
+	IXGBE_CAT(I2C_CLK_OUT, m),	\
+	IXGBE_CAT(I2C_DATA_IN, m),	\
+	IXGBE_CAT(I2C_DATA_OUT, m),	\
+	IXGBE_CAT(I2C_DATA_OE_N_EN, m),	\
+	IXGBE_CAT(I2C_BB_EN, m),	\
+	IXGBE_CAT(I2C_CLK_OE_N_EN, m),	\
+	IXGBE_CAT(I2CCTL, m)
+
+enum ixgbe_mvals {
+	IXGBE_MVALS_INIT(IDX),
+	IXGBE_MVALS_IDX_LIMIT
+};
+
 enum ixgbe_eeprom_type {
 	ixgbe_eeprom_uninitialized = 0,
 	ixgbe_eeprom_spi,
@@ -3112,6 +3297,8 @@ struct ixgbe_phy_operations {
 	s32 (*read_i2c_combined)(struct ixgbe_hw *, u8 addr, u16 reg, u16 *val);
 	s32 (*write_i2c_combined)(struct ixgbe_hw *, u8 addr, u16 reg, u16 val);
 	s32 (*check_overtemp)(struct ixgbe_hw *);
+	s32 (*set_phy_power)(struct ixgbe_hw *, bool on);
+	s32 (*handle_lasi)(struct ixgbe_hw *hw);
 };
 
 struct ixgbe_eeprom_info {
@@ -3173,6 +3360,7 @@ struct ixgbe_phy_info {
 	bool                            multispeed_fiber;
 	bool                            reset_if_overtemp;
 	bool                            qsfp_shared_i2c_bus;
+	u32				nw_mng_if_sel;
 };
 
 #include "ixgbe_mbx.h"
@@ -3216,6 +3404,7 @@ struct ixgbe_hw {
 	struct ixgbe_eeprom_info	eeprom;
 	struct ixgbe_bus_info		bus;
 	struct ixgbe_mbx_info		mbx;
+	const u32			*mvals;
 	u16				device_id;
 	u16				vendor_id;
 	u16				subsystem_device_id;
@@ -3234,6 +3423,7 @@ struct ixgbe_info {
 	struct ixgbe_eeprom_operations	*eeprom_ops;
 	struct ixgbe_phy_operations	*phy_ops;
 	struct ixgbe_mbx_operations	*mbx_ops;
+	const u32			*mvals;
 };
 
 
@@ -3339,4 +3529,6 @@ struct ixgbe_info {
 #define IXGBE_SB_IOSF_TARGET_KX4_PCS0	2
 #define IXGBE_SB_IOSF_TARGET_KX4_PCS1	3
 
+#define IXGBE_NW_MNG_IF_SEL		0x00011178
+#define IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE	BIT(24)
 #endif /* _IXGBE_TYPE_H_ */
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
index f5f948d08b43..032a5870abd1 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
@@ -202,7 +202,7 @@ s32 ixgbe_init_eeprom_params_X540(struct ixgbe_hw *hw)
 		eeprom->semaphore_delay = 10;
 		eeprom->type = ixgbe_flash;
 
-		eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+		eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 		eeprom_size = (u16)((eec & IXGBE_EEC_SIZE) >>
 				    IXGBE_EEC_SIZE_SHIFT);
 		eeprom->word_size = 1 << (eeprom_size +
@@ -504,8 +504,8 @@ static s32 ixgbe_update_flash_X540(struct ixgbe_hw *hw)
 		return status;
 	}
 
-	flup = IXGBE_READ_REG(hw, IXGBE_EEC) | IXGBE_EEC_FLUP;
-	IXGBE_WRITE_REG(hw, IXGBE_EEC, flup);
+	flup = IXGBE_READ_REG(hw, IXGBE_EEC(hw)) | IXGBE_EEC_FLUP;
+	IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), flup);
 
 	status = ixgbe_poll_flash_update_done_X540(hw);
 	if (status == 0)
@@ -514,11 +514,11 @@ static s32 ixgbe_update_flash_X540(struct ixgbe_hw *hw)
 		hw_dbg(hw, "Flash update time out\n");
 
 	if (hw->revision_id == 0) {
-		flup = IXGBE_READ_REG(hw, IXGBE_EEC);
+		flup = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 
 		if (flup & IXGBE_EEC_SEC1VAL) {
 			flup |= IXGBE_EEC_FLUP;
-			IXGBE_WRITE_REG(hw, IXGBE_EEC, flup);
+			IXGBE_WRITE_REG(hw, IXGBE_EEC(hw), flup);
 		}
 
 		status = ixgbe_poll_flash_update_done_X540(hw);
@@ -544,7 +544,7 @@ static s32 ixgbe_poll_flash_update_done_X540(struct ixgbe_hw *hw)
 	u32 reg;
 
 	for (i = 0; i < IXGBE_FLUDONE_ATTEMPTS; i++) {
-		reg = IXGBE_READ_REG(hw, IXGBE_EEC);
+		reg = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 		if (reg & IXGBE_EEC_FLUDONE)
 			return 0;
 		udelay(5);
@@ -580,10 +580,10 @@ s32 ixgbe_acquire_swfw_sync_X540(struct ixgbe_hw *hw, u32 mask)
 		if (ixgbe_get_swfw_sync_semaphore(hw))
 			return IXGBE_ERR_SWFW_SYNC;
 
-		swfw_sync = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC);
+		swfw_sync = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC(hw));
 		if (!(swfw_sync & (fwmask | swmask | hwmask))) {
 			swfw_sync |= swmask;
-			IXGBE_WRITE_REG(hw, IXGBE_SWFW_SYNC, swfw_sync);
+			IXGBE_WRITE_REG(hw, IXGBE_SWFW_SYNC(hw), swfw_sync);
 			ixgbe_release_swfw_sync_semaphore(hw);
 			break;
 		} else {
@@ -605,13 +605,13 @@ s32 ixgbe_acquire_swfw_sync_X540(struct ixgbe_hw *hw, u32 mask)
 	 * corresponding FW/HW bits in the SW_FW_SYNC register.
 	 */
 	if (i >= timeout) {
-		swfw_sync = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC);
+		swfw_sync = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC(hw));
 		if (swfw_sync & (fwmask | hwmask)) {
 			if (ixgbe_get_swfw_sync_semaphore(hw))
 				return IXGBE_ERR_SWFW_SYNC;
 
 			swfw_sync |= swmask;
-			IXGBE_WRITE_REG(hw, IXGBE_SWFW_SYNC, swfw_sync);
+			IXGBE_WRITE_REG(hw, IXGBE_SWFW_SYNC(hw), swfw_sync);
 			ixgbe_release_swfw_sync_semaphore(hw);
 		}
 	}
@@ -635,9 +635,9 @@ void ixgbe_release_swfw_sync_X540(struct ixgbe_hw *hw, u32 mask)
 
 	ixgbe_get_swfw_sync_semaphore(hw);
 
-	swfw_sync = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC);
+	swfw_sync = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC(hw));
 	swfw_sync &= ~swmask;
-	IXGBE_WRITE_REG(hw, IXGBE_SWFW_SYNC, swfw_sync);
+	IXGBE_WRITE_REG(hw, IXGBE_SWFW_SYNC(hw), swfw_sync);
 
 	ixgbe_release_swfw_sync_semaphore(hw);
 	usleep_range(5000, 10000);
@@ -660,7 +660,7 @@ static s32 ixgbe_get_swfw_sync_semaphore(struct ixgbe_hw *hw)
 		/* If the SMBI bit is 0 when we read it, then the bit will be
 		 * set and we have the semaphore
 		 */
-		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM);
+		swsm = IXGBE_READ_REG(hw, IXGBE_SWSM(hw));
 		if (!(swsm & IXGBE_SWSM_SMBI))
 			break;
 		usleep_range(50, 100);
@@ -674,7 +674,7 @@ static s32 ixgbe_get_swfw_sync_semaphore(struct ixgbe_hw *hw)
 
 	/* Now get the semaphore between SW/FW through the REGSMP bit */
 	for (i = 0; i < timeout; i++) {
-		swsm = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC);
+		swsm = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC(hw));
 		if (!(swsm & IXGBE_SWFW_REGSMP))
 			return 0;
 
@@ -696,13 +696,13 @@ static void ixgbe_release_swfw_sync_semaphore(struct ixgbe_hw *hw)
 
 	/* Release both semaphores by writing 0 to the bits REGSMP and SMBI */
 
-	swsm = IXGBE_READ_REG(hw, IXGBE_SWSM);
-	swsm &= ~IXGBE_SWSM_SMBI;
-	IXGBE_WRITE_REG(hw, IXGBE_SWSM, swsm);
-
-	swsm = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC);
+	swsm = IXGBE_READ_REG(hw, IXGBE_SWFW_SYNC(hw));
 	swsm &= ~IXGBE_SWFW_REGSMP;
-	IXGBE_WRITE_REG(hw, IXGBE_SWFW_SYNC, swsm);
+	IXGBE_WRITE_REG(hw, IXGBE_SWFW_SYNC(hw), swsm);
+
+	swsm = IXGBE_READ_REG(hw, IXGBE_SWSM(hw));
+	swsm &= ~IXGBE_SWSM_SMBI;
+	IXGBE_WRITE_REG(hw, IXGBE_SWSM(hw), swsm);
 
 	IXGBE_WRITE_FLUSH(hw);
 }
@@ -850,9 +850,14 @@ static struct ixgbe_phy_operations phy_ops_X540 = {
 	.read_i2c_eeprom        = &ixgbe_read_i2c_eeprom_generic,
 	.write_i2c_eeprom       = &ixgbe_write_i2c_eeprom_generic,
 	.check_overtemp         = &ixgbe_tn_check_overtemp,
+	.set_phy_power          = &ixgbe_set_copper_phy_power,
 	.get_firmware_version   = &ixgbe_get_phy_firmware_version_generic,
 };
 
+static const u32 ixgbe_mvals_X540[IXGBE_MVALS_IDX_LIMIT] = {
+	IXGBE_MVALS_INIT(X540)
+};
+
 struct ixgbe_info ixgbe_X540_info = {
 	.mac                    = ixgbe_mac_X540,
 	.get_invariants         = &ixgbe_get_invariants_X540,
@@ -860,4 +865,5 @@ struct ixgbe_info ixgbe_X540_info = {
 	.eeprom_ops             = &eeprom_ops_X540,
 	.phy_ops                = &phy_ops_X540,
 	.mbx_ops                = &mbx_ops_generic,
+	.mvals			= ixgbe_mvals_X540,
 };
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
index cf5cf819a6b8..7581da13e92a 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
  *
  *  Intel 10 Gigabit PCI Express Linux driver
- *  Copyright(c) 1999 - 2014 Intel Corporation.
+ *  Copyright(c) 1999 - 2015 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,
@@ -26,6 +26,22 @@
 #include "ixgbe_common.h"
 #include "ixgbe_phy.h"
 
+/** ixgbe_setup_mux_ctl - Setup ESDP register for I2C mux control
+ *  @hw: pointer to hardware structure
+ **/
+static void ixgbe_setup_mux_ctl(struct ixgbe_hw *hw)
+{
+	u32 esdp = IXGBE_READ_REG(hw, IXGBE_ESDP);
+
+	if (hw->bus.lan_id) {
+		esdp &= ~(IXGBE_ESDP_SDP1_NATIVE | IXGBE_ESDP_SDP1);
+		esdp |= IXGBE_ESDP_SDP1_DIR;
+	}
+	esdp &= ~(IXGBE_ESDP_SDP0_NATIVE | IXGBE_ESDP_SDP0_DIR);
+	IXGBE_WRITE_REG(hw, IXGBE_ESDP, esdp);
+	IXGBE_WRITE_FLUSH(hw);
+}
+
 /** ixgbe_identify_phy_x550em - Get PHY type based on device id
  *  @hw: pointer to hardware structure
  *
@@ -33,18 +49,11 @@
  */
 static s32 ixgbe_identify_phy_x550em(struct ixgbe_hw *hw)
 {
-	u32 esdp = IXGBE_READ_REG(hw, IXGBE_ESDP);
-
 	switch (hw->device_id) {
 	case IXGBE_DEV_ID_X550EM_X_SFP:
 		/* set up for CS4227 usage */
 		hw->phy.phy_semaphore_mask = IXGBE_GSSR_SHARED_I2C_SM;
-		if (hw->bus.lan_id) {
-			esdp &= ~(IXGBE_ESDP_SDP1_NATIVE | IXGBE_ESDP_SDP1);
-			esdp |= IXGBE_ESDP_SDP1_DIR;
-		}
-		esdp &= ~(IXGBE_ESDP_SDP0_NATIVE | IXGBE_ESDP_SDP0_DIR);
-		IXGBE_WRITE_REG(hw, IXGBE_ESDP, esdp);
+		ixgbe_setup_mux_ctl(hw);
 
 		return ixgbe_identify_module_generic(hw);
 	case IXGBE_DEV_ID_X550EM_X_KX4:
@@ -90,7 +99,7 @@ static s32 ixgbe_init_eeprom_params_X550(struct ixgbe_hw *hw)
 		eeprom->semaphore_delay = 10;
 		eeprom->type = ixgbe_flash;
 
-		eec = IXGBE_READ_REG(hw, IXGBE_EEC);
+		eec = IXGBE_READ_REG(hw, IXGBE_EEC(hw));
 		eeprom_size = (u16)((eec & IXGBE_EEC_SIZE) >>
 				    IXGBE_EEC_SIZE_SHIFT);
 		eeprom->word_size = 1 << (eeprom_size +
@@ -103,6 +112,39 @@ static s32 ixgbe_init_eeprom_params_X550(struct ixgbe_hw *hw)
 	return 0;
 }
 
+/**
+ * ixgbe_iosf_wait - Wait for IOSF command completion
+ * @hw: pointer to hardware structure
+ * @ctrl: pointer to location to receive final IOSF control value
+ *
+ * Return: failing status on timeout
+ *
+ * Note: ctrl can be NULL if the IOSF control register value is not needed
+ */
+static s32 ixgbe_iosf_wait(struct ixgbe_hw *hw, u32 *ctrl)
+{
+	u32 i, command;
+
+	/* Check every 10 usec to see if the address cycle completed.
+	 * The SB IOSF BUSY bit will clear when the operation is
+	 * complete.
+	 */
+	for (i = 0; i < IXGBE_MDIO_COMMAND_TIMEOUT; i++) {
+		command = IXGBE_READ_REG(hw, IXGBE_SB_IOSF_INDIRECT_CTRL);
+		if (!(command & IXGBE_SB_IOSF_CTRL_BUSY))
+			break;
+		usleep_range(10, 20);
+	}
+	if (ctrl)
+		*ctrl = command;
+	if (i == IXGBE_MDIO_COMMAND_TIMEOUT) {
+		hw_dbg(hw, "IOSF wait timed out\n");
+		return IXGBE_ERR_PHY;
+	}
+
+	return 0;
+}
+
 /** ixgbe_read_iosf_sb_reg_x550 - Writes a value to specified register of the
  *  IOSF device
  *  @hw: pointer to hardware structure
@@ -113,7 +155,17 @@ static s32 ixgbe_init_eeprom_params_X550(struct ixgbe_hw *hw)
 static s32 ixgbe_read_iosf_sb_reg_x550(struct ixgbe_hw *hw, u32 reg_addr,
 				       u32 device_type, u32 *data)
 {
-	u32 i, command, error;
+	u32 gssr = IXGBE_GSSR_PHY1_SM | IXGBE_GSSR_PHY0_SM;
+	u32 command, error;
+	s32 ret;
+
+	ret = hw->mac.ops.acquire_swfw_sync(hw, gssr);
+	if (ret)
+		return ret;
+
+	ret = ixgbe_iosf_wait(hw, NULL);
+	if (ret)
+		goto out;
 
 	command = ((reg_addr << IXGBE_SB_IOSF_CTRL_ADDR_SHIFT) |
 		   (device_type << IXGBE_SB_IOSF_CTRL_TARGET_SELECT_SHIFT));
@@ -121,17 +173,7 @@ static s32 ixgbe_read_iosf_sb_reg_x550(struct ixgbe_hw *hw, u32 reg_addr,
 	/* Write IOSF control register */
 	IXGBE_WRITE_REG(hw, IXGBE_SB_IOSF_INDIRECT_CTRL, command);
 
-	/* Check every 10 usec to see if the address cycle completed.
-	 * The SB IOSF BUSY bit will clear when the operation is
-	 * complete
-	 */
-	for (i = 0; i < IXGBE_MDIO_COMMAND_TIMEOUT; i++) {
-		usleep_range(10, 20);
-
-		command = IXGBE_READ_REG(hw, IXGBE_SB_IOSF_INDIRECT_CTRL);
-		if ((command & IXGBE_SB_IOSF_CTRL_BUSY) == 0)
-			break;
-	}
+	ret = ixgbe_iosf_wait(hw, &command);
 
 	if ((command & IXGBE_SB_IOSF_CTRL_RESP_STAT_MASK) != 0) {
 		error = (command & IXGBE_SB_IOSF_CTRL_CMPL_ERR_MASK) >>
@@ -140,14 +182,12 @@ static s32 ixgbe_read_iosf_sb_reg_x550(struct ixgbe_hw *hw, u32 reg_addr,
 		return IXGBE_ERR_PHY;
 	}
 
-	if (i == IXGBE_MDIO_COMMAND_TIMEOUT) {
-		hw_dbg(hw, "Read timed out\n");
-		return IXGBE_ERR_PHY;
-	}
-
-	*data = IXGBE_READ_REG(hw, IXGBE_SB_IOSF_INDIRECT_DATA);
+	if (!ret)
+		*data = IXGBE_READ_REG(hw, IXGBE_SB_IOSF_INDIRECT_DATA);
 
-	return 0;
+out:
+	hw->mac.ops.release_swfw_sync(hw, gssr);
+	return ret;
 }
 
 /** ixgbe_read_ee_hostif_data_X550 - Read EEPROM word using a host interface
@@ -673,6 +713,249 @@ static s32 ixgbe_write_ee_hostif_buffer_X550(struct ixgbe_hw *hw,
 	return status;
 }
 
+/** ixgbe_write_iosf_sb_reg_x550 - Writes a value to specified register of the
+ *  IOSF device
+ *
+ *  @hw: pointer to hardware structure
+ *  @reg_addr: 32 bit PHY register to write
+ *  @device_type: 3 bit device type
+ *  @data: Data to write to the register
+ **/
+static s32 ixgbe_write_iosf_sb_reg_x550(struct ixgbe_hw *hw, u32 reg_addr,
+					u32 device_type, u32 data)
+{
+	u32 gssr = IXGBE_GSSR_PHY1_SM | IXGBE_GSSR_PHY0_SM;
+	u32 command, error;
+	s32 ret;
+
+	ret = hw->mac.ops.acquire_swfw_sync(hw, gssr);
+	if (ret)
+		return ret;
+
+	ret = ixgbe_iosf_wait(hw, NULL);
+	if (ret)
+		goto out;
+
+	command = ((reg_addr << IXGBE_SB_IOSF_CTRL_ADDR_SHIFT) |
+		   (device_type << IXGBE_SB_IOSF_CTRL_TARGET_SELECT_SHIFT));
+
+	/* Write IOSF control register */
+	IXGBE_WRITE_REG(hw, IXGBE_SB_IOSF_INDIRECT_CTRL, command);
+
+	/* Write IOSF data register */
+	IXGBE_WRITE_REG(hw, IXGBE_SB_IOSF_INDIRECT_DATA, data);
+
+	ret = ixgbe_iosf_wait(hw, &command);
+
+	if ((command & IXGBE_SB_IOSF_CTRL_RESP_STAT_MASK) != 0) {
+		error = (command & IXGBE_SB_IOSF_CTRL_CMPL_ERR_MASK) >>
+			 IXGBE_SB_IOSF_CTRL_CMPL_ERR_SHIFT;
+		hw_dbg(hw, "Failed to write, error %x\n", error);
+		return IXGBE_ERR_PHY;
+	}
+
+out:
+	hw->mac.ops.release_swfw_sync(hw, gssr);
+	return ret;
+}
+
+/** ixgbe_setup_ixfi_x550em - Configure the KR PHY for iXFI mode.
+ *  @hw: pointer to hardware structure
+ *  @speed: the link speed to force
+ *
+ *  Configures the integrated KR PHY to use iXFI mode. Used to connect an
+ *  internal and external PHY at a specific speed, without autonegotiation.
+ **/
+static s32 ixgbe_setup_ixfi_x550em(struct ixgbe_hw *hw, ixgbe_link_speed *speed)
+{
+	s32 status;
+	u32 reg_val;
+
+	/* Disable AN and force speed to 10G Serial. */
+	status = ixgbe_read_iosf_sb_reg_x550(hw,
+					IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
+					IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	if (status)
+		return status;
+
+	reg_val &= ~IXGBE_KRM_LINK_CTRL_1_TETH_AN_ENABLE;
+	reg_val &= ~IXGBE_KRM_LINK_CTRL_1_TETH_FORCE_SPEED_MASK;
+
+	/* Select forced link speed for internal PHY. */
+	switch (*speed) {
+	case IXGBE_LINK_SPEED_10GB_FULL:
+		reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_FORCE_SPEED_10G;
+		break;
+	case IXGBE_LINK_SPEED_1GB_FULL:
+		reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_FORCE_SPEED_1G;
+		break;
+	default:
+		/* Other link speeds are not supported by internal KR PHY. */
+		return IXGBE_ERR_LINK_SETUP;
+	}
+
+	status = ixgbe_write_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	if (status)
+		return status;
+
+	/* Disable training protocol FSM. */
+	status = ixgbe_read_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_RX_TRN_LINKUP_CTRL(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	if (status)
+		return status;
+
+	reg_val |= IXGBE_KRM_RX_TRN_LINKUP_CTRL_CONV_WO_PROTOCOL;
+	status = ixgbe_write_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_RX_TRN_LINKUP_CTRL(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	if (status)
+		return status;
+
+	/* Disable Flex from training TXFFE. */
+	status = ixgbe_read_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_DSP_TXFFE_STATE_4(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	if (status)
+		return status;
+
+	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_C0_EN;
+	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_CP1_CN1_EN;
+	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_CO_ADAPT_EN;
+	status = ixgbe_write_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_DSP_TXFFE_STATE_4(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	if (status)
+		return status;
+
+	status = ixgbe_read_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_DSP_TXFFE_STATE_5(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	if (status)
+		return status;
+
+	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_C0_EN;
+	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_CP1_CN1_EN;
+	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_CO_ADAPT_EN;
+	status = ixgbe_write_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_DSP_TXFFE_STATE_5(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	if (status)
+		return status;
+
+	/* Enable override for coefficients. */
+	status = ixgbe_read_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_TX_COEFF_CTRL_1(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	if (status)
+		return status;
+
+	reg_val |= IXGBE_KRM_TX_COEFF_CTRL_1_OVRRD_EN;
+	reg_val |= IXGBE_KRM_TX_COEFF_CTRL_1_CZERO_EN;
+	reg_val |= IXGBE_KRM_TX_COEFF_CTRL_1_CPLUS1_OVRRD_EN;
+	reg_val |= IXGBE_KRM_TX_COEFF_CTRL_1_CMINUS1_OVRRD_EN;
+	status = ixgbe_write_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_TX_COEFF_CTRL_1(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	if (status)
+		return status;
+
+	/* Toggle port SW reset by AN reset. */
+	status = ixgbe_read_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	if (status)
+		return status;
+
+	reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_RESTART;
+	status = ixgbe_write_iosf_sb_reg_x550(hw,
+				IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
+				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+
+	return status;
+}
+
+/**
+ * ixgbe_setup_mac_link_t_X550em - Sets the auto advertised link speed
+ * @hw: pointer to hardware structure
+ * @speed: new link speed
+ * @autoneg_wait_to_complete: true when waiting for completion is needed
+ *
+ * Setup internal/external PHY link speed based on link speed, then set
+ * external PHY auto advertised link speed.
+ *
+ * Returns error status for any failure
+ **/
+static s32 ixgbe_setup_mac_link_t_X550em(struct ixgbe_hw *hw,
+					 ixgbe_link_speed speed,
+					 bool autoneg_wait)
+{
+	s32 status;
+	ixgbe_link_speed force_speed;
+
+	/* Setup internal/external PHY link speed to iXFI (10G), unless
+	 * only 1G is auto advertised then setup KX link.
+	 */
+	if (speed & IXGBE_LINK_SPEED_10GB_FULL)
+		force_speed = IXGBE_LINK_SPEED_10GB_FULL;
+	else
+		force_speed = IXGBE_LINK_SPEED_1GB_FULL;
+
+	/* If internal link mode is XFI, then setup XFI internal link. */
+	if (!(hw->phy.nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE)) {
+		status = ixgbe_setup_ixfi_x550em(hw, &force_speed);
+
+		if (status)
+			return status;
+	}
+
+	return hw->phy.ops.setup_link_speed(hw, speed, autoneg_wait);
+}
+
+/** ixgbe_check_link_t_X550em - Determine link and speed status
+  * @hw: pointer to hardware structure
+  * @speed: pointer to link speed
+  * @link_up: true when link is up
+  * @link_up_wait_to_complete: bool used to wait for link up or not
+  *
+  * Check that both the MAC and X557 external PHY have link.
+  **/
+static s32 ixgbe_check_link_t_X550em(struct ixgbe_hw *hw,
+				     ixgbe_link_speed *speed,
+				     bool *link_up,
+				     bool link_up_wait_to_complete)
+{
+	u32 status;
+	u16 autoneg_status;
+
+	if (hw->mac.ops.get_media_type(hw) != ixgbe_media_type_copper)
+		return IXGBE_ERR_CONFIG;
+
+	status = ixgbe_check_mac_link_generic(hw, speed, link_up,
+					      link_up_wait_to_complete);
+
+	/* If check link fails or MAC link is not up, then return */
+	if (status || !(*link_up))
+		return status;
+
+	 /* MAC link is up, so check external PHY link.
+	  * Read this twice back to back to indicate current status.
+	  */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_AUTO_NEG_STATUS,
+				      IXGBE_MDIO_AUTO_NEG_DEV_TYPE,
+				      &autoneg_status);
+	if (status)
+		return status;
+
+	/* If external PHY link is not up, then indicate link not up */
+	if (!(autoneg_status & IXGBE_MDIO_AUTO_NEG_LINK_STATUS))
+		*link_up = false;
+
+	return 0;
+}
+
 /** ixgbe_init_mac_link_ops_X550em - init mac link function pointers
  *  @hw: pointer to hardware structure
  **/
@@ -680,13 +963,21 @@ static void ixgbe_init_mac_link_ops_X550em(struct ixgbe_hw *hw)
 {
 	struct ixgbe_mac_info *mac = &hw->mac;
 
-	/* CS4227 does not support autoneg, so disable the laser control
-	 * functions for SFP+ fiber
-	 */
-	if (hw->device_id == IXGBE_DEV_ID_X550EM_X_SFP) {
+	switch (mac->ops.get_media_type(hw)) {
+	case ixgbe_media_type_fiber:
+		/* CS4227 does not support autoneg, so disable the laser control
+		 * functions for SFP+ fiber
+		 */
 		mac->ops.disable_tx_laser = NULL;
 		mac->ops.enable_tx_laser = NULL;
 		mac->ops.flap_tx_laser = NULL;
+		break;
+	case ixgbe_media_type_copper:
+		mac->ops.setup_link = ixgbe_setup_mac_link_t_X550em;
+		mac->ops.check_link = ixgbe_check_link_t_X550em;
+		break;
+	default:
+		break;
 	}
 }
 
@@ -778,169 +1069,221 @@ static s32 ixgbe_get_link_capabilities_X550em(struct ixgbe_hw *hw,
 	return 0;
 }
 
-/** ixgbe_write_iosf_sb_reg_x550 - Writes a value to specified register of the
- *  IOSF device
+/**
+ * ixgbe_get_lasi_ext_t_x550em - Determime external Base T PHY interrupt cause
+ * @hw: pointer to hardware structure
+ * @lsc: pointer to boolean flag which indicates whether external Base T
+ *	 PHY interrupt is lsc
  *
- *  @hw: pointer to hardware structure
- *  @reg_addr: 32 bit PHY register to write
- *  @device_type: 3 bit device type
- *  @data: Data to write to the register
+ * Determime if external Base T PHY interrupt cause is high temperature
+ * failure alarm or link status change.
+ *
+ * Return IXGBE_ERR_OVERTEMP if interrupt is high temperature
+ * failure alarm, else return PHY access status.
  **/
-static s32 ixgbe_write_iosf_sb_reg_x550(struct ixgbe_hw *hw, u32 reg_addr,
-					u32 device_type, u32 data)
+static s32 ixgbe_get_lasi_ext_t_x550em(struct ixgbe_hw *hw, bool *lsc)
 {
-	u32 i, command, error;
+	u32 status;
+	u16 reg;
 
-	command = ((reg_addr << IXGBE_SB_IOSF_CTRL_ADDR_SHIFT) |
-		   (device_type << IXGBE_SB_IOSF_CTRL_TARGET_SELECT_SHIFT));
+	*lsc = false;
 
-	/* Write IOSF control register */
-	IXGBE_WRITE_REG(hw, IXGBE_SB_IOSF_INDIRECT_CTRL, command);
+	/* Vendor alarm triggered */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_CHIP_STD_INT_FLAG,
+				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				      &reg);
 
-	/* Write IOSF data register */
-	IXGBE_WRITE_REG(hw, IXGBE_SB_IOSF_INDIRECT_DATA, data);
+	if (status || !(reg & IXGBE_MDIO_GLOBAL_VEN_ALM_INT_EN))
+		return status;
 
-	/* Check every 10 usec to see if the address cycle completed.
-	 * The SB IOSF BUSY bit will clear when the operation is
-	 * complete
-	 */
-	for (i = 0; i < IXGBE_MDIO_COMMAND_TIMEOUT; i++) {
-		usleep_range(10, 20);
+	/* Vendor Auto-Neg alarm triggered or Global alarm 1 triggered */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_INT_CHIP_VEN_FLAG,
+				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				      &reg);
 
-		command = IXGBE_READ_REG(hw, IXGBE_SB_IOSF_INDIRECT_CTRL);
-		if ((command & IXGBE_SB_IOSF_CTRL_BUSY) == 0)
-			break;
-	}
+	if (status || !(reg & (IXGBE_MDIO_GLOBAL_AN_VEN_ALM_INT_EN |
+				IXGBE_MDIO_GLOBAL_ALARM_1_INT)))
+		return status;
 
-	if ((command & IXGBE_SB_IOSF_CTRL_RESP_STAT_MASK) != 0) {
-		error = (command & IXGBE_SB_IOSF_CTRL_CMPL_ERR_MASK) >>
-			 IXGBE_SB_IOSF_CTRL_CMPL_ERR_SHIFT;
-		hw_dbg(hw, "Failed to write, error %x\n", error);
-		return IXGBE_ERR_PHY;
-	}
+	/* High temperature failure alarm triggered */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_ALARM_1,
+				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				      &reg);
 
-	if (i == IXGBE_MDIO_COMMAND_TIMEOUT) {
-		hw_dbg(hw, "Write timed out\n");
-		return IXGBE_ERR_PHY;
+	if (status)
+		return status;
+
+	/* If high temperature failure, then return over temp error and exit */
+	if (reg & IXGBE_MDIO_GLOBAL_ALM_1_HI_TMP_FAIL) {
+		/* power down the PHY in case the PHY FW didn't already */
+		ixgbe_set_copper_phy_power(hw, false);
+		return IXGBE_ERR_OVERTEMP;
 	}
 
+	/* Vendor alarm 2 triggered */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_CHIP_STD_INT_FLAG,
+				      IXGBE_MDIO_AUTO_NEG_DEV_TYPE, &reg);
+
+	if (status || !(reg & IXGBE_MDIO_GLOBAL_STD_ALM2_INT))
+		return status;
+
+	/* link connect/disconnect event occurred */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_AUTO_NEG_VENDOR_TX_ALARM2,
+				      IXGBE_MDIO_AUTO_NEG_DEV_TYPE, &reg);
+
+	if (status)
+		return status;
+
+	/* Indicate LSC */
+	if (reg & IXGBE_MDIO_AUTO_NEG_VEN_LSC)
+		*lsc = true;
+
 	return 0;
 }
 
-/** ixgbe_setup_ixfi_x550em - Configure the KR PHY for iXFI mode.
- *  @hw: pointer to hardware structure
- *  @speed: the link speed to force
+/**
+ * ixgbe_enable_lasi_ext_t_x550em - Enable external Base T PHY interrupts
+ * @hw: pointer to hardware structure
  *
- *  Configures the integrated KR PHY to use iXFI mode. Used to connect an
- *  internal and external PHY at a specific speed, without autonegotiation.
+ * Enable link status change and temperature failure alarm for the external
+ * Base T PHY
+ *
+ * Returns PHY access status
  **/
-static s32 ixgbe_setup_ixfi_x550em(struct ixgbe_hw *hw, ixgbe_link_speed *speed)
+static s32 ixgbe_enable_lasi_ext_t_x550em(struct ixgbe_hw *hw)
 {
-	s32 status;
-	u32 reg_val;
+	u32 status;
+	u16 reg;
+	bool lsc;
 
-	/* Disable AN and force speed to 10G Serial. */
-	status = ixgbe_read_iosf_sb_reg_x550(hw,
-					IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
-					IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	/* Clear interrupt flags */
+	status = ixgbe_get_lasi_ext_t_x550em(hw, &lsc);
+
+	/* Enable link status change alarm */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_PMA_TX_VEN_LASI_INT_MASK,
+				      IXGBE_MDIO_AUTO_NEG_DEV_TYPE, &reg);
 	if (status)
 		return status;
 
-	reg_val &= ~IXGBE_KRM_LINK_CTRL_1_TETH_AN_ENABLE;
-	reg_val &= ~IXGBE_KRM_LINK_CTRL_1_TETH_FORCE_SPEED_MASK;
+	reg |= IXGBE_MDIO_PMA_TX_VEN_LASI_INT_EN;
 
-	/* Select forced link speed for internal PHY. */
-	switch (*speed) {
-	case IXGBE_LINK_SPEED_10GB_FULL:
-		reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_FORCE_SPEED_10G;
-		break;
-	case IXGBE_LINK_SPEED_1GB_FULL:
-		reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_FORCE_SPEED_1G;
-		break;
-	default:
-		/* Other link speeds are not supported by internal KR PHY. */
-		return IXGBE_ERR_LINK_SETUP;
-	}
-
-	status = ixgbe_write_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_RX_TRN_LINKUP_CTRL(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	status = hw->phy.ops.write_reg(hw, IXGBE_MDIO_PMA_TX_VEN_LASI_INT_MASK,
+				       IXGBE_MDIO_AUTO_NEG_DEV_TYPE, reg);
 	if (status)
 		return status;
 
-	/* Disable training protocol FSM. */
-	status = ixgbe_read_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_RX_TRN_LINKUP_CTRL(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	/* Enables high temperature failure alarm */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_INT_MASK,
+				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				      &reg);
 	if (status)
 		return status;
 
-	reg_val |= IXGBE_KRM_RX_TRN_LINKUP_CTRL_CONV_WO_PROTOCOL;
-	status = ixgbe_write_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_RX_TRN_LINKUP_CTRL(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
-	if (status)
-		return status;
+	reg |= IXGBE_MDIO_GLOBAL_INT_HI_TEMP_EN;
 
-	/* Disable Flex from training TXFFE. */
-	status = ixgbe_read_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_DSP_TXFFE_STATE_4(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	status = hw->phy.ops.write_reg(hw, IXGBE_MDIO_GLOBAL_INT_MASK,
+				       IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				       reg);
 	if (status)
 		return status;
 
-	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_C0_EN;
-	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_CP1_CN1_EN;
-	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_CO_ADAPT_EN;
-	status = ixgbe_write_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_DSP_TXFFE_STATE_4(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	/* Enable vendor Auto-Neg alarm and Global Interrupt Mask 1 alarm */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_INT_CHIP_VEN_MASK,
+				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				      &reg);
 	if (status)
 		return status;
 
-	status = ixgbe_read_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_DSP_TXFFE_STATE_5(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
-	if (status)
-		return status;
+	reg |= (IXGBE_MDIO_GLOBAL_AN_VEN_ALM_INT_EN |
+		IXGBE_MDIO_GLOBAL_ALARM_1_INT);
 
-	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_C0_EN;
-	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_CP1_CN1_EN;
-	reg_val &= ~IXGBE_KRM_DSP_TXFFE_STATE_CO_ADAPT_EN;
-	status = ixgbe_write_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_DSP_TXFFE_STATE_5(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	status = hw->phy.ops.write_reg(hw, IXGBE_MDIO_GLOBAL_INT_CHIP_VEN_MASK,
+				       IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				       reg);
 	if (status)
 		return status;
 
-	/* Enable override for coefficients. */
-	status = ixgbe_read_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_TX_COEFF_CTRL_1(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+	/* Enable chip-wide vendor alarm */
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_INT_CHIP_STD_MASK,
+				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				      &reg);
 	if (status)
 		return status;
 
-	reg_val |= IXGBE_KRM_TX_COEFF_CTRL_1_OVRRD_EN;
-	reg_val |= IXGBE_KRM_TX_COEFF_CTRL_1_CZERO_EN;
-	reg_val |= IXGBE_KRM_TX_COEFF_CTRL_1_CPLUS1_OVRRD_EN;
-	reg_val |= IXGBE_KRM_TX_COEFF_CTRL_1_CMINUS1_OVRRD_EN;
-	status = ixgbe_write_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_TX_COEFF_CTRL_1(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	reg |= IXGBE_MDIO_GLOBAL_VEN_ALM_INT_EN;
+
+	status = hw->phy.ops.write_reg(hw, IXGBE_MDIO_GLOBAL_INT_CHIP_STD_MASK,
+				       IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+				       reg);
+
+	return status;
+}
+
+/**
+ * ixgbe_handle_lasi_ext_t_x550em - Handle external Base T PHY interrupt
+ * @hw: pointer to hardware structure
+ *
+ * Handle external Base T PHY interrupt. If high temperature
+ * failure alarm then return error, else if link status change
+ * then setup internal/external PHY link
+ *
+ * Return IXGBE_ERR_OVERTEMP if interrupt is high temperature
+ * failure alarm, else return PHY access status.
+ **/
+static s32 ixgbe_handle_lasi_ext_t_x550em(struct ixgbe_hw *hw)
+{
+	struct ixgbe_phy_info *phy = &hw->phy;
+	bool lsc;
+	u32 status;
+
+	status = ixgbe_get_lasi_ext_t_x550em(hw, &lsc);
 	if (status)
 		return status;
 
-	/* Toggle port SW reset by AN reset. */
+	if (lsc)
+		return phy->ops.setup_internal_link(hw);
+
+	return 0;
+}
+
+/**
+ * ixgbe_setup_kr_speed_x550em - Configure the KR PHY for link speed.
+ * @hw: pointer to hardware structure
+ * @speed: link speed
+ *
+ * Configures the integrated KR PHY.
+ **/
+static s32 ixgbe_setup_kr_speed_x550em(struct ixgbe_hw *hw,
+				       ixgbe_link_speed speed)
+{
+	s32 status;
+	u32 reg_val;
+
 	status = ixgbe_read_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
+					IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
+					IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
 	if (status)
 		return status;
 
+	reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_ENABLE;
+	reg_val &= ~(IXGBE_KRM_LINK_CTRL_1_TETH_AN_FEC_REQ |
+		     IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_FEC);
+	reg_val &= ~(IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_KR |
+		     IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_KX);
+
+	/* Advertise 10G support. */
+	if (speed & IXGBE_LINK_SPEED_10GB_FULL)
+		reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_KR;
+
+	/* Advertise 1G support. */
+	if (speed & IXGBE_LINK_SPEED_1GB_FULL)
+		reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_KX;
+
+	/* Restart auto-negotiation. */
 	reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_RESTART;
 	status = ixgbe_write_iosf_sb_reg_x550(hw,
-				IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
-				IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+					IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
+					IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
 
 	return status;
 }
@@ -990,85 +1333,82 @@ static s32 ixgbe_setup_kx4_x550em(struct ixgbe_hw *hw)
  **/
 static s32 ixgbe_setup_kr_x550em(struct ixgbe_hw *hw)
 {
-	s32 status;
-	u32 reg_val;
+	return ixgbe_setup_kr_speed_x550em(hw, hw->phy.autoneg_advertised);
+}
 
-	status = ixgbe_read_iosf_sb_reg_x550(hw,
-					IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
-					IXGBE_SB_IOSF_TARGET_KR_PHY, &reg_val);
-	if (status)
-		return status;
+/** ixgbe_ext_phy_t_x550em_get_link - Get ext phy link status
+ *  @hw: address of hardware structure
+ *  @link_up: address of boolean to indicate link status
+ *
+ *  Returns error code if unable to get link status.
+ **/
+static s32 ixgbe_ext_phy_t_x550em_get_link(struct ixgbe_hw *hw, bool *link_up)
+{
+	u32 ret;
+	u16 autoneg_status;
 
-	reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_ENABLE;
-	reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_FEC_REQ;
-	reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_FEC;
-	reg_val &= ~(IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_KR |
-		     IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_KX);
+	*link_up = false;
 
-	/* Advertise 10G support. */
-	if (hw->phy.autoneg_advertised & IXGBE_LINK_SPEED_10GB_FULL)
-		reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_KR;
+	/* read this twice back to back to indicate current status */
+	ret = hw->phy.ops.read_reg(hw, IXGBE_MDIO_AUTO_NEG_STATUS,
+				   IXGBE_MDIO_AUTO_NEG_DEV_TYPE,
+				   &autoneg_status);
+	if (ret)
+		return ret;
 
-	/* Advertise 1G support. */
-	if (hw->phy.autoneg_advertised & IXGBE_LINK_SPEED_1GB_FULL)
-		reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_CAP_KX;
+	ret = hw->phy.ops.read_reg(hw, IXGBE_MDIO_AUTO_NEG_STATUS,
+				   IXGBE_MDIO_AUTO_NEG_DEV_TYPE,
+				   &autoneg_status);
+	if (ret)
+		return ret;
 
-	/* Restart auto-negotiation. */
-	reg_val |= IXGBE_KRM_LINK_CTRL_1_TETH_AN_RESTART;
-	status = ixgbe_write_iosf_sb_reg_x550(hw,
-					IXGBE_KRM_LINK_CTRL_1(hw->bus.lan_id),
-					IXGBE_SB_IOSF_TARGET_KR_PHY, reg_val);
+	*link_up = !!(autoneg_status & IXGBE_MDIO_AUTO_NEG_LINK_STATUS);
 
-	return status;
+	return 0;
 }
 
-/** ixgbe_setup_internal_phy_x550em - Configure integrated KR PHY
+/** ixgbe_setup_internal_phy_t_x550em - Configure KR PHY to X557 link
  *  @hw: point to hardware structure
  *
- *  Configures the integrated KR PHY to talk to the external PHY. The base
- *  driver will call this function when it gets notification via interrupt from
- *  the external PHY. This function forces the internal PHY into iXFI mode at
- *  the correct speed.
+ *  Configures the link between the integrated KR PHY and the external X557 PHY
+ *  The driver will call this function when it gets a link status change
+ *  interrupt from the X557 PHY. This function configures the link speed
+ *  between the PHYs to match the link speed of the BASE-T link.
  *
- *  A return of a non-zero value indicates an error, and the base driver should
- *  not report link up.
+ * A return of a non-zero value indicates an error, and the base driver should
+ * not report link up.
  **/
-static s32 ixgbe_setup_internal_phy_x550em(struct ixgbe_hw *hw)
+static s32 ixgbe_setup_internal_phy_t_x550em(struct ixgbe_hw *hw)
 {
-	u32 status;
-	u16 lasi, autoneg_status, speed;
 	ixgbe_link_speed force_speed;
+	bool link_up;
+	u32 status;
+	u16 speed;
+
+	if (hw->mac.ops.get_media_type(hw) != ixgbe_media_type_copper)
+		return IXGBE_ERR_CONFIG;
 
-	/* Verify that the external link status has changed */
-	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_XENPAK_LASI_STATUS,
-				      IXGBE_MDIO_PMA_PMD_DEV_TYPE, &lasi);
+	/* If link is not up, then there is no setup necessary so return  */
+	status = ixgbe_ext_phy_t_x550em_get_link(hw, &link_up);
 	if (status)
 		return status;
 
-	/* If there was no change in link status, we can just exit */
-	if (!(lasi & IXGBE_XENPAK_LASI_LINK_STATUS_ALARM))
+	if (!link_up)
 		return 0;
 
-	/* we read this twice back to back to indicate current status */
-	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_AUTO_NEG_STATUS,
+	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_AUTO_NEG_VENDOR_STAT,
 				      IXGBE_MDIO_AUTO_NEG_DEV_TYPE,
-				      &autoneg_status);
+				      &speed);
 	if (status)
 		return status;
 
-	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_AUTO_NEG_STATUS,
-				      IXGBE_MDIO_AUTO_NEG_DEV_TYPE,
-				      &autoneg_status);
+	/* If link is not still up, then no setup is necessary so return */
+	status = ixgbe_ext_phy_t_x550em_get_link(hw, &link_up);
 	if (status)
 		return status;
 
-	/* If link is not up return an error indicating treat link as down */
-	if (!(autoneg_status & IXGBE_MDIO_AUTO_NEG_LINK_STATUS))
-		return IXGBE_ERR_INVALID_LINK_SETTINGS;
-
-	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_AUTO_NEG_VENDOR_STAT,
-				      IXGBE_MDIO_AUTO_NEG_DEV_TYPE,
-				      &speed);
+	if (!link_up)
+		return 0;
 
 	/* clear everything but the speed and duplex bits */
 	speed &= IXGBE_MDIO_AUTO_NEG_VENDOR_STATUS_MASK;
@@ -1088,6 +1428,22 @@ static s32 ixgbe_setup_internal_phy_x550em(struct ixgbe_hw *hw)
 	return ixgbe_setup_ixfi_x550em(hw, &force_speed);
 }
 
+/** ixgbe_reset_phy_t_X550em - Performs X557 PHY reset and enables LASI
+ *  @hw: pointer to hardware structure
+ **/
+static s32 ixgbe_reset_phy_t_X550em(struct ixgbe_hw *hw)
+{
+	s32 status;
+
+	status = ixgbe_reset_phy_generic(hw);
+
+	if (status)
+		return status;
+
+	/* Configure Link Status Alarm and Temperature Threshold interrupts */
+	return ixgbe_enable_lasi_ext_t_x550em(hw);
+}
+
 /** ixgbe_init_phy_ops_X550em - PHY/SFP specific init
  *  @hw: pointer to hardware structure
  *
@@ -1098,25 +1454,32 @@ static s32 ixgbe_setup_internal_phy_x550em(struct ixgbe_hw *hw)
 static s32 ixgbe_init_phy_ops_X550em(struct ixgbe_hw *hw)
 {
 	struct ixgbe_phy_info *phy = &hw->phy;
+	ixgbe_link_speed speed;
 	s32 ret_val;
-	u32 esdp;
 
-	if (hw->device_id == IXGBE_DEV_ID_X550EM_X_SFP) {
-		esdp = IXGBE_READ_REG(hw, IXGBE_ESDP);
-		phy->phy_semaphore_mask = IXGBE_GSSR_SHARED_I2C_SM;
+	hw->mac.ops.set_lan_id(hw);
 
-		if (hw->bus.lan_id) {
-			esdp &= ~(IXGBE_ESDP_SDP1_NATIVE | IXGBE_ESDP_SDP1);
-			esdp |= IXGBE_ESDP_SDP1_DIR;
+	if (hw->mac.ops.get_media_type(hw) == ixgbe_media_type_fiber) {
+		phy->phy_semaphore_mask = IXGBE_GSSR_SHARED_I2C_SM;
+		ixgbe_setup_mux_ctl(hw);
+
+		/* Save NW management interface connected on board. This is used
+		 * to determine internal PHY mode.
+		 */
+		phy->nw_mng_if_sel = IXGBE_READ_REG(hw, IXGBE_NW_MNG_IF_SEL);
+
+		/* If internal PHY mode is KR, then initialize KR link */
+		if (phy->nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE) {
+			speed = IXGBE_LINK_SPEED_10GB_FULL |
+				IXGBE_LINK_SPEED_1GB_FULL;
+			ret_val = ixgbe_setup_kr_speed_x550em(hw, speed);
 		}
-		esdp &= ~(IXGBE_ESDP_SDP0_NATIVE | IXGBE_ESDP_SDP0_DIR);
-		IXGBE_WRITE_REG(hw, IXGBE_ESDP, esdp);
 	}
 
 	/* Identify the PHY or SFP module */
 	ret_val = phy->ops.identify(hw);
 
-	/* Setup function pointers based on detected SFP module and speeds */
+	/* Setup function pointers based on detected hardware */
 	ixgbe_init_mac_link_ops_X550em(hw);
 	if (phy->sfp_type != ixgbe_sfp_type_unknown)
 		phy->ops.reset = NULL;
@@ -1134,11 +1497,30 @@ static s32 ixgbe_init_phy_ops_X550em(struct ixgbe_hw *hw)
 		phy->ops.write_reg = ixgbe_write_phy_reg_x550em;
 		break;
 	case ixgbe_phy_x550em_ext_t:
-		phy->ops.setup_internal_link = ixgbe_setup_internal_phy_x550em;
+		/* Save NW management interface connected on board. This is used
+		 * to determine internal PHY mode
+		 */
+		phy->nw_mng_if_sel = IXGBE_READ_REG(hw, IXGBE_NW_MNG_IF_SEL);
+
+		/* If internal link mode is XFI, then setup iXFI internal link,
+		 * else setup KR now.
+		 */
+		if (!(phy->nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE)) {
+			phy->ops.setup_internal_link =
+					ixgbe_setup_internal_phy_t_x550em;
+		} else {
+			speed = IXGBE_LINK_SPEED_10GB_FULL |
+				IXGBE_LINK_SPEED_1GB_FULL;
+			ret_val = ixgbe_setup_kr_speed_x550em(hw, speed);
+		}
+
+		phy->ops.handle_lasi = ixgbe_handle_lasi_ext_t_x550em;
+		phy->ops.reset = ixgbe_reset_phy_t_X550em;
 		break;
 	default:
 		break;
 	}
+
 	return ret_val;
 }
 
@@ -1177,67 +1559,37 @@ static enum ixgbe_media_type ixgbe_get_media_type_X550em(struct ixgbe_hw *hw)
  **/
 static s32 ixgbe_init_ext_t_x550em(struct ixgbe_hw *hw)
 {
-	u32 status;
+	s32 status;
 	u16 reg;
-	u32 retries = 2;
-
-	do {
-		/* decrement retries counter and exit if we hit 0 */
-		if (retries < 1) {
-			hw_dbg(hw, "External PHY not yet finished resetting.");
-			return IXGBE_ERR_PHY;
-		}
-		retries--;
-
-		status = hw->phy.ops.read_reg(hw,
-					      IXGBE_MDIO_TX_VENDOR_ALARMS_3,
-					      IXGBE_MDIO_PMA_PMD_DEV_TYPE,
-					      &reg);
-		if (status)
-			return status;
 
-		/* Verify PHY FW reset has completed */
-	} while ((reg & IXGBE_MDIO_TX_VENDOR_ALARMS_3_RST_MASK) != 1);
-
-	/* Set port to low power mode */
 	status = hw->phy.ops.read_reg(hw,
-				      IXGBE_MDIO_VENDOR_SPECIFIC_1_CONTROL,
-				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
-				      &reg);
-	if (status)
-		return status;
-
-	/* Enable the transmitter */
-	status = hw->phy.ops.read_reg(hw,
-				      IXGBE_MDIO_PMD_STD_TX_DISABLE_CNTR,
+				      IXGBE_MDIO_TX_VENDOR_ALARMS_3,
 				      IXGBE_MDIO_PMA_PMD_DEV_TYPE,
 				      &reg);
 	if (status)
 		return status;
 
-	reg &= ~IXGBE_MDIO_PMD_GLOBAL_TX_DISABLE;
+	/* If PHY FW reset completed bit is set then this is the first
+	 * SW instance after a power on so the PHY FW must be un-stalled.
+	 */
+	if (reg & IXGBE_MDIO_TX_VENDOR_ALARMS_3_RST_MASK) {
+		status = hw->phy.ops.read_reg(hw,
+					IXGBE_MDIO_GLOBAL_RES_PR_10,
+					IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+					&reg);
+		if (status)
+			return status;
 
-	status = hw->phy.ops.write_reg(hw,
-				       IXGBE_MDIO_PMD_STD_TX_DISABLE_CNTR,
-				       IXGBE_MDIO_PMA_PMD_DEV_TYPE,
-				       reg);
-	if (status)
-		return status;
+		reg &= ~IXGBE_MDIO_POWER_UP_STALL;
 
-	/* Un-stall the PHY FW */
-	status = hw->phy.ops.read_reg(hw,
-				      IXGBE_MDIO_GLOBAL_RES_PR_10,
-				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
-				      &reg);
-	if (status)
-		return status;
-
-	reg &= ~IXGBE_MDIO_POWER_UP_STALL;
+		status = hw->phy.ops.write_reg(hw,
+					IXGBE_MDIO_GLOBAL_RES_PR_10,
+					IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+					reg);
+		if (status)
+			return status;
+	}
 
-	status = hw->phy.ops.write_reg(hw,
-				       IXGBE_MDIO_GLOBAL_RES_PR_10,
-				       IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
-				       reg);
 	return status;
 }
 
@@ -1254,6 +1606,7 @@ static s32 ixgbe_reset_hw_X550em(struct ixgbe_hw *hw)
 	s32 status;
 	u32 ctrl = 0;
 	u32 i;
+	u32 hlreg0;
 	bool link_up = false;
 
 	/* Call adapter stop to disable Tx/Rx and clear interrupts */
@@ -1338,6 +1691,15 @@ mac_reset_top:
 	hw->mac.num_rar_entries = 128;
 	hw->mac.ops.init_rx_addrs(hw);
 
+	if (hw->device_id == IXGBE_DEV_ID_X550EM_X_10G_T) {
+		hlreg0 = IXGBE_READ_REG(hw, IXGBE_HLREG0);
+		hlreg0 &= ~IXGBE_HLREG0_MDCSPD;
+		IXGBE_WRITE_REG(hw, IXGBE_HLREG0, hlreg0);
+	}
+
+	if (hw->device_id == IXGBE_DEV_ID_X550EM_X_SFP)
+		ixgbe_setup_mux_ctl(hw);
+
 	return status;
 }
 
@@ -1490,6 +1852,10 @@ static struct ixgbe_eeprom_operations eeprom_ops_X550EM_x = {
 	.read_i2c_sff8472	= &ixgbe_read_i2c_sff8472_generic, \
 	.read_i2c_eeprom	= &ixgbe_read_i2c_eeprom_generic, \
 	.write_i2c_eeprom	= &ixgbe_write_i2c_eeprom_generic, \
+	.read_reg		= &ixgbe_read_phy_reg_generic, \
+	.write_reg		= &ixgbe_write_phy_reg_generic, \
+	.setup_link		= &ixgbe_setup_phy_link_generic, \
+	.set_phy_power		= &ixgbe_set_copper_phy_power, \
 	.check_overtemp		= &ixgbe_tn_check_overtemp, \
 	.get_firmware_version	= &ixgbe_get_phy_firmware_version_generic,
 
@@ -1497,9 +1863,6 @@ static struct ixgbe_phy_operations phy_ops_X550 = {
 	X550_COMMON_PHY
 	.init			= NULL,
 	.identify		= &ixgbe_identify_phy_generic,
-	.read_reg		= &ixgbe_read_phy_reg_generic,
-	.write_reg		= &ixgbe_write_phy_reg_generic,
-	.setup_link		= &ixgbe_setup_phy_link_generic,
 	.read_i2c_combined	= &ixgbe_read_i2c_combined_generic,
 	.write_i2c_combined	= &ixgbe_write_i2c_combined_generic,
 };
@@ -1508,9 +1871,14 @@ static struct ixgbe_phy_operations phy_ops_X550EM_x = {
 	X550_COMMON_PHY
 	.init			= &ixgbe_init_phy_ops_X550em,
 	.identify		= &ixgbe_identify_phy_x550em,
-	.read_reg		= NULL, /* defined later */
-	.write_reg		= NULL, /* defined later */
-	.setup_link		= NULL, /* defined later */
+};
+
+static const u32 ixgbe_mvals_X550[IXGBE_MVALS_IDX_LIMIT] = {
+	IXGBE_MVALS_INIT(X550)
+};
+
+static const u32 ixgbe_mvals_X550EM_x[IXGBE_MVALS_IDX_LIMIT] = {
+	IXGBE_MVALS_INIT(X550EM_x)
 };
 
 struct ixgbe_info ixgbe_X550_info = {
@@ -1520,6 +1888,7 @@ struct ixgbe_info ixgbe_X550_info = {
 	.eeprom_ops		= &eeprom_ops_X550,
 	.phy_ops		= &phy_ops_X550,
 	.mbx_ops		= &mbx_ops_generic,
+	.mvals			= ixgbe_mvals_X550,
 };
 
 struct ixgbe_info ixgbe_X550EM_x_info = {
@@ -1529,4 +1898,5 @@ struct ixgbe_info ixgbe_X550EM_x_info = {
 	.eeprom_ops		= &eeprom_ops_X550EM_x,
 	.phy_ops		= &phy_ops_X550EM_x,
 	.mbx_ops		= &mbx_ops_generic,
+	.mvals			= ixgbe_mvals_X550EM_x,
 };
diff --git a/drivers/net/ethernet/marvell/Kconfig b/drivers/net/ethernet/marvell/Kconfig
index d323a695dfbc..80af9ffce5ea 100644
--- a/drivers/net/ethernet/marvell/Kconfig
+++ b/drivers/net/ethernet/marvell/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_MARVELL
 	default y
 	depends on PCI || CPU_PXA168 || MV64X60 || PPC32 || PLAT_ORION || INET
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index 1c75829eb166..d52639bc491f 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -3125,9 +3125,8 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 
 	mib_counters_clear(mp);
 
-	init_timer(&mp->mib_counters_timer);
-	mp->mib_counters_timer.data = (unsigned long)mp;
-	mp->mib_counters_timer.function = mib_counters_timer_wrapper;
+	setup_timer(&mp->mib_counters_timer, mib_counters_timer_wrapper,
+		    (unsigned long)mp);
 	mp->mib_counters_timer.expires = jiffies + 30 * HZ;
 
 	spin_lock_init(&mp->mib_counters_lock);
@@ -3136,9 +3135,7 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 
 	netif_napi_add(dev, &mp->napi, mv643xx_eth_poll, NAPI_POLL_WEIGHT);
 
-	init_timer(&mp->rx_oom);
-	mp->rx_oom.data = (unsigned long)mp;
-	mp->rx_oom.function = oom_timer_wrapper;
+	setup_timer(&mp->rx_oom, oom_timer_wrapper, (unsigned long)mp);
 
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index ce5f7f9cff06..370e20ed224c 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -310,6 +310,7 @@ struct mvneta_port {
 	unsigned int link;
 	unsigned int duplex;
 	unsigned int speed;
+	unsigned int tx_csum_limit;
 	int use_inband_status:1;
 };
 
@@ -1013,6 +1014,12 @@ static void mvneta_defaults_set(struct mvneta_port *pp)
 		val = mvreg_read(pp, MVNETA_GMAC_CLOCK_DIVIDER);
 		val |= MVNETA_GMAC_1MS_CLOCK_ENABLE;
 		mvreg_write(pp, MVNETA_GMAC_CLOCK_DIVIDER, val);
+	} else {
+		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
+		val &= ~(MVNETA_GMAC_INBAND_AN_ENABLE |
+		       MVNETA_GMAC_AN_SPEED_EN |
+		       MVNETA_GMAC_AN_DUPLEX_EN);
+		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
 	}
 
 	mvneta_set_ucast_table(pp, -1);
@@ -1359,7 +1366,7 @@ static void *mvneta_frag_alloc(const struct mvneta_port *pp)
 static void mvneta_frag_free(const struct mvneta_port *pp, void *data)
 {
 	if (likely(pp->frag_size <= PAGE_SIZE))
-		put_page(virt_to_head_page(data));
+		skb_free_frag(data);
 	else
 		kfree(data);
 }
@@ -2502,8 +2509,10 @@ static int mvneta_change_mtu(struct net_device *dev, int mtu)
 
 	dev->mtu = mtu;
 
-	if (!netif_running(dev))
+	if (!netif_running(dev)) {
+		netdev_update_features(dev);
 		return 0;
+	}
 
 	/* The interface is running, so we have to force a
 	 * reallocation of the queues
@@ -2532,9 +2541,26 @@ static int mvneta_change_mtu(struct net_device *dev, int mtu)
 	mvneta_start_dev(pp);
 	mvneta_port_up(pp);
 
+	netdev_update_features(dev);
+
 	return 0;
 }
 
+static netdev_features_t mvneta_fix_features(struct net_device *dev,
+					     netdev_features_t features)
+{
+	struct mvneta_port *pp = netdev_priv(dev);
+
+	if (pp->tx_csum_limit && dev->mtu > pp->tx_csum_limit) {
+		features &= ~(NETIF_F_IP_CSUM | NETIF_F_TSO);
+		netdev_info(dev,
+			    "Disable IP checksum for MTU greater than %dB\n",
+			    pp->tx_csum_limit);
+	}
+
+	return features;
+}
+
 /* Get mac address */
 static void mvneta_get_mac_addr(struct mvneta_port *pp, unsigned char *addr)
 {
@@ -2856,6 +2882,7 @@ static const struct net_device_ops mvneta_netdev_ops = {
 	.ndo_set_rx_mode     = mvneta_set_rx_mode,
 	.ndo_set_mac_address = mvneta_set_mac_addr,
 	.ndo_change_mtu      = mvneta_change_mtu,
+	.ndo_fix_features    = mvneta_fix_features,
 	.ndo_get_stats64     = mvneta_get_stats64,
 	.ndo_do_ioctl        = mvneta_ioctl,
 };
@@ -3101,6 +3128,9 @@ static int mvneta_probe(struct platform_device *pdev)
 		}
 	}
 
+	if (of_device_is_compatible(dn, "marvell,armada-370-neta"))
+		pp->tx_csum_limit = 1600;
+
 	pp->tx_ring_size = MVNETA_MAX_TXD;
 	pp->rx_ring_size = MVNETA_MAX_RXD;
 
@@ -3179,6 +3209,7 @@ static int mvneta_remove(struct platform_device *pdev)
 
 static const struct of_device_id mvneta_match[] = {
 	{ .compatible = "marvell,armada-370-neta" },
+	{ .compatible = "marvell,armada-xp-neta" },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, mvneta_match);
diff --git a/drivers/net/ethernet/mellanox/Kconfig b/drivers/net/ethernet/mellanox/Kconfig
index 8cf7563a8d92..52a6665b7abf 100644
--- a/drivers/net/ethernet/mellanox/Kconfig
+++ b/drivers/net/ethernet/mellanox/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_MELLANOX
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index 529ef0594b90..82040137d7d9 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -49,6 +49,7 @@
 #include "mlx4.h"
 #include "fw.h"
 #include "fw_qos.h"
+#include "mlx4_stats.h"
 
 #define CMD_POLL_TOKEN 0xffff
 #define INBOX_MASK	0xffffffffffffff00ULL
@@ -882,7 +883,7 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 {
 	struct ib_smp *smp = inbox->buf;
 	u32 index;
-	u8 port;
+	u8 port, slave_port;
 	u8 opcode_modifier;
 	u16 *table;
 	int err;
@@ -894,7 +895,8 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 	__be32 slave_cap_mask;
 	__be64 slave_node_guid;
 
-	port = vhcr->in_modifier;
+	slave_port = vhcr->in_modifier;
+	port = mlx4_slave_convert_port(dev, slave, slave_port);
 
 	/* network-view bit is for driver use only, and should not be passed to FW */
 	opcode_modifier = vhcr->op_modifier & ~0x8; /* clear netw view bit */
@@ -930,8 +932,9 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 			if (smp->attr_id == IB_SMP_ATTR_PORT_INFO) {
 				/*get the slave specific caps:*/
 				/*do the command */
+				smp->attr_mod = cpu_to_be32(port);
 				err = mlx4_cmd_box(dev, inbox->dma, outbox->dma,
-					    vhcr->in_modifier, opcode_modifier,
+					    port, opcode_modifier,
 					    vhcr->op, MLX4_CMD_TIME_CLASS_C, MLX4_CMD_NATIVE);
 				/* modify the response for slaves */
 				if (!err && slave != mlx4_master_func_num(dev)) {
@@ -975,7 +978,7 @@ static int mlx4_MAD_IFC_wrapper(struct mlx4_dev *dev, int slave,
 			}
 			if (smp->attr_id == IB_SMP_ATTR_NODE_INFO) {
 				err = mlx4_cmd_box(dev, inbox->dma, outbox->dma,
-					     vhcr->in_modifier, opcode_modifier,
+					     port, opcode_modifier,
 					     vhcr->op, MLX4_CMD_TIME_CLASS_C, MLX4_CMD_NATIVE);
 				if (!err) {
 					slave_node_guid =  mlx4_get_slave_node_guid(dev, slave);
@@ -2915,7 +2918,7 @@ int mlx4_set_vf_mac(struct mlx4_dev *dev, int port, int vf, u64 mac)
 	port = mlx4_slaves_closest_port(dev, slave, port);
 	s_info = &priv->mfunc.master.vf_admin[slave].vport[port];
 	s_info->mac = mac;
-	mlx4_info(dev, "default mac on vf %d port %d to %llX will take afect only after vf restart\n",
+	mlx4_info(dev, "default mac on vf %d port %d to %llX will take effect only after vf restart\n",
 		  vf, port, s_info->mac);
 	return 0;
 }
@@ -3164,6 +3167,92 @@ int mlx4_set_vf_link_state(struct mlx4_dev *dev, int port, int vf, int link_stat
 }
 EXPORT_SYMBOL_GPL(mlx4_set_vf_link_state);
 
+int mlx4_get_counter_stats(struct mlx4_dev *dev, int counter_index,
+			   struct mlx4_counter *counter_stats, int reset)
+{
+	struct mlx4_cmd_mailbox *mailbox = NULL;
+	struct mlx4_counter *tmp_counter;
+	int err;
+	u32 if_stat_in_mod;
+
+	if (!counter_stats)
+		return -EINVAL;
+
+	if (counter_index == MLX4_SINK_COUNTER_INDEX(dev))
+		return 0;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	memset(mailbox->buf, 0, sizeof(struct mlx4_counter));
+	if_stat_in_mod = counter_index;
+	if (reset)
+		if_stat_in_mod |= MLX4_QUERY_IF_STAT_RESET;
+	err = mlx4_cmd_box(dev, 0, mailbox->dma,
+			   if_stat_in_mod, 0,
+			   MLX4_CMD_QUERY_IF_STAT,
+			   MLX4_CMD_TIME_CLASS_C,
+			   MLX4_CMD_NATIVE);
+	if (err) {
+		mlx4_dbg(dev, "%s: failed to read statistics for counter index %d\n",
+			 __func__, counter_index);
+		goto if_stat_out;
+	}
+	tmp_counter = (struct mlx4_counter *)mailbox->buf;
+	counter_stats->counter_mode = tmp_counter->counter_mode;
+	if (counter_stats->counter_mode == 0) {
+		counter_stats->rx_frames =
+			cpu_to_be64(be64_to_cpu(counter_stats->rx_frames) +
+				    be64_to_cpu(tmp_counter->rx_frames));
+		counter_stats->tx_frames =
+			cpu_to_be64(be64_to_cpu(counter_stats->tx_frames) +
+				    be64_to_cpu(tmp_counter->tx_frames));
+		counter_stats->rx_bytes =
+			cpu_to_be64(be64_to_cpu(counter_stats->rx_bytes) +
+				    be64_to_cpu(tmp_counter->rx_bytes));
+		counter_stats->tx_bytes =
+			cpu_to_be64(be64_to_cpu(counter_stats->tx_bytes) +
+				    be64_to_cpu(tmp_counter->tx_bytes));
+	}
+
+if_stat_out:
+	mlx4_free_cmd_mailbox(dev, mailbox);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx4_get_counter_stats);
+
+int mlx4_get_vf_stats(struct mlx4_dev *dev, int port, int vf_idx,
+		      struct ifla_vf_stats *vf_stats)
+{
+	struct mlx4_counter tmp_vf_stats;
+	int slave;
+	int err = 0;
+
+	if (!vf_stats)
+		return -EINVAL;
+
+	if (!mlx4_is_master(dev))
+		return -EPROTONOSUPPORT;
+
+	slave = mlx4_get_slave_indx(dev, vf_idx);
+	if (slave < 0)
+		return -EINVAL;
+
+	port = mlx4_slaves_closest_port(dev, slave, port);
+	err = mlx4_calc_vf_counters(dev, slave, port, &tmp_vf_stats);
+	if (!err && tmp_vf_stats.counter_mode == 0) {
+		vf_stats->rx_packets = be64_to_cpu(tmp_vf_stats.rx_frames);
+		vf_stats->tx_packets = be64_to_cpu(tmp_vf_stats.tx_frames);
+		vf_stats->rx_bytes = be64_to_cpu(tmp_vf_stats.rx_bytes);
+		vf_stats->tx_bytes = be64_to_cpu(tmp_vf_stats.tx_bytes);
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx4_get_vf_stats);
+
 int mlx4_vf_smi_enabled(struct mlx4_dev *dev, int slave, int port)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -3197,6 +3286,12 @@ int mlx4_vf_set_enable_smi_admin(struct mlx4_dev *dev, int slave, int port,
 				 int enabled)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_active_ports actv_ports = mlx4_get_active_ports(
+			&priv->dev, slave);
+	int min_port = find_first_bit(actv_ports.ports,
+				      priv->dev.caps.num_ports) + 1;
+	int max_port = min_port - 1 +
+		bitmap_weight(actv_ports.ports, priv->dev.caps.num_ports);
 
 	if (slave == mlx4_master_func_num(dev))
 		return 0;
@@ -3206,6 +3301,11 @@ int mlx4_vf_set_enable_smi_admin(struct mlx4_dev *dev, int slave, int port,
 	    enabled < 0 || enabled > 1)
 		return -EINVAL;
 
+	if (min_port == max_port && dev->caps.num_ports > 1) {
+		mlx4_info(dev, "SMI access disallowed for single ported VFs\n");
+		return -EPROTONOSUPPORT;
+	}
+
 	priv->mfunc.master.vf_admin[slave].enable_smi[port] = enabled;
 	return 0;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx4/cq.c b/drivers/net/ethernet/mellanox/mlx4/cq.c
index e71f31387ac6..3348e646db70 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cq.c
@@ -292,7 +292,7 @@ int mlx4_cq_alloc(struct mlx4_dev *dev, int nent,
 	u64 mtt_addr;
 	int err;
 
-	if (vector > dev->caps.num_comp_vectors + dev->caps.comp_pool)
+	if (vector >= dev->caps.num_comp_vectors)
 		return -EINVAL;
 
 	cq->vector = vector;
@@ -319,7 +319,7 @@ int mlx4_cq_alloc(struct mlx4_dev *dev, int nent,
 		cq_context->flags  |= cpu_to_be32(1 << 19);
 
 	cq_context->logsize_usrpage = cpu_to_be32((ilog2(nent) << 24) | uar->index);
-	cq_context->comp_eqn	    = priv->eq_table.eq[vector].eqn;
+	cq_context->comp_eqn	    = priv->eq_table.eq[MLX4_CQ_TO_EQ_VECTOR(vector)].eqn;
 	cq_context->log_page_size   = mtt->page_shift - MLX4_ICM_PAGE_SHIFT;
 
 	mtt_addr = mlx4_mtt_addr(dev, mtt);
@@ -339,11 +339,11 @@ int mlx4_cq_alloc(struct mlx4_dev *dev, int nent,
 	init_completion(&cq->free);
 	cq->comp = mlx4_add_cq_to_tasklet;
 	cq->tasklet_ctx.priv =
-		&priv->eq_table.eq[cq->vector].tasklet_ctx;
+		&priv->eq_table.eq[MLX4_CQ_TO_EQ_VECTOR(vector)].tasklet_ctx;
 	INIT_LIST_HEAD(&cq->tasklet_ctx.list);
 
 
-	cq->irq = priv->eq_table.eq[cq->vector].irq;
+	cq->irq = priv->eq_table.eq[MLX4_CQ_TO_EQ_VECTOR(vector)].irq;
 	return 0;
 
 err_radix:
@@ -368,7 +368,10 @@ void mlx4_cq_free(struct mlx4_dev *dev, struct mlx4_cq *cq)
 	if (err)
 		mlx4_warn(dev, "HW2SW_CQ failed (%d) for CQN %06x\n", err, cq->cqn);
 
-	synchronize_irq(priv->eq_table.eq[cq->vector].irq);
+	synchronize_irq(priv->eq_table.eq[MLX4_CQ_TO_EQ_VECTOR(cq->vector)].irq);
+	if (priv->eq_table.eq[MLX4_CQ_TO_EQ_VECTOR(cq->vector)].irq !=
+	    priv->eq_table.eq[MLX4_EQ_ASYNC].irq)
+		synchronize_irq(priv->eq_table.eq[MLX4_EQ_ASYNC].irq);
 
 	spin_lock_irq(&cq_table->lock);
 	radix_tree_delete(&cq_table->tree, cq->cqn);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_cq.c b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
index 22da4d0d0f05..63769df872a4 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
@@ -66,6 +66,7 @@ int mlx4_en_create_cq(struct mlx4_en_priv *priv,
 
 	cq->ring = ring;
 	cq->is_tx = mode;
+	cq->vector = mdev->dev->caps.num_comp_vectors;
 
 	/* Allocate HW buffers on provided NUMA node.
 	 * dev->numa_node is used in mtt range allocation flow.
@@ -101,12 +102,7 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	int err = 0;
 	char name[25];
 	int timestamp_en = 0;
-	struct cpu_rmap *rmap =
-#ifdef CONFIG_RFS_ACCEL
-		priv->dev->rx_cpu_rmap;
-#else
-		NULL;
-#endif
+	bool assigned_eq = false;
 
 	cq->dev = mdev->pndev[priv->port];
 	cq->mcq.set_ci_db  = cq->wqres.db.db;
@@ -116,23 +112,19 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	memset(cq->buf, 0, cq->buf_size);
 
 	if (cq->is_tx == RX) {
-		if (mdev->dev->caps.comp_pool) {
-			if (!cq->vector) {
-				sprintf(name, "%s-%d", priv->dev->name,
-					cq->ring);
-				/* Set IRQ for specific name (per ring) */
-				if (mlx4_assign_eq(mdev->dev, name, rmap,
-						   &cq->vector)) {
-					cq->vector = (cq->ring + 1 + priv->port)
-					    % mdev->dev->caps.num_comp_vectors;
-					mlx4_warn(mdev, "Failed assigning an EQ to %s, falling back to legacy EQ's\n",
-						  name);
-				}
-
+		if (!mlx4_is_eq_vector_valid(mdev->dev, priv->port,
+					     cq->vector)) {
+			cq->vector = cpumask_first(priv->rx_ring[cq->ring]->affinity_mask);
+
+			err = mlx4_assign_eq(mdev->dev, priv->port,
+					     &cq->vector);
+			if (err) {
+				mlx4_err(mdev, "Failed assigning an EQ to %s\n",
+					 name);
+				goto free_eq;
 			}
-		} else {
-			cq->vector = (cq->ring + 1 + priv->port) %
-				mdev->dev->caps.num_comp_vectors;
+
+			assigned_eq = true;
 		}
 
 		cq->irq_desc =
@@ -159,7 +151,7 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 			    &mdev->priv_uar, cq->wqres.db.dma, &cq->mcq,
 			    cq->vector, 0, timestamp_en);
 	if (err)
-		return err;
+		goto free_eq;
 
 	cq->mcq.comp  = cq->is_tx ? mlx4_en_tx_irq : mlx4_en_rx_irq;
 	cq->mcq.event = mlx4_en_cq_event;
@@ -168,13 +160,6 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 		netif_napi_add(cq->dev, &cq->napi, mlx4_en_poll_tx_cq,
 			       NAPI_POLL_WEIGHT);
 	} else {
-		struct mlx4_en_rx_ring *ring = priv->rx_ring[cq->ring];
-
-		err = irq_set_affinity_hint(cq->mcq.irq,
-					    ring->affinity_mask);
-		if (err)
-			mlx4_warn(mdev, "Failed setting affinity hint\n");
-
 		netif_napi_add(cq->dev, &cq->napi, mlx4_en_poll_rx_cq, 64);
 		napi_hash_add(&cq->napi);
 	}
@@ -182,6 +167,12 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	napi_enable(&cq->napi);
 
 	return 0;
+
+free_eq:
+	if (assigned_eq)
+		mlx4_release_eq(mdev->dev, cq->vector);
+	cq->vector = mdev->dev->caps.num_comp_vectors;
+	return err;
 }
 
 void mlx4_en_destroy_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq **pcq)
@@ -191,9 +182,9 @@ void mlx4_en_destroy_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq **pcq)
 
 	mlx4_en_unmap_buffer(&cq->wqres.buf);
 	mlx4_free_hwq_res(mdev->dev, &cq->wqres, cq->buf_size);
-	if (priv->mdev->dev->caps.comp_pool && cq->vector) {
+	if (mlx4_is_eq_vector_valid(mdev->dev, priv->port, cq->vector) &&
+	    cq->is_tx == RX)
 		mlx4_release_eq(priv->mdev->dev, cq->vector);
-	}
 	cq->vector = 0;
 	cq->buf_size = 0;
 	cq->buf = NULL;
@@ -207,7 +198,6 @@ void mlx4_en_deactivate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq)
 	if (!cq->is_tx) {
 		napi_hash_del(&cq->napi);
 		synchronize_rcu();
-		irq_set_affinity_hint(cq->mcq.irq, NULL);
 	}
 	netif_napi_del(&cq->napi);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
index a2ddf3d75ff8..99ba1c50e585 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
@@ -119,6 +119,12 @@ static const char main_strings[][ETH_GSTRING_LEN] = {
 	"queue_stopped", "wake_queue", "tx_timeout", "rx_alloc_failed",
 	"rx_csum_good", "rx_csum_none", "rx_csum_complete", "tx_chksum_offload",
 
+	/* pf statistics */
+	"pf_rx_packets",
+	"pf_rx_bytes",
+	"pf_tx_packets",
+	"pf_tx_bytes",
+
 	/* priority flow control statistics rx */
 	"rx_pause_prio_0", "rx_pause_duration_prio_0",
 	"rx_pause_transition_prio_0",
@@ -368,6 +374,11 @@ static void mlx4_en_get_ethtool_stats(struct net_device *dev,
 		if (bitmap_iterator_test(&it))
 			data[index++] = ((unsigned long *)&priv->port_stats)[i];
 
+	for (i = 0; i < NUM_PF_STATS; i++, bitmap_iterator_inc(&it))
+		if (bitmap_iterator_test(&it))
+			data[index++] =
+				((unsigned long *)&priv->pf_stats)[i];
+
 	for (i = 0; i < NUM_FLOW_PRIORITY_STATS_RX;
 	     i++, bitmap_iterator_inc(&it))
 		if (bitmap_iterator_test(&it))
@@ -448,6 +459,12 @@ static void mlx4_en_get_strings(struct net_device *dev,
 				strcpy(data + (index++) * ETH_GSTRING_LEN,
 				       main_strings[strings]);
 
+		for (i = 0; i < NUM_PF_STATS; i++, strings++,
+		     bitmap_iterator_inc(&it))
+			if (bitmap_iterator_test(&it))
+				strcpy(data + (index++) * ETH_GSTRING_LEN,
+				       main_strings[strings]);
+
 		for (i = 0; i < NUM_FLOW_STATS; i++, strings++,
 		     bitmap_iterator_inc(&it))
 			if (bitmap_iterator_test(&it))
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index cf467a9f6cc7..e0de2fd1ce12 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -1597,6 +1597,9 @@ int mlx4_en_start_port(struct net_device *dev)
 	}
 	mdev->mac_removed[priv->port] = 0;
 
+	priv->counter_index =
+			mlx4_get_default_counter_index(mdev->dev, priv->port);
+
 	err = mlx4_en_config_rss_steer(priv);
 	if (err) {
 		en_err(priv, "Failed configuring rss steering\n");
@@ -1755,6 +1758,7 @@ void mlx4_en_stop_port(struct net_device *dev, int detach)
 
 	/* Set port as not active */
 	priv->port_up = false;
+	priv->counter_index = MLX4_SINK_COUNTER_INDEX(mdev->dev);
 
 	/* Promsicuous mode */
 	if (mdev->dev->caps.steering_mode ==
@@ -1891,6 +1895,7 @@ static void mlx4_en_clear_stats(struct net_device *dev)
 	       sizeof(priv->rx_priority_flowstats));
 	memset(&priv->tx_priority_flowstats, 0,
 	       sizeof(priv->tx_priority_flowstats));
+	memset(&priv->pf_stats, 0, sizeof(priv->pf_stats));
 
 	for (i = 0; i < priv->tx_ring_num; i++) {
 		priv->tx_ring[i]->bytes = 0;
@@ -1954,7 +1959,6 @@ void mlx4_en_free_resources(struct mlx4_en_priv *priv)
 	int i;
 
 #ifdef CONFIG_RFS_ACCEL
-	free_irq_cpu_rmap(priv->dev->rx_cpu_rmap);
 	priv->dev->rx_cpu_rmap = NULL;
 #endif
 
@@ -1973,10 +1977,6 @@ void mlx4_en_free_resources(struct mlx4_en_priv *priv)
 			mlx4_en_destroy_cq(priv, &priv->rx_cq[i]);
 	}
 
-	if (priv->base_tx_qpn) {
-		mlx4_qp_release_range(priv->mdev->dev, priv->base_tx_qpn, priv->tx_ring_num);
-		priv->base_tx_qpn = 0;
-	}
 }
 
 int mlx4_en_alloc_resources(struct mlx4_en_priv *priv)
@@ -2012,11 +2012,7 @@ int mlx4_en_alloc_resources(struct mlx4_en_priv *priv)
 	}
 
 #ifdef CONFIG_RFS_ACCEL
-	if (priv->mdev->dev->caps.comp_pool) {
-		priv->dev->rx_cpu_rmap = alloc_irq_cpu_rmap(priv->mdev->dev->caps.comp_pool);
-		if (!priv->dev->rx_cpu_rmap)
-			goto err;
-	}
+	priv->dev->rx_cpu_rmap = mlx4_get_cpu_rmap(priv->mdev->dev, priv->port);
 #endif
 
 	return 0;
@@ -2292,6 +2288,15 @@ static int mlx4_en_set_vf_link_state(struct net_device *dev, int vf, int link_st
 	return mlx4_set_vf_link_state(mdev->dev, en_priv->port, vf, link_state);
 }
 
+static int mlx4_en_get_vf_stats(struct net_device *dev, int vf,
+				struct ifla_vf_stats *vf_stats)
+{
+	struct mlx4_en_priv *en_priv = netdev_priv(dev);
+	struct mlx4_en_dev *mdev = en_priv->mdev;
+
+	return mlx4_get_vf_stats(mdev->dev, en_priv->port, vf, vf_stats);
+}
+
 #define PORT_ID_BYTE_LEN 8
 static int mlx4_en_get_phys_port_id(struct net_device *dev,
 				    struct netdev_phys_item_id *ppid)
@@ -2489,6 +2494,7 @@ static const struct net_device_ops mlx4_netdev_ops_master = {
 	.ndo_set_vf_rate	= mlx4_en_set_vf_rate,
 	.ndo_set_vf_spoofchk	= mlx4_en_set_vf_spoofchk,
 	.ndo_set_vf_link_state	= mlx4_en_set_vf_link_state,
+	.ndo_get_vf_stats       = mlx4_en_get_vf_stats,
 	.ndo_get_vf_config	= mlx4_en_get_vf_config,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= mlx4_en_netpoll,
@@ -2686,7 +2692,7 @@ void mlx4_en_update_pfc_stats_bitmap(struct mlx4_dev *dev,
 				     u8 rx_ppp, u8 rx_pause,
 				     u8 tx_ppp, u8 tx_pause)
 {
-	int last_i = NUM_MAIN_STATS + NUM_PORT_STATS;
+	int last_i = NUM_MAIN_STATS + NUM_PORT_STATS + NUM_PF_STATS;
 
 	if (!mlx4_is_slave(dev) &&
 	    (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_FLOWSTATS_EN)) {
@@ -2748,6 +2754,11 @@ void mlx4_en_set_stats_bitmap(struct mlx4_dev *dev,
 	bitmap_set(stats_bitmap->bitmap, last_i, NUM_PORT_STATS);
 	last_i += NUM_PORT_STATS;
 
+	if (mlx4_is_master(dev))
+		bitmap_set(stats_bitmap->bitmap, last_i,
+			   NUM_PF_STATS);
+	last_i += NUM_PF_STATS;
+
 	mlx4_en_update_pfc_stats_bitmap(dev, stats_bitmap,
 					rx_ppp, rx_pause,
 					tx_ppp, tx_pause);
@@ -2783,6 +2794,7 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 
 	priv = netdev_priv(dev);
 	memset(priv, 0, sizeof(struct mlx4_en_priv));
+	priv->counter_index = MLX4_SINK_COUNTER_INDEX(mdev->dev);
 	spin_lock_init(&priv->stats_lock);
 	INIT_WORK(&priv->rx_mode_task, mlx4_en_do_set_rx_mode);
 	INIT_WORK(&priv->watchdog_task, mlx4_en_restart);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_port.c b/drivers/net/ethernet/mellanox/mlx4/en_port.c
index 0a56f010c846..ee99e67187f5 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_port.c
@@ -149,6 +149,7 @@ static unsigned long en_stats_adder(__be64 *start, __be64 *next, int num)
 
 int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 {
+	struct mlx4_counter tmp_counter_stats;
 	struct mlx4_en_stat_out_mbox *mlx4_en_stats;
 	struct mlx4_en_stat_out_flow_control_mbox *flowstats;
 	struct mlx4_en_priv *priv = netdev_priv(mdev->pndev[port]);
@@ -156,7 +157,7 @@ int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 	struct mlx4_cmd_mailbox *mailbox;
 	u64 in_mod = reset << 8 | port;
 	int err;
-	int i;
+	int i, counter_index;
 
 	mailbox = mlx4_alloc_cmd_mailbox(mdev->dev);
 	if (IS_ERR(mailbox))
@@ -202,6 +203,20 @@ int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 		priv->port_stats.tso_packets       += ring->tso_packets;
 		priv->port_stats.xmit_more         += ring->xmit_more;
 	}
+	if (mlx4_is_master(mdev->dev)) {
+		stats->rx_packets = en_stats_adder(&mlx4_en_stats->RTOT_prio_0,
+						   &mlx4_en_stats->RTOT_prio_1,
+						   NUM_PRIORITIES);
+		stats->tx_packets = en_stats_adder(&mlx4_en_stats->TTOT_prio_0,
+						   &mlx4_en_stats->TTOT_prio_1,
+						   NUM_PRIORITIES);
+		stats->rx_bytes = en_stats_adder(&mlx4_en_stats->ROCT_prio_0,
+						 &mlx4_en_stats->ROCT_prio_1,
+						 NUM_PRIORITIES);
+		stats->tx_bytes = en_stats_adder(&mlx4_en_stats->TOCT_prio_0,
+						 &mlx4_en_stats->TOCT_prio_1,
+						 NUM_PRIORITIES);
+	}
 
 	/* net device stats */
 	stats->rx_errors = be64_to_cpu(mlx4_en_stats->PCS) +
@@ -296,6 +311,11 @@ int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 
 	spin_unlock_bh(&priv->stats_lock);
 
+	memset(&tmp_counter_stats, 0, sizeof(tmp_counter_stats));
+	counter_index = mlx4_get_default_counter_index(mdev->dev, port);
+	err = mlx4_get_counter_stats(mdev->dev, counter_index,
+				     &tmp_counter_stats, reset);
+
 	/* 0xffs indicates invalid value */
 	memset(mailbox->buf, 0xff, sizeof(*flowstats) * MLX4_NUM_PRIORITIES);
 
@@ -314,6 +334,13 @@ int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 
 	spin_lock_bh(&priv->stats_lock);
 
+	if (tmp_counter_stats.counter_mode == 0) {
+		priv->pf_stats.rx_bytes   = be64_to_cpu(tmp_counter_stats.rx_bytes);
+		priv->pf_stats.tx_bytes   = be64_to_cpu(tmp_counter_stats.tx_bytes);
+		priv->pf_stats.rx_packets = be64_to_cpu(tmp_counter_stats.rx_frames);
+		priv->pf_stats.tx_packets = be64_to_cpu(tmp_counter_stats.tx_frames);
+	}
+
 	for (i = 0; i < MLX4_NUM_PRIORITIES; i++)	{
 		priv->rx_priority_flowstats[i].rx_pause =
 			be64_to_cpu(flowstats[i].rx_pause);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_resources.c b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
index 34f2fdf4fe5d..e482fa1bb741 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_resources.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
@@ -66,7 +66,7 @@ void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
 		context->pri_path.sched_queue |= user_prio << 3;
 		context->pri_path.feup = MLX4_FEUP_FORCE_ETH_UP;
 	}
-	context->pri_path.counter_index = 0xff;
+	context->pri_path.counter_index = priv->counter_index;
 	context->cqn_send = cpu_to_be32(cqn);
 	context->cqn_recv = cpu_to_be32(cqn);
 	context->db_rec_addr = cpu_to_be64(priv->res.db.dma << 2);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index 2a77a6b19121..7a4f20bb7fcb 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -337,15 +337,10 @@ void mlx4_en_set_num_rx_rings(struct mlx4_en_dev *mdev)
 	struct mlx4_dev *dev = mdev->dev;
 
 	mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_ETH) {
-		if (!dev->caps.comp_pool)
-			num_of_eqs = max_t(int, MIN_RX_RINGS,
-					   min_t(int,
-						 dev->caps.num_comp_vectors,
-						 DEF_RX_RINGS));
-		else
-			num_of_eqs = min_t(int, MAX_MSIX_P_PORT,
-					   dev->caps.comp_pool/
-					   dev->caps.num_ports) - 1;
+		num_of_eqs = max_t(int, MIN_RX_RINGS,
+				   min_t(int,
+					 mlx4_get_eqs_per_port(mdev->dev, i),
+					 DEF_RX_RINGS));
 
 		num_rx_rings = mlx4_low_memory_profile() ? MIN_RX_RINGS :
 			min_t(int, num_of_eqs,
@@ -723,7 +718,7 @@ static int get_fixed_ipv6_csum(__wsum hw_checksum, struct sk_buff *skb,
 }
 #endif
 static int check_csum(struct mlx4_cqe *cqe, struct sk_buff *skb, void *va,
-		      int hwtstamp_rx_filter)
+		      netdev_features_t dev_features)
 {
 	__wsum hw_checksum = 0;
 
@@ -731,14 +726,8 @@ static int check_csum(struct mlx4_cqe *cqe, struct sk_buff *skb, void *va,
 
 	hw_checksum = csum_unfold((__force __sum16)cqe->checksum);
 
-	if (((struct ethhdr *)va)->h_proto == htons(ETH_P_8021Q) &&
-	    hwtstamp_rx_filter != HWTSTAMP_FILTER_NONE) {
-		/* next protocol non IPv4 or IPv6 */
-		if (((struct vlan_hdr *)hdr)->h_vlan_encapsulated_proto
-		    != htons(ETH_P_IP) &&
-		    ((struct vlan_hdr *)hdr)->h_vlan_encapsulated_proto
-		    != htons(ETH_P_IPV6))
-			return -1;
+	if (cqe->vlan_my_qpn & cpu_to_be32(MLX4_CQE_VLAN_PRESENT_MASK) &&
+	    !(dev_features & NETIF_F_HW_VLAN_CTAG_RX)) {
 		hw_checksum = get_fixed_vlan_csum(hw_checksum, hdr);
 		hdr += sizeof(struct vlan_hdr);
 	}
@@ -901,7 +890,8 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 
 			if (ip_summed == CHECKSUM_COMPLETE) {
 				void *va = skb_frag_address(skb_shinfo(gro_skb)->frags);
-				if (check_csum(cqe, gro_skb, va, ring->hwtstamp_rx_filter)) {
+				if (check_csum(cqe, gro_skb, va,
+					       dev->features)) {
 					ip_summed = CHECKSUM_NONE;
 					ring->csum_none++;
 					ring->csum_complete--;
@@ -956,7 +946,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 		}
 
 		if (ip_summed == CHECKSUM_COMPLETE) {
-			if (check_csum(cqe, skb, skb->data, ring->hwtstamp_rx_filter)) {
+			if (check_csum(cqe, skb, skb->data, dev->features)) {
 				ip_summed = CHECKSUM_NONE;
 				ring->csum_complete--;
 				ring->csum_none++;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index 7bed3a88579f..c10d98f6ad96 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -66,6 +66,7 @@ int mlx4_en_create_tx_ring(struct mlx4_en_priv *priv,
 	ring->size = size;
 	ring->size_mask = size - 1;
 	ring->stride = stride;
+	ring->full_size = ring->size - HEADROOM - MAX_DESC_TXBBS;
 
 	tmp = size * sizeof(struct mlx4_en_tx_info);
 	ring->tx_info = kmalloc_node(tmp, GFP_KERNEL | __GFP_NOWARN, node);
@@ -180,6 +181,7 @@ void mlx4_en_destroy_tx_ring(struct mlx4_en_priv *priv,
 		mlx4_bf_free(mdev->dev, &ring->bf);
 	mlx4_qp_remove(mdev->dev, &ring->qp);
 	mlx4_qp_free(mdev->dev, &ring->qp);
+	mlx4_qp_release_range(priv->mdev->dev, ring->qpn, 1);
 	mlx4_en_unmap_buffer(&ring->wqres.buf);
 	mlx4_free_hwq_res(mdev->dev, &ring->wqres, ring->buf_size);
 	kfree(ring->bounce_buf);
@@ -231,6 +233,11 @@ void mlx4_en_deactivate_tx_ring(struct mlx4_en_priv *priv,
 		       MLX4_QP_STATE_RST, NULL, 0, 0, &ring->qp);
 }
 
+static inline bool mlx4_en_is_tx_ring_full(struct mlx4_en_tx_ring *ring)
+{
+	return ring->prod - ring->cons > ring->full_size;
+}
+
 static void mlx4_en_stamp_wqe(struct mlx4_en_priv *priv,
 			      struct mlx4_en_tx_ring *ring, int index,
 			      u8 owner)
@@ -473,11 +480,10 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 
 	netdev_tx_completed_queue(ring->tx_queue, packets, bytes);
 
-	/*
-	 * Wakeup Tx queue if this stopped, and at least 1 packet
-	 * was completed
+	/* Wakeup Tx queue if this stopped, and ring is not full.
 	 */
-	if (netif_tx_queue_stopped(ring->tx_queue) && txbbs_skipped > 0) {
+	if (netif_tx_queue_stopped(ring->tx_queue) &&
+	    !mlx4_en_is_tx_ring_full(ring)) {
 		netif_tx_wake_queue(ring->tx_queue);
 		ring->wake_queue++;
 	}
@@ -921,8 +927,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	skb_tx_timestamp(skb);
 
 	/* Check available TXBBs And 2K spare for prefetch */
-	stop_queue = (int)(ring->prod - ring_cons) >
-		      ring->size - HEADROOM - MAX_DESC_TXBBS;
+	stop_queue = mlx4_en_is_tx_ring_full(ring);
 	if (unlikely(stop_queue)) {
 		netif_tx_stop_queue(ring->tx_queue);
 		ring->queue_stopped++;
@@ -991,8 +996,7 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 		smp_rmb();
 
 		ring_cons = ACCESS_ONCE(ring->cons);
-		if (unlikely(((int)(ring->prod - ring_cons)) <=
-			     ring->size - HEADROOM - MAX_DESC_TXBBS)) {
+		if (unlikely(!mlx4_en_is_tx_ring_full(ring))) {
 			netif_tx_wake_queue(ring->tx_queue);
 			ring->wake_queue++;
 		}
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index 2619c9fbf42d..aae13adfb492 100644
--- a/drivers/net/ethernet/mellanox/mlx4/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/eq.c
@@ -145,7 +145,7 @@ void mlx4_gen_slave_eqe(struct work_struct *work)
 	struct mlx4_slave_event_eq *slave_eq = &mfunc->master.slave_eq;
 	struct mlx4_eqe *eqe;
 	u8 slave;
-	int i;
+	int i, phys_port, slave_port;
 
 	for (eqe = next_slave_event_eqe(slave_eq); eqe;
 	      eqe = next_slave_event_eqe(slave_eq)) {
@@ -154,9 +154,20 @@ void mlx4_gen_slave_eqe(struct work_struct *work)
 		/* All active slaves need to receive the event */
 		if (slave == ALL_SLAVES) {
 			for (i = 0; i <= dev->persist->num_vfs; i++) {
+				phys_port = 0;
+				if (eqe->type == MLX4_EVENT_TYPE_PORT_MNG_CHG_EVENT &&
+				    eqe->subtype == MLX4_DEV_PMC_SUBTYPE_PORT_INFO) {
+					phys_port  = eqe->event.port_mgmt_change.port;
+					slave_port = mlx4_phys_to_slave_port(dev, i, phys_port);
+					if (slave_port < 0) /* VF doesn't have this port */
+						continue;
+					eqe->event.port_mgmt_change.port = slave_port;
+				}
 				if (mlx4_GEN_EQE(dev, i, eqe))
 					mlx4_warn(dev, "Failed to generate event for slave %d\n",
 						  i);
+				if (phys_port)
+					eqe->event.port_mgmt_change.port = phys_port;
 			}
 		} else {
 			if (mlx4_GEN_EQE(dev, slave, eqe))
@@ -210,6 +221,22 @@ static void mlx4_slave_event(struct mlx4_dev *dev, int slave,
 	slave_event(dev, slave, eqe);
 }
 
+#if defined(CONFIG_SMP)
+static void mlx4_set_eq_affinity_hint(struct mlx4_priv *priv, int vec)
+{
+	int hint_err;
+	struct mlx4_dev *dev = &priv->dev;
+	struct mlx4_eq *eq = &priv->eq_table.eq[vec];
+
+	if (!eq->affinity_mask || cpumask_empty(eq->affinity_mask))
+		return;
+
+	hint_err = irq_set_affinity_hint(eq->irq, eq->affinity_mask);
+	if (hint_err)
+		mlx4_warn(dev, "irq_set_affinity_hint failed, err %d\n", hint_err);
+}
+#endif
+
 int mlx4_gen_pkey_eqe(struct mlx4_dev *dev, int slave, u8 port)
 {
 	struct mlx4_eqe eqe;
@@ -224,7 +251,7 @@ int mlx4_gen_pkey_eqe(struct mlx4_dev *dev, int slave, u8 port)
 
 	eqe.type = MLX4_EVENT_TYPE_PORT_MNG_CHG_EVENT;
 	eqe.subtype = MLX4_DEV_PMC_SUBTYPE_PKEY_TABLE;
-	eqe.event.port_mgmt_change.port = port;
+	eqe.event.port_mgmt_change.port = mlx4_phys_to_slave_port(dev, slave, port);
 
 	return mlx4_GEN_EQE(dev, slave, &eqe);
 }
@@ -241,7 +268,7 @@ int mlx4_gen_guid_change_eqe(struct mlx4_dev *dev, int slave, u8 port)
 
 	eqe.type = MLX4_EVENT_TYPE_PORT_MNG_CHG_EVENT;
 	eqe.subtype = MLX4_DEV_PMC_SUBTYPE_GUID_INFO;
-	eqe.event.port_mgmt_change.port = port;
+	eqe.event.port_mgmt_change.port = mlx4_phys_to_slave_port(dev, slave, port);
 
 	return mlx4_GEN_EQE(dev, slave, &eqe);
 }
@@ -251,6 +278,7 @@ int mlx4_gen_port_state_change_eqe(struct mlx4_dev *dev, int slave, u8 port,
 				   u8 port_subtype_change)
 {
 	struct mlx4_eqe eqe;
+	u8 slave_port = mlx4_phys_to_slave_port(dev, slave, port);
 
 	/*don't send if we don't have the that slave */
 	if (dev->persist->num_vfs < slave)
@@ -259,7 +287,7 @@ int mlx4_gen_port_state_change_eqe(struct mlx4_dev *dev, int slave, u8 port,
 
 	eqe.type = MLX4_EVENT_TYPE_PORT_CHANGE;
 	eqe.subtype = port_subtype_change;
-	eqe.event.port_change.port = cpu_to_be32(port << 28);
+	eqe.event.port_change.port = cpu_to_be32(slave_port << 28);
 
 	mlx4_dbg(dev, "%s: sending: %d to slave: %d on port: %d\n", __func__,
 		 port_subtype_change, slave, port);
@@ -589,6 +617,10 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 						if (SLAVE_PORT_GEN_EVENT_DOWN ==  gen_event) {
 							if (i == mlx4_master_func_num(dev))
 								continue;
+							eqe->event.port_change.port =
+								cpu_to_be32(
+								(be32_to_cpu(eqe->event.port_change.port) & 0xFFFFFFF)
+								| (mlx4_phys_to_slave_port(dev, i, port) << 28));
 							mlx4_slave_event(dev, i, eqe);
 						}
 					}
@@ -879,8 +911,8 @@ static int mlx4_num_eq_uar(struct mlx4_dev *dev)
 	 * we need to map, take the difference of highest index and
 	 * the lowest index we'll use and add 1.
 	 */
-	return (dev->caps.num_comp_vectors + 1 + dev->caps.reserved_eqs +
-		 dev->caps.comp_pool)/4 - dev->caps.reserved_eqs/4 + 1;
+	return (dev->caps.num_comp_vectors + 1 + dev->caps.reserved_eqs) / 4 -
+		dev->caps.reserved_eqs / 4 + 1;
 }
 
 static void __iomem *mlx4_get_eq_uar(struct mlx4_dev *dev, struct mlx4_eq *eq)
@@ -1069,32 +1101,21 @@ static void mlx4_free_eq(struct mlx4_dev *dev,
 static void mlx4_free_irqs(struct mlx4_dev *dev)
 {
 	struct mlx4_eq_table *eq_table = &mlx4_priv(dev)->eq_table;
-	struct mlx4_priv *priv = mlx4_priv(dev);
-	int	i, vec;
+	int	i;
 
 	if (eq_table->have_irq)
 		free_irq(dev->persist->pdev->irq, dev);
 
 	for (i = 0; i < dev->caps.num_comp_vectors + 1; ++i)
 		if (eq_table->eq[i].have_irq) {
+			free_cpumask_var(eq_table->eq[i].affinity_mask);
+#if defined(CONFIG_SMP)
+			irq_set_affinity_hint(eq_table->eq[i].irq, NULL);
+#endif
 			free_irq(eq_table->eq[i].irq, eq_table->eq + i);
 			eq_table->eq[i].have_irq = 0;
 		}
 
-	for (i = 0; i < dev->caps.comp_pool; i++) {
-		/*
-		 * Freeing the assigned irq's
-		 * all bits should be 0, but we need to validate
-		 */
-		if (priv->msix_ctl.pool_bm & 1ULL << i) {
-			/* NO need protecting*/
-			vec = dev->caps.num_comp_vectors + 1 + i;
-			free_irq(priv->eq_table.eq[vec].irq,
-				 &priv->eq_table.eq[vec]);
-		}
-	}
-
-
 	kfree(eq_table->irq_names);
 }
 
@@ -1175,76 +1196,73 @@ int mlx4_init_eq_table(struct mlx4_dev *dev)
 	}
 
 	priv->eq_table.irq_names =
-		kmalloc(MLX4_IRQNAME_SIZE * (dev->caps.num_comp_vectors + 1 +
-					     dev->caps.comp_pool),
+		kmalloc(MLX4_IRQNAME_SIZE * (dev->caps.num_comp_vectors + 1),
 			GFP_KERNEL);
 	if (!priv->eq_table.irq_names) {
 		err = -ENOMEM;
-		goto err_out_bitmap;
+		goto err_out_clr_int;
 	}
 
-	for (i = 0; i < dev->caps.num_comp_vectors; ++i) {
-		err = mlx4_create_eq(dev, dev->caps.num_cqs -
-					  dev->caps.reserved_cqs +
-					  MLX4_NUM_SPARE_EQE,
-				     (dev->flags & MLX4_FLAG_MSI_X) ? i : 0,
-				     &priv->eq_table.eq[i]);
-		if (err) {
-			--i;
-			goto err_out_unmap;
-		}
-	}
-
-	err = mlx4_create_eq(dev, MLX4_NUM_ASYNC_EQE + MLX4_NUM_SPARE_EQE,
-			     (dev->flags & MLX4_FLAG_MSI_X) ? dev->caps.num_comp_vectors : 0,
-			     &priv->eq_table.eq[dev->caps.num_comp_vectors]);
-	if (err)
-		goto err_out_comp;
-
-	/*if additional completion vectors poolsize is 0 this loop will not run*/
-	for (i = dev->caps.num_comp_vectors + 1;
-	      i < dev->caps.num_comp_vectors + dev->caps.comp_pool + 1; ++i) {
+	for (i = 0; i < dev->caps.num_comp_vectors + 1; ++i) {
+		if (i == MLX4_EQ_ASYNC) {
+			err = mlx4_create_eq(dev,
+					     MLX4_NUM_ASYNC_EQE + MLX4_NUM_SPARE_EQE,
+					     0, &priv->eq_table.eq[MLX4_EQ_ASYNC]);
+		} else {
+			struct mlx4_eq	*eq = &priv->eq_table.eq[i];
+#ifdef CONFIG_RFS_ACCEL
+			int port = find_first_bit(eq->actv_ports.ports,
+						  dev->caps.num_ports) + 1;
+
+			if (port <= dev->caps.num_ports) {
+				struct mlx4_port_info *info =
+					&mlx4_priv(dev)->port[port];
+
+				if (!info->rmap) {
+					info->rmap = alloc_irq_cpu_rmap(
+						mlx4_get_eqs_per_port(dev, port));
+					if (!info->rmap) {
+						mlx4_warn(dev, "Failed to allocate cpu rmap\n");
+						err = -ENOMEM;
+						goto err_out_unmap;
+					}
+				}
 
-		err = mlx4_create_eq(dev, dev->caps.num_cqs -
-					  dev->caps.reserved_cqs +
-					  MLX4_NUM_SPARE_EQE,
-				     (dev->flags & MLX4_FLAG_MSI_X) ? i : 0,
-				     &priv->eq_table.eq[i]);
-		if (err) {
-			--i;
-			goto err_out_unmap;
+				err = irq_cpu_rmap_add(
+					info->rmap, eq->irq);
+				if (err)
+					mlx4_warn(dev, "Failed adding irq rmap\n");
+			}
+#endif
+			err = mlx4_create_eq(dev, dev->caps.num_cqs -
+						  dev->caps.reserved_cqs +
+						  MLX4_NUM_SPARE_EQE,
+					     (dev->flags & MLX4_FLAG_MSI_X) ?
+					     i + 1 - !!(i > MLX4_EQ_ASYNC) : 0,
+					     eq);
 		}
+		if (err)
+			goto err_out_unmap;
 	}
 
-
 	if (dev->flags & MLX4_FLAG_MSI_X) {
 		const char *eq_name;
 
-		for (i = 0; i < dev->caps.num_comp_vectors + 1; ++i) {
-			if (i < dev->caps.num_comp_vectors) {
-				snprintf(priv->eq_table.irq_names +
-					 i * MLX4_IRQNAME_SIZE,
-					 MLX4_IRQNAME_SIZE,
-					 "mlx4-comp-%d@pci:%s", i,
-					 pci_name(dev->persist->pdev));
-			} else {
-				snprintf(priv->eq_table.irq_names +
-					 i * MLX4_IRQNAME_SIZE,
-					 MLX4_IRQNAME_SIZE,
-					 "mlx4-async@pci:%s",
-					 pci_name(dev->persist->pdev));
-			}
+		snprintf(priv->eq_table.irq_names +
+			 MLX4_EQ_ASYNC * MLX4_IRQNAME_SIZE,
+			 MLX4_IRQNAME_SIZE,
+			 "mlx4-async@pci:%s",
+			 pci_name(dev->persist->pdev));
+		eq_name = priv->eq_table.irq_names +
+			MLX4_EQ_ASYNC * MLX4_IRQNAME_SIZE;
 
-			eq_name = priv->eq_table.irq_names +
-				  i * MLX4_IRQNAME_SIZE;
-			err = request_irq(priv->eq_table.eq[i].irq,
-					  mlx4_msi_x_interrupt, 0, eq_name,
-					  priv->eq_table.eq + i);
-			if (err)
-				goto err_out_async;
+		err = request_irq(priv->eq_table.eq[MLX4_EQ_ASYNC].irq,
+				  mlx4_msi_x_interrupt, 0, eq_name,
+				  priv->eq_table.eq + MLX4_EQ_ASYNC);
+		if (err)
+			goto err_out_unmap;
 
-			priv->eq_table.eq[i].have_irq = 1;
-		}
+		priv->eq_table.eq[MLX4_EQ_ASYNC].have_irq = 1;
 	} else {
 		snprintf(priv->eq_table.irq_names,
 			 MLX4_IRQNAME_SIZE,
@@ -1253,36 +1271,38 @@ int mlx4_init_eq_table(struct mlx4_dev *dev)
 		err = request_irq(dev->persist->pdev->irq, mlx4_interrupt,
 				  IRQF_SHARED, priv->eq_table.irq_names, dev);
 		if (err)
-			goto err_out_async;
+			goto err_out_unmap;
 
 		priv->eq_table.have_irq = 1;
 	}
 
 	err = mlx4_MAP_EQ(dev, get_async_ev_mask(dev), 0,
-			  priv->eq_table.eq[dev->caps.num_comp_vectors].eqn);
+			  priv->eq_table.eq[MLX4_EQ_ASYNC].eqn);
 	if (err)
 		mlx4_warn(dev, "MAP_EQ for async EQ %d failed (%d)\n",
-			   priv->eq_table.eq[dev->caps.num_comp_vectors].eqn, err);
+			   priv->eq_table.eq[MLX4_EQ_ASYNC].eqn, err);
 
-	for (i = 0; i < dev->caps.num_comp_vectors + 1; ++i)
-		eq_set_ci(&priv->eq_table.eq[i], 1);
+	/* arm ASYNC eq */
+	eq_set_ci(&priv->eq_table.eq[MLX4_EQ_ASYNC], 1);
 
 	return 0;
 
-err_out_async:
-	mlx4_free_eq(dev, &priv->eq_table.eq[dev->caps.num_comp_vectors]);
-
-err_out_comp:
-	i = dev->caps.num_comp_vectors - 1;
-
 err_out_unmap:
-	while (i >= 0) {
-		mlx4_free_eq(dev, &priv->eq_table.eq[i]);
-		--i;
+	while (i >= 0)
+		mlx4_free_eq(dev, &priv->eq_table.eq[i--]);
+#ifdef CONFIG_RFS_ACCEL
+	for (i = 1; i <= dev->caps.num_ports; i++) {
+		if (mlx4_priv(dev)->port[i].rmap) {
+			free_irq_cpu_rmap(mlx4_priv(dev)->port[i].rmap);
+			mlx4_priv(dev)->port[i].rmap = NULL;
+		}
 	}
+#endif
+	mlx4_free_irqs(dev);
+
+err_out_clr_int:
 	if (!mlx4_is_slave(dev))
 		mlx4_unmap_clr_int(dev);
-	mlx4_free_irqs(dev);
 
 err_out_bitmap:
 	mlx4_unmap_uar(dev);
@@ -1300,11 +1320,19 @@ void mlx4_cleanup_eq_table(struct mlx4_dev *dev)
 	int i;
 
 	mlx4_MAP_EQ(dev, get_async_ev_mask(dev), 1,
-		    priv->eq_table.eq[dev->caps.num_comp_vectors].eqn);
+		    priv->eq_table.eq[MLX4_EQ_ASYNC].eqn);
 
+#ifdef CONFIG_RFS_ACCEL
+	for (i = 1; i <= dev->caps.num_ports; i++) {
+		if (mlx4_priv(dev)->port[i].rmap) {
+			free_irq_cpu_rmap(mlx4_priv(dev)->port[i].rmap);
+			mlx4_priv(dev)->port[i].rmap = NULL;
+		}
+	}
+#endif
 	mlx4_free_irqs(dev);
 
-	for (i = 0; i < dev->caps.num_comp_vectors + dev->caps.comp_pool + 1; ++i)
+	for (i = 0; i < dev->caps.num_comp_vectors + 1; ++i)
 		mlx4_free_eq(dev, &priv->eq_table.eq[i]);
 
 	if (!mlx4_is_slave(dev))
@@ -1355,87 +1383,169 @@ int mlx4_test_interrupts(struct mlx4_dev *dev)
 
 	/* Return to default */
 	mlx4_MAP_EQ(dev, get_async_ev_mask(dev), 0,
-		    priv->eq_table.eq[dev->caps.num_comp_vectors].eqn);
+		    priv->eq_table.eq[MLX4_EQ_ASYNC].eqn);
 	return err;
 }
 EXPORT_SYMBOL(mlx4_test_interrupts);
 
-int mlx4_assign_eq(struct mlx4_dev *dev, char *name, struct cpu_rmap *rmap,
-		   int *vector)
+bool mlx4_is_eq_vector_valid(struct mlx4_dev *dev, u8 port, int vector)
 {
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	vector = MLX4_CQ_TO_EQ_VECTOR(vector);
+	if (vector < 0 || (vector >= dev->caps.num_comp_vectors + 1) ||
+	    (vector == MLX4_EQ_ASYNC))
+		return false;
+
+	return test_bit(port - 1, priv->eq_table.eq[vector].actv_ports.ports);
+}
+EXPORT_SYMBOL(mlx4_is_eq_vector_valid);
 
+u32 mlx4_get_eqs_per_port(struct mlx4_dev *dev, u8 port)
+{
 	struct mlx4_priv *priv = mlx4_priv(dev);
-	int vec = 0, err = 0, i;
+	unsigned int i;
+	unsigned int sum = 0;
+
+	for (i = 0; i < dev->caps.num_comp_vectors + 1; i++)
+		sum += !!test_bit(port - 1,
+				  priv->eq_table.eq[i].actv_ports.ports);
+
+	return sum;
+}
+EXPORT_SYMBOL(mlx4_get_eqs_per_port);
+
+int mlx4_is_eq_shared(struct mlx4_dev *dev, int vector)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	vector = MLX4_CQ_TO_EQ_VECTOR(vector);
+	if (vector <= 0 || (vector >= dev->caps.num_comp_vectors + 1))
+		return -EINVAL;
+
+	return !!(bitmap_weight(priv->eq_table.eq[vector].actv_ports.ports,
+				dev->caps.num_ports) > 1);
+}
+EXPORT_SYMBOL(mlx4_is_eq_shared);
+
+struct cpu_rmap *mlx4_get_cpu_rmap(struct mlx4_dev *dev, int port)
+{
+	return mlx4_priv(dev)->port[port].rmap;
+}
+EXPORT_SYMBOL(mlx4_get_cpu_rmap);
+
+int mlx4_assign_eq(struct mlx4_dev *dev, u8 port, int *vector)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	int err = 0, i = 0;
+	u32 min_ref_count_val = (u32)-1;
+	int requested_vector = MLX4_CQ_TO_EQ_VECTOR(*vector);
+	int *prequested_vector = NULL;
+
 
 	mutex_lock(&priv->msix_ctl.pool_lock);
-	for (i = 0; !vec && i < dev->caps.comp_pool; i++) {
-		if (~priv->msix_ctl.pool_bm & 1ULL << i) {
-			priv->msix_ctl.pool_bm |= 1ULL << i;
-			vec = dev->caps.num_comp_vectors + 1 + i;
-			snprintf(priv->eq_table.irq_names +
-					vec * MLX4_IRQNAME_SIZE,
-					MLX4_IRQNAME_SIZE, "%s", name);
-#ifdef CONFIG_RFS_ACCEL
-			if (rmap) {
-				err = irq_cpu_rmap_add(rmap,
-						       priv->eq_table.eq[vec].irq);
-				if (err)
-					mlx4_warn(dev, "Failed adding irq rmap\n");
+	if (requested_vector < (dev->caps.num_comp_vectors + 1) &&
+	    (requested_vector >= 0) &&
+	    (requested_vector != MLX4_EQ_ASYNC)) {
+		if (test_bit(port - 1,
+			     priv->eq_table.eq[requested_vector].actv_ports.ports)) {
+			prequested_vector = &requested_vector;
+		} else {
+			struct mlx4_eq *eq;
+
+			for (i = 1; i < port;
+			     requested_vector += mlx4_get_eqs_per_port(dev, i++))
+				;
+
+			eq = &priv->eq_table.eq[requested_vector];
+			if (requested_vector < dev->caps.num_comp_vectors + 1 &&
+			    test_bit(port - 1, eq->actv_ports.ports)) {
+				prequested_vector = &requested_vector;
 			}
-#endif
-			err = request_irq(priv->eq_table.eq[vec].irq,
-					  mlx4_msi_x_interrupt, 0,
-					  &priv->eq_table.irq_names[vec<<5],
-					  priv->eq_table.eq + vec);
-			if (err) {
-				/*zero out bit by fliping it*/
-				priv->msix_ctl.pool_bm ^= 1 << i;
-				vec = 0;
-				continue;
-				/*we dont want to break here*/
+		}
+	}
+
+	if  (!prequested_vector) {
+		requested_vector = -1;
+		for (i = 0; min_ref_count_val && i < dev->caps.num_comp_vectors + 1;
+		     i++) {
+			struct mlx4_eq *eq = &priv->eq_table.eq[i];
+
+			if (min_ref_count_val > eq->ref_count &&
+			    test_bit(port - 1, eq->actv_ports.ports)) {
+				min_ref_count_val = eq->ref_count;
+				requested_vector = i;
 			}
+		}
+
+		if (requested_vector < 0) {
+			err = -ENOSPC;
+			goto err_unlock;
+		}
+
+		prequested_vector = &requested_vector;
+	}
+
+	if (!test_bit(*prequested_vector, priv->msix_ctl.pool_bm) &&
+	    dev->flags & MLX4_FLAG_MSI_X) {
+		set_bit(*prequested_vector, priv->msix_ctl.pool_bm);
+		snprintf(priv->eq_table.irq_names +
+			 *prequested_vector * MLX4_IRQNAME_SIZE,
+			 MLX4_IRQNAME_SIZE, "mlx4-%d@%s",
+			 *prequested_vector, dev_name(&dev->persist->pdev->dev));
 
-			eq_set_ci(&priv->eq_table.eq[vec], 1);
+		err = request_irq(priv->eq_table.eq[*prequested_vector].irq,
+				  mlx4_msi_x_interrupt, 0,
+				  &priv->eq_table.irq_names[*prequested_vector << 5],
+				  priv->eq_table.eq + *prequested_vector);
+
+		if (err) {
+			clear_bit(*prequested_vector, priv->msix_ctl.pool_bm);
+			*prequested_vector = -1;
+		} else {
+#if defined(CONFIG_SMP)
+			mlx4_set_eq_affinity_hint(priv, *prequested_vector);
+#endif
+			eq_set_ci(&priv->eq_table.eq[*prequested_vector], 1);
+			priv->eq_table.eq[*prequested_vector].have_irq = 1;
 		}
 	}
+
+	if (!err && *prequested_vector >= 0)
+		priv->eq_table.eq[*prequested_vector].ref_count++;
+
+err_unlock:
 	mutex_unlock(&priv->msix_ctl.pool_lock);
 
-	if (vec) {
-		*vector = vec;
-	} else {
+	if (!err && *prequested_vector >= 0)
+		*vector = MLX4_EQ_TO_CQ_VECTOR(*prequested_vector);
+	else
 		*vector = 0;
-		err = (i == dev->caps.comp_pool) ? -ENOSPC : err;
-	}
+
 	return err;
 }
 EXPORT_SYMBOL(mlx4_assign_eq);
 
-int mlx4_eq_get_irq(struct mlx4_dev *dev, int vec)
+int mlx4_eq_get_irq(struct mlx4_dev *dev, int cq_vec)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 
-	return priv->eq_table.eq[vec].irq;
+	return priv->eq_table.eq[MLX4_CQ_TO_EQ_VECTOR(cq_vec)].irq;
 }
 EXPORT_SYMBOL(mlx4_eq_get_irq);
 
 void mlx4_release_eq(struct mlx4_dev *dev, int vec)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
-	/*bm index*/
-	int i = vec - dev->caps.num_comp_vectors - 1;
-
-	if (likely(i >= 0)) {
-		/*sanity check , making sure were not trying to free irq's
-		  Belonging to a legacy EQ*/
-		mutex_lock(&priv->msix_ctl.pool_lock);
-		if (priv->msix_ctl.pool_bm & 1ULL << i) {
-			free_irq(priv->eq_table.eq[vec].irq,
-				 &priv->eq_table.eq[vec]);
-			priv->msix_ctl.pool_bm &= ~(1ULL << i);
-		}
-		mutex_unlock(&priv->msix_ctl.pool_lock);
-	}
+	int eq_vec = MLX4_CQ_TO_EQ_VECTOR(vec);
 
+	mutex_lock(&priv->msix_ctl.pool_lock);
+	priv->eq_table.eq[eq_vec].ref_count--;
+
+	/* once we allocated EQ, we don't release it because it might be binded
+	 * to cpu_rmap.
+	 */
+	mutex_unlock(&priv->msix_ctl.pool_lock);
 }
 EXPORT_SYMBOL(mlx4_release_eq);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/intf.c b/drivers/net/ethernet/mellanox/mlx4/intf.c
index 6fce58718837..0d80aed59043 100644
--- a/drivers/net/ethernet/mellanox/mlx4/intf.c
+++ b/drivers/net/ethernet/mellanox/mlx4/intf.c
@@ -93,8 +93,14 @@ int mlx4_register_interface(struct mlx4_interface *intf)
 	mutex_lock(&intf_mutex);
 
 	list_add_tail(&intf->list, &intf_list);
-	list_for_each_entry(priv, &dev_list, dev_list)
+	list_for_each_entry(priv, &dev_list, dev_list) {
+		if (mlx4_is_mfunc(&priv->dev) && (intf->flags & MLX4_INTFF_BONDING)) {
+			mlx4_dbg(&priv->dev,
+				 "SRIOV, disabling HA mode for intf proto %d\n", intf->protocol);
+			intf->flags &= ~MLX4_INTFF_BONDING;
+		}
 		mlx4_add_device(intf, priv);
+	}
 
 	mutex_unlock(&intf_mutex);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index ced5ecab5aa7..12fbfcb44d8a 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -479,7 +479,15 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		}
 	}
 
-	dev->caps.max_counters = 1 << ilog2(dev_cap->max_counters);
+	if (mlx4_is_master(dev) && (dev->caps.num_ports == 2) &&
+	    (port_type_array[0] == MLX4_PORT_TYPE_IB) &&
+	    (port_type_array[1] == MLX4_PORT_TYPE_ETH)) {
+		mlx4_warn(dev,
+			  "Granular QoS per VF not supported with IB/Eth configuration\n");
+		dev->caps.flags2 &= ~MLX4_DEV_CAP_FLAG2_QOS_VPP;
+	}
+
+	dev->caps.max_counters = dev_cap->max_counters;
 
 	dev->caps.reserved_qps_cnt[MLX4_QP_REGION_FW] = dev_cap->reserved_qps;
 	dev->caps.reserved_qps_cnt[MLX4_QP_REGION_ETH_ADDR] =
@@ -1674,6 +1682,25 @@ static int map_internal_clock(struct mlx4_dev *dev)
 	return 0;
 }
 
+int mlx4_get_internal_clock_params(struct mlx4_dev *dev,
+				   struct mlx4_clock_params *params)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	if (mlx4_is_slave(dev))
+		return -ENOTSUPP;
+
+	if (!params)
+		return -EINVAL;
+
+	params->bar = priv->fw.clock_bar;
+	params->offset = priv->fw.clock_offset;
+	params->size = MLX4_CLOCK_SIZE;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx4_get_internal_clock_params);
+
 static void unmap_internal_clock(struct mlx4_dev *dev)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -2193,20 +2220,73 @@ err_free_icm:
 static int mlx4_init_counters_table(struct mlx4_dev *dev)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
-	int nent;
+	int nent_pow2;
 
 	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_COUNTERS))
 		return -ENOENT;
 
-	nent = dev->caps.max_counters;
-	return mlx4_bitmap_init(&priv->counters_bitmap, nent, nent - 1, 0, 0);
+	if (!dev->caps.max_counters)
+		return -ENOSPC;
+
+	nent_pow2 = roundup_pow_of_two(dev->caps.max_counters);
+	/* reserve last counter index for sink counter */
+	return mlx4_bitmap_init(&priv->counters_bitmap, nent_pow2,
+				nent_pow2 - 1, 0,
+				nent_pow2 - dev->caps.max_counters + 1);
 }
 
 static void mlx4_cleanup_counters_table(struct mlx4_dev *dev)
 {
+	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_COUNTERS))
+		return;
+
+	if (!dev->caps.max_counters)
+		return;
+
 	mlx4_bitmap_cleanup(&mlx4_priv(dev)->counters_bitmap);
 }
 
+static void mlx4_cleanup_default_counters(struct mlx4_dev *dev)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	int port;
+
+	for (port = 0; port < dev->caps.num_ports; port++)
+		if (priv->def_counter[port] != -1)
+			mlx4_counter_free(dev,  priv->def_counter[port]);
+}
+
+static int mlx4_allocate_default_counters(struct mlx4_dev *dev)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	int port, err = 0;
+	u32 idx;
+
+	for (port = 0; port < dev->caps.num_ports; port++)
+		priv->def_counter[port] = -1;
+
+	for (port = 0; port < dev->caps.num_ports; port++) {
+		err = mlx4_counter_alloc(dev, &idx);
+
+		if (!err || err == -ENOSPC) {
+			priv->def_counter[port] = idx;
+		} else if (err == -ENOENT) {
+			err = 0;
+			continue;
+		} else {
+			mlx4_err(dev, "%s: failed to allocate default counter port %d err %d\n",
+				 __func__, port + 1, err);
+			mlx4_cleanup_default_counters(dev);
+			return err;
+		}
+
+		mlx4_dbg(dev, "%s: default counter index %d for port %d\n",
+			 __func__, priv->def_counter[port], port + 1);
+	}
+
+	return err;
+}
+
 int __mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -2215,8 +2295,10 @@ int __mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx)
 		return -ENOENT;
 
 	*idx = mlx4_bitmap_alloc(&priv->counters_bitmap);
-	if (*idx == -1)
-		return -ENOMEM;
+	if (*idx == -1) {
+		*idx = MLX4_SINK_COUNTER_INDEX(dev);
+		return -ENOSPC;
+	}
 
 	return 0;
 }
@@ -2239,8 +2321,35 @@ int mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx)
 }
 EXPORT_SYMBOL_GPL(mlx4_counter_alloc);
 
+static int __mlx4_clear_if_stat(struct mlx4_dev *dev,
+				u8 counter_index)
+{
+	struct mlx4_cmd_mailbox *if_stat_mailbox;
+	int err;
+	u32 if_stat_in_mod = (counter_index & 0xff) | MLX4_QUERY_IF_STAT_RESET;
+
+	if_stat_mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(if_stat_mailbox))
+		return PTR_ERR(if_stat_mailbox);
+
+	err = mlx4_cmd_box(dev, 0, if_stat_mailbox->dma, if_stat_in_mod, 0,
+			   MLX4_CMD_QUERY_IF_STAT, MLX4_CMD_TIME_CLASS_C,
+			   MLX4_CMD_NATIVE);
+
+	mlx4_free_cmd_mailbox(dev, if_stat_mailbox);
+	return err;
+}
+
 void __mlx4_counter_free(struct mlx4_dev *dev, u32 idx)
 {
+	if (!(dev->caps.flags & MLX4_DEV_CAP_FLAG_COUNTERS))
+		return;
+
+	if (idx == MLX4_SINK_COUNTER_INDEX(dev))
+		return;
+
+	__mlx4_clear_if_stat(dev, idx);
+
 	mlx4_bitmap_free(&mlx4_priv(dev)->counters_bitmap, idx, MLX4_USE_RR);
 	return;
 }
@@ -2260,6 +2369,14 @@ void mlx4_counter_free(struct mlx4_dev *dev, u32 idx)
 }
 EXPORT_SYMBOL_GPL(mlx4_counter_free);
 
+int mlx4_get_default_counter_index(struct mlx4_dev *dev, int port)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+
+	return priv->def_counter[port - 1];
+}
+EXPORT_SYMBOL_GPL(mlx4_get_default_counter_index);
+
 void mlx4_set_admin_guid(struct mlx4_dev *dev, __be64 guid, int entry, int port)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -2364,11 +2481,11 @@ static int mlx4_setup_hca(struct mlx4_dev *dev)
 	if (err) {
 		if (dev->flags & MLX4_FLAG_MSI_X) {
 			mlx4_warn(dev, "NOP command failed to generate MSI-X interrupt IRQ %d)\n",
-				  priv->eq_table.eq[dev->caps.num_comp_vectors].irq);
+				  priv->eq_table.eq[MLX4_EQ_ASYNC].irq);
 			mlx4_warn(dev, "Trying again without MSI-X\n");
 		} else {
 			mlx4_err(dev, "NOP command failed to generate interrupt (IRQ %d), aborting\n",
-				 priv->eq_table.eq[dev->caps.num_comp_vectors].irq);
+				 priv->eq_table.eq[MLX4_EQ_ASYNC].irq);
 			mlx4_err(dev, "BIOS or ACPI interrupt routing problem?\n");
 		}
 
@@ -2395,10 +2512,18 @@ static int mlx4_setup_hca(struct mlx4_dev *dev)
 		goto err_srq_table_free;
 	}
 
-	err = mlx4_init_counters_table(dev);
-	if (err && err != -ENOENT) {
-		mlx4_err(dev, "Failed to initialize counters table, aborting\n");
-		goto err_qp_table_free;
+	if (!mlx4_is_slave(dev)) {
+		err = mlx4_init_counters_table(dev);
+		if (err && err != -ENOENT) {
+			mlx4_err(dev, "Failed to initialize counters table, aborting\n");
+			goto err_qp_table_free;
+		}
+	}
+
+	err = mlx4_allocate_default_counters(dev);
+	if (err) {
+		mlx4_err(dev, "Failed to allocate default counters, aborting\n");
+		goto err_counters_table_free;
 	}
 
 	if (!mlx4_is_slave(dev)) {
@@ -2432,15 +2557,19 @@ static int mlx4_setup_hca(struct mlx4_dev *dev)
 			if (err) {
 				mlx4_err(dev, "Failed to set port %d, aborting\n",
 					 port);
-				goto err_counters_table_free;
+				goto err_default_countes_free;
 			}
 		}
 	}
 
 	return 0;
 
+err_default_countes_free:
+	mlx4_cleanup_default_counters(dev);
+
 err_counters_table_free:
-	mlx4_cleanup_counters_table(dev);
+	if (!mlx4_is_slave(dev))
+		mlx4_cleanup_counters_table(dev);
 
 err_qp_table_free:
 	mlx4_cleanup_qp_table(dev);
@@ -2481,14 +2610,45 @@ err_uar_table_free:
 	return err;
 }
 
+static int mlx4_init_affinity_hint(struct mlx4_dev *dev, int port, int eqn)
+{
+	int requested_cpu = 0;
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_eq *eq;
+	int off = 0;
+	int i;
+
+	if (eqn > dev->caps.num_comp_vectors)
+		return -EINVAL;
+
+	for (i = 1; i < port; i++)
+		off += mlx4_get_eqs_per_port(dev, i);
+
+	requested_cpu = eqn - off - !!(eqn > MLX4_EQ_ASYNC);
+
+	/* Meaning EQs are shared, and this call comes from the second port */
+	if (requested_cpu < 0)
+		return 0;
+
+	eq = &priv->eq_table.eq[eqn];
+
+	if (!zalloc_cpumask_var(&eq->affinity_mask, GFP_KERNEL))
+		return -ENOMEM;
+
+	cpumask_set_cpu(requested_cpu, eq->affinity_mask);
+
+	return 0;
+}
+
 static void mlx4_enable_msi_x(struct mlx4_dev *dev)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	struct msix_entry *entries;
 	int i;
+	int port = 0;
 
 	if (msi_x) {
-		int nreq = dev->caps.num_ports * num_online_cpus() + MSIX_LEGACY_SZ;
+		int nreq = dev->caps.num_ports * num_online_cpus() + 1;
 
 		nreq = min_t(int, dev->caps.num_eqs - dev->caps.reserved_eqs,
 			     nreq);
@@ -2503,20 +2663,55 @@ static void mlx4_enable_msi_x(struct mlx4_dev *dev)
 		nreq = pci_enable_msix_range(dev->persist->pdev, entries, 2,
 					     nreq);
 
-		if (nreq < 0) {
+		if (nreq < 0 || nreq < MLX4_EQ_ASYNC) {
 			kfree(entries);
 			goto no_msi;
-		} else if (nreq < MSIX_LEGACY_SZ +
-			   dev->caps.num_ports * MIN_MSIX_P_PORT) {
-			/*Working in legacy mode , all EQ's shared*/
-			dev->caps.comp_pool           = 0;
-			dev->caps.num_comp_vectors = nreq - 1;
-		} else {
-			dev->caps.comp_pool           = nreq - MSIX_LEGACY_SZ;
-			dev->caps.num_comp_vectors = MSIX_LEGACY_SZ - 1;
 		}
-		for (i = 0; i < nreq; ++i)
-			priv->eq_table.eq[i].irq = entries[i].vector;
+		/* 1 is reserved for events (asyncrounous EQ) */
+		dev->caps.num_comp_vectors = nreq - 1;
+
+		priv->eq_table.eq[MLX4_EQ_ASYNC].irq = entries[0].vector;
+		bitmap_zero(priv->eq_table.eq[MLX4_EQ_ASYNC].actv_ports.ports,
+			    dev->caps.num_ports);
+
+		for (i = 0; i < dev->caps.num_comp_vectors + 1; i++) {
+			if (i == MLX4_EQ_ASYNC)
+				continue;
+
+			priv->eq_table.eq[i].irq =
+				entries[i + 1 - !!(i > MLX4_EQ_ASYNC)].vector;
+
+			if (MLX4_IS_LEGACY_EQ_MODE(dev->caps)) {
+				bitmap_fill(priv->eq_table.eq[i].actv_ports.ports,
+					    dev->caps.num_ports);
+				/* We don't set affinity hint when there
+				 * aren't enough EQs
+				 */
+			} else {
+				set_bit(port,
+					priv->eq_table.eq[i].actv_ports.ports);
+				if (mlx4_init_affinity_hint(dev, port + 1, i))
+					mlx4_warn(dev, "Couldn't init hint cpumask for EQ %d\n",
+						  i);
+			}
+			/* We divide the Eqs evenly between the two ports.
+			 * (dev->caps.num_comp_vectors / dev->caps.num_ports)
+			 * refers to the number of Eqs per port
+			 * (i.e eqs_per_port). Theoretically, we would like to
+			 * write something like (i + 1) % eqs_per_port == 0.
+			 * However, since there's an asynchronous Eq, we have
+			 * to skip over it by comparing this condition to
+			 * !!((i + 1) > MLX4_EQ_ASYNC).
+			 */
+			if ((dev->caps.num_comp_vectors > dev->caps.num_ports) &&
+			    ((i + 1) %
+			     (dev->caps.num_comp_vectors / dev->caps.num_ports)) ==
+			    !!((i + 1) > MLX4_EQ_ASYNC))
+				/* If dev->caps.num_comp_vectors < dev->caps.num_ports,
+				 * everything is shared anyway.
+				 */
+				port++;
+		}
 
 		dev->flags |= MLX4_FLAG_MSI_X;
 
@@ -2526,10 +2721,15 @@ static void mlx4_enable_msi_x(struct mlx4_dev *dev)
 
 no_msi:
 	dev->caps.num_comp_vectors = 1;
-	dev->caps.comp_pool	   = 0;
 
-	for (i = 0; i < 2; ++i)
+	BUG_ON(MLX4_EQ_ASYNC >= 2);
+	for (i = 0; i < 2; ++i) {
 		priv->eq_table.eq[i].irq = dev->persist->pdev->irq;
+		if (i != MLX4_EQ_ASYNC) {
+			bitmap_fill(priv->eq_table.eq[i].actv_ports.ports,
+				    dev->caps.num_ports);
+		}
+	}
 }
 
 static int mlx4_init_port_info(struct mlx4_dev *dev, int port)
@@ -2594,6 +2794,10 @@ static void mlx4_cleanup_port_info(struct mlx4_port_info *info)
 	device_remove_file(&info->dev->persist->pdev->dev, &info->port_attr);
 	device_remove_file(&info->dev->persist->pdev->dev,
 			   &info->port_mtu_attr);
+#ifdef CONFIG_RFS_ACCEL
+	free_irq_cpu_rmap(info->rmap);
+	info->rmap = NULL;
+#endif
 }
 
 static int mlx4_init_steering(struct mlx4_dev *dev)
@@ -2749,6 +2953,7 @@ disable_sriov:
 free_mem:
 	dev->persist->num_vfs = 0;
 	kfree(dev->dev_vfs);
+        dev->dev_vfs = NULL;
 	return dev_flags & ~MLX4_FLAG_MASTER;
 }
 
@@ -2900,6 +3105,7 @@ slave_start:
 								  existing_vfs,
 								  reset_flow);
 
+				mlx4_close_fw(dev);
 				mlx4_cmd_cleanup(dev, MLX4_CMD_CLEANUP_ALL);
 				dev->flags = dev_flags;
 				if (!SRIOV_VALID_STATE(dev->flags)) {
@@ -2988,18 +3194,6 @@ slave_start:
 	/* In master functions, the communication channel must be initialized
 	 * after obtaining its address from fw */
 	if (mlx4_is_master(dev)) {
-		int ib_ports = 0;
-
-		mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB)
-			ib_ports++;
-
-		if (ib_ports &&
-		    (num_vfs_argc > 1 || probe_vfs_argc > 1)) {
-			mlx4_err(dev,
-				 "Invalid syntax of num_vfs/probe_vfs with IB port - single port VFs syntax is only supported when all ports are configured as ethernet\n");
-			err = -EINVAL;
-			goto err_close;
-		}
 		if (dev->caps.num_ports < 2 &&
 		    num_vfs_argc > 1) {
 			err = -EINVAL;
@@ -3036,7 +3230,7 @@ slave_start:
 	if (err)
 		goto err_master_mfunc;
 
-	priv->msix_ctl.pool_bm = 0;
+	bitmap_zero(priv->msix_ctl.pool_bm, MAX_MSIX);
 	mutex_init(&priv->msix_ctl.pool_lock);
 
 	mlx4_enable_msi_x(dev);
@@ -3058,7 +3252,6 @@ slave_start:
 	    !mlx4_is_mfunc(dev)) {
 		dev->flags &= ~MLX4_FLAG_MSI_X;
 		dev->caps.num_comp_vectors = 1;
-		dev->caps.comp_pool	   = 0;
 		pci_disable_msix(pdev);
 		err = mlx4_setup_hca(dev);
 	}
@@ -3109,7 +3302,9 @@ err_port:
 	for (--port; port >= 1; --port)
 		mlx4_cleanup_port_info(&priv->port[port]);
 
-	mlx4_cleanup_counters_table(dev);
+	mlx4_cleanup_default_counters(dev);
+	if (!mlx4_is_slave(dev))
+		mlx4_cleanup_counters_table(dev);
 	mlx4_cleanup_qp_table(dev);
 	mlx4_cleanup_srq_table(dev);
 	mlx4_cleanup_cq_table(dev);
@@ -3407,7 +3602,9 @@ static void mlx4_unload_one(struct pci_dev *pdev)
 		mlx4_free_resource_tracker(dev,
 					   RES_TR_FREE_SLAVES_ONLY);
 
-	mlx4_cleanup_counters_table(dev);
+	mlx4_cleanup_default_counters(dev);
+	if (!mlx4_is_slave(dev))
+		mlx4_cleanup_counters_table(dev);
 	mlx4_cleanup_qp_table(dev);
 	mlx4_cleanup_srq_table(dev);
 	mlx4_cleanup_cq_table(dev);
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index 502d3dd2c888..a092c5c34d43 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -65,6 +65,8 @@
 
 #define INIT_HCA_TPT_MW_ENABLE          (1 << 7)
 
+#define MLX4_QUERY_IF_STAT_RESET	BIT(31)
+
 enum {
 	MLX4_HCR_BASE		= 0x80680,
 	MLX4_HCR_SIZE		= 0x0001c,
@@ -287,6 +289,12 @@ struct mlx4_icm_table {
 #define MLX4_CQE_SIZE_MASK_STRIDE	0x3
 #define MLX4_EQE_SIZE_MASK_STRIDE	0x30
 
+#define MLX4_EQ_ASYNC			0
+#define MLX4_EQ_TO_CQ_VECTOR(vector)	((vector) - \
+					 !!((int)(vector) >= MLX4_EQ_ASYNC))
+#define MLX4_CQ_TO_EQ_VECTOR(vector)	((vector) + \
+					 !!((int)(vector) >= MLX4_EQ_ASYNC))
+
 /*
  * Must be packed because mtt_seg is 64 bits but only aligned to 32 bits.
  */
@@ -391,6 +399,9 @@ struct mlx4_eq {
 	struct mlx4_buf_list   *page_list;
 	struct mlx4_mtt		mtt;
 	struct mlx4_eq_tasklet	tasklet_ctx;
+	struct mlx4_active_ports actv_ports;
+	u32			ref_count;
+	cpumask_var_t		affinity_mask;
 };
 
 struct mlx4_slave_eqe {
@@ -808,6 +819,7 @@ struct mlx4_port_info {
 	struct mlx4_vlan_table	vlan_table;
 	struct mlx4_roce_gid_table gid_table;
 	int			base_qpn;
+	struct cpu_rmap		*rmap;
 };
 
 struct mlx4_sense {
@@ -818,7 +830,7 @@ struct mlx4_sense {
 };
 
 struct mlx4_msix_ctl {
-	u64		pool_bm;
+	DECLARE_BITMAP(pool_bm, MAX_MSIX);
 	struct mutex	pool_lock;
 };
 
@@ -864,6 +876,7 @@ struct mlx4_priv {
 	struct mlx4_qp_table	qp_table;
 	struct mlx4_mcg_table	mcg_table;
 	struct mlx4_bitmap	counters_bitmap;
+	int			def_counter[MLX4_MAX_PORTS];
 
 	struct mlx4_catas_err	catas_err;
 
@@ -997,6 +1010,8 @@ int __mlx4_write_mtt(struct mlx4_dev *dev, struct mlx4_mtt *mtt,
 		     int start_index, int npages, u64 *page_list);
 int __mlx4_counter_alloc(struct mlx4_dev *dev, u32 *idx);
 void __mlx4_counter_free(struct mlx4_dev *dev, u32 idx);
+int mlx4_calc_vf_counters(struct mlx4_dev *dev, int slave, int port,
+			  struct mlx4_counter *data);
 int __mlx4_xrcd_alloc(struct mlx4_dev *dev, u32 *xrcdn);
 void __mlx4_xrcd_free(struct mlx4_dev *dev, u32 xrcdn);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index d021f079f181..666d1669eb52 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -279,6 +279,7 @@ struct mlx4_en_tx_ring {
 	u32			size; /* number of TXBBs */
 	u32			size_mask;
 	u16			stride;
+	u32			full_size;
 	u16			cqn;	/* index of port CQ associated with this ring */
 	u32			buf_size;
 	__be32			doorbell_qpn;
@@ -338,7 +339,7 @@ struct mlx4_en_cq {
 	struct napi_struct	napi;
 	int size;
 	int buf_size;
-	unsigned vector;
+	int vector;
 	enum cq_type is_tx;
 	u16 moder_time;
 	u16 moder_cnt;
@@ -566,6 +567,7 @@ struct mlx4_en_priv {
 #endif
 	struct mlx4_en_perf_stats pstats;
 	struct mlx4_en_pkt_stats pkstats;
+	struct mlx4_en_counter_stats pf_stats;
 	struct mlx4_en_flow_stats_rx rx_priority_flowstats[MLX4_NUM_PRIORITIES];
 	struct mlx4_en_flow_stats_tx tx_priority_flowstats[MLX4_NUM_PRIORITIES];
 	struct mlx4_en_flow_stats_rx rx_flowstats;
@@ -579,9 +581,9 @@ struct mlx4_en_priv {
 	int vids[128];
 	bool wol;
 	struct device *ddev;
-	int base_tx_qpn;
 	struct hlist_head mac_hash[MLX4_EN_MAC_HASH_SIZE];
 	struct hwtstamp_config hwtstamp_config;
+	u32 counter_index;
 
 #ifdef CONFIG_MLX4_EN_DCB
 	struct ieee_ets ets;
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_stats.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_stats.h
index 00555832a4ae..7fd466c0b929 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_stats.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_stats.h
@@ -23,6 +23,14 @@ struct mlx4_en_pkt_stats {
 #define NUM_PKT_STATS		43
 };
 
+struct mlx4_en_counter_stats {
+	unsigned long rx_packets;
+	unsigned long rx_bytes;
+	unsigned long tx_packets;
+	unsigned long tx_bytes;
+#define NUM_PF_STATS      4
+};
+
 struct mlx4_en_port_stats {
 	unsigned long tso_packets;
 	unsigned long xmit_more;
@@ -99,7 +107,7 @@ enum {
 };
 
 #define NUM_ALL_STATS	(NUM_MAIN_STATS + NUM_PORT_STATS + NUM_PKT_STATS + \
-			 NUM_FLOW_STATS + NUM_PERF_STATS)
+			 NUM_FLOW_STATS + NUM_PERF_STATS + NUM_PF_STATS)
 
 #define MLX4_FIND_NETDEV_STAT(n) (offsetof(struct net_device_stats, n) / \
 				  sizeof(((struct net_device_stats *)0)->n))
diff --git a/drivers/net/ethernet/mellanox/mlx4/profile.c b/drivers/net/ethernet/mellanox/mlx4/profile.c
index 2bf437aafc53..bae8b22edbb7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/profile.c
+++ b/drivers/net/ethernet/mellanox/mlx4/profile.c
@@ -82,7 +82,6 @@ u64 mlx4_make_profile(struct mlx4_dev *dev,
 
 	u64 total_size = 0;
 	struct mlx4_resource *profile;
-	struct mlx4_resource tmp;
 	struct sysinfo si;
 	int i, j;
 
@@ -149,11 +148,8 @@ u64 mlx4_make_profile(struct mlx4_dev *dev,
 	 */
 	for (i = MLX4_RES_NUM; i > 0; --i)
 		for (j = 1; j < i; ++j) {
-			if (profile[j].size > profile[j - 1].size) {
-				tmp	       = profile[j];
-				profile[j]     = profile[j - 1];
-				profile[j - 1] = tmp;
-			}
+			if (profile[j].size > profile[j - 1].size)
+				swap(profile[j], profile[j - 1]);
 		}
 
 	for (i = 0; i < MLX4_RES_NUM; ++i) {
diff --git a/drivers/net/ethernet/mellanox/mlx4/qp.c b/drivers/net/ethernet/mellanox/mlx4/qp.c
index b75214a80d0e..20268634a9ab 100644
--- a/drivers/net/ethernet/mellanox/mlx4/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx4/qp.c
@@ -749,7 +749,7 @@ int mlx4_init_qp_table(struct mlx4_dev *dev)
 
 	{
 		int sort[MLX4_NUM_QP_REGION];
-		int i, j, tmp;
+		int i, j;
 		int last_base = dev->caps.num_qps;
 
 		for (i = 1; i < MLX4_NUM_QP_REGION; ++i)
@@ -758,11 +758,8 @@ int mlx4_init_qp_table(struct mlx4_dev *dev)
 		for (i = MLX4_NUM_QP_REGION; i > MLX4_QP_REGION_BOTTOM; --i) {
 			for (j = MLX4_QP_REGION_BOTTOM + 2; j < i; ++j) {
 				if (dev->caps.reserved_qps_cnt[sort[j]] >
-				    dev->caps.reserved_qps_cnt[sort[j - 1]]) {
-					tmp             = sort[j];
-					sort[j]         = sort[j - 1];
-					sort[j - 1]     = tmp;
-				}
+				    dev->caps.reserved_qps_cnt[sort[j - 1]])
+					swap(sort[j], sort[j - 1]);
 			}
 		}
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index bafe2180cf0c..731423ca575d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -46,8 +46,11 @@
 
 #include "mlx4.h"
 #include "fw.h"
+#include "mlx4_stats.h"
 
 #define MLX4_MAC_VALID		(1ull << 63)
+#define MLX4_PF_COUNTERS_PER_PORT	2
+#define MLX4_VF_COUNTERS_PER_PORT	1
 
 struct mac_res {
 	struct list_head list;
@@ -459,11 +462,21 @@ void mlx4_init_quotas(struct mlx4_dev *dev)
 	dev->quotas.mpt =
 		priv->mfunc.master.res_tracker.res_alloc[RES_MPT].quota[pf];
 }
+
+static int get_max_gauranteed_vfs_counter(struct mlx4_dev *dev)
+{
+	/* reduce the sink counter */
+	return (dev->caps.max_counters - 1 -
+		(MLX4_PF_COUNTERS_PER_PORT * MLX4_MAX_PORTS))
+		/ MLX4_MAX_PORTS;
+}
+
 int mlx4_init_resource_tracker(struct mlx4_dev *dev)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	int i, j;
 	int t;
+	int max_vfs_guarantee_counter = get_max_gauranteed_vfs_counter(dev);
 
 	priv->mfunc.master.res_tracker.slave_list =
 		kzalloc(dev->num_slaves * sizeof(struct slave_list),
@@ -499,6 +512,9 @@ int mlx4_init_resource_tracker(struct mlx4_dev *dev)
 			res_alloc->allocated = kzalloc((dev->persist->
 							num_vfs + 1) *
 						       sizeof(int), GFP_KERNEL);
+		/* Reduce the sink counter */
+		if (i == RES_COUNTER)
+			res_alloc->res_free = dev->caps.max_counters - 1;
 
 		if (!res_alloc->quota || !res_alloc->guaranteed ||
 		    !res_alloc->allocated)
@@ -577,9 +593,17 @@ int mlx4_init_resource_tracker(struct mlx4_dev *dev)
 				break;
 			case RES_COUNTER:
 				res_alloc->quota[t] = dev->caps.max_counters;
-				res_alloc->guaranteed[t] = 0;
 				if (t == mlx4_master_func_num(dev))
-					res_alloc->res_free = res_alloc->quota[t];
+					res_alloc->guaranteed[t] =
+						MLX4_PF_COUNTERS_PER_PORT *
+						MLX4_MAX_PORTS;
+				else if (t <= max_vfs_guarantee_counter)
+					res_alloc->guaranteed[t] =
+						MLX4_VF_COUNTERS_PER_PORT *
+						MLX4_MAX_PORTS;
+				else
+					res_alloc->guaranteed[t] = 0;
+				res_alloc->res_free -= res_alloc->guaranteed[t];
 				break;
 			default:
 				break;
@@ -700,6 +724,9 @@ static void update_gid(struct mlx4_dev *dev, struct mlx4_cmd_mailbox *inbox,
 	}
 }
 
+static int handle_counter(struct mlx4_dev *dev, struct mlx4_qp_context *qpc,
+			  u8 slave, int port);
+
 static int update_vport_qp_param(struct mlx4_dev *dev,
 				 struct mlx4_cmd_mailbox *inbox,
 				 u8 slave, u32 qpn)
@@ -715,6 +742,10 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 	vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
 	qp_type	= (be32_to_cpu(qpc->flags) >> 16) & 0xff;
 
+	err = handle_counter(dev, qpc, slave, port);
+	if (err)
+		goto out;
+
 	if (MLX4_VGT != vp_oper->state.default_vlan) {
 		/* the reserved QPs (special, proxy, tunnel)
 		 * do not operate over vlans
@@ -859,6 +890,83 @@ static void put_res(struct mlx4_dev *dev, int slave, u64 res_id,
 	spin_unlock_irq(mlx4_tlock(dev));
 }
 
+static int counter_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
+			     u64 in_param, u64 *out_param, int port);
+
+static int handle_existing_counter(struct mlx4_dev *dev, u8 slave, int port,
+				   int counter_index)
+{
+	struct res_common *r;
+	struct res_counter *counter;
+	int ret = 0;
+
+	if (counter_index == MLX4_SINK_COUNTER_INDEX(dev))
+		return ret;
+
+	spin_lock_irq(mlx4_tlock(dev));
+	r = find_res(dev, counter_index, RES_COUNTER);
+	if (!r || r->owner != slave)
+		ret = -EINVAL;
+	counter = container_of(r, struct res_counter, com);
+	if (!counter->port)
+		counter->port = port;
+
+	spin_unlock_irq(mlx4_tlock(dev));
+	return ret;
+}
+
+static int handle_unexisting_counter(struct mlx4_dev *dev,
+				     struct mlx4_qp_context *qpc, u8 slave,
+				     int port)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_resource_tracker *tracker = &priv->mfunc.master.res_tracker;
+	struct res_common *tmp;
+	struct res_counter *counter;
+	u64 counter_idx = MLX4_SINK_COUNTER_INDEX(dev);
+	int err = 0;
+
+	spin_lock_irq(mlx4_tlock(dev));
+	list_for_each_entry(tmp,
+			    &tracker->slave_list[slave].res_list[RES_COUNTER],
+			    list) {
+		counter = container_of(tmp, struct res_counter, com);
+		if (port == counter->port) {
+			qpc->pri_path.counter_index  = counter->com.res_id;
+			spin_unlock_irq(mlx4_tlock(dev));
+			return 0;
+		}
+	}
+	spin_unlock_irq(mlx4_tlock(dev));
+
+	/* No existing counter, need to allocate a new counter */
+	err = counter_alloc_res(dev, slave, RES_OP_RESERVE, 0, 0, &counter_idx,
+				port);
+	if (err == -ENOENT) {
+		err = 0;
+	} else if (err && err != -ENOSPC) {
+		mlx4_err(dev, "%s: failed to create new counter for slave %d err %d\n",
+			 __func__, slave, err);
+	} else {
+		qpc->pri_path.counter_index = counter_idx;
+		mlx4_dbg(dev, "%s: alloc new counter for slave %d index %d\n",
+			 __func__, slave, qpc->pri_path.counter_index);
+		err = 0;
+	}
+
+	return err;
+}
+
+static int handle_counter(struct mlx4_dev *dev, struct mlx4_qp_context *qpc,
+			  u8 slave, int port)
+{
+	if (qpc->pri_path.counter_index != MLX4_SINK_COUNTER_INDEX(dev))
+		return handle_existing_counter(dev, slave, port,
+					       qpc->pri_path.counter_index);
+
+	return handle_unexisting_counter(dev, qpc, slave, port);
+}
+
 static struct res_common *alloc_qp_tr(int id)
 {
 	struct res_qp *ret;
@@ -952,7 +1060,7 @@ static struct res_common *alloc_srq_tr(int id)
 	return &ret->com;
 }
 
-static struct res_common *alloc_counter_tr(int id)
+static struct res_common *alloc_counter_tr(int id, int port)
 {
 	struct res_counter *ret;
 
@@ -962,6 +1070,7 @@ static struct res_common *alloc_counter_tr(int id)
 
 	ret->com.res_id = id;
 	ret->com.state = RES_COUNTER_ALLOCATED;
+	ret->port = port;
 
 	return &ret->com;
 }
@@ -1022,7 +1131,7 @@ static struct res_common *alloc_tr(u64 id, enum mlx4_resource type, int slave,
 		pr_err("implementation missing\n");
 		return NULL;
 	case RES_COUNTER:
-		ret = alloc_counter_tr(id);
+		ret = alloc_counter_tr(id, extra);
 		break;
 	case RES_XRCD:
 		ret = alloc_xrcdn_tr(id);
@@ -1039,6 +1148,53 @@ static struct res_common *alloc_tr(u64 id, enum mlx4_resource type, int slave,
 	return ret;
 }
 
+int mlx4_calc_vf_counters(struct mlx4_dev *dev, int slave, int port,
+			  struct mlx4_counter *data)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_resource_tracker *tracker = &priv->mfunc.master.res_tracker;
+	struct res_common *tmp;
+	struct res_counter *counter;
+	int *counters_arr;
+	int i = 0, err = 0;
+
+	memset(data, 0, sizeof(*data));
+
+	counters_arr = kmalloc_array(dev->caps.max_counters,
+				     sizeof(*counters_arr), GFP_KERNEL);
+	if (!counters_arr)
+		return -ENOMEM;
+
+	spin_lock_irq(mlx4_tlock(dev));
+	list_for_each_entry(tmp,
+			    &tracker->slave_list[slave].res_list[RES_COUNTER],
+			    list) {
+		counter = container_of(tmp, struct res_counter, com);
+		if (counter->port == port) {
+			counters_arr[i] = (int)tmp->res_id;
+			i++;
+		}
+	}
+	spin_unlock_irq(mlx4_tlock(dev));
+	counters_arr[i] = -1;
+
+	i = 0;
+
+	while (counters_arr[i] != -1) {
+		err = mlx4_get_counter_stats(dev, counters_arr[i], data,
+					     0);
+		if (err) {
+			memset(data, 0, sizeof(*data));
+			goto table_changed;
+		}
+		i++;
+	}
+
+table_changed:
+	kfree(counters_arr);
+	return 0;
+}
+
 static int add_res_range(struct mlx4_dev *dev, int slave, u64 base, int count,
 			 enum mlx4_resource type, int extra)
 {
@@ -2001,7 +2157,7 @@ static int vlan_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
 }
 
 static int counter_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
-			     u64 in_param, u64 *out_param)
+			     u64 in_param, u64 *out_param, int port)
 {
 	u32 index;
 	int err;
@@ -2019,7 +2175,7 @@ static int counter_alloc_res(struct mlx4_dev *dev, int slave, int op, int cmd,
 		return err;
 	}
 
-	err = add_res_range(dev, slave, index, 1, RES_COUNTER, 0);
+	err = add_res_range(dev, slave, index, 1, RES_COUNTER, port);
 	if (err) {
 		__mlx4_counter_free(dev, index);
 		mlx4_release_resource(dev, slave, RES_COUNTER, 1, 0);
@@ -2101,7 +2257,7 @@ int mlx4_ALLOC_RES_wrapper(struct mlx4_dev *dev, int slave,
 
 	case RES_COUNTER:
 		err = counter_alloc_res(dev, slave, vhcr->op_modifier, alop,
-					vhcr->in_param, &vhcr->out_param);
+					vhcr->in_param, &vhcr->out_param, 0);
 		break;
 
 	case RES_XRCD:
@@ -2335,6 +2491,9 @@ static int counter_free_res(struct mlx4_dev *dev, int slave, int op, int cmd,
 		return -EINVAL;
 
 	index = get_param_l(&in_param);
+	if (index == MLX4_SINK_COUNTER_INDEX(dev))
+		return 0;
+
 	err = rem_res_range(dev, slave, index, 1, RES_COUNTER, 0);
 	if (err)
 		return err;
@@ -2703,6 +2862,10 @@ static void adjust_proxy_tun_qkey(struct mlx4_dev *dev, struct mlx4_vhcr *vhcr,
 	context->qkey = cpu_to_be32(qkey);
 }
 
+static int adjust_qp_sched_queue(struct mlx4_dev *dev, int slave,
+				 struct mlx4_qp_context *qpc,
+				 struct mlx4_cmd_mailbox *inbox);
+
 int mlx4_RST2INIT_QP_wrapper(struct mlx4_dev *dev, int slave,
 			     struct mlx4_vhcr *vhcr,
 			     struct mlx4_cmd_mailbox *inbox,
@@ -2725,6 +2888,10 @@ int mlx4_RST2INIT_QP_wrapper(struct mlx4_dev *dev, int slave,
 	struct res_srq *srq;
 	int local_qpn = be32_to_cpu(qpc->local_qpn) & 0xffffff;
 
+	err = adjust_qp_sched_queue(dev, slave, qpc, inbox);
+	if (err)
+		return err;
+
 	err = qp_res_start_move_to(dev, slave, qpn, RES_QP_HW, &qp, 0);
 	if (err)
 		return err;
@@ -3526,8 +3693,8 @@ static int adjust_qp_sched_queue(struct mlx4_dev *dev, int slave,
 	pri_sched_queue = (qpc->pri_path.sched_queue & ~(1 << 6)) |
 			  ((port & 1) << 6);
 
-	if (optpar & MLX4_QP_OPTPAR_PRIMARY_ADDR_PATH ||
-	    mlx4_is_eth(dev, port + 1)) {
+	if (optpar & (MLX4_QP_OPTPAR_PRIMARY_ADDR_PATH | MLX4_QP_OPTPAR_SCHED_QUEUE) ||
+	    qpc->pri_path.sched_queue || mlx4_is_eth(dev, port + 1)) {
 		qpc->pri_path.sched_queue = pri_sched_queue;
 	}
 
@@ -3965,6 +4132,22 @@ static int validate_eth_header_mac(int slave, struct _rule_hw *eth_header,
 	return 0;
 }
 
+static void handle_eth_header_mcast_prio(struct mlx4_net_trans_rule_hw_ctrl *ctrl,
+					 struct _rule_hw *eth_header)
+{
+	if (is_multicast_ether_addr(eth_header->eth.dst_mac) ||
+	    is_broadcast_ether_addr(eth_header->eth.dst_mac)) {
+		struct mlx4_net_trans_rule_hw_eth *eth =
+			(struct mlx4_net_trans_rule_hw_eth *)eth_header;
+		struct _rule_hw *next_rule = (struct _rule_hw *)(eth + 1);
+		bool last_rule = next_rule->size == 0 && next_rule->id == 0 &&
+			next_rule->rsvd == 0;
+
+		if (last_rule)
+			ctrl->prio = cpu_to_be16(MLX4_DOMAIN_NIC);
+	}
+}
+
 /*
  * In case of missing eth header, append eth header with a MAC address
  * assigned to the VF.
@@ -4117,6 +4300,12 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 	rule_header = (struct _rule_hw *)(ctrl + 1);
 	header_id = map_hw_to_sw_id(be16_to_cpu(rule_header->id));
 
+	if (header_id == MLX4_NET_TRANS_RULE_ID_ETH)
+		handle_eth_header_mcast_prio(ctrl, rule_header);
+
+	if (slave == dev->caps.function)
+		goto execute;
+
 	switch (header_id) {
 	case MLX4_NET_TRANS_RULE_ID_ETH:
 		if (validate_eth_header_mac(slave, rule_header, rlist)) {
@@ -4143,6 +4332,7 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 		goto err_put;
 	}
 
+execute:
 	err = mlx4_cmd_imm(dev, inbox->dma, &vhcr->out_param,
 			   vhcr->in_modifier, 0,
 			   MLX4_QP_FLOW_STEERING_ATTACH, MLX4_CMD_TIME_CLASS_A,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Kconfig b/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
index 8ff57e8e3e91..158c88c69ef9 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
@@ -3,6 +3,18 @@
 #
 
 config MLX5_CORE
-	tristate
+	tristate "Mellanox Technologies ConnectX-4 and Connect-IB core driver"
 	depends on PCI
 	default n
+	---help---
+	  Core driver for low level functionality of the ConnectX-4 and
+	  Connect-IB cards by Mellanox Technologies.
+
+config MLX5_CORE_EN
+	bool "Mellanox Technologies ConnectX-4 Ethernet support"
+	depends on NETDEVICES && ETHERNET && PCI && MLX5_CORE
+	default n
+	---help---
+	  Ethernet support in Mellanox Technologies ConnectX-4 NIC.
+	  Ethernet and Infiniband support in ConnectX-4 are currently mutually
+	  exclusive.
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Makefile b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
index 105780bb980b..26a68b8af2c5 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
@@ -2,4 +2,7 @@ obj-$(CONFIG_MLX5_CORE)		+= mlx5_core.o
 
 mlx5_core-y :=	main.o cmd.o debugfs.o fw.o eq.o uar.o pagealloc.o \
 		health.o mcg.o cq.o srq.o alloc.o qp.o port.o mr.o pd.o   \
-		mad.o
+		mad.o transobj.o vport.o
+mlx5_core-$(CONFIG_MLX5_CORE_EN) += wq.o flow_table.o \
+		en_main.o en_flow_table.o en_ethtool.o en_tx.o en_rx.o \
+		en_txrx.o
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/alloc.c b/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
index ac0f7bf4be95..0715b497511f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/alloc.c
@@ -42,95 +42,36 @@
 #include "mlx5_core.h"
 
 /* Handling for queue buffers -- we allocate a bunch of memory and
- * register it in a memory region at HCA virtual address 0.  If the
- * requested size is > max_direct, we split the allocation into
- * multiple pages, so we don't require too much contiguous memory.
+ * register it in a memory region at HCA virtual address 0.
  */
 
-int mlx5_buf_alloc(struct mlx5_core_dev *dev, int size, int max_direct,
-		   struct mlx5_buf *buf)
+int mlx5_buf_alloc(struct mlx5_core_dev *dev, int size, struct mlx5_buf *buf)
 {
 	dma_addr_t t;
 
 	buf->size = size;
-	if (size <= max_direct) {
-		buf->nbufs        = 1;
-		buf->npages       = 1;
-		buf->page_shift   = (u8)get_order(size) + PAGE_SHIFT;
-		buf->direct.buf   = dma_zalloc_coherent(&dev->pdev->dev,
-							size, &t, GFP_KERNEL);
-		if (!buf->direct.buf)
-			return -ENOMEM;
-
-		buf->direct.map = t;
-
-		while (t & ((1 << buf->page_shift) - 1)) {
-			--buf->page_shift;
-			buf->npages *= 2;
-		}
-	} else {
-		int i;
-
-		buf->direct.buf  = NULL;
-		buf->nbufs       = (size + PAGE_SIZE - 1) / PAGE_SIZE;
-		buf->npages      = buf->nbufs;
-		buf->page_shift  = PAGE_SHIFT;
-		buf->page_list   = kcalloc(buf->nbufs, sizeof(*buf->page_list),
-					   GFP_KERNEL);
-		if (!buf->page_list)
-			return -ENOMEM;
-
-		for (i = 0; i < buf->nbufs; i++) {
-			buf->page_list[i].buf =
-				dma_zalloc_coherent(&dev->pdev->dev, PAGE_SIZE,
-						    &t, GFP_KERNEL);
-			if (!buf->page_list[i].buf)
-				goto err_free;
-
-			buf->page_list[i].map = t;
-		}
-
-		if (BITS_PER_LONG == 64) {
-			struct page **pages;
-			pages = kmalloc(sizeof(*pages) * buf->nbufs, GFP_KERNEL);
-			if (!pages)
-				goto err_free;
-			for (i = 0; i < buf->nbufs; i++)
-				pages[i] = virt_to_page(buf->page_list[i].buf);
-			buf->direct.buf = vmap(pages, buf->nbufs, VM_MAP, PAGE_KERNEL);
-			kfree(pages);
-			if (!buf->direct.buf)
-				goto err_free;
-		}
-	}
+	buf->npages       = 1;
+	buf->page_shift   = (u8)get_order(size) + PAGE_SHIFT;
+	buf->direct.buf   = dma_zalloc_coherent(&dev->pdev->dev,
+						size, &t, GFP_KERNEL);
+	if (!buf->direct.buf)
+		return -ENOMEM;
 
-	return 0;
+	buf->direct.map = t;
 
-err_free:
-	mlx5_buf_free(dev, buf);
+	while (t & ((1 << buf->page_shift) - 1)) {
+		--buf->page_shift;
+		buf->npages *= 2;
+	}
 
-	return -ENOMEM;
+	return 0;
 }
 EXPORT_SYMBOL_GPL(mlx5_buf_alloc);
 
 void mlx5_buf_free(struct mlx5_core_dev *dev, struct mlx5_buf *buf)
 {
-	int i;
-
-	if (buf->nbufs == 1)
-		dma_free_coherent(&dev->pdev->dev, buf->size, buf->direct.buf,
-				  buf->direct.map);
-	else {
-		if (BITS_PER_LONG == 64)
-			vunmap(buf->direct.buf);
-
-		for (i = 0; i < buf->nbufs; i++)
-			if (buf->page_list[i].buf)
-				dma_free_coherent(&dev->pdev->dev, PAGE_SIZE,
-						  buf->page_list[i].buf,
-						  buf->page_list[i].map);
-		kfree(buf->page_list);
-	}
+	dma_free_coherent(&dev->pdev->dev, buf->size, buf->direct.buf,
+			  buf->direct.map);
 }
 EXPORT_SYMBOL_GPL(mlx5_buf_free);
 
@@ -230,10 +171,7 @@ void mlx5_fill_page_array(struct mlx5_buf *buf, __be64 *pas)
 	int i;
 
 	for (i = 0; i < buf->npages; i++) {
-		if (buf->nbufs == 1)
-			addr = buf->direct.map + (i << buf->page_shift);
-		else
-			addr = buf->page_list[i].map;
+		addr = buf->direct.map + (i << buf->page_shift);
 
 		pas[i] = cpu_to_be64(addr);
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
index e3273faf4568..75ff58dc1ff5 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
@@ -75,25 +75,6 @@ enum {
 	MLX5_CMD_DELIVERY_STAT_CMD_DESCR_ERR		= 0x10,
 };
 
-enum {
-	MLX5_CMD_STAT_OK			= 0x0,
-	MLX5_CMD_STAT_INT_ERR			= 0x1,
-	MLX5_CMD_STAT_BAD_OP_ERR		= 0x2,
-	MLX5_CMD_STAT_BAD_PARAM_ERR		= 0x3,
-	MLX5_CMD_STAT_BAD_SYS_STATE_ERR		= 0x4,
-	MLX5_CMD_STAT_BAD_RES_ERR		= 0x5,
-	MLX5_CMD_STAT_RES_BUSY			= 0x6,
-	MLX5_CMD_STAT_LIM_ERR			= 0x8,
-	MLX5_CMD_STAT_BAD_RES_STATE_ERR		= 0x9,
-	MLX5_CMD_STAT_IX_ERR			= 0xa,
-	MLX5_CMD_STAT_NO_RES_ERR		= 0xf,
-	MLX5_CMD_STAT_BAD_INP_LEN_ERR		= 0x50,
-	MLX5_CMD_STAT_BAD_OUTP_LEN_ERR		= 0x51,
-	MLX5_CMD_STAT_BAD_QP_STATE_ERR		= 0x10,
-	MLX5_CMD_STAT_BAD_PKT_ERR		= 0x30,
-	MLX5_CMD_STAT_BAD_SIZE_OUTS_CQES_ERR	= 0x40,
-};
-
 static struct mlx5_cmd_work_ent *alloc_cmd(struct mlx5_cmd *cmd,
 					   struct mlx5_cmd_msg *in,
 					   struct mlx5_cmd_msg *out,
@@ -390,8 +371,17 @@ const char *mlx5_command_str(int command)
 	case MLX5_CMD_OP_ARM_RQ:
 		return "ARM_RQ";
 
-	case MLX5_CMD_OP_RESIZE_SRQ:
-		return "RESIZE_SRQ";
+	case MLX5_CMD_OP_CREATE_XRC_SRQ:
+		return "CREATE_XRC_SRQ";
+
+	case MLX5_CMD_OP_DESTROY_XRC_SRQ:
+		return "DESTROY_XRC_SRQ";
+
+	case MLX5_CMD_OP_QUERY_XRC_SRQ:
+		return "QUERY_XRC_SRQ";
+
+	case MLX5_CMD_OP_ARM_XRC_SRQ:
+		return "ARM_XRC_SRQ";
 
 	case MLX5_CMD_OP_ALLOC_PD:
 		return "ALLOC_PD";
@@ -408,8 +398,8 @@ const char *mlx5_command_str(int command)
 	case MLX5_CMD_OP_ATTACH_TO_MCG:
 		return "ATTACH_TO_MCG";
 
-	case MLX5_CMD_OP_DETACH_FROM_MCG:
-		return "DETACH_FROM_MCG";
+	case MLX5_CMD_OP_DETTACH_FROM_MCG:
+		return "DETTACH_FROM_MCG";
 
 	case MLX5_CMD_OP_ALLOC_XRCD:
 		return "ALLOC_XRCD";
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cq.c b/drivers/net/ethernet/mellanox/mlx5/core/cq.c
index eb0cf81f5f45..04ab7e445eae 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cq.c
@@ -219,6 +219,24 @@ int mlx5_core_modify_cq(struct mlx5_core_dev *dev, struct mlx5_core_cq *cq,
 }
 EXPORT_SYMBOL(mlx5_core_modify_cq);
 
+int mlx5_core_modify_cq_moderation(struct mlx5_core_dev *dev,
+				   struct mlx5_core_cq *cq,
+				   u16 cq_period,
+				   u16 cq_max_count)
+{
+	struct mlx5_modify_cq_mbox_in in;
+
+	memset(&in, 0, sizeof(in));
+
+	in.cqn              = cpu_to_be32(cq->cqn);
+	in.ctx.cq_period    = cpu_to_be16(cq_period);
+	in.ctx.cq_max_count = cpu_to_be16(cq_max_count);
+	in.field_select     = cpu_to_be32(MLX5_CQ_MODIFY_PERIOD |
+					  MLX5_CQ_MODIFY_COUNT);
+
+	return mlx5_core_modify_cq(dev, cq, &in, sizeof(in));
+}
+
 int mlx5_init_cq_table(struct mlx5_core_dev *dev)
 {
 	struct mlx5_cq_table *table = &dev->priv.cq_table;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
new file mode 100644
index 000000000000..3d23bd657e3c
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -0,0 +1,521 @@
+/*
+ * Copyright (c) 2015, 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/if_vlan.h>
+#include <linux/etherdevice.h>
+#include <linux/mlx5/driver.h>
+#include <linux/mlx5/qp.h>
+#include <linux/mlx5/cq.h>
+#include <linux/mlx5/vport.h>
+#include "wq.h"
+#include "transobj.h"
+#include "mlx5_core.h"
+
+#define MLX5E_MAX_NUM_TC	8
+
+#define MLX5E_PARAMS_MINIMUM_LOG_SQ_SIZE                0x7
+#define MLX5E_PARAMS_DEFAULT_LOG_SQ_SIZE                0xa
+#define MLX5E_PARAMS_MAXIMUM_LOG_SQ_SIZE                0xd
+
+#define MLX5E_PARAMS_MINIMUM_LOG_RQ_SIZE                0x7
+#define MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE                0xa
+#define MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE                0xd
+
+#define MLX5E_PARAMS_DEFAULT_LRO_WQE_SZ                 (16 * 1024)
+#define MLX5E_PARAMS_DEFAULT_RX_CQ_MODERATION_USEC      0x10
+#define MLX5E_PARAMS_DEFAULT_RX_CQ_MODERATION_PKTS      0x20
+#define MLX5E_PARAMS_DEFAULT_TX_CQ_MODERATION_USEC      0x10
+#define MLX5E_PARAMS_DEFAULT_TX_CQ_MODERATION_PKTS      0x20
+#define MLX5E_PARAMS_DEFAULT_MIN_RX_WQES                0x80
+#define MLX5E_PARAMS_DEFAULT_RX_HASH_LOG_TBL_SZ         0x7
+
+#define MLX5E_TX_CQ_POLL_BUDGET        128
+#define MLX5E_UPDATE_STATS_INTERVAL    200 /* msecs */
+
+static const char vport_strings[][ETH_GSTRING_LEN] = {
+	/* vport statistics */
+	"rx_packets",
+	"rx_bytes",
+	"tx_packets",
+	"tx_bytes",
+	"rx_error_packets",
+	"rx_error_bytes",
+	"tx_error_packets",
+	"tx_error_bytes",
+	"rx_unicast_packets",
+	"rx_unicast_bytes",
+	"tx_unicast_packets",
+	"tx_unicast_bytes",
+	"rx_multicast_packets",
+	"rx_multicast_bytes",
+	"tx_multicast_packets",
+	"tx_multicast_bytes",
+	"rx_broadcast_packets",
+	"rx_broadcast_bytes",
+	"tx_broadcast_packets",
+	"tx_broadcast_bytes",
+
+	/* SW counters */
+	"tso_packets",
+	"tso_bytes",
+	"lro_packets",
+	"lro_bytes",
+	"rx_csum_good",
+	"rx_csum_none",
+	"tx_csum_offload",
+	"tx_queue_stopped",
+	"tx_queue_wake",
+	"tx_queue_dropped",
+	"rx_wqe_err",
+};
+
+struct mlx5e_vport_stats {
+	/* HW counters */
+	u64 rx_packets;
+	u64 rx_bytes;
+	u64 tx_packets;
+	u64 tx_bytes;
+	u64 rx_error_packets;
+	u64 rx_error_bytes;
+	u64 tx_error_packets;
+	u64 tx_error_bytes;
+	u64 rx_unicast_packets;
+	u64 rx_unicast_bytes;
+	u64 tx_unicast_packets;
+	u64 tx_unicast_bytes;
+	u64 rx_multicast_packets;
+	u64 rx_multicast_bytes;
+	u64 tx_multicast_packets;
+	u64 tx_multicast_bytes;
+	u64 rx_broadcast_packets;
+	u64 rx_broadcast_bytes;
+	u64 tx_broadcast_packets;
+	u64 tx_broadcast_bytes;
+
+	/* SW counters */
+	u64 tso_packets;
+	u64 tso_bytes;
+	u64 lro_packets;
+	u64 lro_bytes;
+	u64 rx_csum_good;
+	u64 rx_csum_none;
+	u64 tx_csum_offload;
+	u64 tx_queue_stopped;
+	u64 tx_queue_wake;
+	u64 tx_queue_dropped;
+	u64 rx_wqe_err;
+
+#define NUM_VPORT_COUNTERS     31
+};
+
+static const char rq_stats_strings[][ETH_GSTRING_LEN] = {
+	"packets",
+	"csum_none",
+	"lro_packets",
+	"lro_bytes",
+	"wqe_err"
+};
+
+struct mlx5e_rq_stats {
+	u64 packets;
+	u64 csum_none;
+	u64 lro_packets;
+	u64 lro_bytes;
+	u64 wqe_err;
+#define NUM_RQ_STATS 5
+};
+
+static const char sq_stats_strings[][ETH_GSTRING_LEN] = {
+	"packets",
+	"tso_packets",
+	"tso_bytes",
+	"csum_offload_none",
+	"stopped",
+	"wake",
+	"dropped",
+	"nop"
+};
+
+struct mlx5e_sq_stats {
+	u64 packets;
+	u64 tso_packets;
+	u64 tso_bytes;
+	u64 csum_offload_none;
+	u64 stopped;
+	u64 wake;
+	u64 dropped;
+	u64 nop;
+#define NUM_SQ_STATS 8
+};
+
+struct mlx5e_stats {
+	struct mlx5e_vport_stats   vport;
+};
+
+struct mlx5e_params {
+	u8  log_sq_size;
+	u8  log_rq_size;
+	u16 num_channels;
+	u8  default_vlan_prio;
+	u8  num_tc;
+	u16 rx_cq_moderation_usec;
+	u16 rx_cq_moderation_pkts;
+	u16 tx_cq_moderation_usec;
+	u16 tx_cq_moderation_pkts;
+	u16 min_rx_wqes;
+	u16 rx_hash_log_tbl_sz;
+	bool lro_en;
+	u32 lro_wqe_sz;
+};
+
+enum {
+	MLX5E_RQ_STATE_POST_WQES_ENABLE,
+};
+
+enum cq_flags {
+	MLX5E_CQ_HAS_CQES = 1,
+};
+
+struct mlx5e_cq {
+	/* data path - accessed per cqe */
+	struct mlx5_cqwq           wq;
+	unsigned long              flags;
+
+	/* data path - accessed per napi poll */
+	struct napi_struct        *napi;
+	struct mlx5_core_cq        mcq;
+	struct mlx5e_channel      *channel;
+
+	/* control */
+	struct mlx5_wq_ctrl        wq_ctrl;
+} ____cacheline_aligned_in_smp;
+
+struct mlx5e_rq {
+	/* data path */
+	struct mlx5_wq_ll      wq;
+	u32                    wqe_sz;
+	struct sk_buff       **skb;
+
+	struct device         *pdev;
+	struct net_device     *netdev;
+	struct mlx5e_rq_stats  stats;
+	struct mlx5e_cq        cq;
+
+	unsigned long          state;
+	int                    ix;
+
+	/* control */
+	struct mlx5_wq_ctrl    wq_ctrl;
+	u32                    rqn;
+	struct mlx5e_channel  *channel;
+} ____cacheline_aligned_in_smp;
+
+struct mlx5e_tx_skb_cb {
+	u32 num_bytes;
+	u8  num_wqebbs;
+	u8  num_dma;
+};
+
+#define MLX5E_TX_SKB_CB(__skb) ((struct mlx5e_tx_skb_cb *)__skb->cb)
+
+struct mlx5e_sq_dma {
+	dma_addr_t addr;
+	u32        size;
+};
+
+enum {
+	MLX5E_SQ_STATE_WAKE_TXQ_ENABLE,
+};
+
+struct mlx5e_sq {
+	/* data path */
+
+	/* dirtied @completion */
+	u16                        cc;
+	u32                        dma_fifo_cc;
+
+	/* dirtied @xmit */
+	u16                        pc ____cacheline_aligned_in_smp;
+	u32                        dma_fifo_pc;
+	u32                        bf_offset;
+	struct mlx5e_sq_stats      stats;
+
+	struct mlx5e_cq            cq;
+
+	/* pointers to per packet info: write@xmit, read@completion */
+	struct sk_buff           **skb;
+	struct mlx5e_sq_dma       *dma_fifo;
+
+	/* read only */
+	struct mlx5_wq_cyc         wq;
+	u32                        dma_fifo_mask;
+	void __iomem              *uar_map;
+	struct netdev_queue       *txq;
+	u32                        sqn;
+	u32                        bf_buf_size;
+	u16                        max_inline;
+	u16                        edge;
+	struct device             *pdev;
+	__be32                     mkey_be;
+	unsigned long              state;
+
+	/* control path */
+	struct mlx5_wq_ctrl        wq_ctrl;
+	struct mlx5_uar            uar;
+	struct mlx5e_channel      *channel;
+	int                        tc;
+} ____cacheline_aligned_in_smp;
+
+static inline bool mlx5e_sq_has_room_for(struct mlx5e_sq *sq, u16 n)
+{
+	return (((sq->wq.sz_m1 & (sq->cc - sq->pc)) >= n) ||
+		(sq->cc  == sq->pc));
+}
+
+enum channel_flags {
+	MLX5E_CHANNEL_NAPI_SCHED = 1,
+};
+
+struct mlx5e_channel {
+	/* data path */
+	struct mlx5e_rq            rq;
+	struct mlx5e_sq            sq[MLX5E_MAX_NUM_TC];
+	struct napi_struct         napi;
+	struct device             *pdev;
+	struct net_device         *netdev;
+	__be32                     mkey_be;
+	u8                         num_tc;
+	unsigned long              flags;
+	int                        tc_to_txq_map[MLX5E_MAX_NUM_TC];
+
+	/* control */
+	struct mlx5e_priv         *priv;
+	int                        ix;
+	int                        cpu;
+};
+
+enum mlx5e_traffic_types {
+	MLX5E_TT_IPV4_TCP = 0,
+	MLX5E_TT_IPV6_TCP = 1,
+	MLX5E_TT_IPV4_UDP = 2,
+	MLX5E_TT_IPV6_UDP = 3,
+	MLX5E_TT_IPV4     = 4,
+	MLX5E_TT_IPV6     = 5,
+	MLX5E_TT_ANY      = 6,
+	MLX5E_NUM_TT      = 7,
+};
+
+enum {
+	MLX5E_RQT_SPREADING  = 0,
+	MLX5E_RQT_DEFAULT_RQ = 1,
+	MLX5E_NUM_RQT        = 2,
+};
+
+struct mlx5e_eth_addr_info {
+	u8  addr[ETH_ALEN + 2];
+	u32 tt_vec;
+	u32 ft_ix[MLX5E_NUM_TT]; /* flow table index per traffic type */
+};
+
+#define MLX5E_ETH_ADDR_HASH_SIZE (1 << BITS_PER_BYTE)
+
+struct mlx5e_eth_addr_db {
+	struct hlist_head          netdev_uc[MLX5E_ETH_ADDR_HASH_SIZE];
+	struct hlist_head          netdev_mc[MLX5E_ETH_ADDR_HASH_SIZE];
+	struct mlx5e_eth_addr_info broadcast;
+	struct mlx5e_eth_addr_info allmulti;
+	struct mlx5e_eth_addr_info promisc;
+	bool                       broadcast_enabled;
+	bool                       allmulti_enabled;
+	bool                       promisc_enabled;
+};
+
+enum {
+	MLX5E_STATE_ASYNC_EVENTS_ENABLE,
+	MLX5E_STATE_OPENED,
+};
+
+struct mlx5e_vlan_db {
+	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
+	u32           active_vlans_ft_ix[VLAN_N_VID];
+	u32           untagged_rule_ft_ix;
+	u32           any_vlan_rule_ft_ix;
+	bool          filter_disabled;
+};
+
+struct mlx5e_flow_table {
+	void *vlan;
+	void *main;
+};
+
+struct mlx5e_priv {
+	/* priv data path fields - start */
+	int                        num_tc;
+	int                        default_vlan_prio;
+	struct mlx5e_sq            **txq_to_sq_map;
+	/* priv data path fields - end */
+
+	unsigned long              state;
+	struct mutex               state_lock; /* Protects Interface state */
+	struct mlx5_uar            cq_uar;
+	u32                        pdn;
+	u32                        tdn;
+	struct mlx5_core_mr        mr;
+
+	struct mlx5e_channel     **channel;
+	u32                        tisn[MLX5E_MAX_NUM_TC];
+	u32                        rqtn;
+	u32                        tirn[MLX5E_NUM_TT];
+
+	struct mlx5e_flow_table    ft;
+	struct mlx5e_eth_addr_db   eth_addr;
+	struct mlx5e_vlan_db       vlan;
+
+	struct mlx5e_params        params;
+	spinlock_t                 async_events_spinlock; /* sync hw events */
+	struct work_struct         update_carrier_work;
+	struct work_struct         set_rx_mode_work;
+	struct delayed_work        update_stats_work;
+
+	struct mlx5_core_dev      *mdev;
+	struct net_device         *netdev;
+	struct mlx5e_stats         stats;
+};
+
+#define MLX5E_NET_IP_ALIGN 2
+
+struct mlx5e_tx_wqe {
+	struct mlx5_wqe_ctrl_seg ctrl;
+	struct mlx5_wqe_eth_seg  eth;
+};
+
+struct mlx5e_rx_wqe {
+	struct mlx5_wqe_srq_next_seg  next;
+	struct mlx5_wqe_data_seg      data;
+};
+
+enum mlx5e_link_mode {
+	MLX5E_1000BASE_CX_SGMII	 = 0,
+	MLX5E_1000BASE_KX	 = 1,
+	MLX5E_10GBASE_CX4	 = 2,
+	MLX5E_10GBASE_KX4	 = 3,
+	MLX5E_10GBASE_KR	 = 4,
+	MLX5E_20GBASE_KR2	 = 5,
+	MLX5E_40GBASE_CR4	 = 6,
+	MLX5E_40GBASE_KR4	 = 7,
+	MLX5E_56GBASE_R4	 = 8,
+	MLX5E_10GBASE_CR	 = 12,
+	MLX5E_10GBASE_SR	 = 13,
+	MLX5E_10GBASE_ER	 = 14,
+	MLX5E_40GBASE_SR4	 = 15,
+	MLX5E_40GBASE_LR4	 = 16,
+	MLX5E_100GBASE_CR4	 = 20,
+	MLX5E_100GBASE_SR4	 = 21,
+	MLX5E_100GBASE_KR4	 = 22,
+	MLX5E_100GBASE_LR4	 = 23,
+	MLX5E_100BASE_TX	 = 24,
+	MLX5E_100BASE_T		 = 25,
+	MLX5E_10GBASE_T		 = 26,
+	MLX5E_25GBASE_CR	 = 27,
+	MLX5E_25GBASE_KR	 = 28,
+	MLX5E_25GBASE_SR	 = 29,
+	MLX5E_50GBASE_CR2	 = 30,
+	MLX5E_50GBASE_KR2	 = 31,
+	MLX5E_LINK_MODES_NUMBER,
+};
+
+#define MLX5E_PROT_MASK(link_mode) (1 << link_mode)
+
+void mlx5e_send_nop(struct mlx5e_sq *sq, bool notify_hw);
+u16 mlx5e_select_queue(struct net_device *dev, struct sk_buff *skb,
+		       void *accel_priv, select_queue_fallback_t fallback);
+netdev_tx_t mlx5e_xmit(struct sk_buff *skb, struct net_device *dev);
+
+void mlx5e_completion_event(struct mlx5_core_cq *mcq);
+void mlx5e_cq_error_event(struct mlx5_core_cq *mcq, enum mlx5_event event);
+int mlx5e_napi_poll(struct napi_struct *napi, int budget);
+bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq);
+bool mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget);
+bool mlx5e_post_rx_wqes(struct mlx5e_rq *rq);
+struct mlx5_cqe64 *mlx5e_get_cqe(struct mlx5e_cq *cq);
+
+void mlx5e_update_stats(struct mlx5e_priv *priv);
+
+int mlx5e_open_flow_table(struct mlx5e_priv *priv);
+void mlx5e_close_flow_table(struct mlx5e_priv *priv);
+void mlx5e_init_eth_addr(struct mlx5e_priv *priv);
+void mlx5e_set_rx_mode_core(struct mlx5e_priv *priv);
+void mlx5e_set_rx_mode_work(struct work_struct *work);
+
+int mlx5e_vlan_rx_add_vid(struct net_device *dev, __always_unused __be16 proto,
+			  u16 vid);
+int mlx5e_vlan_rx_kill_vid(struct net_device *dev, __always_unused __be16 proto,
+			   u16 vid);
+void mlx5e_enable_vlan_filter(struct mlx5e_priv *priv);
+void mlx5e_disable_vlan_filter(struct mlx5e_priv *priv);
+int mlx5e_add_all_vlan_rules(struct mlx5e_priv *priv);
+void mlx5e_del_all_vlan_rules(struct mlx5e_priv *priv);
+
+int mlx5e_open_locked(struct net_device *netdev);
+int mlx5e_close_locked(struct net_device *netdev);
+int mlx5e_update_priv_params(struct mlx5e_priv *priv,
+			     struct mlx5e_params *new_params);
+
+static inline void mlx5e_tx_notify_hw(struct mlx5e_sq *sq,
+				      struct mlx5e_tx_wqe *wqe)
+{
+	/* ensure wqe is visible to device before updating doorbell record */
+	dma_wmb();
+
+	*sq->wq.db = cpu_to_be32(sq->pc);
+
+	/* ensure doorbell record is visible to device before ringing the
+	 * doorbell
+	 */
+	wmb();
+
+	mlx5_write64((__be32 *)&wqe->ctrl,
+		     sq->uar_map + MLX5_BF_OFFSET + sq->bf_offset,
+		     NULL);
+
+	sq->bf_offset ^= sq->bf_buf_size;
+}
+
+static inline void mlx5e_cq_arm(struct mlx5e_cq *cq)
+{
+	struct mlx5_core_cq *mcq;
+
+	mcq = &cq->mcq;
+	mlx5_cq_arm(mcq, MLX5_CQ_DB_REQ_NOT, mcq->uar->map, NULL, cq->wq.cc);
+}
+
+extern const struct ethtool_ops mlx5e_ethtool_ops;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
new file mode 100644
index 000000000000..388938482ff9
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
@@ -0,0 +1,679 @@
+/*
+ * Copyright (c) 2015, 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 "en.h"
+
+static void mlx5e_get_drvinfo(struct net_device *dev,
+			      struct ethtool_drvinfo *drvinfo)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	strlcpy(drvinfo->driver, DRIVER_NAME, sizeof(drvinfo->driver));
+	strlcpy(drvinfo->version, DRIVER_VERSION " (" DRIVER_RELDATE ")",
+		sizeof(drvinfo->version));
+	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
+		 "%d.%d.%d",
+		 fw_rev_maj(mdev), fw_rev_min(mdev), fw_rev_sub(mdev));
+	strlcpy(drvinfo->bus_info, pci_name(mdev->pdev),
+		sizeof(drvinfo->bus_info));
+}
+
+static const struct {
+	u32 supported;
+	u32 advertised;
+	u32 speed;
+} ptys2ethtool_table[MLX5E_LINK_MODES_NUMBER] = {
+	[MLX5E_1000BASE_CX_SGMII] = {
+		.supported  = SUPPORTED_1000baseKX_Full,
+		.advertised = ADVERTISED_1000baseKX_Full,
+		.speed      = 1000,
+	},
+	[MLX5E_1000BASE_KX] = {
+		.supported  = SUPPORTED_1000baseKX_Full,
+		.advertised = ADVERTISED_1000baseKX_Full,
+		.speed      = 1000,
+	},
+	[MLX5E_10GBASE_CX4] = {
+		.supported  = SUPPORTED_10000baseKX4_Full,
+		.advertised = ADVERTISED_10000baseKX4_Full,
+		.speed      = 10000,
+	},
+	[MLX5E_10GBASE_KX4] = {
+		.supported  = SUPPORTED_10000baseKX4_Full,
+		.advertised = ADVERTISED_10000baseKX4_Full,
+		.speed      = 10000,
+	},
+	[MLX5E_10GBASE_KR] = {
+		.supported  = SUPPORTED_10000baseKR_Full,
+		.advertised = ADVERTISED_10000baseKR_Full,
+		.speed      = 10000,
+	},
+	[MLX5E_20GBASE_KR2] = {
+		.supported  = SUPPORTED_20000baseKR2_Full,
+		.advertised = ADVERTISED_20000baseKR2_Full,
+		.speed      = 20000,
+	},
+	[MLX5E_40GBASE_CR4] = {
+		.supported  = SUPPORTED_40000baseCR4_Full,
+		.advertised = ADVERTISED_40000baseCR4_Full,
+		.speed      = 40000,
+	},
+	[MLX5E_40GBASE_KR4] = {
+		.supported  = SUPPORTED_40000baseKR4_Full,
+		.advertised = ADVERTISED_40000baseKR4_Full,
+		.speed      = 40000,
+	},
+	[MLX5E_56GBASE_R4] = {
+		.supported  = SUPPORTED_56000baseKR4_Full,
+		.advertised = ADVERTISED_56000baseKR4_Full,
+		.speed      = 56000,
+	},
+	[MLX5E_10GBASE_CR] = {
+		.supported  = SUPPORTED_10000baseKR_Full,
+		.advertised = ADVERTISED_10000baseKR_Full,
+		.speed      = 10000,
+	},
+	[MLX5E_10GBASE_SR] = {
+		.supported  = SUPPORTED_10000baseKR_Full,
+		.advertised = ADVERTISED_10000baseKR_Full,
+		.speed      = 10000,
+	},
+	[MLX5E_10GBASE_ER] = {
+		.supported  = SUPPORTED_10000baseKR_Full,
+		.advertised = ADVERTISED_10000baseKR_Full,
+		.speed      = 10000,
+	},
+	[MLX5E_40GBASE_SR4] = {
+		.supported  = SUPPORTED_40000baseSR4_Full,
+		.advertised = ADVERTISED_40000baseSR4_Full,
+		.speed      = 40000,
+	},
+	[MLX5E_40GBASE_LR4] = {
+		.supported  = SUPPORTED_40000baseLR4_Full,
+		.advertised = ADVERTISED_40000baseLR4_Full,
+		.speed      = 40000,
+	},
+	[MLX5E_100GBASE_CR4] = {
+		.speed      = 100000,
+	},
+	[MLX5E_100GBASE_SR4] = {
+		.speed      = 100000,
+	},
+	[MLX5E_100GBASE_KR4] = {
+		.speed      = 100000,
+	},
+	[MLX5E_100GBASE_LR4] = {
+		.speed      = 100000,
+	},
+	[MLX5E_100BASE_TX]   = {
+		.speed      = 100,
+	},
+	[MLX5E_100BASE_T]    = {
+		.supported  = SUPPORTED_100baseT_Full,
+		.advertised = ADVERTISED_100baseT_Full,
+		.speed      = 100,
+	},
+	[MLX5E_10GBASE_T]    = {
+		.supported  = SUPPORTED_10000baseT_Full,
+		.advertised = ADVERTISED_10000baseT_Full,
+		.speed      = 1000,
+	},
+	[MLX5E_25GBASE_CR]   = {
+		.speed      = 25000,
+	},
+	[MLX5E_25GBASE_KR]   = {
+		.speed      = 25000,
+	},
+	[MLX5E_25GBASE_SR]   = {
+		.speed      = 25000,
+	},
+	[MLX5E_50GBASE_CR2]  = {
+		.speed      = 50000,
+	},
+	[MLX5E_50GBASE_KR2]  = {
+		.speed      = 50000,
+	},
+};
+
+static int mlx5e_get_sset_count(struct net_device *dev, int sset)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	switch (sset) {
+	case ETH_SS_STATS:
+		return NUM_VPORT_COUNTERS +
+		       priv->params.num_channels * NUM_RQ_STATS +
+		       priv->params.num_channels * priv->num_tc *
+						   NUM_SQ_STATS;
+	/* fallthrough */
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void mlx5e_get_strings(struct net_device *dev,
+			      uint32_t stringset, uint8_t *data)
+{
+	int i, j, tc, idx = 0;
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	switch (stringset) {
+	case ETH_SS_PRIV_FLAGS:
+		break;
+
+	case ETH_SS_TEST:
+		break;
+
+	case ETH_SS_STATS:
+		/* VPORT counters */
+		for (i = 0; i < NUM_VPORT_COUNTERS; i++)
+			strcpy(data + (idx++) * ETH_GSTRING_LEN,
+			       vport_strings[i]);
+
+		/* per channel counters */
+		for (i = 0; i < priv->params.num_channels; i++)
+			for (j = 0; j < NUM_RQ_STATS; j++)
+				sprintf(data + (idx++) * ETH_GSTRING_LEN,
+					"rx%d_%s", i, rq_stats_strings[j]);
+
+		for (i = 0; i < priv->params.num_channels; i++)
+			for (tc = 0; tc < priv->num_tc; tc++)
+				for (j = 0; j < NUM_SQ_STATS; j++)
+					sprintf(data +
+						(idx++) * ETH_GSTRING_LEN,
+						"tx%d_%d_%s", i, tc,
+						sq_stats_strings[j]);
+		break;
+	}
+}
+
+static void mlx5e_get_ethtool_stats(struct net_device *dev,
+				    struct ethtool_stats *stats, u64 *data)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	int i, j, tc, idx = 0;
+
+	if (!data)
+		return;
+
+	mutex_lock(&priv->state_lock);
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state))
+		mlx5e_update_stats(priv);
+	mutex_unlock(&priv->state_lock);
+
+	for (i = 0; i < NUM_VPORT_COUNTERS; i++)
+		data[idx++] = ((u64 *)&priv->stats.vport)[i];
+
+	/* per channel counters */
+	for (i = 0; i < priv->params.num_channels; i++)
+		for (j = 0; j < NUM_RQ_STATS; j++)
+			data[idx++] = !test_bit(MLX5E_STATE_OPENED,
+						&priv->state) ? 0 :
+				       ((u64 *)&priv->channel[i]->rq.stats)[j];
+
+	for (i = 0; i < priv->params.num_channels; i++)
+		for (tc = 0; tc < priv->num_tc; tc++)
+			for (j = 0; j < NUM_SQ_STATS; j++)
+				data[idx++] = !test_bit(MLX5E_STATE_OPENED,
+							&priv->state) ? 0 :
+				((u64 *)&priv->channel[i]->sq[tc].stats)[j];
+}
+
+static void mlx5e_get_ringparam(struct net_device *dev,
+				struct ethtool_ringparam *param)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	param->rx_max_pending = 1 << MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE;
+	param->tx_max_pending = 1 << MLX5E_PARAMS_MAXIMUM_LOG_SQ_SIZE;
+	param->rx_pending     = 1 << priv->params.log_rq_size;
+	param->tx_pending     = 1 << priv->params.log_sq_size;
+}
+
+static int mlx5e_set_ringparam(struct net_device *dev,
+			       struct ethtool_ringparam *param)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5e_params new_params;
+	u16 min_rx_wqes;
+	u8 log_rq_size;
+	u8 log_sq_size;
+	int err = 0;
+
+	if (param->rx_jumbo_pending) {
+		netdev_info(dev, "%s: rx_jumbo_pending not supported\n",
+			    __func__);
+		return -EINVAL;
+	}
+	if (param->rx_mini_pending) {
+		netdev_info(dev, "%s: rx_mini_pending not supported\n",
+			    __func__);
+		return -EINVAL;
+	}
+	if (param->rx_pending < (1 << MLX5E_PARAMS_MINIMUM_LOG_RQ_SIZE)) {
+		netdev_info(dev, "%s: rx_pending (%d) < min (%d)\n",
+			    __func__, param->rx_pending,
+			    1 << MLX5E_PARAMS_MINIMUM_LOG_RQ_SIZE);
+		return -EINVAL;
+	}
+	if (param->rx_pending > (1 << MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE)) {
+		netdev_info(dev, "%s: rx_pending (%d) > max (%d)\n",
+			    __func__, param->rx_pending,
+			    1 << MLX5E_PARAMS_MAXIMUM_LOG_RQ_SIZE);
+		return -EINVAL;
+	}
+	if (param->tx_pending < (1 << MLX5E_PARAMS_MINIMUM_LOG_SQ_SIZE)) {
+		netdev_info(dev, "%s: tx_pending (%d) < min (%d)\n",
+			    __func__, param->tx_pending,
+			    1 << MLX5E_PARAMS_MINIMUM_LOG_SQ_SIZE);
+		return -EINVAL;
+	}
+	if (param->tx_pending > (1 << MLX5E_PARAMS_MAXIMUM_LOG_SQ_SIZE)) {
+		netdev_info(dev, "%s: tx_pending (%d) > max (%d)\n",
+			    __func__, param->tx_pending,
+			    1 << MLX5E_PARAMS_MAXIMUM_LOG_SQ_SIZE);
+		return -EINVAL;
+	}
+
+	log_rq_size = order_base_2(param->rx_pending);
+	log_sq_size = order_base_2(param->tx_pending);
+	min_rx_wqes = min_t(u16, param->rx_pending - 1,
+			    MLX5E_PARAMS_DEFAULT_MIN_RX_WQES);
+
+	if (log_rq_size == priv->params.log_rq_size &&
+	    log_sq_size == priv->params.log_sq_size &&
+	    min_rx_wqes == priv->params.min_rx_wqes)
+		return 0;
+
+	mutex_lock(&priv->state_lock);
+	new_params = priv->params;
+	new_params.log_rq_size = log_rq_size;
+	new_params.log_sq_size = log_sq_size;
+	new_params.min_rx_wqes = min_rx_wqes;
+	err = mlx5e_update_priv_params(priv, &new_params);
+	mutex_unlock(&priv->state_lock);
+
+	return err;
+}
+
+static void mlx5e_get_channels(struct net_device *dev,
+			       struct ethtool_channels *ch)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	int ncv = priv->mdev->priv.eq_table.num_comp_vectors;
+
+	ch->max_combined   = ncv;
+	ch->combined_count = priv->params.num_channels;
+}
+
+static int mlx5e_set_channels(struct net_device *dev,
+			      struct ethtool_channels *ch)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	int ncv = priv->mdev->priv.eq_table.num_comp_vectors;
+	unsigned int count = ch->combined_count;
+	struct mlx5e_params new_params;
+	int err = 0;
+
+	if (!count) {
+		netdev_info(dev, "%s: combined_count=0 not supported\n",
+			    __func__);
+		return -EINVAL;
+	}
+	if (ch->rx_count || ch->tx_count) {
+		netdev_info(dev, "%s: separate rx/tx count not supported\n",
+			    __func__);
+		return -EINVAL;
+	}
+	if (count > ncv) {
+		netdev_info(dev, "%s: count (%d) > max (%d)\n",
+			    __func__, count, ncv);
+		return -EINVAL;
+	}
+
+	if (priv->params.num_channels == count)
+		return 0;
+
+	mutex_lock(&priv->state_lock);
+	new_params = priv->params;
+	new_params.num_channels = count;
+	err = mlx5e_update_priv_params(priv, &new_params);
+	mutex_unlock(&priv->state_lock);
+
+	return err;
+}
+
+static int mlx5e_get_coalesce(struct net_device *netdev,
+			      struct ethtool_coalesce *coal)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+
+	coal->rx_coalesce_usecs       = priv->params.rx_cq_moderation_usec;
+	coal->rx_max_coalesced_frames = priv->params.rx_cq_moderation_pkts;
+	coal->tx_coalesce_usecs       = priv->params.tx_cq_moderation_usec;
+	coal->tx_max_coalesced_frames = priv->params.tx_cq_moderation_pkts;
+
+	return 0;
+}
+
+static int mlx5e_set_coalesce(struct net_device *netdev,
+			      struct ethtool_coalesce *coal)
+{
+	struct mlx5e_priv *priv    = netdev_priv(netdev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+	struct mlx5e_channel *c;
+	int tc;
+	int i;
+
+	priv->params.tx_cq_moderation_usec = coal->tx_coalesce_usecs;
+	priv->params.tx_cq_moderation_pkts = coal->tx_max_coalesced_frames;
+	priv->params.rx_cq_moderation_usec = coal->rx_coalesce_usecs;
+	priv->params.rx_cq_moderation_pkts = coal->rx_max_coalesced_frames;
+
+	for (i = 0; i < priv->params.num_channels; ++i) {
+		c = priv->channel[i];
+
+		for (tc = 0; tc < c->num_tc; tc++) {
+			mlx5_core_modify_cq_moderation(mdev,
+						&c->sq[tc].cq.mcq,
+						coal->tx_coalesce_usecs,
+						coal->tx_max_coalesced_frames);
+		}
+
+		mlx5_core_modify_cq_moderation(mdev, &c->rq.cq.mcq,
+					       coal->rx_coalesce_usecs,
+					       coal->rx_max_coalesced_frames);
+	}
+
+	return 0;
+}
+
+static u32 ptys2ethtool_supported_link(u32 eth_proto_cap)
+{
+	int i;
+	u32 supported_modes = 0;
+
+	for (i = 0; i < MLX5E_LINK_MODES_NUMBER; ++i) {
+		if (eth_proto_cap & MLX5E_PROT_MASK(i))
+			supported_modes |= ptys2ethtool_table[i].supported;
+	}
+	return supported_modes;
+}
+
+static u32 ptys2ethtool_adver_link(u32 eth_proto_cap)
+{
+	int i;
+	u32 advertising_modes = 0;
+
+	for (i = 0; i < MLX5E_LINK_MODES_NUMBER; ++i) {
+		if (eth_proto_cap & MLX5E_PROT_MASK(i))
+			advertising_modes |= ptys2ethtool_table[i].advertised;
+	}
+	return advertising_modes;
+}
+
+static u32 ptys2ethtool_supported_port(u32 eth_proto_cap)
+{
+	if (eth_proto_cap & (MLX5E_PROT_MASK(MLX5E_10GBASE_CR)
+			   | MLX5E_PROT_MASK(MLX5E_10GBASE_SR)
+			   | MLX5E_PROT_MASK(MLX5E_40GBASE_CR4)
+			   | MLX5E_PROT_MASK(MLX5E_40GBASE_SR4)
+			   | MLX5E_PROT_MASK(MLX5E_100GBASE_SR4)
+			   | MLX5E_PROT_MASK(MLX5E_1000BASE_CX_SGMII))) {
+		return SUPPORTED_FIBRE;
+	}
+
+	if (eth_proto_cap & (MLX5E_PROT_MASK(MLX5E_100GBASE_KR4)
+			   | MLX5E_PROT_MASK(MLX5E_40GBASE_KR4)
+			   | MLX5E_PROT_MASK(MLX5E_10GBASE_KR)
+			   | MLX5E_PROT_MASK(MLX5E_10GBASE_KX4)
+			   | MLX5E_PROT_MASK(MLX5E_1000BASE_KX))) {
+		return SUPPORTED_Backplane;
+	}
+	return 0;
+}
+
+static void get_speed_duplex(struct net_device *netdev,
+			     u32 eth_proto_oper,
+			     struct ethtool_cmd *cmd)
+{
+	int i;
+	u32 speed = SPEED_UNKNOWN;
+	u8 duplex = DUPLEX_UNKNOWN;
+
+	if (!netif_carrier_ok(netdev))
+		goto out;
+
+	for (i = 0; i < MLX5E_LINK_MODES_NUMBER; ++i) {
+		if (eth_proto_oper & MLX5E_PROT_MASK(i)) {
+			speed = ptys2ethtool_table[i].speed;
+			duplex = DUPLEX_FULL;
+			break;
+		}
+	}
+out:
+	ethtool_cmd_speed_set(cmd, speed);
+	cmd->duplex = duplex;
+}
+
+static void get_supported(u32 eth_proto_cap, u32 *supported)
+{
+	*supported |= ptys2ethtool_supported_port(eth_proto_cap);
+	*supported |= ptys2ethtool_supported_link(eth_proto_cap);
+	*supported |= SUPPORTED_Pause | SUPPORTED_Asym_Pause;
+}
+
+static void get_advertising(u32 eth_proto_cap, u8 tx_pause,
+			    u8 rx_pause, u32 *advertising)
+{
+	*advertising |= ptys2ethtool_adver_link(eth_proto_cap);
+	*advertising |= tx_pause ? ADVERTISED_Pause : 0;
+	*advertising |= (tx_pause ^ rx_pause) ? ADVERTISED_Asym_Pause : 0;
+}
+
+static u8 get_connector_port(u32 eth_proto)
+{
+	if (eth_proto & (MLX5E_PROT_MASK(MLX5E_10GBASE_SR)
+			 | MLX5E_PROT_MASK(MLX5E_40GBASE_SR4)
+			 | MLX5E_PROT_MASK(MLX5E_100GBASE_SR4)
+			 | MLX5E_PROT_MASK(MLX5E_1000BASE_CX_SGMII))) {
+			return PORT_FIBRE;
+	}
+
+	if (eth_proto & (MLX5E_PROT_MASK(MLX5E_40GBASE_CR4)
+			 | MLX5E_PROT_MASK(MLX5E_10GBASE_CR)
+			 | MLX5E_PROT_MASK(MLX5E_100GBASE_CR4))) {
+			return PORT_DA;
+	}
+
+	if (eth_proto & (MLX5E_PROT_MASK(MLX5E_10GBASE_KX4)
+			 | MLX5E_PROT_MASK(MLX5E_10GBASE_KR)
+			 | MLX5E_PROT_MASK(MLX5E_40GBASE_KR4)
+			 | MLX5E_PROT_MASK(MLX5E_100GBASE_KR4))) {
+			return PORT_NONE;
+	}
+
+	return PORT_OTHER;
+}
+
+static void get_lp_advertising(u32 eth_proto_lp, u32 *lp_advertising)
+{
+	*lp_advertising = ptys2ethtool_adver_link(eth_proto_lp);
+}
+
+static int mlx5e_get_settings(struct net_device *netdev,
+			      struct ethtool_cmd *cmd)
+{
+	struct mlx5e_priv *priv    = netdev_priv(netdev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+	u32 out[MLX5_ST_SZ_DW(ptys_reg)];
+	u32 eth_proto_cap;
+	u32 eth_proto_admin;
+	u32 eth_proto_lp;
+	u32 eth_proto_oper;
+	int err;
+
+	err = mlx5_query_port_ptys(mdev, out, sizeof(out), MLX5_PTYS_EN, 1);
+
+	if (err) {
+		netdev_err(netdev, "%s: query port ptys failed: %d\n",
+			   __func__, err);
+		goto err_query_ptys;
+	}
+
+	eth_proto_cap   = MLX5_GET(ptys_reg, out, eth_proto_capability);
+	eth_proto_admin = MLX5_GET(ptys_reg, out, eth_proto_admin);
+	eth_proto_oper  = MLX5_GET(ptys_reg, out, eth_proto_oper);
+	eth_proto_lp    = MLX5_GET(ptys_reg, out, eth_proto_lp_advertise);
+
+	cmd->supported   = 0;
+	cmd->advertising = 0;
+
+	get_supported(eth_proto_cap, &cmd->supported);
+	get_advertising(eth_proto_admin, 0, 0, &cmd->advertising);
+	get_speed_duplex(netdev, eth_proto_oper, cmd);
+
+	eth_proto_oper = eth_proto_oper ? eth_proto_oper : eth_proto_cap;
+
+	cmd->port = get_connector_port(eth_proto_oper);
+	get_lp_advertising(eth_proto_lp, &cmd->lp_advertising);
+
+	cmd->transceiver = XCVR_INTERNAL;
+
+err_query_ptys:
+	return err;
+}
+
+static u32 mlx5e_ethtool2ptys_adver_link(u32 link_modes)
+{
+	u32 i, ptys_modes = 0;
+
+	for (i = 0; i < MLX5E_LINK_MODES_NUMBER; ++i) {
+		if (ptys2ethtool_table[i].advertised & link_modes)
+			ptys_modes |= MLX5E_PROT_MASK(i);
+	}
+
+	return ptys_modes;
+}
+
+static u32 mlx5e_ethtool2ptys_speed_link(u32 speed)
+{
+	u32 i, speed_links = 0;
+
+	for (i = 0; i < MLX5E_LINK_MODES_NUMBER; ++i) {
+		if (ptys2ethtool_table[i].speed == speed)
+			speed_links |= MLX5E_PROT_MASK(i);
+	}
+
+	return speed_links;
+}
+
+static int mlx5e_set_settings(struct net_device *netdev,
+			      struct ethtool_cmd *cmd)
+{
+	struct mlx5e_priv *priv    = netdev_priv(netdev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+	u32 link_modes;
+	u32 speed;
+	u32 eth_proto_cap, eth_proto_admin;
+	u8 port_status;
+	int err;
+
+	speed = ethtool_cmd_speed(cmd);
+
+	link_modes = cmd->autoneg == AUTONEG_ENABLE ?
+		mlx5e_ethtool2ptys_adver_link(cmd->advertising) :
+		mlx5e_ethtool2ptys_speed_link(speed);
+
+	err = mlx5_query_port_proto_cap(mdev, &eth_proto_cap, MLX5_PTYS_EN);
+	if (err) {
+		netdev_err(netdev, "%s: query port eth proto cap failed: %d\n",
+			   __func__, err);
+		goto out;
+	}
+
+	link_modes = link_modes & eth_proto_cap;
+	if (!link_modes) {
+		netdev_err(netdev, "%s: Not supported link mode(s) requested",
+			   __func__);
+		err = -EINVAL;
+		goto out;
+	}
+
+	err = mlx5_query_port_proto_admin(mdev, &eth_proto_admin, MLX5_PTYS_EN);
+	if (err) {
+		netdev_err(netdev, "%s: query port eth proto admin failed: %d\n",
+			   __func__, err);
+		goto out;
+	}
+
+	if (link_modes == eth_proto_admin)
+		goto out;
+
+	err = mlx5_set_port_proto(mdev, link_modes, MLX5_PTYS_EN);
+	if (err) {
+		netdev_err(netdev, "%s: set port eth proto admin failed: %d\n",
+			   __func__, err);
+		goto out;
+	}
+
+	err = mlx5_query_port_status(mdev, &port_status);
+	if (err)
+		goto out;
+
+	if (port_status == MLX5_PORT_DOWN)
+		return 0;
+
+	err = mlx5_set_port_status(mdev, MLX5_PORT_DOWN);
+	if (err)
+		goto out;
+	err = mlx5_set_port_status(mdev, MLX5_PORT_UP);
+out:
+	return err;
+}
+
+const struct ethtool_ops mlx5e_ethtool_ops = {
+	.get_drvinfo       = mlx5e_get_drvinfo,
+	.get_link          = ethtool_op_get_link,
+	.get_strings       = mlx5e_get_strings,
+	.get_sset_count    = mlx5e_get_sset_count,
+	.get_ethtool_stats = mlx5e_get_ethtool_stats,
+	.get_ringparam     = mlx5e_get_ringparam,
+	.set_ringparam     = mlx5e_set_ringparam,
+	.get_channels      = mlx5e_get_channels,
+	.set_channels      = mlx5e_set_channels,
+	.get_coalesce      = mlx5e_get_coalesce,
+	.set_coalesce      = mlx5e_set_coalesce,
+	.get_settings      = mlx5e_get_settings,
+	.set_settings      = mlx5e_set_settings,
+};
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_flow_table.c b/drivers/net/ethernet/mellanox/mlx5/core/en_flow_table.c
new file mode 100644
index 000000000000..120db80c47aa
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_flow_table.c
@@ -0,0 +1,860 @@
+/*
+ * Copyright (c) 2015, 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/list.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/tcp.h>
+#include <linux/mlx5/flow_table.h>
+#include "en.h"
+
+enum {
+	MLX5E_FULLMATCH = 0,
+	MLX5E_ALLMULTI  = 1,
+	MLX5E_PROMISC   = 2,
+};
+
+enum {
+	MLX5E_UC        = 0,
+	MLX5E_MC_IPV4   = 1,
+	MLX5E_MC_IPV6   = 2,
+	MLX5E_MC_OTHER  = 3,
+};
+
+enum {
+	MLX5E_ACTION_NONE = 0,
+	MLX5E_ACTION_ADD  = 1,
+	MLX5E_ACTION_DEL  = 2,
+};
+
+struct mlx5e_eth_addr_hash_node {
+	struct hlist_node          hlist;
+	u8                         action;
+	struct mlx5e_eth_addr_info ai;
+};
+
+static inline int mlx5e_hash_eth_addr(u8 *addr)
+{
+	return addr[5];
+}
+
+static void mlx5e_add_eth_addr_to_hash(struct hlist_head *hash, u8 *addr)
+{
+	struct mlx5e_eth_addr_hash_node *hn;
+	int ix = mlx5e_hash_eth_addr(addr);
+	int found = 0;
+
+	hlist_for_each_entry(hn, &hash[ix], hlist)
+		if (ether_addr_equal_64bits(hn->ai.addr, addr)) {
+			found = 1;
+			break;
+		}
+
+	if (found) {
+		hn->action = MLX5E_ACTION_NONE;
+		return;
+	}
+
+	hn = kzalloc(sizeof(*hn), GFP_ATOMIC);
+	if (!hn)
+		return;
+
+	ether_addr_copy(hn->ai.addr, addr);
+	hn->action = MLX5E_ACTION_ADD;
+
+	hlist_add_head(&hn->hlist, &hash[ix]);
+}
+
+static void mlx5e_del_eth_addr_from_hash(struct mlx5e_eth_addr_hash_node *hn)
+{
+	hlist_del(&hn->hlist);
+	kfree(hn);
+}
+
+static void mlx5e_del_eth_addr_from_flow_table(struct mlx5e_priv *priv,
+					       struct mlx5e_eth_addr_info *ai)
+{
+	void *ft = priv->ft.main;
+
+	if (ai->tt_vec & (1 << MLX5E_TT_IPV6_TCP))
+		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV6_TCP]);
+
+	if (ai->tt_vec & (1 << MLX5E_TT_IPV4_TCP))
+		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV4_TCP]);
+
+	if (ai->tt_vec & (1 << MLX5E_TT_IPV6_UDP))
+		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV6_UDP]);
+
+	if (ai->tt_vec & (1 << MLX5E_TT_IPV4_UDP))
+		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV4_UDP]);
+
+	if (ai->tt_vec & (1 << MLX5E_TT_IPV6))
+		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV6]);
+
+	if (ai->tt_vec & (1 << MLX5E_TT_IPV4))
+		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV4]);
+
+	if (ai->tt_vec & (1 << MLX5E_TT_ANY))
+		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_ANY]);
+}
+
+static int mlx5e_get_eth_addr_type(u8 *addr)
+{
+	if (is_unicast_ether_addr(addr))
+		return MLX5E_UC;
+
+	if ((addr[0] == 0x01) &&
+	    (addr[1] == 0x00) &&
+	    (addr[2] == 0x5e) &&
+	   !(addr[3] &  0x80))
+		return MLX5E_MC_IPV4;
+
+	if ((addr[0] == 0x33) &&
+	    (addr[1] == 0x33))
+		return MLX5E_MC_IPV6;
+
+	return MLX5E_MC_OTHER;
+}
+
+static u32 mlx5e_get_tt_vec(struct mlx5e_eth_addr_info *ai, int type)
+{
+	int eth_addr_type;
+	u32 ret;
+
+	switch (type) {
+	case MLX5E_FULLMATCH:
+		eth_addr_type = mlx5e_get_eth_addr_type(ai->addr);
+		switch (eth_addr_type) {
+		case MLX5E_UC:
+			ret =
+				(1 << MLX5E_TT_IPV4_TCP) |
+				(1 << MLX5E_TT_IPV6_TCP) |
+				(1 << MLX5E_TT_IPV4_UDP) |
+				(1 << MLX5E_TT_IPV6_UDP) |
+				(1 << MLX5E_TT_IPV4)     |
+				(1 << MLX5E_TT_IPV6)     |
+				(1 << MLX5E_TT_ANY)      |
+				0;
+			break;
+
+		case MLX5E_MC_IPV4:
+			ret =
+				(1 << MLX5E_TT_IPV4_UDP) |
+				(1 << MLX5E_TT_IPV4)     |
+				0;
+			break;
+
+		case MLX5E_MC_IPV6:
+			ret =
+				(1 << MLX5E_TT_IPV6_UDP) |
+				(1 << MLX5E_TT_IPV6)     |
+				0;
+			break;
+
+		case MLX5E_MC_OTHER:
+			ret =
+				(1 << MLX5E_TT_ANY)      |
+				0;
+			break;
+		}
+
+		break;
+
+	case MLX5E_ALLMULTI:
+		ret =
+			(1 << MLX5E_TT_IPV4_UDP) |
+			(1 << MLX5E_TT_IPV6_UDP) |
+			(1 << MLX5E_TT_IPV4)     |
+			(1 << MLX5E_TT_IPV6)     |
+			(1 << MLX5E_TT_ANY)      |
+			0;
+		break;
+
+	default: /* MLX5E_PROMISC */
+		ret =
+			(1 << MLX5E_TT_IPV4_TCP) |
+			(1 << MLX5E_TT_IPV6_TCP) |
+			(1 << MLX5E_TT_IPV4_UDP) |
+			(1 << MLX5E_TT_IPV6_UDP) |
+			(1 << MLX5E_TT_IPV4)     |
+			(1 << MLX5E_TT_IPV6)     |
+			(1 << MLX5E_TT_ANY)      |
+			0;
+		break;
+	}
+
+	return ret;
+}
+
+static int __mlx5e_add_eth_addr_rule(struct mlx5e_priv *priv,
+				     struct mlx5e_eth_addr_info *ai, int type,
+				     void *flow_context, void *match_criteria)
+{
+	u8 match_criteria_enable = 0;
+	void *match_value;
+	void *dest;
+	u8   *dmac;
+	u8   *match_criteria_dmac;
+	void *ft   = priv->ft.main;
+	u32  *tirn = priv->tirn;
+	u32  tt_vec;
+	int  err;
+
+	match_value = MLX5_ADDR_OF(flow_context, flow_context, match_value);
+	dmac = MLX5_ADDR_OF(fte_match_param, match_value,
+			    outer_headers.dmac_47_16);
+	match_criteria_dmac = MLX5_ADDR_OF(fte_match_param, match_criteria,
+					   outer_headers.dmac_47_16);
+	dest = MLX5_ADDR_OF(flow_context, flow_context, destination);
+
+	MLX5_SET(flow_context, flow_context, action,
+		 MLX5_FLOW_CONTEXT_ACTION_FWD_DEST);
+	MLX5_SET(flow_context, flow_context, destination_list_size, 1);
+	MLX5_SET(dest_format_struct, dest, destination_type,
+		 MLX5_FLOW_CONTEXT_DEST_TYPE_TIR);
+
+	switch (type) {
+	case MLX5E_FULLMATCH:
+		match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+		memset(match_criteria_dmac, 0xff, ETH_ALEN);
+		ether_addr_copy(dmac, ai->addr);
+		break;
+
+	case MLX5E_ALLMULTI:
+		match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+		match_criteria_dmac[0] = 0x01;
+		dmac[0] = 0x01;
+		break;
+
+	case MLX5E_PROMISC:
+		break;
+	}
+
+	tt_vec = mlx5e_get_tt_vec(ai, type);
+
+	if (tt_vec & (1 << MLX5E_TT_ANY)) {
+		MLX5_SET(dest_format_struct, dest, destination_id,
+			 tirn[MLX5E_TT_ANY]);
+		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
+						match_criteria, flow_context,
+						&ai->ft_ix[MLX5E_TT_ANY]);
+		if (err) {
+			mlx5e_del_eth_addr_from_flow_table(priv, ai);
+			return err;
+		}
+		ai->tt_vec |= (1 << MLX5E_TT_ANY);
+	}
+
+	match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	MLX5_SET_TO_ONES(fte_match_param, match_criteria,
+			 outer_headers.ethertype);
+
+	if (tt_vec & (1 << MLX5E_TT_IPV4)) {
+		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
+			 ETH_P_IP);
+		MLX5_SET(dest_format_struct, dest, destination_id,
+			 tirn[MLX5E_TT_IPV4]);
+		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
+						match_criteria, flow_context,
+						&ai->ft_ix[MLX5E_TT_IPV4]);
+		if (err) {
+			mlx5e_del_eth_addr_from_flow_table(priv, ai);
+			return err;
+		}
+		ai->tt_vec |= (1 << MLX5E_TT_IPV4);
+	}
+
+	if (tt_vec & (1 << MLX5E_TT_IPV6)) {
+		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
+			 ETH_P_IPV6);
+		MLX5_SET(dest_format_struct, dest, destination_id,
+			 tirn[MLX5E_TT_IPV6]);
+		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
+						match_criteria, flow_context,
+						&ai->ft_ix[MLX5E_TT_IPV6]);
+		if (err) {
+			mlx5e_del_eth_addr_from_flow_table(priv, ai);
+			return err;
+		}
+		ai->tt_vec |= (1 << MLX5E_TT_IPV6);
+	}
+
+	MLX5_SET_TO_ONES(fte_match_param, match_criteria,
+			 outer_headers.ip_protocol);
+	MLX5_SET(fte_match_param, match_value, outer_headers.ip_protocol,
+		 IPPROTO_UDP);
+
+	if (tt_vec & (1 << MLX5E_TT_IPV4_UDP)) {
+		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
+			 ETH_P_IP);
+		MLX5_SET(dest_format_struct, dest, destination_id,
+			 tirn[MLX5E_TT_IPV4_UDP]);
+		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
+						match_criteria, flow_context,
+						&ai->ft_ix[MLX5E_TT_IPV4_UDP]);
+		if (err) {
+			mlx5e_del_eth_addr_from_flow_table(priv, ai);
+			return err;
+		}
+		ai->tt_vec |= (1 << MLX5E_TT_IPV4_UDP);
+	}
+
+	if (tt_vec & (1 << MLX5E_TT_IPV6_UDP)) {
+		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
+			 ETH_P_IPV6);
+		MLX5_SET(dest_format_struct, dest, destination_id,
+			 tirn[MLX5E_TT_IPV6_UDP]);
+		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
+						match_criteria, flow_context,
+						&ai->ft_ix[MLX5E_TT_IPV6_UDP]);
+		if (err) {
+			mlx5e_del_eth_addr_from_flow_table(priv, ai);
+			return err;
+		}
+		ai->tt_vec |= (1 << MLX5E_TT_IPV6_UDP);
+	}
+
+	MLX5_SET(fte_match_param, match_value, outer_headers.ip_protocol,
+		 IPPROTO_TCP);
+
+	if (tt_vec & (1 << MLX5E_TT_IPV4_TCP)) {
+		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
+			 ETH_P_IP);
+		MLX5_SET(dest_format_struct, dest, destination_id,
+			 tirn[MLX5E_TT_IPV4_TCP]);
+		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
+						match_criteria, flow_context,
+						&ai->ft_ix[MLX5E_TT_IPV4_TCP]);
+		if (err) {
+			mlx5e_del_eth_addr_from_flow_table(priv, ai);
+			return err;
+		}
+		ai->tt_vec |= (1 << MLX5E_TT_IPV4_TCP);
+	}
+
+	if (tt_vec & (1 << MLX5E_TT_IPV6_TCP)) {
+		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
+			 ETH_P_IPV6);
+		MLX5_SET(dest_format_struct, dest, destination_id,
+			 tirn[MLX5E_TT_IPV6_TCP]);
+		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
+						match_criteria, flow_context,
+						&ai->ft_ix[MLX5E_TT_IPV6_TCP]);
+		if (err) {
+			mlx5e_del_eth_addr_from_flow_table(priv, ai);
+			return err;
+		}
+		ai->tt_vec |= (1 << MLX5E_TT_IPV6_TCP);
+	}
+
+	return 0;
+}
+
+static int mlx5e_add_eth_addr_rule(struct mlx5e_priv *priv,
+				   struct mlx5e_eth_addr_info *ai, int type)
+{
+	u32 *flow_context;
+	u32 *match_criteria;
+	int err;
+
+	flow_context   = mlx5_vzalloc(MLX5_ST_SZ_BYTES(flow_context) +
+				      MLX5_ST_SZ_BYTES(dest_format_struct));
+	match_criteria = mlx5_vzalloc(MLX5_ST_SZ_BYTES(fte_match_param));
+	if (!flow_context || !match_criteria) {
+		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
+		err = -ENOMEM;
+		goto add_eth_addr_rule_out;
+	}
+
+	err = __mlx5e_add_eth_addr_rule(priv, ai, type, flow_context,
+					match_criteria);
+	if (err)
+		netdev_err(priv->netdev, "%s: failed\n", __func__);
+
+add_eth_addr_rule_out:
+	kvfree(match_criteria);
+	kvfree(flow_context);
+	return err;
+}
+
+enum mlx5e_vlan_rule_type {
+	MLX5E_VLAN_RULE_TYPE_UNTAGGED,
+	MLX5E_VLAN_RULE_TYPE_ANY_VID,
+	MLX5E_VLAN_RULE_TYPE_MATCH_VID,
+};
+
+static int mlx5e_add_vlan_rule(struct mlx5e_priv *priv,
+			       enum mlx5e_vlan_rule_type rule_type, u16 vid)
+{
+	u8 match_criteria_enable = 0;
+	u32 *flow_context;
+	void *match_value;
+	void *dest;
+	u32 *match_criteria;
+	u32 *ft_ix;
+	int err;
+
+	flow_context   = mlx5_vzalloc(MLX5_ST_SZ_BYTES(flow_context) +
+				      MLX5_ST_SZ_BYTES(dest_format_struct));
+	match_criteria = mlx5_vzalloc(MLX5_ST_SZ_BYTES(fte_match_param));
+	if (!flow_context || !match_criteria) {
+		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
+		err = -ENOMEM;
+		goto add_vlan_rule_out;
+	}
+	match_value = MLX5_ADDR_OF(flow_context, flow_context, match_value);
+	dest = MLX5_ADDR_OF(flow_context, flow_context, destination);
+
+	MLX5_SET(flow_context, flow_context, action,
+		 MLX5_FLOW_CONTEXT_ACTION_FWD_DEST);
+	MLX5_SET(flow_context, flow_context, destination_list_size, 1);
+	MLX5_SET(dest_format_struct, dest, destination_type,
+		 MLX5_FLOW_CONTEXT_DEST_TYPE_FLOW_TABLE);
+	MLX5_SET(dest_format_struct, dest, destination_id,
+		 mlx5_get_flow_table_id(priv->ft.main));
+
+	match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	MLX5_SET_TO_ONES(fte_match_param, match_criteria,
+			 outer_headers.vlan_tag);
+
+	switch (rule_type) {
+	case MLX5E_VLAN_RULE_TYPE_UNTAGGED:
+		ft_ix = &priv->vlan.untagged_rule_ft_ix;
+		break;
+	case MLX5E_VLAN_RULE_TYPE_ANY_VID:
+		ft_ix = &priv->vlan.any_vlan_rule_ft_ix;
+		MLX5_SET(fte_match_param, match_value, outer_headers.vlan_tag,
+			 1);
+		break;
+	default: /* MLX5E_VLAN_RULE_TYPE_MATCH_VID */
+		ft_ix = &priv->vlan.active_vlans_ft_ix[vid];
+		MLX5_SET(fte_match_param, match_value, outer_headers.vlan_tag,
+			 1);
+		MLX5_SET_TO_ONES(fte_match_param, match_criteria,
+				 outer_headers.first_vid);
+		MLX5_SET(fte_match_param, match_value, outer_headers.first_vid,
+			 vid);
+		break;
+	}
+
+	err = mlx5_add_flow_table_entry(priv->ft.vlan, match_criteria_enable,
+					match_criteria, flow_context, ft_ix);
+	if (err)
+		netdev_err(priv->netdev, "%s: failed\n", __func__);
+
+add_vlan_rule_out:
+	kvfree(match_criteria);
+	kvfree(flow_context);
+	return err;
+}
+
+static void mlx5e_del_vlan_rule(struct mlx5e_priv *priv,
+				enum mlx5e_vlan_rule_type rule_type, u16 vid)
+{
+	switch (rule_type) {
+	case MLX5E_VLAN_RULE_TYPE_UNTAGGED:
+		mlx5_del_flow_table_entry(priv->ft.vlan,
+					  priv->vlan.untagged_rule_ft_ix);
+		break;
+	case MLX5E_VLAN_RULE_TYPE_ANY_VID:
+		mlx5_del_flow_table_entry(priv->ft.vlan,
+					  priv->vlan.any_vlan_rule_ft_ix);
+		break;
+	case MLX5E_VLAN_RULE_TYPE_MATCH_VID:
+		mlx5_del_flow_table_entry(priv->ft.vlan,
+					  priv->vlan.active_vlans_ft_ix[vid]);
+		break;
+	}
+}
+
+void mlx5e_enable_vlan_filter(struct mlx5e_priv *priv)
+{
+	WARN_ON(!mutex_is_locked(&priv->state_lock));
+
+	if (priv->vlan.filter_disabled) {
+		priv->vlan.filter_disabled = false;
+		if (test_bit(MLX5E_STATE_OPENED, &priv->state))
+			mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID,
+					    0);
+	}
+}
+
+void mlx5e_disable_vlan_filter(struct mlx5e_priv *priv)
+{
+	WARN_ON(!mutex_is_locked(&priv->state_lock));
+
+	if (!priv->vlan.filter_disabled) {
+		priv->vlan.filter_disabled = true;
+		if (test_bit(MLX5E_STATE_OPENED, &priv->state))
+			mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID,
+					    0);
+	}
+}
+
+int mlx5e_vlan_rx_add_vid(struct net_device *dev, __always_unused __be16 proto,
+			  u16 vid)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	int err = 0;
+
+	mutex_lock(&priv->state_lock);
+
+	set_bit(vid, priv->vlan.active_vlans);
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state))
+		err = mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID,
+					  vid);
+
+	mutex_unlock(&priv->state_lock);
+
+	return err;
+}
+
+int mlx5e_vlan_rx_kill_vid(struct net_device *dev, __always_unused __be16 proto,
+			   u16 vid)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	mutex_lock(&priv->state_lock);
+
+	clear_bit(vid, priv->vlan.active_vlans);
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state))
+		mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID, vid);
+
+	mutex_unlock(&priv->state_lock);
+
+	return 0;
+}
+
+int mlx5e_add_all_vlan_rules(struct mlx5e_priv *priv)
+{
+	u16 vid;
+	int err;
+
+	for_each_set_bit(vid, priv->vlan.active_vlans, VLAN_N_VID) {
+		err = mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID,
+					  vid);
+		if (err)
+			return err;
+	}
+
+	err = mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
+	if (err)
+		return err;
+
+	if (priv->vlan.filter_disabled) {
+		err = mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID,
+					  0);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+void mlx5e_del_all_vlan_rules(struct mlx5e_priv *priv)
+{
+	u16 vid;
+
+	if (priv->vlan.filter_disabled)
+		mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID, 0);
+
+	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
+
+	for_each_set_bit(vid, priv->vlan.active_vlans, VLAN_N_VID)
+		mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID, vid);
+}
+
+#define mlx5e_for_each_hash_node(hn, tmp, hash, i) \
+	for (i = 0; i < MLX5E_ETH_ADDR_HASH_SIZE; i++) \
+		hlist_for_each_entry_safe(hn, tmp, &hash[i], hlist)
+
+static void mlx5e_execute_action(struct mlx5e_priv *priv,
+				 struct mlx5e_eth_addr_hash_node *hn)
+{
+	switch (hn->action) {
+	case MLX5E_ACTION_ADD:
+		mlx5e_add_eth_addr_rule(priv, &hn->ai, MLX5E_FULLMATCH);
+		hn->action = MLX5E_ACTION_NONE;
+		break;
+
+	case MLX5E_ACTION_DEL:
+		mlx5e_del_eth_addr_from_flow_table(priv, &hn->ai);
+		mlx5e_del_eth_addr_from_hash(hn);
+		break;
+	}
+}
+
+static void mlx5e_sync_netdev_addr(struct mlx5e_priv *priv)
+{
+	struct net_device *netdev = priv->netdev;
+	struct netdev_hw_addr *ha;
+
+	netif_addr_lock_bh(netdev);
+
+	mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_uc,
+				   priv->netdev->dev_addr);
+
+	netdev_for_each_uc_addr(ha, netdev)
+		mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_uc, ha->addr);
+
+	netdev_for_each_mc_addr(ha, netdev)
+		mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_mc, ha->addr);
+
+	netif_addr_unlock_bh(netdev);
+}
+
+static void mlx5e_apply_netdev_addr(struct mlx5e_priv *priv)
+{
+	struct mlx5e_eth_addr_hash_node *hn;
+	struct hlist_node *tmp;
+	int i;
+
+	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_uc, i)
+		mlx5e_execute_action(priv, hn);
+
+	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_mc, i)
+		mlx5e_execute_action(priv, hn);
+}
+
+static void mlx5e_handle_netdev_addr(struct mlx5e_priv *priv)
+{
+	struct mlx5e_eth_addr_hash_node *hn;
+	struct hlist_node *tmp;
+	int i;
+
+	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_uc, i)
+		hn->action = MLX5E_ACTION_DEL;
+	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_mc, i)
+		hn->action = MLX5E_ACTION_DEL;
+
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state))
+		mlx5e_sync_netdev_addr(priv);
+
+	mlx5e_apply_netdev_addr(priv);
+}
+
+void mlx5e_set_rx_mode_core(struct mlx5e_priv *priv)
+{
+	struct mlx5e_eth_addr_db *ea = &priv->eth_addr;
+	struct net_device *ndev = priv->netdev;
+
+	bool rx_mode_enable   = test_bit(MLX5E_STATE_OPENED, &priv->state);
+	bool promisc_enabled   = rx_mode_enable && (ndev->flags & IFF_PROMISC);
+	bool allmulti_enabled  = rx_mode_enable && (ndev->flags & IFF_ALLMULTI);
+	bool broadcast_enabled = rx_mode_enable;
+
+	bool enable_promisc    = !ea->promisc_enabled   &&  promisc_enabled;
+	bool disable_promisc   =  ea->promisc_enabled   && !promisc_enabled;
+	bool enable_allmulti   = !ea->allmulti_enabled  &&  allmulti_enabled;
+	bool disable_allmulti  =  ea->allmulti_enabled  && !allmulti_enabled;
+	bool enable_broadcast  = !ea->broadcast_enabled &&  broadcast_enabled;
+	bool disable_broadcast =  ea->broadcast_enabled && !broadcast_enabled;
+
+	if (enable_promisc)
+		mlx5e_add_eth_addr_rule(priv, &ea->promisc, MLX5E_PROMISC);
+	if (enable_allmulti)
+		mlx5e_add_eth_addr_rule(priv, &ea->allmulti, MLX5E_ALLMULTI);
+	if (enable_broadcast)
+		mlx5e_add_eth_addr_rule(priv, &ea->broadcast, MLX5E_FULLMATCH);
+
+	mlx5e_handle_netdev_addr(priv);
+
+	if (disable_broadcast)
+		mlx5e_del_eth_addr_from_flow_table(priv, &ea->broadcast);
+	if (disable_allmulti)
+		mlx5e_del_eth_addr_from_flow_table(priv, &ea->allmulti);
+	if (disable_promisc)
+		mlx5e_del_eth_addr_from_flow_table(priv, &ea->promisc);
+
+	ea->promisc_enabled   = promisc_enabled;
+	ea->allmulti_enabled  = allmulti_enabled;
+	ea->broadcast_enabled = broadcast_enabled;
+}
+
+void mlx5e_set_rx_mode_work(struct work_struct *work)
+{
+	struct mlx5e_priv *priv = container_of(work, struct mlx5e_priv,
+					       set_rx_mode_work);
+
+	mutex_lock(&priv->state_lock);
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state))
+		mlx5e_set_rx_mode_core(priv);
+	mutex_unlock(&priv->state_lock);
+}
+
+void mlx5e_init_eth_addr(struct mlx5e_priv *priv)
+{
+	ether_addr_copy(priv->eth_addr.broadcast.addr, priv->netdev->broadcast);
+}
+
+static int mlx5e_create_main_flow_table(struct mlx5e_priv *priv)
+{
+	struct mlx5_flow_table_group *g;
+	u8 *dmac;
+
+	g = kcalloc(9, sizeof(*g), GFP_KERNEL);
+	if (!g)
+		return -ENOMEM;
+
+	g[0].log_sz = 2;
+	g[0].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	MLX5_SET_TO_ONES(fte_match_param, g[0].match_criteria,
+			 outer_headers.ethertype);
+	MLX5_SET_TO_ONES(fte_match_param, g[0].match_criteria,
+			 outer_headers.ip_protocol);
+
+	g[1].log_sz = 1;
+	g[1].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	MLX5_SET_TO_ONES(fte_match_param, g[1].match_criteria,
+			 outer_headers.ethertype);
+
+	g[2].log_sz = 0;
+
+	g[3].log_sz = 14;
+	g[3].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	dmac = MLX5_ADDR_OF(fte_match_param, g[3].match_criteria,
+			    outer_headers.dmac_47_16);
+	memset(dmac, 0xff, ETH_ALEN);
+	MLX5_SET_TO_ONES(fte_match_param, g[3].match_criteria,
+			 outer_headers.ethertype);
+	MLX5_SET_TO_ONES(fte_match_param, g[3].match_criteria,
+			 outer_headers.ip_protocol);
+
+	g[4].log_sz = 13;
+	g[4].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	dmac = MLX5_ADDR_OF(fte_match_param, g[4].match_criteria,
+			    outer_headers.dmac_47_16);
+	memset(dmac, 0xff, ETH_ALEN);
+	MLX5_SET_TO_ONES(fte_match_param, g[4].match_criteria,
+			 outer_headers.ethertype);
+
+	g[5].log_sz = 11;
+	g[5].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	dmac = MLX5_ADDR_OF(fte_match_param, g[5].match_criteria,
+			    outer_headers.dmac_47_16);
+	memset(dmac, 0xff, ETH_ALEN);
+
+	g[6].log_sz = 2;
+	g[6].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	dmac = MLX5_ADDR_OF(fte_match_param, g[6].match_criteria,
+			    outer_headers.dmac_47_16);
+	dmac[0] = 0x01;
+	MLX5_SET_TO_ONES(fte_match_param, g[6].match_criteria,
+			 outer_headers.ethertype);
+	MLX5_SET_TO_ONES(fte_match_param, g[6].match_criteria,
+			 outer_headers.ip_protocol);
+
+	g[7].log_sz = 1;
+	g[7].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	dmac = MLX5_ADDR_OF(fte_match_param, g[7].match_criteria,
+			    outer_headers.dmac_47_16);
+	dmac[0] = 0x01;
+	MLX5_SET_TO_ONES(fte_match_param, g[7].match_criteria,
+			 outer_headers.ethertype);
+
+	g[8].log_sz = 0;
+	g[8].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	dmac = MLX5_ADDR_OF(fte_match_param, g[8].match_criteria,
+			    outer_headers.dmac_47_16);
+	dmac[0] = 0x01;
+	priv->ft.main = mlx5_create_flow_table(priv->mdev, 1,
+					       MLX5_FLOW_TABLE_TYPE_NIC_RCV,
+					       9, g);
+	kfree(g);
+
+	return priv->ft.main ? 0 : -ENOMEM;
+}
+
+static void mlx5e_destroy_main_flow_table(struct mlx5e_priv *priv)
+{
+	mlx5_destroy_flow_table(priv->ft.main);
+}
+
+static int mlx5e_create_vlan_flow_table(struct mlx5e_priv *priv)
+{
+	struct mlx5_flow_table_group *g;
+
+	g = kcalloc(2, sizeof(*g), GFP_KERNEL);
+	if (!g)
+		return -ENOMEM;
+
+	g[0].log_sz = 12;
+	g[0].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	MLX5_SET_TO_ONES(fte_match_param, g[0].match_criteria,
+			 outer_headers.vlan_tag);
+	MLX5_SET_TO_ONES(fte_match_param, g[0].match_criteria,
+			 outer_headers.first_vid);
+
+	/* untagged + any vlan id */
+	g[1].log_sz = 1;
+	g[1].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	MLX5_SET_TO_ONES(fte_match_param, g[1].match_criteria,
+			 outer_headers.vlan_tag);
+
+	priv->ft.vlan = mlx5_create_flow_table(priv->mdev, 0,
+					       MLX5_FLOW_TABLE_TYPE_NIC_RCV,
+					       2, g);
+
+	kfree(g);
+	return priv->ft.vlan ? 0 : -ENOMEM;
+}
+
+static void mlx5e_destroy_vlan_flow_table(struct mlx5e_priv *priv)
+{
+	mlx5_destroy_flow_table(priv->ft.vlan);
+}
+
+int mlx5e_open_flow_table(struct mlx5e_priv *priv)
+{
+	int err;
+
+	err = mlx5e_create_main_flow_table(priv);
+	if (err)
+		return err;
+
+	err = mlx5e_create_vlan_flow_table(priv);
+	if (err)
+		goto err_destroy_main_flow_table;
+
+	return 0;
+
+err_destroy_main_flow_table:
+	mlx5e_destroy_main_flow_table(priv);
+
+	return err;
+}
+
+void mlx5e_close_flow_table(struct mlx5e_priv *priv)
+{
+	mlx5e_destroy_vlan_flow_table(priv);
+	mlx5e_destroy_main_flow_table(priv);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
new file mode 100644
index 000000000000..40206da1f9d7
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -0,0 +1,1915 @@
+/*
+ * Copyright (c) 2015, 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/mlx5/flow_table.h>
+#include "en.h"
+
+struct mlx5e_rq_param {
+	u32                        rqc[MLX5_ST_SZ_DW(rqc)];
+	struct mlx5_wq_param       wq;
+};
+
+struct mlx5e_sq_param {
+	u32                        sqc[MLX5_ST_SZ_DW(sqc)];
+	struct mlx5_wq_param       wq;
+};
+
+struct mlx5e_cq_param {
+	u32                        cqc[MLX5_ST_SZ_DW(cqc)];
+	struct mlx5_wq_param       wq;
+	u16                        eq_ix;
+};
+
+struct mlx5e_channel_param {
+	struct mlx5e_rq_param      rq;
+	struct mlx5e_sq_param      sq;
+	struct mlx5e_cq_param      rx_cq;
+	struct mlx5e_cq_param      tx_cq;
+};
+
+static void mlx5e_update_carrier(struct mlx5e_priv *priv)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	u8 port_state;
+
+	port_state = mlx5_query_vport_state(mdev,
+		MLX5_QUERY_VPORT_STATE_IN_OP_MOD_VNIC_VPORT);
+
+	if (port_state == VPORT_STATE_UP)
+		netif_carrier_on(priv->netdev);
+	else
+		netif_carrier_off(priv->netdev);
+}
+
+static void mlx5e_update_carrier_work(struct work_struct *work)
+{
+	struct mlx5e_priv *priv = container_of(work, struct mlx5e_priv,
+					       update_carrier_work);
+
+	mutex_lock(&priv->state_lock);
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state))
+		mlx5e_update_carrier(priv);
+	mutex_unlock(&priv->state_lock);
+}
+
+void mlx5e_update_stats(struct mlx5e_priv *priv)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	struct mlx5e_vport_stats *s = &priv->stats.vport;
+	struct mlx5e_rq_stats *rq_stats;
+	struct mlx5e_sq_stats *sq_stats;
+	u32 in[MLX5_ST_SZ_DW(query_vport_counter_in)];
+	u32 *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_vport_counter_out);
+	u64 tx_offload_none;
+	int i, j;
+
+	out = mlx5_vzalloc(outlen);
+	if (!out)
+		return;
+
+	/* Collect firts the SW counters and then HW for consistency */
+	s->tso_packets		= 0;
+	s->tso_bytes		= 0;
+	s->tx_queue_stopped	= 0;
+	s->tx_queue_wake	= 0;
+	s->tx_queue_dropped	= 0;
+	tx_offload_none		= 0;
+	s->lro_packets		= 0;
+	s->lro_bytes		= 0;
+	s->rx_csum_none		= 0;
+	s->rx_wqe_err		= 0;
+	for (i = 0; i < priv->params.num_channels; i++) {
+		rq_stats = &priv->channel[i]->rq.stats;
+
+		s->lro_packets	+= rq_stats->lro_packets;
+		s->lro_bytes	+= rq_stats->lro_bytes;
+		s->rx_csum_none	+= rq_stats->csum_none;
+		s->rx_wqe_err   += rq_stats->wqe_err;
+
+		for (j = 0; j < priv->num_tc; j++) {
+			sq_stats = &priv->channel[i]->sq[j].stats;
+
+			s->tso_packets		+= sq_stats->tso_packets;
+			s->tso_bytes		+= sq_stats->tso_bytes;
+			s->tx_queue_stopped	+= sq_stats->stopped;
+			s->tx_queue_wake	+= sq_stats->wake;
+			s->tx_queue_dropped	+= sq_stats->dropped;
+			tx_offload_none		+= sq_stats->csum_offload_none;
+		}
+	}
+
+	/* HW counters */
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(query_vport_counter_in, in, opcode,
+		 MLX5_CMD_OP_QUERY_VPORT_COUNTER);
+	MLX5_SET(query_vport_counter_in, in, op_mod, 0);
+	MLX5_SET(query_vport_counter_in, in, other_vport, 0);
+
+	memset(out, 0, outlen);
+
+	if (mlx5_cmd_exec(mdev, in, sizeof(in), out, outlen))
+		goto free_out;
+
+#define MLX5_GET_CTR(p, x) \
+	MLX5_GET64(query_vport_counter_out, p, x)
+
+	s->rx_error_packets     =
+		MLX5_GET_CTR(out, received_errors.packets);
+	s->rx_error_bytes       =
+		MLX5_GET_CTR(out, received_errors.octets);
+	s->tx_error_packets     =
+		MLX5_GET_CTR(out, transmit_errors.packets);
+	s->tx_error_bytes       =
+		MLX5_GET_CTR(out, transmit_errors.octets);
+
+	s->rx_unicast_packets   =
+		MLX5_GET_CTR(out, received_eth_unicast.packets);
+	s->rx_unicast_bytes     =
+		MLX5_GET_CTR(out, received_eth_unicast.octets);
+	s->tx_unicast_packets   =
+		MLX5_GET_CTR(out, transmitted_eth_unicast.packets);
+	s->tx_unicast_bytes     =
+		MLX5_GET_CTR(out, transmitted_eth_unicast.octets);
+
+	s->rx_multicast_packets =
+		MLX5_GET_CTR(out, received_eth_multicast.packets);
+	s->rx_multicast_bytes   =
+		MLX5_GET_CTR(out, received_eth_multicast.octets);
+	s->tx_multicast_packets =
+		MLX5_GET_CTR(out, transmitted_eth_multicast.packets);
+	s->tx_multicast_bytes   =
+		MLX5_GET_CTR(out, transmitted_eth_multicast.octets);
+
+	s->rx_broadcast_packets =
+		MLX5_GET_CTR(out, received_eth_broadcast.packets);
+	s->rx_broadcast_bytes   =
+		MLX5_GET_CTR(out, received_eth_broadcast.octets);
+	s->tx_broadcast_packets =
+		MLX5_GET_CTR(out, transmitted_eth_broadcast.packets);
+	s->tx_broadcast_bytes   =
+		MLX5_GET_CTR(out, transmitted_eth_broadcast.octets);
+
+	s->rx_packets =
+		s->rx_unicast_packets +
+		s->rx_multicast_packets +
+		s->rx_broadcast_packets;
+	s->rx_bytes =
+		s->rx_unicast_bytes +
+		s->rx_multicast_bytes +
+		s->rx_broadcast_bytes;
+	s->tx_packets =
+		s->tx_unicast_packets +
+		s->tx_multicast_packets +
+		s->tx_broadcast_packets;
+	s->tx_bytes =
+		s->tx_unicast_bytes +
+		s->tx_multicast_bytes +
+		s->tx_broadcast_bytes;
+
+	/* Update calculated offload counters */
+	s->tx_csum_offload = s->tx_packets - tx_offload_none;
+	s->rx_csum_good    = s->rx_packets - s->rx_csum_none;
+
+free_out:
+	kvfree(out);
+}
+
+static void mlx5e_update_stats_work(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct mlx5e_priv *priv = container_of(dwork, struct mlx5e_priv,
+					       update_stats_work);
+	mutex_lock(&priv->state_lock);
+	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
+		mlx5e_update_stats(priv);
+		schedule_delayed_work(dwork,
+				      msecs_to_jiffies(
+					      MLX5E_UPDATE_STATS_INTERVAL));
+	}
+	mutex_unlock(&priv->state_lock);
+}
+
+static void __mlx5e_async_event(struct mlx5e_priv *priv,
+				enum mlx5_dev_event event)
+{
+	switch (event) {
+	case MLX5_DEV_EVENT_PORT_UP:
+	case MLX5_DEV_EVENT_PORT_DOWN:
+		schedule_work(&priv->update_carrier_work);
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void mlx5e_async_event(struct mlx5_core_dev *mdev, void *vpriv,
+			      enum mlx5_dev_event event, unsigned long param)
+{
+	struct mlx5e_priv *priv = vpriv;
+
+	spin_lock(&priv->async_events_spinlock);
+	if (test_bit(MLX5E_STATE_ASYNC_EVENTS_ENABLE, &priv->state))
+		__mlx5e_async_event(priv, event);
+	spin_unlock(&priv->async_events_spinlock);
+}
+
+static void mlx5e_enable_async_events(struct mlx5e_priv *priv)
+{
+	set_bit(MLX5E_STATE_ASYNC_EVENTS_ENABLE, &priv->state);
+}
+
+static void mlx5e_disable_async_events(struct mlx5e_priv *priv)
+{
+	spin_lock_irq(&priv->async_events_spinlock);
+	clear_bit(MLX5E_STATE_ASYNC_EVENTS_ENABLE, &priv->state);
+	spin_unlock_irq(&priv->async_events_spinlock);
+}
+
+#define MLX5E_HW2SW_MTU(hwmtu) (hwmtu - (ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN))
+#define MLX5E_SW2HW_MTU(swmtu) (swmtu + (ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN))
+
+static int mlx5e_create_rq(struct mlx5e_channel *c,
+			   struct mlx5e_rq_param *param,
+			   struct mlx5e_rq *rq)
+{
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+	void *rqc = param->rqc;
+	void *rqc_wq = MLX5_ADDR_OF(rqc, rqc, wq);
+	int wq_sz;
+	int err;
+	int i;
+
+	err = mlx5_wq_ll_create(mdev, &param->wq, rqc_wq, &rq->wq,
+				&rq->wq_ctrl);
+	if (err)
+		return err;
+
+	rq->wq.db = &rq->wq.db[MLX5_RCV_DBR];
+
+	wq_sz = mlx5_wq_ll_get_size(&rq->wq);
+	rq->skb = kzalloc_node(wq_sz * sizeof(*rq->skb), GFP_KERNEL,
+			       cpu_to_node(c->cpu));
+	if (!rq->skb) {
+		err = -ENOMEM;
+		goto err_rq_wq_destroy;
+	}
+
+	rq->wqe_sz = (priv->params.lro_en) ? priv->params.lro_wqe_sz :
+					     MLX5E_SW2HW_MTU(priv->netdev->mtu);
+	rq->wqe_sz = SKB_DATA_ALIGN(rq->wqe_sz + MLX5E_NET_IP_ALIGN);
+
+	for (i = 0; i < wq_sz; i++) {
+		struct mlx5e_rx_wqe *wqe = mlx5_wq_ll_get_wqe(&rq->wq, i);
+		u32 byte_count = rq->wqe_sz - MLX5E_NET_IP_ALIGN;
+
+		wqe->data.lkey       = c->mkey_be;
+		wqe->data.byte_count =
+			cpu_to_be32(byte_count | MLX5_HW_START_PADDING);
+	}
+
+	rq->pdev    = c->pdev;
+	rq->netdev  = c->netdev;
+	rq->channel = c;
+	rq->ix      = c->ix;
+
+	return 0;
+
+err_rq_wq_destroy:
+	mlx5_wq_destroy(&rq->wq_ctrl);
+
+	return err;
+}
+
+static void mlx5e_destroy_rq(struct mlx5e_rq *rq)
+{
+	kfree(rq->skb);
+	mlx5_wq_destroy(&rq->wq_ctrl);
+}
+
+static int mlx5e_enable_rq(struct mlx5e_rq *rq, struct mlx5e_rq_param *param)
+{
+	struct mlx5e_channel *c = rq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	void *in;
+	void *rqc;
+	void *wq;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(create_rq_in) +
+		sizeof(u64) * rq->wq_ctrl.buf.npages;
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	rqc = MLX5_ADDR_OF(create_rq_in, in, ctx);
+	wq  = MLX5_ADDR_OF(rqc, rqc, wq);
+
+	memcpy(rqc, param->rqc, sizeof(param->rqc));
+
+	MLX5_SET(rqc,  rqc, cqn,		c->rq.cq.mcq.cqn);
+	MLX5_SET(rqc,  rqc, state,		MLX5_RQC_STATE_RST);
+	MLX5_SET(rqc,  rqc, flush_in_error_en,	1);
+	MLX5_SET(wq,   wq,  log_wq_pg_sz,	rq->wq_ctrl.buf.page_shift -
+						PAGE_SHIFT);
+	MLX5_SET64(wq, wq,  dbr_addr,		rq->wq_ctrl.db.dma);
+
+	mlx5_fill_page_array(&rq->wq_ctrl.buf,
+			     (__be64 *)MLX5_ADDR_OF(wq, wq, pas));
+
+	err = mlx5_core_create_rq(mdev, in, inlen, &rq->rqn);
+
+	kvfree(in);
+
+	return err;
+}
+
+static int mlx5e_modify_rq(struct mlx5e_rq *rq, int curr_state, int next_state)
+{
+	struct mlx5e_channel *c = rq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	void *in;
+	void *rqc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(modify_rq_in);
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	rqc = MLX5_ADDR_OF(modify_rq_in, in, ctx);
+
+	MLX5_SET(modify_rq_in, in, rq_state, curr_state);
+	MLX5_SET(rqc, rqc, state, next_state);
+
+	err = mlx5_core_modify_rq(mdev, rq->rqn, in, inlen);
+
+	kvfree(in);
+
+	return err;
+}
+
+static void mlx5e_disable_rq(struct mlx5e_rq *rq)
+{
+	struct mlx5e_channel *c = rq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	mlx5_core_destroy_rq(mdev, rq->rqn);
+}
+
+static int mlx5e_wait_for_min_rx_wqes(struct mlx5e_rq *rq)
+{
+	struct mlx5e_channel *c = rq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_wq_ll *wq = &rq->wq;
+	int i;
+
+	for (i = 0; i < 1000; i++) {
+		if (wq->cur_sz >= priv->params.min_rx_wqes)
+			return 0;
+
+		msleep(20);
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int mlx5e_open_rq(struct mlx5e_channel *c,
+			 struct mlx5e_rq_param *param,
+			 struct mlx5e_rq *rq)
+{
+	int err;
+
+	err = mlx5e_create_rq(c, param, rq);
+	if (err)
+		return err;
+
+	err = mlx5e_enable_rq(rq, param);
+	if (err)
+		goto err_destroy_rq;
+
+	err = mlx5e_modify_rq(rq, MLX5_RQC_STATE_RST, MLX5_RQC_STATE_RDY);
+	if (err)
+		goto err_disable_rq;
+
+	set_bit(MLX5E_RQ_STATE_POST_WQES_ENABLE, &rq->state);
+	mlx5e_send_nop(&c->sq[0], true); /* trigger mlx5e_post_rx_wqes() */
+
+	return 0;
+
+err_disable_rq:
+	mlx5e_disable_rq(rq);
+err_destroy_rq:
+	mlx5e_destroy_rq(rq);
+
+	return err;
+}
+
+static void mlx5e_close_rq(struct mlx5e_rq *rq)
+{
+	clear_bit(MLX5E_RQ_STATE_POST_WQES_ENABLE, &rq->state);
+	napi_synchronize(&rq->channel->napi); /* prevent mlx5e_post_rx_wqes */
+
+	mlx5e_modify_rq(rq, MLX5_RQC_STATE_RDY, MLX5_RQC_STATE_ERR);
+	while (!mlx5_wq_ll_is_empty(&rq->wq))
+		msleep(20);
+
+	/* avoid destroying rq before mlx5e_poll_rx_cq() is done with it */
+	napi_synchronize(&rq->channel->napi);
+
+	mlx5e_disable_rq(rq);
+	mlx5e_destroy_rq(rq);
+}
+
+static void mlx5e_free_sq_db(struct mlx5e_sq *sq)
+{
+	kfree(sq->dma_fifo);
+	kfree(sq->skb);
+}
+
+static int mlx5e_alloc_sq_db(struct mlx5e_sq *sq, int numa)
+{
+	int wq_sz = mlx5_wq_cyc_get_size(&sq->wq);
+	int df_sz = wq_sz * MLX5_SEND_WQEBB_NUM_DS;
+
+	sq->skb = kzalloc_node(wq_sz * sizeof(*sq->skb), GFP_KERNEL, numa);
+	sq->dma_fifo = kzalloc_node(df_sz * sizeof(*sq->dma_fifo), GFP_KERNEL,
+				    numa);
+
+	if (!sq->skb || !sq->dma_fifo) {
+		mlx5e_free_sq_db(sq);
+		return -ENOMEM;
+	}
+
+	sq->dma_fifo_mask = df_sz - 1;
+
+	return 0;
+}
+
+static int mlx5e_create_sq(struct mlx5e_channel *c,
+			   int tc,
+			   struct mlx5e_sq_param *param,
+			   struct mlx5e_sq *sq)
+{
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	void *sqc = param->sqc;
+	void *sqc_wq = MLX5_ADDR_OF(sqc, sqc, wq);
+	int txq_ix;
+	int err;
+
+	err = mlx5_alloc_map_uar(mdev, &sq->uar);
+	if (err)
+		return err;
+
+	err = mlx5_wq_cyc_create(mdev, &param->wq, sqc_wq, &sq->wq,
+				 &sq->wq_ctrl);
+	if (err)
+		goto err_unmap_free_uar;
+
+	sq->wq.db       = &sq->wq.db[MLX5_SND_DBR];
+	sq->uar_map     = sq->uar.map;
+	sq->bf_buf_size = (1 << MLX5_CAP_GEN(mdev, log_bf_reg_size)) / 2;
+
+	err = mlx5e_alloc_sq_db(sq, cpu_to_node(c->cpu));
+	if (err)
+		goto err_sq_wq_destroy;
+
+	txq_ix = c->ix + tc * priv->params.num_channels;
+	sq->txq = netdev_get_tx_queue(priv->netdev, txq_ix);
+
+	sq->pdev    = c->pdev;
+	sq->mkey_be = c->mkey_be;
+	sq->channel = c;
+	sq->tc      = tc;
+	sq->edge    = (sq->wq.sz_m1 + 1) - MLX5_SEND_WQE_MAX_WQEBBS;
+	priv->txq_to_sq_map[txq_ix] = sq;
+
+	return 0;
+
+err_sq_wq_destroy:
+	mlx5_wq_destroy(&sq->wq_ctrl);
+
+err_unmap_free_uar:
+	mlx5_unmap_free_uar(mdev, &sq->uar);
+
+	return err;
+}
+
+static void mlx5e_destroy_sq(struct mlx5e_sq *sq)
+{
+	struct mlx5e_channel *c = sq->channel;
+	struct mlx5e_priv *priv = c->priv;
+
+	mlx5e_free_sq_db(sq);
+	mlx5_wq_destroy(&sq->wq_ctrl);
+	mlx5_unmap_free_uar(priv->mdev, &sq->uar);
+}
+
+static int mlx5e_enable_sq(struct mlx5e_sq *sq, struct mlx5e_sq_param *param)
+{
+	struct mlx5e_channel *c = sq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	void *in;
+	void *sqc;
+	void *wq;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(create_sq_in) +
+		sizeof(u64) * sq->wq_ctrl.buf.npages;
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	sqc = MLX5_ADDR_OF(create_sq_in, in, ctx);
+	wq = MLX5_ADDR_OF(sqc, sqc, wq);
+
+	memcpy(sqc, param->sqc, sizeof(param->sqc));
+
+	MLX5_SET(sqc,  sqc, user_index,		sq->tc);
+	MLX5_SET(sqc,  sqc, tis_num_0,		priv->tisn[sq->tc]);
+	MLX5_SET(sqc,  sqc, cqn,		c->sq[sq->tc].cq.mcq.cqn);
+	MLX5_SET(sqc,  sqc, state,		MLX5_SQC_STATE_RST);
+	MLX5_SET(sqc,  sqc, tis_lst_sz,		1);
+	MLX5_SET(sqc,  sqc, flush_in_error_en,	1);
+
+	MLX5_SET(wq,   wq, wq_type,       MLX5_WQ_TYPE_CYCLIC);
+	MLX5_SET(wq,   wq, uar_page,      sq->uar.index);
+	MLX5_SET(wq,   wq, log_wq_pg_sz,  sq->wq_ctrl.buf.page_shift -
+					  PAGE_SHIFT);
+	MLX5_SET64(wq, wq, dbr_addr,      sq->wq_ctrl.db.dma);
+
+	mlx5_fill_page_array(&sq->wq_ctrl.buf,
+			     (__be64 *)MLX5_ADDR_OF(wq, wq, pas));
+
+	err = mlx5_core_create_sq(mdev, in, inlen, &sq->sqn);
+
+	kvfree(in);
+
+	return err;
+}
+
+static int mlx5e_modify_sq(struct mlx5e_sq *sq, int curr_state, int next_state)
+{
+	struct mlx5e_channel *c = sq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	void *in;
+	void *sqc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(modify_sq_in);
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	sqc = MLX5_ADDR_OF(modify_sq_in, in, ctx);
+
+	MLX5_SET(modify_sq_in, in, sq_state, curr_state);
+	MLX5_SET(sqc, sqc, state, next_state);
+
+	err = mlx5_core_modify_sq(mdev, sq->sqn, in, inlen);
+
+	kvfree(in);
+
+	return err;
+}
+
+static void mlx5e_disable_sq(struct mlx5e_sq *sq)
+{
+	struct mlx5e_channel *c = sq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	mlx5_core_destroy_sq(mdev, sq->sqn);
+}
+
+static int mlx5e_open_sq(struct mlx5e_channel *c,
+			 int tc,
+			 struct mlx5e_sq_param *param,
+			 struct mlx5e_sq *sq)
+{
+	int err;
+
+	err = mlx5e_create_sq(c, tc, param, sq);
+	if (err)
+		return err;
+
+	err = mlx5e_enable_sq(sq, param);
+	if (err)
+		goto err_destroy_sq;
+
+	err = mlx5e_modify_sq(sq, MLX5_SQC_STATE_RST, MLX5_SQC_STATE_RDY);
+	if (err)
+		goto err_disable_sq;
+
+	set_bit(MLX5E_SQ_STATE_WAKE_TXQ_ENABLE, &sq->state);
+	netdev_tx_reset_queue(sq->txq);
+	netif_tx_start_queue(sq->txq);
+
+	return 0;
+
+err_disable_sq:
+	mlx5e_disable_sq(sq);
+err_destroy_sq:
+	mlx5e_destroy_sq(sq);
+
+	return err;
+}
+
+static inline void netif_tx_disable_queue(struct netdev_queue *txq)
+{
+	__netif_tx_lock_bh(txq);
+	netif_tx_stop_queue(txq);
+	__netif_tx_unlock_bh(txq);
+}
+
+static void mlx5e_close_sq(struct mlx5e_sq *sq)
+{
+	clear_bit(MLX5E_SQ_STATE_WAKE_TXQ_ENABLE, &sq->state);
+	napi_synchronize(&sq->channel->napi); /* prevent netif_tx_wake_queue */
+	netif_tx_disable_queue(sq->txq);
+
+	/* ensure hw is notified of all pending wqes */
+	if (mlx5e_sq_has_room_for(sq, 1))
+		mlx5e_send_nop(sq, true);
+
+	mlx5e_modify_sq(sq, MLX5_SQC_STATE_RDY, MLX5_SQC_STATE_ERR);
+	while (sq->cc != sq->pc) /* wait till sq is empty */
+		msleep(20);
+
+	/* avoid destroying sq before mlx5e_poll_tx_cq() is done with it */
+	napi_synchronize(&sq->channel->napi);
+
+	mlx5e_disable_sq(sq);
+	mlx5e_destroy_sq(sq);
+}
+
+static int mlx5e_create_cq(struct mlx5e_channel *c,
+			   struct mlx5e_cq_param *param,
+			   struct mlx5e_cq *cq)
+{
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+	struct mlx5_core_cq *mcq = &cq->mcq;
+	int eqn_not_used;
+	int irqn;
+	int err;
+	u32 i;
+
+	param->wq.numa = cpu_to_node(c->cpu);
+	param->eq_ix   = c->ix;
+
+	err = mlx5_cqwq_create(mdev, &param->wq, param->cqc, &cq->wq,
+			       &cq->wq_ctrl);
+	if (err)
+		return err;
+
+	mlx5_vector2eqn(mdev, param->eq_ix, &eqn_not_used, &irqn);
+
+	cq->napi        = &c->napi;
+
+	mcq->cqe_sz     = 64;
+	mcq->set_ci_db  = cq->wq_ctrl.db.db;
+	mcq->arm_db     = cq->wq_ctrl.db.db + 1;
+	*mcq->set_ci_db = 0;
+	*mcq->arm_db    = 0;
+	mcq->vector     = param->eq_ix;
+	mcq->comp       = mlx5e_completion_event;
+	mcq->event      = mlx5e_cq_error_event;
+	mcq->irqn       = irqn;
+	mcq->uar        = &priv->cq_uar;
+
+	for (i = 0; i < mlx5_cqwq_get_size(&cq->wq); i++) {
+		struct mlx5_cqe64 *cqe = mlx5_cqwq_get_wqe(&cq->wq, i);
+
+		cqe->op_own = 0xf1;
+	}
+
+	cq->channel = c;
+
+	return 0;
+}
+
+static void mlx5e_destroy_cq(struct mlx5e_cq *cq)
+{
+	mlx5_wq_destroy(&cq->wq_ctrl);
+}
+
+static int mlx5e_enable_cq(struct mlx5e_cq *cq, struct mlx5e_cq_param *param)
+{
+	struct mlx5e_channel *c = cq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+	struct mlx5_core_cq *mcq = &cq->mcq;
+
+	void *in;
+	void *cqc;
+	int inlen;
+	int irqn_not_used;
+	int eqn;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(create_cq_in) +
+		sizeof(u64) * cq->wq_ctrl.buf.npages;
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	cqc = MLX5_ADDR_OF(create_cq_in, in, cq_context);
+
+	memcpy(cqc, param->cqc, sizeof(param->cqc));
+
+	mlx5_fill_page_array(&cq->wq_ctrl.buf,
+			     (__be64 *)MLX5_ADDR_OF(create_cq_in, in, pas));
+
+	mlx5_vector2eqn(mdev, param->eq_ix, &eqn, &irqn_not_used);
+
+	MLX5_SET(cqc,   cqc, c_eqn,         eqn);
+	MLX5_SET(cqc,   cqc, uar_page,      mcq->uar->index);
+	MLX5_SET(cqc,   cqc, log_page_size, cq->wq_ctrl.buf.page_shift -
+					    PAGE_SHIFT);
+	MLX5_SET64(cqc, cqc, dbr_addr,      cq->wq_ctrl.db.dma);
+
+	err = mlx5_core_create_cq(mdev, mcq, in, inlen);
+
+	kvfree(in);
+
+	if (err)
+		return err;
+
+	mlx5e_cq_arm(cq);
+
+	return 0;
+}
+
+static void mlx5e_disable_cq(struct mlx5e_cq *cq)
+{
+	struct mlx5e_channel *c = cq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	mlx5_core_destroy_cq(mdev, &cq->mcq);
+}
+
+static int mlx5e_open_cq(struct mlx5e_channel *c,
+			 struct mlx5e_cq_param *param,
+			 struct mlx5e_cq *cq,
+			 u16 moderation_usecs,
+			 u16 moderation_frames)
+{
+	int err;
+	struct mlx5e_priv *priv = c->priv;
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	err = mlx5e_create_cq(c, param, cq);
+	if (err)
+		return err;
+
+	err = mlx5e_enable_cq(cq, param);
+	if (err)
+		goto err_destroy_cq;
+
+	err = mlx5_core_modify_cq_moderation(mdev, &cq->mcq,
+					     moderation_usecs,
+					     moderation_frames);
+	if (err)
+		goto err_destroy_cq;
+
+	return 0;
+
+err_destroy_cq:
+	mlx5e_destroy_cq(cq);
+
+	return err;
+}
+
+static void mlx5e_close_cq(struct mlx5e_cq *cq)
+{
+	mlx5e_disable_cq(cq);
+	mlx5e_destroy_cq(cq);
+}
+
+static int mlx5e_get_cpu(struct mlx5e_priv *priv, int ix)
+{
+	return cpumask_first(priv->mdev->priv.irq_info[ix].mask);
+}
+
+static int mlx5e_open_tx_cqs(struct mlx5e_channel *c,
+			     struct mlx5e_channel_param *cparam)
+{
+	struct mlx5e_priv *priv = c->priv;
+	int err;
+	int tc;
+
+	for (tc = 0; tc < c->num_tc; tc++) {
+		err = mlx5e_open_cq(c, &cparam->tx_cq, &c->sq[tc].cq,
+				    priv->params.tx_cq_moderation_usec,
+				    priv->params.tx_cq_moderation_pkts);
+		if (err)
+			goto err_close_tx_cqs;
+	}
+
+	return 0;
+
+err_close_tx_cqs:
+	for (tc--; tc >= 0; tc--)
+		mlx5e_close_cq(&c->sq[tc].cq);
+
+	return err;
+}
+
+static void mlx5e_close_tx_cqs(struct mlx5e_channel *c)
+{
+	int tc;
+
+	for (tc = 0; tc < c->num_tc; tc++)
+		mlx5e_close_cq(&c->sq[tc].cq);
+}
+
+static int mlx5e_open_sqs(struct mlx5e_channel *c,
+			  struct mlx5e_channel_param *cparam)
+{
+	int err;
+	int tc;
+
+	for (tc = 0; tc < c->num_tc; tc++) {
+		err = mlx5e_open_sq(c, tc, &cparam->sq, &c->sq[tc]);
+		if (err)
+			goto err_close_sqs;
+	}
+
+	return 0;
+
+err_close_sqs:
+	for (tc--; tc >= 0; tc--)
+		mlx5e_close_sq(&c->sq[tc]);
+
+	return err;
+}
+
+static void mlx5e_close_sqs(struct mlx5e_channel *c)
+{
+	int tc;
+
+	for (tc = 0; tc < c->num_tc; tc++)
+		mlx5e_close_sq(&c->sq[tc]);
+}
+
+static void mlx5e_build_tc_to_txq_map(struct mlx5e_channel *c,
+				      int num_channels)
+{
+	int i;
+
+	for (i = 0; i < MLX5E_MAX_NUM_TC; i++)
+		c->tc_to_txq_map[i] = c->ix + i * num_channels;
+}
+
+static int mlx5e_open_channel(struct mlx5e_priv *priv, int ix,
+			      struct mlx5e_channel_param *cparam,
+			      struct mlx5e_channel **cp)
+{
+	struct net_device *netdev = priv->netdev;
+	int cpu = mlx5e_get_cpu(priv, ix);
+	struct mlx5e_channel *c;
+	int err;
+
+	c = kzalloc_node(sizeof(*c), GFP_KERNEL, cpu_to_node(cpu));
+	if (!c)
+		return -ENOMEM;
+
+	c->priv     = priv;
+	c->ix       = ix;
+	c->cpu      = cpu;
+	c->pdev     = &priv->mdev->pdev->dev;
+	c->netdev   = priv->netdev;
+	c->mkey_be  = cpu_to_be32(priv->mr.key);
+	c->num_tc   = priv->num_tc;
+
+	mlx5e_build_tc_to_txq_map(c, priv->params.num_channels);
+
+	netif_napi_add(netdev, &c->napi, mlx5e_napi_poll, 64);
+
+	err = mlx5e_open_tx_cqs(c, cparam);
+	if (err)
+		goto err_napi_del;
+
+	err = mlx5e_open_cq(c, &cparam->rx_cq, &c->rq.cq,
+			    priv->params.rx_cq_moderation_usec,
+			    priv->params.rx_cq_moderation_pkts);
+	if (err)
+		goto err_close_tx_cqs;
+
+	napi_enable(&c->napi);
+
+	err = mlx5e_open_sqs(c, cparam);
+	if (err)
+		goto err_disable_napi;
+
+	err = mlx5e_open_rq(c, &cparam->rq, &c->rq);
+	if (err)
+		goto err_close_sqs;
+
+	netif_set_xps_queue(netdev, get_cpu_mask(c->cpu), ix);
+	*cp = c;
+
+	return 0;
+
+err_close_sqs:
+	mlx5e_close_sqs(c);
+
+err_disable_napi:
+	napi_disable(&c->napi);
+	mlx5e_close_cq(&c->rq.cq);
+
+err_close_tx_cqs:
+	mlx5e_close_tx_cqs(c);
+
+err_napi_del:
+	netif_napi_del(&c->napi);
+	kfree(c);
+
+	return err;
+}
+
+static void mlx5e_close_channel(struct mlx5e_channel *c)
+{
+	mlx5e_close_rq(&c->rq);
+	mlx5e_close_sqs(c);
+	napi_disable(&c->napi);
+	mlx5e_close_cq(&c->rq.cq);
+	mlx5e_close_tx_cqs(c);
+	netif_napi_del(&c->napi);
+	kfree(c);
+}
+
+static void mlx5e_build_rq_param(struct mlx5e_priv *priv,
+				 struct mlx5e_rq_param *param)
+{
+	void *rqc = param->rqc;
+	void *wq = MLX5_ADDR_OF(rqc, rqc, wq);
+
+	MLX5_SET(wq, wq, wq_type,          MLX5_WQ_TYPE_LINKED_LIST);
+	MLX5_SET(wq, wq, end_padding_mode, MLX5_WQ_END_PAD_MODE_ALIGN);
+	MLX5_SET(wq, wq, log_wq_stride,    ilog2(sizeof(struct mlx5e_rx_wqe)));
+	MLX5_SET(wq, wq, log_wq_sz,        priv->params.log_rq_size);
+	MLX5_SET(wq, wq, pd,               priv->pdn);
+
+	param->wq.numa   = dev_to_node(&priv->mdev->pdev->dev);
+	param->wq.linear = 1;
+}
+
+static void mlx5e_build_sq_param(struct mlx5e_priv *priv,
+				 struct mlx5e_sq_param *param)
+{
+	void *sqc = param->sqc;
+	void *wq = MLX5_ADDR_OF(sqc, sqc, wq);
+
+	MLX5_SET(wq, wq, log_wq_sz,     priv->params.log_sq_size);
+	MLX5_SET(wq, wq, log_wq_stride, ilog2(MLX5_SEND_WQE_BB));
+	MLX5_SET(wq, wq, pd,            priv->pdn);
+
+	param->wq.numa = dev_to_node(&priv->mdev->pdev->dev);
+}
+
+static void mlx5e_build_common_cq_param(struct mlx5e_priv *priv,
+					struct mlx5e_cq_param *param)
+{
+	void *cqc = param->cqc;
+
+	MLX5_SET(cqc, cqc, uar_page, priv->cq_uar.index);
+}
+
+static void mlx5e_build_rx_cq_param(struct mlx5e_priv *priv,
+				    struct mlx5e_cq_param *param)
+{
+	void *cqc = param->cqc;
+
+	MLX5_SET(cqc, cqc, log_cq_size,  priv->params.log_rq_size);
+
+	mlx5e_build_common_cq_param(priv, param);
+}
+
+static void mlx5e_build_tx_cq_param(struct mlx5e_priv *priv,
+				    struct mlx5e_cq_param *param)
+{
+	void *cqc = param->cqc;
+
+	MLX5_SET(cqc, cqc, log_cq_size,  priv->params.log_sq_size);
+
+	mlx5e_build_common_cq_param(priv, param);
+}
+
+static void mlx5e_build_channel_param(struct mlx5e_priv *priv,
+				      struct mlx5e_channel_param *cparam)
+{
+	memset(cparam, 0, sizeof(*cparam));
+
+	mlx5e_build_rq_param(priv, &cparam->rq);
+	mlx5e_build_sq_param(priv, &cparam->sq);
+	mlx5e_build_rx_cq_param(priv, &cparam->rx_cq);
+	mlx5e_build_tx_cq_param(priv, &cparam->tx_cq);
+}
+
+static int mlx5e_open_channels(struct mlx5e_priv *priv)
+{
+	struct mlx5e_channel_param cparam;
+	int err = -ENOMEM;
+	int i;
+	int j;
+
+	priv->channel = kcalloc(priv->params.num_channels,
+				sizeof(struct mlx5e_channel *), GFP_KERNEL);
+
+	priv->txq_to_sq_map = kcalloc(priv->params.num_channels * priv->num_tc,
+				      sizeof(struct mlx5e_sq *), GFP_KERNEL);
+
+	if (!priv->channel || !priv->txq_to_sq_map)
+		goto err_free_txq_to_sq_map;
+
+	mlx5e_build_channel_param(priv, &cparam);
+	for (i = 0; i < priv->params.num_channels; i++) {
+		err = mlx5e_open_channel(priv, i, &cparam, &priv->channel[i]);
+		if (err)
+			goto err_close_channels;
+	}
+
+	for (j = 0; j < priv->params.num_channels; j++) {
+		err = mlx5e_wait_for_min_rx_wqes(&priv->channel[j]->rq);
+		if (err)
+			goto err_close_channels;
+	}
+
+	return 0;
+
+err_close_channels:
+	for (i--; i >= 0; i--)
+		mlx5e_close_channel(priv->channel[i]);
+
+err_free_txq_to_sq_map:
+	kfree(priv->txq_to_sq_map);
+	kfree(priv->channel);
+
+	return err;
+}
+
+static void mlx5e_close_channels(struct mlx5e_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < priv->params.num_channels; i++)
+		mlx5e_close_channel(priv->channel[i]);
+
+	kfree(priv->txq_to_sq_map);
+	kfree(priv->channel);
+}
+
+static int mlx5e_open_tis(struct mlx5e_priv *priv, int tc)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	u32 in[MLX5_ST_SZ_DW(create_tis_in)];
+	void *tisc = MLX5_ADDR_OF(create_tis_in, in, ctx);
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(tisc, tisc, prio,  tc);
+	MLX5_SET(tisc, tisc, transport_domain, priv->tdn);
+
+	return mlx5_core_create_tis(mdev, in, sizeof(in), &priv->tisn[tc]);
+}
+
+static void mlx5e_close_tis(struct mlx5e_priv *priv, int tc)
+{
+	mlx5_core_destroy_tis(priv->mdev, priv->tisn[tc]);
+}
+
+static int mlx5e_open_tises(struct mlx5e_priv *priv)
+{
+	int num_tc = priv->num_tc;
+	int err;
+	int tc;
+
+	for (tc = 0; tc < num_tc; tc++) {
+		err = mlx5e_open_tis(priv, tc);
+		if (err)
+			goto err_close_tises;
+	}
+
+	return 0;
+
+err_close_tises:
+	for (tc--; tc >= 0; tc--)
+		mlx5e_close_tis(priv, tc);
+
+	return err;
+}
+
+static void mlx5e_close_tises(struct mlx5e_priv *priv)
+{
+	int num_tc = priv->num_tc;
+	int tc;
+
+	for (tc = 0; tc < num_tc; tc++)
+		mlx5e_close_tis(priv, tc);
+}
+
+static int mlx5e_open_rqt(struct mlx5e_priv *priv)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	u32 *in;
+	u32 out[MLX5_ST_SZ_DW(create_rqt_out)];
+	void *rqtc;
+	int inlen;
+	int err;
+	int sz;
+	int i;
+
+	sz = 1 << priv->params.rx_hash_log_tbl_sz;
+
+	inlen = MLX5_ST_SZ_BYTES(create_rqt_in) + sizeof(u32) * sz;
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	rqtc = MLX5_ADDR_OF(create_rqt_in, in, rqt_context);
+
+	MLX5_SET(rqtc, rqtc, rqt_actual_size, sz);
+	MLX5_SET(rqtc, rqtc, rqt_max_size, sz);
+
+	for (i = 0; i < sz; i++) {
+		int ix = i % priv->params.num_channels;
+
+		MLX5_SET(rqtc, rqtc, rq_num[i], priv->channel[ix]->rq.rqn);
+	}
+
+	MLX5_SET(create_rqt_in, in, opcode, MLX5_CMD_OP_CREATE_RQT);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(mdev, in, inlen, out, sizeof(out));
+	if (!err)
+		priv->rqtn = MLX5_GET(create_rqt_out, out, rqtn);
+
+	kvfree(in);
+
+	return err;
+}
+
+static void mlx5e_close_rqt(struct mlx5e_priv *priv)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_rqt_in)];
+	u32 out[MLX5_ST_SZ_DW(destroy_rqt_out)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(destroy_rqt_in, in, opcode, MLX5_CMD_OP_DESTROY_RQT);
+	MLX5_SET(destroy_rqt_in, in, rqtn, priv->rqtn);
+
+	mlx5_cmd_exec_check_status(priv->mdev, in, sizeof(in), out,
+				   sizeof(out));
+}
+
+static void mlx5e_build_tir_ctx(struct mlx5e_priv *priv, u32 *tirc, int tt)
+{
+	void *hfso = MLX5_ADDR_OF(tirc, tirc, rx_hash_field_selector_outer);
+
+	MLX5_SET(tirc, tirc, transport_domain, priv->tdn);
+
+#define ROUGH_MAX_L2_L3_HDR_SZ 256
+
+#define MLX5_HASH_IP     (MLX5_HASH_FIELD_SEL_SRC_IP   |\
+			  MLX5_HASH_FIELD_SEL_DST_IP)
+
+#define MLX5_HASH_ALL    (MLX5_HASH_FIELD_SEL_SRC_IP   |\
+			  MLX5_HASH_FIELD_SEL_DST_IP   |\
+			  MLX5_HASH_FIELD_SEL_L4_SPORT |\
+			  MLX5_HASH_FIELD_SEL_L4_DPORT)
+
+	if (priv->params.lro_en) {
+		MLX5_SET(tirc, tirc, lro_enable_mask,
+			 MLX5_TIRC_LRO_ENABLE_MASK_IPV4_LRO |
+			 MLX5_TIRC_LRO_ENABLE_MASK_IPV6_LRO);
+		MLX5_SET(tirc, tirc, lro_max_ip_payload_size,
+			 (priv->params.lro_wqe_sz -
+			  ROUGH_MAX_L2_L3_HDR_SZ) >> 8);
+		MLX5_SET(tirc, tirc, lro_timeout_period_usecs,
+			 MLX5_CAP_ETH(priv->mdev,
+				      lro_timer_supported_periods[3]));
+	}
+
+	switch (tt) {
+	case MLX5E_TT_ANY:
+		MLX5_SET(tirc, tirc, disp_type,
+			 MLX5_TIRC_DISP_TYPE_DIRECT);
+		MLX5_SET(tirc, tirc, inline_rqn,
+			 priv->channel[0]->rq.rqn);
+		break;
+	default:
+		MLX5_SET(tirc, tirc, disp_type,
+			 MLX5_TIRC_DISP_TYPE_INDIRECT);
+		MLX5_SET(tirc, tirc, indirect_table,
+			 priv->rqtn);
+		MLX5_SET(tirc, tirc, rx_hash_fn,
+			 MLX5_TIRC_RX_HASH_FN_HASH_TOEPLITZ);
+		MLX5_SET(tirc, tirc, rx_hash_symmetric, 1);
+		netdev_rss_key_fill(MLX5_ADDR_OF(tirc, tirc,
+						 rx_hash_toeplitz_key),
+				    MLX5_FLD_SZ_BYTES(tirc,
+						      rx_hash_toeplitz_key));
+		break;
+	}
+
+	switch (tt) {
+	case MLX5E_TT_IPV4_TCP:
+		MLX5_SET(rx_hash_field_select, hfso, l3_prot_type,
+			 MLX5_L3_PROT_TYPE_IPV4);
+		MLX5_SET(rx_hash_field_select, hfso, l4_prot_type,
+			 MLX5_L4_PROT_TYPE_TCP);
+		MLX5_SET(rx_hash_field_select, hfso, selected_fields,
+			 MLX5_HASH_ALL);
+		break;
+
+	case MLX5E_TT_IPV6_TCP:
+		MLX5_SET(rx_hash_field_select, hfso, l3_prot_type,
+			 MLX5_L3_PROT_TYPE_IPV6);
+		MLX5_SET(rx_hash_field_select, hfso, l4_prot_type,
+			 MLX5_L4_PROT_TYPE_TCP);
+		MLX5_SET(rx_hash_field_select, hfso, selected_fields,
+			 MLX5_HASH_ALL);
+		break;
+
+	case MLX5E_TT_IPV4_UDP:
+		MLX5_SET(rx_hash_field_select, hfso, l3_prot_type,
+			 MLX5_L3_PROT_TYPE_IPV4);
+		MLX5_SET(rx_hash_field_select, hfso, l4_prot_type,
+			 MLX5_L4_PROT_TYPE_UDP);
+		MLX5_SET(rx_hash_field_select, hfso, selected_fields,
+			 MLX5_HASH_ALL);
+		break;
+
+	case MLX5E_TT_IPV6_UDP:
+		MLX5_SET(rx_hash_field_select, hfso, l3_prot_type,
+			 MLX5_L3_PROT_TYPE_IPV6);
+		MLX5_SET(rx_hash_field_select, hfso, l4_prot_type,
+			 MLX5_L4_PROT_TYPE_UDP);
+		MLX5_SET(rx_hash_field_select, hfso, selected_fields,
+			 MLX5_HASH_ALL);
+		break;
+
+	case MLX5E_TT_IPV4:
+		MLX5_SET(rx_hash_field_select, hfso, l3_prot_type,
+			 MLX5_L3_PROT_TYPE_IPV4);
+		MLX5_SET(rx_hash_field_select, hfso, selected_fields,
+			 MLX5_HASH_IP);
+		break;
+
+	case MLX5E_TT_IPV6:
+		MLX5_SET(rx_hash_field_select, hfso, l3_prot_type,
+			 MLX5_L3_PROT_TYPE_IPV6);
+		MLX5_SET(rx_hash_field_select, hfso, selected_fields,
+			 MLX5_HASH_IP);
+		break;
+	}
+}
+
+static int mlx5e_open_tir(struct mlx5e_priv *priv, int tt)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	u32 *in;
+	void *tirc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(create_tir_in);
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	tirc = MLX5_ADDR_OF(create_tir_in, in, ctx);
+
+	mlx5e_build_tir_ctx(priv, tirc, tt);
+
+	err = mlx5_core_create_tir(mdev, in, inlen, &priv->tirn[tt]);
+
+	kvfree(in);
+
+	return err;
+}
+
+static void mlx5e_close_tir(struct mlx5e_priv *priv, int tt)
+{
+	mlx5_core_destroy_tir(priv->mdev, priv->tirn[tt]);
+}
+
+static int mlx5e_open_tirs(struct mlx5e_priv *priv)
+{
+	int err;
+	int i;
+
+	for (i = 0; i < MLX5E_NUM_TT; i++) {
+		err = mlx5e_open_tir(priv, i);
+		if (err)
+			goto err_close_tirs;
+	}
+
+	return 0;
+
+err_close_tirs:
+	for (i--; i >= 0; i--)
+		mlx5e_close_tir(priv, i);
+
+	return err;
+}
+
+static void mlx5e_close_tirs(struct mlx5e_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < MLX5E_NUM_TT; i++)
+		mlx5e_close_tir(priv, i);
+}
+
+static int mlx5e_set_dev_port_mtu(struct net_device *netdev)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+	int hw_mtu;
+	int err;
+
+	err = mlx5_set_port_mtu(mdev, MLX5E_SW2HW_MTU(netdev->mtu), 1);
+	if (err)
+		return err;
+
+	mlx5_query_port_oper_mtu(mdev, &hw_mtu, 1);
+
+	if (MLX5E_HW2SW_MTU(hw_mtu) != netdev->mtu)
+		netdev_warn(netdev, "%s: Port MTU %d is different than netdev mtu %d\n",
+			    __func__, MLX5E_HW2SW_MTU(hw_mtu), netdev->mtu);
+
+	netdev->mtu = MLX5E_HW2SW_MTU(hw_mtu);
+	return 0;
+}
+
+int mlx5e_open_locked(struct net_device *netdev)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	int num_txqs;
+	int err;
+
+	num_txqs = priv->params.num_channels * priv->params.num_tc;
+	netif_set_real_num_tx_queues(netdev, num_txqs);
+	netif_set_real_num_rx_queues(netdev, priv->params.num_channels);
+
+	err = mlx5e_set_dev_port_mtu(netdev);
+	if (err)
+		return err;
+
+	err = mlx5e_open_tises(priv);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5e_open_tises failed, %d\n",
+			   __func__, err);
+		return err;
+	}
+
+	err = mlx5e_open_channels(priv);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5e_open_channels failed, %d\n",
+			   __func__, err);
+		goto err_close_tises;
+	}
+
+	err = mlx5e_open_rqt(priv);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5e_open_rqt failed, %d\n",
+			   __func__, err);
+		goto err_close_channels;
+	}
+
+	err = mlx5e_open_tirs(priv);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5e_open_tir failed, %d\n",
+			   __func__, err);
+		goto err_close_rqls;
+	}
+
+	err = mlx5e_open_flow_table(priv);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5e_open_flow_table failed, %d\n",
+			   __func__, err);
+		goto err_close_tirs;
+	}
+
+	err = mlx5e_add_all_vlan_rules(priv);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5e_add_all_vlan_rules failed, %d\n",
+			   __func__, err);
+		goto err_close_flow_table;
+	}
+
+	mlx5e_init_eth_addr(priv);
+
+	set_bit(MLX5E_STATE_OPENED, &priv->state);
+
+	mlx5e_update_carrier(priv);
+	mlx5e_set_rx_mode_core(priv);
+
+	schedule_delayed_work(&priv->update_stats_work, 0);
+	return 0;
+
+err_close_flow_table:
+	mlx5e_close_flow_table(priv);
+
+err_close_tirs:
+	mlx5e_close_tirs(priv);
+
+err_close_rqls:
+	mlx5e_close_rqt(priv);
+
+err_close_channels:
+	mlx5e_close_channels(priv);
+
+err_close_tises:
+	mlx5e_close_tises(priv);
+
+	return err;
+}
+
+static int mlx5e_open(struct net_device *netdev)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	int err;
+
+	mutex_lock(&priv->state_lock);
+	err = mlx5e_open_locked(netdev);
+	mutex_unlock(&priv->state_lock);
+
+	return err;
+}
+
+int mlx5e_close_locked(struct net_device *netdev)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+
+	clear_bit(MLX5E_STATE_OPENED, &priv->state);
+
+	mlx5e_set_rx_mode_core(priv);
+	mlx5e_del_all_vlan_rules(priv);
+	netif_carrier_off(priv->netdev);
+	mlx5e_close_flow_table(priv);
+	mlx5e_close_tirs(priv);
+	mlx5e_close_rqt(priv);
+	mlx5e_close_channels(priv);
+	mlx5e_close_tises(priv);
+
+	return 0;
+}
+
+static int mlx5e_close(struct net_device *netdev)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	int err;
+
+	mutex_lock(&priv->state_lock);
+	err = mlx5e_close_locked(netdev);
+	mutex_unlock(&priv->state_lock);
+
+	return err;
+}
+
+int mlx5e_update_priv_params(struct mlx5e_priv *priv,
+			     struct mlx5e_params *new_params)
+{
+	int err = 0;
+	int was_opened;
+
+	WARN_ON(!mutex_is_locked(&priv->state_lock));
+
+	was_opened = test_bit(MLX5E_STATE_OPENED, &priv->state);
+	if (was_opened)
+		mlx5e_close_locked(priv->netdev);
+
+	priv->params = *new_params;
+
+	if (was_opened)
+		err = mlx5e_open_locked(priv->netdev);
+
+	return err;
+}
+
+static struct rtnl_link_stats64 *
+mlx5e_get_stats(struct net_device *dev, struct rtnl_link_stats64 *stats)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5e_vport_stats *vstats = &priv->stats.vport;
+
+	stats->rx_packets = vstats->rx_packets;
+	stats->rx_bytes   = vstats->rx_bytes;
+	stats->tx_packets = vstats->tx_packets;
+	stats->tx_bytes   = vstats->tx_bytes;
+	stats->multicast  = vstats->rx_multicast_packets +
+			    vstats->tx_multicast_packets;
+	stats->tx_errors  = vstats->tx_error_packets;
+	stats->rx_errors  = vstats->rx_error_packets;
+	stats->tx_dropped = vstats->tx_queue_dropped;
+	stats->rx_crc_errors = 0;
+	stats->rx_length_errors = 0;
+
+	return stats;
+}
+
+static void mlx5e_set_rx_mode(struct net_device *dev)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	schedule_work(&priv->set_rx_mode_work);
+}
+
+static int mlx5e_set_mac(struct net_device *netdev, void *addr)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct sockaddr *saddr = addr;
+
+	if (!is_valid_ether_addr(saddr->sa_data))
+		return -EADDRNOTAVAIL;
+
+	netif_addr_lock_bh(netdev);
+	ether_addr_copy(netdev->dev_addr, saddr->sa_data);
+	netif_addr_unlock_bh(netdev);
+
+	schedule_work(&priv->set_rx_mode_work);
+
+	return 0;
+}
+
+static int mlx5e_set_features(struct net_device *netdev,
+			      netdev_features_t features)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	netdev_features_t changes = features ^ netdev->features;
+	struct mlx5e_params new_params;
+	bool update_params = false;
+
+	mutex_lock(&priv->state_lock);
+	new_params = priv->params;
+
+	if (changes & NETIF_F_LRO) {
+		new_params.lro_en = !!(features & NETIF_F_LRO);
+		update_params = true;
+	}
+
+	if (update_params)
+		mlx5e_update_priv_params(priv, &new_params);
+
+	if (changes & NETIF_F_HW_VLAN_CTAG_FILTER) {
+		if (features & NETIF_F_HW_VLAN_CTAG_FILTER)
+			mlx5e_enable_vlan_filter(priv);
+		else
+			mlx5e_disable_vlan_filter(priv);
+	}
+
+	mutex_unlock(&priv->state_lock);
+
+	return 0;
+}
+
+static int mlx5e_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+	int max_mtu;
+	int err;
+
+	mlx5_query_port_max_mtu(mdev, &max_mtu, 1);
+
+	if (new_mtu > max_mtu) {
+		netdev_err(netdev,
+			   "%s: Bad MTU (%d) > (%d) Max\n",
+			   __func__, new_mtu, max_mtu);
+		return -EINVAL;
+	}
+
+	mutex_lock(&priv->state_lock);
+	netdev->mtu = new_mtu;
+	err = mlx5e_update_priv_params(priv, &priv->params);
+	mutex_unlock(&priv->state_lock);
+
+	return err;
+}
+
+static struct net_device_ops mlx5e_netdev_ops = {
+	.ndo_open                = mlx5e_open,
+	.ndo_stop                = mlx5e_close,
+	.ndo_start_xmit          = mlx5e_xmit,
+	.ndo_get_stats64         = mlx5e_get_stats,
+	.ndo_set_rx_mode         = mlx5e_set_rx_mode,
+	.ndo_set_mac_address     = mlx5e_set_mac,
+	.ndo_vlan_rx_add_vid	 = mlx5e_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	 = mlx5e_vlan_rx_kill_vid,
+	.ndo_set_features        = mlx5e_set_features,
+	.ndo_change_mtu		 = mlx5e_change_mtu,
+};
+
+static int mlx5e_check_required_hca_cap(struct mlx5_core_dev *mdev)
+{
+	if (MLX5_CAP_GEN(mdev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return -ENOTSUPP;
+	if (!MLX5_CAP_GEN(mdev, eth_net_offloads) ||
+	    !MLX5_CAP_GEN(mdev, nic_flow_table) ||
+	    !MLX5_CAP_ETH(mdev, csum_cap) ||
+	    !MLX5_CAP_ETH(mdev, max_lso_cap) ||
+	    !MLX5_CAP_ETH(mdev, vlan_cap) ||
+	    !MLX5_CAP_ETH(mdev, rss_ind_tbl_cap) ||
+	    MLX5_CAP_FLOWTABLE(mdev,
+			       flow_table_properties_nic_receive.max_ft_level)
+			       < 3) {
+		mlx5_core_warn(mdev,
+			       "Not creating net device, some required device capabilities are missing\n");
+		return -ENOTSUPP;
+	}
+	return 0;
+}
+
+static void mlx5e_build_netdev_priv(struct mlx5_core_dev *mdev,
+				    struct net_device *netdev,
+				    int num_comp_vectors)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+
+	priv->params.log_sq_size           =
+		MLX5E_PARAMS_DEFAULT_LOG_SQ_SIZE;
+	priv->params.log_rq_size           =
+		MLX5E_PARAMS_DEFAULT_LOG_RQ_SIZE;
+	priv->params.rx_cq_moderation_usec =
+		MLX5E_PARAMS_DEFAULT_RX_CQ_MODERATION_USEC;
+	priv->params.rx_cq_moderation_pkts =
+		MLX5E_PARAMS_DEFAULT_RX_CQ_MODERATION_PKTS;
+	priv->params.tx_cq_moderation_usec =
+		MLX5E_PARAMS_DEFAULT_TX_CQ_MODERATION_USEC;
+	priv->params.tx_cq_moderation_pkts =
+		MLX5E_PARAMS_DEFAULT_TX_CQ_MODERATION_PKTS;
+	priv->params.min_rx_wqes           =
+		MLX5E_PARAMS_DEFAULT_MIN_RX_WQES;
+	priv->params.rx_hash_log_tbl_sz    =
+		(order_base_2(num_comp_vectors) >
+		 MLX5E_PARAMS_DEFAULT_RX_HASH_LOG_TBL_SZ) ?
+		order_base_2(num_comp_vectors)           :
+		MLX5E_PARAMS_DEFAULT_RX_HASH_LOG_TBL_SZ;
+	priv->params.num_tc                = 1;
+	priv->params.default_vlan_prio     = 0;
+
+	priv->params.lro_en = false && !!MLX5_CAP_ETH(priv->mdev, lro_cap);
+	priv->params.lro_wqe_sz            =
+		MLX5E_PARAMS_DEFAULT_LRO_WQE_SZ;
+
+	priv->mdev                         = mdev;
+	priv->netdev                       = netdev;
+	priv->params.num_channels          = num_comp_vectors;
+	priv->num_tc                       = priv->params.num_tc;
+	priv->default_vlan_prio            = priv->params.default_vlan_prio;
+
+	spin_lock_init(&priv->async_events_spinlock);
+	mutex_init(&priv->state_lock);
+
+	INIT_WORK(&priv->update_carrier_work, mlx5e_update_carrier_work);
+	INIT_WORK(&priv->set_rx_mode_work, mlx5e_set_rx_mode_work);
+	INIT_DELAYED_WORK(&priv->update_stats_work, mlx5e_update_stats_work);
+}
+
+static void mlx5e_set_netdev_dev_addr(struct net_device *netdev)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+
+	mlx5_query_nic_vport_mac_address(priv->mdev, netdev->dev_addr);
+}
+
+static void mlx5e_build_netdev(struct net_device *netdev)
+{
+	struct mlx5e_priv *priv = netdev_priv(netdev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	SET_NETDEV_DEV(netdev, &mdev->pdev->dev);
+
+	if (priv->num_tc > 1) {
+		mlx5e_netdev_ops.ndo_select_queue = mlx5e_select_queue;
+	}
+
+	netdev->netdev_ops        = &mlx5e_netdev_ops;
+	netdev->watchdog_timeo    = 15 * HZ;
+
+	netdev->ethtool_ops	  = &mlx5e_ethtool_ops;
+
+	netdev->vlan_features    |= NETIF_F_SG;
+	netdev->vlan_features    |= NETIF_F_IP_CSUM;
+	netdev->vlan_features    |= NETIF_F_IPV6_CSUM;
+	netdev->vlan_features    |= NETIF_F_GRO;
+	netdev->vlan_features    |= NETIF_F_TSO;
+	netdev->vlan_features    |= NETIF_F_TSO6;
+	netdev->vlan_features    |= NETIF_F_RXCSUM;
+	netdev->vlan_features    |= NETIF_F_RXHASH;
+
+	if (!!MLX5_CAP_ETH(mdev, lro_cap))
+		netdev->vlan_features    |= NETIF_F_LRO;
+
+	netdev->hw_features       = netdev->vlan_features;
+	netdev->hw_features      |= NETIF_F_HW_VLAN_CTAG_RX;
+	netdev->hw_features      |= NETIF_F_HW_VLAN_CTAG_FILTER;
+
+	netdev->features          = netdev->hw_features;
+	if (!priv->params.lro_en)
+		netdev->features  &= ~NETIF_F_LRO;
+
+	netdev->features         |= NETIF_F_HIGHDMA;
+
+	netdev->priv_flags       |= IFF_UNICAST_FLT;
+
+	mlx5e_set_netdev_dev_addr(netdev);
+}
+
+static int mlx5e_create_mkey(struct mlx5e_priv *priv, u32 pdn,
+			     struct mlx5_core_mr *mr)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	struct mlx5_create_mkey_mbox_in *in;
+	int err;
+
+	in = mlx5_vzalloc(sizeof(*in));
+	if (!in)
+		return -ENOMEM;
+
+	in->seg.flags = MLX5_PERM_LOCAL_WRITE |
+			MLX5_PERM_LOCAL_READ  |
+			MLX5_ACCESS_MODE_PA;
+	in->seg.flags_pd = cpu_to_be32(pdn | MLX5_MKEY_LEN64);
+	in->seg.qpn_mkey7_0 = cpu_to_be32(0xffffff << 8);
+
+	err = mlx5_core_create_mkey(mdev, mr, in, sizeof(*in), NULL, NULL,
+				    NULL);
+
+	kvfree(in);
+
+	return err;
+}
+
+static void *mlx5e_create_netdev(struct mlx5_core_dev *mdev)
+{
+	struct net_device *netdev;
+	struct mlx5e_priv *priv;
+	int ncv = mdev->priv.eq_table.num_comp_vectors;
+	int err;
+
+	if (mlx5e_check_required_hca_cap(mdev))
+		return NULL;
+
+	netdev = alloc_etherdev_mqs(sizeof(struct mlx5e_priv), ncv, ncv);
+	if (!netdev) {
+		mlx5_core_err(mdev, "alloc_etherdev_mqs() failed\n");
+		return NULL;
+	}
+
+	mlx5e_build_netdev_priv(mdev, netdev, ncv);
+	mlx5e_build_netdev(netdev);
+
+	netif_carrier_off(netdev);
+
+	priv = netdev_priv(netdev);
+
+	err = mlx5_alloc_map_uar(mdev, &priv->cq_uar);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5_alloc_map_uar failed, %d\n",
+			   __func__, err);
+		goto err_free_netdev;
+	}
+
+	err = mlx5_core_alloc_pd(mdev, &priv->pdn);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5_core_alloc_pd failed, %d\n",
+			   __func__, err);
+		goto err_unmap_free_uar;
+	}
+
+	err = mlx5_alloc_transport_domain(mdev, &priv->tdn);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5_alloc_transport_domain failed, %d\n",
+			   __func__, err);
+		goto err_dealloc_pd;
+	}
+
+	err = mlx5e_create_mkey(priv, priv->pdn, &priv->mr);
+	if (err) {
+		netdev_err(netdev, "%s: mlx5e_create_mkey failed, %d\n",
+			   __func__, err);
+		goto err_dealloc_transport_domain;
+	}
+
+	err = register_netdev(netdev);
+	if (err) {
+		netdev_err(netdev, "%s: register_netdev failed, %d\n",
+			   __func__, err);
+		goto err_destroy_mkey;
+	}
+
+	mlx5e_enable_async_events(priv);
+
+	return priv;
+
+err_destroy_mkey:
+	mlx5_core_destroy_mkey(mdev, &priv->mr);
+
+err_dealloc_transport_domain:
+	mlx5_dealloc_transport_domain(mdev, priv->tdn);
+
+err_dealloc_pd:
+	mlx5_core_dealloc_pd(mdev, priv->pdn);
+
+err_unmap_free_uar:
+	mlx5_unmap_free_uar(mdev, &priv->cq_uar);
+
+err_free_netdev:
+	free_netdev(netdev);
+
+	return NULL;
+}
+
+static void mlx5e_destroy_netdev(struct mlx5_core_dev *mdev, void *vpriv)
+{
+	struct mlx5e_priv *priv = vpriv;
+	struct net_device *netdev = priv->netdev;
+
+	unregister_netdev(netdev);
+	mlx5_core_destroy_mkey(priv->mdev, &priv->mr);
+	mlx5_dealloc_transport_domain(priv->mdev, priv->tdn);
+	mlx5_core_dealloc_pd(priv->mdev, priv->pdn);
+	mlx5_unmap_free_uar(priv->mdev, &priv->cq_uar);
+	mlx5e_disable_async_events(priv);
+	flush_scheduled_work();
+	free_netdev(netdev);
+}
+
+static void *mlx5e_get_netdev(void *vpriv)
+{
+	struct mlx5e_priv *priv = vpriv;
+
+	return priv->netdev;
+}
+
+static struct mlx5_interface mlx5e_interface = {
+	.add       = mlx5e_create_netdev,
+	.remove    = mlx5e_destroy_netdev,
+	.event     = mlx5e_async_event,
+	.protocol  = MLX5_INTERFACE_PROTOCOL_ETH,
+	.get_dev   = mlx5e_get_netdev,
+};
+
+void mlx5e_init(void)
+{
+	mlx5_register_interface(&mlx5e_interface);
+}
+
+void mlx5e_cleanup(void)
+{
+	mlx5_unregister_interface(&mlx5e_interface);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
new file mode 100644
index 000000000000..9a9374131f5b
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
@@ -0,0 +1,252 @@
+/*
+ * Copyright (c) 2015, 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/ip.h>
+#include <linux/ipv6.h>
+#include <linux/tcp.h>
+#include "en.h"
+
+static inline int mlx5e_alloc_rx_wqe(struct mlx5e_rq *rq,
+				     struct mlx5e_rx_wqe *wqe, u16 ix)
+{
+	struct sk_buff *skb;
+	dma_addr_t dma_addr;
+
+	skb = netdev_alloc_skb(rq->netdev, rq->wqe_sz);
+	if (unlikely(!skb))
+		return -ENOMEM;
+
+	dma_addr = dma_map_single(rq->pdev,
+				  /* hw start padding */
+				  skb->data,
+				  /* hw end padding */
+				  rq->wqe_sz,
+				  DMA_FROM_DEVICE);
+
+	if (unlikely(dma_mapping_error(rq->pdev, dma_addr)))
+		goto err_free_skb;
+
+	skb_reserve(skb, MLX5E_NET_IP_ALIGN);
+
+	*((dma_addr_t *)skb->cb) = dma_addr;
+	wqe->data.addr = cpu_to_be64(dma_addr + MLX5E_NET_IP_ALIGN);
+
+	rq->skb[ix] = skb;
+
+	return 0;
+
+err_free_skb:
+	dev_kfree_skb(skb);
+
+	return -ENOMEM;
+}
+
+bool mlx5e_post_rx_wqes(struct mlx5e_rq *rq)
+{
+	struct mlx5_wq_ll *wq = &rq->wq;
+
+	if (unlikely(!test_bit(MLX5E_RQ_STATE_POST_WQES_ENABLE, &rq->state)))
+		return false;
+
+	while (!mlx5_wq_ll_is_full(wq)) {
+		struct mlx5e_rx_wqe *wqe = mlx5_wq_ll_get_wqe(wq, wq->head);
+
+		if (unlikely(mlx5e_alloc_rx_wqe(rq, wqe, wq->head)))
+			break;
+
+		mlx5_wq_ll_push(wq, be16_to_cpu(wqe->next.next_wqe_index));
+	}
+
+	/* ensure wqes are visible to device before updating doorbell record */
+	dma_wmb();
+
+	mlx5_wq_ll_update_db_record(wq);
+
+	return !mlx5_wq_ll_is_full(wq);
+}
+
+static void mlx5e_lro_update_hdr(struct sk_buff *skb, struct mlx5_cqe64 *cqe)
+{
+	struct ethhdr	*eth	= (struct ethhdr *)(skb->data);
+	struct iphdr	*ipv4	= (struct iphdr *)(skb->data + ETH_HLEN);
+	struct ipv6hdr	*ipv6	= (struct ipv6hdr *)(skb->data + ETH_HLEN);
+	struct tcphdr	*tcp;
+
+	u8 l4_hdr_type = get_cqe_l4_hdr_type(cqe);
+	int tcp_ack = ((CQE_L4_HDR_TYPE_TCP_ACK_NO_DATA  == l4_hdr_type) ||
+		       (CQE_L4_HDR_TYPE_TCP_ACK_AND_DATA == l4_hdr_type));
+
+	u16 tot_len = be32_to_cpu(cqe->byte_cnt) - ETH_HLEN;
+
+	if (eth->h_proto == htons(ETH_P_IP)) {
+		tcp = (struct tcphdr *)(skb->data + ETH_HLEN +
+					sizeof(struct iphdr));
+		ipv6 = NULL;
+	} else {
+		tcp = (struct tcphdr *)(skb->data + ETH_HLEN +
+					sizeof(struct ipv6hdr));
+		ipv4 = NULL;
+	}
+
+	if (get_cqe_lro_tcppsh(cqe))
+		tcp->psh                = 1;
+
+	if (tcp_ack) {
+		tcp->ack                = 1;
+		tcp->ack_seq            = cqe->lro_ack_seq_num;
+		tcp->window             = cqe->lro_tcp_win;
+	}
+
+	if (ipv4) {
+		ipv4->ttl               = cqe->lro_min_ttl;
+		ipv4->tot_len           = cpu_to_be16(tot_len);
+		ipv4->check             = 0;
+		ipv4->check             = ip_fast_csum((unsigned char *)ipv4,
+						       ipv4->ihl);
+	} else {
+		ipv6->hop_limit         = cqe->lro_min_ttl;
+		ipv6->payload_len       = cpu_to_be16(tot_len -
+						      sizeof(struct ipv6hdr));
+	}
+}
+
+static inline void mlx5e_skb_set_hash(struct mlx5_cqe64 *cqe,
+				      struct sk_buff *skb)
+{
+	u8 cht = cqe->rss_hash_type;
+	int ht = (cht & CQE_RSS_HTYPE_L4) ? PKT_HASH_TYPE_L4 :
+		 (cht & CQE_RSS_HTYPE_IP) ? PKT_HASH_TYPE_L3 :
+					    PKT_HASH_TYPE_NONE;
+	skb_set_hash(skb, be32_to_cpu(cqe->rss_hash_result), ht);
+}
+
+static inline void mlx5e_build_rx_skb(struct mlx5_cqe64 *cqe,
+				      struct mlx5e_rq *rq,
+				      struct sk_buff *skb)
+{
+	struct net_device *netdev = rq->netdev;
+	u32 cqe_bcnt = be32_to_cpu(cqe->byte_cnt);
+	int lro_num_seg;
+
+	skb_put(skb, cqe_bcnt);
+
+	lro_num_seg = be32_to_cpu(cqe->srqn) >> 24;
+	if (lro_num_seg > 1) {
+		mlx5e_lro_update_hdr(skb, cqe);
+		skb_shinfo(skb)->gso_size = MLX5E_PARAMS_DEFAULT_LRO_WQE_SZ;
+		rq->stats.lro_packets++;
+		rq->stats.lro_bytes += cqe_bcnt;
+	}
+
+	if (likely(netdev->features & NETIF_F_RXCSUM) &&
+	    (cqe->hds_ip_ext & CQE_L2_OK) &&
+	    (cqe->hds_ip_ext & CQE_L3_OK) &&
+	    (cqe->hds_ip_ext & CQE_L4_OK)) {
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+	} else {
+		skb->ip_summed = CHECKSUM_NONE;
+		rq->stats.csum_none++;
+	}
+
+	skb->protocol = eth_type_trans(skb, netdev);
+
+	skb_record_rx_queue(skb, rq->ix);
+
+	if (likely(netdev->features & NETIF_F_RXHASH))
+		mlx5e_skb_set_hash(cqe, skb);
+
+	if (cqe_has_vlan(cqe))
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
+				       be16_to_cpu(cqe->vlan_info));
+}
+
+bool mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget)
+{
+	struct mlx5e_rq *rq = container_of(cq, struct mlx5e_rq, cq);
+	int i;
+
+	/* avoid accessing cq (dma coherent memory) if not needed */
+	if (!test_and_clear_bit(MLX5E_CQ_HAS_CQES, &cq->flags))
+		return false;
+
+	for (i = 0; i < budget; i++) {
+		struct mlx5e_rx_wqe *wqe;
+		struct mlx5_cqe64 *cqe;
+		struct sk_buff *skb;
+		__be16 wqe_counter_be;
+		u16 wqe_counter;
+
+		cqe = mlx5e_get_cqe(cq);
+		if (!cqe)
+			break;
+
+		mlx5_cqwq_pop(&cq->wq);
+
+		wqe_counter_be = cqe->wqe_counter;
+		wqe_counter    = be16_to_cpu(wqe_counter_be);
+		wqe            = mlx5_wq_ll_get_wqe(&rq->wq, wqe_counter);
+		skb            = rq->skb[wqe_counter];
+		prefetch(skb->data);
+		rq->skb[wqe_counter] = NULL;
+
+		dma_unmap_single(rq->pdev,
+				 *((dma_addr_t *)skb->cb),
+				 rq->wqe_sz,
+				 DMA_FROM_DEVICE);
+
+		if (unlikely((cqe->op_own >> 4) != MLX5_CQE_RESP_SEND)) {
+			rq->stats.wqe_err++;
+			dev_kfree_skb(skb);
+			goto wq_ll_pop;
+		}
+
+		mlx5e_build_rx_skb(cqe, rq, skb);
+		rq->stats.packets++;
+		napi_gro_receive(cq->napi, skb);
+
+wq_ll_pop:
+		mlx5_wq_ll_pop(&rq->wq, wqe_counter_be,
+			       &wqe->next.next_wqe_index);
+	}
+
+	mlx5_cqwq_update_db_record(&cq->wq);
+
+	/* ensure cq space is freed before enabling more cqes */
+	wmb();
+
+	if (i == budget) {
+		set_bit(MLX5E_CQ_HAS_CQES, &cq->flags);
+		return true;
+	}
+
+	return false;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
new file mode 100644
index 000000000000..03f28f438e55
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
@@ -0,0 +1,350 @@
+/*
+ * Copyright (c) 2015, 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/tcp.h>
+#include <linux/if_vlan.h>
+#include "en.h"
+
+#define MLX5E_SQ_NOPS_ROOM  MLX5_SEND_WQE_MAX_WQEBBS
+#define MLX5E_SQ_STOP_ROOM (MLX5_SEND_WQE_MAX_WQEBBS +\
+			    MLX5E_SQ_NOPS_ROOM)
+
+void mlx5e_send_nop(struct mlx5e_sq *sq, bool notify_hw)
+{
+	struct mlx5_wq_cyc                *wq  = &sq->wq;
+
+	u16 pi = sq->pc & wq->sz_m1;
+	struct mlx5e_tx_wqe              *wqe  = mlx5_wq_cyc_get_wqe(wq, pi);
+
+	struct mlx5_wqe_ctrl_seg         *cseg = &wqe->ctrl;
+
+	memset(cseg, 0, sizeof(*cseg));
+
+	cseg->opmod_idx_opcode = cpu_to_be32((sq->pc << 8) | MLX5_OPCODE_NOP);
+	cseg->qpn_ds           = cpu_to_be32((sq->sqn << 8) | 0x01);
+
+	sq->skb[pi] = NULL;
+	sq->pc++;
+
+	if (notify_hw) {
+		cseg->fm_ce_se = MLX5_WQE_CTRL_CQ_UPDATE;
+		mlx5e_tx_notify_hw(sq, wqe);
+	}
+}
+
+static void mlx5e_dma_pop_last_pushed(struct mlx5e_sq *sq, dma_addr_t *addr,
+				      u32 *size)
+{
+	sq->dma_fifo_pc--;
+	*addr = sq->dma_fifo[sq->dma_fifo_pc & sq->dma_fifo_mask].addr;
+	*size = sq->dma_fifo[sq->dma_fifo_pc & sq->dma_fifo_mask].size;
+}
+
+static void mlx5e_dma_unmap_wqe_err(struct mlx5e_sq *sq, struct sk_buff *skb)
+{
+	dma_addr_t addr;
+	u32 size;
+	int i;
+
+	for (i = 0; i < MLX5E_TX_SKB_CB(skb)->num_dma; i++) {
+		mlx5e_dma_pop_last_pushed(sq, &addr, &size);
+		dma_unmap_single(sq->pdev, addr, size, DMA_TO_DEVICE);
+	}
+}
+
+static inline void mlx5e_dma_push(struct mlx5e_sq *sq, dma_addr_t addr,
+				  u32 size)
+{
+	sq->dma_fifo[sq->dma_fifo_pc & sq->dma_fifo_mask].addr = addr;
+	sq->dma_fifo[sq->dma_fifo_pc & sq->dma_fifo_mask].size = size;
+	sq->dma_fifo_pc++;
+}
+
+static inline void mlx5e_dma_get(struct mlx5e_sq *sq, u32 i, dma_addr_t *addr,
+				 u32 *size)
+{
+	*addr = sq->dma_fifo[i & sq->dma_fifo_mask].addr;
+	*size = sq->dma_fifo[i & sq->dma_fifo_mask].size;
+}
+
+u16 mlx5e_select_queue(struct net_device *dev, struct sk_buff *skb,
+		       void *accel_priv, select_queue_fallback_t fallback)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	int channel_ix = fallback(dev, skb);
+	int up = skb_vlan_tag_present(skb)        ?
+		 skb->vlan_tci >> VLAN_PRIO_SHIFT :
+		 priv->default_vlan_prio;
+	int tc = netdev_get_prio_tc_map(dev, up);
+
+	return priv->channel[channel_ix]->tc_to_txq_map[tc];
+}
+
+static inline u16 mlx5e_get_inline_hdr_size(struct mlx5e_sq *sq,
+					    struct sk_buff *skb)
+{
+#define MLX5E_MIN_INLINE 16 /* eth header with vlan (w/o next ethertype) */
+	return MLX5E_MIN_INLINE;
+}
+
+static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
+{
+	struct mlx5_wq_cyc       *wq   = &sq->wq;
+
+	u16 pi = sq->pc & wq->sz_m1;
+	struct mlx5e_tx_wqe      *wqe  = mlx5_wq_cyc_get_wqe(wq, pi);
+
+	struct mlx5_wqe_ctrl_seg *cseg = &wqe->ctrl;
+	struct mlx5_wqe_eth_seg  *eseg = &wqe->eth;
+	struct mlx5_wqe_data_seg *dseg;
+
+	u8  opcode = MLX5_OPCODE_SEND;
+	dma_addr_t dma_addr = 0;
+	u16 headlen;
+	u16 ds_cnt;
+	u16 ihs;
+	int i;
+
+	memset(wqe, 0, sizeof(*wqe));
+
+	if (likely(skb->ip_summed == CHECKSUM_PARTIAL))
+		eseg->cs_flags	= MLX5_ETH_WQE_L3_CSUM | MLX5_ETH_WQE_L4_CSUM;
+	else
+		sq->stats.csum_offload_none++;
+
+	if (skb_is_gso(skb)) {
+		u32 payload_len;
+
+		eseg->mss    = cpu_to_be16(skb_shinfo(skb)->gso_size);
+		opcode       = MLX5_OPCODE_LSO;
+		ihs          = skb_transport_offset(skb) + tcp_hdrlen(skb);
+		payload_len  = skb->len - ihs;
+		MLX5E_TX_SKB_CB(skb)->num_bytes = skb->len +
+					(skb_shinfo(skb)->gso_segs - 1) * ihs;
+		sq->stats.tso_packets++;
+		sq->stats.tso_bytes += payload_len;
+	} else {
+		ihs = mlx5e_get_inline_hdr_size(sq, skb);
+		MLX5E_TX_SKB_CB(skb)->num_bytes = max_t(unsigned int, skb->len,
+							ETH_ZLEN);
+	}
+
+	skb_copy_from_linear_data(skb, eseg->inline_hdr_start, ihs);
+	skb_pull_inline(skb, ihs);
+
+	eseg->inline_hdr_sz = cpu_to_be16(ihs);
+
+	ds_cnt  = sizeof(*wqe) / MLX5_SEND_WQE_DS;
+	ds_cnt += DIV_ROUND_UP(ihs - sizeof(eseg->inline_hdr_start),
+			       MLX5_SEND_WQE_DS);
+	dseg    = (struct mlx5_wqe_data_seg *)cseg + ds_cnt;
+
+	MLX5E_TX_SKB_CB(skb)->num_dma = 0;
+
+	headlen = skb_headlen(skb);
+	if (headlen) {
+		dma_addr = dma_map_single(sq->pdev, skb->data, headlen,
+					  DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(sq->pdev, dma_addr)))
+			goto dma_unmap_wqe_err;
+
+		dseg->addr       = cpu_to_be64(dma_addr);
+		dseg->lkey       = sq->mkey_be;
+		dseg->byte_count = cpu_to_be32(headlen);
+
+		mlx5e_dma_push(sq, dma_addr, headlen);
+		MLX5E_TX_SKB_CB(skb)->num_dma++;
+
+		dseg++;
+	}
+
+	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+		struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i];
+		int fsz = skb_frag_size(frag);
+
+		dma_addr = skb_frag_dma_map(sq->pdev, frag, 0, fsz,
+					    DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(sq->pdev, dma_addr)))
+			goto dma_unmap_wqe_err;
+
+		dseg->addr       = cpu_to_be64(dma_addr);
+		dseg->lkey       = sq->mkey_be;
+		dseg->byte_count = cpu_to_be32(fsz);
+
+		mlx5e_dma_push(sq, dma_addr, fsz);
+		MLX5E_TX_SKB_CB(skb)->num_dma++;
+
+		dseg++;
+	}
+
+	ds_cnt += MLX5E_TX_SKB_CB(skb)->num_dma;
+
+	cseg->opmod_idx_opcode = cpu_to_be32((sq->pc << 8) | opcode);
+	cseg->qpn_ds           = cpu_to_be32((sq->sqn << 8) | ds_cnt);
+
+	sq->skb[pi] = skb;
+
+	MLX5E_TX_SKB_CB(skb)->num_wqebbs = DIV_ROUND_UP(ds_cnt,
+							MLX5_SEND_WQEBB_NUM_DS);
+	sq->pc += MLX5E_TX_SKB_CB(skb)->num_wqebbs;
+
+	netdev_tx_sent_queue(sq->txq, MLX5E_TX_SKB_CB(skb)->num_bytes);
+
+	if (unlikely(!mlx5e_sq_has_room_for(sq, MLX5E_SQ_STOP_ROOM))) {
+		netif_tx_stop_queue(sq->txq);
+		sq->stats.stopped++;
+	}
+
+	if (!skb->xmit_more || netif_xmit_stopped(sq->txq)) {
+		cseg->fm_ce_se = MLX5_WQE_CTRL_CQ_UPDATE;
+		mlx5e_tx_notify_hw(sq, wqe);
+	}
+
+	/* fill sq edge with nops to avoid wqe wrap around */
+	while ((sq->pc & wq->sz_m1) > sq->edge)
+		mlx5e_send_nop(sq, false);
+
+	sq->stats.packets++;
+	return NETDEV_TX_OK;
+
+dma_unmap_wqe_err:
+	sq->stats.dropped++;
+	mlx5e_dma_unmap_wqe_err(sq, skb);
+
+	dev_kfree_skb_any(skb);
+
+	return NETDEV_TX_OK;
+}
+
+netdev_tx_t mlx5e_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5e_sq *sq = priv->txq_to_sq_map[skb_get_queue_mapping(skb)];
+
+	return mlx5e_sq_xmit(sq, skb);
+}
+
+bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq)
+{
+	struct mlx5e_sq *sq;
+	u32 dma_fifo_cc;
+	u32 nbytes;
+	u16 npkts;
+	u16 sqcc;
+	int i;
+
+	/* avoid accessing cq (dma coherent memory) if not needed */
+	if (!test_and_clear_bit(MLX5E_CQ_HAS_CQES, &cq->flags))
+		return false;
+
+	sq = container_of(cq, struct mlx5e_sq, cq);
+
+	npkts = 0;
+	nbytes = 0;
+
+	/* sq->cc must be updated only after mlx5_cqwq_update_db_record(),
+	 * otherwise a cq overrun may occur
+	 */
+	sqcc = sq->cc;
+
+	/* avoid dirtying sq cache line every cqe */
+	dma_fifo_cc = sq->dma_fifo_cc;
+
+	for (i = 0; i < MLX5E_TX_CQ_POLL_BUDGET; i++) {
+		struct mlx5_cqe64 *cqe;
+		u16 wqe_counter;
+		bool last_wqe;
+
+		cqe = mlx5e_get_cqe(cq);
+		if (!cqe)
+			break;
+
+		mlx5_cqwq_pop(&cq->wq);
+
+		wqe_counter = be16_to_cpu(cqe->wqe_counter);
+
+		do {
+			struct sk_buff *skb;
+			u16 ci;
+			int j;
+
+			last_wqe = (sqcc == wqe_counter);
+
+			ci = sqcc & sq->wq.sz_m1;
+			skb = sq->skb[ci];
+
+			if (unlikely(!skb)) { /* nop */
+				sq->stats.nop++;
+				sqcc++;
+				continue;
+			}
+
+			for (j = 0; j < MLX5E_TX_SKB_CB(skb)->num_dma; j++) {
+				dma_addr_t addr;
+				u32 size;
+
+				mlx5e_dma_get(sq, dma_fifo_cc, &addr, &size);
+				dma_fifo_cc++;
+				dma_unmap_single(sq->pdev, addr, size,
+						 DMA_TO_DEVICE);
+			}
+
+			npkts++;
+			nbytes += MLX5E_TX_SKB_CB(skb)->num_bytes;
+			sqcc += MLX5E_TX_SKB_CB(skb)->num_wqebbs;
+			dev_kfree_skb(skb);
+		} while (!last_wqe);
+	}
+
+	mlx5_cqwq_update_db_record(&cq->wq);
+
+	/* ensure cq space is freed before enabling more cqes */
+	wmb();
+
+	sq->dma_fifo_cc = dma_fifo_cc;
+	sq->cc = sqcc;
+
+	netdev_tx_completed_queue(sq->txq, npkts, nbytes);
+
+	if (netif_tx_queue_stopped(sq->txq) &&
+	    mlx5e_sq_has_room_for(sq, MLX5E_SQ_STOP_ROOM) &&
+	    likely(test_bit(MLX5E_SQ_STATE_WAKE_TXQ_ENABLE, &sq->state))) {
+				netif_tx_wake_queue(sq->txq);
+				sq->stats.wake++;
+	}
+	if (i == MLX5E_TX_CQ_POLL_BUDGET) {
+		set_bit(MLX5E_CQ_HAS_CQES, &cq->flags);
+		return true;
+	}
+
+	return false;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
new file mode 100644
index 000000000000..2c7cb6755d1d
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2015, 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 "en.h"
+
+struct mlx5_cqe64 *mlx5e_get_cqe(struct mlx5e_cq *cq)
+{
+	struct mlx5_cqwq *wq = &cq->wq;
+	u32 ci = mlx5_cqwq_get_ci(wq);
+	struct mlx5_cqe64 *cqe = mlx5_cqwq_get_wqe(wq, ci);
+	int cqe_ownership_bit = cqe->op_own & MLX5_CQE_OWNER_MASK;
+	int sw_ownership_val = mlx5_cqwq_get_wrap_cnt(wq) & 1;
+
+	if (cqe_ownership_bit != sw_ownership_val)
+		return NULL;
+
+	/* ensure cqe content is read after cqe ownership bit */
+	rmb();
+
+	return cqe;
+}
+
+int mlx5e_napi_poll(struct napi_struct *napi, int budget)
+{
+	struct mlx5e_channel *c = container_of(napi, struct mlx5e_channel,
+					       napi);
+	bool busy = false;
+	int i;
+
+	clear_bit(MLX5E_CHANNEL_NAPI_SCHED, &c->flags);
+
+	for (i = 0; i < c->num_tc; i++)
+		busy |= mlx5e_poll_tx_cq(&c->sq[i].cq);
+
+	busy |= mlx5e_poll_rx_cq(&c->rq.cq, budget);
+
+	busy |= mlx5e_post_rx_wqes(&c->rq);
+
+	if (busy)
+		return budget;
+
+	napi_complete(napi);
+
+	/* avoid losing completion event during/after polling cqs */
+	if (test_bit(MLX5E_CHANNEL_NAPI_SCHED, &c->flags)) {
+		napi_schedule(napi);
+		return 0;
+	}
+
+	for (i = 0; i < c->num_tc; i++)
+		mlx5e_cq_arm(&c->sq[i].cq);
+	mlx5e_cq_arm(&c->rq.cq);
+
+	return 0;
+}
+
+void mlx5e_completion_event(struct mlx5_core_cq *mcq)
+{
+	struct mlx5e_cq *cq = container_of(mcq, struct mlx5e_cq, mcq);
+
+	set_bit(MLX5E_CQ_HAS_CQES, &cq->flags);
+	set_bit(MLX5E_CHANNEL_NAPI_SCHED, &cq->channel->flags);
+	barrier();
+	napi_schedule(cq->napi);
+}
+
+void mlx5e_cq_error_event(struct mlx5_core_cq *mcq, enum mlx5_event event)
+{
+	struct mlx5e_cq *cq = container_of(mcq, struct mlx5e_cq, mcq);
+	struct mlx5e_channel *c = cq->channel;
+	struct mlx5e_priv *priv = c->priv;
+	struct net_device *netdev = priv->netdev;
+
+	netdev_err(netdev, "%s: cqn=0x%.6x event=0x%.2x\n",
+		   __func__, mcq->cqn, event);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eq.c b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
index 58800e4f3958..a40b96d4c662 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
@@ -339,15 +339,14 @@ static void init_eq_buf(struct mlx5_eq *eq)
 int mlx5_create_map_eq(struct mlx5_core_dev *dev, struct mlx5_eq *eq, u8 vecidx,
 		       int nent, u64 mask, const char *name, struct mlx5_uar *uar)
 {
-	struct mlx5_eq_table *table = &dev->priv.eq_table;
+	struct mlx5_priv *priv = &dev->priv;
 	struct mlx5_create_eq_mbox_in *in;
 	struct mlx5_create_eq_mbox_out out;
 	int err;
 	int inlen;
 
 	eq->nent = roundup_pow_of_two(nent + MLX5_NUM_SPARE_EQE);
-	err = mlx5_buf_alloc(dev, eq->nent * MLX5_EQE_SIZE, 2 * PAGE_SIZE,
-			     &eq->buf);
+	err = mlx5_buf_alloc(dev, eq->nent * MLX5_EQE_SIZE, &eq->buf);
 	if (err)
 		return err;
 
@@ -378,14 +377,15 @@ int mlx5_create_map_eq(struct mlx5_core_dev *dev, struct mlx5_eq *eq, u8 vecidx,
 		goto err_in;
 	}
 
-	snprintf(eq->name, MLX5_MAX_EQ_NAME, "%s@pci:%s",
+	snprintf(priv->irq_info[vecidx].name, MLX5_MAX_IRQ_NAME, "%s@pci:%s",
 		 name, pci_name(dev->pdev));
+
 	eq->eqn = out.eq_number;
 	eq->irqn = vecidx;
 	eq->dev = dev;
 	eq->doorbell = uar->map + MLX5_EQ_DOORBEL_OFFSET;
-	err = request_irq(table->msix_arr[vecidx].vector, mlx5_msix_handler, 0,
-			  eq->name, eq);
+	err = request_irq(priv->msix_arr[vecidx].vector, mlx5_msix_handler, 0,
+			  priv->irq_info[vecidx].name, eq);
 	if (err)
 		goto err_eq;
 
@@ -401,7 +401,7 @@ int mlx5_create_map_eq(struct mlx5_core_dev *dev, struct mlx5_eq *eq, u8 vecidx,
 	return 0;
 
 err_irq:
-	free_irq(table->msix_arr[vecidx].vector, eq);
+	free_irq(priv->msix_arr[vecidx].vector, eq);
 
 err_eq:
 	mlx5_cmd_destroy_eq(dev, eq->eqn);
@@ -417,16 +417,15 @@ EXPORT_SYMBOL_GPL(mlx5_create_map_eq);
 
 int mlx5_destroy_unmap_eq(struct mlx5_core_dev *dev, struct mlx5_eq *eq)
 {
-	struct mlx5_eq_table *table = &dev->priv.eq_table;
 	int err;
 
 	mlx5_debug_eq_remove(dev, eq);
-	free_irq(table->msix_arr[eq->irqn].vector, eq);
+	free_irq(dev->priv.msix_arr[eq->irqn].vector, eq);
 	err = mlx5_cmd_destroy_eq(dev, eq->eqn);
 	if (err)
 		mlx5_core_warn(dev, "failed to destroy a previously created eq: eqn %d\n",
 			       eq->eqn);
-	synchronize_irq(table->msix_arr[eq->irqn].vector);
+	synchronize_irq(dev->priv.msix_arr[eq->irqn].vector);
 	mlx5_buf_free(dev, &eq->buf);
 
 	return err;
@@ -456,7 +455,7 @@ int mlx5_start_eqs(struct mlx5_core_dev *dev)
 	u32 async_event_mask = MLX5_ASYNC_EVENT_MASK;
 	int err;
 
-	if (dev->caps.gen.flags & MLX5_DEV_CAP_FLAG_ON_DMND_PG)
+	if (MLX5_CAP_GEN(dev, pg))
 		async_event_mask |= (1ull << MLX5_EVENT_TYPE_PAGE_FAULT);
 
 	err = mlx5_create_map_eq(dev, &table->cmd_eq, MLX5_EQ_VEC_CMD,
@@ -479,7 +478,7 @@ int mlx5_start_eqs(struct mlx5_core_dev *dev)
 
 	err = mlx5_create_map_eq(dev, &table->pages_eq,
 				 MLX5_EQ_VEC_PAGES,
-				 dev->caps.gen.max_vf + 1,
+				 /* TODO: sriov max_vf + */ 1,
 				 1 << MLX5_EVENT_TYPE_PAGE_REQUEST, "mlx5_pages_eq",
 				 &dev->priv.uuari.uars[0]);
 	if (err) {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/flow_table.c b/drivers/net/ethernet/mellanox/mlx5/core/flow_table.c
new file mode 100644
index 000000000000..ca90b9bc3b95
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/flow_table.c
@@ -0,0 +1,422 @@
+/*
+ * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  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/export.h>
+#include <linux/mlx5/driver.h>
+#include <linux/mlx5/flow_table.h>
+#include "mlx5_core.h"
+
+struct mlx5_ftg {
+	struct mlx5_flow_table_group    g;
+	u32				id;
+	u32				start_ix;
+};
+
+struct mlx5_flow_table {
+	struct mlx5_core_dev	*dev;
+	u8			level;
+	u8			type;
+	u32			id;
+	struct mutex		mutex; /* sync bitmap alloc */
+	u16			num_groups;
+	struct mlx5_ftg		*group;
+	unsigned long		*bitmap;
+	u32			size;
+};
+
+static int mlx5_set_flow_entry_cmd(struct mlx5_flow_table *ft, u32 group_ix,
+				   u32 flow_index, void *flow_context)
+{
+	u32 out[MLX5_ST_SZ_DW(set_fte_out)];
+	u32 *in;
+	void *in_flow_context;
+	int fcdls =
+		MLX5_GET(flow_context, flow_context, destination_list_size) *
+		MLX5_ST_SZ_BYTES(dest_format_struct);
+	int inlen = MLX5_ST_SZ_BYTES(set_fte_in) + fcdls;
+	int err;
+
+	in = mlx5_vzalloc(inlen);
+	if (!in) {
+		mlx5_core_warn(ft->dev, "failed to allocate inbox\n");
+		return -ENOMEM;
+	}
+
+	MLX5_SET(set_fte_in, in, table_type, ft->type);
+	MLX5_SET(set_fte_in, in, table_id,   ft->id);
+	MLX5_SET(set_fte_in, in, flow_index, flow_index);
+	MLX5_SET(set_fte_in, in, opcode, MLX5_CMD_OP_SET_FLOW_TABLE_ENTRY);
+
+	in_flow_context = MLX5_ADDR_OF(set_fte_in, in, flow_context);
+	memcpy(in_flow_context, flow_context,
+	       MLX5_ST_SZ_BYTES(flow_context) + fcdls);
+
+	MLX5_SET(flow_context, in_flow_context, group_id,
+		 ft->group[group_ix].id);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(ft->dev, in, inlen, out,
+					 sizeof(out));
+	kvfree(in);
+
+	return err;
+}
+
+static void mlx5_del_flow_entry_cmd(struct mlx5_flow_table *ft, u32 flow_index)
+{
+	u32 in[MLX5_ST_SZ_DW(delete_fte_in)];
+	u32 out[MLX5_ST_SZ_DW(delete_fte_out)];
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+
+#define MLX5_SET_DFTEI(p, x, v) MLX5_SET(delete_fte_in, p, x, v)
+	MLX5_SET_DFTEI(in, table_type, ft->type);
+	MLX5_SET_DFTEI(in, table_id,   ft->id);
+	MLX5_SET_DFTEI(in, flow_index, flow_index);
+	MLX5_SET_DFTEI(in, opcode,     MLX5_CMD_OP_DELETE_FLOW_TABLE_ENTRY);
+
+	mlx5_cmd_exec_check_status(ft->dev, in, sizeof(in), out, sizeof(out));
+}
+
+static void mlx5_destroy_flow_group_cmd(struct mlx5_flow_table *ft, int i)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_flow_group_in)];
+	u32 out[MLX5_ST_SZ_DW(destroy_flow_group_out)];
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+
+#define MLX5_SET_DFGI(p, x, v) MLX5_SET(destroy_flow_group_in, p, x, v)
+	MLX5_SET_DFGI(in, table_type, ft->type);
+	MLX5_SET_DFGI(in, table_id,   ft->id);
+	MLX5_SET_DFGI(in, opcode, MLX5_CMD_OP_DESTROY_FLOW_GROUP);
+	MLX5_SET_DFGI(in, group_id, ft->group[i].id);
+	mlx5_cmd_exec_check_status(ft->dev, in, sizeof(in), out, sizeof(out));
+}
+
+static int mlx5_create_flow_group_cmd(struct mlx5_flow_table *ft, int i)
+{
+	u32 out[MLX5_ST_SZ_DW(create_flow_group_out)];
+	u32 *in;
+	void *in_match_criteria;
+	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
+	struct mlx5_flow_table_group *g = &ft->group[i].g;
+	u32 start_ix = ft->group[i].start_ix;
+	u32 end_ix = start_ix + (1 << g->log_sz) - 1;
+	int err;
+
+	in = mlx5_vzalloc(inlen);
+	if (!in) {
+		mlx5_core_warn(ft->dev, "failed to allocate inbox\n");
+		return -ENOMEM;
+	}
+	in_match_criteria = MLX5_ADDR_OF(create_flow_group_in, in,
+					 match_criteria);
+
+	memset(out, 0, sizeof(out));
+
+#define MLX5_SET_CFGI(p, x, v) MLX5_SET(create_flow_group_in, p, x, v)
+	MLX5_SET_CFGI(in, table_type,            ft->type);
+	MLX5_SET_CFGI(in, table_id,              ft->id);
+	MLX5_SET_CFGI(in, opcode,                MLX5_CMD_OP_CREATE_FLOW_GROUP);
+	MLX5_SET_CFGI(in, start_flow_index,      start_ix);
+	MLX5_SET_CFGI(in, end_flow_index,        end_ix);
+	MLX5_SET_CFGI(in, match_criteria_enable, g->match_criteria_enable);
+
+	memcpy(in_match_criteria, g->match_criteria,
+	       MLX5_ST_SZ_BYTES(fte_match_param));
+
+	err = mlx5_cmd_exec_check_status(ft->dev, in, inlen, out,
+					 sizeof(out));
+	if (!err)
+		ft->group[i].id = MLX5_GET(create_flow_group_out, out,
+					   group_id);
+
+	kvfree(in);
+
+	return err;
+}
+
+static void mlx5_destroy_flow_table_groups(struct mlx5_flow_table *ft)
+{
+	int i;
+
+	for (i = 0; i < ft->num_groups; i++)
+		mlx5_destroy_flow_group_cmd(ft, i);
+}
+
+static int mlx5_create_flow_table_groups(struct mlx5_flow_table *ft)
+{
+	int err;
+	int i;
+
+	for (i = 0; i < ft->num_groups; i++) {
+		err = mlx5_create_flow_group_cmd(ft, i);
+		if (err)
+			goto err_destroy_flow_table_groups;
+	}
+
+	return 0;
+
+err_destroy_flow_table_groups:
+	for (i--; i >= 0; i--)
+		mlx5_destroy_flow_group_cmd(ft, i);
+
+	return err;
+}
+
+static int mlx5_create_flow_table_cmd(struct mlx5_flow_table *ft)
+{
+	u32 in[MLX5_ST_SZ_DW(create_flow_table_in)];
+	u32 out[MLX5_ST_SZ_DW(create_flow_table_out)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(create_flow_table_in, in, table_type, ft->type);
+	MLX5_SET(create_flow_table_in, in, level,      ft->level);
+	MLX5_SET(create_flow_table_in, in, log_size,   order_base_2(ft->size));
+
+	MLX5_SET(create_flow_table_in, in, opcode,
+		 MLX5_CMD_OP_CREATE_FLOW_TABLE);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(ft->dev, in, sizeof(in), out,
+					 sizeof(out));
+	if (err)
+		return err;
+
+	ft->id = MLX5_GET(create_flow_table_out, out, table_id);
+
+	return 0;
+}
+
+static void mlx5_destroy_flow_table_cmd(struct mlx5_flow_table *ft)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_flow_table_in)];
+	u32 out[MLX5_ST_SZ_DW(destroy_flow_table_out)];
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+
+#define MLX5_SET_DFTI(p, x, v) MLX5_SET(destroy_flow_table_in, p, x, v)
+	MLX5_SET_DFTI(in, table_type, ft->type);
+	MLX5_SET_DFTI(in, table_id,   ft->id);
+	MLX5_SET_DFTI(in, opcode, MLX5_CMD_OP_DESTROY_FLOW_TABLE);
+
+	mlx5_cmd_exec_check_status(ft->dev, in, sizeof(in), out, sizeof(out));
+}
+
+static int mlx5_find_group(struct mlx5_flow_table *ft, u8 match_criteria_enable,
+			   u32 *match_criteria, int *group_ix)
+{
+	void *mc_outer = MLX5_ADDR_OF(fte_match_param, match_criteria,
+				      outer_headers);
+	void *mc_misc  = MLX5_ADDR_OF(fte_match_param, match_criteria,
+				      misc_parameters);
+	void *mc_inner = MLX5_ADDR_OF(fte_match_param, match_criteria,
+				      inner_headers);
+	int mc_outer_sz = MLX5_ST_SZ_BYTES(fte_match_set_lyr_2_4);
+	int mc_misc_sz  = MLX5_ST_SZ_BYTES(fte_match_set_misc);
+	int mc_inner_sz = MLX5_ST_SZ_BYTES(fte_match_set_lyr_2_4);
+	int i;
+
+	for (i = 0; i < ft->num_groups; i++) {
+		struct mlx5_flow_table_group *g = &ft->group[i].g;
+		void *gmc_outer = MLX5_ADDR_OF(fte_match_param,
+					       g->match_criteria,
+					       outer_headers);
+		void *gmc_misc  = MLX5_ADDR_OF(fte_match_param,
+					       g->match_criteria,
+					       misc_parameters);
+		void *gmc_inner = MLX5_ADDR_OF(fte_match_param,
+					       g->match_criteria,
+					       inner_headers);
+
+		if (g->match_criteria_enable != match_criteria_enable)
+			continue;
+
+		if (match_criteria_enable & MLX5_MATCH_OUTER_HEADERS)
+			if (memcmp(mc_outer, gmc_outer, mc_outer_sz))
+				continue;
+
+		if (match_criteria_enable & MLX5_MATCH_MISC_PARAMETERS)
+			if (memcmp(mc_misc, gmc_misc, mc_misc_sz))
+				continue;
+
+		if (match_criteria_enable & MLX5_MATCH_INNER_HEADERS)
+			if (memcmp(mc_inner, gmc_inner, mc_inner_sz))
+				continue;
+
+		*group_ix = i;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int alloc_flow_index(struct mlx5_flow_table *ft, int group_ix, u32 *ix)
+{
+	struct mlx5_ftg *g = &ft->group[group_ix];
+	int err = 0;
+
+	mutex_lock(&ft->mutex);
+
+	*ix = find_next_zero_bit(ft->bitmap, ft->size, g->start_ix);
+	if (*ix >= (g->start_ix + (1 << g->g.log_sz)))
+		err = -ENOSPC;
+	else
+		__set_bit(*ix, ft->bitmap);
+
+	mutex_unlock(&ft->mutex);
+
+	return err;
+}
+
+static void mlx5_free_flow_index(struct mlx5_flow_table *ft, u32 ix)
+{
+	__clear_bit(ix, ft->bitmap);
+}
+
+int mlx5_add_flow_table_entry(void *flow_table, u8 match_criteria_enable,
+			      void *match_criteria, void *flow_context,
+			      u32 *flow_index)
+{
+	struct mlx5_flow_table *ft = flow_table;
+	int group_ix;
+	int err;
+
+	err = mlx5_find_group(ft, match_criteria_enable, match_criteria,
+			      &group_ix);
+	if (err) {
+		mlx5_core_warn(ft->dev, "mlx5_find_group failed\n");
+		return err;
+	}
+
+	err = alloc_flow_index(ft, group_ix, flow_index);
+	if (err) {
+		mlx5_core_warn(ft->dev, "alloc_flow_index failed\n");
+		return err;
+	}
+
+	return mlx5_set_flow_entry_cmd(ft, group_ix, *flow_index, flow_context);
+}
+EXPORT_SYMBOL(mlx5_add_flow_table_entry);
+
+void mlx5_del_flow_table_entry(void *flow_table, u32 flow_index)
+{
+	struct mlx5_flow_table *ft = flow_table;
+
+	mlx5_del_flow_entry_cmd(ft, flow_index);
+	mlx5_free_flow_index(ft, flow_index);
+}
+EXPORT_SYMBOL(mlx5_del_flow_table_entry);
+
+void *mlx5_create_flow_table(struct mlx5_core_dev *dev, u8 level, u8 table_type,
+			     u16 num_groups,
+			     struct mlx5_flow_table_group *group)
+{
+	struct mlx5_flow_table *ft;
+	u32 start_ix = 0;
+	u32 ft_size = 0;
+	void *gr;
+	void *bm;
+	int err;
+	int i;
+
+	for (i = 0; i < num_groups; i++)
+		ft_size += (1 << group[i].log_sz);
+
+	ft = kzalloc(sizeof(*ft), GFP_KERNEL);
+	gr = kcalloc(num_groups, sizeof(struct mlx5_ftg), GFP_KERNEL);
+	bm = kcalloc(BITS_TO_LONGS(ft_size), sizeof(uintptr_t), GFP_KERNEL);
+	if (!ft || !gr || !bm)
+		goto err_free_ft;
+
+	ft->group	= gr;
+	ft->bitmap	= bm;
+	ft->num_groups	= num_groups;
+	ft->level	= level;
+	ft->type	= table_type;
+	ft->size	= ft_size;
+	ft->dev		= dev;
+	mutex_init(&ft->mutex);
+
+	for (i = 0; i < ft->num_groups; i++) {
+		memcpy(&ft->group[i].g, &group[i], sizeof(*group));
+		ft->group[i].start_ix = start_ix;
+		start_ix += 1 << group[i].log_sz;
+	}
+
+	err = mlx5_create_flow_table_cmd(ft);
+	if (err)
+		goto err_free_ft;
+
+	err = mlx5_create_flow_table_groups(ft);
+	if (err)
+		goto err_destroy_flow_table_cmd;
+
+	return ft;
+
+err_destroy_flow_table_cmd:
+	mlx5_destroy_flow_table_cmd(ft);
+
+err_free_ft:
+	mlx5_core_warn(dev, "failed to alloc flow table\n");
+	kfree(bm);
+	kfree(gr);
+	kfree(ft);
+
+	return NULL;
+}
+EXPORT_SYMBOL(mlx5_create_flow_table);
+
+void mlx5_destroy_flow_table(void *flow_table)
+{
+	struct mlx5_flow_table *ft = flow_table;
+
+	mlx5_destroy_flow_table_groups(ft);
+	mlx5_destroy_flow_table_cmd(ft);
+	kfree(ft->bitmap);
+	kfree(ft->group);
+	kfree(ft);
+}
+EXPORT_SYMBOL(mlx5_destroy_flow_table);
+
+u32 mlx5_get_flow_table_id(void *flow_table)
+{
+	struct mlx5_flow_table *ft = flow_table;
+
+	return ft->id;
+}
+EXPORT_SYMBOL(mlx5_get_flow_table_id);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fw.c b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
index 4b4cda3bcc5f..9335e5ae18cc 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
@@ -35,79 +35,133 @@
 #include <linux/module.h>
 #include "mlx5_core.h"
 
-int mlx5_cmd_query_adapter(struct mlx5_core_dev *dev)
+static int mlx5_cmd_query_adapter(struct mlx5_core_dev *dev, u32 *out,
+				  int outlen)
 {
-	struct mlx5_cmd_query_adapter_mbox_out *out;
-	struct mlx5_cmd_query_adapter_mbox_in in;
+	u32 in[MLX5_ST_SZ_DW(query_adapter_in)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(query_adapter_in, in, opcode, MLX5_CMD_OP_QUERY_ADAPTER);
+
+	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, outlen);
+}
+
+int mlx5_query_board_id(struct mlx5_core_dev *dev)
+{
+	u32 *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_adapter_out);
 	int err;
 
-	out = kzalloc(sizeof(*out), GFP_KERNEL);
+	out = kzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
-	memset(&in, 0, sizeof(in));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_ADAPTER);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), out, sizeof(*out));
+	err = mlx5_cmd_query_adapter(dev, out, outlen);
 	if (err)
-		goto out_out;
-
-	if (out->hdr.status) {
-		err = mlx5_cmd_status_to_err(&out->hdr);
-		goto out_out;
-	}
+		goto out;
 
-	memcpy(dev->board_id, out->vsd_psid, sizeof(out->vsd_psid));
+	memcpy(dev->board_id,
+	       MLX5_ADDR_OF(query_adapter_out, out,
+			    query_adapter_struct.vsd_contd_psid),
+	       MLX5_FLD_SZ_BYTES(query_adapter_out,
+				 query_adapter_struct.vsd_contd_psid));
 
-out_out:
+out:
 	kfree(out);
-
 	return err;
 }
 
-int mlx5_cmd_query_hca_cap(struct mlx5_core_dev *dev, struct mlx5_caps *caps)
+int mlx5_core_query_vendor_id(struct mlx5_core_dev *mdev, u32 *vendor_id)
 {
-	return mlx5_core_get_caps(dev, caps, HCA_CAP_OPMOD_GET_CUR);
+	u32 *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_adapter_out);
+	int err;
+
+	out = kzalloc(outlen, GFP_KERNEL);
+	if (!out)
+		return -ENOMEM;
+
+	err = mlx5_cmd_query_adapter(mdev, out, outlen);
+	if (err)
+		goto out;
+
+	*vendor_id = MLX5_GET(query_adapter_out, out,
+			      query_adapter_struct.ieee_vendor_id);
+out:
+	kfree(out);
+	return err;
 }
+EXPORT_SYMBOL(mlx5_core_query_vendor_id);
 
-int mlx5_query_odp_caps(struct mlx5_core_dev *dev, struct mlx5_odp_caps *caps)
+int mlx5_query_hca_caps(struct mlx5_core_dev *dev)
 {
-	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;
+	err = mlx5_core_get_caps(dev, MLX5_CAP_GENERAL, HCA_CAP_OPMOD_GET_CUR);
+	if (err)
+		return err;
 
-	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);
+	err = mlx5_core_get_caps(dev, MLX5_CAP_GENERAL, HCA_CAP_OPMOD_GET_MAX);
 	if (err)
-		goto out;
+		return err;
 
-	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;
+	if (MLX5_CAP_GEN(dev, eth_net_offloads)) {
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ETHERNET_OFFLOADS,
+					 HCA_CAP_OPMOD_GET_CUR);
+		if (err)
+			return err;
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ETHERNET_OFFLOADS,
+					 HCA_CAP_OPMOD_GET_MAX);
+		if (err)
+			return err;
 	}
 
-	memcpy(caps, MLX5_ADDR_OF(query_hca_cap_out, out, capability_struct),
-	       sizeof(*caps));
+	if (MLX5_CAP_GEN(dev, pg)) {
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ODP,
+					 HCA_CAP_OPMOD_GET_CUR);
+		if (err)
+			return err;
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ODP,
+					 HCA_CAP_OPMOD_GET_MAX);
+		if (err)
+			return err;
+	}
 
-	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));
+	if (MLX5_CAP_GEN(dev, atomic)) {
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ATOMIC,
+					 HCA_CAP_OPMOD_GET_CUR);
+		if (err)
+			return err;
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ATOMIC,
+					 HCA_CAP_OPMOD_GET_MAX);
+		if (err)
+			return err;
+	}
 
-out:
-	kfree(out);
-	return err;
+	if (MLX5_CAP_GEN(dev, roce)) {
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ROCE,
+					 HCA_CAP_OPMOD_GET_CUR);
+		if (err)
+			return err;
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ROCE,
+					 HCA_CAP_OPMOD_GET_MAX);
+		if (err)
+			return err;
+	}
+
+	if (MLX5_CAP_GEN(dev, nic_flow_table)) {
+		err = mlx5_core_get_caps(dev, MLX5_CAP_FLOW_TABLE,
+					 HCA_CAP_OPMOD_GET_CUR);
+		if (err)
+			return err;
+		err = mlx5_core_get_caps(dev, MLX5_CAP_FLOW_TABLE,
+					 HCA_CAP_OPMOD_GET_MAX);
+		if (err)
+			return err;
+	}
+	return 0;
 }
-EXPORT_SYMBOL(mlx5_query_odp_caps);
 
 int mlx5_cmd_init_hca(struct mlx5_core_dev *dev)
 {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mad.c b/drivers/net/ethernet/mellanox/mlx5/core/mad.c
index ee1b0b965f34..1368dac00da0 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mad.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mad.c
@@ -36,7 +36,7 @@
 #include <linux/mlx5/cmd.h>
 #include "mlx5_core.h"
 
-int mlx5_core_mad_ifc(struct mlx5_core_dev *dev, void *inb, void *outb,
+int mlx5_core_mad_ifc(struct mlx5_core_dev *dev, const void *inb, void *outb,
 		      u16 opmod, u8 port)
 {
 	struct mlx5_mad_ifc_mbox_in *in = NULL;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/main.c b/drivers/net/ethernet/mellanox/mlx5/core/main.c
index 28425e5ea91f..afad529838de 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/main.c
@@ -38,6 +38,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/slab.h>
 #include <linux/io-mapping.h>
+#include <linux/interrupt.h>
 #include <linux/mlx5/driver.h>
 #include <linux/mlx5/cq.h>
 #include <linux/mlx5/qp.h>
@@ -47,10 +48,6 @@
 #include <linux/mlx5/mlx5_ifc.h>
 #include "mlx5_core.h"
 
-#define DRIVER_NAME "mlx5_core"
-#define DRIVER_VERSION "3.0"
-#define DRIVER_RELDATE  "January 2015"
-
 MODULE_AUTHOR("Eli Cohen <eli@mellanox.com>");
 MODULE_DESCRIPTION("Mellanox Connect-IB, ConnectX-4 core driver");
 MODULE_LICENSE("Dual BSD/GPL");
@@ -208,24 +205,28 @@ static void release_bar(struct pci_dev *pdev)
 
 static int mlx5_enable_msix(struct mlx5_core_dev *dev)
 {
-	struct mlx5_eq_table *table = &dev->priv.eq_table;
-	int num_eqs = 1 << dev->caps.gen.log_max_eq;
+	struct mlx5_priv *priv = &dev->priv;
+	struct mlx5_eq_table *table = &priv->eq_table;
+	int num_eqs = 1 << MLX5_CAP_GEN(dev, log_max_eq);
 	int nvec;
 	int i;
 
-	nvec = dev->caps.gen.num_ports * num_online_cpus() + MLX5_EQ_VEC_COMP_BASE;
+	nvec = MLX5_CAP_GEN(dev, num_ports) * num_online_cpus() +
+	       MLX5_EQ_VEC_COMP_BASE;
 	nvec = min_t(int, nvec, num_eqs);
 	if (nvec <= MLX5_EQ_VEC_COMP_BASE)
 		return -ENOMEM;
 
-	table->msix_arr = kzalloc(nvec * sizeof(*table->msix_arr), GFP_KERNEL);
-	if (!table->msix_arr)
-		return -ENOMEM;
+	priv->msix_arr = kcalloc(nvec, sizeof(*priv->msix_arr), GFP_KERNEL);
+
+	priv->irq_info = kcalloc(nvec, sizeof(*priv->irq_info), GFP_KERNEL);
+	if (!priv->msix_arr || !priv->irq_info)
+		goto err_free_msix;
 
 	for (i = 0; i < nvec; i++)
-		table->msix_arr[i].entry = i;
+		priv->msix_arr[i].entry = i;
 
-	nvec = pci_enable_msix_range(dev->pdev, table->msix_arr,
+	nvec = pci_enable_msix_range(dev->pdev, priv->msix_arr,
 				     MLX5_EQ_VEC_COMP_BASE + 1, nvec);
 	if (nvec < 0)
 		return nvec;
@@ -233,14 +234,20 @@ static int mlx5_enable_msix(struct mlx5_core_dev *dev)
 	table->num_comp_vectors = nvec - MLX5_EQ_VEC_COMP_BASE;
 
 	return 0;
+
+err_free_msix:
+	kfree(priv->irq_info);
+	kfree(priv->msix_arr);
+	return -ENOMEM;
 }
 
 static void mlx5_disable_msix(struct mlx5_core_dev *dev)
 {
-	struct mlx5_eq_table *table = &dev->priv.eq_table;
+	struct mlx5_priv *priv = &dev->priv;
 
 	pci_disable_msix(dev->pdev);
-	kfree(table->msix_arr);
+	kfree(priv->irq_info);
+	kfree(priv->msix_arr);
 }
 
 struct mlx5_reg_host_endianess {
@@ -277,98 +284,20 @@ static u16 to_fw_pkey_sz(u32 size)
 	}
 }
 
-/* selectively copy writable fields clearing any reserved area
- */
-static void copy_rw_fields(void *to, struct mlx5_caps *from)
-{
-	__be64 *flags_off = (__be64 *)MLX5_ADDR_OF(cmd_hca_cap, to, reserved_22);
-	u64 v64;
-
-	MLX5_SET(cmd_hca_cap, to, log_max_qp, from->gen.log_max_qp);
-	MLX5_SET(cmd_hca_cap, to, log_max_ra_req_qp, from->gen.log_max_ra_req_qp);
-	MLX5_SET(cmd_hca_cap, to, log_max_ra_res_qp, from->gen.log_max_ra_res_qp);
-	MLX5_SET(cmd_hca_cap, to, pkey_table_size, from->gen.pkey_table_size);
-	MLX5_SET(cmd_hca_cap, to, pkey_table_size, to_fw_pkey_sz(from->gen.pkey_table_size));
-	MLX5_SET(cmd_hca_cap, to, log_uar_page_sz, PAGE_SHIFT - 12);
-	v64 = from->gen.flags & MLX5_CAP_BITS_RW_MASK;
-	*flags_off = cpu_to_be64(v64);
-}
-
-static u16 get_pkey_table_size(int pkey)
-{
-	if (pkey > MLX5_MAX_LOG_PKEY_TABLE)
-		return 0;
-
-	return MLX5_MIN_PKEY_TABLE_SIZE << pkey;
-}
-
-static void fw2drv_caps(struct mlx5_caps *caps, void *out)
-{
-	struct mlx5_general_caps *gen = &caps->gen;
-
-	gen->max_srq_wqes = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_max_srq_sz);
-	gen->max_wqes = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_max_qp_sz);
-	gen->log_max_qp = MLX5_GET_PR(cmd_hca_cap, out, log_max_qp);
-	gen->log_max_strq = MLX5_GET_PR(cmd_hca_cap, out, log_max_strq_sz);
-	gen->log_max_srq = MLX5_GET_PR(cmd_hca_cap, out, log_max_srqs);
-	gen->max_cqes = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_max_cq_sz);
-	gen->log_max_cq = MLX5_GET_PR(cmd_hca_cap, out, log_max_cq);
-	gen->max_eqes = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_max_eq_sz);
-	gen->log_max_mkey = MLX5_GET_PR(cmd_hca_cap, out, log_max_mkey);
-	gen->log_max_eq = MLX5_GET_PR(cmd_hca_cap, out, log_max_eq);
-	gen->max_indirection = MLX5_GET_PR(cmd_hca_cap, out, max_indirection);
-	gen->log_max_mrw_sz = MLX5_GET_PR(cmd_hca_cap, out, log_max_mrw_sz);
-	gen->log_max_bsf_list_size = MLX5_GET_PR(cmd_hca_cap, out, log_max_bsf_list_size);
-	gen->log_max_klm_list_size = MLX5_GET_PR(cmd_hca_cap, out, log_max_klm_list_size);
-	gen->log_max_ra_req_dc = MLX5_GET_PR(cmd_hca_cap, out, log_max_ra_req_dc);
-	gen->log_max_ra_res_dc = MLX5_GET_PR(cmd_hca_cap, out, log_max_ra_res_dc);
-	gen->log_max_ra_req_qp = MLX5_GET_PR(cmd_hca_cap, out, log_max_ra_req_qp);
-	gen->log_max_ra_res_qp = MLX5_GET_PR(cmd_hca_cap, out, log_max_ra_res_qp);
-	gen->max_qp_counters = MLX5_GET_PR(cmd_hca_cap, out, max_qp_cnt);
-	gen->pkey_table_size = get_pkey_table_size(MLX5_GET_PR(cmd_hca_cap, out, pkey_table_size));
-	gen->local_ca_ack_delay = MLX5_GET_PR(cmd_hca_cap, out, local_ca_ack_delay);
-	gen->num_ports = MLX5_GET_PR(cmd_hca_cap, out, num_ports);
-	gen->log_max_msg = MLX5_GET_PR(cmd_hca_cap, out, log_max_msg);
-	gen->stat_rate_support = MLX5_GET_PR(cmd_hca_cap, out, stat_rate_support);
-	gen->flags = be64_to_cpu(*(__be64 *)MLX5_ADDR_OF(cmd_hca_cap, out, reserved_22));
-	pr_debug("flags = 0x%llx\n", gen->flags);
-	gen->uar_sz = MLX5_GET_PR(cmd_hca_cap, out, uar_sz);
-	gen->min_log_pg_sz = MLX5_GET_PR(cmd_hca_cap, out, log_pg_sz);
-	gen->bf_reg_size = MLX5_GET_PR(cmd_hca_cap, out, bf);
-	gen->bf_reg_size = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_bf_reg_size);
-	gen->max_sq_desc_sz = MLX5_GET_PR(cmd_hca_cap, out, max_wqe_sz_sq);
-	gen->max_rq_desc_sz = MLX5_GET_PR(cmd_hca_cap, out, max_wqe_sz_rq);
-	gen->max_dc_sq_desc_sz = MLX5_GET_PR(cmd_hca_cap, out, max_wqe_sz_sq_dc);
-	gen->max_qp_mcg = MLX5_GET_PR(cmd_hca_cap, out, max_qp_mcg);
-	gen->log_max_pd = MLX5_GET_PR(cmd_hca_cap, out, log_max_pd);
-	gen->log_max_xrcd = MLX5_GET_PR(cmd_hca_cap, out, log_max_xrcd);
-	gen->log_uar_page_sz = MLX5_GET_PR(cmd_hca_cap, out, log_uar_page_sz);
-}
-
-static const char *caps_opmod_str(u16 opmod)
-{
-	switch (opmod) {
-	case HCA_CAP_OPMOD_GET_MAX:
-		return "GET_MAX";
-	case HCA_CAP_OPMOD_GET_CUR:
-		return "GET_CUR";
-	default:
-		return "Invalid";
-	}
-}
-
-int mlx5_core_get_caps(struct mlx5_core_dev *dev, struct mlx5_caps *caps,
-		       u16 opmod)
+int mlx5_core_get_caps(struct mlx5_core_dev *dev, enum mlx5_cap_type cap_type,
+		       enum mlx5_cap_mode cap_mode)
 {
 	u8 in[MLX5_ST_SZ_BYTES(query_hca_cap_in)];
 	int out_sz = MLX5_ST_SZ_BYTES(query_hca_cap_out);
-	void *out;
+	void *out, *hca_caps;
+	u16 opmod = (cap_type << 1) | (cap_mode & 0x01);
 	int err;
 
 	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, opmod);
 	err = mlx5_cmd_exec(dev, in, sizeof(in), out, out_sz);
@@ -377,12 +306,30 @@ int mlx5_core_get_caps(struct mlx5_core_dev *dev, struct mlx5_caps *caps,
 
 	err = mlx5_cmd_status_to_err_v2(out);
 	if (err) {
-		mlx5_core_warn(dev, "query max hca cap failed, %d\n", err);
+		mlx5_core_warn(dev,
+			       "QUERY_HCA_CAP : type(%x) opmode(%x) Failed(%d)\n",
+			       cap_type, cap_mode, err);
 		goto query_ex;
 	}
-	mlx5_core_dbg(dev, "%s\n", caps_opmod_str(opmod));
-	fw2drv_caps(caps, MLX5_ADDR_OF(query_hca_cap_out, out, capability_struct));
 
+	hca_caps =  MLX5_ADDR_OF(query_hca_cap_out, out, capability);
+
+	switch (cap_mode) {
+	case HCA_CAP_OPMOD_GET_MAX:
+		memcpy(dev->hca_caps_max[cap_type], hca_caps,
+		       MLX5_UN_SZ_BYTES(hca_cap_union));
+		break;
+	case HCA_CAP_OPMOD_GET_CUR:
+		memcpy(dev->hca_caps_cur[cap_type], hca_caps,
+		       MLX5_UN_SZ_BYTES(hca_cap_union));
+		break;
+	default:
+		mlx5_core_warn(dev,
+			       "Tried to query dev cap type(%x) with wrong opmode(%x)\n",
+			       cap_type, cap_mode);
+		err = -EINVAL;
+		break;
+	}
 query_ex:
 	kfree(out);
 	return err;
@@ -409,49 +356,45 @@ static int handle_hca_cap(struct mlx5_core_dev *dev)
 {
 	void *set_ctx = NULL;
 	struct mlx5_profile *prof = dev->profile;
-	struct mlx5_caps *cur_caps = NULL;
-	struct mlx5_caps *max_caps = NULL;
 	int err = -ENOMEM;
 	int set_sz = MLX5_ST_SZ_BYTES(set_hca_cap_in);
+	void *set_hca_cap;
 
 	set_ctx = kzalloc(set_sz, GFP_KERNEL);
 	if (!set_ctx)
 		goto query_ex;
 
-	max_caps = kzalloc(sizeof(*max_caps), GFP_KERNEL);
-	if (!max_caps)
-		goto query_ex;
-
-	cur_caps = kzalloc(sizeof(*cur_caps), GFP_KERNEL);
-	if (!cur_caps)
-		goto query_ex;
-
-	err = mlx5_core_get_caps(dev, max_caps, HCA_CAP_OPMOD_GET_MAX);
+	err = mlx5_core_get_caps(dev, MLX5_CAP_GENERAL, HCA_CAP_OPMOD_GET_MAX);
 	if (err)
 		goto query_ex;
 
-	err = mlx5_core_get_caps(dev, cur_caps, HCA_CAP_OPMOD_GET_CUR);
+	err = mlx5_core_get_caps(dev, MLX5_CAP_GENERAL, HCA_CAP_OPMOD_GET_CUR);
 	if (err)
 		goto query_ex;
 
+	set_hca_cap = MLX5_ADDR_OF(set_hca_cap_in, set_ctx,
+				   capability);
+	memcpy(set_hca_cap, dev->hca_caps_cur[MLX5_CAP_GENERAL],
+	       MLX5_ST_SZ_BYTES(cmd_hca_cap));
+
+	mlx5_core_dbg(dev, "Current Pkey table size %d Setting new size %d\n",
+		      mlx5_to_sw_pkey_sz(MLX5_CAP_GEN(dev, pkey_table_size)),
+		      128);
 	/* we limit the size of the pkey table to 128 entries for now */
-	cur_caps->gen.pkey_table_size = 128;
+	MLX5_SET(cmd_hca_cap, set_hca_cap, pkey_table_size,
+		 to_fw_pkey_sz(128));
 
 	if (prof->mask & MLX5_PROF_MASK_QP_SIZE)
-		cur_caps->gen.log_max_qp = prof->log_max_qp;
+		MLX5_SET(cmd_hca_cap, set_hca_cap, log_max_qp,
+			 prof->log_max_qp);
 
-	/* disable checksum */
-	cur_caps->gen.flags &= ~MLX5_DEV_CAP_FLAG_CMDIF_CSUM;
+	/* disable cmdif checksum */
+	MLX5_SET(cmd_hca_cap, set_hca_cap, cmdif_checksum, 0);
 
-	copy_rw_fields(MLX5_ADDR_OF(set_hca_cap_in, set_ctx, hca_capability_struct),
-		       cur_caps);
 	err = set_caps(dev, set_ctx, set_sz);
 
 query_ex:
-	kfree(cur_caps);
-	kfree(max_caps);
 	kfree(set_ctx);
-
 	return err;
 }
 
@@ -507,6 +450,74 @@ static int mlx5_core_disable_hca(struct mlx5_core_dev *dev)
 	return 0;
 }
 
+static int mlx5_irq_set_affinity_hint(struct mlx5_core_dev *mdev, int i)
+{
+	struct mlx5_priv *priv  = &mdev->priv;
+	struct msix_entry *msix = priv->msix_arr;
+	int irq                 = msix[i + MLX5_EQ_VEC_COMP_BASE].vector;
+	int numa_node           = dev_to_node(&mdev->pdev->dev);
+	int err;
+
+	if (!zalloc_cpumask_var(&priv->irq_info[i].mask, GFP_KERNEL)) {
+		mlx5_core_warn(mdev, "zalloc_cpumask_var failed");
+		return -ENOMEM;
+	}
+
+	cpumask_set_cpu(cpumask_local_spread(i, numa_node),
+			priv->irq_info[i].mask);
+
+	err = irq_set_affinity_hint(irq, priv->irq_info[i].mask);
+	if (err) {
+		mlx5_core_warn(mdev, "irq_set_affinity_hint failed,irq 0x%.4x",
+			       irq);
+		goto err_clear_mask;
+	}
+
+	return 0;
+
+err_clear_mask:
+	free_cpumask_var(priv->irq_info[i].mask);
+	return err;
+}
+
+static void mlx5_irq_clear_affinity_hint(struct mlx5_core_dev *mdev, int i)
+{
+	struct mlx5_priv *priv  = &mdev->priv;
+	struct msix_entry *msix = priv->msix_arr;
+	int irq                 = msix[i + MLX5_EQ_VEC_COMP_BASE].vector;
+
+	irq_set_affinity_hint(irq, NULL);
+	free_cpumask_var(priv->irq_info[i].mask);
+}
+
+static int mlx5_irq_set_affinity_hints(struct mlx5_core_dev *mdev)
+{
+	int err;
+	int i;
+
+	for (i = 0; i < mdev->priv.eq_table.num_comp_vectors; i++) {
+		err = mlx5_irq_set_affinity_hint(mdev, i);
+		if (err)
+			goto err_out;
+	}
+
+	return 0;
+
+err_out:
+	for (i--; i >= 0; i--)
+		mlx5_irq_clear_affinity_hint(mdev, i);
+
+	return err;
+}
+
+static void mlx5_irq_clear_affinity_hints(struct mlx5_core_dev *mdev)
+{
+	int i;
+
+	for (i = 0; i < mdev->priv.eq_table.num_comp_vectors; i++)
+		mlx5_irq_clear_affinity_hint(mdev, i);
+}
+
 int mlx5_vector2eqn(struct mlx5_core_dev *dev, int vector, int *eqn, int *irqn)
 {
 	struct mlx5_eq_table *table = &dev->priv.eq_table;
@@ -549,7 +560,7 @@ static void free_comp_eqs(struct mlx5_core_dev *dev)
 static int alloc_comp_eqs(struct mlx5_core_dev *dev)
 {
 	struct mlx5_eq_table *table = &dev->priv.eq_table;
-	char name[MLX5_MAX_EQ_NAME];
+	char name[MLX5_MAX_IRQ_NAME];
 	struct mlx5_eq *eq;
 	int ncomp_vec;
 	int nent;
@@ -566,7 +577,7 @@ static int alloc_comp_eqs(struct mlx5_core_dev *dev)
 			goto clean;
 		}
 
-		snprintf(name, MLX5_MAX_EQ_NAME, "mlx5_comp%d", i);
+		snprintf(name, MLX5_MAX_IRQ_NAME, "mlx5_comp%d", i);
 		err = mlx5_create_map_eq(dev, eq,
 					 i + MLX5_EQ_VEC_COMP_BASE, nent, 0,
 					 name, &dev->priv.uuari.uars[0]);
@@ -588,6 +599,61 @@ clean:
 	return err;
 }
 
+#ifdef CONFIG_MLX5_CORE_EN
+static int mlx5_core_set_issi(struct mlx5_core_dev *dev)
+{
+	u32 query_in[MLX5_ST_SZ_DW(query_issi_in)];
+	u32 query_out[MLX5_ST_SZ_DW(query_issi_out)];
+	u32 set_in[MLX5_ST_SZ_DW(set_issi_in)];
+	u32 set_out[MLX5_ST_SZ_DW(set_issi_out)];
+	int err;
+	u32 sup_issi;
+
+	memset(query_in, 0, sizeof(query_in));
+	memset(query_out, 0, sizeof(query_out));
+
+	MLX5_SET(query_issi_in, query_in, opcode, MLX5_CMD_OP_QUERY_ISSI);
+
+	err = mlx5_cmd_exec_check_status(dev, query_in, sizeof(query_in),
+					 query_out, sizeof(query_out));
+	if (err) {
+		if (((struct mlx5_outbox_hdr *)query_out)->status ==
+		    MLX5_CMD_STAT_BAD_OP_ERR) {
+			pr_debug("Only ISSI 0 is supported\n");
+			return 0;
+		}
+
+		pr_err("failed to query ISSI\n");
+		return err;
+	}
+
+	sup_issi = MLX5_GET(query_issi_out, query_out, supported_issi_dw0);
+
+	if (sup_issi & (1 << 1)) {
+		memset(set_in, 0, sizeof(set_in));
+		memset(set_out, 0, sizeof(set_out));
+
+		MLX5_SET(set_issi_in, set_in, opcode, MLX5_CMD_OP_SET_ISSI);
+		MLX5_SET(set_issi_in, set_in, current_issi, 1);
+
+		err = mlx5_cmd_exec_check_status(dev, set_in, sizeof(set_in),
+						 set_out, sizeof(set_out));
+		if (err) {
+			pr_err("failed to set ISSI=1\n");
+			return err;
+		}
+
+		dev->issi = 1;
+
+		return 0;
+	} else if (sup_issi & (1 << 0) || !sup_issi) {
+		return 0;
+	}
+
+	return -ENOTSUPP;
+}
+#endif
+
 static int mlx5_dev_init(struct mlx5_core_dev *dev, struct pci_dev *pdev)
 {
 	struct mlx5_priv *priv = &dev->priv;
@@ -650,6 +716,14 @@ static int mlx5_dev_init(struct mlx5_core_dev *dev, struct pci_dev *pdev)
 		goto err_pagealloc_cleanup;
 	}
 
+#ifdef CONFIG_MLX5_CORE_EN
+	err = mlx5_core_set_issi(dev);
+	if (err) {
+		dev_err(&pdev->dev, "failed to set issi\n");
+		goto err_disable_hca;
+	}
+#endif
+
 	err = mlx5_satisfy_startup_pages(dev, 1);
 	if (err) {
 		dev_err(&pdev->dev, "failed to allocate boot pages\n");
@@ -688,15 +762,15 @@ static int mlx5_dev_init(struct mlx5_core_dev *dev, struct pci_dev *pdev)
 
 	mlx5_start_health_poll(dev);
 
-	err = mlx5_cmd_query_hca_cap(dev, &dev->caps);
+	err = mlx5_query_hca_caps(dev);
 	if (err) {
 		dev_err(&pdev->dev, "query hca failed\n");
 		goto err_stop_poll;
 	}
 
-	err = mlx5_cmd_query_adapter(dev);
+	err = mlx5_query_board_id(dev);
 	if (err) {
-		dev_err(&pdev->dev, "query adapter failed\n");
+		dev_err(&pdev->dev, "query board id failed\n");
 		goto err_stop_poll;
 	}
 
@@ -730,6 +804,12 @@ static int mlx5_dev_init(struct mlx5_core_dev *dev, struct pci_dev *pdev)
 		goto err_stop_eqs;
 	}
 
+	err = mlx5_irq_set_affinity_hints(dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to alloc affinity hint cpumask\n");
+		goto err_free_comp_eqs;
+	}
+
 	MLX5_INIT_DOORBELL_LOCK(&priv->cq_uar_lock);
 
 	mlx5_init_cq_table(dev);
@@ -739,6 +819,9 @@ static int mlx5_dev_init(struct mlx5_core_dev *dev, struct pci_dev *pdev)
 
 	return 0;
 
+err_free_comp_eqs:
+	free_comp_eqs(dev);
+
 err_stop_eqs:
 	mlx5_stop_eqs(dev);
 
@@ -793,6 +876,7 @@ static void mlx5_dev_cleanup(struct mlx5_core_dev *dev)
 	mlx5_cleanup_srq_table(dev);
 	mlx5_cleanup_qp_table(dev);
 	mlx5_cleanup_cq_table(dev);
+	mlx5_irq_clear_affinity_hints(dev);
 	free_comp_eqs(dev);
 	mlx5_stop_eqs(dev);
 	mlx5_free_uuars(dev, &priv->uuari);
@@ -1048,6 +1132,10 @@ static int __init init(void)
 	if (err)
 		goto err_health;
 
+#ifdef CONFIG_MLX5_CORE_EN
+	mlx5e_init();
+#endif
+
 	return 0;
 
 err_health:
@@ -1060,6 +1148,9 @@ err_debug:
 
 static void __exit cleanup(void)
 {
+#ifdef CONFIG_MLX5_CORE_EN
+	mlx5e_cleanup();
+#endif
 	pci_unregister_driver(&mlx5_core_driver);
 	mlx5_health_cleanup();
 	destroy_workqueue(mlx5_core_wq);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mcg.c b/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
index d79fd85d1dd5..d5a0c2d61a18 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mcg.c
@@ -91,7 +91,7 @@ int mlx5_core_detach_mcg(struct mlx5_core_dev *dev, union ib_gid *mgid, u32 qpn)
 
 	memset(&in, 0, sizeof(in));
 	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DETACH_FROM_MCG);
+	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DETTACH_FROM_MCG);
 	memcpy(in.gid, mgid, sizeof(*mgid));
 	in.qpn = cpu_to_be32(qpn);
 	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
index a051b906afdf..fc88ecaecb4b 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  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
@@ -37,6 +37,10 @@
 #include <linux/kernel.h>
 #include <linux/sched.h>
 
+#define DRIVER_NAME "mlx5_core"
+#define DRIVER_VERSION "3.0-1"
+#define DRIVER_RELDATE  "January 2015"
+
 extern int mlx5_core_debug_mask;
 
 #define mlx5_core_dbg(dev, format, ...)					\
@@ -65,11 +69,20 @@ enum {
 	MLX5_CMD_TIME, /* print command execution time */
 };
 
+static inline int mlx5_cmd_exec_check_status(struct mlx5_core_dev *dev, u32 *in,
+					     int in_size, u32 *out,
+					     int out_size)
+{
+	mlx5_cmd_exec(dev, in, in_size, out, out_size);
+	return mlx5_cmd_status_to_err((struct mlx5_outbox_hdr *)out);
+}
 
-int mlx5_cmd_query_hca_cap(struct mlx5_core_dev *dev,
-			   struct mlx5_caps *caps);
-int mlx5_cmd_query_adapter(struct mlx5_core_dev *dev);
+int mlx5_query_hca_caps(struct mlx5_core_dev *dev);
+int mlx5_query_board_id(struct mlx5_core_dev *dev);
 int mlx5_cmd_init_hca(struct mlx5_core_dev *dev);
 int mlx5_cmd_teardown_hca(struct mlx5_core_dev *dev);
 
+void mlx5e_init(void);
+void mlx5e_cleanup(void);
+
 #endif /* __MLX5_CORE_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/port.c b/drivers/net/ethernet/mellanox/mlx5/core/port.c
index 49e90f2612d8..70147999f657 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/port.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/port.c
@@ -102,3 +102,229 @@ int mlx5_set_port_caps(struct mlx5_core_dev *dev, u8 port_num, u32 caps)
 	return err;
 }
 EXPORT_SYMBOL_GPL(mlx5_set_port_caps);
+
+int mlx5_query_port_ptys(struct mlx5_core_dev *dev, u32 *ptys,
+			 int ptys_size, int proto_mask, u8 local_port)
+{
+	u32 in[MLX5_ST_SZ_DW(ptys_reg)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+	MLX5_SET(ptys_reg, in, local_port, local_port);
+	MLX5_SET(ptys_reg, in, proto_mask, proto_mask);
+
+	err = mlx5_core_access_reg(dev, in, sizeof(in), ptys,
+				   ptys_size, MLX5_REG_PTYS, 0, 0);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_port_ptys);
+
+int mlx5_query_port_proto_cap(struct mlx5_core_dev *dev,
+			      u32 *proto_cap, int proto_mask)
+{
+	u32 out[MLX5_ST_SZ_DW(ptys_reg)];
+	int err;
+
+	err = mlx5_query_port_ptys(dev, out, sizeof(out), proto_mask, 1);
+	if (err)
+		return err;
+
+	if (proto_mask == MLX5_PTYS_EN)
+		*proto_cap = MLX5_GET(ptys_reg, out, eth_proto_capability);
+	else
+		*proto_cap = MLX5_GET(ptys_reg, out, ib_proto_capability);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_port_proto_cap);
+
+int mlx5_query_port_proto_admin(struct mlx5_core_dev *dev,
+				u32 *proto_admin, int proto_mask)
+{
+	u32 out[MLX5_ST_SZ_DW(ptys_reg)];
+	int err;
+
+	err = mlx5_query_port_ptys(dev, out, sizeof(out), proto_mask, 1);
+	if (err)
+		return err;
+
+	if (proto_mask == MLX5_PTYS_EN)
+		*proto_admin = MLX5_GET(ptys_reg, out, eth_proto_admin);
+	else
+		*proto_admin = MLX5_GET(ptys_reg, out, ib_proto_admin);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_port_proto_admin);
+
+int mlx5_query_port_link_width_oper(struct mlx5_core_dev *dev,
+				    u8 *link_width_oper, u8 local_port)
+{
+	u32 out[MLX5_ST_SZ_DW(ptys_reg)];
+	int err;
+
+	err = mlx5_query_port_ptys(dev, out, sizeof(out), MLX5_PTYS_IB, local_port);
+	if (err)
+		return err;
+
+	*link_width_oper = MLX5_GET(ptys_reg, out, ib_link_width_oper);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_port_link_width_oper);
+
+int mlx5_query_port_proto_oper(struct mlx5_core_dev *dev,
+			       u8 *proto_oper, int proto_mask,
+			       u8 local_port)
+{
+	u32 out[MLX5_ST_SZ_DW(ptys_reg)];
+	int err;
+
+	err = mlx5_query_port_ptys(dev, out, sizeof(out), proto_mask, local_port);
+	if (err)
+		return err;
+
+	if (proto_mask == MLX5_PTYS_EN)
+		*proto_oper = MLX5_GET(ptys_reg, out, eth_proto_oper);
+	else
+		*proto_oper = MLX5_GET(ptys_reg, out, ib_proto_oper);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_port_proto_oper);
+
+int mlx5_set_port_proto(struct mlx5_core_dev *dev, u32 proto_admin,
+			int proto_mask)
+{
+	u32 in[MLX5_ST_SZ_DW(ptys_reg)];
+	u32 out[MLX5_ST_SZ_DW(ptys_reg)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(ptys_reg, in, local_port, 1);
+	MLX5_SET(ptys_reg, in, proto_mask, proto_mask);
+	if (proto_mask == MLX5_PTYS_EN)
+		MLX5_SET(ptys_reg, in, eth_proto_admin, proto_admin);
+	else
+		MLX5_SET(ptys_reg, in, ib_proto_admin, proto_admin);
+
+	err = mlx5_core_access_reg(dev, in, sizeof(in), out,
+				   sizeof(out), MLX5_REG_PTYS, 0, 1);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_set_port_proto);
+
+int mlx5_set_port_status(struct mlx5_core_dev *dev,
+			 enum mlx5_port_status status)
+{
+	u32 in[MLX5_ST_SZ_DW(paos_reg)];
+	u32 out[MLX5_ST_SZ_DW(paos_reg)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(paos_reg, in, admin_status, status);
+	MLX5_SET(paos_reg, in, ase, 1);
+
+	return mlx5_core_access_reg(dev, in, sizeof(in), out,
+				    sizeof(out), MLX5_REG_PAOS, 0, 1);
+}
+
+int mlx5_query_port_status(struct mlx5_core_dev *dev, u8 *status)
+{
+	u32 in[MLX5_ST_SZ_DW(paos_reg)];
+	u32 out[MLX5_ST_SZ_DW(paos_reg)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+
+	err = mlx5_core_access_reg(dev, in, sizeof(in), out,
+				   sizeof(out), MLX5_REG_PAOS, 0, 0);
+	if (err)
+		return err;
+
+	*status = MLX5_GET(paos_reg, out, oper_status);
+	return err;
+}
+
+static void mlx5_query_port_mtu(struct mlx5_core_dev *dev, int *admin_mtu,
+				int *max_mtu, int *oper_mtu, u8 port)
+{
+	u32 in[MLX5_ST_SZ_DW(pmtu_reg)];
+	u32 out[MLX5_ST_SZ_DW(pmtu_reg)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(pmtu_reg, in, local_port, port);
+
+	mlx5_core_access_reg(dev, in, sizeof(in), out,
+			     sizeof(out), MLX5_REG_PMTU, 0, 0);
+
+	if (max_mtu)
+		*max_mtu  = MLX5_GET(pmtu_reg, out, max_mtu);
+	if (oper_mtu)
+		*oper_mtu = MLX5_GET(pmtu_reg, out, oper_mtu);
+	if (admin_mtu)
+		*admin_mtu = MLX5_GET(pmtu_reg, out, admin_mtu);
+}
+
+int mlx5_set_port_mtu(struct mlx5_core_dev *dev, int mtu, u8 port)
+{
+	u32 in[MLX5_ST_SZ_DW(pmtu_reg)];
+	u32 out[MLX5_ST_SZ_DW(pmtu_reg)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(pmtu_reg, in, admin_mtu, mtu);
+	MLX5_SET(pmtu_reg, in, local_port, port);
+
+	return mlx5_core_access_reg(dev, in, sizeof(in), out,
+				   sizeof(out), MLX5_REG_PMTU, 0, 1);
+}
+EXPORT_SYMBOL_GPL(mlx5_set_port_mtu);
+
+void mlx5_query_port_max_mtu(struct mlx5_core_dev *dev, int *max_mtu,
+			     u8 port)
+{
+	mlx5_query_port_mtu(dev, NULL, max_mtu, NULL, port);
+}
+EXPORT_SYMBOL_GPL(mlx5_query_port_max_mtu);
+
+void mlx5_query_port_oper_mtu(struct mlx5_core_dev *dev, int *oper_mtu,
+			      u8 port)
+{
+	mlx5_query_port_mtu(dev, NULL, NULL, oper_mtu, port);
+}
+EXPORT_SYMBOL_GPL(mlx5_query_port_oper_mtu);
+
+static int mlx5_query_port_pvlc(struct mlx5_core_dev *dev, u32 *pvlc,
+				int pvlc_size,  u8 local_port)
+{
+	u32 in[MLX5_ST_SZ_DW(pvlc_reg)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+	MLX5_SET(ptys_reg, in, local_port, local_port);
+
+	err = mlx5_core_access_reg(dev, in, sizeof(in), pvlc,
+				   pvlc_size, MLX5_REG_PVLC, 0, 0);
+
+	return err;
+}
+
+int mlx5_query_port_vl_hw_cap(struct mlx5_core_dev *dev,
+			      u8 *vl_hw_cap, u8 local_port)
+{
+	u32 out[MLX5_ST_SZ_DW(pvlc_reg)];
+	int err;
+
+	err = mlx5_query_port_pvlc(dev, out, sizeof(out), local_port);
+	if (err)
+		return err;
+
+	*vl_hw_cap = MLX5_GET(pvlc_reg, out, vl_hw_cap);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_port_vl_hw_cap);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/qp.c b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
index dc7dbf7e9d98..8b494b562263 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
@@ -187,10 +187,17 @@ int mlx5_core_create_qp(struct mlx5_core_dev *dev,
 	struct mlx5_destroy_qp_mbox_in din;
 	struct mlx5_destroy_qp_mbox_out dout;
 	int err;
+	void *qpc;
 
 	memset(&out, 0, sizeof(out));
 	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_CREATE_QP);
 
+	if (dev->issi) {
+		qpc = MLX5_ADDR_OF(create_qp_in, in, qpc);
+		/* 0xffffff means we ask to work with cqe version 0 */
+		MLX5_SET(qpc, qpc, user_index, 0xffffff);
+	}
+
 	err = mlx5_cmd_exec(dev, in, inlen, &out, sizeof(out));
 	if (err) {
 		mlx5_core_warn(dev, "ret %d\n", err);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/srq.c b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
index f9d25dcd03c1..c48f504ccbeb 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/srq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
@@ -37,6 +37,7 @@
 #include <linux/mlx5/srq.h>
 #include <rdma/ib_verbs.h>
 #include "mlx5_core.h"
+#include "transobj.h"
 
 void mlx5_srq_event(struct mlx5_core_dev *dev, u32 srqn, int event_type)
 {
@@ -62,6 +63,74 @@ void mlx5_srq_event(struct mlx5_core_dev *dev, u32 srqn, int event_type)
 		complete(&srq->free);
 }
 
+static int get_pas_size(void *srqc)
+{
+	u32 log_page_size = MLX5_GET(srqc, srqc, log_page_size) + 12;
+	u32 log_srq_size  = MLX5_GET(srqc, srqc, log_srq_size);
+	u32 log_rq_stride = MLX5_GET(srqc, srqc, log_rq_stride);
+	u32 page_offset   = MLX5_GET(srqc, srqc, page_offset);
+	u32 po_quanta	  = 1 << (log_page_size - 6);
+	u32 rq_sz	  = 1 << (log_srq_size + 4 + log_rq_stride);
+	u32 page_size	  = 1 << log_page_size;
+	u32 rq_sz_po      = rq_sz + (page_offset * po_quanta);
+	u32 rq_num_pas	  = (rq_sz_po + page_size - 1) / page_size;
+
+	return rq_num_pas * sizeof(u64);
+}
+
+static void rmpc_srqc_reformat(void *srqc, void *rmpc, bool srqc_to_rmpc)
+{
+	void *wq = MLX5_ADDR_OF(rmpc, rmpc, wq);
+
+	if (srqc_to_rmpc) {
+		switch (MLX5_GET(srqc, srqc, state)) {
+		case MLX5_SRQC_STATE_GOOD:
+			MLX5_SET(rmpc, rmpc, state, MLX5_RMPC_STATE_RDY);
+			break;
+		case MLX5_SRQC_STATE_ERROR:
+			MLX5_SET(rmpc, rmpc, state, MLX5_RMPC_STATE_ERR);
+			break;
+		default:
+			pr_warn("%s: %d: Unknown srq state = 0x%x\n", __func__,
+				__LINE__, MLX5_GET(srqc, srqc, state));
+			MLX5_SET(rmpc, rmpc, state, MLX5_GET(srqc, srqc, state));
+		}
+
+		MLX5_SET(wq,   wq, wq_signature,  MLX5_GET(srqc,  srqc, wq_signature));
+		MLX5_SET(wq,   wq, log_wq_pg_sz,  MLX5_GET(srqc,  srqc, log_page_size));
+		MLX5_SET(wq,   wq, log_wq_stride, MLX5_GET(srqc,  srqc, log_rq_stride) + 4);
+		MLX5_SET(wq,   wq, log_wq_sz,     MLX5_GET(srqc,  srqc, log_srq_size));
+		MLX5_SET(wq,   wq, page_offset,   MLX5_GET(srqc,  srqc, page_offset));
+		MLX5_SET(wq,   wq, lwm,           MLX5_GET(srqc,  srqc, lwm));
+		MLX5_SET(wq,   wq, pd,            MLX5_GET(srqc,  srqc, pd));
+		MLX5_SET64(wq, wq, dbr_addr, MLX5_GET64(srqc,	  srqc, dbr_addr));
+	} else {
+		switch (MLX5_GET(rmpc, rmpc, state)) {
+		case MLX5_RMPC_STATE_RDY:
+			MLX5_SET(srqc, srqc, state, MLX5_SRQC_STATE_GOOD);
+			break;
+		case MLX5_RMPC_STATE_ERR:
+			MLX5_SET(srqc, srqc, state, MLX5_SRQC_STATE_ERROR);
+			break;
+		default:
+			pr_warn("%s: %d: Unknown rmp state = 0x%x\n",
+				__func__, __LINE__,
+				MLX5_GET(rmpc, rmpc, state));
+			MLX5_SET(srqc, srqc, state,
+				 MLX5_GET(rmpc, rmpc, state));
+		}
+
+		MLX5_SET(srqc,   srqc, wq_signature,   MLX5_GET(wq,   wq, wq_signature));
+		MLX5_SET(srqc,   srqc, log_page_size,  MLX5_GET(wq,   wq, log_wq_pg_sz));
+		MLX5_SET(srqc,   srqc, log_rq_stride,  MLX5_GET(wq,   wq, log_wq_stride) - 4);
+		MLX5_SET(srqc,   srqc, log_srq_size,   MLX5_GET(wq,   wq, log_wq_sz));
+		MLX5_SET(srqc,   srqc, page_offset,    MLX5_GET(wq,   wq, page_offset));
+		MLX5_SET(srqc,   srqc, lwm,	       MLX5_GET(wq,   wq, lwm));
+		MLX5_SET(srqc,   srqc, pd,	       MLX5_GET(wq,   wq, pd));
+		MLX5_SET64(srqc, srqc, dbr_addr,       MLX5_GET64(wq, wq, dbr_addr));
+	}
+}
+
 struct mlx5_core_srq *mlx5_core_get_srq(struct mlx5_core_dev *dev, u32 srqn)
 {
 	struct mlx5_srq_table *table = &dev->priv.srq_table;
@@ -79,26 +148,311 @@ struct mlx5_core_srq *mlx5_core_get_srq(struct mlx5_core_dev *dev, u32 srqn)
 }
 EXPORT_SYMBOL(mlx5_core_get_srq);
 
-int mlx5_core_create_srq(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
-			 struct mlx5_create_srq_mbox_in *in, int inlen)
+static int create_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
+			  struct mlx5_create_srq_mbox_in *in, int inlen)
 {
 	struct mlx5_create_srq_mbox_out out;
-	struct mlx5_srq_table *table = &dev->priv.srq_table;
-	struct mlx5_destroy_srq_mbox_in din;
-	struct mlx5_destroy_srq_mbox_out dout;
 	int err;
 
 	memset(&out, 0, sizeof(out));
+
 	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_CREATE_SRQ);
-	err = mlx5_cmd_exec(dev, in, inlen, &out, sizeof(out));
-	if (err)
-		return err;
 
-	if (out.hdr.status)
-		return mlx5_cmd_status_to_err(&out.hdr);
+	err = mlx5_cmd_exec_check_status(dev, (u32 *)in, inlen, (u32 *)(&out),
+					 sizeof(out));
 
 	srq->srqn = be32_to_cpu(out.srqn) & 0xffffff;
 
+	return err;
+}
+
+static int destroy_srq_cmd(struct mlx5_core_dev *dev,
+			   struct mlx5_core_srq *srq)
+{
+	struct mlx5_destroy_srq_mbox_in in;
+	struct mlx5_destroy_srq_mbox_out out;
+
+	memset(&in, 0, sizeof(in));
+	memset(&out, 0, sizeof(out));
+	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_SRQ);
+	in.srqn = cpu_to_be32(srq->srqn);
+
+	return mlx5_cmd_exec_check_status(dev, (u32 *)(&in), sizeof(in),
+					  (u32 *)(&out), sizeof(out));
+}
+
+static int arm_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
+		       u16 lwm, int is_srq)
+{
+	struct mlx5_arm_srq_mbox_in	in;
+	struct mlx5_arm_srq_mbox_out	out;
+
+	memset(&in, 0, sizeof(in));
+	memset(&out, 0, sizeof(out));
+
+	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_ARM_RQ);
+	in.hdr.opmod = cpu_to_be16(!!is_srq);
+	in.srqn = cpu_to_be32(srq->srqn);
+	in.lwm = cpu_to_be16(lwm);
+
+	return mlx5_cmd_exec_check_status(dev, (u32 *)(&in),
+					  sizeof(in), (u32 *)(&out),
+					  sizeof(out));
+}
+
+static int query_srq_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
+			 struct mlx5_query_srq_mbox_out *out)
+{
+	struct mlx5_query_srq_mbox_in in;
+
+	memset(&in, 0, sizeof(in));
+
+	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_SRQ);
+	in.srqn = cpu_to_be32(srq->srqn);
+
+	return mlx5_cmd_exec_check_status(dev, (u32 *)(&in), sizeof(in),
+					  (u32 *)out, sizeof(*out));
+}
+
+static int create_xrc_srq_cmd(struct mlx5_core_dev *dev,
+			      struct mlx5_core_srq *srq,
+			      struct mlx5_create_srq_mbox_in *in,
+			      int srq_inlen)
+{
+	u32 create_out[MLX5_ST_SZ_DW(create_xrc_srq_out)];
+	void *create_in;
+	void *srqc;
+	void *xrc_srqc;
+	void *pas;
+	int pas_size;
+	int inlen;
+	int err;
+
+	srqc	  = MLX5_ADDR_OF(create_srq_in, in, srq_context_entry);
+	pas_size  = get_pas_size(srqc);
+	inlen	  = MLX5_ST_SZ_BYTES(create_xrc_srq_in) + pas_size;
+	create_in = mlx5_vzalloc(inlen);
+	if (!create_in)
+		return -ENOMEM;
+
+	xrc_srqc = MLX5_ADDR_OF(create_xrc_srq_in, create_in,
+				xrc_srq_context_entry);
+	pas	 = MLX5_ADDR_OF(create_xrc_srq_in, create_in, pas);
+
+	memcpy(xrc_srqc, srqc, MLX5_ST_SZ_BYTES(srqc));
+	memcpy(pas, in->pas, pas_size);
+	/* 0xffffff means we ask to work with cqe version 0 */
+	MLX5_SET(xrc_srqc,	    xrc_srqc,  user_index, 0xffffff);
+	MLX5_SET(create_xrc_srq_in, create_in, opcode,
+		 MLX5_CMD_OP_CREATE_XRC_SRQ);
+
+	memset(create_out, 0, sizeof(create_out));
+	err = mlx5_cmd_exec_check_status(dev, create_in, inlen, create_out,
+					 sizeof(create_out));
+	if (err)
+		goto out;
+
+	srq->srqn = MLX5_GET(create_xrc_srq_out, create_out, xrc_srqn);
+out:
+	kvfree(create_in);
+	return err;
+}
+
+static int destroy_xrc_srq_cmd(struct mlx5_core_dev *dev,
+			       struct mlx5_core_srq *srq)
+{
+	u32 xrcsrq_in[MLX5_ST_SZ_DW(destroy_xrc_srq_in)];
+	u32 xrcsrq_out[MLX5_ST_SZ_DW(destroy_xrc_srq_out)];
+
+	memset(xrcsrq_in, 0, sizeof(xrcsrq_in));
+	memset(xrcsrq_out, 0, sizeof(xrcsrq_out));
+
+	MLX5_SET(destroy_xrc_srq_in, xrcsrq_in, opcode,
+		 MLX5_CMD_OP_DESTROY_XRC_SRQ);
+	MLX5_SET(destroy_xrc_srq_in, xrcsrq_in, xrc_srqn, srq->srqn);
+
+	return mlx5_cmd_exec_check_status(dev, xrcsrq_in, sizeof(xrcsrq_in),
+					  xrcsrq_out, sizeof(xrcsrq_out));
+}
+
+static int arm_xrc_srq_cmd(struct mlx5_core_dev *dev,
+			   struct mlx5_core_srq *srq, u16 lwm)
+{
+	u32 xrcsrq_in[MLX5_ST_SZ_DW(arm_xrc_srq_in)];
+	u32 xrcsrq_out[MLX5_ST_SZ_DW(arm_xrc_srq_out)];
+
+	memset(xrcsrq_in, 0, sizeof(xrcsrq_in));
+	memset(xrcsrq_out, 0, sizeof(xrcsrq_out));
+
+	MLX5_SET(arm_xrc_srq_in, xrcsrq_in, opcode,   MLX5_CMD_OP_ARM_XRC_SRQ);
+	MLX5_SET(arm_xrc_srq_in, xrcsrq_in, op_mod,   MLX5_ARM_XRC_SRQ_IN_OP_MOD_XRC_SRQ);
+	MLX5_SET(arm_xrc_srq_in, xrcsrq_in, xrc_srqn, srq->srqn);
+	MLX5_SET(arm_xrc_srq_in, xrcsrq_in, lwm,      lwm);
+
+	return  mlx5_cmd_exec_check_status(dev, xrcsrq_in, sizeof(xrcsrq_in),
+					   xrcsrq_out, sizeof(xrcsrq_out));
+}
+
+static int query_xrc_srq_cmd(struct mlx5_core_dev *dev,
+			     struct mlx5_core_srq *srq,
+			     struct mlx5_query_srq_mbox_out *out)
+{
+	u32 xrcsrq_in[MLX5_ST_SZ_DW(query_xrc_srq_in)];
+	u32 *xrcsrq_out;
+	void *srqc;
+	void *xrc_srqc;
+	int err;
+
+	xrcsrq_out = mlx5_vzalloc(MLX5_ST_SZ_BYTES(query_xrc_srq_out));
+	if (!xrcsrq_out)
+		return -ENOMEM;
+	memset(xrcsrq_in, 0, sizeof(xrcsrq_in));
+
+	MLX5_SET(query_xrc_srq_in, xrcsrq_in, opcode,
+		 MLX5_CMD_OP_QUERY_XRC_SRQ);
+	MLX5_SET(query_xrc_srq_in, xrcsrq_in, xrc_srqn, srq->srqn);
+	err =  mlx5_cmd_exec_check_status(dev, xrcsrq_in, sizeof(xrcsrq_in),
+					  xrcsrq_out,
+					  MLX5_ST_SZ_BYTES(query_xrc_srq_out));
+	if (err)
+		goto out;
+
+	xrc_srqc = MLX5_ADDR_OF(query_xrc_srq_out, xrcsrq_out,
+				xrc_srq_context_entry);
+	srqc = MLX5_ADDR_OF(query_srq_out, out, srq_context_entry);
+	memcpy(srqc, xrc_srqc, MLX5_ST_SZ_BYTES(srqc));
+
+out:
+	kvfree(xrcsrq_out);
+	return err;
+}
+
+static int create_rmp_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
+			  struct mlx5_create_srq_mbox_in *in, int srq_inlen)
+{
+	void *create_in;
+	void *rmpc;
+	void *srqc;
+	int pas_size;
+	int inlen;
+	int err;
+
+	srqc = MLX5_ADDR_OF(create_srq_in, in, srq_context_entry);
+	pas_size = get_pas_size(srqc);
+	inlen = MLX5_ST_SZ_BYTES(create_rmp_in) + pas_size;
+	create_in = mlx5_vzalloc(inlen);
+	if (!create_in)
+		return -ENOMEM;
+
+	rmpc = MLX5_ADDR_OF(create_rmp_in, create_in, ctx);
+
+	memcpy(MLX5_ADDR_OF(rmpc, rmpc, wq.pas), in->pas, pas_size);
+	rmpc_srqc_reformat(srqc, rmpc, true);
+
+	err = mlx5_core_create_rmp(dev, create_in, inlen, &srq->srqn);
+
+	kvfree(create_in);
+	return err;
+}
+
+static int destroy_rmp_cmd(struct mlx5_core_dev *dev,
+			   struct mlx5_core_srq *srq)
+{
+	return mlx5_core_destroy_rmp(dev, srq->srqn);
+}
+
+static int arm_rmp_cmd(struct mlx5_core_dev *dev,
+		       struct mlx5_core_srq *srq,
+		       u16 lwm)
+{
+	void *in;
+	void *rmpc;
+	void *wq;
+	void *bitmask;
+	int err;
+
+	in = mlx5_vzalloc(MLX5_ST_SZ_BYTES(modify_rmp_in));
+	if (!in)
+		return -ENOMEM;
+
+	rmpc =	  MLX5_ADDR_OF(modify_rmp_in,   in,   ctx);
+	bitmask = MLX5_ADDR_OF(modify_rmp_in,   in,   bitmask);
+	wq   =	  MLX5_ADDR_OF(rmpc,	        rmpc, wq);
+
+	MLX5_SET(modify_rmp_in, in,	 rmp_state, MLX5_RMPC_STATE_RDY);
+	MLX5_SET(modify_rmp_in, in,	 rmpn,      srq->srqn);
+	MLX5_SET(wq,		wq,	 lwm,	    lwm);
+	MLX5_SET(rmp_bitmask,	bitmask, lwm,	    1);
+	MLX5_SET(rmpc, rmpc, state, MLX5_RMPC_STATE_RDY);
+
+	err = mlx5_core_modify_rmp(dev, in, MLX5_ST_SZ_BYTES(modify_rmp_in));
+
+	kvfree(in);
+	return err;
+}
+
+static int query_rmp_cmd(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
+			 struct mlx5_query_srq_mbox_out *out)
+{
+	u32 *rmp_out;
+	void *rmpc;
+	void *srqc;
+	int err;
+
+	rmp_out =  mlx5_vzalloc(MLX5_ST_SZ_BYTES(query_rmp_out));
+	if (!rmp_out)
+		return -ENOMEM;
+
+	err = mlx5_core_query_rmp(dev, srq->srqn, rmp_out);
+	if (err)
+		goto out;
+
+	srqc = MLX5_ADDR_OF(query_srq_out, out,	    srq_context_entry);
+	rmpc = MLX5_ADDR_OF(query_rmp_out, rmp_out, rmp_context);
+	rmpc_srqc_reformat(srqc, rmpc, false);
+
+out:
+	kvfree(rmp_out);
+	return err;
+}
+
+static int create_srq_split(struct mlx5_core_dev *dev,
+			    struct mlx5_core_srq *srq,
+			    struct mlx5_create_srq_mbox_in *in,
+			    int inlen, int is_xrc)
+{
+	if (!dev->issi)
+		return create_srq_cmd(dev, srq, in, inlen);
+	else if (srq->common.res == MLX5_RES_XSRQ)
+		return create_xrc_srq_cmd(dev, srq, in, inlen);
+	else
+		return create_rmp_cmd(dev, srq, in, inlen);
+}
+
+static int destroy_srq_split(struct mlx5_core_dev *dev,
+			     struct mlx5_core_srq *srq)
+{
+	if (!dev->issi)
+		return destroy_srq_cmd(dev, srq);
+	else if (srq->common.res == MLX5_RES_XSRQ)
+		return destroy_xrc_srq_cmd(dev, srq);
+	else
+		return destroy_rmp_cmd(dev, srq);
+}
+
+int mlx5_core_create_srq(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
+			 struct mlx5_create_srq_mbox_in *in, int inlen,
+			 int is_xrc)
+{
+	int err;
+	struct mlx5_srq_table *table = &dev->priv.srq_table;
+
+	srq->common.res = is_xrc ? MLX5_RES_XSRQ : MLX5_RES_SRQ;
+
+	err = create_srq_split(dev, srq, in, inlen, is_xrc);
+	if (err)
+		return err;
+
 	atomic_set(&srq->refcount, 1);
 	init_completion(&srq->free);
 
@@ -107,25 +461,20 @@ int mlx5_core_create_srq(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 	spin_unlock_irq(&table->lock);
 	if (err) {
 		mlx5_core_warn(dev, "err %d, srqn 0x%x\n", err, srq->srqn);
-		goto err_cmd;
+		goto err_destroy_srq_split;
 	}
 
 	return 0;
 
-err_cmd:
-	memset(&din, 0, sizeof(din));
-	memset(&dout, 0, sizeof(dout));
-	din.srqn = cpu_to_be32(srq->srqn);
-	din.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_SRQ);
-	mlx5_cmd_exec(dev, &din, sizeof(din), &dout, sizeof(dout));
+err_destroy_srq_split:
+	destroy_srq_split(dev, srq);
+
 	return err;
 }
 EXPORT_SYMBOL(mlx5_core_create_srq);
 
 int mlx5_core_destroy_srq(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq)
 {
-	struct mlx5_destroy_srq_mbox_in in;
-	struct mlx5_destroy_srq_mbox_out out;
 	struct mlx5_srq_table *table = &dev->priv.srq_table;
 	struct mlx5_core_srq *tmp;
 	int err;
@@ -142,17 +491,10 @@ int mlx5_core_destroy_srq(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq)
 		return -EINVAL;
 	}
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DESTROY_SRQ);
-	in.srqn = cpu_to_be32(srq->srqn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
+	err = destroy_srq_split(dev, srq);
 	if (err)
 		return err;
 
-	if (out.hdr.status)
-		return mlx5_cmd_status_to_err(&out.hdr);
-
 	if (atomic_dec_and_test(&srq->refcount))
 		complete(&srq->free);
 	wait_for_completion(&srq->free);
@@ -164,48 +506,24 @@ EXPORT_SYMBOL(mlx5_core_destroy_srq);
 int mlx5_core_query_srq(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 			struct mlx5_query_srq_mbox_out *out)
 {
-	struct mlx5_query_srq_mbox_in in;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(out, 0, sizeof(*out));
-
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_SRQ);
-	in.srqn = cpu_to_be32(srq->srqn);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), out, sizeof(*out));
-	if (err)
-		return err;
-
-	if (out->hdr.status)
-		return mlx5_cmd_status_to_err(&out->hdr);
-
-	return err;
+	if (!dev->issi)
+		return query_srq_cmd(dev, srq, out);
+	else if (srq->common.res == MLX5_RES_XSRQ)
+		return query_xrc_srq_cmd(dev, srq, out);
+	else
+		return query_rmp_cmd(dev, srq, out);
 }
 EXPORT_SYMBOL(mlx5_core_query_srq);
 
 int mlx5_core_arm_srq(struct mlx5_core_dev *dev, struct mlx5_core_srq *srq,
 		      u16 lwm, int is_srq)
 {
-	struct mlx5_arm_srq_mbox_in	in;
-	struct mlx5_arm_srq_mbox_out	out;
-	int err;
-
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_ARM_RQ);
-	in.hdr.opmod = cpu_to_be16(!!is_srq);
-	in.srqn = cpu_to_be32(srq->srqn);
-	in.lwm = cpu_to_be16(lwm);
-
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
-	if (err)
-		return err;
-
-	if (out.hdr.status)
-		return mlx5_cmd_status_to_err(&out.hdr);
-
-	return err;
+	if (!dev->issi)
+		return arm_srq_cmd(dev, srq, lwm, is_srq);
+	else if (srq->common.res == MLX5_RES_XSRQ)
+		return arm_xrc_srq_cmd(dev, srq, lwm);
+	else
+		return arm_rmp_cmd(dev, srq, lwm);
 }
 EXPORT_SYMBOL(mlx5_core_arm_srq);
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/transobj.c b/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
new file mode 100644
index 000000000000..8d98b03026d5
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
@@ -0,0 +1,360 @@
+/*
+ * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  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/mlx5/driver.h>
+#include "mlx5_core.h"
+#include "transobj.h"
+
+int mlx5_alloc_transport_domain(struct mlx5_core_dev *dev, u32 *tdn)
+{
+	u32 in[MLX5_ST_SZ_DW(alloc_transport_domain_in)];
+	u32 out[MLX5_ST_SZ_DW(alloc_transport_domain_out)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+
+	MLX5_SET(alloc_transport_domain_in, in, opcode,
+		 MLX5_CMD_OP_ALLOC_TRANSPORT_DOMAIN);
+
+	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+	if (!err)
+		*tdn = MLX5_GET(alloc_transport_domain_out, out,
+				transport_domain);
+
+	return err;
+}
+
+void mlx5_dealloc_transport_domain(struct mlx5_core_dev *dev, u32 tdn)
+{
+	u32 in[MLX5_ST_SZ_DW(dealloc_transport_domain_in)];
+	u32 out[MLX5_ST_SZ_DW(dealloc_transport_domain_out)];
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+
+	MLX5_SET(dealloc_transport_domain_in, in, opcode,
+		 MLX5_CMD_OP_DEALLOC_TRANSPORT_DOMAIN);
+	MLX5_SET(dealloc_transport_domain_in, in, transport_domain, tdn);
+
+	mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+}
+
+int mlx5_core_create_rq(struct mlx5_core_dev *dev, u32 *in, int inlen, u32 *rqn)
+{
+	u32 out[MLX5_ST_SZ_DW(create_rq_out)];
+	int err;
+
+	MLX5_SET(create_rq_in, in, opcode, MLX5_CMD_OP_CREATE_RQ);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	if (!err)
+		*rqn = MLX5_GET(create_rq_out, out, rqn);
+
+	return err;
+}
+
+int mlx5_core_modify_rq(struct mlx5_core_dev *dev, u32 rqn, u32 *in, int inlen)
+{
+	u32 out[MLX5_ST_SZ_DW(modify_rq_out)];
+
+	MLX5_SET(modify_rq_in, in, rqn, rqn);
+	MLX5_SET(modify_rq_in, in, opcode, MLX5_CMD_OP_MODIFY_RQ);
+
+	memset(out, 0, sizeof(out));
+	return mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+}
+
+void mlx5_core_destroy_rq(struct mlx5_core_dev *dev, u32 rqn)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_rq_in)];
+	u32 out[MLX5_ST_SZ_DW(destroy_rq_out)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(destroy_rq_in, in, opcode, MLX5_CMD_OP_DESTROY_RQ);
+	MLX5_SET(destroy_rq_in, in, rqn, rqn);
+
+	mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+}
+
+int mlx5_core_create_sq(struct mlx5_core_dev *dev, u32 *in, int inlen, u32 *sqn)
+{
+	u32 out[MLX5_ST_SZ_DW(create_sq_out)];
+	int err;
+
+	MLX5_SET(create_sq_in, in, opcode, MLX5_CMD_OP_CREATE_SQ);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	if (!err)
+		*sqn = MLX5_GET(create_sq_out, out, sqn);
+
+	return err;
+}
+
+int mlx5_core_modify_sq(struct mlx5_core_dev *dev, u32 sqn, u32 *in, int inlen)
+{
+	u32 out[MLX5_ST_SZ_DW(modify_sq_out)];
+
+	MLX5_SET(modify_sq_in, in, sqn, sqn);
+	MLX5_SET(modify_sq_in, in, opcode, MLX5_CMD_OP_MODIFY_SQ);
+
+	memset(out, 0, sizeof(out));
+	return mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+}
+
+void mlx5_core_destroy_sq(struct mlx5_core_dev *dev, u32 sqn)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_sq_in)];
+	u32 out[MLX5_ST_SZ_DW(destroy_sq_out)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(destroy_sq_in, in, opcode, MLX5_CMD_OP_DESTROY_SQ);
+	MLX5_SET(destroy_sq_in, in, sqn, sqn);
+
+	mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+}
+
+int mlx5_core_create_tir(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			 u32 *tirn)
+{
+	u32 out[MLX5_ST_SZ_DW(create_tir_out)];
+	int err;
+
+	MLX5_SET(create_tir_in, in, opcode, MLX5_CMD_OP_CREATE_TIR);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	if (!err)
+		*tirn = MLX5_GET(create_tir_out, out, tirn);
+
+	return err;
+}
+
+void mlx5_core_destroy_tir(struct mlx5_core_dev *dev, u32 tirn)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_tir_out)];
+	u32 out[MLX5_ST_SZ_DW(destroy_tir_out)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(destroy_tir_in, in, opcode, MLX5_CMD_OP_DESTROY_TIR);
+	MLX5_SET(destroy_tir_in, in, tirn, tirn);
+
+	mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+}
+
+int mlx5_core_create_tis(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			 u32 *tisn)
+{
+	u32 out[MLX5_ST_SZ_DW(create_tis_out)];
+	int err;
+
+	MLX5_SET(create_tis_in, in, opcode, MLX5_CMD_OP_CREATE_TIS);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	if (!err)
+		*tisn = MLX5_GET(create_tis_out, out, tisn);
+
+	return err;
+}
+
+void mlx5_core_destroy_tis(struct mlx5_core_dev *dev, u32 tisn)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_tis_out)];
+	u32 out[MLX5_ST_SZ_DW(destroy_tis_out)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(destroy_tis_in, in, opcode, MLX5_CMD_OP_DESTROY_TIS);
+	MLX5_SET(destroy_tis_in, in, tisn, tisn);
+
+	mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+}
+
+int mlx5_core_create_rmp(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			 u32 *rmpn)
+{
+	u32 out[MLX5_ST_SZ_DW(create_rmp_out)];
+	int err;
+
+	MLX5_SET(create_rmp_in, in, opcode, MLX5_CMD_OP_CREATE_RMP);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	if (!err)
+		*rmpn = MLX5_GET(create_rmp_out, out, rmpn);
+
+	return err;
+}
+
+int mlx5_core_modify_rmp(struct mlx5_core_dev *dev, u32 *in, int inlen)
+{
+	u32 out[MLX5_ST_SZ_DW(modify_rmp_out)];
+
+	MLX5_SET(modify_rmp_in, in, opcode, MLX5_CMD_OP_MODIFY_RMP);
+
+	memset(out, 0, sizeof(out));
+	return mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+}
+
+int mlx5_core_destroy_rmp(struct mlx5_core_dev *dev, u32 rmpn)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_rmp_in)];
+	u32 out[MLX5_ST_SZ_DW(destroy_rmp_out)];
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(destroy_rmp_in, in, opcode, MLX5_CMD_OP_DESTROY_RMP);
+	MLX5_SET(destroy_rmp_in, in, rmpn, rmpn);
+
+	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
+					  sizeof(out));
+}
+
+int mlx5_core_query_rmp(struct mlx5_core_dev *dev, u32 rmpn, u32 *out)
+{
+	u32 in[MLX5_ST_SZ_DW(query_rmp_in)];
+	int outlen = MLX5_ST_SZ_BYTES(query_rmp_out);
+
+	memset(in, 0, sizeof(in));
+	MLX5_SET(query_rmp_in, in, opcode, MLX5_CMD_OP_QUERY_RMP);
+	MLX5_SET(query_rmp_in, in, rmpn,   rmpn);
+
+	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, outlen);
+}
+
+int mlx5_core_arm_rmp(struct mlx5_core_dev *dev, u32 rmpn, u16 lwm)
+{
+	void *in;
+	void *rmpc;
+	void *wq;
+	void *bitmask;
+	int  err;
+
+	in = mlx5_vzalloc(MLX5_ST_SZ_BYTES(modify_rmp_in));
+	if (!in)
+		return -ENOMEM;
+
+	rmpc    = MLX5_ADDR_OF(modify_rmp_in,   in,   ctx);
+	bitmask = MLX5_ADDR_OF(modify_rmp_in,   in,   bitmask);
+	wq      = MLX5_ADDR_OF(rmpc,	        rmpc, wq);
+
+	MLX5_SET(modify_rmp_in, in,	 rmp_state, MLX5_RMPC_STATE_RDY);
+	MLX5_SET(modify_rmp_in, in,	 rmpn,      rmpn);
+	MLX5_SET(wq,		wq,	 lwm,	    lwm);
+	MLX5_SET(rmp_bitmask,	bitmask, lwm,	    1);
+	MLX5_SET(rmpc,		rmpc,	 state,	    MLX5_RMPC_STATE_RDY);
+
+	err =  mlx5_core_modify_rmp(dev, in, MLX5_ST_SZ_BYTES(modify_rmp_in));
+
+	kvfree(in);
+
+	return err;
+}
+
+int mlx5_core_create_xsrq(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			  u32 *xsrqn)
+{
+	u32 out[MLX5_ST_SZ_DW(create_xrc_srq_out)];
+	int err;
+
+	MLX5_SET(create_xrc_srq_in, in, opcode,     MLX5_CMD_OP_CREATE_XRC_SRQ);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
+	if (!err)
+		*xsrqn = MLX5_GET(create_xrc_srq_out, out, xrc_srqn);
+
+	return err;
+}
+
+int mlx5_core_destroy_xsrq(struct mlx5_core_dev *dev, u32 xsrqn)
+{
+	u32 in[MLX5_ST_SZ_DW(destroy_xrc_srq_in)];
+	u32 out[MLX5_ST_SZ_DW(destroy_xrc_srq_out)];
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+
+	MLX5_SET(destroy_xrc_srq_in, in, opcode,   MLX5_CMD_OP_DESTROY_XRC_SRQ);
+	MLX5_SET(destroy_xrc_srq_in, in, xrc_srqn, xsrqn);
+
+	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
+					  sizeof(out));
+}
+
+int mlx5_core_query_xsrq(struct mlx5_core_dev *dev, u32 xsrqn, u32 *out)
+{
+	u32 in[MLX5_ST_SZ_DW(query_xrc_srq_in)];
+	void *srqc;
+	void *xrc_srqc;
+	int err;
+
+	memset(in, 0, sizeof(in));
+	MLX5_SET(query_xrc_srq_in, in, opcode,   MLX5_CMD_OP_QUERY_XRC_SRQ);
+	MLX5_SET(query_xrc_srq_in, in, xrc_srqn, xsrqn);
+
+	err =  mlx5_cmd_exec_check_status(dev, in, sizeof(in),
+					  out,
+					  MLX5_ST_SZ_BYTES(query_xrc_srq_out));
+	if (!err) {
+		xrc_srqc = MLX5_ADDR_OF(query_xrc_srq_out, out,
+					xrc_srq_context_entry);
+		srqc = MLX5_ADDR_OF(query_srq_out, out, srq_context_entry);
+		memcpy(srqc, xrc_srqc, MLX5_ST_SZ_BYTES(srqc));
+	}
+
+	return err;
+}
+
+int mlx5_core_arm_xsrq(struct mlx5_core_dev *dev, u32 xsrqn, u16 lwm)
+{
+	u32 in[MLX5_ST_SZ_DW(arm_xrc_srq_in)];
+	u32 out[MLX5_ST_SZ_DW(arm_xrc_srq_out)];
+
+	memset(in, 0, sizeof(in));
+	memset(out, 0, sizeof(out));
+
+	MLX5_SET(arm_xrc_srq_in, in, opcode,   MLX5_CMD_OP_ARM_XRC_SRQ);
+	MLX5_SET(arm_xrc_srq_in, in, xrc_srqn, xsrqn);
+	MLX5_SET(arm_xrc_srq_in, in, lwm,      lwm);
+	MLX5_SET(arm_xrc_srq_in, in, op_mod,
+		 MLX5_ARM_XRC_SRQ_IN_OP_MOD_XRC_SRQ);
+
+	return  mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
+					   sizeof(out));
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/transobj.h b/drivers/net/ethernet/mellanox/mlx5/core/transobj.h
new file mode 100644
index 000000000000..f9ef244710d5
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/transobj.h
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  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.
+ */
+
+#ifndef __TRANSOBJ_H__
+#define __TRANSOBJ_H__
+
+int mlx5_alloc_transport_domain(struct mlx5_core_dev *dev, u32 *tdn);
+void mlx5_dealloc_transport_domain(struct mlx5_core_dev *dev, u32 tdn);
+int mlx5_core_create_rq(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			u32 *rqn);
+int mlx5_core_modify_rq(struct mlx5_core_dev *dev, u32 rqn, u32 *in, int inlen);
+void mlx5_core_destroy_rq(struct mlx5_core_dev *dev, u32 rqn);
+int mlx5_core_create_sq(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			u32 *sqn);
+int mlx5_core_modify_sq(struct mlx5_core_dev *dev, u32 sqn, u32 *in, int inlen);
+void mlx5_core_destroy_sq(struct mlx5_core_dev *dev, u32 sqn);
+int mlx5_core_create_tir(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			 u32 *tirn);
+void mlx5_core_destroy_tir(struct mlx5_core_dev *dev, u32 tirn);
+int mlx5_core_create_tis(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			 u32 *tisn);
+void mlx5_core_destroy_tis(struct mlx5_core_dev *dev, u32 tisn);
+int mlx5_core_create_rmp(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			 u32 *rmpn);
+int mlx5_core_modify_rmp(struct mlx5_core_dev *dev, u32 *in, int inlen);
+int mlx5_core_destroy_rmp(struct mlx5_core_dev *dev, u32 rmpn);
+int mlx5_core_query_rmp(struct mlx5_core_dev *dev, u32 rmpn, u32 *out);
+int mlx5_core_arm_rmp(struct mlx5_core_dev *dev, u32 rmpn, u16 lwm);
+int mlx5_core_create_xsrq(struct mlx5_core_dev *dev, u32 *in, int inlen,
+			  u32 *rmpn);
+int mlx5_core_destroy_xsrq(struct mlx5_core_dev *dev, u32 rmpn);
+int mlx5_core_query_xsrq(struct mlx5_core_dev *dev, u32 rmpn, u32 *out);
+int mlx5_core_arm_xsrq(struct mlx5_core_dev *dev, u32 rmpn, u16 lwm);
+
+#endif /* __TRANSOBJ_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/uar.c b/drivers/net/ethernet/mellanox/mlx5/core/uar.c
index 5a89bb1d678a..9ef85873ceea 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/uar.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/uar.c
@@ -175,12 +175,13 @@ int mlx5_alloc_uuars(struct mlx5_core_dev *dev, struct mlx5_uuar_info *uuari)
 	for (i = 0; i < tot_uuars; i++) {
 		bf = &uuari->bfs[i];
 
-		bf->buf_size = dev->caps.gen.bf_reg_size / 2;
+		bf->buf_size = (1 << MLX5_CAP_GEN(dev, log_bf_reg_size)) / 2;
 		bf->uar = &uuari->uars[i / MLX5_BF_REGS_PER_PAGE];
 		bf->regreg = uuari->uars[i / MLX5_BF_REGS_PER_PAGE].map;
 		bf->reg = NULL; /* Add WC support */
-		bf->offset = (i % MLX5_BF_REGS_PER_PAGE) * dev->caps.gen.bf_reg_size +
-			MLX5_BF_OFFSET;
+		bf->offset = (i % MLX5_BF_REGS_PER_PAGE) *
+			     (1 << MLX5_CAP_GEN(dev, log_bf_reg_size)) +
+			     MLX5_BF_OFFSET;
 		bf->need_lock = need_uuar_lock(i);
 		spin_lock_init(&bf->lock);
 		spin_lock_init(&bf->lock32);
@@ -223,3 +224,40 @@ int mlx5_free_uuars(struct mlx5_core_dev *dev, struct mlx5_uuar_info *uuari)
 
 	return 0;
 }
+
+int mlx5_alloc_map_uar(struct mlx5_core_dev *mdev, struct mlx5_uar *uar)
+{
+	phys_addr_t pfn;
+	phys_addr_t uar_bar_start;
+	int err;
+
+	err = mlx5_cmd_alloc_uar(mdev, &uar->index);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_cmd_alloc_uar() failed, %d\n", err);
+		return err;
+	}
+
+	uar_bar_start = pci_resource_start(mdev->pdev, 0);
+	pfn           = (uar_bar_start >> PAGE_SHIFT) + uar->index;
+	uar->map      = ioremap(pfn << PAGE_SHIFT, PAGE_SIZE);
+	if (!uar->map) {
+		mlx5_core_warn(mdev, "ioremap() failed, %d\n", err);
+		err = -ENOMEM;
+		goto err_free_uar;
+	}
+
+	return 0;
+
+err_free_uar:
+	mlx5_cmd_free_uar(mdev, uar->index);
+
+	return err;
+}
+EXPORT_SYMBOL(mlx5_alloc_map_uar);
+
+void mlx5_unmap_free_uar(struct mlx5_core_dev *mdev, struct mlx5_uar *uar)
+{
+	iounmap(uar->map);
+	mlx5_cmd_free_uar(mdev, uar->index);
+}
+EXPORT_SYMBOL(mlx5_unmap_free_uar);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/vport.c b/drivers/net/ethernet/mellanox/mlx5/core/vport.c
new file mode 100644
index 000000000000..b94177ebcf3a
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/vport.c
@@ -0,0 +1,345 @@
+/*
+ * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  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/export.h>
+#include <linux/etherdevice.h>
+#include <linux/mlx5/driver.h>
+#include <linux/mlx5/vport.h>
+#include "mlx5_core.h"
+
+u8 mlx5_query_vport_state(struct mlx5_core_dev *mdev, u8 opmod)
+{
+	u32 in[MLX5_ST_SZ_DW(query_vport_state_in)];
+	u32 out[MLX5_ST_SZ_DW(query_vport_state_out)];
+	int err;
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(query_vport_state_in, in, opcode,
+		 MLX5_CMD_OP_QUERY_VPORT_STATE);
+	MLX5_SET(query_vport_state_in, in, op_mod, opmod);
+
+	err = mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out,
+					 sizeof(out));
+	if (err)
+		mlx5_core_warn(mdev, "MLX5_CMD_OP_QUERY_VPORT_STATE failed\n");
+
+	return MLX5_GET(query_vport_state_out, out, state);
+}
+EXPORT_SYMBOL(mlx5_query_vport_state);
+
+void mlx5_query_nic_vport_mac_address(struct mlx5_core_dev *mdev, u8 *addr)
+{
+	u32  in[MLX5_ST_SZ_DW(query_nic_vport_context_in)];
+	u32 *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
+	u8 *out_addr;
+
+	out = mlx5_vzalloc(outlen);
+	if (!out)
+		return;
+
+	out_addr = MLX5_ADDR_OF(query_nic_vport_context_out, out,
+				nic_vport_context.permanent_address);
+
+	memset(in, 0, sizeof(in));
+
+	MLX5_SET(query_nic_vport_context_in, in, opcode,
+		 MLX5_CMD_OP_QUERY_NIC_VPORT_CONTEXT);
+
+	memset(out, 0, outlen);
+	mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out, outlen);
+
+	ether_addr_copy(addr, &out_addr[2]);
+
+	kvfree(out);
+}
+EXPORT_SYMBOL(mlx5_query_nic_vport_mac_address);
+
+int mlx5_query_hca_vport_gid(struct mlx5_core_dev *dev, u8 other_vport,
+			     u8 port_num, u16  vf_num, u16 gid_index,
+			     union ib_gid *gid)
+{
+	int in_sz = MLX5_ST_SZ_BYTES(query_hca_vport_gid_in);
+	int out_sz = MLX5_ST_SZ_BYTES(query_hca_vport_gid_out);
+	int is_group_manager;
+	void *out = NULL;
+	void *in = NULL;
+	union ib_gid *tmp;
+	int tbsz;
+	int nout;
+	int err;
+
+	is_group_manager = MLX5_CAP_GEN(dev, vport_group_manager);
+	tbsz = mlx5_get_gid_table_len(MLX5_CAP_GEN(dev, gid_table_size));
+	mlx5_core_dbg(dev, "vf_num %d, index %d, gid_table_size %d\n",
+		      vf_num, gid_index, tbsz);
+
+	if (gid_index > tbsz && gid_index != 0xffff)
+		return -EINVAL;
+
+	if (gid_index == 0xffff)
+		nout = tbsz;
+	else
+		nout = 1;
+
+	out_sz += nout * sizeof(*gid);
+
+	in = kzalloc(in_sz, GFP_KERNEL);
+	out = kzalloc(out_sz, GFP_KERNEL);
+	if (!in || !out) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	MLX5_SET(query_hca_vport_gid_in, in, opcode, MLX5_CMD_OP_QUERY_HCA_VPORT_GID);
+	if (other_vport) {
+		if (is_group_manager) {
+			MLX5_SET(query_hca_vport_gid_in, in, vport_number, vf_num);
+			MLX5_SET(query_hca_vport_gid_in, in, other_vport, 1);
+		} else {
+			err = -EPERM;
+			goto out;
+		}
+	}
+	MLX5_SET(query_hca_vport_gid_in, in, gid_index, gid_index);
+
+	if (MLX5_CAP_GEN(dev, num_ports) == 2)
+		MLX5_SET(query_hca_vport_gid_in, in, port_num, port_num);
+
+	err = mlx5_cmd_exec(dev, in, in_sz, out, out_sz);
+	if (err)
+		goto out;
+
+	err = mlx5_cmd_status_to_err_v2(out);
+	if (err)
+		goto out;
+
+	tmp = out + MLX5_ST_SZ_BYTES(query_hca_vport_gid_out);
+	gid->global.subnet_prefix = tmp->global.subnet_prefix;
+	gid->global.interface_id = tmp->global.interface_id;
+
+out:
+	kfree(in);
+	kfree(out);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_hca_vport_gid);
+
+int mlx5_query_hca_vport_pkey(struct mlx5_core_dev *dev, u8 other_vport,
+			      u8 port_num, u16 vf_num, u16 pkey_index,
+			      u16 *pkey)
+{
+	int in_sz = MLX5_ST_SZ_BYTES(query_hca_vport_pkey_in);
+	int out_sz = MLX5_ST_SZ_BYTES(query_hca_vport_pkey_out);
+	int is_group_manager;
+	void *out = NULL;
+	void *in = NULL;
+	void *pkarr;
+	int nout;
+	int tbsz;
+	int err;
+	int i;
+
+	is_group_manager = MLX5_CAP_GEN(dev, vport_group_manager);
+
+	tbsz = mlx5_to_sw_pkey_sz(MLX5_CAP_GEN(dev, pkey_table_size));
+	if (pkey_index > tbsz && pkey_index != 0xffff)
+		return -EINVAL;
+
+	if (pkey_index == 0xffff)
+		nout = tbsz;
+	else
+		nout = 1;
+
+	out_sz += nout * MLX5_ST_SZ_BYTES(pkey);
+
+	in = kzalloc(in_sz, GFP_KERNEL);
+	out = kzalloc(out_sz, GFP_KERNEL);
+	if (!in || !out) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	MLX5_SET(query_hca_vport_pkey_in, in, opcode, MLX5_CMD_OP_QUERY_HCA_VPORT_PKEY);
+	if (other_vport) {
+		if (is_group_manager) {
+			MLX5_SET(query_hca_vport_pkey_in, in, vport_number, vf_num);
+			MLX5_SET(query_hca_vport_pkey_in, in, other_vport, 1);
+		} else {
+			err = -EPERM;
+			goto out;
+		}
+	}
+	MLX5_SET(query_hca_vport_pkey_in, in, pkey_index, pkey_index);
+
+	if (MLX5_CAP_GEN(dev, num_ports) == 2)
+		MLX5_SET(query_hca_vport_pkey_in, in, port_num, port_num);
+
+	err = mlx5_cmd_exec(dev, in, in_sz, out, out_sz);
+	if (err)
+		goto out;
+
+	err = mlx5_cmd_status_to_err_v2(out);
+	if (err)
+		goto out;
+
+	pkarr = MLX5_ADDR_OF(query_hca_vport_pkey_out, out, pkey);
+	for (i = 0; i < nout; i++, pkey++, pkarr += MLX5_ST_SZ_BYTES(pkey))
+		*pkey = MLX5_GET_PR(pkey, pkarr, pkey);
+
+out:
+	kfree(in);
+	kfree(out);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_hca_vport_pkey);
+
+int mlx5_query_hca_vport_context(struct mlx5_core_dev *dev,
+				 u8 other_vport, u8 port_num,
+				 u16 vf_num,
+				 struct mlx5_hca_vport_context *rep)
+{
+	int out_sz = MLX5_ST_SZ_BYTES(query_hca_vport_context_out);
+	int in[MLX5_ST_SZ_DW(query_hca_vport_context_in)];
+	int is_group_manager;
+	void *out;
+	void *ctx;
+	int err;
+
+	is_group_manager = MLX5_CAP_GEN(dev, vport_group_manager);
+
+	memset(in, 0, sizeof(in));
+	out = kzalloc(out_sz, GFP_KERNEL);
+	if (!out)
+		return -ENOMEM;
+
+	MLX5_SET(query_hca_vport_context_in, in, opcode, MLX5_CMD_OP_QUERY_HCA_VPORT_CONTEXT);
+
+	if (other_vport) {
+		if (is_group_manager) {
+			MLX5_SET(query_hca_vport_context_in, in, other_vport, 1);
+			MLX5_SET(query_hca_vport_context_in, in, vport_number, vf_num);
+		} else {
+			err = -EPERM;
+			goto ex;
+		}
+	}
+
+	if (MLX5_CAP_GEN(dev, num_ports) == 2)
+		MLX5_SET(query_hca_vport_context_in, in, port_num, port_num);
+
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out,  out_sz);
+	if (err)
+		goto ex;
+	err = mlx5_cmd_status_to_err_v2(out);
+	if (err)
+		goto ex;
+
+	ctx = MLX5_ADDR_OF(query_hca_vport_context_out, out, hca_vport_context);
+	rep->field_select = MLX5_GET_PR(hca_vport_context, ctx, field_select);
+	rep->sm_virt_aware = MLX5_GET_PR(hca_vport_context, ctx, sm_virt_aware);
+	rep->has_smi = MLX5_GET_PR(hca_vport_context, ctx, has_smi);
+	rep->has_raw = MLX5_GET_PR(hca_vport_context, ctx, has_raw);
+	rep->policy = MLX5_GET_PR(hca_vport_context, ctx, vport_state_policy);
+	rep->phys_state = MLX5_GET_PR(hca_vport_context, ctx,
+				      port_physical_state);
+	rep->vport_state = MLX5_GET_PR(hca_vport_context, ctx, vport_state);
+	rep->port_physical_state = MLX5_GET_PR(hca_vport_context, ctx,
+					       port_physical_state);
+	rep->port_guid = MLX5_GET64_PR(hca_vport_context, ctx, port_guid);
+	rep->node_guid = MLX5_GET64_PR(hca_vport_context, ctx, node_guid);
+	rep->cap_mask1 = MLX5_GET_PR(hca_vport_context, ctx, cap_mask1);
+	rep->cap_mask1_perm = MLX5_GET_PR(hca_vport_context, ctx,
+					  cap_mask1_field_select);
+	rep->cap_mask2 = MLX5_GET_PR(hca_vport_context, ctx, cap_mask2);
+	rep->cap_mask2_perm = MLX5_GET_PR(hca_vport_context, ctx,
+					  cap_mask2_field_select);
+	rep->lid = MLX5_GET_PR(hca_vport_context, ctx, lid);
+	rep->init_type_reply = MLX5_GET_PR(hca_vport_context, ctx,
+					   init_type_reply);
+	rep->lmc = MLX5_GET_PR(hca_vport_context, ctx, lmc);
+	rep->subnet_timeout = MLX5_GET_PR(hca_vport_context, ctx,
+					  subnet_timeout);
+	rep->sm_lid = MLX5_GET_PR(hca_vport_context, ctx, sm_lid);
+	rep->sm_sl = MLX5_GET_PR(hca_vport_context, ctx, sm_sl);
+	rep->qkey_violation_counter = MLX5_GET_PR(hca_vport_context, ctx,
+						  qkey_violation_counter);
+	rep->pkey_violation_counter = MLX5_GET_PR(hca_vport_context, ctx,
+						  pkey_violation_counter);
+	rep->grh_required = MLX5_GET_PR(hca_vport_context, ctx, grh_required);
+	rep->sys_image_guid = MLX5_GET64_PR(hca_vport_context, ctx,
+					    system_image_guid);
+
+ex:
+	kfree(out);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_hca_vport_context);
+
+int mlx5_query_hca_vport_system_image_guid(struct mlx5_core_dev *dev,
+					   u64 *sys_image_guid)
+{
+	struct mlx5_hca_vport_context *rep;
+	int err;
+
+	rep = kzalloc(sizeof(*rep), GFP_KERNEL);
+	if (!rep)
+		return -ENOMEM;
+
+	err = mlx5_query_hca_vport_context(dev, 0, 1, 0, rep);
+	if (!err)
+		*sys_image_guid = rep->sys_image_guid;
+
+	kfree(rep);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_hca_vport_system_image_guid);
+
+int mlx5_query_hca_vport_node_guid(struct mlx5_core_dev *dev,
+				   u64 *node_guid)
+{
+	struct mlx5_hca_vport_context *rep;
+	int err;
+
+	rep = kzalloc(sizeof(*rep), GFP_KERNEL);
+	if (!rep)
+		return -ENOMEM;
+
+	err = mlx5_query_hca_vport_context(dev, 0, 1, 0, rep);
+	if (!err)
+		*node_guid = rep->node_guid;
+
+	kfree(rep);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_hca_vport_node_guid);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/wq.c b/drivers/net/ethernet/mellanox/mlx5/core/wq.c
new file mode 100644
index 000000000000..8388411582cf
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/wq.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  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/mlx5/driver.h>
+#include "wq.h"
+#include "mlx5_core.h"
+
+u32 mlx5_wq_cyc_get_size(struct mlx5_wq_cyc *wq)
+{
+	return (u32)wq->sz_m1 + 1;
+}
+
+u32 mlx5_cqwq_get_size(struct mlx5_cqwq *wq)
+{
+	return wq->sz_m1 + 1;
+}
+
+u32 mlx5_wq_ll_get_size(struct mlx5_wq_ll *wq)
+{
+	return (u32)wq->sz_m1 + 1;
+}
+
+static u32 mlx5_wq_cyc_get_byte_size(struct mlx5_wq_cyc *wq)
+{
+	return mlx5_wq_cyc_get_size(wq) << wq->log_stride;
+}
+
+static u32 mlx5_cqwq_get_byte_size(struct mlx5_cqwq *wq)
+{
+	return mlx5_cqwq_get_size(wq) << wq->log_stride;
+}
+
+static u32 mlx5_wq_ll_get_byte_size(struct mlx5_wq_ll *wq)
+{
+	return mlx5_wq_ll_get_size(wq) << wq->log_stride;
+}
+
+int mlx5_wq_cyc_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
+		       void *wqc, struct mlx5_wq_cyc *wq,
+		       struct mlx5_wq_ctrl *wq_ctrl)
+{
+	int err;
+
+	wq->log_stride = MLX5_GET(wq, wqc, log_wq_stride);
+	wq->sz_m1 = (1 << MLX5_GET(wq, wqc, log_wq_sz)) - 1;
+
+	err = mlx5_db_alloc(mdev, &wq_ctrl->db);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_db_alloc() failed, %d\n", err);
+		return err;
+	}
+
+	err = mlx5_buf_alloc(mdev, mlx5_wq_cyc_get_byte_size(wq), &wq_ctrl->buf);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_buf_alloc() failed, %d\n", err);
+		goto err_db_free;
+	}
+
+	wq->buf = wq_ctrl->buf.direct.buf;
+	wq->db  = wq_ctrl->db.db;
+
+	wq_ctrl->mdev = mdev;
+
+	return 0;
+
+err_db_free:
+	mlx5_db_free(mdev, &wq_ctrl->db);
+
+	return err;
+}
+
+int mlx5_cqwq_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
+		     void *cqc, struct mlx5_cqwq *wq,
+		     struct mlx5_wq_ctrl *wq_ctrl)
+{
+	int err;
+
+	wq->log_stride = 6 + MLX5_GET(cqc, cqc, cqe_sz);
+	wq->log_sz = MLX5_GET(cqc, cqc, log_cq_size);
+	wq->sz_m1 = (1 << wq->log_sz) - 1;
+
+	err = mlx5_db_alloc(mdev, &wq_ctrl->db);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_db_alloc() failed, %d\n", err);
+		return err;
+	}
+
+	err = mlx5_buf_alloc(mdev, mlx5_cqwq_get_byte_size(wq), &wq_ctrl->buf);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_buf_alloc() failed, %d\n", err);
+		goto err_db_free;
+	}
+
+	wq->buf = wq_ctrl->buf.direct.buf;
+	wq->db  = wq_ctrl->db.db;
+
+	wq_ctrl->mdev = mdev;
+
+	return 0;
+
+err_db_free:
+	mlx5_db_free(mdev, &wq_ctrl->db);
+
+	return err;
+}
+
+int mlx5_wq_ll_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
+		      void *wqc, struct mlx5_wq_ll *wq,
+		      struct mlx5_wq_ctrl *wq_ctrl)
+{
+	struct mlx5_wqe_srq_next_seg *next_seg;
+	int err;
+	int i;
+
+	wq->log_stride = MLX5_GET(wq, wqc, log_wq_stride);
+	wq->sz_m1 = (1 << MLX5_GET(wq, wqc, log_wq_sz)) - 1;
+
+	err = mlx5_db_alloc(mdev, &wq_ctrl->db);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_db_alloc() failed, %d\n", err);
+		return err;
+	}
+
+	err = mlx5_buf_alloc(mdev, mlx5_wq_ll_get_byte_size(wq), &wq_ctrl->buf);
+	if (err) {
+		mlx5_core_warn(mdev, "mlx5_buf_alloc() failed, %d\n", err);
+		goto err_db_free;
+	}
+
+	wq->buf = wq_ctrl->buf.direct.buf;
+	wq->db  = wq_ctrl->db.db;
+
+	for (i = 0; i < wq->sz_m1; i++) {
+		next_seg = mlx5_wq_ll_get_wqe(wq, i);
+		next_seg->next_wqe_index = cpu_to_be16(i + 1);
+	}
+	next_seg = mlx5_wq_ll_get_wqe(wq, i);
+	wq->tail_next = &next_seg->next_wqe_index;
+
+	wq_ctrl->mdev = mdev;
+
+	return 0;
+
+err_db_free:
+	mlx5_db_free(mdev, &wq_ctrl->db);
+
+	return err;
+}
+
+void mlx5_wq_destroy(struct mlx5_wq_ctrl *wq_ctrl)
+{
+	mlx5_buf_free(wq_ctrl->mdev, &wq_ctrl->buf);
+	mlx5_db_free(wq_ctrl->mdev, &wq_ctrl->db);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/wq.h b/drivers/net/ethernet/mellanox/mlx5/core/wq.h
new file mode 100644
index 000000000000..e0ddd69fb429
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/wq.h
@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  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.
+ */
+
+#ifndef __MLX5_WQ_H__
+#define __MLX5_WQ_H__
+
+#include <linux/mlx5/mlx5_ifc.h>
+
+struct mlx5_wq_param {
+	int		linear;
+	int		numa;
+};
+
+struct mlx5_wq_ctrl {
+	struct mlx5_core_dev	*mdev;
+	struct mlx5_buf		buf;
+	struct mlx5_db		db;
+};
+
+struct mlx5_wq_cyc {
+	void			*buf;
+	__be32			*db;
+	u16			sz_m1;
+	u8			log_stride;
+};
+
+struct mlx5_cqwq {
+	void			*buf;
+	__be32			*db;
+	u32			sz_m1;
+	u32			cc; /* consumer counter */
+	u8			log_sz;
+	u8			log_stride;
+};
+
+struct mlx5_wq_ll {
+	void			*buf;
+	__be32			*db;
+	__be16			*tail_next;
+	u16			sz_m1;
+	u16			head;
+	u16			wqe_ctr;
+	u16			cur_sz;
+	u8			log_stride;
+};
+
+int mlx5_wq_cyc_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
+		       void *wqc, struct mlx5_wq_cyc *wq,
+		       struct mlx5_wq_ctrl *wq_ctrl);
+u32 mlx5_wq_cyc_get_size(struct mlx5_wq_cyc *wq);
+
+int mlx5_cqwq_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
+		     void *cqc, struct mlx5_cqwq *wq,
+		     struct mlx5_wq_ctrl *wq_ctrl);
+u32 mlx5_cqwq_get_size(struct mlx5_cqwq *wq);
+
+int mlx5_wq_ll_create(struct mlx5_core_dev *mdev, struct mlx5_wq_param *param,
+		      void *wqc, struct mlx5_wq_ll *wq,
+		      struct mlx5_wq_ctrl *wq_ctrl);
+u32 mlx5_wq_ll_get_size(struct mlx5_wq_ll *wq);
+
+void mlx5_wq_destroy(struct mlx5_wq_ctrl *wq_ctrl);
+
+static inline u16 mlx5_wq_cyc_ctr2ix(struct mlx5_wq_cyc *wq, u16 ctr)
+{
+	return ctr & wq->sz_m1;
+}
+
+static inline void *mlx5_wq_cyc_get_wqe(struct mlx5_wq_cyc *wq, u16 ix)
+{
+	return wq->buf + (ix << wq->log_stride);
+}
+
+static inline int mlx5_wq_cyc_cc_bigger(u16 cc1, u16 cc2)
+{
+	int equal   = (cc1 == cc2);
+	int smaller = 0x8000 & (cc1 - cc2);
+
+	return !equal && !smaller;
+}
+
+static inline u32 mlx5_cqwq_get_ci(struct mlx5_cqwq *wq)
+{
+	return wq->cc & wq->sz_m1;
+}
+
+static inline void *mlx5_cqwq_get_wqe(struct mlx5_cqwq *wq, u32 ix)
+{
+	return wq->buf + (ix << wq->log_stride);
+}
+
+static inline u32 mlx5_cqwq_get_wrap_cnt(struct mlx5_cqwq *wq)
+{
+	return wq->cc >> wq->log_sz;
+}
+
+static inline void mlx5_cqwq_pop(struct mlx5_cqwq *wq)
+{
+	wq->cc++;
+}
+
+static inline void mlx5_cqwq_update_db_record(struct mlx5_cqwq *wq)
+{
+	*wq->db = cpu_to_be32(wq->cc & 0xffffff);
+}
+
+static inline int mlx5_wq_ll_is_full(struct mlx5_wq_ll *wq)
+{
+	return wq->cur_sz == wq->sz_m1;
+}
+
+static inline int mlx5_wq_ll_is_empty(struct mlx5_wq_ll *wq)
+{
+	return !wq->cur_sz;
+}
+
+static inline void *mlx5_wq_ll_get_wqe(struct mlx5_wq_ll *wq, u16 ix)
+{
+	return wq->buf + (ix << wq->log_stride);
+}
+
+static inline void mlx5_wq_ll_push(struct mlx5_wq_ll *wq, u16 head_next)
+{
+	wq->head = head_next;
+	wq->wqe_ctr++;
+	wq->cur_sz++;
+}
+
+static inline void mlx5_wq_ll_pop(struct mlx5_wq_ll *wq, __be16 ix,
+				  __be16 *next_tail_next)
+{
+	*wq->tail_next = ix;
+	wq->tail_next = next_tail_next;
+	wq->cur_sz--;
+}
+
+static inline void mlx5_wq_ll_update_db_record(struct mlx5_wq_ll *wq)
+{
+	*wq->db = cpu_to_be32(wq->wqe_ctr);
+}
+
+#endif /* __MLX5_WQ_H__ */
diff --git a/drivers/net/ethernet/micrel/Kconfig b/drivers/net/ethernet/micrel/Kconfig
index d16b11ed2e52..b7e2f49696b7 100644
--- a/drivers/net/ethernet/micrel/Kconfig
+++ b/drivers/net/ethernet/micrel/Kconfig
@@ -8,9 +8,7 @@ config NET_VENDOR_MICREL
 	depends on (HAS_IOMEM && DMA_ENGINE) || SPI || PCI || HAS_IOMEM || \
 		   (ARM && ARCH_KS8695)
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/micrel/ksz884x.c b/drivers/net/ethernet/micrel/ksz884x.c
index 6f332ebdf3b5..75dc46c5fca2 100644
--- a/drivers/net/ethernet/micrel/ksz884x.c
+++ b/drivers/net/ethernet/micrel/ksz884x.c
@@ -6664,7 +6664,7 @@ static void mib_read_work(struct work_struct *work)
 				wake_up_interruptible(
 					&hw_priv->counter[i].counter);
 			}
-		} else if (jiffies >= hw_priv->counter[i].time) {
+		} else if (time_after_eq(jiffies, hw_priv->counter[i].time)) {
 			/* Only read MIB counters when the port is connected. */
 			if (media_connected == mib->state)
 				hw_priv->counter[i].read = 1;
@@ -6689,7 +6689,7 @@ static void mib_monitor(unsigned long ptr)
 
 	/* This is used to verify Wake-on-LAN is working. */
 	if (hw_priv->pme_wait) {
-		if (hw_priv->pme_wait <= jiffies) {
+		if (time_is_before_eq_jiffies(hw_priv->pme_wait)) {
 			hw_clr_wol_pme_status(&hw_priv->hw);
 			hw_priv->pme_wait = 0;
 		}
diff --git a/drivers/net/ethernet/microchip/Kconfig b/drivers/net/ethernet/microchip/Kconfig
index afaf0c07f37f..3fd8ca6d4e7c 100644
--- a/drivers/net/ethernet/microchip/Kconfig
+++ b/drivers/net/ethernet/microchip/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_MICROCHIP
 	default y
 	depends on SPI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/moxa/Kconfig b/drivers/net/ethernet/moxa/Kconfig
index 1731e050fa27..5b531da36933 100644
--- a/drivers/net/ethernet/moxa/Kconfig
+++ b/drivers/net/ethernet/moxa/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_MOXART
 	default y
 	depends on (ARM && ARCH_MOXART)
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/moxa/moxart_ether.c b/drivers/net/ethernet/moxa/moxart_ether.c
index 81d0f1c86d6d..becbb5f1f5a7 100644
--- a/drivers/net/ethernet/moxa/moxart_ether.c
+++ b/drivers/net/ethernet/moxa/moxart_ether.c
@@ -244,7 +244,6 @@ static int moxart_rx_poll(struct napi_struct *napi, int budget)
 		napi_gro_receive(&priv->napi, skb);
 		rx++;
 
-		ndev->last_rx = jiffies;
 		priv->stats.rx_packets++;
 		priv->stats.rx_bytes += len;
 		if (desc0 & RX_DESC0_MULTICAST)
diff --git a/drivers/net/ethernet/myricom/Kconfig b/drivers/net/ethernet/myricom/Kconfig
index 3932d081fa21..9645c7245bbf 100644
--- a/drivers/net/ethernet/myricom/Kconfig
+++ b/drivers/net/ethernet/myricom/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_MYRI
 	default y
 	depends on PCI && INET
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say
-	  Y and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
index 2bae50292dcd..83651ac8ddb9 100644
--- a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
+++ b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
@@ -279,7 +279,7 @@ MODULE_FIRMWARE("myri10ge_eth_z8e.dat");
 MODULE_FIRMWARE("myri10ge_rss_ethp_z8e.dat");
 MODULE_FIRMWARE("myri10ge_rss_eth_z8e.dat");
 
-/* Careful: must be accessed under kparam_block_sysfs_write */
+/* Careful: must be accessed under kernel_param_lock() */
 static char *myri10ge_fw_name = NULL;
 module_param(myri10ge_fw_name, charp, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(myri10ge_fw_name, "Firmware image name");
@@ -3427,7 +3427,7 @@ static void myri10ge_select_firmware(struct myri10ge_priv *mgp)
 		}
 	}
 
-	kparam_block_sysfs_write(myri10ge_fw_name);
+	kernel_param_lock(THIS_MODULE);
 	if (myri10ge_fw_name != NULL) {
 		char *fw_name = kstrdup(myri10ge_fw_name, GFP_KERNEL);
 		if (fw_name) {
@@ -3435,7 +3435,7 @@ static void myri10ge_select_firmware(struct myri10ge_priv *mgp)
 			set_fw_name(mgp, fw_name, true);
 		}
 	}
-	kparam_unblock_sysfs_write(myri10ge_fw_name);
+	kernel_param_unlock(THIS_MODULE);
 
 	if (mgp->board_number < MYRI10GE_MAX_BOARDS &&
 	    myri10ge_fw_names[mgp->board_number] != NULL &&
diff --git a/drivers/net/ethernet/natsemi/Kconfig b/drivers/net/ethernet/natsemi/Kconfig
index a100860d45e6..a10ef50e4f12 100644
--- a/drivers/net/ethernet/natsemi/Kconfig
+++ b/drivers/net/ethernet/natsemi/Kconfig
@@ -6,9 +6,7 @@ config NET_VENDOR_NATSEMI
 	bool "National Semi-conductor devices"
 	default y
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -24,8 +22,7 @@ config MACSONIC
 	  Support for NatSemi SONIC based Ethernet devices.  This includes
 	  the onboard Ethernet in many Quadras as well as some LC-PDS,
 	  a few Nubus and all known Comm Slot Ethernet cards.  If you have
-	  one of these say Y and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  one of these say Y here.
 
 	  To compile this driver as a module, choose M here. This module will
 	  be called macsonic.
diff --git a/drivers/net/ethernet/neterion/Kconfig b/drivers/net/ethernet/neterion/Kconfig
index 87abb4f10c43..71899009c468 100644
--- a/drivers/net/ethernet/neterion/Kconfig
+++ b/drivers/net/ethernet/neterion/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_EXAR
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say
-	  Y and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/neterion/s2io.c b/drivers/net/ethernet/neterion/s2io.c
index 1e0f72b65459..c28111749e1f 100644
--- a/drivers/net/ethernet/neterion/s2io.c
+++ b/drivers/net/ethernet/neterion/s2io.c
@@ -5308,7 +5308,8 @@ static int do_s2io_prog_unicast(struct net_device *dev, u8 *addr)
 
 /**
  * s2io_ethtool_sset - Sets different link parameters.
- * @sp : private member of the device structure, which is a pointer to the  * s2io_nic structure.
+ * @sp : private member of the device structure, which is a pointer to the
+ * s2io_nic structure.
  * @info: pointer to the structure with parameters given by ethtool to set
  * link information.
  * Description:
@@ -5793,7 +5794,8 @@ static void s2io_vpd_read(struct s2io_nic *nic)
 
 /**
  *  s2io_ethtool_geeprom  - reads the value stored in the Eeprom.
- *  @sp : private member of the device structure, which is a pointer to the *       s2io_nic structure.
+ *  @sp : private member of the device structure, which is a pointer to the
+ *  s2io_nic structure.
  *  @eeprom : pointer to the user level structure provided by ethtool,
  *  containing all relevant information.
  *  @data_buf : user defined value to be written into Eeprom.
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-traffic.c b/drivers/net/ethernet/neterion/vxge/vxge-traffic.c
index 9e1aaa7f36bb..5f630a24e491 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-traffic.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-traffic.c
@@ -1004,8 +1004,6 @@ void vxge_hw_device_clear_tx_rx(struct __vxge_hw_device *hldev)
 static enum vxge_hw_status
 vxge_hw_channel_dtr_alloc(struct __vxge_hw_channel *channel, void **dtrh)
 {
-	void **tmp_arr;
-
 	if (channel->reserve_ptr - channel->reserve_top > 0) {
 _alloc_after_swap:
 		*dtrh =	channel->reserve_arr[--channel->reserve_ptr];
@@ -1020,10 +1018,7 @@ _alloc_after_swap:
 	 * i.e.	no additional lock need	to be done when	we free	a resource */
 
 	if (channel->length - channel->free_ptr > 0) {
-
-		tmp_arr	= channel->reserve_arr;
-		channel->reserve_arr = channel->free_arr;
-		channel->free_arr = tmp_arr;
+		swap(channel->reserve_arr, channel->free_arr);
 		channel->reserve_ptr = channel->length;
 		channel->reserve_top = channel->free_ptr;
 		channel->free_ptr = channel->length;
diff --git a/drivers/net/ethernet/nuvoton/Kconfig b/drivers/net/ethernet/nuvoton/Kconfig
index 01182b559473..71c973f8e50f 100644
--- a/drivers/net/ethernet/nuvoton/Kconfig
+++ b/drivers/net/ethernet/nuvoton/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_NUVOTON
 	default y
 	depends on ARM && ARCH_W90X900
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/nvidia/Kconfig b/drivers/net/ethernet/nvidia/Kconfig
index ace19e7f6d13..4efc9fe84785 100644
--- a/drivers/net/ethernet/nvidia/Kconfig
+++ b/drivers/net/ethernet/nvidia/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_NVIDIA
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -22,9 +20,7 @@ config FORCEDETH
 	tristate "nForce Ethernet support"
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) controller of this type, say Y and
-	  read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) controller of this type, say Y here.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called forcedeth.
diff --git a/drivers/net/ethernet/oki-semi/Kconfig b/drivers/net/ethernet/oki-semi/Kconfig
index ecd45f9ea9d9..5a975af4824b 100644
--- a/drivers/net/ethernet/oki-semi/Kconfig
+++ b/drivers/net/ethernet/oki-semi/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_OKI
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/packetengines/Kconfig b/drivers/net/ethernet/packetengines/Kconfig
index 8d5180043c70..b5ea2a56106e 100644
--- a/drivers/net/ethernet/packetengines/Kconfig
+++ b/drivers/net/ethernet/packetengines/Kconfig
@@ -7,9 +7,7 @@ config NET_PACKET_ENGINE
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -23,9 +21,7 @@ config HAMACHI
 	depends on PCI
 	select MII
 	---help---
-	  If you have a Gigabit Ethernet card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a Gigabit Ethernet card of this type, say Y here.
 
 	  To compile this driver as a module, choose M here. The module will be
 	  called hamachi.
diff --git a/drivers/net/ethernet/pasemi/Kconfig b/drivers/net/ethernet/pasemi/Kconfig
index 01e6c329d78c..db19c6f49859 100644
--- a/drivers/net/ethernet/pasemi/Kconfig
+++ b/drivers/net/ethernet/pasemi/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_PASEMI
 	default y
 	depends on PPC_PASEMI && PCI && INET
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/qlogic/Kconfig b/drivers/net/ethernet/qlogic/Kconfig
index d49cba129081..f1f0108c275d 100644
--- a/drivers/net/ethernet/qlogic/Kconfig
+++ b/drivers/net/ethernet/qlogic/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_QLOGIC
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index f221126a5c4e..055f3763e577 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -1326,9 +1326,6 @@ struct qlcnic_eswitch {
 };
 
 
-/* Return codes for Error handling */
-#define QL_STATUS_INVALID_PARAM	-1
-
 #define MAX_BW			100	/* % of link speed */
 #define MIN_BW			1	/* % of link speed */
 #define MAX_VLAN_ID		4095
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index 367f3976df56..2f6cc423ab1d 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -1031,7 +1031,7 @@ int qlcnic_init_pci_info(struct qlcnic_adapter *adapter)
 		pfn = pci_info[i].id;
 
 		if (pfn >= ahw->max_vnic_func) {
-			ret = QL_STATUS_INVALID_PARAM;
+			ret = -EINVAL;
 			dev_err(&adapter->pdev->dev, "%s: Invalid function 0x%x, max 0x%x\n",
 				__func__, pfn, ahw->max_vnic_func);
 			goto err_eswitch;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
index 59a721fba018..05c28f2c6df7 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
@@ -24,8 +24,6 @@
 #include <linux/hwmon-sysfs.h>
 #endif
 
-#define QLC_STATUS_UNSUPPORTED_CMD	-2
-
 int qlcnicvf_config_bridged_mode(struct qlcnic_adapter *adapter, u32 enable)
 {
 	return -EOPNOTSUPP;
@@ -166,7 +164,7 @@ static int qlcnic_82xx_store_beacon(struct qlcnic_adapter *adapter,
 	u8 b_state, b_rate;
 
 	if (len != sizeof(u16))
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	memcpy(&beacon, buf, sizeof(u16));
 	err = qlcnic_validate_beacon(adapter, beacon, &b_state, &b_rate);
@@ -383,17 +381,17 @@ static int validate_pm_config(struct qlcnic_adapter *adapter,
 		dest_pci_func = pm_cfg[i].dest_npar;
 		src_index = qlcnic_is_valid_nic_func(adapter, src_pci_func);
 		if (src_index < 0)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 
 		dest_index = qlcnic_is_valid_nic_func(adapter, dest_pci_func);
 		if (dest_index < 0)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 
 		s_esw_id = adapter->npars[src_index].phy_port;
 		d_esw_id = adapter->npars[dest_index].phy_port;
 
 		if (s_esw_id != d_esw_id)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 	}
 
 	return 0;
@@ -414,7 +412,7 @@ static ssize_t qlcnic_sysfs_write_pm_config(struct file *filp,
 	count	= size / sizeof(struct qlcnic_pm_func_cfg);
 	rem	= size % sizeof(struct qlcnic_pm_func_cfg);
 	if (rem)
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	pm_cfg = (struct qlcnic_pm_func_cfg *)buf;
@@ -427,7 +425,7 @@ static ssize_t qlcnic_sysfs_write_pm_config(struct file *filp,
 		action = !!pm_cfg[i].action;
 		index = qlcnic_is_valid_nic_func(adapter, pci_func);
 		if (index < 0)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 
 		id = adapter->npars[index].phy_port;
 		ret = qlcnic_config_port_mirroring(adapter, id,
@@ -440,7 +438,7 @@ static ssize_t qlcnic_sysfs_write_pm_config(struct file *filp,
 		pci_func = pm_cfg[i].pci_func;
 		index = qlcnic_is_valid_nic_func(adapter, pci_func);
 		if (index < 0)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 		id = adapter->npars[index].phy_port;
 		adapter->npars[index].enable_pm = !!pm_cfg[i].action;
 		adapter->npars[index].dest_npar = id;
@@ -499,11 +497,11 @@ static int validate_esw_config(struct qlcnic_adapter *adapter,
 	for (i = 0; i < count; i++) {
 		pci_func = esw_cfg[i].pci_func;
 		if (pci_func >= ahw->max_vnic_func)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 
 		if (adapter->ahw->op_mode == QLCNIC_MGMT_FUNC)
 			if (qlcnic_is_valid_nic_func(adapter, pci_func) < 0)
-				return QL_STATUS_INVALID_PARAM;
+				return -EINVAL;
 
 		switch (esw_cfg[i].op_mode) {
 		case QLCNIC_PORT_DEFAULTS:
@@ -517,25 +515,25 @@ static int validate_esw_config(struct qlcnic_adapter *adapter,
 
 			if (ret != QLCNIC_NON_PRIV_FUNC) {
 				if (esw_cfg[i].mac_anti_spoof != 0)
-					return QL_STATUS_INVALID_PARAM;
+					return -EINVAL;
 				if (esw_cfg[i].mac_override != 1)
-					return QL_STATUS_INVALID_PARAM;
+					return -EINVAL;
 				if (esw_cfg[i].promisc_mode != 1)
-					return QL_STATUS_INVALID_PARAM;
+					return -EINVAL;
 			}
 			break;
 		case QLCNIC_ADD_VLAN:
 			if (!IS_VALID_VLAN(esw_cfg[i].vlan_id))
-				return QL_STATUS_INVALID_PARAM;
+				return -EINVAL;
 			if (!esw_cfg[i].op_type)
-				return QL_STATUS_INVALID_PARAM;
+				return -EINVAL;
 			break;
 		case QLCNIC_DEL_VLAN:
 			if (!esw_cfg[i].op_type)
-				return QL_STATUS_INVALID_PARAM;
+				return -EINVAL;
 			break;
 		default:
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 		}
 	}
 
@@ -559,7 +557,7 @@ static ssize_t qlcnic_sysfs_write_esw_config(struct file *file,
 	count	= size / sizeof(struct qlcnic_esw_func_cfg);
 	rem	= size % sizeof(struct qlcnic_esw_func_cfg);
 	if (rem)
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	esw_cfg = (struct qlcnic_esw_func_cfg *)buf;
@@ -570,7 +568,7 @@ static ssize_t qlcnic_sysfs_write_esw_config(struct file *file,
 	for (i = 0; i < count; i++) {
 		if (adapter->ahw->op_mode == QLCNIC_MGMT_FUNC)
 			if (qlcnic_config_switch_port(adapter, &esw_cfg[i]))
-				return QL_STATUS_INVALID_PARAM;
+				return -EINVAL;
 
 		if (adapter->ahw->pci_func != esw_cfg[i].pci_func)
 			continue;
@@ -604,7 +602,7 @@ static ssize_t qlcnic_sysfs_write_esw_config(struct file *file,
 		pci_func = esw_cfg[i].pci_func;
 		index = qlcnic_is_valid_nic_func(adapter, pci_func);
 		if (index < 0)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 		npar = &adapter->npars[index];
 		switch (esw_cfg[i].op_mode) {
 		case QLCNIC_PORT_DEFAULTS:
@@ -654,7 +652,7 @@ static ssize_t qlcnic_sysfs_read_esw_config(struct file *file,
 
 		esw_cfg[pci_func].pci_func = pci_func;
 		if (qlcnic_get_eswitch_port_config(adapter, &esw_cfg[pci_func]))
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 	}
 	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	return size;
@@ -669,11 +667,11 @@ static int validate_npar_config(struct qlcnic_adapter *adapter,
 	for (i = 0; i < count; i++) {
 		pci_func = np_cfg[i].pci_func;
 		if (qlcnic_is_valid_nic_func(adapter, pci_func) < 0)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 
 		if (!IS_VALID_BW(np_cfg[i].min_bw) ||
 		    !IS_VALID_BW(np_cfg[i].max_bw))
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 	}
 	return 0;
 }
@@ -694,7 +692,7 @@ static ssize_t qlcnic_sysfs_write_npar_config(struct file *file,
 	count	= size / sizeof(struct qlcnic_npar_func_cfg);
 	rem	= size % sizeof(struct qlcnic_npar_func_cfg);
 	if (rem)
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	np_cfg = (struct qlcnic_npar_func_cfg *)buf;
@@ -717,7 +715,7 @@ static ssize_t qlcnic_sysfs_write_npar_config(struct file *file,
 			return ret;
 		index = qlcnic_is_valid_nic_func(adapter, pci_func);
 		if (index < 0)
-			return QL_STATUS_INVALID_PARAM;
+			return -EINVAL;
 		adapter->npars[index].min_bw = nic_info.min_tx_bw;
 		adapter->npars[index].max_bw = nic_info.max_tx_bw;
 	}
@@ -784,13 +782,13 @@ static ssize_t qlcnic_sysfs_get_port_stats(struct file *file,
 	int ret;
 
 	if (qlcnic_83xx_check(adapter))
-		return QLC_STATUS_UNSUPPORTED_CMD;
+		return -EOPNOTSUPP;
 
 	if (size != sizeof(struct qlcnic_esw_statistics))
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	if (offset >= adapter->ahw->max_vnic_func)
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	memset(&port_stats, 0, size);
 	ret = qlcnic_get_port_stats(adapter, offset, QLCNIC_QUERY_RX_COUNTER,
@@ -819,13 +817,13 @@ static ssize_t qlcnic_sysfs_get_esw_stats(struct file *file,
 	int ret;
 
 	if (qlcnic_83xx_check(adapter))
-		return QLC_STATUS_UNSUPPORTED_CMD;
+		return -EOPNOTSUPP;
 
 	if (size != sizeof(struct qlcnic_esw_statistics))
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	if (offset >= QLCNIC_NIU_MAX_XG_PORTS)
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	memset(&esw_stats, 0, size);
 	ret = qlcnic_get_eswitch_stats(adapter, offset, QLCNIC_QUERY_RX_COUNTER,
@@ -853,10 +851,10 @@ static ssize_t qlcnic_sysfs_clear_esw_stats(struct file *file,
 	int ret;
 
 	if (qlcnic_83xx_check(adapter))
-		return QLC_STATUS_UNSUPPORTED_CMD;
+		return -EOPNOTSUPP;
 
 	if (offset >= QLCNIC_NIU_MAX_XG_PORTS)
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	ret = qlcnic_clear_esw_stats(adapter, QLCNIC_STATS_ESWITCH, offset,
 				     QLCNIC_QUERY_RX_COUNTER);
@@ -883,10 +881,10 @@ static ssize_t qlcnic_sysfs_clear_port_stats(struct file *file,
 	int ret;
 
 	if (qlcnic_83xx_check(adapter))
-		return QLC_STATUS_UNSUPPORTED_CMD;
+		return -EOPNOTSUPP;
 
 	if (offset >= adapter->ahw->max_vnic_func)
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	ret = qlcnic_clear_esw_stats(adapter, QLCNIC_STATS_PORT, offset,
 				     QLCNIC_QUERY_RX_COUNTER);
@@ -953,9 +951,7 @@ static ssize_t qlcnic_83xx_sysfs_flash_read_handler(struct file *filp,
 	struct qlcnic_adapter *adapter = dev_get_drvdata(dev);
 
 	if (!size)
-		return QL_STATUS_INVALID_PARAM;
-	if (!buf)
-		return QL_STATUS_INVALID_PARAM;
+		return -EINVAL;
 
 	count = size / sizeof(u32);
 
@@ -1132,9 +1128,6 @@ static ssize_t qlcnic_83xx_sysfs_flash_write_handler(struct file *filp,
 	struct device *dev = container_of(kobj, struct device, kobj);
 	struct qlcnic_adapter *adapter = dev_get_drvdata(dev);
 
-	if (!buf)
-		return QL_STATUS_INVALID_PARAM;
-
 	ret = kstrtoul(buf, 16, &data);
 
 	switch (data) {
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index 25800a1dedcb..02b7115b6aaa 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -3871,9 +3871,6 @@ static int ql_adapter_reset(struct ql_adapter *qdev)
 		return status;
 	}
 
-	end_jiffies = jiffies +
-		max((unsigned long)1, usecs_to_jiffies(30));
-
 	/* Check if bit is set then skip the mailbox command and
 	 * clear the bit, else we are in normal reset process.
 	 */
@@ -3888,6 +3885,7 @@ static int ql_adapter_reset(struct ql_adapter *qdev)
 
 	ql_write32(qdev, RST_FO, (RST_FO_FR << 16) | RST_FO_FR);
 
+	end_jiffies = jiffies + usecs_to_jiffies(30);
 	do {
 		value = ql_read32(qdev, RST_FO);
 		if ((value & RST_FO_FR) == 0)
diff --git a/drivers/net/ethernet/qualcomm/Kconfig b/drivers/net/ethernet/qualcomm/Kconfig
index 9a49f42ac2ba..a76e380cf89a 100644
--- a/drivers/net/ethernet/qualcomm/Kconfig
+++ b/drivers/net/ethernet/qualcomm/Kconfig
@@ -6,9 +6,7 @@ config NET_VENDOR_QUALCOMM
 	bool "Qualcomm devices"
 	default y
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/qualcomm/qca_spi.c b/drivers/net/ethernet/qualcomm/qca_spi.c
index 6af028d5f9bc..2f87909f5186 100644
--- a/drivers/net/ethernet/qualcomm/qca_spi.c
+++ b/drivers/net/ethernet/qualcomm/qca_spi.c
@@ -839,7 +839,7 @@ static const struct of_device_id qca_spi_of_match[] = {
 MODULE_DEVICE_TABLE(of, qca_spi_of_match);
 
 static int
-qca_spi_probe(struct spi_device *spi_device)
+qca_spi_probe(struct spi_device *spi)
 {
 	struct qcaspi *qca = NULL;
 	struct net_device *qcaspi_devs = NULL;
@@ -847,52 +847,52 @@ qca_spi_probe(struct spi_device *spi_device)
 	u16 signature;
 	const char *mac;
 
-	if (!spi_device->dev.of_node) {
-		dev_err(&spi_device->dev, "Missing device tree\n");
+	if (!spi->dev.of_node) {
+		dev_err(&spi->dev, "Missing device tree\n");
 		return -EINVAL;
 	}
 
-	legacy_mode = of_property_read_bool(spi_device->dev.of_node,
+	legacy_mode = of_property_read_bool(spi->dev.of_node,
 					    "qca,legacy-mode");
 
 	if (qcaspi_clkspeed == 0) {
-		if (spi_device->max_speed_hz)
-			qcaspi_clkspeed = spi_device->max_speed_hz;
+		if (spi->max_speed_hz)
+			qcaspi_clkspeed = spi->max_speed_hz;
 		else
 			qcaspi_clkspeed = QCASPI_CLK_SPEED;
 	}
 
 	if ((qcaspi_clkspeed < QCASPI_CLK_SPEED_MIN) ||
 	    (qcaspi_clkspeed > QCASPI_CLK_SPEED_MAX)) {
-		dev_info(&spi_device->dev, "Invalid clkspeed: %d\n",
+		dev_info(&spi->dev, "Invalid clkspeed: %d\n",
 			 qcaspi_clkspeed);
 		return -EINVAL;
 	}
 
 	if ((qcaspi_burst_len < QCASPI_BURST_LEN_MIN) ||
 	    (qcaspi_burst_len > QCASPI_BURST_LEN_MAX)) {
-		dev_info(&spi_device->dev, "Invalid burst len: %d\n",
+		dev_info(&spi->dev, "Invalid burst len: %d\n",
 			 qcaspi_burst_len);
 		return -EINVAL;
 	}
 
 	if ((qcaspi_pluggable < QCASPI_PLUGGABLE_MIN) ||
 	    (qcaspi_pluggable > QCASPI_PLUGGABLE_MAX)) {
-		dev_info(&spi_device->dev, "Invalid pluggable: %d\n",
+		dev_info(&spi->dev, "Invalid pluggable: %d\n",
 			 qcaspi_pluggable);
 		return -EINVAL;
 	}
 
-	dev_info(&spi_device->dev, "ver=%s, clkspeed=%d, burst_len=%d, pluggable=%d\n",
+	dev_info(&spi->dev, "ver=%s, clkspeed=%d, burst_len=%d, pluggable=%d\n",
 		 QCASPI_DRV_VERSION,
 		 qcaspi_clkspeed,
 		 qcaspi_burst_len,
 		 qcaspi_pluggable);
 
-	spi_device->mode = SPI_MODE_3;
-	spi_device->max_speed_hz = qcaspi_clkspeed;
-	if (spi_setup(spi_device) < 0) {
-		dev_err(&spi_device->dev, "Unable to setup SPI device\n");
+	spi->mode = SPI_MODE_3;
+	spi->max_speed_hz = qcaspi_clkspeed;
+	if (spi_setup(spi) < 0) {
+		dev_err(&spi->dev, "Unable to setup SPI device\n");
 		return -EFAULT;
 	}
 
@@ -905,23 +905,23 @@ qca_spi_probe(struct spi_device *spi_device)
 	qca = netdev_priv(qcaspi_devs);
 	if (!qca) {
 		free_netdev(qcaspi_devs);
-		dev_err(&spi_device->dev, "Fail to retrieve private structure\n");
+		dev_err(&spi->dev, "Fail to retrieve private structure\n");
 		return -ENOMEM;
 	}
 	qca->net_dev = qcaspi_devs;
-	qca->spi_dev = spi_device;
+	qca->spi_dev = spi;
 	qca->legacy_mode = legacy_mode;
 
-	spi_set_drvdata(spi_device, qcaspi_devs);
+	spi_set_drvdata(spi, qcaspi_devs);
 
-	mac = of_get_mac_address(spi_device->dev.of_node);
+	mac = of_get_mac_address(spi->dev.of_node);
 
 	if (mac)
 		ether_addr_copy(qca->net_dev->dev_addr, mac);
 
 	if (!is_valid_ether_addr(qca->net_dev->dev_addr)) {
 		eth_hw_addr_random(qca->net_dev);
-		dev_info(&spi_device->dev, "Using random MAC address: %pM\n",
+		dev_info(&spi->dev, "Using random MAC address: %pM\n",
 			 qca->net_dev->dev_addr);
 	}
 
@@ -932,7 +932,7 @@ qca_spi_probe(struct spi_device *spi_device)
 		qcaspi_read_register(qca, SPI_REG_SIGNATURE, &signature);
 
 		if (signature != QCASPI_GOOD_SIGNATURE) {
-			dev_err(&spi_device->dev, "Invalid signature (0x%04X)\n",
+			dev_err(&spi->dev, "Invalid signature (0x%04X)\n",
 				signature);
 			free_netdev(qcaspi_devs);
 			return -EFAULT;
@@ -940,7 +940,7 @@ qca_spi_probe(struct spi_device *spi_device)
 	}
 
 	if (register_netdev(qcaspi_devs)) {
-		dev_info(&spi_device->dev, "Unable to register net device %s\n",
+		dev_info(&spi->dev, "Unable to register net device %s\n",
 			 qcaspi_devs->name);
 		free_netdev(qcaspi_devs);
 		return -EFAULT;
@@ -952,9 +952,9 @@ qca_spi_probe(struct spi_device *spi_device)
 }
 
 static int
-qca_spi_remove(struct spi_device *spi_device)
+qca_spi_remove(struct spi_device *spi)
 {
-	struct net_device *qcaspi_devs = spi_get_drvdata(spi_device);
+	struct net_device *qcaspi_devs = spi_get_drvdata(spi);
 	struct qcaspi *qca = netdev_priv(qcaspi_devs);
 
 	qcaspi_remove_device_debugfs(qca);
diff --git a/drivers/net/ethernet/rdc/Kconfig b/drivers/net/ethernet/rdc/Kconfig
index 2055f7eb2ba9..a9c4e990d29b 100644
--- a/drivers/net/ethernet/rdc/Kconfig
+++ b/drivers/net/ethernet/rdc/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_RDC
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/realtek/Kconfig b/drivers/net/ethernet/realtek/Kconfig
index ae5d027096ed..7c69f4c8134d 100644
--- a/drivers/net/ethernet/realtek/Kconfig
+++ b/drivers/net/ethernet/realtek/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_REALTEK
 	default y
 	depends on PCI || (PARPORT && X86)
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -24,8 +22,7 @@ config ATP
 	select CRC32
 	---help---
 	  This is a network (Ethernet) device which attaches to your parallel
-	  port. Read <file:drivers/net/ethernet/realtek/atp.c> as well as the
-	  Ethernet-HOWTO, available from <http://www.tldp.org/docs.html#howto>,
+	  port. Read the file <file:drivers/net/ethernet/realtek/atp.c>
 	  if you want to use this.  If you intend to use this driver, you
 	  should have said N to the "Parallel printer support", because the two
 	  drivers don't like each other.
@@ -40,9 +37,7 @@ config 8139CP
 	select MII
 	---help---
 	  This is a driver for the Fast Ethernet PCI network cards based on
-	  the RTL8139C+ chips. If you have one of those, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  the RTL8139C+ chips. If you have one of those, say Y here.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called 8139cp.  This is recommended.
@@ -54,8 +49,7 @@ config 8139TOO
 	select MII
 	---help---
 	  This is a driver for the Fast Ethernet PCI network cards based on
-	  the RTL 8129/8130/8139 chips. If you have one of those, say Y and
-	  read the Ethernet-HOWTO <http://www.tldp.org/docs.html#howto>.
+	  the RTL 8129/8130/8139 chips. If you have one of those, say Y here.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called 8139too.  This is recommended.
diff --git a/drivers/net/ethernet/renesas/Kconfig b/drivers/net/ethernet/renesas/Kconfig
index 196e98a2d93b..270c4c9cac7f 100644
--- a/drivers/net/ethernet/renesas/Kconfig
+++ b/drivers/net/ethernet/renesas/Kconfig
@@ -2,6 +2,19 @@
 # Renesas device configuration
 #
 
+config NET_VENDOR_RENESAS
+	bool "Renesas devices"
+	default y
+	---help---
+	  If you have a network (Ethernet) card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about Renesas devices. If you say Y, you will be asked
+	  for your specific device in the following questions.
+
+if NET_VENDOR_RENESAS
+
 config SH_ETH
 	tristate "Renesas SuperH Ethernet support"
 	depends on HAS_DMA
@@ -15,3 +28,19 @@ config SH_ETH
 	  This driver supporting CPUs are:
 		- SH7619, SH7710, SH7712, SH7724, SH7734, SH7763, SH7757,
 		  R8A7740, R8A777x and R8A779x.
+
+config RAVB
+	tristate "Renesas Ethernet AVB support"
+	depends on HAS_DMA
+	depends on ARCH_SHMOBILE || COMPILE_TEST
+	select CRC32
+	select MII
+	select MDIO_BITBANG
+	select PHYLIB
+	select PTP_1588_CLOCK
+	help
+	  Renesas Ethernet AVB device driver.
+	  This driver supports the following SoCs:
+		- R8A779x.
+
+endif # NET_VENDOR_RENESAS
diff --git a/drivers/net/ethernet/renesas/Makefile b/drivers/net/ethernet/renesas/Makefile
index 1c278a8e066a..a05102a7df02 100644
--- a/drivers/net/ethernet/renesas/Makefile
+++ b/drivers/net/ethernet/renesas/Makefile
@@ -3,3 +3,7 @@
 #
 
 obj-$(CONFIG_SH_ETH) += sh_eth.o
+
+ravb-objs := ravb_main.o ravb_ptp.o
+
+obj-$(CONFIG_RAVB) += ravb.o
diff --git a/drivers/net/ethernet/renesas/ravb.h b/drivers/net/ethernet/renesas/ravb.h
new file mode 100644
index 000000000000..8aa50ac4e2d6
--- /dev/null
+++ b/drivers/net/ethernet/renesas/ravb.h
@@ -0,0 +1,832 @@
+/* Renesas Ethernet AVB device driver
+ *
+ * Copyright (C) 2014-2015 Renesas Electronics Corporation
+ * Copyright (C) 2015 Renesas Solutions Corp.
+ * Copyright (C) 2015 Cogent Embedded, Inc. <source@cogentembedded.com>
+ *
+ * Based on the SuperH Ethernet driver
+ *
+ * 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.
+ */
+
+#ifndef __RAVB_H__
+#define __RAVB_H__
+
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mdio-bitbang.h>
+#include <linux/netdevice.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/ptp_clock_kernel.h>
+
+#define BE_TX_RING_SIZE	64	/* TX ring size for Best Effort */
+#define BE_RX_RING_SIZE	1024	/* RX ring size for Best Effort */
+#define NC_TX_RING_SIZE	64	/* TX ring size for Network Control */
+#define NC_RX_RING_SIZE	64	/* RX ring size for Network Control */
+#define BE_TX_RING_MIN	64
+#define BE_RX_RING_MIN	64
+#define BE_TX_RING_MAX	1024
+#define BE_RX_RING_MAX	2048
+
+#define PKT_BUF_SZ	1538
+
+/* Driver's parameters */
+#define RAVB_ALIGN	128
+
+/* Hardware time stamp */
+#define RAVB_TXTSTAMP_VALID	0x00000001	/* TX timestamp valid */
+#define RAVB_TXTSTAMP_ENABLED	0x00000010	/* Enable TX timestamping */
+
+#define RAVB_RXTSTAMP_VALID	0x00000001	/* RX timestamp valid */
+#define RAVB_RXTSTAMP_TYPE	0x00000006	/* RX type mask */
+#define RAVB_RXTSTAMP_TYPE_V2_L2_EVENT 0x00000002
+#define RAVB_RXTSTAMP_TYPE_ALL	0x00000006
+#define RAVB_RXTSTAMP_ENABLED	0x00000010	/* Enable RX timestamping */
+
+enum ravb_reg {
+	/* AVB-DMAC registers */
+	CCC	= 0x0000,
+	DBAT	= 0x0004,
+	DLR	= 0x0008,
+	CSR	= 0x000C,
+	CDAR0	= 0x0010,
+	CDAR1	= 0x0014,
+	CDAR2	= 0x0018,
+	CDAR3	= 0x001C,
+	CDAR4	= 0x0020,
+	CDAR5	= 0x0024,
+	CDAR6	= 0x0028,
+	CDAR7	= 0x002C,
+	CDAR8	= 0x0030,
+	CDAR9	= 0x0034,
+	CDAR10	= 0x0038,
+	CDAR11	= 0x003C,
+	CDAR12	= 0x0040,
+	CDAR13	= 0x0044,
+	CDAR14	= 0x0048,
+	CDAR15	= 0x004C,
+	CDAR16	= 0x0050,
+	CDAR17	= 0x0054,
+	CDAR18	= 0x0058,
+	CDAR19	= 0x005C,
+	CDAR20	= 0x0060,
+	CDAR21	= 0x0064,
+	ESR	= 0x0088,
+	RCR	= 0x0090,
+	RQC0	= 0x0094,
+	RQC1	= 0x0098,
+	RQC2	= 0x009C,
+	RQC3	= 0x00A0,
+	RQC4	= 0x00A4,
+	RPC	= 0x00B0,
+	UFCW	= 0x00BC,
+	UFCS	= 0x00C0,
+	UFCV0	= 0x00C4,
+	UFCV1	= 0x00C8,
+	UFCV2	= 0x00CC,
+	UFCV3	= 0x00D0,
+	UFCV4	= 0x00D4,
+	UFCD0	= 0x00E0,
+	UFCD1	= 0x00E4,
+	UFCD2	= 0x00E8,
+	UFCD3	= 0x00EC,
+	UFCD4	= 0x00F0,
+	SFO	= 0x00FC,
+	SFP0	= 0x0100,
+	SFP1	= 0x0104,
+	SFP2	= 0x0108,
+	SFP3	= 0x010C,
+	SFP4	= 0x0110,
+	SFP5	= 0x0114,
+	SFP6	= 0x0118,
+	SFP7	= 0x011C,
+	SFP8	= 0x0120,
+	SFP9	= 0x0124,
+	SFP10	= 0x0128,
+	SFP11	= 0x012C,
+	SFP12	= 0x0130,
+	SFP13	= 0x0134,
+	SFP14	= 0x0138,
+	SFP15	= 0x013C,
+	SFP16	= 0x0140,
+	SFP17	= 0x0144,
+	SFP18	= 0x0148,
+	SFP19	= 0x014C,
+	SFP20	= 0x0150,
+	SFP21	= 0x0154,
+	SFP22	= 0x0158,
+	SFP23	= 0x015C,
+	SFP24	= 0x0160,
+	SFP25	= 0x0164,
+	SFP26	= 0x0168,
+	SFP27	= 0x016C,
+	SFP28	= 0x0170,
+	SFP29	= 0x0174,
+	SFP30	= 0x0178,
+	SFP31	= 0x017C,
+	SFM0	= 0x01C0,
+	SFM1	= 0x01C4,
+	TGC	= 0x0300,
+	TCCR	= 0x0304,
+	TSR	= 0x0308,
+	TFA0	= 0x0310,
+	TFA1	= 0x0314,
+	TFA2	= 0x0318,
+	CIVR0	= 0x0320,
+	CIVR1	= 0x0324,
+	CDVR0	= 0x0328,
+	CDVR1	= 0x032C,
+	CUL0	= 0x0330,
+	CUL1	= 0x0334,
+	CLL0	= 0x0338,
+	CLL1	= 0x033C,
+	DIC	= 0x0350,
+	DIS	= 0x0354,
+	EIC	= 0x0358,
+	EIS	= 0x035C,
+	RIC0	= 0x0360,
+	RIS0	= 0x0364,
+	RIC1	= 0x0368,
+	RIS1	= 0x036C,
+	RIC2	= 0x0370,
+	RIS2	= 0x0374,
+	TIC	= 0x0378,
+	TIS	= 0x037C,
+	ISS	= 0x0380,
+	GCCR	= 0x0390,
+	GMTT	= 0x0394,
+	GPTC	= 0x0398,
+	GTI	= 0x039C,
+	GTO0	= 0x03A0,
+	GTO1	= 0x03A4,
+	GTO2	= 0x03A8,
+	GIC	= 0x03AC,
+	GIS	= 0x03B0,
+	GCPT	= 0x03B4,	/* Undocumented? */
+	GCT0	= 0x03B8,
+	GCT1	= 0x03BC,
+	GCT2	= 0x03C0,
+
+	/* E-MAC registers */
+	ECMR	= 0x0500,
+	RFLR	= 0x0508,
+	ECSR	= 0x0510,
+	ECSIPR	= 0x0518,
+	PIR	= 0x0520,
+	PSR	= 0x0528,
+	PIPR	= 0x052c,
+	MPR	= 0x0558,
+	PFTCR	= 0x055c,
+	PFRCR	= 0x0560,
+	GECMR	= 0x05b0,
+	MAHR	= 0x05c0,
+	MALR	= 0x05c8,
+	TROCR	= 0x0700,	/* Undocumented? */
+	CDCR	= 0x0708,	/* Undocumented? */
+	LCCR	= 0x0710,	/* Undocumented? */
+	CEFCR	= 0x0740,
+	FRECR	= 0x0748,
+	TSFRCR	= 0x0750,
+	TLFRCR	= 0x0758,
+	RFCR	= 0x0760,
+	CERCR	= 0x0768,	/* Undocumented? */
+	CEECR	= 0x0770,	/* Undocumented? */
+	MAFCR	= 0x0778,
+};
+
+
+/* Register bits of the Ethernet AVB */
+/* CCC */
+enum CCC_BIT {
+	CCC_OPC		= 0x00000003,
+	CCC_OPC_RESET	= 0x00000000,
+	CCC_OPC_CONFIG	= 0x00000001,
+	CCC_OPC_OPERATION = 0x00000002,
+	CCC_DTSR	= 0x00000100,
+	CCC_CSEL	= 0x00030000,
+	CCC_CSEL_HPB	= 0x00010000,
+	CCC_CSEL_ETH_TX	= 0x00020000,
+	CCC_CSEL_GMII_REF = 0x00030000,
+	CCC_BOC		= 0x00100000,	/* Undocumented? */
+	CCC_LBME	= 0x01000000,
+};
+
+/* CSR */
+enum CSR_BIT {
+	CSR_OPS		= 0x0000000F,
+	CSR_OPS_RESET	= 0x00000001,
+	CSR_OPS_CONFIG	= 0x00000002,
+	CSR_OPS_OPERATION = 0x00000004,
+	CSR_OPS_STANDBY	= 0x00000008,	/* Undocumented? */
+	CSR_DTS		= 0x00000100,
+	CSR_TPO0	= 0x00010000,
+	CSR_TPO1	= 0x00020000,
+	CSR_TPO2	= 0x00040000,
+	CSR_TPO3	= 0x00080000,
+	CSR_RPO		= 0x00100000,
+};
+
+/* ESR */
+enum ESR_BIT {
+	ESR_EQN		= 0x0000001F,
+	ESR_ET		= 0x00000F00,
+	ESR_EIL		= 0x00001000,
+};
+
+/* RCR */
+enum RCR_BIT {
+	RCR_EFFS	= 0x00000001,
+	RCR_ENCF	= 0x00000002,
+	RCR_ESF		= 0x0000000C,
+	RCR_ETS0	= 0x00000010,
+	RCR_ETS2	= 0x00000020,
+	RCR_RFCL	= 0x1FFF0000,
+};
+
+/* RQC0/1/2/3/4 */
+enum RQC_BIT {
+	RQC_RSM0	= 0x00000003,
+	RQC_UFCC0	= 0x00000030,
+	RQC_RSM1	= 0x00000300,
+	RQC_UFCC1	= 0x00003000,
+	RQC_RSM2	= 0x00030000,
+	RQC_UFCC2	= 0x00300000,
+	RQC_RSM3	= 0x03000000,
+	RQC_UFCC3	= 0x30000000,
+};
+
+/* RPC */
+enum RPC_BIT {
+	RPC_PCNT	= 0x00000700,
+	RPC_DCNT	= 0x00FF0000,
+};
+
+/* UFCW */
+enum UFCW_BIT {
+	UFCW_WL0	= 0x0000003F,
+	UFCW_WL1	= 0x00003F00,
+	UFCW_WL2	= 0x003F0000,
+	UFCW_WL3	= 0x3F000000,
+};
+
+/* UFCS */
+enum UFCS_BIT {
+	UFCS_SL0	= 0x0000003F,
+	UFCS_SL1	= 0x00003F00,
+	UFCS_SL2	= 0x003F0000,
+	UFCS_SL3	= 0x3F000000,
+};
+
+/* UFCV0/1/2/3/4 */
+enum UFCV_BIT {
+	UFCV_CV0	= 0x0000003F,
+	UFCV_CV1	= 0x00003F00,
+	UFCV_CV2	= 0x003F0000,
+	UFCV_CV3	= 0x3F000000,
+};
+
+/* UFCD0/1/2/3/4 */
+enum UFCD_BIT {
+	UFCD_DV0	= 0x0000003F,
+	UFCD_DV1	= 0x00003F00,
+	UFCD_DV2	= 0x003F0000,
+	UFCD_DV3	= 0x3F000000,
+};
+
+/* SFO */
+enum SFO_BIT {
+	SFO_FPB		= 0x0000003F,
+};
+
+/* RTC */
+enum RTC_BIT {
+	RTC_MFL0	= 0x00000FFF,
+	RTC_MFL1	= 0x0FFF0000,
+};
+
+/* TGC */
+enum TGC_BIT {
+	TGC_TSM0	= 0x00000001,
+	TGC_TSM1	= 0x00000002,
+	TGC_TSM2	= 0x00000004,
+	TGC_TSM3	= 0x00000008,
+	TGC_TQP		= 0x00000030,
+	TGC_TQP_NONAVB	= 0x00000000,
+	TGC_TQP_AVBMODE1 = 0x00000010,
+	TGC_TQP_AVBMODE2 = 0x00000030,
+	TGC_TBD0	= 0x00000300,
+	TGC_TBD1	= 0x00003000,
+	TGC_TBD2	= 0x00030000,
+	TGC_TBD3	= 0x00300000,
+};
+
+/* TCCR */
+enum TCCR_BIT {
+	TCCR_TSRQ0	= 0x00000001,
+	TCCR_TSRQ1	= 0x00000002,
+	TCCR_TSRQ2	= 0x00000004,
+	TCCR_TSRQ3	= 0x00000008,
+	TCCR_TFEN	= 0x00000100,
+	TCCR_TFR	= 0x00000200,
+};
+
+/* TSR */
+enum TSR_BIT {
+	TSR_CCS0	= 0x00000003,
+	TSR_CCS1	= 0x0000000C,
+	TSR_TFFL	= 0x00000700,
+};
+
+/* TFA2 */
+enum TFA2_BIT {
+	TFA2_TSV	= 0x0000FFFF,
+	TFA2_TST	= 0x03FF0000,
+};
+
+/* DIC */
+enum DIC_BIT {
+	DIC_DPE1	= 0x00000002,
+	DIC_DPE2	= 0x00000004,
+	DIC_DPE3	= 0x00000008,
+	DIC_DPE4	= 0x00000010,
+	DIC_DPE5	= 0x00000020,
+	DIC_DPE6	= 0x00000040,
+	DIC_DPE7	= 0x00000080,
+	DIC_DPE8	= 0x00000100,
+	DIC_DPE9	= 0x00000200,
+	DIC_DPE10	= 0x00000400,
+	DIC_DPE11	= 0x00000800,
+	DIC_DPE12	= 0x00001000,
+	DIC_DPE13	= 0x00002000,
+	DIC_DPE14	= 0x00004000,
+	DIC_DPE15	= 0x00008000,
+};
+
+/* DIS */
+enum DIS_BIT {
+	DIS_DPF1	= 0x00000002,
+	DIS_DPF2	= 0x00000004,
+	DIS_DPF3	= 0x00000008,
+	DIS_DPF4	= 0x00000010,
+	DIS_DPF5	= 0x00000020,
+	DIS_DPF6	= 0x00000040,
+	DIS_DPF7	= 0x00000080,
+	DIS_DPF8	= 0x00000100,
+	DIS_DPF9	= 0x00000200,
+	DIS_DPF10	= 0x00000400,
+	DIS_DPF11	= 0x00000800,
+	DIS_DPF12	= 0x00001000,
+	DIS_DPF13	= 0x00002000,
+	DIS_DPF14	= 0x00004000,
+	DIS_DPF15	= 0x00008000,
+};
+
+/* EIC */
+enum EIC_BIT {
+	EIC_MREE	= 0x00000001,
+	EIC_MTEE	= 0x00000002,
+	EIC_QEE		= 0x00000004,
+	EIC_SEE		= 0x00000008,
+	EIC_CLLE0	= 0x00000010,
+	EIC_CLLE1	= 0x00000020,
+	EIC_CULE0	= 0x00000040,
+	EIC_CULE1	= 0x00000080,
+	EIC_TFFE	= 0x00000100,
+};
+
+/* EIS */
+enum EIS_BIT {
+	EIS_MREF	= 0x00000001,
+	EIS_MTEF	= 0x00000002,
+	EIS_QEF		= 0x00000004,
+	EIS_SEF		= 0x00000008,
+	EIS_CLLF0	= 0x00000010,
+	EIS_CLLF1	= 0x00000020,
+	EIS_CULF0	= 0x00000040,
+	EIS_CULF1	= 0x00000080,
+	EIS_TFFF	= 0x00000100,
+	EIS_QFS		= 0x00010000,
+};
+
+/* RIC0 */
+enum RIC0_BIT {
+	RIC0_FRE0	= 0x00000001,
+	RIC0_FRE1	= 0x00000002,
+	RIC0_FRE2	= 0x00000004,
+	RIC0_FRE3	= 0x00000008,
+	RIC0_FRE4	= 0x00000010,
+	RIC0_FRE5	= 0x00000020,
+	RIC0_FRE6	= 0x00000040,
+	RIC0_FRE7	= 0x00000080,
+	RIC0_FRE8	= 0x00000100,
+	RIC0_FRE9	= 0x00000200,
+	RIC0_FRE10	= 0x00000400,
+	RIC0_FRE11	= 0x00000800,
+	RIC0_FRE12	= 0x00001000,
+	RIC0_FRE13	= 0x00002000,
+	RIC0_FRE14	= 0x00004000,
+	RIC0_FRE15	= 0x00008000,
+	RIC0_FRE16	= 0x00010000,
+	RIC0_FRE17	= 0x00020000,
+};
+
+/* RIC0 */
+enum RIS0_BIT {
+	RIS0_FRF0	= 0x00000001,
+	RIS0_FRF1	= 0x00000002,
+	RIS0_FRF2	= 0x00000004,
+	RIS0_FRF3	= 0x00000008,
+	RIS0_FRF4	= 0x00000010,
+	RIS0_FRF5	= 0x00000020,
+	RIS0_FRF6	= 0x00000040,
+	RIS0_FRF7	= 0x00000080,
+	RIS0_FRF8	= 0x00000100,
+	RIS0_FRF9	= 0x00000200,
+	RIS0_FRF10	= 0x00000400,
+	RIS0_FRF11	= 0x00000800,
+	RIS0_FRF12	= 0x00001000,
+	RIS0_FRF13	= 0x00002000,
+	RIS0_FRF14	= 0x00004000,
+	RIS0_FRF15	= 0x00008000,
+	RIS0_FRF16	= 0x00010000,
+	RIS0_FRF17	= 0x00020000,
+};
+
+/* RIC1 */
+enum RIC1_BIT {
+	RIC1_RFWE	= 0x80000000,
+};
+
+/* RIS1 */
+enum RIS1_BIT {
+	RIS1_RFWF	= 0x80000000,
+};
+
+/* RIC2 */
+enum RIC2_BIT {
+	RIC2_QFE0	= 0x00000001,
+	RIC2_QFE1	= 0x00000002,
+	RIC2_QFE2	= 0x00000004,
+	RIC2_QFE3	= 0x00000008,
+	RIC2_QFE4	= 0x00000010,
+	RIC2_QFE5	= 0x00000020,
+	RIC2_QFE6	= 0x00000040,
+	RIC2_QFE7	= 0x00000080,
+	RIC2_QFE8	= 0x00000100,
+	RIC2_QFE9	= 0x00000200,
+	RIC2_QFE10	= 0x00000400,
+	RIC2_QFE11	= 0x00000800,
+	RIC2_QFE12	= 0x00001000,
+	RIC2_QFE13	= 0x00002000,
+	RIC2_QFE14	= 0x00004000,
+	RIC2_QFE15	= 0x00008000,
+	RIC2_QFE16	= 0x00010000,
+	RIC2_QFE17	= 0x00020000,
+	RIC2_RFFE	= 0x80000000,
+};
+
+/* RIS2 */
+enum RIS2_BIT {
+	RIS2_QFF0	= 0x00000001,
+	RIS2_QFF1	= 0x00000002,
+	RIS2_QFF2	= 0x00000004,
+	RIS2_QFF3	= 0x00000008,
+	RIS2_QFF4	= 0x00000010,
+	RIS2_QFF5	= 0x00000020,
+	RIS2_QFF6	= 0x00000040,
+	RIS2_QFF7	= 0x00000080,
+	RIS2_QFF8	= 0x00000100,
+	RIS2_QFF9	= 0x00000200,
+	RIS2_QFF10	= 0x00000400,
+	RIS2_QFF11	= 0x00000800,
+	RIS2_QFF12	= 0x00001000,
+	RIS2_QFF13	= 0x00002000,
+	RIS2_QFF14	= 0x00004000,
+	RIS2_QFF15	= 0x00008000,
+	RIS2_QFF16	= 0x00010000,
+	RIS2_QFF17	= 0x00020000,
+	RIS2_RFFF	= 0x80000000,
+};
+
+/* TIC */
+enum TIC_BIT {
+	TIC_FTE0	= 0x00000001,	/* Undocumented? */
+	TIC_FTE1	= 0x00000002,	/* Undocumented? */
+	TIC_TFUE	= 0x00000100,
+	TIC_TFWE	= 0x00000200,
+};
+
+/* TIS */
+enum TIS_BIT {
+	TIS_FTF0	= 0x00000001,	/* Undocumented? */
+	TIS_FTF1	= 0x00000002,	/* Undocumented? */
+	TIS_TFUF	= 0x00000100,
+	TIS_TFWF	= 0x00000200,
+};
+
+/* ISS */
+enum ISS_BIT {
+	ISS_FRS		= 0x00000001,	/* Undocumented? */
+	ISS_FTS		= 0x00000004,	/* Undocumented? */
+	ISS_ES		= 0x00000040,
+	ISS_MS		= 0x00000080,
+	ISS_TFUS	= 0x00000100,
+	ISS_TFWS	= 0x00000200,
+	ISS_RFWS	= 0x00001000,
+	ISS_CGIS	= 0x00002000,
+	ISS_DPS1	= 0x00020000,
+	ISS_DPS2	= 0x00040000,
+	ISS_DPS3	= 0x00080000,
+	ISS_DPS4	= 0x00100000,
+	ISS_DPS5	= 0x00200000,
+	ISS_DPS6	= 0x00400000,
+	ISS_DPS7	= 0x00800000,
+	ISS_DPS8	= 0x01000000,
+	ISS_DPS9	= 0x02000000,
+	ISS_DPS10	= 0x04000000,
+	ISS_DPS11	= 0x08000000,
+	ISS_DPS12	= 0x10000000,
+	ISS_DPS13	= 0x20000000,
+	ISS_DPS14	= 0x40000000,
+	ISS_DPS15	= 0x80000000,
+};
+
+/* GCCR */
+enum GCCR_BIT {
+	GCCR_TCR	= 0x00000003,
+	GCCR_TCR_NOREQ	= 0x00000000, /* No request */
+	GCCR_TCR_RESET	= 0x00000001, /* gPTP/AVTP presentation timer reset */
+	GCCR_TCR_CAPTURE = 0x00000003, /* Capture value set in GCCR.TCSS */
+	GCCR_LTO	= 0x00000004,
+	GCCR_LTI	= 0x00000008,
+	GCCR_LPTC	= 0x00000010,
+	GCCR_LMTT	= 0x00000020,
+	GCCR_TCSS	= 0x00000300,
+	GCCR_TCSS_GPTP	= 0x00000000,	/* gPTP timer value */
+	GCCR_TCSS_ADJGPTP = 0x00000100, /* Adjusted gPTP timer value */
+	GCCR_TCSS_AVTP	= 0x00000200,	/* AVTP presentation time value */
+};
+
+/* GTI */
+enum GTI_BIT {
+	GTI_TIV		= 0x0FFFFFFF,
+};
+
+/* GIC */
+enum GIC_BIT {
+	GIC_PTCE	= 0x00000001,	/* Undocumented? */
+	GIC_PTME	= 0x00000004,
+};
+
+/* GIS */
+enum GIS_BIT {
+	GIS_PTCF	= 0x00000001,	/* Undocumented? */
+	GIS_PTMF	= 0x00000004,
+};
+
+/* ECMR */
+enum ECMR_BIT {
+	ECMR_PRM	= 0x00000001,
+	ECMR_DM		= 0x00000002,
+	ECMR_TE		= 0x00000020,
+	ECMR_RE		= 0x00000040,
+	ECMR_MPDE	= 0x00000200,
+	ECMR_TXF	= 0x00010000,	/* Undocumented? */
+	ECMR_RXF	= 0x00020000,
+	ECMR_PFR	= 0x00040000,
+	ECMR_ZPF	= 0x00080000,	/* Undocumented? */
+	ECMR_RZPF	= 0x00100000,
+	ECMR_DPAD	= 0x00200000,
+	ECMR_RCSC	= 0x00800000,
+	ECMR_TRCCM	= 0x04000000,
+};
+
+/* ECSR */
+enum ECSR_BIT {
+	ECSR_ICD	= 0x00000001,
+	ECSR_MPD	= 0x00000002,
+	ECSR_LCHNG	= 0x00000004,
+	ECSR_PHYI	= 0x00000008,
+};
+
+/* ECSIPR */
+enum ECSIPR_BIT {
+	ECSIPR_ICDIP	= 0x00000001,
+	ECSIPR_MPDIP	= 0x00000002,
+	ECSIPR_LCHNGIP	= 0x00000004,	/* Undocumented? */
+};
+
+/* PIR */
+enum PIR_BIT {
+	PIR_MDC		= 0x00000001,
+	PIR_MMD		= 0x00000002,
+	PIR_MDO		= 0x00000004,
+	PIR_MDI		= 0x00000008,
+};
+
+/* PSR */
+enum PSR_BIT {
+	PSR_LMON	= 0x00000001,
+};
+
+/* PIPR */
+enum PIPR_BIT {
+	PIPR_PHYIP	= 0x00000001,
+};
+
+/* MPR */
+enum MPR_BIT {
+	MPR_MP		= 0x0000ffff,
+};
+
+/* GECMR */
+enum GECMR_BIT {
+	GECMR_SPEED	= 0x00000001,
+	GECMR_SPEED_100	= 0x00000000,
+	GECMR_SPEED_1000 = 0x00000001,
+};
+
+/* The Ethernet AVB descriptor definitions. */
+struct ravb_desc {
+	__le16 ds;		/* Descriptor size */
+	u8 cc;		/* Content control MSBs (reserved) */
+	u8 die_dt;	/* Descriptor interrupt enable and type */
+	__le32 dptr;	/* Descriptor pointer */
+};
+
+enum DIE_DT {
+	/* Frame data */
+	DT_FMID		= 0x40,
+	DT_FSTART	= 0x50,
+	DT_FEND		= 0x60,
+	DT_FSINGLE	= 0x70,
+	/* Chain control */
+	DT_LINK		= 0x80,
+	DT_LINKFIX	= 0x90,
+	DT_EOS		= 0xa0,
+	/* HW/SW arbitration */
+	DT_FEMPTY	= 0xc0,
+	DT_FEMPTY_IS	= 0xd0,
+	DT_FEMPTY_IC	= 0xe0,
+	DT_FEMPTY_ND	= 0xf0,
+	DT_LEMPTY	= 0x20,
+	DT_EEMPTY	= 0x30,
+};
+
+struct ravb_rx_desc {
+	__le16 ds_cc;	/* Descriptor size and content control LSBs */
+	u8 msc;		/* MAC status code */
+	u8 die_dt;	/* Descriptor interrupt enable and type */
+	__le32 dptr;	/* Descpriptor pointer */
+};
+
+struct ravb_ex_rx_desc {
+	__le16 ds_cc;	/* Descriptor size and content control lower bits */
+	u8 msc;		/* MAC status code */
+	u8 die_dt;	/* Descriptor interrupt enable and type */
+	__le32 dptr;	/* Descpriptor pointer */
+	__le32 ts_n;	/* Timestampe nsec */
+	__le32 ts_sl;	/* Timestamp low */
+	__le16 ts_sh;	/* Timestamp high */
+	__le16 res;	/* Reserved bits */
+};
+
+enum RX_DS_CC_BIT {
+	RX_DS		= 0x0fff, /* Data size */
+	RX_TR		= 0x1000, /* Truncation indication */
+	RX_EI		= 0x2000, /* Error indication */
+	RX_PS		= 0xc000, /* Padding selection */
+};
+
+/* E-MAC status code */
+enum MSC_BIT {
+	MSC_CRC		= 0x01, /* Frame CRC error */
+	MSC_RFE		= 0x02, /* Frame reception error (flagged by PHY) */
+	MSC_RTSF	= 0x04, /* Frame length error (frame too short) */
+	MSC_RTLF	= 0x08, /* Frame length error (frame too long) */
+	MSC_FRE		= 0x10, /* Fraction error (not a multiple of 8 bits) */
+	MSC_CRL		= 0x20, /* Carrier lost */
+	MSC_CEEF	= 0x40, /* Carrier extension error */
+	MSC_MC		= 0x80, /* Multicast frame reception */
+};
+
+struct ravb_tx_desc {
+	__le16 ds_tagl;	/* Descriptor size and frame tag LSBs */
+	u8 tagh_tsr;	/* Frame tag MSBs and timestamp storage request bit */
+	u8 die_dt;	/* Descriptor interrupt enable and type */
+	__le32 dptr;	/* Descpriptor pointer */
+};
+
+enum TX_DS_TAGL_BIT {
+	TX_DS		= 0x0fff, /* Data size */
+	TX_TAGL		= 0xf000, /* Frame tag LSBs */
+};
+
+enum TX_TAGH_TSR_BIT {
+	TX_TAGH		= 0x3f, /* Frame tag MSBs */
+	TX_TSR		= 0x40, /* Timestamp storage request */
+};
+enum RAVB_QUEUE {
+	RAVB_BE = 0,	/* Best Effort Queue */
+	RAVB_NC,	/* Network Control Queue */
+};
+
+#define DBAT_ENTRY_NUM	22
+#define RX_QUEUE_OFFSET	4
+#define NUM_RX_QUEUE	2
+#define NUM_TX_QUEUE	2
+
+struct ravb_tstamp_skb {
+	struct list_head list;
+	struct sk_buff *skb;
+	u16 tag;
+};
+
+struct ravb_ptp_perout {
+	u32 target;
+	u32 period;
+};
+
+#define N_EXT_TS	1
+#define N_PER_OUT	1
+
+struct ravb_ptp {
+	struct ptp_clock *clock;
+	struct ptp_clock_info info;
+	u32 default_addend;
+	u32 current_addend;
+	int extts[N_EXT_TS];
+	struct ravb_ptp_perout perout[N_PER_OUT];
+};
+
+struct ravb_private {
+	struct net_device *ndev;
+	struct platform_device *pdev;
+	void __iomem *addr;
+	struct mdiobb_ctrl mdiobb;
+	u32 num_rx_ring[NUM_RX_QUEUE];
+	u32 num_tx_ring[NUM_TX_QUEUE];
+	u32 desc_bat_size;
+	dma_addr_t desc_bat_dma;
+	struct ravb_desc *desc_bat;
+	dma_addr_t rx_desc_dma[NUM_RX_QUEUE];
+	dma_addr_t tx_desc_dma[NUM_TX_QUEUE];
+	struct ravb_ex_rx_desc *rx_ring[NUM_RX_QUEUE];
+	struct ravb_tx_desc *tx_ring[NUM_TX_QUEUE];
+	struct sk_buff **rx_skb[NUM_RX_QUEUE];
+	struct sk_buff **tx_skb[NUM_TX_QUEUE];
+	void **tx_buffers[NUM_TX_QUEUE];
+	u32 rx_over_errors;
+	u32 rx_fifo_errors;
+	struct net_device_stats stats[NUM_RX_QUEUE];
+	u32 tstamp_tx_ctrl;
+	u32 tstamp_rx_ctrl;
+	struct list_head ts_skb_list;
+	u32 ts_skb_tag;
+	struct ravb_ptp ptp;
+	spinlock_t lock;		/* Register access lock */
+	u32 cur_rx[NUM_RX_QUEUE];	/* Consumer ring indices */
+	u32 dirty_rx[NUM_RX_QUEUE];	/* Producer ring indices */
+	u32 cur_tx[NUM_TX_QUEUE];
+	u32 dirty_tx[NUM_TX_QUEUE];
+	struct napi_struct napi[NUM_RX_QUEUE];
+	struct work_struct work;
+	/* MII transceiver section. */
+	struct mii_bus *mii_bus;	/* MDIO bus control */
+	struct phy_device *phydev;	/* PHY device control */
+	int link;
+	phy_interface_t phy_interface;
+	int msg_enable;
+	int speed;
+	int duplex;
+
+	unsigned no_avb_link:1;
+	unsigned avb_link_active_low:1;
+};
+
+static inline u32 ravb_read(struct net_device *ndev, enum ravb_reg reg)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	return ioread32(priv->addr + reg);
+}
+
+static inline void ravb_write(struct net_device *ndev, u32 data,
+			      enum ravb_reg reg)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	iowrite32(data, priv->addr + reg);
+}
+
+int ravb_wait(struct net_device *ndev, enum ravb_reg reg, u32 mask, u32 value);
+
+irqreturn_t ravb_ptp_interrupt(struct net_device *ndev);
+void ravb_ptp_init(struct net_device *ndev, struct platform_device *pdev);
+void ravb_ptp_stop(struct net_device *ndev);
+
+#endif	/* #ifndef __RAVB_H__ */
diff --git a/drivers/net/ethernet/renesas/ravb_main.c b/drivers/net/ethernet/renesas/ravb_main.c
new file mode 100644
index 000000000000..fd9745714d90
--- /dev/null
+++ b/drivers/net/ethernet/renesas/ravb_main.c
@@ -0,0 +1,1824 @@
+/* Renesas Ethernet AVB device driver
+ *
+ * Copyright (C) 2014-2015 Renesas Electronics Corporation
+ * Copyright (C) 2015 Renesas Solutions Corp.
+ * Copyright (C) 2015 Cogent Embedded, Inc. <source@cogentembedded.com>
+ *
+ * Based on the SuperH Ethernet driver
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License version 2,
+ * as published by the Free Software Foundation.
+ */
+
+#include <linux/cache.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/etherdevice.h>
+#include <linux/ethtool.h>
+#include <linux/if_vlan.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/net_tstamp.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+#include <linux/of_mdio.h>
+#include <linux/of_net.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "ravb.h"
+
+#define RAVB_DEF_MSG_ENABLE \
+		(NETIF_MSG_LINK	  | \
+		 NETIF_MSG_TIMER  | \
+		 NETIF_MSG_RX_ERR | \
+		 NETIF_MSG_TX_ERR)
+
+int ravb_wait(struct net_device *ndev, enum ravb_reg reg, u32 mask, u32 value)
+{
+	int i;
+
+	for (i = 0; i < 10000; i++) {
+		if ((ravb_read(ndev, reg) & mask) == value)
+			return 0;
+		udelay(10);
+	}
+	return -ETIMEDOUT;
+}
+
+static int ravb_config(struct net_device *ndev)
+{
+	int error;
+
+	/* Set config mode */
+	ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_OPC) | CCC_OPC_CONFIG,
+		   CCC);
+	/* Check if the operating mode is changed to the config mode */
+	error = ravb_wait(ndev, CSR, CSR_OPS, CSR_OPS_CONFIG);
+	if (error)
+		netdev_err(ndev, "failed to switch device to config mode\n");
+
+	return error;
+}
+
+static void ravb_set_duplex(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	u32 ecmr = ravb_read(ndev, ECMR);
+
+	if (priv->duplex)	/* Full */
+		ecmr |=  ECMR_DM;
+	else			/* Half */
+		ecmr &= ~ECMR_DM;
+	ravb_write(ndev, ecmr, ECMR);
+}
+
+static void ravb_set_rate(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	switch (priv->speed) {
+	case 100:		/* 100BASE */
+		ravb_write(ndev, GECMR_SPEED_100, GECMR);
+		break;
+	case 1000:		/* 1000BASE */
+		ravb_write(ndev, GECMR_SPEED_1000, GECMR);
+		break;
+	default:
+		break;
+	}
+}
+
+static void ravb_set_buffer_align(struct sk_buff *skb)
+{
+	u32 reserve = (unsigned long)skb->data & (RAVB_ALIGN - 1);
+
+	if (reserve)
+		skb_reserve(skb, RAVB_ALIGN - reserve);
+}
+
+/* Get MAC address from the MAC address registers
+ *
+ * Ethernet AVB device doesn't have ROM for MAC address.
+ * This function gets the MAC address that was used by a bootloader.
+ */
+static void ravb_read_mac_address(struct net_device *ndev, const u8 *mac)
+{
+	if (mac) {
+		ether_addr_copy(ndev->dev_addr, mac);
+	} else {
+		ndev->dev_addr[0] = (ravb_read(ndev, MAHR) >> 24);
+		ndev->dev_addr[1] = (ravb_read(ndev, MAHR) >> 16) & 0xFF;
+		ndev->dev_addr[2] = (ravb_read(ndev, MAHR) >> 8) & 0xFF;
+		ndev->dev_addr[3] = (ravb_read(ndev, MAHR) >> 0) & 0xFF;
+		ndev->dev_addr[4] = (ravb_read(ndev, MALR) >> 8) & 0xFF;
+		ndev->dev_addr[5] = (ravb_read(ndev, MALR) >> 0) & 0xFF;
+	}
+}
+
+static void ravb_mdio_ctrl(struct mdiobb_ctrl *ctrl, u32 mask, int set)
+{
+	struct ravb_private *priv = container_of(ctrl, struct ravb_private,
+						 mdiobb);
+	u32 pir = ravb_read(priv->ndev, PIR);
+
+	if (set)
+		pir |=  mask;
+	else
+		pir &= ~mask;
+	ravb_write(priv->ndev, pir, PIR);
+}
+
+/* MDC pin control */
+static void ravb_set_mdc(struct mdiobb_ctrl *ctrl, int level)
+{
+	ravb_mdio_ctrl(ctrl, PIR_MDC, level);
+}
+
+/* Data I/O pin control */
+static void ravb_set_mdio_dir(struct mdiobb_ctrl *ctrl, int output)
+{
+	ravb_mdio_ctrl(ctrl, PIR_MMD, output);
+}
+
+/* Set data bit */
+static void ravb_set_mdio_data(struct mdiobb_ctrl *ctrl, int value)
+{
+	ravb_mdio_ctrl(ctrl, PIR_MDO, value);
+}
+
+/* Get data bit */
+static int ravb_get_mdio_data(struct mdiobb_ctrl *ctrl)
+{
+	struct ravb_private *priv = container_of(ctrl, struct ravb_private,
+						 mdiobb);
+
+	return (ravb_read(priv->ndev, PIR) & PIR_MDI) != 0;
+}
+
+/* MDIO bus control struct */
+static struct mdiobb_ops bb_ops = {
+	.owner = THIS_MODULE,
+	.set_mdc = ravb_set_mdc,
+	.set_mdio_dir = ravb_set_mdio_dir,
+	.set_mdio_data = ravb_set_mdio_data,
+	.get_mdio_data = ravb_get_mdio_data,
+};
+
+/* Free skb's and DMA buffers for Ethernet AVB */
+static void ravb_ring_free(struct net_device *ndev, int q)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int ring_size;
+	int i;
+
+	/* Free RX skb ringbuffer */
+	if (priv->rx_skb[q]) {
+		for (i = 0; i < priv->num_rx_ring[q]; i++)
+			dev_kfree_skb(priv->rx_skb[q][i]);
+	}
+	kfree(priv->rx_skb[q]);
+	priv->rx_skb[q] = NULL;
+
+	/* Free TX skb ringbuffer */
+	if (priv->tx_skb[q]) {
+		for (i = 0; i < priv->num_tx_ring[q]; i++)
+			dev_kfree_skb(priv->tx_skb[q][i]);
+	}
+	kfree(priv->tx_skb[q]);
+	priv->tx_skb[q] = NULL;
+
+	/* Free aligned TX buffers */
+	if (priv->tx_buffers[q]) {
+		for (i = 0; i < priv->num_tx_ring[q]; i++)
+			kfree(priv->tx_buffers[q][i]);
+	}
+	kfree(priv->tx_buffers[q]);
+	priv->tx_buffers[q] = NULL;
+
+	if (priv->rx_ring[q]) {
+		ring_size = sizeof(struct ravb_ex_rx_desc) *
+			    (priv->num_rx_ring[q] + 1);
+		dma_free_coherent(NULL, ring_size, priv->rx_ring[q],
+				  priv->rx_desc_dma[q]);
+		priv->rx_ring[q] = NULL;
+	}
+
+	if (priv->tx_ring[q]) {
+		ring_size = sizeof(struct ravb_tx_desc) *
+			    (priv->num_tx_ring[q] + 1);
+		dma_free_coherent(NULL, ring_size, priv->tx_ring[q],
+				  priv->tx_desc_dma[q]);
+		priv->tx_ring[q] = NULL;
+	}
+}
+
+/* Format skb and descriptor buffer for Ethernet AVB */
+static void ravb_ring_format(struct net_device *ndev, int q)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct ravb_ex_rx_desc *rx_desc = NULL;
+	struct ravb_tx_desc *tx_desc = NULL;
+	struct ravb_desc *desc = NULL;
+	int rx_ring_size = sizeof(*rx_desc) * priv->num_rx_ring[q];
+	int tx_ring_size = sizeof(*tx_desc) * priv->num_tx_ring[q];
+	struct sk_buff *skb;
+	dma_addr_t dma_addr;
+	void *buffer;
+	int i;
+
+	priv->cur_rx[q] = 0;
+	priv->cur_tx[q] = 0;
+	priv->dirty_rx[q] = 0;
+	priv->dirty_tx[q] = 0;
+
+	memset(priv->rx_ring[q], 0, rx_ring_size);
+	/* Build RX ring buffer */
+	for (i = 0; i < priv->num_rx_ring[q]; i++) {
+		priv->rx_skb[q][i] = NULL;
+		skb = netdev_alloc_skb(ndev, PKT_BUF_SZ + RAVB_ALIGN - 1);
+		if (!skb)
+			break;
+		ravb_set_buffer_align(skb);
+		/* RX descriptor */
+		rx_desc = &priv->rx_ring[q][i];
+		/* The size of the buffer should be on 16-byte boundary. */
+		rx_desc->ds_cc = cpu_to_le16(ALIGN(PKT_BUF_SZ, 16));
+		dma_addr = dma_map_single(&ndev->dev, skb->data,
+					  ALIGN(PKT_BUF_SZ, 16),
+					  DMA_FROM_DEVICE);
+		if (dma_mapping_error(&ndev->dev, dma_addr)) {
+			dev_kfree_skb(skb);
+			break;
+		}
+		priv->rx_skb[q][i] = skb;
+		rx_desc->dptr = cpu_to_le32(dma_addr);
+		rx_desc->die_dt = DT_FEMPTY;
+	}
+	rx_desc = &priv->rx_ring[q][i];
+	rx_desc->dptr = cpu_to_le32((u32)priv->rx_desc_dma[q]);
+	rx_desc->die_dt = DT_LINKFIX; /* type */
+	priv->dirty_rx[q] = (u32)(i - priv->num_rx_ring[q]);
+
+	memset(priv->tx_ring[q], 0, tx_ring_size);
+	/* Build TX ring buffer */
+	for (i = 0; i < priv->num_tx_ring[q]; i++) {
+		priv->tx_skb[q][i] = NULL;
+		priv->tx_buffers[q][i] = NULL;
+		buffer = kmalloc(PKT_BUF_SZ + RAVB_ALIGN - 1, GFP_KERNEL);
+		if (!buffer)
+			break;
+		/* Aligned TX buffer */
+		priv->tx_buffers[q][i] = buffer;
+		tx_desc = &priv->tx_ring[q][i];
+		tx_desc->die_dt = DT_EEMPTY;
+	}
+	tx_desc = &priv->tx_ring[q][i];
+	tx_desc->dptr = cpu_to_le32((u32)priv->tx_desc_dma[q]);
+	tx_desc->die_dt = DT_LINKFIX; /* type */
+
+	/* RX descriptor base address for best effort */
+	desc = &priv->desc_bat[RX_QUEUE_OFFSET + q];
+	desc->die_dt = DT_LINKFIX; /* type */
+	desc->dptr = cpu_to_le32((u32)priv->rx_desc_dma[q]);
+
+	/* TX descriptor base address for best effort */
+	desc = &priv->desc_bat[q];
+	desc->die_dt = DT_LINKFIX; /* type */
+	desc->dptr = cpu_to_le32((u32)priv->tx_desc_dma[q]);
+}
+
+/* Init skb and descriptor buffer for Ethernet AVB */
+static int ravb_ring_init(struct net_device *ndev, int q)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int ring_size;
+
+	/* Allocate RX and TX skb rings */
+	priv->rx_skb[q] = kcalloc(priv->num_rx_ring[q],
+				  sizeof(*priv->rx_skb[q]), GFP_KERNEL);
+	priv->tx_skb[q] = kcalloc(priv->num_tx_ring[q],
+				  sizeof(*priv->tx_skb[q]), GFP_KERNEL);
+	if (!priv->rx_skb[q] || !priv->tx_skb[q])
+		goto error;
+
+	/* Allocate rings for the aligned buffers */
+	priv->tx_buffers[q] = kcalloc(priv->num_tx_ring[q],
+				      sizeof(*priv->tx_buffers[q]), GFP_KERNEL);
+	if (!priv->tx_buffers[q])
+		goto error;
+
+	/* Allocate all RX descriptors. */
+	ring_size = sizeof(struct ravb_ex_rx_desc) * (priv->num_rx_ring[q] + 1);
+	priv->rx_ring[q] = dma_alloc_coherent(NULL, ring_size,
+					      &priv->rx_desc_dma[q],
+					      GFP_KERNEL);
+	if (!priv->rx_ring[q])
+		goto error;
+
+	priv->dirty_rx[q] = 0;
+
+	/* Allocate all TX descriptors. */
+	ring_size = sizeof(struct ravb_tx_desc) * (priv->num_tx_ring[q] + 1);
+	priv->tx_ring[q] = dma_alloc_coherent(NULL, ring_size,
+					      &priv->tx_desc_dma[q],
+					      GFP_KERNEL);
+	if (!priv->tx_ring[q])
+		goto error;
+
+	return 0;
+
+error:
+	ravb_ring_free(ndev, q);
+
+	return -ENOMEM;
+}
+
+/* E-MAC init function */
+static void ravb_emac_init(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	u32 ecmr;
+
+	/* Receive frame limit set register */
+	ravb_write(ndev, ndev->mtu + ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN, RFLR);
+
+	/* PAUSE prohibition */
+	ecmr =  ravb_read(ndev, ECMR);
+	ecmr &= ECMR_DM;
+	ecmr |= ECMR_ZPF | (priv->duplex ? ECMR_DM : 0) | ECMR_TE | ECMR_RE;
+	ravb_write(ndev, ecmr, ECMR);
+
+	ravb_set_rate(ndev);
+
+	/* Set MAC address */
+	ravb_write(ndev,
+		   (ndev->dev_addr[0] << 24) | (ndev->dev_addr[1] << 16) |
+		   (ndev->dev_addr[2] << 8)  | (ndev->dev_addr[3]), MAHR);
+	ravb_write(ndev,
+		   (ndev->dev_addr[4] << 8)  | (ndev->dev_addr[5]), MALR);
+
+	ravb_write(ndev, 1, MPR);
+
+	/* E-MAC status register clear */
+	ravb_write(ndev, ECSR_ICD | ECSR_MPD, ECSR);
+
+	/* E-MAC interrupt enable register */
+	ravb_write(ndev, ECSIPR_ICDIP | ECSIPR_MPDIP | ECSIPR_LCHNGIP, ECSIPR);
+}
+
+/* Device init function for Ethernet AVB */
+static int ravb_dmac_init(struct net_device *ndev)
+{
+	int error;
+
+	/* Set CONFIG mode */
+	error = ravb_config(ndev);
+	if (error)
+		return error;
+
+	error = ravb_ring_init(ndev, RAVB_BE);
+	if (error)
+		return error;
+	error = ravb_ring_init(ndev, RAVB_NC);
+	if (error) {
+		ravb_ring_free(ndev, RAVB_BE);
+		return error;
+	}
+
+	/* Descriptor format */
+	ravb_ring_format(ndev, RAVB_BE);
+	ravb_ring_format(ndev, RAVB_NC);
+
+#if defined(__LITTLE_ENDIAN)
+	ravb_write(ndev, ravb_read(ndev, CCC) & ~CCC_BOC, CCC);
+#else
+	ravb_write(ndev, ravb_read(ndev, CCC) | CCC_BOC, CCC);
+#endif
+
+	/* Set AVB RX */
+	ravb_write(ndev, RCR_EFFS | RCR_ENCF | RCR_ETS0 | 0x18000000, RCR);
+
+	/* Set FIFO size */
+	ravb_write(ndev, TGC_TQP_AVBMODE1 | 0x00222200, TGC);
+
+	/* Timestamp enable */
+	ravb_write(ndev, TCCR_TFEN, TCCR);
+
+	/* Interrupt enable: */
+	/* Frame receive */
+	ravb_write(ndev, RIC0_FRE0 | RIC0_FRE1, RIC0);
+	/* Receive FIFO full warning */
+	ravb_write(ndev, RIC1_RFWE, RIC1);
+	/* Receive FIFO full error, descriptor empty */
+	ravb_write(ndev, RIC2_QFE0 | RIC2_QFE1 | RIC2_RFFE, RIC2);
+	/* Frame transmitted, timestamp FIFO updated */
+	ravb_write(ndev, TIC_FTE0 | TIC_FTE1 | TIC_TFUE, TIC);
+
+	/* Setting the control will start the AVB-DMAC process. */
+	ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_OPC) | CCC_OPC_OPERATION,
+		   CCC);
+
+	return 0;
+}
+
+/* Free TX skb function for AVB-IP */
+static int ravb_tx_free(struct net_device *ndev, int q)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct net_device_stats *stats = &priv->stats[q];
+	struct ravb_tx_desc *desc;
+	int free_num = 0;
+	int entry = 0;
+	u32 size;
+
+	for (; priv->cur_tx[q] - priv->dirty_tx[q] > 0; priv->dirty_tx[q]++) {
+		entry = priv->dirty_tx[q] % priv->num_tx_ring[q];
+		desc = &priv->tx_ring[q][entry];
+		if (desc->die_dt != DT_FEMPTY)
+			break;
+		/* Descriptor type must be checked before all other reads */
+		dma_rmb();
+		size = le16_to_cpu(desc->ds_tagl) & TX_DS;
+		/* Free the original skb. */
+		if (priv->tx_skb[q][entry]) {
+			dma_unmap_single(&ndev->dev, le32_to_cpu(desc->dptr),
+					 size, DMA_TO_DEVICE);
+			dev_kfree_skb_any(priv->tx_skb[q][entry]);
+			priv->tx_skb[q][entry] = NULL;
+			free_num++;
+		}
+		stats->tx_packets++;
+		stats->tx_bytes += size;
+		desc->die_dt = DT_EEMPTY;
+	}
+	return free_num;
+}
+
+static void ravb_get_tx_tstamp(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct ravb_tstamp_skb *ts_skb, *ts_skb2;
+	struct skb_shared_hwtstamps shhwtstamps;
+	struct sk_buff *skb;
+	struct timespec64 ts;
+	u16 tag, tfa_tag;
+	int count;
+	u32 tfa2;
+
+	count = (ravb_read(ndev, TSR) & TSR_TFFL) >> 8;
+	while (count--) {
+		tfa2 = ravb_read(ndev, TFA2);
+		tfa_tag = (tfa2 & TFA2_TST) >> 16;
+		ts.tv_nsec = (u64)ravb_read(ndev, TFA0);
+		ts.tv_sec = ((u64)(tfa2 & TFA2_TSV) << 32) |
+			    ravb_read(ndev, TFA1);
+		memset(&shhwtstamps, 0, sizeof(shhwtstamps));
+		shhwtstamps.hwtstamp = timespec64_to_ktime(ts);
+		list_for_each_entry_safe(ts_skb, ts_skb2, &priv->ts_skb_list,
+					 list) {
+			skb = ts_skb->skb;
+			tag = ts_skb->tag;
+			list_del(&ts_skb->list);
+			kfree(ts_skb);
+			if (tag == tfa_tag) {
+				skb_tstamp_tx(skb, &shhwtstamps);
+				break;
+			}
+		}
+		ravb_write(ndev, ravb_read(ndev, TCCR) | TCCR_TFR, TCCR);
+	}
+}
+
+/* Packet receive function for Ethernet AVB */
+static bool ravb_rx(struct net_device *ndev, int *quota, int q)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int entry = priv->cur_rx[q] % priv->num_rx_ring[q];
+	int boguscnt = (priv->dirty_rx[q] + priv->num_rx_ring[q]) -
+			priv->cur_rx[q];
+	struct net_device_stats *stats = &priv->stats[q];
+	struct ravb_ex_rx_desc *desc;
+	struct sk_buff *skb;
+	dma_addr_t dma_addr;
+	struct timespec64 ts;
+	u16 pkt_len = 0;
+	u8  desc_status;
+	int limit;
+
+	boguscnt = min(boguscnt, *quota);
+	limit = boguscnt;
+	desc = &priv->rx_ring[q][entry];
+	while (desc->die_dt != DT_FEMPTY) {
+		/* Descriptor type must be checked before all other reads */
+		dma_rmb();
+		desc_status = desc->msc;
+		pkt_len = le16_to_cpu(desc->ds_cc) & RX_DS;
+
+		if (--boguscnt < 0)
+			break;
+
+		if (desc_status & MSC_MC)
+			stats->multicast++;
+
+		if (desc_status & (MSC_CRC | MSC_RFE | MSC_RTSF | MSC_RTLF |
+				   MSC_CEEF)) {
+			stats->rx_errors++;
+			if (desc_status & MSC_CRC)
+				stats->rx_crc_errors++;
+			if (desc_status & MSC_RFE)
+				stats->rx_frame_errors++;
+			if (desc_status & (MSC_RTLF | MSC_RTSF))
+				stats->rx_length_errors++;
+			if (desc_status & MSC_CEEF)
+				stats->rx_missed_errors++;
+		} else {
+			u32 get_ts = priv->tstamp_rx_ctrl & RAVB_RXTSTAMP_TYPE;
+
+			skb = priv->rx_skb[q][entry];
+			priv->rx_skb[q][entry] = NULL;
+			dma_sync_single_for_cpu(&ndev->dev,
+						le32_to_cpu(desc->dptr),
+						ALIGN(PKT_BUF_SZ, 16),
+						DMA_FROM_DEVICE);
+			get_ts &= (q == RAVB_NC) ?
+					RAVB_RXTSTAMP_TYPE_V2_L2_EVENT :
+					~RAVB_RXTSTAMP_TYPE_V2_L2_EVENT;
+			if (get_ts) {
+				struct skb_shared_hwtstamps *shhwtstamps;
+
+				shhwtstamps = skb_hwtstamps(skb);
+				memset(shhwtstamps, 0, sizeof(*shhwtstamps));
+				ts.tv_sec = ((u64) le16_to_cpu(desc->ts_sh) <<
+					     32) | le32_to_cpu(desc->ts_sl);
+				ts.tv_nsec = le32_to_cpu(desc->ts_n);
+				shhwtstamps->hwtstamp = timespec64_to_ktime(ts);
+			}
+			skb_put(skb, pkt_len);
+			skb->protocol = eth_type_trans(skb, ndev);
+			napi_gro_receive(&priv->napi[q], skb);
+			stats->rx_packets++;
+			stats->rx_bytes += pkt_len;
+		}
+
+		entry = (++priv->cur_rx[q]) % priv->num_rx_ring[q];
+		desc = &priv->rx_ring[q][entry];
+	}
+
+	/* Refill the RX ring buffers. */
+	for (; priv->cur_rx[q] - priv->dirty_rx[q] > 0; priv->dirty_rx[q]++) {
+		entry = priv->dirty_rx[q] % priv->num_rx_ring[q];
+		desc = &priv->rx_ring[q][entry];
+		/* The size of the buffer should be on 16-byte boundary. */
+		desc->ds_cc = cpu_to_le16(ALIGN(PKT_BUF_SZ, 16));
+
+		if (!priv->rx_skb[q][entry]) {
+			skb = netdev_alloc_skb(ndev,
+					       PKT_BUF_SZ + RAVB_ALIGN - 1);
+			if (!skb)
+				break;	/* Better luck next round. */
+			ravb_set_buffer_align(skb);
+			dma_unmap_single(&ndev->dev, le32_to_cpu(desc->dptr),
+					 ALIGN(PKT_BUF_SZ, 16),
+					 DMA_FROM_DEVICE);
+			dma_addr = dma_map_single(&ndev->dev, skb->data,
+						  le16_to_cpu(desc->ds_cc),
+						  DMA_FROM_DEVICE);
+			skb_checksum_none_assert(skb);
+			if (dma_mapping_error(&ndev->dev, dma_addr)) {
+				dev_kfree_skb_any(skb);
+				break;
+			}
+			desc->dptr = cpu_to_le32(dma_addr);
+			priv->rx_skb[q][entry] = skb;
+		}
+		/* Descriptor type must be set after all the above writes */
+		dma_wmb();
+		desc->die_dt = DT_FEMPTY;
+	}
+
+	*quota -= limit - (++boguscnt);
+
+	return boguscnt <= 0;
+}
+
+static void ravb_rcv_snd_disable(struct net_device *ndev)
+{
+	/* Disable TX and RX */
+	ravb_write(ndev, ravb_read(ndev, ECMR) & ~(ECMR_RE | ECMR_TE), ECMR);
+}
+
+static void ravb_rcv_snd_enable(struct net_device *ndev)
+{
+	/* Enable TX and RX */
+	ravb_write(ndev, ravb_read(ndev, ECMR) | ECMR_RE | ECMR_TE, ECMR);
+}
+
+/* function for waiting dma process finished */
+static int ravb_stop_dma(struct net_device *ndev)
+{
+	int error;
+
+	/* Wait for stopping the hardware TX process */
+	error = ravb_wait(ndev, TCCR,
+			  TCCR_TSRQ0 | TCCR_TSRQ1 | TCCR_TSRQ2 | TCCR_TSRQ3, 0);
+	if (error)
+		return error;
+
+	error = ravb_wait(ndev, CSR, CSR_TPO0 | CSR_TPO1 | CSR_TPO2 | CSR_TPO3,
+			  0);
+	if (error)
+		return error;
+
+	/* Stop the E-MAC's RX/TX processes. */
+	ravb_rcv_snd_disable(ndev);
+
+	/* Wait for stopping the RX DMA process */
+	error = ravb_wait(ndev, CSR, CSR_RPO, 0);
+	if (error)
+		return error;
+
+	/* Stop AVB-DMAC process */
+	return ravb_config(ndev);
+}
+
+/* E-MAC interrupt handler */
+static void ravb_emac_interrupt(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	u32 ecsr, psr;
+
+	ecsr = ravb_read(ndev, ECSR);
+	ravb_write(ndev, ecsr, ECSR);	/* clear interrupt */
+	if (ecsr & ECSR_ICD)
+		ndev->stats.tx_carrier_errors++;
+	if (ecsr & ECSR_LCHNG) {
+		/* Link changed */
+		if (priv->no_avb_link)
+			return;
+		psr = ravb_read(ndev, PSR);
+		if (priv->avb_link_active_low)
+			psr ^= PSR_LMON;
+		if (!(psr & PSR_LMON)) {
+			/* DIsable RX and TX */
+			ravb_rcv_snd_disable(ndev);
+		} else {
+			/* Enable RX and TX */
+			ravb_rcv_snd_enable(ndev);
+		}
+	}
+}
+
+/* Error interrupt handler */
+static void ravb_error_interrupt(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	u32 eis, ris2;
+
+	eis = ravb_read(ndev, EIS);
+	ravb_write(ndev, ~EIS_QFS, EIS);
+	if (eis & EIS_QFS) {
+		ris2 = ravb_read(ndev, RIS2);
+		ravb_write(ndev, ~(RIS2_QFF0 | RIS2_RFFF), RIS2);
+
+		/* Receive Descriptor Empty int */
+		if (ris2 & RIS2_QFF0)
+			priv->stats[RAVB_BE].rx_over_errors++;
+
+		    /* Receive Descriptor Empty int */
+		if (ris2 & RIS2_QFF1)
+			priv->stats[RAVB_NC].rx_over_errors++;
+
+		/* Receive FIFO Overflow int */
+		if (ris2 & RIS2_RFFF)
+			priv->rx_fifo_errors++;
+	}
+}
+
+static irqreturn_t ravb_interrupt(int irq, void *dev_id)
+{
+	struct net_device *ndev = dev_id;
+	struct ravb_private *priv = netdev_priv(ndev);
+	irqreturn_t result = IRQ_NONE;
+	u32 iss;
+
+	spin_lock(&priv->lock);
+	/* Get interrupt status */
+	iss = ravb_read(ndev, ISS);
+
+	/* Received and transmitted interrupts */
+	if (iss & (ISS_FRS | ISS_FTS | ISS_TFUS)) {
+		u32 ris0 = ravb_read(ndev, RIS0);
+		u32 ric0 = ravb_read(ndev, RIC0);
+		u32 tis  = ravb_read(ndev, TIS);
+		u32 tic  = ravb_read(ndev, TIC);
+		int q;
+
+		/* Timestamp updated */
+		if (tis & TIS_TFUF) {
+			ravb_write(ndev, ~TIS_TFUF, TIS);
+			ravb_get_tx_tstamp(ndev);
+			result = IRQ_HANDLED;
+		}
+
+		/* Network control and best effort queue RX/TX */
+		for (q = RAVB_NC; q >= RAVB_BE; q--) {
+			if (((ris0 & ric0) & BIT(q)) ||
+			    ((tis  & tic)  & BIT(q))) {
+				if (napi_schedule_prep(&priv->napi[q])) {
+					/* Mask RX and TX interrupts */
+					ravb_write(ndev, ric0 & ~BIT(q), RIC0);
+					ravb_write(ndev, tic  & ~BIT(q), TIC);
+					__napi_schedule(&priv->napi[q]);
+				} else {
+					netdev_warn(ndev,
+						    "ignoring interrupt, rx status 0x%08x, rx mask 0x%08x,\n",
+						    ris0, ric0);
+					netdev_warn(ndev,
+						    "                    tx status 0x%08x, tx mask 0x%08x.\n",
+						    tis, tic);
+				}
+				result = IRQ_HANDLED;
+			}
+		}
+	}
+
+	/* E-MAC status summary */
+	if (iss & ISS_MS) {
+		ravb_emac_interrupt(ndev);
+		result = IRQ_HANDLED;
+	}
+
+	/* Error status summary */
+	if (iss & ISS_ES) {
+		ravb_error_interrupt(ndev);
+		result = IRQ_HANDLED;
+	}
+
+	if (iss & ISS_CGIS)
+		result = ravb_ptp_interrupt(ndev);
+
+	mmiowb();
+	spin_unlock(&priv->lock);
+	return result;
+}
+
+static int ravb_poll(struct napi_struct *napi, int budget)
+{
+	struct net_device *ndev = napi->dev;
+	struct ravb_private *priv = netdev_priv(ndev);
+	unsigned long flags;
+	int q = napi - priv->napi;
+	int mask = BIT(q);
+	int quota = budget;
+	u32 ris0, tis;
+
+	for (;;) {
+		tis = ravb_read(ndev, TIS);
+		ris0 = ravb_read(ndev, RIS0);
+		if (!((ris0 & mask) || (tis & mask)))
+			break;
+
+		/* Processing RX Descriptor Ring */
+		if (ris0 & mask) {
+			/* Clear RX interrupt */
+			ravb_write(ndev, ~mask, RIS0);
+			if (ravb_rx(ndev, &quota, q))
+				goto out;
+		}
+		/* Processing TX Descriptor Ring */
+		if (tis & mask) {
+			spin_lock_irqsave(&priv->lock, flags);
+			/* Clear TX interrupt */
+			ravb_write(ndev, ~mask, TIS);
+			ravb_tx_free(ndev, q);
+			netif_wake_subqueue(ndev, q);
+			mmiowb();
+			spin_unlock_irqrestore(&priv->lock, flags);
+		}
+	}
+
+	napi_complete(napi);
+
+	/* Re-enable RX/TX interrupts */
+	spin_lock_irqsave(&priv->lock, flags);
+	ravb_write(ndev, ravb_read(ndev, RIC0) | mask, RIC0);
+	ravb_write(ndev, ravb_read(ndev, TIC)  | mask,  TIC);
+	mmiowb();
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Receive error message handling */
+	priv->rx_over_errors =  priv->stats[RAVB_BE].rx_over_errors;
+	priv->rx_over_errors += priv->stats[RAVB_NC].rx_over_errors;
+	if (priv->rx_over_errors != ndev->stats.rx_over_errors) {
+		ndev->stats.rx_over_errors = priv->rx_over_errors;
+		netif_err(priv, rx_err, ndev, "Receive Descriptor Empty\n");
+	}
+	if (priv->rx_fifo_errors != ndev->stats.rx_fifo_errors) {
+		ndev->stats.rx_fifo_errors = priv->rx_fifo_errors;
+		netif_err(priv, rx_err, ndev, "Receive FIFO Overflow\n");
+	}
+out:
+	return budget - quota;
+}
+
+/* PHY state control function */
+static void ravb_adjust_link(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct phy_device *phydev = priv->phydev;
+	bool new_state = false;
+
+	if (phydev->link) {
+		if (phydev->duplex != priv->duplex) {
+			new_state = true;
+			priv->duplex = phydev->duplex;
+			ravb_set_duplex(ndev);
+		}
+
+		if (phydev->speed != priv->speed) {
+			new_state = true;
+			priv->speed = phydev->speed;
+			ravb_set_rate(ndev);
+		}
+		if (!priv->link) {
+			ravb_write(ndev, ravb_read(ndev, ECMR) & ~ECMR_TXF,
+				   ECMR);
+			new_state = true;
+			priv->link = phydev->link;
+			if (priv->no_avb_link)
+				ravb_rcv_snd_enable(ndev);
+		}
+	} else if (priv->link) {
+		new_state = true;
+		priv->link = 0;
+		priv->speed = 0;
+		priv->duplex = -1;
+		if (priv->no_avb_link)
+			ravb_rcv_snd_disable(ndev);
+	}
+
+	if (new_state && netif_msg_link(priv))
+		phy_print_status(phydev);
+}
+
+/* PHY init function */
+static int ravb_phy_init(struct net_device *ndev)
+{
+	struct device_node *np = ndev->dev.parent->of_node;
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct phy_device *phydev;
+	struct device_node *pn;
+
+	priv->link = 0;
+	priv->speed = 0;
+	priv->duplex = -1;
+
+	/* Try connecting to PHY */
+	pn = of_parse_phandle(np, "phy-handle", 0);
+	phydev = of_phy_connect(ndev, pn, ravb_adjust_link, 0,
+				priv->phy_interface);
+	if (!phydev) {
+		netdev_err(ndev, "failed to connect PHY\n");
+		return -ENOENT;
+	}
+
+	netdev_info(ndev, "attached PHY %d (IRQ %d) to driver %s\n",
+		    phydev->addr, phydev->irq, phydev->drv->name);
+
+	priv->phydev = phydev;
+
+	return 0;
+}
+
+/* PHY control start function */
+static int ravb_phy_start(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int error;
+
+	error = ravb_phy_init(ndev);
+	if (error)
+		return error;
+
+	phy_start(priv->phydev);
+
+	return 0;
+}
+
+static int ravb_get_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int error = -ENODEV;
+	unsigned long flags;
+
+	if (priv->phydev) {
+		spin_lock_irqsave(&priv->lock, flags);
+		error = phy_ethtool_gset(priv->phydev, ecmd);
+		spin_unlock_irqrestore(&priv->lock, flags);
+	}
+
+	return error;
+}
+
+static int ravb_set_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	unsigned long flags;
+	int error;
+
+	if (!priv->phydev)
+		return -ENODEV;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Disable TX and RX */
+	ravb_rcv_snd_disable(ndev);
+
+	error = phy_ethtool_sset(priv->phydev, ecmd);
+	if (error)
+		goto error_exit;
+
+	if (ecmd->duplex == DUPLEX_FULL)
+		priv->duplex = 1;
+	else
+		priv->duplex = 0;
+
+	ravb_set_duplex(ndev);
+
+error_exit:
+	mdelay(1);
+
+	/* Enable TX and RX */
+	ravb_rcv_snd_enable(ndev);
+
+	mmiowb();
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return error;
+}
+
+static int ravb_nway_reset(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int error = -ENODEV;
+	unsigned long flags;
+
+	if (priv->phydev) {
+		spin_lock_irqsave(&priv->lock, flags);
+		error = phy_start_aneg(priv->phydev);
+		spin_unlock_irqrestore(&priv->lock, flags);
+	}
+
+	return error;
+}
+
+static u32 ravb_get_msglevel(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	return priv->msg_enable;
+}
+
+static void ravb_set_msglevel(struct net_device *ndev, u32 value)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	priv->msg_enable = value;
+}
+
+static const char ravb_gstrings_stats[][ETH_GSTRING_LEN] = {
+	"rx_queue_0_current",
+	"tx_queue_0_current",
+	"rx_queue_0_dirty",
+	"tx_queue_0_dirty",
+	"rx_queue_0_packets",
+	"tx_queue_0_packets",
+	"rx_queue_0_bytes",
+	"tx_queue_0_bytes",
+	"rx_queue_0_mcast_packets",
+	"rx_queue_0_errors",
+	"rx_queue_0_crc_errors",
+	"rx_queue_0_frame_errors",
+	"rx_queue_0_length_errors",
+	"rx_queue_0_missed_errors",
+	"rx_queue_0_over_errors",
+
+	"rx_queue_1_current",
+	"tx_queue_1_current",
+	"rx_queue_1_dirty",
+	"tx_queue_1_dirty",
+	"rx_queue_1_packets",
+	"tx_queue_1_packets",
+	"rx_queue_1_bytes",
+	"tx_queue_1_bytes",
+	"rx_queue_1_mcast_packets",
+	"rx_queue_1_errors",
+	"rx_queue_1_crc_errors",
+	"rx_queue_1_frame_errors_",
+	"rx_queue_1_length_errors",
+	"rx_queue_1_missed_errors",
+	"rx_queue_1_over_errors",
+};
+
+#define RAVB_STATS_LEN	ARRAY_SIZE(ravb_gstrings_stats)
+
+static int ravb_get_sset_count(struct net_device *netdev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_STATS:
+		return RAVB_STATS_LEN;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void ravb_get_ethtool_stats(struct net_device *ndev,
+				   struct ethtool_stats *stats, u64 *data)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int i = 0;
+	int q;
+
+	/* Device-specific stats */
+	for (q = RAVB_BE; q < NUM_RX_QUEUE; q++) {
+		struct net_device_stats *stats = &priv->stats[q];
+
+		data[i++] = priv->cur_rx[q];
+		data[i++] = priv->cur_tx[q];
+		data[i++] = priv->dirty_rx[q];
+		data[i++] = priv->dirty_tx[q];
+		data[i++] = stats->rx_packets;
+		data[i++] = stats->tx_packets;
+		data[i++] = stats->rx_bytes;
+		data[i++] = stats->tx_bytes;
+		data[i++] = stats->multicast;
+		data[i++] = stats->rx_errors;
+		data[i++] = stats->rx_crc_errors;
+		data[i++] = stats->rx_frame_errors;
+		data[i++] = stats->rx_length_errors;
+		data[i++] = stats->rx_missed_errors;
+		data[i++] = stats->rx_over_errors;
+	}
+}
+
+static void ravb_get_strings(struct net_device *ndev, u32 stringset, u8 *data)
+{
+	switch (stringset) {
+	case ETH_SS_STATS:
+		memcpy(data, *ravb_gstrings_stats, sizeof(ravb_gstrings_stats));
+		break;
+	}
+}
+
+static void ravb_get_ringparam(struct net_device *ndev,
+			       struct ethtool_ringparam *ring)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	ring->rx_max_pending = BE_RX_RING_MAX;
+	ring->tx_max_pending = BE_TX_RING_MAX;
+	ring->rx_pending = priv->num_rx_ring[RAVB_BE];
+	ring->tx_pending = priv->num_tx_ring[RAVB_BE];
+}
+
+static int ravb_set_ringparam(struct net_device *ndev,
+			      struct ethtool_ringparam *ring)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int error;
+
+	if (ring->tx_pending > BE_TX_RING_MAX ||
+	    ring->rx_pending > BE_RX_RING_MAX ||
+	    ring->tx_pending < BE_TX_RING_MIN ||
+	    ring->rx_pending < BE_RX_RING_MIN)
+		return -EINVAL;
+	if (ring->rx_mini_pending || ring->rx_jumbo_pending)
+		return -EINVAL;
+
+	if (netif_running(ndev)) {
+		netif_device_detach(ndev);
+		/* Stop PTP Clock driver */
+		ravb_ptp_stop(ndev);
+		/* Wait for DMA stopping */
+		error = ravb_stop_dma(ndev);
+		if (error) {
+			netdev_err(ndev,
+				   "cannot set ringparam! Any AVB processes are still running?\n");
+			return error;
+		}
+		synchronize_irq(ndev->irq);
+
+		/* Free all the skb's in the RX queue and the DMA buffers. */
+		ravb_ring_free(ndev, RAVB_BE);
+		ravb_ring_free(ndev, RAVB_NC);
+	}
+
+	/* Set new parameters */
+	priv->num_rx_ring[RAVB_BE] = ring->rx_pending;
+	priv->num_tx_ring[RAVB_BE] = ring->tx_pending;
+
+	if (netif_running(ndev)) {
+		error = ravb_dmac_init(ndev);
+		if (error) {
+			netdev_err(ndev,
+				   "%s: ravb_dmac_init() failed, error %d\n",
+				   __func__, error);
+			return error;
+		}
+
+		ravb_emac_init(ndev);
+
+		/* Initialise PTP Clock driver */
+		ravb_ptp_init(ndev, priv->pdev);
+
+		netif_device_attach(ndev);
+	}
+
+	return 0;
+}
+
+static int ravb_get_ts_info(struct net_device *ndev,
+			    struct ethtool_ts_info *info)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_SOFTWARE |
+		SOF_TIMESTAMPING_RX_SOFTWARE |
+		SOF_TIMESTAMPING_SOFTWARE |
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+	info->tx_types = (1 << HWTSTAMP_TX_OFF) | (1 << HWTSTAMP_TX_ON);
+	info->rx_filters =
+		(1 << HWTSTAMP_FILTER_NONE) |
+		(1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
+		(1 << HWTSTAMP_FILTER_ALL);
+	info->phc_index = ptp_clock_index(priv->ptp.clock);
+
+	return 0;
+}
+
+static const struct ethtool_ops ravb_ethtool_ops = {
+	.get_settings		= ravb_get_settings,
+	.set_settings		= ravb_set_settings,
+	.nway_reset		= ravb_nway_reset,
+	.get_msglevel		= ravb_get_msglevel,
+	.set_msglevel		= ravb_set_msglevel,
+	.get_link		= ethtool_op_get_link,
+	.get_strings		= ravb_get_strings,
+	.get_ethtool_stats	= ravb_get_ethtool_stats,
+	.get_sset_count		= ravb_get_sset_count,
+	.get_ringparam		= ravb_get_ringparam,
+	.set_ringparam		= ravb_set_ringparam,
+	.get_ts_info		= ravb_get_ts_info,
+};
+
+/* Network device open function for Ethernet AVB */
+static int ravb_open(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	int error;
+
+	napi_enable(&priv->napi[RAVB_BE]);
+	napi_enable(&priv->napi[RAVB_NC]);
+
+	error = request_irq(ndev->irq, ravb_interrupt, IRQF_SHARED, ndev->name,
+			    ndev);
+	if (error) {
+		netdev_err(ndev, "cannot request IRQ\n");
+		goto out_napi_off;
+	}
+
+	/* Device init */
+	error = ravb_dmac_init(ndev);
+	if (error)
+		goto out_free_irq;
+	ravb_emac_init(ndev);
+
+	/* Initialise PTP Clock driver */
+	ravb_ptp_init(ndev, priv->pdev);
+
+	netif_tx_start_all_queues(ndev);
+
+	/* PHY control start */
+	error = ravb_phy_start(ndev);
+	if (error)
+		goto out_ptp_stop;
+
+	return 0;
+
+out_ptp_stop:
+	/* Stop PTP Clock driver */
+	ravb_ptp_stop(ndev);
+out_free_irq:
+	free_irq(ndev->irq, ndev);
+out_napi_off:
+	napi_disable(&priv->napi[RAVB_NC]);
+	napi_disable(&priv->napi[RAVB_BE]);
+	return error;
+}
+
+/* Timeout function for Ethernet AVB */
+static void ravb_tx_timeout(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	netif_err(priv, tx_err, ndev,
+		  "transmit timed out, status %08x, resetting...\n",
+		  ravb_read(ndev, ISS));
+
+	/* tx_errors count up */
+	ndev->stats.tx_errors++;
+
+	schedule_work(&priv->work);
+}
+
+static void ravb_tx_timeout_work(struct work_struct *work)
+{
+	struct ravb_private *priv = container_of(work, struct ravb_private,
+						 work);
+	struct net_device *ndev = priv->ndev;
+
+	netif_tx_stop_all_queues(ndev);
+
+	/* Stop PTP Clock driver */
+	ravb_ptp_stop(ndev);
+
+	/* Wait for DMA stopping */
+	ravb_stop_dma(ndev);
+
+	ravb_ring_free(ndev, RAVB_BE);
+	ravb_ring_free(ndev, RAVB_NC);
+
+	/* Device init */
+	ravb_dmac_init(ndev);
+	ravb_emac_init(ndev);
+
+	/* Initialise PTP Clock driver */
+	ravb_ptp_init(ndev, priv->pdev);
+
+	netif_tx_start_all_queues(ndev);
+}
+
+/* Packet transmit function for Ethernet AVB */
+static netdev_tx_t ravb_start_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct ravb_tstamp_skb *ts_skb = NULL;
+	u16 q = skb_get_queue_mapping(skb);
+	struct ravb_tx_desc *desc;
+	unsigned long flags;
+	u32 dma_addr;
+	void *buffer;
+	u32 entry;
+	u32 tccr;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (priv->cur_tx[q] - priv->dirty_tx[q] >= priv->num_tx_ring[q]) {
+		netif_err(priv, tx_queued, ndev,
+			  "still transmitting with the full ring!\n");
+		netif_stop_subqueue(ndev, q);
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return NETDEV_TX_BUSY;
+	}
+	entry = priv->cur_tx[q] % priv->num_tx_ring[q];
+	priv->tx_skb[q][entry] = skb;
+
+	if (skb_put_padto(skb, ETH_ZLEN))
+		goto drop;
+
+	buffer = PTR_ALIGN(priv->tx_buffers[q][entry], RAVB_ALIGN);
+	memcpy(buffer, skb->data, skb->len);
+	desc = &priv->tx_ring[q][entry];
+	desc->ds_tagl = cpu_to_le16(skb->len);
+	dma_addr = dma_map_single(&ndev->dev, buffer, skb->len, DMA_TO_DEVICE);
+	if (dma_mapping_error(&ndev->dev, dma_addr))
+		goto drop;
+	desc->dptr = cpu_to_le32(dma_addr);
+
+	/* TX timestamp required */
+	if (q == RAVB_NC) {
+		ts_skb = kmalloc(sizeof(*ts_skb), GFP_ATOMIC);
+		if (!ts_skb) {
+			dma_unmap_single(&ndev->dev, dma_addr, skb->len,
+					 DMA_TO_DEVICE);
+			goto drop;
+		}
+		ts_skb->skb = skb;
+		ts_skb->tag = priv->ts_skb_tag++;
+		priv->ts_skb_tag &= 0x3ff;
+		list_add_tail(&ts_skb->list, &priv->ts_skb_list);
+
+		/* TAG and timestamp required flag */
+		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+		skb_tx_timestamp(skb);
+		desc->tagh_tsr = (ts_skb->tag >> 4) | TX_TSR;
+		desc->ds_tagl |= le16_to_cpu(ts_skb->tag << 12);
+	}
+
+	/* Descriptor type must be set after all the above writes */
+	dma_wmb();
+	desc->die_dt = DT_FSINGLE;
+
+	tccr = ravb_read(ndev, TCCR);
+	if (!(tccr & (TCCR_TSRQ0 << q)))
+		ravb_write(ndev, tccr | (TCCR_TSRQ0 << q), TCCR);
+
+	priv->cur_tx[q]++;
+	if (priv->cur_tx[q] - priv->dirty_tx[q] >= priv->num_tx_ring[q] &&
+	    !ravb_tx_free(ndev, q))
+		netif_stop_subqueue(ndev, q);
+
+exit:
+	mmiowb();
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return NETDEV_TX_OK;
+
+drop:
+	dev_kfree_skb_any(skb);
+	priv->tx_skb[q][entry] = NULL;
+	goto exit;
+}
+
+static u16 ravb_select_queue(struct net_device *ndev, struct sk_buff *skb,
+			     void *accel_priv, select_queue_fallback_t fallback)
+{
+	/* If skb needs TX timestamp, it is handled in network control queue */
+	return (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) ? RAVB_NC :
+							       RAVB_BE;
+
+}
+
+static struct net_device_stats *ravb_get_stats(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct net_device_stats *nstats, *stats0, *stats1;
+
+	nstats = &ndev->stats;
+	stats0 = &priv->stats[RAVB_BE];
+	stats1 = &priv->stats[RAVB_NC];
+
+	nstats->tx_dropped += ravb_read(ndev, TROCR);
+	ravb_write(ndev, 0, TROCR);	/* (write clear) */
+	nstats->collisions += ravb_read(ndev, CDCR);
+	ravb_write(ndev, 0, CDCR);	/* (write clear) */
+	nstats->tx_carrier_errors += ravb_read(ndev, LCCR);
+	ravb_write(ndev, 0, LCCR);	/* (write clear) */
+
+	nstats->tx_carrier_errors += ravb_read(ndev, CERCR);
+	ravb_write(ndev, 0, CERCR);	/* (write clear) */
+	nstats->tx_carrier_errors += ravb_read(ndev, CEECR);
+	ravb_write(ndev, 0, CEECR);	/* (write clear) */
+
+	nstats->rx_packets = stats0->rx_packets + stats1->rx_packets;
+	nstats->tx_packets = stats0->tx_packets + stats1->tx_packets;
+	nstats->rx_bytes = stats0->rx_bytes + stats1->rx_bytes;
+	nstats->tx_bytes = stats0->tx_bytes + stats1->tx_bytes;
+	nstats->multicast = stats0->multicast + stats1->multicast;
+	nstats->rx_errors = stats0->rx_errors + stats1->rx_errors;
+	nstats->rx_crc_errors = stats0->rx_crc_errors + stats1->rx_crc_errors;
+	nstats->rx_frame_errors =
+		stats0->rx_frame_errors + stats1->rx_frame_errors;
+	nstats->rx_length_errors =
+		stats0->rx_length_errors + stats1->rx_length_errors;
+	nstats->rx_missed_errors =
+		stats0->rx_missed_errors + stats1->rx_missed_errors;
+	nstats->rx_over_errors =
+		stats0->rx_over_errors + stats1->rx_over_errors;
+
+	return nstats;
+}
+
+/* Update promiscuous bit */
+static void ravb_set_rx_mode(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	unsigned long flags;
+	u32 ecmr;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ecmr = ravb_read(ndev, ECMR);
+	if (ndev->flags & IFF_PROMISC)
+		ecmr |=  ECMR_PRM;
+	else
+		ecmr &= ~ECMR_PRM;
+	ravb_write(ndev, ecmr, ECMR);
+	mmiowb();
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+/* Device close function for Ethernet AVB */
+static int ravb_close(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct ravb_tstamp_skb *ts_skb, *ts_skb2;
+
+	netif_tx_stop_all_queues(ndev);
+
+	/* Disable interrupts by clearing the interrupt masks. */
+	ravb_write(ndev, 0, RIC0);
+	ravb_write(ndev, 0, RIC1);
+	ravb_write(ndev, 0, RIC2);
+	ravb_write(ndev, 0, TIC);
+
+	/* Stop PTP Clock driver */
+	ravb_ptp_stop(ndev);
+
+	/* Set the config mode to stop the AVB-DMAC's processes */
+	if (ravb_stop_dma(ndev) < 0)
+		netdev_err(ndev,
+			   "device will be stopped after h/w processes are done.\n");
+
+	/* Clear the timestamp list */
+	list_for_each_entry_safe(ts_skb, ts_skb2, &priv->ts_skb_list, list) {
+		list_del(&ts_skb->list);
+		kfree(ts_skb);
+	}
+
+	/* PHY disconnect */
+	if (priv->phydev) {
+		phy_stop(priv->phydev);
+		phy_disconnect(priv->phydev);
+		priv->phydev = NULL;
+	}
+
+	free_irq(ndev->irq, ndev);
+
+	napi_disable(&priv->napi[RAVB_NC]);
+	napi_disable(&priv->napi[RAVB_BE]);
+
+	/* Free all the skb's in the RX queue and the DMA buffers. */
+	ravb_ring_free(ndev, RAVB_BE);
+	ravb_ring_free(ndev, RAVB_NC);
+
+	return 0;
+}
+
+static int ravb_hwtstamp_get(struct net_device *ndev, struct ifreq *req)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct hwtstamp_config config;
+
+	config.flags = 0;
+	config.tx_type = priv->tstamp_tx_ctrl ? HWTSTAMP_TX_ON :
+						HWTSTAMP_TX_OFF;
+	if (priv->tstamp_rx_ctrl & RAVB_RXTSTAMP_TYPE_V2_L2_EVENT)
+		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
+	else if (priv->tstamp_rx_ctrl & RAVB_RXTSTAMP_TYPE_ALL)
+		config.rx_filter = HWTSTAMP_FILTER_ALL;
+	else
+		config.rx_filter = HWTSTAMP_FILTER_NONE;
+
+	return copy_to_user(req->ifr_data, &config, sizeof(config)) ?
+		-EFAULT : 0;
+}
+
+/* Control hardware time stamping */
+static int ravb_hwtstamp_set(struct net_device *ndev, struct ifreq *req)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct hwtstamp_config config;
+	u32 tstamp_rx_ctrl = RAVB_RXTSTAMP_ENABLED;
+	u32 tstamp_tx_ctrl;
+
+	if (copy_from_user(&config, req->ifr_data, sizeof(config)))
+		return -EFAULT;
+
+	/* Reserved for future extensions */
+	if (config.flags)
+		return -EINVAL;
+
+	switch (config.tx_type) {
+	case HWTSTAMP_TX_OFF:
+		tstamp_tx_ctrl = 0;
+		break;
+	case HWTSTAMP_TX_ON:
+		tstamp_tx_ctrl = RAVB_TXTSTAMP_ENABLED;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (config.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		tstamp_rx_ctrl = 0;
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+		tstamp_rx_ctrl |= RAVB_RXTSTAMP_TYPE_V2_L2_EVENT;
+		break;
+	default:
+		config.rx_filter = HWTSTAMP_FILTER_ALL;
+		tstamp_rx_ctrl |= RAVB_RXTSTAMP_TYPE_ALL;
+	}
+
+	priv->tstamp_tx_ctrl = tstamp_tx_ctrl;
+	priv->tstamp_rx_ctrl = tstamp_rx_ctrl;
+
+	return copy_to_user(req->ifr_data, &config, sizeof(config)) ?
+		-EFAULT : 0;
+}
+
+/* ioctl to device function */
+static int ravb_do_ioctl(struct net_device *ndev, struct ifreq *req, int cmd)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	struct phy_device *phydev = priv->phydev;
+
+	if (!netif_running(ndev))
+		return -EINVAL;
+
+	if (!phydev)
+		return -ENODEV;
+
+	switch (cmd) {
+	case SIOCGHWTSTAMP:
+		return ravb_hwtstamp_get(ndev, req);
+	case SIOCSHWTSTAMP:
+		return ravb_hwtstamp_set(ndev, req);
+	}
+
+	return phy_mii_ioctl(phydev, req, cmd);
+}
+
+static const struct net_device_ops ravb_netdev_ops = {
+	.ndo_open		= ravb_open,
+	.ndo_stop		= ravb_close,
+	.ndo_start_xmit		= ravb_start_xmit,
+	.ndo_select_queue	= ravb_select_queue,
+	.ndo_get_stats		= ravb_get_stats,
+	.ndo_set_rx_mode	= ravb_set_rx_mode,
+	.ndo_tx_timeout		= ravb_tx_timeout,
+	.ndo_do_ioctl		= ravb_do_ioctl,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_set_mac_address	= eth_mac_addr,
+	.ndo_change_mtu		= eth_change_mtu,
+};
+
+/* MDIO bus init function */
+static int ravb_mdio_init(struct ravb_private *priv)
+{
+	struct platform_device *pdev = priv->pdev;
+	struct device *dev = &pdev->dev;
+	int error;
+
+	/* Bitbang init */
+	priv->mdiobb.ops = &bb_ops;
+
+	/* MII controller setting */
+	priv->mii_bus = alloc_mdio_bitbang(&priv->mdiobb);
+	if (!priv->mii_bus)
+		return -ENOMEM;
+
+	/* Hook up MII support for ethtool */
+	priv->mii_bus->name = "ravb_mii";
+	priv->mii_bus->parent = dev;
+	snprintf(priv->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
+		 pdev->name, pdev->id);
+
+	/* Register MDIO bus */
+	error = of_mdiobus_register(priv->mii_bus, dev->of_node);
+	if (error)
+		goto out_free_bus;
+
+	return 0;
+
+out_free_bus:
+	free_mdio_bitbang(priv->mii_bus);
+	return error;
+}
+
+/* MDIO bus release function */
+static int ravb_mdio_release(struct ravb_private *priv)
+{
+	/* Unregister mdio bus */
+	mdiobus_unregister(priv->mii_bus);
+
+	/* Free bitbang info */
+	free_mdio_bitbang(priv->mii_bus);
+
+	return 0;
+}
+
+static int ravb_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct ravb_private *priv;
+	struct net_device *ndev;
+	int error, irq, q;
+	struct resource *res;
+
+	if (!np) {
+		dev_err(&pdev->dev,
+			"this driver is required to be instantiated from device tree\n");
+		return -EINVAL;
+	}
+
+	/* Get base address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "invalid resource\n");
+		return -EINVAL;
+	}
+
+	ndev = alloc_etherdev_mqs(sizeof(struct ravb_private),
+				  NUM_TX_QUEUE, NUM_RX_QUEUE);
+	if (!ndev)
+		return -ENOMEM;
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
+
+	/* The Ether-specific entries in the device structure. */
+	ndev->base_addr = res->start;
+	ndev->dma = -1;
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		error = -ENODEV;
+		goto out_release;
+	}
+	ndev->irq = irq;
+
+	SET_NETDEV_DEV(ndev, &pdev->dev);
+
+	priv = netdev_priv(ndev);
+	priv->ndev = ndev;
+	priv->pdev = pdev;
+	priv->num_tx_ring[RAVB_BE] = BE_TX_RING_SIZE;
+	priv->num_rx_ring[RAVB_BE] = BE_RX_RING_SIZE;
+	priv->num_tx_ring[RAVB_NC] = NC_TX_RING_SIZE;
+	priv->num_rx_ring[RAVB_NC] = NC_RX_RING_SIZE;
+	priv->addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->addr)) {
+		error = PTR_ERR(priv->addr);
+		goto out_release;
+	}
+
+	spin_lock_init(&priv->lock);
+	INIT_WORK(&priv->work, ravb_tx_timeout_work);
+
+	priv->phy_interface = of_get_phy_mode(np);
+
+	priv->no_avb_link = of_property_read_bool(np, "renesas,no-ether-link");
+	priv->avb_link_active_low =
+		of_property_read_bool(np, "renesas,ether-link-active-low");
+
+	/* Set function */
+	ndev->netdev_ops = &ravb_netdev_ops;
+	ndev->ethtool_ops = &ravb_ethtool_ops;
+
+	/* Set AVB config mode */
+	ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_OPC) | CCC_OPC_CONFIG,
+		   CCC);
+
+	/* Set CSEL value */
+	ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_CSEL) | CCC_CSEL_HPB,
+		   CCC);
+
+	/* Set GTI value */
+	ravb_write(ndev, ((1000 << 20) / 130) & GTI_TIV, GTI);
+
+	/* Request GTI loading */
+	ravb_write(ndev, ravb_read(ndev, GCCR) | GCCR_LTI, GCCR);
+
+	/* Allocate descriptor base address table */
+	priv->desc_bat_size = sizeof(struct ravb_desc) * DBAT_ENTRY_NUM;
+	priv->desc_bat = dma_alloc_coherent(NULL, priv->desc_bat_size,
+					    &priv->desc_bat_dma, GFP_KERNEL);
+	if (!priv->desc_bat) {
+		dev_err(&ndev->dev,
+			"Cannot allocate desc base address table (size %d bytes)\n",
+			priv->desc_bat_size);
+		error = -ENOMEM;
+		goto out_release;
+	}
+	for (q = RAVB_BE; q < DBAT_ENTRY_NUM; q++)
+		priv->desc_bat[q].die_dt = DT_EOS;
+	ravb_write(ndev, priv->desc_bat_dma, DBAT);
+
+	/* Initialise HW timestamp list */
+	INIT_LIST_HEAD(&priv->ts_skb_list);
+
+	/* Debug message level */
+	priv->msg_enable = RAVB_DEF_MSG_ENABLE;
+
+	/* Read and set MAC address */
+	ravb_read_mac_address(ndev, of_get_mac_address(np));
+	if (!is_valid_ether_addr(ndev->dev_addr)) {
+		dev_warn(&pdev->dev,
+			 "no valid MAC address supplied, using a random one\n");
+		eth_hw_addr_random(ndev);
+	}
+
+	/* MDIO bus init */
+	error = ravb_mdio_init(priv);
+	if (error) {
+		dev_err(&ndev->dev, "failed to initialize MDIO\n");
+		goto out_dma_free;
+	}
+
+	netif_napi_add(ndev, &priv->napi[RAVB_BE], ravb_poll, 64);
+	netif_napi_add(ndev, &priv->napi[RAVB_NC], ravb_poll, 64);
+
+	/* Network device register */
+	error = register_netdev(ndev);
+	if (error)
+		goto out_napi_del;
+
+	/* Print device information */
+	netdev_info(ndev, "Base address at %#x, %pM, IRQ %d.\n",
+		    (u32)ndev->base_addr, ndev->dev_addr, ndev->irq);
+
+	platform_set_drvdata(pdev, ndev);
+
+	return 0;
+
+out_napi_del:
+	netif_napi_del(&priv->napi[RAVB_NC]);
+	netif_napi_del(&priv->napi[RAVB_BE]);
+	ravb_mdio_release(priv);
+out_dma_free:
+	dma_free_coherent(NULL, priv->desc_bat_size, priv->desc_bat,
+			  priv->desc_bat_dma);
+out_release:
+	if (ndev)
+		free_netdev(ndev);
+
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	return error;
+}
+
+static int ravb_remove(struct platform_device *pdev)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	dma_free_coherent(NULL, priv->desc_bat_size, priv->desc_bat,
+			  priv->desc_bat_dma);
+	/* Set reset mode */
+	ravb_write(ndev, CCC_OPC_RESET, CCC);
+	pm_runtime_put_sync(&pdev->dev);
+	unregister_netdev(ndev);
+	netif_napi_del(&priv->napi[RAVB_NC]);
+	netif_napi_del(&priv->napi[RAVB_BE]);
+	ravb_mdio_release(priv);
+	pm_runtime_disable(&pdev->dev);
+	free_netdev(ndev);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int ravb_runtime_nop(struct device *dev)
+{
+	/* Runtime PM callback shared between ->runtime_suspend()
+	 * and ->runtime_resume(). Simply returns success.
+	 *
+	 * This driver re-initializes all registers after
+	 * pm_runtime_get_sync() anyway so there is no need
+	 * to save and restore registers here.
+	 */
+	return 0;
+}
+
+static const struct dev_pm_ops ravb_dev_pm_ops = {
+	.runtime_suspend = ravb_runtime_nop,
+	.runtime_resume = ravb_runtime_nop,
+};
+
+#define RAVB_PM_OPS (&ravb_dev_pm_ops)
+#else
+#define RAVB_PM_OPS NULL
+#endif
+
+static const struct of_device_id ravb_match_table[] = {
+	{ .compatible = "renesas,etheravb-r8a7790" },
+	{ .compatible = "renesas,etheravb-r8a7794" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ravb_match_table);
+
+static struct platform_driver ravb_driver = {
+	.probe		= ravb_probe,
+	.remove		= ravb_remove,
+	.driver = {
+		.name	= "ravb",
+		.pm	= RAVB_PM_OPS,
+		.of_match_table = ravb_match_table,
+	},
+};
+
+module_platform_driver(ravb_driver);
+
+MODULE_AUTHOR("Mitsuhiro Kimura, Masaru Nagai");
+MODULE_DESCRIPTION("Renesas Ethernet AVB driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/ethernet/renesas/ravb_ptp.c b/drivers/net/ethernet/renesas/ravb_ptp.c
new file mode 100644
index 000000000000..7a8ce920c49e
--- /dev/null
+++ b/drivers/net/ethernet/renesas/ravb_ptp.c
@@ -0,0 +1,359 @@
+/* PTP 1588 clock using the Renesas Ethernet AVB
+ *
+ * Copyright (C) 2013-2015 Renesas Electronics Corporation
+ * Copyright (C) 2015 Renesas Solutions Corp.
+ * Copyright (C) 2015 Cogent Embedded, Inc. <source@cogentembedded.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 "ravb.h"
+
+static int ravb_ptp_tcr_request(struct ravb_private *priv, u32 request)
+{
+	struct net_device *ndev = priv->ndev;
+	int error;
+
+	error = ravb_wait(ndev, GCCR, GCCR_TCR, GCCR_TCR_NOREQ);
+	if (error)
+		return error;
+
+	ravb_write(ndev, ravb_read(ndev, GCCR) | request, GCCR);
+	return ravb_wait(ndev, GCCR, GCCR_TCR, GCCR_TCR_NOREQ);
+}
+
+/* Caller must hold the lock */
+static int ravb_ptp_time_read(struct ravb_private *priv, struct timespec64 *ts)
+{
+	struct net_device *ndev = priv->ndev;
+	int error;
+
+	error = ravb_ptp_tcr_request(priv, GCCR_TCR_CAPTURE);
+	if (error)
+		return error;
+
+	ts->tv_nsec = ravb_read(ndev, GCT0);
+	ts->tv_sec  = ravb_read(ndev, GCT1) |
+		((s64)ravb_read(ndev, GCT2) << 32);
+
+	return 0;
+}
+
+/* Caller must hold the lock */
+static int ravb_ptp_time_write(struct ravb_private *priv,
+				const struct timespec64 *ts)
+{
+	struct net_device *ndev = priv->ndev;
+	int error;
+	u32 gccr;
+
+	error = ravb_ptp_tcr_request(priv, GCCR_TCR_RESET);
+	if (error)
+		return error;
+
+	gccr = ravb_read(ndev, GCCR);
+	if (gccr & GCCR_LTO)
+		return -EBUSY;
+	ravb_write(ndev, ts->tv_nsec, GTO0);
+	ravb_write(ndev, ts->tv_sec,  GTO1);
+	ravb_write(ndev, (ts->tv_sec >> 32) & 0xffff, GTO2);
+	ravb_write(ndev, gccr | GCCR_LTO, GCCR);
+
+	return 0;
+}
+
+/* Caller must hold the lock */
+static int ravb_ptp_update_compare(struct ravb_private *priv, u32 ns)
+{
+	struct net_device *ndev = priv->ndev;
+	/* When the comparison value (GPTC.PTCV) is in range of
+	 * [x-1 to x+1] (x is the configured increment value in
+	 * GTI.TIV), it may happen that a comparison match is
+	 * not detected when the timer wraps around.
+	 */
+	u32 gti_ns_plus_1 = (priv->ptp.current_addend >> 20) + 1;
+	u32 gccr;
+
+	if (ns < gti_ns_plus_1)
+		ns = gti_ns_plus_1;
+	else if (ns > 0 - gti_ns_plus_1)
+		ns = 0 - gti_ns_plus_1;
+
+	gccr = ravb_read(ndev, GCCR);
+	if (gccr & GCCR_LPTC)
+		return -EBUSY;
+	ravb_write(ndev, ns, GPTC);
+	ravb_write(ndev, gccr | GCCR_LPTC, GCCR);
+
+	return 0;
+}
+
+/* PTP clock operations */
+static int ravb_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct ravb_private *priv = container_of(ptp, struct ravb_private,
+						 ptp.info);
+	struct net_device *ndev = priv->ndev;
+	unsigned long flags;
+	u32 diff, addend;
+	bool neg_adj = false;
+	u32 gccr;
+
+	if (ppb < 0) {
+		neg_adj = true;
+		ppb = -ppb;
+	}
+	addend = priv->ptp.default_addend;
+	diff = div_u64((u64)addend * ppb, NSEC_PER_SEC);
+
+	addend = neg_adj ? addend - diff : addend + diff;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	priv->ptp.current_addend = addend;
+
+	gccr = ravb_read(ndev, GCCR);
+	if (gccr & GCCR_LTI) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return -EBUSY;
+	}
+	ravb_write(ndev, addend & GTI_TIV, GTI);
+	ravb_write(ndev, gccr | GCCR_LTI, GCCR);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static int ravb_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct ravb_private *priv = container_of(ptp, struct ravb_private,
+						 ptp.info);
+	struct timespec64 ts;
+	unsigned long flags;
+	int error;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	error = ravb_ptp_time_read(priv, &ts);
+	if (!error) {
+		u64 now = ktime_to_ns(timespec64_to_ktime(ts));
+
+		ts = ns_to_timespec64(now + delta);
+		error = ravb_ptp_time_write(priv, &ts);
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return error;
+}
+
+static int ravb_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
+{
+	struct ravb_private *priv = container_of(ptp, struct ravb_private,
+						 ptp.info);
+	unsigned long flags;
+	int error;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	error = ravb_ptp_time_read(priv, ts);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return error;
+}
+
+static int ravb_ptp_settime64(struct ptp_clock_info *ptp,
+			      const struct timespec64 *ts)
+{
+	struct ravb_private *priv = container_of(ptp, struct ravb_private,
+						 ptp.info);
+	unsigned long flags;
+	int error;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	error = ravb_ptp_time_write(priv, ts);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return error;
+}
+
+static int ravb_ptp_extts(struct ptp_clock_info *ptp,
+			  struct ptp_extts_request *req, int on)
+{
+	struct ravb_private *priv = container_of(ptp, struct ravb_private,
+						 ptp.info);
+	struct net_device *ndev = priv->ndev;
+	unsigned long flags;
+	u32 gic;
+
+	if (req->index)
+		return -EINVAL;
+
+	if (priv->ptp.extts[req->index] == on)
+		return 0;
+	priv->ptp.extts[req->index] = on;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	gic = ravb_read(ndev, GIC);
+	if (on)
+		gic |= GIC_PTCE;
+	else
+		gic &= ~GIC_PTCE;
+	ravb_write(ndev, gic, GIC);
+	mmiowb();
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static int ravb_ptp_perout(struct ptp_clock_info *ptp,
+			   struct ptp_perout_request *req, int on)
+{
+	struct ravb_private *priv = container_of(ptp, struct ravb_private,
+						 ptp.info);
+	struct net_device *ndev = priv->ndev;
+	struct ravb_ptp_perout *perout;
+	unsigned long flags;
+	int error = 0;
+	u32 gic;
+
+	if (req->index)
+		return -EINVAL;
+
+	if (on) {
+		u64 start_ns;
+		u64 period_ns;
+
+		start_ns = req->start.sec * NSEC_PER_SEC + req->start.nsec;
+		period_ns = req->period.sec * NSEC_PER_SEC + req->period.nsec;
+
+		if (start_ns > U32_MAX) {
+			netdev_warn(ndev,
+				    "ptp: start value (nsec) is over limit. Maximum size of start is only 32 bits\n");
+			return -ERANGE;
+		}
+
+		if (period_ns > U32_MAX) {
+			netdev_warn(ndev,
+				    "ptp: period value (nsec) is over limit. Maximum size of period is only 32 bits\n");
+			return -ERANGE;
+		}
+
+		spin_lock_irqsave(&priv->lock, flags);
+
+		perout = &priv->ptp.perout[req->index];
+		perout->target = (u32)start_ns;
+		perout->period = (u32)period_ns;
+		error = ravb_ptp_update_compare(priv, (u32)start_ns);
+		if (!error) {
+			/* Unmask interrupt */
+			gic = ravb_read(ndev, GIC);
+			gic |= GIC_PTME;
+			ravb_write(ndev, gic, GIC);
+		}
+	} else	{
+		spin_lock_irqsave(&priv->lock, flags);
+
+		perout = &priv->ptp.perout[req->index];
+		perout->period = 0;
+
+		/* Mask interrupt */
+		gic = ravb_read(ndev, GIC);
+		gic &= ~GIC_PTME;
+		ravb_write(ndev, gic, GIC);
+	}
+	mmiowb();
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return error;
+}
+
+static int ravb_ptp_enable(struct ptp_clock_info *ptp,
+			   struct ptp_clock_request *req, int on)
+{
+	switch (req->type) {
+	case PTP_CLK_REQ_EXTTS:
+		return ravb_ptp_extts(ptp, &req->extts, on);
+	case PTP_CLK_REQ_PEROUT:
+		return ravb_ptp_perout(ptp, &req->perout, on);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static const struct ptp_clock_info ravb_ptp_info = {
+	.owner		= THIS_MODULE,
+	.name		= "ravb clock",
+	.max_adj	= 50000000,
+	.n_ext_ts	= N_EXT_TS,
+	.n_per_out	= N_PER_OUT,
+	.adjfreq	= ravb_ptp_adjfreq,
+	.adjtime	= ravb_ptp_adjtime,
+	.gettime64	= ravb_ptp_gettime64,
+	.settime64	= ravb_ptp_settime64,
+	.enable		= ravb_ptp_enable,
+};
+
+/* Caller must hold the lock */
+irqreturn_t ravb_ptp_interrupt(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	u32 gis = ravb_read(ndev, GIS);
+
+	gis &= ravb_read(ndev, GIC);
+	if (gis & GIS_PTCF) {
+		struct ptp_clock_event event;
+
+		event.type = PTP_CLOCK_EXTTS;
+		event.index = 0;
+		event.timestamp = ravb_read(ndev, GCPT);
+		ptp_clock_event(priv->ptp.clock, &event);
+	}
+	if (gis & GIS_PTMF) {
+		struct ravb_ptp_perout *perout = priv->ptp.perout;
+
+		if (perout->period) {
+			perout->target += perout->period;
+			ravb_ptp_update_compare(priv, perout->target);
+		}
+	}
+
+	if (gis) {
+		ravb_write(ndev, ~gis, GIS);
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+void ravb_ptp_init(struct net_device *ndev, struct platform_device *pdev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+	unsigned long flags;
+	u32 gccr;
+
+	priv->ptp.info = ravb_ptp_info;
+
+	priv->ptp.default_addend = ravb_read(ndev, GTI);
+	priv->ptp.current_addend = priv->ptp.default_addend;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ravb_wait(ndev, GCCR, GCCR_TCR, GCCR_TCR_NOREQ);
+	gccr = ravb_read(ndev, GCCR) & ~GCCR_TCSS;
+	ravb_write(ndev, gccr | GCCR_TCSS_ADJGPTP, GCCR);
+	mmiowb();
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	priv->ptp.clock = ptp_clock_register(&priv->ptp.info, &pdev->dev);
+}
+
+void ravb_ptp_stop(struct net_device *ndev)
+{
+	struct ravb_private *priv = netdev_priv(ndev);
+
+	ravb_write(ndev, 0, GIC);
+	ravb_write(ndev, 0, GIS);
+
+	ptp_clock_unregister(priv->ptp.clock);
+}
diff --git a/drivers/net/ethernet/rocker/rocker.c b/drivers/net/ethernet/rocker/rocker.c
index cf98cc9bbc8d..2d8578cade03 100644
--- a/drivers/net/ethernet/rocker/rocker.c
+++ b/drivers/net/ethernet/rocker/rocker.c
@@ -181,7 +181,7 @@ struct rocker_desc_info {
 	size_t data_size;
 	size_t tlv_size;
 	struct rocker_desc *desc;
-	DEFINE_DMA_UNMAP_ADDR(mapaddr);
+	dma_addr_t mapaddr;
 };
 
 struct rocker_dma_ring_info {
@@ -225,6 +225,7 @@ struct rocker_port {
 	struct napi_struct napi_rx;
 	struct rocker_dma_ring_info tx_ring;
 	struct rocker_dma_ring_info rx_ring;
+	struct list_head trans_mem;
 };
 
 struct rocker {
@@ -236,21 +237,21 @@ struct rocker {
 	struct {
 		u64 id;
 	} hw;
-	spinlock_t cmd_ring_lock;
+	spinlock_t cmd_ring_lock;		/* for cmd ring accesses */
 	struct rocker_dma_ring_info cmd_ring;
 	struct rocker_dma_ring_info event_ring;
 	DECLARE_HASHTABLE(flow_tbl, 16);
-	spinlock_t flow_tbl_lock;
+	spinlock_t flow_tbl_lock;		/* for flow tbl accesses */
 	u64 flow_tbl_next_cookie;
 	DECLARE_HASHTABLE(group_tbl, 16);
-	spinlock_t group_tbl_lock;
+	spinlock_t group_tbl_lock;		/* for group tbl accesses */
 	DECLARE_HASHTABLE(fdb_tbl, 16);
-	spinlock_t fdb_tbl_lock;
+	spinlock_t fdb_tbl_lock;		/* for fdb tbl accesses */
 	unsigned long internal_vlan_bitmap[ROCKER_INTERNAL_VLAN_BITMAP_LEN];
 	DECLARE_HASHTABLE(internal_vlan_tbl, 8);
-	spinlock_t internal_vlan_tbl_lock;
+	spinlock_t internal_vlan_tbl_lock;	/* for vlan tbl accesses */
 	DECLARE_HASHTABLE(neigh_tbl, 16);
-	spinlock_t neigh_tbl_lock;
+	spinlock_t neigh_tbl_lock;		/* for neigh tbl accesses */
 	u32 neigh_tbl_next_index;
 };
 
@@ -294,7 +295,7 @@ static bool rocker_vlan_id_is_internal(__be16 vlan_id)
 	return (_vlan_id >= start && _vlan_id <= end);
 }
 
-static __be16 rocker_port_vid_to_vlan(struct rocker_port *rocker_port,
+static __be16 rocker_port_vid_to_vlan(const struct rocker_port *rocker_port,
 				      u16 vid, bool *pop_vlan)
 {
 	__be16 vlan_id;
@@ -311,7 +312,7 @@ static __be16 rocker_port_vid_to_vlan(struct rocker_port *rocker_port,
 	return vlan_id;
 }
 
-static u16 rocker_port_vlan_to_vid(struct rocker_port *rocker_port,
+static u16 rocker_port_vlan_to_vid(const struct rocker_port *rocker_port,
 				   __be16 vlan_id)
 {
 	if (rocker_vlan_id_is_internal(vlan_id))
@@ -320,11 +321,88 @@ static u16 rocker_port_vlan_to_vid(struct rocker_port *rocker_port,
 	return ntohs(vlan_id);
 }
 
-static bool rocker_port_is_bridged(struct rocker_port *rocker_port)
+static bool rocker_port_is_bridged(const struct rocker_port *rocker_port)
 {
 	return !!rocker_port->bridge_dev;
 }
 
+#define ROCKER_OP_FLAG_REMOVE		BIT(0)
+#define ROCKER_OP_FLAG_NOWAIT		BIT(1)
+#define ROCKER_OP_FLAG_LEARNED		BIT(2)
+#define ROCKER_OP_FLAG_REFRESH		BIT(3)
+
+static void *__rocker_port_mem_alloc(struct rocker_port *rocker_port,
+				     enum switchdev_trans trans, int flags,
+				     size_t size)
+{
+	struct list_head *elem = NULL;
+	gfp_t gfp_flags = (flags & ROCKER_OP_FLAG_NOWAIT) ?
+			  GFP_ATOMIC : GFP_KERNEL;
+
+	/* If in transaction prepare phase, allocate the memory
+	 * and enqueue it on a per-port list.  If in transaction
+	 * commit phase, dequeue the memory from the per-port list
+	 * rather than re-allocating the memory.  The idea is the
+	 * driver code paths for prepare and commit are identical
+	 * so the memory allocated in the prepare phase is the
+	 * memory used in the commit phase.
+	 */
+
+	switch (trans) {
+	case SWITCHDEV_TRANS_PREPARE:
+		elem = kzalloc(size + sizeof(*elem), gfp_flags);
+		if (!elem)
+			return NULL;
+		list_add_tail(elem, &rocker_port->trans_mem);
+		break;
+	case SWITCHDEV_TRANS_COMMIT:
+		BUG_ON(list_empty(&rocker_port->trans_mem));
+		elem = rocker_port->trans_mem.next;
+		list_del_init(elem);
+		break;
+	case SWITCHDEV_TRANS_NONE:
+		elem = kzalloc(size + sizeof(*elem), gfp_flags);
+		if (elem)
+			INIT_LIST_HEAD(elem);
+		break;
+	default:
+		break;
+	}
+
+	return elem ? elem + 1 : NULL;
+}
+
+static void *rocker_port_kzalloc(struct rocker_port *rocker_port,
+				 enum switchdev_trans trans, int flags,
+				 size_t size)
+{
+	return __rocker_port_mem_alloc(rocker_port, trans, flags, size);
+}
+
+static void *rocker_port_kcalloc(struct rocker_port *rocker_port,
+				 enum switchdev_trans trans, int flags,
+				 size_t n, size_t size)
+{
+	return __rocker_port_mem_alloc(rocker_port, trans, flags, n * size);
+}
+
+static void rocker_port_kfree(enum switchdev_trans trans, const void *mem)
+{
+	struct list_head *elem;
+
+	/* Frees are ignored if in transaction prepare phase.  The
+	 * memory remains on the per-port list until freed in the
+	 * commit phase.
+	 */
+
+	if (trans == SWITCHDEV_TRANS_PREPARE)
+		return;
+
+	elem = (struct list_head *)mem - 1;
+	BUG_ON(!list_empty(elem));
+	kfree(elem);
+}
+
 struct rocker_wait {
 	wait_queue_head_t wait;
 	bool done;
@@ -343,20 +421,23 @@ static void rocker_wait_init(struct rocker_wait *wait)
 	rocker_wait_reset(wait);
 }
 
-static struct rocker_wait *rocker_wait_create(gfp_t gfp)
+static struct rocker_wait *rocker_wait_create(struct rocker_port *rocker_port,
+					      enum switchdev_trans trans,
+					      int flags)
 {
 	struct rocker_wait *wait;
 
-	wait = kmalloc(sizeof(*wait), gfp);
+	wait = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*wait));
 	if (!wait)
 		return NULL;
 	rocker_wait_init(wait);
 	return wait;
 }
 
-static void rocker_wait_destroy(struct rocker_wait *work)
+static void rocker_wait_destroy(enum switchdev_trans trans,
+				struct rocker_wait *wait)
 {
-	kfree(work);
+	rocker_port_kfree(trans, wait);
 }
 
 static bool rocker_wait_event_timeout(struct rocker_wait *wait,
@@ -374,18 +455,18 @@ static void rocker_wait_wake_up(struct rocker_wait *wait)
 	wake_up(&wait->wait);
 }
 
-static u32 rocker_msix_vector(struct rocker *rocker, unsigned int vector)
+static u32 rocker_msix_vector(const struct rocker *rocker, unsigned int vector)
 {
 	return rocker->msix_entries[vector].vector;
 }
 
-static u32 rocker_msix_tx_vector(struct rocker_port *rocker_port)
+static u32 rocker_msix_tx_vector(const struct rocker_port *rocker_port)
 {
 	return rocker_msix_vector(rocker_port->rocker,
 				  ROCKER_MSIX_VEC_TX(rocker_port->port_number));
 }
 
-static u32 rocker_msix_rx_vector(struct rocker_port *rocker_port)
+static u32 rocker_msix_rx_vector(const struct rocker_port *rocker_port)
 {
 	return rocker_msix_vector(rocker_port->rocker,
 				  ROCKER_MSIX_VEC_RX(rocker_port->port_number));
@@ -404,9 +485,9 @@ static u32 rocker_msix_rx_vector(struct rocker_port *rocker_port)
  * HW basic testing functions
  *****************************/
 
-static int rocker_reg_test(struct rocker *rocker)
+static int rocker_reg_test(const struct rocker *rocker)
 {
-	struct pci_dev *pdev = rocker->pdev;
+	const struct pci_dev *pdev = rocker->pdev;
 	u64 test_reg;
 	u64 rnd;
 
@@ -434,12 +515,12 @@ static int rocker_reg_test(struct rocker *rocker)
 	return 0;
 }
 
-static int rocker_dma_test_one(struct rocker *rocker, struct rocker_wait *wait,
-			       u32 test_type, dma_addr_t dma_handle,
-			       unsigned char *buf, unsigned char *expect,
-			       size_t size)
+static int rocker_dma_test_one(const struct rocker *rocker,
+			       struct rocker_wait *wait, u32 test_type,
+			       dma_addr_t dma_handle, const unsigned char *buf,
+			       const unsigned char *expect, size_t size)
 {
-	struct pci_dev *pdev = rocker->pdev;
+	const struct pci_dev *pdev = rocker->pdev;
 	int i;
 
 	rocker_wait_reset(wait);
@@ -463,7 +544,7 @@ static int rocker_dma_test_one(struct rocker *rocker, struct rocker_wait *wait,
 #define ROCKER_TEST_DMA_BUF_SIZE (PAGE_SIZE * 4)
 #define ROCKER_TEST_DMA_FILL_PATTERN 0x96
 
-static int rocker_dma_test_offset(struct rocker *rocker,
+static int rocker_dma_test_offset(const struct rocker *rocker,
 				  struct rocker_wait *wait, int offset)
 {
 	struct pci_dev *pdev = rocker->pdev;
@@ -523,7 +604,8 @@ free_alloc:
 	return err;
 }
 
-static int rocker_dma_test(struct rocker *rocker, struct rocker_wait *wait)
+static int rocker_dma_test(const struct rocker *rocker,
+			   struct rocker_wait *wait)
 {
 	int i;
 	int err;
@@ -545,9 +627,9 @@ static irqreturn_t rocker_test_irq_handler(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int rocker_basic_hw_test(struct rocker *rocker)
+static int rocker_basic_hw_test(const struct rocker *rocker)
 {
-	struct pci_dev *pdev = rocker->pdev;
+	const struct pci_dev *pdev = rocker->pdev;
 	struct rocker_wait wait;
 	int err;
 
@@ -680,7 +762,7 @@ static u64 rocker_tlv_get_u64(const struct rocker_tlv *tlv)
 	return *(u64 *) rocker_tlv_data(tlv);
 }
 
-static void rocker_tlv_parse(struct rocker_tlv **tb, int maxtype,
+static void rocker_tlv_parse(const struct rocker_tlv **tb, int maxtype,
 			     const char *buf, int buf_len)
 {
 	const struct rocker_tlv *tlv;
@@ -693,19 +775,19 @@ static void rocker_tlv_parse(struct rocker_tlv **tb, int maxtype,
 		u32 type = rocker_tlv_type(tlv);
 
 		if (type > 0 && type <= maxtype)
-			tb[type] = (struct rocker_tlv *) tlv;
+			tb[type] = tlv;
 	}
 }
 
-static void rocker_tlv_parse_nested(struct rocker_tlv **tb, int maxtype,
+static void rocker_tlv_parse_nested(const struct rocker_tlv **tb, int maxtype,
 				    const struct rocker_tlv *tlv)
 {
 	rocker_tlv_parse(tb, maxtype, rocker_tlv_data(tlv),
 			 rocker_tlv_len(tlv));
 }
 
-static void rocker_tlv_parse_desc(struct rocker_tlv **tb, int maxtype,
-				  struct rocker_desc_info *desc_info)
+static void rocker_tlv_parse_desc(const struct rocker_tlv **tb, int maxtype,
+				  const struct rocker_desc_info *desc_info)
 {
 	rocker_tlv_parse(tb, maxtype, desc_info->data,
 			 desc_info->desc->tlv_size);
@@ -790,9 +872,9 @@ static void rocker_tlv_nest_end(struct rocker_desc_info *desc_info,
 }
 
 static void rocker_tlv_nest_cancel(struct rocker_desc_info *desc_info,
-				   struct rocker_tlv *start)
+				   const struct rocker_tlv *start)
 {
-	desc_info->tlv_size = (char *) start - desc_info->data;
+	desc_info->tlv_size = (const char *) start - desc_info->data;
 }
 
 /******************************************
@@ -804,7 +886,7 @@ static u32 __pos_inc(u32 pos, size_t limit)
 	return ++pos == limit ? 0 : pos;
 }
 
-static int rocker_desc_err(struct rocker_desc_info *desc_info)
+static int rocker_desc_err(const struct rocker_desc_info *desc_info)
 {
 	int err = desc_info->desc->comp_err & ~ROCKER_DMA_DESC_COMP_ERR_GEN;
 
@@ -832,31 +914,31 @@ static int rocker_desc_err(struct rocker_desc_info *desc_info)
 	return -EINVAL;
 }
 
-static void rocker_desc_gen_clear(struct rocker_desc_info *desc_info)
+static void rocker_desc_gen_clear(const struct rocker_desc_info *desc_info)
 {
 	desc_info->desc->comp_err &= ~ROCKER_DMA_DESC_COMP_ERR_GEN;
 }
 
-static bool rocker_desc_gen(struct rocker_desc_info *desc_info)
+static bool rocker_desc_gen(const struct rocker_desc_info *desc_info)
 {
 	u32 comp_err = desc_info->desc->comp_err;
 
 	return comp_err & ROCKER_DMA_DESC_COMP_ERR_GEN ? true : false;
 }
 
-static void *rocker_desc_cookie_ptr_get(struct rocker_desc_info *desc_info)
+static void *rocker_desc_cookie_ptr_get(const struct rocker_desc_info *desc_info)
 {
 	return (void *)(uintptr_t)desc_info->desc->cookie;
 }
 
-static void rocker_desc_cookie_ptr_set(struct rocker_desc_info *desc_info,
+static void rocker_desc_cookie_ptr_set(const struct rocker_desc_info *desc_info,
 				       void *ptr)
 {
 	desc_info->desc->cookie = (uintptr_t) ptr;
 }
 
 static struct rocker_desc_info *
-rocker_desc_head_get(struct rocker_dma_ring_info *info)
+rocker_desc_head_get(const struct rocker_dma_ring_info *info)
 {
 	static struct rocker_desc_info *desc_info;
 	u32 head = __pos_inc(info->head, info->size);
@@ -868,15 +950,15 @@ rocker_desc_head_get(struct rocker_dma_ring_info *info)
 	return desc_info;
 }
 
-static void rocker_desc_commit(struct rocker_desc_info *desc_info)
+static void rocker_desc_commit(const struct rocker_desc_info *desc_info)
 {
 	desc_info->desc->buf_size = desc_info->data_size;
 	desc_info->desc->tlv_size = desc_info->tlv_size;
 }
 
-static void rocker_desc_head_set(struct rocker *rocker,
+static void rocker_desc_head_set(const struct rocker *rocker,
 				 struct rocker_dma_ring_info *info,
-				 struct rocker_desc_info *desc_info)
+				 const struct rocker_desc_info *desc_info)
 {
 	u32 head = __pos_inc(info->head, info->size);
 
@@ -901,8 +983,8 @@ rocker_desc_tail_get(struct rocker_dma_ring_info *info)
 	return desc_info;
 }
 
-static void rocker_dma_ring_credits_set(struct rocker *rocker,
-					struct rocker_dma_ring_info *info,
+static void rocker_dma_ring_credits_set(const struct rocker *rocker,
+					const struct rocker_dma_ring_info *info,
 					u32 credits)
 {
 	if (credits)
@@ -915,7 +997,7 @@ static unsigned long rocker_dma_ring_size_fix(size_t size)
 		   min(roundup_pow_of_two(size), ROCKER_DMA_SIZE_MAX));
 }
 
-static int rocker_dma_ring_create(struct rocker *rocker,
+static int rocker_dma_ring_create(const struct rocker *rocker,
 				  unsigned int type,
 				  size_t size,
 				  struct rocker_dma_ring_info *info)
@@ -951,8 +1033,8 @@ static int rocker_dma_ring_create(struct rocker *rocker,
 	return 0;
 }
 
-static void rocker_dma_ring_destroy(struct rocker *rocker,
-				    struct rocker_dma_ring_info *info)
+static void rocker_dma_ring_destroy(const struct rocker *rocker,
+				    const struct rocker_dma_ring_info *info)
 {
 	rocker_write64(rocker, DMA_DESC_ADDR(info->type), 0);
 
@@ -962,7 +1044,7 @@ static void rocker_dma_ring_destroy(struct rocker *rocker,
 	kfree(info->desc_info);
 }
 
-static void rocker_dma_ring_pass_to_producer(struct rocker *rocker,
+static void rocker_dma_ring_pass_to_producer(const struct rocker *rocker,
 					     struct rocker_dma_ring_info *info)
 {
 	int i;
@@ -977,8 +1059,8 @@ static void rocker_dma_ring_pass_to_producer(struct rocker *rocker,
 	rocker_desc_commit(&info->desc_info[i]);
 }
 
-static int rocker_dma_ring_bufs_alloc(struct rocker *rocker,
-				      struct rocker_dma_ring_info *info,
+static int rocker_dma_ring_bufs_alloc(const struct rocker *rocker,
+				      const struct rocker_dma_ring_info *info,
 				      int direction, size_t buf_size)
 {
 	struct pci_dev *pdev = rocker->pdev;
@@ -1015,7 +1097,7 @@ static int rocker_dma_ring_bufs_alloc(struct rocker *rocker,
 
 rollback:
 	for (i--; i >= 0; i--) {
-		struct rocker_desc_info *desc_info = &info->desc_info[i];
+		const struct rocker_desc_info *desc_info = &info->desc_info[i];
 
 		pci_unmap_single(pdev, dma_unmap_addr(desc_info, mapaddr),
 				 desc_info->data_size, direction);
@@ -1024,15 +1106,15 @@ rollback:
 	return err;
 }
 
-static void rocker_dma_ring_bufs_free(struct rocker *rocker,
-				      struct rocker_dma_ring_info *info,
+static void rocker_dma_ring_bufs_free(const struct rocker *rocker,
+				      const struct rocker_dma_ring_info *info,
 				      int direction)
 {
 	struct pci_dev *pdev = rocker->pdev;
 	int i;
 
 	for (i = 0; i < info->size; i++) {
-		struct rocker_desc_info *desc_info = &info->desc_info[i];
+		const struct rocker_desc_info *desc_info = &info->desc_info[i];
 		struct rocker_desc *desc = &info->desc[i];
 
 		desc->buf_addr = 0;
@@ -1045,7 +1127,7 @@ static void rocker_dma_ring_bufs_free(struct rocker *rocker,
 
 static int rocker_dma_rings_init(struct rocker *rocker)
 {
-	struct pci_dev *pdev = rocker->pdev;
+	const struct pci_dev *pdev = rocker->pdev;
 	int err;
 
 	err = rocker_dma_ring_create(rocker, ROCKER_DMA_CMD,
@@ -1102,11 +1184,11 @@ static void rocker_dma_rings_fini(struct rocker *rocker)
 	rocker_dma_ring_destroy(rocker, &rocker->cmd_ring);
 }
 
-static int rocker_dma_rx_ring_skb_map(struct rocker *rocker,
-				      struct rocker_port *rocker_port,
+static int rocker_dma_rx_ring_skb_map(const struct rocker_port *rocker_port,
 				      struct rocker_desc_info *desc_info,
 				      struct sk_buff *skb, size_t buf_len)
 {
+	const struct rocker *rocker = rocker_port->rocker;
 	struct pci_dev *pdev = rocker->pdev;
 	dma_addr_t dma_handle;
 
@@ -1126,13 +1208,12 @@ tlv_put_failure:
 	return -EMSGSIZE;
 }
 
-static size_t rocker_port_rx_buf_len(struct rocker_port *rocker_port)
+static size_t rocker_port_rx_buf_len(const struct rocker_port *rocker_port)
 {
 	return rocker_port->dev->mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
 }
 
-static int rocker_dma_rx_ring_skb_alloc(struct rocker *rocker,
-					struct rocker_port *rocker_port,
+static int rocker_dma_rx_ring_skb_alloc(const struct rocker_port *rocker_port,
 					struct rocker_desc_info *desc_info)
 {
 	struct net_device *dev = rocker_port->dev;
@@ -1149,8 +1230,7 @@ static int rocker_dma_rx_ring_skb_alloc(struct rocker *rocker,
 	skb = netdev_alloc_skb_ip_align(dev, buf_len);
 	if (!skb)
 		return -ENOMEM;
-	err = rocker_dma_rx_ring_skb_map(rocker, rocker_port, desc_info,
-					 skb, buf_len);
+	err = rocker_dma_rx_ring_skb_map(rocker_port, desc_info, skb, buf_len);
 	if (err) {
 		dev_kfree_skb_any(skb);
 		return err;
@@ -1159,8 +1239,8 @@ static int rocker_dma_rx_ring_skb_alloc(struct rocker *rocker,
 	return 0;
 }
 
-static void rocker_dma_rx_ring_skb_unmap(struct rocker *rocker,
-					 struct rocker_tlv **attrs)
+static void rocker_dma_rx_ring_skb_unmap(const struct rocker *rocker,
+					 const struct rocker_tlv **attrs)
 {
 	struct pci_dev *pdev = rocker->pdev;
 	dma_addr_t dma_handle;
@@ -1174,10 +1254,10 @@ static void rocker_dma_rx_ring_skb_unmap(struct rocker *rocker,
 	pci_unmap_single(pdev, dma_handle, len, PCI_DMA_FROMDEVICE);
 }
 
-static void rocker_dma_rx_ring_skb_free(struct rocker *rocker,
-					struct rocker_desc_info *desc_info)
+static void rocker_dma_rx_ring_skb_free(const struct rocker *rocker,
+					const struct rocker_desc_info *desc_info)
 {
-	struct rocker_tlv *attrs[ROCKER_TLV_RX_MAX + 1];
+	const struct rocker_tlv *attrs[ROCKER_TLV_RX_MAX + 1];
 	struct sk_buff *skb = rocker_desc_cookie_ptr_get(desc_info);
 
 	if (!skb)
@@ -1187,15 +1267,15 @@ static void rocker_dma_rx_ring_skb_free(struct rocker *rocker,
 	dev_kfree_skb_any(skb);
 }
 
-static int rocker_dma_rx_ring_skbs_alloc(struct rocker *rocker,
-					 struct rocker_port *rocker_port)
+static int rocker_dma_rx_ring_skbs_alloc(const struct rocker_port *rocker_port)
 {
-	struct rocker_dma_ring_info *rx_ring = &rocker_port->rx_ring;
+	const struct rocker_dma_ring_info *rx_ring = &rocker_port->rx_ring;
+	const struct rocker *rocker = rocker_port->rocker;
 	int i;
 	int err;
 
 	for (i = 0; i < rx_ring->size; i++) {
-		err = rocker_dma_rx_ring_skb_alloc(rocker, rocker_port,
+		err = rocker_dma_rx_ring_skb_alloc(rocker_port,
 						   &rx_ring->desc_info[i]);
 		if (err)
 			goto rollback;
@@ -1208,10 +1288,10 @@ rollback:
 	return err;
 }
 
-static void rocker_dma_rx_ring_skbs_free(struct rocker *rocker,
-					 struct rocker_port *rocker_port)
+static void rocker_dma_rx_ring_skbs_free(const struct rocker_port *rocker_port)
 {
-	struct rocker_dma_ring_info *rx_ring = &rocker_port->rx_ring;
+	const struct rocker_dma_ring_info *rx_ring = &rocker_port->rx_ring;
+	const struct rocker *rocker = rocker_port->rocker;
 	int i;
 
 	for (i = 0; i < rx_ring->size; i++)
@@ -1257,7 +1337,7 @@ static int rocker_port_dma_rings_init(struct rocker_port *rocker_port)
 		goto err_dma_rx_ring_bufs_alloc;
 	}
 
-	err = rocker_dma_rx_ring_skbs_alloc(rocker, rocker_port);
+	err = rocker_dma_rx_ring_skbs_alloc(rocker_port);
 	if (err) {
 		netdev_err(rocker_port->dev, "failed to alloc rx dma ring skbs\n");
 		goto err_dma_rx_ring_skbs_alloc;
@@ -1283,7 +1363,7 @@ static void rocker_port_dma_rings_fini(struct rocker_port *rocker_port)
 {
 	struct rocker *rocker = rocker_port->rocker;
 
-	rocker_dma_rx_ring_skbs_free(rocker, rocker_port);
+	rocker_dma_rx_ring_skbs_free(rocker_port);
 	rocker_dma_ring_bufs_free(rocker, &rocker_port->rx_ring,
 				  PCI_DMA_BIDIRECTIONAL);
 	rocker_dma_ring_destroy(rocker, &rocker_port->rx_ring);
@@ -1292,7 +1372,8 @@ static void rocker_port_dma_rings_fini(struct rocker_port *rocker_port)
 	rocker_dma_ring_destroy(rocker, &rocker_port->tx_ring);
 }
 
-static void rocker_port_set_enable(struct rocker_port *rocker_port, bool enable)
+static void rocker_port_set_enable(const struct rocker_port *rocker_port,
+				   bool enable)
 {
 	u64 val = rocker_read64(rocker_port->rocker, PORT_PHYS_ENABLE);
 
@@ -1310,7 +1391,7 @@ static void rocker_port_set_enable(struct rocker_port *rocker_port, bool enable)
 static irqreturn_t rocker_cmd_irq_handler(int irq, void *dev_id)
 {
 	struct rocker *rocker = dev_id;
-	struct rocker_desc_info *desc_info;
+	const struct rocker_desc_info *desc_info;
 	struct rocker_wait *wait;
 	u32 credits = 0;
 
@@ -1319,7 +1400,7 @@ static irqreturn_t rocker_cmd_irq_handler(int irq, void *dev_id)
 		wait = rocker_desc_cookie_ptr_get(desc_info);
 		if (wait->nowait) {
 			rocker_desc_gen_clear(desc_info);
-			rocker_wait_destroy(wait);
+			rocker_wait_destroy(SWITCHDEV_TRANS_NONE, wait);
 		} else {
 			rocker_wait_wake_up(wait);
 		}
@@ -1331,22 +1412,22 @@ static irqreturn_t rocker_cmd_irq_handler(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static void rocker_port_link_up(struct rocker_port *rocker_port)
+static void rocker_port_link_up(const struct rocker_port *rocker_port)
 {
 	netif_carrier_on(rocker_port->dev);
 	netdev_info(rocker_port->dev, "Link is up\n");
 }
 
-static void rocker_port_link_down(struct rocker_port *rocker_port)
+static void rocker_port_link_down(const struct rocker_port *rocker_port)
 {
 	netif_carrier_off(rocker_port->dev);
 	netdev_info(rocker_port->dev, "Link is down\n");
 }
 
-static int rocker_event_link_change(struct rocker *rocker,
+static int rocker_event_link_change(const struct rocker *rocker,
 				    const struct rocker_tlv *info)
 {
-	struct rocker_tlv *attrs[ROCKER_TLV_EVENT_LINK_CHANGED_MAX + 1];
+	const struct rocker_tlv *attrs[ROCKER_TLV_EVENT_LINK_CHANGED_MAX + 1];
 	unsigned int port_number;
 	bool link_up;
 	struct rocker_port *rocker_port;
@@ -1373,22 +1454,18 @@ static int rocker_event_link_change(struct rocker *rocker,
 	return 0;
 }
 
-#define ROCKER_OP_FLAG_REMOVE		BIT(0)
-#define ROCKER_OP_FLAG_NOWAIT		BIT(1)
-#define ROCKER_OP_FLAG_LEARNED		BIT(2)
-#define ROCKER_OP_FLAG_REFRESH		BIT(3)
-
 static int rocker_port_fdb(struct rocker_port *rocker_port,
+			   enum switchdev_trans trans,
 			   const unsigned char *addr,
 			   __be16 vlan_id, int flags);
 
-static int rocker_event_mac_vlan_seen(struct rocker *rocker,
+static int rocker_event_mac_vlan_seen(const struct rocker *rocker,
 				      const struct rocker_tlv *info)
 {
-	struct rocker_tlv *attrs[ROCKER_TLV_EVENT_MAC_VLAN_MAX + 1];
+	const struct rocker_tlv *attrs[ROCKER_TLV_EVENT_MAC_VLAN_MAX + 1];
 	unsigned int port_number;
 	struct rocker_port *rocker_port;
-	unsigned char *addr;
+	const unsigned char *addr;
 	int flags = ROCKER_OP_FLAG_NOWAIT | ROCKER_OP_FLAG_LEARNED;
 	__be16 vlan_id;
 
@@ -1411,14 +1488,15 @@ static int rocker_event_mac_vlan_seen(struct rocker *rocker,
 	    rocker_port->stp_state != BR_STATE_FORWARDING)
 		return 0;
 
-	return rocker_port_fdb(rocker_port, addr, vlan_id, flags);
+	return rocker_port_fdb(rocker_port, SWITCHDEV_TRANS_NONE,
+			       addr, vlan_id, flags);
 }
 
-static int rocker_event_process(struct rocker *rocker,
-				struct rocker_desc_info *desc_info)
+static int rocker_event_process(const struct rocker *rocker,
+				const struct rocker_desc_info *desc_info)
 {
-	struct rocker_tlv *attrs[ROCKER_TLV_EVENT_MAX + 1];
-	struct rocker_tlv *info;
+	const struct rocker_tlv *attrs[ROCKER_TLV_EVENT_MAX + 1];
+	const struct rocker_tlv *info;
 	u16 type;
 
 	rocker_tlv_parse_desc(attrs, ROCKER_TLV_EVENT_MAX, desc_info);
@@ -1442,8 +1520,8 @@ static int rocker_event_process(struct rocker *rocker,
 static irqreturn_t rocker_event_irq_handler(int irq, void *dev_id)
 {
 	struct rocker *rocker = dev_id;
-	struct pci_dev *pdev = rocker->pdev;
-	struct rocker_desc_info *desc_info;
+	const struct pci_dev *pdev = rocker->pdev;
+	const struct rocker_desc_info *desc_info;
 	u32 credits = 0;
 	int err;
 
@@ -1487,65 +1565,75 @@ static irqreturn_t rocker_rx_irq_handler(int irq, void *dev_id)
  * Command interface
  ********************/
 
-typedef int (*rocker_cmd_cb_t)(struct rocker *rocker,
-			       struct rocker_port *rocker_port,
-			       struct rocker_desc_info *desc_info,
-			       void *priv);
+typedef int (*rocker_cmd_prep_cb_t)(const struct rocker_port *rocker_port,
+				    struct rocker_desc_info *desc_info,
+				    void *priv);
+
+typedef int (*rocker_cmd_proc_cb_t)(const struct rocker_port *rocker_port,
+				    const struct rocker_desc_info *desc_info,
+				    void *priv);
 
-static int rocker_cmd_exec(struct rocker *rocker,
-			   struct rocker_port *rocker_port,
-			   rocker_cmd_cb_t prepare, void *prepare_priv,
-			   rocker_cmd_cb_t process, void *process_priv,
-			   bool nowait)
+static int rocker_cmd_exec(struct rocker_port *rocker_port,
+			   enum switchdev_trans trans, int flags,
+			   rocker_cmd_prep_cb_t prepare, void *prepare_priv,
+			   rocker_cmd_proc_cb_t process, void *process_priv)
 {
+	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_desc_info *desc_info;
 	struct rocker_wait *wait;
-	unsigned long flags;
+	bool nowait = !!(flags & ROCKER_OP_FLAG_NOWAIT);
+	unsigned long lock_flags;
 	int err;
 
-	wait = rocker_wait_create(nowait ? GFP_ATOMIC : GFP_KERNEL);
+	wait = rocker_wait_create(rocker_port, trans, flags);
 	if (!wait)
 		return -ENOMEM;
 	wait->nowait = nowait;
 
-	spin_lock_irqsave(&rocker->cmd_ring_lock, flags);
+	spin_lock_irqsave(&rocker->cmd_ring_lock, lock_flags);
+
 	desc_info = rocker_desc_head_get(&rocker->cmd_ring);
 	if (!desc_info) {
-		spin_unlock_irqrestore(&rocker->cmd_ring_lock, flags);
+		spin_unlock_irqrestore(&rocker->cmd_ring_lock, lock_flags);
 		err = -EAGAIN;
 		goto out;
 	}
-	err = prepare(rocker, rocker_port, desc_info, prepare_priv);
+
+	err = prepare(rocker_port, desc_info, prepare_priv);
 	if (err) {
-		spin_unlock_irqrestore(&rocker->cmd_ring_lock, flags);
+		spin_unlock_irqrestore(&rocker->cmd_ring_lock, lock_flags);
 		goto out;
 	}
+
 	rocker_desc_cookie_ptr_set(desc_info, wait);
-	rocker_desc_head_set(rocker, &rocker->cmd_ring, desc_info);
-	spin_unlock_irqrestore(&rocker->cmd_ring_lock, flags);
+
+	if (trans != SWITCHDEV_TRANS_PREPARE)
+		rocker_desc_head_set(rocker, &rocker->cmd_ring, desc_info);
+
+	spin_unlock_irqrestore(&rocker->cmd_ring_lock, lock_flags);
 
 	if (nowait)
 		return 0;
 
-	if (!rocker_wait_event_timeout(wait, HZ / 10))
-		return -EIO;
+	if (trans != SWITCHDEV_TRANS_PREPARE)
+		if (!rocker_wait_event_timeout(wait, HZ / 10))
+			return -EIO;
 
 	err = rocker_desc_err(desc_info);
 	if (err)
 		return err;
 
 	if (process)
-		err = process(rocker, rocker_port, desc_info, process_priv);
+		err = process(rocker_port, desc_info, process_priv);
 
 	rocker_desc_gen_clear(desc_info);
 out:
-	rocker_wait_destroy(wait);
+	rocker_wait_destroy(trans, wait);
 	return err;
 }
 
 static int
-rocker_cmd_get_port_settings_prep(struct rocker *rocker,
-				  struct rocker_port *rocker_port,
+rocker_cmd_get_port_settings_prep(const struct rocker_port *rocker_port,
 				  struct rocker_desc_info *desc_info,
 				  void *priv)
 {
@@ -1565,14 +1653,13 @@ rocker_cmd_get_port_settings_prep(struct rocker *rocker,
 }
 
 static int
-rocker_cmd_get_port_settings_ethtool_proc(struct rocker *rocker,
-					  struct rocker_port *rocker_port,
-					  struct rocker_desc_info *desc_info,
+rocker_cmd_get_port_settings_ethtool_proc(const struct rocker_port *rocker_port,
+					  const struct rocker_desc_info *desc_info,
 					  void *priv)
 {
 	struct ethtool_cmd *ecmd = priv;
-	struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
-	struct rocker_tlv *info_attrs[ROCKER_TLV_CMD_PORT_SETTINGS_MAX + 1];
+	const struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
+	const struct rocker_tlv *info_attrs[ROCKER_TLV_CMD_PORT_SETTINGS_MAX + 1];
 	u32 speed;
 	u8 duplex;
 	u8 autoneg;
@@ -1604,15 +1691,14 @@ rocker_cmd_get_port_settings_ethtool_proc(struct rocker *rocker,
 }
 
 static int
-rocker_cmd_get_port_settings_macaddr_proc(struct rocker *rocker,
-					  struct rocker_port *rocker_port,
-					  struct rocker_desc_info *desc_info,
+rocker_cmd_get_port_settings_macaddr_proc(const struct rocker_port *rocker_port,
+					  const struct rocker_desc_info *desc_info,
 					  void *priv)
 {
 	unsigned char *macaddr = priv;
-	struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
-	struct rocker_tlv *info_attrs[ROCKER_TLV_CMD_PORT_SETTINGS_MAX + 1];
-	struct rocker_tlv *attr;
+	const struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
+	const struct rocker_tlv *info_attrs[ROCKER_TLV_CMD_PORT_SETTINGS_MAX + 1];
+	const struct rocker_tlv *attr;
 
 	rocker_tlv_parse_desc(attrs, ROCKER_TLV_CMD_MAX, desc_info);
 	if (!attrs[ROCKER_TLV_CMD_INFO])
@@ -1637,17 +1723,16 @@ struct port_name {
 };
 
 static int
-rocker_cmd_get_port_settings_phys_name_proc(struct rocker *rocker,
-					    struct rocker_port *rocker_port,
-					    struct rocker_desc_info *desc_info,
+rocker_cmd_get_port_settings_phys_name_proc(const struct rocker_port *rocker_port,
+					    const struct rocker_desc_info *desc_info,
 					    void *priv)
 {
-	struct rocker_tlv *info_attrs[ROCKER_TLV_CMD_PORT_SETTINGS_MAX + 1];
-	struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
+	const struct rocker_tlv *info_attrs[ROCKER_TLV_CMD_PORT_SETTINGS_MAX + 1];
+	const struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
 	struct port_name *name = priv;
-	struct rocker_tlv *attr;
+	const struct rocker_tlv *attr;
 	size_t i, j, len;
-	char *str;
+	const char *str;
 
 	rocker_tlv_parse_desc(attrs, ROCKER_TLV_CMD_MAX, desc_info);
 	if (!attrs[ROCKER_TLV_CMD_INFO])
@@ -1679,8 +1764,7 @@ rocker_cmd_get_port_settings_phys_name_proc(struct rocker *rocker,
 }
 
 static int
-rocker_cmd_set_port_settings_ethtool_prep(struct rocker *rocker,
-					  struct rocker_port *rocker_port,
+rocker_cmd_set_port_settings_ethtool_prep(const struct rocker_port *rocker_port,
 					  struct rocker_desc_info *desc_info,
 					  void *priv)
 {
@@ -1710,12 +1794,11 @@ rocker_cmd_set_port_settings_ethtool_prep(struct rocker *rocker,
 }
 
 static int
-rocker_cmd_set_port_settings_macaddr_prep(struct rocker *rocker,
-					  struct rocker_port *rocker_port,
+rocker_cmd_set_port_settings_macaddr_prep(const struct rocker_port *rocker_port,
 					  struct rocker_desc_info *desc_info,
 					  void *priv)
 {
-	unsigned char *macaddr = priv;
+	const unsigned char *macaddr = priv;
 	struct rocker_tlv *cmd_info;
 
 	if (rocker_tlv_put_u16(desc_info, ROCKER_TLV_CMD_TYPE,
@@ -1735,8 +1818,7 @@ rocker_cmd_set_port_settings_macaddr_prep(struct rocker *rocker,
 }
 
 static int
-rocker_cmd_set_port_learning_prep(struct rocker *rocker,
-				  struct rocker_port *rocker_port,
+rocker_cmd_set_port_learning_prep(const struct rocker_port *rocker_port,
 				  struct rocker_desc_info *desc_info,
 				  void *priv)
 {
@@ -1761,46 +1843,48 @@ rocker_cmd_set_port_learning_prep(struct rocker *rocker,
 static int rocker_cmd_get_port_settings_ethtool(struct rocker_port *rocker_port,
 						struct ethtool_cmd *ecmd)
 {
-	return rocker_cmd_exec(rocker_port->rocker, rocker_port,
+	return rocker_cmd_exec(rocker_port, SWITCHDEV_TRANS_NONE, 0,
 			       rocker_cmd_get_port_settings_prep, NULL,
 			       rocker_cmd_get_port_settings_ethtool_proc,
-			       ecmd, false);
+			       ecmd);
 }
 
 static int rocker_cmd_get_port_settings_macaddr(struct rocker_port *rocker_port,
 						unsigned char *macaddr)
 {
-	return rocker_cmd_exec(rocker_port->rocker, rocker_port,
+	return rocker_cmd_exec(rocker_port, SWITCHDEV_TRANS_NONE, 0,
 			       rocker_cmd_get_port_settings_prep, NULL,
 			       rocker_cmd_get_port_settings_macaddr_proc,
-			       macaddr, false);
+			       macaddr);
 }
 
 static int rocker_cmd_set_port_settings_ethtool(struct rocker_port *rocker_port,
 						struct ethtool_cmd *ecmd)
 {
-	return rocker_cmd_exec(rocker_port->rocker, rocker_port,
+	return rocker_cmd_exec(rocker_port, SWITCHDEV_TRANS_NONE, 0,
 			       rocker_cmd_set_port_settings_ethtool_prep,
-			       ecmd, NULL, NULL, false);
+			       ecmd, NULL, NULL);
 }
 
 static int rocker_cmd_set_port_settings_macaddr(struct rocker_port *rocker_port,
 						unsigned char *macaddr)
 {
-	return rocker_cmd_exec(rocker_port->rocker, rocker_port,
+	return rocker_cmd_exec(rocker_port, SWITCHDEV_TRANS_NONE, 0,
 			       rocker_cmd_set_port_settings_macaddr_prep,
-			       macaddr, NULL, NULL, false);
+			       macaddr, NULL, NULL);
 }
 
-static int rocker_port_set_learning(struct rocker_port *rocker_port)
+static int rocker_port_set_learning(struct rocker_port *rocker_port,
+				    enum switchdev_trans trans)
 {
-	return rocker_cmd_exec(rocker_port->rocker, rocker_port,
+	return rocker_cmd_exec(rocker_port, trans, 0,
 			       rocker_cmd_set_port_learning_prep,
-			       NULL, NULL, NULL, false);
+			       NULL, NULL, NULL);
 }
 
-static int rocker_cmd_flow_tbl_add_ig_port(struct rocker_desc_info *desc_info,
-					   struct rocker_flow_tbl_entry *entry)
+static int
+rocker_cmd_flow_tbl_add_ig_port(struct rocker_desc_info *desc_info,
+				const struct rocker_flow_tbl_entry *entry)
 {
 	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT,
 			       entry->key.ig_port.in_pport))
@@ -1815,8 +1899,9 @@ static int rocker_cmd_flow_tbl_add_ig_port(struct rocker_desc_info *desc_info,
 	return 0;
 }
 
-static int rocker_cmd_flow_tbl_add_vlan(struct rocker_desc_info *desc_info,
-					struct rocker_flow_tbl_entry *entry)
+static int
+rocker_cmd_flow_tbl_add_vlan(struct rocker_desc_info *desc_info,
+			     const struct rocker_flow_tbl_entry *entry)
 {
 	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT,
 			       entry->key.vlan.in_pport))
@@ -1838,8 +1923,9 @@ static int rocker_cmd_flow_tbl_add_vlan(struct rocker_desc_info *desc_info,
 	return 0;
 }
 
-static int rocker_cmd_flow_tbl_add_term_mac(struct rocker_desc_info *desc_info,
-					    struct rocker_flow_tbl_entry *entry)
+static int
+rocker_cmd_flow_tbl_add_term_mac(struct rocker_desc_info *desc_info,
+				 const struct rocker_flow_tbl_entry *entry)
 {
 	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT,
 			       entry->key.term_mac.in_pport))
@@ -1875,7 +1961,7 @@ static int rocker_cmd_flow_tbl_add_term_mac(struct rocker_desc_info *desc_info,
 
 static int
 rocker_cmd_flow_tbl_add_ucast_routing(struct rocker_desc_info *desc_info,
-				      struct rocker_flow_tbl_entry *entry)
+				      const struct rocker_flow_tbl_entry *entry)
 {
 	if (rocker_tlv_put_be16(desc_info, ROCKER_TLV_OF_DPA_ETHERTYPE,
 				entry->key.ucast_routing.eth_type))
@@ -1896,8 +1982,9 @@ rocker_cmd_flow_tbl_add_ucast_routing(struct rocker_desc_info *desc_info,
 	return 0;
 }
 
-static int rocker_cmd_flow_tbl_add_bridge(struct rocker_desc_info *desc_info,
-					  struct rocker_flow_tbl_entry *entry)
+static int
+rocker_cmd_flow_tbl_add_bridge(struct rocker_desc_info *desc_info,
+			       const struct rocker_flow_tbl_entry *entry)
 {
 	if (entry->key.bridge.has_eth_dst &&
 	    rocker_tlv_put(desc_info, ROCKER_TLV_OF_DPA_DST_MAC,
@@ -1929,8 +2016,9 @@ static int rocker_cmd_flow_tbl_add_bridge(struct rocker_desc_info *desc_info,
 	return 0;
 }
 
-static int rocker_cmd_flow_tbl_add_acl(struct rocker_desc_info *desc_info,
-				       struct rocker_flow_tbl_entry *entry)
+static int
+rocker_cmd_flow_tbl_add_acl(struct rocker_desc_info *desc_info,
+			    const struct rocker_flow_tbl_entry *entry)
 {
 	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_IN_PPORT,
 			       entry->key.acl.in_pport))
@@ -1995,12 +2083,11 @@ static int rocker_cmd_flow_tbl_add_acl(struct rocker_desc_info *desc_info,
 	return 0;
 }
 
-static int rocker_cmd_flow_tbl_add(struct rocker *rocker,
-				   struct rocker_port *rocker_port,
+static int rocker_cmd_flow_tbl_add(const struct rocker_port *rocker_port,
 				   struct rocker_desc_info *desc_info,
 				   void *priv)
 {
-	struct rocker_flow_tbl_entry *entry = priv;
+	const struct rocker_flow_tbl_entry *entry = priv;
 	struct rocker_tlv *cmd_info;
 	int err = 0;
 
@@ -2053,8 +2140,7 @@ static int rocker_cmd_flow_tbl_add(struct rocker *rocker,
 	return 0;
 }
 
-static int rocker_cmd_flow_tbl_del(struct rocker *rocker,
-				   struct rocker_port *rocker_port,
+static int rocker_cmd_flow_tbl_del(const struct rocker_port *rocker_port,
 				   struct rocker_desc_info *desc_info,
 				   void *priv)
 {
@@ -2090,7 +2176,7 @@ rocker_cmd_group_tbl_add_l2_interface(struct rocker_desc_info *desc_info,
 
 static int
 rocker_cmd_group_tbl_add_l2_rewrite(struct rocker_desc_info *desc_info,
-				    struct rocker_group_tbl_entry *entry)
+				    const struct rocker_group_tbl_entry *entry)
 {
 	if (rocker_tlv_put_u32(desc_info, ROCKER_TLV_OF_DPA_GROUP_ID_LOWER,
 			       entry->l2_rewrite.group_id))
@@ -2113,7 +2199,7 @@ rocker_cmd_group_tbl_add_l2_rewrite(struct rocker_desc_info *desc_info,
 
 static int
 rocker_cmd_group_tbl_add_group_ids(struct rocker_desc_info *desc_info,
-				   struct rocker_group_tbl_entry *entry)
+				   const struct rocker_group_tbl_entry *entry)
 {
 	int i;
 	struct rocker_tlv *group_ids;
@@ -2139,7 +2225,7 @@ rocker_cmd_group_tbl_add_group_ids(struct rocker_desc_info *desc_info,
 
 static int
 rocker_cmd_group_tbl_add_l3_unicast(struct rocker_desc_info *desc_info,
-				    struct rocker_group_tbl_entry *entry)
+				    const struct rocker_group_tbl_entry *entry)
 {
 	if (!is_zero_ether_addr(entry->l3_unicast.eth_src) &&
 	    rocker_tlv_put(desc_info, ROCKER_TLV_OF_DPA_SRC_MAC,
@@ -2163,8 +2249,7 @@ rocker_cmd_group_tbl_add_l3_unicast(struct rocker_desc_info *desc_info,
 	return 0;
 }
 
-static int rocker_cmd_group_tbl_add(struct rocker *rocker,
-				    struct rocker_port *rocker_port,
+static int rocker_cmd_group_tbl_add(const struct rocker_port *rocker_port,
 				    struct rocker_desc_info *desc_info,
 				    void *priv)
 {
@@ -2209,8 +2294,7 @@ static int rocker_cmd_group_tbl_add(struct rocker *rocker,
 	return 0;
 }
 
-static int rocker_cmd_group_tbl_del(struct rocker *rocker,
-				    struct rocker_port *rocker_port,
+static int rocker_cmd_group_tbl_del(const struct rocker_port *rocker_port,
 				    struct rocker_desc_info *desc_info,
 				    void *priv)
 {
@@ -2293,7 +2377,8 @@ static void rocker_free_tbls(struct rocker *rocker)
 }
 
 static struct rocker_flow_tbl_entry *
-rocker_flow_tbl_find(struct rocker *rocker, struct rocker_flow_tbl_entry *match)
+rocker_flow_tbl_find(const struct rocker *rocker,
+		     const struct rocker_flow_tbl_entry *match)
 {
 	struct rocker_flow_tbl_entry *found;
 	size_t key_len = match->key_len ? match->key_len : sizeof(found->key);
@@ -2308,24 +2393,25 @@ rocker_flow_tbl_find(struct rocker *rocker, struct rocker_flow_tbl_entry *match)
 }
 
 static int rocker_flow_tbl_add(struct rocker_port *rocker_port,
-			       struct rocker_flow_tbl_entry *match,
-			       bool nowait)
+			       enum switchdev_trans trans, int flags,
+			       struct rocker_flow_tbl_entry *match)
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_flow_tbl_entry *found;
 	size_t key_len = match->key_len ? match->key_len : sizeof(found->key);
-	unsigned long flags;
+	unsigned long lock_flags;
 
 	match->key_crc32 = crc32(~0, &match->key, key_len);
 
-	spin_lock_irqsave(&rocker->flow_tbl_lock, flags);
+	spin_lock_irqsave(&rocker->flow_tbl_lock, lock_flags);
 
 	found = rocker_flow_tbl_find(rocker, match);
 
 	if (found) {
 		match->cookie = found->cookie;
-		hash_del(&found->entry);
-		kfree(found);
+		if (trans != SWITCHDEV_TRANS_PREPARE)
+			hash_del(&found->entry);
+		rocker_port_kfree(trans, found);
 		found = match;
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_MOD;
 	} else {
@@ -2334,73 +2420,69 @@ static int rocker_flow_tbl_add(struct rocker_port *rocker_port,
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_ADD;
 	}
 
-	hash_add(rocker->flow_tbl, &found->entry, found->key_crc32);
+	if (trans != SWITCHDEV_TRANS_PREPARE)
+		hash_add(rocker->flow_tbl, &found->entry, found->key_crc32);
 
-	spin_unlock_irqrestore(&rocker->flow_tbl_lock, flags);
+	spin_unlock_irqrestore(&rocker->flow_tbl_lock, lock_flags);
 
-	return rocker_cmd_exec(rocker, rocker_port,
-			       rocker_cmd_flow_tbl_add,
-			       found, NULL, NULL, nowait);
+	return rocker_cmd_exec(rocker_port, trans, flags,
+			       rocker_cmd_flow_tbl_add, found, NULL, NULL);
 }
 
 static int rocker_flow_tbl_del(struct rocker_port *rocker_port,
-			       struct rocker_flow_tbl_entry *match,
-			       bool nowait)
+			       enum switchdev_trans trans, int flags,
+			       struct rocker_flow_tbl_entry *match)
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_flow_tbl_entry *found;
 	size_t key_len = match->key_len ? match->key_len : sizeof(found->key);
-	unsigned long flags;
+	unsigned long lock_flags;
 	int err = 0;
 
 	match->key_crc32 = crc32(~0, &match->key, key_len);
 
-	spin_lock_irqsave(&rocker->flow_tbl_lock, flags);
+	spin_lock_irqsave(&rocker->flow_tbl_lock, lock_flags);
 
 	found = rocker_flow_tbl_find(rocker, match);
 
 	if (found) {
-		hash_del(&found->entry);
+		if (trans != SWITCHDEV_TRANS_PREPARE)
+			hash_del(&found->entry);
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_FLOW_DEL;
 	}
 
-	spin_unlock_irqrestore(&rocker->flow_tbl_lock, flags);
+	spin_unlock_irqrestore(&rocker->flow_tbl_lock, lock_flags);
 
-	kfree(match);
+	rocker_port_kfree(trans, match);
 
 	if (found) {
-		err = rocker_cmd_exec(rocker, rocker_port,
+		err = rocker_cmd_exec(rocker_port, trans, flags,
 				      rocker_cmd_flow_tbl_del,
-				      found, NULL, NULL, nowait);
-		kfree(found);
+				      found, NULL, NULL);
+		rocker_port_kfree(trans, found);
 	}
 
 	return err;
 }
 
-static gfp_t rocker_op_flags_gfp(int flags)
-{
-	return flags & ROCKER_OP_FLAG_NOWAIT ? GFP_ATOMIC : GFP_KERNEL;
-}
-
 static int rocker_flow_tbl_do(struct rocker_port *rocker_port,
-			      int flags, struct rocker_flow_tbl_entry *entry)
+			      enum switchdev_trans trans, int flags,
+			      struct rocker_flow_tbl_entry *entry)
 {
-	bool nowait = flags & ROCKER_OP_FLAG_NOWAIT;
-
 	if (flags & ROCKER_OP_FLAG_REMOVE)
-		return rocker_flow_tbl_del(rocker_port, entry, nowait);
+		return rocker_flow_tbl_del(rocker_port, trans, flags, entry);
 	else
-		return rocker_flow_tbl_add(rocker_port, entry, nowait);
+		return rocker_flow_tbl_add(rocker_port, trans, flags, entry);
 }
 
 static int rocker_flow_tbl_ig_port(struct rocker_port *rocker_port,
-				   int flags, u32 in_pport, u32 in_pport_mask,
+				   enum switchdev_trans trans, int flags,
+				   u32 in_pport, u32 in_pport_mask,
 				   enum rocker_of_dpa_table_id goto_tbl)
 {
 	struct rocker_flow_tbl_entry *entry;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
@@ -2410,18 +2492,19 @@ static int rocker_flow_tbl_ig_port(struct rocker_port *rocker_port,
 	entry->key.ig_port.in_pport_mask = in_pport_mask;
 	entry->key.ig_port.goto_tbl = goto_tbl;
 
-	return rocker_flow_tbl_do(rocker_port, flags, entry);
+	return rocker_flow_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static int rocker_flow_tbl_vlan(struct rocker_port *rocker_port,
-				int flags, u32 in_pport,
-				__be16 vlan_id, __be16 vlan_id_mask,
+				enum switchdev_trans trans, int flags,
+				u32 in_pport, __be16 vlan_id,
+				__be16 vlan_id_mask,
 				enum rocker_of_dpa_table_id goto_tbl,
 				bool untagged, __be16 new_vlan_id)
 {
 	struct rocker_flow_tbl_entry *entry;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
@@ -2435,10 +2518,11 @@ static int rocker_flow_tbl_vlan(struct rocker_port *rocker_port,
 	entry->key.vlan.untagged = untagged;
 	entry->key.vlan.new_vlan_id = new_vlan_id;
 
-	return rocker_flow_tbl_do(rocker_port, flags, entry);
+	return rocker_flow_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static int rocker_flow_tbl_term_mac(struct rocker_port *rocker_port,
+				    enum switchdev_trans trans,
 				    u32 in_pport, u32 in_pport_mask,
 				    __be16 eth_type, const u8 *eth_dst,
 				    const u8 *eth_dst_mask, __be16 vlan_id,
@@ -2447,7 +2531,7 @@ static int rocker_flow_tbl_term_mac(struct rocker_port *rocker_port,
 {
 	struct rocker_flow_tbl_entry *entry;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
@@ -2471,11 +2555,11 @@ static int rocker_flow_tbl_term_mac(struct rocker_port *rocker_port,
 	entry->key.term_mac.vlan_id_mask = vlan_id_mask;
 	entry->key.term_mac.copy_to_cpu = copy_to_cpu;
 
-	return rocker_flow_tbl_do(rocker_port, flags, entry);
+	return rocker_flow_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static int rocker_flow_tbl_bridge(struct rocker_port *rocker_port,
-				  int flags,
+				  enum switchdev_trans trans, int flags,
 				  const u8 *eth_dst, const u8 *eth_dst_mask,
 				  __be16 vlan_id, u32 tunnel_id,
 				  enum rocker_of_dpa_table_id goto_tbl,
@@ -2487,7 +2571,7 @@ static int rocker_flow_tbl_bridge(struct rocker_port *rocker_port,
 	bool dflt = !eth_dst || (eth_dst && eth_dst_mask);
 	bool wild = false;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
@@ -2500,7 +2584,7 @@ static int rocker_flow_tbl_bridge(struct rocker_port *rocker_port,
 	if (eth_dst_mask) {
 		entry->key.bridge.has_eth_dst_mask = 1;
 		ether_addr_copy(entry->key.bridge.eth_dst_mask, eth_dst_mask);
-		if (memcmp(eth_dst_mask, ff_mac, ETH_ALEN))
+		if (!ether_addr_equal(eth_dst_mask, ff_mac))
 			wild = true;
 	}
 
@@ -2525,10 +2609,11 @@ static int rocker_flow_tbl_bridge(struct rocker_port *rocker_port,
 	entry->key.bridge.group_id = group_id;
 	entry->key.bridge.copy_to_cpu = copy_to_cpu;
 
-	return rocker_flow_tbl_do(rocker_port, flags, entry);
+	return rocker_flow_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static int rocker_flow_tbl_ucast4_routing(struct rocker_port *rocker_port,
+					  enum switchdev_trans trans,
 					  __be16 eth_type, __be32 dst,
 					  __be32 dst_mask, u32 priority,
 					  enum rocker_of_dpa_table_id goto_tbl,
@@ -2536,7 +2621,7 @@ static int rocker_flow_tbl_ucast4_routing(struct rocker_port *rocker_port,
 {
 	struct rocker_flow_tbl_entry *entry;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
@@ -2550,30 +2635,29 @@ static int rocker_flow_tbl_ucast4_routing(struct rocker_port *rocker_port,
 	entry->key_len = offsetof(struct rocker_flow_tbl_key,
 				  ucast_routing.group_id);
 
-	return rocker_flow_tbl_do(rocker_port, flags, entry);
+	return rocker_flow_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static int rocker_flow_tbl_acl(struct rocker_port *rocker_port,
-			       int flags, u32 in_pport,
-			       u32 in_pport_mask,
+			       enum switchdev_trans trans, int flags,
+			       u32 in_pport, u32 in_pport_mask,
 			       const u8 *eth_src, const u8 *eth_src_mask,
 			       const u8 *eth_dst, const u8 *eth_dst_mask,
-			       __be16 eth_type,
-			       __be16 vlan_id, __be16 vlan_id_mask,
-			       u8 ip_proto, u8 ip_proto_mask,
-			       u8 ip_tos, u8 ip_tos_mask,
+			       __be16 eth_type, __be16 vlan_id,
+			       __be16 vlan_id_mask, u8 ip_proto,
+			       u8 ip_proto_mask, u8 ip_tos, u8 ip_tos_mask,
 			       u32 group_id)
 {
 	u32 priority;
 	struct rocker_flow_tbl_entry *entry;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
 	priority = ROCKER_PRIORITY_ACL_NORMAL;
 	if (eth_dst && eth_dst_mask) {
-		if (memcmp(eth_dst_mask, mcast_mac, ETH_ALEN) == 0)
+		if (ether_addr_equal(eth_dst_mask, mcast_mac))
 			priority = ROCKER_PRIORITY_ACL_DFLT;
 		else if (is_link_local_ether_addr(eth_dst))
 			priority = ROCKER_PRIORITY_ACL_CTRL;
@@ -2602,12 +2686,12 @@ static int rocker_flow_tbl_acl(struct rocker_port *rocker_port,
 	entry->key.acl.ip_tos_mask = ip_tos_mask;
 	entry->key.acl.group_id = group_id;
 
-	return rocker_flow_tbl_do(rocker_port, flags, entry);
+	return rocker_flow_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static struct rocker_group_tbl_entry *
-rocker_group_tbl_find(struct rocker *rocker,
-		      struct rocker_group_tbl_entry *match)
+rocker_group_tbl_find(const struct rocker *rocker,
+		      const struct rocker_group_tbl_entry *match)
 {
 	struct rocker_group_tbl_entry *found;
 
@@ -2620,34 +2704,36 @@ rocker_group_tbl_find(struct rocker *rocker,
 	return NULL;
 }
 
-static void rocker_group_tbl_entry_free(struct rocker_group_tbl_entry *entry)
+static void rocker_group_tbl_entry_free(enum switchdev_trans trans,
+					struct rocker_group_tbl_entry *entry)
 {
 	switch (ROCKER_GROUP_TYPE_GET(entry->group_id)) {
 	case ROCKER_OF_DPA_GROUP_TYPE_L2_FLOOD:
 	case ROCKER_OF_DPA_GROUP_TYPE_L2_MCAST:
-		kfree(entry->group_ids);
+		rocker_port_kfree(trans, entry->group_ids);
 		break;
 	default:
 		break;
 	}
-	kfree(entry);
+	rocker_port_kfree(trans, entry);
 }
 
 static int rocker_group_tbl_add(struct rocker_port *rocker_port,
-				struct rocker_group_tbl_entry *match,
-				bool nowait)
+				enum switchdev_trans trans, int flags,
+				struct rocker_group_tbl_entry *match)
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_group_tbl_entry *found;
-	unsigned long flags;
+	unsigned long lock_flags;
 
-	spin_lock_irqsave(&rocker->group_tbl_lock, flags);
+	spin_lock_irqsave(&rocker->group_tbl_lock, lock_flags);
 
 	found = rocker_group_tbl_find(rocker, match);
 
 	if (found) {
-		hash_del(&found->entry);
-		rocker_group_tbl_entry_free(found);
+		if (trans != SWITCHDEV_TRANS_PREPARE)
+			hash_del(&found->entry);
+		rocker_group_tbl_entry_free(trans, found);
 		found = match;
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_GROUP_MOD;
 	} else {
@@ -2655,116 +2741,118 @@ static int rocker_group_tbl_add(struct rocker_port *rocker_port,
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_GROUP_ADD;
 	}
 
-	hash_add(rocker->group_tbl, &found->entry, found->group_id);
+	if (trans != SWITCHDEV_TRANS_PREPARE)
+		hash_add(rocker->group_tbl, &found->entry, found->group_id);
 
-	spin_unlock_irqrestore(&rocker->group_tbl_lock, flags);
+	spin_unlock_irqrestore(&rocker->group_tbl_lock, lock_flags);
 
-	return rocker_cmd_exec(rocker, rocker_port,
-			       rocker_cmd_group_tbl_add,
-			       found, NULL, NULL, nowait);
+	return rocker_cmd_exec(rocker_port, trans, flags,
+			       rocker_cmd_group_tbl_add, found, NULL, NULL);
 }
 
 static int rocker_group_tbl_del(struct rocker_port *rocker_port,
-				struct rocker_group_tbl_entry *match,
-				bool nowait)
+				enum switchdev_trans trans, int flags,
+				struct rocker_group_tbl_entry *match)
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_group_tbl_entry *found;
-	unsigned long flags;
+	unsigned long lock_flags;
 	int err = 0;
 
-	spin_lock_irqsave(&rocker->group_tbl_lock, flags);
+	spin_lock_irqsave(&rocker->group_tbl_lock, lock_flags);
 
 	found = rocker_group_tbl_find(rocker, match);
 
 	if (found) {
-		hash_del(&found->entry);
+		if (trans != SWITCHDEV_TRANS_PREPARE)
+			hash_del(&found->entry);
 		found->cmd = ROCKER_TLV_CMD_TYPE_OF_DPA_GROUP_DEL;
 	}
 
-	spin_unlock_irqrestore(&rocker->group_tbl_lock, flags);
+	spin_unlock_irqrestore(&rocker->group_tbl_lock, lock_flags);
 
-	rocker_group_tbl_entry_free(match);
+	rocker_group_tbl_entry_free(trans, match);
 
 	if (found) {
-		err = rocker_cmd_exec(rocker, rocker_port,
+		err = rocker_cmd_exec(rocker_port, trans, flags,
 				      rocker_cmd_group_tbl_del,
-				      found, NULL, NULL, nowait);
-		rocker_group_tbl_entry_free(found);
+				      found, NULL, NULL);
+		rocker_group_tbl_entry_free(trans, found);
 	}
 
 	return err;
 }
 
 static int rocker_group_tbl_do(struct rocker_port *rocker_port,
-			       int flags, struct rocker_group_tbl_entry *entry)
+			       enum switchdev_trans trans, int flags,
+			       struct rocker_group_tbl_entry *entry)
 {
-	bool nowait = flags & ROCKER_OP_FLAG_NOWAIT;
-
 	if (flags & ROCKER_OP_FLAG_REMOVE)
-		return rocker_group_tbl_del(rocker_port, entry, nowait);
+		return rocker_group_tbl_del(rocker_port, trans, flags, entry);
 	else
-		return rocker_group_tbl_add(rocker_port, entry, nowait);
+		return rocker_group_tbl_add(rocker_port, trans, flags, entry);
 }
 
 static int rocker_group_l2_interface(struct rocker_port *rocker_port,
-				     int flags, __be16 vlan_id,
-				     u32 out_pport, int pop_vlan)
+				     enum switchdev_trans trans, int flags,
+				     __be16 vlan_id, u32 out_pport,
+				     int pop_vlan)
 {
 	struct rocker_group_tbl_entry *entry;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
 	entry->group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 	entry->l2_interface.pop_vlan = pop_vlan;
 
-	return rocker_group_tbl_do(rocker_port, flags, entry);
+	return rocker_group_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static int rocker_group_l2_fan_out(struct rocker_port *rocker_port,
+				   enum switchdev_trans trans,
 				   int flags, u8 group_count,
-				   u32 *group_ids, u32 group_id)
+				   const u32 *group_ids, u32 group_id)
 {
 	struct rocker_group_tbl_entry *entry;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
 	entry->group_id = group_id;
 	entry->group_count = group_count;
 
-	entry->group_ids = kcalloc(group_count, sizeof(u32),
-				   rocker_op_flags_gfp(flags));
+	entry->group_ids = rocker_port_kcalloc(rocker_port, trans, flags,
+					       group_count, sizeof(u32));
 	if (!entry->group_ids) {
-		kfree(entry);
+		rocker_port_kfree(trans, entry);
 		return -ENOMEM;
 	}
 	memcpy(entry->group_ids, group_ids, group_count * sizeof(u32));
 
-	return rocker_group_tbl_do(rocker_port, flags, entry);
+	return rocker_group_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static int rocker_group_l2_flood(struct rocker_port *rocker_port,
-				 int flags, __be16 vlan_id,
-				 u8 group_count, u32 *group_ids,
-				 u32 group_id)
+				 enum switchdev_trans trans, int flags,
+				 __be16 vlan_id, u8 group_count,
+				 const u32 *group_ids, u32 group_id)
 {
-	return rocker_group_l2_fan_out(rocker_port, flags,
+	return rocker_group_l2_fan_out(rocker_port, trans, flags,
 				       group_count, group_ids,
 				       group_id);
 }
 
 static int rocker_group_l3_unicast(struct rocker_port *rocker_port,
-				   int flags, u32 index, u8 *src_mac,
-				   u8 *dst_mac, __be16 vlan_id,
-				   bool ttl_check, u32 pport)
+				   enum switchdev_trans trans, int flags,
+				   u32 index, const u8 *src_mac, const u8 *dst_mac,
+				   __be16 vlan_id, bool ttl_check, u32 pport)
 {
 	struct rocker_group_tbl_entry *entry;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
@@ -2777,11 +2865,11 @@ static int rocker_group_l3_unicast(struct rocker_port *rocker_port,
 	entry->l3_unicast.ttl_check = ttl_check;
 	entry->l3_unicast.group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, pport);
 
-	return rocker_group_tbl_do(rocker_port, flags, entry);
+	return rocker_group_tbl_do(rocker_port, trans, flags, entry);
 }
 
 static struct rocker_neigh_tbl_entry *
-	rocker_neigh_tbl_find(struct rocker *rocker, __be32 ip_addr)
+rocker_neigh_tbl_find(const struct rocker *rocker, __be32 ip_addr)
 {
 	struct rocker_neigh_tbl_entry *found;
 
@@ -2794,37 +2882,44 @@ static struct rocker_neigh_tbl_entry *
 }
 
 static void _rocker_neigh_add(struct rocker *rocker,
+			      enum switchdev_trans trans,
 			      struct rocker_neigh_tbl_entry *entry)
 {
-	entry->index = rocker->neigh_tbl_next_index++;
+	if (trans != SWITCHDEV_TRANS_COMMIT)
+		entry->index = rocker->neigh_tbl_next_index++;
+	if (trans == SWITCHDEV_TRANS_PREPARE)
+		return;
 	entry->ref_count++;
 	hash_add(rocker->neigh_tbl, &entry->entry,
 		 be32_to_cpu(entry->ip_addr));
 }
 
-static void _rocker_neigh_del(struct rocker *rocker,
+static void _rocker_neigh_del(enum switchdev_trans trans,
 			      struct rocker_neigh_tbl_entry *entry)
 {
+	if (trans == SWITCHDEV_TRANS_PREPARE)
+		return;
 	if (--entry->ref_count == 0) {
 		hash_del(&entry->entry);
-		kfree(entry);
+		rocker_port_kfree(trans, entry);
 	}
 }
 
-static void _rocker_neigh_update(struct rocker *rocker,
-				 struct rocker_neigh_tbl_entry *entry,
-				 u8 *eth_dst, bool ttl_check)
+static void _rocker_neigh_update(struct rocker_neigh_tbl_entry *entry,
+				 enum switchdev_trans trans,
+				 const u8 *eth_dst, bool ttl_check)
 {
 	if (eth_dst) {
 		ether_addr_copy(entry->eth_dst, eth_dst);
 		entry->ttl_check = ttl_check;
-	} else {
+	} else if (trans != SWITCHDEV_TRANS_PREPARE) {
 		entry->ref_count++;
 	}
 }
 
 static int rocker_port_ipv4_neigh(struct rocker_port *rocker_port,
-				  int flags, __be32 ip_addr, u8 *eth_dst)
+				  enum switchdev_trans trans,
+				  int flags, __be32 ip_addr, const u8 *eth_dst)
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_neigh_tbl_entry *entry;
@@ -2840,7 +2935,7 @@ static int rocker_port_ipv4_neigh(struct rocker_port *rocker_port,
 	bool removing;
 	int err = 0;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
@@ -2857,12 +2952,12 @@ static int rocker_port_ipv4_neigh(struct rocker_port *rocker_port,
 		entry->dev = rocker_port->dev;
 		ether_addr_copy(entry->eth_dst, eth_dst);
 		entry->ttl_check = true;
-		_rocker_neigh_add(rocker, entry);
+		_rocker_neigh_add(rocker, trans, entry);
 	} else if (removing) {
 		memcpy(entry, found, sizeof(*entry));
-		_rocker_neigh_del(rocker, found);
+		_rocker_neigh_del(trans, found);
 	} else if (updating) {
-		_rocker_neigh_update(rocker, found, eth_dst, true);
+		_rocker_neigh_update(found, trans, eth_dst, true);
 		memcpy(entry, found, sizeof(*entry));
 	} else {
 		err = -ENOENT;
@@ -2879,7 +2974,7 @@ static int rocker_port_ipv4_neigh(struct rocker_port *rocker_port,
 	 * other routes' nexthops.
 	 */
 
-	err = rocker_group_l3_unicast(rocker_port, flags,
+	err = rocker_group_l3_unicast(rocker_port, trans, flags,
 				      entry->index,
 				      rocker_port->dev->dev_addr,
 				      entry->eth_dst,
@@ -2895,7 +2990,7 @@ static int rocker_port_ipv4_neigh(struct rocker_port *rocker_port,
 
 	if (adding || removing) {
 		group_id = ROCKER_GROUP_L3_UNICAST(entry->index);
-		err = rocker_flow_tbl_ucast4_routing(rocker_port,
+		err = rocker_flow_tbl_ucast4_routing(rocker_port, trans,
 						     eth_type, ip_addr,
 						     inet_make_mask(32),
 						     priority, goto_tbl,
@@ -2909,13 +3004,13 @@ static int rocker_port_ipv4_neigh(struct rocker_port *rocker_port,
 
 err_out:
 	if (!adding)
-		kfree(entry);
+		rocker_port_kfree(trans, entry);
 
 	return err;
 }
 
 static int rocker_port_ipv4_resolve(struct rocker_port *rocker_port,
-				    __be32 ip_addr)
+				    enum switchdev_trans trans, __be32 ip_addr)
 {
 	struct net_device *dev = rocker_port->dev;
 	struct neighbour *n = __ipv4_neigh_lookup(dev, (__force u32)ip_addr);
@@ -2933,7 +3028,8 @@ static int rocker_port_ipv4_resolve(struct rocker_port *rocker_port,
 	 */
 
 	if (n->nud_state & NUD_VALID)
-		err = rocker_port_ipv4_neigh(rocker_port, 0, ip_addr, n->ha);
+		err = rocker_port_ipv4_neigh(rocker_port, trans, 0,
+					     ip_addr, n->ha);
 	else
 		neigh_event_send(n, NULL);
 
@@ -2941,7 +3037,8 @@ static int rocker_port_ipv4_resolve(struct rocker_port *rocker_port,
 	return err;
 }
 
-static int rocker_port_ipv4_nh(struct rocker_port *rocker_port, int flags,
+static int rocker_port_ipv4_nh(struct rocker_port *rocker_port,
+			       enum switchdev_trans trans, int flags,
 			       __be32 ip_addr, u32 *index)
 {
 	struct rocker *rocker = rocker_port->rocker;
@@ -2954,7 +3051,7 @@ static int rocker_port_ipv4_nh(struct rocker_port *rocker_port, int flags,
 	bool resolved = true;
 	int err = 0;
 
-	entry = kzalloc(sizeof(*entry), rocker_op_flags_gfp(flags));
+	entry = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*entry));
 	if (!entry)
 		return -ENOMEM;
 
@@ -2971,13 +3068,13 @@ static int rocker_port_ipv4_nh(struct rocker_port *rocker_port, int flags,
 	if (adding) {
 		entry->ip_addr = ip_addr;
 		entry->dev = rocker_port->dev;
-		_rocker_neigh_add(rocker, entry);
+		_rocker_neigh_add(rocker, trans, entry);
 		*index = entry->index;
 		resolved = false;
 	} else if (removing) {
-		_rocker_neigh_del(rocker, found);
+		_rocker_neigh_del(trans, found);
 	} else if (updating) {
-		_rocker_neigh_update(rocker, found, NULL, false);
+		_rocker_neigh_update(found, trans, NULL, false);
 		resolved = !is_zero_ether_addr(found->eth_dst);
 	} else {
 		err = -ENOENT;
@@ -2986,7 +3083,7 @@ static int rocker_port_ipv4_nh(struct rocker_port *rocker_port, int flags,
 	spin_unlock_irqrestore(&rocker->neigh_tbl_lock, lock_flags);
 
 	if (!adding)
-		kfree(entry);
+		rocker_port_kfree(trans, entry);
 
 	if (err)
 		return err;
@@ -2994,24 +3091,25 @@ static int rocker_port_ipv4_nh(struct rocker_port *rocker_port, int flags,
 	/* Resolved means neigh ip_addr is resolved to neigh mac. */
 
 	if (!resolved)
-		err = rocker_port_ipv4_resolve(rocker_port, ip_addr);
+		err = rocker_port_ipv4_resolve(rocker_port, trans, ip_addr);
 
 	return err;
 }
 
 static int rocker_port_vlan_flood_group(struct rocker_port *rocker_port,
+					enum switchdev_trans trans,
 					int flags, __be16 vlan_id)
 {
 	struct rocker_port *p;
-	struct rocker *rocker = rocker_port->rocker;
+	const struct rocker *rocker = rocker_port->rocker;
 	u32 group_id = ROCKER_GROUP_L2_FLOOD(vlan_id, 0);
 	u32 *group_ids;
 	u8 group_count = 0;
 	int err = 0;
 	int i;
 
-	group_ids = kcalloc(rocker->port_count, sizeof(u32),
-			    rocker_op_flags_gfp(flags));
+	group_ids = rocker_port_kcalloc(rocker_port, trans, flags,
+					rocker->port_count, sizeof(u32));
 	if (!group_ids)
 		return -ENOMEM;
 
@@ -3022,6 +3120,8 @@ static int rocker_port_vlan_flood_group(struct rocker_port *rocker_port,
 
 	for (i = 0; i < rocker->port_count; i++) {
 		p = rocker->ports[i];
+		if (!p)
+			continue;
 		if (!rocker_port_is_bridged(p))
 			continue;
 		if (test_bit(ntohs(vlan_id), p->vlan_bitmap)) {
@@ -3034,23 +3134,22 @@ static int rocker_port_vlan_flood_group(struct rocker_port *rocker_port,
 	if (group_count == 0)
 		goto no_ports_in_vlan;
 
-	err = rocker_group_l2_flood(rocker_port, flags, vlan_id,
-				    group_count, group_ids,
-				    group_id);
+	err = rocker_group_l2_flood(rocker_port, trans, flags, vlan_id,
+				    group_count, group_ids, group_id);
 	if (err)
 		netdev_err(rocker_port->dev,
 			   "Error (%d) port VLAN l2 flood group\n", err);
 
 no_ports_in_vlan:
-	kfree(group_ids);
+	rocker_port_kfree(trans, group_ids);
 	return err;
 }
 
 static int rocker_port_vlan_l2_groups(struct rocker_port *rocker_port,
-				      int flags, __be16 vlan_id,
-				      bool pop_vlan)
+				      enum switchdev_trans trans, int flags,
+				      __be16 vlan_id, bool pop_vlan)
 {
-	struct rocker *rocker = rocker_port->rocker;
+	const struct rocker *rocker = rocker_port->rocker;
 	struct rocker_port *p;
 	bool adding = !(flags & ROCKER_OP_FLAG_REMOVE);
 	u32 out_pport;
@@ -3065,9 +3164,8 @@ static int rocker_port_vlan_l2_groups(struct rocker_port *rocker_port,
 	if (rocker_port->stp_state == BR_STATE_LEARNING ||
 	    rocker_port->stp_state == BR_STATE_FORWARDING) {
 		out_pport = rocker_port->pport;
-		err = rocker_group_l2_interface(rocker_port, flags,
-						vlan_id, out_pport,
-						pop_vlan);
+		err = rocker_group_l2_interface(rocker_port, trans, flags,
+						vlan_id, out_pport, pop_vlan);
 		if (err) {
 			netdev_err(rocker_port->dev,
 				   "Error (%d) port VLAN l2 group for pport %d\n",
@@ -3083,7 +3181,7 @@ static int rocker_port_vlan_l2_groups(struct rocker_port *rocker_port,
 
 	for (i = 0; i < rocker->port_count; i++) {
 		p = rocker->ports[i];
-		if (test_bit(ntohs(vlan_id), p->vlan_bitmap))
+		if (p && test_bit(ntohs(vlan_id), p->vlan_bitmap))
 			ref++;
 	}
 
@@ -3091,9 +3189,8 @@ static int rocker_port_vlan_l2_groups(struct rocker_port *rocker_port,
 		return 0;
 
 	out_pport = 0;
-	err = rocker_group_l2_interface(rocker_port, flags,
-					vlan_id, out_pport,
-					pop_vlan);
+	err = rocker_group_l2_interface(rocker_port, trans, flags,
+					vlan_id, out_pport, pop_vlan);
 	if (err) {
 		netdev_err(rocker_port->dev,
 			   "Error (%d) port VLAN l2 group for CPU port\n", err);
@@ -3149,14 +3246,14 @@ static struct rocker_ctrl {
 };
 
 static int rocker_port_ctrl_vlan_acl(struct rocker_port *rocker_port,
-				     int flags, struct rocker_ctrl *ctrl,
-				     __be16 vlan_id)
+				     enum switchdev_trans trans, int flags,
+				     const struct rocker_ctrl *ctrl, __be16 vlan_id)
 {
 	u32 in_pport = rocker_port->pport;
 	u32 in_pport_mask = 0xffffffff;
 	u32 out_pport = 0;
-	u8 *eth_src = NULL;
-	u8 *eth_src_mask = NULL;
+	const u8 *eth_src = NULL;
+	const u8 *eth_src_mask = NULL;
 	__be16 vlan_id_mask = htons(0xffff);
 	u8 ip_proto = 0;
 	u8 ip_proto_mask = 0;
@@ -3165,7 +3262,7 @@ static int rocker_port_ctrl_vlan_acl(struct rocker_port *rocker_port,
 	u32 group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 	int err;
 
-	err = rocker_flow_tbl_acl(rocker_port, flags,
+	err = rocker_flow_tbl_acl(rocker_port, trans, flags,
 				  in_pport, in_pport_mask,
 				  eth_src, eth_src_mask,
 				  ctrl->eth_dst, ctrl->eth_dst_mask,
@@ -3182,7 +3279,8 @@ static int rocker_port_ctrl_vlan_acl(struct rocker_port *rocker_port,
 }
 
 static int rocker_port_ctrl_vlan_bridge(struct rocker_port *rocker_port,
-					int flags, struct rocker_ctrl *ctrl,
+					enum switchdev_trans trans, int flags,
+					const struct rocker_ctrl *ctrl,
 					__be16 vlan_id)
 {
 	enum rocker_of_dpa_table_id goto_tbl =
@@ -3194,7 +3292,7 @@ static int rocker_port_ctrl_vlan_bridge(struct rocker_port *rocker_port,
 	if (!rocker_port_is_bridged(rocker_port))
 		return 0;
 
-	err = rocker_flow_tbl_bridge(rocker_port, flags,
+	err = rocker_flow_tbl_bridge(rocker_port, trans, flags,
 				     ctrl->eth_dst, ctrl->eth_dst_mask,
 				     vlan_id, tunnel_id,
 				     goto_tbl, group_id, ctrl->copy_to_cpu);
@@ -3206,8 +3304,8 @@ static int rocker_port_ctrl_vlan_bridge(struct rocker_port *rocker_port,
 }
 
 static int rocker_port_ctrl_vlan_term(struct rocker_port *rocker_port,
-				      int flags, struct rocker_ctrl *ctrl,
-				      __be16 vlan_id)
+				      enum switchdev_trans trans, int flags,
+				      const struct rocker_ctrl *ctrl, __be16 vlan_id)
 {
 	u32 in_pport_mask = 0xffffffff;
 	__be16 vlan_id_mask = htons(0xffff);
@@ -3216,7 +3314,7 @@ static int rocker_port_ctrl_vlan_term(struct rocker_port *rocker_port,
 	if (ntohs(vlan_id) == 0)
 		vlan_id = rocker_port->internal_vlan_id;
 
-	err = rocker_flow_tbl_term_mac(rocker_port,
+	err = rocker_flow_tbl_term_mac(rocker_port, trans,
 				       rocker_port->pport, in_pport_mask,
 				       ctrl->eth_type, ctrl->eth_dst,
 				       ctrl->eth_dst_mask, vlan_id,
@@ -3229,32 +3327,34 @@ static int rocker_port_ctrl_vlan_term(struct rocker_port *rocker_port,
 	return err;
 }
 
-static int rocker_port_ctrl_vlan(struct rocker_port *rocker_port, int flags,
-				 struct rocker_ctrl *ctrl, __be16 vlan_id)
+static int rocker_port_ctrl_vlan(struct rocker_port *rocker_port,
+				 enum switchdev_trans trans, int flags,
+				 const struct rocker_ctrl *ctrl, __be16 vlan_id)
 {
 	if (ctrl->acl)
-		return rocker_port_ctrl_vlan_acl(rocker_port, flags,
+		return rocker_port_ctrl_vlan_acl(rocker_port, trans, flags,
 						 ctrl, vlan_id);
 	if (ctrl->bridge)
-		return rocker_port_ctrl_vlan_bridge(rocker_port, flags,
+		return rocker_port_ctrl_vlan_bridge(rocker_port, trans, flags,
 						    ctrl, vlan_id);
 
 	if (ctrl->term)
-		return rocker_port_ctrl_vlan_term(rocker_port, flags,
+		return rocker_port_ctrl_vlan_term(rocker_port, trans, flags,
 						  ctrl, vlan_id);
 
 	return -EOPNOTSUPP;
 }
 
 static int rocker_port_ctrl_vlan_add(struct rocker_port *rocker_port,
-				     int flags, __be16 vlan_id)
+				     enum switchdev_trans trans, int flags,
+				     __be16 vlan_id)
 {
 	int err = 0;
 	int i;
 
 	for (i = 0; i < ROCKER_CTRL_MAX; i++) {
 		if (rocker_port->ctrls[i]) {
-			err = rocker_port_ctrl_vlan(rocker_port, flags,
+			err = rocker_port_ctrl_vlan(rocker_port, trans, flags,
 						    &rocker_ctrls[i], vlan_id);
 			if (err)
 				return err;
@@ -3264,8 +3364,9 @@ static int rocker_port_ctrl_vlan_add(struct rocker_port *rocker_port,
 	return err;
 }
 
-static int rocker_port_ctrl(struct rocker_port *rocker_port, int flags,
-			    struct rocker_ctrl *ctrl)
+static int rocker_port_ctrl(struct rocker_port *rocker_port,
+			    enum switchdev_trans trans, int flags,
+			    const struct rocker_ctrl *ctrl)
 {
 	u16 vid;
 	int err = 0;
@@ -3273,7 +3374,7 @@ static int rocker_port_ctrl(struct rocker_port *rocker_port, int flags,
 	for (vid = 1; vid < VLAN_N_VID; vid++) {
 		if (!test_bit(vid, rocker_port->vlan_bitmap))
 			continue;
-		err = rocker_port_ctrl_vlan(rocker_port, flags,
+		err = rocker_port_ctrl_vlan(rocker_port, trans, flags,
 					    ctrl, htons(vid));
 		if (err)
 			break;
@@ -3282,8 +3383,8 @@ static int rocker_port_ctrl(struct rocker_port *rocker_port, int flags,
 	return err;
 }
 
-static int rocker_port_vlan(struct rocker_port *rocker_port, int flags,
-			    u16 vid)
+static int rocker_port_vlan(struct rocker_port *rocker_port,
+			    enum switchdev_trans trans, int flags, u16 vid)
 {
 	enum rocker_of_dpa_table_id goto_tbl =
 		ROCKER_OF_DPA_TABLE_ID_TERMINATION_MAC;
@@ -3297,50 +3398,57 @@ static int rocker_port_vlan(struct rocker_port *rocker_port, int flags,
 
 	internal_vlan_id = rocker_port_vid_to_vlan(rocker_port, vid, &untagged);
 
-	if (adding && test_and_set_bit(ntohs(internal_vlan_id),
-				       rocker_port->vlan_bitmap))
+	if (adding && test_bit(ntohs(internal_vlan_id),
+			       rocker_port->vlan_bitmap))
 			return 0; /* already added */
-	else if (!adding && !test_and_clear_bit(ntohs(internal_vlan_id),
-						rocker_port->vlan_bitmap))
+	else if (!adding && !test_bit(ntohs(internal_vlan_id),
+				      rocker_port->vlan_bitmap))
 			return 0; /* already removed */
 
+	change_bit(ntohs(internal_vlan_id), rocker_port->vlan_bitmap);
+
 	if (adding) {
-		err = rocker_port_ctrl_vlan_add(rocker_port, flags,
+		err = rocker_port_ctrl_vlan_add(rocker_port, trans, flags,
 						internal_vlan_id);
 		if (err) {
 			netdev_err(rocker_port->dev,
 				   "Error (%d) port ctrl vlan add\n", err);
-			return err;
+			goto err_out;
 		}
 	}
 
-	err = rocker_port_vlan_l2_groups(rocker_port, flags,
+	err = rocker_port_vlan_l2_groups(rocker_port, trans, flags,
 					 internal_vlan_id, untagged);
 	if (err) {
 		netdev_err(rocker_port->dev,
 			   "Error (%d) port VLAN l2 groups\n", err);
-		return err;
+		goto err_out;
 	}
 
-	err = rocker_port_vlan_flood_group(rocker_port, flags,
+	err = rocker_port_vlan_flood_group(rocker_port, trans, flags,
 					   internal_vlan_id);
 	if (err) {
 		netdev_err(rocker_port->dev,
 			   "Error (%d) port VLAN l2 flood group\n", err);
-		return err;
+		goto err_out;
 	}
 
-	err = rocker_flow_tbl_vlan(rocker_port, flags,
+	err = rocker_flow_tbl_vlan(rocker_port, trans, flags,
 				   in_pport, vlan_id, vlan_id_mask,
 				   goto_tbl, untagged, internal_vlan_id);
 	if (err)
 		netdev_err(rocker_port->dev,
 			   "Error (%d) port VLAN table\n", err);
 
+err_out:
+	if (trans == SWITCHDEV_TRANS_PREPARE)
+		change_bit(ntohs(internal_vlan_id), rocker_port->vlan_bitmap);
+
 	return err;
 }
 
-static int rocker_port_ig_tbl(struct rocker_port *rocker_port, int flags)
+static int rocker_port_ig_tbl(struct rocker_port *rocker_port,
+			      enum switchdev_trans trans, int flags)
 {
 	enum rocker_of_dpa_table_id goto_tbl;
 	u32 in_pport;
@@ -3355,7 +3463,7 @@ static int rocker_port_ig_tbl(struct rocker_port *rocker_port, int flags)
 	in_pport_mask = 0xffff0000;
 	goto_tbl = ROCKER_OF_DPA_TABLE_ID_VLAN;
 
-	err = rocker_flow_tbl_ig_port(rocker_port, flags,
+	err = rocker_flow_tbl_ig_port(rocker_port, trans, flags,
 				      in_pport, in_pport_mask,
 				      goto_tbl);
 	if (err)
@@ -3367,7 +3475,8 @@ static int rocker_port_ig_tbl(struct rocker_port *rocker_port, int flags)
 
 struct rocker_fdb_learn_work {
 	struct work_struct work;
-	struct net_device *dev;
+	struct rocker_port *rocker_port;
+	enum switchdev_trans trans;
 	int flags;
 	u8 addr[ETH_ALEN];
 	u16 vid;
@@ -3375,27 +3484,28 @@ struct rocker_fdb_learn_work {
 
 static void rocker_port_fdb_learn_work(struct work_struct *work)
 {
-	struct rocker_fdb_learn_work *lw =
+	const struct rocker_fdb_learn_work *lw =
 		container_of(work, struct rocker_fdb_learn_work, work);
 	bool removing = (lw->flags & ROCKER_OP_FLAG_REMOVE);
 	bool learned = (lw->flags & ROCKER_OP_FLAG_LEARNED);
-	struct netdev_switch_notifier_fdb_info info;
+	struct switchdev_notifier_fdb_info info;
 
 	info.addr = lw->addr;
 	info.vid = lw->vid;
 
 	if (learned && removing)
-		call_netdev_switch_notifiers(NETDEV_SWITCH_FDB_DEL,
-					     lw->dev, &info.info);
+		call_switchdev_notifiers(SWITCHDEV_FDB_DEL,
+					 lw->rocker_port->dev, &info.info);
 	else if (learned && !removing)
-		call_netdev_switch_notifiers(NETDEV_SWITCH_FDB_ADD,
-					     lw->dev, &info.info);
+		call_switchdev_notifiers(SWITCHDEV_FDB_ADD,
+					 lw->rocker_port->dev, &info.info);
 
-	kfree(work);
+	rocker_port_kfree(lw->trans, work);
 }
 
 static int rocker_port_fdb_learn(struct rocker_port *rocker_port,
-				 int flags, const u8 *addr, __be16 vlan_id)
+				 enum switchdev_trans trans, int flags,
+				 const u8 *addr, __be16 vlan_id)
 {
 	struct rocker_fdb_learn_work *lw;
 	enum rocker_of_dpa_table_id goto_tbl =
@@ -3411,8 +3521,8 @@ static int rocker_port_fdb_learn(struct rocker_port *rocker_port,
 		group_id = ROCKER_GROUP_L2_INTERFACE(vlan_id, out_pport);
 
 	if (!(flags & ROCKER_OP_FLAG_REFRESH)) {
-		err = rocker_flow_tbl_bridge(rocker_port, flags, addr, NULL,
-					     vlan_id, tunnel_id, goto_tbl,
+		err = rocker_flow_tbl_bridge(rocker_port, trans, flags, addr,
+					     NULL, vlan_id, tunnel_id, goto_tbl,
 					     group_id, copy_to_cpu);
 		if (err)
 			return err;
@@ -3424,24 +3534,29 @@ static int rocker_port_fdb_learn(struct rocker_port *rocker_port,
 	if (!rocker_port_is_bridged(rocker_port))
 		return 0;
 
-	lw = kmalloc(sizeof(*lw), rocker_op_flags_gfp(flags));
+	lw = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*lw));
 	if (!lw)
 		return -ENOMEM;
 
 	INIT_WORK(&lw->work, rocker_port_fdb_learn_work);
 
-	lw->dev = rocker_port->dev;
+	lw->rocker_port = rocker_port;
+	lw->trans = trans;
 	lw->flags = flags;
 	ether_addr_copy(lw->addr, addr);
 	lw->vid = rocker_port_vlan_to_vid(rocker_port, vlan_id);
 
-	schedule_work(&lw->work);
+	if (trans == SWITCHDEV_TRANS_PREPARE)
+		rocker_port_kfree(trans, lw);
+	else
+		schedule_work(&lw->work);
 
 	return 0;
 }
 
 static struct rocker_fdb_tbl_entry *
-rocker_fdb_tbl_find(struct rocker *rocker, struct rocker_fdb_tbl_entry *match)
+rocker_fdb_tbl_find(const struct rocker *rocker,
+		    const struct rocker_fdb_tbl_entry *match)
 {
 	struct rocker_fdb_tbl_entry *found;
 
@@ -3453,6 +3568,7 @@ rocker_fdb_tbl_find(struct rocker *rocker, struct rocker_fdb_tbl_entry *match)
 }
 
 static int rocker_port_fdb(struct rocker_port *rocker_port,
+			   enum switchdev_trans trans,
 			   const unsigned char *addr,
 			   __be16 vlan_id, int flags)
 {
@@ -3462,7 +3578,7 @@ static int rocker_port_fdb(struct rocker_port *rocker_port,
 	bool removing = (flags & ROCKER_OP_FLAG_REMOVE);
 	unsigned long lock_flags;
 
-	fdb = kzalloc(sizeof(*fdb), rocker_op_flags_gfp(flags));
+	fdb = rocker_port_kzalloc(rocker_port, trans, flags, sizeof(*fdb));
 	if (!fdb)
 		return -ENOMEM;
 
@@ -3477,32 +3593,34 @@ static int rocker_port_fdb(struct rocker_port *rocker_port,
 	found = rocker_fdb_tbl_find(rocker, fdb);
 
 	if (removing && found) {
-		kfree(fdb);
-		hash_del(&found->entry);
+		rocker_port_kfree(trans, fdb);
+		if (trans != SWITCHDEV_TRANS_PREPARE)
+			hash_del(&found->entry);
 	} else if (!removing && !found) {
-		hash_add(rocker->fdb_tbl, &fdb->entry, fdb->key_crc32);
+		if (trans != SWITCHDEV_TRANS_PREPARE)
+			hash_add(rocker->fdb_tbl, &fdb->entry, fdb->key_crc32);
 	}
 
 	spin_unlock_irqrestore(&rocker->fdb_tbl_lock, lock_flags);
 
 	/* Check if adding and already exists, or removing and can't find */
 	if (!found != !removing) {
-		kfree(fdb);
+		rocker_port_kfree(trans, fdb);
 		if (!found && removing)
 			return 0;
 		/* Refreshing existing to update aging timers */
 		flags |= ROCKER_OP_FLAG_REFRESH;
 	}
 
-	return rocker_port_fdb_learn(rocker_port, flags, addr, vlan_id);
+	return rocker_port_fdb_learn(rocker_port, trans, flags, addr, vlan_id);
 }
 
-static int rocker_port_fdb_flush(struct rocker_port *rocker_port)
+static int rocker_port_fdb_flush(struct rocker_port *rocker_port,
+				 enum switchdev_trans trans, int flags)
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_fdb_tbl_entry *found;
 	unsigned long lock_flags;
-	int flags = ROCKER_OP_FLAG_NOWAIT | ROCKER_OP_FLAG_REMOVE;
 	struct hlist_node *tmp;
 	int bkt;
 	int err = 0;
@@ -3511,6 +3629,8 @@ static int rocker_port_fdb_flush(struct rocker_port *rocker_port)
 	    rocker_port->stp_state == BR_STATE_FORWARDING)
 		return 0;
 
+	flags |= ROCKER_OP_FLAG_REMOVE;
+
 	spin_lock_irqsave(&rocker->fdb_tbl_lock, lock_flags);
 
 	hash_for_each_safe(rocker->fdb_tbl, bkt, tmp, found, entry) {
@@ -3518,12 +3638,13 @@ static int rocker_port_fdb_flush(struct rocker_port *rocker_port)
 			continue;
 		if (!found->learned)
 			continue;
-		err = rocker_port_fdb_learn(rocker_port, flags,
+		err = rocker_port_fdb_learn(rocker_port, trans, flags,
 					    found->key.addr,
 					    found->key.vlan_id);
 		if (err)
 			goto err_out;
-		hash_del(&found->entry);
+		if (trans != SWITCHDEV_TRANS_PREPARE)
+			hash_del(&found->entry);
 	}
 
 err_out:
@@ -3533,7 +3654,8 @@ err_out:
 }
 
 static int rocker_port_router_mac(struct rocker_port *rocker_port,
-				  int flags, __be16 vlan_id)
+				  enum switchdev_trans trans, int flags,
+				  __be16 vlan_id)
 {
 	u32 in_pport_mask = 0xffffffff;
 	__be16 eth_type;
@@ -3546,7 +3668,7 @@ static int rocker_port_router_mac(struct rocker_port *rocker_port,
 		vlan_id = rocker_port->internal_vlan_id;
 
 	eth_type = htons(ETH_P_IP);
-	err = rocker_flow_tbl_term_mac(rocker_port,
+	err = rocker_flow_tbl_term_mac(rocker_port, trans,
 				       rocker_port->pport, in_pport_mask,
 				       eth_type, rocker_port->dev->dev_addr,
 				       dst_mac_mask, vlan_id, vlan_id_mask,
@@ -3555,7 +3677,7 @@ static int rocker_port_router_mac(struct rocker_port *rocker_port,
 		return err;
 
 	eth_type = htons(ETH_P_IPV6);
-	err = rocker_flow_tbl_term_mac(rocker_port,
+	err = rocker_flow_tbl_term_mac(rocker_port, trans,
 				       rocker_port->pport, in_pport_mask,
 				       eth_type, rocker_port->dev->dev_addr,
 				       dst_mac_mask, vlan_id, vlan_id_mask,
@@ -3564,13 +3686,13 @@ static int rocker_port_router_mac(struct rocker_port *rocker_port,
 	return err;
 }
 
-static int rocker_port_fwding(struct rocker_port *rocker_port)
+static int rocker_port_fwding(struct rocker_port *rocker_port,
+			      enum switchdev_trans trans, int flags)
 {
 	bool pop_vlan;
 	u32 out_pport;
 	__be16 vlan_id;
 	u16 vid;
-	int flags = ROCKER_OP_FLAG_NOWAIT;
 	int err;
 
 	/* Port will be forwarding-enabled if its STP state is LEARNING
@@ -3590,9 +3712,8 @@ static int rocker_port_fwding(struct rocker_port *rocker_port)
 			continue;
 		vlan_id = htons(vid);
 		pop_vlan = rocker_vlan_id_is_internal(vlan_id);
-		err = rocker_group_l2_interface(rocker_port, flags,
-						vlan_id, out_pport,
-						pop_vlan);
+		err = rocker_group_l2_interface(rocker_port, trans, flags,
+						vlan_id, out_pport, pop_vlan);
 		if (err) {
 			netdev_err(rocker_port->dev,
 				   "Error (%d) port VLAN l2 group for pport %d\n",
@@ -3604,13 +3725,21 @@ static int rocker_port_fwding(struct rocker_port *rocker_port)
 	return 0;
 }
 
-static int rocker_port_stp_update(struct rocker_port *rocker_port, u8 state)
+static int rocker_port_stp_update(struct rocker_port *rocker_port,
+				  enum switchdev_trans trans, int flags,
+				  u8 state)
 {
 	bool want[ROCKER_CTRL_MAX] = { 0, };
-	int flags;
+	bool prev_ctrls[ROCKER_CTRL_MAX];
+	u8 prev_state;
 	int err;
 	int i;
 
+	if (trans == SWITCHDEV_TRANS_PREPARE) {
+		memcpy(prev_ctrls, rocker_port->ctrls, sizeof(prev_ctrls));
+		prev_state = rocker_port->stp_state;
+	}
+
 	if (rocker_port->stp_state == state)
 		return 0;
 
@@ -3638,45 +3767,57 @@ static int rocker_port_stp_update(struct rocker_port *rocker_port, u8 state)
 
 	for (i = 0; i < ROCKER_CTRL_MAX; i++) {
 		if (want[i] != rocker_port->ctrls[i]) {
-			flags = ROCKER_OP_FLAG_NOWAIT |
-				(want[i] ? 0 : ROCKER_OP_FLAG_REMOVE);
-			err = rocker_port_ctrl(rocker_port, flags,
+			int ctrl_flags = flags |
+					 (want[i] ? 0 : ROCKER_OP_FLAG_REMOVE);
+			err = rocker_port_ctrl(rocker_port, trans, ctrl_flags,
 					       &rocker_ctrls[i]);
 			if (err)
-				return err;
+				goto err_out;
 			rocker_port->ctrls[i] = want[i];
 		}
 	}
 
-	err = rocker_port_fdb_flush(rocker_port);
+	err = rocker_port_fdb_flush(rocker_port, trans, flags);
 	if (err)
-		return err;
+		goto err_out;
+
+	err = rocker_port_fwding(rocker_port, trans, flags);
+
+err_out:
+	if (trans == SWITCHDEV_TRANS_PREPARE) {
+		memcpy(rocker_port->ctrls, prev_ctrls, sizeof(prev_ctrls));
+		rocker_port->stp_state = prev_state;
+	}
 
-	return rocker_port_fwding(rocker_port);
+	return err;
 }
 
-static int rocker_port_fwd_enable(struct rocker_port *rocker_port)
+static int rocker_port_fwd_enable(struct rocker_port *rocker_port,
+				  enum switchdev_trans trans, int flags)
 {
 	if (rocker_port_is_bridged(rocker_port))
 		/* bridge STP will enable port */
 		return 0;
 
 	/* port is not bridged, so simulate going to FORWARDING state */
-	return rocker_port_stp_update(rocker_port, BR_STATE_FORWARDING);
+	return rocker_port_stp_update(rocker_port, trans, flags,
+				      BR_STATE_FORWARDING);
 }
 
-static int rocker_port_fwd_disable(struct rocker_port *rocker_port)
+static int rocker_port_fwd_disable(struct rocker_port *rocker_port,
+				   enum switchdev_trans trans, int flags)
 {
 	if (rocker_port_is_bridged(rocker_port))
 		/* bridge STP will disable port */
 		return 0;
 
 	/* port is not bridged, so simulate going to DISABLED state */
-	return rocker_port_stp_update(rocker_port, BR_STATE_DISABLED);
+	return rocker_port_stp_update(rocker_port, trans, flags,
+				      BR_STATE_DISABLED);
 }
 
 static struct rocker_internal_vlan_tbl_entry *
-rocker_internal_vlan_tbl_find(struct rocker *rocker, int ifindex)
+rocker_internal_vlan_tbl_find(const struct rocker *rocker, int ifindex)
 {
 	struct rocker_internal_vlan_tbl_entry *found;
 
@@ -3731,8 +3872,9 @@ found:
 	return found->vlan_id;
 }
 
-static void rocker_port_internal_vlan_id_put(struct rocker_port *rocker_port,
-					     int ifindex)
+static void
+rocker_port_internal_vlan_id_put(const struct rocker_port *rocker_port,
+				 int ifindex)
 {
 	struct rocker *rocker = rocker_port->rocker;
 	struct rocker_internal_vlan_tbl_entry *found;
@@ -3760,11 +3902,12 @@ not_found:
 	spin_unlock_irqrestore(&rocker->internal_vlan_tbl_lock, lock_flags);
 }
 
-static int rocker_port_fib_ipv4(struct rocker_port *rocker_port, __be32 dst,
-				int dst_len, struct fib_info *fi, u32 tb_id,
-				int flags)
+static int rocker_port_fib_ipv4(struct rocker_port *rocker_port,
+				enum switchdev_trans trans, __be32 dst,
+				int dst_len, const struct fib_info *fi,
+				u32 tb_id, int flags)
 {
-	struct fib_nh *nh;
+	const struct fib_nh *nh;
 	__be16 eth_type = htons(ETH_P_IP);
 	__be32 dst_mask = inet_make_mask(dst_len);
 	__be16 internal_vlan_id = rocker_port->internal_vlan_id;
@@ -3784,7 +3927,7 @@ static int rocker_port_fib_ipv4(struct rocker_port *rocker_port, __be32 dst,
 	has_gw = !!nh->nh_gw;
 
 	if (has_gw && nh_on_port) {
-		err = rocker_port_ipv4_nh(rocker_port, flags,
+		err = rocker_port_ipv4_nh(rocker_port, trans, flags,
 					  nh->nh_gw, &index);
 		if (err)
 			return err;
@@ -3795,7 +3938,7 @@ static int rocker_port_fib_ipv4(struct rocker_port *rocker_port, __be32 dst,
 		group_id = ROCKER_GROUP_L2_INTERFACE(internal_vlan_id, 0);
 	}
 
-	err = rocker_flow_tbl_ucast4_routing(rocker_port, eth_type, dst,
+	err = rocker_flow_tbl_ucast4_routing(rocker_port, trans, eth_type, dst,
 					     dst_mask, priority, goto_tbl,
 					     group_id, flags);
 	if (err)
@@ -3834,7 +3977,7 @@ static int rocker_port_open(struct net_device *dev)
 		goto err_request_rx_irq;
 	}
 
-	err = rocker_port_fwd_enable(rocker_port);
+	err = rocker_port_fwd_enable(rocker_port, SWITCHDEV_TRANS_NONE, 0);
 	if (err)
 		goto err_fwd_enable;
 
@@ -3861,7 +4004,8 @@ static int rocker_port_stop(struct net_device *dev)
 	rocker_port_set_enable(rocker_port, false);
 	napi_disable(&rocker_port->napi_rx);
 	napi_disable(&rocker_port->napi_tx);
-	rocker_port_fwd_disable(rocker_port);
+	rocker_port_fwd_disable(rocker_port, SWITCHDEV_TRANS_NONE,
+				ROCKER_OP_FLAG_NOWAIT);
 	free_irq(rocker_msix_rx_vector(rocker_port), rocker_port);
 	free_irq(rocker_msix_tx_vector(rocker_port), rocker_port);
 	rocker_port_dma_rings_fini(rocker_port);
@@ -3869,12 +4013,12 @@ static int rocker_port_stop(struct net_device *dev)
 	return 0;
 }
 
-static void rocker_tx_desc_frags_unmap(struct rocker_port *rocker_port,
-				       struct rocker_desc_info *desc_info)
+static void rocker_tx_desc_frags_unmap(const struct rocker_port *rocker_port,
+				       const struct rocker_desc_info *desc_info)
 {
-	struct rocker *rocker = rocker_port->rocker;
+	const struct rocker *rocker = rocker_port->rocker;
 	struct pci_dev *pdev = rocker->pdev;
-	struct rocker_tlv *attrs[ROCKER_TLV_TX_MAX + 1];
+	const struct rocker_tlv *attrs[ROCKER_TLV_TX_MAX + 1];
 	struct rocker_tlv *attr;
 	int rem;
 
@@ -3882,7 +4026,7 @@ static void rocker_tx_desc_frags_unmap(struct rocker_port *rocker_port,
 	if (!attrs[ROCKER_TLV_TX_FRAGS])
 		return;
 	rocker_tlv_for_each_nested(attr, attrs[ROCKER_TLV_TX_FRAGS], rem) {
-		struct rocker_tlv *frag_attrs[ROCKER_TLV_TX_FRAG_ATTR_MAX + 1];
+		const struct rocker_tlv *frag_attrs[ROCKER_TLV_TX_FRAG_ATTR_MAX + 1];
 		dma_addr_t dma_handle;
 		size_t len;
 
@@ -3899,11 +4043,11 @@ static void rocker_tx_desc_frags_unmap(struct rocker_port *rocker_port,
 	}
 }
 
-static int rocker_tx_desc_frag_map_put(struct rocker_port *rocker_port,
+static int rocker_tx_desc_frag_map_put(const struct rocker_port *rocker_port,
 				       struct rocker_desc_info *desc_info,
 				       char *buf, size_t buf_len)
 {
-	struct rocker *rocker = rocker_port->rocker;
+	const struct rocker *rocker = rocker_port->rocker;
 	struct pci_dev *pdev = rocker->pdev;
 	dma_addr_t dma_handle;
 	struct rocker_tlv *frag;
@@ -4008,269 +4152,359 @@ static int rocker_port_set_mac_address(struct net_device *dev, void *p)
 	return 0;
 }
 
-static int rocker_port_vlan_rx_add_vid(struct net_device *dev,
-				       __be16 proto, u16 vid)
+static int rocker_port_get_phys_port_name(struct net_device *dev,
+					  char *buf, size_t len)
 {
 	struct rocker_port *rocker_port = netdev_priv(dev);
+	struct port_name name = { .buf = buf, .len = len };
 	int err;
 
-	err = rocker_port_vlan(rocker_port, 0, vid);
-	if (err)
-		return err;
+	err = rocker_cmd_exec(rocker_port, SWITCHDEV_TRANS_NONE, 0,
+			      rocker_cmd_get_port_settings_prep, NULL,
+			      rocker_cmd_get_port_settings_phys_name_proc,
+			      &name);
 
-	return rocker_port_router_mac(rocker_port, 0, htons(vid));
+	return err ? -EOPNOTSUPP : 0;
 }
 
-static int rocker_port_vlan_rx_kill_vid(struct net_device *dev,
-					__be16 proto, u16 vid)
+static const struct net_device_ops rocker_port_netdev_ops = {
+	.ndo_open			= rocker_port_open,
+	.ndo_stop			= rocker_port_stop,
+	.ndo_start_xmit			= rocker_port_xmit,
+	.ndo_set_mac_address		= rocker_port_set_mac_address,
+	.ndo_bridge_getlink		= switchdev_port_bridge_getlink,
+	.ndo_bridge_setlink		= switchdev_port_bridge_setlink,
+	.ndo_bridge_dellink		= switchdev_port_bridge_dellink,
+	.ndo_fdb_add			= switchdev_port_fdb_add,
+	.ndo_fdb_del			= switchdev_port_fdb_del,
+	.ndo_fdb_dump			= switchdev_port_fdb_dump,
+	.ndo_get_phys_port_name		= rocker_port_get_phys_port_name,
+};
+
+/********************
+ * swdev interface
+ ********************/
+
+static int rocker_port_attr_get(struct net_device *dev,
+				struct switchdev_attr *attr)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
-	int err;
+	const struct rocker_port *rocker_port = netdev_priv(dev);
+	const struct rocker *rocker = rocker_port->rocker;
 
-	err = rocker_port_router_mac(rocker_port, ROCKER_OP_FLAG_REMOVE,
-				     htons(vid));
-	if (err)
-		return err;
+	switch (attr->id) {
+	case SWITCHDEV_ATTR_PORT_PARENT_ID:
+		attr->u.ppid.id_len = sizeof(rocker->hw.id);
+		memcpy(&attr->u.ppid.id, &rocker->hw.id, attr->u.ppid.id_len);
+		break;
+	case SWITCHDEV_ATTR_PORT_BRIDGE_FLAGS:
+		attr->u.brport_flags = rocker_port->brport_flags;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
 
-	return rocker_port_vlan(rocker_port, ROCKER_OP_FLAG_REMOVE, vid);
+	return 0;
 }
 
-static int rocker_port_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
-			       struct net_device *dev,
-			       const unsigned char *addr, u16 vid,
-			       u16 nlm_flags)
+static void rocker_port_trans_abort(const struct rocker_port *rocker_port)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
-	__be16 vlan_id = rocker_port_vid_to_vlan(rocker_port, vid, NULL);
-	int flags = 0;
-
-	if (!rocker_port_is_bridged(rocker_port))
-		return -EINVAL;
+	struct list_head *mem, *tmp;
 
-	return rocker_port_fdb(rocker_port, addr, vlan_id, flags);
+	list_for_each_safe(mem, tmp, &rocker_port->trans_mem) {
+		list_del(mem);
+		kfree(mem);
+	}
 }
 
-static int rocker_port_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
-			       struct net_device *dev,
-			       const unsigned char *addr, u16 vid)
+static int rocker_port_brport_flags_set(struct rocker_port *rocker_port,
+					enum switchdev_trans trans,
+					unsigned long brport_flags)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
-	__be16 vlan_id = rocker_port_vid_to_vlan(rocker_port, vid, NULL);
-	int flags = ROCKER_OP_FLAG_REMOVE;
+	unsigned long orig_flags;
+	int err = 0;
 
-	if (!rocker_port_is_bridged(rocker_port))
-		return -EINVAL;
+	orig_flags = rocker_port->brport_flags;
+	rocker_port->brport_flags = brport_flags;
+	if ((orig_flags ^ rocker_port->brport_flags) & BR_LEARNING)
+		err = rocker_port_set_learning(rocker_port, trans);
 
-	return rocker_port_fdb(rocker_port, addr, vlan_id, flags);
+	if (trans == SWITCHDEV_TRANS_PREPARE)
+		rocker_port->brport_flags = orig_flags;
+
+	return err;
 }
 
-static int rocker_fdb_fill_info(struct sk_buff *skb,
-				struct rocker_port *rocker_port,
-				const unsigned char *addr, u16 vid,
-				u32 portid, u32 seq, int type,
-				unsigned int flags)
+static int rocker_port_attr_set(struct net_device *dev,
+				struct switchdev_attr *attr)
 {
-	struct nlmsghdr *nlh;
-	struct ndmsg *ndm;
+	struct rocker_port *rocker_port = netdev_priv(dev);
+	int err = 0;
 
-	nlh = nlmsg_put(skb, portid, seq, type, sizeof(*ndm), flags);
-	if (!nlh)
-		return -EMSGSIZE;
+	switch (attr->trans) {
+	case SWITCHDEV_TRANS_PREPARE:
+		BUG_ON(!list_empty(&rocker_port->trans_mem));
+		break;
+	case SWITCHDEV_TRANS_ABORT:
+		rocker_port_trans_abort(rocker_port);
+		return 0;
+	default:
+		break;
+	}
 
-	ndm = nlmsg_data(nlh);
-	ndm->ndm_family	 = AF_BRIDGE;
-	ndm->ndm_pad1    = 0;
-	ndm->ndm_pad2    = 0;
-	ndm->ndm_flags	 = NTF_SELF;
-	ndm->ndm_type	 = 0;
-	ndm->ndm_ifindex = rocker_port->dev->ifindex;
-	ndm->ndm_state   = NUD_REACHABLE;
+	switch (attr->id) {
+	case SWITCHDEV_ATTR_PORT_STP_STATE:
+		err = rocker_port_stp_update(rocker_port, attr->trans,
+					     ROCKER_OP_FLAG_NOWAIT,
+					     attr->u.stp_state);
+		break;
+	case SWITCHDEV_ATTR_PORT_BRIDGE_FLAGS:
+		err = rocker_port_brport_flags_set(rocker_port, attr->trans,
+						   attr->u.brport_flags);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
 
-	if (nla_put(skb, NDA_LLADDR, ETH_ALEN, addr))
-		goto nla_put_failure;
+	return err;
+}
 
-	if (vid && nla_put_u16(skb, NDA_VLAN, vid))
-		goto nla_put_failure;
+static int rocker_port_vlan_add(struct rocker_port *rocker_port,
+				enum switchdev_trans trans, u16 vid, u16 flags)
+{
+	int err;
 
-	nlmsg_end(skb, nlh);
-	return 0;
+	/* XXX deal with flags for PVID and untagged */
 
-nla_put_failure:
-	nlmsg_cancel(skb, nlh);
-	return -EMSGSIZE;
+	err = rocker_port_vlan(rocker_port, trans, 0, vid);
+	if (err)
+		return err;
+
+	err = rocker_port_router_mac(rocker_port, trans, 0, htons(vid));
+	if (err)
+		rocker_port_vlan(rocker_port, trans,
+				 ROCKER_OP_FLAG_REMOVE, vid);
+
+	return err;
 }
 
-static int rocker_port_fdb_dump(struct sk_buff *skb,
-				struct netlink_callback *cb,
-				struct net_device *dev,
-				struct net_device *filter_dev,
-				int idx)
+static int rocker_port_vlans_add(struct rocker_port *rocker_port,
+				 enum switchdev_trans trans,
+				 const struct switchdev_obj_vlan *vlan)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
-	struct rocker *rocker = rocker_port->rocker;
-	struct rocker_fdb_tbl_entry *found;
-	struct hlist_node *tmp;
-	int bkt;
-	unsigned long lock_flags;
-	const unsigned char *addr;
 	u16 vid;
 	int err;
 
-	spin_lock_irqsave(&rocker->fdb_tbl_lock, lock_flags);
-	hash_for_each_safe(rocker->fdb_tbl, bkt, tmp, found, entry) {
-		if (found->key.pport != rocker_port->pport)
-			continue;
-		if (idx < cb->args[0])
-			goto skip;
-		addr = found->key.addr;
-		vid = rocker_port_vlan_to_vid(rocker_port, found->key.vlan_id);
-		err = rocker_fdb_fill_info(skb, rocker_port, addr, vid,
-					   NETLINK_CB(cb->skb).portid,
-					   cb->nlh->nlmsg_seq,
-					   RTM_NEWNEIGH, NLM_F_MULTI);
-		if (err < 0)
-			break;
-skip:
-		++idx;
+	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
+		err = rocker_port_vlan_add(rocker_port, trans,
+					   vid, vlan->flags);
+		if (err)
+			return err;
 	}
-	spin_unlock_irqrestore(&rocker->fdb_tbl_lock, lock_flags);
-	return idx;
+
+	return 0;
 }
 
-static int rocker_port_bridge_setlink(struct net_device *dev,
-				      struct nlmsghdr *nlh, u16 flags)
+static int rocker_port_fdb_add(struct rocker_port *rocker_port,
+			       enum switchdev_trans trans,
+			       const struct switchdev_obj_fdb *fdb)
+{
+	__be16 vlan_id = rocker_port_vid_to_vlan(rocker_port, fdb->vid, NULL);
+	int flags = 0;
+
+	if (!rocker_port_is_bridged(rocker_port))
+		return -EINVAL;
+
+	return rocker_port_fdb(rocker_port, trans, fdb->addr, vlan_id, flags);
+}
+
+static int rocker_port_obj_add(struct net_device *dev,
+			       struct switchdev_obj *obj)
 {
 	struct rocker_port *rocker_port = netdev_priv(dev);
-	struct nlattr *protinfo;
-	struct nlattr *attr;
-	int err;
+	const struct switchdev_obj_ipv4_fib *fib4;
+	int err = 0;
 
-	protinfo = nlmsg_find_attr(nlh, sizeof(struct ifinfomsg),
-				   IFLA_PROTINFO);
-	if (protinfo) {
-		attr = nla_find_nested(protinfo, IFLA_BRPORT_LEARNING);
-		if (attr) {
-			if (nla_len(attr) < sizeof(u8))
-				return -EINVAL;
-
-			if (nla_get_u8(attr))
-				rocker_port->brport_flags |= BR_LEARNING;
-			else
-				rocker_port->brport_flags &= ~BR_LEARNING;
-			err = rocker_port_set_learning(rocker_port);
-			if (err)
-				return err;
-		}
-		attr = nla_find_nested(protinfo, IFLA_BRPORT_LEARNING_SYNC);
-		if (attr) {
-			if (nla_len(attr) < sizeof(u8))
-				return -EINVAL;
-
-			if (nla_get_u8(attr))
-				rocker_port->brport_flags |= BR_LEARNING_SYNC;
-			else
-				rocker_port->brport_flags &= ~BR_LEARNING_SYNC;
-		}
+	switch (obj->trans) {
+	case SWITCHDEV_TRANS_PREPARE:
+		BUG_ON(!list_empty(&rocker_port->trans_mem));
+		break;
+	case SWITCHDEV_TRANS_ABORT:
+		rocker_port_trans_abort(rocker_port);
+		return 0;
+	default:
+		break;
 	}
 
-	return 0;
+	switch (obj->id) {
+	case SWITCHDEV_OBJ_PORT_VLAN:
+		err = rocker_port_vlans_add(rocker_port, obj->trans,
+					    &obj->u.vlan);
+		break;
+	case SWITCHDEV_OBJ_IPV4_FIB:
+		fib4 = &obj->u.ipv4_fib;
+		err = rocker_port_fib_ipv4(rocker_port, obj->trans,
+					   htonl(fib4->dst), fib4->dst_len,
+					   fib4->fi, fib4->tb_id, 0);
+		break;
+	case SWITCHDEV_OBJ_PORT_FDB:
+		err = rocker_port_fdb_add(rocker_port, obj->trans, &obj->u.fdb);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+
+	return err;
 }
 
-static int rocker_port_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
-				      struct net_device *dev,
-				      u32 filter_mask, int nlflags)
+static int rocker_port_vlan_del(struct rocker_port *rocker_port,
+				u16 vid, u16 flags)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
-	u16 mode = BRIDGE_MODE_UNDEF;
-	u32 mask = BR_LEARNING | BR_LEARNING_SYNC;
+	int err;
 
-	return ndo_dflt_bridge_getlink(skb, pid, seq, dev, mode,
-				       rocker_port->brport_flags, mask,
-				       nlflags);
+	err = rocker_port_router_mac(rocker_port, SWITCHDEV_TRANS_NONE,
+				     ROCKER_OP_FLAG_REMOVE, htons(vid));
+	if (err)
+		return err;
+
+	return rocker_port_vlan(rocker_port, SWITCHDEV_TRANS_NONE,
+				ROCKER_OP_FLAG_REMOVE, vid);
 }
 
-static int rocker_port_get_phys_port_name(struct net_device *dev,
-					  char *buf, size_t len)
+static int rocker_port_vlans_del(struct rocker_port *rocker_port,
+				 const struct switchdev_obj_vlan *vlan)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
-	struct port_name name = { .buf = buf, .len = len };
+	u16 vid;
 	int err;
 
-	err = rocker_cmd_exec(rocker_port->rocker, rocker_port,
-			      rocker_cmd_get_port_settings_prep, NULL,
-			      rocker_cmd_get_port_settings_phys_name_proc,
-			      &name, false);
+	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
+		err = rocker_port_vlan_del(rocker_port, vid, vlan->flags);
+		if (err)
+			return err;
+	}
 
-	return err ? -EOPNOTSUPP : 0;
+	return 0;
 }
 
-static const struct net_device_ops rocker_port_netdev_ops = {
-	.ndo_open			= rocker_port_open,
-	.ndo_stop			= rocker_port_stop,
-	.ndo_start_xmit			= rocker_port_xmit,
-	.ndo_set_mac_address		= rocker_port_set_mac_address,
-	.ndo_vlan_rx_add_vid		= rocker_port_vlan_rx_add_vid,
-	.ndo_vlan_rx_kill_vid		= rocker_port_vlan_rx_kill_vid,
-	.ndo_fdb_add			= rocker_port_fdb_add,
-	.ndo_fdb_del			= rocker_port_fdb_del,
-	.ndo_fdb_dump			= rocker_port_fdb_dump,
-	.ndo_bridge_setlink		= rocker_port_bridge_setlink,
-	.ndo_bridge_getlink		= rocker_port_bridge_getlink,
-	.ndo_get_phys_port_name		= rocker_port_get_phys_port_name,
-};
+static int rocker_port_fdb_del(struct rocker_port *rocker_port,
+			       enum switchdev_trans trans,
+			       const struct switchdev_obj_fdb *fdb)
+{
+	__be16 vlan_id = rocker_port_vid_to_vlan(rocker_port, fdb->vid, NULL);
+	int flags = ROCKER_OP_FLAG_NOWAIT | ROCKER_OP_FLAG_REMOVE;
 
-/********************
- * swdev interface
- ********************/
+	if (!rocker_port_is_bridged(rocker_port))
+		return -EINVAL;
+
+	return rocker_port_fdb(rocker_port, trans, fdb->addr, vlan_id, flags);
+}
 
-static int rocker_port_swdev_parent_id_get(struct net_device *dev,
-					   struct netdev_phys_item_id *psid)
+static int rocker_port_obj_del(struct net_device *dev,
+			       struct switchdev_obj *obj)
 {
 	struct rocker_port *rocker_port = netdev_priv(dev);
-	struct rocker *rocker = rocker_port->rocker;
+	const struct switchdev_obj_ipv4_fib *fib4;
+	int err = 0;
 
-	psid->id_len = sizeof(rocker->hw.id);
-	memcpy(&psid->id, &rocker->hw.id, psid->id_len);
-	return 0;
+	switch (obj->id) {
+	case SWITCHDEV_OBJ_PORT_VLAN:
+		err = rocker_port_vlans_del(rocker_port, &obj->u.vlan);
+		break;
+	case SWITCHDEV_OBJ_IPV4_FIB:
+		fib4 = &obj->u.ipv4_fib;
+		err = rocker_port_fib_ipv4(rocker_port, SWITCHDEV_TRANS_NONE,
+					   htonl(fib4->dst), fib4->dst_len,
+					   fib4->fi, fib4->tb_id,
+					   ROCKER_OP_FLAG_REMOVE);
+		break;
+	case SWITCHDEV_OBJ_PORT_FDB:
+		err = rocker_port_fdb_del(rocker_port, obj->trans, &obj->u.fdb);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+
+	return err;
 }
 
-static int rocker_port_swdev_port_stp_update(struct net_device *dev, u8 state)
+static int rocker_port_fdb_dump(const struct rocker_port *rocker_port,
+				struct switchdev_obj *obj)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
+	struct rocker *rocker = rocker_port->rocker;
+	struct switchdev_obj_fdb *fdb = &obj->u.fdb;
+	struct rocker_fdb_tbl_entry *found;
+	struct hlist_node *tmp;
+	unsigned long lock_flags;
+	int bkt;
+	int err = 0;
 
-	return rocker_port_stp_update(rocker_port, state);
+	spin_lock_irqsave(&rocker->fdb_tbl_lock, lock_flags);
+	hash_for_each_safe(rocker->fdb_tbl, bkt, tmp, found, entry) {
+		if (found->key.pport != rocker_port->pport)
+			continue;
+		fdb->addr = found->key.addr;
+		fdb->vid = rocker_port_vlan_to_vid(rocker_port,
+						   found->key.vlan_id);
+		err = obj->cb(rocker_port->dev, obj);
+		if (err)
+			break;
+	}
+	spin_unlock_irqrestore(&rocker->fdb_tbl_lock, lock_flags);
+
+	return err;
 }
 
-static int rocker_port_swdev_fib_ipv4_add(struct net_device *dev,
-					  __be32 dst, int dst_len,
-					  struct fib_info *fi,
-					  u8 tos, u8 type,
-					  u32 nlflags, u32 tb_id)
+static int rocker_port_vlan_dump(const struct rocker_port *rocker_port,
+				 struct switchdev_obj *obj)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
-	int flags = 0;
+	struct switchdev_obj_vlan *vlan = &obj->u.vlan;
+	u16 vid;
+	int err = 0;
 
-	return rocker_port_fib_ipv4(rocker_port, dst, dst_len,
-				    fi, tb_id, flags);
+	for (vid = 1; vid < VLAN_N_VID; vid++) {
+		if (!test_bit(vid, rocker_port->vlan_bitmap))
+			continue;
+		vlan->flags = 0;
+		if (rocker_vlan_id_is_internal(htons(vid)))
+			vlan->flags |= BRIDGE_VLAN_INFO_PVID;
+		vlan->vid_begin = vlan->vid_end = vid;
+		err = obj->cb(rocker_port->dev, obj);
+		if (err)
+			break;
+	}
+
+	return err;
 }
 
-static int rocker_port_swdev_fib_ipv4_del(struct net_device *dev,
-					  __be32 dst, int dst_len,
-					  struct fib_info *fi,
-					  u8 tos, u8 type, u32 tb_id)
+static int rocker_port_obj_dump(struct net_device *dev,
+				struct switchdev_obj *obj)
 {
-	struct rocker_port *rocker_port = netdev_priv(dev);
-	int flags = ROCKER_OP_FLAG_REMOVE;
+	const struct rocker_port *rocker_port = netdev_priv(dev);
+	int err = 0;
 
-	return rocker_port_fib_ipv4(rocker_port, dst, dst_len,
-				    fi, tb_id, flags);
+	switch (obj->id) {
+	case SWITCHDEV_OBJ_PORT_FDB:
+		err = rocker_port_fdb_dump(rocker_port, obj);
+		break;
+	case SWITCHDEV_OBJ_PORT_VLAN:
+		err = rocker_port_vlan_dump(rocker_port, obj);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+
+	return err;
 }
 
-static const struct swdev_ops rocker_port_swdev_ops = {
-	.swdev_parent_id_get		= rocker_port_swdev_parent_id_get,
-	.swdev_port_stp_update		= rocker_port_swdev_port_stp_update,
-	.swdev_fib_ipv4_add		= rocker_port_swdev_fib_ipv4_add,
-	.swdev_fib_ipv4_del		= rocker_port_swdev_fib_ipv4_del,
+static const struct switchdev_ops rocker_port_switchdev_ops = {
+	.switchdev_port_attr_get	= rocker_port_attr_get,
+	.switchdev_port_attr_set	= rocker_port_attr_set,
+	.switchdev_port_obj_add		= rocker_port_obj_add,
+	.switchdev_port_obj_del		= rocker_port_obj_del,
+	.switchdev_port_obj_dump	= rocker_port_obj_dump,
 };
 
 /********************
@@ -4334,8 +4568,7 @@ static void rocker_port_get_strings(struct net_device *netdev, u32 stringset,
 }
 
 static int
-rocker_cmd_get_port_stats_prep(struct rocker *rocker,
-			       struct rocker_port *rocker_port,
+rocker_cmd_get_port_stats_prep(const struct rocker_port *rocker_port,
 			       struct rocker_desc_info *desc_info,
 			       void *priv)
 {
@@ -4359,14 +4592,13 @@ rocker_cmd_get_port_stats_prep(struct rocker *rocker,
 }
 
 static int
-rocker_cmd_get_port_stats_ethtool_proc(struct rocker *rocker,
-				       struct rocker_port *rocker_port,
-				       struct rocker_desc_info *desc_info,
+rocker_cmd_get_port_stats_ethtool_proc(const struct rocker_port *rocker_port,
+				       const struct rocker_desc_info *desc_info,
 				       void *priv)
 {
-	struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
-	struct rocker_tlv *stats_attrs[ROCKER_TLV_CMD_PORT_STATS_MAX + 1];
-	struct rocker_tlv *pattr;
+	const struct rocker_tlv *attrs[ROCKER_TLV_CMD_MAX + 1];
+	const struct rocker_tlv *stats_attrs[ROCKER_TLV_CMD_PORT_STATS_MAX + 1];
+	const struct rocker_tlv *pattr;
 	u32 pport;
 	u64 *data = priv;
 	int i;
@@ -4400,10 +4632,10 @@ rocker_cmd_get_port_stats_ethtool_proc(struct rocker *rocker,
 static int rocker_cmd_get_port_stats_ethtool(struct rocker_port *rocker_port,
 					     void *priv)
 {
-	return rocker_cmd_exec(rocker_port->rocker, rocker_port,
+	return rocker_cmd_exec(rocker_port, SWITCHDEV_TRANS_NONE, 0,
 			       rocker_cmd_get_port_stats_prep, NULL,
 			       rocker_cmd_get_port_stats_ethtool_proc,
-			       priv, false);
+			       priv);
 }
 
 static void rocker_port_get_stats(struct net_device *dev,
@@ -4417,8 +4649,6 @@ static void rocker_port_get_stats(struct net_device *dev,
 		for (i = 0; i < ARRAY_SIZE(rocker_port_stats); ++i)
 			data[i] = 0;
 	}
-
-	return;
 }
 
 static int rocker_port_get_sset_count(struct net_device *netdev, int sset)
@@ -4453,8 +4683,8 @@ static struct rocker_port *rocker_port_napi_tx_get(struct napi_struct *napi)
 static int rocker_port_poll_tx(struct napi_struct *napi, int budget)
 {
 	struct rocker_port *rocker_port = rocker_port_napi_tx_get(napi);
-	struct rocker *rocker = rocker_port->rocker;
-	struct rocker_desc_info *desc_info;
+	const struct rocker *rocker = rocker_port->rocker;
+	const struct rocker_desc_info *desc_info;
 	u32 credits = 0;
 	int err;
 
@@ -4472,8 +4702,9 @@ static int rocker_port_poll_tx(struct napi_struct *napi, int budget)
 		if (err == 0) {
 			rocker_port->dev->stats.tx_packets++;
 			rocker_port->dev->stats.tx_bytes += skb->len;
-		} else
+		} else {
 			rocker_port->dev->stats.tx_errors++;
+		}
 
 		dev_kfree_skb_any(skb);
 		credits++;
@@ -4488,11 +4719,11 @@ static int rocker_port_poll_tx(struct napi_struct *napi, int budget)
 	return 0;
 }
 
-static int rocker_port_rx_proc(struct rocker *rocker,
-			       struct rocker_port *rocker_port,
+static int rocker_port_rx_proc(const struct rocker *rocker,
+			       const struct rocker_port *rocker_port,
 			       struct rocker_desc_info *desc_info)
 {
-	struct rocker_tlv *attrs[ROCKER_TLV_RX_MAX + 1];
+	const struct rocker_tlv *attrs[ROCKER_TLV_RX_MAX + 1];
 	struct sk_buff *skb = rocker_desc_cookie_ptr_get(desc_info);
 	size_t rx_len;
 
@@ -4514,7 +4745,7 @@ static int rocker_port_rx_proc(struct rocker *rocker,
 
 	netif_receive_skb(skb);
 
-	return rocker_dma_rx_ring_skb_alloc(rocker, rocker_port, desc_info);
+	return rocker_dma_rx_ring_skb_alloc(rocker_port, desc_info);
 }
 
 static struct rocker_port *rocker_port_napi_rx_get(struct napi_struct *napi)
@@ -4525,7 +4756,7 @@ static struct rocker_port *rocker_port_napi_rx_get(struct napi_struct *napi)
 static int rocker_port_poll_rx(struct napi_struct *napi, int budget)
 {
 	struct rocker_port *rocker_port = rocker_port_napi_rx_get(napi);
-	struct rocker *rocker = rocker_port->rocker;
+	const struct rocker *rocker = rocker_port->rocker;
 	struct rocker_desc_info *desc_info;
 	u32 credits = 0;
 	int err;
@@ -4565,9 +4796,9 @@ static int rocker_port_poll_rx(struct napi_struct *napi, int budget)
  * PCI driver ops
  *****************/
 
-static void rocker_carrier_init(struct rocker_port *rocker_port)
+static void rocker_carrier_init(const struct rocker_port *rocker_port)
 {
-	struct rocker *rocker = rocker_port->rocker;
+	const struct rocker *rocker = rocker_port->rocker;
 	u64 link_status = rocker_read64(rocker, PORT_PHYS_LINK_STATUS);
 	bool link_up;
 
@@ -4578,23 +4809,26 @@ static void rocker_carrier_init(struct rocker_port *rocker_port)
 		netif_carrier_off(rocker_port->dev);
 }
 
-static void rocker_remove_ports(struct rocker *rocker)
+static void rocker_remove_ports(const struct rocker *rocker)
 {
 	struct rocker_port *rocker_port;
 	int i;
 
 	for (i = 0; i < rocker->port_count; i++) {
 		rocker_port = rocker->ports[i];
-		rocker_port_ig_tbl(rocker_port, ROCKER_OP_FLAG_REMOVE);
+		if (!rocker_port)
+			continue;
+		rocker_port_ig_tbl(rocker_port, SWITCHDEV_TRANS_NONE,
+				   ROCKER_OP_FLAG_REMOVE);
 		unregister_netdev(rocker_port->dev);
 	}
 	kfree(rocker->ports);
 }
 
-static void rocker_port_dev_addr_init(struct rocker *rocker,
-				      struct rocker_port *rocker_port)
+static void rocker_port_dev_addr_init(struct rocker_port *rocker_port)
 {
-	struct pci_dev *pdev = rocker->pdev;
+	const struct rocker *rocker = rocker_port->rocker;
+	const struct pci_dev *pdev = rocker->pdev;
 	int err;
 
 	err = rocker_cmd_get_port_settings_macaddr(rocker_port,
@@ -4607,9 +4841,10 @@ static void rocker_port_dev_addr_init(struct rocker *rocker,
 
 static int rocker_probe_port(struct rocker *rocker, unsigned int port_number)
 {
-	struct pci_dev *pdev = rocker->pdev;
+	const struct pci_dev *pdev = rocker->pdev;
 	struct rocker_port *rocker_port;
 	struct net_device *dev;
+	u16 untagged_vid = 0;
 	int err;
 
 	dev = alloc_etherdev(sizeof(struct rocker_port));
@@ -4621,20 +4856,19 @@ static int rocker_probe_port(struct rocker *rocker, unsigned int port_number)
 	rocker_port->port_number = port_number;
 	rocker_port->pport = port_number + 1;
 	rocker_port->brport_flags = BR_LEARNING | BR_LEARNING_SYNC;
+	INIT_LIST_HEAD(&rocker_port->trans_mem);
 
-	rocker_port_dev_addr_init(rocker, rocker_port);
+	rocker_port_dev_addr_init(rocker_port);
 	dev->netdev_ops = &rocker_port_netdev_ops;
 	dev->ethtool_ops = &rocker_port_ethtool_ops;
-	dev->swdev_ops = &rocker_port_swdev_ops;
+	dev->switchdev_ops = &rocker_port_switchdev_ops;
 	netif_napi_add(dev, &rocker_port->napi_tx, rocker_port_poll_tx,
 		       NAPI_POLL_WEIGHT);
 	netif_napi_add(dev, &rocker_port->napi_rx, rocker_port_poll_rx,
 		       NAPI_POLL_WEIGHT);
 	rocker_carrier_init(rocker_port);
 
-	dev->features |= NETIF_F_NETNS_LOCAL |
-			 NETIF_F_HW_VLAN_CTAG_FILTER |
-			 NETIF_F_HW_SWITCH_OFFLOAD;
+	dev->features |= NETIF_F_NETNS_LOCAL;
 
 	err = register_netdev(dev);
 	if (err) {
@@ -4643,18 +4877,29 @@ static int rocker_probe_port(struct rocker *rocker, unsigned int port_number)
 	}
 	rocker->ports[port_number] = rocker_port;
 
-	rocker_port_set_learning(rocker_port);
+	rocker_port_set_learning(rocker_port, SWITCHDEV_TRANS_NONE);
 
-	rocker_port->internal_vlan_id =
-		rocker_port_internal_vlan_id_get(rocker_port, dev->ifindex);
-	err = rocker_port_ig_tbl(rocker_port, 0);
+	err = rocker_port_ig_tbl(rocker_port, SWITCHDEV_TRANS_NONE, 0);
 	if (err) {
 		dev_err(&pdev->dev, "install ig port table failed\n");
 		goto err_port_ig_tbl;
 	}
 
+	rocker_port->internal_vlan_id =
+		rocker_port_internal_vlan_id_get(rocker_port, dev->ifindex);
+
+	err = rocker_port_vlan_add(rocker_port, SWITCHDEV_TRANS_NONE,
+				   untagged_vid, 0);
+	if (err) {
+		netdev_err(rocker_port->dev, "install untagged VLAN failed\n");
+		goto err_untagged_vlan;
+	}
+
 	return 0;
 
+err_untagged_vlan:
+	rocker_port_ig_tbl(rocker_port, SWITCHDEV_TRANS_NONE,
+			   ROCKER_OP_FLAG_REMOVE);
 err_port_ig_tbl:
 	unregister_netdev(dev);
 err_register_netdev:
@@ -4669,7 +4914,7 @@ static int rocker_probe_ports(struct rocker *rocker)
 	int err;
 
 	alloc_size = sizeof(struct rocker_port *) * rocker->port_count;
-	rocker->ports = kmalloc(alloc_size, GFP_KERNEL);
+	rocker->ports = kzalloc(alloc_size, GFP_KERNEL);
 	if (!rocker->ports)
 		return -ENOMEM;
 	for (i = 0; i < rocker->port_count; i++) {
@@ -4718,7 +4963,7 @@ err_enable_msix:
 	return err;
 }
 
-static void rocker_msix_fini(struct rocker *rocker)
+static void rocker_msix_fini(const struct rocker *rocker)
 {
 	pci_disable_msix(rocker->pdev);
 	kfree(rocker->msix_entries);
@@ -4884,7 +5129,7 @@ static struct pci_driver rocker_pci_driver = {
  * Net device notifier event handler
  ************************************/
 
-static bool rocker_port_dev_check(struct net_device *dev)
+static bool rocker_port_dev_check(const struct net_device *dev)
 {
 	return dev->netdev_ops == &rocker_port_netdev_ops;
 }
@@ -4892,45 +5137,55 @@ static bool rocker_port_dev_check(struct net_device *dev)
 static int rocker_port_bridge_join(struct rocker_port *rocker_port,
 				   struct net_device *bridge)
 {
+	u16 untagged_vid = 0;
 	int err;
 
+	/* Port is joining bridge, so the internal VLAN for the
+	 * port is going to change to the bridge internal VLAN.
+	 * Let's remove untagged VLAN (vid=0) from port and
+	 * re-add once internal VLAN has changed.
+	 */
+
+	err = rocker_port_vlan_del(rocker_port, untagged_vid, 0);
+	if (err)
+		return err;
+
 	rocker_port_internal_vlan_id_put(rocker_port,
 					 rocker_port->dev->ifindex);
+	rocker_port->internal_vlan_id =
+		rocker_port_internal_vlan_id_get(rocker_port, bridge->ifindex);
 
 	rocker_port->bridge_dev = bridge;
 
-	/* Use bridge internal VLAN ID for untagged pkts */
-	err = rocker_port_vlan(rocker_port, ROCKER_OP_FLAG_REMOVE, 0);
-	if (err)
-		return err;
-	rocker_port->internal_vlan_id =
-		rocker_port_internal_vlan_id_get(rocker_port,
-						 bridge->ifindex);
-	return rocker_port_vlan(rocker_port, 0, 0);
+	return rocker_port_vlan_add(rocker_port, SWITCHDEV_TRANS_NONE,
+				    untagged_vid, 0);
 }
 
 static int rocker_port_bridge_leave(struct rocker_port *rocker_port)
 {
+	u16 untagged_vid = 0;
 	int err;
 
-	rocker_port_internal_vlan_id_put(rocker_port,
-					 rocker_port->bridge_dev->ifindex);
-
-	rocker_port->bridge_dev = NULL;
-
-	/* Use port internal VLAN ID for untagged pkts */
-	err = rocker_port_vlan(rocker_port, ROCKER_OP_FLAG_REMOVE, 0);
+	err = rocker_port_vlan_del(rocker_port, untagged_vid, 0);
 	if (err)
 		return err;
+
+	rocker_port_internal_vlan_id_put(rocker_port,
+					 rocker_port->bridge_dev->ifindex);
 	rocker_port->internal_vlan_id =
 		rocker_port_internal_vlan_id_get(rocker_port,
 						 rocker_port->dev->ifindex);
-	err = rocker_port_vlan(rocker_port, 0, 0);
+
+	rocker_port->bridge_dev = NULL;
+
+	err = rocker_port_vlan_add(rocker_port, SWITCHDEV_TRANS_NONE,
+				   untagged_vid, 0);
 	if (err)
 		return err;
 
 	if (rocker_port->dev->flags & IFF_UP)
-		err = rocker_port_fwd_enable(rocker_port);
+		err = rocker_port_fwd_enable(rocker_port,
+					     SWITCHDEV_TRANS_NONE, 0);
 
 	return err;
 }
@@ -4989,10 +5244,12 @@ static struct notifier_block rocker_netdevice_nb __read_mostly = {
 static int rocker_neigh_update(struct net_device *dev, struct neighbour *n)
 {
 	struct rocker_port *rocker_port = netdev_priv(dev);
-	int flags = (n->nud_state & NUD_VALID) ? 0 : ROCKER_OP_FLAG_REMOVE;
+	int flags = (n->nud_state & NUD_VALID ? 0 : ROCKER_OP_FLAG_REMOVE) |
+		    ROCKER_OP_FLAG_NOWAIT;
 	__be32 ip_addr = *(__be32 *)n->primary_key;
 
-	return rocker_port_ipv4_neigh(rocker_port, flags, ip_addr, n->ha);
+	return rocker_port_ipv4_neigh(rocker_port, SWITCHDEV_TRANS_NONE,
+				      flags, ip_addr, n->ha);
 }
 
 static int rocker_netevent_event(struct notifier_block *unused,
@@ -5040,7 +5297,7 @@ static int __init rocker_module_init(void)
 	return 0;
 
 err_pci_register_driver:
-	unregister_netdevice_notifier(&rocker_netevent_nb);
+	unregister_netevent_notifier(&rocker_netevent_nb);
 	unregister_netdevice_notifier(&rocker_netdevice_nb);
 	return err;
 }
diff --git a/drivers/net/ethernet/rocker/rocker.h b/drivers/net/ethernet/rocker/rocker.h
index a4e9591d7457..c61fbf968036 100644
--- a/drivers/net/ethernet/rocker/rocker.h
+++ b/drivers/net/ethernet/rocker/rocker.h
@@ -65,9 +65,9 @@ enum {
 #define ROCKER_TEST_DMA_CTRL		0x0034
 
 /* Rocker test register ctrl */
-#define ROCKER_TEST_DMA_CTRL_CLEAR	(1 << 0)
-#define ROCKER_TEST_DMA_CTRL_FILL	(1 << 1)
-#define ROCKER_TEST_DMA_CTRL_INVERT	(1 << 2)
+#define ROCKER_TEST_DMA_CTRL_CLEAR	BIT(0)
+#define ROCKER_TEST_DMA_CTRL_FILL	BIT(1)
+#define ROCKER_TEST_DMA_CTRL_INVERT	BIT(2)
 
 /* Rocker DMA ring register offsets */
 #define ROCKER_DMA_DESC_ADDR(x)		(0x1000 + (x) * 32)  /* 8-byte */
@@ -79,7 +79,7 @@ enum {
 #define ROCKER_DMA_DESC_RES1(x)		(0x101c + (x) * 32)
 
 /* Rocker dma ctrl register bits */
-#define ROCKER_DMA_DESC_CTRL_RESET	(1 << 0)
+#define ROCKER_DMA_DESC_CTRL_RESET	BIT(0)
 
 /* Rocker DMA ring types */
 enum rocker_dma_type {
@@ -111,7 +111,7 @@ struct rocker_desc {
 	u16 comp_err;
 };
 
-#define ROCKER_DMA_DESC_COMP_ERR_GEN	(1 << 15)
+#define ROCKER_DMA_DESC_COMP_ERR_GEN	BIT(15)
 
 /* Rocker DMA TLV struct */
 struct rocker_tlv {
@@ -237,14 +237,14 @@ enum {
 	ROCKER_TLV_RX_MAX = __ROCKER_TLV_RX_MAX - 1,
 };
 
-#define ROCKER_RX_FLAGS_IPV4			(1 << 0)
-#define ROCKER_RX_FLAGS_IPV6			(1 << 1)
-#define ROCKER_RX_FLAGS_CSUM_CALC		(1 << 2)
-#define ROCKER_RX_FLAGS_IPV4_CSUM_GOOD		(1 << 3)
-#define ROCKER_RX_FLAGS_IP_FRAG			(1 << 4)
-#define ROCKER_RX_FLAGS_TCP			(1 << 5)
-#define ROCKER_RX_FLAGS_UDP			(1 << 6)
-#define ROCKER_RX_FLAGS_TCP_UDP_CSUM_GOOD	(1 << 7)
+#define ROCKER_RX_FLAGS_IPV4			BIT(0)
+#define ROCKER_RX_FLAGS_IPV6			BIT(1)
+#define ROCKER_RX_FLAGS_CSUM_CALC		BIT(2)
+#define ROCKER_RX_FLAGS_IPV4_CSUM_GOOD		BIT(3)
+#define ROCKER_RX_FLAGS_IP_FRAG			BIT(4)
+#define ROCKER_RX_FLAGS_TCP			BIT(5)
+#define ROCKER_RX_FLAGS_UDP			BIT(6)
+#define ROCKER_RX_FLAGS_TCP_UDP_CSUM_GOOD	BIT(7)
 
 enum {
 	ROCKER_TLV_TX_UNSPEC,
@@ -460,6 +460,6 @@ enum rocker_of_dpa_overlay_type {
 #define ROCKER_SWITCH_ID		0x0320 /* 8-byte */
 
 /* Rocker control bits */
-#define ROCKER_CONTROL_RESET		(1 << 0)
+#define ROCKER_CONTROL_RESET		BIT(0)
 
 #endif
diff --git a/drivers/net/ethernet/seeq/Kconfig b/drivers/net/ethernet/seeq/Kconfig
index 11f168e46ebe..69c62d89295e 100644
--- a/drivers/net/ethernet/seeq/Kconfig
+++ b/drivers/net/ethernet/seeq/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_SEEQ
 	default y
 	depends on HAS_IOMEM
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/sfc/Kconfig b/drivers/net/ethernet/sfc/Kconfig
index 088921294448..4dd92b7b80f4 100644
--- a/drivers/net/ethernet/sfc/Kconfig
+++ b/drivers/net/ethernet/sfc/Kconfig
@@ -36,3 +36,12 @@ config SFC_SRIOV
 	  This enables support for the SFC9000 I/O Virtualization
 	  features, allowing accelerated network performance in
 	  virtualized environments.
+config SFC_MCDI_LOGGING
+	bool "Solarflare SFC9000/SFC9100-family MCDI logging support"
+	depends on SFC
+	default y
+	---help---
+	  This enables support for tracing of MCDI (Management-Controller-to-
+	  Driver-Interface) commands and responses, allowing debugging of
+	  driver/firmware interaction.  The tracing is actually enabled by
+	  a sysfs file 'mcdi_logging' under the PCI device.
diff --git a/drivers/net/ethernet/sfc/Makefile b/drivers/net/ethernet/sfc/Makefile
index 3a83c0dca8e6..ce8470fe79d5 100644
--- a/drivers/net/ethernet/sfc/Makefile
+++ b/drivers/net/ethernet/sfc/Makefile
@@ -3,6 +3,6 @@ sfc-y			+= efx.o nic.o farch.o falcon.o siena.o ef10.o tx.o \
 			   tenxpress.o txc43128_phy.o falcon_boards.o \
 			   mcdi.o mcdi_port.o mcdi_mon.o ptp.o
 sfc-$(CONFIG_SFC_MTD)	+= mtd.o
-sfc-$(CONFIG_SFC_SRIOV)	+= siena_sriov.o
+sfc-$(CONFIG_SFC_SRIOV)	+= sriov.o siena_sriov.o ef10_sriov.o
 
 obj-$(CONFIG_SFC)	+= sfc.o
diff --git a/drivers/net/ethernet/sfc/ef10.c b/drivers/net/ethernet/sfc/ef10.c
index fbb6cfa0f5f1..847643455468 100644
--- a/drivers/net/ethernet/sfc/ef10.c
+++ b/drivers/net/ethernet/sfc/ef10.c
@@ -15,6 +15,7 @@
 #include "nic.h"
 #include "workarounds.h"
 #include "selftest.h"
+#include "ef10_sriov.h"
 #include <linux/in.h>
 #include <linux/jhash.h>
 #include <linux/wait.h>
@@ -30,6 +31,9 @@ enum {
 
 /* The reserved RSS context value */
 #define EFX_EF10_RSS_CONTEXT_INVALID	0xffffffff
+/* The maximum size of a shared RSS context */
+/* TODO: this should really be from the mcdi protocol export */
+#define EFX_EF10_MAX_SHARED_RSS_CONTEXT_SIZE 64UL
 
 /* The filter table(s) are managed by firmware and we have write-only
  * access.  When removing filters we must identify them to the
@@ -77,7 +81,6 @@ struct efx_ef10_filter_table {
 /* An arbitrary search limit for the software hash table */
 #define EFX_EF10_FILTER_SEARCH_LIMIT 200
 
-static void efx_ef10_rx_push_rss_config(struct efx_nic *efx);
 static void efx_ef10_rx_free_indir_table(struct efx_nic *efx);
 static void efx_ef10_filter_table_remove(struct efx_nic *efx);
 
@@ -92,8 +95,49 @@ static int efx_ef10_get_warm_boot_count(struct efx_nic *efx)
 
 static unsigned int efx_ef10_mem_map_size(struct efx_nic *efx)
 {
-	return resource_size(&efx->pci_dev->resource[EFX_MEM_BAR]);
+	int bar;
+
+	bar = efx->type->mem_bar;
+	return resource_size(&efx->pci_dev->resource[bar]);
+}
+
+static int efx_ef10_get_pf_index(struct efx_nic *efx)
+{
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_FUNCTION_INFO_OUT_LEN);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	size_t outlen;
+	int rc;
+
+	rc = efx_mcdi_rpc(efx, MC_CMD_GET_FUNCTION_INFO, NULL, 0, outbuf,
+			  sizeof(outbuf), &outlen);
+	if (rc)
+		return rc;
+	if (outlen < sizeof(outbuf))
+		return -EIO;
+
+	nic_data->pf_index = MCDI_DWORD(outbuf, GET_FUNCTION_INFO_OUT_PF);
+	return 0;
+}
+
+#ifdef CONFIG_SFC_SRIOV
+static int efx_ef10_get_vf_index(struct efx_nic *efx)
+{
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_FUNCTION_INFO_OUT_LEN);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	size_t outlen;
+	int rc;
+
+	rc = efx_mcdi_rpc(efx, MC_CMD_GET_FUNCTION_INFO, NULL, 0, outbuf,
+			  sizeof(outbuf), &outlen);
+	if (rc)
+		return rc;
+	if (outlen < sizeof(outbuf))
+		return -EIO;
+
+	nic_data->vf_index = MCDI_DWORD(outbuf, GET_FUNCTION_INFO_OUT_VF);
+	return 0;
 }
+#endif
 
 static int efx_ef10_init_datapath_caps(struct efx_nic *efx)
 {
@@ -117,6 +161,13 @@ static int efx_ef10_init_datapath_caps(struct efx_nic *efx)
 	nic_data->datapath_caps =
 		MCDI_DWORD(outbuf, GET_CAPABILITIES_OUT_FLAGS1);
 
+	/* record the DPCPU firmware IDs to determine VEB vswitching support.
+	 */
+	nic_data->rx_dpcpu_fw_id =
+		MCDI_WORD(outbuf, GET_CAPABILITIES_OUT_RX_DPCPU_FW_ID);
+	nic_data->tx_dpcpu_fw_id =
+		MCDI_WORD(outbuf, GET_CAPABILITIES_OUT_TX_DPCPU_FW_ID);
+
 	if (!(nic_data->datapath_caps &
 	      (1 << MC_CMD_GET_CAPABILITIES_OUT_TX_TSO_LBN))) {
 		netif_err(efx, drv, efx->net_dev,
@@ -147,7 +198,7 @@ static int efx_ef10_get_sysclk_freq(struct efx_nic *efx)
 	return rc > 0 ? rc : -ERANGE;
 }
 
-static int efx_ef10_get_mac_address(struct efx_nic *efx, u8 *mac_address)
+static int efx_ef10_get_mac_address_pf(struct efx_nic *efx, u8 *mac_address)
 {
 	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_MAC_ADDRESSES_OUT_LEN);
 	size_t outlen;
@@ -167,9 +218,66 @@ static int efx_ef10_get_mac_address(struct efx_nic *efx, u8 *mac_address)
 	return 0;
 }
 
+static int efx_ef10_get_mac_address_vf(struct efx_nic *efx, u8 *mac_address)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VPORT_GET_MAC_ADDRESSES_IN_LEN);
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_VPORT_GET_MAC_ADDRESSES_OUT_LENMAX);
+	size_t outlen;
+	int num_addrs, rc;
+
+	MCDI_SET_DWORD(inbuf, VPORT_GET_MAC_ADDRESSES_IN_VPORT_ID,
+		       EVB_PORT_ID_ASSIGNED);
+	rc = efx_mcdi_rpc(efx, MC_CMD_VPORT_GET_MAC_ADDRESSES, inbuf,
+			  sizeof(inbuf), outbuf, sizeof(outbuf), &outlen);
+
+	if (rc)
+		return rc;
+	if (outlen < MC_CMD_VPORT_GET_MAC_ADDRESSES_OUT_LENMIN)
+		return -EIO;
+
+	num_addrs = MCDI_DWORD(outbuf,
+			       VPORT_GET_MAC_ADDRESSES_OUT_MACADDR_COUNT);
+
+	WARN_ON(num_addrs != 1);
+
+	ether_addr_copy(mac_address,
+			MCDI_PTR(outbuf, VPORT_GET_MAC_ADDRESSES_OUT_MACADDR));
+
+	return 0;
+}
+
+static ssize_t efx_ef10_show_link_control_flag(struct device *dev,
+					       struct device_attribute *attr,
+					       char *buf)
+{
+	struct efx_nic *efx = pci_get_drvdata(to_pci_dev(dev));
+
+	return sprintf(buf, "%d\n",
+		       ((efx->mcdi->fn_flags) &
+			(1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_LINKCTRL))
+		       ? 1 : 0);
+}
+
+static ssize_t efx_ef10_show_primary_flag(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	struct efx_nic *efx = pci_get_drvdata(to_pci_dev(dev));
+
+	return sprintf(buf, "%d\n",
+		       ((efx->mcdi->fn_flags) &
+			(1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_PRIMARY))
+		       ? 1 : 0);
+}
+
+static DEVICE_ATTR(link_control_flag, 0444, efx_ef10_show_link_control_flag,
+		   NULL);
+static DEVICE_ATTR(primary_flag, 0444, efx_ef10_show_primary_flag, NULL);
+
 static int efx_ef10_probe(struct efx_nic *efx)
 {
 	struct efx_ef10_nic_data *nic_data;
+	struct net_device *net_dev = efx->net_dev;
 	int i, rc;
 
 	/* We can have one VI for each 8K region.  However, until we
@@ -178,7 +286,7 @@ static int efx_ef10_probe(struct efx_nic *efx)
 	efx->max_channels =
 		min_t(unsigned int,
 		      EFX_MAX_CHANNELS,
-		      resource_size(&efx->pci_dev->resource[EFX_MEM_BAR]) /
+		      efx_ef10_mem_map_size(efx) /
 		      (EFX_VI_PAGE_SIZE * EFX_TXQ_TYPES));
 	if (WARN_ON(efx->max_channels == 0))
 		return -EIO;
@@ -188,6 +296,9 @@ static int efx_ef10_probe(struct efx_nic *efx)
 		return -ENOMEM;
 	efx->nic_data = nic_data;
 
+	/* we assume later that we can copy from this buffer in dwords */
+	BUILD_BUG_ON(MCDI_CTL_SDU_LEN_MAX_V2 % 4);
+
 	rc = efx_nic_alloc_buffer(efx, &nic_data->mcdi_buf,
 				  8 + MCDI_CTL_SDU_LEN_MAX_V2, GFP_KERNEL);
 	if (rc)
@@ -209,6 +320,8 @@ static int efx_ef10_probe(struct efx_nic *efx)
 
 	nic_data->rx_rss_context = EFX_EF10_RSS_CONTEXT_INVALID;
 
+	nic_data->vport_id = EVB_PORT_ID_ASSIGNED;
+
 	/* In case we're recovering from a crash (kexec), we want to
 	 * cancel any outstanding request by the previous user of this
 	 * function.  We send a special message using the least
@@ -230,45 +343,85 @@ static int efx_ef10_probe(struct efx_nic *efx)
 	if (rc)
 		goto fail3;
 
+	rc = device_create_file(&efx->pci_dev->dev,
+				&dev_attr_link_control_flag);
+	if (rc)
+		goto fail3;
+
+	rc = device_create_file(&efx->pci_dev->dev, &dev_attr_primary_flag);
+	if (rc)
+		goto fail4;
+
+	rc = efx_ef10_get_pf_index(efx);
+	if (rc)
+		goto fail5;
+
 	rc = efx_ef10_init_datapath_caps(efx);
 	if (rc < 0)
-		goto fail3;
+		goto fail5;
 
 	efx->rx_packet_len_offset =
 		ES_DZ_RX_PREFIX_PKTLEN_OFST - ES_DZ_RX_PREFIX_SIZE;
 
 	rc = efx_mcdi_port_get_number(efx);
 	if (rc < 0)
-		goto fail3;
+		goto fail5;
 	efx->port_num = rc;
+	net_dev->dev_port = rc;
 
-	rc = efx_ef10_get_mac_address(efx, efx->net_dev->perm_addr);
+	rc = efx->type->get_mac_address(efx, efx->net_dev->perm_addr);
 	if (rc)
-		goto fail3;
+		goto fail5;
 
 	rc = efx_ef10_get_sysclk_freq(efx);
 	if (rc < 0)
-		goto fail3;
+		goto fail5;
 	efx->timer_quantum_ns = 1536000 / rc; /* 1536 cycles */
 
-	/* Check whether firmware supports bug 35388 workaround */
+	/* Check whether firmware supports bug 35388 workaround.
+	 * First try to enable it, then if we get EPERM, just
+	 * ask if it's already enabled
+	 */
 	rc = efx_mcdi_set_workaround(efx, MC_CMD_WORKAROUND_BUG35388, true);
-	if (rc == 0)
+	if (rc == 0) {
 		nic_data->workaround_35388 = true;
-	else if (rc != -ENOSYS && rc != -ENOENT)
-		goto fail3;
+	} else if (rc == -EPERM) {
+		unsigned int enabled;
+
+		rc = efx_mcdi_get_workarounds(efx, NULL, &enabled);
+		if (rc)
+			goto fail3;
+		nic_data->workaround_35388 = enabled &
+			MC_CMD_GET_WORKAROUNDS_OUT_BUG35388;
+	} else if (rc != -ENOSYS && rc != -ENOENT) {
+		goto fail5;
+	}
 	netif_dbg(efx, probe, efx->net_dev,
 		  "workaround for bug 35388 is %sabled\n",
 		  nic_data->workaround_35388 ? "en" : "dis");
 
 	rc = efx_mcdi_mon_probe(efx);
-	if (rc)
-		goto fail3;
+	if (rc && rc != -EPERM)
+		goto fail5;
 
 	efx_ptp_probe(efx, NULL);
 
+#ifdef CONFIG_SFC_SRIOV
+	if ((efx->pci_dev->physfn) && (!efx->pci_dev->is_physfn)) {
+		struct pci_dev *pci_dev_pf = efx->pci_dev->physfn;
+		struct efx_nic *efx_pf = pci_get_drvdata(pci_dev_pf);
+
+		efx_pf->type->get_mac_address(efx_pf, nic_data->port_id);
+	} else
+#endif
+		ether_addr_copy(nic_data->port_id, efx->net_dev->perm_addr);
+
 	return 0;
 
+fail5:
+	device_remove_file(&efx->pci_dev->dev, &dev_attr_primary_flag);
+fail4:
+	device_remove_file(&efx->pci_dev->dev, &dev_attr_link_control_flag);
 fail3:
 	efx_mcdi_fini(efx);
 fail2:
@@ -281,7 +434,7 @@ fail1:
 
 static int efx_ef10_free_vis(struct efx_nic *efx)
 {
-	MCDI_DECLARE_BUF_OUT_OR_ERR(outbuf, 0);
+	MCDI_DECLARE_BUF_ERR(outbuf);
 	size_t outlen;
 	int rc = efx_mcdi_rpc_quiet(efx, MC_CMD_FREE_VIS, NULL, 0,
 				    outbuf, sizeof(outbuf), &outlen);
@@ -352,9 +505,9 @@ static int efx_ef10_alloc_piobufs(struct efx_nic *efx, unsigned int n)
 static int efx_ef10_link_piobufs(struct efx_nic *efx)
 {
 	struct efx_ef10_nic_data *nic_data = efx->nic_data;
-	MCDI_DECLARE_BUF(inbuf,
-			 max(MC_CMD_LINK_PIOBUF_IN_LEN,
-			     MC_CMD_UNLINK_PIOBUF_IN_LEN));
+	_MCDI_DECLARE_BUF(inbuf,
+			  max(MC_CMD_LINK_PIOBUF_IN_LEN,
+			      MC_CMD_UNLINK_PIOBUF_IN_LEN));
 	struct efx_channel *channel;
 	struct efx_tx_queue *tx_queue;
 	unsigned int offset, index;
@@ -363,6 +516,8 @@ static int efx_ef10_link_piobufs(struct efx_nic *efx)
 	BUILD_BUG_ON(MC_CMD_LINK_PIOBUF_OUT_LEN != 0);
 	BUILD_BUG_ON(MC_CMD_UNLINK_PIOBUF_OUT_LEN != 0);
 
+	memset(inbuf, 0, sizeof(inbuf));
+
 	/* Link a buffer to each VI in the write-combining mapping */
 	for (index = 0; index < nic_data->n_piobufs; ++index) {
 		MCDI_SET_DWORD(inbuf, LINK_PIOBUF_IN_PIOBUF_HANDLE,
@@ -475,6 +630,25 @@ static void efx_ef10_remove(struct efx_nic *efx)
 	struct efx_ef10_nic_data *nic_data = efx->nic_data;
 	int rc;
 
+#ifdef CONFIG_SFC_SRIOV
+	struct efx_ef10_nic_data *nic_data_pf;
+	struct pci_dev *pci_dev_pf;
+	struct efx_nic *efx_pf;
+	struct ef10_vf *vf;
+
+	if (efx->pci_dev->is_virtfn) {
+		pci_dev_pf = efx->pci_dev->physfn;
+		if (pci_dev_pf) {
+			efx_pf = pci_get_drvdata(pci_dev_pf);
+			nic_data_pf = efx_pf->nic_data;
+			vf = nic_data_pf->vf + nic_data->vf_index;
+			vf->efx = NULL;
+		} else
+			netif_info(efx, drv, efx->net_dev,
+				   "Could not get the PF id from VF\n");
+	}
+#endif
+
 	efx_ptp_remove(efx);
 
 	efx_mcdi_mon_remove(efx);
@@ -490,11 +664,78 @@ static void efx_ef10_remove(struct efx_nic *efx)
 	if (!nic_data->must_restore_piobufs)
 		efx_ef10_free_piobufs(efx);
 
+	device_remove_file(&efx->pci_dev->dev, &dev_attr_primary_flag);
+	device_remove_file(&efx->pci_dev->dev, &dev_attr_link_control_flag);
+
 	efx_mcdi_fini(efx);
 	efx_nic_free_buffer(efx, &nic_data->mcdi_buf);
 	kfree(nic_data);
 }
 
+static int efx_ef10_probe_pf(struct efx_nic *efx)
+{
+	return efx_ef10_probe(efx);
+}
+
+#ifdef CONFIG_SFC_SRIOV
+static int efx_ef10_probe_vf(struct efx_nic *efx)
+{
+	int rc;
+	struct pci_dev *pci_dev_pf;
+
+	/* If the parent PF has no VF data structure, it doesn't know about this
+	 * VF so fail probe.  The VF needs to be re-created.  This can happen
+	 * if the PF driver is unloaded while the VF is assigned to a guest.
+	 */
+	pci_dev_pf = efx->pci_dev->physfn;
+	if (pci_dev_pf) {
+		struct efx_nic *efx_pf = pci_get_drvdata(pci_dev_pf);
+		struct efx_ef10_nic_data *nic_data_pf = efx_pf->nic_data;
+
+		if (!nic_data_pf->vf) {
+			netif_info(efx, drv, efx->net_dev,
+				   "The VF cannot link to its parent PF; "
+				   "please destroy and re-create the VF\n");
+			return -EBUSY;
+		}
+	}
+
+	rc = efx_ef10_probe(efx);
+	if (rc)
+		return rc;
+
+	rc = efx_ef10_get_vf_index(efx);
+	if (rc)
+		goto fail;
+
+	if (efx->pci_dev->is_virtfn) {
+		if (efx->pci_dev->physfn) {
+			struct efx_nic *efx_pf =
+				pci_get_drvdata(efx->pci_dev->physfn);
+			struct efx_ef10_nic_data *nic_data_p = efx_pf->nic_data;
+			struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+			nic_data_p->vf[nic_data->vf_index].efx = efx;
+			nic_data_p->vf[nic_data->vf_index].pci_dev =
+				efx->pci_dev;
+		} else
+			netif_info(efx, drv, efx->net_dev,
+				   "Could not get the PF id from VF\n");
+	}
+
+	return 0;
+
+fail:
+	efx_ef10_remove(efx);
+	return rc;
+}
+#else
+static int efx_ef10_probe_vf(struct efx_nic *efx __attribute__ ((unused)))
+{
+	return 0;
+}
+#endif
+
 static int efx_ef10_alloc_vis(struct efx_nic *efx,
 			      unsigned int min_vis, unsigned int max_vis)
 {
@@ -687,7 +928,9 @@ static int efx_ef10_init_nic(struct efx_nic *efx)
 		nic_data->must_restore_piobufs = false;
 	}
 
-	efx_ef10_rx_push_rss_config(efx);
+	/* don't fail init if RSS setup doesn't work */
+	efx->type->rx_push_rss_config(efx, false, efx->rx_indir_table);
+
 	return 0;
 }
 
@@ -702,6 +945,14 @@ static void efx_ef10_reset_mc_allocations(struct efx_nic *efx)
 	nic_data->rx_rss_context = EFX_EF10_RSS_CONTEXT_INVALID;
 }
 
+static enum reset_type efx_ef10_map_reset_reason(enum reset_type reason)
+{
+	if (reason == RESET_TYPE_MC_FAILURE)
+		return RESET_TYPE_DATAPATH;
+
+	return efx_mcdi_map_reset_reason(reason);
+}
+
 static int efx_ef10_map_reset_flags(u32 *flags)
 {
 	enum {
@@ -760,93 +1011,112 @@ static int efx_ef10_reset(struct efx_nic *efx, enum reset_type reset_type)
 	[GENERIC_STAT_ ## ext_name] = { #ext_name, 0, 0 }
 
 static const struct efx_hw_stat_desc efx_ef10_stat_desc[EF10_STAT_COUNT] = {
-	EF10_DMA_STAT(tx_bytes, TX_BYTES),
-	EF10_DMA_STAT(tx_packets, TX_PKTS),
-	EF10_DMA_STAT(tx_pause, TX_PAUSE_PKTS),
-	EF10_DMA_STAT(tx_control, TX_CONTROL_PKTS),
-	EF10_DMA_STAT(tx_unicast, TX_UNICAST_PKTS),
-	EF10_DMA_STAT(tx_multicast, TX_MULTICAST_PKTS),
-	EF10_DMA_STAT(tx_broadcast, TX_BROADCAST_PKTS),
-	EF10_DMA_STAT(tx_lt64, TX_LT64_PKTS),
-	EF10_DMA_STAT(tx_64, TX_64_PKTS),
-	EF10_DMA_STAT(tx_65_to_127, TX_65_TO_127_PKTS),
-	EF10_DMA_STAT(tx_128_to_255, TX_128_TO_255_PKTS),
-	EF10_DMA_STAT(tx_256_to_511, TX_256_TO_511_PKTS),
-	EF10_DMA_STAT(tx_512_to_1023, TX_512_TO_1023_PKTS),
-	EF10_DMA_STAT(tx_1024_to_15xx, TX_1024_TO_15XX_PKTS),
-	EF10_DMA_STAT(tx_15xx_to_jumbo, TX_15XX_TO_JUMBO_PKTS),
-	EF10_DMA_STAT(rx_bytes, RX_BYTES),
-	EF10_DMA_INVIS_STAT(rx_bytes_minus_good_bytes, RX_BAD_BYTES),
-	EF10_OTHER_STAT(rx_good_bytes),
-	EF10_OTHER_STAT(rx_bad_bytes),
-	EF10_DMA_STAT(rx_packets, RX_PKTS),
-	EF10_DMA_STAT(rx_good, RX_GOOD_PKTS),
-	EF10_DMA_STAT(rx_bad, RX_BAD_FCS_PKTS),
-	EF10_DMA_STAT(rx_pause, RX_PAUSE_PKTS),
-	EF10_DMA_STAT(rx_control, RX_CONTROL_PKTS),
-	EF10_DMA_STAT(rx_unicast, RX_UNICAST_PKTS),
-	EF10_DMA_STAT(rx_multicast, RX_MULTICAST_PKTS),
-	EF10_DMA_STAT(rx_broadcast, RX_BROADCAST_PKTS),
-	EF10_DMA_STAT(rx_lt64, RX_UNDERSIZE_PKTS),
-	EF10_DMA_STAT(rx_64, RX_64_PKTS),
-	EF10_DMA_STAT(rx_65_to_127, RX_65_TO_127_PKTS),
-	EF10_DMA_STAT(rx_128_to_255, RX_128_TO_255_PKTS),
-	EF10_DMA_STAT(rx_256_to_511, RX_256_TO_511_PKTS),
-	EF10_DMA_STAT(rx_512_to_1023, RX_512_TO_1023_PKTS),
-	EF10_DMA_STAT(rx_1024_to_15xx, RX_1024_TO_15XX_PKTS),
-	EF10_DMA_STAT(rx_15xx_to_jumbo, RX_15XX_TO_JUMBO_PKTS),
-	EF10_DMA_STAT(rx_gtjumbo, RX_GTJUMBO_PKTS),
-	EF10_DMA_STAT(rx_bad_gtjumbo, RX_JABBER_PKTS),
-	EF10_DMA_STAT(rx_overflow, RX_OVERFLOW_PKTS),
-	EF10_DMA_STAT(rx_align_error, RX_ALIGN_ERROR_PKTS),
-	EF10_DMA_STAT(rx_length_error, RX_LENGTH_ERROR_PKTS),
-	EF10_DMA_STAT(rx_nodesc_drops, RX_NODESC_DROPS),
+	EF10_DMA_STAT(port_tx_bytes, TX_BYTES),
+	EF10_DMA_STAT(port_tx_packets, TX_PKTS),
+	EF10_DMA_STAT(port_tx_pause, TX_PAUSE_PKTS),
+	EF10_DMA_STAT(port_tx_control, TX_CONTROL_PKTS),
+	EF10_DMA_STAT(port_tx_unicast, TX_UNICAST_PKTS),
+	EF10_DMA_STAT(port_tx_multicast, TX_MULTICAST_PKTS),
+	EF10_DMA_STAT(port_tx_broadcast, TX_BROADCAST_PKTS),
+	EF10_DMA_STAT(port_tx_lt64, TX_LT64_PKTS),
+	EF10_DMA_STAT(port_tx_64, TX_64_PKTS),
+	EF10_DMA_STAT(port_tx_65_to_127, TX_65_TO_127_PKTS),
+	EF10_DMA_STAT(port_tx_128_to_255, TX_128_TO_255_PKTS),
+	EF10_DMA_STAT(port_tx_256_to_511, TX_256_TO_511_PKTS),
+	EF10_DMA_STAT(port_tx_512_to_1023, TX_512_TO_1023_PKTS),
+	EF10_DMA_STAT(port_tx_1024_to_15xx, TX_1024_TO_15XX_PKTS),
+	EF10_DMA_STAT(port_tx_15xx_to_jumbo, TX_15XX_TO_JUMBO_PKTS),
+	EF10_DMA_STAT(port_rx_bytes, RX_BYTES),
+	EF10_DMA_INVIS_STAT(port_rx_bytes_minus_good_bytes, RX_BAD_BYTES),
+	EF10_OTHER_STAT(port_rx_good_bytes),
+	EF10_OTHER_STAT(port_rx_bad_bytes),
+	EF10_DMA_STAT(port_rx_packets, RX_PKTS),
+	EF10_DMA_STAT(port_rx_good, RX_GOOD_PKTS),
+	EF10_DMA_STAT(port_rx_bad, RX_BAD_FCS_PKTS),
+	EF10_DMA_STAT(port_rx_pause, RX_PAUSE_PKTS),
+	EF10_DMA_STAT(port_rx_control, RX_CONTROL_PKTS),
+	EF10_DMA_STAT(port_rx_unicast, RX_UNICAST_PKTS),
+	EF10_DMA_STAT(port_rx_multicast, RX_MULTICAST_PKTS),
+	EF10_DMA_STAT(port_rx_broadcast, RX_BROADCAST_PKTS),
+	EF10_DMA_STAT(port_rx_lt64, RX_UNDERSIZE_PKTS),
+	EF10_DMA_STAT(port_rx_64, RX_64_PKTS),
+	EF10_DMA_STAT(port_rx_65_to_127, RX_65_TO_127_PKTS),
+	EF10_DMA_STAT(port_rx_128_to_255, RX_128_TO_255_PKTS),
+	EF10_DMA_STAT(port_rx_256_to_511, RX_256_TO_511_PKTS),
+	EF10_DMA_STAT(port_rx_512_to_1023, RX_512_TO_1023_PKTS),
+	EF10_DMA_STAT(port_rx_1024_to_15xx, RX_1024_TO_15XX_PKTS),
+	EF10_DMA_STAT(port_rx_15xx_to_jumbo, RX_15XX_TO_JUMBO_PKTS),
+	EF10_DMA_STAT(port_rx_gtjumbo, RX_GTJUMBO_PKTS),
+	EF10_DMA_STAT(port_rx_bad_gtjumbo, RX_JABBER_PKTS),
+	EF10_DMA_STAT(port_rx_overflow, RX_OVERFLOW_PKTS),
+	EF10_DMA_STAT(port_rx_align_error, RX_ALIGN_ERROR_PKTS),
+	EF10_DMA_STAT(port_rx_length_error, RX_LENGTH_ERROR_PKTS),
+	EF10_DMA_STAT(port_rx_nodesc_drops, RX_NODESC_DROPS),
 	GENERIC_SW_STAT(rx_nodesc_trunc),
 	GENERIC_SW_STAT(rx_noskb_drops),
-	EF10_DMA_STAT(rx_pm_trunc_bb_overflow, PM_TRUNC_BB_OVERFLOW),
-	EF10_DMA_STAT(rx_pm_discard_bb_overflow, PM_DISCARD_BB_OVERFLOW),
-	EF10_DMA_STAT(rx_pm_trunc_vfifo_full, PM_TRUNC_VFIFO_FULL),
-	EF10_DMA_STAT(rx_pm_discard_vfifo_full, PM_DISCARD_VFIFO_FULL),
-	EF10_DMA_STAT(rx_pm_trunc_qbb, PM_TRUNC_QBB),
-	EF10_DMA_STAT(rx_pm_discard_qbb, PM_DISCARD_QBB),
-	EF10_DMA_STAT(rx_pm_discard_mapping, PM_DISCARD_MAPPING),
-	EF10_DMA_STAT(rx_dp_q_disabled_packets, RXDP_Q_DISABLED_PKTS),
-	EF10_DMA_STAT(rx_dp_di_dropped_packets, RXDP_DI_DROPPED_PKTS),
-	EF10_DMA_STAT(rx_dp_streaming_packets, RXDP_STREAMING_PKTS),
-	EF10_DMA_STAT(rx_dp_hlb_fetch, RXDP_EMERGENCY_FETCH_CONDITIONS),
-	EF10_DMA_STAT(rx_dp_hlb_wait, RXDP_EMERGENCY_WAIT_CONDITIONS),
+	EF10_DMA_STAT(port_rx_pm_trunc_bb_overflow, PM_TRUNC_BB_OVERFLOW),
+	EF10_DMA_STAT(port_rx_pm_discard_bb_overflow, PM_DISCARD_BB_OVERFLOW),
+	EF10_DMA_STAT(port_rx_pm_trunc_vfifo_full, PM_TRUNC_VFIFO_FULL),
+	EF10_DMA_STAT(port_rx_pm_discard_vfifo_full, PM_DISCARD_VFIFO_FULL),
+	EF10_DMA_STAT(port_rx_pm_trunc_qbb, PM_TRUNC_QBB),
+	EF10_DMA_STAT(port_rx_pm_discard_qbb, PM_DISCARD_QBB),
+	EF10_DMA_STAT(port_rx_pm_discard_mapping, PM_DISCARD_MAPPING),
+	EF10_DMA_STAT(port_rx_dp_q_disabled_packets, RXDP_Q_DISABLED_PKTS),
+	EF10_DMA_STAT(port_rx_dp_di_dropped_packets, RXDP_DI_DROPPED_PKTS),
+	EF10_DMA_STAT(port_rx_dp_streaming_packets, RXDP_STREAMING_PKTS),
+	EF10_DMA_STAT(port_rx_dp_hlb_fetch, RXDP_HLB_FETCH_CONDITIONS),
+	EF10_DMA_STAT(port_rx_dp_hlb_wait, RXDP_HLB_WAIT_CONDITIONS),
+	EF10_DMA_STAT(rx_unicast, VADAPTER_RX_UNICAST_PACKETS),
+	EF10_DMA_STAT(rx_unicast_bytes, VADAPTER_RX_UNICAST_BYTES),
+	EF10_DMA_STAT(rx_multicast, VADAPTER_RX_MULTICAST_PACKETS),
+	EF10_DMA_STAT(rx_multicast_bytes, VADAPTER_RX_MULTICAST_BYTES),
+	EF10_DMA_STAT(rx_broadcast, VADAPTER_RX_BROADCAST_PACKETS),
+	EF10_DMA_STAT(rx_broadcast_bytes, VADAPTER_RX_BROADCAST_BYTES),
+	EF10_DMA_STAT(rx_bad, VADAPTER_RX_BAD_PACKETS),
+	EF10_DMA_STAT(rx_bad_bytes, VADAPTER_RX_BAD_BYTES),
+	EF10_DMA_STAT(rx_overflow, VADAPTER_RX_OVERFLOW),
+	EF10_DMA_STAT(tx_unicast, VADAPTER_TX_UNICAST_PACKETS),
+	EF10_DMA_STAT(tx_unicast_bytes, VADAPTER_TX_UNICAST_BYTES),
+	EF10_DMA_STAT(tx_multicast, VADAPTER_TX_MULTICAST_PACKETS),
+	EF10_DMA_STAT(tx_multicast_bytes, VADAPTER_TX_MULTICAST_BYTES),
+	EF10_DMA_STAT(tx_broadcast, VADAPTER_TX_BROADCAST_PACKETS),
+	EF10_DMA_STAT(tx_broadcast_bytes, VADAPTER_TX_BROADCAST_BYTES),
+	EF10_DMA_STAT(tx_bad, VADAPTER_TX_BAD_PACKETS),
+	EF10_DMA_STAT(tx_bad_bytes, VADAPTER_TX_BAD_BYTES),
+	EF10_DMA_STAT(tx_overflow, VADAPTER_TX_OVERFLOW),
 };
 
-#define HUNT_COMMON_STAT_MASK ((1ULL << EF10_STAT_tx_bytes) |		\
-			       (1ULL << EF10_STAT_tx_packets) |		\
-			       (1ULL << EF10_STAT_tx_pause) |		\
-			       (1ULL << EF10_STAT_tx_unicast) |		\
-			       (1ULL << EF10_STAT_tx_multicast) |	\
-			       (1ULL << EF10_STAT_tx_broadcast) |	\
-			       (1ULL << EF10_STAT_rx_bytes) |		\
-			       (1ULL << EF10_STAT_rx_bytes_minus_good_bytes) | \
-			       (1ULL << EF10_STAT_rx_good_bytes) |	\
-			       (1ULL << EF10_STAT_rx_bad_bytes) |	\
-			       (1ULL << EF10_STAT_rx_packets) |		\
-			       (1ULL << EF10_STAT_rx_good) |		\
-			       (1ULL << EF10_STAT_rx_bad) |		\
-			       (1ULL << EF10_STAT_rx_pause) |		\
-			       (1ULL << EF10_STAT_rx_control) |		\
-			       (1ULL << EF10_STAT_rx_unicast) |		\
-			       (1ULL << EF10_STAT_rx_multicast) |	\
-			       (1ULL << EF10_STAT_rx_broadcast) |	\
-			       (1ULL << EF10_STAT_rx_lt64) |		\
-			       (1ULL << EF10_STAT_rx_64) |		\
-			       (1ULL << EF10_STAT_rx_65_to_127) |	\
-			       (1ULL << EF10_STAT_rx_128_to_255) |	\
-			       (1ULL << EF10_STAT_rx_256_to_511) |	\
-			       (1ULL << EF10_STAT_rx_512_to_1023) |	\
-			       (1ULL << EF10_STAT_rx_1024_to_15xx) |	\
-			       (1ULL << EF10_STAT_rx_15xx_to_jumbo) |	\
-			       (1ULL << EF10_STAT_rx_gtjumbo) |		\
-			       (1ULL << EF10_STAT_rx_bad_gtjumbo) |	\
-			       (1ULL << EF10_STAT_rx_overflow) |	\
-			       (1ULL << EF10_STAT_rx_nodesc_drops) |	\
+#define HUNT_COMMON_STAT_MASK ((1ULL << EF10_STAT_port_tx_bytes) |	\
+			       (1ULL << EF10_STAT_port_tx_packets) |	\
+			       (1ULL << EF10_STAT_port_tx_pause) |	\
+			       (1ULL << EF10_STAT_port_tx_unicast) |	\
+			       (1ULL << EF10_STAT_port_tx_multicast) |	\
+			       (1ULL << EF10_STAT_port_tx_broadcast) |	\
+			       (1ULL << EF10_STAT_port_rx_bytes) |	\
+			       (1ULL <<                                 \
+				EF10_STAT_port_rx_bytes_minus_good_bytes) | \
+			       (1ULL << EF10_STAT_port_rx_good_bytes) |	\
+			       (1ULL << EF10_STAT_port_rx_bad_bytes) |	\
+			       (1ULL << EF10_STAT_port_rx_packets) |	\
+			       (1ULL << EF10_STAT_port_rx_good) |	\
+			       (1ULL << EF10_STAT_port_rx_bad) |	\
+			       (1ULL << EF10_STAT_port_rx_pause) |	\
+			       (1ULL << EF10_STAT_port_rx_control) |	\
+			       (1ULL << EF10_STAT_port_rx_unicast) |	\
+			       (1ULL << EF10_STAT_port_rx_multicast) |	\
+			       (1ULL << EF10_STAT_port_rx_broadcast) |	\
+			       (1ULL << EF10_STAT_port_rx_lt64) |	\
+			       (1ULL << EF10_STAT_port_rx_64) |		\
+			       (1ULL << EF10_STAT_port_rx_65_to_127) |	\
+			       (1ULL << EF10_STAT_port_rx_128_to_255) |	\
+			       (1ULL << EF10_STAT_port_rx_256_to_511) |	\
+			       (1ULL << EF10_STAT_port_rx_512_to_1023) |\
+			       (1ULL << EF10_STAT_port_rx_1024_to_15xx) |\
+			       (1ULL << EF10_STAT_port_rx_15xx_to_jumbo) |\
+			       (1ULL << EF10_STAT_port_rx_gtjumbo) |	\
+			       (1ULL << EF10_STAT_port_rx_bad_gtjumbo) |\
+			       (1ULL << EF10_STAT_port_rx_overflow) |	\
+			       (1ULL << EF10_STAT_port_rx_nodesc_drops) |\
 			       (1ULL << GENERIC_STAT_rx_nodesc_trunc) |	\
 			       (1ULL << GENERIC_STAT_rx_noskb_drops))
 
@@ -854,39 +1124,39 @@ static const struct efx_hw_stat_desc efx_ef10_stat_desc[EF10_STAT_COUNT] = {
  * switchable port we do not expose these because they might not
  * include all the packets they should.
  */
-#define HUNT_10G_ONLY_STAT_MASK ((1ULL << EF10_STAT_tx_control) |	\
-				 (1ULL << EF10_STAT_tx_lt64) |		\
-				 (1ULL << EF10_STAT_tx_64) |		\
-				 (1ULL << EF10_STAT_tx_65_to_127) |	\
-				 (1ULL << EF10_STAT_tx_128_to_255) |	\
-				 (1ULL << EF10_STAT_tx_256_to_511) |	\
-				 (1ULL << EF10_STAT_tx_512_to_1023) |	\
-				 (1ULL << EF10_STAT_tx_1024_to_15xx) |	\
-				 (1ULL << EF10_STAT_tx_15xx_to_jumbo))
+#define HUNT_10G_ONLY_STAT_MASK ((1ULL << EF10_STAT_port_tx_control) |	\
+				 (1ULL << EF10_STAT_port_tx_lt64) |	\
+				 (1ULL << EF10_STAT_port_tx_64) |	\
+				 (1ULL << EF10_STAT_port_tx_65_to_127) |\
+				 (1ULL << EF10_STAT_port_tx_128_to_255) |\
+				 (1ULL << EF10_STAT_port_tx_256_to_511) |\
+				 (1ULL << EF10_STAT_port_tx_512_to_1023) |\
+				 (1ULL << EF10_STAT_port_tx_1024_to_15xx) |\
+				 (1ULL << EF10_STAT_port_tx_15xx_to_jumbo))
 
 /* These statistics are only provided by the 40G MAC.  For a 10G/40G
  * switchable port we do expose these because the errors will otherwise
  * be silent.
  */
-#define HUNT_40G_EXTRA_STAT_MASK ((1ULL << EF10_STAT_rx_align_error) |	\
-				  (1ULL << EF10_STAT_rx_length_error))
+#define HUNT_40G_EXTRA_STAT_MASK ((1ULL << EF10_STAT_port_rx_align_error) |\
+				  (1ULL << EF10_STAT_port_rx_length_error))
 
 /* These statistics are only provided if the firmware supports the
  * capability PM_AND_RXDP_COUNTERS.
  */
 #define HUNT_PM_AND_RXDP_STAT_MASK (					\
-	(1ULL << EF10_STAT_rx_pm_trunc_bb_overflow) |			\
-	(1ULL << EF10_STAT_rx_pm_discard_bb_overflow) |			\
-	(1ULL << EF10_STAT_rx_pm_trunc_vfifo_full) |			\
-	(1ULL << EF10_STAT_rx_pm_discard_vfifo_full) |			\
-	(1ULL << EF10_STAT_rx_pm_trunc_qbb) |				\
-	(1ULL << EF10_STAT_rx_pm_discard_qbb) |				\
-	(1ULL << EF10_STAT_rx_pm_discard_mapping) |			\
-	(1ULL << EF10_STAT_rx_dp_q_disabled_packets) |			\
-	(1ULL << EF10_STAT_rx_dp_di_dropped_packets) |			\
-	(1ULL << EF10_STAT_rx_dp_streaming_packets) |			\
-	(1ULL << EF10_STAT_rx_dp_hlb_fetch) |				\
-	(1ULL << EF10_STAT_rx_dp_hlb_wait))
+	(1ULL << EF10_STAT_port_rx_pm_trunc_bb_overflow) |		\
+	(1ULL << EF10_STAT_port_rx_pm_discard_bb_overflow) |		\
+	(1ULL << EF10_STAT_port_rx_pm_trunc_vfifo_full) |		\
+	(1ULL << EF10_STAT_port_rx_pm_discard_vfifo_full) |		\
+	(1ULL << EF10_STAT_port_rx_pm_trunc_qbb) |			\
+	(1ULL << EF10_STAT_port_rx_pm_discard_qbb) |			\
+	(1ULL << EF10_STAT_port_rx_pm_discard_mapping) |		\
+	(1ULL << EF10_STAT_port_rx_dp_q_disabled_packets) |		\
+	(1ULL << EF10_STAT_port_rx_dp_di_dropped_packets) |		\
+	(1ULL << EF10_STAT_port_rx_dp_streaming_packets) |		\
+	(1ULL << EF10_STAT_port_rx_dp_hlb_fetch) |			\
+	(1ULL << EF10_STAT_port_rx_dp_hlb_wait))
 
 static u64 efx_ef10_raw_stat_mask(struct efx_nic *efx)
 {
@@ -894,6 +1164,10 @@ static u64 efx_ef10_raw_stat_mask(struct efx_nic *efx)
 	u32 port_caps = efx_mcdi_phy_get_caps(efx);
 	struct efx_ef10_nic_data *nic_data = efx->nic_data;
 
+	if (!(efx->mcdi->fn_flags &
+	      1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_LINKCTRL))
+		return 0;
+
 	if (port_caps & (1 << MC_CMD_PHY_CAP_40000FDX_LBN))
 		raw_mask |= HUNT_40G_EXTRA_STAT_MASK;
 	else
@@ -908,13 +1182,28 @@ static u64 efx_ef10_raw_stat_mask(struct efx_nic *efx)
 
 static void efx_ef10_get_stat_mask(struct efx_nic *efx, unsigned long *mask)
 {
-	u64 raw_mask = efx_ef10_raw_stat_mask(efx);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	u64 raw_mask[2];
+
+	raw_mask[0] = efx_ef10_raw_stat_mask(efx);
+
+	/* Only show vadaptor stats when EVB capability is present */
+	if (nic_data->datapath_caps &
+	    (1 << MC_CMD_GET_CAPABILITIES_OUT_EVB_LBN)) {
+		raw_mask[0] |= ~((1ULL << EF10_STAT_rx_unicast) - 1);
+		raw_mask[1] = (1ULL << (EF10_STAT_COUNT - 63)) - 1;
+	} else {
+		raw_mask[1] = 0;
+	}
 
 #if BITS_PER_LONG == 64
-	mask[0] = raw_mask;
+	mask[0] = raw_mask[0];
+	mask[1] = raw_mask[1];
 #else
-	mask[0] = raw_mask & 0xffffffff;
-	mask[1] = raw_mask >> 32;
+	mask[0] = raw_mask[0] & 0xffffffff;
+	mask[1] = raw_mask[0] >> 32;
+	mask[2] = raw_mask[1] & 0xffffffff;
+	mask[3] = raw_mask[1] >> 32;
 #endif
 }
 
@@ -927,7 +1216,51 @@ static size_t efx_ef10_describe_stats(struct efx_nic *efx, u8 *names)
 				      mask, names);
 }
 
-static int efx_ef10_try_update_nic_stats(struct efx_nic *efx)
+static size_t efx_ef10_update_stats_common(struct efx_nic *efx, u64 *full_stats,
+					   struct rtnl_link_stats64 *core_stats)
+{
+	DECLARE_BITMAP(mask, EF10_STAT_COUNT);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	u64 *stats = nic_data->stats;
+	size_t stats_count = 0, index;
+
+	efx_ef10_get_stat_mask(efx, mask);
+
+	if (full_stats) {
+		for_each_set_bit(index, mask, EF10_STAT_COUNT) {
+			if (efx_ef10_stat_desc[index].name) {
+				*full_stats++ = stats[index];
+				++stats_count;
+			}
+		}
+	}
+
+	if (core_stats) {
+		core_stats->rx_packets = stats[EF10_STAT_rx_unicast] +
+					 stats[EF10_STAT_rx_multicast] +
+					 stats[EF10_STAT_rx_broadcast];
+		core_stats->tx_packets = stats[EF10_STAT_tx_unicast] +
+					 stats[EF10_STAT_tx_multicast] +
+					 stats[EF10_STAT_tx_broadcast];
+		core_stats->rx_bytes = stats[EF10_STAT_rx_unicast_bytes] +
+				       stats[EF10_STAT_rx_multicast_bytes] +
+				       stats[EF10_STAT_rx_broadcast_bytes];
+		core_stats->tx_bytes = stats[EF10_STAT_tx_unicast_bytes] +
+				       stats[EF10_STAT_tx_multicast_bytes] +
+				       stats[EF10_STAT_tx_broadcast_bytes];
+		core_stats->rx_dropped = stats[GENERIC_STAT_rx_nodesc_trunc] +
+					 stats[GENERIC_STAT_rx_noskb_drops];
+		core_stats->multicast = stats[EF10_STAT_rx_multicast];
+		core_stats->rx_crc_errors = stats[EF10_STAT_rx_bad];
+		core_stats->rx_fifo_errors = stats[EF10_STAT_rx_overflow];
+		core_stats->rx_errors = core_stats->rx_crc_errors;
+		core_stats->tx_errors = stats[EF10_STAT_tx_bad];
+	}
+
+	return stats_count;
+}
+
+static int efx_ef10_try_update_nic_stats_pf(struct efx_nic *efx)
 {
 	struct efx_ef10_nic_data *nic_data = efx->nic_data;
 	DECLARE_BITMAP(mask, EF10_STAT_COUNT);
@@ -952,67 +1285,114 @@ static int efx_ef10_try_update_nic_stats(struct efx_nic *efx)
 		return -EAGAIN;
 
 	/* Update derived statistics */
-	efx_nic_fix_nodesc_drop_stat(efx, &stats[EF10_STAT_rx_nodesc_drops]);
-	stats[EF10_STAT_rx_good_bytes] =
-		stats[EF10_STAT_rx_bytes] -
-		stats[EF10_STAT_rx_bytes_minus_good_bytes];
-	efx_update_diff_stat(&stats[EF10_STAT_rx_bad_bytes],
-			     stats[EF10_STAT_rx_bytes_minus_good_bytes]);
+	efx_nic_fix_nodesc_drop_stat(efx,
+				     &stats[EF10_STAT_port_rx_nodesc_drops]);
+	stats[EF10_STAT_port_rx_good_bytes] =
+		stats[EF10_STAT_port_rx_bytes] -
+		stats[EF10_STAT_port_rx_bytes_minus_good_bytes];
+	efx_update_diff_stat(&stats[EF10_STAT_port_rx_bad_bytes],
+			     stats[EF10_STAT_port_rx_bytes_minus_good_bytes]);
 	efx_update_sw_stats(efx, stats);
 	return 0;
 }
 
 
-static size_t efx_ef10_update_stats(struct efx_nic *efx, u64 *full_stats,
-				    struct rtnl_link_stats64 *core_stats)
+static size_t efx_ef10_update_stats_pf(struct efx_nic *efx, u64 *full_stats,
+				       struct rtnl_link_stats64 *core_stats)
 {
-	DECLARE_BITMAP(mask, EF10_STAT_COUNT);
-	struct efx_ef10_nic_data *nic_data = efx->nic_data;
-	u64 *stats = nic_data->stats;
-	size_t stats_count = 0, index;
 	int retry;
 
-	efx_ef10_get_stat_mask(efx, mask);
-
 	/* If we're unlucky enough to read statistics during the DMA, wait
 	 * up to 10ms for it to finish (typically takes <500us)
 	 */
 	for (retry = 0; retry < 100; ++retry) {
-		if (efx_ef10_try_update_nic_stats(efx) == 0)
+		if (efx_ef10_try_update_nic_stats_pf(efx) == 0)
 			break;
 		udelay(100);
 	}
 
-	if (full_stats) {
-		for_each_set_bit(index, mask, EF10_STAT_COUNT) {
-			if (efx_ef10_stat_desc[index].name) {
-				*full_stats++ = stats[index];
-				++stats_count;
-			}
-		}
+	return efx_ef10_update_stats_common(efx, full_stats, core_stats);
+}
+
+static int efx_ef10_try_update_nic_stats_vf(struct efx_nic *efx)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_MAC_STATS_IN_LEN);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	DECLARE_BITMAP(mask, EF10_STAT_COUNT);
+	__le64 generation_start, generation_end;
+	u64 *stats = nic_data->stats;
+	u32 dma_len = MC_CMD_MAC_NSTATS * sizeof(u64);
+	struct efx_buffer stats_buf;
+	__le64 *dma_stats;
+	int rc;
+
+	spin_unlock_bh(&efx->stats_lock);
+
+	if (in_interrupt()) {
+		/* If in atomic context, cannot update stats.  Just update the
+		 * software stats and return so the caller can continue.
+		 */
+		spin_lock_bh(&efx->stats_lock);
+		efx_update_sw_stats(efx, stats);
+		return 0;
 	}
 
-	if (core_stats) {
-		core_stats->rx_packets = stats[EF10_STAT_rx_packets];
-		core_stats->tx_packets = stats[EF10_STAT_tx_packets];
-		core_stats->rx_bytes = stats[EF10_STAT_rx_bytes];
-		core_stats->tx_bytes = stats[EF10_STAT_tx_bytes];
-		core_stats->rx_dropped = stats[EF10_STAT_rx_nodesc_drops] +
-					 stats[GENERIC_STAT_rx_nodesc_trunc] +
-					 stats[GENERIC_STAT_rx_noskb_drops];
-		core_stats->multicast = stats[EF10_STAT_rx_multicast];
-		core_stats->rx_length_errors =
-			stats[EF10_STAT_rx_gtjumbo] +
-			stats[EF10_STAT_rx_length_error];
-		core_stats->rx_crc_errors = stats[EF10_STAT_rx_bad];
-		core_stats->rx_frame_errors = stats[EF10_STAT_rx_align_error];
-		core_stats->rx_fifo_errors = stats[EF10_STAT_rx_overflow];
-		core_stats->rx_errors = (core_stats->rx_length_errors +
-					 core_stats->rx_crc_errors +
-					 core_stats->rx_frame_errors);
+	efx_ef10_get_stat_mask(efx, mask);
+
+	rc = efx_nic_alloc_buffer(efx, &stats_buf, dma_len, GFP_ATOMIC);
+	if (rc) {
+		spin_lock_bh(&efx->stats_lock);
+		return rc;
 	}
 
-	return stats_count;
+	dma_stats = stats_buf.addr;
+	dma_stats[MC_CMD_MAC_GENERATION_END] = EFX_MC_STATS_GENERATION_INVALID;
+
+	MCDI_SET_QWORD(inbuf, MAC_STATS_IN_DMA_ADDR, stats_buf.dma_addr);
+	MCDI_POPULATE_DWORD_1(inbuf, MAC_STATS_IN_CMD,
+			      MAC_STATS_IN_DMA, 1);
+	MCDI_SET_DWORD(inbuf, MAC_STATS_IN_DMA_LEN, dma_len);
+	MCDI_SET_DWORD(inbuf, MAC_STATS_IN_PORT_ID, EVB_PORT_ID_ASSIGNED);
+
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_MAC_STATS, inbuf, sizeof(inbuf),
+				NULL, 0, NULL);
+	spin_lock_bh(&efx->stats_lock);
+	if (rc) {
+		/* Expect ENOENT if DMA queues have not been set up */
+		if (rc != -ENOENT || atomic_read(&efx->active_queues))
+			efx_mcdi_display_error(efx, MC_CMD_MAC_STATS,
+					       sizeof(inbuf), NULL, 0, rc);
+		goto out;
+	}
+
+	generation_end = dma_stats[MC_CMD_MAC_GENERATION_END];
+	if (generation_end == EFX_MC_STATS_GENERATION_INVALID) {
+		WARN_ON_ONCE(1);
+		goto out;
+	}
+	rmb();
+	efx_nic_update_stats(efx_ef10_stat_desc, EF10_STAT_COUNT, mask,
+			     stats, stats_buf.addr, false);
+	rmb();
+	generation_start = dma_stats[MC_CMD_MAC_GENERATION_START];
+	if (generation_end != generation_start) {
+		rc = -EAGAIN;
+		goto out;
+	}
+
+	efx_update_sw_stats(efx, stats);
+out:
+	efx_nic_free_buffer(efx, &stats_buf);
+	return rc;
+}
+
+static size_t efx_ef10_update_stats_vf(struct efx_nic *efx, u64 *full_stats,
+				       struct rtnl_link_stats64 *core_stats)
+{
+	if (efx_ef10_try_update_nic_stats_vf(efx))
+		return 0;
+
+	return efx_ef10_update_stats_common(efx, full_stats, core_stats);
 }
 
 static void efx_ef10_push_irq_moderation(struct efx_channel *channel)
@@ -1044,6 +1424,14 @@ static void efx_ef10_push_irq_moderation(struct efx_channel *channel)
 	}
 }
 
+static void efx_ef10_get_wol_vf(struct efx_nic *efx,
+				struct ethtool_wolinfo *wol) {}
+
+static int efx_ef10_set_wol_vf(struct efx_nic *efx, u32 type)
+{
+	return -EOPNOTSUPP;
+}
+
 static void efx_ef10_get_wol(struct efx_nic *efx, struct ethtool_wolinfo *wol)
 {
 	wol->supported = 0;
@@ -1123,13 +1511,17 @@ static int efx_ef10_mcdi_poll_reboot(struct efx_nic *efx)
 	/* All our allocations have been reset */
 	efx_ef10_reset_mc_allocations(efx);
 
+	/* Driver-created vswitches and vports must be re-created */
+	nic_data->must_probe_vswitching = true;
+	nic_data->vport_id = EVB_PORT_ID_ASSIGNED;
+
 	/* The datapath firmware might have been changed */
 	nic_data->must_check_datapath_caps = true;
 
 	/* MAC statistics have been cleared on the NIC; clear the local
 	 * statistic that we update with efx_update_diff_stat().
 	 */
-	nic_data->stats[EF10_STAT_rx_bad_bytes] = 0;
+	nic_data->stats[EF10_STAT_port_rx_bad_bytes] = 0;
 
 	return -EIO;
 }
@@ -1232,16 +1624,17 @@ static void efx_ef10_tx_init(struct efx_tx_queue *tx_queue)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_INIT_TXQ_IN_LEN(EFX_MAX_DMAQ_SIZE * 8 /
 						       EFX_BUF_SIZE));
-	MCDI_DECLARE_BUF(outbuf, MC_CMD_INIT_TXQ_OUT_LEN);
 	bool csum_offload = tx_queue->queue & EFX_TXQ_TYPE_OFFLOAD;
 	size_t entries = tx_queue->txd.buf.len / EFX_BUF_SIZE;
 	struct efx_channel *channel = tx_queue->channel;
 	struct efx_nic *efx = tx_queue->efx;
-	size_t inlen, outlen;
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	size_t inlen;
 	dma_addr_t dma_addr;
 	efx_qword_t *txd;
 	int rc;
 	int i;
+	BUILD_BUG_ON(MC_CMD_INIT_TXQ_OUT_LEN != 0);
 
 	MCDI_SET_DWORD(inbuf, INIT_TXQ_IN_SIZE, tx_queue->ptr_mask + 1);
 	MCDI_SET_DWORD(inbuf, INIT_TXQ_IN_TARGET_EVQ, channel->channel);
@@ -1251,7 +1644,7 @@ static void efx_ef10_tx_init(struct efx_tx_queue *tx_queue)
 			      INIT_TXQ_IN_FLAG_IP_CSUM_DIS, !csum_offload,
 			      INIT_TXQ_IN_FLAG_TCP_CSUM_DIS, !csum_offload);
 	MCDI_SET_DWORD(inbuf, INIT_TXQ_IN_OWNER_ID, 0);
-	MCDI_SET_DWORD(inbuf, INIT_TXQ_IN_PORT_ID, EVB_PORT_ID_ASSIGNED);
+	MCDI_SET_DWORD(inbuf, INIT_TXQ_IN_PORT_ID, nic_data->vport_id);
 
 	dma_addr = tx_queue->txd.buf.dma_addr;
 
@@ -1266,7 +1659,7 @@ static void efx_ef10_tx_init(struct efx_tx_queue *tx_queue)
 	inlen = MC_CMD_INIT_TXQ_IN_LEN(entries);
 
 	rc = efx_mcdi_rpc(efx, MC_CMD_INIT_TXQ, inbuf, inlen,
-			  outbuf, sizeof(outbuf), &outlen);
+			  NULL, 0, NULL);
 	if (rc)
 		goto fail;
 
@@ -1299,7 +1692,7 @@ fail:
 static void efx_ef10_tx_fini(struct efx_tx_queue *tx_queue)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_FINI_TXQ_IN_LEN);
-	MCDI_DECLARE_BUF(outbuf, MC_CMD_FINI_TXQ_OUT_LEN);
+	MCDI_DECLARE_BUF_ERR(outbuf);
 	struct efx_nic *efx = tx_queue->efx;
 	size_t outlen;
 	int rc;
@@ -1378,19 +1771,33 @@ static void efx_ef10_tx_write(struct efx_tx_queue *tx_queue)
 	}
 }
 
-static int efx_ef10_alloc_rss_context(struct efx_nic *efx, u32 *context)
+static int efx_ef10_alloc_rss_context(struct efx_nic *efx, u32 *context,
+				      bool exclusive, unsigned *context_size)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_RSS_CONTEXT_ALLOC_IN_LEN);
 	MCDI_DECLARE_BUF(outbuf, MC_CMD_RSS_CONTEXT_ALLOC_OUT_LEN);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
 	size_t outlen;
 	int rc;
+	u32 alloc_type = exclusive ?
+				MC_CMD_RSS_CONTEXT_ALLOC_IN_TYPE_EXCLUSIVE :
+				MC_CMD_RSS_CONTEXT_ALLOC_IN_TYPE_SHARED;
+	unsigned rss_spread = exclusive ?
+				efx->rss_spread :
+				min(rounddown_pow_of_two(efx->rss_spread),
+				    EFX_EF10_MAX_SHARED_RSS_CONTEXT_SIZE);
+
+	if (!exclusive && rss_spread == 1) {
+		*context = EFX_EF10_RSS_CONTEXT_INVALID;
+		if (context_size)
+			*context_size = 1;
+		return 0;
+	}
 
 	MCDI_SET_DWORD(inbuf, RSS_CONTEXT_ALLOC_IN_UPSTREAM_PORT_ID,
-		       EVB_PORT_ID_ASSIGNED);
-	MCDI_SET_DWORD(inbuf, RSS_CONTEXT_ALLOC_IN_TYPE,
-		       MC_CMD_RSS_CONTEXT_ALLOC_IN_TYPE_EXCLUSIVE);
-	MCDI_SET_DWORD(inbuf, RSS_CONTEXT_ALLOC_IN_NUM_QUEUES,
-		       EFX_MAX_CHANNELS);
+		       nic_data->vport_id);
+	MCDI_SET_DWORD(inbuf, RSS_CONTEXT_ALLOC_IN_TYPE, alloc_type);
+	MCDI_SET_DWORD(inbuf, RSS_CONTEXT_ALLOC_IN_NUM_QUEUES, rss_spread);
 
 	rc = efx_mcdi_rpc(efx, MC_CMD_RSS_CONTEXT_ALLOC, inbuf, sizeof(inbuf),
 		outbuf, sizeof(outbuf), &outlen);
@@ -1402,6 +1809,9 @@ static int efx_ef10_alloc_rss_context(struct efx_nic *efx, u32 *context)
 
 	*context = MCDI_DWORD(outbuf, RSS_CONTEXT_ALLOC_OUT_RSS_CONTEXT_ID);
 
+	if (context_size)
+		*context_size = rss_spread;
+
 	return 0;
 }
 
@@ -1418,7 +1828,8 @@ static void efx_ef10_free_rss_context(struct efx_nic *efx, u32 context)
 	WARN_ON(rc != 0);
 }
 
-static int efx_ef10_populate_rss_table(struct efx_nic *efx, u32 context)
+static int efx_ef10_populate_rss_table(struct efx_nic *efx, u32 context,
+				       const u32 *rx_indir_table)
 {
 	MCDI_DECLARE_BUF(tablebuf, MC_CMD_RSS_CONTEXT_SET_TABLE_IN_LEN);
 	MCDI_DECLARE_BUF(keybuf, MC_CMD_RSS_CONTEXT_SET_KEY_IN_LEN);
@@ -1432,7 +1843,7 @@ static int efx_ef10_populate_rss_table(struct efx_nic *efx, u32 context)
 	for (i = 0; i < ARRAY_SIZE(efx->rx_indir_table); ++i)
 		MCDI_PTR(tablebuf,
 			 RSS_CONTEXT_SET_TABLE_IN_INDIRECTION_TABLE)[i] =
-				(u8) efx->rx_indir_table[i];
+				(u8) rx_indir_table[i];
 
 	rc = efx_mcdi_rpc(efx, MC_CMD_RSS_CONTEXT_SET_TABLE, tablebuf,
 			  sizeof(tablebuf), NULL, 0, NULL);
@@ -1460,27 +1871,119 @@ static void efx_ef10_rx_free_indir_table(struct efx_nic *efx)
 	nic_data->rx_rss_context = EFX_EF10_RSS_CONTEXT_INVALID;
 }
 
-static void efx_ef10_rx_push_rss_config(struct efx_nic *efx)
+static int efx_ef10_rx_push_shared_rss_config(struct efx_nic *efx,
+					      unsigned *context_size)
 {
+	u32 new_rx_rss_context;
 	struct efx_ef10_nic_data *nic_data = efx->nic_data;
-	int rc;
+	int rc = efx_ef10_alloc_rss_context(efx, &new_rx_rss_context,
+					    false, context_size);
 
-	netif_dbg(efx, drv, efx->net_dev, "pushing RSS config\n");
+	if (rc != 0)
+		return rc;
 
-	if (nic_data->rx_rss_context == EFX_EF10_RSS_CONTEXT_INVALID) {
-		rc = efx_ef10_alloc_rss_context(efx, &nic_data->rx_rss_context);
-		if (rc != 0)
-			goto fail;
+	nic_data->rx_rss_context = new_rx_rss_context;
+	nic_data->rx_rss_context_exclusive = false;
+	efx_set_default_rx_indir_table(efx);
+	return 0;
+}
+
+static int efx_ef10_rx_push_exclusive_rss_config(struct efx_nic *efx,
+						 const u32 *rx_indir_table)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	int rc;
+	u32 new_rx_rss_context;
+
+	if (nic_data->rx_rss_context == EFX_EF10_RSS_CONTEXT_INVALID ||
+	    !nic_data->rx_rss_context_exclusive) {
+		rc = efx_ef10_alloc_rss_context(efx, &new_rx_rss_context,
+						true, NULL);
+		if (rc == -EOPNOTSUPP)
+			return rc;
+		else if (rc != 0)
+			goto fail1;
+	} else {
+		new_rx_rss_context = nic_data->rx_rss_context;
 	}
 
-	rc = efx_ef10_populate_rss_table(efx, nic_data->rx_rss_context);
+	rc = efx_ef10_populate_rss_table(efx, new_rx_rss_context,
+					 rx_indir_table);
 	if (rc != 0)
-		goto fail;
+		goto fail2;
 
-	return;
+	if (nic_data->rx_rss_context != new_rx_rss_context)
+		efx_ef10_rx_free_indir_table(efx);
+	nic_data->rx_rss_context = new_rx_rss_context;
+	nic_data->rx_rss_context_exclusive = true;
+	if (rx_indir_table != efx->rx_indir_table)
+		memcpy(efx->rx_indir_table, rx_indir_table,
+		       sizeof(efx->rx_indir_table));
+	return 0;
 
-fail:
+fail2:
+	if (new_rx_rss_context != nic_data->rx_rss_context)
+		efx_ef10_free_rss_context(efx, new_rx_rss_context);
+fail1:
 	netif_err(efx, hw, efx->net_dev, "%s: failed rc=%d\n", __func__, rc);
+	return rc;
+}
+
+static int efx_ef10_pf_rx_push_rss_config(struct efx_nic *efx, bool user,
+					  const u32 *rx_indir_table)
+{
+	int rc;
+
+	if (efx->rss_spread == 1)
+		return 0;
+
+	rc = efx_ef10_rx_push_exclusive_rss_config(efx, rx_indir_table);
+
+	if (rc == -ENOBUFS && !user) {
+		unsigned context_size;
+		bool mismatch = false;
+		size_t i;
+
+		for (i = 0; i < ARRAY_SIZE(efx->rx_indir_table) && !mismatch;
+		     i++)
+			mismatch = rx_indir_table[i] !=
+				ethtool_rxfh_indir_default(i, efx->rss_spread);
+
+		rc = efx_ef10_rx_push_shared_rss_config(efx, &context_size);
+		if (rc == 0) {
+			if (context_size != efx->rss_spread)
+				netif_warn(efx, probe, efx->net_dev,
+					   "Could not allocate an exclusive RSS"
+					   " context; allocated a shared one of"
+					   " different size."
+					   " Wanted %u, got %u.\n",
+					   efx->rss_spread, context_size);
+			else if (mismatch)
+				netif_warn(efx, probe, efx->net_dev,
+					   "Could not allocate an exclusive RSS"
+					   " context; allocated a shared one but"
+					   " could not apply custom"
+					   " indirection.\n");
+			else
+				netif_info(efx, probe, efx->net_dev,
+					   "Could not allocate an exclusive RSS"
+					   " context; allocated a shared one.\n");
+		}
+	}
+	return rc;
+}
+
+static int efx_ef10_vf_rx_push_rss_config(struct efx_nic *efx, bool user,
+					  const u32 *rx_indir_table
+					  __attribute__ ((unused)))
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+	if (user)
+		return -EOPNOTSUPP;
+	if (nic_data->rx_rss_context != EFX_EF10_RSS_CONTEXT_INVALID)
+		return 0;
+	return efx_ef10_rx_push_shared_rss_config(efx, NULL);
 }
 
 static int efx_ef10_rx_probe(struct efx_rx_queue *rx_queue)
@@ -1496,14 +1999,15 @@ static void efx_ef10_rx_init(struct efx_rx_queue *rx_queue)
 	MCDI_DECLARE_BUF(inbuf,
 			 MC_CMD_INIT_RXQ_IN_LEN(EFX_MAX_DMAQ_SIZE * 8 /
 						EFX_BUF_SIZE));
-	MCDI_DECLARE_BUF(outbuf, MC_CMD_INIT_RXQ_OUT_LEN);
 	struct efx_channel *channel = efx_rx_queue_channel(rx_queue);
 	size_t entries = rx_queue->rxd.buf.len / EFX_BUF_SIZE;
 	struct efx_nic *efx = rx_queue->efx;
-	size_t inlen, outlen;
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	size_t inlen;
 	dma_addr_t dma_addr;
 	int rc;
 	int i;
+	BUILD_BUG_ON(MC_CMD_INIT_RXQ_OUT_LEN != 0);
 
 	rx_queue->scatter_n = 0;
 	rx_queue->scatter_len = 0;
@@ -1517,7 +2021,7 @@ static void efx_ef10_rx_init(struct efx_rx_queue *rx_queue)
 			      INIT_RXQ_IN_FLAG_PREFIX, 1,
 			      INIT_RXQ_IN_FLAG_TIMESTAMP, 1);
 	MCDI_SET_DWORD(inbuf, INIT_RXQ_IN_OWNER_ID, 0);
-	MCDI_SET_DWORD(inbuf, INIT_RXQ_IN_PORT_ID, EVB_PORT_ID_ASSIGNED);
+	MCDI_SET_DWORD(inbuf, INIT_RXQ_IN_PORT_ID, nic_data->vport_id);
 
 	dma_addr = rx_queue->rxd.buf.dma_addr;
 
@@ -1532,7 +2036,7 @@ static void efx_ef10_rx_init(struct efx_rx_queue *rx_queue)
 	inlen = MC_CMD_INIT_RXQ_IN_LEN(entries);
 
 	rc = efx_mcdi_rpc(efx, MC_CMD_INIT_RXQ, inbuf, inlen,
-			  outbuf, sizeof(outbuf), &outlen);
+			  NULL, 0, NULL);
 	if (rc)
 		netdev_WARN(efx->net_dev, "failed to initialise RXQ %d\n",
 			    efx_rx_queue_index(rx_queue));
@@ -1541,7 +2045,7 @@ static void efx_ef10_rx_init(struct efx_rx_queue *rx_queue)
 static void efx_ef10_rx_fini(struct efx_rx_queue *rx_queue)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_FINI_RXQ_IN_LEN);
-	MCDI_DECLARE_BUF(outbuf, MC_CMD_FINI_RXQ_OUT_LEN);
+	MCDI_DECLARE_BUF_ERR(outbuf);
 	struct efx_nic *efx = rx_queue->efx;
 	size_t outlen;
 	int rc;
@@ -1703,7 +2207,7 @@ static int efx_ef10_ev_init(struct efx_channel *channel)
 static void efx_ef10_ev_fini(struct efx_channel *channel)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_FINI_EVQ_IN_LEN);
-	MCDI_DECLARE_BUF(outbuf, MC_CMD_FINI_EVQ_OUT_LEN);
+	MCDI_DECLARE_BUF_ERR(outbuf);
 	struct efx_nic *efx = channel->efx;
 	size_t outlen;
 	int rc;
@@ -2286,11 +2790,12 @@ static void efx_ef10_filter_push_prep(struct efx_nic *efx,
 			       match_fields);
 	}
 
-	MCDI_SET_DWORD(inbuf, FILTER_OP_IN_PORT_ID, EVB_PORT_ID_ASSIGNED);
+	MCDI_SET_DWORD(inbuf, FILTER_OP_IN_PORT_ID, nic_data->vport_id);
 	MCDI_SET_DWORD(inbuf, FILTER_OP_IN_RX_DEST,
 		       spec->dmaq_id == EFX_FILTER_RX_DMAQ_ID_DROP ?
 		       MC_CMD_FILTER_OP_IN_RX_DEST_DROP :
 		       MC_CMD_FILTER_OP_IN_RX_DEST_HOST);
+	MCDI_SET_DWORD(inbuf, FILTER_OP_IN_TX_DOMAIN, 0);
 	MCDI_SET_DWORD(inbuf, FILTER_OP_IN_TX_DEST,
 		       MC_CMD_FILTER_OP_IN_TX_DEST_DEFAULT);
 	MCDI_SET_DWORD(inbuf, FILTER_OP_IN_RX_QUEUE,
@@ -3055,6 +3560,9 @@ fail:
 	return rc;
 }
 
+/* Caller must hold efx->filter_sem for read if race against
+ * efx_ef10_filter_table_remove() is possible
+ */
 static void efx_ef10_filter_table_restore(struct efx_nic *efx)
 {
 	struct efx_ef10_filter_table *table = efx->filter_state;
@@ -3064,9 +3572,14 @@ static void efx_ef10_filter_table_restore(struct efx_nic *efx)
 	bool failed = false;
 	int rc;
 
+	WARN_ON(!rwsem_is_locked(&efx->filter_sem));
+
 	if (!nic_data->must_restore_filters)
 		return;
 
+	if (!table)
+		return;
+
 	spin_lock_bh(&efx->filter_lock);
 
 	for (filter_idx = 0; filter_idx < HUNT_FILTER_TBL_ROWS; filter_idx++) {
@@ -3102,6 +3615,7 @@ static void efx_ef10_filter_table_restore(struct efx_nic *efx)
 		nic_data->must_restore_filters = false;
 }
 
+/* Caller must hold efx->filter_sem for write */
 static void efx_ef10_filter_table_remove(struct efx_nic *efx)
 {
 	struct efx_ef10_filter_table *table = efx->filter_state;
@@ -3110,6 +3624,10 @@ static void efx_ef10_filter_table_remove(struct efx_nic *efx)
 	unsigned int filter_idx;
 	int rc;
 
+	efx->filter_state = NULL;
+	if (!table)
+		return;
+
 	for (filter_idx = 0; filter_idx < HUNT_FILTER_TBL_ROWS; filter_idx++) {
 		spec = efx_ef10_filter_entry_spec(table, filter_idx);
 		if (!spec)
@@ -3135,6 +3653,9 @@ static void efx_ef10_filter_table_remove(struct efx_nic *efx)
 	kfree(table);
 }
 
+/* Caller must hold efx->filter_sem for read if race against
+ * efx_ef10_filter_table_remove() is possible
+ */
 static void efx_ef10_filter_sync_rx_mode(struct efx_nic *efx)
 {
 	struct efx_ef10_filter_table *table = efx->filter_state;
@@ -3149,6 +3670,9 @@ static void efx_ef10_filter_sync_rx_mode(struct efx_nic *efx)
 	if (!efx_dev_registered(efx))
 		return;
 
+	if (!table)
+		return;
+
 	/* Mark old filters that may need to be removed */
 	spin_lock_bh(&efx->filter_lock);
 	n = table->dev_uc_count < 0 ? 1 : table->dev_uc_count;
@@ -3280,6 +3804,78 @@ static void efx_ef10_filter_sync_rx_mode(struct efx_nic *efx)
 	WARN_ON(remove_failed);
 }
 
+static int efx_ef10_set_mac_address(struct efx_nic *efx)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VADAPTOR_SET_MAC_IN_LEN);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	bool was_enabled = efx->port_enabled;
+	int rc;
+
+	efx_device_detach_sync(efx);
+	efx_net_stop(efx->net_dev);
+	down_write(&efx->filter_sem);
+	efx_ef10_filter_table_remove(efx);
+
+	ether_addr_copy(MCDI_PTR(inbuf, VADAPTOR_SET_MAC_IN_MACADDR),
+			efx->net_dev->dev_addr);
+	MCDI_SET_DWORD(inbuf, VADAPTOR_SET_MAC_IN_UPSTREAM_PORT_ID,
+		       nic_data->vport_id);
+	rc = efx_mcdi_rpc(efx, MC_CMD_VADAPTOR_SET_MAC, inbuf,
+			  sizeof(inbuf), NULL, 0, NULL);
+
+	efx_ef10_filter_table_probe(efx);
+	up_write(&efx->filter_sem);
+	if (was_enabled)
+		efx_net_open(efx->net_dev);
+	netif_device_attach(efx->net_dev);
+
+#if !defined(CONFIG_SFC_SRIOV)
+	if (rc == -EPERM)
+		netif_err(efx, drv, efx->net_dev,
+			  "Cannot change MAC address; use sfboot to enable mac-spoofing"
+			  " on this interface\n");
+#else
+	if (rc == -EPERM) {
+		struct pci_dev *pci_dev_pf = efx->pci_dev->physfn;
+
+		/* Switch to PF and change MAC address on vport */
+		if (efx->pci_dev->is_virtfn && pci_dev_pf) {
+			struct efx_nic *efx_pf = pci_get_drvdata(pci_dev_pf);
+
+			if (!efx_ef10_sriov_set_vf_mac(efx_pf,
+						       nic_data->vf_index,
+						       efx->net_dev->dev_addr))
+				return 0;
+		}
+		netif_err(efx, drv, efx->net_dev,
+			  "Cannot change MAC address; use sfboot to enable mac-spoofing"
+			  " on this interface\n");
+	} else if (efx->pci_dev->is_virtfn) {
+		/* Successfully changed by VF (with MAC spoofing), so update the
+		 * parent PF if possible.
+		 */
+		struct pci_dev *pci_dev_pf = efx->pci_dev->physfn;
+
+		if (pci_dev_pf) {
+			struct efx_nic *efx_pf = pci_get_drvdata(pci_dev_pf);
+			struct efx_ef10_nic_data *nic_data = efx_pf->nic_data;
+			unsigned int i;
+
+			for (i = 0; i < efx_pf->vf_count; ++i) {
+				struct ef10_vf *vf = nic_data->vf + i;
+
+				if (vf->efx == efx) {
+					ether_addr_copy(vf->mac,
+							efx->net_dev->dev_addr);
+					return 0;
+				}
+			}
+		}
+	}
+#endif
+	return rc;
+}
+
 static int efx_ef10_mac_reconfigure(struct efx_nic *efx)
 {
 	efx_ef10_filter_sync_rx_mode(efx);
@@ -3287,6 +3883,13 @@ static int efx_ef10_mac_reconfigure(struct efx_nic *efx)
 	return efx_mcdi_set_mac(efx);
 }
 
+static int efx_ef10_mac_reconfigure_vf(struct efx_nic *efx)
+{
+	efx_ef10_filter_sync_rx_mode(efx);
+
+	return 0;
+}
+
 static int efx_ef10_start_bist(struct efx_nic *efx, u32 bist_type)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_START_BIST_IN_LEN);
@@ -3494,6 +4097,9 @@ static void efx_ef10_ptp_write_host_time(struct efx_nic *efx, u32 host_time)
 	_efx_writed(efx, cpu_to_le32(host_time), ER_DZ_MC_DB_LWRD);
 }
 
+static void efx_ef10_ptp_write_host_time_vf(struct efx_nic *efx,
+					    u32 host_time) {}
+
 static int efx_ef10_rx_enable_timestamping(struct efx_channel *channel,
 					   bool temp)
 {
@@ -3571,6 +4177,12 @@ static int efx_ef10_ptp_set_ts_sync_events(struct efx_nic *efx, bool en,
 	return 0;
 }
 
+static int efx_ef10_ptp_set_ts_config_vf(struct efx_nic *efx,
+					 struct hwtstamp_config *init)
+{
+	return -EOPNOTSUPP;
+}
+
 static int efx_ef10_ptp_set_ts_config(struct efx_nic *efx,
 				      struct hwtstamp_config *init)
 {
@@ -3607,14 +4219,118 @@ static int efx_ef10_ptp_set_ts_config(struct efx_nic *efx,
 	}
 }
 
+const struct efx_nic_type efx_hunt_a0_vf_nic_type = {
+	.is_vf = true,
+	.mem_bar = EFX_MEM_VF_BAR,
+	.mem_map_size = efx_ef10_mem_map_size,
+	.probe = efx_ef10_probe_vf,
+	.remove = efx_ef10_remove,
+	.dimension_resources = efx_ef10_dimension_resources,
+	.init = efx_ef10_init_nic,
+	.fini = efx_port_dummy_op_void,
+	.map_reset_reason = efx_ef10_map_reset_reason,
+	.map_reset_flags = efx_ef10_map_reset_flags,
+	.reset = efx_ef10_reset,
+	.probe_port = efx_mcdi_port_probe,
+	.remove_port = efx_mcdi_port_remove,
+	.fini_dmaq = efx_ef10_fini_dmaq,
+	.prepare_flr = efx_ef10_prepare_flr,
+	.finish_flr = efx_port_dummy_op_void,
+	.describe_stats = efx_ef10_describe_stats,
+	.update_stats = efx_ef10_update_stats_vf,
+	.start_stats = efx_port_dummy_op_void,
+	.pull_stats = efx_port_dummy_op_void,
+	.stop_stats = efx_port_dummy_op_void,
+	.set_id_led = efx_mcdi_set_id_led,
+	.push_irq_moderation = efx_ef10_push_irq_moderation,
+	.reconfigure_mac = efx_ef10_mac_reconfigure_vf,
+	.check_mac_fault = efx_mcdi_mac_check_fault,
+	.reconfigure_port = efx_mcdi_port_reconfigure,
+	.get_wol = efx_ef10_get_wol_vf,
+	.set_wol = efx_ef10_set_wol_vf,
+	.resume_wol = efx_port_dummy_op_void,
+	.mcdi_request = efx_ef10_mcdi_request,
+	.mcdi_poll_response = efx_ef10_mcdi_poll_response,
+	.mcdi_read_response = efx_ef10_mcdi_read_response,
+	.mcdi_poll_reboot = efx_ef10_mcdi_poll_reboot,
+	.irq_enable_master = efx_port_dummy_op_void,
+	.irq_test_generate = efx_ef10_irq_test_generate,
+	.irq_disable_non_ev = efx_port_dummy_op_void,
+	.irq_handle_msi = efx_ef10_msi_interrupt,
+	.irq_handle_legacy = efx_ef10_legacy_interrupt,
+	.tx_probe = efx_ef10_tx_probe,
+	.tx_init = efx_ef10_tx_init,
+	.tx_remove = efx_ef10_tx_remove,
+	.tx_write = efx_ef10_tx_write,
+	.rx_push_rss_config = efx_ef10_vf_rx_push_rss_config,
+	.rx_probe = efx_ef10_rx_probe,
+	.rx_init = efx_ef10_rx_init,
+	.rx_remove = efx_ef10_rx_remove,
+	.rx_write = efx_ef10_rx_write,
+	.rx_defer_refill = efx_ef10_rx_defer_refill,
+	.ev_probe = efx_ef10_ev_probe,
+	.ev_init = efx_ef10_ev_init,
+	.ev_fini = efx_ef10_ev_fini,
+	.ev_remove = efx_ef10_ev_remove,
+	.ev_process = efx_ef10_ev_process,
+	.ev_read_ack = efx_ef10_ev_read_ack,
+	.ev_test_generate = efx_ef10_ev_test_generate,
+	.filter_table_probe = efx_ef10_filter_table_probe,
+	.filter_table_restore = efx_ef10_filter_table_restore,
+	.filter_table_remove = efx_ef10_filter_table_remove,
+	.filter_update_rx_scatter = efx_ef10_filter_update_rx_scatter,
+	.filter_insert = efx_ef10_filter_insert,
+	.filter_remove_safe = efx_ef10_filter_remove_safe,
+	.filter_get_safe = efx_ef10_filter_get_safe,
+	.filter_clear_rx = efx_ef10_filter_clear_rx,
+	.filter_count_rx_used = efx_ef10_filter_count_rx_used,
+	.filter_get_rx_id_limit = efx_ef10_filter_get_rx_id_limit,
+	.filter_get_rx_ids = efx_ef10_filter_get_rx_ids,
+#ifdef CONFIG_RFS_ACCEL
+	.filter_rfs_insert = efx_ef10_filter_rfs_insert,
+	.filter_rfs_expire_one = efx_ef10_filter_rfs_expire_one,
+#endif
+#ifdef CONFIG_SFC_MTD
+	.mtd_probe = efx_port_dummy_op_int,
+#endif
+	.ptp_write_host_time = efx_ef10_ptp_write_host_time_vf,
+	.ptp_set_ts_config = efx_ef10_ptp_set_ts_config_vf,
+#ifdef CONFIG_SFC_SRIOV
+	.vswitching_probe = efx_ef10_vswitching_probe_vf,
+	.vswitching_restore = efx_ef10_vswitching_restore_vf,
+	.vswitching_remove = efx_ef10_vswitching_remove_vf,
+	.sriov_get_phys_port_id = efx_ef10_sriov_get_phys_port_id,
+#endif
+	.get_mac_address = efx_ef10_get_mac_address_vf,
+	.set_mac_address = efx_ef10_set_mac_address,
+
+	.revision = EFX_REV_HUNT_A0,
+	.max_dma_mask = DMA_BIT_MASK(ESF_DZ_TX_KER_BUF_ADDR_WIDTH),
+	.rx_prefix_size = ES_DZ_RX_PREFIX_SIZE,
+	.rx_hash_offset = ES_DZ_RX_PREFIX_HASH_OFST,
+	.rx_ts_offset = ES_DZ_RX_PREFIX_TSTAMP_OFST,
+	.can_rx_scatter = true,
+	.always_rx_scatter = true,
+	.max_interrupt_mode = EFX_INT_MODE_MSIX,
+	.timer_period_max = 1 << ERF_DD_EVQ_IND_TIMER_VAL_WIDTH,
+	.offload_features = (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
+			     NETIF_F_RXHASH | NETIF_F_NTUPLE),
+	.mcdi_max_ver = 2,
+	.max_rx_ip_filters = HUNT_FILTER_TBL_ROWS,
+	.hwtstamp_filters = 1 << HWTSTAMP_FILTER_NONE |
+			    1 << HWTSTAMP_FILTER_ALL,
+};
+
 const struct efx_nic_type efx_hunt_a0_nic_type = {
+	.is_vf = false,
+	.mem_bar = EFX_MEM_BAR,
 	.mem_map_size = efx_ef10_mem_map_size,
-	.probe = efx_ef10_probe,
+	.probe = efx_ef10_probe_pf,
 	.remove = efx_ef10_remove,
 	.dimension_resources = efx_ef10_dimension_resources,
 	.init = efx_ef10_init_nic,
 	.fini = efx_port_dummy_op_void,
-	.map_reset_reason = efx_mcdi_map_reset_reason,
+	.map_reset_reason = efx_ef10_map_reset_reason,
 	.map_reset_flags = efx_ef10_map_reset_flags,
 	.reset = efx_ef10_reset,
 	.probe_port = efx_mcdi_port_probe,
@@ -3623,7 +4339,7 @@ const struct efx_nic_type efx_hunt_a0_nic_type = {
 	.prepare_flr = efx_ef10_prepare_flr,
 	.finish_flr = efx_port_dummy_op_void,
 	.describe_stats = efx_ef10_describe_stats,
-	.update_stats = efx_ef10_update_stats,
+	.update_stats = efx_ef10_update_stats_pf,
 	.start_stats = efx_mcdi_mac_start_stats,
 	.pull_stats = efx_mcdi_mac_pull_stats,
 	.stop_stats = efx_mcdi_mac_stop_stats,
@@ -3650,7 +4366,7 @@ const struct efx_nic_type efx_hunt_a0_nic_type = {
 	.tx_init = efx_ef10_tx_init,
 	.tx_remove = efx_ef10_tx_remove,
 	.tx_write = efx_ef10_tx_write,
-	.rx_push_rss_config = efx_ef10_rx_push_rss_config,
+	.rx_push_rss_config = efx_ef10_pf_rx_push_rss_config,
 	.rx_probe = efx_ef10_rx_probe,
 	.rx_init = efx_ef10_rx_init,
 	.rx_remove = efx_ef10_rx_remove,
@@ -3689,11 +4405,24 @@ const struct efx_nic_type efx_hunt_a0_nic_type = {
 	.ptp_write_host_time = efx_ef10_ptp_write_host_time,
 	.ptp_set_ts_sync_events = efx_ef10_ptp_set_ts_sync_events,
 	.ptp_set_ts_config = efx_ef10_ptp_set_ts_config,
+#ifdef CONFIG_SFC_SRIOV
+	.sriov_configure = efx_ef10_sriov_configure,
 	.sriov_init = efx_ef10_sriov_init,
 	.sriov_fini = efx_ef10_sriov_fini,
-	.sriov_mac_address_changed = efx_ef10_sriov_mac_address_changed,
 	.sriov_wanted = efx_ef10_sriov_wanted,
 	.sriov_reset = efx_ef10_sriov_reset,
+	.sriov_flr = efx_ef10_sriov_flr,
+	.sriov_set_vf_mac = efx_ef10_sriov_set_vf_mac,
+	.sriov_set_vf_vlan = efx_ef10_sriov_set_vf_vlan,
+	.sriov_set_vf_spoofchk = efx_ef10_sriov_set_vf_spoofchk,
+	.sriov_get_vf_config = efx_ef10_sriov_get_vf_config,
+	.sriov_set_vf_link_state = efx_ef10_sriov_set_vf_link_state,
+	.vswitching_probe = efx_ef10_vswitching_probe_pf,
+	.vswitching_restore = efx_ef10_vswitching_restore_pf,
+	.vswitching_remove = efx_ef10_vswitching_remove_pf,
+#endif
+	.get_mac_address = efx_ef10_get_mac_address_pf,
+	.set_mac_address = efx_ef10_set_mac_address,
 
 	.revision = EFX_REV_HUNT_A0,
 	.max_dma_mask = DMA_BIT_MASK(ESF_DZ_TX_KER_BUF_ADDR_WIDTH),
diff --git a/drivers/net/ethernet/sfc/ef10_sriov.c b/drivers/net/ethernet/sfc/ef10_sriov.c
new file mode 100644
index 000000000000..6c9b6e45509a
--- /dev/null
+++ b/drivers/net/ethernet/sfc/ef10_sriov.c
@@ -0,0 +1,783 @@
+/****************************************************************************
+ * Driver for Solarflare network controllers and boards
+ * Copyright 2015 Solarflare Communications 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, incorporated herein by reference.
+ */
+#include <linux/pci.h>
+#include <linux/module.h>
+#include "net_driver.h"
+#include "ef10_sriov.h"
+#include "efx.h"
+#include "nic.h"
+#include "mcdi_pcol.h"
+
+static int efx_ef10_evb_port_assign(struct efx_nic *efx, unsigned int port_id,
+				    unsigned int vf_fn)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_EVB_PORT_ASSIGN_IN_LEN);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+	MCDI_SET_DWORD(inbuf, EVB_PORT_ASSIGN_IN_PORT_ID, port_id);
+	MCDI_POPULATE_DWORD_2(inbuf, EVB_PORT_ASSIGN_IN_FUNCTION,
+			      EVB_PORT_ASSIGN_IN_PF, nic_data->pf_index,
+			      EVB_PORT_ASSIGN_IN_VF, vf_fn);
+
+	return efx_mcdi_rpc(efx, MC_CMD_EVB_PORT_ASSIGN, inbuf, sizeof(inbuf),
+			    NULL, 0, NULL);
+}
+
+static int efx_ef10_vport_add_mac(struct efx_nic *efx,
+				  unsigned int port_id, u8 *mac)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VPORT_ADD_MAC_ADDRESS_IN_LEN);
+
+	MCDI_SET_DWORD(inbuf, VPORT_ADD_MAC_ADDRESS_IN_VPORT_ID, port_id);
+	ether_addr_copy(MCDI_PTR(inbuf, VPORT_ADD_MAC_ADDRESS_IN_MACADDR), mac);
+
+	return efx_mcdi_rpc(efx, MC_CMD_VPORT_ADD_MAC_ADDRESS, inbuf,
+			    sizeof(inbuf), NULL, 0, NULL);
+}
+
+static int efx_ef10_vport_del_mac(struct efx_nic *efx,
+				  unsigned int port_id, u8 *mac)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VPORT_DEL_MAC_ADDRESS_IN_LEN);
+
+	MCDI_SET_DWORD(inbuf, VPORT_DEL_MAC_ADDRESS_IN_VPORT_ID, port_id);
+	ether_addr_copy(MCDI_PTR(inbuf, VPORT_DEL_MAC_ADDRESS_IN_MACADDR), mac);
+
+	return efx_mcdi_rpc(efx, MC_CMD_VPORT_DEL_MAC_ADDRESS, inbuf,
+			    sizeof(inbuf), NULL, 0, NULL);
+}
+
+static int efx_ef10_vswitch_alloc(struct efx_nic *efx, unsigned int port_id,
+				  unsigned int vswitch_type)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VSWITCH_ALLOC_IN_LEN);
+	int rc;
+
+	MCDI_SET_DWORD(inbuf, VSWITCH_ALLOC_IN_UPSTREAM_PORT_ID, port_id);
+	MCDI_SET_DWORD(inbuf, VSWITCH_ALLOC_IN_TYPE, vswitch_type);
+	MCDI_SET_DWORD(inbuf, VSWITCH_ALLOC_IN_NUM_VLAN_TAGS, 2);
+	MCDI_POPULATE_DWORD_1(inbuf, VSWITCH_ALLOC_IN_FLAGS,
+			      VSWITCH_ALLOC_IN_FLAG_AUTO_PORT, 0);
+
+	/* Quietly try to allocate 2 VLAN tags */
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_VSWITCH_ALLOC, inbuf, sizeof(inbuf),
+				NULL, 0, NULL);
+
+	/* If 2 VLAN tags is too many, revert to trying with 1 VLAN tags */
+	if (rc == -EPROTO) {
+		MCDI_SET_DWORD(inbuf, VSWITCH_ALLOC_IN_NUM_VLAN_TAGS, 1);
+		rc = efx_mcdi_rpc(efx, MC_CMD_VSWITCH_ALLOC, inbuf,
+				  sizeof(inbuf), NULL, 0, NULL);
+	} else if (rc) {
+		efx_mcdi_display_error(efx, MC_CMD_VSWITCH_ALLOC,
+				       MC_CMD_VSWITCH_ALLOC_IN_LEN,
+				       NULL, 0, rc);
+	}
+	return rc;
+}
+
+static int efx_ef10_vswitch_free(struct efx_nic *efx, unsigned int port_id)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VSWITCH_FREE_IN_LEN);
+
+	MCDI_SET_DWORD(inbuf, VSWITCH_FREE_IN_UPSTREAM_PORT_ID, port_id);
+
+	return efx_mcdi_rpc(efx, MC_CMD_VSWITCH_FREE, inbuf, sizeof(inbuf),
+			    NULL, 0, NULL);
+}
+
+static int efx_ef10_vport_alloc(struct efx_nic *efx,
+				unsigned int port_id_in,
+				unsigned int vport_type,
+				u16 vlan,
+				unsigned int *port_id_out)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VPORT_ALLOC_IN_LEN);
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_VPORT_ALLOC_OUT_LEN);
+	size_t outlen;
+	int rc;
+
+	EFX_WARN_ON_PARANOID(!port_id_out);
+
+	MCDI_SET_DWORD(inbuf, VPORT_ALLOC_IN_UPSTREAM_PORT_ID, port_id_in);
+	MCDI_SET_DWORD(inbuf, VPORT_ALLOC_IN_TYPE, vport_type);
+	MCDI_SET_DWORD(inbuf, VPORT_ALLOC_IN_NUM_VLAN_TAGS,
+		       (vlan != EFX_EF10_NO_VLAN));
+	MCDI_POPULATE_DWORD_1(inbuf, VPORT_ALLOC_IN_FLAGS,
+			      VPORT_ALLOC_IN_FLAG_AUTO_PORT, 0);
+	if (vlan != EFX_EF10_NO_VLAN)
+		MCDI_POPULATE_DWORD_1(inbuf, VPORT_ALLOC_IN_VLAN_TAGS,
+				      VPORT_ALLOC_IN_VLAN_TAG_0, vlan);
+
+	rc = efx_mcdi_rpc(efx, MC_CMD_VPORT_ALLOC, inbuf, sizeof(inbuf),
+			  outbuf, sizeof(outbuf), &outlen);
+	if (rc)
+		return rc;
+	if (outlen < MC_CMD_VPORT_ALLOC_OUT_LEN)
+		return -EIO;
+
+	*port_id_out = MCDI_DWORD(outbuf, VPORT_ALLOC_OUT_VPORT_ID);
+	return 0;
+}
+
+static int efx_ef10_vport_free(struct efx_nic *efx, unsigned int port_id)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VPORT_FREE_IN_LEN);
+
+	MCDI_SET_DWORD(inbuf, VPORT_FREE_IN_VPORT_ID, port_id);
+
+	return efx_mcdi_rpc(efx, MC_CMD_VPORT_FREE, inbuf, sizeof(inbuf),
+			    NULL, 0, NULL);
+}
+
+static int efx_ef10_vadaptor_alloc(struct efx_nic *efx, unsigned int port_id)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VADAPTOR_ALLOC_IN_LEN);
+
+	MCDI_SET_DWORD(inbuf, VADAPTOR_ALLOC_IN_UPSTREAM_PORT_ID, port_id);
+	return efx_mcdi_rpc(efx, MC_CMD_VADAPTOR_ALLOC, inbuf, sizeof(inbuf),
+			    NULL, 0, NULL);
+}
+
+static int efx_ef10_vadaptor_free(struct efx_nic *efx, unsigned int port_id)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VADAPTOR_FREE_IN_LEN);
+
+	MCDI_SET_DWORD(inbuf, VADAPTOR_FREE_IN_UPSTREAM_PORT_ID, port_id);
+	return efx_mcdi_rpc(efx, MC_CMD_VADAPTOR_FREE, inbuf, sizeof(inbuf),
+			    NULL, 0, NULL);
+}
+
+static void efx_ef10_sriov_free_vf_vports(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	int i;
+
+	if (!nic_data->vf)
+		return;
+
+	for (i = 0; i < efx->vf_count; i++) {
+		struct ef10_vf *vf = nic_data->vf + i;
+
+		/* If VF is assigned, do not free the vport  */
+		if (vf->pci_dev &&
+		    vf->pci_dev->dev_flags & PCI_DEV_FLAGS_ASSIGNED)
+			continue;
+
+		if (vf->vport_assigned) {
+			efx_ef10_evb_port_assign(efx, EVB_PORT_ID_NULL, i);
+			vf->vport_assigned = 0;
+		}
+
+		if (!is_zero_ether_addr(vf->mac)) {
+			efx_ef10_vport_del_mac(efx, vf->vport_id, vf->mac);
+			eth_zero_addr(vf->mac);
+		}
+
+		if (vf->vport_id) {
+			efx_ef10_vport_free(efx, vf->vport_id);
+			vf->vport_id = 0;
+		}
+
+		vf->efx = NULL;
+	}
+}
+
+static void efx_ef10_sriov_free_vf_vswitching(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+	efx_ef10_sriov_free_vf_vports(efx);
+	kfree(nic_data->vf);
+	nic_data->vf = NULL;
+}
+
+static int efx_ef10_sriov_assign_vf_vport(struct efx_nic *efx,
+					  unsigned int vf_i)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	struct ef10_vf *vf = nic_data->vf + vf_i;
+	int rc;
+
+	if (WARN_ON_ONCE(!nic_data->vf))
+		return -EOPNOTSUPP;
+
+	rc = efx_ef10_vport_alloc(efx, EVB_PORT_ID_ASSIGNED,
+				  MC_CMD_VPORT_ALLOC_IN_VPORT_TYPE_NORMAL,
+				  vf->vlan, &vf->vport_id);
+	if (rc)
+		return rc;
+
+	rc = efx_ef10_vport_add_mac(efx, vf->vport_id, vf->mac);
+	if (rc) {
+		eth_zero_addr(vf->mac);
+		return rc;
+	}
+
+	rc =  efx_ef10_evb_port_assign(efx, vf->vport_id, vf_i);
+	if (rc)
+		return rc;
+
+	vf->vport_assigned = 1;
+	return 0;
+}
+
+static int efx_ef10_sriov_alloc_vf_vswitching(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	unsigned int i;
+	int rc;
+
+	nic_data->vf = kcalloc(efx->vf_count, sizeof(struct ef10_vf),
+			       GFP_KERNEL);
+	if (!nic_data->vf)
+		return -ENOMEM;
+
+	for (i = 0; i < efx->vf_count; i++) {
+		random_ether_addr(nic_data->vf[i].mac);
+		nic_data->vf[i].efx = NULL;
+		nic_data->vf[i].vlan = EFX_EF10_NO_VLAN;
+
+		rc = efx_ef10_sriov_assign_vf_vport(efx, i);
+		if (rc)
+			goto fail;
+	}
+
+	return 0;
+fail:
+	efx_ef10_sriov_free_vf_vports(efx);
+	kfree(nic_data->vf);
+	nic_data->vf = NULL;
+	return rc;
+}
+
+static int efx_ef10_sriov_restore_vf_vswitching(struct efx_nic *efx)
+{
+	unsigned int i;
+	int rc;
+
+	for (i = 0; i < efx->vf_count; i++) {
+		rc = efx_ef10_sriov_assign_vf_vport(efx, i);
+		if (rc)
+			goto fail;
+	}
+
+	return 0;
+fail:
+	efx_ef10_sriov_free_vf_vswitching(efx);
+	return rc;
+}
+
+/* On top of the default firmware vswitch setup, create a VEB vswitch and
+ * expansion vport for use by this function.
+ */
+int efx_ef10_vswitching_probe_pf(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	struct net_device *net_dev = efx->net_dev;
+	int rc;
+
+	if (pci_sriov_get_totalvfs(efx->pci_dev) <= 0) {
+		/* vswitch not needed as we have no VFs */
+		efx_ef10_vadaptor_alloc(efx, nic_data->vport_id);
+		return 0;
+	}
+
+	rc = efx_ef10_vswitch_alloc(efx, EVB_PORT_ID_ASSIGNED,
+				    MC_CMD_VSWITCH_ALLOC_IN_VSWITCH_TYPE_VEB);
+	if (rc)
+		goto fail1;
+
+	rc = efx_ef10_vport_alloc(efx, EVB_PORT_ID_ASSIGNED,
+				  MC_CMD_VPORT_ALLOC_IN_VPORT_TYPE_NORMAL,
+				  EFX_EF10_NO_VLAN, &nic_data->vport_id);
+	if (rc)
+		goto fail2;
+
+	rc = efx_ef10_vport_add_mac(efx, nic_data->vport_id, net_dev->dev_addr);
+	if (rc)
+		goto fail3;
+	ether_addr_copy(nic_data->vport_mac, net_dev->dev_addr);
+
+	rc = efx_ef10_vadaptor_alloc(efx, nic_data->vport_id);
+	if (rc)
+		goto fail4;
+
+	return 0;
+fail4:
+	efx_ef10_vport_del_mac(efx, nic_data->vport_id, nic_data->vport_mac);
+	eth_zero_addr(nic_data->vport_mac);
+fail3:
+	efx_ef10_vport_free(efx, nic_data->vport_id);
+	nic_data->vport_id = EVB_PORT_ID_ASSIGNED;
+fail2:
+	efx_ef10_vswitch_free(efx, EVB_PORT_ID_ASSIGNED);
+fail1:
+	return rc;
+}
+
+int efx_ef10_vswitching_probe_vf(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+	return efx_ef10_vadaptor_alloc(efx, nic_data->vport_id);
+}
+
+int efx_ef10_vswitching_restore_pf(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	int rc;
+
+	if (!nic_data->must_probe_vswitching)
+		return 0;
+
+	rc = efx_ef10_vswitching_probe_pf(efx);
+	if (rc)
+		goto fail;
+
+	rc = efx_ef10_sriov_restore_vf_vswitching(efx);
+	if (rc)
+		goto fail;
+
+	nic_data->must_probe_vswitching = false;
+fail:
+	return rc;
+}
+
+int efx_ef10_vswitching_restore_vf(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	int rc;
+
+	if (!nic_data->must_probe_vswitching)
+		return 0;
+
+	rc = efx_ef10_vadaptor_free(efx, EVB_PORT_ID_ASSIGNED);
+	if (rc)
+		return rc;
+
+	nic_data->must_probe_vswitching = false;
+	return 0;
+}
+
+void efx_ef10_vswitching_remove_pf(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+	efx_ef10_sriov_free_vf_vswitching(efx);
+
+	efx_ef10_vadaptor_free(efx, nic_data->vport_id);
+
+	if (nic_data->vport_id == EVB_PORT_ID_ASSIGNED)
+		return; /* No vswitch was ever created */
+
+	if (!is_zero_ether_addr(nic_data->vport_mac)) {
+		efx_ef10_vport_del_mac(efx, nic_data->vport_id,
+				       efx->net_dev->dev_addr);
+		eth_zero_addr(nic_data->vport_mac);
+	}
+	efx_ef10_vport_free(efx, nic_data->vport_id);
+	nic_data->vport_id = EVB_PORT_ID_ASSIGNED;
+
+	/* Only free the vswitch if no VFs are assigned */
+	if (!pci_vfs_assigned(efx->pci_dev))
+		efx_ef10_vswitch_free(efx, nic_data->vport_id);
+}
+
+void efx_ef10_vswitching_remove_vf(struct efx_nic *efx)
+{
+	efx_ef10_vadaptor_free(efx, EVB_PORT_ID_ASSIGNED);
+}
+
+static int efx_ef10_pci_sriov_enable(struct efx_nic *efx, int num_vfs)
+{
+	int rc = 0;
+	struct pci_dev *dev = efx->pci_dev;
+
+	efx->vf_count = num_vfs;
+
+	rc = efx_ef10_sriov_alloc_vf_vswitching(efx);
+	if (rc)
+		goto fail1;
+
+	rc = pci_enable_sriov(dev, num_vfs);
+	if (rc)
+		goto fail2;
+
+	return 0;
+fail2:
+	efx_ef10_sriov_free_vf_vswitching(efx);
+fail1:
+	efx->vf_count = 0;
+	netif_err(efx, probe, efx->net_dev,
+		  "Failed to enable SRIOV VFs\n");
+	return rc;
+}
+
+static int efx_ef10_pci_sriov_disable(struct efx_nic *efx, bool force)
+{
+	struct pci_dev *dev = efx->pci_dev;
+	unsigned int vfs_assigned = 0;
+
+	vfs_assigned = pci_vfs_assigned(dev);
+
+	if (vfs_assigned && !force) {
+		netif_info(efx, drv, efx->net_dev, "VFs are assigned to guests; "
+			   "please detach them before disabling SR-IOV\n");
+		return -EBUSY;
+	}
+
+	if (!vfs_assigned)
+		pci_disable_sriov(dev);
+
+	efx_ef10_sriov_free_vf_vswitching(efx);
+	efx->vf_count = 0;
+	return 0;
+}
+
+int efx_ef10_sriov_configure(struct efx_nic *efx, int num_vfs)
+{
+	if (num_vfs == 0)
+		return efx_ef10_pci_sriov_disable(efx, false);
+	else
+		return efx_ef10_pci_sriov_enable(efx, num_vfs);
+}
+
+int efx_ef10_sriov_init(struct efx_nic *efx)
+{
+	return 0;
+}
+
+void efx_ef10_sriov_fini(struct efx_nic *efx)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	unsigned int i;
+	int rc;
+
+	if (!nic_data->vf) {
+		/* Remove any un-assigned orphaned VFs */
+		if (pci_num_vf(efx->pci_dev) && !pci_vfs_assigned(efx->pci_dev))
+			pci_disable_sriov(efx->pci_dev);
+		return;
+	}
+
+	/* Remove any VFs in the host */
+	for (i = 0; i < efx->vf_count; ++i) {
+		struct efx_nic *vf_efx = nic_data->vf[i].efx;
+
+		if (vf_efx)
+			vf_efx->pci_dev->driver->remove(vf_efx->pci_dev);
+	}
+
+	rc = efx_ef10_pci_sriov_disable(efx, true);
+	if (rc)
+		netif_dbg(efx, drv, efx->net_dev,
+			  "Disabling SRIOV was not successful rc=%d\n", rc);
+	else
+		netif_dbg(efx, drv, efx->net_dev, "SRIOV disabled\n");
+}
+
+static int efx_ef10_vport_del_vf_mac(struct efx_nic *efx, unsigned int port_id,
+				     u8 *mac)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_VPORT_DEL_MAC_ADDRESS_IN_LEN);
+	MCDI_DECLARE_BUF_ERR(outbuf);
+	size_t outlen;
+	int rc;
+
+	MCDI_SET_DWORD(inbuf, VPORT_DEL_MAC_ADDRESS_IN_VPORT_ID, port_id);
+	ether_addr_copy(MCDI_PTR(inbuf, VPORT_DEL_MAC_ADDRESS_IN_MACADDR), mac);
+
+	rc = efx_mcdi_rpc(efx, MC_CMD_VPORT_DEL_MAC_ADDRESS, inbuf,
+			  sizeof(inbuf), outbuf, sizeof(outbuf), &outlen);
+
+	return rc;
+}
+
+int efx_ef10_sriov_set_vf_mac(struct efx_nic *efx, int vf_i, u8 *mac)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	struct ef10_vf *vf;
+	int rc;
+
+	if (!nic_data->vf)
+		return -EOPNOTSUPP;
+
+	if (vf_i >= efx->vf_count)
+		return -EINVAL;
+	vf = nic_data->vf + vf_i;
+
+	if (vf->efx) {
+		efx_device_detach_sync(vf->efx);
+		efx_net_stop(vf->efx->net_dev);
+
+		down_write(&vf->efx->filter_sem);
+		vf->efx->type->filter_table_remove(vf->efx);
+
+		rc = efx_ef10_vadaptor_free(vf->efx, EVB_PORT_ID_ASSIGNED);
+		if (rc) {
+			up_write(&vf->efx->filter_sem);
+			return rc;
+		}
+	}
+
+	rc = efx_ef10_evb_port_assign(efx, EVB_PORT_ID_NULL, vf_i);
+	if (rc)
+		return rc;
+
+	if (!is_zero_ether_addr(vf->mac)) {
+		rc = efx_ef10_vport_del_vf_mac(efx, vf->vport_id, vf->mac);
+		if (rc)
+			return rc;
+	}
+
+	if (!is_zero_ether_addr(mac)) {
+		rc = efx_ef10_vport_add_mac(efx, vf->vport_id, mac);
+		if (rc) {
+			eth_zero_addr(vf->mac);
+			goto fail;
+		}
+		if (vf->efx)
+			ether_addr_copy(vf->efx->net_dev->dev_addr, mac);
+	}
+
+	ether_addr_copy(vf->mac, mac);
+
+	rc = efx_ef10_evb_port_assign(efx, vf->vport_id, vf_i);
+	if (rc)
+		goto fail;
+
+	if (vf->efx) {
+		/* VF cannot use the vport_id that the PF created */
+		rc = efx_ef10_vadaptor_alloc(vf->efx, EVB_PORT_ID_ASSIGNED);
+		if (rc) {
+			up_write(&vf->efx->filter_sem);
+			return rc;
+		}
+		vf->efx->type->filter_table_probe(vf->efx);
+		up_write(&vf->efx->filter_sem);
+		efx_net_open(vf->efx->net_dev);
+		netif_device_attach(vf->efx->net_dev);
+	}
+
+	return 0;
+
+fail:
+	memset(vf->mac, 0, ETH_ALEN);
+	return rc;
+}
+
+int efx_ef10_sriov_set_vf_vlan(struct efx_nic *efx, int vf_i, u16 vlan,
+			       u8 qos)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	struct ef10_vf *vf;
+	u16 old_vlan, new_vlan;
+	int rc = 0, rc2 = 0;
+
+	if (vf_i >= efx->vf_count)
+		return -EINVAL;
+	if (qos != 0)
+		return -EINVAL;
+
+	vf = nic_data->vf + vf_i;
+
+	new_vlan = (vlan == 0) ? EFX_EF10_NO_VLAN : vlan;
+	if (new_vlan == vf->vlan)
+		return 0;
+
+	if (vf->efx) {
+		efx_device_detach_sync(vf->efx);
+		efx_net_stop(vf->efx->net_dev);
+
+		down_write(&vf->efx->filter_sem);
+		vf->efx->type->filter_table_remove(vf->efx);
+
+		rc = efx_ef10_vadaptor_free(vf->efx, EVB_PORT_ID_ASSIGNED);
+		if (rc)
+			goto restore_filters;
+	}
+
+	if (vf->vport_assigned) {
+		rc = efx_ef10_evb_port_assign(efx, EVB_PORT_ID_NULL, vf_i);
+		if (rc) {
+			netif_warn(efx, drv, efx->net_dev,
+				   "Failed to change vlan on VF %d.\n", vf_i);
+			netif_warn(efx, drv, efx->net_dev,
+				   "This is likely because the VF is bound to a driver in a VM.\n");
+			netif_warn(efx, drv, efx->net_dev,
+				   "Please unload the driver in the VM.\n");
+			goto restore_vadaptor;
+		}
+		vf->vport_assigned = 0;
+	}
+
+	if (!is_zero_ether_addr(vf->mac)) {
+		rc = efx_ef10_vport_del_mac(efx, vf->vport_id, vf->mac);
+		if (rc)
+			goto restore_evb_port;
+	}
+
+	if (vf->vport_id) {
+		rc = efx_ef10_vport_free(efx, vf->vport_id);
+		if (rc)
+			goto restore_mac;
+		vf->vport_id = 0;
+	}
+
+	/* Do the actual vlan change */
+	old_vlan = vf->vlan;
+	vf->vlan = new_vlan;
+
+	/* Restore everything in reverse order */
+	rc = efx_ef10_vport_alloc(efx, EVB_PORT_ID_ASSIGNED,
+				  MC_CMD_VPORT_ALLOC_IN_VPORT_TYPE_NORMAL,
+				  vf->vlan, &vf->vport_id);
+	if (rc)
+		goto reset_nic;
+
+restore_mac:
+	if (!is_zero_ether_addr(vf->mac)) {
+		rc2 = efx_ef10_vport_add_mac(efx, vf->vport_id, vf->mac);
+		if (rc2) {
+			eth_zero_addr(vf->mac);
+			goto reset_nic;
+		}
+	}
+
+restore_evb_port:
+	rc2 = efx_ef10_evb_port_assign(efx, vf->vport_id, vf_i);
+	if (rc2)
+		goto reset_nic;
+	else
+		vf->vport_assigned = 1;
+
+restore_vadaptor:
+	if (vf->efx) {
+		rc2 = efx_ef10_vadaptor_alloc(vf->efx, EVB_PORT_ID_ASSIGNED);
+		if (rc2)
+			goto reset_nic;
+	}
+
+restore_filters:
+	if (vf->efx) {
+		rc2 = vf->efx->type->filter_table_probe(vf->efx);
+		if (rc2)
+			goto reset_nic;
+
+		up_write(&vf->efx->filter_sem);
+
+		rc2 = efx_net_open(vf->efx->net_dev);
+		if (rc2)
+			goto reset_nic;
+
+		netif_device_attach(vf->efx->net_dev);
+	}
+	return rc;
+
+reset_nic:
+	if (vf->efx) {
+		up_write(&vf->efx->filter_sem);
+		netif_err(efx, drv, efx->net_dev,
+			  "Failed to restore VF - scheduling reset.\n");
+		efx_schedule_reset(vf->efx, RESET_TYPE_DATAPATH);
+	} else {
+		netif_err(efx, drv, efx->net_dev,
+			  "Failed to restore the VF and cannot reset the VF "
+			  "- VF is not functional.\n");
+		netif_err(efx, drv, efx->net_dev,
+			  "Please reload the driver attached to the VF.\n");
+	}
+
+	return rc ? rc : rc2;
+}
+
+int efx_ef10_sriov_set_vf_spoofchk(struct efx_nic *efx, int vf_i,
+				   bool spoofchk)
+{
+	return spoofchk ? -EOPNOTSUPP : 0;
+}
+
+int efx_ef10_sriov_set_vf_link_state(struct efx_nic *efx, int vf_i,
+				     int link_state)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_LINK_STATE_MODE_IN_LEN);
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+	BUILD_BUG_ON(IFLA_VF_LINK_STATE_AUTO !=
+		     MC_CMD_LINK_STATE_MODE_IN_LINK_STATE_AUTO);
+	BUILD_BUG_ON(IFLA_VF_LINK_STATE_ENABLE !=
+		     MC_CMD_LINK_STATE_MODE_IN_LINK_STATE_UP);
+	BUILD_BUG_ON(IFLA_VF_LINK_STATE_DISABLE !=
+		     MC_CMD_LINK_STATE_MODE_IN_LINK_STATE_DOWN);
+	MCDI_POPULATE_DWORD_2(inbuf, LINK_STATE_MODE_IN_FUNCTION,
+			      LINK_STATE_MODE_IN_FUNCTION_PF,
+			      nic_data->pf_index,
+			      LINK_STATE_MODE_IN_FUNCTION_VF, vf_i);
+	MCDI_SET_DWORD(inbuf, LINK_STATE_MODE_IN_NEW_MODE, link_state);
+	return efx_mcdi_rpc(efx, MC_CMD_LINK_STATE_MODE, inbuf, sizeof(inbuf),
+			    NULL, 0, NULL); /* don't care what old mode was */
+}
+
+int efx_ef10_sriov_get_vf_config(struct efx_nic *efx, int vf_i,
+				 struct ifla_vf_info *ivf)
+{
+	MCDI_DECLARE_BUF(inbuf, MC_CMD_LINK_STATE_MODE_IN_LEN);
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_LINK_STATE_MODE_OUT_LEN);
+
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+	struct ef10_vf *vf;
+	size_t outlen;
+	int rc;
+
+	if (vf_i >= efx->vf_count)
+		return -EINVAL;
+
+	if (!nic_data->vf)
+		return -EOPNOTSUPP;
+
+	vf = nic_data->vf + vf_i;
+
+	ivf->vf = vf_i;
+	ivf->min_tx_rate = 0;
+	ivf->max_tx_rate = 0;
+	ether_addr_copy(ivf->mac, vf->mac);
+	ivf->vlan = (vf->vlan == EFX_EF10_NO_VLAN) ? 0 : vf->vlan;
+	ivf->qos = 0;
+
+	MCDI_POPULATE_DWORD_2(inbuf, LINK_STATE_MODE_IN_FUNCTION,
+			      LINK_STATE_MODE_IN_FUNCTION_PF,
+			      nic_data->pf_index,
+			      LINK_STATE_MODE_IN_FUNCTION_VF, vf_i);
+	MCDI_SET_DWORD(inbuf, LINK_STATE_MODE_IN_NEW_MODE,
+		       MC_CMD_LINK_STATE_MODE_IN_DO_NOT_CHANGE);
+	rc = efx_mcdi_rpc(efx, MC_CMD_LINK_STATE_MODE, inbuf, sizeof(inbuf),
+			  outbuf, sizeof(outbuf), &outlen);
+	if (rc)
+		return rc;
+	if (outlen < MC_CMD_LINK_STATE_MODE_OUT_LEN)
+		return -EIO;
+	ivf->linkstate = MCDI_DWORD(outbuf, LINK_STATE_MODE_OUT_OLD_MODE);
+
+	return 0;
+}
+
+int efx_ef10_sriov_get_phys_port_id(struct efx_nic *efx,
+				    struct netdev_phys_item_id *ppid)
+{
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+	if (!is_valid_ether_addr(nic_data->port_id))
+		return -EOPNOTSUPP;
+
+	ppid->id_len = ETH_ALEN;
+	memcpy(ppid->id, nic_data->port_id, ppid->id_len);
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/sfc/ef10_sriov.h b/drivers/net/ethernet/sfc/ef10_sriov.h
new file mode 100644
index 000000000000..db4ef537c610
--- /dev/null
+++ b/drivers/net/ethernet/sfc/ef10_sriov.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ * Driver for Solarflare network controllers and boards
+ * Copyright 2015 Solarflare Communications 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, incorporated herein by reference.
+ */
+
+#ifndef EF10_SRIOV_H
+#define EF10_SRIOV_H
+
+#include "net_driver.h"
+
+/**
+ * struct ef10_vf - PF's store of VF data
+ * @efx: efx_nic struct for the current VF
+ * @pci_dev: the pci_dev struct for the VF, retained while the VF is assigned
+ * @vport_id: vport ID for the VF
+ * @vport_assigned: record whether the vport is currently assigned to the VF
+ * @mac: MAC address for the VF, zero when address is removed from the vport
+ * @vlan: Default VLAN for the VF or #EFX_EF10_NO_VLAN
+ */
+struct ef10_vf {
+	struct efx_nic *efx;
+	struct pci_dev *pci_dev;
+	unsigned int vport_id;
+	unsigned int vport_assigned;
+	u8 mac[ETH_ALEN];
+	u16 vlan;
+#define EFX_EF10_NO_VLAN       0
+};
+
+static inline bool efx_ef10_sriov_wanted(struct efx_nic *efx)
+{
+	return false;
+}
+
+int efx_ef10_sriov_configure(struct efx_nic *efx, int num_vfs);
+int efx_ef10_sriov_init(struct efx_nic *efx);
+static inline void efx_ef10_sriov_reset(struct efx_nic *efx) {}
+void efx_ef10_sriov_fini(struct efx_nic *efx);
+static inline void efx_ef10_sriov_flr(struct efx_nic *efx, unsigned vf_i) {}
+
+int efx_ef10_sriov_set_vf_mac(struct efx_nic *efx, int vf, u8 *mac);
+
+int efx_ef10_sriov_set_vf_vlan(struct efx_nic *efx, int vf_i,
+			       u16 vlan, u8 qos);
+
+int efx_ef10_sriov_set_vf_spoofchk(struct efx_nic *efx, int vf,
+				   bool spoofchk);
+
+int efx_ef10_sriov_get_vf_config(struct efx_nic *efx, int vf_i,
+				 struct ifla_vf_info *ivf);
+
+int efx_ef10_sriov_set_vf_link_state(struct efx_nic *efx, int vf_i,
+				     int link_state);
+
+int efx_ef10_sriov_get_phys_port_id(struct efx_nic *efx,
+				    struct netdev_phys_item_id *ppid);
+
+int efx_ef10_vswitching_probe_pf(struct efx_nic *efx);
+int efx_ef10_vswitching_probe_vf(struct efx_nic *efx);
+int efx_ef10_vswitching_restore_pf(struct efx_nic *efx);
+int efx_ef10_vswitching_restore_vf(struct efx_nic *efx);
+void efx_ef10_vswitching_remove_pf(struct efx_nic *efx);
+void efx_ef10_vswitching_remove_vf(struct efx_nic *efx);
+
+#endif /* EF10_SRIOV_H */
diff --git a/drivers/net/ethernet/sfc/efx.c b/drivers/net/ethernet/sfc/efx.c
index 4b00545a3ace..804b9ad553d3 100644
--- a/drivers/net/ethernet/sfc/efx.c
+++ b/drivers/net/ethernet/sfc/efx.c
@@ -26,6 +26,7 @@
 #include "efx.h"
 #include "nic.h"
 #include "selftest.h"
+#include "sriov.h"
 
 #include "mcdi.h"
 #include "workarounds.h"
@@ -76,6 +77,7 @@ const char *const efx_reset_type_names[] = {
 	[RESET_TYPE_RECOVER_OR_ALL]     = "RECOVER_OR_ALL",
 	[RESET_TYPE_WORLD]              = "WORLD",
 	[RESET_TYPE_RECOVER_OR_DISABLE] = "RECOVER_OR_DISABLE",
+	[RESET_TYPE_DATAPATH]           = "DATAPATH",
 	[RESET_TYPE_MC_BIST]		= "MC_BIST",
 	[RESET_TYPE_DISABLE]            = "DISABLE",
 	[RESET_TYPE_TX_WATCHDOG]        = "TX_WATCHDOG",
@@ -948,6 +950,16 @@ void efx_link_set_wanted_fc(struct efx_nic *efx, u8 wanted_fc)
 
 static void efx_fini_port(struct efx_nic *efx);
 
+/* We assume that efx->type->reconfigure_mac will always try to sync RX
+ * filters and therefore needs to read-lock the filter table against freeing
+ */
+void efx_mac_reconfigure(struct efx_nic *efx)
+{
+	down_read(&efx->filter_sem);
+	efx->type->reconfigure_mac(efx);
+	up_read(&efx->filter_sem);
+}
+
 /* Push loopback/power/transmit disable settings to the PHY, and reconfigure
  * the MAC appropriately. All other PHY configuration changes are pushed
  * through phy_op->set_settings(), and pushed asynchronously to the MAC
@@ -1001,7 +1013,7 @@ static void efx_mac_work(struct work_struct *data)
 
 	mutex_lock(&efx->mac_lock);
 	if (efx->port_enabled)
-		efx->type->reconfigure_mac(efx);
+		efx_mac_reconfigure(efx);
 	mutex_unlock(&efx->mac_lock);
 }
 
@@ -1041,11 +1053,11 @@ static int efx_init_port(struct efx_nic *efx)
 
 	/* Reconfigure the MAC before creating dma queues (required for
 	 * Falcon/A1 where RX_INGR_EN/TX_DRAIN_EN isn't supported) */
-	efx->type->reconfigure_mac(efx);
+	efx_mac_reconfigure(efx);
 
 	/* Ensure the PHY advertises the correct flow control settings */
 	rc = efx->phy_op->reconfigure(efx);
-	if (rc)
+	if (rc && rc != -EPERM)
 		goto fail2;
 
 	mutex_unlock(&efx->mac_lock);
@@ -1067,7 +1079,7 @@ static void efx_start_port(struct efx_nic *efx)
 	efx->port_enabled = true;
 
 	/* Ensure MAC ingress/egress is enabled */
-	efx->type->reconfigure_mac(efx);
+	efx_mac_reconfigure(efx);
 
 	mutex_unlock(&efx->mac_lock);
 }
@@ -1200,10 +1212,12 @@ static int efx_init_io(struct efx_nic *efx)
 	struct pci_dev *pci_dev = efx->pci_dev;
 	dma_addr_t dma_mask = efx->type->max_dma_mask;
 	unsigned int mem_map_size = efx->type->mem_map_size(efx);
-	int rc;
+	int rc, bar;
 
 	netif_dbg(efx, probe, efx->net_dev, "initialising I/O\n");
 
+	bar = efx->type->mem_bar;
+
 	rc = pci_enable_device(pci_dev);
 	if (rc) {
 		netif_err(efx, probe, efx->net_dev,
@@ -1234,8 +1248,8 @@ static int efx_init_io(struct efx_nic *efx)
 	netif_dbg(efx, probe, efx->net_dev,
 		  "using DMA mask %llx\n", (unsigned long long) dma_mask);
 
-	efx->membase_phys = pci_resource_start(efx->pci_dev, EFX_MEM_BAR);
-	rc = pci_request_region(pci_dev, EFX_MEM_BAR, "sfc");
+	efx->membase_phys = pci_resource_start(efx->pci_dev, bar);
+	rc = pci_request_region(pci_dev, bar, "sfc");
 	if (rc) {
 		netif_err(efx, probe, efx->net_dev,
 			  "request for memory BAR failed\n");
@@ -1258,7 +1272,7 @@ static int efx_init_io(struct efx_nic *efx)
 	return 0;
 
  fail4:
-	pci_release_region(efx->pci_dev, EFX_MEM_BAR);
+	pci_release_region(efx->pci_dev, bar);
  fail3:
 	efx->membase_phys = 0;
  fail2:
@@ -1269,6 +1283,8 @@ static int efx_init_io(struct efx_nic *efx)
 
 static void efx_fini_io(struct efx_nic *efx)
 {
+	int bar;
+
 	netif_dbg(efx, drv, efx->net_dev, "shutting down I/O\n");
 
 	if (efx->membase) {
@@ -1277,11 +1293,23 @@ static void efx_fini_io(struct efx_nic *efx)
 	}
 
 	if (efx->membase_phys) {
-		pci_release_region(efx->pci_dev, EFX_MEM_BAR);
+		bar = efx->type->mem_bar;
+		pci_release_region(efx->pci_dev, bar);
 		efx->membase_phys = 0;
 	}
 
-	pci_disable_device(efx->pci_dev);
+	/* Don't disable bus-mastering if VFs are assigned */
+	if (!pci_vfs_assigned(efx->pci_dev))
+		pci_disable_device(efx->pci_dev);
+}
+
+void efx_set_default_rx_indir_table(struct efx_nic *efx)
+{
+	size_t i;
+
+	for (i = 0; i < ARRAY_SIZE(efx->rx_indir_table); i++)
+		efx->rx_indir_table[i] =
+			ethtool_rxfh_indir_default(i, efx->rss_spread);
 }
 
 static unsigned int efx_wanted_parallelism(struct efx_nic *efx)
@@ -1304,7 +1332,7 @@ static unsigned int efx_wanted_parallelism(struct efx_nic *efx)
 			if (!cpumask_test_cpu(cpu, thread_mask)) {
 				++count;
 				cpumask_or(thread_mask, thread_mask,
-					   topology_thread_cpumask(cpu));
+					   topology_sibling_cpumask(cpu));
 			}
 		}
 
@@ -1314,15 +1342,19 @@ static unsigned int efx_wanted_parallelism(struct efx_nic *efx)
 	/* If RSS is requested for the PF *and* VFs then we can't write RSS
 	 * table entries that are inaccessible to VFs
 	 */
-	if (efx->type->sriov_wanted(efx) && efx_vf_size(efx) > 1 &&
-	    count > efx_vf_size(efx)) {
-		netif_warn(efx, probe, efx->net_dev,
-			   "Reducing number of RSS channels from %u to %u for "
-			   "VF support. Increase vf-msix-limit to use more "
-			   "channels on the PF.\n",
-			   count, efx_vf_size(efx));
-		count = efx_vf_size(efx);
+#ifdef CONFIG_SFC_SRIOV
+	if (efx->type->sriov_wanted) {
+		if (efx->type->sriov_wanted(efx) && efx_vf_size(efx) > 1 &&
+		    count > efx_vf_size(efx)) {
+			netif_warn(efx, probe, efx->net_dev,
+				   "Reducing number of RSS channels from %u to %u for "
+				   "VF support. Increase vf-msix-limit to use more "
+				   "channels on the PF.\n",
+				   count, efx_vf_size(efx));
+			count = efx_vf_size(efx);
+		}
 	}
+#endif
 
 	return count;
 }
@@ -1426,10 +1458,15 @@ static int efx_probe_interrupts(struct efx_nic *efx)
 	}
 
 	/* RSS might be usable on VFs even if it is disabled on the PF */
-
-	efx->rss_spread = ((efx->n_rx_channels > 1 ||
-			    !efx->type->sriov_wanted(efx)) ?
-			   efx->n_rx_channels : efx_vf_size(efx));
+#ifdef CONFIG_SFC_SRIOV
+	if (efx->type->sriov_wanted) {
+		efx->rss_spread = ((efx->n_rx_channels > 1 ||
+				    !efx->type->sriov_wanted(efx)) ?
+				   efx->n_rx_channels : efx_vf_size(efx));
+		return 0;
+	}
+#endif
+	efx->rss_spread = efx->n_rx_channels;
 
 	return 0;
 }
@@ -1593,7 +1630,6 @@ static void efx_set_channels(struct efx_nic *efx)
 
 static int efx_probe_nic(struct efx_nic *efx)
 {
-	size_t i;
 	int rc;
 
 	netif_dbg(efx, probe, efx->net_dev, "creating NIC\n");
@@ -1616,10 +1652,9 @@ static int efx_probe_nic(struct efx_nic *efx)
 		goto fail2;
 
 	if (efx->n_channels > 1)
-		netdev_rss_key_fill(&efx->rx_hash_key, sizeof(efx->rx_hash_key));
-	for (i = 0; i < ARRAY_SIZE(efx->rx_indir_table); i++)
-		efx->rx_indir_table[i] =
-			ethtool_rxfh_indir_default(i, efx->rss_spread);
+		netdev_rss_key_fill(&efx->rx_hash_key,
+				    sizeof(efx->rx_hash_key));
+	efx_set_default_rx_indir_table(efx);
 
 	netif_set_real_num_tx_queues(efx->net_dev, efx->n_tx_channels);
 	netif_set_real_num_rx_queues(efx->net_dev, efx->n_rx_channels);
@@ -1650,10 +1685,11 @@ static int efx_probe_filters(struct efx_nic *efx)
 	int rc;
 
 	spin_lock_init(&efx->filter_lock);
-
+	init_rwsem(&efx->filter_sem);
+	down_write(&efx->filter_sem);
 	rc = efx->type->filter_table_probe(efx);
 	if (rc)
-		return rc;
+		goto out_unlock;
 
 #ifdef CONFIG_RFS_ACCEL
 	if (efx->type->offload_features & NETIF_F_NTUPLE) {
@@ -1662,12 +1698,14 @@ static int efx_probe_filters(struct efx_nic *efx)
 					   GFP_KERNEL);
 		if (!efx->rps_flow_id) {
 			efx->type->filter_table_remove(efx);
-			return -ENOMEM;
+			rc = -ENOMEM;
+			goto out_unlock;
 		}
 	}
 #endif
-
-	return 0;
+out_unlock:
+	up_write(&efx->filter_sem);
+	return rc;
 }
 
 static void efx_remove_filters(struct efx_nic *efx)
@@ -1675,12 +1713,16 @@ static void efx_remove_filters(struct efx_nic *efx)
 #ifdef CONFIG_RFS_ACCEL
 	kfree(efx->rps_flow_id);
 #endif
+	down_write(&efx->filter_sem);
 	efx->type->filter_table_remove(efx);
+	up_write(&efx->filter_sem);
 }
 
 static void efx_restore_filters(struct efx_nic *efx)
 {
+	down_read(&efx->filter_sem);
 	efx->type->filter_table_restore(efx);
+	up_read(&efx->filter_sem);
 }
 
 /**************************************************************************
@@ -1712,21 +1754,33 @@ static int efx_probe_all(struct efx_nic *efx)
 	}
 	efx->rxq_entries = efx->txq_entries = EFX_DEFAULT_DMAQ_SIZE;
 
+#ifdef CONFIG_SFC_SRIOV
+	rc = efx->type->vswitching_probe(efx);
+	if (rc) /* not fatal; the PF will still work fine */
+		netif_warn(efx, probe, efx->net_dev,
+			   "failed to setup vswitching rc=%d;"
+			   " VFs may not function\n", rc);
+#endif
+
 	rc = efx_probe_filters(efx);
 	if (rc) {
 		netif_err(efx, probe, efx->net_dev,
 			  "failed to create filter tables\n");
-		goto fail3;
+		goto fail4;
 	}
 
 	rc = efx_probe_channels(efx);
 	if (rc)
-		goto fail4;
+		goto fail5;
 
 	return 0;
 
- fail4:
+ fail5:
 	efx_remove_filters(efx);
+ fail4:
+#ifdef CONFIG_SFC_SRIOV
+	efx->type->vswitching_remove(efx);
+#endif
  fail3:
 	efx_remove_port(efx);
  fail2:
@@ -1816,6 +1870,9 @@ static void efx_remove_all(struct efx_nic *efx)
 {
 	efx_remove_channels(efx);
 	efx_remove_filters(efx);
+#ifdef CONFIG_SFC_SRIOV
+	efx->type->vswitching_remove(efx);
+#endif
 	efx_remove_port(efx);
 	efx_remove_nic(efx);
 }
@@ -2059,7 +2116,7 @@ static int efx_busy_poll(struct napi_struct *napi)
  *************************************************************************/
 
 /* Context: process, rtnl_lock() held. */
-static int efx_net_open(struct net_device *net_dev)
+int efx_net_open(struct net_device *net_dev)
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
 	int rc;
@@ -2088,7 +2145,7 @@ static int efx_net_open(struct net_device *net_dev)
  * Note that the kernel will ignore our return code; this method
  * should really be a void.
  */
-static int efx_net_stop(struct net_device *net_dev)
+int efx_net_stop(struct net_device *net_dev)
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
 
@@ -2146,7 +2203,7 @@ static int efx_change_mtu(struct net_device *net_dev, int new_mtu)
 
 	mutex_lock(&efx->mac_lock);
 	net_dev->mtu = new_mtu;
-	efx->type->reconfigure_mac(efx);
+	efx_mac_reconfigure(efx);
 	mutex_unlock(&efx->mac_lock);
 
 	efx_start_all(efx);
@@ -2159,6 +2216,8 @@ static int efx_set_mac_address(struct net_device *net_dev, void *data)
 	struct efx_nic *efx = netdev_priv(net_dev);
 	struct sockaddr *addr = data;
 	u8 *new_addr = addr->sa_data;
+	u8 old_addr[6];
+	int rc;
 
 	if (!is_valid_ether_addr(new_addr)) {
 		netif_err(efx, drv, efx->net_dev,
@@ -2167,12 +2226,20 @@ static int efx_set_mac_address(struct net_device *net_dev, void *data)
 		return -EADDRNOTAVAIL;
 	}
 
+	/* save old address */
+	ether_addr_copy(old_addr, net_dev->dev_addr);
 	ether_addr_copy(net_dev->dev_addr, new_addr);
-	efx->type->sriov_mac_address_changed(efx);
+	if (efx->type->set_mac_address) {
+		rc = efx->type->set_mac_address(efx);
+		if (rc) {
+			ether_addr_copy(net_dev->dev_addr, old_addr);
+			return rc;
+		}
+	}
 
 	/* Reconfigure the MAC */
 	mutex_lock(&efx->mac_lock);
-	efx->type->reconfigure_mac(efx);
+	efx_mac_reconfigure(efx);
 	mutex_unlock(&efx->mac_lock);
 
 	return 0;
@@ -2199,7 +2266,7 @@ static int efx_set_features(struct net_device *net_dev, netdev_features_t data)
 	return 0;
 }
 
-static const struct net_device_ops efx_farch_netdev_ops = {
+static const struct net_device_ops efx_netdev_ops = {
 	.ndo_open		= efx_net_open,
 	.ndo_stop		= efx_net_stop,
 	.ndo_get_stats64	= efx_net_stats,
@@ -2212,10 +2279,12 @@ static const struct net_device_ops efx_farch_netdev_ops = {
 	.ndo_set_rx_mode	= efx_set_rx_mode,
 	.ndo_set_features	= efx_set_features,
 #ifdef CONFIG_SFC_SRIOV
-	.ndo_set_vf_mac		= efx_siena_sriov_set_vf_mac,
-	.ndo_set_vf_vlan	= efx_siena_sriov_set_vf_vlan,
-	.ndo_set_vf_spoofchk	= efx_siena_sriov_set_vf_spoofchk,
-	.ndo_get_vf_config	= efx_siena_sriov_get_vf_config,
+	.ndo_set_vf_mac		= efx_sriov_set_vf_mac,
+	.ndo_set_vf_vlan	= efx_sriov_set_vf_vlan,
+	.ndo_set_vf_spoofchk	= efx_sriov_set_vf_spoofchk,
+	.ndo_get_vf_config	= efx_sriov_get_vf_config,
+	.ndo_set_vf_link_state  = efx_sriov_set_vf_link_state,
+	.ndo_get_phys_port_id   = efx_sriov_get_phys_port_id,
 #endif
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller = efx_netpoll,
@@ -2229,29 +2298,6 @@ static const struct net_device_ops efx_farch_netdev_ops = {
 #endif
 };
 
-static const struct net_device_ops efx_ef10_netdev_ops = {
-	.ndo_open		= efx_net_open,
-	.ndo_stop		= efx_net_stop,
-	.ndo_get_stats64	= efx_net_stats,
-	.ndo_tx_timeout		= efx_watchdog,
-	.ndo_start_xmit		= efx_hard_start_xmit,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_do_ioctl		= efx_ioctl,
-	.ndo_change_mtu		= efx_change_mtu,
-	.ndo_set_mac_address	= efx_set_mac_address,
-	.ndo_set_rx_mode	= efx_set_rx_mode,
-	.ndo_set_features	= efx_set_features,
-#ifdef CONFIG_NET_POLL_CONTROLLER
-	.ndo_poll_controller	= efx_netpoll,
-#endif
-#ifdef CONFIG_NET_RX_BUSY_POLL
-	.ndo_busy_poll		= efx_busy_poll,
-#endif
-#ifdef CONFIG_RFS_ACCEL
-	.ndo_rx_flow_steer	= efx_filter_rfs,
-#endif
-};
-
 static void efx_update_name(struct efx_nic *efx)
 {
 	strcpy(efx->name, efx->net_dev->name);
@@ -2264,8 +2310,7 @@ static int efx_netdev_event(struct notifier_block *this,
 {
 	struct net_device *net_dev = netdev_notifier_info_to_dev(ptr);
 
-	if ((net_dev->netdev_ops == &efx_farch_netdev_ops ||
-	     net_dev->netdev_ops == &efx_ef10_netdev_ops) &&
+	if ((net_dev->netdev_ops == &efx_netdev_ops) &&
 	    event == NETDEV_CHANGENAME)
 		efx_update_name(netdev_priv(net_dev));
 
@@ -2284,6 +2329,28 @@ show_phy_type(struct device *dev, struct device_attribute *attr, char *buf)
 }
 static DEVICE_ATTR(phy_type, 0444, show_phy_type, NULL);
 
+#ifdef CONFIG_SFC_MCDI_LOGGING
+static ssize_t show_mcdi_log(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct efx_nic *efx = pci_get_drvdata(to_pci_dev(dev));
+	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", mcdi->logging_enabled);
+}
+static ssize_t set_mcdi_log(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct efx_nic *efx = pci_get_drvdata(to_pci_dev(dev));
+	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
+	bool enable = count > 0 && *buf != '0';
+
+	mcdi->logging_enabled = enable;
+	return count;
+}
+static DEVICE_ATTR(mcdi_logging, 0644, show_mcdi_log, set_mcdi_log);
+#endif
+
 static int efx_register_netdev(struct efx_nic *efx)
 {
 	struct net_device *net_dev = efx->net_dev;
@@ -2292,12 +2359,9 @@ static int efx_register_netdev(struct efx_nic *efx)
 
 	net_dev->watchdog_timeo = 5 * HZ;
 	net_dev->irq = efx->pci_dev->irq;
-	if (efx_nic_rev(efx) >= EFX_REV_HUNT_A0) {
-		net_dev->netdev_ops = &efx_ef10_netdev_ops;
+	net_dev->netdev_ops = &efx_netdev_ops;
+	if (efx_nic_rev(efx) >= EFX_REV_HUNT_A0)
 		net_dev->priv_flags |= IFF_UNICAST_FLT;
-	} else {
-		net_dev->netdev_ops = &efx_farch_netdev_ops;
-	}
 	net_dev->ethtool_ops = &efx_ethtool_ops;
 	net_dev->gso_max_segs = EFX_TSO_MAX_SEGS;
 
@@ -2344,9 +2408,21 @@ static int efx_register_netdev(struct efx_nic *efx)
 			  "failed to init net dev attributes\n");
 		goto fail_registered;
 	}
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	rc = device_create_file(&efx->pci_dev->dev, &dev_attr_mcdi_logging);
+	if (rc) {
+		netif_err(efx, drv, efx->net_dev,
+			  "failed to init net dev attributes\n");
+		goto fail_attr_mcdi_logging;
+	}
+#endif
 
 	return 0;
 
+#ifdef CONFIG_SFC_MCDI_LOGGING
+fail_attr_mcdi_logging:
+	device_remove_file(&efx->pci_dev->dev, &dev_attr_phy_type);
+#endif
 fail_registered:
 	rtnl_lock();
 	efx_dissociate(efx);
@@ -2365,13 +2441,14 @@ static void efx_unregister_netdev(struct efx_nic *efx)
 
 	BUG_ON(netdev_priv(efx->net_dev) != efx);
 
-	strlcpy(efx->name, pci_name(efx->pci_dev), sizeof(efx->name));
-	device_remove_file(&efx->pci_dev->dev, &dev_attr_phy_type);
-
-	rtnl_lock();
-	unregister_netdevice(efx->net_dev);
-	efx->state = STATE_UNINIT;
-	rtnl_unlock();
+	if (efx_dev_registered(efx)) {
+		strlcpy(efx->name, pci_name(efx->pci_dev), sizeof(efx->name));
+#ifdef CONFIG_SFC_MCDI_LOGGING
+		device_remove_file(&efx->pci_dev->dev, &dev_attr_mcdi_logging);
+#endif
+		device_remove_file(&efx->pci_dev->dev, &dev_attr_phy_type);
+		unregister_netdev(efx->net_dev);
+	}
 }
 
 /**************************************************************************
@@ -2393,7 +2470,8 @@ void efx_reset_down(struct efx_nic *efx, enum reset_type method)
 	efx_disable_interrupts(efx);
 
 	mutex_lock(&efx->mac_lock);
-	if (efx->port_initialized && method != RESET_TYPE_INVISIBLE)
+	if (efx->port_initialized && method != RESET_TYPE_INVISIBLE &&
+	    method != RESET_TYPE_DATAPATH)
 		efx->phy_op->fini(efx);
 	efx->type->fini(efx);
 }
@@ -2422,11 +2500,13 @@ int efx_reset_up(struct efx_nic *efx, enum reset_type method, bool ok)
 	if (!ok)
 		goto fail;
 
-	if (efx->port_initialized && method != RESET_TYPE_INVISIBLE) {
+	if (efx->port_initialized && method != RESET_TYPE_INVISIBLE &&
+	    method != RESET_TYPE_DATAPATH) {
 		rc = efx->phy_op->init(efx);
 		if (rc)
 			goto fail;
-		if (efx->phy_op->reconfigure(efx))
+		rc = efx->phy_op->reconfigure(efx);
+		if (rc && rc != -EPERM)
 			netif_err(efx, drv, efx->net_dev,
 				  "could not restore PHY settings\n");
 	}
@@ -2434,8 +2514,20 @@ int efx_reset_up(struct efx_nic *efx, enum reset_type method, bool ok)
 	rc = efx_enable_interrupts(efx);
 	if (rc)
 		goto fail;
+
+#ifdef CONFIG_SFC_SRIOV
+	rc = efx->type->vswitching_restore(efx);
+	if (rc) /* not fatal; the PF will still work fine */
+		netif_warn(efx, probe, efx->net_dev,
+			   "failed to restore vswitching rc=%d;"
+			   " VFs may not function\n", rc);
+#endif
+
+	down_read(&efx->filter_sem);
 	efx_restore_filters(efx);
-	efx->type->sriov_reset(efx);
+	up_read(&efx->filter_sem);
+	if (efx->type->sriov_reset)
+		efx->type->sriov_reset(efx);
 
 	mutex_unlock(&efx->mac_lock);
 
@@ -2605,6 +2697,7 @@ void efx_schedule_reset(struct efx_nic *efx, enum reset_type type)
 	case RESET_TYPE_WORLD:
 	case RESET_TYPE_DISABLE:
 	case RESET_TYPE_RECOVER_OR_DISABLE:
+	case RESET_TYPE_DATAPATH:
 	case RESET_TYPE_MC_BIST:
 	case RESET_TYPE_MCDI_TIMEOUT:
 		method = type;
@@ -2655,6 +2748,8 @@ static const struct pci_device_id efx_pci_table[] = {
 	 .driver_data = (unsigned long) &siena_a0_nic_type},
 	{PCI_DEVICE(PCI_VENDOR_ID_SOLARFLARE, 0x0903),  /* SFC9120 PF */
 	 .driver_data = (unsigned long) &efx_hunt_a0_nic_type},
+	{PCI_DEVICE(PCI_VENDOR_ID_SOLARFLARE, 0x1903),  /* SFC9120 VF */
+	 .driver_data = (unsigned long) &efx_hunt_a0_vf_nic_type},
 	{PCI_DEVICE(PCI_VENDOR_ID_SOLARFLARE, 0x0923),  /* SFC9140 PF */
 	 .driver_data = (unsigned long) &efx_hunt_a0_nic_type},
 	{0}			/* end of list */
@@ -2809,7 +2904,8 @@ static void efx_pci_remove_main(struct efx_nic *efx)
 }
 
 /* Final NIC shutdown
- * This is called only at module unload (or hotplug removal).
+ * This is called only at module unload (or hotplug removal).  A PF can call
+ * this on its VFs to ensure they are unbound first.
  */
 static void efx_pci_remove(struct pci_dev *pci_dev)
 {
@@ -2824,9 +2920,12 @@ static void efx_pci_remove(struct pci_dev *pci_dev)
 	efx_dissociate(efx);
 	dev_close(efx->net_dev);
 	efx_disable_interrupts(efx);
+	efx->state = STATE_UNINIT;
 	rtnl_unlock();
 
-	efx->type->sriov_fini(efx);
+	if (efx->type->sriov_fini)
+		efx->type->sriov_fini(efx);
+
 	efx_unregister_netdev(efx);
 
 	efx_mtd_remove(efx);
@@ -3008,7 +3107,8 @@ static int efx_pci_probe(struct pci_dev *pci_dev,
 	netif_info(efx, probe, efx->net_dev,
 		   "Solarflare NIC detected\n");
 
-	efx_probe_vpd_strings(efx);
+	if (!efx->type->is_vf)
+		efx_probe_vpd_strings(efx);
 
 	/* Set up basic I/O (BAR mappings etc) */
 	rc = efx_init_io(efx);
@@ -3023,10 +3123,12 @@ static int efx_pci_probe(struct pci_dev *pci_dev,
 	if (rc)
 		goto fail4;
 
-	rc = efx->type->sriov_init(efx);
-	if (rc)
-		netif_err(efx, probe, efx->net_dev,
-			  "SR-IOV can't be enabled rc %d\n", rc);
+	if (efx->type->sriov_init) {
+		rc = efx->type->sriov_init(efx);
+		if (rc)
+			netif_err(efx, probe, efx->net_dev,
+				  "SR-IOV can't be enabled rc %d\n", rc);
+	}
 
 	netif_dbg(efx, probe, efx->net_dev, "initialisation successful\n");
 
@@ -3058,6 +3160,26 @@ static int efx_pci_probe(struct pci_dev *pci_dev,
 	return rc;
 }
 
+/* efx_pci_sriov_configure returns the actual number of Virtual Functions
+ * enabled on success
+ */
+#ifdef CONFIG_SFC_SRIOV
+static int efx_pci_sriov_configure(struct pci_dev *dev, int num_vfs)
+{
+	int rc;
+	struct efx_nic *efx = pci_get_drvdata(dev);
+
+	if (efx->type->sriov_configure) {
+		rc = efx->type->sriov_configure(efx, num_vfs);
+		if (rc)
+			return rc;
+		else
+			return num_vfs;
+	} else
+		return -EOPNOTSUPP;
+}
+#endif
+
 static int efx_pm_freeze(struct device *dev)
 {
 	struct efx_nic *efx = pci_get_drvdata(to_pci_dev(dev));
@@ -3280,6 +3402,9 @@ static struct pci_driver efx_pci_driver = {
 	.remove		= efx_pci_remove,
 	.driver.pm	= &efx_pm_ops,
 	.err_handler	= &efx_err_handlers,
+#ifdef CONFIG_SFC_SRIOV
+	.sriov_configure = efx_pci_sriov_configure,
+#endif
 };
 
 /**************************************************************************
@@ -3302,9 +3427,11 @@ static int __init efx_init_module(void)
 	if (rc)
 		goto err_notifier;
 
+#ifdef CONFIG_SFC_SRIOV
 	rc = efx_init_sriov();
 	if (rc)
 		goto err_sriov;
+#endif
 
 	reset_workqueue = create_singlethread_workqueue("sfc_reset");
 	if (!reset_workqueue) {
@@ -3321,8 +3448,10 @@ static int __init efx_init_module(void)
  err_pci:
 	destroy_workqueue(reset_workqueue);
  err_reset:
+#ifdef CONFIG_SFC_SRIOV
 	efx_fini_sriov();
  err_sriov:
+#endif
 	unregister_netdevice_notifier(&efx_netdev_notifier);
  err_notifier:
 	return rc;
@@ -3334,7 +3463,9 @@ static void __exit efx_exit_module(void)
 
 	pci_unregister_driver(&efx_pci_driver);
 	destroy_workqueue(reset_workqueue);
+#ifdef CONFIG_SFC_SRIOV
 	efx_fini_sriov();
+#endif
 	unregister_netdevice_notifier(&efx_netdev_notifier);
 
 }
diff --git a/drivers/net/ethernet/sfc/efx.h b/drivers/net/ethernet/sfc/efx.h
index 2587c582a821..acb1e0718485 100644
--- a/drivers/net/ethernet/sfc/efx.h
+++ b/drivers/net/ethernet/sfc/efx.h
@@ -15,7 +15,12 @@
 #include "filter.h"
 
 /* All controllers use BAR 0 for I/O space and BAR 2(&3) for memory */
+/* All VFs use BAR 0/1 for memory */
 #define EFX_MEM_BAR 2
+#define EFX_MEM_VF_BAR 0
+
+int efx_net_open(struct net_device *net_dev);
+int efx_net_stop(struct net_device *net_dev);
 
 /* TX */
 int efx_probe_tx_queue(struct efx_tx_queue *tx_queue);
@@ -32,6 +37,7 @@ unsigned int efx_tx_max_skb_descs(struct efx_nic *efx);
 extern unsigned int efx_piobuf_size;
 
 /* RX */
+void efx_set_default_rx_indir_table(struct efx_nic *efx);
 void efx_rx_config_page_split(struct efx_nic *efx);
 int efx_probe_rx_queue(struct efx_rx_queue *rx_queue);
 void efx_remove_rx_queue(struct efx_rx_queue *rx_queue);
@@ -71,6 +77,8 @@ void efx_schedule_slow_fill(struct efx_rx_queue *rx_queue);
 
 /* Filters */
 
+void efx_mac_reconfigure(struct efx_nic *efx);
+
 /**
  * efx_filter_insert_filter - add or replace a filter
  * @efx: NIC in which to insert the filter
@@ -220,6 +228,13 @@ static inline void efx_mtd_rename(struct efx_nic *efx) {}
 static inline void efx_mtd_remove(struct efx_nic *efx) {}
 #endif
 
+#ifdef CONFIG_SFC_SRIOV
+static inline unsigned int efx_vf_size(struct efx_nic *efx)
+{
+	return 1 << efx->vi_scale;
+}
+#endif
+
 static inline void efx_schedule_channel(struct efx_channel *channel)
 {
 	netif_vdbg(channel->efx, intr, channel->efx->net_dev,
diff --git a/drivers/net/ethernet/sfc/enum.h b/drivers/net/ethernet/sfc/enum.h
index d1dbb5fb31bb..c94f56271dd4 100644
--- a/drivers/net/ethernet/sfc/enum.h
+++ b/drivers/net/ethernet/sfc/enum.h
@@ -143,6 +143,7 @@ enum efx_loopback_mode {
  * @RESET_TYPE_WORLD: Reset as much as possible
  * @RESET_TYPE_RECOVER_OR_DISABLE: Try to recover. Apply RESET_TYPE_DISABLE if
  * unsuccessful.
+ * @RESET_TYPE_DATAPATH: Reset datapath only.
  * @RESET_TYPE_MC_BIST: MC entering BIST mode.
  * @RESET_TYPE_DISABLE: Reset datapath, MAC and PHY; leave NIC disabled
  * @RESET_TYPE_TX_WATCHDOG: reset due to TX watchdog
@@ -159,6 +160,7 @@ enum reset_type {
 	RESET_TYPE_ALL,
 	RESET_TYPE_WORLD,
 	RESET_TYPE_RECOVER_OR_DISABLE,
+	RESET_TYPE_DATAPATH,
 	RESET_TYPE_MC_BIST,
 	RESET_TYPE_DISABLE,
 	RESET_TYPE_MAX_METHOD,
diff --git a/drivers/net/ethernet/sfc/ethtool.c b/drivers/net/ethernet/sfc/ethtool.c
index 4835bc0d0de8..034797661f96 100644
--- a/drivers/net/ethernet/sfc/ethtool.c
+++ b/drivers/net/ethernet/sfc/ethtool.c
@@ -734,7 +734,7 @@ static int efx_ethtool_set_pauseparam(struct net_device *net_dev,
 	/* Reconfigure the MAC. The PHY *may* generate a link state change event
 	 * if the user just changed the advertised capabilities, but there's no
 	 * harm doing this twice */
-	efx->type->reconfigure_mac(efx);
+	efx_mac_reconfigure(efx);
 
 out:
 	mutex_unlock(&efx->mac_lock);
@@ -1109,9 +1109,8 @@ static int efx_ethtool_set_rxfh(struct net_device *net_dev, const u32 *indir,
 		return -EOPNOTSUPP;
 	if (!indir)
 		return 0;
-	memcpy(efx->rx_indir_table, indir, sizeof(efx->rx_indir_table));
-	efx->type->rx_push_rss_config(efx);
-	return 0;
+
+	return efx->type->rx_push_rss_config(efx, true, indir);
 }
 
 static int efx_ethtool_get_ts_info(struct net_device *net_dev,
diff --git a/drivers/net/ethernet/sfc/falcon.c b/drivers/net/ethernet/sfc/falcon.c
index f166c8ef38a3..80e69af21642 100644
--- a/drivers/net/ethernet/sfc/falcon.c
+++ b/drivers/net/ethernet/sfc/falcon.c
@@ -477,16 +477,29 @@ static irqreturn_t falcon_legacy_interrupt_a1(int irq, void *dev_id)
  *
  **************************************************************************
  */
+static int dummy_rx_push_rss_config(struct efx_nic *efx, bool user,
+				    const u32 *rx_indir_table)
+{
+	(void) efx;
+	(void) user;
+	(void) rx_indir_table;
+	return -ENOSYS;
+}
 
-static void falcon_b0_rx_push_rss_config(struct efx_nic *efx)
+static int falcon_b0_rx_push_rss_config(struct efx_nic *efx, bool user,
+					const u32 *rx_indir_table)
 {
 	efx_oword_t temp;
 
+	(void) user;
 	/* Set hash key for IPv4 */
 	memcpy(&temp, efx->rx_hash_key, sizeof(temp));
 	efx_writeo(efx, &temp, FR_BZ_RX_RSS_TKEY);
 
+	memcpy(efx->rx_indir_table, rx_indir_table,
+	       sizeof(efx->rx_indir_table));
 	efx_farch_rx_push_indir_table(efx);
+	return 0;
 }
 
 /**************************************************************************
@@ -2507,7 +2520,7 @@ static int falcon_init_nic(struct efx_nic *efx)
 	falcon_init_rx_cfg(efx);
 
 	if (efx_nic_rev(efx) >= EFX_REV_FALCON_B0) {
-		falcon_b0_rx_push_rss_config(efx);
+		falcon_b0_rx_push_rss_config(efx, false, efx->rx_indir_table);
 
 		/* Set destination of both TX and RX Flush events */
 		EFX_POPULATE_OWORD_1(temp, FRF_BZ_FLS_EVQ_ID, 0);
@@ -2687,6 +2700,8 @@ static int falcon_set_wol(struct efx_nic *efx, u32 type)
  */
 
 const struct efx_nic_type falcon_a1_nic_type = {
+	.is_vf = false,
+	.mem_bar = EFX_MEM_BAR,
 	.mem_map_size = falcon_a1_mem_map_size,
 	.probe = falcon_probe_nic,
 	.remove = falcon_remove_nic,
@@ -2729,7 +2744,7 @@ const struct efx_nic_type falcon_a1_nic_type = {
 	.tx_init = efx_farch_tx_init,
 	.tx_remove = efx_farch_tx_remove,
 	.tx_write = efx_farch_tx_write,
-	.rx_push_rss_config = efx_port_dummy_op_void,
+	.rx_push_rss_config = dummy_rx_push_rss_config,
 	.rx_probe = efx_farch_rx_probe,
 	.rx_init = efx_farch_rx_init,
 	.rx_remove = efx_farch_rx_remove,
@@ -2766,11 +2781,6 @@ const struct efx_nic_type falcon_a1_nic_type = {
 	.mtd_write = falcon_mtd_write,
 	.mtd_sync = falcon_mtd_sync,
 #endif
-	.sriov_init = efx_falcon_sriov_init,
-	.sriov_fini = efx_falcon_sriov_fini,
-	.sriov_mac_address_changed = efx_falcon_sriov_mac_address_changed,
-	.sriov_wanted = efx_falcon_sriov_wanted,
-	.sriov_reset = efx_falcon_sriov_reset,
 
 	.revision = EFX_REV_FALCON_A1,
 	.txd_ptr_tbl_base = FR_AA_TX_DESC_PTR_TBL_KER,
@@ -2788,6 +2798,8 @@ const struct efx_nic_type falcon_a1_nic_type = {
 };
 
 const struct efx_nic_type falcon_b0_nic_type = {
+	.is_vf = false,
+	.mem_bar = EFX_MEM_BAR,
 	.mem_map_size = falcon_b0_mem_map_size,
 	.probe = falcon_probe_nic,
 	.remove = falcon_remove_nic,
@@ -2867,11 +2879,6 @@ const struct efx_nic_type falcon_b0_nic_type = {
 	.mtd_write = falcon_mtd_write,
 	.mtd_sync = falcon_mtd_sync,
 #endif
-	.sriov_init = efx_falcon_sriov_init,
-	.sriov_fini = efx_falcon_sriov_fini,
-	.sriov_mac_address_changed = efx_falcon_sriov_mac_address_changed,
-	.sriov_wanted = efx_falcon_sriov_wanted,
-	.sriov_reset = efx_falcon_sriov_reset,
 
 	.revision = EFX_REV_FALCON_B0,
 	.txd_ptr_tbl_base = FR_BZ_TX_DESC_PTR_TBL,
diff --git a/drivers/net/ethernet/sfc/farch.c b/drivers/net/ethernet/sfc/farch.c
index bb89e96a125e..f08266f0eca2 100644
--- a/drivers/net/ethernet/sfc/farch.c
+++ b/drivers/net/ethernet/sfc/farch.c
@@ -20,6 +20,8 @@
 #include "efx.h"
 #include "nic.h"
 #include "farch_regs.h"
+#include "sriov.h"
+#include "siena_sriov.h"
 #include "io.h"
 #include "workarounds.h"
 
@@ -1198,13 +1200,17 @@ efx_farch_handle_driver_event(struct efx_channel *channel, efx_qword_t *event)
 		netif_vdbg(efx, hw, efx->net_dev, "channel %d TXQ %d flushed\n",
 			   channel->channel, ev_sub_data);
 		efx_farch_handle_tx_flush_done(efx, event);
+#ifdef CONFIG_SFC_SRIOV
 		efx_siena_sriov_tx_flush_done(efx, event);
+#endif
 		break;
 	case FSE_AZ_RX_DESCQ_FLS_DONE_EV:
 		netif_vdbg(efx, hw, efx->net_dev, "channel %d RXQ %d flushed\n",
 			   channel->channel, ev_sub_data);
 		efx_farch_handle_rx_flush_done(efx, event);
+#ifdef CONFIG_SFC_SRIOV
 		efx_siena_sriov_rx_flush_done(efx, event);
+#endif
 		break;
 	case FSE_AZ_EVQ_INIT_DONE_EV:
 		netif_dbg(efx, hw, efx->net_dev,
@@ -1242,8 +1248,11 @@ efx_farch_handle_driver_event(struct efx_channel *channel, efx_qword_t *event)
 				  " RX Q %d is disabled.\n", ev_sub_data,
 				  ev_sub_data);
 			efx_schedule_reset(efx, RESET_TYPE_DMA_ERROR);
-		} else
+		}
+#ifdef CONFIG_SFC_SRIOV
+		else
 			efx_siena_sriov_desc_fetch_err(efx, ev_sub_data);
+#endif
 		break;
 	case FSE_BZ_TX_DSC_ERROR_EV:
 		if (ev_sub_data < EFX_VI_BASE) {
@@ -1252,8 +1261,11 @@ efx_farch_handle_driver_event(struct efx_channel *channel, efx_qword_t *event)
 				  " TX Q %d is disabled.\n", ev_sub_data,
 				  ev_sub_data);
 			efx_schedule_reset(efx, RESET_TYPE_DMA_ERROR);
-		} else
+		}
+#ifdef CONFIG_SFC_SRIOV
+		else
 			efx_siena_sriov_desc_fetch_err(efx, ev_sub_data);
+#endif
 		break;
 	default:
 		netif_vdbg(efx, hw, efx->net_dev,
@@ -1317,9 +1329,11 @@ int efx_farch_ev_process(struct efx_channel *channel, int budget)
 		case FSE_AZ_EV_CODE_DRIVER_EV:
 			efx_farch_handle_driver_event(channel, &event);
 			break;
+#ifdef CONFIG_SFC_SRIOV
 		case FSE_CZ_EV_CODE_USER_EV:
 			efx_siena_sriov_event(channel, &event);
 			break;
+#endif
 		case FSE_CZ_EV_CODE_MCDI_EV:
 			efx_mcdi_process_event(channel, &event);
 			break;
@@ -1685,28 +1699,32 @@ void efx_farch_dimension_resources(struct efx_nic *efx, unsigned sram_lim_qw)
 	vi_count = max(efx->n_channels, efx->n_tx_channels * EFX_TXQ_TYPES);
 
 #ifdef CONFIG_SFC_SRIOV
-	if (efx->type->sriov_wanted(efx)) {
-		unsigned vi_dc_entries, buftbl_free, entries_per_vf, vf_limit;
-
-		nic_data->vf_buftbl_base = buftbl_min;
-
-		vi_dc_entries = RX_DC_ENTRIES + TX_DC_ENTRIES;
-		vi_count = max(vi_count, EFX_VI_BASE);
-		buftbl_free = (sram_lim_qw - buftbl_min -
-			       vi_count * vi_dc_entries);
-
-		entries_per_vf = ((vi_dc_entries + EFX_VF_BUFTBL_PER_VI) *
-				  efx_vf_size(efx));
-		vf_limit = min(buftbl_free / entries_per_vf,
-			       (1024U - EFX_VI_BASE) >> efx->vi_scale);
-
-		if (efx->vf_count > vf_limit) {
-			netif_err(efx, probe, efx->net_dev,
-				  "Reducing VF count from from %d to %d\n",
-				  efx->vf_count, vf_limit);
-			efx->vf_count = vf_limit;
+	if (efx->type->sriov_wanted) {
+		if (efx->type->sriov_wanted(efx)) {
+			unsigned vi_dc_entries, buftbl_free;
+			unsigned entries_per_vf, vf_limit;
+
+			nic_data->vf_buftbl_base = buftbl_min;
+
+			vi_dc_entries = RX_DC_ENTRIES + TX_DC_ENTRIES;
+			vi_count = max(vi_count, EFX_VI_BASE);
+			buftbl_free = (sram_lim_qw - buftbl_min -
+				       vi_count * vi_dc_entries);
+
+			entries_per_vf = ((vi_dc_entries +
+					   EFX_VF_BUFTBL_PER_VI) *
+					  efx_vf_size(efx));
+			vf_limit = min(buftbl_free / entries_per_vf,
+				       (1024U - EFX_VI_BASE) >> efx->vi_scale);
+
+			if (efx->vf_count > vf_limit) {
+				netif_err(efx, probe, efx->net_dev,
+					  "Reducing VF count from from %d to %d\n",
+					  efx->vf_count, vf_limit);
+				efx->vf_count = vf_limit;
+			}
+			vi_count += efx->vf_count * efx_vf_size(efx);
 		}
-		vi_count += efx->vf_count * efx_vf_size(efx);
 	}
 #endif
 
diff --git a/drivers/net/ethernet/sfc/mcdi.c b/drivers/net/ethernet/sfc/mcdi.c
index d37928f01949..81640f8bb811 100644
--- a/drivers/net/ethernet/sfc/mcdi.c
+++ b/drivers/net/ethernet/sfc/mcdi.c
@@ -8,6 +8,7 @@
  */
 
 #include <linux/delay.h>
+#include <linux/moduleparam.h>
 #include <asm/cmpxchg.h>
 #include "net_driver.h"
 #include "nic.h"
@@ -54,18 +55,32 @@ static int efx_mcdi_drv_attach(struct efx_nic *efx, bool driver_operating,
 static bool efx_mcdi_poll_once(struct efx_nic *efx);
 static void efx_mcdi_abandon(struct efx_nic *efx);
 
+#ifdef CONFIG_SFC_MCDI_LOGGING
+static bool mcdi_logging_default;
+module_param(mcdi_logging_default, bool, 0644);
+MODULE_PARM_DESC(mcdi_logging_default,
+		 "Enable MCDI logging on newly-probed functions");
+#endif
+
 int efx_mcdi_init(struct efx_nic *efx)
 {
 	struct efx_mcdi_iface *mcdi;
 	bool already_attached;
-	int rc;
+	int rc = -ENOMEM;
 
 	efx->mcdi = kzalloc(sizeof(*efx->mcdi), GFP_KERNEL);
 	if (!efx->mcdi)
-		return -ENOMEM;
+		goto fail;
 
 	mcdi = efx_mcdi(efx);
 	mcdi->efx = efx;
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	/* consuming code assumes buffer is page-sized */
+	mcdi->logging_buffer = (char *)__get_free_page(GFP_KERNEL);
+	if (!mcdi->logging_buffer)
+		goto fail1;
+	mcdi->logging_enabled = mcdi_logging_default;
+#endif
 	init_waitqueue_head(&mcdi->wq);
 	spin_lock_init(&mcdi->iface_lock);
 	mcdi->state = MCDI_STATE_QUIESCENT;
@@ -81,7 +96,7 @@ int efx_mcdi_init(struct efx_nic *efx)
 	/* Recover from a failed assertion before probing */
 	rc = efx_mcdi_handle_assertion(efx);
 	if (rc)
-		return rc;
+		goto fail2;
 
 	/* Let the MC (and BMC, if this is a LOM) know that the driver
 	 * is loaded. We should do this before we reset the NIC.
@@ -90,7 +105,7 @@ int efx_mcdi_init(struct efx_nic *efx)
 	if (rc) {
 		netif_err(efx, probe, efx->net_dev,
 			  "Unable to register driver with MCPU\n");
-		return rc;
+		goto fail2;
 	}
 	if (already_attached)
 		/* Not a fatal error */
@@ -102,6 +117,15 @@ int efx_mcdi_init(struct efx_nic *efx)
 		efx->primary = efx;
 
 	return 0;
+fail2:
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	free_page((unsigned long)mcdi->logging_buffer);
+fail1:
+#endif
+	kfree(efx->mcdi);
+	efx->mcdi = NULL;
+fail:
+	return rc;
 }
 
 void efx_mcdi_fini(struct efx_nic *efx)
@@ -114,6 +138,10 @@ void efx_mcdi_fini(struct efx_nic *efx)
 	/* Relinquish the device (back to the BMC, if this is a LOM) */
 	efx_mcdi_drv_attach(efx, false, NULL);
 
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	free_page((unsigned long)efx->mcdi->iface.logging_buffer);
+#endif
+
 	kfree(efx->mcdi);
 }
 
@@ -121,6 +149,9 @@ static void efx_mcdi_send_request(struct efx_nic *efx, unsigned cmd,
 				  const efx_dword_t *inbuf, size_t inlen)
 {
 	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	char *buf = mcdi->logging_buffer; /* page-sized */
+#endif
 	efx_dword_t hdr[2];
 	size_t hdr_len;
 	u32 xflags, seqno;
@@ -165,6 +196,31 @@ static void efx_mcdi_send_request(struct efx_nic *efx, unsigned cmd,
 		hdr_len = 8;
 	}
 
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	if (mcdi->logging_enabled && !WARN_ON_ONCE(!buf)) {
+		int bytes = 0;
+		int i;
+		/* Lengths should always be a whole number of dwords, so scream
+		 * if they're not.
+		 */
+		WARN_ON_ONCE(hdr_len % 4);
+		WARN_ON_ONCE(inlen % 4);
+
+		/* We own the logging buffer, as only one MCDI can be in
+		 * progress on a NIC at any one time.  So no need for locking.
+		 */
+		for (i = 0; i < hdr_len / 4 && bytes < PAGE_SIZE; i++)
+			bytes += snprintf(buf + bytes, PAGE_SIZE - bytes,
+					  " %08x", le32_to_cpu(hdr[i].u32[0]));
+
+		for (i = 0; i < inlen / 4 && bytes < PAGE_SIZE; i++)
+			bytes += snprintf(buf + bytes, PAGE_SIZE - bytes,
+					  " %08x", le32_to_cpu(inbuf[i].u32[0]));
+
+		netif_info(efx, hw, efx->net_dev, "MCDI RPC REQ:%s\n", buf);
+	}
+#endif
+
 	efx->type->mcdi_request(efx, hdr, hdr_len, inbuf, inlen);
 
 	mcdi->new_epoch = false;
@@ -206,6 +262,9 @@ static void efx_mcdi_read_response_header(struct efx_nic *efx)
 {
 	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
 	unsigned int respseq, respcmd, error;
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	char *buf = mcdi->logging_buffer; /* page-sized */
+#endif
 	efx_dword_t hdr;
 
 	efx->type->mcdi_read_response(efx, &hdr, 0, 4);
@@ -223,6 +282,39 @@ static void efx_mcdi_read_response_header(struct efx_nic *efx)
 			EFX_DWORD_FIELD(hdr, MC_CMD_V2_EXTN_IN_ACTUAL_LEN);
 	}
 
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	if (mcdi->logging_enabled && !WARN_ON_ONCE(!buf)) {
+		size_t hdr_len, data_len;
+		int bytes = 0;
+		int i;
+
+		WARN_ON_ONCE(mcdi->resp_hdr_len % 4);
+		hdr_len = mcdi->resp_hdr_len / 4;
+		/* MCDI_DECLARE_BUF ensures that underlying buffer is padded
+		 * to dword size, and the MCDI buffer is always dword size
+		 */
+		data_len = DIV_ROUND_UP(mcdi->resp_data_len, 4);
+
+		/* We own the logging buffer, as only one MCDI can be in
+		 * progress on a NIC at any one time.  So no need for locking.
+		 */
+		for (i = 0; i < hdr_len && bytes < PAGE_SIZE; i++) {
+			efx->type->mcdi_read_response(efx, &hdr, (i * 4), 4);
+			bytes += snprintf(buf + bytes, PAGE_SIZE - bytes,
+					  " %08x", le32_to_cpu(hdr.u32[0]));
+		}
+
+		for (i = 0; i < data_len && bytes < PAGE_SIZE; i++) {
+			efx->type->mcdi_read_response(efx, &hdr,
+					mcdi->resp_hdr_len + (i * 4), 4);
+			bytes += snprintf(buf + bytes, PAGE_SIZE - bytes,
+					  " %08x", le32_to_cpu(hdr.u32[0]));
+		}
+
+		netif_info(efx, hw, efx->net_dev, "MCDI RPC RESP:%s\n", buf);
+	}
+#endif
+
 	if (error && mcdi->resp_data_len == 0) {
 		netif_err(efx, hw, efx->net_dev, "MC rebooted\n");
 		mcdi->resprc = -EIO;
@@ -406,7 +498,7 @@ static bool efx_mcdi_complete_async(struct efx_mcdi_iface *mcdi, bool timeout)
 	struct efx_mcdi_async_param *async;
 	size_t hdr_len, data_len, err_len;
 	efx_dword_t *outbuf;
-	MCDI_DECLARE_BUF_OUT_OR_ERR(errbuf, 0);
+	MCDI_DECLARE_BUF_ERR(errbuf);
 	int rc;
 
 	if (cmpxchg(&mcdi->state,
@@ -534,7 +626,7 @@ static int _efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned cmd, size_t inlen,
 				size_t *outlen_actual, bool quiet)
 {
 	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
-	MCDI_DECLARE_BUF_OUT_OR_ERR(errbuf, 0);
+	MCDI_DECLARE_BUF_ERR(errbuf);
 	int rc;
 
 	if (mcdi->mode == MCDI_MODE_POLL)
@@ -1035,7 +1127,9 @@ void efx_mcdi_process_event(struct efx_channel *channel,
 		/* MAC stats are gather lazily.  We can ignore this. */
 		break;
 	case MCDI_EVENT_CODE_FLR:
-		efx_siena_sriov_flr(efx, MCDI_EVENT_FIELD(*event, FLR_VF));
+		if (efx->type->sriov_flr)
+			efx->type->sriov_flr(efx,
+					     MCDI_EVENT_FIELD(*event, FLR_VF));
 		break;
 	case MCDI_EVENT_CODE_PTP_RX:
 	case MCDI_EVENT_CODE_PTP_FAULT:
@@ -1081,9 +1175,7 @@ void efx_mcdi_process_event(struct efx_channel *channel,
 
 void efx_mcdi_print_fwver(struct efx_nic *efx, char *buf, size_t len)
 {
-	MCDI_DECLARE_BUF(outbuf,
-			 max(MC_CMD_GET_VERSION_OUT_LEN,
-			     MC_CMD_GET_CAPABILITIES_OUT_LEN));
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_VERSION_OUT_LEN);
 	size_t outlength;
 	const __le16 *ver_words;
 	size_t offset;
@@ -1108,19 +1200,11 @@ void efx_mcdi_print_fwver(struct efx_nic *efx, char *buf, size_t len)
 	 * single version.  Report which variants are running.
 	 */
 	if (efx_nic_rev(efx) >= EFX_REV_HUNT_A0) {
-		BUILD_BUG_ON(MC_CMD_GET_CAPABILITIES_IN_LEN != 0);
-		rc = efx_mcdi_rpc(efx, MC_CMD_GET_CAPABILITIES, NULL, 0,
-				  outbuf, sizeof(outbuf), &outlength);
-		if (rc || outlength < MC_CMD_GET_CAPABILITIES_OUT_LEN)
-			offset += snprintf(
-				buf + offset, len - offset, " rx? tx?");
-		else
-			offset += snprintf(
-				buf + offset, len - offset, " rx%x tx%x",
-				MCDI_WORD(outbuf,
-					  GET_CAPABILITIES_OUT_RX_DPCPU_FW_ID),
-				MCDI_WORD(outbuf,
-					  GET_CAPABILITIES_OUT_TX_DPCPU_FW_ID));
+		struct efx_ef10_nic_data *nic_data = efx->nic_data;
+
+		offset += snprintf(buf + offset, len - offset, " rx%x tx%x",
+				   nic_data->rx_dpcpu_fw_id,
+				   nic_data->tx_dpcpu_fw_id);
 
 		/* It's theoretically possible for the string to exceed 31
 		 * characters, though in practice the first three version
@@ -1150,10 +1234,26 @@ static int efx_mcdi_drv_attach(struct efx_nic *efx, bool driver_operating,
 	MCDI_SET_DWORD(inbuf, DRV_ATTACH_IN_UPDATE, 1);
 	MCDI_SET_DWORD(inbuf, DRV_ATTACH_IN_FIRMWARE_ID, MC_CMD_FW_LOW_LATENCY);
 
-	rc = efx_mcdi_rpc(efx, MC_CMD_DRV_ATTACH, inbuf, sizeof(inbuf),
-			  outbuf, sizeof(outbuf), &outlen);
-	if (rc)
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_DRV_ATTACH, inbuf, sizeof(inbuf),
+				outbuf, sizeof(outbuf), &outlen);
+	/* If we're not the primary PF, trying to ATTACH with a FIRMWARE_ID
+	 * specified will fail with EPERM, and we have to tell the MC we don't
+	 * care what firmware we get.
+	 */
+	if (rc == -EPERM) {
+		netif_dbg(efx, probe, efx->net_dev,
+			  "efx_mcdi_drv_attach with fw-variant setting failed EPERM, trying without it\n");
+		MCDI_SET_DWORD(inbuf, DRV_ATTACH_IN_FIRMWARE_ID,
+			       MC_CMD_FW_DONT_CARE);
+		rc = efx_mcdi_rpc_quiet(efx, MC_CMD_DRV_ATTACH, inbuf,
+					sizeof(inbuf), outbuf, sizeof(outbuf),
+					&outlen);
+	}
+	if (rc) {
+		efx_mcdi_display_error(efx, MC_CMD_DRV_ATTACH, sizeof(inbuf),
+				       outbuf, outlen, rc);
 		goto fail;
+	}
 	if (outlen < MC_CMD_DRV_ATTACH_OUT_LEN) {
 		rc = -EIO;
 		goto fail;
@@ -1178,16 +1278,6 @@ static int efx_mcdi_drv_attach(struct efx_nic *efx, bool driver_operating,
 	 * and are completely trusted by firmware.  Abort probing
 	 * if that's not true for this function.
 	 */
-	if (driver_operating &&
-	    (efx->mcdi->fn_flags &
-	     (1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_LINKCTRL |
-	      1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_TRUSTED)) !=
-	    (1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_LINKCTRL |
-	     1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_TRUSTED)) {
-		netif_err(efx, probe, efx->net_dev,
-			  "This driver version only supports one function per port\n");
-		return -ENODEV;
-	}
 
 	if (was_attached != NULL)
 		*was_attached = MCDI_DWORD(outbuf, DRV_ATTACH_OUT_OLD_STATE);
@@ -1385,10 +1475,13 @@ fail1:
 	return rc;
 }
 
+/* Returns 1 if an assertion was read, 0 if no assertion had fired,
+ * negative on error.
+ */
 static int efx_mcdi_read_assertion(struct efx_nic *efx)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_GET_ASSERTS_IN_LEN);
-	MCDI_DECLARE_BUF_OUT_OR_ERR(outbuf, MC_CMD_GET_ASSERTS_OUT_LEN);
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_ASSERTS_OUT_LEN);
 	unsigned int flags, index;
 	const char *reason;
 	size_t outlen;
@@ -1406,6 +1499,8 @@ static int efx_mcdi_read_assertion(struct efx_nic *efx)
 		rc = efx_mcdi_rpc_quiet(efx, MC_CMD_GET_ASSERTS,
 					inbuf, MC_CMD_GET_ASSERTS_IN_LEN,
 					outbuf, sizeof(outbuf), &outlen);
+		if (rc == -EPERM)
+			return 0;
 	} while ((rc == -EINTR || rc == -EIO) && retry-- > 0);
 
 	if (rc) {
@@ -1443,24 +1538,31 @@ static int efx_mcdi_read_assertion(struct efx_nic *efx)
 			  MCDI_ARRAY_DWORD(outbuf, GET_ASSERTS_OUT_GP_REGS_OFFS,
 					   index));
 
-	return 0;
+	return 1;
 }
 
-static void efx_mcdi_exit_assertion(struct efx_nic *efx)
+static int efx_mcdi_exit_assertion(struct efx_nic *efx)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_REBOOT_IN_LEN);
+	int rc;
 
 	/* If the MC is running debug firmware, it might now be
 	 * waiting for a debugger to attach, but we just want it to
 	 * reboot.  We set a flag that makes the command a no-op if it
-	 * has already done so.  We don't know what return code to
-	 * expect (0 or -EIO), so ignore it.
+	 * has already done so.
+	 * The MCDI will thus return either 0 or -EIO.
 	 */
 	BUILD_BUG_ON(MC_CMD_REBOOT_OUT_LEN != 0);
 	MCDI_SET_DWORD(inbuf, REBOOT_IN_FLAGS,
 		       MC_CMD_REBOOT_FLAGS_AFTER_ASSERTION);
-	(void) efx_mcdi_rpc(efx, MC_CMD_REBOOT, inbuf, MC_CMD_REBOOT_IN_LEN,
-			    NULL, 0, NULL);
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_REBOOT, inbuf, MC_CMD_REBOOT_IN_LEN,
+				NULL, 0, NULL);
+	if (rc == -EIO)
+		rc = 0;
+	if (rc)
+		efx_mcdi_display_error(efx, MC_CMD_REBOOT, MC_CMD_REBOOT_IN_LEN,
+				       NULL, 0, rc);
+	return rc;
 }
 
 int efx_mcdi_handle_assertion(struct efx_nic *efx)
@@ -1468,12 +1570,10 @@ int efx_mcdi_handle_assertion(struct efx_nic *efx)
 	int rc;
 
 	rc = efx_mcdi_read_assertion(efx);
-	if (rc)
+	if (rc <= 0)
 		return rc;
 
-	efx_mcdi_exit_assertion(efx);
-
-	return 0;
+	return efx_mcdi_exit_assertion(efx);
 }
 
 void efx_mcdi_set_id_led(struct efx_nic *efx, enum efx_led_mode mode)
@@ -1550,7 +1650,9 @@ int efx_mcdi_reset(struct efx_nic *efx, enum reset_type method)
 	if (rc)
 		return rc;
 
-	if (method == RESET_TYPE_WORLD)
+	if (method == RESET_TYPE_DATAPATH)
+		return 0;
+	else if (method == RESET_TYPE_WORLD)
 		return efx_mcdi_reset_mc(efx);
 	else
 		return efx_mcdi_reset_func(efx);
@@ -1688,6 +1790,36 @@ int efx_mcdi_set_workaround(struct efx_nic *efx, u32 type, bool enabled)
 			    NULL, 0, NULL);
 }
 
+int efx_mcdi_get_workarounds(struct efx_nic *efx, unsigned int *impl_out,
+			     unsigned int *enabled_out)
+{
+	MCDI_DECLARE_BUF(outbuf, MC_CMD_GET_WORKAROUNDS_OUT_LEN);
+	size_t outlen;
+	int rc;
+
+	rc = efx_mcdi_rpc(efx, MC_CMD_GET_WORKAROUNDS, NULL, 0,
+			  outbuf, sizeof(outbuf), &outlen);
+	if (rc)
+		goto fail;
+
+	if (outlen < MC_CMD_GET_WORKAROUNDS_OUT_LEN) {
+		rc = -EIO;
+		goto fail;
+	}
+
+	if (impl_out)
+		*impl_out = MCDI_DWORD(outbuf, GET_WORKAROUNDS_OUT_IMPLEMENTED);
+
+	if (enabled_out)
+		*enabled_out = MCDI_DWORD(outbuf, GET_WORKAROUNDS_OUT_ENABLED);
+
+	return 0;
+
+fail:
+	netif_err(efx, hw, efx->net_dev, "%s: failed rc=%d\n", __func__, rc);
+	return rc;
+}
+
 #ifdef CONFIG_SFC_MTD
 
 #define EFX_MCDI_NVRAM_LEN_MAX 128
diff --git a/drivers/net/ethernet/sfc/mcdi.h b/drivers/net/ethernet/sfc/mcdi.h
index 56465f7465a2..1838afe2da92 100644
--- a/drivers/net/ethernet/sfc/mcdi.h
+++ b/drivers/net/ethernet/sfc/mcdi.h
@@ -58,6 +58,8 @@ enum efx_mcdi_mode {
  *	enabled
  * @async_list: Queue of asynchronous requests
  * @async_timer: Timer for asynchronous request timeout
+ * @logging_buffer: buffer that may be used to build MCDI tracing messages
+ * @logging_enabled: whether to trace MCDI
  */
 struct efx_mcdi_iface {
 	struct efx_nic *efx;
@@ -74,6 +76,10 @@ struct efx_mcdi_iface {
 	spinlock_t async_lock;
 	struct list_head async_list;
 	struct timer_list async_timer;
+#ifdef CONFIG_SFC_MCDI_LOGGING
+	char *logging_buffer;
+	bool logging_enabled;
+#endif
 };
 
 struct efx_mcdi_mon {
@@ -176,10 +182,12 @@ void efx_mcdi_sensor_event(struct efx_nic *efx, efx_qword_t *ev);
  * 32-bit-aligned.  Also, on Siena we must copy to the MC shared
  * memory strictly 32 bits at a time, so add any necessary padding.
  */
-#define MCDI_DECLARE_BUF(_name, _len)					\
+#define _MCDI_DECLARE_BUF(_name, _len)					\
 	efx_dword_t _name[DIV_ROUND_UP(_len, 4)]
-#define MCDI_DECLARE_BUF_OUT_OR_ERR(_name, _len)			\
-	MCDI_DECLARE_BUF(_name, max_t(size_t, _len, 8))
+#define MCDI_DECLARE_BUF(_name, _len)					\
+	_MCDI_DECLARE_BUF(_name, _len) = {{{0}}}
+#define MCDI_DECLARE_BUF_ERR(_name)					\
+	MCDI_DECLARE_BUF(_name, 8)
 #define _MCDI_PTR(_buf, _offset)					\
 	((u8 *)(_buf) + (_offset))
 #define MCDI_PTR(_buf, _field)						\
@@ -339,6 +347,8 @@ bool efx_mcdi_mac_check_fault(struct efx_nic *efx);
 enum reset_type efx_mcdi_map_reset_reason(enum reset_type reason);
 int efx_mcdi_reset(struct efx_nic *efx, enum reset_type method);
 int efx_mcdi_set_workaround(struct efx_nic *efx, u32 type, bool enabled);
+int efx_mcdi_get_workarounds(struct efx_nic *efx, unsigned int *impl_out,
+			     unsigned int *enabled_out);
 
 #ifdef CONFIG_SFC_MCDI_MON
 int efx_mcdi_mon_probe(struct efx_nic *efx);
diff --git a/drivers/net/ethernet/sfc/mcdi_pcol.h b/drivers/net/ethernet/sfc/mcdi_pcol.h
index e028de10e1b7..45fca9fc66b7 100644
--- a/drivers/net/ethernet/sfc/mcdi_pcol.h
+++ b/drivers/net/ethernet/sfc/mcdi_pcol.h
@@ -638,6 +638,8 @@
  */
 #define MC_CMD_READ32 0x1
 
+#define MC_CMD_0x1_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_READ32_IN msgrequest */
 #define    MC_CMD_READ32_IN_LEN 8
 #define       MC_CMD_READ32_IN_ADDR_OFST 0
@@ -659,6 +661,8 @@
  */
 #define MC_CMD_WRITE32 0x2
 
+#define MC_CMD_0x2_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_WRITE32_IN msgrequest */
 #define    MC_CMD_WRITE32_IN_LENMIN 8
 #define    MC_CMD_WRITE32_IN_LENMAX 252
@@ -679,6 +683,8 @@
  */
 #define MC_CMD_COPYCODE 0x3
 
+#define MC_CMD_0x3_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_COPYCODE_IN msgrequest */
 #define    MC_CMD_COPYCODE_IN_LEN 16
 /* Source address */
@@ -717,6 +723,8 @@
  */
 #define MC_CMD_SET_FUNC 0x4
 
+#define MC_CMD_0x4_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_FUNC_IN msgrequest */
 #define    MC_CMD_SET_FUNC_IN_LEN 4
 /* Set function */
@@ -732,6 +740,8 @@
  */
 #define MC_CMD_GET_BOOT_STATUS 0x5
 
+#define MC_CMD_0x5_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_GET_BOOT_STATUS_IN msgrequest */
 #define    MC_CMD_GET_BOOT_STATUS_IN_LEN 0
 
@@ -758,6 +768,8 @@
  */
 #define MC_CMD_GET_ASSERTS 0x6
 
+#define MC_CMD_0x6_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_GET_ASSERTS_IN msgrequest */
 #define    MC_CMD_GET_ASSERTS_IN_LEN 4
 /* Set to clear assertion */
@@ -794,6 +806,8 @@
  */
 #define MC_CMD_LOG_CTRL 0x7
 
+#define MC_CMD_0x7_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_LOG_CTRL_IN msgrequest */
 #define    MC_CMD_LOG_CTRL_IN_LEN 8
 /* Log destination */
@@ -814,6 +828,8 @@
  */
 #define MC_CMD_GET_VERSION 0x8
 
+#define MC_CMD_0x8_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_VERSION_IN msgrequest */
 #define    MC_CMD_GET_VERSION_IN_LEN 0
 
@@ -870,6 +886,8 @@
  */
 #define MC_CMD_PTP 0xb
 
+#define MC_CMD_0xb_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_PTP_IN msgrequest */
 #define    MC_CMD_PTP_IN_LEN 1
 /* PTP operation code */
@@ -1404,6 +1422,8 @@
  */
 #define MC_CMD_CSR_READ32 0xc
 
+#define MC_CMD_0xc_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_CSR_READ32_IN msgrequest */
 #define    MC_CMD_CSR_READ32_IN_LEN 12
 /* Address */
@@ -1428,6 +1448,8 @@
  */
 #define MC_CMD_CSR_WRITE32 0xd
 
+#define MC_CMD_0xd_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_CSR_WRITE32_IN msgrequest */
 #define    MC_CMD_CSR_WRITE32_IN_LENMIN 12
 #define    MC_CMD_CSR_WRITE32_IN_LENMAX 252
@@ -1452,6 +1474,8 @@
  */
 #define MC_CMD_HP 0x54
 
+#define MC_CMD_0x54_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_HP_IN msgrequest */
 #define    MC_CMD_HP_IN_LEN 16
 /* HP OCSD sub-command. When address is not NULL, request activation of OCSD at
@@ -1493,6 +1517,8 @@
  */
 #define MC_CMD_STACKINFO 0xf
 
+#define MC_CMD_0xf_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_STACKINFO_IN msgrequest */
 #define    MC_CMD_STACKINFO_IN_LEN 0
 
@@ -1513,6 +1539,8 @@
  */
 #define MC_CMD_MDIO_READ 0x10
 
+#define MC_CMD_0x10_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_MDIO_READ_IN msgrequest */
 #define    MC_CMD_MDIO_READ_IN_LEN 16
 /* Bus number; there are two MDIO buses: one for the internal PHY, and one for
@@ -1552,6 +1580,8 @@
  */
 #define MC_CMD_MDIO_WRITE 0x11
 
+#define MC_CMD_0x11_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_MDIO_WRITE_IN msgrequest */
 #define    MC_CMD_MDIO_WRITE_IN_LEN 20
 /* Bus number; there are two MDIO buses: one for the internal PHY, and one for
@@ -1591,6 +1621,8 @@
  */
 #define MC_CMD_DBI_WRITE 0x12
 
+#define MC_CMD_0x12_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DBI_WRITE_IN msgrequest */
 #define    MC_CMD_DBI_WRITE_IN_LENMIN 12
 #define    MC_CMD_DBI_WRITE_IN_LENMAX 252
@@ -1739,6 +1771,8 @@
  */
 #define MC_CMD_GET_BOARD_CFG 0x18
 
+#define MC_CMD_0x18_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_BOARD_CFG_IN msgrequest */
 #define    MC_CMD_GET_BOARD_CFG_IN_LEN 0
 
@@ -1778,6 +1812,8 @@
  */
 #define MC_CMD_DBI_READX 0x19
 
+#define MC_CMD_0x19_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DBI_READX_IN msgrequest */
 #define    MC_CMD_DBI_READX_IN_LENMIN 8
 #define    MC_CMD_DBI_READX_IN_LENMAX 248
@@ -1822,6 +1858,8 @@
  */
 #define MC_CMD_SET_RAND_SEED 0x1a
 
+#define MC_CMD_0x1a_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_RAND_SEED_IN msgrequest */
 #define    MC_CMD_SET_RAND_SEED_IN_LEN 16
 /* Seed value. */
@@ -1863,6 +1901,8 @@
  */
 #define MC_CMD_DRV_ATTACH 0x1c
 
+#define MC_CMD_0x1c_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_DRV_ATTACH_IN msgrequest */
 #define    MC_CMD_DRV_ATTACH_IN_LEN 12
 /* new state (0=detached, 1=attached) to set if UPDATE=1 */
@@ -1875,6 +1915,8 @@
 #define          MC_CMD_FW_FULL_FEATURED 0x0
 /* enum: Prefer to use firmware with fewer features but lower latency */
 #define          MC_CMD_FW_LOW_LATENCY 0x1
+/* enum: Only this option is allowed for non-admin functions */
+#define          MC_CMD_FW_DONT_CARE  0xffffffff
 
 /* MC_CMD_DRV_ATTACH_OUT msgresponse */
 #define    MC_CMD_DRV_ATTACH_OUT_LEN 4
@@ -1920,6 +1962,8 @@
  */
 #define MC_CMD_PORT_RESET 0x20
 
+#define MC_CMD_0x20_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_PORT_RESET_IN msgrequest */
 #define    MC_CMD_PORT_RESET_IN_LEN 0
 
@@ -1934,6 +1978,7 @@
  * extended version of the deprecated MC_CMD_PORT_RESET with added fields.
  */
 #define MC_CMD_ENTITY_RESET 0x20
+/*      MC_CMD_0x20_PRIVILEGE_CTG SRIOV_CTG_GENERAL */
 
 /* MC_CMD_ENTITY_RESET_IN msgrequest */
 #define    MC_CMD_ENTITY_RESET_IN_LEN 4
@@ -2023,6 +2068,8 @@
  */
 #define MC_CMD_PUTS 0x23
 
+#define MC_CMD_0x23_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_PUTS_IN msgrequest */
 #define    MC_CMD_PUTS_IN_LENMIN 13
 #define    MC_CMD_PUTS_IN_LENMAX 252
@@ -2050,6 +2097,8 @@
  */
 #define MC_CMD_GET_PHY_CFG 0x24
 
+#define MC_CMD_0x24_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_PHY_CFG_IN msgrequest */
 #define    MC_CMD_GET_PHY_CFG_IN_LEN 0
 
@@ -2149,6 +2198,8 @@
  */
 #define MC_CMD_START_BIST 0x25
 
+#define MC_CMD_0x25_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_START_BIST_IN msgrequest */
 #define    MC_CMD_START_BIST_IN_LEN 4
 /* Type of test. */
@@ -2185,6 +2236,8 @@
  */
 #define MC_CMD_POLL_BIST 0x26
 
+#define MC_CMD_0x26_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_POLL_BIST_IN msgrequest */
 #define    MC_CMD_POLL_BIST_IN_LEN 0
 
@@ -2344,6 +2397,8 @@
  */
 #define MC_CMD_GET_LOOPBACK_MODES 0x28
 
+#define MC_CMD_0x28_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_LOOPBACK_MODES_IN msgrequest */
 #define    MC_CMD_GET_LOOPBACK_MODES_IN_LEN 0
 
@@ -2463,6 +2518,8 @@
  */
 #define MC_CMD_GET_LINK 0x29
 
+#define MC_CMD_0x29_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_LINK_IN msgrequest */
 #define    MC_CMD_GET_LINK_IN_LEN 0
 
@@ -2519,6 +2576,8 @@
  */
 #define MC_CMD_SET_LINK 0x2a
 
+#define MC_CMD_0x2a_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_SET_LINK_IN msgrequest */
 #define    MC_CMD_SET_LINK_IN_LEN 16
 /* ??? */
@@ -2550,6 +2609,8 @@
  */
 #define MC_CMD_SET_ID_LED 0x2b
 
+#define MC_CMD_0x2b_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_SET_ID_LED_IN msgrequest */
 #define    MC_CMD_SET_ID_LED_IN_LEN 4
 /* Set LED state. */
@@ -2568,6 +2629,8 @@
  */
 #define MC_CMD_SET_MAC 0x2c
 
+#define MC_CMD_0x2c_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_SET_MAC_IN msgrequest */
 #define    MC_CMD_SET_MAC_IN_LEN 24
 /* The MTU is the MTU programmed directly into the XMAC/GMAC (inclusive of
@@ -2609,6 +2672,8 @@
  */
 #define MC_CMD_PHY_STATS 0x2d
 
+#define MC_CMD_0x2d_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_PHY_STATS_IN msgrequest */
 #define    MC_CMD_PHY_STATS_IN_LEN 8
 /* ??? */
@@ -2687,8 +2752,10 @@
  */
 #define MC_CMD_MAC_STATS 0x2e
 
+#define MC_CMD_0x2e_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_MAC_STATS_IN msgrequest */
-#define    MC_CMD_MAC_STATS_IN_LEN 16
+#define    MC_CMD_MAC_STATS_IN_LEN 20
 /* ??? */
 #define       MC_CMD_MAC_STATS_IN_DMA_ADDR_OFST 0
 #define       MC_CMD_MAC_STATS_IN_DMA_ADDR_LEN 8
@@ -2710,6 +2777,8 @@
 #define        MC_CMD_MAC_STATS_IN_PERIOD_MS_LBN 16
 #define        MC_CMD_MAC_STATS_IN_PERIOD_MS_WIDTH 16
 #define       MC_CMD_MAC_STATS_IN_DMA_LEN_OFST 12
+/* port id so vadapter stats can be provided */
+#define       MC_CMD_MAC_STATS_IN_PORT_ID_OFST 16
 
 /* MC_CMD_MAC_STATS_OUT_DMA msgresponse */
 #define    MC_CMD_MAC_STATS_OUT_DMA_LEN 0
@@ -2824,11 +2893,31 @@
 /* enum: RXDP counter: Number of times an emergency descriptor fetch was
  * performed. Valid for EF10 with PM_AND_RXDP_COUNTERS capability only.
  */
-#define          MC_CMD_MAC_RXDP_EMERGENCY_FETCH_CONDITIONS  0x47
+#define          MC_CMD_MAC_RXDP_HLB_FETCH_CONDITIONS  0x47
 /* enum: RXDP counter: Number of times the DPCPU waited for an existing
  * descriptor fetch. Valid for EF10 with PM_AND_RXDP_COUNTERS capability only.
  */
-#define          MC_CMD_MAC_RXDP_EMERGENCY_WAIT_CONDITIONS  0x48
+#define          MC_CMD_MAC_RXDP_HLB_WAIT_CONDITIONS  0x48
+#define          MC_CMD_MAC_VADAPTER_RX_DMABUF_START  0x4c /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_UNICAST_PACKETS  0x4c /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_UNICAST_BYTES  0x4d /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_MULTICAST_PACKETS  0x4e /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_MULTICAST_BYTES  0x4f /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_BROADCAST_PACKETS  0x50 /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_BROADCAST_BYTES  0x51 /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_BAD_PACKETS  0x52 /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_BAD_BYTES  0x53 /* enum */
+#define          MC_CMD_MAC_VADAPTER_RX_OVERFLOW  0x54 /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_DMABUF_START  0x57 /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_UNICAST_PACKETS  0x57 /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_UNICAST_BYTES  0x58 /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_MULTICAST_PACKETS  0x59 /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_MULTICAST_BYTES  0x5a /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_BROADCAST_PACKETS  0x5b /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_BROADCAST_BYTES  0x5c /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_BAD_PACKETS  0x5d /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_BAD_BYTES  0x5e /* enum */
+#define          MC_CMD_MAC_VADAPTER_TX_OVERFLOW  0x5f /* enum */
 /* enum: Start of GMAC stats buffer space, for Siena only. */
 #define          MC_CMD_GMAC_DMABUF_START  0x40
 /* enum: End of GMAC stats buffer space, for Siena only. */
@@ -2926,6 +3015,8 @@
  */
 #define MC_CMD_WOL_FILTER_SET 0x32
 
+#define MC_CMD_0x32_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_WOL_FILTER_SET_IN msgrequest */
 #define    MC_CMD_WOL_FILTER_SET_IN_LEN 192
 #define       MC_CMD_WOL_FILTER_SET_IN_FILTER_MODE_OFST 0
@@ -3020,6 +3111,8 @@
  */
 #define MC_CMD_WOL_FILTER_REMOVE 0x33
 
+#define MC_CMD_0x33_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_WOL_FILTER_REMOVE_IN msgrequest */
 #define    MC_CMD_WOL_FILTER_REMOVE_IN_LEN 4
 #define       MC_CMD_WOL_FILTER_REMOVE_IN_FILTER_ID_OFST 0
@@ -3035,6 +3128,8 @@
  */
 #define MC_CMD_WOL_FILTER_RESET 0x34
 
+#define MC_CMD_0x34_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_WOL_FILTER_RESET_IN msgrequest */
 #define    MC_CMD_WOL_FILTER_RESET_IN_LEN 4
 #define       MC_CMD_WOL_FILTER_RESET_IN_MASK_OFST 0
@@ -3069,6 +3164,8 @@
  */
 #define MC_CMD_NVRAM_TYPES 0x36
 
+#define MC_CMD_0x36_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_TYPES_IN msgrequest */
 #define    MC_CMD_NVRAM_TYPES_IN_LEN 0
 
@@ -3125,6 +3222,8 @@
  */
 #define MC_CMD_NVRAM_INFO 0x37
 
+#define MC_CMD_0x37_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_INFO_IN msgrequest */
 #define    MC_CMD_NVRAM_INFO_IN_LEN 4
 #define       MC_CMD_NVRAM_INFO_IN_TYPE_OFST 0
@@ -3157,6 +3256,8 @@
  */
 #define MC_CMD_NVRAM_UPDATE_START 0x38
 
+#define MC_CMD_0x38_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_UPDATE_START_IN msgrequest */
 #define    MC_CMD_NVRAM_UPDATE_START_IN_LEN 4
 #define       MC_CMD_NVRAM_UPDATE_START_IN_TYPE_OFST 0
@@ -3175,6 +3276,8 @@
  */
 #define MC_CMD_NVRAM_READ 0x39
 
+#define MC_CMD_0x39_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_READ_IN msgrequest */
 #define    MC_CMD_NVRAM_READ_IN_LEN 12
 #define       MC_CMD_NVRAM_READ_IN_TYPE_OFST 0
@@ -3202,6 +3305,8 @@
  */
 #define MC_CMD_NVRAM_WRITE 0x3a
 
+#define MC_CMD_0x3a_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_WRITE_IN msgrequest */
 #define    MC_CMD_NVRAM_WRITE_IN_LENMIN 13
 #define    MC_CMD_NVRAM_WRITE_IN_LENMAX 252
@@ -3228,6 +3333,8 @@
  */
 #define MC_CMD_NVRAM_ERASE 0x3b
 
+#define MC_CMD_0x3b_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_ERASE_IN msgrequest */
 #define    MC_CMD_NVRAM_ERASE_IN_LEN 12
 #define       MC_CMD_NVRAM_ERASE_IN_TYPE_OFST 0
@@ -3248,6 +3355,8 @@
  */
 #define MC_CMD_NVRAM_UPDATE_FINISH 0x3c
 
+#define MC_CMD_0x3c_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_UPDATE_FINISH_IN msgrequest */
 #define    MC_CMD_NVRAM_UPDATE_FINISH_IN_LEN 8
 #define       MC_CMD_NVRAM_UPDATE_FINISH_IN_TYPE_OFST 0
@@ -3279,6 +3388,8 @@
  */
 #define MC_CMD_REBOOT 0x3d
 
+#define MC_CMD_0x3d_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_REBOOT_IN msgrequest */
 #define    MC_CMD_REBOOT_IN_LEN 4
 #define       MC_CMD_REBOOT_IN_FLAGS_OFST 0
@@ -3316,6 +3427,8 @@
  */
 #define MC_CMD_REBOOT_MODE 0x3f
 
+#define MC_CMD_0x3f_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_REBOOT_MODE_IN msgrequest */
 #define    MC_CMD_REBOOT_MODE_IN_LEN 4
 #define       MC_CMD_REBOOT_MODE_IN_VALUE_OFST 0
@@ -3368,6 +3481,8 @@
  */
 #define MC_CMD_SENSOR_INFO 0x41
 
+#define MC_CMD_0x41_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SENSOR_INFO_IN msgrequest */
 #define    MC_CMD_SENSOR_INFO_IN_LEN 0
 
@@ -3542,6 +3657,8 @@
  */
 #define MC_CMD_READ_SENSORS 0x42
 
+#define MC_CMD_0x42_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_READ_SENSORS_IN msgrequest */
 #define    MC_CMD_READ_SENSORS_IN_LEN 8
 /* DMA address of host buffer for sensor readings (must be 4Kbyte aligned). */
@@ -3602,6 +3719,8 @@
  */
 #define MC_CMD_GET_PHY_STATE 0x43
 
+#define MC_CMD_0x43_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_PHY_STATE_IN msgrequest */
 #define    MC_CMD_GET_PHY_STATE_IN_LEN 0
 
@@ -3636,6 +3755,8 @@
  */
 #define MC_CMD_WOL_FILTER_GET 0x45
 
+#define MC_CMD_0x45_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_WOL_FILTER_GET_IN msgrequest */
 #define    MC_CMD_WOL_FILTER_GET_IN_LEN 0
 
@@ -3651,6 +3772,8 @@
  */
 #define MC_CMD_ADD_LIGHTSOUT_OFFLOAD 0x46
 
+#define MC_CMD_0x46_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_ADD_LIGHTSOUT_OFFLOAD_IN msgrequest */
 #define    MC_CMD_ADD_LIGHTSOUT_OFFLOAD_IN_LENMIN 8
 #define    MC_CMD_ADD_LIGHTSOUT_OFFLOAD_IN_LENMAX 252
@@ -3692,6 +3815,8 @@
  */
 #define MC_CMD_REMOVE_LIGHTSOUT_OFFLOAD 0x47
 
+#define MC_CMD_0x47_PRIVILEGE_CTG SRIOV_CTG_LINK
+
 /* MC_CMD_REMOVE_LIGHTSOUT_OFFLOAD_IN msgrequest */
 #define    MC_CMD_REMOVE_LIGHTSOUT_OFFLOAD_IN_LEN 8
 #define       MC_CMD_REMOVE_LIGHTSOUT_OFFLOAD_IN_PROTOCOL_OFST 0
@@ -3722,6 +3847,8 @@
  */
 #define MC_CMD_TESTASSERT 0x49
 
+#define MC_CMD_0x49_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_TESTASSERT_IN msgrequest */
 #define    MC_CMD_TESTASSERT_IN_LEN 0
 
@@ -3739,6 +3866,8 @@
  */
 #define MC_CMD_WORKAROUND 0x4a
 
+#define MC_CMD_0x4a_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_WORKAROUND_IN msgrequest */
 #define    MC_CMD_WORKAROUND_IN_LEN 8
 #define       MC_CMD_WORKAROUND_IN_TYPE_OFST 0
@@ -3765,6 +3894,8 @@
  */
 #define MC_CMD_GET_PHY_MEDIA_INFO 0x4b
 
+#define MC_CMD_0x4b_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_GET_PHY_MEDIA_INFO_IN msgrequest */
 #define    MC_CMD_GET_PHY_MEDIA_INFO_IN_LEN 4
 #define       MC_CMD_GET_PHY_MEDIA_INFO_IN_PAGE_OFST 0
@@ -3788,6 +3919,8 @@
  */
 #define MC_CMD_NVRAM_TEST 0x4c
 
+#define MC_CMD_0x4c_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_TEST_IN msgrequest */
 #define    MC_CMD_NVRAM_TEST_IN_LEN 4
 #define       MC_CMD_NVRAM_TEST_IN_TYPE_OFST 0
@@ -3849,6 +3982,8 @@
  */
 #define MC_CMD_SENSOR_SET_LIMS 0x4e
 
+#define MC_CMD_0x4e_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SENSOR_SET_LIMS_IN msgrequest */
 #define    MC_CMD_SENSOR_SET_LIMS_IN_LEN 20
 #define       MC_CMD_SENSOR_SET_LIMS_IN_SENSOR_OFST 0
@@ -3890,6 +4025,8 @@
  */
 #define MC_CMD_NVRAM_PARTITIONS 0x51
 
+#define MC_CMD_0x51_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_PARTITIONS_IN msgrequest */
 #define    MC_CMD_NVRAM_PARTITIONS_IN_LEN 0
 
@@ -3913,6 +4050,8 @@
  */
 #define MC_CMD_NVRAM_METADATA 0x52
 
+#define MC_CMD_0x52_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_NVRAM_METADATA_IN msgrequest */
 #define    MC_CMD_NVRAM_METADATA_IN_LEN 4
 /* Partition type ID code */
@@ -3958,6 +4097,8 @@
  */
 #define MC_CMD_GET_MAC_ADDRESSES 0x55
 
+#define MC_CMD_0x55_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_MAC_ADDRESSES_IN msgrequest */
 #define    MC_CMD_GET_MAC_ADDRESSES_IN_LEN 0
 
@@ -4087,11 +4228,66 @@
 
 
 /***********************************/
+/* MC_CMD_GET_WORKAROUNDS
+ * Read the list of all implemented and all currently enabled workarounds. The
+ * enums here must correspond with those in MC_CMD_WORKAROUND.
+ */
+#define MC_CMD_GET_WORKAROUNDS 0x59
+
+/* MC_CMD_GET_WORKAROUNDS_OUT msgresponse */
+#define    MC_CMD_GET_WORKAROUNDS_OUT_LEN 8
+/* Each workaround is represented by a single bit according to the enums below.
+ */
+#define       MC_CMD_GET_WORKAROUNDS_OUT_IMPLEMENTED_OFST 0
+#define       MC_CMD_GET_WORKAROUNDS_OUT_ENABLED_OFST 4
+/* enum: Bug 17230 work around. */
+#define          MC_CMD_GET_WORKAROUNDS_OUT_BUG17230 0x2
+/* enum: Bug 35388 work around (unsafe EVQ writes). */
+#define          MC_CMD_GET_WORKAROUNDS_OUT_BUG35388 0x4
+/* enum: Bug35017 workaround (A64 tables must be identity map) */
+#define          MC_CMD_GET_WORKAROUNDS_OUT_BUG35017 0x8
+
+
+/***********************************/
+/* MC_CMD_LINK_STATE_MODE
+ * Read/set link state mode of a VF
+ */
+#define MC_CMD_LINK_STATE_MODE 0x5c
+
+#define MC_CMD_0x5c_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
+/* MC_CMD_LINK_STATE_MODE_IN msgrequest */
+#define    MC_CMD_LINK_STATE_MODE_IN_LEN 8
+/* The target function to have its link state mode read or set, must be a VF
+ * e.g. VF 1,3 = 0x00030001
+ */
+#define       MC_CMD_LINK_STATE_MODE_IN_FUNCTION_OFST 0
+#define        MC_CMD_LINK_STATE_MODE_IN_FUNCTION_PF_LBN 0
+#define        MC_CMD_LINK_STATE_MODE_IN_FUNCTION_PF_WIDTH 16
+#define        MC_CMD_LINK_STATE_MODE_IN_FUNCTION_VF_LBN 16
+#define        MC_CMD_LINK_STATE_MODE_IN_FUNCTION_VF_WIDTH 16
+/* New link state mode to be set */
+#define       MC_CMD_LINK_STATE_MODE_IN_NEW_MODE_OFST 4
+#define          MC_CMD_LINK_STATE_MODE_IN_LINK_STATE_AUTO       0x0 /* enum */
+#define          MC_CMD_LINK_STATE_MODE_IN_LINK_STATE_UP         0x1 /* enum */
+#define          MC_CMD_LINK_STATE_MODE_IN_LINK_STATE_DOWN       0x2 /* enum */
+/* enum: Use this value to just read the existing setting without modifying it.
+ */
+#define          MC_CMD_LINK_STATE_MODE_IN_DO_NOT_CHANGE         0xffffffff
+
+/* MC_CMD_LINK_STATE_MODE_OUT msgresponse */
+#define    MC_CMD_LINK_STATE_MODE_OUT_LEN 4
+#define       MC_CMD_LINK_STATE_MODE_OUT_OLD_MODE_OFST 0
+
+
+/***********************************/
 /* MC_CMD_READ_REGS
  * Get a dump of the MCPU registers
  */
 #define MC_CMD_READ_REGS 0x50
 
+#define MC_CMD_0x50_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_READ_REGS_IN msgrequest */
 #define    MC_CMD_READ_REGS_IN_LEN 0
 
@@ -4115,6 +4311,8 @@
  */
 #define MC_CMD_INIT_EVQ 0x80
 
+#define MC_CMD_0x80_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_INIT_EVQ_IN msgrequest */
 #define    MC_CMD_INIT_EVQ_IN_LENMIN 44
 #define    MC_CMD_INIT_EVQ_IN_LENMAX 548
@@ -4213,6 +4411,8 @@
  */
 #define MC_CMD_INIT_RXQ 0x81
 
+#define MC_CMD_0x81_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_INIT_RXQ_IN msgrequest */
 #define    MC_CMD_INIT_RXQ_IN_LENMIN 36
 #define    MC_CMD_INIT_RXQ_IN_LENMAX 252
@@ -4265,6 +4465,8 @@
  */
 #define MC_CMD_INIT_TXQ 0x82
 
+#define MC_CMD_0x82_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_INIT_TXQ_IN msgrequest */
 #define    MC_CMD_INIT_TXQ_IN_LENMIN 36
 #define    MC_CMD_INIT_TXQ_IN_LENMAX 252
@@ -4322,6 +4524,8 @@
  */
 #define MC_CMD_FINI_EVQ 0x83
 
+#define MC_CMD_0x83_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_FINI_EVQ_IN msgrequest */
 #define    MC_CMD_FINI_EVQ_IN_LEN 4
 /* Instance of EVQ to destroy. Should be the same instance as that previously
@@ -4339,6 +4543,8 @@
  */
 #define MC_CMD_FINI_RXQ 0x84
 
+#define MC_CMD_0x84_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_FINI_RXQ_IN msgrequest */
 #define    MC_CMD_FINI_RXQ_IN_LEN 4
 /* Instance of RXQ to destroy */
@@ -4354,6 +4560,8 @@
  */
 #define MC_CMD_FINI_TXQ 0x85
 
+#define MC_CMD_0x85_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_FINI_TXQ_IN msgrequest */
 #define    MC_CMD_FINI_TXQ_IN_LEN 4
 /* Instance of TXQ to destroy */
@@ -4369,6 +4577,8 @@
  */
 #define MC_CMD_DRIVER_EVENT 0x86
 
+#define MC_CMD_0x86_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_DRIVER_EVENT_IN msgrequest */
 #define    MC_CMD_DRIVER_EVENT_IN_LEN 12
 /* Handle of target EVQ */
@@ -4392,6 +4602,8 @@
  */
 #define MC_CMD_PROXY_CMD 0x5b
 
+#define MC_CMD_0x5b_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_PROXY_CMD_IN msgrequest */
 #define    MC_CMD_PROXY_CMD_IN_LEN 4
 /* The handle of the target function. */
@@ -4414,6 +4626,8 @@
  */
 #define MC_CMD_ALLOC_BUFTBL_CHUNK 0x87
 
+#define MC_CMD_0x87_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_ALLOC_BUFTBL_CHUNK_IN msgrequest */
 #define    MC_CMD_ALLOC_BUFTBL_CHUNK_IN_LEN 8
 /* Owner ID to use */
@@ -4437,6 +4651,8 @@
  */
 #define MC_CMD_PROGRAM_BUFTBL_ENTRIES 0x88
 
+#define MC_CMD_0x88_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_PROGRAM_BUFTBL_ENTRIES_IN msgrequest */
 #define    MC_CMD_PROGRAM_BUFTBL_ENTRIES_IN_LENMIN 20
 #define    MC_CMD_PROGRAM_BUFTBL_ENTRIES_IN_LENMAX 268
@@ -4463,6 +4679,8 @@
  */
 #define MC_CMD_FREE_BUFTBL_CHUNK 0x89
 
+#define MC_CMD_0x89_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_FREE_BUFTBL_CHUNK_IN msgrequest */
 #define    MC_CMD_FREE_BUFTBL_CHUNK_IN_LEN 4
 #define       MC_CMD_FREE_BUFTBL_CHUNK_IN_HANDLE_OFST 0
@@ -4477,6 +4695,8 @@
  */
 #define MC_CMD_FILTER_OP 0x8a
 
+#define MC_CMD_0x8a_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_FILTER_OP_IN msgrequest */
 #define    MC_CMD_FILTER_OP_IN_LEN 108
 /* identifies the type of operation requested */
@@ -4637,6 +4857,8 @@
  */
 #define MC_CMD_GET_PARSER_DISP_INFO 0xe4
 
+#define MC_CMD_0xe4_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_PARSER_DISP_INFO_IN msgrequest */
 #define    MC_CMD_GET_PARSER_DISP_INFO_IN_LEN 4
 /* identifies the type of operation requested */
@@ -4669,6 +4891,8 @@
  */
 #define MC_CMD_PARSER_DISP_RW 0xe5
 
+#define MC_CMD_0xe5_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_PARSER_DISP_RW_IN msgrequest */
 #define    MC_CMD_PARSER_DISP_RW_IN_LEN 32
 /* identifies the target of the operation */
@@ -4719,6 +4943,8 @@
  */
 #define MC_CMD_GET_PF_COUNT 0xb6
 
+#define MC_CMD_0xb6_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_PF_COUNT_IN msgrequest */
 #define    MC_CMD_GET_PF_COUNT_IN_LEN 0
 
@@ -4750,6 +4976,8 @@
  */
 #define MC_CMD_GET_PORT_ASSIGNMENT 0xb8
 
+#define MC_CMD_0xb8_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_PORT_ASSIGNMENT_IN msgrequest */
 #define    MC_CMD_GET_PORT_ASSIGNMENT_IN_LEN 0
 
@@ -4765,6 +4993,8 @@
  */
 #define MC_CMD_SET_PORT_ASSIGNMENT 0xb9
 
+#define MC_CMD_0xb9_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_PORT_ASSIGNMENT_IN msgrequest */
 #define    MC_CMD_SET_PORT_ASSIGNMENT_IN_LEN 4
 /* Identifies the port assignment for this function. */
@@ -4780,6 +5010,8 @@
  */
 #define MC_CMD_ALLOC_VIS 0x8b
 
+#define MC_CMD_0x8b_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_ALLOC_VIS_IN msgrequest */
 #define    MC_CMD_ALLOC_VIS_IN_LEN 8
 /* The minimum number of VIs that is acceptable */
@@ -4804,6 +5036,8 @@
  */
 #define MC_CMD_FREE_VIS 0x8c
 
+#define MC_CMD_0x8c_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_FREE_VIS_IN msgrequest */
 #define    MC_CMD_FREE_VIS_IN_LEN 0
 
@@ -4817,6 +5051,8 @@
  */
 #define MC_CMD_GET_SRIOV_CFG 0xba
 
+#define MC_CMD_0xba_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_SRIOV_CFG_IN msgrequest */
 #define    MC_CMD_GET_SRIOV_CFG_IN_LEN 0
 
@@ -4841,6 +5077,8 @@
  */
 #define MC_CMD_SET_SRIOV_CFG 0xbb
 
+#define MC_CMD_0xbb_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_SRIOV_CFG_IN msgrequest */
 #define    MC_CMD_SET_SRIOV_CFG_IN_LEN 20
 /* Number of VFs currently enabled. */
@@ -4870,6 +5108,8 @@
  */
 #define MC_CMD_GET_VI_ALLOC_INFO 0x8d
 
+#define MC_CMD_0x8d_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_VI_ALLOC_INFO_IN msgrequest */
 #define    MC_CMD_GET_VI_ALLOC_INFO_IN_LEN 0
 
@@ -4889,6 +5129,8 @@
  */
 #define MC_CMD_DUMP_VI_STATE 0x8e
 
+#define MC_CMD_0x8e_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_DUMP_VI_STATE_IN msgrequest */
 #define    MC_CMD_DUMP_VI_STATE_IN_LEN 4
 /* The VI number to query. */
@@ -4998,6 +5240,8 @@
  */
 #define MC_CMD_ALLOC_PIOBUF 0x8f
 
+#define MC_CMD_0x8f_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_ALLOC_PIOBUF_IN msgrequest */
 #define    MC_CMD_ALLOC_PIOBUF_IN_LEN 0
 
@@ -5013,6 +5257,8 @@
  */
 #define MC_CMD_FREE_PIOBUF 0x90
 
+#define MC_CMD_0x90_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_FREE_PIOBUF_IN msgrequest */
 #define    MC_CMD_FREE_PIOBUF_IN_LEN 4
 /* Handle for allocated push I/O buffer. */
@@ -5028,6 +5274,8 @@
  */
 #define MC_CMD_GET_VI_TLP_PROCESSING 0xb0
 
+#define MC_CMD_0xb0_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_VI_TLP_PROCESSING_IN msgrequest */
 #define    MC_CMD_GET_VI_TLP_PROCESSING_IN_LEN 4
 /* VI number to get information for. */
@@ -5062,6 +5310,8 @@
  */
 #define MC_CMD_SET_VI_TLP_PROCESSING 0xb1
 
+#define MC_CMD_0xb1_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_SET_VI_TLP_PROCESSING_IN msgrequest */
 #define    MC_CMD_SET_VI_TLP_PROCESSING_IN_LEN 8
 /* VI number to set information for. */
@@ -5096,6 +5346,8 @@
  */
 #define MC_CMD_GET_TLP_PROCESSING_GLOBALS 0xbc
 
+#define MC_CMD_0xbc_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_GET_TLP_PROCESSING_GLOBALS_IN msgrequest */
 #define    MC_CMD_GET_TLP_PROCESSING_GLOBALS_IN_LEN 4
 #define       MC_CMD_GET_TLP_PROCESSING_GLOBALS_IN_TLP_GLOBAL_CATEGORY_OFST 0
@@ -5157,6 +5409,8 @@
  */
 #define MC_CMD_SET_TLP_PROCESSING_GLOBALS 0xbd
 
+#define MC_CMD_0xbd_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_TLP_PROCESSING_GLOBALS_IN msgrequest */
 #define    MC_CMD_SET_TLP_PROCESSING_GLOBALS_IN_LEN 8
 #define       MC_CMD_SET_TLP_PROCESSING_GLOBALS_IN_TLP_GLOBAL_CATEGORY_OFST 0
@@ -5203,6 +5457,8 @@
  */
 #define MC_CMD_SATELLITE_DOWNLOAD 0x91
 
+#define MC_CMD_0x91_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SATELLITE_DOWNLOAD_IN msgrequest: The reset requirements for the CPUs
  * are subtle, and so downloads must proceed in a number of phases.
  *
@@ -5318,6 +5574,7 @@
  */
 #define MC_CMD_GET_CAPABILITIES 0xbe
 
+#define MC_CMD_0xbe_PRIVILEGE_CTG SRIOV_CTG_GENERAL
 /* MC_CMD_GET_CAPABILITIES_IN msgrequest */
 #define    MC_CMD_GET_CAPABILITIES_IN_LEN 0
 
@@ -5343,6 +5600,8 @@
 #define        MC_CMD_GET_CAPABILITIES_OUT_MCAST_FILTER_CHAINING_WIDTH 1
 #define        MC_CMD_GET_CAPABILITIES_OUT_PM_AND_RXDP_COUNTERS_LBN 27
 #define        MC_CMD_GET_CAPABILITIES_OUT_PM_AND_RXDP_COUNTERS_WIDTH 1
+#define        MC_CMD_GET_CAPABILITIES_OUT_EVB_LBN 30
+#define        MC_CMD_GET_CAPABILITIES_OUT_EVB_WIDTH 1
 /* RxDPCPU firmware id. */
 #define       MC_CMD_GET_CAPABILITIES_OUT_RX_DPCPU_FW_ID_OFST 4
 #define       MC_CMD_GET_CAPABILITIES_OUT_RX_DPCPU_FW_ID_LEN 2
@@ -5433,6 +5692,8 @@
  */
 #define MC_CMD_TCM_BUCKET_ALLOC 0xb2
 
+#define MC_CMD_0xb2_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_TCM_BUCKET_ALLOC_IN msgrequest */
 #define    MC_CMD_TCM_BUCKET_ALLOC_IN_LEN 0
 
@@ -5448,6 +5709,8 @@
  */
 #define MC_CMD_TCM_BUCKET_FREE 0xb3
 
+#define MC_CMD_0xb3_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_TCM_BUCKET_FREE_IN msgrequest */
 #define    MC_CMD_TCM_BUCKET_FREE_IN_LEN 4
 /* the bucket id */
@@ -5463,6 +5726,8 @@
  */
 #define MC_CMD_TCM_BUCKET_INIT 0xb4
 
+#define MC_CMD_0xb4_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_TCM_BUCKET_INIT_IN msgrequest */
 #define    MC_CMD_TCM_BUCKET_INIT_IN_LEN 8
 /* the bucket id */
@@ -5480,6 +5745,8 @@
  */
 #define MC_CMD_TCM_TXQ_INIT 0xb5
 
+#define MC_CMD_0xb5_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_TCM_TXQ_INIT_IN msgrequest */
 #define    MC_CMD_TCM_TXQ_INIT_IN_LEN 28
 /* the txq id */
@@ -5511,6 +5778,8 @@
  */
 #define MC_CMD_LINK_PIOBUF 0x92
 
+#define MC_CMD_0x92_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_LINK_PIOBUF_IN msgrequest */
 #define    MC_CMD_LINK_PIOBUF_IN_LEN 8
 /* Handle for allocated push I/O buffer. */
@@ -5528,6 +5797,8 @@
  */
 #define MC_CMD_UNLINK_PIOBUF 0x93
 
+#define MC_CMD_0x93_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_UNLINK_PIOBUF_IN msgrequest */
 #define    MC_CMD_UNLINK_PIOBUF_IN_LEN 4
 /* Function Local Instance (VI) number. */
@@ -5543,6 +5814,8 @@
  */
 #define MC_CMD_VSWITCH_ALLOC 0x94
 
+#define MC_CMD_0x94_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VSWITCH_ALLOC_IN msgrequest */
 #define    MC_CMD_VSWITCH_ALLOC_IN_LEN 16
 /* The port to connect to the v-switch's upstream port. */
@@ -5572,6 +5845,8 @@
  */
 #define MC_CMD_VSWITCH_FREE 0x95
 
+#define MC_CMD_0x95_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VSWITCH_FREE_IN msgrequest */
 #define    MC_CMD_VSWITCH_FREE_IN_LEN 4
 /* The port to which the v-switch is connected. */
@@ -5587,6 +5862,8 @@
  */
 #define MC_CMD_VPORT_ALLOC 0x96
 
+#define MC_CMD_0x96_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VPORT_ALLOC_IN msgrequest */
 #define    MC_CMD_VPORT_ALLOC_IN_LEN 20
 /* The port to which the v-switch is connected. */
@@ -5636,6 +5913,8 @@
  */
 #define MC_CMD_VPORT_FREE 0x97
 
+#define MC_CMD_0x97_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VPORT_FREE_IN msgrequest */
 #define    MC_CMD_VPORT_FREE_IN_LEN 4
 /* The handle of the v-port */
@@ -5651,8 +5930,10 @@
  */
 #define MC_CMD_VADAPTOR_ALLOC 0x98
 
+#define MC_CMD_0x98_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VADAPTOR_ALLOC_IN msgrequest */
-#define    MC_CMD_VADAPTOR_ALLOC_IN_LEN 16
+#define    MC_CMD_VADAPTOR_ALLOC_IN_LEN 30
 /* The port to connect to the v-adaptor's port. */
 #define       MC_CMD_VADAPTOR_ALLOC_IN_UPSTREAM_PORT_ID_OFST 0
 /* Flags controlling v-adaptor creation */
@@ -5661,6 +5942,19 @@
 #define        MC_CMD_VADAPTOR_ALLOC_IN_FLAG_AUTO_VADAPTOR_WIDTH 1
 /* The number of VLAN tags to strip on receive */
 #define       MC_CMD_VADAPTOR_ALLOC_IN_NUM_VLANS_OFST 12
+/* The number of VLAN tags to transparently insert/remove. */
+#define       MC_CMD_VADAPTOR_ALLOC_IN_NUM_VLAN_TAGS_OFST 16
+/* The actual VLAN tags to insert/remove */
+#define       MC_CMD_VADAPTOR_ALLOC_IN_VLAN_TAGS_OFST 20
+#define        MC_CMD_VADAPTOR_ALLOC_IN_VLAN_TAG_0_LBN 0
+#define        MC_CMD_VADAPTOR_ALLOC_IN_VLAN_TAG_0_WIDTH 16
+#define        MC_CMD_VADAPTOR_ALLOC_IN_VLAN_TAG_1_LBN 16
+#define        MC_CMD_VADAPTOR_ALLOC_IN_VLAN_TAG_1_WIDTH 16
+/* The MAC address to assign to this v-adaptor */
+#define       MC_CMD_VADAPTOR_ALLOC_IN_MACADDR_OFST 24
+#define       MC_CMD_VADAPTOR_ALLOC_IN_MACADDR_LEN 6
+/* enum: Derive the MAC address from the upstream port */
+#define          MC_CMD_VADAPTOR_ALLOC_IN_AUTO_MAC  0x0
 
 /* MC_CMD_VADAPTOR_ALLOC_OUT msgresponse */
 #define    MC_CMD_VADAPTOR_ALLOC_OUT_LEN 0
@@ -5672,6 +5966,8 @@
  */
 #define MC_CMD_VADAPTOR_FREE 0x99
 
+#define MC_CMD_0x99_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VADAPTOR_FREE_IN msgrequest */
 #define    MC_CMD_VADAPTOR_FREE_IN_LEN 4
 /* The port to which the v-adaptor is connected. */
@@ -5682,11 +5978,53 @@
 
 
 /***********************************/
+/* MC_CMD_VADAPTOR_SET_MAC
+ * assign a new MAC address to a v-adaptor.
+ */
+#define MC_CMD_VADAPTOR_SET_MAC 0x5d
+
+#define MC_CMD_0x5d_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
+/* MC_CMD_VADAPTOR_SET_MAC_IN msgrequest */
+#define    MC_CMD_VADAPTOR_SET_MAC_IN_LEN 10
+/* The port to which the v-adaptor is connected. */
+#define       MC_CMD_VADAPTOR_SET_MAC_IN_UPSTREAM_PORT_ID_OFST 0
+/* The new MAC address to assign to this v-adaptor */
+#define       MC_CMD_VADAPTOR_SET_MAC_IN_MACADDR_OFST 4
+#define       MC_CMD_VADAPTOR_SET_MAC_IN_MACADDR_LEN 6
+
+/* MC_CMD_VADAPTOR_SET_MAC_OUT msgresponse */
+#define    MC_CMD_VADAPTOR_SET_MAC_OUT_LEN 0
+
+
+/***********************************/
+/* MC_CMD_VADAPTOR_GET_MAC
+ * read the MAC address assigned to a v-adaptor.
+ */
+#define MC_CMD_VADAPTOR_GET_MAC 0x5e
+
+#define MC_CMD_0x5e_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
+/* MC_CMD_VADAPTOR_GET_MAC_IN msgrequest */
+#define    MC_CMD_VADAPTOR_GET_MAC_IN_LEN 4
+/* The port to which the v-adaptor is connected. */
+#define       MC_CMD_VADAPTOR_GET_MAC_IN_UPSTREAM_PORT_ID_OFST 0
+
+/* MC_CMD_VADAPTOR_GET_MAC_OUT msgresponse */
+#define    MC_CMD_VADAPTOR_GET_MAC_OUT_LEN 6
+/* The MAC address assigned to this v-adaptor */
+#define       MC_CMD_VADAPTOR_GET_MAC_OUT_MACADDR_OFST 0
+#define       MC_CMD_VADAPTOR_GET_MAC_OUT_MACADDR_LEN 6
+
+
+/***********************************/
 /* MC_CMD_EVB_PORT_ASSIGN
  * assign a port to a PCI function.
  */
 #define MC_CMD_EVB_PORT_ASSIGN 0x9a
 
+#define MC_CMD_0x9a_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_EVB_PORT_ASSIGN_IN msgrequest */
 #define    MC_CMD_EVB_PORT_ASSIGN_IN_LEN 8
 /* The port to assign. */
@@ -5708,6 +6046,8 @@
  */
 #define MC_CMD_RDWR_A64_REGIONS 0x9b
 
+#define MC_CMD_0x9b_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_RDWR_A64_REGIONS_IN msgrequest */
 #define    MC_CMD_RDWR_A64_REGIONS_IN_LEN 17
 #define       MC_CMD_RDWR_A64_REGIONS_IN_REGION0_OFST 0
@@ -5736,6 +6076,8 @@
  */
 #define MC_CMD_ONLOAD_STACK_ALLOC 0x9c
 
+#define MC_CMD_0x9c_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_ONLOAD_STACK_ALLOC_IN msgrequest */
 #define    MC_CMD_ONLOAD_STACK_ALLOC_IN_LEN 4
 /* The handle of the owning upstream port */
@@ -5753,6 +6095,8 @@
  */
 #define MC_CMD_ONLOAD_STACK_FREE 0x9d
 
+#define MC_CMD_0x9d_PRIVILEGE_CTG SRIOV_CTG_ONLOAD
+
 /* MC_CMD_ONLOAD_STACK_FREE_IN msgrequest */
 #define    MC_CMD_ONLOAD_STACK_FREE_IN_LEN 4
 /* The handle of the Onload stack */
@@ -5768,6 +6112,8 @@
  */
 #define MC_CMD_RSS_CONTEXT_ALLOC 0x9e
 
+#define MC_CMD_0x9e_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_RSS_CONTEXT_ALLOC_IN msgrequest */
 #define    MC_CMD_RSS_CONTEXT_ALLOC_IN_LEN 12
 /* The handle of the owning upstream port */
@@ -5800,6 +6146,8 @@
  */
 #define MC_CMD_RSS_CONTEXT_FREE 0x9f
 
+#define MC_CMD_0x9f_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_RSS_CONTEXT_FREE_IN msgrequest */
 #define    MC_CMD_RSS_CONTEXT_FREE_IN_LEN 4
 /* The handle of the RSS context */
@@ -5815,6 +6163,8 @@
  */
 #define MC_CMD_RSS_CONTEXT_SET_KEY 0xa0
 
+#define MC_CMD_0xa0_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_RSS_CONTEXT_SET_KEY_IN msgrequest */
 #define    MC_CMD_RSS_CONTEXT_SET_KEY_IN_LEN 44
 /* The handle of the RSS context */
@@ -5833,6 +6183,8 @@
  */
 #define MC_CMD_RSS_CONTEXT_GET_KEY 0xa1
 
+#define MC_CMD_0xa1_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_RSS_CONTEXT_GET_KEY_IN msgrequest */
 #define    MC_CMD_RSS_CONTEXT_GET_KEY_IN_LEN 4
 /* The handle of the RSS context */
@@ -5851,6 +6203,8 @@
  */
 #define MC_CMD_RSS_CONTEXT_SET_TABLE 0xa2
 
+#define MC_CMD_0xa2_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_RSS_CONTEXT_SET_TABLE_IN msgrequest */
 #define    MC_CMD_RSS_CONTEXT_SET_TABLE_IN_LEN 132
 /* The handle of the RSS context */
@@ -5869,6 +6223,8 @@
  */
 #define MC_CMD_RSS_CONTEXT_GET_TABLE 0xa3
 
+#define MC_CMD_0xa3_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_RSS_CONTEXT_GET_TABLE_IN msgrequest */
 #define    MC_CMD_RSS_CONTEXT_GET_TABLE_IN_LEN 4
 /* The handle of the RSS context */
@@ -5887,6 +6243,8 @@
  */
 #define MC_CMD_RSS_CONTEXT_SET_FLAGS 0xe1
 
+#define MC_CMD_0xe1_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_RSS_CONTEXT_SET_FLAGS_IN msgrequest */
 #define    MC_CMD_RSS_CONTEXT_SET_FLAGS_IN_LEN 8
 /* The handle of the RSS context */
@@ -5912,6 +6270,8 @@
  */
 #define MC_CMD_RSS_CONTEXT_GET_FLAGS 0xe2
 
+#define MC_CMD_0xe2_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_RSS_CONTEXT_GET_FLAGS_IN msgrequest */
 #define    MC_CMD_RSS_CONTEXT_GET_FLAGS_IN_LEN 4
 /* The handle of the RSS context */
@@ -5937,6 +6297,8 @@
  */
 #define MC_CMD_DOT1P_MAPPING_ALLOC 0xa4
 
+#define MC_CMD_0xa4_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DOT1P_MAPPING_ALLOC_IN msgrequest */
 #define    MC_CMD_DOT1P_MAPPING_ALLOC_IN_LEN 8
 /* The handle of the owning upstream port */
@@ -5959,6 +6321,8 @@
  */
 #define MC_CMD_DOT1P_MAPPING_FREE 0xa5
 
+#define MC_CMD_0xa5_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DOT1P_MAPPING_FREE_IN msgrequest */
 #define    MC_CMD_DOT1P_MAPPING_FREE_IN_LEN 4
 /* The handle of the .1p mapping */
@@ -5974,6 +6338,8 @@
  */
 #define MC_CMD_DOT1P_MAPPING_SET_TABLE 0xa6
 
+#define MC_CMD_0xa6_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DOT1P_MAPPING_SET_TABLE_IN msgrequest */
 #define    MC_CMD_DOT1P_MAPPING_SET_TABLE_IN_LEN 36
 /* The handle of the .1p mapping */
@@ -5994,6 +6360,8 @@
  */
 #define MC_CMD_DOT1P_MAPPING_GET_TABLE 0xa7
 
+#define MC_CMD_0xa7_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DOT1P_MAPPING_GET_TABLE_IN msgrequest */
 #define    MC_CMD_DOT1P_MAPPING_GET_TABLE_IN_LEN 4
 /* The handle of the .1p mapping */
@@ -6014,6 +6382,8 @@
  */
 #define MC_CMD_GET_VECTOR_CFG 0xbf
 
+#define MC_CMD_0xbf_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_VECTOR_CFG_IN msgrequest */
 #define    MC_CMD_GET_VECTOR_CFG_IN_LEN 0
 
@@ -6033,6 +6403,8 @@
  */
 #define MC_CMD_SET_VECTOR_CFG 0xc0
 
+#define MC_CMD_0xc0_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_SET_VECTOR_CFG_IN msgrequest */
 #define    MC_CMD_SET_VECTOR_CFG_IN_LEN 12
 /* Base absolute interrupt vector number, or MC_CMD_RESOURCE_INSTANCE_ANY to
@@ -6423,6 +6795,8 @@
  */
 #define MC_CMD_VPORT_ADD_MAC_ADDRESS 0xa8
 
+#define MC_CMD_0xa8_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VPORT_ADD_MAC_ADDRESS_IN msgrequest */
 #define    MC_CMD_VPORT_ADD_MAC_ADDRESS_IN_LEN 10
 /* The handle of the v-port */
@@ -6441,6 +6815,8 @@
  */
 #define MC_CMD_VPORT_DEL_MAC_ADDRESS 0xa9
 
+#define MC_CMD_0xa9_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VPORT_DEL_MAC_ADDRESS_IN msgrequest */
 #define    MC_CMD_VPORT_DEL_MAC_ADDRESS_IN_LEN 10
 /* The handle of the v-port */
@@ -6459,6 +6835,8 @@
  */
 #define MC_CMD_VPORT_GET_MAC_ADDRESSES 0xaa
 
+#define MC_CMD_0xaa_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_VPORT_GET_MAC_ADDRESSES_IN msgrequest */
 #define    MC_CMD_VPORT_GET_MAC_ADDRESSES_IN_LEN 4
 /* The handle of the v-port */
@@ -6486,6 +6864,8 @@
  */
 #define MC_CMD_DUMP_BUFTBL_ENTRIES 0xab
 
+#define MC_CMD_0xab_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DUMP_BUFTBL_ENTRIES_IN msgrequest */
 #define    MC_CMD_DUMP_BUFTBL_ENTRIES_IN_LEN 8
 /* Index of the first buffer table entry. */
@@ -6510,6 +6890,8 @@
  */
 #define MC_CMD_SET_RXDP_CONFIG 0xc1
 
+#define MC_CMD_0xc1_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_RXDP_CONFIG_IN msgrequest */
 #define    MC_CMD_SET_RXDP_CONFIG_IN_LEN 4
 #define       MC_CMD_SET_RXDP_CONFIG_IN_DATA_OFST 0
@@ -6526,6 +6908,8 @@
  */
 #define MC_CMD_GET_RXDP_CONFIG 0xc2
 
+#define MC_CMD_0xc2_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_GET_RXDP_CONFIG_IN msgrequest */
 #define    MC_CMD_GET_RXDP_CONFIG_IN_LEN 0
 
@@ -6890,6 +7274,8 @@
  */
 #define MC_CMD_GET_CLOCK 0xac
 
+#define MC_CMD_0xac_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_CLOCK_IN msgrequest */
 #define    MC_CMD_GET_CLOCK_IN_LEN 0
 
@@ -6907,6 +7293,8 @@
  */
 #define MC_CMD_SET_CLOCK 0xad
 
+#define MC_CMD_0xad_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_CLOCK_IN msgrequest */
 #define    MC_CMD_SET_CLOCK_IN_LEN 12
 /* Requested system frequency in MHz; 0 leaves unchanged. */
@@ -6932,6 +7320,8 @@
  */
 #define MC_CMD_DPCPU_RPC 0xae
 
+#define MC_CMD_0xae_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DPCPU_RPC_IN msgrequest */
 #define    MC_CMD_DPCPU_RPC_IN_LEN 36
 #define       MC_CMD_DPCPU_RPC_IN_CPU_OFST 0
@@ -7016,6 +7406,8 @@
  */
 #define MC_CMD_TRIGGER_INTERRUPT 0xe3
 
+#define MC_CMD_0xe3_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_TRIGGER_INTERRUPT_IN msgrequest */
 #define    MC_CMD_TRIGGER_INTERRUPT_IN_LEN 4
 /* Interrupt level relative to base for function. */
@@ -7031,6 +7423,8 @@
  */
 #define MC_CMD_CAP_BLK_READ 0xe7
 
+#define MC_CMD_0xe7_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_CAP_BLK_READ_IN msgrequest */
 #define    MC_CMD_CAP_BLK_READ_IN_LEN 12
 #define       MC_CMD_CAP_BLK_READ_IN_CAP_REG_OFST 0
@@ -7055,6 +7449,8 @@
  */
 #define MC_CMD_DUMP_DO 0xe8
 
+#define MC_CMD_0xe8_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DUMP_DO_IN msgrequest */
 #define    MC_CMD_DUMP_DO_IN_LEN 52
 #define       MC_CMD_DUMP_DO_IN_PADDING_OFST 0
@@ -7108,6 +7504,8 @@
  */
 #define MC_CMD_DUMP_CONFIGURE_UNSOLICITED 0xe9
 
+#define MC_CMD_0xe9_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_DUMP_CONFIGURE_UNSOLICITED_IN msgrequest */
 #define    MC_CMD_DUMP_CONFIGURE_UNSOLICITED_IN_LEN 52
 #define       MC_CMD_DUMP_CONFIGURE_UNSOLICITED_IN_ENABLE_OFST 0
@@ -7151,6 +7549,8 @@
  */
 #define MC_CMD_SET_PSU 0xea
 
+#define MC_CMD_0xea_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_PSU_IN msgrequest */
 #define    MC_CMD_SET_PSU_IN_LEN 12
 #define       MC_CMD_SET_PSU_IN_PARAM_OFST 0
@@ -7171,6 +7571,8 @@
  */
 #define MC_CMD_GET_FUNCTION_INFO 0xec
 
+#define MC_CMD_0xec_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_FUNCTION_INFO_IN msgrequest */
 #define    MC_CMD_GET_FUNCTION_INFO_IN_LEN 0
 
@@ -7188,6 +7590,8 @@
  */
 #define MC_CMD_ENABLE_OFFLINE_BIST 0xed
 
+#define MC_CMD_0xed_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_ENABLE_OFFLINE_BIST_IN msgrequest */
 #define    MC_CMD_ENABLE_OFFLINE_BIST_IN_LEN 0
 
@@ -7203,6 +7607,8 @@
  */
 #define MC_CMD_UART_SEND_DATA 0xee
 
+#define MC_CMD_0xee_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_UART_SEND_DATA_OUT msgrequest */
 #define    MC_CMD_UART_SEND_DATA_OUT_LENMIN 16
 #define    MC_CMD_UART_SEND_DATA_OUT_LENMAX 252
@@ -7231,6 +7637,8 @@
  */
 #define MC_CMD_UART_RECV_DATA 0xef
 
+#define MC_CMD_0xef_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_UART_RECV_DATA_OUT msgrequest */
 #define    MC_CMD_UART_RECV_DATA_OUT_LEN 16
 /* CRC32 over OFFSET, LENGTH, RESERVED */
@@ -7266,6 +7674,8 @@
  */
 #define MC_CMD_READ_FUSES 0xf0
 
+#define MC_CMD_0xf0_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_READ_FUSES_IN msgrequest */
 #define    MC_CMD_READ_FUSES_IN_LEN 8
 /* Offset in OTP to read */
@@ -7292,6 +7702,8 @@
  */
 #define MC_CMD_KR_TUNE 0xf1
 
+#define MC_CMD_0xf1_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_KR_TUNE_IN msgrequest */
 #define    MC_CMD_KR_TUNE_IN_LENMIN 4
 #define    MC_CMD_KR_TUNE_IN_LENMAX 252
@@ -7550,6 +7962,8 @@
  */
 #define MC_CMD_PCIE_TUNE 0xf2
 
+#define MC_CMD_0xf2_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_PCIE_TUNE_IN msgrequest */
 #define    MC_CMD_PCIE_TUNE_IN_LENMIN 4
 #define    MC_CMD_PCIE_TUNE_IN_LENMAX 252
@@ -7711,6 +8125,8 @@
  */
 #define MC_CMD_LICENSING 0xf3
 
+#define MC_CMD_0xf3_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_LICENSING_IN msgrequest */
 #define    MC_CMD_LICENSING_IN_LEN 4
 /* identifies the type of operation requested */
@@ -7756,6 +8172,8 @@
  */
 #define MC_CMD_MC2MC_PROXY 0xf4
 
+#define MC_CMD_0xf4_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_MC2MC_PROXY_IN msgrequest */
 #define    MC_CMD_MC2MC_PROXY_IN_LEN 0
 
@@ -7771,6 +8189,8 @@
  */
 #define MC_CMD_GET_LICENSED_APP_STATE 0xf5
 
+#define MC_CMD_0xf5_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_GET_LICENSED_APP_STATE_IN msgrequest */
 #define    MC_CMD_GET_LICENSED_APP_STATE_IN_LEN 4
 /* application ID to query (LICENSED_APP_ID_xxx) */
@@ -7792,6 +8212,8 @@
  */
 #define MC_CMD_LICENSED_APP_OP 0xf6
 
+#define MC_CMD_0xf6_PRIVILEGE_CTG SRIOV_CTG_GENERAL
+
 /* MC_CMD_LICENSED_APP_OP_IN msgrequest */
 #define    MC_CMD_LICENSED_APP_OP_IN_LENMIN 8
 #define    MC_CMD_LICENSED_APP_OP_IN_LENMAX 252
@@ -7847,6 +8269,8 @@
  */
 #define MC_CMD_SET_PORT_SNIFF_CONFIG 0xf7
 
+#define MC_CMD_0xf7_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_SET_PORT_SNIFF_CONFIG_IN msgrequest */
 #define    MC_CMD_SET_PORT_SNIFF_CONFIG_IN_LEN 16
 /* configuration flags */
@@ -7881,6 +8305,8 @@
  */
 #define MC_CMD_GET_PORT_SNIFF_CONFIG 0xf8
 
+#define MC_CMD_0xf8_PRIVILEGE_CTG SRIOV_CTG_ADMIN
+
 /* MC_CMD_GET_PORT_SNIFF_CONFIG_IN msgrequest */
 #define    MC_CMD_GET_PORT_SNIFF_CONFIG_IN_LEN 0
 
diff --git a/drivers/net/ethernet/sfc/mcdi_port.c b/drivers/net/ethernet/sfc/mcdi_port.c
index fb19b70eac01..7f295c4d7b80 100644
--- a/drivers/net/ethernet/sfc/mcdi_port.c
+++ b/drivers/net/ethernet/sfc/mcdi_port.c
@@ -865,6 +865,7 @@ int efx_mcdi_set_mac(struct efx_nic *efx)
 
 	BUILD_BUG_ON(MC_CMD_SET_MAC_OUT_LEN != 0);
 
+	/* This has no effect on EF10 */
 	ether_addr_copy(MCDI_PTR(cmdbytes, SET_MAC_IN_ADDR),
 			efx->net_dev->dev_addr);
 
@@ -923,6 +924,7 @@ enum efx_stats_action {
 static int efx_mcdi_mac_stats(struct efx_nic *efx,
 			      enum efx_stats_action action, int clear)
 {
+	struct efx_ef10_nic_data *nic_data = efx->nic_data;
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_MAC_STATS_IN_LEN);
 	int rc;
 	int change = action == EFX_STATS_PULL ? 0 : 1;
@@ -944,9 +946,14 @@ static int efx_mcdi_mac_stats(struct efx_nic *efx,
 			      MAC_STATS_IN_PERIODIC_NOEVENT, 1,
 			      MAC_STATS_IN_PERIOD_MS, period);
 	MCDI_SET_DWORD(inbuf, MAC_STATS_IN_DMA_LEN, dma_len);
-
-	rc = efx_mcdi_rpc(efx, MC_CMD_MAC_STATS, inbuf, sizeof(inbuf),
-			  NULL, 0, NULL);
+	MCDI_SET_DWORD(inbuf, MAC_STATS_IN_PORT_ID, nic_data->vport_id);
+
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_MAC_STATS, inbuf, sizeof(inbuf),
+				NULL, 0, NULL);
+	/* Expect ENOENT if DMA queues have not been set up */
+	if (rc && (rc != -ENOENT || atomic_read(&efx->active_queues)))
+		efx_mcdi_display_error(efx, MC_CMD_MAC_STATS, sizeof(inbuf),
+				       NULL, 0, rc);
 	return rc;
 }
 
diff --git a/drivers/net/ethernet/sfc/net_driver.h b/drivers/net/ethernet/sfc/net_driver.h
index 325dd94bca46..d72f522bf9c3 100644
--- a/drivers/net/ethernet/sfc/net_driver.h
+++ b/drivers/net/ethernet/sfc/net_driver.h
@@ -25,6 +25,7 @@
 #include <linux/highmem.h>
 #include <linux/workqueue.h>
 #include <linux/mutex.h>
+#include <linux/rwsem.h>
 #include <linux/vmalloc.h>
 #include <linux/i2c.h>
 #include <linux/mtd/mtd.h>
@@ -793,7 +794,6 @@ union efx_multicast_hash {
 	efx_oword_t oword[EFX_MCAST_HASH_ENTRIES / sizeof(efx_oword_t) / 8];
 };
 
-struct efx_vf;
 struct vfdi_status;
 
 /**
@@ -897,7 +897,8 @@ struct vfdi_status;
  * @loopback_mode: Loopback status
  * @loopback_modes: Supported loopback mode bitmask
  * @loopback_selftest: Offline self-test private state
- * @filter_lock: Filter table lock
+ * @filter_sem: Filter table rw_semaphore, for freeing the table
+ * @filter_lock: Filter table lock, for mere content changes
  * @filter_state: Architecture-dependent filter table state
  * @rps_flow_id: Flow IDs of filters allocated for accelerated RFS,
  *	indexed by filter ID
@@ -909,7 +910,6 @@ struct vfdi_status;
  *	completed (either success or failure). Not used when MCDI is used to
  *	flush receive queues.
  * @flush_wq: wait queue used by efx_nic_flush_queues() to wait for flush completions.
- * @vf: Array of &struct efx_vf objects.
  * @vf_count: Number of VFs intended to be enabled.
  * @vf_init_count: Number of VFs that have been fully initialised.
  * @vi_scale: log2 number of vnics per VF.
@@ -1040,6 +1040,7 @@ struct efx_nic {
 
 	void *loopback_selftest;
 
+	struct rw_semaphore filter_sem;
 	spinlock_t filter_lock;
 	void *filter_state;
 #ifdef CONFIG_RFS_ACCEL
@@ -1053,7 +1054,6 @@ struct efx_nic {
 	wait_queue_head_t flush_wq;
 
 #ifdef CONFIG_SFC_SRIOV
-	struct efx_vf *vf;
 	unsigned vf_count;
 	unsigned vf_init_count;
 	unsigned vi_scale;
@@ -1092,6 +1092,7 @@ struct efx_mtd_partition {
 
 /**
  * struct efx_nic_type - Efx device type definition
+ * @mem_bar: Get the memory BAR
  * @mem_map_size: Get memory BAR mapped size
  * @probe: Probe the controller
  * @remove: Free resources allocated by probe()
@@ -1204,6 +1205,7 @@ struct efx_mtd_partition {
  * @ptp_set_ts_config: Set hardware timestamp configuration.  The flags
  *	and tx_type will already have been validated but this operation
  *	must validate and update rx_filter.
+ * @set_mac_address: Set the MAC address of the device
  * @revision: Hardware architecture revision
  * @txd_ptr_tbl_base: TX descriptor ring base address
  * @rxd_ptr_tbl_base: RX descriptor ring base address
@@ -1226,6 +1228,8 @@ struct efx_mtd_partition {
  * @hwtstamp_filters: Mask of hardware timestamp filter types supported
  */
 struct efx_nic_type {
+	bool is_vf;
+	unsigned int mem_bar;
 	unsigned int (*mem_map_size)(struct efx_nic *efx);
 	int (*probe)(struct efx_nic *efx);
 	void (*remove)(struct efx_nic *efx);
@@ -1277,7 +1281,8 @@ struct efx_nic_type {
 	void (*tx_init)(struct efx_tx_queue *tx_queue);
 	void (*tx_remove)(struct efx_tx_queue *tx_queue);
 	void (*tx_write)(struct efx_tx_queue *tx_queue);
-	void (*rx_push_rss_config)(struct efx_nic *efx);
+	int (*rx_push_rss_config)(struct efx_nic *efx, bool user,
+				  const u32 *rx_indir_table);
 	int (*rx_probe)(struct efx_rx_queue *rx_queue);
 	void (*rx_init)(struct efx_rx_queue *rx_queue);
 	void (*rx_remove)(struct efx_rx_queue *rx_queue);
@@ -1330,11 +1335,28 @@ struct efx_nic_type {
 	int (*ptp_set_ts_sync_events)(struct efx_nic *efx, bool en, bool temp);
 	int (*ptp_set_ts_config)(struct efx_nic *efx,
 				 struct hwtstamp_config *init);
+	int (*sriov_configure)(struct efx_nic *efx, int num_vfs);
 	int (*sriov_init)(struct efx_nic *efx);
 	void (*sriov_fini)(struct efx_nic *efx);
-	void (*sriov_mac_address_changed)(struct efx_nic *efx);
 	bool (*sriov_wanted)(struct efx_nic *efx);
 	void (*sriov_reset)(struct efx_nic *efx);
+	void (*sriov_flr)(struct efx_nic *efx, unsigned vf_i);
+	int (*sriov_set_vf_mac)(struct efx_nic *efx, int vf_i, u8 *mac);
+	int (*sriov_set_vf_vlan)(struct efx_nic *efx, int vf_i, u16 vlan,
+				 u8 qos);
+	int (*sriov_set_vf_spoofchk)(struct efx_nic *efx, int vf_i,
+				     bool spoofchk);
+	int (*sriov_get_vf_config)(struct efx_nic *efx, int vf_i,
+				   struct ifla_vf_info *ivi);
+	int (*sriov_set_vf_link_state)(struct efx_nic *efx, int vf_i,
+				       int link_state);
+	int (*sriov_get_phys_port_id)(struct efx_nic *efx,
+				      struct netdev_phys_item_id *ppid);
+	int (*vswitching_probe)(struct efx_nic *efx);
+	int (*vswitching_restore)(struct efx_nic *efx);
+	void (*vswitching_remove)(struct efx_nic *efx);
+	int (*get_mac_address)(struct efx_nic *efx, unsigned char *perm_addr);
+	int (*set_mac_address)(struct efx_nic *efx);
 
 	int revision;
 	unsigned int txd_ptr_tbl_base;
diff --git a/drivers/net/ethernet/sfc/nic.h b/drivers/net/ethernet/sfc/nic.h
index 93d10cbbd1cf..31ff9084d9a4 100644
--- a/drivers/net/ethernet/sfc/nic.h
+++ b/drivers/net/ethernet/sfc/nic.h
@@ -381,6 +381,7 @@ enum {
  * @efx: Pointer back to main interface structure
  * @wol_filter_id: Wake-on-LAN packet filter id
  * @stats: Hardware statistics
+ * @vf: Array of &struct siena_vf objects
  * @vf_buftbl_base: The zeroth buffer table index used to back VF queues.
  * @vfdi_status: Common VFDI status page to be dmad to VF address space.
  * @local_addr_list: List of local addresses. Protected by %local_lock.
@@ -394,6 +395,7 @@ struct siena_nic_data {
 	int wol_filter_id;
 	u64 stats[SIENA_STAT_COUNT];
 #ifdef CONFIG_SFC_SRIOV
+	struct siena_vf *vf;
 	struct efx_channel *vfdi_channel;
 	unsigned vf_buftbl_base;
 	struct efx_buffer vfdi_status;
@@ -405,59 +407,77 @@ struct siena_nic_data {
 };
 
 enum {
-	EF10_STAT_tx_bytes = GENERIC_STAT_COUNT,
-	EF10_STAT_tx_packets,
-	EF10_STAT_tx_pause,
-	EF10_STAT_tx_control,
-	EF10_STAT_tx_unicast,
-	EF10_STAT_tx_multicast,
-	EF10_STAT_tx_broadcast,
-	EF10_STAT_tx_lt64,
-	EF10_STAT_tx_64,
-	EF10_STAT_tx_65_to_127,
-	EF10_STAT_tx_128_to_255,
-	EF10_STAT_tx_256_to_511,
-	EF10_STAT_tx_512_to_1023,
-	EF10_STAT_tx_1024_to_15xx,
-	EF10_STAT_tx_15xx_to_jumbo,
-	EF10_STAT_rx_bytes,
-	EF10_STAT_rx_bytes_minus_good_bytes,
-	EF10_STAT_rx_good_bytes,
-	EF10_STAT_rx_bad_bytes,
-	EF10_STAT_rx_packets,
-	EF10_STAT_rx_good,
-	EF10_STAT_rx_bad,
-	EF10_STAT_rx_pause,
-	EF10_STAT_rx_control,
+	EF10_STAT_port_tx_bytes = GENERIC_STAT_COUNT,
+	EF10_STAT_port_tx_packets,
+	EF10_STAT_port_tx_pause,
+	EF10_STAT_port_tx_control,
+	EF10_STAT_port_tx_unicast,
+	EF10_STAT_port_tx_multicast,
+	EF10_STAT_port_tx_broadcast,
+	EF10_STAT_port_tx_lt64,
+	EF10_STAT_port_tx_64,
+	EF10_STAT_port_tx_65_to_127,
+	EF10_STAT_port_tx_128_to_255,
+	EF10_STAT_port_tx_256_to_511,
+	EF10_STAT_port_tx_512_to_1023,
+	EF10_STAT_port_tx_1024_to_15xx,
+	EF10_STAT_port_tx_15xx_to_jumbo,
+	EF10_STAT_port_rx_bytes,
+	EF10_STAT_port_rx_bytes_minus_good_bytes,
+	EF10_STAT_port_rx_good_bytes,
+	EF10_STAT_port_rx_bad_bytes,
+	EF10_STAT_port_rx_packets,
+	EF10_STAT_port_rx_good,
+	EF10_STAT_port_rx_bad,
+	EF10_STAT_port_rx_pause,
+	EF10_STAT_port_rx_control,
+	EF10_STAT_port_rx_unicast,
+	EF10_STAT_port_rx_multicast,
+	EF10_STAT_port_rx_broadcast,
+	EF10_STAT_port_rx_lt64,
+	EF10_STAT_port_rx_64,
+	EF10_STAT_port_rx_65_to_127,
+	EF10_STAT_port_rx_128_to_255,
+	EF10_STAT_port_rx_256_to_511,
+	EF10_STAT_port_rx_512_to_1023,
+	EF10_STAT_port_rx_1024_to_15xx,
+	EF10_STAT_port_rx_15xx_to_jumbo,
+	EF10_STAT_port_rx_gtjumbo,
+	EF10_STAT_port_rx_bad_gtjumbo,
+	EF10_STAT_port_rx_overflow,
+	EF10_STAT_port_rx_align_error,
+	EF10_STAT_port_rx_length_error,
+	EF10_STAT_port_rx_nodesc_drops,
+	EF10_STAT_port_rx_pm_trunc_bb_overflow,
+	EF10_STAT_port_rx_pm_discard_bb_overflow,
+	EF10_STAT_port_rx_pm_trunc_vfifo_full,
+	EF10_STAT_port_rx_pm_discard_vfifo_full,
+	EF10_STAT_port_rx_pm_trunc_qbb,
+	EF10_STAT_port_rx_pm_discard_qbb,
+	EF10_STAT_port_rx_pm_discard_mapping,
+	EF10_STAT_port_rx_dp_q_disabled_packets,
+	EF10_STAT_port_rx_dp_di_dropped_packets,
+	EF10_STAT_port_rx_dp_streaming_packets,
+	EF10_STAT_port_rx_dp_hlb_fetch,
+	EF10_STAT_port_rx_dp_hlb_wait,
 	EF10_STAT_rx_unicast,
+	EF10_STAT_rx_unicast_bytes,
 	EF10_STAT_rx_multicast,
+	EF10_STAT_rx_multicast_bytes,
 	EF10_STAT_rx_broadcast,
-	EF10_STAT_rx_lt64,
-	EF10_STAT_rx_64,
-	EF10_STAT_rx_65_to_127,
-	EF10_STAT_rx_128_to_255,
-	EF10_STAT_rx_256_to_511,
-	EF10_STAT_rx_512_to_1023,
-	EF10_STAT_rx_1024_to_15xx,
-	EF10_STAT_rx_15xx_to_jumbo,
-	EF10_STAT_rx_gtjumbo,
-	EF10_STAT_rx_bad_gtjumbo,
+	EF10_STAT_rx_broadcast_bytes,
+	EF10_STAT_rx_bad,
+	EF10_STAT_rx_bad_bytes,
 	EF10_STAT_rx_overflow,
-	EF10_STAT_rx_align_error,
-	EF10_STAT_rx_length_error,
-	EF10_STAT_rx_nodesc_drops,
-	EF10_STAT_rx_pm_trunc_bb_overflow,
-	EF10_STAT_rx_pm_discard_bb_overflow,
-	EF10_STAT_rx_pm_trunc_vfifo_full,
-	EF10_STAT_rx_pm_discard_vfifo_full,
-	EF10_STAT_rx_pm_trunc_qbb,
-	EF10_STAT_rx_pm_discard_qbb,
-	EF10_STAT_rx_pm_discard_mapping,
-	EF10_STAT_rx_dp_q_disabled_packets,
-	EF10_STAT_rx_dp_di_dropped_packets,
-	EF10_STAT_rx_dp_streaming_packets,
-	EF10_STAT_rx_dp_hlb_fetch,
-	EF10_STAT_rx_dp_hlb_wait,
+	EF10_STAT_tx_unicast,
+	EF10_STAT_tx_unicast_bytes,
+	EF10_STAT_tx_multicast,
+	EF10_STAT_tx_multicast_bytes,
+	EF10_STAT_tx_broadcast,
+	EF10_STAT_tx_broadcast_bytes,
+	EF10_STAT_tx_bad,
+	EF10_STAT_tx_bad_bytes,
+	EF10_STAT_tx_overflow,
 	EF10_STAT_COUNT
 };
 
@@ -483,12 +503,21 @@ enum {
  * @must_restore_piobufs: Flag: PIO buffers have yet to be restored after MC
  *	reboot
  * @rx_rss_context: Firmware handle for our RSS context
+ * @rx_rss_context_exclusive: Whether our RSS context is exclusive or shared
  * @stats: Hardware statistics
  * @workaround_35388: Flag: firmware supports workaround for bug 35388
  * @must_check_datapath_caps: Flag: @datapath_caps needs to be revalidated
  *	after MC reboot
  * @datapath_caps: Capabilities of datapath firmware (FLAGS1 field of
  *	%MC_CMD_GET_CAPABILITIES response)
+ * @rx_dpcpu_fw_id: Firmware ID of the RxDPCPU
+ * @tx_dpcpu_fw_id: Firmware ID of the TxDPCPU
+ * @vport_id: The function's vport ID, only relevant for PFs
+ * @must_probe_vswitching: Flag: vswitching has yet to be setup after MC reboot
+ * @pf_index: The number for this PF, or the parent PF if this is a VF
+#ifdef CONFIG_SFC_SRIOV
+ * @vf: Pointer to VF data structure
+#endif
  */
 struct efx_ef10_nic_data {
 	struct efx_buffer mcdi_buf;
@@ -503,126 +532,27 @@ struct efx_ef10_nic_data {
 	unsigned int piobuf_handle[EF10_TX_PIOBUF_COUNT];
 	bool must_restore_piobufs;
 	u32 rx_rss_context;
+	bool rx_rss_context_exclusive;
 	u64 stats[EF10_STAT_COUNT];
 	bool workaround_35388;
 	bool must_check_datapath_caps;
 	u32 datapath_caps;
-};
-
-/*
- * On the SFC9000 family each port is associated with 1 PCI physical
- * function (PF) handled by sfc and a configurable number of virtual
- * functions (VFs) that may be handled by some other driver, often in
- * a VM guest.  The queue pointer registers are mapped in both PF and
- * VF BARs such that an 8K region provides access to a single RX, TX
- * and event queue (collectively a Virtual Interface, VI or VNIC).
- *
- * The PF has access to all 1024 VIs while VFs are mapped to VIs
- * according to VI_BASE and VI_SCALE: VF i has access to VIs numbered
- * in range [VI_BASE + i << VI_SCALE, VI_BASE + i + 1 << VI_SCALE).
- * The number of VIs and the VI_SCALE value are configurable but must
- * be established at boot time by firmware.
- */
-
-/* Maximum VI_SCALE parameter supported by Siena */
-#define EFX_VI_SCALE_MAX 6
-/* Base VI to use for SR-IOV. Must be aligned to (1 << EFX_VI_SCALE_MAX),
- * so this is the smallest allowed value. */
-#define EFX_VI_BASE 128U
-/* Maximum number of VFs allowed */
-#define EFX_VF_COUNT_MAX 127
-/* Limit EVQs on VFs to be only 8k to reduce buffer table reservation */
-#define EFX_MAX_VF_EVQ_SIZE 8192UL
-/* The number of buffer table entries reserved for each VI on a VF */
-#define EFX_VF_BUFTBL_PER_VI					\
-	((EFX_MAX_VF_EVQ_SIZE + 2 * EFX_MAX_DMAQ_SIZE) *	\
-	 sizeof(efx_qword_t) / EFX_BUF_SIZE)
-
+	unsigned int rx_dpcpu_fw_id;
+	unsigned int tx_dpcpu_fw_id;
+	unsigned int vport_id;
+	bool must_probe_vswitching;
+	unsigned int pf_index;
+	u8 port_id[ETH_ALEN];
 #ifdef CONFIG_SFC_SRIOV
-
-/* SIENA */
-static inline bool efx_siena_sriov_wanted(struct efx_nic *efx)
-{
-	return efx->vf_count != 0;
-}
-
-static inline bool efx_siena_sriov_enabled(struct efx_nic *efx)
-{
-	return efx->vf_init_count != 0;
-}
-
-static inline unsigned int efx_vf_size(struct efx_nic *efx)
-{
-	return 1 << efx->vi_scale;
-}
+	unsigned int vf_index;
+	struct ef10_vf *vf;
+#endif
+	u8 vport_mac[ETH_ALEN];
+};
 
 int efx_init_sriov(void);
-void efx_siena_sriov_probe(struct efx_nic *efx);
-int efx_siena_sriov_init(struct efx_nic *efx);
-void efx_siena_sriov_mac_address_changed(struct efx_nic *efx);
-void efx_siena_sriov_tx_flush_done(struct efx_nic *efx, efx_qword_t *event);
-void efx_siena_sriov_rx_flush_done(struct efx_nic *efx, efx_qword_t *event);
-void efx_siena_sriov_event(struct efx_channel *channel, efx_qword_t *event);
-void efx_siena_sriov_desc_fetch_err(struct efx_nic *efx, unsigned dmaq);
-void efx_siena_sriov_flr(struct efx_nic *efx, unsigned flr);
-void efx_siena_sriov_reset(struct efx_nic *efx);
-void efx_siena_sriov_fini(struct efx_nic *efx);
 void efx_fini_sriov(void);
 
-/* EF10 */
-static inline bool efx_ef10_sriov_wanted(struct efx_nic *efx) { return false; }
-static inline int efx_ef10_sriov_init(struct efx_nic *efx) { return -EOPNOTSUPP; }
-static inline void efx_ef10_sriov_mac_address_changed(struct efx_nic *efx) {}
-static inline void efx_ef10_sriov_reset(struct efx_nic *efx) {}
-static inline void efx_ef10_sriov_fini(struct efx_nic *efx) {}
-
-#else
-
-/* SIENA */
-static inline bool efx_siena_sriov_wanted(struct efx_nic *efx) { return false; }
-static inline bool efx_siena_sriov_enabled(struct efx_nic *efx) { return false; }
-static inline unsigned int efx_vf_size(struct efx_nic *efx) { return 0; }
-static inline int efx_init_sriov(void) { return 0; }
-static inline void efx_siena_sriov_probe(struct efx_nic *efx) {}
-static inline int efx_siena_sriov_init(struct efx_nic *efx) { return -EOPNOTSUPP; }
-static inline void efx_siena_sriov_mac_address_changed(struct efx_nic *efx) {}
-static inline void efx_siena_sriov_tx_flush_done(struct efx_nic *efx,
-						 efx_qword_t *event) {}
-static inline void efx_siena_sriov_rx_flush_done(struct efx_nic *efx,
-						 efx_qword_t *event) {}
-static inline void efx_siena_sriov_event(struct efx_channel *channel,
-					 efx_qword_t *event) {}
-static inline void efx_siena_sriov_desc_fetch_err(struct efx_nic *efx,
-						  unsigned dmaq) {}
-static inline void efx_siena_sriov_flr(struct efx_nic *efx, unsigned flr) {}
-static inline void efx_siena_sriov_reset(struct efx_nic *efx) {}
-static inline void efx_siena_sriov_fini(struct efx_nic *efx) {}
-static inline void efx_fini_sriov(void) {}
-
-/* EF10 */
-static inline bool efx_ef10_sriov_wanted(struct efx_nic *efx) { return false; }
-static inline int efx_ef10_sriov_init(struct efx_nic *efx) { return -EOPNOTSUPP; }
-static inline void efx_ef10_sriov_mac_address_changed(struct efx_nic *efx) {}
-static inline void efx_ef10_sriov_reset(struct efx_nic *efx) {}
-static inline void efx_ef10_sriov_fini(struct efx_nic *efx) {}
-
-#endif
-
-/* FALCON */
-static inline bool efx_falcon_sriov_wanted(struct efx_nic *efx) { return false; }
-static inline int efx_falcon_sriov_init(struct efx_nic *efx) { return -EOPNOTSUPP; }
-static inline void efx_falcon_sriov_mac_address_changed(struct efx_nic *efx) {}
-static inline void efx_falcon_sriov_reset(struct efx_nic *efx) {}
-static inline void efx_falcon_sriov_fini(struct efx_nic *efx) {}
-
-int efx_siena_sriov_set_vf_mac(struct net_device *dev, int vf, u8 *mac);
-int efx_siena_sriov_set_vf_vlan(struct net_device *dev, int vf,
-				u16 vlan, u8 qos);
-int efx_siena_sriov_get_vf_config(struct net_device *dev, int vf,
-				  struct ifla_vf_info *ivf);
-int efx_siena_sriov_set_vf_spoofchk(struct net_device *net_dev, int vf,
-				    bool spoofchk);
-
 struct ethtool_ts_info;
 int efx_ptp_probe(struct efx_nic *efx, struct efx_channel *channel);
 void efx_ptp_defer_probe_with_channel(struct efx_nic *efx);
@@ -654,6 +584,7 @@ extern const struct efx_nic_type falcon_a1_nic_type;
 extern const struct efx_nic_type falcon_b0_nic_type;
 extern const struct efx_nic_type siena_a0_nic_type;
 extern const struct efx_nic_type efx_hunt_a0_nic_type;
+extern const struct efx_nic_type efx_hunt_a0_vf_nic_type;
 
 /**************************************************************************
  *
diff --git a/drivers/net/ethernet/sfc/ptp.c b/drivers/net/ethernet/sfc/ptp.c
index a2e9aee05cdd..ad62615a93dc 100644
--- a/drivers/net/ethernet/sfc/ptp.c
+++ b/drivers/net/ethernet/sfc/ptp.c
@@ -306,7 +306,7 @@ struct efx_ptp_data {
 	struct work_struct pps_work;
 	struct workqueue_struct *pps_workwq;
 	bool nic_ts_enabled;
-	MCDI_DECLARE_BUF(txbuf, MC_CMD_PTP_IN_TRANSMIT_LENMAX);
+	_MCDI_DECLARE_BUF(txbuf, MC_CMD_PTP_IN_TRANSMIT_LENMAX);
 
 	unsigned int good_syncs;
 	unsigned int fast_syncs;
@@ -389,11 +389,8 @@ size_t efx_ptp_update_stats(struct efx_nic *efx, u64 *stats)
 	MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
 	rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
 			  outbuf, sizeof(outbuf), NULL);
-	if (rc) {
-		netif_err(efx, hw, efx->net_dev,
-			  "MC_CMD_PTP_OP_STATUS failed (%d)\n", rc);
+	if (rc)
 		memset(outbuf, 0, sizeof(outbuf));
-	}
 	efx_nic_update_stats(efx_ptp_stat_desc, PTP_STAT_COUNT,
 			     efx_ptp_stat_mask,
 			     stats, _MCDI_PTR(outbuf, 0), false);
@@ -490,14 +487,20 @@ static int efx_ptp_get_attributes(struct efx_nic *efx)
 	 */
 	MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_GET_ATTRIBUTES);
 	MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
-	rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
-			  outbuf, sizeof(outbuf), &out_len);
-	if (rc == 0)
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
+				outbuf, sizeof(outbuf), &out_len);
+	if (rc == 0) {
 		fmt = MCDI_DWORD(outbuf, PTP_OUT_GET_ATTRIBUTES_TIME_FORMAT);
-	else if (rc == -EINVAL)
+	} else if (rc == -EINVAL) {
 		fmt = MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_NANOSECONDS;
-	else
+	} else if (rc == -EPERM) {
+		netif_info(efx, probe, efx->net_dev, "no PTP support\n");
+		return rc;
+	} else {
+		efx_mcdi_display_error(efx, MC_CMD_PTP, sizeof(inbuf),
+				       outbuf, sizeof(outbuf), rc);
 		return rc;
+	}
 
 	if (fmt == MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_27FRACTION) {
 		ptp->ns_to_nic_time = efx_ptp_ns_to_s27;
@@ -541,8 +544,8 @@ static int efx_ptp_get_timestamp_corrections(struct efx_nic *efx)
 		       MC_CMD_PTP_OP_GET_TIMESTAMP_CORRECTIONS);
 	MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
 
-	rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
-			  outbuf, sizeof(outbuf), NULL);
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
+				outbuf, sizeof(outbuf), NULL);
 	if (rc == 0) {
 		efx->ptp_data->ts_corrections.tx = MCDI_DWORD(outbuf,
 			PTP_OUT_GET_TIMESTAMP_CORRECTIONS_TRANSMIT);
@@ -558,6 +561,8 @@ static int efx_ptp_get_timestamp_corrections(struct efx_nic *efx)
 		efx->ptp_data->ts_corrections.pps_out = 0;
 		efx->ptp_data->ts_corrections.pps_in = 0;
 	} else {
+		efx_mcdi_display_error(efx, MC_CMD_PTP, sizeof(inbuf), outbuf,
+				       sizeof(outbuf), rc);
 		return rc;
 	}
 
@@ -568,7 +573,7 @@ static int efx_ptp_get_timestamp_corrections(struct efx_nic *efx)
 static int efx_ptp_enable(struct efx_nic *efx)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_ENABLE_LEN);
-	MCDI_DECLARE_BUF_OUT_OR_ERR(outbuf, 0);
+	MCDI_DECLARE_BUF_ERR(outbuf);
 	int rc;
 
 	MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_ENABLE);
@@ -596,7 +601,7 @@ static int efx_ptp_enable(struct efx_nic *efx)
 static int efx_ptp_disable(struct efx_nic *efx)
 {
 	MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_DISABLE_LEN);
-	MCDI_DECLARE_BUF_OUT_OR_ERR(outbuf, 0);
+	MCDI_DECLARE_BUF_ERR(outbuf);
 	int rc;
 
 	MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_DISABLE);
@@ -604,7 +609,12 @@ static int efx_ptp_disable(struct efx_nic *efx)
 	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
 				outbuf, sizeof(outbuf), NULL);
 	rc = (rc == -EALREADY) ? 0 : rc;
-	if (rc)
+	/* If we get ENOSYS, the NIC doesn't support PTP, and thus this function
+	 * should only have been called during probe.
+	 */
+	if (rc == -ENOSYS || rc == -EPERM)
+		netif_info(efx, probe, efx->net_dev, "no PTP support\n");
+	else if (rc)
 		efx_mcdi_display_error(efx, MC_CMD_PTP,
 				       MC_CMD_PTP_IN_DISABLE_LEN,
 				       outbuf, sizeof(outbuf), rc);
diff --git a/drivers/net/ethernet/sfc/rx.c b/drivers/net/ethernet/sfc/rx.c
index c0ad95d2f63d..809ea4610a77 100644
--- a/drivers/net/ethernet/sfc/rx.c
+++ b/drivers/net/ethernet/sfc/rx.c
@@ -224,12 +224,17 @@ static void efx_unmap_rx_buffer(struct efx_nic *efx,
 	}
 }
 
-static void efx_free_rx_buffer(struct efx_rx_buffer *rx_buf)
+static void efx_free_rx_buffers(struct efx_rx_queue *rx_queue,
+				struct efx_rx_buffer *rx_buf,
+				unsigned int num_bufs)
 {
-	if (rx_buf->page) {
-		put_page(rx_buf->page);
-		rx_buf->page = NULL;
-	}
+	do {
+		if (rx_buf->page) {
+			put_page(rx_buf->page);
+			rx_buf->page = NULL;
+		}
+		rx_buf = efx_rx_buf_next(rx_queue, rx_buf);
+	} while (--num_bufs);
 }
 
 /* Attempt to recycle the page if there is an RX recycle ring; the page can
@@ -278,7 +283,7 @@ static void efx_fini_rx_buffer(struct efx_rx_queue *rx_queue,
 	/* If this is the last buffer in a page, unmap and free it. */
 	if (rx_buf->flags & EFX_RX_BUF_LAST_IN_PAGE) {
 		efx_unmap_rx_buffer(rx_queue->efx, rx_buf);
-		efx_free_rx_buffer(rx_buf);
+		efx_free_rx_buffers(rx_queue, rx_buf, 1);
 	}
 	rx_buf->page = NULL;
 }
@@ -304,10 +309,7 @@ static void efx_discard_rx_packet(struct efx_channel *channel,
 
 	efx_recycle_rx_pages(channel, rx_buf, n_frags);
 
-	do {
-		efx_free_rx_buffer(rx_buf);
-		rx_buf = efx_rx_buf_next(rx_queue, rx_buf);
-	} while (--n_frags);
+	efx_free_rx_buffers(rx_queue, rx_buf, n_frags);
 }
 
 /**
@@ -431,11 +433,10 @@ efx_rx_packet_gro(struct efx_channel *channel, struct efx_rx_buffer *rx_buf,
 
 	skb = napi_get_frags(napi);
 	if (unlikely(!skb)) {
-		while (n_frags--) {
-			put_page(rx_buf->page);
-			rx_buf->page = NULL;
-			rx_buf = efx_rx_buf_next(&channel->rx_queue, rx_buf);
-		}
+		struct efx_rx_queue *rx_queue;
+
+		rx_queue = efx_channel_get_rx_queue(channel);
+		efx_free_rx_buffers(rx_queue, rx_buf, n_frags);
 		return;
 	}
 
@@ -622,7 +623,10 @@ static void efx_rx_deliver(struct efx_channel *channel, u8 *eh,
 
 	skb = efx_rx_mk_skb(channel, rx_buf, n_frags, eh, hdr_len);
 	if (unlikely(skb == NULL)) {
-		efx_free_rx_buffer(rx_buf);
+		struct efx_rx_queue *rx_queue;
+
+		rx_queue = efx_channel_get_rx_queue(channel);
+		efx_free_rx_buffers(rx_queue, rx_buf, n_frags);
 		return;
 	}
 	skb_record_rx_queue(skb, channel->rx_queue.core_index);
@@ -661,8 +665,12 @@ void __efx_rx_packet(struct efx_channel *channel)
 	 * loopback layer, and free the rx_buf here
 	 */
 	if (unlikely(efx->loopback_selftest)) {
+		struct efx_rx_queue *rx_queue;
+
 		efx_loopback_rx_packet(efx, eh, rx_buf->len);
-		efx_free_rx_buffer(rx_buf);
+		rx_queue = efx_channel_get_rx_queue(channel);
+		efx_free_rx_buffers(rx_queue, rx_buf,
+				    channel->rx_pkt_n_frags);
 		goto out;
 	}
 
diff --git a/drivers/net/ethernet/sfc/siena.c b/drivers/net/ethernet/sfc/siena.c
index f12c811938d2..b323b9167526 100644
--- a/drivers/net/ethernet/sfc/siena.c
+++ b/drivers/net/ethernet/sfc/siena.c
@@ -25,6 +25,7 @@
 #include "mcdi.h"
 #include "mcdi_pcol.h"
 #include "selftest.h"
+#include "siena_sriov.h"
 
 /* Hardware control for SFC9000 family including SFL9021 (aka Siena). */
 
@@ -306,7 +307,9 @@ static int siena_probe_nic(struct efx_nic *efx)
 	if (rc)
 		goto fail5;
 
+#ifdef CONFIG_SFC_SRIOV
 	efx_siena_sriov_probe(efx);
+#endif
 	efx_ptp_defer_probe_with_channel(efx);
 
 	return 0;
@@ -321,7 +324,8 @@ fail1:
 	return rc;
 }
 
-static void siena_rx_push_rss_config(struct efx_nic *efx)
+static int siena_rx_push_rss_config(struct efx_nic *efx, bool user,
+				    const u32 *rx_indir_table)
 {
 	efx_oword_t temp;
 
@@ -343,7 +347,11 @@ static void siena_rx_push_rss_config(struct efx_nic *efx)
 	       FRF_CZ_RX_RSS_IPV6_TKEY_HI_WIDTH / 8);
 	efx_writeo(efx, &temp, FR_CZ_RX_RSS_IPV6_REG3);
 
+	memcpy(efx->rx_indir_table, rx_indir_table,
+	       sizeof(efx->rx_indir_table));
 	efx_farch_rx_push_indir_table(efx);
+
+	return 0;
 }
 
 /* This call performs hardware-specific global initialisation, such as
@@ -386,7 +394,7 @@ static int siena_init_nic(struct efx_nic *efx)
 			    EFX_RX_USR_BUF_SIZE >> 5);
 	efx_writeo(efx, &temp, FR_AZ_RX_CFG);
 
-	siena_rx_push_rss_config(efx);
+	siena_rx_push_rss_config(efx, false, efx->rx_indir_table);
 
 	/* Enable event logging */
 	rc = efx_mcdi_log_ctrl(efx, true, false, 0);
@@ -909,6 +917,8 @@ fail:
  */
 
 const struct efx_nic_type siena_a0_nic_type = {
+	.is_vf = false,
+	.mem_bar = EFX_MEM_BAR,
 	.mem_map_size = siena_mem_map_size,
 	.probe = siena_probe_nic,
 	.remove = siena_remove_nic,
@@ -996,11 +1006,22 @@ const struct efx_nic_type siena_a0_nic_type = {
 #endif
 	.ptp_write_host_time = siena_ptp_write_host_time,
 	.ptp_set_ts_config = siena_ptp_set_ts_config,
+#ifdef CONFIG_SFC_SRIOV
+	.sriov_configure = efx_siena_sriov_configure,
 	.sriov_init = efx_siena_sriov_init,
 	.sriov_fini = efx_siena_sriov_fini,
-	.sriov_mac_address_changed = efx_siena_sriov_mac_address_changed,
 	.sriov_wanted = efx_siena_sriov_wanted,
 	.sriov_reset = efx_siena_sriov_reset,
+	.sriov_flr = efx_siena_sriov_flr,
+	.sriov_set_vf_mac = efx_siena_sriov_set_vf_mac,
+	.sriov_set_vf_vlan = efx_siena_sriov_set_vf_vlan,
+	.sriov_set_vf_spoofchk = efx_siena_sriov_set_vf_spoofchk,
+	.sriov_get_vf_config = efx_siena_sriov_get_vf_config,
+	.vswitching_probe = efx_port_dummy_op_int,
+	.vswitching_restore = efx_port_dummy_op_int,
+	.vswitching_remove = efx_port_dummy_op_void,
+	.set_mac_address = efx_siena_sriov_mac_address_changed,
+#endif
 
 	.revision = EFX_REV_SIENA_A0,
 	.txd_ptr_tbl_base = FR_BZ_TX_DESC_PTR_TBL,
diff --git a/drivers/net/ethernet/sfc/siena_sriov.c b/drivers/net/ethernet/sfc/siena_sriov.c
index fe83430796fd..da7b94f34604 100644
--- a/drivers/net/ethernet/sfc/siena_sriov.c
+++ b/drivers/net/ethernet/sfc/siena_sriov.c
@@ -16,6 +16,7 @@
 #include "filter.h"
 #include "mcdi_pcol.h"
 #include "farch_regs.h"
+#include "siena_sriov.h"
 #include "vfdi.h"
 
 /* Number of longs required to track all the VIs in a VF */
@@ -38,7 +39,7 @@ enum efx_vf_tx_filter_mode {
 };
 
 /**
- * struct efx_vf - Back-end resource and protocol state for a PCI VF
+ * struct siena_vf - Back-end resource and protocol state for a PCI VF
  * @efx: The Efx NIC owning this VF
  * @pci_rid: The PCI requester ID for this VF
  * @pci_name: The PCI name (formatted address) of this VF
@@ -83,7 +84,7 @@ enum efx_vf_tx_filter_mode {
  * @rxq_retry_count: Number of receive queues in @rxq_retry_mask.
  * @reset_work: Work item to schedule a VF reset.
  */
-struct efx_vf {
+struct siena_vf {
 	struct efx_nic *efx;
 	unsigned int pci_rid;
 	char pci_name[13]; /* dddd:bb:dd.f */
@@ -189,7 +190,7 @@ MODULE_PARM_DESC(max_vfs,
  */
 static struct workqueue_struct *vfdi_workqueue;
 
-static unsigned abs_index(struct efx_vf *vf, unsigned index)
+static unsigned abs_index(struct siena_vf *vf, unsigned index)
 {
 	return EFX_VI_BASE + vf->index * efx_vf_size(vf->efx) + index;
 }
@@ -207,8 +208,8 @@ static int efx_siena_sriov_cmd(struct efx_nic *efx, bool enable,
 	MCDI_SET_DWORD(inbuf, SRIOV_IN_VI_BASE, EFX_VI_BASE);
 	MCDI_SET_DWORD(inbuf, SRIOV_IN_VF_COUNT, efx->vf_count);
 
-	rc = efx_mcdi_rpc(efx, MC_CMD_SRIOV, inbuf, MC_CMD_SRIOV_IN_LEN,
-			  outbuf, MC_CMD_SRIOV_OUT_LEN, &outlen);
+	rc = efx_mcdi_rpc_quiet(efx, MC_CMD_SRIOV, inbuf, MC_CMD_SRIOV_IN_LEN,
+				outbuf, MC_CMD_SRIOV_OUT_LEN, &outlen);
 	if (rc)
 		return rc;
 	if (outlen < MC_CMD_SRIOV_OUT_LEN)
@@ -299,7 +300,7 @@ out:
 /* The TX filter is entirely controlled by this driver, and is modified
  * underneath the feet of the VF
  */
-static void efx_siena_sriov_reset_tx_filter(struct efx_vf *vf)
+static void efx_siena_sriov_reset_tx_filter(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct efx_filter_spec filter;
@@ -343,7 +344,7 @@ static void efx_siena_sriov_reset_tx_filter(struct efx_vf *vf)
 }
 
 /* The RX filter is managed here on behalf of the VF driver */
-static void efx_siena_sriov_reset_rx_filter(struct efx_vf *vf)
+static void efx_siena_sriov_reset_rx_filter(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct efx_filter_spec filter;
@@ -382,7 +383,7 @@ static void efx_siena_sriov_reset_rx_filter(struct efx_vf *vf)
 	}
 }
 
-static void __efx_siena_sriov_update_vf_addr(struct efx_vf *vf)
+static void __efx_siena_sriov_update_vf_addr(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct siena_nic_data *nic_data = efx->nic_data;
@@ -397,7 +398,7 @@ static void __efx_siena_sriov_update_vf_addr(struct efx_vf *vf)
  * local_page_list, either by acquiring local_lock or by running from
  * efx_siena_sriov_peer_work()
  */
-static void __efx_siena_sriov_push_vf_status(struct efx_vf *vf)
+static void __efx_siena_sriov_push_vf_status(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct siena_nic_data *nic_data = efx->nic_data;
@@ -509,8 +510,9 @@ static bool bad_buf_count(unsigned buf_count, unsigned max_entry_count)
  * Optionally set VF index and VI index within the VF.
  */
 static bool map_vi_index(struct efx_nic *efx, unsigned abs_index,
-			 struct efx_vf **vf_out, unsigned *rel_index_out)
+			 struct siena_vf **vf_out, unsigned *rel_index_out)
 {
+	struct siena_nic_data *nic_data = efx->nic_data;
 	unsigned vf_i;
 
 	if (abs_index < EFX_VI_BASE)
@@ -520,13 +522,13 @@ static bool map_vi_index(struct efx_nic *efx, unsigned abs_index,
 		return true;
 
 	if (vf_out)
-		*vf_out = efx->vf + vf_i;
+		*vf_out = nic_data->vf + vf_i;
 	if (rel_index_out)
 		*rel_index_out = abs_index % efx_vf_size(efx);
 	return false;
 }
 
-static int efx_vfdi_init_evq(struct efx_vf *vf)
+static int efx_vfdi_init_evq(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct vfdi_req *req = vf->buf.addr;
@@ -567,7 +569,7 @@ static int efx_vfdi_init_evq(struct efx_vf *vf)
 	return VFDI_RC_SUCCESS;
 }
 
-static int efx_vfdi_init_rxq(struct efx_vf *vf)
+static int efx_vfdi_init_rxq(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct vfdi_req *req = vf->buf.addr;
@@ -608,7 +610,7 @@ static int efx_vfdi_init_rxq(struct efx_vf *vf)
 	return VFDI_RC_SUCCESS;
 }
 
-static int efx_vfdi_init_txq(struct efx_vf *vf)
+static int efx_vfdi_init_txq(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct vfdi_req *req = vf->buf.addr;
@@ -655,7 +657,7 @@ static int efx_vfdi_init_txq(struct efx_vf *vf)
 }
 
 /* Returns true when efx_vfdi_fini_all_queues should wake */
-static bool efx_vfdi_flush_wake(struct efx_vf *vf)
+static bool efx_vfdi_flush_wake(struct siena_vf *vf)
 {
 	/* Ensure that all updates are visible to efx_vfdi_fini_all_queues() */
 	smp_mb();
@@ -664,7 +666,7 @@ static bool efx_vfdi_flush_wake(struct efx_vf *vf)
 		atomic_read(&vf->rxq_retry_count);
 }
 
-static void efx_vfdi_flush_clear(struct efx_vf *vf)
+static void efx_vfdi_flush_clear(struct siena_vf *vf)
 {
 	memset(vf->txq_mask, 0, sizeof(vf->txq_mask));
 	vf->txq_count = 0;
@@ -674,7 +676,7 @@ static void efx_vfdi_flush_clear(struct efx_vf *vf)
 	atomic_set(&vf->rxq_retry_count, 0);
 }
 
-static int efx_vfdi_fini_all_queues(struct efx_vf *vf)
+static int efx_vfdi_fini_all_queues(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	efx_oword_t reg;
@@ -757,7 +759,7 @@ static int efx_vfdi_fini_all_queues(struct efx_vf *vf)
 	return timeout ? 0 : VFDI_RC_ETIMEDOUT;
 }
 
-static int efx_vfdi_insert_filter(struct efx_vf *vf)
+static int efx_vfdi_insert_filter(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct siena_nic_data *nic_data = efx->nic_data;
@@ -789,7 +791,7 @@ static int efx_vfdi_insert_filter(struct efx_vf *vf)
 	return VFDI_RC_SUCCESS;
 }
 
-static int efx_vfdi_remove_all_filters(struct efx_vf *vf)
+static int efx_vfdi_remove_all_filters(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct siena_nic_data *nic_data = efx->nic_data;
@@ -801,7 +803,7 @@ static int efx_vfdi_remove_all_filters(struct efx_vf *vf)
 	return VFDI_RC_SUCCESS;
 }
 
-static int efx_vfdi_set_status_page(struct efx_vf *vf)
+static int efx_vfdi_set_status_page(struct siena_vf *vf)
 {
 	struct efx_nic *efx = vf->efx;
 	struct siena_nic_data *nic_data = efx->nic_data;
@@ -846,7 +848,7 @@ static int efx_vfdi_set_status_page(struct efx_vf *vf)
 	return VFDI_RC_SUCCESS;
 }
 
-static int efx_vfdi_clear_status_page(struct efx_vf *vf)
+static int efx_vfdi_clear_status_page(struct siena_vf *vf)
 {
 	mutex_lock(&vf->status_lock);
 	vf->status_addr = 0;
@@ -855,7 +857,7 @@ static int efx_vfdi_clear_status_page(struct efx_vf *vf)
 	return VFDI_RC_SUCCESS;
 }
 
-typedef int (*efx_vfdi_op_t)(struct efx_vf *vf);
+typedef int (*efx_vfdi_op_t)(struct siena_vf *vf);
 
 static const efx_vfdi_op_t vfdi_ops[VFDI_OP_LIMIT] = {
 	[VFDI_OP_INIT_EVQ] = efx_vfdi_init_evq,
@@ -870,7 +872,7 @@ static const efx_vfdi_op_t vfdi_ops[VFDI_OP_LIMIT] = {
 
 static void efx_siena_sriov_vfdi(struct work_struct *work)
 {
-	struct efx_vf *vf = container_of(work, struct efx_vf, req);
+	struct siena_vf *vf = container_of(work, struct siena_vf, req);
 	struct efx_nic *efx = vf->efx;
 	struct vfdi_req *req = vf->buf.addr;
 	struct efx_memcpy_req copy[2];
@@ -936,7 +938,8 @@ static void efx_siena_sriov_vfdi(struct work_struct *work)
  * event ring in guest memory with VFDI reset events, then (re-initialise) the
  * event queue to raise an interrupt. The guest driver will then recover.
  */
-static void efx_siena_sriov_reset_vf(struct efx_vf *vf,
+
+static void efx_siena_sriov_reset_vf(struct siena_vf *vf,
 				     struct efx_buffer *buffer)
 {
 	struct efx_nic *efx = vf->efx;
@@ -1006,7 +1009,7 @@ static void efx_siena_sriov_reset_vf(struct efx_vf *vf,
 
 static void efx_siena_sriov_reset_vf_work(struct work_struct *work)
 {
-	struct efx_vf *vf = container_of(work, struct efx_vf, req);
+	struct siena_vf *vf = container_of(work, struct siena_vf, req);
 	struct efx_nic *efx = vf->efx;
 	struct efx_buffer buf;
 
@@ -1055,8 +1058,10 @@ void efx_siena_sriov_probe(struct efx_nic *efx)
 	if (!max_vfs)
 		return;
 
-	if (efx_siena_sriov_cmd(efx, false, &efx->vi_scale, &count))
+	if (efx_siena_sriov_cmd(efx, false, &efx->vi_scale, &count)) {
+		netif_info(efx, probe, efx->net_dev, "no SR-IOV VFs probed\n");
 		return;
+	}
 	if (count > 0 && count > max_vfs)
 		count = max_vfs;
 
@@ -1077,7 +1082,7 @@ static void efx_siena_sriov_peer_work(struct work_struct *data)
 						       peer_work);
 	struct efx_nic *efx = nic_data->efx;
 	struct vfdi_status *vfdi_status = nic_data->vfdi_status.addr;
-	struct efx_vf *vf;
+	struct siena_vf *vf;
 	struct efx_local_addr *local_addr;
 	struct vfdi_endpoint *peer;
 	struct efx_endpoint_page *epp;
@@ -1099,7 +1104,7 @@ static void efx_siena_sriov_peer_work(struct work_struct *data)
 	peer_space = ARRAY_SIZE(vfdi_status->peers) - 1;
 	peer_count = 1;
 	for (pos = 0; pos < efx->vf_count; ++pos) {
-		vf = efx->vf + pos;
+		vf = nic_data->vf + pos;
 
 		mutex_lock(&vf->status_lock);
 		if (vf->rx_filtering && !is_zero_ether_addr(vf->addr.mac_addr)) {
@@ -1155,7 +1160,7 @@ static void efx_siena_sriov_peer_work(struct work_struct *data)
 
 	/* Finally, push the pages */
 	for (pos = 0; pos < efx->vf_count; ++pos) {
-		vf = efx->vf + pos;
+		vf = nic_data->vf + pos;
 
 		mutex_lock(&vf->status_lock);
 		if (vf->status_addr)
@@ -1190,14 +1195,16 @@ static void efx_siena_sriov_free_local(struct efx_nic *efx)
 static int efx_siena_sriov_vf_alloc(struct efx_nic *efx)
 {
 	unsigned index;
-	struct efx_vf *vf;
+	struct siena_vf *vf;
+	struct siena_nic_data *nic_data = efx->nic_data;
 
-	efx->vf = kzalloc(sizeof(struct efx_vf) * efx->vf_count, GFP_KERNEL);
-	if (!efx->vf)
+	nic_data->vf = kcalloc(efx->vf_count, sizeof(*nic_data->vf),
+			       GFP_KERNEL);
+	if (!nic_data->vf)
 		return -ENOMEM;
 
 	for (index = 0; index < efx->vf_count; ++index) {
-		vf = efx->vf + index;
+		vf = nic_data->vf + index;
 
 		vf->efx = efx;
 		vf->index = index;
@@ -1216,11 +1223,12 @@ static int efx_siena_sriov_vf_alloc(struct efx_nic *efx)
 
 static void efx_siena_sriov_vfs_fini(struct efx_nic *efx)
 {
-	struct efx_vf *vf;
+	struct siena_nic_data *nic_data = efx->nic_data;
+	struct siena_vf *vf;
 	unsigned int pos;
 
 	for (pos = 0; pos < efx->vf_count; ++pos) {
-		vf = efx->vf + pos;
+		vf = nic_data->vf + pos;
 
 		efx_nic_free_buffer(efx, &vf->buf);
 		kfree(vf->peer_page_addrs);
@@ -1237,7 +1245,7 @@ static int efx_siena_sriov_vfs_init(struct efx_nic *efx)
 	struct siena_nic_data *nic_data = efx->nic_data;
 	unsigned index, devfn, sriov, buftbl_base;
 	u16 offset, stride;
-	struct efx_vf *vf;
+	struct siena_vf *vf;
 	int rc;
 
 	sriov = pci_find_ext_capability(pci_dev, PCI_EXT_CAP_ID_SRIOV);
@@ -1250,7 +1258,7 @@ static int efx_siena_sriov_vfs_init(struct efx_nic *efx)
 	buftbl_base = nic_data->vf_buftbl_base;
 	devfn = pci_dev->devfn + offset;
 	for (index = 0; index < efx->vf_count; ++index) {
-		vf = efx->vf + index;
+		vf = nic_data->vf + index;
 
 		/* Reserve buffer entries */
 		vf->buftbl_base = buftbl_base;
@@ -1350,7 +1358,7 @@ fail_pci:
 fail_vfs:
 	cancel_work_sync(&nic_data->peer_work);
 	efx_siena_sriov_free_local(efx);
-	kfree(efx->vf);
+	kfree(nic_data->vf);
 fail_alloc:
 	efx_nic_free_buffer(efx, &nic_data->vfdi_status);
 fail_status:
@@ -1361,7 +1369,7 @@ fail_cmd:
 
 void efx_siena_sriov_fini(struct efx_nic *efx)
 {
-	struct efx_vf *vf;
+	struct siena_vf *vf;
 	unsigned int pos;
 	struct siena_nic_data *nic_data = efx->nic_data;
 
@@ -1377,7 +1385,7 @@ void efx_siena_sriov_fini(struct efx_nic *efx)
 
 	/* Flush all reconfiguration work */
 	for (pos = 0; pos < efx->vf_count; ++pos) {
-		vf = efx->vf + pos;
+		vf = nic_data->vf + pos;
 		cancel_work_sync(&vf->req);
 		cancel_work_sync(&vf->reset_work);
 	}
@@ -1388,7 +1396,7 @@ void efx_siena_sriov_fini(struct efx_nic *efx)
 	/* Tear down back-end state */
 	efx_siena_sriov_vfs_fini(efx);
 	efx_siena_sriov_free_local(efx);
-	kfree(efx->vf);
+	kfree(nic_data->vf);
 	efx_nic_free_buffer(efx, &nic_data->vfdi_status);
 	efx_siena_sriov_cmd(efx, false, NULL, NULL);
 }
@@ -1396,7 +1404,7 @@ void efx_siena_sriov_fini(struct efx_nic *efx)
 void efx_siena_sriov_event(struct efx_channel *channel, efx_qword_t *event)
 {
 	struct efx_nic *efx = channel->efx;
-	struct efx_vf *vf;
+	struct siena_vf *vf;
 	unsigned qid, seq, type, data;
 
 	qid = EFX_QWORD_FIELD(*event, FSF_CZ_USER_QID);
@@ -1452,11 +1460,12 @@ error:
 
 void efx_siena_sriov_flr(struct efx_nic *efx, unsigned vf_i)
 {
-	struct efx_vf *vf;
+	struct siena_nic_data *nic_data = efx->nic_data;
+	struct siena_vf *vf;
 
 	if (vf_i > efx->vf_init_count)
 		return;
-	vf = efx->vf + vf_i;
+	vf = nic_data->vf + vf_i;
 	netif_info(efx, hw, efx->net_dev,
 		   "FLR on VF %s\n", vf->pci_name);
 
@@ -1467,21 +1476,23 @@ void efx_siena_sriov_flr(struct efx_nic *efx, unsigned vf_i)
 	vf->evq0_count = 0;
 }
 
-void efx_siena_sriov_mac_address_changed(struct efx_nic *efx)
+int efx_siena_sriov_mac_address_changed(struct efx_nic *efx)
 {
 	struct siena_nic_data *nic_data = efx->nic_data;
 	struct vfdi_status *vfdi_status = nic_data->vfdi_status.addr;
 
 	if (!efx->vf_init_count)
-		return;
+		return 0;
 	ether_addr_copy(vfdi_status->peers[0].mac_addr,
 			efx->net_dev->dev_addr);
 	queue_work(vfdi_workqueue, &nic_data->peer_work);
+
+	return 0;
 }
 
 void efx_siena_sriov_tx_flush_done(struct efx_nic *efx, efx_qword_t *event)
 {
-	struct efx_vf *vf;
+	struct siena_vf *vf;
 	unsigned queue, qid;
 
 	queue = EFX_QWORD_FIELD(*event,  FSF_AZ_DRIVER_EV_SUBDATA);
@@ -1500,7 +1511,7 @@ void efx_siena_sriov_tx_flush_done(struct efx_nic *efx, efx_qword_t *event)
 
 void efx_siena_sriov_rx_flush_done(struct efx_nic *efx, efx_qword_t *event)
 {
-	struct efx_vf *vf;
+	struct siena_vf *vf;
 	unsigned ev_failed, queue, qid;
 
 	queue = EFX_QWORD_FIELD(*event, FSF_AZ_DRIVER_EV_RX_DESCQ_ID);
@@ -1525,7 +1536,7 @@ void efx_siena_sriov_rx_flush_done(struct efx_nic *efx, efx_qword_t *event)
 /* Called from napi. Schedule the reset work item */
 void efx_siena_sriov_desc_fetch_err(struct efx_nic *efx, unsigned dmaq)
 {
-	struct efx_vf *vf;
+	struct siena_vf *vf;
 	unsigned int rel;
 
 	if (map_vi_index(efx, dmaq, &vf, &rel))
@@ -1541,9 +1552,10 @@ void efx_siena_sriov_desc_fetch_err(struct efx_nic *efx, unsigned dmaq)
 /* Reset all VFs */
 void efx_siena_sriov_reset(struct efx_nic *efx)
 {
+	struct siena_nic_data *nic_data = efx->nic_data;
 	unsigned int vf_i;
 	struct efx_buffer buf;
-	struct efx_vf *vf;
+	struct siena_vf *vf;
 
 	ASSERT_RTNL();
 
@@ -1557,7 +1569,7 @@ void efx_siena_sriov_reset(struct efx_nic *efx)
 		return;
 
 	for (vf_i = 0; vf_i < efx->vf_init_count; ++vf_i) {
-		vf = efx->vf + vf_i;
+		vf = nic_data->vf + vf_i;
 		efx_siena_sriov_reset_vf(vf, &buf);
 	}
 
@@ -1573,7 +1585,6 @@ int efx_init_sriov(void)
 	vfdi_workqueue = create_singlethread_workqueue("sfc_vfdi");
 	if (!vfdi_workqueue)
 		return -ENOMEM;
-
 	return 0;
 }
 
@@ -1582,14 +1593,14 @@ void efx_fini_sriov(void)
 	destroy_workqueue(vfdi_workqueue);
 }
 
-int efx_siena_sriov_set_vf_mac(struct net_device *net_dev, int vf_i, u8 *mac)
+int efx_siena_sriov_set_vf_mac(struct efx_nic *efx, int vf_i, u8 *mac)
 {
-	struct efx_nic *efx = netdev_priv(net_dev);
-	struct efx_vf *vf;
+	struct siena_nic_data *nic_data = efx->nic_data;
+	struct siena_vf *vf;
 
 	if (vf_i >= efx->vf_init_count)
 		return -EINVAL;
-	vf = efx->vf + vf_i;
+	vf = nic_data->vf + vf_i;
 
 	mutex_lock(&vf->status_lock);
 	ether_addr_copy(vf->addr.mac_addr, mac);
@@ -1599,16 +1610,16 @@ int efx_siena_sriov_set_vf_mac(struct net_device *net_dev, int vf_i, u8 *mac)
 	return 0;
 }
 
-int efx_siena_sriov_set_vf_vlan(struct net_device *net_dev, int vf_i,
+int efx_siena_sriov_set_vf_vlan(struct efx_nic *efx, int vf_i,
 				u16 vlan, u8 qos)
 {
-	struct efx_nic *efx = netdev_priv(net_dev);
-	struct efx_vf *vf;
+	struct siena_nic_data *nic_data = efx->nic_data;
+	struct siena_vf *vf;
 	u16 tci;
 
 	if (vf_i >= efx->vf_init_count)
 		return -EINVAL;
-	vf = efx->vf + vf_i;
+	vf = nic_data->vf + vf_i;
 
 	mutex_lock(&vf->status_lock);
 	tci = (vlan & VLAN_VID_MASK) | ((qos & 0x7) << VLAN_PRIO_SHIFT);
@@ -1619,16 +1630,16 @@ int efx_siena_sriov_set_vf_vlan(struct net_device *net_dev, int vf_i,
 	return 0;
 }
 
-int efx_siena_sriov_set_vf_spoofchk(struct net_device *net_dev, int vf_i,
+int efx_siena_sriov_set_vf_spoofchk(struct efx_nic *efx, int vf_i,
 				    bool spoofchk)
 {
-	struct efx_nic *efx = netdev_priv(net_dev);
-	struct efx_vf *vf;
+	struct siena_nic_data *nic_data = efx->nic_data;
+	struct siena_vf *vf;
 	int rc;
 
 	if (vf_i >= efx->vf_init_count)
 		return -EINVAL;
-	vf = efx->vf + vf_i;
+	vf = nic_data->vf + vf_i;
 
 	mutex_lock(&vf->txq_lock);
 	if (vf->txq_count == 0) {
@@ -1643,16 +1654,16 @@ int efx_siena_sriov_set_vf_spoofchk(struct net_device *net_dev, int vf_i,
 	return rc;
 }
 
-int efx_siena_sriov_get_vf_config(struct net_device *net_dev, int vf_i,
+int efx_siena_sriov_get_vf_config(struct efx_nic *efx, int vf_i,
 				  struct ifla_vf_info *ivi)
 {
-	struct efx_nic *efx = netdev_priv(net_dev);
-	struct efx_vf *vf;
+	struct siena_nic_data *nic_data = efx->nic_data;
+	struct siena_vf *vf;
 	u16 tci;
 
 	if (vf_i >= efx->vf_init_count)
 		return -EINVAL;
-	vf = efx->vf + vf_i;
+	vf = nic_data->vf + vf_i;
 
 	ivi->vf = vf_i;
 	ether_addr_copy(ivi->mac, vf->addr.mac_addr);
@@ -1666,3 +1677,12 @@ int efx_siena_sriov_get_vf_config(struct net_device *net_dev, int vf_i,
 	return 0;
 }
 
+bool efx_siena_sriov_wanted(struct efx_nic *efx)
+{
+	return efx->vf_count != 0;
+}
+
+int efx_siena_sriov_configure(struct efx_nic *efx, int num_vfs)
+{
+	return 0;
+}
diff --git a/drivers/net/ethernet/sfc/siena_sriov.h b/drivers/net/ethernet/sfc/siena_sriov.h
new file mode 100644
index 000000000000..d88d4dab170a
--- /dev/null
+++ b/drivers/net/ethernet/sfc/siena_sriov.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ * Driver for Solarflare network controllers and boards
+ * Copyright 2015 Solarflare Communications 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, incorporated herein by reference.
+ */
+
+#ifndef SIENA_SRIOV_H
+#define SIENA_SRIOV_H
+
+#include "net_driver.h"
+
+/* On the SFC9000 family each port is associated with 1 PCI physical
+ * function (PF) handled by sfc and a configurable number of virtual
+ * functions (VFs) that may be handled by some other driver, often in
+ * a VM guest.  The queue pointer registers are mapped in both PF and
+ * VF BARs such that an 8K region provides access to a single RX, TX
+ * and event queue (collectively a Virtual Interface, VI or VNIC).
+ *
+ * The PF has access to all 1024 VIs while VFs are mapped to VIs
+ * according to VI_BASE and VI_SCALE: VF i has access to VIs numbered
+ * in range [VI_BASE + i << VI_SCALE, VI_BASE + i + 1 << VI_SCALE).
+ * The number of VIs and the VI_SCALE value are configurable but must
+ * be established at boot time by firmware.
+ */
+
+/* Maximum VI_SCALE parameter supported by Siena */
+#define EFX_VI_SCALE_MAX 6
+/* Base VI to use for SR-IOV. Must be aligned to (1 << EFX_VI_SCALE_MAX),
+ * so this is the smallest allowed value.
+ */
+#define EFX_VI_BASE 128U
+/* Maximum number of VFs allowed */
+#define EFX_VF_COUNT_MAX 127
+/* Limit EVQs on VFs to be only 8k to reduce buffer table reservation */
+#define EFX_MAX_VF_EVQ_SIZE 8192UL
+/* The number of buffer table entries reserved for each VI on a VF */
+#define EFX_VF_BUFTBL_PER_VI					\
+	((EFX_MAX_VF_EVQ_SIZE + 2 * EFX_MAX_DMAQ_SIZE) *	\
+	 sizeof(efx_qword_t) / EFX_BUF_SIZE)
+
+int efx_siena_sriov_configure(struct efx_nic *efx, int num_vfs);
+int efx_siena_sriov_init(struct efx_nic *efx);
+void efx_siena_sriov_fini(struct efx_nic *efx);
+int efx_siena_sriov_mac_address_changed(struct efx_nic *efx);
+bool efx_siena_sriov_wanted(struct efx_nic *efx);
+void efx_siena_sriov_reset(struct efx_nic *efx);
+void efx_siena_sriov_flr(struct efx_nic *efx, unsigned flr);
+
+int efx_siena_sriov_set_vf_mac(struct efx_nic *efx, int vf, u8 *mac);
+int efx_siena_sriov_set_vf_vlan(struct efx_nic *efx, int vf,
+				u16 vlan, u8 qos);
+int efx_siena_sriov_set_vf_spoofchk(struct efx_nic *efx, int vf,
+				    bool spoofchk);
+int efx_siena_sriov_get_vf_config(struct efx_nic *efx, int vf,
+				  struct ifla_vf_info *ivf);
+
+#ifdef CONFIG_SFC_SRIOV
+
+static inline bool efx_siena_sriov_enabled(struct efx_nic *efx)
+{
+	return efx->vf_init_count != 0;
+}
+#else /* !CONFIG_SFC_SRIOV */
+static inline bool efx_siena_sriov_enabled(struct efx_nic *efx)
+{
+	return false;
+}
+#endif /* CONFIG_SFC_SRIOV */
+
+void efx_siena_sriov_probe(struct efx_nic *efx);
+void efx_siena_sriov_tx_flush_done(struct efx_nic *efx, efx_qword_t *event);
+void efx_siena_sriov_rx_flush_done(struct efx_nic *efx, efx_qword_t *event);
+void efx_siena_sriov_event(struct efx_channel *channel, efx_qword_t *event);
+void efx_siena_sriov_desc_fetch_err(struct efx_nic *efx, unsigned dmaq);
+
+#endif /* SIENA_SRIOV_H */
diff --git a/drivers/net/ethernet/sfc/sriov.c b/drivers/net/ethernet/sfc/sriov.c
new file mode 100644
index 000000000000..816c44689e67
--- /dev/null
+++ b/drivers/net/ethernet/sfc/sriov.c
@@ -0,0 +1,83 @@
+/****************************************************************************
+ * Driver for Solarflare network controllers and boards
+ * Copyright 2014-2015 Solarflare Communications 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, incorporated herein by reference.
+ */
+#include <linux/module.h>
+#include "net_driver.h"
+#include "nic.h"
+#include "sriov.h"
+
+int efx_sriov_set_vf_mac(struct net_device *net_dev, int vf_i, u8 *mac)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+
+	if (efx->type->sriov_set_vf_mac)
+		return efx->type->sriov_set_vf_mac(efx, vf_i, mac);
+	else
+		return -EOPNOTSUPP;
+}
+
+int efx_sriov_set_vf_vlan(struct net_device *net_dev, int vf_i, u16 vlan,
+			  u8 qos)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+
+	if (efx->type->sriov_set_vf_vlan) {
+		if ((vlan & ~VLAN_VID_MASK) ||
+		    (qos & ~(VLAN_PRIO_MASK >> VLAN_PRIO_SHIFT)))
+			return -EINVAL;
+
+		return efx->type->sriov_set_vf_vlan(efx, vf_i, vlan, qos);
+	} else {
+		return -EOPNOTSUPP;
+	}
+}
+
+int efx_sriov_set_vf_spoofchk(struct net_device *net_dev, int vf_i,
+			      bool spoofchk)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+
+	if (efx->type->sriov_set_vf_spoofchk)
+		return efx->type->sriov_set_vf_spoofchk(efx, vf_i, spoofchk);
+	else
+		return -EOPNOTSUPP;
+}
+
+int efx_sriov_get_vf_config(struct net_device *net_dev, int vf_i,
+			    struct ifla_vf_info *ivi)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+
+	if (efx->type->sriov_get_vf_config)
+		return efx->type->sriov_get_vf_config(efx, vf_i, ivi);
+	else
+		return -EOPNOTSUPP;
+}
+
+int efx_sriov_set_vf_link_state(struct net_device *net_dev, int vf_i,
+				int link_state)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+
+	if (efx->type->sriov_set_vf_link_state)
+		return efx->type->sriov_set_vf_link_state(efx, vf_i,
+							  link_state);
+	else
+		return -EOPNOTSUPP;
+}
+
+int efx_sriov_get_phys_port_id(struct net_device *net_dev,
+			       struct netdev_phys_item_id *ppid)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
+
+	if (efx->type->sriov_get_phys_port_id)
+		return efx->type->sriov_get_phys_port_id(efx, ppid);
+	else
+		return -EOPNOTSUPP;
+}
diff --git a/drivers/net/ethernet/sfc/sriov.h b/drivers/net/ethernet/sfc/sriov.h
new file mode 100644
index 000000000000..400df526586d
--- /dev/null
+++ b/drivers/net/ethernet/sfc/sriov.h
@@ -0,0 +1,31 @@
+/****************************************************************************
+ * Driver for Solarflare network controllers and boards
+ * Copyright 2014-2015 Solarflare Communications 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, incorporated herein by reference.
+ */
+
+#ifndef EFX_SRIOV_H
+#define EFX_SRIOV_H
+
+#include "net_driver.h"
+
+#ifdef CONFIG_SFC_SRIOV
+
+int efx_sriov_set_vf_mac(struct net_device *net_dev, int vf_i, u8 *mac);
+int efx_sriov_set_vf_vlan(struct net_device *net_dev, int vf_i, u16 vlan,
+			  u8 qos);
+int efx_sriov_set_vf_spoofchk(struct net_device *net_dev, int vf_i,
+			      bool spoofchk);
+int efx_sriov_get_vf_config(struct net_device *net_dev, int vf_i,
+			    struct ifla_vf_info *ivi);
+int efx_sriov_set_vf_link_state(struct net_device *net_dev, int vf_i,
+				int link_state);
+int efx_sriov_get_phys_port_id(struct net_device *net_dev,
+			       struct netdev_phys_item_id *ppid);
+
+#endif /* CONFIG_SFC_SRIOV */
+
+#endif /* EFX_SRIOV_H */
diff --git a/drivers/net/ethernet/sgi/Kconfig b/drivers/net/ethernet/sgi/Kconfig
index e832f46660c9..fbbb21c13e95 100644
--- a/drivers/net/ethernet/sgi/Kconfig
+++ b/drivers/net/ethernet/sgi/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_SGI
 	default y
 	depends on (PCI && SGI_IP27) || SGI_IP32
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -24,9 +22,7 @@ config SGI_IOC3_ETH
 	select CRC32
 	select MII
 	---help---
-	  If you have a network (Ethernet) card of this type, say Y and read
-	  the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card of this type, say Y here.
 
 config SGI_O2MACE_ETH
 	tristate "SGI O2 MACE Fast Ethernet support"
diff --git a/drivers/net/ethernet/silan/Kconfig b/drivers/net/ethernet/silan/Kconfig
index 3409b3f97a1b..ac982be38510 100644
--- a/drivers/net/ethernet/silan/Kconfig
+++ b/drivers/net/ethernet/silan/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_SILAN
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/sis/Kconfig b/drivers/net/ethernet/sis/Kconfig
index 68d052b09af1..22ec98ec9d3e 100644
--- a/drivers/net/ethernet/sis/Kconfig
+++ b/drivers/net/ethernet/sis/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_SIS
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/sis/sis900.h b/drivers/net/ethernet/sis/sis900.h
index 1341f33e6084..7d430d322931 100644
--- a/drivers/net/ethernet/sis/sis900.h
+++ b/drivers/net/ethernet/sis/sis900.h
@@ -56,7 +56,7 @@ enum sis900_configuration_register_bits {
 	EDB_MASTER_EN = 0x00002000
 };
 
-enum sis900_eeprom_access_reigster_bits {
+enum sis900_eeprom_access_register_bits {
 	MDC  = 0x00000040, MDDIR = 0x00000020, MDIO = 0x00000010, /* 7016 specific */
 	EECS = 0x00000008, EECLK = 0x00000004, EEDO = 0x00000002,
 	EEDI = 0x00000001
@@ -73,7 +73,7 @@ enum sis900_interrupt_register_bits {
 	RxERR  = 0x00000004, RxDESC = 0x00000002, RxOK  = 0x00000001
 };
 
-enum sis900_interrupt_enable_reigster_bits {
+enum sis900_interrupt_enable_register_bits {
 	IE = 0x00000001
 };
 
diff --git a/drivers/net/ethernet/smsc/Kconfig b/drivers/net/ethernet/smsc/Kconfig
index 3e97a8b43147..eb9230e2092f 100644
--- a/drivers/net/ethernet/smsc/Kconfig
+++ b/drivers/net/ethernet/smsc/Kconfig
@@ -9,9 +9,7 @@ config NET_VENDOR_SMSC
 		   ISA || M32R || MAC || MIPS || MN10300 || NIOS2 || PCI || \
 		   PCMCIA || SUPERH || XTENSA
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -29,8 +27,7 @@ config SMC9194
 	  option if you have a DELL laptop with the docking station, or
 	  another SMC9192/9194 based chipset.  Say Y if you want it compiled
 	  into the kernel, and read the file
-	  <file:Documentation/networking/smc9.txt> and the Ethernet-HOWTO,
-	  available from <http://www.tldp.org/docs.html#howto>.
+	  <file:Documentation/networking/smc9.txt>.
 
 	  To compile this driver as a module, choose M here. The module
 	  will be called smc9194.
@@ -46,8 +43,7 @@ config SMC91X
 	  This is a driver for SMC's 91x series of Ethernet chipsets,
 	  including the SMC91C94 and the SMC91C111. Say Y if you want it
 	  compiled into the kernel, and read the file
-	  <file:Documentation/networking/smc9.txt>  and the Ethernet-HOWTO,
-	  available from  <http://www.tldp.org/docs.html#howto>.
+	  <file:Documentation/networking/smc9.txt>.
 
 	  This driver is also available as a module ( = code which can be
 	  inserted in and removed from the running kernel whenever you want).
@@ -85,9 +81,7 @@ config SMC911X
 	---help---
 	  This is a driver for SMSC's LAN911x series of Ethernet chipsets
 	  including the new LAN9115, LAN9116, LAN9117, and LAN9118.
-	  Say Y if you want it compiled into the kernel,
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  Say Y here if you want it compiled into the kernel.
 
 	  This driver is also available as a module. The module will be
 	  called smc911x.  If you want to compile it as a module, say M
@@ -122,9 +116,7 @@ config SMSC9420
 	select SMSC_PHY
 	---help---
 	  This is a driver for SMSC's LAN9420 PCI ethernet adapter.
-	  Say Y if you want it compiled into the kernel,
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  Say Y here if you want it compiled into the kernel.
 
 	  This driver is also available as a module. The module will be
 	  called smsc9420.  If you want to compile it as a module, say M
diff --git a/drivers/net/ethernet/stmicro/Kconfig b/drivers/net/ethernet/stmicro/Kconfig
index f4a80da00650..1c1157d2bd40 100644
--- a/drivers/net/ethernet/stmicro/Kconfig
+++ b/drivers/net/ethernet/stmicro/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_STMICRO
 	default y
 	depends on HAS_IOMEM
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/stmicro/stmmac/Kconfig b/drivers/net/ethernet/stmicro/stmmac/Kconfig
index 7d3af190be55..cec147d1d34f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Kconfig
+++ b/drivers/net/ethernet/stmicro/stmmac/Kconfig
@@ -16,6 +16,7 @@ if STMMAC_ETH
 config STMMAC_PLATFORM
 	tristate "STMMAC Platform bus support"
 	depends on STMMAC_ETH
+	select MFD_SYSCON
 	default y
 	---help---
 	  This selects the platform specific bus support for the stmmac driver.
@@ -26,6 +27,95 @@ config STMMAC_PLATFORM
 
 	  If unsure, say N.
 
+if STMMAC_PLATFORM
+
+config DWMAC_GENERIC
+	tristate "Generic driver for DWMAC"
+	default STMMAC_PLATFORM
+	---help---
+	  Generic DWMAC driver for platforms that don't require any
+	  platform specific code to function or is using platform
+	  data for setup.
+
+config DWMAC_IPQ806X
+	tristate "QCA IPQ806x DWMAC support"
+	default ARCH_QCOM
+	depends on OF
+	select MFD_SYSCON
+	help
+	  Support for QCA IPQ806X DWMAC Ethernet.
+
+	  This selects the IPQ806x SoC glue layer support for the stmmac
+	  device driver. This driver does not use any of the hardware
+	  acceleration features available on this SoC. Network devices
+	  will behave like standard non-accelerated ethernet interfaces.
+
+config DWMAC_LPC18XX
+	tristate "NXP LPC18xx/43xx DWMAC support"
+	default ARCH_LPC18XX
+	depends on OF
+	select MFD_SYSCON
+	---help---
+	  Support for NXP LPC18xx/43xx DWMAC Ethernet.
+
+config DWMAC_MESON
+	tristate "Amlogic Meson dwmac support"
+	default ARCH_MESON
+	depends on OF
+	help
+	  Support for Ethernet controller on Amlogic Meson SoCs.
+
+	  This selects the Amlogic Meson SoC glue layer support for
+	  the stmmac device driver. This driver is used for Meson6 and
+	  Meson8 SoCs.
+
+config DWMAC_ROCKCHIP
+	tristate "Rockchip dwmac support"
+	default ARCH_ROCKCHIP
+	depends on OF
+	select MFD_SYSCON
+	help
+	  Support for Ethernet controller on Rockchip RK3288 SoC.
+
+	  This selects the Rockchip RK3288 SoC glue layer support for
+	  the stmmac device driver.
+
+config DWMAC_SOCFPGA
+	tristate "SOCFPGA dwmac support"
+	default ARCH_SOCFPGA
+	depends on OF
+	select MFD_SYSCON
+	help
+	  Support for ethernet controller on Altera SOCFPGA
+
+	  This selects the Altera SOCFPGA SoC glue layer support
+	  for the stmmac device driver. This driver is used for
+	  arria5 and cyclone5 FPGA SoCs.
+
+config DWMAC_STI
+	tristate "STi GMAC support"
+	default ARCH_STI
+	depends on OF
+	select MFD_SYSCON
+	---help---
+	  Support for ethernet controller on STi SOCs.
+
+	  This selects STi SoC glue layer support for the stmmac
+	  device driver. This driver is used on for the STi series
+	  SOCs GMAC ethernet controller.
+
+config DWMAC_SUNXI
+	tristate "Allwinner GMAC support"
+	default ARCH_SUNXI
+	depends on OF
+	---help---
+	  Support for Allwinner A20/A31 GMAC ethernet controllers.
+
+	  This selects Allwinner SoC glue layer support for the
+	  stmmac device driver. This driver is used for A20/A31
+	  GMAC ethernet controller.
+endif
+
 config STMMAC_PCI
 	tristate "STMMAC PCI bus support"
 	depends on STMMAC_ETH && PCI
diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile
index 73c2715a27f3..b3901616f4f6 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Makefile
+++ b/drivers/net/ethernet/stmicro/stmmac/Makefile
@@ -4,9 +4,17 @@ stmmac-objs:= stmmac_main.o stmmac_ethtool.o stmmac_mdio.o ring_mode.o	\
 	      dwmac100_core.o dwmac100_dma.o enh_desc.o norm_desc.o	\
 	      mmc_core.o stmmac_hwtstamp.o stmmac_ptp.o $(stmmac-y)
 
-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-rk.o
+# Ordering matters. Generic driver must be last.
+obj-$(CONFIG_STMMAC_PLATFORM)	+= stmmac-platform.o
+obj-$(CONFIG_DWMAC_IPQ806X)	+= dwmac-ipq806x.o
+obj-$(CONFIG_DWMAC_LPC18XX)	+= dwmac-lpc18xx.o
+obj-$(CONFIG_DWMAC_MESON)	+= dwmac-meson.o
+obj-$(CONFIG_DWMAC_ROCKCHIP)	+= dwmac-rk.o
+obj-$(CONFIG_DWMAC_SOCFPGA)	+= dwmac-socfpga.o
+obj-$(CONFIG_DWMAC_STI)		+= dwmac-sti.o
+obj-$(CONFIG_DWMAC_SUNXI)	+= dwmac-sunxi.o
+obj-$(CONFIG_DWMAC_GENERIC)	+= dwmac-generic.o
+stmmac-platform-objs:= stmmac_platform.o
 
 obj-$(CONFIG_STMMAC_PCI) += stmmac-pci.o
 stmmac-pci-objs:= stmmac_pci.o
diff --git a/drivers/net/ethernet/stmicro/stmmac/descs.h b/drivers/net/ethernet/stmicro/stmmac/descs.h
index ad3996038018..799c2929c536 100644
--- a/drivers/net/ethernet/stmicro/stmmac/descs.h
+++ b/drivers/net/ethernet/stmicro/stmmac/descs.h
@@ -158,6 +158,8 @@ struct dma_desc {
 			u32 buffer2_size:13;
 			u32 reserved4:3;
 		} etx;		/* -- enhanced -- */
+
+		u64 all_flags;
 	} des01;
 	unsigned int des2;
 	unsigned int des3;
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-generic.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-generic.c
new file mode 100644
index 000000000000..e817a1a44379
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-generic.c
@@ -0,0 +1,41 @@
+/*
+ * Generic DWMAC platform driver
+ *
+ * Copyright (C) 2007-2011  STMicroelectronics Ltd
+ * Copyright (C) 2015 Joachim Eastwood <manabian@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/of.h>
+#include <linux/platform_device.h>
+
+#include "stmmac.h"
+#include "stmmac_platform.h"
+
+static const struct of_device_id dwmac_generic_match[] = {
+	{ .compatible = "st,spear600-gmac"},
+	{ .compatible = "snps,dwmac-3.610"},
+	{ .compatible = "snps,dwmac-3.70a"},
+	{ .compatible = "snps,dwmac-3.710"},
+	{ .compatible = "snps,dwmac"},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, dwmac_generic_match);
+
+static struct platform_driver dwmac_generic_driver = {
+	.probe  = stmmac_pltfr_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name           = STMMAC_RESOURCE_NAME,
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = of_match_ptr(dwmac_generic_match),
+	},
+};
+module_platform_driver(dwmac_generic_driver);
+
+MODULE_DESCRIPTION("Generic dwmac driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c
new file mode 100644
index 000000000000..7e3129e7f143
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c
@@ -0,0 +1,365 @@
+/*
+ * Qualcomm Atheros IPQ806x GMAC glue layer
+ *
+ * Copyright (C) 2015 The Linux Foundation
+ *
+ * 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/platform_device.h>
+#include <linux/phy.h>
+#include <linux/regmap.h>
+#include <linux/clk.h>
+#include <linux/reset.h>
+#include <linux/of_net.h>
+#include <linux/mfd/syscon.h>
+#include <linux/stmmac.h>
+#include <linux/of_mdio.h>
+#include <linux/module.h>
+
+#include "stmmac_platform.h"
+
+#define NSS_COMMON_CLK_GATE			0x8
+#define NSS_COMMON_CLK_GATE_PTP_EN(x)		BIT(0x10 + x)
+#define NSS_COMMON_CLK_GATE_RGMII_RX_EN(x)	BIT(0x9 + (x * 2))
+#define NSS_COMMON_CLK_GATE_RGMII_TX_EN(x)	BIT(0x8 + (x * 2))
+#define NSS_COMMON_CLK_GATE_GMII_RX_EN(x)	BIT(0x4 + x)
+#define NSS_COMMON_CLK_GATE_GMII_TX_EN(x)	BIT(0x0 + x)
+
+#define NSS_COMMON_CLK_DIV0			0xC
+#define NSS_COMMON_CLK_DIV_OFFSET(x)		(x * 8)
+#define NSS_COMMON_CLK_DIV_MASK			0x7f
+
+#define NSS_COMMON_CLK_SRC_CTRL			0x14
+#define NSS_COMMON_CLK_SRC_CTRL_OFFSET(x)	(1 << x)
+/* Mode is coded on 1 bit but is different depending on the MAC ID:
+ * MAC0: QSGMII=0 RGMII=1
+ * MAC1: QSGMII=0 SGMII=0 RGMII=1
+ * MAC2 & MAC3: QSGMII=0 SGMII=1
+ */
+#define NSS_COMMON_CLK_SRC_CTRL_RGMII(x)	1
+#define NSS_COMMON_CLK_SRC_CTRL_SGMII(x)	((x >= 2) ? 1 : 0)
+
+#define NSS_COMMON_MACSEC_CTL			0x28
+#define NSS_COMMON_MACSEC_CTL_EXT_BYPASS_EN(x)	(1 << x)
+
+#define NSS_COMMON_GMAC_CTL(x)			(0x30 + (x * 4))
+#define NSS_COMMON_GMAC_CTL_CSYS_REQ		BIT(19)
+#define NSS_COMMON_GMAC_CTL_PHY_IFACE_SEL	BIT(16)
+#define NSS_COMMON_GMAC_CTL_IFG_LIMIT_OFFSET	8
+#define NSS_COMMON_GMAC_CTL_IFG_OFFSET		0
+#define NSS_COMMON_GMAC_CTL_IFG_MASK		0x3f
+
+#define NSS_COMMON_CLK_DIV_RGMII_1000		1
+#define NSS_COMMON_CLK_DIV_RGMII_100		9
+#define NSS_COMMON_CLK_DIV_RGMII_10		99
+#define NSS_COMMON_CLK_DIV_SGMII_1000		0
+#define NSS_COMMON_CLK_DIV_SGMII_100		4
+#define NSS_COMMON_CLK_DIV_SGMII_10		49
+
+#define QSGMII_PCS_MODE_CTL			0x68
+#define QSGMII_PCS_MODE_CTL_AUTONEG_EN(x)	BIT((x * 8) + 7)
+
+#define QSGMII_PCS_CAL_LCKDT_CTL		0x120
+#define QSGMII_PCS_CAL_LCKDT_CTL_RST		BIT(19)
+
+/* Only GMAC1/2/3 support SGMII and their CTL register are not contiguous */
+#define QSGMII_PHY_SGMII_CTL(x)			((x == 1) ? 0x134 : \
+						 (0x13c + (4 * (x - 2))))
+#define QSGMII_PHY_CDR_EN			BIT(0)
+#define QSGMII_PHY_RX_FRONT_EN			BIT(1)
+#define QSGMII_PHY_RX_SIGNAL_DETECT_EN		BIT(2)
+#define QSGMII_PHY_TX_DRIVER_EN			BIT(3)
+#define QSGMII_PHY_QSGMII_EN			BIT(7)
+#define QSGMII_PHY_PHASE_LOOP_GAIN_OFFSET	12
+#define QSGMII_PHY_PHASE_LOOP_GAIN_MASK		0x7
+#define QSGMII_PHY_RX_DC_BIAS_OFFSET		18
+#define QSGMII_PHY_RX_DC_BIAS_MASK		0x3
+#define QSGMII_PHY_RX_INPUT_EQU_OFFSET		20
+#define QSGMII_PHY_RX_INPUT_EQU_MASK		0x3
+#define QSGMII_PHY_CDR_PI_SLEW_OFFSET		22
+#define QSGMII_PHY_CDR_PI_SLEW_MASK		0x3
+#define QSGMII_PHY_TX_DRV_AMP_OFFSET		28
+#define QSGMII_PHY_TX_DRV_AMP_MASK		0xf
+
+struct ipq806x_gmac {
+	struct platform_device *pdev;
+	struct regmap *nss_common;
+	struct regmap *qsgmii_csr;
+	uint32_t id;
+	struct clk *core_clk;
+	phy_interface_t phy_mode;
+};
+
+static int get_clk_div_sgmii(struct ipq806x_gmac *gmac, unsigned int speed)
+{
+	struct device *dev = &gmac->pdev->dev;
+	int div;
+
+	switch (speed) {
+	case SPEED_1000:
+		div = NSS_COMMON_CLK_DIV_SGMII_1000;
+		break;
+
+	case SPEED_100:
+		div = NSS_COMMON_CLK_DIV_SGMII_100;
+		break;
+
+	case SPEED_10:
+		div = NSS_COMMON_CLK_DIV_SGMII_10;
+		break;
+
+	default:
+		dev_err(dev, "Speed %dMbps not supported in SGMII\n", speed);
+		return -EINVAL;
+	}
+
+	return div;
+}
+
+static int get_clk_div_rgmii(struct ipq806x_gmac *gmac, unsigned int speed)
+{
+	struct device *dev = &gmac->pdev->dev;
+	int div;
+
+	switch (speed) {
+	case SPEED_1000:
+		div = NSS_COMMON_CLK_DIV_RGMII_1000;
+		break;
+
+	case SPEED_100:
+		div = NSS_COMMON_CLK_DIV_RGMII_100;
+		break;
+
+	case SPEED_10:
+		div = NSS_COMMON_CLK_DIV_RGMII_10;
+		break;
+
+	default:
+		dev_err(dev, "Speed %dMbps not supported in RGMII\n", speed);
+		return -EINVAL;
+	}
+
+	return div;
+}
+
+static int ipq806x_gmac_set_speed(struct ipq806x_gmac *gmac, unsigned int speed)
+{
+	uint32_t clk_bits, val;
+	int div;
+
+	switch (gmac->phy_mode) {
+	case PHY_INTERFACE_MODE_RGMII:
+		div = get_clk_div_rgmii(gmac, speed);
+		clk_bits = NSS_COMMON_CLK_GATE_RGMII_RX_EN(gmac->id) |
+			   NSS_COMMON_CLK_GATE_RGMII_TX_EN(gmac->id);
+		break;
+
+	case PHY_INTERFACE_MODE_SGMII:
+		div = get_clk_div_sgmii(gmac, speed);
+		clk_bits = NSS_COMMON_CLK_GATE_GMII_RX_EN(gmac->id) |
+			   NSS_COMMON_CLK_GATE_GMII_TX_EN(gmac->id);
+		break;
+
+	default:
+		dev_err(&gmac->pdev->dev, "Unsupported PHY mode: \"%s\"\n",
+			phy_modes(gmac->phy_mode));
+		return -EINVAL;
+	}
+
+	/* Disable the clocks */
+	regmap_read(gmac->nss_common, NSS_COMMON_CLK_GATE, &val);
+	val &= ~clk_bits;
+	regmap_write(gmac->nss_common, NSS_COMMON_CLK_GATE, val);
+
+	/* Set the divider */
+	regmap_read(gmac->nss_common, NSS_COMMON_CLK_DIV0, &val);
+	val &= ~(NSS_COMMON_CLK_DIV_MASK
+		 << NSS_COMMON_CLK_DIV_OFFSET(gmac->id));
+	val |= div << NSS_COMMON_CLK_DIV_OFFSET(gmac->id);
+	regmap_write(gmac->nss_common, NSS_COMMON_CLK_DIV0, val);
+
+	/* Enable the clock back */
+	regmap_read(gmac->nss_common, NSS_COMMON_CLK_GATE, &val);
+	val |= clk_bits;
+	regmap_write(gmac->nss_common, NSS_COMMON_CLK_GATE, val);
+
+	return 0;
+}
+
+static void *ipq806x_gmac_of_parse(struct ipq806x_gmac *gmac)
+{
+	struct device *dev = &gmac->pdev->dev;
+
+	gmac->phy_mode = of_get_phy_mode(dev->of_node);
+	if (gmac->phy_mode < 0) {
+		dev_err(dev, "missing phy mode property\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (of_property_read_u32(dev->of_node, "qcom,id", &gmac->id) < 0) {
+		dev_err(dev, "missing qcom id property\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	/* The GMACs are called 1 to 4 in the documentation, but to simplify the
+	 * code and keep it consistent with the Linux convention, we'll number
+	 * them from 0 to 3 here.
+	 */
+	if (gmac->id < 0 || gmac->id > 3) {
+		dev_err(dev, "invalid gmac id\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	gmac->core_clk = devm_clk_get(dev, "stmmaceth");
+	if (IS_ERR(gmac->core_clk)) {
+		dev_err(dev, "missing stmmaceth clk property\n");
+		return gmac->core_clk;
+	}
+	clk_set_rate(gmac->core_clk, 266000000);
+
+	/* Setup the register map for the nss common registers */
+	gmac->nss_common = syscon_regmap_lookup_by_phandle(dev->of_node,
+							   "qcom,nss-common");
+	if (IS_ERR(gmac->nss_common)) {
+		dev_err(dev, "missing nss-common node\n");
+		return gmac->nss_common;
+	}
+
+	/* Setup the register map for the qsgmii csr registers */
+	gmac->qsgmii_csr = syscon_regmap_lookup_by_phandle(dev->of_node,
+							   "qcom,qsgmii-csr");
+	if (IS_ERR(gmac->qsgmii_csr)) {
+		dev_err(dev, "missing qsgmii-csr node\n");
+		return gmac->qsgmii_csr;
+	}
+
+	return NULL;
+}
+
+static void *ipq806x_gmac_setup(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct ipq806x_gmac *gmac;
+	int val;
+	void *err;
+
+	gmac = devm_kzalloc(dev, sizeof(*gmac), GFP_KERNEL);
+	if (!gmac)
+		return ERR_PTR(-ENOMEM);
+
+	gmac->pdev = pdev;
+
+	err = ipq806x_gmac_of_parse(gmac);
+	if (err) {
+		dev_err(dev, "device tree parsing error\n");
+		return err;
+	}
+
+	regmap_write(gmac->qsgmii_csr, QSGMII_PCS_CAL_LCKDT_CTL,
+		     QSGMII_PCS_CAL_LCKDT_CTL_RST);
+
+	/* Inter frame gap is set to 12 */
+	val = 12 << NSS_COMMON_GMAC_CTL_IFG_OFFSET |
+	      12 << NSS_COMMON_GMAC_CTL_IFG_LIMIT_OFFSET;
+	/* We also initiate an AXI low power exit request */
+	val |= NSS_COMMON_GMAC_CTL_CSYS_REQ;
+	switch (gmac->phy_mode) {
+	case PHY_INTERFACE_MODE_RGMII:
+		val |= NSS_COMMON_GMAC_CTL_PHY_IFACE_SEL;
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		val &= ~NSS_COMMON_GMAC_CTL_PHY_IFACE_SEL;
+		break;
+	default:
+		dev_err(&pdev->dev, "Unsupported PHY mode: \"%s\"\n",
+			phy_modes(gmac->phy_mode));
+		return NULL;
+	}
+	regmap_write(gmac->nss_common, NSS_COMMON_GMAC_CTL(gmac->id), val);
+
+	/* Configure the clock src according to the mode */
+	regmap_read(gmac->nss_common, NSS_COMMON_CLK_SRC_CTRL, &val);
+	val &= ~NSS_COMMON_CLK_SRC_CTRL_OFFSET(gmac->id);
+	switch (gmac->phy_mode) {
+	case PHY_INTERFACE_MODE_RGMII:
+		val |= NSS_COMMON_CLK_SRC_CTRL_RGMII(gmac->id) <<
+			NSS_COMMON_CLK_SRC_CTRL_OFFSET(gmac->id);
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		val |= NSS_COMMON_CLK_SRC_CTRL_SGMII(gmac->id) <<
+			NSS_COMMON_CLK_SRC_CTRL_OFFSET(gmac->id);
+		break;
+	default:
+		dev_err(&pdev->dev, "Unsupported PHY mode: \"%s\"\n",
+			phy_modes(gmac->phy_mode));
+		return NULL;
+	}
+	regmap_write(gmac->nss_common, NSS_COMMON_CLK_SRC_CTRL, val);
+
+	/* Enable PTP clock */
+	regmap_read(gmac->nss_common, NSS_COMMON_CLK_GATE, &val);
+	val |= NSS_COMMON_CLK_GATE_PTP_EN(gmac->id);
+	regmap_write(gmac->nss_common, NSS_COMMON_CLK_GATE, val);
+
+	if (gmac->phy_mode == PHY_INTERFACE_MODE_SGMII) {
+		regmap_write(gmac->qsgmii_csr, QSGMII_PHY_SGMII_CTL(gmac->id),
+			     QSGMII_PHY_CDR_EN |
+			     QSGMII_PHY_RX_FRONT_EN |
+			     QSGMII_PHY_RX_SIGNAL_DETECT_EN |
+			     QSGMII_PHY_TX_DRIVER_EN |
+			     QSGMII_PHY_QSGMII_EN |
+			     0x4 << QSGMII_PHY_PHASE_LOOP_GAIN_OFFSET |
+			     0x3 << QSGMII_PHY_RX_DC_BIAS_OFFSET |
+			     0x1 << QSGMII_PHY_RX_INPUT_EQU_OFFSET |
+			     0x2 << QSGMII_PHY_CDR_PI_SLEW_OFFSET |
+			     0xC << QSGMII_PHY_TX_DRV_AMP_OFFSET);
+	}
+
+	return gmac;
+}
+
+static void ipq806x_gmac_fix_mac_speed(void *priv, unsigned int speed)
+{
+	struct ipq806x_gmac *gmac = priv;
+
+	ipq806x_gmac_set_speed(gmac, speed);
+}
+
+static const struct stmmac_of_data ipq806x_gmac_data = {
+	.has_gmac	= 1,
+	.setup		= ipq806x_gmac_setup,
+	.fix_mac_speed	= ipq806x_gmac_fix_mac_speed,
+};
+
+static const struct of_device_id ipq806x_gmac_dwmac_match[] = {
+	{ .compatible = "qcom,ipq806x-gmac", .data = &ipq806x_gmac_data },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ipq806x_gmac_dwmac_match);
+
+static struct platform_driver ipq806x_gmac_dwmac_driver = {
+	.probe = stmmac_pltfr_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name		= "ipq806x-gmac-dwmac",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table	= ipq806x_gmac_dwmac_match,
+	},
+};
+module_platform_driver(ipq806x_gmac_dwmac_driver);
+
+MODULE_AUTHOR("Mathieu Olivari <mathieu@codeaurora.org>");
+MODULE_DESCRIPTION("Qualcomm Atheros IPQ806x DWMAC specific glue layer");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-lpc18xx.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-lpc18xx.c
new file mode 100644
index 000000000000..cb888d3ebbdc
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-lpc18xx.c
@@ -0,0 +1,99 @@
+/*
+ * DWMAC glue for NXP LPC18xx/LPC43xx Ethernet
+ *
+ * Copyright (C) 2015 Joachim Eastwood <manabian@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/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_net.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/stmmac.h>
+
+#include "stmmac_platform.h"
+
+/* Register defines for CREG syscon */
+#define LPC18XX_CREG_CREG6			0x12c
+# define LPC18XX_CREG_CREG6_ETHMODE_MASK	0x7
+# define LPC18XX_CREG_CREG6_ETHMODE_MII		0x0
+# define LPC18XX_CREG_CREG6_ETHMODE_RMII	0x4
+
+struct lpc18xx_dwmac_priv_data {
+	struct regmap *reg;
+	int interface;
+};
+
+static void *lpc18xx_dwmac_setup(struct platform_device *pdev)
+{
+	struct lpc18xx_dwmac_priv_data *dwmac;
+
+	dwmac = devm_kzalloc(&pdev->dev, sizeof(*dwmac), GFP_KERNEL);
+	if (!dwmac)
+		return ERR_PTR(-ENOMEM);
+
+	dwmac->interface = of_get_phy_mode(pdev->dev.of_node);
+	if (dwmac->interface < 0)
+		return ERR_PTR(dwmac->interface);
+
+	dwmac->reg = syscon_regmap_lookup_by_compatible("nxp,lpc1850-creg");
+	if (IS_ERR(dwmac->reg)) {
+		dev_err(&pdev->dev, "Syscon lookup failed\n");
+		return dwmac->reg;
+	}
+
+	return dwmac;
+}
+
+static int lpc18xx_dwmac_init(struct platform_device *pdev, void *priv)
+{
+	struct lpc18xx_dwmac_priv_data *dwmac = priv;
+	u8 ethmode;
+
+	if (dwmac->interface == PHY_INTERFACE_MODE_MII) {
+		ethmode = LPC18XX_CREG_CREG6_ETHMODE_MII;
+	} else if (dwmac->interface == PHY_INTERFACE_MODE_RMII) {
+		ethmode = LPC18XX_CREG_CREG6_ETHMODE_RMII;
+	} else {
+		dev_err(&pdev->dev, "Only MII and RMII mode supported\n");
+		return -EINVAL;
+	}
+
+	regmap_update_bits(dwmac->reg, LPC18XX_CREG_CREG6,
+			   LPC18XX_CREG_CREG6_ETHMODE_MASK, ethmode);
+
+	return 0;
+}
+
+static const struct stmmac_of_data lpc18xx_dwmac_data = {
+	.has_gmac = 1,
+	.setup = lpc18xx_dwmac_setup,
+	.init = lpc18xx_dwmac_init,
+};
+
+static const struct of_device_id lpc18xx_dwmac_match[] = {
+	{ .compatible = "nxp,lpc1850-dwmac", .data = &lpc18xx_dwmac_data },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, lpc18xx_dwmac_match);
+
+static struct platform_driver lpc18xx_dwmac_driver = {
+	.probe  = stmmac_pltfr_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name           = "lpc18xx-dwmac",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = lpc18xx_dwmac_match,
+	},
+};
+module_platform_driver(lpc18xx_dwmac_driver);
+
+MODULE_AUTHOR("Joachim Eastwood <manabian@gmail.com>");
+MODULE_DESCRIPTION("DWMAC glue for LPC18xx/43xx Ethernet");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c
index cca028d632f6..61a324a87d09 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c
@@ -15,6 +15,7 @@
 #include <linux/ethtool.h>
 #include <linux/io.h>
 #include <linux/ioport.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/stmmac.h>
 
@@ -63,7 +64,28 @@ static void *meson6_dwmac_setup(struct platform_device *pdev)
 	return dwmac;
 }
 
-const struct stmmac_of_data meson6_dwmac_data = {
+static const struct stmmac_of_data meson6_dwmac_data = {
 	.setup		= meson6_dwmac_setup,
 	.fix_mac_speed	= meson6_dwmac_fix_mac_speed,
 };
+
+static const struct of_device_id meson6_dwmac_match[] = {
+	{ .compatible = "amlogic,meson6-dwmac", .data = &meson6_dwmac_data},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, meson6_dwmac_match);
+
+static struct platform_driver meson6_dwmac_driver = {
+	.probe  = stmmac_pltfr_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name           = "meson6-dwmac",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = meson6_dwmac_match,
+	},
+};
+module_platform_driver(meson6_dwmac_driver);
+
+MODULE_AUTHOR("Beniamino Galvani <b.galvani@gmail.com>");
+MODULE_DESCRIPTION("Amlogic Meson DWMAC glue layer");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c
index 6249a4ec08f0..00a1e1e09d4f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-rk.c
@@ -22,23 +22,36 @@
 #include <linux/phy.h>
 #include <linux/of_net.h>
 #include <linux/gpio.h>
+#include <linux/module.h>
 #include <linux/of_gpio.h>
 #include <linux/of_device.h>
+#include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
 #include <linux/delay.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
 
+#include "stmmac_platform.h"
+
+struct rk_priv_data;
+struct rk_gmac_ops {
+	void (*set_to_rgmii)(struct rk_priv_data *bsp_priv,
+			     int tx_delay, int rx_delay);
+	void (*set_to_rmii)(struct rk_priv_data *bsp_priv);
+	void (*set_rgmii_speed)(struct rk_priv_data *bsp_priv, int speed);
+	void (*set_rmii_speed)(struct rk_priv_data *bsp_priv, int speed);
+};
+
 struct rk_priv_data {
 	struct platform_device *pdev;
 	int phy_iface;
 	struct regulator *regulator;
+	struct rk_gmac_ops *ops;
 
 	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;
@@ -61,103 +74,228 @@ struct rk_priv_data {
 
 #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)
+#define RK3288_GMAC_PHY_INTF_SEL_RGMII	(GRF_BIT(6) | GRF_CLR_BIT(7) | \
+					 GRF_CLR_BIT(8))
+#define RK3288_GMAC_PHY_INTF_SEL_RMII	(GRF_CLR_BIT(6) | GRF_CLR_BIT(7) | \
+					 GRF_BIT(8))
+#define RK3288_GMAC_FLOW_CTRL		GRF_BIT(9)
+#define RK3288_GMAC_FLOW_CTRL_CLR	GRF_CLR_BIT(9)
+#define RK3288_GMAC_SPEED_10M		GRF_CLR_BIT(10)
+#define RK3288_GMAC_SPEED_100M		GRF_BIT(10)
+#define RK3288_GMAC_RMII_CLK_25M	GRF_BIT(11)
+#define RK3288_GMAC_RMII_CLK_2_5M	GRF_CLR_BIT(11)
+#define RK3288_GMAC_CLK_125M		(GRF_CLR_BIT(12) | GRF_CLR_BIT(13))
+#define RK3288_GMAC_CLK_25M		(GRF_BIT(12) | GRF_BIT(13))
+#define RK3288_GMAC_CLK_2_5M		(GRF_CLR_BIT(12) | GRF_BIT(13))
+#define RK3288_GMAC_RMII_MODE		GRF_BIT(14)
+#define RK3288_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)
+#define RK3288_GMAC_TXCLK_DLY_ENABLE	GRF_BIT(14)
+#define RK3288_GMAC_TXCLK_DLY_DISABLE	GRF_CLR_BIT(14)
+#define RK3288_GMAC_RXCLK_DLY_ENABLE	GRF_BIT(15)
+#define RK3288_GMAC_RXCLK_DLY_DISABLE	GRF_CLR_BIT(15)
+#define RK3288_GMAC_CLK_RX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 7)
+#define RK3288_GMAC_CLK_TX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 0)
+
+static void rk3288_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__);
+		dev_err(dev, "Missing rockchip,grf property\n");
 		return;
 	}
 
 	regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
-		     GMAC_PHY_INTF_SEL_RGMII | GMAC_RMII_MODE_CLR);
+		     RK3288_GMAC_PHY_INTF_SEL_RGMII |
+		     RK3288_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));
+		     RK3288_GMAC_RXCLK_DLY_ENABLE |
+		     RK3288_GMAC_TXCLK_DLY_ENABLE |
+		     RK3288_GMAC_CLK_RX_DL_CFG(rx_delay) |
+		     RK3288_GMAC_CLK_TX_DL_CFG(tx_delay));
 }
 
-static void set_to_rmii(struct rk_priv_data *bsp_priv)
+static void rk3288_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__);
+		dev_err(dev, "Missing rockchip,grf property\n");
 		return;
 	}
 
 	regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
-		     GMAC_PHY_INTF_SEL_RMII | GMAC_RMII_MODE);
+		     RK3288_GMAC_PHY_INTF_SEL_RMII | RK3288_GMAC_RMII_MODE);
 }
 
-static void set_rgmii_speed(struct rk_priv_data *bsp_priv, int speed)
+static void rk3288_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__);
+		dev_err(dev, "Missing rockchip,grf property\n");
 		return;
 	}
 
 	if (speed == 10)
-		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1, GMAC_CLK_2_5M);
+		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
+			     RK3288_GMAC_CLK_2_5M);
 	else if (speed == 100)
-		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1, GMAC_CLK_25M);
+		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
+			     RK3288_GMAC_CLK_25M);
 	else if (speed == 1000)
-		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1, GMAC_CLK_125M);
+		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
+			     RK3288_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)
+static void rk3288_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__);
+		dev_err(dev, "Missing rockchip,grf property\n");
 		return;
 	}
 
 	if (speed == 10) {
 		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
-			     GMAC_RMII_CLK_2_5M | GMAC_SPEED_10M);
+			     RK3288_GMAC_RMII_CLK_2_5M |
+			     RK3288_GMAC_SPEED_10M);
 	} else if (speed == 100) {
 		regmap_write(bsp_priv->grf, RK3288_GRF_SOC_CON1,
-			     GMAC_RMII_CLK_25M | GMAC_SPEED_100M);
+			     RK3288_GMAC_RMII_CLK_25M |
+			     RK3288_GMAC_SPEED_100M);
 	} else {
 		dev_err(dev, "unknown speed value for RMII! speed=%d", speed);
 	}
 }
 
+struct rk_gmac_ops rk3288_ops = {
+	.set_to_rgmii = rk3288_set_to_rgmii,
+	.set_to_rmii = rk3288_set_to_rmii,
+	.set_rgmii_speed = rk3288_set_rgmii_speed,
+	.set_rmii_speed = rk3288_set_rmii_speed,
+};
+
+#define RK3368_GRF_SOC_CON15	0x043c
+#define RK3368_GRF_SOC_CON16	0x0440
+
+/* RK3368_GRF_SOC_CON15 */
+#define RK3368_GMAC_PHY_INTF_SEL_RGMII	(GRF_BIT(9) | GRF_CLR_BIT(10) | \
+					 GRF_CLR_BIT(11))
+#define RK3368_GMAC_PHY_INTF_SEL_RMII	(GRF_CLR_BIT(9) | GRF_CLR_BIT(10) | \
+					 GRF_BIT(11))
+#define RK3368_GMAC_FLOW_CTRL		GRF_BIT(8)
+#define RK3368_GMAC_FLOW_CTRL_CLR	GRF_CLR_BIT(8)
+#define RK3368_GMAC_SPEED_10M		GRF_CLR_BIT(7)
+#define RK3368_GMAC_SPEED_100M		GRF_BIT(7)
+#define RK3368_GMAC_RMII_CLK_25M	GRF_BIT(3)
+#define RK3368_GMAC_RMII_CLK_2_5M	GRF_CLR_BIT(3)
+#define RK3368_GMAC_CLK_125M		(GRF_CLR_BIT(4) | GRF_CLR_BIT(5))
+#define RK3368_GMAC_CLK_25M		(GRF_BIT(4) | GRF_BIT(5))
+#define RK3368_GMAC_CLK_2_5M		(GRF_CLR_BIT(4) | GRF_BIT(5))
+#define RK3368_GMAC_RMII_MODE		GRF_BIT(6)
+#define RK3368_GMAC_RMII_MODE_CLR	GRF_CLR_BIT(6)
+
+/* RK3368_GRF_SOC_CON16 */
+#define RK3368_GMAC_TXCLK_DLY_ENABLE	GRF_BIT(7)
+#define RK3368_GMAC_TXCLK_DLY_DISABLE	GRF_CLR_BIT(7)
+#define RK3368_GMAC_RXCLK_DLY_ENABLE	GRF_BIT(15)
+#define RK3368_GMAC_RXCLK_DLY_DISABLE	GRF_CLR_BIT(15)
+#define RK3368_GMAC_CLK_RX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 8)
+#define RK3368_GMAC_CLK_TX_DL_CFG(val)	HIWORD_UPDATE(val, 0x7F, 0)
+
+static void rk3368_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, RK3368_GRF_SOC_CON15,
+		     RK3368_GMAC_PHY_INTF_SEL_RGMII |
+		     RK3368_GMAC_RMII_MODE_CLR);
+	regmap_write(bsp_priv->grf, RK3368_GRF_SOC_CON16,
+		     RK3368_GMAC_RXCLK_DLY_ENABLE |
+		     RK3368_GMAC_TXCLK_DLY_ENABLE |
+		     RK3368_GMAC_CLK_RX_DL_CFG(rx_delay) |
+		     RK3368_GMAC_CLK_TX_DL_CFG(tx_delay));
+}
+
+static void rk3368_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, RK3368_GRF_SOC_CON15,
+		     RK3368_GMAC_PHY_INTF_SEL_RMII | RK3368_GMAC_RMII_MODE);
+}
+
+static void rk3368_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, RK3368_GRF_SOC_CON15,
+			     RK3368_GMAC_CLK_2_5M);
+	else if (speed == 100)
+		regmap_write(bsp_priv->grf, RK3368_GRF_SOC_CON15,
+			     RK3368_GMAC_CLK_25M);
+	else if (speed == 1000)
+		regmap_write(bsp_priv->grf, RK3368_GRF_SOC_CON15,
+			     RK3368_GMAC_CLK_125M);
+	else
+		dev_err(dev, "unknown speed value for RGMII! speed=%d", speed);
+}
+
+static void rk3368_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, RK3368_GRF_SOC_CON15,
+			     RK3368_GMAC_RMII_CLK_2_5M |
+			     RK3368_GMAC_SPEED_10M);
+	} else if (speed == 100) {
+		regmap_write(bsp_priv->grf, RK3368_GRF_SOC_CON15,
+			     RK3368_GMAC_RMII_CLK_25M |
+			     RK3368_GMAC_SPEED_100M);
+	} else {
+		dev_err(dev, "unknown speed value for RMII! speed=%d", speed);
+	}
+}
+
+struct rk_gmac_ops rk3368_ops = {
+	.set_to_rgmii = rk3368_set_to_rgmii,
+	.set_to_rmii = rk3368_set_to_rmii,
+	.set_rgmii_speed = rk3368_set_rgmii_speed,
+	.set_rmii_speed = rk3368_set_rmii_speed,
+};
+
 static int gmac_clk_init(struct rk_priv_data *bsp_priv)
 {
 	struct device *dev = &bsp_priv->pdev->dev;
@@ -166,49 +304,49 @@ static int gmac_clk_init(struct rk_priv_data *bsp_priv)
 
 	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");
+		dev_err(dev, "cannot get clock %s\n",
+			"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");
+		dev_err(dev, "cannot get clock %s\n",
+			"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");
+		dev_err(dev, "cannot get clock %s\n",
+			"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");
+		dev_err(dev, "cannot get clock %s\n",
+			"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");
+		dev_err(dev, "cannot get clock %s\n",
+			"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");
+			dev_err(dev, "cannot get clock %s\n",
+				"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");
+				dev_err(dev, "cannot get clock %s\n",
+					"clk_mac_refout");
 		}
 	}
 
 	if (bsp_priv->clock_input) {
-		dev_info(dev, "%s: clock input from PHY\n", __func__);
+		dev_info(dev, "clock input from PHY\n");
 	} else {
 		if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RMII)
-			clk_set_rate(bsp_priv->clk_mac_pll, 50000000);
+			clk_set_rate(bsp_priv->clk_mac, 50000000);
 	}
 
 	return 0;
@@ -292,26 +430,25 @@ static int phy_power_on(struct rk_priv_data *bsp_priv, bool enable)
 	struct device *dev = &bsp_priv->pdev->dev;
 
 	if (!ldo) {
-		dev_err(dev, "%s: no regulator found\n", __func__);
+		dev_err(dev, "no regulator found\n");
 		return -1;
 	}
 
 	if (enable) {
 		ret = regulator_enable(ldo);
 		if (ret)
-			dev_err(dev, "%s: fail to enable phy-supply\n",
-				__func__);
+			dev_err(dev, "fail to enable phy-supply\n");
 	} else {
 		ret = regulator_disable(ldo);
 		if (ret)
-			dev_err(dev, "%s: fail to disable phy-supply\n",
-				__func__);
+			dev_err(dev, "fail to disable phy-supply\n");
 	}
 
 	return 0;
 }
 
-static void *rk_gmac_setup(struct platform_device *pdev)
+static struct rk_priv_data *rk_gmac_setup(struct platform_device *pdev,
+					  struct rk_gmac_ops *ops)
 {
 	struct rk_priv_data *bsp_priv;
 	struct device *dev = &pdev->dev;
@@ -324,6 +461,7 @@ static void *rk_gmac_setup(struct platform_device *pdev)
 		return ERR_PTR(-ENOMEM);
 
 	bsp_priv->phy_iface = of_get_phy_mode(dev->of_node);
+	bsp_priv->ops = ops;
 
 	bsp_priv->regulator = devm_regulator_get_optional(dev, "phy");
 	if (IS_ERR(bsp_priv->regulator)) {
@@ -337,12 +475,11 @@ static void *rk_gmac_setup(struct platform_device *pdev)
 
 	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__);
+		dev_err(dev, "Can not read property: clock_in_out.\n");
 		bsp_priv->clock_input = true;
 	} else {
-		dev_info(dev, "%s: clock input or output? (%s).\n",
-			 __func__, strings);
+		dev_info(dev, "clock input or output? (%s).\n",
+			 strings);
 		if (!strcmp(strings, "input"))
 			bsp_priv->clock_input = true;
 		else
@@ -352,22 +489,22 @@ static void *rk_gmac_setup(struct platform_device *pdev)
 	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);
+		dev_err(dev, "Can not read property: tx_delay.");
+		dev_err(dev, "set tx_delay to 0x%x\n",
+			bsp_priv->tx_delay);
 	} else {
-		dev_info(dev, "%s: TX delay(0x%x).\n", __func__, value);
+		dev_info(dev, "TX delay(0x%x).\n", 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);
+		dev_err(dev, "Can not read property: rx_delay.");
+		dev_err(dev, "set rx_delay to 0x%x\n",
+			bsp_priv->rx_delay);
 	} else {
-		dev_info(dev, "%s: RX delay(0x%x).\n", __func__, value);
+		dev_info(dev, "RX delay(0x%x).\n", value);
 		bsp_priv->rx_delay = value;
 	}
 
@@ -377,13 +514,14 @@ static void *rk_gmac_setup(struct platform_device *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);
+		dev_info(dev, "init for RGMII\n");
+		bsp_priv->ops->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);
+		dev_info(dev, "init for RMII\n");
+		bsp_priv->ops->set_to_rmii(bsp_priv);
 	} else {
-		dev_err(dev, "%s: NO interface defined!\n", __func__);
+		dev_err(dev, "NO interface defined!\n");
 	}
 
 	gmac_clk_init(bsp_priv);
@@ -391,6 +529,16 @@ static void *rk_gmac_setup(struct platform_device *pdev)
 	return bsp_priv;
 }
 
+static void *rk3288_gmac_setup(struct platform_device *pdev)
+{
+	return rk_gmac_setup(pdev, &rk3288_ops);
+}
+
+static void *rk3368_gmac_setup(struct platform_device *pdev)
+{
+	return rk_gmac_setup(pdev, &rk3368_ops);
+}
+
 static int rk_gmac_init(struct platform_device *pdev, void *priv)
 {
 	struct rk_priv_data *bsp_priv = priv;
@@ -421,17 +569,47 @@ static void rk_fix_speed(void *priv, unsigned int speed)
 	struct device *dev = &bsp_priv->pdev->dev;
 
 	if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RGMII)
-		set_rgmii_speed(bsp_priv, speed);
+		bsp_priv->ops->set_rgmii_speed(bsp_priv, speed);
 	else if (bsp_priv->phy_iface == PHY_INTERFACE_MODE_RMII)
-		set_rmii_speed(bsp_priv, speed);
+		bsp_priv->ops->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 = {
+static const struct stmmac_of_data rk3288_gmac_data = {
 	.has_gmac = 1,
 	.fix_mac_speed = rk_fix_speed,
-	.setup = rk_gmac_setup,
+	.setup = rk3288_gmac_setup,
 	.init = rk_gmac_init,
 	.exit = rk_gmac_exit,
 };
+
+static const struct stmmac_of_data rk3368_gmac_data = {
+	.has_gmac = 1,
+	.fix_mac_speed = rk_fix_speed,
+	.setup = rk3368_gmac_setup,
+	.init = rk_gmac_init,
+	.exit = rk_gmac_exit,
+};
+
+static const struct of_device_id rk_gmac_dwmac_match[] = {
+	{ .compatible = "rockchip,rk3288-gmac", .data = &rk3288_gmac_data},
+	{ .compatible = "rockchip,rk3368-gmac", .data = &rk3368_gmac_data},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rk_gmac_dwmac_match);
+
+static struct platform_driver rk_gmac_dwmac_driver = {
+	.probe  = stmmac_pltfr_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name           = "rk_gmac-dwmac",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = rk_gmac_dwmac_match,
+	},
+};
+module_platform_driver(rk_gmac_dwmac_driver);
+
+MODULE_AUTHOR("Chen-Zhi (Roger Chen) <roger.chen@rock-chips.com>");
+MODULE_DESCRIPTION("Rockchip RK3288 DWMAC specific glue layer");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
index 5a36bd2c7837..8141c5b844ae 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
@@ -257,9 +257,28 @@ static int socfpga_dwmac_init(struct platform_device *pdev, void *priv)
 	return ret;
 }
 
-const struct stmmac_of_data socfpga_gmac_data = {
+static const struct stmmac_of_data socfpga_gmac_data = {
 	.setup = socfpga_dwmac_probe,
 	.init = socfpga_dwmac_init,
 	.exit = socfpga_dwmac_exit,
 	.fix_mac_speed = socfpga_dwmac_fix_mac_speed,
 };
+
+static const struct of_device_id socfpga_dwmac_match[] = {
+	{ .compatible = "altr,socfpga-stmmac", .data = &socfpga_gmac_data },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, socfpga_dwmac_match);
+
+static struct platform_driver socfpga_dwmac_driver = {
+	.probe  = stmmac_pltfr_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name           = "socfpga-dwmac",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = socfpga_dwmac_match,
+	},
+};
+module_platform_driver(socfpga_dwmac_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
index bb6e2dc61bec..a2e8111c5d14 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
@@ -17,6 +17,7 @@
 #include <linux/stmmac.h>
 #include <linux/phy.h>
 #include <linux/mfd/syscon.h>
+#include <linux/module.h>
 #include <linux/regmap.h>
 #include <linux/clk.h>
 #include <linux/of.h>
@@ -351,16 +352,40 @@ static void *sti_dwmac_setup(struct platform_device *pdev)
 	return dwmac;
 }
 
-const struct stmmac_of_data stih4xx_dwmac_data = {
+static const struct stmmac_of_data stih4xx_dwmac_data = {
 	.fix_mac_speed = stih4xx_fix_retime_src,
 	.setup = sti_dwmac_setup,
 	.init = stix4xx_init,
 	.exit = sti_dwmac_exit,
 };
 
-const struct stmmac_of_data stid127_dwmac_data = {
+static const struct stmmac_of_data stid127_dwmac_data = {
 	.fix_mac_speed = stid127_fix_retime_src,
 	.setup = sti_dwmac_setup,
 	.init = stid127_init,
 	.exit = sti_dwmac_exit,
 };
+
+static const struct of_device_id sti_dwmac_match[] = {
+	{ .compatible = "st,stih415-dwmac", .data = &stih4xx_dwmac_data},
+	{ .compatible = "st,stih416-dwmac", .data = &stih4xx_dwmac_data},
+	{ .compatible = "st,stid127-dwmac", .data = &stid127_dwmac_data},
+	{ .compatible = "st,stih407-dwmac", .data = &stih4xx_dwmac_data},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sti_dwmac_match);
+
+static struct platform_driver sti_dwmac_driver = {
+	.probe  = stmmac_pltfr_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name           = "sti-dwmac",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = sti_dwmac_match,
+	},
+};
+module_platform_driver(sti_dwmac_driver);
+
+MODULE_AUTHOR("Srinivas Kandagatla <srinivas.kandagatla@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics DWMAC Specific Glue layer");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-sunxi.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-sunxi.c
index c5ea9ab75b03..15048ca39759 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-sunxi.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-sunxi.c
@@ -18,7 +18,9 @@
 
 #include <linux/stmmac.h>
 #include <linux/clk.h>
+#include <linux/module.h>
 #include <linux/phy.h>
+#include <linux/platform_device.h>
 #include <linux/of_net.h>
 #include <linux/regulator/consumer.h>
 
@@ -132,7 +134,7 @@ static void sun7i_fix_speed(void *priv, unsigned int speed)
 
 /* of_data specifying hardware features and callbacks.
  * hardware features were copied from Allwinner drivers. */
-const struct stmmac_of_data sun7i_gmac_data = {
+static const struct stmmac_of_data sun7i_gmac_data = {
 	.has_gmac = 1,
 	.tx_coe = 1,
 	.fix_mac_speed = sun7i_fix_speed,
@@ -140,3 +142,24 @@ const struct stmmac_of_data sun7i_gmac_data = {
 	.init = sun7i_gmac_init,
 	.exit = sun7i_gmac_exit,
 };
+
+static const struct of_device_id sun7i_dwmac_match[] = {
+	{ .compatible = "allwinner,sun7i-a20-gmac", .data = &sun7i_gmac_data},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sun7i_dwmac_match);
+
+static struct platform_driver sun7i_dwmac_driver = {
+	.probe  = stmmac_pltfr_probe,
+	.remove = stmmac_pltfr_remove,
+	.driver = {
+		.name           = "sun7i-dwmac",
+		.pm		= &stmmac_pltfr_pm_ops,
+		.of_match_table = sun7i_dwmac_match,
+	},
+};
+module_platform_driver(sun7i_dwmac_driver);
+
+MODULE_AUTHOR("Chen-Yu Tsai <wens@csie.org>");
+MODULE_DESCRIPTION("Allwinner sunxi DWMAC specific glue layer");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
index 1e2bcf5f89e1..7d944449f5ef 100644
--- a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
@@ -240,6 +240,7 @@ static int enh_desc_get_rx_status(void *data, struct stmmac_extra_stats *x,
 static void enh_desc_init_rx_desc(struct dma_desc *p, int disable_rx_ic,
 				  int mode, int end)
 {
+	p->des01.all_flags = 0;
 	p->des01.erx.own = 1;
 	p->des01.erx.buffer1_size = BUF_SIZE_8KiB - 1;
 
@@ -254,7 +255,7 @@ static void enh_desc_init_rx_desc(struct dma_desc *p, int disable_rx_ic,
 
 static void enh_desc_init_tx_desc(struct dma_desc *p, int mode, int end)
 {
-	p->des01.etx.own = 0;
+	p->des01.all_flags = 0;
 	if (mode == STMMAC_CHAIN_MODE)
 		ehn_desc_tx_set_on_chain(p, end);
 	else
diff --git a/drivers/net/ethernet/stmicro/stmmac/mmc_core.c b/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
index 08c483bd2ec7..3f20bb1fe570 100644
--- a/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
@@ -73,7 +73,7 @@
 #define MMC_RX_OCTETCOUNT_G		0x00000188
 #define MMC_RX_BROADCASTFRAME_G		0x0000018c
 #define MMC_RX_MULTICASTFRAME_G		0x00000190
-#define MMC_RX_CRC_ERRROR		0x00000194
+#define MMC_RX_CRC_ERROR		0x00000194
 #define MMC_RX_ALIGN_ERROR		0x00000198
 #define MMC_RX_RUN_ERROR		0x0000019C
 #define MMC_RX_JABBER_ERROR		0x000001A0
@@ -196,7 +196,7 @@ void dwmac_mmc_read(void __iomem *ioaddr, struct stmmac_counters *mmc)
 	mmc->mmc_rx_octetcount_g += readl(ioaddr + MMC_RX_OCTETCOUNT_G);
 	mmc->mmc_rx_broadcastframe_g += readl(ioaddr + MMC_RX_BROADCASTFRAME_G);
 	mmc->mmc_rx_multicastframe_g += readl(ioaddr + MMC_RX_MULTICASTFRAME_G);
-	mmc->mmc_rx_crc_error += readl(ioaddr + MMC_RX_CRC_ERRROR);
+	mmc->mmc_rx_crc_error += readl(ioaddr + MMC_RX_CRC_ERROR);
 	mmc->mmc_rx_align_error += readl(ioaddr + MMC_RX_ALIGN_ERROR);
 	mmc->mmc_rx_run_error += readl(ioaddr + MMC_RX_RUN_ERROR);
 	mmc->mmc_rx_jabber_error += readl(ioaddr + MMC_RX_JABBER_ERROR);
diff --git a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
index 35ad4f427ae2..48c3456445b2 100644
--- a/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/norm_desc.c
@@ -123,6 +123,7 @@ static int ndesc_get_rx_status(void *data, struct stmmac_extra_stats *x,
 static void ndesc_init_rx_desc(struct dma_desc *p, int disable_rx_ic, int mode,
 			       int end)
 {
+	p->des01.all_flags = 0;
 	p->des01.rx.own = 1;
 	p->des01.rx.buffer1_size = BUF_SIZE_2KiB - 1;
 
@@ -137,7 +138,7 @@ static void ndesc_init_rx_desc(struct dma_desc *p, int disable_rx_ic, int mode,
 
 static void ndesc_init_tx_desc(struct dma_desc *p, int mode, int end)
 {
-	p->des01.tx.own = 0;
+	p->des01.all_flags = 0;
 	if (mode == STMMAC_CHAIN_MODE)
 		ndesc_tx_set_on_chain(p, end);
 	else
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index 73bab983edd9..1f3b33a6c6a8 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -34,6 +34,14 @@
 #include <linux/ptp_clock_kernel.h>
 #include <linux/reset.h>
 
+struct stmmac_resources {
+	void __iomem *addr;
+	const char *mac;
+	int wol_irq;
+	int lpi_irq;
+	int irq;
+};
+
 struct stmmac_tx_info {
 	dma_addr_t buf;
 	bool map_as_page;
@@ -135,9 +143,9 @@ void stmmac_ptp_unregister(struct stmmac_priv *priv);
 int stmmac_resume(struct net_device *ndev);
 int stmmac_suspend(struct net_device *ndev);
 int stmmac_dvr_remove(struct net_device *ndev);
-struct stmmac_priv *stmmac_dvr_probe(struct device *device,
-				     struct plat_stmmacenet_data *plat_dat,
-				     void __iomem *addr);
+int stmmac_dvr_probe(struct device *device,
+		     struct plat_stmmacenet_data *plat_dat,
+		     struct stmmac_resources *res);
 void stmmac_disable_eee_mode(struct stmmac_priv *priv);
 bool stmmac_eee_init(struct stmmac_priv *priv);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 2c5ce2baca87..50f7a7a26821 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -52,6 +52,7 @@
 #include "stmmac_ptp.h"
 #include "stmmac.h"
 #include <linux/reset.h>
+#include <linux/of_mdio.h>
 
 #define STMMAC_ALIGN(x)	L1_CACHE_ALIGN(x)
 
@@ -816,18 +817,25 @@ static int stmmac_init_phy(struct net_device *dev)
 	priv->speed = 0;
 	priv->oldduplex = -1;
 
-	if (priv->plat->phy_bus_name)
-		snprintf(bus_id, MII_BUS_ID_SIZE, "%s-%x",
-			 priv->plat->phy_bus_name, priv->plat->bus_id);
-	else
-		snprintf(bus_id, MII_BUS_ID_SIZE, "stmmac-%x",
-			 priv->plat->bus_id);
+	if (priv->plat->phy_node) {
+		phydev = of_phy_connect(dev, priv->plat->phy_node,
+					&stmmac_adjust_link, 0, interface);
+	} else {
+		if (priv->plat->phy_bus_name)
+			snprintf(bus_id, MII_BUS_ID_SIZE, "%s-%x",
+				 priv->plat->phy_bus_name, priv->plat->bus_id);
+		else
+			snprintf(bus_id, MII_BUS_ID_SIZE, "stmmac-%x",
+				 priv->plat->bus_id);
 
-	snprintf(phy_id_fmt, MII_BUS_ID_SIZE + 3, PHY_ID_FMT, bus_id,
-		 priv->plat->phy_addr);
-	pr_debug("stmmac_init_phy:  trying to attach to %s\n", phy_id_fmt);
+		snprintf(phy_id_fmt, MII_BUS_ID_SIZE + 3, PHY_ID_FMT, bus_id,
+			 priv->plat->phy_addr);
+		pr_debug("stmmac_init_phy:  trying to attach to %s\n",
+			 phy_id_fmt);
 
-	phydev = phy_connect(dev, phy_id_fmt, &stmmac_adjust_link, interface);
+		phydev = phy_connect(dev, phy_id_fmt, &stmmac_adjust_link,
+				     interface);
+	}
 
 	if (IS_ERR(phydev)) {
 		pr_err("%s: Could not attach to PHY\n", dev->name);
@@ -848,7 +856,7 @@ static int stmmac_init_phy(struct net_device *dev)
 	 * device as well.
 	 * Note: phydev->phy_id is the result of reading the UID PHY registers.
 	 */
-	if (phydev->phy_id == 0) {
+	if (!priv->plat->phy_node && phydev->phy_id == 0) {
 		phy_disconnect(phydev);
 		return -ENODEV;
 	}
@@ -975,13 +983,11 @@ static int stmmac_init_rx_buffers(struct stmmac_priv *priv, struct dma_desc *p,
 {
 	struct sk_buff *skb;
 
-	skb = __netdev_alloc_skb(priv->dev, priv->dma_buf_sz + NET_IP_ALIGN,
-				 flags);
+	skb = __netdev_alloc_skb_ip_align(priv->dev, priv->dma_buf_sz, flags);
 	if (!skb) {
 		pr_err("%s: Rx init fails; skb is NULL\n", __func__);
 		return -ENOMEM;
 	}
-	skb_reserve(skb, NET_IP_ALIGN);
 	priv->rx_skbuff[i] = skb;
 	priv->rx_skbuff_dma[i] = dma_map_single(priv->device, skb->data,
 						priv->dma_buf_sz,
@@ -1189,41 +1195,41 @@ static int alloc_dma_desc_resources(struct stmmac_priv *priv)
 		goto err_tx_skbuff;
 
 	if (priv->extend_desc) {
-		priv->dma_erx = dma_alloc_coherent(priv->device, rxsize *
-						   sizeof(struct
-							  dma_extended_desc),
-						   &priv->dma_rx_phy,
-						   GFP_KERNEL);
+		priv->dma_erx = dma_zalloc_coherent(priv->device, rxsize *
+						    sizeof(struct
+							   dma_extended_desc),
+						    &priv->dma_rx_phy,
+						    GFP_KERNEL);
 		if (!priv->dma_erx)
 			goto err_dma;
 
-		priv->dma_etx = dma_alloc_coherent(priv->device, txsize *
-						   sizeof(struct
-							  dma_extended_desc),
-						   &priv->dma_tx_phy,
-						   GFP_KERNEL);
+		priv->dma_etx = dma_zalloc_coherent(priv->device, txsize *
+						    sizeof(struct
+							   dma_extended_desc),
+						    &priv->dma_tx_phy,
+						    GFP_KERNEL);
 		if (!priv->dma_etx) {
 			dma_free_coherent(priv->device, priv->dma_rx_size *
-					sizeof(struct dma_extended_desc),
-					priv->dma_erx, priv->dma_rx_phy);
+					  sizeof(struct dma_extended_desc),
+					  priv->dma_erx, priv->dma_rx_phy);
 			goto err_dma;
 		}
 	} else {
-		priv->dma_rx = dma_alloc_coherent(priv->device, rxsize *
-						  sizeof(struct dma_desc),
-						  &priv->dma_rx_phy,
-						  GFP_KERNEL);
+		priv->dma_rx = dma_zalloc_coherent(priv->device, rxsize *
+						   sizeof(struct dma_desc),
+						   &priv->dma_rx_phy,
+						   GFP_KERNEL);
 		if (!priv->dma_rx)
 			goto err_dma;
 
-		priv->dma_tx = dma_alloc_coherent(priv->device, txsize *
-						  sizeof(struct dma_desc),
-						  &priv->dma_tx_phy,
-						  GFP_KERNEL);
+		priv->dma_tx = dma_zalloc_coherent(priv->device, txsize *
+						   sizeof(struct dma_desc),
+						   &priv->dma_tx_phy,
+						   GFP_KERNEL);
 		if (!priv->dma_tx) {
 			dma_free_coherent(priv->device, priv->dma_rx_size *
-					sizeof(struct dma_desc),
-					priv->dma_rx, priv->dma_rx_phy);
+					  sizeof(struct dma_desc),
+					  priv->dma_rx, priv->dma_rx_phy);
 			goto err_dma;
 		}
 	}
@@ -2800,16 +2806,15 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
  * stmmac_dvr_probe
  * @device: device pointer
  * @plat_dat: platform data pointer
- * @addr: iobase memory address
+ * @res: stmmac resource pointer
  * Description: this is the main probe function used to
  * call the alloc_etherdev, allocate the priv structure.
  * Return:
- * on success the new private structure is returned, otherwise the error
- * pointer.
+ * returns 0 on success, otherwise errno.
  */
-struct stmmac_priv *stmmac_dvr_probe(struct device *device,
-				     struct plat_stmmacenet_data *plat_dat,
-				     void __iomem *addr)
+int stmmac_dvr_probe(struct device *device,
+		     struct plat_stmmacenet_data *plat_dat,
+		     struct stmmac_resources *res)
 {
 	int ret = 0;
 	struct net_device *ndev = NULL;
@@ -2817,7 +2822,7 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 
 	ndev = alloc_etherdev(sizeof(struct stmmac_priv));
 	if (!ndev)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
 	SET_NETDEV_DEV(ndev, device);
 
@@ -2828,8 +2833,17 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	stmmac_set_ethtool_ops(ndev);
 	priv->pause = pause;
 	priv->plat = plat_dat;
-	priv->ioaddr = addr;
-	priv->dev->base_addr = (unsigned long)addr;
+	priv->ioaddr = res->addr;
+	priv->dev->base_addr = (unsigned long)res->addr;
+
+	priv->dev->irq = res->irq;
+	priv->wol_irq = res->wol_irq;
+	priv->lpi_irq = res->lpi_irq;
+
+	if (res->mac)
+		memcpy(priv->dev->dev_addr, res->mac, ETH_ALEN);
+
+	dev_set_drvdata(device, priv);
 
 	/* Verify driver arguments */
 	stmmac_verify_args();
@@ -2944,7 +2958,7 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 		}
 	}
 
-	return priv;
+	return 0;
 
 error_mdio_register:
 	unregister_netdev(ndev);
@@ -2957,7 +2971,7 @@ error_pclk_get:
 error_clk_get:
 	free_netdev(ndev);
 
-	return ERR_PTR(ret);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(stmmac_dvr_probe);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
index 3bca908716e2..d71a721ea61c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_pci.c
@@ -163,7 +163,7 @@ static int stmmac_pci_probe(struct pci_dev *pdev,
 {
 	struct stmmac_pci_info *info = (struct stmmac_pci_info *)id->driver_data;
 	struct plat_stmmacenet_data *plat;
-	struct stmmac_priv *priv;
+	struct stmmac_resources res;
 	int i;
 	int ret;
 
@@ -214,19 +214,12 @@ static int stmmac_pci_probe(struct pci_dev *pdev,
 
 	pci_enable_msi(pdev);
 
-	priv = stmmac_dvr_probe(&pdev->dev, plat, pcim_iomap_table(pdev)[i]);
-	if (IS_ERR(priv)) {
-		dev_err(&pdev->dev, "%s: main driver probe failed\n", __func__);
-		return PTR_ERR(priv);
-	}
-	priv->dev->irq = pdev->irq;
-	priv->wol_irq = pdev->irq;
-
-	pci_set_drvdata(pdev, priv->dev);
+	memset(&res, 0, sizeof(res));
+	res.addr = pcim_iomap_table(pdev)[i];
+	res.wol_irq = pdev->irq;
+	res.irq = pdev->irq;
 
-	dev_dbg(&pdev->dev, "STMMAC PCI driver registration completed\n");
-
-	return 0;
+	return stmmac_dvr_probe(&pdev->dev, plat, &res);
 }
 
 /**
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index 68aec5c460db..f3918c7e7eeb 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -28,29 +28,11 @@
 #include <linux/of.h>
 #include <linux/of_net.h>
 #include <linux/of_device.h>
+#include <linux/of_mdio.h>
 
 #include "stmmac.h"
 #include "stmmac_platform.h"
 
-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},
-	{ .compatible = "st,stih416-dwmac", .data = &stih4xx_dwmac_data},
-	{ .compatible = "st,stid127-dwmac", .data = &stid127_dwmac_data},
-	{ .compatible = "st,stih407-dwmac", .data = &stih4xx_dwmac_data},
-	{ .compatible = "altr,socfpga-stmmac", .data = &socfpga_gmac_data },
-	{ .compatible = "st,spear600-gmac"},
-	{ .compatible = "snps,dwmac-3.610"},
-	{ .compatible = "snps,dwmac-3.70a"},
-	{ .compatible = "snps,dwmac-3.710"},
-	{ .compatible = "snps,dwmac"},
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, stmmac_dt_ids);
-
 #ifdef CONFIG_OF
 
 /**
@@ -129,14 +111,9 @@ static int stmmac_probe_config_dt(struct platform_device *pdev,
 	struct device_node *np = pdev->dev.of_node;
 	struct stmmac_dma_cfg *dma_cfg;
 	const struct of_device_id *device;
+	struct device *dev = &pdev->dev;
 
-	if (!np)
-		return -ENODEV;
-
-	device = of_match_device(stmmac_dt_ids, &pdev->dev);
-	if (!device)
-		return -ENODEV;
-
+	device = of_match_device(dev->driver->of_match_table, dev);
 	if (device->data) {
 		const struct stmmac_of_data *data = device->data;
 		plat->has_gmac = data->has_gmac;
@@ -168,13 +145,24 @@ static int stmmac_probe_config_dt(struct platform_device *pdev,
 	/* Default to phy auto-detection */
 	plat->phy_addr = -1;
 
+	/* If we find a phy-handle property, use it as the PHY */
+	plat->phy_node = of_parse_phandle(np, "phy-handle", 0);
+
+	/* If phy-handle is not specified, check if we have a fixed-phy */
+	if (!plat->phy_node && of_phy_is_fixed_link(np)) {
+		if ((of_phy_register_fixed_link(np) < 0))
+			return -ENODEV;
+
+		plat->phy_node = of_node_get(np);
+	}
+
 	/* "snps,phy-addr" is not a standard property. Mark it as deprecated
 	 * and warn of its use. Remove this when phy node support is added.
 	 */
 	if (of_property_read_u32(np, "snps,phy-addr", &plat->phy_addr) == 0)
 		dev_warn(&pdev->dev, "snps,phy-addr property is deprecated\n");
 
-	if (plat->phy_bus_name)
+	if (plat->phy_node || plat->phy_bus_name)
 		plat->mdio_bus_data = NULL;
 	else
 		plat->mdio_bus_data =
@@ -232,8 +220,10 @@ static int stmmac_probe_config_dt(struct platform_device *pdev,
 	if (of_find_property(np, "snps,pbl", NULL)) {
 		dma_cfg = devm_kzalloc(&pdev->dev, sizeof(*dma_cfg),
 				       GFP_KERNEL);
-		if (!dma_cfg)
+		if (!dma_cfg) {
+			of_node_put(np);
 			return -ENOMEM;
+		}
 		plat->dma_cfg = dma_cfg;
 		of_property_read_u32(np, "snps,pbl", &dma_cfg->pbl);
 		dma_cfg->fixed_burst =
@@ -268,27 +258,26 @@ static int stmmac_probe_config_dt(struct platform_device *pdev,
  * the necessary platform resources, invoke custom helper (if required) and
  * invoke the main probe function.
  */
-static int stmmac_pltfr_probe(struct platform_device *pdev)
+int stmmac_pltfr_probe(struct platform_device *pdev)
 {
+	struct stmmac_resources stmmac_res;
 	int ret = 0;
 	struct resource *res;
 	struct device *dev = &pdev->dev;
-	void __iomem *addr = NULL;
-	struct stmmac_priv *priv = NULL;
 	struct plat_stmmacenet_data *plat_dat = NULL;
-	const char *mac = NULL;
-	int irq, wol_irq, lpi_irq;
+
+	memset(&stmmac_res, 0, sizeof(stmmac_res));
 
 	/* Get IRQ information early to have an ability to ask for deferred
 	 * probe if needed before we went too far with resource allocation.
 	 */
-	irq = platform_get_irq_byname(pdev, "macirq");
-	if (irq < 0) {
-		if (irq != -EPROBE_DEFER) {
+	stmmac_res.irq = platform_get_irq_byname(pdev, "macirq");
+	if (stmmac_res.irq < 0) {
+		if (stmmac_res.irq != -EPROBE_DEFER) {
 			dev_err(dev,
 				"MAC IRQ configuration information not found\n");
 		}
-		return irq;
+		return stmmac_res.irq;
 	}
 
 	/* On some platforms e.g. SPEAr the wake up irq differs from the mac irq
@@ -298,21 +287,21 @@ static int stmmac_pltfr_probe(struct platform_device *pdev)
 	 * In case the wake up interrupt is not passed from the platform
 	 * so the driver will continue to use the mac irq (ndev->irq)
 	 */
-	wol_irq = platform_get_irq_byname(pdev, "eth_wake_irq");
-	if (wol_irq < 0) {
-		if (wol_irq == -EPROBE_DEFER)
+	stmmac_res.wol_irq = platform_get_irq_byname(pdev, "eth_wake_irq");
+	if (stmmac_res.wol_irq < 0) {
+		if (stmmac_res.wol_irq == -EPROBE_DEFER)
 			return -EPROBE_DEFER;
-		wol_irq = irq;
+		stmmac_res.wol_irq = stmmac_res.irq;
 	}
 
-	lpi_irq = platform_get_irq_byname(pdev, "eth_lpi");
-	if (lpi_irq == -EPROBE_DEFER)
+	stmmac_res.lpi_irq = platform_get_irq_byname(pdev, "eth_lpi");
+	if (stmmac_res.lpi_irq == -EPROBE_DEFER)
 		return -EPROBE_DEFER;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	addr = devm_ioremap_resource(dev, res);
-	if (IS_ERR(addr))
-		return PTR_ERR(addr);
+	stmmac_res.addr = devm_ioremap_resource(dev, res);
+	if (IS_ERR(stmmac_res.addr))
+		return PTR_ERR(stmmac_res.addr);
 
 	plat_dat = dev_get_platdata(&pdev->dev);
 
@@ -332,7 +321,7 @@ static int stmmac_pltfr_probe(struct platform_device *pdev)
 	plat_dat->unicast_filter_entries = 1;
 
 	if (pdev->dev.of_node) {
-		ret = stmmac_probe_config_dt(pdev, plat_dat, &mac);
+		ret = stmmac_probe_config_dt(pdev, plat_dat, &stmmac_res.mac);
 		if (ret) {
 			pr_err("%s: main dt probe failed", __func__);
 			return ret;
@@ -353,27 +342,9 @@ static int stmmac_pltfr_probe(struct platform_device *pdev)
 			return ret;
 	}
 
-	priv = stmmac_dvr_probe(&(pdev->dev), plat_dat, addr);
-	if (IS_ERR(priv)) {
-		pr_err("%s: main driver probe failed", __func__);
-		return PTR_ERR(priv);
-	}
-
-	/* Copy IRQ values to priv structure which is now avaialble */
-	priv->dev->irq = irq;
-	priv->wol_irq = wol_irq;
-	priv->lpi_irq = lpi_irq;
-
-	/* Get MAC address if available (DT) */
-	if (mac)
-		memcpy(priv->dev->dev_addr, mac, ETH_ALEN);
-
-	platform_set_drvdata(pdev, priv->dev);
-
-	pr_debug("STMMAC platform driver registration completed");
-
-	return 0;
+	return stmmac_dvr_probe(&pdev->dev, plat_dat, &stmmac_res);
 }
+EXPORT_SYMBOL_GPL(stmmac_pltfr_probe);
 
 /**
  * stmmac_pltfr_remove
@@ -381,7 +352,7 @@ static int stmmac_pltfr_probe(struct platform_device *pdev)
  * Description: this function calls the main to free the net resources
  * and calls the platforms hook and release the resources (e.g. mem).
  */
-static int stmmac_pltfr_remove(struct platform_device *pdev)
+int stmmac_pltfr_remove(struct platform_device *pdev)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct stmmac_priv *priv = netdev_priv(ndev);
@@ -395,6 +366,7 @@ static int stmmac_pltfr_remove(struct platform_device *pdev)
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(stmmac_pltfr_remove);
 
 #ifdef CONFIG_PM_SLEEP
 /**
@@ -438,21 +410,6 @@ static int stmmac_pltfr_resume(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
-static SIMPLE_DEV_PM_OPS(stmmac_pltfr_pm_ops,
-			 stmmac_pltfr_suspend, stmmac_pltfr_resume);
-
-static struct platform_driver stmmac_pltfr_driver = {
-	.probe = stmmac_pltfr_probe,
-	.remove = stmmac_pltfr_remove,
-	.driver = {
-		   .name = STMMAC_RESOURCE_NAME,
-		   .pm = &stmmac_pltfr_pm_ops,
-		   .of_match_table = of_match_ptr(stmmac_dt_ids),
-	},
-};
-
-module_platform_driver(stmmac_pltfr_driver);
-
-MODULE_DESCRIPTION("STMMAC 10/100/1000 Ethernet PLATFORM driver");
-MODULE_AUTHOR("Giuseppe Cavallaro <peppe.cavallaro@st.com>");
-MODULE_LICENSE("GPL");
+SIMPLE_DEV_PM_OPS(stmmac_pltfr_pm_ops, stmmac_pltfr_suspend,
+				       stmmac_pltfr_resume);
+EXPORT_SYMBOL_GPL(stmmac_pltfr_pm_ops);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
index 093eb99e5ffd..71da86d7bd00 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.h
@@ -19,11 +19,8 @@
 #ifndef __STMMAC_PLATFORM_H__
 #define __STMMAC_PLATFORM_H__
 
-extern const struct stmmac_of_data meson6_dwmac_data;
-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;
+int stmmac_pltfr_probe(struct platform_device *pdev);
+int stmmac_pltfr_remove(struct platform_device *pdev);
+extern const struct dev_pm_ops stmmac_pltfr_pm_ops;
 
 #endif /* __STMMAC_PLATFORM_H__ */
diff --git a/drivers/net/ethernet/sun/Kconfig b/drivers/net/ethernet/sun/Kconfig
index 3074aa374c6b..dee94b67638c 100644
--- a/drivers/net/ethernet/sun/Kconfig
+++ b/drivers/net/ethernet/sun/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_SUN
 	default y
 	depends on SUN3 || SBUS || PCI || SUN_LDOMS
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say
-	  Y and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/sun/cassini.c b/drivers/net/ethernet/sun/cassini.c
index 3dc1f68b322d..6ce973187225 100644
--- a/drivers/net/ethernet/sun/cassini.c
+++ b/drivers/net/ethernet/sun/cassini.c
@@ -3058,7 +3058,6 @@ static void cas_init_mac(struct cas *cp)
 	/* setup core arbitration weight register */
 	writel(CAWR_RR_DIS, cp->regs + REG_CAWR);
 
-	/* XXX Use pci_dma_burst_advice() */
 #if !defined(CONFIG_SPARC64) && !defined(CONFIG_ALPHA)
 	/* set the infinite burst register for chips that don't have
 	 * pci issues.
diff --git a/drivers/net/ethernet/tehuti/Kconfig b/drivers/net/ethernet/tehuti/Kconfig
index 1fc027eda33e..b17f0ca3f395 100644
--- a/drivers/net/ethernet/tehuti/Kconfig
+++ b/drivers/net/ethernet/tehuti/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_TEHUTI
 	default y
 	depends on PCI
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/ti/Kconfig b/drivers/net/ethernet/ti/Kconfig
index 631e0afd07d2..e7f0b7d95b65 100644
--- a/drivers/net/ethernet/ti/Kconfig
+++ b/drivers/net/ethernet/ti/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_TI
 	default y
 	depends on PCI || EISA || AR7 || ARCH_DAVINCI || ARCH_OMAP2PLUS || ARCH_KEYSTONE
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -110,9 +108,7 @@ config TLAN
 	depends on (PCI || EISA)
 	---help---
 	  If you have a PCI Ethernet network card based on the ThunderLAN chip
-	  which is supported by this driver, say Y and read the
-	  Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  which is supported by this driver, say Y here.
 
 	  Devices currently supported by this driver are Compaq Netelligent,
 	  Compaq NetFlex and Olicom cards.  Please read the file
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index b536b4c82752..462820514fae 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1361,7 +1361,6 @@ static int cpsw_ndo_stop(struct net_device *ndev)
 	if (cpsw_common_res_usage_state(priv) <= 1) {
 		cpts_unregister(priv->cpts);
 		cpsw_intr_disable(priv);
-		cpdma_ctlr_int_ctrl(priv->dma, false);
 		cpdma_ctlr_stop(priv->dma);
 		cpsw_ale_stop(priv->ale);
 	}
@@ -1456,7 +1455,7 @@ static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)
 
 		if (priv->cpts->rx_enable)
 			ctrl |= CTRL_V2_RX_TS_BITS;
-	break;
+		break;
 	case CPSW_VERSION_3:
 	default:
 		ctrl &= ~CTRL_V3_ALL_TS_MASK;
@@ -1466,7 +1465,7 @@ static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)
 
 		if (priv->cpts->rx_enable)
 			ctrl |= CTRL_V3_RX_TS_BITS;
-	break;
+		break;
 	}
 
 	mtype = (30 << TS_SEQ_ID_OFFSET_SHIFT) | EVENT_MSG_BITS;
@@ -1589,10 +1588,8 @@ static void cpsw_ndo_tx_timeout(struct net_device *ndev)
 	cpsw_err(priv, tx_err, "transmit timeout, restarting dma\n");
 	ndev->stats.tx_errors++;
 	cpsw_intr_disable(priv);
-	cpdma_ctlr_int_ctrl(priv->dma, false);
 	cpdma_chan_stop(priv->txch);
 	cpdma_chan_start(priv->txch);
-	cpdma_ctlr_int_ctrl(priv->dma, true);
 	cpsw_intr_enable(priv);
 }
 
@@ -1629,10 +1626,8 @@ static void cpsw_ndo_poll_controller(struct net_device *ndev)
 	struct cpsw_priv *priv = netdev_priv(ndev);
 
 	cpsw_intr_disable(priv);
-	cpdma_ctlr_int_ctrl(priv->dma, false);
 	cpsw_rx_interrupt(priv->irqs_table[0], priv);
 	cpsw_tx_interrupt(priv->irqs_table[1], priv);
-	cpdma_ctlr_int_ctrl(priv->dma, true);
 	cpsw_intr_enable(priv);
 }
 #endif
diff --git a/drivers/net/ethernet/ti/cpsw_ale.c b/drivers/net/ethernet/ti/cpsw_ale.c
index 6e927b4583aa..43b061bd8e07 100644
--- a/drivers/net/ethernet/ti/cpsw_ale.c
+++ b/drivers/net/ethernet/ti/cpsw_ale.c
@@ -268,39 +268,6 @@ int cpsw_ale_flush_multicast(struct cpsw_ale *ale, int port_mask, int vid)
 }
 EXPORT_SYMBOL_GPL(cpsw_ale_flush_multicast);
 
-static void cpsw_ale_flush_ucast(struct cpsw_ale *ale, u32 *ale_entry,
-				 int port_mask)
-{
-	int port;
-
-	port = cpsw_ale_get_port_num(ale_entry);
-	if ((BIT(port) & port_mask) == 0)
-		return; /* ports dont intersect, not interested */
-	cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_FREE);
-}
-
-int cpsw_ale_flush(struct cpsw_ale *ale, int port_mask)
-{
-	u32 ale_entry[ALE_ENTRY_WORDS];
-	int ret, idx;
-
-	for (idx = 0; idx < ale->params.ale_entries; idx++) {
-		cpsw_ale_read(ale, idx, ale_entry);
-		ret = cpsw_ale_get_entry_type(ale_entry);
-		if (ret != ALE_TYPE_ADDR && ret != ALE_TYPE_VLAN_ADDR)
-			continue;
-
-		if (cpsw_ale_get_mcast(ale_entry))
-			cpsw_ale_flush_mcast(ale, ale_entry, port_mask);
-		else
-			cpsw_ale_flush_ucast(ale, ale_entry, port_mask);
-
-		cpsw_ale_write(ale, idx, ale_entry);
-	}
-	return 0;
-}
-EXPORT_SYMBOL_GPL(cpsw_ale_flush);
-
 static inline void cpsw_ale_set_vlan_entry_type(u32 *ale_entry,
 						int flags, u16 vid)
 {
@@ -752,18 +719,6 @@ static void cpsw_ale_timer(unsigned long arg)
 	}
 }
 
-int cpsw_ale_set_ageout(struct cpsw_ale *ale, int ageout)
-{
-	del_timer_sync(&ale->timer);
-	ale->ageout = ageout * HZ;
-	if (ale->ageout) {
-		ale->timer.expires = jiffies + ale->ageout;
-		add_timer(&ale->timer);
-	}
-	return 0;
-}
-EXPORT_SYMBOL_GPL(cpsw_ale_set_ageout);
-
 void cpsw_ale_start(struct cpsw_ale *ale)
 {
 	u32 rev;
diff --git a/drivers/net/ethernet/ti/cpsw_ale.h b/drivers/net/ethernet/ti/cpsw_ale.h
index af1e7ecd87c6..a7001894f3da 100644
--- a/drivers/net/ethernet/ti/cpsw_ale.h
+++ b/drivers/net/ethernet/ti/cpsw_ale.h
@@ -90,8 +90,6 @@ int cpsw_ale_destroy(struct cpsw_ale *ale);
 void cpsw_ale_start(struct cpsw_ale *ale);
 void cpsw_ale_stop(struct cpsw_ale *ale);
 
-int cpsw_ale_set_ageout(struct cpsw_ale *ale, int ageout);
-int cpsw_ale_flush(struct cpsw_ale *ale, int port_mask);
 int cpsw_ale_flush_multicast(struct cpsw_ale *ale, int port_mask, int vid);
 int cpsw_ale_add_ucast(struct cpsw_ale *ale, u8 *addr, int port,
 		       int flags, u16 vid);
diff --git a/drivers/net/ethernet/ti/netcp_core.c b/drivers/net/ethernet/ti/netcp_core.c
index 43efc3a0cda5..5ec4ed3f6c8d 100644
--- a/drivers/net/ethernet/ti/netcp_core.c
+++ b/drivers/net/ethernet/ti/netcp_core.c
@@ -537,7 +537,7 @@ int netcp_unregister_rxhook(struct netcp_intf *netcp_priv, int order,
 static void netcp_frag_free(bool is_frag, void *ptr)
 {
 	if (is_frag)
-		put_page(virt_to_head_page(ptr));
+		skb_free_frag(ptr);
 	else
 		kfree(ptr);
 }
@@ -698,7 +698,6 @@ static int netcp_process_one_rx_packet(struct netcp_intf *netcp)
 		}
 	}
 
-	netcp->ndev->last_rx = jiffies;
 	netcp->ndev->stats.rx_packets++;
 	netcp->ndev->stats.rx_bytes += skb->len;
 
diff --git a/drivers/net/ethernet/tile/tilepro.c b/drivers/net/ethernet/tile/tilepro.c
index 3d8f60d9643e..6f0a4495c7f3 100644
--- a/drivers/net/ethernet/tile/tilepro.c
+++ b/drivers/net/ethernet/tile/tilepro.c
@@ -721,9 +721,6 @@ static bool tile_net_poll_aux(struct tile_net_cpu *info, int index)
 	if (!hash_default)
 		__inv_buffer(buf, len);
 
-	/* ISSUE: Is this needed? */
-	dev->last_rx = jiffies;
-
 #ifdef TILE_NET_DUMP_PACKETS
 	dump_packet(buf, len, "rx");
 #endif /* TILE_NET_DUMP_PACKETS */
diff --git a/drivers/net/ethernet/toshiba/Kconfig b/drivers/net/ethernet/toshiba/Kconfig
index 5d244b6b5e3a..6f1d5b623768 100644
--- a/drivers/net/ethernet/toshiba/Kconfig
+++ b/drivers/net/ethernet/toshiba/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_TOSHIBA
 	default y
 	depends on PCI && (PPC_IBM_CELL_BLADE || MIPS) || PPC_PS3
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/toshiba/ps3_gelic_net.c b/drivers/net/ethernet/toshiba/ps3_gelic_net.c
index ac62a5e248b0..79f0ec4e51ac 100644
--- a/drivers/net/ethernet/toshiba/ps3_gelic_net.c
+++ b/drivers/net/ethernet/toshiba/ps3_gelic_net.c
@@ -102,6 +102,18 @@ static void gelic_card_get_ether_port_status(struct gelic_card *card,
 	}
 }
 
+/**
+ * gelic_descr_get_status -- returns the status of a descriptor
+ * @descr: descriptor to look at
+ *
+ * returns the status as in the dmac_cmd_status field of the descriptor
+ */
+static enum gelic_descr_dma_status
+gelic_descr_get_status(struct gelic_descr *descr)
+{
+	return be32_to_cpu(descr->dmac_cmd_status) & GELIC_DESCR_DMA_STAT_MASK;
+}
+
 static int gelic_card_set_link_mode(struct gelic_card *card, int mode)
 {
 	int status;
@@ -278,18 +290,6 @@ void gelic_card_down(struct gelic_card *card)
 }
 
 /**
- * gelic_descr_get_status -- returns the status of a descriptor
- * @descr: descriptor to look at
- *
- * returns the status as in the dmac_cmd_status field of the descriptor
- */
-static enum gelic_descr_dma_status
-gelic_descr_get_status(struct gelic_descr *descr)
-{
-	return be32_to_cpu(descr->dmac_cmd_status) & GELIC_DESCR_DMA_STAT_MASK;
-}
-
-/**
  * gelic_card_free_chain - free descriptor chain
  * @card: card structure
  * @descr_in: address of desc
diff --git a/drivers/net/ethernet/toshiba/spider_net.c b/drivers/net/ethernet/toshiba/spider_net.c
index 8e9371a3388a..3c54a2cae5df 100644
--- a/drivers/net/ethernet/toshiba/spider_net.c
+++ b/drivers/net/ethernet/toshiba/spider_net.c
@@ -604,8 +604,7 @@ spider_net_set_multi(struct net_device *netdev)
 	int i;
 	u32 reg;
 	struct spider_net_card *card = netdev_priv(netdev);
-	unsigned long bitmask[SPIDER_NET_MULTICAST_HASHES / BITS_PER_LONG] =
-		{0, };
+	DECLARE_BITMAP(bitmask, SPIDER_NET_MULTICAST_HASHES) = {};
 
 	spider_net_set_promisc(card);
 
diff --git a/drivers/net/ethernet/tundra/Kconfig b/drivers/net/ethernet/tundra/Kconfig
index cf7d69b62c42..81d845e4e23b 100644
--- a/drivers/net/ethernet/tundra/Kconfig
+++ b/drivers/net/ethernet/tundra/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_TUNDRA
 	default y
 	depends on TSI108_BRIDGE
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/via/Kconfig b/drivers/net/ethernet/via/Kconfig
index f66ddaee0c87..2f1264b882b9 100644
--- a/drivers/net/ethernet/via/Kconfig
+++ b/drivers/net/ethernet/via/Kconfig
@@ -6,9 +6,7 @@ config NET_VENDOR_VIA
 	bool "VIA devices"
 	default y
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
@@ -19,7 +17,8 @@ if NET_VENDOR_VIA
 
 config VIA_RHINE
 	tristate "VIA Rhine support"
-	depends on (PCI || USE_OF)
+	depends on (PCI || OF_IRQ)
+	depends on HAS_DMA
 	select CRC32
 	select MII
 	---help---
@@ -43,7 +42,8 @@ config VIA_RHINE_MMIO
 
 config VIA_VELOCITY
 	tristate "VIA Velocity support"
-	depends on (PCI || USE_OF)
+	depends on (PCI || (OF_ADDRESS && OF_IRQ))
+	depends on HAS_DMA
 	select CRC32
 	select CRC_CCITT
 	select MII
diff --git a/drivers/net/ethernet/via/via-rhine.c b/drivers/net/ethernet/via/via-rhine.c
index de2850497c09..a83263743665 100644
--- a/drivers/net/ethernet/via/via-rhine.c
+++ b/drivers/net/ethernet/via/via-rhine.c
@@ -96,7 +96,6 @@ static const int multicast_filter_limit = 32;
 #include <linux/ioport.h>
 #include <linux/interrupt.h>
 #include <linux/pci.h>
-#include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/of_irq.h>
 #include <linux/platform_device.h>
@@ -472,8 +471,7 @@ struct rhine_private {
 
 	/* Frequently used values: keep some adjacent for cache effect. */
 	u32 quirks;
-	struct rx_desc *rx_head_desc;
-	unsigned int cur_rx, dirty_rx;	/* Producer/consumer ring indices */
+	unsigned int cur_rx;
 	unsigned int cur_tx, dirty_tx;
 	unsigned int rx_buf_sz;		/* Based on MTU+slack. */
 	struct rhine_stats rx_stats;
@@ -1213,17 +1211,61 @@ static void free_ring(struct net_device* dev)
 
 }
 
-static void alloc_rbufs(struct net_device *dev)
+struct rhine_skb_dma {
+	struct sk_buff *skb;
+	dma_addr_t dma;
+};
+
+static inline int rhine_skb_dma_init(struct net_device *dev,
+				     struct rhine_skb_dma *sd)
 {
 	struct rhine_private *rp = netdev_priv(dev);
 	struct device *hwdev = dev->dev.parent;
-	dma_addr_t next;
+	const int size = rp->rx_buf_sz;
+
+	sd->skb = netdev_alloc_skb(dev, size);
+	if (!sd->skb)
+		return -ENOMEM;
+
+	sd->dma = dma_map_single(hwdev, sd->skb->data, size, DMA_FROM_DEVICE);
+	if (unlikely(dma_mapping_error(hwdev, sd->dma))) {
+		netif_err(rp, drv, dev, "Rx DMA mapping failure\n");
+		dev_kfree_skb_any(sd->skb);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void rhine_reset_rbufs(struct rhine_private *rp)
+{
 	int i;
 
-	rp->dirty_rx = rp->cur_rx = 0;
+	rp->cur_rx = 0;
+
+	for (i = 0; i < RX_RING_SIZE; i++)
+		rp->rx_ring[i].rx_status = cpu_to_le32(DescOwn);
+}
+
+static inline void rhine_skb_dma_nic_store(struct rhine_private *rp,
+					   struct rhine_skb_dma *sd, int entry)
+{
+	rp->rx_skbuff_dma[entry] = sd->dma;
+	rp->rx_skbuff[entry] = sd->skb;
+
+	rp->rx_ring[entry].addr = cpu_to_le32(sd->dma);
+	dma_wmb();
+}
+
+static void free_rbufs(struct net_device* dev);
+
+static int alloc_rbufs(struct net_device *dev)
+{
+	struct rhine_private *rp = netdev_priv(dev);
+	dma_addr_t next;
+	int rc, i;
 
 	rp->rx_buf_sz = (dev->mtu <= 1500 ? PKT_BUF_SZ : dev->mtu + 32);
-	rp->rx_head_desc = &rp->rx_ring[0];
 	next = rp->rx_ring_dma;
 
 	/* Init the ring entries */
@@ -1239,23 +1281,20 @@ static void alloc_rbufs(struct net_device *dev)
 
 	/* Fill in the Rx buffers.  Handle allocation failure gracefully. */
 	for (i = 0; i < RX_RING_SIZE; i++) {
-		struct sk_buff *skb = netdev_alloc_skb(dev, rp->rx_buf_sz);
-		rp->rx_skbuff[i] = skb;
-		if (skb == NULL)
-			break;
+		struct rhine_skb_dma sd;
 
-		rp->rx_skbuff_dma[i] =
-			dma_map_single(hwdev, skb->data, rp->rx_buf_sz,
-				       DMA_FROM_DEVICE);
-		if (dma_mapping_error(hwdev, rp->rx_skbuff_dma[i])) {
-			rp->rx_skbuff_dma[i] = 0;
-			dev_kfree_skb(skb);
-			break;
+		rc = rhine_skb_dma_init(dev, &sd);
+		if (rc < 0) {
+			free_rbufs(dev);
+			goto out;
 		}
-		rp->rx_ring[i].addr = cpu_to_le32(rp->rx_skbuff_dma[i]);
-		rp->rx_ring[i].rx_status = cpu_to_le32(DescOwn);
+
+		rhine_skb_dma_nic_store(rp, &sd, i);
 	}
-	rp->dirty_rx = (unsigned int)(i - RX_RING_SIZE);
+
+	rhine_reset_rbufs(rp);
+out:
+	return rc;
 }
 
 static void free_rbufs(struct net_device* dev)
@@ -1659,16 +1698,18 @@ static int rhine_open(struct net_device *dev)
 
 	rc = request_irq(rp->irq, rhine_interrupt, IRQF_SHARED, dev->name, dev);
 	if (rc)
-		return rc;
+		goto out;
 
 	netif_dbg(rp, ifup, dev, "%s() irq %d\n", __func__, rp->irq);
 
 	rc = alloc_ring(dev);
-	if (rc) {
-		free_irq(rp->irq, dev);
-		return rc;
-	}
-	alloc_rbufs(dev);
+	if (rc < 0)
+		goto out_free_irq;
+
+	rc = alloc_rbufs(dev);
+	if (rc < 0)
+		goto out_free_ring;
+
 	alloc_tbufs(dev);
 	rhine_chip_reset(dev);
 	rhine_task_enable(rp);
@@ -1680,7 +1721,14 @@ static int rhine_open(struct net_device *dev)
 
 	netif_start_queue(dev);
 
-	return 0;
+out:
+	return rc;
+
+out_free_ring:
+	free_ring(dev);
+out_free_irq:
+	free_irq(rp->irq, dev);
+	goto out;
 }
 
 static void rhine_reset_task(struct work_struct *work)
@@ -1700,9 +1748,9 @@ static void rhine_reset_task(struct work_struct *work)
 
 	/* clear all descriptors */
 	free_tbufs(dev);
-	free_rbufs(dev);
 	alloc_tbufs(dev);
-	alloc_rbufs(dev);
+
+	rhine_reset_rbufs(rp);
 
 	/* Reinitialize the hardware. */
 	rhine_chip_reset(dev);
@@ -1730,6 +1778,11 @@ static void rhine_tx_timeout(struct net_device *dev)
 	schedule_work(&rp->reset_task);
 }
 
+static inline bool rhine_tx_queue_full(struct rhine_private *rp)
+{
+	return (rp->cur_tx - rp->dirty_tx) >= TX_QUEUE_LEN;
+}
+
 static netdev_tx_t rhine_start_tx(struct sk_buff *skb,
 				  struct net_device *dev)
 {
@@ -1800,11 +1853,17 @@ static netdev_tx_t rhine_start_tx(struct sk_buff *skb,
 
 	netdev_sent_queue(dev, skb->len);
 	/* lock eth irq */
-	wmb();
+	dma_wmb();
 	rp->tx_ring[entry].tx_status |= cpu_to_le32(DescOwn);
 	wmb();
 
 	rp->cur_tx++;
+	/*
+	 * Nobody wants cur_tx write to rot for ages after the NIC will have
+	 * seen the transmit request, especially as the transmit completion
+	 * handler could miss it.
+	 */
+	smp_wmb();
 
 	/* Non-x86 Todo: explicitly flush cache lines here. */
 
@@ -1817,8 +1876,14 @@ static netdev_tx_t rhine_start_tx(struct sk_buff *skb,
 	       ioaddr + ChipCmd1);
 	IOSYNC;
 
-	if (rp->cur_tx == rp->dirty_tx + TX_QUEUE_LEN)
+	/* dirty_tx may be pessimistically out-of-sync. See rhine_tx. */
+	if (rhine_tx_queue_full(rp)) {
 		netif_stop_queue(dev);
+		smp_rmb();
+		/* Rejuvenate. */
+		if (!rhine_tx_queue_full(rp))
+			netif_wake_queue(dev);
+	}
 
 	netif_dbg(rp, tx_queued, dev, "Transmit frame #%d queued in slot %d\n",
 		  rp->cur_tx - 1, entry);
@@ -1866,13 +1931,24 @@ static void rhine_tx(struct net_device *dev)
 {
 	struct rhine_private *rp = netdev_priv(dev);
 	struct device *hwdev = dev->dev.parent;
-	int txstatus = 0, entry = rp->dirty_tx % TX_RING_SIZE;
 	unsigned int pkts_compl = 0, bytes_compl = 0;
+	unsigned int dirty_tx = rp->dirty_tx;
+	unsigned int cur_tx;
 	struct sk_buff *skb;
 
+	/*
+	 * The race with rhine_start_tx does not matter here as long as the
+	 * driver enforces a value of cur_tx that was relevant when the
+	 * packet was scheduled to the network chipset.
+	 * Executive summary: smp_rmb() balances smp_wmb() in rhine_start_tx.
+	 */
+	smp_rmb();
+	cur_tx = rp->cur_tx;
 	/* find and cleanup dirty tx descriptors */
-	while (rp->dirty_tx != rp->cur_tx) {
-		txstatus = le32_to_cpu(rp->tx_ring[entry].tx_status);
+	while (dirty_tx != cur_tx) {
+		unsigned int entry = dirty_tx % TX_RING_SIZE;
+		u32 txstatus = le32_to_cpu(rp->tx_ring[entry].tx_status);
+
 		netif_dbg(rp, tx_done, dev, "Tx scavenge %d status %08x\n",
 			  entry, txstatus);
 		if (txstatus & DescOwn)
@@ -1921,12 +1997,23 @@ static void rhine_tx(struct net_device *dev)
 		pkts_compl++;
 		dev_consume_skb_any(skb);
 		rp->tx_skbuff[entry] = NULL;
-		entry = (++rp->dirty_tx) % TX_RING_SIZE;
+		dirty_tx++;
 	}
 
+	rp->dirty_tx = dirty_tx;
+	/* Pity we can't rely on the nearby BQL completion implicit barrier. */
+	smp_wmb();
+
 	netdev_completed_queue(dev, pkts_compl, bytes_compl);
-	if ((rp->cur_tx - rp->dirty_tx) < TX_QUEUE_LEN - 4)
+
+	/* cur_tx may be optimistically out-of-sync. See rhine_start_tx. */
+	if (!rhine_tx_queue_full(rp) && netif_queue_stopped(dev)) {
 		netif_wake_queue(dev);
+		smp_rmb();
+		/* Rejuvenate. */
+		if (rhine_tx_queue_full(rp))
+			netif_stop_queue(dev);
+	}
 }
 
 /**
@@ -1944,22 +2031,33 @@ static inline u16 rhine_get_vlan_tci(struct sk_buff *skb, int data_size)
 	return be16_to_cpup((__be16 *)trailer);
 }
 
+static inline void rhine_rx_vlan_tag(struct sk_buff *skb, struct rx_desc *desc,
+				     int data_size)
+{
+	dma_rmb();
+	if (unlikely(desc->desc_length & cpu_to_le32(DescTag))) {
+		u16 vlan_tci;
+
+		vlan_tci = rhine_get_vlan_tci(skb, data_size);
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci);
+	}
+}
+
 /* Process up to limit frames from receive ring */
 static int rhine_rx(struct net_device *dev, int limit)
 {
 	struct rhine_private *rp = netdev_priv(dev);
 	struct device *hwdev = dev->dev.parent;
-	int count;
 	int entry = rp->cur_rx % RX_RING_SIZE;
+	int count;
 
 	netif_dbg(rp, rx_status, dev, "%s(), entry %d status %08x\n", __func__,
-		  entry, le32_to_cpu(rp->rx_head_desc->rx_status));
+		  entry, le32_to_cpu(rp->rx_ring[entry].rx_status));
 
 	/* If EOP is set on the next entry, it's a new packet. Send it up. */
 	for (count = 0; count < limit; ++count) {
-		struct rx_desc *desc = rp->rx_head_desc;
+		struct rx_desc *desc = rp->rx_ring + entry;
 		u32 desc_status = le32_to_cpu(desc->rx_status);
-		u32 desc_length = le32_to_cpu(desc->desc_length);
 		int data_size = desc_status >> 16;
 
 		if (desc_status & DescOwn)
@@ -1975,10 +2073,6 @@ static int rhine_rx(struct net_device *dev, int limit)
 	"entry %#x length %d status %08x!\n",
 					    entry, data_size,
 					    desc_status);
-				netdev_warn(dev,
-					    "Oversized Ethernet frame %p vs %p\n",
-					    rp->rx_head_desc,
-					    &rp->rx_ring[entry]);
 				dev->stats.rx_length_errors++;
 			} else if (desc_status & RxErr) {
 				/* There was a error. */
@@ -2000,16 +2094,17 @@ static int rhine_rx(struct net_device *dev, int limit)
 				}
 			}
 		} else {
-			struct sk_buff *skb = NULL;
 			/* Length should omit the CRC */
 			int pkt_len = data_size - 4;
-			u16 vlan_tci = 0;
+			struct sk_buff *skb;
 
 			/* Check if the packet is long enough to accept without
 			   copying to a minimally-sized skbuff. */
-			if (pkt_len < rx_copybreak)
+			if (pkt_len < rx_copybreak) {
 				skb = netdev_alloc_skb_ip_align(dev, pkt_len);
-			if (skb) {
+				if (unlikely(!skb))
+					goto drop;
+
 				dma_sync_single_for_cpu(hwdev,
 							rp->rx_skbuff_dma[entry],
 							rp->rx_buf_sz,
@@ -2018,32 +2113,31 @@ static int rhine_rx(struct net_device *dev, int limit)
 				skb_copy_to_linear_data(skb,
 						 rp->rx_skbuff[entry]->data,
 						 pkt_len);
-				skb_put(skb, pkt_len);
+
 				dma_sync_single_for_device(hwdev,
 							   rp->rx_skbuff_dma[entry],
 							   rp->rx_buf_sz,
 							   DMA_FROM_DEVICE);
 			} else {
+				struct rhine_skb_dma sd;
+
+				if (unlikely(rhine_skb_dma_init(dev, &sd) < 0))
+					goto drop;
+
 				skb = rp->rx_skbuff[entry];
-				if (skb == NULL) {
-					netdev_err(dev, "Inconsistent Rx descriptor chain\n");
-					break;
-				}
-				rp->rx_skbuff[entry] = NULL;
-				skb_put(skb, pkt_len);
+
 				dma_unmap_single(hwdev,
 						 rp->rx_skbuff_dma[entry],
 						 rp->rx_buf_sz,
 						 DMA_FROM_DEVICE);
+				rhine_skb_dma_nic_store(rp, &sd, entry);
 			}
 
-			if (unlikely(desc_length & DescTag))
-				vlan_tci = rhine_get_vlan_tci(skb, data_size);
-
+			skb_put(skb, pkt_len);
 			skb->protocol = eth_type_trans(skb, dev);
 
-			if (unlikely(desc_length & DescTag))
-				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci);
+			rhine_rx_vlan_tag(skb, desc, data_size);
+
 			netif_receive_skb(skb);
 
 			u64_stats_update_begin(&rp->rx_stats.syncp);
@@ -2051,35 +2145,16 @@ static int rhine_rx(struct net_device *dev, int limit)
 			rp->rx_stats.packets++;
 			u64_stats_update_end(&rp->rx_stats.syncp);
 		}
+give_descriptor_to_nic:
+		desc->rx_status = cpu_to_le32(DescOwn);
 		entry = (++rp->cur_rx) % RX_RING_SIZE;
-		rp->rx_head_desc = &rp->rx_ring[entry];
-	}
-
-	/* Refill the Rx ring buffers. */
-	for (; rp->cur_rx - rp->dirty_rx > 0; rp->dirty_rx++) {
-		struct sk_buff *skb;
-		entry = rp->dirty_rx % RX_RING_SIZE;
-		if (rp->rx_skbuff[entry] == NULL) {
-			skb = netdev_alloc_skb(dev, rp->rx_buf_sz);
-			rp->rx_skbuff[entry] = skb;
-			if (skb == NULL)
-				break;	/* Better luck next round. */
-			rp->rx_skbuff_dma[entry] =
-				dma_map_single(hwdev, skb->data,
-					       rp->rx_buf_sz,
-					       DMA_FROM_DEVICE);
-			if (dma_mapping_error(hwdev,
-					      rp->rx_skbuff_dma[entry])) {
-				dev_kfree_skb(skb);
-				rp->rx_skbuff_dma[entry] = 0;
-				break;
-			}
-			rp->rx_ring[entry].addr = cpu_to_le32(rp->rx_skbuff_dma[entry]);
-		}
-		rp->rx_ring[entry].rx_status = cpu_to_le32(DescOwn);
 	}
 
 	return count;
+
+drop:
+	dev->stats.rx_dropped++;
+	goto give_descriptor_to_nic;
 }
 
 static void rhine_restart_tx(struct net_device *dev) {
@@ -2484,9 +2559,8 @@ static int rhine_resume(struct device *device)
 	enable_mmio(rp->pioaddr, rp->quirks);
 	rhine_power_init(dev);
 	free_tbufs(dev);
-	free_rbufs(dev);
 	alloc_tbufs(dev);
-	alloc_rbufs(dev);
+	rhine_reset_rbufs(rp);
 	rhine_task_enable(rp);
 	spin_lock_bh(&rp->lock);
 	init_registers(dev);
diff --git a/drivers/net/ethernet/wiznet/Kconfig b/drivers/net/ethernet/wiznet/Kconfig
index b4d281626fb4..f98b91d21f33 100644
--- a/drivers/net/ethernet/wiznet/Kconfig
+++ b/drivers/net/ethernet/wiznet/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_WIZNET
 	depends on HAS_IOMEM
 	default y
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/xilinx/Kconfig b/drivers/net/ethernet/xilinx/Kconfig
index 7b90a5eba099..4f5c024c6192 100644
--- a/drivers/net/ethernet/xilinx/Kconfig
+++ b/drivers/net/ethernet/xilinx/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_XILINX
 	default y
 	depends on PPC || PPC32 || MICROBLAZE || ARCH_ZYNQ
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/xilinx/ll_temac_main.c b/drivers/net/ethernet/xilinx/ll_temac_main.c
index af2694dc6f90..5a1068df7038 100644
--- a/drivers/net/ethernet/xilinx/ll_temac_main.c
+++ b/drivers/net/ethernet/xilinx/ll_temac_main.c
@@ -62,12 +62,12 @@
 
 u32 temac_ior(struct temac_local *lp, int offset)
 {
-	return in_be32((u32 *)(lp->regs + offset));
+	return in_be32(lp->regs + offset);
 }
 
 void temac_iow(struct temac_local *lp, int offset, u32 value)
 {
-	out_be32((u32 *) (lp->regs + offset), value);
+	out_be32(lp->regs + offset, value);
 }
 
 int temac_indirect_busywait(struct temac_local *lp)
@@ -124,7 +124,7 @@ void temac_indirect_out32(struct temac_local *lp, int reg, u32 value)
  */
 static u32 temac_dma_in32(struct temac_local *lp, int reg)
 {
-	return in_be32((u32 *)(lp->sdma_regs + (reg << 2)));
+	return in_be32(lp->sdma_regs + (reg << 2));
 }
 
 /**
@@ -134,7 +134,7 @@ static u32 temac_dma_in32(struct temac_local *lp, int reg)
  */
 static void temac_dma_out32(struct temac_local *lp, int reg, u32 value)
 {
-	out_be32((u32 *)(lp->sdma_regs + (reg << 2)), value);
+	out_be32(lp->sdma_regs + (reg << 2), value);
 }
 
 /* DMA register access functions can be DCR based or memory mapped.
@@ -400,7 +400,7 @@ static void temac_set_multicast_list(struct net_device *ndev)
 	mutex_unlock(&lp->indirect_mutex);
 }
 
-struct temac_option {
+static struct temac_option {
 	int flg;
 	u32 opt;
 	u32 reg;
@@ -587,7 +587,7 @@ static void temac_device_reset(struct net_device *ndev)
 	ndev->trans_start = jiffies; /* prevent tx timeout */
 }
 
-void temac_adjust_link(struct net_device *ndev)
+static void temac_adjust_link(struct net_device *ndev)
 {
 	struct temac_local *lp = netdev_priv(ndev);
 	struct phy_device *phy = lp->phy_dev;
@@ -688,10 +688,8 @@ static int temac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	cur_p = &lp->tx_bd_v[lp->tx_bd_tail];
 
 	if (temac_check_tx_bd_space(lp, num_frag)) {
-		if (!netif_queue_stopped(ndev)) {
+		if (!netif_queue_stopped(ndev))
 			netif_stop_queue(ndev);
-			return NETDEV_TX_BUSY;
-		}
 		return NETDEV_TX_BUSY;
 	}
 
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet.h b/drivers/net/ethernet/xilinx/xilinx_axienet.h
index 4c9b4fa1d3c1..7cb9abac95c8 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet.h
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet.h
@@ -11,16 +11,16 @@
 #include <linux/netdevice.h>
 #include <linux/spinlock.h>
 #include <linux/interrupt.h>
+#include <linux/if_vlan.h>
 
 /* Packet size info */
 #define XAE_HDR_SIZE			14 /* Size of Ethernet header */
-#define XAE_HDR_VLAN_SIZE		18 /* Size of an Ethernet hdr + VLAN */
 #define XAE_TRL_SIZE			 4 /* Size of Ethernet trailer (FCS) */
 #define XAE_MTU			      1500 /* Max MTU of an Ethernet frame */
 #define XAE_JUMBO_MTU		      9000 /* Max MTU of a jumbo Eth. frame */
 
 #define XAE_MAX_FRAME_SIZE	 (XAE_MTU + XAE_HDR_SIZE + XAE_TRL_SIZE)
-#define XAE_MAX_VLAN_FRAME_SIZE  (XAE_MTU + XAE_HDR_VLAN_SIZE + XAE_TRL_SIZE)
+#define XAE_MAX_VLAN_FRAME_SIZE  (XAE_MTU + VLAN_ETH_HLEN + XAE_TRL_SIZE)
 #define XAE_MAX_JUMBO_FRAME_SIZE (XAE_JUMBO_MTU + XAE_HDR_SIZE + XAE_TRL_SIZE)
 
 /* Configuration options */
@@ -38,18 +38,21 @@
 #define XAE_OPTION_FLOW_CONTROL			(1 << 4)
 
 /* Strip FCS and PAD from incoming frames. Note: PAD from VLAN frames is not
- * stripped. Default: disabled (set) */
+ * stripped. Default: disabled (set)
+ */
 #define XAE_OPTION_FCS_STRIP			(1 << 5)
 
 /* Generate FCS field and add PAD automatically for outgoing frames.
- * Default: enabled (set) */
+ * Default: enabled (set)
+ */
 #define XAE_OPTION_FCS_INSERT			(1 << 6)
 
 /* Enable Length/Type error checking for incoming frames. When this option is
  * set, the MAC will filter frames that have a mismatched type/length field
  * and if XAE_OPTION_REPORT_RXERR is set, the user is notified when these
  * types of frames are encountered. When this option is cleared, the MAC will
- * allow these types of frames to be received. Default: enabled (set) */
+ * allow these types of frames to be received. Default: enabled (set)
+ */
 #define XAE_OPTION_LENTYPE_ERR			(1 << 7)
 
 /* Enable the transmitter. Default: enabled (set) */
@@ -159,12 +162,12 @@
 #define XAE_MDIO_MWD_OFFSET	0x00000508 /* MII Management Write Data */
 #define XAE_MDIO_MRD_OFFSET	0x0000050C /* MII Management Read Data */
 #define XAE_MDIO_MIS_OFFSET	0x00000600 /* MII Management Interrupt Status */
-#define XAE_MDIO_MIP_OFFSET	0x00000620 /* MII Mgmt Interrupt Pending
-					    * register offset */
-#define XAE_MDIO_MIE_OFFSET	0x00000640 /* MII Management Interrupt Enable
-					    * register offset */
-#define XAE_MDIO_MIC_OFFSET	0x00000660 /* MII Management Interrupt Clear
-					    * register offset. */
+/* MII Mgmt Interrupt Pending register offset */
+#define XAE_MDIO_MIP_OFFSET	0x00000620
+/* MII Management Interrupt Enable register offset */
+#define XAE_MDIO_MIE_OFFSET	0x00000640
+/* MII Management Interrupt Clear register offset. */
+#define XAE_MDIO_MIC_OFFSET	0x00000660
 #define XAE_UAW0_OFFSET		0x00000700 /* Unicast address word 0 */
 #define XAE_UAW1_OFFSET		0x00000704 /* Unicast address word 1 */
 #define XAE_FMI_OFFSET		0x00000708 /* Filter Mask Index */
@@ -176,18 +179,17 @@
 #define XAE_MCAST_TABLE_OFFSET	0x00020000 /* Multicast table address */
 
 /* Bit Masks for Axi Ethernet RAF register */
-#define XAE_RAF_MCSTREJ_MASK		0x00000002 /* Reject receive multicast
-						    * destination address */
-#define XAE_RAF_BCSTREJ_MASK		0x00000004 /* Reject receive broadcast
-						    * destination address */
+/* Reject receive multicast destination address */
+#define XAE_RAF_MCSTREJ_MASK		0x00000002
+/* Reject receive broadcast destination address */
+#define XAE_RAF_BCSTREJ_MASK		0x00000004
 #define XAE_RAF_TXVTAGMODE_MASK		0x00000018 /* Tx VLAN TAG mode */
 #define XAE_RAF_RXVTAGMODE_MASK		0x00000060 /* Rx VLAN TAG mode */
 #define XAE_RAF_TXVSTRPMODE_MASK	0x00000180 /* Tx VLAN STRIP mode */
 #define XAE_RAF_RXVSTRPMODE_MASK	0x00000600 /* Rx VLAN STRIP mode */
 #define XAE_RAF_NEWFNCENBL_MASK		0x00000800 /* New function mode */
-#define XAE_RAF_EMULTIFLTRENBL_MASK	0x00001000 /* Exteneded Multicast
-						    * Filtering mode
-						    */
+/* Exteneded Multicast Filtering mode */
+#define XAE_RAF_EMULTIFLTRENBL_MASK	0x00001000
 #define XAE_RAF_STATSRST_MASK		0x00002000 /* Stats. Counter Reset */
 #define XAE_RAF_RXBADFRMEN_MASK		0x00004000 /* Recv Bad Frame Enable */
 #define XAE_RAF_TXVTAGMODE_SHIFT	3 /* Tx Tag mode shift bits */
@@ -197,15 +199,16 @@
 
 /* Bit Masks for Axi Ethernet TPF and IFGP registers */
 #define XAE_TPF_TPFV_MASK		0x0000FFFF /* Tx pause frame value */
-#define XAE_IFGP0_IFGP_MASK		0x0000007F /* Transmit inter-frame
-						    * gap adjustment value */
+/* Transmit inter-frame gap adjustment value */
+#define XAE_IFGP0_IFGP_MASK		0x0000007F
 
 /* Bit Masks for Axi Ethernet IS, IE and IP registers, Same masks apply
- * for all 3 registers. */
-#define XAE_INT_HARDACSCMPLT_MASK	0x00000001 /* Hard register access
-						    * complete */
-#define XAE_INT_AUTONEG_MASK		0x00000002 /* Auto negotiation
-						    * complete */
+ * for all 3 registers.
+ */
+/* Hard register access complete */
+#define XAE_INT_HARDACSCMPLT_MASK	0x00000001
+/* Auto negotiation complete */
+#define XAE_INT_AUTONEG_MASK		0x00000002
 #define XAE_INT_RXCMPIT_MASK		0x00000004 /* Rx complete */
 #define XAE_INT_RXRJECT_MASK		0x00000008 /* Rx frame rejected */
 #define XAE_INT_RXFIFOOVR_MASK		0x00000010 /* Rx fifo overrun */
@@ -215,10 +218,9 @@
 #define XAE_INT_PHYRSTCMPLT_MASK	0x00000100 /* Phy Reset complete */
 #define XAE_INT_ALL_MASK		0x0000003F /* All the ints */
 
+/* INT bits that indicate receive errors */
 #define XAE_INT_RECV_ERROR_MASK				\
-	(XAE_INT_RXRJECT_MASK | XAE_INT_RXFIFOOVR_MASK) /* INT bits that
-							 * indicate receive
-							 * errors */
+	(XAE_INT_RXRJECT_MASK | XAE_INT_RXFIFOOVR_MASK)
 
 /* Bit masks for Axi Ethernet VLAN TPID Word 0 register */
 #define XAE_TPID_0_MASK		0x0000FFFF /* TPID 0 */
@@ -231,27 +233,28 @@
 /* Bit masks for Axi Ethernet RCW1 register */
 #define XAE_RCW1_RST_MASK	0x80000000 /* Reset */
 #define XAE_RCW1_JUM_MASK	0x40000000 /* Jumbo frame enable */
-#define XAE_RCW1_FCS_MASK	0x20000000 /* In-Band FCS enable
-					    * (FCS not stripped) */
+/* In-Band FCS enable (FCS not stripped) */
+#define XAE_RCW1_FCS_MASK	0x20000000
 #define XAE_RCW1_RX_MASK	0x10000000 /* Receiver enable */
 #define XAE_RCW1_VLAN_MASK	0x08000000 /* VLAN frame enable */
-#define XAE_RCW1_LT_DIS_MASK	0x02000000 /* Length/type field valid check
-					    * disable */
-#define XAE_RCW1_CL_DIS_MASK	0x01000000 /* Control frame Length check
-					    * disable */
-#define XAE_RCW1_PAUSEADDR_MASK 0x0000FFFF /* Pause frame source address
-					    * bits [47:32]. Bits [31:0] are
-					    * stored in register RCW0 */
+/* Length/type field valid check disable */
+#define XAE_RCW1_LT_DIS_MASK	0x02000000
+/* Control frame Length check disable */
+#define XAE_RCW1_CL_DIS_MASK	0x01000000
+/* Pause frame source address bits [47:32]. Bits [31:0] are
+ * stored in register RCW0
+ */
+#define XAE_RCW1_PAUSEADDR_MASK 0x0000FFFF
 
 /* Bit masks for Axi Ethernet TC register */
 #define XAE_TC_RST_MASK		0x80000000 /* Reset */
 #define XAE_TC_JUM_MASK		0x40000000 /* Jumbo frame enable */
-#define XAE_TC_FCS_MASK		0x20000000 /* In-Band FCS enable
-					    * (FCS not generated) */
+/* In-Band FCS enable (FCS not generated) */
+#define XAE_TC_FCS_MASK		0x20000000
 #define XAE_TC_TX_MASK		0x10000000 /* Transmitter enable */
 #define XAE_TC_VLAN_MASK	0x08000000 /* VLAN frame enable */
-#define XAE_TC_IFG_MASK		0x02000000 /* Inter-frame gap adjustment
-					    * enable */
+/* Inter-frame gap adjustment enable */
+#define XAE_TC_IFG_MASK		0x02000000
 
 /* Bit masks for Axi Ethernet FCC register */
 #define XAE_FCC_FCRX_MASK	0x20000000 /* Rx flow control enable */
@@ -301,10 +304,10 @@
 #define XAE_MDIO_INT_MIIM_RDY_MASK	0x00000001 /* MIIM Interrupt */
 
 /* Bit masks for Axi Ethernet UAW1 register */
-#define XAE_UAW1_UNICASTADDR_MASK	0x0000FFFF /* Station address bits
-						    * [47:32]; Station address
-						    * bits [31:0] are stored in
-						    * register UAW0 */
+/* Station address bits [47:32]; Station address
+ * bits [31:0] are stored in register UAW0
+ */
+#define XAE_UAW1_UNICASTADDR_MASK	0x0000FFFF
 
 /* Bit masks for Axi Ethernet FMI register */
 #define XAE_FMI_PM_MASK			0x80000000 /* Promis. mode enable */
@@ -320,8 +323,8 @@
 #define XAE_PHY_TYPE_SGMII		4
 #define XAE_PHY_TYPE_1000BASE_X		5
 
-#define XAE_MULTICAST_CAM_TABLE_NUM	4 /* Total number of entries in the
-					   * hardware multicast table. */
+ /* Total number of entries in the hardware multicast table. */
+#define XAE_MULTICAST_CAM_TABLE_NUM	4
 
 /* Axi Ethernet Synthesis features */
 #define XAE_FEATURE_PARTIAL_RX_CSUM	(1 << 0)
@@ -407,8 +410,11 @@ struct axidma_bd {
  *		  Txed/Rxed in the existing hardware. If jumbo option is
  *		  supported, the maximum frame size would be 9k. Else it is
  *		  1522 bytes (assuming support for basic VLAN)
- * @jumbo_support: Stores hardware configuration for jumbo support. If hardware
- *		   can handle jumbo packets, this entry will be 1, else 0.
+ * @rxmem:	Stores rx memory size for jumbo frame handling.
+ * @csum_offload_on_tx_path:	Stores the checksum selection on TX side.
+ * @csum_offload_on_rx_path:	Stores the checksum selection on RX side.
+ * @coalesce_count_rx:	Store the irq coalesce on RX side.
+ * @coalesce_count_tx:	Store the irq coalesce on TX side.
  */
 struct axienet_local {
 	struct net_device *ndev;
@@ -446,7 +452,7 @@ struct axienet_local {
 	u32 rx_bd_ci;
 
 	u32 max_frm_size;
-	u32 jumbo_support;
+	u32 rxmem;
 
 	int csum_offload_on_tx_path;
 	int csum_offload_on_rx_path;
@@ -472,7 +478,7 @@ struct axienet_option {
  * @lp:         Pointer to axienet local structure
  * @offset:     Address offset from the base address of Axi Ethernet core
  *
- * returns: The contents of the Axi Ethernet register
+ * Return: The contents of the Axi Ethernet register
  *
  * This function returns the contents of the corresponding register.
  */
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
index 28b7e7d9c272..4208dd7ef101 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
@@ -117,7 +117,7 @@ static struct axienet_option axienet_options[] = {
  * @lp:		Pointer to axienet local structure
  * @reg:	Address offset from the base address of the Axi DMA core
  *
- * returns: The contents of the Axi DMA register
+ * Return: The contents of the Axi DMA register
  *
  * This function returns the contents of the corresponding Axi DMA register.
  */
@@ -179,8 +179,7 @@ static void axienet_dma_bd_release(struct net_device *ndev)
  * axienet_dma_bd_init - Setup buffer descriptor rings for Axi DMA
  * @ndev:	Pointer to the net_device structure
  *
- * returns: 0, on success
- *	    -ENOMEM, on failure
+ * Return: 0, on success -ENOMEM, on failure
  *
  * This function is called to initialize the Rx and Tx DMA descriptor
  * rings. This initializes the descriptors with required default values
@@ -198,9 +197,7 @@ static int axienet_dma_bd_init(struct net_device *ndev)
 	lp->tx_bd_tail = 0;
 	lp->rx_bd_ci = 0;
 
-	/*
-	 * Allocate the Tx and Rx buffer descriptors.
-	 */
+	/* Allocate the Tx and Rx buffer descriptors. */
 	lp->tx_bd_v = dma_zalloc_coherent(ndev->dev.parent,
 					  sizeof(*lp->tx_bd_v) * TX_BD_NUM,
 					  &lp->tx_bd_p, GFP_KERNEL);
@@ -263,7 +260,8 @@ static int axienet_dma_bd_init(struct net_device *ndev)
 	axienet_dma_out32(lp, XAXIDMA_TX_CR_OFFSET, cr);
 
 	/* Populate the tail pointer and bring the Rx Axi DMA engine out of
-	 * halted state. This will make the Rx side ready for reception.*/
+	 * halted state. This will make the Rx side ready for reception.
+	 */
 	axienet_dma_out32(lp, XAXIDMA_RX_CDESC_OFFSET, lp->rx_bd_p);
 	cr = axienet_dma_in32(lp, XAXIDMA_RX_CR_OFFSET);
 	axienet_dma_out32(lp, XAXIDMA_RX_CR_OFFSET,
@@ -273,7 +271,8 @@ static int axienet_dma_bd_init(struct net_device *ndev)
 
 	/* Write to the RS (Run-stop) bit in the Tx channel control register.
 	 * Tx channel is now ready to run. But only after we write to the
-	 * tail pointer register that the Tx channel will start transmitting */
+	 * tail pointer register that the Tx channel will start transmitting.
+	 */
 	axienet_dma_out32(lp, XAXIDMA_TX_CDESC_OFFSET, lp->tx_bd_p);
 	cr = axienet_dma_in32(lp, XAXIDMA_TX_CR_OFFSET);
 	axienet_dma_out32(lp, XAXIDMA_TX_CR_OFFSET,
@@ -320,7 +319,7 @@ static void axienet_set_mac_address(struct net_device *ndev, void *address)
  * @ndev:	Pointer to the net_device structure
  * @p:		6 byte Address to be written as MAC address
  *
- * returns: 0 for all conditions. Presently, there is no failure case.
+ * Return: 0 for all conditions. Presently, there is no failure case.
  *
  * This function is called to initialize the MAC address of the Axi Ethernet
  * core. It calls the core specific axienet_set_mac_address. This is the
@@ -354,7 +353,8 @@ static void axienet_set_multicast_list(struct net_device *ndev)
 	    netdev_mc_count(ndev) > XAE_MULTICAST_CAM_TABLE_NUM) {
 		/* We must make the kernel realize we had to move into
 		 * promiscuous mode. If it was a promiscuous mode request
-		 * the flag is already set. If not we set it. */
+		 * the flag is already set. If not we set it.
+		 */
 		ndev->flags |= IFF_PROMISC;
 		reg = axienet_ior(lp, XAE_FMI_OFFSET);
 		reg |= XAE_FMI_PM_MASK;
@@ -438,14 +438,15 @@ static void __axienet_device_reset(struct axienet_local *lp,
 	/* Reset Axi DMA. This would reset Axi Ethernet core as well. The reset
 	 * process of Axi DMA takes a while to complete as all pending
 	 * commands/transfers will be flushed or completed during this
-	 * reset process. */
+	 * reset process.
+	 */
 	axienet_dma_out32(lp, offset, XAXIDMA_CR_RESET_MASK);
 	timeout = DELAY_OF_ONE_MILLISEC;
 	while (axienet_dma_in32(lp, offset) & XAXIDMA_CR_RESET_MASK) {
 		udelay(1);
 		if (--timeout == 0) {
-			dev_err(dev, "axienet_device_reset DMA "
-				"reset timeout!\n");
+			netdev_err(lp->ndev, "%s: DMA reset timeout!\n",
+				   __func__);
 			break;
 		}
 	}
@@ -471,19 +472,21 @@ static void axienet_device_reset(struct net_device *ndev)
 	__axienet_device_reset(lp, &ndev->dev, XAXIDMA_RX_CR_OFFSET);
 
 	lp->max_frm_size = XAE_MAX_VLAN_FRAME_SIZE;
+	lp->options |= XAE_OPTION_VLAN;
 	lp->options &= (~XAE_OPTION_JUMBO);
 
 	if ((ndev->mtu > XAE_MTU) &&
-	    (ndev->mtu <= XAE_JUMBO_MTU) &&
-	    (lp->jumbo_support)) {
-		lp->max_frm_size = ndev->mtu + XAE_HDR_VLAN_SIZE +
-				   XAE_TRL_SIZE;
-		lp->options |= XAE_OPTION_JUMBO;
+		(ndev->mtu <= XAE_JUMBO_MTU)) {
+		lp->max_frm_size = ndev->mtu + VLAN_ETH_HLEN +
+					XAE_TRL_SIZE;
+
+		if (lp->max_frm_size <= lp->rxmem)
+			lp->options |= XAE_OPTION_JUMBO;
 	}
 
 	if (axienet_dma_bd_init(ndev)) {
-		dev_err(&ndev->dev, "axienet_device_reset descriptor "
-			"allocation failed\n");
+		netdev_err(ndev, "%s: descriptor allocation failed\n",
+			   __func__);
 	}
 
 	axienet_status = axienet_ior(lp, XAE_RCW1_OFFSET);
@@ -497,7 +500,8 @@ static void axienet_device_reset(struct net_device *ndev)
 	axienet_iow(lp, XAE_FCC_OFFSET, XAE_FCC_FCRX_MASK);
 
 	/* Sync default options with HW but leave receiver and
-	 * transmitter disabled.*/
+	 * transmitter disabled.
+	 */
 	axienet_setoptions(ndev, lp->options &
 			   ~(XAE_OPTION_TXEN | XAE_OPTION_RXEN));
 	axienet_set_mac_address(ndev, NULL);
@@ -558,8 +562,8 @@ static void axienet_adjust_link(struct net_device *ndev)
 			lp->last_link = link_state;
 			phy_print_status(phy);
 		} else {
-			dev_err(&ndev->dev, "Error setting Axi Ethernet "
-				"mac speed\n");
+			netdev_err(ndev,
+				   "Error setting Axi Ethernet mac speed\n");
 		}
 	}
 }
@@ -617,7 +621,7 @@ static void axienet_start_xmit_done(struct net_device *ndev)
  * @lp:		Pointer to the axienet_local structure
  * @num_frag:	The number of BDs to check for
  *
- * returns: 0, on success
+ * Return: 0, on success
  *	    NETDEV_TX_BUSY, if any of the descriptors are not free
  *
  * This function is invoked before BDs are allocated and transmission starts.
@@ -640,7 +644,7 @@ static inline int axienet_check_tx_bd_space(struct axienet_local *lp,
  * @skb:	sk_buff pointer that contains data to be Txed.
  * @ndev:	Pointer to net_device structure.
  *
- * returns: NETDEV_TX_OK, on success
+ * Return: NETDEV_TX_OK, on success
  *	    NETDEV_TX_BUSY, if any of the descriptors are not free
  *
  * This function is invoked from upper layers to initiate transmission. The
@@ -726,15 +730,15 @@ static void axienet_recv(struct net_device *ndev)
 	u32 csumstatus;
 	u32 size = 0;
 	u32 packets = 0;
-	dma_addr_t tail_p;
+	dma_addr_t tail_p = 0;
 	struct axienet_local *lp = netdev_priv(ndev);
 	struct sk_buff *skb, *new_skb;
 	struct axidma_bd *cur_p;
 
-	tail_p = lp->rx_bd_p + sizeof(*lp->rx_bd_v) * lp->rx_bd_ci;
 	cur_p = &lp->rx_bd_v[lp->rx_bd_ci];
 
 	while ((cur_p->status & XAXIDMA_BD_STS_COMPLETE_MASK)) {
+		tail_p = lp->rx_bd_p + sizeof(*lp->rx_bd_v) * lp->rx_bd_ci;
 		skb = (struct sk_buff *) (cur_p->sw_id_offset);
 		length = cur_p->app4 & 0x0000FFFF;
 
@@ -786,7 +790,8 @@ static void axienet_recv(struct net_device *ndev)
 	ndev->stats.rx_packets += packets;
 	ndev->stats.rx_bytes += size;
 
-	axienet_dma_out32(lp, XAXIDMA_RX_TDESC_OFFSET, tail_p);
+	if (tail_p)
+		axienet_dma_out32(lp, XAXIDMA_RX_TDESC_OFFSET, tail_p);
 }
 
 /**
@@ -794,7 +799,7 @@ static void axienet_recv(struct net_device *ndev)
  * @irq:	irq number
  * @_ndev:	net_device pointer
  *
- * returns: IRQ_HANDLED for all cases.
+ * Return: IRQ_HANDLED for all cases.
  *
  * This is the Axi DMA Tx done Isr. It invokes "axienet_start_xmit_done"
  * to complete the BD processing.
@@ -808,6 +813,7 @@ static irqreturn_t axienet_tx_irq(int irq, void *_ndev)
 
 	status = axienet_dma_in32(lp, XAXIDMA_TX_SR_OFFSET);
 	if (status & (XAXIDMA_IRQ_IOC_MASK | XAXIDMA_IRQ_DELAY_MASK)) {
+		axienet_dma_out32(lp, XAXIDMA_TX_SR_OFFSET, status);
 		axienet_start_xmit_done(lp->ndev);
 		goto out;
 	}
@@ -831,9 +837,9 @@ static irqreturn_t axienet_tx_irq(int irq, void *_ndev)
 		axienet_dma_out32(lp, XAXIDMA_RX_CR_OFFSET, cr);
 
 		tasklet_schedule(&lp->dma_err_tasklet);
+		axienet_dma_out32(lp, XAXIDMA_TX_SR_OFFSET, status);
 	}
 out:
-	axienet_dma_out32(lp, XAXIDMA_TX_SR_OFFSET, status);
 	return IRQ_HANDLED;
 }
 
@@ -842,7 +848,7 @@ out:
  * @irq:	irq number
  * @_ndev:	net_device pointer
  *
- * returns: IRQ_HANDLED for all cases.
+ * Return: IRQ_HANDLED for all cases.
  *
  * This is the Axi DMA Rx Isr. It invokes "axienet_recv" to complete the BD
  * processing.
@@ -856,6 +862,7 @@ static irqreturn_t axienet_rx_irq(int irq, void *_ndev)
 
 	status = axienet_dma_in32(lp, XAXIDMA_RX_SR_OFFSET);
 	if (status & (XAXIDMA_IRQ_IOC_MASK | XAXIDMA_IRQ_DELAY_MASK)) {
+		axienet_dma_out32(lp, XAXIDMA_RX_SR_OFFSET, status);
 		axienet_recv(lp->ndev);
 		goto out;
 	}
@@ -879,9 +886,9 @@ static irqreturn_t axienet_rx_irq(int irq, void *_ndev)
 		axienet_dma_out32(lp, XAXIDMA_RX_CR_OFFSET, cr);
 
 		tasklet_schedule(&lp->dma_err_tasklet);
+		axienet_dma_out32(lp, XAXIDMA_RX_SR_OFFSET, status);
 	}
 out:
-	axienet_dma_out32(lp, XAXIDMA_RX_SR_OFFSET, status);
 	return IRQ_HANDLED;
 }
 
@@ -891,7 +898,7 @@ static void axienet_dma_err_handler(unsigned long data);
  * axienet_open - Driver open routine.
  * @ndev:	Pointer to net_device structure
  *
- * returns: 0, on success.
+ * Return: 0, on success.
  *	    -ENODEV, if PHY cannot be connected to
  *	    non-zero error value on failure
  *
@@ -914,7 +921,8 @@ static int axienet_open(struct net_device *ndev)
 	/* Disable the MDIO interface till Axi Ethernet Reset is completed.
 	 * When we do an Axi Ethernet reset, it resets the complete core
 	 * including the MDIO. If MDIO is not disabled when the reset
-	 * process is started, MDIO will be broken afterwards. */
+	 * process is started, MDIO will be broken afterwards.
+	 */
 	axienet_iow(lp, XAE_MDIO_MC_OFFSET,
 		    (mdio_mcreg & (~XAE_MDIO_MC_MDIOEN_MASK)));
 	axienet_device_reset(ndev);
@@ -925,14 +933,20 @@ static int axienet_open(struct net_device *ndev)
 		return ret;
 
 	if (lp->phy_node) {
-		lp->phy_dev = of_phy_connect(lp->ndev, lp->phy_node,
+		if (lp->phy_type == XAE_PHY_TYPE_GMII) {
+			lp->phy_dev = of_phy_connect(lp->ndev, lp->phy_node,
 					     axienet_adjust_link, 0,
 					     PHY_INTERFACE_MODE_GMII);
-		if (!lp->phy_dev) {
-			dev_err(lp->dev, "of_phy_connect() failed\n");
-			return -ENODEV;
+		} else if (lp->phy_type == XAE_PHY_TYPE_RGMII_2_0) {
+			lp->phy_dev = of_phy_connect(lp->ndev, lp->phy_node,
+					     axienet_adjust_link, 0,
+					     PHY_INTERFACE_MODE_RGMII_ID);
 		}
-		phy_start(lp->phy_dev);
+
+		if (!lp->phy_dev)
+			dev_err(lp->dev, "of_phy_connect() failed\n");
+		else
+			phy_start(lp->phy_dev);
 	}
 
 	/* Enable tasklets for Axi DMA error handling */
@@ -965,7 +979,7 @@ err_tx_irq:
  * axienet_stop - Driver stop routine.
  * @ndev:	Pointer to net_device structure
  *
- * returns: 0, on success.
+ * Return: 0, on success.
  *
  * This is the driver stop routine. It calls phy_disconnect to stop the PHY
  * device. It also removes the interrupt handlers and disables the interrupts.
@@ -1005,7 +1019,7 @@ static int axienet_stop(struct net_device *ndev)
  * @ndev:	Pointer to net_device structure
  * @new_mtu:	New mtu value to be applied
  *
- * returns: Always returns 0 (success).
+ * Return: Always returns 0 (success).
  *
  * This is the change mtu driver routine. It checks if the Axi Ethernet
  * hardware supports jumbo frames before changing the mtu. This can be
@@ -1017,15 +1031,15 @@ static int axienet_change_mtu(struct net_device *ndev, int new_mtu)
 
 	if (netif_running(ndev))
 		return -EBUSY;
-	if (lp->jumbo_support) {
-		if ((new_mtu > XAE_JUMBO_MTU) || (new_mtu < 64))
-			return -EINVAL;
-		ndev->mtu = new_mtu;
-	} else {
-		if ((new_mtu > XAE_MTU) || (new_mtu < 64))
-			return -EINVAL;
-		ndev->mtu = new_mtu;
-	}
+
+	if ((new_mtu + VLAN_ETH_HLEN +
+		XAE_TRL_SIZE) > lp->rxmem)
+		return -EINVAL;
+
+	if ((new_mtu > XAE_JUMBO_MTU) || (new_mtu < 64))
+		return -EINVAL;
+
+	ndev->mtu = new_mtu;
 
 	return 0;
 }
@@ -1072,6 +1086,8 @@ static const struct net_device_ops axienet_netdev_ops = {
  * not be found, the function returns -ENODEV. This function calls the
  * relevant PHY ethtool API to get the PHY settings.
  * Issue "ethtool ethX" under linux prompt to execute this function.
+ *
+ * Return: 0 on success, -ENODEV if PHY doesn't exist
  */
 static int axienet_ethtools_get_settings(struct net_device *ndev,
 					 struct ethtool_cmd *ecmd)
@@ -1093,6 +1109,8 @@ static int axienet_ethtools_get_settings(struct net_device *ndev,
  * relevant PHY ethtool API to set the PHY.
  * Issue e.g. "ethtool -s ethX speed 1000" under linux prompt to execute this
  * function.
+ *
+ * Return: 0 on success, -ENODEV if PHY doesn't exist
  */
 static int axienet_ethtools_set_settings(struct net_device *ndev,
 					 struct ethtool_cmd *ecmd)
@@ -1127,6 +1145,8 @@ static void axienet_ethtools_get_drvinfo(struct net_device *ndev,
  *
  * This implements ethtool command for getting the total register length
  * information.
+ *
+ * Return: the total regs length
  */
 static int axienet_ethtools_get_regs_len(struct net_device *ndev)
 {
@@ -1213,11 +1233,13 @@ axienet_ethtools_get_pauseparam(struct net_device *ndev,
  * axienet_ethtools_set_pauseparam - Set device pause parameter(flow control)
  *				     settings.
  * @ndev:	Pointer to net_device structure
- * @epauseparam:Pointer to ethtool_pauseparam structure
+ * @epauseparm:Pointer to ethtool_pauseparam structure
  *
  * This implements ethtool command for enabling flow control on Rx and Tx
  * paths. Issue "ethtool -A ethX tx on|off" under linux prompt to execute this
  * function.
+ *
+ * Return: 0 on success, -EFAULT if device is running
  */
 static int
 axienet_ethtools_set_pauseparam(struct net_device *ndev,
@@ -1227,8 +1249,8 @@ axienet_ethtools_set_pauseparam(struct net_device *ndev,
 	struct axienet_local *lp = netdev_priv(ndev);
 
 	if (netif_running(ndev)) {
-		printk(KERN_ERR	"%s: Please stop netif before applying "
-		       "configruation\n", ndev->name);
+		netdev_err(ndev,
+			   "Please stop netif before applying configuration\n");
 		return -EFAULT;
 	}
 
@@ -1254,6 +1276,8 @@ axienet_ethtools_set_pauseparam(struct net_device *ndev,
  * This implements ethtool command for getting the DMA interrupt coalescing
  * count on Tx and Rx paths. Issue "ethtool -c ethX" under linux prompt to
  * execute this function.
+ *
+ * Return: 0 always
  */
 static int axienet_ethtools_get_coalesce(struct net_device *ndev,
 					 struct ethtool_coalesce *ecoalesce)
@@ -1277,6 +1301,8 @@ static int axienet_ethtools_get_coalesce(struct net_device *ndev,
  * This implements ethtool command for setting the DMA interrupt coalescing
  * count on Tx and Rx paths. Issue "ethtool -C ethX rx-frames 5" under linux
  * prompt to execute this function.
+ *
+ * Return: 0, on success, Non-zero error value on failure.
  */
 static int axienet_ethtools_set_coalesce(struct net_device *ndev,
 					 struct ethtool_coalesce *ecoalesce)
@@ -1284,8 +1310,8 @@ static int axienet_ethtools_set_coalesce(struct net_device *ndev,
 	struct axienet_local *lp = netdev_priv(ndev);
 
 	if (netif_running(ndev)) {
-		printk(KERN_ERR	"%s: Please stop netif before applying "
-		       "configruation\n", ndev->name);
+		netdev_err(ndev,
+			   "Please stop netif before applying configuration\n");
 		return -EFAULT;
 	}
 
@@ -1354,7 +1380,8 @@ static void axienet_dma_err_handler(unsigned long data)
 	/* Disable the MDIO interface till Axi Ethernet Reset is completed.
 	 * When we do an Axi Ethernet reset, it resets the complete core
 	 * including the MDIO. So if MDIO is not disabled when the reset
-	 * process is started, MDIO will be broken afterwards. */
+	 * process is started, MDIO will be broken afterwards.
+	 */
 	axienet_iow(lp, XAE_MDIO_MC_OFFSET, (mdio_mcreg &
 		    ~XAE_MDIO_MC_MDIOEN_MASK));
 
@@ -1425,7 +1452,8 @@ static void axienet_dma_err_handler(unsigned long data)
 	axienet_dma_out32(lp, XAXIDMA_TX_CR_OFFSET, cr);
 
 	/* Populate the tail pointer and bring the Rx Axi DMA engine out of
-	 * halted state. This will make the Rx side ready for reception.*/
+	 * halted state. This will make the Rx side ready for reception.
+	 */
 	axienet_dma_out32(lp, XAXIDMA_RX_CDESC_OFFSET, lp->rx_bd_p);
 	cr = axienet_dma_in32(lp, XAXIDMA_RX_CR_OFFSET);
 	axienet_dma_out32(lp, XAXIDMA_RX_CR_OFFSET,
@@ -1435,7 +1463,8 @@ static void axienet_dma_err_handler(unsigned long data)
 
 	/* Write to the RS (Run-stop) bit in the Tx channel control register.
 	 * Tx channel is now ready to run. But only after we write to the
-	 * tail pointer register that the Tx channel will start transmitting */
+	 * tail pointer register that the Tx channel will start transmitting
+	 */
 	axienet_dma_out32(lp, XAXIDMA_TX_CDESC_OFFSET, lp->tx_bd_p);
 	cr = axienet_dma_in32(lp, XAXIDMA_TX_CR_OFFSET);
 	axienet_dma_out32(lp, XAXIDMA_TX_CR_OFFSET,
@@ -1451,7 +1480,8 @@ static void axienet_dma_err_handler(unsigned long data)
 	axienet_iow(lp, XAE_FCC_OFFSET, XAE_FCC_FCRX_MASK);
 
 	/* Sync default options with HW but leave receiver and
-	 * transmitter disabled.*/
+	 * transmitter disabled.
+	 */
 	axienet_setoptions(ndev, lp->options &
 			   ~(XAE_OPTION_TXEN | XAE_OPTION_RXEN));
 	axienet_set_mac_address(ndev, NULL);
@@ -1460,11 +1490,10 @@ static void axienet_dma_err_handler(unsigned long data)
 }
 
 /**
- * axienet_of_probe - Axi Ethernet probe function.
- * @op:		Pointer to platform device structure.
- * @match:	Pointer to device id structure
+ * axienet_probe - Axi Ethernet probe function.
+ * @pdev:	Pointer to platform device structure.
  *
- * returns: 0, on success
+ * Return: 0, on success
  *	    Non-zero error value on failure.
  *
  * This is the probe routine for Axi Ethernet driver. This is called before
@@ -1472,22 +1501,23 @@ static void axienet_dma_err_handler(unsigned long data)
  * device. Parses through device tree and populates fields of
  * axienet_local. It registers the Ethernet device.
  */
-static int axienet_of_probe(struct platform_device *op)
+static int axienet_probe(struct platform_device *pdev)
 {
-	__be32 *p;
-	int size, ret = 0;
+	int ret;
 	struct device_node *np;
 	struct axienet_local *lp;
 	struct net_device *ndev;
-	const void *addr;
+	u8 mac_addr[6];
+	struct resource *ethres, dmares;
+	u32 value;
 
 	ndev = alloc_etherdev(sizeof(*lp));
 	if (!ndev)
 		return -ENOMEM;
 
-	platform_set_drvdata(op, ndev);
+	platform_set_drvdata(pdev, ndev);
 
-	SET_NETDEV_DEV(ndev, &op->dev);
+	SET_NETDEV_DEV(ndev, &pdev->dev);
 	ndev->flags &= ~IFF_MULTICAST;  /* clear multicast */
 	ndev->features = NETIF_F_SG;
 	ndev->netdev_ops = &axienet_netdev_ops;
@@ -1495,21 +1525,23 @@ static int axienet_of_probe(struct platform_device *op)
 
 	lp = netdev_priv(ndev);
 	lp->ndev = ndev;
-	lp->dev = &op->dev;
+	lp->dev = &pdev->dev;
 	lp->options = XAE_OPTION_DEFAULTS;
 	/* Map device registers */
-	lp->regs = of_iomap(op->dev.of_node, 0);
+	ethres = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	lp->regs = devm_ioremap_resource(&pdev->dev, ethres);
 	if (!lp->regs) {
-		dev_err(&op->dev, "could not map Axi Ethernet regs.\n");
+		dev_err(&pdev->dev, "could not map Axi Ethernet regs.\n");
 		ret = -ENOMEM;
-		goto nodev;
+		goto free_netdev;
 	}
+
 	/* Setup checksum offload, but default to off if not specified */
 	lp->features = 0;
 
-	p = (__be32 *) of_get_property(op->dev.of_node, "xlnx,txcsum", NULL);
-	if (p) {
-		switch (be32_to_cpup(p)) {
+	ret = of_property_read_u32(pdev->dev.of_node, "xlnx,txcsum", &value);
+	if (!ret) {
+		switch (value) {
 		case 1:
 			lp->csum_offload_on_tx_path =
 				XAE_FEATURE_PARTIAL_TX_CSUM;
@@ -1528,9 +1560,9 @@ static int axienet_of_probe(struct platform_device *op)
 			lp->csum_offload_on_tx_path = XAE_NO_CSUM_OFFLOAD;
 		}
 	}
-	p = (__be32 *) of_get_property(op->dev.of_node, "xlnx,rxcsum", NULL);
-	if (p) {
-		switch (be32_to_cpup(p)) {
+	ret = of_property_read_u32(pdev->dev.of_node, "xlnx,rxcsum", &value);
+	if (!ret) {
+		switch (value) {
 		case 1:
 			lp->csum_offload_on_rx_path =
 				XAE_FEATURE_PARTIAL_RX_CSUM;
@@ -1546,82 +1578,77 @@ static int axienet_of_probe(struct platform_device *op)
 		}
 	}
 	/* For supporting jumbo frames, the Axi Ethernet hardware must have
-	 * a larger Rx/Tx Memory. Typically, the size must be more than or
-	 * equal to 16384 bytes, so that we can enable jumbo option and start
-	 * supporting jumbo frames. Here we check for memory allocated for
-	 * Rx/Tx in the hardware from the device-tree and accordingly set
-	 * flags. */
-	p = (__be32 *) of_get_property(op->dev.of_node, "xlnx,rxmem", NULL);
-	if (p) {
-		if ((be32_to_cpup(p)) >= 0x4000)
-			lp->jumbo_support = 1;
-	}
-	p = (__be32 *) of_get_property(op->dev.of_node, "xlnx,phy-type", NULL);
-	if (p)
-		lp->phy_type = be32_to_cpup(p);
+	 * a larger Rx/Tx Memory. Typically, the size must be large so that
+	 * we can enable jumbo option and start supporting jumbo frames.
+	 * Here we check for memory allocated for Rx/Tx in the hardware from
+	 * the device-tree and accordingly set flags.
+	 */
+	of_property_read_u32(pdev->dev.of_node, "xlnx,rxmem", &lp->rxmem);
+	of_property_read_u32(pdev->dev.of_node, "xlnx,phy-type", &lp->phy_type);
 
 	/* Find the DMA node, map the DMA registers, and decode the DMA IRQs */
-	np = of_parse_phandle(op->dev.of_node, "axistream-connected", 0);
-	if (!np) {
-		dev_err(&op->dev, "could not find DMA node\n");
-		ret = -ENODEV;
-		goto err_iounmap;
+	np = of_parse_phandle(pdev->dev.of_node, "axistream-connected", 0);
+	if (IS_ERR(np)) {
+		dev_err(&pdev->dev, "could not find DMA node\n");
+		ret = PTR_ERR(np);
+		goto free_netdev;
 	}
-	lp->dma_regs = of_iomap(np, 0);
-	if (lp->dma_regs) {
-		dev_dbg(&op->dev, "MEM base: %p\n", lp->dma_regs);
-	} else {
-		dev_err(&op->dev, "unable to map DMA registers\n");
-		of_node_put(np);
+	ret = of_address_to_resource(np, 0, &dmares);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to get DMA resource\n");
+		goto free_netdev;
+	}
+	lp->dma_regs = devm_ioremap_resource(&pdev->dev, &dmares);
+	if (!lp->dma_regs) {
+		dev_err(&pdev->dev, "could not map DMA regs\n");
+		ret = -ENOMEM;
+		goto free_netdev;
 	}
 	lp->rx_irq = irq_of_parse_and_map(np, 1);
 	lp->tx_irq = irq_of_parse_and_map(np, 0);
 	of_node_put(np);
 	if ((lp->rx_irq <= 0) || (lp->tx_irq <= 0)) {
-		dev_err(&op->dev, "could not determine irqs\n");
+		dev_err(&pdev->dev, "could not determine irqs\n");
 		ret = -ENOMEM;
-		goto err_iounmap_2;
+		goto free_netdev;
 	}
 
 	/* Retrieve the MAC address */
-	addr = of_get_property(op->dev.of_node, "local-mac-address", &size);
-	if ((!addr) || (size != 6)) {
-		dev_err(&op->dev, "could not find MAC address\n");
-		ret = -ENODEV;
-		goto err_iounmap_2;
+	ret = of_property_read_u8_array(pdev->dev.of_node,
+					"local-mac-address", mac_addr, 6);
+	if (ret) {
+		dev_err(&pdev->dev, "could not find MAC address\n");
+		goto free_netdev;
 	}
-	axienet_set_mac_address(ndev, (void *) addr);
+	axienet_set_mac_address(ndev, (void *)mac_addr);
 
 	lp->coalesce_count_rx = XAXIDMA_DFT_RX_THRESHOLD;
 	lp->coalesce_count_tx = XAXIDMA_DFT_TX_THRESHOLD;
 
-	lp->phy_node = of_parse_phandle(op->dev.of_node, "phy-handle", 0);
-	ret = axienet_mdio_setup(lp, op->dev.of_node);
-	if (ret)
-		dev_warn(&op->dev, "error registering MDIO bus\n");
+	lp->phy_node = of_parse_phandle(pdev->dev.of_node, "phy-handle", 0);
+	if (lp->phy_node) {
+		ret = axienet_mdio_setup(lp, pdev->dev.of_node);
+		if (ret)
+			dev_warn(&pdev->dev, "error registering MDIO bus\n");
+	}
 
 	ret = register_netdev(lp->ndev);
 	if (ret) {
 		dev_err(lp->dev, "register_netdev() error (%i)\n", ret);
-		goto err_iounmap_2;
+		goto free_netdev;
 	}
 
 	return 0;
 
-err_iounmap_2:
-	if (lp->dma_regs)
-		iounmap(lp->dma_regs);
-err_iounmap:
-	iounmap(lp->regs);
-nodev:
+free_netdev:
 	free_netdev(ndev);
-	ndev = NULL;
+
 	return ret;
 }
 
-static int axienet_of_remove(struct platform_device *op)
+static int axienet_remove(struct platform_device *pdev)
 {
-	struct net_device *ndev = platform_get_drvdata(op);
+	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct axienet_local *lp = netdev_priv(ndev);
 
 	axienet_mdio_teardown(lp);
@@ -1630,24 +1657,21 @@ static int axienet_of_remove(struct platform_device *op)
 	of_node_put(lp->phy_node);
 	lp->phy_node = NULL;
 
-	iounmap(lp->regs);
-	if (lp->dma_regs)
-		iounmap(lp->dma_regs);
 	free_netdev(ndev);
 
 	return 0;
 }
 
-static struct platform_driver axienet_of_driver = {
-	.probe = axienet_of_probe,
-	.remove = axienet_of_remove,
+static struct platform_driver axienet_driver = {
+	.probe = axienet_probe,
+	.remove = axienet_remove,
 	.driver = {
 		 .name = "xilinx_axienet",
 		 .of_match_table = axienet_of_match,
 	},
 };
 
-module_platform_driver(axienet_of_driver);
+module_platform_driver(axienet_driver);
 
 MODULE_DESCRIPTION("Xilinx Axi Ethernet driver");
 MODULE_AUTHOR("Xilinx");
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet_mdio.c b/drivers/net/ethernet/xilinx/xilinx_axienet_mdio.c
index 3b67d60d4378..2a5a16834c01 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet_mdio.c
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet_mdio.c
@@ -37,7 +37,7 @@ int axienet_mdio_wait_until_ready(struct axienet_local *lp)
  * @phy_id:	Address of the PHY device
  * @reg:	PHY register to read
  *
- * returns:	The register contents on success, -ETIMEDOUT on a timeout
+ * Return:	The register contents on success, -ETIMEDOUT on a timeout
  *
  * Reads the contents of the requested register from the requested PHY
  * address by first writing the details into MCR register. After a while
@@ -80,7 +80,7 @@ static int axienet_mdio_read(struct mii_bus *bus, int phy_id, int reg)
  * @reg:	PHY register to write to
  * @val:	Value to be written into the register
  *
- * returns:	0 on success, -ETIMEDOUT on a timeout
+ * Return:	0 on success, -ETIMEDOUT on a timeout
  *
  * Writes the value to the requested register by first writing the value
  * into MWD register. The the MCR register is then appropriately setup
@@ -119,7 +119,7 @@ static int axienet_mdio_write(struct mii_bus *bus, int phy_id, int reg,
  * @lp:		Pointer to axienet local data structure.
  * @np:		Pointer to device node
  *
- * returns:	0 on success, -ETIMEDOUT on a timeout, -ENOMEM when
+ * Return:	0 on success, -ETIMEDOUT on a timeout, -ENOMEM when
  *		mdiobus_alloc (to allocate memory for mii bus structure) fails.
  *
  * Sets up the MDIO interface by initializing the MDIO clock and enabling the
@@ -161,19 +161,19 @@ int axienet_mdio_setup(struct axienet_local *lp, struct device_node *np)
 
 	np1 = of_find_node_by_name(NULL, "cpu");
 	if (!np1) {
-		printk(KERN_WARNING "%s(): Could not find CPU device node.",
-		       __func__);
-		printk(KERN_WARNING "Setting MDIO clock divisor to "
-		       "default %d\n", DEFAULT_CLOCK_DIVISOR);
+		netdev_warn(lp->ndev, "Could not find CPU device node.\n");
+		netdev_warn(lp->ndev,
+			    "Setting MDIO clock divisor to default %d\n",
+			    DEFAULT_CLOCK_DIVISOR);
 		clk_div = DEFAULT_CLOCK_DIVISOR;
 		goto issue;
 	}
 	property_p = (u32 *) of_get_property(np1, "clock-frequency", NULL);
 	if (!property_p) {
-		printk(KERN_WARNING "%s(): Could not find CPU property: "
-		       "clock-frequency.", __func__);
-		printk(KERN_WARNING "Setting MDIO clock divisor to "
-		       "default %d\n", DEFAULT_CLOCK_DIVISOR);
+		netdev_warn(lp->ndev, "clock-frequency property not found.\n");
+		netdev_warn(lp->ndev,
+			    "Setting MDIO clock divisor to default %d\n",
+			    DEFAULT_CLOCK_DIVISOR);
 		clk_div = DEFAULT_CLOCK_DIVISOR;
 		of_node_put(np1);
 		goto issue;
@@ -183,12 +183,14 @@ int axienet_mdio_setup(struct axienet_local *lp, struct device_node *np)
 	clk_div = (host_clock / (MAX_MDIO_FREQ * 2)) - 1;
 	/* If there is any remainder from the division of
 	 * fHOST / (MAX_MDIO_FREQ * 2), then we need to add
-	 * 1 to the clock divisor or we will surely be above 2.5 MHz */
+	 * 1 to the clock divisor or we will surely be above 2.5 MHz
+	 */
 	if (host_clock % (MAX_MDIO_FREQ * 2))
 		clk_div++;
 
-	printk(KERN_DEBUG "%s(): Setting MDIO clock divisor to %u based "
-	       "on %u Hz host clock.\n", __func__, clk_div, host_clock);
+	netdev_dbg(lp->ndev,
+		   "Setting MDIO clock divisor to %u/%u Hz host clock.\n",
+		   clk_div, host_clock);
 
 	of_node_put(np1);
 issue:
diff --git a/drivers/net/ethernet/xircom/Kconfig b/drivers/net/ethernet/xircom/Kconfig
index 69f56a6de821..d6208a4c9866 100644
--- a/drivers/net/ethernet/xircom/Kconfig
+++ b/drivers/net/ethernet/xircom/Kconfig
@@ -7,9 +7,7 @@ config NET_VENDOR_XIRCOM
 	default y
 	depends on PCMCIA
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question doesn't directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/ethernet/xscale/Kconfig b/drivers/net/ethernet/xscale/Kconfig
index b81bc9fca378..af3432fe9a5e 100644
--- a/drivers/net/ethernet/xscale/Kconfig
+++ b/drivers/net/ethernet/xscale/Kconfig
@@ -8,9 +8,7 @@ config NET_VENDOR_XSCALE
 	depends on NET_VENDOR_INTEL && (ARM && ARCH_IXP4XX && \
 		   IXP4XX_NPE && IXP4XX_QMGR)
 	---help---
-	  If you have a network (Ethernet) card belonging to this class, say Y
-	  and read the Ethernet-HOWTO, available from
-	  <http://www.tldp.org/docs.html#howto>.
+	  If you have a network (Ethernet) card belonging to this class, say Y.
 
 	  Note that the answer to this question does not directly affect the
 	  kernel: saying N will just cause the configurator to skip all
diff --git a/drivers/net/fddi/skfp/srf.c b/drivers/net/fddi/skfp/srf.c
index cc27dea3414e..9956680402de 100644
--- a/drivers/net/fddi/skfp/srf.c
+++ b/drivers/net/fddi/skfp/srf.c
@@ -414,7 +414,7 @@ static void smt_send_srf(struct s_smc *smc)
 	smt->smt_len = SMT_MAX_INFO_LEN - pcon.pc_len ;
 	mb->sm_len = smt->smt_len + sizeof(struct smt_header) ;
 
-	DB_SMT("SRF: sending SRF at %x, len %d\n",smt,mb->sm_len) ;
+	DB_SMT("SRF: sending SRF at %p, len %d\n",smt,mb->sm_len) ;
 	DB_SMT("SRF: state SR%d Threshold %d\n",
 		smc->srf.sr_state,smc->srf.SRThreshold/TICKS_PER_SECOND) ;
 #ifdef	DEBUG
diff --git a/drivers/net/geneve.c b/drivers/net/geneve.c
new file mode 100644
index 000000000000..78d49d186e05
--- /dev/null
+++ b/drivers/net/geneve.c
@@ -0,0 +1,523 @@
+/*
+ * GENEVE: Generic Network Virtualization Encapsulation
+ *
+ * Copyright (c) 2015 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/hash.h>
+#include <net/rtnetlink.h>
+#include <net/geneve.h>
+
+#define GENEVE_NETDEV_VER	"0.6"
+
+#define GENEVE_UDP_PORT		6081
+
+#define GENEVE_N_VID		(1u << 24)
+#define GENEVE_VID_MASK		(GENEVE_N_VID - 1)
+
+#define VNI_HASH_BITS		10
+#define VNI_HASH_SIZE		(1<<VNI_HASH_BITS)
+
+static bool log_ecn_error = true;
+module_param(log_ecn_error, bool, 0644);
+MODULE_PARM_DESC(log_ecn_error, "Log packets received with corrupted ECN");
+
+/* per-network namespace private data for this module */
+struct geneve_net {
+	struct list_head  geneve_list;
+	struct hlist_head vni_list[VNI_HASH_SIZE];
+};
+
+/* Pseudo network device */
+struct geneve_dev {
+	struct hlist_node  hlist;	/* vni hash table */
+	struct net	   *net;	/* netns for packet i/o */
+	struct net_device  *dev;	/* netdev for geneve tunnel */
+	struct geneve_sock *sock;	/* socket used for geneve tunnel */
+	u8                 vni[3];	/* virtual network ID for tunnel */
+	u8                 ttl;		/* TTL override */
+	u8                 tos;		/* TOS override */
+	struct sockaddr_in remote;	/* IPv4 address for link partner */
+	struct list_head   next;	/* geneve's per namespace list */
+};
+
+static int geneve_net_id;
+
+static inline __u32 geneve_net_vni_hash(u8 vni[3])
+{
+	__u32 vnid;
+
+	vnid = (vni[0] << 16) | (vni[1] << 8) | vni[2];
+	return hash_32(vnid, VNI_HASH_BITS);
+}
+
+/* geneve receive/decap routine */
+static void geneve_rx(struct geneve_sock *gs, struct sk_buff *skb)
+{
+	struct genevehdr *gnvh = geneve_hdr(skb);
+	struct geneve_dev *dummy, *geneve = NULL;
+	struct geneve_net *gn;
+	struct iphdr *iph = NULL;
+	struct pcpu_sw_netstats *stats;
+	struct hlist_head *vni_list_head;
+	int err = 0;
+	__u32 hash;
+
+	iph = ip_hdr(skb); /* Still outer IP header... */
+
+	gn = gs->rcv_data;
+
+	/* Find the device for this VNI */
+	hash = geneve_net_vni_hash(gnvh->vni);
+	vni_list_head = &gn->vni_list[hash];
+	hlist_for_each_entry_rcu(dummy, vni_list_head, hlist) {
+		if (!memcmp(gnvh->vni, dummy->vni, sizeof(dummy->vni)) &&
+		    iph->saddr == dummy->remote.sin_addr.s_addr) {
+			geneve = dummy;
+			break;
+		}
+	}
+	if (!geneve)
+		goto drop;
+
+	/* Drop packets w/ critical options,
+	 * since we don't support any...
+	 */
+	if (gnvh->critical)
+		goto drop;
+
+	skb_reset_mac_header(skb);
+	skb_scrub_packet(skb, !net_eq(geneve->net, dev_net(geneve->dev)));
+	skb->protocol = eth_type_trans(skb, geneve->dev);
+	skb_postpull_rcsum(skb, eth_hdr(skb), ETH_HLEN);
+
+	/* Ignore packet loops (and multicast echo) */
+	if (ether_addr_equal(eth_hdr(skb)->h_source, geneve->dev->dev_addr))
+		goto drop;
+
+	skb_reset_network_header(skb);
+
+	iph = ip_hdr(skb); /* Now inner IP header... */
+	err = IP_ECN_decapsulate(iph, skb);
+
+	if (unlikely(err)) {
+		if (log_ecn_error)
+			net_info_ratelimited("non-ECT from %pI4 with TOS=%#x\n",
+					     &iph->saddr, iph->tos);
+		if (err > 1) {
+			++geneve->dev->stats.rx_frame_errors;
+			++geneve->dev->stats.rx_errors;
+			goto drop;
+		}
+	}
+
+	stats = this_cpu_ptr(geneve->dev->tstats);
+	u64_stats_update_begin(&stats->syncp);
+	stats->rx_packets++;
+	stats->rx_bytes += skb->len;
+	u64_stats_update_end(&stats->syncp);
+
+	netif_rx(skb);
+
+	return;
+drop:
+	/* Consume bad packet */
+	kfree_skb(skb);
+}
+
+/* Setup stats when device is created */
+static int geneve_init(struct net_device *dev)
+{
+	dev->tstats = netdev_alloc_pcpu_stats(struct pcpu_sw_netstats);
+	if (!dev->tstats)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void geneve_uninit(struct net_device *dev)
+{
+	free_percpu(dev->tstats);
+}
+
+static int geneve_open(struct net_device *dev)
+{
+	struct geneve_dev *geneve = netdev_priv(dev);
+	struct net *net = geneve->net;
+	struct geneve_net *gn = net_generic(geneve->net, geneve_net_id);
+	struct geneve_sock *gs;
+
+	gs = geneve_sock_add(net, htons(GENEVE_UDP_PORT), geneve_rx, gn,
+	                     false, false);
+	if (IS_ERR(gs))
+		return PTR_ERR(gs);
+
+	geneve->sock = gs;
+
+	return 0;
+}
+
+static int geneve_stop(struct net_device *dev)
+{
+	struct geneve_dev *geneve = netdev_priv(dev);
+	struct geneve_sock *gs = geneve->sock;
+
+	geneve_sock_release(gs);
+
+	return 0;
+}
+
+static netdev_tx_t geneve_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct geneve_dev *geneve = netdev_priv(dev);
+	struct geneve_sock *gs = geneve->sock;
+	struct rtable *rt = NULL;
+	const struct iphdr *iip; /* interior IP header */
+	struct flowi4 fl4;
+	int err;
+	__be16 sport;
+	__u8 tos, ttl;
+
+	iip = ip_hdr(skb);
+
+	skb_reset_mac_header(skb);
+
+	/* TODO: port min/max limits should be configurable */
+	sport = udp_flow_src_port(dev_net(dev), skb, 0, 0, true);
+
+	tos = geneve->tos;
+	if (tos == 1)
+		tos = ip_tunnel_get_dsfield(iip, skb);
+
+	memset(&fl4, 0, sizeof(fl4));
+	fl4.flowi4_tos = RT_TOS(tos);
+	fl4.daddr = geneve->remote.sin_addr.s_addr;
+	rt = ip_route_output_key(geneve->net, &fl4);
+	if (IS_ERR(rt)) {
+		netdev_dbg(dev, "no route to %pI4\n", &fl4.daddr);
+		dev->stats.tx_carrier_errors++;
+		goto tx_error;
+	}
+	if (rt->dst.dev == dev) { /* is this necessary? */
+		netdev_dbg(dev, "circular route to %pI4\n", &fl4.daddr);
+		dev->stats.collisions++;
+		goto rt_tx_error;
+	}
+
+	tos = ip_tunnel_ecn_encap(tos, iip, skb);
+
+	ttl = geneve->ttl;
+	if (!ttl && IN_MULTICAST(ntohl(fl4.daddr)))
+		ttl = 1;
+
+	ttl = ttl ? : ip4_dst_hoplimit(&rt->dst);
+
+	/* no need to handle local destination and encap bypass...yet... */
+
+	err = geneve_xmit_skb(gs, rt, skb, fl4.saddr, fl4.daddr,
+	                      tos, ttl, 0, sport, htons(GENEVE_UDP_PORT), 0,
+	                      geneve->vni, 0, NULL, false,
+	                      !net_eq(geneve->net, dev_net(geneve->dev)));
+	if (err < 0)
+		ip_rt_put(rt);
+
+	iptunnel_xmit_stats(err, &dev->stats, dev->tstats);
+
+	return NETDEV_TX_OK;
+
+rt_tx_error:
+	ip_rt_put(rt);
+tx_error:
+	dev->stats.tx_errors++;
+	dev_kfree_skb(skb);
+	return NETDEV_TX_OK;
+}
+
+static const struct net_device_ops geneve_netdev_ops = {
+	.ndo_init		= geneve_init,
+	.ndo_uninit		= geneve_uninit,
+	.ndo_open		= geneve_open,
+	.ndo_stop		= geneve_stop,
+	.ndo_start_xmit		= geneve_xmit,
+	.ndo_get_stats64	= ip_tunnel_get_stats64,
+	.ndo_change_mtu		= eth_change_mtu,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_set_mac_address	= eth_mac_addr,
+};
+
+static void geneve_get_drvinfo(struct net_device *dev,
+			       struct ethtool_drvinfo *drvinfo)
+{
+	strlcpy(drvinfo->version, GENEVE_NETDEV_VER, sizeof(drvinfo->version));
+	strlcpy(drvinfo->driver, "geneve", sizeof(drvinfo->driver));
+}
+
+static const struct ethtool_ops geneve_ethtool_ops = {
+	.get_drvinfo	= geneve_get_drvinfo,
+	.get_link	= ethtool_op_get_link,
+};
+
+/* Info for udev, that this is a virtual tunnel endpoint */
+static struct device_type geneve_type = {
+	.name = "geneve",
+};
+
+/* Initialize the device structure. */
+static void geneve_setup(struct net_device *dev)
+{
+	ether_setup(dev);
+
+	dev->netdev_ops = &geneve_netdev_ops;
+	dev->ethtool_ops = &geneve_ethtool_ops;
+	dev->destructor = free_netdev;
+
+	SET_NETDEV_DEVTYPE(dev, &geneve_type);
+
+	dev->tx_queue_len = 0;
+	dev->features    |= NETIF_F_LLTX;
+	dev->features    |= NETIF_F_SG | NETIF_F_HW_CSUM;
+	dev->features    |= NETIF_F_RXCSUM;
+	dev->features    |= NETIF_F_GSO_SOFTWARE;
+
+	dev->vlan_features = dev->features;
+	dev->features    |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_STAG_TX;
+
+	dev->hw_features |= NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_RXCSUM;
+	dev->hw_features |= NETIF_F_GSO_SOFTWARE;
+	dev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_STAG_TX;
+
+	netif_keep_dst(dev);
+	dev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
+}
+
+static const struct nla_policy geneve_policy[IFLA_GENEVE_MAX + 1] = {
+	[IFLA_GENEVE_ID]		= { .type = NLA_U32 },
+	[IFLA_GENEVE_REMOTE]		= { .len = FIELD_SIZEOF(struct iphdr, daddr) },
+	[IFLA_GENEVE_TTL]		= { .type = NLA_U8 },
+	[IFLA_GENEVE_TOS]		= { .type = NLA_U8 },
+};
+
+static int geneve_validate(struct nlattr *tb[], struct nlattr *data[])
+{
+	if (tb[IFLA_ADDRESS]) {
+		if (nla_len(tb[IFLA_ADDRESS]) != ETH_ALEN)
+			return -EINVAL;
+
+		if (!is_valid_ether_addr(nla_data(tb[IFLA_ADDRESS])))
+			return -EADDRNOTAVAIL;
+	}
+
+	if (!data)
+		return -EINVAL;
+
+	if (data[IFLA_GENEVE_ID]) {
+		__u32 vni =  nla_get_u32(data[IFLA_GENEVE_ID]);
+
+		if (vni >= GENEVE_VID_MASK)
+			return -ERANGE;
+	}
+
+	return 0;
+}
+
+static int geneve_newlink(struct net *net, struct net_device *dev,
+			 struct nlattr *tb[], struct nlattr *data[])
+{
+	struct geneve_net *gn = net_generic(net, geneve_net_id);
+	struct geneve_dev *dummy, *geneve = netdev_priv(dev);
+	struct hlist_head *vni_list_head;
+	struct sockaddr_in remote;	/* IPv4 address for link partner */
+	__u32 vni, hash;
+	int err;
+
+	if (!data[IFLA_GENEVE_ID] || !data[IFLA_GENEVE_REMOTE])
+		return -EINVAL;
+
+	geneve->net = net;
+	geneve->dev = dev;
+
+	vni = nla_get_u32(data[IFLA_GENEVE_ID]);
+	geneve->vni[0] = (vni & 0x00ff0000) >> 16;
+	geneve->vni[1] = (vni & 0x0000ff00) >> 8;
+	geneve->vni[2] =  vni & 0x000000ff;
+
+	geneve->remote.sin_addr.s_addr =
+		nla_get_in_addr(data[IFLA_GENEVE_REMOTE]);
+	if (IN_MULTICAST(ntohl(geneve->remote.sin_addr.s_addr)))
+		return -EINVAL;
+
+	remote = geneve->remote;
+	hash = geneve_net_vni_hash(geneve->vni);
+	vni_list_head = &gn->vni_list[hash];
+	hlist_for_each_entry_rcu(dummy, vni_list_head, hlist) {
+		if (!memcmp(geneve->vni, dummy->vni, sizeof(dummy->vni)) &&
+		    !memcmp(&remote, &dummy->remote, sizeof(dummy->remote)))
+			return -EBUSY;
+	}
+
+	if (tb[IFLA_ADDRESS] == NULL)
+		eth_hw_addr_random(dev);
+
+	err = register_netdevice(dev);
+	if (err)
+		return err;
+
+	if (data[IFLA_GENEVE_TTL])
+		geneve->ttl = nla_get_u8(data[IFLA_GENEVE_TTL]);
+
+	if (data[IFLA_GENEVE_TOS])
+		geneve->tos = nla_get_u8(data[IFLA_GENEVE_TOS]);
+
+	list_add(&geneve->next, &gn->geneve_list);
+
+	hlist_add_head_rcu(&geneve->hlist, &gn->vni_list[hash]);
+
+	return 0;
+}
+
+static void geneve_dellink(struct net_device *dev, struct list_head *head)
+{
+	struct geneve_dev *geneve = netdev_priv(dev);
+
+	if (!hlist_unhashed(&geneve->hlist))
+		hlist_del_rcu(&geneve->hlist);
+
+	list_del(&geneve->next);
+	unregister_netdevice_queue(dev, head);
+}
+
+static size_t geneve_get_size(const struct net_device *dev)
+{
+	return nla_total_size(sizeof(__u32)) +	/* IFLA_GENEVE_ID */
+		nla_total_size(sizeof(struct in_addr)) + /* IFLA_GENEVE_REMOTE */
+		nla_total_size(sizeof(__u8)) +  /* IFLA_GENEVE_TTL */
+		nla_total_size(sizeof(__u8)) +  /* IFLA_GENEVE_TOS */
+		0;
+}
+
+static int geneve_fill_info(struct sk_buff *skb, const struct net_device *dev)
+{
+	struct geneve_dev *geneve = netdev_priv(dev);
+	__u32 vni;
+
+	vni = (geneve->vni[0] << 16) | (geneve->vni[1] << 8) | geneve->vni[2];
+	if (nla_put_u32(skb, IFLA_GENEVE_ID, vni))
+		goto nla_put_failure;
+
+	if (nla_put_in_addr(skb, IFLA_GENEVE_REMOTE,
+			    geneve->remote.sin_addr.s_addr))
+		goto nla_put_failure;
+
+	if (nla_put_u8(skb, IFLA_GENEVE_TTL, geneve->ttl) ||
+	    nla_put_u8(skb, IFLA_GENEVE_TOS, geneve->tos))
+		goto nla_put_failure;
+
+	return 0;
+
+nla_put_failure:
+	return -EMSGSIZE;
+}
+
+static struct rtnl_link_ops geneve_link_ops __read_mostly = {
+	.kind		= "geneve",
+	.maxtype	= IFLA_GENEVE_MAX,
+	.policy		= geneve_policy,
+	.priv_size	= sizeof(struct geneve_dev),
+	.setup		= geneve_setup,
+	.validate	= geneve_validate,
+	.newlink	= geneve_newlink,
+	.dellink	= geneve_dellink,
+	.get_size	= geneve_get_size,
+	.fill_info	= geneve_fill_info,
+};
+
+static __net_init int geneve_init_net(struct net *net)
+{
+	struct geneve_net *gn = net_generic(net, geneve_net_id);
+	unsigned int h;
+
+	INIT_LIST_HEAD(&gn->geneve_list);
+
+	for (h = 0; h < VNI_HASH_SIZE; ++h)
+		INIT_HLIST_HEAD(&gn->vni_list[h]);
+
+	return 0;
+}
+
+static void __net_exit geneve_exit_net(struct net *net)
+{
+	struct geneve_net *gn = net_generic(net, geneve_net_id);
+	struct geneve_dev *geneve, *next;
+	struct net_device *dev, *aux;
+	LIST_HEAD(list);
+
+	rtnl_lock();
+
+	/* gather any geneve devices that were moved into this ns */
+	for_each_netdev_safe(net, dev, aux)
+		if (dev->rtnl_link_ops == &geneve_link_ops)
+			unregister_netdevice_queue(dev, &list);
+
+	/* now gather any other geneve devices that were created in this ns */
+	list_for_each_entry_safe(geneve, next, &gn->geneve_list, next) {
+		/* If geneve->dev is in the same netns, it was already added
+		 * to the list by the previous loop.
+		 */
+		if (!net_eq(dev_net(geneve->dev), net))
+			unregister_netdevice_queue(geneve->dev, &list);
+	}
+
+	/* unregister the devices gathered above */
+	unregister_netdevice_many(&list);
+	rtnl_unlock();
+}
+
+static struct pernet_operations geneve_net_ops = {
+	.init = geneve_init_net,
+	.exit = geneve_exit_net,
+	.id   = &geneve_net_id,
+	.size = sizeof(struct geneve_net),
+};
+
+static int __init geneve_init_module(void)
+{
+	int rc;
+
+	rc = register_pernet_subsys(&geneve_net_ops);
+	if (rc)
+		goto out1;
+
+	rc = rtnl_link_register(&geneve_link_ops);
+	if (rc)
+		goto out2;
+
+	return 0;
+out2:
+	unregister_pernet_subsys(&geneve_net_ops);
+out1:
+	return rc;
+}
+late_initcall(geneve_init_module);
+
+static void __exit geneve_cleanup_module(void)
+{
+	rtnl_link_unregister(&geneve_link_ops);
+	unregister_pernet_subsys(&geneve_net_ops);
+}
+module_exit(geneve_cleanup_module);
+
+MODULE_LICENSE("GPL");
+MODULE_VERSION(GENEVE_NETDEV_VER);
+MODULE_AUTHOR("John W. Linville <linville@tuxdriver.com>");
+MODULE_DESCRIPTION("Interface driver for GENEVE encapsulated traffic");
+MODULE_ALIAS_RTNL_LINK("geneve");
diff --git a/drivers/net/hamradio/bpqether.c b/drivers/net/hamradio/bpqether.c
index 63ff08a26da8..7856b6ccf5c5 100644
--- a/drivers/net/hamradio/bpqether.c
+++ b/drivers/net/hamradio/bpqether.c
@@ -76,7 +76,6 @@
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
 #include <linux/stat.h>
-#include <linux/netfilter.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/rtnetlink.h>
diff --git a/drivers/net/hyperv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index 41071d32bc8e..dd4544085db3 100644
--- a/drivers/net/hyperv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -161,6 +161,7 @@ struct netvsc_device_info {
 	unsigned char mac_adr[ETH_ALEN];
 	bool link_state;	/* 0 - link up, 1 - link down */
 	int  ring_size;
+	u32  max_num_vrss_chns;
 };
 
 enum rndis_device_state {
@@ -611,6 +612,12 @@ struct multi_send_data {
 	u32 count; /* counter of batched packets */
 };
 
+struct netvsc_stats {
+	u64 packets;
+	u64 bytes;
+	struct u64_stats_sync syncp;
+};
+
 /* The context of the netvsc device  */
 struct net_device_context {
 	/* point back to our device context */
@@ -618,6 +625,9 @@ struct net_device_context {
 	struct delayed_work dwork;
 	struct work_struct work;
 	u32 msg_enable; /* debug level */
+
+	struct netvsc_stats __percpu *tx_stats;
+	struct netvsc_stats __percpu *rx_stats;
 };
 
 /* Per netvsc device */
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index ea091bc5ff09..23126a74f357 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -28,6 +28,7 @@
 #include <linux/slab.h>
 #include <linux/netdevice.h>
 #include <linux/if_ether.h>
+#include <linux/vmalloc.h>
 #include <asm/sync_bitops.h>
 
 #include "hyperv_net.h"
@@ -227,13 +228,18 @@ static int netvsc_init_buf(struct hv_device *device)
 	struct netvsc_device *net_device;
 	struct nvsp_message *init_packet;
 	struct net_device *ndev;
+	int node;
 
 	net_device = get_outbound_net_device(device);
 	if (!net_device)
 		return -ENODEV;
 	ndev = net_device->ndev;
 
-	net_device->recv_buf = vzalloc(net_device->recv_buf_size);
+	node = cpu_to_node(device->channel->target_cpu);
+	net_device->recv_buf = vzalloc_node(net_device->recv_buf_size, node);
+	if (!net_device->recv_buf)
+		net_device->recv_buf = vzalloc(net_device->recv_buf_size);
+
 	if (!net_device->recv_buf) {
 		netdev_err(ndev, "unable to allocate receive "
 			"buffer of size %d\n", net_device->recv_buf_size);
@@ -321,7 +327,9 @@ static int netvsc_init_buf(struct hv_device *device)
 
 	/* Now setup the send buffer.
 	 */
-	net_device->send_buf = vzalloc(net_device->send_buf_size);
+	net_device->send_buf = vzalloc_node(net_device->send_buf_size, node);
+	if (!net_device->send_buf)
+		net_device->send_buf = vzalloc(net_device->send_buf_size);
 	if (!net_device->send_buf) {
 		netdev_err(ndev, "unable to allocate send "
 			   "buffer of size %d\n", net_device->send_buf_size);
@@ -743,6 +751,7 @@ static inline int netvsc_send_pkt(
 	u64 req_id;
 	int ret;
 	struct hv_page_buffer *pgbuf;
+	u32 ring_avail = hv_ringbuf_avail_percent(&out_channel->outbound);
 
 	nvmsg.hdr.msg_type = NVSP_MSG1_TYPE_SEND_RNDIS_PKT;
 	if (packet->is_data_pkt) {
@@ -769,32 +778,42 @@ static inline int netvsc_send_pkt(
 	if (out_channel->rescind)
 		return -ENODEV;
 
+	/*
+	 * It is possible that once we successfully place this packet
+	 * on the ringbuffer, we may stop the queue. In that case, we want
+	 * to notify the host independent of the xmit_more flag. We don't
+	 * need to be precise here; in the worst case we may signal the host
+	 * unnecessarily.
+	 */
+	if (ring_avail < (RING_AVAIL_PERCENT_LOWATER + 1))
+		packet->xmit_more = false;
+
 	if (packet->page_buf_cnt) {
 		pgbuf = packet->cp_partial ? packet->page_buf +
 			packet->rmsg_pgcnt : packet->page_buf;
-		ret = vmbus_sendpacket_pagebuffer(out_channel,
-						  pgbuf,
-						  packet->page_buf_cnt,
-						  &nvmsg,
-						  sizeof(struct nvsp_message),
-						  req_id);
+		ret = vmbus_sendpacket_pagebuffer_ctl(out_channel,
+						      pgbuf,
+						      packet->page_buf_cnt,
+						      &nvmsg,
+						      sizeof(struct nvsp_message),
+						      req_id,
+						      VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED,
+						      !packet->xmit_more);
 	} else {
-		ret = vmbus_sendpacket(
-				out_channel, &nvmsg,
-				sizeof(struct nvsp_message),
-				req_id,
-				VM_PKT_DATA_INBAND,
-				VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
+		ret = vmbus_sendpacket_ctl(out_channel, &nvmsg,
+					   sizeof(struct nvsp_message),
+					   req_id,
+					   VM_PKT_DATA_INBAND,
+					   VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED,
+					   !packet->xmit_more);
 	}
 
 	if (ret == 0) {
 		atomic_inc(&net_device->num_outstanding_sends);
 		atomic_inc(&net_device->queue_sends[q_idx]);
 
-		if (hv_ringbuf_avail_percent(&out_channel->outbound) <
-			RING_AVAIL_PERCENT_LOWATER) {
-			netif_tx_stop_queue(netdev_get_tx_queue(
-					    ndev, q_idx));
+		if (ring_avail < RING_AVAIL_PERCENT_LOWATER) {
+			netif_tx_stop_queue(netdev_get_tx_queue(ndev, q_idx));
 
 			if (atomic_read(&net_device->
 				queue_sends[q_idx]) < 1)
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 5993c7e2d723..358475ed9b59 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -46,6 +46,8 @@ static int ring_size = 128;
 module_param(ring_size, int, S_IRUGO);
 MODULE_PARM_DESC(ring_size, "Ring buffer size (# of pages)");
 
+static int max_num_vrss_chns = 8;
+
 static const u32 default_msg = NETIF_MSG_DRV | NETIF_MSG_PROBE |
 				NETIF_MSG_LINK | NETIF_MSG_IFUP |
 				NETIF_MSG_IFDOWN | NETIF_MSG_RX_ERR |
@@ -196,12 +198,12 @@ static bool netvsc_set_hash(u32 *hash, struct sk_buff *skb)
 	struct flow_keys flow;
 	int data_len;
 
-	if (!skb_flow_dissect(skb, &flow) ||
-	    !(flow.n_proto == htons(ETH_P_IP) ||
-	      flow.n_proto == htons(ETH_P_IPV6)))
+	if (!skb_flow_dissect_flow_keys(skb, &flow) ||
+	    !(flow.basic.n_proto == htons(ETH_P_IP) ||
+	      flow.basic.n_proto == htons(ETH_P_IPV6)))
 		return false;
 
-	if (flow.ip_proto == IPPROTO_TCP)
+	if (flow.basic.ip_proto == IPPROTO_TCP)
 		data_len = 12;
 	else
 		data_len = 8;
@@ -391,7 +393,7 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	u32 skb_length;
 	u32 pkt_sz;
 	struct hv_page_buffer page_buf[MAX_PAGE_BUFFER_COUNT];
-
+	struct netvsc_stats *tx_stats = this_cpu_ptr(net_device_ctx->tx_stats);
 
 	/* We will atmost need two pages to describe the rndis
 	 * header. We can only transmit MAX_PAGE_BUFFER_COUNT number
@@ -580,8 +582,10 @@ do_send:
 
 drop:
 	if (ret == 0) {
-		net->stats.tx_bytes += skb_length;
-		net->stats.tx_packets++;
+		u64_stats_update_begin(&tx_stats->syncp);
+		tx_stats->packets++;
+		tx_stats->bytes += skb_length;
+		u64_stats_update_end(&tx_stats->syncp);
 	} else {
 		if (ret != -EAGAIN) {
 			dev_kfree_skb_any(skb);
@@ -644,13 +648,17 @@ int netvsc_recv_callback(struct hv_device *device_obj,
 				struct ndis_tcp_ip_checksum_info *csum_info)
 {
 	struct net_device *net;
+	struct net_device_context *net_device_ctx;
 	struct sk_buff *skb;
+	struct netvsc_stats *rx_stats;
 
 	net = ((struct netvsc_device *)hv_get_drvdata(device_obj))->ndev;
 	if (!net || net->reg_state != NETREG_REGISTERED) {
 		packet->status = NVSP_STAT_FAIL;
 		return 0;
 	}
+	net_device_ctx = netdev_priv(net);
+	rx_stats = this_cpu_ptr(net_device_ctx->rx_stats);
 
 	/* Allocate a skb - TODO direct I/O to pages? */
 	skb = netdev_alloc_skb_ip_align(net, packet->total_data_buflen);
@@ -686,8 +694,10 @@ int netvsc_recv_callback(struct hv_device *device_obj,
 	skb_record_rx_queue(skb, packet->channel->
 			    offermsg.offer.sub_channel_index);
 
-	net->stats.rx_packets++;
-	net->stats.rx_bytes += packet->total_data_buflen;
+	u64_stats_update_begin(&rx_stats->syncp);
+	rx_stats->packets++;
+	rx_stats->bytes += packet->total_data_buflen;
+	u64_stats_update_end(&rx_stats->syncp);
 
 	/*
 	 * Pass the skb back up. Network stack will deallocate the skb when it
@@ -747,12 +757,53 @@ static int netvsc_change_mtu(struct net_device *ndev, int mtu)
 	ndevctx->device_ctx = hdev;
 	hv_set_drvdata(hdev, ndev);
 	device_info.ring_size = ring_size;
+	device_info.max_num_vrss_chns = max_num_vrss_chns;
 	rndis_filter_device_add(hdev, &device_info);
 	netif_tx_wake_all_queues(ndev);
 
 	return 0;
 }
 
+static struct rtnl_link_stats64 *netvsc_get_stats64(struct net_device *net,
+						    struct rtnl_link_stats64 *t)
+{
+	struct net_device_context *ndev_ctx = netdev_priv(net);
+	int cpu;
+
+	for_each_possible_cpu(cpu) {
+		struct netvsc_stats *tx_stats = per_cpu_ptr(ndev_ctx->tx_stats,
+							    cpu);
+		struct netvsc_stats *rx_stats = per_cpu_ptr(ndev_ctx->rx_stats,
+							    cpu);
+		u64 tx_packets, tx_bytes, rx_packets, rx_bytes;
+		unsigned int start;
+
+		do {
+			start = u64_stats_fetch_begin_irq(&tx_stats->syncp);
+			tx_packets = tx_stats->packets;
+			tx_bytes = tx_stats->bytes;
+		} while (u64_stats_fetch_retry_irq(&tx_stats->syncp, start));
+
+		do {
+			start = u64_stats_fetch_begin_irq(&rx_stats->syncp);
+			rx_packets = rx_stats->packets;
+			rx_bytes = rx_stats->bytes;
+		} while (u64_stats_fetch_retry_irq(&rx_stats->syncp, start));
+
+		t->tx_bytes	+= tx_bytes;
+		t->tx_packets	+= tx_packets;
+		t->rx_bytes	+= rx_bytes;
+		t->rx_packets	+= rx_packets;
+	}
+
+	t->tx_dropped	= net->stats.tx_dropped;
+	t->tx_errors	= net->stats.tx_dropped;
+
+	t->rx_dropped	= net->stats.rx_dropped;
+	t->rx_errors	= net->stats.rx_errors;
+
+	return t;
+}
 
 static int netvsc_set_mac_addr(struct net_device *ndev, void *p)
 {
@@ -804,6 +855,7 @@ static const struct net_device_ops device_ops = {
 	.ndo_validate_addr =		eth_validate_addr,
 	.ndo_set_mac_address =		netvsc_set_mac_addr,
 	.ndo_select_queue =		netvsc_select_queue,
+	.ndo_get_stats64 =		netvsc_get_stats64,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller =		netvsc_poll_controller,
 #endif
@@ -855,6 +907,14 @@ static void netvsc_link_change(struct work_struct *w)
 		netdev_notify_peers(net);
 }
 
+static void netvsc_free_netdev(struct net_device *netdev)
+{
+	struct net_device_context *net_device_ctx = netdev_priv(netdev);
+
+	free_percpu(net_device_ctx->tx_stats);
+	free_percpu(net_device_ctx->rx_stats);
+	free_netdev(netdev);
+}
 
 static int netvsc_probe(struct hv_device *dev,
 			const struct hv_vmbus_device_id *dev_id)
@@ -883,6 +943,18 @@ static int netvsc_probe(struct hv_device *dev,
 		netdev_dbg(net, "netvsc msg_enable: %d\n",
 			   net_device_ctx->msg_enable);
 
+	net_device_ctx->tx_stats = netdev_alloc_pcpu_stats(struct netvsc_stats);
+	if (!net_device_ctx->tx_stats) {
+		free_netdev(net);
+		return -ENOMEM;
+	}
+	net_device_ctx->rx_stats = netdev_alloc_pcpu_stats(struct netvsc_stats);
+	if (!net_device_ctx->rx_stats) {
+		free_percpu(net_device_ctx->tx_stats);
+		free_netdev(net);
+		return -ENOMEM;
+	}
+
 	hv_set_drvdata(dev, net);
 	INIT_DELAYED_WORK(&net_device_ctx->dwork, netvsc_link_change);
 	INIT_WORK(&net_device_ctx->work, do_set_multicast);
@@ -906,10 +978,11 @@ static int netvsc_probe(struct hv_device *dev,
 
 	/* Notify the netvsc driver of the new device */
 	device_info.ring_size = ring_size;
+	device_info.max_num_vrss_chns = max_num_vrss_chns;
 	ret = rndis_filter_device_add(dev, &device_info);
 	if (ret != 0) {
 		netdev_err(net, "unable to add netvsc device (ret %d)\n", ret);
-		free_netdev(net);
+		netvsc_free_netdev(net);
 		hv_set_drvdata(dev, NULL);
 		return ret;
 	}
@@ -923,7 +996,7 @@ static int netvsc_probe(struct hv_device *dev,
 	if (ret != 0) {
 		pr_err("Unable to register netdev.\n");
 		rndis_filter_device_remove(dev);
-		free_netdev(net);
+		netvsc_free_netdev(net);
 	} else {
 		schedule_delayed_work(&net_device_ctx->dwork, 0);
 	}
@@ -962,7 +1035,7 @@ static int netvsc_remove(struct hv_device *dev)
 	 */
 	rndis_filter_device_remove(dev);
 
-	free_netdev(net);
+	netvsc_free_netdev(net);
 	return 0;
 }
 
diff --git a/drivers/net/hyperv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index 9118cea91882..236aeb76ef22 100644
--- a/drivers/net/hyperv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -27,6 +27,7 @@
 #include <linux/netdevice.h>
 #include <linux/if_vlan.h>
 #include <linux/nls.h>
+#include <linux/vmalloc.h>
 
 #include "hyperv_net.h"
 
@@ -1013,6 +1014,9 @@ int rndis_filter_device_add(struct hv_device *dev,
 	struct ndis_recv_scale_cap rsscap;
 	u32 rsscap_size = sizeof(struct ndis_recv_scale_cap);
 	u32 mtu, size;
+	u32 num_rss_qs;
+	const struct cpumask *node_cpu_mask;
+	u32 num_possible_rss_qs;
 
 	rndis_device = get_rndis_device();
 	if (!rndis_device)
@@ -1100,9 +1104,18 @@ int rndis_filter_device_add(struct hv_device *dev,
 	if (ret || rsscap.num_recv_que < 2)
 		goto out;
 
+	num_rss_qs = min(device_info->max_num_vrss_chns, rsscap.num_recv_que);
+
 	net_device->max_chn = rsscap.num_recv_que;
-	net_device->num_chn = (num_online_cpus() < rsscap.num_recv_que) ?
-			       num_online_cpus() : rsscap.num_recv_que;
+
+	/*
+	 * We will limit the VRSS channels to the number CPUs in the NUMA node
+	 * the primary channel is currently bound to.
+	 */
+	node_cpu_mask = cpumask_of_node(cpu_to_node(dev->channel->target_cpu));
+	num_possible_rss_qs = cpumask_weight(node_cpu_mask);
+	net_device->num_chn = min(num_possible_rss_qs, num_rss_qs);
+
 	if (net_device->num_chn == 1)
 		goto out;
 
diff --git a/drivers/net/ieee802154/Kconfig b/drivers/net/ieee802154/Kconfig
index 1a3c3e57aa0b..1dd5ab8e5054 100644
--- a/drivers/net/ieee802154/Kconfig
+++ b/drivers/net/ieee802154/Kconfig
@@ -53,3 +53,13 @@ config IEEE802154_CC2520
 
 	  This driver can also be built as a module. To do so, say M here.
 	  the module will be called 'cc2520'.
+
+config IEEE802154_ATUSB
+	tristate "ATUSB transceiver driver"
+	depends on IEEE802154_DRIVERS && MAC802154 && USB
+	---help---
+	  Say Y here to enable the ATUSB IEEE 802.15.4 wireless
+	  controller.
+
+	  This driver can also be built as a module. To do so say M here.
+	  The module will be called 'atusb'.
diff --git a/drivers/net/ieee802154/Makefile b/drivers/net/ieee802154/Makefile
index d77fa4d77e27..cf1d2a6db023 100644
--- a/drivers/net/ieee802154/Makefile
+++ b/drivers/net/ieee802154/Makefile
@@ -2,3 +2,4 @@ obj-$(CONFIG_IEEE802154_FAKELB) += fakelb.o
 obj-$(CONFIG_IEEE802154_AT86RF230) += at86rf230.o
 obj-$(CONFIG_IEEE802154_MRF24J40) += mrf24j40.o
 obj-$(CONFIG_IEEE802154_CC2520) += cc2520.o
+obj-$(CONFIG_IEEE802154_ATUSB) += atusb.o
diff --git a/drivers/net/ieee802154/at86rf230.c b/drivers/net/ieee802154/at86rf230.c
index 67d00fbc2e0e..f7bd9f3ddaac 100644
--- a/drivers/net/ieee802154/at86rf230.c
+++ b/drivers/net/ieee802154/at86rf230.c
@@ -35,6 +35,8 @@
 #include <net/mac802154.h>
 #include <net/cfg802154.h>
 
+#include "at86rf230.h"
+
 struct at86rf230_local;
 /* at86rf2xx chip depend data.
  * All timings are in us.
@@ -45,12 +47,14 @@ struct at86rf2xx_chip_data {
 	u16 t_reset_to_off;
 	u16 t_off_to_aack;
 	u16 t_off_to_tx_on;
+	u16 t_off_to_sleep;
+	u16 t_sleep_to_off;
 	u16 t_frame;
 	u16 t_p_ack;
 	int rssi_base_val;
 
 	int (*set_channel)(struct at86rf230_local *, u8, u8);
-	int (*get_desense_steps)(struct at86rf230_local *, s32);
+	int (*set_txpower)(struct at86rf230_local *, s32);
 };
 
 #define AT86RF2XX_MAX_BUF		(127 + 3)
@@ -86,6 +90,7 @@ struct at86rf230_local {
 	struct at86rf2xx_chip_data *data;
 	struct regmap *regmap;
 	int slp_tr;
+	bool sleep;
 
 	struct completion state_complete;
 	struct at86rf230_state_change state;
@@ -102,200 +107,6 @@ struct at86rf230_local {
 	struct at86rf230_state_change tx;
 };
 
-#define RG_TRX_STATUS	(0x01)
-#define SR_TRX_STATUS		0x01, 0x1f, 0
-#define SR_RESERVED_01_3	0x01, 0x20, 5
-#define SR_CCA_STATUS		0x01, 0x40, 6
-#define SR_CCA_DONE		0x01, 0x80, 7
-#define RG_TRX_STATE	(0x02)
-#define SR_TRX_CMD		0x02, 0x1f, 0
-#define SR_TRAC_STATUS		0x02, 0xe0, 5
-#define RG_TRX_CTRL_0	(0x03)
-#define SR_CLKM_CTRL		0x03, 0x07, 0
-#define SR_CLKM_SHA_SEL		0x03, 0x08, 3
-#define SR_PAD_IO_CLKM		0x03, 0x30, 4
-#define SR_PAD_IO		0x03, 0xc0, 6
-#define RG_TRX_CTRL_1	(0x04)
-#define SR_IRQ_POLARITY		0x04, 0x01, 0
-#define SR_IRQ_MASK_MODE	0x04, 0x02, 1
-#define SR_SPI_CMD_MODE		0x04, 0x0c, 2
-#define SR_RX_BL_CTRL		0x04, 0x10, 4
-#define SR_TX_AUTO_CRC_ON	0x04, 0x20, 5
-#define SR_IRQ_2_EXT_EN		0x04, 0x40, 6
-#define SR_PA_EXT_EN		0x04, 0x80, 7
-#define RG_PHY_TX_PWR	(0x05)
-#define SR_TX_PWR		0x05, 0x0f, 0
-#define SR_PA_LT		0x05, 0x30, 4
-#define SR_PA_BUF_LT		0x05, 0xc0, 6
-#define RG_PHY_RSSI	(0x06)
-#define SR_RSSI			0x06, 0x1f, 0
-#define SR_RND_VALUE		0x06, 0x60, 5
-#define SR_RX_CRC_VALID		0x06, 0x80, 7
-#define RG_PHY_ED_LEVEL	(0x07)
-#define SR_ED_LEVEL		0x07, 0xff, 0
-#define RG_PHY_CC_CCA	(0x08)
-#define SR_CHANNEL		0x08, 0x1f, 0
-#define SR_CCA_MODE		0x08, 0x60, 5
-#define SR_CCA_REQUEST		0x08, 0x80, 7
-#define RG_CCA_THRES	(0x09)
-#define SR_CCA_ED_THRES		0x09, 0x0f, 0
-#define SR_RESERVED_09_1	0x09, 0xf0, 4
-#define RG_RX_CTRL	(0x0a)
-#define SR_PDT_THRES		0x0a, 0x0f, 0
-#define SR_RESERVED_0a_1	0x0a, 0xf0, 4
-#define RG_SFD_VALUE	(0x0b)
-#define SR_SFD_VALUE		0x0b, 0xff, 0
-#define RG_TRX_CTRL_2	(0x0c)
-#define SR_OQPSK_DATA_RATE	0x0c, 0x03, 0
-#define SR_SUB_MODE		0x0c, 0x04, 2
-#define SR_BPSK_QPSK		0x0c, 0x08, 3
-#define SR_OQPSK_SUB1_RC_EN	0x0c, 0x10, 4
-#define SR_RESERVED_0c_5	0x0c, 0x60, 5
-#define SR_RX_SAFE_MODE		0x0c, 0x80, 7
-#define RG_ANT_DIV	(0x0d)
-#define SR_ANT_CTRL		0x0d, 0x03, 0
-#define SR_ANT_EXT_SW_EN	0x0d, 0x04, 2
-#define SR_ANT_DIV_EN		0x0d, 0x08, 3
-#define SR_RESERVED_0d_2	0x0d, 0x70, 4
-#define SR_ANT_SEL		0x0d, 0x80, 7
-#define RG_IRQ_MASK	(0x0e)
-#define SR_IRQ_MASK		0x0e, 0xff, 0
-#define RG_IRQ_STATUS	(0x0f)
-#define SR_IRQ_0_PLL_LOCK	0x0f, 0x01, 0
-#define SR_IRQ_1_PLL_UNLOCK	0x0f, 0x02, 1
-#define SR_IRQ_2_RX_START	0x0f, 0x04, 2
-#define SR_IRQ_3_TRX_END	0x0f, 0x08, 3
-#define SR_IRQ_4_CCA_ED_DONE	0x0f, 0x10, 4
-#define SR_IRQ_5_AMI		0x0f, 0x20, 5
-#define SR_IRQ_6_TRX_UR		0x0f, 0x40, 6
-#define SR_IRQ_7_BAT_LOW	0x0f, 0x80, 7
-#define RG_VREG_CTRL	(0x10)
-#define SR_RESERVED_10_6	0x10, 0x03, 0
-#define SR_DVDD_OK		0x10, 0x04, 2
-#define SR_DVREG_EXT		0x10, 0x08, 3
-#define SR_RESERVED_10_3	0x10, 0x30, 4
-#define SR_AVDD_OK		0x10, 0x40, 6
-#define SR_AVREG_EXT		0x10, 0x80, 7
-#define RG_BATMON	(0x11)
-#define SR_BATMON_VTH		0x11, 0x0f, 0
-#define SR_BATMON_HR		0x11, 0x10, 4
-#define SR_BATMON_OK		0x11, 0x20, 5
-#define SR_RESERVED_11_1	0x11, 0xc0, 6
-#define RG_XOSC_CTRL	(0x12)
-#define SR_XTAL_TRIM		0x12, 0x0f, 0
-#define SR_XTAL_MODE		0x12, 0xf0, 4
-#define RG_RX_SYN	(0x15)
-#define SR_RX_PDT_LEVEL		0x15, 0x0f, 0
-#define SR_RESERVED_15_2	0x15, 0x70, 4
-#define SR_RX_PDT_DIS		0x15, 0x80, 7
-#define RG_XAH_CTRL_1	(0x17)
-#define SR_RESERVED_17_8	0x17, 0x01, 0
-#define SR_AACK_PROM_MODE	0x17, 0x02, 1
-#define SR_AACK_ACK_TIME	0x17, 0x04, 2
-#define SR_RESERVED_17_5	0x17, 0x08, 3
-#define SR_AACK_UPLD_RES_FT	0x17, 0x10, 4
-#define SR_AACK_FLTR_RES_FT	0x17, 0x20, 5
-#define SR_CSMA_LBT_MODE	0x17, 0x40, 6
-#define SR_RESERVED_17_1	0x17, 0x80, 7
-#define RG_FTN_CTRL	(0x18)
-#define SR_RESERVED_18_2	0x18, 0x7f, 0
-#define SR_FTN_START		0x18, 0x80, 7
-#define RG_PLL_CF	(0x1a)
-#define SR_RESERVED_1a_2	0x1a, 0x7f, 0
-#define SR_PLL_CF_START		0x1a, 0x80, 7
-#define RG_PLL_DCU	(0x1b)
-#define SR_RESERVED_1b_3	0x1b, 0x3f, 0
-#define SR_RESERVED_1b_2	0x1b, 0x40, 6
-#define SR_PLL_DCU_START	0x1b, 0x80, 7
-#define RG_PART_NUM	(0x1c)
-#define SR_PART_NUM		0x1c, 0xff, 0
-#define RG_VERSION_NUM	(0x1d)
-#define SR_VERSION_NUM		0x1d, 0xff, 0
-#define RG_MAN_ID_0	(0x1e)
-#define SR_MAN_ID_0		0x1e, 0xff, 0
-#define RG_MAN_ID_1	(0x1f)
-#define SR_MAN_ID_1		0x1f, 0xff, 0
-#define RG_SHORT_ADDR_0	(0x20)
-#define SR_SHORT_ADDR_0		0x20, 0xff, 0
-#define RG_SHORT_ADDR_1	(0x21)
-#define SR_SHORT_ADDR_1		0x21, 0xff, 0
-#define RG_PAN_ID_0	(0x22)
-#define SR_PAN_ID_0		0x22, 0xff, 0
-#define RG_PAN_ID_1	(0x23)
-#define SR_PAN_ID_1		0x23, 0xff, 0
-#define RG_IEEE_ADDR_0	(0x24)
-#define SR_IEEE_ADDR_0		0x24, 0xff, 0
-#define RG_IEEE_ADDR_1	(0x25)
-#define SR_IEEE_ADDR_1		0x25, 0xff, 0
-#define RG_IEEE_ADDR_2	(0x26)
-#define SR_IEEE_ADDR_2		0x26, 0xff, 0
-#define RG_IEEE_ADDR_3	(0x27)
-#define SR_IEEE_ADDR_3		0x27, 0xff, 0
-#define RG_IEEE_ADDR_4	(0x28)
-#define SR_IEEE_ADDR_4		0x28, 0xff, 0
-#define RG_IEEE_ADDR_5	(0x29)
-#define SR_IEEE_ADDR_5		0x29, 0xff, 0
-#define RG_IEEE_ADDR_6	(0x2a)
-#define SR_IEEE_ADDR_6		0x2a, 0xff, 0
-#define RG_IEEE_ADDR_7	(0x2b)
-#define SR_IEEE_ADDR_7		0x2b, 0xff, 0
-#define RG_XAH_CTRL_0	(0x2c)
-#define SR_SLOTTED_OPERATION	0x2c, 0x01, 0
-#define SR_MAX_CSMA_RETRIES	0x2c, 0x0e, 1
-#define SR_MAX_FRAME_RETRIES	0x2c, 0xf0, 4
-#define RG_CSMA_SEED_0	(0x2d)
-#define SR_CSMA_SEED_0		0x2d, 0xff, 0
-#define RG_CSMA_SEED_1	(0x2e)
-#define SR_CSMA_SEED_1		0x2e, 0x07, 0
-#define SR_AACK_I_AM_COORD	0x2e, 0x08, 3
-#define SR_AACK_DIS_ACK		0x2e, 0x10, 4
-#define SR_AACK_SET_PD		0x2e, 0x20, 5
-#define SR_AACK_FVN_MODE	0x2e, 0xc0, 6
-#define RG_CSMA_BE	(0x2f)
-#define SR_MIN_BE		0x2f, 0x0f, 0
-#define SR_MAX_BE		0x2f, 0xf0, 4
-
-#define CMD_REG		0x80
-#define CMD_REG_MASK	0x3f
-#define CMD_WRITE	0x40
-#define CMD_FB		0x20
-
-#define IRQ_BAT_LOW	(1 << 7)
-#define IRQ_TRX_UR	(1 << 6)
-#define IRQ_AMI		(1 << 5)
-#define IRQ_CCA_ED	(1 << 4)
-#define IRQ_TRX_END	(1 << 3)
-#define IRQ_RX_START	(1 << 2)
-#define IRQ_PLL_UNL	(1 << 1)
-#define IRQ_PLL_LOCK	(1 << 0)
-
-#define IRQ_ACTIVE_HIGH	0
-#define IRQ_ACTIVE_LOW	1
-
-#define STATE_P_ON		0x00	/* BUSY */
-#define STATE_BUSY_RX		0x01
-#define STATE_BUSY_TX		0x02
-#define STATE_FORCE_TRX_OFF	0x03
-#define STATE_FORCE_TX_ON	0x04	/* IDLE */
-/* 0x05 */				/* INVALID_PARAMETER */
-#define STATE_RX_ON		0x06
-/* 0x07 */				/* SUCCESS */
-#define STATE_TRX_OFF		0x08
-#define STATE_TX_ON		0x09
-/* 0x0a - 0x0e */			/* 0x0a - UNSUPPORTED_ATTRIBUTE */
-#define STATE_SLEEP		0x0F
-#define STATE_PREP_DEEP_SLEEP	0x10
-#define STATE_BUSY_RX_AACK	0x11
-#define STATE_BUSY_TX_ARET	0x12
-#define STATE_RX_AACK_ON	0x16
-#define STATE_TX_ARET_ON	0x19
-#define STATE_RX_ON_NOCLK	0x1C
-#define STATE_RX_AACK_ON_NOCLK	0x1D
-#define STATE_BUSY_RX_AACK_NOCLK 0x1E
-#define STATE_TRANSITION_IN_PROGRESS 0x1F
-
-#define TRX_STATE_MASK		(0x1F)
-
 #define AT86RF2XX_NUMREGS 0x3F
 
 static void
@@ -304,18 +115,66 @@ at86rf230_async_state_change(struct at86rf230_local *lp,
 			     const u8 state, void (*complete)(void *context),
 			     const bool irq_enable);
 
+static inline void
+at86rf230_sleep(struct at86rf230_local *lp)
+{
+	if (gpio_is_valid(lp->slp_tr)) {
+		gpio_set_value(lp->slp_tr, 1);
+		usleep_range(lp->data->t_off_to_sleep,
+			     lp->data->t_off_to_sleep + 10);
+		lp->sleep = true;
+	}
+}
+
+static inline void
+at86rf230_awake(struct at86rf230_local *lp)
+{
+	if (gpio_is_valid(lp->slp_tr)) {
+		gpio_set_value(lp->slp_tr, 0);
+		usleep_range(lp->data->t_sleep_to_off,
+			     lp->data->t_sleep_to_off + 100);
+		lp->sleep = false;
+	}
+}
+
 static inline int
 __at86rf230_write(struct at86rf230_local *lp,
 		  unsigned int addr, unsigned int data)
 {
-	return regmap_write(lp->regmap, addr, data);
+	bool sleep = lp->sleep;
+	int ret;
+
+	/* awake for register setting if sleep */
+	if (sleep)
+		at86rf230_awake(lp);
+
+	ret = regmap_write(lp->regmap, addr, data);
+
+	/* sleep again if was sleeping */
+	if (sleep)
+		at86rf230_sleep(lp);
+
+	return ret;
 }
 
 static inline int
 __at86rf230_read(struct at86rf230_local *lp,
 		 unsigned int addr, unsigned int *data)
 {
-	return regmap_read(lp->regmap, addr, data);
+	bool sleep = lp->sleep;
+	int ret;
+
+	/* awake for register setting if sleep */
+	if (sleep)
+		at86rf230_awake(lp);
+
+	ret = regmap_read(lp->regmap, addr, data);
+
+	/* sleep again if was sleeping */
+	if (sleep)
+		at86rf230_sleep(lp);
+
+	return ret;
 }
 
 static inline int
@@ -337,7 +196,20 @@ at86rf230_write_subreg(struct at86rf230_local *lp,
 		       unsigned int addr, unsigned int mask,
 		       unsigned int shift, unsigned int data)
 {
-	return regmap_update_bits(lp->regmap, addr, mask, data << shift);
+	bool sleep = lp->sleep;
+	int ret;
+
+	/* awake for register setting if sleep */
+	if (sleep)
+		at86rf230_awake(lp);
+
+	ret = regmap_update_bits(lp->regmap, addr, mask, data << shift);
+
+	/* sleep again if was sleeping */
+	if (sleep)
+		at86rf230_sleep(lp);
+
+	return ret;
 }
 
 static inline void
@@ -1010,7 +882,7 @@ at86rf230_xmit_start(void *context)
 		if (lp->is_tx_from_off) {
 			lp->is_tx_from_off = false;
 			at86rf230_async_state_change(lp, ctx, STATE_TX_ARET_ON,
-						     at86rf230_xmit_tx_on,
+						     at86rf230_write_frame,
 						     false);
 		} else {
 			at86rf230_async_state_change(lp, ctx, STATE_TX_ON,
@@ -1061,13 +933,34 @@ at86rf230_ed(struct ieee802154_hw *hw, u8 *level)
 static int
 at86rf230_start(struct ieee802154_hw *hw)
 {
-	return at86rf230_sync_state_change(hw->priv, STATE_RX_AACK_ON);
+	struct at86rf230_local *lp = hw->priv;
+
+	at86rf230_awake(lp);
+	enable_irq(lp->spi->irq);
+
+	return at86rf230_sync_state_change(lp, STATE_RX_AACK_ON);
 }
 
 static void
 at86rf230_stop(struct ieee802154_hw *hw)
 {
-	at86rf230_sync_state_change(hw->priv, STATE_FORCE_TRX_OFF);
+	struct at86rf230_local *lp = hw->priv;
+	u8 csma_seed[2];
+
+	at86rf230_sync_state_change(lp, STATE_FORCE_TRX_OFF);
+
+	disable_irq(lp->spi->irq);
+
+	/* It's recommended to set random new csma_seeds before sleep state.
+	 * Makes only sense in the stop callback, not doing this inside of
+	 * at86rf230_sleep, this is also used when we don't transmit afterwards
+	 * when calling start callback again.
+	 */
+	get_random_bytes(csma_seed, ARRAY_SIZE(csma_seed));
+	at86rf230_write_subreg(lp, SR_CSMA_SEED_0, csma_seed[0]);
+	at86rf230_write_subreg(lp, SR_CSMA_SEED_1, csma_seed[1]);
+
+	at86rf230_sleep(lp);
 }
 
 static int
@@ -1076,6 +969,50 @@ at86rf23x_set_channel(struct at86rf230_local *lp, u8 page, u8 channel)
 	return at86rf230_write_subreg(lp, SR_CHANNEL, channel);
 }
 
+#define AT86RF2XX_MAX_ED_LEVELS 0xF
+static const s32 at86rf23x_ed_levels[AT86RF2XX_MAX_ED_LEVELS + 1] = {
+	-9100, -8900, -8700, -8500, -8300, -8100, -7900, -7700, -7500, -7300,
+	-7100, -6900, -6700, -6500, -6300, -6100,
+};
+
+static const s32 at86rf212_ed_levels_100[AT86RF2XX_MAX_ED_LEVELS + 1] = {
+	-10000, -9800, -9600, -9400, -9200, -9000, -8800, -8600, -8400, -8200,
+	-8000, -7800, -7600, -7400, -7200, -7000,
+};
+
+static const s32 at86rf212_ed_levels_98[AT86RF2XX_MAX_ED_LEVELS + 1] = {
+	-9800, -9600, -9400, -9200, -9000, -8800, -8600, -8400, -8200, -8000,
+	-7800, -7600, -7400, -7200, -7000, -6800,
+};
+
+static inline int
+at86rf212_update_cca_ed_level(struct at86rf230_local *lp, int rssi_base_val)
+{
+	unsigned int cca_ed_thres;
+	int rc;
+
+	rc = at86rf230_read_subreg(lp, SR_CCA_ED_THRES, &cca_ed_thres);
+	if (rc < 0)
+		return rc;
+
+	switch (rssi_base_val) {
+	case -98:
+		lp->hw->phy->supported.cca_ed_levels = at86rf212_ed_levels_98;
+		lp->hw->phy->supported.cca_ed_levels_size = ARRAY_SIZE(at86rf212_ed_levels_98);
+		lp->hw->phy->cca_ed_level = at86rf212_ed_levels_98[cca_ed_thres];
+		break;
+	case -100:
+		lp->hw->phy->supported.cca_ed_levels = at86rf212_ed_levels_100;
+		lp->hw->phy->supported.cca_ed_levels_size = ARRAY_SIZE(at86rf212_ed_levels_100);
+		lp->hw->phy->cca_ed_level = at86rf212_ed_levels_100[cca_ed_thres];
+		break;
+	default:
+		WARN_ON(1);
+	}
+
+	return 0;
+}
+
 static int
 at86rf212_set_channel(struct at86rf230_local *lp, u8 page, u8 channel)
 {
@@ -1098,6 +1035,10 @@ at86rf212_set_channel(struct at86rf230_local *lp, u8 page, u8 channel)
 	if (rc < 0)
 		return rc;
 
+	rc = at86rf212_update_cca_ed_level(lp, lp->data->rssi_base_val);
+	if (rc < 0)
+		return rc;
+
 	/* This sets the symbol_duration according frequency on the 212.
 	 * TODO move this handling while set channel and page in cfg802154.
 	 * We can do that, this timings are according 802.15.4 standard.
@@ -1193,23 +1134,56 @@ at86rf230_set_hw_addr_filt(struct ieee802154_hw *hw,
 	return 0;
 }
 
+#define AT86RF23X_MAX_TX_POWERS 0xF
+static const s32 at86rf233_powers[AT86RF23X_MAX_TX_POWERS + 1] = {
+	400, 370, 340, 300, 250, 200, 100, 0, -100, -200, -300, -400, -600,
+	-800, -1200, -1700,
+};
+
+static const s32 at86rf231_powers[AT86RF23X_MAX_TX_POWERS + 1] = {
+	300, 280, 230, 180, 130, 70, 0, -100, -200, -300, -400, -500, -700,
+	-900, -1200, -1700,
+};
+
+#define AT86RF212_MAX_TX_POWERS 0x1F
+static const s32 at86rf212_powers[AT86RF212_MAX_TX_POWERS + 1] = {
+	500, 400, 300, 200, 100, 0, -100, -200, -300, -400, -500, -600, -700,
+	-800, -900, -1000, -1100, -1200, -1300, -1400, -1500, -1600, -1700,
+	-1800, -1900, -2000, -2100, -2200, -2300, -2400, -2500, -2600,
+};
+
 static int
-at86rf230_set_txpower(struct ieee802154_hw *hw, s8 db)
+at86rf23x_set_txpower(struct at86rf230_local *lp, s32 mbm)
 {
-	struct at86rf230_local *lp = hw->priv;
+	u32 i;
 
-	/* typical maximum output is 5dBm with RG_PHY_TX_PWR 0x60, lower five
-	 * bits decrease power in 1dB steps. 0x60 represents extra PA gain of
-	 * 0dB.
-	 * thus, supported values for db range from -26 to 5, for 31dB of
-	 * reduction to 0dB of reduction.
-	 */
-	if (db > 5 || db < -26)
-		return -EINVAL;
+	for (i = 0; i < lp->hw->phy->supported.tx_powers_size; i++) {
+		if (lp->hw->phy->supported.tx_powers[i] == mbm)
+			return at86rf230_write_subreg(lp, SR_TX_PWR_23X, i);
+	}
+
+	return -EINVAL;
+}
+
+static int
+at86rf212_set_txpower(struct at86rf230_local *lp, s32 mbm)
+{
+	u32 i;
 
-	db = -(db - 5);
+	for (i = 0; i < lp->hw->phy->supported.tx_powers_size; i++) {
+		if (lp->hw->phy->supported.tx_powers[i] == mbm)
+			return at86rf230_write_subreg(lp, SR_TX_PWR_212, i);
+	}
 
-	return __at86rf230_write(lp, RG_PHY_TX_PWR, 0x60 | db);
+	return -EINVAL;
+}
+
+static int
+at86rf230_set_txpower(struct ieee802154_hw *hw, s32 mbm)
+{
+	struct at86rf230_local *lp = hw->priv;
+
+	return lp->data->set_txpower(lp, mbm);
 }
 
 static int
@@ -1254,28 +1228,19 @@ at86rf230_set_cca_mode(struct ieee802154_hw *hw,
 	return at86rf230_write_subreg(lp, SR_CCA_MODE, val);
 }
 
-static int
-at86rf212_get_desens_steps(struct at86rf230_local *lp, s32 level)
-{
-	return (level - lp->data->rssi_base_val) * 100 / 207;
-}
-
-static int
-at86rf23x_get_desens_steps(struct at86rf230_local *lp, s32 level)
-{
-	return (level - lp->data->rssi_base_val) / 2;
-}
 
 static int
-at86rf230_set_cca_ed_level(struct ieee802154_hw *hw, s32 level)
+at86rf230_set_cca_ed_level(struct ieee802154_hw *hw, s32 mbm)
 {
 	struct at86rf230_local *lp = hw->priv;
+	u32 i;
 
-	if (level < lp->data->rssi_base_val || level > 30)
-		return -EINVAL;
+	for (i = 0; i < hw->phy->supported.cca_ed_levels_size; i++) {
+		if (hw->phy->supported.cca_ed_levels[i] == mbm)
+			return at86rf230_write_subreg(lp, SR_CCA_ED_THRES, i);
+	}
 
-	return at86rf230_write_subreg(lp, SR_CCA_ED_THRES,
-				      lp->data->get_desense_steps(lp, level));
+	return -EINVAL;
 }
 
 static int
@@ -1361,11 +1326,13 @@ static struct at86rf2xx_chip_data at86rf233_data = {
 	.t_reset_to_off = 26,
 	.t_off_to_aack = 80,
 	.t_off_to_tx_on = 80,
+	.t_off_to_sleep = 35,
+	.t_sleep_to_off = 210,
 	.t_frame = 4096,
 	.t_p_ack = 545,
 	.rssi_base_val = -91,
 	.set_channel = at86rf23x_set_channel,
-	.get_desense_steps = at86rf23x_get_desens_steps
+	.set_txpower = at86rf23x_set_txpower,
 };
 
 static struct at86rf2xx_chip_data at86rf231_data = {
@@ -1374,11 +1341,13 @@ static struct at86rf2xx_chip_data at86rf231_data = {
 	.t_reset_to_off = 37,
 	.t_off_to_aack = 110,
 	.t_off_to_tx_on = 110,
+	.t_off_to_sleep = 35,
+	.t_sleep_to_off = 380,
 	.t_frame = 4096,
 	.t_p_ack = 545,
 	.rssi_base_val = -91,
 	.set_channel = at86rf23x_set_channel,
-	.get_desense_steps = at86rf23x_get_desens_steps
+	.set_txpower = at86rf23x_set_txpower,
 };
 
 static struct at86rf2xx_chip_data at86rf212_data = {
@@ -1387,11 +1356,13 @@ static struct at86rf2xx_chip_data at86rf212_data = {
 	.t_reset_to_off = 26,
 	.t_off_to_aack = 200,
 	.t_off_to_tx_on = 200,
+	.t_off_to_sleep = 35,
+	.t_sleep_to_off = 380,
 	.t_frame = 4096,
 	.t_p_ack = 545,
 	.rssi_base_val = -100,
 	.set_channel = at86rf212_set_channel,
-	.get_desense_steps = at86rf212_get_desens_steps
+	.set_txpower = at86rf212_set_txpower,
 };
 
 static int at86rf230_hw_init(struct at86rf230_local *lp, u8 xtal_trim)
@@ -1563,9 +1534,22 @@ at86rf230_detect_device(struct at86rf230_local *lp)
 		return -EINVAL;
 	}
 
-	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->flags = IEEE802154_HW_TX_OMIT_CKSUM |
+			IEEE802154_HW_CSMA_PARAMS |
+			IEEE802154_HW_FRAME_RETRIES | IEEE802154_HW_AFILT |
+			IEEE802154_HW_PROMISCUOUS;
+
+	lp->hw->phy->flags = WPAN_PHY_FLAG_TXPOWER |
+			     WPAN_PHY_FLAG_CCA_ED_LEVEL |
+			     WPAN_PHY_FLAG_CCA_MODE;
+
+	lp->hw->phy->supported.cca_modes = BIT(NL802154_CCA_ENERGY) |
+		BIT(NL802154_CCA_CARRIER) | BIT(NL802154_CCA_ENERGY_CARRIER);
+	lp->hw->phy->supported.cca_opts = BIT(NL802154_CCA_OPT_ENERGY_CARRIER_AND) |
+		BIT(NL802154_CCA_OPT_ENERGY_CARRIER_OR);
+
+	lp->hw->phy->supported.cca_ed_levels = at86rf23x_ed_levels;
+	lp->hw->phy->supported.cca_ed_levels_size = ARRAY_SIZE(at86rf23x_ed_levels);
 
 	lp->hw->phy->cca.mode = NL802154_CCA_ENERGY;
 
@@ -1573,36 +1557,49 @@ at86rf230_detect_device(struct at86rf230_local *lp)
 	case 2:
 		chip = "at86rf230";
 		rc = -ENOTSUPP;
-		break;
+		goto not_supp;
 	case 3:
 		chip = "at86rf231";
 		lp->data = &at86rf231_data;
-		lp->hw->phy->channels_supported[0] = 0x7FFF800;
+		lp->hw->phy->supported.channels[0] = 0x7FFF800;
 		lp->hw->phy->current_channel = 11;
 		lp->hw->phy->symbol_duration = 16;
+		lp->hw->phy->supported.tx_powers = at86rf231_powers;
+		lp->hw->phy->supported.tx_powers_size = ARRAY_SIZE(at86rf231_powers);
 		break;
 	case 7:
 		chip = "at86rf212";
 		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->supported.channels[0] = 0x00007FF;
+		lp->hw->phy->supported.channels[2] = 0x00007FF;
 		lp->hw->phy->current_channel = 5;
 		lp->hw->phy->symbol_duration = 25;
+		lp->hw->phy->supported.lbt = NL802154_SUPPORTED_BOOL_BOTH;
+		lp->hw->phy->supported.tx_powers = at86rf212_powers;
+		lp->hw->phy->supported.tx_powers_size = ARRAY_SIZE(at86rf212_powers);
+		lp->hw->phy->supported.cca_ed_levels = at86rf212_ed_levels_100;
+		lp->hw->phy->supported.cca_ed_levels_size = ARRAY_SIZE(at86rf212_ed_levels_100);
 		break;
 	case 11:
 		chip = "at86rf233";
 		lp->data = &at86rf233_data;
-		lp->hw->phy->channels_supported[0] = 0x7FFF800;
+		lp->hw->phy->supported.channels[0] = 0x7FFF800;
 		lp->hw->phy->current_channel = 13;
 		lp->hw->phy->symbol_duration = 16;
+		lp->hw->phy->supported.tx_powers = at86rf233_powers;
+		lp->hw->phy->supported.tx_powers_size = ARRAY_SIZE(at86rf233_powers);
 		break;
 	default:
 		chip = "unknown";
 		rc = -ENOTSUPP;
-		break;
+		goto not_supp;
 	}
 
+	lp->hw->phy->cca_ed_level = lp->hw->phy->supported.cca_ed_levels[7];
+	lp->hw->phy->transmit_power = lp->hw->phy->supported.tx_powers[0];
+
+not_supp:
 	dev_info(&lp->spi->dev, "Detected %s chip version %d\n", chip, version);
 
 	return rc;
@@ -1696,7 +1693,6 @@ static int at86rf230_probe(struct spi_device *spi)
 	lp->spi = spi;
 	lp->slp_tr = slp_tr;
 	hw->parent = &spi->dev;
-	hw->vif_data_size = sizeof(*lp);
 	ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
 
 	lp->regmap = devm_regmap_init_spi(spi, &at86rf230_regmap_spi_config);
@@ -1728,13 +1724,19 @@ static int at86rf230_probe(struct spi_device *spi)
 
 	irq_type = irq_get_trigger_type(spi->irq);
 	if (!irq_type)
-		irq_type = IRQF_TRIGGER_RISING;
+		irq_type = IRQF_TRIGGER_HIGH;
 
 	rc = devm_request_irq(&spi->dev, spi->irq, at86rf230_isr,
 			      IRQF_SHARED | irq_type, dev_name(&spi->dev), lp);
 	if (rc)
 		goto free_dev;
 
+	/* disable_irq by default and wait for starting hardware */
+	disable_irq(spi->irq);
+
+	/* going into sleep by default */
+	at86rf230_sleep(lp);
+
 	rc = ieee802154_register_hw(lp->hw);
 	if (rc)
 		goto free_dev;
diff --git a/drivers/net/ieee802154/at86rf230.h b/drivers/net/ieee802154/at86rf230.h
new file mode 100644
index 000000000000..1e6d1cc677f6
--- /dev/null
+++ b/drivers/net/ieee802154/at86rf230.h
@@ -0,0 +1,220 @@
+/*
+ * AT86RF230/RF231 driver
+ *
+ * Copyright (C) 2009-2012 Siemens AG
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * Written by:
+ * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
+ * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
+ */
+
+#ifndef _AT86RF230_H
+#define _AT86RF230_H
+
+#define RG_TRX_STATUS	(0x01)
+#define SR_TRX_STATUS		0x01, 0x1f, 0
+#define SR_RESERVED_01_3	0x01, 0x20, 5
+#define SR_CCA_STATUS		0x01, 0x40, 6
+#define SR_CCA_DONE		0x01, 0x80, 7
+#define RG_TRX_STATE	(0x02)
+#define SR_TRX_CMD		0x02, 0x1f, 0
+#define SR_TRAC_STATUS		0x02, 0xe0, 5
+#define RG_TRX_CTRL_0	(0x03)
+#define SR_CLKM_CTRL		0x03, 0x07, 0
+#define SR_CLKM_SHA_SEL		0x03, 0x08, 3
+#define SR_PAD_IO_CLKM		0x03, 0x30, 4
+#define SR_PAD_IO		0x03, 0xc0, 6
+#define RG_TRX_CTRL_1	(0x04)
+#define SR_IRQ_POLARITY		0x04, 0x01, 0
+#define SR_IRQ_MASK_MODE	0x04, 0x02, 1
+#define SR_SPI_CMD_MODE		0x04, 0x0c, 2
+#define SR_RX_BL_CTRL		0x04, 0x10, 4
+#define SR_TX_AUTO_CRC_ON	0x04, 0x20, 5
+#define SR_IRQ_2_EXT_EN		0x04, 0x40, 6
+#define SR_PA_EXT_EN		0x04, 0x80, 7
+#define RG_PHY_TX_PWR	(0x05)
+#define SR_TX_PWR_23X		0x05, 0x0f, 0
+#define SR_PA_LT_230		0x05, 0x30, 4
+#define SR_PA_BUF_LT_230	0x05, 0xc0, 6
+#define SR_TX_PWR_212		0x05, 0x1f, 0
+#define SR_GC_PA_212		0x05, 0x60, 5
+#define SR_PA_BOOST_LT_212	0x05, 0x80, 7
+#define RG_PHY_RSSI	(0x06)
+#define SR_RSSI			0x06, 0x1f, 0
+#define SR_RND_VALUE		0x06, 0x60, 5
+#define SR_RX_CRC_VALID		0x06, 0x80, 7
+#define RG_PHY_ED_LEVEL	(0x07)
+#define SR_ED_LEVEL		0x07, 0xff, 0
+#define RG_PHY_CC_CCA	(0x08)
+#define SR_CHANNEL		0x08, 0x1f, 0
+#define SR_CCA_MODE		0x08, 0x60, 5
+#define SR_CCA_REQUEST		0x08, 0x80, 7
+#define RG_CCA_THRES	(0x09)
+#define SR_CCA_ED_THRES		0x09, 0x0f, 0
+#define SR_RESERVED_09_1	0x09, 0xf0, 4
+#define RG_RX_CTRL	(0x0a)
+#define SR_PDT_THRES		0x0a, 0x0f, 0
+#define SR_RESERVED_0a_1	0x0a, 0xf0, 4
+#define RG_SFD_VALUE	(0x0b)
+#define SR_SFD_VALUE		0x0b, 0xff, 0
+#define RG_TRX_CTRL_2	(0x0c)
+#define SR_OQPSK_DATA_RATE	0x0c, 0x03, 0
+#define SR_SUB_MODE		0x0c, 0x04, 2
+#define SR_BPSK_QPSK		0x0c, 0x08, 3
+#define SR_OQPSK_SUB1_RC_EN	0x0c, 0x10, 4
+#define SR_RESERVED_0c_5	0x0c, 0x60, 5
+#define SR_RX_SAFE_MODE		0x0c, 0x80, 7
+#define RG_ANT_DIV	(0x0d)
+#define SR_ANT_CTRL		0x0d, 0x03, 0
+#define SR_ANT_EXT_SW_EN	0x0d, 0x04, 2
+#define SR_ANT_DIV_EN		0x0d, 0x08, 3
+#define SR_RESERVED_0d_2	0x0d, 0x70, 4
+#define SR_ANT_SEL		0x0d, 0x80, 7
+#define RG_IRQ_MASK	(0x0e)
+#define SR_IRQ_MASK		0x0e, 0xff, 0
+#define RG_IRQ_STATUS	(0x0f)
+#define SR_IRQ_0_PLL_LOCK	0x0f, 0x01, 0
+#define SR_IRQ_1_PLL_UNLOCK	0x0f, 0x02, 1
+#define SR_IRQ_2_RX_START	0x0f, 0x04, 2
+#define SR_IRQ_3_TRX_END	0x0f, 0x08, 3
+#define SR_IRQ_4_CCA_ED_DONE	0x0f, 0x10, 4
+#define SR_IRQ_5_AMI		0x0f, 0x20, 5
+#define SR_IRQ_6_TRX_UR		0x0f, 0x40, 6
+#define SR_IRQ_7_BAT_LOW	0x0f, 0x80, 7
+#define RG_VREG_CTRL	(0x10)
+#define SR_RESERVED_10_6	0x10, 0x03, 0
+#define SR_DVDD_OK		0x10, 0x04, 2
+#define SR_DVREG_EXT		0x10, 0x08, 3
+#define SR_RESERVED_10_3	0x10, 0x30, 4
+#define SR_AVDD_OK		0x10, 0x40, 6
+#define SR_AVREG_EXT		0x10, 0x80, 7
+#define RG_BATMON	(0x11)
+#define SR_BATMON_VTH		0x11, 0x0f, 0
+#define SR_BATMON_HR		0x11, 0x10, 4
+#define SR_BATMON_OK		0x11, 0x20, 5
+#define SR_RESERVED_11_1	0x11, 0xc0, 6
+#define RG_XOSC_CTRL	(0x12)
+#define SR_XTAL_TRIM		0x12, 0x0f, 0
+#define SR_XTAL_MODE		0x12, 0xf0, 4
+#define RG_RX_SYN	(0x15)
+#define SR_RX_PDT_LEVEL		0x15, 0x0f, 0
+#define SR_RESERVED_15_2	0x15, 0x70, 4
+#define SR_RX_PDT_DIS		0x15, 0x80, 7
+#define RG_XAH_CTRL_1	(0x17)
+#define SR_RESERVED_17_8	0x17, 0x01, 0
+#define SR_AACK_PROM_MODE	0x17, 0x02, 1
+#define SR_AACK_ACK_TIME	0x17, 0x04, 2
+#define SR_RESERVED_17_5	0x17, 0x08, 3
+#define SR_AACK_UPLD_RES_FT	0x17, 0x10, 4
+#define SR_AACK_FLTR_RES_FT	0x17, 0x20, 5
+#define SR_CSMA_LBT_MODE	0x17, 0x40, 6
+#define SR_RESERVED_17_1	0x17, 0x80, 7
+#define RG_FTN_CTRL	(0x18)
+#define SR_RESERVED_18_2	0x18, 0x7f, 0
+#define SR_FTN_START		0x18, 0x80, 7
+#define RG_PLL_CF	(0x1a)
+#define SR_RESERVED_1a_2	0x1a, 0x7f, 0
+#define SR_PLL_CF_START		0x1a, 0x80, 7
+#define RG_PLL_DCU	(0x1b)
+#define SR_RESERVED_1b_3	0x1b, 0x3f, 0
+#define SR_RESERVED_1b_2	0x1b, 0x40, 6
+#define SR_PLL_DCU_START	0x1b, 0x80, 7
+#define RG_PART_NUM	(0x1c)
+#define SR_PART_NUM		0x1c, 0xff, 0
+#define RG_VERSION_NUM	(0x1d)
+#define SR_VERSION_NUM		0x1d, 0xff, 0
+#define RG_MAN_ID_0	(0x1e)
+#define SR_MAN_ID_0		0x1e, 0xff, 0
+#define RG_MAN_ID_1	(0x1f)
+#define SR_MAN_ID_1		0x1f, 0xff, 0
+#define RG_SHORT_ADDR_0	(0x20)
+#define SR_SHORT_ADDR_0		0x20, 0xff, 0
+#define RG_SHORT_ADDR_1	(0x21)
+#define SR_SHORT_ADDR_1		0x21, 0xff, 0
+#define RG_PAN_ID_0	(0x22)
+#define SR_PAN_ID_0		0x22, 0xff, 0
+#define RG_PAN_ID_1	(0x23)
+#define SR_PAN_ID_1		0x23, 0xff, 0
+#define RG_IEEE_ADDR_0	(0x24)
+#define SR_IEEE_ADDR_0		0x24, 0xff, 0
+#define RG_IEEE_ADDR_1	(0x25)
+#define SR_IEEE_ADDR_1		0x25, 0xff, 0
+#define RG_IEEE_ADDR_2	(0x26)
+#define SR_IEEE_ADDR_2		0x26, 0xff, 0
+#define RG_IEEE_ADDR_3	(0x27)
+#define SR_IEEE_ADDR_3		0x27, 0xff, 0
+#define RG_IEEE_ADDR_4	(0x28)
+#define SR_IEEE_ADDR_4		0x28, 0xff, 0
+#define RG_IEEE_ADDR_5	(0x29)
+#define SR_IEEE_ADDR_5		0x29, 0xff, 0
+#define RG_IEEE_ADDR_6	(0x2a)
+#define SR_IEEE_ADDR_6		0x2a, 0xff, 0
+#define RG_IEEE_ADDR_7	(0x2b)
+#define SR_IEEE_ADDR_7		0x2b, 0xff, 0
+#define RG_XAH_CTRL_0	(0x2c)
+#define SR_SLOTTED_OPERATION	0x2c, 0x01, 0
+#define SR_MAX_CSMA_RETRIES	0x2c, 0x0e, 1
+#define SR_MAX_FRAME_RETRIES	0x2c, 0xf0, 4
+#define RG_CSMA_SEED_0	(0x2d)
+#define SR_CSMA_SEED_0		0x2d, 0xff, 0
+#define RG_CSMA_SEED_1	(0x2e)
+#define SR_CSMA_SEED_1		0x2e, 0x07, 0
+#define SR_AACK_I_AM_COORD	0x2e, 0x08, 3
+#define SR_AACK_DIS_ACK		0x2e, 0x10, 4
+#define SR_AACK_SET_PD		0x2e, 0x20, 5
+#define SR_AACK_FVN_MODE	0x2e, 0xc0, 6
+#define RG_CSMA_BE	(0x2f)
+#define SR_MIN_BE		0x2f, 0x0f, 0
+#define SR_MAX_BE		0x2f, 0xf0, 4
+
+#define CMD_REG		0x80
+#define CMD_REG_MASK	0x3f
+#define CMD_WRITE	0x40
+#define CMD_FB		0x20
+
+#define IRQ_BAT_LOW	BIT(7)
+#define IRQ_TRX_UR	BIT(6)
+#define IRQ_AMI		BIT(5)
+#define IRQ_CCA_ED	BIT(4)
+#define IRQ_TRX_END	BIT(3)
+#define IRQ_RX_START	BIT(2)
+#define IRQ_PLL_UNL	BIT(1)
+#define IRQ_PLL_LOCK	BIT(0)
+
+#define IRQ_ACTIVE_HIGH	0
+#define IRQ_ACTIVE_LOW	1
+
+#define STATE_P_ON		0x00	/* BUSY */
+#define STATE_BUSY_RX		0x01
+#define STATE_BUSY_TX		0x02
+#define STATE_FORCE_TRX_OFF	0x03
+#define STATE_FORCE_TX_ON	0x04	/* IDLE */
+/* 0x05 */				/* INVALID_PARAMETER */
+#define STATE_RX_ON		0x06
+/* 0x07 */				/* SUCCESS */
+#define STATE_TRX_OFF		0x08
+#define STATE_TX_ON		0x09
+/* 0x0a - 0x0e */			/* 0x0a - UNSUPPORTED_ATTRIBUTE */
+#define STATE_SLEEP		0x0F
+#define STATE_PREP_DEEP_SLEEP	0x10
+#define STATE_BUSY_RX_AACK	0x11
+#define STATE_BUSY_TX_ARET	0x12
+#define STATE_RX_AACK_ON	0x16
+#define STATE_TX_ARET_ON	0x19
+#define STATE_RX_ON_NOCLK	0x1C
+#define STATE_RX_AACK_ON_NOCLK	0x1D
+#define STATE_BUSY_RX_AACK_NOCLK 0x1E
+#define STATE_TRANSITION_IN_PROGRESS 0x1F
+
+#define TRX_STATE_MASK		(0x1F)
+
+#endif /* !_AT86RF230_H */
diff --git a/drivers/net/ieee802154/atusb.c b/drivers/net/ieee802154/atusb.c
new file mode 100644
index 000000000000..80dfc725b8dc
--- /dev/null
+++ b/drivers/net/ieee802154/atusb.c
@@ -0,0 +1,762 @@
+/*
+ * atusb.c - Driver for the ATUSB IEEE 802.15.4 dongle
+ *
+ * Written 2013 by Werner Almesberger <werner@almesberger.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, version 2
+ *
+ * Based on at86rf230.c and spi_atusb.c.
+ * at86rf230.c is
+ * Copyright (C) 2009 Siemens AG
+ * Written by: Dmitry Eremin-Solenikov <dmitry.baryshkov@siemens.com>
+ *
+ * spi_atusb.c is
+ * Copyright (c) 2011 Richard Sharpe <realrichardsharpe@gmail.com>
+ * Copyright (c) 2011 Stefan Schmidt <stefan@datenfreihafen.org>
+ * Copyright (c) 2011 Werner Almesberger <werner@almesberger.net>
+ *
+ * USB initialization is
+ * Copyright (c) 2013 Alexander Aring <alex.aring@gmail.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/usb.h>
+#include <linux/skbuff.h>
+
+#include <net/cfg802154.h>
+#include <net/mac802154.h>
+
+#include "at86rf230.h"
+#include "atusb.h"
+
+#define ATUSB_JEDEC_ATMEL	0x1f	/* JEDEC manufacturer ID */
+
+#define ATUSB_NUM_RX_URBS	4	/* allow for a bit of local latency */
+#define ATUSB_ALLOC_DELAY_MS	100	/* delay after failed allocation */
+#define ATUSB_TX_TIMEOUT_MS	200	/* on the air timeout */
+
+struct atusb {
+	struct ieee802154_hw *hw;
+	struct usb_device *usb_dev;
+	int shutdown;			/* non-zero if shutting down */
+	int err;			/* set by first error */
+
+	/* RX variables */
+	struct delayed_work work;	/* memory allocations */
+	struct usb_anchor idle_urbs;	/* URBs waiting to be submitted */
+	struct usb_anchor rx_urbs;	/* URBs waiting for reception */
+
+	/* TX variables */
+	struct usb_ctrlrequest tx_dr;
+	struct urb *tx_urb;
+	struct sk_buff *tx_skb;
+	uint8_t tx_ack_seq;		/* current TX ACK sequence number */
+};
+
+/* ----- USB commands without data ----------------------------------------- */
+
+/* To reduce the number of error checks in the code, we record the first error
+ * in atusb->err and reject all subsequent requests until the error is cleared.
+ */
+
+static int atusb_control_msg(struct atusb *atusb, unsigned int pipe,
+			     __u8 request, __u8 requesttype,
+			     __u16 value, __u16 index,
+			     void *data, __u16 size, int timeout)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+	int ret;
+
+	if (atusb->err)
+		return atusb->err;
+
+	ret = usb_control_msg(usb_dev, pipe, request, requesttype,
+			      value, index, data, size, timeout);
+	if (ret < 0) {
+		atusb->err = ret;
+		dev_err(&usb_dev->dev,
+			"atusb_control_msg: req 0x%02x val 0x%x idx 0x%x, error %d\n",
+			request, value, index, ret);
+	}
+	return ret;
+}
+
+static int atusb_command(struct atusb *atusb, uint8_t cmd, uint8_t arg)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+
+	dev_dbg(&usb_dev->dev, "atusb_command: cmd = 0x%x\n", cmd);
+	return atusb_control_msg(atusb, usb_sndctrlpipe(usb_dev, 0),
+				 cmd, ATUSB_REQ_TO_DEV, arg, 0, NULL, 0, 1000);
+}
+
+static int atusb_write_reg(struct atusb *atusb, uint8_t reg, uint8_t value)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+
+	dev_dbg(&usb_dev->dev, "atusb_write_reg: 0x%02x <- 0x%02x\n",
+		reg, value);
+	return atusb_control_msg(atusb, usb_sndctrlpipe(usb_dev, 0),
+				 ATUSB_REG_WRITE, ATUSB_REQ_TO_DEV,
+				 value, reg, NULL, 0, 1000);
+}
+
+static int atusb_read_reg(struct atusb *atusb, uint8_t reg)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+	int ret;
+	uint8_t value;
+
+	dev_dbg(&usb_dev->dev, "atusb: reg = 0x%x\n", reg);
+	ret = atusb_control_msg(atusb, usb_rcvctrlpipe(usb_dev, 0),
+				ATUSB_REG_READ, ATUSB_REQ_FROM_DEV,
+				0, reg, &value, 1, 1000);
+	return ret >= 0 ? value : ret;
+}
+
+static int atusb_write_subreg(struct atusb *atusb, uint8_t reg, uint8_t mask,
+			      uint8_t shift, uint8_t value)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+	uint8_t orig, tmp;
+	int ret = 0;
+
+	dev_dbg(&usb_dev->dev, "atusb_write_subreg: 0x%02x <- 0x%02x\n",
+		reg, value);
+
+	orig = atusb_read_reg(atusb, reg);
+
+	/* Write the value only into that part of the register which is allowed
+	 * by the mask. All other bits stay as before.
+	 */
+	tmp = orig & ~mask;
+	tmp |= (value << shift) & mask;
+
+	if (tmp != orig)
+		ret = atusb_write_reg(atusb, reg, tmp);
+
+	return ret;
+}
+
+static int atusb_get_and_clear_error(struct atusb *atusb)
+{
+	int err = atusb->err;
+
+	atusb->err = 0;
+	return err;
+}
+
+/* ----- skb allocation ---------------------------------------------------- */
+
+#define MAX_PSDU	127
+#define MAX_RX_XFER	(1 + MAX_PSDU + 2 + 1)	/* PHR+PSDU+CRC+LQI */
+
+#define SKB_ATUSB(skb)	(*(struct atusb **)(skb)->cb)
+
+static void atusb_in(struct urb *urb);
+
+static int atusb_submit_rx_urb(struct atusb *atusb, struct urb *urb)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+	struct sk_buff *skb = urb->context;
+	int ret;
+
+	if (!skb) {
+		skb = alloc_skb(MAX_RX_XFER, GFP_KERNEL);
+		if (!skb) {
+			dev_warn_ratelimited(&usb_dev->dev,
+					     "atusb_in: can't allocate skb\n");
+			return -ENOMEM;
+		}
+		skb_put(skb, MAX_RX_XFER);
+		SKB_ATUSB(skb) = atusb;
+	}
+
+	usb_fill_bulk_urb(urb, usb_dev, usb_rcvbulkpipe(usb_dev, 1),
+			  skb->data, MAX_RX_XFER, atusb_in, skb);
+	usb_anchor_urb(urb, &atusb->rx_urbs);
+
+	ret = usb_submit_urb(urb, GFP_KERNEL);
+	if (ret) {
+		usb_unanchor_urb(urb);
+		kfree_skb(skb);
+		urb->context = NULL;
+	}
+	return ret;
+}
+
+static void atusb_work_urbs(struct work_struct *work)
+{
+	struct atusb *atusb =
+	    container_of(to_delayed_work(work), struct atusb, work);
+	struct usb_device *usb_dev = atusb->usb_dev;
+	struct urb *urb;
+	int ret;
+
+	if (atusb->shutdown)
+		return;
+
+	do {
+		urb = usb_get_from_anchor(&atusb->idle_urbs);
+		if (!urb)
+			return;
+		ret = atusb_submit_rx_urb(atusb, urb);
+	} while (!ret);
+
+	usb_anchor_urb(urb, &atusb->idle_urbs);
+	dev_warn_ratelimited(&usb_dev->dev,
+			     "atusb_in: can't allocate/submit URB (%d)\n", ret);
+	schedule_delayed_work(&atusb->work,
+			      msecs_to_jiffies(ATUSB_ALLOC_DELAY_MS) + 1);
+}
+
+/* ----- Asynchronous USB -------------------------------------------------- */
+
+static void atusb_tx_done(struct atusb *atusb, uint8_t seq)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+	uint8_t expect = atusb->tx_ack_seq;
+
+	dev_dbg(&usb_dev->dev, "atusb_tx_done (0x%02x/0x%02x)\n", seq, expect);
+	if (seq == expect) {
+		/* TODO check for ifs handling in firmware */
+		ieee802154_xmit_complete(atusb->hw, atusb->tx_skb, false);
+	} else {
+		/* TODO I experience this case when atusb has a tx complete
+		 * irq before probing, we should fix the firmware it's an
+		 * unlikely case now that seq == expect is then true, but can
+		 * happen and fail with a tx_skb = NULL;
+		 */
+		ieee802154_wake_queue(atusb->hw);
+		if (atusb->tx_skb)
+			dev_kfree_skb_irq(atusb->tx_skb);
+	}
+}
+
+static void atusb_in_good(struct urb *urb)
+{
+	struct usb_device *usb_dev = urb->dev;
+	struct sk_buff *skb = urb->context;
+	struct atusb *atusb = SKB_ATUSB(skb);
+	uint8_t len, lqi;
+
+	if (!urb->actual_length) {
+		dev_dbg(&usb_dev->dev, "atusb_in: zero-sized URB ?\n");
+		return;
+	}
+
+	len = *skb->data;
+
+	if (urb->actual_length == 1) {
+		atusb_tx_done(atusb, len);
+		return;
+	}
+
+	if (len + 1 > urb->actual_length - 1) {
+		dev_dbg(&usb_dev->dev, "atusb_in: frame len %d+1 > URB %u-1\n",
+			len, urb->actual_length);
+		return;
+	}
+
+	if (!ieee802154_is_valid_psdu_len(len)) {
+		dev_dbg(&usb_dev->dev, "atusb_in: frame corrupted\n");
+		return;
+	}
+
+	lqi = skb->data[len + 1];
+	dev_dbg(&usb_dev->dev, "atusb_in: rx len %d lqi 0x%02x\n", len, lqi);
+	skb_pull(skb, 1);	/* remove PHR */
+	skb_trim(skb, len);	/* get payload only */
+	ieee802154_rx_irqsafe(atusb->hw, skb, lqi);
+	urb->context = NULL;	/* skb is gone */
+}
+
+static void atusb_in(struct urb *urb)
+{
+	struct usb_device *usb_dev = urb->dev;
+	struct sk_buff *skb = urb->context;
+	struct atusb *atusb = SKB_ATUSB(skb);
+
+	dev_dbg(&usb_dev->dev, "atusb_in: status %d len %d\n",
+		urb->status, urb->actual_length);
+	if (urb->status) {
+		if (urb->status == -ENOENT) { /* being killed */
+			kfree_skb(skb);
+			urb->context = NULL;
+			return;
+		}
+		dev_dbg(&usb_dev->dev, "atusb_in: URB error %d\n", urb->status);
+	} else {
+		atusb_in_good(urb);
+	}
+
+	usb_anchor_urb(urb, &atusb->idle_urbs);
+	if (!atusb->shutdown)
+		schedule_delayed_work(&atusb->work, 0);
+}
+
+/* ----- URB allocation/deallocation --------------------------------------- */
+
+static void atusb_free_urbs(struct atusb *atusb)
+{
+	struct urb *urb;
+
+	while (1) {
+		urb = usb_get_from_anchor(&atusb->idle_urbs);
+		if (!urb)
+			break;
+		if (urb->context)
+			kfree_skb(urb->context);
+		usb_free_urb(urb);
+	}
+}
+
+static int atusb_alloc_urbs(struct atusb *atusb, int n)
+{
+	struct urb *urb;
+
+	while (n) {
+		urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!urb) {
+			atusb_free_urbs(atusb);
+			return -ENOMEM;
+		}
+		usb_anchor_urb(urb, &atusb->idle_urbs);
+		n--;
+	}
+	return 0;
+}
+
+/* ----- IEEE 802.15.4 interface operations -------------------------------- */
+
+static void atusb_xmit_complete(struct urb *urb)
+{
+	dev_dbg(&urb->dev->dev, "atusb_xmit urb completed");
+}
+
+static int atusb_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
+{
+	struct atusb *atusb = hw->priv;
+	struct usb_device *usb_dev = atusb->usb_dev;
+	int ret;
+
+	dev_dbg(&usb_dev->dev, "atusb_xmit (%d)\n", skb->len);
+	atusb->tx_skb = skb;
+	atusb->tx_ack_seq++;
+	atusb->tx_dr.wIndex = cpu_to_le16(atusb->tx_ack_seq);
+	atusb->tx_dr.wLength = cpu_to_le16(skb->len);
+
+	usb_fill_control_urb(atusb->tx_urb, usb_dev,
+			     usb_sndctrlpipe(usb_dev, 0),
+			     (unsigned char *)&atusb->tx_dr, skb->data,
+			     skb->len, atusb_xmit_complete, NULL);
+	ret = usb_submit_urb(atusb->tx_urb, GFP_ATOMIC);
+	dev_dbg(&usb_dev->dev, "atusb_xmit done (%d)\n", ret);
+	return ret;
+}
+
+static int atusb_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
+{
+	struct atusb *atusb = hw->priv;
+	int ret;
+
+	/* This implicitly sets the CCA (Clear Channel Assessment) mode to 0,
+	 * "Mode 3a, Carrier sense OR energy above threshold".
+	 * We should probably make this configurable. @@@
+	 */
+	ret = atusb_write_reg(atusb, RG_PHY_CC_CCA, channel);
+	if (ret < 0)
+		return ret;
+	msleep(1);	/* @@@ ugly synchronization */
+	return 0;
+}
+
+static int atusb_ed(struct ieee802154_hw *hw, u8 *level)
+{
+	BUG_ON(!level);
+	*level = 0xbe;
+	return 0;
+}
+
+static int atusb_set_hw_addr_filt(struct ieee802154_hw *hw,
+				  struct ieee802154_hw_addr_filt *filt,
+				  unsigned long changed)
+{
+	struct atusb *atusb = hw->priv;
+	struct device *dev = &atusb->usb_dev->dev;
+
+	if (changed & IEEE802154_AFILT_SADDR_CHANGED) {
+		u16 addr = le16_to_cpu(filt->short_addr);
+
+		dev_vdbg(dev, "atusb_set_hw_addr_filt called for saddr\n");
+		atusb_write_reg(atusb, RG_SHORT_ADDR_0, addr);
+		atusb_write_reg(atusb, RG_SHORT_ADDR_1, addr >> 8);
+	}
+
+	if (changed & IEEE802154_AFILT_PANID_CHANGED) {
+		u16 pan = le16_to_cpu(filt->pan_id);
+
+		dev_vdbg(dev, "atusb_set_hw_addr_filt called for pan id\n");
+		atusb_write_reg(atusb, RG_PAN_ID_0, pan);
+		atusb_write_reg(atusb, RG_PAN_ID_1, pan >> 8);
+	}
+
+	if (changed & IEEE802154_AFILT_IEEEADDR_CHANGED) {
+		u8 i, addr[IEEE802154_EXTENDED_ADDR_LEN];
+
+		memcpy(addr, &filt->ieee_addr, IEEE802154_EXTENDED_ADDR_LEN);
+		dev_vdbg(dev, "atusb_set_hw_addr_filt called for IEEE addr\n");
+		for (i = 0; i < 8; i++)
+			atusb_write_reg(atusb, RG_IEEE_ADDR_0 + i, addr[i]);
+	}
+
+	if (changed & IEEE802154_AFILT_PANC_CHANGED) {
+		dev_vdbg(dev,
+			 "atusb_set_hw_addr_filt called for panc change\n");
+		if (filt->pan_coord)
+			atusb_write_subreg(atusb, SR_AACK_I_AM_COORD, 1);
+		else
+			atusb_write_subreg(atusb, SR_AACK_I_AM_COORD, 0);
+	}
+
+	return atusb_get_and_clear_error(atusb);
+}
+
+static int atusb_start(struct ieee802154_hw *hw)
+{
+	struct atusb *atusb = hw->priv;
+	struct usb_device *usb_dev = atusb->usb_dev;
+	int ret;
+
+	dev_dbg(&usb_dev->dev, "atusb_start\n");
+	schedule_delayed_work(&atusb->work, 0);
+	atusb_command(atusb, ATUSB_RX_MODE, 1);
+	ret = atusb_get_and_clear_error(atusb);
+	if (ret < 0)
+		usb_kill_anchored_urbs(&atusb->idle_urbs);
+	return ret;
+}
+
+static void atusb_stop(struct ieee802154_hw *hw)
+{
+	struct atusb *atusb = hw->priv;
+	struct usb_device *usb_dev = atusb->usb_dev;
+
+	dev_dbg(&usb_dev->dev, "atusb_stop\n");
+	usb_kill_anchored_urbs(&atusb->idle_urbs);
+	atusb_command(atusb, ATUSB_RX_MODE, 0);
+	atusb_get_and_clear_error(atusb);
+}
+
+#define ATUSB_MAX_TX_POWERS 0xF
+static const s32 atusb_powers[ATUSB_MAX_TX_POWERS + 1] = {
+	300, 280, 230, 180, 130, 70, 0, -100, -200, -300, -400, -500, -700,
+	-900, -1200, -1700,
+};
+
+static int
+atusb_set_txpower(struct ieee802154_hw *hw, s32 mbm)
+{
+	struct atusb *atusb = hw->priv;
+	u32 i;
+
+	for (i = 0; i < hw->phy->supported.tx_powers_size; i++) {
+		if (hw->phy->supported.tx_powers[i] == mbm)
+			return atusb_write_subreg(atusb, SR_TX_PWR_23X, i);
+	}
+
+	return -EINVAL;
+}
+
+static int
+atusb_set_promiscuous_mode(struct ieee802154_hw *hw, const bool on)
+{
+	struct atusb *atusb = hw->priv;
+	int ret;
+
+	if (on) {
+		ret = atusb_write_subreg(atusb, SR_AACK_DIS_ACK, 1);
+		if (ret < 0)
+			return ret;
+
+		ret = atusb_write_subreg(atusb, SR_AACK_PROM_MODE, 1);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = atusb_write_subreg(atusb, SR_AACK_PROM_MODE, 0);
+		if (ret < 0)
+			return ret;
+
+		ret = atusb_write_subreg(atusb, SR_AACK_DIS_ACK, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static struct ieee802154_ops atusb_ops = {
+	.owner			= THIS_MODULE,
+	.xmit_async		= atusb_xmit,
+	.ed			= atusb_ed,
+	.set_channel		= atusb_channel,
+	.start			= atusb_start,
+	.stop			= atusb_stop,
+	.set_hw_addr_filt	= atusb_set_hw_addr_filt,
+	.set_txpower		= atusb_set_txpower,
+	.set_promiscuous_mode	= atusb_set_promiscuous_mode,
+};
+
+/* ----- Firmware and chip version information ----------------------------- */
+
+static int atusb_get_and_show_revision(struct atusb *atusb)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+	unsigned char buffer[3];
+	int ret;
+
+	/* Get a couple of the ATMega Firmware values */
+	ret = atusb_control_msg(atusb, usb_rcvctrlpipe(usb_dev, 0),
+				ATUSB_ID, ATUSB_REQ_FROM_DEV, 0, 0,
+				buffer, 3, 1000);
+	if (ret >= 0)
+		dev_info(&usb_dev->dev,
+			 "Firmware: major: %u, minor: %u, hardware type: %u\n",
+			 buffer[0], buffer[1], buffer[2]);
+	if (buffer[0] == 0 && buffer[1] < 2) {
+		dev_info(&usb_dev->dev,
+			 "Firmware version (%u.%u) is predates our first public release.",
+			 buffer[0], buffer[1]);
+		dev_info(&usb_dev->dev, "Please update to version 0.2 or newer");
+	}
+
+	return ret;
+}
+
+static int atusb_get_and_show_build(struct atusb *atusb)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+	char build[ATUSB_BUILD_SIZE + 1];
+	int ret;
+
+	ret = atusb_control_msg(atusb, usb_rcvctrlpipe(usb_dev, 0),
+				ATUSB_BUILD, ATUSB_REQ_FROM_DEV, 0, 0,
+				build, ATUSB_BUILD_SIZE, 1000);
+	if (ret >= 0) {
+		build[ret] = 0;
+		dev_info(&usb_dev->dev, "Firmware: build %s\n", build);
+	}
+
+	return ret;
+}
+
+static int atusb_get_and_show_chip(struct atusb *atusb)
+{
+	struct usb_device *usb_dev = atusb->usb_dev;
+	uint8_t man_id_0, man_id_1, part_num, version_num;
+
+	man_id_0 = atusb_read_reg(atusb, RG_MAN_ID_0);
+	man_id_1 = atusb_read_reg(atusb, RG_MAN_ID_1);
+	part_num = atusb_read_reg(atusb, RG_PART_NUM);
+	version_num = atusb_read_reg(atusb, RG_VERSION_NUM);
+
+	if (atusb->err)
+		return atusb->err;
+
+	if ((man_id_1 << 8 | man_id_0) != ATUSB_JEDEC_ATMEL) {
+		dev_err(&usb_dev->dev,
+			"non-Atmel transceiver xxxx%02x%02x\n",
+			man_id_1, man_id_0);
+		goto fail;
+	}
+	if (part_num != 3 && part_num != 2) {
+		dev_err(&usb_dev->dev,
+			"unexpected transceiver, part 0x%02x version 0x%02x\n",
+			part_num, version_num);
+		goto fail;
+	}
+
+	dev_info(&usb_dev->dev, "ATUSB: AT86RF231 version %d\n", version_num);
+
+	return 0;
+
+fail:
+	atusb->err = -ENODEV;
+	return -ENODEV;
+}
+
+/* ----- Setup ------------------------------------------------------------- */
+
+static int atusb_probe(struct usb_interface *interface,
+		       const struct usb_device_id *id)
+{
+	struct usb_device *usb_dev = interface_to_usbdev(interface);
+	struct ieee802154_hw *hw;
+	struct atusb *atusb = NULL;
+	int ret = -ENOMEM;
+
+	hw = ieee802154_alloc_hw(sizeof(struct atusb), &atusb_ops);
+	if (!hw)
+		return -ENOMEM;
+
+	atusb = hw->priv;
+	atusb->hw = hw;
+	atusb->usb_dev = usb_get_dev(usb_dev);
+	usb_set_intfdata(interface, atusb);
+
+	atusb->shutdown = 0;
+	atusb->err = 0;
+	INIT_DELAYED_WORK(&atusb->work, atusb_work_urbs);
+	init_usb_anchor(&atusb->idle_urbs);
+	init_usb_anchor(&atusb->rx_urbs);
+
+	if (atusb_alloc_urbs(atusb, ATUSB_NUM_RX_URBS))
+		goto fail;
+
+	atusb->tx_dr.bRequestType = ATUSB_REQ_TO_DEV;
+	atusb->tx_dr.bRequest = ATUSB_TX;
+	atusb->tx_dr.wValue = cpu_to_le16(0);
+
+	atusb->tx_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!atusb->tx_urb)
+		goto fail;
+
+	hw->parent = &usb_dev->dev;
+	hw->flags = IEEE802154_HW_TX_OMIT_CKSUM | IEEE802154_HW_AFILT |
+		    IEEE802154_HW_PROMISCUOUS;
+
+	hw->phy->flags = WPAN_PHY_FLAG_TXPOWER;
+
+	hw->phy->current_page = 0;
+	hw->phy->current_channel = 11;	/* reset default */
+	hw->phy->supported.channels[0] = 0x7FFF800;
+	hw->phy->supported.tx_powers = atusb_powers;
+	hw->phy->supported.tx_powers_size = ARRAY_SIZE(atusb_powers);
+	hw->phy->transmit_power = hw->phy->supported.tx_powers[0];
+	ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
+
+	atusb_command(atusb, ATUSB_RF_RESET, 0);
+	atusb_get_and_show_chip(atusb);
+	atusb_get_and_show_revision(atusb);
+	atusb_get_and_show_build(atusb);
+	ret = atusb_get_and_clear_error(atusb);
+	if (ret) {
+		dev_err(&atusb->usb_dev->dev,
+			"%s: initialization failed, error = %d\n",
+			__func__, ret);
+		goto fail;
+	}
+
+	ret = ieee802154_register_hw(hw);
+	if (ret)
+		goto fail;
+
+	/* If we just powered on, we're now in P_ON and need to enter TRX_OFF
+	 * explicitly. Any resets after that will send us straight to TRX_OFF,
+	 * making the command below redundant.
+	 */
+	atusb_write_reg(atusb, RG_TRX_STATE, STATE_FORCE_TRX_OFF);
+	msleep(1);	/* reset => TRX_OFF, tTR13 = 37 us */
+
+#if 0
+	/* Calculating the maximum time available to empty the frame buffer
+	 * on reception:
+	 *
+	 * According to [1], the inter-frame gap is
+	 * R * 20 * 16 us + 128 us
+	 * where R is a random number from 0 to 7. Furthermore, we have 20 bit
+	 * times (80 us at 250 kbps) of SHR of the next frame before the
+	 * transceiver begins storing data in the frame buffer.
+	 *
+	 * This yields a minimum time of 208 us between the last data of a
+	 * frame and the first data of the next frame. This time is further
+	 * reduced by interrupt latency in the atusb firmware.
+	 *
+	 * atusb currently needs about 500 us to retrieve a maximum-sized
+	 * frame. We therefore have to allow reception of a new frame to begin
+	 * while we retrieve the previous frame.
+	 *
+	 * [1] "JN-AN-1035 Calculating data rates in an IEEE 802.15.4-based
+	 *      network", Jennic 2006.
+	 *     http://www.jennic.com/download_file.php?supportFile=JN-AN-1035%20Calculating%20802-15-4%20Data%20Rates-1v0.pdf
+	 */
+
+	atusb_write_subreg(atusb, SR_RX_SAFE_MODE, 1);
+#endif
+	atusb_write_reg(atusb, RG_IRQ_MASK, 0xff);
+
+	ret = atusb_get_and_clear_error(atusb);
+	if (!ret)
+		return 0;
+
+	dev_err(&atusb->usb_dev->dev,
+		"%s: setup failed, error = %d\n",
+		__func__, ret);
+
+	ieee802154_unregister_hw(hw);
+fail:
+	atusb_free_urbs(atusb);
+	usb_kill_urb(atusb->tx_urb);
+	usb_free_urb(atusb->tx_urb);
+	usb_put_dev(usb_dev);
+	ieee802154_free_hw(hw);
+	return ret;
+}
+
+static void atusb_disconnect(struct usb_interface *interface)
+{
+	struct atusb *atusb = usb_get_intfdata(interface);
+
+	dev_dbg(&atusb->usb_dev->dev, "atusb_disconnect\n");
+
+	atusb->shutdown = 1;
+	cancel_delayed_work_sync(&atusb->work);
+
+	usb_kill_anchored_urbs(&atusb->rx_urbs);
+	atusb_free_urbs(atusb);
+	usb_kill_urb(atusb->tx_urb);
+	usb_free_urb(atusb->tx_urb);
+
+	ieee802154_unregister_hw(atusb->hw);
+
+	ieee802154_free_hw(atusb->hw);
+
+	usb_set_intfdata(interface, NULL);
+	usb_put_dev(atusb->usb_dev);
+
+	pr_debug("atusb_disconnect done\n");
+}
+
+/* The devices we work with */
+static const struct usb_device_id atusb_device_table[] = {
+	{
+		.match_flags		= USB_DEVICE_ID_MATCH_DEVICE |
+					  USB_DEVICE_ID_MATCH_INT_INFO,
+		.idVendor		= ATUSB_VENDOR_ID,
+		.idProduct		= ATUSB_PRODUCT_ID,
+		.bInterfaceClass	= USB_CLASS_VENDOR_SPEC
+	},
+	/* end with null element */
+	{}
+};
+MODULE_DEVICE_TABLE(usb, atusb_device_table);
+
+static struct usb_driver atusb_driver = {
+	.name		= "atusb",
+	.probe		= atusb_probe,
+	.disconnect	= atusb_disconnect,
+	.id_table	= atusb_device_table,
+};
+module_usb_driver(atusb_driver);
+
+MODULE_AUTHOR("Alexander Aring <alex.aring@gmail.com>");
+MODULE_AUTHOR("Richard Sharpe <realrichardsharpe@gmail.com>");
+MODULE_AUTHOR("Stefan Schmidt <stefan@datenfreihafen.org>");
+MODULE_AUTHOR("Werner Almesberger <werner@almesberger.net>");
+MODULE_DESCRIPTION("ATUSB IEEE 802.15.4 Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ieee802154/atusb.h b/drivers/net/ieee802154/atusb.h
new file mode 100644
index 000000000000..0690edcad57b
--- /dev/null
+++ b/drivers/net/ieee802154/atusb.h
@@ -0,0 +1,84 @@
+/*
+ * atusb.h - Definitions shared between kernel and ATUSB firmware
+ *
+ * Written 2013 by Werner Almesberger <werner@almesberger.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, version 2, or
+ * (at your option) any later version.
+ *
+ * This file should be identical for kernel and firmware.
+ * Kernel: drivers/net/ieee802154/atusb.h
+ * Firmware: ben-wpan/atusb/fw/include/atusb/atusb.h
+ */
+
+#ifndef _ATUSB_H
+#define _ATUSB_H
+
+#define ATUSB_VENDOR_ID	0x20b7	/* Qi Hardware*/
+#define ATUSB_PRODUCT_ID 0x1540	/* 802.15.4, device 0 */
+				/*     -- -         - */
+
+#define ATUSB_BUILD_SIZE 256	/* maximum build version/date message length */
+
+/* Commands to our device. Make sure this is synced with the firmware */
+enum atusb_requests {
+	ATUSB_ID			= 0x00,	/* system status/control grp */
+	ATUSB_BUILD,
+	ATUSB_RESET,
+	ATUSB_RF_RESET			= 0x10,	/* debug/test group */
+	ATUSB_POLL_INT,
+	ATUSB_TEST,			/* atusb-sil only */
+	ATUSB_TIMER,
+	ATUSB_GPIO,
+	ATUSB_SLP_TR,
+	ATUSB_GPIO_CLEANUP,
+	ATUSB_REG_WRITE			= 0x20,	/* transceiver group */
+	ATUSB_REG_READ,
+	ATUSB_BUF_WRITE,
+	ATUSB_BUF_READ,
+	ATUSB_SRAM_WRITE,
+	ATUSB_SRAM_READ,
+	ATUSB_SPI_WRITE			= 0x30,	/* SPI group */
+	ATUSB_SPI_READ1,
+	ATUSB_SPI_READ2,
+	ATUSB_SPI_WRITE2_SYNC,
+	ATUSB_RX_MODE			= 0x40, /* HardMAC group */
+	ATUSB_TX,
+};
+
+/* Direction	bRequest		wValue		wIndex	wLength
+ *
+ * ->host	ATUSB_ID		-		-	3
+ * ->host	ATUSB_BUILD		-		-	#bytes
+ * host->	ATUSB_RESET		-		-	0
+ *
+ * host->	ATUSB_RF_RESET		-		-	0
+ * ->host	ATUSB_POLL_INT		-		-	1
+ * host->	ATUSB_TEST		-		-	0
+ * ->host	ATUSB_TIMER		-		-	#bytes (6)
+ * ->host	ATUSB_GPIO		dir+data	mask+p#	3
+ * host->	ATUSB_SLP_TR		-		-	0
+ * host->	ATUSB_GPIO_CLEANUP	-		-	0
+ *
+ * host->	ATUSB_REG_WRITE		value		addr	0
+ * ->host	ATUSB_REG_READ		-		addr	1
+ * host->	ATUSB_BUF_WRITE		-		-	#bytes
+ * ->host	ATUSB_BUF_READ		-		-	#bytes
+ * host->	ATUSB_SRAM_WRITE	-		addr	#bytes
+ * ->host	ATUSB_SRAM_READ		-		addr	#bytes
+ *
+ * host->	ATUSB_SPI_WRITE		byte0		byte1	#bytes
+ * ->host	ATUSB_SPI_READ1		byte0		-	#bytes
+ * ->host	ATUSB_SPI_READ2		byte0		byte1	#bytes
+ * ->host	ATUSB_SPI_WRITE2_SYNC	byte0		byte1	0/1
+ *
+ * host->	ATUSB_RX_MODE		on		-	0
+ * host->	ATUSB_TX		flags		ack_seq	#bytes
+ */
+
+#define ATUSB_REQ_FROM_DEV	(USB_TYPE_VENDOR | USB_DIR_IN)
+#define ATUSB_REQ_TO_DEV	(USB_TYPE_VENDOR | USB_DIR_OUT)
+
+#endif /* !_ATUSB_H */
diff --git a/drivers/net/ieee802154/cc2520.c b/drivers/net/ieee802154/cc2520.c
index f833b8bb6663..b6fc29579667 100644
--- a/drivers/net/ieee802154/cc2520.c
+++ b/drivers/net/ieee802154/cc2520.c
@@ -196,6 +196,7 @@ struct cc2520_private {
 	u8 *buf;			/* SPI TX/Rx data buffer */
 	struct mutex buffer_mutex;	/* SPI buffer mutex */
 	bool is_tx;			/* Flag for sync b/w Tx and Rx */
+	bool amplified;			/* Flag for CC2591 */
 	int fifo_pin;			/* FIFO GPIO pin number */
 	struct work_struct fifop_irqwork;/* Workqueue for FIFOP */
 	spinlock_t lock;		/* Lock for is_tx*/
@@ -589,22 +590,23 @@ cc2520_filter(struct ieee802154_hw *hw,
 	      struct ieee802154_hw_addr_filt *filt, unsigned long changed)
 {
 	struct cc2520_private *priv = hw->priv;
+	int ret = 0;
 
 	if (changed & IEEE802154_AFILT_PANID_CHANGED) {
 		u16 panid = le16_to_cpu(filt->pan_id);
 
 		dev_vdbg(&priv->spi->dev,
 			 "cc2520_filter called for pan id\n");
-		cc2520_write_ram(priv, CC2520RAM_PANID,
-				 sizeof(panid), (u8 *)&panid);
+		ret = cc2520_write_ram(priv, CC2520RAM_PANID,
+				       sizeof(panid), (u8 *)&panid);
 	}
 
 	if (changed & IEEE802154_AFILT_IEEEADDR_CHANGED) {
 		dev_vdbg(&priv->spi->dev,
 			 "cc2520_filter called for IEEE addr\n");
-		cc2520_write_ram(priv, CC2520RAM_IEEEADDR,
-				 sizeof(filt->ieee_addr),
-				 (u8 *)&filt->ieee_addr);
+		ret = cc2520_write_ram(priv, CC2520RAM_IEEEADDR,
+				       sizeof(filt->ieee_addr),
+				       (u8 *)&filt->ieee_addr);
 	}
 
 	if (changed & IEEE802154_AFILT_SADDR_CHANGED) {
@@ -612,20 +614,113 @@ cc2520_filter(struct ieee802154_hw *hw,
 
 		dev_vdbg(&priv->spi->dev,
 			 "cc2520_filter called for saddr\n");
-		cc2520_write_ram(priv, CC2520RAM_SHORTADDR,
-				 sizeof(addr), (u8 *)&addr);
+		ret = cc2520_write_ram(priv, CC2520RAM_SHORTADDR,
+				       sizeof(addr), (u8 *)&addr);
 	}
 
 	if (changed & IEEE802154_AFILT_PANC_CHANGED) {
 		dev_vdbg(&priv->spi->dev,
 			 "cc2520_filter called for panc change\n");
 		if (filt->pan_coord)
-			cc2520_write_register(priv, CC2520_FRMFILT0, 0x02);
+			ret = cc2520_write_register(priv, CC2520_FRMFILT0,
+						    0x02);
 		else
-			cc2520_write_register(priv, CC2520_FRMFILT0, 0x00);
+			ret = cc2520_write_register(priv, CC2520_FRMFILT0,
+						    0x00);
 	}
 
-	return 0;
+	return ret;
+}
+
+static inline int cc2520_set_tx_power(struct cc2520_private *priv, s32 mbm)
+{
+	u8 power;
+
+	switch (mbm) {
+	case 500:
+		power = 0xF7;
+		break;
+	case 300:
+		power = 0xF2;
+		break;
+	case 200:
+		power = 0xAB;
+		break;
+	case 100:
+		power = 0x13;
+		break;
+	case 0:
+		power = 0x32;
+		break;
+	case -200:
+		power = 0x81;
+		break;
+	case -400:
+		power = 0x88;
+		break;
+	case -700:
+		power = 0x2C;
+		break;
+	case -1800:
+		power = 0x03;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return cc2520_write_register(priv, CC2520_TXPOWER, power);
+}
+
+static inline int cc2520_cc2591_set_tx_power(struct cc2520_private *priv,
+					     s32 mbm)
+{
+	u8 power;
+
+	switch (mbm) {
+	case 1700:
+		power = 0xF9;
+		break;
+	case 1600:
+		power = 0xF0;
+		break;
+	case 1400:
+		power = 0xA0;
+		break;
+	case 1100:
+		power = 0x2C;
+		break;
+	case -100:
+		power = 0x03;
+		break;
+	case -800:
+		power = 0x01;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return cc2520_write_register(priv, CC2520_TXPOWER, power);
+}
+
+#define CC2520_MAX_TX_POWERS 0x8
+static const s32 cc2520_powers[CC2520_MAX_TX_POWERS + 1] = {
+	500, 300, 200, 100, 0, -200, -400, -700, -1800,
+};
+
+#define CC2520_CC2591_MAX_TX_POWERS 0x5
+static const s32 cc2520_cc2591_powers[CC2520_CC2591_MAX_TX_POWERS + 1] = {
+	1700, 1600, 1400, 1100, -100, -800,
+};
+
+static int
+cc2520_set_txpower(struct ieee802154_hw *hw, s32 mbm)
+{
+	struct cc2520_private *priv = hw->priv;
+
+	if (!priv->amplified)
+		return cc2520_set_tx_power(priv, mbm);
+
+	return cc2520_cc2591_set_tx_power(priv, mbm);
 }
 
 static const struct ieee802154_ops cc2520_ops = {
@@ -636,6 +731,7 @@ static const struct ieee802154_ops cc2520_ops = {
 	.ed = cc2520_ed,
 	.set_channel = cc2520_set_channel,
 	.set_hw_addr_filt = cc2520_filter,
+	.set_txpower = cc2520_set_txpower,
 };
 
 static int cc2520_register(struct cc2520_private *priv)
@@ -649,13 +745,25 @@ static int cc2520_register(struct cc2520_private *priv)
 	priv->hw->priv = 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;
-	priv->hw->flags = IEEE802154_HW_OMIT_CKSUM | IEEE802154_HW_AACK |
-			  IEEE802154_HW_AFILT;
+	priv->hw->phy->supported.channels[0] = 0x7FFF800;
+	priv->hw->flags = IEEE802154_HW_OMIT_CKSUM | IEEE802154_HW_AFILT;
+
+	priv->hw->phy->flags = WPAN_PHY_FLAG_TXPOWER;
+
+	if (!priv->amplified) {
+		priv->hw->phy->supported.tx_powers = cc2520_powers;
+		priv->hw->phy->supported.tx_powers_size = ARRAY_SIZE(cc2520_powers);
+		priv->hw->phy->transmit_power = priv->hw->phy->supported.tx_powers[4];
+	} else {
+		priv->hw->phy->supported.tx_powers = cc2520_cc2591_powers;
+		priv->hw->phy->supported.tx_powers_size = ARRAY_SIZE(cc2520_cc2591_powers);
+		priv->hw->phy->transmit_power = priv->hw->phy->supported.tx_powers[0];
+	}
+
+	priv->hw->phy->current_channel = 11;
 
 	dev_vdbg(&priv->spi->dev, "registered cc2520\n");
 	ret = ieee802154_register_hw(priv->hw);
@@ -738,7 +846,9 @@ static int cc2520_get_platform_data(struct spi_device *spi,
 	pdata->vreg = of_get_named_gpio(np, "vreg-gpio", 0);
 	pdata->reset = of_get_named_gpio(np, "reset-gpio", 0);
 
-	pdata->amplified = of_property_read_bool(np, "amplified");
+	/* CC2591 front end for CC2520 */
+	if (of_property_read_bool(np, "amplified"))
+		priv->amplified = true;
 
 	return 0;
 }
@@ -781,11 +891,7 @@ static int cc2520_hw_init(struct cc2520_private *priv)
 	 * amplifier. See section 8 page 17 of TI application note AN065.
 	 * http://www.ti.com/lit/an/swra229a/swra229a.pdf
 	 */
-	if (pdata.amplified) {
-		ret = cc2520_write_register(priv, CC2520_TXPOWER, 0xF9);
-		if (ret)
-			goto err_ret;
-
+	if (priv->amplified) {
 		ret = cc2520_write_register(priv, CC2520_AGCCTRL1, 0x16);
 		if (ret)
 			goto err_ret;
@@ -806,10 +912,6 @@ static int cc2520_hw_init(struct cc2520_private *priv)
 		if (ret)
 			goto err_ret;
 	} else {
-		ret = cc2520_write_register(priv, CC2520_TXPOWER, 0xF7);
-		if (ret)
-			goto err_ret;
-
 		ret = cc2520_write_register(priv, CC2520_AGCCTRL1, 0x11);
 		if (ret)
 			goto err_ret;
@@ -904,6 +1006,9 @@ static int cc2520_probe(struct spi_device *spi)
 	spin_lock_init(&priv->lock);
 	init_completion(&priv->tx_complete);
 
+	/* Assumption that CC2591 is not connected */
+	priv->amplified = false;
+
 	/* Request all the gpio's */
 	if (!gpio_is_valid(pdata.fifo)) {
 		dev_err(&spi->dev, "fifo gpio is not valid\n");
diff --git a/drivers/net/ieee802154/fakelb.c b/drivers/net/ieee802154/fakelb.c
index dc2bfb600b4b..860d4aed8274 100644
--- a/drivers/net/ieee802154/fakelb.c
+++ b/drivers/net/ieee802154/fakelb.c
@@ -27,25 +27,27 @@
 #include <net/mac802154.h>
 #include <net/cfg802154.h>
 
-static int numlbs = 1;
+static int numlbs = 2;
 
-struct fakelb_dev_priv {
+static LIST_HEAD(fakelb_phys);
+static DEFINE_SPINLOCK(fakelb_phys_lock);
+
+static LIST_HEAD(fakelb_ifup_phys);
+static DEFINE_RWLOCK(fakelb_ifup_phys_lock);
+
+struct fakelb_phy {
 	struct ieee802154_hw *hw;
 
-	struct list_head list;
-	struct fakelb_priv *fake;
+	u8 page;
+	u8 channel;
 
-	spinlock_t lock;
-	bool working;
-};
+	bool suspended;
 
-struct fakelb_priv {
 	struct list_head list;
-	rwlock_t lock;
+	struct list_head list_ifup;
 };
 
-static int
-fakelb_hw_ed(struct ieee802154_hw *hw, u8 *level)
+static int fakelb_hw_ed(struct ieee802154_hw *hw, u8 *level)
 {
 	BUG_ON(!level);
 	*level = 0xbe;
@@ -53,78 +55,66 @@ fakelb_hw_ed(struct ieee802154_hw *hw, u8 *level)
 	return 0;
 }
 
-static int
-fakelb_hw_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
+static int fakelb_hw_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
 {
-	pr_debug("set channel to %d\n", channel);
+	struct fakelb_phy *phy = hw->priv;
 
+	write_lock_bh(&fakelb_ifup_phys_lock);
+	phy->page = page;
+	phy->channel = channel;
+	write_unlock_bh(&fakelb_ifup_phys_lock);
 	return 0;
 }
 
-static void
-fakelb_hw_deliver(struct fakelb_dev_priv *priv, struct sk_buff *skb)
+static int fakelb_hw_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
 {
-	struct sk_buff *newskb;
+	struct fakelb_phy *current_phy = hw->priv, *phy;
 
-	spin_lock(&priv->lock);
-	if (priv->working) {
-		newskb = pskb_copy(skb, GFP_ATOMIC);
-		ieee802154_rx_irqsafe(priv->hw, newskb, 0xcc);
-	}
-	spin_unlock(&priv->lock);
-}
+	read_lock_bh(&fakelb_ifup_phys_lock);
+	WARN_ON(current_phy->suspended);
+	list_for_each_entry(phy, &fakelb_ifup_phys, list_ifup) {
+		if (current_phy == phy)
+			continue;
 
-static int
-fakelb_hw_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
-{
-	struct fakelb_dev_priv *priv = hw->priv;
-	struct fakelb_priv *fake = priv->fake;
-
-	read_lock_bh(&fake->lock);
-	if (priv->list.next == priv->list.prev) {
-		/* we are the only one device */
-		fakelb_hw_deliver(priv, skb);
-	} else {
-		struct fakelb_dev_priv *dp;
-		list_for_each_entry(dp, &priv->fake->list, list) {
-			if (dp != priv &&
-			    (dp->hw->phy->current_channel ==
-			     priv->hw->phy->current_channel))
-				fakelb_hw_deliver(dp, skb);
+		if (current_phy->page == phy->page &&
+		    current_phy->channel == phy->channel) {
+			struct sk_buff *newskb = pskb_copy(skb, GFP_ATOMIC);
+
+			if (newskb)
+				ieee802154_rx_irqsafe(phy->hw, newskb, 0xcc);
 		}
 	}
-	read_unlock_bh(&fake->lock);
+	read_unlock_bh(&fakelb_ifup_phys_lock);
 
+	ieee802154_xmit_complete(hw, skb, false);
 	return 0;
 }
 
-static int
-fakelb_hw_start(struct ieee802154_hw *hw) {
-	struct fakelb_dev_priv *priv = hw->priv;
-	int ret = 0;
+static int fakelb_hw_start(struct ieee802154_hw *hw)
+{
+	struct fakelb_phy *phy = hw->priv;
 
-	spin_lock(&priv->lock);
-	if (priv->working)
-		ret = -EBUSY;
-	else
-		priv->working = 1;
-	spin_unlock(&priv->lock);
+	write_lock_bh(&fakelb_ifup_phys_lock);
+	phy->suspended = false;
+	list_add(&phy->list_ifup, &fakelb_ifup_phys);
+	write_unlock_bh(&fakelb_ifup_phys_lock);
 
-	return ret;
+	return 0;
 }
 
-static void
-fakelb_hw_stop(struct ieee802154_hw *hw) {
-	struct fakelb_dev_priv *priv = hw->priv;
+static void fakelb_hw_stop(struct ieee802154_hw *hw)
+{
+	struct fakelb_phy *phy = hw->priv;
 
-	spin_lock(&priv->lock);
-	priv->working = 0;
-	spin_unlock(&priv->lock);
+	write_lock_bh(&fakelb_ifup_phys_lock);
+	phy->suspended = true;
+	list_del(&phy->list_ifup);
+	write_unlock_bh(&fakelb_ifup_phys_lock);
 }
 
 static const struct ieee802154_ops fakelb_ops = {
 	.owner = THIS_MODULE,
-	.xmit_sync = fakelb_hw_xmit,
+	.xmit_async = fakelb_hw_xmit,
 	.ed = fakelb_hw_ed,
 	.set_channel = fakelb_hw_channel,
 	.start = fakelb_hw_start,
@@ -135,54 +125,54 @@ static const struct ieee802154_ops fakelb_ops = {
 module_param(numlbs, int, 0);
 MODULE_PARM_DESC(numlbs, " number of pseudo devices");
 
-static int fakelb_add_one(struct device *dev, struct fakelb_priv *fake)
+static int fakelb_add_one(struct device *dev)
 {
-	struct fakelb_dev_priv *priv;
-	int err;
 	struct ieee802154_hw *hw;
+	struct fakelb_phy *phy;
+	int err;
 
-	hw = ieee802154_alloc_hw(sizeof(*priv), &fakelb_ops);
+	hw = ieee802154_alloc_hw(sizeof(*phy), &fakelb_ops);
 	if (!hw)
 		return -ENOMEM;
 
-	priv = hw->priv;
-	priv->hw = hw;
+	phy = hw->priv;
+	phy->hw = hw;
 
 	/* 868 MHz BPSK	802.15.4-2003 */
-	hw->phy->channels_supported[0] |= 1;
+	hw->phy->supported.channels[0] |= 1;
 	/* 915 MHz BPSK	802.15.4-2003 */
-	hw->phy->channels_supported[0] |= 0x7fe;
+	hw->phy->supported.channels[0] |= 0x7fe;
 	/* 2.4 GHz O-QPSK 802.15.4-2003 */
-	hw->phy->channels_supported[0] |= 0x7FFF800;
+	hw->phy->supported.channels[0] |= 0x7FFF800;
 	/* 868 MHz ASK 802.15.4-2006 */
-	hw->phy->channels_supported[1] |= 1;
+	hw->phy->supported.channels[1] |= 1;
 	/* 915 MHz ASK 802.15.4-2006 */
-	hw->phy->channels_supported[1] |= 0x7fe;
+	hw->phy->supported.channels[1] |= 0x7fe;
 	/* 868 MHz O-QPSK 802.15.4-2006 */
-	hw->phy->channels_supported[2] |= 1;
+	hw->phy->supported.channels[2] |= 1;
 	/* 915 MHz O-QPSK 802.15.4-2006 */
-	hw->phy->channels_supported[2] |= 0x7fe;
+	hw->phy->supported.channels[2] |= 0x7fe;
 	/* 2.4 GHz CSS 802.15.4a-2007 */
-	hw->phy->channels_supported[3] |= 0x3fff;
+	hw->phy->supported.channels[3] |= 0x3fff;
 	/* UWB Sub-gigahertz 802.15.4a-2007 */
-	hw->phy->channels_supported[4] |= 1;
+	hw->phy->supported.channels[4] |= 1;
 	/* UWB Low band 802.15.4a-2007 */
-	hw->phy->channels_supported[4] |= 0x1e;
+	hw->phy->supported.channels[4] |= 0x1e;
 	/* UWB High band 802.15.4a-2007 */
-	hw->phy->channels_supported[4] |= 0xffe0;
+	hw->phy->supported.channels[4] |= 0xffe0;
 	/* 750 MHz O-QPSK 802.15.4c-2009 */
-	hw->phy->channels_supported[5] |= 0xf;
+	hw->phy->supported.channels[5] |= 0xf;
 	/* 750 MHz MPSK 802.15.4c-2009 */
-	hw->phy->channels_supported[5] |= 0xf0;
+	hw->phy->supported.channels[5] |= 0xf0;
 	/* 950 MHz BPSK 802.15.4d-2009 */
-	hw->phy->channels_supported[6] |= 0x3ff;
+	hw->phy->supported.channels[6] |= 0x3ff;
 	/* 950 MHz GFSK 802.15.4d-2009 */
-	hw->phy->channels_supported[6] |= 0x3ffc00;
+	hw->phy->supported.channels[6] |= 0x3ffc00;
 
-	INIT_LIST_HEAD(&priv->list);
-	priv->fake = fake;
-
-	spin_lock_init(&priv->lock);
+	ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
+	/* fake phy channel 13 as default */
+	hw->phy->current_channel = 13;
+	phy->channel = hw->phy->current_channel;
 
 	hw->parent = dev;
 
@@ -190,67 +180,55 @@ static int fakelb_add_one(struct device *dev, struct fakelb_priv *fake)
 	if (err)
 		goto err_reg;
 
-	write_lock_bh(&fake->lock);
-	list_add_tail(&priv->list, &fake->list);
-	write_unlock_bh(&fake->lock);
+	spin_lock(&fakelb_phys_lock);
+	list_add_tail(&phy->list, &fakelb_phys);
+	spin_unlock(&fakelb_phys_lock);
 
 	return 0;
 
 err_reg:
-	ieee802154_free_hw(priv->hw);
+	ieee802154_free_hw(phy->hw);
 	return err;
 }
 
-static void fakelb_del(struct fakelb_dev_priv *priv)
+static void fakelb_del(struct fakelb_phy *phy)
 {
-	write_lock_bh(&priv->fake->lock);
-	list_del(&priv->list);
-	write_unlock_bh(&priv->fake->lock);
+	list_del(&phy->list);
 
-	ieee802154_unregister_hw(priv->hw);
-	ieee802154_free_hw(priv->hw);
+	ieee802154_unregister_hw(phy->hw);
+	ieee802154_free_hw(phy->hw);
 }
 
 static int fakelb_probe(struct platform_device *pdev)
 {
-	struct fakelb_priv *priv;
-	struct fakelb_dev_priv *dp;
-	int err = -ENOMEM;
-	int i;
-
-	priv = devm_kzalloc(&pdev->dev, sizeof(struct fakelb_priv),
-			    GFP_KERNEL);
-	if (!priv)
-		goto err_alloc;
-
-	INIT_LIST_HEAD(&priv->list);
-	rwlock_init(&priv->lock);
+	struct fakelb_phy *phy, *tmp;
+	int err, i;
 
 	for (i = 0; i < numlbs; i++) {
-		err = fakelb_add_one(&pdev->dev, priv);
+		err = fakelb_add_one(&pdev->dev);
 		if (err < 0)
 			goto err_slave;
 	}
 
-	platform_set_drvdata(pdev, priv);
 	dev_info(&pdev->dev, "added ieee802154 hardware\n");
 	return 0;
 
 err_slave:
-	list_for_each_entry(dp, &priv->list, list)
-		fakelb_del(dp);
-err_alloc:
+	spin_lock(&fakelb_phys_lock);
+	list_for_each_entry_safe(phy, tmp, &fakelb_phys, list)
+		fakelb_del(phy);
+	spin_unlock(&fakelb_phys_lock);
 	return err;
 }
 
 static int fakelb_remove(struct platform_device *pdev)
 {
-	struct fakelb_priv *priv = platform_get_drvdata(pdev);
-	struct fakelb_dev_priv *dp, *temp;
-
-	list_for_each_entry_safe(dp, temp, &priv->list, list)
-		fakelb_del(dp);
+	struct fakelb_phy *phy, *tmp;
 
+	spin_lock(&fakelb_phys_lock);
+	list_for_each_entry_safe(phy, tmp, &fakelb_phys, list)
+		fakelb_del(phy);
+	spin_unlock(&fakelb_phys_lock);
 	return 0;
 }
 
diff --git a/drivers/net/ieee802154/mrf24j40.c b/drivers/net/ieee802154/mrf24j40.c
index fba2dfd910f7..2549760e039f 100644
--- a/drivers/net/ieee802154/mrf24j40.c
+++ b/drivers/net/ieee802154/mrf24j40.c
@@ -533,6 +533,7 @@ static int mrf24j40_handle_rx(struct mrf24j40 *devrec)
 	u8 lqi = 0;
 	u8 val;
 	int ret = 0;
+	int ret2;
 	struct sk_buff *skb;
 
 	/* Turn off reception of packets off the air. This prevents the
@@ -569,9 +570,9 @@ static int mrf24j40_handle_rx(struct mrf24j40 *devrec)
 
 out:
 	/* Turn back on reception of packets off the air. */
-	ret = read_short_reg(devrec, REG_BBREG1, &val);
-	if (ret)
-		return ret;
+	ret2 = read_short_reg(devrec, REG_BBREG1, &val);
+	if (ret2)
+		return ret2;
 	val &= ~0x4; /* Clear RXDECINV */
 	write_short_reg(devrec, REG_BBREG1, val);
 
@@ -750,9 +751,8 @@ static int mrf24j40_probe(struct spi_device *spi)
 
 	devrec->hw->priv = devrec;
 	devrec->hw->parent = &devrec->spi->dev;
-	devrec->hw->phy->channels_supported[0] = CHANNEL_MASK;
-	devrec->hw->flags = IEEE802154_HW_OMIT_CKSUM | IEEE802154_HW_AACK |
-			    IEEE802154_HW_AFILT;
+	devrec->hw->phy->supported.channels[0] = CHANNEL_MASK;
+	devrec->hw->flags = IEEE802154_HW_OMIT_CKSUM | IEEE802154_HW_AFILT;
 
 	dev_dbg(printdev(devrec), "registered mrf24j40\n");
 	ret = ieee802154_register_hw(devrec->hw);
diff --git a/drivers/net/ipvlan/ipvlan.h b/drivers/net/ipvlan/ipvlan.h
index 54549a6223dd..953a97492fab 100644
--- a/drivers/net/ipvlan/ipvlan.h
+++ b/drivers/net/ipvlan/ipvlan.h
@@ -39,6 +39,8 @@
 #define IPVLAN_MAC_FILTER_SIZE	(1 << IPVLAN_MAC_FILTER_BITS)
 #define IPVLAN_MAC_FILTER_MASK	(IPVLAN_MAC_FILTER_SIZE - 1)
 
+#define IPVLAN_QBACKLOG_LIMIT	1000
+
 typedef enum {
 	IPVL_IPV6 = 0,
 	IPVL_ICMPV6,
@@ -93,6 +95,8 @@ struct ipvl_port {
 	struct hlist_head	hlhead[IPVLAN_HASH_SIZE];
 	struct list_head	ipvlans;
 	struct rcu_head		rcu;
+	struct work_struct	wq;
+	struct sk_buff_head	backlog;
 	int			count;
 	u16			mode;
 };
@@ -112,6 +116,7 @@ void ipvlan_set_port_mode(struct ipvl_port *port, u32 nval);
 void ipvlan_init_secret(void);
 unsigned int ipvlan_mac_hash(const unsigned char *addr);
 rx_handler_result_t ipvlan_handle_frame(struct sk_buff **pskb);
+void ipvlan_process_multicast(struct work_struct *work);
 int ipvlan_queue_xmit(struct sk_buff *skb, struct net_device *dev);
 void ipvlan_ht_addr_add(struct ipvl_dev *ipvlan, struct ipvl_addr *addr);
 struct ipvl_addr *ipvlan_find_addr(const struct ipvl_dev *ipvlan,
diff --git a/drivers/net/ipvlan/ipvlan_core.c b/drivers/net/ipvlan/ipvlan_core.c
index c30b5c300c05..8afbedad620d 100644
--- a/drivers/net/ipvlan/ipvlan_core.c
+++ b/drivers/net/ipvlan/ipvlan_core.c
@@ -189,62 +189,69 @@ unsigned int ipvlan_mac_hash(const unsigned char *addr)
 	return hash & IPVLAN_MAC_FILTER_MASK;
 }
 
-static void ipvlan_multicast_frame(struct ipvl_port *port, struct sk_buff *skb,
-				   const struct ipvl_dev *in_dev, bool local)
+void ipvlan_process_multicast(struct work_struct *work)
 {
-	struct ethhdr *eth = eth_hdr(skb);
+	struct ipvl_port *port = container_of(work, struct ipvl_port, wq);
+	struct ethhdr *ethh;
 	struct ipvl_dev *ipvlan;
-	struct sk_buff *nskb;
+	struct sk_buff *skb, *nskb;
+	struct sk_buff_head list;
 	unsigned int len;
 	unsigned int mac_hash;
 	int ret;
+	u8 pkt_type;
+	bool hlocal, dlocal;
 
-	if (skb->protocol == htons(ETH_P_PAUSE))
-		return;
-
-	rcu_read_lock();
-	list_for_each_entry_rcu(ipvlan, &port->ipvlans, pnode) {
-		if (local && (ipvlan == in_dev))
-			continue;
+	__skb_queue_head_init(&list);
 
-		mac_hash = ipvlan_mac_hash(eth->h_dest);
-		if (!test_bit(mac_hash, ipvlan->mac_filters))
-			continue;
+	spin_lock_bh(&port->backlog.lock);
+	skb_queue_splice_tail_init(&port->backlog, &list);
+	spin_unlock_bh(&port->backlog.lock);
 
-		ret = NET_RX_DROP;
-		len = skb->len + ETH_HLEN;
-		nskb = skb_clone(skb, GFP_ATOMIC);
-		if (!nskb)
-			goto mcast_acct;
+	while ((skb = __skb_dequeue(&list)) != NULL) {
+		ethh = eth_hdr(skb);
+		hlocal = ether_addr_equal(ethh->h_source, port->dev->dev_addr);
+		mac_hash = ipvlan_mac_hash(ethh->h_dest);
 
-		if (ether_addr_equal(eth->h_dest, ipvlan->phy_dev->broadcast))
-			nskb->pkt_type = PACKET_BROADCAST;
+		if (ether_addr_equal(ethh->h_dest, port->dev->broadcast))
+			pkt_type = PACKET_BROADCAST;
 		else
-			nskb->pkt_type = PACKET_MULTICAST;
-
-		nskb->dev = ipvlan->dev;
-		if (local)
-			ret = dev_forward_skb(ipvlan->dev, nskb);
-		else
-			ret = netif_rx(nskb);
-mcast_acct:
-		ipvlan_count_rx(ipvlan, len, ret == NET_RX_SUCCESS, true);
-	}
-	rcu_read_unlock();
-
-	/* Locally generated? ...Forward a copy to the main-device as
-	 * well. On the RX side we'll ignore it (wont give it to any
-	 * of the virtual devices.
-	 */
-	if (local) {
-		nskb = skb_clone(skb, GFP_ATOMIC);
-		if (nskb) {
-			if (ether_addr_equal(eth->h_dest, port->dev->broadcast))
-				nskb->pkt_type = PACKET_BROADCAST;
+			pkt_type = PACKET_MULTICAST;
+
+		dlocal = false;
+		rcu_read_lock();
+		list_for_each_entry_rcu(ipvlan, &port->ipvlans, pnode) {
+			if (hlocal && (ipvlan->dev == skb->dev)) {
+				dlocal = true;
+				continue;
+			}
+			if (!test_bit(mac_hash, ipvlan->mac_filters))
+				continue;
+
+			ret = NET_RX_DROP;
+			len = skb->len + ETH_HLEN;
+			nskb = skb_clone(skb, GFP_ATOMIC);
+			if (!nskb)
+				goto acct;
+
+			nskb->pkt_type = pkt_type;
+			nskb->dev = ipvlan->dev;
+			if (hlocal)
+				ret = dev_forward_skb(ipvlan->dev, nskb);
 			else
-				nskb->pkt_type = PACKET_MULTICAST;
-
-			dev_forward_skb(port->dev, nskb);
+				ret = netif_rx(nskb);
+acct:
+			ipvlan_count_rx(ipvlan, len, ret == NET_RX_SUCCESS, true);
+		}
+		rcu_read_unlock();
+
+		if (dlocal) {
+			/* If the packet originated here, send it out. */
+			skb->dev = port->dev;
+			skb->pkt_type = pkt_type;
+			dev_queue_xmit(skb);
+		} else {
+			kfree_skb(skb);
 		}
 	}
 }
@@ -446,6 +453,26 @@ out:
 	return ret;
 }
 
+static void ipvlan_multicast_enqueue(struct ipvl_port *port,
+				     struct sk_buff *skb)
+{
+	if (skb->protocol == htons(ETH_P_PAUSE)) {
+		kfree_skb(skb);
+		return;
+	}
+
+	spin_lock(&port->backlog.lock);
+	if (skb_queue_len(&port->backlog) < IPVLAN_QBACKLOG_LIMIT) {
+		__skb_queue_tail(&port->backlog, skb);
+		spin_unlock(&port->backlog.lock);
+		schedule_work(&port->wq);
+	} else {
+		spin_unlock(&port->backlog.lock);
+		atomic_long_inc(&skb->dev->rx_dropped);
+		kfree_skb(skb);
+	}
+}
+
 static int ipvlan_xmit_mode_l3(struct sk_buff *skb, struct net_device *dev)
 {
 	const struct ipvl_dev *ipvlan = netdev_priv(dev);
@@ -493,11 +520,8 @@ static int ipvlan_xmit_mode_l2(struct sk_buff *skb, struct net_device *dev)
 		return dev_forward_skb(ipvlan->phy_dev, skb);
 
 	} else if (is_multicast_ether_addr(eth->h_dest)) {
-		u8 ip_summed = skb->ip_summed;
-
-		skb->ip_summed = CHECKSUM_UNNECESSARY;
-		ipvlan_multicast_frame(ipvlan->port, skb, ipvlan, true);
-		skb->ip_summed = ip_summed;
+		ipvlan_multicast_enqueue(ipvlan->port, skb);
+		return NET_XMIT_SUCCESS;
 	}
 
 	skb->dev = ipvlan->phy_dev;
@@ -581,8 +605,18 @@ static rx_handler_result_t ipvlan_handle_mode_l2(struct sk_buff **pskb,
 	int addr_type;
 
 	if (is_multicast_ether_addr(eth->h_dest)) {
-		if (ipvlan_external_frame(skb, port))
-			ipvlan_multicast_frame(port, skb, NULL, false);
+		if (ipvlan_external_frame(skb, port)) {
+			struct sk_buff *nskb = skb_clone(skb, GFP_ATOMIC);
+
+			/* External frames are queued for device local
+			 * distribution, but a copy is given to master
+			 * straight away to avoid sending duplicates later
+			 * when work-queue processes this frame. This is
+			 * achieved by returning RX_HANDLER_PASS.
+			 */
+			if (nskb)
+				ipvlan_multicast_enqueue(port, nskb);
+		}
 	} else {
 		struct ipvl_addr *addr;
 
diff --git a/drivers/net/ipvlan/ipvlan_main.c b/drivers/net/ipvlan/ipvlan_main.c
index 77b92a0fe557..1acc283160d9 100644
--- a/drivers/net/ipvlan/ipvlan_main.c
+++ b/drivers/net/ipvlan/ipvlan_main.c
@@ -54,6 +54,9 @@ static int ipvlan_port_create(struct net_device *dev)
 	for (idx = 0; idx < IPVLAN_HASH_SIZE; idx++)
 		INIT_HLIST_HEAD(&port->hlhead[idx]);
 
+	skb_queue_head_init(&port->backlog);
+	INIT_WORK(&port->wq, ipvlan_process_multicast);
+
 	err = netdev_rx_handler_register(dev, ipvlan_handle_frame, port);
 	if (err)
 		goto err;
@@ -72,6 +75,8 @@ static void ipvlan_port_destroy(struct net_device *dev)
 
 	dev->priv_flags &= ~IFF_IPVLAN_MASTER;
 	netdev_rx_handler_unregister(dev);
+	cancel_work_sync(&port->wq);
+	__skb_queue_purge(&port->backlog);
 	kfree_rcu(port, rcu);
 }
 
@@ -213,17 +218,6 @@ static void ipvlan_change_rx_flags(struct net_device *dev, int change)
 		dev_set_allmulti(phy_dev, dev->flags & IFF_ALLMULTI? 1 : -1);
 }
 
-static void ipvlan_set_broadcast_mac_filter(struct ipvl_dev *ipvlan, bool set)
-{
-	struct net_device *dev = ipvlan->dev;
-	unsigned int hashbit = ipvlan_mac_hash(dev->broadcast);
-
-	if (set && !test_bit(hashbit, ipvlan->mac_filters))
-		__set_bit(hashbit, ipvlan->mac_filters);
-	else if (!set && test_bit(hashbit, ipvlan->mac_filters))
-		__clear_bit(hashbit, ipvlan->mac_filters);
-}
-
 static void ipvlan_set_multicast_mac_filter(struct net_device *dev)
 {
 	struct ipvl_dev *ipvlan = netdev_priv(dev);
@@ -238,6 +232,12 @@ static void ipvlan_set_multicast_mac_filter(struct net_device *dev)
 		netdev_for_each_mc_addr(ha, dev)
 			__set_bit(ipvlan_mac_hash(ha->addr), mc_filters);
 
+		/* Turn-on broadcast bit irrespective of address family,
+		 * since broadcast is deferred to a work-queue, hence no
+		 * impact on fast-path processing.
+		 */
+		__set_bit(ipvlan_mac_hash(dev->broadcast), mc_filters);
+
 		bitmap_copy(ipvlan->mac_filters, mc_filters,
 			    IPVLAN_MAC_FILTER_SIZE);
 	}
@@ -705,7 +705,6 @@ static int ipvlan_add_addr4(struct ipvl_dev *ipvlan, struct in_addr *ip4_addr)
 	 */
 	if (netif_running(ipvlan->dev))
 		ipvlan_ht_addr_add(ipvlan, addr);
-	ipvlan_set_broadcast_mac_filter(ipvlan, true);
 
 	return 0;
 }
@@ -722,8 +721,6 @@ static void ipvlan_del_addr4(struct ipvl_dev *ipvlan, struct in_addr *ip4_addr)
 	list_del(&addr->anode);
 	ipvlan->ipv4cnt--;
 	WARN_ON(ipvlan->ipv4cnt < 0);
-	if (!ipvlan->ipv4cnt)
-	    ipvlan_set_broadcast_mac_filter(ipvlan, false);
 	kfree_rcu(addr, rcu);
 
 	return;
diff --git a/drivers/net/irda/irda-usb.c b/drivers/net/irda/irda-usb.c
index f6c916312577..25f21968fa5c 100644
--- a/drivers/net/irda/irda-usb.c
+++ b/drivers/net/irda/irda-usb.c
@@ -848,7 +848,9 @@ static void irda_usb_receive(struct urb *urb)
 		 * Jean II */
 		self->rx_defer_timer.function = irda_usb_rx_defer_expired;
 		self->rx_defer_timer.data = (unsigned long) urb;
-		mod_timer(&self->rx_defer_timer, jiffies + (10 * HZ / 1000));
+		mod_timer(&self->rx_defer_timer,
+			  jiffies + msecs_to_jiffies(10));
+
 		return;
 	}
 	
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index 8c350c5d54ad..f8370808a018 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -48,15 +48,70 @@ struct macvtap_queue {
 #define MACVTAP_FEATURES (IFF_VNET_HDR | IFF_MULTI_QUEUE)
 
 #define MACVTAP_VNET_LE 0x80000000
+#define MACVTAP_VNET_BE 0x40000000
+
+#ifdef CONFIG_TUN_VNET_CROSS_LE
+static inline bool macvtap_legacy_is_little_endian(struct macvtap_queue *q)
+{
+	return q->flags & MACVTAP_VNET_BE ? false :
+		virtio_legacy_is_little_endian();
+}
+
+static long macvtap_get_vnet_be(struct macvtap_queue *q, int __user *sp)
+{
+	int s = !!(q->flags & MACVTAP_VNET_BE);
+
+	if (put_user(s, sp))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long macvtap_set_vnet_be(struct macvtap_queue *q, int __user *sp)
+{
+	int s;
+
+	if (get_user(s, sp))
+		return -EFAULT;
+
+	if (s)
+		q->flags |= MACVTAP_VNET_BE;
+	else
+		q->flags &= ~MACVTAP_VNET_BE;
+
+	return 0;
+}
+#else
+static inline bool macvtap_legacy_is_little_endian(struct macvtap_queue *q)
+{
+	return virtio_legacy_is_little_endian();
+}
+
+static long macvtap_get_vnet_be(struct macvtap_queue *q, int __user *argp)
+{
+	return -EINVAL;
+}
+
+static long macvtap_set_vnet_be(struct macvtap_queue *q, int __user *argp)
+{
+	return -EINVAL;
+}
+#endif /* CONFIG_TUN_VNET_CROSS_LE */
+
+static inline bool macvtap_is_little_endian(struct macvtap_queue *q)
+{
+	return q->flags & MACVTAP_VNET_LE ||
+		macvtap_legacy_is_little_endian(q);
+}
 
 static inline u16 macvtap16_to_cpu(struct macvtap_queue *q, __virtio16 val)
 {
-	return __virtio16_to_cpu(q->flags & MACVTAP_VNET_LE, val);
+	return __virtio16_to_cpu(macvtap_is_little_endian(q), val);
 }
 
 static inline __virtio16 cpu_to_macvtap16(struct macvtap_queue *q, u16 val)
 {
-	return __cpu_to_virtio16(q->flags & MACVTAP_VNET_LE, val);
+	return __cpu_to_virtio16(macvtap_is_little_endian(q), val);
 }
 
 static struct proto macvtap_proto = {
@@ -263,27 +318,21 @@ out:
 static void macvtap_del_queues(struct net_device *dev)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
-	struct macvtap_queue *q, *tmp, *qlist[MAX_MACVTAP_QUEUES];
-	int i, j = 0;
+	struct macvtap_queue *q, *tmp;
 
 	ASSERT_RTNL();
 	list_for_each_entry_safe(q, tmp, &vlan->queue_list, next) {
 		list_del_init(&q->next);
-		qlist[j++] = q;
 		RCU_INIT_POINTER(q->vlan, NULL);
 		if (q->enabled)
 			vlan->numvtaps--;
 		vlan->numqueues--;
+		sock_put(&q->sk);
 	}
-	for (i = 0; i < vlan->numvtaps; i++)
-		RCU_INIT_POINTER(vlan->taps[i], NULL);
 	BUG_ON(vlan->numvtaps);
 	BUG_ON(vlan->numqueues);
 	/* guarantee that any future macvtap_set_queue will fail */
 	vlan->numvtaps = MAX_MACVTAP_QUEUES;
-
-	for (--j; j >= 0; j--)
-		sock_put(&qlist[j]->sk);
 }
 
 static rx_handler_result_t macvtap_handle_frame(struct sk_buff **pskb)
@@ -476,7 +525,7 @@ static int macvtap_open(struct inode *inode, struct file *file)
 
 	err = -ENOMEM;
 	q = (struct macvtap_queue *)sk_alloc(net, AF_UNSPEC, GFP_KERNEL,
-					     &macvtap_proto);
+					     &macvtap_proto, 0);
 	if (!q)
 		goto out;
 
@@ -1006,6 +1055,7 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 	unsigned int __user *up = argp;
 	unsigned short u;
 	int __user *sp = argp;
+	struct sockaddr sa;
 	int s;
 	int ret;
 
@@ -1090,6 +1140,12 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 			q->flags &= ~MACVTAP_VNET_LE;
 		return 0;
 
+	case TUNGETVNETBE:
+		return macvtap_get_vnet_be(q, sp);
+
+	case TUNSETVNETBE:
+		return macvtap_set_vnet_be(q, sp);
+
 	case TUNSETOFFLOAD:
 		/* let the user check for future flags */
 		if (arg & ~(TUN_F_CSUM | TUN_F_TSO4 | TUN_F_TSO6 |
@@ -1101,6 +1157,37 @@ static long macvtap_ioctl(struct file *file, unsigned int cmd,
 		rtnl_unlock();
 		return ret;
 
+	case SIOCGIFHWADDR:
+		rtnl_lock();
+		vlan = macvtap_get_vlan(q);
+		if (!vlan) {
+			rtnl_unlock();
+			return -ENOLINK;
+		}
+		ret = 0;
+		u = vlan->dev->type;
+		if (copy_to_user(&ifr->ifr_name, vlan->dev->name, IFNAMSIZ) ||
+		    copy_to_user(&ifr->ifr_hwaddr.sa_data, vlan->dev->dev_addr, ETH_ALEN) ||
+		    put_user(u, &ifr->ifr_hwaddr.sa_family))
+			ret = -EFAULT;
+		macvtap_put_vlan(vlan);
+		rtnl_unlock();
+		return ret;
+
+	case SIOCSIFHWADDR:
+		if (copy_from_user(&sa, &ifr->ifr_hwaddr, sizeof(sa)))
+			return -EFAULT;
+		rtnl_lock();
+		vlan = macvtap_get_vlan(q);
+		if (!vlan) {
+			rtnl_unlock();
+			return -ENOLINK;
+		}
+		ret = dev_set_mac_address(vlan->dev, &sa);
+		macvtap_put_vlan(vlan);
+		rtnl_unlock();
+		return ret;
+
 	default:
 		return -EINVAL;
 	}
diff --git a/drivers/net/netconsole.c b/drivers/net/netconsole.c
index 15731d1db918..97f3acd44798 100644
--- a/drivers/net/netconsole.c
+++ b/drivers/net/netconsole.c
@@ -79,6 +79,12 @@ static LIST_HEAD(target_list);
 /* This needs to be a spinlock because write_msg() cannot sleep */
 static DEFINE_SPINLOCK(target_list_lock);
 
+/*
+ * Console driver for extended netconsoles.  Registered on the first use to
+ * avoid unnecessarily enabling ext message formatting.
+ */
+static struct console netconsole_ext;
+
 /**
  * struct netconsole_target - Represents a configured netconsole target.
  * @list:	Links this target into the target_list.
@@ -104,14 +110,15 @@ struct netconsole_target {
 #ifdef	CONFIG_NETCONSOLE_DYNAMIC
 	struct config_item	item;
 #endif
-	int			enabled;
-	struct mutex		mutex;
+	bool			enabled;
+	bool			extended;
 	struct netpoll		np;
 };
 
 #ifdef	CONFIG_NETCONSOLE_DYNAMIC
 
 static struct configfs_subsystem netconsole_subsys;
+static DEFINE_MUTEX(dynamic_netconsole_mutex);
 
 static int __init dynamic_netconsole_init(void)
 {
@@ -185,9 +192,13 @@ static struct netconsole_target *alloc_param_target(char *target_config)
 	strlcpy(nt->np.dev_name, "eth0", IFNAMSIZ);
 	nt->np.local_port = 6665;
 	nt->np.remote_port = 6666;
-	mutex_init(&nt->mutex);
 	eth_broadcast_addr(nt->np.remote_mac);
 
+	if (*target_config == '+') {
+		nt->extended = true;
+		target_config++;
+	}
+
 	/* Parse parameters and setup netpoll */
 	err = netpoll_parse_options(&nt->np, target_config);
 	if (err)
@@ -197,7 +208,7 @@ static struct netconsole_target *alloc_param_target(char *target_config)
 	if (err)
 		goto fail;
 
-	nt->enabled = 1;
+	nt->enabled = true;
 
 	return nt;
 
@@ -258,6 +269,11 @@ static ssize_t show_enabled(struct netconsole_target *nt, char *buf)
 	return snprintf(buf, PAGE_SIZE, "%d\n", nt->enabled);
 }
 
+static ssize_t show_extended(struct netconsole_target *nt, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", nt->extended);
+}
+
 static ssize_t show_dev_name(struct netconsole_target *nt, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%s\n", nt->np.dev_name);
@@ -322,13 +338,18 @@ static ssize_t store_enabled(struct netconsole_target *nt,
 		return err;
 	if (enabled < 0 || enabled > 1)
 		return -EINVAL;
-	if (enabled == nt->enabled) {
+	if ((bool)enabled == nt->enabled) {
 		pr_info("network logging has already %s\n",
 			nt->enabled ? "started" : "stopped");
 		return -EINVAL;
 	}
 
-	if (enabled) {	/* 1 */
+	if (enabled) {	/* true */
+		if (nt->extended && !(netconsole_ext.flags & CON_ENABLED)) {
+			netconsole_ext.flags |= CON_ENABLED;
+			register_console(&netconsole_ext);
+		}
+
 		/*
 		 * Skip netpoll_parse_options() -- all the attributes are
 		 * already configured via configfs. Just print them out.
@@ -340,13 +361,13 @@ static ssize_t store_enabled(struct netconsole_target *nt,
 			return err;
 
 		pr_info("netconsole: network logging started\n");
-	} else {	/* 0 */
+	} else {	/* false */
 		/* We need to disable the netconsole before cleaning it up
 		 * otherwise we might end up in write_msg() with
-		 * nt->np.dev == NULL and nt->enabled == 1
+		 * nt->np.dev == NULL and nt->enabled == true
 		 */
 		spin_lock_irqsave(&target_list_lock, flags);
-		nt->enabled = 0;
+		nt->enabled = false;
 		spin_unlock_irqrestore(&target_list_lock, flags);
 		netpoll_cleanup(&nt->np);
 	}
@@ -356,6 +377,30 @@ static ssize_t store_enabled(struct netconsole_target *nt,
 	return strnlen(buf, count);
 }
 
+static ssize_t store_extended(struct netconsole_target *nt,
+			      const char *buf,
+			      size_t count)
+{
+	int extended;
+	int err;
+
+	if (nt->enabled) {
+		pr_err("target (%s) is enabled, disable to update parameters\n",
+		       config_item_name(&nt->item));
+		return -EINVAL;
+	}
+
+	err = kstrtoint(buf, 10, &extended);
+	if (err < 0)
+		return err;
+	if (extended < 0 || extended > 1)
+		return -EINVAL;
+
+	nt->extended = extended;
+
+	return strnlen(buf, count);
+}
+
 static ssize_t store_dev_name(struct netconsole_target *nt,
 			      const char *buf,
 			      size_t count)
@@ -508,6 +553,7 @@ static struct netconsole_target_attr netconsole_target_##_name =	\
 	__CONFIGFS_ATTR(_name, S_IRUGO | S_IWUSR, show_##_name, store_##_name)
 
 NETCONSOLE_TARGET_ATTR_RW(enabled);
+NETCONSOLE_TARGET_ATTR_RW(extended);
 NETCONSOLE_TARGET_ATTR_RW(dev_name);
 NETCONSOLE_TARGET_ATTR_RW(local_port);
 NETCONSOLE_TARGET_ATTR_RW(remote_port);
@@ -518,6 +564,7 @@ NETCONSOLE_TARGET_ATTR_RW(remote_mac);
 
 static struct configfs_attribute *netconsole_target_attrs[] = {
 	&netconsole_target_enabled.attr,
+	&netconsole_target_extended.attr,
 	&netconsole_target_dev_name.attr,
 	&netconsole_target_local_port.attr,
 	&netconsole_target_remote_port.attr,
@@ -562,10 +609,10 @@ static ssize_t netconsole_target_attr_store(struct config_item *item,
 	struct netconsole_target_attr *na =
 		container_of(attr, struct netconsole_target_attr, attr);
 
-	mutex_lock(&nt->mutex);
+	mutex_lock(&dynamic_netconsole_mutex);
 	if (na->store)
 		ret = na->store(nt, buf, count);
-	mutex_unlock(&nt->mutex);
+	mutex_unlock(&dynamic_netconsole_mutex);
 
 	return ret;
 }
@@ -594,7 +641,7 @@ static struct config_item *make_netconsole_target(struct config_group *group,
 
 	/*
 	 * Allocate and initialize with defaults.
-	 * Target is disabled at creation (enabled == 0).
+	 * Target is disabled at creation (!enabled).
 	 */
 	nt = kzalloc(sizeof(*nt), GFP_KERNEL);
 	if (!nt)
@@ -604,7 +651,6 @@ static struct config_item *make_netconsole_target(struct config_group *group,
 	strlcpy(nt->np.dev_name, "eth0", IFNAMSIZ);
 	nt->np.local_port = 6665;
 	nt->np.remote_port = 6666;
-	mutex_init(&nt->mutex);
 	eth_broadcast_addr(nt->np.remote_mac);
 
 	/* Initialize the config_item member */
@@ -695,7 +741,7 @@ restart:
 				spin_lock_irqsave(&target_list_lock, flags);
 				dev_put(nt->np.dev);
 				nt->np.dev = NULL;
-				nt->enabled = 0;
+				nt->enabled = false;
 				stopped = true;
 				netconsole_target_put(nt);
 				goto restart;
@@ -729,6 +775,82 @@ static struct notifier_block netconsole_netdev_notifier = {
 	.notifier_call  = netconsole_netdev_event,
 };
 
+/**
+ * send_ext_msg_udp - send extended log message to target
+ * @nt: target to send message to
+ * @msg: extended log message to send
+ * @msg_len: length of message
+ *
+ * Transfer extended log @msg to @nt.  If @msg is longer than
+ * MAX_PRINT_CHUNK, it'll be split and transmitted in multiple chunks with
+ * ncfrag header field added to identify them.
+ */
+static void send_ext_msg_udp(struct netconsole_target *nt, const char *msg,
+			     int msg_len)
+{
+	static char buf[MAX_PRINT_CHUNK]; /* protected by target_list_lock */
+	const char *header, *body;
+	int offset = 0;
+	int header_len, body_len;
+
+	if (msg_len <= MAX_PRINT_CHUNK) {
+		netpoll_send_udp(&nt->np, msg, msg_len);
+		return;
+	}
+
+	/* need to insert extra header fields, detect header and body */
+	header = msg;
+	body = memchr(msg, ';', msg_len);
+	if (WARN_ON_ONCE(!body))
+		return;
+
+	header_len = body - header;
+	body_len = msg_len - header_len - 1;
+	body++;
+
+	/*
+	 * Transfer multiple chunks with the following extra header.
+	 * "ncfrag=<byte-offset>/<total-bytes>"
+	 */
+	memcpy(buf, header, header_len);
+
+	while (offset < body_len) {
+		int this_header = header_len;
+		int this_chunk;
+
+		this_header += scnprintf(buf + this_header,
+					 sizeof(buf) - this_header,
+					 ",ncfrag=%d/%d;", offset, body_len);
+
+		this_chunk = min(body_len - offset,
+				 MAX_PRINT_CHUNK - this_header);
+		if (WARN_ON_ONCE(this_chunk <= 0))
+			return;
+
+		memcpy(buf + this_header, body + offset, this_chunk);
+
+		netpoll_send_udp(&nt->np, buf, this_header + this_chunk);
+
+		offset += this_chunk;
+	}
+}
+
+static void write_ext_msg(struct console *con, const char *msg,
+			  unsigned int len)
+{
+	struct netconsole_target *nt;
+	unsigned long flags;
+
+	if ((oops_only && !oops_in_progress) || list_empty(&target_list))
+		return;
+
+	spin_lock_irqsave(&target_list_lock, flags);
+	list_for_each_entry(nt, &target_list, list)
+		if (nt->extended && nt->enabled && netif_running(nt->np.dev))
+			send_ext_msg_udp(nt, msg, len);
+	spin_unlock_irqrestore(&target_list_lock, flags);
+}
+
 static void write_msg(struct console *con, const char *msg, unsigned int len)
 {
 	int frag, left;
@@ -744,8 +866,7 @@ static void write_msg(struct console *con, const char *msg, unsigned int len)
 
 	spin_lock_irqsave(&target_list_lock, flags);
 	list_for_each_entry(nt, &target_list, list) {
-		netconsole_target_get(nt);
-		if (nt->enabled && netif_running(nt->np.dev)) {
+		if (!nt->extended && nt->enabled && netif_running(nt->np.dev)) {
 			/*
 			 * We nest this inside the for-each-target loop above
 			 * so that we're able to get as much logging out to
@@ -760,11 +881,16 @@ static void write_msg(struct console *con, const char *msg, unsigned int len)
 				left -= frag;
 			}
 		}
-		netconsole_target_put(nt);
 	}
 	spin_unlock_irqrestore(&target_list_lock, flags);
 }
 
+static struct console netconsole_ext = {
+	.name	= "netcon_ext",
+	.flags	= CON_EXTENDED,	/* starts disabled, registered on first use */
+	.write	= write_ext_msg,
+};
+
 static struct console netconsole = {
 	.name	= "netcon",
 	.flags	= CON_ENABLED,
@@ -787,7 +913,11 @@ static int __init init_netconsole(void)
 				goto fail;
 			}
 			/* Dump existing printks when we register */
-			netconsole.flags |= CON_PRINTBUFFER;
+			if (nt->extended)
+				netconsole_ext.flags |= CON_PRINTBUFFER |
+							CON_ENABLED;
+			else
+				netconsole.flags |= CON_PRINTBUFFER;
 
 			spin_lock_irqsave(&target_list_lock, flags);
 			list_add(&nt->list, &target_list);
@@ -803,6 +933,8 @@ static int __init init_netconsole(void)
 	if (err)
 		goto undonotifier;
 
+	if (netconsole_ext.flags & CON_ENABLED)
+		register_console(&netconsole_ext);
 	register_console(&netconsole);
 	pr_info("network logging started\n");
 
@@ -831,6 +963,7 @@ static void __exit cleanup_netconsole(void)
 {
 	struct netconsole_target *nt, *tmp;
 
+	unregister_console(&netconsole_ext);
 	unregister_console(&netconsole);
 	dynamic_netconsole_exit();
 	unregister_netdevice_notifier(&netconsole_netdev_notifier);
diff --git a/drivers/net/ntb_netdev.c b/drivers/net/ntb_netdev.c
index 5a7e6397440a..3cc316cb7e6b 100644
--- a/drivers/net/ntb_netdev.c
+++ b/drivers/net/ntb_netdev.c
@@ -5,6 +5,7 @@
  *   GPL LICENSE SUMMARY
  *
  *   Copyright(c) 2012 Intel Corporation. All rights reserved.
+ *   Copyright (C) 2015 EMC 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
@@ -13,6 +14,7 @@
  *   BSD LICENSE
  *
  *   Copyright(c) 2012 Intel Corporation. All rights reserved.
+ *   Copyright (C) 2015 EMC Corporation. All Rights Reserved.
  *
  *   Redistribution and use in source and binary forms, with or without
  *   modification, are permitted provided that the following conditions
@@ -40,7 +42,7 @@
  *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * Intel PCIe NTB Network Linux driver
+ * PCIe NTB Network Linux driver
  *
  * Contact Information:
  * Jon Mason <jon.mason@intel.com>
@@ -50,6 +52,7 @@
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/ntb.h>
+#include <linux/ntb_transport.h>
 
 #define NTB_NETDEV_VER	"0.7"
 
@@ -70,26 +73,19 @@ struct ntb_netdev {
 
 static LIST_HEAD(dev_list);
 
-static void ntb_netdev_event_handler(void *data, int status)
+static void ntb_netdev_event_handler(void *data, int link_is_up)
 {
 	struct net_device *ndev = data;
 	struct ntb_netdev *dev = netdev_priv(ndev);
 
-	netdev_dbg(ndev, "Event %x, Link %x\n", status,
+	netdev_dbg(ndev, "Event %x, Link %x\n", link_is_up,
 		   ntb_transport_link_query(dev->qp));
 
-	switch (status) {
-	case NTB_LINK_DOWN:
+	if (link_is_up) {
+		if (ntb_transport_link_query(dev->qp))
+			netif_carrier_on(ndev);
+	} else {
 		netif_carrier_off(ndev);
-		break;
-	case NTB_LINK_UP:
-		if (!ntb_transport_link_query(dev->qp))
-			return;
-
-		netif_carrier_on(ndev);
-		break;
-	default:
-		netdev_warn(ndev, "Unsupported event type %d\n", status);
 	}
 }
 
@@ -160,8 +156,6 @@ static netdev_tx_t ntb_netdev_start_xmit(struct sk_buff *skb,
 	struct ntb_netdev *dev = netdev_priv(ndev);
 	int rc;
 
-	netdev_dbg(ndev, "%s: skb len %d\n", __func__, skb->len);
-
 	rc = ntb_transport_tx_enqueue(dev->qp, skb, skb->data, skb->len);
 	if (rc)
 		goto err;
@@ -322,20 +316,26 @@ static const struct ntb_queue_handlers ntb_netdev_handlers = {
 	.event_handler = ntb_netdev_event_handler,
 };
 
-static int ntb_netdev_probe(struct pci_dev *pdev)
+static int ntb_netdev_probe(struct device *client_dev)
 {
+	struct ntb_dev *ntb;
 	struct net_device *ndev;
+	struct pci_dev *pdev;
 	struct ntb_netdev *dev;
 	int rc;
 
-	ndev = alloc_etherdev(sizeof(struct ntb_netdev));
+	ntb = dev_ntb(client_dev->parent);
+	pdev = ntb->pdev;
+	if (!pdev)
+		return -ENODEV;
+
+	ndev = alloc_etherdev(sizeof(*dev));
 	if (!ndev)
 		return -ENOMEM;
 
 	dev = netdev_priv(ndev);
 	dev->ndev = ndev;
 	dev->pdev = pdev;
-	BUG_ON(!dev->pdev);
 	ndev->features = NETIF_F_HIGHDMA;
 
 	ndev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
@@ -349,7 +349,8 @@ static int ntb_netdev_probe(struct pci_dev *pdev)
 	ndev->netdev_ops = &ntb_netdev_ops;
 	ndev->ethtool_ops = &ntb_ethtool_ops;
 
-	dev->qp = ntb_transport_create_queue(ndev, pdev, &ntb_netdev_handlers);
+	dev->qp = ntb_transport_create_queue(ndev, client_dev,
+					     &ntb_netdev_handlers);
 	if (!dev->qp) {
 		rc = -EIO;
 		goto err;
@@ -372,12 +373,17 @@ err:
 	return rc;
 }
 
-static void ntb_netdev_remove(struct pci_dev *pdev)
+static void ntb_netdev_remove(struct device *client_dev)
 {
+	struct ntb_dev *ntb;
 	struct net_device *ndev;
+	struct pci_dev *pdev;
 	struct ntb_netdev *dev;
 	bool found = false;
 
+	ntb = dev_ntb(client_dev->parent);
+	pdev = ntb->pdev;
+
 	list_for_each_entry(dev, &dev_list, list) {
 		if (dev->pdev == pdev) {
 			found = true;
@@ -396,7 +402,7 @@ static void ntb_netdev_remove(struct pci_dev *pdev)
 	free_netdev(ndev);
 }
 
-static struct ntb_client ntb_netdev_client = {
+static struct ntb_transport_client ntb_netdev_client = {
 	.driver.name = KBUILD_MODNAME,
 	.driver.owner = THIS_MODULE,
 	.probe = ntb_netdev_probe,
@@ -407,16 +413,16 @@ static int __init ntb_netdev_init_module(void)
 {
 	int rc;
 
-	rc = ntb_register_client_dev(KBUILD_MODNAME);
+	rc = ntb_transport_register_client_dev(KBUILD_MODNAME);
 	if (rc)
 		return rc;
-	return ntb_register_client(&ntb_netdev_client);
+	return ntb_transport_register_client(&ntb_netdev_client);
 }
 module_init(ntb_netdev_init_module);
 
 static void __exit ntb_netdev_exit_module(void)
 {
-	ntb_unregister_client(&ntb_netdev_client);
-	ntb_unregister_client_dev(KBUILD_MODNAME);
+	ntb_transport_unregister_client(&ntb_netdev_client);
+	ntb_transport_unregister_client_dev(KBUILD_MODNAME);
 }
 module_exit(ntb_netdev_exit_module);
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 70641d2c0429..cf18940f4e84 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -24,13 +24,6 @@ config AMD_PHY
 	---help---
 	  Currently supports the am79c874
 
-config AMD_XGBE_PHY
-	tristate "Driver for the AMD 10GbE (amd-xgbe) PHYs"
-	depends on (OF || ACPI) && HAS_IOMEM
-	depends on ARM64 || COMPILE_TEST
-	---help---
-	  Currently supports the AMD 10GbE PHY
-
 config MARVELL_PHY
 	tristate "Drivers for Marvell PHYs"
 	---help---
@@ -119,6 +112,11 @@ config MICREL_PHY
 	---help---
 	  Supports the KSZ9021, VSC8201, KS8001 PHYs.
 
+config DP83867_PHY
+	tristate "Drivers for Texas Instruments DP83867 Gigabit PHY"
+	---help---
+	  Currently supports the DP83867 PHY.
+
 config FIXED_PHY
 	tristate "Driver for MDIO Bus/PHY emulation with fixed speed/link PHYs"
 	depends on PHYLIB
@@ -212,7 +210,6 @@ config MDIO_BCM_UNIMAC
 	  This hardware can be found in the Broadcom GENET Ethernet MAC
 	  controllers as well as some Broadcom Ethernet switches such as the
 	  Starfighter 2 switches.
-
 endif # PHYLIB
 
 config MICREL_KS8995MA
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index 501ea7699a2d..fcc25a0c45cd 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -22,6 +22,7 @@ obj-$(CONFIG_MDIO_BITBANG)	+= mdio-bitbang.o
 obj-$(CONFIG_MDIO_GPIO)		+= mdio-gpio.o
 obj-$(CONFIG_NATIONAL_PHY)	+= national.o
 obj-$(CONFIG_DP83640_PHY)	+= dp83640.o
+obj-$(CONFIG_DP83867_PHY)	+= dp83867.o
 obj-$(CONFIG_STE10XP)		+= ste10Xp.o
 obj-$(CONFIG_MICREL_PHY)	+= micrel.o
 obj-$(CONFIG_MDIO_OCTEON)	+= mdio-octeon.o
@@ -33,5 +34,4 @@ obj-$(CONFIG_MDIO_BUS_MUX_GPIO)	+= mdio-mux-gpio.o
 obj-$(CONFIG_MDIO_BUS_MUX_MMIOREG) += mdio-mux-mmioreg.o
 obj-$(CONFIG_MDIO_SUN4I)	+= mdio-sun4i.o
 obj-$(CONFIG_MDIO_MOXART)	+= mdio-moxart.o
-obj-$(CONFIG_AMD_XGBE_PHY)	+= amd-xgbe-phy.o
 obj-$(CONFIG_MDIO_BCM_UNIMAC)	+= mdio-bcm-unimac.o
diff --git a/drivers/net/phy/amd-xgbe-phy.c b/drivers/net/phy/amd-xgbe-phy.c
deleted file mode 100644
index 34a75cba3b73..000000000000
--- a/drivers/net/phy/amd-xgbe-phy.c
+++ /dev/null
@@ -1,1901 +0,0 @@
-/*
- * AMD 10Gb Ethernet PHY driver
- *
- * This file is available to you under your choice of the following two
- * licenses:
- *
- * License 1: GPLv2
- *
- * Copyright (c) 2014 Advanced Micro Devices, Inc.
- *
- * This file is free software; you may copy, redistribute and/or modify
- * it under 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 is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * 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/>.
- *
- *
- * License 2: Modified BSD
- *
- * Copyright (c) 2014 Advanced Micro Devices, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyright
- *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of Advanced Micro Devices, Inc. nor the
- *       names of its contributors may be used to endorse or promote products
- *       derived from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
- * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/kernel.h>
-#include <linux/device.h>
-#include <linux/platform_device.h>
-#include <linux/string.h>
-#include <linux/errno.h>
-#include <linux/unistd.h>
-#include <linux/slab.h>
-#include <linux/interrupt.h>
-#include <linux/init.h>
-#include <linux/delay.h>
-#include <linux/workqueue.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/skbuff.h>
-#include <linux/mm.h>
-#include <linux/module.h>
-#include <linux/mii.h>
-#include <linux/ethtool.h>
-#include <linux/phy.h>
-#include <linux/mdio.h>
-#include <linux/io.h>
-#include <linux/of.h>
-#include <linux/of_platform.h>
-#include <linux/of_device.h>
-#include <linux/uaccess.h>
-#include <linux/bitops.h>
-#include <linux/property.h>
-#include <linux/acpi.h>
-#include <linux/jiffies.h>
-
-MODULE_AUTHOR("Tom Lendacky <thomas.lendacky@amd.com>");
-MODULE_LICENSE("Dual BSD/GPL");
-MODULE_VERSION("1.0.0-a");
-MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
-
-#define XGBE_PHY_ID	0x000162d0
-#define XGBE_PHY_MASK	0xfffffff0
-
-#define XGBE_PHY_SPEEDSET_PROPERTY	"amd,speed-set"
-#define XGBE_PHY_BLWC_PROPERTY		"amd,serdes-blwc"
-#define XGBE_PHY_CDR_RATE_PROPERTY	"amd,serdes-cdr-rate"
-#define XGBE_PHY_PQ_SKEW_PROPERTY	"amd,serdes-pq-skew"
-#define XGBE_PHY_TX_AMP_PROPERTY	"amd,serdes-tx-amp"
-#define XGBE_PHY_DFE_CFG_PROPERTY	"amd,serdes-dfe-tap-config"
-#define XGBE_PHY_DFE_ENA_PROPERTY	"amd,serdes-dfe-tap-enable"
-
-#define XGBE_PHY_SPEEDS			3
-#define XGBE_PHY_SPEED_1000		0
-#define XGBE_PHY_SPEED_2500		1
-#define XGBE_PHY_SPEED_10000		2
-
-#define XGBE_AN_MS_TIMEOUT		500
-
-#define XGBE_AN_INT_CMPLT		0x01
-#define XGBE_AN_INC_LINK		0x02
-#define XGBE_AN_PG_RCV			0x04
-#define XGBE_AN_INT_MASK		0x07
-
-#define XNP_MCF_NULL_MESSAGE		0x001
-#define XNP_ACK_PROCESSED		BIT(12)
-#define XNP_MP_FORMATTED		BIT(13)
-#define XNP_NP_EXCHANGE			BIT(15)
-
-#define XGBE_PHY_RATECHANGE_COUNT	500
-
-#define XGBE_PHY_KR_TRAINING_START	0x01
-#define XGBE_PHY_KR_TRAINING_ENABLE	0x02
-
-#define XGBE_PHY_FEC_ENABLE		0x01
-#define XGBE_PHY_FEC_FORWARD		0x02
-#define XGBE_PHY_FEC_MASK		0x03
-
-#ifndef MDIO_PMA_10GBR_PMD_CTRL
-#define MDIO_PMA_10GBR_PMD_CTRL		0x0096
-#endif
-
-#ifndef MDIO_PMA_10GBR_FEC_ABILITY
-#define MDIO_PMA_10GBR_FEC_ABILITY	0x00aa
-#endif
-
-#ifndef MDIO_PMA_10GBR_FEC_CTRL
-#define MDIO_PMA_10GBR_FEC_CTRL		0x00ab
-#endif
-
-#ifndef MDIO_AN_XNP
-#define MDIO_AN_XNP			0x0016
-#endif
-
-#ifndef MDIO_AN_LPX
-#define MDIO_AN_LPX			0x0019
-#endif
-
-#ifndef MDIO_AN_INTMASK
-#define MDIO_AN_INTMASK			0x8001
-#endif
-
-#ifndef MDIO_AN_INT
-#define MDIO_AN_INT			0x8002
-#endif
-
-#ifndef MDIO_CTRL1_SPEED1G
-#define MDIO_CTRL1_SPEED1G		(MDIO_CTRL1_SPEED10G & ~BMCR_SPEED100)
-#endif
-
-/* SerDes integration register offsets */
-#define SIR0_KR_RT_1			0x002c
-#define SIR0_STATUS			0x0040
-#define SIR1_SPEED			0x0000
-
-/* SerDes integration register entry bit positions and sizes */
-#define SIR0_KR_RT_1_RESET_INDEX	11
-#define SIR0_KR_RT_1_RESET_WIDTH	1
-#define SIR0_STATUS_RX_READY_INDEX	0
-#define SIR0_STATUS_RX_READY_WIDTH	1
-#define SIR0_STATUS_TX_READY_INDEX	8
-#define SIR0_STATUS_TX_READY_WIDTH	1
-#define SIR1_SPEED_CDR_RATE_INDEX	12
-#define SIR1_SPEED_CDR_RATE_WIDTH	4
-#define SIR1_SPEED_DATARATE_INDEX	4
-#define SIR1_SPEED_DATARATE_WIDTH	2
-#define SIR1_SPEED_PLLSEL_INDEX		3
-#define SIR1_SPEED_PLLSEL_WIDTH		1
-#define SIR1_SPEED_RATECHANGE_INDEX	6
-#define SIR1_SPEED_RATECHANGE_WIDTH	1
-#define SIR1_SPEED_TXAMP_INDEX		8
-#define SIR1_SPEED_TXAMP_WIDTH		4
-#define SIR1_SPEED_WORDMODE_INDEX	0
-#define SIR1_SPEED_WORDMODE_WIDTH	3
-
-#define SPEED_10000_BLWC		0
-#define SPEED_10000_CDR			0x7
-#define SPEED_10000_PLL			0x1
-#define SPEED_10000_PQ			0x12
-#define SPEED_10000_RATE		0x0
-#define SPEED_10000_TXAMP		0xa
-#define SPEED_10000_WORD		0x7
-#define SPEED_10000_DFE_TAP_CONFIG	0x1
-#define SPEED_10000_DFE_TAP_ENABLE	0x7f
-
-#define SPEED_2500_BLWC			1
-#define SPEED_2500_CDR			0x2
-#define SPEED_2500_PLL			0x0
-#define SPEED_2500_PQ			0xa
-#define SPEED_2500_RATE			0x1
-#define SPEED_2500_TXAMP		0xf
-#define SPEED_2500_WORD			0x1
-#define SPEED_2500_DFE_TAP_CONFIG	0x3
-#define SPEED_2500_DFE_TAP_ENABLE	0x0
-
-#define SPEED_1000_BLWC			1
-#define SPEED_1000_CDR			0x2
-#define SPEED_1000_PLL			0x0
-#define SPEED_1000_PQ			0xa
-#define SPEED_1000_RATE			0x3
-#define SPEED_1000_TXAMP		0xf
-#define SPEED_1000_WORD			0x1
-#define SPEED_1000_DFE_TAP_CONFIG	0x3
-#define SPEED_1000_DFE_TAP_ENABLE	0x0
-
-/* SerDes RxTx register offsets */
-#define RXTX_REG6			0x0018
-#define RXTX_REG20			0x0050
-#define RXTX_REG22			0x0058
-#define RXTX_REG114			0x01c8
-#define RXTX_REG129			0x0204
-
-/* SerDes RxTx register entry bit positions and sizes */
-#define RXTX_REG6_RESETB_RXD_INDEX	8
-#define RXTX_REG6_RESETB_RXD_WIDTH	1
-#define RXTX_REG20_BLWC_ENA_INDEX	2
-#define RXTX_REG20_BLWC_ENA_WIDTH	1
-#define RXTX_REG114_PQ_REG_INDEX	9
-#define RXTX_REG114_PQ_REG_WIDTH	7
-#define RXTX_REG129_RXDFE_CONFIG_INDEX	14
-#define RXTX_REG129_RXDFE_CONFIG_WIDTH	2
-
-/* Bit setting and getting macros
- *  The get macro will extract the current bit field value from within
- *  the variable
- *
- *  The set macro will clear the current bit field value within the
- *  variable and then set the bit field of the variable to the
- *  specified value
- */
-#define GET_BITS(_var, _index, _width)					\
-	(((_var) >> (_index)) & ((0x1 << (_width)) - 1))
-
-#define SET_BITS(_var, _index, _width, _val)				\
-do {									\
-	(_var) &= ~(((0x1 << (_width)) - 1) << (_index));		\
-	(_var) |= (((_val) & ((0x1 << (_width)) - 1)) << (_index));	\
-} while (0)
-
-#define XSIR_GET_BITS(_var, _prefix, _field)				\
-	GET_BITS((_var),						\
-		 _prefix##_##_field##_INDEX,				\
-		 _prefix##_##_field##_WIDTH)
-
-#define XSIR_SET_BITS(_var, _prefix, _field, _val)			\
-	SET_BITS((_var),						\
-		 _prefix##_##_field##_INDEX,				\
-		 _prefix##_##_field##_WIDTH, (_val))
-
-/* Macros for reading or writing SerDes integration registers
- *  The ioread macros will get bit fields or full values using the
- *  register definitions formed using the input names
- *
- *  The iowrite macros will set bit fields or full values using the
- *  register definitions formed using the input names
- */
-#define XSIR0_IOREAD(_priv, _reg)					\
-	ioread16((_priv)->sir0_regs + _reg)
-
-#define XSIR0_IOREAD_BITS(_priv, _reg, _field)				\
-	GET_BITS(XSIR0_IOREAD((_priv), _reg),				\
-		 _reg##_##_field##_INDEX,				\
-		 _reg##_##_field##_WIDTH)
-
-#define XSIR0_IOWRITE(_priv, _reg, _val)				\
-	iowrite16((_val), (_priv)->sir0_regs + _reg)
-
-#define XSIR0_IOWRITE_BITS(_priv, _reg, _field, _val)			\
-do {									\
-	u16 reg_val = XSIR0_IOREAD((_priv), _reg);			\
-	SET_BITS(reg_val,						\
-		 _reg##_##_field##_INDEX,				\
-		 _reg##_##_field##_WIDTH, (_val));			\
-	XSIR0_IOWRITE((_priv), _reg, reg_val);				\
-} while (0)
-
-#define XSIR1_IOREAD(_priv, _reg)					\
-	ioread16((_priv)->sir1_regs + _reg)
-
-#define XSIR1_IOREAD_BITS(_priv, _reg, _field)				\
-	GET_BITS(XSIR1_IOREAD((_priv), _reg),				\
-		 _reg##_##_field##_INDEX,				\
-		 _reg##_##_field##_WIDTH)
-
-#define XSIR1_IOWRITE(_priv, _reg, _val)				\
-	iowrite16((_val), (_priv)->sir1_regs + _reg)
-
-#define XSIR1_IOWRITE_BITS(_priv, _reg, _field, _val)			\
-do {									\
-	u16 reg_val = XSIR1_IOREAD((_priv), _reg);			\
-	SET_BITS(reg_val,						\
-		 _reg##_##_field##_INDEX,				\
-		 _reg##_##_field##_WIDTH, (_val));			\
-	XSIR1_IOWRITE((_priv), _reg, reg_val);				\
-} while (0)
-
-/* Macros for reading or writing SerDes RxTx registers
- *  The ioread macros will get bit fields or full values using the
- *  register definitions formed using the input names
- *
- *  The iowrite macros will set bit fields or full values using the
- *  register definitions formed using the input names
- */
-#define XRXTX_IOREAD(_priv, _reg)					\
-	ioread16((_priv)->rxtx_regs + _reg)
-
-#define XRXTX_IOREAD_BITS(_priv, _reg, _field)				\
-	GET_BITS(XRXTX_IOREAD((_priv), _reg),				\
-		 _reg##_##_field##_INDEX,				\
-		 _reg##_##_field##_WIDTH)
-
-#define XRXTX_IOWRITE(_priv, _reg, _val)				\
-	iowrite16((_val), (_priv)->rxtx_regs + _reg)
-
-#define XRXTX_IOWRITE_BITS(_priv, _reg, _field, _val)			\
-do {									\
-	u16 reg_val = XRXTX_IOREAD((_priv), _reg);			\
-	SET_BITS(reg_val,						\
-		 _reg##_##_field##_INDEX,				\
-		 _reg##_##_field##_WIDTH, (_val));			\
-	XRXTX_IOWRITE((_priv), _reg, reg_val);				\
-} while (0)
-
-static const u32 amd_xgbe_phy_serdes_blwc[] = {
-	SPEED_1000_BLWC,
-	SPEED_2500_BLWC,
-	SPEED_10000_BLWC,
-};
-
-static const u32 amd_xgbe_phy_serdes_cdr_rate[] = {
-	SPEED_1000_CDR,
-	SPEED_2500_CDR,
-	SPEED_10000_CDR,
-};
-
-static const u32 amd_xgbe_phy_serdes_pq_skew[] = {
-	SPEED_1000_PQ,
-	SPEED_2500_PQ,
-	SPEED_10000_PQ,
-};
-
-static const u32 amd_xgbe_phy_serdes_tx_amp[] = {
-	SPEED_1000_TXAMP,
-	SPEED_2500_TXAMP,
-	SPEED_10000_TXAMP,
-};
-
-static const u32 amd_xgbe_phy_serdes_dfe_tap_cfg[] = {
-	SPEED_1000_DFE_TAP_CONFIG,
-	SPEED_2500_DFE_TAP_CONFIG,
-	SPEED_10000_DFE_TAP_CONFIG,
-};
-
-static const u32 amd_xgbe_phy_serdes_dfe_tap_ena[] = {
-	SPEED_1000_DFE_TAP_ENABLE,
-	SPEED_2500_DFE_TAP_ENABLE,
-	SPEED_10000_DFE_TAP_ENABLE,
-};
-
-enum amd_xgbe_phy_an {
-	AMD_XGBE_AN_READY = 0,
-	AMD_XGBE_AN_PAGE_RECEIVED,
-	AMD_XGBE_AN_INCOMPAT_LINK,
-	AMD_XGBE_AN_COMPLETE,
-	AMD_XGBE_AN_NO_LINK,
-	AMD_XGBE_AN_ERROR,
-};
-
-enum amd_xgbe_phy_rx {
-	AMD_XGBE_RX_BPA = 0,
-	AMD_XGBE_RX_XNP,
-	AMD_XGBE_RX_COMPLETE,
-	AMD_XGBE_RX_ERROR,
-};
-
-enum amd_xgbe_phy_mode {
-	AMD_XGBE_MODE_KR,
-	AMD_XGBE_MODE_KX,
-};
-
-enum amd_xgbe_phy_speedset {
-	AMD_XGBE_PHY_SPEEDSET_1000_10000 = 0,
-	AMD_XGBE_PHY_SPEEDSET_2500_10000,
-};
-
-struct amd_xgbe_phy_priv {
-	struct platform_device *pdev;
-	struct acpi_device *adev;
-	struct device *dev;
-
-	struct phy_device *phydev;
-
-	/* SerDes related mmio resources */
-	struct resource *rxtx_res;
-	struct resource *sir0_res;
-	struct resource *sir1_res;
-
-	/* SerDes related mmio registers */
-	void __iomem *rxtx_regs;	/* SerDes Rx/Tx CSRs */
-	void __iomem *sir0_regs;	/* SerDes integration registers (1/2) */
-	void __iomem *sir1_regs;	/* SerDes integration registers (2/2) */
-
-	int an_irq;
-	char an_irq_name[IFNAMSIZ + 32];
-	struct work_struct an_irq_work;
-	unsigned int an_irq_allocated;
-
-	unsigned int speed_set;
-
-	/* SerDes UEFI configurable settings.
-	 *   Switching between modes/speeds requires new values for some
-	 *   SerDes settings.  The values can be supplied as device
-	 *   properties in array format.  The first array entry is for
-	 *   1GbE, second for 2.5GbE and third for 10GbE
-	 */
-	u32 serdes_blwc[XGBE_PHY_SPEEDS];
-	u32 serdes_cdr_rate[XGBE_PHY_SPEEDS];
-	u32 serdes_pq_skew[XGBE_PHY_SPEEDS];
-	u32 serdes_tx_amp[XGBE_PHY_SPEEDS];
-	u32 serdes_dfe_tap_cfg[XGBE_PHY_SPEEDS];
-	u32 serdes_dfe_tap_ena[XGBE_PHY_SPEEDS];
-
-	/* Auto-negotiation state machine support */
-	struct mutex an_mutex;
-	enum amd_xgbe_phy_an an_result;
-	enum amd_xgbe_phy_an an_state;
-	enum amd_xgbe_phy_rx kr_state;
-	enum amd_xgbe_phy_rx kx_state;
-	struct work_struct an_work;
-	struct workqueue_struct *an_workqueue;
-	unsigned int an_supported;
-	unsigned int parallel_detect;
-	unsigned int fec_ability;
-	unsigned long an_start;
-
-	unsigned int lpm_ctrl;		/* CTRL1 for resume */
-};
-
-static int amd_xgbe_an_enable_kr_training(struct phy_device *phydev)
-{
-	int ret;
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL);
-	if (ret < 0)
-		return ret;
-
-	ret |= XGBE_PHY_KR_TRAINING_ENABLE;
-	phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL, ret);
-
-	return 0;
-}
-
-static int amd_xgbe_an_disable_kr_training(struct phy_device *phydev)
-{
-	int ret;
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL);
-	if (ret < 0)
-		return ret;
-
-	ret &= ~XGBE_PHY_KR_TRAINING_ENABLE;
-	phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL, ret);
-
-	return 0;
-}
-
-static int amd_xgbe_phy_pcs_power_cycle(struct phy_device *phydev)
-{
-	int ret;
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1);
-	if (ret < 0)
-		return ret;
-
-	ret |= MDIO_CTRL1_LPOWER;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1, ret);
-
-	usleep_range(75, 100);
-
-	ret &= ~MDIO_CTRL1_LPOWER;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1, ret);
-
-	return 0;
-}
-
-static void amd_xgbe_phy_serdes_start_ratechange(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-
-	/* Assert Rx and Tx ratechange */
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, RATECHANGE, 1);
-}
-
-static void amd_xgbe_phy_serdes_complete_ratechange(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	unsigned int wait;
-	u16 status;
-
-	/* Release Rx and Tx ratechange */
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, RATECHANGE, 0);
-
-	/* Wait for Rx and Tx ready */
-	wait = XGBE_PHY_RATECHANGE_COUNT;
-	while (wait--) {
-		usleep_range(50, 75);
-
-		status = XSIR0_IOREAD(priv, SIR0_STATUS);
-		if (XSIR_GET_BITS(status, SIR0_STATUS, RX_READY) &&
-		    XSIR_GET_BITS(status, SIR0_STATUS, TX_READY))
-			goto rx_reset;
-	}
-
-	netdev_dbg(phydev->attached_dev, "SerDes rx/tx not ready (%#hx)\n",
-		   status);
-
-rx_reset:
-	/* Perform Rx reset for the DFE changes */
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG6, RESETB_RXD, 0);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG6, RESETB_RXD, 1);
-}
-
-static int amd_xgbe_phy_xgmii_mode(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ret;
-
-	/* Enable KR training */
-	ret = amd_xgbe_an_enable_kr_training(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Set PCS to KR/10G speed */
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2);
-	if (ret < 0)
-		return ret;
-
-	ret &= ~MDIO_PCS_CTRL2_TYPE;
-	ret |= MDIO_PCS_CTRL2_10GBR;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2, ret);
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1);
-	if (ret < 0)
-		return ret;
-
-	ret &= ~MDIO_CTRL1_SPEEDSEL;
-	ret |= MDIO_CTRL1_SPEED10G;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1, ret);
-
-	ret = amd_xgbe_phy_pcs_power_cycle(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Set SerDes to 10G speed */
-	amd_xgbe_phy_serdes_start_ratechange(phydev);
-
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, DATARATE, SPEED_10000_RATE);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, WORDMODE, SPEED_10000_WORD);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, PLLSEL, SPEED_10000_PLL);
-
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, CDR_RATE,
-			   priv->serdes_cdr_rate[XGBE_PHY_SPEED_10000]);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, TXAMP,
-			   priv->serdes_tx_amp[XGBE_PHY_SPEED_10000]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG20, BLWC_ENA,
-			   priv->serdes_blwc[XGBE_PHY_SPEED_10000]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG114, PQ_REG,
-			   priv->serdes_pq_skew[XGBE_PHY_SPEED_10000]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG129, RXDFE_CONFIG,
-			   priv->serdes_dfe_tap_cfg[XGBE_PHY_SPEED_10000]);
-	XRXTX_IOWRITE(priv, RXTX_REG22,
-		      priv->serdes_dfe_tap_ena[XGBE_PHY_SPEED_10000]);
-
-	amd_xgbe_phy_serdes_complete_ratechange(phydev);
-
-	return 0;
-}
-
-static int amd_xgbe_phy_gmii_2500_mode(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ret;
-
-	/* Disable KR training */
-	ret = amd_xgbe_an_disable_kr_training(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Set PCS to KX/1G speed */
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2);
-	if (ret < 0)
-		return ret;
-
-	ret &= ~MDIO_PCS_CTRL2_TYPE;
-	ret |= MDIO_PCS_CTRL2_10GBX;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2, ret);
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1);
-	if (ret < 0)
-		return ret;
-
-	ret &= ~MDIO_CTRL1_SPEEDSEL;
-	ret |= MDIO_CTRL1_SPEED1G;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1, ret);
-
-	ret = amd_xgbe_phy_pcs_power_cycle(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Set SerDes to 2.5G speed */
-	amd_xgbe_phy_serdes_start_ratechange(phydev);
-
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, DATARATE, SPEED_2500_RATE);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, WORDMODE, SPEED_2500_WORD);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, PLLSEL, SPEED_2500_PLL);
-
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, CDR_RATE,
-			   priv->serdes_cdr_rate[XGBE_PHY_SPEED_2500]);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, TXAMP,
-			   priv->serdes_tx_amp[XGBE_PHY_SPEED_2500]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG20, BLWC_ENA,
-			   priv->serdes_blwc[XGBE_PHY_SPEED_2500]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG114, PQ_REG,
-			   priv->serdes_pq_skew[XGBE_PHY_SPEED_2500]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG129, RXDFE_CONFIG,
-			   priv->serdes_dfe_tap_cfg[XGBE_PHY_SPEED_2500]);
-	XRXTX_IOWRITE(priv, RXTX_REG22,
-		      priv->serdes_dfe_tap_ena[XGBE_PHY_SPEED_2500]);
-
-	amd_xgbe_phy_serdes_complete_ratechange(phydev);
-
-	return 0;
-}
-
-static int amd_xgbe_phy_gmii_mode(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ret;
-
-	/* Disable KR training */
-	ret = amd_xgbe_an_disable_kr_training(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Set PCS to KX/1G speed */
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2);
-	if (ret < 0)
-		return ret;
-
-	ret &= ~MDIO_PCS_CTRL2_TYPE;
-	ret |= MDIO_PCS_CTRL2_10GBX;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2, ret);
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1);
-	if (ret < 0)
-		return ret;
-
-	ret &= ~MDIO_CTRL1_SPEEDSEL;
-	ret |= MDIO_CTRL1_SPEED1G;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1, ret);
-
-	ret = amd_xgbe_phy_pcs_power_cycle(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Set SerDes to 1G speed */
-	amd_xgbe_phy_serdes_start_ratechange(phydev);
-
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, DATARATE, SPEED_1000_RATE);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, WORDMODE, SPEED_1000_WORD);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, PLLSEL, SPEED_1000_PLL);
-
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, CDR_RATE,
-			   priv->serdes_cdr_rate[XGBE_PHY_SPEED_1000]);
-	XSIR1_IOWRITE_BITS(priv, SIR1_SPEED, TXAMP,
-			   priv->serdes_tx_amp[XGBE_PHY_SPEED_1000]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG20, BLWC_ENA,
-			   priv->serdes_blwc[XGBE_PHY_SPEED_1000]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG114, PQ_REG,
-			   priv->serdes_pq_skew[XGBE_PHY_SPEED_1000]);
-	XRXTX_IOWRITE_BITS(priv, RXTX_REG129, RXDFE_CONFIG,
-			   priv->serdes_dfe_tap_cfg[XGBE_PHY_SPEED_1000]);
-	XRXTX_IOWRITE(priv, RXTX_REG22,
-		      priv->serdes_dfe_tap_ena[XGBE_PHY_SPEED_1000]);
-
-	amd_xgbe_phy_serdes_complete_ratechange(phydev);
-
-	return 0;
-}
-
-static int amd_xgbe_phy_cur_mode(struct phy_device *phydev,
-				 enum amd_xgbe_phy_mode *mode)
-{
-	int ret;
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2);
-	if (ret < 0)
-		return ret;
-
-	if ((ret & MDIO_PCS_CTRL2_TYPE) == MDIO_PCS_CTRL2_10GBR)
-		*mode = AMD_XGBE_MODE_KR;
-	else
-		*mode = AMD_XGBE_MODE_KX;
-
-	return 0;
-}
-
-static bool amd_xgbe_phy_in_kr_mode(struct phy_device *phydev)
-{
-	enum amd_xgbe_phy_mode mode;
-
-	if (amd_xgbe_phy_cur_mode(phydev, &mode))
-		return false;
-
-	return (mode == AMD_XGBE_MODE_KR);
-}
-
-static int amd_xgbe_phy_switch_mode(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ret;
-
-	/* If we are in KR switch to KX, and vice-versa */
-	if (amd_xgbe_phy_in_kr_mode(phydev)) {
-		if (priv->speed_set == AMD_XGBE_PHY_SPEEDSET_1000_10000)
-			ret = amd_xgbe_phy_gmii_mode(phydev);
-		else
-			ret = amd_xgbe_phy_gmii_2500_mode(phydev);
-	} else {
-		ret = amd_xgbe_phy_xgmii_mode(phydev);
-	}
-
-	return ret;
-}
-
-static int amd_xgbe_phy_set_mode(struct phy_device *phydev,
-				 enum amd_xgbe_phy_mode mode)
-{
-	enum amd_xgbe_phy_mode cur_mode;
-	int ret;
-
-	ret = amd_xgbe_phy_cur_mode(phydev, &cur_mode);
-	if (ret)
-		return ret;
-
-	if (mode != cur_mode)
-		ret = amd_xgbe_phy_switch_mode(phydev);
-
-	return ret;
-}
-
-static bool amd_xgbe_phy_use_xgmii_mode(struct phy_device *phydev)
-{
-	if (phydev->autoneg == AUTONEG_ENABLE) {
-		if (phydev->advertising & ADVERTISED_10000baseKR_Full)
-			return true;
-	} else {
-		if (phydev->speed == SPEED_10000)
-			return true;
-	}
-
-	return false;
-}
-
-static bool amd_xgbe_phy_use_gmii_2500_mode(struct phy_device *phydev)
-{
-	if (phydev->autoneg == AUTONEG_ENABLE) {
-		if (phydev->advertising & ADVERTISED_2500baseX_Full)
-			return true;
-	} else {
-		if (phydev->speed == SPEED_2500)
-			return true;
-	}
-
-	return false;
-}
-
-static bool amd_xgbe_phy_use_gmii_mode(struct phy_device *phydev)
-{
-	if (phydev->autoneg == AUTONEG_ENABLE) {
-		if (phydev->advertising & ADVERTISED_1000baseKX_Full)
-			return true;
-	} else {
-		if (phydev->speed == SPEED_1000)
-			return true;
-	}
-
-	return false;
-}
-
-static int amd_xgbe_phy_set_an(struct phy_device *phydev, bool enable,
-			       bool restart)
-{
-	int ret;
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1);
-	if (ret < 0)
-		return ret;
-
-	ret &= ~MDIO_AN_CTRL1_ENABLE;
-
-	if (enable)
-		ret |= MDIO_AN_CTRL1_ENABLE;
-
-	if (restart)
-		ret |= MDIO_AN_CTRL1_RESTART;
-
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1, ret);
-
-	return 0;
-}
-
-static int amd_xgbe_phy_restart_an(struct phy_device *phydev)
-{
-	return amd_xgbe_phy_set_an(phydev, true, true);
-}
-
-static int amd_xgbe_phy_disable_an(struct phy_device *phydev)
-{
-	return amd_xgbe_phy_set_an(phydev, false, false);
-}
-
-static enum amd_xgbe_phy_an amd_xgbe_an_tx_training(struct phy_device *phydev,
-						    enum amd_xgbe_phy_rx *state)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ad_reg, lp_reg, ret;
-
-	*state = AMD_XGBE_RX_COMPLETE;
-
-	/* If we're not in KR mode then we're done */
-	if (!amd_xgbe_phy_in_kr_mode(phydev))
-		return AMD_XGBE_AN_PAGE_RECEIVED;
-
-	/* Enable/Disable FEC */
-	ad_reg = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2);
-	if (ad_reg < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	lp_reg = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_LPA + 2);
-	if (lp_reg < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_FEC_CTRL);
-	if (ret < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	ret &= ~XGBE_PHY_FEC_MASK;
-	if ((ad_reg & 0xc000) && (lp_reg & 0xc000))
-		ret |= priv->fec_ability;
-
-	phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_FEC_CTRL, ret);
-
-	/* Start KR training */
-	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL);
-	if (ret < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	if (ret & XGBE_PHY_KR_TRAINING_ENABLE) {
-		XSIR0_IOWRITE_BITS(priv, SIR0_KR_RT_1, RESET, 1);
-
-		ret |= XGBE_PHY_KR_TRAINING_START;
-		phy_write_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_PMD_CTRL,
-			      ret);
-
-		XSIR0_IOWRITE_BITS(priv, SIR0_KR_RT_1, RESET, 0);
-	}
-
-	return AMD_XGBE_AN_PAGE_RECEIVED;
-}
-
-static enum amd_xgbe_phy_an amd_xgbe_an_tx_xnp(struct phy_device *phydev,
-					       enum amd_xgbe_phy_rx *state)
-{
-	u16 msg;
-
-	*state = AMD_XGBE_RX_XNP;
-
-	msg = XNP_MCF_NULL_MESSAGE;
-	msg |= XNP_MP_FORMATTED;
-
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_XNP + 2, 0);
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_XNP + 1, 0);
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_XNP, msg);
-
-	return AMD_XGBE_AN_PAGE_RECEIVED;
-}
-
-static enum amd_xgbe_phy_an amd_xgbe_an_rx_bpa(struct phy_device *phydev,
-					       enum amd_xgbe_phy_rx *state)
-{
-	unsigned int link_support;
-	int ret, ad_reg, lp_reg;
-
-	/* Read Base Ability register 2 first */
-	ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_LPA + 1);
-	if (ret < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	/* Check for a supported mode, otherwise restart in a different one */
-	link_support = amd_xgbe_phy_in_kr_mode(phydev) ? 0x80 : 0x20;
-	if (!(ret & link_support))
-		return AMD_XGBE_AN_INCOMPAT_LINK;
-
-	/* Check Extended Next Page support */
-	ad_reg = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE);
-	if (ad_reg < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	lp_reg = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_LPA);
-	if (lp_reg < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	return ((ad_reg & XNP_NP_EXCHANGE) || (lp_reg & XNP_NP_EXCHANGE)) ?
-	       amd_xgbe_an_tx_xnp(phydev, state) :
-	       amd_xgbe_an_tx_training(phydev, state);
-}
-
-static enum amd_xgbe_phy_an amd_xgbe_an_rx_xnp(struct phy_device *phydev,
-					       enum amd_xgbe_phy_rx *state)
-{
-	int ad_reg, lp_reg;
-
-	/* Check Extended Next Page support */
-	ad_reg = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_XNP);
-	if (ad_reg < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	lp_reg = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_LPX);
-	if (lp_reg < 0)
-		return AMD_XGBE_AN_ERROR;
-
-	return ((ad_reg & XNP_NP_EXCHANGE) || (lp_reg & XNP_NP_EXCHANGE)) ?
-	       amd_xgbe_an_tx_xnp(phydev, state) :
-	       amd_xgbe_an_tx_training(phydev, state);
-}
-
-static enum amd_xgbe_phy_an amd_xgbe_an_page_received(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	enum amd_xgbe_phy_rx *state;
-	unsigned long an_timeout;
-	int ret;
-
-	if (!priv->an_start) {
-		priv->an_start = jiffies;
-	} else {
-		an_timeout = priv->an_start +
-			     msecs_to_jiffies(XGBE_AN_MS_TIMEOUT);
-		if (time_after(jiffies, an_timeout)) {
-			/* Auto-negotiation timed out, reset state */
-			priv->kr_state = AMD_XGBE_RX_BPA;
-			priv->kx_state = AMD_XGBE_RX_BPA;
-
-			priv->an_start = jiffies;
-		}
-	}
-
-	state = amd_xgbe_phy_in_kr_mode(phydev) ? &priv->kr_state
-						: &priv->kx_state;
-
-	switch (*state) {
-	case AMD_XGBE_RX_BPA:
-		ret = amd_xgbe_an_rx_bpa(phydev, state);
-		break;
-
-	case AMD_XGBE_RX_XNP:
-		ret = amd_xgbe_an_rx_xnp(phydev, state);
-		break;
-
-	default:
-		ret = AMD_XGBE_AN_ERROR;
-	}
-
-	return ret;
-}
-
-static enum amd_xgbe_phy_an amd_xgbe_an_incompat_link(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ret;
-
-	/* Be sure we aren't looping trying to negotiate */
-	if (amd_xgbe_phy_in_kr_mode(phydev)) {
-		priv->kr_state = AMD_XGBE_RX_ERROR;
-
-		if (!(phydev->advertising & SUPPORTED_1000baseKX_Full) &&
-		    !(phydev->advertising & SUPPORTED_2500baseX_Full))
-			return AMD_XGBE_AN_NO_LINK;
-
-		if (priv->kx_state != AMD_XGBE_RX_BPA)
-			return AMD_XGBE_AN_NO_LINK;
-	} else {
-		priv->kx_state = AMD_XGBE_RX_ERROR;
-
-		if (!(phydev->advertising & SUPPORTED_10000baseKR_Full))
-			return AMD_XGBE_AN_NO_LINK;
-
-		if (priv->kr_state != AMD_XGBE_RX_BPA)
-			return AMD_XGBE_AN_NO_LINK;
-	}
-
-	ret = amd_xgbe_phy_disable_an(phydev);
-	if (ret)
-		return AMD_XGBE_AN_ERROR;
-
-	ret = amd_xgbe_phy_switch_mode(phydev);
-	if (ret)
-		return AMD_XGBE_AN_ERROR;
-
-	ret = amd_xgbe_phy_restart_an(phydev);
-	if (ret)
-		return AMD_XGBE_AN_ERROR;
-
-	return AMD_XGBE_AN_INCOMPAT_LINK;
-}
-
-static irqreturn_t amd_xgbe_an_isr(int irq, void *data)
-{
-	struct amd_xgbe_phy_priv *priv = (struct amd_xgbe_phy_priv *)data;
-
-	/* Interrupt reason must be read and cleared outside of IRQ context */
-	disable_irq_nosync(priv->an_irq);
-
-	queue_work(priv->an_workqueue, &priv->an_irq_work);
-
-	return IRQ_HANDLED;
-}
-
-static void amd_xgbe_an_irq_work(struct work_struct *work)
-{
-	struct amd_xgbe_phy_priv *priv = container_of(work,
-						      struct amd_xgbe_phy_priv,
-						      an_irq_work);
-
-	/* Avoid a race between enabling the IRQ and exiting the work by
-	 * waiting for the work to finish and then queueing it
-	 */
-	flush_work(&priv->an_work);
-	queue_work(priv->an_workqueue, &priv->an_work);
-}
-
-static void amd_xgbe_an_state_machine(struct work_struct *work)
-{
-	struct amd_xgbe_phy_priv *priv = container_of(work,
-						      struct amd_xgbe_phy_priv,
-						      an_work);
-	struct phy_device *phydev = priv->phydev;
-	enum amd_xgbe_phy_an cur_state = priv->an_state;
-	int int_reg, int_mask;
-
-	mutex_lock(&priv->an_mutex);
-
-	/* Read the interrupt */
-	int_reg = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_INT);
-	if (!int_reg)
-		goto out;
-
-next_int:
-	if (int_reg < 0) {
-		priv->an_state = AMD_XGBE_AN_ERROR;
-		int_mask = XGBE_AN_INT_MASK;
-	} else if (int_reg & XGBE_AN_PG_RCV) {
-		priv->an_state = AMD_XGBE_AN_PAGE_RECEIVED;
-		int_mask = XGBE_AN_PG_RCV;
-	} else if (int_reg & XGBE_AN_INC_LINK) {
-		priv->an_state = AMD_XGBE_AN_INCOMPAT_LINK;
-		int_mask = XGBE_AN_INC_LINK;
-	} else if (int_reg & XGBE_AN_INT_CMPLT) {
-		priv->an_state = AMD_XGBE_AN_COMPLETE;
-		int_mask = XGBE_AN_INT_CMPLT;
-	} else {
-		priv->an_state = AMD_XGBE_AN_ERROR;
-		int_mask = 0;
-	}
-
-	/* Clear the interrupt to be processed */
-	int_reg &= ~int_mask;
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_INT, int_reg);
-
-	priv->an_result = priv->an_state;
-
-again:
-	cur_state = priv->an_state;
-
-	switch (priv->an_state) {
-	case AMD_XGBE_AN_READY:
-		priv->an_supported = 0;
-		break;
-
-	case AMD_XGBE_AN_PAGE_RECEIVED:
-		priv->an_state = amd_xgbe_an_page_received(phydev);
-		priv->an_supported++;
-		break;
-
-	case AMD_XGBE_AN_INCOMPAT_LINK:
-		priv->an_supported = 0;
-		priv->parallel_detect = 0;
-		priv->an_state = amd_xgbe_an_incompat_link(phydev);
-		break;
-
-	case AMD_XGBE_AN_COMPLETE:
-		priv->parallel_detect = priv->an_supported ? 0 : 1;
-		netdev_dbg(phydev->attached_dev, "%s successful\n",
-			   priv->an_supported ? "Auto negotiation"
-					      : "Parallel detection");
-		break;
-
-	case AMD_XGBE_AN_NO_LINK:
-		break;
-
-	default:
-		priv->an_state = AMD_XGBE_AN_ERROR;
-	}
-
-	if (priv->an_state == AMD_XGBE_AN_NO_LINK) {
-		int_reg = 0;
-		phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_INT, 0);
-	} else if (priv->an_state == AMD_XGBE_AN_ERROR) {
-		netdev_err(phydev->attached_dev,
-			   "error during auto-negotiation, state=%u\n",
-			   cur_state);
-
-		int_reg = 0;
-		phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_INT, 0);
-	}
-
-	if (priv->an_state >= AMD_XGBE_AN_COMPLETE) {
-		priv->an_result = priv->an_state;
-		priv->an_state = AMD_XGBE_AN_READY;
-		priv->kr_state = AMD_XGBE_RX_BPA;
-		priv->kx_state = AMD_XGBE_RX_BPA;
-		priv->an_start = 0;
-	}
-
-	if (cur_state != priv->an_state)
-		goto again;
-
-	if (int_reg)
-		goto next_int;
-
-out:
-	enable_irq(priv->an_irq);
-
-	mutex_unlock(&priv->an_mutex);
-}
-
-static int amd_xgbe_an_init(struct phy_device *phydev)
-{
-	int ret;
-
-	/* Set up Advertisement register 3 first */
-	ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2);
-	if (ret < 0)
-		return ret;
-
-	if (phydev->advertising & SUPPORTED_10000baseR_FEC)
-		ret |= 0xc000;
-	else
-		ret &= ~0xc000;
-
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 2, ret);
-
-	/* Set up Advertisement register 2 next */
-	ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 1);
-	if (ret < 0)
-		return ret;
-
-	if (phydev->advertising & SUPPORTED_10000baseKR_Full)
-		ret |= 0x80;
-	else
-		ret &= ~0x80;
-
-	if ((phydev->advertising & SUPPORTED_1000baseKX_Full) ||
-	    (phydev->advertising & SUPPORTED_2500baseX_Full))
-		ret |= 0x20;
-	else
-		ret &= ~0x20;
-
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE + 1, ret);
-
-	/* Set up Advertisement register 1 last */
-	ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE);
-	if (ret < 0)
-		return ret;
-
-	if (phydev->advertising & SUPPORTED_Pause)
-		ret |= 0x400;
-	else
-		ret &= ~0x400;
-
-	if (phydev->advertising & SUPPORTED_Asym_Pause)
-		ret |= 0x800;
-	else
-		ret &= ~0x800;
-
-	/* We don't intend to perform XNP */
-	ret &= ~XNP_NP_EXCHANGE;
-
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE, ret);
-
-	return 0;
-}
-
-static int amd_xgbe_phy_soft_reset(struct phy_device *phydev)
-{
-	int count, ret;
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1);
-	if (ret < 0)
-		return ret;
-
-	ret |= MDIO_CTRL1_RESET;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1, ret);
-
-	count = 50;
-	do {
-		msleep(20);
-		ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1);
-		if (ret < 0)
-			return ret;
-	} while ((ret & MDIO_CTRL1_RESET) && --count);
-
-	if (ret & MDIO_CTRL1_RESET)
-		return -ETIMEDOUT;
-
-	/* Disable auto-negotiation for now */
-	ret = amd_xgbe_phy_disable_an(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Clear auto-negotiation interrupts */
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_INT, 0);
-
-	return 0;
-}
-
-static int amd_xgbe_phy_config_init(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	struct net_device *netdev = phydev->attached_dev;
-	int ret;
-
-	if (!priv->an_irq_allocated) {
-		/* Allocate the auto-negotiation workqueue and interrupt */
-		snprintf(priv->an_irq_name, sizeof(priv->an_irq_name) - 1,
-			 "%s-pcs", netdev_name(netdev));
-
-		priv->an_workqueue =
-			create_singlethread_workqueue(priv->an_irq_name);
-		if (!priv->an_workqueue) {
-			netdev_err(netdev, "phy workqueue creation failed\n");
-			return -ENOMEM;
-		}
-
-		ret = devm_request_irq(priv->dev, priv->an_irq,
-				       amd_xgbe_an_isr, 0, priv->an_irq_name,
-				       priv);
-		if (ret) {
-			netdev_err(netdev, "phy irq request failed\n");
-			destroy_workqueue(priv->an_workqueue);
-			return ret;
-		}
-
-		priv->an_irq_allocated = 1;
-	}
-
-	/* Set initial mode - call the mode setting routines
-	 * directly to insure we are properly configured
-	 */
-	if (amd_xgbe_phy_use_xgmii_mode(phydev))
-		ret = amd_xgbe_phy_xgmii_mode(phydev);
-	else if (amd_xgbe_phy_use_gmii_mode(phydev))
-		ret = amd_xgbe_phy_gmii_mode(phydev);
-	else if (amd_xgbe_phy_use_gmii_2500_mode(phydev))
-		ret = amd_xgbe_phy_gmii_2500_mode(phydev);
-	else
-		ret = -EINVAL;
-	if (ret < 0)
-		return ret;
-
-	/* Set up advertisement registers based on current settings */
-	ret = amd_xgbe_an_init(phydev);
-	if (ret)
-		return ret;
-
-	/* Enable auto-negotiation interrupts */
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_INTMASK, 0x07);
-
-	return 0;
-}
-
-static int amd_xgbe_phy_setup_forced(struct phy_device *phydev)
-{
-	int ret;
-
-	/* Disable auto-negotiation */
-	ret = amd_xgbe_phy_disable_an(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Validate/Set specified speed */
-	switch (phydev->speed) {
-	case SPEED_10000:
-		ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KR);
-		break;
-
-	case SPEED_2500:
-	case SPEED_1000:
-		ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KX);
-		break;
-
-	default:
-		ret = -EINVAL;
-	}
-
-	if (ret < 0)
-		return ret;
-
-	/* Validate duplex mode */
-	if (phydev->duplex != DUPLEX_FULL)
-		return -EINVAL;
-
-	phydev->pause = 0;
-	phydev->asym_pause = 0;
-
-	return 0;
-}
-
-static int __amd_xgbe_phy_config_aneg(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	u32 mmd_mask = phydev->c45_ids.devices_in_package;
-	int ret;
-
-	if (phydev->autoneg != AUTONEG_ENABLE)
-		return amd_xgbe_phy_setup_forced(phydev);
-
-	/* Make sure we have the AN MMD present */
-	if (!(mmd_mask & MDIO_DEVS_AN))
-		return -EINVAL;
-
-	/* Disable auto-negotiation interrupt */
-	disable_irq(priv->an_irq);
-
-	/* Start auto-negotiation in a supported mode */
-	if (phydev->advertising & SUPPORTED_10000baseKR_Full)
-		ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KR);
-	else if ((phydev->advertising & SUPPORTED_1000baseKX_Full) ||
-		 (phydev->advertising & SUPPORTED_2500baseX_Full))
-		ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KX);
-	else
-		ret = -EINVAL;
-	if (ret < 0) {
-		enable_irq(priv->an_irq);
-		return ret;
-	}
-
-	/* Disable and stop any in progress auto-negotiation */
-	ret = amd_xgbe_phy_disable_an(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* Clear any auto-negotitation interrupts */
-	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_INT, 0);
-
-	priv->an_result = AMD_XGBE_AN_READY;
-	priv->an_state = AMD_XGBE_AN_READY;
-	priv->kr_state = AMD_XGBE_RX_BPA;
-	priv->kx_state = AMD_XGBE_RX_BPA;
-
-	/* Re-enable auto-negotiation interrupt */
-	enable_irq(priv->an_irq);
-
-	/* Set up advertisement registers based on current settings */
-	ret = amd_xgbe_an_init(phydev);
-	if (ret)
-		return ret;
-
-	/* Enable and start auto-negotiation */
-	return amd_xgbe_phy_restart_an(phydev);
-}
-
-static int amd_xgbe_phy_config_aneg(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ret;
-
-	mutex_lock(&priv->an_mutex);
-
-	ret = __amd_xgbe_phy_config_aneg(phydev);
-
-	mutex_unlock(&priv->an_mutex);
-
-	return ret;
-}
-
-static int amd_xgbe_phy_aneg_done(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-
-	return (priv->an_result == AMD_XGBE_AN_COMPLETE);
-}
-
-static int amd_xgbe_phy_update_link(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ret;
-
-	/* If we're doing auto-negotiation don't report link down */
-	if (priv->an_state != AMD_XGBE_AN_READY) {
-		phydev->link = 1;
-		return 0;
-	}
-
-	/* Link status is latched low, so read once to clear
-	 * and then read again to get current state
-	 */
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_STAT1);
-	if (ret < 0)
-		return ret;
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_STAT1);
-	if (ret < 0)
-		return ret;
-
-	phydev->link = (ret & MDIO_STAT1_LSTATUS) ? 1 : 0;
-
-	return 0;
-}
-
-static int amd_xgbe_phy_read_status(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	u32 mmd_mask = phydev->c45_ids.devices_in_package;
-	int ret, ad_ret, lp_ret;
-
-	ret = amd_xgbe_phy_update_link(phydev);
-	if (ret)
-		return ret;
-
-	if ((phydev->autoneg == AUTONEG_ENABLE) &&
-	    !priv->parallel_detect) {
-		if (!(mmd_mask & MDIO_DEVS_AN))
-			return -EINVAL;
-
-		if (!amd_xgbe_phy_aneg_done(phydev))
-			return 0;
-
-		/* Compare Advertisement and Link Partner register 1 */
-		ad_ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE);
-		if (ad_ret < 0)
-			return ad_ret;
-		lp_ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_LPA);
-		if (lp_ret < 0)
-			return lp_ret;
-
-		ad_ret &= lp_ret;
-		phydev->pause = (ad_ret & 0x400) ? 1 : 0;
-		phydev->asym_pause = (ad_ret & 0x800) ? 1 : 0;
-
-		/* Compare Advertisement and Link Partner register 2 */
-		ad_ret = phy_read_mmd(phydev, MDIO_MMD_AN,
-				      MDIO_AN_ADVERTISE + 1);
-		if (ad_ret < 0)
-			return ad_ret;
-		lp_ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_LPA + 1);
-		if (lp_ret < 0)
-			return lp_ret;
-
-		ad_ret &= lp_ret;
-		if (ad_ret & 0x80) {
-			phydev->speed = SPEED_10000;
-			ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KR);
-			if (ret)
-				return ret;
-		} else {
-			switch (priv->speed_set) {
-			case AMD_XGBE_PHY_SPEEDSET_1000_10000:
-				phydev->speed = SPEED_1000;
-				break;
-
-			case AMD_XGBE_PHY_SPEEDSET_2500_10000:
-				phydev->speed = SPEED_2500;
-				break;
-			}
-
-			ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KX);
-			if (ret)
-				return ret;
-		}
-
-		phydev->duplex = DUPLEX_FULL;
-	} else {
-		if (amd_xgbe_phy_in_kr_mode(phydev)) {
-			phydev->speed = SPEED_10000;
-		} else {
-			switch (priv->speed_set) {
-			case AMD_XGBE_PHY_SPEEDSET_1000_10000:
-				phydev->speed = SPEED_1000;
-				break;
-
-			case AMD_XGBE_PHY_SPEEDSET_2500_10000:
-				phydev->speed = SPEED_2500;
-				break;
-			}
-		}
-		phydev->duplex = DUPLEX_FULL;
-		phydev->pause = 0;
-		phydev->asym_pause = 0;
-	}
-
-	return 0;
-}
-
-static int amd_xgbe_phy_suspend(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	int ret;
-
-	mutex_lock(&phydev->lock);
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1);
-	if (ret < 0)
-		goto unlock;
-
-	priv->lpm_ctrl = ret;
-
-	ret |= MDIO_CTRL1_LPOWER;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1, ret);
-
-	ret = 0;
-
-unlock:
-	mutex_unlock(&phydev->lock);
-
-	return ret;
-}
-
-static int amd_xgbe_phy_resume(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-
-	mutex_lock(&phydev->lock);
-
-	priv->lpm_ctrl &= ~MDIO_CTRL1_LPOWER;
-	phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL1, priv->lpm_ctrl);
-
-	mutex_unlock(&phydev->lock);
-
-	return 0;
-}
-
-static unsigned int amd_xgbe_phy_resource_count(struct platform_device *pdev,
-						unsigned int type)
-{
-	unsigned int count;
-	int i;
-
-	for (i = 0, count = 0; i < pdev->num_resources; i++) {
-		struct resource *r = &pdev->resource[i];
-
-		if (type == resource_type(r))
-			count++;
-	}
-
-	return count;
-}
-
-static int amd_xgbe_phy_probe(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv;
-	struct platform_device *phy_pdev;
-	struct device *dev, *phy_dev;
-	unsigned int phy_resnum, phy_irqnum;
-	int ret;
-
-	if (!phydev->bus || !phydev->bus->parent)
-		return -EINVAL;
-
-	dev = phydev->bus->parent;
-
-	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	priv->pdev = to_platform_device(dev);
-	priv->adev = ACPI_COMPANION(dev);
-	priv->dev = dev;
-	priv->phydev = phydev;
-	mutex_init(&priv->an_mutex);
-	INIT_WORK(&priv->an_irq_work, amd_xgbe_an_irq_work);
-	INIT_WORK(&priv->an_work, amd_xgbe_an_state_machine);
-
-	if (!priv->adev || acpi_disabled) {
-		struct device_node *bus_node;
-		struct device_node *phy_node;
-
-		bus_node = priv->dev->of_node;
-		phy_node = of_parse_phandle(bus_node, "phy-handle", 0);
-		if (!phy_node) {
-			dev_err(dev, "unable to parse phy-handle\n");
-			ret = -EINVAL;
-			goto err_priv;
-		}
-
-		phy_pdev = of_find_device_by_node(phy_node);
-		of_node_put(phy_node);
-
-		if (!phy_pdev) {
-			dev_err(dev, "unable to obtain phy device\n");
-			ret = -EINVAL;
-			goto err_priv;
-		}
-
-		phy_resnum = 0;
-		phy_irqnum = 0;
-	} else {
-		/* In ACPI, the XGBE and PHY resources are the grouped
-		 * together with the PHY resources at the end
-		 */
-		phy_pdev = priv->pdev;
-		phy_resnum = amd_xgbe_phy_resource_count(phy_pdev,
-							 IORESOURCE_MEM) - 3;
-		phy_irqnum = amd_xgbe_phy_resource_count(phy_pdev,
-							 IORESOURCE_IRQ) - 1;
-	}
-	phy_dev = &phy_pdev->dev;
-
-	/* Get the device mmio areas */
-	priv->rxtx_res = platform_get_resource(phy_pdev, IORESOURCE_MEM,
-					       phy_resnum++);
-	priv->rxtx_regs = devm_ioremap_resource(dev, priv->rxtx_res);
-	if (IS_ERR(priv->rxtx_regs)) {
-		dev_err(dev, "rxtx ioremap failed\n");
-		ret = PTR_ERR(priv->rxtx_regs);
-		goto err_put;
-	}
-
-	priv->sir0_res = platform_get_resource(phy_pdev, IORESOURCE_MEM,
-					       phy_resnum++);
-	priv->sir0_regs = devm_ioremap_resource(dev, priv->sir0_res);
-	if (IS_ERR(priv->sir0_regs)) {
-		dev_err(dev, "sir0 ioremap failed\n");
-		ret = PTR_ERR(priv->sir0_regs);
-		goto err_rxtx;
-	}
-
-	priv->sir1_res = platform_get_resource(phy_pdev, IORESOURCE_MEM,
-					       phy_resnum++);
-	priv->sir1_regs = devm_ioremap_resource(dev, priv->sir1_res);
-	if (IS_ERR(priv->sir1_regs)) {
-		dev_err(dev, "sir1 ioremap failed\n");
-		ret = PTR_ERR(priv->sir1_regs);
-		goto err_sir0;
-	}
-
-	/* Get the auto-negotiation interrupt */
-	ret = platform_get_irq(phy_pdev, phy_irqnum);
-	if (ret < 0) {
-		dev_err(dev, "platform_get_irq failed\n");
-		goto err_sir1;
-	}
-	priv->an_irq = ret;
-
-	/* Get the device speed set property */
-	ret = device_property_read_u32(phy_dev, XGBE_PHY_SPEEDSET_PROPERTY,
-				       &priv->speed_set);
-	if (ret) {
-		dev_err(dev, "invalid %s property\n",
-			XGBE_PHY_SPEEDSET_PROPERTY);
-		goto err_sir1;
-	}
-
-	switch (priv->speed_set) {
-	case AMD_XGBE_PHY_SPEEDSET_1000_10000:
-	case AMD_XGBE_PHY_SPEEDSET_2500_10000:
-		break;
-	default:
-		dev_err(dev, "invalid %s property\n",
-			XGBE_PHY_SPEEDSET_PROPERTY);
-		ret = -EINVAL;
-		goto err_sir1;
-	}
-
-	if (device_property_present(phy_dev, XGBE_PHY_BLWC_PROPERTY)) {
-		ret = device_property_read_u32_array(phy_dev,
-						     XGBE_PHY_BLWC_PROPERTY,
-						     priv->serdes_blwc,
-						     XGBE_PHY_SPEEDS);
-		if (ret) {
-			dev_err(dev, "invalid %s property\n",
-				XGBE_PHY_BLWC_PROPERTY);
-			goto err_sir1;
-		}
-	} else {
-		memcpy(priv->serdes_blwc, amd_xgbe_phy_serdes_blwc,
-		       sizeof(priv->serdes_blwc));
-	}
-
-	if (device_property_present(phy_dev, XGBE_PHY_CDR_RATE_PROPERTY)) {
-		ret = device_property_read_u32_array(phy_dev,
-						     XGBE_PHY_CDR_RATE_PROPERTY,
-						     priv->serdes_cdr_rate,
-						     XGBE_PHY_SPEEDS);
-		if (ret) {
-			dev_err(dev, "invalid %s property\n",
-				XGBE_PHY_CDR_RATE_PROPERTY);
-			goto err_sir1;
-		}
-	} else {
-		memcpy(priv->serdes_cdr_rate, amd_xgbe_phy_serdes_cdr_rate,
-		       sizeof(priv->serdes_cdr_rate));
-	}
-
-	if (device_property_present(phy_dev, XGBE_PHY_PQ_SKEW_PROPERTY)) {
-		ret = device_property_read_u32_array(phy_dev,
-						     XGBE_PHY_PQ_SKEW_PROPERTY,
-						     priv->serdes_pq_skew,
-						     XGBE_PHY_SPEEDS);
-		if (ret) {
-			dev_err(dev, "invalid %s property\n",
-				XGBE_PHY_PQ_SKEW_PROPERTY);
-			goto err_sir1;
-		}
-	} else {
-		memcpy(priv->serdes_pq_skew, amd_xgbe_phy_serdes_pq_skew,
-		       sizeof(priv->serdes_pq_skew));
-	}
-
-	if (device_property_present(phy_dev, XGBE_PHY_TX_AMP_PROPERTY)) {
-		ret = device_property_read_u32_array(phy_dev,
-						     XGBE_PHY_TX_AMP_PROPERTY,
-						     priv->serdes_tx_amp,
-						     XGBE_PHY_SPEEDS);
-		if (ret) {
-			dev_err(dev, "invalid %s property\n",
-				XGBE_PHY_TX_AMP_PROPERTY);
-			goto err_sir1;
-		}
-	} else {
-		memcpy(priv->serdes_tx_amp, amd_xgbe_phy_serdes_tx_amp,
-		       sizeof(priv->serdes_tx_amp));
-	}
-
-	if (device_property_present(phy_dev, XGBE_PHY_DFE_CFG_PROPERTY)) {
-		ret = device_property_read_u32_array(phy_dev,
-						     XGBE_PHY_DFE_CFG_PROPERTY,
-						     priv->serdes_dfe_tap_cfg,
-						     XGBE_PHY_SPEEDS);
-		if (ret) {
-			dev_err(dev, "invalid %s property\n",
-				XGBE_PHY_DFE_CFG_PROPERTY);
-			goto err_sir1;
-		}
-	} else {
-		memcpy(priv->serdes_dfe_tap_cfg,
-		       amd_xgbe_phy_serdes_dfe_tap_cfg,
-		       sizeof(priv->serdes_dfe_tap_cfg));
-	}
-
-	if (device_property_present(phy_dev, XGBE_PHY_DFE_ENA_PROPERTY)) {
-		ret = device_property_read_u32_array(phy_dev,
-						     XGBE_PHY_DFE_ENA_PROPERTY,
-						     priv->serdes_dfe_tap_ena,
-						     XGBE_PHY_SPEEDS);
-		if (ret) {
-			dev_err(dev, "invalid %s property\n",
-				XGBE_PHY_DFE_ENA_PROPERTY);
-			goto err_sir1;
-		}
-	} else {
-		memcpy(priv->serdes_dfe_tap_ena,
-		       amd_xgbe_phy_serdes_dfe_tap_ena,
-		       sizeof(priv->serdes_dfe_tap_ena));
-	}
-
-	/* Initialize supported features */
-	phydev->supported = SUPPORTED_Autoneg;
-	phydev->supported |= SUPPORTED_Pause | SUPPORTED_Asym_Pause;
-	phydev->supported |= SUPPORTED_Backplane;
-	phydev->supported |= SUPPORTED_10000baseKR_Full;
-	switch (priv->speed_set) {
-	case AMD_XGBE_PHY_SPEEDSET_1000_10000:
-		phydev->supported |= SUPPORTED_1000baseKX_Full;
-		break;
-	case AMD_XGBE_PHY_SPEEDSET_2500_10000:
-		phydev->supported |= SUPPORTED_2500baseX_Full;
-		break;
-	}
-
-	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_PMA_10GBR_FEC_ABILITY);
-	if (ret < 0)
-		return ret;
-	priv->fec_ability = ret & XGBE_PHY_FEC_MASK;
-	if (priv->fec_ability & XGBE_PHY_FEC_ENABLE)
-		phydev->supported |= SUPPORTED_10000baseR_FEC;
-
-	phydev->advertising = phydev->supported;
-
-	phydev->priv = priv;
-
-	if (!priv->adev || acpi_disabled)
-		platform_device_put(phy_pdev);
-
-	return 0;
-
-err_sir1:
-	devm_iounmap(dev, priv->sir1_regs);
-	devm_release_mem_region(dev, priv->sir1_res->start,
-				resource_size(priv->sir1_res));
-
-err_sir0:
-	devm_iounmap(dev, priv->sir0_regs);
-	devm_release_mem_region(dev, priv->sir0_res->start,
-				resource_size(priv->sir0_res));
-
-err_rxtx:
-	devm_iounmap(dev, priv->rxtx_regs);
-	devm_release_mem_region(dev, priv->rxtx_res->start,
-				resource_size(priv->rxtx_res));
-
-err_put:
-	if (!priv->adev || acpi_disabled)
-		platform_device_put(phy_pdev);
-
-err_priv:
-	devm_kfree(dev, priv);
-
-	return ret;
-}
-
-static void amd_xgbe_phy_remove(struct phy_device *phydev)
-{
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
-	struct device *dev = priv->dev;
-
-	if (priv->an_irq_allocated) {
-		devm_free_irq(dev, priv->an_irq, priv);
-
-		flush_workqueue(priv->an_workqueue);
-		destroy_workqueue(priv->an_workqueue);
-	}
-
-	/* Release resources */
-	devm_iounmap(dev, priv->sir1_regs);
-	devm_release_mem_region(dev, priv->sir1_res->start,
-				resource_size(priv->sir1_res));
-
-	devm_iounmap(dev, priv->sir0_regs);
-	devm_release_mem_region(dev, priv->sir0_res->start,
-				resource_size(priv->sir0_res));
-
-	devm_iounmap(dev, priv->rxtx_regs);
-	devm_release_mem_region(dev, priv->rxtx_res->start,
-				resource_size(priv->rxtx_res));
-
-	devm_kfree(dev, priv);
-}
-
-static int amd_xgbe_match_phy_device(struct phy_device *phydev)
-{
-	return phydev->c45_ids.device_ids[MDIO_MMD_PCS] == XGBE_PHY_ID;
-}
-
-static struct phy_driver amd_xgbe_phy_driver[] = {
-	{
-		.phy_id			= XGBE_PHY_ID,
-		.phy_id_mask		= XGBE_PHY_MASK,
-		.name			= "AMD XGBE PHY",
-		.features		= 0,
-		.flags			= PHY_IS_INTERNAL,
-		.probe			= amd_xgbe_phy_probe,
-		.remove			= amd_xgbe_phy_remove,
-		.soft_reset		= amd_xgbe_phy_soft_reset,
-		.config_init		= amd_xgbe_phy_config_init,
-		.suspend		= amd_xgbe_phy_suspend,
-		.resume			= amd_xgbe_phy_resume,
-		.config_aneg		= amd_xgbe_phy_config_aneg,
-		.aneg_done		= amd_xgbe_phy_aneg_done,
-		.read_status		= amd_xgbe_phy_read_status,
-		.match_phy_device	= amd_xgbe_match_phy_device,
-		.driver			= {
-			.owner = THIS_MODULE,
-		},
-	},
-};
-
-module_phy_driver(amd_xgbe_phy_driver);
-
-static struct mdio_device_id __maybe_unused amd_xgbe_phy_ids[] = {
-	{ XGBE_PHY_ID, XGBE_PHY_MASK },
-	{ }
-};
-MODULE_DEVICE_TABLE(mdio, amd_xgbe_phy_ids);
diff --git a/drivers/net/phy/bcm7xxx.c b/drivers/net/phy/bcm7xxx.c
index b5dc59de094e..6b701b3ded74 100644
--- a/drivers/net/phy/bcm7xxx.c
+++ b/drivers/net/phy/bcm7xxx.c
@@ -136,8 +136,8 @@ static int bcm7xxx_28nm_d0_afe_config_init(struct phy_device *phydev)
 	/* AFE_RX_LP_COUNTER, set RX bandwidth to maximum */
 	phy_write_misc(phydev, AFE_RX_LP_COUNTER, 0x7fc0);
 
-	/* AFE_TX_CONFIG, set 1000BT Cfeed=110 for all ports */
-	phy_write_misc(phydev, AFE_TX_CONFIG, 0x0061);
+	/* AFE_TX_CONFIG, set 100BT Cfeed=011 to improve rise/fall time */
+	phy_write_misc(phydev, AFE_TX_CONFIG, 0x431);
 
 	/* AFE_VDCA_ICTRL_0, set Iq=1101 instead of 0111 for AB symmetry */
 	phy_write_misc(phydev, AFE_VDCA_ICTRL_0, 0xa7da);
@@ -167,6 +167,9 @@ static int bcm7xxx_28nm_e0_plus_afe_config_init(struct phy_device *phydev)
 	/* AFE_RXCONFIG_1, provide more margin for INL/DNL measurement */
 	phy_write_misc(phydev, AFE_RXCONFIG_1, 0x9b2f);
 
+	/* AFE_TX_CONFIG, set 100BT Cfeed=011 to improve rise/fall time */
+	phy_write_misc(phydev, AFE_TX_CONFIG, 0x431);
+
 	/* AFE_VDCA_ICTRL_0, set Iq=1101 instead of 0111 for AB symmetry */
 	phy_write_misc(phydev, AFE_VDCA_ICTRL_0, 0xa7da);
 
@@ -243,6 +246,13 @@ static int bcm7xxx_28nm_config_init(struct phy_device *phydev)
 	pr_info_once("%s: %s PHY revision: 0x%02x, patch: %d\n",
 		     dev_name(&phydev->dev), phydev->drv->name, rev, patch);
 
+	/* Dummy read to a register to workaround an issue upon reset where the
+	 * internal inverter may not allow the first MDIO transaction to pass
+	 * the MDIO management controller and make us return 0xffff for such
+	 * reads.
+	 */
+	phy_read(phydev, MII_BMSR);
+
 	switch (rev) {
 	case 0xb0:
 		ret = bcm7xxx_28nm_b0_afe_config_init(phydev);
diff --git a/drivers/net/phy/davicom.c b/drivers/net/phy/davicom.c
index 0d16c7d9e1bf..2a328703b4ae 100644
--- a/drivers/net/phy/davicom.c
+++ b/drivers/net/phy/davicom.c
@@ -158,6 +158,18 @@ static struct phy_driver dm91xx_driver[] = {
 	.config_intr	= dm9161_config_intr,
 	.driver		= { .owner = THIS_MODULE,},
 }, {
+	.phy_id		= 0x0181b8b0,
+	.name		= "Davicom DM9161B/C",
+	.phy_id_mask	= 0x0ffffff0,
+	.features	= PHY_BASIC_FEATURES,
+	.flags		= PHY_HAS_INTERRUPT,
+	.config_init	= dm9161_config_init,
+	.config_aneg	= dm9161_config_aneg,
+	.read_status	= genphy_read_status,
+	.ack_interrupt	= dm9161_ack_interrupt,
+	.config_intr	= dm9161_config_intr,
+	.driver		= { .owner = THIS_MODULE,},
+}, {
 	.phy_id		= 0x0181b8a0,
 	.name		= "Davicom DM9161A",
 	.phy_id_mask	= 0x0ffffff0,
@@ -186,6 +198,7 @@ module_phy_driver(dm91xx_driver);
 
 static struct mdio_device_id __maybe_unused davicom_tbl[] = {
 	{ 0x0181b880, 0x0ffffff0 },
+	{ 0x0181b8b0, 0x0ffffff0 },
 	{ 0x0181b8a0, 0x0ffffff0 },
 	{ 0x00181b80, 0x0ffffff0 },
 	{ }
diff --git a/drivers/net/phy/dp83867.c b/drivers/net/phy/dp83867.c
new file mode 100644
index 000000000000..c7a12e2e07b7
--- /dev/null
+++ b/drivers/net/phy/dp83867.c
@@ -0,0 +1,239 @@
+/*
+ * Driver for the Texas Instruments DP83867 PHY
+ *
+ * Copyright (C) 2015 Texas Instruments Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/ethtool.h>
+#include <linux/kernel.h>
+#include <linux/mii.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/phy.h>
+
+#include <dt-bindings/net/ti-dp83867.h>
+
+#define DP83867_PHY_ID		0x2000a231
+#define DP83867_DEVADDR		0x1f
+
+#define MII_DP83867_PHYCTRL	0x10
+#define MII_DP83867_MICR	0x12
+#define MII_DP83867_ISR		0x13
+#define DP83867_CTRL		0x1f
+
+/* Extended Registers */
+#define DP83867_RGMIICTL	0x0032
+#define DP83867_RGMIIDCTL	0x0086
+
+#define DP83867_SW_RESET	BIT(15)
+#define DP83867_SW_RESTART	BIT(14)
+
+/* MICR Interrupt bits */
+#define MII_DP83867_MICR_AN_ERR_INT_EN		BIT(15)
+#define MII_DP83867_MICR_SPEED_CHNG_INT_EN	BIT(14)
+#define MII_DP83867_MICR_DUP_MODE_CHNG_INT_EN	BIT(13)
+#define MII_DP83867_MICR_PAGE_RXD_INT_EN	BIT(12)
+#define MII_DP83867_MICR_AUTONEG_COMP_INT_EN	BIT(11)
+#define MII_DP83867_MICR_LINK_STS_CHNG_INT_EN	BIT(10)
+#define MII_DP83867_MICR_FALSE_CARRIER_INT_EN	BIT(8)
+#define MII_DP83867_MICR_SLEEP_MODE_CHNG_INT_EN	BIT(4)
+#define MII_DP83867_MICR_WOL_INT_EN		BIT(3)
+#define MII_DP83867_MICR_XGMII_ERR_INT_EN	BIT(2)
+#define MII_DP83867_MICR_POL_CHNG_INT_EN	BIT(1)
+#define MII_DP83867_MICR_JABBER_INT_EN		BIT(0)
+
+/* RGMIICTL bits */
+#define DP83867_RGMII_TX_CLK_DELAY_EN		BIT(1)
+#define DP83867_RGMII_RX_CLK_DELAY_EN		BIT(0)
+
+/* PHY CTRL bits */
+#define DP83867_PHYCR_FIFO_DEPTH_SHIFT		14
+
+/* RGMIIDCTL bits */
+#define DP83867_RGMII_TX_CLK_DELAY_SHIFT	4
+
+struct dp83867_private {
+	int rx_id_delay;
+	int tx_id_delay;
+	int fifo_depth;
+};
+
+static int dp83867_ack_interrupt(struct phy_device *phydev)
+{
+	int err = phy_read(phydev, MII_DP83867_ISR);
+
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
+static int dp83867_config_intr(struct phy_device *phydev)
+{
+	int micr_status;
+
+	if (phydev->interrupts == PHY_INTERRUPT_ENABLED) {
+		micr_status = phy_read(phydev, MII_DP83867_MICR);
+		if (micr_status < 0)
+			return micr_status;
+
+		micr_status |=
+			(MII_DP83867_MICR_AN_ERR_INT_EN |
+			MII_DP83867_MICR_SPEED_CHNG_INT_EN |
+			MII_DP83867_MICR_DUP_MODE_CHNG_INT_EN |
+			MII_DP83867_MICR_SLEEP_MODE_CHNG_INT_EN);
+
+		return phy_write(phydev, MII_DP83867_MICR, micr_status);
+	}
+
+	micr_status = 0x0;
+	return phy_write(phydev, MII_DP83867_MICR, micr_status);
+}
+
+#ifdef CONFIG_OF_MDIO
+static int dp83867_of_init(struct phy_device *phydev)
+{
+	struct dp83867_private *dp83867 = phydev->priv;
+	struct device *dev = &phydev->dev;
+	struct device_node *of_node = dev->of_node;
+	int ret;
+
+	if (!of_node && dev->parent->of_node)
+		of_node = dev->parent->of_node;
+
+	if (!phydev->dev.of_node)
+		return -ENODEV;
+
+	ret = of_property_read_u32(of_node, "ti,rx-internal-delay",
+				   &dp83867->rx_id_delay);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(of_node, "ti,tx-internal-delay",
+				   &dp83867->tx_id_delay);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(of_node, "ti,fifo-depth",
+				   &dp83867->fifo_depth);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+#else
+static int dp83867_of_init(struct phy_device *phydev)
+{
+	return 0;
+}
+#endif /* CONFIG_OF_MDIO */
+
+static int dp83867_config_init(struct phy_device *phydev)
+{
+	struct dp83867_private *dp83867;
+	int ret;
+	u16 val, delay;
+
+	if (!phydev->priv) {
+		dp83867 = devm_kzalloc(&phydev->dev, sizeof(*dp83867),
+				       GFP_KERNEL);
+		if (!dp83867)
+			return -ENOMEM;
+
+		phydev->priv = dp83867;
+		ret = dp83867_of_init(phydev);
+		if (ret)
+			return ret;
+	} else {
+		dp83867 = (struct dp83867_private *)phydev->priv;
+	}
+
+	if (phy_interface_is_rgmii(phydev)) {
+		ret = phy_write(phydev, MII_DP83867_PHYCTRL,
+			(dp83867->fifo_depth << DP83867_PHYCR_FIFO_DEPTH_SHIFT));
+		if (ret)
+			return ret;
+	}
+
+	if ((phydev->interface >= PHY_INTERFACE_MODE_RGMII_ID) ||
+	    (phydev->interface <= PHY_INTERFACE_MODE_RGMII_RXID)) {
+		val = phy_read_mmd_indirect(phydev, DP83867_RGMIICTL,
+					    DP83867_DEVADDR, phydev->addr);
+
+		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID)
+			val |= (DP83867_RGMII_TX_CLK_DELAY_EN | DP83867_RGMII_RX_CLK_DELAY_EN);
+
+		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)
+			val |= DP83867_RGMII_TX_CLK_DELAY_EN;
+
+		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID)
+			val |= DP83867_RGMII_RX_CLK_DELAY_EN;
+
+		phy_write_mmd_indirect(phydev, DP83867_RGMIICTL,
+				       DP83867_DEVADDR, phydev->addr, val);
+
+		delay = (dp83867->rx_id_delay |
+			(dp83867->tx_id_delay << DP83867_RGMII_TX_CLK_DELAY_SHIFT));
+
+		phy_write_mmd_indirect(phydev, DP83867_RGMIIDCTL,
+				       DP83867_DEVADDR, phydev->addr, delay);
+	}
+
+	return 0;
+}
+
+static int dp83867_phy_reset(struct phy_device *phydev)
+{
+	int err;
+
+	err = phy_write(phydev, DP83867_CTRL, DP83867_SW_RESET);
+	if (err < 0)
+		return err;
+
+	return dp83867_config_init(phydev);
+}
+
+static struct phy_driver dp83867_driver[] = {
+	{
+		.phy_id		= DP83867_PHY_ID,
+		.phy_id_mask	= 0xfffffff0,
+		.name		= "TI DP83867",
+		.features	= PHY_GBIT_FEATURES,
+		.flags		= PHY_HAS_INTERRUPT,
+
+		.config_init	= dp83867_config_init,
+		.soft_reset	= dp83867_phy_reset,
+
+		/* IRQ related */
+		.ack_interrupt	= dp83867_ack_interrupt,
+		.config_intr	= dp83867_config_intr,
+
+		.config_aneg	= genphy_config_aneg,
+		.read_status	= genphy_read_status,
+		.suspend	= genphy_suspend,
+		.resume		= genphy_resume,
+
+		.driver		= {.owner = THIS_MODULE,}
+	},
+};
+module_phy_driver(dp83867_driver);
+
+static struct mdio_device_id __maybe_unused dp83867_tbl[] = {
+	{ DP83867_PHY_ID, 0xfffffff0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(mdio, dp83867_tbl);
+
+MODULE_DESCRIPTION("Texas Instruments DP83867 PHY driver");
+MODULE_AUTHOR("Dan Murphy <dmurphy@ti.com");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/phy/icplus.c b/drivers/net/phy/icplus.c
index 8644f039d922..0dbc445a5fa0 100644
--- a/drivers/net/phy/icplus.c
+++ b/drivers/net/phy/icplus.c
@@ -139,10 +139,7 @@ static int ip1001_config_init(struct phy_device *phydev)
 	if (c < 0)
 		return c;
 
-	if ((phydev->interface == PHY_INTERFACE_MODE_RGMII) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)) {
+	if (phy_interface_is_rgmii(phydev)) {
 
 		c = phy_read(phydev, IP10XX_SPEC_CTRL_STATUS);
 		if (c < 0)
diff --git a/drivers/net/phy/marvell.c b/drivers/net/phy/marvell.c
index 1b1698f98818..f721444c2b0a 100644
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -317,10 +317,7 @@ static int m88e1121_config_aneg(struct phy_device *phydev)
 	if (err < 0)
 		return err;
 
-	if ((phydev->interface == PHY_INTERFACE_MODE_RGMII) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)) {
+	if (phy_interface_is_rgmii(phydev)) {
 
 		mscr = phy_read(phydev, MII_88E1121_PHY_MSCR_REG) &
 			MII_88E1121_PHY_MSCR_DELAY_MASK;
@@ -469,10 +466,7 @@ static int m88e1111_config_init(struct phy_device *phydev)
 	int err;
 	int temp;
 
-	if ((phydev->interface == PHY_INTERFACE_MODE_RGMII) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID)) {
+	if (phy_interface_is_rgmii(phydev)) {
 
 		temp = phy_read(phydev, MII_M1111_PHY_EXT_CR);
 		if (temp < 0)
diff --git a/drivers/net/phy/mdio-bcm-unimac.c b/drivers/net/phy/mdio-bcm-unimac.c
index 414fdf1f343f..6a52a7f0fa0d 100644
--- a/drivers/net/phy/mdio-bcm-unimac.c
+++ b/drivers/net/phy/mdio-bcm-unimac.c
@@ -81,7 +81,13 @@ static int unimac_mdio_read(struct mii_bus *bus, int phy_id, int reg)
 		return -ETIMEDOUT;
 
 	cmd = __raw_readl(priv->base + MDIO_CMD);
-	if (cmd & MDIO_READ_FAIL)
+
+	/* Some broken devices are known not to release the line during
+	 * turn-around, e.g: Broadcom BCM53125 external switches, so check for
+	 * that condition here and ignore the MDIO controller read failure
+	 * indication.
+	 */
+	if (!(bus->phy_ignore_ta_mask & 1 << phy_id) && (cmd & MDIO_READ_FAIL))
 		return -EIO;
 
 	return cmd & 0xffff;
@@ -114,6 +120,48 @@ static int unimac_mdio_write(struct mii_bus *bus, int phy_id,
 	return 0;
 }
 
+/* Workaround for integrated BCM7xxx Gigabit PHYs which have a problem with
+ * their internal MDIO management controller making them fail to successfully
+ * be read from or written to for the first transaction.  We insert a dummy
+ * BMSR read here to make sure that phy_get_device() and get_phy_id() can
+ * correctly read the PHY MII_PHYSID1/2 registers and successfully register a
+ * PHY device for this peripheral.
+ *
+ * Once the PHY driver is registered, we can workaround subsequent reads from
+ * there (e.g: during system-wide power management).
+ *
+ * bus->reset is invoked before mdiobus_scan during mdiobus_register and is
+ * therefore the right location to stick that workaround. Since we do not want
+ * to read from non-existing PHYs, we either use bus->phy_mask or do a manual
+ * Device Tree scan to limit the search area.
+ */
+static int unimac_mdio_reset(struct mii_bus *bus)
+{
+	struct device_node *np = bus->dev.of_node;
+	struct device_node *child;
+	u32 read_mask = 0;
+	int addr;
+
+	if (!np) {
+		read_mask = ~bus->phy_mask;
+	} else {
+		for_each_available_child_of_node(np, child) {
+			addr = of_mdio_parse_addr(&bus->dev, child);
+			if (addr < 0)
+				continue;
+
+			read_mask |= 1 << addr;
+		}
+	}
+
+	for (addr = 0; addr < PHY_MAX_ADDR; addr++) {
+		if (read_mask & 1 << addr)
+			mdiobus_read(bus, addr, MII_BMSR);
+	}
+
+	return 0;
+}
+
 static int unimac_mdio_probe(struct platform_device *pdev)
 {
 	struct unimac_mdio_priv *priv;
@@ -149,6 +197,7 @@ static int unimac_mdio_probe(struct platform_device *pdev)
 	bus->parent = &pdev->dev;
 	bus->read = unimac_mdio_read;
 	bus->write = unimac_mdio_write;
+	bus->reset = unimac_mdio_reset;
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%s", pdev->name);
 
 	bus->irq = kcalloc(PHY_MAX_ADDR, sizeof(int), GFP_KERNEL);
diff --git a/drivers/net/phy/mdio-bitbang.c b/drivers/net/phy/mdio-bitbang.c
index daec9b05d168..61a543c788cc 100644
--- a/drivers/net/phy/mdio-bitbang.c
+++ b/drivers/net/phy/mdio-bitbang.c
@@ -165,8 +165,11 @@ static int mdiobb_read(struct mii_bus *bus, int phy, int reg)
 
 	ctrl->ops->set_mdio_dir(ctrl, 0);
 
-	/* check the turnaround bit: the PHY should be driving it to zero */
-	if (mdiobb_get_bit(ctrl) != 0) {
+	/* check the turnaround bit: the PHY should be driving it to zero, if this
+	 * PHY is listed in phy_ignore_ta_mask as having broken TA, skip that
+	 */
+	if (mdiobb_get_bit(ctrl) != 0 &&
+	    !(bus->phy_ignore_ta_mask & (1 << phy))) {
 		/* PHY didn't drive TA low -- flush any bits it
 		 * may be trying to send.
 		 */
diff --git a/drivers/net/phy/mdio-gpio.c b/drivers/net/phy/mdio-gpio.c
index 53d18150f4e2..7dc21e56a7aa 100644
--- a/drivers/net/phy/mdio-gpio.c
+++ b/drivers/net/phy/mdio-gpio.c
@@ -158,6 +158,7 @@ static struct mii_bus *mdio_gpio_bus_init(struct device *dev,
 	new_bus->name = "GPIO Bitbanged MDIO",
 
 	new_bus->phy_mask = pdata->phy_mask;
+	new_bus->phy_ignore_ta_mask = pdata->phy_ignore_ta_mask;
 	new_bus->irq = pdata->irqs;
 	new_bus->parent = dev;
 
diff --git a/drivers/net/phy/mdio-mux-gpio.c b/drivers/net/phy/mdio-mux-gpio.c
index 66edd99bc302..7ddb1ab70891 100644
--- a/drivers/net/phy/mdio-mux-gpio.c
+++ b/drivers/net/phy/mdio-mux-gpio.c
@@ -35,7 +35,8 @@ static int mdio_mux_gpio_switch_fn(int current_child, int desired_child,
 	for (n = 0; n < s->gpios->ndescs; n++)
 		values[n] = (desired_child >> n) & 1;
 
-	gpiod_set_array_cansleep(s->gpios->ndescs, s->gpios->desc, values);
+	gpiod_set_array_value_cansleep(s->gpios->ndescs, s->gpios->desc,
+				       values);
 
 	return 0;
 }
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index ebdc357c5131..499185eaf413 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -288,9 +288,10 @@ static int kszphy_config_init(struct phy_device *phydev)
 }
 
 static int ksz9021_load_values_from_of(struct phy_device *phydev,
-				       struct device_node *of_node, u16 reg,
-				       char *field1, char *field2,
-				       char *field3, char *field4)
+				       const struct device_node *of_node,
+				       u16 reg,
+				       const char *field1, const char *field2,
+				       const char *field3, const char *field4)
 {
 	int val1 = -1;
 	int val2 = -2;
@@ -336,8 +337,8 @@ static int ksz9021_load_values_from_of(struct phy_device *phydev,
 
 static int ksz9021_config_init(struct phy_device *phydev)
 {
-	struct device *dev = &phydev->dev;
-	struct device_node *of_node = dev->of_node;
+	const struct device *dev = &phydev->dev;
+	const struct device_node *of_node = dev->of_node;
 
 	if (!of_node && dev->parent->of_node)
 		of_node = dev->parent->of_node;
@@ -365,6 +366,11 @@ static int ksz9021_config_init(struct phy_device *phydev)
 #define KSZ9031_PS_TO_REG		60
 
 /* Extended registers */
+/* MMD Address 0x0 */
+#define MII_KSZ9031RN_FLP_BURST_TX_LO	3
+#define MII_KSZ9031RN_FLP_BURST_TX_HI	4
+
+/* MMD Address 0x2 */
 #define MII_KSZ9031RN_CONTROL_PAD_SKEW	4
 #define MII_KSZ9031RN_RX_DATA_PAD_SKEW	5
 #define MII_KSZ9031RN_TX_DATA_PAD_SKEW	6
@@ -389,9 +395,9 @@ static int ksz9031_extended_read(struct phy_device *phydev,
 }
 
 static int ksz9031_of_load_skew_values(struct phy_device *phydev,
-				       struct device_node *of_node,
+				       const struct device_node *of_node,
 				       u16 reg, size_t field_sz,
-				       char *field[], u8 numfields)
+				       const char *field[], u8 numfields)
 {
 	int val[4] = {-1, -2, -3, -4};
 	int matches = 0;
@@ -425,20 +431,36 @@ static int ksz9031_of_load_skew_values(struct phy_device *phydev,
 	return ksz9031_extended_write(phydev, OP_DATA, 2, reg, newval);
 }
 
+static int ksz9031_center_flp_timing(struct phy_device *phydev)
+{
+	int result;
+
+	/* Center KSZ9031RNX FLP timing at 16ms. */
+	result = ksz9031_extended_write(phydev, OP_DATA, 0,
+					MII_KSZ9031RN_FLP_BURST_TX_HI, 0x0006);
+	result = ksz9031_extended_write(phydev, OP_DATA, 0,
+					MII_KSZ9031RN_FLP_BURST_TX_LO, 0x1A80);
+
+	if (result)
+		return result;
+
+	return genphy_restart_aneg(phydev);
+}
+
 static int ksz9031_config_init(struct phy_device *phydev)
 {
-	struct device *dev = &phydev->dev;
-	struct device_node *of_node = dev->of_node;
-	char *clk_skews[2] = {"rxc-skew-ps", "txc-skew-ps"};
-	char *rx_data_skews[4] = {
+	const struct device *dev = &phydev->dev;
+	const struct device_node *of_node = dev->of_node;
+	static const char *clk_skews[2] = {"rxc-skew-ps", "txc-skew-ps"};
+	static const char *rx_data_skews[4] = {
 		"rxd0-skew-ps", "rxd1-skew-ps",
 		"rxd2-skew-ps", "rxd3-skew-ps"
 	};
-	char *tx_data_skews[4] = {
+	static const char *tx_data_skews[4] = {
 		"txd0-skew-ps", "txd1-skew-ps",
 		"txd2-skew-ps", "txd3-skew-ps"
 	};
-	char *control_skews[2] = {"txen-skew-ps", "rxdv-skew-ps"};
+	static const char *control_skews[2] = {"txen-skew-ps", "rxdv-skew-ps"};
 
 	if (!of_node && dev->parent->of_node)
 		of_node = dev->parent->of_node;
@@ -460,7 +482,8 @@ static int ksz9031_config_init(struct phy_device *phydev)
 				MII_KSZ9031RN_TX_DATA_PAD_SKEW, 4,
 				tx_data_skews, 4);
 	}
-	return 0;
+
+	return ksz9031_center_flp_timing(phydev);
 }
 
 #define KSZ8873MLL_GLOBAL_CONTROL_4	0x06
@@ -519,7 +542,7 @@ ksz9021_wr_mmd_phyreg(struct phy_device *phydev, int ptrad, int devnum,
 static int kszphy_probe(struct phy_device *phydev)
 {
 	const struct kszphy_type *type = phydev->drv->driver_data;
-	struct device_node *np = phydev->dev.of_node;
+	const struct device_node *np = phydev->dev.of_node;
 	struct kszphy_priv *priv;
 	struct clk *clk;
 	int ret;
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index 47cd578052fc..b2197b506acb 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -58,6 +58,31 @@ static const char *phy_speed_to_str(int speed)
 	}
 }
 
+#define PHY_STATE_STR(_state)			\
+	case PHY_##_state:			\
+		return __stringify(_state);	\
+
+static const char *phy_state_to_str(enum phy_state st)
+{
+	switch (st) {
+	PHY_STATE_STR(DOWN)
+	PHY_STATE_STR(STARTING)
+	PHY_STATE_STR(READY)
+	PHY_STATE_STR(PENDING)
+	PHY_STATE_STR(UP)
+	PHY_STATE_STR(AN)
+	PHY_STATE_STR(RUNNING)
+	PHY_STATE_STR(NOLINK)
+	PHY_STATE_STR(FORCING)
+	PHY_STATE_STR(CHANGELINK)
+	PHY_STATE_STR(HALTED)
+	PHY_STATE_STR(RESUMING)
+	}
+
+	return NULL;
+}
+
+
 /**
  * phy_print_status - Convenience function to print out the current phy status
  * @phydev: the phy_device struct
@@ -784,10 +809,13 @@ void phy_state_machine(struct work_struct *work)
 	struct phy_device *phydev =
 			container_of(dwork, struct phy_device, state_queue);
 	bool needs_aneg = false, do_suspend = false;
+	enum phy_state old_state;
 	int err = 0;
 
 	mutex_lock(&phydev->lock);
 
+	old_state = phydev->state;
+
 	if (phydev->drv->link_change_notify)
 		phydev->drv->link_change_notify(phydev);
 
@@ -952,6 +980,9 @@ void phy_state_machine(struct work_struct *work)
 	if (err < 0)
 		phy_error(phydev);
 
+	dev_dbg(&phydev->dev, "PHY state change %s -> %s\n",
+		phy_state_to_str(old_state), phy_state_to_str(phydev->state));
+
 	queue_delayed_work(system_power_efficient_wq, &phydev->state_queue,
 			   PHY_STATE_TIME * HZ);
 }
@@ -1062,8 +1093,7 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 	if ((phydev->duplex == DUPLEX_FULL) &&
 	    ((phydev->interface == PHY_INTERFACE_MODE_MII) ||
 	    (phydev->interface == PHY_INTERFACE_MODE_GMII) ||
-	    (phydev->interface >= PHY_INTERFACE_MODE_RGMII &&
-	     phydev->interface <= PHY_INTERFACE_MODE_RGMII_TXID) ||
+	     phy_interface_is_rgmii(phydev) ||
 	     phy_is_internal(phydev))) {
 		int eee_lp, eee_cap, eee_adv;
 		u32 lp, cap, adv;
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index bdfe51fc3a65..0302483de240 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -230,7 +230,7 @@ static int get_phy_c45_ids(struct mii_bus *bus, int addr, u32 *phy_id,
 	for (i = 1;
 	     i < num_ids && c45_ids->devices_in_package == 0;
 	     i++) {
-		reg_addr = MII_ADDR_C45 | i << 16 | MDIO_DEVS2;
+retry:		reg_addr = MII_ADDR_C45 | i << 16 | MDIO_DEVS2;
 		phy_reg = mdiobus_read(bus, addr, reg_addr);
 		if (phy_reg < 0)
 			return -EIO;
@@ -242,12 +242,20 @@ static int get_phy_c45_ids(struct mii_bus *bus, int addr, u32 *phy_id,
 			return -EIO;
 		c45_ids->devices_in_package |= (phy_reg & 0xffff);
 
-		/* If mostly Fs, there is no device there,
-		 * let's get out of here.
-		 */
 		if ((c45_ids->devices_in_package & 0x1fffffff) == 0x1fffffff) {
-			*phy_id = 0xffffffff;
-			return 0;
+			if (i) {
+				/*  If mostly Fs, there is no device there,
+				 *  then let's continue to probe more, as some
+				 *  10G PHYs have zero Devices In package,
+				 *  e.g. Cortina CS4315/CS4340 PHY.
+				 */
+				i = 0;
+				goto retry;
+			} else {
+				/* no device there, let's get out of here */
+				*phy_id = 0xffffffff;
+				return 0;
+			}
 		}
 	}
 
@@ -796,10 +804,11 @@ static int genphy_config_advert(struct phy_device *phydev)
 	if (phydev->supported & (SUPPORTED_1000baseT_Half |
 				 SUPPORTED_1000baseT_Full)) {
 		adv |= ethtool_adv_to_mii_ctrl1000_t(advertise);
-		if (adv != oldadv)
-			changed = 1;
 	}
 
+	if (adv != oldadv)
+		changed = 1;
+
 	err = phy_write(phydev, MII_CTRL1000, adv);
 	if (err < 0)
 		return err;
diff --git a/drivers/net/phy/realtek.c b/drivers/net/phy/realtek.c
index 96a0f0fab3ca..45353613b2ed 100644
--- a/drivers/net/phy/realtek.c
+++ b/drivers/net/phy/realtek.c
@@ -22,8 +22,12 @@
 #define RTL821x_INER		0x12
 #define RTL821x_INER_INIT	0x6400
 #define RTL821x_INSR		0x13
+#define RTL8211E_INER_LINK_STATUS 0x400
 
-#define	RTL8211E_INER_LINK_STATUS	0x400
+#define RTL8211F_INER_LINK_STATUS 0x0010
+#define RTL8211F_INSR		0x1d
+#define RTL8211F_PAGE_SELECT	0x1f
+#define RTL8211F_TX_DELAY	0x100
 
 MODULE_DESCRIPTION("Realtek PHY driver");
 MODULE_AUTHOR("Johnson Leung");
@@ -38,6 +42,18 @@ static int rtl821x_ack_interrupt(struct phy_device *phydev)
 	return (err < 0) ? err : 0;
 }
 
+static int rtl8211f_ack_interrupt(struct phy_device *phydev)
+{
+	int err;
+
+	phy_write(phydev, RTL8211F_PAGE_SELECT, 0xa43);
+	err = phy_read(phydev, RTL8211F_INSR);
+	/* restore to default page 0 */
+	phy_write(phydev, RTL8211F_PAGE_SELECT, 0x0);
+
+	return (err < 0) ? err : 0;
+}
+
 static int rtl8211b_config_intr(struct phy_device *phydev)
 {
 	int err;
@@ -64,6 +80,41 @@ static int rtl8211e_config_intr(struct phy_device *phydev)
 	return err;
 }
 
+static int rtl8211f_config_intr(struct phy_device *phydev)
+{
+	int err;
+
+	if (phydev->interrupts == PHY_INTERRUPT_ENABLED)
+		err = phy_write(phydev, RTL821x_INER,
+				RTL8211F_INER_LINK_STATUS);
+	else
+		err = phy_write(phydev, RTL821x_INER, 0);
+
+	return err;
+}
+
+static int rtl8211f_config_init(struct phy_device *phydev)
+{
+	int ret;
+	u16 reg;
+
+	ret = genphy_config_init(phydev);
+	if (ret < 0)
+		return ret;
+
+	if (phydev->interface == PHY_INTERFACE_MODE_RGMII) {
+		/* enable TXDLY */
+		phy_write(phydev, RTL8211F_PAGE_SELECT, 0xd08);
+		reg = phy_read(phydev, 0x11);
+		reg |= RTL8211F_TX_DELAY;
+		phy_write(phydev, 0x11, reg);
+		/* restore to default page 0 */
+		phy_write(phydev, RTL8211F_PAGE_SELECT, 0x0);
+	}
+
+	return 0;
+}
+
 static struct phy_driver realtek_drvs[] = {
 	{
 		.phy_id         = 0x00008201,
@@ -98,6 +149,20 @@ static struct phy_driver realtek_drvs[] = {
 		.suspend	= genphy_suspend,
 		.resume		= genphy_resume,
 		.driver		= { .owner = THIS_MODULE,},
+	}, {
+		.phy_id		= 0x001cc916,
+		.name		= "RTL8211F Gigabit Ethernet",
+		.phy_id_mask	= 0x001fffff,
+		.features	= PHY_GBIT_FEATURES,
+		.flags		= PHY_HAS_INTERRUPT,
+		.config_aneg	= &genphy_config_aneg,
+		.config_init	= &rtl8211f_config_init,
+		.read_status	= &genphy_read_status,
+		.ack_interrupt	= &rtl8211f_ack_interrupt,
+		.config_intr	= &rtl8211f_config_intr,
+		.suspend	= genphy_suspend,
+		.resume		= genphy_resume,
+		.driver		= { .owner = THIS_MODULE },
 	},
 };
 
@@ -106,6 +171,7 @@ module_phy_driver(realtek_drvs);
 static struct mdio_device_id __maybe_unused realtek_tbl[] = {
 	{ 0x001cc912, 0x001fffff },
 	{ 0x001cc915, 0x001fffff },
+	{ 0x001cc916, 0x001fffff },
 	{ }
 };
 
diff --git a/drivers/net/phy/vitesse.c b/drivers/net/phy/vitesse.c
index 76cad712ddb2..17cad185169d 100644
--- a/drivers/net/phy/vitesse.c
+++ b/drivers/net/phy/vitesse.c
@@ -66,6 +66,7 @@
 #define PHY_ID_VSC8244			0x000fc6c0
 #define PHY_ID_VSC8514			0x00070670
 #define PHY_ID_VSC8574			0x000704a0
+#define PHY_ID_VSC8641			0x00070431
 #define PHY_ID_VSC8662			0x00070660
 #define PHY_ID_VSC8221			0x000fc550
 #define PHY_ID_VSC8211			0x000fc4b0
@@ -272,6 +273,18 @@ static struct phy_driver vsc82xx_driver[] = {
 	.config_intr    = &vsc82xx_config_intr,
 	.driver         = { .owner = THIS_MODULE,},
 }, {
+	.phy_id         = PHY_ID_VSC8641,
+	.name           = "Vitesse VSC8641",
+	.phy_id_mask    = 0x000ffff0,
+	.features       = PHY_GBIT_FEATURES,
+	.flags          = PHY_HAS_INTERRUPT,
+	.config_init    = &vsc824x_config_init,
+	.config_aneg    = &vsc82x4_config_aneg,
+	.read_status    = &genphy_read_status,
+	.ack_interrupt  = &vsc824x_ack_interrupt,
+	.config_intr    = &vsc82xx_config_intr,
+	.driver         = { .owner = THIS_MODULE,},
+}, {
 	.phy_id         = PHY_ID_VSC8662,
 	.name           = "Vitesse VSC8662",
 	.phy_id_mask    = 0x000ffff0,
@@ -318,6 +331,7 @@ static struct mdio_device_id __maybe_unused vitesse_tbl[] = {
 	{ PHY_ID_VSC8244, 0x000fffc0 },
 	{ PHY_ID_VSC8514, 0x000ffff0 },
 	{ PHY_ID_VSC8574, 0x000ffff0 },
+	{ PHY_ID_VSC8641, 0x000ffff0 },
 	{ PHY_ID_VSC8662, 0x000ffff0 },
 	{ PHY_ID_VSC8221, 0x000ffff0 },
 	{ PHY_ID_VSC8211, 0x000ffff0 },
diff --git a/drivers/net/ppp/pppoe.c b/drivers/net/ppp/pppoe.c
index b62a5e3a1c65..3837ae344f63 100644
--- a/drivers/net/ppp/pppoe.c
+++ b/drivers/net/ppp/pppoe.c
@@ -550,11 +550,11 @@ static struct proto pppoe_sk_proto __read_mostly = {
  * Initialize a new struct sock.
  *
  **********************************************************************/
-static int pppoe_create(struct net *net, struct socket *sock)
+static int pppoe_create(struct net *net, struct socket *sock, int kern)
 {
 	struct sock *sk;
 
-	sk = sk_alloc(net, PF_PPPOX, GFP_KERNEL, &pppoe_sk_proto);
+	sk = sk_alloc(net, PF_PPPOX, GFP_KERNEL, &pppoe_sk_proto, kern);
 	if (!sk)
 		return -ENOMEM;
 
diff --git a/drivers/net/ppp/pppox.c b/drivers/net/ppp/pppox.c
index 2940e9fe351b..0e1b30622477 100644
--- a/drivers/net/ppp/pppox.c
+++ b/drivers/net/ppp/pppox.c
@@ -118,7 +118,7 @@ static int pppox_create(struct net *net, struct socket *sock, int protocol,
 	    !try_module_get(pppox_protos[protocol]->owner))
 		goto out;
 
-	rc = pppox_protos[protocol]->create(net, sock);
+	rc = pppox_protos[protocol]->create(net, sock, kern);
 
 	module_put(pppox_protos[protocol]->owner);
 out:
diff --git a/drivers/net/ppp/pptp.c b/drivers/net/ppp/pptp.c
index e3bfbd4d0136..686f37daa262 100644
--- a/drivers/net/ppp/pptp.c
+++ b/drivers/net/ppp/pptp.c
@@ -28,8 +28,6 @@
 #include <linux/file.h>
 #include <linux/in.h>
 #include <linux/ip.h>
-#include <linux/netfilter.h>
-#include <linux/netfilter_ipv4.h>
 #include <linux/rcupdate.h>
 #include <linux/spinlock.h>
 
@@ -561,14 +559,14 @@ static void pptp_sock_destruct(struct sock *sk)
 	skb_queue_purge(&sk->sk_receive_queue);
 }
 
-static int pptp_create(struct net *net, struct socket *sock)
+static int pptp_create(struct net *net, struct socket *sock, int kern)
 {
 	int error = -ENOMEM;
 	struct sock *sk;
 	struct pppox_sock *po;
 	struct pptp_opt *opt;
 
-	sk = sk_alloc(net, PF_PPPOX, GFP_KERNEL, &pptp_sk_proto);
+	sk = sk_alloc(net, PF_PPPOX, GFP_KERNEL, &pptp_sk_proto, kern);
 	if (!sk)
 		goto out;
 
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index 6928448f6b7f..daa054b3ff03 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -1924,7 +1924,7 @@ static netdev_features_t team_fix_features(struct net_device *dev,
 	struct team *team = netdev_priv(dev);
 	netdev_features_t mask;
 
-	mask = features | NETIF_F_HW_SWITCH_OFFLOAD;
+	mask = features;
 	features &= ~NETIF_F_ONE_FOR_ALL;
 	features |= NETIF_F_ALL_FOR_ALL;
 
@@ -1977,8 +1977,12 @@ static const struct net_device_ops team_netdev_ops = {
 	.ndo_del_slave		= team_del_slave,
 	.ndo_fix_features	= team_fix_features,
 	.ndo_change_carrier     = team_change_carrier,
-	.ndo_bridge_setlink     = ndo_dflt_netdev_switch_port_bridge_setlink,
-	.ndo_bridge_dellink     = ndo_dflt_netdev_switch_port_bridge_dellink,
+	.ndo_bridge_setlink	= switchdev_port_bridge_setlink,
+	.ndo_bridge_getlink	= switchdev_port_bridge_getlink,
+	.ndo_bridge_dellink	= switchdev_port_bridge_dellink,
+	.ndo_fdb_add		= switchdev_port_fdb_add,
+	.ndo_fdb_del		= switchdev_port_fdb_del,
+	.ndo_fdb_dump		= switchdev_port_fdb_dump,
 	.ndo_features_check	= passthru_features_check,
 };
 
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index e470ae59d405..06a039414628 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -111,6 +111,7 @@ do {								\
 #define TUN_FASYNC	IFF_ATTACH_QUEUE
 /* High bits in flags field are unused. */
 #define TUN_VNET_LE     0x80000000
+#define TUN_VNET_BE     0x40000000
 
 #define TUN_FEATURES (IFF_NO_PI | IFF_ONE_QUEUE | IFF_VNET_HDR | \
 		      IFF_MULTI_QUEUE)
@@ -146,7 +147,6 @@ struct tun_file {
 	struct socket socket;
 	struct socket_wq wq;
 	struct tun_struct __rcu *tun;
-	struct net *net;
 	struct fasync_struct *fasync;
 	/* only used for fasnyc */
 	unsigned int flags;
@@ -206,14 +206,68 @@ struct tun_struct {
 	u32 flow_count;
 };
 
+#ifdef CONFIG_TUN_VNET_CROSS_LE
+static inline bool tun_legacy_is_little_endian(struct tun_struct *tun)
+{
+	return tun->flags & TUN_VNET_BE ? false :
+		virtio_legacy_is_little_endian();
+}
+
+static long tun_get_vnet_be(struct tun_struct *tun, int __user *argp)
+{
+	int be = !!(tun->flags & TUN_VNET_BE);
+
+	if (put_user(be, argp))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long tun_set_vnet_be(struct tun_struct *tun, int __user *argp)
+{
+	int be;
+
+	if (get_user(be, argp))
+		return -EFAULT;
+
+	if (be)
+		tun->flags |= TUN_VNET_BE;
+	else
+		tun->flags &= ~TUN_VNET_BE;
+
+	return 0;
+}
+#else
+static inline bool tun_legacy_is_little_endian(struct tun_struct *tun)
+{
+	return virtio_legacy_is_little_endian();
+}
+
+static long tun_get_vnet_be(struct tun_struct *tun, int __user *argp)
+{
+	return -EINVAL;
+}
+
+static long tun_set_vnet_be(struct tun_struct *tun, int __user *argp)
+{
+	return -EINVAL;
+}
+#endif /* CONFIG_TUN_VNET_CROSS_LE */
+
+static inline bool tun_is_little_endian(struct tun_struct *tun)
+{
+	return tun->flags & TUN_VNET_LE ||
+		tun_legacy_is_little_endian(tun);
+}
+
 static inline u16 tun16_to_cpu(struct tun_struct *tun, __virtio16 val)
 {
-	return __virtio16_to_cpu(tun->flags & TUN_VNET_LE, val);
+	return __virtio16_to_cpu(tun_is_little_endian(tun), val);
 }
 
 static inline __virtio16 cpu_to_tun16(struct tun_struct *tun, u16 val)
 {
-	return __cpu_to_virtio16(tun->flags & TUN_VNET_LE, val);
+	return __cpu_to_virtio16(tun_is_little_endian(tun), val);
 }
 
 static inline u32 tun_hashfn(u32 rxhash)
@@ -493,10 +547,7 @@ static void __tun_detach(struct tun_file *tfile, bool clean)
 			    tun->dev->reg_state == NETREG_REGISTERED)
 				unregister_netdevice(tun->dev);
 		}
-
-		BUG_ON(!test_bit(SOCK_EXTERNALLY_ALLOCATED,
-				 &tfile->socket.flags));
-		sk_release_kernel(&tfile->sk);
+		sock_put(&tfile->sk);
 	}
 }
 
@@ -1492,18 +1543,10 @@ out:
 	return ret;
 }
 
-static int tun_release(struct socket *sock)
-{
-	if (sock->sk)
-		sock_put(sock->sk);
-	return 0;
-}
-
 /* Ops structure to mimic raw sockets with tun */
 static const struct proto_ops tun_socket_ops = {
 	.sendmsg = tun_sendmsg,
 	.recvmsg = tun_recvmsg,
-	.release = tun_release,
 };
 
 static struct proto tun_proto = {
@@ -1865,7 +1908,7 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 	if (cmd == TUNSETIFF && !tun) {
 		ifr.ifr_name[IFNAMSIZ-1] = '\0';
 
-		ret = tun_set_iff(tfile->net, file, &ifr);
+		ret = tun_set_iff(sock_net(&tfile->sk), file, &ifr);
 
 		if (ret)
 			goto unlock;
@@ -2056,6 +2099,14 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 			tun->flags &= ~TUN_VNET_LE;
 		break;
 
+	case TUNGETVNETBE:
+		ret = tun_get_vnet_be(tun, argp);
+		break;
+
+	case TUNSETVNETBE:
+		ret = tun_set_vnet_be(tun, argp);
+		break;
+
 	case TUNATTACHFILTER:
 		/* Can be set only for TAPs */
 		ret = -EINVAL;
@@ -2154,16 +2205,16 @@ out:
 
 static int tun_chr_open(struct inode *inode, struct file * file)
 {
+	struct net *net = current->nsproxy->net_ns;
 	struct tun_file *tfile;
 
 	DBG1(KERN_INFO, "tunX: tun_chr_open\n");
 
-	tfile = (struct tun_file *)sk_alloc(&init_net, AF_UNSPEC, GFP_KERNEL,
-					    &tun_proto);
+	tfile = (struct tun_file *)sk_alloc(net, AF_UNSPEC, GFP_KERNEL,
+					    &tun_proto, 0);
 	if (!tfile)
 		return -ENOMEM;
 	RCU_INIT_POINTER(tfile->tun, NULL);
-	tfile->net = get_net(current->nsproxy->net_ns);
 	tfile->flags = 0;
 	tfile->ifindex = 0;
 
@@ -2174,13 +2225,11 @@ static int tun_chr_open(struct inode *inode, struct file * file)
 	tfile->socket.ops = &tun_socket_ops;
 
 	sock_init_data(&tfile->socket, &tfile->sk);
-	sk_change_net(&tfile->sk, tfile->net);
 
 	tfile->sk.sk_write_space = tun_sock_write_space;
 	tfile->sk.sk_sndbuf = INT_MAX;
 
 	file->private_data = tfile;
-	set_bit(SOCK_EXTERNALLY_ALLOCATED, &tfile->socket.flags);
 	INIT_LIST_HEAD(&tfile->next);
 
 	sock_set_flag(&tfile->sk, SOCK_ZEROCOPY);
@@ -2191,10 +2240,8 @@ static int tun_chr_open(struct inode *inode, struct file * file)
 static int tun_chr_close(struct inode *inode, struct file *file)
 {
 	struct tun_file *tfile = file->private_data;
-	struct net *net = tfile->net;
 
 	tun_detach(tfile, true);
-	put_net(net);
 
 	return 0;
 }
diff --git a/drivers/net/vmxnet3/vmxnet3_defs.h b/drivers/net/vmxnet3/vmxnet3_defs.h
index 3718d024f638..221a53025fd0 100644
--- a/drivers/net/vmxnet3/vmxnet3_defs.h
+++ b/drivers/net/vmxnet3/vmxnet3_defs.h
@@ -1,7 +1,7 @@
 /*
  * Linux driver for VMware's vmxnet3 ethernet NIC.
  *
- * Copyright (C) 2008-2009, VMware, Inc. All Rights Reserved.
+ * Copyright (C) 2008-2015, VMware, 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
@@ -277,6 +277,40 @@ struct Vmxnet3_RxCompDesc {
 #endif  /* __BIG_ENDIAN_BITFIELD */
 };
 
+struct Vmxnet3_RxCompDescExt {
+	__le32		dword1;
+	u8		segCnt;       /* Number of aggregated packets */
+	u8		dupAckCnt;    /* Number of duplicate Acks */
+	__le16		tsDelta;      /* TCP timestamp difference */
+	__le32		dword2;
+#ifdef __BIG_ENDIAN_BITFIELD
+	u32		gen:1;        /* generation bit */
+	u32		type:7;       /* completion type */
+	u32		fcs:1;        /* Frame CRC correct */
+	u32		frg:1;        /* IP Fragment */
+	u32		v4:1;         /* IPv4 */
+	u32		v6:1;         /* IPv6 */
+	u32		ipc:1;        /* IP Checksum Correct */
+	u32		tcp:1;        /* TCP packet */
+	u32		udp:1;        /* UDP packet */
+	u32		tuc:1;        /* TCP/UDP Checksum Correct */
+	u32		mss:16;
+#else
+	u32		mss:16;
+	u32		tuc:1;        /* TCP/UDP Checksum Correct */
+	u32		udp:1;        /* UDP packet */
+	u32		tcp:1;        /* TCP packet */
+	u32		ipc:1;        /* IP Checksum Correct */
+	u32		v6:1;         /* IPv6 */
+	u32		v4:1;         /* IPv4 */
+	u32		frg:1;        /* IP Fragment */
+	u32		fcs:1;        /* Frame CRC correct */
+	u32		type:7;       /* completion type */
+	u32		gen:1;        /* generation bit */
+#endif  /* __BIG_ENDIAN_BITFIELD */
+};
+
+
 /* fields in RxCompDesc we access via Vmxnet3_GenericDesc.dword[3] */
 #define VMXNET3_RCD_TUC_SHIFT	16
 #define VMXNET3_RCD_IPC_SHIFT	19
@@ -310,6 +344,7 @@ union Vmxnet3_GenericDesc {
 	struct Vmxnet3_RxDesc		rxd;
 	struct Vmxnet3_TxCompDesc	tcd;
 	struct Vmxnet3_RxCompDesc	rcd;
+	struct Vmxnet3_RxCompDescExt 	rcdExt;
 };
 
 #define VMXNET3_INIT_GEN       1
@@ -361,6 +396,7 @@ enum {
 /* completion descriptor types */
 #define VMXNET3_CDTYPE_TXCOMP      0    /* Tx Completion Descriptor */
 #define VMXNET3_CDTYPE_RXCOMP      3    /* Rx Completion Descriptor */
+#define VMXNET3_CDTYPE_RXCOMP_LRO  4    /* Rx Completion Descriptor for LRO */
 
 enum {
 	VMXNET3_GOS_BITS_UNK    = 0,   /* unknown */
diff --git a/drivers/net/vmxnet3/vmxnet3_drv.c b/drivers/net/vmxnet3/vmxnet3_drv.c
index 61c0840c448c..da11bb5e9c7f 100644
--- a/drivers/net/vmxnet3/vmxnet3_drv.c
+++ b/drivers/net/vmxnet3/vmxnet3_drv.c
@@ -861,6 +861,9 @@ vmxnet3_parse_and_copy_hdr(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
 					     , skb_headlen(skb));
 		}
 
+		if (skb->len <= VMXNET3_HDR_COPY_SIZE)
+			ctx->copy_size = skb->len;
+
 		/* make sure headers are accessible directly */
 		if (unlikely(!pskb_may_pull(skb, ctx->copy_size)))
 			goto err;
@@ -1160,6 +1163,52 @@ vmxnet3_rx_error(struct vmxnet3_rx_queue *rq, struct Vmxnet3_RxCompDesc *rcd,
 }
 
 
+static u32
+vmxnet3_get_hdr_len(struct vmxnet3_adapter *adapter, struct sk_buff *skb,
+		    union Vmxnet3_GenericDesc *gdesc)
+{
+	u32 hlen, maplen;
+	union {
+		void *ptr;
+		struct ethhdr *eth;
+		struct iphdr *ipv4;
+		struct ipv6hdr *ipv6;
+		struct tcphdr *tcp;
+	} hdr;
+	BUG_ON(gdesc->rcd.tcp == 0);
+
+	maplen = skb_headlen(skb);
+	if (unlikely(sizeof(struct iphdr) + sizeof(struct tcphdr) > maplen))
+		return 0;
+
+	hdr.eth = eth_hdr(skb);
+	if (gdesc->rcd.v4) {
+		BUG_ON(hdr.eth->h_proto != htons(ETH_P_IP));
+		hdr.ptr += sizeof(struct ethhdr);
+		BUG_ON(hdr.ipv4->protocol != IPPROTO_TCP);
+		hlen = hdr.ipv4->ihl << 2;
+		hdr.ptr += hdr.ipv4->ihl << 2;
+	} else if (gdesc->rcd.v6) {
+		BUG_ON(hdr.eth->h_proto != htons(ETH_P_IPV6));
+		hdr.ptr += sizeof(struct ethhdr);
+		/* Use an estimated value, since we also need to handle
+		 * TSO case.
+		 */
+		if (hdr.ipv6->nexthdr != IPPROTO_TCP)
+			return sizeof(struct ipv6hdr) + sizeof(struct tcphdr);
+		hlen = sizeof(struct ipv6hdr);
+		hdr.ptr += sizeof(struct ipv6hdr);
+	} else {
+		/* Non-IP pkt, dont estimate header length */
+		return 0;
+	}
+
+	if (hlen + sizeof(struct tcphdr) > maplen)
+		return 0;
+
+	return (hlen + (hdr.tcp->doff << 2));
+}
+
 static int
 vmxnet3_rq_rx_complete(struct vmxnet3_rx_queue *rq,
 		       struct vmxnet3_adapter *adapter, int quota)
@@ -1171,6 +1220,7 @@ vmxnet3_rq_rx_complete(struct vmxnet3_rx_queue *rq,
 	bool skip_page_frags = false;
 	struct Vmxnet3_RxCompDesc *rcd;
 	struct vmxnet3_rx_ctx *ctx = &rq->rx_ctx;
+	u16 segCnt = 0, mss = 0;
 #ifdef __BIG_ENDIAN_BITFIELD
 	struct Vmxnet3_RxDesc rxCmdDesc;
 	struct Vmxnet3_RxCompDesc rxComp;
@@ -1259,7 +1309,19 @@ vmxnet3_rq_rx_complete(struct vmxnet3_rx_queue *rq,
 						       PCI_DMA_FROMDEVICE);
 			rxd->addr = cpu_to_le64(rbi->dma_addr);
 			rxd->len = rbi->len;
-
+			if (adapter->version == 2 &&
+			    rcd->type == VMXNET3_CDTYPE_RXCOMP_LRO) {
+				struct Vmxnet3_RxCompDescExt *rcdlro;
+				rcdlro = (struct Vmxnet3_RxCompDescExt *)rcd;
+
+				segCnt = rcdlro->segCnt;
+				BUG_ON(segCnt <= 1);
+				mss = rcdlro->mss;
+				if (unlikely(segCnt <= 1))
+					segCnt = 0;
+			} else {
+				segCnt = 0;
+			}
 		} else {
 			BUG_ON(ctx->skb == NULL && !skip_page_frags);
 
@@ -1273,47 +1335,75 @@ vmxnet3_rq_rx_complete(struct vmxnet3_rx_queue *rq,
 			if (skip_page_frags)
 				goto rcd_done;
 
-			new_page = alloc_page(GFP_ATOMIC);
-			if (unlikely(new_page == NULL)) {
+			if (rcd->len) {
+				new_page = alloc_page(GFP_ATOMIC);
 				/* Replacement page frag could not be allocated.
 				 * Reuse this page. Drop the pkt and free the
 				 * skb which contained this page as a frag. Skip
 				 * processing all the following non-sop frags.
 				 */
-				rq->stats.rx_buf_alloc_failure++;
-				dev_kfree_skb(ctx->skb);
-				ctx->skb = NULL;
-				skip_page_frags = true;
-				goto rcd_done;
-			}
+				if (unlikely(!new_page)) {
+					rq->stats.rx_buf_alloc_failure++;
+					dev_kfree_skb(ctx->skb);
+					ctx->skb = NULL;
+					skip_page_frags = true;
+					goto rcd_done;
+				}
 
-			if (rcd->len) {
 				dma_unmap_page(&adapter->pdev->dev,
 					       rbi->dma_addr, rbi->len,
 					       PCI_DMA_FROMDEVICE);
 
 				vmxnet3_append_frag(ctx->skb, rcd, rbi);
-			}
 
-			/* Immediate refill */
-			rbi->page = new_page;
-			rbi->dma_addr = dma_map_page(&adapter->pdev->dev,
-						     rbi->page,
-						     0, PAGE_SIZE,
-						     PCI_DMA_FROMDEVICE);
-			rxd->addr = cpu_to_le64(rbi->dma_addr);
-			rxd->len = rbi->len;
+				/* Immediate refill */
+				rbi->page = new_page;
+				rbi->dma_addr = dma_map_page(&adapter->pdev->dev
+							, rbi->page,
+							0, PAGE_SIZE,
+							PCI_DMA_FROMDEVICE);
+				rxd->addr = cpu_to_le64(rbi->dma_addr);
+				rxd->len = rbi->len;
+			}
 		}
 
 
 		skb = ctx->skb;
 		if (rcd->eop) {
+			u32 mtu = adapter->netdev->mtu;
 			skb->len += skb->data_len;
 
 			vmxnet3_rx_csum(adapter, skb,
 					(union Vmxnet3_GenericDesc *)rcd);
 			skb->protocol = eth_type_trans(skb, adapter->netdev);
-
+			if (!rcd->tcp || !adapter->lro)
+				goto not_lro;
+
+			if (segCnt != 0 && mss != 0) {
+				skb_shinfo(skb)->gso_type = rcd->v4 ?
+					SKB_GSO_TCPV4 : SKB_GSO_TCPV6;
+				skb_shinfo(skb)->gso_size = mss;
+				skb_shinfo(skb)->gso_segs = segCnt;
+			} else if (segCnt != 0 || skb->len > mtu) {
+				u32 hlen;
+
+				hlen = vmxnet3_get_hdr_len(adapter, skb,
+					(union Vmxnet3_GenericDesc *)rcd);
+				if (hlen == 0)
+					goto not_lro;
+
+				skb_shinfo(skb)->gso_type =
+					rcd->v4 ? SKB_GSO_TCPV4 : SKB_GSO_TCPV6;
+				if (segCnt != 0) {
+					skb_shinfo(skb)->gso_segs = segCnt;
+					skb_shinfo(skb)->gso_size =
+						DIV_ROUND_UP(skb->len -
+							hlen, segCnt);
+				} else {
+					skb_shinfo(skb)->gso_size = mtu - hlen;
+				}
+			}
+not_lro:
 			if (unlikely(rcd->ts))
 				__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), rcd->tci);
 
@@ -3038,14 +3128,19 @@ vmxnet3_probe_device(struct pci_dev *pdev,
 		goto err_alloc_pci;
 
 	ver = VMXNET3_READ_BAR1_REG(adapter, VMXNET3_REG_VRRS);
-	if (ver & 1) {
+	if (ver & 2) {
+		VMXNET3_WRITE_BAR1_REG(adapter, VMXNET3_REG_VRRS, 2);
+		adapter->version = 2;
+	} else if (ver & 1) {
 		VMXNET3_WRITE_BAR1_REG(adapter, VMXNET3_REG_VRRS, 1);
+		adapter->version = 1;
 	} else {
 		dev_err(&pdev->dev,
 			"Incompatible h/w version (0x%x) for adapter\n", ver);
 		err = -EBUSY;
 		goto err_ver;
 	}
+	dev_dbg(&pdev->dev, "Using device version %d\n", adapter->version);
 
 	ver = VMXNET3_READ_BAR1_REG(adapter, VMXNET3_REG_UVRS);
 	if (ver & 1) {
@@ -3184,6 +3279,32 @@ vmxnet3_remove_device(struct pci_dev *pdev)
 	free_netdev(netdev);
 }
 
+static void vmxnet3_shutdown_device(struct pci_dev *pdev)
+{
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct vmxnet3_adapter *adapter = netdev_priv(netdev);
+	unsigned long flags;
+
+	/* Reset_work may be in the middle of resetting the device, wait for its
+	 * completion.
+	 */
+	while (test_and_set_bit(VMXNET3_STATE_BIT_RESETTING, &adapter->state))
+		msleep(1);
+
+	if (test_and_set_bit(VMXNET3_STATE_BIT_QUIESCED,
+			     &adapter->state)) {
+		clear_bit(VMXNET3_STATE_BIT_RESETTING, &adapter->state);
+		return;
+	}
+	spin_lock_irqsave(&adapter->cmd_lock, flags);
+	VMXNET3_WRITE_BAR1_REG(adapter, VMXNET3_REG_CMD,
+			       VMXNET3_CMD_QUIESCE_DEV);
+	spin_unlock_irqrestore(&adapter->cmd_lock, flags);
+	vmxnet3_disable_all_intrs(adapter);
+
+	clear_bit(VMXNET3_STATE_BIT_RESETTING, &adapter->state);
+}
+
 
 #ifdef CONFIG_PM
 
@@ -3360,6 +3481,7 @@ static struct pci_driver vmxnet3_driver = {
 	.id_table	= vmxnet3_pciid_table,
 	.probe		= vmxnet3_probe_device,
 	.remove		= vmxnet3_remove_device,
+	.shutdown	= vmxnet3_shutdown_device,
 #ifdef CONFIG_PM
 	.driver.pm	= &vmxnet3_pm_ops,
 #endif
diff --git a/drivers/net/vmxnet3/vmxnet3_int.h b/drivers/net/vmxnet3/vmxnet3_int.h
index 6bb769ae7de9..2652245631d1 100644
--- a/drivers/net/vmxnet3/vmxnet3_int.h
+++ b/drivers/net/vmxnet3/vmxnet3_int.h
@@ -69,10 +69,10 @@
 /*
  * Version numbers
  */
-#define VMXNET3_DRIVER_VERSION_STRING   "1.3.5.0-k"
+#define VMXNET3_DRIVER_VERSION_STRING   "1.4.2.0-k"
 
 /* a 32-bit int, each byte encode a verion number in VMXNET3_DRIVER_VERSION */
-#define VMXNET3_DRIVER_VERSION_NUM      0x01030500
+#define VMXNET3_DRIVER_VERSION_NUM      0x01040200
 
 #if defined(CONFIG_PCI_MSI)
 	/* RSS only makes sense if MSI-X is supported. */
@@ -328,6 +328,10 @@ struct vmxnet3_adapter {
 
 	u8			__iomem *hw_addr0; /* for BAR 0 */
 	u8			__iomem *hw_addr1; /* for BAR 1 */
+	u8                              version;
+
+	bool				rxcsum;
+	bool				lro;
 
 #ifdef VMXNET3_RSS
 	struct UPT1_RSSConf		*rss_conf;
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 21a0fbf1ed94..34c519eb1db5 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -336,7 +336,7 @@ static int vxlan_fdb_info(struct sk_buff *skb, struct vxlan_dev *vxlan,
 
 	if (!net_eq(dev_net(vxlan->dev), vxlan->net) &&
 	    nla_put_s32(skb, NDA_LINK_NETNSID,
-			peernet2id(dev_net(vxlan->dev), vxlan->net)))
+			peernet2id_alloc(dev_net(vxlan->dev), vxlan->net)))
 		goto nla_put_failure;
 
 	if (send_eth && nla_put(skb, NDA_LLADDR, ETH_ALEN, &fdb->eth_addr))
@@ -1921,6 +1921,8 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 		memset(&fl4, 0, sizeof(fl4));
 		fl4.flowi4_oif = rdst->remote_ifindex;
 		fl4.flowi4_tos = RT_TOS(tos);
+		fl4.flowi4_mark = skb->mark;
+		fl4.flowi4_proto = IPPROTO_UDP;
 		fl4.daddr = dst->sin.sin_addr.s_addr;
 		fl4.saddr = vxlan->saddr.sin.sin_addr.s_addr;
 
@@ -1981,6 +1983,7 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 		fl6.flowi6_oif = rdst->remote_ifindex;
 		fl6.daddr = dst->sin6.sin6_addr;
 		fl6.saddr = vxlan->saddr.sin6.sin6_addr;
+		fl6.flowi6_mark = skb->mark;
 		fl6.flowi6_proto = IPPROTO_UDP;
 
 		if (ipv6_stub->ipv6_dst_lookup(sk, &ndst, &fl6)) {
@@ -2128,9 +2131,10 @@ static void vxlan_cleanup(unsigned long arg)
 	if (!netif_running(vxlan->dev))
 		return;
 
-	spin_lock_bh(&vxlan->hash_lock);
 	for (h = 0; h < FDB_HASH_SIZE; ++h) {
 		struct hlist_node *p, *n;
+
+		spin_lock_bh(&vxlan->hash_lock);
 		hlist_for_each_safe(p, n, &vxlan->fdb_head[h]) {
 			struct vxlan_fdb *f
 				= container_of(p, struct vxlan_fdb, hlist);
@@ -2149,8 +2153,8 @@ static void vxlan_cleanup(unsigned long arg)
 			} else if (time_before(timeout, next_timer))
 				next_timer = timeout;
 		}
+		spin_unlock_bh(&vxlan->hash_lock);
 	}
-	spin_unlock_bh(&vxlan->hash_lock);
 
 	mod_timer(&vxlan->age_timer, next_timer);
 }
diff --git a/drivers/net/wan/cosa.c b/drivers/net/wan/cosa.c
index bcfa01add7cc..7193b7304fdd 100644
--- a/drivers/net/wan/cosa.c
+++ b/drivers/net/wan/cosa.c
@@ -517,7 +517,7 @@ static int cosa_probe(int base, int irq, int dma)
 		 */
 		set_current_state(TASK_INTERRUPTIBLE);
 		cosa_putstatus(cosa, SR_TX_INT_ENA);
-		schedule_timeout(30);
+		schedule_timeout(msecs_to_jiffies(300));
 		irq = probe_irq_off(irqs);
 		/* Disable all IRQs from the card */
 		cosa_putstatus(cosa, 0);
diff --git a/drivers/net/wan/dscc4.c b/drivers/net/wan/dscc4.c
index 08223569cebd..7a72407208b1 100644
--- a/drivers/net/wan/dscc4.c
+++ b/drivers/net/wan/dscc4.c
@@ -551,7 +551,7 @@ static int dscc4_wait_ack_cec(struct dscc4_dev_priv *dpriv,
 			       msg, i);
 			goto done;
 		}
-		schedule_timeout_uninterruptible(10);
+		schedule_timeout_uninterruptible(msecs_to_jiffies(100));
 		rmb();
 	} while (++i > 0);
 	netdev_err(dev, "%s timeout\n", msg);
@@ -596,7 +596,7 @@ static inline int dscc4_xpr_ack(struct dscc4_dev_priv *dpriv)
 		    (dpriv->iqtx[cur] & cpu_to_le32(Xpr)))
 			break;
 		smp_rmb();
-		schedule_timeout_uninterruptible(10);
+		schedule_timeout_uninterruptible(msecs_to_jiffies(100));
 	} while (++i > 0);
 
 	return (i >= 0 ) ? i : -EAGAIN;
@@ -1033,7 +1033,7 @@ static void dscc4_pci_reset(struct pci_dev *pdev, void __iomem *ioaddr)
 	/* Flush posted writes */
 	readl(ioaddr + GSTAR);
 
-	schedule_timeout_uninterruptible(10);
+	schedule_timeout_uninterruptible(msecs_to_jiffies(100));
 
 	for (i = 0; i < 16; i++)
 		pci_write_config_dword(pdev, i << 2, dscc4_pci_config_store[i]);
@@ -1046,7 +1046,6 @@ static void dscc4_pci_reset(struct pci_dev *pdev, void __iomem *ioaddr)
 static int dscc4_open(struct net_device *dev)
 {
 	struct dscc4_dev_priv *dpriv = dscc4_priv(dev);
-	struct dscc4_pci_priv *ppriv;
 	int ret = -EAGAIN;
 
 	if ((dscc4_loopback_check(dpriv) < 0))
@@ -1055,8 +1054,6 @@ static int dscc4_open(struct net_device *dev)
 	if ((ret = hdlc_open(dev)))
 		goto err;
 
-	ppriv = dpriv->pci_priv;
-
 	/*
 	 * Due to various bugs, there is no way to reliably reset a
 	 * specific port (manufacturer's dependent special PCI #RST wiring
diff --git a/drivers/net/wan/lapbether.c b/drivers/net/wan/lapbether.c
index 2f5eda8a7227..6676607164d6 100644
--- a/drivers/net/wan/lapbether.c
+++ b/drivers/net/wan/lapbether.c
@@ -40,7 +40,6 @@
 #include <linux/interrupt.h>
 #include <linux/notifier.h>
 #include <linux/stat.h>
-#include <linux/netfilter.h>
 #include <linux/module.h>
 #include <linux/lapb.h>
 #include <linux/init.h>
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index 16604bdf5197..a63ab2e83105 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -277,6 +277,7 @@ source "drivers/net/wireless/libertas/Kconfig"
 source "drivers/net/wireless/orinoco/Kconfig"
 source "drivers/net/wireless/p54/Kconfig"
 source "drivers/net/wireless/rt2x00/Kconfig"
+source "drivers/net/wireless/mediatek/Kconfig"
 source "drivers/net/wireless/rtlwifi/Kconfig"
 source "drivers/net/wireless/ti/Kconfig"
 source "drivers/net/wireless/zd1211rw/Kconfig"
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 0c8891686718..6b9e729dd8ac 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -45,6 +45,8 @@ obj-$(CONFIG_IWLWIFI)	+= iwlwifi/
 obj-$(CONFIG_IWLEGACY)	+= iwlegacy/
 obj-$(CONFIG_RT2X00)	+= rt2x00/
 
+obj-$(CONFIG_WL_MEDIATEK)	+= mediatek/
+
 obj-$(CONFIG_P54_COMMON)	+= p54/
 
 obj-$(CONFIG_ATH_CARDS)		+= ath/
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
index f07a61899545..15f057ed41ad 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/adm8211.c
@@ -1098,14 +1098,18 @@ static void adm8211_hw_init(struct ieee80211_hw *dev)
 		pci_read_config_byte(priv->pdev, PCI_CACHE_LINE_SIZE, &cline);
 
 		switch (cline) {
-		case  0x8: reg |= (0x1 << 14);
-			   break;
-		case 0x16: reg |= (0x2 << 14);
-			   break;
-		case 0x32: reg |= (0x3 << 14);
-			   break;
-		  default: reg |= (0x0 << 14);
-			   break;
+		case  0x8:
+			reg |= (0x1 << 14);
+			break;
+		case 0x10:
+			reg |= (0x2 << 14);
+			break;
+		case 0x20:
+			reg |= (0x3 << 14);
+			break;
+		default:
+			reg |= (0x0 << 14);
+			break;
 		}
 	}
 
@@ -1353,12 +1357,7 @@ static void adm8211_configure_filter(struct ieee80211_hw *dev,
 
 	new_flags = 0;
 
-	if (*total_flags & FIF_PROMISC_IN_BSS) {
-		new_flags |= FIF_PROMISC_IN_BSS;
-		priv->nar |= ADM8211_NAR_PR;
-		priv->nar &= ~ADM8211_NAR_MM;
-		mc_filter[1] = mc_filter[0] = ~0;
-	} else if (*total_flags & FIF_ALLMULTI || multicast == ~(0ULL)) {
+	if (*total_flags & FIF_ALLMULTI || multicast == ~(0ULL)) {
 		new_flags |= FIF_ALLMULTI;
 		priv->nar &= ~ADM8211_NAR_PR;
 		priv->nar |= ADM8211_NAR_MM;
@@ -1374,9 +1373,9 @@ static void adm8211_configure_filter(struct ieee80211_hw *dev,
 	ADM8211_CSR_READ(NAR);
 
 	if (priv->nar & ADM8211_NAR_PR)
-		dev->flags |= IEEE80211_HW_RX_INCLUDES_FCS;
+		ieee80211_hw_set(dev, RX_INCLUDES_FCS);
 	else
-		dev->flags &= ~IEEE80211_HW_RX_INCLUDES_FCS;
+		__clear_bit(IEEE80211_HW_RX_INCLUDES_FCS, dev->flags);
 
 	if (*total_flags & FIF_BCN_PRBRESP_PROMISC)
 		adm8211_set_bssid(dev, bcast);
@@ -1862,8 +1861,8 @@ static int adm8211_probe(struct pci_dev *pdev,
 	SET_IEEE80211_PERM_ADDR(dev, perm_addr);
 
 	dev->extra_tx_headroom = sizeof(struct adm8211_tx_hdr);
-	/* dev->flags = IEEE80211_HW_RX_INCLUDES_FCS in promisc mode */
-	dev->flags = IEEE80211_HW_SIGNAL_UNSPEC;
+	/* dev->flags = RX_INCLUDES_FCS in promisc mode */
+	ieee80211_hw_set(dev, SIGNAL_UNSPEC);
 	dev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
 
 	dev->max_signal = 100;    /* FIXME: find better value */
diff --git a/drivers/net/wireless/at76c50x-usb.c b/drivers/net/wireless/at76c50x-usb.c
index 49219c508963..dab25136214a 100644
--- a/drivers/net/wireless/at76c50x-usb.c
+++ b/drivers/net/wireless/at76c50x-usb.c
@@ -2360,8 +2360,8 @@ static int at76_init_new_device(struct at76_priv *priv,
 	priv->hw->wiphy->max_scan_ie_len = 0;
 	priv->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
 	priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &at76_supported_band;
-	priv->hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-			  IEEE80211_HW_SIGNAL_UNSPEC;
+	ieee80211_hw_set(priv->hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(priv->hw, SIGNAL_UNSPEC);
 	priv->hw->max_signal = 100;
 
 	SET_IEEE80211_DEV(priv->hw, &interface->dev);
diff --git a/drivers/net/wireless/at76c50x-usb.h b/drivers/net/wireless/at76c50x-usb.h
index 55090a38ac95..ae03271f878e 100644
--- a/drivers/net/wireless/at76c50x-usb.h
+++ b/drivers/net/wireless/at76c50x-usb.h
@@ -447,7 +447,7 @@ struct at76_priv {
 	int mac80211_registered;
 };
 
-#define AT76_SUPPORTED_FILTERS FIF_PROMISC_IN_BSS
+#define AT76_SUPPORTED_FILTERS 0
 
 #define SCAN_POLL_INTERVAL	(HZ / 4)
 
diff --git a/drivers/net/wireless/ath/ar5523/ar5523.c b/drivers/net/wireless/ath/ar5523/ar5523.c
index 5147ebe4cd05..3b343c63aa52 100644
--- a/drivers/net/wireless/ath/ar5523/ar5523.c
+++ b/drivers/net/wireless/ath/ar5523/ar5523.c
@@ -1319,8 +1319,7 @@ out_unlock:
 
 }
 
-#define AR5523_SUPPORTED_FILTERS (FIF_PROMISC_IN_BSS | \
-				  FIF_ALLMULTI | \
+#define AR5523_SUPPORTED_FILTERS (FIF_ALLMULTI | \
 				  FIF_FCSFAIL | \
 				  FIF_OTHER_BSS)
 
@@ -1683,9 +1682,9 @@ static int ar5523_probe(struct usb_interface *intf,
 			(id->driver_info & AR5523_FLAG_ABG) ? '5' : '2');
 
 	ar->vif = NULL;
-	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		    IEEE80211_HW_SIGNAL_DBM |
-		    IEEE80211_HW_HAS_RATE_CONTROL;
+	ieee80211_hw_set(hw, HAS_RATE_CONTROL);
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
 	hw->extra_tx_headroom = sizeof(struct ar5523_tx_desc) +
 				sizeof(struct ar5523_chunk);
 	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
diff --git a/drivers/net/wireless/ath/ath.h b/drivers/net/wireless/ath/ath.h
index 7e9481099a8e..65ef483ebf50 100644
--- a/drivers/net/wireless/ath/ath.h
+++ b/drivers/net/wireless/ath/ath.h
@@ -251,6 +251,7 @@ void ath_printk(const char *level, const struct ath_common *common,
  * @ATH_DBG_DFS: radar datection
  * @ATH_DBG_WOW: Wake on Wireless
  * @ATH_DBG_DYNACK: dynack handling
+ * @ATH_DBG_SPECTRAL_SCAN: FFT spectral scan
  * @ATH_DBG_ANY: enable all debugging
  *
  * The debug level is used to control the amount and type of debugging output
@@ -280,6 +281,7 @@ enum ATH_DEBUG {
 	ATH_DBG_WOW		= 0x00020000,
 	ATH_DBG_CHAN_CTX	= 0x00040000,
 	ATH_DBG_DYNACK		= 0x00080000,
+	ATH_DBG_SPECTRAL_SCAN	= 0x00100000,
 	ATH_DBG_ANY		= 0xffffffff
 };
 
diff --git a/drivers/net/wireless/ath/ath10k/Makefile b/drivers/net/wireless/ath/ath10k/Makefile
index f4dbb3e93bf8..9729e6941635 100644
--- a/drivers/net/wireless/ath/ath10k/Makefile
+++ b/drivers/net/wireless/ath/ath10k/Makefile
@@ -10,13 +10,15 @@ ath10k_core-y += mac.o \
 		 wmi.o \
 		 wmi-tlv.o \
 		 bmi.o \
-		 hw.o
+		 hw.o \
+		 p2p.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
 ath10k_core-$(CONFIG_MAC80211_DEBUGFS) += debugfs_sta.o
+ath10k_core-$(CONFIG_PM) += wow.o
 
 obj-$(CONFIG_ATH10K_PCI) += ath10k_pci.o
 ath10k_pci-y += pci.o \
diff --git a/drivers/net/wireless/ath/ath10k/core.c b/drivers/net/wireless/ath/ath10k/core.c
index c0e454bb6a8d..59496a90ad5e 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -48,6 +48,7 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.name = "qca988x hw2.0",
 		.patch_load_addr = QCA988X_HW_2_0_PATCH_LOAD_ADDR,
 		.uart_pin = 7,
+		.has_shifted_cc_wraparound = true,
 		.fw = {
 			.dir = QCA988X_HW_2_0_FW_DIR,
 			.fw = QCA988X_HW_2_0_FW_FILE,
@@ -387,7 +388,9 @@ static int ath10k_download_and_run_otp(struct ath10k *ar)
 
 	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot otp execute result %d\n", result);
 
-	if (!skip_otp && result != 0) {
+	if (!(skip_otp || test_bit(ATH10K_FW_FEATURE_IGNORE_OTP_RESULT,
+				   ar->fw_features))
+	    && result != 0) {
 		ath10k_err(ar, "otp calibration failed: %d", result);
 		return -EINVAL;
 	}
@@ -482,31 +485,79 @@ static int ath10k_fetch_cal_file(struct ath10k *ar)
 	return 0;
 }
 
-static int ath10k_core_fetch_firmware_api_1(struct ath10k *ar)
+static int ath10k_core_fetch_spec_board_file(struct ath10k *ar)
 {
-	int ret = 0;
+	char filename[100];
 
-	if (ar->hw_params.fw.fw == NULL) {
-		ath10k_err(ar, "firmware file not defined\n");
-		return -EINVAL;
-	}
+	scnprintf(filename, sizeof(filename), "board-%s-%s.bin",
+		  ath10k_bus_str(ar->hif.bus), ar->spec_board_id);
+
+	ar->board = ath10k_fetch_fw_file(ar, ar->hw_params.fw.dir, filename);
+	if (IS_ERR(ar->board))
+		return PTR_ERR(ar->board);
+
+	ar->board_data = ar->board->data;
+	ar->board_len = ar->board->size;
+	ar->spec_board_loaded = true;
 
-	if (ar->hw_params.fw.board == NULL) {
-		ath10k_err(ar, "board data file not defined");
+	return 0;
+}
+
+static int ath10k_core_fetch_generic_board_file(struct ath10k *ar)
+{
+	if (!ar->hw_params.fw.board) {
+		ath10k_err(ar, "failed to find board file fw entry\n");
 		return -EINVAL;
 	}
 
 	ar->board = ath10k_fetch_fw_file(ar,
 					 ar->hw_params.fw.dir,
 					 ar->hw_params.fw.board);
-	if (IS_ERR(ar->board)) {
-		ret = PTR_ERR(ar->board);
-		ath10k_err(ar, "could not fetch board data (%d)\n", ret);
-		goto err;
-	}
+	if (IS_ERR(ar->board))
+		return PTR_ERR(ar->board);
 
 	ar->board_data = ar->board->data;
 	ar->board_len = ar->board->size;
+	ar->spec_board_loaded = false;
+
+	return 0;
+}
+
+static int ath10k_core_fetch_board_file(struct ath10k *ar)
+{
+	int ret;
+
+	if (strlen(ar->spec_board_id) > 0) {
+		ret = ath10k_core_fetch_spec_board_file(ar);
+		if (ret) {
+			ath10k_info(ar, "failed to load spec board file, falling back to generic: %d\n",
+				    ret);
+			goto generic;
+		}
+
+		ath10k_dbg(ar, ATH10K_DBG_BOOT, "found specific board file for %s\n",
+			   ar->spec_board_id);
+		return 0;
+	}
+
+generic:
+	ret = ath10k_core_fetch_generic_board_file(ar);
+	if (ret) {
+		ath10k_err(ar, "failed to fetch generic board data: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ath10k_core_fetch_firmware_api_1(struct ath10k *ar)
+{
+	int ret = 0;
+
+	if (ar->hw_params.fw.fw == NULL) {
+		ath10k_err(ar, "firmware file not defined\n");
+		return -EINVAL;
+	}
 
 	ar->firmware = ath10k_fetch_fw_file(ar,
 					    ar->hw_params.fw.dir,
@@ -675,6 +726,17 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 			ath10k_dbg(ar, ATH10K_DBG_BOOT, "found fw ie wmi op version %d\n",
 				   ar->wmi.op_version);
 			break;
+		case ATH10K_FW_IE_HTT_OP_VERSION:
+			if (ie_len != sizeof(u32))
+				break;
+
+			version = (__le32 *)data;
+
+			ar->htt.op_version = le32_to_cpup(version);
+
+			ath10k_dbg(ar, ATH10K_DBG_BOOT, "found fw ie htt op version %d\n",
+				   ar->htt.op_version);
+			break;
 		default:
 			ath10k_warn(ar, "Unknown FW IE: %u\n",
 				    le32_to_cpu(hdr->id));
@@ -695,27 +757,6 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 		goto err;
 	}
 
-	/* now fetch the board file */
-	if (ar->hw_params.fw.board == NULL) {
-		ath10k_err(ar, "board data file not defined");
-		ret = -EINVAL;
-		goto err;
-	}
-
-	ar->board = ath10k_fetch_fw_file(ar,
-					 ar->hw_params.fw.dir,
-					 ar->hw_params.fw.board);
-	if (IS_ERR(ar->board)) {
-		ret = PTR_ERR(ar->board);
-		ath10k_err(ar, "could not fetch board data '%s/%s' (%d)\n",
-			   ar->hw_params.fw.dir, ar->hw_params.fw.board,
-			   ret);
-		goto err;
-	}
-
-	ar->board_data = ar->board->data;
-	ar->board_len = ar->board->size;
-
 	return 0;
 
 err:
@@ -730,6 +771,19 @@ 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);
 
+	ret = ath10k_core_fetch_board_file(ar);
+	if (ret) {
+		ath10k_err(ar, "failed to fetch board file: %d\n", ret);
+		return ret;
+	}
+
+	ar->fw_api = 5;
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
+
+	ret = ath10k_core_fetch_firmware_api_n(ar, ATH10K_FW_API5_FILE);
+	if (ret == 0)
+		goto success;
+
 	ar->fw_api = 4;
 	ath10k_dbg(ar, ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
 
@@ -958,6 +1012,8 @@ static int ath10k_core_init_firmware_features(struct ath10k *ar)
 		ar->max_num_stations = TARGET_NUM_STATIONS;
 		ar->max_num_vdevs = TARGET_NUM_VDEVS;
 		ar->htt.max_num_pending_tx = TARGET_NUM_MSDU_DESC;
+		ar->fw_stats_req_mask = WMI_STAT_PDEV | WMI_STAT_VDEV |
+			WMI_STAT_PEER;
 		break;
 	case ATH10K_FW_WMI_OP_VERSION_10_1:
 	case ATH10K_FW_WMI_OP_VERSION_10_2:
@@ -966,12 +1022,17 @@ static int ath10k_core_init_firmware_features(struct ath10k *ar)
 		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;
+		ar->fw_stats_req_mask = WMI_STAT_PEER;
 		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->max_num_vdevs = TARGET_TLV_NUM_VDEVS;
+		ar->max_num_tdls_vdevs = TARGET_TLV_NUM_TDLS_VDEVS;
 		ar->htt.max_num_pending_tx = TARGET_TLV_NUM_MSDU_DESC;
+		ar->wow.max_num_patterns = TARGET_TLV_NUM_WOW_PATTERNS;
+		ar->fw_stats_req_mask = WMI_STAT_PDEV | WMI_STAT_VDEV |
+			WMI_STAT_PEER;
 		break;
 	case ATH10K_FW_WMI_OP_VERSION_UNSET:
 	case ATH10K_FW_WMI_OP_VERSION_MAX:
@@ -979,6 +1040,29 @@ static int ath10k_core_init_firmware_features(struct ath10k *ar)
 		return -EINVAL;
 	}
 
+	/* Backwards compatibility for firmwares without
+	 * ATH10K_FW_IE_HTT_OP_VERSION.
+	 */
+	if (ar->htt.op_version == ATH10K_FW_HTT_OP_VERSION_UNSET) {
+		switch (ar->wmi.op_version) {
+		case ATH10K_FW_WMI_OP_VERSION_MAIN:
+			ar->htt.op_version = ATH10K_FW_HTT_OP_VERSION_MAIN;
+			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->htt.op_version = ATH10K_FW_HTT_OP_VERSION_10_1;
+			break;
+		case ATH10K_FW_WMI_OP_VERSION_TLV:
+			ar->htt.op_version = ATH10K_FW_HTT_OP_VERSION_TLV;
+			break;
+		case ATH10K_FW_WMI_OP_VERSION_UNSET:
+		case ATH10K_FW_WMI_OP_VERSION_MAX:
+			WARN_ON(1);
+			return -EINVAL;
+		}
+	}
+
 	return 0;
 }
 
@@ -1001,6 +1085,22 @@ int ath10k_core_start(struct ath10k *ar, enum ath10k_firmware_mode mode)
 	if (status)
 		goto err;
 
+	/* Some of of qca988x solutions are having global reset issue
+         * during target initialization. Bypassing PLL setting before
+         * downloading firmware and letting the SoC run on REF_CLK is
+         * fixing the problem. Corresponding firmware change is also needed
+         * to set the clock source once the target is initialized.
+	 */
+	if (test_bit(ATH10K_FW_FEATURE_SUPPORTS_SKIP_CLOCK_INIT,
+		     ar->fw_features)) {
+		status = ath10k_bmi_write32(ar, hi_skip_clock_init, 1);
+		if (status) {
+			ath10k_err(ar, "could not write to skip_clock_init: %d\n",
+				   status);
+			goto err;
+		}
+	}
+
 	status = ath10k_download_fw(ar, mode);
 	if (status)
 		goto err;
@@ -1080,9 +1180,8 @@ int ath10k_core_start(struct ath10k *ar, enum ath10k_firmware_mode mode)
 
 	if (mode == ATH10K_FIRMWARE_MODE_NORMAL) {
 		status = ath10k_wmi_wait_for_service_ready(ar);
-		if (status <= 0) {
+		if (status) {
 			ath10k_warn(ar, "wmi service ready event not received");
-			status = -ETIMEDOUT;
 			goto err_hif_stop;
 		}
 	}
@@ -1098,9 +1197,8 @@ int ath10k_core_start(struct ath10k *ar, enum ath10k_firmware_mode mode)
 	}
 
 	status = ath10k_wmi_wait_for_unified_ready(ar);
-	if (status <= 0) {
+	if (status) {
 		ath10k_err(ar, "wmi unified ready event not received\n");
-		status = -ETIMEDOUT;
 		goto err_hif_stop;
 	}
 
@@ -1151,6 +1249,7 @@ EXPORT_SYMBOL(ath10k_core_start);
 int ath10k_wait_for_suspend(struct ath10k *ar, u32 suspend_opt)
 {
 	int ret;
+	unsigned long time_left;
 
 	reinit_completion(&ar->target_suspend);
 
@@ -1160,9 +1259,9 @@ int ath10k_wait_for_suspend(struct ath10k *ar, u32 suspend_opt)
 		return ret;
 	}
 
-	ret = wait_for_completion_timeout(&ar->target_suspend, 1 * HZ);
+	time_left = wait_for_completion_timeout(&ar->target_suspend, 1 * HZ);
 
-	if (ret == 0) {
+	if (!time_left) {
 		ath10k_warn(ar, "suspend timed out - target pause event never came\n");
 		return -ETIMEDOUT;
 	}
@@ -1386,6 +1485,7 @@ struct ath10k *ath10k_core_create(size_t priv_size, struct device *dev,
 	init_completion(&ar->scan.completed);
 	init_completion(&ar->scan.on_channel);
 	init_completion(&ar->target_suspend);
+	init_completion(&ar->wow.wakeup_completed);
 
 	init_completion(&ar->install_key_done);
 	init_completion(&ar->vdev_setup_done);
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index f65310c3ba5f..78094f23c9dd 100644
--- a/drivers/net/wireless/ath/ath10k/core.h
+++ b/drivers/net/wireless/ath/ath10k/core.h
@@ -35,6 +35,7 @@
 #include "../dfs_pattern_detector.h"
 #include "spectral.h"
 #include "thermal.h"
+#include "wow.h"
 
 #define MS(_v, _f) (((_v) & _f##_MASK) >> _f##_LSB)
 #define SM(_v, _f) (((_v) << _f##_LSB) & _f##_MASK)
@@ -43,15 +44,16 @@
 #define ATH10K_SCAN_ID 0
 #define WMI_READY_TIMEOUT (5 * HZ)
 #define ATH10K_FLUSH_TIMEOUT_HZ (5*HZ)
-#define ATH10K_NUM_CHANS 38
+#define ATH10K_CONNECTION_LOSS_HZ (3*HZ)
+#define ATH10K_NUM_CHANS 39
 
 /* Antenna noise floor */
 #define ATH10K_DEFAULT_NOISE_FLOOR -95
 
 #define ATH10K_MAX_NUM_MGMT_PENDING 128
 
-/* number of failed packets */
-#define ATH10K_KICKOUT_THRESHOLD 50
+/* number of failed packets (20 packets with 16 sw reties each) */
+#define ATH10K_KICKOUT_THRESHOLD (20 * 16)
 
 /*
  * Use insanely high numbers to make sure that the firmware implementation
@@ -82,6 +84,8 @@ struct ath10k_skb_cb {
 	dma_addr_t paddr;
 	u8 eid;
 	u8 vdev_id;
+	enum ath10k_hw_txrx_mode txmode;
+	bool is_protected;
 
 	struct {
 		u8 tid;
@@ -301,6 +305,7 @@ struct ath10k_vif {
 	enum ath10k_beacon_state beacon_state;
 	void *beacon_buf;
 	dma_addr_t beacon_paddr;
+	unsigned long tx_paused; /* arbitrary values defined by target */
 
 	struct ath10k *ar;
 	struct ieee80211_vif *vif;
@@ -334,13 +339,13 @@ struct ath10k_vif {
 		} ap;
 	} u;
 
-	u8 fixed_rate;
-	u8 fixed_nss;
-	u8 force_sgi;
 	bool use_cts_prot;
 	int num_legacy_stations;
 	int txpower;
 	struct wmi_wmm_params_all_arg wmm_params;
+	struct work_struct ap_csa_work;
+	struct delayed_work connection_loss_work;
+	struct cfg80211_bitrate_mask bitrate_mask;
 };
 
 struct ath10k_vif_iter {
@@ -440,6 +445,23 @@ enum ath10k_fw_features {
 	 */
 	ATH10K_FW_FEATURE_MULTI_VIF_PS_SUPPORT = 5,
 
+	/* Some firmware revisions have an incomplete WoWLAN implementation
+	 * despite WMI service bit being advertised. This feature flag is used
+	 * to distinguish whether WoWLAN is really supported or not.
+	 */
+	ATH10K_FW_FEATURE_WOWLAN_SUPPORT = 6,
+
+	/* Don't trust error code from otp.bin */
+	ATH10K_FW_FEATURE_IGNORE_OTP_RESULT,
+
+	/* Some firmware revisions pad 4th hw address to 4 byte boundary making
+	 * it 8 bytes long in Native Wifi Rx decap.
+	 */
+	ATH10K_FW_FEATURE_NO_NWIFI_DECAP_4ADDR_PADDING,
+
+	/* Firmware supports bypassing PLL setting on init. */
+	ATH10K_FW_FEATURE_SUPPORTS_SKIP_CLOCK_INIT = 9,
+
 	/* keep last */
 	ATH10K_FW_FEATURE_COUNT,
 };
@@ -498,6 +520,11 @@ static inline const char *ath10k_scan_state_str(enum ath10k_scan_state state)
 	return "unknown";
 }
 
+enum ath10k_tx_pause_reason {
+	ATH10K_TX_PAUSE_Q_FULL,
+	ATH10K_TX_PAUSE_MAX,
+};
+
 struct ath10k {
 	struct ath_common ath_common;
 	struct ieee80211_hw *hw;
@@ -511,12 +538,15 @@ struct ath10k {
 	u32 fw_version_minor;
 	u16 fw_version_release;
 	u16 fw_version_build;
+	u32 fw_stats_req_mask;
 	u32 phy_capability;
 	u32 hw_min_tx_power;
 	u32 hw_max_tx_power;
 	u32 ht_cap_info;
 	u32 vht_cap_info;
 	u32 num_rf_chains;
+	/* protected by conf_mutex */
+	bool ani_enabled;
 
 	DECLARE_BITMAP(fw_features, ATH10K_FW_FEATURE_COUNT);
 
@@ -541,6 +571,13 @@ struct ath10k {
 		u32 patch_load_addr;
 		int uart_pin;
 
+		/* This is true if given HW chip has a quirky Cycle Counter
+		 * wraparound which resets to 0x7fffffff instead of 0. All
+		 * other CC related counters (e.g. Rx Clear Count) are divided
+		 * by 2 so they never wraparound themselves.
+		 */
+		bool has_shifted_cc_wraparound;
+
 		struct ath10k_hw_params_fw {
 			const char *dir;
 			const char *fw;
@@ -565,6 +602,9 @@ struct ath10k {
 
 	const struct firmware *cal_file;
 
+	char spec_board_id[100];
+	bool spec_board_loaded;
+
 	int fw_api;
 	enum ath10k_cal_mode cal_mode;
 
@@ -593,6 +633,7 @@ struct ath10k {
 	struct cfg80211_chan_def chandef;
 
 	unsigned long long free_vdev_map;
+	struct ath10k_vif *monitor_arvif;
 	bool monitor;
 	int monitor_vdev_id;
 	bool monitor_started;
@@ -633,6 +674,7 @@ struct ath10k {
 	int max_num_peers;
 	int max_num_stations;
 	int max_num_vdevs;
+	int max_num_tdls_vdevs;
 
 	struct work_struct offchan_tx_work;
 	struct sk_buff_head offchan_tx_queue;
@@ -653,8 +695,18 @@ struct ath10k {
 	u32 survey_last_cycle_count;
 	struct survey_info survey[ATH10K_NUM_CHANS];
 
+	/* Channel info events are expected to come in pairs without and with
+	 * COMPLETE flag set respectively for each channel visit during scan.
+	 *
+	 * However there are deviations from this rule. This flag is used to
+	 * avoid reporting garbage data.
+	 */
+	bool ch_info_can_report_survey;
+
 	struct dfs_pattern_detector *dfs_detector;
 
+	unsigned long tx_paused; /* see ATH10K_TX_PAUSE_ */
+
 #ifdef CONFIG_ATH10K_DEBUGFS
 	struct ath10k_debug debug;
 #endif
@@ -686,6 +738,7 @@ struct ath10k {
 	} stats;
 
 	struct ath10k_thermal thermal;
+	struct ath10k_wow wow;
 
 	/* 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 301081db1ef6..8fa606a9c4dd 100644
--- a/drivers/net/wireless/ath/ath10k/debug.c
+++ b/drivers/net/wireless/ath/ath10k/debug.c
@@ -124,10 +124,14 @@ 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 cal %s max_sta %d\n",
+	ath10k_info(ar, "%s (0x%08x, 0x%08x%s%s%s) 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,
+		    (strlen(ar->spec_board_id) > 0 ? ", " : ""),
+		    ar->spec_board_id,
+		    (strlen(ar->spec_board_id) > 0 && !ar->spec_board_loaded
+		     ? " fallback" : ""),
 		    ar->hw->wiphy->fw_version,
 		    ar->fw_api,
 		    ar->htt.target_version_major,
@@ -380,12 +384,12 @@ unlock:
 
 static int ath10k_debug_fw_stats_request(struct ath10k *ar)
 {
-	unsigned long timeout;
+	unsigned long timeout, time_left;
 	int ret;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	timeout = jiffies + msecs_to_jiffies(1*HZ);
+	timeout = jiffies + msecs_to_jiffies(1 * HZ);
 
 	ath10k_debug_fw_stats_reset(ar);
 
@@ -395,18 +399,16 @@ static int ath10k_debug_fw_stats_request(struct ath10k *ar)
 
 		reinit_completion(&ar->debug.fw_stats_complete);
 
-		ret = ath10k_wmi_request_stats(ar,
-					       WMI_STAT_PDEV |
-					       WMI_STAT_VDEV |
-					       WMI_STAT_PEER);
+		ret = ath10k_wmi_request_stats(ar, ar->fw_stats_req_mask);
 		if (ret) {
 			ath10k_warn(ar, "could not request stats (%d)\n", ret);
 			return ret;
 		}
 
-		ret = wait_for_completion_timeout(&ar->debug.fw_stats_complete,
-						  1*HZ);
-		if (ret == 0)
+		time_left =
+		wait_for_completion_timeout(&ar->debug.fw_stats_complete,
+					    1 * HZ);
+		if (!time_left)
 			return -ETIMEDOUT;
 
 		spin_lock_bh(&ar->data_lock);
@@ -1708,6 +1710,61 @@ static int ath10k_debug_cal_data_release(struct inode *inode,
 	return 0;
 }
 
+static ssize_t ath10k_write_ani_enable(struct file *file,
+				       const char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	int ret;
+	u8 enable;
+
+	if (kstrtou8_from_user(user_buf, count, 0, &enable))
+		return -EINVAL;
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (ar->ani_enabled == enable) {
+		ret = count;
+		goto exit;
+	}
+
+	ret = ath10k_wmi_pdev_set_param(ar, ar->wmi.pdev_param->ani_enable,
+					enable);
+	if (ret) {
+		ath10k_warn(ar, "ani_enable failed from debugfs: %d\n", ret);
+		goto exit;
+	}
+	ar->ani_enabled = enable;
+
+	ret = count;
+
+exit:
+	mutex_unlock(&ar->conf_mutex);
+
+	return ret;
+}
+
+static ssize_t ath10k_read_ani_enable(struct file *file, char __user *user_buf,
+				      size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	int len = 0;
+	char buf[32];
+
+	len = scnprintf(buf, sizeof(buf) - len, "%d\n",
+			ar->ani_enabled);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static const struct file_operations fops_ani_enable = {
+	.read = ath10k_read_ani_enable,
+	.write = ath10k_write_ani_enable,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
 static const struct file_operations fops_cal_data = {
 	.open = ath10k_debug_cal_data_open,
 	.read = ath10k_debug_cal_data_read,
@@ -1991,6 +2048,50 @@ static const struct file_operations fops_pktlog_filter = {
 	.open = simple_open
 };
 
+static ssize_t ath10k_write_quiet_period(struct file *file,
+					 const char __user *ubuf,
+					 size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	u32 period;
+
+	if (kstrtouint_from_user(ubuf, count, 0, &period))
+		return -EINVAL;
+
+	if (period < ATH10K_QUIET_PERIOD_MIN) {
+		ath10k_warn(ar, "Quiet period %u can not be lesser than 25ms\n",
+			    period);
+		return -EINVAL;
+	}
+	mutex_lock(&ar->conf_mutex);
+	ar->thermal.quiet_period = period;
+	ath10k_thermal_set_throttling(ar);
+	mutex_unlock(&ar->conf_mutex);
+
+	return count;
+}
+
+static ssize_t ath10k_read_quiet_period(struct file *file, char __user *ubuf,
+					size_t count, loff_t *ppos)
+{
+	char buf[32];
+	struct ath10k *ar = file->private_data;
+	int len = 0;
+
+	mutex_lock(&ar->conf_mutex);
+	len = scnprintf(buf, sizeof(buf) - len, "%d\n",
+			ar->thermal.quiet_period);
+	mutex_unlock(&ar->conf_mutex);
+
+	return simple_read_from_buffer(ubuf, count, ppos, buf, len);
+}
+
+static const struct file_operations fops_quiet_period = {
+	.read = ath10k_read_quiet_period,
+	.write = ath10k_write_quiet_period,
+	.open = simple_open
+};
+
 int ath10k_debug_create(struct ath10k *ar)
 {
 	ar->debug.fw_crash_data = vzalloc(sizeof(*ar->debug.fw_crash_data));
@@ -2068,6 +2169,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("ani_enable", S_IRUSR | S_IWUSR,
+			    ar->debug.debugfs_phy, ar, &fops_ani_enable);
+
 	debugfs_create_file("nf_cal_period", S_IRUSR | S_IWUSR,
 			    ar->debug.debugfs_phy, ar, &fops_nf_cal_period);
 
@@ -2088,6 +2192,9 @@ int ath10k_debug_register(struct ath10k *ar)
 	debugfs_create_file("pktlog_filter", S_IRUGO | S_IWUSR,
 			    ar->debug.debugfs_phy, ar, &fops_pktlog_filter);
 
+	debugfs_create_file("quiet_period", S_IRUGO | S_IWUSR,
+			    ar->debug.debugfs_phy, ar, &fops_quiet_period);
+
 	return 0;
 }
 
diff --git a/drivers/net/wireless/ath/ath10k/debug.h b/drivers/net/wireless/ath/ath10k/debug.h
index a12b8323f9f1..53bd6a19eab6 100644
--- a/drivers/net/wireless/ath/ath10k/debug.h
+++ b/drivers/net/wireless/ath/ath10k/debug.h
@@ -36,6 +36,7 @@ enum ath10k_debug_mask {
 	ATH10K_DBG_REGULATORY	= 0x00000800,
 	ATH10K_DBG_TESTMODE	= 0x00001000,
 	ATH10K_DBG_WMI_PRINT	= 0x00002000,
+	ATH10K_DBG_PCI_PS	= 0x00004000,
 	ATH10K_DBG_ANY		= 0xffffffff,
 };
 
diff --git a/drivers/net/wireless/ath/ath10k/htc.c b/drivers/net/wireless/ath/ath10k/htc.c
index 2fd9e180272b..85bfa2acb801 100644
--- a/drivers/net/wireless/ath/ath10k/htc.c
+++ b/drivers/net/wireless/ath/ath10k/htc.c
@@ -86,21 +86,6 @@ static void ath10k_htc_notify_tx_completion(struct ath10k_htc_ep *ep,
 	ep->ep_ops.ep_tx_complete(ep->htc->ar, skb);
 }
 
-/* assumes tx_lock is held */
-static bool ath10k_htc_ep_need_credit_update(struct ath10k_htc_ep *ep)
-{
-	struct ath10k *ar = ep->htc->ar;
-
-	if (!ep->tx_credit_flow_enabled)
-		return false;
-	if (ep->tx_credits >= ep->tx_credits_per_max_message)
-		return false;
-
-	ath10k_dbg(ar, ATH10K_DBG_HTC, "HTC: endpoint %d needs credit update\n",
-		   ep->eid);
-	return true;
-}
-
 static void ath10k_htc_prepare_tx_skb(struct ath10k_htc_ep *ep,
 				      struct sk_buff *skb)
 {
@@ -111,13 +96,10 @@ static void ath10k_htc_prepare_tx_skb(struct ath10k_htc_ep *ep,
 	hdr->eid = ep->eid;
 	hdr->len = __cpu_to_le16(skb->len - sizeof(*hdr));
 	hdr->flags = 0;
+	hdr->flags |= ATH10K_HTC_FLAG_NEED_CREDIT_UPDATE;
 
 	spin_lock_bh(&ep->htc->tx_lock);
 	hdr->seq_no = ep->seq_no++;
-
-	if (ath10k_htc_ep_need_credit_update(ep))
-		hdr->flags |= ATH10K_HTC_FLAG_NEED_CREDIT_UPDATE;
-
 	spin_unlock_bh(&ep->htc->tx_lock);
 }
 
@@ -414,7 +396,8 @@ static int ath10k_htc_rx_completion_handler(struct ath10k *ar,
 		struct ath10k_htc_msg *msg = (struct ath10k_htc_msg *)skb->data;
 
 		switch (__le16_to_cpu(msg->hdr.message_id)) {
-		default:
+		case ATH10K_HTC_MSG_READY_ID:
+		case ATH10K_HTC_MSG_CONNECT_SERVICE_RESP_ID:
 			/* handle HTC control message */
 			if (completion_done(&htc->ctl_resp)) {
 				/*
@@ -438,6 +421,10 @@ static int ath10k_htc_rx_completion_handler(struct ath10k *ar,
 			break;
 		case ATH10K_HTC_MSG_SEND_SUSPEND_COMPLETE:
 			htc->htc_ops.target_send_suspend_complete(ar);
+			break;
+		default:
+			ath10k_warn(ar, "ignoring unsolicited htc ep0 event\n");
+			break;
 		}
 		goto out;
 	}
@@ -548,6 +535,7 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 {
 	struct ath10k *ar = htc->ar;
 	int i, status = 0;
+	unsigned long time_left;
 	struct ath10k_htc_svc_conn_req conn_req;
 	struct ath10k_htc_svc_conn_resp conn_resp;
 	struct ath10k_htc_msg *msg;
@@ -555,9 +543,9 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 	u16 credit_count;
 	u16 credit_size;
 
-	status = wait_for_completion_timeout(&htc->ctl_resp,
-					     ATH10K_HTC_WAIT_TIMEOUT_HZ);
-	if (status == 0) {
+	time_left = wait_for_completion_timeout(&htc->ctl_resp,
+						ATH10K_HTC_WAIT_TIMEOUT_HZ);
+	if (!time_left) {
 		/* Workaround: In some cases the PCI HIF doesn't
 		 * receive interrupt for the control response message
 		 * even if the buffer was completed. It is suspected
@@ -569,10 +557,11 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 		for (i = 0; i < CE_COUNT; i++)
 			ath10k_hif_send_complete_check(htc->ar, i, 1);
 
-		status = wait_for_completion_timeout(&htc->ctl_resp,
-						     ATH10K_HTC_WAIT_TIMEOUT_HZ);
+		time_left =
+		wait_for_completion_timeout(&htc->ctl_resp,
+					    ATH10K_HTC_WAIT_TIMEOUT_HZ);
 
-		if (status == 0)
+		if (!time_left)
 			status = -ETIMEDOUT;
 	}
 
@@ -646,6 +635,7 @@ int ath10k_htc_connect_service(struct ath10k_htc *htc,
 	struct sk_buff *skb;
 	unsigned int max_msg_size = 0;
 	int length, status;
+	unsigned long time_left;
 	bool disable_credit_flow_ctrl = false;
 	u16 message_id, service_id, flags = 0;
 	u8 tx_alloc = 0;
@@ -701,10 +691,10 @@ int ath10k_htc_connect_service(struct ath10k_htc *htc,
 	}
 
 	/* wait for response */
-	status = wait_for_completion_timeout(&htc->ctl_resp,
-					     ATH10K_HTC_CONN_SVC_TIMEOUT_HZ);
-	if (status == 0) {
-		ath10k_err(ar, "Service connect timeout: %d\n", status);
+	time_left = wait_for_completion_timeout(&htc->ctl_resp,
+						ATH10K_HTC_CONN_SVC_TIMEOUT_HZ);
+	if (!time_left) {
+		ath10k_err(ar, "Service connect timeout\n");
 		return -ETIMEDOUT;
 	}
 
diff --git a/drivers/net/wireless/ath/ath10k/htt.c b/drivers/net/wireless/ath/ath10k/htt.c
index 4f59ab923e48..6da6ef26143a 100644
--- a/drivers/net/wireless/ath/ath10k/htt.c
+++ b/drivers/net/wireless/ath/ath10k/htt.c
@@ -22,6 +22,86 @@
 #include "core.h"
 #include "debug.h"
 
+static const enum htt_t2h_msg_type htt_main_t2h_msg_types[] = {
+	[HTT_MAIN_T2H_MSG_TYPE_VERSION_CONF] = HTT_T2H_MSG_TYPE_VERSION_CONF,
+	[HTT_MAIN_T2H_MSG_TYPE_RX_IND] = HTT_T2H_MSG_TYPE_RX_IND,
+	[HTT_MAIN_T2H_MSG_TYPE_RX_FLUSH] = HTT_T2H_MSG_TYPE_RX_FLUSH,
+	[HTT_MAIN_T2H_MSG_TYPE_PEER_MAP] = HTT_T2H_MSG_TYPE_PEER_MAP,
+	[HTT_MAIN_T2H_MSG_TYPE_PEER_UNMAP] = HTT_T2H_MSG_TYPE_PEER_UNMAP,
+	[HTT_MAIN_T2H_MSG_TYPE_RX_ADDBA] = HTT_T2H_MSG_TYPE_RX_ADDBA,
+	[HTT_MAIN_T2H_MSG_TYPE_RX_DELBA] = HTT_T2H_MSG_TYPE_RX_DELBA,
+	[HTT_MAIN_T2H_MSG_TYPE_TX_COMPL_IND] = HTT_T2H_MSG_TYPE_TX_COMPL_IND,
+	[HTT_MAIN_T2H_MSG_TYPE_PKTLOG] = HTT_T2H_MSG_TYPE_PKTLOG,
+	[HTT_MAIN_T2H_MSG_TYPE_STATS_CONF] = HTT_T2H_MSG_TYPE_STATS_CONF,
+	[HTT_MAIN_T2H_MSG_TYPE_RX_FRAG_IND] = HTT_T2H_MSG_TYPE_RX_FRAG_IND,
+	[HTT_MAIN_T2H_MSG_TYPE_SEC_IND] = HTT_T2H_MSG_TYPE_SEC_IND,
+	[HTT_MAIN_T2H_MSG_TYPE_TX_INSPECT_IND] =
+		HTT_T2H_MSG_TYPE_TX_INSPECT_IND,
+	[HTT_MAIN_T2H_MSG_TYPE_MGMT_TX_COMPL_IND] =
+		HTT_T2H_MSG_TYPE_MGMT_TX_COMPLETION,
+	[HTT_MAIN_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND] =
+		HTT_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND,
+	[HTT_MAIN_T2H_MSG_TYPE_RX_PN_IND] = HTT_T2H_MSG_TYPE_RX_PN_IND,
+	[HTT_MAIN_T2H_MSG_TYPE_RX_OFFLOAD_DELIVER_IND] =
+		HTT_T2H_MSG_TYPE_RX_OFFLOAD_DELIVER_IND,
+	[HTT_MAIN_T2H_MSG_TYPE_TEST] = HTT_T2H_MSG_TYPE_TEST,
+};
+
+static const enum htt_t2h_msg_type htt_10x_t2h_msg_types[] = {
+	[HTT_10X_T2H_MSG_TYPE_VERSION_CONF] = HTT_T2H_MSG_TYPE_VERSION_CONF,
+	[HTT_10X_T2H_MSG_TYPE_RX_IND] = HTT_T2H_MSG_TYPE_RX_IND,
+	[HTT_10X_T2H_MSG_TYPE_RX_FLUSH] = HTT_T2H_MSG_TYPE_RX_FLUSH,
+	[HTT_10X_T2H_MSG_TYPE_PEER_MAP] = HTT_T2H_MSG_TYPE_PEER_MAP,
+	[HTT_10X_T2H_MSG_TYPE_PEER_UNMAP] = HTT_T2H_MSG_TYPE_PEER_UNMAP,
+	[HTT_10X_T2H_MSG_TYPE_RX_ADDBA] = HTT_T2H_MSG_TYPE_RX_ADDBA,
+	[HTT_10X_T2H_MSG_TYPE_RX_DELBA] = HTT_T2H_MSG_TYPE_RX_DELBA,
+	[HTT_10X_T2H_MSG_TYPE_TX_COMPL_IND] = HTT_T2H_MSG_TYPE_TX_COMPL_IND,
+	[HTT_10X_T2H_MSG_TYPE_PKTLOG] = HTT_T2H_MSG_TYPE_PKTLOG,
+	[HTT_10X_T2H_MSG_TYPE_STATS_CONF] = HTT_T2H_MSG_TYPE_STATS_CONF,
+	[HTT_10X_T2H_MSG_TYPE_RX_FRAG_IND] = HTT_T2H_MSG_TYPE_RX_FRAG_IND,
+	[HTT_10X_T2H_MSG_TYPE_SEC_IND] = HTT_T2H_MSG_TYPE_SEC_IND,
+	[HTT_10X_T2H_MSG_TYPE_RC_UPDATE_IND] = HTT_T2H_MSG_TYPE_RC_UPDATE_IND,
+	[HTT_10X_T2H_MSG_TYPE_TX_INSPECT_IND] = HTT_T2H_MSG_TYPE_TX_INSPECT_IND,
+	[HTT_10X_T2H_MSG_TYPE_TEST] = HTT_T2H_MSG_TYPE_TEST,
+	[HTT_10X_T2H_MSG_TYPE_CHAN_CHANGE] = HTT_T2H_MSG_TYPE_CHAN_CHANGE,
+	[HTT_10X_T2H_MSG_TYPE_AGGR_CONF] = HTT_T2H_MSG_TYPE_AGGR_CONF,
+	[HTT_10X_T2H_MSG_TYPE_STATS_NOUPLOAD] = HTT_T2H_MSG_TYPE_STATS_NOUPLOAD,
+	[HTT_10X_T2H_MSG_TYPE_MGMT_TX_COMPL_IND] =
+		HTT_T2H_MSG_TYPE_MGMT_TX_COMPLETION,
+};
+
+static const enum htt_t2h_msg_type htt_tlv_t2h_msg_types[] = {
+	[HTT_TLV_T2H_MSG_TYPE_VERSION_CONF] = HTT_T2H_MSG_TYPE_VERSION_CONF,
+	[HTT_TLV_T2H_MSG_TYPE_RX_IND] = HTT_T2H_MSG_TYPE_RX_IND,
+	[HTT_TLV_T2H_MSG_TYPE_RX_FLUSH] = HTT_T2H_MSG_TYPE_RX_FLUSH,
+	[HTT_TLV_T2H_MSG_TYPE_PEER_MAP] = HTT_T2H_MSG_TYPE_PEER_MAP,
+	[HTT_TLV_T2H_MSG_TYPE_PEER_UNMAP] = HTT_T2H_MSG_TYPE_PEER_UNMAP,
+	[HTT_TLV_T2H_MSG_TYPE_RX_ADDBA] = HTT_T2H_MSG_TYPE_RX_ADDBA,
+	[HTT_TLV_T2H_MSG_TYPE_RX_DELBA] = HTT_T2H_MSG_TYPE_RX_DELBA,
+	[HTT_TLV_T2H_MSG_TYPE_TX_COMPL_IND] = HTT_T2H_MSG_TYPE_TX_COMPL_IND,
+	[HTT_TLV_T2H_MSG_TYPE_PKTLOG] = HTT_T2H_MSG_TYPE_PKTLOG,
+	[HTT_TLV_T2H_MSG_TYPE_STATS_CONF] = HTT_T2H_MSG_TYPE_STATS_CONF,
+	[HTT_TLV_T2H_MSG_TYPE_RX_FRAG_IND] = HTT_T2H_MSG_TYPE_RX_FRAG_IND,
+	[HTT_TLV_T2H_MSG_TYPE_SEC_IND] = HTT_T2H_MSG_TYPE_SEC_IND,
+	[HTT_TLV_T2H_MSG_TYPE_RC_UPDATE_IND] = HTT_T2H_MSG_TYPE_RC_UPDATE_IND,
+	[HTT_TLV_T2H_MSG_TYPE_TX_INSPECT_IND] = HTT_T2H_MSG_TYPE_TX_INSPECT_IND,
+	[HTT_TLV_T2H_MSG_TYPE_MGMT_TX_COMPL_IND] =
+		HTT_T2H_MSG_TYPE_MGMT_TX_COMPLETION,
+	[HTT_TLV_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND] =
+		HTT_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND,
+	[HTT_TLV_T2H_MSG_TYPE_RX_PN_IND] = HTT_T2H_MSG_TYPE_RX_PN_IND,
+	[HTT_TLV_T2H_MSG_TYPE_RX_OFFLOAD_DELIVER_IND] =
+		HTT_T2H_MSG_TYPE_RX_OFFLOAD_DELIVER_IND,
+	[HTT_TLV_T2H_MSG_TYPE_RX_IN_ORD_PADDR_IND] =
+		HTT_T2H_MSG_TYPE_RX_IN_ORD_PADDR_IND,
+	[HTT_TLV_T2H_MSG_TYPE_WDI_IPA_OP_RESPONSE] =
+		HTT_T2H_MSG_TYPE_WDI_IPA_OP_RESPONSE,
+	[HTT_TLV_T2H_MSG_TYPE_CHAN_CHANGE] = HTT_T2H_MSG_TYPE_CHAN_CHANGE,
+	[HTT_TLV_T2H_MSG_TYPE_RX_OFLD_PKT_ERR] =
+		HTT_T2H_MSG_TYPE_RX_OFLD_PKT_ERR,
+	[HTT_TLV_T2H_MSG_TYPE_TEST] = HTT_T2H_MSG_TYPE_TEST,
+};
+
 int ath10k_htt_connect(struct ath10k_htt *htt)
 {
 	struct ath10k_htc_svc_conn_req conn_req;
@@ -66,6 +146,24 @@ int ath10k_htt_init(struct ath10k *ar)
 		8 + /* llc snap */
 		2; /* ip4 dscp or ip6 priority */
 
+	switch (ar->htt.op_version) {
+	case ATH10K_FW_HTT_OP_VERSION_10_1:
+		ar->htt.t2h_msg_types = htt_10x_t2h_msg_types;
+		ar->htt.t2h_msg_types_max = HTT_10X_T2H_NUM_MSGS;
+		break;
+	case ATH10K_FW_HTT_OP_VERSION_TLV:
+		ar->htt.t2h_msg_types = htt_tlv_t2h_msg_types;
+		ar->htt.t2h_msg_types_max = HTT_TLV_T2H_NUM_MSGS;
+		break;
+	case ATH10K_FW_HTT_OP_VERSION_MAIN:
+		ar->htt.t2h_msg_types = htt_main_t2h_msg_types;
+		ar->htt.t2h_msg_types_max = HTT_MAIN_T2H_NUM_MSGS;
+		break;
+	case ATH10K_FW_HTT_OP_VERSION_MAX:
+	case ATH10K_FW_HTT_OP_VERSION_UNSET:
+		WARN_ON(1);
+		return -EINVAL;
+	}
 	return 0;
 }
 
diff --git a/drivers/net/wireless/ath/ath10k/htt.h b/drivers/net/wireless/ath/ath10k/htt.h
index 874bf44ff7a2..7e8a0d835663 100644
--- a/drivers/net/wireless/ath/ath10k/htt.h
+++ b/drivers/net/wireless/ath/ath10k/htt.h
@@ -25,7 +25,9 @@
 #include <net/mac80211.h>
 
 #include "htc.h"
+#include "hw.h"
 #include "rx_desc.h"
+#include "hw.h"
 
 enum htt_dbg_stats_type {
 	HTT_DBG_STATS_WAL_PDEV_TXRX = 1 << 0,
@@ -271,35 +273,108 @@ enum htt_mgmt_tx_status {
 
 /*=== target -> host messages ===============================================*/
 
-enum htt_t2h_msg_type {
-	HTT_T2H_MSG_TYPE_VERSION_CONF		= 0x0,
-	HTT_T2H_MSG_TYPE_RX_IND			= 0x1,
-	HTT_T2H_MSG_TYPE_RX_FLUSH		= 0x2,
-	HTT_T2H_MSG_TYPE_PEER_MAP		= 0x3,
-	HTT_T2H_MSG_TYPE_PEER_UNMAP		= 0x4,
-	HTT_T2H_MSG_TYPE_RX_ADDBA		= 0x5,
-	HTT_T2H_MSG_TYPE_RX_DELBA		= 0x6,
-	HTT_T2H_MSG_TYPE_TX_COMPL_IND		= 0x7,
-	HTT_T2H_MSG_TYPE_PKTLOG			= 0x8,
-	HTT_T2H_MSG_TYPE_STATS_CONF		= 0x9,
-	HTT_T2H_MSG_TYPE_RX_FRAG_IND		= 0xa,
-	HTT_T2H_MSG_TYPE_SEC_IND		= 0xb,
-	HTT_T2H_MSG_TYPE_RC_UPDATE_IND		= 0xc,
-	HTT_T2H_MSG_TYPE_TX_INSPECT_IND		= 0xd,
-	HTT_T2H_MSG_TYPE_MGMT_TX_COMPLETION	= 0xe,
-	HTT_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND	= 0xf,
-	HTT_T2H_MSG_TYPE_RX_PN_IND		= 0x10,
-	HTT_T2H_MSG_TYPE_RX_OFFLOAD_DELIVER_IND = 0x11,
-	HTT_T2H_MSG_TYPE_RX_IN_ORD_PADDR_IND	= 0x12,
+enum htt_main_t2h_msg_type {
+	HTT_MAIN_T2H_MSG_TYPE_VERSION_CONF             = 0x0,
+	HTT_MAIN_T2H_MSG_TYPE_RX_IND                   = 0x1,
+	HTT_MAIN_T2H_MSG_TYPE_RX_FLUSH                 = 0x2,
+	HTT_MAIN_T2H_MSG_TYPE_PEER_MAP                 = 0x3,
+	HTT_MAIN_T2H_MSG_TYPE_PEER_UNMAP               = 0x4,
+	HTT_MAIN_T2H_MSG_TYPE_RX_ADDBA                 = 0x5,
+	HTT_MAIN_T2H_MSG_TYPE_RX_DELBA                 = 0x6,
+	HTT_MAIN_T2H_MSG_TYPE_TX_COMPL_IND             = 0x7,
+	HTT_MAIN_T2H_MSG_TYPE_PKTLOG                   = 0x8,
+	HTT_MAIN_T2H_MSG_TYPE_STATS_CONF               = 0x9,
+	HTT_MAIN_T2H_MSG_TYPE_RX_FRAG_IND              = 0xa,
+	HTT_MAIN_T2H_MSG_TYPE_SEC_IND                  = 0xb,
+	HTT_MAIN_T2H_MSG_TYPE_TX_INSPECT_IND           = 0xd,
+	HTT_MAIN_T2H_MSG_TYPE_MGMT_TX_COMPL_IND        = 0xe,
+	HTT_MAIN_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND     = 0xf,
+	HTT_MAIN_T2H_MSG_TYPE_RX_PN_IND                = 0x10,
+	HTT_MAIN_T2H_MSG_TYPE_RX_OFFLOAD_DELIVER_IND   = 0x11,
+	HTT_MAIN_T2H_MSG_TYPE_TEST,
+	/* keep this last */
+	HTT_MAIN_T2H_NUM_MSGS
+};
+
+enum htt_10x_t2h_msg_type {
+	HTT_10X_T2H_MSG_TYPE_VERSION_CONF              = 0x0,
+	HTT_10X_T2H_MSG_TYPE_RX_IND                    = 0x1,
+	HTT_10X_T2H_MSG_TYPE_RX_FLUSH                  = 0x2,
+	HTT_10X_T2H_MSG_TYPE_PEER_MAP                  = 0x3,
+	HTT_10X_T2H_MSG_TYPE_PEER_UNMAP                = 0x4,
+	HTT_10X_T2H_MSG_TYPE_RX_ADDBA                  = 0x5,
+	HTT_10X_T2H_MSG_TYPE_RX_DELBA                  = 0x6,
+	HTT_10X_T2H_MSG_TYPE_TX_COMPL_IND              = 0x7,
+	HTT_10X_T2H_MSG_TYPE_PKTLOG                    = 0x8,
+	HTT_10X_T2H_MSG_TYPE_STATS_CONF                = 0x9,
+	HTT_10X_T2H_MSG_TYPE_RX_FRAG_IND               = 0xa,
+	HTT_10X_T2H_MSG_TYPE_SEC_IND                   = 0xb,
+	HTT_10X_T2H_MSG_TYPE_RC_UPDATE_IND             = 0xc,
+	HTT_10X_T2H_MSG_TYPE_TX_INSPECT_IND            = 0xd,
+	HTT_10X_T2H_MSG_TYPE_TEST                      = 0xe,
+	HTT_10X_T2H_MSG_TYPE_CHAN_CHANGE               = 0xf,
+	HTT_10X_T2H_MSG_TYPE_AGGR_CONF                 = 0x11,
+	HTT_10X_T2H_MSG_TYPE_STATS_NOUPLOAD            = 0x12,
+	HTT_10X_T2H_MSG_TYPE_MGMT_TX_COMPL_IND         = 0x13,
+	/* keep this last */
+	HTT_10X_T2H_NUM_MSGS
+};
+
+enum htt_tlv_t2h_msg_type {
+	HTT_TLV_T2H_MSG_TYPE_VERSION_CONF              = 0x0,
+	HTT_TLV_T2H_MSG_TYPE_RX_IND                    = 0x1,
+	HTT_TLV_T2H_MSG_TYPE_RX_FLUSH                  = 0x2,
+	HTT_TLV_T2H_MSG_TYPE_PEER_MAP                  = 0x3,
+	HTT_TLV_T2H_MSG_TYPE_PEER_UNMAP                = 0x4,
+	HTT_TLV_T2H_MSG_TYPE_RX_ADDBA                  = 0x5,
+	HTT_TLV_T2H_MSG_TYPE_RX_DELBA                  = 0x6,
+	HTT_TLV_T2H_MSG_TYPE_TX_COMPL_IND              = 0x7,
+	HTT_TLV_T2H_MSG_TYPE_PKTLOG                    = 0x8,
+	HTT_TLV_T2H_MSG_TYPE_STATS_CONF                = 0x9,
+	HTT_TLV_T2H_MSG_TYPE_RX_FRAG_IND               = 0xa,
+	HTT_TLV_T2H_MSG_TYPE_SEC_IND                   = 0xb,
+	HTT_TLV_T2H_MSG_TYPE_RC_UPDATE_IND             = 0xc, /* deprecated */
+	HTT_TLV_T2H_MSG_TYPE_TX_INSPECT_IND            = 0xd,
+	HTT_TLV_T2H_MSG_TYPE_MGMT_TX_COMPL_IND         = 0xe,
+	HTT_TLV_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND      = 0xf,
+	HTT_TLV_T2H_MSG_TYPE_RX_PN_IND                 = 0x10,
+	HTT_TLV_T2H_MSG_TYPE_RX_OFFLOAD_DELIVER_IND    = 0x11,
+	HTT_TLV_T2H_MSG_TYPE_RX_IN_ORD_PADDR_IND       = 0x12,
 	/* 0x13 reservd */
-	HTT_T2H_MSG_TYPE_WDI_IPA_OP_RESPONSE	= 0x14,
+	HTT_TLV_T2H_MSG_TYPE_WDI_IPA_OP_RESPONSE       = 0x14,
+	HTT_TLV_T2H_MSG_TYPE_CHAN_CHANGE               = 0x15,
+	HTT_TLV_T2H_MSG_TYPE_RX_OFLD_PKT_ERR           = 0x16,
+	HTT_TLV_T2H_MSG_TYPE_TEST,
+	/* keep this last */
+	HTT_TLV_T2H_NUM_MSGS
+};
 
-	/* FIXME: Do not depend on this event id. Numbering of this event id is
-	 * broken across different firmware revisions and HTT version fails to
-	 * indicate this.
-	 */
+enum htt_t2h_msg_type {
+	HTT_T2H_MSG_TYPE_VERSION_CONF,
+	HTT_T2H_MSG_TYPE_RX_IND,
+	HTT_T2H_MSG_TYPE_RX_FLUSH,
+	HTT_T2H_MSG_TYPE_PEER_MAP,
+	HTT_T2H_MSG_TYPE_PEER_UNMAP,
+	HTT_T2H_MSG_TYPE_RX_ADDBA,
+	HTT_T2H_MSG_TYPE_RX_DELBA,
+	HTT_T2H_MSG_TYPE_TX_COMPL_IND,
+	HTT_T2H_MSG_TYPE_PKTLOG,
+	HTT_T2H_MSG_TYPE_STATS_CONF,
+	HTT_T2H_MSG_TYPE_RX_FRAG_IND,
+	HTT_T2H_MSG_TYPE_SEC_IND,
+	HTT_T2H_MSG_TYPE_RC_UPDATE_IND,
+	HTT_T2H_MSG_TYPE_TX_INSPECT_IND,
+	HTT_T2H_MSG_TYPE_MGMT_TX_COMPLETION,
+	HTT_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND,
+	HTT_T2H_MSG_TYPE_RX_PN_IND,
+	HTT_T2H_MSG_TYPE_RX_OFFLOAD_DELIVER_IND,
+	HTT_T2H_MSG_TYPE_RX_IN_ORD_PADDR_IND,
+	HTT_T2H_MSG_TYPE_WDI_IPA_OP_RESPONSE,
+	HTT_T2H_MSG_TYPE_CHAN_CHANGE,
+	HTT_T2H_MSG_TYPE_RX_OFLD_PKT_ERR,
+	HTT_T2H_MSG_TYPE_AGGR_CONF,
+	HTT_T2H_MSG_TYPE_STATS_NOUPLOAD,
 	HTT_T2H_MSG_TYPE_TEST,
-
 	/* keep this last */
 	HTT_T2H_NUM_MSGS
 };
@@ -1222,6 +1297,7 @@ struct htt_tx_done {
 	u32 msdu_id;
 	bool discard;
 	bool no_ack;
+	bool success;
 };
 
 struct htt_peer_map_event {
@@ -1248,6 +1324,10 @@ struct ath10k_htt {
 	u8 target_version_major;
 	u8 target_version_minor;
 	struct completion target_version_received;
+	enum ath10k_fw_htt_op_version op_version;
+
+	const enum htt_t2h_msg_type *t2h_msg_types;
+	u32 t2h_msg_types_max;
 
 	struct {
 		/*
diff --git a/drivers/net/wireless/ath/ath10k/htt_rx.c b/drivers/net/wireless/ath/ath10k/htt_rx.c
index 01a2b384f358..89eb16b30fc4 100644
--- a/drivers/net/wireless/ath/ath10k/htt_rx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_rx.c
@@ -637,58 +637,21 @@ static int ath10k_htt_rx_crypto_tail_len(struct ath10k *ar,
 	return 0;
 }
 
-struct rfc1042_hdr {
-	u8 llc_dsap;
-	u8 llc_ssap;
-	u8 llc_ctrl;
-	u8 snap_oui[3];
-	__be16 snap_type;
-} __packed;
-
 struct amsdu_subframe_hdr {
 	u8 dst[ETH_ALEN];
 	u8 src[ETH_ALEN];
 	__be16 len;
 } __packed;
 
-static const u8 rx_legacy_rate_idx[] = {
-	3,	/* 0x00  - 11Mbps  */
-	2,	/* 0x01  - 5.5Mbps */
-	1,	/* 0x02  - 2Mbps   */
-	0,	/* 0x03  - 1Mbps   */
-	3,	/* 0x04  - 11Mbps  */
-	2,	/* 0x05  - 5.5Mbps */
-	1,	/* 0x06  - 2Mbps   */
-	0,	/* 0x07  - 1Mbps   */
-	10,	/* 0x08  - 48Mbps  */
-	8,	/* 0x09  - 24Mbps  */
-	6,	/* 0x0A  - 12Mbps  */
-	4,	/* 0x0B  - 6Mbps   */
-	11,	/* 0x0C  - 54Mbps  */
-	9,	/* 0x0D  - 36Mbps  */
-	7,	/* 0x0E  - 18Mbps  */
-	5,	/* 0x0F  - 9Mbps   */
-};
-
 static void ath10k_htt_rx_h_rates(struct ath10k *ar,
 				  struct ieee80211_rx_status *status,
 				  struct htt_rx_desc *rxd)
 {
-	enum ieee80211_band band;
-	u8 cck, rate, rate_idx, bw, sgi, mcs, nss;
+	struct ieee80211_supported_band *sband;
+	u8 cck, rate, bw, sgi, mcs, nss;
 	u8 preamble = 0;
 	u32 info1, info2, info3;
 
-	/* Band value can't be set as undefined but freq can be 0 - use that to
-	 * determine whether band is provided.
-	 *
-	 * FIXME: Perhaps this can go away if CCK rate reporting is a little
-	 * reworked?
-	 */
-	if (!status->freq)
-		return;
-
-	band = status->band;
 	info1 = __le32_to_cpu(rxd->ppdu_start.info1);
 	info2 = __le32_to_cpu(rxd->ppdu_start.info2);
 	info3 = __le32_to_cpu(rxd->ppdu_start.info3);
@@ -697,31 +660,18 @@ static void ath10k_htt_rx_h_rates(struct ath10k *ar,
 
 	switch (preamble) {
 	case HTT_RX_LEGACY:
+		/* To get legacy rate index band is required. Since band can't
+		 * be undefined check if freq is non-zero.
+		 */
+		if (!status->freq)
+			return;
+
 		cck = info1 & RX_PPDU_START_INFO1_L_SIG_RATE_SELECT;
 		rate = MS(info1, RX_PPDU_START_INFO1_L_SIG_RATE);
-		rate_idx = 0;
-
-		if (rate < 0x08 || rate > 0x0F)
-			break;
-
-		switch (band) {
-		case IEEE80211_BAND_2GHZ:
-			if (cck)
-				rate &= ~BIT(3);
-			rate_idx = rx_legacy_rate_idx[rate];
-			break;
-		case IEEE80211_BAND_5GHZ:
-			rate_idx = rx_legacy_rate_idx[rate];
-			/* We are using same rate table registering
-			   HW - ath10k_rates[]. In case of 5GHz skip
-			   CCK rates, so -4 here */
-			rate_idx -= 4;
-			break;
-		default:
-			break;
-		}
+		rate &= ~RX_PPDU_START_RATE_FLAG;
 
-		status->rate_idx = rate_idx;
+		sband = &ar->mac.sbands[status->band];
+		status->rate_idx = ath10k_mac_hw_rate_to_idx(sband, rate);
 		break;
 	case HTT_RX_HT:
 	case HTT_RX_HT_WITH_TXBF:
@@ -773,8 +723,87 @@ static void ath10k_htt_rx_h_rates(struct ath10k *ar,
 	}
 }
 
+static struct ieee80211_channel *
+ath10k_htt_rx_h_peer_channel(struct ath10k *ar, struct htt_rx_desc *rxd)
+{
+	struct ath10k_peer *peer;
+	struct ath10k_vif *arvif;
+	struct cfg80211_chan_def def;
+	u16 peer_id;
+
+	lockdep_assert_held(&ar->data_lock);
+
+	if (!rxd)
+		return NULL;
+
+	if (rxd->attention.flags &
+	    __cpu_to_le32(RX_ATTENTION_FLAGS_PEER_IDX_INVALID))
+		return NULL;
+
+	if (!(rxd->msdu_end.info0 &
+	      __cpu_to_le32(RX_MSDU_END_INFO0_FIRST_MSDU)))
+		return NULL;
+
+	peer_id = MS(__le32_to_cpu(rxd->mpdu_start.info0),
+		     RX_MPDU_START_INFO0_PEER_IDX);
+
+	peer = ath10k_peer_find_by_id(ar, peer_id);
+	if (!peer)
+		return NULL;
+
+	arvif = ath10k_get_arvif(ar, peer->vdev_id);
+	if (WARN_ON_ONCE(!arvif))
+		return NULL;
+
+	if (WARN_ON(ath10k_mac_vif_chan(arvif->vif, &def)))
+		return NULL;
+
+	return def.chan;
+}
+
+static struct ieee80211_channel *
+ath10k_htt_rx_h_vdev_channel(struct ath10k *ar, u32 vdev_id)
+{
+	struct ath10k_vif *arvif;
+	struct cfg80211_chan_def def;
+
+	lockdep_assert_held(&ar->data_lock);
+
+	list_for_each_entry(arvif, &ar->arvifs, list) {
+		if (arvif->vdev_id == vdev_id &&
+		    ath10k_mac_vif_chan(arvif->vif, &def) == 0)
+			return def.chan;
+	}
+
+	return NULL;
+}
+
+static void
+ath10k_htt_rx_h_any_chan_iter(struct ieee80211_hw *hw,
+			      struct ieee80211_chanctx_conf *conf,
+			      void *data)
+{
+	struct cfg80211_chan_def *def = data;
+
+	*def = conf->def;
+}
+
+static struct ieee80211_channel *
+ath10k_htt_rx_h_any_channel(struct ath10k *ar)
+{
+	struct cfg80211_chan_def def = {};
+
+	ieee80211_iter_chan_contexts_atomic(ar->hw,
+					    ath10k_htt_rx_h_any_chan_iter,
+					    &def);
+
+	return def.chan;
+}
+
 static bool ath10k_htt_rx_h_channel(struct ath10k *ar,
-				    struct ieee80211_rx_status *status)
+				    struct ieee80211_rx_status *status,
+				    struct htt_rx_desc *rxd,
+				    u32 vdev_id)
 {
 	struct ieee80211_channel *ch;
 
@@ -782,6 +811,12 @@ static bool ath10k_htt_rx_h_channel(struct ath10k *ar,
 	ch = ar->scan_channel;
 	if (!ch)
 		ch = ar->rx_channel;
+	if (!ch)
+		ch = ath10k_htt_rx_h_peer_channel(ar, rxd);
+	if (!ch)
+		ch = ath10k_htt_rx_h_vdev_channel(ar, vdev_id);
+	if (!ch)
+		ch = ath10k_htt_rx_h_any_channel(ar);
 	spin_unlock_bh(&ar->data_lock);
 
 	if (!ch)
@@ -819,7 +854,8 @@ static void ath10k_htt_rx_h_mactime(struct ath10k *ar,
 
 static void ath10k_htt_rx_h_ppdu(struct ath10k *ar,
 				 struct sk_buff_head *amsdu,
-				 struct ieee80211_rx_status *status)
+				 struct ieee80211_rx_status *status,
+				 u32 vdev_id)
 {
 	struct sk_buff *first;
 	struct htt_rx_desc *rxd;
@@ -851,7 +887,7 @@ static void ath10k_htt_rx_h_ppdu(struct ath10k *ar,
 		status->flag |= RX_FLAG_NO_SIGNAL_VAL;
 
 		ath10k_htt_rx_h_signal(ar, status, rxd);
-		ath10k_htt_rx_h_channel(ar, status);
+		ath10k_htt_rx_h_channel(ar, status, rxd, vdev_id);
 		ath10k_htt_rx_h_rates(ar, status, rxd);
 	}
 
@@ -929,10 +965,16 @@ static void ath10k_process_rx(struct ath10k *ar,
 	ieee80211_rx(ar->hw, skb);
 }
 
-static int ath10k_htt_rx_nwifi_hdrlen(struct ieee80211_hdr *hdr)
+static int ath10k_htt_rx_nwifi_hdrlen(struct ath10k *ar,
+				      struct ieee80211_hdr *hdr)
 {
-	/* nwifi header is padded to 4 bytes. this fixes 4addr rx */
-	return round_up(ieee80211_hdrlen(hdr->frame_control), 4);
+	int len = ieee80211_hdrlen(hdr->frame_control);
+
+	if (!test_bit(ATH10K_FW_FEATURE_NO_NWIFI_DECAP_4ADDR_PADDING,
+		      ar->fw_features))
+		len = round_up(len, 4);
+
+	return len;
 }
 
 static void ath10k_htt_rx_h_undecap_raw(struct ath10k *ar,
@@ -1031,7 +1073,7 @@ static void ath10k_htt_rx_h_undecap_nwifi(struct ath10k *ar,
 
 	/* pull decapped header and copy SA & DA */
 	hdr = (struct ieee80211_hdr *)msdu->data;
-	hdr_len = ath10k_htt_rx_nwifi_hdrlen(hdr);
+	hdr_len = ath10k_htt_rx_nwifi_hdrlen(ar, hdr);
 	ether_addr_copy(da, ieee80211_get_DA(hdr));
 	ether_addr_copy(sa, ieee80211_get_SA(hdr));
 	skb_pull(msdu, hdr_len);
@@ -1522,7 +1564,7 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 			break;
 		}
 
-		ath10k_htt_rx_h_ppdu(ar, &amsdu, rx_status);
+		ath10k_htt_rx_h_ppdu(ar, &amsdu, rx_status, 0xffff);
 		ath10k_htt_rx_h_unchain(ar, &amsdu, ret > 0);
 		ath10k_htt_rx_h_filter(ar, &amsdu, rx_status);
 		ath10k_htt_rx_h_mpdu(ar, &amsdu, rx_status);
@@ -1569,7 +1611,7 @@ static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
 		return;
 	}
 
-	ath10k_htt_rx_h_ppdu(ar, &amsdu, rx_status);
+	ath10k_htt_rx_h_ppdu(ar, &amsdu, rx_status, 0xffff);
 	ath10k_htt_rx_h_filter(ar, &amsdu, rx_status);
 	ath10k_htt_rx_h_mpdu(ar, &amsdu, rx_status);
 	ath10k_htt_rx_h_deliver(ar, &amsdu, rx_status);
@@ -1598,6 +1640,7 @@ static void ath10k_htt_rx_frm_tx_compl(struct ath10k *ar,
 		tx_done.no_ack = true;
 		break;
 	case HTT_DATA_TX_STATUS_OK:
+		tx_done.success = true;
 		break;
 	case HTT_DATA_TX_STATUS_DISCARD:
 	case HTT_DATA_TX_STATUS_POSTPONE:
@@ -1796,7 +1839,7 @@ static void ath10k_htt_rx_h_rx_offload(struct ath10k *ar,
 		status->flag |= RX_FLAG_NO_SIGNAL_VAL;
 
 		ath10k_htt_rx_h_rx_offload_prot(status, msdu);
-		ath10k_htt_rx_h_channel(ar, status);
+		ath10k_htt_rx_h_channel(ar, status, NULL, rx->vdev_id);
 		ath10k_process_rx(ar, status, msdu);
 	}
 }
@@ -1869,7 +1912,7 @@ static void ath10k_htt_rx_in_ord_ind(struct ath10k *ar, struct sk_buff *skb)
 			 * better to report something than nothing though. This
 			 * should still give an idea about rx rate to the user.
 			 */
-			ath10k_htt_rx_h_ppdu(ar, &amsdu, status);
+			ath10k_htt_rx_h_ppdu(ar, &amsdu, status, vdev_id);
 			ath10k_htt_rx_h_filter(ar, &amsdu, status);
 			ath10k_htt_rx_h_mpdu(ar, &amsdu, status);
 			ath10k_htt_rx_h_deliver(ar, &amsdu, status);
@@ -1892,6 +1935,7 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct ath10k_htt *htt = &ar->htt;
 	struct htt_resp *resp = (struct htt_resp *)skb->data;
+	enum htt_t2h_msg_type type;
 
 	/* confirm alignment */
 	if (!IS_ALIGNED((unsigned long)skb->data, 4))
@@ -1899,7 +1943,16 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 
 	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt rx, msg_type: 0x%0X\n",
 		   resp->hdr.msg_type);
-	switch (resp->hdr.msg_type) {
+
+	if (resp->hdr.msg_type >= ar->htt.t2h_msg_types_max) {
+		ath10k_dbg(ar, ATH10K_DBG_HTT, "htt rx, unsupported msg_type: 0x%0X\n max: 0x%0X",
+			   resp->hdr.msg_type, ar->htt.t2h_msg_types_max);
+		dev_kfree_skb_any(skb);
+		return;
+	}
+	type = ar->htt.t2h_msg_types[resp->hdr.msg_type];
+
+	switch (type) {
 	case HTT_T2H_MSG_TYPE_VERSION_CONF: {
 		htt->target_version_major = resp->ver_resp.major;
 		htt->target_version_minor = resp->ver_resp.minor;
@@ -1937,6 +1990,7 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 
 		switch (status) {
 		case HTT_MGMT_TX_STATUS_OK:
+			tx_done.success = true;
 			break;
 		case HTT_MGMT_TX_STATUS_RETRY:
 			tx_done.no_ack = true;
@@ -1976,7 +2030,6 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 		break;
 	}
 	case HTT_T2H_MSG_TYPE_TEST:
-		/* FIX THIS */
 		break;
 	case HTT_T2H_MSG_TYPE_STATS_CONF:
 		trace_ath10k_htt_stats(ar, skb->data, skb->len);
@@ -2018,11 +2071,8 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 		return;
 	}
 	case HTT_T2H_MSG_TYPE_TX_CREDIT_UPDATE_IND:
-		/* FIXME: This WMI-TLV event is overlapping with 10.2
-		 * CHAN_CHANGE - both being 0xF. Neither is being used in
-		 * practice so no immediate action is necessary. Nevertheless
-		 * HTT may need an abstraction layer like WMI has one day.
-		 */
+		break;
+	case HTT_T2H_MSG_TYPE_CHAN_CHANGE:
 		break;
 	default:
 		ath10k_warn(ar, "htt event (%d) not handled\n",
diff --git a/drivers/net/wireless/ath/ath10k/htt_tx.c b/drivers/net/wireless/ath/ath10k/htt_tx.c
index cbd2bc9e6202..a60ef7d1d5fc 100644
--- a/drivers/net/wireless/ath/ath10k/htt_tx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_tx.c
@@ -26,7 +26,7 @@ void __ath10k_htt_tx_dec_pending(struct ath10k_htt *htt)
 {
 	htt->num_pending_tx--;
 	if (htt->num_pending_tx == htt->max_num_pending_tx - 1)
-		ieee80211_wake_queues(htt->ar->hw);
+		ath10k_mac_tx_unlock(htt->ar, ATH10K_TX_PAUSE_Q_FULL);
 }
 
 static void ath10k_htt_tx_dec_pending(struct ath10k_htt *htt)
@@ -49,7 +49,7 @@ static int ath10k_htt_tx_inc_pending(struct ath10k_htt *htt)
 
 	htt->num_pending_tx++;
 	if (htt->num_pending_tx == htt->max_num_pending_tx)
-		ieee80211_stop_queues(htt->ar->hw);
+		ath10k_mac_tx_lock(htt->ar, ATH10K_TX_PAUSE_Q_FULL);
 
 exit:
 	spin_unlock_bh(&htt->tx_lock);
@@ -420,9 +420,8 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	int res;
 	u8 flags0 = 0;
 	u16 msdu_id, flags1 = 0;
-	dma_addr_t paddr;
-	u32 frags_paddr;
-	bool use_frags;
+	dma_addr_t paddr = 0;
+	u32 frags_paddr = 0;
 
 	res = ath10k_htt_tx_inc_pending(htt);
 	if (res)
@@ -440,12 +439,6 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	prefetch_len = min(htt->prefetch_len, msdu->len);
 	prefetch_len = roundup(prefetch_len, 4);
 
-	/* Since HTT 3.0 there is no separate mgmt tx command. However in case
-	 * of mgmt tx using TX_FRM there is not tx fragment list. Instead of tx
-	 * fragment list host driver specifies directly frame pointer. */
-	use_frags = htt->target_version_major < 3 ||
-		    !ieee80211_is_mgmt(hdr->frame_control);
-
 	skb_cb->htt.txbuf = dma_pool_alloc(htt->tx_pool, GFP_ATOMIC,
 					   &paddr);
 	if (!skb_cb->htt.txbuf) {
@@ -466,7 +459,12 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	if (res)
 		goto err_free_txbuf;
 
-	if (likely(use_frags)) {
+	switch (skb_cb->txmode) {
+	case ATH10K_HW_TXRX_RAW:
+	case ATH10K_HW_TXRX_NATIVE_WIFI:
+		flags0 |= HTT_DATA_TX_DESC_FLAGS0_MAC_HDR_PRESENT;
+		/* pass through */
+	case ATH10K_HW_TXRX_ETHERNET:
 		frags = skb_cb->htt.txbuf->frags;
 
 		frags[0].paddr = __cpu_to_le32(skb_cb->paddr);
@@ -474,15 +472,17 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 		frags[1].paddr = 0;
 		frags[1].len = 0;
 
-		flags0 |= SM(ATH10K_HW_TXRX_NATIVE_WIFI,
-			     HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
+		flags0 |= SM(skb_cb->txmode, HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
 
 		frags_paddr = skb_cb->htt.txbuf_paddr;
-	} else {
+		break;
+	case ATH10K_HW_TXRX_MGMT:
 		flags0 |= SM(ATH10K_HW_TXRX_MGMT,
 			     HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
+		flags0 |= HTT_DATA_TX_DESC_FLAGS0_MAC_HDR_PRESENT;
 
 		frags_paddr = skb_cb->paddr;
+		break;
 	}
 
 	/* Normally all commands go through HTC which manages tx credits for
@@ -508,11 +508,9 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 			prefetch_len);
 	skb_cb->htt.txbuf->htc_hdr.flags = 0;
 
-	if (!ieee80211_has_protected(hdr->frame_control))
+	if (!skb_cb->is_protected)
 		flags0 |= HTT_DATA_TX_DESC_FLAGS0_NO_ENCRYPT;
 
-	flags0 |= HTT_DATA_TX_DESC_FLAGS0_MAC_HDR_PRESENT;
-
 	flags1 |= SM((u16)vdev_id, HTT_DATA_TX_DESC_FLAGS1_VDEV_ID);
 	flags1 |= SM((u16)tid, HTT_DATA_TX_DESC_FLAGS1_EXT_TID);
 	if (msdu->ip_summed == CHECKSUM_PARTIAL) {
diff --git a/drivers/net/wireless/ath/ath10k/hw.c b/drivers/net/wireless/ath/ath10k/hw.c
index 839a8791fb9e..5997f00afe3b 100644
--- a/drivers/net/wireless/ath/ath10k/hw.c
+++ b/drivers/net/wireless/ath/ath10k/hw.c
@@ -15,6 +15,7 @@
  */
 
 #include <linux/types.h>
+#include "core.h"
 #include "hw.h"
 
 const struct ath10k_hw_regs qca988x_regs = {
@@ -56,3 +57,23 @@ const struct ath10k_hw_regs qca6174_regs = {
 	.soc_chip_id_address			= 0x000f0,
 	.scratch_3_address			= 0x0028,
 };
+
+void ath10k_hw_fill_survey_time(struct ath10k *ar, struct survey_info *survey,
+				u32 cc, u32 rcc, u32 cc_prev, u32 rcc_prev)
+{
+	u32 cc_fix = 0;
+
+	survey->filled |= SURVEY_INFO_TIME |
+			  SURVEY_INFO_TIME_BUSY;
+
+	if (ar->hw_params.has_shifted_cc_wraparound && cc < cc_prev) {
+		cc_fix = 0x7fffffff;
+		survey->filled &= ~SURVEY_INFO_TIME_BUSY;
+	}
+
+	cc -= cc_prev - cc_fix;
+	rcc -= rcc_prev;
+
+	survey->time = CCNT_TO_MSEC(cc);
+	survey->time_busy = CCNT_TO_MSEC(rcc);
+}
diff --git a/drivers/net/wireless/ath/ath10k/hw.h b/drivers/net/wireless/ath/ath10k/hw.h
index 460771fcfe9e..85cca29375fe 100644
--- a/drivers/net/wireless/ath/ath10k/hw.h
+++ b/drivers/net/wireless/ath/ath10k/hw.h
@@ -78,6 +78,9 @@ enum qca6174_chip_id_rev {
 /* added support for ATH10K_FW_IE_WMI_OP_VERSION */
 #define ATH10K_FW_API4_FILE		"firmware-4.bin"
 
+/* HTT id conflict fix for management frames over HTT */
+#define ATH10K_FW_API5_FILE		"firmware-5.bin"
+
 #define ATH10K_FW_UTF_FILE		"utf.bin"
 
 /* includes also the null byte */
@@ -104,6 +107,11 @@ enum ath10k_fw_ie_type {
 	 * FW API 4 and above.
 	 */
 	ATH10K_FW_IE_WMI_OP_VERSION = 5,
+
+	/* HTT "operations" interface version, 32 bit value. Supported from
+	 * FW API 5 and above.
+	 */
+	ATH10K_FW_IE_HTT_OP_VERSION = 6,
 };
 
 enum ath10k_fw_wmi_op_version {
@@ -119,6 +127,20 @@ enum ath10k_fw_wmi_op_version {
 	ATH10K_FW_WMI_OP_VERSION_MAX,
 };
 
+enum ath10k_fw_htt_op_version {
+	ATH10K_FW_HTT_OP_VERSION_UNSET = 0,
+
+	ATH10K_FW_HTT_OP_VERSION_MAIN = 1,
+
+	/* also used in 10.2 and 10.2.4 branches */
+	ATH10K_FW_HTT_OP_VERSION_10_1 = 2,
+
+	ATH10K_FW_HTT_OP_VERSION_TLV = 3,
+
+	/* keep last */
+	ATH10K_FW_HTT_OP_VERSION_MAX,
+};
+
 enum ath10k_hw_rev {
 	ATH10K_HW_QCA988X,
 	ATH10K_HW_QCA6174,
@@ -147,6 +169,9 @@ struct ath10k_hw_regs {
 extern const struct ath10k_hw_regs qca988x_regs;
 extern const struct ath10k_hw_regs qca6174_regs;
 
+void ath10k_hw_fill_survey_time(struct ath10k *ar, struct survey_info *survey,
+				u32 cc, u32 rcc, u32 cc_prev, u32 rcc_prev);
+
 #define QCA_REV_988X(ar) ((ar)->hw_rev == ATH10K_HW_QCA988X)
 #define QCA_REV_6174(ar) ((ar)->hw_rev == ATH10K_HW_QCA6174)
 
@@ -180,6 +205,27 @@ struct ath10k_pktlog_hdr {
 	u8 payload[0];
 } __packed;
 
+enum ath10k_hw_rate_ofdm {
+	ATH10K_HW_RATE_OFDM_48M = 0,
+	ATH10K_HW_RATE_OFDM_24M,
+	ATH10K_HW_RATE_OFDM_12M,
+	ATH10K_HW_RATE_OFDM_6M,
+	ATH10K_HW_RATE_OFDM_54M,
+	ATH10K_HW_RATE_OFDM_36M,
+	ATH10K_HW_RATE_OFDM_18M,
+	ATH10K_HW_RATE_OFDM_9M,
+};
+
+enum ath10k_hw_rate_cck {
+	ATH10K_HW_RATE_CCK_LP_11M = 0,
+	ATH10K_HW_RATE_CCK_LP_5_5M,
+	ATH10K_HW_RATE_CCK_LP_2M,
+	ATH10K_HW_RATE_CCK_LP_1M,
+	ATH10K_HW_RATE_CCK_SP_11M,
+	ATH10K_HW_RATE_CCK_SP_5_5M,
+	ATH10K_HW_RATE_CCK_SP_2M,
+};
+
 /* Target specific defines for MAIN firmware */
 #define TARGET_NUM_VDEVS			8
 #define TARGET_NUM_PEER_AST			2
@@ -223,7 +269,7 @@ struct ath10k_pktlog_hdr {
 #define TARGET_10X_NUM_WDS_ENTRIES		32
 #define TARGET_10X_DMA_BURST_SIZE		0
 #define TARGET_10X_MAC_AGGR_DELIM		0
-#define TARGET_10X_AST_SKID_LIMIT		16
+#define TARGET_10X_AST_SKID_LIMIT		128
 #define TARGET_10X_NUM_STATIONS			128
 #define TARGET_10X_NUM_PEERS			((TARGET_10X_NUM_STATIONS) + \
 						 (TARGET_10X_NUM_VDEVS))
@@ -256,13 +302,13 @@ struct ath10k_pktlog_hdr {
 #define TARGET_10_2_DMA_BURST_SIZE		1
 
 /* Target specific defines for WMI-TLV firmware */
-#define TARGET_TLV_NUM_VDEVS			3
+#define TARGET_TLV_NUM_VDEVS			4
 #define TARGET_TLV_NUM_STATIONS			32
-#define TARGET_TLV_NUM_PEERS			((TARGET_TLV_NUM_STATIONS) + \
-						 (TARGET_TLV_NUM_VDEVS) + \
-						 2)
+#define TARGET_TLV_NUM_PEERS			35
+#define TARGET_TLV_NUM_TDLS_VDEVS		1
 #define TARGET_TLV_NUM_TIDS			((TARGET_TLV_NUM_PEERS) * 2)
 #define TARGET_TLV_NUM_MSDU_DESC		(1024 + 32)
+#define TARGET_TLV_NUM_WOW_PATTERNS		22
 
 /* Number of Copy Engines supported */
 #define CE_COUNT 8
@@ -406,6 +452,9 @@ struct ath10k_pktlog_hdr {
 #define SCRATCH_3_ADDRESS			ar->regs->scratch_3_address
 #define CPU_INTR_ADDRESS			0x0010
 
+/* Cycle counters are running at 88MHz */
+#define CCNT_TO_MSEC(x) ((x) / 88000)
+
 /* Firmware indications to the Host via SCRATCH_3 register. */
 #define FW_INDICATOR_ADDRESS	(SOC_CORE_BASE_ADDRESS + SCRATCH_3_ADDRESS)
 #define FW_IND_EVENT_PENDING			1
diff --git a/drivers/net/wireless/ath/ath10k/mac.c b/drivers/net/wireless/ath/ath10k/mac.c
index 973485bd4121..218b6af63447 100644
--- a/drivers/net/wireless/ath/ath10k/mac.c
+++ b/drivers/net/wireless/ath/ath10k/mac.c
@@ -28,7 +28,131 @@
 #include "txrx.h"
 #include "testmode.h"
 #include "wmi.h"
+#include "wmi-tlv.h"
 #include "wmi-ops.h"
+#include "wow.h"
+
+/*********/
+/* Rates */
+/*********/
+
+static struct ieee80211_rate ath10k_rates[] = {
+	{ .bitrate = 10,
+	  .hw_value = ATH10K_HW_RATE_CCK_LP_1M },
+	{ .bitrate = 20,
+	  .hw_value = ATH10K_HW_RATE_CCK_LP_2M,
+	  .hw_value_short = ATH10K_HW_RATE_CCK_SP_2M,
+	  .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+	{ .bitrate = 55,
+	  .hw_value = ATH10K_HW_RATE_CCK_LP_5_5M,
+	  .hw_value_short = ATH10K_HW_RATE_CCK_SP_5_5M,
+	  .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+	{ .bitrate = 110,
+	  .hw_value = ATH10K_HW_RATE_CCK_LP_11M,
+	  .hw_value_short = ATH10K_HW_RATE_CCK_SP_11M,
+	  .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+
+	{ .bitrate = 60, .hw_value = ATH10K_HW_RATE_OFDM_6M },
+	{ .bitrate = 90, .hw_value = ATH10K_HW_RATE_OFDM_9M },
+	{ .bitrate = 120, .hw_value = ATH10K_HW_RATE_OFDM_12M },
+	{ .bitrate = 180, .hw_value = ATH10K_HW_RATE_OFDM_18M },
+	{ .bitrate = 240, .hw_value = ATH10K_HW_RATE_OFDM_24M },
+	{ .bitrate = 360, .hw_value = ATH10K_HW_RATE_OFDM_36M },
+	{ .bitrate = 480, .hw_value = ATH10K_HW_RATE_OFDM_48M },
+	{ .bitrate = 540, .hw_value = ATH10K_HW_RATE_OFDM_54M },
+};
+
+#define ATH10K_MAC_FIRST_OFDM_RATE_IDX 4
+
+#define ath10k_a_rates (ath10k_rates + ATH10K_MAC_FIRST_OFDM_RATE_IDX)
+#define ath10k_a_rates_size (ARRAY_SIZE(ath10k_rates) - \
+			     ATH10K_MAC_FIRST_OFDM_RATE_IDX)
+#define ath10k_g_rates (ath10k_rates + 0)
+#define ath10k_g_rates_size (ARRAY_SIZE(ath10k_rates))
+
+static bool ath10k_mac_bitrate_is_cck(int bitrate)
+{
+	switch (bitrate) {
+	case 10:
+	case 20:
+	case 55:
+	case 110:
+		return true;
+	}
+
+	return false;
+}
+
+static u8 ath10k_mac_bitrate_to_rate(int bitrate)
+{
+	return DIV_ROUND_UP(bitrate, 5) |
+	       (ath10k_mac_bitrate_is_cck(bitrate) ? BIT(7) : 0);
+}
+
+u8 ath10k_mac_hw_rate_to_idx(const struct ieee80211_supported_band *sband,
+			     u8 hw_rate)
+{
+	const struct ieee80211_rate *rate;
+	int i;
+
+	for (i = 0; i < sband->n_bitrates; i++) {
+		rate = &sband->bitrates[i];
+
+		if (rate->hw_value == hw_rate)
+			return i;
+		else if (rate->flags & IEEE80211_RATE_SHORT_PREAMBLE &&
+			 rate->hw_value_short == hw_rate)
+			return i;
+	}
+
+	return 0;
+}
+
+u8 ath10k_mac_bitrate_to_idx(const struct ieee80211_supported_band *sband,
+			     u32 bitrate)
+{
+	int i;
+
+	for (i = 0; i < sband->n_bitrates; i++)
+		if (sband->bitrates[i].bitrate == bitrate)
+			return i;
+
+	return 0;
+}
+
+static int ath10k_mac_get_max_vht_mcs_map(u16 mcs_map, int nss)
+{
+	switch ((mcs_map >> (2 * nss)) & 0x3) {
+	case IEEE80211_VHT_MCS_SUPPORT_0_7: return BIT(8) - 1;
+	case IEEE80211_VHT_MCS_SUPPORT_0_8: return BIT(9) - 1;
+	case IEEE80211_VHT_MCS_SUPPORT_0_9: return BIT(10) - 1;
+	}
+	return 0;
+}
+
+static u32
+ath10k_mac_max_ht_nss(const u8 ht_mcs_mask[IEEE80211_HT_MCS_MASK_LEN])
+{
+	int nss;
+
+	for (nss = IEEE80211_HT_MCS_MASK_LEN - 1; nss >= 0; nss--)
+		if (ht_mcs_mask[nss])
+			return nss + 1;
+
+	return 1;
+}
+
+static u32
+ath10k_mac_max_vht_nss(const u16 vht_mcs_mask[NL80211_VHT_NSS_MAX])
+{
+	int nss;
+
+	for (nss = NL80211_VHT_NSS_MAX - 1; nss >= 0; nss--)
+		if (vht_mcs_mask[nss])
+			return nss + 1;
+
+	return 1;
+}
 
 /**********/
 /* Crypto */
@@ -37,7 +161,7 @@
 static int ath10k_send_key(struct ath10k_vif *arvif,
 			   struct ieee80211_key_conf *key,
 			   enum set_key_cmd cmd,
-			   const u8 *macaddr, bool def_idx)
+			   const u8 *macaddr, u32 flags)
 {
 	struct ath10k *ar = arvif->ar;
 	struct wmi_vdev_install_key_arg arg = {
@@ -45,16 +169,12 @@ static int ath10k_send_key(struct ath10k_vif *arvif,
 		.key_idx = key->keyidx,
 		.key_len = key->keylen,
 		.key_data = key->key,
+		.key_flags = flags,
 		.macaddr = macaddr,
 	};
 
 	lockdep_assert_held(&arvif->ar->conf_mutex);
 
-	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
-		arg.key_flags = WMI_KEY_PAIRWISE;
-	else
-		arg.key_flags = WMI_KEY_GROUP;
-
 	switch (key->cipher) {
 	case WLAN_CIPHER_SUITE_CCMP:
 		arg.key_cipher = WMI_CIPHER_AES_CCM;
@@ -68,17 +188,10 @@ static int ath10k_send_key(struct ath10k_vif *arvif,
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104:
 		arg.key_cipher = WMI_CIPHER_WEP;
-		/* AP/IBSS mode requires self-key to be groupwise
-		 * Otherwise pairwise key must be set */
-		if (memcmp(macaddr, arvif->vif->addr, ETH_ALEN))
-			arg.key_flags = WMI_KEY_PAIRWISE;
-
-		if (def_idx)
-			arg.key_flags |= WMI_KEY_TX_USAGE;
 		break;
 	case WLAN_CIPHER_SUITE_AES_CMAC:
-		/* this one needs to be done in software */
-		return 1;
+		WARN_ON(1);
+		return -EINVAL;
 	default:
 		ath10k_warn(ar, "cipher %d is not supported\n", key->cipher);
 		return -EOPNOTSUPP;
@@ -95,21 +208,22 @@ static int ath10k_send_key(struct ath10k_vif *arvif,
 static int ath10k_install_key(struct ath10k_vif *arvif,
 			      struct ieee80211_key_conf *key,
 			      enum set_key_cmd cmd,
-			      const u8 *macaddr, bool def_idx)
+			      const u8 *macaddr, u32 flags)
 {
 	struct ath10k *ar = arvif->ar;
 	int ret;
+	unsigned long time_left;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
 	reinit_completion(&ar->install_key_done);
 
-	ret = ath10k_send_key(arvif, key, cmd, macaddr, def_idx);
+	ret = ath10k_send_key(arvif, key, cmd, macaddr, flags);
 	if (ret)
 		return ret;
 
-	ret = wait_for_completion_timeout(&ar->install_key_done, 3*HZ);
-	if (ret == 0)
+	time_left = wait_for_completion_timeout(&ar->install_key_done, 3 * HZ);
+	if (time_left == 0)
 		return -ETIMEDOUT;
 
 	return 0;
@@ -122,7 +236,7 @@ static int ath10k_install_peer_wep_keys(struct ath10k_vif *arvif,
 	struct ath10k_peer *peer;
 	int ret;
 	int i;
-	bool def_idx;
+	u32 flags;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
@@ -136,14 +250,20 @@ static int ath10k_install_peer_wep_keys(struct ath10k_vif *arvif,
 	for (i = 0; i < ARRAY_SIZE(arvif->wep_keys); i++) {
 		if (arvif->wep_keys[i] == NULL)
 			continue;
-		/* set TX_USAGE flag for default key id */
-		if (arvif->def_wep_key_idx == i)
-			def_idx = true;
-		else
-			def_idx = false;
+
+		flags = 0;
+		flags |= WMI_KEY_PAIRWISE;
+
+		ret = ath10k_install_key(arvif, arvif->wep_keys[i], SET_KEY,
+					 addr, flags);
+		if (ret)
+			return ret;
+
+		flags = 0;
+		flags |= WMI_KEY_GROUP;
 
 		ret = ath10k_install_key(arvif, arvif->wep_keys[i], SET_KEY,
-					 addr, def_idx);
+					 addr, flags);
 		if (ret)
 			return ret;
 
@@ -152,6 +272,27 @@ static int ath10k_install_peer_wep_keys(struct ath10k_vif *arvif,
 		spin_unlock_bh(&ar->data_lock);
 	}
 
+	/* In some cases (notably with static WEP IBSS with multiple keys)
+	 * multicast Tx becomes broken. Both pairwise and groupwise keys are
+	 * installed already. Using WMI_KEY_TX_USAGE in different combinations
+	 * didn't seem help. Using def_keyid vdev parameter seems to be
+	 * effective so use that.
+	 *
+	 * FIXME: Revisit. Perhaps this can be done in a less hacky way.
+	 */
+	if (arvif->def_wep_key_idx == -1)
+		return 0;
+
+	ret = ath10k_wmi_vdev_set_param(arvif->ar,
+					arvif->vdev_id,
+					arvif->ar->wmi.vdev_param->def_keyid,
+					arvif->def_wep_key_idx);
+	if (ret) {
+		ath10k_warn(ar, "failed to re-set def wpa key idxon vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
 	return 0;
 }
 
@@ -163,6 +304,7 @@ static int ath10k_clear_peer_keys(struct ath10k_vif *arvif,
 	int first_errno = 0;
 	int ret;
 	int i;
+	u32 flags = 0;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
@@ -179,7 +321,7 @@ static int ath10k_clear_peer_keys(struct ath10k_vif *arvif,
 
 		/* key flags are not required to delete the key */
 		ret = ath10k_install_key(arvif, peer->keys[i],
-					 DISABLE_KEY, addr, false);
+					 DISABLE_KEY, addr, flags);
 		if (ret && first_errno == 0)
 			first_errno = ret;
 
@@ -229,6 +371,7 @@ static int ath10k_clear_vdev_key(struct ath10k_vif *arvif,
 	int first_errno = 0;
 	int ret;
 	int i;
+	u32 flags = 0;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
@@ -254,7 +397,7 @@ static int ath10k_clear_vdev_key(struct ath10k_vif *arvif,
 		if (i == ARRAY_SIZE(peer->keys))
 			break;
 		/* key flags are not required to delete the key */
-		ret = ath10k_install_key(arvif, key, DISABLE_KEY, addr, false);
+		ret = ath10k_install_key(arvif, key, DISABLE_KEY, addr, flags);
 		if (ret && first_errno == 0)
 			first_errno = ret;
 
@@ -266,6 +409,39 @@ static int ath10k_clear_vdev_key(struct ath10k_vif *arvif,
 	return first_errno;
 }
 
+static int ath10k_mac_vif_update_wep_key(struct ath10k_vif *arvif,
+					 struct ieee80211_key_conf *key)
+{
+	struct ath10k *ar = arvif->ar;
+	struct ath10k_peer *peer;
+	int ret;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	list_for_each_entry(peer, &ar->peers, list) {
+		if (!memcmp(peer->addr, arvif->vif->addr, ETH_ALEN))
+			continue;
+
+		if (!memcmp(peer->addr, arvif->bssid, ETH_ALEN))
+			continue;
+
+		if (peer->keys[key->keyidx] == key)
+			continue;
+
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vif vdev %i update key %i needs update\n",
+			   arvif->vdev_id, key->keyidx);
+
+		ret = ath10k_install_peer_wep_keys(arvif, peer->addr);
+		if (ret) {
+			ath10k_warn(ar, "failed to update wep keys on vdev %i for peer %pM: %d\n",
+				    arvif->vdev_id, peer->addr, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
 /*********************/
 /* General utilities */
 /*********************/
@@ -364,7 +540,56 @@ static u8 ath10k_parse_mpdudensity(u8 mpdudensity)
 	}
 }
 
-static int ath10k_peer_create(struct ath10k *ar, u32 vdev_id, const u8 *addr)
+int ath10k_mac_vif_chan(struct ieee80211_vif *vif,
+			struct cfg80211_chan_def *def)
+{
+	struct ieee80211_chanctx_conf *conf;
+
+	rcu_read_lock();
+	conf = rcu_dereference(vif->chanctx_conf);
+	if (!conf) {
+		rcu_read_unlock();
+		return -ENOENT;
+	}
+
+	*def = conf->def;
+	rcu_read_unlock();
+
+	return 0;
+}
+
+static void ath10k_mac_num_chanctxs_iter(struct ieee80211_hw *hw,
+					 struct ieee80211_chanctx_conf *conf,
+					 void *data)
+{
+	int *num = data;
+
+	(*num)++;
+}
+
+static int ath10k_mac_num_chanctxs(struct ath10k *ar)
+{
+	int num = 0;
+
+	ieee80211_iter_chan_contexts_atomic(ar->hw,
+					    ath10k_mac_num_chanctxs_iter,
+					    &num);
+
+	return num;
+}
+
+static void
+ath10k_mac_get_any_chandef_iter(struct ieee80211_hw *hw,
+				struct ieee80211_chanctx_conf *conf,
+				void *data)
+{
+	struct cfg80211_chan_def **def = data;
+
+	*def = &conf->def;
+}
+
+static int ath10k_peer_create(struct ath10k *ar, u32 vdev_id, const u8 *addr,
+			      enum wmi_peer_type peer_type)
 {
 	int ret;
 
@@ -373,7 +598,7 @@ static int ath10k_peer_create(struct ath10k *ar, u32 vdev_id, const u8 *addr)
 	if (ar->num_peers >= ar->max_num_peers)
 		return -ENOBUFS;
 
-	ret = ath10k_wmi_peer_create(ar, vdev_id, addr);
+	ret = ath10k_wmi_peer_create(ar, vdev_id, addr, peer_type);
 	if (ret) {
 		ath10k_warn(ar, "failed to create wmi peer %pM on vdev %i: %i\n",
 			    addr, vdev_id, ret);
@@ -517,6 +742,38 @@ static void ath10k_peer_cleanup_all(struct ath10k *ar)
 	ar->num_stations = 0;
 }
 
+static int ath10k_mac_tdls_peer_update(struct ath10k *ar, u32 vdev_id,
+				       struct ieee80211_sta *sta,
+				       enum wmi_tdls_peer_state state)
+{
+	int ret;
+	struct wmi_tdls_peer_update_cmd_arg arg = {};
+	struct wmi_tdls_peer_capab_arg cap = {};
+	struct wmi_channel_arg chan_arg = {};
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	arg.vdev_id = vdev_id;
+	arg.peer_state = state;
+	ether_addr_copy(arg.addr, sta->addr);
+
+	cap.peer_max_sp = sta->max_sp;
+	cap.peer_uapsd_queues = sta->uapsd_queues;
+
+	if (state == WMI_TDLS_PEER_STATE_CONNECTED &&
+	    !sta->tdls_initiator)
+		cap.is_peer_responder = 1;
+
+	ret = ath10k_wmi_tdls_peer_update(ar, &arg, &cap, &chan_arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to update tdls peer %pM on vdev %i: %i\n",
+			    arg.addr, vdev_id, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 /************************/
 /* Interface management */
 /************************/
@@ -561,16 +818,16 @@ static void ath10k_mac_vif_beacon_cleanup(struct ath10k_vif *arvif)
 
 static inline int ath10k_vdev_setup_sync(struct ath10k *ar)
 {
-	int ret;
+	unsigned long time_left;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
 	if (test_bit(ATH10K_FLAG_CRASH_FLUSH, &ar->dev_flags))
 		return -ESHUTDOWN;
 
-	ret = wait_for_completion_timeout(&ar->vdev_setup_done,
-					  ATH10K_VDEV_SETUP_TIMEOUT_HZ);
-	if (ret == 0)
+	time_left = wait_for_completion_timeout(&ar->vdev_setup_done,
+						ATH10K_VDEV_SETUP_TIMEOUT_HZ);
+	if (time_left == 0)
 		return -ETIMEDOUT;
 
 	return 0;
@@ -578,13 +835,21 @@ static inline int ath10k_vdev_setup_sync(struct ath10k *ar)
 
 static int ath10k_monitor_vdev_start(struct ath10k *ar, int vdev_id)
 {
-	struct cfg80211_chan_def *chandef = &ar->chandef;
+	struct cfg80211_chan_def *chandef = NULL;
 	struct ieee80211_channel *channel = chandef->chan;
 	struct wmi_vdev_start_request_arg arg = {};
 	int ret = 0;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
+	ieee80211_iter_chan_contexts_atomic(ar->hw,
+					    ath10k_mac_get_any_chandef_iter,
+					    &chandef);
+	if (WARN_ON_ONCE(!chandef))
+		return -ENOENT;
+
+	channel = chandef->chan;
+
 	arg.vdev_id = vdev_id;
 	arg.channel.freq = channel->center_freq;
 	arg.channel.band_center_freq1 = chandef->center_freq1;
@@ -766,27 +1031,78 @@ static int ath10k_monitor_stop(struct ath10k *ar)
 	return 0;
 }
 
+static bool ath10k_mac_monitor_vdev_is_needed(struct ath10k *ar)
+{
+	int num_ctx;
+
+	/* At least one chanctx is required to derive a channel to start
+	 * monitor vdev on.
+	 */
+	num_ctx = ath10k_mac_num_chanctxs(ar);
+	if (num_ctx == 0)
+		return false;
+
+	/* If there's already an existing special monitor interface then don't
+	 * bother creating another monitor vdev.
+	 */
+	if (ar->monitor_arvif)
+		return false;
+
+	return ar->monitor ||
+	       test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
+}
+
+static bool ath10k_mac_monitor_vdev_is_allowed(struct ath10k *ar)
+{
+	int num_ctx;
+
+	num_ctx = ath10k_mac_num_chanctxs(ar);
+
+	/* FIXME: Current interface combinations and cfg80211/mac80211 code
+	 * shouldn't allow this but make sure to prevent handling the following
+	 * case anyway since multi-channel DFS hasn't been tested at all.
+	 */
+	if (test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags) && num_ctx > 1)
+		return false;
+
+	return true;
+}
+
 static int ath10k_monitor_recalc(struct ath10k *ar)
 {
-	bool should_start;
+	bool needed;
+	bool allowed;
+	int ret;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	should_start = ar->monitor ||
-		       ar->filter_flags & FIF_PROMISC_IN_BSS ||
-		       test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
+	needed = ath10k_mac_monitor_vdev_is_needed(ar);
+	allowed = ath10k_mac_monitor_vdev_is_allowed(ar);
 
 	ath10k_dbg(ar, ATH10K_DBG_MAC,
-		   "mac monitor recalc started? %d should? %d\n",
-		   ar->monitor_started, should_start);
+		   "mac monitor recalc started? %d needed? %d allowed? %d\n",
+		   ar->monitor_started, needed, allowed);
+
+	if (WARN_ON(needed && !allowed)) {
+		if (ar->monitor_started) {
+			ath10k_dbg(ar, ATH10K_DBG_MAC, "mac monitor stopping disallowed monitor\n");
 
-	if (should_start == ar->monitor_started)
+			ret = ath10k_monitor_stop(ar);
+			if (ret)
+				ath10k_warn(ar, "failed to stop disallowed monitor: %d\n", ret);
+				/* not serious */
+		}
+
+		return -EPERM;
+	}
+
+	if (needed == ar->monitor_started)
 		return 0;
 
-	if (should_start)
+	if (needed)
 		return ath10k_monitor_start(ar);
-
-	return ath10k_monitor_stop(ar);
+	else
+		return ath10k_monitor_stop(ar);
 }
 
 static int ath10k_recalc_rtscts_prot(struct ath10k_vif *arvif)
@@ -798,12 +1114,14 @@ static int ath10k_recalc_rtscts_prot(struct ath10k_vif *arvif)
 
 	vdev_param = ar->wmi.vdev_param->enable_rtscts;
 
-	if (arvif->use_cts_prot || arvif->num_legacy_stations > 0)
-		rts_cts |= SM(WMI_RTSCTS_ENABLED, WMI_RTSCTS_SET);
+	rts_cts |= SM(WMI_RTSCTS_ENABLED, WMI_RTSCTS_SET);
 
 	if (arvif->num_legacy_stations > 0)
 		rts_cts |= SM(WMI_RTSCTS_ACROSS_SW_RETRIES,
 			      WMI_RTSCTS_PROFILE);
+	else
+		rts_cts |= SM(WMI_RTSCTS_FOR_SECOND_RATESERIES,
+			      WMI_RTSCTS_PROFILE);
 
 	return ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 					 rts_cts);
@@ -846,6 +1164,27 @@ static int ath10k_stop_cac(struct ath10k *ar)
 	return 0;
 }
 
+static void ath10k_mac_has_radar_iter(struct ieee80211_hw *hw,
+				      struct ieee80211_chanctx_conf *conf,
+				      void *data)
+{
+	bool *ret = data;
+
+	if (!*ret && conf->radar_enabled)
+		*ret = true;
+}
+
+static bool ath10k_mac_has_radar_enabled(struct ath10k *ar)
+{
+	bool has_radar = false;
+
+	ieee80211_iter_chan_contexts_atomic(ar->hw,
+					    ath10k_mac_has_radar_iter,
+					    &has_radar);
+
+	return has_radar;
+}
+
 static void ath10k_recalc_radar_detection(struct ath10k *ar)
 {
 	int ret;
@@ -854,7 +1193,7 @@ static void ath10k_recalc_radar_detection(struct ath10k *ar)
 
 	ath10k_stop_cac(ar);
 
-	if (!ar->radar_enabled)
+	if (!ath10k_mac_has_radar_enabled(ar))
 		return;
 
 	if (ar->num_started_vdevs > 0)
@@ -872,10 +1211,44 @@ static void ath10k_recalc_radar_detection(struct ath10k *ar)
 	}
 }
 
-static int ath10k_vdev_start_restart(struct ath10k_vif *arvif, bool restart)
+static int ath10k_vdev_stop(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	int ret;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	reinit_completion(&ar->vdev_setup_done);
+
+	ret = ath10k_wmi_vdev_stop(ar, arvif->vdev_id);
+	if (ret) {
+		ath10k_warn(ar, "failed to stop WMI vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	ret = ath10k_vdev_setup_sync(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to syncronise setup for vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	WARN_ON(ar->num_started_vdevs == 0);
+
+	if (ar->num_started_vdevs != 0) {
+		ar->num_started_vdevs--;
+		ath10k_recalc_radar_detection(ar);
+	}
+
+	return ret;
+}
+
+static int ath10k_vdev_start_restart(struct ath10k_vif *arvif,
+				     const struct cfg80211_chan_def *chandef,
+				     bool restart)
 {
 	struct ath10k *ar = arvif->ar;
-	struct cfg80211_chan_def *chandef = &ar->chandef;
 	struct wmi_vdev_start_request_arg arg = {};
 	int ret = 0;
 
@@ -939,47 +1312,16 @@ static int ath10k_vdev_start_restart(struct ath10k_vif *arvif, bool restart)
 	return ret;
 }
 
-static int ath10k_vdev_start(struct ath10k_vif *arvif)
+static int ath10k_vdev_start(struct ath10k_vif *arvif,
+			     const struct cfg80211_chan_def *def)
 {
-	return ath10k_vdev_start_restart(arvif, false);
+	return ath10k_vdev_start_restart(arvif, def, false);
 }
 
-static int ath10k_vdev_restart(struct ath10k_vif *arvif)
-{
-	return ath10k_vdev_start_restart(arvif, true);
-}
-
-static int ath10k_vdev_stop(struct ath10k_vif *arvif)
+static int ath10k_vdev_restart(struct ath10k_vif *arvif,
+			       const struct cfg80211_chan_def *def)
 {
-	struct ath10k *ar = arvif->ar;
-	int ret;
-
-	lockdep_assert_held(&ar->conf_mutex);
-
-	reinit_completion(&ar->vdev_setup_done);
-
-	ret = ath10k_wmi_vdev_stop(ar, arvif->vdev_id);
-	if (ret) {
-		ath10k_warn(ar, "failed to stop WMI vdev %i: %d\n",
-			    arvif->vdev_id, ret);
-		return ret;
-	}
-
-	ret = ath10k_vdev_setup_sync(ar);
-	if (ret) {
-		ath10k_warn(ar, "failed to synchronize setup for vdev %i stop: %d\n",
-			    arvif->vdev_id, ret);
-		return ret;
-	}
-
-	WARN_ON(ar->num_started_vdevs == 0);
-
-	if (ar->num_started_vdevs != 0) {
-		ar->num_started_vdevs--;
-		ath10k_recalc_radar_detection(ar);
-	}
-
-	return ret;
+	return ath10k_vdev_start_restart(arvif, def, true);
 }
 
 static int ath10k_mac_setup_bcn_p2p_ie(struct ath10k_vif *arvif,
@@ -1056,6 +1398,10 @@ static int ath10k_mac_setup_bcn_tmpl(struct ath10k_vif *arvif)
 	if (!test_bit(WMI_SERVICE_BEACON_OFFLOAD, ar->wmi.svc_map))
 		return 0;
 
+	if (arvif->vdev_type != WMI_VDEV_TYPE_AP &&
+	    arvif->vdev_type != WMI_VDEV_TYPE_IBSS)
+		return 0;
+
 	bcn = ieee80211_beacon_get_template(hw, vif, &offs);
 	if (!bcn) {
 		ath10k_warn(ar, "failed to get beacon template from mac80211\n");
@@ -1101,6 +1447,9 @@ static int ath10k_mac_setup_prb_tmpl(struct ath10k_vif *arvif)
 	if (!test_bit(WMI_SERVICE_BEACON_OFFLOAD, ar->wmi.svc_map))
 		return 0;
 
+	if (arvif->vdev_type != WMI_VDEV_TYPE_AP)
+		return 0;
+
 	prb = ieee80211_proberesp_get(hw, vif);
 	if (!prb) {
 		ath10k_warn(ar, "failed to get probe resp template from mac80211\n");
@@ -1119,6 +1468,80 @@ static int ath10k_mac_setup_prb_tmpl(struct ath10k_vif *arvif)
 	return 0;
 }
 
+static int ath10k_mac_vif_fix_hidden_ssid(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	struct cfg80211_chan_def def;
+	int ret;
+
+	/* When originally vdev is started during assign_vif_chanctx() some
+	 * information is missing, notably SSID. Firmware revisions with beacon
+	 * offloading require the SSID to be provided during vdev (re)start to
+	 * handle hidden SSID properly.
+	 *
+	 * Vdev restart must be done after vdev has been both started and
+	 * upped. Otherwise some firmware revisions (at least 10.2) fail to
+	 * deliver vdev restart response event causing timeouts during vdev
+	 * syncing in ath10k.
+	 *
+	 * Note: The vdev down/up and template reinstallation could be skipped
+	 * since only wmi-tlv firmware are known to have beacon offload and
+	 * wmi-tlv doesn't seem to misbehave like 10.2 wrt vdev restart
+	 * response delivery. It's probably more robust to keep it as is.
+	 */
+	if (!test_bit(WMI_SERVICE_BEACON_OFFLOAD, ar->wmi.svc_map))
+		return 0;
+
+	if (WARN_ON(!arvif->is_started))
+		return -EINVAL;
+
+	if (WARN_ON(!arvif->is_up))
+		return -EINVAL;
+
+	if (WARN_ON(ath10k_mac_vif_chan(arvif->vif, &def)))
+		return -EINVAL;
+
+	ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
+	if (ret) {
+		ath10k_warn(ar, "failed to bring down ap vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	/* Vdev down reset beacon & presp templates. Reinstall them. Otherwise
+	 * firmware will crash upon vdev up.
+	 */
+
+	ret = ath10k_mac_setup_bcn_tmpl(arvif);
+	if (ret) {
+		ath10k_warn(ar, "failed to update beacon template: %d\n", ret);
+		return ret;
+	}
+
+	ret = ath10k_mac_setup_prb_tmpl(arvif);
+	if (ret) {
+		ath10k_warn(ar, "failed to update presp template: %d\n", ret);
+		return ret;
+	}
+
+	ret = ath10k_vdev_restart(arvif, &def);
+	if (ret) {
+		ath10k_warn(ar, "failed to restart ap vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	ret = ath10k_wmi_vdev_up(arvif->ar, arvif->vdev_id, arvif->aid,
+				 arvif->bssid);
+	if (ret) {
+		ath10k_warn(ar, "failed to bring up ap vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 static void ath10k_control_beaconing(struct ath10k_vif *arvif,
 				     struct ieee80211_bss_conf *info)
 {
@@ -1128,9 +1551,11 @@ static void ath10k_control_beaconing(struct ath10k_vif *arvif,
 	lockdep_assert_held(&arvif->ar->conf_mutex);
 
 	if (!info->enable_beacon) {
-		ath10k_vdev_stop(arvif);
+		ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
+		if (ret)
+			ath10k_warn(ar, "failed to down vdev_id %i: %d\n",
+				    arvif->vdev_id, ret);
 
-		arvif->is_started = false;
 		arvif->is_up = false;
 
 		spin_lock_bh(&arvif->ar->data_lock);
@@ -1142,10 +1567,6 @@ static void ath10k_control_beaconing(struct ath10k_vif *arvif,
 
 	arvif->tx_seq_no = 0x1000;
 
-	ret = ath10k_vdev_start(arvif);
-	if (ret)
-		return;
-
 	arvif->aid = 0;
 	ether_addr_copy(arvif->bssid, info->bssid);
 
@@ -1154,13 +1575,18 @@ static void ath10k_control_beaconing(struct ath10k_vif *arvif,
 	if (ret) {
 		ath10k_warn(ar, "failed to bring up vdev %d: %i\n",
 			    arvif->vdev_id, ret);
-		ath10k_vdev_stop(arvif);
 		return;
 	}
 
-	arvif->is_started = true;
 	arvif->is_up = true;
 
+	ret = ath10k_mac_vif_fix_hidden_ssid(arvif);
+	if (ret) {
+		ath10k_warn(ar, "failed to fix hidden ssid for vdev %i, expect trouble: %d\n",
+			    arvif->vdev_id, ret);
+		return;
+	}
+
 	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d up\n", arvif->vdev_id);
 }
 
@@ -1175,11 +1601,6 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 	lockdep_assert_held(&arvif->ar->conf_mutex);
 
 	if (!info->ibss_joined) {
-		ret = ath10k_peer_delete(arvif->ar, arvif->vdev_id, self_peer);
-		if (ret)
-			ath10k_warn(ar, "failed to delete IBSS self peer %pM for vdev %d: %d\n",
-				    self_peer, arvif->vdev_id, ret);
-
 		if (is_zero_ether_addr(arvif->bssid))
 			return;
 
@@ -1188,13 +1609,6 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 		return;
 	}
 
-	ret = ath10k_peer_create(arvif->ar, arvif->vdev_id, self_peer);
-	if (ret) {
-		ath10k_warn(ar, "failed to create IBSS self peer %pM for vdev %d: %d\n",
-			    self_peer, arvif->vdev_id, ret);
-		return;
-	}
-
 	vdev_param = arvif->ar->wmi.vdev_param->atim_window;
 	ret = ath10k_wmi_vdev_set_param(arvif->ar, arvif->vdev_id, vdev_param,
 					ATH10K_DEFAULT_ATIM);
@@ -1294,7 +1708,14 @@ static int ath10k_mac_vif_setup_ps(struct ath10k_vif *arvif)
 		enable_ps = false;
 	}
 
-	if (enable_ps) {
+	if (!arvif->is_started) {
+		/* mac80211 can update vif powersave state while disconnected.
+		 * Firmware doesn't behave nicely and consumes more power than
+		 * necessary if PS is disabled on a non-started vdev. Hence
+		 * force-enable PS for non-running vdevs.
+		 */
+		psmode = WMI_STA_PS_MODE_ENABLED;
+	} else if (enable_ps) {
 		psmode = WMI_STA_PS_MODE_ENABLED;
 		param = WMI_STA_PS_PARAM_INACTIVITY_TIME;
 
@@ -1361,6 +1782,123 @@ static int ath10k_mac_vif_disable_keepalive(struct ath10k_vif *arvif)
 	return 0;
 }
 
+static void ath10k_mac_vif_ap_csa_count_down(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	struct ieee80211_vif *vif = arvif->vif;
+	int ret;
+
+	lockdep_assert_held(&arvif->ar->conf_mutex);
+
+	if (WARN_ON(!test_bit(WMI_SERVICE_BEACON_OFFLOAD, ar->wmi.svc_map)))
+		return;
+
+	if (arvif->vdev_type != WMI_VDEV_TYPE_AP)
+		return;
+
+	if (!vif->csa_active)
+		return;
+
+	if (!arvif->is_up)
+		return;
+
+	if (!ieee80211_csa_is_complete(vif)) {
+		ieee80211_csa_update_counter(vif);
+
+		ret = ath10k_mac_setup_bcn_tmpl(arvif);
+		if (ret)
+			ath10k_warn(ar, "failed to update bcn tmpl during csa: %d\n",
+				    ret);
+
+		ret = ath10k_mac_setup_prb_tmpl(arvif);
+		if (ret)
+			ath10k_warn(ar, "failed to update prb tmpl during csa: %d\n",
+				    ret);
+	} else {
+		ieee80211_csa_finish(vif);
+	}
+}
+
+static void ath10k_mac_vif_ap_csa_work(struct work_struct *work)
+{
+	struct ath10k_vif *arvif = container_of(work, struct ath10k_vif,
+						ap_csa_work);
+	struct ath10k *ar = arvif->ar;
+
+	mutex_lock(&ar->conf_mutex);
+	ath10k_mac_vif_ap_csa_count_down(arvif);
+	mutex_unlock(&ar->conf_mutex);
+}
+
+static void ath10k_mac_handle_beacon_iter(void *data, u8 *mac,
+					  struct ieee80211_vif *vif)
+{
+	struct sk_buff *skb = data;
+	struct ieee80211_mgmt *mgmt = (void *)skb->data;
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+
+	if (vif->type != NL80211_IFTYPE_STATION)
+		return;
+
+	if (!ether_addr_equal(mgmt->bssid, vif->bss_conf.bssid))
+		return;
+
+	cancel_delayed_work(&arvif->connection_loss_work);
+}
+
+void ath10k_mac_handle_beacon(struct ath10k *ar, struct sk_buff *skb)
+{
+	ieee80211_iterate_active_interfaces_atomic(ar->hw,
+						   IEEE80211_IFACE_ITER_NORMAL,
+						   ath10k_mac_handle_beacon_iter,
+						   skb);
+}
+
+static void ath10k_mac_handle_beacon_miss_iter(void *data, u8 *mac,
+					       struct ieee80211_vif *vif)
+{
+	u32 *vdev_id = data;
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct ath10k *ar = arvif->ar;
+	struct ieee80211_hw *hw = ar->hw;
+
+	if (arvif->vdev_id != *vdev_id)
+		return;
+
+	if (!arvif->is_up)
+		return;
+
+	ieee80211_beacon_loss(vif);
+
+	/* Firmware doesn't report beacon loss events repeatedly. If AP probe
+	 * (done by mac80211) succeeds but beacons do not resume then it
+	 * doesn't make sense to continue operation. Queue connection loss work
+	 * which can be cancelled when beacon is received.
+	 */
+	ieee80211_queue_delayed_work(hw, &arvif->connection_loss_work,
+				     ATH10K_CONNECTION_LOSS_HZ);
+}
+
+void ath10k_mac_handle_beacon_miss(struct ath10k *ar, u32 vdev_id)
+{
+	ieee80211_iterate_active_interfaces_atomic(ar->hw,
+						   IEEE80211_IFACE_ITER_NORMAL,
+						   ath10k_mac_handle_beacon_miss_iter,
+						   &vdev_id);
+}
+
+static void ath10k_mac_vif_sta_connection_loss_work(struct work_struct *work)
+{
+	struct ath10k_vif *arvif = container_of(work, struct ath10k_vif,
+						connection_loss_work.work);
+	struct ieee80211_vif *vif = arvif->vif;
+
+	if (!arvif->is_up)
+		return;
+
+	ieee80211_connection_loss(vif);
+}
+
 /**********************/
 /* Station management */
 /**********************/
@@ -1388,12 +1926,18 @@ static void ath10k_peer_assoc_h_basic(struct ath10k *ar,
 				      struct wmi_peer_assoc_complete_arg *arg)
 {
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	u32 aid;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
+	if (vif->type == NL80211_IFTYPE_STATION)
+		aid = vif->bss_conf.aid;
+	else
+		aid = sta->aid;
+
 	ether_addr_copy(arg->addr, sta->addr);
 	arg->vdev_id = arvif->vdev_id;
-	arg->peer_aid = sta->aid;
+	arg->peer_aid = aid;
 	arg->peer_flags |= WMI_PEER_AUTH;
 	arg->peer_listen_intval = ath10k_peer_assoc_h_listen_intval(ar, vif);
 	arg->peer_num_spatial_streams = 1;
@@ -1405,15 +1949,18 @@ static void ath10k_peer_assoc_h_crypto(struct ath10k *ar,
 				       struct wmi_peer_assoc_complete_arg *arg)
 {
 	struct ieee80211_bss_conf *info = &vif->bss_conf;
+	struct cfg80211_chan_def def;
 	struct cfg80211_bss *bss;
 	const u8 *rsnie = NULL;
 	const u8 *wpaie = NULL;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	bss = cfg80211_get_bss(ar->hw->wiphy, ar->hw->conf.chandef.chan,
-			       info->bssid, NULL, 0, IEEE80211_BSS_TYPE_ANY,
-			       IEEE80211_PRIVACY_ANY);
+	if (WARN_ON(ath10k_mac_vif_chan(vif, &def)))
+		return;
+
+	bss = cfg80211_get_bss(ar->hw->wiphy, def.chan, info->bssid, NULL, 0,
+			       IEEE80211_BSS_TYPE_ANY, IEEE80211_PRIVACY_ANY);
 	if (bss) {
 		const struct cfg80211_bss_ies *ies;
 
@@ -1443,19 +1990,29 @@ static void ath10k_peer_assoc_h_crypto(struct ath10k *ar,
 }
 
 static void ath10k_peer_assoc_h_rates(struct ath10k *ar,
+				      struct ieee80211_vif *vif,
 				      struct ieee80211_sta *sta,
 				      struct wmi_peer_assoc_complete_arg *arg)
 {
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 	struct wmi_rate_set_arg *rateset = &arg->peer_legacy_rates;
+	struct cfg80211_chan_def def;
 	const struct ieee80211_supported_band *sband;
 	const struct ieee80211_rate *rates;
+	enum ieee80211_band band;
 	u32 ratemask;
+	u8 rate;
 	int i;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	sband = ar->hw->wiphy->bands[ar->hw->conf.chandef.chan->band];
-	ratemask = sta->supp_rates[ar->hw->conf.chandef.chan->band];
+	if (WARN_ON(ath10k_mac_vif_chan(vif, &def)))
+		return;
+
+	band = def.chan->band;
+	sband = ar->hw->wiphy->bands[band];
+	ratemask = sta->supp_rates[band];
+	ratemask &= arvif->bitrate_mask.control[band].legacy;
 	rates = sband->bitrates;
 
 	rateset->num_rates = 0;
@@ -1464,24 +2021,66 @@ static void ath10k_peer_assoc_h_rates(struct ath10k *ar,
 		if (!(ratemask & 1))
 			continue;
 
-		rateset->rates[rateset->num_rates] = rates->hw_value;
+		rate = ath10k_mac_bitrate_to_rate(rates->bitrate);
+		rateset->rates[rateset->num_rates] = rate;
 		rateset->num_rates++;
 	}
 }
 
+static bool
+ath10k_peer_assoc_h_ht_masked(const u8 ht_mcs_mask[IEEE80211_HT_MCS_MASK_LEN])
+{
+	int nss;
+
+	for (nss = 0; nss < IEEE80211_HT_MCS_MASK_LEN; nss++)
+		if (ht_mcs_mask[nss])
+			return false;
+
+	return true;
+}
+
+static bool
+ath10k_peer_assoc_h_vht_masked(const u16 vht_mcs_mask[NL80211_VHT_NSS_MAX])
+{
+	int nss;
+
+	for (nss = 0; nss < NL80211_VHT_NSS_MAX; nss++)
+		if (vht_mcs_mask[nss])
+			return false;
+
+	return true;
+}
+
 static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
+				   struct ieee80211_vif *vif,
 				   struct ieee80211_sta *sta,
 				   struct wmi_peer_assoc_complete_arg *arg)
 {
 	const struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
-	int i, n;
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct cfg80211_chan_def def;
+	enum ieee80211_band band;
+	const u8 *ht_mcs_mask;
+	const u16 *vht_mcs_mask;
+	int i, n, max_nss;
 	u32 stbc;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
+	if (WARN_ON(ath10k_mac_vif_chan(vif, &def)))
+		return;
+
 	if (!ht_cap->ht_supported)
 		return;
 
+	band = def.chan->band;
+	ht_mcs_mask = arvif->bitrate_mask.control[band].ht_mcs;
+	vht_mcs_mask = arvif->bitrate_mask.control[band].vht_mcs;
+
+	if (ath10k_peer_assoc_h_ht_masked(ht_mcs_mask) &&
+	    ath10k_peer_assoc_h_vht_masked(vht_mcs_mask))
+		return;
+
 	arg->peer_flags |= WMI_PEER_HT;
 	arg->peer_max_mpdu = (1 << (IEEE80211_HT_MAX_AMPDU_FACTOR +
 				    ht_cap->ampdu_factor)) - 1;
@@ -1500,11 +2099,13 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 		arg->peer_rate_caps |= WMI_RC_CW40_FLAG;
 	}
 
-	if (ht_cap->cap & IEEE80211_HT_CAP_SGI_20)
-		arg->peer_rate_caps |= WMI_RC_SGI_FLAG;
+	if (arvif->bitrate_mask.control[band].gi != NL80211_TXRATE_FORCE_LGI) {
+		if (ht_cap->cap & IEEE80211_HT_CAP_SGI_20)
+			arg->peer_rate_caps |= WMI_RC_SGI_FLAG;
 
-	if (ht_cap->cap & IEEE80211_HT_CAP_SGI_40)
-		arg->peer_rate_caps |= WMI_RC_SGI_FLAG;
+		if (ht_cap->cap & IEEE80211_HT_CAP_SGI_40)
+			arg->peer_rate_caps |= WMI_RC_SGI_FLAG;
+	}
 
 	if (ht_cap->cap & IEEE80211_HT_CAP_TX_STBC) {
 		arg->peer_rate_caps |= WMI_RC_TX_STBC_FLAG;
@@ -1524,9 +2125,12 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 	else if (ht_cap->mcs.rx_mask[1])
 		arg->peer_rate_caps |= WMI_RC_DS_FLAG;
 
-	for (i = 0, n = 0; i < IEEE80211_HT_MCS_MASK_LEN*8; i++)
-		if (ht_cap->mcs.rx_mask[i/8] & (1 << i%8))
+	for (i = 0, n = 0, max_nss = 0; i < IEEE80211_HT_MCS_MASK_LEN * 8; i++)
+		if ((ht_cap->mcs.rx_mask[i / 8] & BIT(i % 8)) &&
+		    (ht_mcs_mask[i / 8] & BIT(i % 8))) {
+			max_nss = (i / 8) + 1;
 			arg->peer_ht_rates.rates[n++] = i;
+		}
 
 	/*
 	 * This is a workaround for HT-enabled STAs which break the spec
@@ -1543,7 +2147,7 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 			arg->peer_ht_rates.rates[i] = i;
 	} else {
 		arg->peer_ht_rates.num_rates = n;
-		arg->peer_num_spatial_streams = sta->rx_nss;
+		arg->peer_num_spatial_streams = max_nss;
 	}
 
 	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac ht peer %pM mcs cnt %d nss %d\n",
@@ -1619,19 +2223,84 @@ static int ath10k_peer_assoc_qos_ap(struct ath10k *ar,
 	return 0;
 }
 
+static u16
+ath10k_peer_assoc_h_vht_limit(u16 tx_mcs_set,
+			      const u16 vht_mcs_limit[NL80211_VHT_NSS_MAX])
+{
+	int idx_limit;
+	int nss;
+	u16 mcs_map;
+	u16 mcs;
+
+	for (nss = 0; nss < NL80211_VHT_NSS_MAX; nss++) {
+		mcs_map = ath10k_mac_get_max_vht_mcs_map(tx_mcs_set, nss) &
+			  vht_mcs_limit[nss];
+
+		if (mcs_map)
+			idx_limit = fls(mcs_map) - 1;
+		else
+			idx_limit = -1;
+
+		switch (idx_limit) {
+		case 0: /* fall through */
+		case 1: /* fall through */
+		case 2: /* fall through */
+		case 3: /* fall through */
+		case 4: /* fall through */
+		case 5: /* fall through */
+		case 6: /* fall through */
+		default:
+			/* see ath10k_mac_can_set_bitrate_mask() */
+			WARN_ON(1);
+			/* fall through */
+		case -1:
+			mcs = IEEE80211_VHT_MCS_NOT_SUPPORTED;
+			break;
+		case 7:
+			mcs = IEEE80211_VHT_MCS_SUPPORT_0_7;
+			break;
+		case 8:
+			mcs = IEEE80211_VHT_MCS_SUPPORT_0_8;
+			break;
+		case 9:
+			mcs = IEEE80211_VHT_MCS_SUPPORT_0_9;
+			break;
+		}
+
+		tx_mcs_set &= ~(0x3 << (nss * 2));
+		tx_mcs_set |= mcs << (nss * 2);
+	}
+
+	return tx_mcs_set;
+}
+
 static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
+				    struct ieee80211_vif *vif,
 				    struct ieee80211_sta *sta,
 				    struct wmi_peer_assoc_complete_arg *arg)
 {
 	const struct ieee80211_sta_vht_cap *vht_cap = &sta->vht_cap;
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct cfg80211_chan_def def;
+	enum ieee80211_band band;
+	const u16 *vht_mcs_mask;
 	u8 ampdu_factor;
 
+	if (WARN_ON(ath10k_mac_vif_chan(vif, &def)))
+		return;
+
 	if (!vht_cap->vht_supported)
 		return;
 
+	band = def.chan->band;
+	vht_mcs_mask = arvif->bitrate_mask.control[band].vht_mcs;
+
+	if (ath10k_peer_assoc_h_vht_masked(vht_mcs_mask))
+		return;
+
 	arg->peer_flags |= WMI_PEER_VHT;
 
-	if (ar->hw->conf.chandef.chan->band == IEEE80211_BAND_2GHZ)
+	if (def.chan->band == IEEE80211_BAND_2GHZ)
 		arg->peer_flags |= WMI_PEER_VHT_2G;
 
 	arg->peer_vht_caps = vht_cap->cap;
@@ -1657,8 +2326,8 @@ static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
 		__le16_to_cpu(vht_cap->vht_mcs.rx_mcs_map);
 	arg->peer_vht_rates.tx_max_rate =
 		__le16_to_cpu(vht_cap->vht_mcs.tx_highest);
-	arg->peer_vht_rates.tx_mcs_set =
-		__le16_to_cpu(vht_cap->vht_mcs.tx_mcs_map);
+	arg->peer_vht_rates.tx_mcs_set = ath10k_peer_assoc_h_vht_limit(
+		__le16_to_cpu(vht_cap->vht_mcs.tx_mcs_map), vht_mcs_mask);
 
 	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vht peer %pM max_mpdu %d flags 0x%x\n",
 		   sta->addr, arg->peer_max_mpdu, arg->peer_flags);
@@ -1697,10 +2366,10 @@ static void ath10k_peer_assoc_h_qos(struct ath10k *ar,
 		   sta->addr, !!(arg->peer_flags & WMI_PEER_QOS));
 }
 
-static bool ath10k_mac_sta_has_11g_rates(struct ieee80211_sta *sta)
+static bool ath10k_mac_sta_has_ofdm_only(struct ieee80211_sta *sta)
 {
-	/* First 4 rates in ath10k_rates are CCK (11b) rates. */
-	return sta->supp_rates[IEEE80211_BAND_2GHZ] >> 4;
+	return sta->supp_rates[IEEE80211_BAND_2GHZ] >>
+	       ATH10K_MAC_FIRST_OFDM_RATE_IDX;
 }
 
 static void ath10k_peer_assoc_h_phymode(struct ath10k *ar,
@@ -1708,21 +2377,35 @@ static void ath10k_peer_assoc_h_phymode(struct ath10k *ar,
 					struct ieee80211_sta *sta,
 					struct wmi_peer_assoc_complete_arg *arg)
 {
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct cfg80211_chan_def def;
+	enum ieee80211_band band;
+	const u8 *ht_mcs_mask;
+	const u16 *vht_mcs_mask;
 	enum wmi_phy_mode phymode = MODE_UNKNOWN;
 
-	switch (ar->hw->conf.chandef.chan->band) {
+	if (WARN_ON(ath10k_mac_vif_chan(vif, &def)))
+		return;
+
+	band = def.chan->band;
+	ht_mcs_mask = arvif->bitrate_mask.control[band].ht_mcs;
+	vht_mcs_mask = arvif->bitrate_mask.control[band].vht_mcs;
+
+	switch (band) {
 	case IEEE80211_BAND_2GHZ:
-		if (sta->vht_cap.vht_supported) {
+		if (sta->vht_cap.vht_supported &&
+		    !ath10k_peer_assoc_h_vht_masked(vht_mcs_mask)) {
 			if (sta->bandwidth == IEEE80211_STA_RX_BW_40)
 				phymode = MODE_11AC_VHT40;
 			else
 				phymode = MODE_11AC_VHT20;
-		} else if (sta->ht_cap.ht_supported) {
+		} else if (sta->ht_cap.ht_supported &&
+			   !ath10k_peer_assoc_h_ht_masked(ht_mcs_mask)) {
 			if (sta->bandwidth == IEEE80211_STA_RX_BW_40)
 				phymode = MODE_11NG_HT40;
 			else
 				phymode = MODE_11NG_HT20;
-		} else if (ath10k_mac_sta_has_11g_rates(sta)) {
+		} else if (ath10k_mac_sta_has_ofdm_only(sta)) {
 			phymode = MODE_11G;
 		} else {
 			phymode = MODE_11B;
@@ -1733,15 +2416,17 @@ static void ath10k_peer_assoc_h_phymode(struct ath10k *ar,
 		/*
 		 * Check VHT first.
 		 */
-		if (sta->vht_cap.vht_supported) {
+		if (sta->vht_cap.vht_supported &&
+		    !ath10k_peer_assoc_h_vht_masked(vht_mcs_mask)) {
 			if (sta->bandwidth == IEEE80211_STA_RX_BW_80)
 				phymode = MODE_11AC_VHT80;
 			else if (sta->bandwidth == IEEE80211_STA_RX_BW_40)
 				phymode = MODE_11AC_VHT40;
 			else if (sta->bandwidth == IEEE80211_STA_RX_BW_20)
 				phymode = MODE_11AC_VHT20;
-		} else if (sta->ht_cap.ht_supported) {
-			if (sta->bandwidth == IEEE80211_STA_RX_BW_40)
+		} else if (sta->ht_cap.ht_supported &&
+			   !ath10k_peer_assoc_h_ht_masked(ht_mcs_mask)) {
+			if (sta->bandwidth >= IEEE80211_STA_RX_BW_40)
 				phymode = MODE_11NA_HT40;
 			else
 				phymode = MODE_11NA_HT20;
@@ -1772,9 +2457,9 @@ static int ath10k_peer_assoc_prepare(struct ath10k *ar,
 
 	ath10k_peer_assoc_h_basic(ar, vif, sta, arg);
 	ath10k_peer_assoc_h_crypto(ar, vif, arg);
-	ath10k_peer_assoc_h_rates(ar, sta, arg);
-	ath10k_peer_assoc_h_ht(ar, sta, arg);
-	ath10k_peer_assoc_h_vht(ar, sta, arg);
+	ath10k_peer_assoc_h_rates(ar, vif, sta, arg);
+	ath10k_peer_assoc_h_ht(ar, vif, sta, arg);
+	ath10k_peer_assoc_h_vht(ar, vif, sta, arg);
 	ath10k_peer_assoc_h_qos(ar, vif, sta, arg);
 	ath10k_peer_assoc_h_phymode(ar, vif, sta, arg);
 
@@ -1993,6 +2678,8 @@ static void ath10k_bss_disassoc(struct ieee80211_hw *hw,
 	}
 
 	arvif->is_up = false;
+
+	cancel_delayed_work_sync(&arvif->connection_loss_work);
 }
 
 static int ath10k_station_assoc(struct ath10k *ar,
@@ -2013,7 +2700,6 @@ static int ath10k_station_assoc(struct ath10k *ar,
 		return ret;
 	}
 
-	peer_arg.peer_reassoc = reassoc;
 	ret = ath10k_wmi_peer_assoc(ar, &peer_arg);
 	if (ret) {
 		ath10k_warn(ar, "failed to run peer assoc for STA %pM vdev %i: %d\n",
@@ -2274,6 +2960,149 @@ static void ath10k_reg_notifier(struct wiphy *wiphy,
 /* TX handlers */
 /***************/
 
+void ath10k_mac_tx_lock(struct ath10k *ar, int reason)
+{
+	lockdep_assert_held(&ar->htt.tx_lock);
+
+	WARN_ON(reason >= ATH10K_TX_PAUSE_MAX);
+	ar->tx_paused |= BIT(reason);
+	ieee80211_stop_queues(ar->hw);
+}
+
+static void ath10k_mac_tx_unlock_iter(void *data, u8 *mac,
+				      struct ieee80211_vif *vif)
+{
+	struct ath10k *ar = data;
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+
+	if (arvif->tx_paused)
+		return;
+
+	ieee80211_wake_queue(ar->hw, arvif->vdev_id);
+}
+
+void ath10k_mac_tx_unlock(struct ath10k *ar, int reason)
+{
+	lockdep_assert_held(&ar->htt.tx_lock);
+
+	WARN_ON(reason >= ATH10K_TX_PAUSE_MAX);
+	ar->tx_paused &= ~BIT(reason);
+
+	if (ar->tx_paused)
+		return;
+
+	ieee80211_iterate_active_interfaces_atomic(ar->hw,
+						   IEEE80211_IFACE_ITER_RESUME_ALL,
+						   ath10k_mac_tx_unlock_iter,
+						   ar);
+}
+
+void ath10k_mac_vif_tx_lock(struct ath10k_vif *arvif, int reason)
+{
+	struct ath10k *ar = arvif->ar;
+
+	lockdep_assert_held(&ar->htt.tx_lock);
+
+	WARN_ON(reason >= BITS_PER_LONG);
+	arvif->tx_paused |= BIT(reason);
+	ieee80211_stop_queue(ar->hw, arvif->vdev_id);
+}
+
+void ath10k_mac_vif_tx_unlock(struct ath10k_vif *arvif, int reason)
+{
+	struct ath10k *ar = arvif->ar;
+
+	lockdep_assert_held(&ar->htt.tx_lock);
+
+	WARN_ON(reason >= BITS_PER_LONG);
+	arvif->tx_paused &= ~BIT(reason);
+
+	if (ar->tx_paused)
+		return;
+
+	if (arvif->tx_paused)
+		return;
+
+	ieee80211_wake_queue(ar->hw, arvif->vdev_id);
+}
+
+static void ath10k_mac_vif_handle_tx_pause(struct ath10k_vif *arvif,
+					   enum wmi_tlv_tx_pause_id pause_id,
+					   enum wmi_tlv_tx_pause_action action)
+{
+	struct ath10k *ar = arvif->ar;
+
+	lockdep_assert_held(&ar->htt.tx_lock);
+
+	switch (pause_id) {
+	case WMI_TLV_TX_PAUSE_ID_MCC:
+	case WMI_TLV_TX_PAUSE_ID_P2P_CLI_NOA:
+	case WMI_TLV_TX_PAUSE_ID_P2P_GO_PS:
+	case WMI_TLV_TX_PAUSE_ID_AP_PS:
+	case WMI_TLV_TX_PAUSE_ID_IBSS_PS:
+		switch (action) {
+		case WMI_TLV_TX_PAUSE_ACTION_STOP:
+			ath10k_mac_vif_tx_lock(arvif, pause_id);
+			break;
+		case WMI_TLV_TX_PAUSE_ACTION_WAKE:
+			ath10k_mac_vif_tx_unlock(arvif, pause_id);
+			break;
+		default:
+			ath10k_warn(ar, "received unknown tx pause action %d on vdev %i, ignoring\n",
+				    action, arvif->vdev_id);
+			break;
+		}
+		break;
+	case WMI_TLV_TX_PAUSE_ID_AP_PEER_PS:
+	case WMI_TLV_TX_PAUSE_ID_AP_PEER_UAPSD:
+	case WMI_TLV_TX_PAUSE_ID_STA_ADD_BA:
+	case WMI_TLV_TX_PAUSE_ID_HOST:
+	default:
+		/* FIXME: Some pause_ids aren't vdev specific. Instead they
+		 * target peer_id and tid. Implementing these could improve
+		 * traffic scheduling fairness across multiple connected
+		 * stations in AP/IBSS modes.
+		 */
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
+			   "mac ignoring unsupported tx pause vdev %i id %d\n",
+			   arvif->vdev_id, pause_id);
+		break;
+	}
+}
+
+struct ath10k_mac_tx_pause {
+	u32 vdev_id;
+	enum wmi_tlv_tx_pause_id pause_id;
+	enum wmi_tlv_tx_pause_action action;
+};
+
+static void ath10k_mac_handle_tx_pause_iter(void *data, u8 *mac,
+					    struct ieee80211_vif *vif)
+{
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct ath10k_mac_tx_pause *arg = data;
+
+	ath10k_mac_vif_handle_tx_pause(arvif, arg->pause_id, arg->action);
+}
+
+void ath10k_mac_handle_tx_pause(struct ath10k *ar, u32 vdev_id,
+				enum wmi_tlv_tx_pause_id pause_id,
+				enum wmi_tlv_tx_pause_action action)
+{
+	struct ath10k_mac_tx_pause arg = {
+		.vdev_id = vdev_id,
+		.pause_id = pause_id,
+		.action = action,
+	};
+
+	spin_lock_bh(&ar->htt.tx_lock);
+	ieee80211_iterate_active_interfaces_atomic(ar->hw,
+						   IEEE80211_IFACE_ITER_RESUME_ALL,
+						   ath10k_mac_handle_tx_pause_iter,
+						   &arg);
+	spin_unlock_bh(&ar->htt.tx_lock);
+}
+
 static u8 ath10k_tx_h_get_tid(struct ieee80211_hdr *hdr)
 {
 	if (ieee80211_is_mgmt(hdr->frame_control))
@@ -2300,6 +3129,52 @@ static u8 ath10k_tx_h_get_vdev_id(struct ath10k *ar, struct ieee80211_vif *vif)
 	return 0;
 }
 
+static enum ath10k_hw_txrx_mode
+ath10k_tx_h_get_txmode(struct ath10k *ar, struct ieee80211_vif *vif,
+		       struct ieee80211_sta *sta, struct sk_buff *skb)
+{
+	const struct ieee80211_hdr *hdr = (void *)skb->data;
+	__le16 fc = hdr->frame_control;
+
+	if (!vif || vif->type == NL80211_IFTYPE_MONITOR)
+		return ATH10K_HW_TXRX_RAW;
+
+	if (ieee80211_is_mgmt(fc))
+		return ATH10K_HW_TXRX_MGMT;
+
+	/* Workaround:
+	 *
+	 * NullFunc frames are mostly used to ping if a client or AP are still
+	 * reachable and responsive. This implies tx status reports must be
+	 * accurate - otherwise either mac80211 or userspace (e.g. hostapd) can
+	 * come to a conclusion that the other end disappeared and tear down
+	 * BSS connection or it can never disconnect from BSS/client (which is
+	 * the case).
+	 *
+	 * Firmware with HTT older than 3.0 delivers incorrect tx status for
+	 * NullFunc frames to driver. However there's a HTT Mgmt Tx command
+	 * which seems to deliver correct tx reports for NullFunc frames. The
+	 * downside of using it is it ignores client powersave state so it can
+	 * end up disconnecting sleeping clients in AP mode. It should fix STA
+	 * mode though because AP don't sleep.
+	 */
+	if (ar->htt.target_version_major < 3 &&
+	    (ieee80211_is_nullfunc(fc) || ieee80211_is_qos_nullfunc(fc)) &&
+	    !test_bit(ATH10K_FW_FEATURE_HAS_WMI_MGMT_TX, ar->fw_features))
+		return ATH10K_HW_TXRX_MGMT;
+
+	/* Workaround:
+	 *
+	 * Some wmi-tlv firmwares for qca6174 have broken Tx key selection for
+	 * NativeWifi txmode - it selects AP key instead of peer key. It seems
+	 * to work with Ethernet txmode so use it.
+	 */
+	if (ieee80211_is_data_present(fc) && sta && sta->tdls)
+		return ATH10K_HW_TXRX_ETHERNET;
+
+	return ATH10K_HW_TXRX_NATIVE_WIFI;
+}
+
 /* HTT Tx uses Native Wifi tx mode which expects 802.11 frames without QoS
  * Control in the header.
  */
@@ -2317,16 +3192,42 @@ static void ath10k_tx_h_nwifi(struct ieee80211_hw *hw, struct sk_buff *skb)
 		skb->data, (void *)qos_ctl - (void *)skb->data);
 	skb_pull(skb, IEEE80211_QOS_CTL_LEN);
 
-	/* Fw/Hw generates a corrupted QoS Control Field for QoS NullFunc
-	 * frames. Powersave is handled by the fw/hw so QoS NyllFunc frames are
-	 * used only for CQM purposes (e.g. hostapd station keepalive ping) so
-	 * it is safe to downgrade to NullFunc.
+	/* Some firmware revisions don't handle sending QoS NullFunc well.
+	 * These frames are mainly used for CQM purposes so it doesn't really
+	 * matter whether QoS NullFunc or NullFunc are sent.
 	 */
 	hdr = (void *)skb->data;
-	if (ieee80211_is_qos_nullfunc(hdr->frame_control)) {
-		hdr->frame_control &= ~__cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
+	if (ieee80211_is_qos_nullfunc(hdr->frame_control))
 		cb->htt.tid = HTT_DATA_TX_EXT_TID_NON_QOS_MCAST_BCAST;
-	}
+
+	hdr->frame_control &= ~__cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
+}
+
+static void ath10k_tx_h_8023(struct sk_buff *skb)
+{
+	struct ieee80211_hdr *hdr;
+	struct rfc1042_hdr *rfc1042;
+	struct ethhdr *eth;
+	size_t hdrlen;
+	u8 da[ETH_ALEN];
+	u8 sa[ETH_ALEN];
+	__be16 type;
+
+	hdr = (void *)skb->data;
+	hdrlen = ieee80211_hdrlen(hdr->frame_control);
+	rfc1042 = (void *)skb->data + hdrlen;
+
+	ether_addr_copy(da, ieee80211_get_DA(hdr));
+	ether_addr_copy(sa, ieee80211_get_SA(hdr));
+	type = rfc1042->snap_type;
+
+	skb_pull(skb, hdrlen + sizeof(*rfc1042));
+	skb_push(skb, sizeof(*eth));
+
+	eth = (void *)skb->data;
+	ether_addr_copy(eth->h_dest, da);
+	ether_addr_copy(eth->h_source, sa);
+	eth->h_proto = type;
 }
 
 static void ath10k_tx_h_add_p2p_noa_ie(struct ath10k *ar,
@@ -2365,45 +3266,51 @@ static bool ath10k_mac_need_offchan_tx_work(struct ath10k *ar)
 		 ar->htt.target_version_minor >= 4);
 }
 
-static void ath10k_tx_htt(struct ath10k *ar, struct sk_buff *skb)
+static int ath10k_mac_tx_wmi_mgmt(struct ath10k *ar, struct sk_buff *skb)
 {
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct sk_buff_head *q = &ar->wmi_mgmt_tx_queue;
 	int ret = 0;
 
-	if (ar->htt.target_version_major >= 3) {
-		/* Since HTT 3.0 there is no separate mgmt tx command */
-		ret = ath10k_htt_tx(&ar->htt, skb);
-		goto exit;
+	spin_lock_bh(&ar->data_lock);
+
+	if (skb_queue_len(q) == ATH10K_MAX_NUM_MGMT_PENDING) {
+		ath10k_warn(ar, "wmi mgmt tx queue is full\n");
+		ret = -ENOSPC;
+		goto unlock;
 	}
 
-	if (ieee80211_is_mgmt(hdr->frame_control)) {
-		if (test_bit(ATH10K_FW_FEATURE_HAS_WMI_MGMT_TX,
-			     ar->fw_features)) {
-			if (skb_queue_len(&ar->wmi_mgmt_tx_queue) >=
-			    ATH10K_MAX_NUM_MGMT_PENDING) {
-				ath10k_warn(ar, "reached WMI management transmit queue limit\n");
-				ret = -EBUSY;
-				goto exit;
-			}
+	__skb_queue_tail(q, skb);
+	ieee80211_queue_work(ar->hw, &ar->wmi_mgmt_tx_work);
 
-			skb_queue_tail(&ar->wmi_mgmt_tx_queue, skb);
-			ieee80211_queue_work(ar->hw, &ar->wmi_mgmt_tx_work);
-		} else {
-			ret = ath10k_htt_mgmt_tx(&ar->htt, skb);
-		}
-	} else if (!test_bit(ATH10K_FW_FEATURE_HAS_WMI_MGMT_TX,
-			     ar->fw_features) &&
-		   ieee80211_is_nullfunc(hdr->frame_control)) {
-		/* FW does not report tx status properly for NullFunc frames
-		 * unless they are sent through mgmt tx path. mac80211 sends
-		 * those frames when it detects link/beacon loss and depends
-		 * on the tx status to be correct. */
-		ret = ath10k_htt_mgmt_tx(&ar->htt, skb);
-	} else {
-		ret = ath10k_htt_tx(&ar->htt, skb);
+unlock:
+	spin_unlock_bh(&ar->data_lock);
+
+	return ret;
+}
+
+static void ath10k_mac_tx(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct ath10k_skb_cb *cb = ATH10K_SKB_CB(skb);
+	struct ath10k_htt *htt = &ar->htt;
+	int ret = 0;
+
+	switch (cb->txmode) {
+	case ATH10K_HW_TXRX_RAW:
+	case ATH10K_HW_TXRX_NATIVE_WIFI:
+	case ATH10K_HW_TXRX_ETHERNET:
+		ret = ath10k_htt_tx(htt, skb);
+		break;
+	case ATH10K_HW_TXRX_MGMT:
+		if (test_bit(ATH10K_FW_FEATURE_HAS_WMI_MGMT_TX,
+			     ar->fw_features))
+			ret = ath10k_mac_tx_wmi_mgmt(ar, skb);
+		else if (ar->htt.target_version_major >= 3)
+			ret = ath10k_htt_tx(htt, skb);
+		else
+			ret = ath10k_htt_mgmt_tx(htt, skb);
+		break;
 	}
 
-exit:
 	if (ret) {
 		ath10k_warn(ar, "failed to transmit packet, dropping: %d\n",
 			    ret);
@@ -2433,6 +3340,7 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 	const u8 *peer_addr;
 	int vdev_id;
 	int ret;
+	unsigned long time_left;
 
 	/* FW requirement: We must create a peer before FW will send out
 	 * an offchannel frame. Otherwise the frame will be stuck and
@@ -2465,7 +3373,8 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 				   peer_addr, vdev_id);
 
 		if (!peer) {
-			ret = ath10k_peer_create(ar, vdev_id, peer_addr);
+			ret = ath10k_peer_create(ar, vdev_id, peer_addr,
+						 WMI_PEER_TYPE_DEFAULT);
 			if (ret)
 				ath10k_warn(ar, "failed to create peer %pM on vdev %d: %d\n",
 					    peer_addr, vdev_id, ret);
@@ -2476,11 +3385,11 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 		ar->offchan_tx_skb = skb;
 		spin_unlock_bh(&ar->data_lock);
 
-		ath10k_tx_htt(ar, skb);
+		ath10k_mac_tx(ar, skb);
 
-		ret = wait_for_completion_timeout(&ar->offchan_tx_completed,
-						  3 * HZ);
-		if (ret == 0)
+		time_left =
+		wait_for_completion_timeout(&ar->offchan_tx_completed, 3 * HZ);
+		if (time_left == 0)
 			ath10k_warn(ar, "timed out waiting for offchannel skb %p\n",
 				    skb);
 
@@ -2700,21 +3609,38 @@ static void ath10k_tx(struct ieee80211_hw *hw,
 	struct ath10k *ar = hw->priv;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_vif *vif = info->control.vif;
+	struct ieee80211_sta *sta = control->sta;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	__le16 fc = hdr->frame_control;
 
 	/* We should disable CCK RATE due to P2P */
 	if (info->flags & IEEE80211_TX_CTL_NO_CCK_RATE)
 		ath10k_dbg(ar, ATH10K_DBG_MAC, "IEEE80211_TX_CTL_NO_CCK_RATE\n");
 
 	ATH10K_SKB_CB(skb)->htt.is_offchan = false;
+	ATH10K_SKB_CB(skb)->htt.freq = 0;
 	ATH10K_SKB_CB(skb)->htt.tid = ath10k_tx_h_get_tid(hdr);
 	ATH10K_SKB_CB(skb)->vdev_id = ath10k_tx_h_get_vdev_id(ar, vif);
+	ATH10K_SKB_CB(skb)->txmode = ath10k_tx_h_get_txmode(ar, vif, sta, skb);
+	ATH10K_SKB_CB(skb)->is_protected = ieee80211_has_protected(fc);
 
-	/* it makes no sense to process injected frames like that */
-	if (vif && vif->type != NL80211_IFTYPE_MONITOR) {
+	switch (ATH10K_SKB_CB(skb)->txmode) {
+	case ATH10K_HW_TXRX_MGMT:
+	case ATH10K_HW_TXRX_NATIVE_WIFI:
 		ath10k_tx_h_nwifi(hw, skb);
 		ath10k_tx_h_add_p2p_noa_ie(ar, vif, skb);
 		ath10k_tx_h_seq_no(vif, skb);
+		break;
+	case ATH10K_HW_TXRX_ETHERNET:
+		ath10k_tx_h_8023(skb);
+		break;
+	case ATH10K_HW_TXRX_RAW:
+		/* FIXME: Packet injection isn't implemented. It should be
+		 * doable with firmware 10.2 on qca988x.
+		 */
+		WARN_ON_ONCE(1);
+		ieee80211_free_txskb(hw, skb);
+		return;
 	}
 
 	if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN) {
@@ -2736,7 +3662,7 @@ static void ath10k_tx(struct ieee80211_hw *hw,
 		}
 	}
 
-	ath10k_tx_htt(ar, skb);
+	ath10k_mac_tx(ar, skb);
 }
 
 /* Must not be called with conf_mutex held as workers can use that also. */
@@ -2761,11 +3687,13 @@ void ath10k_halt(struct ath10k *ar)
 	clear_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
 	ar->filter_flags = 0;
 	ar->monitor = false;
+	ar->monitor_arvif = NULL;
 
 	if (ar->monitor_started)
 		ath10k_monitor_stop(ar);
 
 	ar->monitor_started = false;
+	ar->tx_paused = 0;
 
 	ath10k_scan_finish(ar);
 	ath10k_peer_cleanup_all(ar);
@@ -2859,6 +3787,7 @@ static int ath10k_set_antenna(struct ieee80211_hw *hw, u32 tx_ant, u32 rx_ant)
 static int ath10k_start(struct ieee80211_hw *hw)
 {
 	struct ath10k *ar = hw->priv;
+	u32 burst_enable;
 	int ret = 0;
 
 	/*
@@ -2913,6 +3842,24 @@ static int ath10k_start(struct ieee80211_hw *hw)
 		goto err_core_stop;
 	}
 
+	if (test_bit(WMI_SERVICE_ADAPTIVE_OCS, ar->wmi.svc_map)) {
+		ret = ath10k_wmi_adaptive_qcs(ar, true);
+		if (ret) {
+			ath10k_warn(ar, "failed to enable adaptive qcs: %d\n",
+				    ret);
+			goto err_core_stop;
+		}
+	}
+
+	if (test_bit(WMI_SERVICE_BURST, ar->wmi.svc_map)) {
+		burst_enable = ar->wmi.pdev_param->burst_enable;
+		ret = ath10k_wmi_pdev_set_param(ar, burst_enable, 0);
+		if (ret) {
+			ath10k_warn(ar, "failed to disable burst: %d\n", ret);
+			goto err_core_stop;
+		}
+	}
+
 	if (ar->cfg_tx_chainmask)
 		__ath10k_set_antenna(ar, ar->cfg_tx_chainmask,
 				     ar->cfg_rx_chainmask);
@@ -2934,10 +3881,21 @@ static int ath10k_start(struct ieee80211_hw *hw)
 		goto err_core_stop;
 	}
 
+	ret = ath10k_wmi_pdev_set_param(ar,
+					ar->wmi.pdev_param->ani_enable, 1);
+	if (ret) {
+		ath10k_warn(ar, "failed to enable ani by default: %d\n",
+			    ret);
+		goto err_core_stop;
+	}
+
+	ar->ani_enabled = true;
+
 	ar->num_started_vdevs = 0;
 	ath10k_regd_update(ar);
 
 	ath10k_spectral_start(ar);
+	ath10k_thermal_set_throttling(ar);
 
 	mutex_unlock(&ar->conf_mutex);
 	return 0;
@@ -2991,97 +3949,6 @@ static int ath10k_config_ps(struct ath10k *ar)
 	return ret;
 }
 
-static const char *chandef_get_width(enum nl80211_chan_width width)
-{
-	switch (width) {
-	case NL80211_CHAN_WIDTH_20_NOHT:
-		return "20 (noht)";
-	case NL80211_CHAN_WIDTH_20:
-		return "20";
-	case NL80211_CHAN_WIDTH_40:
-		return "40";
-	case NL80211_CHAN_WIDTH_80:
-		return "80";
-	case NL80211_CHAN_WIDTH_80P80:
-		return "80+80";
-	case NL80211_CHAN_WIDTH_160:
-		return "160";
-	case NL80211_CHAN_WIDTH_5:
-		return "5";
-	case NL80211_CHAN_WIDTH_10:
-		return "10";
-	}
-	return "?";
-}
-
-static void ath10k_config_chan(struct ath10k *ar)
-{
-	struct ath10k_vif *arvif;
-	int ret;
-
-	lockdep_assert_held(&ar->conf_mutex);
-
-	ath10k_dbg(ar, ATH10K_DBG_MAC,
-		   "mac config channel to %dMHz (cf1 %dMHz cf2 %dMHz width %s)\n",
-		   ar->chandef.chan->center_freq,
-		   ar->chandef.center_freq1,
-		   ar->chandef.center_freq2,
-		   chandef_get_width(ar->chandef.width));
-
-	/* First stop monitor interface. Some FW versions crash if there's a
-	 * lone monitor interface. */
-	if (ar->monitor_started)
-		ath10k_monitor_stop(ar);
-
-	list_for_each_entry(arvif, &ar->arvifs, list) {
-		if (!arvif->is_started)
-			continue;
-
-		if (!arvif->is_up)
-			continue;
-
-		if (arvif->vdev_type == WMI_VDEV_TYPE_MONITOR)
-			continue;
-
-		ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
-		if (ret) {
-			ath10k_warn(ar, "failed to down vdev %d: %d\n",
-				    arvif->vdev_id, ret);
-			continue;
-		}
-	}
-
-	/* all vdevs are downed now - attempt to restart and re-up them */
-
-	list_for_each_entry(arvif, &ar->arvifs, list) {
-		if (!arvif->is_started)
-			continue;
-
-		if (arvif->vdev_type == WMI_VDEV_TYPE_MONITOR)
-			continue;
-
-		ret = ath10k_vdev_restart(arvif);
-		if (ret) {
-			ath10k_warn(ar, "failed to restart vdev %d: %d\n",
-				    arvif->vdev_id, ret);
-			continue;
-		}
-
-		if (!arvif->is_up)
-			continue;
-
-		ret = ath10k_wmi_vdev_up(arvif->ar, arvif->vdev_id, arvif->aid,
-					 arvif->bssid);
-		if (ret) {
-			ath10k_warn(ar, "failed to bring vdev up %d: %d\n",
-				    arvif->vdev_id, ret);
-			continue;
-		}
-	}
-
-	ath10k_monitor_recalc(ar);
-}
-
 static int ath10k_mac_txpower_setup(struct ath10k *ar, int txpower)
 {
 	int ret;
@@ -3147,26 +4014,6 @@ static int ath10k_config(struct ieee80211_hw *hw, u32 changed)
 
 	mutex_lock(&ar->conf_mutex);
 
-	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-		ath10k_dbg(ar, ATH10K_DBG_MAC,
-			   "mac config channel %dMHz flags 0x%x radar %d\n",
-			   conf->chandef.chan->center_freq,
-			   conf->chandef.chan->flags,
-			   conf->radar_enabled);
-
-		spin_lock_bh(&ar->data_lock);
-		ar->rx_channel = conf->chandef.chan;
-		spin_unlock_bh(&ar->data_lock);
-
-		ar->radar_enabled = conf->radar_enabled;
-		ath10k_recalc_radar_detection(ar);
-
-		if (!cfg80211_chandef_identical(&ar->chandef, &conf->chandef)) {
-			ar->chandef = conf->chandef;
-			ath10k_config_chan(ar);
-		}
-	}
-
 	if (changed & IEEE80211_CONF_CHANGE_PS)
 		ath10k_config_ps(ar);
 
@@ -3208,6 +4055,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	int ret = 0;
 	u32 value;
 	int bit;
+	int i;
 	u32 vdev_param;
 
 	vif->driver_flags |= IEEE80211_VIF_SUPPORTS_UAPSD;
@@ -3220,6 +4068,17 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	arvif->vif = vif;
 
 	INIT_LIST_HEAD(&arvif->list);
+	INIT_WORK(&arvif->ap_csa_work, ath10k_mac_vif_ap_csa_work);
+	INIT_DELAYED_WORK(&arvif->connection_loss_work,
+			  ath10k_mac_vif_sta_connection_loss_work);
+
+	for (i = 0; i < ARRAY_SIZE(arvif->bitrate_mask.control); i++) {
+		arvif->bitrate_mask.control[i].legacy = 0xffffffff;
+		memset(arvif->bitrate_mask.control[i].ht_mcs, 0xff,
+		       sizeof(arvif->bitrate_mask.control[i].ht_mcs));
+		memset(arvif->bitrate_mask.control[i].vht_mcs, 0xff,
+		       sizeof(arvif->bitrate_mask.control[i].vht_mcs));
+	}
 
 	if (ar->free_vdev_map == 0) {
 		ath10k_warn(ar, "Free vdev map is empty, no more interfaces allowed.\n");
@@ -3262,6 +4121,15 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		break;
 	}
 
+	/* Using vdev_id as queue number will make it very easy to do per-vif
+	 * tx queue locking. This shouldn't wrap due to interface combinations
+	 * but do a modulo for correctness sake and prevent using offchannel tx
+	 * queues for regular vif tx.
+	 */
+	vif->cab_queue = arvif->vdev_id % (IEEE80211_MAX_QUEUES - 1);
+	for (i = 0; i < ARRAY_SIZE(vif->hw_queue); i++)
+		vif->hw_queue[i] = arvif->vdev_id % (IEEE80211_MAX_QUEUES - 1);
+
 	/* Some firmware revisions don't wait for beacon tx completion before
 	 * sending another SWBA event. This could lead to hardware using old
 	 * (freed) beacon data in some cases, e.g. tx credit starvation
@@ -3343,14 +4211,18 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		}
 	}
 
-	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
-		ret = ath10k_peer_create(ar, arvif->vdev_id, vif->addr);
+	if (arvif->vdev_type == WMI_VDEV_TYPE_AP ||
+	    arvif->vdev_type == WMI_VDEV_TYPE_IBSS) {
+		ret = ath10k_peer_create(ar, arvif->vdev_id, vif->addr,
+					 WMI_PEER_TYPE_DEFAULT);
 		if (ret) {
-			ath10k_warn(ar, "failed to create vdev %i peer for AP: %d\n",
+			ath10k_warn(ar, "failed to create vdev %i peer for AP/IBSS: %d\n",
 				    arvif->vdev_id, ret);
 			goto err_vdev_delete;
 		}
+	}
 
+	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
 		ret = ath10k_mac_set_kickout(arvif);
 		if (ret) {
 			ath10k_warn(ar, "failed to set vdev %i kickout parameters: %d\n",
@@ -3406,11 +4278,21 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		goto err_peer_delete;
 	}
 
+	if (vif->type == NL80211_IFTYPE_MONITOR) {
+		ar->monitor_arvif = arvif;
+		ret = ath10k_monitor_recalc(ar);
+		if (ret) {
+			ath10k_warn(ar, "failed to recalc monitor: %d\n", ret);
+			goto err_peer_delete;
+		}
+	}
+
 	mutex_unlock(&ar->conf_mutex);
 	return 0;
 
 err_peer_delete:
-	if (arvif->vdev_type == WMI_VDEV_TYPE_AP)
+	if (arvif->vdev_type == WMI_VDEV_TYPE_AP ||
+	    arvif->vdev_type == WMI_VDEV_TYPE_IBSS)
 		ath10k_wmi_peer_delete(ar, arvif->vdev_id, vif->addr);
 
 err_vdev_delete:
@@ -3430,6 +4312,14 @@ err:
 	return ret;
 }
 
+static void ath10k_mac_vif_tx_unlock_all(struct ath10k_vif *arvif)
+{
+	int i;
+
+	for (i = 0; i < BITS_PER_LONG; i++)
+		ath10k_mac_vif_tx_unlock(arvif, i);
+}
+
 static void ath10k_remove_interface(struct ieee80211_hw *hw,
 				    struct ieee80211_vif *vif)
 {
@@ -3437,6 +4327,9 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 	int ret;
 
+	cancel_work_sync(&arvif->ap_csa_work);
+	cancel_delayed_work_sync(&arvif->connection_loss_work);
+
 	mutex_lock(&ar->conf_mutex);
 
 	spin_lock_bh(&ar->data_lock);
@@ -3451,11 +4344,12 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 	ar->free_vdev_map |= 1LL << arvif->vdev_id;
 	list_del(&arvif->list);
 
-	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
+	if (arvif->vdev_type == WMI_VDEV_TYPE_AP ||
+	    arvif->vdev_type == WMI_VDEV_TYPE_IBSS) {
 		ret = ath10k_wmi_peer_delete(arvif->ar, arvif->vdev_id,
 					     vif->addr);
 		if (ret)
-			ath10k_warn(ar, "failed to submit AP self-peer removal on vdev %i: %d\n",
+			ath10k_warn(ar, "failed to submit AP/IBSS self-peer removal on vdev %i: %d\n",
 				    arvif->vdev_id, ret);
 
 		kfree(arvif->u.ap.noa_data);
@@ -3472,7 +4366,8 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 	/* Some firmware revisions don't notify host about self-peer removal
 	 * until after associated vdev is deleted.
 	 */
-	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
+	if (arvif->vdev_type == WMI_VDEV_TYPE_AP ||
+	    arvif->vdev_type == WMI_VDEV_TYPE_IBSS) {
 		ret = ath10k_wait_for_peer_deleted(ar, arvif->vdev_id,
 						   vif->addr);
 		if (ret)
@@ -3486,6 +4381,17 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 
 	ath10k_peer_cleanup(ar, arvif->vdev_id);
 
+	if (vif->type == NL80211_IFTYPE_MONITOR) {
+		ar->monitor_arvif = NULL;
+		ret = ath10k_monitor_recalc(ar);
+		if (ret)
+			ath10k_warn(ar, "failed to recalc monitor: %d\n", ret);
+	}
+
+	spin_lock_bh(&ar->htt.tx_lock);
+	ath10k_mac_vif_tx_unlock_all(arvif);
+	spin_unlock_bh(&ar->htt.tx_lock);
+
 	mutex_unlock(&ar->conf_mutex);
 }
 
@@ -3493,8 +4399,7 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
  * FIXME: Has to be verified.
  */
 #define SUPPORTED_FILTERS			\
-	(FIF_PROMISC_IN_BSS |			\
-	FIF_ALLMULTI |				\
+	(FIF_ALLMULTI |				\
 	FIF_CONTROL |				\
 	FIF_PSPOLL |				\
 	FIF_OTHER_BSS |				\
@@ -3615,6 +4520,13 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		if (ret)
 			ath10k_warn(ar, "failed to recalculate rts/cts prot for vdev %d: %d\n",
 				    arvif->vdev_id, ret);
+
+		vdev_param = ar->wmi.vdev_param->protection_mode;
+		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
+						info->use_cts_prot ? 1 : 0);
+		if (ret)
+			ath10k_warn(ar, "failed to set protection mode %d on vdev %i: %d\n",
+					info->use_cts_prot, arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
@@ -3791,10 +4703,14 @@ static void ath10k_set_key_h_def_keyidx(struct ath10k *ar,
 	 * frames with multi-vif APs. This is not required for main firmware
 	 * branch (e.g. 636).
 	 *
-	 * FIXME: This has been tested only in AP. It remains unknown if this
-	 * is required for multi-vif STA interfaces on 10.1 */
+	 * This is also needed for 636 fw for IBSS-RSN to work more reliably.
+	 *
+	 * FIXME: It remains unknown if this is required for multi-vif STA
+	 * interfaces on 10.1.
+	 */
 
-	if (arvif->vdev_type != WMI_VDEV_TYPE_AP)
+	if (arvif->vdev_type != WMI_VDEV_TYPE_AP &&
+	    arvif->vdev_type != WMI_VDEV_TYPE_IBSS)
 		return;
 
 	if (key->cipher == WLAN_CIPHER_SUITE_WEP40)
@@ -3826,8 +4742,14 @@ static int ath10k_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	const u8 *peer_addr;
 	bool is_wep = key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
 		      key->cipher == WLAN_CIPHER_SUITE_WEP104;
-	bool def_idx = false;
 	int ret = 0;
+	int ret2;
+	u32 flags = 0;
+	u32 flags2;
+
+	/* this one needs to be done in software */
+	if (key->cipher == WLAN_CIPHER_SUITE_AES_CMAC)
+		return 1;
 
 	if (key->keyidx > WMI_MAX_KEY_INDEX)
 		return -ENOSPC;
@@ -3843,6 +4765,13 @@ static int ath10k_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	key->hw_key_idx = key->keyidx;
 
+	if (is_wep) {
+		if (cmd == SET_KEY)
+			arvif->wep_keys[key->keyidx] = key;
+		else
+			arvif->wep_keys[key->keyidx] = NULL;
+	}
+
 	/* the peer should not disappear in mid-way (unless FW goes awry) since
 	 * we already hold conf_mutex. we just make sure its there now. */
 	spin_lock_bh(&ar->data_lock);
@@ -3862,30 +4791,61 @@ static int ath10k_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		}
 	}
 
-	if (is_wep) {
-		if (cmd == SET_KEY)
-			arvif->wep_keys[key->keyidx] = key;
-		else
-			arvif->wep_keys[key->keyidx] = NULL;
+	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
+		flags |= WMI_KEY_PAIRWISE;
+	else
+		flags |= WMI_KEY_GROUP;
 
+	if (is_wep) {
 		if (cmd == DISABLE_KEY)
 			ath10k_clear_vdev_key(arvif, key);
-	}
 
-	/* set TX_USAGE flag for all the keys incase of dot1x-WEP. For
-	 * static WEP, do not set this flag for the keys whose key id
-	 * is  greater than default key id.
-	 */
-	if (arvif->def_wep_key_idx == -1)
-		def_idx = true;
+		/* When WEP keys are uploaded it's possible that there are
+		 * stations associated already (e.g. when merging) without any
+		 * keys. Static WEP needs an explicit per-peer key upload.
+		 */
+		if (vif->type == NL80211_IFTYPE_ADHOC &&
+		    cmd == SET_KEY)
+			ath10k_mac_vif_update_wep_key(arvif, key);
+
+		/* 802.1x never sets the def_wep_key_idx so each set_key()
+		 * call changes default tx key.
+		 *
+		 * Static WEP sets def_wep_key_idx via .set_default_unicast_key
+		 * after first set_key().
+		 */
+		if (cmd == SET_KEY && arvif->def_wep_key_idx == -1)
+			flags |= WMI_KEY_TX_USAGE;
+	}
 
-	ret = ath10k_install_key(arvif, key, cmd, peer_addr, def_idx);
+	ret = ath10k_install_key(arvif, key, cmd, peer_addr, flags);
 	if (ret) {
 		ath10k_warn(ar, "failed to install key for vdev %i peer %pM: %d\n",
 			    arvif->vdev_id, peer_addr, ret);
 		goto exit;
 	}
 
+	/* mac80211 sets static WEP keys as groupwise while firmware requires
+	 * them to be installed twice as both pairwise and groupwise.
+	 */
+	if (is_wep && !sta && vif->type == NL80211_IFTYPE_STATION) {
+		flags2 = flags;
+		flags2 &= ~WMI_KEY_GROUP;
+		flags2 |= WMI_KEY_PAIRWISE;
+
+		ret = ath10k_install_key(arvif, key, cmd, peer_addr, flags2);
+		if (ret) {
+			ath10k_warn(ar, "failed to install (ucast) key for vdev %i peer %pM: %d\n",
+				    arvif->vdev_id, peer_addr, ret);
+			ret2 = ath10k_install_key(arvif, key, DISABLE_KEY,
+						  peer_addr, flags);
+			if (ret2)
+				ath10k_warn(ar, "failed to disable (mcast) key for vdev %i peer %pM: %d\n",
+					    arvif->vdev_id, peer_addr, ret2);
+			goto exit;
+		}
+	}
+
 	ath10k_set_key_h_def_keyidx(ar, arvif, cmd, key);
 
 	spin_lock_bh(&ar->data_lock);
@@ -3933,6 +4893,7 @@ static void ath10k_set_default_unicast_key(struct ieee80211_hw *hw,
 	}
 
 	arvif->def_wep_key_idx = keyidx;
+
 unlock:
 	mutex_unlock(&arvif->ar->conf_mutex);
 }
@@ -3943,6 +4904,10 @@ static void ath10k_sta_rc_update_wk(struct work_struct *wk)
 	struct ath10k_vif *arvif;
 	struct ath10k_sta *arsta;
 	struct ieee80211_sta *sta;
+	struct cfg80211_chan_def def;
+	enum ieee80211_band band;
+	const u8 *ht_mcs_mask;
+	const u16 *vht_mcs_mask;
 	u32 changed, bw, nss, smps;
 	int err;
 
@@ -3951,6 +4916,13 @@ static void ath10k_sta_rc_update_wk(struct work_struct *wk)
 	arvif = arsta->arvif;
 	ar = arvif->ar;
 
+	if (WARN_ON(ath10k_mac_vif_chan(arvif->vif, &def)))
+		return;
+
+	band = def.chan->band;
+	ht_mcs_mask = arvif->bitrate_mask.control[band].ht_mcs;
+	vht_mcs_mask = arvif->bitrate_mask.control[band].vht_mcs;
+
 	spin_lock_bh(&ar->data_lock);
 
 	changed = arsta->changed;
@@ -3964,6 +4936,10 @@ static void ath10k_sta_rc_update_wk(struct work_struct *wk)
 
 	mutex_lock(&ar->conf_mutex);
 
+	nss = max_t(u32, 1, nss);
+	nss = min(nss, max(ath10k_mac_max_ht_nss(ht_mcs_mask),
+			   ath10k_mac_max_vht_nss(vht_mcs_mask)));
+
 	if (changed & IEEE80211_RC_BW_CHANGED) {
 		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac update sta %pM peer bw %d\n",
 			   sta->addr, bw);
@@ -4011,14 +4987,14 @@ static void ath10k_sta_rc_update_wk(struct work_struct *wk)
 	mutex_unlock(&ar->conf_mutex);
 }
 
-static int ath10k_mac_inc_num_stations(struct ath10k_vif *arvif)
+static int ath10k_mac_inc_num_stations(struct ath10k_vif *arvif,
+				       struct ieee80211_sta *sta)
 {
 	struct ath10k *ar = arvif->ar;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	if (arvif->vdev_type != WMI_VDEV_TYPE_AP &&
-	    arvif->vdev_type != WMI_VDEV_TYPE_IBSS)
+	if (arvif->vdev_type == WMI_VDEV_TYPE_STA && !sta->tdls)
 		return 0;
 
 	if (ar->num_stations >= ar->max_num_stations)
@@ -4029,19 +5005,72 @@ static int ath10k_mac_inc_num_stations(struct ath10k_vif *arvif)
 	return 0;
 }
 
-static void ath10k_mac_dec_num_stations(struct ath10k_vif *arvif)
+static void ath10k_mac_dec_num_stations(struct ath10k_vif *arvif,
+					struct ieee80211_sta *sta)
 {
 	struct ath10k *ar = arvif->ar;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	if (arvif->vdev_type != WMI_VDEV_TYPE_AP &&
-	    arvif->vdev_type != WMI_VDEV_TYPE_IBSS)
+	if (arvif->vdev_type == WMI_VDEV_TYPE_STA && !sta->tdls)
 		return;
 
 	ar->num_stations--;
 }
 
+struct ath10k_mac_tdls_iter_data {
+	u32 num_tdls_stations;
+	struct ieee80211_vif *curr_vif;
+};
+
+static void ath10k_mac_tdls_vif_stations_count_iter(void *data,
+						    struct ieee80211_sta *sta)
+{
+	struct ath10k_mac_tdls_iter_data *iter_data = data;
+	struct ath10k_sta *arsta = (struct ath10k_sta *)sta->drv_priv;
+	struct ieee80211_vif *sta_vif = arsta->arvif->vif;
+
+	if (sta->tdls && sta_vif == iter_data->curr_vif)
+		iter_data->num_tdls_stations++;
+}
+
+static int ath10k_mac_tdls_vif_stations_count(struct ieee80211_hw *hw,
+					      struct ieee80211_vif *vif)
+{
+	struct ath10k_mac_tdls_iter_data data = {};
+
+	data.curr_vif = vif;
+
+	ieee80211_iterate_stations_atomic(hw,
+					  ath10k_mac_tdls_vif_stations_count_iter,
+					  &data);
+	return data.num_tdls_stations;
+}
+
+static void ath10k_mac_tdls_vifs_count_iter(void *data, u8 *mac,
+					    struct ieee80211_vif *vif)
+{
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	int *num_tdls_vifs = data;
+
+	if (vif->type != NL80211_IFTYPE_STATION)
+		return;
+
+	if (ath10k_mac_tdls_vif_stations_count(arvif->ar->hw, vif) > 0)
+		(*num_tdls_vifs)++;
+}
+
+static int ath10k_mac_tdls_vifs_count(struct ieee80211_hw *hw)
+{
+	int num_tdls_vifs = 0;
+
+	ieee80211_iterate_active_interfaces_atomic(hw,
+						   IEEE80211_IFACE_ITER_NORMAL,
+						   ath10k_mac_tdls_vifs_count_iter,
+						   &num_tdls_vifs);
+	return num_tdls_vifs;
+}
+
 static int ath10k_sta_state(struct ieee80211_hw *hw,
 			    struct ieee80211_vif *vif,
 			    struct ieee80211_sta *sta,
@@ -4072,41 +5101,80 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 		/*
 		 * New station addition.
 		 */
+		enum wmi_peer_type peer_type = WMI_PEER_TYPE_DEFAULT;
+		u32 num_tdls_stations;
+		u32 num_tdls_vifs;
+
 		ath10k_dbg(ar, ATH10K_DBG_MAC,
 			   "mac vdev %d peer create %pM (new sta) sta %d / %d peer %d / %d\n",
 			   arvif->vdev_id, sta->addr,
 			   ar->num_stations + 1, ar->max_num_stations,
 			   ar->num_peers + 1, ar->max_num_peers);
 
-		ret = ath10k_mac_inc_num_stations(arvif);
+		ret = ath10k_mac_inc_num_stations(arvif, sta);
 		if (ret) {
 			ath10k_warn(ar, "refusing to associate station: too many connected already (%d)\n",
 				    ar->max_num_stations);
 			goto exit;
 		}
 
-		ret = ath10k_peer_create(ar, arvif->vdev_id, sta->addr);
+		if (sta->tdls)
+			peer_type = WMI_PEER_TYPE_TDLS;
+
+		ret = ath10k_peer_create(ar, arvif->vdev_id, sta->addr,
+					 peer_type);
 		if (ret) {
 			ath10k_warn(ar, "failed to add peer %pM for vdev %d when adding a new sta: %i\n",
 				    sta->addr, arvif->vdev_id, ret);
-			ath10k_mac_dec_num_stations(arvif);
+			ath10k_mac_dec_num_stations(arvif, sta);
+			goto exit;
+		}
+
+		if (!sta->tdls)
+			goto exit;
+
+		num_tdls_stations = ath10k_mac_tdls_vif_stations_count(hw, vif);
+		num_tdls_vifs = ath10k_mac_tdls_vifs_count(hw);
+
+		if (num_tdls_vifs >= ar->max_num_tdls_vdevs &&
+		    num_tdls_stations == 0) {
+			ath10k_warn(ar, "vdev %i exceeded maximum number of tdls vdevs %i\n",
+				    arvif->vdev_id, ar->max_num_tdls_vdevs);
+			ath10k_peer_delete(ar, arvif->vdev_id, sta->addr);
+			ath10k_mac_dec_num_stations(arvif, sta);
+			ret = -ENOBUFS;
 			goto exit;
 		}
 
-		if (vif->type == NL80211_IFTYPE_STATION) {
-			WARN_ON(arvif->is_started);
+		if (num_tdls_stations == 0) {
+			/* This is the first tdls peer in current vif */
+			enum wmi_tdls_state state = WMI_TDLS_ENABLE_ACTIVE;
 
-			ret = ath10k_vdev_start(arvif);
+			ret = ath10k_wmi_update_fw_tdls_state(ar, arvif->vdev_id,
+							      state);
 			if (ret) {
-				ath10k_warn(ar, "failed to start vdev %i: %d\n",
+				ath10k_warn(ar, "failed to update fw tdls state on vdev %i: %i\n",
 					    arvif->vdev_id, ret);
-				WARN_ON(ath10k_peer_delete(ar, arvif->vdev_id,
-							   sta->addr));
-				ath10k_mac_dec_num_stations(arvif);
+				ath10k_peer_delete(ar, arvif->vdev_id,
+						   sta->addr);
+				ath10k_mac_dec_num_stations(arvif, sta);
 				goto exit;
 			}
+		}
 
-			arvif->is_started = true;
+		ret = ath10k_mac_tdls_peer_update(ar, arvif->vdev_id, sta,
+						  WMI_TDLS_PEER_STATE_PEERING);
+		if (ret) {
+			ath10k_warn(ar,
+				    "failed to update tdls peer %pM for vdev %d when adding a new sta: %i\n",
+				    sta->addr, arvif->vdev_id, ret);
+			ath10k_peer_delete(ar, arvif->vdev_id, sta->addr);
+			ath10k_mac_dec_num_stations(arvif, sta);
+
+			if (num_tdls_stations != 0)
+				goto exit;
+			ath10k_wmi_update_fw_tdls_state(ar, arvif->vdev_id,
+							WMI_TDLS_DISABLE);
 		}
 	} else if ((old_state == IEEE80211_STA_NONE &&
 		    new_state == IEEE80211_STA_NOTEXIST)) {
@@ -4117,23 +5185,26 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 			   "mac vdev %d peer delete %pM (sta gone)\n",
 			   arvif->vdev_id, sta->addr);
 
-		if (vif->type == NL80211_IFTYPE_STATION) {
-			WARN_ON(!arvif->is_started);
-
-			ret = ath10k_vdev_stop(arvif);
-			if (ret)
-				ath10k_warn(ar, "failed to stop vdev %i: %d\n",
-					    arvif->vdev_id, ret);
-
-			arvif->is_started = false;
-		}
-
 		ret = ath10k_peer_delete(ar, arvif->vdev_id, sta->addr);
 		if (ret)
 			ath10k_warn(ar, "failed to delete peer %pM for vdev %d: %i\n",
 				    sta->addr, arvif->vdev_id, ret);
 
-		ath10k_mac_dec_num_stations(arvif);
+		ath10k_mac_dec_num_stations(arvif, sta);
+
+		if (!sta->tdls)
+			goto exit;
+
+		if (ath10k_mac_tdls_vif_stations_count(hw, vif))
+			goto exit;
+
+		/* This was the last tdls peer in current vif */
+		ret = ath10k_wmi_update_fw_tdls_state(ar, arvif->vdev_id,
+						      WMI_TDLS_DISABLE);
+		if (ret) {
+			ath10k_warn(ar, "failed to update fw tdls state on vdev %i: %i\n",
+				    arvif->vdev_id, ret);
+		}
 	} else if (old_state == IEEE80211_STA_AUTH &&
 		   new_state == IEEE80211_STA_ASSOC &&
 		   (vif->type == NL80211_IFTYPE_AP ||
@@ -4149,9 +5220,30 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 			ath10k_warn(ar, "failed to associate station %pM for vdev %i: %i\n",
 				    sta->addr, arvif->vdev_id, ret);
 	} else if (old_state == IEEE80211_STA_ASSOC &&
-		   new_state == IEEE80211_STA_AUTH &&
-		   (vif->type == NL80211_IFTYPE_AP ||
-		    vif->type == NL80211_IFTYPE_ADHOC)) {
+		   new_state == IEEE80211_STA_AUTHORIZED &&
+		   sta->tdls) {
+		/*
+		 * Tdls station authorized.
+		 */
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac tdls sta %pM authorized\n",
+			   sta->addr);
+
+		ret = ath10k_station_assoc(ar, vif, sta, false);
+		if (ret) {
+			ath10k_warn(ar, "failed to associate tdls station %pM for vdev %i: %i\n",
+				    sta->addr, arvif->vdev_id, ret);
+			goto exit;
+		}
+
+		ret = ath10k_mac_tdls_peer_update(ar, arvif->vdev_id, sta,
+						  WMI_TDLS_PEER_STATE_CONNECTED);
+		if (ret)
+			ath10k_warn(ar, "failed to update tdls peer %pM for vdev %i: %i\n",
+				    sta->addr, arvif->vdev_id, ret);
+	} else if (old_state == IEEE80211_STA_ASSOC &&
+		    new_state == IEEE80211_STA_AUTH &&
+		    (vif->type == NL80211_IFTYPE_AP ||
+		     vif->type == NL80211_IFTYPE_ADHOC)) {
 		/*
 		 * Disassociation.
 		 */
@@ -4356,6 +5448,7 @@ static int ath10k_remain_on_channel(struct ieee80211_hw *hw,
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 	struct wmi_start_scan_arg arg;
 	int ret = 0;
+	u32 scan_time_msec;
 
 	mutex_lock(&ar->conf_mutex);
 
@@ -4382,7 +5475,7 @@ static int ath10k_remain_on_channel(struct ieee80211_hw *hw,
 	if (ret)
 		goto exit;
 
-	duration = max(duration, WMI_SCAN_CHAN_MIN_TIME_MSEC);
+	scan_time_msec = ar->hw->wiphy->max_remain_on_channel_duration * 2;
 
 	memset(&arg, 0, sizeof(arg));
 	ath10k_wmi_start_scan_init(ar, &arg);
@@ -4390,11 +5483,12 @@ static int ath10k_remain_on_channel(struct ieee80211_hw *hw,
 	arg.scan_id = ATH10K_SCAN_ID;
 	arg.n_channels = 1;
 	arg.channels[0] = chan->center_freq;
-	arg.dwell_time_active = duration;
-	arg.dwell_time_passive = duration;
-	arg.max_scan_time = 2 * duration;
+	arg.dwell_time_active = scan_time_msec;
+	arg.dwell_time_passive = scan_time_msec;
+	arg.max_scan_time = scan_time_msec;
 	arg.scan_ctrl_flags |= WMI_SCAN_FLAG_PASSIVE;
 	arg.scan_ctrl_flags |= WMI_SCAN_FILTER_PROBE_REQ;
+	arg.burst_duration_ms = duration;
 
 	ret = ath10k_start_scan(ar, &arg);
 	if (ret) {
@@ -4417,6 +5511,9 @@ static int ath10k_remain_on_channel(struct ieee80211_hw *hw,
 		goto exit;
 	}
 
+	ieee80211_queue_delayed_work(ar->hw, &ar->scan.timeout,
+				     msecs_to_jiffies(duration));
+
 	ret = 0;
 exit:
 	mutex_unlock(&ar->conf_mutex);
@@ -4512,70 +5609,6 @@ static int ath10k_tx_last_beacon(struct ieee80211_hw *hw)
 	return 1;
 }
 
-#ifdef CONFIG_PM
-static int ath10k_suspend(struct ieee80211_hw *hw,
-			  struct cfg80211_wowlan *wowlan)
-{
-	struct ath10k *ar = hw->priv;
-	int ret;
-
-	mutex_lock(&ar->conf_mutex);
-
-	ret = ath10k_wait_for_suspend(ar, WMI_PDEV_SUSPEND);
-	if (ret) {
-		if (ret == -ETIMEDOUT)
-			goto resume;
-		ret = 1;
-		goto exit;
-	}
-
-	ret = ath10k_hif_suspend(ar);
-	if (ret) {
-		ath10k_warn(ar, "failed to suspend hif: %d\n", ret);
-		goto resume;
-	}
-
-	ret = 0;
-	goto exit;
-resume:
-	ret = ath10k_wmi_pdev_resume_target(ar);
-	if (ret)
-		ath10k_warn(ar, "failed to resume target: %d\n", ret);
-
-	ret = 1;
-exit:
-	mutex_unlock(&ar->conf_mutex);
-	return ret;
-}
-
-static int ath10k_resume(struct ieee80211_hw *hw)
-{
-	struct ath10k *ar = hw->priv;
-	int ret;
-
-	mutex_lock(&ar->conf_mutex);
-
-	ret = ath10k_hif_resume(ar);
-	if (ret) {
-		ath10k_warn(ar, "failed to resume hif: %d\n", ret);
-		ret = 1;
-		goto exit;
-	}
-
-	ret = ath10k_wmi_pdev_resume_target(ar);
-	if (ret) {
-		ath10k_warn(ar, "failed to resume target: %d\n", ret);
-		ret = 1;
-		goto exit;
-	}
-
-	ret = 0;
-exit:
-	mutex_unlock(&ar->conf_mutex);
-	return ret;
-}
-#endif
-
 static void ath10k_reconfig_complete(struct ieee80211_hw *hw,
 				     enum ieee80211_reconfig_type reconfig_type)
 {
@@ -4635,343 +5668,286 @@ exit:
 	return ret;
 }
 
-/* Helper table for legacy fixed_rate/bitrate_mask */
-static const u8 cck_ofdm_rate[] = {
-	/* CCK */
-	3, /* 1Mbps */
-	2, /* 2Mbps */
-	1, /* 5.5Mbps */
-	0, /* 11Mbps */
-	/* OFDM */
-	3, /* 6Mbps */
-	7, /* 9Mbps */
-	2, /* 12Mbps */
-	6, /* 18Mbps */
-	1, /* 24Mbps */
-	5, /* 36Mbps */
-	0, /* 48Mbps */
-	4, /* 54Mbps */
-};
-
-/* Check if only one bit set */
-static int ath10k_check_single_mask(u32 mask)
-{
-	int bit;
-
-	bit = ffs(mask);
-	if (!bit)
-		return 0;
-
-	mask &= ~BIT(bit - 1);
-	if (mask)
-		return 2;
-
-	return 1;
-}
-
 static bool
-ath10k_default_bitrate_mask(struct ath10k *ar,
-			    enum ieee80211_band band,
-			    const struct cfg80211_bitrate_mask *mask)
+ath10k_mac_bitrate_mask_has_single_rate(struct ath10k *ar,
+					enum ieee80211_band band,
+					const struct cfg80211_bitrate_mask *mask)
 {
-	u32 legacy = 0x00ff;
-	u8 ht = 0xff, i;
-	u16 vht = 0x3ff;
-	u16 nrf = ar->num_rf_chains;
-
-	if (ar->cfg_tx_chainmask)
-		nrf = get_nss_from_chainmask(ar->cfg_tx_chainmask);
-
-	switch (band) {
-	case IEEE80211_BAND_2GHZ:
-		legacy = 0x00fff;
-		vht = 0;
-		break;
-	case IEEE80211_BAND_5GHZ:
-		break;
-	default:
-		return false;
-	}
+	int num_rates = 0;
+	int i;
 
-	if (mask->control[band].legacy != legacy)
-		return false;
+	num_rates += hweight32(mask->control[band].legacy);
 
-	for (i = 0; i < nrf; i++)
-		if (mask->control[band].ht_mcs[i] != ht)
-			return false;
+	for (i = 0; i < ARRAY_SIZE(mask->control[band].ht_mcs); i++)
+		num_rates += hweight8(mask->control[band].ht_mcs[i]);
 
-	for (i = 0; i < nrf; i++)
-		if (mask->control[band].vht_mcs[i] != vht)
-			return false;
+	for (i = 0; i < ARRAY_SIZE(mask->control[band].vht_mcs); i++)
+		num_rates += hweight16(mask->control[band].vht_mcs[i]);
 
-	return true;
+	return num_rates == 1;
 }
 
 static bool
-ath10k_bitrate_mask_nss(const struct cfg80211_bitrate_mask *mask,
-			enum ieee80211_band band,
-			u8 *fixed_nss)
-{
-	int ht_nss = 0, vht_nss = 0, i;
+ath10k_mac_bitrate_mask_get_single_nss(struct ath10k *ar,
+				       enum ieee80211_band band,
+				       const struct cfg80211_bitrate_mask *mask,
+				       int *nss)
+{
+	struct ieee80211_supported_band *sband = &ar->mac.sbands[band];
+	u16 vht_mcs_map = le16_to_cpu(sband->vht_cap.vht_mcs.tx_mcs_map);
+	u8 ht_nss_mask = 0;
+	u8 vht_nss_mask = 0;
+	int i;
 
-	/* check legacy */
-	if (ath10k_check_single_mask(mask->control[band].legacy))
+	if (mask->control[band].legacy)
 		return false;
 
-	/* check HT */
-	for (i = 0; i < IEEE80211_HT_MCS_MASK_LEN; i++) {
-		if (mask->control[band].ht_mcs[i] == 0xff)
+	for (i = 0; i < ARRAY_SIZE(mask->control[band].ht_mcs); i++) {
+		if (mask->control[band].ht_mcs[i] == 0)
 			continue;
-		else if (mask->control[band].ht_mcs[i] == 0x00)
-			break;
-
-		return false;
+		else if (mask->control[band].ht_mcs[i] ==
+			 sband->ht_cap.mcs.rx_mask[i])
+			ht_nss_mask |= BIT(i);
+		else
+			return false;
 	}
 
-	ht_nss = i;
-
-	/* check VHT */
-	for (i = 0; i < NL80211_VHT_NSS_MAX; i++) {
-		if (mask->control[band].vht_mcs[i] == 0x03ff)
+	for (i = 0; i < ARRAY_SIZE(mask->control[band].vht_mcs); i++) {
+		if (mask->control[band].vht_mcs[i] == 0)
 			continue;
-		else if (mask->control[band].vht_mcs[i] == 0x0000)
-			break;
-
-		return false;
+		else if (mask->control[band].vht_mcs[i] ==
+			 ath10k_mac_get_max_vht_mcs_map(vht_mcs_map, i))
+			vht_nss_mask |= BIT(i);
+		else
+			return false;
 	}
 
-	vht_nss = i;
-
-	if (ht_nss > 0 && vht_nss > 0)
-		return false;
-
-	if (ht_nss)
-		*fixed_nss = ht_nss;
-	else if (vht_nss)
-		*fixed_nss = vht_nss;
-	else
-		return false;
-
-	return true;
-}
-
-static bool
-ath10k_bitrate_mask_correct(const struct cfg80211_bitrate_mask *mask,
-			    enum ieee80211_band band,
-			    enum wmi_rate_preamble *preamble)
-{
-	int legacy = 0, ht = 0, vht = 0, i;
-
-	*preamble = WMI_RATE_PREAMBLE_OFDM;
-
-	/* check legacy */
-	legacy = ath10k_check_single_mask(mask->control[band].legacy);
-	if (legacy > 1)
-		return false;
-
-	/* check HT */
-	for (i = 0; i < IEEE80211_HT_MCS_MASK_LEN; i++)
-		ht += ath10k_check_single_mask(mask->control[band].ht_mcs[i]);
-	if (ht > 1)
+	if (ht_nss_mask != vht_nss_mask)
 		return false;
 
-	/* check VHT */
-	for (i = 0; i < NL80211_VHT_NSS_MAX; i++)
-		vht += ath10k_check_single_mask(mask->control[band].vht_mcs[i]);
-	if (vht > 1)
+	if (ht_nss_mask == 0)
 		return false;
 
-	/* Currently we support only one fixed_rate */
-	if ((legacy + ht + vht) != 1)
+	if (BIT(fls(ht_nss_mask)) - 1 != ht_nss_mask)
 		return false;
 
-	if (ht)
-		*preamble = WMI_RATE_PREAMBLE_HT;
-	else if (vht)
-		*preamble = WMI_RATE_PREAMBLE_VHT;
+	*nss = fls(ht_nss_mask);
 
 	return true;
 }
 
-static bool
-ath10k_bitrate_mask_rate(struct ath10k *ar,
-			 const struct cfg80211_bitrate_mask *mask,
-			 enum ieee80211_band band,
-			 u8 *fixed_rate,
-			 u8 *fixed_nss)
+static int
+ath10k_mac_bitrate_mask_get_single_rate(struct ath10k *ar,
+					enum ieee80211_band band,
+					const struct cfg80211_bitrate_mask *mask,
+					u8 *rate, u8 *nss)
 {
-	u8 rate = 0, pream = 0, nss = 0, i;
-	enum wmi_rate_preamble preamble;
-
-	/* Check if single rate correct */
-	if (!ath10k_bitrate_mask_correct(mask, band, &preamble))
-		return false;
-
-	pream = preamble;
-
-	switch (preamble) {
-	case WMI_RATE_PREAMBLE_CCK:
-	case WMI_RATE_PREAMBLE_OFDM:
-		i = ffs(mask->control[band].legacy) - 1;
-
-		if (band == IEEE80211_BAND_2GHZ && i < 4)
-			pream = WMI_RATE_PREAMBLE_CCK;
-
-		if (band == IEEE80211_BAND_5GHZ)
-			i += 4;
-
-		if (i >= ARRAY_SIZE(cck_ofdm_rate))
-			return false;
+	struct ieee80211_supported_band *sband = &ar->mac.sbands[band];
+	int rate_idx;
+	int i;
+	u16 bitrate;
+	u8 preamble;
+	u8 hw_rate;
 
-		rate = cck_ofdm_rate[i];
-		break;
-	case WMI_RATE_PREAMBLE_HT:
-		for (i = 0; i < IEEE80211_HT_MCS_MASK_LEN; i++)
-			if (mask->control[band].ht_mcs[i])
-				break;
+	if (hweight32(mask->control[band].legacy) == 1) {
+		rate_idx = ffs(mask->control[band].legacy) - 1;
 
-		if (i == IEEE80211_HT_MCS_MASK_LEN)
-			return false;
+		hw_rate = sband->bitrates[rate_idx].hw_value;
+		bitrate = sband->bitrates[rate_idx].bitrate;
 
-		rate = ffs(mask->control[band].ht_mcs[i]) - 1;
-		nss = i;
-		break;
-	case WMI_RATE_PREAMBLE_VHT:
-		for (i = 0; i < NL80211_VHT_NSS_MAX; i++)
-			if (mask->control[band].vht_mcs[i])
-				break;
+		if (ath10k_mac_bitrate_is_cck(bitrate))
+			preamble = WMI_RATE_PREAMBLE_CCK;
+		else
+			preamble = WMI_RATE_PREAMBLE_OFDM;
 
-		if (i == NL80211_VHT_NSS_MAX)
-			return false;
+		*nss = 1;
+		*rate = preamble << 6 |
+			(*nss - 1) << 4 |
+			hw_rate << 0;
 
-		rate = ffs(mask->control[band].vht_mcs[i]) - 1;
-		nss = i;
-		break;
+		return 0;
 	}
 
-	*fixed_nss = nss + 1;
-	nss <<= 4;
-	pream <<= 6;
+	for (i = 0; i < ARRAY_SIZE(mask->control[band].ht_mcs); i++) {
+		if (hweight8(mask->control[band].ht_mcs[i]) == 1) {
+			*nss = i + 1;
+			*rate = WMI_RATE_PREAMBLE_HT << 6 |
+				(*nss - 1) << 4 |
+				(ffs(mask->control[band].ht_mcs[i]) - 1);
 
-	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac fixed rate pream 0x%02x nss 0x%02x rate 0x%02x\n",
-		   pream, nss, rate);
+			return 0;
+		}
+	}
 
-	*fixed_rate = pream | nss | rate;
+	for (i = 0; i < ARRAY_SIZE(mask->control[band].vht_mcs); i++) {
+		if (hweight16(mask->control[band].vht_mcs[i]) == 1) {
+			*nss = i + 1;
+			*rate = WMI_RATE_PREAMBLE_VHT << 6 |
+				(*nss - 1) << 4 |
+				(ffs(mask->control[band].vht_mcs[i]) - 1);
 
-	return true;
-}
-
-static bool ath10k_get_fixed_rate_nss(struct ath10k *ar,
-				      const struct cfg80211_bitrate_mask *mask,
-				      enum ieee80211_band band,
-				      u8 *fixed_rate,
-				      u8 *fixed_nss)
-{
-	/* First check full NSS mask, if we can simply limit NSS */
-	if (ath10k_bitrate_mask_nss(mask, band, fixed_nss))
-		return true;
+			return 0;
+		}
+	}
 
-	/* Next Check single rate is set */
-	return ath10k_bitrate_mask_rate(ar, mask, band, fixed_rate, fixed_nss);
+	return -EINVAL;
 }
 
-static int ath10k_set_fixed_rate_param(struct ath10k_vif *arvif,
-				       u8 fixed_rate,
-				       u8 fixed_nss,
-				       u8 force_sgi)
+static int ath10k_mac_set_fixed_rate_params(struct ath10k_vif *arvif,
+					    u8 rate, u8 nss, u8 sgi)
 {
 	struct ath10k *ar = arvif->ar;
 	u32 vdev_param;
-	int ret = 0;
-
-	mutex_lock(&ar->conf_mutex);
-
-	if (arvif->fixed_rate == fixed_rate &&
-	    arvif->fixed_nss == fixed_nss &&
-	    arvif->force_sgi == force_sgi)
-		goto exit;
+	int ret;
 
-	if (fixed_rate == WMI_FIXED_RATE_NONE)
-		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac disable fixed bitrate mask\n");
+	lockdep_assert_held(&ar->conf_mutex);
 
-	if (force_sgi)
-		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac force sgi\n");
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac set fixed rate params vdev %i rate 0x%02hhx nss %hhu sgi %hhu\n",
+		   arvif->vdev_id, rate, nss, sgi);
 
 	vdev_param = ar->wmi.vdev_param->fixed_rate;
-	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-					vdev_param, fixed_rate);
+	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param, rate);
 	if (ret) {
 		ath10k_warn(ar, "failed to set fixed rate param 0x%02x: %d\n",
-			    fixed_rate, ret);
-		ret = -EINVAL;
-		goto exit;
+			    rate, ret);
+		return ret;
 	}
 
-	arvif->fixed_rate = fixed_rate;
-
 	vdev_param = ar->wmi.vdev_param->nss;
-	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-					vdev_param, fixed_nss);
+	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param, nss);
+	if (ret) {
+		ath10k_warn(ar, "failed to set nss param %d: %d\n", nss, ret);
+		return ret;
+	}
 
+	vdev_param = ar->wmi.vdev_param->sgi;
+	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param, sgi);
 	if (ret) {
-		ath10k_warn(ar, "failed to set fixed nss param %d: %d\n",
-			    fixed_nss, ret);
-		ret = -EINVAL;
-		goto exit;
+		ath10k_warn(ar, "failed to set sgi param %d: %d\n", sgi, ret);
+		return ret;
 	}
 
-	arvif->fixed_nss = fixed_nss;
+	return 0;
+}
 
-	vdev_param = ar->wmi.vdev_param->sgi;
-	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
-					force_sgi);
+static bool
+ath10k_mac_can_set_bitrate_mask(struct ath10k *ar,
+				enum ieee80211_band band,
+				const struct cfg80211_bitrate_mask *mask)
+{
+	int i;
+	u16 vht_mcs;
 
-	if (ret) {
-		ath10k_warn(ar, "failed to set sgi param %d: %d\n",
-			    force_sgi, ret);
-		ret = -EINVAL;
-		goto exit;
+	/* Due to firmware limitation in WMI_PEER_ASSOC_CMDID it is impossible
+	 * to express all VHT MCS rate masks. Effectively only the following
+	 * ranges can be used: none, 0-7, 0-8 and 0-9.
+	 */
+	for (i = 0; i < NL80211_VHT_NSS_MAX; i++) {
+		vht_mcs = mask->control[band].vht_mcs[i];
+
+		switch (vht_mcs) {
+		case 0:
+		case BIT(8) - 1:
+		case BIT(9) - 1:
+		case BIT(10) - 1:
+			break;
+		default:
+			ath10k_warn(ar, "refusing bitrate mask with missing 0-7 VHT MCS rates\n");
+			return false;
+		}
 	}
 
-	arvif->force_sgi = force_sgi;
+	return true;
+}
 
-exit:
-	mutex_unlock(&ar->conf_mutex);
-	return ret;
+static void ath10k_mac_set_bitrate_mask_iter(void *data,
+					     struct ieee80211_sta *sta)
+{
+	struct ath10k_vif *arvif = data;
+	struct ath10k_sta *arsta = (struct ath10k_sta *)sta->drv_priv;
+	struct ath10k *ar = arvif->ar;
+
+	if (arsta->arvif != arvif)
+		return;
+
+	spin_lock_bh(&ar->data_lock);
+	arsta->changed |= IEEE80211_RC_SUPP_RATES_CHANGED;
+	spin_unlock_bh(&ar->data_lock);
+
+	ieee80211_queue_work(ar->hw, &arsta->update_wk);
 }
 
-static int ath10k_set_bitrate_mask(struct ieee80211_hw *hw,
-				   struct ieee80211_vif *vif,
-				   const struct cfg80211_bitrate_mask *mask)
+static int ath10k_mac_op_set_bitrate_mask(struct ieee80211_hw *hw,
+					  struct ieee80211_vif *vif,
+					  const struct cfg80211_bitrate_mask *mask)
 {
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct cfg80211_chan_def def;
 	struct ath10k *ar = arvif->ar;
-	enum ieee80211_band band = ar->hw->conf.chandef.chan->band;
-	u8 fixed_rate = WMI_FIXED_RATE_NONE;
-	u8 fixed_nss = ar->num_rf_chains;
-	u8 force_sgi;
+	enum ieee80211_band band;
+	const u8 *ht_mcs_mask;
+	const u16 *vht_mcs_mask;
+	u8 rate;
+	u8 nss;
+	u8 sgi;
+	int single_nss;
+	int ret;
 
-	if (ar->cfg_tx_chainmask)
-		fixed_nss = get_nss_from_chainmask(ar->cfg_tx_chainmask);
+	if (ath10k_mac_vif_chan(vif, &def))
+		return -EPERM;
+
+	band = def.chan->band;
+	ht_mcs_mask = mask->control[band].ht_mcs;
+	vht_mcs_mask = mask->control[band].vht_mcs;
 
-	force_sgi = mask->control[band].gi;
-	if (force_sgi == NL80211_TXRATE_FORCE_LGI)
+	sgi = mask->control[band].gi;
+	if (sgi == NL80211_TXRATE_FORCE_LGI)
 		return -EINVAL;
 
-	if (!ath10k_default_bitrate_mask(ar, band, mask)) {
-		if (!ath10k_get_fixed_rate_nss(ar, mask, band,
-					       &fixed_rate,
-					       &fixed_nss))
+	if (ath10k_mac_bitrate_mask_has_single_rate(ar, band, mask)) {
+		ret = ath10k_mac_bitrate_mask_get_single_rate(ar, band, mask,
+							      &rate, &nss);
+		if (ret) {
+			ath10k_warn(ar, "failed to get single rate for vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+			return ret;
+		}
+	} else if (ath10k_mac_bitrate_mask_get_single_nss(ar, band, mask,
+							  &single_nss)) {
+		rate = WMI_FIXED_RATE_NONE;
+		nss = single_nss;
+	} else {
+		rate = WMI_FIXED_RATE_NONE;
+		nss = min(ar->num_rf_chains,
+			  max(ath10k_mac_max_ht_nss(ht_mcs_mask),
+			      ath10k_mac_max_vht_nss(vht_mcs_mask)));
+
+		if (!ath10k_mac_can_set_bitrate_mask(ar, band, mask))
 			return -EINVAL;
+
+		mutex_lock(&ar->conf_mutex);
+
+		arvif->bitrate_mask = *mask;
+		ieee80211_iterate_stations_atomic(ar->hw,
+						  ath10k_mac_set_bitrate_mask_iter,
+						  arvif);
+
+		mutex_unlock(&ar->conf_mutex);
 	}
 
-	if (fixed_rate == WMI_FIXED_RATE_NONE && force_sgi) {
-		ath10k_warn(ar, "failed to force SGI usage for default rate settings\n");
-		return -EINVAL;
+	mutex_lock(&ar->conf_mutex);
+
+	ret = ath10k_mac_set_fixed_rate_params(arvif, rate, nss, sgi);
+	if (ret) {
+		ath10k_warn(ar, "failed to set fixed rate params on vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+		goto exit;
 	}
 
-	return ath10k_set_fixed_rate_param(arvif, fixed_rate,
-					   fixed_nss, force_sgi);
+exit:
+	mutex_unlock(&ar->conf_mutex);
+
+	return ret;
 }
 
 static void ath10k_sta_rc_update(struct ieee80211_hw *hw,
@@ -5090,6 +6066,317 @@ static int ath10k_ampdu_action(struct ieee80211_hw *hw,
 	return -EINVAL;
 }
 
+static void
+ath10k_mac_update_rx_channel(struct ath10k *ar,
+			     struct ieee80211_chanctx_conf *ctx,
+			     struct ieee80211_vif_chanctx_switch *vifs,
+			     int n_vifs)
+{
+	struct cfg80211_chan_def *def = NULL;
+
+	/* Both locks are required because ar->rx_channel is modified. This
+	 * allows readers to hold either lock.
+	 */
+	lockdep_assert_held(&ar->conf_mutex);
+	lockdep_assert_held(&ar->data_lock);
+
+	WARN_ON(ctx && vifs);
+	WARN_ON(vifs && n_vifs != 1);
+
+	/* FIXME: Sort of an optimization and a workaround. Peers and vifs are
+	 * on a linked list now. Doing a lookup peer -> vif -> chanctx for each
+	 * ppdu on Rx may reduce performance on low-end systems. It should be
+	 * possible to make tables/hashmaps to speed the lookup up (be vary of
+	 * cpu data cache lines though regarding sizes) but to keep the initial
+	 * implementation simple and less intrusive fallback to the slow lookup
+	 * only for multi-channel cases. Single-channel cases will remain to
+	 * use the old channel derival and thus performance should not be
+	 * affected much.
+	 */
+	rcu_read_lock();
+	if (!ctx && ath10k_mac_num_chanctxs(ar) == 1) {
+		ieee80211_iter_chan_contexts_atomic(ar->hw,
+					ath10k_mac_get_any_chandef_iter,
+					&def);
+
+		if (vifs)
+			def = &vifs[0].new_ctx->def;
+
+		ar->rx_channel = def->chan;
+	} else if (ctx && ath10k_mac_num_chanctxs(ar) == 0) {
+		ar->rx_channel = ctx->def.chan;
+	} else {
+		ar->rx_channel = NULL;
+	}
+	rcu_read_unlock();
+}
+
+static int
+ath10k_mac_op_add_chanctx(struct ieee80211_hw *hw,
+			  struct ieee80211_chanctx_conf *ctx)
+{
+	struct ath10k *ar = hw->priv;
+
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
+		   "mac chanctx add freq %hu width %d ptr %p\n",
+		   ctx->def.chan->center_freq, ctx->def.width, ctx);
+
+	mutex_lock(&ar->conf_mutex);
+
+	spin_lock_bh(&ar->data_lock);
+	ath10k_mac_update_rx_channel(ar, ctx, NULL, 0);
+	spin_unlock_bh(&ar->data_lock);
+
+	ath10k_recalc_radar_detection(ar);
+	ath10k_monitor_recalc(ar);
+
+	mutex_unlock(&ar->conf_mutex);
+
+	return 0;
+}
+
+static void
+ath10k_mac_op_remove_chanctx(struct ieee80211_hw *hw,
+			     struct ieee80211_chanctx_conf *ctx)
+{
+	struct ath10k *ar = hw->priv;
+
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
+		   "mac chanctx remove freq %hu width %d ptr %p\n",
+		   ctx->def.chan->center_freq, ctx->def.width, ctx);
+
+	mutex_lock(&ar->conf_mutex);
+
+	spin_lock_bh(&ar->data_lock);
+	ath10k_mac_update_rx_channel(ar, NULL, NULL, 0);
+	spin_unlock_bh(&ar->data_lock);
+
+	ath10k_recalc_radar_detection(ar);
+	ath10k_monitor_recalc(ar);
+
+	mutex_unlock(&ar->conf_mutex);
+}
+
+static void
+ath10k_mac_op_change_chanctx(struct ieee80211_hw *hw,
+			     struct ieee80211_chanctx_conf *ctx,
+			     u32 changed)
+{
+	struct ath10k *ar = hw->priv;
+
+	mutex_lock(&ar->conf_mutex);
+
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
+		   "mac chanctx change freq %hu width %d ptr %p changed %x\n",
+		   ctx->def.chan->center_freq, ctx->def.width, ctx, changed);
+
+	/* This shouldn't really happen because channel switching should use
+	 * switch_vif_chanctx().
+	 */
+	if (WARN_ON(changed & IEEE80211_CHANCTX_CHANGE_CHANNEL))
+		goto unlock;
+
+	ath10k_recalc_radar_detection(ar);
+
+	/* FIXME: How to configure Rx chains properly? */
+
+	/* No other actions are actually necessary. Firmware maintains channel
+	 * definitions per vdev internally and there's no host-side channel
+	 * context abstraction to configure, e.g. channel width.
+	 */
+
+unlock:
+	mutex_unlock(&ar->conf_mutex);
+}
+
+static int
+ath10k_mac_op_assign_vif_chanctx(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_chanctx_conf *ctx)
+{
+	struct ath10k *ar = hw->priv;
+	struct ath10k_vif *arvif = (void *)vif->drv_priv;
+	int ret;
+
+	mutex_lock(&ar->conf_mutex);
+
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
+		   "mac chanctx assign ptr %p vdev_id %i\n",
+		   ctx, arvif->vdev_id);
+
+	if (WARN_ON(arvif->is_started)) {
+		mutex_unlock(&ar->conf_mutex);
+		return -EBUSY;
+	}
+
+	ret = ath10k_vdev_start(arvif, &ctx->def);
+	if (ret) {
+		ath10k_warn(ar, "failed to start vdev %i addr %pM on freq %d: %d\n",
+			    arvif->vdev_id, vif->addr,
+			    ctx->def.chan->center_freq, ret);
+		goto err;
+	}
+
+	arvif->is_started = true;
+
+	if (vif->type == NL80211_IFTYPE_MONITOR) {
+		ret = ath10k_wmi_vdev_up(ar, arvif->vdev_id, 0, vif->addr);
+		if (ret) {
+			ath10k_warn(ar, "failed to up monitor vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+			goto err_stop;
+		}
+
+		arvif->is_up = true;
+	}
+
+	mutex_unlock(&ar->conf_mutex);
+	return 0;
+
+err_stop:
+	ath10k_vdev_stop(arvif);
+	arvif->is_started = false;
+
+err:
+	mutex_unlock(&ar->conf_mutex);
+	return ret;
+}
+
+static void
+ath10k_mac_op_unassign_vif_chanctx(struct ieee80211_hw *hw,
+				   struct ieee80211_vif *vif,
+				   struct ieee80211_chanctx_conf *ctx)
+{
+	struct ath10k *ar = hw->priv;
+	struct ath10k_vif *arvif = (void *)vif->drv_priv;
+	int ret;
+
+	mutex_lock(&ar->conf_mutex);
+
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
+		   "mac chanctx unassign ptr %p vdev_id %i\n",
+		   ctx, arvif->vdev_id);
+
+	WARN_ON(!arvif->is_started);
+
+	if (vif->type == NL80211_IFTYPE_MONITOR) {
+		WARN_ON(!arvif->is_up);
+
+		ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
+		if (ret)
+			ath10k_warn(ar, "failed to down monitor vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+
+		arvif->is_up = false;
+	}
+
+	ret = ath10k_vdev_stop(arvif);
+	if (ret)
+		ath10k_warn(ar, "failed to stop vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+
+	arvif->is_started = false;
+
+	mutex_unlock(&ar->conf_mutex);
+}
+
+static int
+ath10k_mac_op_switch_vif_chanctx(struct ieee80211_hw *hw,
+				 struct ieee80211_vif_chanctx_switch *vifs,
+				 int n_vifs,
+				 enum ieee80211_chanctx_switch_mode mode)
+{
+	struct ath10k *ar = hw->priv;
+	struct ath10k_vif *arvif;
+	int ret;
+	int i;
+
+	mutex_lock(&ar->conf_mutex);
+
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
+		   "mac chanctx switch n_vifs %d mode %d\n",
+		   n_vifs, mode);
+
+	/* First stop monitor interface. Some FW versions crash if there's a
+	 * lone monitor interface.
+	 */
+	if (ar->monitor_started)
+		ath10k_monitor_stop(ar);
+
+	for (i = 0; i < n_vifs; i++) {
+		arvif = ath10k_vif_to_arvif(vifs[i].vif);
+
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
+			   "mac chanctx switch vdev_id %i freq %hu->%hu width %d->%d\n",
+			   arvif->vdev_id,
+			   vifs[i].old_ctx->def.chan->center_freq,
+			   vifs[i].new_ctx->def.chan->center_freq,
+			   vifs[i].old_ctx->def.width,
+			   vifs[i].new_ctx->def.width);
+
+		if (WARN_ON(!arvif->is_started))
+			continue;
+
+		if (WARN_ON(!arvif->is_up))
+			continue;
+
+		ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
+		if (ret) {
+			ath10k_warn(ar, "failed to down vdev %d: %d\n",
+				    arvif->vdev_id, ret);
+			continue;
+		}
+	}
+
+	/* All relevant vdevs are downed and associated channel resources
+	 * should be available for the channel switch now.
+	 */
+
+	spin_lock_bh(&ar->data_lock);
+	ath10k_mac_update_rx_channel(ar, NULL, vifs, n_vifs);
+	spin_unlock_bh(&ar->data_lock);
+
+	for (i = 0; i < n_vifs; i++) {
+		arvif = ath10k_vif_to_arvif(vifs[i].vif);
+
+		if (WARN_ON(!arvif->is_started))
+			continue;
+
+		if (WARN_ON(!arvif->is_up))
+			continue;
+
+		ret = ath10k_mac_setup_bcn_tmpl(arvif);
+		if (ret)
+			ath10k_warn(ar, "failed to update bcn tmpl during csa: %d\n",
+				    ret);
+
+		ret = ath10k_mac_setup_prb_tmpl(arvif);
+		if (ret)
+			ath10k_warn(ar, "failed to update prb tmpl during csa: %d\n",
+				    ret);
+
+		ret = ath10k_vdev_restart(arvif, &vifs[i].new_ctx->def);
+		if (ret) {
+			ath10k_warn(ar, "failed to restart vdev %d: %d\n",
+				    arvif->vdev_id, ret);
+			continue;
+		}
+
+		ret = ath10k_wmi_vdev_up(arvif->ar, arvif->vdev_id, arvif->aid,
+					 arvif->bssid);
+		if (ret) {
+			ath10k_warn(ar, "failed to bring vdev up %d: %d\n",
+				    arvif->vdev_id, ret);
+			continue;
+		}
+	}
+
+	ath10k_monitor_recalc(ar);
+
+	mutex_unlock(&ar->conf_mutex);
+	return 0;
+}
+
 static const struct ieee80211_ops ath10k_ops = {
 	.tx				= ath10k_tx,
 	.start				= ath10k_start,
@@ -5114,31 +6401,31 @@ static const struct ieee80211_ops ath10k_ops = {
 	.get_antenna			= ath10k_get_antenna,
 	.reconfig_complete		= ath10k_reconfig_complete,
 	.get_survey			= ath10k_get_survey,
-	.set_bitrate_mask		= ath10k_set_bitrate_mask,
+	.set_bitrate_mask		= ath10k_mac_op_set_bitrate_mask,
 	.sta_rc_update			= ath10k_sta_rc_update,
 	.get_tsf			= ath10k_get_tsf,
 	.ampdu_action			= ath10k_ampdu_action,
 	.get_et_sset_count		= ath10k_debug_get_et_sset_count,
 	.get_et_stats			= ath10k_debug_get_et_stats,
 	.get_et_strings			= ath10k_debug_get_et_strings,
+	.add_chanctx			= ath10k_mac_op_add_chanctx,
+	.remove_chanctx			= ath10k_mac_op_remove_chanctx,
+	.change_chanctx			= ath10k_mac_op_change_chanctx,
+	.assign_vif_chanctx		= ath10k_mac_op_assign_vif_chanctx,
+	.unassign_vif_chanctx		= ath10k_mac_op_unassign_vif_chanctx,
+	.switch_vif_chanctx		= ath10k_mac_op_switch_vif_chanctx,
 
 	CFG80211_TESTMODE_CMD(ath10k_tm_cmd)
 
 #ifdef CONFIG_PM
-	.suspend			= ath10k_suspend,
-	.resume				= ath10k_resume,
+	.suspend			= ath10k_wow_op_suspend,
+	.resume				= ath10k_wow_op_resume,
 #endif
 #ifdef CONFIG_MAC80211_DEBUGFS
 	.sta_add_debugfs		= ath10k_sta_add_debugfs,
 #endif
 };
 
-#define RATETAB_ENT(_rate, _rateid, _flags) { \
-	.bitrate		= (_rate), \
-	.flags			= (_flags), \
-	.hw_value		= (_rateid), \
-}
-
 #define CHAN2G(_channel, _freq, _flags) { \
 	.band			= IEEE80211_BAND_2GHZ, \
 	.hw_value		= (_channel), \
@@ -5194,6 +6481,7 @@ static const struct ieee80211_channel ath10k_5ghz_channels[] = {
 	CHAN5G(132, 5660, 0),
 	CHAN5G(136, 5680, 0),
 	CHAN5G(140, 5700, 0),
+	CHAN5G(144, 5720, 0),
 	CHAN5G(149, 5745, 0),
 	CHAN5G(153, 5765, 0),
 	CHAN5G(157, 5785, 0),
@@ -5201,31 +6489,6 @@ 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),
-	RATETAB_ENT(20,  0x84, 0),
-	RATETAB_ENT(55,  0x8b, 0),
-	RATETAB_ENT(110, 0x96, 0),
-	/* OFDM */
-	RATETAB_ENT(60,  0x0c, 0),
-	RATETAB_ENT(90,  0x12, 0),
-	RATETAB_ENT(120, 0x18, 0),
-	RATETAB_ENT(180, 0x24, 0),
-	RATETAB_ENT(240, 0x30, 0),
-	RATETAB_ENT(360, 0x48, 0),
-	RATETAB_ENT(480, 0x60, 0),
-	RATETAB_ENT(540, 0x6c, 0),
-};
-
-#define ath10k_a_rates (ath10k_rates + 4)
-#define ath10k_a_rates_size (ARRAY_SIZE(ath10k_rates) - 4)
-#define ath10k_g_rates (ath10k_rates + 0)
-#define ath10k_g_rates_size (ARRAY_SIZE(ath10k_rates))
-
 struct ath10k *ath10k_mac_create(size_t priv_size)
 {
 	struct ieee80211_hw *hw;
@@ -5299,15 +6562,92 @@ static const struct ieee80211_iface_combination ath10k_10x_if_comb[] = {
 	},
 };
 
+static const struct ieee80211_iface_limit ath10k_tlv_if_limit[] = {
+	{
+		.max = 2,
+		.types = BIT(NL80211_IFTYPE_STATION) |
+			 BIT(NL80211_IFTYPE_AP) |
+			 BIT(NL80211_IFTYPE_P2P_CLIENT) |
+			 BIT(NL80211_IFTYPE_P2P_GO),
+	},
+	{
+		.max = 1,
+		.types = BIT(NL80211_IFTYPE_P2P_DEVICE),
+	},
+};
+
+static const struct ieee80211_iface_limit ath10k_tlv_if_limit_ibss[] = {
+	{
+		.max = 1,
+		.types = BIT(NL80211_IFTYPE_STATION),
+	},
+	{
+		.max = 1,
+		.types = BIT(NL80211_IFTYPE_ADHOC),
+	},
+};
+
+/* FIXME: This is not thouroughly tested. These combinations may over- or
+ * underestimate hw/fw capabilities.
+ */
+static struct ieee80211_iface_combination ath10k_tlv_if_comb[] = {
+	{
+		.limits = ath10k_tlv_if_limit,
+		.num_different_channels = 1,
+		.max_interfaces = 3,
+		.n_limits = ARRAY_SIZE(ath10k_tlv_if_limit),
+	},
+	{
+		.limits = ath10k_tlv_if_limit_ibss,
+		.num_different_channels = 1,
+		.max_interfaces = 2,
+		.n_limits = ARRAY_SIZE(ath10k_tlv_if_limit_ibss),
+	},
+};
+
+static struct ieee80211_iface_combination ath10k_tlv_qcs_if_comb[] = {
+	{
+		.limits = ath10k_tlv_if_limit,
+		.num_different_channels = 2,
+		.max_interfaces = 3,
+		.n_limits = ARRAY_SIZE(ath10k_tlv_if_limit),
+	},
+	{
+		.limits = ath10k_tlv_if_limit_ibss,
+		.num_different_channels = 1,
+		.max_interfaces = 2,
+		.n_limits = ARRAY_SIZE(ath10k_tlv_if_limit_ibss),
+	},
+};
+
 static struct ieee80211_sta_vht_cap ath10k_create_vht_cap(struct ath10k *ar)
 {
 	struct ieee80211_sta_vht_cap vht_cap = {0};
 	u16 mcs_map;
+	u32 val;
 	int i;
 
 	vht_cap.vht_supported = 1;
 	vht_cap.cap = ar->vht_cap_info;
 
+	if (ar->vht_cap_info & (IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
+				IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE)) {
+		val = ar->num_rf_chains - 1;
+		val <<= IEEE80211_VHT_CAP_BEAMFORMEE_STS_SHIFT;
+		val &= IEEE80211_VHT_CAP_BEAMFORMEE_STS_MASK;
+
+		vht_cap.cap |= val;
+	}
+
+	if (ar->vht_cap_info & (IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
+				IEEE80211_VHT_CAP_MU_BEAMFORMER_CAPABLE)) {
+		val = ar->num_rf_chains - 1;
+		val <<= IEEE80211_VHT_CAP_SOUNDING_DIMENSIONS_SHIFT;
+		val &= IEEE80211_VHT_CAP_SOUNDING_DIMENSIONS_MASK;
+
+		vht_cap.cap |= val;
+	}
+
 	mcs_map = 0;
 	for (i = 0; i < 8; i++) {
 		if (i < ar->num_rf_chains)
@@ -5438,6 +6778,10 @@ int ath10k_mac_register(struct ath10k *ar)
 	ht_cap = ath10k_get_ht_cap(ar);
 	vht_cap = ath10k_create_vht_cap(ar);
 
+	BUILD_BUG_ON((ARRAY_SIZE(ath10k_2ghz_channels) +
+		      ARRAY_SIZE(ath10k_5ghz_channels)) !=
+		     ATH10K_NUM_CHANS);
+
 	if (ar->phy_capability & WHAL_WLAN_11G_CAPABILITY) {
 		channels = kmemdup(ath10k_2ghz_channels,
 				   sizeof(ath10k_2ghz_channels),
@@ -5492,24 +6836,31 @@ int ath10k_mac_register(struct ath10k *ar)
 			BIT(NL80211_IFTYPE_P2P_CLIENT) |
 			BIT(NL80211_IFTYPE_P2P_GO);
 
-	ar->hw->flags = IEEE80211_HW_SIGNAL_DBM |
-			IEEE80211_HW_SUPPORTS_PS |
-			IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
-			IEEE80211_HW_MFP_CAPABLE |
-			IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-			IEEE80211_HW_HAS_RATE_CONTROL |
-			IEEE80211_HW_AP_LINK_PS |
-			IEEE80211_HW_SPECTRUM_MGMT |
-			IEEE80211_HW_SW_CRYPTO_CONTROL;
+	ieee80211_hw_set(ar->hw, SIGNAL_DBM);
+	ieee80211_hw_set(ar->hw, SUPPORTS_PS);
+	ieee80211_hw_set(ar->hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(ar->hw, MFP_CAPABLE);
+	ieee80211_hw_set(ar->hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(ar->hw, HAS_RATE_CONTROL);
+	ieee80211_hw_set(ar->hw, AP_LINK_PS);
+	ieee80211_hw_set(ar->hw, SPECTRUM_MGMT);
+	ieee80211_hw_set(ar->hw, SW_CRYPTO_CONTROL);
+	ieee80211_hw_set(ar->hw, SUPPORT_FAST_XMIT);
+	ieee80211_hw_set(ar->hw, CONNECTION_MONITOR);
+	ieee80211_hw_set(ar->hw, SUPPORTS_PER_STA_GTK);
+	ieee80211_hw_set(ar->hw, WANT_MONITOR_VIF);
+	ieee80211_hw_set(ar->hw, CHANCTX_STA_CSA);
+	ieee80211_hw_set(ar->hw, QUEUE_CONTROL);
 
 	ar->hw->wiphy->features |= NL80211_FEATURE_STATIC_SMPS;
+	ar->hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
 
 	if (ar->ht_cap_info & WMI_HT_CAP_DYNAMIC_SMPS)
 		ar->hw->wiphy->features |= NL80211_FEATURE_DYNAMIC_SMPS;
 
 	if (ar->ht_cap_info & WMI_HT_CAP_ENABLED) {
-		ar->hw->flags |= IEEE80211_HW_AMPDU_AGGREGATION;
-		ar->hw->flags |= IEEE80211_HW_TX_AMPDU_SETUP_IN_HW;
+		ieee80211_hw_set(ar->hw, AMPDU_AGGREGATION);
+		ieee80211_hw_set(ar->hw, TX_AMPDU_SETUP_IN_HW);
 	}
 
 	ar->hw->wiphy->max_scan_ssids = WLAN_SCAN_PARAMS_MAX_SSID;
@@ -5533,6 +6884,9 @@ int ath10k_mac_register(struct ath10k *ar)
 			NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P;
 	}
 
+	if (test_bit(WMI_SERVICE_TDLS, ar->wmi.svc_map))
+		ar->hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
+
 	ar->hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
 	ar->hw->wiphy->flags |= WIPHY_FLAG_HAS_CHANNEL_SWITCH;
 	ar->hw->wiphy->max_remain_on_channel_duration = 5000;
@@ -5540,20 +6894,46 @@ int ath10k_mac_register(struct ath10k *ar)
 	ar->hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
 	ar->hw->wiphy->features |= NL80211_FEATURE_AP_MODE_CHAN_WIDTH_CHANGE;
 
+	ar->hw->wiphy->max_ap_assoc_sta = ar->max_num_stations;
+
+	ret = ath10k_wow_init(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to init wow: %d\n", ret);
+		goto err_free;
+	}
+
 	/*
 	 * on LL hardware queues are managed entirely by the FW
 	 * so we only advertise to mac we can do the queues thing
 	 */
-	ar->hw->queues = 4;
+	ar->hw->queues = IEEE80211_MAX_QUEUES;
+
+	/* vdev_ids are used as hw queue numbers. Make sure offchan tx queue is
+	 * something that vdev_ids can't reach so that we don't stop the queue
+	 * accidentally.
+	 */
+	ar->hw->offchannel_tx_hw_queue = IEEE80211_MAX_QUEUES - 1;
 
 	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_TLV:
+		if (test_bit(WMI_SERVICE_ADAPTIVE_OCS, ar->wmi.svc_map)) {
+			ar->hw->wiphy->iface_combinations =
+				ath10k_tlv_qcs_if_comb;
+			ar->hw->wiphy->n_iface_combinations =
+				ARRAY_SIZE(ath10k_tlv_qcs_if_comb);
+		} else {
+			ar->hw->wiphy->iface_combinations = ath10k_tlv_if_comb;
+			ar->hw->wiphy->n_iface_combinations =
+				ARRAY_SIZE(ath10k_tlv_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:
diff --git a/drivers/net/wireless/ath/ath10k/mac.h b/drivers/net/wireless/ath/ath10k/mac.h
index 68296117d203..b291f063705c 100644
--- a/drivers/net/wireless/ath/ath10k/mac.h
+++ b/drivers/net/wireless/ath/ath10k/mac.h
@@ -23,11 +23,22 @@
 
 #define WEP_KEYID_SHIFT 6
 
+enum wmi_tlv_tx_pause_id;
+enum wmi_tlv_tx_pause_action;
+
 struct ath10k_generic_iter {
 	struct ath10k *ar;
 	int ret;
 };
 
+struct rfc1042_hdr {
+	u8 llc_dsap;
+	u8 llc_ssap;
+	u8 llc_ctrl;
+	u8 snap_oui[3];
+	__be16 snap_type;
+} __packed;
+
 struct ath10k *ath10k_mac_create(size_t priv_size);
 void ath10k_mac_destroy(struct ath10k *ar);
 int ath10k_mac_register(struct ath10k *ar);
@@ -45,6 +56,24 @@ void ath10k_mac_vif_beacon_free(struct ath10k_vif *arvif);
 void ath10k_drain_tx(struct ath10k *ar);
 bool ath10k_mac_is_peer_wep_key_set(struct ath10k *ar, const u8 *addr,
 				    u8 keyidx);
+int ath10k_mac_vif_chan(struct ieee80211_vif *vif,
+			struct cfg80211_chan_def *def);
+
+void ath10k_mac_handle_beacon(struct ath10k *ar, struct sk_buff *skb);
+void ath10k_mac_handle_beacon_miss(struct ath10k *ar, u32 vdev_id);
+void ath10k_mac_handle_tx_pause(struct ath10k *ar, u32 vdev_id,
+				enum wmi_tlv_tx_pause_id pause_id,
+				enum wmi_tlv_tx_pause_action action);
+
+u8 ath10k_mac_hw_rate_to_idx(const struct ieee80211_supported_band *sband,
+			     u8 hw_rate);
+u8 ath10k_mac_bitrate_to_idx(const struct ieee80211_supported_band *sband,
+			     u32 bitrate);
+
+void ath10k_mac_tx_lock(struct ath10k *ar, int reason);
+void ath10k_mac_tx_unlock(struct ath10k *ar, int reason);
+void ath10k_mac_vif_tx_lock(struct ath10k_vif *arvif, int reason);
+void ath10k_mac_vif_tx_unlock(struct ath10k_vif *arvif, int reason);
 
 static inline struct ath10k_vif *ath10k_vif_to_arvif(struct ieee80211_vif *vif)
 {
diff --git a/drivers/net/wireless/ath/ath10k/p2p.c b/drivers/net/wireless/ath/ath10k/p2p.c
new file mode 100644
index 000000000000..c0b6ffaf3ec1
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/p2p.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2015 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 "wmi.h"
+#include "mac.h"
+#include "p2p.h"
+
+static void ath10k_p2p_noa_ie_fill(u8 *data, size_t len,
+				   const struct wmi_p2p_noa_info *noa)
+{
+	struct ieee80211_p2p_noa_attr *noa_attr;
+	u8  ctwindow_oppps = noa->ctwindow_oppps;
+	u8 ctwindow = ctwindow_oppps >> WMI_P2P_OPPPS_CTWINDOW_OFFSET;
+	bool oppps = !!(ctwindow_oppps & WMI_P2P_OPPPS_ENABLE_BIT);
+	__le16 *noa_attr_len;
+	u16 attr_len;
+	u8 noa_descriptors = noa->num_descriptors;
+	int i;
+
+	/* P2P IE */
+	data[0] = WLAN_EID_VENDOR_SPECIFIC;
+	data[1] = len - 2;
+	data[2] = (WLAN_OUI_WFA >> 16) & 0xff;
+	data[3] = (WLAN_OUI_WFA >> 8) & 0xff;
+	data[4] = (WLAN_OUI_WFA >> 0) & 0xff;
+	data[5] = WLAN_OUI_TYPE_WFA_P2P;
+
+	/* NOA ATTR */
+	data[6] = IEEE80211_P2P_ATTR_ABSENCE_NOTICE;
+	noa_attr_len = (__le16 *)&data[7]; /* 2 bytes */
+	noa_attr = (struct ieee80211_p2p_noa_attr *)&data[9];
+
+	noa_attr->index = noa->index;
+	noa_attr->oppps_ctwindow = ctwindow;
+	if (oppps)
+		noa_attr->oppps_ctwindow |= IEEE80211_P2P_OPPPS_ENABLE_BIT;
+
+	for (i = 0; i < noa_descriptors; i++) {
+		noa_attr->desc[i].count =
+			__le32_to_cpu(noa->descriptors[i].type_count);
+		noa_attr->desc[i].duration = noa->descriptors[i].duration;
+		noa_attr->desc[i].interval = noa->descriptors[i].interval;
+		noa_attr->desc[i].start_time = noa->descriptors[i].start_time;
+	}
+
+	attr_len = 2; /* index + oppps_ctwindow */
+	attr_len += noa_descriptors * sizeof(struct ieee80211_p2p_noa_desc);
+	*noa_attr_len = __cpu_to_le16(attr_len);
+}
+
+static size_t ath10k_p2p_noa_ie_len_compute(const struct wmi_p2p_noa_info *noa)
+{
+	size_t len = 0;
+
+	if (!noa->num_descriptors &&
+	    !(noa->ctwindow_oppps & WMI_P2P_OPPPS_ENABLE_BIT))
+		return 0;
+
+	len += 1 + 1 + 4; /* EID + len + OUI */
+	len += 1 + 2; /* noa attr + attr len */
+	len += 1 + 1; /* index + oppps_ctwindow */
+	len += noa->num_descriptors * sizeof(struct ieee80211_p2p_noa_desc);
+
+	return len;
+}
+
+static void ath10k_p2p_noa_ie_assign(struct ath10k_vif *arvif, void *ie,
+				     size_t len)
+{
+	struct ath10k *ar = arvif->ar;
+
+	lockdep_assert_held(&ar->data_lock);
+
+	kfree(arvif->u.ap.noa_data);
+
+	arvif->u.ap.noa_data = ie;
+	arvif->u.ap.noa_len = len;
+}
+
+static void __ath10k_p2p_noa_update(struct ath10k_vif *arvif,
+				    const struct wmi_p2p_noa_info *noa)
+{
+	struct ath10k *ar = arvif->ar;
+	void *ie;
+	size_t len;
+
+	lockdep_assert_held(&ar->data_lock);
+
+	ath10k_p2p_noa_ie_assign(arvif, NULL, 0);
+
+	len = ath10k_p2p_noa_ie_len_compute(noa);
+	if (!len)
+		return;
+
+	ie = kmalloc(len, GFP_ATOMIC);
+	if (!ie)
+		return;
+
+	ath10k_p2p_noa_ie_fill(ie, len, noa);
+	ath10k_p2p_noa_ie_assign(arvif, ie, len);
+}
+
+void ath10k_p2p_noa_update(struct ath10k_vif *arvif,
+			   const struct wmi_p2p_noa_info *noa)
+{
+	struct ath10k *ar = arvif->ar;
+
+	spin_lock_bh(&ar->data_lock);
+	__ath10k_p2p_noa_update(arvif, noa);
+	spin_unlock_bh(&ar->data_lock);
+}
+
+struct ath10k_p2p_noa_arg {
+	u32 vdev_id;
+	const struct wmi_p2p_noa_info *noa;
+};
+
+static void ath10k_p2p_noa_update_vdev_iter(void *data, u8 *mac,
+					    struct ieee80211_vif *vif)
+{
+	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+	struct ath10k_p2p_noa_arg *arg = data;
+
+	if (arvif->vdev_id != arg->vdev_id)
+		return;
+
+	ath10k_p2p_noa_update(arvif, arg->noa);
+}
+
+void ath10k_p2p_noa_update_by_vdev_id(struct ath10k *ar, u32 vdev_id,
+				      const struct wmi_p2p_noa_info *noa)
+{
+	struct ath10k_p2p_noa_arg arg = {
+		.vdev_id = vdev_id,
+		.noa = noa,
+	};
+
+	ieee80211_iterate_active_interfaces_atomic(ar->hw,
+						   IEEE80211_IFACE_ITER_NORMAL,
+						   ath10k_p2p_noa_update_vdev_iter,
+						   &arg);
+}
diff --git a/drivers/net/wireless/ath/ath10k/p2p.h b/drivers/net/wireless/ath/ath10k/p2p.h
new file mode 100644
index 000000000000..7be616e2e121
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/p2p.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2015 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 _P2P_H
+#define _P2P_H
+
+struct ath10k_vif;
+struct wmi_p2p_noa_info;
+
+void ath10k_p2p_noa_update(struct ath10k_vif *arvif,
+			   const struct wmi_p2p_noa_info *noa);
+void ath10k_p2p_noa_update_by_vdev_id(struct ath10k *ar, u32 vdev_id,
+				      const struct wmi_p2p_noa_info *noa);
+
+#endif
diff --git a/drivers/net/wireless/ath/ath10k/pci.c b/drivers/net/wireless/ath/ath10k/pci.c
index 7681237fe298..ea656e011a96 100644
--- a/drivers/net/wireless/ath/ath10k/pci.c
+++ b/drivers/net/wireless/ath/ath10k/pci.c
@@ -113,7 +113,7 @@ static const struct ce_attr host_ce_config_wlan[] = {
 		.flags = CE_ATTR_FLAGS,
 		.src_nentries = 0,
 		.src_sz_max = 2048,
-		.dest_nentries = 32,
+		.dest_nentries = 128,
 	},
 
 	/* CE3: host->target WMI */
@@ -183,7 +183,7 @@ static const struct ce_pipe_config target_ce_config_wlan[] = {
 	{
 		.pipenum = __cpu_to_le32(2),
 		.pipedir = __cpu_to_le32(PIPEDIR_IN),
-		.nentries = __cpu_to_le32(32),
+		.nentries = __cpu_to_le32(64),
 		.nbytes_max = __cpu_to_le32(2048),
 		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
 		.reserved = __cpu_to_le32(0),
@@ -330,6 +330,205 @@ static const struct service_to_pipe target_service_to_ce_map_wlan[] = {
 	},
 };
 
+static bool ath10k_pci_is_awake(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	u32 val = ioread32(ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS +
+			   RTC_STATE_ADDRESS);
+
+	return RTC_STATE_V_GET(val) == RTC_STATE_V_ON;
+}
+
+static void __ath10k_pci_wake(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+
+	lockdep_assert_held(&ar_pci->ps_lock);
+
+	ath10k_dbg(ar, ATH10K_DBG_PCI_PS, "pci ps wake reg refcount %lu awake %d\n",
+		   ar_pci->ps_wake_refcount, ar_pci->ps_awake);
+
+	iowrite32(PCIE_SOC_WAKE_V_MASK,
+		  ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS +
+		  PCIE_SOC_WAKE_ADDRESS);
+}
+
+static void __ath10k_pci_sleep(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+
+	lockdep_assert_held(&ar_pci->ps_lock);
+
+	ath10k_dbg(ar, ATH10K_DBG_PCI_PS, "pci ps sleep reg refcount %lu awake %d\n",
+		   ar_pci->ps_wake_refcount, ar_pci->ps_awake);
+
+	iowrite32(PCIE_SOC_WAKE_RESET,
+		  ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS +
+		  PCIE_SOC_WAKE_ADDRESS);
+	ar_pci->ps_awake = false;
+}
+
+static int ath10k_pci_wake_wait(struct ath10k *ar)
+{
+	int tot_delay = 0;
+	int curr_delay = 5;
+
+	while (tot_delay < PCIE_WAKE_TIMEOUT) {
+		if (ath10k_pci_is_awake(ar))
+			return 0;
+
+		udelay(curr_delay);
+		tot_delay += curr_delay;
+
+		if (curr_delay < 50)
+			curr_delay += 5;
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int ath10k_pci_wake(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&ar_pci->ps_lock, flags);
+
+	ath10k_dbg(ar, ATH10K_DBG_PCI_PS, "pci ps wake refcount %lu awake %d\n",
+		   ar_pci->ps_wake_refcount, ar_pci->ps_awake);
+
+	/* This function can be called very frequently. To avoid excessive
+	 * CPU stalls for MMIO reads use a cache var to hold the device state.
+	 */
+	if (!ar_pci->ps_awake) {
+		__ath10k_pci_wake(ar);
+
+		ret = ath10k_pci_wake_wait(ar);
+		if (ret == 0)
+			ar_pci->ps_awake = true;
+	}
+
+	if (ret == 0) {
+		ar_pci->ps_wake_refcount++;
+		WARN_ON(ar_pci->ps_wake_refcount == 0);
+	}
+
+	spin_unlock_irqrestore(&ar_pci->ps_lock, flags);
+
+	return ret;
+}
+
+static void ath10k_pci_sleep(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ar_pci->ps_lock, flags);
+
+	ath10k_dbg(ar, ATH10K_DBG_PCI_PS, "pci ps sleep refcount %lu awake %d\n",
+		   ar_pci->ps_wake_refcount, ar_pci->ps_awake);
+
+	if (WARN_ON(ar_pci->ps_wake_refcount == 0))
+		goto skip;
+
+	ar_pci->ps_wake_refcount--;
+
+	mod_timer(&ar_pci->ps_timer, jiffies +
+		  msecs_to_jiffies(ATH10K_PCI_SLEEP_GRACE_PERIOD_MSEC));
+
+skip:
+	spin_unlock_irqrestore(&ar_pci->ps_lock, flags);
+}
+
+static void ath10k_pci_ps_timer(unsigned long ptr)
+{
+	struct ath10k *ar = (void *)ptr;
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ar_pci->ps_lock, flags);
+
+	ath10k_dbg(ar, ATH10K_DBG_PCI_PS, "pci ps timer refcount %lu awake %d\n",
+		   ar_pci->ps_wake_refcount, ar_pci->ps_awake);
+
+	if (ar_pci->ps_wake_refcount > 0)
+		goto skip;
+
+	__ath10k_pci_sleep(ar);
+
+skip:
+	spin_unlock_irqrestore(&ar_pci->ps_lock, flags);
+}
+
+static void ath10k_pci_sleep_sync(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	unsigned long flags;
+
+	del_timer_sync(&ar_pci->ps_timer);
+
+	spin_lock_irqsave(&ar_pci->ps_lock, flags);
+	WARN_ON(ar_pci->ps_wake_refcount > 0);
+	__ath10k_pci_sleep(ar);
+	spin_unlock_irqrestore(&ar_pci->ps_lock, flags);
+}
+
+void ath10k_pci_write32(struct ath10k *ar, u32 offset, u32 value)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	int ret;
+
+	ret = ath10k_pci_wake(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to wake target for write32 of 0x%08x at 0x%08x: %d\n",
+			    value, offset, ret);
+		return;
+	}
+
+	iowrite32(value, ar_pci->mem + offset);
+	ath10k_pci_sleep(ar);
+}
+
+u32 ath10k_pci_read32(struct ath10k *ar, u32 offset)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	u32 val;
+	int ret;
+
+	ret = ath10k_pci_wake(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to wake target for read32 at 0x%08x: %d\n",
+			    offset, ret);
+		return 0xffffffff;
+	}
+
+	val = ioread32(ar_pci->mem + offset);
+	ath10k_pci_sleep(ar);
+
+	return val;
+}
+
+u32 ath10k_pci_soc_read32(struct ath10k *ar, u32 addr)
+{
+	return ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS + addr);
+}
+
+void ath10k_pci_soc_write32(struct ath10k *ar, u32 addr, u32 val)
+{
+	ath10k_pci_write32(ar, RTC_SOC_BASE_ADDRESS + addr, val);
+}
+
+u32 ath10k_pci_reg_read32(struct ath10k *ar, u32 addr)
+{
+	return ath10k_pci_read32(ar, PCIE_LOCAL_BASE_ADDRESS + addr);
+}
+
+void ath10k_pci_reg_write32(struct ath10k *ar, u32 addr, u32 val)
+{
+	ath10k_pci_write32(ar, PCIE_LOCAL_BASE_ADDRESS + addr, val);
+}
+
 static bool ath10k_pci_irq_pending(struct ath10k *ar)
 {
 	u32 cause;
@@ -793,45 +992,6 @@ static int ath10k_pci_diag_write32(struct ath10k *ar, u32 address, u32 value)
 	return ath10k_pci_diag_write_mem(ar, address, &val, sizeof(val));
 }
 
-static bool ath10k_pci_is_awake(struct ath10k *ar)
-{
-	u32 val = ath10k_pci_reg_read32(ar, RTC_STATE_ADDRESS);
-
-	return RTC_STATE_V_GET(val) == RTC_STATE_V_ON;
-}
-
-static int ath10k_pci_wake_wait(struct ath10k *ar)
-{
-	int tot_delay = 0;
-	int curr_delay = 5;
-
-	while (tot_delay < PCIE_WAKE_TIMEOUT) {
-		if (ath10k_pci_is_awake(ar))
-			return 0;
-
-		udelay(curr_delay);
-		tot_delay += curr_delay;
-
-		if (curr_delay < 50)
-			curr_delay += 5;
-	}
-
-	return -ETIMEDOUT;
-}
-
-static int ath10k_pci_wake(struct ath10k *ar)
-{
-	ath10k_pci_reg_write32(ar, PCIE_SOC_WAKE_ADDRESS,
-			       PCIE_SOC_WAKE_V_MASK);
-	return ath10k_pci_wake_wait(ar);
-}
-
-static void ath10k_pci_sleep(struct ath10k *ar)
-{
-	ath10k_pci_reg_write32(ar, PCIE_SOC_WAKE_ADDRESS,
-			       PCIE_SOC_WAKE_RESET);
-}
-
 /* Called by lower (CE) layer when a send to Target completes. */
 static void ath10k_pci_ce_send_done(struct ath10k_ce_pipe *ce_state)
 {
@@ -1212,11 +1372,15 @@ static void ath10k_pci_irq_enable(struct ath10k *ar)
 
 static int ath10k_pci_hif_start(struct ath10k *ar)
 {
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot hif start\n");
 
 	ath10k_pci_irq_enable(ar);
 	ath10k_pci_rx_post(ar);
 
+	pcie_capability_write_word(ar_pci->pdev, PCI_EXP_LNKCTL,
+				   ar_pci->link_ctl);
+
 	return 0;
 }
 
@@ -1260,7 +1424,6 @@ static void ath10k_pci_tx_pipe_cleanup(struct ath10k_pci_pipe *pci_pipe)
 	struct ath10k_ce_ring *ce_ring;
 	struct ce_desc *ce_desc;
 	struct sk_buff *skb;
-	unsigned int id;
 	int i;
 
 	ar = pci_pipe->hif_ce_state;
@@ -1284,8 +1447,6 @@ static void ath10k_pci_tx_pipe_cleanup(struct ath10k_pci_pipe *pci_pipe)
 			continue;
 
 		ce_ring->per_transfer_context[i] = NULL;
-		id = MS(__le16_to_cpu(ce_desc[i].flags),
-			CE_DESC_FLAGS_META_DATA);
 
 		ar_pci->msg_callbacks_current.tx_completion(ar, skb);
 	}
@@ -1329,6 +1490,9 @@ static void ath10k_pci_flush(struct ath10k *ar)
 
 static void ath10k_pci_hif_stop(struct ath10k *ar)
 {
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	unsigned long flags;
+
 	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot hif stop\n");
 
 	/* Most likely the device has HTT Rx ring configured. The only way to
@@ -1347,6 +1511,10 @@ static void ath10k_pci_hif_stop(struct ath10k *ar)
 	ath10k_pci_irq_disable(ar);
 	ath10k_pci_irq_sync(ar);
 	ath10k_pci_flush(ar);
+
+	spin_lock_irqsave(&ar_pci->ps_lock, flags);
+	WARN_ON(ar_pci->ps_wake_refcount > 0);
+	spin_unlock_irqrestore(&ar_pci->ps_lock, flags);
 }
 
 static int ath10k_pci_hif_exchange_bmi_msg(struct ath10k *ar,
@@ -1524,12 +1692,11 @@ static int ath10k_pci_get_num_banks(struct ath10k *ar)
 		switch (MS(ar->chip_id, SOC_CHIP_ID_REV)) {
 		case QCA6174_HW_1_0_CHIP_ID_REV:
 		case QCA6174_HW_1_1_CHIP_ID_REV:
+		case QCA6174_HW_2_1_CHIP_ID_REV:
+		case QCA6174_HW_2_2_CHIP_ID_REV:
 			return 3;
 		case QCA6174_HW_1_3_CHIP_ID_REV:
 			return 2;
-		case QCA6174_HW_2_1_CHIP_ID_REV:
-		case QCA6174_HW_2_2_CHIP_ID_REV:
-			return 6;
 		case QCA6174_HW_3_0_CHIP_ID_REV:
 		case QCA6174_HW_3_1_CHIP_ID_REV:
 		case QCA6174_HW_3_2_CHIP_ID_REV:
@@ -1967,15 +2134,15 @@ static int ath10k_pci_chip_reset(struct ath10k *ar)
 
 static int ath10k_pci_hif_power_up(struct ath10k *ar)
 {
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	int ret;
 
 	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot hif power up\n");
 
-	ret = ath10k_pci_wake(ar);
-	if (ret) {
-		ath10k_err(ar, "failed to wake up target: %d\n", ret);
-		return ret;
-	}
+	pcie_capability_read_word(ar_pci->pdev, PCI_EXP_LNKCTL,
+				  &ar_pci->link_ctl);
+	pcie_capability_write_word(ar_pci->pdev, PCI_EXP_LNKCTL,
+				   ar_pci->link_ctl & ~PCI_EXP_LNKCTL_ASPMC);
 
 	/*
 	 * Bring the target up cleanly.
@@ -2023,7 +2190,6 @@ err_ce:
 	ath10k_pci_ce_deinit(ar);
 
 err_sleep:
-	ath10k_pci_sleep(ar);
 	return ret;
 }
 
@@ -2034,28 +2200,18 @@ static void ath10k_pci_hif_power_down(struct ath10k *ar)
 	/* Currently hif_power_up performs effectively a reset and hif_stop
 	 * resets the chip as well so there's no point in resetting here.
 	 */
-
-	ath10k_pci_sleep(ar);
 }
 
 #ifdef CONFIG_PM
 
-#define ATH10K_PCI_PM_CONTROL 0x44
-
 static int ath10k_pci_hif_suspend(struct ath10k *ar)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	struct pci_dev *pdev = ar_pci->pdev;
-	u32 val;
-
-	pci_read_config_dword(pdev, ATH10K_PCI_PM_CONTROL, &val);
-
-	if ((val & 0x000000ff) != 0x3) {
-		pci_save_state(pdev);
-		pci_disable_device(pdev);
-		pci_write_config_dword(pdev, ATH10K_PCI_PM_CONTROL,
-				       (val & 0xffffff00) | 0x03);
-	}
+	/* The grace timer can still be counting down and ar->ps_awake be true.
+	 * It is known that the device may be asleep after resuming regardless
+	 * of the SoC powersave state before suspending. Hence make sure the
+	 * device is asleep before proceeding.
+	 */
+	ath10k_pci_sleep_sync(ar);
 
 	return 0;
 }
@@ -2066,22 +2222,14 @@ static int ath10k_pci_hif_resume(struct ath10k *ar)
 	struct pci_dev *pdev = ar_pci->pdev;
 	u32 val;
 
-	pci_read_config_dword(pdev, ATH10K_PCI_PM_CONTROL, &val);
-
-	if ((val & 0x000000ff) != 0) {
-		pci_restore_state(pdev);
-		pci_write_config_dword(pdev, ATH10K_PCI_PM_CONTROL,
-				       val & 0xffffff00);
-		/*
-		 * Suspend/Resume resets the PCI configuration space,
-		 * so we have to re-disable the RETRY_TIMEOUT register (0x41)
-		 * to keep PCI Tx retries from interfering with C3 CPU state
-		 */
-		pci_read_config_dword(pdev, 0x40, &val);
-
-		if ((val & 0x0000ff00) != 0)
-			pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
-	}
+	/* Suspend/Resume resets the PCI configuration space, so we have to
+	 * re-disable the RETRY_TIMEOUT register (0x41) to keep PCI Tx retries
+	 * from interfering with C3 CPU state. pci_restore_state won't help
+	 * here since it only restores the first 64 bytes pci config header.
+	 */
+	pci_read_config_dword(pdev, 0x40, &val);
+	if ((val & 0x0000ff00) != 0)
+		pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
 
 	return 0;
 }
@@ -2497,7 +2645,6 @@ static int ath10k_pci_claim(struct ath10k *ar)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	struct pci_dev *pdev = ar_pci->pdev;
-	u32 lcr_val;
 	int ret;
 
 	pci_set_drvdata(pdev, ar);
@@ -2531,10 +2678,6 @@ static int ath10k_pci_claim(struct ath10k *ar)
 
 	pci_set_master(pdev);
 
-	/* Workaround: Disable ASPM */
-	pci_read_config_dword(pdev, 0x80, &lcr_val);
-	pci_write_config_dword(pdev, 0x80, (lcr_val & 0xffffff00));
-
 	/* Arrange for access to Target SoC registers. */
 	ar_pci->mem = pci_iomap(pdev, BAR_NUM, 0);
 	if (!ar_pci->mem) {
@@ -2621,9 +2764,19 @@ static int ath10k_pci_probe(struct pci_dev *pdev,
 	ar_pci->dev = &pdev->dev;
 	ar_pci->ar = ar;
 
+	if (pdev->subsystem_vendor || pdev->subsystem_device)
+		scnprintf(ar->spec_board_id, sizeof(ar->spec_board_id),
+			  "%04x:%04x:%04x:%04x",
+			  pdev->vendor, pdev->device,
+			  pdev->subsystem_vendor, pdev->subsystem_device);
+
 	spin_lock_init(&ar_pci->ce_lock);
+	spin_lock_init(&ar_pci->ps_lock);
+
 	setup_timer(&ar_pci->rx_post_retry, ath10k_pci_rx_replenish_retry,
 		    (unsigned long)ar);
+	setup_timer(&ar_pci->ps_timer, ath10k_pci_ps_timer,
+		    (unsigned long)ar);
 
 	ret = ath10k_pci_claim(ar);
 	if (ret) {
@@ -2631,12 +2784,6 @@ static int ath10k_pci_probe(struct pci_dev *pdev,
 		goto err_core_destroy;
 	}
 
-	ret = ath10k_pci_wake(ar);
-	if (ret) {
-		ath10k_err(ar, "failed to wake up: %d\n", ret);
-		goto err_release;
-	}
-
 	ret = ath10k_pci_alloc_pipes(ar);
 	if (ret) {
 		ath10k_err(ar, "failed to allocate copy engine pipes: %d\n",
@@ -2678,11 +2825,9 @@ static int ath10k_pci_probe(struct pci_dev *pdev,
 	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;
+		goto err_free_irq;
 	}
 
-	ath10k_pci_sleep(ar);
-
 	ret = ath10k_core_register(ar, chip_id);
 	if (ret) {
 		ath10k_err(ar, "failed to register driver core: %d\n", ret);
@@ -2702,9 +2847,7 @@ err_free_pipes:
 	ath10k_pci_free_pipes(ar);
 
 err_sleep:
-	ath10k_pci_sleep(ar);
-
-err_release:
+	ath10k_pci_sleep_sync(ar);
 	ath10k_pci_release(ar);
 
 err_core_destroy:
@@ -2734,6 +2877,7 @@ static void ath10k_pci_remove(struct pci_dev *pdev)
 	ath10k_pci_deinit_irq(ar);
 	ath10k_pci_ce_deinit(ar);
 	ath10k_pci_free_pipes(ar);
+	ath10k_pci_sleep_sync(ar);
 	ath10k_pci_release(ar);
 	ath10k_core_destroy(ar);
 }
@@ -2770,7 +2914,21 @@ module_exit(ath10k_pci_exit);
 MODULE_AUTHOR("Qualcomm Atheros");
 MODULE_DESCRIPTION("Driver support for Atheros QCA988X PCIe devices");
 MODULE_LICENSE("Dual BSD/GPL");
+
+/* QCA988x 2.0 firmware files */
 MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" QCA988X_HW_2_0_FW_FILE);
 MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" ATH10K_FW_API2_FILE);
 MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" ATH10K_FW_API3_FILE);
+MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" ATH10K_FW_API4_FILE);
+MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" ATH10K_FW_API5_FILE);
 MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" QCA988X_HW_2_0_BOARD_DATA_FILE);
+
+/* QCA6174 2.1 firmware files */
+MODULE_FIRMWARE(QCA6174_HW_2_1_FW_DIR "/" ATH10K_FW_API4_FILE);
+MODULE_FIRMWARE(QCA6174_HW_2_1_FW_DIR "/" ATH10K_FW_API5_FILE);
+MODULE_FIRMWARE(QCA6174_HW_2_1_FW_DIR "/" QCA6174_HW_2_1_BOARD_DATA_FILE);
+
+/* QCA6174 3.1 firmware files */
+MODULE_FIRMWARE(QCA6174_HW_3_0_FW_DIR "/" ATH10K_FW_API4_FILE);
+MODULE_FIRMWARE(QCA6174_HW_3_0_FW_DIR "/" ATH10K_FW_API5_FILE);
+MODULE_FIRMWARE(QCA6174_HW_3_0_FW_DIR "/" QCA6174_HW_3_0_BOARD_DATA_FILE);
diff --git a/drivers/net/wireless/ath/ath10k/pci.h b/drivers/net/wireless/ath/ath10k/pci.h
index bddf54320160..d7696ddc03c4 100644
--- a/drivers/net/wireless/ath/ath10k/pci.h
+++ b/drivers/net/wireless/ath/ath10k/pci.h
@@ -185,6 +185,41 @@ struct ath10k_pci {
 	/* Map CE id to ce_state */
 	struct ath10k_ce_pipe ce_states[CE_COUNT_MAX];
 	struct timer_list rx_post_retry;
+
+	/* Due to HW quirks it is recommended to disable ASPM during device
+	 * bootup. To do that the original PCI-E Link Control is stored before
+	 * device bootup is executed and re-programmed later.
+	 */
+	u16 link_ctl;
+
+	/* Protects ps_awake and ps_wake_refcount */
+	spinlock_t ps_lock;
+
+	/* The device has a special powersave-oriented register. When device is
+	 * considered asleep it drains less power and driver is forbidden from
+	 * accessing most MMIO registers. If host were to access them without
+	 * waking up the device might scribble over host memory or return
+	 * 0xdeadbeef readouts.
+	 */
+	unsigned long ps_wake_refcount;
+
+	/* Waking up takes some time (up to 2ms in some cases) so it can be bad
+	 * for latency. To mitigate this the device isn't immediately allowed
+	 * to sleep after all references are undone - instead there's a grace
+	 * period after which the powersave register is updated unless some
+	 * activity to/from device happened in the meantime.
+	 *
+	 * Also see comments on ATH10K_PCI_SLEEP_GRACE_PERIOD_MSEC.
+	 */
+	struct timer_list ps_timer;
+
+	/* MMIO registers are used to communicate with the device. With
+	 * intensive traffic accessing powersave register would be a bit
+	 * wasteful overhead and would needlessly stall CPU. It is far more
+	 * efficient to rely on a variable in RAM and update it only upon
+	 * powersave register state changes.
+	 */
+	bool ps_awake;
 };
 
 static inline struct ath10k_pci *ath10k_pci_priv(struct ath10k *ar)
@@ -209,61 +244,25 @@ static inline struct ath10k_pci *ath10k_pci_priv(struct ath10k *ar)
  * for this device; but that's not guaranteed.
  */
 #define TARG_CPU_SPACE_TO_CE_SPACE(ar, pci_addr, addr)			\
-	(((ioread32((pci_addr)+(SOC_CORE_BASE_ADDRESS|			\
+	(((ath10k_pci_read32(ar, (SOC_CORE_BASE_ADDRESS |		\
 	  CORE_CTRL_ADDRESS)) & 0x7ff) << 21) |				\
 	 0x100000 | ((addr) & 0xfffff))
 
 /* Wait up to this many Ms for a Diagnostic Access CE operation to complete */
 #define DIAG_ACCESS_CE_TIMEOUT_MS 10
 
-/* Target exposes its registers for direct access. However before host can
- * access them it needs to make sure the target is awake (ath10k_pci_wake,
- * ath10k_pci_wake_wait, ath10k_pci_is_awake). Once target is awake it won't go
- * to sleep unless host tells it to (ath10k_pci_sleep).
- *
- * If host tries to access target registers without waking it up it can
- * scribble over host memory.
- *
- * If target is asleep waking it up may take up to even 2ms.
- */
-
-static inline void ath10k_pci_write32(struct ath10k *ar, u32 offset,
-				      u32 value)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-
-	iowrite32(value, ar_pci->mem + offset);
-}
-
-static inline u32 ath10k_pci_read32(struct ath10k *ar, u32 offset)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-
-	return ioread32(ar_pci->mem + offset);
-}
-
-static inline u32 ath10k_pci_soc_read32(struct ath10k *ar, u32 addr)
-{
-	return ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS + addr);
-}
-
-static inline void ath10k_pci_soc_write32(struct ath10k *ar, u32 addr, u32 val)
-{
-	ath10k_pci_write32(ar, RTC_SOC_BASE_ADDRESS + addr, val);
-}
-
-static inline u32 ath10k_pci_reg_read32(struct ath10k *ar, u32 addr)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-
-	return ioread32(ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS + addr);
-}
+void ath10k_pci_write32(struct ath10k *ar, u32 offset, u32 value);
+void ath10k_pci_soc_write32(struct ath10k *ar, u32 addr, u32 val);
+void ath10k_pci_reg_write32(struct ath10k *ar, u32 addr, u32 val);
 
-static inline void ath10k_pci_reg_write32(struct ath10k *ar, u32 addr, u32 val)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+u32 ath10k_pci_read32(struct ath10k *ar, u32 offset);
+u32 ath10k_pci_soc_read32(struct ath10k *ar, u32 addr);
+u32 ath10k_pci_reg_read32(struct ath10k *ar, u32 addr);
 
-	iowrite32(val, ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS + addr);
-}
+/* QCA6174 is known to have Tx/Rx issues when SOC_WAKE register is poked too
+ * frequently. To avoid this put SoC to sleep after a very conservative grace
+ * period. Adjust with great care.
+ */
+#define ATH10K_PCI_SLEEP_GRACE_PERIOD_MSEC 60
 
 #endif /* _PCI_H_ */
diff --git a/drivers/net/wireless/ath/ath10k/rx_desc.h b/drivers/net/wireless/ath/ath10k/rx_desc.h
index e9cc7787bf5f..492b5a5af434 100644
--- a/drivers/net/wireless/ath/ath10k/rx_desc.h
+++ b/drivers/net/wireless/ath/ath10k/rx_desc.h
@@ -661,6 +661,28 @@ struct rx_msdu_end {
 #define RX_PPDU_START_INFO5_SERVICE_MASK 0x0000ffff
 #define RX_PPDU_START_INFO5_SERVICE_LSB  0
 
+/* No idea what this flag means. It seems to be always set in rate. */
+#define RX_PPDU_START_RATE_FLAG BIT(3)
+
+enum rx_ppdu_start_rate {
+	RX_PPDU_START_RATE_OFDM_48M = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_OFDM_48M,
+	RX_PPDU_START_RATE_OFDM_24M = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_OFDM_24M,
+	RX_PPDU_START_RATE_OFDM_12M = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_OFDM_12M,
+	RX_PPDU_START_RATE_OFDM_6M  = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_OFDM_6M,
+	RX_PPDU_START_RATE_OFDM_54M = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_OFDM_54M,
+	RX_PPDU_START_RATE_OFDM_36M = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_OFDM_36M,
+	RX_PPDU_START_RATE_OFDM_18M = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_OFDM_18M,
+	RX_PPDU_START_RATE_OFDM_9M  = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_OFDM_9M,
+
+	RX_PPDU_START_RATE_CCK_LP_11M  = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_CCK_LP_11M,
+	RX_PPDU_START_RATE_CCK_LP_5_5M = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_CCK_LP_5_5M,
+	RX_PPDU_START_RATE_CCK_LP_2M   = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_CCK_LP_2M,
+	RX_PPDU_START_RATE_CCK_LP_1M   = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_CCK_LP_1M,
+	RX_PPDU_START_RATE_CCK_SP_11M  = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_CCK_SP_11M,
+	RX_PPDU_START_RATE_CCK_SP_5_5M = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_CCK_SP_5_5M,
+	RX_PPDU_START_RATE_CCK_SP_2M   = RX_PPDU_START_RATE_FLAG | ATH10K_HW_RATE_CCK_SP_2M,
+};
+
 struct rx_ppdu_start {
 	struct {
 		u8 pri20_mhz;
diff --git a/drivers/net/wireless/ath/ath10k/spectral.c b/drivers/net/wireless/ath/ath10k/spectral.c
index d22addf6118b..8dcd424aa502 100644
--- a/drivers/net/wireless/ath/ath10k/spectral.c
+++ b/drivers/net/wireless/ath/ath10k/spectral.c
@@ -519,9 +519,12 @@ int ath10k_spectral_vif_stop(struct ath10k_vif *arvif)
 
 int ath10k_spectral_create(struct ath10k *ar)
 {
+	/* The buffer size covers whole channels in dual bands up to 128 bins.
+	 * Scan with bigger than 128 bins needs to be run on single band each.
+	 */
 	ar->spectral.rfs_chan_spec_scan = relay_open("spectral_scan",
 						     ar->debug.debugfs_phy,
-						     1024, 256,
+						     1140, 2500,
 						     &rfs_spec_scan_cb, NULL);
 	debugfs_create_file("spectral_scan_ctl",
 			    S_IRUSR | S_IWUSR,
diff --git a/drivers/net/wireless/ath/ath10k/thermal.c b/drivers/net/wireless/ath/ath10k/thermal.c
index aede750809fe..1a899d70dc5d 100644
--- a/drivers/net/wireless/ath/ath10k/thermal.c
+++ b/drivers/net/wireless/ath/ath10k/thermal.c
@@ -23,102 +23,50 @@
 #include "debug.h"
 #include "wmi-ops.h"
 
-static int ath10k_thermal_get_active_vifs(struct ath10k *ar,
-					  enum wmi_vdev_type type)
+static int
+ath10k_thermal_get_max_throttle_state(struct thermal_cooling_device *cdev,
+				      unsigned long *state)
 {
-	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;
+	*state = ATH10K_THERMAL_THROTTLE_MAX;
 
 	return 0;
 }
 
-static int ath10k_thermal_get_cur_dutycycle(struct thermal_cooling_device *cdev,
-					    unsigned long *state)
+static int
+ath10k_thermal_get_cur_throttle_state(struct thermal_cooling_device *cdev,
+				      unsigned long *state)
 {
 	struct ath10k *ar = cdev->devdata;
 
 	mutex_lock(&ar->conf_mutex);
-	*state = ar->thermal.duty_cycle;
+	*state = ar->thermal.throttle_state;
 	mutex_unlock(&ar->conf_mutex);
 
 	return 0;
 }
 
-static int ath10k_thermal_set_cur_dutycycle(struct thermal_cooling_device *cdev,
-					    unsigned long duty_cycle)
+static int
+ath10k_thermal_set_cur_throttle_state(struct thermal_cooling_device *cdev,
+				      unsigned long throttle_state)
 {
 	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;
+	if (throttle_state > ATH10K_THERMAL_THROTTLE_MAX) {
+		ath10k_warn(ar, "throttle state %ld is exceeding the limit %d\n",
+			    throttle_state, ATH10K_THERMAL_THROTTLE_MAX);
+		return -EINVAL;
 	}
-	ar->thermal.duty_cycle = duty_cycle;
-out:
+	mutex_lock(&ar->conf_mutex);
+	ar->thermal.throttle_state = throttle_state;
+	ath10k_thermal_set_throttling(ar);
 	mutex_unlock(&ar->conf_mutex);
-	return ret;
+	return 0;
 }
 
 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,
+	.get_max_state = ath10k_thermal_get_max_throttle_state,
+	.get_cur_state = ath10k_thermal_get_cur_throttle_state,
+	.set_cur_state = ath10k_thermal_set_cur_throttle_state,
 };
 
 static ssize_t ath10k_thermal_show_temp(struct device *dev,
@@ -127,6 +75,7 @@ static ssize_t ath10k_thermal_show_temp(struct device *dev,
 {
 	struct ath10k *ar = dev_get_drvdata(dev);
 	int ret, temperature;
+	unsigned long time_left;
 
 	mutex_lock(&ar->conf_mutex);
 
@@ -148,9 +97,9 @@ static ssize_t ath10k_thermal_show_temp(struct device *dev,
 		goto out;
 	}
 
-	ret = wait_for_completion_timeout(&ar->thermal.wmi_sync,
-					  ATH10K_THERMAL_SYNC_TIMEOUT_HZ);
-	if (ret == 0) {
+	time_left = wait_for_completion_timeout(&ar->thermal.wmi_sync,
+						ATH10K_THERMAL_SYNC_TIMEOUT_HZ);
+	if (!time_left) {
 		ath10k_warn(ar, "failed to synchronize thermal read\n");
 		ret = -ETIMEDOUT;
 		goto out;
@@ -184,6 +133,32 @@ static struct attribute *ath10k_hwmon_attrs[] = {
 };
 ATTRIBUTE_GROUPS(ath10k_hwmon);
 
+void ath10k_thermal_set_throttling(struct ath10k *ar)
+{
+	u32 period, duration, enabled;
+	int ret;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	if (!ar->wmi.ops->gen_pdev_set_quiet_mode)
+		return;
+
+	if (ar->state != ATH10K_STATE_ON)
+		return;
+
+	period = ar->thermal.quiet_period;
+	duration = (period * ar->thermal.throttle_state) / 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);
+	}
+}
+
 int ath10k_thermal_register(struct ath10k *ar)
 {
 	struct thermal_cooling_device *cdev;
@@ -202,11 +177,12 @@ int ath10k_thermal_register(struct ath10k *ar)
 	ret = sysfs_create_link(&ar->dev->kobj, &cdev->device.kobj,
 				"cooling_device");
 	if (ret) {
-		ath10k_err(ar, "failed to create thermal symlink\n");
+		ath10k_err(ar, "failed to create cooling device symlink\n");
 		goto err_cooling_destroy;
 	}
 
 	ar->thermal.cdev = cdev;
+	ar->thermal.quiet_period = ATH10K_QUIET_PERIOD_DEFAULT;
 
 	/* Do not register hwmon device when temperature reading is not
 	 * supported by firmware
@@ -231,7 +207,7 @@ int ath10k_thermal_register(struct ath10k *ar)
 	return 0;
 
 err_remove_link:
-	sysfs_remove_link(&ar->dev->kobj, "thermal_sensor");
+	sysfs_remove_link(&ar->dev->kobj, "cooling_device");
 err_cooling_destroy:
 	thermal_cooling_device_unregister(cdev);
 	return ret;
diff --git a/drivers/net/wireless/ath/ath10k/thermal.h b/drivers/net/wireless/ath/ath10k/thermal.h
index bccc17ae0fde..b610ea5caae8 100644
--- a/drivers/net/wireless/ath/ath10k/thermal.h
+++ b/drivers/net/wireless/ath/ath10k/thermal.h
@@ -19,16 +19,17 @@
 #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)
+#define ATH10K_THERMAL_THROTTLE_MAX     100
 
 struct ath10k_thermal {
 	struct thermal_cooling_device *cdev;
 	struct completion wmi_sync;
 
 	/* protected by conf_mutex */
-	u32 duty_cycle;
+	u32 throttle_state;
+	u32 quiet_period;
 	/* temperature value in Celcius degree
 	 * protected by data_lock
 	 */
@@ -39,6 +40,7 @@ struct ath10k_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);
+void ath10k_thermal_set_throttling(struct ath10k *ar);
 #else
 static inline int ath10k_thermal_register(struct ath10k *ar)
 {
@@ -54,5 +56,9 @@ static inline void ath10k_thermal_event_temperature(struct ath10k *ar,
 {
 }
 
+static inline void ath10k_thermal_set_throttling(struct ath10k *ar)
+{
+}
+
 #endif
 #endif /* _THERMAL_ */
diff --git a/drivers/net/wireless/ath/ath10k/trace.h b/drivers/net/wireless/ath/ath10k/trace.h
index 5407887380ab..71bdb368813d 100644
--- a/drivers/net/wireless/ath/ath10k/trace.h
+++ b/drivers/net/wireless/ath/ath10k/trace.h
@@ -21,11 +21,16 @@
 #include "core.h"
 
 #if !defined(_TRACE_H_)
-static inline u32 ath10k_frm_hdr_len(const void *buf)
+static inline u32 ath10k_frm_hdr_len(const void *buf, size_t len)
 {
 	const struct ieee80211_hdr *hdr = buf;
 
-	return ieee80211_hdrlen(hdr->frame_control);
+	/* In some rare cases (e.g. fcs error) device reports frame buffer
+	 * shorter than what frame header implies (e.g. len = 0). The buffer
+	 * can still be accessed so do a simple min() to guarantee caller
+	 * doesn't get value greater than len.
+	 */
+	return min_t(u32, len, ieee80211_hdrlen(hdr->frame_control));
 }
 #endif
 
@@ -46,7 +51,7 @@ static inline void trace_ ## name(proto) {}
 #undef TRACE_SYSTEM
 #define TRACE_SYSTEM ath10k
 
-#define ATH10K_MSG_MAX 200
+#define ATH10K_MSG_MAX 400
 
 DECLARE_EVENT_CLASS(ath10k_log_event,
 	TP_PROTO(struct ath10k *ar, struct va_format *vaf),
@@ -360,13 +365,13 @@ DECLARE_EVENT_CLASS(ath10k_hdr_event,
 		__string(device, dev_name(ar->dev))
 		__string(driver, dev_driver_string(ar->dev))
 		__field(size_t, len)
-		__dynamic_array(u8, data, ath10k_frm_hdr_len(data))
+		__dynamic_array(u8, data, ath10k_frm_hdr_len(data, len))
 	),
 
 	TP_fast_assign(
 		__assign_str(device, dev_name(ar->dev));
 		__assign_str(driver, dev_driver_string(ar->dev));
-		__entry->len = ath10k_frm_hdr_len(data);
+		__entry->len = ath10k_frm_hdr_len(data, len);
 		memcpy(__get_dynamic_array(data), data, __entry->len);
 	),
 
@@ -387,15 +392,16 @@ DECLARE_EVENT_CLASS(ath10k_payload_event,
 		__string(device, dev_name(ar->dev))
 		__string(driver, dev_driver_string(ar->dev))
 		__field(size_t, len)
-		__dynamic_array(u8, payload, (len - ath10k_frm_hdr_len(data)))
+		__dynamic_array(u8, payload, (len -
+					      ath10k_frm_hdr_len(data, len)))
 	),
 
 	TP_fast_assign(
 		__assign_str(device, dev_name(ar->dev));
 		__assign_str(driver, dev_driver_string(ar->dev));
-		__entry->len = len - ath10k_frm_hdr_len(data);
+		__entry->len = len - ath10k_frm_hdr_len(data, len);
 		memcpy(__get_dynamic_array(payload),
-		       data + ath10k_frm_hdr_len(data), __entry->len);
+		       data + ath10k_frm_hdr_len(data, len), __entry->len);
 	),
 
 	TP_printk(
diff --git a/drivers/net/wireless/ath/ath10k/txrx.c b/drivers/net/wireless/ath/ath10k/txrx.c
index 3f00cec8aef5..826500bb2b1b 100644
--- a/drivers/net/wireless/ath/ath10k/txrx.c
+++ b/drivers/net/wireless/ath/ath10k/txrx.c
@@ -55,8 +55,10 @@ void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 
 	lockdep_assert_held(&htt->tx_lock);
 
-	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt tx completion msdu_id %u discard %d no_ack %d\n",
-		   tx_done->msdu_id, !!tx_done->discard, !!tx_done->no_ack);
+	ath10k_dbg(ar, ATH10K_DBG_HTT,
+		   "htt tx completion msdu_id %u discard %d no_ack %d success %d\n",
+		   tx_done->msdu_id, !!tx_done->discard,
+		   !!tx_done->no_ack, !!tx_done->success);
 
 	if (tx_done->msdu_id >= htt->max_num_pending_tx) {
 		ath10k_warn(ar, "warning: msdu_id %d too big, ignoring\n",
@@ -97,6 +99,9 @@ void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 	if (tx_done->no_ack)
 		info->flags &= ~IEEE80211_TX_STAT_ACK;
 
+	if (tx_done->success && (info->flags & IEEE80211_TX_CTL_NO_ACK))
+		info->flags |= IEEE80211_TX_STAT_NOACK_TRANSMITTED;
+
 	ieee80211_tx_status(htt->ar->hw, msdu);
 	/* we do not own the msdu anymore */
 
diff --git a/drivers/net/wireless/ath/ath10k/wmi-ops.h b/drivers/net/wireless/ath/ath10k/wmi-ops.h
index c8b64e7a6089..47fe2e756bec 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-ops.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-ops.h
@@ -45,6 +45,10 @@ struct wmi_ops {
 			struct wmi_rdy_ev_arg *arg);
 	int (*pull_fw_stats)(struct ath10k *ar, struct sk_buff *skb,
 			     struct ath10k_fw_stats *stats);
+	int (*pull_roam_ev)(struct ath10k *ar, struct sk_buff *skb,
+			    struct wmi_roam_ev_arg *arg);
+	int (*pull_wow_event)(struct ath10k *ar, struct sk_buff *skb,
+			      struct wmi_wow_ev_arg *arg);
 
 	struct sk_buff *(*gen_pdev_suspend)(struct ath10k *ar, u32 suspend_opt);
 	struct sk_buff *(*gen_pdev_resume)(struct ath10k *ar);
@@ -81,7 +85,8 @@ struct wmi_ops {
 	struct sk_buff *(*gen_vdev_wmm_conf)(struct ath10k *ar, u32 vdev_id,
 					     const struct wmi_wmm_params_all_arg *arg);
 	struct sk_buff *(*gen_peer_create)(struct ath10k *ar, u32 vdev_id,
-					   const u8 peer_addr[ETH_ALEN]);
+					   const u8 peer_addr[ETH_ALEN],
+					   enum wmi_peer_type peer_type);
 	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,
@@ -148,6 +153,27 @@ struct wmi_ops {
 					      u32 num_ac);
 	struct sk_buff *(*gen_sta_keepalive)(struct ath10k *ar,
 					     const struct wmi_sta_keepalive_arg *arg);
+	struct sk_buff *(*gen_wow_enable)(struct ath10k *ar);
+	struct sk_buff *(*gen_wow_add_wakeup_event)(struct ath10k *ar, u32 vdev_id,
+						    enum wmi_wow_wakeup_event event,
+						    u32 enable);
+	struct sk_buff *(*gen_wow_host_wakeup_ind)(struct ath10k *ar);
+	struct sk_buff *(*gen_wow_add_pattern)(struct ath10k *ar, u32 vdev_id,
+					       u32 pattern_id,
+					       const u8 *pattern,
+					       const u8 *mask,
+					       int pattern_len,
+					       int pattern_offset);
+	struct sk_buff *(*gen_wow_del_pattern)(struct ath10k *ar, u32 vdev_id,
+					       u32 pattern_id);
+	struct sk_buff *(*gen_update_fw_tdls_state)(struct ath10k *ar,
+						    u32 vdev_id,
+						    enum wmi_tdls_state state);
+	struct sk_buff *(*gen_tdls_peer_update)(struct ath10k *ar,
+						const struct wmi_tdls_peer_update_cmd_arg *arg,
+						const struct wmi_tdls_peer_capab_arg *cap,
+						const struct wmi_channel_arg *chan);
+	struct sk_buff *(*gen_adaptive_qcs)(struct ath10k *ar, bool enable);
 };
 
 int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id);
@@ -274,6 +300,26 @@ ath10k_wmi_pull_fw_stats(struct ath10k *ar, struct sk_buff *skb,
 }
 
 static inline int
+ath10k_wmi_pull_roam_ev(struct ath10k *ar, struct sk_buff *skb,
+			struct wmi_roam_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_roam_ev)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_roam_ev(ar, skb, arg);
+}
+
+static inline int
+ath10k_wmi_pull_wow_event(struct ath10k *ar, struct sk_buff *skb,
+			  struct wmi_wow_ev_arg *arg)
+{
+	if (!ar->wmi.ops->pull_wow_event)
+		return -EOPNOTSUPP;
+
+	return ar->wmi.ops->pull_wow_event(ar, skb, arg);
+}
+
+static inline int
 ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *msdu)
 {
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(msdu);
@@ -624,14 +670,15 @@ ath10k_wmi_vdev_wmm_conf(struct ath10k *ar, u32 vdev_id,
 
 static inline int
 ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
-		       const u8 peer_addr[ETH_ALEN])
+		       const u8 peer_addr[ETH_ALEN],
+		       enum wmi_peer_type peer_type)
 {
 	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);
+	skb = ar->wmi.ops->gen_peer_create(ar, vdev_id, peer_addr, peer_type);
 	if (IS_ERR(skb))
 		return PTR_ERR(skb);
 
@@ -1060,4 +1107,145 @@ ath10k_wmi_sta_keepalive(struct ath10k *ar,
 	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
 }
 
+static inline int
+ath10k_wmi_wow_enable(struct ath10k *ar)
+{
+	struct sk_buff *skb;
+	u32 cmd_id;
+
+	if (!ar->wmi.ops->gen_wow_enable)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_wow_enable(ar);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	cmd_id = ar->wmi.cmd->wow_enable_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
+}
+
+static inline int
+ath10k_wmi_wow_add_wakeup_event(struct ath10k *ar, u32 vdev_id,
+				enum wmi_wow_wakeup_event event,
+				u32 enable)
+{
+	struct sk_buff *skb;
+	u32 cmd_id;
+
+	if (!ar->wmi.ops->gen_wow_add_wakeup_event)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_wow_add_wakeup_event(ar, vdev_id, event, enable);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	cmd_id = ar->wmi.cmd->wow_enable_disable_wake_event_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
+}
+
+static inline int
+ath10k_wmi_wow_host_wakeup_ind(struct ath10k *ar)
+{
+	struct sk_buff *skb;
+	u32 cmd_id;
+
+	if (!ar->wmi.ops->gen_wow_host_wakeup_ind)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_wow_host_wakeup_ind(ar);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	cmd_id = ar->wmi.cmd->wow_hostwakeup_from_sleep_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
+}
+
+static inline int
+ath10k_wmi_wow_add_pattern(struct ath10k *ar, u32 vdev_id, u32 pattern_id,
+			   const u8 *pattern, const u8 *mask,
+			   int pattern_len, int pattern_offset)
+{
+	struct sk_buff *skb;
+	u32 cmd_id;
+
+	if (!ar->wmi.ops->gen_wow_add_pattern)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_wow_add_pattern(ar, vdev_id, pattern_id,
+					       pattern, mask, pattern_len,
+					       pattern_offset);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	cmd_id = ar->wmi.cmd->wow_add_wake_pattern_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
+}
+
+static inline int
+ath10k_wmi_wow_del_pattern(struct ath10k *ar, u32 vdev_id, u32 pattern_id)
+{
+	struct sk_buff *skb;
+	u32 cmd_id;
+
+	if (!ar->wmi.ops->gen_wow_del_pattern)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_wow_del_pattern(ar, vdev_id, pattern_id);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	cmd_id = ar->wmi.cmd->wow_del_wake_pattern_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmd_id);
+}
+
+static inline int
+ath10k_wmi_update_fw_tdls_state(struct ath10k *ar, u32 vdev_id,
+				enum wmi_tdls_state state)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_update_fw_tdls_state)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_update_fw_tdls_state(ar, vdev_id, state);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->tdls_set_state_cmdid);
+}
+
+static inline int
+ath10k_wmi_tdls_peer_update(struct ath10k *ar,
+			    const struct wmi_tdls_peer_update_cmd_arg *arg,
+			    const struct wmi_tdls_peer_capab_arg *cap,
+			    const struct wmi_channel_arg *chan)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_tdls_peer_update)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_tdls_peer_update(ar, arg, cap, chan);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->tdls_peer_update_cmdid);
+}
+
+static inline int
+ath10k_wmi_adaptive_qcs(struct ath10k *ar, bool enable)
+{
+	struct sk_buff *skb;
+
+	if (!ar->wmi.ops->gen_adaptive_qcs)
+		return -EOPNOTSUPP;
+
+	skb = ar->wmi.ops->gen_adaptive_qcs(ar, enable);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->adaptive_qcs_cmdid);
+}
+
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.c b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
index ee0c5f602e29..8fdba3865c96 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.c
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
@@ -16,10 +16,13 @@
  */
 #include "core.h"
 #include "debug.h"
+#include "mac.h"
 #include "hw.h"
+#include "mac.h"
 #include "wmi.h"
 #include "wmi-ops.h"
 #include "wmi-tlv.h"
+#include "p2p.h"
 
 /***************/
 /* TLV helpers */
@@ -31,9 +34,9 @@ struct wmi_tlv_policy {
 
 static const struct wmi_tlv_policy wmi_tlv_policies[] = {
 	[WMI_TLV_TAG_ARRAY_BYTE]
-		= { .min_len = sizeof(u8) },
+		= { .min_len = 0 },
 	[WMI_TLV_TAG_ARRAY_UINT32]
-		= { .min_len = sizeof(u32) },
+		= { .min_len = 0 },
 	[WMI_TLV_TAG_STRUCT_SCAN_EVENT]
 		= { .min_len = sizeof(struct wmi_scan_event) },
 	[WMI_TLV_TAG_STRUCT_MGMT_RX_HDR]
@@ -62,6 +65,14 @@ static const struct wmi_tlv_policy wmi_tlv_policies[] = {
 		= { .min_len = sizeof(struct wmi_tlv_bcn_tx_status_ev) },
 	[WMI_TLV_TAG_STRUCT_DIAG_DATA_CONTAINER_EVENT]
 		= { .min_len = sizeof(struct wmi_tlv_diag_data_ev) },
+	[WMI_TLV_TAG_STRUCT_P2P_NOA_EVENT]
+		= { .min_len = sizeof(struct wmi_tlv_p2p_noa_ev) },
+	[WMI_TLV_TAG_STRUCT_ROAM_EVENT]
+		= { .min_len = sizeof(struct wmi_tlv_roam_ev) },
+	[WMI_TLV_TAG_STRUCT_WOW_EVENT_INFO]
+		= { .min_len = sizeof(struct wmi_tlv_wow_event_info) },
+	[WMI_TLV_TAG_STRUCT_TX_PAUSE_EVENT]
+		= { .min_len = sizeof(struct wmi_tlv_tx_pause_ev) },
 };
 
 static int
@@ -168,6 +179,7 @@ static int ath10k_wmi_tlv_event_bcn_tx_status(struct ath10k *ar,
 {
 	const void **tb;
 	const struct wmi_tlv_bcn_tx_status_ev *ev;
+	struct ath10k_vif *arvif;
 	u32 vdev_id, tx_status;
 	int ret;
 
@@ -201,6 +213,10 @@ static int ath10k_wmi_tlv_event_bcn_tx_status(struct ath10k *ar,
 		break;
 	}
 
+	arvif = ath10k_get_arvif(ar, vdev_id);
+	if (arvif && arvif->is_up && arvif->vif->csa_active)
+		ieee80211_queue_work(ar->hw, &arvif->ap_csa_work);
+
 	kfree(tb);
 	return 0;
 }
@@ -296,6 +312,83 @@ static int ath10k_wmi_tlv_event_diag(struct ath10k *ar,
 	return 0;
 }
 
+static int ath10k_wmi_tlv_event_p2p_noa(struct ath10k *ar,
+					struct sk_buff *skb)
+{
+	const void **tb;
+	const struct wmi_tlv_p2p_noa_ev *ev;
+	const struct wmi_p2p_noa_info *noa;
+	int ret, vdev_id;
+
+	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_P2P_NOA_EVENT];
+	noa = tb[WMI_TLV_TAG_STRUCT_P2P_NOA_INFO];
+
+	if (!ev || !noa) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	vdev_id = __le32_to_cpu(ev->vdev_id);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi tlv p2p noa vdev_id %i descriptors %hhu\n",
+		   vdev_id, noa->num_descriptors);
+
+	ath10k_p2p_noa_update_by_vdev_id(ar, vdev_id, noa);
+	kfree(tb);
+	return 0;
+}
+
+static int ath10k_wmi_tlv_event_tx_pause(struct ath10k *ar,
+					 struct sk_buff *skb)
+{
+	const void **tb;
+	const struct wmi_tlv_tx_pause_ev *ev;
+	int ret, vdev_id;
+	u32 pause_id, action, vdev_map, peer_id, tid_map;
+
+	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_TX_PAUSE_EVENT];
+	if (!ev) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	pause_id = __le32_to_cpu(ev->pause_id);
+	action = __le32_to_cpu(ev->action);
+	vdev_map = __le32_to_cpu(ev->vdev_map);
+	peer_id = __le32_to_cpu(ev->peer_id);
+	tid_map = __le32_to_cpu(ev->tid_map);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi tlv tx pause pause_id %u action %u vdev_map 0x%08x peer_id %u tid_map 0x%08x\n",
+		   pause_id, action, vdev_map, peer_id, tid_map);
+
+	for (vdev_id = 0; vdev_map; vdev_id++) {
+		if (!(vdev_map & BIT(vdev_id)))
+			continue;
+
+		vdev_map &= ~BIT(vdev_id);
+		ath10k_mac_handle_tx_pause(ar, vdev_id, pause_id, action);
+	}
+
+	kfree(tb);
+	return 0;
+}
+
 /***********/
 /* TLV ops */
 /***********/
@@ -309,7 +402,7 @@ static void ath10k_wmi_tlv_op_rx(struct ath10k *ar, struct sk_buff *skb)
 	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;
+		goto out;
 
 	trace_ath10k_wmi_event(ar, id, skb->data, skb->len);
 
@@ -417,11 +510,18 @@ static void ath10k_wmi_tlv_op_rx(struct ath10k *ar, struct sk_buff *skb)
 	case WMI_TLV_DIAG_EVENTID:
 		ath10k_wmi_tlv_event_diag(ar, skb);
 		break;
+	case WMI_TLV_P2P_NOA_EVENTID:
+		ath10k_wmi_tlv_event_p2p_noa(ar, skb);
+		break;
+	case WMI_TLV_TX_PAUSE_EVENTID:
+		ath10k_wmi_tlv_event_tx_pause(ar, skb);
+		break;
 	default:
 		ath10k_warn(ar, "Unknown eventid: %d\n", id);
 		break;
 	}
 
+out:
 	dev_kfree_skb(skb);
 }
 
@@ -1012,6 +1112,65 @@ static int ath10k_wmi_tlv_op_pull_fw_stats(struct ath10k *ar,
 	return 0;
 }
 
+static int ath10k_wmi_tlv_op_pull_roam_ev(struct ath10k *ar,
+					  struct sk_buff *skb,
+					  struct wmi_roam_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_tlv_roam_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_ROAM_EVENT];
+	if (!ev) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	arg->vdev_id = ev->vdev_id;
+	arg->reason = ev->reason;
+	arg->rssi = ev->rssi;
+
+	kfree(tb);
+	return 0;
+}
+
+static int
+ath10k_wmi_tlv_op_pull_wow_ev(struct ath10k *ar, struct sk_buff *skb,
+			      struct wmi_wow_ev_arg *arg)
+{
+	const void **tb;
+	const struct wmi_tlv_wow_event_info *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_WOW_EVENT_INFO];
+	if (!ev) {
+		kfree(tb);
+		return -EPROTO;
+	}
+
+	arg->vdev_id = __le32_to_cpu(ev->vdev_id);
+	arg->flag = __le32_to_cpu(ev->flag);
+	arg->wake_reason = __le32_to_cpu(ev->wake_reason);
+	arg->data_len = __le32_to_cpu(ev->data_len);
+
+	kfree(tb);
+	return 0;
+}
+
 static struct sk_buff *
 ath10k_wmi_tlv_op_gen_pdev_suspend(struct ath10k *ar, u32 opt)
 {
@@ -1160,8 +1319,8 @@ static struct sk_buff *ath10k_wmi_tlv_op_gen_init(struct ath10k *ar)
 	cfg->num_peers = __cpu_to_le32(TARGET_TLV_NUM_PEERS);
 
 	if (test_bit(WMI_SERVICE_RX_FULL_REORDER, ar->wmi.svc_map)) {
-		cfg->num_offload_peers = __cpu_to_le32(3);
-		cfg->num_offload_reorder_bufs = __cpu_to_le32(3);
+		cfg->num_offload_peers = __cpu_to_le32(TARGET_TLV_NUM_VDEVS);
+		cfg->num_offload_reorder_bufs = __cpu_to_le32(TARGET_TLV_NUM_VDEVS);
 	} else {
 		cfg->num_offload_peers = __cpu_to_le32(0);
 		cfg->num_offload_reorder_bufs = __cpu_to_le32(0);
@@ -1178,8 +1337,8 @@ static struct sk_buff *ath10k_wmi_tlv_op_gen_init(struct ath10k *ar)
 	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->bmiss_offload_max_vdev = __cpu_to_le32(TARGET_TLV_NUM_VDEVS);
+	cfg->roam_offload_max_vdev = __cpu_to_le32(TARGET_TLV_NUM_VDEVS);
 	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);
@@ -1193,11 +1352,11 @@ static struct sk_buff *ath10k_wmi_tlv_op_gen_init(struct ath10k *ar)
 	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_vdevs = __cpu_to_le32(TARGET_TLV_NUM_TDLS_VDEVS);
 	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_wow_filters = __cpu_to_le32(ar->wow.max_num_patterns);
 	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);
@@ -1248,7 +1407,7 @@ ath10k_wmi_tlv_op_gen_start_scan(struct ath10k *ar,
 	cmd = (void *)tlv->value;
 
 	ath10k_wmi_put_start_scan_common(&cmd->common, arg);
-	cmd->burst_duration_ms = __cpu_to_le32(0);
+	cmd->burst_duration_ms = __cpu_to_le32(arg->burst_duration_ms);
 	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);
@@ -1408,8 +1567,6 @@ ath10k_wmi_tlv_op_gen_vdev_start(struct ath10k *ar,
 	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)))
@@ -1782,7 +1939,8 @@ ath10k_wmi_tlv_op_gen_sta_keepalive(struct ath10k *ar,
 
 static struct sk_buff *
 ath10k_wmi_tlv_op_gen_peer_create(struct ath10k *ar, u32 vdev_id,
-				  const u8 peer_addr[ETH_ALEN])
+				  const u8 peer_addr[ETH_ALEN],
+				  enum wmi_peer_type peer_type)
 {
 	struct wmi_tlv_peer_create_cmd *cmd;
 	struct wmi_tlv *tlv;
@@ -1797,7 +1955,7 @@ ath10k_wmi_tlv_op_gen_peer_create(struct ath10k *ar, u32 vdev_id,
 	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 */
+	cmd->peer_type = __cpu_to_le32(peer_type);
 	ether_addr_copy(cmd->peer_addr.addr, peer_addr);
 
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv peer create\n");
@@ -2027,7 +2185,7 @@ ath10k_wmi_tlv_op_gen_set_ap_ps(struct ath10k *ar, u32 vdev_id, const u8 *mac,
 	if (!mac)
 		return ERR_PTR(-EINVAL);
 
-	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*tlv) + sizeof(*cmd));
 	if (!skb)
 		return ERR_PTR(-ENOMEM);
 
@@ -2485,6 +2643,387 @@ ath10k_wmi_tlv_op_gen_p2p_go_bcn_ie(struct ath10k *ar, u32 vdev_id,
 	return skb;
 }
 
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_update_fw_tdls_state(struct ath10k *ar, u32 vdev_id,
+					   enum wmi_tdls_state state)
+{
+	struct wmi_tdls_set_state_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	void *ptr;
+	size_t len;
+	/* Set to options from wmi_tlv_tdls_options,
+	 * for now none of them are enabled.
+	 */
+	u32 options = 0;
+
+	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_TDLS_SET_STATE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->state = __cpu_to_le32(state);
+	cmd->notification_interval_ms = __cpu_to_le32(5000);
+	cmd->tx_discovery_threshold = __cpu_to_le32(100);
+	cmd->tx_teardown_threshold = __cpu_to_le32(5);
+	cmd->rssi_teardown_threshold = __cpu_to_le32(-75);
+	cmd->rssi_delta = __cpu_to_le32(-20);
+	cmd->tdls_options = __cpu_to_le32(options);
+	cmd->tdls_peer_traffic_ind_window = __cpu_to_le32(2);
+	cmd->tdls_peer_traffic_response_timeout_ms = __cpu_to_le32(5000);
+	cmd->tdls_puapsd_mask = __cpu_to_le32(0xf);
+	cmd->tdls_puapsd_inactivity_time_ms = __cpu_to_le32(0);
+	cmd->tdls_puapsd_rx_frame_threshold = __cpu_to_le32(10);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv update fw tdls state %d for vdev %i\n",
+		   state, vdev_id);
+	return skb;
+}
+
+static u32 ath10k_wmi_tlv_prepare_peer_qos(u8 uapsd_queues, u8 sp)
+{
+	u32 peer_qos = 0;
+
+	if (uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VO)
+		peer_qos |= WMI_TLV_TDLS_PEER_QOS_AC_VO;
+	if (uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VI)
+		peer_qos |= WMI_TLV_TDLS_PEER_QOS_AC_VI;
+	if (uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_BK)
+		peer_qos |= WMI_TLV_TDLS_PEER_QOS_AC_BK;
+	if (uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_BE)
+		peer_qos |= WMI_TLV_TDLS_PEER_QOS_AC_BE;
+
+	peer_qos |= SM(sp, WMI_TLV_TDLS_PEER_SP);
+
+	return peer_qos;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_tdls_peer_update(struct ath10k *ar,
+				       const struct wmi_tdls_peer_update_cmd_arg *arg,
+				       const struct wmi_tdls_peer_capab_arg *cap,
+				       const struct wmi_channel_arg *chan_arg)
+{
+	struct wmi_tdls_peer_update_cmd *cmd;
+	struct wmi_tdls_peer_capab *peer_cap;
+	struct wmi_channel *chan;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	u32 peer_qos;
+	void *ptr;
+	int len;
+	int i;
+
+	len = sizeof(*tlv) + sizeof(*cmd) +
+	      sizeof(*tlv) + sizeof(*peer_cap) +
+	      sizeof(*tlv) + cap->peer_chan_len * sizeof(*chan);
+
+	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_TDLS_PEER_UPDATE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+
+	cmd = (void *)tlv->value;
+	cmd->vdev_id = __cpu_to_le32(arg->vdev_id);
+	ether_addr_copy(cmd->peer_macaddr.addr, arg->addr);
+	cmd->peer_state = __cpu_to_le32(arg->peer_state);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_TDLS_PEER_CAPABILITIES);
+	tlv->len = __cpu_to_le16(sizeof(*peer_cap));
+	peer_cap = (void *)tlv->value;
+	peer_qos = ath10k_wmi_tlv_prepare_peer_qos(cap->peer_uapsd_queues,
+						   cap->peer_max_sp);
+	peer_cap->peer_qos = __cpu_to_le32(peer_qos);
+	peer_cap->buff_sta_support = __cpu_to_le32(cap->buff_sta_support);
+	peer_cap->off_chan_support = __cpu_to_le32(cap->off_chan_support);
+	peer_cap->peer_curr_operclass = __cpu_to_le32(cap->peer_curr_operclass);
+	peer_cap->self_curr_operclass = __cpu_to_le32(cap->self_curr_operclass);
+	peer_cap->peer_chan_len = __cpu_to_le32(cap->peer_chan_len);
+	peer_cap->peer_operclass_len = __cpu_to_le32(cap->peer_operclass_len);
+
+	for (i = 0; i < WMI_TDLS_MAX_SUPP_OPER_CLASSES; i++)
+		peer_cap->peer_operclass[i] = cap->peer_operclass[i];
+
+	peer_cap->is_peer_responder = __cpu_to_le32(cap->is_peer_responder);
+	peer_cap->pref_offchan_num = __cpu_to_le32(cap->pref_offchan_num);
+	peer_cap->pref_offchan_bw = __cpu_to_le32(cap->pref_offchan_bw);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*peer_cap);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_STRUCT);
+	tlv->len = __cpu_to_le16(cap->peer_chan_len * sizeof(*chan));
+
+	ptr += sizeof(*tlv);
+
+	for (i = 0; i < cap->peer_chan_len; i++) {
+		tlv = ptr;
+		tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_CHANNEL);
+		tlv->len = __cpu_to_le16(sizeof(*chan));
+		chan = (void *)tlv->value;
+		ath10k_wmi_put_wmi_channel(chan, &chan_arg[i]);
+
+		ptr += sizeof(*tlv);
+		ptr += sizeof(*chan);
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi tlv tdls peer update vdev %i state %d n_chans %u\n",
+		   arg->vdev_id, arg->peer_state, cap->peer_chan_len);
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_wow_enable(struct ath10k *ar)
+{
+	struct wmi_tlv_wow_enable_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len;
+
+	len = sizeof(*tlv) + sizeof(*cmd);
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (struct wmi_tlv *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_WOW_ENABLE_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	cmd->enable = __cpu_to_le32(1);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv wow enable\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_wow_add_wakeup_event(struct ath10k *ar,
+					   u32 vdev_id,
+					   enum wmi_wow_wakeup_event event,
+					   u32 enable)
+{
+	struct wmi_tlv_wow_add_del_event_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len;
+
+	len = sizeof(*tlv) + sizeof(*cmd);
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (struct wmi_tlv *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_WOW_ADD_DEL_EVT_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->is_add = __cpu_to_le32(enable);
+	cmd->event_bitmap = __cpu_to_le32(1 << event);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv wow add wakeup event %s enable %d vdev_id %d\n",
+		   wow_wakeup_event(event), enable, vdev_id);
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_gen_wow_host_wakeup_ind(struct ath10k *ar)
+{
+	struct wmi_tlv_wow_host_wakeup_ind *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len;
+
+	len = sizeof(*tlv) + sizeof(*cmd);
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (struct wmi_tlv *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_WOW_HOSTWAKEUP_FROM_SLEEP_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv wow host wakeup ind\n");
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_wow_add_pattern(struct ath10k *ar, u32 vdev_id,
+				      u32 pattern_id, const u8 *pattern,
+				      const u8 *bitmask, int pattern_len,
+				      int pattern_offset)
+{
+	struct wmi_tlv_wow_add_pattern_cmd *cmd;
+	struct wmi_tlv_wow_bitmap_pattern *bitmap;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	void *ptr;
+	size_t len;
+
+	len = sizeof(*tlv) + sizeof(*cmd) +
+	      sizeof(*tlv) +			/* array struct */
+	      sizeof(*tlv) + sizeof(*bitmap) +  /* bitmap */
+	      sizeof(*tlv) +			/* empty ipv4 sync */
+	      sizeof(*tlv) +			/* empty ipv6 sync */
+	      sizeof(*tlv) +			/* empty magic */
+	      sizeof(*tlv) +			/* empty info timeout */
+	      sizeof(*tlv) + sizeof(u32);	/* ratelimit interval */
+
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	/* cmd */
+	ptr = (void *)skb->data;
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_WOW_ADD_PATTERN_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->pattern_id = __cpu_to_le32(pattern_id);
+	cmd->pattern_type = __cpu_to_le32(WOW_BITMAP_PATTERN);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	/* bitmap */
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_STRUCT);
+	tlv->len = __cpu_to_le16(sizeof(*tlv) + sizeof(*bitmap));
+
+	ptr += sizeof(*tlv);
+
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_WOW_BITMAP_PATTERN_T);
+	tlv->len = __cpu_to_le16(sizeof(*bitmap));
+	bitmap = (void *)tlv->value;
+
+	memcpy(bitmap->patternbuf, pattern, pattern_len);
+	memcpy(bitmap->bitmaskbuf, bitmask, pattern_len);
+	bitmap->pattern_offset = __cpu_to_le32(pattern_offset);
+	bitmap->pattern_len = __cpu_to_le32(pattern_len);
+	bitmap->bitmask_len = __cpu_to_le32(pattern_len);
+	bitmap->pattern_id = __cpu_to_le32(pattern_id);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*bitmap);
+
+	/* ipv4 sync */
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_STRUCT);
+	tlv->len = __cpu_to_le16(0);
+
+	ptr += sizeof(*tlv);
+
+	/* ipv6 sync */
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_STRUCT);
+	tlv->len = __cpu_to_le16(0);
+
+	ptr += sizeof(*tlv);
+
+	/* magic */
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_STRUCT);
+	tlv->len = __cpu_to_le16(0);
+
+	ptr += sizeof(*tlv);
+
+	/* pattern info timeout */
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_UINT32);
+	tlv->len = __cpu_to_le16(0);
+
+	ptr += sizeof(*tlv);
+
+	/* ratelimit interval */
+	tlv = ptr;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_ARRAY_UINT32);
+	tlv->len = __cpu_to_le16(sizeof(u32));
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv wow add pattern vdev_id %d pattern_id %d, pattern_offset %d\n",
+		   vdev_id, pattern_id, pattern_offset);
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_wow_del_pattern(struct ath10k *ar, u32 vdev_id,
+				      u32 pattern_id)
+{
+	struct wmi_tlv_wow_del_pattern_cmd *cmd;
+	struct wmi_tlv *tlv;
+	struct sk_buff *skb;
+	size_t len;
+
+	len = sizeof(*tlv) + sizeof(*cmd);
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	tlv = (struct wmi_tlv *)skb->data;
+	tlv->tag = __cpu_to_le16(WMI_TLV_TAG_STRUCT_WOW_DEL_PATTERN_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->pattern_id = __cpu_to_le32(pattern_id);
+	cmd->pattern_type = __cpu_to_le32(WOW_BITMAP_PATTERN);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv wow del pattern vdev_id %d pattern_id %d\n",
+		   vdev_id, pattern_id);
+	return skb;
+}
+
+static struct sk_buff *
+ath10k_wmi_tlv_op_gen_adaptive_qcs(struct ath10k *ar, bool enable)
+{
+	struct wmi_tlv_adaptive_qcs *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_RESMGR_ADAPTIVE_OCS_CMD);
+	tlv->len = __cpu_to_le16(sizeof(*cmd));
+	cmd = (void *)tlv->value;
+	cmd->enable = __cpu_to_le32(enable ? 1 : 0);
+
+	ptr += sizeof(*tlv);
+	ptr += sizeof(*cmd);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi tlv adaptive qcs %d\n", enable);
+	return skb;
+}
+
 /****************/
 /* TLV mappings */
 /****************/
@@ -2609,6 +3148,9 @@ static struct wmi_cmd_map wmi_tlv_cmd_map = {
 	.gpio_output_cmdid = WMI_TLV_GPIO_OUTPUT_CMDID,
 	.pdev_get_temperature_cmdid = WMI_TLV_CMD_UNSUPPORTED,
 	.vdev_set_wmm_params_cmdid = WMI_TLV_VDEV_SET_WMM_PARAMS_CMDID,
+	.tdls_set_state_cmdid = WMI_TLV_TDLS_SET_STATE_CMDID,
+	.tdls_peer_update_cmdid = WMI_TLV_TDLS_PEER_UPDATE_CMDID,
+	.adaptive_qcs_cmdid = WMI_TLV_RESMGR_ADAPTIVE_OCS_CMDID,
 };
 
 static struct wmi_pdev_param_map wmi_tlv_pdev_param_map = {
@@ -2736,6 +3278,8 @@ static const struct wmi_ops wmi_tlv_ops = {
 	.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,
+	.pull_roam_ev = ath10k_wmi_tlv_op_pull_roam_ev,
+	.pull_wow_event = ath10k_wmi_tlv_op_pull_wow_ev,
 
 	.gen_pdev_suspend = ath10k_wmi_tlv_op_gen_pdev_suspend,
 	.gen_pdev_resume = ath10k_wmi_tlv_op_gen_pdev_resume,
@@ -2781,6 +3325,14 @@ static const struct wmi_ops wmi_tlv_ops = {
 	.gen_p2p_go_bcn_ie = ath10k_wmi_tlv_op_gen_p2p_go_bcn_ie,
 	.gen_vdev_sta_uapsd = ath10k_wmi_tlv_op_gen_vdev_sta_uapsd,
 	.gen_sta_keepalive = ath10k_wmi_tlv_op_gen_sta_keepalive,
+	.gen_wow_enable = ath10k_wmi_tlv_op_gen_wow_enable,
+	.gen_wow_add_wakeup_event = ath10k_wmi_tlv_op_gen_wow_add_wakeup_event,
+	.gen_wow_host_wakeup_ind = ath10k_wmi_tlv_gen_wow_host_wakeup_ind,
+	.gen_wow_add_pattern = ath10k_wmi_tlv_op_gen_wow_add_pattern,
+	.gen_wow_del_pattern = ath10k_wmi_tlv_op_gen_wow_del_pattern,
+	.gen_update_fw_tdls_state = ath10k_wmi_tlv_op_gen_update_fw_tdls_state,
+	.gen_tdls_peer_update = ath10k_wmi_tlv_op_gen_tdls_peer_update,
+	.gen_adaptive_qcs = ath10k_wmi_tlv_op_gen_adaptive_qcs,
 };
 
 /************/
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.h b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
index a6c8280cc4b1..ad655c44afdb 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
@@ -1454,6 +1454,174 @@ struct wmi_tlv_stats_ev {
 	__le32 num_chan_stats;
 } __packed;
 
+struct wmi_tlv_p2p_noa_ev {
+	__le32 vdev_id;
+} __packed;
+
+struct wmi_tlv_roam_ev {
+	__le32 vdev_id;
+	__le32 reason;
+	__le32 rssi;
+} __packed;
+
+struct wmi_tlv_wow_add_del_event_cmd {
+	__le32 vdev_id;
+	__le32 is_add;
+	__le32 event_bitmap;
+} __packed;
+
+struct wmi_tlv_wow_enable_cmd {
+	__le32 enable;
+} __packed;
+
+struct wmi_tlv_wow_host_wakeup_ind {
+	__le32 reserved;
+} __packed;
+
+struct wmi_tlv_wow_event_info {
+	__le32 vdev_id;
+	__le32 flag;
+	__le32 wake_reason;
+	__le32 data_len;
+} __packed;
+
+enum wmi_tlv_pattern_type {
+	WOW_PATTERN_MIN = 0,
+	WOW_BITMAP_PATTERN = WOW_PATTERN_MIN,
+	WOW_IPV4_SYNC_PATTERN,
+	WOW_IPV6_SYNC_PATTERN,
+	WOW_WILD_CARD_PATTERN,
+	WOW_TIMER_PATTERN,
+	WOW_MAGIC_PATTERN,
+	WOW_IPV6_RA_PATTERN,
+	WOW_IOAC_PKT_PATTERN,
+	WOW_IOAC_TMR_PATTERN,
+	WOW_PATTERN_MAX
+};
+
+#define WOW_DEFAULT_BITMAP_PATTERN_SIZE		148
+#define WOW_DEFAULT_BITMASK_SIZE		148
+
+struct wmi_tlv_wow_bitmap_pattern {
+	u8 patternbuf[WOW_DEFAULT_BITMAP_PATTERN_SIZE];
+	u8 bitmaskbuf[WOW_DEFAULT_BITMASK_SIZE];
+	__le32 pattern_offset;
+	__le32 pattern_len;
+	__le32 bitmask_len;
+	__le32 pattern_id;
+} __packed;
+
+struct wmi_tlv_wow_add_pattern_cmd {
+	__le32 vdev_id;
+	__le32 pattern_id;
+	__le32 pattern_type;
+} __packed;
+
+struct wmi_tlv_wow_del_pattern_cmd {
+	__le32 vdev_id;
+	__le32 pattern_id;
+	__le32 pattern_type;
+} __packed;
+
+/* TDLS Options */
+enum wmi_tlv_tdls_options {
+	WMI_TLV_TDLS_OFFCHAN_EN = BIT(0),
+	WMI_TLV_TDLS_BUFFER_STA_EN = BIT(1),
+	WMI_TLV_TDLS_SLEEP_STA_EN = BIT(2),
+};
+
+struct wmi_tdls_set_state_cmd {
+	__le32 vdev_id;
+	__le32 state;
+	__le32 notification_interval_ms;
+	__le32 tx_discovery_threshold;
+	__le32 tx_teardown_threshold;
+	__le32 rssi_teardown_threshold;
+	__le32 rssi_delta;
+	__le32 tdls_options;
+	__le32 tdls_peer_traffic_ind_window;
+	__le32 tdls_peer_traffic_response_timeout_ms;
+	__le32 tdls_puapsd_mask;
+	__le32 tdls_puapsd_inactivity_time_ms;
+	__le32 tdls_puapsd_rx_frame_threshold;
+} __packed;
+
+struct wmi_tdls_peer_update_cmd {
+	__le32 vdev_id;
+	struct wmi_mac_addr peer_macaddr;
+	__le32 peer_state;
+} __packed;
+
+enum {
+	WMI_TLV_TDLS_PEER_QOS_AC_VO = BIT(0),
+	WMI_TLV_TDLS_PEER_QOS_AC_VI = BIT(1),
+	WMI_TLV_TDLS_PEER_QOS_AC_BK = BIT(2),
+	WMI_TLV_TDLS_PEER_QOS_AC_BE = BIT(3),
+};
+
+#define WMI_TLV_TDLS_PEER_SP_MASK	0x60
+#define WMI_TLV_TDLS_PEER_SP_LSB	5
+
+struct wmi_tdls_peer_capab {
+	__le32 peer_qos;
+	__le32 buff_sta_support;
+	__le32 off_chan_support;
+	__le32 peer_curr_operclass;
+	__le32 self_curr_operclass;
+	__le32 peer_chan_len;
+	__le32 peer_operclass_len;
+	u8 peer_operclass[WMI_TDLS_MAX_SUPP_OPER_CLASSES];
+	__le32 is_peer_responder;
+	__le32 pref_offchan_num;
+	__le32 pref_offchan_bw;
+} __packed;
+
+struct wmi_tlv_adaptive_qcs {
+	__le32 enable;
+} __packed;
+
+/**
+ * wmi_tlv_tx_pause_id - firmware tx queue pause reason types
+ *
+ * @WMI_TLV_TX_PAUSE_ID_MCC: used for by multi-channel firmware scheduler.
+ *		Only vdev_map is valid.
+ * @WMI_TLV_TX_PAUSE_ID_AP_PEER_PS: peer in AP mode is asleep.
+ *		Only peer_id is valid.
+ * @WMI_TLV_TX_PAUSE_ID_AP_PEER_UAPSD: Only peer_id and tid_map are valid.
+ * @WMI_TLV_TX_PAUSE_ID_P2P_CLI_NOA: Only vdev_map is valid.
+ * @WMI_TLV_TX_PAUSE_ID_P2P_GO_PS: Only vdev_map is valid.
+ * @WMI_TLV_TX_PAUSE_ID_STA_ADD_BA: Only peer_id and tid_map are valid.
+ * @WMI_TLV_TX_PAUSE_ID_AP_PS: When all peers are asleep in AP mode. Only
+ *		vdev_map is valid.
+ * @WMI_TLV_TX_PAUSE_ID_IBSS_PS: When all peers are asleep in IBSS mode. Only
+ *		vdev_map is valid.
+ * @WMI_TLV_TX_PAUSE_ID_HOST: Host itself requested tx pause.
+ */
+enum wmi_tlv_tx_pause_id {
+	WMI_TLV_TX_PAUSE_ID_MCC = 1,
+	WMI_TLV_TX_PAUSE_ID_AP_PEER_PS = 2,
+	WMI_TLV_TX_PAUSE_ID_AP_PEER_UAPSD = 3,
+	WMI_TLV_TX_PAUSE_ID_P2P_CLI_NOA = 4,
+	WMI_TLV_TX_PAUSE_ID_P2P_GO_PS = 5,
+	WMI_TLV_TX_PAUSE_ID_STA_ADD_BA = 6,
+	WMI_TLV_TX_PAUSE_ID_AP_PS = 7,
+	WMI_TLV_TX_PAUSE_ID_IBSS_PS = 8,
+	WMI_TLV_TX_PAUSE_ID_HOST = 21,
+};
+
+enum wmi_tlv_tx_pause_action {
+	WMI_TLV_TX_PAUSE_ACTION_STOP,
+	WMI_TLV_TX_PAUSE_ACTION_WAKE,
+};
+
+struct wmi_tlv_tx_pause_ev {
+	__le32 pause_id;
+	__le32 action;
+	__le32 vdev_map;
+	__le32 peer_id;
+	__le32 tid_map;
+} __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 c7ea77edce24..6c046c244705 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -26,6 +26,8 @@
 #include "mac.h"
 #include "testmode.h"
 #include "wmi-ops.h"
+#include "p2p.h"
+#include "hw.h"
 
 /* MAIN WMI cmd track */
 static struct wmi_cmd_map wmi_cmd_map = {
@@ -884,20 +886,24 @@ void ath10k_wmi_put_wmi_channel(struct wmi_channel *ch,
 
 int ath10k_wmi_wait_for_service_ready(struct ath10k *ar)
 {
-	int ret;
+	unsigned long time_left;
 
-	ret = wait_for_completion_timeout(&ar->wmi.service_ready,
-					  WMI_SERVICE_READY_TIMEOUT_HZ);
-	return ret;
+	time_left = wait_for_completion_timeout(&ar->wmi.service_ready,
+						WMI_SERVICE_READY_TIMEOUT_HZ);
+	if (!time_left)
+		return -ETIMEDOUT;
+	return 0;
 }
 
 int ath10k_wmi_wait_for_unified_ready(struct ath10k *ar)
 {
-	int ret;
+	unsigned long time_left;
 
-	ret = wait_for_completion_timeout(&ar->wmi.unified_ready,
-					  WMI_UNIFIED_READY_TIMEOUT_HZ);
-	return ret;
+	time_left = wait_for_completion_timeout(&ar->wmi.unified_ready,
+						WMI_UNIFIED_READY_TIMEOUT_HZ);
+	if (!time_left)
+		return -ETIMEDOUT;
+	return 0;
 }
 
 struct sk_buff *ath10k_wmi_alloc_skb(struct ath10k *ar, u32 len)
@@ -1351,63 +1357,6 @@ static inline enum ieee80211_band phy_mode_to_band(u32 phy_mode)
 	return band;
 }
 
-static inline u8 get_rate_idx(u32 rate, enum ieee80211_band band)
-{
-	u8 rate_idx = 0;
-
-	/* rate in Kbps */
-	switch (rate) {
-	case 1000:
-		rate_idx = 0;
-		break;
-	case 2000:
-		rate_idx = 1;
-		break;
-	case 5500:
-		rate_idx = 2;
-		break;
-	case 11000:
-		rate_idx = 3;
-		break;
-	case 6000:
-		rate_idx = 4;
-		break;
-	case 9000:
-		rate_idx = 5;
-		break;
-	case 12000:
-		rate_idx = 6;
-		break;
-	case 18000:
-		rate_idx = 7;
-		break;
-	case 24000:
-		rate_idx = 8;
-		break;
-	case 36000:
-		rate_idx = 9;
-		break;
-	case 48000:
-		rate_idx = 10;
-		break;
-	case 54000:
-		rate_idx = 11;
-		break;
-	default:
-		break;
-	}
-
-	if (band == IEEE80211_BAND_5GHZ) {
-		if (rate_idx > 3)
-			/* Omit CCK rates */
-			rate_idx -= 4;
-		else
-			rate_idx = 0;
-	}
-
-	return rate_idx;
-}
-
 /* If keys are configured, HW decrypts all frames
  * with protected bit set. Mark such frames as decrypted.
  */
@@ -1489,6 +1438,7 @@ 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;
+	struct ieee80211_supported_band *sband;
 	u32 rx_status;
 	u32 channel;
 	u32 phy_mode;
@@ -1501,6 +1451,7 @@ int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 	ret = ath10k_wmi_pull_mgmt_rx(ar, skb, &arg);
 	if (ret) {
 		ath10k_warn(ar, "failed to parse mgmt rx event: %d\n", ret);
+		dev_kfree_skb(skb);
 		return ret;
 	}
 
@@ -1559,9 +1510,11 @@ int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 	if (phy_mode == MODE_11B && status->band == IEEE80211_BAND_5GHZ)
 		ath10k_dbg(ar, ATH10K_DBG_MGMT, "wmi mgmt rx 11b (CCK) on 5GHz\n");
 
+	sband = &ar->mac.sbands[status->band];
+
 	status->freq = ieee80211_channel_to_frequency(channel, status->band);
 	status->signal = snr + ATH10K_DEFAULT_NOISE_FLOOR;
-	status->rate_idx = get_rate_idx(rate, status->band);
+	status->rate_idx = ath10k_mac_bitrate_to_idx(sband, rate / 100);
 
 	hdr = (struct ieee80211_hdr *)skb->data;
 	fc = le16_to_cpu(hdr->frame_control);
@@ -1585,6 +1538,9 @@ int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 		}
 	}
 
+	if (ieee80211_is_beacon(hdr->frame_control))
+		ath10k_mac_handle_beacon(ar, skb);
+
 	ath10k_dbg(ar, ATH10K_DBG_MGMT,
 		   "event mgmt rx skb %p len %d ftype %02x stype %02x\n",
 		   skb, skb->len,
@@ -1682,20 +1638,22 @@ void ath10k_wmi_event_chan_info(struct ath10k *ar, struct sk_buff *skb)
 	}
 
 	if (cmd_flags & WMI_CHAN_INFO_FLAG_COMPLETE) {
-		/* During scanning chan info is reported twice for each
-		 * visited channel. The reported cycle count is global
-		 * and per-channel cycle count must be calculated */
-
-		cycle_count -= ar->survey_last_cycle_count;
-		rx_clear_count -= ar->survey_last_rx_clear_count;
-
-		survey = &ar->survey[idx];
-		survey->time = WMI_CHAN_INFO_MSEC(cycle_count);
-		survey->time_rx = WMI_CHAN_INFO_MSEC(rx_clear_count);
-		survey->noise = noise_floor;
-		survey->filled = SURVEY_INFO_TIME |
-				 SURVEY_INFO_TIME_RX |
-				 SURVEY_INFO_NOISE_DBM;
+		if (ar->ch_info_can_report_survey) {
+			survey = &ar->survey[idx];
+			survey->noise = noise_floor;
+			survey->filled = SURVEY_INFO_NOISE_DBM;
+
+			ath10k_hw_fill_survey_time(ar,
+						   survey,
+						   cycle_count,
+						   rx_clear_count,
+						   ar->survey_last_cycle_count,
+						   ar->survey_last_rx_clear_count);
+		}
+
+		ar->ch_info_can_report_survey = false;
+	} else {
+		ar->ch_info_can_report_survey = true;
 	}
 
 	ar->survey_last_rx_clear_count = rx_clear_count;
@@ -2276,109 +2234,25 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 		   tim->bitmap_ctrl, pvm_len);
 }
 
-static void ath10k_p2p_fill_noa_ie(u8 *data, u32 len,
-				   const struct wmi_p2p_noa_info *noa)
-{
-	struct ieee80211_p2p_noa_attr *noa_attr;
-	u8  ctwindow_oppps = noa->ctwindow_oppps;
-	u8 ctwindow = ctwindow_oppps >> WMI_P2P_OPPPS_CTWINDOW_OFFSET;
-	bool oppps = !!(ctwindow_oppps & WMI_P2P_OPPPS_ENABLE_BIT);
-	__le16 *noa_attr_len;
-	u16 attr_len;
-	u8 noa_descriptors = noa->num_descriptors;
-	int i;
-
-	/* P2P IE */
-	data[0] = WLAN_EID_VENDOR_SPECIFIC;
-	data[1] = len - 2;
-	data[2] = (WLAN_OUI_WFA >> 16) & 0xff;
-	data[3] = (WLAN_OUI_WFA >> 8) & 0xff;
-	data[4] = (WLAN_OUI_WFA >> 0) & 0xff;
-	data[5] = WLAN_OUI_TYPE_WFA_P2P;
-
-	/* NOA ATTR */
-	data[6] = IEEE80211_P2P_ATTR_ABSENCE_NOTICE;
-	noa_attr_len = (__le16 *)&data[7]; /* 2 bytes */
-	noa_attr = (struct ieee80211_p2p_noa_attr *)&data[9];
-
-	noa_attr->index = noa->index;
-	noa_attr->oppps_ctwindow = ctwindow;
-	if (oppps)
-		noa_attr->oppps_ctwindow |= IEEE80211_P2P_OPPPS_ENABLE_BIT;
-
-	for (i = 0; i < noa_descriptors; i++) {
-		noa_attr->desc[i].count =
-			__le32_to_cpu(noa->descriptors[i].type_count);
-		noa_attr->desc[i].duration = noa->descriptors[i].duration;
-		noa_attr->desc[i].interval = noa->descriptors[i].interval;
-		noa_attr->desc[i].start_time = noa->descriptors[i].start_time;
-	}
-
-	attr_len = 2; /* index + oppps_ctwindow */
-	attr_len += noa_descriptors * sizeof(struct ieee80211_p2p_noa_desc);
-	*noa_attr_len = __cpu_to_le16(attr_len);
-}
-
-static u32 ath10k_p2p_calc_noa_ie_len(const struct wmi_p2p_noa_info *noa)
-{
-	u32 len = 0;
-	u8 noa_descriptors = noa->num_descriptors;
-	u8 opp_ps_info = noa->ctwindow_oppps;
-	bool opps_enabled = !!(opp_ps_info & WMI_P2P_OPPPS_ENABLE_BIT);
-
-	if (!noa_descriptors && !opps_enabled)
-		return len;
-
-	len += 1 + 1 + 4; /* EID + len + OUI */
-	len += 1 + 2; /* noa attr  + attr len */
-	len += 1 + 1; /* index + oppps_ctwindow */
-	len += noa_descriptors * sizeof(struct ieee80211_p2p_noa_desc);
-
-	return len;
-}
-
 static void ath10k_wmi_update_noa(struct ath10k *ar, struct ath10k_vif *arvif,
 				  struct sk_buff *bcn,
 				  const struct wmi_p2p_noa_info *noa)
 {
-	u8 *new_data, *old_data = arvif->u.ap.noa_data;
-	u32 new_len;
-
 	if (arvif->vdev_subtype != WMI_VDEV_SUBTYPE_P2P_GO)
 		return;
 
 	ath10k_dbg(ar, ATH10K_DBG_MGMT, "noa changed: %d\n", noa->changed);
-	if (noa->changed & WMI_P2P_NOA_CHANGED_BIT) {
-		new_len = ath10k_p2p_calc_noa_ie_len(noa);
-		if (!new_len)
-			goto cleanup;
-
-		new_data = kmalloc(new_len, GFP_ATOMIC);
-		if (!new_data)
-			goto cleanup;
-
-		ath10k_p2p_fill_noa_ie(new_data, new_len, noa);
 
-		spin_lock_bh(&ar->data_lock);
-		arvif->u.ap.noa_data = new_data;
-		arvif->u.ap.noa_len = new_len;
-		spin_unlock_bh(&ar->data_lock);
-		kfree(old_data);
-	}
+	if (noa->changed & WMI_P2P_NOA_CHANGED_BIT)
+		ath10k_p2p_noa_update(arvif, noa);
 
 	if (arvif->u.ap.noa_data)
 		if (!pskb_expand_head(bcn, 0, arvif->u.ap.noa_len, GFP_ATOMIC))
 			memcpy(skb_put(bcn, arvif->u.ap.noa_len),
 			       arvif->u.ap.noa_data,
 			       arvif->u.ap.noa_len);
-	return;
 
-cleanup:
-	spin_lock_bh(&ar->data_lock);
-	arvif->u.ap.noa_data = NULL;
-	arvif->u.ap.noa_len = 0;
-	spin_unlock_bh(&ar->data_lock);
-	kfree(old_data);
+	return;
 }
 
 static int ath10k_wmi_op_pull_swba_ev(struct ath10k *ar, struct sk_buff *skb,
@@ -2555,6 +2429,7 @@ static void ath10k_dfs_radar_report(struct ath10k *ar,
 				    u64 tsf)
 {
 	u32 reg0, reg1, tsf32l;
+	struct ieee80211_channel *ch;
 	struct pulse_event pe;
 	u64 tsf64;
 	u8 rssi, width;
@@ -2583,6 +2458,15 @@ static void ath10k_dfs_radar_report(struct ath10k *ar,
 	if (!ar->dfs_detector)
 		return;
 
+	spin_lock_bh(&ar->data_lock);
+	ch = ar->rx_channel;
+	spin_unlock_bh(&ar->data_lock);
+
+	if (!ch) {
+		ath10k_warn(ar, "failed to derive channel for radar pulse, treating as radar\n");
+		goto radar_detected;
+	}
+
 	/* report event to DFS pattern detector */
 	tsf32l = __le32_to_cpu(phyerr->tsf_timestamp);
 	tsf64 = tsf & (~0xFFFFFFFFULL);
@@ -2598,10 +2482,10 @@ static void ath10k_dfs_radar_report(struct ath10k *ar,
 		rssi = 0;
 
 	pe.ts = tsf64;
-	pe.freq = ar->hw->conf.chandef.chan->center_freq;
+	pe.freq = ch->center_freq;
 	pe.width = width;
 	pe.rssi = rssi;
-
+	pe.chirp = (MS(reg0, RADAR_REPORT_REG0_PULSE_IS_CHIRP) != 0);
 	ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 		   "dfs add pulse freq: %d, width: %d, rssi %d, tsf: %llX\n",
 		   pe.freq, pe.width, pe.rssi, pe.ts);
@@ -2614,6 +2498,7 @@ static void ath10k_dfs_radar_report(struct ath10k *ar,
 		return;
 	}
 
+radar_detected:
 	ath10k_dbg(ar, ATH10K_DBG_REGULATORY, "dfs radar detected\n");
 	ATH10K_DFS_STAT_INC(ar, radar_detected);
 
@@ -2872,7 +2757,43 @@ void ath10k_wmi_event_phyerr(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");
+	struct wmi_roam_ev_arg arg = {};
+	int ret;
+	u32 vdev_id;
+	u32 reason;
+	s32 rssi;
+
+	ret = ath10k_wmi_pull_roam_ev(ar, skb, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse roam event: %d\n", ret);
+		return;
+	}
+
+	vdev_id = __le32_to_cpu(arg.vdev_id);
+	reason = __le32_to_cpu(arg.reason);
+	rssi = __le32_to_cpu(arg.rssi);
+	rssi += WMI_SPECTRAL_NOISE_FLOOR_REF_DEFAULT;
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi roam event vdev %u reason 0x%08x rssi %d\n",
+		   vdev_id, reason, rssi);
+
+	if (reason >= WMI_ROAM_REASON_MAX)
+		ath10k_warn(ar, "ignoring unknown roam event reason %d on vdev %i\n",
+			    reason, vdev_id);
+
+	switch (reason) {
+	case WMI_ROAM_REASON_BEACON_MISS:
+		ath10k_mac_handle_beacon_miss(ar, vdev_id);
+		break;
+	case WMI_ROAM_REASON_BETTER_AP:
+	case WMI_ROAM_REASON_LOW_RSSI:
+	case WMI_ROAM_REASON_SUITABLE_AP_FOUND:
+	case WMI_ROAM_REASON_HO_FAILED:
+		ath10k_warn(ar, "ignoring not implemented roam event reason %d on vdev %i\n",
+			    reason, vdev_id);
+		break;
+	}
 }
 
 void ath10k_wmi_event_profile_match(struct ath10k *ar, struct sk_buff *skb)
@@ -2942,7 +2863,19 @@ 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)
 {
-	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_WOW_WAKEUP_HOST_EVENTID\n");
+	struct wmi_wow_ev_arg ev = {};
+	int ret;
+
+	complete(&ar->wow.wakeup_completed);
+
+	ret = ath10k_wmi_pull_wow_event(ar, skb, &ev);
+	if (ret) {
+		ath10k_warn(ar, "failed to parse wow wakeup event: %d\n", ret);
+		return;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wow wakeup host reason %s\n",
+		   wow_reason(ev.wake_reason));
 }
 
 void ath10k_wmi_event_dcs_interference(struct ath10k *ar, struct sk_buff *skb)
@@ -3231,6 +3164,21 @@ static int ath10k_wmi_op_pull_rdy_ev(struct ath10k *ar, struct sk_buff *skb,
 	return 0;
 }
 
+static int ath10k_wmi_op_pull_roam_ev(struct ath10k *ar, struct sk_buff *skb,
+				      struct wmi_roam_ev_arg *arg)
+{
+	struct wmi_roam_ev *ev = (void *)skb->data;
+
+	if (skb->len < sizeof(*ev))
+		return -EPROTO;
+
+	skb_pull(skb, sizeof(*ev));
+	arg->vdev_id = ev->vdev_id;
+	arg->reason = ev->reason;
+
+	return 0;
+}
+
 int ath10k_wmi_event_ready(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct wmi_rdy_ev_arg arg = {};
@@ -3275,7 +3223,7 @@ static void ath10k_wmi_op_rx(struct ath10k *ar, struct sk_buff *skb)
 	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;
+		goto out;
 
 	trace_ath10k_wmi_event(ar, id, skb->data, skb->len);
 
@@ -3379,6 +3327,7 @@ static void ath10k_wmi_op_rx(struct ath10k *ar, struct sk_buff *skb)
 		break;
 	}
 
+out:
 	dev_kfree_skb(skb);
 }
 
@@ -3392,7 +3341,7 @@ static void ath10k_wmi_10_1_op_rx(struct ath10k *ar, struct sk_buff *skb)
 	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;
+		goto out;
 
 	trace_ath10k_wmi_event(ar, id, skb->data, skb->len);
 
@@ -3515,7 +3464,7 @@ static void ath10k_wmi_10_2_op_rx(struct ath10k *ar, struct sk_buff *skb)
 	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;
+		goto out;
 
 	trace_ath10k_wmi_event(ar, id, skb->data, skb->len);
 
@@ -3623,6 +3572,7 @@ static void ath10k_wmi_10_2_op_rx(struct ath10k *ar, struct sk_buff *skb)
 		break;
 	}
 
+out:
 	dev_kfree_skb(skb);
 }
 
@@ -3989,6 +3939,8 @@ static struct sk_buff *ath10k_wmi_10_2_op_gen_init(struct ath10k *ar)
 	cmd = (struct wmi_init_cmd_10_2 *)buf->data;
 
 	features = WMI_10_2_RX_BATCH_MODE;
+	if (test_bit(WMI_SERVICE_COEX_GPIO, ar->wmi.svc_map))
+		features |= WMI_10_2_COEX_GPIO;
 	cmd->resource_config.feature_mask = __cpu_to_le32(features);
 
 	memcpy(&cmd->resource_config.common, &config, sizeof(config));
@@ -4315,8 +4267,6 @@ ath10k_wmi_op_gen_vdev_start(struct ath10k *ar,
 	const char *cmdname;
 	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)))
@@ -4539,7 +4489,8 @@ ath10k_wmi_op_gen_vdev_spectral_enable(struct ath10k *ar, u32 vdev_id,
 
 static struct sk_buff *
 ath10k_wmi_op_gen_peer_create(struct ath10k *ar, u32 vdev_id,
-			      const u8 peer_addr[ETH_ALEN])
+			      const u8 peer_addr[ETH_ALEN],
+			      enum wmi_peer_type peer_type)
 {
 	struct wmi_peer_create_cmd *cmd;
 	struct sk_buff *skb;
@@ -5223,6 +5174,7 @@ static const struct wmi_ops wmi_ops = {
 	.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,
+	.pull_roam_ev = ath10k_wmi_op_pull_roam_ev,
 
 	.gen_pdev_suspend = ath10k_wmi_op_gen_pdev_suspend,
 	.gen_pdev_resume = ath10k_wmi_op_gen_pdev_resume,
@@ -5268,6 +5220,7 @@ static const struct wmi_ops wmi_ops = {
 	/* .gen_bcn_tmpl not implemented */
 	/* .gen_prb_tmpl not implemented */
 	/* .gen_p2p_go_bcn_ie not implemented */
+	/* .gen_adaptive_qcs not implemented */
 };
 
 static const struct wmi_ops wmi_10_1_ops = {
@@ -5290,6 +5243,7 @@ static const struct wmi_ops wmi_10_1_ops = {
 	.pull_swba = ath10k_wmi_op_pull_swba_ev,
 	.pull_phyerr = ath10k_wmi_op_pull_phyerr_ev,
 	.pull_rdy = ath10k_wmi_op_pull_rdy_ev,
+	.pull_roam_ev = ath10k_wmi_op_pull_roam_ev,
 
 	.gen_pdev_suspend = ath10k_wmi_op_gen_pdev_suspend,
 	.gen_pdev_resume = ath10k_wmi_op_gen_pdev_resume,
@@ -5330,6 +5284,7 @@ static const struct wmi_ops wmi_10_1_ops = {
 	/* .gen_bcn_tmpl not implemented */
 	/* .gen_prb_tmpl not implemented */
 	/* .gen_p2p_go_bcn_ie not implemented */
+	/* .gen_adaptive_qcs not implemented */
 };
 
 static const struct wmi_ops wmi_10_2_ops = {
@@ -5353,6 +5308,7 @@ static const struct wmi_ops wmi_10_2_ops = {
 	.pull_swba = ath10k_wmi_op_pull_swba_ev,
 	.pull_phyerr = ath10k_wmi_op_pull_phyerr_ev,
 	.pull_rdy = ath10k_wmi_op_pull_rdy_ev,
+	.pull_roam_ev = ath10k_wmi_op_pull_roam_ev,
 
 	.gen_pdev_suspend = ath10k_wmi_op_gen_pdev_suspend,
 	.gen_pdev_resume = ath10k_wmi_op_gen_pdev_resume,
@@ -5413,6 +5369,7 @@ static const struct wmi_ops wmi_10_2_4_ops = {
 	.pull_swba = ath10k_wmi_op_pull_swba_ev,
 	.pull_phyerr = ath10k_wmi_op_pull_phyerr_ev,
 	.pull_rdy = ath10k_wmi_op_pull_rdy_ev,
+	.pull_roam_ev = ath10k_wmi_op_pull_roam_ev,
 
 	.gen_pdev_suspend = ath10k_wmi_op_gen_pdev_suspend,
 	.gen_pdev_resume = ath10k_wmi_op_gen_pdev_resume,
@@ -5452,6 +5409,7 @@ static const struct wmi_ops wmi_10_2_4_ops = {
 	/* .gen_bcn_tmpl not implemented */
 	/* .gen_prb_tmpl not implemented */
 	/* .gen_p2p_go_bcn_ie not implemented */
+	/* .gen_adaptive_qcs not implemented */
 };
 
 int ath10k_wmi_attach(struct ath10k *ar)
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index adf935bf0580..cf44a3d080a3 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -148,6 +148,8 @@ enum wmi_service {
 	WMI_SERVICE_STA_RX_IPA_OFFLOAD_SUPPORT,
 	WMI_SERVICE_MDNS_OFFLOAD,
 	WMI_SERVICE_SAP_AUTH_OFFLOAD,
+	WMI_SERVICE_ATF,
+	WMI_SERVICE_COEX_GPIO,
 
 	/* keep last */
 	WMI_SERVICE_MAX,
@@ -177,6 +179,8 @@ enum wmi_10x_service {
 	WMI_10X_SERVICE_SMART_ANTENNA_SW_SUPPORT,
 	WMI_10X_SERVICE_FORCE_FW_HANG,
 	WMI_10X_SERVICE_SMART_ANTENNA_HW_SUPPORT,
+	WMI_10X_SERVICE_ATF,
+	WMI_10X_SERVICE_COEX_GPIO,
 };
 
 enum wmi_main_service {
@@ -293,6 +297,8 @@ static inline char *wmi_service_name(int service_id)
 	SVCSTR(WMI_SERVICE_STA_RX_IPA_OFFLOAD_SUPPORT);
 	SVCSTR(WMI_SERVICE_MDNS_OFFLOAD);
 	SVCSTR(WMI_SERVICE_SAP_AUTH_OFFLOAD);
+	SVCSTR(WMI_SERVICE_ATF);
+	SVCSTR(WMI_SERVICE_COEX_GPIO);
 	default:
 		return NULL;
 	}
@@ -356,6 +362,10 @@ static inline void wmi_10x_svc_map(const __le32 *in, unsigned long *out,
 	       WMI_SERVICE_FORCE_FW_HANG, len);
 	SVCMAP(WMI_10X_SERVICE_SMART_ANTENNA_HW_SUPPORT,
 	       WMI_SERVICE_SMART_ANTENNA_HW_SUPPORT, len);
+	SVCMAP(WMI_10X_SERVICE_ATF,
+	       WMI_SERVICE_ATF, len);
+	SVCMAP(WMI_10X_SERVICE_COEX_GPIO,
+	       WMI_SERVICE_COEX_GPIO, len);
 }
 
 static inline void wmi_main_svc_map(const __le32 *in, unsigned long *out,
@@ -552,6 +562,9 @@ struct wmi_cmd_map {
 	u32 gpio_output_cmdid;
 	u32 pdev_get_temperature_cmdid;
 	u32 vdev_set_wmm_params_cmdid;
+	u32 tdls_set_state_cmdid;
+	u32 tdls_peer_update_cmdid;
+	u32 adaptive_qcs_cmdid;
 };
 
 /*
@@ -1952,6 +1965,7 @@ struct wmi_resource_config_10x {
 enum wmi_10_2_feature_mask {
 	WMI_10_2_RX_BATCH_MODE = BIT(0),
 	WMI_10_2_ATF_CONFIG    = BIT(1),
+	WMI_10_2_COEX_GPIO     = BIT(3),
 };
 
 struct wmi_resource_config_10_2 {
@@ -2166,6 +2180,7 @@ struct wmi_start_scan_arg {
 	u32 max_scan_time;
 	u32 probe_delay;
 	u32 scan_ctrl_flags;
+	u32 burst_duration_ms;
 
 	u32 ie_len;
 	u32 n_channels;
@@ -4333,6 +4348,12 @@ struct wmi_peer_create_cmd {
 	struct wmi_mac_addr peer_macaddr;
 } __packed;
 
+enum wmi_peer_type {
+	WMI_PEER_TYPE_DEFAULT = 0,
+	WMI_PEER_TYPE_BSS = 1,
+	WMI_PEER_TYPE_TDLS = 2,
+};
+
 struct wmi_peer_delete_cmd {
 	__le32 vdev_id;
 	struct wmi_mac_addr peer_macaddr;
@@ -4645,9 +4666,6 @@ struct wmi_peer_sta_kickout_event {
 
 #define WMI_CHAN_INFO_FLAG_COMPLETE BIT(0)
 
-/* FIXME: empirically extrapolated */
-#define WMI_CHAN_INFO_MSEC(x) ((x) / 76595)
-
 /* Beacon filter wmi command info */
 #define BCN_FLT_MAX_SUPPORTED_IES	256
 #define BCN_FLT_MAX_ELEMS_IE_LIST	(BCN_FLT_MAX_SUPPORTED_IES / 32)
@@ -4769,6 +4787,22 @@ struct wmi_dbglog_cfg_cmd {
 	__le32 config_valid;
 } __packed;
 
+enum wmi_roam_reason {
+	WMI_ROAM_REASON_BETTER_AP = 1,
+	WMI_ROAM_REASON_BEACON_MISS = 2,
+	WMI_ROAM_REASON_LOW_RSSI = 3,
+	WMI_ROAM_REASON_SUITABLE_AP_FOUND = 4,
+	WMI_ROAM_REASON_HO_FAILED = 5,
+
+	/* keep last */
+	WMI_ROAM_REASON_MAX,
+};
+
+struct wmi_roam_ev {
+	__le32 vdev_id;
+	__le32 reason;
+} __packed;
+
 #define ATH10K_FRAGMT_THRESHOLD_MIN	540
 #define ATH10K_FRAGMT_THRESHOLD_MAX	2346
 
@@ -4857,11 +4891,200 @@ struct wmi_rdy_ev_arg {
 	const u8 *mac_addr;
 };
 
+struct wmi_roam_ev_arg {
+	__le32 vdev_id;
+	__le32 reason;
+	__le32 rssi;
+};
+
 struct wmi_pdev_temperature_event {
 	/* temperature value in Celcius degree */
 	__le32 temperature;
 } __packed;
 
+/* WOW structures */
+enum wmi_wow_wakeup_event {
+	WOW_BMISS_EVENT = 0,
+	WOW_BETTER_AP_EVENT,
+	WOW_DEAUTH_RECVD_EVENT,
+	WOW_MAGIC_PKT_RECVD_EVENT,
+	WOW_GTK_ERR_EVENT,
+	WOW_FOURWAY_HSHAKE_EVENT,
+	WOW_EAPOL_RECVD_EVENT,
+	WOW_NLO_DETECTED_EVENT,
+	WOW_DISASSOC_RECVD_EVENT,
+	WOW_PATTERN_MATCH_EVENT,
+	WOW_CSA_IE_EVENT,
+	WOW_PROBE_REQ_WPS_IE_EVENT,
+	WOW_AUTH_REQ_EVENT,
+	WOW_ASSOC_REQ_EVENT,
+	WOW_HTT_EVENT,
+	WOW_RA_MATCH_EVENT,
+	WOW_HOST_AUTO_SHUTDOWN_EVENT,
+	WOW_IOAC_MAGIC_EVENT,
+	WOW_IOAC_SHORT_EVENT,
+	WOW_IOAC_EXTEND_EVENT,
+	WOW_IOAC_TIMER_EVENT,
+	WOW_DFS_PHYERR_RADAR_EVENT,
+	WOW_BEACON_EVENT,
+	WOW_CLIENT_KICKOUT_EVENT,
+	WOW_EVENT_MAX,
+};
+
+#define C2S(x) case x: return #x
+
+static inline const char *wow_wakeup_event(enum wmi_wow_wakeup_event ev)
+{
+	switch (ev) {
+	C2S(WOW_BMISS_EVENT);
+	C2S(WOW_BETTER_AP_EVENT);
+	C2S(WOW_DEAUTH_RECVD_EVENT);
+	C2S(WOW_MAGIC_PKT_RECVD_EVENT);
+	C2S(WOW_GTK_ERR_EVENT);
+	C2S(WOW_FOURWAY_HSHAKE_EVENT);
+	C2S(WOW_EAPOL_RECVD_EVENT);
+	C2S(WOW_NLO_DETECTED_EVENT);
+	C2S(WOW_DISASSOC_RECVD_EVENT);
+	C2S(WOW_PATTERN_MATCH_EVENT);
+	C2S(WOW_CSA_IE_EVENT);
+	C2S(WOW_PROBE_REQ_WPS_IE_EVENT);
+	C2S(WOW_AUTH_REQ_EVENT);
+	C2S(WOW_ASSOC_REQ_EVENT);
+	C2S(WOW_HTT_EVENT);
+	C2S(WOW_RA_MATCH_EVENT);
+	C2S(WOW_HOST_AUTO_SHUTDOWN_EVENT);
+	C2S(WOW_IOAC_MAGIC_EVENT);
+	C2S(WOW_IOAC_SHORT_EVENT);
+	C2S(WOW_IOAC_EXTEND_EVENT);
+	C2S(WOW_IOAC_TIMER_EVENT);
+	C2S(WOW_DFS_PHYERR_RADAR_EVENT);
+	C2S(WOW_BEACON_EVENT);
+	C2S(WOW_CLIENT_KICKOUT_EVENT);
+	C2S(WOW_EVENT_MAX);
+	default:
+		return NULL;
+	}
+}
+
+enum wmi_wow_wake_reason {
+	WOW_REASON_UNSPECIFIED = -1,
+	WOW_REASON_NLOD = 0,
+	WOW_REASON_AP_ASSOC_LOST,
+	WOW_REASON_LOW_RSSI,
+	WOW_REASON_DEAUTH_RECVD,
+	WOW_REASON_DISASSOC_RECVD,
+	WOW_REASON_GTK_HS_ERR,
+	WOW_REASON_EAP_REQ,
+	WOW_REASON_FOURWAY_HS_RECV,
+	WOW_REASON_TIMER_INTR_RECV,
+	WOW_REASON_PATTERN_MATCH_FOUND,
+	WOW_REASON_RECV_MAGIC_PATTERN,
+	WOW_REASON_P2P_DISC,
+	WOW_REASON_WLAN_HB,
+	WOW_REASON_CSA_EVENT,
+	WOW_REASON_PROBE_REQ_WPS_IE_RECV,
+	WOW_REASON_AUTH_REQ_RECV,
+	WOW_REASON_ASSOC_REQ_RECV,
+	WOW_REASON_HTT_EVENT,
+	WOW_REASON_RA_MATCH,
+	WOW_REASON_HOST_AUTO_SHUTDOWN,
+	WOW_REASON_IOAC_MAGIC_EVENT,
+	WOW_REASON_IOAC_SHORT_EVENT,
+	WOW_REASON_IOAC_EXTEND_EVENT,
+	WOW_REASON_IOAC_TIMER_EVENT,
+	WOW_REASON_ROAM_HO,
+	WOW_REASON_DFS_PHYERR_RADADR_EVENT,
+	WOW_REASON_BEACON_RECV,
+	WOW_REASON_CLIENT_KICKOUT_EVENT,
+	WOW_REASON_DEBUG_TEST = 0xFF,
+};
+
+static inline const char *wow_reason(enum wmi_wow_wake_reason reason)
+{
+	switch (reason) {
+	C2S(WOW_REASON_UNSPECIFIED);
+	C2S(WOW_REASON_NLOD);
+	C2S(WOW_REASON_AP_ASSOC_LOST);
+	C2S(WOW_REASON_LOW_RSSI);
+	C2S(WOW_REASON_DEAUTH_RECVD);
+	C2S(WOW_REASON_DISASSOC_RECVD);
+	C2S(WOW_REASON_GTK_HS_ERR);
+	C2S(WOW_REASON_EAP_REQ);
+	C2S(WOW_REASON_FOURWAY_HS_RECV);
+	C2S(WOW_REASON_TIMER_INTR_RECV);
+	C2S(WOW_REASON_PATTERN_MATCH_FOUND);
+	C2S(WOW_REASON_RECV_MAGIC_PATTERN);
+	C2S(WOW_REASON_P2P_DISC);
+	C2S(WOW_REASON_WLAN_HB);
+	C2S(WOW_REASON_CSA_EVENT);
+	C2S(WOW_REASON_PROBE_REQ_WPS_IE_RECV);
+	C2S(WOW_REASON_AUTH_REQ_RECV);
+	C2S(WOW_REASON_ASSOC_REQ_RECV);
+	C2S(WOW_REASON_HTT_EVENT);
+	C2S(WOW_REASON_RA_MATCH);
+	C2S(WOW_REASON_HOST_AUTO_SHUTDOWN);
+	C2S(WOW_REASON_IOAC_MAGIC_EVENT);
+	C2S(WOW_REASON_IOAC_SHORT_EVENT);
+	C2S(WOW_REASON_IOAC_EXTEND_EVENT);
+	C2S(WOW_REASON_IOAC_TIMER_EVENT);
+	C2S(WOW_REASON_ROAM_HO);
+	C2S(WOW_REASON_DFS_PHYERR_RADADR_EVENT);
+	C2S(WOW_REASON_BEACON_RECV);
+	C2S(WOW_REASON_CLIENT_KICKOUT_EVENT);
+	C2S(WOW_REASON_DEBUG_TEST);
+	default:
+		return NULL;
+	}
+}
+
+#undef C2S
+
+struct wmi_wow_ev_arg {
+	u32 vdev_id;
+	u32 flag;
+	enum wmi_wow_wake_reason wake_reason;
+	u32 data_len;
+};
+
+#define WOW_MIN_PATTERN_SIZE	1
+#define WOW_MAX_PATTERN_SIZE	148
+#define WOW_MAX_PKT_OFFSET	128
+
+enum wmi_tdls_state {
+	WMI_TDLS_DISABLE,
+	WMI_TDLS_ENABLE_PASSIVE,
+	WMI_TDLS_ENABLE_ACTIVE,
+};
+
+enum wmi_tdls_peer_state {
+	WMI_TDLS_PEER_STATE_PEERING,
+	WMI_TDLS_PEER_STATE_CONNECTED,
+	WMI_TDLS_PEER_STATE_TEARDOWN,
+};
+
+struct wmi_tdls_peer_update_cmd_arg {
+	u32 vdev_id;
+	enum wmi_tdls_peer_state peer_state;
+	u8 addr[ETH_ALEN];
+};
+
+#define WMI_TDLS_MAX_SUPP_OPER_CLASSES 32
+
+struct wmi_tdls_peer_capab_arg {
+	u8 peer_uapsd_queues;
+	u8 peer_max_sp;
+	u32 buff_sta_support;
+	u32 off_chan_support;
+	u32 peer_curr_operclass;
+	u32 self_curr_operclass;
+	u32 peer_chan_len;
+	u32 peer_operclass_len;
+	u8 peer_operclass[WMI_TDLS_MAX_SUPP_OPER_CLASSES];
+	u32 is_peer_responder;
+	u32 pref_offchan_num;
+	u32 pref_offchan_bw;
+};
+
 struct ath10k;
 struct ath10k_vif;
 struct ath10k_fw_stats_pdev;
diff --git a/drivers/net/wireless/ath/ath10k/wow.c b/drivers/net/wireless/ath/ath10k/wow.c
new file mode 100644
index 000000000000..a68d8fd853a3
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/wow.c
@@ -0,0 +1,321 @@
+/*
+ * Copyright (c) 2015 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 "mac.h"
+
+#include <net/mac80211.h>
+#include "hif.h"
+#include "core.h"
+#include "debug.h"
+#include "wmi.h"
+#include "wmi-ops.h"
+
+static const struct wiphy_wowlan_support ath10k_wowlan_support = {
+	.flags = WIPHY_WOWLAN_DISCONNECT |
+		 WIPHY_WOWLAN_MAGIC_PKT,
+	.pattern_min_len = WOW_MIN_PATTERN_SIZE,
+	.pattern_max_len = WOW_MAX_PATTERN_SIZE,
+	.max_pkt_offset = WOW_MAX_PKT_OFFSET,
+};
+
+static int ath10k_wow_vif_cleanup(struct ath10k_vif *arvif)
+{
+	struct ath10k *ar = arvif->ar;
+	int i, ret;
+
+	for (i = 0; i < WOW_EVENT_MAX; i++) {
+		ret = ath10k_wmi_wow_add_wakeup_event(ar, arvif->vdev_id, i, 0);
+		if (ret) {
+			ath10k_warn(ar, "failed to issue wow wakeup for event %s on vdev %i: %d\n",
+				    wow_wakeup_event(i), arvif->vdev_id, ret);
+			return ret;
+		}
+	}
+
+	for (i = 0; i < ar->wow.max_num_patterns; i++) {
+		ret = ath10k_wmi_wow_del_pattern(ar, arvif->vdev_id, i);
+		if (ret) {
+			ath10k_warn(ar, "failed to delete wow pattern %d for vdev %i: %d\n",
+				    i, arvif->vdev_id, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ath10k_wow_cleanup(struct ath10k *ar)
+{
+	struct ath10k_vif *arvif;
+	int ret;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	list_for_each_entry(arvif, &ar->arvifs, list) {
+		ret = ath10k_wow_vif_cleanup(arvif);
+		if (ret) {
+			ath10k_warn(ar, "failed to clean wow wakeups on vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ath10k_vif_wow_set_wakeups(struct ath10k_vif *arvif,
+				      struct cfg80211_wowlan *wowlan)
+{
+	int ret, i;
+	unsigned long wow_mask = 0;
+	struct ath10k *ar = arvif->ar;
+	const struct cfg80211_pkt_pattern *patterns = wowlan->patterns;
+	int pattern_id = 0;
+
+	/* Setup requested WOW features */
+	switch (arvif->vdev_type) {
+	case WMI_VDEV_TYPE_IBSS:
+		__set_bit(WOW_BEACON_EVENT, &wow_mask);
+		 /* fall through */
+	case WMI_VDEV_TYPE_AP:
+		__set_bit(WOW_DEAUTH_RECVD_EVENT, &wow_mask);
+		__set_bit(WOW_DISASSOC_RECVD_EVENT, &wow_mask);
+		__set_bit(WOW_PROBE_REQ_WPS_IE_EVENT, &wow_mask);
+		__set_bit(WOW_AUTH_REQ_EVENT, &wow_mask);
+		__set_bit(WOW_ASSOC_REQ_EVENT, &wow_mask);
+		__set_bit(WOW_HTT_EVENT, &wow_mask);
+		__set_bit(WOW_RA_MATCH_EVENT, &wow_mask);
+		break;
+	case WMI_VDEV_TYPE_STA:
+		if (wowlan->disconnect) {
+			__set_bit(WOW_DEAUTH_RECVD_EVENT, &wow_mask);
+			__set_bit(WOW_DISASSOC_RECVD_EVENT, &wow_mask);
+			__set_bit(WOW_BMISS_EVENT, &wow_mask);
+			__set_bit(WOW_CSA_IE_EVENT, &wow_mask);
+		}
+
+		if (wowlan->magic_pkt)
+			__set_bit(WOW_MAGIC_PKT_RECVD_EVENT, &wow_mask);
+		break;
+	default:
+		break;
+	}
+
+	for (i = 0; i < wowlan->n_patterns; i++) {
+		u8 bitmask[WOW_MAX_PATTERN_SIZE] = {};
+		int j;
+
+		if (patterns[i].pattern_len > WOW_MAX_PATTERN_SIZE)
+			continue;
+
+		/* convert bytemask to bitmask */
+		for (j = 0; j < patterns[i].pattern_len; j++)
+			if (patterns[i].mask[j / 8] & BIT(j % 8))
+				bitmask[j] = 0xff;
+
+		ret = ath10k_wmi_wow_add_pattern(ar, arvif->vdev_id,
+						 pattern_id,
+						 patterns[i].pattern,
+						 bitmask,
+						 patterns[i].pattern_len,
+						 patterns[i].pkt_offset);
+		if (ret) {
+			ath10k_warn(ar, "failed to add pattern %i to vdev %i: %d\n",
+				    pattern_id,
+				    arvif->vdev_id, ret);
+			return ret;
+		}
+
+		pattern_id++;
+		__set_bit(WOW_PATTERN_MATCH_EVENT, &wow_mask);
+	}
+
+	for (i = 0; i < WOW_EVENT_MAX; i++) {
+		if (!test_bit(i, &wow_mask))
+			continue;
+		ret = ath10k_wmi_wow_add_wakeup_event(ar, arvif->vdev_id, i, 1);
+		if (ret) {
+			ath10k_warn(ar, "failed to enable wakeup event %s on vdev %i: %d\n",
+				    wow_wakeup_event(i), arvif->vdev_id, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ath10k_wow_set_wakeups(struct ath10k *ar,
+				  struct cfg80211_wowlan *wowlan)
+{
+	struct ath10k_vif *arvif;
+	int ret;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	list_for_each_entry(arvif, &ar->arvifs, list) {
+		ret = ath10k_vif_wow_set_wakeups(arvif, wowlan);
+		if (ret) {
+			ath10k_warn(ar, "failed to set wow wakeups on vdev %i: %d\n",
+				    arvif->vdev_id, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int ath10k_wow_enable(struct ath10k *ar)
+{
+	int ret;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	reinit_completion(&ar->target_suspend);
+
+	ret = ath10k_wmi_wow_enable(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to issue wow enable: %d\n", ret);
+		return ret;
+	}
+
+	ret = wait_for_completion_timeout(&ar->target_suspend, 3 * HZ);
+	if (ret == 0) {
+		ath10k_warn(ar, "timed out while waiting for suspend completion\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int ath10k_wow_wakeup(struct ath10k *ar)
+{
+	int ret;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	reinit_completion(&ar->wow.wakeup_completed);
+
+	ret = ath10k_wmi_wow_host_wakeup_ind(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to send wow wakeup indication: %d\n",
+			    ret);
+		return ret;
+	}
+
+	ret = wait_for_completion_timeout(&ar->wow.wakeup_completed, 3 * HZ);
+	if (ret == 0) {
+		ath10k_warn(ar, "timed out while waiting for wow wakeup completion\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+int ath10k_wow_op_suspend(struct ieee80211_hw *hw,
+			  struct cfg80211_wowlan *wowlan)
+{
+	struct ath10k *ar = hw->priv;
+	int ret;
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (WARN_ON(!test_bit(ATH10K_FW_FEATURE_WOWLAN_SUPPORT,
+			      ar->fw_features))) {
+		ret = 1;
+		goto exit;
+	}
+
+	ret =  ath10k_wow_cleanup(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to clear wow wakeup events: %d\n",
+			    ret);
+		goto exit;
+	}
+
+	ret = ath10k_wow_set_wakeups(ar, wowlan);
+	if (ret) {
+		ath10k_warn(ar, "failed to set wow wakeup events: %d\n",
+			    ret);
+		goto cleanup;
+	}
+
+	ret = ath10k_wow_enable(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to start wow: %d\n", ret);
+		goto cleanup;
+	}
+
+	ret = ath10k_hif_suspend(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to suspend hif: %d\n", ret);
+		goto wakeup;
+	}
+
+	goto exit;
+
+wakeup:
+	ath10k_wow_wakeup(ar);
+
+cleanup:
+	ath10k_wow_cleanup(ar);
+
+exit:
+	mutex_unlock(&ar->conf_mutex);
+	return ret ? 1 : 0;
+}
+
+int ath10k_wow_op_resume(struct ieee80211_hw *hw)
+{
+	struct ath10k *ar = hw->priv;
+	int ret;
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (WARN_ON(!test_bit(ATH10K_FW_FEATURE_WOWLAN_SUPPORT,
+			      ar->fw_features))) {
+		ret = 1;
+		goto exit;
+	}
+
+	ret = ath10k_hif_resume(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to resume hif: %d\n", ret);
+		goto exit;
+	}
+
+	ret = ath10k_wow_wakeup(ar);
+	if (ret)
+		ath10k_warn(ar, "failed to wakeup from wow: %d\n", ret);
+
+exit:
+	mutex_unlock(&ar->conf_mutex);
+	return ret ? 1 : 0;
+}
+
+int ath10k_wow_init(struct ath10k *ar)
+{
+	if (!test_bit(ATH10K_FW_FEATURE_WOWLAN_SUPPORT, ar->fw_features))
+		return 0;
+
+	if (WARN_ON(!test_bit(WMI_SERVICE_WOW, ar->wmi.svc_map)))
+		return -EINVAL;
+
+	ar->wow.wowlan_support = ath10k_wowlan_support;
+	ar->wow.wowlan_support.n_patterns = ar->wow.max_num_patterns;
+	ar->hw->wiphy->wowlan = &ar->wow.wowlan_support;
+
+	return 0;
+}
diff --git a/drivers/net/wireless/ath/ath10k/wow.h b/drivers/net/wireless/ath/ath10k/wow.h
new file mode 100644
index 000000000000..abbb04b6d1bd
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/wow.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2015 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 _WOW_H_
+#define _WOW_H_
+
+struct ath10k_wow {
+	u32 max_num_patterns;
+	struct completion wakeup_completed;
+	struct wiphy_wowlan_support wowlan_support;
+};
+
+#ifdef CONFIG_PM
+
+int ath10k_wow_init(struct ath10k *ar);
+int ath10k_wow_op_suspend(struct ieee80211_hw *hw,
+			  struct cfg80211_wowlan *wowlan);
+int ath10k_wow_op_resume(struct ieee80211_hw *hw);
+
+#else
+
+static inline int ath10k_wow_init(struct ath10k *ar)
+{
+	return 0;
+}
+
+#endif /* CONFIG_PM */
+#endif /* _WOW_H_ */
diff --git a/drivers/net/wireless/ath/ath5k/ath5k.h b/drivers/net/wireless/ath/ath5k/ath5k.h
index 7ca0d6f930fd..e22b0e778927 100644
--- a/drivers/net/wireless/ath/ath5k/ath5k.h
+++ b/drivers/net/wireless/ath/ath5k/ath5k.h
@@ -1280,7 +1280,6 @@ struct ath5k_hw {
 
 	DECLARE_BITMAP(status, 4);
 #define ATH_STAT_INVALID	0		/* disable hardware accesses */
-#define ATH_STAT_PROMISC	1
 #define ATH_STAT_LEDSOFT	2		/* enable LED gpio status */
 #define ATH_STAT_STARTED	3		/* opened & irqs enabled */
 #define ATH_STAT_RESET		4		/* hw reset */
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index a6131825c9f6..23552f43d125 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -2537,12 +2537,12 @@ ath5k_init_ah(struct ath5k_hw *ah, const struct ath_bus_ops *bus_ops)
 
 	/* Initialize driver private data */
 	SET_IEEE80211_DEV(hw, ah->dev);
-	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-			IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-			IEEE80211_HW_SIGNAL_DBM |
-			IEEE80211_HW_MFP_CAPABLE |
-			IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-			IEEE80211_HW_SUPPORTS_RC_TABLE;
+	ieee80211_hw_set(hw, SUPPORTS_RC_TABLE);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, MFP_CAPABLE);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, HOST_BROADCAST_PS_BUFFERING);
 
 	hw->wiphy->interface_modes =
 		BIT(NL80211_IFTYPE_AP) |
diff --git a/drivers/net/wireless/ath/ath5k/led.c b/drivers/net/wireless/ath/ath5k/led.c
index ca4b7ccd697f..803030fd17d3 100644
--- a/drivers/net/wireless/ath/ath5k/led.c
+++ b/drivers/net/wireless/ath/ath5k/led.c
@@ -124,7 +124,7 @@ ath5k_led_brightness_set(struct led_classdev *led_dev,
 
 static int
 ath5k_register_led(struct ath5k_hw *ah, struct ath5k_led *led,
-		   const char *name, char *trigger)
+		   const char *name, const char *trigger)
 {
 	int err;
 
diff --git a/drivers/net/wireless/ath/ath5k/mac80211-ops.c b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
index 3b4a6463d87a..dc44cfef7517 100644
--- a/drivers/net/wireless/ath/ath5k/mac80211-ops.c
+++ b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
@@ -369,7 +369,7 @@ ath5k_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 		       unsigned int *new_flags, u64 multicast)
 {
 #define SUPPORTED_FIF_FLAGS \
-	(FIF_PROMISC_IN_BSS |  FIF_ALLMULTI | FIF_FCSFAIL | \
+	(FIF_ALLMULTI | FIF_FCSFAIL | \
 	FIF_PLCPFAIL | FIF_CONTROL | FIF_OTHER_BSS | \
 	FIF_BCN_PRBRESP_PROMISC)
 
@@ -393,16 +393,6 @@ ath5k_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 		(AR5K_RX_FILTER_UCAST | AR5K_RX_FILTER_BCAST |
 		AR5K_RX_FILTER_MCAST);
 
-	if (changed_flags & (FIF_PROMISC_IN_BSS | FIF_OTHER_BSS)) {
-		if (*new_flags & FIF_PROMISC_IN_BSS)
-			__set_bit(ATH_STAT_PROMISC, ah->status);
-		else
-			__clear_bit(ATH_STAT_PROMISC, ah->status);
-	}
-
-	if (test_bit(ATH_STAT_PROMISC, ah->status))
-		rfilt |= AR5K_RX_FILTER_PROM;
-
 	/* Note, AR5K_RX_FILTER_MCAST is already enabled */
 	if (*new_flags & FIF_ALLMULTI) {
 		mfilt[0] =  ~0;
@@ -418,8 +408,7 @@ ath5k_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 	if ((*new_flags & FIF_BCN_PRBRESP_PROMISC) || (ah->nvifs > 1))
 		rfilt |= AR5K_RX_FILTER_BEACON;
 
-	/* FIF_CONTROL doc says that if FIF_PROMISC_IN_BSS is not
-	 * set we should only pass on control frames for this
+	/* FIF_CONTROL doc says we should only pass on control frames for this
 	 * station. This needs testing. I believe right now this
 	 * enables *all* control frames, which is OK.. but
 	 * but we should see if we can improve on granularity */
@@ -809,7 +798,6 @@ const struct ieee80211_ops ath5k_hw_ops = {
 	.sw_scan_start		= ath5k_sw_scan_start,
 	.sw_scan_complete	= ath5k_sw_scan_complete,
 	.get_stats		= ath5k_get_stats,
-	/* .get_tkip_seq	= not implemented */
 	/* .set_frag_threshold	= not implemented */
 	/* .set_rts_threshold	= not implemented */
 	/* .sta_add		= not implemented */
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index cce4625a53ad..a511ef3614b9 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -889,7 +889,7 @@ void ath6kl_cfg80211_disconnect_event(struct ath6kl_vif *vif, u8 reason,
 					GFP_KERNEL);
 	} else if (vif->sme_state == SME_CONNECTED) {
 		cfg80211_disconnected(vif->ndev, proto_reason,
-				      NULL, 0, GFP_KERNEL);
+				      NULL, 0, false, GFP_KERNEL);
 	}
 
 	vif->sme_state = SME_DISCONNECTED;
@@ -3467,7 +3467,7 @@ void ath6kl_cfg80211_stop(struct ath6kl_vif *vif)
 					GFP_KERNEL);
 		break;
 	case SME_CONNECTED:
-		cfg80211_disconnected(vif->ndev, 0, NULL, 0, GFP_KERNEL);
+		cfg80211_disconnected(vif->ndev, 0, NULL, 0, true, GFP_KERNEL);
 		break;
 	}
 
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.h b/drivers/net/wireless/ath/ath6kl/wmi.h
index 19f88b4a24fb..05d25a94c781 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.h
+++ b/drivers/net/wireless/ath/ath6kl/wmi.h
@@ -1527,8 +1527,8 @@ struct wmi_connect_event {
 			__le32 nw_type;
 		} sta;
 		struct {
-			u8 phymode;
 			u8 aid;
+			u8 phymode;
 			u8 mac_addr[ETH_ALEN];
 			u8 auth;
 			u8 keymgmt;
diff --git a/drivers/net/wireless/ath/ath9k/ar5008_phy.c b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
index 6c23d279525f..8f8793004b9f 100644
--- a/drivers/net/wireless/ath/ath9k/ar5008_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
@@ -254,86 +254,25 @@ static int ar5008_hw_set_channel(struct ath_hw *ah, struct ath9k_channel *chan)
 	return 0;
 }
 
-/**
- * ar5008_hw_spur_mitigate - convert baseband spur frequency for external radios
- * @ah: atheros hardware structure
- * @chan:
- *
- * For non single-chip solutions. Converts to baseband spur frequency given the
- * input channel frequency and compute register settings below.
- */
-static void ar5008_hw_spur_mitigate(struct ath_hw *ah,
-				    struct ath9k_channel *chan)
+void ar5008_hw_cmn_spur_mitigate(struct ath_hw *ah,
+			  struct ath9k_channel *chan, int bin)
 {
-	int bb_spur = AR_NO_SPUR;
-	int bin, cur_bin;
-	int spur_freq_sd;
-	int spur_delta_phase;
-	int denominator;
+	int cur_bin;
 	int upper, lower, cur_vit_mask;
-	int tmp, new;
 	int i;
-	static int pilot_mask_reg[4] = {
+	int8_t mask_m[123];
+	int8_t mask_p[123];
+	int8_t mask_amt;
+	int tmp_mask;
+	static const int pilot_mask_reg[4] = {
 		AR_PHY_TIMING7, AR_PHY_TIMING8,
 		AR_PHY_PILOT_MASK_01_30, AR_PHY_PILOT_MASK_31_60
 	};
-	static int chan_mask_reg[4] = {
+	static const int chan_mask_reg[4] = {
 		AR_PHY_TIMING9, AR_PHY_TIMING10,
 		AR_PHY_CHANNEL_MASK_01_30, AR_PHY_CHANNEL_MASK_31_60
 	};
-	static int inc[4] = { 0, 100, 0, 0 };
-
-	int8_t mask_m[123];
-	int8_t mask_p[123];
-	int8_t mask_amt;
-	int tmp_mask;
-	int cur_bb_spur;
-	bool is2GHz = IS_CHAN_2GHZ(chan);
-
-	memset(&mask_m, 0, sizeof(int8_t) * 123);
-	memset(&mask_p, 0, sizeof(int8_t) * 123);
-
-	for (i = 0; i < AR_EEPROM_MODAL_SPURS; i++) {
-		cur_bb_spur = ah->eep_ops->get_spur_channel(ah, i, is2GHz);
-		if (AR_NO_SPUR == cur_bb_spur)
-			break;
-		cur_bb_spur = cur_bb_spur - (chan->channel * 10);
-		if ((cur_bb_spur > -95) && (cur_bb_spur < 95)) {
-			bb_spur = cur_bb_spur;
-			break;
-		}
-	}
-
-	if (AR_NO_SPUR == bb_spur)
-		return;
-
-	bin = bb_spur * 32;
-
-	tmp = REG_READ(ah, AR_PHY_TIMING_CTRL4(0));
-	new = tmp | (AR_PHY_TIMING_CTRL4_ENABLE_SPUR_RSSI |
-		     AR_PHY_TIMING_CTRL4_ENABLE_SPUR_FILTER |
-		     AR_PHY_TIMING_CTRL4_ENABLE_CHAN_MASK |
-		     AR_PHY_TIMING_CTRL4_ENABLE_PILOT_MASK);
-
-	REG_WRITE(ah, AR_PHY_TIMING_CTRL4(0), new);
-
-	new = (AR_PHY_SPUR_REG_MASK_RATE_CNTL |
-	       AR_PHY_SPUR_REG_ENABLE_MASK_PPM |
-	       AR_PHY_SPUR_REG_MASK_RATE_SELECT |
-	       AR_PHY_SPUR_REG_ENABLE_VIT_SPUR_RSSI |
-	       SM(SPUR_RSSI_THRESH, AR_PHY_SPUR_REG_SPUR_RSSI_THRESH));
-	REG_WRITE(ah, AR_PHY_SPUR_REG, new);
-
-	spur_delta_phase = ((bb_spur * 524288) / 100) &
-		AR_PHY_TIMING11_SPUR_DELTA_PHASE;
-
-	denominator = IS_CHAN_2GHZ(chan) ? 440 : 400;
-	spur_freq_sd = ((bb_spur * 2048) / denominator) & 0x3ff;
-
-	new = (AR_PHY_TIMING11_USE_SPUR_IN_AGC |
-	       SM(spur_freq_sd, AR_PHY_TIMING11_SPUR_FREQ_SD) |
-	       SM(spur_delta_phase, AR_PHY_TIMING11_SPUR_DELTA_PHASE));
-	REG_WRITE(ah, AR_PHY_TIMING11, new);
+	static const int inc[4] = { 0, 100, 0, 0 };
 
 	cur_bin = -6000;
 	upper = bin + 100;
@@ -343,6 +282,7 @@ static void ar5008_hw_spur_mitigate(struct ath_hw *ah,
 		int pilot_mask = 0;
 		int chan_mask = 0;
 		int bp = 0;
+
 		for (bp = 0; bp < 30; bp++) {
 			if ((cur_bin > lower) && (cur_bin < upper)) {
 				pilot_mask = pilot_mask | 0x1 << bp;
@@ -361,7 +301,6 @@ static void ar5008_hw_spur_mitigate(struct ath_hw *ah,
 
 	for (i = 0; i < 123; i++) {
 		if ((cur_vit_mask > lower) && (cur_vit_mask < upper)) {
-
 			/* workaround for gcc bug #37014 */
 			volatile int tmp_v = abs(cur_vit_mask - bin);
 
@@ -467,6 +406,78 @@ static void ar5008_hw_spur_mitigate(struct ath_hw *ah,
 }
 
 /**
+ * ar5008_hw_spur_mitigate - convert baseband spur frequency for external radios
+ * @ah: atheros hardware structure
+ * @chan:
+ *
+ * For non single-chip solutions. Converts to baseband spur frequency given the
+ * input channel frequency and compute register settings below.
+ */
+static void ar5008_hw_spur_mitigate(struct ath_hw *ah,
+				    struct ath9k_channel *chan)
+{
+	int bb_spur = AR_NO_SPUR;
+	int bin;
+	int spur_freq_sd;
+	int spur_delta_phase;
+	int denominator;
+	int tmp, new;
+	int i;
+
+	int8_t mask_m[123];
+	int8_t mask_p[123];
+	int cur_bb_spur;
+	bool is2GHz = IS_CHAN_2GHZ(chan);
+
+	memset(&mask_m, 0, sizeof(int8_t) * 123);
+	memset(&mask_p, 0, sizeof(int8_t) * 123);
+
+	for (i = 0; i < AR_EEPROM_MODAL_SPURS; i++) {
+		cur_bb_spur = ah->eep_ops->get_spur_channel(ah, i, is2GHz);
+		if (AR_NO_SPUR == cur_bb_spur)
+			break;
+		cur_bb_spur = cur_bb_spur - (chan->channel * 10);
+		if ((cur_bb_spur > -95) && (cur_bb_spur < 95)) {
+			bb_spur = cur_bb_spur;
+			break;
+		}
+	}
+
+	if (AR_NO_SPUR == bb_spur)
+		return;
+
+	bin = bb_spur * 32;
+
+	tmp = REG_READ(ah, AR_PHY_TIMING_CTRL4(0));
+	new = tmp | (AR_PHY_TIMING_CTRL4_ENABLE_SPUR_RSSI |
+		     AR_PHY_TIMING_CTRL4_ENABLE_SPUR_FILTER |
+		     AR_PHY_TIMING_CTRL4_ENABLE_CHAN_MASK |
+		     AR_PHY_TIMING_CTRL4_ENABLE_PILOT_MASK);
+
+	REG_WRITE(ah, AR_PHY_TIMING_CTRL4(0), new);
+
+	new = (AR_PHY_SPUR_REG_MASK_RATE_CNTL |
+	       AR_PHY_SPUR_REG_ENABLE_MASK_PPM |
+	       AR_PHY_SPUR_REG_MASK_RATE_SELECT |
+	       AR_PHY_SPUR_REG_ENABLE_VIT_SPUR_RSSI |
+	       SM(SPUR_RSSI_THRESH, AR_PHY_SPUR_REG_SPUR_RSSI_THRESH));
+	REG_WRITE(ah, AR_PHY_SPUR_REG, new);
+
+	spur_delta_phase = ((bb_spur * 524288) / 100) &
+		AR_PHY_TIMING11_SPUR_DELTA_PHASE;
+
+	denominator = IS_CHAN_2GHZ(chan) ? 440 : 400;
+	spur_freq_sd = ((bb_spur * 2048) / denominator) & 0x3ff;
+
+	new = (AR_PHY_TIMING11_USE_SPUR_IN_AGC |
+	       SM(spur_freq_sd, AR_PHY_TIMING11_SPUR_FREQ_SD) |
+	       SM(spur_delta_phase, AR_PHY_TIMING11_SPUR_DELTA_PHASE));
+	REG_WRITE(ah, AR_PHY_TIMING11, new);
+
+	ar5008_hw_cmn_spur_mitigate(ah, chan, bin);
+}
+
+/**
  * ar5008_hw_rf_alloc_ext_banks - allocates banks for external radio programming
  * @ah: atheros hardware structure
  *
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_phy.c b/drivers/net/wireless/ath/ath9k/ar9002_phy.c
index fc08162b5820..db6624527d99 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_phy.c
@@ -169,29 +169,17 @@ static void ar9002_hw_spur_mitigate(struct ath_hw *ah,
 {
 	int bb_spur = AR_NO_SPUR;
 	int freq;
-	int bin, cur_bin;
+	int bin;
 	int bb_spur_off, spur_subchannel_sd;
 	int spur_freq_sd;
 	int spur_delta_phase;
 	int denominator;
-	int upper, lower, cur_vit_mask;
 	int tmp, newVal;
 	int i;
-	static const int pilot_mask_reg[4] = {
-		AR_PHY_TIMING7, AR_PHY_TIMING8,
-		AR_PHY_PILOT_MASK_01_30, AR_PHY_PILOT_MASK_31_60
-	};
-	static const int chan_mask_reg[4] = {
-		AR_PHY_TIMING9, AR_PHY_TIMING10,
-		AR_PHY_CHANNEL_MASK_01_30, AR_PHY_CHANNEL_MASK_31_60
-	};
-	static const int inc[4] = { 0, 100, 0, 0 };
 	struct chan_centers centers;
 
 	int8_t mask_m[123];
 	int8_t mask_p[123];
-	int8_t mask_amt;
-	int tmp_mask;
 	int cur_bb_spur;
 	bool is2GHz = IS_CHAN_2GHZ(chan);
 
@@ -288,135 +276,7 @@ static void ar9002_hw_spur_mitigate(struct ath_hw *ah,
 	newVal = spur_subchannel_sd << AR_PHY_SFCORR_SPUR_SUBCHNL_SD_S;
 	REG_WRITE(ah, AR_PHY_SFCORR_EXT, newVal);
 
-	cur_bin = -6000;
-	upper = bin + 100;
-	lower = bin - 100;
-
-	for (i = 0; i < 4; i++) {
-		int pilot_mask = 0;
-		int chan_mask = 0;
-		int bp = 0;
-		for (bp = 0; bp < 30; bp++) {
-			if ((cur_bin > lower) && (cur_bin < upper)) {
-				pilot_mask = pilot_mask | 0x1 << bp;
-				chan_mask = chan_mask | 0x1 << bp;
-			}
-			cur_bin += 100;
-		}
-		cur_bin += inc[i];
-		REG_WRITE(ah, pilot_mask_reg[i], pilot_mask);
-		REG_WRITE(ah, chan_mask_reg[i], chan_mask);
-	}
-
-	cur_vit_mask = 6100;
-	upper = bin + 120;
-	lower = bin - 120;
-
-	for (i = 0; i < 123; i++) {
-		if ((cur_vit_mask > lower) && (cur_vit_mask < upper)) {
-
-			/* workaround for gcc bug #37014 */
-			volatile int tmp_v = abs(cur_vit_mask - bin);
-
-			if (tmp_v < 75)
-				mask_amt = 1;
-			else
-				mask_amt = 0;
-			if (cur_vit_mask < 0)
-				mask_m[abs(cur_vit_mask / 100)] = mask_amt;
-			else
-				mask_p[cur_vit_mask / 100] = mask_amt;
-		}
-		cur_vit_mask -= 100;
-	}
-
-	tmp_mask = (mask_m[46] << 30) | (mask_m[47] << 28)
-		| (mask_m[48] << 26) | (mask_m[49] << 24)
-		| (mask_m[50] << 22) | (mask_m[51] << 20)
-		| (mask_m[52] << 18) | (mask_m[53] << 16)
-		| (mask_m[54] << 14) | (mask_m[55] << 12)
-		| (mask_m[56] << 10) | (mask_m[57] << 8)
-		| (mask_m[58] << 6) | (mask_m[59] << 4)
-		| (mask_m[60] << 2) | (mask_m[61] << 0);
-	REG_WRITE(ah, AR_PHY_BIN_MASK_1, tmp_mask);
-	REG_WRITE(ah, AR_PHY_VIT_MASK2_M_46_61, tmp_mask);
-
-	tmp_mask = (mask_m[31] << 28)
-		| (mask_m[32] << 26) | (mask_m[33] << 24)
-		| (mask_m[34] << 22) | (mask_m[35] << 20)
-		| (mask_m[36] << 18) | (mask_m[37] << 16)
-		| (mask_m[48] << 14) | (mask_m[39] << 12)
-		| (mask_m[40] << 10) | (mask_m[41] << 8)
-		| (mask_m[42] << 6) | (mask_m[43] << 4)
-		| (mask_m[44] << 2) | (mask_m[45] << 0);
-	REG_WRITE(ah, AR_PHY_BIN_MASK_2, tmp_mask);
-	REG_WRITE(ah, AR_PHY_MASK2_M_31_45, tmp_mask);
-
-	tmp_mask = (mask_m[16] << 30) | (mask_m[16] << 28)
-		| (mask_m[18] << 26) | (mask_m[18] << 24)
-		| (mask_m[20] << 22) | (mask_m[20] << 20)
-		| (mask_m[22] << 18) | (mask_m[22] << 16)
-		| (mask_m[24] << 14) | (mask_m[24] << 12)
-		| (mask_m[25] << 10) | (mask_m[26] << 8)
-		| (mask_m[27] << 6) | (mask_m[28] << 4)
-		| (mask_m[29] << 2) | (mask_m[30] << 0);
-	REG_WRITE(ah, AR_PHY_BIN_MASK_3, tmp_mask);
-	REG_WRITE(ah, AR_PHY_MASK2_M_16_30, tmp_mask);
-
-	tmp_mask = (mask_m[0] << 30) | (mask_m[1] << 28)
-		| (mask_m[2] << 26) | (mask_m[3] << 24)
-		| (mask_m[4] << 22) | (mask_m[5] << 20)
-		| (mask_m[6] << 18) | (mask_m[7] << 16)
-		| (mask_m[8] << 14) | (mask_m[9] << 12)
-		| (mask_m[10] << 10) | (mask_m[11] << 8)
-		| (mask_m[12] << 6) | (mask_m[13] << 4)
-		| (mask_m[14] << 2) | (mask_m[15] << 0);
-	REG_WRITE(ah, AR_PHY_MASK_CTL, tmp_mask);
-	REG_WRITE(ah, AR_PHY_MASK2_M_00_15, tmp_mask);
-
-	tmp_mask = (mask_p[15] << 28)
-		| (mask_p[14] << 26) | (mask_p[13] << 24)
-		| (mask_p[12] << 22) | (mask_p[11] << 20)
-		| (mask_p[10] << 18) | (mask_p[9] << 16)
-		| (mask_p[8] << 14) | (mask_p[7] << 12)
-		| (mask_p[6] << 10) | (mask_p[5] << 8)
-		| (mask_p[4] << 6) | (mask_p[3] << 4)
-		| (mask_p[2] << 2) | (mask_p[1] << 0);
-	REG_WRITE(ah, AR_PHY_BIN_MASK2_1, tmp_mask);
-	REG_WRITE(ah, AR_PHY_MASK2_P_15_01, tmp_mask);
-
-	tmp_mask = (mask_p[30] << 28)
-		| (mask_p[29] << 26) | (mask_p[28] << 24)
-		| (mask_p[27] << 22) | (mask_p[26] << 20)
-		| (mask_p[25] << 18) | (mask_p[24] << 16)
-		| (mask_p[23] << 14) | (mask_p[22] << 12)
-		| (mask_p[21] << 10) | (mask_p[20] << 8)
-		| (mask_p[19] << 6) | (mask_p[18] << 4)
-		| (mask_p[17] << 2) | (mask_p[16] << 0);
-	REG_WRITE(ah, AR_PHY_BIN_MASK2_2, tmp_mask);
-	REG_WRITE(ah, AR_PHY_MASK2_P_30_16, tmp_mask);
-
-	tmp_mask = (mask_p[45] << 28)
-		| (mask_p[44] << 26) | (mask_p[43] << 24)
-		| (mask_p[42] << 22) | (mask_p[41] << 20)
-		| (mask_p[40] << 18) | (mask_p[39] << 16)
-		| (mask_p[38] << 14) | (mask_p[37] << 12)
-		| (mask_p[36] << 10) | (mask_p[35] << 8)
-		| (mask_p[34] << 6) | (mask_p[33] << 4)
-		| (mask_p[32] << 2) | (mask_p[31] << 0);
-	REG_WRITE(ah, AR_PHY_BIN_MASK2_3, tmp_mask);
-	REG_WRITE(ah, AR_PHY_MASK2_P_45_31, tmp_mask);
-
-	tmp_mask = (mask_p[61] << 30) | (mask_p[60] << 28)
-		| (mask_p[59] << 26) | (mask_p[58] << 24)
-		| (mask_p[57] << 22) | (mask_p[56] << 20)
-		| (mask_p[55] << 18) | (mask_p[54] << 16)
-		| (mask_p[53] << 14) | (mask_p[52] << 12)
-		| (mask_p[51] << 10) | (mask_p[50] << 8)
-		| (mask_p[49] << 6) | (mask_p[48] << 4)
-		| (mask_p[47] << 2) | (mask_p[46] << 0);
-	REG_WRITE(ah, AR_PHY_BIN_MASK2_4, tmp_mask);
-	REG_WRITE(ah, AR_PHY_MASK2_P_61_45, tmp_mask);
+	ar5008_hw_cmn_spur_mitigate(ah, chan, bin);
 
 	REGWRITE_BUFFER_FLUSH(ah);
 }
diff --git a/drivers/net/wireless/ath/ath9k/common-spectral.c b/drivers/net/wireless/ath/ath9k/common-spectral.c
index 5cee231cca1f..a8762711ad74 100644
--- a/drivers/net/wireless/ath/ath9k/common-spectral.c
+++ b/drivers/net/wireless/ath/ath9k/common-spectral.c
@@ -15,6 +15,7 @@
  */
 
 #include <linux/relay.h>
+#include <linux/random.h>
 #include "ath9k.h"
 
 static s8 fix_rssi_inv_only(u8 rssi_val)
@@ -36,21 +37,480 @@ static void ath_debug_send_fft_sample(struct ath_spec_scan_priv *spec_priv,
 	relay_write(spec_priv->rfs_chan_spec_scan, fft_sample_tlv, length);
 }
 
+typedef int (ath_cmn_fft_idx_validator) (u8 *sample_end, int bytes_read);
+
+static int
+ath_cmn_max_idx_verify_ht20_fft(u8 *sample_end, int bytes_read)
+{
+	struct ath_ht20_mag_info *mag_info;
+	u8 *sample;
+	u16 max_magnitude;
+	u8 max_index;
+	u8 max_exp;
+
+	/* Sanity check so that we don't read outside the read
+	 * buffer
+	 */
+	if (bytes_read < SPECTRAL_HT20_SAMPLE_LEN - 1)
+		return -1;
+
+	mag_info = (struct ath_ht20_mag_info *) (sample_end -
+				sizeof(struct ath_ht20_mag_info) + 1);
+
+	sample = sample_end - SPECTRAL_HT20_SAMPLE_LEN + 1;
+
+	max_index = spectral_max_index(mag_info->all_bins,
+				       SPECTRAL_HT20_NUM_BINS);
+	max_magnitude = spectral_max_magnitude(mag_info->all_bins);
+
+	max_exp = mag_info->max_exp & 0xf;
+
+	/* Don't try to read something outside the read buffer
+	 * in case of a missing byte (so bins[0] will be outside
+	 * the read buffer)
+	 */
+	if (bytes_read < SPECTRAL_HT20_SAMPLE_LEN && max_index < 1)
+		return -1;
+
+	if (sample[max_index] != (max_magnitude >> max_exp))
+		return -1;
+	else
+		return 0;
+}
+
+static int
+ath_cmn_max_idx_verify_ht20_40_fft(u8 *sample_end, int bytes_read)
+{
+	struct ath_ht20_40_mag_info *mag_info;
+	u8 *sample;
+	u16 lower_mag, upper_mag;
+	u8 lower_max_index, upper_max_index;
+	u8 max_exp;
+	int dc_pos = SPECTRAL_HT20_40_NUM_BINS / 2;
+
+	/* Sanity check so that we don't read outside the read
+	 * buffer
+	 */
+	if (bytes_read < SPECTRAL_HT20_40_SAMPLE_LEN - 1)
+		return -1;
+
+	mag_info = (struct ath_ht20_40_mag_info *) (sample_end -
+				sizeof(struct ath_ht20_40_mag_info) + 1);
+
+	sample = sample_end - SPECTRAL_HT20_40_SAMPLE_LEN + 1;
+
+	lower_mag = spectral_max_magnitude(mag_info->lower_bins);
+	lower_max_index = spectral_max_index(mag_info->lower_bins,
+					     SPECTRAL_HT20_40_NUM_BINS);
+
+	upper_mag = spectral_max_magnitude(mag_info->upper_bins);
+	upper_max_index = spectral_max_index(mag_info->upper_bins,
+					     SPECTRAL_HT20_40_NUM_BINS);
+
+	max_exp = mag_info->max_exp & 0xf;
+
+	/* Don't try to read something outside the read buffer
+	 * in case of a missing byte (so bins[0] will be outside
+	 * the read buffer)
+	 */
+	if (bytes_read < SPECTRAL_HT20_40_SAMPLE_LEN &&
+	   ((upper_max_index < 1) || (lower_max_index < 1)))
+		return -1;
+
+	/* Some time hardware messes up the index and adds
+	 * the index of the middle point (dc_pos). Try to fix it.
+	 */
+	if ((upper_max_index - dc_pos > 0) &&
+	   (sample[upper_max_index] == (upper_mag >> max_exp)))
+		upper_max_index -= dc_pos;
+
+	if ((lower_max_index - dc_pos > 0) &&
+	   (sample[lower_max_index - dc_pos] == (lower_mag >> max_exp)))
+		lower_max_index -= dc_pos;
+
+	if ((sample[upper_max_index + dc_pos] != (upper_mag >> max_exp)) ||
+	   (sample[lower_max_index] != (lower_mag >> max_exp)))
+		return -1;
+	else
+		return 0;
+}
+
+typedef int (ath_cmn_fft_sample_handler) (struct ath_rx_status *rs,
+			struct ath_spec_scan_priv *spec_priv,
+			u8 *sample_buf, u64 tsf, u16 freq, int chan_type);
+
+static int
+ath_cmn_process_ht20_fft(struct ath_rx_status *rs,
+			struct ath_spec_scan_priv *spec_priv,
+			u8 *sample_buf,
+			u64 tsf, u16 freq, int chan_type)
+{
+	struct fft_sample_ht20 fft_sample_20;
+	struct ath_common *common = ath9k_hw_common(spec_priv->ah);
+	struct ath_hw *ah = spec_priv->ah;
+	struct ath_ht20_mag_info *mag_info;
+	struct fft_sample_tlv *tlv;
+	int i = 0;
+	int ret = 0;
+	int dc_pos = SPECTRAL_HT20_NUM_BINS / 2;
+	u16 magnitude, tmp_mag, length;
+	u8 max_index, bitmap_w, max_exp;
+
+	length = sizeof(fft_sample_20) - sizeof(struct fft_sample_tlv);
+	fft_sample_20.tlv.type = ATH_FFT_SAMPLE_HT20;
+	fft_sample_20.tlv.length = __cpu_to_be16(length);
+	fft_sample_20.freq = __cpu_to_be16(freq);
+	fft_sample_20.rssi = fix_rssi_inv_only(rs->rs_rssi_ctl[0]);
+	fft_sample_20.noise = ah->noise;
+
+	mag_info = (struct ath_ht20_mag_info *) (sample_buf +
+					SPECTRAL_HT20_NUM_BINS);
+
+	magnitude = spectral_max_magnitude(mag_info->all_bins);
+	fft_sample_20.max_magnitude = __cpu_to_be16(magnitude);
+
+	max_index = spectral_max_index(mag_info->all_bins,
+					SPECTRAL_HT20_NUM_BINS);
+	fft_sample_20.max_index = max_index;
+
+	bitmap_w = spectral_bitmap_weight(mag_info->all_bins);
+	fft_sample_20.bitmap_weight = bitmap_w;
+
+	max_exp = mag_info->max_exp & 0xf;
+	fft_sample_20.max_exp = max_exp;
+
+	fft_sample_20.tsf = __cpu_to_be64(tsf);
+
+	memcpy(fft_sample_20.data, sample_buf, SPECTRAL_HT20_NUM_BINS);
+
+	ath_dbg(common, SPECTRAL_SCAN, "FFT HT20 frame: max mag 0x%X,"
+					"max_mag_idx %i\n",
+					magnitude >> max_exp,
+					max_index);
+
+	if (fft_sample_20.data[max_index] != (magnitude >> max_exp)) {
+		ath_dbg(common, SPECTRAL_SCAN, "Magnitude mismatch !\n");
+		ret = -1;
+	}
+
+	/* DC value (value in the middle) is the blind spot of the spectral
+	 * sample and invalid, interpolate it.
+	 */
+	fft_sample_20.data[dc_pos] = (fft_sample_20.data[dc_pos + 1] +
+					fft_sample_20.data[dc_pos - 1]) / 2;
+
+	/* Check if the maximum magnitude is indeed maximum,
+	 * also if the maximum value was at dc_pos, calculate
+	 * a new one (since value at dc_pos is invalid).
+	 */
+	if (max_index == dc_pos) {
+		tmp_mag = 0;
+		for (i = 0; i < dc_pos; i++) {
+			if (fft_sample_20.data[i] > tmp_mag) {
+				tmp_mag = fft_sample_20.data[i];
+				fft_sample_20.max_index = i;
+			}
+		}
+
+		magnitude = tmp_mag << max_exp;
+		fft_sample_20.max_magnitude = __cpu_to_be16(magnitude);
+
+		ath_dbg(common, SPECTRAL_SCAN,
+			"Calculated new lower max 0x%X at %i\n",
+			tmp_mag, fft_sample_20.max_index);
+	} else
+	for (i = 0; i < SPECTRAL_HT20_NUM_BINS; i++) {
+		if (fft_sample_20.data[i] == (magnitude >> max_exp))
+			ath_dbg(common, SPECTRAL_SCAN,
+				"Got max: 0x%X at index %i\n",
+				fft_sample_20.data[i], i);
+
+		if (fft_sample_20.data[i] > (magnitude >> max_exp)) {
+			ath_dbg(common, SPECTRAL_SCAN,
+				"Got bin %i greater than max: 0x%X\n",
+				i, fft_sample_20.data[i]);
+			ret = -1;
+		}
+	}
+
+	if (ret < 0)
+		return ret;
+
+	tlv = (struct fft_sample_tlv *)&fft_sample_20;
+
+	ath_debug_send_fft_sample(spec_priv, tlv);
+
+	return 0;
+}
+
+static int
+ath_cmn_process_ht20_40_fft(struct ath_rx_status *rs,
+			struct ath_spec_scan_priv *spec_priv,
+			u8 *sample_buf,
+			u64 tsf, u16 freq, int chan_type)
+{
+	struct fft_sample_ht20_40 fft_sample_40;
+	struct ath_common *common = ath9k_hw_common(spec_priv->ah);
+	struct ath_hw *ah = spec_priv->ah;
+	struct ath9k_hw_cal_data *caldata = ah->caldata;
+	struct ath_ht20_40_mag_info *mag_info;
+	struct fft_sample_tlv *tlv;
+	int dc_pos = SPECTRAL_HT20_40_NUM_BINS / 2;
+	int i = 0;
+	int ret = 0;
+	s16 ext_nf;
+	u16 lower_mag, upper_mag, tmp_mag, length;
+	s8 lower_rssi, upper_rssi;
+	u8 lower_max_index, upper_max_index;
+	u8 lower_bitmap_w, upper_bitmap_w, max_exp;
+
+	if (caldata)
+		ext_nf = ath9k_hw_getchan_noise(ah, ah->curchan,
+				caldata->nfCalHist[3].privNF);
+	else
+		ext_nf = ATH_DEFAULT_NOISE_FLOOR;
+
+	length = sizeof(fft_sample_40) - sizeof(struct fft_sample_tlv);
+	fft_sample_40.tlv.type = ATH_FFT_SAMPLE_HT20_40;
+	fft_sample_40.tlv.length = __cpu_to_be16(length);
+	fft_sample_40.freq = __cpu_to_be16(freq);
+	fft_sample_40.channel_type = chan_type;
+
+	if (chan_type == NL80211_CHAN_HT40PLUS) {
+		lower_rssi = fix_rssi_inv_only(rs->rs_rssi_ctl[0]);
+		upper_rssi = fix_rssi_inv_only(rs->rs_rssi_ext[0]);
+
+		fft_sample_40.lower_noise = ah->noise;
+		fft_sample_40.upper_noise = ext_nf;
+	} else {
+		lower_rssi = fix_rssi_inv_only(rs->rs_rssi_ext[0]);
+		upper_rssi = fix_rssi_inv_only(rs->rs_rssi_ctl[0]);
+
+		fft_sample_40.lower_noise = ext_nf;
+		fft_sample_40.upper_noise = ah->noise;
+	}
+
+	fft_sample_40.lower_rssi = lower_rssi;
+	fft_sample_40.upper_rssi = upper_rssi;
+
+	mag_info = (struct ath_ht20_40_mag_info *) (sample_buf +
+					SPECTRAL_HT20_40_NUM_BINS);
+
+	lower_mag = spectral_max_magnitude(mag_info->lower_bins);
+	fft_sample_40.lower_max_magnitude = __cpu_to_be16(lower_mag);
+
+	upper_mag = spectral_max_magnitude(mag_info->upper_bins);
+	fft_sample_40.upper_max_magnitude = __cpu_to_be16(upper_mag);
+
+	lower_max_index = spectral_max_index(mag_info->lower_bins,
+					SPECTRAL_HT20_40_NUM_BINS);
+	fft_sample_40.lower_max_index = lower_max_index;
+
+	upper_max_index = spectral_max_index(mag_info->upper_bins,
+					SPECTRAL_HT20_40_NUM_BINS);
+	fft_sample_40.upper_max_index = upper_max_index;
+
+	lower_bitmap_w = spectral_bitmap_weight(mag_info->lower_bins);
+	fft_sample_40.lower_bitmap_weight = lower_bitmap_w;
+
+	upper_bitmap_w = spectral_bitmap_weight(mag_info->upper_bins);
+	fft_sample_40.upper_bitmap_weight = upper_bitmap_w;
+
+	max_exp = mag_info->max_exp & 0xf;
+	fft_sample_40.max_exp = max_exp;
+
+	fft_sample_40.tsf = __cpu_to_be64(tsf);
+
+	memcpy(fft_sample_40.data, sample_buf, SPECTRAL_HT20_40_NUM_BINS);
+
+	ath_dbg(common, SPECTRAL_SCAN, "FFT HT20/40 frame: lower mag 0x%X,"
+					"lower_mag_idx %i, upper mag 0x%X,"
+					"upper_mag_idx %i\n",
+					lower_mag >> max_exp,
+					lower_max_index,
+					upper_mag >> max_exp,
+					upper_max_index);
+
+	/* Some time hardware messes up the index and adds
+	 * the index of the middle point (dc_pos). Try to fix it.
+	 */
+	if ((upper_max_index - dc_pos > 0) &&
+	   (fft_sample_40.data[upper_max_index] == (upper_mag >> max_exp))) {
+		upper_max_index -= dc_pos;
+		fft_sample_40.upper_max_index = upper_max_index;
+	}
+
+	if ((lower_max_index - dc_pos > 0) &&
+	   (fft_sample_40.data[lower_max_index - dc_pos] ==
+	   (lower_mag >> max_exp))) {
+		lower_max_index -= dc_pos;
+		fft_sample_40.lower_max_index = lower_max_index;
+	}
+
+	/* Check if we got the expected magnitude values at
+	 * the expected bins
+	 */
+	if ((fft_sample_40.data[upper_max_index + dc_pos]
+	    != (upper_mag >> max_exp)) ||
+	   (fft_sample_40.data[lower_max_index]
+	    != (lower_mag >> max_exp))) {
+		ath_dbg(common, SPECTRAL_SCAN, "Magnitude mismatch !\n");
+		ret = -1;
+	}
+
+	/* DC value (value in the middle) is the blind spot of the spectral
+	 * sample and invalid, interpolate it.
+	 */
+	fft_sample_40.data[dc_pos] = (fft_sample_40.data[dc_pos + 1] +
+					fft_sample_40.data[dc_pos - 1]) / 2;
+
+	/* Check if the maximum magnitudes are indeed maximum,
+	 * also if the maximum value was at dc_pos, calculate
+	 * a new one (since value at dc_pos is invalid).
+	 */
+	if (lower_max_index == dc_pos) {
+		tmp_mag = 0;
+		for (i = 0; i < dc_pos; i++) {
+			if (fft_sample_40.data[i] > tmp_mag) {
+				tmp_mag = fft_sample_40.data[i];
+				fft_sample_40.lower_max_index = i;
+			}
+		}
+
+		lower_mag = tmp_mag << max_exp;
+		fft_sample_40.lower_max_magnitude = __cpu_to_be16(lower_mag);
+
+		ath_dbg(common, SPECTRAL_SCAN,
+			"Calculated new lower max 0x%X at %i\n",
+			tmp_mag, fft_sample_40.lower_max_index);
+	} else
+	for (i = 0; i < dc_pos; i++) {
+		if (fft_sample_40.data[i] == (lower_mag >> max_exp))
+			ath_dbg(common, SPECTRAL_SCAN,
+				"Got lower mag: 0x%X at index %i\n",
+				fft_sample_40.data[i], i);
+
+		if (fft_sample_40.data[i] > (lower_mag >> max_exp)) {
+			ath_dbg(common, SPECTRAL_SCAN,
+				"Got lower bin %i higher than max: 0x%X\n",
+				i, fft_sample_40.data[i]);
+			ret = -1;
+		}
+	}
+
+	if (upper_max_index == dc_pos) {
+		tmp_mag = 0;
+		for (i = dc_pos; i < SPECTRAL_HT20_40_NUM_BINS; i++) {
+			if (fft_sample_40.data[i] > tmp_mag) {
+				tmp_mag = fft_sample_40.data[i];
+				fft_sample_40.upper_max_index = i;
+			}
+		}
+		upper_mag = tmp_mag << max_exp;
+		fft_sample_40.upper_max_magnitude = __cpu_to_be16(upper_mag);
+
+		ath_dbg(common, SPECTRAL_SCAN,
+			"Calculated new upper max 0x%X at %i\n",
+			tmp_mag, i);
+	} else
+	for (i = dc_pos; i < SPECTRAL_HT20_40_NUM_BINS; i++) {
+		if (fft_sample_40.data[i] == (upper_mag >> max_exp))
+			ath_dbg(common, SPECTRAL_SCAN,
+				"Got upper mag: 0x%X at index %i\n",
+				fft_sample_40.data[i], i);
+
+		if (fft_sample_40.data[i] > (upper_mag >> max_exp)) {
+			ath_dbg(common, SPECTRAL_SCAN,
+				"Got upper bin %i higher than max: 0x%X\n",
+				i, fft_sample_40.data[i]);
+
+			ret = -1;
+		}
+	}
+
+	if (ret < 0)
+		return ret;
+
+	tlv = (struct fft_sample_tlv *)&fft_sample_40;
+
+	ath_debug_send_fft_sample(spec_priv, tlv);
+
+	return 0;
+}
+
+static inline void
+ath_cmn_copy_fft_frame(u8 *in, u8 *out, int sample_len, int sample_bytes)
+{
+	switch (sample_bytes - sample_len) {
+	case -1:
+		/* First byte missing */
+		memcpy(&out[1], in,
+		       sample_len - 1);
+		break;
+	case 0:
+		/* Length correct, nothing to do. */
+		memcpy(out, in, sample_len);
+		break;
+	case 1:
+		/* MAC added 2 extra bytes AND first byte
+		 * is missing.
+		 */
+		memcpy(&out[1], in, 30);
+		out[31] = in[31];
+		memcpy(&out[32], &in[33],
+		       sample_len - 32);
+		break;
+	case 2:
+		/* MAC added 2 extra bytes at bin 30 and 32,
+		 * remove them.
+		 */
+		memcpy(out, in, 30);
+		out[30] = in[31];
+		memcpy(&out[31], &in[33],
+		       sample_len - 31);
+		break;
+	default:
+		break;
+	}
+}
+
+static int
+ath_cmn_is_fft_buf_full(struct ath_spec_scan_priv *spec_priv)
+{
+	int i = 0;
+	int ret = 0;
+	struct rchan *rc = spec_priv->rfs_chan_spec_scan;
+
+	for_each_online_cpu(i)
+		ret += relay_buf_full(rc->buf[i]);
+
+	i = num_online_cpus();
+
+	if (ret == i)
+		return 1;
+	else
+		return 0;
+}
+
 /* returns 1 if this was a spectral frame, even if not handled. */
 int ath_cmn_process_fft(struct ath_spec_scan_priv *spec_priv, struct ieee80211_hdr *hdr,
 		    struct ath_rx_status *rs, u64 tsf)
 {
+	u8 sample_buf[SPECTRAL_SAMPLE_MAX_LEN] = {0};
 	struct ath_hw *ah = spec_priv->ah;
 	struct ath_common *common = ath9k_hw_common(spec_priv->ah);
-	u8 num_bins, *bins, *vdata = (u8 *)hdr;
-	struct fft_sample_ht20 fft_sample_20;
-	struct fft_sample_ht20_40 fft_sample_40;
-	struct fft_sample_tlv *tlv;
+	u8 num_bins, *vdata = (u8 *)hdr;
 	struct ath_radar_info *radar_info;
 	int len = rs->rs_datalen;
-	int dc_pos;
-	u16 fft_len, length, freq = ah->curchan->chan->center_freq;
+	int i;
+	int got_slen = 0;
+	u8  *sample_start;
+	int sample_bytes = 0;
+	int ret = 0;
+	u16 fft_len, sample_len, freq = ah->curchan->chan->center_freq;
 	enum nl80211_channel_type chan_type;
+	ath_cmn_fft_idx_validator *fft_idx_validator;
+	ath_cmn_fft_sample_handler *fft_handler;
 
 	/* AR9280 and before report via ATH9K_PHYERR_RADAR, AR93xx and newer
 	 * via ATH9K_PHYERR_SPECTRAL. Haven't seen ATH9K_PHYERR_FALSE_RADAR_EXT
@@ -68,140 +528,170 @@ int ath_cmn_process_fft(struct ath_spec_scan_priv *spec_priv, struct ieee80211_h
 	if (!(radar_info->pulse_bw_info & SPECTRAL_SCAN_BITMASK))
 		return 0;
 
+	/* Output buffers are full, no need to process anything
+	 * since there is no space to put the result anyway
+	 */
+	ret = ath_cmn_is_fft_buf_full(spec_priv);
+	if (ret == 1) {
+		ath_dbg(common, SPECTRAL_SCAN, "FFT report ignored, no space "
+						"left on output buffers\n");
+		return 1;
+	}
+
 	chan_type = cfg80211_get_chandef_type(&common->hw->conf.chandef);
 	if ((chan_type == NL80211_CHAN_HT40MINUS) ||
 	    (chan_type == NL80211_CHAN_HT40PLUS)) {
 		fft_len = SPECTRAL_HT20_40_TOTAL_DATA_LEN;
+		sample_len = SPECTRAL_HT20_40_SAMPLE_LEN;
 		num_bins = SPECTRAL_HT20_40_NUM_BINS;
-		bins = (u8 *)fft_sample_40.data;
+		fft_idx_validator = &ath_cmn_max_idx_verify_ht20_40_fft;
+		fft_handler = &ath_cmn_process_ht20_40_fft;
 	} else {
 		fft_len = SPECTRAL_HT20_TOTAL_DATA_LEN;
+		sample_len = SPECTRAL_HT20_SAMPLE_LEN;
 		num_bins = SPECTRAL_HT20_NUM_BINS;
-		bins = (u8 *)fft_sample_20.data;
-	}
-
-	/* Variation in the data length is possible and will be fixed later */
-	if ((len > fft_len + 2) || (len < fft_len - 1))
-		return 1;
-
-	switch (len - fft_len) {
-	case 0:
-		/* length correct, nothing to do. */
-		memcpy(bins, vdata, num_bins);
-		break;
-	case -1:
-		/* first byte missing, duplicate it. */
-		memcpy(&bins[1], vdata, num_bins - 1);
-		bins[0] = vdata[0];
-		break;
-	case 2:
-		/* MAC added 2 extra bytes at bin 30 and 32, remove them. */
-		memcpy(bins, vdata, 30);
-		bins[30] = vdata[31];
-		memcpy(&bins[31], &vdata[33], num_bins - 31);
-		break;
-	case 1:
-		/* MAC added 2 extra bytes AND first byte is missing. */
-		bins[0] = vdata[0];
-		memcpy(&bins[1], vdata, 30);
-		bins[31] = vdata[31];
-		memcpy(&bins[32], &vdata[33], num_bins - 32);
-		break;
-	default:
-		return 1;
+		fft_idx_validator = ath_cmn_max_idx_verify_ht20_fft;
+		fft_handler = &ath_cmn_process_ht20_fft;
 	}
 
-	/* DC value (value in the middle) is the blind spot of the spectral
-	 * sample and invalid, interpolate it.
-	 */
-	dc_pos = num_bins / 2;
-	bins[dc_pos] = (bins[dc_pos + 1] + bins[dc_pos - 1]) / 2;
-
-	if ((chan_type == NL80211_CHAN_HT40MINUS) ||
-	    (chan_type == NL80211_CHAN_HT40PLUS)) {
-		s8 lower_rssi, upper_rssi;
-		s16 ext_nf;
-		u8 lower_max_index, upper_max_index;
-		u8 lower_bitmap_w, upper_bitmap_w;
-		u16 lower_mag, upper_mag;
-		struct ath9k_hw_cal_data *caldata = ah->caldata;
-		struct ath_ht20_40_mag_info *mag_info;
-
-		if (caldata)
-			ext_nf = ath9k_hw_getchan_noise(ah, ah->curchan,
-					caldata->nfCalHist[3].privNF);
-		else
-			ext_nf = ATH_DEFAULT_NOISE_FLOOR;
-
-		length = sizeof(fft_sample_40) - sizeof(struct fft_sample_tlv);
-		fft_sample_40.tlv.type = ATH_FFT_SAMPLE_HT20_40;
-		fft_sample_40.tlv.length = __cpu_to_be16(length);
-		fft_sample_40.freq = __cpu_to_be16(freq);
-		fft_sample_40.channel_type = chan_type;
-
-		if (chan_type == NL80211_CHAN_HT40PLUS) {
-			lower_rssi = fix_rssi_inv_only(rs->rs_rssi_ctl[0]);
-			upper_rssi = fix_rssi_inv_only(rs->rs_rssi_ext[0]);
-
-			fft_sample_40.lower_noise = ah->noise;
-			fft_sample_40.upper_noise = ext_nf;
-		} else {
-			lower_rssi = fix_rssi_inv_only(rs->rs_rssi_ext[0]);
-			upper_rssi = fix_rssi_inv_only(rs->rs_rssi_ctl[0]);
-
-			fft_sample_40.lower_noise = ext_nf;
-			fft_sample_40.upper_noise = ah->noise;
+	ath_dbg(common, SPECTRAL_SCAN, "Got radar dump bw_info: 0x%X,"
+					"len: %i fft_len: %i\n",
+					radar_info->pulse_bw_info,
+					len,
+					fft_len);
+	sample_start = vdata;
+	for (i = 0; i < len - 2; i++) {
+		sample_bytes++;
+
+		/* Only a single sample received, no need to look
+		 * for the sample's end, do the correction based
+		 * on the packet's length instead. Note that hw
+		 * will always put the radar_info structure on
+		 * the end.
+		 */
+		if (len <= fft_len + 2) {
+			sample_bytes = len - sizeof(struct ath_radar_info);
+			got_slen = 1;
 		}
-		fft_sample_40.lower_rssi = lower_rssi;
-		fft_sample_40.upper_rssi = upper_rssi;
-
-		mag_info = ((struct ath_ht20_40_mag_info *)radar_info) - 1;
-		lower_mag = spectral_max_magnitude(mag_info->lower_bins);
-		upper_mag = spectral_max_magnitude(mag_info->upper_bins);
-		fft_sample_40.lower_max_magnitude = __cpu_to_be16(lower_mag);
-		fft_sample_40.upper_max_magnitude = __cpu_to_be16(upper_mag);
-		lower_max_index = spectral_max_index(mag_info->lower_bins);
-		upper_max_index = spectral_max_index(mag_info->upper_bins);
-		fft_sample_40.lower_max_index = lower_max_index;
-		fft_sample_40.upper_max_index = upper_max_index;
-		lower_bitmap_w = spectral_bitmap_weight(mag_info->lower_bins);
-		upper_bitmap_w = spectral_bitmap_weight(mag_info->upper_bins);
-		fft_sample_40.lower_bitmap_weight = lower_bitmap_w;
-		fft_sample_40.upper_bitmap_weight = upper_bitmap_w;
-		fft_sample_40.max_exp = mag_info->max_exp & 0xf;
 
-		fft_sample_40.tsf = __cpu_to_be64(tsf);
-
-		tlv = (struct fft_sample_tlv *)&fft_sample_40;
-	} else {
-		u8 max_index, bitmap_w;
-		u16 magnitude;
-		struct ath_ht20_mag_info *mag_info;
-
-		length = sizeof(fft_sample_20) - sizeof(struct fft_sample_tlv);
-		fft_sample_20.tlv.type = ATH_FFT_SAMPLE_HT20;
-		fft_sample_20.tlv.length = __cpu_to_be16(length);
-		fft_sample_20.freq = __cpu_to_be16(freq);
-
-		fft_sample_20.rssi = fix_rssi_inv_only(rs->rs_rssi_ctl[0]);
-		fft_sample_20.noise = ah->noise;
-
-		mag_info = ((struct ath_ht20_mag_info *)radar_info) - 1;
-		magnitude = spectral_max_magnitude(mag_info->all_bins);
-		fft_sample_20.max_magnitude = __cpu_to_be16(magnitude);
-		max_index = spectral_max_index(mag_info->all_bins);
-		fft_sample_20.max_index = max_index;
-		bitmap_w = spectral_bitmap_weight(mag_info->all_bins);
-		fft_sample_20.bitmap_weight = bitmap_w;
-		fft_sample_20.max_exp = mag_info->max_exp & 0xf;
-
-		fft_sample_20.tsf = __cpu_to_be64(tsf);
+		/* Search for the end of the FFT frame between
+		 * sample_len - 1 and sample_len + 2. exp_max is 3
+		 * bits long and it's the only value on the last
+		 * byte of the frame so since it'll be smaller than
+		 * the next byte (the first bin of the next sample)
+		 * 90% of the time, we can use it as a separator.
+		 */
+		if (vdata[i] <= 0x7 && sample_bytes >= sample_len - 1) {
+
+			/* Got a frame length within boundaries, there are
+			 * four scenarios here:
+			 *
+			 * a) sample_len -> We got the correct length
+			 * b) sample_len + 2 -> 2 bytes added around bin[31]
+			 * c) sample_len - 1 -> The first byte is missing
+			 * d) sample_len + 1 -> b + c at the same time
+			 *
+			 * When MAC adds 2 extra bytes, bin[31] and bin[32]
+			 * have the same value, so we can use that for further
+			 * verification in cases b and d.
+			 */
+
+			/* Did we go too far ? If so we couldn't determine
+			 * this sample's boundaries, discard any further
+			 * data
+			 */
+			if ((sample_bytes > sample_len + 2) ||
+			   ((sample_bytes > sample_len) &&
+			   (sample_start[31] != sample_start[32])))
+				break;
+
+			/* See if we got a valid frame by checking the
+			 * consistency of mag_info fields. This is to
+			 * prevent from "fixing" a correct frame.
+			 * Failure is non-fatal, later frames may
+			 * be valid.
+			 */
+			if (!fft_idx_validator(&vdata[i], i)) {
+				ath_dbg(common, SPECTRAL_SCAN,
+					"Found valid fft frame at %i\n", i);
+				got_slen = 1;
+			}
+
+			/* We expect 1 - 2 more bytes */
+			else if ((sample_start[31] == sample_start[32]) &&
+				(sample_bytes >= sample_len) &&
+				(sample_bytes < sample_len + 2) &&
+				(vdata[i + 1] <= 0x7))
+				continue;
+
+			/* Try to distinguish cases a and c */
+			else if ((sample_bytes == sample_len - 1) &&
+				(vdata[i + 1] <= 0x7))
+				continue;
+
+			got_slen = 1;
+		}
 
-		tlv = (struct fft_sample_tlv *)&fft_sample_20;
+		if (got_slen) {
+			ath_dbg(common, SPECTRAL_SCAN, "FFT frame len: %i\n",
+				sample_bytes);
+
+			/* Only try to fix a frame if it's the only one
+			 * on the report, else just skip it.
+			 */
+			if (sample_bytes != sample_len && len <= fft_len + 2) {
+				ath_cmn_copy_fft_frame(sample_start,
+						       sample_buf, sample_len,
+						       sample_bytes);
+
+				fft_handler(rs, spec_priv, sample_buf,
+					    tsf, freq, chan_type);
+
+				memset(sample_buf, 0, SPECTRAL_SAMPLE_MAX_LEN);
+
+				/* Mix the received bins to the /dev/random
+				 * pool
+				 */
+				add_device_randomness(sample_buf, num_bins);
+			}
+
+			/* Process a normal frame */
+			if (sample_bytes == sample_len) {
+				ret = fft_handler(rs, spec_priv, sample_start,
+						  tsf, freq, chan_type);
+
+				/* Mix the received bins to the /dev/random
+				 * pool
+				 */
+				add_device_randomness(sample_start, num_bins);
+			}
+
+			/* Short report processed, break out of the
+			 * loop.
+			 */
+			if (len <= fft_len + 2)
+				break;
+
+			sample_start = &vdata[i + 1];
+
+			/* -1 to grab sample_len -1, -2 since
+			 * they 'll get increased by one. In case
+			 * of failure try to recover by going byte
+			 * by byte instead.
+			 */
+			if (ret == 0) {
+				i += num_bins - 2;
+				sample_bytes = num_bins - 2;
+			}
+			got_slen = 0;
+		}
 	}
 
-	ath_debug_send_fft_sample(spec_priv, tlv);
-
+	i -= num_bins - 2;
+	if (len - i != sizeof(struct ath_radar_info))
+		ath_dbg(common, SPECTRAL_SCAN, "FFT report truncated"
+						"(bytes left: %i)\n",
+						len - i);
 	return 1;
 }
 EXPORT_SYMBOL(ath_cmn_process_fft);
diff --git a/drivers/net/wireless/ath/ath9k/common-spectral.h b/drivers/net/wireless/ath/ath9k/common-spectral.h
index 82d9dd29652c..998743be9c67 100644
--- a/drivers/net/wireless/ath/ath9k/common-spectral.h
+++ b/drivers/net/wireless/ath/ath9k/common-spectral.h
@@ -66,6 +66,8 @@ struct ath_ht20_fft_packet {
 } __packed;
 
 #define SPECTRAL_HT20_TOTAL_DATA_LEN	(sizeof(struct ath_ht20_fft_packet))
+#define	SPECTRAL_HT20_SAMPLE_LEN	(sizeof(struct ath_ht20_mag_info) +\
+					SPECTRAL_HT20_NUM_BINS)
 
 /* Dynamic 20/40 mode:
  *
@@ -101,6 +103,10 @@ struct ath_spec_scan_priv {
 };
 
 #define SPECTRAL_HT20_40_TOTAL_DATA_LEN	(sizeof(struct ath_ht20_40_fft_packet))
+#define	SPECTRAL_HT20_40_SAMPLE_LEN	(sizeof(struct ath_ht20_40_mag_info) +\
+					SPECTRAL_HT20_40_NUM_BINS)
+
+#define	SPECTRAL_SAMPLE_MAX_LEN		SPECTRAL_HT20_40_SAMPLE_LEN
 
 /* grabs the max magnitude from the all/upper/lower bins */
 static inline u16 spectral_max_magnitude(u8 *bins)
@@ -111,17 +117,32 @@ static inline u16 spectral_max_magnitude(u8 *bins)
 }
 
 /* return the max magnitude from the all/upper/lower bins */
-static inline u8 spectral_max_index(u8 *bins)
+static inline u8 spectral_max_index(u8 *bins, int num_bins)
 {
 	s8 m = (bins[2] & 0xfc) >> 2;
-
-	/* TODO: this still doesn't always report the right values ... */
-	if (m > 32)
+	u8 zero_idx = num_bins / 2;
+
+	/* It's a 5 bit signed int, remove its sign and use one's
+	 * complement interpretation to add the sign back to the 8
+	 * bit int
+	 */
+	if (m & 0x20) {
+		m &= ~0x20;
 		m |= 0xe0;
-	else
-		m &= ~0xe0;
+	}
+
+	/* Bring the zero point to the beginning
+	 * instead of the middle so that we can use
+	 * it for array lookup and that we don't deal
+	 * with negative values later
+	 */
+	m += zero_idx;
+
+	/* Sanity check to make sure index is within bounds */
+	if (m < 0 || m > num_bins - 1)
+		m = 0;
 
-	return m + 29;
+	return m;
 }
 
 /* return the bitmap weight from the all/upper/lower bins */
diff --git a/drivers/net/wireless/ath/ath9k/htc.h b/drivers/net/wireless/ath/ath9k/htc.h
index e82a0d4ce23f..16dff4b89a86 100644
--- a/drivers/net/wireless/ath/ath9k/htc.h
+++ b/drivers/net/wireless/ath/ath9k/htc.h
@@ -440,9 +440,9 @@ static inline void ath9k_htc_stop_btcoex(struct ath9k_htc_priv *priv)
 }
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 
-#define OP_BT_PRIORITY_DETECTED    BIT(3)
-#define OP_BT_SCAN                 BIT(4)
-#define OP_TSF_RESET               BIT(6)
+#define OP_BT_PRIORITY_DETECTED    3
+#define OP_BT_SCAN                 4
+#define OP_TSF_RESET               6
 
 enum htc_op_flags {
 	HTC_FWFLAG_NO_RMW,
@@ -531,6 +531,7 @@ struct ath9k_htc_priv {
 	struct ath9k_debug debug;
 #endif
 	struct mutex mutex;
+	struct ieee80211_vif *csa_vif;
 };
 
 static inline void ath_read_cachesize(struct ath_common *common, int *csz)
@@ -584,6 +585,7 @@ void ath9k_htc_tx_drain(struct ath9k_htc_priv *priv);
 void ath9k_htc_txstatus(struct ath9k_htc_priv *priv, void *wmi_event);
 void ath9k_tx_failed_tasklet(unsigned long data);
 void ath9k_htc_tx_cleanup_timer(unsigned long data);
+bool ath9k_htc_csa_is_finished(struct ath9k_htc_priv *priv);
 
 int ath9k_rx_init(struct ath9k_htc_priv *priv);
 void ath9k_rx_cleanup(struct ath9k_htc_priv *priv);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
index e8b6ec3c1dbb..e6bcb4c90fa0 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
@@ -257,6 +257,8 @@ static void ath9k_htc_send_beacon(struct ath9k_htc_priv *priv,
 	}
 
 	spin_unlock_bh(&priv->beacon_lock);
+
+	ath9k_htc_csa_is_finished(priv);
 }
 
 static int ath9k_htc_choose_bslot(struct ath9k_htc_priv *priv,
@@ -503,3 +505,20 @@ void ath9k_htc_beacon_reconfig(struct ath9k_htc_priv *priv)
 		return;
 	}
 }
+
+bool ath9k_htc_csa_is_finished(struct ath9k_htc_priv *priv)
+{
+	struct ieee80211_vif *vif;
+
+	vif = priv->csa_vif;
+	if (!vif || !vif->csa_active)
+		return false;
+
+	if (!ieee80211_csa_is_complete(vif))
+		return false;
+
+	ieee80211_csa_finish(vif);
+
+	priv->csa_vif = NULL;
+	return true;
+}
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index d7beefe60683..39eaf9b6e9b4 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -594,7 +594,7 @@ static void ath9k_init_misc(struct ath9k_htc_priv *priv)
 
 	priv->spec_priv.ah = priv->ah;
 	priv->spec_priv.spec_config.enabled = 0;
-	priv->spec_priv.spec_config.short_repeat = false;
+	priv->spec_priv.spec_config.short_repeat = true;
 	priv->spec_priv.spec_config.count = 8;
 	priv->spec_priv.spec_config.endless = false;
 	priv->spec_priv.spec_config.period = 0x12;
@@ -717,18 +717,18 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 	struct ath_common *common = ath9k_hw_common(priv->ah);
 	struct base_eep_header *pBase;
 
-	hw->flags = IEEE80211_HW_SIGNAL_DBM |
-		IEEE80211_HW_AMPDU_AGGREGATION |
-		IEEE80211_HW_SPECTRUM_MGMT |
-		IEEE80211_HW_HAS_RATE_CONTROL |
-		IEEE80211_HW_RX_INCLUDES_FCS |
-		IEEE80211_HW_PS_NULLFUNC_STACK |
-		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		IEEE80211_HW_MFP_CAPABLE |
-		IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	ieee80211_hw_set(hw, HOST_BROADCAST_PS_BUFFERING);
+	ieee80211_hw_set(hw, MFP_CAPABLE);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, HAS_RATE_CONTROL);
+	ieee80211_hw_set(hw, SPECTRUM_MGMT);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
 
 	if (ath9k_ps_enable)
-		hw->flags |= IEEE80211_HW_SUPPORTS_PS;
+		ieee80211_hw_set(hw, SUPPORTS_PS);
 
 	hw->wiphy->interface_modes =
 		BIT(NL80211_IFTYPE_STATION) |
@@ -744,7 +744,8 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 	hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN |
-			    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+			    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
+			    WIPHY_FLAG_HAS_CHANNEL_SWITCH;
 
 	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
 
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index 564923c0df87..dab1323dfec7 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -1134,6 +1134,9 @@ static void ath9k_htc_remove_interface(struct ieee80211_hw *hw,
 	priv->nvifs--;
 	priv->vif_slot &= ~(1 << avp->index);
 
+	if (priv->csa_vif == vif)
+		priv->csa_vif = NULL;
+
 	ath9k_htc_remove_station(priv, vif, NULL);
 
 	DEC_VIF(priv, vif->type);
@@ -1238,8 +1241,7 @@ out:
 }
 
 #define SUPPORTED_FILTERS			\
-	(FIF_PROMISC_IN_BSS |			\
-	FIF_ALLMULTI |				\
+	(FIF_ALLMULTI |				\
 	FIF_CONTROL |				\
 	FIF_PSPOLL |				\
 	FIF_OTHER_BSS |				\
@@ -1842,6 +1844,19 @@ static int ath9k_htc_get_antenna(struct ieee80211_hw *hw, u32 *tx_ant,
 	return 0;
 }
 
+static void ath9k_htc_channel_switch_beacon(struct ieee80211_hw *hw,
+					    struct ieee80211_vif *vif,
+					    struct cfg80211_chan_def *chandef)
+{
+	struct ath9k_htc_priv *priv = hw->priv;
+
+	/* mac80211 does not support CSA in multi-if cases (yet) */
+	if (WARN_ON(priv->csa_vif))
+		return;
+
+	priv->csa_vif = vif;
+}
+
 struct ieee80211_ops ath9k_htc_ops = {
 	.tx                 = ath9k_htc_tx,
 	.start              = ath9k_htc_start,
@@ -1868,6 +1883,7 @@ struct ieee80211_ops ath9k_htc_ops = {
 	.set_bitrate_mask   = ath9k_htc_set_bitrate_mask,
 	.get_stats	    = ath9k_htc_get_stats,
 	.get_antenna	    = ath9k_htc_get_antenna,
+	.channel_switch_beacon	= ath9k_htc_channel_switch_beacon,
 
 #ifdef CONFIG_ATH9K_HTC_DEBUGFS
 	.get_et_sset_count  = ath9k_htc_get_et_sset_count,
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
index a0f58e2aa553..cc9648f844ae 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
@@ -872,14 +872,7 @@ u32 ath9k_htc_calcrxfilter(struct ath9k_htc_priv *priv)
 	if (priv->rxfilter & FIF_PROBE_REQ)
 		rfilt |= ATH9K_RX_FILTER_PROBEREQ;
 
-	/*
-	 * Set promiscuous mode when FIF_PROMISC_IN_BSS is enabled for station
-	 * mode interface or when in monitor mode. AP mode does not need this
-	 * since it receives all in-BSS frames anyway.
-	 */
-	if (((ah->opmode != NL80211_IFTYPE_AP) &&
-	     (priv->rxfilter & FIF_PROMISC_IN_BSS)) ||
-	    ah->is_monitoring)
+	if (ah->is_monitoring)
 		rfilt |= ATH9K_RX_FILTER_PROM;
 
 	if (priv->rxfilter & FIF_CONTROL)
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index c1d2d0340feb..e8454db17634 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -1119,6 +1119,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_cmn_spur_mitigate(struct ath_hw *ah,
+				 struct ath9k_channel *chan, int bin);
 void ar5008_hw_init_rate_txpower(struct ath_hw *ah, int16_t *rate_array,
 				 struct ath9k_channel *chan, int ht40_delta);
 
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index f8d11efa7b0f..eff0e5325e6a 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -796,7 +796,7 @@ static void ath9k_set_mcc_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	if (!ath9k_is_chanctx_enabled())
 		return;
 
-	hw->flags |= IEEE80211_HW_QUEUE_CONTROL;
+	ieee80211_hw_set(hw, QUEUE_CONTROL);
 	hw->queues = ATH9K_NUM_TX_QUEUES;
 	hw->offchannel_tx_hw_queue = hw->queues - 1;
 	hw->wiphy->interface_modes &= ~ BIT(NL80211_IFTYPE_WDS);
@@ -818,20 +818,20 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 
-	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-		IEEE80211_HW_SIGNAL_DBM |
-		IEEE80211_HW_PS_NULLFUNC_STACK |
-		IEEE80211_HW_SPECTRUM_MGMT |
-		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		IEEE80211_HW_SUPPORTS_RC_TABLE |
-		IEEE80211_HW_SUPPORTS_HT_CCK_RATES;
+	ieee80211_hw_set(hw, SUPPORTS_HT_CCK_RATES);
+	ieee80211_hw_set(hw, SUPPORTS_RC_TABLE);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, SPECTRUM_MGMT);
+	ieee80211_hw_set(hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, HOST_BROADCAST_PS_BUFFERING);
 
 	if (ath9k_ps_enable)
-		hw->flags |= IEEE80211_HW_SUPPORTS_PS;
+		ieee80211_hw_set(hw, SUPPORTS_PS);
 
 	if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_HT) {
-		hw->flags |= IEEE80211_HW_AMPDU_AGGREGATION;
+		ieee80211_hw_set(hw, AMPDU_AGGREGATION);
 
 		if (AR_SREV_9280_20_OR_LATER(ah))
 			hw->radiotap_mcs_details |=
@@ -839,7 +839,7 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	}
 
 	if (AR_SREV_9160_10_OR_LATER(sc->sc_ah) || ath9k_modparam_nohwcrypt)
-		hw->flags |= IEEE80211_HW_MFP_CAPABLE;
+		ieee80211_hw_set(hw, MFP_CAPABLE);
 
 	hw->wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR |
 			       NL80211_FEATURE_AP_MODE_CHAN_WIDTH_CHANGE |
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index b0badef71ce7..cfd45cb8ccfc 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -216,11 +216,13 @@ static bool ath_prepare_reset(struct ath_softc *sc)
 	ath_stop_ani(sc);
 	ath9k_hw_disable_interrupts(ah);
 
-	if (!ath_drain_all_txq(sc))
-		ret = false;
-
-	if (!ath_stoprecv(sc))
-		ret = false;
+	if (AR_SREV_9300_20_OR_LATER(ah)) {
+		ret &= ath_stoprecv(sc);
+		ret &= ath_drain_all_txq(sc);
+	} else {
+		ret &= ath_drain_all_txq(sc);
+		ret &= ath_stoprecv(sc);
+	}
 
 	return ret;
 }
@@ -1442,8 +1444,7 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 }
 
 #define SUPPORTED_FILTERS			\
-	(FIF_PROMISC_IN_BSS |			\
-	FIF_ALLMULTI |				\
+	(FIF_ALLMULTI |				\
 	FIF_CONTROL |				\
 	FIF_PSPOLL |				\
 	FIF_OTHER_BSS |				\
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 6fb40ef86fd6..6c75fb1ab77d 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -392,11 +392,6 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 	if (sc->cur_chan->rxfilter & FIF_PROBE_REQ)
 		rfilt |= ATH9K_RX_FILTER_PROBEREQ;
 
-	/*
-	 * Set promiscuous mode when FIF_PROMISC_IN_BSS is enabled for station
-	 * mode interface or when in monitor mode. AP mode does not need this
-	 * since it receives all in-BSS frames anyway.
-	 */
 	if (sc->sc_ah->is_monitoring)
 		rfilt |= ATH9K_RX_FILTER_PROM;
 
diff --git a/drivers/net/wireless/ath/carl9170/fw.c b/drivers/net/wireless/ath/carl9170/fw.c
index 47d5c2e910ad..88045f93a76c 100644
--- a/drivers/net/wireless/ath/carl9170/fw.c
+++ b/drivers/net/wireless/ath/carl9170/fw.c
@@ -286,7 +286,7 @@ static int carl9170_fw(struct ar9170 *ar, const __u8 *data, size_t len)
 	}
 
 	if (SUPP(CARL9170FW_PSM) && SUPP(CARL9170FW_FIXED_5GHZ_PSM))
-		ar->hw->flags |= IEEE80211_HW_SUPPORTS_PS;
+		ieee80211_hw_set(ar->hw, SUPPORTS_PS);
 
 	if (!SUPP(CARL9170FW_USB_INIT_FIRMWARE)) {
 		dev_err(&ar->udev->dev, "firmware does not provide "
@@ -310,8 +310,7 @@ static int carl9170_fw(struct ar9170 *ar, const __u8 *data, size_t len)
 	if (SUPP(CARL9170FW_RX_FILTER)) {
 		ar->fw.rx_filter = true;
 		ar->rx_filter_caps = FIF_FCSFAIL | FIF_PLCPFAIL |
-			FIF_CONTROL | FIF_PSPOLL | FIF_OTHER_BSS |
-			FIF_PROMISC_IN_BSS;
+			FIF_CONTROL | FIF_PSPOLL | FIF_OTHER_BSS;
 	}
 
 	if (SUPP(CARL9170FW_HW_COUNTERS))
diff --git a/drivers/net/wireless/ath/carl9170/led.c b/drivers/net/wireless/ath/carl9170/led.c
index 78dadc797558..2c74425f5059 100644
--- a/drivers/net/wireless/ath/carl9170/led.c
+++ b/drivers/net/wireless/ath/carl9170/led.c
@@ -122,7 +122,7 @@ static void carl9170_led_set_brightness(struct led_classdev *led,
 }
 
 static int carl9170_led_register_led(struct ar9170 *ar, int i, char *name,
-				     char *trigger)
+				     const char *trigger)
 {
 	int err;
 
diff --git a/drivers/net/wireless/ath/carl9170/main.c b/drivers/net/wireless/ath/carl9170/main.c
index f1455a04cb62..170c209f99b8 100644
--- a/drivers/net/wireless/ath/carl9170/main.c
+++ b/drivers/net/wireless/ath/carl9170/main.c
@@ -1011,9 +1011,8 @@ static void carl9170_op_configure_filter(struct ieee80211_hw *hw,
 	if (multicast != ar->cur_mc_hash)
 		WARN_ON(carl9170_update_multicast(ar, multicast));
 
-	if (changed_flags & (FIF_OTHER_BSS | FIF_PROMISC_IN_BSS)) {
-		ar->sniffer_enabled = !!(*new_flags &
-			(FIF_OTHER_BSS | FIF_PROMISC_IN_BSS));
+	if (changed_flags & FIF_OTHER_BSS) {
+		ar->sniffer_enabled = !!(*new_flags & FIF_OTHER_BSS);
 
 		WARN_ON(carl9170_set_operating_mode(ar));
 	}
@@ -1033,7 +1032,7 @@ static void carl9170_op_configure_filter(struct ieee80211_hw *hw,
 		if (!(*new_flags & FIF_PSPOLL))
 			rx_filter |= CARL9170_RX_FILTER_CTL_PSPOLL;
 
-		if (!(*new_flags & (FIF_OTHER_BSS | FIF_PROMISC_IN_BSS))) {
+		if (!(*new_flags & FIF_OTHER_BSS)) {
 			rx_filter |= CARL9170_RX_FILTER_OTHER_RA;
 			rx_filter |= CARL9170_RX_FILTER_DECRY_FAIL;
 		}
@@ -1845,22 +1844,22 @@ void *carl9170_alloc(size_t priv_size)
 	/* firmware decides which modes we support */
 	hw->wiphy->interface_modes = 0;
 
-	hw->flags |= IEEE80211_HW_RX_INCLUDES_FCS |
-		     IEEE80211_HW_MFP_CAPABLE |
-		     IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		     IEEE80211_HW_SUPPORTS_PS |
-		     IEEE80211_HW_PS_NULLFUNC_STACK |
-		     IEEE80211_HW_NEED_DTIM_BEFORE_ASSOC |
-		     IEEE80211_HW_SUPPORTS_RC_TABLE |
-		     IEEE80211_HW_SIGNAL_DBM |
-		     IEEE80211_HW_SUPPORTS_HT_CCK_RATES;
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, MFP_CAPABLE);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, SUPPORTS_PS);
+	ieee80211_hw_set(hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(hw, NEED_DTIM_BEFORE_ASSOC);
+	ieee80211_hw_set(hw, SUPPORTS_RC_TABLE);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, SUPPORTS_HT_CCK_RATES);
 
 	if (!modparam_noht) {
 		/*
 		 * see the comment above, why we allow the user
 		 * to disable HT by a module parameter.
 		 */
-		hw->flags |= IEEE80211_HW_AMPDU_AGGREGATION;
+		ieee80211_hw_set(hw, AMPDU_AGGREGATION);
 	}
 
 	hw->extra_tx_headroom = sizeof(struct _carl9170_tx_superframe);
diff --git a/drivers/net/wireless/ath/carl9170/usb.c b/drivers/net/wireless/ath/carl9170/usb.c
index c9f93310c0d6..76842e6ca38e 100644
--- a/drivers/net/wireless/ath/carl9170/usb.c
+++ b/drivers/net/wireless/ath/carl9170/usb.c
@@ -651,6 +651,7 @@ int carl9170_exec_cmd(struct ar9170 *ar, const enum carl9170_cmd_oids cmd,
 	unsigned int plen, void *payload, unsigned int outlen, void *out)
 {
 	int err = -ENOMEM;
+	unsigned long time_left;
 
 	if (!IS_ACCEPTING_CMD(ar))
 		return -EIO;
@@ -672,8 +673,8 @@ int carl9170_exec_cmd(struct ar9170 *ar, const enum carl9170_cmd_oids cmd,
 	err = __carl9170_exec_cmd(ar, &ar->cmd, false);
 
 	if (!(cmd & CARL9170_CMD_ASYNC_FLAG)) {
-		err = wait_for_completion_timeout(&ar->cmd_wait, HZ);
-		if (err == 0) {
+		time_left = wait_for_completion_timeout(&ar->cmd_wait, HZ);
+		if (time_left == 0) {
 			err = -ETIMEDOUT;
 			goto err_unbuf;
 		}
diff --git a/drivers/net/wireless/ath/dfs_pattern_detector.c b/drivers/net/wireless/ath/dfs_pattern_detector.c
index c657ca26a71a..656ce42b339a 100644
--- a/drivers/net/wireless/ath/dfs_pattern_detector.c
+++ b/drivers/net/wireless/ath/dfs_pattern_detector.c
@@ -41,30 +41,31 @@ struct radar_types {
 
 /* percentage on ppb threshold to trigger detection */
 #define MIN_PPB_THRESH	50
-#define PPB_THRESH(PPB) ((PPB * MIN_PPB_THRESH + 50) / 100)
+#define PPB_THRESH_RATE(PPB, RATE) ((PPB * RATE + 100 - RATE) / 100)
+#define PPB_THRESH(PPB) PPB_THRESH_RATE(PPB, MIN_PPB_THRESH)
 #define PRF2PRI(PRF) ((1000000 + PRF / 2) / PRF)
 /* percentage of pulse width tolerance */
 #define WIDTH_TOLERANCE 5
 #define WIDTH_LOWER(X) ((X*(100-WIDTH_TOLERANCE)+50)/100)
 #define WIDTH_UPPER(X) ((X*(100+WIDTH_TOLERANCE)+50)/100)
 
-#define ETSI_PATTERN(ID, WMIN, WMAX, PMIN, PMAX, PRF, PPB)	\
+#define ETSI_PATTERN(ID, WMIN, WMAX, PMIN, PMAX, PRF, PPB, CHIRP)	\
 {								\
 	ID, WIDTH_LOWER(WMIN), WIDTH_UPPER(WMAX),		\
 	(PRF2PRI(PMAX) - PRI_TOLERANCE),			\
 	(PRF2PRI(PMIN) * PRF + PRI_TOLERANCE), PRF, PPB * PRF,	\
-	PPB_THRESH(PPB), PRI_TOLERANCE,				\
+	PPB_THRESH(PPB), PRI_TOLERANCE,	CHIRP			\
 }
 
 /* radar types as defined by ETSI EN-301-893 v1.5.1 */
 static const struct radar_detector_specs etsi_radar_ref_types_v15[] = {
-	ETSI_PATTERN(0,  0,  1,  700,  700, 1, 18),
-	ETSI_PATTERN(1,  0,  5,  200, 1000, 1, 10),
-	ETSI_PATTERN(2,  0, 15,  200, 1600, 1, 15),
-	ETSI_PATTERN(3,  0, 15, 2300, 4000, 1, 25),
-	ETSI_PATTERN(4, 20, 30, 2000, 4000, 1, 20),
-	ETSI_PATTERN(5,  0,  2,  300,  400, 3, 10),
-	ETSI_PATTERN(6,  0,  2,  400, 1200, 3, 15),
+	ETSI_PATTERN(0,  0,  1,  700,  700, 1, 18, false),
+	ETSI_PATTERN(1,  0,  5,  200, 1000, 1, 10, false),
+	ETSI_PATTERN(2,  0, 15,  200, 1600, 1, 15, false),
+	ETSI_PATTERN(3,  0, 15, 2300, 4000, 1, 25, false),
+	ETSI_PATTERN(4, 20, 30, 2000, 4000, 1, 20, false),
+	ETSI_PATTERN(5,  0,  2,  300,  400, 3, 10, false),
+	ETSI_PATTERN(6,  0,  2,  400, 1200, 3, 15, false),
 };
 
 static const struct radar_types etsi_radar_types_v15 = {
@@ -73,21 +74,30 @@ static const struct radar_types etsi_radar_types_v15 = {
 	.radar_types		= etsi_radar_ref_types_v15,
 };
 
-#define FCC_PATTERN(ID, WMIN, WMAX, PMIN, PMAX, PRF, PPB)	\
+#define FCC_PATTERN(ID, WMIN, WMAX, PMIN, PMAX, PRF, PPB, CHIRP)	\
 {								\
 	ID, WIDTH_LOWER(WMIN), WIDTH_UPPER(WMAX),		\
 	PMIN - PRI_TOLERANCE,					\
 	PMAX * PRF + PRI_TOLERANCE, PRF, PPB * PRF,		\
-	PPB_THRESH(PPB), PRI_TOLERANCE,				\
+	PPB_THRESH(PPB), PRI_TOLERANCE,	CHIRP			\
 }
 
+/* radar types released on August 14, 2014
+ * type 1 PRI values randomly selected within the range of 518 and 3066.
+ * divide it to 3 groups is good enough for both of radar detection and
+ * avoiding false detection based on practical test results
+ * collected for more than a year.
+ */
 static const struct radar_detector_specs fcc_radar_ref_types[] = {
-	FCC_PATTERN(0, 0, 1, 1428, 1428, 1, 18),
-	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, 1),
-	FCC_PATTERN(5, 0, 1, 333, 333, 1, 9),
+	FCC_PATTERN(0, 0, 1, 1428, 1428, 1, 18, false),
+	FCC_PATTERN(101, 0, 1, 518, 938, 1, 57, false),
+	FCC_PATTERN(102, 0, 1, 938, 2000, 1, 27, false),
+	FCC_PATTERN(103, 0, 1, 2000, 3066, 1, 18, false),
+	FCC_PATTERN(2, 0, 5, 150, 230, 1, 23, false),
+	FCC_PATTERN(3, 6, 10, 200, 500, 1, 16, false),
+	FCC_PATTERN(4, 11, 20, 200, 500, 1, 12, false),
+	FCC_PATTERN(5, 50, 100, 1000, 2000, 1, 1, true),
+	FCC_PATTERN(6, 0, 1, 333, 333, 1, 9, false),
 };
 
 static const struct radar_types fcc_radar_types = {
@@ -96,17 +106,23 @@ static const struct radar_types fcc_radar_types = {
 	.radar_types		= fcc_radar_ref_types,
 };
 
-#define JP_PATTERN FCC_PATTERN
+#define JP_PATTERN(ID, WMIN, WMAX, PMIN, PMAX, PRF, PPB, RATE, CHIRP)	\
+{								\
+	ID, WIDTH_LOWER(WMIN), WIDTH_UPPER(WMAX),		\
+	PMIN - PRI_TOLERANCE,					\
+	PMAX * PRF + PRI_TOLERANCE, PRF, PPB * PRF,		\
+	PPB_THRESH_RATE(PPB, RATE), PRI_TOLERANCE, CHIRP	\
+}
 static const struct radar_detector_specs jp_radar_ref_types[] = {
-	JP_PATTERN(0, 0, 1, 1428, 1428, 1, 18),
-	JP_PATTERN(1, 2, 3, 3846, 3846, 1, 18),
-	JP_PATTERN(2, 0, 1, 1388, 1388, 1, 18),
-	JP_PATTERN(3, 1, 2, 4000, 4000, 1, 18),
-	JP_PATTERN(4, 0, 5, 150, 230, 1, 23),
-	JP_PATTERN(5, 6, 10, 200, 500, 1, 16),
-	JP_PATTERN(6, 11, 20, 200, 500, 1, 12),
-	JP_PATTERN(7, 50, 100, 1000, 2000, 1, 20),
-	JP_PATTERN(5, 0, 1, 333, 333, 1, 9),
+	JP_PATTERN(0, 0, 1, 1428, 1428, 1, 18, 29, false),
+	JP_PATTERN(1, 2, 3, 3846, 3846, 1, 18, 29, false),
+	JP_PATTERN(2, 0, 1, 1388, 1388, 1, 18, 50, false),
+	JP_PATTERN(3, 1, 2, 4000, 4000, 1, 18, 50, false),
+	JP_PATTERN(4, 0, 5, 150, 230, 1, 23, 50, false),
+	JP_PATTERN(5, 6, 10, 200, 500, 1, 16, 50, false),
+	JP_PATTERN(6, 11, 20, 200, 500, 1, 12, 50, false),
+	JP_PATTERN(7, 50, 100, 1000, 2000, 1, 20, 50, false),
+	JP_PATTERN(5, 0, 1, 333, 333, 1, 9, 50, false),
 };
 
 static const struct radar_types jp_radar_types = {
diff --git a/drivers/net/wireless/ath/dfs_pattern_detector.h b/drivers/net/wireless/ath/dfs_pattern_detector.h
index dde2652b787c..25a43d632f90 100644
--- a/drivers/net/wireless/ath/dfs_pattern_detector.h
+++ b/drivers/net/wireless/ath/dfs_pattern_detector.h
@@ -40,12 +40,14 @@ struct ath_dfs_pool_stats {
  * @freq: channel frequency in MHz
  * @width: pulse duration in us
  * @rssi: rssi of radar event
+ * @chirp: chirp detected in pulse
  */
 struct pulse_event {
 	u64 ts;
 	u16 freq;
 	u8 width;
 	u8 rssi;
+	bool chirp;
 };
 
 /**
@@ -59,6 +61,7 @@ struct pulse_event {
  * @ppb: pulses per bursts for this type
  * @ppb_thresh: number of pulses required to trigger detection
  * @max_pri_tolerance: pulse time stamp tolerance on both sides [us]
+ * @chirp: chirp required for the radar pattern
  */
 struct radar_detector_specs {
 	u8 type_id;
@@ -70,6 +73,7 @@ struct radar_detector_specs {
 	u8 ppb;
 	u8 ppb_thresh;
 	u8 max_pri_tolerance;
+	bool chirp;
 };
 
 /**
diff --git a/drivers/net/wireless/ath/dfs_pri_detector.c b/drivers/net/wireless/ath/dfs_pri_detector.c
index 43b608178884..1b5ad1965607 100644
--- a/drivers/net/wireless/ath/dfs_pri_detector.c
+++ b/drivers/net/wireless/ath/dfs_pri_detector.c
@@ -390,6 +390,10 @@ static struct pri_sequence *pri_detector_add_pulse(struct pri_detector *de,
 	if ((ts - de->last_ts) < rs->max_pri_tolerance)
 		/* if delta to last pulse is too short, don't use this pulse */
 		return NULL;
+	/* radar detector spec needs chirp, but not detected */
+	if (rs->chirp && rs->chirp != event->chirp)
+		return NULL;
+
 	de->last_ts = ts;
 
 	max_updated_seq = pseq_handler_add_to_existing_seqs(de, ts);
diff --git a/drivers/net/wireless/ath/wcn36xx/main.c b/drivers/net/wireless/ath/wcn36xx/main.c
index 0783d2ed8238..900e72a089d8 100644
--- a/drivers/net/wireless/ath/wcn36xx/main.c
+++ b/drivers/net/wireless/ath/wcn36xx/main.c
@@ -944,12 +944,12 @@ static int wcn36xx_init_ieee80211(struct wcn36xx *wcn)
 		WLAN_CIPHER_SUITE_CCMP,
 	};
 
-	wcn->hw->flags = IEEE80211_HW_SIGNAL_DBM |
-		IEEE80211_HW_HAS_RATE_CONTROL |
-		IEEE80211_HW_SUPPORTS_PS |
-		IEEE80211_HW_CONNECTION_MONITOR |
-		IEEE80211_HW_AMPDU_AGGREGATION |
-		IEEE80211_HW_TIMING_BEACON_ONLY;
+	ieee80211_hw_set(wcn->hw, TIMING_BEACON_ONLY);
+	ieee80211_hw_set(wcn->hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(wcn->hw, CONNECTION_MONITOR);
+	ieee80211_hw_set(wcn->hw, SUPPORTS_PS);
+	ieee80211_hw_set(wcn->hw, SIGNAL_DBM);
+	ieee80211_hw_set(wcn->hw, HAS_RATE_CONTROL);
 
 	wcn->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 		BIT(NL80211_IFTYPE_AP) |
diff --git a/drivers/net/wireless/ath/wcn36xx/smd.c b/drivers/net/wireless/ath/wcn36xx/smd.c
index dbd894428be6..c9263e1c75d4 100644
--- a/drivers/net/wireless/ath/wcn36xx/smd.c
+++ b/drivers/net/wireless/ath/wcn36xx/smd.c
@@ -216,9 +216,7 @@ static void wcn36xx_smd_set_sta_params(struct wcn36xx *wcn,
 		memcpy(&sta_params->bssid, vif->addr, ETH_ALEN);
 
 	sta_params->encrypt_type = priv_vif->encrypt_type;
-	sta_params->short_preamble_supported =
-		!(WCN36XX_FLAGS(wcn) &
-		  IEEE80211_HW_2GHZ_SHORT_PREAMBLE_INCAPABLE);
+	sta_params->short_preamble_supported = true;
 
 	sta_params->rifs_mode = 0;
 	sta_params->rmf = 0;
diff --git a/drivers/net/wireless/ath/wil6210/Makefile b/drivers/net/wireless/ath/wil6210/Makefile
index caa717bf52f3..050506f842e9 100644
--- a/drivers/net/wireless/ath/wil6210/Makefile
+++ b/drivers/net/wireless/ath/wil6210/Makefile
@@ -12,6 +12,7 @@ wil6210-y += debug.o
 wil6210-y += rx_reorder.o
 wil6210-y += ioctl.o
 wil6210-y += fw.o
+wil6210-y += pmc.o
 wil6210-$(CONFIG_WIL6210_TRACING) += trace.o
 wil6210-y += wil_platform.o
 wil6210-y += ethtool.o
diff --git a/drivers/net/wireless/ath/wil6210/cfg80211.c b/drivers/net/wireless/ath/wil6210/cfg80211.c
index b97172667bc7..c79cfe02ec80 100644
--- a/drivers/net/wireless/ath/wil6210/cfg80211.c
+++ b/drivers/net/wireless/ath/wil6210/cfg80211.c
@@ -289,6 +289,26 @@ static int wil_cfg80211_scan(struct wiphy *wiphy,
 	}
 
 	wil_dbg_misc(wil, "Start scan_request 0x%p\n", request);
+	wil_dbg_misc(wil, "SSID count: %d", request->n_ssids);
+
+	for (i = 0; i < request->n_ssids; i++) {
+		wil_dbg_misc(wil, "SSID[%d]", i);
+		print_hex_dump_bytes("SSID ", DUMP_PREFIX_OFFSET,
+				     request->ssids[i].ssid,
+				     request->ssids[i].ssid_len);
+	}
+
+	if (request->n_ssids)
+		rc = wmi_set_ssid(wil, request->ssids[0].ssid_len,
+				  request->ssids[0].ssid);
+	else
+		rc = wmi_set_ssid(wil, 0, NULL);
+
+	if (rc) {
+		wil_err(wil, "set SSID for scan request failed: %d\n", rc);
+		return rc;
+	}
+
 	wil->scan_request = request;
 	mod_timer(&wil->scan_timer, jiffies + WIL6210_SCAN_TO);
 
@@ -402,11 +422,8 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 	rsn_eid = sme->ie ?
 			cfg80211_find_ie(WLAN_EID_RSN, sme->ie, sme->ie_len) :
 			NULL;
-
-	if (sme->privacy && !rsn_eid) {
-		wil_err(wil, "Missing RSN IE for secure connection\n");
-		return -EINVAL;
-	}
+	if (sme->privacy && !rsn_eid)
+		wil_info(wil, "WSC connection\n");
 
 	bss = cfg80211_get_bss(wiphy, sme->channel, sme->bssid,
 			       sme->ssid, sme->ssid_len,
@@ -425,10 +442,17 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 	wil->privacy = sme->privacy;
 
 	if (wil->privacy) {
-		/* For secure assoc, send WMI_DELETE_CIPHER_KEY_CMD */
-		rc = wmi_del_cipher_key(wil, 0, bss->bssid);
+		/* For secure assoc, remove old keys */
+		rc = wmi_del_cipher_key(wil, 0, bss->bssid,
+					WMI_KEY_USE_PAIRWISE);
+		if (rc) {
+			wil_err(wil, "WMI_DELETE_CIPHER_KEY_CMD(PTK) failed\n");
+			goto out;
+		}
+		rc = wmi_del_cipher_key(wil, 0, bss->bssid,
+					WMI_KEY_USE_RX_GROUP);
 		if (rc) {
-			wil_err(wil, "WMI_DELETE_CIPHER_KEY_CMD failed\n");
+			wil_err(wil, "WMI_DELETE_CIPHER_KEY_CMD(GTK) failed\n");
 			goto out;
 		}
 	}
@@ -458,11 +482,18 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 		goto out;
 	}
 	if (wil->privacy) {
-		conn.dot11_auth_mode = WMI_AUTH11_SHARED;
-		conn.auth_mode = WMI_AUTH_WPA2_PSK;
-		conn.pairwise_crypto_type = WMI_CRYPT_AES_GCMP;
-		conn.pairwise_crypto_len = 16;
-	} else {
+		if (rsn_eid) { /* regular secure connection */
+			conn.dot11_auth_mode = WMI_AUTH11_SHARED;
+			conn.auth_mode = WMI_AUTH_WPA2_PSK;
+			conn.pairwise_crypto_type = WMI_CRYPT_AES_GCMP;
+			conn.pairwise_crypto_len = 16;
+			conn.group_crypto_type = WMI_CRYPT_AES_GCMP;
+			conn.group_crypto_len = 16;
+		} else { /* WSC */
+			conn.dot11_auth_mode = WMI_AUTH11_WSC;
+			conn.auth_mode = WMI_AUTH_NONE;
+		}
+	} else { /* insecure connection */
 		conn.dot11_auth_mode = WMI_AUTH11_OPEN;
 		conn.auth_mode = WMI_AUTH_NONE;
 	}
@@ -507,6 +538,8 @@ static int wil_cfg80211_disconnect(struct wiphy *wiphy,
 	int rc;
 	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
 
+	wil_dbg_misc(wil, "%s(reason=%d)\n", __func__, reason_code);
+
 	rc = wmi_send(wil, WMI_DISCONNECT_CMDID, NULL, 0);
 
 	return rc;
@@ -561,6 +594,39 @@ static int wil_cfg80211_set_channel(struct wiphy *wiphy,
 	return 0;
 }
 
+static enum wmi_key_usage wil_detect_key_usage(struct wil6210_priv *wil,
+					       bool pairwise)
+{
+	struct wireless_dev *wdev = wil->wdev;
+	enum wmi_key_usage rc;
+	static const char * const key_usage_str[] = {
+		[WMI_KEY_USE_PAIRWISE]	= "WMI_KEY_USE_PAIRWISE",
+		[WMI_KEY_USE_RX_GROUP]	= "WMI_KEY_USE_RX_GROUP",
+		[WMI_KEY_USE_TX_GROUP]	= "WMI_KEY_USE_TX_GROUP",
+	};
+
+	if (pairwise) {
+		rc = WMI_KEY_USE_PAIRWISE;
+	} else {
+		switch (wdev->iftype) {
+		case NL80211_IFTYPE_STATION:
+			rc = WMI_KEY_USE_RX_GROUP;
+			break;
+		case NL80211_IFTYPE_AP:
+			rc = WMI_KEY_USE_TX_GROUP;
+			break;
+		default:
+			/* TODO: Rx GTK or Tx GTK? */
+			wil_err(wil, "Can't determine GTK type\n");
+			rc = WMI_KEY_USE_RX_GROUP;
+			break;
+		}
+	}
+	wil_dbg_misc(wil, "%s() -> %s\n", __func__, key_usage_str[rc]);
+
+	return rc;
+}
+
 static int wil_cfg80211_add_key(struct wiphy *wiphy,
 				struct net_device *ndev,
 				u8 key_index, bool pairwise,
@@ -568,13 +634,13 @@ static int wil_cfg80211_add_key(struct wiphy *wiphy,
 				struct key_params *params)
 {
 	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	enum wmi_key_usage key_usage = wil_detect_key_usage(wil, pairwise);
 
-	/* group key is not used */
-	if (!pairwise)
-		return 0;
+	wil_dbg_misc(wil, "%s(%pM[%d] %s)\n", __func__, mac_addr, key_index,
+		     pairwise ? "PTK" : "GTK");
 
-	return wmi_add_cipher_key(wil, key_index, mac_addr,
-				  params->key_len, params->key);
+	return wmi_add_cipher_key(wil, key_index, mac_addr, params->key_len,
+				  params->key, key_usage);
 }
 
 static int wil_cfg80211_del_key(struct wiphy *wiphy,
@@ -583,12 +649,12 @@ static int wil_cfg80211_del_key(struct wiphy *wiphy,
 				const u8 *mac_addr)
 {
 	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	enum wmi_key_usage key_usage = wil_detect_key_usage(wil, pairwise);
 
-	/* group key is not used */
-	if (!pairwise)
-		return 0;
+	wil_dbg_misc(wil, "%s(%pM[%d] %s)\n", __func__, mac_addr, key_index,
+		     pairwise ? "PTK" : "GTK");
 
-	return wmi_del_cipher_key(wil, key_index, mac_addr);
+	return wmi_del_cipher_key(wil, key_index, mac_addr, key_usage);
 }
 
 /* Need to be present or wiphy_new() will WARN */
@@ -661,11 +727,6 @@ static int wil_fix_bcon(struct wil6210_priv *wil,
 	if (bcon->probe_resp_len <= hlen)
 		return 0;
 
-	if (!bcon->proberesp_ies) {
-		bcon->proberesp_ies = f->u.probe_resp.variable;
-		bcon->proberesp_ies_len = bcon->probe_resp_len - hlen;
-		rc = 1;
-	}
 	if (!bcon->assocresp_ies) {
 		bcon->assocresp_ies = f->u.probe_resp.variable;
 		bcon->assocresp_ies_len = bcon->probe_resp_len - hlen;
@@ -680,9 +741,19 @@ static int wil_cfg80211_change_beacon(struct wiphy *wiphy,
 				      struct cfg80211_beacon_data *bcon)
 {
 	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	struct ieee80211_mgmt *f = (struct ieee80211_mgmt *)bcon->probe_resp;
+	size_t hlen = offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
+	const u8 *pr_ies = NULL;
+	size_t pr_ies_len = 0;
 	int rc;
 
 	wil_dbg_misc(wil, "%s()\n", __func__);
+	wil_print_bcon_data(bcon);
+
+	if (bcon->probe_resp_len > hlen) {
+		pr_ies = f->u.probe_resp.variable;
+		pr_ies_len = bcon->probe_resp_len - hlen;
+	}
 
 	if (wil_fix_bcon(wil, bcon)) {
 		wil_dbg_misc(wil, "Fixed bcon\n");
@@ -695,9 +766,7 @@ static int wil_cfg80211_change_beacon(struct wiphy *wiphy,
 	 * wmi_set_ie(wil, WMI_FRAME_BEACON, bcon->beacon_ies_len,
 	 * bcon->beacon_ies);
 	 */
-	rc = wmi_set_ie(wil, WMI_FRAME_PROBE_RESP,
-			bcon->proberesp_ies_len,
-			bcon->proberesp_ies);
+	rc = wmi_set_ie(wil, WMI_FRAME_PROBE_RESP, pr_ies_len, pr_ies);
 	if (rc) {
 		wil_err(wil, "set_ie(PROBE_RESP) failed\n");
 		return rc;
@@ -725,6 +794,11 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 	struct cfg80211_beacon_data *bcon = &info->beacon;
 	struct cfg80211_crypto_settings *crypto = &info->crypto;
 	u8 wmi_nettype = wil_iftype_nl2wmi(wdev->iftype);
+	struct ieee80211_mgmt *f = (struct ieee80211_mgmt *)bcon->probe_resp;
+	size_t hlen = offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
+	const u8 *pr_ies = NULL;
+	size_t pr_ies_len = 0;
+	u8 hidden_ssid;
 
 	wil_dbg_misc(wil, "%s()\n", __func__);
 
@@ -737,6 +811,8 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 		     channel->center_freq, info->privacy ? "secure" : "open");
 	wil_dbg_misc(wil, "Privacy: %d auth_type %d\n",
 		     info->privacy, info->auth_type);
+	wil_dbg_misc(wil, "Hidden SSID mode: %d\n",
+		     info->hidden_ssid);
 	wil_dbg_misc(wil, "BI %d DTIM %d\n", info->beacon_interval,
 		     info->dtim_period);
 	print_hex_dump_bytes("SSID ", DUMP_PREFIX_OFFSET,
@@ -744,6 +820,11 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 	wil_print_bcon_data(bcon);
 	wil_print_crypto(wil, crypto);
 
+	if (bcon->probe_resp_len > hlen) {
+		pr_ies = f->u.probe_resp.variable;
+		pr_ies_len = bcon->probe_resp_len - hlen;
+	}
+
 	if (wil_fix_bcon(wil, bcon)) {
 		wil_dbg_misc(wil, "Fixed bcon\n");
 		wil_print_bcon_data(bcon);
@@ -771,17 +852,34 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 	 * wmi_set_ie(wil, WMI_FRAME_BEACON, bcon->beacon_ies_len,
 	 * bcon->beacon_ies);
 	 */
-	wmi_set_ie(wil, WMI_FRAME_PROBE_RESP, bcon->proberesp_ies_len,
-		   bcon->proberesp_ies);
+	wmi_set_ie(wil, WMI_FRAME_PROBE_RESP, pr_ies_len, pr_ies);
 	wmi_set_ie(wil, WMI_FRAME_ASSOC_RESP, bcon->assocresp_ies_len,
 		   bcon->assocresp_ies);
 
 	wil->privacy = info->privacy;
 
+	switch (info->hidden_ssid) {
+	case NL80211_HIDDEN_SSID_NOT_IN_USE:
+		hidden_ssid = WMI_HIDDEN_SSID_DISABLED;
+		break;
+
+	case NL80211_HIDDEN_SSID_ZERO_LEN:
+		hidden_ssid = WMI_HIDDEN_SSID_SEND_EMPTY;
+		break;
+
+	case NL80211_HIDDEN_SSID_ZERO_CONTENTS:
+		hidden_ssid = WMI_HIDDEN_SSID_CLEAR;
+		break;
+
+	default:
+		rc = -EOPNOTSUPP;
+		goto out;
+	}
+
 	netif_carrier_on(ndev);
 
 	rc = wmi_pcp_start(wil, info->beacon_interval, wmi_nettype,
-			   channel->hw_value);
+			   channel->hw_value, hidden_ssid);
 	if (rc)
 		goto err_pcp_start;
 
@@ -814,13 +912,9 @@ static int wil_cfg80211_stop_ap(struct wiphy *wiphy,
 	wmi_pcp_stop(wil);
 
 	__wil_down(wil);
-	__wil_up(wil);
 
 	mutex_unlock(&wil->mutex);
 
-	/* some functions above might fail (e.g. __wil_up). Nevertheless, we
-	 * return success because AP has stopped
-	 */
 	return 0;
 }
 
@@ -830,6 +924,9 @@ static int wil_cfg80211_del_station(struct wiphy *wiphy,
 {
 	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
 
+	wil_dbg_misc(wil, "%s(%pM, reason=%d)\n", __func__, params->mac,
+		     params->reason_code);
+
 	mutex_lock(&wil->mutex);
 	wil6210_disconnect(wil, params->mac, params->reason_code, false);
 	mutex_unlock(&wil->mutex);
@@ -967,8 +1064,7 @@ static struct cfg80211_ops wil_cfg80211_ops = {
 
 static void wil_wiphy_init(struct wiphy *wiphy)
 {
-	/* TODO: set real value */
-	wiphy->max_scan_ssids = 10;
+	wiphy->max_scan_ssids = 1;
 	wiphy->max_scan_ie_len = WMI_MAX_IE_LEN;
 	wiphy->max_num_pmkids = 0 /* TODO: */;
 	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index bbc22d88f78f..75219a1b8805 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2015 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
@@ -24,6 +24,7 @@
 #include "wil6210.h"
 #include "wmi.h"
 #include "txrx.h"
+#include "pmc.h"
 
 /* Nasty hack. Better have per device instances */
 static u32 mem_addr;
@@ -123,15 +124,17 @@ static int wil_vring_debugfs_show(struct seq_file *s, void *data)
 
 			if (cid < WIL6210_MAX_CID)
 				seq_printf(s,
-					   "\n%pM CID %d TID %d BACK([%u] %u TU A%s) [%3d|%3d] idle %s\n",
+					   "\n%pM CID %d TID %d 1x%s BACK([%u] %u TU A%s) [%3d|%3d] idle %s\n",
 					   wil->sta[cid].addr, cid, tid,
+					   txdata->dot1x_open ? "+" : "-",
 					   txdata->agg_wsize,
 					   txdata->agg_timeout,
 					   txdata->agg_amsdu ? "+" : "-",
 					   used, avail, sidle);
 			else
 				seq_printf(s,
-					   "\nBroadcast [%3d|%3d] idle %s\n",
+					   "\nBroadcast 1x%s [%3d|%3d] idle %s\n",
+					   txdata->dot1x_open ? "+" : "-",
 					   used, avail, sidle);
 
 			wil_print_vring(s, wil, name, vring, '_', 'H');
@@ -702,6 +705,89 @@ static const struct file_operations fops_back = {
 	.open  = simple_open,
 };
 
+/* pmc control, write:
+ * - "alloc <num descriptors> <descriptor_size>" to allocate PMC
+ * - "free" to release memory allocated for PMC
+ */
+static ssize_t wil_write_pmccfg(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[9];
+	int num_descs, desc_size;
+
+	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", cmd, &num_descs, &desc_size);
+	kfree(kbuf);
+
+	if (rc < 0)
+		return rc;
+
+	if (rc < 1) {
+		wil_err(wil, "pmccfg: no params given\n");
+		return -EINVAL;
+	}
+
+	if (0 == strcmp(cmd, "alloc")) {
+		if (rc != 3) {
+			wil_err(wil, "pmccfg: alloc requires 2 params\n");
+			return -EINVAL;
+		}
+		wil_pmc_alloc(wil, num_descs, desc_size);
+	} else if (0 == strcmp(cmd, "free")) {
+		if (rc != 1) {
+			wil_err(wil, "pmccfg: free does not have any params\n");
+			return -EINVAL;
+		}
+		wil_pmc_free(wil, true);
+	} else {
+		wil_err(wil, "pmccfg: Unrecognized command \"%s\"\n", cmd);
+		return -EINVAL;
+	}
+
+	return len;
+}
+
+static ssize_t wil_read_pmccfg(struct file *file, char __user *user_buf,
+			       size_t count, loff_t *ppos)
+{
+	struct wil6210_priv *wil = file->private_data;
+	char text[256];
+	char help[] = "pmc control, write:\n"
+	" - \"alloc <num descriptors> <descriptor_size>\" to allocate pmc\n"
+	" - \"free\" to free memory allocated for pmc\n";
+
+	sprintf(text, "Last command status: %d\n\n%s",
+		wil_pmc_last_cmd_status(wil),
+		help);
+
+	return simple_read_from_buffer(user_buf, count, ppos, text,
+				       strlen(text) + 1);
+}
+
+static const struct file_operations fops_pmccfg = {
+	.read = wil_read_pmccfg,
+	.write = wil_write_pmccfg,
+	.open  = simple_open,
+};
+
+static const struct file_operations fops_pmcdata = {
+	.open		= simple_open,
+	.read		= wil_pmc_read,
+	.llseek		= wil_pmc_llseek,
+};
+
 /*---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,
@@ -1111,8 +1197,7 @@ static int wil_link_debugfs_show(struct seq_file *s, void *data)
 			status = "connected";
 			break;
 		}
-		seq_printf(s, "[%d] %pM %s%s\n", i, p->addr, status,
-			   (p->data_port_open ? " data_port_open" : ""));
+		seq_printf(s, "[%d] %pM %s\n", i, p->addr, status);
 
 		if (p->status == wil_sta_connected) {
 			rc = wil_cid_fill_sinfo(wil, i, &sinfo);
@@ -1275,7 +1360,7 @@ 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;
+	int i, tid, mcs;
 
 	for (i = 0; i < ARRAY_SIZE(wil->sta); i++) {
 		struct wil_sta_info *p = &wil->sta[i];
@@ -1292,8 +1377,7 @@ __acquires(&p->tid_rx_lock) __releases(&p->tid_rx_lock)
 			status = "connected";
 			break;
 		}
-		seq_printf(s, "[%d] %pM %s%s\n", i, p->addr, status,
-			   (p->data_port_open ? " data_port_open" : ""));
+		seq_printf(s, "[%d] %pM %s\n", i, p->addr, status);
 
 		if (p->status == wil_sta_connected) {
 			spin_lock_bh(&p->tid_rx_lock);
@@ -1306,6 +1390,12 @@ __acquires(&p->tid_rx_lock) __releases(&p->tid_rx_lock)
 				}
 			}
 			spin_unlock_bh(&p->tid_rx_lock);
+			seq_puts(s, "Rx/MCS:");
+			for (mcs = 0; mcs < ARRAY_SIZE(p->stats.rx_per_mcs);
+			     mcs++)
+				seq_printf(s, " %lld",
+					   p->stats.rx_per_mcs[mcs]);
+			seq_puts(s, "\n");
 		}
 	}
 
@@ -1363,6 +1453,8 @@ static const struct {
 	{"tx_mgmt",		  S_IWUSR,	&fops_txmgmt},
 	{"wmi_send",		  S_IWUSR,	&fops_wmi},
 	{"back",	S_IRUGO | S_IWUSR,	&fops_back},
+	{"pmccfg",	S_IRUGO | S_IWUSR,	&fops_pmccfg},
+	{"pmcdata",	S_IRUGO,		&fops_pmcdata},
 	{"temp",	S_IRUGO,		&fops_temp},
 	{"freq",	S_IRUGO,		&fops_freq},
 	{"link",	S_IRUGO,		&fops_link},
@@ -1440,6 +1532,8 @@ int wil6210_debugfs_init(struct wil6210_priv *wil)
 	if (IS_ERR_OR_NULL(dbg))
 		return -ENODEV;
 
+	wil_pmc_init(wil);
+
 	wil6210_debugfs_init_files(wil, dbg);
 	wil6210_debugfs_init_isr(wil, dbg);
 	wil6210_debugfs_init_blobs(wil, dbg);
@@ -1459,4 +1553,9 @@ void wil6210_debugfs_remove(struct wil6210_priv *wil)
 {
 	debugfs_remove_recursive(wil->debug);
 	wil->debug = NULL;
+
+	/* free pmc memory without sending command to fw, as it will
+	 * be reset on the way down anyway
+	 */
+	wil_pmc_free(wil, false);
 }
diff --git a/drivers/net/wireless/ath/wil6210/main.c b/drivers/net/wireless/ath/wil6210/main.c
index c2a238426425..6ca6193ab8a6 100644
--- a/drivers/net/wireless/ath/wil6210/main.c
+++ b/drivers/net/wireless/ath/wil6210/main.c
@@ -25,6 +25,10 @@
 #define WAIT_FOR_DISCONNECT_TIMEOUT_MS 2000
 #define WAIT_FOR_DISCONNECT_INTERVAL_MS 10
 
+bool debug_fw; /* = false; */
+module_param(debug_fw, bool, S_IRUGO);
+MODULE_PARM_DESC(debug_fw, " do not perform card reset. For FW debug");
+
 bool no_fw_recovery;
 module_param(no_fw_recovery, bool, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(no_fw_recovery, " disable automatic FW error recovery");
@@ -58,7 +62,7 @@ static int mtu_max_set(const char *val, const struct kernel_param *kp)
 	return ret;
 }
 
-static struct kernel_param_ops mtu_max_ops = {
+static const struct kernel_param_ops mtu_max_ops = {
 	.set = mtu_max_set,
 	.get = param_get_uint,
 };
@@ -87,7 +91,7 @@ static int ring_order_set(const char *val, const struct kernel_param *kp)
 	return 0;
 }
 
-static struct kernel_param_ops ring_order_ops = {
+static const struct kernel_param_ops ring_order_ops = {
 	.set = ring_order_set,
 	.get = param_get_uint,
 };
@@ -96,6 +100,8 @@ module_param_cb(rx_ring_order, &ring_order_ops, &rx_ring_order, S_IRUGO);
 MODULE_PARM_DESC(rx_ring_order, " Rx ring order; size = 1 << order");
 module_param_cb(tx_ring_order, &ring_order_ops, &tx_ring_order, S_IRUGO);
 MODULE_PARM_DESC(tx_ring_order, " Tx ring order; size = 1 << order");
+module_param_cb(bcast_ring_order, &ring_order_ops, &bcast_ring_order, S_IRUGO);
+MODULE_PARM_DESC(bcast_ring_order, " Bcast ring order; size = 1 << order");
 
 #define RST_DELAY (20) /* msec, for loop in @wil_target_reset */
 #define RST_COUNT (1 + 1000/RST_DELAY) /* round up to be above 1 sec total */
@@ -146,7 +152,6 @@ __acquires(&sta->tid_rx_lock) __releases(&sta->tid_rx_lock)
 	wil_dbg_misc(wil, "%s(CID %d, status %d)\n", __func__, cid,
 		     sta->status);
 
-	sta->data_port_open = false;
 	if (sta->status != wil_sta_unused) {
 		if (!from_event)
 			wmi_disconnect_sta(wil, sta->addr, reason_code);
@@ -224,7 +229,7 @@ static void _wil6210_disconnect(struct wil6210_priv *wil, const u8 *bssid,
 		if (test_bit(wil_status_fwconnected, wil->status)) {
 			clear_bit(wil_status_fwconnected, wil->status);
 			cfg80211_disconnected(ndev, reason_code,
-					      NULL, 0, GFP_KERNEL);
+					      NULL, 0, false, GFP_KERNEL);
 		} else if (test_bit(wil_status_fwconnecting, wil->status)) {
 			cfg80211_connect_result(ndev, bssid, NULL, 0, NULL, 0,
 						WLAN_STATUS_UNSPECIFIED_FAILURE,
@@ -373,9 +378,10 @@ int wil_bcast_init(struct wil6210_priv *wil)
 	if (ri < 0)
 		return ri;
 
+	wil->bcast_vring = ri;
 	rc = wil_vring_init_bcast(wil, ri, 1 << bcast_ring_order);
-	if (rc == 0)
-		wil->bcast_vring = ri;
+	if (rc)
+		wil->bcast_vring = -1;
 
 	return rc;
 }
@@ -547,7 +553,7 @@ static inline void wil_release_cpu(struct wil6210_priv *wil)
 static int wil_target_reset(struct wil6210_priv *wil)
 {
 	int delay = 0;
-	u32 x;
+	u32 x, x1 = 0;
 
 	wil_dbg_misc(wil, "Resetting \"%s\"...\n", wil->hw_name);
 
@@ -602,12 +608,16 @@ static int wil_target_reset(struct wil6210_priv *wil)
 	do {
 		msleep(RST_DELAY);
 		x = R(RGF_USER_BL + offsetof(struct RGF_BL, ready));
+		if (x1 != x) {
+			wil_dbg_misc(wil, "BL.ready 0x%08x => 0x%08x\n", x1, x);
+			x1 = x;
+		}
 		if (delay++ > RST_COUNT) {
 			wil_err(wil, "Reset not completed, bl.ready 0x%08x\n",
 				x);
 			return -ETIME;
 		}
-	} while (!(x & BIT_BL_READY));
+	} while (x != BIT_BL_READY);
 
 	C(RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_RST_PWGD);
 
@@ -686,6 +696,17 @@ int wil_reset(struct wil6210_priv *wil, bool load_fw)
 	WARN_ON(!mutex_is_locked(&wil->mutex));
 	WARN_ON(test_bit(wil_status_napi_en, wil->status));
 
+	if (debug_fw) {
+		static const u8 mac[ETH_ALEN] = {
+			0x00, 0xde, 0xad, 0x12, 0x34, 0x56,
+		};
+		struct net_device *ndev = wil_to_ndev(wil);
+
+		ether_addr_copy(ndev->perm_addr, mac);
+		ether_addr_copy(ndev->dev_addr, ndev->perm_addr);
+		return 0;
+	}
+
 	cancel_work_sync(&wil->disconnect_worker);
 	wil6210_disconnect(wil, NULL, WLAN_REASON_DEAUTH_LEAVING, false);
 	wil_bcast_fini(wil);
diff --git a/drivers/net/wireless/ath/wil6210/netdev.c b/drivers/net/wireless/ath/wil6210/netdev.c
index f2f7ea29558e..8ef18ace110f 100644
--- a/drivers/net/wireless/ath/wil6210/netdev.c
+++ b/drivers/net/wireless/ath/wil6210/netdev.c
@@ -24,6 +24,11 @@ static int wil_open(struct net_device *ndev)
 
 	wil_dbg_misc(wil, "%s()\n", __func__);
 
+	if (debug_fw) {
+		wil_err(wil, "%s() while in debug_fw mode\n", __func__);
+		return -EINVAL;
+	}
+
 	return wil_up(wil);
 }
 
@@ -127,7 +132,7 @@ static void wil_dev_setup(struct net_device *dev)
 	dev->tx_queue_len = WIL_TX_Q_LEN_DEFAULT;
 }
 
-void *wil_if_alloc(struct device *dev, void __iomem *csr)
+void *wil_if_alloc(struct device *dev)
 {
 	struct net_device *ndev;
 	struct wireless_dev *wdev;
@@ -142,7 +147,6 @@ void *wil_if_alloc(struct device *dev, void __iomem *csr)
 	}
 
 	wil = wdev_to_wil(wdev);
-	wil->csr = csr;
 	wil->wdev = wdev;
 
 	wil_dbg_misc(wil, "%s()\n", __func__);
diff --git a/drivers/net/wireless/ath/wil6210/pcie_bus.c b/drivers/net/wireless/ath/wil6210/pcie_bus.c
index 109986114abf..aa3ecc607ca3 100644
--- a/drivers/net/wireless/ath/wil6210/pcie_bus.c
+++ b/drivers/net/wireless/ath/wil6210/pcie_bus.c
@@ -27,10 +27,6 @@ MODULE_PARM_DESC(use_msi,
 		 " Use MSI interrupt: "
 		 "0 - don't, 1 - (default) - single, or 3");
 
-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)
 {
@@ -133,8 +129,6 @@ static int wil_if_pcie_enable(struct wil6210_priv *wil)
 	mutex_lock(&wil->mutex);
 	rc = wil_reset(wil, false);
 	mutex_unlock(&wil->mutex);
-	if (debug_fw)
-		rc = 0;
 	if (rc)
 		goto release_irq;
 
@@ -169,7 +163,6 @@ 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;
 	int rc;
 
 	/* check HW */
@@ -184,9 +177,28 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		return -ENODEV;
 	}
 
+	wil = wil_if_alloc(dev);
+	if (IS_ERR(wil)) {
+		rc = (int)PTR_ERR(wil);
+		dev_err(dev, "wil_if_alloc failed: %d\n", rc);
+		return rc;
+	}
+	wil->pdev = pdev;
+	pci_set_drvdata(pdev, wil);
+	/* rollback to if_free */
+
+	wil->platform_handle =
+			wil_platform_init(&pdev->dev, &wil->platform_ops);
+	if (!wil->platform_handle) {
+		rc = -ENODEV;
+		wil_err(wil, "wil_platform_init failed\n");
+		goto if_free;
+	}
+	/* rollback to err_plat */
+
 	rc = pci_enable_device(pdev);
 	if (rc) {
-		dev_err(&pdev->dev,
+		wil_err(wil,
 			"pci_enable_device failed, retry with MSI only\n");
 		/* Work around for platforms that can't allocate IRQ:
 		 * retry with MSI only
@@ -194,47 +206,37 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		pdev->msi_enabled = 1;
 		rc = pci_enable_device(pdev);
 	}
-	if (rc)
-		return -ENODEV;
+	if (rc) {
+		wil_err(wil,
+			"pci_enable_device failed, even with MSI only\n");
+		goto err_plat;
+	}
 	/* rollback to err_disable_pdev */
 
 	rc = pci_request_region(pdev, 0, WIL_NAME);
 	if (rc) {
-		dev_err(&pdev->dev, "pci_request_region failed\n");
+		wil_err(wil, "pci_request_region failed\n");
 		goto err_disable_pdev;
 	}
 	/* rollback to err_release_reg */
 
-	csr = pci_ioremap_bar(pdev, 0);
-	if (!csr) {
-		dev_err(&pdev->dev, "pci_ioremap_bar failed\n");
+	wil->csr = pci_ioremap_bar(pdev, 0);
+	if (!wil->csr) {
+		wil_err(wil, "pci_ioremap_bar failed\n");
 		rc = -ENODEV;
 		goto err_release_reg;
 	}
 	/* rollback to err_iounmap */
-	dev_info(&pdev->dev, "CSR at %pR -> 0x%p\n", &pdev->resource[0], csr);
-
-	wil = wil_if_alloc(dev, csr);
-	if (IS_ERR(wil)) {
-		rc = (int)PTR_ERR(wil);
-		dev_err(dev, "wil_if_alloc failed: %d\n", rc);
-		goto err_iounmap;
-	}
-	/* rollback to if_free */
+	wil_info(wil, "CSR at %pR -> 0x%p\n", &pdev->resource[0], wil->csr);
 
-	pci_set_drvdata(pdev, wil);
-	wil->pdev = pdev;
 	wil_set_capabilities(wil);
 	wil6210_clear_irq(wil);
 
-	wil->platform_handle =
-			wil_platform_init(&pdev->dev, &wil->platform_ops);
-
 	/* FW should raise IRQ when ready */
 	rc = wil_if_pcie_enable(wil);
 	if (rc) {
 		wil_err(wil, "Enable device failed\n");
-		goto if_free;
+		goto err_iounmap;
 	}
 	/* rollback to bus_disable */
 
@@ -249,18 +251,19 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	return 0;
 
- bus_disable:
+bus_disable:
 	wil_if_pcie_disable(wil);
- if_free:
+err_iounmap:
+	pci_iounmap(pdev, wil->csr);
+err_release_reg:
+	pci_release_region(pdev, 0);
+err_disable_pdev:
+	pci_disable_device(pdev);
+err_plat:
 	if (wil->platform_ops.uninit)
 		wil->platform_ops.uninit(wil->platform_handle);
+if_free:
 	wil_if_free(wil);
- err_iounmap:
-	pci_iounmap(pdev, csr);
- err_release_reg:
-	pci_release_region(pdev, 0);
- err_disable_pdev:
-	pci_disable_device(pdev);
 
 	return rc;
 }
@@ -275,12 +278,12 @@ static void wil_pcie_remove(struct pci_dev *pdev)
 	wil6210_debugfs_remove(wil);
 	wil_if_remove(wil);
 	wil_if_pcie_disable(wil);
-	if (wil->platform_ops.uninit)
-		wil->platform_ops.uninit(wil->platform_handle);
-	wil_if_free(wil);
 	pci_iounmap(pdev, csr);
 	pci_release_region(pdev, 0);
 	pci_disable_device(pdev);
+	if (wil->platform_ops.uninit)
+		wil->platform_ops.uninit(wil->platform_handle);
+	wil_if_free(wil);
 }
 
 static const struct pci_device_id wil6210_pcie_ids[] = {
@@ -297,7 +300,27 @@ static struct pci_driver wil6210_driver = {
 	.name		= WIL_NAME,
 };
 
-module_pci_driver(wil6210_driver);
+static int __init wil6210_driver_init(void)
+{
+	int rc;
+
+	rc = wil_platform_modinit();
+	if (rc)
+		return rc;
+
+	rc = pci_register_driver(&wil6210_driver);
+	if (rc)
+		wil_platform_modexit();
+	return rc;
+}
+module_init(wil6210_driver_init);
+
+static void __exit wil6210_driver_exit(void)
+{
+	pci_unregister_driver(&wil6210_driver);
+	wil_platform_modexit();
+}
+module_exit(wil6210_driver_exit);
 
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Qualcomm Atheros <wil6210@qca.qualcomm.com>");
diff --git a/drivers/net/wireless/ath/wil6210/pmc.c b/drivers/net/wireless/ath/wil6210/pmc.c
new file mode 100644
index 000000000000..8a8cdc61b25b
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/pmc.c
@@ -0,0 +1,375 @@
+/*
+ * Copyright (c) 2012-2015 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/types.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include "wmi.h"
+#include "wil6210.h"
+#include "txrx.h"
+#include "pmc.h"
+
+struct desc_alloc_info {
+	dma_addr_t pa;
+	void	  *va;
+};
+
+static int wil_is_pmc_allocated(struct pmc_ctx *pmc)
+{
+	return !!pmc->pring_va;
+}
+
+void wil_pmc_init(struct wil6210_priv *wil)
+{
+	memset(&wil->pmc, 0, sizeof(struct pmc_ctx));
+	mutex_init(&wil->pmc.lock);
+}
+
+/**
+ * Allocate the physical ring (p-ring) and the required
+ * number of descriptors of required size.
+ * Initialize the descriptors as required by pmc dma.
+ * The descriptors' buffers dwords are initialized to hold
+ * dword's serial number in the lsw and reserved value
+ * PCM_DATA_INVALID_DW_VAL in the msw.
+ */
+void wil_pmc_alloc(struct wil6210_priv *wil,
+		   int num_descriptors,
+		   int descriptor_size)
+{
+	u32 i;
+	struct pmc_ctx *pmc = &wil->pmc;
+	struct device *dev = wil_to_dev(wil);
+	struct wmi_pmc_cmd pmc_cmd = {0};
+
+	mutex_lock(&pmc->lock);
+
+	if (wil_is_pmc_allocated(pmc)) {
+		/* sanity check */
+		wil_err(wil, "%s: ERROR pmc is already allocated\n", __func__);
+		goto no_release_err;
+	}
+
+	pmc->num_descriptors = num_descriptors;
+	pmc->descriptor_size = descriptor_size;
+
+	wil_dbg_misc(wil, "%s: %d descriptors x %d bytes each\n",
+		     __func__, num_descriptors, descriptor_size);
+
+	/* allocate descriptors info list in pmc context*/
+	pmc->descriptors = kcalloc(num_descriptors,
+				  sizeof(struct desc_alloc_info),
+				  GFP_KERNEL);
+	if (!pmc->descriptors) {
+		wil_err(wil, "%s: ERROR allocating pmc skb list\n", __func__);
+		goto no_release_err;
+	}
+
+	wil_dbg_misc(wil,
+		     "%s: allocated descriptors info list %p\n",
+		     __func__, pmc->descriptors);
+
+	/* Allocate pring buffer and descriptors.
+	 * vring->va should be aligned on its size rounded up to power of 2
+	 * This is granted by the dma_alloc_coherent
+	 */
+	pmc->pring_va = dma_alloc_coherent(dev,
+			sizeof(struct vring_tx_desc) * num_descriptors,
+			&pmc->pring_pa,
+			GFP_KERNEL);
+
+	wil_dbg_misc(wil,
+		     "%s: allocated pring %p => %pad. %zd x %d = total %zd bytes\n",
+		     __func__,
+		     pmc->pring_va, &pmc->pring_pa,
+		     sizeof(struct vring_tx_desc),
+		     num_descriptors,
+		     sizeof(struct vring_tx_desc) * num_descriptors);
+
+	if (!pmc->pring_va) {
+		wil_err(wil, "%s: ERROR allocating pmc pring\n", __func__);
+		goto release_pmc_skb_list;
+	}
+
+	/* initially, all descriptors are SW owned
+	 * For Tx, Rx, and PMC, ownership bit is at the same location, thus
+	 * we can use any
+	 */
+	for (i = 0; i < num_descriptors; i++) {
+		struct vring_tx_desc *_d = &pmc->pring_va[i];
+		struct vring_tx_desc dd, *d = &dd;
+		int j = 0;
+
+		pmc->descriptors[i].va = dma_alloc_coherent(dev,
+			descriptor_size,
+			&pmc->descriptors[i].pa,
+			GFP_KERNEL);
+
+		if (unlikely(!pmc->descriptors[i].va)) {
+			wil_err(wil,
+				"%s: ERROR allocating pmc descriptor %d",
+				__func__, i);
+			goto release_pmc_skbs;
+		}
+
+		for (j = 0; j < descriptor_size / sizeof(u32); j++) {
+			u32 *p = (u32 *)pmc->descriptors[i].va + j;
+			*p = PCM_DATA_INVALID_DW_VAL | j;
+		}
+
+		/* configure dma descriptor */
+		d->dma.addr.addr_low =
+			cpu_to_le32(lower_32_bits(pmc->descriptors[i].pa));
+		d->dma.addr.addr_high =
+			cpu_to_le16((u16)upper_32_bits(pmc->descriptors[i].pa));
+		d->dma.status = 0; /* 0 = HW_OWNED */
+		d->dma.length = cpu_to_le16(descriptor_size);
+		d->dma.d0 = BIT(9) | RX_DMA_D0_CMD_DMA_IT;
+		*_d = *d;
+	}
+
+	wil_dbg_misc(wil, "%s: allocated successfully\n", __func__);
+
+	pmc_cmd.op = WMI_PMC_ALLOCATE;
+	pmc_cmd.ring_size = cpu_to_le16(pmc->num_descriptors);
+	pmc_cmd.mem_base = cpu_to_le64(pmc->pring_pa);
+
+	wil_dbg_misc(wil, "%s: send WMI_PMC_CMD with ALLOCATE op\n", __func__);
+	pmc->last_cmd_status = wmi_send(wil,
+					WMI_PMC_CMDID,
+					&pmc_cmd,
+					sizeof(pmc_cmd));
+	if (pmc->last_cmd_status) {
+		wil_err(wil,
+			"%s: WMI_PMC_CMD with ALLOCATE op failed with status %d",
+			__func__, pmc->last_cmd_status);
+		goto release_pmc_skbs;
+	}
+
+	mutex_unlock(&pmc->lock);
+
+	return;
+
+release_pmc_skbs:
+	wil_err(wil, "%s: exit on error: Releasing skbs...\n", __func__);
+	for (i = 0; pmc->descriptors[i].va && i < num_descriptors; i++) {
+		dma_free_coherent(dev,
+				  descriptor_size,
+				  pmc->descriptors[i].va,
+				  pmc->descriptors[i].pa);
+
+		pmc->descriptors[i].va = NULL;
+	}
+	wil_err(wil, "%s: exit on error: Releasing pring...\n", __func__);
+
+	dma_free_coherent(dev,
+			  sizeof(struct vring_tx_desc) * num_descriptors,
+			  pmc->pring_va,
+			  pmc->pring_pa);
+
+	pmc->pring_va = NULL;
+
+release_pmc_skb_list:
+	wil_err(wil, "%s: exit on error: Releasing descriptors info list...\n",
+		__func__);
+	kfree(pmc->descriptors);
+	pmc->descriptors = NULL;
+
+no_release_err:
+	pmc->last_cmd_status = -ENOMEM;
+	mutex_unlock(&pmc->lock);
+}
+
+/**
+ * Traverse the p-ring and release all buffers.
+ * At the end release the p-ring memory
+ */
+void wil_pmc_free(struct wil6210_priv *wil, int send_pmc_cmd)
+{
+	struct pmc_ctx *pmc = &wil->pmc;
+	struct device *dev = wil_to_dev(wil);
+	struct wmi_pmc_cmd pmc_cmd = {0};
+
+	mutex_lock(&pmc->lock);
+
+	pmc->last_cmd_status = 0;
+
+	if (!wil_is_pmc_allocated(pmc)) {
+		wil_dbg_misc(wil, "%s: Error, can't free - not allocated\n",
+			     __func__);
+		pmc->last_cmd_status = -EPERM;
+		mutex_unlock(&pmc->lock);
+		return;
+	}
+
+	if (send_pmc_cmd) {
+		wil_dbg_misc(wil, "%s: send WMI_PMC_CMD with RELEASE op\n",
+			     __func__);
+		pmc_cmd.op = WMI_PMC_RELEASE;
+		pmc->last_cmd_status =
+				wmi_send(wil, WMI_PMC_CMDID, &pmc_cmd,
+					 sizeof(pmc_cmd));
+		if (pmc->last_cmd_status) {
+			wil_err(wil,
+				"%s WMI_PMC_CMD with RELEASE op failed, status %d",
+				__func__, pmc->last_cmd_status);
+			/* There's nothing we can do with this error.
+			 * Normally, it should never occur.
+			 * Continue to freeing all memory allocated for pmc.
+			 */
+		}
+	}
+
+	if (pmc->pring_va) {
+		size_t buf_size = sizeof(struct vring_tx_desc) *
+				  pmc->num_descriptors;
+
+		wil_dbg_misc(wil, "%s: free pring va %p\n",
+			     __func__, pmc->pring_va);
+		dma_free_coherent(dev, buf_size, pmc->pring_va, pmc->pring_pa);
+
+		pmc->pring_va = NULL;
+	} else {
+		pmc->last_cmd_status = -ENOENT;
+	}
+
+	if (pmc->descriptors) {
+		int i;
+
+		for (i = 0;
+		     pmc->descriptors[i].va && i < pmc->num_descriptors; i++) {
+			dma_free_coherent(dev,
+					  pmc->descriptor_size,
+					  pmc->descriptors[i].va,
+					  pmc->descriptors[i].pa);
+			pmc->descriptors[i].va = NULL;
+		}
+		wil_dbg_misc(wil, "%s: free descriptor info %d/%d\n",
+			     __func__, i, pmc->num_descriptors);
+		wil_dbg_misc(wil,
+			     "%s: free pmc descriptors info list %p\n",
+			     __func__, pmc->descriptors);
+		kfree(pmc->descriptors);
+		pmc->descriptors = NULL;
+	} else {
+		pmc->last_cmd_status = -ENOENT;
+	}
+
+	mutex_unlock(&pmc->lock);
+}
+
+/**
+ * Status of the last operation requested via debugfs: alloc/free/read.
+ * 0 - success or negative errno
+ */
+int wil_pmc_last_cmd_status(struct wil6210_priv *wil)
+{
+	wil_dbg_misc(wil, "%s: status %d\n", __func__,
+		     wil->pmc.last_cmd_status);
+
+	return wil->pmc.last_cmd_status;
+}
+
+/**
+ * Read from required position up to the end of current descriptor,
+ * depends on descriptor size configured during alloc request.
+ */
+ssize_t wil_pmc_read(struct file *filp, char __user *buf, size_t count,
+		     loff_t *f_pos)
+{
+	struct wil6210_priv *wil = filp->private_data;
+	struct pmc_ctx *pmc = &wil->pmc;
+	size_t retval = 0;
+	unsigned long long idx;
+	loff_t offset;
+	size_t pmc_size = pmc->descriptor_size * pmc->num_descriptors;
+
+	mutex_lock(&pmc->lock);
+
+	if (!wil_is_pmc_allocated(pmc)) {
+		wil_err(wil, "%s: error, pmc is not allocated!\n", __func__);
+		pmc->last_cmd_status = -EPERM;
+		mutex_unlock(&pmc->lock);
+		return -EPERM;
+	}
+
+	wil_dbg_misc(wil,
+		     "%s: size %u, pos %lld\n",
+		     __func__, (unsigned)count, *f_pos);
+
+	pmc->last_cmd_status = 0;
+
+	idx = *f_pos;
+	do_div(idx, pmc->descriptor_size);
+	offset = *f_pos - (idx * pmc->descriptor_size);
+
+	if (*f_pos >= pmc_size) {
+		wil_dbg_misc(wil, "%s: reached end of pmc buf: %lld >= %u\n",
+			     __func__, *f_pos, (unsigned)pmc_size);
+		pmc->last_cmd_status = -ERANGE;
+		goto out;
+	}
+
+	wil_dbg_misc(wil,
+		     "%s: read from pos %lld (descriptor %llu, offset %llu) %zu bytes\n",
+		     __func__, *f_pos, idx, offset, count);
+
+	/* if no errors, return the copied byte count */
+	retval = simple_read_from_buffer(buf,
+					 count,
+					 &offset,
+					 pmc->descriptors[idx].va,
+					 pmc->descriptor_size);
+	*f_pos += retval;
+out:
+	mutex_unlock(&pmc->lock);
+
+	return retval;
+}
+
+loff_t wil_pmc_llseek(struct file *filp, loff_t off, int whence)
+{
+	loff_t newpos;
+	struct wil6210_priv *wil = filp->private_data;
+	struct pmc_ctx *pmc = &wil->pmc;
+	size_t pmc_size = pmc->descriptor_size * pmc->num_descriptors;
+
+	switch (whence) {
+	case 0: /* SEEK_SET */
+		newpos = off;
+		break;
+
+	case 1: /* SEEK_CUR */
+		newpos = filp->f_pos + off;
+		break;
+
+	case 2: /* SEEK_END */
+		newpos = pmc_size;
+		break;
+
+	default: /* can't happen */
+		return -EINVAL;
+	}
+
+	if (newpos < 0)
+		return -EINVAL;
+	if (newpos > pmc_size)
+		newpos = pmc_size;
+
+	filp->f_pos = newpos;
+
+	return newpos;
+}
diff --git a/drivers/net/wireless/ath/wil6210/pmc.h b/drivers/net/wireless/ath/wil6210/pmc.h
new file mode 100644
index 000000000000..bebc8d52e1e6
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/pmc.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2012-2015 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/types.h>
+
+#define PCM_DATA_INVALID_DW_VAL (0xB0BA0000)
+
+void wil_pmc_init(struct wil6210_priv *wil);
+void wil_pmc_alloc(struct wil6210_priv *wil,
+		   int num_descriptors, int descriptor_size);
+void wil_pmc_free(struct wil6210_priv *wil, int send_pmc_cmd);
+int wil_pmc_last_cmd_status(struct wil6210_priv *wil);
+ssize_t wil_pmc_read(struct file *, char __user *, size_t, loff_t *);
+loff_t wil_pmc_llseek(struct file *filp, loff_t off, int whence);
diff --git a/drivers/net/wireless/ath/wil6210/txrx.c b/drivers/net/wireless/ath/wil6210/txrx.c
index e8bd512d81a9..aa20af86e1d6 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.c
+++ b/drivers/net/wireless/ath/wil6210/txrx.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2015 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
@@ -236,7 +236,7 @@ static int wil_vring_alloc_skb(struct wil6210_priv *wil, struct vring *vring,
 		return -ENOMEM;
 	}
 
-	d->dma.d0 = BIT(9) | RX_DMA_D0_CMD_DMA_IT;
+	d->dma.d0 = RX_DMA_D0_CMD_DMA_RT | RX_DMA_D0_CMD_DMA_IT;
 	wil_desc_addr_set(&d->dma.addr, pa);
 	/* ip_length don't care */
 	/* b11 don't care */
@@ -427,6 +427,8 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	cid = wil_rxdesc_cid(d);
 	stats = &wil->sta[cid].stats;
 	stats->last_mcs_rx = wil_rxdesc_mcs(d);
+	if (stats->last_mcs_rx < ARRAY_SIZE(stats->rx_per_mcs))
+		stats->rx_per_mcs[stats->last_mcs_rx]++;
 
 	/* use radiotap header only if required */
 	if (ndev->type == ARPHRD_IEEE80211_RADIOTAP)
@@ -724,6 +726,8 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 
 	cmd.vring_cfg.tx_sw_ring.ring_mem_base = cpu_to_le64(vring->pa);
 
+	if (!wil->privacy)
+		txdata->dot1x_open = true;
 	rc = wmi_call(wil, WMI_VRING_CFG_CMDID, &cmd, sizeof(cmd),
 		      WMI_VRING_CFG_DONE_EVENTID, &reply, sizeof(reply), 100);
 	if (rc)
@@ -738,11 +742,13 @@ 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))
+	if (txdata->dot1x_open && (agg_wsize >= 0))
 		wil_addba_tx_request(wil, id, agg_wsize);
 
 	return 0;
  out_free:
+	txdata->dot1x_open = false;
+	txdata->enabled = 0;
 	wil_vring_free(wil, vring, 1);
  out:
 
@@ -792,6 +798,8 @@ int wil_vring_init_bcast(struct wil6210_priv *wil, int id, int size)
 
 	cmd.vring_cfg.tx_sw_ring.ring_mem_base = cpu_to_le64(vring->pa);
 
+	if (!wil->privacy)
+		txdata->dot1x_open = true;
 	rc = wmi_call(wil, WMI_BCAST_VRING_CFG_CMDID, &cmd, sizeof(cmd),
 		      WMI_VRING_CFG_DONE_EVENTID, &reply, sizeof(reply), 100);
 	if (rc)
@@ -809,6 +817,8 @@ int wil_vring_init_bcast(struct wil6210_priv *wil, int id, int size)
 
 	return 0;
  out_free:
+	txdata->enabled = 0;
+	txdata->dot1x_open = false;
 	wil_vring_free(wil, vring, 1);
  out:
 
@@ -828,6 +838,7 @@ void wil_vring_fini_tx(struct wil6210_priv *wil, int id)
 	wil_dbg_misc(wil, "%s() id=%d\n", __func__, id);
 
 	spin_lock_bh(&txdata->lock);
+	txdata->dot1x_open = false;
 	txdata->enabled = 0; /* no Tx can be in progress or start anew */
 	spin_unlock_bh(&txdata->lock);
 	/* make sure NAPI won't touch this vring */
@@ -848,12 +859,11 @@ static struct vring *wil_find_tx_ucast(struct wil6210_priv *wil,
 	if (cid < 0)
 		return NULL;
 
-	if (!wil->sta[cid].data_port_open &&
-	    (skb->protocol != cpu_to_be16(ETH_P_PAE)))
-		return NULL;
-
 	/* TODO: fix for multiple TID */
 	for (i = 0; i < ARRAY_SIZE(wil->vring2cid_tid); i++) {
+		if (!wil->vring_tx_data[i].dot1x_open &&
+		    (skb->protocol != cpu_to_be16(ETH_P_PAE)))
+			continue;
 		if (wil->vring2cid_tid[i][0] == cid) {
 			struct vring *v = &wil->vring_tx[i];
 
@@ -883,7 +893,7 @@ static struct vring *wil_find_tx_vring_sta(struct wil6210_priv *wil,
 
 	/* 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
+	 * find 1-st vring eligible for this skb and use it.
 	 */
 	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
 		v = &wil->vring_tx[i];
@@ -894,9 +904,9 @@ static struct vring *wil_find_tx_vring_sta(struct wil6210_priv *wil,
 		if (cid >= WIL6210_MAX_CID) /* skip BCAST */
 			continue;
 
-		if (!wil->sta[cid].data_port_open &&
+		if (!wil->vring_tx_data[i].dot1x_open &&
 		    (skb->protocol != cpu_to_be16(ETH_P_PAE)))
-			break;
+			continue;
 
 		wil_dbg_txrx(wil, "Tx -> ring %d\n", i);
 
@@ -918,7 +928,6 @@ static struct vring *wil_find_tx_vring_sta(struct wil6210_priv *wil,
  *    in all cases override dest address to unicast peer's address
  * Use old strategy when new is not supported yet:
  *  - for PBSS
- *  - for secure link
  */
 static struct vring *wil_find_tx_bcast_1(struct wil6210_priv *wil,
 					 struct sk_buff *skb)
@@ -931,6 +940,9 @@ static struct vring *wil_find_tx_bcast_1(struct wil6210_priv *wil,
 	v = &wil->vring_tx[i];
 	if (!v->va)
 		return NULL;
+	if (!wil->vring_tx_data[i].dot1x_open &&
+	    (skb->protocol != cpu_to_be16(ETH_P_PAE)))
+		return NULL;
 
 	return v;
 }
@@ -963,7 +975,8 @@ static struct vring *wil_find_tx_bcast_2(struct wil6210_priv *wil,
 		cid = wil->vring2cid_tid[i][0];
 		if (cid >= WIL6210_MAX_CID) /* skip BCAST */
 			continue;
-		if (!wil->sta[cid].data_port_open)
+		if (!wil->vring_tx_data[i].dot1x_open &&
+		    (skb->protocol != cpu_to_be16(ETH_P_PAE)))
 			continue;
 
 		/* don't Tx back to source when re-routing Rx->Tx at the AP */
@@ -989,7 +1002,8 @@ found:
 		cid = wil->vring2cid_tid[i][0];
 		if (cid >= WIL6210_MAX_CID) /* skip BCAST */
 			continue;
-		if (!wil->sta[cid].data_port_open)
+		if (!wil->vring_tx_data[i].dot1x_open &&
+		    (skb->protocol != cpu_to_be16(ETH_P_PAE)))
 			continue;
 
 		if (0 == memcmp(wil->sta[cid].addr, src, ETH_ALEN))
@@ -1016,9 +1030,6 @@ static struct vring *wil_find_tx_bcast(struct wil6210_priv *wil,
 	if (wdev->iftype != NL80211_IFTYPE_AP)
 		return wil_find_tx_bcast_2(wil, skb);
 
-	if (wil->privacy)
-		return wil_find_tx_bcast_2(wil, skb);
-
 	return wil_find_tx_bcast_1(wil, skb);
 }
 
@@ -1144,13 +1155,8 @@ static int __wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 	wil_tx_desc_map(d, pa, len, vring_index);
 	if (unlikely(mcast)) {
 		d->mac.d[0] |= BIT(MAC_CFG_DESC_TX_0_MCS_EN_POS); /* MCS 0 */
-		if (unlikely(len > WIL_BCAST_MCS0_LIMIT)) {
-			/* set MCS 1 */
+		if (unlikely(len > WIL_BCAST_MCS0_LIMIT)) /* set MCS 1 */
 			d->mac.d[0] |= (1 << MAC_CFG_DESC_TX_0_MCS_INDEX_POS);
-			/* packet mode 2 */
-			d->mac.d[1] |= BIT(MAC_CFG_DESC_TX_1_PKT_MODE_EN_POS) |
-				       (2 << MAC_CFG_DESC_TX_1_PKT_MODE_POS);
-		}
 	}
 	/* Process TCP/UDP checksum offloading */
 	if (unlikely(wil_tx_desc_offload_cksum_set(wil, d, skb))) {
diff --git a/drivers/net/wireless/ath/wil6210/txrx.h b/drivers/net/wireless/ath/wil6210/txrx.h
index d90c8aa20c15..0c4638487c74 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.h
+++ b/drivers/net/wireless/ath/wil6210/txrx.h
@@ -384,19 +384,27 @@ struct vring_rx_mac {
  *  [word 7] length
  */
 
-#define RX_DMA_D0_CMD_DMA_IT     BIT(10)
-
-/* Error field, offload bits */
-#define RX_DMA_ERROR_L3_ERR   BIT(4)
-#define RX_DMA_ERROR_L4_ERR   BIT(5)
+#define RX_DMA_D0_CMD_DMA_EOP	BIT(8)
+#define RX_DMA_D0_CMD_DMA_RT	BIT(9)  /* always 1 */
+#define RX_DMA_D0_CMD_DMA_IT	BIT(10) /* interrupt */
+
+/* Error field */
+#define RX_DMA_ERROR_FCS	BIT(0)
+#define RX_DMA_ERROR_MIC	BIT(1)
+#define RX_DMA_ERROR_KEY	BIT(2) /* Key missing */
+#define RX_DMA_ERROR_REPLAY	BIT(3)
+#define RX_DMA_ERROR_L3_ERR	BIT(4)
+#define RX_DMA_ERROR_L4_ERR	BIT(5)
 
 /* Status field */
-#define RX_DMA_STATUS_DU         BIT(0)
-#define RX_DMA_STATUS_ERROR      BIT(2)
-
+#define RX_DMA_STATUS_DU	BIT(0)
+#define RX_DMA_STATUS_EOP	BIT(1)
+#define RX_DMA_STATUS_ERROR	BIT(2)
+#define RX_DMA_STATUS_MI	BIT(3) /* MAC Interrupt is asserted */
 #define RX_DMA_STATUS_L3I	BIT(4)
 #define RX_DMA_STATUS_L4I	BIT(5)
 #define RX_DMA_STATUS_PHY_INFO	BIT(6)
+#define RX_DMA_STATUS_FFM	BIT(7) /* EtherType Flex Filter Match */
 
 struct vring_rx_dma {
 	u32 d0;
diff --git a/drivers/net/wireless/ath/wil6210/wil6210.h b/drivers/net/wireless/ath/wil6210/wil6210.h
index 4310972c9e16..275355d46a36 100644
--- a/drivers/net/wireless/ath/wil6210/wil6210.h
+++ b/drivers/net/wireless/ath/wil6210/wil6210.h
@@ -21,6 +21,7 @@
 #include <linux/wireless.h>
 #include <net/cfg80211.h>
 #include <linux/timex.h>
+#include <linux/types.h>
 #include "wil_platform.h"
 
 extern bool no_fw_recovery;
@@ -29,10 +30,11 @@ extern unsigned short rx_ring_overflow_thrsh;
 extern int agg_wsize;
 extern u32 vring_idle_trsh;
 extern bool rx_align_2;
+extern bool debug_fw;
 
 #define WIL_NAME "wil6210"
 #define WIL_FW_NAME "wil6210.fw" /* code */
-#define WIL_FW2_NAME "wil6210.board" /* board & radio parameters */
+#define WIL_FW2_NAME "wil6210.brd" /* board & radio parameters */
 
 #define WIL_MAX_BUS_REQUEST_KBPS 800000 /* ~6.1Gbps */
 
@@ -279,7 +281,7 @@ struct fw_map {
 };
 
 /* array size should be in sync with actual definition in the wmi.c */
-extern const struct fw_map fw_mapping[7];
+extern const struct fw_map fw_mapping[8];
 
 /**
  * mk_cidxtid - construct @cidxtid field
@@ -396,6 +398,7 @@ struct vring {
  * Additional data for Tx Vring
  */
 struct vring_tx_data {
+	bool dot1x_open;
 	int enabled;
 	cycles_t idle, last_idle, begin;
 	u8 agg_wsize; /* agreed aggregation window, 0 - no agg */
@@ -461,6 +464,7 @@ enum wil_sta_status {
 };
 
 #define WIL_STA_TID_NUM (16)
+#define WIL_MCS_MAX (12) /* Maximum MCS supported */
 
 struct wil_net_stats {
 	unsigned long	rx_packets;
@@ -470,6 +474,7 @@ struct wil_net_stats {
 	unsigned long	tx_errors;
 	unsigned long	rx_dropped;
 	u16 last_mcs_rx;
+	u64 rx_per_mcs[WIL_MCS_MAX + 1];
 };
 
 /**
@@ -484,7 +489,6 @@ struct wil_sta_info {
 	u8 addr[ETH_ALEN];
 	enum wil_sta_status status;
 	struct wil_net_stats stats;
-	bool data_port_open; /* can send any data, not only EAPOL */
 	/* Rx BACK */
 	struct wil_tid_ampdu_rx *tid_rx[WIL_STA_TID_NUM];
 	spinlock_t tid_rx_lock; /* guarding tid_rx array */
@@ -526,6 +530,17 @@ struct wil_probe_client_req {
 	u8 cid;
 };
 
+struct pmc_ctx {
+	/* alloc, free, and read operations must own the lock */
+	struct mutex		lock;
+	struct vring_tx_desc	*pring_va;
+	dma_addr_t		pring_pa;
+	struct desc_alloc_info  *descriptors;
+	int			last_cmd_status;
+	int			num_descriptors;
+	int			descriptor_size;
+};
+
 struct wil6210_priv {
 	struct pci_dev *pdev;
 	int n_msi;
@@ -610,6 +625,8 @@ struct wil6210_priv {
 
 	void *platform_handle;
 	struct wil_platform_ops platform_ops;
+
+	struct pmc_ctx pmc;
 };
 
 #define wil_to_wiphy(i) (i->wdev->wiphy)
@@ -669,7 +686,7 @@ void wil_memcpy_fromio_32(void *dst, const volatile void __iomem *src,
 void wil_memcpy_toio_32(volatile void __iomem *dst, const void *src,
 			size_t count);
 
-void *wil_if_alloc(struct device *dev, void __iomem *csr);
+void *wil_if_alloc(struct device *dev);
 void wil_if_free(struct wil6210_priv *wil);
 int wil_if_add(struct wil6210_priv *wil);
 void wil_if_remove(struct wil6210_priv *wil);
@@ -701,9 +718,10 @@ int wmi_get_ssid(struct wil6210_priv *wil, u8 *ssid_len, void *ssid);
 int wmi_set_channel(struct wil6210_priv *wil, int channel);
 int wmi_get_channel(struct wil6210_priv *wil, int *channel);
 int wmi_del_cipher_key(struct wil6210_priv *wil, u8 key_index,
-		       const void *mac_addr);
+		       const void *mac_addr, int key_usage);
 int wmi_add_cipher_key(struct wil6210_priv *wil, u8 key_index,
-		       const void *mac_addr, int key_len, const void *key);
+		       const void *mac_addr, int key_len, const void *key,
+		       int key_usage);
 int wmi_echo(struct wil6210_priv *wil);
 int wmi_set_ie(struct wil6210_priv *wil, u8 type, u16 ie_len, const void *ie);
 int wmi_rx_chain_add(struct wil6210_priv *wil, struct vring *vring);
@@ -746,7 +764,8 @@ struct wireless_dev *wil_cfg80211_init(struct device *dev);
 void wil_wdev_free(struct wil6210_priv *wil);
 
 int wmi_set_mac_address(struct wil6210_priv *wil, void *addr);
-int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype, u8 chan);
+int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype,
+		  u8 chan, u8 hidden_ssid);
 int wmi_pcp_stop(struct wil6210_priv *wil);
 void wil6210_disconnect(struct wil6210_priv *wil, const u8 *bssid,
 			u16 reason_code, bool from_event);
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform.c b/drivers/net/wireless/ath/wil6210/wil_platform.c
index 976a071ba74e..de15f1422fe9 100644
--- a/drivers/net/wireless/ath/wil6210/wil_platform.c
+++ b/drivers/net/wireless/ath/wil6210/wil_platform.c
@@ -17,6 +17,15 @@
 #include "linux/device.h"
 #include "wil_platform.h"
 
+int __init wil_platform_modinit(void)
+{
+	return 0;
+}
+
+void wil_platform_modexit(void)
+{
+}
+
 /**
  * wil_platform_init() - wil6210 platform module init
  *
@@ -26,10 +35,11 @@
  */
 void *wil_platform_init(struct device *dev, struct wil_platform_ops *ops)
 {
-	void *handle = NULL;
+	void *handle = ops; /* to return some non-NULL for 'void' impl. */
 
 	if (!ops) {
-		dev_err(dev, "Invalid parameter. Cannot init platform module\n");
+		dev_err(dev,
+			"Invalid parameter. Cannot init platform module\n");
 		return NULL;
 	}
 
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform.h b/drivers/net/wireless/ath/wil6210/wil_platform.h
index 158c73b049a9..d7fa19b7886d 100644
--- a/drivers/net/wireless/ath/wil6210/wil_platform.h
+++ b/drivers/net/wireless/ath/wil6210/wil_platform.h
@@ -31,4 +31,7 @@ struct wil_platform_ops {
 
 void *wil_platform_init(struct device *dev, struct wil_platform_ops *ops);
 
+int __init wil_platform_modinit(void);
+void wil_platform_modexit(void);
+
 #endif /* __WIL_PLATFORM_H__ */
diff --git a/drivers/net/wireless/ath/wil6210/wmi.c b/drivers/net/wireless/ath/wil6210/wmi.c
index 9fe2085be2c5..c759759afbb2 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.c
+++ b/drivers/net/wireless/ath/wil6210/wmi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2015 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
@@ -85,6 +85,7 @@ const struct fw_map fw_mapping[] = {
 	{0x880000, 0x88a000, 0x880000, "rgf"},     /* various RGF       40k */
 	{0x88a000, 0x88b000, 0x88a000, "AGC_tbl"}, /* AGC table          4k */
 	{0x88b000, 0x88c000, 0x88b000, "rgf_ext"}, /* Pcie_ext_rgf       4k */
+	{0x88c000, 0x88c200, 0x88c000, "mac_rgf_ext"}, /* mac_ext_rgf  512b */
 	{0x8c0000, 0x949000, 0x8c0000, "upper"},   /* upper area       548k */
 	/*
 	 * 920000..930000 ucode code RAM
@@ -543,55 +544,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)
+static void wmi_evt_vring_en(struct wil6210_priv *wil, int id, void *d, int len)
 {
-	struct vring_tx_data *t;
-	int i;
+	struct wmi_vring_en_event *evt = d;
+	u8 vri = evt->vring_index;
 
-	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_dbg_wmi(wil, "Enable vring %d\n", vri);
 
-		wil_addba_tx_request(wil, i, wsize);
-	}
-}
-
-static void wmi_evt_linkup(struct wil6210_priv *wil, int id, void *d, int len)
-{
-	struct wmi_data_port_open_event *evt = d;
-	u8 cid = evt->cid;
-
-	wil_dbg_wmi(wil, "Link UP for CID %d\n", cid);
-
-	if (cid >= ARRAY_SIZE(wil->sta)) {
-		wil_err(wil, "Link UP for invalid CID %d\n", cid);
+	if (vri >= ARRAY_SIZE(wil->vring_tx)) {
+		wil_err(wil, "Enable for invalid vring %d\n", vri);
 		return;
 	}
-
-	wil->sta[cid].data_port_open = true;
-	if (agg_wsize >= 0)
-		wil_addba_tx_cid(wil, cid, agg_wsize);
-}
-
-static void wmi_evt_linkdown(struct wil6210_priv *wil, int id, void *d, int len)
-{
-	struct net_device *ndev = wil_to_ndev(wil);
-	struct wmi_wbe_link_down_event *evt = d;
-	u8 cid = evt->cid;
-
-	wil_dbg_wmi(wil, "Link DOWN for CID %d, reason %d\n",
-		    cid, le32_to_cpu(evt->reason));
-
-	if (cid >= ARRAY_SIZE(wil->sta)) {
-		wil_err(wil, "Link DOWN for invalid CID %d\n", cid);
+	wil->vring_tx_data[vri].dot1x_open = true;
+	if (vri == wil->bcast_vring) /* no BA for bcast */
 		return;
-	}
-
-	wil->sta[cid].data_port_open = false;
-	netif_carrier_off(ndev);
+	if (agg_wsize >= 0)
+		wil_addba_tx_request(wil, vri, agg_wsize);
 }
 
 static void wmi_evt_ba_status(struct wil6210_priv *wil, int id, void *d,
@@ -695,11 +663,10 @@ static const struct {
 	{WMI_CONNECT_EVENTID,		wmi_evt_connect},
 	{WMI_DISCONNECT_EVENTID,	wmi_evt_disconnect},
 	{WMI_EAPOL_RX_EVENTID,		wmi_evt_eapol_rx},
-	{WMI_DATA_PORT_OPEN_EVENTID,	wmi_evt_linkup},
-	{WMI_WBE_LINKDOWN_EVENTID,	wmi_evt_linkdown},
 	{WMI_BA_STATUS_EVENTID,		wmi_evt_ba_status},
 	{WMI_RCP_ADDBA_REQ_EVENTID,	wmi_evt_addba_rx_req},
 	{WMI_DELBA_EVENTID,		wmi_evt_delba},
+	{WMI_VRING_EN_EVENTID,		wmi_evt_vring_en},
 };
 
 /*
@@ -844,7 +811,7 @@ int wmi_echo(struct wil6210_priv *wil)
 	};
 
 	return wmi_call(wil, WMI_ECHO_CMDID, &cmd, sizeof(cmd),
-			 WMI_ECHO_RSP_EVENTID, NULL, 0, 20);
+			WMI_ECHO_RSP_EVENTID, NULL, 0, 50);
 }
 
 int wmi_set_mac_address(struct wil6210_priv *wil, void *addr)
@@ -858,7 +825,8 @@ int wmi_set_mac_address(struct wil6210_priv *wil, void *addr)
 	return wmi_send(wil, WMI_SET_MAC_ADDRESS_CMDID, &cmd, sizeof(cmd));
 }
 
-int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype, u8 chan)
+int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype,
+		  u8 chan, u8 hidden_ssid)
 {
 	int rc;
 
@@ -868,6 +836,7 @@ int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype, u8 chan)
 		.disable_sec_offload = 1,
 		.channel = chan - 1,
 		.pcp_max_assoc_sta = max_assoc_sta,
+		.hidden_ssid = hidden_ssid,
 	};
 	struct {
 		struct wil6210_mbox_hdr_wmi wmi;
@@ -985,7 +954,7 @@ int wmi_p2p_cfg(struct wil6210_priv *wil, int channel)
 }
 
 int wmi_del_cipher_key(struct wil6210_priv *wil, u8 key_index,
-		       const void *mac_addr)
+		       const void *mac_addr, int key_usage)
 {
 	struct wmi_delete_cipher_key_cmd cmd = {
 		.key_index = key_index,
@@ -998,11 +967,12 @@ int wmi_del_cipher_key(struct wil6210_priv *wil, u8 key_index,
 }
 
 int wmi_add_cipher_key(struct wil6210_priv *wil, u8 key_index,
-		       const void *mac_addr, int key_len, const void *key)
+		       const void *mac_addr, int key_len, const void *key,
+		       int key_usage)
 {
 	struct wmi_add_cipher_key_cmd cmd = {
 		.key_index = key_index,
-		.key_usage = WMI_KEY_USE_PAIRWISE,
+		.key_usage = key_usage,
 		.key_len = key_len,
 	};
 
@@ -1238,7 +1208,8 @@ int wmi_addba_rx_resp(struct wil6210_priv *wil, u8 cid, u8 tid, u8 token,
 		    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);
+		      WMI_RCP_ADDBA_RESP_SENT_EVENTID, &reply, sizeof(reply),
+		      100);
 	if (rc)
 		return rc;
 
diff --git a/drivers/net/wireless/ath/wil6210/wmi.h b/drivers/net/wireless/ath/wil6210/wmi.h
index b29055315350..6e90e78f1554 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.h
+++ b/drivers/net/wireless/ath/wil6210/wmi.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2015 Qualcomm Atheros, Inc.
  * Copyright (c) 2006-2012 Wilocity .
  *
  * Permission to use, copy, modify, and/or distribute this software for any
@@ -253,8 +253,8 @@ struct wmi_set_passphrase_cmd {
  */
 enum wmi_key_usage {
 	WMI_KEY_USE_PAIRWISE	= 0,
-	WMI_KEY_USE_GROUP	= 1,
-	WMI_KEY_USE_TX		= 2,  /* default Tx Key - Static WEP only */
+	WMI_KEY_USE_RX_GROUP	= 1,
+	WMI_KEY_USE_TX_GROUP	= 2,
 };
 
 struct wmi_add_cipher_key_cmd {
@@ -495,10 +495,18 @@ struct wmi_power_mgmt_cfg_cmd {
 /*
  * WMI_PCP_START_CMDID
  */
+
+enum wmi_hidden_ssid {
+	WMI_HIDDEN_SSID_DISABLED	= 0,
+	WMI_HIDDEN_SSID_SEND_EMPTY	= 1,
+	WMI_HIDDEN_SSID_CLEAR	= 2,
+};
+
 struct wmi_pcp_start_cmd {
 	__le16 bcon_interval;
 	u8 pcp_max_assoc_sta;
-	u8 reserved0[9];
+	u8 hidden_ssid;
+	u8 reserved0[8];
 	u8 network_type;
 	u8 channel;
 	u8 disable_sec_offload;
@@ -836,6 +844,21 @@ struct wmi_temp_sense_cmd {
 } __packed;
 
 /*
+ * WMI_PMC_CMDID
+ */
+enum wmi_pmc_op_e {
+	WMI_PMC_ALLOCATE = 0,
+	WMI_PMC_RELEASE = 1,
+};
+
+struct wmi_pmc_cmd {
+	u8 op;		/* enum wmi_pmc_cmd_op_type */
+	u8 reserved;
+	__le16 ring_size;
+	__le64 mem_base;
+} __packed;
+
+/*
  * WMI Events
  */
 
@@ -870,7 +893,7 @@ enum wmi_event_id {
 	WMI_VRING_CFG_DONE_EVENTID		= 0x1821,
 	WMI_BA_STATUS_EVENTID			= 0x1823,
 	WMI_RCP_ADDBA_REQ_EVENTID		= 0x1824,
-	WMI_ADDBA_RESP_SENT_EVENTID		= 0x1825,
+	WMI_RCP_ADDBA_RESP_SENT_EVENTID		= 0x1825,
 	WMI_DELBA_EVENTID			= 0x1826,
 	WMI_GET_SSID_EVENTID			= 0x1828,
 	WMI_GET_PCP_CHANNEL_EVENTID		= 0x182a,
@@ -882,7 +905,7 @@ enum wmi_event_id {
 	WMI_WRITE_MAC_TXQ_EVENTID		= 0x1833,
 	WMI_WRITE_MAC_XQ_FIELD_EVENTID		= 0x1834,
 
-	WMI_BEAFORMING_MGMT_DONE_EVENTID	= 0x1836,
+	WMI_BEAMFORMING_MGMT_DONE_EVENTID	= 0x1836,
 	WMI_BF_TXSS_MGMT_DONE_EVENTID		= 0x1837,
 	WMI_BF_RXSS_MGMT_DONE_EVENTID		= 0x1839,
 	WMI_RS_MGMT_DONE_EVENTID		= 0x1852,
@@ -894,11 +917,12 @@ enum wmi_event_id {
 
 	/* Performance monitoring events */
 	WMI_DATA_PORT_OPEN_EVENTID		= 0x1860,
-	WMI_WBE_LINKDOWN_EVENTID		= 0x1861,
+	WMI_WBE_LINK_DOWN_EVENTID		= 0x1861,
 
 	WMI_BF_CTRL_DONE_EVENTID		= 0x1862,
 	WMI_NOTIFY_REQ_DONE_EVENTID		= 0x1863,
 	WMI_GET_STATUS_DONE_EVENTID		= 0x1864,
+	WMI_VRING_EN_EVENTID			= 0x1865,
 
 	WMI_UNIT_TEST_EVENTID			= 0x1900,
 	WMI_FLASH_READ_DONE_EVENTID		= 0x1902,
@@ -1147,7 +1171,7 @@ struct wmi_vring_cfg_done_event {
 } __packed;
 
 /*
- * WMI_ADDBA_RESP_SENT_EVENTID
+ * WMI_RCP_ADDBA_RESP_SENT_EVENTID
  */
 struct wmi_rcp_addba_resp_sent_event {
 	u8 cidxtid;
@@ -1179,7 +1203,7 @@ struct wmi_cfg_rx_chain_done_event {
 } __packed;
 
 /*
- * WMI_WBE_LINKDOWN_EVENTID
+ * WMI_WBE_LINK_DOWN_EVENTID
  */
 enum wmi_wbe_link_down_event_reason {
 	WMI_WBE_REASON_USER_REQUEST	= 0,
@@ -1202,6 +1226,14 @@ struct wmi_data_port_open_event {
 } __packed;
 
 /*
+ * WMI_VRING_EN_EVENTID
+ */
+struct wmi_vring_en_event {
+	u8 vring_index;
+	u8 reserved[3];
+} __packed;
+
+/*
  * WMI_GET_PCP_CHANNEL_EVENTID
  */
 struct wmi_get_pcp_channel_event {
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index b2f9521fe551..28490702124a 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -3131,8 +3131,6 @@ static void b43_adjust_opmode(struct b43_wldev *dev)
 		ctl |= B43_MACCTL_KEEP_BAD;
 	if (wl->filter_flags & FIF_PLCPFAIL)
 		ctl |= B43_MACCTL_KEEP_BADPLCP;
-	if (wl->filter_flags & FIF_PROMISC_IN_BSS)
-		ctl |= B43_MACCTL_PROMISC;
 	if (wl->filter_flags & FIF_BCN_PRBRESP_PROMISC)
 		ctl |= B43_MACCTL_BEACPROMISC;
 
@@ -4310,16 +4308,14 @@ static void b43_op_configure_filter(struct ieee80211_hw *hw,
 		goto out_unlock;
 	}
 
-	*fflags &= FIF_PROMISC_IN_BSS |
-		  FIF_ALLMULTI |
+	*fflags &= FIF_ALLMULTI |
 		  FIF_FCSFAIL |
 		  FIF_PLCPFAIL |
 		  FIF_CONTROL |
 		  FIF_OTHER_BSS |
 		  FIF_BCN_PRBRESP_PROMISC;
 
-	changed &= FIF_PROMISC_IN_BSS |
-		   FIF_ALLMULTI |
+	changed &= FIF_ALLMULTI |
 		   FIF_FCSFAIL |
 		   FIF_PLCPFAIL |
 		   FIF_CONTROL |
@@ -5365,6 +5361,10 @@ static void b43_supported_bands(struct b43_wldev *dev, bool *have_2ghz_phy,
 		*have_5ghz_phy = true;
 		return;
 	case 0x4321: /* BCM4306 */
+		/* There are 14e4:4321 PCI devs with 2.4 GHz BCM4321 (N-PHY) */
+		if (dev->phy.type != B43_PHYTYPE_G)
+			break;
+		/* fall through */
 	case 0x4313: /* BCM4311 */
 	case 0x431a: /* BCM4318 */
 	case 0x432a: /* BCM4321 */
@@ -5609,8 +5609,8 @@ static struct b43_wl *b43_wireless_init(struct b43_bus_dev *dev)
 	wl = hw_to_b43_wl(hw);
 
 	/* fill hw info */
-	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		    IEEE80211_HW_SIGNAL_DBM;
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
 
 	hw->wiphy->interface_modes =
 		BIT(NL80211_IFTYPE_AP) |
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index c77b7f59505c..afc1fb3e38df 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -2055,8 +2055,6 @@ static void b43legacy_adjust_opmode(struct b43legacy_wldev *dev)
 		ctl |= B43legacy_MACCTL_KEEP_BAD;
 	if (wl->filter_flags & FIF_PLCPFAIL)
 		ctl |= B43legacy_MACCTL_KEEP_BADPLCP;
-	if (wl->filter_flags & FIF_PROMISC_IN_BSS)
-		ctl |= B43legacy_MACCTL_PROMISC;
 	if (wl->filter_flags & FIF_BCN_PRBRESP_PROMISC)
 		ctl |= B43legacy_MACCTL_BEACPROMISC;
 
@@ -2922,16 +2920,14 @@ static void b43legacy_op_configure_filter(struct ieee80211_hw *hw,
 	}
 
 	spin_lock_irqsave(&wl->irq_lock, flags);
-	*fflags &= FIF_PROMISC_IN_BSS |
-		  FIF_ALLMULTI |
+	*fflags &= FIF_ALLMULTI |
 		  FIF_FCSFAIL |
 		  FIF_PLCPFAIL |
 		  FIF_CONTROL |
 		  FIF_OTHER_BSS |
 		  FIF_BCN_PRBRESP_PROMISC;
 
-	changed &= FIF_PROMISC_IN_BSS |
-		   FIF_ALLMULTI |
+	changed &= FIF_ALLMULTI |
 		   FIF_FCSFAIL |
 		   FIF_PLCPFAIL |
 		   FIF_CONTROL |
@@ -3836,8 +3832,9 @@ static int b43legacy_wireless_init(struct ssb_device *dev)
 	}
 
 	/* fill hw info */
-	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		    IEEE80211_HW_SIGNAL_DBM;
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+
 	hw->wiphy->interface_modes =
 		BIT(NL80211_IFTYPE_AP) |
 		BIT(NL80211_IFTYPE_STATION) |
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
index 9b508bd3b839..410a6645d316 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
@@ -33,6 +33,7 @@
 #include <linux/suspend.h>
 #include <linux/errno.h>
 #include <linux/module.h>
+#include <linux/acpi.h>
 #include <net/cfg80211.h>
 
 #include <defs.h>
@@ -987,6 +988,7 @@ static void brcmf_sdiod_freezer_detach(struct brcmf_sdio_dev *sdiodev)
 
 static int brcmf_sdiod_remove(struct brcmf_sdio_dev *sdiodev)
 {
+	sdiodev->state = BRCMF_SDIOD_DOWN;
 	if (sdiodev->bus) {
 		brcmf_sdio_remove(sdiodev->bus);
 		sdiodev->bus = NULL;
@@ -1011,6 +1013,14 @@ static int brcmf_sdiod_remove(struct brcmf_sdio_dev *sdiodev)
 	return 0;
 }
 
+static void brcmf_sdiod_host_fixup(struct mmc_host *host)
+{
+	/* runtime-pm powers off the device */
+	pm_runtime_forbid(host->parent);
+	/* avoid removal detection upon resume */
+	host->caps |= MMC_CAP_NONREMOVABLE;
+}
+
 static int brcmf_sdiod_probe(struct brcmf_sdio_dev *sdiodev)
 {
 	struct sdio_func *func;
@@ -1076,7 +1086,7 @@ static int brcmf_sdiod_probe(struct brcmf_sdio_dev *sdiodev)
 		ret = -ENODEV;
 		goto out;
 	}
-	pm_runtime_forbid(host->parent);
+	brcmf_sdiod_host_fixup(host);
 out:
 	if (ret)
 		brcmf_sdiod_remove(sdiodev);
@@ -1108,12 +1118,25 @@ MODULE_DEVICE_TABLE(sdio, brcmf_sdmmc_ids);
 static struct brcmfmac_sdio_platform_data *brcmfmac_sdio_pdata;
 
 
+static void brcmf_sdiod_acpi_set_power_manageable(struct device *dev,
+						  int val)
+{
+#if IS_ENABLED(CONFIG_ACPI)
+	struct acpi_device *adev;
+
+	adev = ACPI_COMPANION(dev);
+	if (adev)
+		adev->flags.power_manageable = 0;
+#endif
+}
+
 static int brcmf_ops_sdio_probe(struct sdio_func *func,
 				const struct sdio_device_id *id)
 {
 	int err;
 	struct brcmf_sdio_dev *sdiodev;
 	struct brcmf_bus *bus_if;
+	struct device *dev;
 
 	brcmf_dbg(SDIO, "Enter\n");
 	brcmf_dbg(SDIO, "Class=%x\n", func->class);
@@ -1121,6 +1144,10 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
 	brcmf_dbg(SDIO, "sdio device ID: 0x%04x\n", func->device);
 	brcmf_dbg(SDIO, "Function#: %d\n", func->num);
 
+	dev = &func->dev;
+	/* prohibit ACPI power management for this device */
+	brcmf_sdiod_acpi_set_power_manageable(dev, 0);
+
 	/* Consume func num 1 but dont do anything with it. */
 	if (func->num == 1)
 		return 0;
@@ -1246,15 +1273,15 @@ static int brcmf_ops_sdio_suspend(struct device *dev)
 	brcmf_sdiod_freezer_on(sdiodev);
 	brcmf_sdio_wd_timer(sdiodev->bus, 0);
 
+	sdio_flags = MMC_PM_KEEP_POWER;
 	if (sdiodev->wowl_enabled) {
-		sdio_flags = MMC_PM_KEEP_POWER;
 		if (sdiodev->pdata->oob_irq_supported)
 			enable_irq_wake(sdiodev->pdata->oob_irq_nr);
 		else
-			sdio_flags = MMC_PM_WAKE_SDIO_IRQ;
-		if (sdio_set_host_pm_flags(sdiodev->func[1], sdio_flags))
-			brcmf_err("Failed to set pm_flags %x\n", sdio_flags);
+			sdio_flags |= MMC_PM_WAKE_SDIO_IRQ;
 	}
+	if (sdio_set_host_pm_flags(sdiodev->func[1], sdio_flags))
+		brcmf_err("Failed to set pm_flags %x\n", sdio_flags);
 	return 0;
 }
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
index 8a15ebbce4a3..d86d1f1f1c91 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
@@ -52,8 +52,6 @@
 #define BRCMF_PNO_SCAN_COMPLETE		1
 #define BRCMF_PNO_SCAN_INCOMPLETE	0
 
-#define BRCMF_IFACE_MAX_CNT		3
-
 #define WPA_OUI				"\x00\x50\xF2"	/* WPA OUI */
 #define WPA_OUI_TYPE			1
 #define RSN_OUI				"\x00\x0F\xAC"	/* RSN OUI */
@@ -129,13 +127,47 @@ static struct ieee80211_rate __wl_rates[] = {
 	RATETAB_ENT(BRCM_RATE_54M, 0),
 };
 
-#define wl_a_rates		(__wl_rates + 4)
-#define wl_a_rates_size	8
 #define wl_g_rates		(__wl_rates + 0)
-#define wl_g_rates_size	12
+#define wl_g_rates_size		ARRAY_SIZE(__wl_rates)
+#define wl_a_rates		(__wl_rates + 4)
+#define wl_a_rates_size		(wl_g_rates_size - 4)
+
+#define CHAN2G(_channel, _freq) {				\
+	.band			= IEEE80211_BAND_2GHZ,		\
+	.center_freq		= (_freq),			\
+	.hw_value		= (_channel),			\
+	.flags			= IEEE80211_CHAN_DISABLED,	\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+#define CHAN5G(_channel) {					\
+	.band			= IEEE80211_BAND_5GHZ,		\
+	.center_freq		= 5000 + (5 * (_channel)),	\
+	.hw_value		= (_channel),			\
+	.flags			= IEEE80211_CHAN_DISABLED,	\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+static struct ieee80211_channel __wl_2ghz_channels[] = {
+	CHAN2G(1, 2412), CHAN2G(2, 2417), CHAN2G(3, 2422), CHAN2G(4, 2427),
+	CHAN2G(5, 2432), CHAN2G(6, 2437), CHAN2G(7, 2442), CHAN2G(8, 2447),
+	CHAN2G(9, 2452), CHAN2G(10, 2457), CHAN2G(11, 2462), CHAN2G(12, 2467),
+	CHAN2G(13, 2472), CHAN2G(14, 2484)
+};
+
+static struct ieee80211_channel __wl_5ghz_channels[] = {
+	CHAN5G(34), CHAN5G(36), CHAN5G(38), CHAN5G(40), CHAN5G(42),
+	CHAN5G(44), CHAN5G(46), CHAN5G(48), CHAN5G(52), CHAN5G(56),
+	CHAN5G(60), CHAN5G(64), CHAN5G(100), CHAN5G(104), CHAN5G(108),
+	CHAN5G(112), CHAN5G(116), CHAN5G(120), CHAN5G(124), CHAN5G(128),
+	CHAN5G(132), CHAN5G(136), CHAN5G(140), CHAN5G(144), CHAN5G(149),
+	CHAN5G(153), CHAN5G(157), CHAN5G(161), CHAN5G(165)
+};
 
 /* Band templates duplicated per wiphy. The channel info
- * is filled in after querying the device.
+ * above is added to the band during setup.
  */
 static const struct ieee80211_supported_band __wl_band_2ghz = {
 	.band = IEEE80211_BAND_2GHZ,
@@ -143,7 +175,7 @@ static const struct ieee80211_supported_band __wl_band_2ghz = {
 	.n_bitrates = wl_g_rates_size,
 };
 
-static const struct ieee80211_supported_band __wl_band_5ghz_a = {
+static const struct ieee80211_supported_band __wl_band_5ghz = {
 	.band = IEEE80211_BAND_5GHZ,
 	.bitrates = wl_a_rates,
 	.n_bitrates = wl_a_rates_size,
@@ -1262,7 +1294,7 @@ static void brcmf_link_down(struct brcmf_cfg80211_vif *vif, u16 reason)
 		}
 		clear_bit(BRCMF_VIF_STATUS_CONNECTED, &vif->sme_state);
 		cfg80211_disconnected(vif->wdev.netdev, reason, NULL, 0,
-				      GFP_KERNEL);
+				      true, GFP_KERNEL);
 
 	}
 	clear_bit(BRCMF_VIF_STATUS_CONNECTING, &vif->sme_state);
@@ -1928,7 +1960,7 @@ brcmf_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *ndev,
 
 	clear_bit(BRCMF_VIF_STATUS_CONNECTED, &ifp->vif->sme_state);
 	clear_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state);
-	cfg80211_disconnected(ndev, reason_code, NULL, 0, GFP_KERNEL);
+	cfg80211_disconnected(ndev, reason_code, NULL, 0, true, GFP_KERNEL);
 
 	memcpy(&scbval.ea, &profile->bssid, ETH_ALEN);
 	scbval.val = cpu_to_le32(reason_code);
@@ -2364,27 +2396,80 @@ brcmf_cfg80211_reconfigure_wep(struct brcmf_if *ifp)
 		brcmf_err("set wsec error (%d)\n", err);
 }
 
+static void brcmf_convert_sta_flags(u32 fw_sta_flags, struct station_info *si)
+{
+	struct nl80211_sta_flag_update *sfu;
+
+	brcmf_dbg(TRACE, "flags %08x\n", fw_sta_flags);
+	si->filled |= BIT(NL80211_STA_INFO_STA_FLAGS);
+	sfu = &si->sta_flags;
+	sfu->mask = BIT(NL80211_STA_FLAG_WME) |
+		    BIT(NL80211_STA_FLAG_AUTHENTICATED) |
+		    BIT(NL80211_STA_FLAG_ASSOCIATED) |
+		    BIT(NL80211_STA_FLAG_AUTHORIZED);
+	if (fw_sta_flags & BRCMF_STA_WME)
+		sfu->set |= BIT(NL80211_STA_FLAG_WME);
+	if (fw_sta_flags & BRCMF_STA_AUTHE)
+		sfu->set |= BIT(NL80211_STA_FLAG_AUTHENTICATED);
+	if (fw_sta_flags & BRCMF_STA_ASSOC)
+		sfu->set |= BIT(NL80211_STA_FLAG_ASSOCIATED);
+	if (fw_sta_flags & BRCMF_STA_AUTHO)
+		sfu->set |= BIT(NL80211_STA_FLAG_AUTHORIZED);
+}
+
+static void brcmf_fill_bss_param(struct brcmf_if *ifp, struct station_info *si)
+{
+	struct {
+		__le32 len;
+		struct brcmf_bss_info_le bss_le;
+	} *buf;
+	u16 capability;
+	int err;
+
+	buf = kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
+	if (!buf)
+		return;
+
+	buf->len = cpu_to_le32(WL_BSS_INFO_MAX);
+	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BSS_INFO, buf,
+				     WL_BSS_INFO_MAX);
+	if (err) {
+		brcmf_err("Failed to get bss info (%d)\n", err);
+		return;
+	}
+	si->filled |= BIT(NL80211_STA_INFO_BSS_PARAM);
+	si->bss_param.beacon_interval = le16_to_cpu(buf->bss_le.beacon_period);
+	si->bss_param.dtim_period = buf->bss_le.dtim_period;
+	capability = le16_to_cpu(buf->bss_le.capability);
+	if (capability & IEEE80211_HT_STBC_PARAM_DUAL_CTS_PROT)
+		si->bss_param.flags |= BSS_PARAM_FLAGS_CTS_PROT;
+	if (capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		si->bss_param.flags |= BSS_PARAM_FLAGS_SHORT_PREAMBLE;
+	if (capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)
+		si->bss_param.flags |= BSS_PARAM_FLAGS_SHORT_SLOT_TIME;
+}
+
 static s32
 brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
 			   const u8 *mac, struct station_info *sinfo)
 {
 	struct brcmf_if *ifp = netdev_priv(ndev);
-	struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
-	struct brcmf_scb_val_le scb_val;
-	int rssi;
-	s32 rate;
 	s32 err = 0;
-	u8 *bssid = profile->bssid;
 	struct brcmf_sta_info_le sta_info_le;
-	u32 beacon_period;
-	u32 dtim_period;
+	u32 sta_flags;
+	u32 is_tdls_peer;
 
 	brcmf_dbg(TRACE, "Enter, MAC %pM\n", mac);
 	if (!check_vif_up(ifp->vif))
 		return -EIO;
 
-	if (brcmf_is_apmode(ifp->vif)) {
-		memcpy(&sta_info_le, mac, ETH_ALEN);
+	memset(&sta_info_le, 0, sizeof(sta_info_le));
+	memcpy(&sta_info_le, mac, ETH_ALEN);
+	err = brcmf_fil_iovar_data_get(ifp, "tdls_sta_info",
+				       &sta_info_le,
+				       sizeof(sta_info_le));
+	is_tdls_peer = !err;
+	if (err) {
 		err = brcmf_fil_iovar_data_get(ifp, "sta_info",
 					       &sta_info_le,
 					       sizeof(sta_info_le));
@@ -2392,73 +2477,48 @@ brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
 			brcmf_err("GET STA INFO failed, %d\n", err);
 			goto done;
 		}
-		sinfo->filled = BIT(NL80211_STA_INFO_INACTIVE_TIME);
-		sinfo->inactive_time = le32_to_cpu(sta_info_le.idle) * 1000;
-		if (le32_to_cpu(sta_info_le.flags) & BRCMF_STA_ASSOC) {
-			sinfo->filled |= BIT(NL80211_STA_INFO_CONNECTED_TIME);
-			sinfo->connected_time = le32_to_cpu(sta_info_le.in);
-		}
-		brcmf_dbg(TRACE, "STA idle time : %d ms, connected time :%d sec\n",
-			  sinfo->inactive_time, sinfo->connected_time);
-	} else if (ifp->vif->wdev.iftype == NL80211_IFTYPE_STATION) {
-		if (memcmp(mac, bssid, ETH_ALEN)) {
-			brcmf_err("Wrong Mac address cfg_mac-%pM wl_bssid-%pM\n",
-				  mac, bssid);
-			err = -ENOENT;
-			goto done;
-		}
-		/* Report the current tx rate */
-		err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_RATE, &rate);
-		if (err) {
-			brcmf_err("Could not get rate (%d)\n", err);
-			goto done;
-		} else {
+	}
+	brcmf_dbg(TRACE, "version %d\n", le16_to_cpu(sta_info_le.ver));
+	sinfo->filled = BIT(NL80211_STA_INFO_INACTIVE_TIME);
+	sinfo->inactive_time = le32_to_cpu(sta_info_le.idle) * 1000;
+	sta_flags = le32_to_cpu(sta_info_le.flags);
+	brcmf_convert_sta_flags(sta_flags, sinfo);
+	sinfo->sta_flags.mask |= BIT(NL80211_STA_FLAG_TDLS_PEER);
+	if (is_tdls_peer)
+		sinfo->sta_flags.set |= BIT(NL80211_STA_FLAG_TDLS_PEER);
+	else
+		sinfo->sta_flags.set &= ~BIT(NL80211_STA_FLAG_TDLS_PEER);
+	if (sta_flags & BRCMF_STA_ASSOC) {
+		sinfo->filled |= BIT(NL80211_STA_INFO_CONNECTED_TIME);
+		sinfo->connected_time = le32_to_cpu(sta_info_le.in);
+		brcmf_fill_bss_param(ifp, sinfo);
+	}
+	if (sta_flags & BRCMF_STA_SCBSTATS) {
+		sinfo->filled |= BIT(NL80211_STA_INFO_TX_FAILED);
+		sinfo->tx_failed = le32_to_cpu(sta_info_le.tx_failures);
+		sinfo->filled |= BIT(NL80211_STA_INFO_TX_PACKETS);
+		sinfo->tx_packets = le32_to_cpu(sta_info_le.tx_pkts);
+		sinfo->tx_packets += le32_to_cpu(sta_info_le.tx_mcast_pkts);
+		sinfo->filled |= BIT(NL80211_STA_INFO_RX_PACKETS);
+		sinfo->rx_packets = le32_to_cpu(sta_info_le.rx_ucast_pkts);
+		sinfo->rx_packets += le32_to_cpu(sta_info_le.rx_mcast_pkts);
+		if (sinfo->tx_packets) {
 			sinfo->filled |= BIT(NL80211_STA_INFO_TX_BITRATE);
-			sinfo->txrate.legacy = rate * 5;
-			brcmf_dbg(CONN, "Rate %d Mbps\n", rate / 2);
+			sinfo->txrate.legacy = le32_to_cpu(sta_info_le.tx_rate);
+			sinfo->txrate.legacy /= 100;
 		}
-
-		if (test_bit(BRCMF_VIF_STATUS_CONNECTED,
-			     &ifp->vif->sme_state)) {
-			memset(&scb_val, 0, sizeof(scb_val));
-			err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_RSSI,
-						     &scb_val, sizeof(scb_val));
-			if (err) {
-				brcmf_err("Could not get rssi (%d)\n", err);
-				goto done;
-			} else {
-				rssi = le32_to_cpu(scb_val.val);
-				sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL);
-				sinfo->signal = rssi;
-				brcmf_dbg(CONN, "RSSI %d dBm\n", rssi);
-			}
-			err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_BCNPRD,
-						    &beacon_period);
-			if (err) {
-				brcmf_err("Could not get beacon period (%d)\n",
-					  err);
-				goto done;
-			} else {
-				sinfo->bss_param.beacon_interval =
-					beacon_period;
-				brcmf_dbg(CONN, "Beacon peroid %d\n",
-					  beacon_period);
-			}
-			err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_DTIMPRD,
-						    &dtim_period);
-			if (err) {
-				brcmf_err("Could not get DTIM period (%d)\n",
-					  err);
-				goto done;
-			} else {
-				sinfo->bss_param.dtim_period = dtim_period;
-				brcmf_dbg(CONN, "DTIM peroid %d\n",
-					  dtim_period);
-			}
-			sinfo->filled |= BIT(NL80211_STA_INFO_BSS_PARAM);
+		if (sinfo->rx_packets) {
+			sinfo->filled |= BIT(NL80211_STA_INFO_RX_BITRATE);
+			sinfo->rxrate.legacy = le32_to_cpu(sta_info_le.rx_rate);
+			sinfo->rxrate.legacy /= 100;
 		}
-	} else
-		err = -EPERM;
+		if (le16_to_cpu(sta_info_le.ver) >= 4) {
+			sinfo->filled |= BIT(NL80211_STA_INFO_TX_BYTES);
+			sinfo->tx_bytes = le64_to_cpu(sta_info_le.tx_tot_bytes);
+			sinfo->filled |= BIT(NL80211_STA_INFO_RX_BYTES);
+			sinfo->rx_bytes = le64_to_cpu(sta_info_le.rx_tot_bytes);
+		}
+	}
 done:
 	brcmf_dbg(TRACE, "Exit\n");
 	return err;
@@ -5253,40 +5313,6 @@ dongle_scantime_out:
 	return err;
 }
 
-/* Filter the list of channels received from firmware counting only
- * the 20MHz channels. The wiphy band data only needs those which get
- * flagged to indicate if they can take part in higher bandwidth.
- */
-static void brcmf_count_20mhz_channels(struct brcmf_cfg80211_info *cfg,
-				       struct brcmf_chanspec_list *chlist,
-				       u32 chcnt[])
-{
-	u32 total = le32_to_cpu(chlist->count);
-	struct brcmu_chan ch;
-	int i;
-
-	for (i = 0; i < total; i++) {
-		ch.chspec = (u16)le32_to_cpu(chlist->element[i]);
-		cfg->d11inf.decchspec(&ch);
-
-		/* Firmware gives a ordered list. We skip non-20MHz
-		 * channels is 2G. For 5G we can abort upon reaching
-		 * a non-20MHz channel in the list.
-		 */
-		if (ch.bw != BRCMU_CHAN_BW_20) {
-			if (ch.band == BRCMU_CHAN_BAND_5G)
-				break;
-			else
-				continue;
-		}
-
-		if (ch.band == BRCMU_CHAN_BAND_2G)
-			chcnt[0] += 1;
-		else if (ch.band == BRCMU_CHAN_BAND_5G)
-			chcnt[1] += 1;
-	}
-}
-
 static void brcmf_update_bw40_channel_flag(struct ieee80211_channel *channel,
 					   struct brcmu_chan *ch)
 {
@@ -5322,7 +5348,6 @@ static int brcmf_construct_chaninfo(struct brcmf_cfg80211_info *cfg,
 	u32 i, j;
 	u32 total;
 	u32 chaninfo;
-	u32 chcnt[2] = { 0, 0 };
 	u32 index;
 
 	pbuf = kzalloc(BRCMF_DCMD_MEDLEN, GFP_KERNEL);
@@ -5339,42 +5364,15 @@ static int brcmf_construct_chaninfo(struct brcmf_cfg80211_info *cfg,
 		goto fail_pbuf;
 	}
 
-	brcmf_count_20mhz_channels(cfg, list, chcnt);
 	wiphy = cfg_to_wiphy(cfg);
-	if (chcnt[0]) {
-		band = kmemdup(&__wl_band_2ghz, sizeof(__wl_band_2ghz),
-			       GFP_KERNEL);
-		if (band == NULL) {
-			err = -ENOMEM;
-			goto fail_pbuf;
-		}
-		band->channels = kcalloc(chcnt[0], sizeof(*channel),
-					 GFP_KERNEL);
-		if (band->channels == NULL) {
-			kfree(band);
-			err = -ENOMEM;
-			goto fail_pbuf;
-		}
-		band->n_channels = 0;
-		wiphy->bands[IEEE80211_BAND_2GHZ] = band;
-	}
-	if (chcnt[1]) {
-		band = kmemdup(&__wl_band_5ghz_a, sizeof(__wl_band_5ghz_a),
-			       GFP_KERNEL);
-		if (band == NULL) {
-			err = -ENOMEM;
-			goto fail_band2g;
-		}
-		band->channels = kcalloc(chcnt[1], sizeof(*channel),
-					 GFP_KERNEL);
-		if (band->channels == NULL) {
-			kfree(band);
-			err = -ENOMEM;
-			goto fail_band2g;
-		}
-		band->n_channels = 0;
-		wiphy->bands[IEEE80211_BAND_5GHZ] = band;
-	}
+	band = wiphy->bands[IEEE80211_BAND_2GHZ];
+	if (band)
+		for (i = 0; i < band->n_channels; i++)
+			band->channels[i].flags = IEEE80211_CHAN_DISABLED;
+	band = wiphy->bands[IEEE80211_BAND_5GHZ];
+	if (band)
+		for (i = 0; i < band->n_channels; i++)
+			band->channels[i].flags = IEEE80211_CHAN_DISABLED;
 
 	total = le32_to_cpu(list->count);
 	for (i = 0; i < total; i++) {
@@ -5389,6 +5387,8 @@ static int brcmf_construct_chaninfo(struct brcmf_cfg80211_info *cfg,
 			brcmf_err("Invalid channel Spec. 0x%x.\n", ch.chspec);
 			continue;
 		}
+		if (!band)
+			continue;
 		if (!(bw_cap[band->band] & WLC_BW_40MHZ_BIT) &&
 		    ch.bw == BRCMU_CHAN_BW_40)
 			continue;
@@ -5416,9 +5416,9 @@ static int brcmf_construct_chaninfo(struct brcmf_cfg80211_info *cfg,
 		} else if (ch.bw == BRCMU_CHAN_BW_40) {
 			brcmf_update_bw40_channel_flag(&channel[index], &ch);
 		} else {
-			/* disable other bandwidths for now as mentioned
-			 * order assure they are enabled for subsequent
-			 * chanspecs.
+			/* enable the channel and disable other bandwidths
+			 * for now as mentioned order assure they are enabled
+			 * for subsequent chanspecs.
 			 */
 			channel[index].flags = IEEE80211_CHAN_NO_HT40 |
 					       IEEE80211_CHAN_NO_80MHZ;
@@ -5437,16 +5437,8 @@ static int brcmf_construct_chaninfo(struct brcmf_cfg80211_info *cfg,
 						IEEE80211_CHAN_NO_IR;
 			}
 		}
-		if (index == band->n_channels)
-			band->n_channels++;
 	}
-	kfree(pbuf);
-	return 0;
 
-fail_band2g:
-	kfree(wiphy->bands[IEEE80211_BAND_2GHZ]->channels);
-	kfree(wiphy->bands[IEEE80211_BAND_2GHZ]);
-	wiphy->bands[IEEE80211_BAND_2GHZ] = NULL;
 fail_pbuf:
 	kfree(pbuf);
 	return err;
@@ -5674,53 +5666,6 @@ static int brcmf_setup_wiphybands(struct wiphy *wiphy)
 	return 0;
 }
 
-static const struct ieee80211_iface_limit brcmf_iface_limits_mbss[] = {
-	{
-		.max = 1,
-		.types = BIT(NL80211_IFTYPE_STATION) |
-			 BIT(NL80211_IFTYPE_ADHOC)
-	},
-	{
-		.max = 4,
-		.types = BIT(NL80211_IFTYPE_AP)
-	},
-	{
-		.max = 1,
-		.types = BIT(NL80211_IFTYPE_P2P_CLIENT) |
-			 BIT(NL80211_IFTYPE_P2P_GO)
-	},
-	{
-		.max = 1,
-		.types = BIT(NL80211_IFTYPE_P2P_DEVICE)
-	}
-};
-
-static const struct ieee80211_iface_limit brcmf_iface_limits_sbss[] = {
-	{
-		.max = 2,
-		.types = BIT(NL80211_IFTYPE_STATION) |
-			 BIT(NL80211_IFTYPE_ADHOC) |
-			 BIT(NL80211_IFTYPE_AP)
-	},
-	{
-		.max = 1,
-		.types = BIT(NL80211_IFTYPE_P2P_CLIENT) |
-			 BIT(NL80211_IFTYPE_P2P_GO)
-	},
-	{
-		.max = 1,
-		.types = BIT(NL80211_IFTYPE_P2P_DEVICE)
-	}
-};
-static struct ieee80211_iface_combination brcmf_iface_combos[] = {
-	{
-		 .max_interfaces = BRCMF_IFACE_MAX_CNT,
-		 .num_different_channels = 1,
-		 .n_limits = ARRAY_SIZE(brcmf_iface_limits_sbss),
-		 .limits = brcmf_iface_limits_sbss,
-	}
-};
-
 static const struct ieee80211_txrx_stypes
 brcmf_txrx_stypes[NUM_NL80211_IFTYPES] = {
 	[NL80211_IFTYPE_STATION] = {
@@ -5750,6 +5695,67 @@ brcmf_txrx_stypes[NUM_NL80211_IFTYPES] = {
 	}
 };
 
+static int brcmf_setup_ifmodes(struct wiphy *wiphy, struct brcmf_if *ifp)
+{
+	struct ieee80211_iface_combination *combo = NULL;
+	struct ieee80211_iface_limit *limits = NULL;
+	int i = 0, max_iface_cnt;
+
+	combo = kzalloc(sizeof(*combo), GFP_KERNEL);
+	if (!combo)
+		goto err;
+
+	limits = kzalloc(sizeof(*limits) * 4, GFP_KERNEL);
+	if (!limits)
+		goto err;
+
+	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+				 BIT(NL80211_IFTYPE_ADHOC) |
+				 BIT(NL80211_IFTYPE_AP);
+
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MCHAN))
+		combo->num_different_channels = 2;
+	else
+		combo->num_different_channels = 1;
+
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MBSS)) {
+		limits[i].max = 1;
+		limits[i++].types = BIT(NL80211_IFTYPE_STATION);
+		limits[i].max = 4;
+		limits[i++].types = BIT(NL80211_IFTYPE_AP);
+		max_iface_cnt = 5;
+	} else {
+		limits[i].max = 2;
+		limits[i++].types = BIT(NL80211_IFTYPE_STATION) |
+				    BIT(NL80211_IFTYPE_AP);
+		max_iface_cnt = 2;
+	}
+
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_P2P)) {
+		wiphy->interface_modes |= BIT(NL80211_IFTYPE_P2P_CLIENT) |
+					  BIT(NL80211_IFTYPE_P2P_GO) |
+					  BIT(NL80211_IFTYPE_P2P_DEVICE);
+		limits[i].max = 1;
+		limits[i++].types = BIT(NL80211_IFTYPE_P2P_CLIENT) |
+				    BIT(NL80211_IFTYPE_P2P_GO);
+		limits[i].max = 1;
+		limits[i++].types = BIT(NL80211_IFTYPE_P2P_DEVICE);
+		max_iface_cnt += 2;
+	}
+	combo->max_interfaces = max_iface_cnt;
+	combo->limits = limits;
+	combo->n_limits = i;
+
+	wiphy->iface_combinations = combo;
+	wiphy->n_iface_combinations = 1;
+	return 0;
+
+err:
+	kfree(limits);
+	kfree(combo);
+	return -ENOMEM;
+}
+
 static void brcmf_wiphy_pno_params(struct wiphy *wiphy)
 {
 	/* scheduled scan settings */
@@ -5779,28 +5785,19 @@ static void brcmf_wiphy_wowl_params(struct wiphy *wiphy)
 
 static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 {
-	struct ieee80211_iface_combination ifc_combo;
+	struct ieee80211_supported_band *band;
+	__le32 bandlist[3];
+	u32 n_bands;
+	int err, i;
+
 	wiphy->max_scan_ssids = WL_NUM_SCAN_MAX;
 	wiphy->max_scan_ie_len = BRCMF_SCAN_IE_LEN_MAX;
 	wiphy->max_num_pmkids = WL_NUM_PMKIDS_MAX;
-	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
-				 BIT(NL80211_IFTYPE_ADHOC) |
-				 BIT(NL80211_IFTYPE_AP) |
-				 BIT(NL80211_IFTYPE_P2P_CLIENT) |
-				 BIT(NL80211_IFTYPE_P2P_GO) |
-				 BIT(NL80211_IFTYPE_P2P_DEVICE);
-	/* need VSDB firmware feature for concurrent channels */
-	ifc_combo = brcmf_iface_combos[0];
-	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MCHAN))
-		ifc_combo.num_different_channels = 2;
-	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_MBSS)) {
-		ifc_combo.n_limits = ARRAY_SIZE(brcmf_iface_limits_mbss),
-		ifc_combo.limits = brcmf_iface_limits_mbss;
-	}
-	wiphy->iface_combinations = kmemdup(&ifc_combo,
-					    sizeof(ifc_combo),
-					    GFP_KERNEL);
-	wiphy->n_iface_combinations = ARRAY_SIZE(brcmf_iface_combos);
+
+	err = brcmf_setup_ifmodes(wiphy, ifp);
+	if (err)
+		return err;
+
 	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
 	wiphy->cipher_suites = __wl_cipher_suites;
 	wiphy->n_cipher_suites = ARRAY_SIZE(__wl_cipher_suites);
@@ -5812,7 +5809,8 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 		wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM;
 	wiphy->mgmt_stypes = brcmf_txrx_stypes;
 	wiphy->max_remain_on_channel_duration = 5000;
-	brcmf_wiphy_pno_params(wiphy);
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_PNO))
+		brcmf_wiphy_pno_params(wiphy);
 
 	/* vendor commands/events support */
 	wiphy->vendor_commands = brcmf_vendor_cmds;
@@ -5821,7 +5819,52 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_WOWL))
 		brcmf_wiphy_wowl_params(wiphy);
 
-	return brcmf_setup_wiphybands(wiphy);
+	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BANDLIST, &bandlist,
+				     sizeof(bandlist));
+	if (err) {
+		brcmf_err("could not obtain band info: err=%d\n", err);
+		return err;
+	}
+	/* first entry in bandlist is number of bands */
+	n_bands = le32_to_cpu(bandlist[0]);
+	for (i = 1; i <= n_bands && i < ARRAY_SIZE(bandlist); i++) {
+		if (bandlist[i] == cpu_to_le32(WLC_BAND_2G)) {
+			band = kmemdup(&__wl_band_2ghz, sizeof(__wl_band_2ghz),
+				       GFP_KERNEL);
+			if (!band)
+				return -ENOMEM;
+
+			band->channels = kmemdup(&__wl_2ghz_channels,
+						 sizeof(__wl_2ghz_channels),
+						 GFP_KERNEL);
+			if (!band->channels) {
+				kfree(band);
+				return -ENOMEM;
+			}
+
+			band->n_channels = ARRAY_SIZE(__wl_2ghz_channels);
+			wiphy->bands[IEEE80211_BAND_2GHZ] = band;
+		}
+		if (bandlist[i] == cpu_to_le32(WLC_BAND_5G)) {
+			band = kmemdup(&__wl_band_5ghz, sizeof(__wl_band_5ghz),
+				       GFP_KERNEL);
+			if (!band)
+				return -ENOMEM;
+
+			band->channels = kmemdup(&__wl_5ghz_channels,
+						 sizeof(__wl_5ghz_channels),
+						 GFP_KERNEL);
+			if (!band->channels) {
+				kfree(band);
+				return -ENOMEM;
+			}
+
+			band->n_channels = ARRAY_SIZE(__wl_5ghz_channels);
+			wiphy->bands[IEEE80211_BAND_5GHZ] = band;
+		}
+	}
+	err = brcmf_setup_wiphybands(wiphy);
+	return err;
 }
 
 static s32 brcmf_config_dongle(struct brcmf_cfg80211_info *cfg)
@@ -6007,11 +6050,20 @@ static void brcmf_cfg80211_reg_notifier(struct wiphy *wiphy,
 	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));
+	if (brcmf_fil_iovar_data_set(ifp, "country", &ccreq, sizeof(ccreq))) {
+		brcmf_err("firmware rejected country setting\n");
+		return;
+	}
+	brcmf_setup_wiphybands(wiphy);
 }
 
 static void brcmf_free_wiphy(struct wiphy *wiphy)
 {
+	if (!wiphy)
+		return;
+
+	if (wiphy->iface_combinations)
+		kfree(wiphy->iface_combinations->limits);
 	kfree(wiphy->iface_combinations);
 	if (wiphy->bands[IEEE80211_BAND_2GHZ]) {
 		kfree(wiphy->bands[IEEE80211_BAND_2GHZ]->channels);
@@ -6047,6 +6099,7 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 		brcmf_err("Could not allocate wiphy device\n");
 		return NULL;
 	}
+	memcpy(wiphy->perm_addr, drvr->mac, ETH_ALEN);
 	set_wiphy_dev(wiphy, busdev);
 
 	cfg = wiphy_priv(wiphy);
@@ -6154,10 +6207,8 @@ void brcmf_cfg80211_detach(struct brcmf_cfg80211_info *cfg)
 	if (!cfg)
 		return;
 
-	WARN_ON(!list_empty(&cfg->vif_list));
-	wiphy_unregister(cfg->wiphy);
 	brcmf_btcoex_detach(cfg);
-	brcmf_p2p_detach(&cfg->p2p);
+	wiphy_unregister(cfg->wiphy);
 	wl_deinit_priv(cfg);
 	brcmf_free_wiphy(cfg->wiphy);
 }
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.c b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
index ab2fac8b2760..288f8314f208 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/chip.c
@@ -649,6 +649,7 @@ static u32 brcmf_chip_tcm_rambase(struct brcmf_chip_priv *ci)
 	case BRCM_CC_43567_CHIP_ID:
 	case BRCM_CC_43569_CHIP_ID:
 	case BRCM_CC_43570_CHIP_ID:
+	case BRCM_CC_4358_CHIP_ID:
 	case BRCM_CC_43602_CHIP_ID:
 		return 0x180000;
 	default:
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/commonring.c b/drivers/net/wireless/brcm80211/brcmfmac/commonring.c
index 77656c711bed..7b0e52195a85 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/commonring.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/commonring.c
@@ -22,17 +22,6 @@
 #include "core.h"
 #include "commonring.h"
 
-
-/* dma flushing needs implementation for mips and arm platforms. Should
- * be put in util. Note, this is not real flushing. It is virtual non
- * cached memory. Only write buffers should have to be drained. Though
- * this may be different depending on platform......
- * SEE ALSO msgbuf.c
- */
-#define brcmf_dma_flush(addr, len)
-#define brcmf_dma_invalidate_cache(addr, len)
-
-
 void brcmf_commonring_register_cb(struct brcmf_commonring *commonring,
 				  int (*cr_ring_bell)(void *ctx),
 				  int (*cr_update_rptr)(void *ctx),
@@ -206,14 +195,9 @@ int brcmf_commonring_write_complete(struct brcmf_commonring *commonring)
 	address = commonring->buf_addr;
 	address += (commonring->f_ptr * commonring->item_len);
 	if (commonring->f_ptr > commonring->w_ptr) {
-		brcmf_dma_flush(address,
-				(commonring->depth - commonring->f_ptr) *
-				commonring->item_len);
 		address = commonring->buf_addr;
 		commonring->f_ptr = 0;
 	}
-	brcmf_dma_flush(address, (commonring->w_ptr - commonring->f_ptr) *
-			commonring->item_len);
 
 	commonring->f_ptr = commonring->w_ptr;
 
@@ -239,8 +223,6 @@ void brcmf_commonring_write_cancel(struct brcmf_commonring *commonring,
 void *brcmf_commonring_get_read_ptr(struct brcmf_commonring *commonring,
 				    u16 *n_items)
 {
-	void *ret_addr;
-
 	if (commonring->cr_update_wptr)
 		commonring->cr_update_wptr(commonring->cr_ctx);
 
@@ -251,21 +233,18 @@ void *brcmf_commonring_get_read_ptr(struct brcmf_commonring *commonring,
 	if (*n_items == 0)
 		return NULL;
 
-	ret_addr = commonring->buf_addr +
-		   (commonring->r_ptr * commonring->item_len);
-
-	commonring->r_ptr += *n_items;
-	if (commonring->r_ptr == commonring->depth)
-		commonring->r_ptr = 0;
-
-	brcmf_dma_invalidate_cache(ret_addr, *n_ items * commonring->item_len);
-
-	return ret_addr;
+	return commonring->buf_addr +
+	       (commonring->r_ptr * commonring->item_len);
 }
 
 
-int brcmf_commonring_read_complete(struct brcmf_commonring *commonring)
+int brcmf_commonring_read_complete(struct brcmf_commonring *commonring,
+				   u16 n_items)
 {
+	commonring->r_ptr += n_items;
+	if (commonring->r_ptr == commonring->depth)
+		commonring->r_ptr = 0;
+
 	if (commonring->cr_write_rptr)
 		return commonring->cr_write_rptr(commonring->cr_ctx);
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/commonring.h b/drivers/net/wireless/brcm80211/brcmfmac/commonring.h
index 3d404016a92e..b85033611c8d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/commonring.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/commonring.h
@@ -62,7 +62,8 @@ void brcmf_commonring_write_cancel(struct brcmf_commonring *commonring,
 				   u16 n_items);
 void *brcmf_commonring_get_read_ptr(struct brcmf_commonring *commonring,
 				    u16 *n_items);
-int brcmf_commonring_read_complete(struct brcmf_commonring *commonring);
+int brcmf_commonring_read_complete(struct brcmf_commonring *commonring,
+				   u16 n_items);
 
 #define brcmf_commonring_n_items(commonring) (commonring->depth)
 #define brcmf_commonring_len_item(commonring) (commonring->item_len)
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/core.c b/drivers/net/wireless/brcm80211/brcmfmac/core.c
index f8f47dcfa886..fe9d3fbf5fe2 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/core.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/core.c
@@ -867,8 +867,6 @@ static void brcmf_del_if(struct brcmf_pub *drvr, s32 bssidx)
 		}
 		/* unregister will take care of freeing it */
 		unregister_netdev(ifp->ndev);
-	} else {
-		kfree(ifp);
 	}
 }
 
@@ -1100,6 +1098,8 @@ void brcmf_detach(struct device *dev)
 
 	/* stop firmware event handling */
 	brcmf_fweh_detach(drvr);
+	if (drvr->config)
+		brcmf_p2p_detach(&drvr->config->p2p);
 
 	brcmf_bus_change_state(bus_if, BRCMF_BUS_DOWN);
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/debug.c b/drivers/net/wireless/brcm80211/brcmfmac/debug.c
index 9b473d50b005..2d6d00553858 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/debug.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/debug.c
@@ -41,15 +41,6 @@ void brcmf_debugfs_exit(void)
 	root_folder = NULL;
 }
 
-static int brcmf_debugfs_chipinfo_read(struct seq_file *seq, void *data)
-{
-	struct brcmf_bus *bus = dev_get_drvdata(seq->private);
-
-	seq_printf(seq, "chip: %x(%u) rev %u\n",
-		   bus->chip, bus->chip, bus->chiprev);
-	return 0;
-}
-
 int brcmf_debugfs_attach(struct brcmf_pub *drvr)
 {
 	struct device *dev = drvr->bus_if->dev;
@@ -58,7 +49,6 @@ int brcmf_debugfs_attach(struct brcmf_pub *drvr)
 		return -ENODEV;
 
 	drvr->dbgfs_dir = debugfs_create_dir(dev_name(dev), root_folder);
-	brcmf_debugfs_add_entry(drvr, "chipinfo", brcmf_debugfs_chipinfo_read);
 
 	return PTR_ERR_OR_ZERO(drvr->dbgfs_dir);
 }
@@ -74,44 +64,12 @@ struct dentry *brcmf_debugfs_get_devdir(struct brcmf_pub *drvr)
 	return drvr->dbgfs_dir;
 }
 
-struct brcmf_debugfs_entry {
-	int (*read)(struct seq_file *seq, void *data);
-	struct brcmf_pub *drvr;
-};
-
-static int brcmf_debugfs_entry_open(struct inode *inode, struct file *f)
-{
-	struct brcmf_debugfs_entry *entry = inode->i_private;
-
-	return single_open(f, entry->read, entry->drvr->bus_if->dev);
-}
-
-static const struct file_operations brcmf_debugfs_def_ops = {
-	.owner = THIS_MODULE,
-	.open = brcmf_debugfs_entry_open,
-	.release = single_release,
-	.read = seq_read,
-	.llseek = seq_lseek
-};
-
 int brcmf_debugfs_add_entry(struct brcmf_pub *drvr, const char *fn,
 			    int (*read_fn)(struct seq_file *seq, void *data))
 {
-	struct dentry *dentry =  drvr->dbgfs_dir;
-	struct brcmf_debugfs_entry *entry;
-
-	if (IS_ERR_OR_NULL(dentry))
-		return -ENOENT;
-
-	entry = devm_kzalloc(drvr->bus_if->dev, sizeof(*entry), GFP_KERNEL);
-	if (!entry)
-		return -ENOMEM;
-
-	entry->read = read_fn;
-	entry->drvr = drvr;
-
-	dentry = debugfs_create_file(fn, S_IRUGO, dentry, entry,
-				     &brcmf_debugfs_def_ops);
+	struct dentry *e;
 
-	return PTR_ERR_OR_ZERO(dentry);
+	e = debugfs_create_devm_seqfile(drvr->bus_if->dev, fn,
+					drvr->dbgfs_dir, read_fn);
+	return PTR_ERR_OR_ZERO(e);
 }
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/feature.c b/drivers/net/wireless/brcm80211/brcmfmac/feature.c
index 7748a1ccf14f..1e94e94e01dc 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/feature.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/feature.c
@@ -124,10 +124,12 @@ void brcmf_feat_attach(struct brcmf_pub *drvr)
 	struct brcmf_if *ifp = drvr->iflist[0];
 
 	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_MCHAN, "mchan");
+	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_PNO, "pfn");
 	if (drvr->bus_if->wowl_supported)
 		brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_WOWL, "wowl");
 	if (drvr->bus_if->chip != BRCM_CC_43362_CHIP_ID)
 		brcmf_feat_iovar_int_set(ifp, BRCMF_FEAT_MBSS, "mbss", 0);
+	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_P2P, "p2p");
 
 	/* set chip related quirks */
 	switch (drvr->bus_if->chip) {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/feature.h b/drivers/net/wireless/brcm80211/brcmfmac/feature.h
index f5832e077bb7..6b381f799f22 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/feature.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/feature.h
@@ -19,12 +19,18 @@
 /*
  * Features:
  *
+ * MBSS: multiple BSSID support (eg. guest network in AP mode).
  * MCHAN: multi-channel for concurrent P2P.
+ * PNO: preferred network offload.
+ * WOWL: Wake-On-WLAN.
+ * P2P: peer-to-peer
  */
 #define BRCMF_FEAT_LIST \
 	BRCMF_FEAT_DEF(MBSS) \
 	BRCMF_FEAT_DEF(MCHAN) \
-	BRCMF_FEAT_DEF(WOWL)
+	BRCMF_FEAT_DEF(PNO) \
+	BRCMF_FEAT_DEF(WOWL) \
+	BRCMF_FEAT_DEF(P2P)
 /*
  * Quirks:
  *
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/firmware.c b/drivers/net/wireless/brcm80211/brcmfmac/firmware.c
index 9cb99152ad17..743f16b6a072 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/firmware.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/firmware.c
@@ -23,6 +23,10 @@
 #include "debug.h"
 #include "firmware.h"
 
+#define BRCMF_FW_MAX_NVRAM_SIZE			64000
+#define BRCMF_FW_NVRAM_DEVPATH_LEN		19	/* devpath0=pcie/1/4/ */
+#define BRCMF_FW_NVRAM_PCIEDEV_LEN		10	/* pcie/1/4/ + \0 */
+
 char brcmf_firmware_path[BRCMF_FW_PATH_LEN];
 module_param_string(firmware_path, brcmf_firmware_path,
 		    BRCMF_FW_PATH_LEN, 0440);
@@ -39,25 +43,35 @@ enum nvram_parser_state {
  * struct nvram_parser - internal info for parser.
  *
  * @state: current parser state.
- * @fwnv: input buffer being parsed.
+ * @data: input buffer being parsed.
  * @nvram: output buffer with parse result.
  * @nvram_len: lenght of parse result.
  * @line: current line.
  * @column: current column in line.
  * @pos: byte offset in input buffer.
  * @entry: start position of key,value entry.
+ * @multi_dev_v1: detect pcie multi device v1 (compressed).
+ * @multi_dev_v2: detect pcie multi device v2.
  */
 struct nvram_parser {
 	enum nvram_parser_state state;
-	const struct firmware *fwnv;
+	const u8 *data;
 	u8 *nvram;
 	u32 nvram_len;
 	u32 line;
 	u32 column;
 	u32 pos;
 	u32 entry;
+	bool multi_dev_v1;
+	bool multi_dev_v2;
 };
 
+/**
+ * is_nvram_char() - check if char is a valid one for NVRAM entry
+ *
+ * It accepts all printable ASCII chars except for '#' which opens a comment.
+ * Please note that ' ' (space) while accepted is not a valid key name char.
+ */
 static bool is_nvram_char(char c)
 {
 	/* comment marker excluded */
@@ -65,7 +79,7 @@ static bool is_nvram_char(char c)
 		return false;
 
 	/* key and value may have any other readable character */
-	return (c > 0x20 && c < 0x7f);
+	return (c >= 0x20 && c < 0x7f);
 }
 
 static bool is_whitespace(char c)
@@ -77,7 +91,7 @@ static enum nvram_parser_state brcmf_nvram_handle_idle(struct nvram_parser *nvp)
 {
 	char c;
 
-	c = nvp->fwnv->data[nvp->pos];
+	c = nvp->data[nvp->pos];
 	if (c == '\n')
 		return COMMENT;
 	if (is_whitespace(c))
@@ -101,14 +115,18 @@ static enum nvram_parser_state brcmf_nvram_handle_key(struct nvram_parser *nvp)
 	enum nvram_parser_state st = nvp->state;
 	char c;
 
-	c = nvp->fwnv->data[nvp->pos];
+	c = nvp->data[nvp->pos];
 	if (c == '=') {
 		/* ignore RAW1 by treating as comment */
-		if (strncmp(&nvp->fwnv->data[nvp->entry], "RAW1", 4) == 0)
+		if (strncmp(&nvp->data[nvp->entry], "RAW1", 4) == 0)
 			st = COMMENT;
 		else
 			st = VALUE;
-	} else if (!is_nvram_char(c)) {
+		if (strncmp(&nvp->data[nvp->entry], "devpath", 7) == 0)
+			nvp->multi_dev_v1 = true;
+		if (strncmp(&nvp->data[nvp->entry], "pcie/", 5) == 0)
+			nvp->multi_dev_v2 = true;
+	} else if (!is_nvram_char(c) || c == ' ') {
 		brcmf_dbg(INFO, "warning: ln=%d:col=%d: '=' expected, skip invalid key entry\n",
 			  nvp->line, nvp->column);
 		return COMMENT;
@@ -127,12 +145,14 @@ brcmf_nvram_handle_value(struct nvram_parser *nvp)
 	char *ekv;
 	u32 cplen;
 
-	c = nvp->fwnv->data[nvp->pos];
+	c = nvp->data[nvp->pos];
 	if (!is_nvram_char(c)) {
 		/* key,value pair complete */
-		ekv = (u8 *)&nvp->fwnv->data[nvp->pos];
-		skv = (u8 *)&nvp->fwnv->data[nvp->entry];
+		ekv = (u8 *)&nvp->data[nvp->pos];
+		skv = (u8 *)&nvp->data[nvp->entry];
 		cplen = ekv - skv;
+		if (nvp->nvram_len + cplen + 1 >= BRCMF_FW_MAX_NVRAM_SIZE)
+			return END;
 		/* copy to output buffer */
 		memcpy(&nvp->nvram[nvp->nvram_len], skv, cplen);
 		nvp->nvram_len += cplen;
@@ -148,17 +168,20 @@ brcmf_nvram_handle_value(struct nvram_parser *nvp)
 static enum nvram_parser_state
 brcmf_nvram_handle_comment(struct nvram_parser *nvp)
 {
-	char *eol, *sol;
-
-	sol = (char *)&nvp->fwnv->data[nvp->pos];
-	eol = strchr(sol, '\n');
-	if (eol == NULL)
-		return END;
+	char *eoc, *sol;
+
+	sol = (char *)&nvp->data[nvp->pos];
+	eoc = strchr(sol, '\n');
+	if (!eoc) {
+		eoc = strchr(sol, '\0');
+		if (!eoc)
+			return END;
+	}
 
 	/* eat all moving to next line */
 	nvp->line++;
 	nvp->column = 1;
-	nvp->pos += (eol - sol) + 1;
+	nvp->pos += (eoc - sol) + 1;
 	return IDLE;
 }
 
@@ -178,12 +201,20 @@ static enum nvram_parser_state
 };
 
 static int brcmf_init_nvram_parser(struct nvram_parser *nvp,
-				   const struct firmware *nv)
+				   const u8 *data, size_t data_len)
 {
+	size_t size;
+
 	memset(nvp, 0, sizeof(*nvp));
-	nvp->fwnv = nv;
+	nvp->data = data;
+	/* Limit size to MAX_NVRAM_SIZE, some files contain lot of comment */
+	if (data_len > BRCMF_FW_MAX_NVRAM_SIZE)
+		size = BRCMF_FW_MAX_NVRAM_SIZE;
+	else
+		size = data_len;
 	/* Alloc for extra 0 byte + roundup by 4 + length field */
-	nvp->nvram = kzalloc(nv->size + 1 + 3 + sizeof(u32), GFP_KERNEL);
+	size += 1 + 3 + sizeof(u32);
+	nvp->nvram = kzalloc(size, GFP_KERNEL);
 	if (!nvp->nvram)
 		return -ENOMEM;
 
@@ -192,26 +223,171 @@ static int brcmf_init_nvram_parser(struct nvram_parser *nvp,
 	return 0;
 }
 
+/* brcmf_fw_strip_multi_v1 :Some nvram files contain settings for multiple
+ * devices. Strip it down for one device, use domain_nr/bus_nr to determine
+ * which data is to be returned. v1 is the version where nvram is stored
+ * compressed and "devpath" maps to index for valid entries.
+ */
+static void brcmf_fw_strip_multi_v1(struct nvram_parser *nvp, u16 domain_nr,
+				    u16 bus_nr)
+{
+	/* Device path with a leading '=' key-value separator */
+	char pci_path[] = "=pci/?/?";
+	size_t pci_len;
+	char pcie_path[] = "=pcie/?/?";
+	size_t pcie_len;
+
+	u32 i, j;
+	bool found;
+	u8 *nvram;
+	u8 id;
+
+	nvram = kzalloc(nvp->nvram_len + 1 + 3 + sizeof(u32), GFP_KERNEL);
+	if (!nvram)
+		goto fail;
+
+	/* min length: devpath0=pcie/1/4/ + 0:x=y */
+	if (nvp->nvram_len < BRCMF_FW_NVRAM_DEVPATH_LEN + 6)
+		goto fail;
+
+	/* First search for the devpathX and see if it is the configuration
+	 * for domain_nr/bus_nr. Search complete nvp
+	 */
+	snprintf(pci_path, sizeof(pci_path), "=pci/%d/%d", domain_nr,
+		 bus_nr);
+	pci_len = strlen(pci_path);
+	snprintf(pcie_path, sizeof(pcie_path), "=pcie/%d/%d", domain_nr,
+		 bus_nr);
+	pcie_len = strlen(pcie_path);
+	found = false;
+	i = 0;
+	while (i < nvp->nvram_len - BRCMF_FW_NVRAM_DEVPATH_LEN) {
+		/* Format: devpathX=pcie/Y/Z/
+		 * Y = domain_nr, Z = bus_nr, X = virtual ID
+		 */
+		if (strncmp(&nvp->nvram[i], "devpath", 7) == 0 &&
+		    (!strncmp(&nvp->nvram[i + 8], pci_path, pci_len) ||
+		     !strncmp(&nvp->nvram[i + 8], pcie_path, pcie_len))) {
+			id = nvp->nvram[i + 7] - '0';
+			found = true;
+			break;
+		}
+		while (nvp->nvram[i] != 0)
+			i++;
+		i++;
+	}
+	if (!found)
+		goto fail;
+
+	/* Now copy all valid entries, release old nvram and assign new one */
+	i = 0;
+	j = 0;
+	while (i < nvp->nvram_len) {
+		if ((nvp->nvram[i] - '0' == id) && (nvp->nvram[i + 1] == ':')) {
+			i += 2;
+			while (nvp->nvram[i] != 0) {
+				nvram[j] = nvp->nvram[i];
+				i++;
+				j++;
+			}
+			nvram[j] = 0;
+			j++;
+		}
+		while (nvp->nvram[i] != 0)
+			i++;
+		i++;
+	}
+	kfree(nvp->nvram);
+	nvp->nvram = nvram;
+	nvp->nvram_len = j;
+	return;
+
+fail:
+	kfree(nvram);
+	nvp->nvram_len = 0;
+}
+
+/* brcmf_fw_strip_multi_v2 :Some nvram files contain settings for multiple
+ * devices. Strip it down for one device, use domain_nr/bus_nr to determine
+ * which data is to be returned. v2 is the version where nvram is stored
+ * uncompressed, all relevant valid entries are identified by
+ * pcie/domain_nr/bus_nr:
+ */
+static void brcmf_fw_strip_multi_v2(struct nvram_parser *nvp, u16 domain_nr,
+				    u16 bus_nr)
+{
+	char prefix[BRCMF_FW_NVRAM_PCIEDEV_LEN];
+	size_t len;
+	u32 i, j;
+	u8 *nvram;
+
+	nvram = kzalloc(nvp->nvram_len + 1 + 3 + sizeof(u32), GFP_KERNEL);
+	if (!nvram)
+		goto fail;
+
+	/* Copy all valid entries, release old nvram and assign new one.
+	 * Valid entries are of type pcie/X/Y/ where X = domain_nr and
+	 * Y = bus_nr.
+	 */
+	snprintf(prefix, sizeof(prefix), "pcie/%d/%d/", domain_nr, bus_nr);
+	len = strlen(prefix);
+	i = 0;
+	j = 0;
+	while (i < nvp->nvram_len - len) {
+		if (strncmp(&nvp->nvram[i], prefix, len) == 0) {
+			i += len;
+			while (nvp->nvram[i] != 0) {
+				nvram[j] = nvp->nvram[i];
+				i++;
+				j++;
+			}
+			nvram[j] = 0;
+			j++;
+		}
+		while (nvp->nvram[i] != 0)
+			i++;
+		i++;
+	}
+	kfree(nvp->nvram);
+	nvp->nvram = nvram;
+	nvp->nvram_len = j;
+	return;
+fail:
+	kfree(nvram);
+	nvp->nvram_len = 0;
+}
+
 /* brcmf_nvram_strip :Takes a buffer of "<var>=<value>\n" lines read from a fil
  * and ending in a NUL. Removes carriage returns, empty lines, comment lines,
  * and converts newlines to NULs. Shortens buffer as needed and pads with NULs.
  * End of buffer is completed with token identifying length of buffer.
  */
-static void *brcmf_fw_nvram_strip(const struct firmware *nv, u32 *new_length)
+static void *brcmf_fw_nvram_strip(const u8 *data, size_t data_len,
+				  u32 *new_length, u16 domain_nr, u16 bus_nr)
 {
 	struct nvram_parser nvp;
 	u32 pad;
 	u32 token;
 	__le32 token_le;
 
-	if (brcmf_init_nvram_parser(&nvp, nv) < 0)
+	if (brcmf_init_nvram_parser(&nvp, data, data_len) < 0)
 		return NULL;
 
-	while (nvp.pos < nv->size) {
+	while (nvp.pos < data_len) {
 		nvp.state = nv_parser_states[nvp.state](&nvp);
 		if (nvp.state == END)
 			break;
 	}
+	if (nvp.multi_dev_v1)
+		brcmf_fw_strip_multi_v1(&nvp, domain_nr, bus_nr);
+	else if (nvp.multi_dev_v2)
+		brcmf_fw_strip_multi_v2(&nvp, domain_nr, bus_nr);
+
+	if (nvp.nvram_len == 0) {
+		kfree(nvp.nvram);
+		return NULL;
+	}
+
 	pad = nvp.nvram_len;
 	*new_length = roundup(nvp.nvram_len + 1, 4);
 	while (pad != *new_length) {
@@ -239,6 +415,8 @@ struct brcmf_fw {
 	u16 flags;
 	const struct firmware *code;
 	const char *nvram_name;
+	u16 domain_nr;
+	u16 bus_nr;
 	void (*done)(struct device *dev, const struct firmware *fw,
 		     void *nvram_image, u32 nvram_len);
 };
@@ -254,7 +432,8 @@ static void brcmf_fw_request_nvram_done(const struct firmware *fw, void *ctx)
 		goto fail;
 
 	if (fw) {
-		nvram = brcmf_fw_nvram_strip(fw, &nvram_length);
+		nvram = brcmf_fw_nvram_strip(fw->data, fw->size, &nvram_length,
+					     fwctx->domain_nr, fwctx->bus_nr);
 		release_firmware(fw);
 		if (!nvram && !(fwctx->flags & BRCMF_FW_REQ_NV_OPTIONAL))
 			goto fail;
@@ -309,11 +488,12 @@ fail:
 	kfree(fwctx);
 }
 
-int brcmf_fw_get_firmwares(struct device *dev, u16 flags,
-			   const char *code, const char *nvram,
-			   void (*fw_cb)(struct device *dev,
-					 const struct firmware *fw,
-					 void *nvram_image, u32 nvram_len))
+int brcmf_fw_get_firmwares_pcie(struct device *dev, u16 flags,
+				const char *code, const char *nvram,
+				void (*fw_cb)(struct device *dev,
+					      const struct firmware *fw,
+					      void *nvram_image, u32 nvram_len),
+				u16 domain_nr, u16 bus_nr)
 {
 	struct brcmf_fw *fwctx;
 
@@ -333,8 +513,21 @@ int brcmf_fw_get_firmwares(struct device *dev, u16 flags,
 	fwctx->done = fw_cb;
 	if (flags & BRCMF_FW_REQUEST_NVRAM)
 		fwctx->nvram_name = nvram;
+	fwctx->domain_nr = domain_nr;
+	fwctx->bus_nr = bus_nr;
 
 	return request_firmware_nowait(THIS_MODULE, true, code, dev,
 				       GFP_KERNEL, fwctx,
 				       brcmf_fw_request_code_done);
 }
+
+int brcmf_fw_get_firmwares(struct device *dev, u16 flags,
+			   const char *code, const char *nvram,
+			   void (*fw_cb)(struct device *dev,
+					 const struct firmware *fw,
+					 void *nvram_image, u32 nvram_len))
+{
+	return brcmf_fw_get_firmwares_pcie(dev, flags, code, nvram, fw_cb, 0,
+					   0);
+}
+
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/firmware.h b/drivers/net/wireless/brcm80211/brcmfmac/firmware.h
index 4d3482356b77..604dd48ab4e0 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/firmware.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/firmware.h
@@ -32,6 +32,12 @@ void brcmf_fw_nvram_free(void *nvram);
  * fails it will not use the callback, but call device_release_driver()
  * instead which will call the driver .remove() callback.
  */
+int brcmf_fw_get_firmwares_pcie(struct device *dev, u16 flags,
+				const char *code, const char *nvram,
+				void (*fw_cb)(struct device *dev,
+					      const struct firmware *fw,
+					      void *nvram_image, u32 nvram_len),
+				u16 domain_nr, u16 bus_nr);
 int brcmf_fw_get_firmwares(struct device *dev, u16 flags,
 			   const char *code, const char *nvram,
 			   void (*fw_cb)(struct device *dev,
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c b/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
index eb1325371d3a..59440631fec5 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
@@ -249,8 +249,8 @@ void brcmf_flowring_delete(struct brcmf_flowring *flow, u8 flowid)
 }
 
 
-void brcmf_flowring_enqueue(struct brcmf_flowring *flow, u8 flowid,
-			    struct sk_buff *skb)
+u32 brcmf_flowring_enqueue(struct brcmf_flowring *flow, u8 flowid,
+			   struct sk_buff *skb)
 {
 	struct brcmf_flowring_ring *ring;
 
@@ -271,6 +271,7 @@ void brcmf_flowring_enqueue(struct brcmf_flowring *flow, u8 flowid,
 		if (skb_queue_len(&ring->skblist) < BRCMF_FLOWRING_LOW)
 			brcmf_flowring_block(flow, flowid, false);
 	}
+	return skb_queue_len(&ring->skblist);
 }
 
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/flowring.h b/drivers/net/wireless/brcm80211/brcmfmac/flowring.h
index a34cd394c616..5551861a44bc 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/flowring.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/flowring.h
@@ -64,8 +64,8 @@ u32 brcmf_flowring_create(struct brcmf_flowring *flow, u8 da[ETH_ALEN],
 void brcmf_flowring_delete(struct brcmf_flowring *flow, u8 flowid);
 void brcmf_flowring_open(struct brcmf_flowring *flow, u8 flowid);
 u8 brcmf_flowring_tid(struct brcmf_flowring *flow, u8 flowid);
-void brcmf_flowring_enqueue(struct brcmf_flowring *flow, u8 flowid,
-			    struct sk_buff *skb);
+u32 brcmf_flowring_enqueue(struct brcmf_flowring *flow, u8 flowid,
+			   struct sk_buff *skb);
 struct sk_buff *brcmf_flowring_dequeue(struct brcmf_flowring *flow, u8 flowid);
 void brcmf_flowring_reinsert(struct brcmf_flowring *flow, u8 flowid,
 			     struct sk_buff *skb);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h b/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
index 374920965108..297911f38fa0 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
@@ -32,7 +32,11 @@
 #define	BRCMF_BSS_INFO_VERSION	109 /* curr ver of brcmf_bss_info_le struct */
 #define BRCMF_BSS_RSSI_ON_CHANNEL	0x0002
 
-#define BRCMF_STA_ASSOC			0x10		/* Associated */
+#define BRCMF_STA_WME              0x00000002      /* WMM association */
+#define BRCMF_STA_AUTHE            0x00000008      /* Authenticated */
+#define BRCMF_STA_ASSOC            0x00000010      /* Associated */
+#define BRCMF_STA_AUTHO            0x00000020      /* Authorized */
+#define BRCMF_STA_SCBSTATS         0x00004000      /* Per STA debug stats */
 
 /* size of brcmf_scan_params not including variable length array */
 #define BRCMF_SCAN_PARAMS_FIXED_SIZE	64
@@ -113,6 +117,7 @@
 #define BRCMF_WOWL_MAXPATTERNSIZE	128
 
 #define BRCMF_COUNTRY_BUF_SZ		4
+#define BRCMF_ANT_MAX			4
 
 /* join preference types for join_pref iovar */
 enum brcmf_join_pref_types {
@@ -456,25 +461,61 @@ struct brcmf_channel_info_le {
 };
 
 struct brcmf_sta_info_le {
-	__le16	ver;		/* version of this struct */
-	__le16	len;		/* length in bytes of this structure */
-	__le16	cap;		/* sta's advertised capabilities */
-	__le32	flags;		/* flags defined below */
-	__le32	idle;		/* time since data pkt rx'd from sta */
-	u8	ea[ETH_ALEN];		/* Station address */
-	__le32	count;			/* # rates in this set */
-	u8	rates[BRCMF_MAXRATES_IN_SET];	/* rates in 500kbps units */
+	__le16 ver;		/* version of this struct */
+	__le16 len;		/* length in bytes of this structure */
+	__le16 cap;		/* sta's advertised capabilities */
+	__le32 flags;		/* flags defined below */
+	__le32 idle;		/* time since data pkt rx'd from sta */
+	u8 ea[ETH_ALEN];		/* Station address */
+	__le32 count;			/* # rates in this set */
+	u8 rates[BRCMF_MAXRATES_IN_SET];	/* rates in 500kbps units */
 						/* w/hi bit set if basic */
-	__le32	in;		/* seconds elapsed since associated */
-	__le32	listen_interval_inms; /* Min Listen interval in ms for STA */
-	__le32	tx_pkts;	/* # of packets transmitted */
-	__le32	tx_failures;	/* # of packets failed */
-	__le32	rx_ucast_pkts;	/* # of unicast packets received */
-	__le32	rx_mcast_pkts;	/* # of multicast packets received */
-	__le32	tx_rate;	/* Rate of last successful tx frame */
-	__le32	rx_rate;	/* Rate of last successful rx frame */
-	__le32	rx_decrypt_succeeds;	/* # of packet decrypted successfully */
-	__le32	rx_decrypt_failures;	/* # of packet decrypted failed */
+	__le32 in;		/* seconds elapsed since associated */
+	__le32 listen_interval_inms; /* Min Listen interval in ms for STA */
+	__le32 tx_pkts;	/* # of packets transmitted */
+	__le32 tx_failures;	/* # of packets failed */
+	__le32 rx_ucast_pkts;	/* # of unicast packets received */
+	__le32 rx_mcast_pkts;	/* # of multicast packets received */
+	__le32 tx_rate;	/* Rate of last successful tx frame */
+	__le32 rx_rate;	/* Rate of last successful rx frame */
+	__le32 rx_decrypt_succeeds;	/* # of packet decrypted successfully */
+	__le32 rx_decrypt_failures;	/* # of packet decrypted failed */
+	__le32 tx_tot_pkts;    /* # of tx pkts (ucast + mcast) */
+	__le32 rx_tot_pkts;    /* # of data packets recvd (uni + mcast) */
+	__le32 tx_mcast_pkts;  /* # of mcast pkts txed */
+	__le64 tx_tot_bytes;   /* data bytes txed (ucast + mcast) */
+	__le64 rx_tot_bytes;   /* data bytes recvd (ucast + mcast) */
+	__le64 tx_ucast_bytes; /* data bytes txed (ucast) */
+	__le64 tx_mcast_bytes; /* # data bytes txed (mcast) */
+	__le64 rx_ucast_bytes; /* data bytes recvd (ucast) */
+	__le64 rx_mcast_bytes; /* data bytes recvd (mcast) */
+	s8 rssi[BRCMF_ANT_MAX];   /* per antenna rssi */
+	s8 nf[BRCMF_ANT_MAX];     /* per antenna noise floor */
+	__le16 aid;                    /* association ID */
+	__le16 ht_capabilities;        /* advertised ht caps */
+	__le16 vht_flags;              /* converted vht flags */
+	__le32 tx_pkts_retry_cnt;      /* # of frames where a retry was
+					 * exhausted.
+					 */
+	__le32 tx_pkts_retry_exhausted; /* # of user frames where a retry
+					 * was exhausted
+					 */
+	s8 rx_lastpkt_rssi[BRCMF_ANT_MAX]; /* Per antenna RSSI of last
+					    * received data frame.
+					    */
+	/* TX WLAN retry/failure statistics:
+	 * Separated for host requested frames and locally generated frames.
+	 * Include unicast frame only where the retries/failures can be counted.
+	 */
+	__le32 tx_pkts_total;          /* # user frames sent successfully */
+	__le32 tx_pkts_retries;        /* # user frames retries */
+	__le32 tx_pkts_fw_total;       /* # FW generated sent successfully */
+	__le32 tx_pkts_fw_retries;     /* # retries for FW generated frames */
+	__le32 tx_pkts_fw_retry_exhausted;     /* # FW generated where a retry
+						* was exhausted
+						*/
+	__le32 rx_pkts_retried;        /* # rx with retry bit set */
+	__le32 tx_rate_fallback;       /* lowest fallback TX rate */
 };
 
 struct brcmf_chanspec_list {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
index f0dda0ecd23b..5017eaa4af45 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
@@ -635,7 +635,7 @@ static int brcmf_fws_hanger_pushpkt(struct brcmf_fws_hanger *h,
 	return 0;
 }
 
-static int brcmf_fws_hanger_poppkt(struct brcmf_fws_hanger *h,
+static inline int brcmf_fws_hanger_poppkt(struct brcmf_fws_hanger *h,
 					  u32 slot_id, struct sk_buff **pktout,
 					  bool remove_item)
 {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
index 4ec9811f49c8..898c3801e658 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
@@ -73,8 +73,10 @@
 #define BRCMF_MSGBUF_TX_FLUSH_CNT1		32
 #define BRCMF_MSGBUF_TX_FLUSH_CNT2		96
 
-#define BRCMF_MSGBUF_DELAY_TXWORKER_THRS	64
+#define BRCMF_MSGBUF_DELAY_TXWORKER_THRS	96
 #define BRCMF_MSGBUF_TRICKLE_TXWORKER_THRS	32
+#define BRCMF_MSGBUF_UPDATE_RX_PTR_THRS		48
+
 
 struct msgbuf_common_hdr {
 	u8				msgtype;
@@ -278,16 +280,6 @@ struct brcmf_msgbuf_pktids {
 	struct brcmf_msgbuf_pktid *array;
 };
 
-
-/* dma flushing needs implementation for mips and arm platforms. Should
- * be put in util. Note, this is not real flushing. It is virtual non
- * cached memory. Only write buffers should have to be drained. Though
- * this may be different depending on platform......
- */
-#define brcmf_dma_flush(addr, len)
-#define brcmf_dma_invalidate_cache(addr, len)
-
-
 static void brcmf_msgbuf_rxbuf_ioctlresp_post(struct brcmf_msgbuf *msgbuf);
 
 
@@ -462,7 +454,6 @@ static int brcmf_msgbuf_tx_ioctl(struct brcmf_pub *drvr, int ifidx,
 		memcpy(msgbuf->ioctbuf, buf, buf_len);
 	else
 		memset(msgbuf->ioctbuf, 0, buf_len);
-	brcmf_dma_flush(ioctl_buf, buf_len);
 
 	err = brcmf_commonring_write_complete(commonring);
 	brcmf_commonring_unlock(commonring);
@@ -511,11 +502,9 @@ static int brcmf_msgbuf_query_dcmd(struct brcmf_pub *drvr, int ifidx,
 				     msgbuf->rx_pktids,
 				     msgbuf->ioctl_resp_pktid);
 	if (msgbuf->ioctl_resp_ret_len != 0) {
-		if (!skb) {
-			brcmf_err("Invalid packet id idx recv'd %d\n",
-				  msgbuf->ioctl_resp_pktid);
+		if (!skb)
 			return -EBADF;
-		}
+
 		memcpy(buf, skb->data, (len < msgbuf->ioctl_resp_ret_len) ?
 				       len : msgbuf->ioctl_resp_ret_len);
 	}
@@ -797,6 +786,8 @@ static int brcmf_msgbuf_txdata(struct brcmf_pub *drvr, int ifidx,
 	struct brcmf_flowring *flow = msgbuf->flow;
 	struct ethhdr *eh = (struct ethhdr *)(skb->data);
 	u32 flowid;
+	u32 queue_count;
+	bool force;
 
 	flowid = brcmf_flowring_lookup(flow, eh->h_dest, skb->priority, ifidx);
 	if (flowid == BRCMF_FLOWRING_INVALID_ID) {
@@ -804,8 +795,9 @@ static int brcmf_msgbuf_txdata(struct brcmf_pub *drvr, int ifidx,
 		if (flowid == BRCMF_FLOWRING_INVALID_ID)
 			return -ENOMEM;
 	}
-	brcmf_flowring_enqueue(flow, flowid, skb);
-	brcmf_msgbuf_schedule_txdata(msgbuf, flowid, false);
+	queue_count = brcmf_flowring_enqueue(flow, flowid, skb);
+	force = ((queue_count % BRCMF_MSGBUF_TRICKLE_TXWORKER_THRS) == 0);
+	brcmf_msgbuf_schedule_txdata(msgbuf, flowid, force);
 
 	return 0;
 }
@@ -874,10 +866,8 @@ brcmf_msgbuf_process_txstatus(struct brcmf_msgbuf *msgbuf, void *buf)
 	flowid -= BRCMF_NROF_H2D_COMMON_MSGRINGS;
 	skb = brcmf_msgbuf_get_pktid(msgbuf->drvr->bus_if->dev,
 				     msgbuf->tx_pktids, idx);
-	if (!skb) {
-		brcmf_err("Invalid packet id idx recv'd %d\n", idx);
+	if (!skb)
 		return;
-	}
 
 	set_bit(flowid, msgbuf->txstatus_done_map);
 	commonring = msgbuf->flowrings[flowid];
@@ -1156,6 +1146,8 @@ brcmf_msgbuf_process_rx_complete(struct brcmf_msgbuf *msgbuf, void *buf)
 
 	skb = brcmf_msgbuf_get_pktid(msgbuf->drvr->bus_if->dev,
 				     msgbuf->rx_pktids, idx);
+	if (!skb)
+		return;
 
 	if (data_offset)
 		skb_pull(skb, data_offset);
@@ -1267,19 +1259,27 @@ static void brcmf_msgbuf_process_rx(struct brcmf_msgbuf *msgbuf,
 {
 	void *buf;
 	u16 count;
+	u16 processed;
 
 again:
 	buf = brcmf_commonring_get_read_ptr(commonring, &count);
 	if (buf == NULL)
 		return;
 
+	processed = 0;
 	while (count) {
 		brcmf_msgbuf_process_msgtype(msgbuf,
 					     buf + msgbuf->rx_dataoffset);
 		buf += brcmf_commonring_len_item(commonring);
+		processed++;
+		if (processed == BRCMF_MSGBUF_UPDATE_RX_PTR_THRS) {
+			brcmf_commonring_read_complete(commonring, processed);
+			processed = 0;
+		}
 		count--;
 	}
-	brcmf_commonring_read_complete(commonring);
+	if (processed)
+		brcmf_commonring_read_complete(commonring, processed);
 
 	if (commonring->r_ptr == 0)
 		goto again;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/of.c b/drivers/net/wireless/brcm80211/brcmfmac/of.c
index c824570ddea3..03f35e0c52ca 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/of.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/of.c
@@ -39,10 +39,16 @@ void brcmf_of_probe(struct brcmf_sdio_dev *sdiodev)
 	if (!sdiodev->pdata)
 		return;
 
+	if (of_property_read_u32(np, "brcm,drive-strength", &val) == 0)
+		sdiodev->pdata->drive_strength = val;
+
+	/* make sure there are interrupts defined in the node */
+	if (!of_find_property(np, "interrupts", NULL))
+		return;
+
 	irq = irq_of_parse_and_map(np, 0);
 	if (!irq) {
 		brcmf_err("interrupt could not be mapped\n");
-		devm_kfree(dev, sdiodev->pdata);
 		return;
 	}
 	irqf = irqd_get_trigger_type(irq_get_irq_data(irq));
@@ -50,7 +56,4 @@ void brcmf_of_probe(struct brcmf_sdio_dev *sdiodev)
 	sdiodev->pdata->oob_irq_supported = true;
 	sdiodev->pdata->oob_irq_nr = irq;
 	sdiodev->pdata->oob_irq_flags = irqf;
-
-	if (of_property_read_u32(np, "brcm,drive-strength", &val) == 0)
-		sdiodev->pdata->drive_strength = val;
 }
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c b/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
index 710fbe570eb2..a9ba775a24c1 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
@@ -16,6 +16,7 @@
 #include <linux/slab.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
+#include <linux/rtnetlink.h>
 #include <net/cfg80211.h>
 
 #include <brcmu_wifi.h>
@@ -1908,105 +1909,6 @@ s32 brcmf_p2p_notify_rx_mgmt_p2p_probereq(struct brcmf_if *ifp,
 
 
 /**
- * brcmf_p2p_attach() - attach for P2P.
- *
- * @cfg: driver private data for cfg80211 interface.
- */
-s32 brcmf_p2p_attach(struct brcmf_cfg80211_info *cfg)
-{
-	struct brcmf_if *pri_ifp;
-	struct brcmf_if *p2p_ifp;
-	struct brcmf_cfg80211_vif *p2p_vif;
-	struct brcmf_p2p_info *p2p;
-	struct brcmf_pub *drvr;
-	s32 bssidx;
-	s32 err = 0;
-
-	p2p = &cfg->p2p;
-	p2p->cfg = cfg;
-
-	drvr = cfg->pub;
-
-	pri_ifp = drvr->iflist[0];
-	p2p_ifp = drvr->iflist[1];
-
-	p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif = pri_ifp->vif;
-
-	if (p2p_ifp) {
-		p2p_vif = brcmf_alloc_vif(cfg, NL80211_IFTYPE_P2P_DEVICE,
-					  false);
-		if (IS_ERR(p2p_vif)) {
-			brcmf_err("could not create discovery vif\n");
-			err = -ENOMEM;
-			goto exit;
-		}
-
-		p2p_vif->ifp = p2p_ifp;
-		p2p_ifp->vif = p2p_vif;
-		p2p_vif->wdev.netdev = p2p_ifp->ndev;
-		p2p_ifp->ndev->ieee80211_ptr = &p2p_vif->wdev;
-		SET_NETDEV_DEV(p2p_ifp->ndev, wiphy_dev(cfg->wiphy));
-
-		p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif = p2p_vif;
-
-		brcmf_p2p_generate_bss_mac(p2p, NULL);
-		memcpy(p2p_ifp->mac_addr, p2p->dev_addr, ETH_ALEN);
-		brcmf_p2p_set_firmware(pri_ifp, p2p->dev_addr);
-
-		/* Initialize P2P Discovery in the firmware */
-		err = brcmf_fil_iovar_int_set(pri_ifp, "p2p_disc", 1);
-		if (err < 0) {
-			brcmf_err("set p2p_disc error\n");
-			brcmf_free_vif(p2p_vif);
-			goto exit;
-		}
-		/* obtain bsscfg index for P2P discovery */
-		err = brcmf_fil_iovar_int_get(pri_ifp, "p2p_dev", &bssidx);
-		if (err < 0) {
-			brcmf_err("retrieving discover bsscfg index failed\n");
-			brcmf_free_vif(p2p_vif);
-			goto exit;
-		}
-		/* Verify that firmware uses same bssidx as driver !! */
-		if (p2p_ifp->bssidx != bssidx) {
-			brcmf_err("Incorrect bssidx=%d, compared to p2p_ifp->bssidx=%d\n",
-				  bssidx, p2p_ifp->bssidx);
-			brcmf_free_vif(p2p_vif);
-			goto exit;
-		}
-
-		init_completion(&p2p->send_af_done);
-		INIT_WORK(&p2p->afx_hdl.afx_work, brcmf_p2p_afx_handler);
-		init_completion(&p2p->afx_hdl.act_frm_scan);
-		init_completion(&p2p->wait_next_af);
-	}
-exit:
-	return err;
-}
-
-
-/**
- * brcmf_p2p_detach() - detach P2P.
- *
- * @p2p: P2P specific data.
- */
-void brcmf_p2p_detach(struct brcmf_p2p_info *p2p)
-{
-	struct brcmf_cfg80211_vif *vif;
-
-	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
-	if (vif != NULL) {
-		brcmf_p2p_cancel_remain_on_channel(vif->ifp);
-		brcmf_p2p_deinit_discovery(p2p);
-		/* remove discovery interface */
-		brcmf_free_vif(vif);
-		p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif = NULL;
-	}
-	/* just set it all to zero */
-	memset(p2p, 0, sizeof(*p2p));
-}
-
-/**
  * brcmf_p2p_get_current_chanspec() - Get current operation channel.
  *
  * @p2p: P2P specific data.
@@ -2238,6 +2140,7 @@ static void brcmf_p2p_delete_p2pdev(struct brcmf_p2p_info *p2p,
 {
 	cfg80211_unregister_wdev(&vif->wdev);
 	p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif = NULL;
+	brcmf_remove_interface(vif->ifp->drvr, vif->ifp->bssidx);
 	brcmf_free_vif(vif);
 }
 
@@ -2364,6 +2267,8 @@ int brcmf_p2p_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev)
 		break;
 
 	case NL80211_IFTYPE_P2P_DEVICE:
+		brcmf_p2p_cancel_remain_on_channel(vif->ifp);
+		brcmf_p2p_deinit_discovery(p2p);
 		brcmf_p2p_delete_p2pdev(p2p, vif);
 		return 0;
 	default:
@@ -2425,3 +2330,103 @@ void brcmf_p2p_stop_device(struct wiphy *wiphy, struct wireless_dev *wdev)
 	clear_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state);
 	mutex_unlock(&cfg->usr_sync);
 }
+
+/**
+ * brcmf_p2p_attach() - attach for P2P.
+ *
+ * @cfg: driver private data for cfg80211 interface.
+ */
+s32 brcmf_p2p_attach(struct brcmf_cfg80211_info *cfg)
+{
+	struct brcmf_if *pri_ifp;
+	struct brcmf_if *p2p_ifp;
+	struct brcmf_cfg80211_vif *p2p_vif;
+	struct brcmf_p2p_info *p2p;
+	struct brcmf_pub *drvr;
+	s32 bssidx;
+	s32 err = 0;
+
+	p2p = &cfg->p2p;
+	p2p->cfg = cfg;
+
+	drvr = cfg->pub;
+
+	pri_ifp = drvr->iflist[0];
+	p2p_ifp = drvr->iflist[1];
+
+	p2p->bss_idx[P2PAPI_BSSCFG_PRIMARY].vif = pri_ifp->vif;
+
+	if (p2p_ifp) {
+		p2p_vif = brcmf_alloc_vif(cfg, NL80211_IFTYPE_P2P_DEVICE,
+					  false);
+		if (IS_ERR(p2p_vif)) {
+			brcmf_err("could not create discovery vif\n");
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		p2p_vif->ifp = p2p_ifp;
+		p2p_ifp->vif = p2p_vif;
+		p2p_vif->wdev.netdev = p2p_ifp->ndev;
+		p2p_ifp->ndev->ieee80211_ptr = &p2p_vif->wdev;
+		SET_NETDEV_DEV(p2p_ifp->ndev, wiphy_dev(cfg->wiphy));
+
+		p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif = p2p_vif;
+
+		brcmf_p2p_generate_bss_mac(p2p, NULL);
+		memcpy(p2p_ifp->mac_addr, p2p->dev_addr, ETH_ALEN);
+		brcmf_p2p_set_firmware(pri_ifp, p2p->dev_addr);
+
+		/* Initialize P2P Discovery in the firmware */
+		err = brcmf_fil_iovar_int_set(pri_ifp, "p2p_disc", 1);
+		if (err < 0) {
+			brcmf_err("set p2p_disc error\n");
+			brcmf_free_vif(p2p_vif);
+			goto exit;
+		}
+		/* obtain bsscfg index for P2P discovery */
+		err = brcmf_fil_iovar_int_get(pri_ifp, "p2p_dev", &bssidx);
+		if (err < 0) {
+			brcmf_err("retrieving discover bsscfg index failed\n");
+			brcmf_free_vif(p2p_vif);
+			goto exit;
+		}
+		/* Verify that firmware uses same bssidx as driver !! */
+		if (p2p_ifp->bssidx != bssidx) {
+			brcmf_err("Incorrect bssidx=%d, compared to p2p_ifp->bssidx=%d\n",
+				  bssidx, p2p_ifp->bssidx);
+			brcmf_free_vif(p2p_vif);
+			goto exit;
+		}
+
+		init_completion(&p2p->send_af_done);
+		INIT_WORK(&p2p->afx_hdl.afx_work, brcmf_p2p_afx_handler);
+		init_completion(&p2p->afx_hdl.act_frm_scan);
+		init_completion(&p2p->wait_next_af);
+	}
+exit:
+	return err;
+}
+
+/**
+ * brcmf_p2p_detach() - detach P2P.
+ *
+ * @p2p: P2P specific data.
+ */
+void brcmf_p2p_detach(struct brcmf_p2p_info *p2p)
+{
+	struct brcmf_cfg80211_vif *vif;
+
+	vif = p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif;
+	if (vif != NULL) {
+		brcmf_p2p_cancel_remain_on_channel(vif->ifp);
+		brcmf_p2p_deinit_discovery(p2p);
+		/* remove discovery interface */
+		rtnl_lock();
+		brcmf_p2p_delete_p2pdev(p2p, vif);
+		rtnl_unlock();
+	}
+	/* just set it all to zero */
+	memset(p2p, 0, sizeof(*p2p));
+}
+
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c b/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
index 1831ecd0813e..3a98c4306d1d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
@@ -51,6 +51,8 @@ enum brcmf_pcie_state {
 #define BRCMF_PCIE_4356_NVRAM_NAME		"brcm/brcmfmac4356-pcie.txt"
 #define BRCMF_PCIE_43570_FW_NAME		"brcm/brcmfmac43570-pcie.bin"
 #define BRCMF_PCIE_43570_NVRAM_NAME		"brcm/brcmfmac43570-pcie.txt"
+#define BRCMF_PCIE_4358_FW_NAME			"brcm/brcmfmac4358-pcie.bin"
+#define BRCMF_PCIE_4358_NVRAM_NAME		"brcm/brcmfmac4358-pcie.txt"
 
 #define BRCMF_PCIE_FW_UP_TIMEOUT		2000 /* msec */
 
@@ -110,10 +112,11 @@ enum brcmf_pcie_state {
 						 BRCMF_PCIE_MB_INT_D2H3_DB0 | \
 						 BRCMF_PCIE_MB_INT_D2H3_DB1)
 
-#define BRCMF_PCIE_MIN_SHARED_VERSION		4
+#define BRCMF_PCIE_MIN_SHARED_VERSION		5
 #define BRCMF_PCIE_MAX_SHARED_VERSION		5
 #define BRCMF_PCIE_SHARED_VERSION_MASK		0x00FF
-#define BRCMF_PCIE_SHARED_TXPUSH_SUPPORT	0x4000
+#define BRCMF_PCIE_SHARED_DMA_INDEX		0x10000
+#define BRCMF_PCIE_SHARED_DMA_2B_IDX		0x100000
 
 #define BRCMF_PCIE_FLAGS_HTOD_SPLIT		0x4000
 #define BRCMF_PCIE_FLAGS_DTOH_SPLIT		0x8000
@@ -145,6 +148,10 @@ enum brcmf_pcie_state {
 #define BRCMF_SHARED_RING_H2D_R_IDX_PTR_OFFSET	8
 #define BRCMF_SHARED_RING_D2H_W_IDX_PTR_OFFSET	12
 #define BRCMF_SHARED_RING_D2H_R_IDX_PTR_OFFSET	16
+#define BRCMF_SHARED_RING_H2D_WP_HADDR_OFFSET	20
+#define BRCMF_SHARED_RING_H2D_RP_HADDR_OFFSET	28
+#define BRCMF_SHARED_RING_D2H_WP_HADDR_OFFSET	36
+#define BRCMF_SHARED_RING_D2H_RP_HADDR_OFFSET	44
 #define BRCMF_SHARED_RING_TCM_MEMLOC_OFFSET	0
 #define BRCMF_SHARED_RING_MAX_SUB_QUEUES	52
 
@@ -189,6 +196,8 @@ MODULE_FIRMWARE(BRCMF_PCIE_4356_FW_NAME);
 MODULE_FIRMWARE(BRCMF_PCIE_4356_NVRAM_NAME);
 MODULE_FIRMWARE(BRCMF_PCIE_43570_FW_NAME);
 MODULE_FIRMWARE(BRCMF_PCIE_43570_NVRAM_NAME);
+MODULE_FIRMWARE(BRCMF_PCIE_4358_FW_NAME);
+MODULE_FIRMWARE(BRCMF_PCIE_4358_NVRAM_NAME);
 
 
 struct brcmf_pcie_console {
@@ -244,6 +253,13 @@ struct brcmf_pciedev_info {
 	bool mbdata_completed;
 	bool irq_allocated;
 	bool wowl_enabled;
+	u8 dma_idx_sz;
+	void *idxbuf;
+	u32 idxbuf_sz;
+	dma_addr_t idxbuf_dmahandle;
+	u16 (*read_ptr)(struct brcmf_pciedev_info *devinfo, u32 mem_offset);
+	void (*write_ptr)(struct brcmf_pciedev_info *devinfo, u32 mem_offset,
+			  u16 value);
 };
 
 struct brcmf_pcie_ringbuf {
@@ -273,15 +289,6 @@ static const u32 brcmf_ring_itemsize[BRCMF_NROF_COMMON_MSGRINGS] = {
 };
 
 
-/* dma flushing needs implementation for mips and arm platforms. Should
- * be put in util. Note, this is not real flushing. It is virtual non
- * cached memory. Only write buffers should have to be drained. Though
- * this may be different depending on platform......
- */
-#define brcmf_dma_flush(addr, len)
-#define brcmf_dma_invalidate_cache(addr, len)
-
-
 static u32
 brcmf_pcie_read_reg32(struct brcmf_pciedev_info *devinfo, u32 reg_offset)
 {
@@ -329,6 +336,25 @@ brcmf_pcie_write_tcm16(struct brcmf_pciedev_info *devinfo, u32 mem_offset,
 }
 
 
+static u16
+brcmf_pcie_read_idx(struct brcmf_pciedev_info *devinfo, u32 mem_offset)
+{
+	u16 *address = devinfo->idxbuf + mem_offset;
+
+	return (*(address));
+}
+
+
+static void
+brcmf_pcie_write_idx(struct brcmf_pciedev_info *devinfo, u32 mem_offset,
+		     u16 value)
+{
+	u16 *address = devinfo->idxbuf + mem_offset;
+
+	*(address) = value;
+}
+
+
 static u32
 brcmf_pcie_read_tcm32(struct brcmf_pciedev_info *devinfo, u32 mem_offset)
 {
@@ -874,7 +900,7 @@ static int brcmf_pcie_ring_mb_write_rptr(void *ctx)
 	brcmf_dbg(PCIE, "W r_ptr %d (%d), ring %d\n", commonring->r_ptr,
 		  commonring->w_ptr, ring->id);
 
-	brcmf_pcie_write_tcm16(devinfo, ring->r_idx_addr, commonring->r_ptr);
+	devinfo->write_ptr(devinfo, ring->r_idx_addr, commonring->r_ptr);
 
 	return 0;
 }
@@ -892,7 +918,7 @@ static int brcmf_pcie_ring_mb_write_wptr(void *ctx)
 	brcmf_dbg(PCIE, "W w_ptr %d (%d), ring %d\n", commonring->w_ptr,
 		  commonring->r_ptr, ring->id);
 
-	brcmf_pcie_write_tcm16(devinfo, ring->w_idx_addr, commonring->w_ptr);
+	devinfo->write_ptr(devinfo, ring->w_idx_addr, commonring->w_ptr);
 
 	return 0;
 }
@@ -921,7 +947,7 @@ static int brcmf_pcie_ring_mb_update_rptr(void *ctx)
 	if (devinfo->state != BRCMFMAC_PCIE_STATE_UP)
 		return -EIO;
 
-	commonring->r_ptr = brcmf_pcie_read_tcm16(devinfo, ring->r_idx_addr);
+	commonring->r_ptr = devinfo->read_ptr(devinfo, ring->r_idx_addr);
 
 	brcmf_dbg(PCIE, "R r_ptr %d (%d), ring %d\n", commonring->r_ptr,
 		  commonring->w_ptr, ring->id);
@@ -939,7 +965,7 @@ static int brcmf_pcie_ring_mb_update_wptr(void *ctx)
 	if (devinfo->state != BRCMFMAC_PCIE_STATE_UP)
 		return -EIO;
 
-	commonring->w_ptr = brcmf_pcie_read_tcm16(devinfo, ring->w_idx_addr);
+	commonring->w_ptr = devinfo->read_ptr(devinfo, ring->w_idx_addr);
 
 	brcmf_dbg(PCIE, "R w_ptr %d (%d), ring %d\n", commonring->w_ptr,
 		  commonring->r_ptr, ring->id);
@@ -1044,6 +1070,13 @@ static void brcmf_pcie_release_ringbuffers(struct brcmf_pciedev_info *devinfo)
 	}
 	kfree(devinfo->shared.flowrings);
 	devinfo->shared.flowrings = NULL;
+	if (devinfo->idxbuf) {
+		dma_free_coherent(&devinfo->pdev->dev,
+				  devinfo->idxbuf_sz,
+				  devinfo->idxbuf,
+				  devinfo->idxbuf_dmahandle);
+		devinfo->idxbuf = NULL;
+	}
 }
 
 
@@ -1059,19 +1092,72 @@ static int brcmf_pcie_init_ringbuffers(struct brcmf_pciedev_info *devinfo)
 	u32 addr;
 	u32 ring_mem_ptr;
 	u32 i;
+	u64 address;
+	u32 bufsz;
 	u16 max_sub_queues;
+	u8 idx_offset;
 
 	ring_addr = devinfo->shared.ring_info_addr;
 	brcmf_dbg(PCIE, "Base ring addr = 0x%08x\n", ring_addr);
+	addr = ring_addr + BRCMF_SHARED_RING_MAX_SUB_QUEUES;
+	max_sub_queues = brcmf_pcie_read_tcm16(devinfo, addr);
 
-	addr = ring_addr + BRCMF_SHARED_RING_D2H_W_IDX_PTR_OFFSET;
-	d2h_w_idx_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
-	addr = ring_addr + BRCMF_SHARED_RING_D2H_R_IDX_PTR_OFFSET;
-	d2h_r_idx_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
-	addr = ring_addr + BRCMF_SHARED_RING_H2D_W_IDX_PTR_OFFSET;
-	h2d_w_idx_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
-	addr = ring_addr + BRCMF_SHARED_RING_H2D_R_IDX_PTR_OFFSET;
-	h2d_r_idx_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
+	if (devinfo->dma_idx_sz != 0) {
+		bufsz = (BRCMF_NROF_D2H_COMMON_MSGRINGS + max_sub_queues) *
+			devinfo->dma_idx_sz * 2;
+		devinfo->idxbuf = dma_alloc_coherent(&devinfo->pdev->dev, bufsz,
+						     &devinfo->idxbuf_dmahandle,
+						     GFP_KERNEL);
+		if (!devinfo->idxbuf)
+			devinfo->dma_idx_sz = 0;
+	}
+
+	if (devinfo->dma_idx_sz == 0) {
+		addr = ring_addr + BRCMF_SHARED_RING_D2H_W_IDX_PTR_OFFSET;
+		d2h_w_idx_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
+		addr = ring_addr + BRCMF_SHARED_RING_D2H_R_IDX_PTR_OFFSET;
+		d2h_r_idx_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
+		addr = ring_addr + BRCMF_SHARED_RING_H2D_W_IDX_PTR_OFFSET;
+		h2d_w_idx_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
+		addr = ring_addr + BRCMF_SHARED_RING_H2D_R_IDX_PTR_OFFSET;
+		h2d_r_idx_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
+		idx_offset = sizeof(u32);
+		devinfo->write_ptr = brcmf_pcie_write_tcm16;
+		devinfo->read_ptr = brcmf_pcie_read_tcm16;
+		brcmf_dbg(PCIE, "Using TCM indices\n");
+	} else {
+		memset(devinfo->idxbuf, 0, bufsz);
+		devinfo->idxbuf_sz = bufsz;
+		idx_offset = devinfo->dma_idx_sz;
+		devinfo->write_ptr = brcmf_pcie_write_idx;
+		devinfo->read_ptr = brcmf_pcie_read_idx;
+
+		h2d_w_idx_ptr = 0;
+		addr = ring_addr + BRCMF_SHARED_RING_H2D_WP_HADDR_OFFSET;
+		address = (u64)devinfo->idxbuf_dmahandle;
+		brcmf_pcie_write_tcm32(devinfo, addr, address & 0xffffffff);
+		brcmf_pcie_write_tcm32(devinfo, addr + 4, address >> 32);
+
+		h2d_r_idx_ptr = h2d_w_idx_ptr + max_sub_queues * idx_offset;
+		addr = ring_addr + BRCMF_SHARED_RING_H2D_RP_HADDR_OFFSET;
+		address += max_sub_queues * idx_offset;
+		brcmf_pcie_write_tcm32(devinfo, addr, address & 0xffffffff);
+		brcmf_pcie_write_tcm32(devinfo, addr + 4, address >> 32);
+
+		d2h_w_idx_ptr = h2d_r_idx_ptr + max_sub_queues * idx_offset;
+		addr = ring_addr + BRCMF_SHARED_RING_D2H_WP_HADDR_OFFSET;
+		address += max_sub_queues * idx_offset;
+		brcmf_pcie_write_tcm32(devinfo, addr, address & 0xffffffff);
+		brcmf_pcie_write_tcm32(devinfo, addr + 4, address >> 32);
+
+		d2h_r_idx_ptr = d2h_w_idx_ptr +
+				BRCMF_NROF_D2H_COMMON_MSGRINGS * idx_offset;
+		addr = ring_addr + BRCMF_SHARED_RING_D2H_RP_HADDR_OFFSET;
+		address += BRCMF_NROF_D2H_COMMON_MSGRINGS * idx_offset;
+		brcmf_pcie_write_tcm32(devinfo, addr, address & 0xffffffff);
+		brcmf_pcie_write_tcm32(devinfo, addr + 4, address >> 32);
+		brcmf_dbg(PCIE, "Using host memory indices\n");
+	}
 
 	addr = ring_addr + BRCMF_SHARED_RING_TCM_MEMLOC_OFFSET;
 	ring_mem_ptr = brcmf_pcie_read_tcm32(devinfo, addr);
@@ -1085,8 +1171,8 @@ static int brcmf_pcie_init_ringbuffers(struct brcmf_pciedev_info *devinfo)
 		ring->id = i;
 		devinfo->shared.commonrings[i] = ring;
 
-		h2d_w_idx_ptr += sizeof(u32);
-		h2d_r_idx_ptr += sizeof(u32);
+		h2d_w_idx_ptr += idx_offset;
+		h2d_r_idx_ptr += idx_offset;
 		ring_mem_ptr += BRCMF_RING_MEM_SZ;
 	}
 
@@ -1100,13 +1186,11 @@ static int brcmf_pcie_init_ringbuffers(struct brcmf_pciedev_info *devinfo)
 		ring->id = i;
 		devinfo->shared.commonrings[i] = ring;
 
-		d2h_w_idx_ptr += sizeof(u32);
-		d2h_r_idx_ptr += sizeof(u32);
+		d2h_w_idx_ptr += idx_offset;
+		d2h_r_idx_ptr += idx_offset;
 		ring_mem_ptr += BRCMF_RING_MEM_SZ;
 	}
 
-	addr = ring_addr + BRCMF_SHARED_RING_MAX_SUB_QUEUES;
-	max_sub_queues = brcmf_pcie_read_tcm16(devinfo, addr);
 	devinfo->shared.nrof_flowrings =
 			max_sub_queues - BRCMF_NROF_H2D_COMMON_MSGRINGS;
 	rings = kcalloc(devinfo->shared.nrof_flowrings, sizeof(*ring),
@@ -1130,15 +1214,15 @@ static int brcmf_pcie_init_ringbuffers(struct brcmf_pciedev_info *devinfo)
 					     ring);
 		ring->w_idx_addr = h2d_w_idx_ptr;
 		ring->r_idx_addr = h2d_r_idx_ptr;
-		h2d_w_idx_ptr += sizeof(u32);
-		h2d_r_idx_ptr += sizeof(u32);
+		h2d_w_idx_ptr += idx_offset;
+		h2d_r_idx_ptr += idx_offset;
 	}
 	devinfo->shared.flowrings = rings;
 
 	return 0;
 
 fail:
-	brcmf_err("Allocating commonring buffers failed\n");
+	brcmf_err("Allocating ring buffers failed\n");
 	brcmf_pcie_release_ringbuffers(devinfo);
 	return -ENOMEM;
 }
@@ -1171,7 +1255,6 @@ static int brcmf_pcie_init_scratchbuffers(struct brcmf_pciedev_info *devinfo)
 		goto fail;
 
 	memset(devinfo->shared.scratch, 0, BRCMF_DMA_D2H_SCRATCH_BUF_LEN);
-	brcmf_dma_flush(devinfo->shared.scratch, BRCMF_DMA_D2H_SCRATCH_BUF_LEN);
 
 	addr = devinfo->shared.tcm_base_address +
 	       BRCMF_SHARED_DMA_SCRATCH_ADDR_OFFSET;
@@ -1189,7 +1272,6 @@ static int brcmf_pcie_init_scratchbuffers(struct brcmf_pciedev_info *devinfo)
 		goto fail;
 
 	memset(devinfo->shared.ringupd, 0, BRCMF_DMA_D2H_RINGUPD_BUF_LEN);
-	brcmf_dma_flush(devinfo->shared.ringupd, BRCMF_DMA_D2H_RINGUPD_BUF_LEN);
 
 	addr = devinfo->shared.tcm_base_address +
 	       BRCMF_SHARED_DMA_RINGUPD_ADDR_OFFSET;
@@ -1276,10 +1358,13 @@ brcmf_pcie_init_share_ram_info(struct brcmf_pciedev_info *devinfo,
 		brcmf_err("Unsupported PCIE version %d\n", version);
 		return -EINVAL;
 	}
-	if (shared->flags & BRCMF_PCIE_SHARED_TXPUSH_SUPPORT) {
-		brcmf_err("Unsupported legacy TX mode 0x%x\n",
-			  shared->flags & BRCMF_PCIE_SHARED_TXPUSH_SUPPORT);
-		return -EINVAL;
+
+	/* check firmware support dma indicies */
+	if (shared->flags & BRCMF_PCIE_SHARED_DMA_INDEX) {
+		if (shared->flags & BRCMF_PCIE_SHARED_DMA_2B_IDX)
+			devinfo->dma_idx_sz = sizeof(u16);
+		else
+			devinfo->dma_idx_sz = sizeof(u32);
 	}
 
 	addr = sharedram_addr + BRCMF_SHARED_MAX_RXBUFPOST_OFFSET;
@@ -1333,6 +1418,10 @@ static int brcmf_pcie_get_fwnames(struct brcmf_pciedev_info *devinfo)
 		fw_name = BRCMF_PCIE_43570_FW_NAME;
 		nvram_name = BRCMF_PCIE_43570_NVRAM_NAME;
 		break;
+	case BRCM_CC_4358_CHIP_ID:
+		fw_name = BRCMF_PCIE_4358_FW_NAME;
+		nvram_name = BRCMF_PCIE_4358_NVRAM_NAME;
+		break;
 	default:
 		brcmf_err("Unsupported chip 0x%04x\n", devinfo->ci->chip);
 		return -ENODEV;
@@ -1540,20 +1629,7 @@ static void brcmf_pcie_buscore_write32(void *ctx, u32 addr, u32 value)
 
 static int brcmf_pcie_buscoreprep(void *ctx)
 {
-	struct brcmf_pciedev_info *devinfo = (struct brcmf_pciedev_info *)ctx;
-	int err;
-
-	err = brcmf_pcie_get_resource(devinfo);
-	if (err == 0) {
-		/* Set CC watchdog to reset all the cores on the chip to bring
-		 * back dongle to a sane state.
-		 */
-		brcmf_pcie_buscore_write32(ctx, CORE_CC_REG(SI_ENUM_BASE,
-							    watchdog), 4);
-		msleep(100);
-	}
-
-	return err;
+	return brcmf_pcie_get_resource(ctx);
 }
 
 
@@ -1609,7 +1685,7 @@ static void brcmf_pcie_setup(struct device *dev, const struct firmware *fw,
 		bus->msgbuf->commonrings[i] =
 				&devinfo->shared.commonrings[i]->commonring;
 
-	flowrings = kcalloc(devinfo->shared.nrof_flowrings, sizeof(flowrings),
+	flowrings = kcalloc(devinfo->shared.nrof_flowrings, sizeof(*flowrings),
 			    GFP_KERNEL);
 	if (!flowrings)
 		goto fail;
@@ -1641,8 +1717,13 @@ brcmf_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	struct brcmf_pciedev_info *devinfo;
 	struct brcmf_pciedev *pcie_bus_dev;
 	struct brcmf_bus *bus;
+	u16 domain_nr;
+	u16 bus_nr;
 
-	brcmf_dbg(PCIE, "Enter %x:%x\n", pdev->vendor, pdev->device);
+	domain_nr = pci_domain_nr(pdev->bus) + 1;
+	bus_nr = pdev->bus->number;
+	brcmf_dbg(PCIE, "Enter %x:%x (%d/%d)\n", pdev->vendor, pdev->device,
+		  domain_nr, bus_nr);
 
 	ret = -ENOMEM;
 	devinfo = kzalloc(sizeof(*devinfo), GFP_KERNEL);
@@ -1691,10 +1772,10 @@ brcmf_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (ret)
 		goto fail_bus;
 
-	ret = brcmf_fw_get_firmwares(bus->dev, BRCMF_FW_REQUEST_NVRAM |
-					       BRCMF_FW_REQ_NV_OPTIONAL,
-				     devinfo->fw_name, devinfo->nvram_name,
-				     brcmf_pcie_setup);
+	ret = brcmf_fw_get_firmwares_pcie(bus->dev, BRCMF_FW_REQUEST_NVRAM |
+						    BRCMF_FW_REQ_NV_OPTIONAL,
+					  devinfo->fw_name, devinfo->nvram_name,
+					  brcmf_pcie_setup, domain_nr, bus_nr);
 	if (ret == 0)
 		return 0;
 fail_bus:
@@ -1730,6 +1811,7 @@ brcmf_pcie_remove(struct pci_dev *pdev)
 		brcmf_pcie_intr_disable(devinfo);
 
 	brcmf_detach(&pdev->dev);
+	brcmf_pcie_reset_device(devinfo);
 
 	kfree(bus->bus_priv.pcie);
 	kfree(bus->msgbuf->flowrings);
@@ -1850,9 +1932,11 @@ static struct pci_device_id brcmf_pcie_devid_table[] = {
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_4356_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43567_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43570_DEVICE_ID),
+	BRCMF_PCIE_DEVICE(BRCM_PCIE_4358_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43602_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43602_2G_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43602_5G_DEVICE_ID),
+	BRCMF_PCIE_DEVICE(BRCM_PCIE_43602_RAW_DEVICE_ID),
 	{ /* end: all zeroes */ }
 };
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
index ab0c89833013..d36f5f3d931b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
@@ -601,6 +601,8 @@ static const struct sdiod_drive_str sdiod_drvstr_tab2_3v3[] = {
 #define BCM43241B0_NVRAM_NAME		"brcm/brcmfmac43241b0-sdio.txt"
 #define BCM43241B4_FIRMWARE_NAME	"brcm/brcmfmac43241b4-sdio.bin"
 #define BCM43241B4_NVRAM_NAME		"brcm/brcmfmac43241b4-sdio.txt"
+#define BCM43241B5_FIRMWARE_NAME	"brcm/brcmfmac43241b5-sdio.bin"
+#define BCM43241B5_NVRAM_NAME		"brcm/brcmfmac43241b5-sdio.txt"
 #define BCM4329_FIRMWARE_NAME		"brcm/brcmfmac4329-sdio.bin"
 #define BCM4329_NVRAM_NAME		"brcm/brcmfmac4329-sdio.txt"
 #define BCM4330_FIRMWARE_NAME		"brcm/brcmfmac4330-sdio.bin"
@@ -628,6 +630,8 @@ MODULE_FIRMWARE(BCM43241B0_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM43241B0_NVRAM_NAME);
 MODULE_FIRMWARE(BCM43241B4_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM43241B4_NVRAM_NAME);
+MODULE_FIRMWARE(BCM43241B5_FIRMWARE_NAME);
+MODULE_FIRMWARE(BCM43241B5_NVRAM_NAME);
 MODULE_FIRMWARE(BCM4329_FIRMWARE_NAME);
 MODULE_FIRMWARE(BCM4329_NVRAM_NAME);
 MODULE_FIRMWARE(BCM4330_FIRMWARE_NAME);
@@ -667,7 +671,8 @@ enum brcmf_firmware_type {
 static const struct brcmf_firmware_names brcmf_fwname_data[] = {
 	{ BRCM_CC_43143_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM43143) },
 	{ BRCM_CC_43241_CHIP_ID, 0x0000001F, BRCMF_FIRMWARE_NVRAM(BCM43241B0) },
-	{ BRCM_CC_43241_CHIP_ID, 0xFFFFFFE0, BRCMF_FIRMWARE_NVRAM(BCM43241B4) },
+	{ BRCM_CC_43241_CHIP_ID, 0x00000020, BRCMF_FIRMWARE_NVRAM(BCM43241B4) },
+	{ BRCM_CC_43241_CHIP_ID, 0xFFFFFFC0, BRCMF_FIRMWARE_NVRAM(BCM43241B5) },
 	{ 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) },
@@ -2815,6 +2820,8 @@ static int brcmf_sdio_bus_txdata(struct device *dev, struct sk_buff *pkt)
 	struct brcmf_sdio *bus = sdiodev->bus;
 
 	brcmf_dbg(TRACE, "Enter: pkt: data %p len %d\n", pkt->data, pkt->len);
+	if (sdiodev->state != BRCMF_SDIOD_DATA)
+		return -EIO;
 
 	/* Add space for the header */
 	skb_push(pkt, bus->tx_hdrlen);
@@ -2943,6 +2950,8 @@ brcmf_sdio_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 	int ret;
 
 	brcmf_dbg(TRACE, "Enter\n");
+	if (sdiodev->state != BRCMF_SDIOD_DATA)
+		return -EIO;
 
 	/* Send from dpc */
 	bus->ctrl_frame_buf = msg;
@@ -3233,6 +3242,8 @@ brcmf_sdio_bus_rxctl(struct device *dev, unsigned char *msg, uint msglen)
 	struct brcmf_sdio *bus = sdiodev->bus;
 
 	brcmf_dbg(TRACE, "Enter\n");
+	if (sdiodev->state != BRCMF_SDIOD_DATA)
+		return -EIO;
 
 	/* Wait until control frame is available */
 	timeleft = brcmf_sdio_dcmd_resp_wait(bus, &bus->rxlen, &pending);
@@ -3550,10 +3561,6 @@ void brcmf_sdio_isr(struct brcmf_sdio *bus)
 		return;
 	}
 
-	if (bus->sdiodev->state != BRCMF_SDIOD_DATA) {
-		brcmf_err("bus is down. we have nothing to do\n");
-		return;
-	}
 	/* Count the interrupt call */
 	bus->sdcnt.intrcount++;
 	if (in_interrupt())
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/usb.c b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
index 5df6aa72cc2d..daba86d881bc 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/usb.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
@@ -1270,8 +1270,13 @@ static int brcmf_usb_probe_cb(struct brcmf_usbdev_info *devinfo)
 	bus->chiprev = bus_pub->chiprev;
 
 	/* request firmware here */
-	brcmf_fw_get_firmwares(dev, 0, brcmf_usb_get_fwname(devinfo), NULL,
-			       brcmf_usb_probe_phase2);
+	ret = brcmf_fw_get_firmwares(dev, 0, brcmf_usb_get_fwname(devinfo),
+				     NULL, brcmf_usb_probe_phase2);
+	if (ret) {
+		brcmf_err("firmware request failed: %d\n", ret);
+		goto fail;
+	}
+
 	return 0;
 
 fail:
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index 48135063347e..ab775a5d5b33 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
@@ -41,8 +41,7 @@
 #define BRCMS_FLUSH_TIMEOUT	500 /* msec */
 
 /* Flags we support */
-#define MAC_FILTERS (FIF_PROMISC_IN_BSS | \
-	FIF_ALLMULTI | \
+#define MAC_FILTERS (FIF_ALLMULTI | \
 	FIF_FCSFAIL | \
 	FIF_CONTROL | \
 	FIF_OTHER_BSS | \
@@ -743,8 +742,6 @@ brcms_ops_configure_filter(struct ieee80211_hw *hw,
 	changed_flags &= MAC_FILTERS;
 	*total_flags &= MAC_FILTERS;
 
-	if (changed_flags & FIF_PROMISC_IN_BSS)
-		brcms_dbg_info(core, "FIF_PROMISC_IN_BSS\n");
 	if (changed_flags & FIF_ALLMULTI)
 		brcms_dbg_info(core, "FIF_ALLMULTI\n");
 	if (changed_flags & FIF_FCSFAIL)
@@ -1063,10 +1060,9 @@ static int ieee_hw_rate_init(struct ieee80211_hw *hw)
  */
 static int ieee_hw_init(struct ieee80211_hw *hw)
 {
-	hw->flags = IEEE80211_HW_SIGNAL_DBM
-	    /* | IEEE80211_HW_CONNECTION_MONITOR  What is this? */
-	    | IEEE80211_HW_REPORTS_TX_ACK_STATUS
-	    | IEEE80211_HW_AMPDU_AGGREGATION;
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
 
 	hw->extra_tx_headroom = brcms_c_get_header_len();
 	hw->queues = N_TX_QUEUES;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index 369527e27689..9728be0e704b 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -3571,7 +3571,7 @@ void brcms_c_mac_promisc(struct brcms_c_info *wlc, uint filter_flags)
 
 	wlc->filter_flags = filter_flags;
 
-	if (filter_flags & (FIF_PROMISC_IN_BSS | FIF_OTHER_BSS))
+	if (filter_flags & FIF_OTHER_BSS)
 		promisc_bits |= MCTL_PROMISC;
 
 	if (filter_flags & FIF_BCN_PRBRESP_PROMISC)
diff --git a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
index 4efdd51af9c8..7a6daa37dc6b 100644
--- a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
+++ b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
@@ -45,6 +45,7 @@
 #define BRCM_CC_43567_CHIP_ID		43567
 #define BRCM_CC_43569_CHIP_ID		43569
 #define BRCM_CC_43570_CHIP_ID		43570
+#define BRCM_CC_4358_CHIP_ID		0x4358
 #define BRCM_CC_43602_CHIP_ID		43602
 
 /* USB Device IDs */
@@ -59,9 +60,11 @@
 #define BRCM_PCIE_4356_DEVICE_ID	0x43ec
 #define BRCM_PCIE_43567_DEVICE_ID	0x43d3
 #define BRCM_PCIE_43570_DEVICE_ID	0x43d9
+#define BRCM_PCIE_4358_DEVICE_ID	0x43e9
 #define BRCM_PCIE_43602_DEVICE_ID	0x43ba
 #define BRCM_PCIE_43602_2G_DEVICE_ID	0x43bb
 #define BRCM_PCIE_43602_5G_DEVICE_ID	0x43bc
+#define BRCM_PCIE_43602_RAW_DEVICE_ID	43602
 
 /* brcmsmac IDs */
 #define BCM4313_D11N2G_ID	0x4727	/* 4313 802.11n 2.4G device */
diff --git a/drivers/net/wireless/cw1200/main.c b/drivers/net/wireless/cw1200/main.c
index 3689dbbd10bd..0e51e27d2e3f 100644
--- a/drivers/net/wireless/cw1200/main.c
+++ b/drivers/net/wireless/cw1200/main.c
@@ -278,14 +278,14 @@ static struct ieee80211_hw *cw1200_init_common(const u8 *macaddr,
 	else
 		priv->ba_tx_tid_mask = 0xff; /* Enable TX BLKACK for all TIDs */
 
-	hw->flags = IEEE80211_HW_SIGNAL_DBM |
-		    IEEE80211_HW_SUPPORTS_PS |
-		    IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
-		    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		    IEEE80211_HW_CONNECTION_MONITOR |
-		    IEEE80211_HW_AMPDU_AGGREGATION |
-		    IEEE80211_HW_TX_AMPDU_SETUP_IN_HW |
-		    IEEE80211_HW_NEED_DTIM_BEFORE_ASSOC;
+	ieee80211_hw_set(hw, NEED_DTIM_BEFORE_ASSOC);
+	ieee80211_hw_set(hw, TX_AMPDU_SETUP_IN_HW);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(hw, CONNECTION_MONITOR);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, SUPPORTS_PS);
 
 	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 					  BIT(NL80211_IFTYPE_ADHOC) |
diff --git a/drivers/net/wireless/cw1200/sta.c b/drivers/net/wireless/cw1200/sta.c
index b0f65fa09428..b86500b4418f 100644
--- a/drivers/net/wireless/cw1200/sta.c
+++ b/drivers/net/wireless/cw1200/sta.c
@@ -578,13 +578,11 @@ void cw1200_configure_filter(struct ieee80211_hw *dev,
 {
 	struct cw1200_common *priv = dev->priv;
 	bool listening = !!(*total_flags &
-			    (FIF_PROMISC_IN_BSS |
-			     FIF_OTHER_BSS |
+			    (FIF_OTHER_BSS |
 			     FIF_BCN_PRBRESP_PROMISC |
 			     FIF_PROBE_REQ));
 
-	*total_flags &= FIF_PROMISC_IN_BSS |
-			FIF_OTHER_BSS |
+	*total_flags &= FIF_OTHER_BSS |
 			FIF_FCSFAIL |
 			FIF_BCN_PRBRESP_PROMISC |
 			FIF_PROBE_REQ;
@@ -592,14 +590,12 @@ void cw1200_configure_filter(struct ieee80211_hw *dev,
 	down(&priv->scan.lock);
 	mutex_lock(&priv->conf_mutex);
 
-	priv->rx_filter.promiscuous = (*total_flags & FIF_PROMISC_IN_BSS)
-			? 1 : 0;
+	priv->rx_filter.promiscuous = 0;
 	priv->rx_filter.bssid = (*total_flags & (FIF_OTHER_BSS |
 			FIF_PROBE_REQ)) ? 1 : 0;
 	priv->rx_filter.fcs = (*total_flags & FIF_FCSFAIL) ? 1 : 0;
 	priv->disable_beacon_filter = !(*total_flags &
 					(FIF_BCN_PRBRESP_PROMISC |
-					 FIF_PROMISC_IN_BSS |
 					 FIF_PROBE_REQ));
 	if (priv->listening != listening) {
 		priv->listening = listening;
diff --git a/drivers/net/wireless/iwlegacy/3945-mac.c b/drivers/net/wireless/iwlegacy/3945-mac.c
index e5665804d986..7f4cb692cc57 100644
--- a/drivers/net/wireless/iwlegacy/3945-mac.c
+++ b/drivers/net/wireless/iwlegacy/3945-mac.c
@@ -3048,7 +3048,7 @@ il3945_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 	D_MAC80211("Enter: changed: 0x%x, total: 0x%x\n", changed_flags,
 		   *total_flags);
 
-	CHK(FIF_OTHER_BSS | FIF_PROMISC_IN_BSS, RXON_FILTER_PROMISC_MSK);
+	CHK(FIF_OTHER_BSS, RXON_FILTER_PROMISC_MSK);
 	CHK(FIF_CONTROL, RXON_FILTER_CTL2HOST_MSK);
 	CHK(FIF_BCN_PRBRESP_PROMISC, RXON_FILTER_BCON_AWARE_MSK);
 
@@ -3074,7 +3074,7 @@ il3945_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 	 * filters into the device.
 	 */
 	*total_flags &=
-	    FIF_OTHER_BSS | FIF_ALLMULTI | FIF_PROMISC_IN_BSS |
+	    FIF_OTHER_BSS | FIF_ALLMULTI |
 	    FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
 }
 
@@ -3561,8 +3561,10 @@ il3945_setup_mac(struct il_priv *il)
 	hw->vif_data_size = sizeof(struct il_vif_priv);
 
 	/* Tell mac80211 our characteristics */
-	hw->flags = IEEE80211_HW_SIGNAL_DBM | IEEE80211_HW_SPECTRUM_MGMT |
-		    IEEE80211_HW_SUPPORTS_PS | IEEE80211_HW_SUPPORTS_DYNAMIC_PS;
+	ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(hw, SUPPORTS_PS);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, SPECTRUM_MGMT);
 
 	hw->wiphy->interface_modes =
 	    BIT(NL80211_IFTYPE_STATION) | BIT(NL80211_IFTYPE_ADHOC);
diff --git a/drivers/net/wireless/iwlegacy/4965-mac.c b/drivers/net/wireless/iwlegacy/4965-mac.c
index 976f65fe9c38..44fa422f255e 100644
--- a/drivers/net/wireless/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/iwlegacy/4965-mac.c
@@ -5751,11 +5751,13 @@ il4965_mac_setup_register(struct il_priv *il, u32 max_probe_length)
 	hw->rate_control_algorithm = "iwl-4965-rs";
 
 	/* Tell mac80211 our characteristics */
-	hw->flags =
-	    IEEE80211_HW_SIGNAL_DBM | IEEE80211_HW_AMPDU_AGGREGATION |
-	    IEEE80211_HW_NEED_DTIM_BEFORE_ASSOC | IEEE80211_HW_SPECTRUM_MGMT |
-	    IEEE80211_HW_REPORTS_TX_ACK_STATUS | IEEE80211_HW_SUPPORTS_PS |
-	    IEEE80211_HW_SUPPORTS_DYNAMIC_PS;
+	ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(hw, SUPPORTS_PS);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, SPECTRUM_MGMT);
+	ieee80211_hw_set(hw, NEED_DTIM_BEFORE_ASSOC);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
 	if (il->cfg->sku & IL_SKU_N)
 		hw->wiphy->features |= NL80211_FEATURE_DYNAMIC_SMPS |
 				       NL80211_FEATURE_STATIC_SMPS;
@@ -6166,7 +6168,7 @@ il4965_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 	D_MAC80211("Enter: changed: 0x%x, total: 0x%x\n", changed_flags,
 		   *total_flags);
 
-	CHK(FIF_OTHER_BSS | FIF_PROMISC_IN_BSS, RXON_FILTER_PROMISC_MSK);
+	CHK(FIF_OTHER_BSS, RXON_FILTER_PROMISC_MSK);
 	/* Setting _just_ RXON_FILTER_CTL2HOST_MSK causes FH errors */
 	CHK(FIF_CONTROL, RXON_FILTER_CTL2HOST_MSK | RXON_FILTER_PROMISC_MSK);
 	CHK(FIF_BCN_PRBRESP_PROMISC, RXON_FILTER_BCON_AWARE_MSK);
@@ -6192,7 +6194,7 @@ il4965_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 	 * filters into the device.
 	 */
 	*total_flags &=
-	    FIF_OTHER_BSS | FIF_ALLMULTI | FIF_PROMISC_IN_BSS |
+	    FIF_OTHER_BSS | FIF_ALLMULTI |
 	    FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
 }
 
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index f89f446e5c8a..aba095761ac6 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -21,6 +21,7 @@ config IWLWIFI
 		Intel 7260 Wi-Fi Adapter
 		Intel 3160 Wi-Fi Adapter
 		Intel 7265 Wi-Fi Adapter
+		Intel 8260 Wi-Fi Adapter
 		Intel 3165 Wi-Fi Adapter
 
 
@@ -54,16 +55,17 @@ config IWLDVM
 	tristate "Intel Wireless WiFi DVM Firmware support"
 	default IWLWIFI
 	help
-	  This is the driver that supports the DVM firmware which is
-	  used by most existing devices (with the exception of 7260
-	  and 3160).
+	  This is the driver that supports the DVM firmware. The list
+	  of the devices that use this firmware is available here:
+	  https://wireless.wiki.kernel.org/en/users/drivers/iwlwifi#firmware
 
 config IWLMVM
 	tristate "Intel Wireless WiFi MVM Firmware support"
 	select WANT_DEV_COREDUMP
 	help
-	  This is the driver that supports the MVM firmware which is
-	  currently only available for 7260 and 3160 devices.
+	  This is the driver that supports the MVM firmware. The list
+	  of the devices that use this firmware is available here:
+	  https://wireless.wiki.kernel.org/en/users/drivers/iwlwifi#firmware
 
 # don't call it _MODULE -- will confuse Kconfig/fixdep/...
 config IWLWIFI_OPMODE_MODULAR
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index 3d32f4120174..dbfc5b18bcb7 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -9,6 +9,7 @@ iwlwifi-objs		+= iwl-phy-db.o iwl-nvm-parse.o
 iwlwifi-objs		+= pcie/drv.o pcie/rx.o pcie/tx.o pcie/trans.o
 iwlwifi-$(CONFIG_IWLDVM) += iwl-1000.o iwl-2000.o iwl-5000.o iwl-6000.o
 iwlwifi-$(CONFIG_IWLMVM) += iwl-7000.o iwl-8000.o
+iwlwifi-objs		+= iwl-trans.o
 
 iwlwifi-objs += $(iwlwifi-m)
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index 5abd62ed8cb4..7acaa266b704 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -104,15 +104,16 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 	hw->rate_control_algorithm = "iwl-agn-rs";
 
 	/* Tell mac80211 our characteristics */
-	hw->flags = IEEE80211_HW_SIGNAL_DBM |
-		    IEEE80211_HW_AMPDU_AGGREGATION |
-		    IEEE80211_HW_NEED_DTIM_BEFORE_ASSOC |
-		    IEEE80211_HW_SPECTRUM_MGMT |
-		    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		    IEEE80211_HW_QUEUE_CONTROL |
-		    IEEE80211_HW_SUPPORTS_PS |
-		    IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
-		    IEEE80211_HW_WANT_MONITOR_VIF;
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(hw, NEED_DTIM_BEFORE_ASSOC);
+	ieee80211_hw_set(hw, SPECTRUM_MGMT);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, QUEUE_CONTROL);
+	ieee80211_hw_set(hw, SUPPORTS_PS);
+	ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(hw, SUPPORT_FAST_XMIT);
+	ieee80211_hw_set(hw, WANT_MONITOR_VIF);
 
 	hw->offchannel_tx_hw_queue = IWL_AUX_QUEUE;
 	hw->radiotap_mcs_details |= IEEE80211_RADIOTAP_MCS_HAVE_FMT;
@@ -135,7 +136,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 	 */
 	if (priv->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_MFP &&
 	    !iwlwifi_mod_params.sw_crypto)
-		hw->flags |= IEEE80211_HW_MFP_CAPABLE;
+		ieee80211_hw_set(hw, MFP_CAPABLE);
 
 	hw->sta_data_size = sizeof(struct iwl_station_priv);
 	hw->vif_data_size = sizeof(struct iwl_vif_priv);
@@ -1061,7 +1062,7 @@ static void iwlagn_configure_filter(struct ieee80211_hw *hw,
 	IWL_DEBUG_MAC80211(priv, "Enter: changed: 0x%x, total: 0x%x\n",
 			changed_flags, *total_flags);
 
-	CHK(FIF_OTHER_BSS | FIF_PROMISC_IN_BSS, RXON_FILTER_PROMISC_MSK);
+	CHK(FIF_OTHER_BSS, RXON_FILTER_PROMISC_MSK);
 	/* Setting _just_ RXON_FILTER_CTL2HOST_MSK causes FH errors */
 	CHK(FIF_CONTROL, RXON_FILTER_CTL2HOST_MSK | RXON_FILTER_PROMISC_MSK);
 	CHK(FIF_BCN_PRBRESP_PROMISC, RXON_FILTER_BCON_AWARE_MSK);
@@ -1088,7 +1089,7 @@ static void iwlagn_configure_filter(struct ieee80211_hw *hw,
 	 * since we currently do not support programming multicast
 	 * filters into the device.
 	 */
-	*total_flags &= FIF_OTHER_BSS | FIF_ALLMULTI | FIF_PROMISC_IN_BSS |
+	*total_flags &= FIF_OTHER_BSS | FIF_ALLMULTI |
 			FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
 }
 
@@ -1140,7 +1141,6 @@ static void iwlagn_mac_event_callback(struct ieee80211_hw *hw,
 		return;
 
 	IWL_DEBUG_MAC80211(priv, "enter\n");
-	mutex_lock(&priv->mutex);
 
 	if (priv->lib->bt_params &&
 	    priv->lib->bt_params->advanced_bt_coexist) {
@@ -1149,13 +1149,12 @@ static void iwlagn_mac_event_callback(struct ieee80211_hw *hw,
 		else if (event->u.rssi.data == RSSI_EVENT_HIGH)
 			priv->bt_enable_pspoll = false;
 
-		iwlagn_send_advance_bt_config(priv);
+		queue_work(priv->workqueue, &priv->bt_runtime_config);
 	} else {
 		IWL_DEBUG_MAC80211(priv, "Advanced BT coex disabled,"
 				"ignoring RSSI callback\n");
 	}
 
-	mutex_unlock(&priv->mutex);
 	IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
@@ -1343,9 +1342,9 @@ static int iwlagn_mac_add_interface(struct ieee80211_hw *hw,
 	 * other interfaces are added, this is safe.
 	 */
 	if (vif->type == NL80211_IFTYPE_MONITOR)
-		priv->hw->flags |= IEEE80211_HW_RX_INCLUDES_FCS;
+		ieee80211_hw_set(priv->hw, RX_INCLUDES_FCS);
 	else
-		priv->hw->flags &= ~IEEE80211_HW_RX_INCLUDES_FCS;
+		__clear_bit(IEEE80211_HW_RX_INCLUDES_FCS, priv->hw->flags);
 
 	err = iwl_setup_interface(priv, ctx);
 	if (!err || reset)
diff --git a/drivers/net/wireless/iwlwifi/iwl-7000.c b/drivers/net/wireless/iwlwifi/iwl-7000.c
index 74ad278116be..cc35f796d406 100644
--- a/drivers/net/wireless/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-7000.c
@@ -69,7 +69,7 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL7260_UCODE_API_MAX	13
+#define IWL7260_UCODE_API_MAX	15
 
 /* Oldest version we won't warn about */
 #define IWL7260_UCODE_API_OK	12
@@ -124,6 +124,28 @@ static const struct iwl_base_params iwl7000_base_params = {
 	.apmg_wake_up_wa = true,
 };
 
+static const struct iwl_tt_params iwl7000_high_temp_tt_params = {
+	.ct_kill_entry = 118,
+	.ct_kill_exit = 96,
+	.ct_kill_duration = 5,
+	.dynamic_smps_entry = 114,
+	.dynamic_smps_exit = 110,
+	.tx_protection_entry = 114,
+	.tx_protection_exit = 108,
+	.tx_backoff = {
+		{.temperature = 112, .backoff = 300},
+		{.temperature = 113, .backoff = 800},
+		{.temperature = 114, .backoff = 1500},
+		{.temperature = 115, .backoff = 3000},
+		{.temperature = 116, .backoff = 5000},
+		{.temperature = 117, .backoff = 10000},
+	},
+	.support_ct_kill = true,
+	.support_dynamic_smps = true,
+	.support_tx_protection = true,
+	.support_tx_backoff = true,
+};
+
 static const struct iwl_ht_params iwl7000_ht_params = {
 	.stbc = true,
 	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
@@ -166,6 +188,7 @@ const struct iwl_cfg iwl7260_2ac_cfg_high_temp = {
 	.host_interrupt_operation_mode = true,
 	.lp_xtal_workaround = true,
 	.dccm_len = IWL7260_DCCM_LEN,
+	.thermal_params = &iwl7000_high_temp_tt_params,
 };
 
 const struct iwl_cfg iwl7260_2n_cfg = {
diff --git a/drivers/net/wireless/iwlwifi/iwl-8000.c b/drivers/net/wireless/iwlwifi/iwl-8000.c
index ce6321b7d241..72040cd0b979 100644
--- a/drivers/net/wireless/iwlwifi/iwl-8000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-8000.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2014 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2014 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -69,7 +69,7 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL8000_UCODE_API_MAX	13
+#define IWL8000_UCODE_API_MAX	15
 
 /* Oldest version we won't warn about */
 #define IWL8000_UCODE_API_OK	12
@@ -122,24 +122,49 @@ static const struct iwl_ht_params iwl8000_ht_params = {
 	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
 };
 
-#define IWL_DEVICE_8000						\
-	.ucode_api_max = IWL8000_UCODE_API_MAX,			\
-	.ucode_api_ok = IWL8000_UCODE_API_OK,			\
-	.ucode_api_min = IWL8000_UCODE_API_MIN,			\
-	.device_family = IWL_DEVICE_FAMILY_8000,		\
-	.max_inst_size = IWL60_RTC_INST_SIZE,			\
-	.max_data_size = IWL60_RTC_DATA_SIZE,			\
-	.base_params = &iwl8000_base_params,			\
-	.led_mode = IWL_LED_RF_STATE,				\
-	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_8000,	\
-	.d0i3 = true,						\
-	.non_shared_ant = ANT_A,				\
-	.dccm_offset = IWL8260_DCCM_OFFSET,			\
-	.dccm_len = IWL8260_DCCM_LEN,				\
-	.dccm2_offset = IWL8260_DCCM2_OFFSET,			\
-	.dccm2_len = IWL8260_DCCM2_LEN,				\
-	.smem_offset = IWL8260_SMEM_OFFSET,			\
-	.smem_len = IWL8260_SMEM_LEN
+static const struct iwl_tt_params iwl8000_tt_params = {
+	.ct_kill_entry = 115,
+	.ct_kill_exit = 93,
+	.ct_kill_duration = 5,
+	.dynamic_smps_entry = 111,
+	.dynamic_smps_exit = 107,
+	.tx_protection_entry = 112,
+	.tx_protection_exit = 105,
+	.tx_backoff = {
+		{.temperature = 110, .backoff = 200},
+		{.temperature = 111, .backoff = 600},
+		{.temperature = 112, .backoff = 1200},
+		{.temperature = 113, .backoff = 2000},
+		{.temperature = 114, .backoff = 4000},
+	},
+	.support_ct_kill = true,
+	.support_dynamic_smps = true,
+	.support_tx_protection = true,
+	.support_tx_backoff = true,
+};
+
+#define IWL_DEVICE_8000							\
+	.ucode_api_max = IWL8000_UCODE_API_MAX,				\
+	.ucode_api_ok = IWL8000_UCODE_API_OK,				\
+	.ucode_api_min = IWL8000_UCODE_API_MIN,				\
+	.device_family = IWL_DEVICE_FAMILY_8000,			\
+	.max_inst_size = IWL60_RTC_INST_SIZE,				\
+	.max_data_size = IWL60_RTC_DATA_SIZE,				\
+	.base_params = &iwl8000_base_params,				\
+	.led_mode = IWL_LED_RF_STATE,					\
+	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_8000,		\
+	.d0i3 = true,							\
+	.non_shared_ant = ANT_A,					\
+	.dccm_offset = IWL8260_DCCM_OFFSET,				\
+	.dccm_len = IWL8260_DCCM_LEN,					\
+	.dccm2_offset = IWL8260_DCCM2_OFFSET,				\
+	.dccm2_len = IWL8260_DCCM2_LEN,					\
+	.smem_offset = IWL8260_SMEM_OFFSET,				\
+	.smem_len = IWL8260_SMEM_LEN,					\
+	.default_nvm_file_B_step = DEFAULT_NVM_FILE_FAMILY_8000B,	\
+	.default_nvm_file_C_step = DEFAULT_NVM_FILE_FAMILY_8000C,	\
+	.thermal_params = &iwl8000_tt_params,				\
+	.apmg_not_supported = true
 
 const struct iwl_cfg iwl8260_2n_cfg = {
 	.name = "Intel(R) Dual Band Wireless N 8260",
@@ -177,8 +202,6 @@ const struct iwl_cfg iwl8260_2ac_sdio_cfg = {
 	.ht_params = &iwl8000_ht_params,
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
-	.default_nvm_file_B_step = DEFAULT_NVM_FILE_FAMILY_8000B,
-	.default_nvm_file_C_step = DEFAULT_NVM_FILE_FAMILY_8000C,
 	.max_rx_agg_size = MAX_RX_AGG_SIZE_8260_SDIO,
 	.disable_dummy_notification = true,
 	.max_ht_ampdu_exponent  = MAX_HT_AMPDU_EXPONENT_8260_SDIO,
@@ -192,8 +215,6 @@ const struct iwl_cfg iwl4165_2ac_sdio_cfg = {
 	.ht_params = &iwl8000_ht_params,
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
-	.default_nvm_file_B_step = DEFAULT_NVM_FILE_FAMILY_8000B,
-	.default_nvm_file_C_step = DEFAULT_NVM_FILE_FAMILY_8000C,
 	.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 3f33f753ce2f..08c14afeb148 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -195,6 +195,49 @@ struct iwl_ht_params {
 };
 
 /*
+ * Tx-backoff threshold
+ * @temperature: The threshold in Celsius
+ * @backoff: The tx-backoff in uSec
+ */
+struct iwl_tt_tx_backoff {
+	s32 temperature;
+	u32 backoff;
+};
+
+#define TT_TX_BACKOFF_SIZE 6
+
+/**
+ * struct iwl_tt_params - thermal throttling parameters
+ * @ct_kill_entry: CT Kill entry threshold
+ * @ct_kill_exit: CT Kill exit threshold
+ * @ct_kill_duration: The time  intervals (in uSec) in which the driver needs
+ *	to checks whether to exit CT Kill.
+ * @dynamic_smps_entry: Dynamic SMPS entry threshold
+ * @dynamic_smps_exit: Dynamic SMPS exit threshold
+ * @tx_protection_entry: TX protection entry threshold
+ * @tx_protection_exit: TX protection exit threshold
+ * @tx_backoff: Array of thresholds for tx-backoff , in ascending order.
+ * @support_ct_kill: Support CT Kill?
+ * @support_dynamic_smps: Support dynamic SMPS?
+ * @support_tx_protection: Support tx protection?
+ * @support_tx_backoff: Support tx-backoff?
+ */
+struct iwl_tt_params {
+	s32 ct_kill_entry;
+	s32 ct_kill_exit;
+	u32 ct_kill_duration;
+	s32 dynamic_smps_entry;
+	s32 dynamic_smps_exit;
+	s32 tx_protection_entry;
+	s32 tx_protection_exit;
+	struct iwl_tt_tx_backoff tx_backoff[TT_TX_BACKOFF_SIZE];
+	bool support_ct_kill;
+	bool support_dynamic_smps;
+	bool support_tx_protection;
+	bool support_tx_backoff;
+};
+
+/*
  * information on how to parse the EEPROM
  */
 #define EEPROM_REG_BAND_1_CHANNELS		0x08
@@ -316,6 +359,8 @@ struct iwl_cfg {
 	const u32 dccm2_len;
 	const u32 smem_offset;
 	const u32 smem_len;
+	const struct iwl_tt_params *thermal_params;
+	bool apmg_not_supported;
 };
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h b/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h
index 223b8752f924..948ce0802fa7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -64,19 +65,21 @@ TRACE_EVENT(iwlwifi_dev_hcmd,
 
 TRACE_EVENT(iwlwifi_dev_rx,
 	TP_PROTO(const struct device *dev, const struct iwl_trans *trans,
-		 void *rxbuf, size_t len),
-	TP_ARGS(dev, trans, rxbuf, len),
+		 struct iwl_rx_packet *pkt, size_t len),
+	TP_ARGS(dev, trans, pkt, len),
 	TP_STRUCT__entry(
 		DEV_ENTRY
-		__dynamic_array(u8, rxbuf, iwl_rx_trace_len(trans, rxbuf, len))
+		__field(u8, cmd)
+		__dynamic_array(u8, rxbuf, iwl_rx_trace_len(trans, pkt, len))
 	),
 	TP_fast_assign(
 		DEV_ASSIGN;
-		memcpy(__get_dynamic_array(rxbuf), rxbuf,
-		       iwl_rx_trace_len(trans, rxbuf, len));
+		__entry->cmd = pkt->hdr.cmd;
+		memcpy(__get_dynamic_array(rxbuf), pkt,
+		       iwl_rx_trace_len(trans, pkt, len));
 	),
 	TP_printk("[%s] RX cmd %#.2x",
-		  __get_str(dev), ((u8 *)__get_dynamic_array(rxbuf))[4])
+		  __get_str(dev), __entry->cmd)
 );
 
 TRACE_EVENT(iwlwifi_dev_tx,
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 7267152e7dc7..6685259927f8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2007 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -423,13 +423,19 @@ static int iwl_set_ucode_api_flags(struct iwl_drv *drv, const u8 *data,
 {
 	const struct iwl_ucode_api *ucode_api = (void *)data;
 	u32 api_index = le32_to_cpu(ucode_api->api_index);
+	u32 api_flags = le32_to_cpu(ucode_api->api_flags);
+	int i;
 
-	if (api_index >= IWL_API_ARRAY_SIZE) {
+	if (api_index >= IWL_API_MAX_BITS / 32) {
 		IWL_ERR(drv, "api_index larger than supported by driver\n");
-		return -EINVAL;
+		/* don't return an error so we can load FW that has more bits */
+		return 0;
 	}
 
-	capa->api[api_index] = le32_to_cpu(ucode_api->api_flags);
+	for (i = 0; i < 32; i++) {
+		if (api_flags & BIT(i))
+			__set_bit(i + 32 * api_index, capa->_api);
+	}
 
 	return 0;
 }
@@ -439,13 +445,19 @@ static int iwl_set_ucode_capabilities(struct iwl_drv *drv, const u8 *data,
 {
 	const struct iwl_ucode_capa *ucode_capa = (void *)data;
 	u32 api_index = le32_to_cpu(ucode_capa->api_index);
+	u32 api_flags = le32_to_cpu(ucode_capa->api_capa);
+	int i;
 
-	if (api_index >= IWL_CAPABILITIES_ARRAY_SIZE) {
+	if (api_index >= IWL_CAPABILITIES_MAX_BITS / 32) {
 		IWL_ERR(drv, "api_index larger than supported by driver\n");
-		return -EINVAL;
+		/* don't return an error so we can load FW that has more bits */
+		return 0;
 	}
 
-	capa->capa[api_index] = le32_to_cpu(ucode_capa->api_capa);
+	for (i = 0; i < 32; i++) {
+		if (api_flags & BIT(i))
+			__set_bit(i + 32 * api_index, capa->_capa);
+	}
 
 	return 0;
 }
@@ -1148,7 +1160,7 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 	if (err)
 		goto try_again;
 
-	if (drv->fw.ucode_capa.api[0] & IWL_UCODE_TLV_API_NEW_VERSION)
+	if (fw_has_api(&drv->fw.ucode_capa, IWL_UCODE_TLV_API_NEW_VERSION))
 		api_ver = drv->fw.ucode_ver;
 	else
 		api_ver = IWL_UCODE_API(drv->fw.ucode_ver);
@@ -1239,6 +1251,8 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 		sizeof(struct iwl_fw_dbg_trigger_txq_timer);
 	trigger_tlv_sz[FW_DBG_TRIGGER_TIME_EVENT] =
 		sizeof(struct iwl_fw_dbg_trigger_time_event);
+	trigger_tlv_sz[FW_DBG_TRIGGER_BA] =
+		sizeof(struct iwl_fw_dbg_trigger_ba);
 
 	for (i = 0; i < ARRAY_SIZE(drv->fw.dbg_trigger_tlv); i++) {
 		if (pieces->dbg_trigger_tlv[i]) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/iwlwifi/iwl-fh.h
index d45dc021cda2..d56064861a9c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fh.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fh.h
@@ -438,12 +438,6 @@ static inline unsigned int FH_MEM_CBBC_QUEUE(unsigned int chnl)
 #define RX_QUEUE_MASK                         255
 #define RX_QUEUE_SIZE_LOG                     8
 
-/*
- * RX related structures and functions
- */
-#define RX_FREE_BUFFERS 64
-#define RX_LOW_WATERMARK 8
-
 /**
  * struct iwl_rb_status - reserve buffer status
  * 	host memory mapped FH registers
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h b/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
index 251bf8dc4a12..e57dbd0ef2e1 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2014 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2014 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -254,6 +254,7 @@ iwl_fw_error_next_data(struct iwl_fw_error_dump_data *data)
  *	detection.
  * @FW_DBG_TRIGGER_TIME_EVENT: trigger log collection upon time events related
  *	events.
+ * @FW_DBG_TRIGGER_BA: trigger log collection upon BlockAck related events.
  */
 enum iwl_fw_dbg_trigger {
 	FW_DBG_TRIGGER_INVALID = 0,
@@ -267,6 +268,7 @@ enum iwl_fw_dbg_trigger {
 	FW_DBG_TRIGGER_RSSI,
 	FW_DBG_TRIGGER_TXQ_TIMERS,
 	FW_DBG_TRIGGER_TIME_EVENT,
+	FW_DBG_TRIGGER_BA,
 
 	/* must be last */
 	FW_DBG_TRIGGER_MAX,
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
index 62db2e5e45eb..a9b5ae4ebec0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -237,6 +237,8 @@ enum iwl_ucode_tlv_flag {
 	IWL_UCODE_TLV_FLAGS_GO_UAPSD		= BIT(30),
 };
 
+typedef unsigned int __bitwise__ iwl_ucode_tlv_api_t;
+
 /**
  * enum iwl_ucode_tlv_api - ucode api
  * @IWL_UCODE_TLV_API_BT_COEX_SPLIT: new API for BT Coex
@@ -255,22 +257,27 @@ enum iwl_ucode_tlv_flag {
  * @IWL_UCODE_TLV_API_LQ_SS_PARAMS: Configure STBC/BFER via LQ CMD ss_params
  * @IWL_UCODE_TLV_API_STATS_V10: uCode supports/uses statistics API version 10
  * @IWL_UCODE_TLV_API_NEW_VERSION: new versioning format
+ * @IWL_UCODE_TLV_API_EXT_SCAN_PRIORITY: scan APIs use 8-level priority
+ *	instead of 3.
  */
 enum iwl_ucode_tlv_api {
-	IWL_UCODE_TLV_API_BT_COEX_SPLIT         = BIT(3),
-	IWL_UCODE_TLV_API_FRAGMENTED_SCAN	= BIT(8),
-	IWL_UCODE_TLV_API_WIFI_MCC_UPDATE	= BIT(9),
-	IWL_UCODE_TLV_API_HDC_PHASE_0		= BIT(10),
-	IWL_UCODE_TLV_API_TX_POWER_DEV		= BIT(11),
-	IWL_UCODE_TLV_API_BASIC_DWELL		= BIT(13),
-	IWL_UCODE_TLV_API_SCD_CFG		= BIT(15),
-	IWL_UCODE_TLV_API_SINGLE_SCAN_EBS	= BIT(16),
-	IWL_UCODE_TLV_API_ASYNC_DTM		= BIT(17),
-	IWL_UCODE_TLV_API_LQ_SS_PARAMS		= BIT(18),
-	IWL_UCODE_TLV_API_STATS_V10		= BIT(19),
-	IWL_UCODE_TLV_API_NEW_VERSION		= BIT(20),
+	IWL_UCODE_TLV_API_BT_COEX_SPLIT         = (__force iwl_ucode_tlv_api_t)3,
+	IWL_UCODE_TLV_API_FRAGMENTED_SCAN	= (__force iwl_ucode_tlv_api_t)8,
+	IWL_UCODE_TLV_API_WIFI_MCC_UPDATE	= (__force iwl_ucode_tlv_api_t)9,
+	IWL_UCODE_TLV_API_HDC_PHASE_0		= (__force iwl_ucode_tlv_api_t)10,
+	IWL_UCODE_TLV_API_TX_POWER_DEV		= (__force iwl_ucode_tlv_api_t)11,
+	IWL_UCODE_TLV_API_BASIC_DWELL		= (__force iwl_ucode_tlv_api_t)13,
+	IWL_UCODE_TLV_API_SCD_CFG		= (__force iwl_ucode_tlv_api_t)15,
+	IWL_UCODE_TLV_API_SINGLE_SCAN_EBS	= (__force iwl_ucode_tlv_api_t)16,
+	IWL_UCODE_TLV_API_ASYNC_DTM		= (__force iwl_ucode_tlv_api_t)17,
+	IWL_UCODE_TLV_API_LQ_SS_PARAMS		= (__force iwl_ucode_tlv_api_t)18,
+	IWL_UCODE_TLV_API_STATS_V10		= (__force iwl_ucode_tlv_api_t)19,
+	IWL_UCODE_TLV_API_NEW_VERSION		= (__force iwl_ucode_tlv_api_t)20,
+	IWL_UCODE_TLV_API_EXT_SCAN_PRIORITY	= (__force iwl_ucode_tlv_api_t)24,
 };
 
+typedef unsigned int __bitwise__ iwl_ucode_tlv_capa_t;
+
 /**
  * enum iwl_ucode_tlv_capa - ucode capabilities
  * @IWL_UCODE_TLV_CAPA_D0I3_SUPPORT: supports D0i3
@@ -290,6 +297,7 @@ enum iwl_ucode_tlv_api {
  *	which also implies support for the scheduler configuration command
  * @IWL_UCODE_TLV_CAPA_TDLS_CHANNEL_SWITCH: supports TDLS channel switching
  * @IWL_UCODE_TLV_CAPA_HOTSPOT_SUPPORT: supports Hot Spot Command
+ * @IWL_UCODE_TLV_CAPA_DC2DC_SUPPORT: supports DC2DC Command
  * @IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS: support radio and beacon statistics
  * @IWL_UCODE_TLV_CAPA_BT_COEX_PLCR: enabled BT Coex packet level co-running
  * @IWL_UCODE_TLV_CAPA_LAR_MULTI_MCC: ucode supports LAR updates with different
@@ -299,22 +307,23 @@ enum iwl_ucode_tlv_api {
  * @IWL_UCODE_TLV_CAPA_BT_COEX_RRC: supports BT Coex RRC
  */
 enum iwl_ucode_tlv_capa {
-	IWL_UCODE_TLV_CAPA_D0I3_SUPPORT			= BIT(0),
-	IWL_UCODE_TLV_CAPA_LAR_SUPPORT			= BIT(1),
-	IWL_UCODE_TLV_CAPA_UMAC_SCAN			= BIT(2),
-	IWL_UCODE_TLV_CAPA_BEAMFORMER			= BIT(3),
-	IWL_UCODE_TLV_CAPA_TDLS_SUPPORT			= BIT(6),
-	IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT	= BIT(8),
-	IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT	= BIT(9),
-	IWL_UCODE_TLV_CAPA_WFA_TPC_REP_IE_SUPPORT	= BIT(10),
-	IWL_UCODE_TLV_CAPA_QUIET_PERIOD_SUPPORT		= BIT(11),
-	IWL_UCODE_TLV_CAPA_DQA_SUPPORT			= BIT(12),
-	IWL_UCODE_TLV_CAPA_TDLS_CHANNEL_SWITCH		= BIT(13),
-	IWL_UCODE_TLV_CAPA_HOTSPOT_SUPPORT		= BIT(18),
-	IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS		= BIT(22),
-	IWL_UCODE_TLV_CAPA_BT_COEX_PLCR			= BIT(28),
-	IWL_UCODE_TLV_CAPA_LAR_MULTI_MCC		= BIT(29),
-	IWL_UCODE_TLV_CAPA_BT_COEX_RRC			= BIT(30),
+	IWL_UCODE_TLV_CAPA_D0I3_SUPPORT			= (__force iwl_ucode_tlv_capa_t)0,
+	IWL_UCODE_TLV_CAPA_LAR_SUPPORT			= (__force iwl_ucode_tlv_capa_t)1,
+	IWL_UCODE_TLV_CAPA_UMAC_SCAN			= (__force iwl_ucode_tlv_capa_t)2,
+	IWL_UCODE_TLV_CAPA_BEAMFORMER			= (__force iwl_ucode_tlv_capa_t)3,
+	IWL_UCODE_TLV_CAPA_TDLS_SUPPORT			= (__force iwl_ucode_tlv_capa_t)6,
+	IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT	= (__force iwl_ucode_tlv_capa_t)8,
+	IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT	= (__force iwl_ucode_tlv_capa_t)9,
+	IWL_UCODE_TLV_CAPA_WFA_TPC_REP_IE_SUPPORT	= (__force iwl_ucode_tlv_capa_t)10,
+	IWL_UCODE_TLV_CAPA_QUIET_PERIOD_SUPPORT		= (__force iwl_ucode_tlv_capa_t)11,
+	IWL_UCODE_TLV_CAPA_DQA_SUPPORT			= (__force iwl_ucode_tlv_capa_t)12,
+	IWL_UCODE_TLV_CAPA_TDLS_CHANNEL_SWITCH		= (__force iwl_ucode_tlv_capa_t)13,
+	IWL_UCODE_TLV_CAPA_HOTSPOT_SUPPORT		= (__force iwl_ucode_tlv_capa_t)18,
+	IWL_UCODE_TLV_CAPA_DC2DC_CONFIG_SUPPORT		= (__force iwl_ucode_tlv_capa_t)19,
+	IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS		= (__force iwl_ucode_tlv_capa_t)22,
+	IWL_UCODE_TLV_CAPA_BT_COEX_PLCR			= (__force iwl_ucode_tlv_capa_t)28,
+	IWL_UCODE_TLV_CAPA_LAR_MULTI_MCC		= (__force iwl_ucode_tlv_capa_t)29,
+	IWL_UCODE_TLV_CAPA_BT_COEX_RRC			= (__force iwl_ucode_tlv_capa_t)30,
 };
 
 /* The default calibrate table size if not specified by firmware file */
@@ -325,13 +334,14 @@ enum iwl_ucode_tlv_capa {
 /* The default max probe length if not specified by the firmware file */
 #define IWL_DEFAULT_MAX_PROBE_LENGTH	200
 
+#define IWL_API_MAX_BITS		64
+#define IWL_CAPABILITIES_MAX_BITS	64
+
 /*
  * For 16.0 uCode and above, there is no differentiation between sections,
  * just an offset to the HW address.
  */
 #define IWL_UCODE_SECTION_MAX 12
-#define IWL_API_ARRAY_SIZE	1
-#define IWL_CAPABILITIES_ARRAY_SIZE	1
 #define CPU1_CPU2_SEPARATOR_SECTION	0xFFFFCCCC
 
 /* uCode version contains 4 values: Major/Minor/API/Serial */
@@ -424,11 +434,13 @@ struct iwl_fw_dbg_reg_op {
  * @SMEM_MODE: monitor stores the data in SMEM
  * @EXTERNAL_MODE: monitor stores the data in allocated DRAM
  * @MARBH_MODE: monitor stores the data in MARBH buffer
+ * @MIPI_MODE: monitor outputs the data through the MIPI interface
  */
 enum iwl_fw_dbg_monitor_mode {
 	SMEM_MODE = 0,
 	EXTERNAL_MODE = 1,
 	MARBH_MODE = 2,
+	MIPI_MODE = 3,
 };
 
 /**
@@ -436,6 +448,7 @@ enum iwl_fw_dbg_monitor_mode {
  *
  * @version: version of the TLV - currently 0
  * @monitor_mode: %enum iwl_fw_dbg_monitor_mode
+ * @size_power: buffer size will be 2^(size_power + 11)
  * @base_reg: addr of the base addr register (PRPH)
  * @end_reg:  addr of the end addr register (PRPH)
  * @write_ptr_reg: the addr of the reg of the write pointer
@@ -449,7 +462,8 @@ enum iwl_fw_dbg_monitor_mode {
 struct iwl_fw_dbg_dest_tlv {
 	u8 version;
 	u8 monitor_mode;
-	u8 reserved[2];
+	u8 size_power;
+	u8 reserved;
 	__le32 base_reg;
 	__le32 end_reg;
 	__le32 write_ptr_reg;
@@ -659,6 +673,33 @@ struct iwl_fw_dbg_trigger_time_event {
 } __packed;
 
 /**
+ * struct iwl_fw_dbg_trigger_ba - configures BlockAck related trigger
+ * rx_ba_start: tid bitmap to configure on what tid the trigger should occur
+ *	when an Rx BlockAck session is started.
+ * rx_ba_stop: tid bitmap to configure on what tid the trigger should occur
+ *	when an Rx BlockAck session is stopped.
+ * tx_ba_start: tid bitmap to configure on what tid the trigger should occur
+ *	when a Tx BlockAck session is started.
+ * tx_ba_stop: tid bitmap to configure on what tid the trigger should occur
+ *	when a Tx BlockAck session is stopped.
+ * rx_bar: tid bitmap to configure on what tid the trigger should occur
+ *	when a BAR is received (for a Tx BlockAck session).
+ * tx_bar: tid bitmap to configure on what tid the trigger should occur
+ *	when a BAR is send (for an Rx BlocAck session).
+ * frame_timeout: tid bitmap to configure on what tid the trigger should occur
+ *	when a frame times out in the reodering buffer.
+ */
+struct iwl_fw_dbg_trigger_ba {
+	__le16 rx_ba_start;
+	__le16 rx_ba_stop;
+	__le16 tx_ba_start;
+	__le16 tx_ba_stop;
+	__le16 rx_bar;
+	__le16 tx_bar;
+	__le16 frame_timeout;
+} __packed;
+
+/**
  * struct iwl_fw_dbg_conf_tlv - a TLV that describes a debug configuration.
  * @id: conf id
  * @usniffer: should the uSniffer image be used
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/iwlwifi/iwl-fw.h
index cf75bafae51d..3e3c9d8b3c37 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw.h
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -105,10 +105,24 @@ struct iwl_ucode_capabilities {
 	u32 n_scan_channels;
 	u32 standard_phy_calibration_size;
 	u32 flags;
-	u32 api[IWL_API_ARRAY_SIZE];
-	u32 capa[IWL_CAPABILITIES_ARRAY_SIZE];
+	unsigned long _api[BITS_TO_LONGS(IWL_API_MAX_BITS)];
+	unsigned long _capa[BITS_TO_LONGS(IWL_CAPABILITIES_MAX_BITS)];
 };
 
+static inline bool
+fw_has_api(const struct iwl_ucode_capabilities *capabilities,
+	   iwl_ucode_tlv_api_t api)
+{
+	return test_bit((__force long)api, capabilities->_api);
+}
+
+static inline bool
+fw_has_capa(const struct iwl_ucode_capabilities *capabilities,
+	    iwl_ucode_tlv_capa_t capa)
+{
+	return test_bit((__force long)capa, capabilities->_capa);
+}
+
 /* one for each uCode image (inst/data, init/runtime/wowlan) */
 struct fw_desc {
 	const void *data;	/* vmalloc'ed data */
@@ -205,6 +219,8 @@ static inline const char *get_fw_dbg_mode_string(int mode)
 		return "EXTERNAL_DRAM";
 	case MARBH_MODE:
 		return "MARBH";
+	case MIPI_MODE:
+		return "MIPI";
 	default:
 		return "UNKNOWN";
 	}
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
index 75e96db6626b..80fefe7d7b8c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
@@ -249,7 +249,7 @@ static u32 iwl_get_channel_flags(u8 ch_num, int ch_idx, bool is_5ghz,
 	 */
 	if ((nvm_flags & NVM_CHANNEL_GO_CONCURRENT) &&
 	    (flags & IEEE80211_CHAN_NO_IR))
-		flags |= IEEE80211_CHAN_GO_CONCURRENT;
+		flags |= IEEE80211_CHAN_IR_CONCURRENT;
 
 	return flags;
 }
@@ -471,7 +471,7 @@ static int iwl_get_radio_cfg(const struct iwl_cfg *cfg, const __le16 *nvm_sw,
 	if (cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		return le16_to_cpup(nvm_sw + RADIO_CFG);
 
-	return le32_to_cpup((__le32 *)(nvm_sw + RADIO_CFG_FAMILY_8000));
+	return le32_to_cpup((__le32 *)(phy_sku + RADIO_CFG_FAMILY_8000));
 
 }
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index 88a57e6e232f..5af1c776d2d4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -348,6 +348,9 @@ enum secure_load_status_reg {
 #define MON_BUFF_WRPTR			(0xa03c44)
 #define MON_BUFF_CYCLE_CNT		(0xa03c48)
 
+#define MON_DMARB_RD_CTL_ADDR		(0xa03c60)
+#define MON_DMARB_RD_DATA_ADDR		(0xa03c5c)
+
 #define DBGC_IN_SAMPLE			(0xa03c00)
 
 /* enable the ID buf for read */
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.c b/drivers/net/wireless/iwlwifi/iwl-trans.c
new file mode 100644
index 000000000000..9f8bcefc04c5
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.c
@@ -0,0 +1,113 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2015 Intel Mobile Communications GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2015 Intel Mobile Communications GmbH
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#include <linux/kernel.h>
+#include "iwl-trans.h"
+
+struct iwl_trans *iwl_trans_alloc(unsigned int priv_size,
+				  struct device *dev,
+				  const struct iwl_cfg *cfg,
+				  const struct iwl_trans_ops *ops,
+				  size_t dev_cmd_headroom)
+{
+	struct iwl_trans *trans;
+#ifdef CONFIG_LOCKDEP
+	static struct lock_class_key __key;
+#endif
+
+	trans = kzalloc(sizeof(*trans) + priv_size, GFP_KERNEL);
+	if (!trans)
+		return NULL;
+
+#ifdef CONFIG_LOCKDEP
+	lockdep_init_map(&trans->sync_cmd_lockdep_map, "sync_cmd_lockdep_map",
+			 &__key, 0);
+#endif
+
+	trans->dev = dev;
+	trans->cfg = cfg;
+	trans->ops = ops;
+	trans->dev_cmd_headroom = dev_cmd_headroom;
+
+	snprintf(trans->dev_cmd_pool_name, sizeof(trans->dev_cmd_pool_name),
+		 "iwl_cmd_pool:%s", dev_name(trans->dev));
+	trans->dev_cmd_pool =
+		kmem_cache_create(trans->dev_cmd_pool_name,
+				  sizeof(struct iwl_device_cmd)
+				  + trans->dev_cmd_headroom,
+				  sizeof(void *),
+				  SLAB_HWCACHE_ALIGN,
+				  NULL);
+	if (!trans->dev_cmd_pool)
+		goto free;
+
+	return trans;
+ free:
+	kfree(trans);
+	return NULL;
+}
+
+void iwl_trans_free(struct iwl_trans *trans)
+{
+	kmem_cache_destroy(trans->dev_cmd_pool);
+	kfree(trans);
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 56254a837214..87a230a7f4b6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -641,6 +641,8 @@ struct iwl_trans {
 
 	enum iwl_d0i3_mode d0i3_mode;
 
+	bool wowlan_d0i3;
+
 	/* pointer to trans specific struct */
 	/*Ensure that this pointer will always be aligned to sizeof pointer */
 	char trans_specific[0] __aligned(sizeof(void *));
@@ -1011,19 +1013,19 @@ static inline void iwl_trans_fw_error(struct iwl_trans *trans)
 }
 
 /*****************************************************
+ * transport helper functions
+ *****************************************************/
+struct iwl_trans *iwl_trans_alloc(unsigned int priv_size,
+				  struct device *dev,
+				  const struct iwl_cfg *cfg,
+				  const struct iwl_trans_ops *ops,
+				  size_t dev_cmd_headroom);
+void iwl_trans_free(struct iwl_trans *trans);
+
+/*****************************************************
 * driver (transport) register/unregister functions
 ******************************************************/
 int __must_check iwl_pci_register_driver(void);
 void iwl_pci_unregister_driver(void);
 
-static inline void trans_lockdep_init(struct iwl_trans *trans)
-{
-#ifdef CONFIG_LOCKDEP
-	static struct lock_class_key __key;
-
-	lockdep_init_map(&trans->sync_cmd_lockdep_map, "sync_cmd_lockdep_map",
-			 &__key, 0);
-#endif
-}
-
 #endif /* __iwl_trans_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex.c b/drivers/net/wireless/iwlwifi/mvm/coex.c
index 13a0a03158de..b4737e296c92 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -408,23 +408,12 @@ iwl_get_coex_type(struct iwl_mvm *mvm, const struct ieee80211_vif *vif)
 
 int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 {
-	struct iwl_bt_coex_cmd *bt_cmd;
-	struct iwl_host_cmd cmd = {
-		.id = BT_CONFIG,
-		.len = { sizeof(*bt_cmd), },
-		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
-	};
-	int ret;
+	struct iwl_bt_coex_cmd bt_cmd = {};
 	u32 mode;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_send_bt_init_conf_old(mvm);
 
-	bt_cmd = kzalloc(sizeof(*bt_cmd), GFP_KERNEL);
-	if (!bt_cmd)
-		return -ENOMEM;
-	cmd.data[0] = bt_cmd;
-
 	lockdep_assert_held(&mvm->mutex);
 
 	if (unlikely(mvm->bt_force_ant_mode != BT_FORCE_ANT_DIS)) {
@@ -440,36 +429,33 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 			mode = 0;
 		}
 
-		bt_cmd->mode = cpu_to_le32(mode);
+		bt_cmd.mode = cpu_to_le32(mode);
 		goto send_cmd;
 	}
 
 	mode = iwlwifi_mod_params.bt_coex_active ? BT_COEX_NW : BT_COEX_DISABLE;
-	bt_cmd->mode = cpu_to_le32(mode);
+	bt_cmd.mode = cpu_to_le32(mode);
 
 	if (IWL_MVM_BT_COEX_SYNC2SCO)
-		bt_cmd->enabled_modules |=
+		bt_cmd.enabled_modules |=
 			cpu_to_le32(BT_COEX_SYNC2SCO_ENABLED);
 
 	if (iwl_mvm_bt_is_plcr_supported(mvm))
-		bt_cmd->enabled_modules |= cpu_to_le32(BT_COEX_CORUN_ENABLED);
+		bt_cmd.enabled_modules |= cpu_to_le32(BT_COEX_CORUN_ENABLED);
 
 	if (IWL_MVM_BT_COEX_MPLUT) {
-		bt_cmd->enabled_modules |= cpu_to_le32(BT_COEX_MPLUT_ENABLED);
-		bt_cmd->enabled_modules |=
+		bt_cmd.enabled_modules |= cpu_to_le32(BT_COEX_MPLUT_ENABLED);
+		bt_cmd.enabled_modules |=
 			cpu_to_le32(BT_COEX_MPLUT_BOOST_ENABLED);
 	}
 
-	bt_cmd->enabled_modules |= cpu_to_le32(BT_COEX_HIGH_BAND_RET);
+	bt_cmd.enabled_modules |= cpu_to_le32(BT_COEX_HIGH_BAND_RET);
 
 send_cmd:
 	memset(&mvm->last_bt_notif, 0, sizeof(mvm->last_bt_notif));
 	memset(&mvm->last_bt_ci_cmd, 0, sizeof(mvm->last_bt_ci_cmd));
 
-	ret = iwl_mvm_send_cmd(mvm, &cmd);
-
-	kfree(bt_cmd);
-	return ret;
+	return iwl_mvm_send_cmd_pdu(mvm, BT_CONFIG, 0, sizeof(bt_cmd), &bt_cmd);
 }
 
 static int iwl_mvm_bt_coex_reduced_txp(struct iwl_mvm *mvm, u8 sta_id,
@@ -746,7 +732,7 @@ int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_bt_coex_profile_notif *notif = (void *)pkt->data;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_rx_bt_coex_notif_old(mvm, rxb, dev_cmd);
 
 	IWL_DEBUG_COEX(mvm, "BT Coex Notification received\n");
@@ -770,52 +756,14 @@ int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
 	return 0;
 }
 
-static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
-				   struct ieee80211_vif *vif)
-{
-	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	struct iwl_bt_iterator_data *data = _data;
-	struct iwl_mvm *mvm = data->mvm;
-
-	struct ieee80211_sta *sta;
-	struct iwl_mvm_sta *mvmsta;
-
-	struct ieee80211_chanctx_conf *chanctx_conf;
-
-	rcu_read_lock();
-	chanctx_conf = rcu_dereference(vif->chanctx_conf);
-	/* If channel context is invalid or not on 2.4GHz - don't count it */
-	if (!chanctx_conf ||
-	    chanctx_conf->def.chan->band != IEEE80211_BAND_2GHZ) {
-		rcu_read_unlock();
-		return;
-	}
-	rcu_read_unlock();
-
-	if (vif->type != NL80211_IFTYPE_STATION ||
-	    mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT)
-		return;
-
-	sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[mvmvif->ap_sta_id],
-					lockdep_is_held(&mvm->mutex));
-
-	/* This can happen if the station has been removed right now */
-	if (IS_ERR_OR_NULL(sta))
-		return;
-
-	mvmsta = iwl_mvm_sta_from_mac80211(sta);
-}
-
 void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			   enum ieee80211_rssi_event_data rssi_event)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	struct iwl_bt_iterator_data data = {
-		.mvm = mvm,
-	};
 	int ret;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT)) {
+	if (!fw_has_api(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_API_BT_COEX_SPLIT)) {
 		iwl_mvm_bt_rssi_event_old(mvm, vif, rssi_event);
 		return;
 	}
@@ -853,10 +801,6 @@ void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	if (ret)
 		IWL_ERR(mvm, "couldn't send BT_CONFIG HCMD upon RSSI event\n");
-
-	ieee80211_iterate_active_interfaces_atomic(
-		mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
-		iwl_mvm_bt_rssi_iterator, &data);
 }
 
 #define LINK_QUAL_AGG_TIME_LIMIT_DEF	(4000)
@@ -870,7 +814,7 @@ u16 iwl_mvm_coex_agg_time_limit(struct iwl_mvm *mvm,
 	struct iwl_mvm_phy_ctxt *phy_ctxt = mvmvif->phy_ctxt;
 	enum iwl_bt_coex_lut_type lut_type;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_coex_agg_time_limit_old(mvm, sta);
 
 	if (IWL_COEX_IS_TTC_ON(mvm->last_bt_notif.ttc_rrc_status, phy_ctxt->id))
@@ -897,7 +841,7 @@ bool iwl_mvm_bt_coex_is_mimo_allowed(struct iwl_mvm *mvm,
 	struct iwl_mvm_phy_ctxt *phy_ctxt = mvmvif->phy_ctxt;
 	enum iwl_bt_coex_lut_type lut_type;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_bt_coex_is_mimo_allowed_old(mvm, sta);
 
 	if (IWL_COEX_IS_TTC_ON(mvm->last_bt_notif.ttc_rrc_status, phy_ctxt->id))
@@ -927,7 +871,7 @@ bool iwl_mvm_bt_coex_is_ant_avail(struct iwl_mvm *mvm, u8 ant)
 	if (ant & mvm->cfg->non_shared_ant)
 		return true;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_bt_coex_is_shared_ant_avail_old(mvm);
 
 	return le32_to_cpu(mvm->last_bt_notif.bt_activity_grading) <
@@ -940,10 +884,10 @@ bool iwl_mvm_bt_coex_is_shared_ant_avail(struct iwl_mvm *mvm)
 	if (mvm->cfg->bt_shared_single_ant)
 		return true;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_bt_coex_is_shared_ant_avail_old(mvm);
 
-	return le32_to_cpu(mvm->last_bt_notif.bt_activity_grading) == BT_OFF;
+	return le32_to_cpu(mvm->last_bt_notif.bt_activity_grading) < BT_HIGH_TRAFFIC;
 }
 
 bool iwl_mvm_bt_coex_is_tpc_allowed(struct iwl_mvm *mvm,
@@ -951,7 +895,7 @@ bool iwl_mvm_bt_coex_is_tpc_allowed(struct iwl_mvm *mvm,
 {
 	u32 bt_activity = le32_to_cpu(mvm->last_bt_notif.bt_activity_grading);
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_bt_coex_is_tpc_allowed_old(mvm, band);
 
 	if (band != IEEE80211_BAND_2GHZ)
@@ -994,7 +938,8 @@ u8 iwl_mvm_bt_coex_tx_prio(struct iwl_mvm *mvm, struct ieee80211_hdr *hdr,
 
 void iwl_mvm_bt_coex_vif_change(struct iwl_mvm *mvm)
 {
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT)) {
+	if (!fw_has_api(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_API_BT_COEX_SPLIT)) {
 		iwl_mvm_bt_coex_vif_change_old(mvm);
 		return;
 	}
@@ -1012,7 +957,7 @@ int iwl_mvm_rx_ant_coupling_notif(struct iwl_mvm *mvm,
 	u8 __maybe_unused lower_bound, upper_bound;
 	u8 lut;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_rx_ant_coupling_notif_old(mvm, rxb, dev_cmd);
 
 	if (!iwl_mvm_bt_is_plcr_supported(mvm))
diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/iwlwifi/mvm/d3.c
index 4310cf102d78..4165d104e4c3 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/iwlwifi/mvm/d3.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -761,7 +761,7 @@ void iwl_mvm_set_last_nonqos_seq(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 static int iwl_mvm_switch_to_d3(struct iwl_mvm *mvm)
 {
-	iwl_mvm_cancel_scan(mvm);
+	iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_REGULAR, true);
 
 	iwl_trans_stop_device(mvm->trans);
 
@@ -981,7 +981,8 @@ iwl_mvm_netdetect_config(struct iwl_mvm *mvm,
 	if (ret)
 		return ret;
 
-	ret = iwl_mvm_scan_offload_start(mvm, vif, nd_config, &mvm->nd_ies);
+	ret = iwl_mvm_sched_scan_start(mvm, vif, nd_config, &mvm->nd_ies,
+				       IWL_MVM_SCAN_NETDETECT);
 	if (ret)
 		return ret;
 
@@ -1169,7 +1170,8 @@ 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 (wowlan->any) {
+	mvm->trans->wowlan_d0i3 = wowlan->any;
+	if (mvm->trans->wowlan_d0i3) {
 		/* 'any' trigger means d0i3 usage */
 		if (mvm->trans->d0i3_mode == IWL_D0I3_MODE_ON_SUSPEND) {
 			int ret = iwl_mvm_enter_d0i3_sync(mvm);
@@ -1784,7 +1786,7 @@ static void iwl_mvm_query_netdetect_reasons(struct iwl_mvm *mvm,
 	for_each_set_bit(i, &matched_profiles, mvm->n_nd_match_sets) {
 		struct iwl_scan_offload_profile_match *fw_match;
 		struct cfg80211_wowlan_nd_match *match;
-		int n_channels = 0;
+		int idx, n_channels = 0;
 
 		fw_match = &query.matches[i];
 
@@ -1799,8 +1801,12 @@ static void iwl_mvm_query_netdetect_reasons(struct iwl_mvm *mvm,
 
 		net_detect->matches[net_detect->n_matches++] = match;
 
-		match->ssid.ssid_len = mvm->nd_match_sets[i].ssid.ssid_len;
-		memcpy(match->ssid.ssid, mvm->nd_match_sets[i].ssid.ssid,
+		/* We inverted the order of the SSIDs in the scan
+		 * request, so invert the index here.
+		 */
+		idx = mvm->n_nd_match_sets - i - 1;
+		match->ssid.ssid_len = mvm->nd_match_sets[idx].ssid.ssid_len;
+		memcpy(match->ssid.ssid, mvm->nd_match_sets[idx].ssid.ssid,
 		       match->ssid.ssid_len);
 
 		if (mvm->n_nd_channels < n_channels)
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
index 5f37eab5008d..5c8a65de0e77 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -190,6 +190,21 @@ static ssize_t iwl_dbgfs_pm_params_write(struct ieee80211_vif *vif, char *buf,
 	return ret ?: count;
 }
 
+static ssize_t iwl_dbgfs_tx_pwr_lmt_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct ieee80211_vif *vif = file->private_data;
+	char buf[64];
+	int bufsz = sizeof(buf);
+	int pos;
+
+	pos = scnprintf(buf, bufsz, "bss limit = %d\n",
+			vif->bss_conf.txpower);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
 static ssize_t iwl_dbgfs_pm_params_read(struct file *file,
 					char __user *user_buf,
 					size_t count, loff_t *ppos)
@@ -607,6 +622,7 @@ static ssize_t iwl_dbgfs_rx_phyinfo_read(struct file *file,
 	} while (0)
 
 MVM_DEBUGFS_READ_FILE_OPS(mac_params);
+MVM_DEBUGFS_READ_FILE_OPS(tx_pwr_lmt);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(pm_params, 32);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(bf_params, 256);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(low_latency, 10);
@@ -641,6 +657,7 @@ void iwl_mvm_vif_dbgfs_register(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 		MVM_DEBUGFS_ADD_FILE_VIF(pm_params, mvmvif->dbgfs_dir, S_IWUSR |
 					 S_IRUSR);
 
+	MVM_DEBUGFS_ADD_FILE_VIF(tx_pwr_lmt, mvmvif->dbgfs_dir, S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE_VIF(mac_params, mvmvif->dbgfs_dir, S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE_VIF(low_latency, mvmvif->dbgfs_dir,
 				 S_IRUSR | S_IWUSR);
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
index 9ac04c1ea706..ffb4b5cef275 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -493,7 +493,8 @@ static ssize_t iwl_dbgfs_bt_notif_read(struct file *file, char __user *user_buf,
 
 	mutex_lock(&mvm->mutex);
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT)) {
+	if (!fw_has_api(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_API_BT_COEX_SPLIT)) {
 		struct iwl_bt_coex_profile_notif_old *notif =
 			&mvm->last_bt_notif_old;
 
@@ -550,7 +551,8 @@ static ssize_t iwl_dbgfs_bt_cmd_read(struct file *file, char __user *user_buf,
 
 	mutex_lock(&mvm->mutex);
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT)) {
+	if (!fw_has_api(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_API_BT_COEX_SPLIT)) {
 		struct iwl_bt_coex_ci_cmd_old *cmd = &mvm->last_bt_ci_cmd_old;
 
 		pos += scnprintf(buf+pos, bufsz-pos,
@@ -916,7 +918,8 @@ iwl_dbgfs_scan_ant_rxchain_write(struct iwl_mvm *mvm, char *buf,
 
 	if (mvm->scan_rx_ant != scan_rx_ant) {
 		mvm->scan_rx_ant = scan_rx_ant;
-		if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)
+		if (fw_has_capa(&mvm->fw->ucode_capa,
+				IWL_UCODE_TLV_CAPA_UMAC_SCAN))
 			iwl_mvm_config_scan(mvm);
 	}
 
@@ -1356,6 +1359,7 @@ static ssize_t iwl_dbgfs_d0i3_refs_read(struct file *file,
 	PRINT_MVM_REF(IWL_MVM_REF_UCODE_DOWN);
 	PRINT_MVM_REF(IWL_MVM_REF_SCAN);
 	PRINT_MVM_REF(IWL_MVM_REF_ROC);
+	PRINT_MVM_REF(IWL_MVM_REF_ROC_AUX);
 	PRINT_MVM_REF(IWL_MVM_REF_P2P_CLIENT);
 	PRINT_MVM_REF(IWL_MVM_REF_AP_IBSS);
 	PRINT_MVM_REF(IWL_MVM_REF_USER);
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
index d6cced47d561..5e4cbdb44c60 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -274,50 +274,18 @@ struct iwl_scan_offload_profile_cfg {
 } __packed;
 
 /**
- * iwl_scan_offload_schedule - schedule of scan offload
+ * iwl_scan_schedule_lmac - schedule of scan offload
  * @delay:		delay between iterations, in seconds.
  * @iterations:		num of scan iterations
  * @full_scan_mul:	number of partial scans before each full scan
  */
-struct iwl_scan_offload_schedule {
+struct iwl_scan_schedule_lmac {
 	__le16 delay;
 	u8 iterations;
 	u8 full_scan_mul;
-} __packed;
-
-/*
- * iwl_scan_offload_flags
- *
- * IWL_SCAN_OFFLOAD_FLAG_PASS_ALL: pass all results - no filtering.
- * IWL_SCAN_OFFLOAD_FLAG_CACHED_CHANNEL: add cached channels to partial scan.
- * IWL_SCAN_OFFLOAD_FLAG_EBS_QUICK_MODE: EBS duration is 100mSec - typical
- *	beacon period. Finding channel activity in this mode is not guaranteed.
- * IWL_SCAN_OFFLOAD_FLAG_EBS_ACCURATE_MODE: EBS duration is 200mSec.
- *	Assuming beacon period is 100ms finding channel activity is guaranteed.
- */
-enum iwl_scan_offload_flags {
-	IWL_SCAN_OFFLOAD_FLAG_PASS_ALL		= BIT(0),
-	IWL_SCAN_OFFLOAD_FLAG_CACHED_CHANNEL	= BIT(2),
-	IWL_SCAN_OFFLOAD_FLAG_EBS_QUICK_MODE	= BIT(5),
-	IWL_SCAN_OFFLOAD_FLAG_EBS_ACCURATE_MODE	= BIT(6),
-};
-
-/**
- * iwl_scan_offload_req - scan offload request command
- * @flags:		bitmap - enum iwl_scan_offload_flags.
- * @watchdog:		maximum scan duration in TU.
- * @delay:		delay in seconds before first iteration.
- * @schedule_line:	scan offload schedule, for fast and regular scan.
- */
-struct iwl_scan_offload_req {
-	__le16 flags;
-	__le16 watchdog;
-	__le16 delay;
-	__le16 reserved;
-	struct iwl_scan_offload_schedule schedule_line[2];
-} __packed;
+} __packed; /* SCAN_SCHEDULE_API_S */
 
-enum iwl_scan_offload_compleate_status {
+enum iwl_scan_offload_complete_status {
 	IWL_SCAN_OFFLOAD_COMPLETED	= 1,
 	IWL_SCAN_OFFLOAD_ABORTED	= 2,
 };
@@ -326,6 +294,7 @@ enum iwl_scan_ebs_status {
 	IWL_SCAN_EBS_SUCCESS,
 	IWL_SCAN_EBS_FAILED,
 	IWL_SCAN_EBS_CHAN_NOT_FOUND,
+	IWL_SCAN_EBS_INACTIVE,
 };
 
 /**
@@ -463,8 +432,19 @@ enum iwl_scan_priority {
 	IWL_SCAN_PRIORITY_HIGH,
 };
 
+enum iwl_scan_priority_ext {
+	IWL_SCAN_PRIORITY_EXT_0_LOWEST,
+	IWL_SCAN_PRIORITY_EXT_1,
+	IWL_SCAN_PRIORITY_EXT_2,
+	IWL_SCAN_PRIORITY_EXT_3,
+	IWL_SCAN_PRIORITY_EXT_4,
+	IWL_SCAN_PRIORITY_EXT_5,
+	IWL_SCAN_PRIORITY_EXT_6,
+	IWL_SCAN_PRIORITY_EXT_7_HIGHEST,
+};
+
 /**
- * iwl_scan_req_unified_lmac - SCAN_REQUEST_CMD_API_S_VER_1
+ * iwl_scan_req_lmac - SCAN_REQUEST_CMD_API_S_VER_1
  * @reserved1: for alignment and future use
  * @channel_num: num of channels to scan
  * @active-dwell: dwell time for active channels
@@ -487,7 +467,7 @@ enum iwl_scan_priority {
  * @channel_opt: channel optimization options, for full and partial scan
  * @data: channel configuration and probe request packet.
  */
-struct iwl_scan_req_unified_lmac {
+struct iwl_scan_req_lmac {
 	/* SCAN_REQUEST_FIXED_PART_API_S_VER_7 */
 	__le32 reserved1;
 	u8 n_channels;
@@ -508,7 +488,7 @@ struct iwl_scan_req_unified_lmac {
 	/* SCAN_REQ_PERIODIC_PARAMS_API_S */
 	__le32 iter_num;
 	__le32 delay;
-	struct iwl_scan_offload_schedule schedule[2];
+	struct iwl_scan_schedule_lmac schedule[2];
 	struct iwl_scan_channel_opt channel_opt[2];
 	u8 data[];
 } __packed;
@@ -582,7 +562,11 @@ struct iwl_mvm_umac_cmd_hdr {
 	u8 ver;
 } __packed;
 
-#define IWL_MVM_MAX_SIMULTANEOUS_SCANS 8
+/* The maximum of either of these cannot exceed 8, because we use an
+ * 8-bit mask (see IWL_MVM_SCAN_MASK in mvm.h).
+ */
+#define IWL_MVM_MAX_UMAC_SCANS 8
+#define IWL_MVM_MAX_LMAC_SCANS 1
 
 enum scan_config_flags {
 	SCAN_CONFIG_FLAG_ACTIVATE			= BIT(0),
@@ -865,4 +849,27 @@ struct iwl_scan_offload_profiles_query {
 	struct iwl_scan_offload_profile_match matches[IWL_SCAN_MAX_PROFILES];
 } __packed; /* SCAN_OFFLOAD_PROFILES_QUERY_RSP_S_VER_2 */
 
+/**
+ * struct iwl_umac_scan_iter_complete_notif - notifies end of scanning iteration
+ * @uid: scan id, &enum iwl_umac_scan_uid_offsets
+ * @scanned_channels: number of channels scanned and number of valid elements in
+ *	results array
+ * @status: one of SCAN_COMP_STATUS_*
+ * @bt_status: BT on/off status
+ * @last_channel: last channel that was scanned
+ * @tsf_low: TSF timer (lower half) in usecs
+ * @tsf_high: TSF timer (higher half) in usecs
+ * @results: array of scan results, only "scanned_channels" of them are valid
+ */
+struct iwl_umac_scan_iter_complete_notif {
+	__le32 uid;
+	u8 scanned_channels;
+	u8 status;
+	u8 bt_status;
+	u8 last_channel;
+	__le32 tsf_low;
+	__le32 tsf_high;
+	struct iwl_scan_results_notif results[];
+} __packed; /* SCAN_ITER_COMPLETE_NTF_UMAC_API_S_VER_1 */
+
 #endif
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api.h b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
index 01b1da6ad359..16e9ef49397f 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -108,6 +108,7 @@ enum {
 	ANTENNA_COUPLING_NOTIFICATION = 0xa,
 
 	/* UMAC scan commands */
+	SCAN_ITERATION_COMPLETE_UMAC = 0xb5,
 	SCAN_CFG_CMD = 0xc,
 	SCAN_REQ_UMAC = 0xd,
 	SCAN_ABORT_UMAC = 0xe,
@@ -147,13 +148,6 @@ enum {
 
 	LQ_CMD = 0x4e,
 
-	/* Calibration */
-	TEMPERATURE_NOTIFICATION = 0x62,
-	CALIBRATION_CFG_CMD = 0x65,
-	CALIBRATION_RES_NOTIFICATION = 0x66,
-	CALIBRATION_COMPLETE_NOTIFICATION = 0x67,
-	RADIO_VERSION_NOTIFICATION = 0x68,
-
 	/* Scan offload */
 	SCAN_OFFLOAD_REQUEST_CMD = 0x51,
 	SCAN_OFFLOAD_ABORT_CMD = 0x52,
@@ -177,12 +171,8 @@ enum {
 	/* Thermal Throttling*/
 	REPLY_THERMAL_MNG_BACKOFF = 0x7e,
 
-	/* Scanning */
-	SCAN_REQUEST_CMD = 0x80,
-	SCAN_ABORT_CMD = 0x81,
-	SCAN_START_NOTIFICATION = 0x82,
-	SCAN_RESULTS_NOTIFICATION = 0x83,
-	SCAN_COMPLETE_NOTIFICATION = 0x84,
+	/* Set/Get DC2DC frequency tune */
+	DC2DC_CONFIG_CMD = 0x83,
 
 	/* NVM */
 	NVM_ACCESS_CMD = 0x88,
@@ -1402,6 +1392,49 @@ struct iwl_mvm_marker {
 	__le32 metadata[0];
 } __packed; /* MARKER_API_S_VER_1 */
 
+/*
+ * enum iwl_dc2dc_config_id - flag ids
+ *
+ * Ids of dc2dc configuration flags
+ */
+enum iwl_dc2dc_config_id {
+	DCDC_LOW_POWER_MODE_MSK_SET  = 0x1, /* not used */
+	DCDC_FREQ_TUNE_SET = 0x2,
+}; /* MARKER_ID_API_E_VER_1 */
+
+/**
+ * struct iwl_dc2dc_config_cmd - configure dc2dc values
+ *
+ * (DC2DC_CONFIG_CMD = 0x83)
+ *
+ * Set/Get & configure dc2dc values.
+ * The command always returns the current dc2dc values.
+ *
+ * @flags: set/get dc2dc
+ * @enable_low_power_mode: not used.
+ * @dc2dc_freq_tune0: frequency divider - digital domain
+ * @dc2dc_freq_tune1: frequency divider - analog domain
+ */
+struct iwl_dc2dc_config_cmd {
+	__le32 flags;
+	__le32 enable_low_power_mode; /* not used */
+	__le32 dc2dc_freq_tune0;
+	__le32 dc2dc_freq_tune1;
+} __packed; /* DC2DC_CONFIG_CMD_API_S_VER_1 */
+
+/**
+ * struct iwl_dc2dc_config_resp - response for iwl_dc2dc_config_cmd
+ *
+ * Current dc2dc values returned by the FW.
+ *
+ * @dc2dc_freq_tune0: frequency divider - digital domain
+ * @dc2dc_freq_tune1: frequency divider - analog domain
+ */
+struct iwl_dc2dc_config_resp {
+	__le32 dc2dc_freq_tune0;
+	__le32 dc2dc_freq_tune1;
+} __packed; /* DC2DC_CONFIG_RESP_API_S_VER_1 */
+
 /***********************************
  * Smart Fifo API
  ***********************************/
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw.c b/drivers/net/wireless/iwlwifi/mvm/fw.c
index df869633f4dd..eb10c5ee4a14 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/iwlwifi/mvm/fw.c
@@ -623,7 +623,7 @@ static int iwl_mvm_config_ltr(struct iwl_mvm *mvm)
 	if (!mvm->trans->ltr_enabled)
 		return 0;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_HDC_PHASE_0))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_HDC_PHASE_0))
 		return iwl_mvm_config_ltr_v1(mvm);
 
 	return iwl_mvm_send_cmd_pdu(mvm, LTR_CONFIG, 0,
@@ -662,9 +662,9 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 		 * device that are triggered by the INIT firwmare (MFUART).
 		 */
 		_iwl_trans_stop_device(mvm->trans, false);
-		_iwl_trans_start_hw(mvm->trans, false);
+		ret = _iwl_trans_start_hw(mvm->trans, false);
 		if (ret)
-			return ret;
+			goto error;
 	}
 
 	if (iwlmvm_mod_params.init_dbg)
@@ -754,7 +754,7 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 			goto error;
 	}
 
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) {
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
 		ret = iwl_mvm_config_scan(mvm);
 		if (ret)
 			goto error;
@@ -832,21 +832,6 @@ int iwl_mvm_rx_card_state_notif(struct iwl_mvm *mvm,
 	return 0;
 }
 
-int iwl_mvm_rx_radio_ver(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
-			 struct iwl_device_cmd *cmd)
-{
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_radio_version_notif *radio_version = (void *)pkt->data;
-
-	/* TODO: what to do with that? */
-	IWL_DEBUG_INFO(mvm,
-		       "Radio version: flavor: 0x%08x, step 0x%08x, dash 0x%08x\n",
-		       le32_to_cpu(radio_version->radio_flavor),
-		       le32_to_cpu(radio_version->radio_step),
-		       le32_to_cpu(radio_version->radio_dash));
-	return 0;
-}
-
 int iwl_mvm_rx_mfuart_notif(struct iwl_mvm *mvm,
 			    struct iwl_rx_cmd_buffer *rxb,
 			    struct iwl_device_cmd *cmd)
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
index 8088c7137f7c..1812dd018af2 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
@@ -852,7 +852,7 @@ static int iwl_mvm_mac_ctxt_cmd_listener(struct iwl_mvm *mvm,
 				       MAC_FILTER_IN_BEACON |
 				       MAC_FILTER_IN_PROBE_REQUEST |
 				       MAC_FILTER_IN_CRC32);
-	mvm->hw->flags |= IEEE80211_HW_RX_INCLUDES_FCS;
+	ieee80211_hw_set(mvm->hw, RX_INCLUDES_FCS);
 
 	return iwl_mvm_mac_ctxt_send_cmd(mvm, &cmd);
 }
@@ -1270,7 +1270,7 @@ int iwl_mvm_mac_ctxt_remove(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 	mvmvif->uploaded = false;
 
 	if (vif->type == NL80211_IFTYPE_MONITOR)
-		mvm->hw->flags &= ~IEEE80211_HW_RX_INCLUDES_FCS;
+		__clear_bit(IEEE80211_HW_RX_INCLUDES_FCS, mvm->hw->flags);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index dda9f7b5f342..dfdab38e2d4a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -80,7 +80,6 @@
 #include "sta.h"
 #include "time-event.h"
 #include "iwl-eeprom-parse.h"
-#include "fw-api-scan.h"
 #include "iwl-phy-db.h"
 #include "testmode.h"
 #include "iwl-fw-error-dump.h"
@@ -319,7 +318,7 @@ struct ieee80211_regdomain *iwl_mvm_get_regdomain(struct wiphy *wiphy,
 	resp = iwl_mvm_update_mcc(mvm, alpha2, src_id);
 	if (IS_ERR_OR_NULL(resp)) {
 		IWL_DEBUG_LAR(mvm, "Could not get update from FW %d\n",
-			      PTR_RET(resp));
+			      PTR_ERR_OR_ZERO(resp));
 		goto out;
 	}
 
@@ -335,7 +334,7 @@ struct ieee80211_regdomain *iwl_mvm_get_regdomain(struct wiphy *wiphy,
 	kfree(resp);
 	if (IS_ERR_OR_NULL(regd)) {
 		IWL_DEBUG_LAR(mvm, "Could not get parse update from FW %d\n",
-			      PTR_RET(regd));
+			      PTR_ERR_OR_ZERO(regd));
 		goto out;
 	}
 
@@ -416,20 +415,27 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 {
 	struct ieee80211_hw *hw = mvm->hw;
 	int num_mac, ret, i;
+	static const u32 mvm_ciphers[] = {
+		WLAN_CIPHER_SUITE_WEP40,
+		WLAN_CIPHER_SUITE_WEP104,
+		WLAN_CIPHER_SUITE_TKIP,
+		WLAN_CIPHER_SUITE_CCMP,
+	};
 
 	/* Tell mac80211 our characteristics */
-	hw->flags = IEEE80211_HW_SIGNAL_DBM |
-		    IEEE80211_HW_SPECTRUM_MGMT |
-		    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		    IEEE80211_HW_QUEUE_CONTROL |
-		    IEEE80211_HW_WANT_MONITOR_VIF |
-		    IEEE80211_HW_SUPPORTS_PS |
-		    IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
-		    IEEE80211_HW_AMPDU_AGGREGATION |
-		    IEEE80211_HW_TIMING_BEACON_ONLY |
-		    IEEE80211_HW_CONNECTION_MONITOR |
-		    IEEE80211_HW_CHANCTX_STA_CSA |
-		    IEEE80211_HW_SUPPORTS_CLONED_SKBS;
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, SPECTRUM_MGMT);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, QUEUE_CONTROL);
+	ieee80211_hw_set(hw, WANT_MONITOR_VIF);
+	ieee80211_hw_set(hw, SUPPORTS_PS);
+	ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(hw, TIMING_BEACON_ONLY);
+	ieee80211_hw_set(hw, CONNECTION_MONITOR);
+	ieee80211_hw_set(hw, CHANCTX_STA_CSA);
+	ieee80211_hw_set(hw, SUPPORT_FAST_XMIT);
+	ieee80211_hw_set(hw, SUPPORTS_CLONED_SKBS);
 
 	hw->queues = mvm->first_agg_queue;
 	hw->offchannel_tx_hw_queue = IWL_MVM_OFFCHANNEL_QUEUE;
@@ -441,19 +447,38 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	hw->uapsd_queues = IWL_MVM_UAPSD_QUEUES;
 	hw->uapsd_max_sp_len = IWL_UAPSD_MAX_SP;
 
+	BUILD_BUG_ON(ARRAY_SIZE(mvm->ciphers) < ARRAY_SIZE(mvm_ciphers) + 2);
+	memcpy(mvm->ciphers, mvm_ciphers, sizeof(mvm_ciphers));
+	hw->wiphy->n_cipher_suites = ARRAY_SIZE(mvm_ciphers);
+	hw->wiphy->cipher_suites = mvm->ciphers;
+
 	/*
 	 * Enable 11w if advertised by firmware and software crypto
 	 * is not enabled (as the firmware will interpret some mgmt
 	 * packets, so enabling it with software crypto isn't safe)
 	 */
 	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_MFP &&
-	    !iwlwifi_mod_params.sw_crypto)
-		hw->flags |= IEEE80211_HW_MFP_CAPABLE;
+	    !iwlwifi_mod_params.sw_crypto) {
+		ieee80211_hw_set(hw, MFP_CAPABLE);
+		mvm->ciphers[hw->wiphy->n_cipher_suites] =
+			WLAN_CIPHER_SUITE_AES_CMAC;
+		hw->wiphy->n_cipher_suites++;
+	}
 
-	hw->flags |= IEEE80211_SINGLE_HW_SCAN_ON_ALL_BANDS;
+	/* currently FW API supports only one optional cipher scheme */
+	if (mvm->fw->cs[0].cipher) {
+		mvm->hw->n_cipher_schemes = 1;
+		mvm->hw->cipher_schemes = &mvm->fw->cs[0];
+		mvm->ciphers[hw->wiphy->n_cipher_suites] =
+			mvm->fw->cs[0].cipher;
+		hw->wiphy->n_cipher_suites++;
+	}
+
+	ieee80211_hw_set(hw, SINGLE_SCAN_ON_ALL_BANDS);
 	hw->wiphy->features |=
 		NL80211_FEATURE_SCHED_SCAN_RANDOM_MAC_ADDR |
-		NL80211_FEATURE_SCAN_RANDOM_MAC_ADDR;
+		NL80211_FEATURE_SCAN_RANDOM_MAC_ADDR |
+		NL80211_FEATURE_ND_RANDOM_MAC_ADDR;
 
 	hw->sta_data_size = sizeof(struct iwl_mvm_sta);
 	hw->vif_data_size = sizeof(struct iwl_mvm_vif);
@@ -506,10 +531,19 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 
 	iwl_mvm_reset_phy_ctxts(mvm);
 
-	hw->wiphy->max_scan_ie_len = iwl_mvm_max_scan_ie_len(mvm, false);
+	hw->wiphy->max_scan_ie_len = iwl_mvm_max_scan_ie_len(mvm);
 
 	hw->wiphy->max_scan_ssids = PROBE_OPTION_MAX;
 
+	BUILD_BUG_ON(IWL_MVM_SCAN_STOPPING_MASK & IWL_MVM_SCAN_MASK);
+	BUILD_BUG_ON(IWL_MVM_MAX_UMAC_SCANS > HWEIGHT32(IWL_MVM_SCAN_MASK) ||
+		     IWL_MVM_MAX_LMAC_SCANS > HWEIGHT32(IWL_MVM_SCAN_MASK));
+
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN))
+		mvm->max_scans = IWL_MVM_MAX_UMAC_SCANS;
+	else
+		mvm->max_scans = IWL_MVM_MAX_LMAC_SCANS;
+
 	if (mvm->nvm_data->bands[IEEE80211_BAND_2GHZ].n_channels)
 		hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
 			&mvm->nvm_data->bands[IEEE80211_BAND_2GHZ];
@@ -517,10 +551,10 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 		hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
 			&mvm->nvm_data->bands[IEEE80211_BAND_5GHZ];
 
-		if ((mvm->fw->ucode_capa.capa[0] &
-		     IWL_UCODE_TLV_CAPA_BEAMFORMER) &&
-		    (mvm->fw->ucode_capa.api[0] &
-		     IWL_UCODE_TLV_API_LQ_SS_PARAMS))
+		if (fw_has_capa(&mvm->fw->ucode_capa,
+				IWL_UCODE_TLV_CAPA_BEAMFORMER) &&
+		    fw_has_api(&mvm->fw->ucode_capa,
+			       IWL_UCODE_TLV_API_LQ_SS_PARAMS))
 			hw->wiphy->bands[IEEE80211_BAND_5GHZ]->vht_cap.cap |=
 				IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE;
 	}
@@ -532,14 +566,12 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	else
 		hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
-	if (IWL_UCODE_API(mvm->fw->ucode_ver) >= 10) {
-		hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_SCHED_SCAN;
-		hw->wiphy->max_sched_scan_ssids = PROBE_OPTION_MAX;
-		hw->wiphy->max_match_sets = IWL_SCAN_MAX_PROFILES;
-		/* we create the 802.11 header and zero length SSID IE. */
-		hw->wiphy->max_sched_scan_ie_len =
-			SCAN_OFFLOAD_PROBE_REQ_SIZE - 24 - 2;
-	}
+	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_SCHED_SCAN;
+	hw->wiphy->max_sched_scan_ssids = PROBE_OPTION_MAX;
+	hw->wiphy->max_match_sets = IWL_SCAN_MAX_PROFILES;
+	/* we create the 802.11 header and zero length SSID IE. */
+	hw->wiphy->max_sched_scan_ie_len =
+		SCAN_OFFLOAD_PROBE_REQ_SIZE - 24 - 2;
 
 	hw->wiphy->features |= NL80211_FEATURE_P2P_GO_CTWIN |
 			       NL80211_FEATURE_LOW_PRIORITY_SCAN |
@@ -548,30 +580,24 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 			       NL80211_FEATURE_STATIC_SMPS |
 			       NL80211_FEATURE_SUPPORTS_WMM_ADMISSION;
 
-	if (mvm->fw->ucode_capa.capa[0] &
-	    IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT)
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT))
 		hw->wiphy->features |= NL80211_FEATURE_TX_POWER_INSERTION;
-	if (mvm->fw->ucode_capa.capa[0] &
-	    IWL_UCODE_TLV_CAPA_QUIET_PERIOD_SUPPORT)
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_QUIET_PERIOD_SUPPORT))
 		hw->wiphy->features |= NL80211_FEATURE_QUIET;
 
-	if (mvm->fw->ucode_capa.capa[0] &
-	    IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT)
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT))
 		hw->wiphy->features |=
 			NL80211_FEATURE_DS_PARAM_SET_IE_IN_PROBES;
 
-	if (mvm->fw->ucode_capa.capa[0] &
-	    IWL_UCODE_TLV_CAPA_WFA_TPC_REP_IE_SUPPORT)
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_WFA_TPC_REP_IE_SUPPORT))
 		hw->wiphy->features |= NL80211_FEATURE_WFA_TPC_IE_IN_PROBES;
 
 	mvm->rts_threshold = IEEE80211_MAX_RTS_THRESHOLD;
 
-	/* currently FW API supports only one optional cipher scheme */
-	if (mvm->fw->cs[0].cipher) {
-		mvm->hw->n_cipher_schemes = 1;
-		mvm->hw->cipher_schemes = &mvm->fw->cs[0];
-	}
-
 #ifdef CONFIG_PM_SLEEP
 	if (iwl_mvm_is_d0i3_supported(mvm) &&
 	    device_can_wakeup(mvm->trans->dev)) {
@@ -611,13 +637,14 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	if (ret)
 		return ret;
 
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_TDLS_SUPPORT) {
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_TDLS_SUPPORT)) {
 		IWL_DEBUG_TDLS(mvm, "TDLS supported\n");
 		hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
 	}
 
-	if (mvm->fw->ucode_capa.capa[0] &
-	    IWL_UCODE_TLV_CAPA_TDLS_CHANNEL_SWITCH) {
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_TDLS_CHANNEL_SWITCH)) {
 		IWL_DEBUG_TDLS(mvm, "TDLS channel switch supported\n");
 		hw->wiphy->features |= NL80211_FEATURE_TDLS_CHANNEL_SWITCH;
 	}
@@ -730,6 +757,60 @@ static inline bool iwl_enable_tx_ampdu(const struct iwl_cfg *cfg)
 	return true;
 }
 
+#define CHECK_BA_TRIGGER(_mvm, _trig, _tid_bm, _tid, _fmt...)	\
+	do {							\
+		if (!(le16_to_cpu(_tid_bm) & BIT(_tid)))	\
+			break;					\
+		iwl_mvm_fw_dbg_collect_trig(_mvm, _trig, _fmt);	\
+	} while (0)
+
+static void
+iwl_mvm_ampdu_check_trigger(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			    struct ieee80211_sta *sta, u16 tid, u16 rx_ba_ssn,
+			    enum ieee80211_ampdu_mlme_action action)
+{
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_ba *ba_trig;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_BA))
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_BA);
+	ba_trig = (void *)trig->data;
+
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trig))
+		return;
+
+	switch (action) {
+	case IEEE80211_AMPDU_TX_OPERATIONAL: {
+		struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+		struct iwl_mvm_tid_data *tid_data = &mvmsta->tid_data[tid];
+
+		CHECK_BA_TRIGGER(mvm, trig, ba_trig->tx_ba_start, tid,
+				 "TX AGG START: MAC %pM tid %d ssn %d\n",
+				 sta->addr, tid, tid_data->ssn);
+		break;
+		}
+	case IEEE80211_AMPDU_TX_STOP_CONT:
+		CHECK_BA_TRIGGER(mvm, trig, ba_trig->tx_ba_stop, tid,
+				 "TX AGG STOP: MAC %pM tid %d\n",
+				 sta->addr, tid);
+		break;
+	case IEEE80211_AMPDU_RX_START:
+		CHECK_BA_TRIGGER(mvm, trig, ba_trig->rx_ba_start, tid,
+				 "RX AGG START: MAC %pM tid %d ssn %d\n",
+				 sta->addr, tid, rx_ba_ssn);
+		break;
+	case IEEE80211_AMPDU_RX_STOP:
+		CHECK_BA_TRIGGER(mvm, trig, ba_trig->rx_ba_stop, tid,
+				 "RX AGG STOP: MAC %pM tid %d\n",
+				 sta->addr, tid);
+		break;
+	default:
+		break;
+	}
+}
+
 static int iwl_mvm_mac_ampdu_action(struct ieee80211_hw *hw,
 				    struct ieee80211_vif *vif,
 				    enum ieee80211_ampdu_mlme_action action,
@@ -806,6 +887,16 @@ static int iwl_mvm_mac_ampdu_action(struct ieee80211_hw *hw,
 		ret = -EINVAL;
 		break;
 	}
+
+	if (!ret) {
+		u16 rx_ba_ssn = 0;
+
+		if (action == IEEE80211_AMPDU_RX_START)
+			rx_ba_ssn = *ssn;
+
+		iwl_mvm_ampdu_check_trigger(mvm, vif, sta, tid,
+					    rx_ba_ssn, action);
+	}
 	mutex_unlock(&mvm->mutex);
 
 	/*
@@ -1227,22 +1318,23 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 
 	iwl_trans_stop_device(mvm->trans);
 
-	mvm->scan_status = IWL_MVM_SCAN_NONE;
+	mvm->scan_status = 0;
 	mvm->ps_disabled = false;
 	mvm->calibrating = false;
 
 	/* just in case one was running */
 	ieee80211_remain_on_channel_expired(mvm->hw);
 
-	ieee80211_iterate_active_interfaces_atomic(
-		mvm->hw, IEEE80211_IFACE_ITER_RESUME_ALL,
-		iwl_mvm_cleanup_iterator, mvm);
+	/*
+	 * cleanup all interfaces, even inactive ones, as some might have
+	 * gone down during the HW restart
+	 */
+	ieee80211_iterate_interfaces(mvm->hw, 0, iwl_mvm_cleanup_iterator, mvm);
 
 	mvm->p2p_device_vif = NULL;
 	mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
 
 	iwl_mvm_reset_phy_ctxts(mvm);
-	memset(mvm->fw_key_table, 0, sizeof(mvm->fw_key_table));
 	memset(mvm->sta_drained, 0, sizeof(mvm->sta_drained));
 	memset(mvm->tfd_drained, 0, sizeof(mvm->tfd_drained));
 	memset(&mvm->last_bt_notif, 0, sizeof(mvm->last_bt_notif));
@@ -1404,7 +1496,7 @@ void __iwl_mvm_mac_stop(struct iwl_mvm *mvm)
 	 * The work item could be running or queued if the
 	 * ROC time event stops just as we get here.
 	 */
-	cancel_work_sync(&mvm->roc_done_wk);
+	flush_work(&mvm->roc_done_wk);
 
 	iwl_trans_stop_device(mvm->trans);
 
@@ -1417,20 +1509,24 @@ void __iwl_mvm_mac_stop(struct iwl_mvm *mvm)
 	/*
 	 * Clear IN_HW_RESTART flag when stopping the hw (as restart_complete()
 	 * won't be called in this case).
+	 * But make sure to cleanup interfaces that have gone down before/during
+	 * HW restart was requested.
 	 */
-	clear_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status);
+	if (test_and_clear_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
+		ieee80211_iterate_interfaces(mvm->hw, 0,
+					     iwl_mvm_cleanup_iterator, mvm);
 
 	/* We shouldn't have any UIDs still set.  Loop over all the UIDs to
 	 * make sure there's nothing left there and warn if any is found.
 	 */
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) {
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
 		int i;
 
-		for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++) {
-			if (WARN_ONCE(mvm->scan_uid[i],
-				      "UMAC scan UID %d was not cleaned\n",
-				      mvm->scan_uid[i]))
-				mvm->scan_uid[i] = 0;
+		for (i = 0; i < mvm->max_scans; i++) {
+			if (WARN_ONCE(mvm->scan_uid_status[i],
+				      "UMAC scan UID %d status was not cleaned\n",
+				      i))
+				mvm->scan_uid_status[i] = 0;
 		}
 	}
 
@@ -1495,7 +1591,7 @@ static int iwl_mvm_set_tx_power(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 		.pwr_restriction = cpu_to_le16(8 * tx_power),
 	};
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_TX_POWER_DEV))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_TX_POWER_DEV))
 		return iwl_mvm_set_tx_power_old(mvm, vif, tx_power);
 
 	if (tx_power == IWL_DEFAULT_MAX_TX_POWER)
@@ -2354,7 +2450,7 @@ static void iwl_mvm_bss_info_changed(struct ieee80211_hw *hw,
 	mutex_lock(&mvm->mutex);
 
 	if (changes & BSS_CHANGED_IDLE && !bss_conf->idle)
-		iwl_mvm_scan_offload_stop(mvm, true);
+		iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_SCHED, true);
 
 	switch (vif->type) {
 	case NL80211_IFTYPE_STATION:
@@ -2373,89 +2469,21 @@ static void iwl_mvm_bss_info_changed(struct ieee80211_hw *hw,
 	iwl_mvm_unref(mvm, IWL_MVM_REF_BSS_CHANGED);
 }
 
-static int iwl_mvm_cancel_scan_wait_notif(struct iwl_mvm *mvm,
-					  enum iwl_scan_status scan_type)
-{
-	int ret;
-	bool wait_for_handlers = false;
-
-	mutex_lock(&mvm->mutex);
-
-	if (mvm->scan_status != scan_type) {
-		ret = 0;
-		/* make sure there are no pending notifications */
-		wait_for_handlers = true;
-		goto out;
-	}
-
-	switch (scan_type) {
-	case IWL_MVM_SCAN_SCHED:
-		ret = iwl_mvm_scan_offload_stop(mvm, true);
-		break;
-	case IWL_MVM_SCAN_OS:
-		ret = iwl_mvm_cancel_scan(mvm);
-		break;
-	case IWL_MVM_SCAN_NONE:
-	default:
-		WARN_ON_ONCE(1);
-		ret = -EINVAL;
-		break;
-	}
-	if (ret)
-		goto out;
-
-	wait_for_handlers = true;
-out:
-	mutex_unlock(&mvm->mutex);
-
-	/* make sure we consume the completion notification */
-	if (wait_for_handlers)
-		iwl_mvm_wait_for_async_handlers(mvm);
-
-	return ret;
-}
 static int iwl_mvm_mac_hw_scan(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif,
 			       struct ieee80211_scan_request *hw_req)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
-	struct cfg80211_scan_request *req = &hw_req->req;
 	int ret;
 
-	if (req->n_channels == 0 ||
-	    req->n_channels > mvm->fw->ucode_capa.n_scan_channels)
+	if (hw_req->req.n_channels == 0 ||
+	    hw_req->req.n_channels > mvm->fw->ucode_capa.n_scan_channels)
 		return -EINVAL;
 
-	if (!(mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
-		ret = iwl_mvm_cancel_scan_wait_notif(mvm, IWL_MVM_SCAN_SCHED);
-		if (ret)
-			return ret;
-	}
-
 	mutex_lock(&mvm->mutex);
-
-	if (iwl_mvm_is_lar_supported(mvm) && !mvm->lar_regdom_set) {
-		IWL_ERR(mvm, "scan while LAR regdomain is not set\n");
-		ret = -EBUSY;
-		goto out;
-	}
-
-	if (mvm->scan_status != IWL_MVM_SCAN_NONE) {
-		ret = -EBUSY;
-		goto out;
-	}
-
-	iwl_mvm_ref(mvm, IWL_MVM_REF_SCAN);
-
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)
-		ret = iwl_mvm_scan_umac(mvm, vif, hw_req);
-	else
-		ret = iwl_mvm_unified_scan_lmac(mvm, vif, hw_req);
-
-	if (ret)
-		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
-out:
+	ret = iwl_mvm_reg_scan_start(mvm, vif, &hw_req->req, &hw_req->ies);
 	mutex_unlock(&mvm->mutex);
+
 	return ret;
 }
 
@@ -2473,12 +2501,8 @@ static void iwl_mvm_mac_cancel_hw_scan(struct ieee80211_hw *hw,
 	 * cancel scan scan before ieee80211_scan_work() could run.
 	 * To handle that, simply return if the scan is not running.
 	*/
-	/* FIXME: for now, we ignore this race for UMAC scans, since
-	 * they don't set the scan_status.
-	 */
-	if ((mvm->scan_status == IWL_MVM_SCAN_OS) ||
-	    (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN))
-		iwl_mvm_cancel_scan(mvm);
+	if (mvm->scan_status & IWL_MVM_SCAN_REGULAR)
+		iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_REGULAR, true);
 
 	mutex_unlock(&mvm->mutex);
 }
@@ -2794,35 +2818,17 @@ static int iwl_mvm_mac_sched_scan_start(struct ieee80211_hw *hw,
 					struct ieee80211_scan_ies *ies)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
-	int ret;
 
-	if (!(mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
-		ret = iwl_mvm_cancel_scan_wait_notif(mvm, IWL_MVM_SCAN_OS);
-		if (ret)
-			return ret;
-	}
+	int ret;
 
 	mutex_lock(&mvm->mutex);
 
-	if (iwl_mvm_is_lar_supported(mvm) && !mvm->lar_regdom_set) {
-		IWL_ERR(mvm, "sched-scan while LAR regdomain is not set\n");
-		ret = -EBUSY;
-		goto out;
-	}
-
 	if (!vif->bss_conf.idle) {
 		ret = -EBUSY;
 		goto out;
 	}
 
-	if (mvm->scan_status != IWL_MVM_SCAN_NONE) {
-		ret = -EBUSY;
-		goto out;
-	}
-
-	ret = iwl_mvm_scan_offload_start(mvm, vif, req, ies);
-	if (ret)
-		mvm->scan_status = IWL_MVM_SCAN_NONE;
+	ret = iwl_mvm_sched_scan_start(mvm, vif, req, ies, IWL_MVM_SCAN_SCHED);
 
 out:
 	mutex_unlock(&mvm->mutex);
@@ -2845,16 +2851,12 @@ static int iwl_mvm_mac_sched_scan_stop(struct ieee80211_hw *hw,
 	 * could run.  To handle this, simply return if the scan is
 	 * not running.
 	*/
-	/* FIXME: for now, we ignore this race for UMAC scans, since
-	 * they don't set the scan_status.
-	 */
-	if (mvm->scan_status != IWL_MVM_SCAN_SCHED &&
-	    !(mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
+	if (!(mvm->scan_status & IWL_MVM_SCAN_SCHED)) {
 		mutex_unlock(&mvm->mutex);
 		return 0;
 	}
 
-	ret = iwl_mvm_scan_offload_stop(mvm, false);
+	ret = iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_SCHED, false);
 	mutex_unlock(&mvm->mutex);
 	iwl_mvm_wait_for_async_handlers(mvm);
 
@@ -2883,7 +2885,7 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
 		break;
 	case WLAN_CIPHER_SUITE_AES_CMAC:
-		WARN_ON_ONCE(!(hw->flags & IEEE80211_HW_MFP_CAPABLE));
+		WARN_ON_ONCE(!ieee80211_hw_check(hw, MFP_CAPABLE));
 		break;
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104:
@@ -2922,8 +2924,21 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 			break;
 		}
 
+		/* During FW restart, in order to restore the state as it was,
+		 * don't try to reprogram keys we previously failed for.
+		 */
+		if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) &&
+		    key->hw_key_idx == STA_KEY_IDX_INVALID) {
+			IWL_DEBUG_MAC80211(mvm,
+					   "skip invalid idx key programming during restart\n");
+			ret = 0;
+			break;
+		}
+
 		IWL_DEBUG_MAC80211(mvm, "set hwcrypto key\n");
-		ret = iwl_mvm_set_sta_key(mvm, vif, sta, key, false);
+		ret = iwl_mvm_set_sta_key(mvm, vif, sta, key,
+					  test_bit(IWL_MVM_STATUS_IN_HW_RESTART,
+						   &mvm->status));
 		if (ret) {
 			IWL_WARN(mvm, "set key failed\n");
 			/*
@@ -3001,7 +3016,7 @@ static bool iwl_mvm_rx_aux_roc(struct iwl_notif_wait_data *notif_wait,
 	return true;
 }
 
-#define AUX_ROC_MAX_DELAY_ON_CHANNEL 5000
+#define AUX_ROC_MAX_DELAY_ON_CHANNEL 200
 static int iwl_mvm_send_aux_roc_cmd(struct iwl_mvm *mvm,
 				    struct ieee80211_channel *channel,
 				    struct ieee80211_vif *vif,
@@ -3106,8 +3121,8 @@ static int iwl_mvm_roc(struct ieee80211_hw *hw,
 
 	switch (vif->type) {
 	case NL80211_IFTYPE_STATION:
-		if (mvm->fw->ucode_capa.capa[0] &
-		    IWL_UCODE_TLV_CAPA_HOTSPOT_SUPPORT) {
+		if (fw_has_capa(&mvm->fw->ucode_capa,
+				IWL_UCODE_TLV_CAPA_HOTSPOT_SUPPORT)) {
 			/* Use aux roc framework (HS20) */
 			ret = iwl_mvm_send_aux_roc_cmd(mvm, channel,
 						       vif, duration);
@@ -3899,7 +3914,7 @@ static int iwl_mvm_mac_get_survey(struct ieee80211_hw *hw, int idx,
 	if (idx != 0)
 		return -ENOENT;
 
-	if (!(mvm->fw->ucode_capa.capa[0] &
+	if (fw_has_capa(&mvm->fw->ucode_capa,
 			IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS))
 		return -ENOENT;
 
@@ -3946,8 +3961,8 @@ static void iwl_mvm_mac_sta_statistics(struct ieee80211_hw *hw,
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 
-	if (!(mvm->fw->ucode_capa.capa[0] &
-				IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS))
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_RADIO_BEACON_STATS))
 		return;
 
 	/* if beacon filtering isn't on mac80211 does it anyway */
@@ -3977,9 +3992,9 @@ static void iwl_mvm_mac_sta_statistics(struct ieee80211_hw *hw,
 	mutex_unlock(&mvm->mutex);
 }
 
-static void iwl_mvm_mac_event_callback(struct ieee80211_hw *hw,
-				       struct ieee80211_vif *vif,
-				       const struct ieee80211_event *event)
+static void iwl_mvm_event_mlme_callback(struct iwl_mvm *mvm,
+					struct ieee80211_vif *vif,
+					const struct ieee80211_event *event)
 {
 #define CHECK_MLME_TRIGGER(_mvm, _trig, _buf, _cnt, _fmt...)	\
 	do {							\
@@ -3988,7 +4003,6 @@ static void iwl_mvm_mac_event_callback(struct ieee80211_hw *hw,
 		iwl_mvm_fw_dbg_collect_trig(_mvm, _trig, _fmt);\
 	} while (0)
 
-	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 	struct iwl_fw_dbg_trigger_tlv *trig;
 	struct iwl_fw_dbg_trigger_mlme *trig_mlme;
 
@@ -4032,6 +4046,75 @@ static void iwl_mvm_mac_event_callback(struct ieee80211_hw *hw,
 #undef CHECK_MLME_TRIGGER
 }
 
+static void iwl_mvm_event_bar_rx_callback(struct iwl_mvm *mvm,
+					  struct ieee80211_vif *vif,
+					  const struct ieee80211_event *event)
+{
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_ba *ba_trig;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_BA))
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_BA);
+	ba_trig = (void *)trig->data;
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trig))
+		return;
+
+	if (!(le16_to_cpu(ba_trig->rx_bar) & BIT(event->u.ba.tid)))
+		return;
+
+	iwl_mvm_fw_dbg_collect_trig(mvm, trig,
+				    "BAR received from %pM, tid %d, ssn %d",
+				    event->u.ba.sta->addr, event->u.ba.tid,
+				    event->u.ba.ssn);
+}
+
+static void
+iwl_mvm_event_frame_timeout_callback(struct iwl_mvm *mvm,
+				     struct ieee80211_vif *vif,
+				     const struct ieee80211_event *event)
+{
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_ba *ba_trig;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_BA))
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_BA);
+	ba_trig = (void *)trig->data;
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trig))
+		return;
+
+	if (!(le16_to_cpu(ba_trig->frame_timeout) & BIT(event->u.ba.tid)))
+		return;
+
+	iwl_mvm_fw_dbg_collect_trig(mvm, trig,
+				    "Frame from %pM timed out, tid %d",
+				    event->u.ba.sta->addr, event->u.ba.tid);
+}
+
+static void iwl_mvm_mac_event_callback(struct ieee80211_hw *hw,
+				       struct ieee80211_vif *vif,
+				       const struct ieee80211_event *event)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+
+	switch (event->type) {
+	case MLME_EVENT:
+		iwl_mvm_event_mlme_callback(mvm, vif, event);
+		break;
+	case BAR_RX_EVENT:
+		iwl_mvm_event_bar_rx_callback(mvm, vif, event);
+		break;
+	case BA_FRAME_TIMEOUT:
+		iwl_mvm_event_frame_timeout_callback(mvm, vif, event);
+		break;
+	default:
+		break;
+	}
+}
+
 const struct ieee80211_ops iwl_mvm_hw_ops = {
 	.tx = iwl_mvm_mac_tx,
 	.ampdu_action = iwl_mvm_mac_ampdu_action,
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index cf70f681d1ac..2d4bad5fe825 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -76,6 +76,7 @@
 #include "iwl-notif-wait.h"
 #include "iwl-eeprom-parse.h"
 #include "iwl-fw-file.h"
+#include "iwl-config.h"
 #include "sta.h"
 #include "fw-api.h"
 #include "constants.h"
@@ -275,6 +276,7 @@ enum iwl_mvm_ref_type {
 	IWL_MVM_REF_UCODE_DOWN,
 	IWL_MVM_REF_SCAN,
 	IWL_MVM_REF_ROC,
+	IWL_MVM_REF_ROC_AUX,
 	IWL_MVM_REF_P2P_CLIENT,
 	IWL_MVM_REF_AP_IBSS,
 	IWL_MVM_REF_USER,
@@ -445,10 +447,26 @@ iwl_mvm_vif_from_mac80211(struct ieee80211_vif *vif)
 
 extern const u8 tid_to_mac80211_ac[];
 
+#define IWL_MVM_SCAN_STOPPING_SHIFT	8
+
 enum iwl_scan_status {
-	IWL_MVM_SCAN_NONE,
-	IWL_MVM_SCAN_OS,
-	IWL_MVM_SCAN_SCHED,
+	IWL_MVM_SCAN_REGULAR		= BIT(0),
+	IWL_MVM_SCAN_SCHED		= BIT(1),
+	IWL_MVM_SCAN_NETDETECT		= BIT(2),
+
+	IWL_MVM_SCAN_STOPPING_REGULAR	= BIT(8),
+	IWL_MVM_SCAN_STOPPING_SCHED	= BIT(9),
+	IWL_MVM_SCAN_STOPPING_NETDETECT	= BIT(10),
+
+	IWL_MVM_SCAN_REGULAR_MASK	= IWL_MVM_SCAN_REGULAR |
+					  IWL_MVM_SCAN_STOPPING_REGULAR,
+	IWL_MVM_SCAN_SCHED_MASK		= IWL_MVM_SCAN_SCHED |
+					  IWL_MVM_SCAN_STOPPING_SCHED,
+	IWL_MVM_SCAN_NETDETECT_MASK	= IWL_MVM_SCAN_NETDETECT |
+					  IWL_MVM_SCAN_STOPPING_NETDETECT,
+
+	IWL_MVM_SCAN_STOPPING_MASK	= 0xff << IWL_MVM_SCAN_STOPPING_SHIFT,
+	IWL_MVM_SCAN_MASK		= 0xff,
 };
 
 /**
@@ -463,49 +481,6 @@ struct iwl_nvm_section {
 	const u8 *data;
 };
 
-/*
- * Tx-backoff threshold
- * @temperature: The threshold in Celsius
- * @backoff: The tx-backoff in uSec
- */
-struct iwl_tt_tx_backoff {
-	s32 temperature;
-	u32 backoff;
-};
-
-#define TT_TX_BACKOFF_SIZE 6
-
-/**
- * struct iwl_tt_params - thermal throttling parameters
- * @ct_kill_entry: CT Kill entry threshold
- * @ct_kill_exit: CT Kill exit threshold
- * @ct_kill_duration: The time  intervals (in uSec) in which the driver needs
- *	to checks whether to exit CT Kill.
- * @dynamic_smps_entry: Dynamic SMPS entry threshold
- * @dynamic_smps_exit: Dynamic SMPS exit threshold
- * @tx_protection_entry: TX protection entry threshold
- * @tx_protection_exit: TX protection exit threshold
- * @tx_backoff: Array of thresholds for tx-backoff , in ascending order.
- * @support_ct_kill: Support CT Kill?
- * @support_dynamic_smps: Support dynamic SMPS?
- * @support_tx_protection: Support tx protection?
- * @support_tx_backoff: Support tx-backoff?
- */
-struct iwl_tt_params {
-	s32 ct_kill_entry;
-	s32 ct_kill_exit;
-	u32 ct_kill_duration;
-	s32 dynamic_smps_entry;
-	s32 dynamic_smps_exit;
-	s32 tx_protection_entry;
-	s32 tx_protection_exit;
-	struct iwl_tt_tx_backoff tx_backoff[TT_TX_BACKOFF_SIZE];
-	bool support_ct_kill;
-	bool support_dynamic_smps;
-	bool support_tx_protection;
-	bool support_tx_backoff;
-};
-
 /**
  * struct iwl_mvm_tt_mgnt - Thermal Throttling Management structure
  * @ct_kill_exit: worker to exit thermal kill
@@ -520,7 +495,7 @@ struct iwl_mvm_tt_mgmt {
 	bool dynamic_smps;
 	u32 tx_backoff;
 	u32 min_backoff;
-	const struct iwl_tt_params *params;
+	struct iwl_tt_params params;
 	bool throttle;
 };
 
@@ -647,13 +622,15 @@ struct iwl_mvm {
 	u32 rts_threshold;
 
 	/* Scan status, cmd (pre-allocated) and auxiliary station */
-	enum iwl_scan_status scan_status;
+	unsigned int scan_status;
 	void *scan_cmd;
 	struct iwl_mcast_filter_cmd *mcast_filter_cmd;
 
+	/* max number of simultaneous scans the FW supports */
+	unsigned int max_scans;
+
 	/* UMAC scan tracking */
-	u32 scan_uid[IWL_MVM_MAX_SIMULTANEOUS_SCANS];
-	u8 scan_seq_num, sched_scan_seq_num;
+	u32 scan_uid_status[IWL_MVM_MAX_UMAC_SCANS];
 
 	/* rx chain antennas set through debugfs for the scan command */
 	u8 scan_rx_ant;
@@ -843,6 +820,8 @@ struct iwl_mvm {
 	} tdls_cs;
 
 	struct iwl_mvm_shared_mem_cfg shared_mem_cfg;
+
+	u32 ciphers[6];
 };
 
 /* Extract MVM priv from op_mode and _hw */
@@ -912,14 +891,15 @@ 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);
+	       fw_has_capa(&mvm->fw->ucode_capa,
+			   IWL_UCODE_TLV_CAPA_D0I3_SUPPORT);
 }
 
 static inline bool iwl_mvm_is_lar_supported(struct iwl_mvm *mvm)
 {
 	bool nvm_lar = mvm->nvm_data->lar_enabled;
-	bool tlv_lar = mvm->fw->ucode_capa.capa[0] &
-		IWL_UCODE_TLV_CAPA_LAR_SUPPORT;
+	bool tlv_lar = fw_has_capa(&mvm->fw->ucode_capa,
+				   IWL_UCODE_TLV_CAPA_LAR_SUPPORT);
 
 	if (iwlwifi_mod_params.lar_disable)
 		return false;
@@ -936,24 +916,28 @@ static inline bool iwl_mvm_is_lar_supported(struct iwl_mvm *mvm)
 
 static inline bool iwl_mvm_is_wifi_mcc_supported(struct iwl_mvm *mvm)
 {
-	return mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_WIFI_MCC_UPDATE ||
-	       mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_LAR_MULTI_MCC;
+	return fw_has_api(&mvm->fw->ucode_capa,
+			  IWL_UCODE_TLV_API_WIFI_MCC_UPDATE) ||
+	       fw_has_capa(&mvm->fw->ucode_capa,
+			   IWL_UCODE_TLV_CAPA_LAR_MULTI_MCC);
 }
 
 static inline bool iwl_mvm_is_scd_cfg_supported(struct iwl_mvm *mvm)
 {
-	return mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_SCD_CFG;
+	return fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_SCD_CFG);
 }
 
 static inline bool iwl_mvm_bt_is_plcr_supported(struct iwl_mvm *mvm)
 {
-	return (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_BT_COEX_PLCR) &&
+	return fw_has_capa(&mvm->fw->ucode_capa,
+			   IWL_UCODE_TLV_CAPA_BT_COEX_PLCR) &&
 		IWL_MVM_BT_COEX_CORUNNING;
 }
 
 static inline bool iwl_mvm_bt_is_rrc_supported(struct iwl_mvm *mvm)
 {
-	return (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_BT_COEX_RRC) &&
+	return fw_has_capa(&mvm->fw->ucode_capa,
+			   IWL_UCODE_TLV_CAPA_BT_COEX_RRC) &&
 		IWL_MVM_BT_COEX_RRC;
 }
 
@@ -1083,8 +1067,6 @@ int iwl_mvm_rx_tx_cmd(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 		      struct iwl_device_cmd *cmd);
 int iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 			struct iwl_device_cmd *cmd);
-int iwl_mvm_rx_radio_ver(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
-			 struct iwl_device_cmd *cmd);
 int iwl_mvm_rx_ant_coupling_notif(struct iwl_mvm *mvm,
 				  struct iwl_rx_cmd_buffer *rxb,
 				  struct iwl_device_cmd *cmd);
@@ -1093,8 +1075,6 @@ int iwl_mvm_rx_fw_error(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 int iwl_mvm_rx_card_state_notif(struct iwl_mvm *mvm,
 				struct iwl_rx_cmd_buffer *rxb,
 				struct iwl_device_cmd *cmd);
-int iwl_mvm_rx_radio_ver(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
-			 struct iwl_device_cmd *cmd);
 int iwl_mvm_rx_mfuart_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 			    struct iwl_device_cmd *cmd);
 int iwl_mvm_rx_shared_mem_cfg_notif(struct iwl_mvm *mvm,
@@ -1146,48 +1126,38 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm, bool force_upload,
 			  struct ieee80211_vif *disabled_vif);
 
 /* Scanning */
+int iwl_mvm_reg_scan_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			   struct cfg80211_scan_request *req,
+			   struct ieee80211_scan_ies *ies);
 int iwl_mvm_scan_size(struct iwl_mvm *mvm);
-int iwl_mvm_cancel_scan(struct iwl_mvm *mvm);
-int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm, bool is_sched_scan);
+int iwl_mvm_scan_stop(struct iwl_mvm *mvm, int type, bool notify);
+int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm);
 void iwl_mvm_report_scan_aborted(struct iwl_mvm *mvm);
 
 /* Scheduled scan */
-int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
-					   struct iwl_rx_cmd_buffer *rxb,
-					   struct iwl_device_cmd *cmd);
-int iwl_mvm_rx_scan_offload_iter_complete_notif(struct iwl_mvm *mvm,
-						struct iwl_rx_cmd_buffer *rxb,
-						struct iwl_device_cmd *cmd);
-int iwl_mvm_config_sched_scan_profiles(struct iwl_mvm *mvm,
-				       struct cfg80211_sched_scan_request *req);
-int iwl_mvm_scan_offload_start(struct iwl_mvm *mvm,
-			       struct ieee80211_vif *vif,
-			       struct cfg80211_sched_scan_request *req,
-			       struct ieee80211_scan_ies *ies);
-int iwl_mvm_scan_offload_stop(struct iwl_mvm *mvm, bool notify);
-int iwl_mvm_rx_scan_offload_results(struct iwl_mvm *mvm,
-				    struct iwl_rx_cmd_buffer *rxb,
-				    struct iwl_device_cmd *cmd);
-
-/* Unified scan */
-int iwl_mvm_unified_scan_lmac(struct iwl_mvm *mvm,
-			      struct ieee80211_vif *vif,
-			      struct ieee80211_scan_request *req);
-int iwl_mvm_unified_sched_scan_lmac(struct iwl_mvm *mvm,
-				    struct ieee80211_vif *vif,
-				    struct cfg80211_sched_scan_request *req,
-				    struct ieee80211_scan_ies *ies);
+int iwl_mvm_rx_lmac_scan_complete_notif(struct iwl_mvm *mvm,
+					struct iwl_rx_cmd_buffer *rxb,
+					struct iwl_device_cmd *cmd);
+int iwl_mvm_rx_lmac_scan_iter_complete_notif(struct iwl_mvm *mvm,
+					     struct iwl_rx_cmd_buffer *rxb,
+					     struct iwl_device_cmd *cmd);
+int iwl_mvm_sched_scan_start(struct iwl_mvm *mvm,
+			     struct ieee80211_vif *vif,
+			     struct cfg80211_sched_scan_request *req,
+			     struct ieee80211_scan_ies *ies,
+			     int type);
+int iwl_mvm_rx_scan_match_found(struct iwl_mvm *mvm,
+				struct iwl_rx_cmd_buffer *rxb,
+				struct iwl_device_cmd *cmd);
 
 /* UMAC scan */
 int iwl_mvm_config_scan(struct iwl_mvm *mvm);
-int iwl_mvm_scan_umac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-		      struct ieee80211_scan_request *req);
-int iwl_mvm_sched_scan_umac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			    struct cfg80211_sched_scan_request *req,
-			    struct ieee80211_scan_ies *ies);
 int iwl_mvm_rx_umac_scan_complete_notif(struct iwl_mvm *mvm,
 					struct iwl_rx_cmd_buffer *rxb,
 					struct iwl_device_cmd *cmd);
+int iwl_mvm_rx_umac_scan_iter_complete_notif(struct iwl_mvm *mvm,
+					     struct iwl_rx_cmd_buffer *rxb,
+					     struct iwl_device_cmd *cmd);
 
 /* MVM debugfs */
 #ifdef CONFIG_IWLWIFI_DEBUGFS
diff --git a/drivers/net/wireless/iwlwifi/mvm/nvm.c b/drivers/net/wireless/iwlwifi/mvm/nvm.c
index 87b2a30a2308..2a6be350704a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/nvm.c
+++ b/drivers/net/wireless/iwlwifi/mvm/nvm.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -316,8 +316,8 @@ iwl_parse_nvm_sections(struct iwl_mvm *mvm)
 	phy_sku = (const __le16 *)sections[NVM_SECTION_TYPE_PHY_SKU].data;
 
 	lar_enabled = !iwlwifi_mod_params.lar_disable &&
-		      (mvm->fw->ucode_capa.capa[0] &
-		       IWL_UCODE_TLV_CAPA_LAR_SUPPORT);
+		      fw_has_capa(&mvm->fw->ucode_capa,
+				  IWL_UCODE_TLV_CAPA_LAR_SUPPORT);
 
 	return iwl_parse_nvm_data(mvm->trans->dev, mvm->cfg, hw, sw, calib,
 				  regulatory, mac_override, phy_sku,
@@ -583,9 +583,9 @@ int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic)
 		kfree(nvm_buffer);
 	}
 
-	/* load external NVM if configured */
+	/* Only if PNVM selected in the mod param - load external NVM  */
 	if (mvm->nvm_file_name) {
-		/* read External NVM file - take the default */
+		/* read External NVM file from the mod param */
 		ret = iwl_mvm_read_external_nvm(mvm);
 		if (ret) {
 			/* choose the nvm_file name according to the
@@ -792,8 +792,8 @@ int iwl_mvm_init_mcc(struct iwl_mvm *mvm)
 	char mcc[3];
 
 	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
-		tlv_lar = mvm->fw->ucode_capa.capa[0] &
-			IWL_UCODE_TLV_CAPA_LAR_SUPPORT;
+		tlv_lar = fw_has_capa(&mvm->fw->ucode_capa,
+				      IWL_UCODE_TLV_CAPA_LAR_SUPPORT);
 		nvm_lar = mvm->nvm_data->lar_enabled;
 		if (tlv_lar != nvm_lar)
 			IWL_INFO(mvm,
diff --git a/drivers/net/wireless/iwlwifi/mvm/ops.c b/drivers/net/wireless/iwlwifi/mvm/ops.c
index 2ea01238754e..e4fa50075ffd 100644
--- a/drivers/net/wireless/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/iwlwifi/mvm/ops.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -194,7 +194,7 @@ static void iwl_mvm_nic_config(struct iwl_op_mode *op_mode)
 	 * (PCIe power is lost before PERST# is asserted), causing ME FW
 	 * to lose ownership and not being able to obtain it back.
 	 */
-	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+	if (!mvm->trans->cfg->apmg_not_supported)
 		iwl_set_bits_mask_prph(mvm->trans, APMG_PS_CTRL_REG,
 				       APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
 				       ~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
@@ -238,15 +238,16 @@ static const struct iwl_rx_handlers iwl_mvm_rx_handlers[] = {
 	RX_HANDLER(EOSP_NOTIFICATION, iwl_mvm_rx_eosp_notif, false),
 
 	RX_HANDLER(SCAN_ITERATION_COMPLETE,
-		   iwl_mvm_rx_scan_offload_iter_complete_notif, false),
+		   iwl_mvm_rx_lmac_scan_iter_complete_notif, false),
 	RX_HANDLER(SCAN_OFFLOAD_COMPLETE,
-		   iwl_mvm_rx_scan_offload_complete_notif, true),
-	RX_HANDLER(MATCH_FOUND_NOTIFICATION, iwl_mvm_rx_scan_offload_results,
+		   iwl_mvm_rx_lmac_scan_complete_notif, true),
+	RX_HANDLER(MATCH_FOUND_NOTIFICATION, iwl_mvm_rx_scan_match_found,
 		   false),
 	RX_HANDLER(SCAN_COMPLETE_UMAC, iwl_mvm_rx_umac_scan_complete_notif,
 		   true),
+	RX_HANDLER(SCAN_ITERATION_COMPLETE_UMAC,
+		   iwl_mvm_rx_umac_scan_iter_complete_notif, false),
 
-	RX_HANDLER(RADIO_VERSION_NOTIFICATION, iwl_mvm_rx_radio_ver, false),
 	RX_HANDLER(CARD_STATE_NOTIFICATION, iwl_mvm_rx_card_state_notif, false),
 
 	RX_HANDLER(MISSED_BEACONS_NOTIFICATION, iwl_mvm_rx_missed_beacons_notif,
@@ -280,17 +281,11 @@ static const char *const iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(BINDING_CONTEXT_CMD),
 	CMD(TIME_QUOTA_CMD),
 	CMD(NON_QOS_TX_COUNTER_CMD),
-	CMD(RADIO_VERSION_NOTIFICATION),
-	CMD(SCAN_REQUEST_CMD),
-	CMD(SCAN_ABORT_CMD),
-	CMD(SCAN_START_NOTIFICATION),
-	CMD(SCAN_RESULTS_NOTIFICATION),
-	CMD(SCAN_COMPLETE_NOTIFICATION),
+	CMD(DC2DC_CONFIG_CMD),
 	CMD(NVM_ACCESS_CMD),
 	CMD(PHY_CONFIGURATION_CMD),
 	CMD(CALIB_RES_NOTIF_PHY_DB),
 	CMD(SET_CALIB_DEFAULT_CMD),
-	CMD(CALIBRATION_COMPLETE_NOTIFICATION),
 	CMD(ADD_STA_KEY),
 	CMD(ADD_STA),
 	CMD(REMOVE_STA),
@@ -359,6 +354,7 @@ static const char *const iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(TDLS_CHANNEL_SWITCH_NOTIFICATION),
 	CMD(TDLS_CONFIG_CMD),
 	CMD(MCC_UPDATE_CMD),
+	CMD(SCAN_ITERATION_COMPLETE_UMAC),
 };
 #undef CMD
 
@@ -520,15 +516,12 @@ 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 if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
-		if (CSR_HW_REV_STEP(trans->hw_rev) == SILICON_B_STEP)
-			mvm->nvm_file_name = mvm->cfg->default_nvm_file_B_step;
-		else
-			mvm->nvm_file_name = mvm->cfg->default_nvm_file_C_step;
-	}
+	else
+		IWL_DEBUG_EEPROM(mvm->trans->dev,
+				 "working without external nvm file\n");
 
 	if (WARN(cfg->no_power_up_nic_in_init && !mvm->nvm_file_name,
 		 "not allowing power-up and not having nvm_file\n"))
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 33cd68ae7bf9..daff1d0a8e4a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -1,7 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -138,7 +138,7 @@ struct rs_tx_column;
 
 typedef bool (*allow_column_func_t) (struct iwl_mvm *mvm,
 				     struct ieee80211_sta *sta,
-				     struct iwl_scale_tbl_info *tbl,
+				     struct rs_rate *rate,
 				     const struct rs_tx_column *next_col);
 
 struct rs_tx_column {
@@ -150,14 +150,14 @@ struct rs_tx_column {
 };
 
 static bool rs_ant_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
-			 struct iwl_scale_tbl_info *tbl,
+			 struct rs_rate *rate,
 			 const struct rs_tx_column *next_col)
 {
 	return iwl_mvm_bt_coex_is_ant_avail(mvm, next_col->ant);
 }
 
 static bool rs_mimo_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
-			  struct iwl_scale_tbl_info *tbl,
+			  struct rs_rate *rate,
 			  const struct rs_tx_column *next_col)
 {
 	struct iwl_mvm_sta *mvmsta;
@@ -187,7 +187,7 @@ static bool rs_mimo_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 }
 
 static bool rs_siso_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
-			  struct iwl_scale_tbl_info *tbl,
+			  struct rs_rate *rate,
 			  const struct rs_tx_column *next_col)
 {
 	if (!sta->ht_cap.ht_supported)
@@ -197,10 +197,9 @@ static bool rs_siso_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 }
 
 static bool rs_sgi_allow(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
-			 struct iwl_scale_tbl_info *tbl,
+			 struct rs_rate *rate,
 			 const struct rs_tx_column *next_col)
 {
-	struct rs_rate *rate = &tbl->rate;
 	struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
 	struct ieee80211_sta_vht_cap *vht_cap = &sta->vht_cap;
 
@@ -1128,8 +1127,8 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	u32 tx_resp_hwrate = (uintptr_t)info->status.status_driver_data[1];
 	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 	struct iwl_lq_sta *lq_sta = &mvmsta->lq_sta;
-	bool allow_ant_mismatch = mvm->fw->ucode_capa.api[0] &
-		IWL_UCODE_TLV_API_LQ_SS_PARAMS;
+	bool allow_ant_mismatch = fw_has_api(&mvm->fw->ucode_capa,
+					     IWL_UCODE_TLV_API_LQ_SS_PARAMS);
 
 	/* Treat uninitialized rate scaling data same as non-existing. */
 	if (!lq_sta) {
@@ -1659,7 +1658,8 @@ static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 
 		for (j = 0; j < MAX_COLUMN_CHECKS; j++) {
 			allow_func = next_col->checks[j];
-			if (allow_func && !allow_func(mvm, sta, tbl, next_col))
+			if (allow_func && !allow_func(mvm, sta, &tbl->rate,
+						      next_col))
 				break;
 		}
 
@@ -2136,7 +2136,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 	}
 
 	/* current tx rate */
-	index = lq_sta->last_txrate_idx;
+	index = rate->index;
 
 	/* rates available for this association, and for modulation mode */
 	rate_mask = rs_get_supported_rates(lq_sta, rate);
@@ -2184,14 +2184,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 		 * or search for a new one? */
 		rs_stay_in_table(lq_sta, false);
 
-		goto out;
-	}
-	/* Else we have enough samples; calculate estimate of
-	 * actual average throughput */
-	if (window->average_tpt != ((window->success_ratio *
-			tbl->expected_tpt[index] + 64) / 128)) {
-		window->average_tpt = ((window->success_ratio *
-					tbl->expected_tpt[index] + 64) / 128);
+		return;
 	}
 
 	/* If we are searching for better modulation mode, check success. */
@@ -2403,9 +2396,6 @@ lq_update:
 			rs_set_stay_in_table(mvm, 0, lq_sta);
 		}
 	}
-
-out:
-	lq_sta->last_txrate_idx = index;
 }
 
 struct rs_init_rate_info {
@@ -2548,7 +2538,6 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
 	rate = &tbl->rate;
 
 	rs_get_initial_rate(mvm, lq_sta, band, rate);
-	lq_sta->last_txrate_idx = rate->index;
 
 	WARN_ON_ONCE(rate->ant != ANT_A && rate->ant != ANT_B);
 	if (rate->ant == ANT_A)
@@ -2725,7 +2714,7 @@ static void rs_vht_init(struct iwl_mvm *mvm,
 	    (vht_cap->cap & IEEE80211_VHT_CAP_RXSTBC_MASK))
 		lq_sta->stbc_capable = true;
 
-	if ((mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_BEAMFORMER) &&
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_BEAMFORMER) &&
 	    (num_of_ant(iwl_mvm_get_valid_tx_ant(mvm)) > 1) &&
 	    (vht_cap->cap & IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE))
 		lq_sta->bfer_capable = true;
@@ -3009,7 +2998,7 @@ static void rs_build_rates_table(struct iwl_mvm *mvm,
 	valid_tx_ant = iwl_mvm_get_valid_tx_ant(mvm);
 
 	/* TODO: remove old API when min FW API hits 14 */
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LQ_SS_PARAMS) &&
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_LQ_SS_PARAMS) &&
 	    rs_stbc_allow(mvm, sta, lq_sta))
 		rate.stbc = true;
 
@@ -3223,12 +3212,9 @@ static void rs_fill_lq_cmd(struct iwl_mvm *mvm,
 
 	rs_build_rates_table(mvm, sta, lq_sta, initial_rate);
 
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LQ_SS_PARAMS)
+	if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_LQ_SS_PARAMS))
 		rs_set_lq_ss_params(mvm, sta, lq_sta, initial_rate);
 
-	if (num_of_ant(initial_rate->ant) == 1)
-		lq_cmd->single_stream_ant_msk = initial_rate->ant;
-
 	mvmsta = iwl_mvm_sta_from_mac80211(sta);
 	mvmvif = iwl_mvm_vif_from_mac80211(mvmsta->vif);
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.h b/drivers/net/wireless/iwlwifi/mvm/rs.h
index e4aa9346a231..2a3da314305a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.h
@@ -322,8 +322,6 @@ struct iwl_lq_sta {
 	struct iwl_scale_tbl_info lq_info[LQ_SIZE]; /* "active", "search" */
 	u8 tx_agg_tid_en;
 
-	/* used to be in sta_info */
-	int last_txrate_idx;
 	/* last tx rate_n_flags */
 	u32 last_rate_n_flags;
 	/* packets destined for this STA are aggregated */
diff --git a/drivers/net/wireless/iwlwifi/mvm/rx.c b/drivers/net/wireless/iwlwifi/mvm/rx.c
index d6314ddf57b5..8f1d93b7a13a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rx.c
@@ -570,7 +570,7 @@ void iwl_mvm_handle_rx_statistics(struct iwl_mvm *mvm,
 	};
 	u32 temperature;
 
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_STATS_V10) {
+	if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_STATS_V10)) {
 		struct iwl_notif_statistics_v10 *stats = (void *)&pkt->data;
 
 		if (iwl_rx_packet_payload_len(pkt) != v10_len)
@@ -610,7 +610,7 @@ void iwl_mvm_handle_rx_statistics(struct iwl_mvm *mvm,
 	/* Only handle rx statistics temperature changes if async temp
 	 * notifications are not supported
 	 */
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_ASYNC_DTM))
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_ASYNC_DTM))
 		iwl_mvm_tt_temp_changed(mvm, temperature);
 
 	ieee80211_iterate_active_interfaces(mvm->hw,
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index 1075a213bd6a..5de144968723 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -67,11 +67,8 @@
 #include <net/mac80211.h>
 
 #include "mvm.h"
-#include "iwl-eeprom-parse.h"
 #include "fw-api-scan.h"
 
-#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
 
@@ -79,23 +76,31 @@ struct iwl_mvm_scan_params {
 	u32 max_out_time;
 	u32 suspend_time;
 	bool passive_fragmented;
+	u32 n_channels;
+	u16 delay;
+	int n_ssids;
+	struct cfg80211_ssid *ssids;
+	struct ieee80211_channel **channels;
+	u16 interval; /* interval between scans (in secs) */
+	u32 flags;
+	u8 *mac_addr;
+	u8 *mac_addr_mask;
+	bool no_cck;
+	bool pass_all;
+	int n_match_sets;
+	struct iwl_scan_probe_req preq;
+	struct cfg80211_match_set *match_sets;
 	struct _dwell {
 		u16 passive;
 		u16 active;
 		u16 fragmented;
 	} dwell[IEEE80211_NUM_BANDS];
+	struct {
+		u8 iterations;
+		u8 full_scan_mul; /* not used for UMAC */
+	} schedule[2];
 };
 
-enum iwl_umac_scan_uid_type {
-	IWL_UMAC_SCAN_UID_REG_SCAN	= BIT(0),
-	IWL_UMAC_SCAN_UID_SCHED_SCAN	= BIT(1),
-	IWL_UMAC_SCAN_UID_ALL		= IWL_UMAC_SCAN_UID_REG_SCAN |
-					  IWL_UMAC_SCAN_UID_SCHED_SCAN,
-};
-
-static int iwl_umac_scan_stop(struct iwl_mvm *mvm,
-			      enum iwl_umac_scan_uid_type type, bool notify);
-
 static u8 iwl_mvm_scan_rx_ant(struct iwl_mvm *mvm)
 {
 	if (mvm->scan_rx_ant != ANT_NONE)
@@ -143,28 +148,6 @@ iwl_mvm_scan_rate_n_flags(struct iwl_mvm *mvm, enum ieee80211_band band,
 }
 
 /*
- * We insert the SSIDs in an inverted order, because the FW will
- * invert it back. The most prioritized SSID, which is first in the
- * request list, is not copied here, but inserted directly to the probe
- * request.
- */
-static void iwl_mvm_scan_fill_ssids(struct iwl_ssid_ie *cmd_ssid,
-				    struct cfg80211_ssid *ssids,
-				    int n_ssids, int first)
-{
-	int fw_idx, req_idx;
-
-	for (req_idx = n_ssids - 1, fw_idx = 0; req_idx >= first;
-	     req_idx--, fw_idx++) {
-		cmd_ssid[fw_idx].id = WLAN_EID_SSID;
-		cmd_ssid[fw_idx].len = ssids[req_idx].ssid_len;
-		memcpy(cmd_ssid[fw_idx].ssid,
-		       ssids[req_idx].ssid,
-		       ssids[req_idx].ssid_len);
-	}
-}
-
-/*
  * If req->n_ssids > 0, it means we should do an active scan.
  * In case of active scan w/o directed scan, we receive a zero-length SSID
  * just to notify that this scan is active and not passive.
@@ -177,7 +160,7 @@ static void iwl_mvm_scan_fill_ssids(struct iwl_ssid_ie *cmd_ssid,
 static u16 iwl_mvm_get_active_dwell(struct iwl_mvm *mvm,
 				    enum ieee80211_band band, int n_ssids)
 {
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BASIC_DWELL)
+	if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BASIC_DWELL))
 		return 10;
 	if (band == IEEE80211_BAND_2GHZ)
 		return 20  + 3 * (n_ssids + 1);
@@ -187,7 +170,7 @@ static u16 iwl_mvm_get_active_dwell(struct iwl_mvm *mvm,
 static u16 iwl_mvm_get_passive_dwell(struct iwl_mvm *mvm,
 				     enum ieee80211_band band)
 {
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BASIC_DWELL)
+	if (fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_BASIC_DWELL))
 			return 110;
 	return band == IEEE80211_BAND_2GHZ ? 100 + 20 : 100 + 10;
 }
@@ -203,10 +186,9 @@ static void iwl_mvm_scan_condition_iterator(void *data, u8 *mac,
 		*global_cnt += 1;
 }
 
-static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
-				     struct ieee80211_vif *vif,
-				     int n_ssids, u32 flags,
-				     struct iwl_mvm_scan_params *params)
+static void iwl_mvm_scan_calc_dwell(struct iwl_mvm *mvm,
+				    struct ieee80211_vif *vif,
+				    struct iwl_mvm_scan_params *params)
 {
 	int global_cnt = 0;
 	enum ieee80211_band band;
@@ -216,7 +198,6 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 					    IEEE80211_IFACE_ITER_NORMAL,
 					    iwl_mvm_scan_condition_iterator,
 					    &global_cnt);
-
 	if (!global_cnt)
 		goto not_bound;
 
@@ -224,8 +205,9 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 	params->max_out_time = 120;
 
 	if (iwl_mvm_low_latency(mvm)) {
-		if (mvm->fw->ucode_capa.api[0] &
-		    IWL_UCODE_TLV_API_FRAGMENTED_SCAN) {
+		if (fw_has_api(&mvm->fw->ucode_capa,
+			       IWL_UCODE_TLV_API_FRAGMENTED_SCAN)) {
+
 			params->suspend_time = 105;
 			/*
 			 * If there is more than one active interface make
@@ -239,8 +221,9 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 		}
 	}
 
-	if (frag_passive_dwell && (mvm->fw->ucode_capa.api[0] &
-				   IWL_UCODE_TLV_API_FRAGMENTED_SCAN)) {
+	if (frag_passive_dwell &&
+	    fw_has_api(&mvm->fw->ucode_capa,
+		       IWL_UCODE_TLV_API_FRAGMENTED_SCAN)) {
 		/*
 		 * P2P device scan should not be fragmented to avoid negative
 		 * impact on P2P device discovery. Configure max_out_time to be
@@ -257,7 +240,8 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 		}
 	}
 
-	if (flags & NL80211_SCAN_FLAG_LOW_PRIORITY)
+	if ((params->flags & NL80211_SCAN_FLAG_LOW_PRIORITY) &&
+	    (params->max_out_time > 200))
 		params->max_out_time = 200;
 
 not_bound:
@@ -268,20 +252,34 @@ not_bound:
 
 		params->dwell[band].passive = iwl_mvm_get_passive_dwell(mvm,
 									band);
-		params->dwell[band].active = iwl_mvm_get_active_dwell(mvm, band,
-								      n_ssids);
+		params->dwell[band].active =
+			iwl_mvm_get_active_dwell(mvm, band, params->n_ssids);
 	}
+
+	IWL_DEBUG_SCAN(mvm,
+		       "scan parameters: max_out_time %d, suspend_time %d, passive_fragmented %d\n",
+		       params->max_out_time, params->suspend_time,
+		       params->passive_fragmented);
+	IWL_DEBUG_SCAN(mvm,
+		       "dwell[IEEE80211_BAND_2GHZ]: passive %d, active %d, fragmented %d\n",
+		       params->dwell[IEEE80211_BAND_2GHZ].passive,
+		       params->dwell[IEEE80211_BAND_2GHZ].active,
+		       params->dwell[IEEE80211_BAND_2GHZ].fragmented);
+	IWL_DEBUG_SCAN(mvm,
+		       "dwell[IEEE80211_BAND_5GHZ]: passive %d, active %d, fragmented %d\n",
+		       params->dwell[IEEE80211_BAND_5GHZ].passive,
+		       params->dwell[IEEE80211_BAND_5GHZ].active,
+		       params->dwell[IEEE80211_BAND_5GHZ].fragmented);
 }
 
 static inline bool iwl_mvm_rrm_scan_needed(struct iwl_mvm *mvm)
 {
 	/* require rrm scan whenever the fw supports it */
-	return mvm->fw->ucode_capa.capa[0] &
-	       IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT;
+	return fw_has_capa(&mvm->fw->ucode_capa,
+			   IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT);
 }
 
-static int iwl_mvm_max_scan_ie_fw_cmd_room(struct iwl_mvm *mvm,
-					   bool is_sched_scan)
+static int iwl_mvm_max_scan_ie_fw_cmd_room(struct iwl_mvm *mvm)
 {
 	int max_probe_len;
 
@@ -297,9 +295,9 @@ static int iwl_mvm_max_scan_ie_fw_cmd_room(struct iwl_mvm *mvm,
 	return max_probe_len;
 }
 
-int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm, bool is_sched_scan)
+int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm)
 {
-	int max_ie_len = iwl_mvm_max_scan_ie_fw_cmd_room(mvm, is_sched_scan);
+	int max_ie_len = iwl_mvm_max_scan_ie_fw_cmd_room(mvm);
 
 	/* TODO: [BUG] This function should return the maximum allowed size of
 	 * scan IEs, however the LMAC scan api contains both 2GHZ and 5GHZ IEs
@@ -314,22 +312,41 @@ int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm, bool is_sched_scan)
 	return max_ie_len;
 }
 
-int iwl_mvm_rx_scan_offload_iter_complete_notif(struct iwl_mvm *mvm,
-						struct iwl_rx_cmd_buffer *rxb,
-						struct iwl_device_cmd *cmd)
+static u8 *iwl_mvm_dump_channel_list(struct iwl_scan_results_notif *res,
+				     int num_res, u8 *buf, size_t buf_size)
+{
+	int i;
+	u8 *pos = buf, *end = buf + buf_size;
+
+	for (i = 0; pos < end && i < num_res; i++)
+		pos += snprintf(pos, end - pos, " %u", res[i].channel);
+
+	/* terminate the string in case the buffer was too short */
+	*(buf + buf_size - 1) = '\0';
+
+	return buf;
+}
+
+int iwl_mvm_rx_lmac_scan_iter_complete_notif(struct iwl_mvm *mvm,
+					     struct iwl_rx_cmd_buffer *rxb,
+					     struct iwl_device_cmd *cmd)
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_lmac_scan_complete_notif *notif = (void *)pkt->data;
+	u8 buf[256];
 
 	IWL_DEBUG_SCAN(mvm,
-		       "Scan offload iteration complete: status=0x%x scanned channels=%d\n",
-		       notif->status, notif->scanned_channels);
+		       "Scan offload iteration complete: status=0x%x scanned channels=%d channels list: %s\n",
+		       notif->status, notif->scanned_channels,
+		       iwl_mvm_dump_channel_list(notif->results,
+						 notif->scanned_channels, buf,
+						 sizeof(buf)));
 	return 0;
 }
 
-int iwl_mvm_rx_scan_offload_results(struct iwl_mvm *mvm,
-				    struct iwl_rx_cmd_buffer *rxb,
-				    struct iwl_device_cmd *cmd)
+int iwl_mvm_rx_scan_match_found(struct iwl_mvm *mvm,
+				struct iwl_rx_cmd_buffer *rxb,
+				struct iwl_device_cmd *cmd)
 {
 	IWL_DEBUG_SCAN(mvm, "Scheduled scan results\n");
 	ieee80211_sched_scan_results(mvm->hw);
@@ -337,41 +354,78 @@ int iwl_mvm_rx_scan_offload_results(struct iwl_mvm *mvm,
 	return 0;
 }
 
-int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
-					   struct iwl_rx_cmd_buffer *rxb,
-					   struct iwl_device_cmd *cmd)
+static const char *iwl_mvm_ebs_status_str(enum iwl_scan_ebs_status status)
 {
-	struct iwl_rx_packet *pkt = rxb_addr(rxb);
-	struct iwl_periodic_scan_complete *scan_notif;
+	switch (status) {
+	case IWL_SCAN_EBS_SUCCESS:
+		return "successful";
+	case IWL_SCAN_EBS_INACTIVE:
+		return "inactive";
+	case IWL_SCAN_EBS_FAILED:
+	case IWL_SCAN_EBS_CHAN_NOT_FOUND:
+	default:
+		return "failed";
+	}
+}
 
-	scan_notif = (void *)pkt->data;
+int iwl_mvm_rx_lmac_scan_complete_notif(struct iwl_mvm *mvm,
+					struct iwl_rx_cmd_buffer *rxb,
+					struct iwl_device_cmd *cmd)
+{
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_periodic_scan_complete *scan_notif = (void *)pkt->data;
+	bool aborted = (scan_notif->status == IWL_SCAN_OFFLOAD_ABORTED);
 
 	/* scan status must be locked for proper checking */
 	lockdep_assert_held(&mvm->mutex);
 
-	IWL_DEBUG_SCAN(mvm,
-		       "%s completed, status %s, EBS status %s\n",
-		       mvm->scan_status == IWL_MVM_SCAN_SCHED ?
-				"Scheduled scan" : "Scan",
-		       scan_notif->status == IWL_SCAN_OFFLOAD_COMPLETED ?
-				"completed" : "aborted",
-		       scan_notif->ebs_status == IWL_SCAN_EBS_SUCCESS ?
-				"success" : "failed");
+	/* We first check if we were stopping a scan, in which case we
+	 * just clear the stopping flag.  Then we check if it was a
+	 * firmware initiated stop, in which case we need to inform
+	 * mac80211.
+	 * Note that we can have a stopping and a running scan
+	 * simultaneously, but we can't have two different types of
+	 * scans stopping or running at the same time (since LMAC
+	 * doesn't support it).
+	 */
+
+	if (mvm->scan_status & IWL_MVM_SCAN_STOPPING_SCHED) {
+		WARN_ON_ONCE(mvm->scan_status & IWL_MVM_SCAN_STOPPING_REGULAR);
+
+		IWL_DEBUG_SCAN(mvm, "Scheduled scan %s, EBS status %s\n",
+			       aborted ? "aborted" : "completed",
+			       iwl_mvm_ebs_status_str(scan_notif->ebs_status));
 
+		mvm->scan_status &= ~IWL_MVM_SCAN_STOPPING_SCHED;
+	} else if (mvm->scan_status & IWL_MVM_SCAN_STOPPING_REGULAR) {
+		IWL_DEBUG_SCAN(mvm, "Regular scan %s, EBS status %s\n",
+			       aborted ? "aborted" : "completed",
+			       iwl_mvm_ebs_status_str(scan_notif->ebs_status));
 
-	/* only call mac80211 completion if the stop was initiated by FW */
-	if (mvm->scan_status == IWL_MVM_SCAN_SCHED) {
-		mvm->scan_status = IWL_MVM_SCAN_NONE;
+		mvm->scan_status &= ~IWL_MVM_SCAN_STOPPING_REGULAR;
+	} else if (mvm->scan_status & IWL_MVM_SCAN_SCHED) {
+		WARN_ON_ONCE(mvm->scan_status & IWL_MVM_SCAN_REGULAR);
+
+		IWL_DEBUG_SCAN(mvm, "Scheduled scan %s, EBS status %s (FW)\n",
+			       aborted ? "aborted" : "completed",
+			       iwl_mvm_ebs_status_str(scan_notif->ebs_status));
+
+		mvm->scan_status &= ~IWL_MVM_SCAN_SCHED;
 		ieee80211_sched_scan_stopped(mvm->hw);
-	} else if (mvm->scan_status == IWL_MVM_SCAN_OS) {
-		mvm->scan_status = IWL_MVM_SCAN_NONE;
+	} else if (mvm->scan_status & IWL_MVM_SCAN_REGULAR) {
+		IWL_DEBUG_SCAN(mvm, "Regular scan %s, EBS status %s (FW)\n",
+			       aborted ? "aborted" : "completed",
+			       iwl_mvm_ebs_status_str(scan_notif->ebs_status));
+
+		mvm->scan_status &= ~IWL_MVM_SCAN_REGULAR;
 		ieee80211_scan_completed(mvm->hw,
 				scan_notif->status == IWL_SCAN_OFFLOAD_ABORTED);
 		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
 	}
 
-	if (scan_notif->ebs_status)
-		mvm->last_ebs_successful = false;
+	mvm->last_ebs_successful =
+			scan_notif->ebs_status == IWL_SCAN_EBS_SUCCESS ||
+			scan_notif->ebs_status == IWL_SCAN_EBS_INACTIVE;
 
 	return 0;
 }
@@ -390,9 +444,12 @@ static int iwl_ssid_exist(u8 *ssid, u8 ssid_len, struct iwl_ssid_ie *ssid_list)
 	return -1;
 }
 
-static void iwl_scan_offload_build_ssid(struct cfg80211_sched_scan_request *req,
-					struct iwl_ssid_ie *direct_scan,
-					u32 *ssid_bitmap, bool basic_ssid)
+/* We insert the SSIDs in an inverted order, because the FW will
+ * invert it back.
+ */
+static void iwl_scan_build_ssids(struct iwl_mvm_scan_params *params,
+				 struct iwl_ssid_ie *ssids,
+				 u32 *ssid_bitmap)
 {
 	int i, j;
 	int index;
@@ -402,39 +459,41 @@ static void iwl_scan_offload_build_ssid(struct cfg80211_sched_scan_request *req,
 	 * iwl_config_sched_scan_profiles() uses the order of these ssids to
 	 * config match list.
 	 */
-	for (i = 0; i < req->n_match_sets && i < PROBE_OPTION_MAX; i++) {
+	for (i = 0, j = params->n_match_sets - 1;
+	     j >= 0 && i < PROBE_OPTION_MAX;
+	     i++, j--) {
 		/* skip empty SSID matchsets */
-		if (!req->match_sets[i].ssid.ssid_len)
+		if (!params->match_sets[j].ssid.ssid_len)
 			continue;
-		direct_scan[i].id = WLAN_EID_SSID;
-		direct_scan[i].len = req->match_sets[i].ssid.ssid_len;
-		memcpy(direct_scan[i].ssid, req->match_sets[i].ssid.ssid,
-		       direct_scan[i].len);
+		ssids[i].id = WLAN_EID_SSID;
+		ssids[i].len = params->match_sets[j].ssid.ssid_len;
+		memcpy(ssids[i].ssid, params->match_sets[j].ssid.ssid,
+		       ssids[i].len);
 	}
 
 	/* add SSIDs from scan SSID list */
 	*ssid_bitmap = 0;
-	for (j = 0; j < req->n_ssids && i < PROBE_OPTION_MAX; j++) {
-		index = iwl_ssid_exist(req->ssids[j].ssid,
-				       req->ssids[j].ssid_len,
-				       direct_scan);
+	for (j = params->n_ssids - 1;
+	     j >= 0 && i < PROBE_OPTION_MAX;
+	     i++, j--) {
+		index = iwl_ssid_exist(params->ssids[j].ssid,
+				       params->ssids[j].ssid_len,
+				       ssids);
 		if (index < 0) {
-			if (!req->ssids[j].ssid_len && basic_ssid)
-				continue;
-			direct_scan[i].id = WLAN_EID_SSID;
-			direct_scan[i].len = req->ssids[j].ssid_len;
-			memcpy(direct_scan[i].ssid, req->ssids[j].ssid,
-			       direct_scan[i].len);
-			*ssid_bitmap |= BIT(i + 1);
-			i++;
+			ssids[i].id = WLAN_EID_SSID;
+			ssids[i].len = params->ssids[j].ssid_len;
+			memcpy(ssids[i].ssid, params->ssids[j].ssid,
+			       ssids[i].len);
+			*ssid_bitmap |= BIT(i);
 		} else {
-			*ssid_bitmap |= BIT(index + 1);
+			*ssid_bitmap |= BIT(index);
 		}
 	}
 }
 
-int iwl_mvm_config_sched_scan_profiles(struct iwl_mvm *mvm,
-				       struct cfg80211_sched_scan_request *req)
+static int
+iwl_mvm_config_sched_scan_profiles(struct iwl_mvm *mvm,
+				   struct cfg80211_sched_scan_request *req)
 {
 	struct iwl_scan_offload_profile *profile;
 	struct iwl_scan_offload_profile_cfg *profile_cfg;
@@ -515,30 +574,7 @@ static bool iwl_mvm_scan_pass_all(struct iwl_mvm *mvm,
 	return true;
 }
 
-int iwl_mvm_scan_offload_start(struct iwl_mvm *mvm,
-			       struct ieee80211_vif *vif,
-			       struct cfg80211_sched_scan_request *req,
-			       struct ieee80211_scan_ies *ies)
-{
-	int ret;
-
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) {
-		ret = iwl_mvm_config_sched_scan_profiles(mvm, req);
-		if (ret)
-			return ret;
-		ret = iwl_mvm_sched_scan_umac(mvm, vif, req, ies);
-	} else {
-		mvm->scan_status = IWL_MVM_SCAN_SCHED;
-		ret = iwl_mvm_config_sched_scan_profiles(mvm, req);
-		if (ret)
-			return ret;
-		ret = iwl_mvm_unified_sched_scan_lmac(mvm, vif, req, ies);
-	}
-
-	return ret;
-}
-
-static int iwl_mvm_send_scan_offload_abort(struct iwl_mvm *mvm)
+static int iwl_mvm_lmac_scan_abort(struct iwl_mvm *mvm)
 {
 	int ret;
 	struct iwl_host_cmd cmd = {
@@ -546,12 +582,6 @@ static int iwl_mvm_send_scan_offload_abort(struct iwl_mvm *mvm)
 	};
 	u32 status;
 
-	/* Exit instantly with error when device is not ready
-	 * to receive scan abort command or it does not perform
-	 * scheduled scan currently */
-	if (mvm->scan_status == IWL_MVM_SCAN_NONE)
-		return -EIO;
-
 	ret = iwl_mvm_send_cmd_status(mvm, &cmd, &status);
 	if (ret)
 		return ret;
@@ -571,69 +601,9 @@ static int iwl_mvm_send_scan_offload_abort(struct iwl_mvm *mvm)
 	return ret;
 }
 
-int iwl_mvm_scan_offload_stop(struct iwl_mvm *mvm, bool notify)
-{
-	int ret;
-	struct iwl_notification_wait wait_scan_done;
-	static const u8 scan_done_notif[] = { SCAN_OFFLOAD_COMPLETE, };
-	bool sched = mvm->scan_status == IWL_MVM_SCAN_SCHED;
-
-	lockdep_assert_held(&mvm->mutex);
-
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)
-		return iwl_umac_scan_stop(mvm, IWL_UMAC_SCAN_UID_SCHED_SCAN,
-					  notify);
-
-	if (mvm->scan_status == IWL_MVM_SCAN_NONE)
-		return 0;
-
-	if (iwl_mvm_is_radio_killed(mvm)) {
-		ret = 0;
-		goto out;
-	}
-
-	iwl_init_notification_wait(&mvm->notif_wait, &wait_scan_done,
-				   scan_done_notif,
-				   ARRAY_SIZE(scan_done_notif),
-				   NULL, NULL);
-
-	ret = iwl_mvm_send_scan_offload_abort(mvm);
-	if (ret) {
-		IWL_DEBUG_SCAN(mvm, "Send stop %sscan failed %d\n",
-			       sched ? "offloaded " : "", ret);
-		iwl_remove_notification(&mvm->notif_wait, &wait_scan_done);
-		goto out;
-	}
-
-	IWL_DEBUG_SCAN(mvm, "Successfully sent stop %sscan\n",
-		       sched ? "offloaded " : "");
-
-	ret = iwl_wait_notification(&mvm->notif_wait, &wait_scan_done, 1 * HZ);
-out:
-	/*
-	 * Clear the scan status so the next scan requests will succeed. This
-	 * also ensures the Rx handler doesn't do anything, as the scan was
-	 * stopped from above. Since the rx handler won't do anything now,
-	 * we have to release the scan reference here.
-	 */
-	if (mvm->scan_status == IWL_MVM_SCAN_OS)
-		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
-
-	mvm->scan_status = IWL_MVM_SCAN_NONE;
-
-	if (notify) {
-		if (sched)
-			ieee80211_sched_scan_stopped(mvm->hw);
-		else
-			ieee80211_scan_completed(mvm->hw, true);
-	}
-
-	return ret;
-}
-
-static void iwl_mvm_unified_scan_fill_tx_cmd(struct iwl_mvm *mvm,
-					     struct iwl_scan_req_tx_cmd *tx_cmd,
-					     bool no_cck)
+static void iwl_mvm_scan_fill_tx_cmd(struct iwl_mvm *mvm,
+				     struct iwl_scan_req_tx_cmd *tx_cmd,
+				     bool no_cck)
 {
 	tx_cmd[0].tx_flags = cpu_to_le32(TX_CMD_FLG_SEQ_CTL |
 					 TX_CMD_FLG_BT_DIS);
@@ -654,7 +624,7 @@ static void
 iwl_mvm_lmac_scan_cfg_channels(struct iwl_mvm *mvm,
 			       struct ieee80211_channel **channels,
 			       int n_channels, u32 ssid_bitmap,
-			       struct iwl_scan_req_unified_lmac *cmd)
+			       struct iwl_scan_req_lmac *cmd)
 {
 	struct iwl_scan_channel_cfg_lmac *channel_cfg = (void *)&cmd->data;
 	int i;
@@ -707,13 +677,14 @@ static u8 *iwl_mvm_copy_and_insert_ds_elem(struct iwl_mvm *mvm, const u8 *ies,
 }
 
 static void
-iwl_mvm_build_unified_scan_probe(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-				 struct ieee80211_scan_ies *ies,
-				 struct iwl_scan_probe_req *preq,
-				 const u8 *mac_addr, const u8 *mac_addr_mask)
+iwl_mvm_build_scan_probe(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			 struct ieee80211_scan_ies *ies,
+			 struct iwl_mvm_scan_params *params)
 {
-	struct ieee80211_mgmt *frame = (struct ieee80211_mgmt *)preq->buf;
+	struct ieee80211_mgmt *frame = (void *)params->preq.buf;
 	u8 *pos, *newpos;
+	const u8 *mac_addr = params->flags & NL80211_SCAN_FLAG_RANDOM_ADDR ?
+		params->mac_addr : NULL;
 
 	/*
 	 * Unfortunately, right now the offload scan doesn't support randomising
@@ -722,7 +693,8 @@ iwl_mvm_build_unified_scan_probe(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	 * random, only when it's restarted, but at least that helps a bit.
 	 */
 	if (mac_addr)
-		get_random_mask_addr(frame->sa, mac_addr, mac_addr_mask);
+		get_random_mask_addr(frame->sa, mac_addr,
+				     params->mac_addr_mask);
 	else
 		memcpy(frame->sa, vif->addr, ETH_ALEN);
 
@@ -735,245 +707,167 @@ iwl_mvm_build_unified_scan_probe(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	*pos++ = WLAN_EID_SSID;
 	*pos++ = 0;
 
-	preq->mac_header.offset = 0;
-	preq->mac_header.len = cpu_to_le16(24 + 2);
+	params->preq.mac_header.offset = 0;
+	params->preq.mac_header.len = cpu_to_le16(24 + 2);
 
 	/* Insert ds parameter set element on 2.4 GHz band */
 	newpos = iwl_mvm_copy_and_insert_ds_elem(mvm,
 						 ies->ies[IEEE80211_BAND_2GHZ],
 						 ies->len[IEEE80211_BAND_2GHZ],
 						 pos);
-	preq->band_data[0].offset = cpu_to_le16(pos - preq->buf);
-	preq->band_data[0].len = cpu_to_le16(newpos - pos);
+	params->preq.band_data[0].offset = cpu_to_le16(pos - params->preq.buf);
+	params->preq.band_data[0].len = cpu_to_le16(newpos - pos);
 	pos = newpos;
 
 	memcpy(pos, ies->ies[IEEE80211_BAND_5GHZ],
 	       ies->len[IEEE80211_BAND_5GHZ]);
-	preq->band_data[1].offset = cpu_to_le16(pos - preq->buf);
-	preq->band_data[1].len = cpu_to_le16(ies->len[IEEE80211_BAND_5GHZ]);
+	params->preq.band_data[1].offset = cpu_to_le16(pos - params->preq.buf);
+	params->preq.band_data[1].len =
+		cpu_to_le16(ies->len[IEEE80211_BAND_5GHZ]);
 	pos += ies->len[IEEE80211_BAND_5GHZ];
 
 	memcpy(pos, ies->common_ies, ies->common_ie_len);
-	preq->common_data.offset = cpu_to_le16(pos - preq->buf);
-	preq->common_data.len = cpu_to_le16(ies->common_ie_len);
+	params->preq.common_data.offset = cpu_to_le16(pos - params->preq.buf);
+	params->preq.common_data.len = cpu_to_le16(ies->common_ie_len);
 }
 
-static void
-iwl_mvm_build_generic_unified_scan_cmd(struct iwl_mvm *mvm,
-				       struct iwl_scan_req_unified_lmac *cmd,
-				       struct iwl_mvm_scan_params *params)
+static __le32 iwl_mvm_scan_priority(struct iwl_mvm *mvm,
+				    enum iwl_scan_priority_ext prio)
+{
+	if (fw_has_api(&mvm->fw->ucode_capa,
+		       IWL_UCODE_TLV_API_EXT_SCAN_PRIORITY))
+		return cpu_to_le32(prio);
+
+	if (prio <= IWL_SCAN_PRIORITY_EXT_2)
+		return cpu_to_le32(IWL_SCAN_PRIORITY_LOW);
+
+	if (prio <= IWL_SCAN_PRIORITY_EXT_4)
+		return cpu_to_le32(IWL_SCAN_PRIORITY_MEDIUM);
+
+	return cpu_to_le32(IWL_SCAN_PRIORITY_HIGH);
+}
+
+static void iwl_mvm_scan_lmac_dwell(struct iwl_mvm *mvm,
+				    struct iwl_scan_req_lmac *cmd,
+				    struct iwl_mvm_scan_params *params)
 {
-	memset(cmd, 0, ksize(cmd));
 	cmd->active_dwell = params->dwell[IEEE80211_BAND_2GHZ].active;
 	cmd->passive_dwell = params->dwell[IEEE80211_BAND_2GHZ].passive;
 	if (params->passive_fragmented)
 		cmd->fragmented_dwell =
 				params->dwell[IEEE80211_BAND_2GHZ].fragmented;
-	cmd->rx_chain_select = iwl_mvm_scan_rx_chain(mvm);
 	cmd->max_out_time = cpu_to_le32(params->max_out_time);
 	cmd->suspend_time = cpu_to_le32(params->suspend_time);
-	cmd->scan_prio = cpu_to_le32(IWL_SCAN_PRIORITY_HIGH);
-	cmd->iter_num = cpu_to_le32(1);
-
-	if (iwl_mvm_rrm_scan_needed(mvm))
-		cmd->scan_flags |=
-			cpu_to_le32(IWL_MVM_LMAC_SCAN_FLAGS_RRM_ENABLED);
+	cmd->scan_prio = iwl_mvm_scan_priority(mvm, IWL_SCAN_PRIORITY_EXT_6);
 }
 
-int iwl_mvm_unified_scan_lmac(struct iwl_mvm *mvm,
-			      struct ieee80211_vif *vif,
-			      struct ieee80211_scan_request *req)
+static inline bool iwl_mvm_scan_fits(struct iwl_mvm *mvm, int n_ssids,
+				     struct ieee80211_scan_ies *ies,
+				     int n_channels)
 {
-	struct iwl_host_cmd hcmd = {
-		.id = SCAN_OFFLOAD_REQUEST_CMD,
-		.len = { sizeof(struct iwl_scan_req_unified_lmac) +
-			 sizeof(struct iwl_scan_channel_cfg_lmac) *
-				mvm->fw->ucode_capa.n_scan_channels +
-			 sizeof(struct iwl_scan_probe_req), },
-		.data = { mvm->scan_cmd, },
-		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
-	};
-	struct iwl_scan_req_unified_lmac *cmd = mvm->scan_cmd;
-	struct iwl_scan_probe_req *preq;
-	struct iwl_mvm_scan_params params = {};
-	u32 flags;
-	u32 ssid_bitmap = 0;
-	int ret, i;
-
-	lockdep_assert_held(&mvm->mutex);
-
-	/* we should have failed registration if scan_cmd was NULL */
-	if (WARN_ON(mvm->scan_cmd == NULL))
-		return -ENOMEM;
-
-	if (req->req.n_ssids > PROBE_OPTION_MAX ||
-	    req->ies.common_ie_len + req->ies.len[NL80211_BAND_2GHZ] +
-	    req->ies.len[NL80211_BAND_5GHZ] >
-		iwl_mvm_max_scan_ie_fw_cmd_room(mvm, false) ||
-	    req->req.n_channels > mvm->fw->ucode_capa.n_scan_channels)
-		return -ENOBUFS;
+	return ((n_ssids <= PROBE_OPTION_MAX) &&
+		(n_channels <= mvm->fw->ucode_capa.n_scan_channels) &
+		(ies->common_ie_len +
+		 ies->len[NL80211_BAND_2GHZ] +
+		 ies->len[NL80211_BAND_5GHZ] <=
+		 iwl_mvm_max_scan_ie_fw_cmd_room(mvm)));
+}
 
-	mvm->scan_status = IWL_MVM_SCAN_OS;
+static inline bool iwl_mvm_scan_use_ebs(struct iwl_mvm *mvm,
+					struct ieee80211_vif *vif,
+					int n_iterations)
+{
+	const struct iwl_ucode_capabilities *capa = &mvm->fw->ucode_capa;
 
-	iwl_mvm_scan_calc_params(mvm, vif, req->req.n_ssids, req->req.flags,
-				 &params);
+	/* We can only use EBS if:
+	 *	1. the feature is supported;
+	 *	2. the last EBS was successful;
+	 *	3. if only single scan, the single scan EBS API is supported;
+	 *	4. it's not a p2p find operation.
+	 */
+	return ((capa->flags & IWL_UCODE_TLV_FLAGS_EBS_SUPPORT) &&
+		mvm->last_ebs_successful &&
+		(n_iterations > 1 ||
+		 fw_has_api(capa, IWL_UCODE_TLV_API_SINGLE_SCAN_EBS)) &&
+		vif->type != NL80211_IFTYPE_P2P_DEVICE);
+}
 
-	iwl_mvm_build_generic_unified_scan_cmd(mvm, cmd, &params);
+static int iwl_mvm_scan_total_iterations(struct iwl_mvm_scan_params *params)
+{
+	return params->schedule[0].iterations + params->schedule[1].iterations;
+}
 
-	cmd->n_channels = (u8)req->req.n_channels;
+static int iwl_mvm_scan_lmac_flags(struct iwl_mvm *mvm,
+				   struct iwl_mvm_scan_params *params)
+{
+	int flags = 0;
 
-	flags = IWL_MVM_LMAC_SCAN_FLAG_PASS_ALL;
+	if (params->n_ssids == 0)
+		flags |= IWL_MVM_LMAC_SCAN_FLAG_PASSIVE;
 
-	if (req->req.n_ssids == 1 && req->req.ssids[0].ssid_len != 0)
+	if (params->n_ssids == 1 && params->ssids[0].ssid_len != 0)
 		flags |= IWL_MVM_LMAC_SCAN_FLAG_PRE_CONNECTION;
 
-	if (params.passive_fragmented)
+	if (params->passive_fragmented)
 		flags |= IWL_MVM_LMAC_SCAN_FLAG_FRAGMENTED;
 
-	if (req->req.n_ssids == 0)
-		flags |= IWL_MVM_LMAC_SCAN_FLAG_PASSIVE;
-
-	cmd->scan_flags |= cpu_to_le32(flags);
-
-	cmd->flags = iwl_mvm_scan_rxon_flags(req->req.channels[0]->band);
-	cmd->filter_flags = cpu_to_le32(MAC_FILTER_ACCEPT_GRP |
-					MAC_FILTER_IN_BEACON);
-	iwl_mvm_unified_scan_fill_tx_cmd(mvm, cmd->tx_cmd, req->req.no_cck);
-	iwl_mvm_scan_fill_ssids(cmd->direct_scan, req->req.ssids,
-				req->req.n_ssids, 0);
-
-	cmd->schedule[0].delay = 0;
-	cmd->schedule[0].iterations = 1;
-	cmd->schedule[0].full_scan_mul = 0;
-	cmd->schedule[1].delay = 0;
-	cmd->schedule[1].iterations = 0;
-	cmd->schedule[1].full_scan_mul = 0;
-
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_SINGLE_SCAN_EBS &&
-	    mvm->last_ebs_successful) {
-		cmd->channel_opt[0].flags =
-			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);
-	}
-
-	for (i = 1; i <= req->req.n_ssids; i++)
-		ssid_bitmap |= BIT(i);
-
-	iwl_mvm_lmac_scan_cfg_channels(mvm, req->req.channels,
-				       req->req.n_channels, ssid_bitmap,
-				       cmd);
+	if (iwl_mvm_rrm_scan_needed(mvm))
+		flags |= IWL_MVM_LMAC_SCAN_FLAGS_RRM_ENABLED;
 
-	preq = (void *)(cmd->data + sizeof(struct iwl_scan_channel_cfg_lmac) *
-			mvm->fw->ucode_capa.n_scan_channels);
+	if (params->pass_all)
+		flags |= IWL_MVM_LMAC_SCAN_FLAG_PASS_ALL;
+	else
+		flags |= IWL_MVM_LMAC_SCAN_FLAG_MATCH;
 
-	iwl_mvm_build_unified_scan_probe(mvm, vif, &req->ies, preq,
-		req->req.flags & NL80211_SCAN_FLAG_RANDOM_ADDR ?
-			req->req.mac_addr : NULL,
-		req->req.mac_addr_mask);
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	if (mvm->scan_iter_notif_enabled)
+		flags |= IWL_MVM_LMAC_SCAN_FLAG_ITER_COMPLETE;
+#endif
 
-	ret = iwl_mvm_send_cmd(mvm, &hcmd);
-	if (!ret) {
-		IWL_DEBUG_SCAN(mvm, "Scan request was sent successfully\n");
-	} else {
-		/*
-		 * If the scan failed, it usually means that the FW was unable
-		 * to allocate the time events. Warn on it, but maybe we
-		 * should try to send the command again with different params.
-		 */
-		IWL_ERR(mvm, "Scan failed! ret %d\n", ret);
-		mvm->scan_status = IWL_MVM_SCAN_NONE;
-		ret = -EIO;
-	}
-	return ret;
+	return flags;
 }
 
-int iwl_mvm_unified_sched_scan_lmac(struct iwl_mvm *mvm,
-				    struct ieee80211_vif *vif,
-				    struct cfg80211_sched_scan_request *req,
-				    struct ieee80211_scan_ies *ies)
+static int iwl_mvm_scan_lmac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			     struct iwl_mvm_scan_params *params)
 {
-	struct iwl_host_cmd hcmd = {
-		.id = SCAN_OFFLOAD_REQUEST_CMD,
-		.len = { sizeof(struct iwl_scan_req_unified_lmac) +
-			 sizeof(struct iwl_scan_channel_cfg_lmac) *
-				mvm->fw->ucode_capa.n_scan_channels +
-			 sizeof(struct iwl_scan_probe_req), },
-		.data = { mvm->scan_cmd, },
-		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
-	};
-	struct iwl_scan_req_unified_lmac *cmd = mvm->scan_cmd;
-	struct iwl_scan_probe_req *preq;
-	struct iwl_mvm_scan_params params = {};
-	int ret;
-	u32 flags = 0, ssid_bitmap = 0;
+	struct iwl_scan_req_lmac *cmd = mvm->scan_cmd;
+	struct iwl_scan_probe_req *preq =
+		(void *)(cmd->data + sizeof(struct iwl_scan_channel_cfg_lmac) *
+			 mvm->fw->ucode_capa.n_scan_channels);
+	u32 ssid_bitmap = 0;
+	int n_iterations = iwl_mvm_scan_total_iterations(params);
 
 	lockdep_assert_held(&mvm->mutex);
 
-	/* we should have failed registration if scan_cmd was NULL */
-	if (WARN_ON(mvm->scan_cmd == NULL))
-		return -ENOMEM;
-
-	if (req->n_ssids > PROBE_OPTION_MAX ||
-	    ies->common_ie_len + ies->len[NL80211_BAND_2GHZ] +
-	    ies->len[NL80211_BAND_5GHZ] >
-		iwl_mvm_max_scan_ie_fw_cmd_room(mvm, true) ||
-	    req->n_channels > mvm->fw->ucode_capa.n_scan_channels)
-		return -ENOBUFS;
-
-	iwl_mvm_scan_calc_params(mvm, vif, req->n_ssids, 0, &params);
-
-	iwl_mvm_build_generic_unified_scan_cmd(mvm, cmd, &params);
-
-	cmd->n_channels = (u8)req->n_channels;
-
-	cmd->delay = cpu_to_le32(req->delay);
-
-	if (iwl_mvm_scan_pass_all(mvm, req))
-		flags |= IWL_MVM_LMAC_SCAN_FLAG_PASS_ALL;
-	else
-		flags |= IWL_MVM_LMAC_SCAN_FLAG_MATCH;
+	memset(cmd, 0, ksize(cmd));
 
-	if (req->n_ssids == 1 && req->ssids[0].ssid_len != 0)
-		flags |= IWL_MVM_LMAC_SCAN_FLAG_PRE_CONNECTION;
+	iwl_mvm_scan_lmac_dwell(mvm, cmd, params);
 
-	if (params.passive_fragmented)
-		flags |= IWL_MVM_LMAC_SCAN_FLAG_FRAGMENTED;
-
-	if (req->n_ssids == 0)
-		flags |= IWL_MVM_LMAC_SCAN_FLAG_PASSIVE;
+	cmd->rx_chain_select = iwl_mvm_scan_rx_chain(mvm);
+	cmd->iter_num = cpu_to_le32(1);
+	cmd->n_channels = (u8)params->n_channels;
 
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	if (mvm->scan_iter_notif_enabled)
-		flags |= IWL_MVM_LMAC_SCAN_FLAG_ITER_COMPLETE;
-#endif
+	cmd->delay = cpu_to_le32(params->delay);
 
-	cmd->scan_flags |= cpu_to_le32(flags);
+	cmd->scan_flags = cpu_to_le32(iwl_mvm_scan_lmac_flags(mvm, params));
 
-	cmd->flags = iwl_mvm_scan_rxon_flags(req->channels[0]->band);
+	cmd->flags = iwl_mvm_scan_rxon_flags(params->channels[0]->band);
 	cmd->filter_flags = cpu_to_le32(MAC_FILTER_ACCEPT_GRP |
 					MAC_FILTER_IN_BEACON);
-	iwl_mvm_unified_scan_fill_tx_cmd(mvm, cmd->tx_cmd, false);
-	iwl_scan_offload_build_ssid(req, cmd->direct_scan, &ssid_bitmap, false);
+	iwl_mvm_scan_fill_tx_cmd(mvm, cmd->tx_cmd, params->no_cck);
+	iwl_scan_build_ssids(params, cmd->direct_scan, &ssid_bitmap);
 
-	cmd->schedule[0].delay = cpu_to_le16(req->interval / MSEC_PER_SEC);
-	cmd->schedule[0].iterations = IWL_FAST_SCHED_SCAN_ITERATIONS;
-	cmd->schedule[0].full_scan_mul = 1;
+	/* this API uses bits 1-20 instead of 0-19 */
+	ssid_bitmap <<= 1;
 
-	cmd->schedule[1].delay = cpu_to_le16(req->interval / MSEC_PER_SEC);
-	cmd->schedule[1].iterations = 0xff;
-	cmd->schedule[1].full_scan_mul = IWL_FULL_SCAN_MULTIPLIER;
+	cmd->schedule[0].delay = cpu_to_le16(params->interval);
+	cmd->schedule[0].iterations = params->schedule[0].iterations;
+	cmd->schedule[0].full_scan_mul = params->schedule[0].full_scan_mul;
+	cmd->schedule[1].delay = cpu_to_le16(params->interval);
+	cmd->schedule[1].iterations = params->schedule[1].iterations;
+	cmd->schedule[1].full_scan_mul = params->schedule[1].iterations;
 
-	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_EBS_SUPPORT &&
-	    mvm->last_ebs_successful) {
+	if (iwl_mvm_scan_use_ebs(mvm, vif, n_iterations)) {
 		cmd->channel_opt[0].flags =
 			cpu_to_le16(IWL_SCAN_CHANNEL_FLAG_EBS |
 				    IWL_SCAN_CHANNEL_FLAG_EBS_ACCURATE |
@@ -988,61 +882,14 @@ int iwl_mvm_unified_sched_scan_lmac(struct iwl_mvm *mvm,
 			cpu_to_le16(IWL_SPARSE_EBS_SCAN_RATIO);
 	}
 
-	iwl_mvm_lmac_scan_cfg_channels(mvm, req->channels, req->n_channels,
-				       ssid_bitmap, cmd);
-
-	preq = (void *)(cmd->data + sizeof(struct iwl_scan_channel_cfg_lmac) *
-			mvm->fw->ucode_capa.n_scan_channels);
+	iwl_mvm_lmac_scan_cfg_channels(mvm, params->channels,
+				       params->n_channels, ssid_bitmap, cmd);
 
-	iwl_mvm_build_unified_scan_probe(mvm, vif, ies, preq,
-		req->flags & NL80211_SCAN_FLAG_RANDOM_ADDR ?
-			req->mac_addr : NULL,
-		req->mac_addr_mask);
+	*preq = params->preq;
 
-	ret = iwl_mvm_send_cmd(mvm, &hcmd);
-	if (!ret) {
-		IWL_DEBUG_SCAN(mvm,
-			       "Sched scan request was sent successfully\n");
-	} else {
-		/*
-		 * If the scan failed, it usually means that the FW was unable
-		 * to allocate the time events. Warn on it, but maybe we
-		 * should try to send the command again with different params.
-		 */
-		IWL_ERR(mvm, "Sched scan failed! ret %d\n", ret);
-		mvm->scan_status = IWL_MVM_SCAN_NONE;
-		ret = -EIO;
-	}
-	return ret;
-}
-
-
-int iwl_mvm_cancel_scan(struct iwl_mvm *mvm)
-{
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)
-		return iwl_umac_scan_stop(mvm, IWL_UMAC_SCAN_UID_REG_SCAN,
-					  true);
-
-	if (mvm->scan_status == IWL_MVM_SCAN_NONE)
-		return 0;
-
-	if (iwl_mvm_is_radio_killed(mvm)) {
-		ieee80211_scan_completed(mvm->hw, true);
-		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
-		mvm->scan_status = IWL_MVM_SCAN_NONE;
-		return 0;
-	}
-
-	return iwl_mvm_scan_offload_stop(mvm, true);
+	return 0;
 }
 
-/* UMAC scan API */
-
-struct iwl_umac_scan_done {
-	struct iwl_mvm *mvm;
-	enum iwl_umac_scan_uid_type type;
-};
-
 static int rate_to_scan_rate_flag(unsigned int rate)
 {
 	static const int rate_to_scan_rate[IWL_RATE_COUNT] = {
@@ -1151,79 +998,21 @@ int iwl_mvm_config_scan(struct iwl_mvm *mvm)
 	return ret;
 }
 
-static int iwl_mvm_find_scan_uid(struct iwl_mvm *mvm, u32 uid)
-{
-	int i;
-
-	for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++)
-		if (mvm->scan_uid[i] == uid)
-			return i;
-
-	return i;
-}
-
-static int iwl_mvm_find_free_scan_uid(struct iwl_mvm *mvm)
-{
-	return iwl_mvm_find_scan_uid(mvm, 0);
-}
-
-static bool iwl_mvm_find_scan_type(struct iwl_mvm *mvm,
-				   enum iwl_umac_scan_uid_type type)
-{
-	int i;
-
-	for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++)
-		if (mvm->scan_uid[i] & type)
-			return true;
-
-	return false;
-}
-
-static int iwl_mvm_find_first_scan(struct iwl_mvm *mvm,
-				   enum iwl_umac_scan_uid_type type)
+static int iwl_mvm_scan_uid_by_status(struct iwl_mvm *mvm, int status)
 {
 	int i;
 
-	for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++)
-		if (mvm->scan_uid[i] & type)
+	for (i = 0; i < mvm->max_scans; i++)
+		if (mvm->scan_uid_status[i] == status)
 			return i;
 
-	return i;
+	return -ENOENT;
 }
 
-static u32 iwl_generate_scan_uid(struct iwl_mvm *mvm,
-				 enum iwl_umac_scan_uid_type type)
-{
-	u32 uid;
-
-	/* make sure exactly one bit is on in scan type */
-	WARN_ON(hweight8(type) != 1);
-
-	/*
-	 * Make sure scan uids are unique. If one scan lasts long time while
-	 * others are completing frequently, the seq number will wrap up and
-	 * we may have more than one scan with the same uid.
-	 */
-	do {
-		uid = type | (mvm->scan_seq_num <<
-			      IWL_UMAC_SCAN_UID_SEQ_OFFSET);
-		mvm->scan_seq_num++;
-	} while (iwl_mvm_find_scan_uid(mvm, uid) <
-		 IWL_MVM_MAX_SIMULTANEOUS_SCANS);
-
-	IWL_DEBUG_SCAN(mvm, "Generated scan UID %u\n", uid);
-
-	return uid;
-}
-
-static void
-iwl_mvm_build_generic_umac_scan_cmd(struct iwl_mvm *mvm,
+static void iwl_mvm_scan_umac_dwell(struct iwl_mvm *mvm,
 				    struct iwl_scan_req_umac *cmd,
 				    struct iwl_mvm_scan_params *params)
 {
-	memset(cmd, 0, ksize(cmd));
-	cmd->hdr.size = cpu_to_le16(iwl_mvm_scan_size(mvm) -
-				    sizeof(struct iwl_mvm_umac_cmd_hdr));
 	cmd->active_dwell = params->dwell[IEEE80211_BAND_2GHZ].active;
 	cmd->passive_dwell = params->dwell[IEEE80211_BAND_2GHZ].passive;
 	if (params->passive_fragmented)
@@ -1231,7 +1020,15 @@ iwl_mvm_build_generic_umac_scan_cmd(struct iwl_mvm *mvm,
 				params->dwell[IEEE80211_BAND_2GHZ].fragmented;
 	cmd->max_out_time = cpu_to_le32(params->max_out_time);
 	cmd->suspend_time = cpu_to_le32(params->suspend_time);
-	cmd->scan_priority = cpu_to_le32(IWL_SCAN_PRIORITY_HIGH);
+	cmd->scan_priority =
+		iwl_mvm_scan_priority(mvm, IWL_SCAN_PRIORITY_EXT_6);
+
+	if (iwl_mvm_scan_total_iterations(params) == 0)
+		cmd->ooc_priority =
+			iwl_mvm_scan_priority(mvm, IWL_SCAN_PRIORITY_EXT_6);
+	else
+		cmd->ooc_priority =
+			iwl_mvm_scan_priority(mvm, IWL_SCAN_PRIORITY_EXT_2);
 }
 
 static void
@@ -1251,230 +1048,326 @@ iwl_mvm_umac_scan_cfg_channels(struct iwl_mvm *mvm,
 	}
 }
 
-static u32 iwl_mvm_scan_umac_common_flags(struct iwl_mvm *mvm, int n_ssids,
-					  struct cfg80211_ssid *ssids,
-					  int fragmented)
+static u32 iwl_mvm_scan_umac_flags(struct iwl_mvm *mvm,
+				   struct iwl_mvm_scan_params *params)
 {
 	int flags = 0;
 
-	if (n_ssids == 0)
+	if (params->n_ssids == 0)
 		flags = IWL_UMAC_SCAN_GEN_FLAGS_PASSIVE;
 
-	if (n_ssids == 1 && ssids[0].ssid_len != 0)
+	if (params->n_ssids == 1 && params->ssids[0].ssid_len != 0)
 		flags |= IWL_UMAC_SCAN_GEN_FLAGS_PRE_CONNECT;
 
-	if (fragmented)
+	if (params->passive_fragmented)
 		flags |= IWL_UMAC_SCAN_GEN_FLAGS_FRAGMENTED;
 
 	if (iwl_mvm_rrm_scan_needed(mvm))
 		flags |= IWL_UMAC_SCAN_GEN_FLAGS_RRM_ENABLED;
 
+	if (params->pass_all)
+		flags |= IWL_UMAC_SCAN_GEN_FLAGS_PASS_ALL;
+	else
+		flags |= IWL_UMAC_SCAN_GEN_FLAGS_MATCH;
+
+	if (iwl_mvm_scan_total_iterations(params) > 1)
+		flags |= IWL_UMAC_SCAN_GEN_FLAGS_PERIODIC;
+
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	if (mvm->scan_iter_notif_enabled)
+		flags |= IWL_UMAC_SCAN_GEN_FLAGS_ITER_COMPLETE;
+#endif
 	return flags;
 }
 
-int iwl_mvm_scan_umac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-		      struct ieee80211_scan_request *req)
+static int iwl_mvm_scan_umac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			     struct iwl_mvm_scan_params *params,
+			     int type)
 {
-	struct iwl_host_cmd hcmd = {
-		.id = SCAN_REQ_UMAC,
-		.len = { iwl_mvm_scan_size(mvm), },
-		.data = { mvm->scan_cmd, },
-		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
-	};
 	struct iwl_scan_req_umac *cmd = mvm->scan_cmd;
 	struct iwl_scan_req_umac_tail *sec_part = (void *)&cmd->data +
 		sizeof(struct iwl_scan_channel_cfg_umac) *
 			mvm->fw->ucode_capa.n_scan_channels;
-	struct iwl_mvm_scan_params params = {};
-	u32 uid, flags;
+	int uid;
 	u32 ssid_bitmap = 0;
-	int ret, i, uid_idx;
+	int n_iterations = iwl_mvm_scan_total_iterations(params);
 
 	lockdep_assert_held(&mvm->mutex);
 
-	uid_idx = iwl_mvm_find_free_scan_uid(mvm);
-	if (uid_idx >= IWL_MVM_MAX_SIMULTANEOUS_SCANS)
-		return -EBUSY;
+	uid = iwl_mvm_scan_uid_by_status(mvm, 0);
+	if (uid < 0)
+		return uid;
 
-	/* we should have failed registration if scan_cmd was NULL */
-	if (WARN_ON(mvm->scan_cmd == NULL))
-		return -ENOMEM;
-
-	if (WARN_ON(req->req.n_ssids > PROBE_OPTION_MAX ||
-		    req->ies.common_ie_len +
-		    req->ies.len[NL80211_BAND_2GHZ] +
-		    req->ies.len[NL80211_BAND_5GHZ] + 24 + 2 >
-		    SCAN_OFFLOAD_PROBE_REQ_SIZE || req->req.n_channels >
-		    mvm->fw->ucode_capa.n_scan_channels))
-		return -ENOBUFS;
+	memset(cmd, 0, ksize(cmd));
+	cmd->hdr.size = cpu_to_le16(iwl_mvm_scan_size(mvm) -
+				    sizeof(struct iwl_mvm_umac_cmd_hdr));
 
-	iwl_mvm_scan_calc_params(mvm, vif, req->req.n_ssids, req->req.flags,
-				 &params);
+	iwl_mvm_scan_umac_dwell(mvm, cmd, params);
 
-	iwl_mvm_build_generic_umac_scan_cmd(mvm, cmd, &params);
+	mvm->scan_uid_status[uid] = type;
 
-	uid = iwl_generate_scan_uid(mvm, IWL_UMAC_SCAN_UID_REG_SCAN);
-	mvm->scan_uid[uid_idx] = uid;
 	cmd->uid = cpu_to_le32(uid);
+	cmd->general_flags = cpu_to_le32(iwl_mvm_scan_umac_flags(mvm, params));
 
-	cmd->ooc_priority = cpu_to_le32(IWL_SCAN_PRIORITY_HIGH);
-
-	flags = iwl_mvm_scan_umac_common_flags(mvm, req->req.n_ssids,
-					       req->req.ssids,
-					       params.passive_fragmented);
-
-	flags |= IWL_UMAC_SCAN_GEN_FLAGS_PASS_ALL;
-
-	cmd->general_flags = cpu_to_le32(flags);
-
-	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_SINGLE_SCAN_EBS &&
-	    mvm->last_ebs_successful)
+	if (iwl_mvm_scan_use_ebs(mvm, vif, n_iterations))
 		cmd->channel_flags = IWL_SCAN_CHANNEL_FLAG_EBS |
 				     IWL_SCAN_CHANNEL_FLAG_EBS_ACCURATE |
 				     IWL_SCAN_CHANNEL_FLAG_CACHE_ADD;
 
-	cmd->n_channels = req->req.n_channels;
+	cmd->n_channels = params->n_channels;
+
+	iwl_scan_build_ssids(params, sec_part->direct_scan, &ssid_bitmap);
 
-	for (i = 0; i < req->req.n_ssids; i++)
-		ssid_bitmap |= BIT(i);
+	iwl_mvm_umac_scan_cfg_channels(mvm, params->channels,
+				       params->n_channels, ssid_bitmap, cmd);
 
-	iwl_mvm_umac_scan_cfg_channels(mvm, req->req.channels,
-				       req->req.n_channels, ssid_bitmap, cmd);
+	/* With UMAC we use only one schedule for now, so use the sum
+	 * of the iterations (with a a maximum of 255).
+	 */
+	sec_part->schedule[0].iter_count =
+		(n_iterations > 255) ? 255 : n_iterations;
+	sec_part->schedule[0].interval = cpu_to_le16(params->interval);
 
-	sec_part->schedule[0].iter_count = 1;
-	sec_part->delay = 0;
+	sec_part->delay = cpu_to_le16(params->delay);
+	sec_part->preq = params->preq;
 
-	iwl_mvm_build_unified_scan_probe(mvm, vif, &req->ies, &sec_part->preq,
-		req->req.flags & NL80211_SCAN_FLAG_RANDOM_ADDR ?
-			req->req.mac_addr : NULL,
-		req->req.mac_addr_mask);
+	return 0;
+}
 
-	iwl_mvm_scan_fill_ssids(sec_part->direct_scan, req->req.ssids,
-				req->req.n_ssids, 0);
+static int iwl_mvm_num_scans(struct iwl_mvm *mvm)
+{
+	return hweight32(mvm->scan_status & IWL_MVM_SCAN_MASK);
+}
 
-	ret = iwl_mvm_send_cmd(mvm, &hcmd);
-	if (!ret) {
-		IWL_DEBUG_SCAN(mvm,
-			       "Scan request was sent successfully\n");
-	} else {
-		/*
-		 * If the scan failed, it usually means that the FW was unable
-		 * to allocate the time events. Warn on it, but maybe we
-		 * should try to send the command again with different params.
-		 */
-		IWL_ERR(mvm, "Scan failed! ret %d\n", ret);
+static int iwl_mvm_check_running_scans(struct iwl_mvm *mvm, int type)
+{
+	/* This looks a bit arbitrary, but the idea is that if we run
+	 * out of possible simultaneous scans and the userspace is
+	 * trying to run a scan type that is already running, we
+	 * return -EBUSY.  But if the userspace wants to start a
+	 * different type of scan, we stop the opposite type to make
+	 * space for the new request.  The reason is backwards
+	 * compatibility with old wpa_supplicant that wouldn't stop a
+	 * scheduled scan before starting a normal scan.
+	 */
+
+	if (iwl_mvm_num_scans(mvm) < mvm->max_scans)
+		return 0;
+
+	/* Use a switch, even though this is a bitmask, so that more
+	 * than one bits set will fall in default and we will warn.
+	 */
+	switch (type) {
+	case IWL_MVM_SCAN_REGULAR:
+		if (mvm->scan_status & IWL_MVM_SCAN_REGULAR_MASK)
+			return -EBUSY;
+		return iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_SCHED, true);
+	case IWL_MVM_SCAN_SCHED:
+		if (mvm->scan_status & IWL_MVM_SCAN_SCHED_MASK)
+			return -EBUSY;
+		iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_REGULAR, true);
+	case IWL_MVM_SCAN_NETDETECT:
+		/* No need to stop anything for net-detect since the
+		 * firmware is restarted anyway.  This way, any sched
+		 * scans that were running will be restarted when we
+		 * resume.
+		*/
+		return 0;
+	default:
+		WARN_ON(1);
+		break;
 	}
-	return ret;
+
+	return -EIO;
 }
 
-int iwl_mvm_sched_scan_umac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			    struct cfg80211_sched_scan_request *req,
-			    struct ieee80211_scan_ies *ies)
+int iwl_mvm_reg_scan_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			   struct cfg80211_scan_request *req,
+			   struct ieee80211_scan_ies *ies)
 {
-
 	struct iwl_host_cmd hcmd = {
-		.id = SCAN_REQ_UMAC,
 		.len = { iwl_mvm_scan_size(mvm), },
 		.data = { mvm->scan_cmd, },
 		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
 	};
-	struct iwl_scan_req_umac *cmd = mvm->scan_cmd;
-	struct iwl_scan_req_umac_tail *sec_part = (void *)&cmd->data +
-		sizeof(struct iwl_scan_channel_cfg_umac) *
-			mvm->fw->ucode_capa.n_scan_channels;
 	struct iwl_mvm_scan_params params = {};
-	u32 uid, flags;
-	u32 ssid_bitmap = 0;
-	int ret, uid_idx;
+	int ret;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	uid_idx = iwl_mvm_find_free_scan_uid(mvm);
-	if (uid_idx >= IWL_MVM_MAX_SIMULTANEOUS_SCANS)
+	if (iwl_mvm_is_lar_supported(mvm) && !mvm->lar_regdom_set) {
+		IWL_ERR(mvm, "scan while LAR regdomain is not set\n");
 		return -EBUSY;
+	}
+
+	ret = iwl_mvm_check_running_scans(mvm, IWL_MVM_SCAN_REGULAR);
+	if (ret)
+		return ret;
+
+	iwl_mvm_ref(mvm, IWL_MVM_REF_SCAN);
 
 	/* we should have failed registration if scan_cmd was NULL */
-	if (WARN_ON(mvm->scan_cmd == NULL))
+	if (WARN_ON(!mvm->scan_cmd))
 		return -ENOMEM;
 
-	if (WARN_ON(req->n_ssids > PROBE_OPTION_MAX ||
-		    ies->common_ie_len + ies->len[NL80211_BAND_2GHZ] +
-		    ies->len[NL80211_BAND_5GHZ] + 24 + 2 >
-		    SCAN_OFFLOAD_PROBE_REQ_SIZE || req->n_channels >
-		    mvm->fw->ucode_capa.n_scan_channels))
+	if (!iwl_mvm_scan_fits(mvm, req->n_ssids, ies, req->n_channels))
 		return -ENOBUFS;
 
-	iwl_mvm_scan_calc_params(mvm, vif, req->n_ssids, req->flags,
-					 &params);
-
-	iwl_mvm_build_generic_umac_scan_cmd(mvm, cmd, &params);
-
-	cmd->flags = cpu_to_le32(IWL_UMAC_SCAN_FLAG_PREEMPTIVE);
-
-	uid = iwl_generate_scan_uid(mvm, IWL_UMAC_SCAN_UID_SCHED_SCAN);
-	mvm->scan_uid[uid_idx] = uid;
-	cmd->uid = cpu_to_le32(uid);
+	params.n_ssids = req->n_ssids;
+	params.flags = req->flags;
+	params.n_channels = req->n_channels;
+	params.delay = 0;
+	params.interval = 0;
+	params.ssids = req->ssids;
+	params.channels = req->channels;
+	params.mac_addr = req->mac_addr;
+	params.mac_addr_mask = req->mac_addr_mask;
+	params.no_cck = req->no_cck;
+	params.pass_all = true;
+	params.n_match_sets = 0;
+	params.match_sets = NULL;
+
+	params.schedule[0].iterations = 1;
+	params.schedule[0].full_scan_mul = 0;
+	params.schedule[1].iterations = 0;
+	params.schedule[1].full_scan_mul = 0;
+
+	iwl_mvm_scan_calc_dwell(mvm, vif, &params);
+
+	iwl_mvm_build_scan_probe(mvm, vif, ies, &params);
+
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
+		hcmd.id = SCAN_REQ_UMAC;
+		ret = iwl_mvm_scan_umac(mvm, vif, &params,
+					IWL_MVM_SCAN_REGULAR);
+	} else {
+		hcmd.id = SCAN_OFFLOAD_REQUEST_CMD;
+		ret = iwl_mvm_scan_lmac(mvm, vif, &params);
+	}
 
-	cmd->ooc_priority = cpu_to_le32(IWL_SCAN_PRIORITY_LOW);
+	if (ret)
+		return ret;
 
-	flags = iwl_mvm_scan_umac_common_flags(mvm, req->n_ssids, req->ssids,
-					       params.passive_fragmented);
+	ret = iwl_mvm_send_cmd(mvm, &hcmd);
+	if (!ret) {
+		IWL_DEBUG_SCAN(mvm, "Scan request was sent successfully\n");
+		mvm->scan_status |= IWL_MVM_SCAN_REGULAR;
+	} else {
+		/* If the scan failed, it usually means that the FW was unable
+		 * to allocate the time events. Warn on it, but maybe we
+		 * should try to send the command again with different params.
+		 */
+		IWL_ERR(mvm, "Scan failed! ret %d\n", ret);
+	}
 
-	flags |= IWL_UMAC_SCAN_GEN_FLAGS_PERIODIC;
+	if (ret)
+		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
 
-	if (iwl_mvm_scan_pass_all(mvm, req))
-		flags |= IWL_UMAC_SCAN_GEN_FLAGS_PASS_ALL;
-	else
-		flags |= IWL_UMAC_SCAN_GEN_FLAGS_MATCH;
+	return ret;
+}
 
-	cmd->general_flags = cpu_to_le32(flags);
+int iwl_mvm_sched_scan_start(struct iwl_mvm *mvm,
+			     struct ieee80211_vif *vif,
+			     struct cfg80211_sched_scan_request *req,
+			     struct ieee80211_scan_ies *ies,
+			     int type)
+{
+	struct iwl_host_cmd hcmd = {
+		.len = { iwl_mvm_scan_size(mvm), },
+		.data = { mvm->scan_cmd, },
+		.dataflags = { IWL_HCMD_DFL_NOCOPY, },
+	};
+	struct iwl_mvm_scan_params params = {};
+	int ret;
 
-	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_EBS_SUPPORT &&
-	    mvm->last_ebs_successful)
-		cmd->channel_flags = IWL_SCAN_CHANNEL_FLAG_EBS |
-				     IWL_SCAN_CHANNEL_FLAG_EBS_ACCURATE |
-				     IWL_SCAN_CHANNEL_FLAG_CACHE_ADD;
+	lockdep_assert_held(&mvm->mutex);
 
-	cmd->n_channels = req->n_channels;
+	if (iwl_mvm_is_lar_supported(mvm) && !mvm->lar_regdom_set) {
+		IWL_ERR(mvm, "sched-scan while LAR regdomain is not set\n");
+		return -EBUSY;
+	}
 
-	iwl_scan_offload_build_ssid(req, sec_part->direct_scan, &ssid_bitmap,
-				    false);
+	ret = iwl_mvm_check_running_scans(mvm, type);
+	if (ret)
+		return ret;
 
-	/* This API uses bits 0-19 instead of 1-20. */
-	ssid_bitmap = ssid_bitmap >> 1;
+	/* we should have failed registration if scan_cmd was NULL */
+	if (WARN_ON(!mvm->scan_cmd))
+		return -ENOMEM;
 
-	iwl_mvm_umac_scan_cfg_channels(mvm, req->channels, req->n_channels,
-				       ssid_bitmap, cmd);
+	if (!iwl_mvm_scan_fits(mvm, req->n_ssids, ies, req->n_channels))
+		return -ENOBUFS;
 
-	sec_part->schedule[0].interval =
-				cpu_to_le16(req->interval / MSEC_PER_SEC);
-	sec_part->schedule[0].iter_count = 0xff;
+	params.n_ssids = req->n_ssids;
+	params.flags = req->flags;
+	params.n_channels = req->n_channels;
+	params.ssids = req->ssids;
+	params.channels = req->channels;
+	params.mac_addr = req->mac_addr;
+	params.mac_addr_mask = req->mac_addr_mask;
+	params.no_cck = false;
+	params.pass_all =  iwl_mvm_scan_pass_all(mvm, req);
+	params.n_match_sets = req->n_match_sets;
+	params.match_sets = req->match_sets;
+
+	params.schedule[0].iterations = IWL_FAST_SCHED_SCAN_ITERATIONS;
+	params.schedule[0].full_scan_mul = 1;
+	params.schedule[1].iterations = 0xff;
+	params.schedule[1].full_scan_mul = IWL_FULL_SCAN_MULTIPLIER;
+
+	if (req->interval > U16_MAX) {
+		IWL_DEBUG_SCAN(mvm,
+			       "interval value is > 16-bits, set to max possible\n");
+		params.interval = U16_MAX;
+	} else {
+		params.interval = req->interval / MSEC_PER_SEC;
+	}
 
+	/* In theory, LMAC scans can handle a 32-bit delay, but since
+	 * waiting for over 18 hours to start the scan is a bit silly
+	 * and to keep it aligned with UMAC scans (which only support
+	 * 16-bit delays), trim it down to 16-bits.
+	 */
 	if (req->delay > U16_MAX) {
 		IWL_DEBUG_SCAN(mvm,
 			       "delay value is > 16-bits, set to max possible\n");
-		sec_part->delay = cpu_to_le16(U16_MAX);
+		params.delay = U16_MAX;
+	} else {
+		params.delay = req->delay;
+	}
+
+	iwl_mvm_scan_calc_dwell(mvm, vif, &params);
+
+	ret = iwl_mvm_config_sched_scan_profiles(mvm, req);
+	if (ret)
+		return ret;
+
+	iwl_mvm_build_scan_probe(mvm, vif, ies, &params);
+
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
+		hcmd.id = SCAN_REQ_UMAC;
+		ret = iwl_mvm_scan_umac(mvm, vif, &params, IWL_MVM_SCAN_SCHED);
 	} else {
-		sec_part->delay = cpu_to_le16(req->delay);
+		hcmd.id = SCAN_OFFLOAD_REQUEST_CMD;
+		ret = iwl_mvm_scan_lmac(mvm, vif, &params);
 	}
 
-	iwl_mvm_build_unified_scan_probe(mvm, vif, ies, &sec_part->preq,
-		req->flags & NL80211_SCAN_FLAG_RANDOM_ADDR ?
-			req->mac_addr : NULL,
-		req->mac_addr_mask);
+	if (ret)
+		return ret;
 
 	ret = iwl_mvm_send_cmd(mvm, &hcmd);
 	if (!ret) {
 		IWL_DEBUG_SCAN(mvm,
 			       "Sched scan request was sent successfully\n");
+		mvm->scan_status |= type;
 	} else {
-		/*
-		 * If the scan failed, it usually means that the FW was unable
+		/* If the scan failed, it usually means that the FW was unable
 		 * to allocate the time events. Warn on it, but maybe we
 		 * should try to send the command again with different params.
 		 */
 		IWL_ERR(mvm, "Sched scan failed! ret %d\n", ret);
 	}
+
 	return ret;
 }
 
@@ -1485,150 +1378,124 @@ int iwl_mvm_rx_umac_scan_complete_notif(struct iwl_mvm *mvm,
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_umac_scan_complete *notif = (void *)pkt->data;
 	u32 uid = __le32_to_cpu(notif->uid);
-	bool sched = !!(uid & IWL_UMAC_SCAN_UID_SCHED_SCAN);
-	int uid_idx = iwl_mvm_find_scan_uid(mvm, uid);
+	bool aborted = (notif->status == IWL_SCAN_OFFLOAD_ABORTED);
 
-	/*
-	 * Scan uid may be set to zero in case of scan abort request from above.
-	 */
-	if (uid_idx >= IWL_MVM_MAX_SIMULTANEOUS_SCANS)
+	if (WARN_ON(!(mvm->scan_uid_status[uid] & mvm->scan_status)))
 		return 0;
 
+	/* if the scan is already stopping, we don't need to notify mac80211 */
+	if (mvm->scan_uid_status[uid] == IWL_MVM_SCAN_REGULAR) {
+		ieee80211_scan_completed(mvm->hw, aborted);
+		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
+	} else if (mvm->scan_uid_status[uid] == IWL_MVM_SCAN_SCHED) {
+		ieee80211_sched_scan_stopped(mvm->hw);
+	}
+
+	mvm->scan_status &= ~mvm->scan_uid_status[uid];
+
 	IWL_DEBUG_SCAN(mvm,
-		       "Scan completed, uid %u type %s, status %s, EBS status %s\n",
-		       uid, sched ? "sched" : "regular",
+		       "Scan completed, uid %u type %u, status %s, EBS status %s\n",
+		       uid, mvm->scan_uid_status[uid],
 		       notif->status == IWL_SCAN_OFFLOAD_COMPLETED ?
 				"completed" : "aborted",
-		       notif->ebs_status == IWL_SCAN_EBS_SUCCESS ?
-				"success" : "failed");
+		       iwl_mvm_ebs_status_str(notif->ebs_status));
 
-	if (notif->ebs_status)
+	if (notif->ebs_status != IWL_SCAN_EBS_SUCCESS &&
+	    notif->ebs_status != IWL_SCAN_EBS_INACTIVE)
 		mvm->last_ebs_successful = false;
 
-	mvm->scan_uid[uid_idx] = 0;
-
-	if (!sched) {
-		ieee80211_scan_completed(mvm->hw,
-					 notif->status ==
-						IWL_SCAN_OFFLOAD_ABORTED);
-		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
-	} else if (!iwl_mvm_find_scan_type(mvm, IWL_UMAC_SCAN_UID_SCHED_SCAN)) {
-		ieee80211_sched_scan_stopped(mvm->hw);
-	} else {
-		IWL_DEBUG_SCAN(mvm, "Another sched scan is running\n");
-	}
+	mvm->scan_uid_status[uid] = 0;
 
 	return 0;
 }
 
-static bool iwl_scan_umac_done_check(struct iwl_notif_wait_data *notif_wait,
-				     struct iwl_rx_packet *pkt, void *data)
+int iwl_mvm_rx_umac_scan_iter_complete_notif(struct iwl_mvm *mvm,
+					     struct iwl_rx_cmd_buffer *rxb,
+					     struct iwl_device_cmd *cmd)
 {
-	struct iwl_umac_scan_done *scan_done = data;
-	struct iwl_umac_scan_complete *notif = (void *)pkt->data;
-	u32 uid = __le32_to_cpu(notif->uid);
-	int uid_idx = iwl_mvm_find_scan_uid(scan_done->mvm, uid);
-
-	if (WARN_ON(pkt->hdr.cmd != SCAN_COMPLETE_UMAC))
-		return false;
-
-	if (uid_idx >= IWL_MVM_MAX_SIMULTANEOUS_SCANS)
-		return false;
-
-	/*
-	 * Clear scan uid of scans that was aborted from above and completed
-	 * in FW so the RX handler does nothing. Set last_ebs_successful here if
-	 * needed.
-	 */
-	scan_done->mvm->scan_uid[uid_idx] = 0;
-
-	if (notif->ebs_status)
-		scan_done->mvm->last_ebs_successful = false;
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_umac_scan_iter_complete_notif *notif = (void *)pkt->data;
+	u8 buf[256];
 
-	return !iwl_mvm_find_scan_type(scan_done->mvm, scan_done->type);
+	IWL_DEBUG_SCAN(mvm,
+		       "UMAC Scan iteration complete: status=0x%x scanned_channels=%d channels list: %s\n",
+		       notif->status, notif->scanned_channels,
+		       iwl_mvm_dump_channel_list(notif->results,
+						 notif->scanned_channels, buf,
+						 sizeof(buf)));
+	return 0;
 }
 
-static int iwl_umac_scan_abort_one(struct iwl_mvm *mvm, u32 uid)
+static int iwl_mvm_umac_scan_abort(struct iwl_mvm *mvm, int type)
 {
 	struct iwl_umac_scan_abort cmd = {
 		.hdr.size = cpu_to_le16(sizeof(struct iwl_umac_scan_abort) -
 					sizeof(struct iwl_mvm_umac_cmd_hdr)),
-		.uid = cpu_to_le32(uid),
 	};
+	int uid, ret;
 
 	lockdep_assert_held(&mvm->mutex);
 
+	/* We should always get a valid index here, because we already
+	 * checked that this type of scan was running in the generic
+	 * code.
+	 */
+	uid = iwl_mvm_scan_uid_by_status(mvm, type);
+	if (WARN_ON_ONCE(uid < 0))
+		return uid;
+
+	cmd.uid = cpu_to_le32(uid);
+
 	IWL_DEBUG_SCAN(mvm, "Sending scan abort, uid %u\n", uid);
 
-	return iwl_mvm_send_cmd_pdu(mvm, SCAN_ABORT_UMAC, 0, sizeof(cmd), &cmd);
+	ret = iwl_mvm_send_cmd_pdu(mvm, SCAN_ABORT_UMAC, 0, sizeof(cmd), &cmd);
+	if (!ret)
+		mvm->scan_uid_status[uid] = type << IWL_MVM_SCAN_STOPPING_SHIFT;
+
+	return ret;
 }
 
-static int iwl_umac_scan_stop(struct iwl_mvm *mvm,
-			      enum iwl_umac_scan_uid_type type, bool notify)
+static int iwl_mvm_scan_stop_wait(struct iwl_mvm *mvm, int type)
 {
 	struct iwl_notification_wait wait_scan_done;
-	static const u8 scan_done_notif[] = { SCAN_COMPLETE_UMAC, };
-	struct iwl_umac_scan_done scan_done = {
-		.mvm = mvm,
-		.type = type,
-	};
-	int i, ret = -EIO;
+	static const u8 scan_done_notif[] = { SCAN_COMPLETE_UMAC,
+					      SCAN_OFFLOAD_COMPLETE, };
+	int ret;
+
+	lockdep_assert_held(&mvm->mutex);
 
 	iwl_init_notification_wait(&mvm->notif_wait, &wait_scan_done,
 				   scan_done_notif,
 				   ARRAY_SIZE(scan_done_notif),
-				   iwl_scan_umac_done_check, &scan_done);
+				   NULL, NULL);
 
 	IWL_DEBUG_SCAN(mvm, "Preparing to stop scan, type %x\n", type);
 
-	for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++) {
-		if (mvm->scan_uid[i] & type) {
-			int err;
-
-			if (iwl_mvm_is_radio_killed(mvm) &&
-			    (type & IWL_UMAC_SCAN_UID_REG_SCAN)) {
-				ieee80211_scan_completed(mvm->hw, true);
-				iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
-				break;
-			}
-
-			err = iwl_umac_scan_abort_one(mvm, mvm->scan_uid[i]);
-			if (!err)
-				ret = 0;
-		}
-	}
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN))
+		ret = iwl_mvm_umac_scan_abort(mvm, type);
+	else
+		ret = iwl_mvm_lmac_scan_abort(mvm);
 
 	if (ret) {
-		IWL_DEBUG_SCAN(mvm, "Couldn't stop scan\n");
+		IWL_DEBUG_SCAN(mvm, "couldn't stop scan type %d\n", type);
 		iwl_remove_notification(&mvm->notif_wait, &wait_scan_done);
 		return ret;
 	}
 
 	ret = iwl_wait_notification(&mvm->notif_wait, &wait_scan_done, 1 * HZ);
-	if (ret)
-		return ret;
-
-	if (notify) {
-		if (type & IWL_UMAC_SCAN_UID_SCHED_SCAN)
-			ieee80211_sched_scan_stopped(mvm->hw);
-		if (type & IWL_UMAC_SCAN_UID_REG_SCAN) {
-			ieee80211_scan_completed(mvm->hw, true);
-			iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
-		}
-	}
 
 	return ret;
 }
 
 int iwl_mvm_scan_size(struct iwl_mvm *mvm)
 {
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN)
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN))
 		return sizeof(struct iwl_scan_req_umac) +
 			sizeof(struct iwl_scan_channel_cfg_umac) *
 				mvm->fw->ucode_capa.n_scan_channels +
 			sizeof(struct iwl_scan_req_umac_tail);
 
-	return sizeof(struct iwl_scan_req_unified_lmac) +
+	return sizeof(struct iwl_scan_req_lmac) +
 		sizeof(struct iwl_scan_channel_cfg_lmac) *
 		mvm->fw->ucode_capa.n_scan_channels +
 		sizeof(struct iwl_scan_probe_req);
@@ -1640,47 +1507,76 @@ int iwl_mvm_scan_size(struct iwl_mvm *mvm)
  */
 void iwl_mvm_report_scan_aborted(struct iwl_mvm *mvm)
 {
-	if (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_UMAC_SCAN) {
-		u32 uid, i;
+	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
+		int uid, i;
 
-		uid = iwl_mvm_find_first_scan(mvm, IWL_UMAC_SCAN_UID_REG_SCAN);
-		if (uid < IWL_MVM_MAX_SIMULTANEOUS_SCANS) {
+		uid = iwl_mvm_scan_uid_by_status(mvm, IWL_MVM_SCAN_REGULAR);
+		if (uid >= 0) {
 			ieee80211_scan_completed(mvm->hw, true);
-			mvm->scan_uid[uid] = 0;
+			mvm->scan_uid_status[uid] = 0;
 		}
-		uid = iwl_mvm_find_first_scan(mvm,
-					      IWL_UMAC_SCAN_UID_SCHED_SCAN);
-		if (uid < IWL_MVM_MAX_SIMULTANEOUS_SCANS && !mvm->restart_fw) {
+		uid = iwl_mvm_scan_uid_by_status(mvm, IWL_MVM_SCAN_SCHED);
+		if (uid >= 0 && !mvm->restart_fw) {
 			ieee80211_sched_scan_stopped(mvm->hw);
-			mvm->scan_uid[uid] = 0;
+			mvm->scan_uid_status[uid] = 0;
 		}
 
 		/* We shouldn't have any UIDs still set.  Loop over all the
 		 * UIDs to make sure there's nothing left there and warn if
 		 * any is found.
 		 */
-		for (i = 0; i < IWL_MVM_MAX_SIMULTANEOUS_SCANS; i++) {
-			if (WARN_ONCE(mvm->scan_uid[i],
-				      "UMAC scan UID %d was not cleaned\n",
-				      mvm->scan_uid[i]))
-				mvm->scan_uid[i] = 0;
+		for (i = 0; i < mvm->max_scans; i++) {
+			if (WARN_ONCE(mvm->scan_uid_status[i],
+				      "UMAC scan UID %d status was not cleaned\n",
+				      i))
+				mvm->scan_uid_status[i] = 0;
 		}
 	} else {
-		switch (mvm->scan_status) {
-		case IWL_MVM_SCAN_NONE:
-			break;
-		case IWL_MVM_SCAN_OS:
+		if (mvm->scan_status & IWL_MVM_SCAN_REGULAR)
 			ieee80211_scan_completed(mvm->hw, true);
-			break;
-		case IWL_MVM_SCAN_SCHED:
-			/*
-			 * Sched scan will be restarted by mac80211 in
-			 * restart_hw, so do not report if FW is about to be
-			 * restarted.
-			 */
-			if (!mvm->restart_fw)
-				ieee80211_sched_scan_stopped(mvm->hw);
-			break;
-		}
+
+		/* Sched scan will be restarted by mac80211 in
+		 * restart_hw, so do not report if FW is about to be
+		 * restarted.
+		 */
+		if ((mvm->scan_status & IWL_MVM_SCAN_SCHED) && !mvm->restart_fw)
+			ieee80211_sched_scan_stopped(mvm->hw);
+	}
+}
+
+int iwl_mvm_scan_stop(struct iwl_mvm *mvm, int type, bool notify)
+{
+	int ret;
+
+	if (!(mvm->scan_status & type))
+		return 0;
+
+	if (iwl_mvm_is_radio_killed(mvm)) {
+		ret = 0;
+		goto out;
+	}
+
+	ret = iwl_mvm_scan_stop_wait(mvm, type);
+	if (!ret)
+		mvm->scan_status |= type << IWL_MVM_SCAN_STOPPING_SHIFT;
+out:
+	/* Clear the scan status so the next scan requests will
+	 * succeed and mark the scan as stopping, so that the Rx
+	 * handler doesn't do anything, as the scan was stopped from
+	 * above.
+	 */
+	mvm->scan_status &= ~type;
+
+	if (type == IWL_MVM_SCAN_REGULAR) {
+		/* Since the rx handler won't do anything now, we have
+		 * to release the scan reference here.
+		 */
+		iwl_mvm_unref(mvm, IWL_MVM_REF_SCAN);
+		if (notify)
+			ieee80211_scan_completed(mvm->hw, true);
+	} else if (notify) {
+		ieee80211_sched_scan_stopped(mvm->hw);
 	}
+
+	return ret;
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/iwlwifi/mvm/sta.c
index 1845b79487c8..d68dc697a4a0 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.c
@@ -5,8 +5,8 @@
  *
  * GPL LICENSE SUMMARY
  *
- * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2012 - 2015 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -31,8 +31,8 @@
  *
  * BSD LICENSE
  *
- * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2012 - 2015 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -1000,13 +1000,13 @@ int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	fifo = iwl_mvm_ac_to_tx_fifo[tid_to_mac80211_ac[tid]];
 
+	iwl_mvm_enable_agg_txq(mvm, queue, fifo, mvmsta->sta_id, tid,
+			       buf_size, ssn, wdg_timeout);
+
 	ret = iwl_mvm_sta_tx_agg(mvm, sta, tid, queue, true);
 	if (ret)
 		return -EIO;
 
-	iwl_mvm_enable_agg_txq(mvm, queue, fifo, mvmsta->sta_id, tid,
-			       buf_size, ssn, wdg_timeout);
-
 	/*
 	 * Even though in theory the peer could have different
 	 * aggregation reorder buffer sizes for different sessions,
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.c b/drivers/net/wireless/iwlwifi/mvm/time-event.c
index fd7b0d36f9a6..d24b6a83e68c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -108,12 +108,14 @@ void iwl_mvm_roc_done_wk(struct work_struct *wk)
 	 * in the case that the time event actually completed in the firmware
 	 * (which is handled in iwl_mvm_te_handle_notif).
 	 */
-	if (test_and_clear_bit(IWL_MVM_STATUS_ROC_RUNNING, &mvm->status))
+	if (test_and_clear_bit(IWL_MVM_STATUS_ROC_RUNNING, &mvm->status)) {
 		queues |= BIT(IWL_MVM_OFFCHANNEL_QUEUE);
-	if (test_and_clear_bit(IWL_MVM_STATUS_ROC_AUX_RUNNING, &mvm->status))
+		iwl_mvm_unref(mvm, IWL_MVM_REF_ROC);
+	}
+	if (test_and_clear_bit(IWL_MVM_STATUS_ROC_AUX_RUNNING, &mvm->status)) {
 		queues |= BIT(mvm->aux_queue);
-
-	iwl_mvm_unref(mvm, IWL_MVM_REF_ROC);
+		iwl_mvm_unref(mvm, IWL_MVM_REF_ROC_AUX);
+	}
 
 	synchronize_net();
 
@@ -393,6 +395,7 @@ static int iwl_mvm_aux_roc_te_handle_notif(struct iwl_mvm *mvm,
 	} else if (le32_to_cpu(notif->action) == TE_V2_NOTIF_HOST_EVENT_START) {
 		set_bit(IWL_MVM_STATUS_ROC_AUX_RUNNING, &mvm->status);
 		te_data->running = true;
+		iwl_mvm_ref(mvm, IWL_MVM_REF_ROC_AUX);
 		ieee80211_ready_on_channel(mvm->hw); /* Start TE */
 	} else {
 		IWL_DEBUG_TE(mvm,
@@ -794,13 +797,12 @@ int iwl_mvm_start_p2p_roc(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
 {
-	struct iwl_mvm_vif *mvmvif;
+	struct iwl_mvm_vif *mvmvif = NULL;
 	struct iwl_mvm_time_event_data *te_data;
 	bool is_p2p = false;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	mvmvif = NULL;
 	spin_lock_bh(&mvm->time_event_lock);
 
 	/*
@@ -818,17 +820,14 @@ void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
 		}
 	}
 
-	/*
-	 * Iterate over the list of aux roc time events and find the time
-	 * event that is associated with a BSS interface.
-	 * This assumes that a BSS interface can have only a single time
-	 * event at any given time and this time event corresponds to a ROC
-	 * request
+	/* There can only be at most one AUX ROC time event, we just use the
+	 * list to simplify/unify code. Remove it if it exists.
 	 */
-	list_for_each_entry(te_data, &mvm->aux_roc_te_list, list) {
+	te_data = list_first_entry_or_null(&mvm->aux_roc_te_list,
+					   struct iwl_mvm_time_event_data,
+					   list);
+	if (te_data)
 		mvmvif = iwl_mvm_vif_from_mac80211(te_data->vif);
-		goto remove_te;
-	}
 
 remove_te:
 	spin_unlock_bh(&mvm->time_event_lock);
diff --git a/drivers/net/wireless/iwlwifi/mvm/tt.c b/drivers/net/wireless/iwlwifi/mvm/tt.c
index ba615ad2176c..80d07db6e7e8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tt.c
@@ -70,7 +70,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;
+	u32 duration = tt->params.ct_kill_duration;
 
 	if (test_bit(IWL_MVM_STATUS_HW_CTKILL, &mvm->status))
 		return;
@@ -223,7 +223,7 @@ static void check_exit_ctkill(struct work_struct *work)
 	tt = container_of(work, struct iwl_mvm_tt_mgmt, ct_kill_exit.work);
 	mvm = container_of(tt, struct iwl_mvm, thermal_throttle);
 
-	duration = tt->params->ct_kill_duration;
+	duration = tt->params.ct_kill_duration;
 
 	mutex_lock(&mvm->mutex);
 
@@ -247,7 +247,7 @@ static void check_exit_ctkill(struct work_struct *work)
 
 	IWL_DEBUG_TEMP(mvm, "NIC temperature: %d\n", temp);
 
-	if (temp <= tt->params->ct_kill_exit) {
+	if (temp <= tt->params.ct_kill_exit) {
 		mutex_unlock(&mvm->mutex);
 		iwl_mvm_exit_ctkill(mvm);
 		return;
@@ -325,7 +325,7 @@ void iwl_mvm_tt_tx_backoff(struct iwl_mvm *mvm, u32 backoff)
 
 void iwl_mvm_tt_handler(struct iwl_mvm *mvm)
 {
-	const struct iwl_tt_params *params = mvm->thermal_throttle.params;
+	struct iwl_tt_params *params = &mvm->thermal_throttle.params;
 	struct iwl_mvm_tt_mgmt *tt = &mvm->thermal_throttle;
 	s32 temperature = mvm->temperature;
 	bool throttle_enable = false;
@@ -340,7 +340,7 @@ void iwl_mvm_tt_handler(struct iwl_mvm *mvm)
 	}
 
 	if (params->support_ct_kill &&
-	    temperature <= tt->params->ct_kill_exit) {
+	    temperature <= params->ct_kill_exit) {
 		iwl_mvm_exit_ctkill(mvm);
 		return;
 	}
@@ -400,7 +400,7 @@ void iwl_mvm_tt_handler(struct iwl_mvm *mvm)
 	}
 }
 
-static const struct iwl_tt_params iwl7000_tt_params = {
+static const struct iwl_tt_params iwl_mvm_default_tt_params = {
 	.ct_kill_entry = 118,
 	.ct_kill_exit = 96,
 	.ct_kill_duration = 5,
@@ -422,38 +422,16 @@ static const struct iwl_tt_params iwl7000_tt_params = {
 	.support_tx_backoff = true,
 };
 
-static const struct iwl_tt_params iwl7000_high_temp_tt_params = {
-	.ct_kill_entry = 118,
-	.ct_kill_exit = 96,
-	.ct_kill_duration = 5,
-	.dynamic_smps_entry = 114,
-	.dynamic_smps_exit = 110,
-	.tx_protection_entry = 114,
-	.tx_protection_exit = 108,
-	.tx_backoff = {
-		{.temperature = 112, .backoff = 300},
-		{.temperature = 113, .backoff = 800},
-		{.temperature = 114, .backoff = 1500},
-		{.temperature = 115, .backoff = 3000},
-		{.temperature = 116, .backoff = 5000},
-		{.temperature = 117, .backoff = 10000},
-	},
-	.support_ct_kill = true,
-	.support_dynamic_smps = true,
-	.support_tx_protection = true,
-	.support_tx_backoff = true,
-};
-
 void iwl_mvm_tt_initialize(struct iwl_mvm *mvm, u32 min_backoff)
 {
 	struct iwl_mvm_tt_mgmt *tt = &mvm->thermal_throttle;
 
 	IWL_DEBUG_TEMP(mvm, "Initialize Thermal Throttling\n");
 
-	if (mvm->cfg->high_temp)
-		tt->params = &iwl7000_high_temp_tt_params;
+	if (mvm->cfg->thermal_params)
+		tt->params = *mvm->cfg->thermal_params;
 	else
-		tt->params = &iwl7000_tt_params;
+		tt->params = iwl_mvm_default_tt_params;
 
 	tt->throttle = false;
 	tt->dynamic_smps = false;
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/iwlwifi/mvm/tx.c
index ef32e177f662..7ba7a118ff5c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tx.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -70,6 +70,30 @@
 #include "mvm.h"
 #include "sta.h"
 
+static void
+iwl_mvm_bar_check_trigger(struct iwl_mvm *mvm, const u8 *addr,
+			  u16 tid, u16 ssn)
+{
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_ba *ba_trig;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_BA))
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_BA);
+	ba_trig = (void *)trig->data;
+
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, NULL, trig))
+		return;
+
+	if (!(le16_to_cpu(ba_trig->tx_bar) & BIT(tid)))
+		return;
+
+	iwl_mvm_fw_dbg_collect_trig(mvm, trig,
+				    "BAR sent to %pM, tid %d, ssn %d",
+				    addr, tid, ssn);
+}
+
 /*
  * Sets most of the Tx cmd's fields
  */
@@ -101,12 +125,15 @@ void iwl_mvm_set_tx_cmd(struct iwl_mvm *mvm, struct sk_buff *skb,
 	} else if (ieee80211_is_back_req(fc)) {
 		struct ieee80211_bar *bar = (void *)skb->data;
 		u16 control = le16_to_cpu(bar->control);
+		u16 ssn = le16_to_cpu(bar->start_seq_num);
 
 		tx_flags |= TX_CMD_FLG_ACK | TX_CMD_FLG_BAR;
 		tx_cmd->tid_tspec = (control &
 				     IEEE80211_BAR_CTRL_TID_INFO_MASK) >>
 			IEEE80211_BAR_CTRL_TID_INFO_SHIFT;
 		WARN_ON_ONCE(tx_cmd->tid_tspec >= IWL_MAX_TID_COUNT);
+		iwl_mvm_bar_check_trigger(mvm, bar->ra, tx_cmd->tid_tspec,
+					  ssn);
 	} else {
 		tx_cmd->tid_tspec = IWL_TID_NON_QOS;
 		if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ)
@@ -144,8 +171,8 @@ void iwl_mvm_set_tx_cmd(struct iwl_mvm *mvm, struct sk_buff *skb,
 	    !is_multicast_ether_addr(ieee80211_get_DA(hdr)))
 		tx_flags |= TX_CMD_FLG_PROT_REQUIRE;
 
-	if ((mvm->fw->ucode_capa.capa[0] &
-	     IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT) &&
+	if (fw_has_capa(&mvm->fw->ucode_capa,
+			IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT) &&
 	    ieee80211_action_contains_tpc(skb))
 		tx_flags |= TX_CMD_FLG_WRITE_TX_POWER;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index bc55a8b82db6..03f8e06dded7 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -584,7 +584,7 @@ void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
 	struct iwl_error_event_table table;
 	u32 base;
 
-	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_NEW_VERSION)) {
+	if (!fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_NEW_VERSION)) {
 		iwl_mvm_dump_nic_error_log_old(mvm);
 		return;
 	}
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index b18569734922..2ed1e4d2774d 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -6,7 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2007 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * 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
@@ -32,7 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -629,7 +629,18 @@ static int iwl_pci_resume(struct device *device)
 	if (!trans->op_mode)
 		return 0;
 
-	iwl_enable_rfkill_int(trans);
+	/*
+	 * On suspend, ict is disabled, and the interrupt mask
+	 * gets cleared. Reconfigure them both in case of d0i3
+	 * image. Otherwise, only enable rfkill interrupt (in
+	 * order to keep track of the rfkill status)
+	 */
+	if (trans->wowlan_d0i3) {
+		iwl_pcie_reset_ict(trans);
+		iwl_enable_interrupts(trans);
+	} else {
+		iwl_enable_rfkill_int(trans);
+	}
 
 	hw_rfkill = iwl_is_rfkill_set(trans);
 	iwl_trans_pcie_rf_kill(trans, hw_rfkill);
diff --git a/drivers/net/wireless/iwlwifi/pcie/internal.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index 01996c9d98a7..31f72a61cc3f 100644
--- a/drivers/net/wireless/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -1,7 +1,7 @@
 /******************************************************************************
  *
- * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2003 - 2015 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * Portions of this file are derived from the ipw3945 project, as well
  * as portions of the ieee80211 subsystem header files.
@@ -44,6 +44,15 @@
 #include "iwl-io.h"
 #include "iwl-op-mode.h"
 
+/*
+ * RX related structures and functions
+ */
+#define RX_NUM_QUEUES 1
+#define RX_POST_REQ_ALLOC 2
+#define RX_CLAIM_REQ_ALLOC 8
+#define RX_POOL_SIZE ((RX_CLAIM_REQ_ALLOC - RX_POST_REQ_ALLOC) * RX_NUM_QUEUES)
+#define RX_LOW_WATERMARK 8
+
 struct iwl_host_cmd;
 
 /*This file includes the declaration that are internal to the
@@ -77,29 +86,29 @@ struct isr_statistics {
  * struct iwl_rxq - Rx queue
  * @bd: driver's pointer to buffer of receive buffer descriptors (rbd)
  * @bd_dma: bus address of buffer of receive buffer descriptors (rbd)
- * @pool:
- * @queue:
  * @read: Shared index to newest available Rx buffer
  * @write: Shared index to oldest written Rx packet
  * @free_count: Number of pre-allocated buffers in rx_free
+ * @used_count: Number of RBDs handled to allocator to use for allocation
  * @write_actual:
- * @rx_free: list of free SKBs for use
- * @rx_used: List of Rx buffers with no SKB
+ * @rx_free: list of RBDs with allocated RB ready for use
+ * @rx_used: list of RBDs with no RB attached
  * @need_update: flag to indicate we need to update read/write index
  * @rb_stts: driver's pointer to receive buffer status
  * @rb_stts_dma: bus address of receive buffer status
  * @lock:
+ * @pool: initial pool of iwl_rx_mem_buffer for the queue
+ * @queue: actual rx queue
  *
  * NOTE:  rx_free and rx_used are used as a FIFO for iwl_rx_mem_buffers
  */
 struct iwl_rxq {
 	__le32 *bd;
 	dma_addr_t bd_dma;
-	struct iwl_rx_mem_buffer pool[RX_QUEUE_SIZE + RX_FREE_BUFFERS];
-	struct iwl_rx_mem_buffer *queue[RX_QUEUE_SIZE];
 	u32 read;
 	u32 write;
 	u32 free_count;
+	u32 used_count;
 	u32 write_actual;
 	struct list_head rx_free;
 	struct list_head rx_used;
@@ -107,6 +116,32 @@ struct iwl_rxq {
 	struct iwl_rb_status *rb_stts;
 	dma_addr_t rb_stts_dma;
 	spinlock_t lock;
+	struct iwl_rx_mem_buffer pool[RX_QUEUE_SIZE];
+	struct iwl_rx_mem_buffer *queue[RX_QUEUE_SIZE];
+};
+
+/**
+ * struct iwl_rb_allocator - Rx allocator
+ * @pool: initial pool of allocator
+ * @req_pending: number of requests the allcator had not processed yet
+ * @req_ready: number of requests honored and ready for claiming
+ * @rbd_allocated: RBDs with pages allocated and ready to be handled to
+ *	the queue. This is a list of &struct iwl_rx_mem_buffer
+ * @rbd_empty: RBDs with no page attached for allocator use. This is a list
+ *	of &struct iwl_rx_mem_buffer
+ * @lock: protects the rbd_allocated and rbd_empty lists
+ * @alloc_wq: work queue for background calls
+ * @rx_alloc: work struct for background calls
+ */
+struct iwl_rb_allocator {
+	struct iwl_rx_mem_buffer pool[RX_POOL_SIZE];
+	atomic_t req_pending;
+	atomic_t req_ready;
+	struct list_head rbd_allocated;
+	struct list_head rbd_empty;
+	spinlock_t lock;
+	struct workqueue_struct *alloc_wq;
+	struct work_struct rx_alloc;
 };
 
 struct iwl_dma_ptr {
@@ -250,7 +285,7 @@ iwl_pcie_get_scratchbuf_dma(struct iwl_txq *txq, int idx)
 /**
  * struct iwl_trans_pcie - PCIe transport specific data
  * @rxq: all the RX queue data
- * @rx_replenish: work that will be called when buffers need to be allocated
+ * @rba: allocator for RX replenishing
  * @drv - pointer to iwl_drv
  * @trans: pointer to the generic transport area
  * @scd_base_addr: scheduler sram base address in SRAM
@@ -273,7 +308,7 @@ iwl_pcie_get_scratchbuf_dma(struct iwl_txq *txq, int idx)
  */
 struct iwl_trans_pcie {
 	struct iwl_rxq rxq;
-	struct work_struct rx_replenish;
+	struct iwl_rb_allocator rba;
 	struct iwl_trans *trans;
 	struct iwl_drv *drv;
 
@@ -320,7 +355,7 @@ struct iwl_trans_pcie {
 
 	/*protect hw register */
 	spinlock_t reg_lock;
-	bool cmd_in_flight;
+	bool cmd_hold_nic_awake;
 	bool ref_cmd_in_flight;
 
 	/* protect ref counter */
diff --git a/drivers/net/wireless/iwlwifi/pcie/rx.c b/drivers/net/wireless/iwlwifi/pcie/rx.c
index 7ff69c642103..a3fbaa0ef5e0 100644
--- a/drivers/net/wireless/iwlwifi/pcie/rx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/rx.c
@@ -1,7 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
- * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
  *
  * Portions of this file are derived from the ipw3945 project, as well
  * as portions of the ieee80211 subsystem header files.
@@ -74,16 +74,29 @@
  * resets the Rx queue buffers with new memory.
  *
  * The management in the driver is as follows:
- * + A list of pre-allocated SKBs is stored in iwl->rxq->rx_free.  When
- *   iwl->rxq->free_count drops to or below RX_LOW_WATERMARK, work is scheduled
- *   to replenish the iwl->rxq->rx_free.
- * + In iwl_pcie_rx_replenish (scheduled) if 'processed' != 'read' then the
- *   iwl->rxq is replenished and the READ INDEX is updated (updating the
- *   'processed' and 'read' driver indexes as well)
+ * + A list of pre-allocated RBDs is stored in iwl->rxq->rx_free.
+ *   When the interrupt handler is called, the request is processed.
+ *   The page is either stolen - transferred to the upper layer
+ *   or reused - added immediately to the iwl->rxq->rx_free list.
+ * + When the page is stolen - the driver updates the matching queue's used
+ *   count, detaches the RBD and transfers it to the queue used list.
+ *   When there are two used RBDs - they are transferred to the allocator empty
+ *   list. Work is then scheduled for the allocator to start allocating
+ *   eight buffers.
+ *   When there are another 6 used RBDs - they are transferred to the allocator
+ *   empty list and the driver tries to claim the pre-allocated buffers and
+ *   add them to iwl->rxq->rx_free. If it fails - it continues to claim them
+ *   until ready.
+ *   When there are 8+ buffers in the free list - either from allocation or from
+ *   8 reused unstolen pages - restock is called to update the FW and indexes.
+ * + In order to make sure the allocator always has RBDs to use for allocation
+ *   the allocator has initial pool in the size of num_queues*(8-2) - the
+ *   maximum missing RBDs per allocation request (request posted with 2
+ *    empty RBDs, there is no guarantee when the other 6 RBDs are supplied).
+ *   The queues supplies the recycle of the rest of the RBDs.
  * + A received packet is processed and handed to the kernel network stack,
  *   detached from the iwl->rxq.  The driver 'processed' index is updated.
- * + The Host/Firmware iwl->rxq is replenished at irq thread time from the
- *   rx_free list. If there are no allocated buffers in iwl->rxq->rx_free,
+ * + If there are no allocated buffers in iwl->rxq->rx_free,
  *   the READ INDEX is not incremented and iwl->status(RX_STALLED) is set.
  *   If there were enough free buffers and RX_STALLED is set it is cleared.
  *
@@ -92,18 +105,32 @@
  *
  * iwl_rxq_alloc()            Allocates rx_free
  * iwl_pcie_rx_replenish()    Replenishes rx_free list from rx_used, and calls
- *                            iwl_pcie_rxq_restock
+ *                            iwl_pcie_rxq_restock.
+ *                            Used only during initialization.
  * iwl_pcie_rxq_restock()     Moves available buffers from rx_free into Rx
  *                            queue, updates firmware pointers, and updates
- *                            the WRITE index.  If insufficient rx_free buffers
- *                            are available, schedules iwl_pcie_rx_replenish
+ *                            the WRITE index.
+ * iwl_pcie_rx_allocator()     Background work for allocating pages.
  *
  * -- enable interrupts --
  * ISR - iwl_rx()             Detach iwl_rx_mem_buffers from pool up to the
  *                            READ INDEX, detaching the SKB from the pool.
  *                            Moves the packet buffer from queue to rx_used.
+ *                            Posts and claims requests to the allocator.
  *                            Calls iwl_pcie_rxq_restock to refill any empty
  *                            slots.
+ *
+ * RBD life-cycle:
+ *
+ * Init:
+ * rxq.pool -> rxq.rx_used -> rxq.rx_free -> rxq.queue
+ *
+ * Regular Receive interrupt:
+ * Page Stolen:
+ * rxq.queue -> rxq.rx_used -> allocator.rbd_empty ->
+ * allocator.rbd_allocated -> rxq.rx_free -> rxq.queue
+ * Page not Stolen:
+ * rxq.queue -> rxq.rx_free -> rxq.queue
  * ...
  *
  */
@@ -240,10 +267,6 @@ static void iwl_pcie_rxq_restock(struct iwl_trans *trans)
 		rxq->free_count--;
 	}
 	spin_unlock(&rxq->lock);
-	/* If the pre-allocated buffer pool is dropping low, schedule to
-	 * refill it */
-	if (rxq->free_count <= RX_LOW_WATERMARK)
-		schedule_work(&trans_pcie->rx_replenish);
 
 	/* If we've added more space for the firmware to place data, tell it.
 	 * Increment device's write pointer in multiples of 8. */
@@ -255,6 +278,44 @@ static void iwl_pcie_rxq_restock(struct iwl_trans *trans)
 }
 
 /*
+ * iwl_pcie_rx_alloc_page - allocates and returns a page.
+ *
+ */
+static struct page *iwl_pcie_rx_alloc_page(struct iwl_trans *trans)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_rxq *rxq = &trans_pcie->rxq;
+	struct page *page;
+	gfp_t gfp_mask = GFP_KERNEL;
+
+	if (rxq->free_count > RX_LOW_WATERMARK)
+		gfp_mask |= __GFP_NOWARN;
+
+	if (trans_pcie->rx_page_order > 0)
+		gfp_mask |= __GFP_COMP;
+
+	/* Alloc a new receive buffer */
+	page = alloc_pages(gfp_mask, trans_pcie->rx_page_order);
+	if (!page) {
+		if (net_ratelimit())
+			IWL_DEBUG_INFO(trans, "alloc_pages failed, order: %d\n",
+				       trans_pcie->rx_page_order);
+		/* Issue an error if the hardware has consumed more than half
+		 * of its free buffer list and we don't have enough
+		 * pre-allocated buffers.
+`		 */
+		if (rxq->free_count <= RX_LOW_WATERMARK &&
+		    iwl_rxq_space(rxq) > (RX_QUEUE_SIZE / 2) &&
+		    net_ratelimit())
+			IWL_CRIT(trans,
+				 "Failed to alloc_pages with GFP_KERNEL. Only %u free buffers remaining.\n",
+				 rxq->free_count);
+		return NULL;
+	}
+	return page;
+}
+
+/*
  * iwl_pcie_rxq_alloc_rbs - allocate a page for each used RBD
  *
  * A used RBD is an Rx buffer that has been given to the stack. To use it again
@@ -263,13 +324,12 @@ static void iwl_pcie_rxq_restock(struct iwl_trans *trans)
  * iwl_pcie_rxq_restock. The latter function will update the HW to use the newly
  * allocated buffers.
  */
-static void iwl_pcie_rxq_alloc_rbs(struct iwl_trans *trans, gfp_t priority)
+static void iwl_pcie_rxq_alloc_rbs(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rxq *rxq = &trans_pcie->rxq;
 	struct iwl_rx_mem_buffer *rxb;
 	struct page *page;
-	gfp_t gfp_mask = priority;
 
 	while (1) {
 		spin_lock(&rxq->lock);
@@ -279,32 +339,10 @@ static void iwl_pcie_rxq_alloc_rbs(struct iwl_trans *trans, gfp_t priority)
 		}
 		spin_unlock(&rxq->lock);
 
-		if (rxq->free_count > RX_LOW_WATERMARK)
-			gfp_mask |= __GFP_NOWARN;
-
-		if (trans_pcie->rx_page_order > 0)
-			gfp_mask |= __GFP_COMP;
-
 		/* Alloc a new receive buffer */
-		page = alloc_pages(gfp_mask, trans_pcie->rx_page_order);
-		if (!page) {
-			if (net_ratelimit())
-				IWL_DEBUG_INFO(trans, "alloc_pages failed, "
-					   "order: %d\n",
-					   trans_pcie->rx_page_order);
-
-			if ((rxq->free_count <= RX_LOW_WATERMARK) &&
-			    net_ratelimit())
-				IWL_CRIT(trans, "Failed to alloc_pages with %s."
-					 "Only %u free buffers remaining.\n",
-					 priority == GFP_ATOMIC ?
-					 "GFP_ATOMIC" : "GFP_KERNEL",
-					 rxq->free_count);
-			/* We don't reschedule replenish work here -- we will
-			 * call the restock method and if it still needs
-			 * more buffers it will schedule replenish */
+		page = iwl_pcie_rx_alloc_page(trans);
+		if (!page)
 			return;
-		}
 
 		spin_lock(&rxq->lock);
 
@@ -355,7 +393,7 @@ static void iwl_pcie_rxq_free_rbs(struct iwl_trans *trans)
 
 	lockdep_assert_held(&rxq->lock);
 
-	for (i = 0; i < RX_FREE_BUFFERS + RX_QUEUE_SIZE; i++) {
+	for (i = 0; i < RX_QUEUE_SIZE; i++) {
 		if (!rxq->pool[i].page)
 			continue;
 		dma_unmap_page(trans->dev, rxq->pool[i].page_dma,
@@ -372,32 +410,144 @@ static void iwl_pcie_rxq_free_rbs(struct iwl_trans *trans)
  * When moving to rx_free an page is allocated for the slot.
  *
  * Also restock the Rx queue via iwl_pcie_rxq_restock.
- * This is called as a scheduled work item (except for during initialization)
+ * This is called only during initialization
  */
-static void iwl_pcie_rx_replenish(struct iwl_trans *trans, gfp_t gfp)
+static void iwl_pcie_rx_replenish(struct iwl_trans *trans)
 {
-	iwl_pcie_rxq_alloc_rbs(trans, gfp);
+	iwl_pcie_rxq_alloc_rbs(trans);
 
 	iwl_pcie_rxq_restock(trans);
 }
 
-static void iwl_pcie_rx_replenish_work(struct work_struct *data)
+/*
+ * iwl_pcie_rx_allocator - Allocates pages in the background for RX queues
+ *
+ * Allocates for each received request 8 pages
+ * Called as a scheduled work item.
+ */
+static void iwl_pcie_rx_allocator(struct iwl_trans *trans)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_rb_allocator *rba = &trans_pcie->rba;
+
+	while (atomic_read(&rba->req_pending)) {
+		int i;
+		struct list_head local_empty;
+		struct list_head local_allocated;
+
+		INIT_LIST_HEAD(&local_allocated);
+		spin_lock(&rba->lock);
+		/* swap out the entire rba->rbd_empty to a local list */
+		list_replace_init(&rba->rbd_empty, &local_empty);
+		spin_unlock(&rba->lock);
+
+		for (i = 0; i < RX_CLAIM_REQ_ALLOC;) {
+			struct iwl_rx_mem_buffer *rxb;
+			struct page *page;
+
+			/* List should never be empty - each reused RBD is
+			 * returned to the list, and initial pool covers any
+			 * possible gap between the time the page is allocated
+			 * to the time the RBD is added.
+			 */
+			BUG_ON(list_empty(&local_empty));
+			/* Get the first rxb from the rbd list */
+			rxb = list_first_entry(&local_empty,
+					       struct iwl_rx_mem_buffer, list);
+			BUG_ON(rxb->page);
+
+			/* Alloc a new receive buffer */
+			page = iwl_pcie_rx_alloc_page(trans);
+			if (!page)
+				continue;
+			rxb->page = page;
+
+			/* Get physical address of the RB */
+			rxb->page_dma = dma_map_page(trans->dev, page, 0,
+					PAGE_SIZE << trans_pcie->rx_page_order,
+					DMA_FROM_DEVICE);
+			if (dma_mapping_error(trans->dev, rxb->page_dma)) {
+				rxb->page = NULL;
+				__free_pages(page, trans_pcie->rx_page_order);
+				continue;
+			}
+			/* dma address must be no more than 36 bits */
+			BUG_ON(rxb->page_dma & ~DMA_BIT_MASK(36));
+			/* and also 256 byte aligned! */
+			BUG_ON(rxb->page_dma & DMA_BIT_MASK(8));
+
+			/* move the allocated entry to the out list */
+			list_move(&rxb->list, &local_allocated);
+			i++;
+		}
+
+		spin_lock(&rba->lock);
+		/* add the allocated rbds to the allocator allocated list */
+		list_splice_tail(&local_allocated, &rba->rbd_allocated);
+		/* add the unused rbds back to the allocator empty list */
+		list_splice_tail(&local_empty, &rba->rbd_empty);
+		spin_unlock(&rba->lock);
+
+		atomic_dec(&rba->req_pending);
+		atomic_inc(&rba->req_ready);
+	}
+}
+
+/*
+ * iwl_pcie_rx_allocator_get - Returns the pre-allocated pages
+.*
+.* Called by queue when the queue posted allocation request and
+ * has freed 8 RBDs in order to restock itself.
+ */
+static int iwl_pcie_rx_allocator_get(struct iwl_trans *trans,
+				     struct iwl_rx_mem_buffer
+				     *out[RX_CLAIM_REQ_ALLOC])
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_rb_allocator *rba = &trans_pcie->rba;
+	int i;
+
+	if (atomic_dec_return(&rba->req_ready) < 0) {
+		atomic_inc(&rba->req_ready);
+		IWL_DEBUG_RX(trans,
+			     "Allocation request not ready, pending requests = %d\n",
+			     atomic_read(&rba->req_pending));
+		return -ENOMEM;
+	}
+
+	spin_lock(&rba->lock);
+	for (i = 0; i < RX_CLAIM_REQ_ALLOC; i++) {
+		/* Get next free Rx buffer, remove it from free list */
+		out[i] = list_first_entry(&rba->rbd_allocated,
+			       struct iwl_rx_mem_buffer, list);
+		list_del(&out[i]->list);
+	}
+	spin_unlock(&rba->lock);
+
+	return 0;
+}
+
+static void iwl_pcie_rx_allocator_work(struct work_struct *data)
 {
+	struct iwl_rb_allocator *rba_p =
+		container_of(data, struct iwl_rb_allocator, rx_alloc);
 	struct iwl_trans_pcie *trans_pcie =
-	    container_of(data, struct iwl_trans_pcie, rx_replenish);
+		container_of(rba_p, struct iwl_trans_pcie, rba);
 
-	iwl_pcie_rx_replenish(trans_pcie->trans, GFP_KERNEL);
+	iwl_pcie_rx_allocator(trans_pcie->trans);
 }
 
 static int iwl_pcie_rx_alloc(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rxq *rxq = &trans_pcie->rxq;
+	struct iwl_rb_allocator *rba = &trans_pcie->rba;
 	struct device *dev = trans->dev;
 
 	memset(&trans_pcie->rxq, 0, sizeof(trans_pcie->rxq));
 
 	spin_lock_init(&rxq->lock);
+	spin_lock_init(&rba->lock);
 
 	if (WARN_ON(rxq->bd || rxq->rb_stts))
 		return -EINVAL;
@@ -487,15 +637,49 @@ static void iwl_pcie_rx_init_rxb_lists(struct iwl_rxq *rxq)
 	INIT_LIST_HEAD(&rxq->rx_free);
 	INIT_LIST_HEAD(&rxq->rx_used);
 	rxq->free_count = 0;
+	rxq->used_count = 0;
 
-	for (i = 0; i < RX_FREE_BUFFERS + RX_QUEUE_SIZE; i++)
+	for (i = 0; i < RX_QUEUE_SIZE; i++)
 		list_add(&rxq->pool[i].list, &rxq->rx_used);
 }
 
+static void iwl_pcie_rx_init_rba(struct iwl_rb_allocator *rba)
+{
+	int i;
+
+	lockdep_assert_held(&rba->lock);
+
+	INIT_LIST_HEAD(&rba->rbd_allocated);
+	INIT_LIST_HEAD(&rba->rbd_empty);
+
+	for (i = 0; i < RX_POOL_SIZE; i++)
+		list_add(&rba->pool[i].list, &rba->rbd_empty);
+}
+
+static void iwl_pcie_rx_free_rba(struct iwl_trans *trans)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_rb_allocator *rba = &trans_pcie->rba;
+	int i;
+
+	lockdep_assert_held(&rba->lock);
+
+	for (i = 0; i < RX_POOL_SIZE; i++) {
+		if (!rba->pool[i].page)
+			continue;
+		dma_unmap_page(trans->dev, rba->pool[i].page_dma,
+			       PAGE_SIZE << trans_pcie->rx_page_order,
+			       DMA_FROM_DEVICE);
+		__free_pages(rba->pool[i].page, trans_pcie->rx_page_order);
+		rba->pool[i].page = NULL;
+	}
+}
+
 int iwl_pcie_rx_init(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rxq *rxq = &trans_pcie->rxq;
+	struct iwl_rb_allocator *rba = &trans_pcie->rba;
 	int i, err;
 
 	if (!rxq->bd) {
@@ -503,11 +687,21 @@ int iwl_pcie_rx_init(struct iwl_trans *trans)
 		if (err)
 			return err;
 	}
+	if (!rba->alloc_wq)
+		rba->alloc_wq = alloc_workqueue("rb_allocator",
+						WQ_HIGHPRI | WQ_UNBOUND, 1);
+	INIT_WORK(&rba->rx_alloc, iwl_pcie_rx_allocator_work);
+
+	spin_lock(&rba->lock);
+	atomic_set(&rba->req_pending, 0);
+	atomic_set(&rba->req_ready, 0);
+	/* free all first - we might be reconfigured for a different size */
+	iwl_pcie_rx_free_rba(trans);
+	iwl_pcie_rx_init_rba(rba);
+	spin_unlock(&rba->lock);
 
 	spin_lock(&rxq->lock);
 
-	INIT_WORK(&trans_pcie->rx_replenish, iwl_pcie_rx_replenish_work);
-
 	/* free all first - we might be reconfigured for a different size */
 	iwl_pcie_rxq_free_rbs(trans);
 	iwl_pcie_rx_init_rxb_lists(rxq);
@@ -522,7 +716,7 @@ int iwl_pcie_rx_init(struct iwl_trans *trans)
 	memset(rxq->rb_stts, 0, sizeof(*rxq->rb_stts));
 	spin_unlock(&rxq->lock);
 
-	iwl_pcie_rx_replenish(trans, GFP_KERNEL);
+	iwl_pcie_rx_replenish(trans);
 
 	iwl_pcie_rx_hw_init(trans, rxq);
 
@@ -537,6 +731,7 @@ void iwl_pcie_rx_free(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rxq *rxq = &trans_pcie->rxq;
+	struct iwl_rb_allocator *rba = &trans_pcie->rba;
 
 	/*if rxq->bd is NULL, it means that nothing has been allocated,
 	 * exit now */
@@ -545,7 +740,15 @@ void iwl_pcie_rx_free(struct iwl_trans *trans)
 		return;
 	}
 
-	cancel_work_sync(&trans_pcie->rx_replenish);
+	cancel_work_sync(&rba->rx_alloc);
+	if (rba->alloc_wq) {
+		destroy_workqueue(rba->alloc_wq);
+		rba->alloc_wq = NULL;
+	}
+
+	spin_lock(&rba->lock);
+	iwl_pcie_rx_free_rba(trans);
+	spin_unlock(&rba->lock);
 
 	spin_lock(&rxq->lock);
 	iwl_pcie_rxq_free_rbs(trans);
@@ -566,6 +769,43 @@ void iwl_pcie_rx_free(struct iwl_trans *trans)
 	rxq->rb_stts = NULL;
 }
 
+/*
+ * iwl_pcie_rx_reuse_rbd - Recycle used RBDs
+ *
+ * Called when a RBD can be reused. The RBD is transferred to the allocator.
+ * When there are 2 empty RBDs - a request for allocation is posted
+ */
+static void iwl_pcie_rx_reuse_rbd(struct iwl_trans *trans,
+				  struct iwl_rx_mem_buffer *rxb,
+				  struct iwl_rxq *rxq)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_rb_allocator *rba = &trans_pcie->rba;
+
+	/* Count the used RBDs */
+	rxq->used_count++;
+
+	/* Move the RBD to the used list, will be moved to allocator in batches
+	 * before claiming or posting a request*/
+	list_add_tail(&rxb->list, &rxq->rx_used);
+
+	/* If we have RX_POST_REQ_ALLOC new released rx buffers -
+	 * issue a request for allocator. Modulo RX_CLAIM_REQ_ALLOC is
+	 * used for the case we failed to claim RX_CLAIM_REQ_ALLOC,
+	 * after but we still need to post another request.
+	 */
+	if ((rxq->used_count % RX_CLAIM_REQ_ALLOC) == RX_POST_REQ_ALLOC) {
+		/* Move the 2 RBDs to the allocator ownership.
+		 Allocator has another 6 from pool for the request completion*/
+		spin_lock(&rba->lock);
+		list_splice_tail_init(&rxq->rx_used, &rba->rbd_empty);
+		spin_unlock(&rba->lock);
+
+		atomic_inc(&rba->req_pending);
+		queue_work(rba->alloc_wq, &rba->rx_alloc);
+	}
+}
+
 static void iwl_pcie_rx_handle_rb(struct iwl_trans *trans,
 				struct iwl_rx_mem_buffer *rxb)
 {
@@ -688,13 +928,13 @@ static void iwl_pcie_rx_handle_rb(struct iwl_trans *trans,
 			 */
 			__free_pages(rxb->page, trans_pcie->rx_page_order);
 			rxb->page = NULL;
-			list_add_tail(&rxb->list, &rxq->rx_used);
+			iwl_pcie_rx_reuse_rbd(trans, rxb, rxq);
 		} else {
 			list_add_tail(&rxb->list, &rxq->rx_free);
 			rxq->free_count++;
 		}
 	} else
-		list_add_tail(&rxb->list, &rxq->rx_used);
+		iwl_pcie_rx_reuse_rbd(trans, rxb, rxq);
 }
 
 /*
@@ -704,10 +944,7 @@ static void iwl_pcie_rx_handle(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_rxq *rxq = &trans_pcie->rxq;
-	u32 r, i;
-	u8 fill_rx = 0;
-	u32 count = 8;
-	int total_empty;
+	u32 r, i, j;
 
 restart:
 	spin_lock(&rxq->lock);
@@ -720,14 +957,6 @@ restart:
 	if (i == r)
 		IWL_DEBUG_RX(trans, "HW = SW = %d\n", r);
 
-	/* calculate total frames need to be restock after handling RX */
-	total_empty = r - rxq->write_actual;
-	if (total_empty < 0)
-		total_empty += RX_QUEUE_SIZE;
-
-	if (total_empty > (RX_QUEUE_SIZE / 2))
-		fill_rx = 1;
-
 	while (i != r) {
 		struct iwl_rx_mem_buffer *rxb;
 
@@ -739,29 +968,48 @@ restart:
 		iwl_pcie_rx_handle_rb(trans, rxb);
 
 		i = (i + 1) & RX_QUEUE_MASK;
-		/* If there are a lot of unused frames,
-		 * restock the Rx queue so ucode wont assert. */
-		if (fill_rx) {
-			count++;
-			if (count >= 8) {
-				rxq->read = i;
-				spin_unlock(&rxq->lock);
-				iwl_pcie_rx_replenish(trans, GFP_ATOMIC);
-				count = 0;
-				goto restart;
+
+		/* If we have RX_CLAIM_REQ_ALLOC released rx buffers -
+		 * try to claim the pre-allocated buffers from the allocator */
+		if (rxq->used_count >= RX_CLAIM_REQ_ALLOC) {
+			struct iwl_rb_allocator *rba = &trans_pcie->rba;
+			struct iwl_rx_mem_buffer *out[RX_CLAIM_REQ_ALLOC];
+
+			/* Add the remaining 6 empty RBDs for allocator use */
+			spin_lock(&rba->lock);
+			list_splice_tail_init(&rxq->rx_used, &rba->rbd_empty);
+			spin_unlock(&rba->lock);
+
+			/* If not ready - continue, will try to reclaim later.
+			* No need to reschedule work - allocator exits only on
+			* success */
+			if (!iwl_pcie_rx_allocator_get(trans, out)) {
+				/* If success - then RX_CLAIM_REQ_ALLOC
+				 * buffers were retrieved and should be added
+				 * to free list */
+				rxq->used_count -= RX_CLAIM_REQ_ALLOC;
+				for (j = 0; j < RX_CLAIM_REQ_ALLOC; j++) {
+					list_add_tail(&out[j]->list,
+						      &rxq->rx_free);
+					rxq->free_count++;
+				}
 			}
 		}
+		/* handle restock for two cases:
+		* - we just pulled buffers from the allocator
+		* - we have 8+ unstolen pages accumulated */
+		if (rxq->free_count >=  RX_CLAIM_REQ_ALLOC) {
+			rxq->read = i;
+			spin_unlock(&rxq->lock);
+			iwl_pcie_rxq_restock(trans);
+			goto restart;
+		}
 	}
 
 	/* Backtrack one entry */
 	rxq->read = i;
 	spin_unlock(&rxq->lock);
 
-	if (fill_rx)
-		iwl_pcie_rx_replenish(trans, GFP_ATOMIC);
-	else
-		iwl_pcie_rxq_restock(trans);
-
 	if (trans_pcie->napi.poll)
 		napi_gro_flush(&trans_pcie->napi, false);
 }
@@ -775,6 +1023,7 @@ static void iwl_pcie_irq_handle_error(struct iwl_trans *trans)
 
 	/* W/A for WiFi/WiMAX coex and WiMAX own the RF */
 	if (trans->cfg->internal_wimax_coex &&
+	    !trans->cfg->apmg_not_supported &&
 	    (!(iwl_read_prph(trans, APMG_CLK_CTRL_REG) &
 			     APMS_CLK_VAL_MRB_FUNC_MODE) ||
 	     (iwl_read_prph(trans, APMG_PS_CTRL_REG) &
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index d6f6515fe663..43ae658af6ec 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -101,14 +101,26 @@ static void iwl_pcie_free_fw_monitor(struct iwl_trans *trans)
 	trans_pcie->fw_mon_size = 0;
 }
 
-static void iwl_pcie_alloc_fw_monitor(struct iwl_trans *trans)
+static void iwl_pcie_alloc_fw_monitor(struct iwl_trans *trans, u8 max_power)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct page *page = NULL;
 	dma_addr_t phys;
-	u32 size;
+	u32 size = 0;
 	u8 power;
 
+	if (!max_power) {
+		/* default max_power is maximum */
+		max_power = 26;
+	} else {
+		max_power += 11;
+	}
+
+	if (WARN(max_power > 26,
+		 "External buffer size for monitor is too big %d, check the FW TLV\n",
+		 max_power))
+		return;
+
 	if (trans_pcie->fw_mon_page) {
 		dma_sync_single_for_device(trans->dev, trans_pcie->fw_mon_phys,
 					   trans_pcie->fw_mon_size,
@@ -117,7 +129,7 @@ static void iwl_pcie_alloc_fw_monitor(struct iwl_trans *trans)
 	}
 
 	phys = 0;
-	for (power = 26; power >= 11; power--) {
+	for (power = max_power; power >= 11; power--) {
 		int order;
 
 		size = BIT(power);
@@ -143,6 +155,12 @@ static void iwl_pcie_alloc_fw_monitor(struct iwl_trans *trans)
 	if (WARN_ON_ONCE(!page))
 		return;
 
+	if (power != max_power)
+		IWL_ERR(trans,
+			"Sorry - debug buffer is only %luK while you requested %luK\n",
+			(unsigned long)BIT(power - 10),
+			(unsigned long)BIT(max_power - 10));
+
 	trans_pcie->fw_mon_page = page;
 	trans_pcie->fw_mon_phys = phys;
 	trans_pcie->fw_mon_size = size;
@@ -164,6 +182,9 @@ static void iwl_trans_pcie_write_shr(struct iwl_trans *trans, u32 reg, u32 val)
 
 static void iwl_pcie_set_pwr(struct iwl_trans *trans, bool vaux)
 {
+	if (!trans->cfg->apmg_not_supported)
+		return;
+
 	if (vaux && pci_pme_capable(to_pci_dev(trans->dev), PCI_D3cold))
 		iwl_set_bits_mask_prph(trans, APMG_PS_CTRL_REG,
 				       APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
@@ -297,7 +318,7 @@ static int iwl_pcie_apm_init(struct iwl_trans *trans)
 	 * bits do not disable clocks.  This preserves any hardware
 	 * bits already set by default in "CLK_CTRL_REG" after reset.
 	 */
-	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+	if (!trans->cfg->apmg_not_supported) {
 		iwl_write_prph(trans, APMG_CLK_EN_REG,
 			       APMG_CLK_VAL_DMA_CLK_RQT);
 		udelay(20);
@@ -497,8 +518,7 @@ static int iwl_pcie_nic_init(struct iwl_trans *trans)
 
 	spin_unlock(&trans_pcie->irq_lock);
 
-	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
-		iwl_pcie_set_pwr(trans, false);
+	iwl_pcie_set_pwr(trans, false);
 
 	iwl_op_mode_nic_config(trans->op_mode);
 
@@ -834,7 +854,7 @@ static void iwl_pcie_apply_destination(struct iwl_trans *trans)
 		 get_fw_dbg_mode_string(dest->monitor_mode));
 
 	if (dest->monitor_mode == EXTERNAL_MODE)
-		iwl_pcie_alloc_fw_monitor(trans);
+		iwl_pcie_alloc_fw_monitor(trans, dest->size_power);
 	else
 		IWL_WARN(trans, "PCI should have external buffer debug\n");
 
@@ -908,7 +928,7 @@ static int iwl_pcie_load_given_ucode(struct iwl_trans *trans,
 	/* supported for 7000 only for the moment */
 	if (iwlwifi_mod_params.fw_monitor &&
 	    trans->cfg->device_family == IWL_DEVICE_FAMILY_7000) {
-		iwl_pcie_alloc_fw_monitor(trans);
+		iwl_pcie_alloc_fw_monitor(trans, 0);
 
 		if (trans_pcie->fw_mon_size) {
 			iwl_write_prph(trans, MON_BUFF_BASE_ADDR,
@@ -955,12 +975,8 @@ static int iwl_pcie_load_given_ucode_8000(struct iwl_trans *trans,
 		return ret;
 
 	/* load to FW the binary sections of CPU2 */
-	ret = iwl_pcie_load_cpu_sections_8000(trans, image, 2,
-					      &first_ucode_section);
-	if (ret)
-		return ret;
-
-	return 0;
+	return iwl_pcie_load_cpu_sections_8000(trans, image, 2,
+					       &first_ucode_section);
 }
 
 static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
@@ -1049,7 +1065,7 @@ static void iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 		iwl_pcie_rx_stop(trans);
 
 		/* Power-down device's busmaster DMA clocks */
-		if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) {
+		if (!trans->cfg->apmg_not_supported) {
 			iwl_write_prph(trans, APMG_CLK_DIS_REG,
 				       APMG_CLK_VAL_DMA_CLK_RQT);
 			udelay(5);
@@ -1346,14 +1362,13 @@ void iwl_trans_pcie_free(struct iwl_trans *trans)
 	iounmap(trans_pcie->hw_base);
 	pci_release_regions(trans_pcie->pci_dev);
 	pci_disable_device(trans_pcie->pci_dev);
-	kmem_cache_destroy(trans->dev_cmd_pool);
 
 	if (trans_pcie->napi.poll)
 		netif_napi_del(&trans_pcie->napi);
 
 	iwl_pcie_free_fw_monitor(trans);
 
-	kfree(trans);
+	iwl_trans_free(trans);
 }
 
 static void iwl_trans_pcie_set_pmi(struct iwl_trans *trans, bool state)
@@ -1372,7 +1387,7 @@ static bool iwl_trans_pcie_grab_nic_access(struct iwl_trans *trans, bool silent,
 
 	spin_lock_irqsave(&trans_pcie->reg_lock, *flags);
 
-	if (trans_pcie->cmd_in_flight)
+	if (trans_pcie->cmd_hold_nic_awake)
 		goto out;
 
 	/* this bit wakes up the NIC */
@@ -1438,7 +1453,7 @@ static void iwl_trans_pcie_release_nic_access(struct iwl_trans *trans,
 	 */
 	__acquire(&trans_pcie->reg_lock);
 
-	if (trans_pcie->cmd_in_flight)
+	if (trans_pcie->cmd_hold_nic_awake)
 		goto out;
 
 	__iwl_trans_pcie_clear_bit(trans, CSR_GP_CNTRL,
@@ -2200,6 +2215,29 @@ static u32 iwl_trans_pcie_fh_regs_dump(struct iwl_trans *trans,
 	return sizeof(**data) + fh_regs_len;
 }
 
+static u32
+iwl_trans_pci_dump_marbh_monitor(struct iwl_trans *trans,
+				 struct iwl_fw_error_dump_fw_mon *fw_mon_data,
+				 u32 monitor_len)
+{
+	u32 buf_size_in_dwords = (monitor_len >> 2);
+	u32 *buffer = (u32 *)fw_mon_data->data;
+	unsigned long flags;
+	u32 i;
+
+	if (!iwl_trans_grab_nic_access(trans, false, &flags))
+		return 0;
+
+	__iwl_write_prph(trans, MON_DMARB_RD_CTL_ADDR, 0x1);
+	for (i = 0; i < buf_size_in_dwords; i++)
+		buffer[i] = __iwl_read_prph(trans, MON_DMARB_RD_DATA_ADDR);
+	__iwl_write_prph(trans, MON_DMARB_RD_CTL_ADDR, 0x0);
+
+	iwl_trans_release_nic_access(trans, &flags);
+
+	return monitor_len;
+}
+
 static
 struct iwl_trans_dump_data *iwl_trans_pcie_dump_data(struct iwl_trans *trans)
 {
@@ -2252,7 +2290,8 @@ struct iwl_trans_dump_data *iwl_trans_pcie_dump_data(struct iwl_trans *trans)
 		      trans->dbg_dest_tlv->end_shift;
 
 		/* Make "end" point to the actual end */
-		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000 ||
+		    trans->dbg_dest_tlv->monitor_mode == MARBH_MODE)
 			end += (1 << trans->dbg_dest_tlv->end_shift);
 		monitor_len = end - base;
 		len += sizeof(*data) + sizeof(struct iwl_fw_error_dump_fw_mon) +
@@ -2328,9 +2367,6 @@ struct iwl_trans_dump_data *iwl_trans_pcie_dump_data(struct iwl_trans *trans)
 
 		len += sizeof(*data) + sizeof(*fw_mon_data);
 		if (trans_pcie->fw_mon_page) {
-			data->len = cpu_to_le32(trans_pcie->fw_mon_size +
-						sizeof(*fw_mon_data));
-
 			/*
 			 * The firmware is now asserted, it won't write anything
 			 * to the buffer. CPU can take ownership to fetch the
@@ -2345,10 +2381,8 @@ struct iwl_trans_dump_data *iwl_trans_pcie_dump_data(struct iwl_trans *trans)
 			       page_address(trans_pcie->fw_mon_page),
 			       trans_pcie->fw_mon_size);
 
-			len += trans_pcie->fw_mon_size;
-		} else {
-			/* If we are here then the buffer is internal */
-
+			monitor_len = trans_pcie->fw_mon_size;
+		} else if (trans->dbg_dest_tlv->monitor_mode == SMEM_MODE) {
 			/*
 			 * Update pointers to reflect actual values after
 			 * shifting
@@ -2357,10 +2391,18 @@ struct iwl_trans_dump_data *iwl_trans_pcie_dump_data(struct iwl_trans *trans)
 			       trans->dbg_dest_tlv->base_shift;
 			iwl_trans_read_mem(trans, base, fw_mon_data->data,
 					   monitor_len / sizeof(u32));
-			data->len = cpu_to_le32(sizeof(*fw_mon_data) +
-						monitor_len);
-			len += monitor_len;
+		} else if (trans->dbg_dest_tlv->monitor_mode == MARBH_MODE) {
+			monitor_len =
+				iwl_trans_pci_dump_marbh_monitor(trans,
+								 fw_mon_data,
+								 monitor_len);
+		} else {
+			/* Didn't match anything - output no monitor data */
+			monitor_len = 0;
 		}
+
+		len += monitor_len;
+		data->len = cpu_to_le32(monitor_len + sizeof(*fw_mon_data));
 	}
 
 	dump_data->len = len;
@@ -2419,18 +2461,13 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	u16 pci_cmd;
 	int err;
 
-	trans = kzalloc(sizeof(struct iwl_trans) +
-			sizeof(struct iwl_trans_pcie), GFP_KERNEL);
-	if (!trans) {
-		err = -ENOMEM;
-		goto out;
-	}
+	trans = iwl_trans_alloc(sizeof(struct iwl_trans_pcie),
+				&pdev->dev, cfg, &trans_ops_pcie, 0);
+	if (!trans)
+		return ERR_PTR(-ENOMEM);
 
 	trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
-	trans->ops = &trans_ops_pcie;
-	trans->cfg = cfg;
-	trans_lockdep_init(trans);
 	trans_pcie->trans = trans;
 	spin_lock_init(&trans_pcie->irq_lock);
 	spin_lock_init(&trans_pcie->reg_lock);
@@ -2554,25 +2591,8 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	/* Initialize the wait queue for commands */
 	init_waitqueue_head(&trans_pcie->wait_command_queue);
 
-	snprintf(trans->dev_cmd_pool_name, sizeof(trans->dev_cmd_pool_name),
-		 "iwl_cmd_pool:%s", dev_name(trans->dev));
-
-	trans->dev_cmd_headroom = 0;
-	trans->dev_cmd_pool =
-		kmem_cache_create(trans->dev_cmd_pool_name,
-				  sizeof(struct iwl_device_cmd)
-				  + trans->dev_cmd_headroom,
-				  sizeof(void *),
-				  SLAB_HWCACHE_ALIGN,
-				  NULL);
-
-	if (!trans->dev_cmd_pool) {
-		err = -ENOMEM;
-		goto out_pci_disable_msi;
-	}
-
 	if (iwl_pcie_alloc_ict(trans))
-		goto out_free_cmd_pool;
+		goto out_pci_disable_msi;
 
 	err = request_threaded_irq(pdev->irq, iwl_pcie_isr,
 				   iwl_pcie_irq_handler,
@@ -2589,8 +2609,6 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 
 out_free_ict:
 	iwl_pcie_free_ict(trans);
-out_free_cmd_pool:
-	kmem_cache_destroy(trans->dev_cmd_pool);
 out_pci_disable_msi:
 	pci_disable_msi(pdev);
 out_pci_release_regions:
@@ -2598,7 +2616,6 @@ out_pci_release_regions:
 out_pci_disable_device:
 	pci_disable_device(pdev);
 out_no_pci:
-	kfree(trans);
-out:
+	iwl_trans_free(trans);
 	return ERR_PTR(err);
 }
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 06952aadfd7b..2b86c2135de3 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -1039,22 +1039,16 @@ static int iwl_pcie_set_cmd_in_flight(struct iwl_trans *trans,
 		iwl_trans_pcie_ref(trans);
 	}
 
-	if (trans_pcie->cmd_in_flight)
-		return 0;
-
-	trans_pcie->cmd_in_flight = true;
-
 	/*
 	 * wake up the NIC to make sure that the firmware will see the host
 	 * command - we will let the NIC sleep once all the host commands
 	 * returned. This needs to be done only on NICs that have
 	 * apmg_wake_up_wa set.
 	 */
-	if (trans->cfg->base_params->apmg_wake_up_wa) {
+	if (trans->cfg->base_params->apmg_wake_up_wa &&
+	    !trans_pcie->cmd_hold_nic_awake) {
 		__iwl_trans_pcie_set_bit(trans, CSR_GP_CNTRL,
 					 CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
-		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
-			udelay(2);
 
 		ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
 				   CSR_GP_CNTRL_REG_VAL_MAC_ACCESS_EN,
@@ -1064,10 +1058,10 @@ static int iwl_pcie_set_cmd_in_flight(struct iwl_trans *trans,
 		if (ret < 0) {
 			__iwl_trans_pcie_clear_bit(trans, CSR_GP_CNTRL,
 					CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
-			trans_pcie->cmd_in_flight = false;
 			IWL_ERR(trans, "Failed to wake NIC for hcmd\n");
 			return -EIO;
 		}
+		trans_pcie->cmd_hold_nic_awake = true;
 	}
 
 	return 0;
@@ -1085,15 +1079,14 @@ static int iwl_pcie_clear_cmd_in_flight(struct iwl_trans *trans)
 		iwl_trans_pcie_unref(trans);
 	}
 
-	if (WARN_ON(!trans_pcie->cmd_in_flight))
-		return 0;
-
-	trans_pcie->cmd_in_flight = false;
+	if (trans->cfg->base_params->apmg_wake_up_wa) {
+		if (WARN_ON(!trans_pcie->cmd_hold_nic_awake))
+			return 0;
 
-	if (trans->cfg->base_params->apmg_wake_up_wa)
+		trans_pcie->cmd_hold_nic_awake = false;
 		__iwl_trans_pcie_clear_bit(trans, CSR_GP_CNTRL,
-					CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
-
+					   CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+	}
 	return 0;
 }
 
diff --git a/drivers/net/wireless/libertas/cfg.c b/drivers/net/wireless/libertas/cfg.c
index 1a4d558022d8..8317afd065b4 100644
--- a/drivers/net/wireless/libertas/cfg.c
+++ b/drivers/net/wireless/libertas/cfg.c
@@ -835,14 +835,13 @@ static int lbs_cfg_scan(struct wiphy *wiphy,
  * Events
  */
 
-void lbs_send_disconnect_notification(struct lbs_private *priv)
+void lbs_send_disconnect_notification(struct lbs_private *priv,
+				      bool locally_generated)
 {
 	lbs_deb_enter(LBS_DEB_CFG80211);
 
-	cfg80211_disconnected(priv->dev,
-		0,
-		NULL, 0,
-		GFP_KERNEL);
+	cfg80211_disconnected(priv->dev, 0, NULL, 0, locally_generated,
+			      GFP_KERNEL);
 
 	lbs_deb_leave(LBS_DEB_CFG80211);
 }
@@ -1458,7 +1457,7 @@ int lbs_disconnect(struct lbs_private *priv, u16 reason)
 
 	cfg80211_disconnected(priv->dev,
 			reason,
-			NULL, 0,
+			NULL, 0, true,
 			GFP_KERNEL);
 	priv->connect_status = LBS_DISCONNECTED;
 
@@ -2031,7 +2030,7 @@ static int lbs_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 	ret = lbs_cmd_with_response(priv, CMD_802_11_AD_HOC_STOP, &cmd);
 
 	/* TODO: consider doing this at MACREG_INT_CODE_ADHOC_BCN_LOST time */
-	lbs_mac_event_disconnected(priv);
+	lbs_mac_event_disconnected(priv, true);
 
 	lbs_deb_leave_args(LBS_DEB_CFG80211, "ret %d", ret);
 	return ret;
diff --git a/drivers/net/wireless/libertas/cfg.h b/drivers/net/wireless/libertas/cfg.h
index 10995f59fe34..acccc2922401 100644
--- a/drivers/net/wireless/libertas/cfg.h
+++ b/drivers/net/wireless/libertas/cfg.h
@@ -10,7 +10,8 @@ struct wireless_dev *lbs_cfg_alloc(struct device *dev);
 int lbs_cfg_register(struct lbs_private *priv);
 void lbs_cfg_free(struct lbs_private *priv);
 
-void lbs_send_disconnect_notification(struct lbs_private *priv);
+void lbs_send_disconnect_notification(struct lbs_private *priv,
+				      bool locally_generated);
 void lbs_send_mic_failureevent(struct lbs_private *priv, u32 event);
 
 void lbs_scan_done(struct lbs_private *priv);
diff --git a/drivers/net/wireless/libertas/cmd.h b/drivers/net/wireless/libertas/cmd.h
index 4279e8ab95f2..0c5444b02c64 100644
--- a/drivers/net/wireless/libertas/cmd.h
+++ b/drivers/net/wireless/libertas/cmd.h
@@ -68,7 +68,8 @@ int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len);
 
 /* From cmdresp.c */
 
-void lbs_mac_event_disconnected(struct lbs_private *priv);
+void lbs_mac_event_disconnected(struct lbs_private *priv,
+				bool locally_generated);
 
 
 
diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/libertas/cmdresp.c
index 65f18f1e869c..e5442e8956f7 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/libertas/cmdresp.c
@@ -19,10 +19,13 @@
  * reset link state etc.
  *
  * @priv:	A pointer to struct lbs_private structure
+ * @locally_generated: indicates disconnect was requested locally
+ *		(usually by userspace)
  *
  * returns:	n/a
  */
-void lbs_mac_event_disconnected(struct lbs_private *priv)
+void lbs_mac_event_disconnected(struct lbs_private *priv,
+				bool locally_generated)
 {
 	if (priv->connect_status != LBS_CONNECTED)
 		return;
@@ -36,7 +39,7 @@ void lbs_mac_event_disconnected(struct lbs_private *priv)
 	msleep_interruptible(1000);
 
 	if (priv->wdev->iftype == NL80211_IFTYPE_STATION)
-		lbs_send_disconnect_notification(priv);
+		lbs_send_disconnect_notification(priv, locally_generated);
 
 	/* report disconnect to upper layer */
 	netif_stop_queue(priv->dev);
@@ -229,17 +232,17 @@ int lbs_process_event(struct lbs_private *priv, u32 event)
 
 	case MACREG_INT_CODE_DEAUTHENTICATED:
 		lbs_deb_cmd("EVENT: deauthenticated\n");
-		lbs_mac_event_disconnected(priv);
+		lbs_mac_event_disconnected(priv, false);
 		break;
 
 	case MACREG_INT_CODE_DISASSOCIATED:
 		lbs_deb_cmd("EVENT: disassociated\n");
-		lbs_mac_event_disconnected(priv);
+		lbs_mac_event_disconnected(priv, false);
 		break;
 
 	case MACREG_INT_CODE_LINK_LOST_NO_SCAN:
 		lbs_deb_cmd("EVENT: link lost\n");
-		lbs_mac_event_disconnected(priv);
+		lbs_mac_event_disconnected(priv, true);
 		break;
 
 	case MACREG_INT_CODE_PS_SLEEP:
diff --git a/drivers/net/wireless/libertas_tf/if_usb.c b/drivers/net/wireless/libertas_tf/if_usb.c
index 1a20cee5febe..799a2efe5793 100644
--- a/drivers/net/wireless/libertas_tf/if_usb.c
+++ b/drivers/net/wireless/libertas_tf/if_usb.c
@@ -821,15 +821,15 @@ static int if_usb_prog_firmware(struct if_usb_card *cardp)
 
 	lbtf_deb_enter(LBTF_DEB_USB);
 
-	kparam_block_sysfs_write(fw_name);
+	kernel_param_lock(THIS_MODULE);
 	ret = request_firmware(&cardp->fw, lbtf_fw_name, &cardp->udev->dev);
 	if (ret < 0) {
 		pr_err("request_firmware() failed with %#x\n", ret);
 		pr_err("firmware %s not found\n", lbtf_fw_name);
-		kparam_unblock_sysfs_write(fw_name);
+		kernel_param_unlock(THIS_MODULE);
 		goto done;
 	}
-	kparam_unblock_sysfs_write(fw_name);
+	kernel_param_unlock(THIS_MODULE);
 
 	if (check_fwfile_format(cardp->fw->data, cardp->fw->size))
 		goto release_fw;
diff --git a/drivers/net/wireless/libertas_tf/main.c b/drivers/net/wireless/libertas_tf/main.c
index ed02e4bf2c26..a47f0acc099a 100644
--- a/drivers/net/wireless/libertas_tf/main.c
+++ b/drivers/net/wireless/libertas_tf/main.c
@@ -439,7 +439,7 @@ static u64 lbtf_op_prepare_multicast(struct ieee80211_hw *hw,
 	return mc_count;
 }
 
-#define SUPPORTED_FIF_FLAGS  (FIF_PROMISC_IN_BSS | FIF_ALLMULTI)
+#define SUPPORTED_FIF_FLAGS  FIF_ALLMULTI
 static void lbtf_op_configure_filter(struct ieee80211_hw *hw,
 			unsigned int changed_flags,
 			unsigned int *new_flags,
@@ -458,10 +458,7 @@ static void lbtf_op_configure_filter(struct ieee80211_hw *hw,
 		return;
 	}
 
-	if (*new_flags & (FIF_PROMISC_IN_BSS))
-		priv->mac_control |= CMD_ACT_MAC_PROMISCUOUS_ENABLE;
-	else
-		priv->mac_control &= ~CMD_ACT_MAC_PROMISCUOUS_ENABLE;
+	priv->mac_control &= ~CMD_ACT_MAC_PROMISCUOUS_ENABLE;
 	if (*new_flags & (FIF_ALLMULTI) ||
 	    multicast > MRVDRV_MAX_MULTICAST_LIST_SIZE) {
 		priv->mac_control |= CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
@@ -637,7 +634,7 @@ struct lbtf_private *lbtf_add_card(void *card, struct device *dmdev)
 	priv->tx_skb = NULL;
 
 	hw->queues = 1;
-	hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	ieee80211_hw_set(hw, HOST_BROADCAST_PS_BUFFERING);
 	hw->extra_tx_headroom = sizeof(struct txpd);
 	memcpy(priv->channels, lbtf_channels, sizeof(lbtf_channels));
 	memcpy(priv->rates, lbtf_rates, sizeof(lbtf_rates));
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index d5c0a1af08b9..99e873dc8684 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -1286,7 +1286,7 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw,
 	if (control->sta)
 		hwsim_check_sta_magic(control->sta);
 
-	if (hw->flags & IEEE80211_HW_SUPPORTS_RC_TABLE)
+	if (ieee80211_hw_check(hw, SUPPORTS_RC_TABLE))
 		ieee80211_get_tx_rates(txi->control.vif, control->sta, skb,
 				       txi->control.rates,
 				       ARRAY_SIZE(txi->control.rates));
@@ -1395,7 +1395,7 @@ static void mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
 {
 	u32 _pid = ACCESS_ONCE(wmediumd_portid);
 
-	if (hw->flags & IEEE80211_HW_SUPPORTS_RC_TABLE) {
+	if (ieee80211_hw_check(hw, SUPPORTS_RC_TABLE)) {
 		struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
 		ieee80211_get_tx_rates(txi->control.vif, NULL, skb,
 				       txi->control.rates,
@@ -1432,7 +1432,7 @@ static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
 	if (skb == NULL)
 		return;
 	info = IEEE80211_SKB_CB(skb);
-	if (hw->flags & IEEE80211_HW_SUPPORTS_RC_TABLE)
+	if (ieee80211_hw_check(hw, SUPPORTS_RC_TABLE))
 		ieee80211_get_tx_rates(vif, NULL, skb,
 				       info->control.rates,
 				       ARRAY_SIZE(info->control.rates));
@@ -1554,8 +1554,6 @@ static void mac80211_hwsim_configure_filter(struct ieee80211_hw *hw,
 	wiphy_debug(hw->wiphy, "%s\n", __func__);
 
 	data->rx_filter = 0;
-	if (*total_flags & FIF_PROMISC_IN_BSS)
-		data->rx_filter |= FIF_PROMISC_IN_BSS;
 	if (*total_flags & FIF_ALLMULTI)
 		data->rx_filter |= FIF_ALLMULTI;
 
@@ -2393,15 +2391,16 @@ static int mac80211_hwsim_new_radio(struct genl_info *info,
 	if (param->p2p_device)
 		hw->wiphy->interface_modes |= BIT(NL80211_IFTYPE_P2P_DEVICE);
 
-	hw->flags = IEEE80211_HW_MFP_CAPABLE |
-		    IEEE80211_HW_SIGNAL_DBM |
-		    IEEE80211_HW_AMPDU_AGGREGATION |
-		    IEEE80211_HW_WANT_MONITOR_VIF |
-		    IEEE80211_HW_QUEUE_CONTROL |
-		    IEEE80211_HW_SUPPORTS_HT_CCK_RATES |
-		    IEEE80211_HW_CHANCTX_STA_CSA;
+	ieee80211_hw_set(hw, SUPPORT_FAST_XMIT);
+	ieee80211_hw_set(hw, CHANCTX_STA_CSA);
+	ieee80211_hw_set(hw, SUPPORTS_HT_CCK_RATES);
+	ieee80211_hw_set(hw, QUEUE_CONTROL);
+	ieee80211_hw_set(hw, WANT_MONITOR_VIF);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(hw, MFP_CAPABLE);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
 	if (rctbl)
-		hw->flags |= IEEE80211_HW_SUPPORTS_RC_TABLE;
+		ieee80211_hw_set(hw, SUPPORTS_RC_TABLE);
 
 	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS |
 			    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
@@ -2438,6 +2437,31 @@ static int mac80211_hwsim_new_radio(struct genl_info *info,
 			sband->n_channels = ARRAY_SIZE(hwsim_channels_5ghz);
 			sband->bitrates = data->rates + 4;
 			sband->n_bitrates = ARRAY_SIZE(hwsim_rates) - 4;
+
+			sband->vht_cap.vht_supported = true;
+			sband->vht_cap.cap =
+				IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
+				IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ |
+				IEEE80211_VHT_CAP_RXLDPC |
+				IEEE80211_VHT_CAP_SHORT_GI_80 |
+				IEEE80211_VHT_CAP_SHORT_GI_160 |
+				IEEE80211_VHT_CAP_TXSTBC |
+				IEEE80211_VHT_CAP_RXSTBC_1 |
+				IEEE80211_VHT_CAP_RXSTBC_2 |
+				IEEE80211_VHT_CAP_RXSTBC_3 |
+				IEEE80211_VHT_CAP_RXSTBC_4 |
+				IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK;
+			sband->vht_cap.vht_mcs.rx_mcs_map =
+				cpu_to_le16(IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
+					    IEEE80211_VHT_MCS_SUPPORT_0_9 << 2 |
+					    IEEE80211_VHT_MCS_SUPPORT_0_9 << 4 |
+					    IEEE80211_VHT_MCS_SUPPORT_0_9 << 6 |
+					    IEEE80211_VHT_MCS_SUPPORT_0_9 << 8 |
+					    IEEE80211_VHT_MCS_SUPPORT_0_9 << 10 |
+					    IEEE80211_VHT_MCS_SUPPORT_0_9 << 12 |
+					    IEEE80211_VHT_MCS_SUPPORT_0_9 << 14);
+			sband->vht_cap.vht_mcs.tx_mcs_map =
+				sband->vht_cap.vht_mcs.rx_mcs_map;
 			break;
 		default:
 			continue;
@@ -2458,31 +2482,6 @@ static int mac80211_hwsim_new_radio(struct genl_info *info,
 		sband->ht_cap.mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
 
 		hw->wiphy->bands[band] = sband;
-
-		sband->vht_cap.vht_supported = true;
-		sband->vht_cap.cap =
-			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
-			IEEE80211_VHT_CAP_SUPP_CHAN_WIDTH_160_80PLUS80MHZ |
-			IEEE80211_VHT_CAP_RXLDPC |
-			IEEE80211_VHT_CAP_SHORT_GI_80 |
-			IEEE80211_VHT_CAP_SHORT_GI_160 |
-			IEEE80211_VHT_CAP_TXSTBC |
-			IEEE80211_VHT_CAP_RXSTBC_1 |
-			IEEE80211_VHT_CAP_RXSTBC_2 |
-			IEEE80211_VHT_CAP_RXSTBC_3 |
-			IEEE80211_VHT_CAP_RXSTBC_4 |
-			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK;
-		sband->vht_cap.vht_mcs.rx_mcs_map =
-			cpu_to_le16(IEEE80211_VHT_MCS_SUPPORT_0_8 << 0 |
-				    IEEE80211_VHT_MCS_SUPPORT_0_8 << 2 |
-				    IEEE80211_VHT_MCS_SUPPORT_0_9 << 4 |
-				    IEEE80211_VHT_MCS_SUPPORT_0_8 << 6 |
-				    IEEE80211_VHT_MCS_SUPPORT_0_8 << 8 |
-				    IEEE80211_VHT_MCS_SUPPORT_0_9 << 10 |
-				    IEEE80211_VHT_MCS_SUPPORT_0_9 << 12 |
-				    IEEE80211_VHT_MCS_SUPPORT_0_8 << 14);
-		sband->vht_cap.vht_mcs.tx_mcs_map =
-			sband->vht_cap.vht_mcs.rx_mcs_map;
 	}
 
 	/* By default all radios belong to the first group */
@@ -2510,7 +2509,7 @@ static int mac80211_hwsim_new_radio(struct genl_info *info,
 	}
 
 	if (param->no_vif)
-		hw->flags |= IEEE80211_HW_NO_AUTO_VIF;
+		ieee80211_hw_set(hw, NO_AUTO_VIF);
 
 	err = ieee80211_register_hw(hw);
 	if (err < 0) {
diff --git a/drivers/net/wireless/mediatek/Kconfig b/drivers/net/wireless/mediatek/Kconfig
new file mode 100644
index 000000000000..cba300c6b5da
--- /dev/null
+++ b/drivers/net/wireless/mediatek/Kconfig
@@ -0,0 +1,10 @@
+menuconfig WL_MEDIATEK
+	bool "Mediatek Wireless LAN support"
+	---help---
+	  Enable community drivers for MediaTek WiFi devices.
+	  Those drivers make use of the Linux mac80211 stack.
+
+
+if WL_MEDIATEK
+source "drivers/net/wireless/mediatek/mt7601u/Kconfig"
+endif # WL_MEDIATEK
diff --git a/drivers/net/wireless/mediatek/Makefile b/drivers/net/wireless/mediatek/Makefile
new file mode 100644
index 000000000000..9d5f182fd7fd
--- /dev/null
+++ b/drivers/net/wireless/mediatek/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_MT7601U)	+= mt7601u/
diff --git a/drivers/net/wireless/mediatek/mt7601u/Kconfig b/drivers/net/wireless/mediatek/mt7601u/Kconfig
new file mode 100644
index 000000000000..f46bed92796b
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/Kconfig
@@ -0,0 +1,6 @@
+config MT7601U
+	tristate "MediaTek MT7601U (USB) support"
+	depends on MAC80211
+	depends on USB
+	---help---
+	  This adds support for MT7601U-based wireless USB dongles.
diff --git a/drivers/net/wireless/mediatek/mt7601u/Makefile b/drivers/net/wireless/mediatek/mt7601u/Makefile
new file mode 100644
index 000000000000..ea9ed8a5db4d
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/Makefile
@@ -0,0 +1,9 @@
+ccflags-y += -D__CHECK_ENDIAN__
+
+obj-$(CONFIG_MT7601U)	+= mt7601u.o
+
+mt7601u-objs	= \
+	usb.o init.o main.o mcu.o trace.o dma.o core.o eeprom.o phy.o \
+	mac.o util.o debugfs.o tx.o
+
+CFLAGS_trace.o := -I$(src)
diff --git a/drivers/net/wireless/mediatek/mt7601u/core.c b/drivers/net/wireless/mediatek/mt7601u/core.c
new file mode 100644
index 000000000000..0aabd790f985
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/core.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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 "mt7601u.h"
+
+int mt7601u_wait_asic_ready(struct mt7601u_dev *dev)
+{
+	int i = 100;
+	u32 val;
+
+	do {
+		if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
+			return -EIO;
+
+		val = mt7601u_rr(dev, MT_MAC_CSR0);
+		if (val && ~val)
+			return 0;
+
+		udelay(10);
+	} while (i--);
+
+	return -EIO;
+}
+
+bool mt76_poll(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val,
+	       int timeout)
+{
+	u32 cur;
+
+	timeout /= 10;
+	do {
+		if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
+			return false;
+
+		cur = mt7601u_rr(dev, offset) & mask;
+		if (cur == val)
+			return true;
+
+		udelay(10);
+	} while (timeout-- > 0);
+
+	dev_err(dev->dev, "Error: Time out with reg %08x\n", offset);
+
+	return false;
+}
+
+bool mt76_poll_msec(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val,
+		    int timeout)
+{
+	u32 cur;
+
+	timeout /= 10;
+	do {
+		if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
+			return false;
+
+		cur = mt7601u_rr(dev, offset) & mask;
+		if (cur == val)
+			return true;
+
+		msleep(10);
+	} while (timeout-- > 0);
+
+	dev_err(dev->dev, "Error: Time out with reg %08x\n", offset);
+
+	return false;
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/debugfs.c b/drivers/net/wireless/mediatek/mt7601u/debugfs.c
new file mode 100644
index 000000000000..fc008475a03b
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/debugfs.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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/debugfs.h>
+
+#include "mt7601u.h"
+#include "eeprom.h"
+
+static int
+mt76_reg_set(void *data, u64 val)
+{
+	struct mt7601u_dev *dev = data;
+
+	mt76_wr(dev, dev->debugfs_reg, val);
+	return 0;
+}
+
+static int
+mt76_reg_get(void *data, u64 *val)
+{
+	struct mt7601u_dev *dev = data;
+
+	*val = mt76_rr(dev, dev->debugfs_reg);
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(fops_regval, mt76_reg_get, mt76_reg_set, "0x%08llx\n");
+
+static int
+mt7601u_ampdu_stat_read(struct seq_file *file, void *data)
+{
+	struct mt7601u_dev *dev = file->private;
+	int i, j;
+
+#define stat_printf(grp, off, name)					\
+	seq_printf(file, #name ":\t%llu\n", dev->stats.grp[off])
+
+	stat_printf(rx_stat, 0, rx_crc_err);
+	stat_printf(rx_stat, 1, rx_phy_err);
+	stat_printf(rx_stat, 2, rx_false_cca);
+	stat_printf(rx_stat, 3, rx_plcp_err);
+	stat_printf(rx_stat, 4, rx_fifo_overflow);
+	stat_printf(rx_stat, 5, rx_duplicate);
+
+	stat_printf(tx_stat, 0, tx_fail_cnt);
+	stat_printf(tx_stat, 1, tx_bcn_cnt);
+	stat_printf(tx_stat, 2, tx_success);
+	stat_printf(tx_stat, 3, tx_retransmit);
+	stat_printf(tx_stat, 4, tx_zero_len);
+	stat_printf(tx_stat, 5, tx_underflow);
+
+	stat_printf(aggr_stat, 0, non_aggr_tx);
+	stat_printf(aggr_stat, 1, aggr_tx);
+
+	stat_printf(zero_len_del, 0, tx_zero_len_del);
+	stat_printf(zero_len_del, 1, rx_zero_len_del);
+#undef stat_printf
+
+	seq_puts(file, "Aggregations stats:\n");
+	for (i = 0; i < 4; i++) {
+		for (j = 0; j < 8; j++)
+			seq_printf(file, "%08llx ",
+				   dev->stats.aggr_n[i * 8 + j]);
+		seq_putc(file, '\n');
+	}
+
+	seq_printf(file, "recent average AMPDU len: %d\n",
+		   atomic_read(&dev->avg_ampdu_len));
+
+	return 0;
+}
+
+static int
+mt7601u_ampdu_stat_open(struct inode *inode, struct file *f)
+{
+	return single_open(f, mt7601u_ampdu_stat_read, inode->i_private);
+}
+
+static const struct file_operations fops_ampdu_stat = {
+	.open = mt7601u_ampdu_stat_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int
+mt7601u_eeprom_param_read(struct seq_file *file, void *data)
+{
+	struct mt7601u_dev *dev = file->private;
+	struct mt7601u_rate_power *rp = &dev->ee->power_rate_table;
+	struct tssi_data *td = &dev->ee->tssi_data;
+	int i;
+
+	seq_printf(file, "RF freq offset: %hhx\n", dev->ee->rf_freq_off);
+	seq_printf(file, "RSSI offset: %hhx %hhx\n",
+		   dev->ee->rssi_offset[0], dev->ee->rssi_offset[1]);
+	seq_printf(file, "Reference temp: %hhx\n", dev->ee->ref_temp);
+	seq_printf(file, "LNA gain: %hhx\n", dev->ee->lna_gain);
+	seq_printf(file, "Reg channels: %hhu-%hhu\n", dev->ee->reg.start,
+		   dev->ee->reg.start + dev->ee->reg.num - 1);
+
+	seq_puts(file, "Per rate power:\n");
+	for (i = 0; i < 2; i++)
+		seq_printf(file, "\t raw:%02hhx bw20:%02hhx bw40:%02hhx\n",
+			   rp->cck[i].raw, rp->cck[i].bw20, rp->cck[i].bw40);
+	for (i = 0; i < 4; i++)
+		seq_printf(file, "\t raw:%02hhx bw20:%02hhx bw40:%02hhx\n",
+			   rp->ofdm[i].raw, rp->ofdm[i].bw20, rp->ofdm[i].bw40);
+	for (i = 0; i < 4; i++)
+		seq_printf(file, "\t raw:%02hhx bw20:%02hhx bw40:%02hhx\n",
+			   rp->ht[i].raw, rp->ht[i].bw20, rp->ht[i].bw40);
+
+	seq_puts(file, "Per channel power:\n");
+	for (i = 0; i < 7; i++)
+		seq_printf(file, "\t tx_power  ch%u:%02hhx ch%u:%02hhx\n",
+			   i * 2 + 1, dev->ee->chan_pwr[i * 2],
+			   i * 2 + 2, dev->ee->chan_pwr[i * 2 + 1]);
+
+	if (!dev->ee->tssi_enabled)
+		return 0;
+
+	seq_puts(file, "TSSI:\n");
+	seq_printf(file, "\t slope:%02hhx\n", td->slope);
+	seq_printf(file, "\t offset=%02hhx %02hhx %02hhx\n",
+		   td->offset[0], td->offset[1], td->offset[2]);
+	seq_printf(file, "\t delta_off:%08x\n", td->tx0_delta_offset);
+
+	return 0;
+}
+
+static int
+mt7601u_eeprom_param_open(struct inode *inode, struct file *f)
+{
+	return single_open(f, mt7601u_eeprom_param_read, inode->i_private);
+}
+
+static const struct file_operations fops_eeprom_param = {
+	.open = mt7601u_eeprom_param_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+void mt7601u_init_debugfs(struct mt7601u_dev *dev)
+{
+	struct dentry *dir;
+
+	dir = debugfs_create_dir("mt7601u", dev->hw->wiphy->debugfsdir);
+	if (!dir)
+		return;
+
+	debugfs_create_u8("temperature", S_IRUSR, dir, &dev->raw_temp);
+	debugfs_create_u32("temp_mode", S_IRUSR, dir, &dev->temp_mode);
+
+	debugfs_create_u32("regidx", S_IRUSR | S_IWUSR, dir, &dev->debugfs_reg);
+	debugfs_create_file("regval", S_IRUSR | S_IWUSR, dir, dev,
+			    &fops_regval);
+	debugfs_create_file("ampdu_stat", S_IRUSR, dir, dev, &fops_ampdu_stat);
+	debugfs_create_file("eeprom_param", S_IRUSR, dir, dev,
+			    &fops_eeprom_param);
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/dma.c b/drivers/net/wireless/mediatek/mt7601u/dma.c
new file mode 100644
index 000000000000..7217da4f1543
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/dma.c
@@ -0,0 +1,505 @@
+/*
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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 "mt7601u.h"
+#include "dma.h"
+#include "usb.h"
+#include "trace.h"
+
+static int mt7601u_submit_rx_buf(struct mt7601u_dev *dev,
+				 struct mt7601u_dma_buf_rx *e, gfp_t gfp);
+
+static unsigned int ieee80211_get_hdrlen_from_buf(const u8 *data, unsigned len)
+{
+	const struct ieee80211_hdr *hdr = (const struct ieee80211_hdr *)data;
+	unsigned int hdrlen;
+
+	if (unlikely(len < 10))
+		return 0;
+	hdrlen = ieee80211_hdrlen(hdr->frame_control);
+	if (unlikely(hdrlen > len))
+		return 0;
+	return hdrlen;
+}
+
+static struct sk_buff *
+mt7601u_rx_skb_from_seg(struct mt7601u_dev *dev, struct mt7601u_rxwi *rxwi,
+			void *data, u32 seg_len, u32 truesize, struct page *p)
+{
+	struct sk_buff *skb;
+	u32 true_len, hdr_len = 0, copy, frag;
+
+	skb = alloc_skb(p ? 128 : seg_len, GFP_ATOMIC);
+	if (!skb)
+		return NULL;
+
+	true_len = mt76_mac_process_rx(dev, skb, data, rxwi);
+	if (!true_len || true_len > seg_len)
+		goto bad_frame;
+
+	hdr_len = ieee80211_get_hdrlen_from_buf(data, true_len);
+	if (!hdr_len)
+		goto bad_frame;
+
+	if (rxwi->rxinfo & cpu_to_le32(MT_RXINFO_L2PAD)) {
+		memcpy(skb_put(skb, hdr_len), data, hdr_len);
+
+		data += hdr_len + 2;
+		true_len -= hdr_len;
+		hdr_len = 0;
+	}
+
+	/* If not doing paged RX allocated skb will always have enough space */
+	copy = (true_len <= skb_tailroom(skb)) ? true_len : hdr_len + 8;
+	frag = true_len - copy;
+
+	memcpy(skb_put(skb, copy), data, copy);
+	data += copy;
+
+	if (frag) {
+		skb_add_rx_frag(skb, 0, p, data - page_address(p),
+				frag, truesize);
+		get_page(p);
+	}
+
+	return skb;
+
+bad_frame:
+	dev_err_ratelimited(dev->dev, "Error: incorrect frame len:%u hdr:%u\n",
+			    true_len, hdr_len);
+	dev_kfree_skb(skb);
+	return NULL;
+}
+
+static void mt7601u_rx_process_seg(struct mt7601u_dev *dev, u8 *data,
+				   u32 seg_len, struct page *p)
+{
+	struct sk_buff *skb;
+	struct mt7601u_rxwi *rxwi;
+	u32 fce_info, truesize = seg_len;
+
+	/* DMA_INFO field at the beginning of the segment contains only some of
+	 * the information, we need to read the FCE descriptor from the end.
+	 */
+	fce_info = get_unaligned_le32(data + seg_len - MT_FCE_INFO_LEN);
+	seg_len -= MT_FCE_INFO_LEN;
+
+	data += MT_DMA_HDR_LEN;
+	seg_len -= MT_DMA_HDR_LEN;
+
+	rxwi = (struct mt7601u_rxwi *) data;
+	data += sizeof(struct mt7601u_rxwi);
+	seg_len -= sizeof(struct mt7601u_rxwi);
+
+	if (unlikely(rxwi->zero[0] || rxwi->zero[1] || rxwi->zero[2]))
+		dev_err_once(dev->dev, "Error: RXWI zero fields are set\n");
+	if (unlikely(MT76_GET(MT_RXD_INFO_TYPE, fce_info)))
+		dev_err_once(dev->dev, "Error: RX path seen a non-pkt urb\n");
+
+	trace_mt_rx(dev, rxwi, fce_info);
+
+	skb = mt7601u_rx_skb_from_seg(dev, rxwi, data, seg_len, truesize, p);
+	if (!skb)
+		return;
+
+	ieee80211_rx_ni(dev->hw, skb);
+}
+
+static u16 mt7601u_rx_next_seg_len(u8 *data, u32 data_len)
+{
+	u32 min_seg_len = MT_DMA_HDR_LEN + MT_RX_INFO_LEN +
+		sizeof(struct mt7601u_rxwi) + MT_FCE_INFO_LEN;
+	u16 dma_len = get_unaligned_le16(data);
+
+	if (data_len < min_seg_len ||
+	    WARN_ON(!dma_len) ||
+	    WARN_ON(dma_len + MT_DMA_HDRS > data_len) ||
+	    WARN_ON(dma_len & 0x3))
+		return 0;
+
+	return MT_DMA_HDRS + dma_len;
+}
+
+static void
+mt7601u_rx_process_entry(struct mt7601u_dev *dev, struct mt7601u_dma_buf_rx *e)
+{
+	u32 seg_len, data_len = e->urb->actual_length;
+	u8 *data = page_address(e->p);
+	struct page *new_p = NULL;
+	int cnt = 0;
+
+	if (!test_bit(MT7601U_STATE_INITIALIZED, &dev->state))
+		return;
+
+	/* Copy if there is very little data in the buffer. */
+	if (data_len > 512)
+		new_p = dev_alloc_pages(MT_RX_ORDER);
+
+	while ((seg_len = mt7601u_rx_next_seg_len(data, data_len))) {
+		mt7601u_rx_process_seg(dev, data, seg_len, new_p ? e->p : NULL);
+
+		data_len -= seg_len;
+		data += seg_len;
+		cnt++;
+	}
+
+	if (cnt > 1)
+		trace_mt_rx_dma_aggr(dev, cnt, !!new_p);
+
+	if (new_p) {
+		/* we have one extra ref from the allocator */
+		__free_pages(e->p, MT_RX_ORDER);
+
+		e->p = new_p;
+	}
+}
+
+static struct mt7601u_dma_buf_rx *
+mt7601u_rx_get_pending_entry(struct mt7601u_dev *dev)
+{
+	struct mt7601u_rx_queue *q = &dev->rx_q;
+	struct mt7601u_dma_buf_rx *buf = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->rx_lock, flags);
+
+	if (!q->pending)
+		goto out;
+
+	buf = &q->e[q->start];
+	q->pending--;
+	q->start = (q->start + 1) % q->entries;
+out:
+	spin_unlock_irqrestore(&dev->rx_lock, flags);
+
+	return buf;
+}
+
+static void mt7601u_complete_rx(struct urb *urb)
+{
+	struct mt7601u_dev *dev = urb->context;
+	struct mt7601u_rx_queue *q = &dev->rx_q;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->rx_lock, flags);
+
+	if (mt7601u_urb_has_error(urb))
+		dev_err(dev->dev, "Error: RX urb failed:%d\n", urb->status);
+	if (WARN_ONCE(q->e[q->end].urb != urb, "RX urb mismatch"))
+		goto out;
+
+	q->end = (q->end + 1) % q->entries;
+	q->pending++;
+	tasklet_schedule(&dev->rx_tasklet);
+out:
+	spin_unlock_irqrestore(&dev->rx_lock, flags);
+}
+
+static void mt7601u_rx_tasklet(unsigned long data)
+{
+	struct mt7601u_dev *dev = (struct mt7601u_dev *) data;
+	struct mt7601u_dma_buf_rx *e;
+
+	while ((e = mt7601u_rx_get_pending_entry(dev))) {
+		if (e->urb->status)
+			continue;
+
+		mt7601u_rx_process_entry(dev, e);
+		mt7601u_submit_rx_buf(dev, e, GFP_ATOMIC);
+	}
+}
+
+static void mt7601u_complete_tx(struct urb *urb)
+{
+	struct mt7601u_tx_queue *q = urb->context;
+	struct mt7601u_dev *dev = q->dev;
+	struct sk_buff *skb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->tx_lock, flags);
+
+	if (mt7601u_urb_has_error(urb))
+		dev_err(dev->dev, "Error: TX urb failed:%d\n", urb->status);
+	if (WARN_ONCE(q->e[q->start].urb != urb, "TX urb mismatch"))
+		goto out;
+
+	skb = q->e[q->start].skb;
+	trace_mt_tx_dma_done(dev, skb);
+
+	mt7601u_tx_status(dev, skb);
+
+	if (q->used == q->entries - q->entries / 8)
+		ieee80211_wake_queue(dev->hw, skb_get_queue_mapping(skb));
+
+	q->start = (q->start + 1) % q->entries;
+	q->used--;
+
+	if (urb->status)
+		goto out;
+
+	set_bit(MT7601U_STATE_MORE_STATS, &dev->state);
+	if (!test_and_set_bit(MT7601U_STATE_READING_STATS, &dev->state))
+		queue_delayed_work(dev->stat_wq, &dev->stat_work,
+				   msecs_to_jiffies(10));
+out:
+	spin_unlock_irqrestore(&dev->tx_lock, flags);
+}
+
+static int mt7601u_dma_submit_tx(struct mt7601u_dev *dev,
+				 struct sk_buff *skb, u8 ep)
+{
+	struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
+	unsigned snd_pipe = usb_sndbulkpipe(usb_dev, dev->out_eps[ep]);
+	struct mt7601u_dma_buf_tx *e;
+	struct mt7601u_tx_queue *q = &dev->tx_q[ep];
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&dev->tx_lock, flags);
+
+	if (WARN_ON(q->entries <= q->used)) {
+		ret = -ENOSPC;
+		goto out;
+	}
+
+	e = &q->e[q->end];
+	e->skb = skb;
+	usb_fill_bulk_urb(e->urb, usb_dev, snd_pipe, skb->data, skb->len,
+			  mt7601u_complete_tx, q);
+	ret = usb_submit_urb(e->urb, GFP_ATOMIC);
+	if (ret) {
+		/* Special-handle ENODEV from TX urb submission because it will
+		 * often be the first ENODEV we see after device is removed.
+		 */
+		if (ret == -ENODEV)
+			set_bit(MT7601U_STATE_REMOVED, &dev->state);
+		else
+			dev_err(dev->dev, "Error: TX urb submit failed:%d\n",
+				ret);
+		goto out;
+	}
+
+	q->end = (q->end + 1) % q->entries;
+	q->used++;
+
+	if (q->used >= q->entries)
+		ieee80211_stop_queue(dev->hw, skb_get_queue_mapping(skb));
+out:
+	spin_unlock_irqrestore(&dev->tx_lock, flags);
+
+	return ret;
+}
+
+/* Map hardware Q to USB endpoint number */
+static u8 q2ep(u8 qid)
+{
+	/* TODO: take management packets to queue 5 */
+	return qid + 1;
+}
+
+/* Map USB endpoint number to Q id in the DMA engine */
+static enum mt76_qsel ep2dmaq(u8 ep)
+{
+	if (ep == 5)
+		return MT_QSEL_MGMT;
+	return MT_QSEL_EDCA;
+}
+
+int mt7601u_dma_enqueue_tx(struct mt7601u_dev *dev, struct sk_buff *skb,
+			   struct mt76_wcid *wcid, int hw_q)
+{
+	u8 ep = q2ep(hw_q);
+	u32 dma_flags;
+	int ret;
+
+	dma_flags = MT_TXD_PKT_INFO_80211;
+	if (wcid->hw_key_idx == 0xff)
+		dma_flags |= MT_TXD_PKT_INFO_WIV;
+
+	ret = mt7601u_dma_skb_wrap_pkt(skb, ep2dmaq(ep), dma_flags);
+	if (ret)
+		return ret;
+
+	ret = mt7601u_dma_submit_tx(dev, skb, ep);
+	if (ret) {
+		ieee80211_free_txskb(dev->hw, skb);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void mt7601u_kill_rx(struct mt7601u_dev *dev)
+{
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->rx_lock, flags);
+
+	for (i = 0; i < dev->rx_q.entries; i++) {
+		int next = dev->rx_q.end;
+
+		spin_unlock_irqrestore(&dev->rx_lock, flags);
+		usb_poison_urb(dev->rx_q.e[next].urb);
+		spin_lock_irqsave(&dev->rx_lock, flags);
+	}
+
+	spin_unlock_irqrestore(&dev->rx_lock, flags);
+}
+
+static int mt7601u_submit_rx_buf(struct mt7601u_dev *dev,
+				 struct mt7601u_dma_buf_rx *e, gfp_t gfp)
+{
+	struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
+	u8 *buf = page_address(e->p);
+	unsigned pipe;
+	int ret;
+
+	pipe = usb_rcvbulkpipe(usb_dev, dev->in_eps[MT_EP_IN_PKT_RX]);
+
+	usb_fill_bulk_urb(e->urb, usb_dev, pipe, buf, MT_RX_URB_SIZE,
+			  mt7601u_complete_rx, dev);
+
+	trace_mt_submit_urb(dev, e->urb);
+	ret = usb_submit_urb(e->urb, gfp);
+	if (ret)
+		dev_err(dev->dev, "Error: submit RX URB failed:%d\n", ret);
+
+	return ret;
+}
+
+static int mt7601u_submit_rx(struct mt7601u_dev *dev)
+{
+	int i, ret;
+
+	for (i = 0; i < dev->rx_q.entries; i++) {
+		ret = mt7601u_submit_rx_buf(dev, &dev->rx_q.e[i], GFP_KERNEL);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void mt7601u_free_rx(struct mt7601u_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < dev->rx_q.entries; i++) {
+		__free_pages(dev->rx_q.e[i].p, MT_RX_ORDER);
+		usb_free_urb(dev->rx_q.e[i].urb);
+	}
+}
+
+static int mt7601u_alloc_rx(struct mt7601u_dev *dev)
+{
+	int i;
+
+	memset(&dev->rx_q, 0, sizeof(dev->rx_q));
+	dev->rx_q.dev = dev;
+	dev->rx_q.entries = N_RX_ENTRIES;
+
+	for (i = 0; i < N_RX_ENTRIES; i++) {
+		dev->rx_q.e[i].urb = usb_alloc_urb(0, GFP_KERNEL);
+		dev->rx_q.e[i].p = dev_alloc_pages(MT_RX_ORDER);
+
+		if (!dev->rx_q.e[i].urb || !dev->rx_q.e[i].p)
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void mt7601u_free_tx_queue(struct mt7601u_tx_queue *q)
+{
+	int i;
+
+	WARN_ON(q->used);
+
+	for (i = 0; i < q->entries; i++)  {
+		usb_poison_urb(q->e[i].urb);
+		usb_free_urb(q->e[i].urb);
+	}
+}
+
+static void mt7601u_free_tx(struct mt7601u_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < __MT_EP_OUT_MAX; i++)
+		mt7601u_free_tx_queue(&dev->tx_q[i]);
+}
+
+static int mt7601u_alloc_tx_queue(struct mt7601u_dev *dev,
+				  struct mt7601u_tx_queue *q)
+{
+	int i;
+
+	q->dev = dev;
+	q->entries = N_TX_ENTRIES;
+
+	for (i = 0; i < N_TX_ENTRIES; i++) {
+		q->e[i].urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!q->e[i].urb)
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int mt7601u_alloc_tx(struct mt7601u_dev *dev)
+{
+	int i;
+
+	dev->tx_q = devm_kcalloc(dev->dev, __MT_EP_OUT_MAX,
+				 sizeof(*dev->tx_q), GFP_KERNEL);
+
+	for (i = 0; i < __MT_EP_OUT_MAX; i++)
+		if (mt7601u_alloc_tx_queue(dev, &dev->tx_q[i]))
+			return -ENOMEM;
+
+	return 0;
+}
+
+int mt7601u_dma_init(struct mt7601u_dev *dev)
+{
+	int ret = -ENOMEM;
+
+	tasklet_init(&dev->rx_tasklet, mt7601u_rx_tasklet, (unsigned long) dev);
+
+	ret = mt7601u_alloc_tx(dev);
+	if (ret)
+		goto err;
+	ret = mt7601u_alloc_rx(dev);
+	if (ret)
+		goto err;
+
+	ret = mt7601u_submit_rx(dev);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	mt7601u_dma_cleanup(dev);
+	return ret;
+}
+
+void mt7601u_dma_cleanup(struct mt7601u_dev *dev)
+{
+	mt7601u_kill_rx(dev);
+
+	tasklet_kill(&dev->rx_tasklet);
+
+	mt7601u_free_rx(dev);
+	mt7601u_free_tx(dev);
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/dma.h b/drivers/net/wireless/mediatek/mt7601u/dma.h
new file mode 100644
index 000000000000..978e8a90b87f
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/dma.h
@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef __MT7601U_DMA_H
+#define __MT7601U_DMA_H
+
+#include <asm/unaligned.h>
+#include <linux/skbuff.h>
+
+#include "util.h"
+
+#define MT_DMA_HDR_LEN			4
+#define MT_RX_INFO_LEN			4
+#define MT_FCE_INFO_LEN			4
+#define MT_DMA_HDRS			(MT_DMA_HDR_LEN + MT_RX_INFO_LEN)
+
+/* Common Tx DMA descriptor fields */
+#define MT_TXD_INFO_LEN			GENMASK(15, 0)
+#define MT_TXD_INFO_D_PORT		GENMASK(29, 27)
+#define MT_TXD_INFO_TYPE		GENMASK(31, 30)
+
+enum mt76_msg_port {
+	WLAN_PORT,
+	CPU_RX_PORT,
+	CPU_TX_PORT,
+	HOST_PORT,
+	VIRTUAL_CPU_RX_PORT,
+	VIRTUAL_CPU_TX_PORT,
+	DISCARD,
+};
+
+enum mt76_info_type {
+	DMA_PACKET,
+	DMA_COMMAND,
+};
+
+/* Tx DMA packet specific flags */
+#define MT_TXD_PKT_INFO_NEXT_VLD	BIT(16)
+#define MT_TXD_PKT_INFO_TX_BURST	BIT(17)
+#define MT_TXD_PKT_INFO_80211		BIT(19)
+#define MT_TXD_PKT_INFO_TSO		BIT(20)
+#define MT_TXD_PKT_INFO_CSO		BIT(21)
+#define MT_TXD_PKT_INFO_WIV		BIT(24)
+#define MT_TXD_PKT_INFO_QSEL		GENMASK(26, 25)
+
+enum mt76_qsel {
+	MT_QSEL_MGMT,
+	MT_QSEL_HCCA,
+	MT_QSEL_EDCA,
+	MT_QSEL_EDCA_2,
+};
+
+/* Tx DMA MCU command specific flags */
+#define MT_TXD_CMD_INFO_SEQ		GENMASK(19, 16)
+#define MT_TXD_CMD_INFO_TYPE		GENMASK(26, 20)
+
+static inline int mt7601u_dma_skb_wrap(struct sk_buff *skb,
+				       enum mt76_msg_port d_port,
+				       enum mt76_info_type type, u32 flags)
+{
+	u32 info;
+
+	/* Buffer layout:
+	 *	|   4B   | xfer len |      pad       |  4B  |
+	 *	| TXINFO | pkt/cmd  | zero pad to 4B | zero |
+	 *
+	 * length field of TXINFO should be set to 'xfer len'.
+	 */
+
+	info = flags |
+		MT76_SET(MT_TXD_INFO_LEN, round_up(skb->len, 4)) |
+		MT76_SET(MT_TXD_INFO_D_PORT, d_port) |
+		MT76_SET(MT_TXD_INFO_TYPE, type);
+
+	put_unaligned_le32(info, skb_push(skb, sizeof(info)));
+	return skb_put_padto(skb, round_up(skb->len, 4) + 4);
+}
+
+static inline int
+mt7601u_dma_skb_wrap_pkt(struct sk_buff *skb, enum mt76_qsel qsel, u32 flags)
+{
+	flags |= MT76_SET(MT_TXD_PKT_INFO_QSEL, qsel);
+	return mt7601u_dma_skb_wrap(skb, WLAN_PORT, DMA_PACKET, flags);
+}
+
+/* Common Rx DMA descriptor fields */
+#define MT_RXD_INFO_LEN			GENMASK(13, 0)
+#define MT_RXD_INFO_PCIE_INTR		BIT(24)
+#define MT_RXD_INFO_QSEL		GENMASK(26, 25)
+#define MT_RXD_INFO_PORT		GENMASK(29, 27)
+#define MT_RXD_INFO_TYPE		GENMASK(31, 30)
+
+/* Rx DMA packet specific flags */
+#define MT_RXD_PKT_INFO_UDP_ERR		BIT(16)
+#define MT_RXD_PKT_INFO_TCP_ERR		BIT(17)
+#define MT_RXD_PKT_INFO_IP_ERR		BIT(18)
+#define MT_RXD_PKT_INFO_PKT_80211	BIT(19)
+#define MT_RXD_PKT_INFO_L3L4_DONE	BIT(20)
+#define MT_RXD_PKT_INFO_MAC_LEN		GENMASK(23, 21)
+
+/* Rx DMA MCU command specific flags */
+#define MT_RXD_CMD_INFO_SELF_GEN	BIT(15)
+#define MT_RXD_CMD_INFO_CMD_SEQ		GENMASK(19, 16)
+#define MT_RXD_CMD_INFO_EVT_TYPE	GENMASK(23, 20)
+
+enum mt76_evt_type {
+	CMD_DONE,
+	CMD_ERROR,
+	CMD_RETRY,
+	EVENT_PWR_RSP,
+	EVENT_WOW_RSP,
+	EVENT_CARRIER_DETECT_RSP,
+	EVENT_DFS_DETECT_RSP,
+};
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/eeprom.c b/drivers/net/wireless/mediatek/mt7601u/eeprom.c
new file mode 100644
index 000000000000..8d8ee0344f7b
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/eeprom.c
@@ -0,0 +1,418 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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/of.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/etherdevice.h>
+#include <asm/unaligned.h>
+#include "mt7601u.h"
+#include "eeprom.h"
+
+static bool
+field_valid(u8 val)
+{
+	return val != 0xff;
+}
+
+static s8
+field_validate(u8 val)
+{
+	if (!field_valid(val))
+		return 0;
+
+	return val;
+}
+
+static int
+mt7601u_efuse_read(struct mt7601u_dev *dev, u16 addr, u8 *data,
+		   enum mt7601u_eeprom_access_modes mode)
+{
+	u32 val;
+	int i;
+
+	val = mt76_rr(dev, MT_EFUSE_CTRL);
+	val &= ~(MT_EFUSE_CTRL_AIN |
+		 MT_EFUSE_CTRL_MODE);
+	val |= MT76_SET(MT_EFUSE_CTRL_AIN, addr & ~0xf) |
+	       MT76_SET(MT_EFUSE_CTRL_MODE, mode) |
+	       MT_EFUSE_CTRL_KICK;
+	mt76_wr(dev, MT_EFUSE_CTRL, val);
+
+	if (!mt76_poll(dev, MT_EFUSE_CTRL, MT_EFUSE_CTRL_KICK, 0, 1000))
+		return -ETIMEDOUT;
+
+	val = mt76_rr(dev, MT_EFUSE_CTRL);
+	if ((val & MT_EFUSE_CTRL_AOUT) == MT_EFUSE_CTRL_AOUT) {
+		/* Parts of eeprom not in the usage map (0x80-0xc0,0xf0)
+		 * will not return valid data but it's ok.
+		 */
+		memset(data, 0xff, 16);
+		return 0;
+	}
+
+	for (i = 0; i < 4; i++) {
+		val = mt76_rr(dev, MT_EFUSE_DATA(i));
+		put_unaligned_le32(val, data + 4 * i);
+	}
+
+	return 0;
+}
+
+static int
+mt7601u_efuse_physical_size_check(struct mt7601u_dev *dev)
+{
+	const int map_reads = DIV_ROUND_UP(MT_EFUSE_USAGE_MAP_SIZE, 16);
+	u8 data[map_reads * 16];
+	int ret, i;
+	u32 start = 0, end = 0, cnt_free;
+
+	for (i = 0; i < map_reads; i++) {
+		ret = mt7601u_efuse_read(dev, MT_EE_USAGE_MAP_START + i * 16,
+					 data + i * 16, MT_EE_PHYSICAL_READ);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < MT_EFUSE_USAGE_MAP_SIZE; i++)
+		if (!data[i]) {
+			if (!start)
+				start = MT_EE_USAGE_MAP_START + i;
+			end = MT_EE_USAGE_MAP_START + i;
+		}
+	cnt_free = end - start + 1;
+
+	if (MT_EFUSE_USAGE_MAP_SIZE - cnt_free < 5) {
+		dev_err(dev->dev, "Error: your device needs default EEPROM file and this driver doesn't support it!\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static bool
+mt7601u_has_tssi(struct mt7601u_dev *dev, u8 *eeprom)
+{
+	u16 nic_conf1 = get_unaligned_le16(eeprom + MT_EE_NIC_CONF_1);
+
+	return ~nic_conf1 && (nic_conf1 & MT_EE_NIC_CONF_1_TX_ALC_EN);
+}
+
+static void
+mt7601u_set_chip_cap(struct mt7601u_dev *dev, u8 *eeprom)
+{
+	u16 nic_conf0 = get_unaligned_le16(eeprom + MT_EE_NIC_CONF_0);
+	u16 nic_conf1 = get_unaligned_le16(eeprom + MT_EE_NIC_CONF_1);
+
+	if (!field_valid(nic_conf1 & 0xff))
+		nic_conf1 &= 0xff00;
+
+	dev->ee->tssi_enabled = mt7601u_has_tssi(dev, eeprom) &&
+				!(nic_conf1 & MT_EE_NIC_CONF_1_TEMP_TX_ALC);
+
+	if (nic_conf1 & MT_EE_NIC_CONF_1_HW_RF_CTRL)
+		dev_err(dev->dev,
+			"Error: this driver does not support HW RF ctrl\n");
+
+	if (!field_valid(nic_conf0 >> 8))
+		return;
+
+	if (MT76_GET(MT_EE_NIC_CONF_0_RX_PATH, nic_conf0) > 1 ||
+	    MT76_GET(MT_EE_NIC_CONF_0_TX_PATH, nic_conf0) > 1)
+		dev_err(dev->dev,
+			"Error: device has more than 1 RX/TX stream!\n");
+}
+
+static int
+mt7601u_set_macaddr(struct mt7601u_dev *dev, const u8 *eeprom)
+{
+	const void *src = eeprom + MT_EE_MAC_ADDR;
+
+	ether_addr_copy(dev->macaddr, src);
+
+	if (!is_valid_ether_addr(dev->macaddr)) {
+		eth_random_addr(dev->macaddr);
+		dev_info(dev->dev,
+			 "Invalid MAC address, using random address %pM\n",
+			 dev->macaddr);
+	}
+
+	mt76_wr(dev, MT_MAC_ADDR_DW0, get_unaligned_le32(dev->macaddr));
+	mt76_wr(dev, MT_MAC_ADDR_DW1, get_unaligned_le16(dev->macaddr + 4) |
+		MT76_SET(MT_MAC_ADDR_DW1_U2ME_MASK, 0xff));
+
+	return 0;
+}
+
+static void mt7601u_set_channel_target_power(struct mt7601u_dev *dev,
+					     u8 *eeprom, u8 max_pwr)
+{
+	u8 trgt_pwr = eeprom[MT_EE_TX_TSSI_TARGET_POWER];
+
+	if (trgt_pwr > max_pwr || !trgt_pwr) {
+		dev_warn(dev->dev, "Error: EEPROM trgt power invalid %hhx!\n",
+			 trgt_pwr);
+		trgt_pwr = 0x20;
+	}
+
+	memset(dev->ee->chan_pwr, trgt_pwr, sizeof(dev->ee->chan_pwr));
+}
+
+static void
+mt7601u_set_channel_power(struct mt7601u_dev *dev, u8 *eeprom)
+{
+	u32 i, val;
+	u8 max_pwr;
+
+	val = mt7601u_rr(dev, MT_TX_ALC_CFG_0);
+	max_pwr = MT76_GET(MT_TX_ALC_CFG_0_LIMIT_0, val);
+
+	if (mt7601u_has_tssi(dev, eeprom)) {
+		mt7601u_set_channel_target_power(dev, eeprom, max_pwr);
+		return;
+	}
+
+	for (i = 0; i < 14; i++) {
+		s8 power = field_validate(eeprom[MT_EE_TX_POWER_OFFSET + i]);
+
+		if (power > max_pwr || power < 0)
+			power = MT7601U_DEFAULT_TX_POWER;
+
+		dev->ee->chan_pwr[i] = power;
+	}
+}
+
+static void
+mt7601u_set_country_reg(struct mt7601u_dev *dev, u8 *eeprom)
+{
+	/* Note: - region 31 is not valid for mt7601u (see rtmp_init.c)
+	 *	 - comments in rtmp_def.h are incorrect (see rt_channel.c)
+	 */
+	static const struct reg_channel_bounds chan_bounds[] = {
+		/* EEPROM country regions 0 - 7 */
+		{  1, 11 },	{  1, 13 },	{ 10,  2 },	{ 10,  4 },
+		{ 14,  1 },	{  1, 14 },	{  3,  7 },	{  5,  9 },
+		/* EEPROM country regions 32 - 33 */
+		{  1, 11 },	{  1, 14 }
+	};
+	u8 val = eeprom[MT_EE_COUNTRY_REGION];
+	int idx = -1;
+
+	if (val < 8)
+		idx = val;
+	if (val > 31 && val < 33)
+		idx = val - 32 + 8;
+
+	if (idx != -1)
+		dev_info(dev->dev,
+			 "EEPROM country region %02hhx (channels %hhd-%hhd)\n",
+			 val, chan_bounds[idx].start,
+			 chan_bounds[idx].start + chan_bounds[idx].num - 1);
+	else
+		idx = 5; /* channels 1 - 14 */
+
+	dev->ee->reg = chan_bounds[idx];
+
+	/* TODO: country region 33 is special - phy should be set to B-mode
+	 *	 before entering channel 14 (see sta/connect.c)
+	 */
+}
+
+static void
+mt7601u_set_rf_freq_off(struct mt7601u_dev *dev, u8 *eeprom)
+{
+	u8 comp;
+
+	dev->ee->rf_freq_off = field_validate(eeprom[MT_EE_FREQ_OFFSET]);
+	comp = field_validate(eeprom[MT_EE_FREQ_OFFSET_COMPENSATION]);
+
+	if (comp & BIT(7))
+		dev->ee->rf_freq_off -= comp & 0x7f;
+	else
+		dev->ee->rf_freq_off += comp;
+}
+
+static void
+mt7601u_set_rssi_offset(struct mt7601u_dev *dev, u8 *eeprom)
+{
+	int i;
+	s8 *rssi_offset = dev->ee->rssi_offset;
+
+	for (i = 0; i < 2; i++) {
+		rssi_offset[i] = eeprom[MT_EE_RSSI_OFFSET + i];
+
+		if (rssi_offset[i] < -10 || rssi_offset[i] > 10) {
+			dev_warn(dev->dev,
+				 "Warning: EEPROM RSSI is invalid %02hhx\n",
+				 rssi_offset[i]);
+			rssi_offset[i] = 0;
+		}
+	}
+}
+
+static void
+mt7601u_extra_power_over_mac(struct mt7601u_dev *dev)
+{
+	u32 val;
+
+	val = ((mt7601u_rr(dev, MT_TX_PWR_CFG_1) & 0x0000ff00) >> 8);
+	val |= ((mt7601u_rr(dev, MT_TX_PWR_CFG_2) & 0x0000ff00) << 8);
+	mt7601u_wr(dev, MT_TX_PWR_CFG_7, val);
+
+	val = ((mt7601u_rr(dev, MT_TX_PWR_CFG_4) & 0x0000ff00) >> 8);
+	mt7601u_wr(dev, MT_TX_PWR_CFG_9, val);
+}
+
+static void
+mt7601u_set_power_rate(struct power_per_rate *rate, s8 delta, u8 value)
+{
+	/* Invalid? Note: vendor driver does not handle this */
+	if (value == 0xff)
+		return;
+
+	rate->raw = s6_validate(value);
+	rate->bw20 = s6_to_int(value);
+	/* Note: vendor driver does cap the value to s6 right away */
+	rate->bw40 = rate->bw20 + delta;
+}
+
+static void
+mt7601u_save_power_rate(struct mt7601u_dev *dev, s8 delta, u32 val, int i)
+{
+	struct mt7601u_rate_power *t = &dev->ee->power_rate_table;
+
+	switch (i) {
+	case 0:
+		mt7601u_set_power_rate(&t->cck[0], delta, (val >> 0) & 0xff);
+		mt7601u_set_power_rate(&t->cck[1], delta, (val >> 8) & 0xff);
+		/* Save cck bw20 for fixups of channel 14 */
+		dev->ee->real_cck_bw20[0] = t->cck[0].bw20;
+		dev->ee->real_cck_bw20[1] = t->cck[1].bw20;
+
+		mt7601u_set_power_rate(&t->ofdm[0], delta, (val >> 16) & 0xff);
+		mt7601u_set_power_rate(&t->ofdm[1], delta, (val >> 24) & 0xff);
+		break;
+	case 1:
+		mt7601u_set_power_rate(&t->ofdm[2], delta, (val >> 0) & 0xff);
+		mt7601u_set_power_rate(&t->ofdm[3], delta, (val >> 8) & 0xff);
+		mt7601u_set_power_rate(&t->ht[0], delta, (val >> 16) & 0xff);
+		mt7601u_set_power_rate(&t->ht[1], delta, (val >> 24) & 0xff);
+		break;
+	case 2:
+		mt7601u_set_power_rate(&t->ht[2], delta, (val >> 0) & 0xff);
+		mt7601u_set_power_rate(&t->ht[3], delta, (val >> 8) & 0xff);
+		break;
+	}
+}
+
+static s8
+get_delta(u8 val)
+{
+	s8 ret;
+
+	if (!field_valid(val) || !(val & BIT(7)))
+		return 0;
+
+	ret = val & 0x1f;
+	if (ret > 8)
+		ret = 8;
+	if (val & BIT(6))
+		ret = -ret;
+
+	return ret;
+}
+
+static void
+mt7601u_config_tx_power_per_rate(struct mt7601u_dev *dev, u8 *eeprom)
+{
+	u32 val;
+	s8 bw40_delta;
+	int i;
+
+	bw40_delta = get_delta(eeprom[MT_EE_TX_POWER_DELTA_BW40]);
+
+	for (i = 0; i < 5; i++) {
+		val = get_unaligned_le32(eeprom + MT_EE_TX_POWER_BYRATE(i));
+
+		mt7601u_save_power_rate(dev, bw40_delta, val, i);
+
+		if (~val)
+			mt7601u_wr(dev, MT_TX_PWR_CFG_0 + i * 4, val);
+	}
+
+	mt7601u_extra_power_over_mac(dev);
+}
+
+static void
+mt7601u_init_tssi_params(struct mt7601u_dev *dev, u8 *eeprom)
+{
+	struct tssi_data *d = &dev->ee->tssi_data;
+
+	if (!dev->ee->tssi_enabled)
+		return;
+
+	d->slope = eeprom[MT_EE_TX_TSSI_SLOPE];
+	d->tx0_delta_offset = eeprom[MT_EE_TX_TSSI_OFFSET] * 1024;
+	d->offset[0] = eeprom[MT_EE_TX_TSSI_OFFSET_GROUP];
+	d->offset[1] = eeprom[MT_EE_TX_TSSI_OFFSET_GROUP + 1];
+	d->offset[2] = eeprom[MT_EE_TX_TSSI_OFFSET_GROUP + 2];
+}
+
+int
+mt7601u_eeprom_init(struct mt7601u_dev *dev)
+{
+	u8 *eeprom;
+	int i, ret;
+
+	ret = mt7601u_efuse_physical_size_check(dev);
+	if (ret)
+		return ret;
+
+	dev->ee = devm_kzalloc(dev->dev, sizeof(*dev->ee), GFP_KERNEL);
+	if (!dev->ee)
+		return -ENOMEM;
+
+	eeprom = kmalloc(MT7601U_EEPROM_SIZE, GFP_KERNEL);
+	if (!eeprom)
+		return -ENOMEM;
+
+	for (i = 0; i + 16 <= MT7601U_EEPROM_SIZE; i += 16) {
+		ret = mt7601u_efuse_read(dev, i, eeprom + i, MT_EE_READ);
+		if (ret)
+			goto out;
+	}
+
+	if (eeprom[MT_EE_VERSION_EE] > MT7601U_EE_MAX_VER)
+		dev_warn(dev->dev,
+			 "Warning: unsupported EEPROM version %02hhx\n",
+			 eeprom[MT_EE_VERSION_EE]);
+	dev_info(dev->dev, "EEPROM ver:%02hhx fae:%02hhx\n",
+		 eeprom[MT_EE_VERSION_EE], eeprom[MT_EE_VERSION_FAE]);
+
+	mt7601u_set_macaddr(dev, eeprom);
+	mt7601u_set_chip_cap(dev, eeprom);
+	mt7601u_set_channel_power(dev, eeprom);
+	mt7601u_set_country_reg(dev, eeprom);
+	mt7601u_set_rf_freq_off(dev, eeprom);
+	mt7601u_set_rssi_offset(dev, eeprom);
+	dev->ee->ref_temp = eeprom[MT_EE_REF_TEMP];
+	dev->ee->lna_gain = eeprom[MT_EE_LNA_GAIN];
+
+	mt7601u_config_tx_power_per_rate(dev, eeprom);
+
+	mt7601u_init_tssi_params(dev, eeprom);
+out:
+	kfree(eeprom);
+	return ret;
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/eeprom.h b/drivers/net/wireless/mediatek/mt7601u/eeprom.h
new file mode 100644
index 000000000000..662d12703b69
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/eeprom.h
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef __MT7601U_EEPROM_H
+#define __MT7601U_EEPROM_H
+
+struct mt7601u_dev;
+
+#define MT7601U_EE_MAX_VER			0x0c
+#define MT7601U_EEPROM_SIZE			256
+
+#define MT7601U_DEFAULT_TX_POWER		6
+
+enum mt76_eeprom_field {
+	MT_EE_CHIP_ID =				0x00,
+	MT_EE_VERSION_FAE =			0x02,
+	MT_EE_VERSION_EE =			0x03,
+	MT_EE_MAC_ADDR =			0x04,
+	MT_EE_NIC_CONF_0 =			0x34,
+	MT_EE_NIC_CONF_1 =			0x36,
+	MT_EE_COUNTRY_REGION =			0x39,
+	MT_EE_FREQ_OFFSET =			0x3a,
+	MT_EE_NIC_CONF_2 =			0x42,
+
+	MT_EE_LNA_GAIN =			0x44,
+	MT_EE_RSSI_OFFSET =			0x46,
+
+	MT_EE_TX_POWER_DELTA_BW40 =		0x50,
+	MT_EE_TX_POWER_OFFSET =			0x52,
+
+	MT_EE_TX_TSSI_SLOPE =			0x6e,
+	MT_EE_TX_TSSI_OFFSET_GROUP =		0x6f,
+	MT_EE_TX_TSSI_OFFSET =			0x76,
+
+	MT_EE_TX_TSSI_TARGET_POWER =		0xd0,
+	MT_EE_REF_TEMP =			0xd1,
+	MT_EE_FREQ_OFFSET_COMPENSATION =	0xdb,
+	MT_EE_TX_POWER_BYRATE_BASE =		0xde,
+
+	MT_EE_USAGE_MAP_START =			0x1e0,
+	MT_EE_USAGE_MAP_END =			0x1fc,
+};
+
+#define MT_EE_NIC_CONF_0_RX_PATH		GENMASK(3, 0)
+#define MT_EE_NIC_CONF_0_TX_PATH		GENMASK(7, 4)
+#define MT_EE_NIC_CONF_0_BOARD_TYPE		GENMASK(13, 12)
+
+#define MT_EE_NIC_CONF_1_HW_RF_CTRL		BIT(0)
+#define MT_EE_NIC_CONF_1_TEMP_TX_ALC		BIT(1)
+#define MT_EE_NIC_CONF_1_LNA_EXT_2G		BIT(2)
+#define MT_EE_NIC_CONF_1_LNA_EXT_5G		BIT(3)
+#define MT_EE_NIC_CONF_1_TX_ALC_EN		BIT(13)
+
+#define MT_EE_NIC_CONF_2_RX_STREAM		GENMASK(3, 0)
+#define MT_EE_NIC_CONF_2_TX_STREAM		GENMASK(7, 4)
+#define MT_EE_NIC_CONF_2_HW_ANTDIV		BIT(8)
+#define MT_EE_NIC_CONF_2_XTAL_OPTION		GENMASK(10, 9)
+#define MT_EE_NIC_CONF_2_TEMP_DISABLE		BIT(11)
+#define MT_EE_NIC_CONF_2_COEX_METHOD		GENMASK(15, 13)
+
+#define MT_EE_TX_POWER_BYRATE(i)		(MT_EE_TX_POWER_BYRATE_BASE + \
+						 (i) * 4)
+
+#define MT_EFUSE_USAGE_MAP_SIZE			(MT_EE_USAGE_MAP_END -	\
+						 MT_EE_USAGE_MAP_START + 1)
+
+enum mt7601u_eeprom_access_modes {
+	MT_EE_READ = 0,
+	MT_EE_PHYSICAL_READ = 1,
+};
+
+struct power_per_rate  {
+	u8 raw;  /* validated s6 value */
+	s8 bw20; /* sign-extended int */
+	s8 bw40; /* sign-extended int */
+};
+
+/* Power per rate - one value per two rates */
+struct mt7601u_rate_power {
+	struct power_per_rate cck[2];
+	struct power_per_rate ofdm[4];
+	struct power_per_rate ht[4];
+};
+
+struct reg_channel_bounds {
+	u8 start;
+	u8 num;
+};
+
+struct mt7601u_eeprom_params {
+	bool tssi_enabled;
+	u8 rf_freq_off;
+	s8 rssi_offset[2];
+	s8 ref_temp;
+	s8 lna_gain;
+
+	u8 chan_pwr[14];
+	struct mt7601u_rate_power power_rate_table;
+	s8 real_cck_bw20[2];
+
+	/* TSSI stuff - only with internal TX ALC */
+	struct tssi_data {
+		int tx0_delta_offset;
+		u8 slope;
+		u8 offset[3];
+	} tssi_data;
+
+	struct reg_channel_bounds reg;
+};
+
+int mt7601u_eeprom_init(struct mt7601u_dev *dev);
+
+static inline u32 s6_validate(u32 reg)
+{
+	WARN_ON(reg & ~GENMASK(5, 0));
+	return reg & GENMASK(5, 0);
+}
+
+static inline int s6_to_int(u32 reg)
+{
+	int s6;
+
+	s6 = s6_validate(reg);
+	if (s6 & BIT(5))
+		s6 -= BIT(6);
+
+	return s6;
+}
+
+static inline u32 int_to_s6(int val)
+{
+	if (val < -0x20)
+		return 0x20;
+	if (val > 0x1f)
+		return 0x1f;
+
+	return val & 0x3f;
+}
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/init.c b/drivers/net/wireless/mediatek/mt7601u/init.c
new file mode 100644
index 000000000000..df3dd56199a7
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/init.c
@@ -0,0 +1,628 @@
+/*
+ * (c) Copyright 2002-2010, Ralink Technology, Inc.
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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 "mt7601u.h"
+#include "eeprom.h"
+#include "trace.h"
+#include "mcu.h"
+
+#include "initvals.h"
+
+static void
+mt7601u_set_wlan_state(struct mt7601u_dev *dev, u32 val, bool enable)
+{
+	int i;
+
+	/* Note: we don't turn off WLAN_CLK because that makes the device
+	 *	 not respond properly on the probe path.
+	 *	 In case anyone (PSM?) wants to use this function we can
+	 *	 bring the clock stuff back and fixup the probe path.
+	 */
+
+	if (enable)
+		val |= (MT_WLAN_FUN_CTRL_WLAN_EN |
+			MT_WLAN_FUN_CTRL_WLAN_CLK_EN);
+	else
+		val &= ~(MT_WLAN_FUN_CTRL_WLAN_EN);
+
+	mt7601u_wr(dev, MT_WLAN_FUN_CTRL, val);
+	udelay(20);
+
+	if (enable) {
+		set_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state);
+	} else {
+		clear_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state);
+		return;
+	}
+
+	for (i = 200; i; i--) {
+		val = mt7601u_rr(dev, MT_CMB_CTRL);
+
+		if (val & MT_CMB_CTRL_XTAL_RDY && val & MT_CMB_CTRL_PLL_LD)
+			break;
+
+		udelay(20);
+	}
+
+	/* Note: vendor driver tries to disable/enable wlan here and retry
+	 *       but the code which does it is so buggy it must have never
+	 *       triggered, so don't bother.
+	 */
+	if (!i)
+		dev_err(dev->dev, "Error: PLL and XTAL check failed!\n");
+}
+
+static void mt7601u_chip_onoff(struct mt7601u_dev *dev, bool enable, bool reset)
+{
+	u32 val;
+
+	mutex_lock(&dev->hw_atomic_mutex);
+
+	val = mt7601u_rr(dev, MT_WLAN_FUN_CTRL);
+
+	if (reset) {
+		val |= MT_WLAN_FUN_CTRL_GPIO_OUT_EN;
+		val &= ~MT_WLAN_FUN_CTRL_FRC_WL_ANT_SEL;
+
+		if (val & MT_WLAN_FUN_CTRL_WLAN_EN) {
+			val |= (MT_WLAN_FUN_CTRL_WLAN_RESET |
+				MT_WLAN_FUN_CTRL_WLAN_RESET_RF);
+			mt7601u_wr(dev, MT_WLAN_FUN_CTRL, val);
+			udelay(20);
+
+			val &= ~(MT_WLAN_FUN_CTRL_WLAN_RESET |
+				 MT_WLAN_FUN_CTRL_WLAN_RESET_RF);
+		}
+	}
+
+	mt7601u_wr(dev, MT_WLAN_FUN_CTRL, val);
+	udelay(20);
+
+	mt7601u_set_wlan_state(dev, val, enable);
+
+	mutex_unlock(&dev->hw_atomic_mutex);
+}
+
+static void mt7601u_reset_csr_bbp(struct mt7601u_dev *dev)
+{
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, (MT_MAC_SYS_CTRL_RESET_CSR |
+					  MT_MAC_SYS_CTRL_RESET_BBP));
+	mt7601u_wr(dev, MT_USB_DMA_CFG, 0);
+	msleep(1);
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, 0);
+}
+
+static void mt7601u_init_usb_dma(struct mt7601u_dev *dev)
+{
+	u32 val;
+
+	val = MT76_SET(MT_USB_DMA_CFG_RX_BULK_AGG_TOUT, MT_USB_AGGR_TIMEOUT) |
+	      MT76_SET(MT_USB_DMA_CFG_RX_BULK_AGG_LMT, MT_USB_AGGR_SIZE_LIMIT) |
+	      MT_USB_DMA_CFG_RX_BULK_EN |
+	      MT_USB_DMA_CFG_TX_BULK_EN;
+	if (dev->in_max_packet == 512)
+		val |= MT_USB_DMA_CFG_RX_BULK_AGG_EN;
+	mt7601u_wr(dev, MT_USB_DMA_CFG, val);
+
+	val |= MT_USB_DMA_CFG_UDMA_RX_WL_DROP;
+	mt7601u_wr(dev, MT_USB_DMA_CFG, val);
+	val &= ~MT_USB_DMA_CFG_UDMA_RX_WL_DROP;
+	mt7601u_wr(dev, MT_USB_DMA_CFG, val);
+}
+
+static int mt7601u_init_bbp(struct mt7601u_dev *dev)
+{
+	int ret;
+
+	ret = mt7601u_wait_bbp_ready(dev);
+	if (ret)
+		return ret;
+
+	ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP, bbp_common_vals,
+				      ARRAY_SIZE(bbp_common_vals));
+	if (ret)
+		return ret;
+
+	return mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP, bbp_chip_vals,
+				       ARRAY_SIZE(bbp_chip_vals));
+}
+
+static void
+mt76_init_beacon_offsets(struct mt7601u_dev *dev)
+{
+	u16 base = MT_BEACON_BASE;
+	u32 regs[4] = {};
+	int i;
+
+	for (i = 0; i < 16; i++) {
+		u16 addr = dev->beacon_offsets[i];
+
+		regs[i / 4] |= ((addr - base) / 64) << (8 * (i % 4));
+	}
+
+	for (i = 0; i < 4; i++)
+		mt7601u_wr(dev, MT_BCN_OFFSET(i), regs[i]);
+}
+
+static int mt7601u_write_mac_initvals(struct mt7601u_dev *dev)
+{
+	int ret;
+
+	ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_WLAN, mac_common_vals,
+				      ARRAY_SIZE(mac_common_vals));
+	if (ret)
+		return ret;
+	ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_WLAN,
+				      mac_chip_vals, ARRAY_SIZE(mac_chip_vals));
+	if (ret)
+		return ret;
+
+	mt76_init_beacon_offsets(dev);
+
+	mt7601u_wr(dev, MT_AUX_CLK_CFG, 0);
+
+	return 0;
+}
+
+static int mt7601u_init_wcid_mem(struct mt7601u_dev *dev)
+{
+	u32 *vals;
+	int i, ret;
+
+	vals = kmalloc(sizeof(*vals) * N_WCIDS * 2, GFP_KERNEL);
+	if (!vals)
+		return -ENOMEM;
+
+	for (i = 0; i < N_WCIDS; i++)  {
+		vals[i * 2] = 0xffffffff;
+		vals[i * 2 + 1] = 0x00ffffff;
+	}
+
+	ret = mt7601u_burst_write_regs(dev, MT_WCID_ADDR_BASE,
+				       vals, N_WCIDS * 2);
+	kfree(vals);
+
+	return ret;
+}
+
+static int mt7601u_init_key_mem(struct mt7601u_dev *dev)
+{
+	u32 vals[4] = {};
+
+	return mt7601u_burst_write_regs(dev, MT_SKEY_MODE_BASE_0,
+					vals, ARRAY_SIZE(vals));
+}
+
+static int mt7601u_init_wcid_attr_mem(struct mt7601u_dev *dev)
+{
+	u32 *vals;
+	int i, ret;
+
+	vals = kmalloc(sizeof(*vals) * N_WCIDS * 2, GFP_KERNEL);
+	if (!vals)
+		return -ENOMEM;
+
+	for (i = 0; i < N_WCIDS * 2; i++)
+		vals[i] = 1;
+
+	ret = mt7601u_burst_write_regs(dev, MT_WCID_ATTR_BASE,
+				       vals, N_WCIDS * 2);
+	kfree(vals);
+
+	return ret;
+}
+
+static void mt7601u_reset_counters(struct mt7601u_dev *dev)
+{
+	mt7601u_rr(dev, MT_RX_STA_CNT0);
+	mt7601u_rr(dev, MT_RX_STA_CNT1);
+	mt7601u_rr(dev, MT_RX_STA_CNT2);
+	mt7601u_rr(dev, MT_TX_STA_CNT0);
+	mt7601u_rr(dev, MT_TX_STA_CNT1);
+	mt7601u_rr(dev, MT_TX_STA_CNT2);
+}
+
+int mt7601u_mac_start(struct mt7601u_dev *dev)
+{
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_ENABLE_TX);
+
+	if (!mt76_poll(dev, MT_WPDMA_GLO_CFG, MT_WPDMA_GLO_CFG_TX_DMA_BUSY |
+		       MT_WPDMA_GLO_CFG_RX_DMA_BUSY, 0, 200000))
+		return -ETIMEDOUT;
+
+	dev->rxfilter = MT_RX_FILTR_CFG_CRC_ERR |
+		MT_RX_FILTR_CFG_PHY_ERR | MT_RX_FILTR_CFG_PROMISC |
+		MT_RX_FILTR_CFG_VER_ERR | MT_RX_FILTR_CFG_DUP |
+		MT_RX_FILTR_CFG_CFACK | MT_RX_FILTR_CFG_CFEND |
+		MT_RX_FILTR_CFG_ACK | MT_RX_FILTR_CFG_CTS |
+		MT_RX_FILTR_CFG_RTS | MT_RX_FILTR_CFG_PSPOLL |
+		MT_RX_FILTR_CFG_BA | MT_RX_FILTR_CFG_CTRL_RSV;
+	mt7601u_wr(dev, MT_RX_FILTR_CFG, dev->rxfilter);
+
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL,
+		   MT_MAC_SYS_CTRL_ENABLE_TX | MT_MAC_SYS_CTRL_ENABLE_RX);
+
+	if (!mt76_poll(dev, MT_WPDMA_GLO_CFG, MT_WPDMA_GLO_CFG_TX_DMA_BUSY |
+		       MT_WPDMA_GLO_CFG_RX_DMA_BUSY, 0, 50))
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static void mt7601u_mac_stop_hw(struct mt7601u_dev *dev)
+{
+	int i, ok;
+
+	if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
+		return;
+
+	mt76_clear(dev, MT_BEACON_TIME_CFG, MT_BEACON_TIME_CFG_TIMER_EN |
+		   MT_BEACON_TIME_CFG_SYNC_MODE | MT_BEACON_TIME_CFG_TBTT_EN |
+		   MT_BEACON_TIME_CFG_BEACON_TX);
+
+	if (!mt76_poll(dev, MT_USB_DMA_CFG, MT_USB_DMA_CFG_TX_BUSY, 0, 1000))
+		dev_warn(dev->dev, "Warning: TX DMA did not stop!\n");
+
+	/* Page count on TxQ */
+	i = 200;
+	while (i-- && ((mt76_rr(dev, 0x0438) & 0xffffffff) ||
+		       (mt76_rr(dev, 0x0a30) & 0x000000ff) ||
+		       (mt76_rr(dev, 0x0a34) & 0x00ff00ff)))
+		msleep(10);
+
+	if (!mt76_poll(dev, MT_MAC_STATUS, MT_MAC_STATUS_TX, 0, 1000))
+		dev_warn(dev->dev, "Warning: MAC TX did not stop!\n");
+
+	mt76_clear(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_ENABLE_RX |
+					 MT_MAC_SYS_CTRL_ENABLE_TX);
+
+	/* Page count on RxQ */
+	ok = 0;
+	i = 200;
+	while (i--) {
+		if ((mt76_rr(dev, 0x0430) & 0x00ff0000) ||
+		    (mt76_rr(dev, 0x0a30) & 0xffffffff) ||
+		    (mt76_rr(dev, 0x0a34) & 0xffffffff))
+			ok++;
+		if (ok > 6)
+			break;
+
+		msleep(1);
+	}
+
+	if (!mt76_poll(dev, MT_MAC_STATUS, MT_MAC_STATUS_RX, 0, 1000))
+		dev_warn(dev->dev, "Warning: MAC RX did not stop!\n");
+
+	if (!mt76_poll(dev, MT_USB_DMA_CFG, MT_USB_DMA_CFG_RX_BUSY, 0, 1000))
+		dev_warn(dev->dev, "Warning: RX DMA did not stop!\n");
+}
+
+void mt7601u_mac_stop(struct mt7601u_dev *dev)
+{
+	mt7601u_mac_stop_hw(dev);
+	flush_delayed_work(&dev->stat_work);
+	cancel_delayed_work_sync(&dev->stat_work);
+}
+
+static void mt7601u_stop_hardware(struct mt7601u_dev *dev)
+{
+	mt7601u_chip_onoff(dev, false, false);
+}
+
+int mt7601u_init_hardware(struct mt7601u_dev *dev)
+{
+	static const u16 beacon_offsets[16] = {
+		/* 512 byte per beacon */
+		0xc000,	0xc200,	0xc400,	0xc600,
+		0xc800,	0xca00,	0xcc00,	0xce00,
+		0xd000,	0xd200,	0xd400,	0xd600,
+		0xd800,	0xda00,	0xdc00,	0xde00
+	};
+	int ret;
+
+	dev->beacon_offsets = beacon_offsets;
+
+	mt7601u_chip_onoff(dev, true, false);
+
+	ret = mt7601u_wait_asic_ready(dev);
+	if (ret)
+		goto err;
+	ret = mt7601u_mcu_init(dev);
+	if (ret)
+		goto err;
+
+	if (!mt76_poll_msec(dev, MT_WPDMA_GLO_CFG,
+			    MT_WPDMA_GLO_CFG_TX_DMA_BUSY |
+			    MT_WPDMA_GLO_CFG_RX_DMA_BUSY, 0, 100)) {
+		ret = -EIO;
+		goto err;
+	}
+
+	/* Wait for ASIC ready after FW load. */
+	ret = mt7601u_wait_asic_ready(dev);
+	if (ret)
+		goto err;
+
+	mt7601u_reset_csr_bbp(dev);
+	mt7601u_init_usb_dma(dev);
+
+	ret = mt7601u_mcu_cmd_init(dev);
+	if (ret)
+		goto err;
+	ret = mt7601u_dma_init(dev);
+	if (ret)
+		goto err_mcu;
+	ret = mt7601u_write_mac_initvals(dev);
+	if (ret)
+		goto err_rx;
+
+	if (!mt76_poll_msec(dev, MT_MAC_STATUS,
+			    MT_MAC_STATUS_TX | MT_MAC_STATUS_RX, 0, 100)) {
+		ret = -EIO;
+		goto err_rx;
+	}
+
+	ret = mt7601u_init_bbp(dev);
+	if (ret)
+		goto err_rx;
+	ret = mt7601u_init_wcid_mem(dev);
+	if (ret)
+		goto err_rx;
+	ret = mt7601u_init_key_mem(dev);
+	if (ret)
+		goto err_rx;
+	ret = mt7601u_init_wcid_attr_mem(dev);
+	if (ret)
+		goto err_rx;
+
+	mt76_clear(dev, MT_BEACON_TIME_CFG, (MT_BEACON_TIME_CFG_TIMER_EN |
+					     MT_BEACON_TIME_CFG_SYNC_MODE |
+					     MT_BEACON_TIME_CFG_TBTT_EN |
+					     MT_BEACON_TIME_CFG_BEACON_TX));
+
+	mt7601u_reset_counters(dev);
+
+	mt7601u_rmw(dev, MT_US_CYC_CFG, MT_US_CYC_CNT, 0x1e);
+
+	mt7601u_wr(dev, MT_TXOP_CTRL_CFG, MT76_SET(MT_TXOP_TRUN_EN, 0x3f) |
+					  MT76_SET(MT_TXOP_EXT_CCA_DLY, 0x58));
+
+	ret = mt7601u_eeprom_init(dev);
+	if (ret)
+		goto err_rx;
+
+	ret = mt7601u_phy_init(dev);
+	if (ret)
+		goto err_rx;
+
+	mt7601u_set_rx_path(dev, 0);
+	mt7601u_set_tx_dac(dev, 0);
+
+	mt7601u_mac_set_ctrlch(dev, false);
+	mt7601u_bbp_set_ctrlch(dev, false);
+	mt7601u_bbp_set_bw(dev, MT_BW_20);
+
+	return 0;
+
+err_rx:
+	mt7601u_dma_cleanup(dev);
+err_mcu:
+	mt7601u_mcu_cmd_deinit(dev);
+err:
+	mt7601u_chip_onoff(dev, false, false);
+	return ret;
+}
+
+void mt7601u_cleanup(struct mt7601u_dev *dev)
+{
+	if (!test_and_clear_bit(MT7601U_STATE_INITIALIZED, &dev->state))
+		return;
+
+	mt7601u_stop_hardware(dev);
+	mt7601u_dma_cleanup(dev);
+	mt7601u_mcu_cmd_deinit(dev);
+}
+
+struct mt7601u_dev *mt7601u_alloc_device(struct device *pdev)
+{
+	struct ieee80211_hw *hw;
+	struct mt7601u_dev *dev;
+
+	hw = ieee80211_alloc_hw(sizeof(*dev), &mt7601u_ops);
+	if (!hw)
+		return NULL;
+
+	dev = hw->priv;
+	dev->dev = pdev;
+	dev->hw = hw;
+	mutex_init(&dev->vendor_req_mutex);
+	mutex_init(&dev->reg_atomic_mutex);
+	mutex_init(&dev->hw_atomic_mutex);
+	mutex_init(&dev->mutex);
+	spin_lock_init(&dev->tx_lock);
+	spin_lock_init(&dev->rx_lock);
+	spin_lock_init(&dev->lock);
+	spin_lock_init(&dev->con_mon_lock);
+	atomic_set(&dev->avg_ampdu_len, 1);
+
+	dev->stat_wq = alloc_workqueue("mt7601u", WQ_UNBOUND, 0);
+	if (!dev->stat_wq) {
+		ieee80211_free_hw(hw);
+		return NULL;
+	}
+
+	return dev;
+}
+
+#define CHAN2G(_idx, _freq) {			\
+	.band = IEEE80211_BAND_2GHZ,		\
+	.center_freq = (_freq),			\
+	.hw_value = (_idx),			\
+	.max_power = 30,			\
+}
+
+static const struct ieee80211_channel mt76_channels_2ghz[] = {
+	CHAN2G(1, 2412),
+	CHAN2G(2, 2417),
+	CHAN2G(3, 2422),
+	CHAN2G(4, 2427),
+	CHAN2G(5, 2432),
+	CHAN2G(6, 2437),
+	CHAN2G(7, 2442),
+	CHAN2G(8, 2447),
+	CHAN2G(9, 2452),
+	CHAN2G(10, 2457),
+	CHAN2G(11, 2462),
+	CHAN2G(12, 2467),
+	CHAN2G(13, 2472),
+	CHAN2G(14, 2484),
+};
+
+#define CCK_RATE(_idx, _rate) {					\
+	.bitrate = _rate,					\
+	.flags = IEEE80211_RATE_SHORT_PREAMBLE,			\
+	.hw_value = (MT_PHY_TYPE_CCK << 8) | _idx,		\
+	.hw_value_short = (MT_PHY_TYPE_CCK << 8) | (8 + _idx),	\
+}
+
+#define OFDM_RATE(_idx, _rate) {				\
+	.bitrate = _rate,					\
+	.hw_value = (MT_PHY_TYPE_OFDM << 8) | _idx,		\
+	.hw_value_short = (MT_PHY_TYPE_OFDM << 8) | _idx,	\
+}
+
+static struct ieee80211_rate mt76_rates[] = {
+	CCK_RATE(0, 10),
+	CCK_RATE(1, 20),
+	CCK_RATE(2, 55),
+	CCK_RATE(3, 110),
+	OFDM_RATE(0, 60),
+	OFDM_RATE(1, 90),
+	OFDM_RATE(2, 120),
+	OFDM_RATE(3, 180),
+	OFDM_RATE(4, 240),
+	OFDM_RATE(5, 360),
+	OFDM_RATE(6, 480),
+	OFDM_RATE(7, 540),
+};
+
+static int
+mt76_init_sband(struct mt7601u_dev *dev, struct ieee80211_supported_band *sband,
+		const struct ieee80211_channel *chan, int n_chan,
+		struct ieee80211_rate *rates, int n_rates)
+{
+	struct ieee80211_sta_ht_cap *ht_cap;
+	void *chanlist;
+	int size;
+
+	size = n_chan * sizeof(*chan);
+	chanlist = devm_kmemdup(dev->dev, chan, size, GFP_KERNEL);
+	if (!chanlist)
+		return -ENOMEM;
+
+	sband->channels = chanlist;
+	sband->n_channels = n_chan;
+	sband->bitrates = rates;
+	sband->n_bitrates = n_rates;
+
+	ht_cap = &sband->ht_cap;
+	ht_cap->ht_supported = true;
+	ht_cap->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
+		      IEEE80211_HT_CAP_GRN_FLD |
+		      IEEE80211_HT_CAP_SGI_20 |
+		      IEEE80211_HT_CAP_SGI_40 |
+		      (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
+
+	ht_cap->mcs.rx_mask[0] = 0xff;
+	ht_cap->mcs.rx_mask[4] = 0x1;
+	ht_cap->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+	ht_cap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+	ht_cap->ampdu_density = IEEE80211_HT_MPDU_DENSITY_2;
+
+	dev->chandef.chan = &sband->channels[0];
+
+	return 0;
+}
+
+static int
+mt76_init_sband_2g(struct mt7601u_dev *dev)
+{
+	dev->sband_2g = devm_kzalloc(dev->dev, sizeof(*dev->sband_2g),
+				     GFP_KERNEL);
+	dev->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = dev->sband_2g;
+
+	WARN_ON(dev->ee->reg.start - 1 + dev->ee->reg.num >
+		ARRAY_SIZE(mt76_channels_2ghz));
+
+	return mt76_init_sband(dev, dev->sband_2g,
+			       &mt76_channels_2ghz[dev->ee->reg.start - 1],
+			       dev->ee->reg.num,
+			       mt76_rates, ARRAY_SIZE(mt76_rates));
+}
+
+int mt7601u_register_device(struct mt7601u_dev *dev)
+{
+	struct ieee80211_hw *hw = dev->hw;
+	struct wiphy *wiphy = hw->wiphy;
+	int ret;
+
+	/* Reserve WCID 0 for mcast - thanks to this APs WCID will go to
+	 * entry no. 1 like it does in the vendor driver.
+	 */
+	dev->wcid_mask[0] |= 1;
+
+	/* init fake wcid for monitor interfaces */
+	dev->mon_wcid = devm_kmalloc(dev->dev, sizeof(*dev->mon_wcid),
+				     GFP_KERNEL);
+	if (!dev->mon_wcid)
+		return -ENOMEM;
+	dev->mon_wcid->idx = 0xff;
+	dev->mon_wcid->hw_key_idx = -1;
+
+	SET_IEEE80211_DEV(hw, dev->dev);
+
+	hw->queues = 4;
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(hw, SUPPORTS_HT_CCK_RATES);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(hw, SUPPORTS_RC_TABLE);
+	hw->max_rates = 1;
+	hw->max_report_rates = 7;
+	hw->max_rate_tries = 1;
+
+	hw->sta_data_size = sizeof(struct mt76_sta);
+	hw->vif_data_size = sizeof(struct mt76_vif);
+
+	SET_IEEE80211_PERM_ADDR(hw, dev->macaddr);
+
+	wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR;
+	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
+
+	ret = mt76_init_sband_2g(dev);
+	if (ret)
+		return ret;
+
+	INIT_DELAYED_WORK(&dev->mac_work, mt7601u_mac_work);
+	INIT_DELAYED_WORK(&dev->stat_work, mt7601u_tx_stat);
+
+	ret = ieee80211_register_hw(hw);
+	if (ret)
+		return ret;
+
+	mt7601u_init_debugfs(dev);
+
+	return 0;
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/initvals.h b/drivers/net/wireless/mediatek/mt7601u/initvals.h
new file mode 100644
index 000000000000..ec11ff66969d
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/initvals.h
@@ -0,0 +1,164 @@
+/*
+ * (c) Copyright 2002-2010, Ralink Technology, Inc.
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef __MT7601U_INITVALS_H
+#define __MT7601U_INITVALS_H
+
+static const struct mt76_reg_pair bbp_common_vals[] = {
+	{  65,	0x2c },
+	{  66,	0x38 },
+	{  68,	0x0b },
+	{  69,	0x12 },
+	{  70,	0x0a },
+	{  73,	0x10 },
+	{  81,	0x37 },
+	{  82,	0x62 },
+	{  83,	0x6a },
+	{  84,	0x99 },
+	{  86,	0x00 },
+	{  91,	0x04 },
+	{  92,	0x00 },
+	{ 103,	0x00 },
+	{ 105,	0x05 },
+	{ 106,	0x35 },
+};
+
+static const struct mt76_reg_pair bbp_chip_vals[] = {
+	{   1, 0x04 },	{   4, 0x40 },	{  20, 0x06 },	{  31, 0x08 },
+	/* CCK Tx Control */
+	{ 178, 0xff },
+	/* AGC/Sync controls */
+	{  66, 0x14 },	{  68, 0x8b },	{  69, 0x12 },	{  70, 0x09 },
+	{  73, 0x11 },	{  75, 0x60 },	{  76, 0x44 },	{  84, 0x9a },
+	{  86, 0x38 },	{  91, 0x07 },	{  92, 0x02 },
+	/* Rx Path Controls */
+	{  99, 0x50 },	{ 101, 0x00 },	{ 103, 0xc0 },	{ 104, 0x92 },
+	{ 105, 0x3c },	{ 106, 0x03 },	{ 128, 0x12 },
+	/* Change RXWI content: Gain Report */
+	{ 142, 0x04 },	{ 143, 0x37 },
+	/* Change RXWI content: Antenna Report */
+	{ 142, 0x03 },	{ 143, 0x99 },
+	/* Calibration Index Register */
+	/* CCK Receiver Control */
+	{ 160, 0xeb },	{ 161, 0xc4 },	{ 162, 0x77 },	{ 163, 0xf9 },
+	{ 164, 0x88 },	{ 165, 0x80 },	{ 166, 0xff },	{ 167, 0xe4 },
+	/* Added AGC controls - these AGC/GLRT registers are accessed
+	 * through R195 and R196.
+	 */
+	{ 195, 0x00 },	{ 196, 0x00 },
+	{ 195, 0x01 },	{ 196, 0x04 },
+	{ 195, 0x02 },	{ 196, 0x20 },
+	{ 195, 0x03 },	{ 196, 0x0a },
+	{ 195, 0x06 },	{ 196, 0x16 },
+	{ 195, 0x07 },	{ 196, 0x05 },
+	{ 195, 0x08 },	{ 196, 0x37 },
+	{ 195, 0x0a },	{ 196, 0x15 },
+	{ 195, 0x0b },	{ 196, 0x17 },
+	{ 195, 0x0c },	{ 196, 0x06 },
+	{ 195, 0x0d },	{ 196, 0x09 },
+	{ 195, 0x0e },	{ 196, 0x05 },
+	{ 195, 0x0f },	{ 196, 0x09 },
+	{ 195, 0x10 },	{ 196, 0x20 },
+	{ 195, 0x20 },	{ 196, 0x17 },
+	{ 195, 0x21 },	{ 196, 0x06 },
+	{ 195, 0x22 },	{ 196, 0x09 },
+	{ 195, 0x23 },	{ 196, 0x17 },
+	{ 195, 0x24 },	{ 196, 0x06 },
+	{ 195, 0x25 },	{ 196, 0x09 },
+	{ 195, 0x26 },	{ 196, 0x17 },
+	{ 195, 0x27 },	{ 196, 0x06 },
+	{ 195, 0x28 },	{ 196, 0x09 },
+	{ 195, 0x29 },	{ 196, 0x05 },
+	{ 195, 0x2a },	{ 196, 0x09 },
+	{ 195, 0x80 },	{ 196, 0x8b },
+	{ 195, 0x81 },	{ 196, 0x12 },
+	{ 195, 0x82 },	{ 196, 0x09 },
+	{ 195, 0x83 },	{ 196, 0x17 },
+	{ 195, 0x84 },	{ 196, 0x11 },
+	{ 195, 0x85 },	{ 196, 0x00 },
+	{ 195, 0x86 },	{ 196, 0x00 },
+	{ 195, 0x87 },	{ 196, 0x18 },
+	{ 195, 0x88 },	{ 196, 0x60 },
+	{ 195, 0x89 },	{ 196, 0x44 },
+	{ 195, 0x8a },	{ 196, 0x8b },
+	{ 195, 0x8b },	{ 196, 0x8b },
+	{ 195, 0x8c },	{ 196, 0x8b },
+	{ 195, 0x8d },	{ 196, 0x8b },
+	{ 195, 0x8e },	{ 196, 0x09 },
+	{ 195, 0x8f },	{ 196, 0x09 },
+	{ 195, 0x90 },	{ 196, 0x09 },
+	{ 195, 0x91 },	{ 196, 0x09 },
+	{ 195, 0x92 },	{ 196, 0x11 },
+	{ 195, 0x93 },	{ 196, 0x11 },
+	{ 195, 0x94 },	{ 196, 0x11 },
+	{ 195, 0x95 },	{ 196, 0x11 },
+	/* PPAD */
+	{  47, 0x80 },	{  60, 0x80 },	{ 150, 0xd2 },	{ 151, 0x32 },
+	{ 152, 0x23 },	{ 153, 0x41 },	{ 154, 0x00 },	{ 155, 0x4f },
+	{ 253, 0x7e },	{ 195, 0x30 },	{ 196, 0x32 },	{ 195, 0x31 },
+	{ 196, 0x23 },	{ 195, 0x32 },	{ 196, 0x45 },	{ 195, 0x35 },
+	{ 196, 0x4a },	{ 195, 0x36 },	{ 196, 0x5a },	{ 195, 0x37 },
+	{ 196, 0x5a },
+};
+
+static const struct mt76_reg_pair mac_common_vals[] = {
+	{ MT_LEGACY_BASIC_RATE,		0x0000013f },
+	{ MT_HT_BASIC_RATE,		0x00008003 },
+	{ MT_MAC_SYS_CTRL,		0x00000000 },
+	{ MT_RX_FILTR_CFG,		0x00017f97 },
+	{ MT_BKOFF_SLOT_CFG,		0x00000209 },
+	{ MT_TX_SW_CFG0,		0x00000000 },
+	{ MT_TX_SW_CFG1,		0x00080606 },
+	{ MT_TX_LINK_CFG,		0x00001020 },
+	{ MT_TX_TIMEOUT_CFG,		0x000a2090 },
+	{ MT_MAX_LEN_CFG,		0x00003fff },
+	{ MT_PBF_TX_MAX_PCNT,		0x1fbf1f1f },
+	{ MT_PBF_RX_MAX_PCNT,		0x0000009f },
+	{ MT_TX_RETRY_CFG,		0x47d01f0f },
+	{ MT_AUTO_RSP_CFG,		0x00000013 },
+	{ MT_CCK_PROT_CFG,		0x05740003 },
+	{ MT_OFDM_PROT_CFG,		0x05740003 },
+	{ MT_MM40_PROT_CFG,		0x03f44084 },
+	{ MT_GF20_PROT_CFG,		0x01744004 },
+	{ MT_GF40_PROT_CFG,		0x03f44084 },
+	{ MT_MM20_PROT_CFG,		0x01744004 },
+	{ MT_TXOP_CTRL_CFG,		0x0000583f },
+	{ MT_TX_RTS_CFG,		0x01092b20 },
+	{ MT_EXP_ACK_TIME,		0x002400ca },
+	{ MT_TXOP_HLDR_ET,		0x00000002 },
+	{ MT_XIFS_TIME_CFG,		0x33a41010 },
+	{ MT_PWR_PIN_CFG,		0x00000000 },
+};
+
+static const struct mt76_reg_pair mac_chip_vals[] = {
+	{ MT_TSO_CTRL,			0x00006050 },
+	{ MT_BCN_OFFSET(0),		0x18100800 },
+	{ MT_BCN_OFFSET(1),		0x38302820 },
+	{ MT_PBF_SYS_CTRL,		0x00080c00 },
+	{ MT_PBF_CFG,			0x7f723c1f },
+	{ MT_FCE_PSE_CTRL,		0x00000001 },
+	{ MT_PAUSE_ENABLE_CONTROL1,	0x00000000 },
+	{ MT_TX0_RF_GAIN_CORR,		0x003b0005 },
+	{ MT_TX0_RF_GAIN_ATTEN,		0x00006900 },
+	{ MT_TX0_BB_GAIN_ATTEN,		0x00000400 },
+	{ MT_TX_ALC_VGA3,		0x00060006 },
+	{ MT_TX_SW_CFG0,		0x00000402 },
+	{ MT_TX_SW_CFG1,		0x00000000 },
+	{ MT_TX_SW_CFG2,		0x00000000 },
+	{ MT_HEADER_TRANS_CTRL_REG,	0x00000000 },
+	{ MT_FCE_CSO,			0x0000030f },
+	{ MT_FCE_PARAMETERS,		0x00256f0f },
+};
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/initvals_phy.h b/drivers/net/wireless/mediatek/mt7601u/initvals_phy.h
new file mode 100644
index 000000000000..a2bdc3e322bf
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/initvals_phy.h
@@ -0,0 +1,291 @@
+/*
+ * (c) Copyright 2002-2010, Ralink Technology, Inc.
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef __MT7601U_PHY_INITVALS_H
+#define __MT7601U_PHY_INITVALS_H
+
+#define RF_REG_PAIR(bank, reg, value)				\
+	{ MT_MCU_MEMMAP_RF | (bank) << 16 | (reg), value }
+
+static const struct mt76_reg_pair rf_central[] = {
+	/* Bank 0 - for central blocks: BG, PLL, XTAL, LO, ADC/DAC */
+	RF_REG_PAIR(0,	 0, 0x02),
+	RF_REG_PAIR(0,	 1, 0x01),
+	RF_REG_PAIR(0,	 2, 0x11),
+	RF_REG_PAIR(0,	 3, 0xff),
+	RF_REG_PAIR(0,	 4, 0x0a),
+	RF_REG_PAIR(0,	 5, 0x20),
+	RF_REG_PAIR(0,	 6, 0x00),
+	/* B/G */
+	RF_REG_PAIR(0,	 7, 0x00),
+	RF_REG_PAIR(0,	 8, 0x00),
+	RF_REG_PAIR(0,	 9, 0x00),
+	RF_REG_PAIR(0,	10, 0x00),
+	RF_REG_PAIR(0,	11, 0x21),
+	/* XO */
+	RF_REG_PAIR(0,	13, 0x00),		/* 40mhz xtal */
+	/* RF_REG_PAIR(0,	13, 0x13), */	/* 20mhz xtal */
+	RF_REG_PAIR(0,	14, 0x7c),
+	RF_REG_PAIR(0,	15, 0x22),
+	RF_REG_PAIR(0,	16, 0x80),
+	/* PLL */
+	RF_REG_PAIR(0,	17, 0x99),
+	RF_REG_PAIR(0,	18, 0x99),
+	RF_REG_PAIR(0,	19, 0x09),
+	RF_REG_PAIR(0,	20, 0x50),
+	RF_REG_PAIR(0,	21, 0xb0),
+	RF_REG_PAIR(0,	22, 0x00),
+	RF_REG_PAIR(0,	23, 0xc5),
+	RF_REG_PAIR(0,	24, 0xfc),
+	RF_REG_PAIR(0,	25, 0x40),
+	RF_REG_PAIR(0,	26, 0x4d),
+	RF_REG_PAIR(0,	27, 0x02),
+	RF_REG_PAIR(0,	28, 0x72),
+	RF_REG_PAIR(0,	29, 0x01),
+	RF_REG_PAIR(0,	30, 0x00),
+	RF_REG_PAIR(0,	31, 0x00),
+	/* test ports */
+	RF_REG_PAIR(0,	32, 0x00),
+	RF_REG_PAIR(0,	33, 0x00),
+	RF_REG_PAIR(0,	34, 0x23),
+	RF_REG_PAIR(0,	35, 0x01), /* change setting to reduce spurs */
+	RF_REG_PAIR(0,	36, 0x00),
+	RF_REG_PAIR(0,	37, 0x00),
+	/* ADC/DAC */
+	RF_REG_PAIR(0,	38, 0x00),
+	RF_REG_PAIR(0,	39, 0x20),
+	RF_REG_PAIR(0,	40, 0x00),
+	RF_REG_PAIR(0,	41, 0xd0),
+	RF_REG_PAIR(0,	42, 0x1b),
+	RF_REG_PAIR(0,	43, 0x02),
+	RF_REG_PAIR(0,	44, 0x00),
+};
+
+static const struct mt76_reg_pair rf_channel[] = {
+	RF_REG_PAIR(4,	 0, 0x01),
+	RF_REG_PAIR(4,	 1, 0x00),
+	RF_REG_PAIR(4,	 2, 0x00),
+	RF_REG_PAIR(4,	 3, 0x00),
+	/* LDO */
+	RF_REG_PAIR(4,	 4, 0x00),
+	RF_REG_PAIR(4,	 5, 0x08),
+	RF_REG_PAIR(4,	 6, 0x00),
+	/* RX */
+	RF_REG_PAIR(4,	 7, 0x5b),
+	RF_REG_PAIR(4,	 8, 0x52),
+	RF_REG_PAIR(4,	 9, 0xb6),
+	RF_REG_PAIR(4,	10, 0x57),
+	RF_REG_PAIR(4,	11, 0x33),
+	RF_REG_PAIR(4,	12, 0x22),
+	RF_REG_PAIR(4,	13, 0x3d),
+	RF_REG_PAIR(4,	14, 0x3e),
+	RF_REG_PAIR(4,	15, 0x13),
+	RF_REG_PAIR(4,	16, 0x22),
+	RF_REG_PAIR(4,	17, 0x23),
+	RF_REG_PAIR(4,	18, 0x02),
+	RF_REG_PAIR(4,	19, 0xa4),
+	RF_REG_PAIR(4,	20, 0x01),
+	RF_REG_PAIR(4,	21, 0x12),
+	RF_REG_PAIR(4,	22, 0x80),
+	RF_REG_PAIR(4,	23, 0xb3),
+	RF_REG_PAIR(4,	24, 0x00), /* reserved */
+	RF_REG_PAIR(4,	25, 0x00), /* reserved */
+	RF_REG_PAIR(4,	26, 0x00), /* reserved */
+	RF_REG_PAIR(4,	27, 0x00), /* reserved */
+	/* LOGEN */
+	RF_REG_PAIR(4,	28, 0x18),
+	RF_REG_PAIR(4,	29, 0xee),
+	RF_REG_PAIR(4,	30, 0x6b),
+	RF_REG_PAIR(4,	31, 0x31),
+	RF_REG_PAIR(4,	32, 0x5d),
+	RF_REG_PAIR(4,	33, 0x00), /* reserved */
+	/* TX */
+	RF_REG_PAIR(4,	34, 0x96),
+	RF_REG_PAIR(4,	35, 0x55),
+	RF_REG_PAIR(4,	36, 0x08),
+	RF_REG_PAIR(4,	37, 0xbb),
+	RF_REG_PAIR(4,	38, 0xb3),
+	RF_REG_PAIR(4,	39, 0xb3),
+	RF_REG_PAIR(4,	40, 0x03),
+	RF_REG_PAIR(4,	41, 0x00), /* reserved */
+	RF_REG_PAIR(4,	42, 0x00), /* reserved */
+	RF_REG_PAIR(4,	43, 0xc5),
+	RF_REG_PAIR(4,	44, 0xc5),
+	RF_REG_PAIR(4,	45, 0xc5),
+	RF_REG_PAIR(4,	46, 0x07),
+	RF_REG_PAIR(4,	47, 0xa8),
+	RF_REG_PAIR(4,	48, 0xef),
+	RF_REG_PAIR(4,	49, 0x1a),
+	/* PA */
+	RF_REG_PAIR(4,	54, 0x07),
+	RF_REG_PAIR(4,	55, 0xa7),
+	RF_REG_PAIR(4,	56, 0xcc),
+	RF_REG_PAIR(4,	57, 0x14),
+	RF_REG_PAIR(4,	58, 0x07),
+	RF_REG_PAIR(4,	59, 0xa8),
+	RF_REG_PAIR(4,	60, 0xd7),
+	RF_REG_PAIR(4,	61, 0x10),
+	RF_REG_PAIR(4,	62, 0x1c),
+	RF_REG_PAIR(4,	63, 0x00), /* reserved */
+};
+
+static const struct mt76_reg_pair rf_vga[] = {
+	RF_REG_PAIR(5,	 0, 0x47),
+	RF_REG_PAIR(5,	 1, 0x00),
+	RF_REG_PAIR(5,	 2, 0x00),
+	RF_REG_PAIR(5,	 3, 0x08),
+	RF_REG_PAIR(5,	 4, 0x04),
+	RF_REG_PAIR(5,	 5, 0x20),
+	RF_REG_PAIR(5,	 6, 0x3a),
+	RF_REG_PAIR(5,	 7, 0x3a),
+	RF_REG_PAIR(5,	 8, 0x00),
+	RF_REG_PAIR(5,	 9, 0x00),
+	RF_REG_PAIR(5,	10, 0x10),
+	RF_REG_PAIR(5,	11, 0x10),
+	RF_REG_PAIR(5,	12, 0x10),
+	RF_REG_PAIR(5,	13, 0x10),
+	RF_REG_PAIR(5,	14, 0x10),
+	RF_REG_PAIR(5,	15, 0x20),
+	RF_REG_PAIR(5,	16, 0x22),
+	RF_REG_PAIR(5,	17, 0x7c),
+	RF_REG_PAIR(5,	18, 0x00),
+	RF_REG_PAIR(5,	19, 0x00),
+	RF_REG_PAIR(5,	20, 0x00),
+	RF_REG_PAIR(5,	21, 0xf1),
+	RF_REG_PAIR(5,	22, 0x11),
+	RF_REG_PAIR(5,	23, 0x02),
+	RF_REG_PAIR(5,	24, 0x41),
+	RF_REG_PAIR(5,	25, 0x20),
+	RF_REG_PAIR(5,	26, 0x00),
+	RF_REG_PAIR(5,	27, 0xd7),
+	RF_REG_PAIR(5,	28, 0xa2),
+	RF_REG_PAIR(5,	29, 0x20),
+	RF_REG_PAIR(5,	30, 0x49),
+	RF_REG_PAIR(5,	31, 0x20),
+	RF_REG_PAIR(5,	32, 0x04),
+	RF_REG_PAIR(5,	33, 0xf1),
+	RF_REG_PAIR(5,	34, 0xa1),
+	RF_REG_PAIR(5,	35, 0x01),
+	RF_REG_PAIR(5,	41, 0x00),
+	RF_REG_PAIR(5,	42, 0x00),
+	RF_REG_PAIR(5,	43, 0x00),
+	RF_REG_PAIR(5,	44, 0x00),
+	RF_REG_PAIR(5,	45, 0x00),
+	RF_REG_PAIR(5,	46, 0x00),
+	RF_REG_PAIR(5,	47, 0x00),
+	RF_REG_PAIR(5,	48, 0x00),
+	RF_REG_PAIR(5,	49, 0x00),
+	RF_REG_PAIR(5,	50, 0x00),
+	RF_REG_PAIR(5,	51, 0x00),
+	RF_REG_PAIR(5,	52, 0x00),
+	RF_REG_PAIR(5,	53, 0x00),
+	RF_REG_PAIR(5,	54, 0x00),
+	RF_REG_PAIR(5,	55, 0x00),
+	RF_REG_PAIR(5,	56, 0x00),
+	RF_REG_PAIR(5,	57, 0x00),
+	RF_REG_PAIR(5,	58, 0x31),
+	RF_REG_PAIR(5,	59, 0x31),
+	RF_REG_PAIR(5,	60, 0x0a),
+	RF_REG_PAIR(5,	61, 0x02),
+	RF_REG_PAIR(5,	62, 0x00),
+	RF_REG_PAIR(5,	63, 0x00),
+};
+
+/* TODO: BBP178 is set to 0xff for "CCK CH14 OBW" which overrides the settings
+ *	 from channel switching. Seems stupid at best.
+ */
+static const struct mt76_reg_pair bbp_high_temp[] = {
+	{  75, 0x60 },
+	{  92, 0x02 },
+	{ 178, 0xff }, /* For CCK CH14 OBW */
+	{ 195, 0x88 }, { 196, 0x60 },
+}, bbp_high_temp_bw20[] = {
+	{  69, 0x12 },
+	{  91, 0x07 },
+	{ 195, 0x23 }, { 196, 0x17 },
+	{ 195, 0x24 }, { 196, 0x06 },
+	{ 195, 0x81 }, { 196, 0x12 },
+	{ 195, 0x83 }, { 196, 0x17 },
+}, bbp_high_temp_bw40[] = {
+	{  69, 0x15 },
+	{  91, 0x04 },
+	{ 195, 0x23 }, { 196, 0x12 },
+	{ 195, 0x24 }, { 196, 0x08 },
+	{ 195, 0x81 }, { 196, 0x15 },
+	{ 195, 0x83 }, { 196, 0x16 },
+}, bbp_low_temp[] = {
+	{ 178, 0xff }, /* For CCK CH14 OBW */
+}, bbp_low_temp_bw20[] = {
+	{  69, 0x12 },
+	{  75, 0x5e },
+	{  91, 0x07 },
+	{  92, 0x02 },
+	{ 195, 0x23 }, { 196, 0x17 },
+	{ 195, 0x24 }, { 196, 0x06 },
+	{ 195, 0x81 }, { 196, 0x12 },
+	{ 195, 0x83 }, { 196, 0x17 },
+	{ 195, 0x88 }, { 196, 0x5e },
+}, bbp_low_temp_bw40[] = {
+	{  69, 0x15 },
+	{  75, 0x5c },
+	{  91, 0x04 },
+	{  92, 0x03 },
+	{ 195, 0x23 }, { 196, 0x10 },
+	{ 195, 0x24 }, { 196, 0x08 },
+	{ 195, 0x81 }, { 196, 0x15 },
+	{ 195, 0x83 }, { 196, 0x16 },
+	{ 195, 0x88 }, { 196, 0x5b },
+}, bbp_normal_temp[] = {
+	{  75, 0x60 },
+	{  92, 0x02 },
+	{ 178, 0xff }, /* For CCK CH14 OBW */
+	{ 195, 0x88 }, { 196, 0x60 },
+}, bbp_normal_temp_bw20[] = {
+	{  69, 0x12 },
+	{  91, 0x07 },
+	{ 195, 0x23 }, { 196, 0x17 },
+	{ 195, 0x24 }, { 196, 0x06 },
+	{ 195, 0x81 }, { 196, 0x12 },
+	{ 195, 0x83 }, { 196, 0x17 },
+}, bbp_normal_temp_bw40[] = {
+	{  69, 0x15 },
+	{  91, 0x04 },
+	{ 195, 0x23 }, { 196, 0x12 },
+	{ 195, 0x24 }, { 196, 0x08 },
+	{ 195, 0x81 }, { 196, 0x15 },
+	{ 195, 0x83 }, { 196, 0x16 },
+};
+
+#define BBP_TABLE(arr) { arr, ARRAY_SIZE(arr), }
+
+static const struct reg_table {
+	const struct mt76_reg_pair *regs;
+	size_t n;
+} bbp_mode_table[3][3] = {
+	{
+		BBP_TABLE(bbp_normal_temp_bw20),
+		BBP_TABLE(bbp_normal_temp_bw40),
+		BBP_TABLE(bbp_normal_temp),
+	}, {
+		BBP_TABLE(bbp_high_temp_bw20),
+		BBP_TABLE(bbp_high_temp_bw40),
+		BBP_TABLE(bbp_high_temp),
+	}, {
+		BBP_TABLE(bbp_low_temp_bw20),
+		BBP_TABLE(bbp_low_temp_bw40),
+		BBP_TABLE(bbp_low_temp),
+	}
+};
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/mac.c b/drivers/net/wireless/mediatek/mt7601u/mac.c
new file mode 100644
index 000000000000..7514bce1ac91
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/mac.c
@@ -0,0 +1,573 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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 "mt7601u.h"
+#include "trace.h"
+#include <linux/etherdevice.h>
+
+static void
+mt76_mac_process_tx_rate(struct ieee80211_tx_rate *txrate, u16 rate)
+{
+	u8 idx = MT76_GET(MT_TXWI_RATE_MCS, rate);
+
+	txrate->idx = 0;
+	txrate->flags = 0;
+	txrate->count = 1;
+
+	switch (MT76_GET(MT_TXWI_RATE_PHY_MODE, rate)) {
+	case MT_PHY_TYPE_OFDM:
+		txrate->idx = idx + 4;
+		return;
+	case MT_PHY_TYPE_CCK:
+		if (idx >= 8)
+			idx -= 8;
+
+		txrate->idx = idx;
+		return;
+	case MT_PHY_TYPE_HT_GF:
+		txrate->flags |= IEEE80211_TX_RC_GREEN_FIELD;
+		/* fall through */
+	case MT_PHY_TYPE_HT:
+		txrate->flags |= IEEE80211_TX_RC_MCS;
+		txrate->idx = idx;
+		break;
+	default:
+		WARN_ON(1);
+		return;
+	}
+
+	if (MT76_GET(MT_TXWI_RATE_BW, rate) == MT_PHY_BW_40)
+		txrate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+
+	if (rate & MT_TXWI_RATE_SGI)
+		txrate->flags |= IEEE80211_TX_RC_SHORT_GI;
+}
+
+static void
+mt76_mac_fill_tx_status(struct mt7601u_dev *dev, struct ieee80211_tx_info *info,
+			struct mt76_tx_status *st)
+{
+	struct ieee80211_tx_rate *rate = info->status.rates;
+	int cur_idx, last_rate;
+	int i;
+
+	last_rate = min_t(int, st->retry, IEEE80211_TX_MAX_RATES - 1);
+	mt76_mac_process_tx_rate(&rate[last_rate], st->rate);
+	if (last_rate < IEEE80211_TX_MAX_RATES - 1)
+		rate[last_rate + 1].idx = -1;
+
+	cur_idx = rate[last_rate].idx + st->retry;
+	for (i = 0; i <= last_rate; i++) {
+		rate[i].flags = rate[last_rate].flags;
+		rate[i].idx = max_t(int, 0, cur_idx - i);
+		rate[i].count = 1;
+	}
+
+	if (last_rate > 0)
+		rate[last_rate - 1].count = st->retry + 1 - last_rate;
+
+	info->status.ampdu_len = 1;
+	info->status.ampdu_ack_len = st->success;
+
+	if (st->is_probe)
+		info->flags |= IEEE80211_TX_CTL_RATE_CTRL_PROBE;
+
+	if (st->aggr)
+		info->flags |= IEEE80211_TX_CTL_AMPDU |
+			       IEEE80211_TX_STAT_AMPDU;
+
+	if (!st->ack_req)
+		info->flags |= IEEE80211_TX_CTL_NO_ACK;
+	else if (st->success)
+		info->flags |= IEEE80211_TX_STAT_ACK;
+}
+
+u16 mt76_mac_tx_rate_val(struct mt7601u_dev *dev,
+			 const struct ieee80211_tx_rate *rate, u8 *nss_val)
+{
+	u16 rateval;
+	u8 phy, rate_idx;
+	u8 nss = 1;
+	u8 bw = 0;
+
+	if (rate->flags & IEEE80211_TX_RC_MCS) {
+		rate_idx = rate->idx;
+		nss = 1 + (rate->idx >> 3);
+		phy = MT_PHY_TYPE_HT;
+		if (rate->flags & IEEE80211_TX_RC_GREEN_FIELD)
+			phy = MT_PHY_TYPE_HT_GF;
+		if (rate->flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
+			bw = 1;
+	} else {
+		const struct ieee80211_rate *r;
+		int band = dev->chandef.chan->band;
+		u16 val;
+
+		r = &dev->hw->wiphy->bands[band]->bitrates[rate->idx];
+		if (rate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
+			val = r->hw_value_short;
+		else
+			val = r->hw_value;
+
+		phy = val >> 8;
+		rate_idx = val & 0xff;
+		bw = 0;
+	}
+
+	rateval = MT76_SET(MT_RXWI_RATE_MCS, rate_idx);
+	rateval |= MT76_SET(MT_RXWI_RATE_PHY, phy);
+	rateval |= MT76_SET(MT_RXWI_RATE_BW, bw);
+	if (rate->flags & IEEE80211_TX_RC_SHORT_GI)
+		rateval |= MT_RXWI_RATE_SGI;
+
+	*nss_val = nss;
+	return rateval;
+}
+
+void mt76_mac_wcid_set_rate(struct mt7601u_dev *dev, struct mt76_wcid *wcid,
+			    const struct ieee80211_tx_rate *rate)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	wcid->tx_rate = mt76_mac_tx_rate_val(dev, rate, &wcid->tx_rate_nss);
+	wcid->tx_rate_set = true;
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
+
+struct mt76_tx_status mt7601u_mac_fetch_tx_status(struct mt7601u_dev *dev)
+{
+	struct mt76_tx_status stat = {};
+	u32 val;
+
+	val = mt7601u_rr(dev, MT_TX_STAT_FIFO);
+	stat.valid = !!(val & MT_TX_STAT_FIFO_VALID);
+	stat.success = !!(val & MT_TX_STAT_FIFO_SUCCESS);
+	stat.aggr = !!(val & MT_TX_STAT_FIFO_AGGR);
+	stat.ack_req = !!(val & MT_TX_STAT_FIFO_ACKREQ);
+	stat.pktid = MT76_GET(MT_TX_STAT_FIFO_PID_TYPE, val);
+	stat.wcid = MT76_GET(MT_TX_STAT_FIFO_WCID, val);
+	stat.rate = MT76_GET(MT_TX_STAT_FIFO_RATE, val);
+
+	return stat;
+}
+
+void mt76_send_tx_status(struct mt7601u_dev *dev, struct mt76_tx_status *stat)
+{
+	struct ieee80211_tx_info info = {};
+	struct ieee80211_sta *sta = NULL;
+	struct mt76_wcid *wcid = NULL;
+	void *msta;
+
+	rcu_read_lock();
+	if (stat->wcid < ARRAY_SIZE(dev->wcid))
+		wcid = rcu_dereference(dev->wcid[stat->wcid]);
+
+	if (wcid) {
+		msta = container_of(wcid, struct mt76_sta, wcid);
+		sta = container_of(msta, struct ieee80211_sta,
+				   drv_priv);
+	}
+
+	mt76_mac_fill_tx_status(dev, &info, stat);
+	ieee80211_tx_status_noskb(dev->hw, sta, &info);
+	rcu_read_unlock();
+}
+
+void mt7601u_mac_set_protection(struct mt7601u_dev *dev, bool legacy_prot,
+				int ht_mode)
+{
+	int mode = ht_mode & IEEE80211_HT_OP_MODE_PROTECTION;
+	bool non_gf = !!(ht_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
+	u32 prot[6];
+	bool ht_rts[4] = {};
+	int i;
+
+	prot[0] = MT_PROT_NAV_SHORT |
+		  MT_PROT_TXOP_ALLOW_ALL |
+		  MT_PROT_RTS_THR_EN;
+	prot[1] = prot[0];
+	if (legacy_prot)
+		prot[1] |= MT_PROT_CTRL_CTS2SELF;
+
+	prot[2] = prot[4] = MT_PROT_NAV_SHORT | MT_PROT_TXOP_ALLOW_BW20;
+	prot[3] = prot[5] = MT_PROT_NAV_SHORT | MT_PROT_TXOP_ALLOW_ALL;
+
+	if (legacy_prot) {
+		prot[2] |= MT_PROT_RATE_CCK_11;
+		prot[3] |= MT_PROT_RATE_CCK_11;
+		prot[4] |= MT_PROT_RATE_CCK_11;
+		prot[5] |= MT_PROT_RATE_CCK_11;
+	} else {
+		prot[2] |= MT_PROT_RATE_OFDM_24;
+		prot[3] |= MT_PROT_RATE_DUP_OFDM_24;
+		prot[4] |= MT_PROT_RATE_OFDM_24;
+		prot[5] |= MT_PROT_RATE_DUP_OFDM_24;
+	}
+
+	switch (mode) {
+	case IEEE80211_HT_OP_MODE_PROTECTION_NONE:
+		break;
+
+	case IEEE80211_HT_OP_MODE_PROTECTION_NONMEMBER:
+		ht_rts[0] = ht_rts[1] = ht_rts[2] = ht_rts[3] = true;
+		break;
+
+	case IEEE80211_HT_OP_MODE_PROTECTION_20MHZ:
+		ht_rts[1] = ht_rts[3] = true;
+		break;
+
+	case IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED:
+		ht_rts[0] = ht_rts[1] = ht_rts[2] = ht_rts[3] = true;
+		break;
+	}
+
+	if (non_gf)
+		ht_rts[2] = ht_rts[3] = true;
+
+	for (i = 0; i < 4; i++)
+		if (ht_rts[i])
+			prot[i + 2] |= MT_PROT_CTRL_RTS_CTS;
+
+	for (i = 0; i < 6; i++)
+		mt7601u_wr(dev, MT_CCK_PROT_CFG + i * 4, prot[i]);
+}
+
+void mt7601u_mac_set_short_preamble(struct mt7601u_dev *dev, bool short_preamb)
+{
+	if (short_preamb)
+		mt76_set(dev, MT_AUTO_RSP_CFG, MT_AUTO_RSP_PREAMB_SHORT);
+	else
+		mt76_clear(dev, MT_AUTO_RSP_CFG, MT_AUTO_RSP_PREAMB_SHORT);
+}
+
+void mt7601u_mac_config_tsf(struct mt7601u_dev *dev, bool enable, int interval)
+{
+	u32 val = mt7601u_rr(dev, MT_BEACON_TIME_CFG);
+
+	val &= ~(MT_BEACON_TIME_CFG_TIMER_EN |
+		 MT_BEACON_TIME_CFG_SYNC_MODE |
+		 MT_BEACON_TIME_CFG_TBTT_EN);
+
+	if (!enable) {
+		mt7601u_wr(dev, MT_BEACON_TIME_CFG, val);
+		return;
+	}
+
+	val &= ~MT_BEACON_TIME_CFG_INTVAL;
+	val |= MT76_SET(MT_BEACON_TIME_CFG_INTVAL, interval << 4) |
+		MT_BEACON_TIME_CFG_TIMER_EN |
+		MT_BEACON_TIME_CFG_SYNC_MODE |
+		MT_BEACON_TIME_CFG_TBTT_EN;
+}
+
+static void mt7601u_check_mac_err(struct mt7601u_dev *dev)
+{
+	u32 val = mt7601u_rr(dev, 0x10f4);
+
+	if (!(val & BIT(29)) || !(val & (BIT(7) | BIT(5))))
+		return;
+
+	dev_err(dev->dev, "Error: MAC specific condition occurred\n");
+
+	mt76_set(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_RESET_CSR);
+	udelay(10);
+	mt76_clear(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_RESET_CSR);
+}
+
+void mt7601u_mac_work(struct work_struct *work)
+{
+	struct mt7601u_dev *dev = container_of(work, struct mt7601u_dev,
+					       mac_work.work);
+	struct {
+		u32 addr_base;
+		u32 span;
+		u64 *stat_base;
+	} spans[] = {
+		{ MT_RX_STA_CNT0,	3,	dev->stats.rx_stat },
+		{ MT_TX_STA_CNT0,	3,	dev->stats.tx_stat },
+		{ MT_TX_AGG_STAT,	1,	dev->stats.aggr_stat },
+		{ MT_MPDU_DENSITY_CNT,	1,	dev->stats.zero_len_del },
+		{ MT_TX_AGG_CNT_BASE0,	8,	&dev->stats.aggr_n[0] },
+		{ MT_TX_AGG_CNT_BASE1,	8,	&dev->stats.aggr_n[16] },
+	};
+	u32 sum, n;
+	int i, j, k;
+
+	/* Note: using MCU_RANDOM_READ is actually slower then reading all the
+	 *	 registers by hand.  MCU takes ca. 20ms to complete read of 24
+	 *	 registers while reading them one by one will takes roughly
+	 *	 24*200us =~ 5ms.
+	 */
+
+	k = 0;
+	n = 0;
+	sum = 0;
+	for (i = 0; i < ARRAY_SIZE(spans); i++)
+		for (j = 0; j < spans[i].span; j++) {
+			u32 val = mt7601u_rr(dev, spans[i].addr_base + j * 4);
+
+			spans[i].stat_base[j * 2] += val & 0xffff;
+			spans[i].stat_base[j * 2 + 1] += val >> 16;
+
+			/* Calculate average AMPDU length */
+			if (spans[i].addr_base != MT_TX_AGG_CNT_BASE0 &&
+			    spans[i].addr_base != MT_TX_AGG_CNT_BASE1)
+				continue;
+
+			n += (val >> 16) + (val & 0xffff);
+			sum += (val & 0xffff) * (1 + k * 2) +
+				(val >> 16) * (2 + k * 2);
+			k++;
+		}
+
+	atomic_set(&dev->avg_ampdu_len, n ? DIV_ROUND_CLOSEST(sum, n) : 1);
+
+	mt7601u_check_mac_err(dev);
+
+	ieee80211_queue_delayed_work(dev->hw, &dev->mac_work, 10 * HZ);
+}
+
+void
+mt7601u_mac_wcid_setup(struct mt7601u_dev *dev, u8 idx, u8 vif_idx, u8 *mac)
+{
+	u8 zmac[ETH_ALEN] = {};
+	u32 attr;
+
+	attr = MT76_SET(MT_WCID_ATTR_BSS_IDX, vif_idx & 7) |
+	       MT76_SET(MT_WCID_ATTR_BSS_IDX_EXT, !!(vif_idx & 8));
+
+	mt76_wr(dev, MT_WCID_ATTR(idx), attr);
+
+	if (mac)
+		memcpy(zmac, mac, sizeof(zmac));
+
+	mt7601u_addr_wr(dev, MT_WCID_ADDR(idx), zmac);
+}
+
+void mt7601u_mac_set_ampdu_factor(struct mt7601u_dev *dev)
+{
+	struct ieee80211_sta *sta;
+	struct mt76_wcid *wcid;
+	void *msta;
+	u8 min_factor = 3;
+	int i;
+
+	rcu_read_lock();
+	for (i = 0; i < ARRAY_SIZE(dev->wcid); i++) {
+		wcid = rcu_dereference(dev->wcid[i]);
+		if (!wcid)
+			continue;
+
+		msta = container_of(wcid, struct mt76_sta, wcid);
+		sta = container_of(msta, struct ieee80211_sta, drv_priv);
+
+		min_factor = min(min_factor, sta->ht_cap.ampdu_factor);
+	}
+	rcu_read_unlock();
+
+	mt7601u_wr(dev, MT_MAX_LEN_CFG, 0xa0fff |
+		   MT76_SET(MT_MAX_LEN_CFG_AMPDU, min_factor));
+}
+
+static void
+mt76_mac_process_rate(struct ieee80211_rx_status *status, u16 rate)
+{
+	u8 idx = MT76_GET(MT_RXWI_RATE_MCS, rate);
+
+	switch (MT76_GET(MT_RXWI_RATE_PHY, rate)) {
+	case MT_PHY_TYPE_OFDM:
+		if (WARN_ON(idx >= 8))
+			idx = 0;
+		idx += 4;
+
+		status->rate_idx = idx;
+		return;
+	case MT_PHY_TYPE_CCK:
+		if (idx >= 8) {
+			idx -= 8;
+			status->flag |= RX_FLAG_SHORTPRE;
+		}
+
+		if (WARN_ON(idx >= 4))
+			idx = 0;
+
+		status->rate_idx = idx;
+		return;
+	case MT_PHY_TYPE_HT_GF:
+		status->flag |= RX_FLAG_HT_GF;
+		/* fall through */
+	case MT_PHY_TYPE_HT:
+		status->flag |= RX_FLAG_HT;
+		status->rate_idx = idx;
+		break;
+	default:
+		WARN_ON(1);
+		return;
+	}
+
+	if (rate & MT_RXWI_RATE_SGI)
+		status->flag |= RX_FLAG_SHORT_GI;
+
+	if (rate & MT_RXWI_RATE_STBC)
+		status->flag |= 1 << RX_FLAG_STBC_SHIFT;
+
+	if (rate & MT_RXWI_RATE_BW)
+		status->flag |= RX_FLAG_40MHZ;
+}
+
+static void
+mt7601u_rx_monitor_beacon(struct mt7601u_dev *dev, struct mt7601u_rxwi *rxwi,
+			  u16 rate, int rssi)
+{
+	dev->bcn_freq_off = rxwi->freq_off;
+	dev->bcn_phy_mode = MT76_GET(MT_RXWI_RATE_PHY, rate);
+	dev->avg_rssi = (dev->avg_rssi * 15) / 16 + (rssi << 8);
+}
+
+static int
+mt7601u_rx_is_our_beacon(struct mt7601u_dev *dev, u8 *data)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)data;
+
+	return ieee80211_is_beacon(hdr->frame_control) &&
+		ether_addr_equal(hdr->addr2, dev->ap_bssid);
+}
+
+u32 mt76_mac_process_rx(struct mt7601u_dev *dev, struct sk_buff *skb,
+			u8 *data, void *rxi)
+{
+	struct ieee80211_rx_status *status = IEEE80211_SKB_RXCB(skb);
+	struct mt7601u_rxwi *rxwi = rxi;
+	u32 len, ctl = le32_to_cpu(rxwi->ctl);
+	u16 rate = le16_to_cpu(rxwi->rate);
+	int rssi;
+
+	len = MT76_GET(MT_RXWI_CTL_MPDU_LEN, ctl);
+	if (len < 10)
+		return 0;
+
+	if (rxwi->rxinfo & cpu_to_le32(MT_RXINFO_DECRYPT)) {
+		status->flag |= RX_FLAG_DECRYPTED;
+		status->flag |= RX_FLAG_IV_STRIPPED | RX_FLAG_MMIC_STRIPPED;
+	}
+
+	status->chains = BIT(0);
+	rssi = mt7601u_phy_get_rssi(dev, rxwi, rate);
+	status->chain_signal[0] = status->signal = rssi;
+	status->freq = dev->chandef.chan->center_freq;
+	status->band = dev->chandef.chan->band;
+
+	mt76_mac_process_rate(status, rate);
+
+	spin_lock_bh(&dev->con_mon_lock);
+	if (mt7601u_rx_is_our_beacon(dev, data))
+		mt7601u_rx_monitor_beacon(dev, rxwi, rate, rssi);
+	else if (rxwi->rxinfo & cpu_to_le32(MT_RXINFO_U2M))
+		dev->avg_rssi = (dev->avg_rssi * 15) / 16 + (rssi << 8);
+	spin_unlock_bh(&dev->con_mon_lock);
+
+	return len;
+}
+
+static enum mt76_cipher_type
+mt76_mac_get_key_info(struct ieee80211_key_conf *key, u8 *key_data)
+{
+	memset(key_data, 0, 32);
+	if (!key)
+		return MT_CIPHER_NONE;
+
+	if (key->keylen > 32)
+		return MT_CIPHER_NONE;
+
+	memcpy(key_data, key->key, key->keylen);
+
+	switch (key->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+		return MT_CIPHER_WEP40;
+	case WLAN_CIPHER_SUITE_WEP104:
+		return MT_CIPHER_WEP104;
+	case WLAN_CIPHER_SUITE_TKIP:
+		return MT_CIPHER_TKIP;
+	case WLAN_CIPHER_SUITE_CCMP:
+		return MT_CIPHER_AES_CCMP;
+	default:
+		return MT_CIPHER_NONE;
+	}
+}
+
+int mt76_mac_wcid_set_key(struct mt7601u_dev *dev, u8 idx,
+			  struct ieee80211_key_conf *key)
+{
+	enum mt76_cipher_type cipher;
+	u8 key_data[32];
+	u8 iv_data[8];
+	u32 val;
+
+	cipher = mt76_mac_get_key_info(key, key_data);
+	if (cipher == MT_CIPHER_NONE && key)
+		return -EINVAL;
+
+	trace_set_key(dev, idx);
+
+	mt7601u_wr_copy(dev, MT_WCID_KEY(idx), key_data, sizeof(key_data));
+
+	memset(iv_data, 0, sizeof(iv_data));
+	if (key) {
+		iv_data[3] = key->keyidx << 6;
+		if (cipher >= MT_CIPHER_TKIP) {
+			/* Note: start with 1 to comply with spec,
+			 *	 (see comment on common/cmm_wpa.c:4291).
+			 */
+			iv_data[0] |= 1;
+			iv_data[3] |= 0x20;
+		}
+	}
+	mt7601u_wr_copy(dev, MT_WCID_IV(idx), iv_data, sizeof(iv_data));
+
+	val = mt7601u_rr(dev, MT_WCID_ATTR(idx));
+	val &= ~MT_WCID_ATTR_PKEY_MODE & ~MT_WCID_ATTR_PKEY_MODE_EXT;
+	val |= MT76_SET(MT_WCID_ATTR_PKEY_MODE, cipher & 7) |
+	       MT76_SET(MT_WCID_ATTR_PKEY_MODE_EXT, cipher >> 3);
+	val &= ~MT_WCID_ATTR_PAIRWISE;
+	val |= MT_WCID_ATTR_PAIRWISE *
+		!!(key && key->flags & IEEE80211_KEY_FLAG_PAIRWISE);
+	mt7601u_wr(dev, MT_WCID_ATTR(idx), val);
+
+	return 0;
+}
+
+int mt76_mac_shared_key_setup(struct mt7601u_dev *dev, u8 vif_idx, u8 key_idx,
+			      struct ieee80211_key_conf *key)
+{
+	enum mt76_cipher_type cipher;
+	u8 key_data[32];
+	u32 val;
+
+	cipher = mt76_mac_get_key_info(key, key_data);
+	if (cipher == MT_CIPHER_NONE && key)
+		return -EINVAL;
+
+	trace_set_shared_key(dev, vif_idx, key_idx);
+
+	mt7601u_wr_copy(dev, MT_SKEY(vif_idx, key_idx),
+			key_data, sizeof(key_data));
+
+	val = mt76_rr(dev, MT_SKEY_MODE(vif_idx));
+	val &= ~(MT_SKEY_MODE_MASK << MT_SKEY_MODE_SHIFT(vif_idx, key_idx));
+	val |= cipher << MT_SKEY_MODE_SHIFT(vif_idx, key_idx);
+	mt76_wr(dev, MT_SKEY_MODE(vif_idx), val);
+
+	return 0;
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/mac.h b/drivers/net/wireless/mediatek/mt7601u/mac.h
new file mode 100644
index 000000000000..2c22d63c63a2
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/mac.h
@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef __MT76_MAC_H
+#define __MT76_MAC_H
+
+struct mt76_tx_status {
+	u8 valid:1;
+	u8 success:1;
+	u8 aggr:1;
+	u8 ack_req:1;
+	u8 is_probe:1;
+	u8 wcid;
+	u8 pktid;
+	u8 retry;
+	u16 rate;
+} __packed __aligned(2);
+
+/* Note: values in original "RSSI" and "SNR" fields are not actually what they
+ *	 are called for MT7601U, names used by this driver are educated guesses
+ *	 (see vendor mac/ral_omac.c).
+ */
+struct mt7601u_rxwi {
+	__le32 rxinfo;
+
+	__le32 ctl;
+
+	__le16 frag_sn;
+	__le16 rate;
+
+	u8 unknown;
+	u8 zero[3];
+
+	u8 snr;
+	u8 ant;
+	u8 gain;
+	u8 freq_off;
+
+	__le32 resv2;
+	__le32 expert_ant;
+} __packed __aligned(4);
+
+#define MT_RXINFO_BA			BIT(0)
+#define MT_RXINFO_DATA			BIT(1)
+#define MT_RXINFO_NULL			BIT(2)
+#define MT_RXINFO_FRAG			BIT(3)
+#define MT_RXINFO_U2M			BIT(4)
+#define MT_RXINFO_MULTICAST		BIT(5)
+#define MT_RXINFO_BROADCAST		BIT(6)
+#define MT_RXINFO_MYBSS			BIT(7)
+#define MT_RXINFO_CRCERR		BIT(8)
+#define MT_RXINFO_ICVERR		BIT(9)
+#define MT_RXINFO_MICERR		BIT(10)
+#define MT_RXINFO_AMSDU			BIT(11)
+#define MT_RXINFO_HTC			BIT(12)
+#define MT_RXINFO_RSSI			BIT(13)
+#define MT_RXINFO_L2PAD			BIT(14)
+#define MT_RXINFO_AMPDU			BIT(15)
+#define MT_RXINFO_DECRYPT		BIT(16)
+#define MT_RXINFO_BSSIDX3		BIT(17)
+#define MT_RXINFO_WAPI_KEY		BIT(18)
+#define MT_RXINFO_PN_LEN		GENMASK(21, 19)
+#define MT_RXINFO_SW_PKT_80211		BIT(22)
+#define MT_RXINFO_TCP_SUM_BYPASS	BIT(28)
+#define MT_RXINFO_IP_SUM_BYPASS		BIT(29)
+#define MT_RXINFO_TCP_SUM_ERR		BIT(30)
+#define MT_RXINFO_IP_SUM_ERR		BIT(31)
+
+#define MT_RXWI_CTL_WCID		GENMASK(7, 0)
+#define MT_RXWI_CTL_KEY_IDX		GENMASK(9, 8)
+#define MT_RXWI_CTL_BSS_IDX		GENMASK(12, 10)
+#define MT_RXWI_CTL_UDF			GENMASK(15, 13)
+#define MT_RXWI_CTL_MPDU_LEN		GENMASK(27, 16)
+#define MT_RXWI_CTL_TID			GENMASK(31, 28)
+
+#define MT_RXWI_FRAG			GENMASK(3, 0)
+#define MT_RXWI_SN			GENMASK(15, 4)
+
+#define MT_RXWI_RATE_MCS		GENMASK(6, 0)
+#define MT_RXWI_RATE_BW			BIT(7)
+#define MT_RXWI_RATE_SGI		BIT(8)
+#define MT_RXWI_RATE_STBC		GENMASK(10, 9)
+#define MT_RXWI_RATE_ETXBF		BIT(11)
+#define MT_RXWI_RATE_SND		BIT(12)
+#define MT_RXWI_RATE_ITXBF		BIT(13)
+#define MT_RXWI_RATE_PHY		GENMASK(15, 14)
+
+#define MT_RXWI_GAIN_RSSI_VAL		GENMASK(5, 0)
+#define MT_RXWI_GAIN_RSSI_LNA_ID	GENMASK(7, 6)
+#define MT_RXWI_ANT_AUX_LNA		BIT(7)
+
+#define MT_RXWI_EANT_ENC_ANT_ID		GENMASK(7, 0)
+
+enum mt76_phy_type {
+	MT_PHY_TYPE_CCK,
+	MT_PHY_TYPE_OFDM,
+	MT_PHY_TYPE_HT,
+	MT_PHY_TYPE_HT_GF,
+};
+
+enum mt76_phy_bandwidth {
+	MT_PHY_BW_20,
+	MT_PHY_BW_40,
+};
+
+struct mt76_txwi {
+	__le16 flags;
+	__le16 rate_ctl;
+
+	u8 ack_ctl;
+	u8 wcid;
+	__le16 len_ctl;
+
+	__le32 iv;
+
+	__le32 eiv;
+
+	u8 aid;
+	u8 txstream;
+	__le16 ctl;
+} __packed __aligned(4);
+
+#define MT_TXWI_FLAGS_FRAG		BIT(0)
+#define MT_TXWI_FLAGS_MMPS		BIT(1)
+#define MT_TXWI_FLAGS_CFACK		BIT(2)
+#define MT_TXWI_FLAGS_TS		BIT(3)
+#define MT_TXWI_FLAGS_AMPDU		BIT(4)
+#define MT_TXWI_FLAGS_MPDU_DENSITY	GENMASK(7, 5)
+#define MT_TXWI_FLAGS_TXOP		GENMASK(9, 8)
+#define MT_TXWI_FLAGS_CWMIN		GENMASK(12, 10)
+#define MT_TXWI_FLAGS_NO_RATE_FALLBACK	BIT(13)
+#define MT_TXWI_FLAGS_TX_RPT		BIT(14)
+#define MT_TXWI_FLAGS_TX_RATE_LUT	BIT(15)
+
+#define MT_TXWI_RATE_MCS		GENMASK(6, 0)
+#define MT_TXWI_RATE_BW			BIT(7)
+#define MT_TXWI_RATE_SGI		BIT(8)
+#define MT_TXWI_RATE_STBC		GENMASK(10, 9)
+#define MT_TXWI_RATE_PHY_MODE		GENMASK(15, 14)
+
+#define MT_TXWI_ACK_CTL_REQ		BIT(0)
+#define MT_TXWI_ACK_CTL_NSEQ		BIT(1)
+#define MT_TXWI_ACK_CTL_BA_WINDOW	GENMASK(7, 2)
+
+#define MT_TXWI_LEN_BYTE_CNT		GENMASK(11, 0)
+#define MT_TXWI_LEN_PKTID		GENMASK(15, 12)
+
+#define MT_TXWI_CTL_TX_POWER_ADJ	GENMASK(3, 0)
+#define MT_TXWI_CTL_CHAN_CHECK_PKT	BIT(4)
+#define MT_TXWI_CTL_PIFS_REV		BIT(6)
+
+u32 mt76_mac_process_rx(struct mt7601u_dev *dev, struct sk_buff *skb,
+			u8 *data, void *rxi);
+int mt76_mac_wcid_set_key(struct mt7601u_dev *dev, u8 idx,
+			  struct ieee80211_key_conf *key);
+void mt76_mac_wcid_set_rate(struct mt7601u_dev *dev, struct mt76_wcid *wcid,
+			    const struct ieee80211_tx_rate *rate);
+
+int mt76_mac_shared_key_setup(struct mt7601u_dev *dev, u8 vif_idx, u8 key_idx,
+			      struct ieee80211_key_conf *key);
+u16 mt76_mac_tx_rate_val(struct mt7601u_dev *dev,
+			 const struct ieee80211_tx_rate *rate, u8 *nss_val);
+struct mt76_tx_status
+mt7601u_mac_fetch_tx_status(struct mt7601u_dev *dev);
+void mt76_send_tx_status(struct mt7601u_dev *dev, struct mt76_tx_status *stat);
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/main.c b/drivers/net/wireless/mediatek/mt7601u/main.c
new file mode 100644
index 000000000000..169384b48b27
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/main.c
@@ -0,0 +1,413 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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 "mt7601u.h"
+#include "mac.h"
+#include <linux/etherdevice.h>
+#include <linux/version.h>
+
+static int mt7601u_start(struct ieee80211_hw *hw)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	int ret;
+
+	mutex_lock(&dev->mutex);
+
+	ret = mt7601u_mac_start(dev);
+	if (ret)
+		goto out;
+
+	ieee80211_queue_delayed_work(dev->hw, &dev->mac_work,
+				     MT_CALIBRATE_INTERVAL);
+	ieee80211_queue_delayed_work(dev->hw, &dev->cal_work,
+				     MT_CALIBRATE_INTERVAL);
+out:
+	mutex_unlock(&dev->mutex);
+	return ret;
+}
+
+static void mt7601u_stop(struct ieee80211_hw *hw)
+{
+	struct mt7601u_dev *dev = hw->priv;
+
+	mutex_lock(&dev->mutex);
+
+	cancel_delayed_work_sync(&dev->cal_work);
+	cancel_delayed_work_sync(&dev->mac_work);
+	mt7601u_mac_stop(dev);
+
+	mutex_unlock(&dev->mutex);
+}
+
+static int mt7601u_add_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	struct mt76_vif *mvif = (struct mt76_vif *) vif->drv_priv;
+	unsigned int idx = 0;
+	unsigned int wcid = GROUP_WCID(idx);
+
+	/* Note: for AP do the AP-STA things mt76 does:
+	 *	- beacon offsets
+	 *	- do mac address tricks
+	 *	- shift vif idx
+	 */
+	mvif->idx = idx;
+
+	if (dev->wcid_mask[wcid / BITS_PER_LONG] & BIT(wcid % BITS_PER_LONG))
+		return -ENOSPC;
+	dev->wcid_mask[wcid / BITS_PER_LONG] |= BIT(wcid % BITS_PER_LONG);
+	mvif->group_wcid.idx = wcid;
+	mvif->group_wcid.hw_key_idx = -1;
+
+	return 0;
+}
+
+static void mt7601u_remove_interface(struct ieee80211_hw *hw,
+				     struct ieee80211_vif *vif)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	struct mt76_vif *mvif = (struct mt76_vif *) vif->drv_priv;
+	unsigned int wcid = mvif->group_wcid.idx;
+
+	dev->wcid_mask[wcid / BITS_PER_LONG] &= ~BIT(wcid % BITS_PER_LONG);
+}
+
+static int mt7601u_config(struct ieee80211_hw *hw, u32 changed)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	int ret = 0;
+
+	mutex_lock(&dev->mutex);
+
+	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+		ieee80211_stop_queues(hw);
+		ret = mt7601u_phy_set_channel(dev, &hw->conf.chandef);
+		ieee80211_wake_queues(hw);
+	}
+
+	mutex_unlock(&dev->mutex);
+
+	return ret;
+}
+
+static void
+mt76_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
+		      unsigned int *total_flags, u64 multicast)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	u32 flags = 0;
+
+#define MT76_FILTER(_flag, _hw) do { \
+		flags |= *total_flags & FIF_##_flag;			\
+		dev->rxfilter &= ~(_hw);				\
+		dev->rxfilter |= !(flags & FIF_##_flag) * (_hw);	\
+	} while (0)
+
+	mutex_lock(&dev->mutex);
+
+	dev->rxfilter &= ~MT_RX_FILTR_CFG_OTHER_BSS;
+
+	MT76_FILTER(OTHER_BSS, MT_RX_FILTR_CFG_PROMISC);
+	MT76_FILTER(FCSFAIL, MT_RX_FILTR_CFG_CRC_ERR);
+	MT76_FILTER(PLCPFAIL, MT_RX_FILTR_CFG_PHY_ERR);
+	MT76_FILTER(CONTROL, MT_RX_FILTR_CFG_ACK |
+			     MT_RX_FILTR_CFG_CTS |
+			     MT_RX_FILTR_CFG_CFEND |
+			     MT_RX_FILTR_CFG_CFACK |
+			     MT_RX_FILTR_CFG_BA |
+			     MT_RX_FILTR_CFG_CTRL_RSV);
+	MT76_FILTER(PSPOLL, MT_RX_FILTR_CFG_PSPOLL);
+
+	*total_flags = flags;
+	mt76_wr(dev, MT_RX_FILTR_CFG, dev->rxfilter);
+
+	mutex_unlock(&dev->mutex);
+}
+
+static void
+mt7601u_bss_info_changed(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			 struct ieee80211_bss_conf *info, u32 changed)
+{
+	struct mt7601u_dev *dev = hw->priv;
+
+	mutex_lock(&dev->mutex);
+
+	if (changed & BSS_CHANGED_ASSOC)
+		mt7601u_phy_con_cal_onoff(dev, info);
+
+	if (changed & BSS_CHANGED_BSSID) {
+		mt7601u_addr_wr(dev, MT_MAC_BSSID_DW0, info->bssid);
+
+		/* Note: this is a hack because beacon_int is not changed
+		 *	 on leave nor is any more appropriate event generated.
+		 *	 rt2x00 doesn't seem to be bothered though.
+		 */
+		if (is_zero_ether_addr(info->bssid))
+			mt7601u_mac_config_tsf(dev, false, 0);
+	}
+
+	if (changed & BSS_CHANGED_BASIC_RATES) {
+		mt7601u_wr(dev, MT_LEGACY_BASIC_RATE, info->basic_rates);
+		mt7601u_wr(dev, MT_HT_FBK_CFG0, 0x65432100);
+		mt7601u_wr(dev, MT_HT_FBK_CFG1, 0xedcba980);
+		mt7601u_wr(dev, MT_LG_FBK_CFG0, 0xedcba988);
+		mt7601u_wr(dev, MT_LG_FBK_CFG1, 0x00002100);
+	}
+
+	if (changed & BSS_CHANGED_BEACON_INT)
+		mt7601u_mac_config_tsf(dev, true, info->beacon_int);
+
+	if (changed & BSS_CHANGED_HT || changed & BSS_CHANGED_ERP_CTS_PROT)
+		mt7601u_mac_set_protection(dev, info->use_cts_prot,
+					   info->ht_operation_mode);
+
+	if (changed & BSS_CHANGED_ERP_PREAMBLE)
+		mt7601u_mac_set_short_preamble(dev, info->use_short_preamble);
+
+	if (changed & BSS_CHANGED_ERP_SLOT) {
+		int slottime = info->use_short_slot ? 9 : 20;
+
+		mt76_rmw_field(dev, MT_BKOFF_SLOT_CFG,
+			       MT_BKOFF_SLOT_CFG_SLOTTIME, slottime);
+	}
+
+	if (changed & BSS_CHANGED_ASSOC)
+		mt7601u_phy_recalibrate_after_assoc(dev);
+
+	mutex_unlock(&dev->mutex);
+}
+
+static int
+mt76_wcid_alloc(struct mt7601u_dev *dev)
+{
+	int i, idx = 0;
+
+	for (i = 0; i < ARRAY_SIZE(dev->wcid_mask); i++) {
+		idx = ffs(~dev->wcid_mask[i]);
+		if (!idx)
+			continue;
+
+		idx--;
+		dev->wcid_mask[i] |= BIT(idx);
+		break;
+	}
+
+	idx = i * BITS_PER_LONG + idx;
+	if (idx > 119)
+		return -1;
+
+	return idx;
+}
+
+static int
+mt7601u_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		struct ieee80211_sta *sta)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	struct mt76_sta *msta = (struct mt76_sta *) sta->drv_priv;
+	struct mt76_vif *mvif = (struct mt76_vif *) vif->drv_priv;
+	int ret = 0;
+	int idx = 0;
+
+	mutex_lock(&dev->mutex);
+
+	idx = mt76_wcid_alloc(dev);
+	if (idx < 0) {
+		ret = -ENOSPC;
+		goto out;
+	}
+
+	msta->wcid.idx = idx;
+	msta->wcid.hw_key_idx = -1;
+	mt7601u_mac_wcid_setup(dev, idx, mvif->idx, sta->addr);
+	mt76_clear(dev, MT_WCID_DROP(idx), MT_WCID_DROP_MASK(idx));
+	rcu_assign_pointer(dev->wcid[idx], &msta->wcid);
+	mt7601u_mac_set_ampdu_factor(dev);
+
+out:
+	mutex_unlock(&dev->mutex);
+
+	return ret;
+}
+
+static int
+mt7601u_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		   struct ieee80211_sta *sta)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	struct mt76_sta *msta = (struct mt76_sta *) sta->drv_priv;
+	int idx = msta->wcid.idx;
+
+	mutex_lock(&dev->mutex);
+	rcu_assign_pointer(dev->wcid[idx], NULL);
+	mt76_set(dev, MT_WCID_DROP(idx), MT_WCID_DROP_MASK(idx));
+	dev->wcid_mask[idx / BITS_PER_LONG] &= ~BIT(idx % BITS_PER_LONG);
+	mt7601u_mac_wcid_setup(dev, idx, 0, NULL);
+	mt7601u_mac_set_ampdu_factor(dev);
+	mutex_unlock(&dev->mutex);
+
+	return 0;
+}
+
+static void
+mt7601u_sta_notify(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		   enum sta_notify_cmd cmd, struct ieee80211_sta *sta)
+{
+}
+
+static void
+mt7601u_sw_scan(struct ieee80211_hw *hw,
+		struct ieee80211_vif *vif,
+		const u8 *mac_addr)
+{
+	struct mt7601u_dev *dev = hw->priv;
+
+	mt7601u_agc_save(dev);
+	set_bit(MT7601U_STATE_SCANNING, &dev->state);
+}
+
+static void
+mt7601u_sw_scan_complete(struct ieee80211_hw *hw,
+			 struct ieee80211_vif *vif)
+{
+	struct mt7601u_dev *dev = hw->priv;
+
+	mt7601u_agc_restore(dev);
+	clear_bit(MT7601U_STATE_SCANNING, &dev->state);
+}
+
+static int
+mt7601u_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 mt7601u_dev *dev = hw->priv;
+	struct mt76_vif *mvif = (struct mt76_vif *) vif->drv_priv;
+	struct mt76_sta *msta = sta ? (struct mt76_sta *) sta->drv_priv : NULL;
+	struct mt76_wcid *wcid = msta ? &msta->wcid : &mvif->group_wcid;
+	int idx = key->keyidx;
+	int ret;
+
+	if (cmd == SET_KEY) {
+		key->hw_key_idx = wcid->idx;
+		wcid->hw_key_idx = idx;
+	} else {
+		if (idx == wcid->hw_key_idx)
+			wcid->hw_key_idx = -1;
+
+		key = NULL;
+	}
+
+	if (!msta) {
+		if (key || wcid->hw_key_idx == idx) {
+			ret = mt76_mac_wcid_set_key(dev, wcid->idx, key);
+			if (ret)
+				return ret;
+		}
+
+		return mt76_mac_shared_key_setup(dev, mvif->idx, idx, key);
+	}
+
+	return mt76_mac_wcid_set_key(dev, msta->wcid.idx, key);
+}
+
+static int mt7601u_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
+{
+	struct mt7601u_dev *dev = hw->priv;
+
+	mt76_rmw_field(dev, MT_TX_RTS_CFG, MT_TX_RTS_CFG_THRESH, value);
+
+	return 0;
+}
+
+static int
+mt76_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		  enum ieee80211_ampdu_mlme_action action,
+		  struct ieee80211_sta *sta, u16 tid, u16 *ssn, u8 buf_size)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	struct mt76_sta *msta = (struct mt76_sta *) sta->drv_priv;
+
+	WARN_ON(msta->wcid.idx > GROUP_WCID(0));
+
+	switch (action) {
+	case IEEE80211_AMPDU_RX_START:
+		mt76_set(dev, MT_WCID_ADDR(msta->wcid.idx) + 4, BIT(16 + tid));
+		break;
+	case IEEE80211_AMPDU_RX_STOP:
+		mt76_clear(dev, MT_WCID_ADDR(msta->wcid.idx) + 4,
+			   BIT(16 + tid));
+		break;
+	case IEEE80211_AMPDU_TX_OPERATIONAL:
+		ieee80211_send_bar(vif, sta->addr, tid, msta->agg_ssn[tid]);
+		break;
+	case IEEE80211_AMPDU_TX_STOP_FLUSH:
+	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
+		break;
+	case IEEE80211_AMPDU_TX_START:
+		msta->agg_ssn[tid] = *ssn << 4;
+		ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+		break;
+	case IEEE80211_AMPDU_TX_STOP_CONT:
+		ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+		break;
+	}
+
+	return 0;
+}
+
+static void
+mt76_sta_rate_tbl_update(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			 struct ieee80211_sta *sta)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	struct mt76_sta *msta = (struct mt76_sta *) sta->drv_priv;
+	struct ieee80211_sta_rates *rates;
+	struct ieee80211_tx_rate rate = {};
+
+	rcu_read_lock();
+	rates = rcu_dereference(sta->rates);
+
+	if (!rates)
+		goto out;
+
+	rate.idx = rates->rate[0].idx;
+	rate.flags = rates->rate[0].flags;
+	mt76_mac_wcid_set_rate(dev, &msta->wcid, &rate);
+
+out:
+	rcu_read_unlock();
+}
+
+const struct ieee80211_ops mt7601u_ops = {
+	.tx = mt7601u_tx,
+	.start = mt7601u_start,
+	.stop = mt7601u_stop,
+	.add_interface = mt7601u_add_interface,
+	.remove_interface = mt7601u_remove_interface,
+	.config = mt7601u_config,
+	.configure_filter = mt76_configure_filter,
+	.bss_info_changed = mt7601u_bss_info_changed,
+	.sta_add = mt7601u_sta_add,
+	.sta_remove = mt7601u_sta_remove,
+	.sta_notify = mt7601u_sta_notify,
+	.set_key = mt7601u_set_key,
+	.conf_tx = mt7601u_conf_tx,
+	.sw_scan_start = mt7601u_sw_scan,
+	.sw_scan_complete = mt7601u_sw_scan_complete,
+	.ampdu_action = mt76_ampdu_action,
+	.sta_rate_tbl_update = mt76_sta_rate_tbl_update,
+	.set_rts_threshold = mt7601u_set_rts_threshold,
+};
diff --git a/drivers/net/wireless/mediatek/mt7601u/mcu.c b/drivers/net/wireless/mediatek/mt7601u/mcu.c
new file mode 100644
index 000000000000..fbb1986eda3c
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/mcu.c
@@ -0,0 +1,534 @@
+/*
+ * (c) Copyright 2002-2010, Ralink Technology, Inc.
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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/kernel.h>
+#include <linux/firmware.h>
+#include <linux/delay.h>
+#include <linux/usb.h>
+#include <linux/skbuff.h>
+
+#include "mt7601u.h"
+#include "dma.h"
+#include "mcu.h"
+#include "usb.h"
+#include "trace.h"
+
+#define MCU_FW_URB_MAX_PAYLOAD		0x3800
+#define MCU_FW_URB_SIZE			(MCU_FW_URB_MAX_PAYLOAD + 12)
+#define MCU_RESP_URB_SIZE		1024
+
+static inline int firmware_running(struct mt7601u_dev *dev)
+{
+	return mt7601u_rr(dev, MT_MCU_COM_REG0) == 1;
+}
+
+static inline void skb_put_le32(struct sk_buff *skb, u32 val)
+{
+	put_unaligned_le32(val, skb_put(skb, 4));
+}
+
+static inline void mt7601u_dma_skb_wrap_cmd(struct sk_buff *skb,
+					    u8 seq, enum mcu_cmd cmd)
+{
+	WARN_ON(mt7601u_dma_skb_wrap(skb, CPU_TX_PORT, DMA_COMMAND,
+				     MT76_SET(MT_TXD_CMD_INFO_SEQ, seq) |
+				     MT76_SET(MT_TXD_CMD_INFO_TYPE, cmd)));
+}
+
+static inline void trace_mt_mcu_msg_send_cs(struct mt7601u_dev *dev,
+					    struct sk_buff *skb, bool need_resp)
+{
+	u32 i, csum = 0;
+
+	for (i = 0; i < skb->len / 4; i++)
+		csum ^= get_unaligned_le32(skb->data + i * 4);
+
+	trace_mt_mcu_msg_send(dev, skb, csum, need_resp);
+}
+
+static struct sk_buff *
+mt7601u_mcu_msg_alloc(struct mt7601u_dev *dev, const void *data, int len)
+{
+	struct sk_buff *skb;
+
+	WARN_ON(len % 4); /* if length is not divisible by 4 we need to pad */
+
+	skb = alloc_skb(len + MT_DMA_HDR_LEN + 4, GFP_KERNEL);
+	skb_reserve(skb, MT_DMA_HDR_LEN);
+	memcpy(skb_put(skb, len), data, len);
+
+	return skb;
+}
+
+static int mt7601u_mcu_wait_resp(struct mt7601u_dev *dev, u8 seq)
+{
+	struct urb *urb = dev->mcu.resp.urb;
+	u32 rxfce;
+	int urb_status, ret, i = 5;
+
+	while (i--) {
+		if (!wait_for_completion_timeout(&dev->mcu.resp_cmpl,
+						 msecs_to_jiffies(300))) {
+			dev_warn(dev->dev, "Warning: %s retrying\n", __func__);
+			continue;
+		}
+
+		/* Make copies of important data before reusing the urb */
+		rxfce = get_unaligned_le32(dev->mcu.resp.buf);
+		urb_status = urb->status * mt7601u_urb_has_error(urb);
+
+		ret = mt7601u_usb_submit_buf(dev, USB_DIR_IN, MT_EP_IN_CMD_RESP,
+					     &dev->mcu.resp, GFP_KERNEL,
+					     mt7601u_complete_urb,
+					     &dev->mcu.resp_cmpl);
+		if (ret)
+			return ret;
+
+		if (urb_status)
+			dev_err(dev->dev, "Error: MCU resp urb failed:%d\n",
+				urb_status);
+
+		if (MT76_GET(MT_RXD_CMD_INFO_CMD_SEQ, rxfce) == seq &&
+		    MT76_GET(MT_RXD_CMD_INFO_EVT_TYPE, rxfce) == CMD_DONE)
+			return 0;
+
+		dev_err(dev->dev, "Error: MCU resp evt:%hhx seq:%hhx-%hhx!\n",
+			MT76_GET(MT_RXD_CMD_INFO_EVT_TYPE, rxfce),
+			seq, MT76_GET(MT_RXD_CMD_INFO_CMD_SEQ, rxfce));
+	}
+
+	dev_err(dev->dev, "Error: %s timed out\n", __func__);
+	return -ETIMEDOUT;
+}
+
+static int
+mt7601u_mcu_msg_send(struct mt7601u_dev *dev, struct sk_buff *skb,
+		     enum mcu_cmd cmd, bool wait_resp)
+{
+	struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
+	unsigned cmd_pipe = usb_sndbulkpipe(usb_dev,
+					    dev->out_eps[MT_EP_OUT_INBAND_CMD]);
+	int sent, ret;
+	u8 seq = 0;
+
+	if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
+		return 0;
+
+	mutex_lock(&dev->mcu.mutex);
+
+	if (wait_resp)
+		while (!seq)
+			seq = ++dev->mcu.msg_seq & 0xf;
+
+	mt7601u_dma_skb_wrap_cmd(skb, seq, cmd);
+
+	if (dev->mcu.resp_cmpl.done)
+		dev_err(dev->dev, "Error: MCU response pre-completed!\n");
+
+	trace_mt_mcu_msg_send_cs(dev, skb, wait_resp);
+	trace_mt_submit_urb_sync(dev, cmd_pipe, skb->len);
+	ret = usb_bulk_msg(usb_dev, cmd_pipe, skb->data, skb->len, &sent, 500);
+	if (ret) {
+		dev_err(dev->dev, "Error: send MCU cmd failed:%d\n", ret);
+		goto out;
+	}
+	if (sent != skb->len)
+		dev_err(dev->dev, "Error: %s sent != skb->len\n", __func__);
+
+	if (wait_resp)
+		ret = mt7601u_mcu_wait_resp(dev, seq);
+out:
+	mutex_unlock(&dev->mcu.mutex);
+
+	consume_skb(skb);
+
+	return ret;
+}
+
+static int mt7601u_mcu_function_select(struct mt7601u_dev *dev,
+				       enum mcu_function func, u32 val)
+{
+	struct sk_buff *skb;
+	struct {
+		__le32 id;
+		__le32 value;
+	} __packed __aligned(4) msg = {
+		.id = cpu_to_le32(func),
+		.value = cpu_to_le32(val),
+	};
+
+	skb = mt7601u_mcu_msg_alloc(dev, &msg, sizeof(msg));
+	return mt7601u_mcu_msg_send(dev, skb, CMD_FUN_SET_OP, func == 5);
+}
+
+int mt7601u_mcu_tssi_read_kick(struct mt7601u_dev *dev, int use_hvga)
+{
+	int ret;
+
+	if (!test_bit(MT7601U_STATE_MCU_RUNNING, &dev->state))
+		return 0;
+
+	ret = mt7601u_mcu_function_select(dev, ATOMIC_TSSI_SETTING,
+					  use_hvga);
+	if (ret) {
+		dev_warn(dev->dev, "Warning: MCU TSSI read kick failed\n");
+		return ret;
+	}
+
+	dev->tssi_read_trig = true;
+
+	return 0;
+}
+
+int
+mt7601u_mcu_calibrate(struct mt7601u_dev *dev, enum mcu_calibrate cal, u32 val)
+{
+	struct sk_buff *skb;
+	struct {
+		__le32 id;
+		__le32 value;
+	} __packed __aligned(4) msg = {
+		.id = cpu_to_le32(cal),
+		.value = cpu_to_le32(val),
+	};
+
+	skb = mt7601u_mcu_msg_alloc(dev, &msg, sizeof(msg));
+	return mt7601u_mcu_msg_send(dev, skb, CMD_CALIBRATION_OP, true);
+}
+
+int mt7601u_write_reg_pairs(struct mt7601u_dev *dev, u32 base,
+			    const struct mt76_reg_pair *data, int n)
+{
+	const int max_vals_per_cmd = INBAND_PACKET_MAX_LEN / 8;
+	struct sk_buff *skb;
+	int cnt, i, ret;
+
+	if (!n)
+		return 0;
+
+	cnt = min(max_vals_per_cmd, n);
+
+	skb = alloc_skb(cnt * 8 + MT_DMA_HDR_LEN + 4, GFP_KERNEL);
+	if (!skb)
+		return -ENOMEM;
+	skb_reserve(skb, MT_DMA_HDR_LEN);
+
+	for (i = 0; i < cnt; i++) {
+		skb_put_le32(skb, base + data[i].reg);
+		skb_put_le32(skb, data[i].value);
+	}
+
+	ret = mt7601u_mcu_msg_send(dev, skb, CMD_RANDOM_WRITE, cnt == n);
+	if (ret)
+		return ret;
+
+	return mt7601u_write_reg_pairs(dev, base, data + cnt, n - cnt);
+}
+
+int mt7601u_burst_write_regs(struct mt7601u_dev *dev, u32 offset,
+			     const u32 *data, int n)
+{
+	const int max_regs_per_cmd = INBAND_PACKET_MAX_LEN / 4 - 1;
+	struct sk_buff *skb;
+	int cnt, i, ret;
+
+	if (!n)
+		return 0;
+
+	cnt = min(max_regs_per_cmd, n);
+
+	skb = alloc_skb(cnt * 4 + MT_DMA_HDR_LEN + 4, GFP_KERNEL);
+	if (!skb)
+		return -ENOMEM;
+	skb_reserve(skb, MT_DMA_HDR_LEN);
+
+	skb_put_le32(skb, MT_MCU_MEMMAP_WLAN + offset);
+	for (i = 0; i < cnt; i++)
+		skb_put_le32(skb, data[i]);
+
+	ret = mt7601u_mcu_msg_send(dev, skb, CMD_BURST_WRITE, cnt == n);
+	if (ret)
+		return ret;
+
+	return mt7601u_burst_write_regs(dev, offset + cnt * 4,
+					data + cnt, n - cnt);
+}
+
+struct mt76_fw_header {
+	__le32 ilm_len;
+	__le32 dlm_len;
+	__le16 build_ver;
+	__le16 fw_ver;
+	u8 pad[4];
+	char build_time[16];
+};
+
+struct mt76_fw {
+	struct mt76_fw_header hdr;
+	u8 ivb[MT_MCU_IVB_SIZE];
+	u8 ilm[];
+};
+
+static int __mt7601u_dma_fw(struct mt7601u_dev *dev,
+			    const struct mt7601u_dma_buf *dma_buf,
+			    const void *data, u32 len, u32 dst_addr)
+{
+	DECLARE_COMPLETION_ONSTACK(cmpl);
+	struct mt7601u_dma_buf buf = *dma_buf; /* we need to fake length */
+	__le32 reg;
+	u32 val;
+	int ret;
+
+	reg = cpu_to_le32(MT76_SET(MT_TXD_INFO_TYPE, DMA_PACKET) |
+			  MT76_SET(MT_TXD_INFO_D_PORT, CPU_TX_PORT) |
+			  MT76_SET(MT_TXD_INFO_LEN, len));
+	memcpy(buf.buf, &reg, sizeof(reg));
+	memcpy(buf.buf + sizeof(reg), data, len);
+	memset(buf.buf + sizeof(reg) + len, 0, 8);
+
+	ret = mt7601u_vendor_single_wr(dev, MT_VEND_WRITE_FCE,
+				       MT_FCE_DMA_ADDR, dst_addr);
+	if (ret)
+		return ret;
+	len = roundup(len, 4);
+	ret = mt7601u_vendor_single_wr(dev, MT_VEND_WRITE_FCE,
+				       MT_FCE_DMA_LEN, len << 16);
+	if (ret)
+		return ret;
+
+	buf.len = MT_DMA_HDR_LEN + len + 4;
+	ret = mt7601u_usb_submit_buf(dev, USB_DIR_OUT, MT_EP_OUT_INBAND_CMD,
+				     &buf, GFP_KERNEL,
+				     mt7601u_complete_urb, &cmpl);
+	if (ret)
+		return ret;
+
+	if (!wait_for_completion_timeout(&cmpl, msecs_to_jiffies(1000))) {
+		dev_err(dev->dev, "Error: firmware upload timed out\n");
+		usb_kill_urb(buf.urb);
+		return -ETIMEDOUT;
+	}
+	if (mt7601u_urb_has_error(buf.urb)) {
+		dev_err(dev->dev, "Error: firmware upload urb failed:%d\n",
+			buf.urb->status);
+		return buf.urb->status;
+	}
+
+	val = mt7601u_rr(dev, MT_TX_CPU_FROM_FCE_CPU_DESC_IDX);
+	val++;
+	mt7601u_wr(dev, MT_TX_CPU_FROM_FCE_CPU_DESC_IDX, val);
+
+	return 0;
+}
+
+static int
+mt7601u_dma_fw(struct mt7601u_dev *dev, struct mt7601u_dma_buf *dma_buf,
+	       const void *data, int len, u32 dst_addr)
+{
+	int n, ret;
+
+	if (len == 0)
+		return 0;
+
+	n = min(MCU_FW_URB_MAX_PAYLOAD, len);
+	ret = __mt7601u_dma_fw(dev, dma_buf, data, n, dst_addr);
+	if (ret)
+		return ret;
+
+	if (!mt76_poll_msec(dev, MT_MCU_COM_REG1, BIT(31), BIT(31), 500))
+		return -ETIMEDOUT;
+
+	return mt7601u_dma_fw(dev, dma_buf, data + n, len - n, dst_addr + n);
+}
+
+static int
+mt7601u_upload_firmware(struct mt7601u_dev *dev, const struct mt76_fw *fw)
+{
+	struct mt7601u_dma_buf dma_buf;
+	void *ivb;
+	u32 ilm_len, dlm_len;
+	int i, ret;
+
+	ivb = kmemdup(fw->ivb, sizeof(fw->ivb), GFP_KERNEL);
+	if (!ivb || mt7601u_usb_alloc_buf(dev, MCU_FW_URB_SIZE, &dma_buf)) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	ilm_len = le32_to_cpu(fw->hdr.ilm_len) - sizeof(fw->ivb);
+	dev_dbg(dev->dev, "loading FW - ILM %u + IVB %zu\n",
+		ilm_len, sizeof(fw->ivb));
+	ret = mt7601u_dma_fw(dev, &dma_buf, fw->ilm, ilm_len, sizeof(fw->ivb));
+	if (ret)
+		goto error;
+
+	dlm_len = le32_to_cpu(fw->hdr.dlm_len);
+	dev_dbg(dev->dev, "loading FW - DLM %u\n", dlm_len);
+	ret = mt7601u_dma_fw(dev, &dma_buf, fw->ilm + ilm_len,
+			     dlm_len, MT_MCU_DLM_OFFSET);
+	if (ret)
+		goto error;
+
+	ret = mt7601u_vendor_request(dev, MT_VEND_DEV_MODE, USB_DIR_OUT,
+				     0x12, 0, ivb, sizeof(fw->ivb));
+	if (ret < 0)
+		goto error;
+	ret = 0;
+
+	for (i = 100; i && !firmware_running(dev); i--)
+		msleep(10);
+	if (!i) {
+		ret = -ETIMEDOUT;
+		goto error;
+	}
+
+	dev_dbg(dev->dev, "Firmware running!\n");
+error:
+	kfree(ivb);
+	mt7601u_usb_free_buf(dev, &dma_buf);
+
+	return ret;
+}
+
+static int mt7601u_load_firmware(struct mt7601u_dev *dev)
+{
+	const struct firmware *fw;
+	const struct mt76_fw_header *hdr;
+	int len, ret;
+	u32 val;
+
+	mt7601u_wr(dev, MT_USB_DMA_CFG, (MT_USB_DMA_CFG_RX_BULK_EN |
+					 MT_USB_DMA_CFG_TX_BULK_EN));
+
+	if (firmware_running(dev))
+		return 0;
+
+	ret = request_firmware(&fw, MT7601U_FIRMWARE, dev->dev);
+	if (ret)
+		return ret;
+
+	if (!fw || !fw->data || fw->size < sizeof(*hdr))
+		goto err_inv_fw;
+
+	hdr = (const struct mt76_fw_header *) fw->data;
+
+	if (le32_to_cpu(hdr->ilm_len) <= MT_MCU_IVB_SIZE)
+		goto err_inv_fw;
+
+	len = sizeof(*hdr);
+	len += le32_to_cpu(hdr->ilm_len);
+	len += le32_to_cpu(hdr->dlm_len);
+
+	if (fw->size != len)
+		goto err_inv_fw;
+
+	val = le16_to_cpu(hdr->fw_ver);
+	dev_info(dev->dev,
+		 "Firmware Version: %d.%d.%02d Build: %x Build time: %.16s\n",
+		 (val >> 12) & 0xf, (val >> 8) & 0xf, val & 0xf,
+		 le16_to_cpu(hdr->build_ver), hdr->build_time);
+
+	len = le32_to_cpu(hdr->ilm_len);
+
+	mt7601u_wr(dev, 0x94c, 0);
+	mt7601u_wr(dev, MT_FCE_PSE_CTRL, 0);
+
+	mt7601u_vendor_reset(dev);
+	msleep(5);
+
+	mt7601u_wr(dev, 0xa44, 0);
+	mt7601u_wr(dev, 0x230, 0x84210);
+	mt7601u_wr(dev, 0x400, 0x80c00);
+	mt7601u_wr(dev, 0x800, 1);
+
+	mt7601u_rmw(dev, MT_PBF_CFG, 0, (MT_PBF_CFG_TX0Q_EN |
+					 MT_PBF_CFG_TX1Q_EN |
+					 MT_PBF_CFG_TX2Q_EN |
+					 MT_PBF_CFG_TX3Q_EN));
+
+	mt7601u_wr(dev, MT_FCE_PSE_CTRL, 1);
+
+	mt7601u_wr(dev, MT_USB_DMA_CFG, (MT_USB_DMA_CFG_RX_BULK_EN |
+					 MT_USB_DMA_CFG_TX_BULK_EN));
+	val = mt76_set(dev, MT_USB_DMA_CFG, MT_USB_DMA_CFG_TX_CLR);
+	val &= ~MT_USB_DMA_CFG_TX_CLR;
+	mt7601u_wr(dev, MT_USB_DMA_CFG, val);
+
+	/* FCE tx_fs_base_ptr */
+	mt7601u_wr(dev, MT_TX_CPU_FROM_FCE_BASE_PTR, 0x400230);
+	/* FCE tx_fs_max_cnt */
+	mt7601u_wr(dev, MT_TX_CPU_FROM_FCE_MAX_COUNT, 1);
+	/* FCE pdma enable */
+	mt7601u_wr(dev, MT_FCE_PDMA_GLOBAL_CONF, 0x44);
+	/* FCE skip_fs_en */
+	mt7601u_wr(dev, MT_FCE_SKIP_FS, 3);
+
+	ret = mt7601u_upload_firmware(dev, (const struct mt76_fw *)fw->data);
+
+	release_firmware(fw);
+
+	return ret;
+
+err_inv_fw:
+	dev_err(dev->dev, "Invalid firmware image\n");
+	release_firmware(fw);
+	return -ENOENT;
+}
+
+int mt7601u_mcu_init(struct mt7601u_dev *dev)
+{
+	int ret;
+
+	mutex_init(&dev->mcu.mutex);
+
+	ret = mt7601u_load_firmware(dev);
+	if (ret)
+		return ret;
+
+	set_bit(MT7601U_STATE_MCU_RUNNING, &dev->state);
+
+	return 0;
+}
+
+int mt7601u_mcu_cmd_init(struct mt7601u_dev *dev)
+{
+	int ret;
+
+	ret = mt7601u_mcu_function_select(dev, Q_SELECT, 1);
+	if (ret)
+		return ret;
+
+	init_completion(&dev->mcu.resp_cmpl);
+	if (mt7601u_usb_alloc_buf(dev, MCU_RESP_URB_SIZE, &dev->mcu.resp)) {
+		mt7601u_usb_free_buf(dev, &dev->mcu.resp);
+		return -ENOMEM;
+	}
+
+	ret = mt7601u_usb_submit_buf(dev, USB_DIR_IN, MT_EP_IN_CMD_RESP,
+				     &dev->mcu.resp, GFP_KERNEL,
+				     mt7601u_complete_urb, &dev->mcu.resp_cmpl);
+	if (ret) {
+		mt7601u_usb_free_buf(dev, &dev->mcu.resp);
+		return ret;
+	}
+
+	return 0;
+}
+
+void mt7601u_mcu_cmd_deinit(struct mt7601u_dev *dev)
+{
+	usb_kill_urb(dev->mcu.resp.urb);
+	mt7601u_usb_free_buf(dev, &dev->mcu.resp);
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/mcu.h b/drivers/net/wireless/mediatek/mt7601u/mcu.h
new file mode 100644
index 000000000000..4a66d1092a18
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/mcu.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef __MT7601U_MCU_H
+#define __MT7601U_MCU_H
+
+struct mt7601u_dev;
+
+/* Register definitions */
+#define MT_MCU_RESET_CTL		0x070C
+#define MT_MCU_INT_LEVEL		0x0718
+#define MT_MCU_COM_REG0			0x0730
+#define MT_MCU_COM_REG1			0x0734
+#define MT_MCU_COM_REG2			0x0738
+#define MT_MCU_COM_REG3			0x073C
+
+#define MT_MCU_IVB_SIZE			0x40
+#define MT_MCU_DLM_OFFSET		0x80000
+
+#define MT_MCU_MEMMAP_WLAN		0x00410000
+#define MT_MCU_MEMMAP_BBP		0x40000000
+#define MT_MCU_MEMMAP_RF		0x80000000
+
+#define INBAND_PACKET_MAX_LEN		192
+
+enum mcu_cmd {
+	CMD_FUN_SET_OP = 1,
+	CMD_LOAD_CR = 2,
+	CMD_INIT_GAIN_OP = 3,
+	CMD_DYNC_VGA_OP = 6,
+	CMD_TDLS_CH_SW = 7,
+	CMD_BURST_WRITE = 8,
+	CMD_READ_MODIFY_WRITE = 9,
+	CMD_RANDOM_READ = 10,
+	CMD_BURST_READ = 11,
+	CMD_RANDOM_WRITE = 12,
+	CMD_LED_MODE_OP = 16,
+	CMD_POWER_SAVING_OP = 20,
+	CMD_WOW_CONFIG = 21,
+	CMD_WOW_QUERY = 22,
+	CMD_WOW_FEATURE = 24,
+	CMD_CARRIER_DETECT_OP = 28,
+	CMD_RADOR_DETECT_OP = 29,
+	CMD_SWITCH_CHANNEL_OP = 30,
+	CMD_CALIBRATION_OP = 31,
+	CMD_BEACON_OP = 32,
+	CMD_ANTENNA_OP = 33,
+};
+
+enum mcu_function {
+	Q_SELECT = 1,
+	ATOMIC_TSSI_SETTING = 5,
+};
+
+enum mcu_power_mode {
+	RADIO_OFF = 0x30,
+	RADIO_ON = 0x31,
+	RADIO_OFF_AUTO_WAKEUP = 0x32,
+	RADIO_OFF_ADVANCE = 0x33,
+	RADIO_ON_ADVANCE = 0x34,
+};
+
+enum mcu_calibrate {
+	MCU_CAL_R = 1,
+	MCU_CAL_DCOC,
+	MCU_CAL_LC,
+	MCU_CAL_LOFT,
+	MCU_CAL_TXIQ,
+	MCU_CAL_BW,
+	MCU_CAL_DPD,
+	MCU_CAL_RXIQ,
+	MCU_CAL_TXDCOC,
+};
+
+int mt7601u_mcu_init(struct mt7601u_dev *dev);
+int mt7601u_mcu_cmd_init(struct mt7601u_dev *dev);
+void mt7601u_mcu_cmd_deinit(struct mt7601u_dev *dev);
+
+int
+mt7601u_mcu_calibrate(struct mt7601u_dev *dev, enum mcu_calibrate cal, u32 val);
+int mt7601u_mcu_tssi_read_kick(struct mt7601u_dev *dev, int use_hvga);
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/mt7601u.h b/drivers/net/wireless/mediatek/mt7601u/mt7601u.h
new file mode 100644
index 000000000000..9102be6b95cb
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/mt7601u.h
@@ -0,0 +1,390 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef MT7601U_H
+#define MT7601U_H
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/usb.h>
+#include <linux/completion.h>
+#include <net/mac80211.h>
+#include <linux/debugfs.h>
+
+#include "regs.h"
+#include "util.h"
+
+#define MT_CALIBRATE_INTERVAL		(4 * HZ)
+
+#define MT_FREQ_CAL_INIT_DELAY		(30 * HZ)
+#define MT_FREQ_CAL_CHECK_INTERVAL	(10 * HZ)
+#define MT_FREQ_CAL_ADJ_INTERVAL	(HZ / 2)
+
+#define MT_BBP_REG_VERSION		0x00
+
+#define MT_USB_AGGR_SIZE_LIMIT		28 /* * 1024B */
+#define MT_USB_AGGR_TIMEOUT		0x80 /* * 33ns */
+#define MT_RX_ORDER			3
+#define MT_RX_URB_SIZE			(PAGE_SIZE << MT_RX_ORDER)
+
+struct mt7601u_dma_buf {
+	struct urb *urb;
+	void *buf;
+	dma_addr_t dma;
+	size_t len;
+};
+
+struct mt7601u_mcu {
+	struct mutex mutex;
+
+	u8 msg_seq;
+
+	struct mt7601u_dma_buf resp;
+	struct completion resp_cmpl;
+};
+
+struct mt7601u_freq_cal {
+	struct delayed_work work;
+	u8 freq;
+	bool enabled;
+	bool adjusting;
+};
+
+struct mac_stats {
+	u64 rx_stat[6];
+	u64 tx_stat[6];
+	u64 aggr_stat[2];
+	u64 aggr_n[32];
+	u64 zero_len_del[2];
+};
+
+#define N_RX_ENTRIES	16
+struct mt7601u_rx_queue {
+	struct mt7601u_dev *dev;
+
+	struct mt7601u_dma_buf_rx {
+		struct urb *urb;
+		struct page *p;
+	} e[N_RX_ENTRIES];
+
+	unsigned int start;
+	unsigned int end;
+	unsigned int entries;
+	unsigned int pending;
+};
+
+#define N_TX_ENTRIES	64
+
+struct mt7601u_tx_queue {
+	struct mt7601u_dev *dev;
+
+	struct mt7601u_dma_buf_tx {
+		struct urb *urb;
+		struct sk_buff *skb;
+	} e[N_TX_ENTRIES];
+
+	unsigned int start;
+	unsigned int end;
+	unsigned int entries;
+	unsigned int used;
+	unsigned int fifo_seq;
+};
+
+/* WCID allocation:
+ *     0: mcast wcid
+ *     1: bssid wcid
+ *  1...: STAs
+ * ...7e: group wcids
+ *    7f: reserved
+ */
+#define N_WCIDS		128
+#define GROUP_WCID(idx)	(N_WCIDS - 2 - idx)
+
+struct mt7601u_eeprom_params;
+
+#define MT_EE_TEMPERATURE_SLOPE		39
+#define MT_FREQ_OFFSET_INVALID		-128
+
+enum mt_temp_mode {
+	MT_TEMP_MODE_NORMAL,
+	MT_TEMP_MODE_HIGH,
+	MT_TEMP_MODE_LOW,
+};
+
+enum mt_bw {
+	MT_BW_20,
+	MT_BW_40,
+};
+
+enum {
+	MT7601U_STATE_INITIALIZED,
+	MT7601U_STATE_REMOVED,
+	MT7601U_STATE_WLAN_RUNNING,
+	MT7601U_STATE_MCU_RUNNING,
+	MT7601U_STATE_SCANNING,
+	MT7601U_STATE_READING_STATS,
+	MT7601U_STATE_MORE_STATS,
+};
+
+/**
+ * struct mt7601u_dev - adapter structure
+ * @lock:		protects @wcid->tx_rate.
+ * @tx_lock:		protects @tx_q and changes of MT7601U_STATE_*_STATS
+			flags in @state.
+ * @rx_lock:		protects @rx_q.
+ * @con_mon_lock:	protects @ap_bssid, @bcn_*, @avg_rssi.
+ * @mutex:		ensures exclusive access from mac80211 callbacks.
+ * @vendor_req_mutex:	ensures atomicity of vendor requests.
+ * @reg_atomic_mutex:	ensures atomicity of indirect register accesses
+ *			(accesses to RF and BBP).
+ * @hw_atomic_mutex:	ensures exclusive access to HW during critical
+ *			operations (power management, channel switch).
+ */
+struct mt7601u_dev {
+	struct ieee80211_hw *hw;
+	struct device *dev;
+
+	unsigned long state;
+
+	struct mutex mutex;
+
+	unsigned long wcid_mask[N_WCIDS / BITS_PER_LONG];
+
+	struct cfg80211_chan_def chandef;
+	struct ieee80211_supported_band *sband_2g;
+
+	struct mt7601u_mcu mcu;
+
+	struct delayed_work cal_work;
+	struct delayed_work mac_work;
+
+	struct workqueue_struct *stat_wq;
+	struct delayed_work stat_work;
+
+	struct mt76_wcid *mon_wcid;
+	struct mt76_wcid __rcu *wcid[N_WCIDS];
+
+	spinlock_t lock;
+
+	const u16 *beacon_offsets;
+
+	u8 macaddr[ETH_ALEN];
+	struct mt7601u_eeprom_params *ee;
+
+	struct mutex vendor_req_mutex;
+	struct mutex reg_atomic_mutex;
+	struct mutex hw_atomic_mutex;
+
+	u32 rxfilter;
+	u32 debugfs_reg;
+
+	u8 out_eps[8];
+	u8 in_eps[8];
+	u16 out_max_packet;
+	u16 in_max_packet;
+
+	/* TX */
+	spinlock_t tx_lock;
+	struct mt7601u_tx_queue *tx_q;
+
+	atomic_t avg_ampdu_len;
+
+	/* RX */
+	spinlock_t rx_lock;
+	struct tasklet_struct rx_tasklet;
+	struct mt7601u_rx_queue rx_q;
+
+	/* Connection monitoring things */
+	spinlock_t con_mon_lock;
+	u8 ap_bssid[ETH_ALEN];
+
+	s8 bcn_freq_off;
+	u8 bcn_phy_mode;
+
+	int avg_rssi; /* starts at 0 and converges */
+
+	u8 agc_save;
+
+	struct mt7601u_freq_cal freq_cal;
+
+	bool tssi_read_trig;
+
+	s8 tssi_init;
+	s8 tssi_init_hvga;
+	s16 tssi_init_hvga_offset_db;
+
+	int prev_pwr_diff;
+
+	enum mt_temp_mode temp_mode;
+	int curr_temp;
+	int dpd_temp;
+	s8 raw_temp;
+	bool pll_lock_protect;
+
+	u8 bw;
+	bool chan_ext_below;
+
+	/* PA mode */
+	u32 rf_pa_mode[2];
+
+	struct mac_stats stats;
+};
+
+struct mt7601u_tssi_params {
+	char tssi0;
+	int trgt_power;
+};
+
+struct mt76_wcid {
+	u8 idx;
+	u8 hw_key_idx;
+
+	u16 tx_rate;
+	bool tx_rate_set;
+	u8 tx_rate_nss;
+};
+
+struct mt76_vif {
+	u8 idx;
+
+	struct mt76_wcid group_wcid;
+};
+
+struct mt76_sta {
+	struct mt76_wcid wcid;
+	u16 agg_ssn[IEEE80211_NUM_TIDS];
+};
+
+struct mt76_reg_pair {
+	u32 reg;
+	u32 value;
+};
+
+struct mt7601u_rxwi;
+
+extern const struct ieee80211_ops mt7601u_ops;
+
+void mt7601u_init_debugfs(struct mt7601u_dev *dev);
+
+u32 mt7601u_rr(struct mt7601u_dev *dev, u32 offset);
+void mt7601u_wr(struct mt7601u_dev *dev, u32 offset, u32 val);
+u32 mt7601u_rmw(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val);
+u32 mt7601u_rmc(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val);
+void mt7601u_wr_copy(struct mt7601u_dev *dev, u32 offset,
+		     const void *data, int len);
+
+int mt7601u_wait_asic_ready(struct mt7601u_dev *dev);
+bool mt76_poll(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val,
+	       int timeout);
+bool mt76_poll_msec(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val,
+		    int timeout);
+
+/* Compatibility with mt76 */
+#define mt76_rmw_field(_dev, _reg, _field, _val)	\
+	mt76_rmw(_dev, _reg, _field, MT76_SET(_field, _val))
+
+static inline u32 mt76_rr(struct mt7601u_dev *dev, u32 offset)
+{
+	return mt7601u_rr(dev, offset);
+}
+
+static inline void mt76_wr(struct mt7601u_dev *dev, u32 offset, u32 val)
+{
+	return mt7601u_wr(dev, offset, val);
+}
+
+static inline u32
+mt76_rmw(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val)
+{
+	return mt7601u_rmw(dev, offset, mask, val);
+}
+
+static inline u32 mt76_set(struct mt7601u_dev *dev, u32 offset, u32 val)
+{
+	return mt76_rmw(dev, offset, 0, val);
+}
+
+static inline u32 mt76_clear(struct mt7601u_dev *dev, u32 offset, u32 val)
+{
+	return mt76_rmw(dev, offset, val, 0);
+}
+
+int mt7601u_write_reg_pairs(struct mt7601u_dev *dev, u32 base,
+			    const struct mt76_reg_pair *data, int len);
+int mt7601u_burst_write_regs(struct mt7601u_dev *dev, u32 offset,
+			     const u32 *data, int n);
+void mt7601u_addr_wr(struct mt7601u_dev *dev, const u32 offset, const u8 *addr);
+
+/* Init */
+struct mt7601u_dev *mt7601u_alloc_device(struct device *dev);
+int mt7601u_init_hardware(struct mt7601u_dev *dev);
+int mt7601u_register_device(struct mt7601u_dev *dev);
+void mt7601u_cleanup(struct mt7601u_dev *dev);
+
+int mt7601u_mac_start(struct mt7601u_dev *dev);
+void mt7601u_mac_stop(struct mt7601u_dev *dev);
+
+/* PHY */
+int mt7601u_phy_init(struct mt7601u_dev *dev);
+int mt7601u_wait_bbp_ready(struct mt7601u_dev *dev);
+void mt7601u_set_rx_path(struct mt7601u_dev *dev, u8 path);
+void mt7601u_set_tx_dac(struct mt7601u_dev *dev, u8 path);
+int mt7601u_bbp_set_bw(struct mt7601u_dev *dev, int bw);
+void mt7601u_agc_save(struct mt7601u_dev *dev);
+void mt7601u_agc_restore(struct mt7601u_dev *dev);
+int mt7601u_phy_set_channel(struct mt7601u_dev *dev,
+			    struct cfg80211_chan_def *chandef);
+void mt7601u_phy_recalibrate_after_assoc(struct mt7601u_dev *dev);
+int mt7601u_phy_get_rssi(struct mt7601u_dev *dev,
+			 struct mt7601u_rxwi *rxwi, u16 rate);
+void mt7601u_phy_con_cal_onoff(struct mt7601u_dev *dev,
+			       struct ieee80211_bss_conf *info);
+
+/* MAC */
+void mt7601u_mac_work(struct work_struct *work);
+void mt7601u_mac_set_protection(struct mt7601u_dev *dev, bool legacy_prot,
+				int ht_mode);
+void mt7601u_mac_set_short_preamble(struct mt7601u_dev *dev, bool short_preamb);
+void mt7601u_mac_config_tsf(struct mt7601u_dev *dev, bool enable, int interval);
+void
+mt7601u_mac_wcid_setup(struct mt7601u_dev *dev, u8 idx, u8 vif_idx, u8 *mac);
+void mt7601u_mac_set_ampdu_factor(struct mt7601u_dev *dev);
+
+/* TX */
+void mt7601u_tx(struct ieee80211_hw *hw, struct ieee80211_tx_control *control,
+		struct sk_buff *skb);
+int mt7601u_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		    u16 queue, const struct ieee80211_tx_queue_params *params);
+void mt7601u_tx_status(struct mt7601u_dev *dev, struct sk_buff *skb);
+void mt7601u_tx_stat(struct work_struct *work);
+
+/* util */
+void mt76_remove_hdr_pad(struct sk_buff *skb);
+int mt76_insert_hdr_pad(struct sk_buff *skb);
+
+u32 mt7601u_bbp_set_ctrlch(struct mt7601u_dev *dev, bool below);
+
+static inline u32 mt7601u_mac_set_ctrlch(struct mt7601u_dev *dev, bool below)
+{
+	return mt7601u_rmc(dev, MT_TX_BAND_CFG, 1, below);
+}
+
+int mt7601u_dma_init(struct mt7601u_dev *dev);
+void mt7601u_dma_cleanup(struct mt7601u_dev *dev);
+
+int mt7601u_dma_enqueue_tx(struct mt7601u_dev *dev, struct sk_buff *skb,
+			   struct mt76_wcid *wcid, int hw_q);
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/phy.c b/drivers/net/wireless/mediatek/mt7601u/phy.c
new file mode 100644
index 000000000000..1908af6add87
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/phy.c
@@ -0,0 +1,1251 @@
+/*
+ * (c) Copyright 2002-2010, Ralink Technology, Inc.
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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 "mt7601u.h"
+#include "mcu.h"
+#include "eeprom.h"
+#include "trace.h"
+#include "initvals_phy.h"
+
+#include <linux/etherdevice.h>
+
+static void mt7601u_agc_reset(struct mt7601u_dev *dev);
+
+static int
+mt7601u_rf_wr(struct mt7601u_dev *dev, u8 bank, u8 offset, u8 value)
+{
+	int ret = 0;
+
+	if (WARN_ON(!test_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state)) ||
+	    WARN_ON(offset > 63))
+		return -EINVAL;
+	if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
+		return 0;
+
+	mutex_lock(&dev->reg_atomic_mutex);
+
+	if (!mt76_poll(dev, MT_RF_CSR_CFG, MT_RF_CSR_CFG_KICK, 0, 100)) {
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+
+	mt7601u_wr(dev, MT_RF_CSR_CFG, MT76_SET(MT_RF_CSR_CFG_DATA, value) |
+				       MT76_SET(MT_RF_CSR_CFG_REG_BANK, bank) |
+				       MT76_SET(MT_RF_CSR_CFG_REG_ID, offset) |
+				       MT_RF_CSR_CFG_WR |
+				       MT_RF_CSR_CFG_KICK);
+	trace_rf_write(dev, bank, offset, value);
+out:
+	mutex_unlock(&dev->reg_atomic_mutex);
+
+	if (ret < 0)
+		dev_err(dev->dev, "Error: RF write %02hhx:%02hhx failed:%d!!\n",
+			bank, offset, ret);
+
+	return ret;
+}
+
+static int
+mt7601u_rf_rr(struct mt7601u_dev *dev, u8 bank, u8 offset)
+{
+	int ret = -ETIMEDOUT;
+	u32 val;
+
+	if (WARN_ON(!test_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state)) ||
+	    WARN_ON(offset > 63))
+		return -EINVAL;
+	if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
+		return 0xff;
+
+	mutex_lock(&dev->reg_atomic_mutex);
+
+	if (!mt76_poll(dev, MT_RF_CSR_CFG, MT_RF_CSR_CFG_KICK, 0, 100))
+		goto out;
+
+	mt7601u_wr(dev, MT_RF_CSR_CFG, MT76_SET(MT_RF_CSR_CFG_REG_BANK, bank) |
+				       MT76_SET(MT_RF_CSR_CFG_REG_ID, offset) |
+				       MT_RF_CSR_CFG_KICK);
+
+	if (!mt76_poll(dev, MT_RF_CSR_CFG, MT_RF_CSR_CFG_KICK, 0, 100))
+		goto out;
+
+	val = mt7601u_rr(dev, MT_RF_CSR_CFG);
+	if (MT76_GET(MT_RF_CSR_CFG_REG_ID, val) == offset &&
+	    MT76_GET(MT_RF_CSR_CFG_REG_BANK, val) == bank) {
+		ret = MT76_GET(MT_RF_CSR_CFG_DATA, val);
+		trace_rf_read(dev, bank, offset, ret);
+	}
+out:
+	mutex_unlock(&dev->reg_atomic_mutex);
+
+	if (ret < 0)
+		dev_err(dev->dev, "Error: RF read %02hhx:%02hhx failed:%d!!\n",
+			bank, offset, ret);
+
+	return ret;
+}
+
+static int
+mt7601u_rf_rmw(struct mt7601u_dev *dev, u8 bank, u8 offset, u8 mask, u8 val)
+{
+	int ret;
+
+	ret = mt7601u_rf_rr(dev, bank, offset);
+	if (ret < 0)
+		return ret;
+	val |= ret & ~mask;
+	ret = mt7601u_rf_wr(dev, bank, offset, val);
+	if (ret)
+		return ret;
+
+	return val;
+}
+
+static int
+mt7601u_rf_set(struct mt7601u_dev *dev, u8 bank, u8 offset, u8 val)
+{
+	return mt7601u_rf_rmw(dev, bank, offset, 0, val);
+}
+
+static int
+mt7601u_rf_clear(struct mt7601u_dev *dev, u8 bank, u8 offset, u8 mask)
+{
+	return mt7601u_rf_rmw(dev, bank, offset, mask, 0);
+}
+
+static void mt7601u_bbp_wr(struct mt7601u_dev *dev, u8 offset, u8 val)
+{
+	if (WARN_ON(!test_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state)) ||
+	    test_bit(MT7601U_STATE_REMOVED, &dev->state))
+		return;
+
+	mutex_lock(&dev->reg_atomic_mutex);
+
+	if (!mt76_poll(dev, MT_BBP_CSR_CFG, MT_BBP_CSR_CFG_BUSY, 0, 1000)) {
+		dev_err(dev->dev, "Error: BBP write %02hhx failed!!\n", offset);
+		goto out;
+	}
+
+	mt7601u_wr(dev, MT_BBP_CSR_CFG,
+		   MT76_SET(MT_BBP_CSR_CFG_VAL, val) |
+		   MT76_SET(MT_BBP_CSR_CFG_REG_NUM, offset) |
+		   MT_BBP_CSR_CFG_RW_MODE | MT_BBP_CSR_CFG_BUSY);
+	trace_bbp_write(dev, offset, val);
+out:
+	mutex_unlock(&dev->reg_atomic_mutex);
+}
+
+static int mt7601u_bbp_rr(struct mt7601u_dev *dev, u8 offset)
+{
+	u32 val;
+	int ret = -ETIMEDOUT;
+
+	if (WARN_ON(!test_bit(MT7601U_STATE_WLAN_RUNNING, &dev->state)))
+		return -EINVAL;
+	if (test_bit(MT7601U_STATE_REMOVED, &dev->state))
+		return 0xff;
+
+	mutex_lock(&dev->reg_atomic_mutex);
+
+	if (!mt76_poll(dev, MT_BBP_CSR_CFG, MT_BBP_CSR_CFG_BUSY, 0, 1000))
+		goto out;
+
+	mt7601u_wr(dev, MT_BBP_CSR_CFG,
+		   MT76_SET(MT_BBP_CSR_CFG_REG_NUM, offset) |
+		   MT_BBP_CSR_CFG_RW_MODE | MT_BBP_CSR_CFG_BUSY |
+		   MT_BBP_CSR_CFG_READ);
+
+	if (!mt76_poll(dev, MT_BBP_CSR_CFG, MT_BBP_CSR_CFG_BUSY, 0, 1000))
+		goto out;
+
+	val = mt7601u_rr(dev, MT_BBP_CSR_CFG);
+	if (MT76_GET(MT_BBP_CSR_CFG_REG_NUM, val) == offset) {
+		ret = MT76_GET(MT_BBP_CSR_CFG_VAL, val);
+		trace_bbp_read(dev, offset, ret);
+	}
+out:
+	mutex_unlock(&dev->reg_atomic_mutex);
+
+	if (ret < 0)
+		dev_err(dev->dev, "Error: BBP read %02hhx failed:%d!!\n",
+			offset, ret);
+
+	return ret;
+}
+
+static int mt7601u_bbp_rmw(struct mt7601u_dev *dev, u8 offset, u8 mask, u8 val)
+{
+	int ret;
+
+	ret = mt7601u_bbp_rr(dev, offset);
+	if (ret < 0)
+		return ret;
+	val |= ret & ~mask;
+	mt7601u_bbp_wr(dev, offset, val);
+
+	return val;
+}
+
+static u8 mt7601u_bbp_rmc(struct mt7601u_dev *dev, u8 offset, u8 mask, u8 val)
+{
+	int ret;
+
+	ret = mt7601u_bbp_rr(dev, offset);
+	if (ret < 0)
+		return ret;
+	val |= ret & ~mask;
+	if (ret != val)
+		mt7601u_bbp_wr(dev, offset, val);
+
+	return val;
+}
+
+int mt7601u_wait_bbp_ready(struct mt7601u_dev *dev)
+{
+	int i = 20;
+	u8 val;
+
+	do {
+		val = mt7601u_bbp_rr(dev, MT_BBP_REG_VERSION);
+		if (val && ~val)
+			break;
+	} while (--i);
+
+	if (!i) {
+		dev_err(dev->dev, "Error: BBP is not ready\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+u32 mt7601u_bbp_set_ctrlch(struct mt7601u_dev *dev, bool below)
+{
+	return mt7601u_bbp_rmc(dev, 3, 0x20, below ? 0x20 : 0);
+}
+
+int mt7601u_phy_get_rssi(struct mt7601u_dev *dev,
+			 struct mt7601u_rxwi *rxwi, u16 rate)
+{
+	static const s8 lna[2][2][3] = {
+		/* main LNA */ {
+			/* bw20 */ { -2, 15, 33 },
+			/* bw40 */ {  0, 16, 34 }
+		},
+		/*  aux LNA */ {
+			/* bw20 */ { -2, 15, 33 },
+			/* bw40 */ { -2, 16, 34 }
+		}
+	};
+	int bw = MT76_GET(MT_RXWI_RATE_BW, rate);
+	int aux_lna = MT76_GET(MT_RXWI_ANT_AUX_LNA, rxwi->ant);
+	int lna_id = MT76_GET(MT_RXWI_GAIN_RSSI_LNA_ID, rxwi->gain);
+	int val;
+
+	if (lna_id) /* LNA id can be 0, 2, 3. */
+		lna_id--;
+
+	val = 8;
+	val -= lna[aux_lna][bw][lna_id];
+	val -= MT76_GET(MT_RXWI_GAIN_RSSI_VAL, rxwi->gain);
+	val -= dev->ee->lna_gain;
+	val -= dev->ee->rssi_offset[0];
+
+	return val;
+}
+
+static void mt7601u_vco_cal(struct mt7601u_dev *dev)
+{
+	mt7601u_rf_wr(dev, 0, 4, 0x0a);
+	mt7601u_rf_wr(dev, 0, 5, 0x20);
+	mt7601u_rf_set(dev, 0, 4, BIT(7));
+	msleep(2);
+}
+
+static int mt7601u_set_bw_filter(struct mt7601u_dev *dev, bool cal)
+{
+	u32 filter = 0;
+	int ret;
+
+	if (!cal)
+		filter |= 0x10000;
+	if (dev->bw != MT_BW_20)
+		filter |= 0x00100;
+
+	/* TX */
+	ret = mt7601u_mcu_calibrate(dev, MCU_CAL_BW, filter | 1);
+	if (ret)
+		return ret;
+	/* RX */
+	return mt7601u_mcu_calibrate(dev, MCU_CAL_BW, filter);
+}
+
+static int mt7601u_load_bbp_temp_table_bw(struct mt7601u_dev *dev)
+{
+	const struct reg_table *t;
+
+	if (WARN_ON(dev->temp_mode > MT_TEMP_MODE_LOW))
+		return -EINVAL;
+
+	t = &bbp_mode_table[dev->temp_mode][dev->bw];
+
+	return mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP, t->regs, t->n);
+}
+
+static int mt7601u_bbp_temp(struct mt7601u_dev *dev, int mode, const char *name)
+{
+	const struct reg_table *t;
+	int ret;
+
+	if (dev->temp_mode == mode)
+		return 0;
+
+	dev->temp_mode = mode;
+	trace_temp_mode(dev, mode);
+
+	t = bbp_mode_table[dev->temp_mode];
+	ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
+				      t[2].regs, t[2].n);
+	if (ret)
+		return ret;
+
+	return mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
+				       t[dev->bw].regs, t[dev->bw].n);
+}
+
+static void mt7601u_apply_ch14_fixup(struct mt7601u_dev *dev, int hw_chan)
+{
+	struct mt7601u_rate_power *t = &dev->ee->power_rate_table;
+
+	if (hw_chan != 14 || dev->bw != MT_BW_20) {
+		mt7601u_bbp_rmw(dev, 4, 0x20, 0);
+		mt7601u_bbp_wr(dev, 178, 0xff);
+
+		t->cck[0].bw20 = dev->ee->real_cck_bw20[0];
+		t->cck[1].bw20 = dev->ee->real_cck_bw20[1];
+	} else { /* Apply CH14 OBW fixup */
+		mt7601u_bbp_wr(dev, 4, 0x60);
+		mt7601u_bbp_wr(dev, 178, 0);
+
+		/* Note: vendor code is buggy here for negative values */
+		t->cck[0].bw20 = dev->ee->real_cck_bw20[0] - 2;
+		t->cck[1].bw20 = dev->ee->real_cck_bw20[1] - 2;
+	}
+}
+
+static int __mt7601u_phy_set_channel(struct mt7601u_dev *dev,
+				     struct cfg80211_chan_def *chandef)
+{
+#define FREQ_PLAN_REGS	4
+	static const u8 freq_plan[14][FREQ_PLAN_REGS] = {
+		{ 0x99,	0x99,	0x09,	0x50 },
+		{ 0x46,	0x44,	0x0a,	0x50 },
+		{ 0xec,	0xee,	0x0a,	0x50 },
+		{ 0x99,	0x99,	0x0b,	0x50 },
+		{ 0x46,	0x44,	0x08,	0x51 },
+		{ 0xec,	0xee,	0x08,	0x51 },
+		{ 0x99,	0x99,	0x09,	0x51 },
+		{ 0x46,	0x44,	0x0a,	0x51 },
+		{ 0xec,	0xee,	0x0a,	0x51 },
+		{ 0x99,	0x99,	0x0b,	0x51 },
+		{ 0x46,	0x44,	0x08,	0x52 },
+		{ 0xec,	0xee,	0x08,	0x52 },
+		{ 0x99,	0x99,	0x09,	0x52 },
+		{ 0x33,	0x33,	0x0b,	0x52 },
+	};
+	struct mt76_reg_pair channel_freq_plan[FREQ_PLAN_REGS] = {
+		{ 17, 0 }, { 18, 0 }, { 19, 0 }, { 20, 0 },
+	};
+	struct mt76_reg_pair bbp_settings[3] = {
+		{ 62, 0x37 - dev->ee->lna_gain },
+		{ 63, 0x37 - dev->ee->lna_gain },
+		{ 64, 0x37 - dev->ee->lna_gain },
+	};
+
+	struct ieee80211_channel *chan = chandef->chan;
+	enum nl80211_channel_type chan_type =
+		cfg80211_get_chandef_type(chandef);
+	struct mt7601u_rate_power *t = &dev->ee->power_rate_table;
+	int chan_idx;
+	bool chan_ext_below;
+	u8 bw;
+	int i, ret;
+
+	bw = MT_BW_20;
+	chan_ext_below = (chan_type == NL80211_CHAN_HT40MINUS);
+	chan_idx = chan->hw_value - 1;
+
+	if (chandef->width == NL80211_CHAN_WIDTH_40) {
+		bw = MT_BW_40;
+
+		if (chan_idx > 1 && chan_type == NL80211_CHAN_HT40MINUS)
+			chan_idx -= 2;
+		else if (chan_idx < 12 && chan_type == NL80211_CHAN_HT40PLUS)
+			chan_idx += 2;
+		else
+			dev_err(dev->dev, "Error: invalid 40MHz channel!!\n");
+	}
+
+	if (bw != dev->bw || chan_ext_below != dev->chan_ext_below) {
+		dev_dbg(dev->dev, "Info: switching HT mode bw:%d below:%d\n",
+			bw, chan_ext_below);
+
+		mt7601u_bbp_set_bw(dev, bw);
+
+		mt7601u_bbp_set_ctrlch(dev, chan_ext_below);
+		mt7601u_mac_set_ctrlch(dev, chan_ext_below);
+		dev->chan_ext_below = chan_ext_below;
+	}
+
+	for (i = 0; i < FREQ_PLAN_REGS; i++)
+		channel_freq_plan[i].value = freq_plan[chan_idx][i];
+
+	ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_RF,
+				      channel_freq_plan, FREQ_PLAN_REGS);
+	if (ret)
+		return ret;
+
+	mt7601u_rmw(dev, MT_TX_ALC_CFG_0, 0x3f3f,
+		    dev->ee->chan_pwr[chan_idx] & 0x3f);
+
+	ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
+				      bbp_settings, ARRAY_SIZE(bbp_settings));
+	if (ret)
+		return ret;
+
+	mt7601u_vco_cal(dev);
+	mt7601u_bbp_set_bw(dev, bw);
+	ret = mt7601u_set_bw_filter(dev, false);
+	if (ret)
+		return ret;
+
+	mt7601u_apply_ch14_fixup(dev, chan->hw_value);
+	mt7601u_wr(dev, MT_TX_PWR_CFG_0, int_to_s6(t->ofdm[1].bw20) << 24 |
+					 int_to_s6(t->ofdm[0].bw20) << 16 |
+					 int_to_s6(t->cck[1].bw20) << 8 |
+					 int_to_s6(t->cck[0].bw20));
+
+	if (test_bit(MT7601U_STATE_SCANNING, &dev->state))
+		mt7601u_agc_reset(dev);
+
+	dev->chandef = *chandef;
+
+	return 0;
+}
+
+int mt7601u_phy_set_channel(struct mt7601u_dev *dev,
+			    struct cfg80211_chan_def *chandef)
+{
+	int ret;
+
+	cancel_delayed_work_sync(&dev->cal_work);
+	cancel_delayed_work_sync(&dev->freq_cal.work);
+
+	mutex_lock(&dev->hw_atomic_mutex);
+	ret = __mt7601u_phy_set_channel(dev, chandef);
+	mutex_unlock(&dev->hw_atomic_mutex);
+	if (ret)
+		return ret;
+
+	if (test_bit(MT7601U_STATE_SCANNING, &dev->state))
+		return 0;
+
+	ieee80211_queue_delayed_work(dev->hw, &dev->cal_work,
+				     MT_CALIBRATE_INTERVAL);
+	if (dev->freq_cal.enabled)
+		ieee80211_queue_delayed_work(dev->hw, &dev->freq_cal.work,
+					     MT_FREQ_CAL_INIT_DELAY);
+	return 0;
+}
+
+#define BBP_R47_FLAG		GENMASK(2, 0)
+#define BBP_R47_F_TSSI		0
+#define BBP_R47_F_PKT_T		1
+#define BBP_R47_F_TX_RATE	2
+#define BBP_R47_F_TEMP		4
+/**
+ * mt7601u_bbp_r47_get - read value through BBP R47/R49 pair
+ * @dev:	pointer to adapter structure
+ * @reg:	value of BBP R47 before the operation
+ * @flag:	one of the BBP_R47_F_* flags
+ *
+ * Convenience helper for reading values through BBP R47/R49 pair.
+ * Takes old value of BBP R47 as @reg, because callers usually have it
+ * cached already.
+ *
+ * Return: value of BBP R49.
+ */
+static u8 mt7601u_bbp_r47_get(struct mt7601u_dev *dev, u8 reg, u8 flag)
+{
+	flag |= reg & ~BBP_R47_FLAG;
+	mt7601u_bbp_wr(dev, 47, flag);
+	usleep_range(500, 700);
+	return mt7601u_bbp_rr(dev, 49);
+}
+
+static s8 mt7601u_read_bootup_temp(struct mt7601u_dev *dev)
+{
+	u8 bbp_val, temp;
+	u32 rf_bp, rf_set;
+	int i;
+
+	rf_set = mt7601u_rr(dev, MT_RF_SETTING_0);
+	rf_bp = mt7601u_rr(dev, MT_RF_BYPASS_0);
+
+	mt7601u_wr(dev, MT_RF_BYPASS_0, 0);
+	mt7601u_wr(dev, MT_RF_SETTING_0, 0x00000010);
+	mt7601u_wr(dev, MT_RF_BYPASS_0, 0x00000010);
+
+	bbp_val = mt7601u_bbp_rmw(dev, 47, 0, 0x10);
+
+	mt7601u_bbp_wr(dev, 22, 0x40);
+
+	for (i = 100; i && (bbp_val & 0x10); i--)
+		bbp_val = mt7601u_bbp_rr(dev, 47);
+
+	temp = mt7601u_bbp_r47_get(dev, bbp_val, BBP_R47_F_TEMP);
+
+	mt7601u_bbp_wr(dev, 22, 0);
+
+	bbp_val = mt7601u_bbp_rr(dev, 21);
+	bbp_val |= 0x02;
+	mt7601u_bbp_wr(dev, 21, bbp_val);
+	bbp_val &= ~0x02;
+	mt7601u_bbp_wr(dev, 21, bbp_val);
+
+	mt7601u_wr(dev, MT_RF_BYPASS_0, 0);
+	mt7601u_wr(dev, MT_RF_SETTING_0, rf_set);
+	mt7601u_wr(dev, MT_RF_BYPASS_0, rf_bp);
+
+	trace_read_temp(dev, temp);
+	return temp;
+}
+
+static s8 mt7601u_read_temp(struct mt7601u_dev *dev)
+{
+	int i;
+	u8 val;
+	s8 temp;
+
+	val = mt7601u_bbp_rmw(dev, 47, 0x7f, 0x10);
+
+	/* Note: this rarely succeeds, temp can change even if it fails. */
+	for (i = 100; i && (val & 0x10); i--)
+		val = mt7601u_bbp_rr(dev, 47);
+
+	temp = mt7601u_bbp_r47_get(dev, val, BBP_R47_F_TEMP);
+
+	trace_read_temp(dev, temp);
+	return temp;
+}
+
+static void mt7601u_rxdc_cal(struct mt7601u_dev *dev)
+{
+	static const struct mt76_reg_pair intro[] = {
+		{ 158, 0x8d }, { 159, 0xfc },
+		{ 158, 0x8c }, { 159, 0x4c },
+	}, outro[] = {
+		{ 158, 0x8d }, { 159, 0xe0 },
+	};
+	u32 mac_ctrl;
+	int i, ret;
+
+	mac_ctrl = mt7601u_rr(dev, MT_MAC_SYS_CTRL);
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, MT_MAC_SYS_CTRL_ENABLE_RX);
+
+	ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
+				      intro, ARRAY_SIZE(intro));
+	if (ret)
+		dev_err(dev->dev, "%s intro failed:%d\n", __func__, ret);
+
+	for (i = 20; i; i--) {
+		usleep_range(300, 500);
+
+		mt7601u_bbp_wr(dev, 158, 0x8c);
+		if (mt7601u_bbp_rr(dev, 159) == 0x0c)
+			break;
+	}
+	if (!i)
+		dev_err(dev->dev, "%s timed out\n", __func__);
+
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, 0);
+
+	ret = mt7601u_write_reg_pairs(dev, MT_MCU_MEMMAP_BBP,
+				      outro, ARRAY_SIZE(outro));
+	if (ret)
+		dev_err(dev->dev, "%s outro failed:%d\n", __func__, ret);
+
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, mac_ctrl);
+}
+
+void mt7601u_phy_recalibrate_after_assoc(struct mt7601u_dev *dev)
+{
+	mt7601u_mcu_calibrate(dev, MCU_CAL_DPD, dev->curr_temp);
+
+	mt7601u_rxdc_cal(dev);
+}
+
+/* Note: function copied from vendor driver */
+static s16 lin2dBd(u16 linear)
+{
+	short exp = 0;
+	unsigned int mantisa;
+	int app, dBd;
+
+	if (WARN_ON(!linear))
+		return -10000;
+
+	mantisa = linear;
+
+	exp = fls(mantisa) - 16;
+	if (exp > 0)
+		mantisa >>= exp;
+	else
+		mantisa <<= abs(exp);
+
+	if (mantisa <= 0xb800)
+		app = (mantisa + (mantisa >> 3) + (mantisa >> 4) - 0x9600);
+	else
+		app = (mantisa - (mantisa >> 3) - (mantisa >> 6) - 0x5a00);
+	if (app < 0)
+		app = 0;
+
+	dBd = ((15 + exp) << 15) + app;
+	dBd = (dBd << 2) + (dBd << 1) + (dBd >> 6) + (dBd >> 7);
+	dBd = (dBd >> 10);
+
+	return dBd;
+}
+
+static void
+mt7601u_set_initial_tssi(struct mt7601u_dev *dev, s16 tssi_db, s16 tssi_hvga_db)
+{
+	struct tssi_data *d = &dev->ee->tssi_data;
+	int init_offset;
+
+	init_offset = -((tssi_db * d->slope + d->offset[1]) / 4096) + 10;
+
+	mt76_rmw(dev, MT_TX_ALC_CFG_1, MT_TX_ALC_CFG_1_TEMP_COMP,
+		 int_to_s6(init_offset) & MT_TX_ALC_CFG_1_TEMP_COMP);
+}
+
+static void mt7601u_tssi_dc_gain_cal(struct mt7601u_dev *dev)
+{
+	u8 rf_vga, rf_mixer, bbp_r47;
+	int i, j;
+	s8 res[4];
+	s16 tssi_init_db, tssi_init_hvga_db;
+
+	mt7601u_wr(dev, MT_RF_SETTING_0, 0x00000030);
+	mt7601u_wr(dev, MT_RF_BYPASS_0, 0x000c0030);
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, 0);
+
+	mt7601u_bbp_wr(dev, 58, 0);
+	mt7601u_bbp_wr(dev, 241, 0x2);
+	mt7601u_bbp_wr(dev, 23, 0x8);
+	bbp_r47 = mt7601u_bbp_rr(dev, 47);
+
+	/* Set VGA gain */
+	rf_vga = mt7601u_rf_rr(dev, 5, 3);
+	mt7601u_rf_wr(dev, 5, 3, 8);
+
+	/* Mixer disable */
+	rf_mixer = mt7601u_rf_rr(dev, 4, 39);
+	mt7601u_rf_wr(dev, 4, 39, 0);
+
+	for (i = 0; i < 4; i++) {
+		mt7601u_rf_wr(dev, 4, 39, (i & 1) ? rf_mixer : 0);
+
+		mt7601u_bbp_wr(dev, 23, (i < 2) ? 0x08 : 0x02);
+		mt7601u_rf_wr(dev, 5, 3, (i < 2) ? 0x08 : 0x11);
+
+		/* BBP TSSI initial and soft reset */
+		mt7601u_bbp_wr(dev, 22, 0);
+		mt7601u_bbp_wr(dev, 244, 0);
+
+		mt7601u_bbp_wr(dev, 21, 1);
+		udelay(1);
+		mt7601u_bbp_wr(dev, 21, 0);
+
+		/* TSSI measurement */
+		mt7601u_bbp_wr(dev, 47, 0x50);
+		mt7601u_bbp_wr(dev, (i & 1) ? 244 : 22, (i & 1) ? 0x31 : 0x40);
+
+		for (j = 20; j; j--)
+			if (!(mt7601u_bbp_rr(dev, 47) & 0x10))
+				break;
+		if (!j)
+			dev_err(dev->dev, "%s timed out\n", __func__);
+
+		/* TSSI read */
+		mt7601u_bbp_wr(dev, 47, 0x40);
+		res[i] = mt7601u_bbp_rr(dev, 49);
+	}
+
+	tssi_init_db = lin2dBd((short)res[1] - res[0]);
+	tssi_init_hvga_db = lin2dBd(((short)res[3] - res[2]) * 4);
+	dev->tssi_init = res[0];
+	dev->tssi_init_hvga = res[2];
+	dev->tssi_init_hvga_offset_db = tssi_init_hvga_db - tssi_init_db;
+
+	dev_dbg(dev->dev,
+		"TSSI_init:%hhx db:%hx hvga:%hhx hvga_db:%hx off_db:%hx\n",
+		dev->tssi_init, tssi_init_db, dev->tssi_init_hvga,
+		tssi_init_hvga_db, dev->tssi_init_hvga_offset_db);
+
+	mt7601u_bbp_wr(dev, 22, 0);
+	mt7601u_bbp_wr(dev, 244, 0);
+
+	mt7601u_bbp_wr(dev, 21, 1);
+	udelay(1);
+	mt7601u_bbp_wr(dev, 21, 0);
+
+	mt7601u_wr(dev, MT_RF_BYPASS_0, 0);
+	mt7601u_wr(dev, MT_RF_SETTING_0, 0);
+
+	mt7601u_rf_wr(dev, 5, 3, rf_vga);
+	mt7601u_rf_wr(dev, 4, 39, rf_mixer);
+	mt7601u_bbp_wr(dev, 47, bbp_r47);
+
+	mt7601u_set_initial_tssi(dev, tssi_init_db, tssi_init_hvga_db);
+}
+
+static int mt7601u_temp_comp(struct mt7601u_dev *dev, bool on)
+{
+	int ret, temp, hi_temp = 400, lo_temp = -200;
+
+	temp = (dev->raw_temp - dev->ee->ref_temp) * MT_EE_TEMPERATURE_SLOPE;
+	dev->curr_temp = temp;
+
+	/* DPD Calibration */
+	if (temp - dev->dpd_temp > 450 || temp - dev->dpd_temp < -450) {
+		dev->dpd_temp = temp;
+
+		ret = mt7601u_mcu_calibrate(dev, MCU_CAL_DPD, dev->dpd_temp);
+		if (ret)
+			return ret;
+
+		mt7601u_vco_cal(dev);
+
+		dev_dbg(dev->dev, "Recalibrate DPD\n");
+	}
+
+	/* PLL Lock Protect */
+	if (temp < -50 && !dev->pll_lock_protect) { /* < 20C */
+		dev->pll_lock_protect =  true;
+
+		mt7601u_rf_wr(dev, 4, 4, 6);
+		mt7601u_rf_clear(dev, 4, 10, 0x30);
+
+		dev_dbg(dev->dev, "PLL lock protect on - too cold\n");
+	} else if (temp > 50 && dev->pll_lock_protect) { /* > 30C */
+		dev->pll_lock_protect = false;
+
+		mt7601u_rf_wr(dev, 4, 4, 0);
+		mt7601u_rf_rmw(dev, 4, 10, 0x30, 0x10);
+
+		dev_dbg(dev->dev, "PLL lock protect off\n");
+	}
+
+	if (on) {
+		hi_temp -= 50;
+		lo_temp -= 50;
+	}
+
+	/* BBP CR for H, L, N temperature */
+	if (temp > hi_temp)
+		return mt7601u_bbp_temp(dev, MT_TEMP_MODE_HIGH, "high");
+	else if (temp > lo_temp)
+		return mt7601u_bbp_temp(dev, MT_TEMP_MODE_NORMAL, "normal");
+	else
+		return mt7601u_bbp_temp(dev, MT_TEMP_MODE_LOW, "low");
+}
+
+/* Note: this is used only with TSSI, we can just use trgt_pwr from eeprom. */
+static int mt7601u_current_tx_power(struct mt7601u_dev *dev)
+{
+	return dev->ee->chan_pwr[dev->chandef.chan->hw_value - 1];
+}
+
+static bool mt7601u_use_hvga(struct mt7601u_dev *dev)
+{
+	return !(mt7601u_current_tx_power(dev) > 20);
+}
+
+static s16
+mt7601u_phy_rf_pa_mode_val(struct mt7601u_dev *dev, int phy_mode, int tx_rate)
+{
+	static const s16 decode_tb[] = { 0, 8847, -5734, -5734 };
+	u32 reg;
+
+	switch (phy_mode) {
+	case MT_PHY_TYPE_OFDM:
+		tx_rate += 4;
+	case MT_PHY_TYPE_CCK:
+		reg = dev->rf_pa_mode[0];
+		break;
+	default:
+		reg = dev->rf_pa_mode[1];
+		break;
+	}
+
+	return decode_tb[(reg >> (tx_rate * 2)) & 0x3];
+}
+
+static struct mt7601u_tssi_params
+mt7601u_tssi_params_get(struct mt7601u_dev *dev)
+{
+	static const u8 ofdm_pkt2rate[8] = { 6, 4, 2, 0, 7, 5, 3, 1 };
+	static const int static_power[4] = { 0, -49152, -98304, 49152 };
+	struct mt7601u_tssi_params p;
+	u8 bbp_r47, pkt_type, tx_rate;
+	struct power_per_rate *rate_table;
+
+	bbp_r47 = mt7601u_bbp_rr(dev, 47);
+
+	p.tssi0 = mt7601u_bbp_r47_get(dev, bbp_r47, BBP_R47_F_TSSI);
+	dev->raw_temp = mt7601u_bbp_r47_get(dev, bbp_r47, BBP_R47_F_TEMP);
+	pkt_type = mt7601u_bbp_r47_get(dev, bbp_r47, BBP_R47_F_PKT_T);
+
+	p.trgt_power = mt7601u_current_tx_power(dev);
+
+	switch (pkt_type & 0x03) {
+	case MT_PHY_TYPE_CCK:
+		tx_rate = (pkt_type >> 4) & 0x03;
+		rate_table = dev->ee->power_rate_table.cck;
+		break;
+
+	case MT_PHY_TYPE_OFDM:
+		tx_rate = ofdm_pkt2rate[(pkt_type >> 4) & 0x07];
+		rate_table = dev->ee->power_rate_table.ofdm;
+		break;
+
+	default:
+		tx_rate = mt7601u_bbp_r47_get(dev, bbp_r47, BBP_R47_F_TX_RATE);
+		tx_rate &= 0x7f;
+		rate_table = dev->ee->power_rate_table.ht;
+		break;
+	}
+
+	if (dev->bw == MT_BW_20)
+		p.trgt_power += rate_table[tx_rate / 2].bw20;
+	else
+		p.trgt_power += rate_table[tx_rate / 2].bw40;
+
+	p.trgt_power <<= 12;
+
+	dev_dbg(dev->dev, "tx_rate:%02hhx pwr:%08x\n", tx_rate, p.trgt_power);
+
+	p.trgt_power += mt7601u_phy_rf_pa_mode_val(dev, pkt_type & 0x03,
+						   tx_rate);
+
+	/* Channel 14, cck, bw20 */
+	if ((pkt_type & 0x03) == MT_PHY_TYPE_CCK) {
+		if (mt7601u_bbp_rr(dev, 4) & 0x20)
+			p.trgt_power += mt7601u_bbp_rr(dev, 178) ? 18022 : 9830;
+		else
+			p.trgt_power += mt7601u_bbp_rr(dev, 178) ? 819 : 24576;
+	}
+
+	p.trgt_power += static_power[mt7601u_bbp_rr(dev, 1) & 0x03];
+
+	p.trgt_power += dev->ee->tssi_data.tx0_delta_offset;
+
+	dev_dbg(dev->dev,
+		"tssi:%02hhx t_power:%08x temp:%02hhx pkt_type:%02hhx\n",
+		p.tssi0, p.trgt_power, dev->raw_temp, pkt_type);
+
+	return p;
+}
+
+static bool mt7601u_tssi_read_ready(struct mt7601u_dev *dev)
+{
+	return !(mt7601u_bbp_rr(dev, 47) & 0x10);
+}
+
+static int mt7601u_tssi_cal(struct mt7601u_dev *dev)
+{
+	struct mt7601u_tssi_params params;
+	int curr_pwr, diff_pwr;
+	char tssi_offset;
+	s8 tssi_init;
+	s16 tssi_m_dc, tssi_db;
+	bool hvga;
+	u32 val;
+
+	if (!dev->ee->tssi_enabled)
+		return 0;
+
+	hvga = mt7601u_use_hvga(dev);
+	if (!dev->tssi_read_trig)
+		return mt7601u_mcu_tssi_read_kick(dev, hvga);
+
+	if (!mt7601u_tssi_read_ready(dev))
+		return 0;
+
+	params = mt7601u_tssi_params_get(dev);
+
+	tssi_init = (hvga ? dev->tssi_init_hvga : dev->tssi_init);
+	tssi_m_dc = params.tssi0 - tssi_init;
+	tssi_db = lin2dBd(tssi_m_dc);
+	dev_dbg(dev->dev, "tssi dc:%04hx db:%04hx hvga:%d\n",
+		tssi_m_dc, tssi_db, hvga);
+
+	if (dev->chandef.chan->hw_value < 5)
+		tssi_offset = dev->ee->tssi_data.offset[0];
+	else if (dev->chandef.chan->hw_value < 9)
+		tssi_offset = dev->ee->tssi_data.offset[1];
+	else
+		tssi_offset = dev->ee->tssi_data.offset[2];
+
+	if (hvga)
+		tssi_db -= dev->tssi_init_hvga_offset_db;
+
+	curr_pwr = tssi_db * dev->ee->tssi_data.slope + (tssi_offset << 9);
+	diff_pwr = params.trgt_power - curr_pwr;
+	dev_dbg(dev->dev, "Power curr:%08x diff:%08x\n", curr_pwr, diff_pwr);
+
+	if (params.tssi0 > 126 && diff_pwr > 0) {
+		dev_err(dev->dev, "Error: TSSI upper saturation\n");
+		diff_pwr = 0;
+	}
+	if (params.tssi0 - tssi_init < 1 && diff_pwr < 0) {
+		dev_err(dev->dev, "Error: TSSI lower saturation\n");
+		diff_pwr = 0;
+	}
+
+	if ((dev->prev_pwr_diff ^ diff_pwr) < 0 && abs(diff_pwr) < 4096 &&
+	    (abs(diff_pwr) > abs(dev->prev_pwr_diff) ||
+	     (diff_pwr > 0 && diff_pwr == -dev->prev_pwr_diff)))
+		diff_pwr = 0;
+	else
+		dev->prev_pwr_diff = diff_pwr;
+
+	diff_pwr += (diff_pwr > 0) ? 2048 : -2048;
+	diff_pwr /= 4096;
+
+	dev_dbg(dev->dev, "final diff: %08x\n", diff_pwr);
+
+	val = mt7601u_rr(dev, MT_TX_ALC_CFG_1);
+	curr_pwr = s6_to_int(MT76_GET(MT_TX_ALC_CFG_1_TEMP_COMP, val));
+	diff_pwr += curr_pwr;
+	val = (val & ~MT_TX_ALC_CFG_1_TEMP_COMP) | int_to_s6(diff_pwr);
+	mt7601u_wr(dev, MT_TX_ALC_CFG_1, val);
+
+	return mt7601u_mcu_tssi_read_kick(dev, hvga);
+}
+
+static u8 mt7601u_agc_default(struct mt7601u_dev *dev)
+{
+	return (dev->ee->lna_gain - 8) * 2 + 0x34;
+}
+
+static void mt7601u_agc_reset(struct mt7601u_dev *dev)
+{
+	u8 agc = mt7601u_agc_default(dev);
+
+	mt7601u_bbp_wr(dev, 66,	agc);
+}
+
+void mt7601u_agc_save(struct mt7601u_dev *dev)
+{
+	dev->agc_save = mt7601u_bbp_rr(dev, 66);
+}
+
+void mt7601u_agc_restore(struct mt7601u_dev *dev)
+{
+	mt7601u_bbp_wr(dev, 66, dev->agc_save);
+}
+
+static void mt7601u_agc_tune(struct mt7601u_dev *dev)
+{
+	u8 val = mt7601u_agc_default(dev);
+
+	if (test_bit(MT7601U_STATE_SCANNING, &dev->state))
+		return;
+
+	/* Note: only in STA mode and not dozing; perhaps do this only if
+	 *	 there is enough rssi updates since last run?
+	 *	 Rssi updates are only on beacons and U2M so should work...
+	 */
+	spin_lock_bh(&dev->con_mon_lock);
+	if (dev->avg_rssi <= -70)
+		val -= 0x20;
+	else if (dev->avg_rssi <= -60)
+		val -= 0x10;
+	spin_unlock_bh(&dev->con_mon_lock);
+
+	if (val != mt7601u_bbp_rr(dev, 66))
+		mt7601u_bbp_wr(dev, 66, val);
+
+	/* TODO: also if lost a lot of beacons try resetting
+	 *       (see RTMPSetAGCInitValue() call in mlme.c).
+	 */
+}
+
+static void mt7601u_phy_calibrate(struct work_struct *work)
+{
+	struct mt7601u_dev *dev = container_of(work, struct mt7601u_dev,
+					    cal_work.work);
+
+	mt7601u_agc_tune(dev);
+	mt7601u_tssi_cal(dev);
+	/* If TSSI calibration was run it already updated temperature. */
+	if (!dev->ee->tssi_enabled)
+		dev->raw_temp = mt7601u_read_temp(dev);
+	mt7601u_temp_comp(dev, true); /* TODO: find right value for @on */
+
+	ieee80211_queue_delayed_work(dev->hw, &dev->cal_work,
+				     MT_CALIBRATE_INTERVAL);
+}
+
+static unsigned long
+__mt7601u_phy_freq_cal(struct mt7601u_dev *dev, s8 last_offset, u8 phy_mode)
+{
+	u8 activate_threshold, deactivate_threshold;
+
+	trace_freq_cal_offset(dev, phy_mode, last_offset);
+
+	/* No beacons received - reschedule soon */
+	if (last_offset == MT_FREQ_OFFSET_INVALID)
+		return MT_FREQ_CAL_ADJ_INTERVAL;
+
+	switch (phy_mode) {
+	case MT_PHY_TYPE_CCK:
+		activate_threshold = 19;
+		deactivate_threshold = 5;
+		break;
+	case MT_PHY_TYPE_OFDM:
+		activate_threshold = 102;
+		deactivate_threshold = 32;
+		break;
+	case MT_PHY_TYPE_HT:
+	case MT_PHY_TYPE_HT_GF:
+		activate_threshold = 82;
+		deactivate_threshold = 20;
+		break;
+	default:
+		WARN_ON(1);
+		return MT_FREQ_CAL_CHECK_INTERVAL;
+	}
+
+	if (abs(last_offset) >= activate_threshold)
+		dev->freq_cal.adjusting = true;
+	else if (abs(last_offset) <= deactivate_threshold)
+		dev->freq_cal.adjusting = false;
+
+	if (!dev->freq_cal.adjusting)
+		return MT_FREQ_CAL_CHECK_INTERVAL;
+
+	if (last_offset > deactivate_threshold) {
+		if (dev->freq_cal.freq > 0)
+			dev->freq_cal.freq--;
+		else
+			dev->freq_cal.adjusting = false;
+	} else if (last_offset < -deactivate_threshold) {
+		if (dev->freq_cal.freq < 0xbf)
+			dev->freq_cal.freq++;
+		else
+			dev->freq_cal.adjusting = false;
+	}
+
+	trace_freq_cal_adjust(dev, dev->freq_cal.freq);
+	mt7601u_rf_wr(dev, 0, 12, dev->freq_cal.freq);
+	mt7601u_vco_cal(dev);
+
+	return dev->freq_cal.adjusting ? MT_FREQ_CAL_ADJ_INTERVAL :
+					 MT_FREQ_CAL_CHECK_INTERVAL;
+}
+
+static void mt7601u_phy_freq_cal(struct work_struct *work)
+{
+	struct mt7601u_dev *dev = container_of(work, struct mt7601u_dev,
+					       freq_cal.work.work);
+	s8 last_offset;
+	u8 phy_mode;
+	unsigned long delay;
+
+	spin_lock_bh(&dev->con_mon_lock);
+	last_offset = dev->bcn_freq_off;
+	phy_mode = dev->bcn_phy_mode;
+	spin_unlock_bh(&dev->con_mon_lock);
+
+	delay = __mt7601u_phy_freq_cal(dev, last_offset, phy_mode);
+	ieee80211_queue_delayed_work(dev->hw, &dev->freq_cal.work, delay);
+
+	spin_lock_bh(&dev->con_mon_lock);
+	dev->bcn_freq_off = MT_FREQ_OFFSET_INVALID;
+	spin_unlock_bh(&dev->con_mon_lock);
+}
+
+void mt7601u_phy_con_cal_onoff(struct mt7601u_dev *dev,
+			       struct ieee80211_bss_conf *info)
+{
+	if (!info->assoc)
+		cancel_delayed_work_sync(&dev->freq_cal.work);
+
+	/* Start/stop collecting beacon data */
+	spin_lock_bh(&dev->con_mon_lock);
+	ether_addr_copy(dev->ap_bssid, info->bssid);
+	dev->avg_rssi = 0;
+	dev->bcn_freq_off = MT_FREQ_OFFSET_INVALID;
+	spin_unlock_bh(&dev->con_mon_lock);
+
+	dev->freq_cal.freq = dev->ee->rf_freq_off;
+	dev->freq_cal.enabled = info->assoc;
+	dev->freq_cal.adjusting = false;
+
+	if (info->assoc)
+		ieee80211_queue_delayed_work(dev->hw, &dev->freq_cal.work,
+					     MT_FREQ_CAL_INIT_DELAY);
+}
+
+static int mt7601u_init_cal(struct mt7601u_dev *dev)
+{
+	u32 mac_ctrl;
+	int ret;
+
+	dev->raw_temp = mt7601u_read_bootup_temp(dev);
+	dev->curr_temp = (dev->raw_temp - dev->ee->ref_temp) *
+		MT_EE_TEMPERATURE_SLOPE;
+	dev->dpd_temp = dev->curr_temp;
+
+	mac_ctrl = mt7601u_rr(dev, MT_MAC_SYS_CTRL);
+
+	ret = mt7601u_mcu_calibrate(dev, MCU_CAL_R, 0);
+	if (ret)
+		return ret;
+
+	ret = mt7601u_rf_rr(dev, 0, 4);
+	if (ret < 0)
+		return ret;
+	ret |= 0x80;
+	ret = mt7601u_rf_wr(dev, 0, 4, ret);
+	if (ret)
+		return ret;
+	msleep(2);
+
+	ret = mt7601u_mcu_calibrate(dev, MCU_CAL_TXDCOC, 0);
+	if (ret)
+		return ret;
+
+	mt7601u_rxdc_cal(dev);
+
+	ret = mt7601u_set_bw_filter(dev, true);
+	if (ret)
+		return ret;
+	ret = mt7601u_mcu_calibrate(dev, MCU_CAL_LOFT, 0);
+	if (ret)
+		return ret;
+	ret = mt7601u_mcu_calibrate(dev, MCU_CAL_TXIQ, 0);
+	if (ret)
+		return ret;
+	ret = mt7601u_mcu_calibrate(dev, MCU_CAL_RXIQ, 0);
+	if (ret)
+		return ret;
+	ret = mt7601u_mcu_calibrate(dev, MCU_CAL_DPD, dev->dpd_temp);
+	if (ret)
+		return ret;
+
+	mt7601u_rxdc_cal(dev);
+
+	mt7601u_tssi_dc_gain_cal(dev);
+
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, mac_ctrl);
+
+	mt7601u_temp_comp(dev, true);
+
+	return 0;
+}
+
+int mt7601u_bbp_set_bw(struct mt7601u_dev *dev, int bw)
+{
+	u32 val, old;
+
+	if (bw == dev->bw) {
+		/* Vendor driver does the rmc even when no change is needed. */
+		mt7601u_bbp_rmc(dev, 4, 0x18, bw == MT_BW_20 ? 0 : 0x10);
+
+		return 0;
+	}
+	dev->bw = bw;
+
+	/* Stop MAC for the time of bw change */
+	old = mt7601u_rr(dev, MT_MAC_SYS_CTRL);
+	val = old & ~(MT_MAC_SYS_CTRL_ENABLE_TX | MT_MAC_SYS_CTRL_ENABLE_RX);
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, val);
+	mt76_poll(dev, MT_MAC_STATUS, MT_MAC_STATUS_TX | MT_MAC_STATUS_RX,
+		  0, 500000);
+
+	mt7601u_bbp_rmc(dev, 4, 0x18, bw == MT_BW_20 ? 0 : 0x10);
+
+	mt7601u_wr(dev, MT_MAC_SYS_CTRL, old);
+
+	return mt7601u_load_bbp_temp_table_bw(dev);
+}
+
+/**
+ * mt7601u_set_rx_path - set rx path in BBP
+ * @dev:	pointer to adapter structure
+ * @path:	rx path to set values are 0-based
+ */
+void mt7601u_set_rx_path(struct mt7601u_dev *dev, u8 path)
+{
+	mt7601u_bbp_rmw(dev, 3, 0x18, path << 3);
+}
+
+/**
+ * mt7601u_set_tx_dac - set which tx DAC to use
+ * @dev:	pointer to adapter structure
+ * @path:	DAC index, values are 0-based
+ */
+void mt7601u_set_tx_dac(struct mt7601u_dev *dev, u8 dac)
+{
+	mt7601u_bbp_rmc(dev, 1, 0x18, dac << 3);
+}
+
+int mt7601u_phy_init(struct mt7601u_dev *dev)
+{
+	int ret;
+
+	dev->rf_pa_mode[0] = mt7601u_rr(dev, MT_RF_PA_MODE_CFG0);
+	dev->rf_pa_mode[1] = mt7601u_rr(dev, MT_RF_PA_MODE_CFG1);
+
+	ret = mt7601u_rf_wr(dev, 0, 12, dev->ee->rf_freq_off);
+	if (ret)
+		return ret;
+	ret = mt7601u_write_reg_pairs(dev, 0, rf_central,
+				      ARRAY_SIZE(rf_central));
+	if (ret)
+		return ret;
+	ret = mt7601u_write_reg_pairs(dev, 0, rf_channel,
+				      ARRAY_SIZE(rf_channel));
+	if (ret)
+		return ret;
+	ret = mt7601u_write_reg_pairs(dev, 0, rf_vga, ARRAY_SIZE(rf_vga));
+	if (ret)
+		return ret;
+
+	ret = mt7601u_init_cal(dev);
+	if (ret)
+		return ret;
+
+	dev->prev_pwr_diff = 100;
+
+	INIT_DELAYED_WORK(&dev->cal_work, mt7601u_phy_calibrate);
+	INIT_DELAYED_WORK(&dev->freq_cal.work, mt7601u_phy_freq_cal);
+
+	return 0;
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/regs.h b/drivers/net/wireless/mediatek/mt7601u/regs.h
new file mode 100644
index 000000000000..afd8978e83fa
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/regs.h
@@ -0,0 +1,636 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef __MT76_REGS_H
+#define __MT76_REGS_H
+
+#include <linux/bitops.h>
+
+#ifndef GENMASK
+#define GENMASK(h, l)       (((U32_C(1) << ((h) - (l) + 1)) - 1) << (l))
+#endif
+
+#define MT_ASIC_VERSION			0x0000
+
+#define MT76XX_REV_E3		0x22
+#define MT76XX_REV_E4		0x33
+
+#define MT_CMB_CTRL			0x0020
+#define MT_CMB_CTRL_XTAL_RDY		BIT(22)
+#define MT_CMB_CTRL_PLL_LD		BIT(23)
+
+#define MT_EFUSE_CTRL			0x0024
+#define MT_EFUSE_CTRL_AOUT		GENMASK(5, 0)
+#define MT_EFUSE_CTRL_MODE		GENMASK(7, 6)
+#define MT_EFUSE_CTRL_LDO_OFF_TIME	GENMASK(13, 8)
+#define MT_EFUSE_CTRL_LDO_ON_TIME	GENMASK(15, 14)
+#define MT_EFUSE_CTRL_AIN		GENMASK(25, 16)
+#define MT_EFUSE_CTRL_KICK		BIT(30)
+#define MT_EFUSE_CTRL_SEL		BIT(31)
+
+#define MT_EFUSE_DATA_BASE		0x0028
+#define MT_EFUSE_DATA(_n)		(MT_EFUSE_DATA_BASE + ((_n) << 2))
+
+#define MT_COEXCFG0			0x0040
+#define MT_COEXCFG0_COEX_EN		BIT(0)
+
+#define MT_WLAN_FUN_CTRL		0x0080
+#define MT_WLAN_FUN_CTRL_WLAN_EN	BIT(0)
+#define MT_WLAN_FUN_CTRL_WLAN_CLK_EN	BIT(1)
+#define MT_WLAN_FUN_CTRL_WLAN_RESET_RF	BIT(2)
+
+#define MT_WLAN_FUN_CTRL_WLAN_RESET	BIT(3) /* MT76x0 */
+#define MT_WLAN_FUN_CTRL_CSR_F20M_CKEN	BIT(3) /* MT76x2 */
+
+#define MT_WLAN_FUN_CTRL_PCIE_CLK_REQ	BIT(4)
+#define MT_WLAN_FUN_CTRL_FRC_WL_ANT_SEL	BIT(5)
+#define MT_WLAN_FUN_CTRL_INV_ANT_SEL	BIT(6)
+#define MT_WLAN_FUN_CTRL_WAKE_HOST	BIT(7)
+
+#define MT_WLAN_FUN_CTRL_THERM_RST	BIT(8) /* MT76x2 */
+#define MT_WLAN_FUN_CTRL_THERM_CKEN	BIT(9) /* MT76x2 */
+
+#define MT_WLAN_FUN_CTRL_GPIO_IN	GENMASK(15, 8) /* MT76x0 */
+#define MT_WLAN_FUN_CTRL_GPIO_OUT	GENMASK(23, 16) /* MT76x0 */
+#define MT_WLAN_FUN_CTRL_GPIO_OUT_EN	GENMASK(31, 24) /* MT76x0 */
+
+#define MT_XO_CTRL0			0x0100
+#define MT_XO_CTRL1			0x0104
+#define MT_XO_CTRL2			0x0108
+#define MT_XO_CTRL3			0x010c
+#define MT_XO_CTRL4			0x0110
+
+#define MT_XO_CTRL5			0x0114
+#define MT_XO_CTRL5_C2_VAL		GENMASK(14, 8)
+
+#define MT_XO_CTRL6			0x0118
+#define MT_XO_CTRL6_C2_CTRL		GENMASK(14, 8)
+
+#define MT_XO_CTRL7			0x011c
+
+#define MT_WLAN_MTC_CTRL		0x10148
+#define MT_WLAN_MTC_CTRL_MTCMOS_PWR_UP	BIT(0)
+#define MT_WLAN_MTC_CTRL_PWR_ACK	BIT(12)
+#define MT_WLAN_MTC_CTRL_PWR_ACK_S	BIT(13)
+#define MT_WLAN_MTC_CTRL_BBP_MEM_PD	GENMASK(19, 16)
+#define MT_WLAN_MTC_CTRL_PBF_MEM_PD	BIT(20)
+#define MT_WLAN_MTC_CTRL_FCE_MEM_PD	BIT(21)
+#define MT_WLAN_MTC_CTRL_TSO_MEM_PD	BIT(22)
+#define MT_WLAN_MTC_CTRL_BBP_MEM_RB	BIT(24)
+#define MT_WLAN_MTC_CTRL_PBF_MEM_RB	BIT(25)
+#define MT_WLAN_MTC_CTRL_FCE_MEM_RB	BIT(26)
+#define MT_WLAN_MTC_CTRL_TSO_MEM_RB	BIT(27)
+#define MT_WLAN_MTC_CTRL_STATE_UP	BIT(28)
+
+#define MT_INT_SOURCE_CSR		0x0200
+#define MT_INT_MASK_CSR			0x0204
+
+#define MT_INT_RX_DONE(_n)		BIT(_n)
+#define MT_INT_RX_DONE_ALL		GENMASK(1, 0)
+#define MT_INT_TX_DONE_ALL		GENMASK(13, 4)
+#define MT_INT_TX_DONE(_n)		BIT(_n + 4)
+#define MT_INT_RX_COHERENT		BIT(16)
+#define MT_INT_TX_COHERENT		BIT(17)
+#define MT_INT_ANY_COHERENT		BIT(18)
+#define MT_INT_MCU_CMD			BIT(19)
+#define MT_INT_TBTT			BIT(20)
+#define MT_INT_PRE_TBTT			BIT(21)
+#define MT_INT_TX_STAT			BIT(22)
+#define MT_INT_AUTO_WAKEUP		BIT(23)
+#define MT_INT_GPTIMER			BIT(24)
+#define MT_INT_RXDELAYINT		BIT(26)
+#define MT_INT_TXDELAYINT		BIT(27)
+
+#define MT_WPDMA_GLO_CFG		0x0208
+#define MT_WPDMA_GLO_CFG_TX_DMA_EN	BIT(0)
+#define MT_WPDMA_GLO_CFG_TX_DMA_BUSY	BIT(1)
+#define MT_WPDMA_GLO_CFG_RX_DMA_EN	BIT(2)
+#define MT_WPDMA_GLO_CFG_RX_DMA_BUSY	BIT(3)
+#define MT_WPDMA_GLO_CFG_DMA_BURST_SIZE	GENMASK(5, 4)
+#define MT_WPDMA_GLO_CFG_TX_WRITEBACK_DONE	BIT(6)
+#define MT_WPDMA_GLO_CFG_BIG_ENDIAN	BIT(7)
+#define MT_WPDMA_GLO_CFG_HDR_SEG_LEN	GENMASK(15, 8)
+#define MT_WPDMA_GLO_CFG_CLK_GATE_DIS	BIT(30)
+#define MT_WPDMA_GLO_CFG_RX_2B_OFFSET	BIT(31)
+
+#define MT_WPDMA_RST_IDX		0x020c
+
+#define MT_WPDMA_DELAY_INT_CFG		0x0210
+
+#define MT_WMM_AIFSN		0x0214
+#define MT_WMM_AIFSN_MASK		GENMASK(3, 0)
+#define MT_WMM_AIFSN_SHIFT(_n)		((_n) * 4)
+
+#define MT_WMM_CWMIN		0x0218
+#define MT_WMM_CWMIN_MASK		GENMASK(3, 0)
+#define MT_WMM_CWMIN_SHIFT(_n)		((_n) * 4)
+
+#define MT_WMM_CWMAX		0x021c
+#define MT_WMM_CWMAX_MASK		GENMASK(3, 0)
+#define MT_WMM_CWMAX_SHIFT(_n)		((_n) * 4)
+
+#define MT_WMM_TXOP_BASE		0x0220
+#define MT_WMM_TXOP(_n)			(MT_WMM_TXOP_BASE + (((_n) / 2) << 2))
+#define MT_WMM_TXOP_SHIFT(_n)		((_n & 1) * 16)
+#define MT_WMM_TXOP_MASK		GENMASK(15, 0)
+
+#define MT_FCE_DMA_ADDR			0x0230
+#define MT_FCE_DMA_LEN			0x0234
+
+#define MT_USB_DMA_CFG			0x238
+#define MT_USB_DMA_CFG_RX_BULK_AGG_TOUT	GENMASK(7, 0)
+#define MT_USB_DMA_CFG_RX_BULK_AGG_LMT	GENMASK(15, 8)
+#define MT_USB_DMA_CFG_PHY_CLR		BIT(16)
+#define MT_USB_DMA_CFG_TX_CLR		BIT(19)
+#define MT_USB_DMA_CFG_TXOP_HALT	BIT(20)
+#define MT_USB_DMA_CFG_RX_BULK_AGG_EN	BIT(21)
+#define MT_USB_DMA_CFG_RX_BULK_EN	BIT(22)
+#define MT_USB_DMA_CFG_TX_BULK_EN	BIT(23)
+#define MT_USB_DMA_CFG_UDMA_RX_WL_DROP	BIT(25)
+#define MT_USB_DMA_CFG_EP_OUT_VALID	GENMASK(29, 27)
+#define MT_USB_DMA_CFG_RX_BUSY		BIT(30)
+#define MT_USB_DMA_CFG_TX_BUSY		BIT(31)
+
+#define MT_TSO_CTRL			0x0250
+#define MT_HEADER_TRANS_CTRL_REG	0x0260
+
+#define MT_US_CYC_CFG			0x02a4
+#define MT_US_CYC_CNT			GENMASK(7, 0)
+
+#define MT_TX_RING_BASE			0x0300
+#define MT_RX_RING_BASE			0x03c0
+#define MT_RING_SIZE			0x10
+
+#define MT_TX_HW_QUEUE_MCU		8
+#define MT_TX_HW_QUEUE_MGMT		9
+
+#define MT_PBF_SYS_CTRL			0x0400
+#define MT_PBF_SYS_CTRL_MCU_RESET	BIT(0)
+#define MT_PBF_SYS_CTRL_DMA_RESET	BIT(1)
+#define MT_PBF_SYS_CTRL_MAC_RESET	BIT(2)
+#define MT_PBF_SYS_CTRL_PBF_RESET	BIT(3)
+#define MT_PBF_SYS_CTRL_ASY_RESET	BIT(4)
+
+#define MT_PBF_CFG			0x0404
+#define MT_PBF_CFG_TX0Q_EN		BIT(0)
+#define MT_PBF_CFG_TX1Q_EN		BIT(1)
+#define MT_PBF_CFG_TX2Q_EN		BIT(2)
+#define MT_PBF_CFG_TX3Q_EN		BIT(3)
+#define MT_PBF_CFG_RX0Q_EN		BIT(4)
+#define MT_PBF_CFG_RX_DROP_EN		BIT(8)
+
+#define MT_PBF_TX_MAX_PCNT		0x0408
+#define MT_PBF_RX_MAX_PCNT		0x040c
+
+#define MT_BCN_OFFSET_BASE		0x041c
+#define MT_BCN_OFFSET(_n)		(MT_BCN_OFFSET_BASE + ((_n) << 2))
+
+#define	MT_RF_CSR_CFG			0x0500
+#define MT_RF_CSR_CFG_DATA		GENMASK(7, 0)
+#define MT_RF_CSR_CFG_REG_ID		GENMASK(13, 8)
+#define MT_RF_CSR_CFG_REG_BANK		GENMASK(17, 14)
+#define MT_RF_CSR_CFG_WR		BIT(30)
+#define MT_RF_CSR_CFG_KICK		BIT(31)
+
+#define MT_RF_BYPASS_0			0x0504
+#define MT_RF_BYPASS_1			0x0508
+#define MT_RF_SETTING_0			0x050c
+
+#define MT_RF_DATA_WRITE		0x0524
+
+#define MT_RF_CTRL			0x0528
+#define MT_RF_CTRL_ADDR			GENMASK(11, 0)
+#define MT_RF_CTRL_WRITE		BIT(12)
+#define MT_RF_CTRL_BUSY			BIT(13)
+#define MT_RF_CTRL_IDX			BIT(16)
+
+#define MT_RF_DATA_READ			0x052c
+
+#define MT_FCE_PSE_CTRL			0x0800
+#define MT_FCE_PARAMETERS		0x0804
+#define MT_FCE_CSO			0x0808
+
+#define MT_FCE_L2_STUFF			0x080c
+#define MT_FCE_L2_STUFF_HT_L2_EN	BIT(0)
+#define MT_FCE_L2_STUFF_QOS_L2_EN	BIT(1)
+#define MT_FCE_L2_STUFF_RX_STUFF_EN	BIT(2)
+#define MT_FCE_L2_STUFF_TX_STUFF_EN	BIT(3)
+#define MT_FCE_L2_STUFF_WR_MPDU_LEN_EN	BIT(4)
+#define MT_FCE_L2_STUFF_MVINV_BSWAP	BIT(5)
+#define MT_FCE_L2_STUFF_TS_CMD_QSEL_EN	GENMASK(15, 8)
+#define MT_FCE_L2_STUFF_TS_LEN_EN	GENMASK(23, 16)
+#define MT_FCE_L2_STUFF_OTHER_PORT	GENMASK(25, 24)
+
+#define MT_FCE_WLAN_FLOW_CONTROL1	0x0824
+
+#define MT_TX_CPU_FROM_FCE_BASE_PTR	0x09a0
+#define MT_TX_CPU_FROM_FCE_MAX_COUNT	0x09a4
+#define MT_TX_CPU_FROM_FCE_CPU_DESC_IDX	0x09a8
+
+#define MT_FCE_PDMA_GLOBAL_CONF		0x09c4
+
+#define MT_PAUSE_ENABLE_CONTROL1	0x0a38
+
+#define MT_FCE_SKIP_FS			0x0a6c
+
+#define MT_MAC_CSR0			0x1000
+
+#define MT_MAC_SYS_CTRL			0x1004
+#define MT_MAC_SYS_CTRL_RESET_CSR	BIT(0)
+#define MT_MAC_SYS_CTRL_RESET_BBP	BIT(1)
+#define MT_MAC_SYS_CTRL_ENABLE_TX	BIT(2)
+#define MT_MAC_SYS_CTRL_ENABLE_RX	BIT(3)
+
+#define MT_MAC_ADDR_DW0			0x1008
+#define MT_MAC_ADDR_DW1			0x100c
+#define MT_MAC_ADDR_DW1_U2ME_MASK	GENMASK(23, 16)
+
+#define MT_MAC_BSSID_DW0		0x1010
+#define MT_MAC_BSSID_DW1		0x1014
+#define MT_MAC_BSSID_DW1_ADDR		GENMASK(15, 0)
+#define MT_MAC_BSSID_DW1_MBSS_MODE	GENMASK(17, 16)
+#define MT_MAC_BSSID_DW1_MBEACON_N	GENMASK(20, 18)
+#define MT_MAC_BSSID_DW1_MBSS_LOCAL_BIT	BIT(21)
+#define MT_MAC_BSSID_DW1_MBSS_MODE_B2	BIT(22)
+#define MT_MAC_BSSID_DW1_MBEACON_N_B3	BIT(23)
+#define MT_MAC_BSSID_DW1_MBSS_IDX_BYTE	GENMASK(26, 24)
+
+#define MT_MAX_LEN_CFG			0x1018
+#define MT_MAX_LEN_CFG_AMPDU		GENMASK(13, 12)
+
+#define MT_BBP_CSR_CFG			0x101c
+#define MT_BBP_CSR_CFG_VAL		GENMASK(7, 0)
+#define MT_BBP_CSR_CFG_REG_NUM		GENMASK(15, 8)
+#define MT_BBP_CSR_CFG_READ		BIT(16)
+#define MT_BBP_CSR_CFG_BUSY		BIT(17)
+#define MT_BBP_CSR_CFG_PAR_DUR		BIT(18)
+#define MT_BBP_CSR_CFG_RW_MODE		BIT(19)
+
+#define MT_AMPDU_MAX_LEN_20M1S		0x1030
+#define MT_AMPDU_MAX_LEN_20M2S		0x1034
+#define MT_AMPDU_MAX_LEN_40M1S		0x1038
+#define MT_AMPDU_MAX_LEN_40M2S		0x103c
+#define MT_AMPDU_MAX_LEN		0x1040
+
+#define MT_WCID_DROP_BASE		0x106c
+#define MT_WCID_DROP(_n)		(MT_WCID_DROP_BASE + ((_n) >> 5) * 4)
+#define MT_WCID_DROP_MASK(_n)		BIT((_n) % 32)
+
+#define MT_BCN_BYPASS_MASK		0x108c
+
+#define MT_MAC_APC_BSSID_BASE		0x1090
+#define MT_MAC_APC_BSSID_L(_n)		(MT_MAC_APC_BSSID_BASE + ((_n) * 8))
+#define MT_MAC_APC_BSSID_H(_n)		(MT_MAC_APC_BSSID_BASE + ((_n) * 8 + 4))
+#define MT_MAC_APC_BSSID_H_ADDR		GENMASK(15, 0)
+#define MT_MAC_APC_BSSID0_H_EN		BIT(16)
+
+#define MT_XIFS_TIME_CFG		0x1100
+#define MT_XIFS_TIME_CFG_CCK_SIFS	GENMASK(7, 0)
+#define MT_XIFS_TIME_CFG_OFDM_SIFS	GENMASK(15, 8)
+#define MT_XIFS_TIME_CFG_OFDM_XIFS	GENMASK(19, 16)
+#define MT_XIFS_TIME_CFG_EIFS		GENMASK(28, 20)
+#define MT_XIFS_TIME_CFG_BB_RXEND_EN	BIT(29)
+
+#define MT_BKOFF_SLOT_CFG		0x1104
+#define MT_BKOFF_SLOT_CFG_SLOTTIME	GENMASK(7, 0)
+#define MT_BKOFF_SLOT_CFG_CC_DELAY	GENMASK(11, 8)
+
+#define MT_BEACON_TIME_CFG		0x1114
+#define MT_BEACON_TIME_CFG_INTVAL	GENMASK(15, 0)
+#define MT_BEACON_TIME_CFG_TIMER_EN	BIT(16)
+#define MT_BEACON_TIME_CFG_SYNC_MODE	GENMASK(18, 17)
+#define MT_BEACON_TIME_CFG_TBTT_EN	BIT(19)
+#define MT_BEACON_TIME_CFG_BEACON_TX	BIT(20)
+#define MT_BEACON_TIME_CFG_TSF_COMP	GENMASK(31, 24)
+
+#define MT_TBTT_SYNC_CFG		0x1118
+#define MT_TBTT_TIMER_CFG		0x1124
+
+#define MT_INT_TIMER_CFG		0x1128
+#define MT_INT_TIMER_CFG_PRE_TBTT	GENMASK(15, 0)
+#define MT_INT_TIMER_CFG_GP_TIMER	GENMASK(31, 16)
+
+#define MT_INT_TIMER_EN			0x112c
+#define MT_INT_TIMER_EN_PRE_TBTT_EN	BIT(0)
+#define MT_INT_TIMER_EN_GP_TIMER_EN	BIT(1)
+
+#define MT_MAC_STATUS			0x1200
+#define MT_MAC_STATUS_TX		BIT(0)
+#define MT_MAC_STATUS_RX		BIT(1)
+
+#define MT_PWR_PIN_CFG			0x1204
+#define MT_AUX_CLK_CFG			0x120c
+
+#define MT_BB_PA_MODE_CFG0		0x1214
+#define MT_BB_PA_MODE_CFG1		0x1218
+#define MT_RF_PA_MODE_CFG0		0x121c
+#define MT_RF_PA_MODE_CFG1		0x1220
+
+#define MT_RF_PA_MODE_ADJ0		0x1228
+#define MT_RF_PA_MODE_ADJ1		0x122c
+
+#define MT_DACCLK_EN_DLY_CFG		0x1264
+
+#define MT_EDCA_CFG_BASE		0x1300
+#define MT_EDCA_CFG_AC(_n)		(MT_EDCA_CFG_BASE + ((_n) << 2))
+#define MT_EDCA_CFG_TXOP		GENMASK(7, 0)
+#define MT_EDCA_CFG_AIFSN		GENMASK(11, 8)
+#define MT_EDCA_CFG_CWMIN		GENMASK(15, 12)
+#define MT_EDCA_CFG_CWMAX		GENMASK(19, 16)
+
+#define MT_TX_PWR_CFG_0			0x1314
+#define MT_TX_PWR_CFG_1			0x1318
+#define MT_TX_PWR_CFG_2			0x131c
+#define MT_TX_PWR_CFG_3			0x1320
+#define MT_TX_PWR_CFG_4			0x1324
+
+#define MT_TX_BAND_CFG			0x132c
+#define MT_TX_BAND_CFG_UPPER_40M	BIT(0)
+#define MT_TX_BAND_CFG_5G		BIT(1)
+#define MT_TX_BAND_CFG_2G		BIT(2)
+
+#define MT_HT_FBK_TO_LEGACY		0x1384
+#define MT_TX_MPDU_ADJ_INT		0x1388
+
+#define MT_TX_PWR_CFG_7			0x13d4
+#define MT_TX_PWR_CFG_8			0x13d8
+#define MT_TX_PWR_CFG_9			0x13dc
+
+#define MT_TX_SW_CFG0			0x1330
+#define MT_TX_SW_CFG1			0x1334
+#define MT_TX_SW_CFG2			0x1338
+
+#define MT_TXOP_CTRL_CFG		0x1340
+#define MT_TXOP_TRUN_EN			GENMASK(5, 0)
+#define MT_TXOP_EXT_CCA_DLY		GENMASK(15, 8)
+#define MT_TXOP_CTRL
+
+#define MT_TX_RTS_CFG			0x1344
+#define MT_TX_RTS_CFG_RETRY_LIMIT	GENMASK(7, 0)
+#define MT_TX_RTS_CFG_THRESH		GENMASK(23, 8)
+#define MT_TX_RTS_FALLBACK		BIT(24)
+
+#define MT_TX_TIMEOUT_CFG		0x1348
+#define MT_TX_RETRY_CFG			0x134c
+#define MT_TX_LINK_CFG			0x1350
+#define MT_HT_FBK_CFG0			0x1354
+#define MT_HT_FBK_CFG1			0x1358
+#define MT_LG_FBK_CFG0			0x135c
+#define MT_LG_FBK_CFG1			0x1360
+
+#define MT_CCK_PROT_CFG			0x1364
+#define MT_OFDM_PROT_CFG		0x1368
+#define MT_MM20_PROT_CFG		0x136c
+#define MT_MM40_PROT_CFG		0x1370
+#define MT_GF20_PROT_CFG		0x1374
+#define MT_GF40_PROT_CFG		0x1378
+
+#define MT_PROT_RATE			GENMASK(15, 0)
+#define MT_PROT_CTRL_RTS_CTS		BIT(16)
+#define MT_PROT_CTRL_CTS2SELF		BIT(17)
+#define MT_PROT_NAV_SHORT		BIT(18)
+#define MT_PROT_NAV_LONG		BIT(19)
+#define MT_PROT_TXOP_ALLOW_CCK		BIT(20)
+#define MT_PROT_TXOP_ALLOW_OFDM		BIT(21)
+#define MT_PROT_TXOP_ALLOW_MM20		BIT(22)
+#define MT_PROT_TXOP_ALLOW_MM40		BIT(23)
+#define MT_PROT_TXOP_ALLOW_GF20		BIT(24)
+#define MT_PROT_TXOP_ALLOW_GF40		BIT(25)
+#define MT_PROT_RTS_THR_EN		BIT(26)
+#define MT_PROT_RATE_CCK_11		0x0003
+#define MT_PROT_RATE_OFDM_6		0x4000
+#define MT_PROT_RATE_OFDM_24		0x4004
+#define MT_PROT_RATE_DUP_OFDM_24	0x4084
+#define MT_PROT_TXOP_ALLOW_ALL		GENMASK(25, 20)
+#define MT_PROT_TXOP_ALLOW_BW20		(MT_PROT_TXOP_ALLOW_ALL &	\
+					 ~MT_PROT_TXOP_ALLOW_MM40 &	\
+					 ~MT_PROT_TXOP_ALLOW_GF40)
+
+#define MT_EXP_ACK_TIME			0x1380
+
+#define MT_TX_PWR_CFG_0_EXT		0x1390
+#define MT_TX_PWR_CFG_1_EXT		0x1394
+
+#define MT_TX_FBK_LIMIT			0x1398
+#define MT_TX_FBK_LIMIT_MPDU_FBK	GENMASK(7, 0)
+#define MT_TX_FBK_LIMIT_AMPDU_FBK	GENMASK(15, 8)
+#define MT_TX_FBK_LIMIT_MPDU_UP_CLEAR	BIT(16)
+#define MT_TX_FBK_LIMIT_AMPDU_UP_CLEAR	BIT(17)
+#define MT_TX_FBK_LIMIT_RATE_LUT	BIT(18)
+
+#define MT_TX0_RF_GAIN_CORR		0x13a0
+#define MT_TX1_RF_GAIN_CORR		0x13a4
+#define MT_TX0_RF_GAIN_ATTEN		0x13a8
+
+#define MT_TX_ALC_CFG_0			0x13b0
+#define MT_TX_ALC_CFG_0_CH_INIT_0	GENMASK(5, 0)
+#define MT_TX_ALC_CFG_0_CH_INIT_1	GENMASK(13, 8)
+#define MT_TX_ALC_CFG_0_LIMIT_0		GENMASK(21, 16)
+#define MT_TX_ALC_CFG_0_LIMIT_1		GENMASK(29, 24)
+
+#define MT_TX_ALC_CFG_1			0x13b4
+#define MT_TX_ALC_CFG_1_TEMP_COMP	GENMASK(5, 0)
+
+#define MT_TX_ALC_CFG_2			0x13a8
+#define MT_TX_ALC_CFG_2_TEMP_COMP	GENMASK(5, 0)
+
+#define MT_TX0_BB_GAIN_ATTEN		0x13c0
+
+#define MT_TX_ALC_VGA3			0x13c8
+
+#define MT_TX_PROT_CFG6			0x13e0
+#define MT_TX_PROT_CFG7			0x13e4
+#define MT_TX_PROT_CFG8			0x13e8
+
+#define MT_PIFS_TX_CFG			0x13ec
+
+#define MT_RX_FILTR_CFG			0x1400
+
+#define MT_RX_FILTR_CFG_CRC_ERR		BIT(0)
+#define MT_RX_FILTR_CFG_PHY_ERR		BIT(1)
+#define MT_RX_FILTR_CFG_PROMISC		BIT(2)
+#define MT_RX_FILTR_CFG_OTHER_BSS	BIT(3)
+#define MT_RX_FILTR_CFG_VER_ERR		BIT(4)
+#define MT_RX_FILTR_CFG_MCAST		BIT(5)
+#define MT_RX_FILTR_CFG_BCAST		BIT(6)
+#define MT_RX_FILTR_CFG_DUP		BIT(7)
+#define MT_RX_FILTR_CFG_CFACK		BIT(8)
+#define MT_RX_FILTR_CFG_CFEND		BIT(9)
+#define MT_RX_FILTR_CFG_ACK		BIT(10)
+#define MT_RX_FILTR_CFG_CTS		BIT(11)
+#define MT_RX_FILTR_CFG_RTS		BIT(12)
+#define MT_RX_FILTR_CFG_PSPOLL		BIT(13)
+#define MT_RX_FILTR_CFG_BA		BIT(14)
+#define MT_RX_FILTR_CFG_BAR		BIT(15)
+#define MT_RX_FILTR_CFG_CTRL_RSV	BIT(16)
+
+#define MT_AUTO_RSP_CFG			0x1404
+
+#define MT_AUTO_RSP_PREAMB_SHORT	BIT(4)
+
+#define MT_LEGACY_BASIC_RATE		0x1408
+#define MT_HT_BASIC_RATE		0x140c
+
+#define MT_RX_PARSER_CFG		0x1418
+#define MT_RX_PARSER_RX_SET_NAV_ALL	BIT(0)
+
+#define MT_EXT_CCA_CFG			0x141c
+#define MT_EXT_CCA_CFG_CCA0		GENMASK(1, 0)
+#define MT_EXT_CCA_CFG_CCA1		GENMASK(3, 2)
+#define MT_EXT_CCA_CFG_CCA2		GENMASK(5, 4)
+#define MT_EXT_CCA_CFG_CCA3		GENMASK(7, 6)
+#define MT_EXT_CCA_CFG_CCA_MASK		GENMASK(11, 8)
+#define MT_EXT_CCA_CFG_ED_CCA_MASK	GENMASK(15, 12)
+
+#define MT_TX_SW_CFG3			0x1478
+
+#define MT_PN_PAD_MODE			0x150c
+
+#define MT_TXOP_HLDR_ET			0x1608
+
+#define MT_PROT_AUTO_TX_CFG		0x1648
+
+#define MT_RX_STA_CNT0			0x1700
+#define MT_RX_STA_CNT1			0x1704
+#define MT_RX_STA_CNT2			0x1708
+#define MT_TX_STA_CNT0			0x170c
+#define MT_TX_STA_CNT1			0x1710
+#define MT_TX_STA_CNT2			0x1714
+
+/* Vendor driver defines content of the second word of STAT_FIFO as follows:
+ *	MT_TX_STAT_FIFO_RATE		GENMASK(26, 16)
+ *	MT_TX_STAT_FIFO_ETXBF		BIT(27)
+ *	MT_TX_STAT_FIFO_SND		BIT(28)
+ *	MT_TX_STAT_FIFO_ITXBF		BIT(29)
+ * However, tests show that b16-31 have the same layout as TXWI rate_ctl
+ * with rate set to rate at which frame was acked.
+ */
+#define MT_TX_STAT_FIFO			0x1718
+#define MT_TX_STAT_FIFO_VALID		BIT(0)
+#define MT_TX_STAT_FIFO_PID_TYPE	GENMASK(4, 1)
+#define MT_TX_STAT_FIFO_SUCCESS		BIT(5)
+#define MT_TX_STAT_FIFO_AGGR		BIT(6)
+#define MT_TX_STAT_FIFO_ACKREQ		BIT(7)
+#define MT_TX_STAT_FIFO_WCID		GENMASK(15, 8)
+#define MT_TX_STAT_FIFO_RATE		GENMASK(31, 16)
+
+#define MT_TX_AGG_STAT			0x171c
+
+#define MT_TX_AGG_CNT_BASE0		0x1720
+
+#define MT_MPDU_DENSITY_CNT		0x1740
+
+#define MT_TX_AGG_CNT_BASE1		0x174c
+
+#define MT_TX_AGG_CNT(_id)		((_id) < 8 ?			\
+					 MT_TX_AGG_CNT_BASE0 + ((_id) << 2) : \
+					 MT_TX_AGG_CNT_BASE1 + ((_id - 8) << 2))
+
+#define MT_TX_STAT_FIFO_EXT		0x1798
+#define MT_TX_STAT_FIFO_EXT_RETRY	GENMASK(7, 0)
+
+#define MT_BBP_CORE_BASE		0x2000
+#define MT_BBP_IBI_BASE			0x2100
+#define MT_BBP_AGC_BASE			0x2300
+#define MT_BBP_TXC_BASE			0x2400
+#define MT_BBP_RXC_BASE			0x2500
+#define MT_BBP_TXO_BASE			0x2600
+#define MT_BBP_TXBE_BASE		0x2700
+#define MT_BBP_RXFE_BASE		0x2800
+#define MT_BBP_RXO_BASE			0x2900
+#define MT_BBP_DFS_BASE			0x2a00
+#define MT_BBP_TR_BASE			0x2b00
+#define MT_BBP_CAL_BASE			0x2c00
+#define MT_BBP_DSC_BASE			0x2e00
+#define MT_BBP_PFMU_BASE		0x2f00
+
+#define MT_BBP(_type, _n)		(MT_BBP_##_type##_BASE + ((_n) << 2))
+
+#define MT_BBP_CORE_R1_BW		GENMASK(4, 3)
+
+#define MT_BBP_AGC_R0_CTRL_CHAN		GENMASK(9, 8)
+#define MT_BBP_AGC_R0_BW		GENMASK(14, 12)
+
+/* AGC, R4/R5 */
+#define MT_BBP_AGC_LNA_GAIN		GENMASK(21, 16)
+
+/* AGC, R8/R9 */
+#define MT_BBP_AGC_GAIN			GENMASK(14, 8)
+
+#define MT_BBP_AGC20_RSSI0		GENMASK(7, 0)
+#define MT_BBP_AGC20_RSSI1		GENMASK(15, 8)
+
+#define MT_BBP_TXBE_R0_CTRL_CHAN	GENMASK(1, 0)
+
+#define MT_WCID_ADDR_BASE		0x1800
+#define MT_WCID_ADDR(_n)		(MT_WCID_ADDR_BASE + (_n) * 8)
+
+#define MT_SRAM_BASE			0x4000
+
+#define MT_WCID_KEY_BASE		0x8000
+#define MT_WCID_KEY(_n)			(MT_WCID_KEY_BASE + (_n) * 32)
+
+#define MT_WCID_IV_BASE			0xa000
+#define MT_WCID_IV(_n)			(MT_WCID_IV_BASE + (_n) * 8)
+
+#define MT_WCID_ATTR_BASE		0xa800
+#define MT_WCID_ATTR(_n)		(MT_WCID_ATTR_BASE + (_n) * 4)
+
+#define MT_WCID_ATTR_PAIRWISE		BIT(0)
+#define MT_WCID_ATTR_PKEY_MODE		GENMASK(3, 1)
+#define MT_WCID_ATTR_BSS_IDX		GENMASK(6, 4)
+#define MT_WCID_ATTR_RXWI_UDF		GENMASK(9, 7)
+#define MT_WCID_ATTR_PKEY_MODE_EXT	BIT(10)
+#define MT_WCID_ATTR_BSS_IDX_EXT	BIT(11)
+#define MT_WCID_ATTR_WAPI_MCBC		BIT(15)
+#define MT_WCID_ATTR_WAPI_KEYID		GENMASK(31, 24)
+
+#define MT_SKEY_BASE_0			0xac00
+#define MT_SKEY_BASE_1			0xb400
+#define MT_SKEY_0(_bss, _idx)		\
+	(MT_SKEY_BASE_0 + (4 * (_bss) + _idx) * 32)
+#define MT_SKEY_1(_bss, _idx)		\
+	(MT_SKEY_BASE_1 + (4 * ((_bss) & 7) + _idx) * 32)
+#define MT_SKEY(_bss, _idx)		\
+	((_bss & 8) ? MT_SKEY_1(_bss, _idx) : MT_SKEY_0(_bss, _idx))
+
+#define MT_SKEY_MODE_BASE_0		0xb000
+#define MT_SKEY_MODE_BASE_1		0xb3f0
+#define MT_SKEY_MODE_0(_bss)		\
+	(MT_SKEY_MODE_BASE_0 + ((_bss / 2) << 2))
+#define MT_SKEY_MODE_1(_bss)		\
+	(MT_SKEY_MODE_BASE_1 + ((((_bss) & 7) / 2) << 2))
+#define MT_SKEY_MODE(_bss)		\
+	((_bss & 8) ? MT_SKEY_MODE_1(_bss) : MT_SKEY_MODE_0(_bss))
+#define MT_SKEY_MODE_MASK		GENMASK(3, 0)
+#define MT_SKEY_MODE_SHIFT(_bss, _idx)	(4 * ((_idx) + 4 * (_bss & 1)))
+
+#define MT_BEACON_BASE			0xc000
+
+#define MT_TEMP_SENSOR			0x1d000
+#define MT_TEMP_SENSOR_VAL		GENMASK(6, 0)
+
+enum mt76_cipher_type {
+	MT_CIPHER_NONE,
+	MT_CIPHER_WEP40,
+	MT_CIPHER_WEP104,
+	MT_CIPHER_TKIP,
+	MT_CIPHER_AES_CCMP,
+	MT_CIPHER_CKIP40,
+	MT_CIPHER_CKIP104,
+	MT_CIPHER_CKIP128,
+	MT_CIPHER_WAPI,
+};
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/trace.c b/drivers/net/wireless/mediatek/mt7601u/trace.c
new file mode 100644
index 000000000000..8abdd3cd546d
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/trace.c
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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/module.h>
+
+#ifndef __CHECKER__
+#define CREATE_TRACE_POINTS
+#include "trace.h"
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/trace.h b/drivers/net/wireless/mediatek/mt7601u/trace.h
new file mode 100644
index 000000000000..289897300ef0
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/trace.h
@@ -0,0 +1,400 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#if !defined(__MT7601U_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
+#define __MT7601U_TRACE_H
+
+#include <linux/tracepoint.h>
+#include "mt7601u.h"
+#include "mac.h"
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM mt7601u
+
+#define MAXNAME		32
+#define DEV_ENTRY	__array(char, wiphy_name, 32)
+#define DEV_ASSIGN	strlcpy(__entry->wiphy_name,			\
+				wiphy_name(dev->hw->wiphy), MAXNAME)
+#define DEV_PR_FMT	"%s "
+#define DEV_PR_ARG	__entry->wiphy_name
+
+#define REG_ENTRY	__field(u32, reg) __field(u32, val)
+#define REG_ASSIGN	__entry->reg = reg; __entry->val = val
+#define REG_PR_FMT	"%04x=%08x"
+#define REG_PR_ARG	__entry->reg, __entry->val
+
+DECLARE_EVENT_CLASS(dev_reg_evt,
+	TP_PROTO(struct mt7601u_dev *dev, u32 reg, u32 val),
+	TP_ARGS(dev, reg, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		REG_ENTRY
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		REG_ASSIGN;
+	),
+	TP_printk(
+		DEV_PR_FMT REG_PR_FMT,
+		DEV_PR_ARG, REG_PR_ARG
+	)
+);
+
+DEFINE_EVENT(dev_reg_evt, reg_read,
+	TP_PROTO(struct mt7601u_dev *dev, u32 reg, u32 val),
+	TP_ARGS(dev, reg, val)
+);
+
+DEFINE_EVENT(dev_reg_evt, reg_write,
+	TP_PROTO(struct mt7601u_dev *dev, u32 reg, u32 val),
+	TP_ARGS(dev, reg, val)
+);
+
+TRACE_EVENT(mt_submit_urb,
+	TP_PROTO(struct mt7601u_dev *dev, struct urb *u),
+	TP_ARGS(dev, u),
+	TP_STRUCT__entry(
+		DEV_ENTRY __field(unsigned, pipe) __field(u32, len)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->pipe = u->pipe;
+		__entry->len = u->transfer_buffer_length;
+	),
+	TP_printk(DEV_PR_FMT "p:%08x len:%u",
+		  DEV_PR_ARG, __entry->pipe, __entry->len)
+);
+
+#define trace_mt_submit_urb_sync(__dev, __pipe, __len) ({	\
+	struct urb u;					\
+	u.pipe = __pipe;				\
+	u.transfer_buffer_length = __len;		\
+	trace_mt_submit_urb(__dev, &u);			\
+})
+
+TRACE_EVENT(mt_mcu_msg_send,
+	TP_PROTO(struct mt7601u_dev *dev,
+		 struct sk_buff *skb, u32 csum, bool resp),
+	TP_ARGS(dev, skb, csum, resp),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, info)
+		__field(u32, csum)
+		__field(bool, resp)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->info = *(u32 *)skb->data;
+		__entry->csum = csum;
+		__entry->resp = resp;
+	),
+	TP_printk(DEV_PR_FMT "i:%08x c:%08x r:%d",
+		  DEV_PR_ARG, __entry->info, __entry->csum, __entry->resp)
+);
+
+TRACE_EVENT(mt_vend_req,
+	TP_PROTO(struct mt7601u_dev *dev, unsigned pipe, u8 req, u8 req_type,
+		 u16 val, u16 offset, void *buf, size_t buflen, int ret),
+	TP_ARGS(dev, pipe, req, req_type, val, offset, buf, buflen, ret),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(unsigned, pipe) __field(u8, req) __field(u8, req_type)
+		__field(u16, val) __field(u16, offset) __field(void*, buf)
+		__field(int, buflen) __field(int, ret)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->pipe = pipe;
+		__entry->req = req;
+		__entry->req_type = req_type;
+		__entry->val = val;
+		__entry->offset = offset;
+		__entry->buf = buf;
+		__entry->buflen = buflen;
+		__entry->ret = ret;
+	),
+	TP_printk(DEV_PR_FMT
+		  "%d p:%08x req:%02hhx %02hhx val:%04hx %04hx buf:%d %d",
+		  DEV_PR_ARG, __entry->ret, __entry->pipe, __entry->req,
+		  __entry->req_type, __entry->val, __entry->offset,
+		  !!__entry->buf, __entry->buflen)
+);
+
+TRACE_EVENT(ee_read,
+	TP_PROTO(struct mt7601u_dev *dev, int offset, u16 val),
+	TP_ARGS(dev, offset, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(int, o) __field(u16, v)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->o = offset;
+		__entry->v = val;
+	),
+	TP_printk(DEV_PR_FMT "%04x=%04x", DEV_PR_ARG, __entry->o, __entry->v)
+);
+
+DECLARE_EVENT_CLASS(dev_rf_reg_evt,
+	TP_PROTO(struct mt7601u_dev *dev, u8 bank, u8 reg, u8 val),
+	TP_ARGS(dev, bank, reg, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u8, bank)
+		__field(u8, reg)
+		__field(u8, val)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		REG_ASSIGN;
+		__entry->bank = bank;
+	),
+	TP_printk(
+		DEV_PR_FMT "%02hhx:%02hhx=%02hhx",
+		DEV_PR_ARG, __entry->bank, __entry->reg, __entry->val
+	)
+);
+
+DEFINE_EVENT(dev_rf_reg_evt, rf_read,
+	TP_PROTO(struct mt7601u_dev *dev, u8 bank, u8 reg, u8 val),
+	TP_ARGS(dev, bank, reg, val)
+);
+
+DEFINE_EVENT(dev_rf_reg_evt, rf_write,
+	TP_PROTO(struct mt7601u_dev *dev, u8 bank, u8 reg, u8 val),
+	TP_ARGS(dev, bank, reg, val)
+);
+
+DECLARE_EVENT_CLASS(dev_bbp_reg_evt,
+	TP_PROTO(struct mt7601u_dev *dev, u8 reg, u8 val),
+	TP_ARGS(dev, reg, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u8, reg)
+		__field(u8, val)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		REG_ASSIGN;
+	),
+	TP_printk(
+		DEV_PR_FMT "%02hhx=%02hhx",
+		DEV_PR_ARG, __entry->reg, __entry->val
+	)
+);
+
+DEFINE_EVENT(dev_bbp_reg_evt, bbp_read,
+	TP_PROTO(struct mt7601u_dev *dev, u8 reg, u8 val),
+	TP_ARGS(dev, reg, val)
+);
+
+DEFINE_EVENT(dev_bbp_reg_evt, bbp_write,
+	TP_PROTO(struct mt7601u_dev *dev, u8 reg, u8 val),
+	TP_ARGS(dev, reg, val)
+);
+
+DECLARE_EVENT_CLASS(dev_simple_evt,
+	TP_PROTO(struct mt7601u_dev *dev, u8 val),
+	TP_ARGS(dev, val),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u8, val)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->val = val;
+	),
+	TP_printk(
+		DEV_PR_FMT "%02hhx", DEV_PR_ARG, __entry->val
+	)
+);
+
+DEFINE_EVENT(dev_simple_evt, temp_mode,
+	TP_PROTO(struct mt7601u_dev *dev, u8 val),
+	TP_ARGS(dev, val)
+);
+
+DEFINE_EVENT(dev_simple_evt, read_temp,
+	TP_PROTO(struct mt7601u_dev *dev, u8 val),
+	TP_ARGS(dev, val)
+);
+
+DEFINE_EVENT(dev_simple_evt, freq_cal_adjust,
+	TP_PROTO(struct mt7601u_dev *dev, u8 val),
+	TP_ARGS(dev, val)
+);
+
+TRACE_EVENT(freq_cal_offset,
+	TP_PROTO(struct mt7601u_dev *dev, u8 phy_mode, s8 freq_off),
+	TP_ARGS(dev, phy_mode, freq_off),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u8, phy_mode)
+		__field(s8, freq_off)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->phy_mode = phy_mode;
+		__entry->freq_off = freq_off;
+	),
+	TP_printk(DEV_PR_FMT "phy:%02hhx off:%02hhx",
+		  DEV_PR_ARG, __entry->phy_mode, __entry->freq_off)
+);
+
+TRACE_EVENT(mt_rx,
+	TP_PROTO(struct mt7601u_dev *dev, struct mt7601u_rxwi *rxwi, u32 f),
+	TP_ARGS(dev, rxwi, f),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field_struct(struct mt7601u_rxwi, rxwi)
+		__field(u32, fce_info)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->rxwi = *rxwi;
+		__entry->fce_info = f;
+	),
+	TP_printk(DEV_PR_FMT "rxi:%08x ctl:%08x frag_sn:%04hx rate:%04hx "
+		  "uknw:%02hhx z:%02hhx%02hhx%02hhx snr:%02hhx "
+		  "ant:%02hhx gain:%02hhx freq_o:%02hhx "
+		  "r:%08x ea:%08x fce:%08x", DEV_PR_ARG,
+		  le32_to_cpu(__entry->rxwi.rxinfo),
+		  le32_to_cpu(__entry->rxwi.ctl),
+		  le16_to_cpu(__entry->rxwi.frag_sn),
+		  le16_to_cpu(__entry->rxwi.rate),
+		  __entry->rxwi.unknown,
+		  __entry->rxwi.zero[0], __entry->rxwi.zero[1],
+		  __entry->rxwi.zero[2],
+		  __entry->rxwi.snr, __entry->rxwi.ant,
+		  __entry->rxwi.gain, __entry->rxwi.freq_off,
+		  __entry->rxwi.resv2, __entry->rxwi.expert_ant,
+		  __entry->fce_info)
+);
+
+TRACE_EVENT(mt_tx,
+	TP_PROTO(struct mt7601u_dev *dev, struct sk_buff *skb,
+		 struct mt76_sta *sta, struct mt76_txwi *h),
+	TP_ARGS(dev, skb, sta, h),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field_struct(struct mt76_txwi, h)
+		__field(struct sk_buff *, skb)
+		__field(struct mt76_sta *, sta)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->h = *h;
+		__entry->skb = skb;
+		__entry->sta = sta;
+	),
+	TP_printk(DEV_PR_FMT "skb:%p sta:%p  flg:%04hx rate_ctl:%04hx "
+		  "ack:%02hhx wcid:%02hhx len_ctl:%05hx", DEV_PR_ARG,
+		  __entry->skb, __entry->sta,
+		  le16_to_cpu(__entry->h.flags),
+		  le16_to_cpu(__entry->h.rate_ctl),
+		  __entry->h.ack_ctl, __entry->h.wcid,
+		  le16_to_cpu(__entry->h.len_ctl))
+);
+
+TRACE_EVENT(mt_tx_dma_done,
+	TP_PROTO(struct mt7601u_dev *dev, struct sk_buff *skb),
+	TP_ARGS(dev, skb),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(struct sk_buff *, skb)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->skb = skb;
+	),
+	TP_printk(DEV_PR_FMT "%p", DEV_PR_ARG, __entry->skb)
+);
+
+TRACE_EVENT(mt_tx_status_cleaned,
+	TP_PROTO(struct mt7601u_dev *dev, int cleaned),
+	TP_ARGS(dev, cleaned),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(int, cleaned)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->cleaned = cleaned;
+	),
+	TP_printk(DEV_PR_FMT "%d", DEV_PR_ARG, __entry->cleaned)
+);
+
+TRACE_EVENT(mt_tx_status,
+	TP_PROTO(struct mt7601u_dev *dev, u32 stat1, u32 stat2),
+	TP_ARGS(dev, stat1, stat2),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u32, stat1)	__field(u32, stat2)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->stat1 = stat1;
+		__entry->stat2 = stat2;
+	),
+	TP_printk(DEV_PR_FMT "%08x %08x",
+		  DEV_PR_ARG, __entry->stat1, __entry->stat2)
+);
+
+TRACE_EVENT(mt_rx_dma_aggr,
+	TP_PROTO(struct mt7601u_dev *dev, int cnt, bool paged),
+	TP_ARGS(dev, cnt, paged),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u8, cnt)
+		__field(bool, paged)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->cnt = cnt;
+		__entry->paged = paged;
+	),
+	TP_printk(DEV_PR_FMT "cnt:%d paged:%d",
+		  DEV_PR_ARG, __entry->cnt, __entry->paged)
+);
+
+DEFINE_EVENT(dev_simple_evt, set_key,
+	TP_PROTO(struct mt7601u_dev *dev, u8 val),
+	TP_ARGS(dev, val)
+);
+
+TRACE_EVENT(set_shared_key,
+	TP_PROTO(struct mt7601u_dev *dev, u8 vid, u8 key),
+	TP_ARGS(dev, vid, key),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+		__field(u8, vid)
+		__field(u8, key)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		__entry->vid = vid;
+		__entry->key = key;
+	),
+	TP_printk(DEV_PR_FMT "phy:%02hhx off:%02hhx",
+		  DEV_PR_ARG, __entry->vid, __entry->key)
+);
+
+#endif
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/mediatek/mt7601u/tx.c b/drivers/net/wireless/mediatek/mt7601u/tx.c
new file mode 100644
index 000000000000..0be2080ceab3
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/tx.c
@@ -0,0 +1,319 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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 "mt7601u.h"
+#include "trace.h"
+
+enum mt76_txq_id {
+	MT_TXQ_VO = IEEE80211_AC_VO,
+	MT_TXQ_VI = IEEE80211_AC_VI,
+	MT_TXQ_BE = IEEE80211_AC_BE,
+	MT_TXQ_BK = IEEE80211_AC_BK,
+	MT_TXQ_PSD,
+	MT_TXQ_MCU,
+	__MT_TXQ_MAX
+};
+
+/* Hardware uses mirrored order of queues with Q0 having the highest priority */
+static u8 q2hwq(u8 q)
+{
+	return q ^ 0x3;
+}
+
+/* Take mac80211 Q id from the skb and translate it to hardware Q id */
+static u8 skb2q(struct sk_buff *skb)
+{
+	int qid = skb_get_queue_mapping(skb);
+
+	if (WARN_ON(qid >= MT_TXQ_PSD)) {
+		qid = MT_TXQ_BE;
+		skb_set_queue_mapping(skb, qid);
+	}
+
+	return q2hwq(qid);
+}
+
+/* Note: TX retry reporting is a bit broken.
+ *	 Retries are reported only once per AMPDU and often come a frame early
+ *	 i.e. they are reported in the last status preceding the AMPDU. Apart
+ *	 from the fact that it's hard to know the length of the AMPDU (which is
+ *	 required to know to how many consecutive frames retries should be
+ *	 applied), if status comes early on full FIFO it gets lost and retries
+ *	 of the whole AMPDU become invisible.
+ *	 As a work-around encode the desired rate in PKT_ID of TX descriptor
+ *	 and based on that guess the retries (every rate is tried once).
+ *	 Only downside here is that for MCS0 we have to rely solely on
+ *	 transmission failures as no retries can ever be reported.
+ *	 Not having to read EXT_FIFO has a nice effect of doubling the number
+ *	 of reports which can be fetched.
+ *	 Also the vendor driver never uses the EXT_FIFO register so it may be
+ *	 undertested.
+ */
+static u8 mt7601u_tx_pktid_enc(struct mt7601u_dev *dev, u8 rate, bool is_probe)
+{
+	u8 encoded = (rate + 1) + is_probe *  8;
+
+	/* Because PKT_ID 0 disables status reporting only 15 values are
+	 * available but 16 are needed (8 MCS * 2 for encoding is_probe)
+	 * - we need to cram together two rates. MCS0 and MCS7 with is_probe
+	 * share PKT_ID 9.
+	 */
+	if (is_probe && rate == 7)
+		return encoded - 7;
+
+	return encoded;
+}
+
+static void
+mt7601u_tx_pktid_dec(struct mt7601u_dev *dev, struct mt76_tx_status *stat)
+{
+	u8 req_rate = stat->pktid;
+	u8 eff_rate = stat->rate & 0x7;
+
+	req_rate -= 1;
+
+	if (req_rate > 7) {
+		stat->is_probe = true;
+		req_rate -= 8;
+
+		/* Decide between MCS0 and MCS7 which share pktid 9 */
+		if (!req_rate && eff_rate)
+			req_rate = 7;
+	}
+
+	stat->retry = req_rate - eff_rate;
+}
+
+static void mt7601u_tx_skb_remove_dma_overhead(struct sk_buff *skb,
+					       struct ieee80211_tx_info *info)
+{
+	int pkt_len = (unsigned long)info->status.status_driver_data[0];
+
+	skb_pull(skb, sizeof(struct mt76_txwi) + 4);
+	if (ieee80211_get_hdrlen_from_skb(skb) % 4)
+		mt76_remove_hdr_pad(skb);
+
+	skb_trim(skb, pkt_len);
+}
+
+void mt7601u_tx_status(struct mt7601u_dev *dev, struct sk_buff *skb)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+	mt7601u_tx_skb_remove_dma_overhead(skb, info);
+
+	ieee80211_tx_info_clear_status(info);
+	info->status.rates[0].idx = -1;
+	info->flags |= IEEE80211_TX_STAT_ACK;
+	ieee80211_tx_status(dev->hw, skb);
+}
+
+static int mt7601u_skb_rooms(struct mt7601u_dev *dev, struct sk_buff *skb)
+{
+	int hdr_len = ieee80211_get_hdrlen_from_skb(skb);
+	u32 need_head;
+
+	need_head = sizeof(struct mt76_txwi) + 4;
+	if (hdr_len % 4)
+		need_head += 2;
+
+	return skb_cow(skb, need_head);
+}
+
+static struct mt76_txwi *
+mt7601u_push_txwi(struct mt7601u_dev *dev, struct sk_buff *skb,
+		  struct ieee80211_sta *sta, struct mt76_wcid *wcid,
+		  int pkt_len)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_tx_rate *rate = &info->control.rates[0];
+	struct mt76_txwi *txwi;
+	unsigned long flags;
+	bool is_probe;
+	u32 pkt_id;
+	u16 rate_ctl;
+	u8 nss;
+
+	txwi = (struct mt76_txwi *)skb_push(skb, sizeof(struct mt76_txwi));
+	memset(txwi, 0, sizeof(*txwi));
+
+	if (!wcid->tx_rate_set)
+		ieee80211_get_tx_rates(info->control.vif, sta, skb,
+				       info->control.rates, 1);
+
+	spin_lock_irqsave(&dev->lock, flags);
+	if (rate->idx < 0 || !rate->count)
+		rate_ctl = wcid->tx_rate;
+	else
+		rate_ctl = mt76_mac_tx_rate_val(dev, rate, &nss);
+	spin_unlock_irqrestore(&dev->lock, flags);
+	txwi->rate_ctl = cpu_to_le16(rate_ctl);
+
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
+		txwi->ack_ctl |= MT_TXWI_ACK_CTL_REQ;
+	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ)
+		txwi->ack_ctl |= MT_TXWI_ACK_CTL_NSEQ;
+
+	if ((info->flags & IEEE80211_TX_CTL_AMPDU) && sta) {
+		u8 ba_size = IEEE80211_MIN_AMPDU_BUF;
+
+		ba_size <<= sta->ht_cap.ampdu_factor;
+		ba_size = min_t(int, 63, ba_size);
+		if (info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)
+			ba_size = 0;
+		txwi->ack_ctl |= MT76_SET(MT_TXWI_ACK_CTL_BA_WINDOW, ba_size);
+
+		txwi->flags = cpu_to_le16(MT_TXWI_FLAGS_AMPDU |
+					  MT76_SET(MT_TXWI_FLAGS_MPDU_DENSITY,
+						   sta->ht_cap.ampdu_density));
+		if (info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE)
+			txwi->flags = 0;
+	}
+
+	txwi->wcid = wcid->idx;
+
+	is_probe = !!(info->flags & IEEE80211_TX_CTL_RATE_CTRL_PROBE);
+	pkt_id = mt7601u_tx_pktid_enc(dev, rate_ctl & 0x7, is_probe);
+	pkt_len |= MT76_SET(MT_TXWI_LEN_PKTID, pkt_id);
+	txwi->len_ctl = cpu_to_le16(pkt_len);
+
+	return txwi;
+}
+
+void mt7601u_tx(struct ieee80211_hw *hw, struct ieee80211_tx_control *control,
+		struct sk_buff *skb)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct mt7601u_dev *dev = hw->priv;
+	struct ieee80211_vif *vif = info->control.vif;
+	struct ieee80211_sta *sta = control->sta;
+	struct mt76_sta *msta = NULL;
+	struct mt76_wcid *wcid = dev->mon_wcid;
+	struct mt76_txwi *txwi;
+	int pkt_len = skb->len;
+	int hw_q = skb2q(skb);
+
+	BUILD_BUG_ON(ARRAY_SIZE(info->status.status_driver_data) < 1);
+	info->status.status_driver_data[0] = (void *)(unsigned long)pkt_len;
+
+	if (mt7601u_skb_rooms(dev, skb) || mt76_insert_hdr_pad(skb)) {
+		ieee80211_free_txskb(dev->hw, skb);
+		return;
+	}
+
+	if (sta) {
+		msta = (struct mt76_sta *) sta->drv_priv;
+		wcid = &msta->wcid;
+	} else if (vif) {
+		struct mt76_vif *mvif = (struct mt76_vif *)vif->drv_priv;
+
+		wcid = &mvif->group_wcid;
+	}
+
+	txwi = mt7601u_push_txwi(dev, skb, sta, wcid, pkt_len);
+
+	if (mt7601u_dma_enqueue_tx(dev, skb, wcid, hw_q))
+		return;
+
+	trace_mt_tx(dev, skb, msta, txwi);
+}
+
+void mt7601u_tx_stat(struct work_struct *work)
+{
+	struct mt7601u_dev *dev = container_of(work, struct mt7601u_dev,
+					       stat_work.work);
+	struct mt76_tx_status stat;
+	unsigned long flags;
+	int cleaned = 0;
+
+	while (!test_bit(MT7601U_STATE_REMOVED, &dev->state)) {
+		stat = mt7601u_mac_fetch_tx_status(dev);
+		if (!stat.valid)
+			break;
+
+		mt7601u_tx_pktid_dec(dev, &stat);
+		mt76_send_tx_status(dev, &stat);
+
+		cleaned++;
+	}
+	trace_mt_tx_status_cleaned(dev, cleaned);
+
+	spin_lock_irqsave(&dev->tx_lock, flags);
+	if (cleaned)
+		queue_delayed_work(dev->stat_wq, &dev->stat_work,
+				   msecs_to_jiffies(10));
+	else if (test_and_clear_bit(MT7601U_STATE_MORE_STATS, &dev->state))
+		queue_delayed_work(dev->stat_wq, &dev->stat_work,
+				   msecs_to_jiffies(20));
+	else
+		clear_bit(MT7601U_STATE_READING_STATS, &dev->state);
+	spin_unlock_irqrestore(&dev->tx_lock, flags);
+}
+
+int mt7601u_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		    u16 queue, const struct ieee80211_tx_queue_params *params)
+{
+	struct mt7601u_dev *dev = hw->priv;
+	u8 cw_min = 5, cw_max = 10, hw_q = q2hwq(queue);
+	u32 val;
+
+	/* TODO: should we do funny things with the parameters?
+	 *	 See what mt7601u_set_default_edca() used to do in init.c.
+	 */
+
+	if (params->cw_min)
+		cw_min = fls(params->cw_min);
+	if (params->cw_max)
+		cw_max = fls(params->cw_max);
+
+	WARN_ON(params->txop > 0xff);
+	WARN_ON(params->aifs > 0xf);
+	WARN_ON(cw_min > 0xf);
+	WARN_ON(cw_max > 0xf);
+
+	val = MT76_SET(MT_EDCA_CFG_AIFSN, params->aifs) |
+	      MT76_SET(MT_EDCA_CFG_CWMIN, cw_min) |
+	      MT76_SET(MT_EDCA_CFG_CWMAX, cw_max);
+	/* TODO: based on user-controlled EnableTxBurst var vendor drv sets
+	 *	 a really long txop on AC0 (see connect.c:2009) but only on
+	 *	 connect? When not connected should be 0.
+	 */
+	if (!hw_q)
+		val |= 0x60;
+	else
+		val |= MT76_SET(MT_EDCA_CFG_TXOP, params->txop);
+	mt76_wr(dev, MT_EDCA_CFG_AC(hw_q), val);
+
+	val = mt76_rr(dev, MT_WMM_TXOP(hw_q));
+	val &= ~(MT_WMM_TXOP_MASK << MT_WMM_TXOP_SHIFT(hw_q));
+	val |= params->txop << MT_WMM_TXOP_SHIFT(hw_q);
+	mt76_wr(dev, MT_WMM_TXOP(hw_q), val);
+
+	val = mt76_rr(dev, MT_WMM_AIFSN);
+	val &= ~(MT_WMM_AIFSN_MASK << MT_WMM_AIFSN_SHIFT(hw_q));
+	val |= params->aifs << MT_WMM_AIFSN_SHIFT(hw_q);
+	mt76_wr(dev, MT_WMM_AIFSN, val);
+
+	val = mt76_rr(dev, MT_WMM_CWMIN);
+	val &= ~(MT_WMM_CWMIN_MASK << MT_WMM_CWMIN_SHIFT(hw_q));
+	val |= cw_min << MT_WMM_CWMIN_SHIFT(hw_q);
+	mt76_wr(dev, MT_WMM_CWMIN, val);
+
+	val = mt76_rr(dev, MT_WMM_CWMAX);
+	val &= ~(MT_WMM_CWMAX_MASK << MT_WMM_CWMAX_SHIFT(hw_q));
+	val |= cw_max << MT_WMM_CWMAX_SHIFT(hw_q);
+	mt76_wr(dev, MT_WMM_CWMAX, val);
+
+	return 0;
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/usb.c b/drivers/net/wireless/mediatek/mt7601u/usb.c
new file mode 100644
index 000000000000..54dba4001865
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/usb.c
@@ -0,0 +1,367 @@
+/*
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+
+#include "mt7601u.h"
+#include "usb.h"
+#include "trace.h"
+
+static struct usb_device_id mt7601u_device_table[] = {
+	{ USB_DEVICE(0x0b05, 0x17d3) },
+	{ USB_DEVICE(0x0e8d, 0x760a) },
+	{ USB_DEVICE(0x0e8d, 0x760b) },
+	{ USB_DEVICE(0x13d3, 0x3431) },
+	{ USB_DEVICE(0x13d3, 0x3434) },
+	{ USB_DEVICE(0x148f, 0x7601) },
+	{ USB_DEVICE(0x148f, 0x760a) },
+	{ USB_DEVICE(0x148f, 0x760b) },
+	{ USB_DEVICE(0x148f, 0x760c) },
+	{ USB_DEVICE(0x148f, 0x760d) },
+	{ USB_DEVICE(0x2001, 0x3d04) },
+	{ USB_DEVICE(0x2717, 0x4106) },
+	{ USB_DEVICE(0x2955, 0x0001) },
+	{ USB_DEVICE(0x2955, 0x1001) },
+	{ USB_DEVICE(0x2a5f, 0x1000) },
+	{ USB_DEVICE(0x7392, 0x7710) },
+	{ 0, }
+};
+
+bool mt7601u_usb_alloc_buf(struct mt7601u_dev *dev, size_t len,
+			   struct mt7601u_dma_buf *buf)
+{
+	struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
+
+	buf->len = len;
+	buf->urb = usb_alloc_urb(0, GFP_KERNEL);
+	buf->buf = usb_alloc_coherent(usb_dev, buf->len, GFP_KERNEL, &buf->dma);
+
+	return !buf->urb || !buf->buf;
+}
+
+void mt7601u_usb_free_buf(struct mt7601u_dev *dev, struct mt7601u_dma_buf *buf)
+{
+	struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
+
+	usb_free_coherent(usb_dev, buf->len, buf->buf, buf->dma);
+	usb_free_urb(buf->urb);
+}
+
+int mt7601u_usb_submit_buf(struct mt7601u_dev *dev, int dir, int ep_idx,
+			   struct mt7601u_dma_buf *buf, gfp_t gfp,
+			   usb_complete_t complete_fn, void *context)
+{
+	struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
+	unsigned pipe;
+	int ret;
+
+	if (dir == USB_DIR_IN)
+		pipe = usb_rcvbulkpipe(usb_dev, dev->in_eps[ep_idx]);
+	else
+		pipe = usb_sndbulkpipe(usb_dev, dev->out_eps[ep_idx]);
+
+	usb_fill_bulk_urb(buf->urb, usb_dev, pipe, buf->buf, buf->len,
+			  complete_fn, context);
+	buf->urb->transfer_dma = buf->dma;
+	buf->urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+	trace_mt_submit_urb(dev, buf->urb);
+	ret = usb_submit_urb(buf->urb, gfp);
+	if (ret)
+		dev_err(dev->dev, "Error: submit URB dir:%d ep:%d failed:%d\n",
+			dir, ep_idx, ret);
+	return ret;
+}
+
+void mt7601u_complete_urb(struct urb *urb)
+{
+	struct completion *cmpl = urb->context;
+
+	complete(cmpl);
+}
+
+static int
+__mt7601u_vendor_request(struct mt7601u_dev *dev, const u8 req,
+			 const u8 direction, const u16 val, const u16 offset,
+			 void *buf, const size_t buflen)
+{
+	int i, ret;
+	struct usb_device *usb_dev = mt7601u_to_usb_dev(dev);
+	const u8 req_type = direction | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
+	const unsigned int pipe = (direction == USB_DIR_IN) ?
+		usb_rcvctrlpipe(usb_dev, 0) : usb_sndctrlpipe(usb_dev, 0);
+
+	for (i = 0; i < MT_VEND_REQ_MAX_RETRY; i++) {
+		ret = usb_control_msg(usb_dev, pipe, req, req_type,
+				      val, offset, buf, buflen,
+				      MT_VEND_REQ_TOUT_MS);
+		trace_mt_vend_req(dev, pipe, req, req_type, val, offset,
+				  buf, buflen, ret);
+
+		if (ret >= 0 || ret == -ENODEV)
+			return ret;
+
+		msleep(5);
+	}
+
+	dev_err(dev->dev, "Vendor request req:%02x off:%04x failed:%d\n",
+		req, offset, ret);
+
+	return ret;
+}
+
+int
+mt7601u_vendor_request(struct mt7601u_dev *dev, const u8 req,
+		       const u8 direction, const u16 val, const u16 offset,
+		       void *buf, const size_t buflen)
+{
+	int ret;
+
+	mutex_lock(&dev->vendor_req_mutex);
+
+	ret = __mt7601u_vendor_request(dev, req, direction, val, offset,
+				       buf, buflen);
+	if (ret == -ENODEV)
+		set_bit(MT7601U_STATE_REMOVED, &dev->state);
+
+	mutex_unlock(&dev->vendor_req_mutex);
+
+	return ret;
+}
+
+void mt7601u_vendor_reset(struct mt7601u_dev *dev)
+{
+	mt7601u_vendor_request(dev, MT_VEND_DEV_MODE, USB_DIR_OUT,
+			       MT_VEND_DEV_MODE_RESET, 0, NULL, 0);
+}
+
+u32 mt7601u_rr(struct mt7601u_dev *dev, u32 offset)
+{
+	int ret;
+	__le32 reg;
+	u32 val;
+
+	WARN_ONCE(offset > USHRT_MAX, "read high off:%08x", offset);
+
+	ret = mt7601u_vendor_request(dev, MT_VEND_MULTI_READ, USB_DIR_IN,
+				     0, offset, &reg, sizeof(reg));
+	val = le32_to_cpu(reg);
+	if (ret > 0 && ret != sizeof(reg)) {
+		dev_err(dev->dev, "Error: wrong size read:%d off:%08x\n",
+			ret, offset);
+		val = ~0;
+	}
+
+	trace_reg_read(dev, offset, val);
+	return val;
+}
+
+int mt7601u_vendor_single_wr(struct mt7601u_dev *dev, const u8 req,
+			     const u16 offset, const u32 val)
+{
+	int ret;
+
+	ret = mt7601u_vendor_request(dev, req, USB_DIR_OUT,
+				     val & 0xffff, offset, NULL, 0);
+	if (ret)
+		return ret;
+	return mt7601u_vendor_request(dev, req, USB_DIR_OUT,
+				      val >> 16, offset + 2, NULL, 0);
+}
+
+void mt7601u_wr(struct mt7601u_dev *dev, u32 offset, u32 val)
+{
+	WARN_ONCE(offset > USHRT_MAX, "write high off:%08x", offset);
+
+	mt7601u_vendor_single_wr(dev, MT_VEND_WRITE, offset, val);
+	trace_reg_write(dev, offset, val);
+}
+
+u32 mt7601u_rmw(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val)
+{
+	val |= mt7601u_rr(dev, offset) & ~mask;
+	mt7601u_wr(dev, offset, val);
+	return val;
+}
+
+u32 mt7601u_rmc(struct mt7601u_dev *dev, u32 offset, u32 mask, u32 val)
+{
+	u32 reg = mt7601u_rr(dev, offset);
+
+	val |= reg & ~mask;
+	if (reg != val)
+		mt7601u_wr(dev, offset, val);
+	return val;
+}
+
+void mt7601u_wr_copy(struct mt7601u_dev *dev, u32 offset,
+		     const void *data, int len)
+{
+	WARN_ONCE(offset & 3, "unaligned write copy off:%08x", offset);
+	WARN_ONCE(len & 3, "short write copy off:%08x", offset);
+
+	mt7601u_burst_write_regs(dev, offset, data, len / 4);
+}
+
+void mt7601u_addr_wr(struct mt7601u_dev *dev, const u32 offset, const u8 *addr)
+{
+	mt7601u_wr(dev, offset, get_unaligned_le32(addr));
+	mt7601u_wr(dev, offset + 4, addr[4] | addr[5] << 8);
+}
+
+static int mt7601u_assign_pipes(struct usb_interface *usb_intf,
+				struct mt7601u_dev *dev)
+{
+	struct usb_endpoint_descriptor *ep_desc;
+	struct usb_host_interface *intf_desc = usb_intf->cur_altsetting;
+	unsigned i, ep_i = 0, ep_o = 0;
+
+	BUILD_BUG_ON(sizeof(dev->in_eps) < __MT_EP_IN_MAX);
+	BUILD_BUG_ON(sizeof(dev->out_eps) < __MT_EP_OUT_MAX);
+
+	for (i = 0; i < intf_desc->desc.bNumEndpoints; i++) {
+		ep_desc = &intf_desc->endpoint[i].desc;
+
+		if (usb_endpoint_is_bulk_in(ep_desc) &&
+		    ep_i++ < __MT_EP_IN_MAX) {
+			dev->in_eps[ep_i - 1] = usb_endpoint_num(ep_desc);
+			dev->in_max_packet = usb_endpoint_maxp(ep_desc);
+			/* Note: this is ignored by usb sub-system but vendor
+			 *	 code does it. We can drop this at some point.
+			 */
+			dev->in_eps[ep_i - 1] |= USB_DIR_IN;
+		} else if (usb_endpoint_is_bulk_out(ep_desc) &&
+			   ep_o++ < __MT_EP_OUT_MAX) {
+			dev->out_eps[ep_o - 1] = usb_endpoint_num(ep_desc);
+			dev->out_max_packet = usb_endpoint_maxp(ep_desc);
+		}
+	}
+
+	if (ep_i != __MT_EP_IN_MAX || ep_o != __MT_EP_OUT_MAX) {
+		dev_err(dev->dev, "Error: wrong pipe number in:%d out:%d\n",
+			ep_i, ep_o);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int mt7601u_probe(struct usb_interface *usb_intf,
+			 const struct usb_device_id *id)
+{
+	struct usb_device *usb_dev = interface_to_usbdev(usb_intf);
+	struct mt7601u_dev *dev;
+	u32 asic_rev, mac_rev;
+	int ret;
+
+	dev = mt7601u_alloc_device(&usb_intf->dev);
+	if (!dev)
+		return -ENOMEM;
+
+	usb_dev = usb_get_dev(usb_dev);
+	usb_reset_device(usb_dev);
+
+	usb_set_intfdata(usb_intf, dev);
+
+	ret = mt7601u_assign_pipes(usb_intf, dev);
+	if (ret)
+		goto err;
+	ret = mt7601u_wait_asic_ready(dev);
+	if (ret)
+		goto err;
+
+	asic_rev = mt7601u_rr(dev, MT_ASIC_VERSION);
+	mac_rev = mt7601u_rr(dev, MT_MAC_CSR0);
+	dev_info(dev->dev, "ASIC revision: %08x MAC revision: %08x\n",
+		 asic_rev, mac_rev);
+
+	/* Note: vendor driver skips this check for MT7601U */
+	if (!(mt7601u_rr(dev, MT_EFUSE_CTRL) & MT_EFUSE_CTRL_SEL))
+		dev_warn(dev->dev, "Warning: eFUSE not present\n");
+
+	ret = mt7601u_init_hardware(dev);
+	if (ret)
+		goto err;
+	ret = mt7601u_register_device(dev);
+	if (ret)
+		goto err_hw;
+
+	set_bit(MT7601U_STATE_INITIALIZED, &dev->state);
+
+	return 0;
+err_hw:
+	mt7601u_cleanup(dev);
+err:
+	usb_set_intfdata(usb_intf, NULL);
+	usb_put_dev(interface_to_usbdev(usb_intf));
+
+	destroy_workqueue(dev->stat_wq);
+	ieee80211_free_hw(dev->hw);
+	return ret;
+}
+
+static void mt7601u_disconnect(struct usb_interface *usb_intf)
+{
+	struct mt7601u_dev *dev = usb_get_intfdata(usb_intf);
+
+	ieee80211_unregister_hw(dev->hw);
+	mt7601u_cleanup(dev);
+
+	usb_set_intfdata(usb_intf, NULL);
+	usb_put_dev(interface_to_usbdev(usb_intf));
+
+	destroy_workqueue(dev->stat_wq);
+	ieee80211_free_hw(dev->hw);
+}
+
+static int mt7601u_suspend(struct usb_interface *usb_intf, pm_message_t state)
+{
+	struct mt7601u_dev *dev = usb_get_intfdata(usb_intf);
+
+	mt7601u_cleanup(dev);
+
+	return 0;
+}
+
+static int mt7601u_resume(struct usb_interface *usb_intf)
+{
+	struct mt7601u_dev *dev = usb_get_intfdata(usb_intf);
+	int ret;
+
+	ret = mt7601u_init_hardware(dev);
+	if (ret)
+		return ret;
+
+	set_bit(MT7601U_STATE_INITIALIZED, &dev->state);
+
+	return 0;
+}
+
+MODULE_DEVICE_TABLE(usb, mt7601u_device_table);
+MODULE_FIRMWARE(MT7601U_FIRMWARE);
+MODULE_LICENSE("GPL");
+
+static struct usb_driver mt7601u_driver = {
+	.name		= KBUILD_MODNAME,
+	.id_table	= mt7601u_device_table,
+	.probe		= mt7601u_probe,
+	.disconnect	= mt7601u_disconnect,
+	.suspend	= mt7601u_suspend,
+	.resume		= mt7601u_resume,
+	.reset_resume	= mt7601u_resume,
+	.soft_unbind	= 1,
+	.disable_hub_initiated_lpm = 1,
+};
+module_usb_driver(mt7601u_driver);
diff --git a/drivers/net/wireless/mediatek/mt7601u/usb.h b/drivers/net/wireless/mediatek/mt7601u/usb.h
new file mode 100644
index 000000000000..49e188fa3798
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/usb.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2015 Jakub Kicinski <kubakici@wp.pl>
+ *
+ * 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.
+ */
+
+#ifndef __MT7601U_USB_H
+#define __MT7601U_USB_H
+
+#include "mt7601u.h"
+
+#define MT7601U_FIRMWARE	"mt7601u.bin"
+
+#define MT_VEND_REQ_MAX_RETRY	10
+#define MT_VEND_REQ_TOUT_MS	300
+
+#define MT_VEND_DEV_MODE_RESET	1
+
+enum mt_vendor_req {
+	MT_VEND_DEV_MODE = 1,
+	MT_VEND_WRITE = 2,
+	MT_VEND_MULTI_READ = 7,
+	MT_VEND_WRITE_FCE = 0x42,
+};
+
+enum mt_usb_ep_in {
+	MT_EP_IN_PKT_RX,
+	MT_EP_IN_CMD_RESP,
+	__MT_EP_IN_MAX,
+};
+
+enum mt_usb_ep_out {
+	MT_EP_OUT_INBAND_CMD,
+	MT_EP_OUT_AC_BK,
+	MT_EP_OUT_AC_BE,
+	MT_EP_OUT_AC_VI,
+	MT_EP_OUT_AC_VO,
+	MT_EP_OUT_HCCA,
+	__MT_EP_OUT_MAX,
+};
+
+static inline struct usb_device *mt7601u_to_usb_dev(struct mt7601u_dev *mt7601u)
+{
+	return interface_to_usbdev(to_usb_interface(mt7601u->dev));
+}
+
+static inline bool mt7601u_urb_has_error(struct urb *urb)
+{
+	return urb->status &&
+		urb->status != -ENOENT &&
+		urb->status != -ECONNRESET &&
+		urb->status != -ESHUTDOWN;
+}
+
+bool mt7601u_usb_alloc_buf(struct mt7601u_dev *dev, size_t len,
+			   struct mt7601u_dma_buf *buf);
+void mt7601u_usb_free_buf(struct mt7601u_dev *dev, struct mt7601u_dma_buf *buf);
+int mt7601u_usb_submit_buf(struct mt7601u_dev *dev, int dir, int ep_idx,
+			   struct mt7601u_dma_buf *buf, gfp_t gfp,
+			   usb_complete_t complete_fn, void *context);
+void mt7601u_complete_urb(struct urb *urb);
+
+int mt7601u_vendor_request(struct mt7601u_dev *dev, const u8 req,
+			   const u8 direction, const u16 val, const u16 offset,
+			   void *buf, const size_t buflen);
+void mt7601u_vendor_reset(struct mt7601u_dev *dev);
+int mt7601u_vendor_single_wr(struct mt7601u_dev *dev, const u8 req,
+			     const u16 offset, const u32 val);
+
+#endif
diff --git a/drivers/net/wireless/mediatek/mt7601u/util.c b/drivers/net/wireless/mediatek/mt7601u/util.c
new file mode 100644
index 000000000000..7c1787c1ddcd
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/util.c
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "mt7601u.h"
+
+void mt76_remove_hdr_pad(struct sk_buff *skb)
+{
+	int len = ieee80211_get_hdrlen_from_skb(skb);
+
+	memmove(skb->data + 2, skb->data, len);
+	skb_pull(skb, 2);
+}
+
+int mt76_insert_hdr_pad(struct sk_buff *skb)
+{
+	int len = ieee80211_get_hdrlen_from_skb(skb);
+	int ret;
+
+	if (len % 4 == 0)
+		return 0;
+
+	ret = skb_cow(skb, 2);
+	if (ret)
+		return ret;
+
+	skb_push(skb, 2);
+	memmove(skb->data, skb->data + 2, len);
+
+	skb->data[len] = 0;
+	skb->data[len + 1] = 0;
+	return 0;
+}
diff --git a/drivers/net/wireless/mediatek/mt7601u/util.h b/drivers/net/wireless/mediatek/mt7601u/util.h
new file mode 100644
index 000000000000..b89140bf1210
--- /dev/null
+++ b/drivers/net/wireless/mediatek/mt7601u/util.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
+ * Copyright (C) 2004 - 2009 Ivo van Doorn <IvDoorn@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.
+ */
+
+#ifndef __MT76_UTIL_H
+#define __MT76_UTIL_H
+
+/*
+ * Power of two check, this will check
+ * if the mask that has been given contains and contiguous set of bits.
+ * Note that we cannot use the is_power_of_2() function since this
+ * check must be done at compile-time.
+ */
+#define is_power_of_two(x)	( !((x) & ((x)-1)) )
+#define low_bit_mask(x)		( ((x)-1) & ~(x) )
+#define is_valid_mask(x)	is_power_of_two(1LU + (x) + low_bit_mask(x))
+
+/*
+ * Macros to find first set bit in a variable.
+ * These macros behave the same as the __ffs() functions but
+ * the most important difference that this is done during
+ * compile-time rather then run-time.
+ */
+#define compile_ffs2(__x) \
+	__builtin_choose_expr(((__x) & 0x1), 0, 1)
+
+#define compile_ffs4(__x) \
+	__builtin_choose_expr(((__x) & 0x3), \
+			      (compile_ffs2((__x))), \
+			      (compile_ffs2((__x) >> 2) + 2))
+
+#define compile_ffs8(__x) \
+	__builtin_choose_expr(((__x) & 0xf), \
+			      (compile_ffs4((__x))), \
+			      (compile_ffs4((__x) >> 4) + 4))
+
+#define compile_ffs16(__x) \
+	__builtin_choose_expr(((__x) & 0xff), \
+			      (compile_ffs8((__x))), \
+			      (compile_ffs8((__x) >> 8) + 8))
+
+#define compile_ffs32(__x) \
+	__builtin_choose_expr(((__x) & 0xffff), \
+			      (compile_ffs16((__x))), \
+			      (compile_ffs16((__x) >> 16) + 16))
+
+/*
+ * This macro will check the requirements for the FIELD{8,16,32} macros
+ * The mask should be a constant non-zero contiguous set of bits which
+ * does not exceed the given typelimit.
+ */
+#define FIELD_CHECK(__mask) \
+	BUILD_BUG_ON(!(__mask) || !is_valid_mask(__mask))
+
+#define MT76_SET(_mask, _val)						\
+	({								\
+		FIELD_CHECK(_mask);					\
+		(((u32) (_val)) << compile_ffs32(_mask)) & _mask;	\
+	})
+
+#define MT76_GET(_mask, _val)						\
+	({								\
+		FIELD_CHECK(_mask);					\
+		(u32) (((_val) & _mask) >> compile_ffs32(_mask));	\
+	})
+
+#endif
diff --git a/drivers/net/wireless/mwifiex/11h.c b/drivers/net/wireless/mwifiex/11h.c
index 3ab87a855122..71a1b580796f 100644
--- a/drivers/net/wireless/mwifiex/11h.c
+++ b/drivers/net/wireless/mwifiex/11h.c
@@ -134,8 +134,8 @@ void mwifiex_dfs_cac_work_queue(struct work_struct *work)
 
 	chandef = priv->dfs_chandef;
 	if (priv->wdev.cac_started) {
-		dev_dbg(priv->adapter->dev,
-			"CAC timer finished; No radar detected\n");
+		mwifiex_dbg(priv->adapter, MSG,
+			    "CAC timer finished; No radar detected\n");
 		cfg80211_cac_event(priv->netdev, &chandef,
 				   NL80211_RADAR_CAC_FINISHED,
 				   GFP_KERNEL);
@@ -161,21 +161,40 @@ int mwifiex_cmd_issue_chan_report_request(struct mwifiex_private *priv,
 	cr_req->chan_desc.chan_width = radar_params->chandef->width;
 	cr_req->msec_dwell_time = cpu_to_le32(radar_params->cac_time_ms);
 
-	dev_dbg(priv->adapter->dev,
-		"11h: issuing DFS Radar check for channel=%d\n",
-		radar_params->chandef->chan->hw_value);
+	if (radar_params->cac_time_ms)
+		mwifiex_dbg(priv->adapter, MSG,
+			    "11h: issuing DFS Radar check for channel=%d\n",
+			    radar_params->chandef->chan->hw_value);
+	else
+		mwifiex_dbg(priv->adapter, MSG, "cancelling CAC\n");
 
 	return 0;
 }
 
+int mwifiex_stop_radar_detection(struct mwifiex_private *priv,
+				 struct cfg80211_chan_def *chandef)
+{
+	struct mwifiex_radar_params radar_params;
+
+	memset(&radar_params, 0, sizeof(struct mwifiex_radar_params));
+	radar_params.chandef = chandef;
+	radar_params.cac_time_ms = 0;
+
+	return mwifiex_send_cmd(priv, HostCmd_CMD_CHAN_REPORT_REQUEST,
+				HostCmd_ACT_GEN_SET, 0, &radar_params, true);
+}
+
 /* This function is to abort ongoing CAC upon stopping AP operations
  * or during unload.
  */
 void mwifiex_abort_cac(struct mwifiex_private *priv)
 {
 	if (priv->wdev.cac_started) {
-		dev_dbg(priv->adapter->dev,
-			"Aborting delayed work for CAC.\n");
+		if (mwifiex_stop_radar_detection(priv, &priv->dfs_chandef))
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "failed to stop CAC in FW\n");
+		mwifiex_dbg(priv->adapter, MSG,
+			    "Aborting delayed work for CAC.\n");
 		cancel_delayed_work_sync(&priv->dfs_cac_work);
 		cfg80211_cac_event(priv->netdev, &priv->dfs_chandef,
 				   NL80211_RADAR_CAC_ABORTED, GFP_KERNEL);
@@ -199,7 +218,8 @@ int mwifiex_11h_handle_chanrpt_ready(struct mwifiex_private *priv,
 				sizeof(u32));
 
 	if (le32_to_cpu(rpt_event->result) != HostCmd_RESULT_OK) {
-		dev_err(priv->adapter->dev, "Error in channel report event\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Error in channel report event\n");
 		return -1;
 	}
 
@@ -212,8 +232,8 @@ int mwifiex_11h_handle_chanrpt_ready(struct mwifiex_private *priv,
 		switch (le16_to_cpu(rpt->header.type)) {
 		case TLV_TYPE_CHANRPT_11H_BASIC:
 			if (rpt->map.radar) {
-				dev_notice(priv->adapter->dev,
-					   "RADAR Detected on channel %d!\n",
+				mwifiex_dbg(priv->adapter, MSG,
+					    "RADAR Detected on channel %d!\n",
 					    priv->dfs_chandef.chan->hw_value);
 				cancel_delayed_work_sync(&priv->dfs_cac_work);
 				cfg80211_cac_event(priv->netdev,
@@ -242,16 +262,20 @@ int mwifiex_11h_handle_radar_detected(struct mwifiex_private *priv,
 	rdr_event = (void *)(skb->data + sizeof(u32));
 
 	if (le32_to_cpu(rdr_event->passed)) {
-		dev_notice(priv->adapter->dev,
-			   "radar detected; indicating kernel\n");
+		mwifiex_dbg(priv->adapter, MSG,
+			    "radar detected; indicating kernel\n");
+		if (mwifiex_stop_radar_detection(priv, &priv->dfs_chandef))
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "Failed to stop CAC in FW\n");
 		cfg80211_radar_event(priv->adapter->wiphy, &priv->dfs_chandef,
 				     GFP_KERNEL);
-		dev_dbg(priv->adapter->dev, "regdomain: %d\n",
-			rdr_event->reg_domain);
-		dev_dbg(priv->adapter->dev, "radar detection type: %d\n",
-			rdr_event->det_type);
+		mwifiex_dbg(priv->adapter, MSG, "regdomain: %d\n",
+			    rdr_event->reg_domain);
+		mwifiex_dbg(priv->adapter, MSG, "radar detection type: %d\n",
+			    rdr_event->det_type);
 	} else {
-		dev_dbg(priv->adapter->dev, "false radar detection event!\n");
+		mwifiex_dbg(priv->adapter, MSG,
+			    "false radar detection event!\n");
 	}
 
 	return 0;
@@ -276,20 +300,20 @@ void mwifiex_dfs_chan_sw_work_queue(struct work_struct *work)
 
 	bss_cfg = &priv->bss_cfg;
 	if (!bss_cfg->beacon_period) {
-		dev_err(priv->adapter->dev,
-			"channel switch: AP already stopped\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "channel switch: AP already stopped\n");
 		return;
 	}
 
-	mwifiex_uap_set_channel(bss_cfg, priv->dfs_chandef);
+	mwifiex_uap_set_channel(priv, bss_cfg, priv->dfs_chandef);
 
 	if (mwifiex_config_start_uap(priv, bss_cfg)) {
-		dev_dbg(priv->adapter->dev,
-			"Failed to start AP after channel switch\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to start AP after channel switch\n");
 		return;
 	}
 
-	dev_notice(priv->adapter->dev,
-		   "indicating channel switch completion to kernel\n");
+	mwifiex_dbg(priv->adapter, MSG,
+		    "indicating channel switch completion to kernel\n");
 	cfg80211_ch_switch_notify(priv->netdev, &priv->dfs_chandef);
 }
diff --git a/drivers/net/wireless/mwifiex/11n.c b/drivers/net/wireless/mwifiex/11n.c
index 433bd6837c79..c174e79e6df2 100644
--- a/drivers/net/wireless/mwifiex/11n.c
+++ b/drivers/net/wireless/mwifiex/11n.c
@@ -42,7 +42,7 @@ int mwifiex_fill_cap_info(struct mwifiex_private *priv, u8 radio_type,
 					priv->wdev.wiphy->bands[radio_type];
 
 	if (WARN_ON_ONCE(!sband)) {
-		dev_err(priv->adapter->dev, "Invalid radio type!\n");
+		mwifiex_dbg(priv->adapter, ERROR, "Invalid radio type!\n");
 		return -EINVAL;
 	}
 
@@ -156,7 +156,7 @@ int mwifiex_ret_11n_delba(struct mwifiex_private *priv,
 int mwifiex_ret_11n_addba_req(struct mwifiex_private *priv,
 			      struct host_cmd_ds_command *resp)
 {
-	int tid;
+	int tid, tid_down;
 	struct host_cmd_ds_11n_addba_rsp *add_ba_rsp = &resp->params.add_ba_rsp;
 	struct mwifiex_tx_ba_stream_tbl *tx_ba_tbl;
 	struct mwifiex_ra_list_tbl *ra_list;
@@ -167,7 +167,9 @@ int mwifiex_ret_11n_addba_req(struct mwifiex_private *priv,
 
 	tid = (block_ack_param_set & IEEE80211_ADDBA_PARAM_TID_MASK)
 	       >> BLOCKACKPARAM_TID_POS;
-	ra_list = mwifiex_wmm_get_ralist_node(priv, tid, add_ba_rsp->
+
+	tid_down = mwifiex_wmm_downgrade_tid(priv, tid);
+	ra_list = mwifiex_wmm_get_ralist_node(priv, tid_down, add_ba_rsp->
 		peer_mac_addr);
 	if (le16_to_cpu(add_ba_rsp->status_code) != BA_RESULT_SUCCESS) {
 		if (ra_list) {
@@ -184,7 +186,7 @@ int mwifiex_ret_11n_addba_req(struct mwifiex_private *priv,
 
 	tx_ba_tbl = mwifiex_get_ba_tbl(priv, tid, add_ba_rsp->peer_mac_addr);
 	if (tx_ba_tbl) {
-		dev_dbg(priv->adapter->dev, "info: BA stream complete\n");
+		mwifiex_dbg(priv->adapter, EVENT, "info: BA stream complete\n");
 		tx_ba_tbl->ba_status = BA_SETUP_COMPLETE;
 		if ((block_ack_param_set & BLOCKACKPARAM_AMSDU_SUPP_MASK) &&
 		    priv->add_ba_param.tx_amsdu &&
@@ -197,7 +199,7 @@ int mwifiex_ret_11n_addba_req(struct mwifiex_private *priv,
 			ra_list->ba_status = BA_SETUP_COMPLETE;
 		}
 	} else {
-		dev_err(priv->adapter->dev, "BA stream not created\n");
+		mwifiex_dbg(priv->adapter, ERROR, "BA stream not created\n");
 	}
 
 	return 0;
@@ -224,7 +226,8 @@ int mwifiex_cmd_recfg_tx_buf(struct mwifiex_private *priv,
 	tx_buf->action = cpu_to_le16(action);
 	switch (action) {
 	case HostCmd_ACT_GEN_SET:
-		dev_dbg(priv->adapter->dev, "cmd: set tx_buf=%d\n", *buf_size);
+		mwifiex_dbg(priv->adapter, CMD,
+			    "cmd: set tx_buf=%d\n", *buf_size);
 		tx_buf->buff_size = cpu_to_le16(*buf_size);
 		break;
 	case HostCmd_ACT_GEN_GET:
@@ -466,7 +469,8 @@ void mwifiex_11n_delete_tx_ba_stream_tbl_entry(struct mwifiex_private *priv,
 	    mwifiex_is_tx_ba_stream_ptr_valid(priv, tx_ba_tsr_tbl))
 		return;
 
-	dev_dbg(priv->adapter->dev, "info: tx_ba_tsr_tbl %p\n", tx_ba_tsr_tbl);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: tx_ba_tsr_tbl %p\n", tx_ba_tsr_tbl);
 
 	list_del(&tx_ba_tsr_tbl->list);
 
@@ -528,13 +532,16 @@ void mwifiex_create_ba_tbl(struct mwifiex_private *priv, u8 *ra, int tid,
 	struct mwifiex_tx_ba_stream_tbl *new_node;
 	struct mwifiex_ra_list_tbl *ra_list;
 	unsigned long flags;
+	int tid_down;
 
 	if (!mwifiex_get_ba_tbl(priv, tid, ra)) {
 		new_node = kzalloc(sizeof(struct mwifiex_tx_ba_stream_tbl),
 				   GFP_ATOMIC);
 		if (!new_node)
 			return;
-		ra_list = mwifiex_wmm_get_ralist_node(priv, tid, ra);
+
+		tid_down = mwifiex_wmm_downgrade_tid(priv, tid);
+		ra_list = mwifiex_wmm_get_ralist_node(priv, tid_down, ra);
 		if (ra_list) {
 			ra_list->ba_status = ba_status;
 			ra_list->amsdu_in_ampdu = false;
@@ -563,7 +570,7 @@ int mwifiex_send_addba(struct mwifiex_private *priv, int tid, u8 *peer_mac)
 	unsigned long flags;
 	u16 block_ack_param_set;
 
-	dev_dbg(priv->adapter->dev, "cmd: %s: tid %d\n", __func__, tid);
+	mwifiex_dbg(priv->adapter, CMD, "cmd: %s: tid %d\n", __func__, tid);
 
 	if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
 	    ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info) &&
@@ -575,9 +582,9 @@ int mwifiex_send_addba(struct mwifiex_private *priv, int tid, u8 *peer_mac)
 		sta_ptr = mwifiex_get_sta_entry(priv, peer_mac);
 		if (!sta_ptr) {
 			spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
-			dev_warn(priv->adapter->dev,
-				 "BA setup with unknown TDLS peer %pM!\n",
-				peer_mac);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "BA setup with unknown TDLS peer %pM!\n",
+				    peer_mac);
 			return -1;
 		}
 		if (sta_ptr->is_11ac_enabled)
@@ -641,6 +648,30 @@ int mwifiex_send_delba(struct mwifiex_private *priv, int tid, u8 *peer_mac,
 }
 
 /*
+ * This function sends delba to specific tid
+ */
+void mwifiex_11n_delba(struct mwifiex_private *priv, int tid)
+{
+	struct mwifiex_rx_reorder_tbl *rx_reor_tbl_ptr;
+
+	if (list_empty(&priv->rx_reorder_tbl_ptr)) {
+		dev_dbg(priv->adapter->dev,
+			"mwifiex_11n_delba: rx_reorder_tbl_ptr empty\n");
+		return;
+	}
+
+	list_for_each_entry(rx_reor_tbl_ptr, &priv->rx_reorder_tbl_ptr, list) {
+		if (rx_reor_tbl_ptr->tid == tid) {
+			dev_dbg(priv->adapter->dev,
+				"Send delba to tid=%d, %pM\n",
+				tid, rx_reor_tbl_ptr->ta);
+			mwifiex_send_delba(priv, tid, rx_reor_tbl_ptr->ta, 0);
+			return;
+		}
+	}
+}
+
+/*
  * This function handles the command response of a delete BA request.
  */
 void mwifiex_11n_delete_ba_stream(struct mwifiex_private *priv, u8 *del_ba)
@@ -706,8 +737,8 @@ int mwifiex_get_tx_ba_stream_tbl(struct mwifiex_private *priv,
 	spin_lock_irqsave(&priv->tx_ba_stream_tbl_lock, flags);
 	list_for_each_entry(tx_ba_tsr_tbl, &priv->tx_ba_stream_tbl_ptr, list) {
 		rx_reo_tbl->tid = (u16) tx_ba_tsr_tbl->tid;
-		dev_dbg(priv->adapter->dev, "data: %s tid=%d\n",
-			__func__, rx_reo_tbl->tid);
+		mwifiex_dbg(priv->adapter, DATA, "data: %s tid=%d\n",
+			    __func__, rx_reo_tbl->tid);
 		memcpy(rx_reo_tbl->ra, tx_ba_tsr_tbl->ra, ETH_ALEN);
 		rx_reo_tbl->amsdu = tx_ba_tsr_tbl->amsdu;
 		rx_reo_tbl++;
@@ -812,3 +843,72 @@ u8 mwifiex_get_sec_chan_offset(int chan)
 
 	return sec_offset;
 }
+
+/* This function will send DELBA to entries in the priv's
+ * Tx BA stream table
+ */
+static void
+mwifiex_send_delba_txbastream_tbl(struct mwifiex_private *priv, u8 tid)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_tx_ba_stream_tbl *tx_ba_stream_tbl_ptr;
+
+	if (list_empty(&priv->tx_ba_stream_tbl_ptr))
+		return;
+
+	list_for_each_entry(tx_ba_stream_tbl_ptr,
+			    &priv->tx_ba_stream_tbl_ptr, list) {
+		if (tx_ba_stream_tbl_ptr->ba_status == BA_SETUP_COMPLETE) {
+			if (tid == tx_ba_stream_tbl_ptr->tid) {
+				dev_dbg(adapter->dev,
+					"Tx:Send delba to tid=%d, %pM\n", tid,
+					tx_ba_stream_tbl_ptr->ra);
+				mwifiex_send_delba(priv,
+						   tx_ba_stream_tbl_ptr->tid,
+						   tx_ba_stream_tbl_ptr->ra, 1);
+				return;
+			}
+		}
+	}
+}
+
+/* This function updates all the tx_win_size
+ */
+void mwifiex_update_ampdu_txwinsize(struct mwifiex_adapter *adapter)
+{
+	u8 i;
+	u32 tx_win_size;
+	struct mwifiex_private *priv;
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		if (!adapter->priv[i])
+			continue;
+		priv = adapter->priv[i];
+		tx_win_size = priv->add_ba_param.tx_win_size;
+
+		if (priv->bss_type == MWIFIEX_BSS_TYPE_STA)
+			priv->add_ba_param.tx_win_size =
+				MWIFIEX_STA_AMPDU_DEF_TXWINSIZE;
+
+		if (priv->bss_type == MWIFIEX_BSS_TYPE_P2P)
+			priv->add_ba_param.tx_win_size =
+				MWIFIEX_STA_AMPDU_DEF_TXWINSIZE;
+
+		if (priv->bss_type == MWIFIEX_BSS_TYPE_UAP)
+			priv->add_ba_param.tx_win_size =
+				MWIFIEX_UAP_AMPDU_DEF_TXWINSIZE;
+
+		if (adapter->coex_win_size) {
+			if (adapter->coex_tx_win_size)
+				priv->add_ba_param.tx_win_size =
+					adapter->coex_tx_win_size;
+		}
+
+		if (tx_win_size != priv->add_ba_param.tx_win_size) {
+			if (!priv->media_connected)
+				continue;
+			for (i = 0; i < MAX_NUM_TID; i++)
+				mwifiex_send_delba_txbastream_tbl(priv, i);
+		}
+	}
+}
diff --git a/drivers/net/wireless/mwifiex/11n_aggr.c b/drivers/net/wireless/mwifiex/11n_aggr.c
index 6183e255e62a..f7c717253a66 100644
--- a/drivers/net/wireless/mwifiex/11n_aggr.c
+++ b/drivers/net/wireless/mwifiex/11n_aggr.c
@@ -187,7 +187,6 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 	skb_aggr = mwifiex_alloc_dma_align_buf(adapter->tx_buf_size,
 					       GFP_ATOMIC | GFP_DMA);
 	if (!skb_aggr) {
-		dev_err(adapter->dev, "%s: alloc skb_aggr\n", __func__);
 		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
 				       ra_list_flags);
 		return -1;
@@ -297,13 +296,13 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 		tx_info_aggr->flags |= MWIFIEX_BUF_FLAG_REQUEUED_PKT;
 		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
 				       ra_list_flags);
-		dev_dbg(adapter->dev, "data: -EBUSY is returned\n");
+		mwifiex_dbg(adapter, ERROR, "data: -EBUSY is returned\n");
 		break;
 	case -1:
 		if (adapter->iface_type != MWIFIEX_PCIE)
 			adapter->data_sent = false;
-		dev_err(adapter->dev, "%s: host_to_card failed: %#x\n",
-			__func__, ret);
+		mwifiex_dbg(adapter, ERROR, "%s: host_to_card failed: %#x\n",
+			    __func__, ret);
 		adapter->dbg.num_tx_host_to_card_failure++;
 		mwifiex_write_data_complete(adapter, skb_aggr, 1, ret);
 		return 0;
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/mwifiex/11n_rxreorder.c
index f75f8acfaca0..2906cd543532 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.c
@@ -51,8 +51,8 @@ static int mwifiex_11n_dispatch_amsdu_pkt(struct mwifiex_private *priv,
 			rx_skb = __skb_dequeue(&list);
 			ret = mwifiex_recv_packet(priv, rx_skb);
 			if (ret == -1)
-				dev_err(priv->adapter->dev,
-					"Rx of A-MSDU failed");
+				mwifiex_dbg(priv->adapter, ERROR,
+					    "Rx of A-MSDU failed");
 		}
 		return 0;
 	}
@@ -304,7 +304,7 @@ mwifiex_flush_data(unsigned long context)
 	if (seq_num < 0)
 		return;
 
-	dev_dbg(ctx->priv->adapter->dev, "info: flush data %d\n", seq_num);
+	mwifiex_dbg(ctx->priv->adapter, INFO, "info: flush data %d\n", seq_num);
 	start_win = (ctx->ptr->start_win + seq_num + 1) & (MAX_TID_VALUE - 1);
 	mwifiex_11n_dispatch_pkt_until_start_win(ctx->priv, ctx->ptr,
 						 start_win);
@@ -367,8 +367,9 @@ mwifiex_11n_create_rx_reorder_tbl(struct mwifiex_private *priv, u8 *ta,
 	}
 	spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
 
-	dev_dbg(priv->adapter->dev, "info: last_seq=%d start_win=%d\n",
-		last_seq, new_node->start_win);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: last_seq=%d start_win=%d\n",
+		    last_seq, new_node->start_win);
 
 	if (last_seq != MWIFIEX_DEF_11N_RX_SEQ_NUM &&
 	    last_seq >= new_node->start_win) {
@@ -382,8 +383,8 @@ mwifiex_11n_create_rx_reorder_tbl(struct mwifiex_private *priv, u8 *ta,
 					GFP_KERNEL);
 	if (!new_node->rx_reorder_ptr) {
 		kfree((u8 *) new_node);
-		dev_err(priv->adapter->dev,
-			"%s: failed to alloc reorder_ptr\n", __func__);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: failed to alloc reorder_ptr\n", __func__);
 		return;
 	}
 
@@ -467,9 +468,9 @@ int mwifiex_cmd_11n_addba_rsp_gen(struct mwifiex_private *priv,
 						cmd_addba_req->peer_mac_addr);
 		if (!sta_ptr) {
 			spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
-			dev_warn(priv->adapter->dev,
-				 "BA setup with unknown TDLS peer %pM!\n",
-				 cmd_addba_req->peer_mac_addr);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "BA setup with unknown TDLS peer %pM!\n",
+				    cmd_addba_req->peer_mac_addr);
 			return -1;
 		}
 		if (sta_ptr->is_11ac_enabled)
@@ -573,14 +574,14 @@ int mwifiex_11n_rx_reorder_pkt(struct mwifiex_private *priv,
 	}
 
 	if (tbl->flags & RXREOR_FORCE_NO_DROP) {
-		dev_dbg(priv->adapter->dev,
-			"RXREOR_FORCE_NO_DROP when HS is activated\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "RXREOR_FORCE_NO_DROP when HS is activated\n");
 		tbl->flags &= ~RXREOR_FORCE_NO_DROP;
 	} else if (init_window_shift && seq_num < start_win &&
 		   seq_num >= tbl->init_win) {
-		dev_dbg(priv->adapter->dev,
-			"Sender TID sequence number reset %d->%d for SSN %d\n",
-			start_win, seq_num, tbl->init_win);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "Sender TID sequence number reset %d->%d for SSN %d\n",
+			    start_win, seq_num, tbl->init_win);
 		tbl->start_win = start_win = seq_num;
 		end_win = ((start_win + win_size) - 1) & (MAX_TID_VALUE - 1);
 	} else {
@@ -662,32 +663,35 @@ mwifiex_del_ba_tbl(struct mwifiex_private *priv, int tid, u8 *peer_mac,
 	struct mwifiex_ra_list_tbl *ra_list;
 	u8 cleanup_rx_reorder_tbl;
 	unsigned long flags;
+	int tid_down;
 
 	if (type == TYPE_DELBA_RECEIVE)
 		cleanup_rx_reorder_tbl = (initiator) ? true : false;
 	else
 		cleanup_rx_reorder_tbl = (initiator) ? false : true;
 
-	dev_dbg(priv->adapter->dev, "event: DELBA: %pM tid=%d initiator=%d\n",
-		peer_mac, tid, initiator);
+	mwifiex_dbg(priv->adapter, EVENT, "event: DELBA: %pM tid=%d initiator=%d\n",
+		    peer_mac, tid, initiator);
 
 	if (cleanup_rx_reorder_tbl) {
 		tbl = mwifiex_11n_get_rx_reorder_tbl(priv, tid,
 								 peer_mac);
 		if (!tbl) {
-			dev_dbg(priv->adapter->dev,
-				"event: TID, TA not found in table\n");
+			mwifiex_dbg(priv->adapter, EVENT,
+				    "event: TID, TA not found in table\n");
 			return;
 		}
 		mwifiex_del_rx_reorder_entry(priv, tbl);
 	} else {
 		ptx_tbl = mwifiex_get_ba_tbl(priv, tid, peer_mac);
 		if (!ptx_tbl) {
-			dev_dbg(priv->adapter->dev,
-				"event: TID, RA not found in table\n");
+			mwifiex_dbg(priv->adapter, EVENT,
+				    "event: TID, RA not found in table\n");
 			return;
 		}
-		ra_list = mwifiex_wmm_get_ralist_node(priv, tid, peer_mac);
+
+		tid_down = mwifiex_wmm_downgrade_tid(priv, tid);
+		ra_list = mwifiex_wmm_get_ralist_node(priv, tid_down, peer_mac);
 		if (ra_list) {
 			ra_list->amsdu_in_ampdu = false;
 			ra_list->ba_status = BA_SETUP_NONE;
@@ -721,8 +725,8 @@ int mwifiex_ret_11n_addba_resp(struct mwifiex_private *priv,
 	 * the stream
 	 */
 	if (le16_to_cpu(add_ba_rsp->status_code) != BA_RESULT_SUCCESS) {
-		dev_err(priv->adapter->dev, "ADDBA RSP: failed %pM tid=%d)\n",
-			add_ba_rsp->peer_mac_addr, tid);
+		mwifiex_dbg(priv->adapter, ERROR, "ADDBA RSP: failed %pM tid=%d)\n",
+			    add_ba_rsp->peer_mac_addr, tid);
 
 		tbl = mwifiex_11n_get_rx_reorder_tbl(priv, tid,
 						     add_ba_rsp->peer_mac_addr);
@@ -746,8 +750,8 @@ int mwifiex_ret_11n_addba_resp(struct mwifiex_private *priv,
 			tbl->amsdu = false;
 	}
 
-	dev_dbg(priv->adapter->dev,
-		"cmd: ADDBA RSP: %pM tid=%d ssn=%d win_size=%d\n",
+	mwifiex_dbg(priv->adapter, CMD,
+		    "cmd: ADDBA RSP: %pM tid=%d ssn=%d win_size=%d\n",
 		add_ba_rsp->peer_mac_addr, tid, add_ba_rsp->ssn, win_size);
 
 	return 0;
@@ -824,3 +828,83 @@ void mwifiex_update_rxreor_flags(struct mwifiex_adapter *adapter, u8 flags)
 
 	return;
 }
+
+/* This function update all the rx_win_size based on coex flag
+ */
+static void mwifiex_update_ampdu_rxwinsize(struct mwifiex_adapter *adapter,
+					   bool coex_flag)
+{
+	u8 i;
+	u32 rx_win_size;
+	struct mwifiex_private *priv;
+
+	dev_dbg(adapter->dev, "Update rxwinsize %d\n", coex_flag);
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		if (!adapter->priv[i])
+			continue;
+		priv = adapter->priv[i];
+		rx_win_size = priv->add_ba_param.rx_win_size;
+		if (coex_flag) {
+			if (priv->bss_type == MWIFIEX_BSS_TYPE_STA)
+				priv->add_ba_param.rx_win_size =
+					MWIFIEX_STA_COEX_AMPDU_DEF_RXWINSIZE;
+			if (priv->bss_type == MWIFIEX_BSS_TYPE_P2P)
+				priv->add_ba_param.rx_win_size =
+					MWIFIEX_STA_COEX_AMPDU_DEF_RXWINSIZE;
+			if (priv->bss_type == MWIFIEX_BSS_TYPE_UAP)
+				priv->add_ba_param.rx_win_size =
+					MWIFIEX_UAP_COEX_AMPDU_DEF_RXWINSIZE;
+		} else {
+			if (priv->bss_type == MWIFIEX_BSS_TYPE_STA)
+				priv->add_ba_param.rx_win_size =
+					MWIFIEX_STA_AMPDU_DEF_RXWINSIZE;
+			if (priv->bss_type == MWIFIEX_BSS_TYPE_P2P)
+				priv->add_ba_param.rx_win_size =
+					MWIFIEX_STA_AMPDU_DEF_RXWINSIZE;
+			if (priv->bss_type == MWIFIEX_BSS_TYPE_UAP)
+				priv->add_ba_param.rx_win_size =
+					MWIFIEX_UAP_AMPDU_DEF_RXWINSIZE;
+		}
+
+		if (adapter->coex_win_size && adapter->coex_rx_win_size)
+			priv->add_ba_param.rx_win_size =
+					adapter->coex_rx_win_size;
+
+		if (rx_win_size != priv->add_ba_param.rx_win_size) {
+			if (!priv->media_connected)
+				continue;
+			for (i = 0; i < MAX_NUM_TID; i++)
+				mwifiex_11n_delba(priv, i);
+		}
+	}
+}
+
+/* This function check coex for RX BA
+ */
+void mwifiex_coex_ampdu_rxwinsize(struct mwifiex_adapter *adapter)
+{
+	u8 i;
+	struct mwifiex_private *priv;
+	u8 count = 0;
+
+	for (i = 0; i < adapter->priv_num; i++) {
+		if (adapter->priv[i]) {
+			priv = adapter->priv[i];
+			if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) {
+				if (priv->media_connected)
+					count++;
+			}
+			if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+				if (priv->bss_started)
+					count++;
+			}
+		}
+		if (count >= MWIFIEX_BSS_COEX_COUNT)
+			break;
+	}
+	if (count >= MWIFIEX_BSS_COEX_COUNT)
+		mwifiex_update_ampdu_rxwinsize(adapter, true);
+	else
+		mwifiex_update_ampdu_rxwinsize(adapter, false);
+}
diff --git a/drivers/net/wireless/mwifiex/Kconfig b/drivers/net/wireless/mwifiex/Kconfig
index aa01c9bc77f9..48edf387683e 100644
--- a/drivers/net/wireless/mwifiex/Kconfig
+++ b/drivers/net/wireless/mwifiex/Kconfig
@@ -12,6 +12,7 @@ config MWIFIEX_SDIO
 	tristate "Marvell WiFi-Ex Driver for SD8786/SD8787/SD8797/SD8887/SD8897"
 	depends on MWIFIEX && MMC
 	select FW_LOADER
+	select WANT_DEV_COREDUMP
 	---help---
 	  This adds support for wireless adapters based on Marvell
 	  8786/8787/8797/8887/8897 chipsets with SDIO interface.
@@ -23,6 +24,7 @@ config MWIFIEX_PCIE
 	tristate "Marvell WiFi-Ex Driver for PCIE 8766/8897"
 	depends on MWIFIEX && PCI
 	select FW_LOADER
+	select WANT_DEV_COREDUMP
 	---help---
 	  This adds support for wireless adapters based on Marvell
 	  8766/8897 chipsets with PCIe interface.
diff --git a/drivers/net/wireless/mwifiex/README b/drivers/net/wireless/mwifiex/README
index 31928caeeed2..2f0f9b5609d0 100644
--- a/drivers/net/wireless/mwifiex/README
+++ b/drivers/net/wireless/mwifiex/README
@@ -230,9 +230,9 @@ getlog
 
 	cat getlog
 
-fw_dump
-	This command is used to dump firmware memory into files.
-	Separate file will be created for each memory segment.
+device_dump
+	This command is used to dump driver information and firmware memory
+	segments.
 	Usage:
 
 	cat fw_dump
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index bf9020ff2d33..b15e4c7acbec 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -67,6 +67,22 @@ u8 mwifiex_chan_type_to_sec_chan_offset(enum nl80211_channel_type chan_type)
 	}
 }
 
+/* This function maps IEEE HT secondary channel type to NL80211 channel type
+ */
+u8 mwifiex_sec_chan_offset_to_chan_type(u8 second_chan_offset)
+{
+	switch (second_chan_offset) {
+	case IEEE80211_HT_PARAM_CHA_SEC_NONE:
+		return NL80211_CHAN_HT20;
+	case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+		return NL80211_CHAN_HT40PLUS;
+	case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+		return NL80211_CHAN_HT40MINUS;
+	default:
+		return NL80211_CHAN_HT20;
+	}
+}
+
 /*
  * This function checks whether WEP is set.
  */
@@ -104,11 +120,11 @@ mwifiex_cfg80211_del_key(struct wiphy *wiphy, struct net_device *netdev,
 	const u8 *peer_mac = pairwise ? mac_addr : bc_mac;
 
 	if (mwifiex_set_encode(priv, NULL, NULL, 0, key_index, peer_mac, 1)) {
-		wiphy_err(wiphy, "deleting the crypto keys\n");
+		mwifiex_dbg(priv->adapter, ERROR, "deleting the crypto keys\n");
 		return -EFAULT;
 	}
 
-	wiphy_dbg(wiphy, "info: crypto keys deleted\n");
+	mwifiex_dbg(priv->adapter, INFO, "info: crypto keys deleted\n");
 	return 0;
 }
 
@@ -163,7 +179,7 @@ mwifiex_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(wdev->netdev);
 
 	if (!buf || !len) {
-		wiphy_err(wiphy, "invalid buffer and length\n");
+		mwifiex_dbg(priv->adapter, ERROR, "invalid buffer and length\n");
 		return -EFAULT;
 	}
 
@@ -172,8 +188,8 @@ mwifiex_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 	    ieee80211_is_probe_resp(mgmt->frame_control)) {
 		/* Since we support offload probe resp, we need to skip probe
 		 * resp in AP or GO mode */
-		wiphy_dbg(wiphy,
-			  "info: skip to send probe resp in AP or GO mode\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: skip to send probe resp in AP or GO mode\n");
 		return 0;
 	}
 
@@ -183,7 +199,8 @@ mwifiex_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 			    pkt_len + sizeof(pkt_len));
 
 	if (!skb) {
-		wiphy_err(wiphy, "allocate skb failed for management frame\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "allocate skb failed for management frame\n");
 		return -ENOMEM;
 	}
 
@@ -206,7 +223,7 @@ mwifiex_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 
 	mwifiex_queue_tx_pkt(priv, skb);
 
-	wiphy_dbg(wiphy, "info: management frame transmitted\n");
+	mwifiex_dbg(priv->adapter, INFO, "info: management frame transmitted\n");
 	return 0;
 }
 
@@ -231,7 +248,7 @@ mwifiex_cfg80211_mgmt_frame_register(struct wiphy *wiphy,
 		mwifiex_send_cmd(priv, HostCmd_CMD_MGMT_FRAME_REG,
 				 HostCmd_ACT_GEN_SET, 0,
 				 &priv->mgmt_frame_mask, false);
-		wiphy_dbg(wiphy, "info: mgmt frame registered\n");
+		mwifiex_dbg(priv->adapter, INFO, "info: mgmt frame registered\n");
 	}
 }
 
@@ -248,13 +265,14 @@ mwifiex_cfg80211_remain_on_channel(struct wiphy *wiphy,
 	int ret;
 
 	if (!chan || !cookie) {
-		wiphy_err(wiphy, "Invalid parameter for ROC\n");
+		mwifiex_dbg(priv->adapter, ERROR, "Invalid parameter for ROC\n");
 		return -EINVAL;
 	}
 
 	if (priv->roc_cfg.cookie) {
-		wiphy_dbg(wiphy, "info: ongoing ROC, cookie = 0x%llx\n",
-			  priv->roc_cfg.cookie);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: ongoing ROC, cookie = 0x%llx\n",
+			    priv->roc_cfg.cookie);
 		return -EBUSY;
 	}
 
@@ -269,7 +287,8 @@ mwifiex_cfg80211_remain_on_channel(struct wiphy *wiphy,
 		cfg80211_ready_on_channel(wdev, *cookie, chan,
 					  duration, GFP_ATOMIC);
 
-		wiphy_dbg(wiphy, "info: ROC, cookie = 0x%llx\n", *cookie);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: ROC, cookie = 0x%llx\n", *cookie);
 	}
 
 	return ret;
@@ -298,7 +317,8 @@ mwifiex_cfg80211_cancel_remain_on_channel(struct wiphy *wiphy,
 
 		memset(&priv->roc_cfg, 0, sizeof(struct mwifiex_roc_cfg));
 
-		wiphy_dbg(wiphy, "info: cancel ROC, cookie = 0x%llx\n", cookie);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: cancel ROC, cookie = 0x%llx\n", cookie);
 	}
 
 	return ret;
@@ -344,8 +364,8 @@ mwifiex_cfg80211_set_power_mgmt(struct wiphy *wiphy,
 	u32 ps_mode;
 
 	if (timeout)
-		wiphy_dbg(wiphy,
-			  "info: ignore timeout value for IEEE Power Save\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: ignore timeout value for IEEE Power Save\n");
 
 	ps_mode = enabled;
 
@@ -370,7 +390,7 @@ mwifiex_cfg80211_set_default_key(struct wiphy *wiphy, struct net_device *netdev,
 		priv->wep_key_curr_index = key_index;
 	} else if (mwifiex_set_encode(priv, NULL, NULL, 0, key_index,
 				      NULL, 0)) {
-		wiphy_err(wiphy, "set default Tx key index\n");
+		mwifiex_dbg(priv->adapter, ERROR, "set default Tx key index\n");
 		return -EFAULT;
 	}
 
@@ -407,7 +427,7 @@ mwifiex_cfg80211_add_key(struct wiphy *wiphy, struct net_device *netdev,
 
 	if (mwifiex_set_encode(priv, params, params->key, params->key_len,
 			       key_index, peer_mac, 0)) {
-		wiphy_err(wiphy, "crypto keys added\n");
+		mwifiex_dbg(priv->adapter, ERROR, "crypto keys added\n");
 		return -EFAULT;
 	}
 
@@ -442,7 +462,8 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 
 	band = mwifiex_band_to_radio_type(adapter->config_bands);
 	if (!wiphy->bands[band]) {
-		wiphy_err(wiphy, "11D: setting domain info in FW\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "11D: setting domain info in FW\n");
 		return -1;
 	}
 
@@ -493,7 +514,8 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
 
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_802_11D_DOMAIN_INFO,
 			     HostCmd_ACT_GEN_SET, 0, NULL, false)) {
-		wiphy_err(wiphy, "11D: setting domain info in FW\n");
+		mwifiex_dbg(adapter, INFO,
+			    "11D: setting domain info in FW\n");
 		return -1;
 	}
 
@@ -516,9 +538,9 @@ static void mwifiex_reg_notifier(struct wiphy *wiphy,
 	struct mwifiex_adapter *adapter = mwifiex_cfg80211_get_adapter(wiphy);
 	struct mwifiex_private *priv = mwifiex_get_priv(adapter,
 							MWIFIEX_BSS_ROLE_ANY);
-
-	wiphy_dbg(wiphy, "info: cfg80211 regulatory domain callback for %c%c\n",
-		  request->alpha2[0], request->alpha2[1]);
+	mwifiex_dbg(adapter, INFO,
+		    "info: cfg80211 regulatory domain callback for %c%c\n",
+		    request->alpha2[0], request->alpha2[1]);
 
 	switch (request->initiator) {
 	case NL80211_REGDOM_SET_BY_DRIVER:
@@ -527,8 +549,9 @@ static void mwifiex_reg_notifier(struct wiphy *wiphy,
 	case NL80211_REGDOM_SET_BY_COUNTRY_IE:
 		break;
 	default:
-		wiphy_err(wiphy, "unknown regdom initiator: %d\n",
-			  request->initiator);
+		mwifiex_dbg(adapter, ERROR,
+			    "unknown regdom initiator: %d\n",
+			    request->initiator);
 		return;
 	}
 
@@ -597,8 +620,8 @@ mwifiex_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 	switch (priv->bss_role) {
 	case MWIFIEX_BSS_ROLE_UAP:
 		if (priv->bss_started) {
-			dev_err(adapter->dev,
-				"cannot change wiphy params when bss started");
+			mwifiex_dbg(adapter, ERROR,
+				    "cannot change wiphy params when bss started");
 			return -EINVAL;
 		}
 
@@ -622,15 +645,16 @@ mwifiex_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 
 		kfree(bss_cfg);
 		if (ret) {
-			wiphy_err(wiphy, "Failed to set wiphy phy params\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Failed to set wiphy phy params\n");
 			return ret;
 		}
 		break;
 
 		case MWIFIEX_BSS_ROLE_STA:
 		if (priv->media_connected) {
-			dev_err(adapter->dev,
-				"cannot change wiphy params when connected");
+			mwifiex_dbg(adapter, ERROR,
+				    "cannot change wiphy params when connected");
 			return -EINVAL;
 		}
 		if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
@@ -724,8 +748,8 @@ static int mwifiex_deinit_priv_params(struct mwifiex_private *priv)
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_MGMT_FRAME_REG,
 			     HostCmd_ACT_GEN_SET, 0,
 			     &priv->mgmt_frame_mask, false)) {
-		dev_warn(priv->adapter->dev,
-			 "could not unregister mgmt frame rx\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "could not unregister mgmt frame rx\n");
 		return -1;
 	}
 
@@ -789,9 +813,9 @@ mwifiex_init_new_priv_params(struct mwifiex_private *priv,
 		priv->bss_role = MWIFIEX_BSS_ROLE_UAP;
 		break;
 	default:
-		dev_err(priv->adapter->dev,
-			"%s: changing to %d not supported\n",
-			dev->name, type);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: changing to %d not supported\n",
+			    dev->name, type);
 		return -EOPNOTSUPP;
 	}
 
@@ -824,12 +848,13 @@ mwifiex_change_vif_to_p2p(struct net_device *dev,
 
 	if (adapter->curr_iface_comb.p2p_intf ==
 	    adapter->iface_limit.p2p_intf) {
-		dev_err(adapter->dev,
-			"cannot create multiple P2P ifaces\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot create multiple P2P ifaces\n");
 		return -1;
 	}
 
-	dev_dbg(priv->adapter->dev, "%s: changing role to p2p\n", dev->name);
+	mwifiex_dbg(adapter, INFO,
+		    "%s: changing role to p2p\n", dev->name);
 
 	if (mwifiex_deinit_priv_params(priv))
 		return -1;
@@ -846,9 +871,9 @@ mwifiex_change_vif_to_p2p(struct net_device *dev,
 			return -EFAULT;
 		break;
 	default:
-		dev_err(priv->adapter->dev,
-			"%s: changing to %d not supported\n",
-			dev->name, type);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: changing to %d not supported\n",
+			    dev->name, type);
 		return -EOPNOTSUPP;
 	}
 
@@ -897,17 +922,17 @@ mwifiex_change_vif_to_sta_adhoc(struct net_device *dev,
 	     curr_iftype != NL80211_IFTYPE_P2P_GO) &&
 	    (adapter->curr_iface_comb.sta_intf ==
 	     adapter->iface_limit.sta_intf)) {
-		dev_err(adapter->dev,
-			"cannot create multiple station/adhoc ifaces\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot create multiple station/adhoc ifaces\n");
 		return -1;
 	}
 
 	if (type == NL80211_IFTYPE_STATION)
-		dev_notice(adapter->dev,
-			   "%s: changing role to station\n", dev->name);
+		mwifiex_dbg(adapter, INFO,
+			    "%s: changing role to station\n", dev->name);
 	else
-		dev_notice(adapter->dev,
-			   "%s: changing role to adhoc\n", dev->name);
+		mwifiex_dbg(adapter, INFO,
+			    "%s: changing role to adhoc\n", dev->name);
 
 	if (mwifiex_deinit_priv_params(priv))
 		return -1;
@@ -954,12 +979,13 @@ mwifiex_change_vif_to_ap(struct net_device *dev,
 
 	if (adapter->curr_iface_comb.uap_intf ==
 	    adapter->iface_limit.uap_intf) {
-		dev_err(adapter->dev,
-			"cannot create multiple AP ifaces\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot create multiple AP ifaces\n");
 		return -1;
 	}
 
-	dev_notice(adapter->dev, "%s: changing role to AP\n", dev->name);
+	mwifiex_dbg(adapter, INFO,
+		    "%s: changing role to AP\n", dev->name);
 
 	if (mwifiex_deinit_priv_params(priv))
 		return -1;
@@ -1020,12 +1046,14 @@ mwifiex_cfg80211_change_virtual_intf(struct wiphy *wiphy,
 			return mwifiex_change_vif_to_ap(dev, curr_iftype, type,
 							flags, params);
 		case NL80211_IFTYPE_UNSPECIFIED:
-			wiphy_warn(wiphy, "%s: kept type as IBSS\n", dev->name);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "%s: kept type as IBSS\n", dev->name);
 		case NL80211_IFTYPE_ADHOC:	/* This shouldn't happen */
 			return 0;
 		default:
-			wiphy_err(wiphy, "%s: changing to %d not supported\n",
-				  dev->name, type);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "%s: changing to %d not supported\n",
+				    dev->name, type);
 			return -EOPNOTSUPP;
 		}
 		break;
@@ -1048,12 +1076,14 @@ mwifiex_cfg80211_change_virtual_intf(struct wiphy *wiphy,
 			return mwifiex_change_vif_to_ap(dev, curr_iftype, type,
 							flags, params);
 		case NL80211_IFTYPE_UNSPECIFIED:
-			wiphy_warn(wiphy, "%s: kept type as STA\n", dev->name);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "%s: kept type as STA\n", dev->name);
 		case NL80211_IFTYPE_STATION:	/* This shouldn't happen */
 			return 0;
 		default:
-			wiphy_err(wiphy, "%s: changing to %d not supported\n",
-				  dev->name, type);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "%s: changing to %d not supported\n",
+				    dev->name, type);
 			return -EOPNOTSUPP;
 		}
 		break;
@@ -1070,12 +1100,14 @@ mwifiex_cfg80211_change_virtual_intf(struct wiphy *wiphy,
 			return mwifiex_change_vif_to_p2p(dev, curr_iftype,
 							 type, flags, params);
 		case NL80211_IFTYPE_UNSPECIFIED:
-			wiphy_warn(wiphy, "%s: kept type as AP\n", dev->name);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "%s: kept type as AP\n", dev->name);
 		case NL80211_IFTYPE_AP:		/* This shouldn't happen */
 			return 0;
 		default:
-			wiphy_err(wiphy, "%s: changing to %d not supported\n",
-				  dev->name, type);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "%s: changing to %d not supported\n",
+				    dev->name, type);
 			return -EOPNOTSUPP;
 		}
 		break;
@@ -1100,19 +1132,22 @@ mwifiex_cfg80211_change_virtual_intf(struct wiphy *wiphy,
 			return mwifiex_change_vif_to_ap(dev, curr_iftype, type,
 							flags, params);
 		case NL80211_IFTYPE_UNSPECIFIED:
-			wiphy_warn(wiphy, "%s: kept type as P2P\n", dev->name);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "%s: kept type as P2P\n", dev->name);
 		case NL80211_IFTYPE_P2P_CLIENT:
 		case NL80211_IFTYPE_P2P_GO:
 			return 0;
 		default:
-			wiphy_err(wiphy, "%s: changing to %d not supported\n",
-				  dev->name, type);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "%s: changing to %d not supported\n",
+				    dev->name, type);
 			return -EOPNOTSUPP;
 		}
 		break;
 	default:
-		wiphy_err(wiphy, "%s: unknown iftype: %d\n",
-			  dev->name, dev->ieee80211_ptr->iftype);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: unknown iftype: %d\n",
+			    dev->name, dev->ieee80211_ptr->iftype);
 		return -EOPNOTSUPP;
 	}
 
@@ -1194,6 +1229,7 @@ mwifiex_parse_htinfo(struct mwifiex_private *priv, u8 tx_htinfo,
  */
 static int
 mwifiex_dump_station_info(struct mwifiex_private *priv,
+			  struct mwifiex_sta_node *node,
 			  struct station_info *sinfo)
 {
 	u32 rate;
@@ -1203,15 +1239,41 @@ mwifiex_dump_station_info(struct mwifiex_private *priv,
 			BIT(NL80211_STA_INFO_TX_BITRATE) |
 			BIT(NL80211_STA_INFO_SIGNAL) | BIT(NL80211_STA_INFO_SIGNAL_AVG);
 
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+		if (!node)
+			return -ENOENT;
+
+		sinfo->filled |= BIT(NL80211_STA_INFO_INACTIVE_TIME) |
+				BIT(NL80211_STA_INFO_TX_FAILED);
+		sinfo->inactive_time =
+			jiffies_to_msecs(jiffies - node->stats.last_rx);
+
+		sinfo->signal = node->stats.rssi;
+		sinfo->signal_avg = node->stats.rssi;
+		sinfo->rx_bytes = node->stats.rx_bytes;
+		sinfo->tx_bytes = node->stats.tx_bytes;
+		sinfo->rx_packets = node->stats.rx_packets;
+		sinfo->tx_packets = node->stats.tx_packets;
+		sinfo->tx_failed = node->stats.tx_failed;
+
+		mwifiex_parse_htinfo(priv, node->stats.last_tx_htinfo,
+				     &sinfo->txrate);
+		sinfo->txrate.legacy = node->stats.last_tx_rate * 5;
+
+		return 0;
+	}
+
 	/* Get signal information from the firmware */
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_RSSI_INFO,
 			     HostCmd_ACT_GEN_GET, 0, NULL, true)) {
-		dev_err(priv->adapter->dev, "failed to get signal information\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "failed to get signal information\n");
 		return -EFAULT;
 	}
 
 	if (mwifiex_drv_get_data_rate(priv, &rate)) {
-		dev_err(priv->adapter->dev, "getting data rate\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "getting data rate error\n");
 		return -EFAULT;
 	}
 
@@ -1267,7 +1329,7 @@ mwifiex_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
 	if (memcmp(mac, priv->cfg_bssid, ETH_ALEN))
 		return -ENOENT;
 
-	return mwifiex_dump_station_info(priv, sinfo);
+	return mwifiex_dump_station_info(priv, NULL, sinfo);
 }
 
 /*
@@ -1278,13 +1340,29 @@ mwifiex_cfg80211_dump_station(struct wiphy *wiphy, struct net_device *dev,
 			      int idx, u8 *mac, struct station_info *sinfo)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+	static struct mwifiex_sta_node *node;
 
-	if (!priv->media_connected || idx)
-		return -ENOENT;
+	if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
+	    priv->media_connected && idx == 0) {
+		ether_addr_copy(mac, priv->cfg_bssid);
+		return mwifiex_dump_station_info(priv, NULL, sinfo);
+	} else if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+		mwifiex_send_cmd(priv, HOST_CMD_APCMD_STA_LIST,
+				 HostCmd_ACT_GEN_GET, 0, NULL, true);
+
+		if (node && (&node->list == &priv->sta_list)) {
+			node = NULL;
+			return -ENOENT;
+		}
 
-	memcpy(mac, priv->cfg_bssid, ETH_ALEN);
+		node = list_prepare_entry(node, &priv->sta_list, list);
+		list_for_each_entry_continue(node, &priv->sta_list, list) {
+			ether_addr_copy(mac, node->mac_addr);
+			return mwifiex_dump_station_info(priv, node, sinfo);
+		}
+	}
 
-	return mwifiex_dump_station_info(priv, sinfo);
+	return -ENOENT;
 }
 
 static int
@@ -1295,7 +1373,7 @@ mwifiex_cfg80211_dump_survey(struct wiphy *wiphy, struct net_device *dev,
 	struct mwifiex_chan_stats *pchan_stats = priv->adapter->chan_stats;
 	enum ieee80211_band band;
 
-	dev_dbg(priv->adapter->dev, "dump_survey idx=%d\n", idx);
+	mwifiex_dbg(priv->adapter, DUMP, "dump_survey idx=%d\n", idx);
 
 	memset(survey, 0, sizeof(struct survey_info));
 
@@ -1472,8 +1550,8 @@ static int mwifiex_cfg80211_set_bitrate_mask(struct wiphy *wiphy,
 	struct mwifiex_adapter *adapter = priv->adapter;
 
 	if (!priv->media_connected) {
-		dev_err(adapter->dev,
-			"Can not set Tx data rate in disconnected state\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Can not set Tx data rate in disconnected state\n");
 		return -EINVAL;
 	}
 
@@ -1556,17 +1634,20 @@ static int mwifiex_cfg80211_change_beacon(struct wiphy *wiphy,
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
 
 	if (GET_BSS_ROLE(priv) != MWIFIEX_BSS_ROLE_UAP) {
-		wiphy_err(wiphy, "%s: bss_type mismatched\n", __func__);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: bss_type mismatched\n", __func__);
 		return -EINVAL;
 	}
 
 	if (!priv->bss_started) {
-		wiphy_err(wiphy, "%s: bss not started\n", __func__);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: bss not started\n", __func__);
 		return -EINVAL;
 	}
 
 	if (mwifiex_set_mgmt_ies(priv, data)) {
-		wiphy_err(wiphy, "%s: setting mgmt ies failed\n", __func__);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: setting mgmt ies failed\n", __func__);
 		return -EFAULT;
 	}
 
@@ -1594,7 +1675,8 @@ mwifiex_cfg80211_del_station(struct wiphy *wiphy, struct net_device *dev,
 	if (!params->mac || is_broadcast_ether_addr(params->mac))
 		return 0;
 
-	wiphy_dbg(wiphy, "%s: mac address %pM\n", __func__, params->mac);
+	mwifiex_dbg(priv->adapter, INFO, "%s: mac address %pM\n",
+		    __func__, params->mac);
 
 	eth_zero_addr(deauth_mac);
 
@@ -1687,14 +1769,23 @@ static int mwifiex_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
 	mwifiex_abort_cac(priv);
 
 	if (mwifiex_del_mgmt_ies(priv))
-		wiphy_err(wiphy, "Failed to delete mgmt IEs!\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to delete mgmt IEs!\n");
 
 	priv->ap_11n_enabled = 0;
 	memset(&priv->bss_cfg, 0, sizeof(priv->bss_cfg));
 
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_STOP,
 			     HostCmd_ACT_GEN_SET, 0, NULL, true)) {
-		wiphy_err(wiphy, "Failed to stop the BSS\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to stop the BSS\n");
+		return -1;
+	}
+
+	if (mwifiex_send_cmd(priv, HOST_CMD_APCMD_SYS_RESET,
+			     HostCmd_ACT_GEN_SET, 0, NULL, true)) {
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to reset BSS\n");
 		return -1;
 	}
 
@@ -1751,12 +1842,13 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 		return -EINVAL;
 	}
 
-	mwifiex_uap_set_channel(bss_cfg, params->chandef);
+	mwifiex_uap_set_channel(priv, bss_cfg, params->chandef);
 	mwifiex_set_uap_rates(bss_cfg, params);
 
 	if (mwifiex_set_secure_params(priv, bss_cfg, params)) {
 		kfree(bss_cfg);
-		wiphy_err(wiphy, "Failed to parse secuirty parameters!\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to parse secuirty parameters!\n");
 		return -1;
 	}
 
@@ -1775,20 +1867,25 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
 
 	mwifiex_set_wmm_params(priv, bss_cfg, params);
 
+	if (mwifiex_is_11h_active(priv))
+		mwifiex_set_tpc_params(priv, bss_cfg, params);
+
 	if (mwifiex_is_11h_active(priv) &&
 	    !cfg80211_chandef_dfs_required(wiphy, &params->chandef,
 					   priv->bss_mode)) {
-		dev_dbg(priv->adapter->dev, "Disable 11h extensions in FW\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "Disable 11h extensions in FW\n");
 		if (mwifiex_11h_activate(priv, false)) {
-			dev_err(priv->adapter->dev,
-				"Failed to disable 11h extensions!!");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "Failed to disable 11h extensions!!");
 			return -1;
 		}
-		priv->state_11h.is_11h_active = true;
+		priv->state_11h.is_11h_active = false;
 	}
 
 	if (mwifiex_config_start_uap(priv, bss_cfg)) {
-		wiphy_err(wiphy, "Failed to start AP\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to start AP\n");
 		kfree(bss_cfg);
 		return -1;
 	}
@@ -1816,8 +1913,9 @@ mwifiex_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
 	if (mwifiex_deauthenticate(priv, NULL))
 		return -EFAULT;
 
-	wiphy_dbg(wiphy, "info: successfully disconnected from %pM:"
-		" reason code %d\n", priv->cfg_bssid, reason_code);
+	mwifiex_dbg(priv->adapter, MSG,
+		    "info: successfully disconnected from %pM:\t"
+		    "reason code %d\n", priv->cfg_bssid, reason_code);
 
 	eth_zero_addr(priv->cfg_bssid);
 	priv->hs2_enabled = false;
@@ -1899,13 +1997,13 @@ mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len,
 
 	req_ssid.ssid_len = ssid_len;
 	if (ssid_len > IEEE80211_MAX_SSID_LEN) {
-		dev_err(priv->adapter->dev, "invalid SSID - aborting\n");
+		mwifiex_dbg(priv->adapter, ERROR, "invalid SSID - aborting\n");
 		return -EINVAL;
 	}
 
 	memcpy(req_ssid.ssid, ssid, ssid_len);
 	if (!req_ssid.ssid_len || req_ssid.ssid[0] < 0x20) {
-		dev_err(priv->adapter->dev, "invalid SSID - aborting\n");
+		mwifiex_dbg(priv->adapter, ERROR, "invalid SSID - aborting\n");
 		return -EINVAL;
 	}
 
@@ -1959,9 +2057,9 @@ mwifiex_cfg80211_assoc(struct mwifiex_private *priv, size_t ssid_len,
 
 	if (sme->key) {
 		if (mwifiex_is_alg_wep(priv->sec_info.encryption_mode)) {
-			dev_dbg(priv->adapter->dev,
-				"info: setting wep encryption"
-				" with key len %d\n", sme->key_len);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: setting wep encryption\t"
+				    "with key len %d\n", sme->key_len);
 			priv->wep_key_curr_index = sme->key_idx;
 			ret = mwifiex_set_encode(priv, NULL, sme->key,
 						 sme->key_len, sme->key_idx,
@@ -1978,7 +2076,7 @@ done:
 		if (is_scanning_required) {
 			/* Do specific SSID scanning */
 			if (mwifiex_request_scan(priv, &req_ssid)) {
-				dev_err(priv->adapter->dev, "scan error\n");
+				mwifiex_dbg(priv->adapter, ERROR, "scan error\n");
 				return -EFAULT;
 			}
 		}
@@ -1997,15 +2095,15 @@ done:
 
 		if (!bss) {
 			if (is_scanning_required) {
-				dev_warn(priv->adapter->dev,
-					 "assoc: requested bss not found in scan results\n");
+				mwifiex_dbg(priv->adapter, WARN,
+					    "assoc: requested bss not found in scan results\n");
 				break;
 			}
 			is_scanning_required = 1;
 		} else {
-			dev_dbg(priv->adapter->dev,
-				"info: trying to associate to '%s' bssid %pM\n",
-				(char *) req_ssid.ssid, bss->bssid);
+			mwifiex_dbg(priv->adapter, MSG,
+				    "info: trying to associate to '%s' bssid %pM\n",
+				    (char *)req_ssid.ssid, bss->bssid);
 			memcpy(&priv->cfg_bssid, bss->bssid, ETH_ALEN);
 			break;
 		}
@@ -2041,26 +2139,29 @@ mwifiex_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 	int ret;
 
 	if (GET_BSS_ROLE(priv) != MWIFIEX_BSS_ROLE_STA) {
-		wiphy_err(wiphy,
-			  "%s: reject infra assoc request in non-STA role\n",
-			  dev->name);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: reject infra assoc request in non-STA role\n",
+			    dev->name);
 		return -EINVAL;
 	}
 
 	if (priv->wdev.current_bss) {
-		wiphy_warn(wiphy, "%s: already connected\n", dev->name);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: already connected\n", dev->name);
 		return -EALREADY;
 	}
 
 	if (adapter->surprise_removed || adapter->is_cmd_timedout) {
-		wiphy_err(wiphy,
-			  "%s: Ignore connection. Card removed or FW in bad state\n",
-			  dev->name);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: Ignore connection.\t"
+			    "Card removed or FW in bad state\n",
+			    dev->name);
 		return -EFAULT;
 	}
 
-	wiphy_dbg(wiphy, "info: Trying to associate to %s and bssid %pM\n",
-		  (char *) sme->ssid, sme->bssid);
+	mwifiex_dbg(adapter, INFO,
+		    "info: Trying to associate to %s and bssid %pM\n",
+		    (char *)sme->ssid, sme->bssid);
 
 	ret = mwifiex_cfg80211_assoc(priv, sme->ssid_len, sme->ssid, sme->bssid,
 				     priv->bss_mode, sme->channel, sme, 0);
@@ -2068,17 +2169,17 @@ mwifiex_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 		cfg80211_connect_result(priv->netdev, priv->cfg_bssid, NULL, 0,
 					NULL, 0, WLAN_STATUS_SUCCESS,
 					GFP_KERNEL);
-		dev_dbg(priv->adapter->dev,
-			"info: associated to bssid %pM successfully\n",
-			priv->cfg_bssid);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "info: associated to bssid %pM successfully\n",
+			    priv->cfg_bssid);
 		if (ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info) &&
 		    priv->adapter->auto_tdls &&
 		    priv->bss_type == MWIFIEX_BSS_TYPE_STA)
 			mwifiex_setup_auto_tdls_timer(priv);
 	} else {
-		dev_dbg(priv->adapter->dev,
-			"info: association to bssid %pM failed\n",
-			priv->cfg_bssid);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "info: association to bssid %pM failed\n",
+			    priv->cfg_bssid);
 		eth_zero_addr(priv->cfg_bssid);
 
 		if (ret > 0)
@@ -2105,7 +2206,6 @@ mwifiex_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 static int mwifiex_set_ibss_params(struct mwifiex_private *priv,
 				   struct cfg80211_ibss_params *params)
 {
-	struct wiphy *wiphy = priv->wdev.wiphy;
 	struct mwifiex_adapter *adapter = priv->adapter;
 	int index = 0, i;
 	u8 config_bands = 0;
@@ -2162,8 +2262,10 @@ static int mwifiex_set_ibss_params(struct mwifiex_private *priv,
 	priv->adhoc_channel = ieee80211_frequency_to_channel(
 				params->chandef.chan->center_freq);
 
-	wiphy_dbg(wiphy, "info: set ibss band %d, chan %d, chan offset %d\n",
-		  config_bands, priv->adhoc_channel, adapter->sec_chan_offset);
+	mwifiex_dbg(adapter, INFO,
+		    "info: set ibss band %d, chan %d, chan offset %d\n",
+		    config_bands, priv->adhoc_channel,
+		    adapter->sec_chan_offset);
 
 	return 0;
 }
@@ -2182,13 +2284,15 @@ mwifiex_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	int ret = 0;
 
 	if (priv->bss_mode != NL80211_IFTYPE_ADHOC) {
-		wiphy_err(wiphy, "request to join ibss received "
-				"when station is not in ibss mode\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "request to join ibss received\t"
+			    "when station is not in ibss mode\n");
 		goto done;
 	}
 
-	wiphy_dbg(wiphy, "info: trying to join to %s and bssid %pM\n",
-		  (char *) params->ssid, params->bssid);
+	mwifiex_dbg(priv->adapter, MSG,
+		    "info: trying to join to %s and bssid %pM\n",
+		    (char *)params->ssid, params->bssid);
 
 	mwifiex_set_ibss_params(priv, params);
 
@@ -2200,12 +2304,12 @@ done:
 	if (!ret) {
 		cfg80211_ibss_joined(priv->netdev, priv->cfg_bssid,
 				     params->chandef.chan, GFP_KERNEL);
-		dev_dbg(priv->adapter->dev,
-			"info: joined/created adhoc network with bssid"
-			" %pM successfully\n", priv->cfg_bssid);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "info: joined/created adhoc network with bssid\t"
+			    "%pM successfully\n", priv->cfg_bssid);
 	} else {
-		dev_dbg(priv->adapter->dev,
-			"info: failed creating/joining adhoc network\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "info: failed creating/joining adhoc network\n");
 	}
 
 	return ret;
@@ -2222,8 +2326,8 @@ mwifiex_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 {
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
 
-	wiphy_dbg(wiphy, "info: disconnecting from essid %pM\n",
-		  priv->cfg_bssid);
+	mwifiex_dbg(priv->adapter, MSG, "info: disconnecting from essid %pM\n",
+		    priv->cfg_bssid);
 	if (mwifiex_deauthenticate(priv, NULL))
 		return -EFAULT;
 
@@ -2250,13 +2354,15 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 	struct ieee_types_header *ie;
 	struct mwifiex_user_scan_cfg *user_scan_cfg;
 
-	wiphy_dbg(wiphy, "info: received scan request on %s\n", dev->name);
+	mwifiex_dbg(priv->adapter, CMD,
+		    "info: received scan request on %s\n", dev->name);
 
 	/* Block scan request if scan operation or scan cleanup when interface
 	 * is disabled is in process
 	 */
 	if (priv->scan_request || priv->scan_aborting) {
-		dev_err(priv->adapter->dev, "cmd: Scan already in process..\n");
+		mwifiex_dbg(priv->adapter, WARN,
+			    "cmd: Scan already in process..\n");
 		return -EBUSY;
 	}
 
@@ -2308,7 +2414,8 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 	ret = mwifiex_scan_networks(priv, user_scan_cfg);
 	kfree(user_scan_cfg);
 	if (ret) {
-		dev_err(priv->adapter->dev, "scan failed: %d\n", ret);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "scan failed: %d\n", ret);
 		priv->scan_aborting = false;
 		priv->scan_request = NULL;
 		return ret;
@@ -2454,15 +2561,15 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 	case NL80211_IFTYPE_ADHOC:
 		if (adapter->curr_iface_comb.sta_intf ==
 		    adapter->iface_limit.sta_intf) {
-			wiphy_err(wiphy,
-				  "cannot create multiple sta/adhoc ifaces\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "cannot create multiple sta/adhoc ifaces\n");
 			return ERR_PTR(-EINVAL);
 		}
 
 		priv = mwifiex_get_unused_priv(adapter);
 		if (!priv) {
-			wiphy_err(wiphy,
-				  "could not get free private struct\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "could not get free private struct\n");
 			return ERR_PTR(-EFAULT);
 		}
 
@@ -2478,21 +2585,21 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		priv->frame_type = MWIFIEX_DATA_FRAME_TYPE_ETH_II;
 		priv->bss_priority = 0;
 		priv->bss_role = MWIFIEX_BSS_ROLE_STA;
-		priv->bss_num = 0;
+		priv->bss_num = adapter->curr_iface_comb.sta_intf;
 
 		break;
 	case NL80211_IFTYPE_AP:
 		if (adapter->curr_iface_comb.uap_intf ==
 		    adapter->iface_limit.uap_intf) {
-			wiphy_err(wiphy,
-				  "cannot create multiple AP ifaces\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "cannot create multiple AP ifaces\n");
 			return ERR_PTR(-EINVAL);
 		}
 
 		priv = mwifiex_get_unused_priv(adapter);
 		if (!priv) {
-			wiphy_err(wiphy,
-				  "could not get free private struct\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "could not get free private struct\n");
 			return ERR_PTR(-EFAULT);
 		}
 
@@ -2504,22 +2611,22 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		priv->bss_priority = 0;
 		priv->bss_role = MWIFIEX_BSS_ROLE_UAP;
 		priv->bss_started = 0;
-		priv->bss_num = 0;
+		priv->bss_num = adapter->curr_iface_comb.uap_intf;
 		priv->bss_mode = type;
 
 		break;
 	case NL80211_IFTYPE_P2P_CLIENT:
 		if (adapter->curr_iface_comb.p2p_intf ==
 		    adapter->iface_limit.p2p_intf) {
-			wiphy_err(wiphy,
-				  "cannot create multiple P2P ifaces\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "cannot create multiple P2P ifaces\n");
 			return ERR_PTR(-EINVAL);
 		}
 
 		priv = mwifiex_get_unused_priv(adapter);
 		if (!priv) {
-			wiphy_err(wiphy,
-				  "could not get free private struct\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "could not get free private struct\n");
 			return ERR_PTR(-EFAULT);
 		}
 
@@ -2540,7 +2647,7 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		priv->bss_priority = MWIFIEX_BSS_ROLE_STA;
 		priv->bss_role = MWIFIEX_BSS_ROLE_STA;
 		priv->bss_started = 0;
-		priv->bss_num = 0;
+		priv->bss_num = adapter->curr_iface_comb.p2p_intf;
 
 		if (mwifiex_cfg80211_init_p2p_client(priv)) {
 			memset(&priv->wdev, 0, sizeof(priv->wdev));
@@ -2550,7 +2657,7 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 		break;
 	default:
-		wiphy_err(wiphy, "type not supported\n");
+		mwifiex_dbg(adapter, ERROR, "type not supported\n");
 		return ERR_PTR(-EINVAL);
 	}
 
@@ -2558,7 +2665,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 			       name_assign_type, ether_setup,
 			       IEEE80211_NUM_ACS, 1);
 	if (!dev) {
-		wiphy_err(wiphy, "no memory available for netdevice\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "no memory available for netdevice\n");
 		memset(&priv->wdev, 0, sizeof(priv->wdev));
 		priv->wdev.iftype = NL80211_IFTYPE_UNSPECIFIED;
 		priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
@@ -2599,7 +2707,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 	/* Register network device */
 	if (register_netdevice(dev)) {
-		wiphy_err(wiphy, "cannot register virtual network device\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot register virtual network device\n");
 		free_netdev(dev);
 		priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
 		priv->netdev = NULL;
@@ -2613,7 +2722,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 						  WQ_MEM_RECLAIM |
 						  WQ_UNBOUND, 1, name);
 	if (!priv->dfs_cac_workqueue) {
-		wiphy_err(wiphy, "cannot register virtual network device\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot register virtual network device\n");
 		free_netdev(dev);
 		priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
 		priv->netdev = NULL;
@@ -2628,7 +2738,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 						      WQ_HIGHPRI | WQ_UNBOUND |
 						      WQ_MEM_RECLAIM, 1, name);
 	if (!priv->dfs_chan_sw_workqueue) {
-		wiphy_err(wiphy, "cannot register virtual network device\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot register virtual network device\n");
 		free_netdev(dev);
 		priv->bss_mode = NL80211_IFTYPE_UNSPECIFIED;
 		priv->netdev = NULL;
@@ -2642,7 +2753,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 	sema_init(&priv->async_sem, 1);
 
-	dev_dbg(adapter->dev, "info: %s: Marvell 802.11 Adapter\n", dev->name);
+	mwifiex_dbg(adapter, INFO,
+		    "info: %s: Marvell 802.11 Adapter\n", dev->name);
 
 #ifdef CONFIG_DEBUG_FS
 	mwifiex_dev_debugfs_init(priv);
@@ -2661,7 +2773,7 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		adapter->curr_iface_comb.p2p_intf++;
 		break;
 	default:
-		wiphy_err(wiphy, "type not supported\n");
+		mwifiex_dbg(adapter, ERROR, "type not supported\n");
 		return ERR_PTR(-EINVAL);
 	}
 
@@ -2721,7 +2833,8 @@ int mwifiex_del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev)
 		adapter->curr_iface_comb.p2p_intf++;
 		break;
 	default:
-		dev_err(adapter->dev, "del_virtual_intf: type not supported\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "del_virtual_intf: type not supported\n");
 		break;
 	}
 
@@ -2839,7 +2952,8 @@ static int mwifiex_set_wowlan_mef_entry(struct mwifiex_private *priv,
 		if (!mwifiex_is_pattern_supported(&wowlan->patterns[i],
 					byte_seq,
 					MWIFIEX_MEF_MAX_BYTESEQ)) {
-			dev_err(priv->adapter->dev, "Pattern not supported\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "Pattern not supported\n");
 			kfree(mef_entry);
 			return -EOPNOTSUPP;
 		}
@@ -2954,21 +3068,22 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 	mwifiex_cancel_all_pending_cmd(adapter);
 
 	if (!wowlan) {
-		dev_warn(adapter->dev, "None of the WOWLAN triggers enabled\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "None of the WOWLAN triggers enabled\n");
 		return 0;
 	}
 
 	priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_STA);
 
 	if (!priv->media_connected) {
-		dev_warn(adapter->dev,
-			 "Can not configure WOWLAN in disconnected state\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Can not configure WOWLAN in disconnected state\n");
 		return 0;
 	}
 
 	ret = mwifiex_set_mef_filter(priv, wowlan);
 	if (ret) {
-		dev_err(adapter->dev, "Failed to set MEF filter\n");
+		mwifiex_dbg(adapter, ERROR, "Failed to set MEF filter\n");
 		return ret;
 	}
 
@@ -2981,7 +3096,8 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 		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");
+			mwifiex_dbg(adapter, ERROR,
+				    "Failed to set HS params\n");
 			return ret;
 		}
 	}
@@ -3041,7 +3157,8 @@ mwifiex_fill_coalesce_rule_info(struct mwifiex_private *priv,
 		if (!mwifiex_is_pattern_supported(&crule->patterns[i],
 						  byte_seq,
 						MWIFIEX_COALESCE_MAX_BYTESEQ)) {
-			dev_err(priv->adapter->dev, "Pattern not supported\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "Pattern not supported\n");
 			return -EOPNOTSUPP;
 		}
 
@@ -3050,8 +3167,8 @@ mwifiex_fill_coalesce_rule_info(struct mwifiex_private *priv,
 
 			pkt_type = mwifiex_get_coalesce_pkt_type(byte_seq);
 			if (pkt_type && mrule->pkt_type) {
-				dev_err(priv->adapter->dev,
-					"Multiple packet types not allowed\n");
+				mwifiex_dbg(priv->adapter, ERROR,
+					    "Multiple packet types not allowed\n");
 				return -EOPNOTSUPP;
 			} else if (pkt_type) {
 				mrule->pkt_type = pkt_type;
@@ -3074,8 +3191,8 @@ mwifiex_fill_coalesce_rule_info(struct mwifiex_private *priv,
 	}
 
 	if (!mrule->pkt_type) {
-		dev_err(priv->adapter->dev,
-			"Packet type can not be determined\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Packet type can not be determined\n");
 		return -EOPNOTSUPP;
 	}
 
@@ -3093,8 +3210,8 @@ static int mwifiex_cfg80211_set_coalesce(struct wiphy *wiphy,
 
 	memset(&coalesce_cfg, 0, sizeof(coalesce_cfg));
 	if (!coalesce) {
-		dev_dbg(adapter->dev,
-			"Disable coalesce and reset all previous rules\n");
+		mwifiex_dbg(adapter, WARN,
+			    "Disable coalesce and reset all previous rules\n");
 		return mwifiex_send_cmd(priv, HostCmd_CMD_COALESCE_CFG,
 					HostCmd_ACT_GEN_SET, 0,
 					&coalesce_cfg, true);
@@ -3105,8 +3222,8 @@ static int mwifiex_cfg80211_set_coalesce(struct wiphy *wiphy,
 		ret = mwifiex_fill_coalesce_rule_info(priv, &coalesce->rules[i],
 						      &coalesce_cfg.rule[i]);
 		if (ret) {
-			dev_err(priv->adapter->dev,
-				"Recheck the patterns provided for rule %d\n",
+			mwifiex_dbg(adapter, ERROR,
+				    "Recheck the patterns provided for rule %d\n",
 				i + 1);
 			return ret;
 		}
@@ -3138,9 +3255,9 @@ mwifiex_cfg80211_tdls_mgmt(struct wiphy *wiphy, struct net_device *dev,
 
 	switch (action_code) {
 	case WLAN_TDLS_SETUP_REQUEST:
-		dev_dbg(priv->adapter->dev,
-			"Send TDLS Setup Request to %pM status_code=%d\n", peer,
-			 status_code);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "Send TDLS Setup Request to %pM status_code=%d\n",
+			    peer, status_code);
 		mwifiex_add_auto_tdls_peer(priv, peer);
 		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
 						   dialog_token, status_code,
@@ -3148,45 +3265,45 @@ mwifiex_cfg80211_tdls_mgmt(struct wiphy *wiphy, struct net_device *dev,
 		break;
 	case WLAN_TDLS_SETUP_RESPONSE:
 		mwifiex_add_auto_tdls_peer(priv, peer);
-		dev_dbg(priv->adapter->dev,
-			"Send TDLS Setup Response to %pM status_code=%d\n",
-			peer, status_code);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "Send TDLS Setup Response to %pM status_code=%d\n",
+			    peer, status_code);
 		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
 						   dialog_token, status_code,
 						   extra_ies, extra_ies_len);
 		break;
 	case WLAN_TDLS_SETUP_CONFIRM:
-		dev_dbg(priv->adapter->dev,
-			"Send TDLS Confirm to %pM status_code=%d\n", peer,
-			status_code);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "Send TDLS Confirm to %pM status_code=%d\n", peer,
+			    status_code);
 		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
 						   dialog_token, status_code,
 						   extra_ies, extra_ies_len);
 		break;
 	case WLAN_TDLS_TEARDOWN:
-		dev_dbg(priv->adapter->dev, "Send TDLS Tear down to %pM\n",
-			peer);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "Send TDLS Tear down to %pM\n", peer);
 		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
 						   dialog_token, status_code,
 						   extra_ies, extra_ies_len);
 		break;
 	case WLAN_TDLS_DISCOVERY_REQUEST:
-		dev_dbg(priv->adapter->dev,
-			"Send TDLS Discovery Request to %pM\n", peer);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "Send TDLS Discovery Request to %pM\n", peer);
 		ret = mwifiex_send_tdls_data_frame(priv, peer, action_code,
 						   dialog_token, status_code,
 						   extra_ies, extra_ies_len);
 		break;
 	case WLAN_PUB_ACTION_TDLS_DISCOVER_RES:
-		dev_dbg(priv->adapter->dev,
-			"Send TDLS Discovery Response to %pM\n", peer);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "Send TDLS Discovery Response to %pM\n", peer);
 		ret = mwifiex_send_tdls_action_frame(priv, peer, action_code,
 						   dialog_token, status_code,
 						   extra_ies, extra_ies_len);
 		break;
 	default:
-		dev_warn(priv->adapter->dev,
-			 "Unknown TDLS mgmt/action frame %pM\n", peer);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Unknown TDLS mgmt/action frame %pM\n", peer);
 		ret = -EINVAL;
 		break;
 	}
@@ -3208,8 +3325,8 @@ mwifiex_cfg80211_tdls_oper(struct wiphy *wiphy, struct net_device *dev,
 	if (!(priv->bss_type == MWIFIEX_BSS_TYPE_STA && priv->media_connected))
 		return -ENOTSUPP;
 
-	dev_dbg(priv->adapter->dev,
-		"TDLS peer=%pM, oper=%d\n", peer, action);
+	mwifiex_dbg(priv->adapter, MSG,
+		    "TDLS peer=%pM, oper=%d\n", peer, action);
 
 	switch (action) {
 	case NL80211_TDLS_ENABLE_LINK:
@@ -3220,22 +3337,22 @@ mwifiex_cfg80211_tdls_oper(struct wiphy *wiphy, struct net_device *dev,
 		break;
 	case NL80211_TDLS_TEARDOWN:
 		/* shouldn't happen!*/
-		dev_warn(priv->adapter->dev,
-			 "tdls_oper: teardown from driver not supported\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "tdls_oper: teardown from driver not supported\n");
 		return -EINVAL;
 	case NL80211_TDLS_SETUP:
 		/* shouldn't happen!*/
-		dev_warn(priv->adapter->dev,
-			 "tdls_oper: setup from driver not supported\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "tdls_oper: setup from driver not supported\n");
 		return -EINVAL;
 	case NL80211_TDLS_DISCOVERY_REQ:
 		/* shouldn't happen!*/
-		dev_warn(priv->adapter->dev,
-			 "tdls_oper: discovery from driver not supported\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "tdls_oper: discovery from driver not supported\n");
 		return -EINVAL;
 	default:
-		dev_err(priv->adapter->dev,
-			"tdls_oper: operation not supported\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "tdls_oper: operation not supported\n");
 		return -ENOTSUPP;
 	}
 
@@ -3268,8 +3385,8 @@ mwifiex_cfg80211_channel_switch(struct wiphy *wiphy, struct net_device *dev,
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
 
 	if (priv->adapter->scan_processing) {
-		dev_err(priv->adapter->dev,
-			"radar detection: scan in process...\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "radar detection: scan in process...\n");
 		return -EBUSY;
 	}
 
@@ -3284,8 +3401,8 @@ mwifiex_cfg80211_channel_switch(struct wiphy *wiphy, struct net_device *dev,
 					   params->beacon_csa.tail,
 					   params->beacon_csa.tail_len);
 	if (!chsw_ie) {
-		dev_err(priv->adapter->dev,
-			"Could not parse channel switch announcement IE\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Could not parse channel switch announcement IE\n");
 		return -EINVAL;
 	}
 
@@ -3297,10 +3414,12 @@ mwifiex_cfg80211_channel_switch(struct wiphy *wiphy, struct net_device *dev,
 	}
 
 	if (mwifiex_del_mgmt_ies(priv))
-		wiphy_err(wiphy, "Failed to delete mgmt IEs!\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to delete mgmt IEs!\n");
 
 	if (mwifiex_set_mgmt_ies(priv, &params->beacon_csa)) {
-		wiphy_err(wiphy, "%s: setting mgmt ies failed\n", __func__);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: setting mgmt ies failed\n", __func__);
 		return -EFAULT;
 	}
 
@@ -3314,6 +3433,45 @@ mwifiex_cfg80211_channel_switch(struct wiphy *wiphy, struct net_device *dev,
 	return 0;
 }
 
+static int mwifiex_cfg80211_get_channel(struct wiphy *wiphy,
+					struct wireless_dev *wdev,
+					struct cfg80211_chan_def *chandef)
+{
+	struct mwifiex_private *priv = mwifiex_netdev_get_priv(wdev->netdev);
+	struct mwifiex_bssdescriptor *curr_bss;
+	struct ieee80211_channel *chan;
+	u8 second_chan_offset;
+	enum nl80211_channel_type chan_type;
+	enum ieee80211_band band;
+	int freq;
+	int ret = -ENODATA;
+
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP &&
+	    cfg80211_chandef_valid(&priv->bss_chandef)) {
+		*chandef = priv->bss_chandef;
+		ret = 0;
+	} else if (priv->media_connected) {
+		curr_bss = &priv->curr_bss_params.bss_descriptor;
+		band = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
+		freq = ieee80211_channel_to_frequency(curr_bss->channel, band);
+		chan = ieee80211_get_channel(wiphy, freq);
+
+		if (curr_bss->bcn_ht_oper) {
+			second_chan_offset = curr_bss->bcn_ht_oper->ht_param &
+					IEEE80211_HT_PARAM_CHA_SEC_OFFSET;
+			chan_type = mwifiex_sec_chan_offset_to_chan_type
+							(second_chan_offset);
+			cfg80211_chandef_create(chandef, chan, chan_type);
+		} else {
+			cfg80211_chandef_create(chandef, chan,
+						NL80211_CHAN_NO_HT);
+		}
+		ret = 0;
+	}
+
+	return ret;
+}
+
 static int
 mwifiex_cfg80211_start_radar_detection(struct wiphy *wiphy,
 				       struct net_device *dev,
@@ -3324,16 +3482,17 @@ mwifiex_cfg80211_start_radar_detection(struct wiphy *wiphy,
 	struct mwifiex_radar_params radar_params;
 
 	if (priv->adapter->scan_processing) {
-		dev_err(priv->adapter->dev,
-			"radar detection: scan already in process...\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "radar detection: scan already in process...\n");
 		return -EBUSY;
 	}
 
 	if (!mwifiex_is_11h_active(priv)) {
-		dev_dbg(priv->adapter->dev, "Enable 11h extensions in FW\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "Enable 11h extensions in FW\n");
 		if (mwifiex_11h_activate(priv, true)) {
-			dev_err(priv->adapter->dev,
-				"Failed to activate 11h extensions!!");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "Failed to activate 11h extensions!!");
 			return -1;
 		}
 		priv->state_11h.is_11h_active = true;
@@ -3418,6 +3577,7 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = {
 	.tdls_oper = mwifiex_cfg80211_tdls_oper,
 	.add_station = mwifiex_cfg80211_add_station,
 	.change_station = mwifiex_cfg80211_change_station,
+	.get_channel = mwifiex_cfg80211_get_channel,
 	.start_radar_detection = mwifiex_cfg80211_start_radar_detection,
 	.channel_switch = mwifiex_cfg80211_channel_switch,
 };
@@ -3492,7 +3652,8 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 	wiphy = wiphy_new(&mwifiex_cfg80211_ops,
 			  sizeof(struct mwifiex_adapter *));
 	if (!wiphy) {
-		dev_err(adapter->dev, "%s: creating new wiphy\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: creating new wiphy\n", __func__);
 		return -ENOMEM;
 	}
 	wiphy->max_scan_ssids = MWIFIEX_MAX_SSID_LIST_LENGTH;
@@ -3524,7 +3685,8 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 			WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD |
 			WIPHY_FLAG_AP_UAPSD |
 			WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
-			WIPHY_FLAG_HAS_CHANNEL_SWITCH;
+			WIPHY_FLAG_HAS_CHANNEL_SWITCH |
+			WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
 	if (ISSUPP_TDLS_ENABLED(adapter->fw_cap_info))
 		wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS |
@@ -3563,20 +3725,22 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 
 	ret = wiphy_register(wiphy);
 	if (ret < 0) {
-		dev_err(adapter->dev,
-			"%s: wiphy_register failed: %d\n", __func__, ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: wiphy_register failed: %d\n", __func__, ret);
 		wiphy_free(wiphy);
 		return ret;
 	}
 
 	if (reg_alpha2 && mwifiex_is_valid_alpha2(reg_alpha2)) {
-		wiphy_info(wiphy, "driver hint alpha2: %2.2s\n", reg_alpha2);
+		mwifiex_dbg(adapter, INFO,
+			    "driver hint alpha2: %2.2s\n", reg_alpha2);
 		regulatory_hint(wiphy, reg_alpha2);
 	} else {
 		country_code = mwifiex_11d_code_2_region(adapter->region_code);
 		if (country_code)
-			wiphy_info(wiphy, "ignoring F/W country code %2.2s\n",
-				   country_code);
+			mwifiex_dbg(adapter, WARN,
+				    "ignoring F/W country code %2.2s\n",
+				    country_code);
 	}
 
 	mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
diff --git a/drivers/net/wireless/mwifiex/cfp.c b/drivers/net/wireless/mwifiex/cfp.c
index e9df8826f124..3ddb8ec676ed 100644
--- a/drivers/net/wireless/mwifiex/cfp.c
+++ b/drivers/net/wireless/mwifiex/cfp.c
@@ -327,8 +327,9 @@ mwifiex_get_cfp(struct mwifiex_private *priv, u8 band, u16 channel, u32 freq)
 		sband = priv->wdev.wiphy->bands[IEEE80211_BAND_5GHZ];
 
 	if (!sband) {
-		dev_err(priv->adapter->dev, "%s: cannot find cfp by band %d\n",
-			__func__, band);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: cannot find cfp by band %d\n",
+			    __func__, band);
 		return cfp;
 	}
 
@@ -349,9 +350,10 @@ mwifiex_get_cfp(struct mwifiex_private *priv, u8 band, u16 channel, u32 freq)
 		}
 	}
 	if (i == sband->n_channels) {
-		dev_err(priv->adapter->dev, "%s: cannot find cfp by band %d"
-			" & channel=%d freq=%d\n", __func__, band, channel,
-			freq);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: cannot find cfp by band %d\t"
+			    "& channel=%d freq=%d\n",
+			    __func__, band, channel, freq);
 	} else {
 		if (!ch)
 			return cfp;
@@ -431,15 +433,17 @@ u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates)
 	    priv->bss_mode == NL80211_IFTYPE_P2P_CLIENT) {
 		switch (adapter->config_bands) {
 		case BAND_B:
-			dev_dbg(adapter->dev, "info: infra band=%d "
-				"supported_rates_b\n", adapter->config_bands);
+			mwifiex_dbg(adapter, INFO, "info: infra band=%d\t"
+				    "supported_rates_b\n",
+				    adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_b,
 					       sizeof(supported_rates_b));
 			break;
 		case BAND_G:
 		case BAND_G | BAND_GN:
-			dev_dbg(adapter->dev, "info: infra band=%d "
-				"supported_rates_g\n", adapter->config_bands);
+			mwifiex_dbg(adapter, INFO, "info: infra band=%d\t"
+				    "supported_rates_g\n",
+				    adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_g,
 					       sizeof(supported_rates_g));
 			break;
@@ -449,15 +453,17 @@ u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates)
 		case BAND_A | BAND_B | BAND_G | BAND_GN | BAND_AN:
 		case BAND_A | BAND_B | BAND_G | BAND_GN | BAND_AN | BAND_AAC:
 		case BAND_B | BAND_G | BAND_GN:
-			dev_dbg(adapter->dev, "info: infra band=%d "
-				"supported_rates_bg\n", adapter->config_bands);
+			mwifiex_dbg(adapter, INFO, "info: infra band=%d\t"
+				    "supported_rates_bg\n",
+				    adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_bg,
 					       sizeof(supported_rates_bg));
 			break;
 		case BAND_A:
 		case BAND_A | BAND_G:
-			dev_dbg(adapter->dev, "info: infra band=%d "
-				"supported_rates_a\n", adapter->config_bands);
+			mwifiex_dbg(adapter, INFO, "info: infra band=%d\t"
+				    "supported_rates_a\n",
+				    adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_a,
 					       sizeof(supported_rates_a));
 			break;
@@ -466,14 +472,16 @@ u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates)
 		case BAND_A | BAND_AN | BAND_AAC:
 		case BAND_A | BAND_G | BAND_AN | BAND_GN:
 		case BAND_A | BAND_G | BAND_AN | BAND_GN | BAND_AAC:
-			dev_dbg(adapter->dev, "info: infra band=%d "
-				"supported_rates_a\n", adapter->config_bands);
+			mwifiex_dbg(adapter, INFO, "info: infra band=%d\t"
+				    "supported_rates_a\n",
+				    adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_a,
 					       sizeof(supported_rates_a));
 			break;
 		case BAND_GN:
-			dev_dbg(adapter->dev, "info: infra band=%d "
-				"supported_rates_n\n", adapter->config_bands);
+			mwifiex_dbg(adapter, INFO, "info: infra band=%d\t"
+				    "supported_rates_n\n",
+				    adapter->config_bands);
 			k = mwifiex_copy_rates(rates, k, supported_rates_n,
 					       sizeof(supported_rates_n));
 			break;
@@ -482,25 +490,25 @@ u32 mwifiex_get_supported_rates(struct mwifiex_private *priv, u8 *rates)
 		/* Ad-hoc mode */
 		switch (adapter->adhoc_start_band) {
 		case BAND_B:
-			dev_dbg(adapter->dev, "info: adhoc B\n");
+			mwifiex_dbg(adapter, INFO, "info: adhoc B\n");
 			k = mwifiex_copy_rates(rates, k, adhoc_rates_b,
 					       sizeof(adhoc_rates_b));
 			break;
 		case BAND_G:
 		case BAND_G | BAND_GN:
-			dev_dbg(adapter->dev, "info: adhoc G only\n");
+			mwifiex_dbg(adapter, INFO, "info: adhoc G only\n");
 			k = mwifiex_copy_rates(rates, k, adhoc_rates_g,
 					       sizeof(adhoc_rates_g));
 			break;
 		case BAND_B | BAND_G:
 		case BAND_B | BAND_G | BAND_GN:
-			dev_dbg(adapter->dev, "info: adhoc BG\n");
+			mwifiex_dbg(adapter, INFO, "info: adhoc BG\n");
 			k = mwifiex_copy_rates(rates, k, adhoc_rates_bg,
 					       sizeof(adhoc_rates_bg));
 			break;
 		case BAND_A:
 		case BAND_A | BAND_AN:
-			dev_dbg(adapter->dev, "info: adhoc A\n");
+			mwifiex_dbg(adapter, INFO, "info: adhoc A\n");
 			k = mwifiex_copy_rates(rates, k, adhoc_rates_a,
 					       sizeof(adhoc_rates_a));
 			break;
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index c5a14ff7eb82..207da40500f4 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -62,7 +62,8 @@ mwifiex_get_cmd_node(struct mwifiex_adapter *adapter)
 
 	spin_lock_irqsave(&adapter->cmd_free_q_lock, flags);
 	if (list_empty(&adapter->cmd_free_q)) {
-		dev_err(adapter->dev, "GET_CMD_NODE: cmd node not available\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "GET_CMD_NODE: cmd node not available\n");
 		spin_unlock_irqrestore(&adapter->cmd_free_q_lock, flags);
 		return NULL;
 	}
@@ -116,7 +117,8 @@ static int mwifiex_cmd_host_cmd(struct mwifiex_private *priv,
 {
 	/* Copy the HOST command to command buffer */
 	memcpy(cmd, pcmd_ptr->cmd, pcmd_ptr->len);
-	dev_dbg(priv->adapter->dev, "cmd: host cmd size = %d\n", pcmd_ptr->len);
+	mwifiex_dbg(priv->adapter, CMD,
+		    "cmd: host cmd size = %d\n", pcmd_ptr->len);
 	return 0;
 }
 
@@ -147,8 +149,9 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 
 	/* Sanity test */
 	if (host_cmd == NULL || host_cmd->size == 0) {
-		dev_err(adapter->dev, "DNLD_CMD: host_cmd is null"
-			" or cmd size is 0, not sending\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "DNLD_CMD: host_cmd is null\t"
+			    "or cmd size is 0, not sending\n");
 		if (cmd_node->wait_q_enabled)
 			adapter->cmd_wait_q.status = -1;
 		mwifiex_recycle_cmd_node(adapter, cmd_node);
@@ -161,8 +164,8 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 	if (adapter->hw_status == MWIFIEX_HW_STATUS_RESET &&
 	    cmd_code != HostCmd_CMD_FUNC_SHUTDOWN &&
 	    cmd_code != HostCmd_CMD_FUNC_INIT) {
-		dev_err(adapter->dev,
-			"DNLD_CMD: FW in reset state, ignore cmd %#x\n",
+		mwifiex_dbg(adapter, ERROR,
+			    "DNLD_CMD: FW in reset state, ignore cmd %#x\n",
 			cmd_code);
 		if (cmd_node->wait_q_enabled)
 			mwifiex_complete_cmd(adapter, cmd_node);
@@ -197,10 +200,12 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 		 */
 		skb_put(cmd_node->cmd_skb, cmd_size - cmd_node->cmd_skb->len);
 
-	dev_dbg(adapter->dev,
-		"cmd: DNLD_CMD: %#x, act %#x, len %d, seqno %#x\n", cmd_code,
-		le16_to_cpu(*(__le16 *) ((u8 *) host_cmd + S_DS_GEN)), cmd_size,
-		le16_to_cpu(host_cmd->seq_num));
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: DNLD_CMD: %#x, act %#x, len %d, seqno %#x\n",
+		    cmd_code,
+		    le16_to_cpu(*(__le16 *)((u8 *)host_cmd + S_DS_GEN)),
+		    cmd_size, le16_to_cpu(host_cmd->seq_num));
+	mwifiex_dbg_dump(adapter, CMD_D, "cmd buffer:", host_cmd, cmd_size);
 
 	if (adapter->iface_type == MWIFIEX_USB) {
 		tmp = cpu_to_le32(MWIFIEX_USB_TYPE_CMD);
@@ -221,7 +226,8 @@ static int mwifiex_dnld_cmd_to_fw(struct mwifiex_private *priv,
 	}
 
 	if (ret == -1) {
-		dev_err(adapter->dev, "DNLD_CMD: host to card failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "DNLD_CMD: host to card failed\n");
 		if (adapter->iface_type == MWIFIEX_USB)
 			adapter->cmd_sent = false;
 		if (cmd_node->wait_q_enabled)
@@ -280,12 +286,14 @@ static int mwifiex_dnld_sleep_confirm_cmd(struct mwifiex_adapter *adapter)
 					(adapter->seq_num, priv->bss_num,
 					 priv->bss_type)));
 
-	dev_dbg(adapter->dev,
-		"cmd: DNLD_CMD: %#x, act %#x, len %d, seqno %#x\n",
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: DNLD_CMD: %#x, act %#x, len %d, seqno %#x\n",
 		le16_to_cpu(sleep_cfm_buf->command),
 		le16_to_cpu(sleep_cfm_buf->action),
 		le16_to_cpu(sleep_cfm_buf->size),
 		le16_to_cpu(sleep_cfm_buf->seq_num));
+	mwifiex_dbg_dump(adapter, CMD_D, "SLEEP_CFM buffer: ", sleep_cfm_buf,
+			 le16_to_cpu(sleep_cfm_buf->size));
 
 	if (adapter->iface_type == MWIFIEX_USB) {
 		sleep_cfm_tmp =
@@ -311,7 +319,7 @@ static int mwifiex_dnld_sleep_confirm_cmd(struct mwifiex_adapter *adapter)
 	}
 
 	if (ret == -1) {
-		dev_err(adapter->dev, "SLEEP_CFM: failed\n");
+		mwifiex_dbg(adapter, ERROR, "SLEEP_CFM: failed\n");
 		adapter->dbg.num_cmd_sleep_cfm_host_to_card_failure++;
 		return -1;
 	}
@@ -362,8 +370,9 @@ int mwifiex_alloc_cmd_buffer(struct mwifiex_adapter *adapter)
 	for (i = 0; i < MWIFIEX_NUM_OF_CMD_BUFFER; i++) {
 		cmd_array[i].skb = dev_alloc_skb(MWIFIEX_SIZE_OF_CMD_BUFFER);
 		if (!cmd_array[i].skb) {
-			dev_err(adapter->dev, "ALLOC_CMD_BUF: out of memory\n");
-			return -1;
+			mwifiex_dbg(adapter, ERROR,
+				    "unable to allocate command buffer\n");
+			return -ENOMEM;
 		}
 	}
 
@@ -386,7 +395,8 @@ int mwifiex_free_cmd_buffer(struct mwifiex_adapter *adapter)
 
 	/* Need to check if cmd pool is allocated or not */
 	if (!adapter->cmd_pool) {
-		dev_dbg(adapter->dev, "info: FREE_CMD_BUF: cmd_pool is null\n");
+		mwifiex_dbg(adapter, FATAL,
+			    "info: FREE_CMD_BUF: cmd_pool is null\n");
 		return 0;
 	}
 
@@ -395,7 +405,8 @@ int mwifiex_free_cmd_buffer(struct mwifiex_adapter *adapter)
 	/* Release shared memory buffers */
 	for (i = 0; i < MWIFIEX_NUM_OF_CMD_BUFFER; i++) {
 		if (cmd_array[i].skb) {
-			dev_dbg(adapter->dev, "cmd: free cmd buffer %d\n", i);
+			mwifiex_dbg(adapter, CMD,
+				    "cmd: free cmd buffer %d\n", i);
 			dev_kfree_skb_any(cmd_array[i].skb);
 		}
 		if (!cmd_array[i].resp_skb)
@@ -409,7 +420,8 @@ int mwifiex_free_cmd_buffer(struct mwifiex_adapter *adapter)
 	}
 	/* Release struct cmd_ctrl_node */
 	if (adapter->cmd_pool) {
-		dev_dbg(adapter->dev, "cmd: free cmd pool\n");
+		mwifiex_dbg(adapter, CMD,
+			    "cmd: free cmd pool\n");
 		kfree(adapter->cmd_pool);
 		adapter->cmd_pool = NULL;
 	}
@@ -457,9 +469,11 @@ int mwifiex_process_event(struct mwifiex_adapter *adapter)
 		memset(rx_info, 0, sizeof(*rx_info));
 		rx_info->bss_num = priv->bss_num;
 		rx_info->bss_type = priv->bss_type;
+		mwifiex_dbg_dump(adapter, EVT_D, "Event Buf:",
+				 skb->data, skb->len);
 	}
 
-	dev_dbg(adapter->dev, "EVENT: cause: %#x\n", eventcause);
+	mwifiex_dbg(adapter, EVENT, "EVENT: cause: %#x\n", eventcause);
 
 	if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP)
 		ret = mwifiex_process_uap_event(priv);
@@ -498,28 +512,33 @@ int mwifiex_send_cmd(struct mwifiex_private *priv, u16 cmd_no,
 	}
 
 	if (adapter->is_suspended) {
-		dev_err(adapter->dev, "PREP_CMD: device in suspended state\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "PREP_CMD: device in suspended state\n");
 		return -1;
 	}
 
 	if (adapter->hs_enabling && cmd_no != HostCmd_CMD_802_11_HS_CFG_ENH) {
-		dev_err(adapter->dev, "PREP_CMD: host entering sleep state\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "PREP_CMD: host entering sleep state\n");
 		return -1;
 	}
 
 	if (adapter->surprise_removed) {
-		dev_err(adapter->dev, "PREP_CMD: card is removed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "PREP_CMD: card is removed\n");
 		return -1;
 	}
 
 	if (adapter->is_cmd_timedout) {
-		dev_err(adapter->dev, "PREP_CMD: FW is in bad state\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "PREP_CMD: FW is in bad state\n");
 		return -1;
 	}
 
 	if (adapter->hw_status == MWIFIEX_HW_STATUS_RESET) {
 		if (cmd_no != HostCmd_CMD_FUNC_INIT) {
-			dev_err(adapter->dev, "PREP_CMD: FW in reset state\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "PREP_CMD: FW in reset state\n");
 			return -1;
 		}
 	}
@@ -528,7 +547,8 @@ int mwifiex_send_cmd(struct mwifiex_private *priv, u16 cmd_no,
 	cmd_node = mwifiex_get_cmd_node(adapter);
 
 	if (!cmd_node) {
-		dev_err(adapter->dev, "PREP_CMD: no free cmd node\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "PREP_CMD: no free cmd node\n");
 		return -1;
 	}
 
@@ -536,7 +556,8 @@ int mwifiex_send_cmd(struct mwifiex_private *priv, u16 cmd_no,
 	mwifiex_init_cmd_node(priv, cmd_node, cmd_oid, data_buf, sync);
 
 	if (!cmd_node->cmd_skb) {
-		dev_err(adapter->dev, "PREP_CMD: no free cmd buf\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "PREP_CMD: no free cmd buf\n");
 		return -1;
 	}
 
@@ -554,6 +575,8 @@ int mwifiex_send_cmd(struct mwifiex_private *priv, u16 cmd_no,
 		case HostCmd_CMD_UAP_BSS_START:
 		case HostCmd_CMD_UAP_BSS_STOP:
 		case HostCmd_CMD_UAP_STA_DEAUTH:
+		case HOST_CMD_APCMD_SYS_RESET:
+		case HOST_CMD_APCMD_STA_LIST:
 			ret = mwifiex_uap_prepare_cmd(priv, cmd_no, cmd_action,
 						      cmd_oid, data_buf,
 						      cmd_ptr);
@@ -571,7 +594,8 @@ int mwifiex_send_cmd(struct mwifiex_private *priv, u16 cmd_no,
 
 	/* Return error, since the command preparation failed */
 	if (ret) {
-		dev_err(adapter->dev, "PREP_CMD: cmd %#x preparation failed\n",
+		mwifiex_dbg(adapter, ERROR,
+			    "PREP_CMD: cmd %#x preparation failed\n",
 			cmd_no);
 		mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
 		return -1;
@@ -626,7 +650,8 @@ void mwifiex_recycle_cmd_node(struct mwifiex_adapter *adapter,
 	mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
 
 	atomic_dec(&adapter->cmd_pending);
-	dev_dbg(adapter->dev, "cmd: FREE_CMD: cmd=%#x, cmd_pending=%d\n",
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: FREE_CMD: cmd=%#x, cmd_pending=%d\n",
 		le16_to_cpu(host_cmd->command),
 		atomic_read(&adapter->cmd_pending));
 }
@@ -648,7 +673,7 @@ mwifiex_insert_cmd_to_pending_q(struct mwifiex_adapter *adapter,
 
 	host_cmd = (struct host_cmd_ds_command *) (cmd_node->cmd_skb->data);
 	if (!host_cmd) {
-		dev_err(adapter->dev, "QUEUE_CMD: host_cmd is NULL\n");
+		mwifiex_dbg(adapter, ERROR, "QUEUE_CMD: host_cmd is NULL\n");
 		return;
 	}
 
@@ -673,7 +698,8 @@ mwifiex_insert_cmd_to_pending_q(struct mwifiex_adapter *adapter,
 	spin_unlock_irqrestore(&adapter->cmd_pending_q_lock, flags);
 
 	atomic_inc(&adapter->cmd_pending);
-	dev_dbg(adapter->dev, "cmd: QUEUE_CMD: cmd=%#x, cmd_pending=%d\n",
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: QUEUE_CMD: cmd=%#x, cmd_pending=%d\n",
 		command, atomic_read(&adapter->cmd_pending));
 }
 
@@ -699,7 +725,8 @@ int mwifiex_exec_next_cmd(struct mwifiex_adapter *adapter)
 
 	/* Check if already in processing */
 	if (adapter->curr_cmd) {
-		dev_err(adapter->dev, "EXEC_NEXT_CMD: cmd in processing\n");
+		mwifiex_dbg(adapter, FATAL,
+			    "EXEC_NEXT_CMD: cmd in processing\n");
 		return -1;
 	}
 
@@ -721,8 +748,9 @@ int mwifiex_exec_next_cmd(struct mwifiex_adapter *adapter)
 	priv = cmd_node->priv;
 
 	if (adapter->ps_state != PS_STATE_AWAKE) {
-		dev_err(adapter->dev, "%s: cannot send cmd in sleep state,"
-				" this should not happen\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: cannot send cmd in sleep state,\t"
+			    "this should not happen\n", __func__);
 		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, cmd_flags);
 		return ret;
 	}
@@ -772,8 +800,9 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 
 	if (!adapter->curr_cmd || !adapter->curr_cmd->resp_skb) {
 		resp = (struct host_cmd_ds_command *) adapter->upld_buf;
-		dev_err(adapter->dev, "CMD_RESP: NULL curr_cmd, %#x\n",
-			le16_to_cpu(resp->command));
+		mwifiex_dbg(adapter, ERROR,
+			    "CMD_RESP: NULL curr_cmd, %#x\n",
+			    le16_to_cpu(resp->command));
 		return -1;
 	}
 
@@ -781,8 +810,9 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 
 	resp = (struct host_cmd_ds_command *) adapter->curr_cmd->resp_skb->data;
 	if (adapter->curr_cmd->cmd_flag & CMD_F_CANCELED) {
-		dev_err(adapter->dev, "CMD_RESP: %#x been canceled\n",
-			le16_to_cpu(resp->command));
+		mwifiex_dbg(adapter, ERROR,
+			    "CMD_RESP: %#x been canceled\n",
+			    le16_to_cpu(resp->command));
 		mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
 		adapter->curr_cmd = NULL;
@@ -794,7 +824,8 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 		/* Copy original response back to response buffer */
 		struct mwifiex_ds_misc_cmd *hostcmd;
 		uint16_t size = le16_to_cpu(resp->size);
-		dev_dbg(adapter->dev, "info: host cmd resp size = %d\n", size);
+		mwifiex_dbg(adapter, INFO,
+			    "info: host cmd resp size = %d\n", size);
 		size = min_t(u16, size, MWIFIEX_SIZE_OF_CMD_BUFFER);
 		if (adapter->curr_cmd->data_buf) {
 			hostcmd = adapter->curr_cmd->data_buf;
@@ -822,13 +853,15 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 	adapter->dbg.last_cmd_resp_id[adapter->dbg.last_cmd_resp_index] =
 								orig_cmdresp_no;
 
-	dev_dbg(adapter->dev,
-		"cmd: CMD_RESP: 0x%x, result %d, len %d, seqno 0x%x\n",
-		orig_cmdresp_no, cmdresp_result,
-		le16_to_cpu(resp->size), le16_to_cpu(resp->seq_num));
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: CMD_RESP: 0x%x, result %d, len %d, seqno 0x%x\n",
+		    orig_cmdresp_no, cmdresp_result,
+		    le16_to_cpu(resp->size), le16_to_cpu(resp->seq_num));
+	mwifiex_dbg_dump(adapter, CMD_D, "CMD_RESP buffer:", resp,
+			 le16_to_cpu(resp->size));
 
 	if (!(orig_cmdresp_no & HostCmd_RET_BIT)) {
-		dev_err(adapter->dev, "CMD_RESP: invalid cmd resp\n");
+		mwifiex_dbg(adapter, ERROR, "CMD_RESP: invalid cmd resp\n");
 		if (adapter->curr_cmd->wait_q_enabled)
 			adapter->cmd_wait_q.status = -1;
 
@@ -852,8 +885,9 @@ int mwifiex_process_cmdresp(struct mwifiex_adapter *adapter)
 	/* Check init command response */
 	if (adapter->hw_status == MWIFIEX_HW_STATUS_INITIALIZING) {
 		if (ret) {
-			dev_err(adapter->dev, "%s: cmd %#x failed during "
-				"initialization\n", __func__, cmdresp_no);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: cmd %#x failed during\t"
+				    "initialization\n", __func__, cmdresp_no);
 			mwifiex_init_fw_complete(adapter);
 			return -1;
 		} else if (adapter->last_init_cmd == cmdresp_no)
@@ -888,7 +922,8 @@ mwifiex_cmd_timeout_func(unsigned long function_context)
 
 	adapter->is_cmd_timedout = 1;
 	if (!adapter->curr_cmd) {
-		dev_dbg(adapter->dev, "cmd: empty curr_cmd\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cmd: empty curr_cmd\n");
 		return;
 	}
 	cmd_node = adapter->curr_cmd;
@@ -897,47 +932,60 @@ mwifiex_cmd_timeout_func(unsigned long function_context)
 			adapter->dbg.last_cmd_id[adapter->dbg.last_cmd_index];
 		adapter->dbg.timeout_cmd_act =
 			adapter->dbg.last_cmd_act[adapter->dbg.last_cmd_index];
-		dev_err(adapter->dev,
-			"%s: Timeout cmd id = %#x, act = %#x\n", __func__,
-			adapter->dbg.timeout_cmd_id,
-			adapter->dbg.timeout_cmd_act);
-
-		dev_err(adapter->dev, "num_data_h2c_failure = %d\n",
-			adapter->dbg.num_tx_host_to_card_failure);
-		dev_err(adapter->dev, "num_cmd_h2c_failure = %d\n",
-			adapter->dbg.num_cmd_host_to_card_failure);
-
-		dev_err(adapter->dev, "is_cmd_timedout = %d\n",
-			adapter->is_cmd_timedout);
-		dev_err(adapter->dev, "num_tx_timeout = %d\n",
-			adapter->dbg.num_tx_timeout);
-
-		dev_err(adapter->dev, "last_cmd_index = %d\n",
-			adapter->dbg.last_cmd_index);
-		dev_err(adapter->dev, "last_cmd_id: %*ph\n",
-			(int)sizeof(adapter->dbg.last_cmd_id),
-			adapter->dbg.last_cmd_id);
-		dev_err(adapter->dev, "last_cmd_act: %*ph\n",
-			(int)sizeof(adapter->dbg.last_cmd_act),
-			adapter->dbg.last_cmd_act);
-
-		dev_err(adapter->dev, "last_cmd_resp_index = %d\n",
-			adapter->dbg.last_cmd_resp_index);
-		dev_err(adapter->dev, "last_cmd_resp_id: %*ph\n",
-			(int)sizeof(adapter->dbg.last_cmd_resp_id),
-			adapter->dbg.last_cmd_resp_id);
-
-		dev_err(adapter->dev, "last_event_index = %d\n",
-			adapter->dbg.last_event_index);
-		dev_err(adapter->dev, "last_event: %*ph\n",
-			(int)sizeof(adapter->dbg.last_event),
-			adapter->dbg.last_event);
-
-		dev_err(adapter->dev, "data_sent=%d cmd_sent=%d\n",
-			adapter->data_sent, adapter->cmd_sent);
-
-		dev_err(adapter->dev, "ps_mode=%d ps_state=%d\n",
-			adapter->ps_mode, adapter->ps_state);
+		mwifiex_dbg(adapter, MSG,
+			    "%s: Timeout cmd id = %#x, act = %#x\n", __func__,
+			    adapter->dbg.timeout_cmd_id,
+			    adapter->dbg.timeout_cmd_act);
+
+		mwifiex_dbg(adapter, MSG,
+			    "num_data_h2c_failure = %d\n",
+			    adapter->dbg.num_tx_host_to_card_failure);
+		mwifiex_dbg(adapter, MSG,
+			    "num_cmd_h2c_failure = %d\n",
+			    adapter->dbg.num_cmd_host_to_card_failure);
+
+		mwifiex_dbg(adapter, MSG,
+			    "is_cmd_timedout = %d\n",
+			    adapter->is_cmd_timedout);
+		mwifiex_dbg(adapter, MSG,
+			    "num_tx_timeout = %d\n",
+			    adapter->dbg.num_tx_timeout);
+
+		mwifiex_dbg(adapter, MSG,
+			    "last_cmd_index = %d\n",
+			    adapter->dbg.last_cmd_index);
+		mwifiex_dbg(adapter, MSG,
+			    "last_cmd_id: %*ph\n",
+			    (int)sizeof(adapter->dbg.last_cmd_id),
+			    adapter->dbg.last_cmd_id);
+		mwifiex_dbg(adapter, MSG,
+			    "last_cmd_act: %*ph\n",
+			    (int)sizeof(adapter->dbg.last_cmd_act),
+			    adapter->dbg.last_cmd_act);
+
+		mwifiex_dbg(adapter, MSG,
+			    "last_cmd_resp_index = %d\n",
+			    adapter->dbg.last_cmd_resp_index);
+		mwifiex_dbg(adapter, MSG,
+			    "last_cmd_resp_id: %*ph\n",
+			    (int)sizeof(adapter->dbg.last_cmd_resp_id),
+			    adapter->dbg.last_cmd_resp_id);
+
+		mwifiex_dbg(adapter, MSG,
+			    "last_event_index = %d\n",
+			    adapter->dbg.last_event_index);
+		mwifiex_dbg(adapter, MSG,
+			    "last_event: %*ph\n",
+			    (int)sizeof(adapter->dbg.last_event),
+			    adapter->dbg.last_event);
+
+		mwifiex_dbg(adapter, MSG,
+			    "data_sent=%d cmd_sent=%d\n",
+			    adapter->data_sent, adapter->cmd_sent);
+
+		mwifiex_dbg(adapter, MSG,
+			    "ps_mode=%d ps_state=%d\n",
+			    adapter->ps_mode, adapter->ps_state);
 
 		if (cmd_node->wait_q_enabled) {
 			adapter->cmd_wait_q.status = -ETIMEDOUT;
@@ -948,8 +996,8 @@ mwifiex_cmd_timeout_func(unsigned long function_context)
 	if (adapter->hw_status == MWIFIEX_HW_STATUS_INITIALIZING)
 		mwifiex_init_fw_complete(adapter);
 
-	if (adapter->if_ops.fw_dump)
-		adapter->if_ops.fw_dump(adapter);
+	if (adapter->if_ops.device_dump)
+		adapter->if_ops.device_dump(adapter);
 
 	if (adapter->if_ops.card_reset)
 		adapter->if_ops.card_reset(adapter);
@@ -1015,7 +1063,7 @@ mwifiex_cancel_all_pending_cmd(struct mwifiex_adapter *adapter)
 			if (!priv)
 				continue;
 			if (priv->scan_request) {
-				dev_dbg(adapter->dev, "info: aborting scan\n");
+				mwifiex_dbg(adapter, WARN, "info: aborting scan\n");
 				cfg80211_scan_done(priv->scan_request, 1);
 				priv->scan_request = NULL;
 			}
@@ -1075,7 +1123,7 @@ mwifiex_cancel_pending_ioctl(struct mwifiex_adapter *adapter)
 			if (!priv)
 				continue;
 			if (priv->scan_request) {
-				dev_dbg(adapter->dev, "info: aborting scan\n");
+				mwifiex_dbg(adapter, WARN, "info: aborting scan\n");
 				cfg80211_scan_done(priv->scan_request, 1);
 				priv->scan_request = NULL;
 			}
@@ -1100,11 +1148,11 @@ mwifiex_check_ps_cond(struct mwifiex_adapter *adapter)
 	    !adapter->curr_cmd && !IS_CARD_RX_RCVD(adapter))
 		mwifiex_dnld_sleep_confirm_cmd(adapter);
 	else
-		dev_dbg(adapter->dev,
-			"cmd: Delay Sleep Confirm (%s%s%s)\n",
-			(adapter->cmd_sent) ? "D" : "",
-			(adapter->curr_cmd) ? "C" : "",
-			(IS_CARD_RX_RCVD(adapter)) ? "R" : "");
+		mwifiex_dbg(adapter, CMD,
+			    "cmd: Delay Sleep Confirm (%s%s%s)\n",
+			    (adapter->cmd_sent) ? "D" : "",
+			    (adapter->curr_cmd) ? "C" : "",
+			    (IS_CARD_RX_RCVD(adapter)) ? "R" : "");
 }
 
 /*
@@ -1120,15 +1168,18 @@ mwifiex_hs_activated_event(struct mwifiex_private *priv, u8 activated)
 			priv->adapter->hs_activated = true;
 			mwifiex_update_rxreor_flags(priv->adapter,
 						    RXREOR_FORCE_NO_DROP);
-			dev_dbg(priv->adapter->dev, "event: hs_activated\n");
+			mwifiex_dbg(priv->adapter, EVENT,
+				    "event: hs_activated\n");
 			priv->adapter->hs_activate_wait_q_woken = true;
 			wake_up_interruptible(
 				&priv->adapter->hs_activate_wait_q);
 		} else {
-			dev_dbg(priv->adapter->dev, "event: HS not configured\n");
+			mwifiex_dbg(priv->adapter, EVENT,
+				    "event: HS not configured\n");
 		}
 	} else {
-		dev_dbg(priv->adapter->dev, "event: hs_deactivated\n");
+		mwifiex_dbg(priv->adapter, EVENT,
+			    "event: hs_deactivated\n");
 		priv->adapter->hs_activated = false;
 	}
 }
@@ -1156,11 +1207,12 @@ int mwifiex_ret_802_11_hs_cfg(struct mwifiex_private *priv,
 		mwifiex_hs_activated_event(priv, true);
 		return 0;
 	} else {
-		dev_dbg(adapter->dev, "cmd: CMD_RESP: HS_CFG cmd reply"
-			" result=%#x, conditions=0x%x gpio=0x%x gap=0x%x\n",
-			resp->result, conditions,
-			phs_cfg->params.hs_config.gpio,
-			phs_cfg->params.hs_config.gap);
+		mwifiex_dbg(adapter, CMD,
+			    "cmd: CMD_RESP: HS_CFG cmd reply\t"
+			    " result=%#x, conditions=0x%x gpio=0x%x gap=0x%x\n",
+			    resp->result, conditions,
+			    phs_cfg->params.hs_config.gpio,
+			    phs_cfg->params.hs_config.gap);
 	}
 	if (conditions != HS_CFG_CANCEL) {
 		adapter->is_hs_configured = true;
@@ -1182,8 +1234,10 @@ int mwifiex_ret_802_11_hs_cfg(struct mwifiex_private *priv,
 void
 mwifiex_process_hs_config(struct mwifiex_adapter *adapter)
 {
-	dev_dbg(adapter->dev, "info: %s: auto cancelling host sleep"
-		" since there is interrupt from the firmware\n", __func__);
+	mwifiex_dbg(adapter, INFO,
+		    "info: %s: auto cancelling host sleep\t"
+		    "since there is interrupt from the firmware\n",
+		    __func__);
 
 	adapter->if_ops.wakeup(adapter);
 	adapter->hs_activated = false;
@@ -1212,13 +1266,14 @@ mwifiex_process_sleep_confirm_resp(struct mwifiex_adapter *adapter,
 	uint16_t seq_num = le16_to_cpu(cmd->seq_num);
 
 	if (!upld_len) {
-		dev_err(adapter->dev, "%s: cmd size is 0\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: cmd size is 0\n", __func__);
 		return;
 	}
 
-	dev_dbg(adapter->dev,
-		"cmd: CMD_RESP: 0x%x, result %d, len %d, seqno 0x%x\n",
-		command, result, le16_to_cpu(cmd->size), seq_num);
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: CMD_RESP: 0x%x, result %d, len %d, seqno 0x%x\n",
+		    command, result, le16_to_cpu(cmd->size), seq_num);
 
 	/* Get BSS number and corresponding priv */
 	priv = mwifiex_get_priv_by_id(adapter, HostCmd_GET_BSS_NO(seq_num),
@@ -1232,15 +1287,16 @@ mwifiex_process_sleep_confirm_resp(struct mwifiex_adapter *adapter,
 	command &= HostCmd_CMD_ID_MASK;
 
 	if (command != HostCmd_CMD_802_11_PS_MODE_ENH) {
-		dev_err(adapter->dev,
-			"%s: rcvd unexpected resp for cmd %#x, result = %x\n",
-			__func__, command, result);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: rcvd unexpected resp for cmd %#x, result = %x\n",
+			    __func__, command, result);
 		return;
 	}
 
 	if (result) {
-		dev_err(adapter->dev, "%s: sleep confirm cmd failed\n",
-			__func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: sleep confirm cmd failed\n",
+			    __func__);
 		adapter->pm_wakeup_card_req = false;
 		adapter->ps_state = PS_STATE_AWAKE;
 		return;
@@ -1305,7 +1361,8 @@ int mwifiex_cmd_enh_power_mode(struct mwifiex_private *priv,
 					sizeof(struct mwifiex_ie_types_header));
 			cmd_size += sizeof(*ps_tlv);
 			tlv += sizeof(*ps_tlv);
-			dev_dbg(adapter->dev, "cmd: PS Command: Enter PS\n");
+			mwifiex_dbg(priv->adapter, CMD,
+				    "cmd: PS Command: Enter PS\n");
 			ps_mode->null_pkt_interval =
 					cpu_to_le16(adapter->null_pkt_interval);
 			ps_mode->multiple_dtims =
@@ -1335,8 +1392,8 @@ int mwifiex_cmd_enh_power_mode(struct mwifiex_private *priv,
 			tlv += sizeof(*auto_ds_tlv);
 			if (auto_ds)
 				idletime = auto_ds->idle_time;
-			dev_dbg(priv->adapter->dev,
-				"cmd: PS Command: Enter Auto Deep Sleep\n");
+			mwifiex_dbg(priv->adapter, CMD,
+				    "cmd: PS Command: Enter Auto Deep Sleep\n");
 			auto_ds_tlv->deep_sleep_timeout = cpu_to_le16(idletime);
 		}
 		cmd->size = cpu_to_le16(cmd_size);
@@ -1363,27 +1420,31 @@ int mwifiex_ret_enh_power_mode(struct mwifiex_private *priv,
 	uint16_t auto_ps_bitmap =
 		le16_to_cpu(ps_mode->params.ps_bitmap);
 
-	dev_dbg(adapter->dev,
-		"info: %s: PS_MODE cmd reply result=%#x action=%#X\n",
-		__func__, resp->result, action);
+	mwifiex_dbg(adapter, INFO,
+		    "info: %s: PS_MODE cmd reply result=%#x action=%#X\n",
+		    __func__, resp->result, action);
 	if (action == EN_AUTO_PS) {
 		if (auto_ps_bitmap & BITMAP_AUTO_DS) {
-			dev_dbg(adapter->dev, "cmd: Enabled auto deep sleep\n");
+			mwifiex_dbg(adapter, CMD,
+				    "cmd: Enabled auto deep sleep\n");
 			priv->adapter->is_deep_sleep = true;
 		}
 		if (auto_ps_bitmap & BITMAP_STA_PS) {
-			dev_dbg(adapter->dev, "cmd: Enabled STA power save\n");
+			mwifiex_dbg(adapter, CMD,
+				    "cmd: Enabled STA power save\n");
 			if (adapter->sleep_period.period)
-				dev_dbg(adapter->dev,
-					"cmd: set to uapsd/pps mode\n");
+				mwifiex_dbg(adapter, CMD,
+					    "cmd: set to uapsd/pps mode\n");
 		}
 	} else if (action == DIS_AUTO_PS) {
 		if (ps_bitmap & BITMAP_AUTO_DS) {
 			priv->adapter->is_deep_sleep = false;
-			dev_dbg(adapter->dev, "cmd: Disabled auto deep sleep\n");
+			mwifiex_dbg(adapter, CMD,
+				    "cmd: Disabled auto deep sleep\n");
 		}
 		if (ps_bitmap & BITMAP_STA_PS) {
-			dev_dbg(adapter->dev, "cmd: Disabled STA power save\n");
+			mwifiex_dbg(adapter, CMD,
+				    "cmd: Disabled STA power save\n");
 			if (adapter->sleep_period.period) {
 				adapter->delay_null_pkt = false;
 				adapter->tx_lock_flag = false;
@@ -1396,7 +1457,8 @@ int mwifiex_ret_enh_power_mode(struct mwifiex_private *priv,
 		else
 			adapter->ps_mode = MWIFIEX_802_11_POWER_MODE_CAM;
 
-		dev_dbg(adapter->dev, "cmd: ps_bitmap=%#x\n", ps_bitmap);
+		mwifiex_dbg(adapter, CMD,
+			    "cmd: ps_bitmap=%#x\n", ps_bitmap);
 
 		if (pm_cfg) {
 			/* This section is for get power save mode */
@@ -1533,29 +1595,29 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 							api_rev->major_ver;
 					adapter->key_api_minor_ver =
 							api_rev->minor_ver;
-					dev_dbg(adapter->dev,
-						"key_api v%d.%d\n",
-						adapter->key_api_major_ver,
-						adapter->key_api_minor_ver);
+					mwifiex_dbg(adapter, INFO,
+						    "key_api v%d.%d\n",
+						    adapter->key_api_major_ver,
+						    adapter->key_api_minor_ver);
 					break;
 				case FW_API_VER_ID:
 					adapter->fw_api_ver =
 							api_rev->major_ver;
-					dev_dbg(adapter->dev,
-						"Firmware api version %d\n",
-						adapter->fw_api_ver);
+					mwifiex_dbg(adapter, INFO,
+						    "Firmware api version %d\n",
+						    adapter->fw_api_ver);
 					break;
 				default:
-					dev_warn(adapter->dev,
-						 "Unknown api_id: %d\n",
-						 api_id);
+					mwifiex_dbg(adapter, FATAL,
+						    "Unknown api_id: %d\n",
+						    api_id);
 					break;
 				}
 				break;
 			default:
-				dev_warn(adapter->dev,
-					 "Unknown GET_HW_SPEC TLV type: %#x\n",
-					 le16_to_cpu(tlv->type));
+				mwifiex_dbg(adapter, FATAL,
+					    "Unknown GET_HW_SPEC TLV type: %#x\n",
+					    le16_to_cpu(tlv->type));
 				break;
 			}
 			parsed_len += le16_to_cpu(tlv->len) +
@@ -1565,14 +1627,16 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 		}
 	}
 
-	dev_dbg(adapter->dev, "info: GET_HW_SPEC: fw_release_number- %#x\n",
-		adapter->fw_release_number);
-	dev_dbg(adapter->dev, "info: GET_HW_SPEC: permanent addr: %pM\n",
-		hw_spec->permanent_addr);
-	dev_dbg(adapter->dev,
-		"info: GET_HW_SPEC: hw_if_version=%#x version=%#x\n",
-		le16_to_cpu(hw_spec->hw_if_version),
-		le16_to_cpu(hw_spec->version));
+	mwifiex_dbg(adapter, INFO,
+		    "info: GET_HW_SPEC: fw_release_number- %#x\n",
+		    adapter->fw_release_number);
+	mwifiex_dbg(adapter, INFO,
+		    "info: GET_HW_SPEC: permanent addr: %pM\n",
+		    hw_spec->permanent_addr);
+	mwifiex_dbg(adapter, INFO,
+		    "info: GET_HW_SPEC: hw_if_version=%#x version=%#x\n",
+		    le16_to_cpu(hw_spec->hw_if_version),
+		    le16_to_cpu(hw_spec->version));
 
 	ether_addr_copy(priv->adapter->perm_addr, hw_spec->permanent_addr);
 	adapter->region_code = le16_to_cpu(hw_spec->region_code);
@@ -1585,8 +1649,8 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 	/* If it's unidentified region code, use the default (USA) */
 	if (i >= MWIFIEX_MAX_REGION_CODE) {
 		adapter->region_code = 0x10;
-		dev_dbg(adapter->dev,
-			"cmd: unknown region code, use default (USA)\n");
+		mwifiex_dbg(adapter, WARN,
+			    "cmd: unknown region code, use default (USA)\n");
 	}
 
 	adapter->hw_dot_11n_dev_cap = le32_to_cpu(hw_spec->dot_11n_dev_cap);
diff --git a/drivers/net/wireless/mwifiex/debugfs.c b/drivers/net/wireless/mwifiex/debugfs.c
index 1fb329dc6744..5a0636d43a1b 100644
--- a/drivers/net/wireless/mwifiex/debugfs.c
+++ b/drivers/net/wireless/mwifiex/debugfs.c
@@ -152,24 +152,24 @@ free_and_exit:
 }
 
 /*
- * Proc firmware dump read handler.
+ * Proc device dump read handler.
  *
- * This function is called when the 'fw_dump' file is opened for
+ * This function is called when the 'device_dump' file is opened for
  * reading.
- * This function dumps firmware memory in different files
- * (ex. DTCM, ITCM, SQRAM etc.) based on the the segments for
+ * This function dumps driver information and firmware memory segments
+ * (ex. DTCM, ITCM, SQRAM etc.) for
  * debugging.
  */
 static ssize_t
-mwifiex_fw_dump_read(struct file *file, char __user *ubuf,
-		     size_t count, loff_t *ppos)
+mwifiex_device_dump_read(struct file *file, char __user *ubuf,
+			 size_t count, loff_t *ppos)
 {
 	struct mwifiex_private *priv = file->private_data;
 
-	if (!priv->adapter->if_ops.fw_dump)
+	if (!priv->adapter->if_ops.device_dump)
 		return -EIO;
 
-	priv->adapter->if_ops.fw_dump(priv->adapter);
+	priv->adapter->if_ops.device_dump(priv->adapter);
 
 	return 0;
 }
@@ -535,6 +535,144 @@ done:
 	return ret;
 }
 
+/* Proc debug_mask file read handler.
+ * This function is called when the 'debug_mask' file is opened for reading
+ * This function can be used read driver debugging mask value.
+ */
+static ssize_t
+mwifiex_debug_mask_read(struct file *file, char __user *ubuf,
+			size_t count, loff_t *ppos)
+{
+	struct mwifiex_private *priv =
+		(struct mwifiex_private *)file->private_data;
+	unsigned long page = get_zeroed_page(GFP_KERNEL);
+	char *buf = (char *)page;
+	size_t ret = 0;
+	int pos = 0;
+
+	if (!buf)
+		return -ENOMEM;
+
+	pos += snprintf(buf, PAGE_SIZE, "debug mask=0x%08x\n",
+			priv->adapter->debug_mask);
+	ret = simple_read_from_buffer(ubuf, count, ppos, buf, pos);
+
+	free_page(page);
+	return ret;
+}
+
+/* Proc debug_mask file read handler.
+ * This function is called when the 'debug_mask' file is opened for reading
+ * This function can be used read driver debugging mask value.
+ */
+static ssize_t
+mwifiex_debug_mask_write(struct file *file, const char __user *ubuf,
+			 size_t count, loff_t *ppos)
+{
+	int ret;
+	unsigned long debug_mask;
+	struct mwifiex_private *priv = (void *)file->private_data;
+	unsigned long addr = get_zeroed_page(GFP_KERNEL);
+	char *buf = (void *)addr;
+	size_t buf_size = min(count, (size_t)(PAGE_SIZE - 1));
+
+	if (!buf)
+		return -ENOMEM;
+
+	if (copy_from_user(buf, ubuf, buf_size)) {
+		ret = -EFAULT;
+		goto done;
+	}
+
+	if (kstrtoul(buf, 0, &debug_mask)) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	priv->adapter->debug_mask = debug_mask;
+	ret = count;
+done:
+	free_page(addr);
+	return ret;
+}
+
+/* Proc memrw file write handler.
+ * This function is called when the 'memrw' file is opened for writing
+ * This function can be used to write to a memory location.
+ */
+static ssize_t
+mwifiex_memrw_write(struct file *file, const char __user *ubuf, size_t count,
+		    loff_t *ppos)
+{
+	int ret;
+	char cmd;
+	struct mwifiex_ds_mem_rw mem_rw;
+	u16 cmd_action;
+	struct mwifiex_private *priv = (void *)file->private_data;
+	unsigned long addr = get_zeroed_page(GFP_KERNEL);
+	char *buf = (void *)addr;
+	size_t buf_size = min(count, (size_t)(PAGE_SIZE - 1));
+
+	if (!buf)
+		return -ENOMEM;
+
+	if (copy_from_user(buf, ubuf, buf_size)) {
+		ret = -EFAULT;
+		goto done;
+	}
+
+	ret = sscanf(buf, "%c %x %x", &cmd, &mem_rw.addr, &mem_rw.value);
+	if (ret != 3) {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	if ((cmd == 'r') || (cmd == 'R')) {
+		cmd_action = HostCmd_ACT_GEN_GET;
+		mem_rw.value = 0;
+	} else if ((cmd == 'w') || (cmd == 'W')) {
+		cmd_action = HostCmd_ACT_GEN_SET;
+	} else {
+		ret = -EINVAL;
+		goto done;
+	}
+
+	memcpy(&priv->mem_rw, &mem_rw, sizeof(mem_rw));
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_MEM_ACCESS, cmd_action, 0,
+			     &mem_rw, true))
+		ret = -1;
+	else
+		ret = count;
+
+done:
+	free_page(addr);
+	return ret;
+}
+
+/* Proc memrw file read handler.
+ * This function is called when the 'memrw' file is opened for reading
+ * This function can be used to read from a memory location.
+ */
+static ssize_t
+mwifiex_memrw_read(struct file *file, char __user *ubuf,
+		   size_t count, loff_t *ppos)
+{
+	struct mwifiex_private *priv = (void *)file->private_data;
+	unsigned long addr = get_zeroed_page(GFP_KERNEL);
+	char *buf = (char *)addr;
+	int ret, pos = 0;
+
+	if (!buf)
+		return -ENOMEM;
+
+	pos += snprintf(buf, PAGE_SIZE, "0x%x 0x%x\n", priv->mem_rw.addr,
+			priv->mem_rw.value);
+	ret = simple_read_from_buffer(ubuf, count, ppos, buf, pos);
+
+	free_page(addr);
+	return ret;
+}
+
 static u32 saved_offset = -1, saved_bytes = -1;
 
 /*
@@ -654,7 +792,8 @@ mwifiex_hscfg_write(struct file *file, const char __user *ubuf,
 	memset(&hscfg, 0, sizeof(struct mwifiex_ds_hs_cfg));
 
 	if (arg_num > 3) {
-		dev_err(priv->adapter->dev, "Too many arguments\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Too many arguments\n");
 		ret = -EINVAL;
 		goto done;
 	}
@@ -746,11 +885,13 @@ static const struct file_operations mwifiex_dfs_##name##_fops = {       \
 MWIFIEX_DFS_FILE_READ_OPS(info);
 MWIFIEX_DFS_FILE_READ_OPS(debug);
 MWIFIEX_DFS_FILE_READ_OPS(getlog);
-MWIFIEX_DFS_FILE_READ_OPS(fw_dump);
+MWIFIEX_DFS_FILE_READ_OPS(device_dump);
 MWIFIEX_DFS_FILE_OPS(regrdwr);
 MWIFIEX_DFS_FILE_OPS(rdeeprom);
+MWIFIEX_DFS_FILE_OPS(memrw);
 MWIFIEX_DFS_FILE_OPS(hscfg);
 MWIFIEX_DFS_FILE_OPS(histogram);
+MWIFIEX_DFS_FILE_OPS(debug_mask);
 
 /*
  * This function creates the debug FS directory structure and the files.
@@ -772,9 +913,11 @@ mwifiex_dev_debugfs_init(struct mwifiex_private *priv)
 	MWIFIEX_DFS_ADD_FILE(getlog);
 	MWIFIEX_DFS_ADD_FILE(regrdwr);
 	MWIFIEX_DFS_ADD_FILE(rdeeprom);
-	MWIFIEX_DFS_ADD_FILE(fw_dump);
+	MWIFIEX_DFS_ADD_FILE(device_dump);
+	MWIFIEX_DFS_ADD_FILE(memrw);
 	MWIFIEX_DFS_ADD_FILE(hscfg);
 	MWIFIEX_DFS_ADD_FILE(histogram);
+	MWIFIEX_DFS_ADD_FILE(debug_mask);
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index 38f24e0427d2..51e344789ba2 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -29,7 +29,7 @@
 #include <uapi/linux/if_arp.h>
 #include <net/mac80211.h>
 
-
+#define MWIFIEX_BSS_COEX_COUNT	     2
 #define MWIFIEX_MAX_BSS_NUM         (3)
 
 #define MWIFIEX_DMA_ALIGN_SZ	    64
@@ -49,7 +49,12 @@
 
 #define MWIFIEX_STA_AMPDU_DEF_TXWINSIZE        64
 #define MWIFIEX_STA_AMPDU_DEF_RXWINSIZE        64
+#define MWIFIEX_STA_COEX_AMPDU_DEF_RXWINSIZE   16
+
 #define MWIFIEX_UAP_AMPDU_DEF_TXWINSIZE        32
+
+#define MWIFIEX_UAP_COEX_AMPDU_DEF_RXWINSIZE   16
+
 #define MWIFIEX_UAP_AMPDU_DEF_RXWINSIZE        16
 #define MWIFIEX_11AC_STA_AMPDU_DEF_TXWINSIZE   64
 #define MWIFIEX_11AC_STA_AMPDU_DEF_RXWINSIZE   64
diff --git a/drivers/net/wireless/mwifiex/ethtool.c b/drivers/net/wireless/mwifiex/ethtool.c
index 65d8d6d4b6ba..58400c69ab26 100644
--- a/drivers/net/wireless/mwifiex/ethtool.c
+++ b/drivers/net/wireless/mwifiex/ethtool.c
@@ -64,104 +64,7 @@ static int mwifiex_ethtool_set_wol(struct net_device *dev,
 	return 0;
 }
 
-static int
-mwifiex_get_dump_flag(struct net_device *dev, struct ethtool_dump *dump)
-{
-	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
-	struct mwifiex_adapter *adapter = priv->adapter;
-	struct memory_type_mapping *entry;
-
-	if (!adapter->if_ops.fw_dump)
-		return -ENOTSUPP;
-
-	dump->flag = adapter->curr_mem_idx;
-	dump->version = 1;
-	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 {
-		dump->len = 0;
-	}
-
-	return 0;
-}
-
-static int
-mwifiex_get_dump_data(struct net_device *dev, struct ethtool_dump *dump,
-		      void *buffer)
-{
-	u8 *p = buffer;
-	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
-	struct mwifiex_adapter *adapter = priv->adapter;
-	struct memory_type_mapping *entry;
-
-	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;
-	}
-
-	entry = &adapter->mem_type_mapping_tbl[adapter->curr_mem_idx];
-
-	if (!entry->mem_ptr)
-		return -EFAULT;
-
-	memcpy(p, entry->mem_ptr, entry->mem_size);
-
-	entry->mem_size = 0;
-	vfree(entry->mem_ptr);
-	entry->mem_ptr = NULL;
-
-	return 0;
-}
-
-static int mwifiex_set_dump(struct net_device *dev, struct ethtool_dump *val)
-{
-	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
-	struct mwifiex_adapter *adapter = priv->adapter;
-
-	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;
-	}
-
-	if (val->flag == MWIFIEX_FW_DUMP_IDX) {
-		adapter->curr_mem_idx = val->flag;
-		adapter->if_ops.fw_dump(adapter);
-		return 0;
-	}
-
-	if (val->flag < 0 || val->flag >= adapter->num_mem_types)
-		return -EINVAL;
-
-	adapter->curr_mem_idx = val->flag;
-
-	return 0;
-}
-
 const struct ethtool_ops mwifiex_ethtool_ops = {
 	.get_wol = mwifiex_ethtool_get_wol,
 	.set_wol = mwifiex_ethtool_set_wol,
-	.get_dump_flag = mwifiex_get_dump_flag,
-	.get_dump_data = mwifiex_get_dump_data,
-	.set_dump = mwifiex_set_dump,
 };
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index 59d8964dd0dc..cd09051710e6 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -128,6 +128,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 #define TLV_TYPE_UAP_SSID			0x0000
 #define TLV_TYPE_UAP_RATES			0x0001
+#define TLV_TYPE_PWR_CONSTRAINT			0x0020
 
 #define PROPRIETARY_TLV_BASE_ID                 0x0100
 #define TLV_TYPE_KEY_MATERIAL       (PROPRIETARY_TLV_BASE_ID + 0)
@@ -174,6 +175,8 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define TLV_TYPE_SCAN_CHANNEL_GAP   (PROPRIETARY_TLV_BASE_ID + 197)
 #define TLV_TYPE_API_REV            (PROPRIETARY_TLV_BASE_ID + 199)
 #define TLV_TYPE_CHANNEL_STATS      (PROPRIETARY_TLV_BASE_ID + 198)
+#define TLV_BTCOEX_WL_AGGR_WINSIZE  (PROPRIETARY_TLV_BASE_ID + 202)
+#define TLV_BTCOEX_WL_SCANTIME      (PROPRIETARY_TLV_BASE_ID + 203)
 
 #define MWIFIEX_TX_DATA_BUF_SIZE_2K        2048
 
@@ -323,15 +326,18 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define HostCmd_CMD_802_11_SUBSCRIBE_EVENT            0x0075
 #define HostCmd_CMD_802_11_TX_RATE_QUERY              0x007f
 #define HostCmd_CMD_802_11_IBSS_COALESCING_STATUS     0x0083
+#define HostCmd_CMD_MEM_ACCESS                        0x0086
 #define HostCmd_CMD_CFG_DATA                          0x008f
 #define HostCmd_CMD_VERSION_EXT                       0x0097
 #define HostCmd_CMD_MEF_CFG                           0x009a
 #define HostCmd_CMD_RSSI_INFO                         0x00a4
 #define HostCmd_CMD_FUNC_INIT                         0x00a9
 #define HostCmd_CMD_FUNC_SHUTDOWN                     0x00aa
+#define HOST_CMD_APCMD_SYS_RESET                      0x00af
 #define HostCmd_CMD_UAP_SYS_CONFIG                    0x00b0
 #define HostCmd_CMD_UAP_BSS_START                     0x00b1
 #define HostCmd_CMD_UAP_BSS_STOP                      0x00b2
+#define HOST_CMD_APCMD_STA_LIST                       0x00b3
 #define HostCmd_CMD_UAP_STA_DEAUTH                    0x00b5
 #define HostCmd_CMD_11N_CFG                           0x00cd
 #define HostCmd_CMD_11N_ADDBA_REQ                     0x00ce
@@ -418,8 +424,12 @@ enum P2P_MODES {
 #define HS_CFG_COND_MAC_EVENT		0x00000004
 #define HS_CFG_COND_MULTICAST_DATA	0x00000008
 
-#define MWIFIEX_TIMEOUT_FOR_AP_RESP		0xfffc
-#define MWIFIEX_STATUS_CODE_AUTH_TIMEOUT	2
+#define CONNECT_ERR_AUTH_ERR_STA_FAILURE	0xFFFB
+#define CONNECT_ERR_ASSOC_ERR_TIMEOUT		0xFFFC
+#define CONNECT_ERR_ASSOC_ERR_AUTH_REFUSED	0xFFFD
+#define CONNECT_ERR_AUTH_MSG_UNHANDLED		0xFFFE
+#define CONNECT_ERR_STA_FAILURE			0xFFFF
+
 
 #define CMD_F_HOSTCMD           (1 << 0)
 #define CMD_F_CANCELED          (1 << 1)
@@ -502,6 +512,7 @@ enum P2P_MODES {
 #define EVENT_EXT_SCAN_REPORT           0x00000058
 #define EVENT_REMAIN_ON_CHAN_EXPIRED    0x0000005f
 #define EVENT_TX_STATUS_REPORT		0x00000074
+#define EVENT_BT_COEX_WLAN_PARA_CHANGE	0X00000076
 
 #define EVENT_ID_MASK                   0xffff
 #define BSS_NUM_MASK                    0xf
@@ -626,7 +637,12 @@ struct uap_rxpd {
 	__le16 rx_pkt_type;
 	__le16 seq_num;
 	u8 priority;
-	u8 reserved1;
+	u8 rx_rate;
+	s8 snr;
+	s8 nf;
+	u8 ht_info;
+	u8 reserved[3];
+	u8 flags;
 };
 
 struct mwifiex_fw_chan_stats {
@@ -1150,6 +1166,13 @@ enum SNMP_MIB_INDEX {
 	DOT11H_I = 10,
 };
 
+enum mwifiex_assocmd_failurepoint {
+	MWIFIEX_ASSOC_CMD_SUCCESS = 0,
+	MWIFIEX_ASSOC_CMD_FAILURE_ASSOC,
+	MWIFIEX_ASSOC_CMD_FAILURE_AUTH,
+	MWIFIEX_ASSOC_CMD_FAILURE_JOIN
+};
+
 #define MAX_SNMP_BUF_SIZE   128
 
 struct host_cmd_ds_802_11_snmp_mib {
@@ -1447,6 +1470,18 @@ struct host_cmd_ds_sta_deauth {
 	__le16 reason;
 } __packed;
 
+struct mwifiex_ie_types_sta_info {
+	struct mwifiex_ie_types_header header;
+	u8 mac[ETH_ALEN];
+	u8 power_mfg_status;
+	s8 rssi;
+};
+
+struct host_cmd_ds_sta_list {
+	u16 sta_count;
+	u8 tlv[0];
+} __packed;
+
 struct mwifiex_ie_types_pwr_capability {
 	struct mwifiex_ie_types_header header;
 	s8 min_pwr;
@@ -1576,6 +1611,13 @@ struct mwifiex_ie_types_extcap {
 	u8 ext_capab[0];
 } __packed;
 
+struct host_cmd_ds_mem_access {
+	__le16 action;
+	__le16 reserved;
+	__le32 addr;
+	__le32 value;
+};
+
 struct mwifiex_ie_types_qos_info {
 	struct mwifiex_ie_types_header header;
 	u8 qos_info;
@@ -1742,6 +1784,27 @@ struct host_cmd_tlv_ageout_timer {
 	__le32 sta_ao_timer;
 } __packed;
 
+struct host_cmd_tlv_power_constraint {
+	struct mwifiex_ie_types_header header;
+	u8 constraint;
+} __packed;
+
+struct mwifiex_ie_types_btcoex_scan_time {
+	struct mwifiex_ie_types_header header;
+	u8 coex_scan;
+	u8 reserved;
+	u16 min_scan_time;
+	u16 max_scan_time;
+} __packed;
+
+struct mwifiex_ie_types_btcoex_aggr_win_size {
+	struct mwifiex_ie_types_header header;
+	u8 coex_win_size;
+	u8 tx_win_size;
+	u8 rx_win_size;
+	u8 reserved;
+} __packed;
+
 struct host_cmd_ds_version_ext {
 	u8 version_str_sel;
 	char version_str[128];
@@ -1958,6 +2021,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_p2p_mode_cfg mode_cfg;
 		struct host_cmd_ds_802_11_ibss_status ibss_coalescing;
 		struct host_cmd_ds_mef_cfg mef_cfg;
+		struct host_cmd_ds_mem_access mem;
 		struct host_cmd_ds_mac_reg_access mac_reg;
 		struct host_cmd_ds_bbp_reg_access bbp_reg;
 		struct host_cmd_ds_rf_reg_access rf_reg;
@@ -1968,6 +2032,7 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_802_11_subsc_evt subsc_evt;
 		struct host_cmd_ds_sys_config uap_sys_config;
 		struct host_cmd_ds_sta_deauth sta_deauth;
+		struct host_cmd_ds_sta_list sta_list;
 		struct host_cmd_11ac_vht_cfg vht_cfg;
 		struct host_cmd_ds_coalesce_cfg coalesce_cfg;
 		struct host_cmd_ds_tdls_oper tdls_oper;
diff --git a/drivers/net/wireless/mwifiex/ie.c b/drivers/net/wireless/mwifiex/ie.c
index f3b6ed249403..0ba894509413 100644
--- a/drivers/net/wireless/mwifiex/ie.c
+++ b/drivers/net/wireless/mwifiex/ie.c
@@ -320,63 +320,81 @@ done:
 /* This function parses  head and tail IEs, from cfg80211_beacon_data and sets
  * these IE to FW.
  */
-static int mwifiex_uap_set_head_tail_ies(struct mwifiex_private *priv,
-					 struct cfg80211_beacon_data *info)
+static int mwifiex_uap_parse_tail_ies(struct mwifiex_private *priv,
+				      struct cfg80211_beacon_data *info)
 {
 	struct mwifiex_ie *gen_ie;
-	struct ieee_types_header *rsn_ie = NULL, *wpa_ie = NULL;
-	struct ieee_types_header *chsw_ie = NULL;
+	struct ieee_types_header *hdr;
+	struct ieee80211_vendor_ie *vendorhdr;
 	u16 gen_idx = MWIFIEX_AUTO_IDX_MASK, ie_len = 0;
-	const u8 *vendor_ie;
+	int left_len, parsed_len = 0;
+
+	if (!info->tail || !info->tail_len)
+		return 0;
 
 	gen_ie = kzalloc(sizeof(*gen_ie), GFP_KERNEL);
 	if (!gen_ie)
 		return -ENOMEM;
-	gen_ie->ie_index = cpu_to_le16(gen_idx);
-	gen_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_BEACON |
-						MGMT_MASK_PROBE_RESP |
-						MGMT_MASK_ASSOC_RESP);
 
-	if (info->tail && info->tail_len) {
-		rsn_ie = (void *)cfg80211_find_ie(WLAN_EID_RSN,
-						  info->tail, info->tail_len);
-		if (rsn_ie) {
-			memcpy(gen_ie->ie_buffer, rsn_ie, rsn_ie->len + 2);
-			ie_len = rsn_ie->len + 2;
-			gen_ie->ie_length = cpu_to_le16(ie_len);
+	left_len = info->tail_len;
+
+	/* Many IEs are generated in FW by parsing bss configuration.
+	 * Let's not add them here; else we may end up duplicating these IEs
+	 */
+	while (left_len > sizeof(struct ieee_types_header)) {
+		hdr = (void *)(info->tail + parsed_len);
+		switch (hdr->element_id) {
+		case WLAN_EID_SSID:
+		case WLAN_EID_SUPP_RATES:
+		case WLAN_EID_COUNTRY:
+		case WLAN_EID_PWR_CONSTRAINT:
+		case WLAN_EID_EXT_SUPP_RATES:
+		case WLAN_EID_HT_CAPABILITY:
+		case WLAN_EID_HT_OPERATION:
+		case WLAN_EID_VHT_CAPABILITY:
+		case WLAN_EID_VHT_OPERATION:
+		case WLAN_EID_VENDOR_SPECIFIC:
+			break;
+		default:
+			memcpy(gen_ie->ie_buffer + ie_len, hdr,
+			       hdr->len + sizeof(struct ieee_types_header));
+			ie_len += hdr->len + sizeof(struct ieee_types_header);
+			break;
 		}
+		left_len -= hdr->len + sizeof(struct ieee_types_header);
+		parsed_len += hdr->len + sizeof(struct ieee_types_header);
+	}
 
-		vendor_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+	/* parse only WPA vendor IE from tail, WMM IE is configured by
+	 * bss_config command
+	 */
+	vendorhdr = (void *)cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
 						    WLAN_OUI_TYPE_MICROSOFT_WPA,
-						    info->tail,
-						    info->tail_len);
-		if (vendor_ie) {
-			wpa_ie = (struct ieee_types_header *)vendor_ie;
-			memcpy(gen_ie->ie_buffer + ie_len,
-			       wpa_ie, wpa_ie->len + 2);
-			ie_len += wpa_ie->len + 2;
-			gen_ie->ie_length = cpu_to_le16(ie_len);
-		}
+						    info->tail, info->tail_len);
+	if (vendorhdr) {
+		memcpy(gen_ie->ie_buffer + ie_len, vendorhdr,
+		       vendorhdr->len + sizeof(struct ieee_types_header));
+		ie_len += vendorhdr->len + sizeof(struct ieee_types_header);
+	}
 
-		chsw_ie = (void *)cfg80211_find_ie(WLAN_EID_CHANNEL_SWITCH,
-						   info->tail, info->tail_len);
-		if (chsw_ie) {
-			memcpy(gen_ie->ie_buffer + ie_len,
-			       chsw_ie, chsw_ie->len + 2);
-			ie_len += chsw_ie->len + 2;
-			gen_ie->ie_length = cpu_to_le16(ie_len);
-		}
+	if (!ie_len) {
+		kfree(gen_ie);
+		return 0;
 	}
 
-	if (rsn_ie || wpa_ie || chsw_ie) {
-		if (mwifiex_update_uap_custom_ie(priv, gen_ie, &gen_idx, NULL,
-						 NULL, NULL, NULL)) {
-			kfree(gen_ie);
-			return -1;
-		}
-		priv->gen_idx = gen_idx;
+	gen_ie->ie_index = cpu_to_le16(gen_idx);
+	gen_ie->mgmt_subtype_mask = cpu_to_le16(MGMT_MASK_BEACON |
+						MGMT_MASK_PROBE_RESP |
+						MGMT_MASK_ASSOC_RESP);
+	gen_ie->ie_length = cpu_to_le16(ie_len);
+
+	if (mwifiex_update_uap_custom_ie(priv, gen_ie, &gen_idx, NULL, NULL,
+					 NULL, NULL)) {
+		kfree(gen_ie);
+		return -1;
 	}
 
+	priv->gen_idx = gen_idx;
 	kfree(gen_ie);
 	return 0;
 }
@@ -390,7 +408,7 @@ int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
 {
 	int ret;
 
-	ret = mwifiex_uap_set_head_tail_ies(priv, info);
+	ret = mwifiex_uap_parse_tail_ies(priv, info);
 		return ret;
 
 	return mwifiex_set_mgmt_beacon_data_ies(priv, info);
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index e12192f5cfad..df7fdc09d38c 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -56,7 +56,7 @@ static void wakeup_timer_fn(unsigned long data)
 {
 	struct mwifiex_adapter *adapter = (struct mwifiex_adapter *)data;
 
-	dev_err(adapter->dev, "Firmware wakeup failed\n");
+	mwifiex_dbg(adapter, ERROR, "Firmware wakeup failed\n");
 	adapter->hw_status = MWIFIEX_HW_STATUS_RESET;
 	mwifiex_cancel_all_pending_cmd(adapter);
 
@@ -172,8 +172,9 @@ static int mwifiex_allocate_adapter(struct mwifiex_adapter *adapter)
 	/* Allocate command buffer */
 	ret = mwifiex_alloc_cmd_buffer(adapter);
 	if (ret) {
-		dev_err(adapter->dev, "%s: failed to alloc cmd buffer\n",
-			__func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: failed to alloc cmd buffer\n",
+			    __func__);
 		return -1;
 	}
 
@@ -182,8 +183,9 @@ static int mwifiex_allocate_adapter(struct mwifiex_adapter *adapter)
 			      + INTF_HEADER_LEN);
 
 	if (!adapter->sleep_cfm) {
-		dev_err(adapter->dev, "%s: failed to alloc sleep cfm"
-			" cmd buffer\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: failed to alloc sleep cfm\t"
+			    " cmd buffer\n", __func__);
 		return -1;
 	}
 	skb_reserve(adapter->sleep_cfm, INTF_HEADER_LEN);
@@ -417,7 +419,7 @@ mwifiex_adapter_cleanup(struct mwifiex_adapter *adapter)
 	mwifiex_free_lock_list(adapter);
 
 	/* Free command buffer */
-	dev_dbg(adapter->dev, "info: free cmd buffer\n");
+	mwifiex_dbg(adapter, INFO, "info: free cmd buffer\n");
 	mwifiex_free_cmd_buffer(adapter);
 
 	for (idx = 0; idx < adapter->num_mem_types; idx++) {
@@ -433,6 +435,7 @@ mwifiex_adapter_cleanup(struct mwifiex_adapter *adapter)
 
 	if (adapter->drv_info_dump) {
 		vfree(adapter->drv_info_dump);
+		adapter->drv_info_dump = NULL;
 		adapter->drv_info_size = 0;
 	}
 
@@ -595,10 +598,11 @@ static void mwifiex_delete_bss_prio_tbl(struct mwifiex_private *priv)
 	for (i = 0; i < adapter->priv_num; ++i) {
 		head = &adapter->bss_prio_tbl[i].bss_prio_head;
 		lock = &adapter->bss_prio_tbl[i].bss_prio_lock;
-		dev_dbg(adapter->dev, "info: delete BSS priority table,"
-				" bss_type = %d, bss_num = %d, i = %d,"
-				" head = %p\n",
-			      priv->bss_type, priv->bss_num, i, head);
+		mwifiex_dbg(adapter, INFO,
+			    "info: delete BSS priority table,\t"
+			    "bss_type = %d, bss_num = %d, i = %d,\t"
+			    "head = %p\n",
+			    priv->bss_type, priv->bss_num, i, head);
 
 		{
 			spin_lock_irqsave(lock, flags);
@@ -609,9 +613,10 @@ static void mwifiex_delete_bss_prio_tbl(struct mwifiex_private *priv)
 			list_for_each_entry_safe(bssprio_node, tmp_node, head,
 						 list) {
 				if (bssprio_node->priv == priv) {
-					dev_dbg(adapter->dev, "info: Delete "
-						"node %p, next = %p\n",
-						bssprio_node, tmp_node);
+					mwifiex_dbg(adapter, INFO,
+						    "info: Delete\t"
+						    "node %p, next = %p\n",
+						    bssprio_node, tmp_node);
 					list_del(&bssprio_node->list);
 					kfree(bssprio_node);
 				}
@@ -659,20 +664,23 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 	adapter->hw_status = MWIFIEX_HW_STATUS_CLOSING;
 	/* wait for mwifiex_process to complete */
 	if (adapter->mwifiex_processing) {
-		dev_warn(adapter->dev, "main process is still running\n");
+		mwifiex_dbg(adapter, WARN,
+			    "main process is still running\n");
 		return ret;
 	}
 
 	/* cancel current command */
 	if (adapter->curr_cmd) {
-		dev_warn(adapter->dev, "curr_cmd is still in processing\n");
+		mwifiex_dbg(adapter, WARN,
+			    "curr_cmd is still in processing\n");
 		del_timer_sync(&adapter->cmd_timer);
 		mwifiex_recycle_cmd_node(adapter, adapter->curr_cmd);
 		adapter->curr_cmd = NULL;
 	}
 
 	/* shut down mwifiex */
-	dev_dbg(adapter->dev, "info: shutdown mwifiex...\n");
+	mwifiex_dbg(adapter, MSG,
+		    "info: shutdown mwifiex...\n");
 
 	/* Clean up Tx/Rx queues and delete BSS priority table */
 	for (i = 0; i < adapter->priv_num; i++) {
@@ -741,8 +749,8 @@ int mwifiex_dnld_fw(struct mwifiex_adapter *adapter,
 		/* check if firmware is already running */
 		ret = adapter->if_ops.check_fw_status(adapter, poll_num);
 		if (!ret) {
-			dev_notice(adapter->dev,
-				   "WLAN FW already running! Skip FW dnld\n");
+			mwifiex_dbg(adapter, MSG,
+				    "WLAN FW already running! Skip FW dnld\n");
 			return 0;
 		}
 
@@ -750,8 +758,8 @@ int mwifiex_dnld_fw(struct mwifiex_adapter *adapter,
 
 		/* check if we are the winner for downloading FW */
 		if (!adapter->winner) {
-			dev_notice(adapter->dev,
-				   "FW already running! Skip FW dnld\n");
+			mwifiex_dbg(adapter, MSG,
+				    "FW already running! Skip FW dnld\n");
 			goto poll_fw;
 		}
 	}
@@ -760,7 +768,8 @@ int mwifiex_dnld_fw(struct mwifiex_adapter *adapter,
 		/* Download firmware with helper */
 		ret = adapter->if_ops.prog_fw(adapter, pmfw);
 		if (ret) {
-			dev_err(adapter->dev, "prog_fw failed ret=%#x\n", ret);
+			mwifiex_dbg(adapter, ERROR,
+				    "prog_fw failed ret=%#x\n", ret);
 			return ret;
 		}
 	}
@@ -769,7 +778,8 @@ poll_fw:
 	/* Check if the firmware is downloaded successfully or not */
 	ret = adapter->if_ops.check_fw_status(adapter, poll_num);
 	if (ret)
-		dev_err(adapter->dev, "FW failed to be active in time\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "FW failed to be active in time\n");
 
 	return ret;
 }
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/mwifiex/ioctl.h
index d2b05c3a96da..4f0174c64946 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/mwifiex/ioctl.h
@@ -113,6 +113,7 @@ struct mwifiex_uap_bss_param {
 	u32 sta_ao_timer;
 	u32 ps_sta_ao_timer;
 	u8 qos_info;
+	u8 power_constraint;
 	struct mwifiex_types_wmm_info wmm_info;
 };
 
@@ -189,6 +190,7 @@ struct tdls_peer_info {
 };
 
 struct mwifiex_debug_info {
+	unsigned int debug_mask;
 	u32 int_counter;
 	u32 packets_out[MAX_NUM_TID];
 	u32 tx_buf_size;
@@ -342,6 +344,11 @@ struct mwifiex_ds_read_eeprom {
 	u8 value[MAX_EEPROM_DATA];
 };
 
+struct mwifiex_ds_mem_rw {
+	u32 addr;
+	u32 value;
+};
+
 #define IEEE_MAX_IE_SIZE		256
 
 #define MWIFIEX_IE_HDR_SIZE	(sizeof(struct mwifiex_ie) - IEEE_MAX_IE_SIZE)
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/mwifiex/join.c
index 411a6c2f4aca..56b024a6aaa5 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/mwifiex/join.c
@@ -53,9 +53,9 @@ mwifiex_cmd_append_generic_ie(struct mwifiex_private *priv, u8 **buffer)
 	 *   parameter buffer pointer.
 	 */
 	if (priv->gen_ie_buf_len) {
-		dev_dbg(priv->adapter->dev,
-			"info: %s: append generic ie len %d to %p\n",
-			__func__, priv->gen_ie_buf_len, *buffer);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: %s: append generic ie len %d to %p\n",
+			    __func__, priv->gen_ie_buf_len, *buffer);
 
 		/* Wrap the generic IE buffer with a pass through TLV type */
 		ie_header.type = cpu_to_le16(TLV_TYPE_PASSTHROUGH);
@@ -125,9 +125,9 @@ mwifiex_cmd_append_tsf_tlv(struct mwifiex_private *priv, u8 **buffer,
 
 	tsf_val = cpu_to_le64(bss_desc->timestamp);
 
-	dev_dbg(priv->adapter->dev,
-		"info: %s: TSF offset calc: %016llx - %016llx\n",
-		__func__, bss_desc->timestamp, bss_desc->fw_tsf);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: %s: TSF offset calc: %016llx - %016llx\n",
+		    __func__, bss_desc->timestamp, bss_desc->fw_tsf);
 
 	memcpy(*buffer, &tsf_val, sizeof(tsf_val));
 	*buffer += sizeof(tsf_val);
@@ -152,7 +152,7 @@ static int mwifiex_get_common_rates(struct mwifiex_private *priv, u8 *rate1,
 
 	tmp = kmemdup(rate1, rate1_size, GFP_KERNEL);
 	if (!tmp) {
-		dev_err(priv->adapter->dev, "failed to alloc tmp buf\n");
+		mwifiex_dbg(priv->adapter, ERROR, "failed to alloc tmp buf\n");
 		return -ENOMEM;
 	}
 
@@ -169,8 +169,8 @@ static int mwifiex_get_common_rates(struct mwifiex_private *priv, u8 *rate1,
 		}
 	}
 
-	dev_dbg(priv->adapter->dev, "info: Tx data rate set to %#x\n",
-		priv->data_rate);
+	mwifiex_dbg(priv->adapter, INFO, "info: Tx data rate set to %#x\n",
+		    priv->data_rate);
 
 	if (!priv->is_data_rate_auto) {
 		while (*ptr) {
@@ -180,9 +180,10 @@ static int mwifiex_get_common_rates(struct mwifiex_private *priv, u8 *rate1,
 			}
 			ptr++;
 		}
-		dev_err(priv->adapter->dev, "previously set fixed data rate %#x"
-			" is not compatible with the network\n",
-			priv->data_rate);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "previously set fixed data rate %#x\t"
+			    "is not compatible with the network\n",
+			    priv->data_rate);
 
 		ret = -1;
 		goto done;
@@ -214,8 +215,9 @@ mwifiex_setup_rates_from_bssdesc(struct mwifiex_private *priv,
 	if (mwifiex_get_common_rates(priv, out_rates, MWIFIEX_SUPPORTED_RATES,
 				     card_rates, card_rates_size)) {
 		*out_rates_size = 0;
-		dev_err(priv->adapter->dev, "%s: cannot get common rates\n",
-			__func__);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: cannot get common rates\n",
+			    __func__);
 		return -1;
 	}
 
@@ -246,8 +248,9 @@ mwifiex_cmd_append_wps_ie(struct mwifiex_private *priv, u8 **buffer)
 	 * parameter buffer pointer.
 	 */
 	if (priv->wps_ie_len) {
-		dev_dbg(priv->adapter->dev, "cmd: append wps ie %d to %p\n",
-			priv->wps_ie_len, *buffer);
+		mwifiex_dbg(priv->adapter, CMD,
+			    "cmd: append wps ie %d to %p\n",
+			    priv->wps_ie_len, *buffer);
 
 		/* Wrap the generic IE buffer with a pass through TLV type */
 		ie_header.type = cpu_to_le16(TLV_TYPE_MGMT_IE);
@@ -292,8 +295,9 @@ mwifiex_cmd_append_wapi_ie(struct mwifiex_private *priv, u8 **buffer)
 	 *   parameter buffer pointer.
 	 */
 	if (priv->wapi_ie_len) {
-		dev_dbg(priv->adapter->dev, "cmd: append wapi ie %d to %p\n",
-			priv->wapi_ie_len, *buffer);
+		mwifiex_dbg(priv->adapter, CMD,
+			    "cmd: append wapi ie %d to %p\n",
+			    priv->wapi_ie_len, *buffer);
 
 		/* Wrap the generic IE buffer with a pass through TLV type */
 		ie_header.type = cpu_to_le16(TLV_TYPE_WAPI_IE);
@@ -453,8 +457,8 @@ int mwifiex_cmd_802_11_associate(struct mwifiex_private *priv,
 	rates_tlv->header.len = cpu_to_le16((u16) rates_size);
 	memcpy(rates_tlv->rates, rates, rates_size);
 	pos += sizeof(rates_tlv->header) + rates_size;
-	dev_dbg(priv->adapter->dev, "info: ASSOC_CMD: rates size = %d\n",
-		rates_size);
+	mwifiex_dbg(priv->adapter, INFO, "info: ASSOC_CMD: rates size = %d\n",
+		    rates_size);
 
 	/* Add the Authentication type to be used for Auth frames */
 	auth_tlv = (struct mwifiex_ie_types_auth_type *) pos;
@@ -487,14 +491,14 @@ int mwifiex_cmd_802_11_associate(struct mwifiex_private *priv,
 		       sizeof(struct mwifiex_chan_scan_param_set));
 		chan_tlv->chan_scan_param[0].chan_number =
 			(bss_desc->phy_param_set.ds_param_set.current_chan);
-		dev_dbg(priv->adapter->dev, "info: Assoc: TLV Chan = %d\n",
-			chan_tlv->chan_scan_param[0].chan_number);
+		mwifiex_dbg(priv->adapter, INFO, "info: Assoc: TLV Chan = %d\n",
+			    chan_tlv->chan_scan_param[0].chan_number);
 
 		chan_tlv->chan_scan_param[0].radio_type =
 			mwifiex_band_to_radio_type((u8) bss_desc->bss_band);
 
-		dev_dbg(priv->adapter->dev, "info: Assoc: TLV Band = %d\n",
-			chan_tlv->chan_scan_param[0].radio_type);
+		mwifiex_dbg(priv->adapter, INFO, "info: Assoc: TLV Band = %d\n",
+			    chan_tlv->chan_scan_param[0].radio_type);
 		pos += sizeof(chan_tlv->header) +
 			sizeof(struct mwifiex_chan_scan_param_set);
 	}
@@ -544,13 +548,31 @@ int mwifiex_cmd_802_11_associate(struct mwifiex_private *priv,
 		tmp_cap &= ~WLAN_CAPABILITY_SHORT_SLOT_TIME;
 
 	tmp_cap &= CAPINFO_MASK;
-	dev_dbg(priv->adapter->dev, "info: ASSOC_CMD: tmp_cap=%4X CAPINFO_MASK=%4lX\n",
-		tmp_cap, CAPINFO_MASK);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: ASSOC_CMD: tmp_cap=%4X CAPINFO_MASK=%4lX\n",
+		    tmp_cap, CAPINFO_MASK);
 	assoc->cap_info_bitmap = cpu_to_le16(tmp_cap);
 
 	return 0;
 }
 
+static const char *assoc_failure_reason_to_str(u16 cap_info)
+{
+	switch (cap_info) {
+	case CONNECT_ERR_AUTH_ERR_STA_FAILURE:
+		return "CONNECT_ERR_AUTH_ERR_STA_FAILURE";
+	case CONNECT_ERR_AUTH_MSG_UNHANDLED:
+		return "CONNECT_ERR_AUTH_MSG_UNHANDLED";
+	case CONNECT_ERR_ASSOC_ERR_TIMEOUT:
+		return "CONNECT_ERR_ASSOC_ERR_TIMEOUT";
+	case CONNECT_ERR_ASSOC_ERR_AUTH_REFUSED:
+		return "CONNECT_ERR_ASSOC_ERR_AUTH_REFUSED";
+	case CONNECT_ERR_STA_FAILURE:
+		return "CONNECT_ERR_STA_FAILURE";
+	}
+
+	return "Unknown connect failure";
+}
 /*
  * Association firmware command response handler
  *
@@ -621,29 +643,48 @@ int mwifiex_ret_802_11_associate(struct mwifiex_private *priv,
 	struct ieee_types_assoc_rsp *assoc_rsp;
 	struct mwifiex_bssdescriptor *bss_desc;
 	bool enable_data = true;
-	u16 cap_info, status_code;
+	u16 cap_info, status_code, aid;
 
 	assoc_rsp = (struct ieee_types_assoc_rsp *) &resp->params;
 
 	cap_info = le16_to_cpu(assoc_rsp->cap_info_bitmap);
 	status_code = le16_to_cpu(assoc_rsp->status_code);
+	aid = le16_to_cpu(assoc_rsp->a_id);
+
+	if ((aid & (BIT(15) | BIT(14))) != (BIT(15) | BIT(14)))
+		dev_err(priv->adapter->dev,
+			"invalid AID value 0x%x; bits 15:14 not set\n",
+			aid);
+
+	aid &= ~(BIT(15) | BIT(14));
 
 	priv->assoc_rsp_size = min(le16_to_cpu(resp->size) - S_DS_GEN,
 				   sizeof(priv->assoc_rsp_buf));
 
 	memcpy(priv->assoc_rsp_buf, &resp->params, priv->assoc_rsp_size);
 
+	assoc_rsp->a_id = cpu_to_le16(aid);
+
 	if (status_code) {
 		priv->adapter->dbg.num_cmd_assoc_failure++;
-		dev_err(priv->adapter->dev,
-			"ASSOC_RESP: failed, status code=%d err=%#x a_id=%#x\n",
-			status_code, cap_info, le16_to_cpu(assoc_rsp->a_id));
-
-		if (cap_info == MWIFIEX_TIMEOUT_FOR_AP_RESP) {
-			if (status_code == MWIFIEX_STATUS_CODE_AUTH_TIMEOUT)
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "ASSOC_RESP: failed,\t"
+			    "status code=%d err=%#x a_id=%#x\n",
+			    status_code, cap_info,
+			    le16_to_cpu(assoc_rsp->a_id));
+
+		mwifiex_dbg(priv->adapter, ERROR, "assoc failure: reason %s\n",
+			    assoc_failure_reason_to_str(cap_info));
+		if (cap_info == CONNECT_ERR_ASSOC_ERR_TIMEOUT) {
+			if (status_code == MWIFIEX_ASSOC_CMD_FAILURE_AUTH) {
 				ret = WLAN_STATUS_AUTH_TIMEOUT;
-			else
+				mwifiex_dbg(priv->adapter, ERROR,
+					    "ASSOC_RESP: AUTH timeout\n");
+			} else {
 				ret = WLAN_STATUS_UNSPECIFIED_FAILURE;
+				mwifiex_dbg(priv->adapter, ERROR,
+					    "ASSOC_RESP: UNSPECIFIED failure\n");
+			}
 		} else {
 			ret = status_code;
 		}
@@ -661,8 +702,8 @@ int mwifiex_ret_802_11_associate(struct mwifiex_private *priv,
 	/* Set the attempted BSSID Index to current */
 	bss_desc = priv->attempted_bss_desc;
 
-	dev_dbg(priv->adapter->dev, "info: ASSOC_RESP: %s\n",
-		bss_desc->ssid.ssid);
+	mwifiex_dbg(priv->adapter, INFO, "info: ASSOC_RESP: %s\n",
+		    bss_desc->ssid.ssid);
 
 	/* Make a copy of current BSSID descriptor */
 	memcpy(&priv->curr_bss_params.bss_descriptor,
@@ -692,8 +733,9 @@ int mwifiex_ret_802_11_associate(struct mwifiex_private *priv,
 			= ((bss_desc->wmm_ie.qos_info_bitmap &
 				IEEE80211_WMM_IE_AP_QOSINFO_UAPSD) ? 1 : 0);
 
-	dev_dbg(priv->adapter->dev, "info: ASSOC_RESP: curr_pkt_filter is %#x\n",
-		priv->curr_pkt_filter);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: ASSOC_RESP: curr_pkt_filter is %#x\n",
+		    priv->curr_pkt_filter);
 	if (priv->sec_info.wpa_enabled || priv->sec_info.wpa2_enabled)
 		priv->wpa_is_gtk_set = false;
 
@@ -709,8 +751,8 @@ int mwifiex_ret_802_11_associate(struct mwifiex_private *priv,
 	}
 
 	if (enable_data)
-		dev_dbg(priv->adapter->dev,
-			"info: post association, re-enabling data flow\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: post association, re-enabling data flow\n");
 
 	/* Reset SNR/NF/RSSI values */
 	priv->data_rssi_last = 0;
@@ -728,7 +770,7 @@ int mwifiex_ret_802_11_associate(struct mwifiex_private *priv,
 
 	priv->adapter->dbg.num_cmd_assoc_success++;
 
-	dev_dbg(priv->adapter->dev, "info: ASSOC_RESP: associated\n");
+	mwifiex_dbg(priv->adapter, INFO, "info: ASSOC_RESP: associated\n");
 
 	/* Add the ra_list here for infra mode as there will be only 1 ra
 	   always */
@@ -815,8 +857,8 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 
 	memcpy(adhoc_start->ssid, req_ssid->ssid, req_ssid->ssid_len);
 
-	dev_dbg(adapter->dev, "info: ADHOC_S_CMD: SSID = %s\n",
-		adhoc_start->ssid);
+	mwifiex_dbg(adapter, INFO, "info: ADHOC_S_CMD: SSID = %s\n",
+		    adhoc_start->ssid);
 
 	memset(bss_desc->ssid.ssid, 0, IEEE80211_MAX_SSID_LEN);
 	memcpy(bss_desc->ssid.ssid, req_ssid->ssid, req_ssid->ssid_len);
@@ -848,12 +890,14 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 	}
 
 	if (!priv->adhoc_channel) {
-		dev_err(adapter->dev, "ADHOC_S_CMD: adhoc_channel cannot be 0\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "ADHOC_S_CMD: adhoc_channel cannot be 0\n");
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "info: ADHOC_S_CMD: creating ADHOC on channel %d\n",
-		priv->adhoc_channel);
+	mwifiex_dbg(adapter, INFO,
+		    "info: ADHOC_S_CMD: creating ADHOC on channel %d\n",
+		    priv->adhoc_channel);
 
 	priv->curr_bss_params.bss_descriptor.channel = priv->adhoc_channel;
 	priv->curr_bss_params.band = adapter->adhoc_start_band;
@@ -885,13 +929,14 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 	/* Set up privacy in bss_desc */
 	if (priv->sec_info.encryption_mode) {
 		/* Ad-Hoc capability privacy on */
-		dev_dbg(adapter->dev,
-			"info: ADHOC_S_CMD: wep_status set privacy to WEP\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: ADHOC_S_CMD: wep_status set privacy to WEP\n");
 		bss_desc->privacy = MWIFIEX_802_11_PRIV_FILTER_8021X_WEP;
 		tmp_cap |= WLAN_CAPABILITY_PRIVACY;
 	} else {
-		dev_dbg(adapter->dev, "info: ADHOC_S_CMD: wep_status NOT set,"
-				" setting privacy to ACCEPT ALL\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: ADHOC_S_CMD: wep_status NOT set,\t"
+			    "setting privacy to ACCEPT ALL\n");
 		bss_desc->privacy = MWIFIEX_802_11_PRIV_FILTER_ACCEPT_ALL;
 	}
 
@@ -902,8 +947,8 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 		if (mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
 				     HostCmd_ACT_GEN_SET, 0,
 				     &priv->curr_pkt_filter, false)) {
-			dev_err(adapter->dev,
-				"ADHOC_S_CMD: G Protection config failed\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "ADHOC_S_CMD: G Protection config failed\n");
 			return -1;
 		}
 	}
@@ -918,10 +963,10 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 	memcpy(&priv->curr_bss_params.data_rates,
 	       &adhoc_start->data_rate, priv->curr_bss_params.num_of_rates);
 
-	dev_dbg(adapter->dev, "info: ADHOC_S_CMD: rates=%4ph\n",
-		adhoc_start->data_rate);
+	mwifiex_dbg(adapter, INFO, "info: ADHOC_S_CMD: rates=%4ph\n",
+		    adhoc_start->data_rate);
 
-	dev_dbg(adapter->dev, "info: ADHOC_S_CMD: AD-HOC Start command is ready\n");
+	mwifiex_dbg(adapter, INFO, "info: ADHOC_S_CMD: AD-HOC Start command is ready\n");
 
 	if (IS_SUPPORT_MULTI_BANDS(adapter)) {
 		/* Append a channel TLV */
@@ -935,8 +980,8 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 		chan_tlv->chan_scan_param[0].chan_number =
 			(u8) priv->curr_bss_params.bss_descriptor.channel;
 
-		dev_dbg(adapter->dev, "info: ADHOC_S_CMD: TLV Chan = %d\n",
-			chan_tlv->chan_scan_param[0].chan_number);
+		mwifiex_dbg(adapter, INFO, "info: ADHOC_S_CMD: TLV Chan = %d\n",
+			    chan_tlv->chan_scan_param[0].chan_number);
 
 		chan_tlv->chan_scan_param[0].radio_type
 		       = mwifiex_band_to_radio_type(priv->curr_bss_params.band);
@@ -951,8 +996,8 @@ mwifiex_cmd_802_11_ad_hoc_start(struct mwifiex_private *priv,
 				chan_tlv->chan_scan_param[0].radio_type |=
 					(IEEE80211_HT_PARAM_CHA_SEC_BELOW << 4);
 		}
-		dev_dbg(adapter->dev, "info: ADHOC_S_CMD: TLV Band = %d\n",
-			chan_tlv->chan_scan_param[0].radio_type);
+		mwifiex_dbg(adapter, INFO, "info: ADHOC_S_CMD: TLV Band = %d\n",
+			    chan_tlv->chan_scan_param[0].radio_type);
 		pos += sizeof(chan_tlv->header) +
 			sizeof(struct mwifiex_chan_scan_param_set);
 		cmd_append_size +=
@@ -1074,8 +1119,8 @@ mwifiex_cmd_802_11_ad_hoc_join(struct mwifiex_private *priv,
 		if (mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
 				     HostCmd_ACT_GEN_SET, 0,
 				     &curr_pkt_filter, false)) {
-			dev_err(priv->adapter->dev,
-				"ADHOC_J_CMD: G Protection config failed\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "ADHOC_J_CMD: G Protection config failed\n");
 			return -1;
 		}
 	}
@@ -1106,14 +1151,15 @@ mwifiex_cmd_802_11_ad_hoc_join(struct mwifiex_private *priv,
 
 	tmp_cap &= CAPINFO_MASK;
 
-	dev_dbg(priv->adapter->dev,
-		"info: ADHOC_J_CMD: tmp_cap=%4X CAPINFO_MASK=%4lX\n",
-		tmp_cap, CAPINFO_MASK);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: ADHOC_J_CMD: tmp_cap=%4X CAPINFO_MASK=%4lX\n",
+		    tmp_cap, CAPINFO_MASK);
 
 	/* Information on BSSID descriptor passed to FW */
-	dev_dbg(priv->adapter->dev, "info: ADHOC_J_CMD: BSSID=%pM, SSID='%s'\n",
-		adhoc_join->bss_descriptor.bssid,
-		adhoc_join->bss_descriptor.ssid);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: ADHOC_J_CMD: BSSID=%pM, SSID='%s'\n",
+		    adhoc_join->bss_descriptor.bssid,
+		    adhoc_join->bss_descriptor.ssid);
 
 	for (i = 0; i < MWIFIEX_SUPPORTED_RATES &&
 		    bss_desc->supported_rates[i]; i++)
@@ -1149,14 +1195,14 @@ mwifiex_cmd_802_11_ad_hoc_join(struct mwifiex_private *priv,
 		       sizeof(struct mwifiex_chan_scan_param_set));
 		chan_tlv->chan_scan_param[0].chan_number =
 			(bss_desc->phy_param_set.ds_param_set.current_chan);
-		dev_dbg(priv->adapter->dev, "info: ADHOC_J_CMD: TLV Chan=%d\n",
-			chan_tlv->chan_scan_param[0].chan_number);
+		mwifiex_dbg(priv->adapter, INFO, "info: ADHOC_J_CMD: TLV Chan=%d\n",
+			    chan_tlv->chan_scan_param[0].chan_number);
 
 		chan_tlv->chan_scan_param[0].radio_type =
 			mwifiex_band_to_radio_type((u8) bss_desc->bss_band);
 
-		dev_dbg(priv->adapter->dev, "info: ADHOC_J_CMD: TLV Band=%d\n",
-			chan_tlv->chan_scan_param[0].radio_type);
+		mwifiex_dbg(priv->adapter, INFO, "info: ADHOC_J_CMD: TLV Band=%d\n",
+			    chan_tlv->chan_scan_param[0].radio_type);
 		pos += sizeof(chan_tlv->header) +
 				sizeof(struct mwifiex_chan_scan_param_set);
 		cmd_append_size += sizeof(chan_tlv->header) +
@@ -1210,7 +1256,7 @@ int mwifiex_ret_802_11_ad_hoc(struct mwifiex_private *priv,
 	/* Join result code 0 --> SUCCESS */
 	reason_code = le16_to_cpu(resp->result);
 	if (reason_code) {
-		dev_err(priv->adapter->dev, "ADHOC_RESP: failed\n");
+		mwifiex_dbg(priv->adapter, ERROR, "ADHOC_RESP: failed\n");
 		if (priv->media_connected)
 			mwifiex_reset_connect_state(priv, reason_code);
 
@@ -1225,8 +1271,8 @@ int mwifiex_ret_802_11_ad_hoc(struct mwifiex_private *priv,
 	priv->media_connected = true;
 
 	if (le16_to_cpu(resp->command) == HostCmd_CMD_802_11_AD_HOC_START) {
-		dev_dbg(priv->adapter->dev, "info: ADHOC_S_RESP %s\n",
-			bss_desc->ssid.ssid);
+		mwifiex_dbg(priv->adapter, INFO, "info: ADHOC_S_RESP %s\n",
+			    bss_desc->ssid.ssid);
 
 		/* Update the created network descriptor with the new BSSID */
 		memcpy(bss_desc->mac_address,
@@ -1238,8 +1284,9 @@ int mwifiex_ret_802_11_ad_hoc(struct mwifiex_private *priv,
 		 * Now the join cmd should be successful.
 		 * If BSSID has changed use SSID to compare instead of BSSID
 		 */
-		dev_dbg(priv->adapter->dev, "info: ADHOC_J_RESP %s\n",
-			bss_desc->ssid.ssid);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: ADHOC_J_RESP %s\n",
+			    bss_desc->ssid.ssid);
 
 		/*
 		 * Make a copy of current BSSID descriptor, only needed for
@@ -1252,10 +1299,10 @@ int mwifiex_ret_802_11_ad_hoc(struct mwifiex_private *priv,
 		priv->adhoc_state = ADHOC_JOINED;
 	}
 
-	dev_dbg(priv->adapter->dev, "info: ADHOC_RESP: channel = %d\n",
-		priv->adhoc_channel);
-	dev_dbg(priv->adapter->dev, "info: ADHOC_RESP: BSSID = %pM\n",
-		priv->curr_bss_params.bss_descriptor.mac_address);
+	mwifiex_dbg(priv->adapter, INFO, "info: ADHOC_RESP: channel = %d\n",
+		    priv->adhoc_channel);
+	mwifiex_dbg(priv->adapter, INFO, "info: ADHOC_RESP: BSSID = %pM\n",
+		    priv->curr_bss_params.bss_descriptor.mac_address);
 
 	if (!netif_carrier_ok(priv->netdev))
 		netif_carrier_on(priv->netdev);
@@ -1317,12 +1364,12 @@ int
 mwifiex_adhoc_start(struct mwifiex_private *priv,
 		    struct cfg80211_ssid *adhoc_ssid)
 {
-	dev_dbg(priv->adapter->dev, "info: Adhoc Channel = %d\n",
-		priv->adhoc_channel);
-	dev_dbg(priv->adapter->dev, "info: curr_bss_params.channel = %d\n",
-		priv->curr_bss_params.bss_descriptor.channel);
-	dev_dbg(priv->adapter->dev, "info: curr_bss_params.band = %d\n",
-		priv->curr_bss_params.band);
+	mwifiex_dbg(priv->adapter, INFO, "info: Adhoc Channel = %d\n",
+		    priv->adhoc_channel);
+	mwifiex_dbg(priv->adapter, INFO, "info: curr_bss_params.channel = %d\n",
+		    priv->curr_bss_params.bss_descriptor.channel);
+	mwifiex_dbg(priv->adapter, INFO, "info: curr_bss_params.band = %d\n",
+		    priv->curr_bss_params.band);
 
 	if (ISSUPP_11ACENABLED(priv->adapter->fw_cap_info) &&
 	    priv->adapter->config_bands & BAND_AAC)
@@ -1343,14 +1390,16 @@ mwifiex_adhoc_start(struct mwifiex_private *priv,
 int mwifiex_adhoc_join(struct mwifiex_private *priv,
 		       struct mwifiex_bssdescriptor *bss_desc)
 {
-	dev_dbg(priv->adapter->dev, "info: adhoc join: curr_bss ssid =%s\n",
-		priv->curr_bss_params.bss_descriptor.ssid.ssid);
-	dev_dbg(priv->adapter->dev, "info: adhoc join: curr_bss ssid_len =%u\n",
-		priv->curr_bss_params.bss_descriptor.ssid.ssid_len);
-	dev_dbg(priv->adapter->dev, "info: adhoc join: ssid =%s\n",
-		bss_desc->ssid.ssid);
-	dev_dbg(priv->adapter->dev, "info: adhoc join: ssid_len =%u\n",
-		bss_desc->ssid.ssid_len);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: adhoc join: curr_bss ssid =%s\n",
+		    priv->curr_bss_params.bss_descriptor.ssid.ssid);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: adhoc join: curr_bss ssid_len =%u\n",
+		    priv->curr_bss_params.bss_descriptor.ssid.ssid_len);
+	mwifiex_dbg(priv->adapter, INFO, "info: adhoc join: ssid =%s\n",
+		    bss_desc->ssid.ssid);
+	mwifiex_dbg(priv->adapter, INFO, "info: adhoc join: ssid_len =%u\n",
+		    bss_desc->ssid.ssid_len);
 
 	/* Check if the requested SSID is already joined */
 	if (priv->curr_bss_params.bss_descriptor.ssid.ssid_len &&
@@ -1358,8 +1407,9 @@ int mwifiex_adhoc_join(struct mwifiex_private *priv,
 			      &priv->curr_bss_params.bss_descriptor.ssid) &&
 	    (priv->curr_bss_params.bss_descriptor.bss_mode ==
 							NL80211_IFTYPE_ADHOC)) {
-		dev_dbg(priv->adapter->dev, "info: ADHOC_J_CMD: new ad-hoc SSID"
-			" is the same as current; not attempting to re-join\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: ADHOC_J_CMD: new ad-hoc SSID\t"
+			    "is the same as current; not attempting to re-join\n");
 		return -1;
 	}
 
@@ -1370,10 +1420,12 @@ int mwifiex_adhoc_join(struct mwifiex_private *priv,
 	else
 		mwifiex_set_ba_params(priv);
 
-	dev_dbg(priv->adapter->dev, "info: curr_bss_params.channel = %d\n",
-		priv->curr_bss_params.bss_descriptor.channel);
-	dev_dbg(priv->adapter->dev, "info: curr_bss_params.band = %c\n",
-		priv->curr_bss_params.band);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: curr_bss_params.channel = %d\n",
+		    priv->curr_bss_params.bss_descriptor.channel);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: curr_bss_params.band = %c\n",
+		    priv->curr_bss_params.band);
 
 	return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_AD_HOC_JOIN,
 				HostCmd_ACT_GEN_SET, 0, bss_desc, true);
@@ -1421,7 +1473,7 @@ int mwifiex_deauthenticate(struct mwifiex_private *priv, u8 *mac)
 		ret = mwifiex_deauthenticate_infra(priv, mac);
 		if (ret)
 			cfg80211_disconnected(priv->netdev, 0, NULL, 0,
-					      GFP_KERNEL);
+					      true, GFP_KERNEL);
 		break;
 	case NL80211_IFTYPE_ADHOC:
 		return mwifiex_send_cmd(priv, HostCmd_CMD_802_11_AD_HOC_STOP,
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index 03a95c7d34bf..3ba4e0e04223 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -24,6 +24,10 @@
 
 #define VERSION	"1.0"
 
+static unsigned int debug_mask = MWIFIEX_DEFAULT_DEBUG_MASK;
+module_param(debug_mask, uint, 0);
+MODULE_PARM_DESC(debug_mask, "bitmap for debug flags");
+
 const char driver_version[] = "mwifiex " VERSION " (%s) ";
 static char *cal_data_cfg;
 module_param(cal_data_cfg, charp, 0);
@@ -63,6 +67,7 @@ static int mwifiex_register(void *card, struct mwifiex_if_ops *if_ops,
 
 	/* Save interface specific operations in adapter */
 	memmove(&adapter->if_ops, if_ops, sizeof(struct mwifiex_if_ops));
+	adapter->debug_mask = debug_mask;
 
 	/* card specific initialization has been deferred until now .. */
 	if (adapter->if_ops.init_if)
@@ -89,7 +94,8 @@ static int mwifiex_register(void *card, struct mwifiex_if_ops *if_ops,
 	return 0;
 
 error:
-	dev_dbg(adapter->dev, "info: leave mwifiex_register with error\n");
+	mwifiex_dbg(adapter, ERROR,
+		    "info: leave mwifiex_register with error\n");
 
 	for (i = 0; i < adapter->priv_num; i++)
 		kfree(adapter->priv[i]);
@@ -231,11 +237,10 @@ int mwifiex_main_process(struct mwifiex_adapter *adapter)
 		goto exit_main_proc;
 	} else {
 		adapter->mwifiex_processing = true;
+		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 	}
 process_start:
 	do {
-		adapter->more_task_flag = false;
-		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 		if ((adapter->hw_status == MWIFIEX_HW_STATUS_CLOSING) ||
 		    (adapter->hw_status == MWIFIEX_HW_STATUS_NOT_READY))
 			break;
@@ -275,7 +280,6 @@ process_start:
 			adapter->pm_wakeup_fw_try = true;
 			mod_timer(&adapter->wakeup_timer, jiffies + (HZ*3));
 			adapter->if_ops.wakeup(adapter);
-			spin_lock_irqsave(&adapter->main_proc_lock, flags);
 			continue;
 		}
 
@@ -335,7 +339,6 @@ process_start:
 		    (adapter->ps_state == PS_STATE_PRE_SLEEP) ||
 		    (adapter->ps_state == PS_STATE_SLEEP_CFM) ||
 		    adapter->tx_lock_flag){
-			spin_lock_irqsave(&adapter->main_proc_lock, flags);
 			continue;
 		}
 
@@ -386,12 +389,14 @@ process_start:
 			}
 			break;
 		}
-		spin_lock_irqsave(&adapter->main_proc_lock, flags);
 	} while (true);
 
 	spin_lock_irqsave(&adapter->main_proc_lock, flags);
-	if (adapter->more_task_flag)
+	if (adapter->more_task_flag) {
+		adapter->more_task_flag = false;
+		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 		goto process_start;
+	}
 	adapter->mwifiex_processing = false;
 	spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 
@@ -455,8 +460,8 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	struct wireless_dev *wdev;
 
 	if (!firmware) {
-		dev_err(adapter->dev,
-			"Failed to get firmware %s\n", adapter->fw_name);
+		mwifiex_dbg(adapter, ERROR,
+			    "Failed to get firmware %s\n", adapter->fw_name);
 		goto err_dnld_fw;
 	}
 
@@ -472,13 +477,13 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	if (ret == -1)
 		goto err_dnld_fw;
 
-	dev_notice(adapter->dev, "WLAN FW is active\n");
+	mwifiex_dbg(adapter, MSG, "WLAN FW is active\n");
 
 	if (cal_data_cfg) {
 		if ((request_firmware(&adapter->cal_data, cal_data_cfg,
 				      adapter->dev)) < 0)
-			dev_err(adapter->dev,
-				"Cal data request_firmware() failed\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Cal data request_firmware() failed\n");
 	}
 
 	/* enable host interrupt after fw dnld is successful */
@@ -503,12 +508,14 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 
 	priv = adapter->priv[MWIFIEX_BSS_ROLE_STA];
 	if (mwifiex_register_cfg80211(adapter)) {
-		dev_err(adapter->dev, "cannot register with cfg80211\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot register with cfg80211\n");
 		goto err_init_fw;
 	}
 
 	if (mwifiex_init_channel_scan_gap(adapter)) {
-		dev_err(adapter->dev, "could not init channel stats table\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "could not init channel stats table\n");
 		goto err_init_fw;
 	}
 
@@ -522,7 +529,8 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	wdev = mwifiex_add_virtual_intf(adapter->wiphy, "mlan%d", NET_NAME_ENUM,
 					NL80211_IFTYPE_STATION, NULL, NULL);
 	if (IS_ERR(wdev)) {
-		dev_err(adapter->dev, "cannot create default STA interface\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot create default STA interface\n");
 		rtnl_unlock();
 		goto err_add_intf;
 	}
@@ -531,7 +539,8 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 		wdev = mwifiex_add_virtual_intf(adapter->wiphy, "uap%d", NET_NAME_ENUM,
 						NL80211_IFTYPE_AP, NULL, NULL);
 		if (IS_ERR(wdev)) {
-			dev_err(adapter->dev, "cannot create AP interface\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "cannot create AP interface\n");
 			rtnl_unlock();
 			goto err_add_intf;
 		}
@@ -542,8 +551,8 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 						NL80211_IFTYPE_P2P_CLIENT, NULL,
 						NULL);
 		if (IS_ERR(wdev)) {
-			dev_err(adapter->dev,
-				"cannot create p2p client interface\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "cannot create p2p client interface\n");
 			rtnl_unlock();
 			goto err_add_intf;
 		}
@@ -551,7 +560,7 @@ static void mwifiex_fw_dpc(const struct firmware *firmware, void *context)
 	rtnl_unlock();
 
 	mwifiex_drv_get_driver_version(adapter, fmt, sizeof(fmt) - 1);
-	dev_notice(adapter->dev, "driver_version = %s\n", fmt);
+	mwifiex_dbg(adapter, MSG, "driver_version = %s\n", fmt);
 	goto done;
 
 err_add_intf:
@@ -561,7 +570,8 @@ err_init_fw:
 	if (adapter->if_ops.disable_int)
 		adapter->if_ops.disable_int(adapter);
 err_dnld_fw:
-	pr_debug("info: %s: unregister device\n", __func__);
+	mwifiex_dbg(adapter, ERROR,
+		    "info: %s: unregister device\n", __func__);
 	if (adapter->if_ops.unregister_dev)
 		adapter->if_ops.unregister_dev(adapter);
 
@@ -602,8 +612,8 @@ static int mwifiex_init_hw_fw(struct mwifiex_adapter *adapter)
 				      adapter->dev, GFP_KERNEL, adapter,
 				      mwifiex_fw_dpc);
 	if (ret < 0)
-		dev_err(adapter->dev,
-			"request_firmware_nowait() returned error %d\n", ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "request_firmware_nowait error %d\n", ret);
 	return ret;
 }
 
@@ -629,7 +639,8 @@ mwifiex_close(struct net_device *dev)
 	struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
 
 	if (priv->scan_request) {
-		dev_dbg(priv->adapter->dev, "aborting scan on ndo_stop\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "aborting scan on ndo_stop\n");
 		cfg80211_scan_done(priv->scan_request, 1);
 		priv->scan_request = NULL;
 		priv->scan_aborting = true;
@@ -650,7 +661,8 @@ int mwifiex_queue_tx_pkt(struct mwifiex_private *priv, struct sk_buff *skb)
 		txq = netdev_get_tx_queue(priv->netdev, index);
 		if (!netif_tx_queue_stopped(txq)) {
 			netif_tx_stop_queue(txq);
-			dev_dbg(priv->adapter->dev, "stop queue: %d\n", index);
+			mwifiex_dbg(priv->adapter, DATA,
+				    "stop queue: %d\n", index);
 		}
 	}
 
@@ -715,8 +727,9 @@ mwifiex_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct mwifiex_txinfo *tx_info;
 	bool multicast;
 
-	dev_dbg(priv->adapter->dev, "data: %lu BSS(%d-%d): Data <= kernel\n",
-		jiffies, priv->bss_type, priv->bss_num);
+	mwifiex_dbg(priv->adapter, DATA,
+		    "data: %lu BSS(%d-%d): Data <= kernel\n",
+		    jiffies, priv->bss_type, priv->bss_num);
 
 	if (priv->adapter->surprise_removed) {
 		kfree_skb(skb);
@@ -724,28 +737,31 @@ mwifiex_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		return 0;
 	}
 	if (!skb->len || (skb->len > ETH_FRAME_LEN)) {
-		dev_err(priv->adapter->dev, "Tx: bad skb len %d\n", skb->len);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Tx: bad skb len %d\n", skb->len);
 		kfree_skb(skb);
 		priv->stats.tx_dropped++;
 		return 0;
 	}
 	if (skb_headroom(skb) < MWIFIEX_MIN_DATA_HEADER_LEN) {
-		dev_dbg(priv->adapter->dev,
-			"data: Tx: insufficient skb headroom %d\n",
-			skb_headroom(skb));
+		mwifiex_dbg(priv->adapter, DATA,
+			    "data: Tx: insufficient skb headroom %d\n",
+			    skb_headroom(skb));
 		/* Insufficient skb headroom - allocate a new skb */
 		new_skb =
 			skb_realloc_headroom(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
 		if (unlikely(!new_skb)) {
-			dev_err(priv->adapter->dev, "Tx: cannot alloca new_skb\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "Tx: cannot alloca new_skb\n");
 			kfree_skb(skb);
 			priv->stats.tx_dropped++;
 			return 0;
 		}
 		kfree_skb(skb);
 		skb = new_skb;
-		dev_dbg(priv->adapter->dev, "info: new skb headroomd %d\n",
-			skb_headroom(skb));
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: new skb headroomd %d\n",
+			    skb_headroom(skb));
 	}
 
 	tx_info = MWIFIEX_SKB_TXCB(skb);
@@ -803,8 +819,8 @@ mwifiex_set_mac_address(struct net_device *dev, void *addr)
 	if (!ret)
 		memcpy(priv->netdev->dev_addr, priv->curr_addr, ETH_ALEN);
 	else
-		dev_err(priv->adapter->dev,
-			"set mac address failed: ret=%d\n", ret);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "set mac address failed: ret=%d\n", ret);
 
 	memcpy(dev->dev_addr, priv->curr_addr, ETH_ALEN);
 
@@ -842,20 +858,22 @@ mwifiex_tx_timeout(struct net_device *dev)
 
 	priv->num_tx_timeout++;
 	priv->tx_timeout_cnt++;
-	dev_err(priv->adapter->dev,
-		"%lu : Tx timeout(#%d), bss_type-num = %d-%d\n",
-		jiffies, priv->tx_timeout_cnt, priv->bss_type, priv->bss_num);
+	mwifiex_dbg(priv->adapter, ERROR,
+		    "%lu : Tx timeout(#%d), bss_type-num = %d-%d\n",
+		    jiffies, priv->tx_timeout_cnt, priv->bss_type,
+		    priv->bss_num);
 	mwifiex_set_trans_start(dev);
 
 	if (priv->tx_timeout_cnt > TX_TIMEOUT_THRESHOLD &&
 	    priv->adapter->if_ops.card_reset) {
-		dev_err(priv->adapter->dev,
-			"tx_timeout_cnt exceeds threshold. Triggering card reset!\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "tx_timeout_cnt exceeds threshold.\t"
+			    "Triggering card reset!\n");
 		priv->adapter->if_ops.card_reset(priv->adapter);
 	}
 }
 
-void mwifiex_dump_drv_info(struct mwifiex_adapter *adapter)
+void mwifiex_drv_info_dump(struct mwifiex_adapter *adapter)
 {
 	void *p;
 	char drv_version[64];
@@ -868,10 +886,11 @@ void mwifiex_dump_drv_info(struct mwifiex_adapter *adapter)
 
 	if (adapter->drv_info_dump) {
 		vfree(adapter->drv_info_dump);
+		adapter->drv_info_dump = NULL;
 		adapter->drv_info_size = 0;
 	}
 
-	dev_info(adapter->dev, "=== DRIVER INFO DUMP START===\n");
+	mwifiex_dbg(adapter, MSG, "===mwifiex driverinfo dump start===\n");
 
 	adapter->drv_info_dump = vzalloc(MWIFIEX_DRV_INFO_SIZE_MAX);
 
@@ -939,12 +958,12 @@ void mwifiex_dump_drv_info(struct mwifiex_adapter *adapter)
 	}
 
 	if (adapter->iface_type == MWIFIEX_SDIO) {
-		p += sprintf(p, "\n=== SDIO register DUMP===\n");
+		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");
+	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++) {
@@ -959,9 +978,99 @@ void mwifiex_dump_drv_info(struct mwifiex_adapter *adapter)
 	}
 
 	adapter->drv_info_size = p - adapter->drv_info_dump;
-	dev_info(adapter->dev, "=== DRIVER INFO DUMP END===\n");
+	mwifiex_dbg(adapter, MSG, "===mwifiex driverinfo dump end===\n");
+}
+EXPORT_SYMBOL_GPL(mwifiex_drv_info_dump);
+
+void mwifiex_upload_device_dump(struct mwifiex_adapter *adapter)
+{
+	u8 idx, *dump_data, *fw_dump_ptr;
+	u32 dump_len;
+
+	dump_len = (strlen("========Start dump driverinfo========\n") +
+		       adapter->drv_info_size +
+		       strlen("\n========End dump========\n"));
+
+	for (idx = 0; idx < adapter->num_mem_types; idx++) {
+		struct memory_type_mapping *entry =
+				&adapter->mem_type_mapping_tbl[idx];
+
+		if (entry->mem_ptr) {
+			dump_len += (strlen("========Start dump ") +
+					strlen(entry->mem_name) +
+					strlen("========\n") +
+					(entry->mem_size + 1) +
+					strlen("\n========End dump========\n"));
+		}
+	}
+
+	dump_data = vzalloc(dump_len + 1);
+	if (!dump_data)
+		goto done;
+
+	fw_dump_ptr = dump_data;
+
+	/* Dump all the memory data into single file, a userspace script will
+	 * be used to split all the memory data to multiple files
+	 */
+	mwifiex_dbg(adapter, MSG,
+		    "== mwifiex dump information to /sys/class/devcoredump start");
+
+	strcpy(fw_dump_ptr, "========Start dump driverinfo========\n");
+	fw_dump_ptr += strlen("========Start dump driverinfo========\n");
+	memcpy(fw_dump_ptr, adapter->drv_info_dump, adapter->drv_info_size);
+	fw_dump_ptr += adapter->drv_info_size;
+	strcpy(fw_dump_ptr, "\n========End dump========\n");
+	fw_dump_ptr += strlen("\n========End dump========\n");
+
+	for (idx = 0; idx < adapter->num_mem_types; idx++) {
+		struct memory_type_mapping *entry =
+					&adapter->mem_type_mapping_tbl[idx];
+
+		if (entry->mem_ptr) {
+			strcpy(fw_dump_ptr, "========Start dump ");
+			fw_dump_ptr += strlen("========Start dump ");
+
+			strcpy(fw_dump_ptr, entry->mem_name);
+			fw_dump_ptr += strlen(entry->mem_name);
+
+			strcpy(fw_dump_ptr, "========\n");
+			fw_dump_ptr += strlen("========\n");
+
+			memcpy(fw_dump_ptr, entry->mem_ptr, entry->mem_size);
+			fw_dump_ptr += entry->mem_size;
+
+			strcpy(fw_dump_ptr, "\n========End dump========\n");
+			fw_dump_ptr += strlen("\n========End dump========\n");
+		}
+	}
+
+	/* device dump data will be free in device coredump release function
+	 * after 5 min
+	 */
+	dev_coredumpv(adapter->dev, dump_data, dump_len, GFP_KERNEL);
+	mwifiex_dbg(adapter, MSG,
+		    "== mwifiex dump information to /sys/class/devcoredump end");
+
+done:
+	for (idx = 0; idx < adapter->num_mem_types; idx++) {
+		struct memory_type_mapping *entry =
+			&adapter->mem_type_mapping_tbl[idx];
+
+		if (entry->mem_ptr) {
+			vfree(entry->mem_ptr);
+			entry->mem_ptr = NULL;
+		}
+		entry->mem_size = 0;
+	}
+
+	if (adapter->drv_info_dump) {
+		vfree(adapter->drv_info_dump);
+		adapter->drv_info_dump = NULL;
+		adapter->drv_info_size = 0;
+	}
 }
-EXPORT_SYMBOL_GPL(mwifiex_dump_drv_info);
+EXPORT_SYMBOL_GPL(mwifiex_upload_device_dump);
 
 /*
  * CFG802.11 network device handler for statistics retrieval.
@@ -1230,21 +1339,24 @@ int mwifiex_remove_card(struct mwifiex_adapter *adapter, struct semaphore *sem)
 		}
 	}
 
-	dev_dbg(adapter->dev, "cmd: calling mwifiex_shutdown_drv...\n");
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: calling mwifiex_shutdown_drv...\n");
 	adapter->init_wait_q_woken = false;
 
 	if (mwifiex_shutdown_drv(adapter) == -EINPROGRESS)
 		wait_event_interruptible(adapter->init_wait_q,
 					 adapter->init_wait_q_woken);
-	dev_dbg(adapter->dev, "cmd: mwifiex_shutdown_drv done\n");
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: mwifiex_shutdown_drv done\n");
 	if (atomic_read(&adapter->rx_pending) ||
 	    atomic_read(&adapter->tx_pending) ||
 	    atomic_read(&adapter->cmd_pending)) {
-		dev_err(adapter->dev, "rx_pending=%d, tx_pending=%d, "
-		       "cmd_pending=%d\n",
-		       atomic_read(&adapter->rx_pending),
-		       atomic_read(&adapter->tx_pending),
-		       atomic_read(&adapter->cmd_pending));
+		mwifiex_dbg(adapter, ERROR,
+			    "rx_pending=%d, tx_pending=%d,\t"
+			    "cmd_pending=%d\n",
+			    atomic_read(&adapter->rx_pending),
+			    atomic_read(&adapter->tx_pending),
+			    atomic_read(&adapter->cmd_pending));
 	}
 
 	for (i = 0; i < adapter->priv_num; i++) {
@@ -1264,11 +1376,13 @@ int mwifiex_remove_card(struct mwifiex_adapter *adapter, struct semaphore *sem)
 	wiphy_free(adapter->wiphy);
 
 	/* Unregister device */
-	dev_dbg(adapter->dev, "info: unregister device\n");
+	mwifiex_dbg(adapter, INFO,
+		    "info: unregister device\n");
 	if (adapter->if_ops.unregister_dev)
 		adapter->if_ops.unregister_dev(adapter);
 	/* Free adapter structure */
-	dev_dbg(adapter->dev, "info: free adapter\n");
+	mwifiex_dbg(adapter, INFO,
+		    "info: free adapter\n");
 	mwifiex_free_adapter(adapter);
 
 exit_remove:
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index fe1256044a6c..ae98b5b83b1f 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -36,6 +36,7 @@
 #include <linux/of.h>
 #include <linux/idr.h>
 #include <linux/inetdevice.h>
+#include <linux/devcoredump.h>
 
 #include "decl.h"
 #include "ioctl.h"
@@ -147,6 +148,54 @@ enum {
 /* Address alignment */
 #define MWIFIEX_ALIGN_ADDR(p, a) (((long)(p) + (a) - 1) & ~((a) - 1))
 
+/**
+ *enum mwifiex_debug_level  -  marvell wifi debug level
+ */
+enum MWIFIEX_DEBUG_LEVEL {
+	MWIFIEX_DBG_MSG		= 0x00000001,
+	MWIFIEX_DBG_FATAL	= 0x00000002,
+	MWIFIEX_DBG_ERROR	= 0x00000004,
+	MWIFIEX_DBG_DATA	= 0x00000008,
+	MWIFIEX_DBG_CMD		= 0x00000010,
+	MWIFIEX_DBG_EVENT	= 0x00000020,
+	MWIFIEX_DBG_INTR	= 0x00000040,
+	MWIFIEX_DBG_IOCTL	= 0x00000080,
+
+	MWIFIEX_DBG_MPA_D	= 0x00008000,
+	MWIFIEX_DBG_DAT_D	= 0x00010000,
+	MWIFIEX_DBG_CMD_D	= 0x00020000,
+	MWIFIEX_DBG_EVT_D	= 0x00040000,
+	MWIFIEX_DBG_FW_D	= 0x00080000,
+	MWIFIEX_DBG_IF_D	= 0x00100000,
+
+	MWIFIEX_DBG_ENTRY	= 0x10000000,
+	MWIFIEX_DBG_WARN	= 0x20000000,
+	MWIFIEX_DBG_INFO	= 0x40000000,
+	MWIFIEX_DBG_DUMP	= 0x80000000,
+
+	MWIFIEX_DBG_ANY		= 0xffffffff
+};
+
+#define MWIFIEX_DEFAULT_DEBUG_MASK	(MWIFIEX_DBG_MSG | \
+					MWIFIEX_DBG_FATAL | \
+					MWIFIEX_DBG_ERROR)
+
+#define mwifiex_dbg(adapter, dbg_mask, fmt, args...)		\
+do {								\
+	if ((adapter)->debug_mask & MWIFIEX_DBG_##dbg_mask)	\
+		if ((adapter)->dev)				\
+			dev_info((adapter)->dev, fmt, ## args);	\
+} while (0)
+
+#define DEBUG_DUMP_DATA_MAX_LEN		128
+#define mwifiex_dbg_dump(adapter, dbg_mask, str, buf, len)	\
+do {								\
+	if ((adapter)->debug_mask & MWIFIEX_DBG_##dbg_mask)	\
+		print_hex_dump(KERN_DEBUG, str,			\
+			       DUMP_PREFIX_OFFSET, 16, 1,	\
+			       buf, len, false);		\
+} while (0)
+
 struct mwifiex_dbg {
 	u32 num_cmd_host_to_card_failure;
 	u32 num_cmd_sleep_cfm_host_to_card_failure;
@@ -451,7 +500,7 @@ enum rdwr_status {
 };
 
 enum mwifiex_iface_work_flags {
-	MWIFIEX_IFACE_WORK_FW_DUMP,
+	MWIFIEX_IFACE_WORK_DEVICE_DUMP,
 	MWIFIEX_IFACE_WORK_CARD_RESET,
 };
 
@@ -593,6 +642,7 @@ struct mwifiex_private {
 	u8 del_list_idx;
 	bool hs2_enabled;
 	struct mwifiex_uap_bss_param bss_cfg;
+	struct cfg80211_chan_def bss_chandef;
 	struct station_parameters *sta_params;
 	struct sk_buff_head tdls_txq;
 	u8 check_tdls_tx;
@@ -611,6 +661,7 @@ struct mwifiex_private {
 	struct delayed_work dfs_chan_sw_work;
 	struct cfg80211_beacon_data beacon_after;
 	struct mwifiex_11h_intf_state state_11h;
+	struct mwifiex_ds_mem_rw mem_rw;
 };
 
 
@@ -690,6 +741,18 @@ struct mwifiex_tdls_capab {
 	struct ieee80211_vht_operation vhtoper;
 };
 
+struct mwifiex_station_stats {
+	u64 last_rx;
+	s8 rssi;
+	u64 rx_bytes;
+	u64 tx_bytes;
+	u32 rx_packets;
+	u32 tx_packets;
+	u32 tx_failed;
+	u8 last_tx_rate;
+	u8 last_tx_htinfo;
+};
+
 /* This is AP/TDLS specific structure which stores information
  * about associated/peer STA
  */
@@ -704,6 +767,7 @@ struct mwifiex_sta_node {
 	u16 max_amsdu;
 	u8 tdls_status;
 	struct mwifiex_tdls_capab tdls_cap;
+	struct mwifiex_station_stats stats;
 };
 
 struct mwifiex_auto_tdls_peer {
@@ -740,8 +804,8 @@ struct mwifiex_if_ops {
 	int (*init_fw_port) (struct mwifiex_adapter *);
 	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 *);
+	void (*device_dump)(struct mwifiex_adapter *);
 	int (*clean_pcie_ring) (struct mwifiex_adapter *adapter);
 	void (*iface_work)(struct work_struct *work);
 	void (*submit_rem_rx_urbs)(struct mwifiex_adapter *adapter);
@@ -750,6 +814,7 @@ struct mwifiex_if_ops {
 
 struct mwifiex_adapter {
 	u8 iface_type;
+	unsigned int debug_mask;
 	struct mwifiex_iface_comb iface_limit;
 	struct mwifiex_iface_comb curr_iface_comb;
 	struct mwifiex_private *priv[MWIFIEX_MAX_BSS_NUM];
@@ -900,7 +965,6 @@ struct mwifiex_adapter {
 	u8 key_api_major_ver, key_api_minor_ver;
 	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;
@@ -909,6 +973,12 @@ struct mwifiex_adapter {
 	u32 num_in_chan_stats;
 	int survey_idx;
 	bool auto_tdls;
+	u8 coex_scan;
+	u8 coex_min_scan_time;
+	u8 coex_max_scan_time;
+	u8 coex_win_size;
+	u8 coex_tx_win_size;
+	u8 coex_rx_win_size;
 };
 
 void mwifiex_process_tx_queue(struct mwifiex_adapter *adapter);
@@ -1085,6 +1155,9 @@ void mwifiex_set_ht_params(struct mwifiex_private *priv,
 void mwifiex_set_vht_params(struct mwifiex_private *priv,
 			    struct mwifiex_uap_bss_param *bss_cfg,
 			    struct cfg80211_ap_settings *params);
+void mwifiex_set_tpc_params(struct mwifiex_private *priv,
+			    struct mwifiex_uap_bss_param *bss_cfg,
+			    struct cfg80211_ap_settings *params);
 void mwifiex_set_uap_rates(struct mwifiex_uap_bss_param *bss_cfg,
 			   struct cfg80211_ap_settings *params);
 void mwifiex_set_vht_width(struct mwifiex_private *priv,
@@ -1095,6 +1168,11 @@ mwifiex_set_wmm_params(struct mwifiex_private *priv,
 		       struct mwifiex_uap_bss_param *bss_cfg,
 		       struct cfg80211_ap_settings *params);
 void mwifiex_set_ba_params(struct mwifiex_private *priv);
+
+void mwifiex_update_ampdu_txwinsize(struct mwifiex_adapter *pmadapter);
+void mwifiex_bt_coex_wlan_param_update_event(struct mwifiex_private *priv,
+					     struct sk_buff *event_skb);
+
 void mwifiex_set_11ac_ba_params(struct mwifiex_private *priv);
 int mwifiex_cmd_802_11_scan_ext(struct mwifiex_private *priv,
 				struct host_cmd_ds_command *cmd,
@@ -1332,6 +1410,7 @@ int mwifiex_check_network_compatibility(struct mwifiex_private *priv,
 					struct mwifiex_bssdescriptor *bss_desc);
 
 u8 mwifiex_chan_type_to_sec_chan_offset(enum nl80211_channel_type chan_type);
+u8 mwifiex_sec_chan_offset_to_chan_type(u8 second_chan_offset);
 
 struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 					      const char *name,
@@ -1349,7 +1428,8 @@ int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
 			 struct cfg80211_beacon_data *data);
 int mwifiex_del_mgmt_ies(struct mwifiex_private *priv);
 u8 *mwifiex_11d_code_2_region(u8 code);
-void mwifiex_uap_set_channel(struct mwifiex_uap_bss_param *bss_cfg,
+void mwifiex_uap_set_channel(struct mwifiex_private *priv,
+			     struct mwifiex_uap_bss_param *bss_cfg,
 			     struct cfg80211_chan_def chandef);
 int mwifiex_config_start_uap(struct mwifiex_private *priv,
 			     struct mwifiex_uap_bss_param *bss_cfg);
@@ -1423,6 +1503,8 @@ mwifiex_clone_skb_for_tx_status(struct mwifiex_private *priv,
 void mwifiex_dfs_cac_work_queue(struct work_struct *work);
 void mwifiex_dfs_chan_sw_work_queue(struct work_struct *work);
 void mwifiex_abort_cac(struct mwifiex_private *priv);
+int mwifiex_stop_radar_detection(struct mwifiex_private *priv,
+				 struct cfg80211_chan_def *chandef);
 int mwifiex_11h_handle_radar_detected(struct mwifiex_private *priv,
 				      struct sk_buff *skb);
 
@@ -1434,10 +1516,12 @@ void mwifiex_hist_data_add(struct mwifiex_private *priv,
 u8 mwifiex_adjust_data_rate(struct mwifiex_private *priv,
 			    u8 rx_rate, u8 ht_info);
 
-void mwifiex_dump_drv_info(struct mwifiex_adapter *adapter);
+void mwifiex_drv_info_dump(struct mwifiex_adapter *adapter);
+void mwifiex_upload_device_dump(struct mwifiex_adapter *adapter);
 void *mwifiex_alloc_dma_align_buf(int rx_len, gfp_t flags);
 void mwifiex_queue_main_work(struct mwifiex_adapter *adapter);
-
+void mwifiex_coex_ampdu_rxwinsize(struct mwifiex_adapter *adapter);
+void mwifiex_11n_delba(struct mwifiex_private *priv, int tid);
 #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 bcc7751d883c..77b9055a2d14 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/mwifiex/pcie.c
@@ -57,7 +57,7 @@ mwifiex_map_pci_memory(struct mwifiex_adapter *adapter, struct sk_buff *skb,
 
 	mapping.addr = pci_map_single(card->dev, skb->data, size, flags);
 	if (pci_dma_mapping_error(card->dev, mapping.addr)) {
-		dev_err(adapter->dev, "failed to map pci memory!\n");
+		mwifiex_dbg(adapter, ERROR, "failed to map pci memory!\n");
 		return -1;
 	}
 	mapping.len = size;
@@ -89,8 +89,9 @@ static bool mwifiex_pcie_ok_to_access_hw(struct mwifiex_adapter *adapter)
 
 	if (card->sleep_cookie_vbase) {
 		cookie_addr = (u32 *)card->sleep_cookie_vbase;
-		dev_dbg(adapter->dev, "info: ACCESS_HW: sleep cookie=0x%x\n",
-			*cookie_addr);
+		mwifiex_dbg(adapter, INFO,
+			    "info: ACCESS_HW: sleep cookie=0x%x\n",
+			    *cookie_addr);
 		if (*cookie_addr == FW_AWAKE_COOKIE)
 			return true;
 	}
@@ -164,7 +165,8 @@ static int mwifiex_pcie_resume(struct device *dev)
 	adapter = card->adapter;
 
 	if (!adapter->is_suspended) {
-		dev_warn(adapter->dev, "Device already resumed\n");
+		mwifiex_dbg(adapter, WARN,
+			    "Device already resumed\n");
 		return 0;
 	}
 
@@ -361,16 +363,16 @@ static void mwifiex_delay_for_sleep_cookie(struct mwifiex_adapter *adapter,
 		sleep_cookie = *(u32 *)buffer;
 
 		if (sleep_cookie == MWIFIEX_DEF_SLEEP_COOKIE) {
-			dev_dbg(adapter->dev,
-				"sleep cookie found at count %d\n", count);
+			mwifiex_dbg(adapter, INFO,
+				    "sleep cookie found at count %d\n", count);
 			break;
 		}
 		usleep_range(20, 30);
 	}
 
 	if (count >= max_delay_loop_cnt)
-		dev_dbg(adapter->dev,
-			"max count reached while accessing sleep cookie\n");
+		mwifiex_dbg(adapter, INFO,
+			    "max count reached while accessing sleep cookie\n");
 }
 
 /* This function wakes up the card by reading fw_status register. */
@@ -380,20 +382,23 @@ static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
 	struct pcie_service_card *card = adapter->card;
 	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 
-	dev_dbg(adapter->dev, "event: Wakeup device...\n");
+	mwifiex_dbg(adapter, EVENT,
+		    "event: Wakeup device...\n");
 
 	if (reg->sleep_cookie)
 		mwifiex_pcie_dev_wakeup_delay(adapter);
 
 	/* Reading fw_status register will wakeup device */
 	if (mwifiex_read_reg(adapter, reg->fw_status, &fw_status)) {
-		dev_warn(adapter->dev, "Reading fw_status register failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Reading fw_status register failed\n");
 		return -1;
 	}
 
 	if (reg->sleep_cookie) {
 		mwifiex_pcie_dev_wakeup_delay(adapter);
-		dev_dbg(adapter->dev, "PCIE wakeup: Setting PS_STATE_AWAKE\n");
+		mwifiex_dbg(adapter, INFO,
+			    "PCIE wakeup: Setting PS_STATE_AWAKE\n");
 		adapter->ps_state = PS_STATE_AWAKE;
 	}
 
@@ -407,7 +412,8 @@ static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
  */
 static int mwifiex_pm_wakeup_card_complete(struct mwifiex_adapter *adapter)
 {
-	dev_dbg(adapter->dev, "cmd: Wakeup device completed\n");
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: Wakeup device completed\n");
 
 	return 0;
 }
@@ -423,7 +429,8 @@ static int mwifiex_pcie_disable_host_int(struct mwifiex_adapter *adapter)
 	if (mwifiex_pcie_ok_to_access_hw(adapter)) {
 		if (mwifiex_write_reg(adapter, PCIE_HOST_INT_MASK,
 				      0x00000000)) {
-			dev_warn(adapter->dev, "Disable host interrupt failed\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Disable host interrupt failed\n");
 			return -1;
 		}
 	}
@@ -443,7 +450,8 @@ static int mwifiex_pcie_enable_host_int(struct mwifiex_adapter *adapter)
 		/* Simply write the mask to the register */
 		if (mwifiex_write_reg(adapter, PCIE_HOST_INT_MASK,
 				      HOST_INTR_MASK)) {
-			dev_warn(adapter->dev, "Enable host interrupt failed\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Enable host interrupt failed\n");
 			return -1;
 		}
 	}
@@ -499,8 +507,8 @@ static int mwifiex_init_rxq_ring(struct mwifiex_adapter *adapter)
 		skb = mwifiex_alloc_dma_align_buf(MWIFIEX_RX_DATA_BUF_SIZE,
 						  GFP_KERNEL | GFP_DMA);
 		if (!skb) {
-			dev_err(adapter->dev,
-				"Unable to allocate skb for RX ring.\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Unable to allocate skb for RX ring.\n");
 			kfree(card->rxbd_ring_vbase);
 			return -ENOMEM;
 		}
@@ -512,10 +520,10 @@ static int mwifiex_init_rxq_ring(struct mwifiex_adapter *adapter)
 
 		buf_pa = MWIFIEX_SKB_DMA_ADDR(skb);
 
-		dev_dbg(adapter->dev,
-			"info: RX ring: skb=%p len=%d data=%p buf_pa=%#x:%x\n",
-			skb, skb->len, skb->data, (u32)buf_pa,
-			(u32)((u64)buf_pa >> 32));
+		mwifiex_dbg(adapter, INFO,
+			    "info: RX ring: skb=%p len=%d data=%p buf_pa=%#x:%x\n",
+			    skb, skb->len, skb->data, (u32)buf_pa,
+			    (u32)((u64)buf_pa >> 32));
 
 		card->rx_buf_list[i] = skb;
 		if (reg->pfu_enabled) {
@@ -556,8 +564,8 @@ static int mwifiex_pcie_init_evt_ring(struct mwifiex_adapter *adapter)
 		/* Allocate skb here so that firmware can DMA data from it */
 		skb = dev_alloc_skb(MAX_EVENT_SIZE);
 		if (!skb) {
-			dev_err(adapter->dev,
-				"Unable to allocate skb for EVENT buf.\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Unable to allocate skb for EVENT buf.\n");
 			kfree(card->evtbd_ring_vbase);
 			return -ENOMEM;
 		}
@@ -569,10 +577,10 @@ static int mwifiex_pcie_init_evt_ring(struct mwifiex_adapter *adapter)
 
 		buf_pa = MWIFIEX_SKB_DMA_ADDR(skb);
 
-		dev_dbg(adapter->dev,
-			"info: EVT ring: skb=%p len=%d data=%p buf_pa=%#x:%x\n",
-			skb, skb->len, skb->data, (u32)buf_pa,
-			(u32)((u64)buf_pa >> 32));
+		mwifiex_dbg(adapter, EVENT,
+			    "info: EVT ring: skb=%p len=%d data=%p buf_pa=%#x:%x\n",
+			    skb, skb->len, skb->data, (u32)buf_pa,
+			    (u32)((u64)buf_pa >> 32));
 
 		card->evt_buf_list[i] = skb;
 		card->evtbd_ring[i] = (void *)(card->evtbd_ring_vbase +
@@ -715,21 +723,23 @@ static int mwifiex_pcie_create_txbd_ring(struct mwifiex_adapter *adapter)
 		card->txbd_ring_size = sizeof(struct mwifiex_pcie_buf_desc) *
 				       MWIFIEX_MAX_TXRX_BD;
 
-	dev_dbg(adapter->dev, "info: txbd_ring: Allocating %d bytes\n",
-		card->txbd_ring_size);
+	mwifiex_dbg(adapter, INFO,
+		    "info: txbd_ring: Allocating %d bytes\n",
+		    card->txbd_ring_size);
 	card->txbd_ring_vbase = pci_alloc_consistent(card->dev,
 						     card->txbd_ring_size,
 						     &card->txbd_ring_pbase);
 	if (!card->txbd_ring_vbase) {
-		dev_err(adapter->dev,
-			"allocate consistent memory (%d bytes) failed!\n",
-			card->txbd_ring_size);
+		mwifiex_dbg(adapter, ERROR,
+			    "allocate consistent memory (%d bytes) failed!\n",
+			    card->txbd_ring_size);
 		return -ENOMEM;
 	}
-	dev_dbg(adapter->dev,
-		"info: txbd_ring - base: %p, pbase: %#x:%x, len: %x\n",
-		card->txbd_ring_vbase, (unsigned int)card->txbd_ring_pbase,
-		(u32)((u64)card->txbd_ring_pbase >> 32), card->txbd_ring_size);
+	mwifiex_dbg(adapter, DATA,
+		    "info: txbd_ring - base: %p, pbase: %#x:%x, len: %x\n",
+		    card->txbd_ring_vbase, (unsigned int)card->txbd_ring_pbase,
+		    (u32)((u64)card->txbd_ring_pbase >> 32),
+		    card->txbd_ring_size);
 
 	return mwifiex_init_txq_ring(adapter);
 }
@@ -777,23 +787,24 @@ static int mwifiex_pcie_create_rxbd_ring(struct mwifiex_adapter *adapter)
 		card->rxbd_ring_size = sizeof(struct mwifiex_pcie_buf_desc) *
 				       MWIFIEX_MAX_TXRX_BD;
 
-	dev_dbg(adapter->dev, "info: rxbd_ring: Allocating %d bytes\n",
-		card->rxbd_ring_size);
+	mwifiex_dbg(adapter, INFO,
+		    "info: rxbd_ring: Allocating %d bytes\n",
+		    card->rxbd_ring_size);
 	card->rxbd_ring_vbase = pci_alloc_consistent(card->dev,
 						     card->rxbd_ring_size,
 						     &card->rxbd_ring_pbase);
 	if (!card->rxbd_ring_vbase) {
-		dev_err(adapter->dev,
-			"allocate consistent memory (%d bytes) failed!\n",
-			card->rxbd_ring_size);
+		mwifiex_dbg(adapter, ERROR,
+			    "allocate consistent memory (%d bytes) failed!\n",
+			    card->rxbd_ring_size);
 		return -ENOMEM;
 	}
 
-	dev_dbg(adapter->dev,
-		"info: rxbd_ring - base: %p, pbase: %#x:%x, len: %#x\n",
-		card->rxbd_ring_vbase, (u32)card->rxbd_ring_pbase,
-		(u32)((u64)card->rxbd_ring_pbase >> 32),
-		card->rxbd_ring_size);
+	mwifiex_dbg(adapter, DATA,
+		    "info: rxbd_ring - base: %p, pbase: %#x:%x, len: %#x\n",
+		    card->rxbd_ring_vbase, (u32)card->rxbd_ring_pbase,
+		    (u32)((u64)card->rxbd_ring_pbase >> 32),
+		    card->rxbd_ring_size);
 
 	return mwifiex_init_rxq_ring(adapter);
 }
@@ -840,23 +851,24 @@ static int mwifiex_pcie_create_evtbd_ring(struct mwifiex_adapter *adapter)
 	card->evtbd_ring_size = sizeof(struct mwifiex_evt_buf_desc) *
 				MWIFIEX_MAX_EVT_BD;
 
-	dev_dbg(adapter->dev, "info: evtbd_ring: Allocating %d bytes\n",
+	mwifiex_dbg(adapter, INFO,
+		    "info: evtbd_ring: Allocating %d bytes\n",
 		card->evtbd_ring_size);
 	card->evtbd_ring_vbase = pci_alloc_consistent(card->dev,
 						      card->evtbd_ring_size,
 						      &card->evtbd_ring_pbase);
 	if (!card->evtbd_ring_vbase) {
-		dev_err(adapter->dev,
-			"allocate consistent memory (%d bytes) failed!\n",
-			card->evtbd_ring_size);
+		mwifiex_dbg(adapter, ERROR,
+			    "allocate consistent memory (%d bytes) failed!\n",
+			    card->evtbd_ring_size);
 		return -ENOMEM;
 	}
 
-	dev_dbg(adapter->dev,
-		"info: CMDRSP/EVT bd_ring - base: %p pbase: %#x:%x len: %#x\n",
-		card->evtbd_ring_vbase, (u32)card->evtbd_ring_pbase,
-		(u32)((u64)card->evtbd_ring_pbase >> 32),
-		card->evtbd_ring_size);
+	mwifiex_dbg(adapter, EVENT,
+		    "info: CMDRSP/EVT bd_ring - base: %p pbase: %#x:%x len: %#x\n",
+		    card->evtbd_ring_vbase, (u32)card->evtbd_ring_pbase,
+		    (u32)((u64)card->evtbd_ring_pbase >> 32),
+		    card->evtbd_ring_size);
 
 	return mwifiex_pcie_init_evt_ring(adapter);
 }
@@ -895,8 +907,8 @@ static int mwifiex_pcie_alloc_cmdrsp_buf(struct mwifiex_adapter *adapter)
 	/* Allocate memory for receiving command response data */
 	skb = dev_alloc_skb(MWIFIEX_UPLD_SIZE);
 	if (!skb) {
-		dev_err(adapter->dev,
-			"Unable to allocate skb for command response data.\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Unable to allocate skb for command response data.\n");
 		return -ENOMEM;
 	}
 	skb_put(skb, MWIFIEX_UPLD_SIZE);
@@ -944,14 +956,16 @@ static int mwifiex_pcie_alloc_sleep_cookie_buf(struct mwifiex_adapter *adapter)
 	card->sleep_cookie_vbase = pci_alloc_consistent(card->dev, sizeof(u32),
 						     &card->sleep_cookie_pbase);
 	if (!card->sleep_cookie_vbase) {
-		dev_err(adapter->dev, "pci_alloc_consistent failed!\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "pci_alloc_consistent failed!\n");
 		return -ENOMEM;
 	}
 	/* Init val of Sleep Cookie */
 	*(u32 *)card->sleep_cookie_vbase = FW_AWAKE_COOKIE;
 
-	dev_dbg(adapter->dev, "alloc_scook: sleep cookie=0x%x\n",
-		*((u32 *)card->sleep_cookie_vbase));
+	mwifiex_dbg(adapter, INFO,
+		    "alloc_scook: sleep cookie=0x%x\n",
+		    *((u32 *)card->sleep_cookie_vbase));
 
 	return 0;
 }
@@ -993,8 +1007,8 @@ static int mwifiex_clean_pcie_ring_buf(struct mwifiex_adapter *adapter)
 		 */
 		if (mwifiex_write_reg(adapter, PCIE_CPU_INT_EVENT,
 				      CPU_INTR_DNLD_RDY)) {
-			dev_err(adapter->dev,
-				"failed to assert dnld-rdy interrupt.\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "failed to assert dnld-rdy interrupt.\n");
 			return -1;
 		}
 	}
@@ -1018,13 +1032,14 @@ static int mwifiex_pcie_send_data_complete(struct mwifiex_adapter *adapter)
 
 	/* Read the TX ring read pointer set by firmware */
 	if (mwifiex_read_reg(adapter, reg->tx_rdptr, &rdptr)) {
-		dev_err(adapter->dev,
-			"SEND COMP: failed to read reg->tx_rdptr\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "SEND COMP: failed to read reg->tx_rdptr\n");
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "SEND COMP: rdptr_prev=0x%x, rdptr=0x%x\n",
-		card->txbd_rdptr, rdptr);
+	mwifiex_dbg(adapter, DATA,
+		    "SEND COMP: rdptr_prev=0x%x, rdptr=0x%x\n",
+		    card->txbd_rdptr, rdptr);
 
 	num_tx_buffs = MWIFIEX_MAX_TXRX_BD << reg->tx_start_ptr;
 	/* free from previous txbd_rdptr to current txbd_rdptr */
@@ -1038,9 +1053,9 @@ static int mwifiex_pcie_send_data_complete(struct mwifiex_adapter *adapter)
 		skb = card->tx_buf_list[wrdoneidx];
 
 		if (skb) {
-			dev_dbg(adapter->dev,
-				"SEND COMP: Detach skb %p at txbd_rdidx=%d\n",
-				skb, wrdoneidx);
+			mwifiex_dbg(adapter, DATA,
+				    "SEND COMP: Detach skb %p at txbd_rdidx=%d\n",
+				    skb, wrdoneidx);
 			mwifiex_unmap_pci_memory(adapter, skb,
 						 PCI_DMA_TODEVICE);
 
@@ -1112,8 +1127,9 @@ mwifiex_pcie_send_data(struct mwifiex_adapter *adapter, struct sk_buff *skb,
 	__le16 *tmp;
 
 	if (!(skb->data && skb->len)) {
-		dev_err(adapter->dev, "%s(): invalid parameter <%p, %#x>\n",
-			__func__, skb->data, skb->len);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s(): invalid parameter <%p, %#x>\n",
+			    __func__, skb->data, skb->len);
 		return -1;
 	}
 
@@ -1121,7 +1137,8 @@ mwifiex_pcie_send_data(struct mwifiex_adapter *adapter, struct sk_buff *skb,
 		mwifiex_pm_wakeup_card(adapter);
 
 	num_tx_buffs = MWIFIEX_MAX_TXRX_BD << reg->tx_start_ptr;
-	dev_dbg(adapter->dev, "info: SEND DATA: <Rd: %#x, Wr: %#x>\n",
+	mwifiex_dbg(adapter, DATA,
+		    "info: SEND DATA: <Rd: %#x, Wr: %#x>\n",
 		card->txbd_rdptr, card->txbd_wrptr);
 	if (mwifiex_pcie_txbd_not_full(card)) {
 		u8 *payload;
@@ -1175,39 +1192,40 @@ mwifiex_pcie_send_data(struct mwifiex_adapter *adapter, struct sk_buff *skb,
 		/* Write the TX ring write pointer in to reg->tx_wrptr */
 		if (mwifiex_write_reg(adapter, reg->tx_wrptr,
 				      card->txbd_wrptr | rx_val)) {
-			dev_err(adapter->dev,
-				"SEND DATA: failed to write reg->tx_wrptr\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "SEND DATA: failed to write reg->tx_wrptr\n");
 			ret = -1;
 			goto done_unmap;
 		}
 		if ((mwifiex_pcie_txbd_not_full(card)) &&
 		    tx_param->next_pkt_len) {
 			/* have more packets and TxBD still can hold more */
-			dev_dbg(adapter->dev,
-				"SEND DATA: delay dnld-rdy interrupt.\n");
+			mwifiex_dbg(adapter, DATA,
+				    "SEND DATA: delay dnld-rdy interrupt.\n");
 			adapter->data_sent = false;
 		} else {
 			/* Send the TX ready interrupt */
 			if (mwifiex_write_reg(adapter, PCIE_CPU_INT_EVENT,
 					      CPU_INTR_DNLD_RDY)) {
-				dev_err(adapter->dev,
-					"SEND DATA: failed to assert dnld-rdy interrupt.\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "SEND DATA: failed to assert dnld-rdy interrupt.\n");
 				ret = -1;
 				goto done_unmap;
 			}
 		}
-		dev_dbg(adapter->dev, "info: SEND DATA: Updated <Rd: %#x, Wr: "
-			"%#x> and sent packet to firmware successfully\n",
-			card->txbd_rdptr, card->txbd_wrptr);
+		mwifiex_dbg(adapter, DATA,
+			    "info: SEND DATA: Updated <Rd: %#x, Wr:\t"
+			    "%#x> and sent packet to firmware successfully\n",
+			    card->txbd_rdptr, card->txbd_wrptr);
 	} else {
-		dev_dbg(adapter->dev,
-			"info: TX Ring full, can't send packets to fw\n");
+		mwifiex_dbg(adapter, DATA,
+			    "info: TX Ring full, can't send packets to fw\n");
 		adapter->data_sent = true;
 		/* Send the TX ready interrupt */
 		if (mwifiex_write_reg(adapter, PCIE_CPU_INT_EVENT,
 				      CPU_INTR_DNLD_RDY))
-			dev_err(adapter->dev,
-				"SEND DATA: failed to assert door-bell intr\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "SEND DATA: failed to assert door-bell intr\n");
 		return -EBUSY;
 	}
 
@@ -1243,8 +1261,8 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 
 	/* Read the RX ring Write pointer set by firmware */
 	if (mwifiex_read_reg(adapter, reg->rx_wrptr, &wrptr)) {
-		dev_err(adapter->dev,
-			"RECV DATA: failed to read reg->rx_wrptr\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "RECV DATA: failed to read reg->rx_wrptr\n");
 		ret = -1;
 		goto done;
 	}
@@ -1277,15 +1295,15 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 		rx_len = le16_to_cpu(pkt_len);
 		if (WARN_ON(rx_len <= INTF_HEADER_LEN ||
 			    rx_len > MWIFIEX_RX_DATA_BUF_SIZE)) {
-			dev_err(adapter->dev,
-				"Invalid RX len %d, Rd=%#x, Wr=%#x\n",
-				rx_len, card->rxbd_rdptr, wrptr);
+			mwifiex_dbg(adapter, ERROR,
+				    "Invalid RX len %d, Rd=%#x, Wr=%#x\n",
+				    rx_len, card->rxbd_rdptr, wrptr);
 			dev_kfree_skb_any(skb_data);
 		} else {
 			skb_put(skb_data, rx_len);
-			dev_dbg(adapter->dev,
-				"info: RECV DATA: Rd=%#x, Wr=%#x, Len=%d\n",
-				card->rxbd_rdptr, wrptr, rx_len);
+			mwifiex_dbg(adapter, DATA,
+				    "info: RECV DATA: Rd=%#x, Wr=%#x, Len=%d\n",
+				    card->rxbd_rdptr, wrptr, rx_len);
 			skb_pull(skb_data, INTF_HEADER_LEN);
 			if (adapter->rx_work_enabled) {
 				skb_queue_tail(&adapter->rx_data_q, skb_data);
@@ -1299,8 +1317,8 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 		skb_tmp = mwifiex_alloc_dma_align_buf(MWIFIEX_RX_DATA_BUF_SIZE,
 						      GFP_KERNEL | GFP_DMA);
 		if (!skb_tmp) {
-			dev_err(adapter->dev,
-				"Unable to allocate skb.\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Unable to allocate skb.\n");
 			return -ENOMEM;
 		}
 
@@ -1311,9 +1329,9 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 
 		buf_pa = MWIFIEX_SKB_DMA_ADDR(skb_tmp);
 
-		dev_dbg(adapter->dev,
-			"RECV DATA: Attach new sk_buff %p at rxbd_rdidx=%d\n",
-			skb_tmp, rd_index);
+		mwifiex_dbg(adapter, INFO,
+			    "RECV DATA: Attach new sk_buff %p at rxbd_rdidx=%d\n",
+			    skb_tmp, rd_index);
 		card->rx_buf_list[rd_index] = skb_tmp;
 
 		if (reg->pfu_enabled) {
@@ -1336,28 +1354,29 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 					     reg->rx_rollover_ind) ^
 					     reg->rx_rollover_ind);
 		}
-		dev_dbg(adapter->dev, "info: RECV DATA: <Rd: %#x, Wr: %#x>\n",
-			card->rxbd_rdptr, wrptr);
+		mwifiex_dbg(adapter, DATA,
+			    "info: RECV DATA: <Rd: %#x, Wr: %#x>\n",
+			    card->rxbd_rdptr, wrptr);
 
 		tx_val = card->txbd_wrptr & reg->tx_wrap_mask;
 		/* Write the RX ring read pointer in to reg->rx_rdptr */
 		if (mwifiex_write_reg(adapter, reg->rx_rdptr,
 				      card->rxbd_rdptr | tx_val)) {
-			dev_err(adapter->dev,
-				"RECV DATA: failed to write reg->rx_rdptr\n");
+			mwifiex_dbg(adapter, DATA,
+				    "RECV DATA: failed to write reg->rx_rdptr\n");
 			ret = -1;
 			goto done;
 		}
 
 		/* Read the RX ring Write pointer set by firmware */
 		if (mwifiex_read_reg(adapter, reg->rx_wrptr, &wrptr)) {
-			dev_err(adapter->dev,
-				"RECV DATA: failed to read reg->rx_wrptr\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "RECV DATA: failed to read reg->rx_wrptr\n");
 			ret = -1;
 			goto done;
 		}
-		dev_dbg(adapter->dev,
-			"info: RECV DATA: Rcvd packet from fw successfully\n");
+		mwifiex_dbg(adapter, DATA,
+			    "info: RECV DATA: Rcvd packet from fw successfully\n");
 		card->rxbd_wrptr = wrptr;
 	}
 
@@ -1376,9 +1395,9 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 
 	if (!(skb->data && skb->len)) {
-		dev_err(adapter->dev,
-			"Invalid parameter in %s <%p. len %d>\n",
-			__func__, skb->data, skb->len);
+		mwifiex_dbg(adapter, ERROR,
+			    "Invalid parameter in %s <%p. len %d>\n",
+			    __func__, skb->data, skb->len);
 		return -1;
 	}
 
@@ -1391,9 +1410,9 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	 * address scratch register
 	 */
 	if (mwifiex_write_reg(adapter, reg->cmd_addr_lo, (u32)buf_pa)) {
-		dev_err(adapter->dev,
-			"%s: failed to write download command to boot code.\n",
-			__func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: failed to write download command to boot code.\n",
+			    __func__);
 		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 		return -1;
 	}
@@ -1403,18 +1422,18 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	 */
 	if (mwifiex_write_reg(adapter, reg->cmd_addr_hi,
 			      (u32)((u64)buf_pa >> 32))) {
-		dev_err(adapter->dev,
-			"%s: failed to write download command to boot code.\n",
-			__func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: failed to write download command to boot code.\n",
+			    __func__);
 		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 		return -1;
 	}
 
 	/* Write the command length to cmd_size scratch register */
 	if (mwifiex_write_reg(adapter, reg->cmd_size, skb->len)) {
-		dev_err(adapter->dev,
-			"%s: failed to write command len to cmd_size scratch reg\n",
-			__func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: failed to write command len to cmd_size scratch reg\n",
+			    __func__);
 		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 		return -1;
 	}
@@ -1422,8 +1441,8 @@ mwifiex_pcie_send_boot_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	/* Ring the door bell */
 	if (mwifiex_write_reg(adapter, PCIE_CPU_INT_EVENT,
 			      CPU_INTR_DOOR_BELL)) {
-		dev_err(adapter->dev,
-			"%s: failed to assert door-bell intr\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: failed to assert door-bell intr\n", __func__);
 		mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_TODEVICE);
 		return -1;
 	}
@@ -1443,8 +1462,8 @@ static int mwifiex_pcie_init_fw_port(struct mwifiex_adapter *adapter)
 	/* Write the RX ring read pointer in to reg->rx_rdptr */
 	if (mwifiex_write_reg(adapter, reg->rx_rdptr, card->rxbd_rdptr |
 			      tx_wrap)) {
-		dev_err(adapter->dev,
-			"RECV DATA: failed to write reg->rx_rdptr\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "RECV DATA: failed to write reg->rx_rdptr\n");
 		return -1;
 	}
 	return 0;
@@ -1462,15 +1481,16 @@ mwifiex_pcie_send_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	u8 *payload = (u8 *)skb->data;
 
 	if (!(skb->data && skb->len)) {
-		dev_err(adapter->dev, "Invalid parameter in %s <%p, %#x>\n",
-			__func__, skb->data, skb->len);
+		mwifiex_dbg(adapter, ERROR,
+			    "Invalid parameter in %s <%p, %#x>\n",
+			    __func__, skb->data, skb->len);
 		return -1;
 	}
 
 	/* Make sure a command response buffer is available */
 	if (!card->cmdrsp_buf) {
-		dev_err(adapter->dev,
-			"No response buffer available, send command failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "No response buffer available, send command failed\n");
 		return -EBUSY;
 	}
 
@@ -1503,8 +1523,8 @@ mwifiex_pcie_send_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 		   address */
 		if (mwifiex_write_reg(adapter, reg->cmdrsp_addr_lo,
 				      (u32)cmdrsp_buf_pa)) {
-			dev_err(adapter->dev,
-				"Failed to write download cmd to boot code.\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Failed to write download cmd to boot code.\n");
 			ret = -1;
 			goto done;
 		}
@@ -1512,8 +1532,8 @@ mwifiex_pcie_send_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 		   address */
 		if (mwifiex_write_reg(adapter, reg->cmdrsp_addr_hi,
 				      (u32)((u64)cmdrsp_buf_pa >> 32))) {
-			dev_err(adapter->dev,
-				"Failed to write download cmd to boot code.\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Failed to write download cmd to boot code.\n");
 			ret = -1;
 			goto done;
 		}
@@ -1523,16 +1543,16 @@ mwifiex_pcie_send_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	/* Write the lower 32bits of the physical address to reg->cmd_addr_lo */
 	if (mwifiex_write_reg(adapter, reg->cmd_addr_lo,
 			      (u32)cmd_buf_pa)) {
-		dev_err(adapter->dev,
-			"Failed to write download cmd to boot code.\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Failed to write download cmd to boot code.\n");
 		ret = -1;
 		goto done;
 	}
 	/* Write the upper 32bits of the physical address to reg->cmd_addr_hi */
 	if (mwifiex_write_reg(adapter, reg->cmd_addr_hi,
 			      (u32)((u64)cmd_buf_pa >> 32))) {
-		dev_err(adapter->dev,
-			"Failed to write download cmd to boot code.\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Failed to write download cmd to boot code.\n");
 		ret = -1;
 		goto done;
 	}
@@ -1540,8 +1560,8 @@ mwifiex_pcie_send_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	/* Write the command length to reg->cmd_size */
 	if (mwifiex_write_reg(adapter, reg->cmd_size,
 			      card->cmd_buf->len)) {
-		dev_err(adapter->dev,
-			"Failed to write cmd len to reg->cmd_size\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Failed to write cmd len to reg->cmd_size\n");
 		ret = -1;
 		goto done;
 	}
@@ -1549,8 +1569,8 @@ mwifiex_pcie_send_cmd(struct mwifiex_adapter *adapter, struct sk_buff *skb)
 	/* Ring the door bell */
 	if (mwifiex_write_reg(adapter, PCIE_CPU_INT_EVENT,
 			      CPU_INTR_DOOR_BELL)) {
-		dev_err(adapter->dev,
-			"Failed to assert door-bell intr\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Failed to assert door-bell intr\n");
 		ret = -1;
 		goto done;
 	}
@@ -1574,7 +1594,8 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 	u16 rx_len;
 	__le16 pkt_len;
 
-	dev_dbg(adapter->dev, "info: Rx CMD Response\n");
+	mwifiex_dbg(adapter, CMD,
+		    "info: Rx CMD Response\n");
 
 	mwifiex_unmap_pci_memory(adapter, skb, PCI_DMA_FROMDEVICE);
 
@@ -1598,8 +1619,8 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 			if (mwifiex_write_reg(adapter,
 					      PCIE_CPU_INT_EVENT,
 					      CPU_INTR_SLEEP_CFM_DONE)) {
-				dev_warn(adapter->dev,
-					 "Write register failed\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "Write register failed\n");
 				return -1;
 			}
 			mwifiex_delay_for_sleep_cookie(adapter,
@@ -1608,8 +1629,8 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 			       mwifiex_pcie_ok_to_access_hw(adapter))
 				usleep_range(50, 60);
 		} else {
-			dev_err(adapter->dev,
-				"There is no command but got cmdrsp\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "There is no command but got cmdrsp\n");
 		}
 		memcpy(adapter->upld_buf, skb->data,
 		       min_t(u32, MWIFIEX_SIZE_OF_CMD_BUFFER, skb->len));
@@ -1628,15 +1649,15 @@ static int mwifiex_pcie_process_cmd_complete(struct mwifiex_adapter *adapter)
 		   will prevent firmware from writing to the same response
 		   buffer again. */
 		if (mwifiex_write_reg(adapter, reg->cmdrsp_addr_lo, 0)) {
-			dev_err(adapter->dev,
-				"cmd_done: failed to clear cmd_rsp_addr_lo\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "cmd_done: failed to clear cmd_rsp_addr_lo\n");
 			return -1;
 		}
 		/* Write the upper 32bits of the cmdrsp buffer physical
 		   address */
 		if (mwifiex_write_reg(adapter, reg->cmdrsp_addr_hi, 0)) {
-			dev_err(adapter->dev,
-				"cmd_done: failed to clear cmd_rsp_addr_hi\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "cmd_done: failed to clear cmd_rsp_addr_hi\n");
 			return -1;
 		}
 	}
@@ -1678,25 +1699,28 @@ static int mwifiex_pcie_process_event_ready(struct mwifiex_adapter *adapter)
 		mwifiex_pm_wakeup_card(adapter);
 
 	if (adapter->event_received) {
-		dev_dbg(adapter->dev, "info: Event being processed, "
-			"do not process this interrupt just yet\n");
+		mwifiex_dbg(adapter, EVENT,
+			    "info: Event being processed,\t"
+			    "do not process this interrupt just yet\n");
 		return 0;
 	}
 
 	if (rdptr >= MWIFIEX_MAX_EVT_BD) {
-		dev_dbg(adapter->dev, "info: Invalid read pointer...\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "info: Invalid read pointer...\n");
 		return -1;
 	}
 
 	/* Read the event ring write pointer set by firmware */
 	if (mwifiex_read_reg(adapter, reg->evt_wrptr, &wrptr)) {
-		dev_err(adapter->dev,
-			"EventReady: failed to read reg->evt_wrptr\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "EventReady: failed to read reg->evt_wrptr\n");
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "info: EventReady: Initial <Rd: 0x%x, Wr: 0x%x>",
-		card->evtbd_rdptr, wrptr);
+	mwifiex_dbg(adapter, EVENT,
+		    "info: EventReady: Initial <Rd: 0x%x, Wr: 0x%x>",
+		    card->evtbd_rdptr, wrptr);
 	if (((wrptr & MWIFIEX_EVTBD_MASK) != (card->evtbd_rdptr
 					      & MWIFIEX_EVTBD_MASK)) ||
 	    ((wrptr & reg->evt_rollover_ind) ==
@@ -1705,7 +1729,8 @@ static int mwifiex_pcie_process_event_ready(struct mwifiex_adapter *adapter)
 		__le16 data_len = 0;
 		u16 evt_len;
 
-		dev_dbg(adapter->dev, "info: Read Index: %d\n", rdptr);
+		mwifiex_dbg(adapter, INFO,
+			    "info: Read Index: %d\n", rdptr);
 		skb_cmd = card->evt_buf_list[rdptr];
 		mwifiex_unmap_pci_memory(adapter, skb_cmd, PCI_DMA_FROMDEVICE);
 
@@ -1721,9 +1746,10 @@ static int mwifiex_pcie_process_event_ready(struct mwifiex_adapter *adapter)
 		   len is 2 bytes followed by type which is 2 bytes */
 		memcpy(&data_len, skb_cmd->data, sizeof(__le16));
 		evt_len = le16_to_cpu(data_len);
-
+		skb_trim(skb_cmd, evt_len);
 		skb_pull(skb_cmd, INTF_HEADER_LEN);
-		dev_dbg(adapter->dev, "info: Event length: %d\n", evt_len);
+		mwifiex_dbg(adapter, EVENT,
+			    "info: Event length: %d\n", evt_len);
 
 		if ((evt_len > 0) && (evt_len  < MAX_EVENT_SIZE))
 			memcpy(adapter->event_body, skb_cmd->data +
@@ -1740,8 +1766,8 @@ static int mwifiex_pcie_process_event_ready(struct mwifiex_adapter *adapter)
 	} else {
 		if (mwifiex_write_reg(adapter, PCIE_CPU_INT_EVENT,
 				      CPU_INTR_EVENT_DONE)) {
-			dev_warn(adapter->dev,
-				 "Write register failed\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Write register failed\n");
 			return -1;
 		}
 	}
@@ -1766,15 +1792,16 @@ static int mwifiex_pcie_event_complete(struct mwifiex_adapter *adapter,
 		return 0;
 
 	if (rdptr >= MWIFIEX_MAX_EVT_BD) {
-		dev_err(adapter->dev, "event_complete: Invalid rdptr 0x%x\n",
-			rdptr);
+		mwifiex_dbg(adapter, ERROR,
+			    "event_complete: Invalid rdptr 0x%x\n",
+			    rdptr);
 		return -EINVAL;
 	}
 
 	/* Read the event ring write pointer set by firmware */
 	if (mwifiex_read_reg(adapter, reg->evt_wrptr, &wrptr)) {
-		dev_err(adapter->dev,
-			"event_complete: failed to read reg->evt_wrptr\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "event_complete: failed to read reg->evt_wrptr\n");
 		return -1;
 	}
 
@@ -1791,9 +1818,9 @@ static int mwifiex_pcie_event_complete(struct mwifiex_adapter *adapter,
 		desc->flags = 0;
 		skb = NULL;
 	} else {
-		dev_dbg(adapter->dev,
-			"info: ERROR: buf still valid at index %d, <%p, %p>\n",
-			rdptr, card->evt_buf_list[rdptr], skb);
+		mwifiex_dbg(adapter, ERROR,
+			    "info: ERROR: buf still valid at index %d, <%p, %p>\n",
+			    rdptr, card->evt_buf_list[rdptr], skb);
 	}
 
 	if ((++card->evtbd_rdptr & MWIFIEX_EVTBD_MASK) == MWIFIEX_MAX_EVT_BD) {
@@ -1802,18 +1829,20 @@ static int mwifiex_pcie_event_complete(struct mwifiex_adapter *adapter,
 					reg->evt_rollover_ind);
 	}
 
-	dev_dbg(adapter->dev, "info: Updated <Rd: 0x%x, Wr: 0x%x>",
-		card->evtbd_rdptr, wrptr);
+	mwifiex_dbg(adapter, EVENT,
+		    "info: Updated <Rd: 0x%x, Wr: 0x%x>",
+		    card->evtbd_rdptr, wrptr);
 
 	/* Write the event ring read pointer in to reg->evt_rdptr */
 	if (mwifiex_write_reg(adapter, reg->evt_rdptr,
 			      card->evtbd_rdptr)) {
-		dev_err(adapter->dev,
-			"event_complete: failed to read reg->evt_rdptr\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "event_complete: failed to read reg->evt_rdptr\n");
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "info: Check Events Again\n");
+	mwifiex_dbg(adapter, EVENT,
+		    "info: Check Events Again\n");
 	ret = mwifiex_pcie_process_event_ready(adapter);
 
 	return ret;
@@ -1840,17 +1869,18 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 
 	if (!firmware || !firmware_len) {
-		dev_err(adapter->dev,
-			"No firmware image found! Terminating download\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "No firmware image found! Terminating download\n");
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "info: Downloading FW image (%d bytes)\n",
-		firmware_len);
+	mwifiex_dbg(adapter, INFO,
+		    "info: Downloading FW image (%d bytes)\n",
+		    firmware_len);
 
 	if (mwifiex_pcie_disable_host_int(adapter)) {
-		dev_err(adapter->dev,
-			"%s: Disabling interrupts failed.\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: Disabling interrupts failed.\n", __func__);
 		return -1;
 	}
 
@@ -1872,8 +1902,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 			ret = mwifiex_read_reg(adapter, reg->cmd_size,
 					       &len);
 			if (ret) {
-				dev_warn(adapter->dev,
-					 "Failed reading len from boot code\n");
+				mwifiex_dbg(adapter, FATAL,
+					    "Failed reading len from boot code\n");
 				goto done;
 			}
 			if (len)
@@ -1884,8 +1914,9 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		if (!len) {
 			break;
 		} else if (len > MWIFIEX_UPLD_SIZE) {
-			pr_err("FW download failure @ %d, invalid length %d\n",
-			       offset, len);
+			mwifiex_dbg(adapter, ERROR,
+				    "FW download failure @ %d, invalid length %d\n",
+				    offset, len);
 			ret = -1;
 			goto done;
 		}
@@ -1895,14 +1926,16 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		if (len & BIT(0)) {
 			block_retry_cnt++;
 			if (block_retry_cnt > MAX_WRITE_IOMEM_RETRY) {
-				pr_err("FW download failure @ %d, over max "
-				       "retry count\n", offset);
+				mwifiex_dbg(adapter, ERROR,
+					    "FW download failure @ %d, over max\t"
+					    "retry count\n", offset);
 				ret = -1;
 				goto done;
 			}
-			dev_err(adapter->dev, "FW CRC error indicated by the "
-				"helper: len = 0x%04X, txlen = %d\n",
-				len, txlen);
+			mwifiex_dbg(adapter, ERROR,
+				    "FW CRC error indicated by the\t"
+				    "helper: len = 0x%04X, txlen = %d\n",
+				    len, txlen);
 			len &= ~BIT(0);
 			/* Setting this to 0 to resend from same offset */
 			txlen = 0;
@@ -1913,7 +1946,7 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 			if (firmware_len - offset < txlen)
 				txlen = firmware_len - offset;
 
-			dev_dbg(adapter->dev, ".");
+			mwifiex_dbg(adapter, INFO, ".");
 
 			tx_blocks = (txlen + card->pcie.blksz_fw_dl - 1) /
 				    card->pcie.blksz_fw_dl;
@@ -1927,8 +1960,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 
 		/* Send the boot command to device */
 		if (mwifiex_pcie_send_boot_cmd(adapter, skb)) {
-			dev_err(adapter->dev,
-				"Failed to send firmware download command\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Failed to send firmware download command\n");
 			ret = -1;
 			goto done;
 		}
@@ -1937,9 +1970,10 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		do {
 			if (mwifiex_read_reg(adapter, PCIE_CPU_INT_STATUS,
 					     &ireg_intr)) {
-				dev_err(adapter->dev, "%s: Failed to read "
-					"interrupt status during fw dnld.\n",
-					__func__);
+				mwifiex_dbg(adapter, ERROR,
+					    "%s: Failed to read\t"
+					    "interrupt status during fw dnld.\n",
+					    __func__);
 				mwifiex_unmap_pci_memory(adapter, skb,
 							 PCI_DMA_TODEVICE);
 				ret = -1;
@@ -1953,8 +1987,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		offset += txlen;
 	} while (true);
 
-	dev_notice(adapter->dev,
-		   "info: FW download over, size %d bytes\n", offset);
+	mwifiex_dbg(adapter, MSG,
+		    "info: FW download over, size %d bytes\n", offset);
 
 	ret = 0;
 
@@ -1980,15 +2014,17 @@ mwifiex_check_fw_status(struct mwifiex_adapter *adapter, u32 poll_num)
 	/* Mask spurios interrupts */
 	if (mwifiex_write_reg(adapter, PCIE_HOST_INT_STATUS_MASK,
 			      HOST_INTR_MASK)) {
-		dev_warn(adapter->dev, "Write register failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Write register failed\n");
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "Setting driver ready signature\n");
+	mwifiex_dbg(adapter, INFO,
+		    "Setting driver ready signature\n");
 	if (mwifiex_write_reg(adapter, reg->drv_rdy,
 			      FIRMWARE_READY_PCIE)) {
-		dev_err(adapter->dev,
-			"Failed to write driver ready signature\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Failed to write driver ready signature\n");
 		return -1;
 	}
 
@@ -2015,12 +2051,13 @@ mwifiex_check_fw_status(struct mwifiex_adapter *adapter, u32 poll_num)
 				     &winner_status))
 			ret = -1;
 		else if (!winner_status) {
-			dev_err(adapter->dev, "PCI-E is the winner\n");
+			mwifiex_dbg(adapter, INFO,
+				    "PCI-E is the winner\n");
 			adapter->winner = 1;
 		} else {
-			dev_err(adapter->dev,
-				"PCI-E is not the winner <%#x,%d>, exit dnld\n",
-				ret, adapter->winner);
+			mwifiex_dbg(adapter, ERROR,
+				    "PCI-E is not the winner <%#x,%d>, exit dnld\n",
+				    ret, adapter->winner);
 		}
 	}
 
@@ -2039,7 +2076,7 @@ static void mwifiex_interrupt_status(struct mwifiex_adapter *adapter)
 		return;
 
 	if (mwifiex_read_reg(adapter, PCIE_HOST_INT_STATUS, &pcie_ireg)) {
-		dev_warn(adapter->dev, "Read register failed\n");
+		mwifiex_dbg(adapter, ERROR, "Read register failed\n");
 		return;
 	}
 
@@ -2050,7 +2087,8 @@ static void mwifiex_interrupt_status(struct mwifiex_adapter *adapter)
 		/* Clear the pending interrupts */
 		if (mwifiex_write_reg(adapter, PCIE_HOST_INT_STATUS,
 				      ~pcie_ireg)) {
-			dev_warn(adapter->dev, "Write register failed\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Write register failed\n");
 			return;
 		}
 		spin_lock_irqsave(&adapter->int_lock, flags);
@@ -2133,21 +2171,24 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 	while (pcie_ireg & HOST_INTR_MASK) {
 		if (pcie_ireg & HOST_INTR_DNLD_DONE) {
 			pcie_ireg &= ~HOST_INTR_DNLD_DONE;
-			dev_dbg(adapter->dev, "info: TX DNLD Done\n");
+			mwifiex_dbg(adapter, INTR,
+				    "info: TX DNLD Done\n");
 			ret = mwifiex_pcie_send_data_complete(adapter);
 			if (ret)
 				return ret;
 		}
 		if (pcie_ireg & HOST_INTR_UPLD_RDY) {
 			pcie_ireg &= ~HOST_INTR_UPLD_RDY;
-			dev_dbg(adapter->dev, "info: Rx DATA\n");
+			mwifiex_dbg(adapter, INTR,
+				    "info: Rx DATA\n");
 			ret = mwifiex_pcie_process_recv_data(adapter);
 			if (ret)
 				return ret;
 		}
 		if (pcie_ireg & HOST_INTR_EVENT_RDY) {
 			pcie_ireg &= ~HOST_INTR_EVENT_RDY;
-			dev_dbg(adapter->dev, "info: Rx EVENT\n");
+			mwifiex_dbg(adapter, INTR,
+				    "info: Rx EVENT\n");
 			ret = mwifiex_pcie_process_event_ready(adapter);
 			if (ret)
 				return ret;
@@ -2156,8 +2197,8 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 		if (pcie_ireg & HOST_INTR_CMD_DONE) {
 			pcie_ireg &= ~HOST_INTR_CMD_DONE;
 			if (adapter->cmd_sent) {
-				dev_dbg(adapter->dev,
-					"info: CMD sent Interrupt\n");
+				mwifiex_dbg(adapter, INTR,
+					    "info: CMD sent Interrupt\n");
 				adapter->cmd_sent = false;
 			}
 			/* Handle command response */
@@ -2169,8 +2210,8 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 		if (mwifiex_pcie_ok_to_access_hw(adapter)) {
 			if (mwifiex_read_reg(adapter, PCIE_HOST_INT_STATUS,
 					     &pcie_ireg)) {
-				dev_warn(adapter->dev,
-					 "Read register failed\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "Read register failed\n");
 				return -1;
 			}
 
@@ -2178,16 +2219,17 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 				if (mwifiex_write_reg(adapter,
 						      PCIE_HOST_INT_STATUS,
 						      ~pcie_ireg)) {
-					dev_warn(adapter->dev,
-						 "Write register failed\n");
+					mwifiex_dbg(adapter, ERROR,
+						    "Write register failed\n");
 					return -1;
 				}
 			}
 
 		}
 	}
-	dev_dbg(adapter->dev, "info: cmd_sent=%d data_sent=%d\n",
-		adapter->cmd_sent, adapter->data_sent);
+	mwifiex_dbg(adapter, INTR,
+		    "info: cmd_sent=%d data_sent=%d\n",
+		    adapter->cmd_sent, adapter->data_sent);
 	if (adapter->ps_state != PS_STATE_SLEEP)
 		mwifiex_pcie_enable_host_int(adapter);
 
@@ -2209,7 +2251,8 @@ static int mwifiex_pcie_host_to_card(struct mwifiex_adapter *adapter, u8 type,
 				     struct mwifiex_tx_param *tx_param)
 {
 	if (!skb) {
-		dev_err(adapter->dev, "Passed NULL skb to %s\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "Passed NULL skb to %s\n", __func__);
 		return -1;
 	}
 
@@ -2232,7 +2275,8 @@ mwifiex_pcie_rdwr_firmware(struct mwifiex_adapter *adapter, u8 doneflag)
 
 	ret = mwifiex_write_reg(adapter, reg->fw_dump_ctrl, FW_DUMP_HOST_READY);
 	if (ret) {
-		dev_err(adapter->dev, "PCIE write err\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "PCIE write err\n");
 		return RDWR_STATUS_FAILURE;
 	}
 
@@ -2243,24 +2287,25 @@ mwifiex_pcie_rdwr_firmware(struct mwifiex_adapter *adapter, u8 doneflag)
 		if (doneflag && ctrl_data == doneflag)
 			return RDWR_STATUS_DONE;
 		if (ctrl_data != FW_DUMP_HOST_READY) {
-			dev_info(adapter->dev,
-				 "The ctrl reg was changed, re-try again!\n");
+			mwifiex_dbg(adapter, WARN,
+				    "The ctrl reg was changed, re-try again!\n");
 			ret = mwifiex_write_reg(adapter, reg->fw_dump_ctrl,
 						FW_DUMP_HOST_READY);
 			if (ret) {
-				dev_err(adapter->dev, "PCIE write err\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "PCIE write err\n");
 				return RDWR_STATUS_FAILURE;
 			}
 		}
 		usleep_range(100, 200);
 	}
 
-	dev_err(adapter->dev, "Fail to pull ctrl_data\n");
+	mwifiex_dbg(adapter, ERROR, "Fail to pull ctrl_data\n");
 	return RDWR_STATUS_FAILURE;
 }
 
 /* This function dump firmware memory to file */
-static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
+static void mwifiex_pcie_fw_dump(struct mwifiex_adapter *adapter)
 {
 	struct pcie_service_card *card = adapter->card;
 	const struct mwifiex_pcie_card_reg *creg = card->pcie.reg;
@@ -2269,7 +2314,6 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 	enum rdwr_status stat;
 	u32 memory_size;
 	int ret;
-	static char *env[] = { "DRIVER=mwifiex_pcie", "EVENT=fw_dump", NULL };
 
 	if (!card->pcie.can_dump_fw)
 		return;
@@ -2284,12 +2328,12 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 		entry->mem_size = 0;
 	}
 
-	dev_info(adapter->dev, "== mwifiex firmware dump start ==\n");
+	mwifiex_dbg(adapter, DUMP, "== mwifiex firmware dump start ==\n");
 
 	/* Read the number of the memories which will dump */
 	stat = mwifiex_pcie_rdwr_firmware(adapter, doneflag);
 	if (stat == RDWR_STATUS_FAILURE)
-		goto done;
+		return;
 
 	reg = creg->fw_dump_start;
 	mwifiex_read_reg_byte(adapter, reg, &dump_num);
@@ -2300,7 +2344,7 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 
 		stat = mwifiex_pcie_rdwr_firmware(adapter, doneflag);
 		if (stat == RDWR_STATUS_FAILURE)
-			goto done;
+			return;
 
 		memory_size = 0;
 		reg = creg->fw_dump_start;
@@ -2311,36 +2355,36 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 		}
 
 		if (memory_size == 0) {
-			dev_info(adapter->dev, "Firmware dump Finished!\n");
+			mwifiex_dbg(adapter, MSG, "Firmware dump Finished!\n");
 			ret = mwifiex_write_reg(adapter, creg->fw_dump_ctrl,
 						FW_DUMP_READ_DONE);
 			if (ret) {
-				dev_err(adapter->dev, "PCIE write err\n");
-				goto done;
+				mwifiex_dbg(adapter, ERROR, "PCIE write err\n");
+				return;
 			}
 			break;
 		}
 
-		dev_info(adapter->dev,
-			 "%s_SIZE=0x%x\n", entry->mem_name, memory_size);
+		mwifiex_dbg(adapter, DUMP,
+			    "%s_SIZE=0x%x\n", entry->mem_name, memory_size);
 		entry->mem_ptr = vmalloc(memory_size + 1);
 		entry->mem_size = memory_size;
 		if (!entry->mem_ptr) {
-			dev_err(adapter->dev,
-				"Vmalloc %s failed\n", entry->mem_name);
-			goto done;
+			mwifiex_dbg(adapter, ERROR,
+				    "Vmalloc %s failed\n", entry->mem_name);
+			return;
 		}
 		dbg_ptr = entry->mem_ptr;
 		end_ptr = dbg_ptr + memory_size;
 
 		doneflag = entry->done_flag;
-		dev_info(adapter->dev, "Start %s output, please wait...\n",
-			 entry->mem_name);
+		mwifiex_dbg(adapter, DUMP, "Start %s output, please wait...\n",
+			    entry->mem_name);
 
 		do {
 			stat = mwifiex_pcie_rdwr_firmware(adapter, doneflag);
 			if (RDWR_STATUS_FAILURE == stat)
-				goto done;
+				return;
 
 			reg_start = creg->fw_dump_start;
 			reg_end = creg->fw_dump_end;
@@ -2349,46 +2393,49 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 				if (dbg_ptr < end_ptr) {
 					dbg_ptr++;
 				} else {
-					dev_err(adapter->dev,
-						"Allocated buf not enough\n");
-					goto done;
+					mwifiex_dbg(adapter, ERROR,
+						    "Allocated buf not enough\n");
+					return;
 				}
 			}
 
 			if (stat != RDWR_STATUS_DONE)
 				continue;
 
-			dev_info(adapter->dev, "%s done: size=0x%tx\n",
-				 entry->mem_name, dbg_ptr - entry->mem_ptr);
+			mwifiex_dbg(adapter, DUMP,
+				    "%s done: size=0x%tx\n",
+				    entry->mem_name, dbg_ptr - entry->mem_ptr);
 			break;
 		} while (true);
 	}
-	dev_info(adapter->dev, "== mwifiex firmware dump end ==\n");
-
-	kobject_uevent_env(&adapter->wiphy->dev.kobj, KOBJ_CHANGE, env);
+	mwifiex_dbg(adapter, DUMP, "== mwifiex firmware dump end ==\n");
+}
 
-done:
-	adapter->curr_mem_idx = 0;
+static void mwifiex_pcie_device_dump_work(struct mwifiex_adapter *adapter)
+{
+	mwifiex_drv_info_dump(adapter);
+	mwifiex_pcie_fw_dump(adapter);
+	mwifiex_upload_device_dump(adapter);
 }
 
 static unsigned long iface_work_flags;
 static struct mwifiex_adapter *save_adapter;
 static void mwifiex_pcie_work(struct work_struct *work)
 {
-	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_FW_DUMP,
+	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP,
 			       &iface_work_flags))
-		mwifiex_pcie_fw_dump_work(save_adapter);
+		mwifiex_pcie_device_dump_work(save_adapter);
 }
 
 static DECLARE_WORK(pcie_work, mwifiex_pcie_work);
 /* This function dumps FW information */
-static void mwifiex_pcie_fw_dump(struct mwifiex_adapter *adapter)
+static void mwifiex_pcie_device_dump(struct mwifiex_adapter *adapter)
 {
 	save_adapter = adapter;
-	if (test_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &iface_work_flags))
+	if (test_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &iface_work_flags))
 		return;
 
-	set_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &iface_work_flags);
+	set_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &iface_work_flags);
 
 	schedule_work(&pcie_work);
 }
@@ -2418,45 +2465,50 @@ static int mwifiex_pcie_init(struct mwifiex_adapter *adapter)
 
 	pci_set_master(pdev);
 
-	dev_dbg(adapter->dev, "try set_consistent_dma_mask(32)\n");
+	mwifiex_dbg(adapter, INFO,
+		    "try set_consistent_dma_mask(32)\n");
 	ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (ret) {
-		dev_err(adapter->dev, "set_dma_mask(32) failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "set_dma_mask(32) failed\n");
 		goto err_set_dma_mask;
 	}
 
 	ret = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (ret) {
-		dev_err(adapter->dev, "set_consistent_dma_mask(64) failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "set_consistent_dma_mask(64) failed\n");
 		goto err_set_dma_mask;
 	}
 
 	ret = pci_request_region(pdev, 0, DRV_NAME);
 	if (ret) {
-		dev_err(adapter->dev, "req_reg(0) error\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "req_reg(0) error\n");
 		goto err_req_region0;
 	}
 	card->pci_mmap = pci_iomap(pdev, 0, 0);
 	if (!card->pci_mmap) {
-		dev_err(adapter->dev, "iomap(0) error\n");
+		mwifiex_dbg(adapter, ERROR, "iomap(0) error\n");
 		ret = -EIO;
 		goto err_iomap0;
 	}
 	ret = pci_request_region(pdev, 2, DRV_NAME);
 	if (ret) {
-		dev_err(adapter->dev, "req_reg(2) error\n");
+		mwifiex_dbg(adapter, ERROR, "req_reg(2) error\n");
 		goto err_req_region2;
 	}
 	card->pci_mmap1 = pci_iomap(pdev, 2, 0);
 	if (!card->pci_mmap1) {
-		dev_err(adapter->dev, "iomap(2) error\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "iomap(2) error\n");
 		ret = -EIO;
 		goto err_iomap2;
 	}
 
-	dev_dbg(adapter->dev,
-		"PCI memory map Virt0: %p PCI memory map Virt2: %p\n",
-		card->pci_mmap, card->pci_mmap1);
+	mwifiex_dbg(adapter, INFO,
+		    "PCI memory map Virt0: %p PCI memory map Virt2: %p\n",
+		    card->pci_mmap, card->pci_mmap1);
 
 	card->cmdrsp_buf = NULL;
 	ret = mwifiex_pcie_create_txbd_ring(adapter);
@@ -2521,10 +2573,11 @@ static void mwifiex_pcie_cleanup(struct mwifiex_adapter *adapter)
 	const struct mwifiex_pcie_card_reg *reg = card->pcie.reg;
 
 	if (user_rmmod) {
-		dev_dbg(adapter->dev, "Clearing driver ready signature\n");
+		mwifiex_dbg(adapter, INFO,
+			    "Clearing driver ready signature\n");
 		if (mwifiex_write_reg(adapter, reg->drv_rdy, 0x00000000))
-			dev_err(adapter->dev,
-				"Failed to write driver not-ready signature\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "Failed to write driver not-ready signature\n");
 	}
 
 	if (pdev) {
@@ -2555,7 +2608,8 @@ static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
 	ret = request_irq(pdev->irq, mwifiex_pcie_interrupt, IRQF_SHARED,
 			  "MRVL_PCIE", pdev);
 	if (ret) {
-		pr_err("request_irq failed: ret=%d\n", ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "request_irq failed: ret=%d\n", ret);
 		adapter->card = NULL;
 		return -1;
 	}
@@ -2582,7 +2636,8 @@ static void mwifiex_unregister_dev(struct mwifiex_adapter *adapter)
 	const struct mwifiex_pcie_card_reg *reg;
 
 	if (card) {
-		dev_dbg(adapter->dev, "%s(): calling free_irq()\n", __func__);
+		mwifiex_dbg(adapter, INFO,
+			    "%s(): calling free_irq()\n", __func__);
 		free_irq(card->dev->irq, card->dev);
 
 		reg = card->pcie.reg;
@@ -2617,7 +2672,7 @@ static struct mwifiex_if_ops pcie_ops = {
 	.cleanup_mpa_buf =		NULL,
 	.init_fw_port =			mwifiex_pcie_init_fw_port,
 	.clean_pcie_ring =		mwifiex_clean_pcie_ring_buf,
-	.fw_dump =			mwifiex_pcie_fw_dump,
+	.device_dump =			mwifiex_pcie_device_dump,
 };
 
 /*
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index 0ffdb7c5afd2..baf9715ddc10 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -241,20 +241,21 @@ mwifiex_is_bss_wpa(struct mwifiex_private *priv,
 	    * LinkSys WRT54G && bss_desc->privacy
 	    */
 	 ) {
-		dev_dbg(priv->adapter->dev, "info: %s: WPA:"
-			" wpa_ie=%#x wpa2_ie=%#x WEP=%s WPA=%s WPA2=%s "
-			"EncMode=%#x privacy=%#x\n", __func__,
-			(bss_desc->bcn_wpa_ie) ?
-			(*(bss_desc->bcn_wpa_ie)).
-			vend_hdr.element_id : 0,
-			(bss_desc->bcn_rsn_ie) ?
-			(*(bss_desc->bcn_rsn_ie)).
-			ieee_hdr.element_id : 0,
-			(priv->sec_info.wep_enabled) ? "e" : "d",
-			(priv->sec_info.wpa_enabled) ? "e" : "d",
-			(priv->sec_info.wpa2_enabled) ? "e" : "d",
-			priv->sec_info.encryption_mode,
-			bss_desc->privacy);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: %s: WPA:\t"
+			    "wpa_ie=%#x wpa2_ie=%#x WEP=%s WPA=%s WPA2=%s\t"
+			    "EncMode=%#x privacy=%#x\n", __func__,
+			    (bss_desc->bcn_wpa_ie) ?
+			    (*bss_desc->bcn_wpa_ie).
+			    vend_hdr.element_id : 0,
+			    (bss_desc->bcn_rsn_ie) ?
+			    (*bss_desc->bcn_rsn_ie).
+			    ieee_hdr.element_id : 0,
+			    (priv->sec_info.wep_enabled) ? "e" : "d",
+			    (priv->sec_info.wpa_enabled) ? "e" : "d",
+			    (priv->sec_info.wpa2_enabled) ? "e" : "d",
+			    priv->sec_info.encryption_mode,
+			    bss_desc->privacy);
 		return true;
 	}
 	return false;
@@ -277,20 +278,21 @@ mwifiex_is_bss_wpa2(struct mwifiex_private *priv,
 		 * Privacy bit may NOT be set in some APs like
 		 * LinkSys WRT54G && bss_desc->privacy
 		 */
-		dev_dbg(priv->adapter->dev, "info: %s: WPA2: "
-			" wpa_ie=%#x wpa2_ie=%#x WEP=%s WPA=%s WPA2=%s "
-			"EncMode=%#x privacy=%#x\n", __func__,
-			(bss_desc->bcn_wpa_ie) ?
-			(*(bss_desc->bcn_wpa_ie)).
-			vend_hdr.element_id : 0,
-			(bss_desc->bcn_rsn_ie) ?
-			(*(bss_desc->bcn_rsn_ie)).
-			ieee_hdr.element_id : 0,
-			(priv->sec_info.wep_enabled) ? "e" : "d",
-			(priv->sec_info.wpa_enabled) ? "e" : "d",
-			(priv->sec_info.wpa2_enabled) ? "e" : "d",
-			priv->sec_info.encryption_mode,
-			bss_desc->privacy);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: %s: WPA2:\t"
+			    "wpa_ie=%#x wpa2_ie=%#x WEP=%s WPA=%s WPA2=%s\t"
+			    "EncMode=%#x privacy=%#x\n", __func__,
+			    (bss_desc->bcn_wpa_ie) ?
+			    (*bss_desc->bcn_wpa_ie).
+			    vend_hdr.element_id : 0,
+			    (bss_desc->bcn_rsn_ie) ?
+			    (*bss_desc->bcn_rsn_ie).
+			    ieee_hdr.element_id : 0,
+			    (priv->sec_info.wep_enabled) ? "e" : "d",
+			    (priv->sec_info.wpa_enabled) ? "e" : "d",
+			    (priv->sec_info.wpa2_enabled) ? "e" : "d",
+			    priv->sec_info.encryption_mode,
+			    bss_desc->privacy);
 		return true;
 	}
 	return false;
@@ -333,18 +335,19 @@ mwifiex_is_bss_dynamic_wep(struct mwifiex_private *priv,
 	    ((!bss_desc->bcn_rsn_ie) ||
 	     ((*(bss_desc->bcn_rsn_ie)).ieee_hdr.element_id != WLAN_EID_RSN)) &&
 	    priv->sec_info.encryption_mode && bss_desc->privacy) {
-		dev_dbg(priv->adapter->dev, "info: %s: dynamic "
-			"WEP: wpa_ie=%#x wpa2_ie=%#x "
-			"EncMode=%#x privacy=%#x\n",
-			__func__,
-			(bss_desc->bcn_wpa_ie) ?
-			(*(bss_desc->bcn_wpa_ie)).
-			vend_hdr.element_id : 0,
-			(bss_desc->bcn_rsn_ie) ?
-			(*(bss_desc->bcn_rsn_ie)).
-			ieee_hdr.element_id : 0,
-			priv->sec_info.encryption_mode,
-			bss_desc->privacy);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: %s: dynamic\t"
+			    "WEP: wpa_ie=%#x wpa2_ie=%#x\t"
+			    "EncMode=%#x privacy=%#x\n",
+			    __func__,
+			    (bss_desc->bcn_wpa_ie) ?
+			    (*bss_desc->bcn_wpa_ie).
+			    vend_hdr.element_id : 0,
+			    (bss_desc->bcn_rsn_ie) ?
+			    (*bss_desc->bcn_rsn_ie).
+			    ieee_hdr.element_id : 0,
+			    priv->sec_info.encryption_mode,
+			    bss_desc->privacy);
 		return true;
 	}
 	return false;
@@ -383,19 +386,20 @@ mwifiex_is_network_compatible(struct mwifiex_private *priv,
 		return 0;
 
 	if (priv->wps.session_enable) {
-		dev_dbg(adapter->dev,
-			"info: return success directly in WPS period\n");
+		mwifiex_dbg(adapter, IOCTL,
+			    "info: return success directly in WPS period\n");
 		return 0;
 	}
 
 	if (bss_desc->chan_sw_ie_present) {
-		dev_err(adapter->dev,
-			"Don't connect to AP with WLAN_EID_CHANNEL_SWITCH\n");
+		mwifiex_dbg(adapter, INFO,
+			    "Don't connect to AP with WLAN_EID_CHANNEL_SWITCH\n");
 		return -1;
 	}
 
 	if (mwifiex_is_bss_wapi(priv, bss_desc)) {
-		dev_dbg(adapter->dev, "info: return success for WAPI AP\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: return success for WAPI AP\n");
 		return 0;
 	}
 
@@ -405,7 +409,8 @@ mwifiex_is_network_compatible(struct mwifiex_private *priv,
 			return 0;
 		} else if (mwifiex_is_bss_static_wep(priv, bss_desc)) {
 			/* Static WEP enabled */
-			dev_dbg(adapter->dev, "info: Disable 11n in WEP mode.\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: Disable 11n in WEP mode.\n");
 			bss_desc->disable_11n = true;
 			return 0;
 		} else if (mwifiex_is_bss_wpa(priv, bss_desc)) {
@@ -418,9 +423,9 @@ mwifiex_is_network_compatible(struct mwifiex_private *priv,
 
 				if (mwifiex_is_wpa_oui_present
 						(bss_desc, CIPHER_SUITE_TKIP)) {
-					dev_dbg(adapter->dev,
-						"info: Disable 11n if AES "
-						"is not supported by AP\n");
+					mwifiex_dbg(adapter, INFO,
+						    "info: Disable 11n if AES\t"
+						    "is not supported by AP\n");
 					bss_desc->disable_11n = true;
 				} else {
 					return -1;
@@ -437,9 +442,9 @@ mwifiex_is_network_compatible(struct mwifiex_private *priv,
 
 				if (mwifiex_is_rsn_oui_present
 						(bss_desc, CIPHER_SUITE_TKIP)) {
-					dev_dbg(adapter->dev,
-						"info: Disable 11n if AES "
-						"is not supported by AP\n");
+					mwifiex_dbg(adapter, INFO,
+						    "info: Disable 11n if AES\t"
+						    "is not supported by AP\n");
 					bss_desc->disable_11n = true;
 				} else {
 					return -1;
@@ -455,17 +460,18 @@ mwifiex_is_network_compatible(struct mwifiex_private *priv,
 		}
 
 		/* Security doesn't match */
-		dev_dbg(adapter->dev,
-			"info: %s: failed: wpa_ie=%#x wpa2_ie=%#x WEP=%s "
-			"WPA=%s WPA2=%s EncMode=%#x privacy=%#x\n", __func__,
-			(bss_desc->bcn_wpa_ie) ?
-			(*(bss_desc->bcn_wpa_ie)).vend_hdr.element_id : 0,
-			(bss_desc->bcn_rsn_ie) ?
-			(*(bss_desc->bcn_rsn_ie)).ieee_hdr.element_id : 0,
-			(priv->sec_info.wep_enabled) ? "e" : "d",
-			(priv->sec_info.wpa_enabled) ? "e" : "d",
-			(priv->sec_info.wpa2_enabled) ? "e" : "d",
-			priv->sec_info.encryption_mode, bss_desc->privacy);
+		mwifiex_dbg(adapter, ERROR,
+			    "info: %s: failed: wpa_ie=%#x wpa2_ie=%#x WEP=%s\t"
+			    "WPA=%s WPA2=%s EncMode=%#x privacy=%#x\n",
+			    __func__,
+			    (bss_desc->bcn_wpa_ie) ?
+			    (*bss_desc->bcn_wpa_ie).vend_hdr.element_id : 0,
+			    (bss_desc->bcn_rsn_ie) ?
+			    (*bss_desc->bcn_rsn_ie).ieee_hdr.element_id : 0,
+			    (priv->sec_info.wep_enabled) ? "e" : "d",
+			    (priv->sec_info.wpa_enabled) ? "e" : "d",
+			    (priv->sec_info.wpa2_enabled) ? "e" : "d",
+			    priv->sec_info.encryption_mode, bss_desc->privacy);
 		return -1;
 	}
 
@@ -560,7 +566,8 @@ mwifiex_append_rate_tlv(struct mwifiex_private *priv,
 	else
 		rates_size = mwifiex_get_supported_rates(priv, rates);
 
-	dev_dbg(priv->adapter->dev, "info: SCAN_CMD: Rates size = %d\n",
+	mwifiex_dbg(priv->adapter, CMD,
+		    "info: SCAN_CMD: Rates size = %d\n",
 		rates_size);
 	rates_tlv = (struct mwifiex_ie_types_rates_param_set *)tlv_pos;
 	rates_tlv->header.type = cpu_to_le16(WLAN_EID_SUPP_RATES);
@@ -600,9 +607,9 @@ mwifiex_scan_channel_list(struct mwifiex_private *priv,
 	u8 radio_type;
 
 	if (!scan_cfg_out || !chan_tlv_out || !scan_chan_list) {
-		dev_dbg(priv->adapter->dev,
-			"info: Scan: Null detect: %p, %p, %p\n",
-		       scan_cfg_out, chan_tlv_out, scan_chan_list);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "info: Scan: Null detect: %p, %p, %p\n",
+			    scan_cfg_out, chan_tlv_out, scan_chan_list);
 		return -1;
 	}
 
@@ -645,16 +652,16 @@ mwifiex_scan_channel_list(struct mwifiex_private *priv,
 			}
 
 			radio_type = tmp_chan_list->radio_type;
-			dev_dbg(priv->adapter->dev,
-				"info: Scan: Chan(%3d), Radio(%d),"
-				" Mode(%d, %d), Dur(%d)\n",
-				tmp_chan_list->chan_number,
-				tmp_chan_list->radio_type,
-				tmp_chan_list->chan_scan_mode_bitmap
-				& MWIFIEX_PASSIVE_SCAN,
-				(tmp_chan_list->chan_scan_mode_bitmap
-				 & MWIFIEX_DISABLE_CHAN_FILT) >> 1,
-				le16_to_cpu(tmp_chan_list->max_scan_time));
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: Scan: Chan(%3d), Radio(%d),\t"
+				    "Mode(%d, %d), Dur(%d)\n",
+				    tmp_chan_list->chan_number,
+				    tmp_chan_list->radio_type,
+				    tmp_chan_list->chan_scan_mode_bitmap
+				    & MWIFIEX_PASSIVE_SCAN,
+				    (tmp_chan_list->chan_scan_mode_bitmap
+				    & MWIFIEX_DISABLE_CHAN_FILT) >> 1,
+				    le16_to_cpu(tmp_chan_list->max_scan_time));
 
 			/* Copy the current channel TLV to the command being
 			   prepared */
@@ -718,9 +725,11 @@ mwifiex_scan_channel_list(struct mwifiex_private *priv,
 		/* The total scan time should be less than scan command timeout
 		   value */
 		if (total_scan_time > MWIFIEX_MAX_TOTAL_SCAN_TIME) {
-			dev_err(priv->adapter->dev, "total scan time %dms"
-				" is over limit (%dms), scan skipped\n",
-				total_scan_time, MWIFIEX_MAX_TOTAL_SCAN_TIME);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "total scan time %dms\t"
+				    "is over limit (%dms), scan skipped\n",
+				    total_scan_time,
+				    MWIFIEX_MAX_TOTAL_SCAN_TIME);
 			ret = -1;
 			break;
 		}
@@ -905,9 +914,10 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 			tlv_pos += (sizeof(wildcard_ssid_tlv->header)
 				+ le16_to_cpu(wildcard_ssid_tlv->header.len));
 
-			dev_dbg(adapter->dev, "info: scan: ssid[%d]: %s, %d\n",
-				i, wildcard_ssid_tlv->ssid,
-				wildcard_ssid_tlv->max_ssid_length);
+			mwifiex_dbg(adapter, INFO,
+				    "info: scan: ssid[%d]: %s, %d\n",
+				    i, wildcard_ssid_tlv->ssid,
+				    wildcard_ssid_tlv->max_ssid_length);
 
 			/* Empty wildcard ssid with a maxlen will match many or
 			   potentially all SSIDs (maxlen == 32), therefore do
@@ -928,8 +938,9 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 			*filtered_scan = true;
 
 		if (user_scan_in->scan_chan_gap) {
-			dev_dbg(adapter->dev, "info: scan: channel gap = %d\n",
-				user_scan_in->scan_chan_gap);
+			mwifiex_dbg(adapter, INFO,
+				    "info: scan: channel gap = %d\n",
+				    user_scan_in->scan_chan_gap);
 			*max_chan_per_scan =
 					MWIFIEX_MAX_CHANNELS_PER_SPECIFIC_SCAN;
 
@@ -961,8 +972,9 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 	   add tlv */
 	if (num_probes) {
 
-		dev_dbg(adapter->dev, "info: scan: num_probes = %d\n",
-			num_probes);
+		mwifiex_dbg(adapter, INFO,
+			    "info: scan: num_probes = %d\n",
+			    num_probes);
 
 		num_probes_tlv = (struct mwifiex_ie_types_num_probes *) tlv_pos;
 		num_probes_tlv->header.type = cpu_to_le16(TLV_TYPE_NUMPROBES);
@@ -1003,7 +1015,8 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 
 	if (user_scan_in && user_scan_in->chan_list[0].chan_number) {
 
-		dev_dbg(adapter->dev, "info: Scan: Using supplied channel list\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: Scan: Using supplied channel list\n");
 
 		for (chan_idx = 0;
 		     chan_idx < MWIFIEX_USER_SCAN_CHAN_MAX &&
@@ -1056,13 +1069,13 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 		    (user_scan_in->chan_list[0].chan_number ==
 		     priv->curr_bss_params.bss_descriptor.channel)) {
 			*scan_current_only = true;
-			dev_dbg(adapter->dev,
-				"info: Scan: Scanning current channel only\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: Scan: Scanning current channel only\n");
 		}
 		chan_num = chan_idx;
 	} else {
-		dev_dbg(adapter->dev,
-			"info: Scan: Creating full region channel list\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: Scan: Creating full region channel list\n");
 		chan_num = mwifiex_scan_create_channel_list(priv, user_scan_in,
 							    scan_chan_list,
 							    *filtered_scan);
@@ -1094,8 +1107,9 @@ mwifiex_ret_802_11_scan_get_tlv_ptrs(struct mwifiex_adapter *adapter,
 	tlv_buf_left = tlv_buf_size;
 	*tlv_data = NULL;
 
-	dev_dbg(adapter->dev, "info: SCAN_RESP: tlv_buf_size = %d\n",
-		tlv_buf_size);
+	mwifiex_dbg(adapter, INFO,
+		    "info: SCAN_RESP: tlv_buf_size = %d\n",
+		    tlv_buf_size);
 
 	while (tlv_buf_left >= sizeof(struct mwifiex_ie_types_header)) {
 
@@ -1103,26 +1117,31 @@ mwifiex_ret_802_11_scan_get_tlv_ptrs(struct mwifiex_adapter *adapter,
 		tlv_len = le16_to_cpu(current_tlv->header.len);
 
 		if (sizeof(tlv->header) + tlv_len > tlv_buf_left) {
-			dev_err(adapter->dev, "SCAN_RESP: TLV buffer corrupt\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "SCAN_RESP: TLV buffer corrupt\n");
 			break;
 		}
 
 		if (req_tlv_type == tlv_type) {
 			switch (tlv_type) {
 			case TLV_TYPE_TSFTIMESTAMP:
-				dev_dbg(adapter->dev, "info: SCAN_RESP: TSF "
-					"timestamp TLV, len = %d\n", tlv_len);
+				mwifiex_dbg(adapter, INFO,
+					    "info: SCAN_RESP: TSF\t"
+					    "timestamp TLV, len = %d\n",
+					    tlv_len);
 				*tlv_data = current_tlv;
 				break;
 			case TLV_TYPE_CHANNELBANDLIST:
-				dev_dbg(adapter->dev, "info: SCAN_RESP: channel"
-					" band list TLV, len = %d\n", tlv_len);
+				mwifiex_dbg(adapter, INFO,
+					    "info: SCAN_RESP: channel\t"
+					    "band list TLV, len = %d\n",
+					    tlv_len);
 				*tlv_data = current_tlv;
 				break;
 			default:
-				dev_err(adapter->dev,
-					"SCAN_RESP: unhandled TLV = %d\n",
-				       tlv_type);
+				mwifiex_dbg(adapter, ERROR,
+					    "SCAN_RESP: unhandled TLV = %d\n",
+					    tlv_type);
 				/* Give up, this seems corrupted */
 				return;
 			}
@@ -1177,8 +1196,9 @@ int mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
 		total_ie_len = element_len + sizeof(struct ieee_types_header);
 
 		if (bytes_left < total_ie_len) {
-			dev_err(adapter->dev, "err: InterpretIE: in processing"
-				" IE, bytes left < IE length\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "err: InterpretIE: in processing\t"
+				    "IE, bytes left < IE length\n");
 			return -1;
 		}
 		switch (element_id) {
@@ -1186,9 +1206,9 @@ int mwifiex_update_bss_desc_with_ie(struct mwifiex_adapter *adapter,
 			bss_entry->ssid.ssid_len = element_len;
 			memcpy(bss_entry->ssid.ssid, (current_ptr + 2),
 			       element_len);
-			dev_dbg(adapter->dev,
-				"info: InterpretIE: ssid: %-32s\n",
-				bss_entry->ssid.ssid);
+			mwifiex_dbg(adapter, INFO,
+				    "info: InterpretIE: ssid: %-32s\n",
+				    bss_entry->ssid.ssid);
 			break;
 
 		case WLAN_EID_SUPP_RATES:
@@ -1419,19 +1439,20 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
 	unsigned long flags;
 
 	if (adapter->scan_processing) {
-		dev_err(adapter->dev, "cmd: Scan already in process...\n");
+		mwifiex_dbg(adapter, WARN,
+			    "cmd: Scan already in process...\n");
 		return -EBUSY;
 	}
 
 	if (priv->scan_block) {
-		dev_err(adapter->dev,
-			"cmd: Scan is blocked during association...\n");
+		mwifiex_dbg(adapter, WARN,
+			    "cmd: Scan is blocked during association...\n");
 		return -EBUSY;
 	}
 
 	if (adapter->surprise_removed || adapter->is_cmd_timedout) {
-		dev_err(adapter->dev,
-			"Ignore scan. Card removed or firmware in bad state\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Ignore scan. Card removed or firmware in bad state\n");
 		return -EFAULT;
 	}
 
@@ -1478,7 +1499,8 @@ int mwifiex_scan_networks(struct mwifiex_private *priv,
 
 			/* Perform internal scan synchronously */
 			if (!priv->scan_request) {
-				dev_dbg(adapter->dev, "wait internal scan\n");
+				mwifiex_dbg(adapter, INFO,
+					    "wait internal scan\n");
 				mwifiex_wait_queue_complete(adapter, cmd_node);
 			}
 		} else {
@@ -1553,8 +1575,8 @@ int mwifiex_check_network_compatibility(struct mwifiex_private *priv,
 			ret = mwifiex_is_network_compatible(priv, bss_desc,
 							    priv->bss_mode);
 			if (ret)
-				dev_err(priv->adapter->dev,
-					"Incompatible network settings\n");
+				mwifiex_dbg(priv->adapter, ERROR,
+					    "Incompatible network settings\n");
 			break;
 		default:
 			ret = 0;
@@ -1656,7 +1678,8 @@ mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
 	 */
 	if (curr_bcn_bytes < ETH_ALEN + sizeof(u8) +
 	    sizeof(struct mwifiex_fixed_bcn_param)) {
-		dev_err(adapter->dev, "InterpretIE: not enough bytes left\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "InterpretIE: not enough bytes left\n");
 		return -EFAULT;
 	}
 
@@ -1669,7 +1692,8 @@ mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
 		rssi = (-rssi) * 100;		/* Convert dBm to mBm */
 		current_ptr += sizeof(u8);
 		curr_bcn_bytes -= sizeof(u8);
-		dev_dbg(adapter->dev, "info: InterpretIE: RSSI=%d\n", rssi);
+		mwifiex_dbg(adapter, INFO,
+			    "info: InterpretIE: RSSI=%d\n", rssi);
 	} else {
 		rssi = rssi_val;
 	}
@@ -1682,14 +1706,16 @@ mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
 	beacon_period = le16_to_cpu(bcn_param->beacon_period);
 
 	cap_info_bitmap = le16_to_cpu(bcn_param->cap_info_bitmap);
-	dev_dbg(adapter->dev, "info: InterpretIE: capabilities=0x%X\n",
-		cap_info_bitmap);
+	mwifiex_dbg(adapter, INFO,
+		    "info: InterpretIE: capabilities=0x%X\n",
+		    cap_info_bitmap);
 
 	/* Rest of the current buffer are IE's */
 	ie_buf = current_ptr;
 	ie_len = curr_bcn_bytes;
-	dev_dbg(adapter->dev, "info: InterpretIE: IELength for this AP = %d\n",
-		curr_bcn_bytes);
+	mwifiex_dbg(adapter, INFO,
+		    "info: InterpretIE: IELength for this AP = %d\n",
+		    curr_bcn_bytes);
 
 	while (curr_bcn_bytes >= sizeof(struct ieee_types_header)) {
 		u8 element_id, element_len;
@@ -1698,8 +1724,8 @@ mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
 		element_len = *(current_ptr + 1);
 		if (curr_bcn_bytes < element_len +
 				sizeof(struct ieee_types_header)) {
-			dev_err(adapter->dev,
-				"%s: bytes left < IE length\n", __func__);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: bytes left < IE length\n", __func__);
 			return -EFAULT;
 		}
 		if (element_id == WLAN_EID_DS_PARAMS) {
@@ -1719,8 +1745,8 @@ mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
 
 		/* Skip entry if on csa closed channel */
 		if (channel == priv->csa_chan) {
-			dev_dbg(adapter->dev,
-				"Dropping entry on csa closed channel\n");
+			mwifiex_dbg(adapter, WARN,
+				    "Dropping entry on csa closed channel\n");
 			return 0;
 		}
 
@@ -1751,7 +1777,7 @@ mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
 			cfg80211_put_bss(priv->wdev.wiphy, bss);
 		}
 	} else {
-		dev_dbg(adapter->dev, "missing BSS channel IE\n");
+		mwifiex_dbg(adapter, WARN, "missing BSS channel IE\n");
 	}
 
 	return 0;
@@ -1765,7 +1791,8 @@ static void mwifiex_complete_scan(struct mwifiex_private *priv)
 	if (adapter->curr_cmd->wait_q_enabled) {
 		adapter->cmd_wait_q.status = 0;
 		if (!priv->scan_request) {
-			dev_dbg(adapter->dev, "complete internal scan\n");
+			mwifiex_dbg(adapter, INFO,
+				    "complete internal scan\n");
 			mwifiex_complete_cmd(adapter, adapter->curr_cmd);
 		}
 	}
@@ -1788,12 +1815,14 @@ static void mwifiex_check_next_scan_command(struct mwifiex_private *priv)
 			mwifiex_complete_scan(priv);
 
 		if (priv->scan_request) {
-			dev_dbg(adapter->dev, "info: notifying scan done\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: notifying scan done\n");
 			cfg80211_scan_done(priv->scan_request, 0);
 			priv->scan_request = NULL;
 		} else {
 			priv->scan_aborting = false;
-			dev_dbg(adapter->dev, "info: scan already aborted\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: scan already aborted\n");
 		}
 	} else if ((priv->scan_aborting && !priv->scan_request) ||
 		   priv->scan_block) {
@@ -1809,12 +1838,14 @@ static void mwifiex_check_next_scan_command(struct mwifiex_private *priv)
 		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
 
 		if (priv->scan_request) {
-			dev_dbg(adapter->dev, "info: aborting scan\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: aborting scan\n");
 			cfg80211_scan_done(priv->scan_request, 1);
 			priv->scan_request = NULL;
 		} else {
 			priv->scan_aborting = false;
-			dev_dbg(adapter->dev, "info: scan already aborted\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: scan already aborted\n");
 		}
 	} else {
 		/* Get scan command from scan_pending_q and put to
@@ -1877,8 +1908,9 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 
 
 	if (scan_rsp->number_of_sets > MWIFIEX_MAX_AP) {
-		dev_err(adapter->dev, "SCAN_RESP: too many AP returned (%d)\n",
-			scan_rsp->number_of_sets);
+		mwifiex_dbg(adapter, ERROR,
+			    "SCAN_RESP: too many AP returned (%d)\n",
+			    scan_rsp->number_of_sets);
 		ret = -1;
 		goto check_next_scan;
 	}
@@ -1887,14 +1919,15 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
 	mwifiex_11h_get_csa_closed_channel(priv);
 
 	bytes_left = le16_to_cpu(scan_rsp->bss_descript_size);
-	dev_dbg(adapter->dev, "info: SCAN_RESP: bss_descript_size %d\n",
-		bytes_left);
+	mwifiex_dbg(adapter, INFO,
+		    "info: SCAN_RESP: bss_descript_size %d\n",
+		    bytes_left);
 
 	scan_resp_size = le16_to_cpu(resp->size);
 
-	dev_dbg(adapter->dev,
-		"info: SCAN_RESP: returned %d APs before parsing\n",
-		scan_rsp->number_of_sets);
+	mwifiex_dbg(adapter, INFO,
+		    "info: SCAN_RESP: returned %d APs before parsing\n",
+		    scan_rsp->number_of_sets);
 
 	bss_info = scan_rsp->bss_desc_and_tlv_buffer;
 
@@ -2007,13 +2040,13 @@ mwifiex_update_chan_statistics(struct mwifiex_private *priv,
 				       le16_to_cpu(fw_chan_stats->cca_scan_dur);
 		chan_stats.cca_busy_dur =
 				       le16_to_cpu(fw_chan_stats->cca_busy_dur);
-		dev_dbg(adapter->dev,
-			"chan=%d, noise=%d, total_network=%d scan_duration=%d, busy_duration=%d\n",
-			chan_stats.chan_num,
-			chan_stats.noise,
-			chan_stats.total_bss,
-			chan_stats.cca_scan_dur,
-			chan_stats.cca_busy_dur);
+		mwifiex_dbg(adapter, INFO,
+			    "chan=%d, noise=%d, total_network=%d scan_duration=%d, busy_duration=%d\n",
+			    chan_stats.chan_num,
+			    chan_stats.noise,
+			    chan_stats.total_bss,
+			    chan_stats.cca_scan_dur,
+			    chan_stats.cca_busy_dur);
 		memcpy(&adapter->chan_stats[adapter->survey_idx++], &chan_stats,
 		       sizeof(struct mwifiex_chan_stats));
 		fw_chan_stats++;
@@ -2035,7 +2068,7 @@ int mwifiex_ret_802_11_scan_ext(struct mwifiex_private *priv,
 	unsigned long cmd_flags, scan_flags;
 	bool complete_scan = false;
 
-	dev_dbg(priv->adapter->dev, "info: EXT scan returns successfully\n");
+	mwifiex_dbg(adapter, INFO, "info: EXT scan returns successfully\n");
 
 	ext_scan_resp = &resp->params.ext_scan;
 
@@ -2048,8 +2081,8 @@ int mwifiex_ret_802_11_scan_ext(struct mwifiex_private *priv,
 		len = le16_to_cpu(tlv->len);
 
 		if (buf_left < (sizeof(struct mwifiex_ie_types_header) + len)) {
-			dev_err(adapter->dev,
-				"error processing scan response TLVs");
+			mwifiex_dbg(adapter, ERROR,
+				    "error processing scan response TLVs");
 			break;
 		}
 
@@ -2075,8 +2108,8 @@ int mwifiex_ret_802_11_scan_ext(struct mwifiex_private *priv,
 			cmd_ptr = (void *)cmd_node->cmd_skb->data;
 			if (le16_to_cpu(cmd_ptr->command) ==
 			    HostCmd_CMD_802_11_SCAN_EXT) {
-				dev_dbg(priv->adapter->dev,
-					"Scan pending in command pending list");
+				mwifiex_dbg(adapter, INFO,
+					    "Scan pending in command pending list");
 				complete_scan = false;
 				break;
 			}
@@ -2114,17 +2147,20 @@ int mwifiex_handle_event_ext_scan_report(struct mwifiex_private *priv,
 	u16 scan_resp_size = le16_to_cpu(event_scan->buf_size);
 
 	if (num_of_set > MWIFIEX_MAX_AP) {
-		dev_err(adapter->dev,
-			"EXT_SCAN: Invalid number of AP returned (%d)!!\n",
-			num_of_set);
+		mwifiex_dbg(adapter, ERROR,
+			    "EXT_SCAN: Invalid number of AP returned (%d)!!\n",
+			    num_of_set);
 		ret = -1;
 		goto check_next_scan;
 	}
 
 	bytes_left = scan_resp_size;
-	dev_dbg(adapter->dev,
-		"EXT_SCAN: size %d, returned %d APs...",
-		scan_resp_size, num_of_set);
+	mwifiex_dbg(adapter, INFO,
+		    "EXT_SCAN: size %d, returned %d APs...",
+		    scan_resp_size, num_of_set);
+	mwifiex_dbg_dump(adapter, CMD_D, "EXT_SCAN buffer:", buf,
+			 scan_resp_size +
+			 sizeof(struct mwifiex_event_scan_result));
 
 	tlv = (struct mwifiex_ie_types_data *)scan_resp;
 
@@ -2132,7 +2168,8 @@ int mwifiex_handle_event_ext_scan_report(struct mwifiex_private *priv,
 		type = le16_to_cpu(tlv->header.type);
 		len = le16_to_cpu(tlv->header.len);
 		if (bytes_left < sizeof(struct mwifiex_ie_types_header) + len) {
-			dev_err(adapter->dev, "EXT_SCAN: Error bytes left < TLV length\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "EXT_SCAN: Error bytes left < TLV length\n");
 			break;
 		}
 		scan_rsp_tlv = NULL;
@@ -2158,8 +2195,9 @@ int mwifiex_handle_event_ext_scan_report(struct mwifiex_private *priv,
 			len = le16_to_cpu(tlv->header.len);
 			if (bytes_left_for_tlv <
 			    sizeof(struct mwifiex_ie_types_header) + len) {
-				dev_err(adapter->dev,
-					"EXT_SCAN: Error in processing TLV, bytes left < TLV length\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "EXT_SCAN: Error in processing TLV,\t"
+					    "bytes left < TLV length\n");
 				scan_rsp_tlv = NULL;
 				bytes_left_for_tlv = 0;
 				continue;
@@ -2199,8 +2237,8 @@ int mwifiex_handle_event_ext_scan_report(struct mwifiex_private *priv,
 		if (scan_info_tlv) {
 			rssi = (s32)(s16)(le16_to_cpu(scan_info_tlv->rssi));
 			rssi *= 100;           /* Convert dBm to mBm */
-			dev_dbg(adapter->dev,
-				"info: InterpretIE: RSSI=%d\n", rssi);
+			mwifiex_dbg(adapter, INFO,
+				    "info: InterpretIE: RSSI=%d\n", rssi);
 			fw_tsf = le64_to_cpu(scan_info_tlv->tsf);
 			radio_type = &scan_info_tlv->radio_type;
 		} else {
@@ -2271,13 +2309,14 @@ static int mwifiex_scan_specific_ssid(struct mwifiex_private *priv,
 	struct mwifiex_user_scan_cfg *scan_cfg;
 
 	if (adapter->scan_processing) {
-		dev_err(adapter->dev, "cmd: Scan already in process...\n");
+		mwifiex_dbg(adapter, WARN,
+			    "cmd: Scan already in process...\n");
 		return -EBUSY;
 	}
 
 	if (priv->scan_block) {
-		dev_err(adapter->dev,
-			"cmd: Scan is blocked during association...\n");
+		mwifiex_dbg(adapter, WARN,
+			    "cmd: Scan is blocked during association...\n");
 		return -EBUSY;
 	}
 
@@ -2309,8 +2348,9 @@ int mwifiex_request_scan(struct mwifiex_private *priv,
 	int ret;
 
 	if (down_interruptible(&priv->async_sem)) {
-		dev_err(priv->adapter->dev, "%s: acquire semaphore\n",
-			__func__);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "%s: acquire semaphore fail\n",
+			    __func__);
 		return -1;
 	}
 
@@ -2400,8 +2440,9 @@ mwifiex_save_curr_bcn(struct mwifiex_private *priv)
 
 	memcpy(priv->curr_bcn_buf, curr_bss->beacon_buf,
 	       curr_bss->beacon_buf_size);
-	dev_dbg(priv->adapter->dev, "info: current beacon saved %d\n",
-		priv->curr_bcn_size);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: current beacon saved %d\n",
+		    priv->curr_bcn_size);
 
 	curr_bss->beacon_buf = priv->curr_bcn_buf;
 
diff --git a/drivers/net/wireless/mwifiex/sdio.c b/drivers/net/wireless/mwifiex/sdio.c
index d10320f89bc1..a0b121f3460c 100644
--- a/drivers/net/wireless/mwifiex/sdio.c
+++ b/drivers/net/wireless/mwifiex/sdio.c
@@ -166,7 +166,8 @@ static int mwifiex_sdio_resume(struct device *dev)
 	adapter = card->adapter;
 
 	if (!adapter->is_suspended) {
-		dev_warn(adapter->dev, "device already resumed\n");
+		mwifiex_dbg(adapter, WARN,
+			    "device already resumed\n");
 		return 0;
 	}
 
@@ -191,8 +192,6 @@ mwifiex_sdio_remove(struct sdio_func *func)
 	struct mwifiex_adapter *adapter;
 	struct mwifiex_private *priv;
 
-	pr_debug("info: SDIO func num=%d\n", func->num);
-
 	card = sdio_get_drvdata(func);
 	if (!card)
 		return;
@@ -201,6 +200,8 @@ mwifiex_sdio_remove(struct sdio_func *func)
 	if (!adapter || !adapter->priv_num)
 		return;
 
+	mwifiex_dbg(adapter, INFO, "info: SDIO func num=%d\n", func->num);
+
 	if (user_rmmod) {
 		if (adapter->is_suspended)
 			mwifiex_sdio_resume(adapter->dev);
@@ -257,12 +258,14 @@ static int mwifiex_sdio_suspend(struct device *dev)
 
 	/* Enable the Host Sleep */
 	if (!mwifiex_enable_hs(adapter)) {
-		dev_err(adapter->dev, "cmd: failed to suspend\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cmd: failed to suspend\n");
 		adapter->hs_enabling = false;
 		return -EFAULT;
 	}
 
-	dev_dbg(adapter->dev, "cmd: suspend with MMC_PM_KEEP_POWER\n");
+	mwifiex_dbg(adapter, INFO,
+		    "cmd: suspend with MMC_PM_KEEP_POWER\n");
 	ret = sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
 
 	/* Indicate device suspended */
@@ -386,8 +389,8 @@ mwifiex_write_data_sync(struct mwifiex_adapter *adapter,
 	u32 ioport = (port & MWIFIEX_SDIO_IO_PORT_MASK);
 
 	if (adapter->is_suspended) {
-		dev_err(adapter->dev,
-			"%s: not allowed while suspended\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: not allowed while suspended\n", __func__);
 		return -1;
 	}
 
@@ -434,7 +437,8 @@ static int mwifiex_read_data_sync(struct mwifiex_adapter *adapter, u8 *buffer,
  */
 static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
 {
-	dev_dbg(adapter->dev, "event: wakeup device...\n");
+	mwifiex_dbg(adapter, EVENT,
+		    "event: wakeup device...\n");
 
 	return mwifiex_write_reg(adapter, CONFIGURATION_REG, HOST_POWER_UP);
 }
@@ -446,7 +450,8 @@ static int mwifiex_pm_wakeup_card(struct mwifiex_adapter *adapter)
  */
 static int mwifiex_pm_wakeup_card_complete(struct mwifiex_adapter *adapter)
 {
-	dev_dbg(adapter->dev, "cmd: wakeup device completed\n");
+	mwifiex_dbg(adapter, EVENT,
+		    "cmd: wakeup device completed\n");
 
 	return mwifiex_write_reg(adapter, CONFIGURATION_REG, 0);
 }
@@ -524,7 +529,8 @@ static int mwifiex_init_sdio_ioport(struct mwifiex_adapter *adapter)
 	else
 		return -1;
 cont:
-	pr_debug("info: SDIO FUNC1 IO port: %#x\n", adapter->ioport);
+	mwifiex_dbg(adapter, INFO,
+		    "info: SDIO FUNC1 IO port: %#x\n", adapter->ioport);
 
 	/* Set Host interrupt reset to read to clear */
 	if (!mwifiex_read_reg(adapter, card->reg->host_int_rsr_reg, &reg))
@@ -556,10 +562,12 @@ static int mwifiex_write_data_to_card(struct mwifiex_adapter *adapter,
 		ret = mwifiex_write_data_sync(adapter, payload, pkt_len, port);
 		if (ret) {
 			i++;
-			dev_err(adapter->dev, "host_to_card, write iomem"
-					" (%d) failed: %d\n", i, ret);
+			mwifiex_dbg(adapter, ERROR,
+				    "host_to_card, write iomem\t"
+				    "(%d) failed: %d\n", i, ret);
 			if (mwifiex_write_reg(adapter, CONFIGURATION_REG, 0x04))
-				dev_err(adapter->dev, "write CFG reg failed\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "write CFG reg failed\n");
 
 			ret = -1;
 			if (i > MAX_WRITE_IOMEM_RETRY)
@@ -584,7 +592,8 @@ static int mwifiex_get_rd_port(struct mwifiex_adapter *adapter, u8 *port)
 	const struct mwifiex_sdio_card_reg *reg = card->reg;
 	u32 rd_bitmap = card->mp_rd_bitmap;
 
-	dev_dbg(adapter->dev, "data: mp_rd_bitmap=0x%08x\n", rd_bitmap);
+	mwifiex_dbg(adapter, DATA,
+		    "data: mp_rd_bitmap=0x%08x\n", rd_bitmap);
 
 	if (card->supports_sdio_new_mode) {
 		if (!(rd_bitmap & reg->data_port_mask))
@@ -598,8 +607,9 @@ static int mwifiex_get_rd_port(struct mwifiex_adapter *adapter, u8 *port)
 	    (card->mp_rd_bitmap & CTRL_PORT_MASK)) {
 		card->mp_rd_bitmap &= (u32) (~CTRL_PORT_MASK);
 		*port = CTRL_PORT;
-		dev_dbg(adapter->dev, "data: port=%d mp_rd_bitmap=0x%08x\n",
-			*port, card->mp_rd_bitmap);
+		mwifiex_dbg(adapter, DATA,
+			    "data: port=%d mp_rd_bitmap=0x%08x\n",
+			    *port, card->mp_rd_bitmap);
 		return 0;
 	}
 
@@ -613,9 +623,9 @@ static int mwifiex_get_rd_port(struct mwifiex_adapter *adapter, u8 *port)
 	if (++card->curr_rd_port == card->max_ports)
 		card->curr_rd_port = reg->start_rd_port;
 
-	dev_dbg(adapter->dev,
-		"data: port=%d mp_rd_bitmap=0x%08x -> 0x%08x\n",
-		*port, rd_bitmap, card->mp_rd_bitmap);
+	mwifiex_dbg(adapter, DATA,
+		    "data: port=%d mp_rd_bitmap=0x%08x -> 0x%08x\n",
+		    *port, rd_bitmap, card->mp_rd_bitmap);
 
 	return 0;
 }
@@ -633,7 +643,8 @@ static int mwifiex_get_wr_port_data(struct mwifiex_adapter *adapter, u32 *port)
 	const struct mwifiex_sdio_card_reg *reg = card->reg;
 	u32 wr_bitmap = card->mp_wr_bitmap;
 
-	dev_dbg(adapter->dev, "data: mp_wr_bitmap=0x%08x\n", wr_bitmap);
+	mwifiex_dbg(adapter, DATA,
+		    "data: mp_wr_bitmap=0x%08x\n", wr_bitmap);
 
 	if (!(wr_bitmap & card->mp_data_port_mask)) {
 		adapter->data_sent = true;
@@ -651,15 +662,16 @@ static int mwifiex_get_wr_port_data(struct mwifiex_adapter *adapter, u32 *port)
 	}
 
 	if ((card->has_control_mask) && (*port == CTRL_PORT)) {
-		dev_err(adapter->dev,
-			"invalid data port=%d cur port=%d mp_wr_bitmap=0x%08x -> 0x%08x\n",
-			*port, card->curr_wr_port, wr_bitmap,
-			card->mp_wr_bitmap);
+		mwifiex_dbg(adapter, ERROR,
+			    "invalid data port=%d cur port=%d mp_wr_bitmap=0x%08x -> 0x%08x\n",
+			    *port, card->curr_wr_port, wr_bitmap,
+			    card->mp_wr_bitmap);
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "data: port=%d mp_wr_bitmap=0x%08x -> 0x%08x\n",
-		*port, wr_bitmap, card->mp_wr_bitmap);
+	mwifiex_dbg(adapter, DATA,
+		    "data: port=%d mp_wr_bitmap=0x%08x -> 0x%08x\n",
+		    *port, wr_bitmap, card->mp_wr_bitmap);
 
 	return 0;
 }
@@ -683,7 +695,8 @@ mwifiex_sdio_poll_card_status(struct mwifiex_adapter *adapter, u8 bits)
 		usleep_range(10, 20);
 	}
 
-	dev_err(adapter->dev, "poll card status failed, tries = %d\n", tries);
+	mwifiex_dbg(adapter, ERROR,
+		    "poll card status failed, tries = %d\n", tries);
 
 	return -1;
 }
@@ -738,7 +751,7 @@ static void mwifiex_interrupt_status(struct mwifiex_adapter *adapter)
 	if (mwifiex_read_data_sync(adapter, card->mp_regs,
 				   card->reg->max_mp_regs,
 				   REG_PORT | MWIFIEX_SDIO_BYTE_MODE_MASK, 0)) {
-		dev_err(adapter->dev, "read mp_regs failed\n");
+		mwifiex_dbg(adapter, ERROR, "read mp_regs failed\n");
 		return;
 	}
 
@@ -751,7 +764,8 @@ static void mwifiex_interrupt_status(struct mwifiex_adapter *adapter)
 		 *	UP_LD_CMD_PORT_HOST_INT_STATUS
 		 * Clear the interrupt status register
 		 */
-		dev_dbg(adapter->dev, "int: sdio_ireg = %#x\n", sdio_ireg);
+		mwifiex_dbg(adapter, INTR,
+			    "int: sdio_ireg = %#x\n", sdio_ireg);
 		spin_lock_irqsave(&adapter->int_lock, flags);
 		adapter->int_status |= sdio_ireg;
 		spin_unlock_irqrestore(&adapter->int_lock, flags);
@@ -802,7 +816,8 @@ static int mwifiex_sdio_enable_host_int(struct mwifiex_adapter *adapter)
 	/* Request the SDIO IRQ */
 	ret = sdio_claim_irq(func, mwifiex_sdio_interrupt);
 	if (ret) {
-		dev_err(adapter->dev, "claim irq failed: ret=%d\n", ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "claim irq failed: ret=%d\n", ret);
 		goto out;
 	}
 
@@ -810,7 +825,8 @@ static int mwifiex_sdio_enable_host_int(struct mwifiex_adapter *adapter)
 	ret = mwifiex_write_reg_locked(func, card->reg->host_int_mask_reg,
 				       card->reg->host_int_enable);
 	if (ret) {
-		dev_err(adapter->dev, "enable host interrupt failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "enable host interrupt failed\n");
 		sdio_release_irq(func);
 	}
 
@@ -830,22 +846,25 @@ static int mwifiex_sdio_card_to_host(struct mwifiex_adapter *adapter,
 	u32 nb;
 
 	if (!buffer) {
-		dev_err(adapter->dev, "%s: buffer is NULL\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: buffer is NULL\n", __func__);
 		return -1;
 	}
 
 	ret = mwifiex_read_data_sync(adapter, buffer, npayload, ioport, 1);
 
 	if (ret) {
-		dev_err(adapter->dev, "%s: read iomem failed: %d\n", __func__,
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: read iomem failed: %d\n", __func__,
 			ret);
 		return -1;
 	}
 
 	nb = le16_to_cpu(*(__le16 *) (buffer));
 	if (nb > npayload) {
-		dev_err(adapter->dev, "%s: invalid packet, nb=%d npayload=%d\n",
-			__func__, nb, npayload);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: invalid packet, nb=%d npayload=%d\n",
+			    __func__, nb, npayload);
 		return -1;
 	}
 
@@ -877,13 +896,14 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 	u32 i = 0;
 
 	if (!firmware_len) {
-		dev_err(adapter->dev,
-			"firmware image not found! Terminating download\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "firmware image not found! Terminating download\n");
 		return -1;
 	}
 
-	dev_dbg(adapter->dev, "info: downloading FW image (%d bytes)\n",
-		firmware_len);
+	mwifiex_dbg(adapter, INFO,
+		    "info: downloading FW image (%d bytes)\n",
+		    firmware_len);
 
 	/* Assume that the allocated buffer is 8-byte aligned */
 	fwbuf = kzalloc(MWIFIEX_UPLD_SIZE, GFP_KERNEL);
@@ -897,8 +917,9 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		ret = mwifiex_sdio_poll_card_status(adapter, CARD_IO_READY |
 						    DN_LD_CARD_RDY);
 		if (ret) {
-			dev_err(adapter->dev, "FW download with helper:"
-				" poll status timeout @ %d\n", offset);
+			mwifiex_dbg(adapter, ERROR,
+				    "FW download with helper:\t"
+				    "poll status timeout @ %d\n", offset);
 			goto done;
 		}
 
@@ -910,19 +931,19 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 			ret = mwifiex_read_reg(adapter, reg->base_0_reg,
 					       &base0);
 			if (ret) {
-				dev_err(adapter->dev,
-					"dev BASE0 register read failed: "
-					"base0=%#04X(%d). Terminating dnld\n",
-					base0, base0);
+				mwifiex_dbg(adapter, ERROR,
+					    "dev BASE0 register read failed:\t"
+					    "base0=%#04X(%d). Terminating dnld\n",
+					    base0, base0);
 				goto done;
 			}
 			ret = mwifiex_read_reg(adapter, reg->base_1_reg,
 					       &base1);
 			if (ret) {
-				dev_err(adapter->dev,
-					"dev BASE1 register read failed: "
-					"base1=%#04X(%d). Terminating dnld\n",
-					base1, base1);
+				mwifiex_dbg(adapter, ERROR,
+					    "dev BASE1 register read failed:\t"
+					    "base1=%#04X(%d). Terminating dnld\n",
+					    base1, base1);
 				goto done;
 			}
 			len = (u16) (((base1 & 0xff) << 8) | (base0 & 0xff));
@@ -936,9 +957,9 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		if (!len) {
 			break;
 		} else if (len > MWIFIEX_UPLD_SIZE) {
-			dev_err(adapter->dev,
-				"FW dnld failed @ %d, invalid length %d\n",
-				offset, len);
+			mwifiex_dbg(adapter, ERROR,
+				    "FW dnld failed @ %d, invalid length %d\n",
+				    offset, len);
 			ret = -1;
 			goto done;
 		}
@@ -948,14 +969,15 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		if (len & BIT(0)) {
 			i++;
 			if (i > MAX_WRITE_IOMEM_RETRY) {
-				dev_err(adapter->dev,
-					"FW dnld failed @ %d, over max retry\n",
-					offset);
+				mwifiex_dbg(adapter, ERROR,
+					    "FW dnld failed @ %d, over max retry\n",
+					    offset);
 				ret = -1;
 				goto done;
 			}
-			dev_err(adapter->dev, "CRC indicated by the helper:"
-				" len = 0x%04X, txlen = %d\n", len, txlen);
+			mwifiex_dbg(adapter, ERROR,
+				    "CRC indicated by the helper:\t"
+				    "len = 0x%04X, txlen = %d\n", len, txlen);
 			len &= ~BIT(0);
 			/* Setting this to 0 to resend from same offset */
 			txlen = 0;
@@ -978,11 +1000,12 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 					      MWIFIEX_SDIO_BLOCK_SIZE,
 					      adapter->ioport);
 		if (ret) {
-			dev_err(adapter->dev,
-				"FW download, write iomem (%d) failed @ %d\n",
-				i, offset);
+			mwifiex_dbg(adapter, ERROR,
+				    "FW download, write iomem (%d) failed @ %d\n",
+				    i, offset);
 			if (mwifiex_write_reg(adapter, CONFIGURATION_REG, 0x04))
-				dev_err(adapter->dev, "write CFG reg failed\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "write CFG reg failed\n");
 
 			ret = -1;
 			goto done;
@@ -991,8 +1014,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 		offset += txlen;
 	} while (true);
 
-	dev_notice(adapter->dev,
-		   "info: FW download over, size %d bytes\n", offset);
+	mwifiex_dbg(adapter, MSG,
+		    "info: FW download over, size %d bytes\n", offset);
 
 	ret = 0;
 done:
@@ -1066,18 +1089,20 @@ static void mwifiex_deaggr_sdio_pkt(struct mwifiex_adapter *adapter,
 		blk_num = *(data + BLOCK_NUMBER_OFFSET);
 		blk_size = adapter->sdio_rx_block_size * blk_num;
 		if (blk_size > total_pkt_len) {
-			dev_err(adapter->dev, "%s: error in pkt,\t"
-				"blk_num=%d, blk_size=%d, total_pkt_len=%d\n",
-				__func__, blk_num, blk_size, total_pkt_len);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: error in blk_size,\t"
+				    "blk_num=%d, blk_size=%d, total_pkt_len=%d\n",
+				    __func__, blk_num, blk_size, total_pkt_len);
 			break;
 		}
 		pkt_len = le16_to_cpu(*(__le16 *)(data + SDIO_HEADER_OFFSET));
 		pkt_type = le16_to_cpu(*(__le16 *)(data + SDIO_HEADER_OFFSET +
 					 2));
 		if ((pkt_len + SDIO_HEADER_OFFSET) > blk_size) {
-			dev_err(adapter->dev, "%s: error in pkt,\t"
-				"pkt_len=%d, blk_size=%d\n",
-				__func__, pkt_len, blk_size);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: error in pkt_len,\t"
+				    "pkt_len=%d, blk_size=%d\n",
+				    __func__, pkt_len, blk_size);
 			break;
 		}
 		skb_deaggr = mwifiex_alloc_dma_align_buf(pkt_len,
@@ -1116,7 +1141,8 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
 
 	switch (upld_typ) {
 	case MWIFIEX_TYPE_AGGR_DATA:
-		dev_dbg(adapter->dev, "info: --- Rx: Aggr Data packet ---\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: --- Rx: Aggr Data packet ---\n");
 		rx_info = MWIFIEX_SKB_RXCB(skb);
 		rx_info->buf_type = MWIFIEX_TYPE_AGGR_DATA;
 		if (adapter->rx_work_enabled) {
@@ -1130,7 +1156,8 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
 		break;
 
 	case MWIFIEX_TYPE_DATA:
-		dev_dbg(adapter->dev, "info: --- Rx: Data packet ---\n");
+		mwifiex_dbg(adapter, DATA,
+			    "info: --- Rx: Data packet ---\n");
 		if (adapter->rx_work_enabled) {
 			skb_queue_tail(&adapter->rx_data_q, skb);
 			adapter->data_received = true;
@@ -1141,7 +1168,8 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
 		break;
 
 	case MWIFIEX_TYPE_CMD:
-		dev_dbg(adapter->dev, "info: --- Rx: Cmd Response ---\n");
+		mwifiex_dbg(adapter, CMD,
+			    "info: --- Rx: Cmd Response ---\n");
 		/* take care of curr_cmd = NULL case */
 		if (!adapter->curr_cmd) {
 			cmd_buf = adapter->upld_buf;
@@ -1163,7 +1191,8 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
 		break;
 
 	case MWIFIEX_TYPE_EVENT:
-		dev_dbg(adapter->dev, "info: --- Rx: Event ---\n");
+		mwifiex_dbg(adapter, EVENT,
+			    "info: --- Rx: Event ---\n");
 		adapter->event_cause = le32_to_cpu(*(__le32 *) skb->data);
 
 		if ((skb->len > 0) && (skb->len  < MAX_EVENT_SIZE))
@@ -1178,7 +1207,8 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
 		break;
 
 	default:
-		dev_err(adapter->dev, "unknown upload type %#x\n", upld_typ);
+		mwifiex_dbg(adapter, ERROR,
+			    "unknown upload type %#x\n", upld_typ);
 		dev_kfree_skb_any(skb);
 		break;
 	}
@@ -1210,16 +1240,18 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 
 	if ((card->has_control_mask) && (port == CTRL_PORT)) {
 		/* Read the command Resp without aggr */
-		dev_dbg(adapter->dev, "info: %s: no aggregation for cmd "
-			"response\n", __func__);
+		mwifiex_dbg(adapter, CMD,
+			    "info: %s: no aggregation for cmd\t"
+			    "response\n", __func__);
 
 		f_do_rx_cur = 1;
 		goto rx_curr_single;
 	}
 
 	if (!card->mpa_rx.enabled) {
-		dev_dbg(adapter->dev, "info: %s: rx aggregation disabled\n",
-			__func__);
+		mwifiex_dbg(adapter, WARN,
+			    "info: %s: rx aggregation disabled\n",
+			    __func__);
 
 		f_do_rx_cur = 1;
 		goto rx_curr_single;
@@ -1230,7 +1262,8 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 	    (card->has_control_mask && (card->mp_rd_bitmap &
 					(~((u32) CTRL_PORT_MASK))))) {
 		/* Some more data RX pending */
-		dev_dbg(adapter->dev, "info: %s: not last packet\n", __func__);
+		mwifiex_dbg(adapter, INFO,
+			    "info: %s: not last packet\n", __func__);
 
 		if (MP_RX_AGGR_IN_PROGRESS(card)) {
 			if (MP_RX_AGGR_BUF_HAS_ROOM(card, rx_len)) {
@@ -1247,7 +1280,8 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 
 	} else {
 		/* No more data RX pending */
-		dev_dbg(adapter->dev, "info: %s: last packet\n", __func__);
+		mwifiex_dbg(adapter, INFO,
+			    "info: %s: last packet\n", __func__);
 
 		if (MP_RX_AGGR_IN_PROGRESS(card)) {
 			f_do_rx_aggr = 1;
@@ -1262,14 +1296,16 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 	}
 
 	if (f_aggr_cur) {
-		dev_dbg(adapter->dev, "info: current packet aggregation\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: current packet aggregation\n");
 		/* Curr pkt can be aggregated */
 		mp_rx_aggr_setup(card, rx_len, port);
 
 		if (MP_RX_AGGR_PKT_LIMIT_REACHED(card) ||
 		    mp_rx_aggr_port_limit_reached(card)) {
-			dev_dbg(adapter->dev, "info: %s: aggregated packet "
-				"limit reached\n", __func__);
+			mwifiex_dbg(adapter, INFO,
+				    "info: %s: aggregated packet\t"
+				    "limit reached\n", __func__);
 			/* No more pkts allowed in Aggr buf, rx it */
 			f_do_rx_aggr = 1;
 		}
@@ -1277,8 +1313,9 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 
 	if (f_do_rx_aggr) {
 		/* do aggr RX now */
-		dev_dbg(adapter->dev, "info: do_rx_aggr: num of packets: %d\n",
-			card->mpa_rx.pkt_cnt);
+		mwifiex_dbg(adapter, DATA,
+			    "info: do_rx_aggr: num of packets: %d\n",
+			    card->mpa_rx.pkt_cnt);
 
 		if (card->supports_sdio_new_mode) {
 			int i;
@@ -1318,8 +1355,9 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 								 GFP_KERNEL |
 								 GFP_DMA);
 			if (!skb_deaggr) {
-				dev_err(adapter->dev, "skb allocation failure drop pkt len=%d type=%d\n",
-					pkt_len, pkt_type);
+				mwifiex_dbg(adapter, ERROR, "skb allocation failure\t"
+					    "drop pkt len=%d type=%d\n",
+					    pkt_len, pkt_type);
 				curr_ptr += len_arr[pind];
 				continue;
 			}
@@ -1339,12 +1377,12 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 				mwifiex_decode_rx_packet(adapter, skb_deaggr,
 							 pkt_type);
 			} else {
-				dev_err(adapter->dev, " drop wrong aggr pkt:\t"
-					"sdio_single_port_rx_aggr=%d\t"
-					"type=%d len=%d max_len=%d\n",
-					adapter->sdio_rx_aggr_enable,
-					pkt_type, pkt_len,
-					len_arr[pind]);
+				mwifiex_dbg(adapter, ERROR,
+					    "drop wrong aggr pkt:\t"
+					    "sdio_single_port_rx_aggr=%d\t"
+					    "type=%d len=%d max_len=%d\n",
+					    adapter->sdio_rx_aggr_enable,
+					    pkt_type, pkt_len, len_arr[pind]);
 				dev_kfree_skb_any(skb_deaggr);
 			}
 			curr_ptr += len_arr[pind];
@@ -1354,13 +1392,14 @@ static int mwifiex_sdio_card_to_host_mp_aggr(struct mwifiex_adapter *adapter,
 
 rx_curr_single:
 	if (f_do_rx_cur) {
-		dev_dbg(adapter->dev, "info: RX: port: %d, rx_len: %d\n",
-			port, rx_len);
+		mwifiex_dbg(adapter, INFO, "info: RX: port: %d, rx_len: %d\n",
+			    port, rx_len);
 
 		skb = mwifiex_alloc_dma_align_buf(rx_len, GFP_KERNEL | GFP_DMA);
 		if (!skb) {
-			dev_err(adapter->dev, "single skb allocated fail,\t"
-				"drop pkt port=%d len=%d\n", port, rx_len);
+			mwifiex_dbg(adapter, ERROR,
+				    "single skb allocated fail,\t"
+				    "drop pkt port=%d len=%d\n", port, rx_len);
 			if (mwifiex_sdio_card_to_host(adapter, &pkt_type,
 						      card->mpa_rx.buf, rx_len,
 						      adapter->ioport + port))
@@ -1376,9 +1415,9 @@ rx_curr_single:
 			goto error;
 		if (!adapter->sdio_rx_aggr_enable &&
 		    pkt_type == MWIFIEX_TYPE_AGGR_DATA) {
-			dev_err(adapter->dev, "drop wrong pkt type %d\t"
-				"current SDIO RX Aggr not enabled\n",
-				pkt_type);
+			mwifiex_dbg(adapter, ERROR, "drop wrong pkt type %d\t"
+				    "current SDIO RX Aggr not enabled\n",
+				    pkt_type);
 			dev_kfree_skb_any(skb);
 			return 0;
 		}
@@ -1386,7 +1425,8 @@ rx_curr_single:
 		mwifiex_decode_rx_packet(adapter, skb, pkt_type);
 	}
 	if (f_post_aggr_cur) {
-		dev_dbg(adapter->dev, "info: current packet aggregation\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: current packet aggregation\n");
 		/* Curr pkt can be aggregated */
 		mp_rx_aggr_setup(card, rx_len, port);
 	}
@@ -1458,7 +1498,7 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 		     MWIFIEX_RX_DATA_BUF_SIZE)
 			return -1;
 		rx_len = (u16) (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE);
-		dev_dbg(adapter->dev, "info: rx_len = %d\n", rx_len);
+		mwifiex_dbg(adapter, INFO, "info: rx_len = %d\n", rx_len);
 
 		skb = mwifiex_alloc_dma_align_buf(rx_len, GFP_KERNEL | GFP_DMA);
 		if (!skb)
@@ -1469,17 +1509,17 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 		if (mwifiex_sdio_card_to_host(adapter, &pkt_type, skb->data,
 					      skb->len, adapter->ioport |
 							CMD_PORT_SLCT)) {
-			dev_err(adapter->dev,
-				"%s: failed to card_to_host", __func__);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: failed to card_to_host", __func__);
 			dev_kfree_skb_any(skb);
 			goto term_cmd;
 		}
 
 		if ((pkt_type != MWIFIEX_TYPE_CMD) &&
 		    (pkt_type != MWIFIEX_TYPE_EVENT))
-			dev_err(adapter->dev,
-				"%s:Received wrong packet on cmd port",
-				__func__);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s:Received wrong packet on cmd port",
+				    __func__);
 
 		mwifiex_decode_rx_packet(adapter, skb, pkt_type);
 	}
@@ -1495,12 +1535,13 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 		}
 		card->mp_wr_bitmap = bitmap;
 
-		dev_dbg(adapter->dev, "int: DNLD: wr_bitmap=0x%x\n",
-			card->mp_wr_bitmap);
+		mwifiex_dbg(adapter, INTR,
+			    "int: DNLD: wr_bitmap=0x%x\n",
+			    card->mp_wr_bitmap);
 		if (adapter->data_sent &&
 		    (card->mp_wr_bitmap & card->mp_data_port_mask)) {
-			dev_dbg(adapter->dev,
-				"info:  <--- Tx DONE Interrupt --->\n");
+			mwifiex_dbg(adapter, INTR,
+				    "info:  <--- Tx DONE Interrupt --->\n");
 			adapter->data_sent = false;
 		}
 	}
@@ -1517,8 +1558,8 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 			adapter->cmd_sent = false;
 	}
 
-	dev_dbg(adapter->dev, "info: cmd_sent=%d data_sent=%d\n",
-		adapter->cmd_sent, adapter->data_sent);
+	mwifiex_dbg(adapter, INTR, "info: cmd_sent=%d data_sent=%d\n",
+		    adapter->cmd_sent, adapter->data_sent);
 	if (sdio_ireg & UP_LD_HOST_INT_STATUS) {
 		bitmap = (u32) card->mp_regs[reg->rd_bitmap_l];
 		bitmap |= ((u32) card->mp_regs[reg->rd_bitmap_u]) << 8;
@@ -1529,40 +1570,45 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 				((u32) card->mp_regs[reg->rd_bitmap_1u]) << 24;
 		}
 		card->mp_rd_bitmap = bitmap;
-		dev_dbg(adapter->dev, "int: UPLD: rd_bitmap=0x%x\n",
-			card->mp_rd_bitmap);
+		mwifiex_dbg(adapter, INTR,
+			    "int: UPLD: rd_bitmap=0x%x\n",
+			    card->mp_rd_bitmap);
 
 		while (true) {
 			ret = mwifiex_get_rd_port(adapter, &port);
 			if (ret) {
-				dev_dbg(adapter->dev,
-					"info: no more rd_port available\n");
+				mwifiex_dbg(adapter, INFO,
+					    "info: no more rd_port available\n");
 				break;
 			}
 			len_reg_l = reg->rd_len_p0_l + (port << 1);
 			len_reg_u = reg->rd_len_p0_u + (port << 1);
 			rx_len = ((u16) card->mp_regs[len_reg_u]) << 8;
 			rx_len |= (u16) card->mp_regs[len_reg_l];
-			dev_dbg(adapter->dev, "info: RX: port=%d rx_len=%u\n",
-				port, rx_len);
+			mwifiex_dbg(adapter, INFO,
+				    "info: RX: port=%d rx_len=%u\n",
+				    port, rx_len);
 			rx_blocks =
 				(rx_len + MWIFIEX_SDIO_BLOCK_SIZE -
 				 1) / MWIFIEX_SDIO_BLOCK_SIZE;
 			if (rx_len <= INTF_HEADER_LEN ||
 			    (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE) >
 			     card->mpa_rx.buf_size) {
-				dev_err(adapter->dev, "invalid rx_len=%d\n",
-					rx_len);
+				mwifiex_dbg(adapter, ERROR,
+					    "invalid rx_len=%d\n",
+					    rx_len);
 				return -1;
 			}
 
 			rx_len = (u16) (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE);
-			dev_dbg(adapter->dev, "info: rx_len = %d\n", rx_len);
+			mwifiex_dbg(adapter, INFO, "info: rx_len = %d\n",
+				    rx_len);
 
 			if (mwifiex_sdio_card_to_host_mp_aggr(adapter, rx_len,
 							      port)) {
-				dev_err(adapter->dev, "card_to_host_mpa failed:"
-					" int status=%#x\n", sdio_ireg);
+				mwifiex_dbg(adapter, ERROR,
+					    "card_to_host_mpa failed: int status=%#x\n",
+					    sdio_ireg);
 				goto term_cmd;
 			}
 		}
@@ -1573,19 +1619,23 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 term_cmd:
 	/* terminate cmd */
 	if (mwifiex_read_reg(adapter, CONFIGURATION_REG, &cr))
-		dev_err(adapter->dev, "read CFG reg failed\n");
+		mwifiex_dbg(adapter, ERROR, "read CFG reg failed\n");
 	else
-		dev_dbg(adapter->dev, "info: CFG reg val = %d\n", cr);
+		mwifiex_dbg(adapter, INFO,
+			    "info: CFG reg val = %d\n", cr);
 
 	if (mwifiex_write_reg(adapter, CONFIGURATION_REG, (cr | 0x04)))
-		dev_err(adapter->dev, "write CFG reg failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "write CFG reg failed\n");
 	else
-		dev_dbg(adapter->dev, "info: write success\n");
+		mwifiex_dbg(adapter, INFO, "info: write success\n");
 
 	if (mwifiex_read_reg(adapter, CONFIGURATION_REG, &cr))
-		dev_err(adapter->dev, "read CFG reg failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "read CFG reg failed\n");
 	else
-		dev_dbg(adapter->dev, "info: CFG reg val =%x\n", cr);
+		mwifiex_dbg(adapter, INFO,
+			    "info: CFG reg val =%x\n", cr);
 
 	return -1;
 }
@@ -1619,8 +1669,9 @@ static int mwifiex_host_to_card_mp_aggr(struct mwifiex_adapter *adapter,
 	if (!card->mpa_tx.enabled ||
 	    (card->has_control_mask && (port == CTRL_PORT)) ||
 	    (card->supports_sdio_new_mode && (port == CMD_PORT_SLCT))) {
-		dev_dbg(adapter->dev, "info: %s: tx aggregation disabled\n",
-			__func__);
+		mwifiex_dbg(adapter, WARN,
+			    "info: %s: tx aggregation disabled\n",
+			    __func__);
 
 		f_send_cur_buf = 1;
 		goto tx_curr_single;
@@ -1628,8 +1679,9 @@ static int mwifiex_host_to_card_mp_aggr(struct mwifiex_adapter *adapter,
 
 	if (next_pkt_len) {
 		/* More pkt in TX queue */
-		dev_dbg(adapter->dev, "info: %s: more packets in queue.\n",
-			__func__);
+		mwifiex_dbg(adapter, INFO,
+			    "info: %s: more packets in queue.\n",
+			    __func__);
 
 		if (MP_TX_AGGR_IN_PROGRESS(card)) {
 			if (MP_TX_AGGR_BUF_HAS_ROOM(card, pkt_len)) {
@@ -1659,8 +1711,9 @@ static int mwifiex_host_to_card_mp_aggr(struct mwifiex_adapter *adapter,
 		}
 	} else {
 		/* Last pkt in TX queue */
-		dev_dbg(adapter->dev, "info: %s: Last packet in Tx Queue.\n",
-			__func__);
+		mwifiex_dbg(adapter, INFO,
+			    "info: %s: Last packet in Tx Queue.\n",
+			    __func__);
 
 		if (MP_TX_AGGR_IN_PROGRESS(card)) {
 			/* some packs in Aggr buf already */
@@ -1677,8 +1730,9 @@ static int mwifiex_host_to_card_mp_aggr(struct mwifiex_adapter *adapter,
 	}
 
 	if (f_precopy_cur_buf) {
-		dev_dbg(adapter->dev, "data: %s: precopy current buffer\n",
-			__func__);
+		mwifiex_dbg(adapter, DATA,
+			    "data: %s: precopy current buffer\n",
+			    __func__);
 		MP_TX_AGGR_BUF_PUT(card, payload, pkt_len, port);
 
 		if (MP_TX_AGGR_PKT_LIMIT_REACHED(card) ||
@@ -1688,9 +1742,10 @@ static int mwifiex_host_to_card_mp_aggr(struct mwifiex_adapter *adapter,
 	}
 
 	if (f_send_aggr_buf) {
-		dev_dbg(adapter->dev, "data: %s: send aggr buffer: %d %d\n",
-			__func__,
-				card->mpa_tx.start_port, card->mpa_tx.ports);
+		mwifiex_dbg(adapter, DATA,
+			    "data: %s: send aggr buffer: %d %d\n",
+			    __func__, card->mpa_tx.start_port,
+			    card->mpa_tx.ports);
 		if (card->supports_sdio_new_mode) {
 			u32 port_count;
 			int i;
@@ -1719,15 +1774,17 @@ static int mwifiex_host_to_card_mp_aggr(struct mwifiex_adapter *adapter,
 
 tx_curr_single:
 	if (f_send_cur_buf) {
-		dev_dbg(adapter->dev, "data: %s: send current buffer %d\n",
-			__func__, port);
+		mwifiex_dbg(adapter, DATA,
+			    "data: %s: send current buffer %d\n",
+			    __func__, port);
 		ret = mwifiex_write_data_to_card(adapter, payload, pkt_len,
 						 adapter->ioport + port);
 	}
 
 	if (f_postcopy_cur_buf) {
-		dev_dbg(adapter->dev, "data: %s: postcopy current buffer\n",
-			__func__);
+		mwifiex_dbg(adapter, DATA,
+			    "data: %s: postcopy current buffer\n",
+			    __func__);
 		MP_TX_AGGR_BUF_PUT(card, payload, pkt_len, port);
 	}
 
@@ -1771,8 +1828,9 @@ static int mwifiex_sdio_host_to_card(struct mwifiex_adapter *adapter,
 	if (type == MWIFIEX_TYPE_DATA) {
 		ret = mwifiex_get_wr_port_data(adapter, &port);
 		if (ret) {
-			dev_err(adapter->dev, "%s: no wr_port available\n",
-				__func__);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: no wr_port available\n",
+				    __func__);
 			return ret;
 		}
 	} else {
@@ -1781,8 +1839,9 @@ static int mwifiex_sdio_host_to_card(struct mwifiex_adapter *adapter,
 
 		if (pkt_len <= INTF_HEADER_LEN ||
 		    pkt_len > MWIFIEX_UPLD_SIZE)
-			dev_err(adapter->dev, "%s: payload=%p, nb=%d\n",
-				__func__, payload, pkt_len);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: payload=%p, nb=%d\n",
+				    __func__, payload, pkt_len);
 
 		if (card->supports_sdio_new_mode)
 			port = CMD_PORT_SLCT;
@@ -1896,7 +1955,8 @@ static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
 	ret = sdio_set_block_size(card->func, MWIFIEX_SDIO_BLOCK_SIZE);
 	sdio_release_host(func);
 	if (ret) {
-		pr_err("cannot set SDIO block size\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "cannot set SDIO block size\n");
 		return ret;
 	}
 
@@ -1977,7 +2037,8 @@ static int mwifiex_init_sdio(struct mwifiex_adapter *adapter)
 					     card->mp_tx_agg_buf_size,
 					     card->mp_rx_agg_buf_size);
 	if (ret) {
-		dev_err(adapter->dev, "failed to alloc sdio mp-a buffers\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "failed to alloc sdio mp-a buffers\n");
 		kfree(card->mp_regs);
 		return -1;
 	}
@@ -2041,8 +2102,9 @@ mwifiex_update_mp_end_port(struct mwifiex_adapter *adapter, u16 port)
 
 	card->curr_wr_port = reg->start_wr_port;
 
-	dev_dbg(adapter->dev, "cmd: mp_end_port %d, data port mask 0x%x\n",
-		port, card->mp_data_port_mask);
+	mwifiex_dbg(adapter, CMD,
+		    "cmd: mp_end_port %d, data port mask 0x%x\n",
+		    port, card->mp_data_port_mask);
 }
 
 static struct mwifiex_adapter *save_adapter;
@@ -2059,7 +2121,7 @@ static void mwifiex_sdio_card_reset_work(struct mwifiex_adapter *adapter)
 	 * We run it in a totally independent workqueue.
 	 */
 
-	pr_err("Resetting card...\n");
+	mwifiex_dbg(adapter, WARN, "Resetting card...\n");
 	mmc_remove_host(target);
 	/* 200ms delay is based on experiment with sdhci controller */
 	mdelay(200);
@@ -2079,14 +2141,14 @@ rdwr_status mwifiex_sdio_rdwr_firmware(struct mwifiex_adapter *adapter,
 	sdio_writeb(card->func, FW_DUMP_HOST_READY, card->reg->fw_dump_ctrl,
 		    &ret);
 	if (ret) {
-		dev_err(adapter->dev, "SDIO Write ERR\n");
+		mwifiex_dbg(adapter, ERROR, "SDIO Write ERR\n");
 		return RDWR_STATUS_FAILURE;
 	}
 	for (tries = 0; tries < MAX_POLL_TRIES; tries++) {
 		ctrl_data = sdio_readb(card->func, card->reg->fw_dump_ctrl,
 				       &ret);
 		if (ret) {
-			dev_err(adapter->dev, "SDIO read err\n");
+			mwifiex_dbg(adapter, ERROR, "SDIO read err\n");
 			return RDWR_STATUS_FAILURE;
 		}
 		if (ctrl_data == FW_DUMP_DONE)
@@ -2094,19 +2156,20 @@ rdwr_status mwifiex_sdio_rdwr_firmware(struct mwifiex_adapter *adapter,
 		if (doneflag && ctrl_data == doneflag)
 			return RDWR_STATUS_DONE;
 		if (ctrl_data != FW_DUMP_HOST_READY) {
-			dev_info(adapter->dev,
-				 "The ctrl reg was changed, re-try again!\n");
+			mwifiex_dbg(adapter, WARN,
+				    "The ctrl reg was changed, re-try again!\n");
 			sdio_writeb(card->func, FW_DUMP_HOST_READY,
 				    card->reg->fw_dump_ctrl, &ret);
 			if (ret) {
-				dev_err(adapter->dev, "SDIO write err\n");
+				mwifiex_dbg(adapter, ERROR, "SDIO write err\n");
 				return RDWR_STATUS_FAILURE;
 			}
 		}
 		usleep_range(100, 200);
 	}
 	if (ctrl_data == FW_DUMP_HOST_READY) {
-		dev_err(adapter->dev, "Fail to pull ctrl_data\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "Fail to pull ctrl_data\n");
 		return RDWR_STATUS_FAILURE;
 	}
 
@@ -2114,7 +2177,7 @@ rdwr_status mwifiex_sdio_rdwr_firmware(struct mwifiex_adapter *adapter,
 }
 
 /* This function dump firmware memory to file */
-static void mwifiex_sdio_fw_dump_work(struct mwifiex_adapter *adapter)
+static void mwifiex_sdio_fw_dump(struct mwifiex_adapter *adapter)
 {
 	struct sdio_mmc_card *card = adapter->card;
 	int ret = 0;
@@ -2122,9 +2185,6 @@ static void mwifiex_sdio_fw_dump_work(struct mwifiex_adapter *adapter)
 	u8 *dbg_ptr, *end_ptr, dump_num, idx, i, read_reg, doneflag = 0;
 	enum rdwr_status stat;
 	u32 memory_size;
-	static char *env[] = { "DRIVER=mwifiex_sdio", "EVENT=fw_dump", NULL };
-
-	mwifiex_dump_drv_info(adapter);
 
 	if (!card->can_dump_fw)
 		return;
@@ -2142,7 +2202,7 @@ static void mwifiex_sdio_fw_dump_work(struct mwifiex_adapter *adapter)
 	mwifiex_pm_wakeup_card(adapter);
 	sdio_claim_host(card->func);
 
-	dev_info(adapter->dev, "== mwifiex firmware dump start ==\n");
+	mwifiex_dbg(adapter, MSG, "== mwifiex firmware dump start ==\n");
 
 	stat = mwifiex_sdio_rdwr_firmware(adapter, doneflag);
 	if (stat == RDWR_STATUS_FAILURE)
@@ -2152,7 +2212,7 @@ static void mwifiex_sdio_fw_dump_work(struct mwifiex_adapter *adapter)
 	/* Read the number of the memories which will dump */
 	dump_num = sdio_readb(card->func, reg, &ret);
 	if (ret) {
-		dev_err(adapter->dev, "SDIO read memory length err\n");
+		mwifiex_dbg(adapter, ERROR, "SDIO read memory length err\n");
 		goto done;
 	}
 
@@ -2169,7 +2229,7 @@ static void mwifiex_sdio_fw_dump_work(struct mwifiex_adapter *adapter)
 		for (i = 0; i < 4; i++) {
 			read_reg = sdio_readb(card->func, reg, &ret);
 			if (ret) {
-				dev_err(adapter->dev, "SDIO read err\n");
+				mwifiex_dbg(adapter, ERROR, "SDIO read err\n");
 				goto done;
 			}
 			memory_size |= (read_reg << i*8);
@@ -2177,25 +2237,33 @@ static void mwifiex_sdio_fw_dump_work(struct mwifiex_adapter *adapter)
 		}
 
 		if (memory_size == 0) {
-			dev_info(adapter->dev, "Firmware dump Finished!\n");
+			mwifiex_dbg(adapter, DUMP, "Firmware dump Finished!\n");
+			ret = mwifiex_write_reg(adapter,
+						card->reg->fw_dump_ctrl,
+						FW_DUMP_READ_DONE);
+			if (ret) {
+				mwifiex_dbg(adapter, ERROR, "SDIO write err\n");
+				return;
+			}
 			break;
 		}
 
-		dev_info(adapter->dev,
-			 "%s_SIZE=0x%x\n", entry->mem_name, memory_size);
+		mwifiex_dbg(adapter, DUMP,
+			    "%s_SIZE=0x%x\n", entry->mem_name, memory_size);
 		entry->mem_ptr = vmalloc(memory_size + 1);
 		entry->mem_size = memory_size;
 		if (!entry->mem_ptr) {
-			dev_err(adapter->dev, "Vmalloc %s failed\n",
-				entry->mem_name);
+			mwifiex_dbg(adapter, ERROR, "Vmalloc %s failed\n",
+				    entry->mem_name);
 			goto done;
 		}
 		dbg_ptr = entry->mem_ptr;
 		end_ptr = dbg_ptr + memory_size;
 
 		doneflag = entry->done_flag;
-		dev_info(adapter->dev, "Start %s output, please wait...\n",
-			 entry->mem_name);
+		mwifiex_dbg(adapter, DUMP,
+			    "Start %s output, please wait...\n",
+			    entry->mem_name);
 
 		do {
 			stat = mwifiex_sdio_rdwr_firmware(adapter, doneflag);
@@ -2207,39 +2275,43 @@ static void mwifiex_sdio_fw_dump_work(struct mwifiex_adapter *adapter)
 			for (reg = reg_start; reg <= reg_end; reg++) {
 				*dbg_ptr = sdio_readb(card->func, reg, &ret);
 				if (ret) {
-					dev_err(adapter->dev,
-						"SDIO read err\n");
+					mwifiex_dbg(adapter, ERROR,
+						    "SDIO read err\n");
 					goto done;
 				}
 				if (dbg_ptr < end_ptr)
 					dbg_ptr++;
 				else
-					dev_err(adapter->dev,
-						"Allocated buf not enough\n");
+					mwifiex_dbg(adapter, ERROR,
+						    "Allocated buf not enough\n");
 			}
 
 			if (stat != RDWR_STATUS_DONE)
 				continue;
 
-			dev_info(adapter->dev, "%s done: size=0x%tx\n",
-				 entry->mem_name, dbg_ptr - entry->mem_ptr);
+			mwifiex_dbg(adapter, DUMP, "%s done: size=0x%tx\n",
+				    entry->mem_name, dbg_ptr - entry->mem_ptr);
 			break;
 		} while (1);
 	}
-	dev_info(adapter->dev, "== mwifiex firmware dump end ==\n");
-
-	kobject_uevent_env(&adapter->wiphy->dev.kobj, KOBJ_CHANGE, env);
+	mwifiex_dbg(adapter, MSG, "== mwifiex firmware dump end ==\n");
 
 done:
 	sdio_release_host(card->func);
-	adapter->curr_mem_idx = 0;
+}
+
+static void mwifiex_sdio_device_dump_work(struct mwifiex_adapter *adapter)
+{
+	mwifiex_drv_info_dump(adapter);
+	mwifiex_sdio_fw_dump(adapter);
+	mwifiex_upload_device_dump(adapter);
 }
 
 static void mwifiex_sdio_work(struct work_struct *work)
 {
-	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_FW_DUMP,
+	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP,
 			       &iface_work_flags))
-		mwifiex_sdio_fw_dump_work(save_adapter);
+		mwifiex_sdio_device_dump_work(save_adapter);
 	if (test_and_clear_bit(MWIFIEX_IFACE_WORK_CARD_RESET,
 			       &iface_work_flags))
 		mwifiex_sdio_card_reset_work(save_adapter);
@@ -2259,13 +2331,13 @@ static void mwifiex_sdio_card_reset(struct mwifiex_adapter *adapter)
 }
 
 /* This function dumps FW information */
-static void mwifiex_sdio_fw_dump(struct mwifiex_adapter *adapter)
+static void mwifiex_sdio_device_dump(struct mwifiex_adapter *adapter)
 {
 	save_adapter = adapter;
-	if (test_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &iface_work_flags))
+	if (test_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &iface_work_flags))
 		return;
 
-	set_bit(MWIFIEX_IFACE_WORK_FW_DUMP, &iface_work_flags);
+	set_bit(MWIFIEX_IFACE_WORK_DEVICE_DUMP, &iface_work_flags);
 	schedule_work(&sdio_work);
 }
 
@@ -2285,7 +2357,7 @@ mwifiex_sdio_reg_dump(struct mwifiex_adapter *adapter, char *drv_buf)
 	if (!p)
 		return 0;
 
-	dev_info(adapter->dev, "SDIO register DUMP START\n");
+	mwifiex_dbg(adapter, MSG, "SDIO register dump start\n");
 
 	mwifiex_pm_wakeup_card(adapter);
 
@@ -2351,13 +2423,13 @@ mwifiex_sdio_reg_dump(struct mwifiex_adapter *adapter, char *drv_buf)
 				reg++;
 		}
 
-		dev_info(adapter->dev, "%s\n", buf);
+		mwifiex_dbg(adapter, MSG, "%s\n", buf);
 		p += sprintf(p, "%s\n", buf);
 	}
 
 	sdio_release_host(cardp->func);
 
-	dev_info(adapter->dev, "SDIO register DUMP END\n");
+	mwifiex_dbg(adapter, MSG, "SDIO register dump end\n");
 
 	return p - drv_buf;
 }
@@ -2382,8 +2454,8 @@ static struct mwifiex_if_ops sdio_ops = {
 	.cmdrsp_complete = mwifiex_sdio_cmdrsp_complete,
 	.event_complete = mwifiex_sdio_event_complete,
 	.card_reset = mwifiex_sdio_card_reset,
-	.fw_dump = mwifiex_sdio_fw_dump,
 	.reg_dump = mwifiex_sdio_reg_dump,
+	.device_dump = mwifiex_sdio_device_dump,
 	.deaggr_pkt = mwifiex_deaggr_sdio_pkt,
 };
 
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index 49422f2a5380..037adcd1f484 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -77,8 +77,8 @@ static int mwifiex_cmd_mac_control(struct mwifiex_private *priv,
 	struct host_cmd_ds_mac_control *mac_ctrl = &cmd->params.mac_ctrl;
 
 	if (cmd_action != HostCmd_ACT_GEN_SET) {
-		dev_err(priv->adapter->dev,
-			"mac_control: only support set cmd\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "mac_control: only support set cmd\n");
 		return -1;
 	}
 
@@ -112,7 +112,8 @@ static int mwifiex_cmd_802_11_snmp_mib(struct mwifiex_private *priv,
 {
 	struct host_cmd_ds_802_11_snmp_mib *snmp_mib = &cmd->params.smib;
 
-	dev_dbg(priv->adapter->dev, "cmd: SNMP_CMD: cmd_oid = 0x%x\n", cmd_oid);
+	mwifiex_dbg(priv->adapter, CMD,
+		    "cmd: SNMP_CMD: cmd_oid = 0x%x\n", cmd_oid);
 	cmd->command = cpu_to_le16(HostCmd_CMD_802_11_SNMP_MIB);
 	cmd->size = cpu_to_le16(sizeof(struct host_cmd_ds_802_11_snmp_mib)
 				- 1 + S_DS_GEN);
@@ -129,11 +130,11 @@ static int mwifiex_cmd_802_11_snmp_mib(struct mwifiex_private *priv,
 		le16_add_cpu(&cmd->size, sizeof(u16));
 	}
 
-	dev_dbg(priv->adapter->dev,
-		"cmd: SNMP_CMD: Action=0x%x, OID=0x%x, OIDSize=0x%x,"
-		" Value=0x%x\n",
-		cmd_action, cmd_oid, le16_to_cpu(snmp_mib->buf_size),
-		le16_to_cpu(*(__le16 *) snmp_mib->value));
+	mwifiex_dbg(priv->adapter, CMD,
+		    "cmd: SNMP_CMD: Action=0x%x, OID=0x%x,\t"
+		    "OIDSize=0x%x, Value=0x%x\n",
+		    cmd_action, cmd_oid, le16_to_cpu(snmp_mib->buf_size),
+		    le16_to_cpu(*(__le16 *)snmp_mib->value));
 	return 0;
 }
 
@@ -356,9 +357,9 @@ mwifiex_cmd_802_11_hs_cfg(struct mwifiex_private *priv,
 	    (hscfg_param->conditions != cpu_to_le32(HS_CFG_CANCEL)) &&
 	    ((adapter->arp_filter_size > 0) &&
 	     (adapter->arp_filter_size <= ARP_FILTER_MAX_BUF_SIZE))) {
-		dev_dbg(adapter->dev,
-			"cmd: Attach %d bytes ArpFilter to HSCfg cmd\n",
-			adapter->arp_filter_size);
+		mwifiex_dbg(adapter, CMD,
+			    "cmd: Attach %d bytes ArpFilter to HSCfg cmd\n",
+			    adapter->arp_filter_size);
 		memcpy(((u8 *) hs_cfg) +
 		       sizeof(struct host_cmd_ds_802_11_hs_cfg_enh),
 		       adapter->arp_filter, adapter->arp_filter_size);
@@ -378,11 +379,11 @@ mwifiex_cmd_802_11_hs_cfg(struct mwifiex_private *priv,
 		hs_cfg->params.hs_config.conditions = hscfg_param->conditions;
 		hs_cfg->params.hs_config.gpio = hscfg_param->gpio;
 		hs_cfg->params.hs_config.gap = hscfg_param->gap;
-		dev_dbg(adapter->dev,
-			"cmd: HS_CFG_CMD: condition:0x%x gpio:0x%x gap:0x%x\n",
-		       hs_cfg->params.hs_config.conditions,
-		       hs_cfg->params.hs_config.gpio,
-		       hs_cfg->params.hs_config.gap);
+		mwifiex_dbg(adapter, CMD,
+			    "cmd: HS_CFG_CMD: condition:0x%x gpio:0x%x gap:0x%x\n",
+			    hs_cfg->params.hs_config.conditions,
+			    hs_cfg->params.hs_config.gpio,
+			    hs_cfg->params.hs_config.gap);
 	}
 
 	return 0;
@@ -462,7 +463,7 @@ static int mwifiex_cmd_802_11_deauthenticate(struct mwifiex_private *priv,
 	/* Set AP MAC address */
 	memcpy(deauth->mac_addr, mac, ETH_ALEN);
 
-	dev_dbg(priv->adapter->dev, "cmd: Deauth: %pM\n", deauth->mac_addr);
+	mwifiex_dbg(priv->adapter, CMD, "cmd: Deauth: %pM\n", deauth->mac_addr);
 
 	deauth->reason_code = cpu_to_le16(WLAN_REASON_DEAUTH_LEAVING);
 
@@ -540,9 +541,9 @@ mwifiex_set_keyparamset_wep(struct mwifiex_private *priv,
 		} else if (!priv->wep_key[i].key_length) {
 			continue;
 		} else {
-			dev_err(priv->adapter->dev,
-				"key%d Length = %d is incorrect\n",
-			       (i + 1), priv->wep_key[i].key_length);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "key%d Length = %d is incorrect\n",
+				    (i + 1), priv->wep_key[i].key_length);
 			return -1;
 		}
 	}
@@ -562,7 +563,8 @@ static int mwifiex_set_aes_key_v2(struct mwifiex_private *priv,
 	u16 size, len = KEY_PARAMS_FIXED_LEN;
 
 	if (enc_key->is_igtk_key) {
-		dev_dbg(adapter->dev, "%s: Set CMAC AES Key\n", __func__);
+		mwifiex_dbg(adapter, INFO,
+			    "%s: Set CMAC AES Key\n", __func__);
 		if (enc_key->is_rx_seq_valid)
 			memcpy(km->key_param_set.key_params.cmac_aes.ipn,
 			       enc_key->pn, enc_key->pn_len);
@@ -575,7 +577,8 @@ static int mwifiex_set_aes_key_v2(struct mwifiex_private *priv,
 		       enc_key->key_material, enc_key->key_len);
 		len += sizeof(struct mwifiex_cmac_aes_param);
 	} else {
-		dev_dbg(adapter->dev, "%s: Set AES Key\n", __func__);
+		mwifiex_dbg(adapter, INFO,
+			    "%s: Set AES Key\n", __func__);
 		if (enc_key->is_rx_seq_valid)
 			memcpy(km->key_param_set.key_params.aes.pn,
 			       enc_key->pn, enc_key->pn_len);
@@ -619,7 +622,7 @@ mwifiex_cmd_802_11_key_material_v2(struct mwifiex_private *priv,
 	km->action = cpu_to_le16(cmd_action);
 
 	if (cmd_action == HostCmd_ACT_GEN_GET) {
-		dev_dbg(adapter->dev, "%s: Get key\n", __func__);
+		mwifiex_dbg(adapter, INFO, "%s: Get key\n", __func__);
 		km->key_param_set.key_idx =
 					enc_key->key_index & KEY_INDEX_MASK;
 		km->key_param_set.type = cpu_to_le16(TLV_TYPE_KEY_PARAM_V2);
@@ -646,7 +649,7 @@ mwifiex_cmd_802_11_key_material_v2(struct mwifiex_private *priv,
 	       sizeof(struct mwifiex_ie_type_key_param_set_v2));
 
 	if (enc_key->key_disable) {
-		dev_dbg(adapter->dev, "%s: Remove key\n", __func__);
+		mwifiex_dbg(adapter, INFO, "%s: Remove key\n", __func__);
 		km->action = cpu_to_le16(HostCmd_ACT_GEN_REMOVE);
 		km->key_param_set.type = cpu_to_le16(TLV_TYPE_KEY_PARAM_V2);
 		km->key_param_set.len = cpu_to_le16(KEY_PARAMS_FIXED_LEN);
@@ -667,7 +670,7 @@ mwifiex_cmd_802_11_key_material_v2(struct mwifiex_private *priv,
 	memcpy(km->key_param_set.mac_addr, mac, ETH_ALEN);
 
 	if (enc_key->key_len <= WLAN_KEY_LEN_WEP104) {
-		dev_dbg(adapter->dev, "%s: Set WEP Key\n", __func__);
+		mwifiex_dbg(adapter, INFO, "%s: Set WEP Key\n", __func__);
 		len += sizeof(struct mwifiex_wep_param);
 		km->key_param_set.len = cpu_to_le16(len);
 		km->key_param_set.key_type = KEY_TYPE_ID_WEP;
@@ -710,7 +713,7 @@ mwifiex_cmd_802_11_key_material_v2(struct mwifiex_private *priv,
 		key_info |= KEY_UNICAST | KEY_TX_KEY | KEY_RX_KEY;
 
 	if (enc_key->is_wapi_key) {
-		dev_dbg(adapter->dev, "%s: Set WAPI Key\n", __func__);
+		mwifiex_dbg(adapter, INFO, "%s: Set WAPI Key\n", __func__);
 		km->key_param_set.key_type = KEY_TYPE_ID_WAPI;
 		memcpy(km->key_param_set.key_params.wapi.pn, enc_key->pn,
 		       PN_LEN);
@@ -750,7 +753,8 @@ mwifiex_cmd_802_11_key_material_v2(struct mwifiex_private *priv,
 		return mwifiex_set_aes_key_v2(priv, cmd, enc_key, km);
 
 	if (enc_key->key_len == WLAN_KEY_LEN_TKIP) {
-		dev_dbg(adapter->dev, "%s: Set TKIP Key\n", __func__);
+		mwifiex_dbg(adapter, INFO,
+			    "%s: Set TKIP Key\n", __func__);
 		if (enc_key->is_rx_seq_valid)
 			memcpy(km->key_param_set.key_params.tkip.pn,
 			       enc_key->pn, enc_key->pn_len);
@@ -814,7 +818,7 @@ mwifiex_cmd_802_11_key_material_v1(struct mwifiex_private *priv,
 		memset(&key_material->key_param_set, 0,
 		       sizeof(struct mwifiex_ie_type_key_param_set));
 	if (enc_key->is_wapi_key) {
-		dev_dbg(priv->adapter->dev, "info: Set WAPI Key\n");
+		mwifiex_dbg(priv->adapter, INFO, "info: Set WAPI Key\n");
 		key_material->key_param_set.key_type_id =
 						cpu_to_le16(KEY_TYPE_ID_WAPI);
 		if (cmd_oid == KEY_INFO_ENABLED)
@@ -860,7 +864,7 @@ mwifiex_cmd_802_11_key_material_v1(struct mwifiex_private *priv,
 	}
 	if (enc_key->key_len == WLAN_KEY_LEN_CCMP) {
 		if (enc_key->is_igtk_key) {
-			dev_dbg(priv->adapter->dev, "cmd: CMAC_AES\n");
+			mwifiex_dbg(priv->adapter, CMD, "cmd: CMAC_AES\n");
 			key_material->key_param_set.key_type_id =
 					cpu_to_le16(KEY_TYPE_ID_AES_CMAC);
 			if (cmd_oid == KEY_INFO_ENABLED)
@@ -873,7 +877,7 @@ mwifiex_cmd_802_11_key_material_v1(struct mwifiex_private *priv,
 			key_material->key_param_set.key_info |=
 							cpu_to_le16(KEY_IGTK);
 		} else {
-			dev_dbg(priv->adapter->dev, "cmd: WPA_AES\n");
+			mwifiex_dbg(priv->adapter, CMD, "cmd: WPA_AES\n");
 			key_material->key_param_set.key_type_id =
 						cpu_to_le16(KEY_TYPE_ID_AES);
 			if (cmd_oid == KEY_INFO_ENABLED)
@@ -892,7 +896,7 @@ mwifiex_cmd_802_11_key_material_v1(struct mwifiex_private *priv,
 							cpu_to_le16(KEY_MCAST);
 		}
 	} else if (enc_key->key_len == WLAN_KEY_LEN_TKIP) {
-		dev_dbg(priv->adapter->dev, "cmd: WPA_TKIP\n");
+		mwifiex_dbg(priv->adapter, CMD, "cmd: WPA_TKIP\n");
 		key_material->key_param_set.key_type_id =
 						cpu_to_le16(KEY_TYPE_ID_TKIP);
 		key_material->key_param_set.key_info =
@@ -999,7 +1003,8 @@ static int mwifiex_cmd_802_11d_domain_info(struct mwifiex_private *priv,
 		&domain_info->domain;
 	u8 no_of_triplet = adapter->domain_reg.no_of_triplet;
 
-	dev_dbg(adapter->dev, "info: 11D: no_of_triplet=0x%x\n", no_of_triplet);
+	mwifiex_dbg(adapter, INFO,
+		    "info: 11D: no_of_triplet=0x%x\n", no_of_triplet);
 
 	cmd->command = cpu_to_le16(HostCmd_CMD_802_11D_DOMAIN_INFO);
 	domain_info->action = cpu_to_le16(cmd_action);
@@ -1071,6 +1076,26 @@ static int mwifiex_cmd_ibss_coalescing_status(struct host_cmd_ds_command *cmd,
 	return 0;
 }
 
+/* This function prepares command buffer to get/set memory location value.
+ */
+static int
+mwifiex_cmd_mem_access(struct host_cmd_ds_command *cmd, u16 cmd_action,
+		       void *pdata_buf)
+{
+	struct mwifiex_ds_mem_rw *mem_rw = (void *)pdata_buf;
+	struct host_cmd_ds_mem_access *mem_access = (void *)&cmd->params.mem;
+
+	cmd->command = cpu_to_le16(HostCmd_CMD_MEM_ACCESS);
+	cmd->size = cpu_to_le16(sizeof(struct host_cmd_ds_mem_access) +
+				S_DS_GEN);
+
+	mem_access->action = cpu_to_le16(cmd_action);
+	mem_access->addr = cpu_to_le32(mem_rw->addr);
+	mem_access->value = cpu_to_le32(mem_rw->value);
+
+	return 0;
+}
+
 /*
  * This function prepares command to set/get register value.
  *
@@ -1215,8 +1240,9 @@ mwifiex_cmd_pcie_host_spec(struct mwifiex_private *priv,
 						(u32)(card->sleep_cookie_pbase);
 		host_spec->sleep_cookie_addr_hi =
 				 (u32)(((u64)(card->sleep_cookie_pbase)) >> 32);
-		dev_dbg(priv->adapter->dev, "sleep_cook_lo phy addr: 0x%x\n",
-			host_spec->sleep_cookie_addr_lo);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "sleep_cook_lo phy addr: 0x%x\n",
+			    host_spec->sleep_cookie_addr_lo);
 	}
 
 	return 0;
@@ -1243,7 +1269,8 @@ mwifiex_cmd_802_11_subsc_evt(struct mwifiex_private *priv,
 				S_DS_GEN);
 
 	subsc_evt->action = cpu_to_le16(subsc_evt_cfg->action);
-	dev_dbg(priv->adapter->dev, "cmd: action: %d\n", subsc_evt_cfg->action);
+	mwifiex_dbg(priv->adapter, CMD,
+		    "cmd: action: %d\n", subsc_evt_cfg->action);
 
 	/*For query requests, no configuration TLV structures are to be added.*/
 	if (subsc_evt_cfg->action == HostCmd_ACT_GEN_GET)
@@ -1252,14 +1279,15 @@ mwifiex_cmd_802_11_subsc_evt(struct mwifiex_private *priv,
 	subsc_evt->events = cpu_to_le16(subsc_evt_cfg->events);
 
 	event_bitmap = subsc_evt_cfg->events;
-	dev_dbg(priv->adapter->dev, "cmd: event bitmap : %16x\n",
-		event_bitmap);
+	mwifiex_dbg(priv->adapter, CMD, "cmd: event bitmap : %16x\n",
+		    event_bitmap);
 
 	if (((subsc_evt_cfg->action == HostCmd_ACT_BITWISE_CLR) ||
 	     (subsc_evt_cfg->action == HostCmd_ACT_BITWISE_SET)) &&
 	    (event_bitmap == 0)) {
-		dev_dbg(priv->adapter->dev, "Error: No event specified "
-			"for bitwise action type\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Error: No event specified\t"
+			    "for bitwise action type\n");
 		return -EINVAL;
 	}
 
@@ -1284,10 +1312,11 @@ mwifiex_cmd_802_11_subsc_evt(struct mwifiex_private *priv,
 		rssi_tlv->abs_value = subsc_evt_cfg->bcn_l_rssi_cfg.abs_value;
 		rssi_tlv->evt_freq = subsc_evt_cfg->bcn_l_rssi_cfg.evt_freq;
 
-		dev_dbg(priv->adapter->dev, "Cfg Beacon Low Rssi event, "
-			"RSSI:-%d dBm, Freq:%d\n",
-			subsc_evt_cfg->bcn_l_rssi_cfg.abs_value,
-			subsc_evt_cfg->bcn_l_rssi_cfg.evt_freq);
+		mwifiex_dbg(priv->adapter, EVENT,
+			    "Cfg Beacon Low Rssi event,\t"
+			    "RSSI:-%d dBm, Freq:%d\n",
+			    subsc_evt_cfg->bcn_l_rssi_cfg.abs_value,
+			    subsc_evt_cfg->bcn_l_rssi_cfg.evt_freq);
 
 		pos += sizeof(struct mwifiex_ie_types_rssi_threshold);
 		le16_add_cpu(&cmd->size,
@@ -1304,10 +1333,11 @@ mwifiex_cmd_802_11_subsc_evt(struct mwifiex_private *priv,
 		rssi_tlv->abs_value = subsc_evt_cfg->bcn_h_rssi_cfg.abs_value;
 		rssi_tlv->evt_freq = subsc_evt_cfg->bcn_h_rssi_cfg.evt_freq;
 
-		dev_dbg(priv->adapter->dev, "Cfg Beacon High Rssi event, "
-			"RSSI:-%d dBm, Freq:%d\n",
-			subsc_evt_cfg->bcn_h_rssi_cfg.abs_value,
-			subsc_evt_cfg->bcn_h_rssi_cfg.evt_freq);
+		mwifiex_dbg(priv->adapter, EVENT,
+			    "Cfg Beacon High Rssi event,\t"
+			    "RSSI:-%d dBm, Freq:%d\n",
+			    subsc_evt_cfg->bcn_h_rssi_cfg.abs_value,
+			    subsc_evt_cfg->bcn_h_rssi_cfg.evt_freq);
 
 		pos += sizeof(struct mwifiex_ie_types_rssi_threshold);
 		le16_add_cpu(&cmd->size,
@@ -1463,12 +1493,14 @@ static int mwifiex_cmd_cfg_data(struct mwifiex_private *priv,
 						data, len);
 		if (ret)
 			return ret;
-		dev_dbg(adapter->dev,
-			"download cfg_data from device tree: %s\n", prop->name);
+		mwifiex_dbg(adapter, INFO,
+			    "download cfg_data from device tree: %s\n",
+			    prop->name);
 	} else if (adapter->cal_data->data && adapter->cal_data->size > 0) {
 		len = mwifiex_parse_cal_cfg((u8 *)adapter->cal_data->data,
 					    adapter->cal_data->size, data);
-		dev_dbg(adapter->dev, "download cfg_data from config file\n");
+		mwifiex_dbg(adapter, INFO,
+			    "download cfg_data from config file\n");
 	} else {
 		return -1;
 	}
@@ -1583,9 +1615,9 @@ mwifiex_cmd_tdls_oper(struct mwifiex_private *priv,
 		tdls_oper->tdls_action = cpu_to_le16(ACT_TDLS_CONFIG);
 
 		if (!params) {
-			dev_err(priv->adapter->dev,
-				"TDLS config params not available for %pM\n",
-				oper->peer_mac);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "TDLS config params not available for %pM\n",
+				    oper->peer_mac);
 			return -ENODATA;
 		}
 
@@ -1663,7 +1695,7 @@ mwifiex_cmd_tdls_oper(struct mwifiex_private *priv,
 
 		break;
 	default:
-		dev_err(priv->adapter->dev, "Unknown TDLS operation\n");
+		mwifiex_dbg(priv->adapter, ERROR, "Unknown TDLS operation\n");
 		return -ENOTSUPP;
 	}
 
@@ -1870,8 +1902,8 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 		ret = mwifiex_cmd_11n_cfg(priv, cmd_ptr, cmd_action, data_buf);
 		break;
 	case HostCmd_CMD_WMM_GET_STATUS:
-		dev_dbg(priv->adapter->dev,
-			"cmd: WMM: WMM_GET_STATUS cmd sent\n");
+		mwifiex_dbg(priv->adapter, CMD,
+			    "cmd: WMM: WMM_GET_STATUS cmd sent\n");
 		cmd_ptr->command = cpu_to_le16(HostCmd_CMD_WMM_GET_STATUS);
 		cmd_ptr->size =
 			cpu_to_le16(sizeof(struct host_cmd_ds_wmm_get_status) +
@@ -1885,6 +1917,9 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 	case HostCmd_CMD_802_11_SCAN_EXT:
 		ret = mwifiex_cmd_802_11_scan_ext(priv, cmd_ptr, data_buf);
 		break;
+	case HostCmd_CMD_MEM_ACCESS:
+		ret = mwifiex_cmd_mem_access(cmd_ptr, cmd_action, data_buf);
+		break;
 	case HostCmd_CMD_MAC_REG_ACCESS:
 	case HostCmd_CMD_BBP_REG_ACCESS:
 	case HostCmd_CMD_RF_REG_ACCESS:
@@ -1932,8 +1967,8 @@ int mwifiex_sta_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
 						   data_buf);
 		break;
 	default:
-		dev_err(priv->adapter->dev,
-			"PREP_CMD: unknown cmd- %#x\n", cmd_no);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "PREP_CMD: unknown cmd- %#x\n", cmd_no);
 		ret = -1;
 		break;
 	}
@@ -2024,8 +2059,8 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta, bool init)
 					       &sdio_sp_rx_aggr_enable,
 					       true);
 			if (ret) {
-				dev_err(priv->adapter->dev,
-					"error while enabling SP aggregation..disable it");
+				mwifiex_dbg(priv->adapter, ERROR,
+					    "error while enabling SP aggregation..disable it");
 				adapter->sdio_rx_aggr_enable = false;
 			}
 		}
@@ -2108,8 +2143,8 @@ int mwifiex_sta_init_cmd(struct mwifiex_private *priv, u8 first_sta, bool init)
 				       HostCmd_ACT_GEN_SET, DOT11D_I,
 				       &state_11d, true);
 		if (ret)
-			dev_err(priv->adapter->dev,
-				"11D: failed to enable 11D\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "11D: failed to enable 11D\n");
 	}
 
 	/* Send cmd to FW to configure 11n specific configuration
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index 88dc6b672ef4..b645884b3b97 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -49,8 +49,9 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
 	struct host_cmd_ds_802_11_ps_mode_enh *pm;
 	unsigned long flags;
 
-	dev_err(adapter->dev, "CMD_RESP: cmd %#x error, result=%#x\n",
-		resp->command, resp->result);
+	mwifiex_dbg(adapter, ERROR,
+		    "CMD_RESP: cmd %#x error, result=%#x\n",
+		    resp->command, resp->result);
 
 	if (adapter->curr_cmd->wait_q_enabled)
 		adapter->cmd_wait_q.status = -1;
@@ -58,9 +59,9 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
 	switch (le16_to_cpu(resp->command)) {
 	case HostCmd_CMD_802_11_PS_MODE_ENH:
 		pm = &resp->params.psmode_enh;
-		dev_err(adapter->dev,
-			"PS_MODE_ENH cmd failed: result=0x%x action=0x%X\n",
-			resp->result, le16_to_cpu(pm->action));
+		mwifiex_dbg(adapter, ERROR,
+			    "PS_MODE_ENH cmd failed: result=0x%x action=0x%X\n",
+			    resp->result, le16_to_cpu(pm->action));
 		/* We do not re-try enter-ps command in ad-hoc mode. */
 		if (le16_to_cpu(pm->action) == EN_AUTO_PS &&
 		    (le16_to_cpu(pm->params.ps_bitmap) & BITMAP_STA_PS) &&
@@ -91,7 +92,8 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
 		break;
 
 	case HostCmd_CMD_SDIO_SP_RX_AGGR_CFG:
-		dev_err(priv->adapter->dev, "SDIO RX single-port aggregation Not support\n");
+		mwifiex_dbg(adapter, MSG,
+			    "SDIO RX single-port aggregation Not support\n");
 		break;
 
 	default:
@@ -187,29 +189,34 @@ static int mwifiex_ret_802_11_snmp_mib(struct mwifiex_private *priv,
 	u16 query_type = le16_to_cpu(smib->query_type);
 	u32 ul_temp;
 
-	dev_dbg(priv->adapter->dev, "info: SNMP_RESP: oid value = %#x,"
-		" query_type = %#x, buf size = %#x\n",
-		oid, query_type, le16_to_cpu(smib->buf_size));
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: SNMP_RESP: oid value = %#x,\t"
+		    "query_type = %#x, buf size = %#x\n",
+		    oid, query_type, le16_to_cpu(smib->buf_size));
 	if (query_type == HostCmd_ACT_GEN_GET) {
 		ul_temp = le16_to_cpu(*((__le16 *) (smib->value)));
 		if (data_buf)
 			*data_buf = ul_temp;
 		switch (oid) {
 		case FRAG_THRESH_I:
-			dev_dbg(priv->adapter->dev,
-				"info: SNMP_RESP: FragThsd =%u\n", ul_temp);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: SNMP_RESP: FragThsd =%u\n",
+				    ul_temp);
 			break;
 		case RTS_THRESH_I:
-			dev_dbg(priv->adapter->dev,
-				"info: SNMP_RESP: RTSThsd =%u\n", ul_temp);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: SNMP_RESP: RTSThsd =%u\n",
+				    ul_temp);
 			break;
 		case SHORT_RETRY_LIM_I:
-			dev_dbg(priv->adapter->dev,
-				"info: SNMP_RESP: TxRetryCount=%u\n", ul_temp);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: SNMP_RESP: TxRetryCount=%u\n",
+				    ul_temp);
 			break;
 		case DTIM_PERIOD_I:
-			dev_dbg(priv->adapter->dev,
-				"info: SNMP_RESP: DTIM period=%u\n", ul_temp);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: SNMP_RESP: DTIM period=%u\n",
+				    ul_temp);
 		default:
 			break;
 		}
@@ -426,14 +433,15 @@ static int mwifiex_ret_tx_power_cfg(struct mwifiex_private *priv,
 			priv->tx_power_level = (u16) pg->power_min;
 		break;
 	default:
-		dev_err(adapter->dev, "CMD_RESP: unknown cmd action %d\n",
-			action);
+		mwifiex_dbg(adapter, ERROR,
+			    "CMD_RESP: unknown cmd action %d\n",
+			    action);
 		return 0;
 	}
-	dev_dbg(adapter->dev,
-		"info: Current TxPower Level = %d, Max Power=%d, Min Power=%d\n",
-	       priv->tx_power_level, priv->max_tx_power_level,
-	       priv->min_tx_power_level);
+	mwifiex_dbg(adapter, INFO,
+		    "info: Current TxPower Level = %d, Max Power=%d, Min Power=%d\n",
+		    priv->tx_power_level, priv->max_tx_power_level,
+		    priv->min_tx_power_level);
 
 	return 0;
 }
@@ -454,10 +462,10 @@ static int mwifiex_ret_rf_tx_power(struct mwifiex_private *priv,
 		priv->min_tx_power_level = txp->min_power;
 	}
 
-	dev_dbg(priv->adapter->dev,
-		"Current TxPower Level=%d, Max Power=%d, Min Power=%d\n",
-		priv->tx_power_level, priv->max_tx_power_level,
-		priv->min_tx_power_level);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "Current TxPower Level=%d, Max Power=%d, Min Power=%d\n",
+		    priv->tx_power_level, priv->max_tx_power_level,
+		    priv->min_tx_power_level);
 
 	return 0;
 }
@@ -473,18 +481,18 @@ static int mwifiex_ret_rf_antenna(struct mwifiex_private *priv,
 	struct mwifiex_adapter *adapter = priv->adapter;
 
 	if (adapter->hw_dev_mcs_support == HT_STREAM_2X2)
-		dev_dbg(adapter->dev,
-			"RF_ANT_RESP: Tx action = 0x%x, Tx Mode = 0x%04x"
-			" Rx action = 0x%x, Rx Mode = 0x%04x\n",
-			le16_to_cpu(ant_mimo->action_tx),
-			le16_to_cpu(ant_mimo->tx_ant_mode),
-			le16_to_cpu(ant_mimo->action_rx),
-			le16_to_cpu(ant_mimo->rx_ant_mode));
+		mwifiex_dbg(adapter, INFO,
+			    "RF_ANT_RESP: Tx action = 0x%x, Tx Mode = 0x%04x\t"
+			    "Rx action = 0x%x, Rx Mode = 0x%04x\n",
+			    le16_to_cpu(ant_mimo->action_tx),
+			    le16_to_cpu(ant_mimo->tx_ant_mode),
+			    le16_to_cpu(ant_mimo->action_rx),
+			    le16_to_cpu(ant_mimo->rx_ant_mode));
 	else
-		dev_dbg(adapter->dev,
-			"RF_ANT_RESP: action = 0x%x, Mode = 0x%04x\n",
-			le16_to_cpu(ant_siso->action),
-			le16_to_cpu(ant_siso->ant_mode));
+		mwifiex_dbg(adapter, INFO,
+			    "RF_ANT_RESP: action = 0x%x, Mode = 0x%04x\n",
+			    le16_to_cpu(ant_siso->action),
+			    le16_to_cpu(ant_siso->ant_mode));
 
 	return 0;
 }
@@ -502,8 +510,8 @@ static int mwifiex_ret_802_11_mac_address(struct mwifiex_private *priv,
 
 	memcpy(priv->curr_addr, cmd_mac_addr->mac_addr, ETH_ALEN);
 
-	dev_dbg(priv->adapter->dev,
-		"info: set mac address: %pM\n", priv->curr_addr);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: set mac address: %pM\n", priv->curr_addr);
 
 	return 0;
 }
@@ -587,7 +595,8 @@ static int mwifiex_ret_802_11_key_material_v1(struct mwifiex_private *priv,
 
 	if (le16_to_cpu(key->action) == HostCmd_ACT_GEN_SET) {
 		if ((le16_to_cpu(key->key_param_set.key_info) & KEY_MCAST)) {
-			dev_dbg(priv->adapter->dev, "info: key: GTK is set\n");
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: key: GTK is set\n");
 			priv->wpa_is_gtk_set = true;
 			priv->scan_block = false;
 		}
@@ -617,7 +626,7 @@ static int mwifiex_ret_802_11_key_material_v2(struct mwifiex_private *priv,
 	key_v2 = &resp->params.key_material_v2;
 	if (le16_to_cpu(key_v2->action) == HostCmd_ACT_GEN_SET) {
 		if ((le16_to_cpu(key_v2->key_param_set.key_info) & KEY_MCAST)) {
-			dev_dbg(priv->adapter->dev, "info: key: GTK is set\n");
+			mwifiex_dbg(priv->adapter, INFO, "info: key: GTK is set\n");
 			priv->wpa_is_gtk_set = true;
 			priv->scan_block = false;
 		}
@@ -663,14 +672,14 @@ static int mwifiex_ret_802_11d_domain_info(struct mwifiex_private *priv,
 				- IEEE80211_COUNTRY_STRING_LEN)
 			      / sizeof(struct ieee80211_country_ie_triplet));
 
-	dev_dbg(priv->adapter->dev,
-		"info: 11D Domain Info Resp: no_of_triplet=%d\n",
-		no_of_triplet);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: 11D Domain Info Resp: no_of_triplet=%d\n",
+		    no_of_triplet);
 
 	if (no_of_triplet > MWIFIEX_MAX_TRIPLET_802_11D) {
-		dev_warn(priv->adapter->dev,
-			 "11D: invalid number of triplets %d returned\n",
-			 no_of_triplet);
+		mwifiex_dbg(priv->adapter, FATAL,
+			    "11D: invalid number of triplets %d returned\n",
+			    no_of_triplet);
 		return -1;
 	}
 
@@ -680,8 +689,8 @@ static int mwifiex_ret_802_11d_domain_info(struct mwifiex_private *priv,
 	case HostCmd_ACT_GEN_GET:
 		break;
 	default:
-		dev_err(priv->adapter->dev,
-			"11D: invalid action:%d\n", domain_info->action);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "11D: invalid action:%d\n", domain_info->action);
 		return -1;
 	}
 
@@ -741,6 +750,19 @@ mwifiex_ret_p2p_mode_cfg(struct mwifiex_private *priv,
 	return 0;
 }
 
+/* This function handles the command response of mem_access command
+ */
+static int
+mwifiex_ret_mem_access(struct mwifiex_private *priv,
+		       struct host_cmd_ds_command *resp, void *pioctl_buf)
+{
+	struct host_cmd_ds_mem_access *mem = (void *)&resp->params.mem;
+
+	priv->mem_rw.addr = le32_to_cpu(mem->addr);
+	priv->mem_rw.value = le32_to_cpu(mem->value);
+
+	return 0;
+}
 /*
  * This function handles the command response of register access.
  *
@@ -830,12 +852,12 @@ static int mwifiex_ret_ibss_coalescing_status(struct mwifiex_private *priv,
 	if (le16_to_cpu(ibss_coal_resp->action) == HostCmd_ACT_GEN_SET)
 		return 0;
 
-	dev_dbg(priv->adapter->dev,
-		"info: new BSSID %pM\n", ibss_coal_resp->bssid);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: new BSSID %pM\n", ibss_coal_resp->bssid);
 
 	/* If rsp has NULL BSSID, Just return..... No Action */
 	if (is_zero_ether_addr(ibss_coal_resp->bssid)) {
-		dev_warn(priv->adapter->dev, "new BSSID is NULL\n");
+		mwifiex_dbg(priv->adapter, FATAL, "new BSSID is NULL\n");
 		return 0;
 	}
 
@@ -871,48 +893,48 @@ static int mwifiex_ret_tdls_oper(struct mwifiex_private *priv,
 	case ACT_TDLS_DELETE:
 		if (reason) {
 			if (!node || reason == TDLS_ERR_LINK_NONEXISTENT)
-				dev_dbg(priv->adapter->dev,
-					"TDLS link delete for %pM failed: reason %d\n",
-					cmd_tdls_oper->peer_mac, reason);
+				mwifiex_dbg(priv->adapter, ERROR,
+					    "TDLS link delete for %pM failed: reason %d\n",
+					    cmd_tdls_oper->peer_mac, reason);
 			else
-				dev_err(priv->adapter->dev,
-					"TDLS link delete for %pM failed: reason %d\n",
-					cmd_tdls_oper->peer_mac, reason);
+				mwifiex_dbg(priv->adapter, ERROR,
+					    "TDLS link delete for %pM failed: reason %d\n",
+					    cmd_tdls_oper->peer_mac, reason);
 		} else {
-			dev_dbg(priv->adapter->dev,
-				"TDLS link delete for %pM successful\n",
-				cmd_tdls_oper->peer_mac);
+			mwifiex_dbg(priv->adapter, MSG,
+				    "TDLS link delete for %pM successful\n",
+				    cmd_tdls_oper->peer_mac);
 		}
 		break;
 	case ACT_TDLS_CREATE:
 		if (reason) {
-			dev_err(priv->adapter->dev,
-				"TDLS link creation for %pM failed: reason %d",
-				cmd_tdls_oper->peer_mac, reason);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "TDLS link creation for %pM failed: reason %d",
+				    cmd_tdls_oper->peer_mac, reason);
 			if (node && reason != TDLS_ERR_LINK_EXISTS)
 				node->tdls_status = TDLS_SETUP_FAILURE;
 		} else {
-			dev_dbg(priv->adapter->dev,
-				"TDLS link creation for %pM successful",
-				cmd_tdls_oper->peer_mac);
+			mwifiex_dbg(priv->adapter, MSG,
+				    "TDLS link creation for %pM successful",
+				    cmd_tdls_oper->peer_mac);
 		}
 		break;
 	case ACT_TDLS_CONFIG:
 		if (reason) {
-			dev_err(priv->adapter->dev,
-				"TDLS link config for %pM failed, reason %d\n",
-				cmd_tdls_oper->peer_mac, reason);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "TDLS link config for %pM failed, reason %d\n",
+				    cmd_tdls_oper->peer_mac, reason);
 			if (node)
 				node->tdls_status = TDLS_SETUP_FAILURE;
 		} else {
-			dev_dbg(priv->adapter->dev,
-				"TDLS link config for %pM successful\n",
-				cmd_tdls_oper->peer_mac);
+			mwifiex_dbg(priv->adapter, MSG,
+				    "TDLS link config for %pM successful\n",
+				    cmd_tdls_oper->peer_mac);
 		}
 		break;
 	default:
-		dev_err(priv->adapter->dev,
-			"Unknown TDLS command action response %d", action);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Unknown TDLS command action response %d", action);
 		return -1;
 	}
 
@@ -929,8 +951,30 @@ static int mwifiex_ret_subsc_evt(struct mwifiex_private *priv,
 
 	/* For every subscribe event command (Get/Set/Clear), FW reports the
 	 * current set of subscribed events*/
-	dev_dbg(priv->adapter->dev, "Bitmap of currently subscribed events: %16x\n",
-		le16_to_cpu(cmd_sub_event->events));
+	mwifiex_dbg(priv->adapter, EVENT,
+		    "Bitmap of currently subscribed events: %16x\n",
+		    le16_to_cpu(cmd_sub_event->events));
+
+	return 0;
+}
+
+static int mwifiex_ret_uap_sta_list(struct mwifiex_private *priv,
+				    struct host_cmd_ds_command *resp)
+{
+	struct host_cmd_ds_sta_list *sta_list =
+		&resp->params.sta_list;
+	struct mwifiex_ie_types_sta_info *sta_info = (void *)&sta_list->tlv;
+	int i;
+	struct mwifiex_sta_node *sta_node;
+
+	for (i = 0; i < sta_list->sta_count; i++) {
+		sta_node = mwifiex_get_sta_entry(priv, sta_info->mac);
+		if (unlikely(!sta_node))
+			continue;
+
+		sta_node->stats.rssi = sta_info->rssi;
+		sta_info++;
+	}
 
 	return 0;
 }
@@ -940,7 +984,7 @@ static int mwifiex_ret_cfg_data(struct mwifiex_private *priv,
 				struct host_cmd_ds_command *resp)
 {
 	if (resp->result != HostCmd_RESULT_OK) {
-		dev_err(priv->adapter->dev, "Cal data cmd resp failed\n");
+		mwifiex_dbg(priv->adapter, ERROR, "Cal data cmd resp failed\n");
 		return -1;
 	}
 
@@ -1008,8 +1052,8 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		break;
 	case HostCmd_CMD_802_11_BG_SCAN_QUERY:
 		ret = mwifiex_ret_802_11_scan(priv, resp);
-		dev_dbg(adapter->dev,
-			"info: CMD_RESP: BG_SCAN result is ready!\n");
+		mwifiex_dbg(adapter, CMD,
+			    "info: CMD_RESP: BG_SCAN result is ready!\n");
 		break;
 	case HostCmd_CMD_TXPWR_CFG:
 		ret = mwifiex_ret_tx_power_cfg(priv, resp);
@@ -1088,8 +1132,8 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 					/ MWIFIEX_SDIO_BLOCK_SIZE)
 				       * MWIFIEX_SDIO_BLOCK_SIZE;
 		adapter->curr_tx_buf_size = adapter->tx_buf_size;
-		dev_dbg(adapter->dev, "cmd: curr_tx_buf_size=%d\n",
-			adapter->curr_tx_buf_size);
+		mwifiex_dbg(adapter, CMD, "cmd: curr_tx_buf_size=%d\n",
+			    adapter->curr_tx_buf_size);
 
 		if (adapter->if_ops.update_mp_end_port)
 			adapter->if_ops.update_mp_end_port(adapter,
@@ -1103,6 +1147,9 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 	case HostCmd_CMD_802_11_IBSS_COALESCING_STATUS:
 		ret = mwifiex_ret_ibss_coalescing_status(priv, resp);
 		break;
+	case HostCmd_CMD_MEM_ACCESS:
+		ret = mwifiex_ret_mem_access(priv, resp, data_buf);
+		break;
 	case HostCmd_CMD_MAC_REG_ACCESS:
 	case HostCmd_CMD_BBP_REG_ACCESS:
 	case HostCmd_CMD_RF_REG_ACCESS:
@@ -1122,6 +1169,9 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		break;
 	case HostCmd_CMD_UAP_SYS_CONFIG:
 		break;
+	case HOST_CMD_APCMD_STA_LIST:
+		ret = mwifiex_ret_uap_sta_list(priv, resp);
+		break;
 	case HostCmd_CMD_UAP_BSS_START:
 		adapter->tx_lock_flag = false;
 		adapter->pps_uapsd_mode = false;
@@ -1133,6 +1183,8 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		break;
 	case HostCmd_CMD_UAP_STA_DEAUTH:
 		break;
+	case HOST_CMD_APCMD_SYS_RESET:
+		break;
 	case HostCmd_CMD_MEF_CFG:
 		break;
 	case HostCmd_CMD_COALESCE_CFG:
@@ -1146,8 +1198,9 @@ int mwifiex_process_sta_cmdresp(struct mwifiex_private *priv, u16 cmdresp_no,
 		ret = mwifiex_ret_sdio_rx_aggr_cfg(priv, resp);
 		break;
 	default:
-		dev_err(adapter->dev, "CMD_RESP: unknown cmd response %#x\n",
-			resp->command);
+		mwifiex_dbg(adapter, ERROR,
+			    "CMD_RESP: unknown cmd response %#x\n",
+			    resp->command);
 		break;
 	}
 
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/mwifiex/sta_event.c
index 0dc7a1d3993d..848de2621958 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/mwifiex/sta_event.c
@@ -48,7 +48,8 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 	if (!priv->media_connected)
 		return;
 
-	dev_dbg(adapter->dev, "info: handles disconnect event\n");
+	mwifiex_dbg(adapter, INFO,
+		    "info: handles disconnect event\n");
 
 	priv->media_connected = false;
 
@@ -104,12 +105,14 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 	 * it could be used for re-assoc
 	 */
 
-	dev_dbg(adapter->dev, "info: previous SSID=%s, SSID len=%u\n",
-		priv->prev_ssid.ssid, priv->prev_ssid.ssid_len);
+	mwifiex_dbg(adapter, INFO,
+		    "info: previous SSID=%s, SSID len=%u\n",
+		    priv->prev_ssid.ssid, priv->prev_ssid.ssid_len);
 
-	dev_dbg(adapter->dev, "info: current SSID=%s, SSID len=%u\n",
-		priv->curr_bss_params.bss_descriptor.ssid.ssid,
-		priv->curr_bss_params.bss_descriptor.ssid.ssid_len);
+	mwifiex_dbg(adapter, INFO,
+		    "info: current SSID=%s, SSID len=%u\n",
+		    priv->curr_bss_params.bss_descriptor.ssid.ssid,
+		    priv->curr_bss_params.bss_descriptor.ssid.ssid_len);
 
 	memcpy(&priv->prev_ssid,
 	       &priv->curr_bss_params.bss_descriptor.ssid,
@@ -127,13 +130,13 @@ mwifiex_reset_connect_state(struct mwifiex_private *priv, u16 reason_code)
 	if (adapter->is_cmd_timedout && adapter->curr_cmd)
 		return;
 	priv->media_connected = false;
-	dev_dbg(adapter->dev,
-		"info: successfully disconnected from %pM: reason code %d\n",
-		priv->cfg_bssid, reason_code);
+	mwifiex_dbg(adapter, MSG,
+		    "info: successfully disconnected from %pM: reason code %d\n",
+		    priv->cfg_bssid, reason_code);
 	if (priv->bss_mode == NL80211_IFTYPE_STATION ||
 	    priv->bss_mode == NL80211_IFTYPE_P2P_CLIENT) {
 		cfg80211_disconnected(priv->netdev, reason_code, NULL, 0,
-				      GFP_KERNEL);
+				      false, GFP_KERNEL);
 	}
 	eth_zero_addr(priv->cfg_bssid);
 
@@ -154,13 +157,13 @@ static int mwifiex_parse_tdls_event(struct mwifiex_private *priv,
 	/* reserved 2 bytes are not mandatory in tdls event */
 	if (event_skb->len < (sizeof(struct mwifiex_tdls_generic_event) -
 			      sizeof(u16) - sizeof(adapter->event_cause))) {
-		dev_err(adapter->dev, "Invalid event length!\n");
+		mwifiex_dbg(adapter, ERROR, "Invalid event length!\n");
 		return -1;
 	}
 
 	sta_ptr = mwifiex_get_sta_entry(priv, tdls_evt->peer_mac);
 	if (!sta_ptr) {
-		dev_err(adapter->dev, "cannot get sta entry!\n");
+		mwifiex_dbg(adapter, ERROR, "cannot get sta entry!\n");
 		return -1;
 	}
 
@@ -180,6 +183,63 @@ static int mwifiex_parse_tdls_event(struct mwifiex_private *priv,
 }
 
 /*
+* This function handles coex events generated by firmware
+*/
+void mwifiex_bt_coex_wlan_param_update_event(struct mwifiex_private *priv,
+					     struct sk_buff *event_skb)
+{
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_ie_types_header *tlv;
+	struct mwifiex_ie_types_btcoex_aggr_win_size *winsizetlv;
+	struct mwifiex_ie_types_btcoex_scan_time *scantlv;
+	s32 len = event_skb->len - sizeof(u32);
+	u8 *cur_ptr = event_skb->data + sizeof(u32);
+	u16 tlv_type, tlv_len;
+
+	while (len >= sizeof(struct mwifiex_ie_types_header)) {
+		tlv = (struct mwifiex_ie_types_header *)cur_ptr;
+		tlv_len = le16_to_cpu(tlv->len);
+		tlv_type = le16_to_cpu(tlv->type);
+
+		if ((tlv_len + sizeof(struct mwifiex_ie_types_header)) > len)
+			break;
+		switch (tlv_type) {
+		case TLV_BTCOEX_WL_AGGR_WINSIZE:
+			winsizetlv =
+			    (struct mwifiex_ie_types_btcoex_aggr_win_size *)tlv;
+			adapter->coex_win_size = winsizetlv->coex_win_size;
+			adapter->coex_tx_win_size =
+				winsizetlv->tx_win_size;
+			adapter->coex_rx_win_size =
+				winsizetlv->rx_win_size;
+			mwifiex_coex_ampdu_rxwinsize(adapter);
+			mwifiex_update_ampdu_txwinsize(adapter);
+			break;
+
+		case TLV_BTCOEX_WL_SCANTIME:
+			scantlv =
+			    (struct mwifiex_ie_types_btcoex_scan_time *)tlv;
+			adapter->coex_scan = scantlv->coex_scan;
+			adapter->coex_min_scan_time = scantlv->min_scan_time;
+			adapter->coex_max_scan_time = scantlv->max_scan_time;
+			break;
+
+		default:
+			break;
+		}
+
+		len -= tlv_len + sizeof(struct mwifiex_ie_types_header);
+		cur_ptr += tlv_len +
+			sizeof(struct mwifiex_ie_types_header);
+	}
+
+	dev_dbg(adapter->dev, "coex_scan=%d min_scan=%d coex_win=%d, tx_win=%d rx_win=%d\n",
+		adapter->coex_scan, adapter->coex_min_scan_time,
+		adapter->coex_win_size, adapter->coex_tx_win_size,
+		adapter->coex_rx_win_size);
+}
+
+/*
  * This function handles events generated by firmware.
  *
  * This is a generic function and handles all events.
@@ -239,21 +299,21 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 
 	switch (eventcause) {
 	case EVENT_DUMMY_HOST_WAKEUP_SIGNAL:
-		dev_err(adapter->dev,
-			"invalid EVENT: DUMMY_HOST_WAKEUP_SIGNAL, ignore it\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "invalid EVENT: DUMMY_HOST_WAKEUP_SIGNAL, ignore it\n");
 		break;
 	case EVENT_LINK_SENSED:
-		dev_dbg(adapter->dev, "event: LINK_SENSED\n");
+		mwifiex_dbg(adapter, EVENT, "event: LINK_SENSED\n");
 		if (!netif_carrier_ok(priv->netdev))
 			netif_carrier_on(priv->netdev);
 		mwifiex_wake_up_net_dev_queue(priv->netdev, adapter);
 		break;
 
 	case EVENT_DEAUTHENTICATED:
-		dev_dbg(adapter->dev, "event: Deauthenticated\n");
+		mwifiex_dbg(adapter, EVENT, "event: Deauthenticated\n");
 		if (priv->wps.session_enable) {
-			dev_dbg(adapter->dev,
-				"info: receive deauth event in wps session\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: receive deauth event in wps session\n");
 			break;
 		}
 		adapter->dbg.num_event_deauth++;
@@ -265,10 +325,10 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_DISASSOCIATED:
-		dev_dbg(adapter->dev, "event: Disassociated\n");
+		mwifiex_dbg(adapter, EVENT, "event: Disassociated\n");
 		if (priv->wps.session_enable) {
-			dev_dbg(adapter->dev,
-				"info: receive disassoc event in wps session\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: receive disassoc event in wps session\n");
 			break;
 		}
 		adapter->dbg.num_event_disassoc++;
@@ -280,7 +340,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_LINK_LOST:
-		dev_dbg(adapter->dev, "event: Link lost\n");
+		mwifiex_dbg(adapter, EVENT, "event: Link lost\n");
 		adapter->dbg.num_event_link_lost++;
 		if (priv->media_connected) {
 			reason_code =
@@ -290,7 +350,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_PS_SLEEP:
-		dev_dbg(adapter->dev, "info: EVENT: SLEEP\n");
+		mwifiex_dbg(adapter, EVENT, "info: EVENT: SLEEP\n");
 
 		adapter->ps_state = PS_STATE_PRE_SLEEP;
 
@@ -298,12 +358,12 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_PS_AWAKE:
-		dev_dbg(adapter->dev, "info: EVENT: AWAKE\n");
+		mwifiex_dbg(adapter, EVENT, "info: EVENT: AWAKE\n");
 		if (!adapter->pps_uapsd_mode &&
 		    priv->media_connected && adapter->sleep_period.period) {
 				adapter->pps_uapsd_mode = true;
-				dev_dbg(adapter->dev,
-					"event: PPS/UAPSD mode activated\n");
+				mwifiex_dbg(adapter, EVENT,
+					    "event: PPS/UAPSD mode activated\n");
 		}
 		adapter->tx_lock_flag = false;
 		if (adapter->pps_uapsd_mode && adapter->gen_null_pkt) {
@@ -333,26 +393,26 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 
 	case EVENT_DEEP_SLEEP_AWAKE:
 		adapter->if_ops.wakeup_complete(adapter);
-		dev_dbg(adapter->dev, "event: DS_AWAKE\n");
+		mwifiex_dbg(adapter, EVENT, "event: DS_AWAKE\n");
 		if (adapter->is_deep_sleep)
 			adapter->is_deep_sleep = false;
 		break;
 
 	case EVENT_HS_ACT_REQ:
-		dev_dbg(adapter->dev, "event: HS_ACT_REQ\n");
+		mwifiex_dbg(adapter, EVENT, "event: HS_ACT_REQ\n");
 		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_HS_CFG_ENH,
 				       0, 0, NULL, false);
 		break;
 
 	case EVENT_MIC_ERR_UNICAST:
-		dev_dbg(adapter->dev, "event: UNICAST MIC ERROR\n");
+		mwifiex_dbg(adapter, EVENT, "event: UNICAST MIC ERROR\n");
 		cfg80211_michael_mic_failure(priv->netdev, priv->cfg_bssid,
 					     NL80211_KEYTYPE_PAIRWISE,
 					     -1, NULL, GFP_KERNEL);
 		break;
 
 	case EVENT_MIC_ERR_MULTICAST:
-		dev_dbg(adapter->dev, "event: MULTICAST MIC ERROR\n");
+		mwifiex_dbg(adapter, EVENT, "event: MULTICAST MIC ERROR\n");
 		cfg80211_michael_mic_failure(priv->netdev, priv->cfg_bssid,
 					     NL80211_KEYTYPE_GROUP,
 					     -1, NULL, GFP_KERNEL);
@@ -362,7 +422,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_ADHOC_BCN_LOST:
-		dev_dbg(adapter->dev, "event: ADHOC_BCN_LOST\n");
+		mwifiex_dbg(adapter, EVENT, "event: ADHOC_BCN_LOST\n");
 		priv->adhoc_is_link_sensed = false;
 		mwifiex_clean_txrx(priv);
 		mwifiex_stop_net_dev_queue(priv->netdev, adapter);
@@ -371,17 +431,17 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_BG_SCAN_REPORT:
-		dev_dbg(adapter->dev, "event: BGS_REPORT\n");
+		mwifiex_dbg(adapter, EVENT, "event: BGS_REPORT\n");
 		ret = mwifiex_send_cmd(priv, HostCmd_CMD_802_11_BG_SCAN_QUERY,
 				       HostCmd_ACT_GEN_GET, 0, NULL, false);
 		break;
 
 	case EVENT_PORT_RELEASE:
-		dev_dbg(adapter->dev, "event: PORT RELEASE\n");
+		mwifiex_dbg(adapter, EVENT, "event: PORT RELEASE\n");
 		break;
 
 	case EVENT_EXT_SCAN_REPORT:
-		dev_dbg(adapter->dev, "event: EXT_SCAN Report\n");
+		mwifiex_dbg(adapter, EVENT, "event: EXT_SCAN Report\n");
 		if (adapter->ext_scan)
 			ret = mwifiex_handle_event_ext_scan_report(priv,
 						adapter->event_skb->data);
@@ -389,7 +449,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_WMM_STATUS_CHANGE:
-		dev_dbg(adapter->dev, "event: WMM status changed\n");
+		mwifiex_dbg(adapter, EVENT, "event: WMM status changed\n");
 		ret = mwifiex_send_cmd(priv, HostCmd_CMD_WMM_GET_STATUS,
 				       0, 0, NULL, false);
 		break;
@@ -401,13 +461,13 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		mwifiex_send_cmd(priv, HostCmd_CMD_RSSI_INFO,
 				 HostCmd_ACT_GEN_GET, 0, NULL, false);
 		priv->subsc_evt_rssi_state = RSSI_LOW_RECVD;
-		dev_dbg(adapter->dev, "event: Beacon RSSI_LOW\n");
+		mwifiex_dbg(adapter, EVENT, "event: Beacon RSSI_LOW\n");
 		break;
 	case EVENT_SNR_LOW:
-		dev_dbg(adapter->dev, "event: Beacon SNR_LOW\n");
+		mwifiex_dbg(adapter, EVENT, "event: Beacon SNR_LOW\n");
 		break;
 	case EVENT_MAX_FAIL:
-		dev_dbg(adapter->dev, "event: MAX_FAIL\n");
+		mwifiex_dbg(adapter, EVENT, "event: MAX_FAIL\n");
 		break;
 	case EVENT_RSSI_HIGH:
 		cfg80211_cqm_rssi_notify(priv->netdev,
@@ -416,47 +476,47 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		mwifiex_send_cmd(priv, HostCmd_CMD_RSSI_INFO,
 				 HostCmd_ACT_GEN_GET, 0, NULL, false);
 		priv->subsc_evt_rssi_state = RSSI_HIGH_RECVD;
-		dev_dbg(adapter->dev, "event: Beacon RSSI_HIGH\n");
+		mwifiex_dbg(adapter, EVENT, "event: Beacon RSSI_HIGH\n");
 		break;
 	case EVENT_SNR_HIGH:
-		dev_dbg(adapter->dev, "event: Beacon SNR_HIGH\n");
+		mwifiex_dbg(adapter, EVENT, "event: Beacon SNR_HIGH\n");
 		break;
 	case EVENT_DATA_RSSI_LOW:
-		dev_dbg(adapter->dev, "event: Data RSSI_LOW\n");
+		mwifiex_dbg(adapter, EVENT, "event: Data RSSI_LOW\n");
 		break;
 	case EVENT_DATA_SNR_LOW:
-		dev_dbg(adapter->dev, "event: Data SNR_LOW\n");
+		mwifiex_dbg(adapter, EVENT, "event: Data SNR_LOW\n");
 		break;
 	case EVENT_DATA_RSSI_HIGH:
-		dev_dbg(adapter->dev, "event: Data RSSI_HIGH\n");
+		mwifiex_dbg(adapter, EVENT, "event: Data RSSI_HIGH\n");
 		break;
 	case EVENT_DATA_SNR_HIGH:
-		dev_dbg(adapter->dev, "event: Data SNR_HIGH\n");
+		mwifiex_dbg(adapter, EVENT, "event: Data SNR_HIGH\n");
 		break;
 	case EVENT_LINK_QUALITY:
-		dev_dbg(adapter->dev, "event: Link Quality\n");
+		mwifiex_dbg(adapter, EVENT, "event: Link Quality\n");
 		break;
 	case EVENT_PRE_BEACON_LOST:
-		dev_dbg(adapter->dev, "event: Pre-Beacon Lost\n");
+		mwifiex_dbg(adapter, EVENT, "event: Pre-Beacon Lost\n");
 		break;
 	case EVENT_IBSS_COALESCED:
-		dev_dbg(adapter->dev, "event: IBSS_COALESCED\n");
+		mwifiex_dbg(adapter, EVENT, "event: IBSS_COALESCED\n");
 		ret = mwifiex_send_cmd(priv,
 				HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
 				HostCmd_ACT_GEN_GET, 0, NULL, false);
 		break;
 	case EVENT_ADDBA:
-		dev_dbg(adapter->dev, "event: ADDBA Request\n");
+		mwifiex_dbg(adapter, EVENT, "event: ADDBA Request\n");
 		mwifiex_send_cmd(priv, HostCmd_CMD_11N_ADDBA_RSP,
 				 HostCmd_ACT_GEN_SET, 0,
 				 adapter->event_body, false);
 		break;
 	case EVENT_DELBA:
-		dev_dbg(adapter->dev, "event: DELBA Request\n");
+		mwifiex_dbg(adapter, EVENT, "event: DELBA Request\n");
 		mwifiex_11n_delete_ba_stream(priv, adapter->event_body);
 		break;
 	case EVENT_BA_STREAM_TIEMOUT:
-		dev_dbg(adapter->dev, "event:  BA Stream timeout\n");
+		mwifiex_dbg(adapter, EVENT, "event:  BA Stream timeout\n");
 		mwifiex_11n_ba_stream_timeout(priv,
 					      (struct host_cmd_ds_11n_batimeout
 					       *)
@@ -464,28 +524,31 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 	case EVENT_AMSDU_AGGR_CTRL:
 		ctrl = le16_to_cpu(*(__le16 *)adapter->event_body);
-		dev_dbg(adapter->dev, "event: AMSDU_AGGR_CTRL %d\n", ctrl);
+		mwifiex_dbg(adapter, EVENT,
+			    "event: AMSDU_AGGR_CTRL %d\n", ctrl);
 
 		adapter->tx_buf_size =
 				min_t(u16, adapter->curr_tx_buf_size, ctrl);
-		dev_dbg(adapter->dev, "event: tx_buf_size %d\n",
-			adapter->tx_buf_size);
+		mwifiex_dbg(adapter, EVENT, "event: tx_buf_size %d\n",
+			    adapter->tx_buf_size);
 		break;
 
 	case EVENT_WEP_ICV_ERR:
-		dev_dbg(adapter->dev, "event: WEP ICV error\n");
+		mwifiex_dbg(adapter, EVENT, "event: WEP ICV error\n");
 		break;
 
 	case EVENT_BW_CHANGE:
-		dev_dbg(adapter->dev, "event: BW Change\n");
+		mwifiex_dbg(adapter, EVENT, "event: BW Change\n");
 		break;
 
 	case EVENT_HOSTWAKE_STAIE:
-		dev_dbg(adapter->dev, "event: HOSTWAKE_STAIE %d\n", eventcause);
+		mwifiex_dbg(adapter, EVENT,
+			    "event: HOSTWAKE_STAIE %d\n", eventcause);
 		break;
 
 	case EVENT_REMAIN_ON_CHAN_EXPIRED:
-		dev_dbg(adapter->dev, "event: Remain on channel expired\n");
+		mwifiex_dbg(adapter, EVENT,
+			    "event: Remain on channel expired\n");
 		cfg80211_remain_on_channel_expired(&priv->wdev,
 						   priv->roc_cfg.cookie,
 						   &priv->roc_cfg.chan,
@@ -496,7 +559,7 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_CHANNEL_SWITCH_ANN:
-		dev_dbg(adapter->dev, "event: Channel Switch Announcement\n");
+		mwifiex_dbg(adapter, EVENT, "event: Channel Switch Announcement\n");
 		priv->csa_expire_time =
 				jiffies + msecs_to_jiffies(DFS_CHAN_MOVE_TIME);
 		priv->csa_chan = priv->curr_bss_params.bss_descriptor.channel;
@@ -511,23 +574,28 @@ int mwifiex_process_sta_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_TX_STATUS_REPORT:
-		dev_dbg(adapter->dev, "event: TX_STATUS Report\n");
+		mwifiex_dbg(adapter, EVENT, "event: TX_STATUS Report\n");
 		mwifiex_parse_tx_status_event(priv, adapter->event_body);
 		break;
 
 	case EVENT_CHANNEL_REPORT_RDY:
-		dev_dbg(adapter->dev, "event: Channel Report\n");
+		mwifiex_dbg(adapter, EVENT, "event: Channel Report\n");
 		ret = mwifiex_11h_handle_chanrpt_ready(priv,
 						       adapter->event_skb);
 		break;
 	case EVENT_RADAR_DETECTED:
-		dev_dbg(adapter->dev, "event: Radar detected\n");
+		mwifiex_dbg(adapter, EVENT, "event: Radar detected\n");
 		ret = mwifiex_11h_handle_radar_detected(priv,
 							adapter->event_skb);
 		break;
+	case EVENT_BT_COEX_WLAN_PARA_CHANGE:
+		dev_dbg(adapter->dev, "EVENT: BT coex wlan param update\n");
+		mwifiex_bt_coex_wlan_param_update_event(priv,
+							adapter->event_skb);
+		break;
 	default:
-		dev_dbg(adapter->dev, "event: unknown event id: %#x\n",
-			eventcause);
+		mwifiex_dbg(adapter, ERROR, "event: unknown event id: %#x\n",
+			    eventcause);
 		break;
 	}
 
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index 0599e41e253c..d8b7d9c20450 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -64,7 +64,10 @@ int mwifiex_wait_queue_complete(struct mwifiex_adapter *adapter,
 						  *(cmd_queued->condition),
 						  (12 * HZ));
 	if (status <= 0) {
-		dev_err(adapter->dev, "cmd_wait_q terminated: %d\n", status);
+		if (status == 0)
+			status = -ETIMEDOUT;
+		mwifiex_dbg(adapter, ERROR,
+			    "cmd_wait_q terminated: %d\n", status);
 		mwifiex_cancel_all_pending_cmd(adapter);
 		return status;
 	}
@@ -91,7 +94,8 @@ int mwifiex_request_set_multicast_list(struct mwifiex_private *priv,
 	old_pkt_filter = priv->curr_pkt_filter;
 
 	if (mcast_list->mode == MWIFIEX_PROMISC_MODE) {
-		dev_dbg(priv->adapter->dev, "info: Enable Promiscuous mode\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: Enable Promiscuous mode\n");
 		priv->curr_pkt_filter |= HostCmd_ACT_MAC_PROMISCUOUS_ENABLE;
 		priv->curr_pkt_filter &=
 			~HostCmd_ACT_MAC_ALL_MULTICAST_ENABLE;
@@ -99,16 +103,16 @@ int mwifiex_request_set_multicast_list(struct mwifiex_private *priv,
 		/* Multicast */
 		priv->curr_pkt_filter &= ~HostCmd_ACT_MAC_PROMISCUOUS_ENABLE;
 		if (mcast_list->mode == MWIFIEX_ALL_MULTI_MODE) {
-			dev_dbg(priv->adapter->dev,
-				"info: Enabling All Multicast!\n");
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: Enabling All Multicast!\n");
 			priv->curr_pkt_filter |=
 				HostCmd_ACT_MAC_ALL_MULTICAST_ENABLE;
 		} else {
 			priv->curr_pkt_filter &=
 				~HostCmd_ACT_MAC_ALL_MULTICAST_ENABLE;
-			dev_dbg(priv->adapter->dev,
-				"info: Set multicast list=%d\n",
-				mcast_list->num_multicast_addr);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: Set multicast list=%d\n",
+				    mcast_list->num_multicast_addr);
 			/* Send multicast addresses to firmware */
 			ret = mwifiex_send_cmd(priv,
 					       HostCmd_CMD_MAC_MULTICAST_ADR,
@@ -116,9 +120,9 @@ int mwifiex_request_set_multicast_list(struct mwifiex_private *priv,
 					       mcast_list, false);
 		}
 	}
-	dev_dbg(priv->adapter->dev,
-		"info: old_pkt_filter=%#x, curr_pkt_filter=%#x\n",
-	       old_pkt_filter, priv->curr_pkt_filter);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: old_pkt_filter=%#x, curr_pkt_filter=%#x\n",
+		    old_pkt_filter, priv->curr_pkt_filter);
 	if (old_pkt_filter != priv->curr_pkt_filter) {
 		ret = mwifiex_send_cmd(priv, HostCmd_CMD_MAC_CONTROL,
 				       HostCmd_ACT_GEN_SET,
@@ -151,7 +155,8 @@ int mwifiex_fill_new_bss_desc(struct mwifiex_private *priv,
 	rcu_read_unlock();
 
 	if (!beacon_ie) {
-		dev_err(priv->adapter->dev, " failed to alloc beacon_ie\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    " failed to alloc beacon_ie\n");
 		return -ENOMEM;
 	}
 
@@ -165,7 +170,8 @@ int mwifiex_fill_new_bss_desc(struct mwifiex_private *priv,
 	bss_desc->bss_band = bss_priv->band;
 	bss_desc->fw_tsf = bss_priv->fw_tsf;
 	if (bss_desc->cap_info_bitmap & WLAN_CAPABILITY_PRIVACY) {
-		dev_dbg(priv->adapter->dev, "info: InterpretIE: AP WEP enabled\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: InterpretIE: AP WEP enabled\n");
 		bss_desc->privacy = MWIFIEX_802_11_PRIV_FILTER_8021X_WEP;
 	} else {
 		bss_desc->privacy = MWIFIEX_802_11_PRIV_FILTER_ACCEPT_ALL;
@@ -219,8 +225,8 @@ static int mwifiex_process_country_ie(struct mwifiex_private *priv,
 
 	if (!strncmp(priv->adapter->country_code, &country_ie[2], 2)) {
 		rcu_read_unlock();
-		wiphy_dbg(priv->wdev.wiphy,
-			  "11D: skip setting domain info in FW\n");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "11D: skip setting domain info in FW\n");
 		return 0;
 	}
 	memcpy(priv->adapter->country_code, &country_ie[2], 2);
@@ -241,8 +247,8 @@ static int mwifiex_process_country_ie(struct mwifiex_private *priv,
 
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_802_11D_DOMAIN_INFO,
 			     HostCmd_ACT_GEN_SET, 0, NULL, false)) {
-		wiphy_err(priv->adapter->wiphy,
-			  "11D: setting domain info in FW\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "11D: setting domain info in FW fail\n");
 		return -1;
 	}
 
@@ -304,14 +310,15 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 
 		if (mwifiex_11h_get_csa_closed_channel(priv) ==
 							(u8)bss_desc->channel) {
-			dev_err(adapter->dev,
-				"Attempt to reconnect on csa closed chan(%d)\n",
-				bss_desc->channel);
+			mwifiex_dbg(adapter, ERROR,
+				    "Attempt to reconnect on csa closed chan(%d)\n",
+				    bss_desc->channel);
 			goto done;
 		}
 
-		dev_dbg(adapter->dev, "info: SSID found in scan list ... "
-				      "associating...\n");
+		mwifiex_dbg(adapter, INFO,
+			    "info: SSID found in scan list ...\t"
+			    "associating...\n");
 
 		mwifiex_stop_net_dev_queue(priv->netdev, adapter);
 		if (netif_carrier_ok(priv->netdev))
@@ -353,15 +360,17 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 			netif_carrier_off(priv->netdev);
 
 		if (!ret) {
-			dev_dbg(adapter->dev, "info: network found in scan"
-							" list. Joining...\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: network found in scan\t"
+				    " list. Joining...\n");
 			ret = mwifiex_adhoc_join(priv, bss_desc);
 			if (bss)
 				cfg80211_put_bss(priv->adapter->wiphy, bss);
 		} else {
-			dev_dbg(adapter->dev, "info: Network not found in "
-				"the list, creating adhoc with ssid = %s\n",
-				req_ssid->ssid);
+			mwifiex_dbg(adapter, INFO,
+				    "info: Network not found in\t"
+				    "the list, creating adhoc with ssid = %s\n",
+				    req_ssid->ssid);
 			ret = mwifiex_adhoc_start(priv, req_ssid);
 		}
 	}
@@ -396,8 +405,9 @@ int mwifiex_set_hs_params(struct mwifiex_private *priv, u16 action,
 	switch (action) {
 	case HostCmd_ACT_GEN_SET:
 		if (adapter->pps_uapsd_mode) {
-			dev_dbg(adapter->dev, "info: Host Sleep IOCTL"
-				" is blocked in UAPSD/PPS mode\n");
+			mwifiex_dbg(adapter, INFO,
+				    "info: Host Sleep IOCTL\t"
+				    "is blocked in UAPSD/PPS mode\n");
 			status = -1;
 			break;
 		}
@@ -494,7 +504,8 @@ int mwifiex_enable_hs(struct mwifiex_adapter *adapter)
 	}
 
 	if (adapter->hs_activated) {
-		dev_dbg(adapter->dev, "cmd: HS Already activated\n");
+		mwifiex_dbg(adapter, CMD,
+			    "cmd: HS Already activated\n");
 		return true;
 	}
 
@@ -510,14 +521,16 @@ int mwifiex_enable_hs(struct mwifiex_adapter *adapter)
 						   MWIFIEX_BSS_ROLE_STA),
 				  HostCmd_ACT_GEN_SET, MWIFIEX_SYNC_CMD,
 				  &hscfg)) {
-		dev_err(adapter->dev, "IOCTL request HS enable failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "IOCTL request HS enable failed\n");
 		return false;
 	}
 
 	if (wait_event_interruptible_timeout(adapter->hs_activate_wait_q,
 					     adapter->hs_activate_wait_q_woken,
 					     (10 * HZ)) <= 0) {
-		dev_err(adapter->dev, "hs_activate_wait_q terminated\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "hs_activate_wait_q terminated\n");
 		return false;
 	}
 
@@ -637,10 +650,11 @@ int mwifiex_set_tx_power(struct mwifiex_private *priv,
 		dbm = (u16) power_cfg->power_level;
 		if ((dbm < priv->min_tx_power_level) ||
 		    (dbm > priv->max_tx_power_level)) {
-			dev_err(priv->adapter->dev, "txpower value %d dBm"
-				" is out of range (%d dBm-%d dBm)\n",
-				dbm, priv->min_tx_power_level,
-				priv->max_tx_power_level);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "txpower value %d dBm\t"
+				    "is out of range (%d dBm-%d dBm)\n",
+				    dbm, priv->min_tx_power_level,
+				    priv->max_tx_power_level);
 			return -1;
 		}
 	}
@@ -739,14 +753,15 @@ static int mwifiex_set_wpa_ie_helper(struct mwifiex_private *priv,
 {
 	if (ie_len) {
 		if (ie_len > sizeof(priv->wpa_ie)) {
-			dev_err(priv->adapter->dev,
-				"failed to copy WPA IE, too big\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "failed to copy WPA IE, too big\n");
 			return -1;
 		}
 		memcpy(priv->wpa_ie, ie_data_ptr, ie_len);
 		priv->wpa_ie_len = (u8) ie_len;
-		dev_dbg(priv->adapter->dev, "cmd: Set Wpa_ie_len=%d IE=%#x\n",
-			priv->wpa_ie_len, priv->wpa_ie[0]);
+		mwifiex_dbg(priv->adapter, CMD,
+			    "cmd: Set Wpa_ie_len=%d IE=%#x\n",
+			    priv->wpa_ie_len, priv->wpa_ie[0]);
 
 		if (priv->wpa_ie[0] == WLAN_EID_VENDOR_SPECIFIC) {
 			priv->sec_info.wpa_enabled = true;
@@ -759,8 +774,9 @@ static int mwifiex_set_wpa_ie_helper(struct mwifiex_private *priv,
 	} else {
 		memset(priv->wpa_ie, 0, sizeof(priv->wpa_ie));
 		priv->wpa_ie_len = 0;
-		dev_dbg(priv->adapter->dev, "info: reset wpa_ie_len=%d IE=%#x\n",
-			priv->wpa_ie_len, priv->wpa_ie[0]);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: reset wpa_ie_len=%d IE=%#x\n",
+			    priv->wpa_ie_len, priv->wpa_ie[0]);
 		priv->sec_info.wpa_enabled = false;
 		priv->sec_info.wpa2_enabled = false;
 	}
@@ -780,23 +796,24 @@ static int mwifiex_set_wapi_ie(struct mwifiex_private *priv,
 {
 	if (ie_len) {
 		if (ie_len > sizeof(priv->wapi_ie)) {
-			dev_dbg(priv->adapter->dev,
-				"info: failed to copy WAPI IE, too big\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "info: failed to copy WAPI IE, too big\n");
 			return -1;
 		}
 		memcpy(priv->wapi_ie, ie_data_ptr, ie_len);
 		priv->wapi_ie_len = ie_len;
-		dev_dbg(priv->adapter->dev, "cmd: Set wapi_ie_len=%d IE=%#x\n",
-			priv->wapi_ie_len, priv->wapi_ie[0]);
+		mwifiex_dbg(priv->adapter, CMD,
+			    "cmd: Set wapi_ie_len=%d IE=%#x\n",
+			    priv->wapi_ie_len, priv->wapi_ie[0]);
 
 		if (priv->wapi_ie[0] == WLAN_EID_BSS_AC_ACCESS_DELAY)
 			priv->sec_info.wapi_enabled = true;
 	} else {
 		memset(priv->wapi_ie, 0, sizeof(priv->wapi_ie));
 		priv->wapi_ie_len = ie_len;
-		dev_dbg(priv->adapter->dev,
-			"info: Reset wapi_ie_len=%d IE=%#x\n",
-		       priv->wapi_ie_len, priv->wapi_ie[0]);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: Reset wapi_ie_len=%d IE=%#x\n",
+			    priv->wapi_ie_len, priv->wapi_ie[0]);
 		priv->sec_info.wapi_enabled = false;
 	}
 	return 0;
@@ -814,8 +831,8 @@ static int mwifiex_set_wps_ie(struct mwifiex_private *priv,
 {
 	if (ie_len) {
 		if (ie_len > MWIFIEX_MAX_VSIE_LEN) {
-			dev_dbg(priv->adapter->dev,
-				"info: failed to copy WPS IE, too big\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "info: failed to copy WPS IE, too big\n");
 			return -1;
 		}
 
@@ -825,13 +842,14 @@ static int mwifiex_set_wps_ie(struct mwifiex_private *priv,
 
 		memcpy(priv->wps_ie, ie_data_ptr, ie_len);
 		priv->wps_ie_len = ie_len;
-		dev_dbg(priv->adapter->dev, "cmd: Set wps_ie_len=%d IE=%#x\n",
-			priv->wps_ie_len, priv->wps_ie[0]);
+		mwifiex_dbg(priv->adapter, CMD,
+			    "cmd: Set wps_ie_len=%d IE=%#x\n",
+			    priv->wps_ie_len, priv->wps_ie[0]);
 	} else {
 		kfree(priv->wps_ie);
 		priv->wps_ie_len = ie_len;
-		dev_dbg(priv->adapter->dev,
-			"info: Reset wps_ie_len=%d\n", priv->wps_ie_len);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: Reset wps_ie_len=%d\n", priv->wps_ie_len);
 	}
 	return 0;
 }
@@ -875,8 +893,8 @@ static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 		/* Copy the required key as the current key */
 		wep_key = &priv->wep_key[index];
 		if (!wep_key->key_length) {
-			dev_err(adapter->dev,
-				"key not set, so cannot enable it\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "key not set, so cannot enable it\n");
 			return -1;
 		}
 
@@ -953,7 +971,8 @@ static int mwifiex_sec_ioctl_set_wpa_key(struct mwifiex_private *priv,
 
 	/* Current driver only supports key length of up to 32 bytes */
 	if (encrypt_key->key_len > WLAN_MAX_KEY_LEN) {
-		dev_err(priv->adapter->dev, "key length too long\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "key length too long\n");
 		return -1;
 	}
 
@@ -1040,7 +1059,7 @@ mwifiex_drv_get_driver_version(struct mwifiex_adapter *adapter, char *version,
 
 	snprintf(version, max_len, driver_version, fw_ver);
 
-	dev_dbg(adapter->dev, "info: MWIFIEX VERSION: %s\n", version);
+	mwifiex_dbg(adapter, MSG, "info: MWIFIEX VERSION: %s\n", version);
 
 	return 0;
 }
@@ -1128,7 +1147,8 @@ mwifiex_remain_on_chan_cfg(struct mwifiex_private *priv, u16 action,
 	}
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_REMAIN_ON_CHAN,
 			     action, 0, &roc_cfg, true)) {
-		dev_err(priv->adapter->dev, "failed to remain on channel\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "failed to remain on channel\n");
 		return -1;
 	}
 
@@ -1313,8 +1333,8 @@ mwifiex_set_gen_ie_helper(struct mwifiex_private *priv, u8 *ie_data_ptr,
 		if ((pvendor_ie->element_id == WLAN_EID_VENDOR_SPECIFIC) &&
 		    (!memcmp(pvendor_ie->oui, wps_oui, sizeof(wps_oui)))) {
 			priv->wps.session_enable = true;
-			dev_dbg(priv->adapter->dev,
-				"info: WPS Session Enabled.\n");
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: WPS Session Enabled.\n");
 			ret = mwifiex_set_wps_ie(priv, ie_data_ptr, ie_len);
 		}
 
@@ -1361,7 +1381,8 @@ static int mwifiex_misc_ioctl_gen_ie(struct mwifiex_private *priv,
 		memset(adapter->arp_filter, 0, sizeof(adapter->arp_filter));
 		if (gen_ie->len > ARP_FILTER_MAX_BUF_SIZE) {
 			adapter->arp_filter_size = 0;
-			dev_err(adapter->dev, "invalid ARP filter size\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "invalid ARP filter size\n");
 			return -1;
 		} else {
 			memcpy(adapter->arp_filter, gen_ie->ie_data,
@@ -1370,7 +1391,7 @@ static int mwifiex_misc_ioctl_gen_ie(struct mwifiex_private *priv,
 		}
 		break;
 	default:
-		dev_err(adapter->dev, "invalid IE type\n");
+		mwifiex_dbg(adapter, ERROR, "invalid IE type\n");
 		return -1;
 	}
 	return 0;
diff --git a/drivers/net/wireless/mwifiex/sta_rx.c b/drivers/net/wireless/mwifiex/sta_rx.c
index b8729c9394e9..d4d4cb1ce95b 100644
--- a/drivers/net/wireless/mwifiex/sta_rx.c
+++ b/drivers/net/wireless/mwifiex/sta_rx.c
@@ -141,7 +141,7 @@ int mwifiex_process_rx_packet(struct mwifiex_private *priv,
 
 	if (priv->hs2_enabled &&
 	    mwifiex_discard_gratuitous_arp(priv, skb)) {
-		dev_dbg(priv->adapter->dev, "Bypassed Gratuitous ARP\n");
+		mwifiex_dbg(priv->adapter, INFO, "Bypassed Gratuitous ARP\n");
 		dev_kfree_skb_any(skb);
 		return 0;
 	}
@@ -166,7 +166,8 @@ int mwifiex_process_rx_packet(struct mwifiex_private *priv,
 
 	ret = mwifiex_recv_packet(priv, skb);
 	if (ret == -1)
-		dev_err(priv->adapter->dev, "recv packet failed\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "recv packet failed\n");
 
 	return ret;
 }
@@ -203,9 +204,9 @@ int mwifiex_process_sta_rx_packet(struct mwifiex_private *priv,
 	rx_pkt_hdr = (void *)local_rx_pd + rx_pkt_offset;
 
 	if ((rx_pkt_offset + rx_pkt_length) > (u16) skb->len) {
-		dev_err(adapter->dev,
-			"wrong rx packet: len=%d, rx_pkt_offset=%d, rx_pkt_length=%d\n",
-			skb->len, rx_pkt_offset, rx_pkt_length);
+		mwifiex_dbg(adapter, ERROR,
+			    "wrong rx packet: len=%d, rx_pkt_offset=%d, rx_pkt_length=%d\n",
+			    skb->len, rx_pkt_offset, rx_pkt_length);
 		priv->stats.rx_dropped++;
 		dev_kfree_skb_any(skb);
 		return ret;
@@ -214,7 +215,7 @@ int mwifiex_process_sta_rx_packet(struct mwifiex_private *priv,
 	if (rx_pkt_type == PKT_TYPE_MGMT) {
 		ret = mwifiex_process_mgmt_packet(priv, skb);
 		if (ret)
-			dev_err(adapter->dev, "Rx of mgmt packet failed");
+			mwifiex_dbg(adapter, ERROR, "Rx of mgmt packet failed");
 		dev_kfree_skb_any(skb);
 		return ret;
 	}
diff --git a/drivers/net/wireless/mwifiex/sta_tx.c b/drivers/net/wireless/mwifiex/sta_tx.c
index 5ce2d9a4f919..355ac5904fac 100644
--- a/drivers/net/wireless/mwifiex/sta_tx.c
+++ b/drivers/net/wireless/mwifiex/sta_tx.c
@@ -53,7 +53,8 @@ void *mwifiex_process_sta_txpd(struct mwifiex_private *priv,
 		       INTF_HEADER_LEN;
 
 	if (!skb->len) {
-		dev_err(adapter->dev, "Tx: bad packet length: %d\n", skb->len);
+		mwifiex_dbg(adapter, ERROR,
+			    "Tx: bad packet length: %d\n", skb->len);
 		tx_info->status_code = -1;
 		return skb->data;
 	}
@@ -184,21 +185,24 @@ int mwifiex_send_null_packet(struct mwifiex_private *priv, u8 flags)
 	switch (ret) {
 	case -EBUSY:
 		dev_kfree_skb_any(skb);
-		dev_err(adapter->dev, "%s: host_to_card failed: ret=%d\n",
-			__func__, ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: host_to_card failed: ret=%d\n",
+			    __func__, ret);
 		adapter->dbg.num_tx_host_to_card_failure++;
 		break;
 	case -1:
 		adapter->data_sent = false;
 		dev_kfree_skb_any(skb);
-		dev_err(adapter->dev, "%s: host_to_card failed: ret=%d\n",
-			__func__, ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: host_to_card failed: ret=%d\n",
+			    __func__, ret);
 		adapter->dbg.num_tx_host_to_card_failure++;
 		break;
 	case 0:
 		dev_kfree_skb_any(skb);
-		dev_dbg(adapter->dev, "data: %s: host_to_card succeeded\n",
-			__func__);
+		mwifiex_dbg(adapter, DATA,
+			    "data: %s: host_to_card succeeded\n",
+			    __func__);
 		adapter->tx_lock_flag = true;
 		break;
 	case -EINPROGRESS:
diff --git a/drivers/net/wireless/mwifiex/tdls.c b/drivers/net/wireless/mwifiex/tdls.c
index 087d84762cd3..2faa1bc42abe 100644
--- a/drivers/net/wireless/mwifiex/tdls.c
+++ b/drivers/net/wireless/mwifiex/tdls.c
@@ -37,7 +37,7 @@ static void mwifiex_restore_tdls_packets(struct mwifiex_private *priv,
 	u32 tid;
 	u8 tid_down;
 
-	dev_dbg(priv->adapter->dev, "%s: %pM\n", __func__, mac);
+	mwifiex_dbg(priv->adapter, DATA, "%s: %pM\n", __func__, mac);
 	spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
 
 	skb_queue_walk_safe(&priv->tdls_txq, skb, tmp) {
@@ -94,7 +94,7 @@ static void mwifiex_hold_tdls_packets(struct mwifiex_private *priv,
 	unsigned long flags;
 	int i;
 
-	dev_dbg(priv->adapter->dev, "%s: %pM\n", __func__, mac);
+	mwifiex_dbg(priv->adapter, DATA, "%s: %pM\n", __func__, mac);
 	spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
 
 	for (i = 0; i < MAX_NUM_TID; i++) {
@@ -132,8 +132,8 @@ mwifiex_tdls_append_rates_ie(struct mwifiex_private *priv,
 	supp_rates_size = min_t(u16, rates_size, MWIFIEX_TDLS_SUPPORTED_RATES);
 
 	if (skb_tailroom(skb) < rates_size + 4) {
-		dev_err(priv->adapter->dev,
-			"Insuffient space while adding rates\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Insuffient space while adding rates\n");
 		return -ENOMEM;
 	}
 
@@ -199,8 +199,8 @@ mwifiex_tdls_add_ht_oper(struct mwifiex_private *priv, const u8 *mac,
 
 	sta_ptr = mwifiex_get_sta_entry(priv, mac);
 	if (unlikely(!sta_ptr)) {
-		dev_warn(priv->adapter->dev,
-			 "TDLS peer station not found in list\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "TDLS peer station not found in list\n");
 		return -1;
 	}
 
@@ -247,15 +247,16 @@ static int mwifiex_tdls_add_vht_oper(struct mwifiex_private *priv,
 
 	sta_ptr = mwifiex_get_sta_entry(priv, mac);
 	if (unlikely(!sta_ptr)) {
-		dev_warn(adapter->dev, "TDLS peer station not found in list\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "TDLS peer station not found in list\n");
 		return -1;
 	}
 
 	if (!mwifiex_is_bss_in_11ac_mode(priv)) {
 		if (sta_ptr->tdls_cap.extcap.ext_capab[7] &
 		   WLAN_EXT_CAPA8_TDLS_WIDE_BW_ENABLED) {
-			dev_dbg(adapter->dev,
-				"TDLS peer doesn't support wider bandwitdh\n");
+			mwifiex_dbg(adapter, WARN,
+				    "TDLS peer doesn't support wider bandwidth\n");
 			return 0;
 		}
 	} else {
@@ -554,7 +555,7 @@ static int mwifiex_prep_tdls_encap_data(struct mwifiex_private *priv,
 		tf->u.discover_req.dialog_token = dialog_token;
 		break;
 	default:
-		dev_err(priv->adapter->dev, "Unknown TDLS frame type.\n");
+		mwifiex_dbg(priv->adapter, ERROR, "Unknown TDLS frame type.\n");
 		return -EINVAL;
 	}
 
@@ -608,8 +609,8 @@ int mwifiex_send_tdls_data_frame(struct mwifiex_private *priv, const u8 *peer,
 
 	skb = dev_alloc_skb(skb_len);
 	if (!skb) {
-		dev_err(priv->adapter->dev,
-			"allocate skb failed for management frame\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "allocate skb failed for management frame\n");
 		return -ENOMEM;
 	}
 	skb_reserve(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
@@ -742,7 +743,7 @@ mwifiex_construct_tdls_action_frame(struct mwifiex_private *priv,
 		mwifiex_tdls_add_qos_capab(skb);
 		break;
 	default:
-		dev_err(priv->adapter->dev, "Unknown TDLS action frame type\n");
+		mwifiex_dbg(priv->adapter, ERROR, "Unknown TDLS action frame type\n");
 		return -EINVAL;
 	}
 
@@ -781,8 +782,8 @@ int mwifiex_send_tdls_action_frame(struct mwifiex_private *priv, const u8 *peer,
 
 	skb = dev_alloc_skb(skb_len);
 	if (!skb) {
-		dev_err(priv->adapter->dev,
-			"allocate skb failed for management frame\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "allocate skb failed for management frame\n");
 		return -ENOMEM;
 	}
 
@@ -848,8 +849,8 @@ void mwifiex_process_tdls_action_frame(struct mwifiex_private *priv,
 
 	peer = buf + ETH_ALEN;
 	action = *(buf + sizeof(struct ethhdr) + 2);
-	dev_dbg(priv->adapter->dev,
-		"rx:tdls action: peer=%pM, action=%d\n", peer, action);
+	mwifiex_dbg(priv->adapter, DATA,
+		    "rx:tdls action: peer=%pM, action=%d\n", peer, action);
 
 	switch (action) {
 	case WLAN_TDLS_SETUP_REQUEST:
@@ -880,7 +881,7 @@ void mwifiex_process_tdls_action_frame(struct mwifiex_private *priv,
 		ie_len = len - sizeof(struct ethhdr) - TDLS_CONFIRM_FIX_LEN;
 		break;
 	default:
-		dev_dbg(priv->adapter->dev, "Unknown TDLS frame type.\n");
+		mwifiex_dbg(priv->adapter, ERROR, "Unknown TDLS frame type.\n");
 		return;
 	}
 
@@ -967,8 +968,8 @@ mwifiex_tdls_process_config_link(struct mwifiex_private *priv, const u8 *peer)
 	sta_ptr = mwifiex_get_sta_entry(priv, peer);
 
 	if (!sta_ptr || sta_ptr->tdls_status == TDLS_SETUP_FAILURE) {
-		dev_err(priv->adapter->dev,
-			"link absent for peer %pM; cannot config\n", peer);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "link absent for peer %pM; cannot config\n", peer);
 		return -EINVAL;
 	}
 
@@ -988,8 +989,8 @@ mwifiex_tdls_process_create_link(struct mwifiex_private *priv, const u8 *peer)
 	sta_ptr = mwifiex_get_sta_entry(priv, peer);
 
 	if (sta_ptr && sta_ptr->tdls_status == TDLS_SETUP_INPROGRESS) {
-		dev_dbg(priv->adapter->dev,
-			"Setup already in progress for peer %pM\n", peer);
+		mwifiex_dbg(priv->adapter, WARN,
+			    "Setup already in progress for peer %pM\n", peer);
 		return 0;
 	}
 
@@ -1046,8 +1047,8 @@ mwifiex_tdls_process_enable_link(struct mwifiex_private *priv, const u8 *peer)
 	sta_ptr = mwifiex_get_sta_entry(priv, peer);
 
 	if (sta_ptr && (sta_ptr->tdls_status != TDLS_SETUP_FAILURE)) {
-		dev_dbg(priv->adapter->dev,
-			"tdls: enable link %pM success\n", peer);
+		mwifiex_dbg(priv->adapter, MSG,
+			    "tdls: enable link %pM success\n", peer);
 
 		sta_ptr->tdls_status = TDLS_SETUP_COMPLETE;
 
@@ -1076,8 +1077,8 @@ mwifiex_tdls_process_enable_link(struct mwifiex_private *priv, const u8 *peer)
 		mwifiex_auto_tdls_update_peer_status(priv, peer,
 						     TDLS_SETUP_COMPLETE);
 	} else {
-		dev_dbg(priv->adapter->dev,
-			"tdls: enable link %pM failed\n", peer);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "tdls: enable link %pM failed\n", peer);
 		if (sta_ptr) {
 			mwifiex_11n_cleanup_reorder_tbl(priv);
 			spin_lock_irqsave(&priv->wmm.ra_list_spinlock,
@@ -1180,9 +1181,9 @@ void mwifiex_disable_all_tdls_links(struct mwifiex_private *priv)
 		tdls_oper.tdls_action = MWIFIEX_TDLS_DISABLE_LINK;
 		if (mwifiex_send_cmd(priv, HostCmd_CMD_TDLS_OPER,
 				     HostCmd_ACT_GEN_SET, 0, &tdls_oper, false))
-			dev_warn(priv->adapter->dev,
-				 "Disable link failed for TDLS peer %pM",
-				 sta_ptr->mac_addr);
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "Disable link failed for TDLS peer %pM",
+				    sta_ptr->mac_addr);
 	}
 
 	mwifiex_del_all_sta_list(priv);
@@ -1204,9 +1205,9 @@ int mwifiex_tdls_check_tx(struct mwifiex_private *priv, struct sk_buff *skb)
 			    (peer->failure_count <
 			     MWIFIEX_TDLS_MAX_FAIL_COUNT)) {
 				peer->tdls_status = TDLS_SETUP_INPROGRESS;
-				dev_dbg(priv->adapter->dev,
-					"setup TDLS link, peer=%pM rssi=%d\n",
-					peer->mac_addr, peer->rssi);
+				mwifiex_dbg(priv->adapter, INFO,
+					    "setup TDLS link, peer=%pM rssi=%d\n",
+					    peer->mac_addr, peer->rssi);
 
 				cfg80211_tdls_oper_request(priv->netdev,
 							   peer->mac_addr,
@@ -1272,8 +1273,8 @@ void mwifiex_add_auto_tdls_peer(struct mwifiex_private *priv, const u8 *mac)
 		tdls_peer->rssi_jiffies = jiffies;
 		INIT_LIST_HEAD(&tdls_peer->list);
 		list_add_tail(&tdls_peer->list, &priv->auto_tdls_list);
-		dev_dbg(priv->adapter->dev, "Add auto TDLS peer= %pM to list\n",
-			mac);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "Add auto TDLS peer= %pM to list\n", mac);
 	}
 
 	spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
@@ -1341,8 +1342,8 @@ void mwifiex_check_auto_tdls(unsigned long context)
 		return;
 
 	if (!priv->auto_tdls_timer_active) {
-		dev_dbg(priv->adapter->dev,
-			"auto TDLS timer inactive; return");
+		mwifiex_dbg(priv->adapter, INFO,
+			    "auto TDLS timer inactive; return");
 		return;
 	}
 
@@ -1368,9 +1369,9 @@ void mwifiex_check_auto_tdls(unsigned long context)
 		     !tdls_peer->rssi) &&
 		    tdls_peer->tdls_status == TDLS_SETUP_COMPLETE) {
 			tdls_peer->tdls_status = TDLS_LINK_TEARDOWN;
-			dev_dbg(priv->adapter->dev,
-				"teardown TDLS link,peer=%pM rssi=%d\n",
-				tdls_peer->mac_addr, -tdls_peer->rssi);
+			mwifiex_dbg(priv->adapter, MSG,
+				    "teardown TDLS link,peer=%pM rssi=%d\n",
+				    tdls_peer->mac_addr, -tdls_peer->rssi);
 			tdls_peer->do_discover = true;
 			priv->check_tdls_tx = true;
 			cfg80211_tdls_oper_request(priv->netdev,
@@ -1384,9 +1385,10 @@ void mwifiex_check_auto_tdls(unsigned long context)
 			   MWIFIEX_TDLS_MAX_FAIL_COUNT) {
 				priv->check_tdls_tx = true;
 				tdls_peer->do_setup = true;
-				dev_dbg(priv->adapter->dev,
-					"check TDLS with peer=%pM rssi=%d\n",
-					tdls_peer->mac_addr, -tdls_peer->rssi);
+				mwifiex_dbg(priv->adapter, INFO,
+					    "check TDLS with peer=%pM\t"
+					    "rssi=%d\n", tdls_peer->mac_addr,
+					    tdls_peer->rssi);
 		}
 	}
 	spin_unlock_irqrestore(&priv->auto_tdls_lock, flags);
diff --git a/drivers/net/wireless/mwifiex/txrx.c b/drivers/net/wireless/mwifiex/txrx.c
index a245f444aeec..5ed9b794053e 100644
--- a/drivers/net/wireless/mwifiex/txrx.c
+++ b/drivers/net/wireless/mwifiex/txrx.c
@@ -50,11 +50,15 @@ int mwifiex_handle_rx_packet(struct mwifiex_adapter *adapter,
 		priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
 
 	if (!priv) {
-		dev_err(adapter->dev, "data: priv not found. Drop RX packet\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "data: priv not found. Drop RX packet\n");
 		dev_kfree_skb_any(skb);
 		return -1;
 	}
 
+	mwifiex_dbg_dump(adapter, DAT_D, "rx pkt:", skb->data,
+			 min_t(size_t, skb->len, DEBUG_DUMP_DATA_MAX_LEN));
+
 	memset(rx_info, 0, sizeof(*rx_info));
 	rx_info->bss_num = priv->bss_num;
 	rx_info->bss_type = priv->bss_type;
@@ -84,13 +88,22 @@ int mwifiex_process_tx(struct mwifiex_private *priv, struct sk_buff *skb,
 	struct mwifiex_adapter *adapter = priv->adapter;
 	u8 *head_ptr;
 	struct txpd *local_tx_pd = NULL;
+	struct mwifiex_sta_node *dest_node;
+	struct ethhdr *hdr = (void *)skb->data;
 
 	hroom = (adapter->iface_type == MWIFIEX_USB) ? 0 : INTF_HEADER_LEN;
 
-	if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP)
+	if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP) {
+		dest_node = mwifiex_get_sta_entry(priv, hdr->h_dest);
+		if (dest_node) {
+			dest_node->stats.tx_bytes += skb->len;
+			dest_node->stats.tx_packets++;
+		}
+
 		head_ptr = mwifiex_process_uap_txpd(priv, skb);
-	else
+	} else {
 		head_ptr = mwifiex_process_sta_txpd(priv, skb);
+	}
 
 	if ((adapter->data_sent || adapter->tx_lock_flag) && head_ptr) {
 		skb_queue_tail(&adapter->tx_data_q, skb);
@@ -112,10 +125,12 @@ int mwifiex_process_tx(struct mwifiex_private *priv, struct sk_buff *skb,
 							   skb, tx_param);
 		}
 	}
+	mwifiex_dbg_dump(adapter, DAT_D, "tx pkt:", skb->data,
+			 min_t(size_t, skb->len, DEBUG_DUMP_DATA_MAX_LEN));
 
 	switch (ret) {
 	case -ENOSR:
-		dev_dbg(adapter->dev, "data: -ENOSR is returned\n");
+		mwifiex_dbg(adapter, ERROR, "data: -ENOSR is returned\n");
 		break;
 	case -EBUSY:
 		if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
@@ -124,13 +139,14 @@ int mwifiex_process_tx(struct mwifiex_private *priv, struct sk_buff *skb,
 				if (local_tx_pd)
 					local_tx_pd->flags = 0;
 		}
-		dev_dbg(adapter->dev, "data: -EBUSY is returned\n");
+		mwifiex_dbg(adapter, ERROR, "data: -EBUSY is returned\n");
 		break;
 	case -1:
 		if (adapter->iface_type != MWIFIEX_PCIE)
 			adapter->data_sent = false;
-		dev_err(adapter->dev, "mwifiex_write_data_async failed: 0x%X\n",
-			ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "mwifiex_write_data_async failed: 0x%X\n",
+			    ret);
 		adapter->dbg.num_tx_host_to_card_failure++;
 		mwifiex_write_data_complete(adapter, skb, 0, ret);
 		break;
@@ -162,7 +178,8 @@ static int mwifiex_host_to_card(struct mwifiex_adapter *adapter,
 	priv = mwifiex_get_priv_by_id(adapter, tx_info->bss_num,
 				      tx_info->bss_type);
 	if (!priv) {
-		dev_err(adapter->dev, "data: priv not found. Drop TX packet\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "data: priv not found. Drop TX packet\n");
 		adapter->dbg.num_tx_host_to_card_failure++;
 		mwifiex_write_data_complete(adapter, skb, 0, 0);
 		return ret;
@@ -187,7 +204,7 @@ static int mwifiex_host_to_card(struct mwifiex_adapter *adapter,
 	}
 	switch (ret) {
 	case -ENOSR:
-		dev_err(adapter->dev, "data: -ENOSR is returned\n");
+		mwifiex_dbg(adapter, ERROR, "data: -ENOSR is returned\n");
 		break;
 	case -EBUSY:
 		if ((GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA) &&
@@ -202,13 +219,13 @@ static int mwifiex_host_to_card(struct mwifiex_adapter *adapter,
 			atomic_add(tx_info->aggr_num, &adapter->tx_queued);
 		else
 			atomic_inc(&adapter->tx_queued);
-		dev_dbg(adapter->dev, "data: -EBUSY is returned\n");
+		mwifiex_dbg(adapter, ERROR, "data: -EBUSY is returned\n");
 		break;
 	case -1:
 		if (adapter->iface_type != MWIFIEX_PCIE)
 			adapter->data_sent = false;
-		dev_err(adapter->dev, "mwifiex_write_data_async failed: 0x%X\n",
-			ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "mwifiex_write_data_async failed: 0x%X\n", ret);
 		adapter->dbg.num_tx_host_to_card_failure++;
 		mwifiex_write_data_complete(adapter, skb, 0, ret);
 		break;
@@ -302,11 +319,11 @@ int mwifiex_write_data_complete(struct mwifiex_adapter *adapter,
 		priv->stats.tx_errors++;
 	}
 
-	if (tx_info->flags & MWIFIEX_BUF_FLAG_BRIDGED_PKT) {
+	if (tx_info->flags & MWIFIEX_BUF_FLAG_BRIDGED_PKT)
 		atomic_dec_return(&adapter->pending_bridged_pkts);
-		if (tx_info->flags & MWIFIEX_BUF_FLAG_AGGR_PKT)
-			goto done;
-	}
+
+	if (tx_info->flags & MWIFIEX_BUF_FLAG_AGGR_PKT)
+		goto done;
 
 	if (aggr)
 		/* For skb_aggr, do not wake up tx queue */
@@ -319,7 +336,7 @@ int mwifiex_write_data_complete(struct mwifiex_adapter *adapter,
 		txq = netdev_get_tx_queue(priv->netdev, index);
 		if (netif_tx_queue_stopped(txq)) {
 			netif_tx_wake_queue(txq);
-			dev_dbg(adapter->dev, "wake queue: %d\n", index);
+			mwifiex_dbg(adapter, DATA, "wake queue: %d\n", index);
 		}
 	}
 done:
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
index f5c2af01ba0a..b74930054b8c 100644
--- a/drivers/net/wireless/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -167,7 +167,7 @@ mwifiex_set_ht_params(struct mwifiex_private *priv,
 	ht_ie = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, params->beacon.tail,
 				 params->beacon.tail_len);
 	if (ht_ie) {
-		memcpy(&bss_cfg->ht_cap, ht_ie,
+		memcpy(&bss_cfg->ht_cap, ht_ie + 2,
 		       sizeof(struct ieee80211_ht_cap));
 		cap_info = le16_to_cpu(bss_cfg->ht_cap.cap_info);
 		memset(&bss_cfg->ht_cap.mcs, 0,
@@ -184,8 +184,8 @@ mwifiex_set_ht_params(struct mwifiex_private *priv,
 			bss_cfg->ht_cap.mcs.rx_mask[1] = 0xff;
 			break;
 		default:
-			dev_warn(priv->adapter->dev,
-				 "Unsupported RX-STBC, default to 2x2\n");
+			mwifiex_dbg(priv->adapter, WARN,
+				    "Unsupported RX-STBC, default to 2x2\n");
 			bss_cfg->ht_cap.mcs.rx_mask[0] = 0xff;
 			bss_cfg->ht_cap.mcs.rx_mask[1] = 0xff;
 			break;
@@ -222,6 +222,23 @@ void mwifiex_set_vht_params(struct mwifiex_private *priv,
 	return;
 }
 
+/* This function updates 11ac related parameters from IE
+ * and sets them into bss_config structure.
+ */
+void mwifiex_set_tpc_params(struct mwifiex_private *priv,
+			    struct mwifiex_uap_bss_param *bss_cfg,
+			    struct cfg80211_ap_settings *params)
+{
+	const u8 *tpc_ie;
+
+	tpc_ie = cfg80211_find_ie(WLAN_EID_TPC_REQUEST, params->beacon.tail,
+				  params->beacon.tail_len);
+	if (tpc_ie)
+		bss_cfg->power_constraint = *(tpc_ie + 2);
+	else
+		bss_cfg->power_constraint = 0;
+}
+
 /* Enable VHT only when cfg80211_ap_settings has VHT IE.
  * Otherwise disable VHT.
  */
@@ -466,6 +483,7 @@ mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
 	struct host_cmd_tlv_auth_type *auth_type;
 	struct host_cmd_tlv_rates *tlv_rates;
 	struct host_cmd_tlv_ageout_timer *ao_timer, *ps_ao_timer;
+	struct host_cmd_tlv_power_constraint *pwr_ct;
 	struct mwifiex_ie_types_htcap *htcap;
 	struct mwifiex_ie_types_wmmcap *wmm_cap;
 	struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
@@ -644,6 +662,15 @@ mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
 		tlv += sizeof(*ao_timer);
 	}
 
+	if (bss_cfg->power_constraint) {
+		pwr_ct = (void *)tlv;
+		pwr_ct->header.type = cpu_to_le16(TLV_TYPE_PWR_CONSTRAINT);
+		pwr_ct->header.len = cpu_to_le16(sizeof(u8));
+		pwr_ct->constraint = bss_cfg->power_constraint;
+		cmd_size += sizeof(*pwr_ct);
+		tlv += sizeof(*pwr_ct);
+	}
+
 	if (bss_cfg->ps_sta_ao_timer) {
 		ps_ao_timer = (struct host_cmd_tlv_ageout_timer *)tlv;
 		ps_ao_timer->header.type =
@@ -754,6 +781,8 @@ int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, u16 cmd_no,
 		break;
 	case HostCmd_CMD_UAP_BSS_START:
 	case HostCmd_CMD_UAP_BSS_STOP:
+	case HOST_CMD_APCMD_SYS_RESET:
+	case HOST_CMD_APCMD_STA_LIST:
 		cmd->command = cpu_to_le16(cmd_no);
 		cmd->size = cpu_to_le16(S_DS_GEN);
 		break;
@@ -767,19 +796,22 @@ int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, u16 cmd_no,
 			return -1;
 		break;
 	default:
-		dev_err(priv->adapter->dev,
-			"PREP_CMD: unknown cmd %#x\n", cmd_no);
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "PREP_CMD: unknown cmd %#x\n", cmd_no);
 		return -1;
 	}
 
 	return 0;
 }
 
-void mwifiex_uap_set_channel(struct mwifiex_uap_bss_param *bss_cfg,
+void mwifiex_uap_set_channel(struct mwifiex_private *priv,
+			     struct mwifiex_uap_bss_param *bss_cfg,
 			     struct cfg80211_chan_def chandef)
 {
 	u8 config_bands = 0;
 
+	priv->bss_chandef = chandef;
+
 	bss_cfg->channel = ieee80211_frequency_to_channel(
 						     chandef.chan->center_freq);
 
@@ -800,30 +832,53 @@ void mwifiex_uap_set_channel(struct mwifiex_uap_bss_param *bss_cfg,
 		if (chandef.width > NL80211_CHAN_WIDTH_40)
 			config_bands |= BAND_AAC;
 	}
+
+	priv->adapter->config_bands = config_bands;
 }
 
 int mwifiex_config_start_uap(struct mwifiex_private *priv,
 			     struct mwifiex_uap_bss_param *bss_cfg)
 {
+	enum state_11d_t state_11d;
+
 	if (mwifiex_del_mgmt_ies(priv))
-		dev_err(priv->adapter->dev, "Failed to delete mgmt IEs!\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to delete mgmt IEs!\n");
 
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_STOP,
 			     HostCmd_ACT_GEN_SET, 0, NULL, true)) {
-		dev_err(priv->adapter->dev, "Failed to stop the BSS\n");
+		mwifiex_dbg(priv->adapter, ERROR, "Failed to stop the BSS\n");
+		return -1;
+	}
+
+	if (mwifiex_send_cmd(priv, HOST_CMD_APCMD_SYS_RESET,
+			     HostCmd_ACT_GEN_SET, 0, NULL, true)) {
+		mwifiex_dbg(priv->adapter, ERROR, "Failed to reset BSS\n");
 		return -1;
 	}
 
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_SYS_CONFIG,
 			     HostCmd_ACT_GEN_SET,
 			     UAP_BSS_PARAMS_I, bss_cfg, false)) {
-		dev_err(priv->adapter->dev, "Failed to set the SSID\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to set the SSID\n");
+		return -1;
+	}
+
+	/* Send cmd to FW to enable 11D function */
+	state_11d = ENABLE_11D;
+	if (mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
+			     HostCmd_ACT_GEN_SET, DOT11D_I,
+			     &state_11d, true)) {
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "11D: failed to enable 11D\n");
 		return -1;
 	}
 
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_START,
 			     HostCmd_ACT_GEN_SET, 0, NULL, false)) {
-		dev_err(priv->adapter->dev, "Failed to start the BSS\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Failed to start the BSS\n");
 		return -1;
 	}
 
diff --git a/drivers/net/wireless/mwifiex/uap_event.c b/drivers/net/wireless/mwifiex/uap_event.c
index f4794cdc36d2..7bc1f850e3b7 100644
--- a/drivers/net/wireless/mwifiex/uap_event.c
+++ b/drivers/net/wireless/mwifiex/uap_event.c
@@ -21,8 +21,70 @@
 #include "main.h"
 #include "11n.h"
 
+#define MWIFIEX_BSS_START_EVT_FIX_SIZE    12
 
+static int mwifiex_check_uap_capabilties(struct mwifiex_private *priv,
+					 struct sk_buff *event)
+{
+	int evt_len;
+	u8 *curr;
+	u16 tlv_len;
+	struct mwifiex_ie_types_data *tlv_hdr;
+	struct ieee_types_wmm_parameter *wmm_param_ie = NULL;
+	int mask = IEEE80211_WMM_IE_AP_QOSINFO_PARAM_SET_CNT_MASK;
+
+	priv->wmm_enabled = false;
+	skb_pull(event, MWIFIEX_BSS_START_EVT_FIX_SIZE);
+	evt_len = event->len;
+	curr = event->data;
+
+	mwifiex_dbg_dump(priv->adapter, EVT_D, "uap capabilties:",
+			 event->data, event->len);
 
+	while ((evt_len >= sizeof(tlv_hdr->header))) {
+		tlv_hdr = (struct mwifiex_ie_types_data *)curr;
+		tlv_len = le16_to_cpu(tlv_hdr->header.len);
+
+		if (evt_len < tlv_len + sizeof(tlv_hdr->header))
+			break;
+
+		switch (le16_to_cpu(tlv_hdr->header.type)) {
+		case WLAN_EID_HT_CAPABILITY:
+			priv->ap_11n_enabled = true;
+			break;
+
+		case WLAN_EID_VHT_CAPABILITY:
+			priv->ap_11ac_enabled = true;
+			break;
+
+		case WLAN_EID_VENDOR_SPECIFIC:
+			/* Point the regular IEEE IE 2 bytes into the Marvell IE
+			 * and setup the IEEE IE type and length byte fields
+			 */
+			wmm_param_ie = (void *)(curr + 2);
+			wmm_param_ie->vend_hdr.len = (u8)tlv_len;
+			wmm_param_ie->vend_hdr.element_id =
+						WLAN_EID_VENDOR_SPECIFIC;
+			mwifiex_dbg(priv->adapter, EVENT,
+				    "info: check uap capabilities:\t"
+				    "wmm parameter set count: %d\n",
+				    wmm_param_ie->qos_info_bitmap & mask);
+
+			mwifiex_wmm_setup_ac_downgrade(priv);
+			priv->wmm_enabled = true;
+			mwifiex_wmm_setup_queue_priorities(priv, wmm_param_ie);
+			break;
+
+		default:
+			break;
+		}
+
+		curr += (tlv_len + sizeof(tlv_hdr->header));
+		evt_len -= (tlv_len + sizeof(tlv_hdr->header));
+	}
+
+	return 0;
+}
 
 /*
  * This function handles AP interface specific events generated by firmware.
@@ -80,8 +142,8 @@ int mwifiex_process_uap_event(struct mwifiex_private *priv)
 
 		node = mwifiex_add_sta_entry(priv, event->sta_addr);
 		if (!node) {
-			dev_warn(adapter->dev,
-				 "could not create station entry!\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "could not create station entry!\n");
 			return -1;
 		}
 
@@ -128,58 +190,63 @@ int mwifiex_process_uap_event(struct mwifiex_private *priv)
 		mwifiex_wake_up_net_dev_queue(priv->netdev, adapter);
 		break;
 	case EVENT_UAP_BSS_START:
-		dev_dbg(adapter->dev, "AP EVENT: event id: %#x\n", eventcause);
+		mwifiex_dbg(adapter, EVENT,
+			    "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);
+		mwifiex_check_uap_capabilties(priv, adapter->event_skb);
 		break;
 	case EVENT_UAP_MIC_COUNTERMEASURES:
 		/* For future development */
-		dev_dbg(adapter->dev, "AP EVENT: event id: %#x\n", eventcause);
+		mwifiex_dbg(adapter, EVENT,
+			    "AP EVENT: event id: %#x\n", eventcause);
 		break;
 	case EVENT_AMSDU_AGGR_CTRL:
 		ctrl = le16_to_cpu(*(__le16 *)adapter->event_body);
-		dev_dbg(adapter->dev, "event: AMSDU_AGGR_CTRL %d\n", ctrl);
+		mwifiex_dbg(adapter, EVENT,
+			    "event: AMSDU_AGGR_CTRL %d\n", ctrl);
 
 		if (priv->media_connected) {
 			adapter->tx_buf_size =
 				min_t(u16, adapter->curr_tx_buf_size, ctrl);
-			dev_dbg(adapter->dev, "event: tx_buf_size %d\n",
-				adapter->tx_buf_size);
+			mwifiex_dbg(adapter, EVENT,
+				    "event: tx_buf_size %d\n",
+				    adapter->tx_buf_size);
 		}
 		break;
 	case EVENT_ADDBA:
-		dev_dbg(adapter->dev, "event: ADDBA Request\n");
+		mwifiex_dbg(adapter, EVENT, "event: ADDBA Request\n");
 		if (priv->media_connected)
 			mwifiex_send_cmd(priv, HostCmd_CMD_11N_ADDBA_RSP,
 					 HostCmd_ACT_GEN_SET, 0,
 					 adapter->event_body, false);
 		break;
 	case EVENT_DELBA:
-		dev_dbg(adapter->dev, "event: DELBA Request\n");
+		mwifiex_dbg(adapter, EVENT, "event: DELBA Request\n");
 		if (priv->media_connected)
 			mwifiex_11n_delete_ba_stream(priv, adapter->event_body);
 		break;
 	case EVENT_BA_STREAM_TIEMOUT:
-		dev_dbg(adapter->dev, "event:  BA Stream timeout\n");
+		mwifiex_dbg(adapter, EVENT, "event:  BA Stream timeout\n");
 		if (priv->media_connected) {
 			ba_timeout = (void *)adapter->event_body;
 			mwifiex_11n_ba_stream_timeout(priv, ba_timeout);
 		}
 		break;
 	case EVENT_EXT_SCAN_REPORT:
-		dev_dbg(adapter->dev, "event: EXT_SCAN Report\n");
+		mwifiex_dbg(adapter, EVENT, "event: EXT_SCAN Report\n");
 		if (adapter->ext_scan)
 			return mwifiex_handle_event_ext_scan_report(priv,
 						adapter->event_skb->data);
 		break;
 	case EVENT_TX_STATUS_REPORT:
-		dev_dbg(adapter->dev, "event: TX_STATUS Report\n");
+		mwifiex_dbg(adapter, EVENT, "event: TX_STATUS Report\n");
 		mwifiex_parse_tx_status_event(priv, adapter->event_body);
 		break;
 	case EVENT_PS_SLEEP:
-		dev_dbg(adapter->dev, "info: EVENT: SLEEP\n");
+		mwifiex_dbg(adapter, EVENT, "info: EVENT: SLEEP\n");
 
 		adapter->ps_state = PS_STATE_PRE_SLEEP;
 
@@ -187,12 +254,12 @@ int mwifiex_process_uap_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_PS_AWAKE:
-		dev_dbg(adapter->dev, "info: EVENT: AWAKE\n");
+		mwifiex_dbg(adapter, EVENT, "info: EVENT: AWAKE\n");
 		if (!adapter->pps_uapsd_mode &&
 		    priv->media_connected && adapter->sleep_period.period) {
 				adapter->pps_uapsd_mode = true;
-				dev_dbg(adapter->dev,
-					"event: PPS/UAPSD mode activated\n");
+				mwifiex_dbg(adapter, EVENT,
+					    "event: PPS/UAPSD mode activated\n");
 		}
 		adapter->tx_lock_flag = false;
 		if (adapter->pps_uapsd_mode && adapter->gen_null_pkt) {
@@ -218,16 +285,21 @@ int mwifiex_process_uap_event(struct mwifiex_private *priv)
 		break;
 
 	case EVENT_CHANNEL_REPORT_RDY:
-		dev_dbg(adapter->dev, "event: Channel Report\n");
+		mwifiex_dbg(adapter, EVENT, "event: Channel Report\n");
 		mwifiex_11h_handle_chanrpt_ready(priv, adapter->event_skb);
 		break;
 	case EVENT_RADAR_DETECTED:
-		dev_dbg(adapter->dev, "event: Radar detected\n");
+		mwifiex_dbg(adapter, EVENT, "event: Radar detected\n");
 		mwifiex_11h_handle_radar_detected(priv, adapter->event_skb);
 		break;
+	case EVENT_BT_COEX_WLAN_PARA_CHANGE:
+		dev_err(adapter->dev, "EVENT: BT coex wlan param update\n");
+		mwifiex_bt_coex_wlan_param_update_event(priv,
+							adapter->event_skb);
+		break;
 	default:
-		dev_dbg(adapter->dev, "event: unknown event id: %#x\n",
-			eventcause);
+		mwifiex_dbg(adapter, EVENT,
+			    "event: unknown event id: %#x\n", eventcause);
 		break;
 	}
 
diff --git a/drivers/net/wireless/mwifiex/uap_txrx.c b/drivers/net/wireless/mwifiex/uap_txrx.c
index 38ac4d74c486..87667418af5f 100644
--- a/drivers/net/wireless/mwifiex/uap_txrx.c
+++ b/drivers/net/wireless/mwifiex/uap_txrx.c
@@ -97,14 +97,15 @@ static void mwifiex_uap_queue_bridged_pkt(struct mwifiex_private *priv,
 	struct mwifiex_txinfo *tx_info;
 	int hdr_chop;
 	struct ethhdr *p_ethhdr;
+	struct mwifiex_sta_node *src_node;
 
 	uap_rx_pd = (struct uap_rxpd *)(skb->data);
 	rx_pkt_hdr = (void *)uap_rx_pd + le16_to_cpu(uap_rx_pd->rx_pkt_offset);
 
 	if ((atomic_read(&adapter->pending_bridged_pkts) >=
 					     MWIFIEX_BRIDGED_PKTS_THR_HIGH)) {
-		dev_err(priv->adapter->dev,
-			"Tx: Bridge packet limit reached. Drop packet!\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "Tx: Bridge packet limit reached. Drop packet!\n");
 		kfree_skb(skb);
 		mwifiex_uap_cleanup_tx_queues(priv);
 		return;
@@ -153,15 +154,15 @@ static void mwifiex_uap_queue_bridged_pkt(struct mwifiex_private *priv,
 	skb_pull(skb, hdr_chop);
 
 	if (skb_headroom(skb) < MWIFIEX_MIN_DATA_HEADER_LEN) {
-		dev_dbg(priv->adapter->dev,
-			"data: Tx: insufficient skb headroom %d\n",
-			skb_headroom(skb));
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "data: Tx: insufficient skb headroom %d\n",
+			    skb_headroom(skb));
 		/* Insufficient skb headroom - allocate a new skb */
 		new_skb =
 			skb_realloc_headroom(skb, MWIFIEX_MIN_DATA_HEADER_LEN);
 		if (unlikely(!new_skb)) {
-			dev_err(priv->adapter->dev,
-				"Tx: cannot allocate new_skb\n");
+			mwifiex_dbg(priv->adapter, ERROR,
+				    "Tx: cannot allocate new_skb\n");
 			kfree_skb(skb);
 			priv->stats.tx_dropped++;
 			return;
@@ -169,8 +170,9 @@ static void mwifiex_uap_queue_bridged_pkt(struct mwifiex_private *priv,
 
 		kfree_skb(skb);
 		skb = new_skb;
-		dev_dbg(priv->adapter->dev, "info: new skb headroom %d\n",
-			skb_headroom(skb));
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: new skb headroom %d\n",
+			    skb_headroom(skb));
 	}
 
 	tx_info = MWIFIEX_SKB_TXCB(skb);
@@ -179,6 +181,15 @@ static void mwifiex_uap_queue_bridged_pkt(struct mwifiex_private *priv,
 	tx_info->bss_type = priv->bss_type;
 	tx_info->flags |= MWIFIEX_BUF_FLAG_BRIDGED_PKT;
 
+	src_node = mwifiex_get_sta_entry(priv, rx_pkt_hdr->eth803_hdr.h_source);
+	if (src_node) {
+		src_node->stats.last_rx = jiffies;
+		src_node->stats.rx_bytes += skb->len;
+		src_node->stats.rx_packets++;
+		src_node->stats.last_tx_rate = uap_rx_pd->rx_rate;
+		src_node->stats.last_tx_htinfo = uap_rx_pd->ht_info;
+	}
+
 	if (is_unicast_ether_addr(rx_pkt_hdr->eth803_hdr.h_dest)) {
 		/* Update bridge packet statistics as the
 		 * packet is not going to kernel/upper layer.
@@ -225,7 +236,8 @@ int mwifiex_handle_uap_rx_forward(struct mwifiex_private *priv,
 
 	/* don't do packet forwarding in disconnected state */
 	if (!priv->media_connected) {
-		dev_err(adapter->dev, "drop packet in disconnected state.\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "drop packet in disconnected state.\n");
 		dev_kfree_skb_any(skb);
 		return 0;
 	}
@@ -273,13 +285,20 @@ int mwifiex_process_uap_rx_packet(struct mwifiex_private *priv,
 	rx_pkt_type = le16_to_cpu(uap_rx_pd->rx_pkt_type);
 	rx_pkt_hdr = (void *)uap_rx_pd + le16_to_cpu(uap_rx_pd->rx_pkt_offset);
 
+	ether_addr_copy(ta, rx_pkt_hdr->eth803_hdr.h_source);
+
 	if ((le16_to_cpu(uap_rx_pd->rx_pkt_offset) +
 	     le16_to_cpu(uap_rx_pd->rx_pkt_length)) > (u16) skb->len) {
-		dev_err(adapter->dev,
-			"wrong rx packet: len=%d, offset=%d, length=%d\n",
-			skb->len, le16_to_cpu(uap_rx_pd->rx_pkt_offset),
-			le16_to_cpu(uap_rx_pd->rx_pkt_length));
+		mwifiex_dbg(adapter, ERROR,
+			    "wrong rx packet: len=%d, offset=%d, length=%d\n",
+			    skb->len, le16_to_cpu(uap_rx_pd->rx_pkt_offset),
+			    le16_to_cpu(uap_rx_pd->rx_pkt_length));
 		priv->stats.rx_dropped++;
+
+		node = mwifiex_get_sta_entry(priv, ta);
+		if (node)
+			node->stats.tx_failed++;
+
 		dev_kfree_skb_any(skb);
 		return 0;
 	}
@@ -287,12 +306,12 @@ int mwifiex_process_uap_rx_packet(struct mwifiex_private *priv,
 	if (rx_pkt_type == PKT_TYPE_MGMT) {
 		ret = mwifiex_process_mgmt_packet(priv, skb);
 		if (ret)
-			dev_err(adapter->dev, "Rx of mgmt packet failed");
+			mwifiex_dbg(adapter, ERROR,
+				    "Rx of mgmt packet failed");
 		dev_kfree_skb_any(skb);
 		return ret;
 	}
 
-	memcpy(ta, rx_pkt_hdr->eth803_hdr.h_source, ETH_ALEN);
 
 	if (rx_pkt_type != PKT_TYPE_BAR && uap_rx_pd->priority < MAX_NUM_TID) {
 		spin_lock_irqsave(&priv->sta_list_spinlock, flags);
@@ -354,7 +373,8 @@ void *mwifiex_process_uap_txpd(struct mwifiex_private *priv,
 		       INTF_HEADER_LEN;
 
 	if (!skb->len) {
-		dev_err(adapter->dev, "Tx: bad packet length: %d\n", skb->len);
+		mwifiex_dbg(adapter, ERROR,
+			    "Tx: bad packet length: %d\n", skb->len);
 		tx_info->status_code = -1;
 		return skb->data;
 	}
diff --git a/drivers/net/wireless/mwifiex/usb.c b/drivers/net/wireless/mwifiex/usb.c
index fd8027f200a0..aada93425f80 100644
--- a/drivers/net/wireless/mwifiex/usb.c
+++ b/drivers/net/wireless/mwifiex/usb.c
@@ -60,7 +60,6 @@ static int mwifiex_usb_submit_rx_urb(struct urb_context *ctx, int size);
 static int mwifiex_usb_recv(struct mwifiex_adapter *adapter,
 			    struct sk_buff *skb, u8 ep)
 {
-	struct device *dev = adapter->dev;
 	u32 recv_type;
 	__le32 tmp;
 	int ret;
@@ -69,13 +68,15 @@ static int mwifiex_usb_recv(struct mwifiex_adapter *adapter,
 		mwifiex_process_hs_config(adapter);
 
 	if (skb->len < INTF_HEADER_LEN) {
-		dev_err(dev, "%s: invalid skb->len\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: invalid skb->len\n", __func__);
 		return -1;
 	}
 
 	switch (ep) {
 	case MWIFIEX_USB_EP_CMD_EVENT:
-		dev_dbg(dev, "%s: EP_CMD_EVENT\n", __func__);
+		mwifiex_dbg(adapter, EVENT,
+			    "%s: EP_CMD_EVENT\n", __func__);
 		skb_copy_from_linear_data(skb, &tmp, INTF_HEADER_LEN);
 		recv_type = le32_to_cpu(tmp);
 		skb_pull(skb, INTF_HEADER_LEN);
@@ -83,11 +84,12 @@ static int mwifiex_usb_recv(struct mwifiex_adapter *adapter,
 		switch (recv_type) {
 		case MWIFIEX_USB_TYPE_CMD:
 			if (skb->len > MWIFIEX_SIZE_OF_CMD_BUFFER) {
-				dev_err(dev, "CMD: skb->len too large\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "CMD: skb->len too large\n");
 				ret = -1;
 				goto exit_restore_skb;
 			} else if (!adapter->curr_cmd) {
-				dev_dbg(dev, "CMD: no curr_cmd\n");
+				mwifiex_dbg(adapter, WARN, "CMD: no curr_cmd\n");
 				if (adapter->ps_state == PS_STATE_SLEEP_CFM) {
 					mwifiex_process_sleep_confirm_resp(
 							adapter, skb->data,
@@ -104,16 +106,19 @@ static int mwifiex_usb_recv(struct mwifiex_adapter *adapter,
 			break;
 		case MWIFIEX_USB_TYPE_EVENT:
 			if (skb->len < sizeof(u32)) {
-				dev_err(dev, "EVENT: skb->len too small\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "EVENT: skb->len too small\n");
 				ret = -1;
 				goto exit_restore_skb;
 			}
 			skb_copy_from_linear_data(skb, &tmp, sizeof(u32));
 			adapter->event_cause = le32_to_cpu(tmp);
-			dev_dbg(dev, "event_cause %#x\n", adapter->event_cause);
+			mwifiex_dbg(adapter, EVENT,
+				    "event_cause %#x\n", adapter->event_cause);
 
 			if (skb->len > MAX_EVENT_SIZE) {
-				dev_err(dev, "EVENT: event body too large\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "EVENT: event body too large\n");
 				ret = -1;
 				goto exit_restore_skb;
 			}
@@ -125,14 +130,16 @@ static int mwifiex_usb_recv(struct mwifiex_adapter *adapter,
 			adapter->event_skb = skb;
 			break;
 		default:
-			dev_err(dev, "unknown recv_type %#x\n", recv_type);
+			mwifiex_dbg(adapter, ERROR,
+				    "unknown recv_type %#x\n", recv_type);
 			return -1;
 		}
 		break;
 	case MWIFIEX_USB_EP_DATA:
-		dev_dbg(dev, "%s: EP_DATA\n", __func__);
+		mwifiex_dbg(adapter, DATA, "%s: EP_DATA\n", __func__);
 		if (skb->len > MWIFIEX_RX_DATA_BUF_SIZE) {
-			dev_err(dev, "DATA: skb->len too large\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "DATA: skb->len too large\n");
 			return -1;
 		}
 
@@ -141,7 +148,8 @@ static int mwifiex_usb_recv(struct mwifiex_adapter *adapter,
 		atomic_inc(&adapter->rx_pending);
 		break;
 	default:
-		dev_err(dev, "%s: unknown endport %#x\n", __func__, ep);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: unknown endport %#x\n", __func__, ep);
 		return -1;
 	}
 
@@ -176,8 +184,8 @@ static void mwifiex_usb_rx_complete(struct urb *urb)
 
 	if (recv_length) {
 		if (urb->status || (adapter->surprise_removed)) {
-			dev_err(adapter->dev,
-				"URB status is failed: %d\n", urb->status);
+			mwifiex_dbg(adapter, ERROR,
+				    "URB status is failed: %d\n", urb->status);
 			/* Do not free skb in case of command ep */
 			if (card->rx_cmd_ep != context->ep)
 				dev_kfree_skb_any(skb);
@@ -190,8 +198,9 @@ static void mwifiex_usb_rx_complete(struct urb *urb)
 
 		status = mwifiex_usb_recv(adapter, skb, context->ep);
 
-		dev_dbg(adapter->dev, "info: recv_length=%d, status=%d\n",
-			recv_length, status);
+		mwifiex_dbg(adapter, INFO,
+			    "info: recv_length=%d, status=%d\n",
+			    recv_length, status);
 		if (status == -EINPROGRESS) {
 			mwifiex_queue_main_work(adapter);
 
@@ -203,8 +212,8 @@ static void mwifiex_usb_rx_complete(struct urb *urb)
 				return;
 		} else {
 			if (status == -1)
-				dev_err(adapter->dev,
-					"received data processing failed!\n");
+				mwifiex_dbg(adapter, ERROR,
+					    "received data processing failed!\n");
 
 			/* Do not free skb in case of command ep */
 			if (card->rx_cmd_ep != context->ep)
@@ -212,8 +221,8 @@ static void mwifiex_usb_rx_complete(struct urb *urb)
 		}
 	} else if (urb->status) {
 		if (!adapter->is_suspended) {
-			dev_warn(adapter->dev,
-				 "Card is removed: %d\n", urb->status);
+			mwifiex_dbg(adapter, FATAL,
+				    "Card is removed: %d\n", urb->status);
 			adapter->surprise_removed = true;
 		}
 		dev_kfree_skb_any(skb);
@@ -249,14 +258,17 @@ static void mwifiex_usb_tx_complete(struct urb *urb)
 	struct mwifiex_adapter *adapter = context->adapter;
 	struct usb_card_rec *card = adapter->card;
 
-	dev_dbg(adapter->dev, "%s: status: %d\n", __func__, urb->status);
+	mwifiex_dbg(adapter, INFO,
+		    "%s: status: %d\n", __func__, urb->status);
 
 	if (context->ep == card->tx_cmd_ep) {
-		dev_dbg(adapter->dev, "%s: CMD\n", __func__);
+		mwifiex_dbg(adapter, CMD,
+			    "%s: CMD\n", __func__);
 		atomic_dec(&card->tx_cmd_urb_pending);
 		adapter->cmd_sent = false;
 	} else {
-		dev_dbg(adapter->dev, "%s: DATA\n", __func__);
+		mwifiex_dbg(adapter, DATA,
+			    "%s: DATA\n", __func__);
 		atomic_dec(&card->tx_data_urb_pending);
 		mwifiex_write_data_complete(adapter, context->skb, 0,
 					    urb->status ? -1 : 0);
@@ -275,8 +287,8 @@ static int mwifiex_usb_submit_rx_urb(struct urb_context *ctx, int size)
 	if (card->rx_cmd_ep != ctx->ep) {
 		ctx->skb = dev_alloc_skb(size);
 		if (!ctx->skb) {
-			dev_err(adapter->dev,
-				"%s: dev_alloc_skb failed\n", __func__);
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: dev_alloc_skb failed\n", __func__);
 			return -ENOMEM;
 		}
 	}
@@ -291,7 +303,7 @@ static int mwifiex_usb_submit_rx_urb(struct urb_context *ctx, int size)
 		atomic_inc(&card->rx_data_urb_pending);
 
 	if (usb_submit_urb(ctx->urb, GFP_ATOMIC)) {
-		dev_err(adapter->dev, "usb_submit_urb failed\n");
+		mwifiex_dbg(adapter, ERROR, "usb_submit_urb failed\n");
 		dev_kfree_skb_any(ctx->skb);
 		ctx->skb = NULL;
 
@@ -468,7 +480,8 @@ static int mwifiex_usb_suspend(struct usb_interface *intf, pm_message_t message)
 	adapter = card->adapter;
 
 	if (unlikely(adapter->is_suspended))
-		dev_warn(adapter->dev, "Device already suspended\n");
+		mwifiex_dbg(adapter, WARN,
+			    "Device already suspended\n");
 
 	mwifiex_enable_hs(adapter);
 
@@ -519,7 +532,8 @@ static int mwifiex_usb_resume(struct usb_interface *intf)
 	adapter = card->adapter;
 
 	if (unlikely(!adapter->is_suspended)) {
-		dev_warn(adapter->dev, "Device already resumed\n");
+		mwifiex_dbg(adapter, WARN,
+			    "Device already resumed\n");
 		return 0;
 	}
 
@@ -578,7 +592,8 @@ static void mwifiex_usb_disconnect(struct usb_interface *intf)
 
 	mwifiex_usb_free(card);
 
-	dev_dbg(adapter->dev, "%s: removing card\n", __func__);
+	mwifiex_dbg(adapter, FATAL,
+		    "%s: removing card\n", __func__);
 	mwifiex_remove_card(adapter, &add_remove_card_sem);
 
 	usb_set_intfdata(intf, NULL);
@@ -608,7 +623,8 @@ static int mwifiex_usb_tx_init(struct mwifiex_adapter *adapter)
 
 	card->tx_cmd.urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!card->tx_cmd.urb) {
-		dev_err(adapter->dev, "tx_cmd.urb allocation failed\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "tx_cmd.urb allocation failed\n");
 		return -ENOMEM;
 	}
 
@@ -620,8 +636,8 @@ static int mwifiex_usb_tx_init(struct mwifiex_adapter *adapter)
 
 		card->tx_data_list[i].urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!card->tx_data_list[i].urb) {
-			dev_err(adapter->dev,
-				"tx_data_list[] urb allocation failed\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "tx_data_list[] urb allocation failed\n");
 			return -ENOMEM;
 		}
 	}
@@ -639,15 +655,13 @@ static int mwifiex_usb_rx_init(struct mwifiex_adapter *adapter)
 
 	card->rx_cmd.urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!card->rx_cmd.urb) {
-		dev_err(adapter->dev, "rx_cmd.urb allocation failed\n");
+		mwifiex_dbg(adapter, ERROR, "rx_cmd.urb allocation failed\n");
 		return -ENOMEM;
 	}
 
 	card->rx_cmd.skb = dev_alloc_skb(MWIFIEX_RX_CMD_BUF_SIZE);
-	if (!card->rx_cmd.skb) {
-		dev_err(adapter->dev, "rx_cmd.skb allocation failed\n");
+	if (!card->rx_cmd.skb)
 		return -ENOMEM;
-	}
 
 	if (mwifiex_usb_submit_rx_urb(&card->rx_cmd, MWIFIEX_RX_CMD_BUF_SIZE))
 		return -1;
@@ -658,8 +672,8 @@ static int mwifiex_usb_rx_init(struct mwifiex_adapter *adapter)
 
 		card->rx_data_list[i].urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (!card->rx_data_list[i].urb) {
-			dev_err(adapter->dev,
-				"rx_data_list[] urb allocation failed\n");
+			mwifiex_dbg(adapter, ERROR,
+				    "rx_data_list[] urb allocation failed\n");
 			return -1;
 		}
 		if (mwifiex_usb_submit_rx_urb(&card->rx_data_list[i],
@@ -683,7 +697,8 @@ static int mwifiex_write_data_sync(struct mwifiex_adapter *adapter, u8 *pbuf,
 	ret = usb_bulk_msg(card->udev, usb_sndbulkpipe(card->udev, ep), pbuf,
 			   *len, &actual_length, timeout);
 	if (ret) {
-		dev_err(adapter->dev, "usb_bulk_msg for tx failed: %d\n", ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "usb_bulk_msg for tx failed: %d\n", ret);
 		return ret;
 	}
 
@@ -702,7 +717,8 @@ static int mwifiex_read_data_sync(struct mwifiex_adapter *adapter, u8 *pbuf,
 	ret = usb_bulk_msg(card->udev, usb_rcvbulkpipe(card->udev, ep), pbuf,
 			   *len, &actual_length, timeout);
 	if (ret) {
-		dev_err(adapter->dev, "usb_bulk_msg for rx failed: %d\n", ret);
+		mwifiex_dbg(adapter, ERROR,
+			    "usb_bulk_msg for rx failed: %d\n", ret);
 		return ret;
 	}
 
@@ -722,13 +738,13 @@ static int mwifiex_usb_host_to_card(struct mwifiex_adapter *adapter, u8 ep,
 	struct urb *tx_urb;
 
 	if (adapter->is_suspended) {
-		dev_err(adapter->dev,
-			"%s: not allowed while suspended\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: not allowed while suspended\n", __func__);
 		return -1;
 	}
 
 	if (adapter->surprise_removed) {
-		dev_err(adapter->dev, "%s: device removed\n", __func__);
+		mwifiex_dbg(adapter, ERROR, "%s: device removed\n", __func__);
 		return -1;
 	}
 
@@ -737,7 +753,7 @@ static int mwifiex_usb_host_to_card(struct mwifiex_adapter *adapter, u8 ep,
 		return -EBUSY;
 	}
 
-	dev_dbg(adapter->dev, "%s: ep=%d\n", __func__, ep);
+	mwifiex_dbg(adapter, INFO, "%s: ep=%d\n", __func__, ep);
 
 	if (ep == card->tx_cmd_ep) {
 		context = &card->tx_cmd;
@@ -764,7 +780,8 @@ static int mwifiex_usb_host_to_card(struct mwifiex_adapter *adapter, u8 ep,
 		atomic_inc(&card->tx_data_urb_pending);
 
 	if (usb_submit_urb(tx_urb, GFP_ATOMIC)) {
-		dev_err(adapter->dev, "%s: usb_submit_urb failed\n", __func__);
+		mwifiex_dbg(adapter, ERROR,
+			    "%s: usb_submit_urb failed\n", __func__);
 		if (ep == card->tx_cmd_ep) {
 			atomic_dec(&card->tx_cmd_urb_pending);
 		} else {
@@ -843,8 +860,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 	u8 check_winner = 1;
 
 	if (!firmware) {
-		dev_err(adapter->dev,
-			"No firmware image found! Terminating download\n");
+		mwifiex_dbg(adapter, ERROR,
+			    "No firmware image found! Terminating download\n");
 		ret = -1;
 		goto fw_exit;
 	}
@@ -889,8 +906,9 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 						MWIFIEX_USB_EP_CMD_EVENT,
 						MWIFIEX_USB_TIMEOUT);
 			if (ret) {
-				dev_err(adapter->dev,
-					"write_data_sync: failed: %d\n", ret);
+				mwifiex_dbg(adapter, ERROR,
+					    "write_data_sync: failed: %d\n",
+					    ret);
 				continue;
 			}
 
@@ -902,8 +920,9 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 						MWIFIEX_USB_EP_CMD_EVENT,
 						MWIFIEX_USB_TIMEOUT);
 			if (ret) {
-				dev_err(adapter->dev,
-					"read_data_sync: failed: %d\n", ret);
+				mwifiex_dbg(adapter, ERROR,
+					    "read_data_sync: failed: %d\n",
+					    ret);
 				continue;
 			}
 
@@ -913,17 +932,17 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 			/* check 1st firmware block resp for highest bit set */
 			if (check_winner) {
 				if (le32_to_cpu(sync_fw.cmd) & 0x80000000) {
-					dev_warn(adapter->dev,
-						 "USB is not the winner %#x\n",
-						 sync_fw.cmd);
+					mwifiex_dbg(adapter, WARN,
+						    "USB is not the winner %#x\n",
+						    sync_fw.cmd);
 
 					/* returning success */
 					ret = 0;
 					goto cleanup;
 				}
 
-				dev_dbg(adapter->dev,
-					"USB is the winner, start to download FW\n");
+				mwifiex_dbg(adapter, MSG,
+					    "start to download FW...\n");
 
 				check_winner = 0;
 				break;
@@ -931,9 +950,9 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 
 			/* check the firmware block response for CRC errors */
 			if (sync_fw.cmd) {
-				dev_err(adapter->dev,
-					"FW received block with CRC %#x\n",
-					sync_fw.cmd);
+				mwifiex_dbg(adapter, ERROR,
+					    "FW received block with CRC %#x\n",
+					    sync_fw.cmd);
 				ret = -1;
 				continue;
 			}
@@ -945,8 +964,8 @@ static int mwifiex_prog_fw_w_helper(struct mwifiex_adapter *adapter,
 	} while ((dnld_cmd != FW_HAS_LAST_BLOCK) && retries);
 
 cleanup:
-	dev_notice(adapter->dev,
-		   "info: FW download over, size %d bytes\n", tlen);
+	mwifiex_dbg(adapter, MSG,
+		    "info: FW download over, size %d bytes\n", tlen);
 
 	kfree(recv_buff);
 	kfree(fwdata);
diff --git a/drivers/net/wireless/mwifiex/util.c b/drivers/net/wireless/mwifiex/util.c
index b8a45872354d..790e61953abf 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/mwifiex/util.c
@@ -26,6 +26,8 @@
 #include "11n.h"
 
 static struct mwifiex_debug_data items[] = {
+	{"debug_mask", item_size(debug_mask),
+	 item_addr(debug_mask), 1},
 	{"int_counter", item_size(int_counter),
 	 item_addr(int_counter), 1},
 	{"wmm_ac_vo", item_size(packets_out[WMM_AC_VO]),
@@ -158,7 +160,8 @@ int mwifiex_init_shutdown_fw(struct mwifiex_private *priv,
 	} else if (func_init_shutdown == MWIFIEX_FUNC_SHUTDOWN) {
 		cmd = HostCmd_CMD_FUNC_SHUTDOWN;
 	} else {
-		dev_err(priv->adapter->dev, "unsupported parameter\n");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "unsupported parameter\n");
 		return -1;
 	}
 
@@ -178,6 +181,7 @@ int mwifiex_get_debug_info(struct mwifiex_private *priv,
 	struct mwifiex_adapter *adapter = priv->adapter;
 
 	if (info) {
+		info->debug_mask = adapter->debug_mask;
 		memcpy(info->packets_out,
 		       priv->wmm.packets_out,
 		       sizeof(priv->wmm.packets_out));
@@ -325,7 +329,7 @@ mwifiex_parse_mgmt_packet(struct mwifiex_private *priv, u8 *payload, u16 len,
 			  struct rxpd *rx_pd)
 {
 	u16 stype;
-	u8 category, action_code;
+	u8 category, action_code, *addr2;
 	struct ieee80211_hdr *ieee_hdr = (void *)payload;
 
 	stype = (le16_to_cpu(ieee_hdr->frame_control) & IEEE80211_FCTL_STYPE);
@@ -333,21 +337,35 @@ mwifiex_parse_mgmt_packet(struct mwifiex_private *priv, u8 *payload, u16 len,
 	switch (stype) {
 	case IEEE80211_STYPE_ACTION:
 		category = *(payload + sizeof(struct ieee80211_hdr));
-		action_code = *(payload + sizeof(struct ieee80211_hdr) + 1);
-		if (category == WLAN_CATEGORY_PUBLIC &&
-		    action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) {
-			dev_dbg(priv->adapter->dev,
-				"TDLS discovery response %pM nf=%d, snr=%d\n",
-				ieee_hdr->addr2, rx_pd->nf, rx_pd->snr);
-			mwifiex_auto_tdls_update_peer_signal(priv,
-							     ieee_hdr->addr2,
-							     rx_pd->snr,
-							     rx_pd->nf);
+		switch (category) {
+		case WLAN_CATEGORY_PUBLIC:
+			action_code = *(payload + sizeof(struct ieee80211_hdr)
+					+ 1);
+			if (action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) {
+				addr2 = ieee_hdr->addr2;
+				mwifiex_dbg(priv->adapter, INFO,
+					    "TDLS discovery response %pM nf=%d, snr=%d\n",
+					    addr2, rx_pd->nf, rx_pd->snr);
+				mwifiex_auto_tdls_update_peer_signal(priv,
+								     addr2,
+								     rx_pd->snr,
+								     rx_pd->nf);
+			}
+			break;
+		case WLAN_CATEGORY_BACK:
+			/*we dont indicate BACK action frames to cfg80211*/
+			mwifiex_dbg(priv->adapter, INFO,
+				    "drop BACK action frames");
+			return -1;
+		default:
+			mwifiex_dbg(priv->adapter, INFO,
+				    "unknown public action frame category %d\n",
+				    category);
 		}
-		break;
 	default:
-		dev_dbg(priv->adapter->dev,
-			"unknown mgmt frame subytpe %#x\n", stype);
+		mwifiex_dbg(priv->adapter, INFO,
+		    "unknown mgmt frame subtype %#x\n", stype);
+		return 0;
 	}
 
 	return 0;
@@ -369,8 +387,8 @@ mwifiex_process_mgmt_packet(struct mwifiex_private *priv,
 
 	if (!priv->mgmt_frame_mask ||
 	    priv->wdev.iftype == NL80211_IFTYPE_UNSPECIFIED) {
-		dev_dbg(priv->adapter->dev,
-			"do not receive mgmt frames on uninitialized intf");
+		mwifiex_dbg(priv->adapter, ERROR,
+			    "do not receive mgmt frames on uninitialized intf");
 		return -1;
 	}
 
@@ -383,8 +401,9 @@ mwifiex_process_mgmt_packet(struct mwifiex_private *priv,
 
 	ieee_hdr = (void *)skb->data;
 	if (ieee80211_is_mgmt(ieee_hdr->frame_control)) {
-		mwifiex_parse_mgmt_packet(priv, (u8 *)ieee_hdr,
-					  pkt_len, rx_pd);
+		if (mwifiex_parse_mgmt_packet(priv, (u8 *)ieee_hdr,
+					      pkt_len, rx_pd))
+			return -1;
 	}
 	/* Remove address4 */
 	memmove(skb->data + sizeof(struct ieee80211_hdr_3addr),
@@ -412,12 +431,25 @@ mwifiex_process_mgmt_packet(struct mwifiex_private *priv,
  */
 int mwifiex_recv_packet(struct mwifiex_private *priv, struct sk_buff *skb)
 {
+	struct mwifiex_sta_node *src_node;
+	struct ethhdr *p_ethhdr;
+
 	if (!skb)
 		return -1;
 
 	priv->stats.rx_bytes += skb->len;
 	priv->stats.rx_packets++;
 
+	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
+		p_ethhdr = (void *)skb->data;
+		src_node = mwifiex_get_sta_entry(priv, p_ethhdr->h_source);
+		if (src_node) {
+			src_node->stats.last_rx = jiffies;
+			src_node->stats.rx_bytes += skb->len;
+			src_node->stats.rx_packets++;
+		}
+	}
+
 	skb->dev = priv->netdev;
 	skb->protocol = eth_type_trans(skb, priv->netdev);
 	skb->ip_summed = CHECKSUM_NONE;
@@ -464,13 +496,14 @@ int mwifiex_recv_packet(struct mwifiex_private *priv, struct sk_buff *skb)
 int mwifiex_complete_cmd(struct mwifiex_adapter *adapter,
 			 struct cmd_ctrl_node *cmd_node)
 {
-	dev_dbg(adapter->dev, "cmd completed: status=%d\n",
-		adapter->cmd_wait_q.status);
+	mwifiex_dbg(adapter, CMD,
+		    "cmd completed: status=%d\n",
+		    adapter->cmd_wait_q.status);
 
 	*(cmd_node->condition) = true;
 
 	if (adapter->cmd_wait_q.status == -ETIMEDOUT)
-		dev_err(adapter->dev, "cmd timeout\n");
+		mwifiex_dbg(adapter, ERROR, "cmd timeout\n");
 	else
 		wake_up_interruptible(&adapter->cmd_wait_q.wait);
 
@@ -536,13 +569,16 @@ void
 mwifiex_set_sta_ht_cap(struct mwifiex_private *priv, const u8 *ies,
 		       int ies_len, struct mwifiex_sta_node *node)
 {
+	struct ieee_types_header *ht_cap_ie;
 	const struct ieee80211_ht_cap *ht_cap;
 
 	if (!ies)
 		return;
 
-	ht_cap = (void *)cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, ies, ies_len);
-	if (ht_cap) {
+	ht_cap_ie = (void *)cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, ies,
+					     ies_len);
+	if (ht_cap_ie) {
+		ht_cap = (void *)(ht_cap_ie + 1);
 		node->is_11n_enabled = 1;
 		node->max_amsdu = le16_to_cpu(ht_cap->cap_info) &
 				  IEEE80211_HT_CAP_MAX_AMSDU ?
diff --git a/drivers/net/wireless/mwifiex/wmm.c b/drivers/net/wireless/mwifiex/wmm.c
index b2e99569a0f8..a8ea21c3340c 100644
--- a/drivers/net/wireless/mwifiex/wmm.c
+++ b/drivers/net/wireless/mwifiex/wmm.c
@@ -107,7 +107,7 @@ mwifiex_wmm_allocate_ralist_node(struct mwifiex_adapter *adapter, const u8 *ra)
 
 	ra_list->total_pkt_count = 0;
 
-	dev_dbg(adapter->dev, "info: allocated ra_list %p\n", ra_list);
+	mwifiex_dbg(adapter, INFO, "info: allocated ra_list %p\n", ra_list);
 
 	return ra_list;
 }
@@ -150,7 +150,8 @@ void mwifiex_ralist_add(struct mwifiex_private *priv, const u8 *ra)
 
 	for (i = 0; i < MAX_NUM_TID; ++i) {
 		ra_list = mwifiex_wmm_allocate_ralist_node(adapter, ra);
-		dev_dbg(adapter->dev, "info: created ra_list %p\n", ra_list);
+		mwifiex_dbg(adapter, INFO,
+			    "info: created ra_list %p\n", ra_list);
 
 		if (!ra_list)
 			break;
@@ -178,8 +179,8 @@ void mwifiex_ralist_add(struct mwifiex_private *priv, const u8 *ra)
 			spin_unlock_irqrestore(&priv->sta_list_spinlock, flags);
 		}
 
-		dev_dbg(adapter->dev, "data: ralist %p: is_11n_enabled=%d\n",
-			ra_list, ra_list->is_11n_enabled);
+		mwifiex_dbg(adapter, DATA, "data: ralist %p: is_11n_enabled=%d\n",
+			    ra_list, ra_list->is_11n_enabled);
 
 		if (ra_list->is_11n_enabled) {
 			ra_list->ba_pkt_count = 0;
@@ -241,11 +242,12 @@ mwifiex_wmm_setup_queue_priorities(struct mwifiex_private *priv,
 		return;
 	}
 
-	dev_dbg(priv->adapter->dev, "info: WMM Parameter IE: version=%d, "
-		"qos_info Parameter Set Count=%d, Reserved=%#x\n",
-		wmm_ie->vend_hdr.version, wmm_ie->qos_info_bitmap &
-		IEEE80211_WMM_IE_AP_QOSINFO_PARAM_SET_CNT_MASK,
-		wmm_ie->reserved);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: WMM Parameter IE: version=%d,\t"
+		    "qos_info Parameter Set Count=%d, Reserved=%#x\n",
+		    wmm_ie->vend_hdr.version, wmm_ie->qos_info_bitmap &
+		    IEEE80211_WMM_IE_AP_QOSINFO_PARAM_SET_CNT_MASK,
+		    wmm_ie->reserved);
 
 	for (num_ac = 0; num_ac < ARRAY_SIZE(wmm_ie->ac_params); num_ac++) {
 		u8 ecw = wmm_ie->ac_params[num_ac].ecw_bitmap;
@@ -257,10 +259,10 @@ mwifiex_wmm_setup_queue_priorities(struct mwifiex_private *priv,
 		priv->wmm.queue_priority[ac_idx] = ac_idx;
 		tmp[ac_idx] = avg_back_off;
 
-		dev_dbg(priv->adapter->dev,
-			"info: WMM: CWmax=%d CWmin=%d Avg Back-off=%d\n",
-			(1 << ((ecw & MWIFIEX_ECW_MAX) >> 4)) - 1,
-			cw_min, avg_back_off);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: WMM: CWmax=%d CWmin=%d Avg Back-off=%d\n",
+			    (1 << ((ecw & MWIFIEX_ECW_MAX) >> 4)) - 1,
+			    cw_min, avg_back_off);
 		mwifiex_wmm_ac_debug_print(&wmm_ie->ac_params[num_ac]);
 	}
 
@@ -333,8 +335,8 @@ mwifiex_wmm_setup_ac_downgrade(struct mwifiex_private *priv)
 {
 	int ac_val;
 
-	dev_dbg(priv->adapter->dev, "info: WMM: AC Priorities:"
-			"BK(0), BE(1), VI(2), VO(3)\n");
+	mwifiex_dbg(priv->adapter, INFO, "info: WMM: AC Priorities:\t"
+		    "BK(0), BE(1), VI(2), VO(3)\n");
 
 	if (!priv->wmm_enabled) {
 		/* WMM is not enabled, default priorities */
@@ -346,9 +348,10 @@ mwifiex_wmm_setup_ac_downgrade(struct mwifiex_private *priv)
 			priv->wmm.ac_down_graded_vals[ac_val]
 				= mwifiex_wmm_eval_downgrade_ac(priv,
 						(enum mwifiex_wmm_ac_e) ac_val);
-			dev_dbg(priv->adapter->dev,
-				"info: WMM: AC PRIO %d maps to %d\n",
-				ac_val, priv->wmm.ac_down_graded_vals[ac_val]);
+			mwifiex_dbg(priv->adapter, INFO,
+				    "info: WMM: AC PRIO %d maps to %d\n",
+				    ac_val,
+				    priv->wmm.ac_down_graded_vals[ac_val]);
 		}
 	}
 }
@@ -428,6 +431,15 @@ mwifiex_wmm_init(struct mwifiex_adapter *adapter)
 							priv->tos_to_tid_inv[i];
 		}
 
+		priv->aggr_prio_tbl[6].amsdu
+					= priv->aggr_prio_tbl[6].ampdu_ap
+					= priv->aggr_prio_tbl[6].ampdu_user
+					= BA_STREAM_NOT_ALLOWED;
+
+		priv->aggr_prio_tbl[7].amsdu = priv->aggr_prio_tbl[7].ampdu_ap
+					= priv->aggr_prio_tbl[7].ampdu_user
+					= BA_STREAM_NOT_ALLOWED;
+
 		mwifiex_set_ba_params(priv);
 		mwifiex_reset_11n_rx_seq_num(priv);
 
@@ -512,8 +524,8 @@ static void mwifiex_wmm_delete_all_ralist(struct mwifiex_private *priv)
 	int i;
 
 	for (i = 0; i < MAX_NUM_TID; ++i) {
-		dev_dbg(priv->adapter->dev,
-			"info: ra_list: freeing buf for tid %d\n", i);
+		mwifiex_dbg(priv->adapter, INFO,
+			    "info: ra_list: freeing buf for tid %d\n", i);
 		list_for_each_entry_safe(ra_list, tmp_node,
 					 &priv->wmm.tid_tbl_ptr[i].ra_list,
 					 list) {
@@ -685,14 +697,15 @@ mwifiex_wmm_add_buf_txqueue(struct mwifiex_private *priv,
 	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_STA &&
 	    ISSUPP_TDLS_ENABLED(adapter->fw_cap_info)) {
 		if (ntohs(eth_hdr->h_proto) == ETH_P_TDLS)
-			dev_dbg(adapter->dev,
-				"TDLS setup packet for %pM. Don't block\n", ra);
+			mwifiex_dbg(adapter, DATA,
+				    "TDLS setup packet for %pM.\t"
+				    "Don't block\n", ra);
 		else if (memcmp(priv->cfg_bssid, ra, ETH_ALEN))
 			tdls_status = mwifiex_get_tdls_link_status(priv, ra);
 	}
 
 	if (!priv->media_connected && !mwifiex_is_skb_mgmt_frame(skb)) {
-		dev_dbg(adapter->dev, "data: drop packet in disconnect\n");
+		mwifiex_dbg(adapter, DATA, "data: drop packet in disconnect\n");
 		mwifiex_write_data_complete(adapter, skb, 0, -1);
 		return;
 	}
@@ -773,6 +786,7 @@ int mwifiex_ret_wmm_get_status(struct mwifiex_private *priv,
 {
 	u8 *curr = (u8 *) &resp->params.get_wmm_status;
 	uint16_t resp_len = le16_to_cpu(resp->size), tlv_len;
+	int mask = IEEE80211_WMM_IE_AP_QOSINFO_PARAM_SET_CNT_MASK;
 	bool valid = true;
 
 	struct mwifiex_ie_types_data *tlv_hdr;
@@ -780,8 +794,9 @@ int mwifiex_ret_wmm_get_status(struct mwifiex_private *priv,
 	struct ieee_types_wmm_parameter *wmm_param_ie = NULL;
 	struct mwifiex_wmm_ac_status *ac_status;
 
-	dev_dbg(priv->adapter->dev, "info: WMM: WMM_GET_STATUS cmdresp received: %d\n",
-		resp_len);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: WMM: WMM_GET_STATUS cmdresp received: %d\n",
+		    resp_len);
 
 	while ((resp_len >= sizeof(tlv_hdr->header)) && valid) {
 		tlv_hdr = (struct mwifiex_ie_types_data *) curr;
@@ -795,12 +810,12 @@ int mwifiex_ret_wmm_get_status(struct mwifiex_private *priv,
 			tlv_wmm_qstatus =
 				(struct mwifiex_ie_types_wmm_queue_status *)
 				tlv_hdr;
-			dev_dbg(priv->adapter->dev,
-				"info: CMD_RESP: WMM_GET_STATUS:"
-				" QSTATUS TLV: %d, %d, %d\n",
-				tlv_wmm_qstatus->queue_index,
-				tlv_wmm_qstatus->flow_required,
-				tlv_wmm_qstatus->disabled);
+			mwifiex_dbg(priv->adapter, CMD,
+				    "info: CMD_RESP: WMM_GET_STATUS:\t"
+				    "QSTATUS TLV: %d, %d, %d\n",
+				    tlv_wmm_qstatus->queue_index,
+				    tlv_wmm_qstatus->flow_required,
+				    tlv_wmm_qstatus->disabled);
 
 			ac_status = &priv->wmm.ac_status[tlv_wmm_qstatus->
 							 queue_index];
@@ -823,11 +838,10 @@ int mwifiex_ret_wmm_get_status(struct mwifiex_private *priv,
 			wmm_param_ie->vend_hdr.element_id =
 						WLAN_EID_VENDOR_SPECIFIC;
 
-			dev_dbg(priv->adapter->dev,
-				"info: CMD_RESP: WMM_GET_STATUS:"
-				" WMM Parameter Set Count: %d\n",
-				wmm_param_ie->qos_info_bitmap &
-				IEEE80211_WMM_IE_AP_QOSINFO_PARAM_SET_CNT_MASK);
+			mwifiex_dbg(priv->adapter, CMD,
+				    "info: CMD_RESP: WMM_GET_STATUS:\t"
+				    "WMM Parameter Set Count: %d\n",
+				    wmm_param_ie->qos_info_bitmap & mask);
 
 			memcpy((u8 *) &priv->curr_bss_params.bss_descriptor.
 			       wmm_ie, wmm_param_ie,
@@ -875,9 +889,9 @@ mwifiex_wmm_process_association_req(struct mwifiex_private *priv,
 	if (!wmm_ie)
 		return 0;
 
-	dev_dbg(priv->adapter->dev,
-		"info: WMM: process assoc req: bss->wmm_ie=%#x\n",
-		wmm_ie->vend_hdr.element_id);
+	mwifiex_dbg(priv->adapter, INFO,
+		    "info: WMM: process assoc req: bss->wmm_ie=%#x\n",
+		    wmm_ie->vend_hdr.element_id);
 
 	if ((priv->wmm_required ||
 	     (ht_cap && (priv->adapter->config_bands & BAND_GN ||
@@ -927,8 +941,8 @@ mwifiex_wmm_compute_drv_pkt_delay(struct mwifiex_private *priv,
 	 */
 	ret_val = (u8) (min(queue_delay, priv->wmm.drv_pkt_delay_max) >> 1);
 
-	dev_dbg(priv->adapter->dev, "data: WMM: Pkt Delay: %d ms,"
-				" %d ms sent to FW\n", queue_delay, ret_val);
+	mwifiex_dbg(priv->adapter, DATA, "data: WMM: Pkt Delay: %d ms,\t"
+		    "%d ms sent to FW\n", queue_delay, ret_val);
 
 	return ret_val;
 }
@@ -1082,14 +1096,15 @@ mwifiex_send_single_packet(struct mwifiex_private *priv,
 	if (skb_queue_empty(&ptr->skb_head)) {
 		spin_unlock_irqrestore(&priv->wmm.ra_list_spinlock,
 				       ra_list_flags);
-		dev_dbg(adapter->dev, "data: nothing to send\n");
+		mwifiex_dbg(adapter, DATA, "data: nothing to send\n");
 		return;
 	}
 
 	skb = skb_dequeue(&ptr->skb_head);
 
 	tx_info = MWIFIEX_SKB_TXCB(skb);
-	dev_dbg(adapter->dev, "data: dequeuing the packet %p %p\n", ptr, skb);
+	mwifiex_dbg(adapter, DATA,
+		    "data: dequeuing the packet %p %p\n", ptr, skb);
 
 	ptr->total_pkt_count--;
 
@@ -1205,7 +1220,7 @@ mwifiex_send_processed_packet(struct mwifiex_private *priv,
 
 	switch (ret) {
 	case -EBUSY:
-		dev_dbg(adapter->dev, "data: -EBUSY is returned\n");
+		mwifiex_dbg(adapter, ERROR, "data: -EBUSY is returned\n");
 		spin_lock_irqsave(&priv->wmm.ra_list_spinlock, ra_list_flags);
 
 		if (!mwifiex_is_ralist_valid(priv, ptr, ptr_index)) {
@@ -1224,7 +1239,7 @@ mwifiex_send_processed_packet(struct mwifiex_private *priv,
 	case -1:
 		if (adapter->iface_type != MWIFIEX_PCIE)
 			adapter->data_sent = false;
-		dev_err(adapter->dev, "host_to_card failed: %#x\n", ret);
+		mwifiex_dbg(adapter, ERROR, "host_to_card failed: %#x\n", ret);
 		adapter->dbg.num_tx_host_to_card_failure++;
 		mwifiex_write_data_complete(adapter, skb, 0, ret);
 		break;
@@ -1263,7 +1278,7 @@ mwifiex_dequeue_tx_packet(struct mwifiex_adapter *adapter)
 
 	tid = mwifiex_get_tid(ptr);
 
-	dev_dbg(adapter->dev, "data: tid=%d\n", tid);
+	mwifiex_dbg(adapter, DATA, "data: tid=%d\n", tid);
 
 	spin_lock_irqsave(&priv->wmm.ra_list_spinlock, flags);
 	if (!mwifiex_is_ralist_valid(priv, ptr, ptr_index)) {
diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index 95921167b53f..77361af68b18 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -2380,7 +2380,7 @@ mwl8k_set_ht_caps(struct ieee80211_hw *hw,
 	if (cap & MWL8K_CAP_GREENFIELD)
 		band->ht_cap.cap |= IEEE80211_HT_CAP_GRN_FLD;
 	if (cap & MWL8K_CAP_AMPDU) {
-		hw->flags |= IEEE80211_HW_AMPDU_AGGREGATION;
+		ieee80211_hw_set(hw, AMPDU_AGGREGATION);
 		band->ht_cap.ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
 		band->ht_cap.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE;
 	}
@@ -5192,7 +5192,7 @@ mwl8k_configure_filter_sniffer(struct ieee80211_hw *hw,
 		priv->sniffer_enabled = true;
 	}
 
-	*total_flags &=	FIF_PROMISC_IN_BSS | FIF_ALLMULTI |
+	*total_flags &=	FIF_ALLMULTI |
 			FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL |
 			FIF_OTHER_BSS;
 
@@ -5431,7 +5431,7 @@ mwl8k_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	u8 *addr = sta->addr, idx;
 	struct mwl8k_sta *sta_info = MWL8K_STA(sta);
 
-	if (!(hw->flags & IEEE80211_HW_AMPDU_AGGREGATION))
+	if (!ieee80211_hw_check(hw, AMPDU_AGGREGATION))
 		return -ENOTSUPP;
 
 	spin_lock(&priv->stream_lock);
@@ -6076,14 +6076,15 @@ static int mwl8k_firmware_load_success(struct mwl8k_priv *priv)
 	hw->queues = MWL8K_TX_WMM_QUEUES;
 
 	/* Set rssi values to dBm */
-	hw->flags |= IEEE80211_HW_SIGNAL_DBM | IEEE80211_HW_HAS_RATE_CONTROL;
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, HAS_RATE_CONTROL);
 
 	/*
 	 * Ask mac80211 to not to trigger PS mode
 	 * based on PM bit of incoming frames.
 	 */
 	if (priv->ap_fw)
-		hw->flags |= IEEE80211_HW_AP_LINK_PS;
+		ieee80211_hw_set(hw, AP_LINK_PS);
 
 	hw->vif_data_size = sizeof(struct mwl8k_vif);
 	hw->sta_data_size = sizeof(struct mwl8k_sta);
diff --git a/drivers/net/wireless/p54/fwio.c b/drivers/net/wireless/p54/fwio.c
index 275408eaf95e..257a9eadd595 100644
--- a/drivers/net/wireless/p54/fwio.c
+++ b/drivers/net/wireless/p54/fwio.c
@@ -351,8 +351,7 @@ int p54_setup_mac(struct p54_common *priv)
 		 * "TRANSPARENT and PROMISCUOUS are mutually exclusive"
 		 * STSW45X0C LMAC API - page 12
 		 */
-		if (((priv->filter_flags & FIF_PROMISC_IN_BSS) ||
-		     (priv->filter_flags & FIF_OTHER_BSS)) &&
+		if (priv->filter_flags & FIF_OTHER_BSS &&
 		    (mode != P54_FILTER_TYPE_PROMISCUOUS))
 			mode |= P54_FILTER_TYPE_TRANSPARENT;
 	} else {
diff --git a/drivers/net/wireless/p54/led.c b/drivers/net/wireless/p54/led.c
index 1f6fd5ff5531..9a8fedd3c0f5 100644
--- a/drivers/net/wireless/p54/led.c
+++ b/drivers/net/wireless/p54/led.c
@@ -83,7 +83,7 @@ static void p54_led_brightness_set(struct led_classdev *led_dev,
 
 static int p54_register_led(struct p54_common *priv,
 			    unsigned int led_index,
-			    char *name, char *trigger)
+			    char *name, const char *trigger)
 {
 	struct p54_led_dev *led = &priv->leds[led_index];
 	int err;
diff --git a/drivers/net/wireless/p54/main.c b/drivers/net/wireless/p54/main.c
index e79674f73dc5..7805864e76f9 100644
--- a/drivers/net/wireless/p54/main.c
+++ b/drivers/net/wireless/p54/main.c
@@ -395,13 +395,11 @@ static void p54_configure_filter(struct ieee80211_hw *dev,
 {
 	struct p54_common *priv = dev->priv;
 
-	*total_flags &= FIF_PROMISC_IN_BSS |
-			FIF_ALLMULTI |
-			FIF_OTHER_BSS;
+	*total_flags &= FIF_ALLMULTI | FIF_OTHER_BSS;
 
 	priv->filter_flags = *total_flags;
 
-	if (changed_flags & (FIF_PROMISC_IN_BSS | FIF_OTHER_BSS))
+	if (changed_flags & FIF_OTHER_BSS)
 		p54_setup_mac(priv);
 
 	if (changed_flags & FIF_ALLMULTI || multicast)
@@ -748,12 +746,12 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
 	spin_lock_init(&priv->tx_stats_lock);
 	skb_queue_head_init(&priv->tx_queue);
 	skb_queue_head_init(&priv->tx_pending);
-	dev->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		     IEEE80211_HW_SIGNAL_DBM |
-		     IEEE80211_HW_SUPPORTS_PS |
-		     IEEE80211_HW_PS_NULLFUNC_STACK |
-		     IEEE80211_HW_MFP_CAPABLE |
-		     IEEE80211_HW_REPORTS_TX_ACK_STATUS;
+	ieee80211_hw_set(dev, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(dev, MFP_CAPABLE);
+	ieee80211_hw_set(dev, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(dev, SUPPORTS_PS);
+	ieee80211_hw_set(dev, RX_INCLUDES_FCS);
+	ieee80211_hw_set(dev, SIGNAL_DBM);
 
 	dev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 				      BIT(NL80211_IFTYPE_ADHOC) |
diff --git a/drivers/net/wireless/ray_cs.c b/drivers/net/wireless/ray_cs.c
index 477f86354dc5..0881ba8535f4 100644
--- a/drivers/net/wireless/ray_cs.c
+++ b/drivers/net/wireless/ray_cs.c
@@ -143,7 +143,7 @@ static int psm;
 static char *essid;
 
 /* Default to encapsulation unless translation requested */
-static bool translate = 1;
+static bool translate = true;
 
 static int country = USA;
 
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index d72ff8e7125d..71a825c750cf 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -356,9 +356,9 @@ struct ndis_80211_pmkid {
 #define CAP_MODE_80211G		4
 #define CAP_MODE_MASK		7
 
-#define WORK_LINK_UP		(1<<0)
-#define WORK_LINK_DOWN		(1<<1)
-#define WORK_SET_MULTICAST_LIST	(1<<2)
+#define WORK_LINK_UP		0
+#define WORK_LINK_DOWN		1
+#define WORK_SET_MULTICAST_LIST	2
 
 #define RNDIS_WLAN_ALG_NONE	0
 #define RNDIS_WLAN_ALG_WEP	(1<<0)
@@ -2861,7 +2861,7 @@ static void rndis_wlan_do_link_down_work(struct usbnet *usbdev)
 
 		deauthenticate(usbdev);
 
-		cfg80211_disconnected(usbdev->net, 0, NULL, 0, GFP_KERNEL);
+		cfg80211_disconnected(usbdev->net, 0, NULL, 0, true, GFP_KERNEL);
 	}
 
 	netif_carrier_off(usbdev->net);
diff --git a/drivers/net/wireless/rsi/rsi_91x_mac80211.c b/drivers/net/wireless/rsi/rsi_91x_mac80211.c
index aeaf87bb5518..7e804324bfa7 100644
--- a/drivers/net/wireless/rsi/rsi_91x_mac80211.c
+++ b/drivers/net/wireless/rsi/rsi_91x_mac80211.c
@@ -1062,10 +1062,9 @@ int rsi_mac80211_attach(struct rsi_common *common)
 	hw->priv = adapter;
 	adapter->hw = hw;
 
-	hw->flags = IEEE80211_HW_SIGNAL_DBM |
-		    IEEE80211_HW_HAS_RATE_CONTROL |
-		    IEEE80211_HW_AMPDU_AGGREGATION |
-		    0;
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, HAS_RATE_CONTROL);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
 
 	hw->queues = MAX_HW_QUEUES;
 	hw->extra_tx_headroom = RSI_NEEDED_HEADROOM;
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index bdf5590ba304..9a3966cd6fbe 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -273,10 +273,8 @@ static void rt2400pci_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_CONTROL,
 			   !(filter_flags & FIF_CONTROL));
-	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME,
-			   !(filter_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME, 1);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_TODS,
-			   !(filter_flags & FIF_PROMISC_IN_BSS) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_VERSION_ERROR, 1);
 	rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
@@ -1576,10 +1574,10 @@ static int rt2400pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all hw fields.
 	 */
-	rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-			       IEEE80211_HW_SIGNAL_DBM |
-			       IEEE80211_HW_SUPPORTS_PS |
-			       IEEE80211_HW_PS_NULLFUNC_STACK;
+	ieee80211_hw_set(rt2x00dev->hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(rt2x00dev->hw, SUPPORTS_PS);
+	ieee80211_hw_set(rt2x00dev->hw, HOST_BROADCAST_PS_BUFFERING);
+	ieee80211_hw_set(rt2x00dev->hw, SIGNAL_DBM);
 
 	SET_IEEE80211_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index 79f4fe65a119..1a6740b4d396 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -274,10 +274,8 @@ static void rt2500pci_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_CONTROL,
 			   !(filter_flags & FIF_CONTROL));
-	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME,
-			   !(filter_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME, 1);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_TODS,
-			   !(filter_flags & FIF_PROMISC_IN_BSS) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_VERSION_ERROR, 1);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_MCAST,
@@ -1871,10 +1869,10 @@ static int rt2500pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all hw fields.
 	 */
-	rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-			       IEEE80211_HW_SIGNAL_DBM |
-			       IEEE80211_HW_SUPPORTS_PS |
-			       IEEE80211_HW_PS_NULLFUNC_STACK;
+	ieee80211_hw_set(rt2x00dev->hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(rt2x00dev->hw, SUPPORTS_PS);
+	ieee80211_hw_set(rt2x00dev->hw, HOST_BROADCAST_PS_BUFFERING);
+	ieee80211_hw_set(rt2x00dev->hw, SIGNAL_DBM);
 
 	SET_IEEE80211_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 05c64597838d..b50d873145d5 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -434,10 +434,8 @@ static void rt2500usb_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_CONTROL,
 			   !(filter_flags & FIF_CONTROL));
-	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_NOT_TO_ME,
-			   !(filter_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_NOT_TO_ME, 1);
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_TODS,
-			   !(filter_flags & FIF_PROMISC_IN_BSS) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_VERSION_ERROR, 1);
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_MULTICAST,
@@ -1698,11 +1696,10 @@ static int rt2500usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	 * multicast and broadcast traffic immediately instead of buffering it
 	 * infinitly and thus dropping it after some time.
 	 */
-	rt2x00dev->hw->flags =
-	    IEEE80211_HW_RX_INCLUDES_FCS |
-	    IEEE80211_HW_SIGNAL_DBM |
-	    IEEE80211_HW_SUPPORTS_PS |
-	    IEEE80211_HW_PS_NULLFUNC_STACK;
+	ieee80211_hw_set(rt2x00dev->hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(rt2x00dev->hw, SUPPORTS_PS);
+	ieee80211_hw_set(rt2x00dev->hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(rt2x00dev->hw, SIGNAL_DBM);
 
 	/*
 	 * Disable powersaving as default.
diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index ebd5625d13f1..95c1d7c0a2f3 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/rt2x00/rt2800.h
@@ -2961,6 +2961,15 @@ enum rt2800_eeprom_word {
 #define BCN_TBTT_OFFSET 64
 
 /*
+ * Hardware has 255 WCID table entries. First 32 entries are reserved for
+ * shared keys. Since parts of the pairwise key table might be shared with
+ * the beacon frame buffers 6 & 7 we could only use the first 222 entries.
+ */
+#define WCID_START	33
+#define WCID_END	222
+#define STA_IDS_SIZE	(WCID_END - WCID_START + 2)
+
+/*
  * RT2800 driver data structure
  */
 struct rt2800_drv_data {
@@ -2971,6 +2980,7 @@ struct rt2800_drv_data {
 	u8 txmixer_gain_24g;
 	u8 txmixer_gain_5g;
 	unsigned int tbtt_tick;
+	DECLARE_BITMAP(sta_ids, STA_IDS_SIZE);
 };
 
 #endif /* RT2800_H */
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index be2d54f257b1..9524564f873b 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -1381,38 +1381,6 @@ int rt2800_config_shared_key(struct rt2x00_dev *rt2x00dev,
 }
 EXPORT_SYMBOL_GPL(rt2800_config_shared_key);
 
-static inline int rt2800_find_wcid(struct rt2x00_dev *rt2x00dev)
-{
-	struct mac_wcid_entry wcid_entry;
-	int idx;
-	u32 offset;
-
-	/*
-	 * Search for the first free WCID entry and return the corresponding
-	 * index.
-	 *
-	 * Make sure the WCID starts _after_ the last possible shared key
-	 * entry (>32).
-	 *
-	 * Since parts of the pairwise key table might be shared with
-	 * the beacon frame buffers 6 & 7 we should only write into the
-	 * first 222 entries.
-	 */
-	for (idx = 33; idx <= 222; idx++) {
-		offset = MAC_WCID_ENTRY(idx);
-		rt2800_register_multiread(rt2x00dev, offset, &wcid_entry,
-					  sizeof(wcid_entry));
-		if (is_broadcast_ether_addr(wcid_entry.mac))
-			return idx;
-	}
-
-	/*
-	 * Use -1 to indicate that we don't have any more space in the WCID
-	 * table.
-	 */
-	return -1;
-}
-
 int rt2800_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 			       struct rt2x00lib_crypto *crypto,
 			       struct ieee80211_key_conf *key)
@@ -1425,7 +1393,7 @@ int rt2800_config_pairwise_key(struct rt2x00_dev *rt2x00dev,
 		 * Allow key configuration only for STAs that are
 		 * known by the hw.
 		 */
-		if (crypto->wcid < 0)
+		if (crypto->wcid > WCID_END)
 			return -ENOSPC;
 		key->hw_key_idx = crypto->wcid;
 
@@ -1455,11 +1423,13 @@ int rt2800_sta_add(struct rt2x00_dev *rt2x00dev, struct ieee80211_vif *vif,
 {
 	int wcid;
 	struct rt2x00_sta *sta_priv = sta_to_rt2x00_sta(sta);
+	struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
 
 	/*
-	 * Find next free WCID.
+	 * Search for the first free WCID entry and return the corresponding
+	 * index.
 	 */
-	wcid = rt2800_find_wcid(rt2x00dev);
+	wcid = find_first_zero_bit(drv_data->sta_ids, STA_IDS_SIZE) + WCID_START;
 
 	/*
 	 * Store selected wcid even if it is invalid so that we can
@@ -1471,9 +1441,11 @@ int rt2800_sta_add(struct rt2x00_dev *rt2x00dev, struct ieee80211_vif *vif,
 	 * No space left in the device, however, we can still communicate
 	 * with the STA -> No error.
 	 */
-	if (wcid < 0)
+	if (wcid > WCID_END)
 		return 0;
 
+	__set_bit(wcid - WCID_START, drv_data->sta_ids);
+
 	/*
 	 * Clean up WCID attributes and write STA address to the device.
 	 */
@@ -1487,11 +1459,16 @@ EXPORT_SYMBOL_GPL(rt2800_sta_add);
 
 int rt2800_sta_remove(struct rt2x00_dev *rt2x00dev, int wcid)
 {
+	struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
+
+	if (wcid > WCID_END)
+		return 0;
 	/*
 	 * Remove WCID entry, no need to clean the attributes as they will
 	 * get renewed when the WCID is reused.
 	 */
 	rt2800_config_wcid(rt2x00dev, NULL, wcid);
+	__clear_bit(wcid - WCID_START, drv_data->sta_ids);
 
 	return 0;
 }
@@ -1513,8 +1490,7 @@ void rt2800_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_PHY_ERROR,
 			   !(filter_flags & FIF_PLCPFAIL));
-	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_NOT_TO_ME,
-			   !(filter_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_NOT_TO_ME, 1);
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_NOT_MY_BSSD, 0);
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_VER_ERROR, 1);
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_MULTICAST,
@@ -7498,13 +7474,12 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all hw fields.
 	 */
-	rt2x00dev->hw->flags =
-	    IEEE80211_HW_SIGNAL_DBM |
-	    IEEE80211_HW_SUPPORTS_PS |
-	    IEEE80211_HW_PS_NULLFUNC_STACK |
-	    IEEE80211_HW_AMPDU_AGGREGATION |
-	    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-	    IEEE80211_HW_SUPPORTS_HT_CCK_RATES;
+	ieee80211_hw_set(rt2x00dev->hw, SUPPORTS_HT_CCK_RATES);
+	ieee80211_hw_set(rt2x00dev->hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(rt2x00dev->hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(rt2x00dev->hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(rt2x00dev->hw, SIGNAL_DBM);
+	ieee80211_hw_set(rt2x00dev->hw, SUPPORTS_PS);
 
 	/*
 	 * Don't set IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING for USB devices
@@ -7514,8 +7489,7 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	 * infinitly and thus dropping it after some time.
 	 */
 	if (!rt2x00_is_usb(rt2x00dev))
-		rt2x00dev->hw->flags |=
-			IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+		ieee80211_hw_set(rt2x00dev->hw, HOST_BROADCAST_PS_BUFFERING);
 
 	SET_IEEE80211_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
@@ -7818,21 +7792,25 @@ EXPORT_SYMBOL_GPL(rt2800_probe_hw);
 /*
  * IEEE80211 stack callback functions.
  */
-void rt2800_get_tkip_seq(struct ieee80211_hw *hw, u8 hw_key_idx, u32 *iv32,
-			 u16 *iv16)
+void rt2800_get_key_seq(struct ieee80211_hw *hw,
+			struct ieee80211_key_conf *key,
+			struct ieee80211_key_seq *seq)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	struct mac_iveiv_entry iveiv_entry;
 	u32 offset;
 
-	offset = MAC_IVEIV_ENTRY(hw_key_idx);
+	if (key->cipher != WLAN_CIPHER_SUITE_TKIP)
+		return;
+
+	offset = MAC_IVEIV_ENTRY(key->hw_key_idx);
 	rt2800_register_multiread(rt2x00dev, offset,
 				      &iveiv_entry, sizeof(iveiv_entry));
 
-	memcpy(iv16, &iveiv_entry.iv[0], sizeof(*iv16));
-	memcpy(iv32, &iveiv_entry.iv[4], sizeof(*iv32));
+	memcpy(&seq->tkip.iv16, &iveiv_entry.iv[0], 2);
+	memcpy(&seq->tkip.iv32, &iveiv_entry.iv[4], 4);
 }
-EXPORT_SYMBOL_GPL(rt2800_get_tkip_seq);
+EXPORT_SYMBOL_GPL(rt2800_get_key_seq);
 
 int rt2800_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 {
@@ -7967,11 +7945,11 @@ int rt2800_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	/*
 	 * Don't allow aggregation for stations the hardware isn't aware
 	 * of because tx status reports for frames to an unknown station
-	 * always contain wcid=255 and thus we can't distinguish between
-	 * multiple stations which leads to unwanted situations when the
-	 * hw reorders frames due to aggregation.
+	 * always contain wcid=WCID_END+1 and thus we can't distinguish
+	 * between multiple stations which leads to unwanted situations
+	 * when the hw reorders frames due to aggregation.
 	 */
-	if (sta_priv->wcid < 0)
+	if (sta_priv->wcid > WCID_END)
 		return 1;
 
 	switch (action) {
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.h b/drivers/net/wireless/rt2x00/rt2800lib.h
index 3019db637a4b..1609b8a7f7eb 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.h
+++ b/drivers/net/wireless/rt2x00/rt2800lib.h
@@ -209,8 +209,9 @@ int rt2800_read_eeprom_efuse(struct rt2x00_dev *rt2x00dev);
 
 int rt2800_probe_hw(struct rt2x00_dev *rt2x00dev);
 
-void rt2800_get_tkip_seq(struct ieee80211_hw *hw, u8 hw_key_idx, u32 *iv32,
-			 u16 *iv16);
+void rt2800_get_key_seq(struct ieee80211_hw *hw,
+			struct ieee80211_key_conf *key,
+			struct ieee80211_key_seq *seq);
 int rt2800_set_rts_threshold(struct ieee80211_hw *hw, u32 value);
 int rt2800_conf_tx(struct ieee80211_hw *hw,
 		   struct ieee80211_vif *vif, u16 queue_idx,
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index cc1b3cc73c5a..0af22573a2eb 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -309,7 +309,7 @@ static const struct ieee80211_ops rt2800pci_mac80211_ops = {
 	.sw_scan_start		= rt2x00mac_sw_scan_start,
 	.sw_scan_complete	= rt2x00mac_sw_scan_complete,
 	.get_stats		= rt2x00mac_get_stats,
-	.get_tkip_seq		= rt2800_get_tkip_seq,
+	.get_key_seq		= rt2800_get_key_seq,
 	.set_rts_threshold	= rt2800_set_rts_threshold,
 	.sta_add		= rt2x00mac_sta_add,
 	.sta_remove		= rt2x00mac_sta_remove,
diff --git a/drivers/net/wireless/rt2x00/rt2800soc.c b/drivers/net/wireless/rt2x00/rt2800soc.c
index aaa7aa4cad9d..a985a5a7945e 100644
--- a/drivers/net/wireless/rt2x00/rt2800soc.c
+++ b/drivers/net/wireless/rt2x00/rt2800soc.c
@@ -148,7 +148,7 @@ static const struct ieee80211_ops rt2800soc_mac80211_ops = {
 	.sw_scan_start		= rt2x00mac_sw_scan_start,
 	.sw_scan_complete	= rt2x00mac_sw_scan_complete,
 	.get_stats		= rt2x00mac_get_stats,
-	.get_tkip_seq		= rt2800_get_tkip_seq,
+	.get_key_seq		= rt2800_get_key_seq,
 	.set_rts_threshold	= rt2800_set_rts_threshold,
 	.sta_add		= rt2x00mac_sta_add,
 	.sta_remove		= rt2x00mac_sta_remove,
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index 6ec2466b52b6..5932306084fd 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -835,7 +835,7 @@ static const struct ieee80211_ops rt2800usb_mac80211_ops = {
 	.sw_scan_start		= rt2x00mac_sw_scan_start,
 	.sw_scan_complete	= rt2x00mac_sw_scan_complete,
 	.get_stats		= rt2x00mac_get_stats,
-	.get_tkip_seq		= rt2800_get_tkip_seq,
+	.get_key_seq		= rt2800_get_key_seq,
 	.set_rts_threshold	= rt2800_set_rts_threshold,
 	.sta_add		= rt2x00mac_sta_add,
 	.sta_remove		= rt2x00mac_sta_remove,
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 300876df056f..3c26ee65a415 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -359,8 +359,7 @@ void rt2x00mac_configure_filter(struct ieee80211_hw *hw,
 	    FIF_PLCPFAIL |
 	    FIF_CONTROL |
 	    FIF_PSPOLL |
-	    FIF_OTHER_BSS |
-	    FIF_PROMISC_IN_BSS;
+	    FIF_OTHER_BSS;
 
 	/*
 	 * Apply some rules to the filters:
@@ -369,9 +368,6 @@ void rt2x00mac_configure_filter(struct ieee80211_hw *hw,
 	 * - Multicast filter seems to kill broadcast traffic so never use it.
 	 */
 	*total_flags |= FIF_ALLMULTI;
-	if (*total_flags & FIF_OTHER_BSS ||
-	    *total_flags & FIF_PROMISC_IN_BSS)
-		*total_flags |= FIF_PROMISC_IN_BSS | FIF_OTHER_BSS;
 
 	/*
 	 * If the device has a single filter for all control frames,
@@ -539,16 +535,8 @@ int rt2x00mac_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		      struct ieee80211_sta *sta)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
-	struct rt2x00_sta *sta_priv = sta_to_rt2x00_sta(sta);
 
-	/*
-	 * If there's no space left in the device table store
-	 * -1 as wcid but tell mac80211 everything went ok.
-	 */
-	if (rt2x00dev->ops->lib->sta_add(rt2x00dev, vif, sta))
-		sta_priv->wcid = -1;
-
-	return 0;
+	return rt2x00dev->ops->lib->sta_add(rt2x00dev, vif, sta);
 }
 EXPORT_SYMBOL_GPL(rt2x00mac_sta_add);
 
@@ -558,12 +546,6 @@ int rt2x00mac_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	struct rt2x00_sta *sta_priv = sta_to_rt2x00_sta(sta);
 
-	/*
-	 * If we never sent the STA to the device no need to clean it up.
-	 */
-	if (sta_priv->wcid < 0)
-		return 0;
-
 	return rt2x00dev->ops->lib->sta_remove(rt2x00dev, sta_priv->wcid);
 }
 EXPORT_SYMBOL_GPL(rt2x00mac_sta_remove);
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index 819455009fe4..c0e730ea1b69 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -530,10 +530,8 @@ static void rt61pci_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CONTROL,
 			   !(filter_flags & (FIF_CONTROL | FIF_PSPOLL)));
-	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME,
-			   !(filter_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_TO_DS,
-			   !(filter_flags & FIF_PROMISC_IN_BSS) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_VERSION_ERROR, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_MULTICAST,
@@ -2760,11 +2758,10 @@ static int rt61pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all hw fields.
 	 */
-	rt2x00dev->hw->flags =
-	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-	    IEEE80211_HW_SIGNAL_DBM |
-	    IEEE80211_HW_SUPPORTS_PS |
-	    IEEE80211_HW_PS_NULLFUNC_STACK;
+	ieee80211_hw_set(rt2x00dev->hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(rt2x00dev->hw, SUPPORTS_PS);
+	ieee80211_hw_set(rt2x00dev->hw, HOST_BROADCAST_PS_BUFFERING);
+	ieee80211_hw_set(rt2x00dev->hw, SIGNAL_DBM);
 
 	SET_IEEE80211_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index a5458cf01fb2..7081e13b4fd6 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -480,10 +480,8 @@ static void rt73usb_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CONTROL,
 			   !(filter_flags & (FIF_CONTROL | FIF_PSPOLL)));
-	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME,
-			   !(filter_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_TO_DS,
-			   !(filter_flags & FIF_PROMISC_IN_BSS) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_VERSION_ERROR, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_MULTICAST,
@@ -2107,16 +2105,15 @@ static int rt73usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all hw fields.
 	 *
-	 * Don't set IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING unless we are
+	 * Don't set IEEE80211_HOST_BROADCAST_PS_BUFFERING unless we are
 	 * capable of sending the buffered frames out after the DTIM
 	 * transmission using rt2x00lib_beacondone. This will send out
 	 * multicast and broadcast traffic immediately instead of buffering it
 	 * infinitly and thus dropping it after some time.
 	 */
-	rt2x00dev->hw->flags =
-	    IEEE80211_HW_SIGNAL_DBM |
-	    IEEE80211_HW_SUPPORTS_PS |
-	    IEEE80211_HW_PS_NULLFUNC_STACK;
+	ieee80211_hw_set(rt2x00dev->hw, PS_NULLFUNC_STACK);
+	ieee80211_hw_set(rt2x00dev->hw, SIGNAL_DBM);
+	ieee80211_hw_set(rt2x00dev->hw, SUPPORTS_PS);
 
 	SET_IEEE80211_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
diff --git a/drivers/net/wireless/rtl818x/rtl8180/dev.c b/drivers/net/wireless/rtl818x/rtl8180/dev.c
index 706b844bce00..a43a16fde59d 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/dev.c
@@ -1802,8 +1802,9 @@ static int rtl8180_probe(struct pci_dev *pdev,
 	priv->band.n_bitrates = 4;
 	dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &priv->band;
 
-	dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-		IEEE80211_HW_RX_INCLUDES_FCS;
+	ieee80211_hw_set(dev, HOST_BROADCAST_PS_BUFFERING);
+	ieee80211_hw_set(dev, RX_INCLUDES_FCS);
+
 	dev->vif_data_size = sizeof(struct rtl8180_vif);
 	dev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 					BIT(NL80211_IFTYPE_ADHOC);
@@ -1868,9 +1869,9 @@ static int rtl8180_probe(struct pci_dev *pdev,
 	}
 
 	if (priv->chip_family != RTL818X_CHIP_FAMILY_RTL8180)
-		dev->flags |= IEEE80211_HW_SIGNAL_DBM;
+		ieee80211_hw_set(dev, SIGNAL_DBM);
 	else
-		dev->flags |= IEEE80211_HW_SIGNAL_UNSPEC;
+		ieee80211_hw_set(dev, SIGNAL_UNSPEC);
 
 	rtl8180_eeprom_read(priv);
 
diff --git a/drivers/net/wireless/rtl818x/rtl8187/dev.c b/drivers/net/wireless/rtl818x/rtl8187/dev.c
index 629ad8cfa17b..b7f72f9c7988 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8187/dev.c
@@ -1478,9 +1478,9 @@ static int rtl8187_probe(struct usb_interface *intf,
 	dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &priv->band;
 
 
-	dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-		     IEEE80211_HW_SIGNAL_DBM |
-		     IEEE80211_HW_RX_INCLUDES_FCS;
+	ieee80211_hw_set(dev, RX_INCLUDES_FCS);
+	ieee80211_hw_set(dev, HOST_BROADCAST_PS_BUFFERING);
+	ieee80211_hw_set(dev, SIGNAL_DBM);
 	/* Initialize rate-control variables */
 	dev->max_rates = 1;
 	dev->max_rate_tries = RETRY_COUNT;
diff --git a/drivers/net/wireless/rtlwifi/Kconfig b/drivers/net/wireless/rtlwifi/Kconfig
index 5cf509d346e8..73067cac289c 100644
--- a/drivers/net/wireless/rtlwifi/Kconfig
+++ b/drivers/net/wireless/rtlwifi/Kconfig
@@ -100,7 +100,7 @@ config RTL8821AE
 	select RTLWIFI_PCI
 	select RTLBTCOEXIST
 	---help---
-	This is the driver for Realtek RTL8i821AE/RTL8812AE 802.11av PCIe
+	This is the driver for Realtek RTL8821AE/RTL8812AE 802.11ac PCIe
 	wireless network adapters.
 
 	If you choose to build it as a module, it will be called rtl8821ae
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index 01f56c7df8b5..0517a4f2d3f2 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -394,20 +394,18 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 		}
 	}
 	/* <5> set hw caps */
-	hw->flags = IEEE80211_HW_SIGNAL_DBM |
-	    IEEE80211_HW_RX_INCLUDES_FCS |
-	    IEEE80211_HW_AMPDU_AGGREGATION |
-	    IEEE80211_HW_CONNECTION_MONITOR |
-	    /* IEEE80211_HW_SUPPORTS_CQM_RSSI | */
-	    IEEE80211_HW_MFP_CAPABLE |
-	    IEEE80211_HW_REPORTS_TX_ACK_STATUS | 0;
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(hw, CONNECTION_MONITOR);
+	ieee80211_hw_set(hw, MFP_CAPABLE);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
 
 	/* swlps or hwlps has been set in diff chip in init_sw_vars */
-	if (rtlpriv->psc.swctrl_lps)
-		hw->flags |= IEEE80211_HW_SUPPORTS_PS |
-			IEEE80211_HW_PS_NULLFUNC_STACK |
-			/* IEEE80211_HW_SUPPORTS_DYNAMIC_PS | */
-			0;
+	if (rtlpriv->psc.swctrl_lps) {
+		ieee80211_hw_set(hw, SUPPORTS_PS);
+		ieee80211_hw_set(hw, PS_NULLFUNC_STACK);
+	}
 	hw->wiphy->interface_modes =
 	    BIT(NL80211_IFTYPE_AP) |
 	    BIT(NL80211_IFTYPE_STATION) |
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c
index cefe26991421..f2b9d11adc9e 100644
--- a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c
@@ -1286,8 +1286,11 @@ static void btc8723b2ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
 						    0x12, 0xe1, 0x90);
 			break;
 		case 3:
-			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1c,
-						    0x3, 0xf1, 0x90);
+			/* This call breaks BT when wireless is active -
+			 * comment it out for now until a better fix is found:
+			 * btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1c,
+			 *			    0x3, 0xf1, 0x90);
+			 */
 			break;
 		case 4:
 			btc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x10,
diff --git a/drivers/net/wireless/rtlwifi/core.h b/drivers/net/wireless/rtlwifi/core.h
index 82733c6b8c46..782ac2fc4b28 100644
--- a/drivers/net/wireless/rtlwifi/core.h
+++ b/drivers/net/wireless/rtlwifi/core.h
@@ -27,8 +27,7 @@
 #define __RTL_CORE_H__
 
 #define RTL_SUPPORTED_FILTERS		\
-	(FIF_PROMISC_IN_BSS | \
-	FIF_ALLMULTI | FIF_CONTROL | \
+	(FIF_ALLMULTI | FIF_CONTROL | \
 	FIF_OTHER_BSS | \
 	FIF_FCSFAIL | \
 	FIF_BCN_PRBRESP_PROMISC)
diff --git a/drivers/net/wireless/rtlwifi/regd.c b/drivers/net/wireless/rtlwifi/regd.c
index 1893d01b9e78..a62bf0a65c32 100644
--- a/drivers/net/wireless/rtlwifi/regd.c
+++ b/drivers/net/wireless/rtlwifi/regd.c
@@ -40,6 +40,7 @@ static struct country_code_to_enum_rd allCountries[] = {
 	{COUNTRY_CODE_GLOBAL_DOMAIN, "JP"},
 	{COUNTRY_CODE_WORLD_WIDE_13, "EC"},
 	{COUNTRY_CODE_TELEC_NETGEAR, "EC"},
+	{COUNTRY_CODE_WORLD_WIDE_13_5G_ALL, "US"},
 };
 
 /*
@@ -124,6 +125,17 @@ static const struct ieee80211_regdomain rtl_regdom_14_60_64 = {
 		      }
 };
 
+static const struct ieee80211_regdomain rtl_regdom_12_13_5g_all = {
+	.n_reg_rules = 4,
+	.alpha2 = "99",
+	.reg_rules = {
+			RTL819x_2GHZ_CH01_11,
+			RTL819x_2GHZ_CH12_13,
+			RTL819x_5GHZ_5150_5350,
+			RTL819x_5GHZ_5470_5850,
+		}
+};
+
 static const struct ieee80211_regdomain rtl_regdom_14 = {
 	.n_reg_rules = 3,
 	.alpha2 = "99",
@@ -348,6 +360,8 @@ static const struct ieee80211_regdomain *_rtl_regdomain_select(
 		return &rtl_regdom_14_60_64;
 	case COUNTRY_CODE_GLOBAL_DOMAIN:
 		return &rtl_regdom_14;
+	case COUNTRY_CODE_WORLD_WIDE_13_5G_ALL:
+		return &rtl_regdom_12_13_5g_all;
 	default:
 		return &rtl_regdom_no_midband;
 	}
@@ -384,6 +398,25 @@ static struct country_code_to_enum_rd *_rtl_regd_find_country(u16 countrycode)
 	return NULL;
 }
 
+static u8 channel_plan_to_country_code(u8 channelplan)
+{
+	switch (channelplan) {
+	case 0x20:
+	case 0x21:
+		return COUNTRY_CODE_WORLD_WIDE_13;
+	case 0x22:
+		return COUNTRY_CODE_IC;
+	case 0x32:
+		return COUNTRY_CODE_TELEC_NETGEAR;
+	case 0x41:
+		return COUNTRY_CODE_GLOBAL_DOMAIN;
+	case 0x7f:
+		return COUNTRY_CODE_WORLD_WIDE_13_5G_ALL;
+	default:
+		return COUNTRY_CODE_MAX; /*Error*/
+	}
+}
+
 int rtl_regd_init(struct ieee80211_hw *hw,
 		  void (*reg_notifier)(struct wiphy *wiphy,
 				       struct regulatory_request *request))
@@ -396,11 +429,12 @@ int rtl_regd_init(struct ieee80211_hw *hw,
 		return -EINVAL;
 
 	/* init country_code from efuse channel plan */
-	rtlpriv->regd.country_code = rtlpriv->efuse.channel_plan;
+	rtlpriv->regd.country_code =
+		channel_plan_to_country_code(rtlpriv->efuse.channel_plan);
 
-	RT_TRACE(rtlpriv, COMP_REGD, DBG_TRACE,
-		 "rtl: EEPROM regdomain: 0x%0x\n",
-		  rtlpriv->regd.country_code);
+	RT_TRACE(rtlpriv, COMP_REGD, DBG_DMESG,
+		 "rtl: EEPROM regdomain: 0x%0x conuntry code: %d\n",
+		 rtlpriv->efuse.channel_plan, rtlpriv->regd.country_code);
 
 	if (rtlpriv->regd.country_code >= COUNTRY_CODE_MAX) {
 		RT_TRACE(rtlpriv, COMP_REGD, DBG_DMESG,
diff --git a/drivers/net/wireless/rtlwifi/regd.h b/drivers/net/wireless/rtlwifi/regd.h
index 3bbbaaa68530..f7f15bce35dd 100644
--- a/drivers/net/wireless/rtlwifi/regd.h
+++ b/drivers/net/wireless/rtlwifi/regd.h
@@ -49,6 +49,7 @@ enum country_code_type_t {
 	COUNTRY_CODE_GLOBAL_DOMAIN = 10,
 	COUNTRY_CODE_WORLD_WIDE_13 = 11,
 	COUNTRY_CODE_TELEC_NETGEAR = 12,
+	COUNTRY_CODE_WORLD_WIDE_13_5G_ALL = 13,
 
 	/*add new channel plan above this line */
 	COUNTRY_CODE_MAX
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
index d930c1f78721..ce4da9d79fbd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
@@ -1123,23 +1123,22 @@ static void rtl88e_dm_init_txpower_tracking(struct ieee80211_hw *hw)
 void rtl88e_dm_check_txpower_tracking(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger;
 
 	if (!rtlpriv->dm.txpower_tracking)
 		return;
 
-	if (!tm_trigger) {
+	if (!rtlpriv->dm.tm_trigger) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER, BIT(17)|BIT(16),
 			      0x03);
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Trigger 88E Thermal Meter!!\n");
-		tm_trigger = 1;
+		rtlpriv->dm.tm_trigger = 1;
 		return;
 	} else {
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Schedule TxPowerTracking !!\n");
 				dm_txpower_track_cb_therm(hw);
-		tm_trigger = 0;
+		rtlpriv->dm.tm_trigger = 0;
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
index 86ce5b1930e6..8ee83b093c0d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
@@ -1354,27 +1354,11 @@ void rtl88ee_set_qos(struct ieee80211_hw *hw, int aci)
 	}
 }
 
-static void rtl88ee_clear_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 tmp;
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISR);
-	rtl_write_dword(rtlpriv, REG_HISR, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
-	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HSISR);
-	rtl_write_dword(rtlpriv, REG_HSISR, tmp);
-}
-
 void rtl88ee_enable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
-	rtl88ee_clear_interrupt(hw);/*clear it here first*/
 	rtl_write_dword(rtlpriv, REG_HIMR,
 			rtlpci->irq_mask[0] & 0xFFFFFFFF);
 	rtl_write_dword(rtlpriv, REG_HIMRE,
@@ -1919,8 +1903,8 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 		 "dev_addr: %pM\n", rtlefuse->dev_addr);
 	/*channel plan */
 	rtlefuse->eeprom_channelplan = hwinfo[EEPROM_CHANNELPLAN];
-	/* set channel paln to world wide 13 */
-	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
+	/* set channel plan from efuse */
+	rtlefuse->channel_plan = rtlefuse->eeprom_channelplan;
 	/*tx power*/
 	_rtl88ee_read_txpower_info_from_hwpg(hw,
 					     rtlefuse->autoload_failflag,
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c
index ef28c8ea1e84..02013df968a0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c
@@ -23,7 +23,7 @@
  *
  *****************************************************************************/
 
-#include "pwrseqcmd.h"
+#include "../pwrseqcmd.h"
 #include "pwrseq.h"
 
 /* drivers should parse below arrays and do the corresponding actions */
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
index 79103347d967..f2d9c6116e5c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
@@ -26,7 +26,7 @@
 #ifndef __RTL8723E_PWRSEQ_H__
 #define __RTL8723E_PWRSEQ_H__
 
-#include "pwrseqcmd.h"
+#include "../pwrseqcmd.h"
 /* Check document WM-20110607-Paul-RTL8188EE_Power_Architecture-R02.vsd
  *	There are 6 HW Power States:
  *	0: POFF--Power Off
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
index f5ee67cda73a..0aca6f47487c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
@@ -1169,23 +1169,22 @@ static void rtl92c_dm_check_txpower_tracking_thermal_meter(
 						struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger;
 
 	if (!rtlpriv->dm.txpower_tracking)
 		return;
 
-	if (!tm_trigger) {
+	if (!rtlpriv->dm.tm_trigger) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER, RFREG_OFFSET_MASK,
 			      0x60);
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Trigger 92S Thermal Meter!!\n");
-		tm_trigger = 1;
+		rtlpriv->dm.tm_trigger = 1;
 		return;
 	} else {
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Schedule TxPowerTracking direct call!!\n");
 		rtl92c_dm_txpower_tracking_directcall(hw);
-		tm_trigger = 0;
+		rtlpriv->dm.tm_trigger = 0;
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
index 29983bc96a89..14b819ea8b71 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
@@ -233,13 +233,14 @@ int rtl92c_download_fw(struct ieee80211_hw *hw)
 	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
 	pfwdata = (u8 *)rtlhal->pfirmware;
 	fwsize = rtlhal->fwsize;
-
 	if (IS_FW_HEADER_EXIST(pfwheader)) {
 		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
 			 "Firmware Version(%d), Signature(%#x),Size(%d)\n",
 			  pfwheader->version, pfwheader->signature,
 			  (int)sizeof(struct rtl92c_firmware_header));
 
+		rtlhal->fw_version = pfwheader->version;
+		rtlhal->fw_subversion = pfwheader->subversion;
 		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
 		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
 	}
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index d310d55d800e..767358a553fb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -840,6 +840,26 @@ static void _rtl92cu_init_wmac_setting(struct ieee80211_hw *hw)
 	rtl92c_set_data_filter(hw, value16);
 }
 
+static void _rtl92cu_init_beacon_parameters(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	rtl_write_word(rtlpriv, REG_BCN_CTRL, 0x1010);
+
+	/* TODO: Remove these magic number */
+	rtl_write_word(rtlpriv, REG_TBTT_PROHIBIT, 0x6404);
+	rtl_write_byte(rtlpriv, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME);
+	rtl_write_byte(rtlpriv, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
+	/* Change beacon AIFS to the largest number
+	 * beacause test chip does not contension before sending beacon.
+	 */
+	if (IS_NORMAL_CHIP(rtlhal->version))
+		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660F);
+	else
+		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x66FF);
+}
+
 static int _rtl92cu_init_mac(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -887,9 +907,9 @@ static int _rtl92cu_init_mac(struct ieee80211_hw *hw)
 	_rtl92cu_init_usb_aggregation(hw);
 	rtlpriv->cfg->ops->set_bw_mode(hw, NL80211_CHAN_HT20);
 	rtl92c_set_min_space(hw, IS_92C_SERIAL(rtlhal->version));
-	rtl92c_init_beacon_parameters(hw, rtlhal->version);
+	_rtl92cu_init_beacon_parameters(hw);
 	rtl92c_init_ampdu_aggregation(hw);
-	rtl92c_init_beacon_max_error(hw, true);
+	rtl92c_init_beacon_max_error(hw);
 	return err;
 }
 
@@ -987,7 +1007,6 @@ int rtl92cu_hw_init(struct ieee80211_hw *hw)
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	int err = 0;
-	static bool iqk_initialized;
 	unsigned long flags;
 
 	/* As this function can take a very long time (up to 350 ms)
@@ -1038,11 +1057,11 @@ int rtl92cu_hw_init(struct ieee80211_hw *hw)
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
 	if (ppsc->rfpwr_state == ERFON) {
 		rtl92c_phy_set_rfpath_switch(hw, 1);
-		if (iqk_initialized) {
+		if (rtlphy->iqk_initialized) {
 			rtl92c_phy_iq_calibrate(hw, true);
 		} else {
 			rtl92c_phy_iq_calibrate(hw, false);
-			iqk_initialized = true;
+			rtlphy->iqk_initialized = true;
 		}
 		rtl92c_dm_check_txpower_tracking(hw);
 		rtl92c_phy_lc_calibrate(hw);
@@ -1323,7 +1342,6 @@ static int _rtl92cu_set_media_status(struct ieee80211_hw *hw,
 	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
 
 	bt_msr &= 0xfc;
-	rtl_write_byte(rtlpriv, REG_BCN_MAX_ERR, 0xFF);
 	if (type == NL80211_IFTYPE_UNSPECIFIED || type ==
 	    NL80211_IFTYPE_STATION) {
 		_rtl92cu_stop_tx_beacon(hw);
@@ -1392,6 +1410,9 @@ void rtl92cu_card_disable(struct ieee80211_hw *hw)
 		_CardDisableHWSM(hw);
 	else
 		_CardDisableWithoutHWSM(hw);
+
+	/* after power off we should do iqk again */
+	rtlpriv->phy.iqk_initialized = false;
 }
 
 void rtl92cu_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
@@ -1452,25 +1473,6 @@ int rtl92cu_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
 	return 0;
 }
 
-static void _InitBeaconParameters(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-
-	rtl_write_word(rtlpriv, REG_BCN_CTRL, 0x1010);
-
-	/* TODO: Remove these magic number */
-	rtl_write_word(rtlpriv, REG_TBTT_PROHIBIT, 0x6404);
-	rtl_write_byte(rtlpriv, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME);
-	rtl_write_byte(rtlpriv, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
-	/* Change beacon AIFS to the largest number
-	 * beacause test chip does not contension before sending beacon. */
-	if (IS_NORMAL_CHIP(rtlhal->version))
-		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660F);
-	else
-		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x66FF);
-}
-
 static void _beacon_function_enable(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1491,7 +1493,7 @@ void rtl92cu_set_beacon_related_registers(struct ieee80211_hw *hw)
 	atim_window = 2;	/*FIX MERGE */
 	rtl_write_word(rtlpriv, REG_ATIMWND, atim_window);
 	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
-	_InitBeaconParameters(hw);
+	_rtl92cu_init_beacon_parameters(hw);
 	rtl_write_byte(rtlpriv, REG_SLOT, 0x09);
 	/*
 	 * Force beacon frame transmission even after receiving beacon frame
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
index adb810794eef..490a7cf7c702 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
@@ -613,7 +613,7 @@ void rtl92c_init_ampdu_aggregation(struct ieee80211_hw *hw)
 	rtl_write_word(rtlpriv, 0x4CA, 0x0708);
 }
 
-void rtl92c_init_beacon_max_error(struct ieee80211_hw *hw, bool infra_mode)
+void rtl92c_init_beacon_max_error(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -641,21 +641,6 @@ void rtl92c_init_retry_function(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_ACKTO, 0x40);
 }
 
-void rtl92c_init_beacon_parameters(struct ieee80211_hw *hw,
-				   enum version_8192c version)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-
-	rtl_write_word(rtlpriv, REG_TBTT_PROHIBIT, 0x6404);/* ms */
-	rtl_write_byte(rtlpriv, REG_DRVERLYINT, DRIVER_EARLY_INT_TIME);/*ms*/
-	rtl_write_byte(rtlpriv, REG_BCNDMATIM, BCN_DMA_ATIME_INT_TIME);
-	if (IS_NORMAL_CHIP(rtlhal->version))
-		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660F);
-	else
-		rtl_write_word(rtlpriv, REG_BCNTCFG, 0x66FF);
-}
-
 void rtl92c_disable_fast_edca(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.h b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.h
index bf53652e4edd..e34f0f14ccd7 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.h
@@ -66,13 +66,10 @@ void rtl92c_init_edca_param(struct ieee80211_hw *hw,
 
 void rtl92c_init_edca(struct ieee80211_hw *hw);
 void rtl92c_init_ampdu_aggregation(struct ieee80211_hw *hw);
-void rtl92c_init_beacon_max_error(struct ieee80211_hw *hw, bool infra_mode);
+void rtl92c_init_beacon_max_error(struct ieee80211_hw *hw);
 void rtl92c_init_rdg_setting(struct ieee80211_hw *hw);
 void rtl92c_init_retry_function(struct ieee80211_hw *hw);
 
-void rtl92c_init_beacon_parameters(struct ieee80211_hw *hw,
-				   enum version_8192c version);
-
 void rtl92c_disable_fast_edca(struct ieee80211_hw *hw);
 void rtl92c_set_min_space(struct ieee80211_hw *hw, bool is2T);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
index 12f6d474b492..c972fa50926d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
@@ -108,13 +108,8 @@ void rtl92cu_phy_set_rf_reg(struct ieee80211_hw *hw,
 bool rtl92cu_phy_mac_config(struct ieee80211_hw *hw)
 {
 	bool rtstatus;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool is92c = IS_92C_SERIAL(rtlhal->version);
 
 	rtstatus = _rtl92cu_phy_config_mac_with_headerfile(hw);
-	if (is92c && IS_HARDWARE_TYPE_8192CE(rtlhal))
-		rtl_write_byte(rtlpriv, 0x14, 0x71);
 	return rtstatus;
 }
 
@@ -122,7 +117,6 @@ bool rtl92cu_phy_bb_config(struct ieee80211_hw *hw)
 {
 	bool rtstatus = true;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u16 regval;
 	u32 regval32;
 	u8 b_reg_hwparafile = 1;
@@ -134,17 +128,11 @@ bool rtl92cu_phy_bb_config(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_AFE_PLL_CTRL, 0x83);
 	rtl_write_byte(rtlpriv, REG_AFE_PLL_CTRL + 1, 0xdb);
 	rtl_write_byte(rtlpriv, REG_RF_CTRL, RF_EN | RF_RSTB | RF_SDMRSTB);
-	if (IS_HARDWARE_TYPE_8192CE(rtlhal)) {
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, FEN_PPLL | FEN_PCIEA |
-			       FEN_DIO_PCIE |	FEN_BB_GLB_RSTn | FEN_BBRSTB);
-	} else if (IS_HARDWARE_TYPE_8192CU(rtlhal)) {
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, FEN_USBA | FEN_USBD |
-			       FEN_BB_GLB_RSTn | FEN_BBRSTB);
-	}
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, FEN_USBA | FEN_USBD |
+		       FEN_BB_GLB_RSTn | FEN_BBRSTB);
 	regval32 = rtl_read_dword(rtlpriv, 0x87c);
 	rtl_write_dword(rtlpriv, 0x87c, regval32 & (~BIT(31)));
-	if (IS_HARDWARE_TYPE_8192CU(rtlhal))
-		rtl_write_byte(rtlpriv, REG_LDOHCI12_CTRL, 0x0f);
+	rtl_write_byte(rtlpriv, REG_LDOHCI12_CTRL, 0x0f);
 	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
 	if (b_reg_hwparafile == 1)
 		rtstatus = _rtl92c_phy_bb8192c_config_parafile(hw);
@@ -162,7 +150,7 @@ bool _rtl92cu_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read Rtl819XMACPHY_Array\n");
 	arraylength =  rtlphy->hwparam_tables[MAC_REG].length ;
 	ptrarray = rtlphy->hwparam_tables[MAC_REG].pdata;
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Img:RTL8192CEMAC_2T_ARRAY\n");
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Img:RTL8192CUMAC_2T_ARRAY\n");
 	for (i = 0; i < arraylength; i = i + 2)
 		rtl_write_byte(rtlpriv, ptrarray[i], (u8) ptrarray[i + 1]);
 	return true;
@@ -259,18 +247,18 @@ bool rtl92cu_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 		radiob_arraylen = rtlphy->hwparam_tables[RADIOB_2T].length;
 		radiob_array_table = rtlphy->hwparam_tables[RADIOB_2T].pdata;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Radio_A:RTL8192CERADIOA_2TARRAY\n");
+			 "Radio_A:RTL8192CURADIOA_2TARRAY\n");
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Radio_B:RTL8192CE_RADIOB_2TARRAY\n");
+			 "Radio_B:RTL8192CU_RADIOB_2TARRAY\n");
 	} else {
 		radioa_arraylen = rtlphy->hwparam_tables[RADIOA_1T].length;
 		radioa_array_table = rtlphy->hwparam_tables[RADIOA_1T].pdata;
 		radiob_arraylen = rtlphy->hwparam_tables[RADIOB_1T].length;
 		radiob_array_table = rtlphy->hwparam_tables[RADIOB_1T].pdata;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Radio_A:RTL8192CE_RADIOA_1TARRAY\n");
+			 "Radio_A:RTL8192CU_RADIOA_1TARRAY\n");
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Radio_B:RTL8192CE_RADIOB_1TARRAY\n");
+			 "Radio_B:RTL8192CU_RADIOB_1TARRAY\n");
 	}
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Radio No %x\n", rfpath);
 	switch (rfpath) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/rf.c b/drivers/net/wireless/rtlwifi/rtl8192cu/rf.c
index b878d56d2f4d..5624ade92cc0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/rf.c
@@ -66,7 +66,6 @@ void rtl92cu_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u32 tx_agc[2] = { 0, 0 }, tmpval = 0;
@@ -74,14 +73,8 @@ void rtl92cu_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 	u8 idx1, idx2;
 	u8 *ptr;
 
-	if (rtlhal->interface == INTF_PCI) {
-		if (rtlefuse->eeprom_regulatory != 0)
-			turbo_scanoff = true;
-	} else {
-		if ((rtlefuse->eeprom_regulatory != 0) ||
-		    (rtlefuse->external_pa))
-			turbo_scanoff = true;
-	}
+	if ((rtlefuse->eeprom_regulatory != 0) || (rtlefuse->external_pa))
+		turbo_scanoff = true;
 	if (mac->act_scanning) {
 		tx_agc[RF90_PATH_A] = 0x3f3f3f3f;
 		tx_agc[RF90_PATH_B] = 0x3f3f3f3f;
@@ -90,11 +83,8 @@ void rtl92cu_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 			    (ppowerlevel[idx1] << 8) |
 			    (ppowerlevel[idx1] << 16) |
 			    (ppowerlevel[idx1] << 24);
-			if (rtlhal->interface == INTF_USB) {
-				if (tx_agc[idx1] > 0x20 &&
-				    rtlefuse->external_pa)
-					tx_agc[idx1] = 0x20;
-			}
+			if (tx_agc[idx1] > 0x20 && rtlefuse->external_pa)
+				tx_agc[idx1] = 0x20;
 		}
 	} else {
 		if (rtlpriv->dm.dynamic_txhighpower_lvl ==
@@ -452,9 +442,6 @@ static bool _rtl92c_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 		udelay(1);
 		switch (rfpath) {
 		case RF90_PATH_A:
-			rtstatus = rtl92cu_phy_config_rf_with_headerfile(hw,
-					(enum radio_path) rfpath);
-			break;
 		case RF90_PATH_B:
 			rtstatus = rtl92cu_phy_config_rf_with_headerfile(hw,
 					(enum radio_path) rfpath);
@@ -483,7 +470,6 @@ static bool _rtl92c_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 		}
 	}
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "<---\n");
-	return rtstatus;
 phy_rf_cfg_fail:
 	return rtstatus;
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
index cbead007171f..95880fe4106e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
@@ -321,7 +321,7 @@ bool rtl92cu_rx_query_desc(struct ieee80211_hw *hw,
 	stats->rate = (u8) GET_RX_DESC_RX_MCS(pdesc);
 	stats->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
 	stats->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	stats->isampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1)
+	stats->isfirst_ampdu = (bool)((GET_RX_DESC_PAGGR(pdesc) == 1)
 				   && (GET_RX_DESC_FAGGR(pdesc) == 1));
 	stats->timestamp_low = GET_RX_DESC_TSFL(pdesc);
 	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
index a1be5a68edfb..587b8c505a76 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/dm.c
@@ -1240,23 +1240,22 @@ static void rtl92d_dm_initialize_txpower_tracking(struct ieee80211_hw *hw)
 void rtl92d_dm_check_txpower_tracking_thermal_meter(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger;
 
 	if (!rtlpriv->dm.txpower_tracking)
 		return;
 
-	if (!tm_trigger) {
+	if (!rtlpriv->dm.tm_trigger) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER, BIT(17) |
 			      BIT(16), 0x03);
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Trigger 92S Thermal Meter!!\n");
-		tm_trigger = 1;
+		rtlpriv->dm.tm_trigger = 1;
 		return;
 	} else {
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Schedule TxPowerTracking direct call!!\n");
 		rtl92d_dm_txpower_tracking_callback_thermalmeter(hw);
-		tm_trigger = 0;
+		rtlpriv->dm.tm_trigger = 0;
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/fw.c b/drivers/net/wireless/rtlwifi/rtl8192ee/fw.c
index c5d4b8013cde..232865cc3ffd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ee/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/fw.c
@@ -875,7 +875,7 @@ static void _rtl92ee_c2h_content_parsing(struct ieee80211_hw *hw, u8 c2h_cmd_id,
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-			 "[C2H], Unkown packet!! CmdId(%#X)!\n", c2h_cmd_id);
+			 "[C2H], Unknown packet!! CmdId(%#X)!\n", c2h_cmd_id);
 		break;
 	}
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c
index da0a6125f314..5f14308e8eb3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c
@@ -1584,28 +1584,11 @@ void rtl92ee_set_qos(struct ieee80211_hw *hw, int aci)
 	}
 }
 
-static void rtl92ee_clear_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 tmp;
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISR);
-	rtl_write_dword(rtlpriv, REG_HISR, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
-	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HSISR);
-	rtl_write_dword(rtlpriv, REG_HSISR, tmp);
-}
-
 void rtl92ee_enable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
-	rtl92ee_clear_interrupt(hw);/*clear it here first*/
-
 	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
 	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
 	rtlpci->irq_enabled = true;
@@ -2194,8 +2177,8 @@ static void _rtl92ee_read_adapter_info(struct ieee80211_hw *hw)
 		 "dev_addr: %pM\n", rtlefuse->dev_addr);
 	/*channel plan */
 	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
-	/* set channel paln to world wide 13 */
-	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
+	/* set channel plan from efuse */
+	rtlefuse->channel_plan = rtlefuse->eeprom_channelplan;
 	/*tx power*/
 	_rtl92ee_read_txpower_info_from_hwpg(hw, rtlefuse->autoload_failflag,
 					     hwinfo);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
index 575980b88658..9bae5a92e30f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/dm.c
@@ -200,7 +200,6 @@ static void _rtl92s_dm_check_txpowertracking_thermalmeter(
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	static u8 tm_trigger;
 	u8 tx_power_checkcnt = 5;
 
 	/* 2T2R TP issue */
@@ -215,13 +214,13 @@ static void _rtl92s_dm_check_txpowertracking_thermalmeter(
 		return;
 	}
 
-	if (!tm_trigger) {
+	if (!rtlpriv->dm.tm_trigger) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER,
 			      RFREG_OFFSET_MASK, 0x60);
-		tm_trigger = 1;
+		rtlpriv->dm.tm_trigger = 1;
 	} else {
 		_rtl92s_dm_txpowertracking_callback_thermalmeter(hw);
-		tm_trigger = 0;
+		rtlpriv->dm.tm_trigger = 0;
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
index 67bb47d77b68..a4b7eac6856f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
@@ -1258,18 +1258,6 @@ void rtl8723e_set_qos(struct ieee80211_hw *hw, int aci)
 	}
 }
 
-static void rtl8723e_clear_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 tmp;
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISR);
-	rtl_write_dword(rtlpriv, REG_HISR, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
-	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
-}
-
 void rtl8723e_enable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1284,7 +1272,6 @@ void rtl8723e_disable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	rtl8723e_clear_interrupt(hw);/*clear it here first*/
 	rtl_write_dword(rtlpriv, 0x3a8, IMR8190_DISABLED);
 	rtl_write_dword(rtlpriv, 0x3ac, IMR8190_DISABLED);
 	rtlpci->irq_enabled = false;
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
index e77c3a46c94a..3a81cdba8ca3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
@@ -909,23 +909,22 @@ static void rtl8723be_dm_txpower_tracking_callback_thermalmeter(
 void rtl8723be_dm_check_txpower_tracking(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger;
 
 	if (!rtlpriv->dm.txpower_tracking)
 		return;
 
-	if (!tm_trigger) {
+	if (!rtlpriv->dm.tm_trigger) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER, BIT(17) | BIT(16),
 			      0x03);
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Trigger 8723be Thermal Meter!!\n");
-		tm_trigger = 1;
+		rtlpriv->dm.tm_trigger = 1;
 		return;
 	} else {
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Schedule TxPowerTracking !!\n");
 		rtl8723be_dm_txpower_tracking_callback_thermalmeter(hw);
-		tm_trigger = 0;
+		rtlpriv->dm.tm_trigger = 0;
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/fw.c b/drivers/net/wireless/rtlwifi/rtl8723be/fw.c
index 69d4f0fc1af1..d5da0f3c1217 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/fw.c
@@ -613,7 +613,7 @@ static void _rtl8723be_c2h_content_parsing(struct ieee80211_hw *hw,
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-			 "[C2H], Unkown packet!! CmdId(%#X)!\n", c2h_cmd_id);
+			 "[C2H], Unknown packet!! CmdId(%#X)!\n", c2h_cmd_id);
 		break;
 	}
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/hw.c b/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
index b681af3c7a35..c983d2fe147f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
@@ -1634,28 +1634,11 @@ void rtl8723be_set_qos(struct ieee80211_hw *hw, int aci)
 	}
 }
 
-static void rtl8723be_clear_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 tmp;
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISR);
-	rtl_write_dword(rtlpriv, REG_HISR, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
-	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HSISR);
-	rtl_write_dword(rtlpriv, REG_HSISR, tmp);
-}
-
 void rtl8723be_enable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
-	rtl8723be_clear_interrupt(hw);/*clear it here first*/
-
 	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
 	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
 	rtlpci->irq_enabled = true;
@@ -2139,8 +2122,8 @@ static void _rtl8723be_read_adapter_info(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 		 "EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid);
 
-	/* set channel plan to world wide 13 */
-	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
+	/* set channel plan from efuse */
+	rtlefuse->channel_plan = rtlefuse->eeprom_channelplan;
 
 	if (rtlhal->oem_id == RT_CID_DEFAULT) {
 		/* Does this one have a Toshiba SMID from group 1? */
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
index 342678d2ed42..b57cfd965196 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
@@ -1068,20 +1068,18 @@ static void rtl8812ae_dm_check_txpower_tracking_thermalmeter(
 		struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger;
 
-	if (!tm_trigger) {
+	if (!rtlpriv->dm.tm_trigger) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER_88E,
 			      BIT(17) | BIT(16), 0x03);
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Trigger 8812 Thermal Meter!!\n");
-		tm_trigger = 1;
+		rtlpriv->dm.tm_trigger = 1;
 		return;
 	}
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 		 "Schedule TxPowerTracking direct call!!\n");
 	rtl8812ae_dm_txpower_tracking_callback_thermalmeter(hw);
-	tm_trigger = 0;
 }
 
 static void rtl8821ae_dm_iq_calibrate(struct ieee80211_hw *hw)
@@ -2519,21 +2517,19 @@ void rtl8821ae_dm_txpower_tracking_callback_thermalmeter(
 void rtl8821ae_dm_check_txpower_tracking_thermalmeter(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger;
-
-	if (!tm_trigger) {
+	if (!rtlpriv->dm.tm_trigger) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER_88E, BIT(17)|BIT(16),
 			      0x03);
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Trigger 8821ae Thermal Meter!!\n");
-		tm_trigger = 1;
+		rtlpriv->dm.tm_trigger = 1;
 		return;
 	} else {
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Schedule TxPowerTracking !!\n");
 
 		rtl8821ae_dm_txpower_tracking_callback_thermalmeter(hw);
-		tm_trigger = 0;
+		rtlpriv->dm.tm_trigger = 0;
 	}
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c b/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c
index 8704eee9f3a4..3236d44b459d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c
@@ -2253,31 +2253,11 @@ void rtl8821ae_set_qos(struct ieee80211_hw *hw, int aci)
 	}
 }
 
-static void rtl8821ae_clear_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 tmp;
-	tmp = rtl_read_dword(rtlpriv, REG_HISR);
-	/*printk("clear interrupt first:\n");
-	printk("0x%x = 0x%08x\n",REG_HISR, tmp);*/
-	rtl_write_dword(rtlpriv, REG_HISR, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
-	/*printk("0x%x = 0x%08x\n",REG_HISRE, tmp);*/
-	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
-
-	tmp = rtl_read_dword(rtlpriv, REG_HSISR);
-	/*printk("0x%x = 0x%08x\n",REG_HSISR, tmp);*/
-	rtl_write_dword(rtlpriv, REG_HSISR, tmp);
-}
-
 void rtl8821ae_enable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
-	rtl8821ae_clear_interrupt(hw);/*clear it here first*/
-
 	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
 	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
 	rtlpci->irq_enabled = true;
@@ -3232,8 +3212,8 @@ static void _rtl8821ae_read_adapter_info(struct ieee80211_hw *hw, bool b_pseudo_
 	if (rtlefuse->eeprom_channelplan == 0xff)
 		rtlefuse->eeprom_channelplan = 0x7F;
 
-	/* set channel paln to world wide 13 */
-	/* rtlefuse->channel_plan = (u8)rtlefuse->eeprom_channelplan; */
+	/* set channel plan from efuse */
+	rtlefuse->channel_plan = rtlefuse->eeprom_channelplan;
 
 	/*parse xtal*/
 	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_8821AE];
diff --git a/drivers/net/wireless/rtlwifi/wifi.h b/drivers/net/wireless/rtlwifi/wifi.h
index 51572912c53d..2b770b5e2620 100644
--- a/drivers/net/wireless/rtlwifi/wifi.h
+++ b/drivers/net/wireless/rtlwifi/wifi.h
@@ -1665,6 +1665,7 @@ struct rtl_dm {
 	u8 last_dtp_lvl;
 	u8 thermalvalue_avg[AVG_THERMAL_NUM];
 	u8 thermalvalue_avg_index;
+	u8 tm_trigger;
 	bool done_txpower;
 	u8 dynamic_txhighpower_lvl;	/*Tx high power level */
 	u8 dm_flag;		/*Indicate each dynamic mechanism's status. */
diff --git a/drivers/net/wireless/ti/wl1251/acx.c b/drivers/net/wireless/ti/wl1251/acx.c
index 5695628757ee..d6fbdda2cba3 100644
--- a/drivers/net/wireless/ti/wl1251/acx.c
+++ b/drivers/net/wireless/ti/wl1251/acx.c
@@ -53,10 +53,7 @@ int wl1251_acx_station_id(struct wl1251 *wl)
 		mac->mac[i] = wl->mac_addr[ETH_ALEN - 1 - i];
 
 	ret = wl1251_cmd_configure(wl, DOT11_STATION_ID, mac, sizeof(*mac));
-	if (ret < 0)
-		goto out;
 
-out:
 	kfree(mac);
 	return ret;
 }
diff --git a/drivers/net/wireless/ti/wl1251/main.c b/drivers/net/wireless/ti/wl1251/main.c
index 5d54d16a59e7..cd4777954f87 100644
--- a/drivers/net/wireless/ti/wl1251/main.c
+++ b/drivers/net/wireless/ti/wl1251/main.c
@@ -763,8 +763,7 @@ static u64 wl1251_op_prepare_multicast(struct ieee80211_hw *hw,
 	return (u64)(unsigned long)fp;
 }
 
-#define WL1251_SUPPORTED_FILTERS (FIF_PROMISC_IN_BSS | \
-				  FIF_ALLMULTI | \
+#define WL1251_SUPPORTED_FILTERS (FIF_ALLMULTI | \
 				  FIF_FCSFAIL | \
 				  FIF_BCN_PRBRESP_PROMISC | \
 				  FIF_CONTROL | \
@@ -795,10 +794,6 @@ static void wl1251_op_configure_filter(struct ieee80211_hw *hw,
 	wl->rx_config = WL1251_DEFAULT_RX_CONFIG;
 	wl->rx_filter = WL1251_DEFAULT_RX_FILTER;
 
-	if (*total & FIF_PROMISC_IN_BSS) {
-		wl->rx_config |= CFG_BSSID_FILTER_EN;
-		wl->rx_config |= CFG_RX_ALL_GOOD;
-	}
 	if (*total & FIF_ALLMULTI)
 		/*
 		 * CFG_MC_FILTER_EN in rx_config needs to be 0 to receive
@@ -825,7 +820,7 @@ static void wl1251_op_configure_filter(struct ieee80211_hw *hw,
 	if (ret < 0)
 		goto out;
 
-	if (*total & FIF_ALLMULTI || *total & FIF_PROMISC_IN_BSS)
+	if (*total & FIF_ALLMULTI)
 		ret = wl1251_acx_group_address_tbl(wl, false, NULL, 0);
 	else if (fp)
 		ret = wl1251_acx_group_address_tbl(wl, fp->enabled,
@@ -1481,7 +1476,8 @@ int wl1251_init_ieee80211(struct wl1251 *wl)
 	/* unit us */
 	/* FIXME: find a proper value */
 
-	wl->hw->flags = IEEE80211_HW_SIGNAL_DBM | IEEE80211_HW_SUPPORTS_PS;
+	ieee80211_hw_set(wl->hw, SIGNAL_DBM);
+	ieee80211_hw_set(wl->hw, SUPPORTS_PS);
 
 	wl->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 					 BIT(NL80211_IFTYPE_ADHOC);
diff --git a/drivers/net/wireless/ti/wl18xx/main.c b/drivers/net/wireless/ti/wl18xx/main.c
index 717c4f5a02c2..49aca2cf7605 100644
--- a/drivers/net/wireless/ti/wl18xx/main.c
+++ b/drivers/net/wireless/ti/wl18xx/main.c
@@ -24,6 +24,7 @@
 #include <linux/ip.h>
 #include <linux/firmware.h>
 #include <linux/etherdevice.h>
+#include <linux/irq.h>
 
 #include "../wlcore/wlcore.h"
 #include "../wlcore/debug.h"
@@ -578,7 +579,7 @@ static struct wl18xx_priv_conf wl18xx_default_priv_conf = {
 
 static const struct wlcore_partition_set wl18xx_ptable[PART_TABLE_LEN] = {
 	[PART_TOP_PRCM_ELP_SOC] = {
-		.mem  = { .start = 0x00A02000, .size  = 0x00010000 },
+		.mem  = { .start = 0x00A00000, .size  = 0x00012000 },
 		.reg  = { .start = 0x00807000, .size  = 0x00005000 },
 		.mem2 = { .start = 0x00800000, .size  = 0x0000B000 },
 		.mem3 = { .start = 0x00000000, .size  = 0x00000000 },
@@ -862,6 +863,7 @@ static int wl18xx_pre_upload(struct wl1271 *wl)
 {
 	u32 tmp;
 	int ret;
+	u16 irq_invert;
 
 	BUILD_BUG_ON(sizeof(struct wl18xx_mac_and_phy_params) >
 		WL18XX_PHY_INIT_MEM_SIZE);
@@ -911,6 +913,28 @@ static int wl18xx_pre_upload(struct wl1271 *wl)
 	/* re-enable FDSP clock */
 	ret = wlcore_write32(wl, WL18XX_PHY_FPGA_SPARE_1,
 			     MEM_FDSP_CLK_120_ENABLE);
+	if (ret < 0)
+		goto out;
+
+	ret = irq_get_trigger_type(wl->irq);
+	if ((ret == IRQ_TYPE_LEVEL_LOW) || (ret == IRQ_TYPE_EDGE_FALLING)) {
+		wl1271_info("using inverted interrupt logic: %d", ret);
+		ret = wlcore_set_partition(wl,
+					   &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+		if (ret < 0)
+			goto out;
+
+		ret = wl18xx_top_reg_read(wl, TOP_FN0_CCCR_REG_32, &irq_invert);
+		if (ret < 0)
+			goto out;
+
+		irq_invert |= BIT(1);
+		ret = wl18xx_top_reg_write(wl, TOP_FN0_CCCR_REG_32, irq_invert);
+		if (ret < 0)
+			goto out;
+
+		ret = wlcore_set_partition(wl, &wl->ptable[PART_PHY_INIT]);
+	}
 
 out:
 	return ret;
@@ -1351,9 +1375,10 @@ out:
 }
 
 #define WL18XX_CONF_FILE_NAME "ti-connectivity/wl18xx-conf.bin"
-static int wl18xx_conf_init(struct wl1271 *wl, struct device *dev)
+
+static int wl18xx_load_conf_file(struct device *dev, struct wlcore_conf *conf,
+				 struct wl18xx_priv_conf *priv_conf)
 {
-	struct wl18xx_priv *priv = wl->priv;
 	struct wlcore_conf_file *conf_file;
 	const struct firmware *fw;
 	int ret;
@@ -1362,14 +1387,14 @@ static int wl18xx_conf_init(struct wl1271 *wl, struct device *dev)
 	if (ret < 0) {
 		wl1271_error("could not get configuration binary %s: %d",
 			     WL18XX_CONF_FILE_NAME, ret);
-		goto out_fallback;
+		return ret;
 	}
 
 	if (fw->size != WL18XX_CONF_SIZE) {
 		wl1271_error("configuration binary file size is wrong, expected %zu got %zu",
 			     WL18XX_CONF_SIZE, fw->size);
 		ret = -EINVAL;
-		goto out;
+		goto out_release;
 	}
 
 	conf_file = (struct wlcore_conf_file *) fw->data;
@@ -1379,7 +1404,7 @@ static int wl18xx_conf_init(struct wl1271 *wl, struct device *dev)
 			     "expected 0x%0x got 0x%0x", WL18XX_CONF_MAGIC,
 			     conf_file->header.magic);
 		ret = -EINVAL;
-		goto out;
+		goto out_release;
 	}
 
 	if (conf_file->header.version != cpu_to_le32(WL18XX_CONF_VERSION)) {
@@ -1387,28 +1412,32 @@ static int wl18xx_conf_init(struct wl1271 *wl, struct device *dev)
 			     "expected 0x%08x got 0x%08x",
 			     WL18XX_CONF_VERSION, conf_file->header.version);
 		ret = -EINVAL;
-		goto out;
+		goto out_release;
 	}
 
-	memcpy(&wl->conf, &conf_file->core, sizeof(wl18xx_conf));
-	memcpy(&priv->conf, &conf_file->priv, sizeof(priv->conf));
+	memcpy(conf, &conf_file->core, sizeof(*conf));
+	memcpy(priv_conf, &conf_file->priv, sizeof(*priv_conf));
+
+out_release:
+	release_firmware(fw);
+	return ret;
+}
 
-	goto out;
+static int wl18xx_conf_init(struct wl1271 *wl, struct device *dev)
+{
+	struct wl18xx_priv *priv = wl->priv;
 
-out_fallback:
-	wl1271_warning("falling back to default config");
+	if (wl18xx_load_conf_file(dev, &wl->conf, &priv->conf) < 0) {
+		wl1271_warning("falling back to default config");
 
-	/* apply driver default configuration */
-	memcpy(&wl->conf, &wl18xx_conf, sizeof(wl18xx_conf));
-	/* apply default private configuration */
-	memcpy(&priv->conf, &wl18xx_default_priv_conf, sizeof(priv->conf));
+		/* apply driver default configuration */
+		memcpy(&wl->conf, &wl18xx_conf, sizeof(wl->conf));
+		/* apply default private configuration */
+		memcpy(&priv->conf, &wl18xx_default_priv_conf,
+		       sizeof(priv->conf));
+	}
 
-	/* For now we just fallback */
 	return 0;
-
-out:
-	release_firmware(fw);
-	return ret;
 }
 
 static int wl18xx_plt_init(struct wl1271 *wl)
diff --git a/drivers/net/wireless/ti/wl18xx/reg.h b/drivers/net/wireless/ti/wl18xx/reg.h
index a433a75f3cd7..bac2364c8e72 100644
--- a/drivers/net/wireless/ti/wl18xx/reg.h
+++ b/drivers/net/wireless/ti/wl18xx/reg.h
@@ -109,6 +109,7 @@
 
 #define WL18XX_WELP_ARM_COMMAND		(WL18XX_REGISTERS_BASE + 0x7100)
 #define WL18XX_ENABLE			(WL18XX_REGISTERS_BASE + 0x01543C)
+#define TOP_FN0_CCCR_REG_32		(WL18XX_TOP_OCP_BASE + 0x64)
 
 /* PRCM registers */
 #define PLATFORM_DETECTION		0xA0E3E0
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index 0be807951afe..337223b9f6f8 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -3175,8 +3175,7 @@ static u64 wl1271_op_prepare_multicast(struct ieee80211_hw *hw,
 	return (u64)(unsigned long)fp;
 }
 
-#define WL1271_SUPPORTED_FILTERS (FIF_PROMISC_IN_BSS | \
-				  FIF_ALLMULTI | \
+#define WL1271_SUPPORTED_FILTERS (FIF_ALLMULTI | \
 				  FIF_FCSFAIL | \
 				  FIF_BCN_PRBRESP_PROMISC | \
 				  FIF_CONTROL | \
@@ -5966,10 +5965,6 @@ static int wl12xx_get_hw_info(struct wl1271 *wl)
 {
 	int ret;
 
-	ret = wl12xx_set_power_on(wl);
-	if (ret < 0)
-		return ret;
-
 	ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &wl->chip.id);
 	if (ret < 0)
 		goto out;
@@ -5985,7 +5980,6 @@ static int wl12xx_get_hw_info(struct wl1271 *wl)
 		ret = wl->ops->get_mac(wl);
 
 out:
-	wl1271_power_off(wl);
 	return ret;
 }
 
@@ -6066,18 +6060,19 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
 	/* FIXME: find a proper value */
 	wl->hw->max_listen_interval = wl->conf.conn.max_listen_interval;
 
-	wl->hw->flags = IEEE80211_HW_SIGNAL_DBM |
-		IEEE80211_HW_SUPPORTS_PS |
-		IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
-		IEEE80211_HW_HAS_RATE_CONTROL |
-		IEEE80211_HW_CONNECTION_MONITOR |
-		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		IEEE80211_HW_SPECTRUM_MGMT |
-		IEEE80211_HW_AP_LINK_PS |
-		IEEE80211_HW_AMPDU_AGGREGATION |
-		IEEE80211_HW_TX_AMPDU_SETUP_IN_HW |
-		IEEE80211_HW_QUEUE_CONTROL |
-		IEEE80211_HW_CHANCTX_STA_CSA;
+	ieee80211_hw_set(wl->hw, SUPPORT_FAST_XMIT);
+	ieee80211_hw_set(wl->hw, CHANCTX_STA_CSA);
+	ieee80211_hw_set(wl->hw, QUEUE_CONTROL);
+	ieee80211_hw_set(wl->hw, TX_AMPDU_SETUP_IN_HW);
+	ieee80211_hw_set(wl->hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(wl->hw, AP_LINK_PS);
+	ieee80211_hw_set(wl->hw, SPECTRUM_MGMT);
+	ieee80211_hw_set(wl->hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(wl->hw, CONNECTION_MONITOR);
+	ieee80211_hw_set(wl->hw, HAS_RATE_CONTROL);
+	ieee80211_hw_set(wl->hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(wl->hw, SIGNAL_DBM);
+	ieee80211_hw_set(wl->hw, SUPPORTS_PS);
 
 	wl->hw->wiphy->cipher_suites = cipher_suites;
 	wl->hw->wiphy->n_cipher_suites = ARRAY_SIZE(cipher_suites);
@@ -6432,10 +6427,22 @@ static void wlcore_nvs_cb(const struct firmware *fw, void *context)
 	else
 		wl->irq_flags |= IRQF_ONESHOT;
 
+	ret = wl12xx_set_power_on(wl);
+	if (ret < 0)
+		goto out_free_nvs;
+
+	ret = wl12xx_get_hw_info(wl);
+	if (ret < 0) {
+		wl1271_error("couldn't get hw info");
+		wl1271_power_off(wl);
+		goto out_free_nvs;
+	}
+
 	ret = request_threaded_irq(wl->irq, hardirq_fn, wlcore_irq,
 				   wl->irq_flags, pdev->name, wl);
 	if (ret < 0) {
-		wl1271_error("request_irq() failed: %d", ret);
+		wl1271_error("interrupt configuration failed");
+		wl1271_power_off(wl);
 		goto out_free_nvs;
 	}
 
@@ -6449,12 +6456,7 @@ static void wlcore_nvs_cb(const struct firmware *fw, void *context)
 	}
 #endif
 	disable_irq(wl->irq);
-
-	ret = wl12xx_get_hw_info(wl);
-	if (ret < 0) {
-		wl1271_error("couldn't get hw info");
-		goto out_irq;
-	}
+	wl1271_power_off(wl);
 
 	ret = wl->ops->identify_chip(wl);
 	if (ret < 0)
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zd1211rw/zd_mac.c
index e7af261e9198..e539d9b1b562 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zd1211rw/zd_mac.c
@@ -1230,7 +1230,7 @@ static u64 zd_op_prepare_multicast(struct ieee80211_hw *hw,
 }
 
 #define SUPPORTED_FIF_FLAGS \
-	(FIF_PROMISC_IN_BSS | FIF_ALLMULTI | FIF_FCSFAIL | FIF_CONTROL | \
+	(FIF_ALLMULTI | FIF_FCSFAIL | FIF_CONTROL | \
 	FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC)
 static void zd_op_configure_filter(struct ieee80211_hw *hw,
 			unsigned int changed_flags,
@@ -1256,7 +1256,7 @@ static void zd_op_configure_filter(struct ieee80211_hw *hw,
 	 * we will have some issue with IPv6 which uses multicast for link
 	 * layer address resolution.
 	 */
-	if (*new_flags & (FIF_PROMISC_IN_BSS | FIF_ALLMULTI))
+	if (*new_flags & FIF_ALLMULTI)
 		zd_mc_add_all(&hash);
 
 	spin_lock_irqsave(&mac->lock, flags);
@@ -1397,10 +1397,10 @@ struct ieee80211_hw *zd_mac_alloc_hw(struct usb_interface *intf)
 
 	hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &mac->band;
 
-	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		    IEEE80211_HW_SIGNAL_UNSPEC |
-		    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-		    IEEE80211_HW_MFP_CAPABLE;
+	ieee80211_hw_set(hw, MFP_CAPABLE);
+	ieee80211_hw_set(hw, HOST_BROADCAST_PS_BUFFERING);
+	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(hw, SIGNAL_UNSPEC);
 
 	hw->wiphy->interface_modes =
 		BIT(NL80211_IFTYPE_MESH_POINT) |
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 4de46aa61d95..880d0d63e872 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -44,15 +44,15 @@
 #include <xen/xen.h>
 #include <xen/events.h>
 #include <xen/interface/memory.h>
+#include <xen/page.h>
 
 #include <asm/xen/hypercall.h>
-#include <asm/xen/page.h>
 
 /* Provide an option to disable split event channels at load time as
  * event channels are limited resource. Split event channels are
  * enabled by default.
  */
-bool separate_tx_rx_irq = 1;
+bool separate_tx_rx_irq = true;
 module_param(separate_tx_rx_irq, bool, 0644);
 
 /* The time that packets can stay on the guest Rx internal queue
@@ -515,14 +515,9 @@ static void xenvif_rx_action(struct xenvif_queue *queue)
 
 	while (xenvif_rx_ring_slots_available(queue, XEN_NETBK_RX_SLOTS_MAX)
 	       && (skb = xenvif_rx_dequeue(queue)) != NULL) {
-		RING_IDX old_req_cons;
-		RING_IDX ring_slots_used;
-
 		queue->last_rx_time = jiffies;
 
-		old_req_cons = queue->rx.req_cons;
 		XENVIF_RX_CB(skb)->meta_slots_used = xenvif_gop_skb(skb, &npo, queue);
-		ring_slots_used = queue->rx.req_cons - old_req_cons;
 
 		__skb_queue_tail(&rxq, skb);
 	}
@@ -753,7 +748,7 @@ static int xenvif_count_requests(struct xenvif_queue *queue,
 		slots++;
 
 		if (unlikely((txp->offset + txp->size) > PAGE_SIZE)) {
-			netdev_err(queue->vif->dev, "Cross page boundary, txp->offset: %x, size: %u\n",
+			netdev_err(queue->vif->dev, "Cross page boundary, txp->offset: %u, size: %u\n",
 				 txp->offset, txp->size);
 			xenvif_fatal_tx_err(queue->vif);
 			return -EINVAL;
@@ -879,7 +874,7 @@ static inline void xenvif_grant_handle_set(struct xenvif_queue *queue,
 	if (unlikely(queue->grant_tx_handle[pending_idx] !=
 		     NETBACK_INVALID_HANDLE)) {
 		netdev_err(queue->vif->dev,
-			   "Trying to overwrite active handle! pending_idx: %x\n",
+			   "Trying to overwrite active handle! pending_idx: 0x%x\n",
 			   pending_idx);
 		BUG();
 	}
@@ -892,7 +887,7 @@ static inline void xenvif_grant_handle_reset(struct xenvif_queue *queue,
 	if (unlikely(queue->grant_tx_handle[pending_idx] ==
 		     NETBACK_INVALID_HANDLE)) {
 		netdev_err(queue->vif->dev,
-			   "Trying to unmap invalid handle! pending_idx: %x\n",
+			   "Trying to unmap invalid handle! pending_idx: 0x%x\n",
 			   pending_idx);
 		BUG();
 	}
@@ -1248,9 +1243,9 @@ static void xenvif_tx_build_gops(struct xenvif_queue *queue,
 		/* No crossing a page as the payload mustn't fragment. */
 		if (unlikely((txreq.offset + txreq.size) > PAGE_SIZE)) {
 			netdev_err(queue->vif->dev,
-				   "txreq.offset: %x, size: %u, end: %lu\n",
+				   "txreq.offset: %u, size: %u, end: %lu\n",
 				   txreq.offset, txreq.size,
-				   (txreq.offset&~PAGE_MASK) + txreq.size);
+				   (unsigned long)(txreq.offset&~PAGE_MASK) + txreq.size);
 			xenvif_fatal_tx_err(queue->vif);
 			break;
 		}
@@ -1598,12 +1593,12 @@ static inline void xenvif_tx_dealloc_action(struct xenvif_queue *queue)
 					queue->pages_to_unmap,
 					gop - queue->tx_unmap_ops);
 		if (ret) {
-			netdev_err(queue->vif->dev, "Unmap fail: nr_ops %tx ret %d\n",
+			netdev_err(queue->vif->dev, "Unmap fail: nr_ops %tu ret %d\n",
 				   gop - queue->tx_unmap_ops, ret);
 			for (i = 0; i < gop - queue->tx_unmap_ops; ++i) {
 				if (gop[i].status != GNTST_okay)
 					netdev_err(queue->vif->dev,
-						   " host_addr: %llx handle: %x status: %d\n",
+						   " host_addr: 0x%llx handle: 0x%x status: %d\n",
 						   gop[i].host_addr,
 						   gop[i].handle,
 						   gop[i].status);
@@ -1736,7 +1731,7 @@ void xenvif_idx_unmap(struct xenvif_queue *queue, u16 pending_idx)
 				&queue->mmap_pages[pending_idx], 1);
 	if (ret) {
 		netdev_err(queue->vif->dev,
-			   "Unmap fail: ret: %d pending_idx: %d host_addr: %llx handle: %x status: %d\n",
+			   "Unmap fail: ret: %d pending_idx: %d host_addr: %llx handle: 0x%x status: %d\n",
 			   ret,
 			   pending_idx,
 			   tx_unmap_op.host_addr,
diff --git a/drivers/net/xen-netback/xenbus.c b/drivers/net/xen-netback/xenbus.c
index fee02414529e..ec383b0f5443 100644
--- a/drivers/net/xen-netback/xenbus.c
+++ b/drivers/net/xen-netback/xenbus.c
@@ -34,6 +34,8 @@ struct backend_info {
 	enum xenbus_state frontend_state;
 	struct xenbus_watch hotplug_status_watch;
 	u8 have_hotplug_status_watch:1;
+
+	const char *hotplug_script;
 };
 
 static int connect_rings(struct backend_info *be, struct xenvif_queue *queue);
@@ -238,6 +240,7 @@ static int netback_remove(struct xenbus_device *dev)
 		xenvif_free(be->vif);
 		be->vif = NULL;
 	}
+	kfree(be->hotplug_script);
 	kfree(be);
 	dev_set_drvdata(&dev->dev, NULL);
 	return 0;
@@ -255,6 +258,7 @@ static int netback_probe(struct xenbus_device *dev,
 	struct xenbus_transaction xbt;
 	int err;
 	int sg;
+	const char *script;
 	struct backend_info *be = kzalloc(sizeof(struct backend_info),
 					  GFP_KERNEL);
 	if (!be) {
@@ -347,6 +351,15 @@ static int netback_probe(struct xenbus_device *dev,
 	if (err)
 		pr_debug("Error writing multi-queue-max-queues\n");
 
+	script = xenbus_read(XBT_NIL, dev->nodename, "script", NULL);
+	if (IS_ERR(script)) {
+		err = PTR_ERR(script);
+		xenbus_dev_fatal(dev, err, "reading script");
+		goto fail;
+	}
+
+	be->hotplug_script = script;
+
 	err = xenbus_switch_state(dev, XenbusStateInitWait);
 	if (err)
 		goto fail;
@@ -379,22 +392,14 @@ static int netback_uevent(struct xenbus_device *xdev,
 			  struct kobj_uevent_env *env)
 {
 	struct backend_info *be = dev_get_drvdata(&xdev->dev);
-	char *val;
 
-	val = xenbus_read(XBT_NIL, xdev->nodename, "script", NULL);
-	if (IS_ERR(val)) {
-		int err = PTR_ERR(val);
-		xenbus_dev_fatal(xdev, err, "reading script");
-		return err;
-	} else {
-		if (add_uevent_var(env, "script=%s", val)) {
-			kfree(val);
-			return -ENOMEM;
-		}
-		kfree(val);
-	}
+	if (!be)
+		return 0;
 
-	if (!be || !be->vif)
+	if (add_uevent_var(env, "script=%s", be->hotplug_script))
+		return -ENOMEM;
+
+	if (!be->vif)
 		return 0;
 
 	return add_uevent_var(env, "vif=%s", be->vif->dev->name);
@@ -676,6 +681,9 @@ static int xen_register_watchers(struct xenbus_device *dev, struct xenvif *vif)
 	char *node;
 	unsigned maxlen = strlen(dev->nodename) + sizeof("/rate");
 
+	if (vif->credit_watch.node)
+		return -EADDRINUSE;
+
 	node = kmalloc(maxlen, GFP_KERNEL);
 	if (!node)
 		return -ENOMEM;
@@ -765,6 +773,7 @@ static void connect(struct backend_info *be)
 	}
 
 	xen_net_read_rate(dev, &credit_bytes, &credit_usec);
+	xen_unregister_watchers(be->vif);
 	xen_register_watchers(dev, be->vif);
 	read_xenbus_vif_flags(be);
 
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index e031c943286e..f948c46d5132 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -45,7 +45,6 @@
 #include <linux/slab.h>
 #include <net/ip.h>
 
-#include <asm/xen/page.h>
 #include <xen/xen.h>
 #include <xen/xenbus.h>
 #include <xen/events.h>
@@ -733,7 +732,7 @@ static int xennet_get_responses(struct netfront_queue *queue,
 		if (unlikely(rx->status < 0 ||
 			     rx->offset + rx->status > PAGE_SIZE)) {
 			if (net_ratelimit())
-				dev_warn(dev, "rx->offset: %x, size: %u\n",
+				dev_warn(dev, "rx->offset: %u, size: %d\n",
 					 rx->offset, rx->status);
 			xennet_move_rx_slot(queue, skb, ref);
 			err = -EINVAL;
@@ -1245,10 +1244,6 @@ static struct net_device *xennet_create_dev(struct xenbus_device *dev)
 	np                   = netdev_priv(netdev);
 	np->xbdev            = dev;
 
-	/* No need to use rtnl_lock() before the call below as it
-	 * happens before register_netdev().
-	 */
-	netif_set_real_num_tx_queues(netdev, 0);
 	np->queues = NULL;
 
 	err = -ENOMEM;
@@ -1560,9 +1555,8 @@ static int xennet_init_queue(struct netfront_queue *queue)
 	spin_lock_init(&queue->tx_lock);
 	spin_lock_init(&queue->rx_lock);
 
-	init_timer(&queue->rx_refill_timer);
-	queue->rx_refill_timer.data = (unsigned long)queue;
-	queue->rx_refill_timer.function = rx_refill_timeout;
+	setup_timer(&queue->rx_refill_timer, rx_refill_timeout,
+		    (unsigned long)queue);
 
 	snprintf(queue->name, sizeof(queue->name), "%s-q%u",
 		 queue->info->netdev->name, queue->id);
@@ -1901,9 +1895,6 @@ abort_transaction_no_dev_fatal:
 	xennet_disconnect_backend(info);
 	kfree(info->queues);
 	info->queues = NULL;
-	rtnl_lock();
-	netif_set_real_num_tx_queues(info->netdev, 0);
-	rtnl_unlock();
  out:
 	return err;
 }
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index 107714e4405f..722673cb785b 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -72,6 +72,6 @@ source "drivers/nfc/pn544/Kconfig"
 source "drivers/nfc/microread/Kconfig"
 source "drivers/nfc/nfcmrvl/Kconfig"
 source "drivers/nfc/st21nfca/Kconfig"
-source "drivers/nfc/st21nfcb/Kconfig"
+source "drivers/nfc/st-nci/Kconfig"
 source "drivers/nfc/nxp-nci/Kconfig"
 endmenu
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index a4292d790f9b..368b6dfe71b3 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -12,7 +12,5 @@ obj-$(CONFIG_NFC_PORT100)	+= port100.o
 obj-$(CONFIG_NFC_MRVL)		+= nfcmrvl/
 obj-$(CONFIG_NFC_TRF7970A)	+= trf7970a.o
 obj-$(CONFIG_NFC_ST21NFCA)  	+= st21nfca/
-obj-$(CONFIG_NFC_ST21NFCB)	+= st21nfcb/
+obj-$(CONFIG_NFC_ST_NCI)	+= st-nci/
 obj-$(CONFIG_NFC_NXP_NCI)	+= nxp-nci/
-
-ccflags-$(CONFIG_NFC_DEBUG) := -DDEBUG
diff --git a/drivers/nfc/mei_phy.c b/drivers/nfc/mei_phy.c
index 11c7cbdade66..2b77ccf77f81 100644
--- a/drivers/nfc/mei_phy.c
+++ b/drivers/nfc/mei_phy.c
@@ -32,6 +32,51 @@ struct mei_nfc_hdr {
 	u16 data_size;
 } __packed;
 
+struct mei_nfc_cmd {
+	struct mei_nfc_hdr hdr;
+	u8 sub_command;
+	u8 data[];
+} __packed;
+
+struct mei_nfc_reply {
+	struct mei_nfc_hdr hdr;
+	u8 sub_command;
+	u8 reply_status;
+	u8 data[];
+} __packed;
+
+struct mei_nfc_if_version {
+	u8 radio_version_sw[3];
+	u8 reserved[3];
+	u8 radio_version_hw[3];
+	u8 i2c_addr;
+	u8 fw_ivn;
+	u8 vendor_id;
+	u8 radio_type;
+} __packed;
+
+struct mei_nfc_connect {
+	u8 fw_ivn;
+	u8 vendor_id;
+} __packed;
+
+struct mei_nfc_connect_resp {
+	u8 fw_ivn;
+	u8 vendor_id;
+	u16 me_major;
+	u16 me_minor;
+	u16 me_hotfix;
+	u16 me_build;
+} __packed;
+
+
+#define MEI_NFC_CMD_MAINTENANCE 0x00
+#define MEI_NFC_CMD_HCI_SEND 0x01
+#define MEI_NFC_CMD_HCI_RECV 0x02
+
+#define MEI_NFC_SUBCMD_CONNECT    0x00
+#define MEI_NFC_SUBCMD_IF_VERSION 0x01
+
 #define MEI_NFC_MAX_READ (MEI_NFC_HEADER_SIZE + MEI_NFC_MAX_HCI_PAYLOAD)
 
 #define MEI_DUMP_SKB_IN(info, skb)				\
@@ -45,51 +90,169 @@ do {								\
 do {								\
 	pr_debug("%s:\n", info);				\
 	print_hex_dump_debug("mei out: ", DUMP_PREFIX_OFFSET,	\
-		       16, 1, (skb)->data, (skb)->len, false);	\
+			16, 1, (skb)->data, (skb)->len, false);	\
 } while (0)
 
-int nfc_mei_phy_enable(void *phy_id)
+#define MEI_DUMP_NFC_HDR(info, _hdr)                                \
+do {                                                                \
+	pr_debug("%s:\n", info);                                    \
+	pr_debug("cmd=%02d status=%d req_id=%d rsvd=%d size=%d\n",  \
+		 (_hdr)->cmd, (_hdr)->status, (_hdr)->req_id,       \
+		 (_hdr)->reserved, (_hdr)->data_size);              \
+} while (0)
+
+static int mei_nfc_if_version(struct nfc_mei_phy *phy)
 {
-	int r;
-	struct nfc_mei_phy *phy = phy_id;
+
+	struct mei_nfc_cmd cmd;
+	struct mei_nfc_reply *reply = NULL;
+	struct mei_nfc_if_version *version;
+	size_t if_version_length;
+	int bytes_recv, r;
 
 	pr_info("%s\n", __func__);
 
-	if (phy->powered == 1)
-		return 0;
+	memset(&cmd, 0, sizeof(struct mei_nfc_cmd));
+	cmd.hdr.cmd = MEI_NFC_CMD_MAINTENANCE;
+	cmd.hdr.data_size = 1;
+	cmd.sub_command = MEI_NFC_SUBCMD_IF_VERSION;
 
-	r = mei_cl_enable_device(phy->device);
+	MEI_DUMP_NFC_HDR("version", &cmd.hdr);
+	r = mei_cl_send(phy->device, (u8 *)&cmd, sizeof(struct mei_nfc_cmd));
 	if (r < 0) {
-		pr_err("Could not enable device\n");
+		pr_err("Could not send IF version cmd\n");
 		return r;
 	}
 
-	r = mei_cl_register_event_cb(phy->device, nfc_mei_event_cb, phy);
-	if (r) {
-		pr_err("Event cb registration failed\n");
-		mei_cl_disable_device(phy->device);
-		phy->powered = 0;
+	/* to be sure on the stack we alloc memory */
+	if_version_length = sizeof(struct mei_nfc_reply) +
+		sizeof(struct mei_nfc_if_version);
 
-		return r;
+	reply = kzalloc(if_version_length, GFP_KERNEL);
+	if (!reply)
+		return -ENOMEM;
+
+	bytes_recv = mei_cl_recv(phy->device, (u8 *)reply, if_version_length);
+	if (bytes_recv < 0 || bytes_recv < sizeof(struct mei_nfc_reply)) {
+		pr_err("Could not read IF version\n");
+		r = -EIO;
+		goto err;
 	}
 
-	phy->powered = 1;
+	version = (struct mei_nfc_if_version *)reply->data;
 
-	return 0;
+	phy->fw_ivn = version->fw_ivn;
+	phy->vendor_id = version->vendor_id;
+	phy->radio_type = version->radio_type;
+
+err:
+	kfree(reply);
+	return r;
 }
-EXPORT_SYMBOL_GPL(nfc_mei_phy_enable);
 
-void nfc_mei_phy_disable(void *phy_id)
+static int mei_nfc_connect(struct nfc_mei_phy *phy)
 {
-	struct nfc_mei_phy *phy = phy_id;
+	struct mei_nfc_cmd *cmd, *reply;
+	struct mei_nfc_connect *connect;
+	struct mei_nfc_connect_resp *connect_resp;
+	size_t connect_length, connect_resp_length;
+	int bytes_recv, r;
 
 	pr_info("%s\n", __func__);
 
-	mei_cl_disable_device(phy->device);
+	connect_length = sizeof(struct mei_nfc_cmd) +
+			sizeof(struct mei_nfc_connect);
 
-	phy->powered = 0;
+	connect_resp_length = sizeof(struct mei_nfc_cmd) +
+			sizeof(struct mei_nfc_connect_resp);
+
+	cmd = kzalloc(connect_length, GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+	connect = (struct mei_nfc_connect *)cmd->data;
+
+	reply = kzalloc(connect_resp_length, GFP_KERNEL);
+	if (!reply) {
+		kfree(cmd);
+		return -ENOMEM;
+	}
+
+	connect_resp = (struct mei_nfc_connect_resp *)reply->data;
+
+	cmd->hdr.cmd = MEI_NFC_CMD_MAINTENANCE;
+	cmd->hdr.data_size = 3;
+	cmd->sub_command = MEI_NFC_SUBCMD_CONNECT;
+	connect->fw_ivn = phy->fw_ivn;
+	connect->vendor_id = phy->vendor_id;
+
+	MEI_DUMP_NFC_HDR("connect request", &cmd->hdr);
+	r = mei_cl_send(phy->device, (u8 *)cmd, connect_length);
+	if (r < 0) {
+		pr_err("Could not send connect cmd %d\n", r);
+		goto err;
+	}
+
+	bytes_recv = mei_cl_recv(phy->device, (u8 *)reply, connect_resp_length);
+	if (bytes_recv < 0) {
+		r = bytes_recv;
+		pr_err("Could not read connect response %d\n", r);
+		goto err;
+	}
+
+	MEI_DUMP_NFC_HDR("connect reply", &reply->hdr);
+
+	pr_info("IVN 0x%x Vendor ID 0x%x\n",
+		 connect_resp->fw_ivn, connect_resp->vendor_id);
+
+	pr_info("ME FW %d.%d.%d.%d\n",
+		connect_resp->me_major, connect_resp->me_minor,
+		connect_resp->me_hotfix, connect_resp->me_build);
+
+	r = 0;
+
+err:
+	kfree(reply);
+	kfree(cmd);
+
+	return r;
+}
+
+static int mei_nfc_send(struct nfc_mei_phy *phy, u8 *buf, size_t length)
+{
+	struct mei_nfc_hdr *hdr;
+	u8 *mei_buf;
+	int err;
+
+	err = -ENOMEM;
+	mei_buf = kzalloc(length + MEI_NFC_HEADER_SIZE, GFP_KERNEL);
+	if (!mei_buf)
+		goto out;
+
+	hdr = (struct mei_nfc_hdr *)mei_buf;
+	hdr->cmd = MEI_NFC_CMD_HCI_SEND;
+	hdr->status = 0;
+	hdr->req_id = phy->req_id;
+	hdr->reserved = 0;
+	hdr->data_size = length;
+
+	MEI_DUMP_NFC_HDR("send", hdr);
+
+	memcpy(mei_buf + MEI_NFC_HEADER_SIZE, buf, length);
+	err = mei_cl_send(phy->device, mei_buf, length + MEI_NFC_HEADER_SIZE);
+	if (err < 0)
+		goto out;
+
+	if (!wait_event_interruptible_timeout(phy->send_wq,
+				phy->recv_req_id == phy->req_id, HZ)) {
+		pr_err("NFC MEI command timeout\n");
+		err = -ETIME;
+	} else {
+		phy->req_id++;
+	}
+out:
+	kfree(mei_buf);
+	return err;
 }
-EXPORT_SYMBOL_GPL(nfc_mei_phy_disable);
 
 /*
  * Writing a frame must not return the number of written bytes.
@@ -103,14 +266,38 @@ static int nfc_mei_phy_write(void *phy_id, struct sk_buff *skb)
 
 	MEI_DUMP_SKB_OUT("mei frame sent", skb);
 
-	r = mei_cl_send(phy->device, skb->data, skb->len);
+	r = mei_nfc_send(phy, skb->data, skb->len);
 	if (r > 0)
 		r = 0;
 
 	return r;
 }
 
-void nfc_mei_event_cb(struct mei_cl_device *device, u32 events, void *context)
+static int mei_nfc_recv(struct nfc_mei_phy *phy, u8 *buf, size_t length)
+{
+	struct mei_nfc_hdr *hdr;
+	int received_length;
+
+	received_length = mei_cl_recv(phy->device, buf, length);
+	if (received_length < 0)
+		return received_length;
+
+	hdr = (struct mei_nfc_hdr *) buf;
+
+	MEI_DUMP_NFC_HDR("receive", hdr);
+	if (hdr->cmd == MEI_NFC_CMD_HCI_SEND) {
+		phy->recv_req_id = hdr->req_id;
+		wake_up(&phy->send_wq);
+
+		return 0;
+	}
+
+	return received_length;
+}
+
+
+static void nfc_mei_event_cb(struct mei_cl_device *device, u32 events,
+			     void *context)
 {
 	struct nfc_mei_phy *phy = context;
 
@@ -125,7 +312,7 @@ void nfc_mei_event_cb(struct mei_cl_device *device, u32 events, void *context)
 		if (!skb)
 			return;
 
-		reply_size = mei_cl_recv(device, skb->data, MEI_NFC_MAX_READ);
+		reply_size = mei_nfc_recv(phy, skb->data, MEI_NFC_MAX_READ);
 		if (reply_size < MEI_NFC_HEADER_SIZE) {
 			kfree_skb(skb);
 			return;
@@ -139,7 +326,61 @@ void nfc_mei_event_cb(struct mei_cl_device *device, u32 events, void *context)
 		nfc_hci_recv_frame(phy->hdev, skb);
 	}
 }
-EXPORT_SYMBOL_GPL(nfc_mei_event_cb);
+
+static int nfc_mei_phy_enable(void *phy_id)
+{
+	int r;
+	struct nfc_mei_phy *phy = phy_id;
+
+	pr_info("%s\n", __func__);
+
+	if (phy->powered == 1)
+		return 0;
+
+	r = mei_cl_enable_device(phy->device);
+	if (r < 0) {
+		pr_err("Could not enable device %d\n", r);
+		return r;
+	}
+
+	r = mei_nfc_if_version(phy);
+	if (r < 0) {
+		pr_err("Could not enable device %d\n", r);
+		goto err;
+	}
+
+	r = mei_nfc_connect(phy);
+	if (r < 0) {
+		pr_err("Could not connect to device %d\n", r);
+		goto err;
+	}
+
+	r = mei_cl_register_event_cb(phy->device, nfc_mei_event_cb, phy);
+	if (r) {
+		pr_err("Event cb registration failed %d\n", r);
+		goto err;
+	}
+
+	phy->powered = 1;
+
+	return 0;
+
+err:
+	phy->powered = 0;
+	mei_cl_disable_device(phy->device);
+	return r;
+}
+
+static void nfc_mei_phy_disable(void *phy_id)
+{
+	struct nfc_mei_phy *phy = phy_id;
+
+	pr_info("%s\n", __func__);
+
+	mei_cl_disable_device(phy->device);
+
+	phy->powered = 0;
+}
 
 struct nfc_phy_ops mei_phy_ops = {
 	.write = nfc_mei_phy_write,
@@ -157,6 +398,7 @@ struct nfc_mei_phy *nfc_mei_phy_alloc(struct mei_cl_device *device)
 		return NULL;
 
 	phy->device = device;
+	init_waitqueue_head(&phy->send_wq);
 	mei_cl_set_drvdata(device, phy);
 
 	return phy;
@@ -165,6 +407,7 @@ EXPORT_SYMBOL_GPL(nfc_mei_phy_alloc);
 
 void nfc_mei_phy_free(struct nfc_mei_phy *phy)
 {
+	mei_cl_disable_device(phy->device);
 	kfree(phy);
 }
 EXPORT_SYMBOL_GPL(nfc_mei_phy_free);
diff --git a/drivers/nfc/mei_phy.h b/drivers/nfc/mei_phy.h
index d669900f8278..fbfa3e61738f 100644
--- a/drivers/nfc/mei_phy.h
+++ b/drivers/nfc/mei_phy.h
@@ -3,27 +3,49 @@
 
 #include <linux/mei_cl_bus.h>
 #include <net/nfc/hci.h>
+#include <linux/uuid.h>
 
+#define MEI_NFC_UUID UUID_LE(0x0bb17a78, 0x2a8e, 0x4c50, \
+		0x94, 0xd4, 0x50, 0x26, 0x67, 0x23, 0x77, 0x5c)
 #define MEI_NFC_HEADER_SIZE 10
 #define MEI_NFC_MAX_HCI_PAYLOAD 300
 
+/**
+ * struct nfc_mei_phy
+ *
+ * @device: mei device
+ * @hdev:   nfc hci device
+
+ * @send_wq: send completion wait queue
+ * @fw_ivn: NFC Interface Version Number
+ * @vendor_id: NFC manufacturer ID
+ * @radio_type: NFC radio type
+ * @reserved: reserved for alignment
+ * @req_id:  message counter
+ * @recv_req_id: reception message counter
+ * @powered: the device is in powered state
+ * @hard_fault: < 0 if hardware error occurred
+ *    and prevents normal operation.
+ */
 struct nfc_mei_phy {
 	struct mei_cl_device *device;
 	struct nfc_hci_dev *hdev;
 
-	int powered;
+	wait_queue_head_t send_wq;
+	u8 fw_ivn;
+	u8 vendor_id;
+	u8 radio_type;
+	u8 reserved;
+
+	u16 req_id;
+	u16 recv_req_id;
 
-	int hard_fault;		/*
-				 * < 0 if hardware error occured
-				 * and prevents normal operation.
-				 */
+	int powered;
+	int hard_fault;
 };
 
 extern struct nfc_phy_ops mei_phy_ops;
 
-int nfc_mei_phy_enable(void *phy_id);
-void nfc_mei_phy_disable(void *phy_id);
-void nfc_mei_event_cb(struct mei_cl_device *device, u32 events, void *context);
 struct nfc_mei_phy *nfc_mei_phy_alloc(struct mei_cl_device *device);
 void nfc_mei_phy_free(struct nfc_mei_phy *phy);
 
diff --git a/drivers/nfc/microread/i2c.c b/drivers/nfc/microread/i2c.c
index 661e2c8143c4..daf352597ef8 100644
--- a/drivers/nfc/microread/i2c.c
+++ b/drivers/nfc/microread/i2c.c
@@ -211,7 +211,6 @@ flush:
 static irqreturn_t microread_i2c_irq_thread_fn(int irq, void *phy_id)
 {
 	struct microread_i2c_phy *phy = phy_id;
-	struct i2c_client *client;
 	struct sk_buff *skb = NULL;
 	int r;
 
@@ -220,8 +219,6 @@ static irqreturn_t microread_i2c_irq_thread_fn(int irq, void *phy_id)
 		return IRQ_NONE;
 	}
 
-	client = phy->i2c_dev;
-
 	if (phy->hard_fault != 0)
 		return IRQ_HANDLED;
 
diff --git a/drivers/nfc/microread/mei.c b/drivers/nfc/microread/mei.c
index 2d1395be64ae..f9f5fc97cdd7 100644
--- a/drivers/nfc/microread/mei.c
+++ b/drivers/nfc/microread/mei.c
@@ -67,7 +67,7 @@ static int microread_mei_remove(struct mei_cl_device *device)
 }
 
 static struct mei_cl_device_id microread_mei_tbl[] = {
-	{ MICROREAD_DRIVER_NAME },
+	{ MICROREAD_DRIVER_NAME, MEI_NFC_UUID},
 
 	/* required last entry */
 	{ }
diff --git a/drivers/nfc/nfcmrvl/Kconfig b/drivers/nfc/nfcmrvl/Kconfig
index 5e18afd9abe2..796be2411440 100644
--- a/drivers/nfc/nfcmrvl/Kconfig
+++ b/drivers/nfc/nfcmrvl/Kconfig
@@ -21,3 +21,14 @@ config NFC_MRVL_USB
 
 	  Say Y here to compile support for Marvell NFC-over-USB driver
 	  into the kernel or say M to compile it as module.
+
+config NFC_MRVL_UART
+	tristate "Marvell NFC-over-UART driver"
+	depends on NFC_MRVL && NFC_NCI_UART
+	help
+	  Marvell NFC-over-UART driver.
+
+	  This driver provides support for Marvell NFC-over-UART devices
+
+	  Say Y here to compile support for Marvell NFC-over-UART driver
+	  into the kernel or say M to compile it as module.
diff --git a/drivers/nfc/nfcmrvl/Makefile b/drivers/nfc/nfcmrvl/Makefile
index 97a0de72dc01..775196274d1f 100644
--- a/drivers/nfc/nfcmrvl/Makefile
+++ b/drivers/nfc/nfcmrvl/Makefile
@@ -7,3 +7,6 @@ obj-$(CONFIG_NFC_MRVL) += nfcmrvl.o
 
 nfcmrvl_usb-y += usb.o
 obj-$(CONFIG_NFC_MRVL_USB) += nfcmrvl_usb.o
+
+nfcmrvl_uart-y += uart.o
+obj-$(CONFIG_NFC_MRVL_UART) += nfcmrvl_uart.o
diff --git a/drivers/nfc/nfcmrvl/main.c b/drivers/nfc/nfcmrvl/main.c
index ad4933cefbd1..4a8866d62941 100644
--- a/drivers/nfc/nfcmrvl/main.c
+++ b/drivers/nfc/nfcmrvl/main.c
@@ -17,6 +17,9 @@
  */
 
 #include <linux/module.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/of_gpio.h>
 #include <linux/nfc.h>
 #include <net/nfc/nci.h>
 #include <net/nfc/nci_core.h>
@@ -63,20 +66,25 @@ static int nfcmrvl_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
 	if (!test_bit(NFCMRVL_NCI_RUNNING, &priv->flags))
 		return -EBUSY;
 
+	if (priv->config.hci_muxed) {
+		unsigned char *hdr;
+		unsigned char len = skb->len;
+
+		hdr = (char *) skb_push(skb, NFCMRVL_HCI_EVENT_HEADER_SIZE);
+		hdr[0] = NFCMRVL_HCI_COMMAND_CODE;
+		hdr[1] = NFCMRVL_HCI_OGF;
+		hdr[2] = NFCMRVL_HCI_OCF;
+		hdr[3] = len;
+	}
+
 	return priv->if_ops->nci_send(priv, skb);
 }
 
 static int nfcmrvl_nci_setup(struct nci_dev *ndev)
 {
-	__u8 val;
-
-	val = NFCMRVL_GPIO_PIN_NFC_NOT_ALLOWED;
-	nci_set_config(ndev, NFCMRVL_NOT_ALLOWED_ID, 1, &val);
-	val = NFCMRVL_GPIO_PIN_NFC_ACTIVE;
-	nci_set_config(ndev, NFCMRVL_ACTIVE_ID, 1, &val);
-	val = NFCMRVL_EXT_COEX_ENABLE;
-	nci_set_config(ndev, NFCMRVL_EXT_COEX_ID, 1, &val);
+	__u8 val = 1;
 
+	nci_set_config(ndev, NFCMRVL_PB_BAIL_OUT, 1, &val);
 	return 0;
 }
 
@@ -88,11 +96,13 @@ static struct nci_ops nfcmrvl_nci_ops = {
 };
 
 struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
-						 struct nfcmrvl_if_ops *ops,
-						 struct device *dev)
+				struct nfcmrvl_if_ops *ops,
+				struct device *dev,
+				struct nfcmrvl_platform_data *pdata)
 {
 	struct nfcmrvl_private *priv;
 	int rc;
+	int headroom = 0;
 	u32 protocols;
 
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
@@ -103,13 +113,30 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
 	priv->if_ops = ops;
 	priv->dev = dev;
 
+	memcpy(&priv->config, pdata, sizeof(*pdata));
+
+	if (priv->config.reset_n_io) {
+		rc = devm_gpio_request_one(dev,
+					   priv->config.reset_n_io,
+					   GPIOF_OUT_INIT_LOW,
+					   "nfcmrvl_reset_n");
+		if (rc < 0)
+			nfc_err(dev, "failed to request reset_n io\n");
+	}
+
+	if (priv->config.hci_muxed)
+		headroom = NFCMRVL_HCI_EVENT_HEADER_SIZE;
+
 	protocols = NFC_PROTO_JEWEL_MASK
-		| NFC_PROTO_MIFARE_MASK | NFC_PROTO_FELICA_MASK
+		| NFC_PROTO_MIFARE_MASK
+		| NFC_PROTO_FELICA_MASK
 		| NFC_PROTO_ISO14443_MASK
 		| NFC_PROTO_ISO14443_B_MASK
+		| NFC_PROTO_ISO15693_MASK
 		| NFC_PROTO_NFC_DEP_MASK;
 
-	priv->ndev = nci_allocate_device(&nfcmrvl_nci_ops, protocols, 0, 0);
+	priv->ndev = nci_allocate_device(&nfcmrvl_nci_ops, protocols,
+					 headroom, 0);
 	if (!priv->ndev) {
 		nfc_err(dev, "nci_allocate_device failed\n");
 		rc = -ENOMEM;
@@ -118,6 +145,8 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
 
 	nci_set_drvdata(priv->ndev, priv);
 
+	nfcmrvl_chip_reset(priv);
+
 	rc = nci_register_device(priv->ndev);
 	if (rc) {
 		nfc_err(dev, "nci_register_device failed %d\n", rc);
@@ -144,21 +173,84 @@ void nfcmrvl_nci_unregister_dev(struct nfcmrvl_private *priv)
 }
 EXPORT_SYMBOL_GPL(nfcmrvl_nci_unregister_dev);
 
-int nfcmrvl_nci_recv_frame(struct nfcmrvl_private *priv, void *data, int count)
+int nfcmrvl_nci_recv_frame(struct nfcmrvl_private *priv, struct sk_buff *skb)
 {
-	struct sk_buff *skb;
-
-	skb = nci_skb_alloc(priv->ndev, count, GFP_ATOMIC);
-	if (!skb)
-		return -ENOMEM;
+	if (priv->config.hci_muxed) {
+		if (skb->data[0] == NFCMRVL_HCI_EVENT_CODE &&
+		    skb->data[1] == NFCMRVL_HCI_NFC_EVENT_CODE) {
+			/* Data packet, let's extract NCI payload */
+			skb_pull(skb, NFCMRVL_HCI_EVENT_HEADER_SIZE);
+		} else {
+			/* Skip this packet */
+			kfree_skb(skb);
+			return 0;
+		}
+	}
 
-	memcpy(skb_put(skb, count), data, count);
-	nci_recv_frame(priv->ndev, skb);
+	if (test_bit(NFCMRVL_NCI_RUNNING, &priv->flags))
+		nci_recv_frame(priv->ndev, skb);
+	else {
+		/* Drop this packet since nobody wants it */
+		kfree_skb(skb);
+		return 0;
+	}
 
-	return count;
+	return 0;
 }
 EXPORT_SYMBOL_GPL(nfcmrvl_nci_recv_frame);
 
+void nfcmrvl_chip_reset(struct nfcmrvl_private *priv)
+{
+	/*
+	 * This function does not take care if someone is using the device.
+	 * To be improved.
+	 */
+
+	if (priv->config.reset_n_io) {
+		nfc_info(priv->dev, "reset the chip\n");
+		gpio_set_value(priv->config.reset_n_io, 0);
+		usleep_range(5000, 10000);
+		gpio_set_value(priv->config.reset_n_io, 1);
+	} else
+		nfc_info(priv->dev, "no reset available on this interface\n");
+}
+
+#ifdef CONFIG_OF
+
+int nfcmrvl_parse_dt(struct device_node *node,
+		     struct nfcmrvl_platform_data *pdata)
+{
+	int reset_n_io;
+
+	reset_n_io = of_get_named_gpio(node, "reset-n-io", 0);
+	if (reset_n_io < 0) {
+		pr_info("no reset-n-io config\n");
+		reset_n_io = 0;
+	} else if (!gpio_is_valid(reset_n_io)) {
+		pr_err("invalid reset-n-io GPIO\n");
+		return reset_n_io;
+	}
+	pdata->reset_n_io = reset_n_io;
+
+	if (of_find_property(node, "hci-muxed", NULL))
+		pdata->hci_muxed = 1;
+	else
+		pdata->hci_muxed = 0;
+
+	return 0;
+}
+
+#else
+
+int nfcmrvl_parse_dt(struct device_node *node,
+		     struct nfcmrvl_platform_data *pdata)
+{
+	return -ENODEV;
+}
+
+#endif
+EXPORT_SYMBOL_GPL(nfcmrvl_parse_dt);
+
 MODULE_AUTHOR("Marvell International Ltd.");
 MODULE_DESCRIPTION("Marvell NFC driver ver " VERSION);
 MODULE_VERSION(VERSION);
diff --git a/drivers/nfc/nfcmrvl/nfcmrvl.h b/drivers/nfc/nfcmrvl/nfcmrvl.h
index 54c4a956bd45..e5a7e5464f2e 100644
--- a/drivers/nfc/nfcmrvl/nfcmrvl.h
+++ b/drivers/nfc/nfcmrvl/nfcmrvl.h
@@ -16,6 +16,11 @@
  * this warranty disclaimer.
  **/
 
+#ifndef _NFCMRVL_H_
+#define _NFCMRVL_H_
+
+#include <linux/platform_data/nfcmrvl.h>
+
 /* Define private flags: */
 #define NFCMRVL_NCI_RUNNING			1
 
@@ -27,11 +32,49 @@
 #define NFCMRVL_GPIO_PIN_NFC_ACTIVE		0xB
 #define NFCMRVL_NCI_MAX_EVENT_SIZE		260
 
+/*
+** NCI FW Parmaters
+*/
+
+#define NFCMRVL_PB_BAIL_OUT			0x11
+
+/*
+** HCI defines
+*/
+
+#define NFCMRVL_HCI_EVENT_HEADER_SIZE		0x04
+#define NFCMRVL_HCI_EVENT_CODE			0x04
+#define NFCMRVL_HCI_NFC_EVENT_CODE		0xFF
+#define NFCMRVL_HCI_COMMAND_CODE		0x01
+#define NFCMRVL_HCI_OGF				0x81
+#define NFCMRVL_HCI_OCF				0xFE
+
+enum nfcmrvl_phy {
+	NFCMRVL_PHY_USB		= 0,
+	NFCMRVL_PHY_UART	= 1,
+};
+
+
 struct nfcmrvl_private {
-	struct nci_dev *ndev;
+
 	unsigned long flags;
+
+	/* Platform configuration */
+	struct nfcmrvl_platform_data config;
+
+	struct nci_dev *ndev;
+
+	/*
+	** PHY related information
+	*/
+
+	/* PHY driver context */
 	void *drv_data;
+	/* PHY device */
 	struct device *dev;
+	/* PHY type */
+	enum nfcmrvl_phy phy;
+	/* Low level driver ops */
 	struct nfcmrvl_if_ops *if_ops;
 };
 
@@ -42,7 +85,16 @@ struct nfcmrvl_if_ops {
 };
 
 void nfcmrvl_nci_unregister_dev(struct nfcmrvl_private *priv);
-int nfcmrvl_nci_recv_frame(struct nfcmrvl_private *priv, void *data, int count);
+int nfcmrvl_nci_recv_frame(struct nfcmrvl_private *priv, struct sk_buff *skb);
 struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
-						 struct nfcmrvl_if_ops *ops,
-						 struct device *dev);
+				struct nfcmrvl_if_ops *ops,
+				struct device *dev,
+				struct nfcmrvl_platform_data *pdata);
+
+
+void nfcmrvl_chip_reset(struct nfcmrvl_private *priv);
+
+int nfcmrvl_parse_dt(struct device_node *node,
+		     struct nfcmrvl_platform_data *pdata);
+
+#endif
diff --git a/drivers/nfc/nfcmrvl/uart.c b/drivers/nfc/nfcmrvl/uart.c
new file mode 100644
index 000000000000..61442d6528a6
--- /dev/null
+++ b/drivers/nfc/nfcmrvl/uart.c
@@ -0,0 +1,225 @@
+/**
+ * Marvell NFC-over-UART driver
+ *
+ * Copyright (C) 2015, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available on the worldwide web at
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/of_gpio.h>
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include "nfcmrvl.h"
+
+static unsigned int hci_muxed;
+static unsigned int flow_control;
+static unsigned int break_control;
+static unsigned int reset_n_io;
+
+/*
+** NFCMRVL NCI OPS
+*/
+
+static int nfcmrvl_uart_nci_open(struct nfcmrvl_private *priv)
+{
+	return 0;
+}
+
+static int nfcmrvl_uart_nci_close(struct nfcmrvl_private *priv)
+{
+	return 0;
+}
+
+static int nfcmrvl_uart_nci_send(struct nfcmrvl_private *priv,
+				 struct sk_buff *skb)
+{
+	struct nci_uart *nu = priv->drv_data;
+
+	return nu->ops.send(nu, skb);
+}
+
+static struct nfcmrvl_if_ops uart_ops = {
+	.nci_open = nfcmrvl_uart_nci_open,
+	.nci_close = nfcmrvl_uart_nci_close,
+	.nci_send = nfcmrvl_uart_nci_send,
+};
+
+#ifdef CONFIG_OF
+
+static int nfcmrvl_uart_parse_dt(struct device_node *node,
+				 struct nfcmrvl_platform_data *pdata)
+{
+	struct device_node *matched_node;
+	int ret;
+
+	matched_node = of_find_compatible_node(node, NULL, "mrvl,nfc-uart");
+	if (!matched_node)
+		return -ENODEV;
+
+	ret = nfcmrvl_parse_dt(matched_node, pdata);
+	if (ret < 0) {
+		pr_err("Failed to get generic entries\n");
+		return ret;
+	}
+
+	if (of_find_property(matched_node, "flow-control", NULL))
+		pdata->flow_control = 1;
+	else
+		pdata->flow_control = 0;
+
+	if (of_find_property(matched_node, "break-control", NULL))
+		pdata->break_control = 1;
+	else
+		pdata->break_control = 0;
+
+	return 0;
+}
+
+#else
+
+static int nfcmrvl_uart_parse_dt(struct device_node *node,
+				 struct nfcmrvl_platform_data *pdata)
+{
+	return -ENODEV;
+}
+
+#endif
+
+/*
+** NCI UART OPS
+*/
+
+static int nfcmrvl_nci_uart_open(struct nci_uart *nu)
+{
+	struct nfcmrvl_private *priv;
+	struct nfcmrvl_platform_data *pdata = NULL;
+	struct nfcmrvl_platform_data config;
+
+	/*
+	 * Platform data cannot be used here since usually it is already used
+	 * by low level serial driver. We can try to retrieve serial device
+	 * and check if DT entries were added.
+	 */
+
+	if (nu->tty->dev->parent && nu->tty->dev->parent->of_node)
+		if (nfcmrvl_uart_parse_dt(nu->tty->dev->parent->of_node,
+					  &config) == 0)
+			pdata = &config;
+
+	if (!pdata) {
+		pr_info("No platform data / DT -> fallback to module params\n");
+		config.hci_muxed = hci_muxed;
+		config.reset_n_io = reset_n_io;
+		config.flow_control = flow_control;
+		config.break_control = break_control;
+		pdata = &config;
+	}
+
+	priv = nfcmrvl_nci_register_dev(nu, &uart_ops, nu->tty->dev, pdata);
+	if (IS_ERR(priv))
+		return PTR_ERR(priv);
+
+	priv->phy = NFCMRVL_PHY_UART;
+
+	nu->drv_data = priv;
+	nu->ndev = priv->ndev;
+
+	/* Set BREAK */
+	if (priv->config.break_control && nu->tty->ops->break_ctl)
+		nu->tty->ops->break_ctl(nu->tty, -1);
+
+	return 0;
+}
+
+static void nfcmrvl_nci_uart_close(struct nci_uart *nu)
+{
+	nfcmrvl_nci_unregister_dev((struct nfcmrvl_private *)nu->drv_data);
+}
+
+static int nfcmrvl_nci_uart_recv(struct nci_uart *nu, struct sk_buff *skb)
+{
+	return nfcmrvl_nci_recv_frame((struct nfcmrvl_private *)nu->drv_data,
+				      skb);
+}
+
+static void nfcmrvl_nci_uart_tx_start(struct nci_uart *nu)
+{
+	struct nfcmrvl_private *priv = (struct nfcmrvl_private *)nu->drv_data;
+
+	/* Remove BREAK to wake up the NFCC */
+	if (priv->config.break_control && nu->tty->ops->break_ctl) {
+		nu->tty->ops->break_ctl(nu->tty, 0);
+		usleep_range(3000, 5000);
+	}
+}
+
+static void nfcmrvl_nci_uart_tx_done(struct nci_uart *nu)
+{
+	struct nfcmrvl_private *priv = (struct nfcmrvl_private *)nu->drv_data;
+
+	/*
+	** To ensure that if the NFCC goes in DEEP SLEEP sate we can wake him
+	** up. we set BREAK. Once we will be ready to send again we will remove
+	** it.
+	*/
+	if (priv->config.break_control && nu->tty->ops->break_ctl)
+		nu->tty->ops->break_ctl(nu->tty, -1);
+}
+
+static struct nci_uart nfcmrvl_nci_uart = {
+	.owner  = THIS_MODULE,
+	.name   = "nfcmrvl_uart",
+	.driver = NCI_UART_DRIVER_MARVELL,
+	.ops	= {
+		.open		= nfcmrvl_nci_uart_open,
+		.close		= nfcmrvl_nci_uart_close,
+		.recv		= nfcmrvl_nci_uart_recv,
+		.tx_start	= nfcmrvl_nci_uart_tx_start,
+		.tx_done	= nfcmrvl_nci_uart_tx_done,
+	}
+};
+
+/*
+** Module init
+*/
+
+static int nfcmrvl_uart_init_module(void)
+{
+	return nci_uart_register(&nfcmrvl_nci_uart);
+}
+
+static void nfcmrvl_uart_exit_module(void)
+{
+	nci_uart_unregister(&nfcmrvl_nci_uart);
+}
+
+module_init(nfcmrvl_uart_init_module);
+module_exit(nfcmrvl_uart_exit_module);
+
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_DESCRIPTION("Marvell NFC-over-UART");
+MODULE_LICENSE("GPL v2");
+
+module_param(flow_control, uint, 0);
+MODULE_PARM_DESC(flow_control, "Tell if UART needs flow control at init.");
+
+module_param(break_control, uint, 0);
+MODULE_PARM_DESC(break_control, "Tell if UART driver must drive break signal.");
+
+module_param(hci_muxed, uint, 0);
+MODULE_PARM_DESC(hci_muxed, "Tell if transport is muxed in HCI one.");
+
+module_param(reset_n_io, uint, 0);
+MODULE_PARM_DESC(reset_n_io, "GPIO that is wired to RESET_N signal.");
diff --git a/drivers/nfc/nfcmrvl/usb.c b/drivers/nfc/nfcmrvl/usb.c
index 6cf15c1a2618..7d1fe436c9f6 100644
--- a/drivers/nfc/nfcmrvl/usb.c
+++ b/drivers/nfc/nfcmrvl/usb.c
@@ -26,7 +26,8 @@
 #define VERSION "1.0"
 
 static struct usb_device_id nfcmrvl_table[] = {
-	{ USB_DEVICE_INTERFACE_CLASS(0x1286, 0x2046, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x1286, 0x2046,
+					USB_CLASS_VENDOR_SPEC, 4, 1) },
 	{ }	/* Terminating entry */
 };
 
@@ -69,18 +70,27 @@ static int nfcmrvl_inc_tx(struct nfcmrvl_usb_drv_data *drv_data)
 static void nfcmrvl_bulk_complete(struct urb *urb)
 {
 	struct nfcmrvl_usb_drv_data *drv_data = urb->context;
+	struct sk_buff *skb;
 	int err;
 
-	dev_dbg(&drv_data->udev->dev, "urb %p status %d count %d",
+	dev_dbg(&drv_data->udev->dev, "urb %p status %d count %d\n",
 		urb, urb->status, urb->actual_length);
 
 	if (!test_bit(NFCMRVL_NCI_RUNNING, &drv_data->flags))
 		return;
 
 	if (!urb->status) {
-		if (nfcmrvl_nci_recv_frame(drv_data->priv, urb->transfer_buffer,
-					   urb->actual_length) < 0)
-			nfc_err(&drv_data->udev->dev, "corrupted Rx packet\n");
+		skb = nci_skb_alloc(drv_data->priv->ndev, urb->actual_length,
+				    GFP_ATOMIC);
+		if (!skb) {
+			nfc_err(&drv_data->udev->dev, "failed to alloc mem\n");
+		} else {
+			memcpy(skb_put(skb, urb->actual_length),
+			       urb->transfer_buffer, urb->actual_length);
+			if (nfcmrvl_nci_recv_frame(drv_data->priv, skb) < 0)
+				nfc_err(&drv_data->udev->dev,
+					"corrupted Rx packet\n");
+		}
 	}
 
 	if (!test_bit(NFCMRVL_USB_BULK_RUNNING, &drv_data->flags))
@@ -292,6 +302,10 @@ static int nfcmrvl_probe(struct usb_interface *intf,
 	struct nfcmrvl_private *priv;
 	int i;
 	struct usb_device *udev = interface_to_usbdev(intf);
+	struct nfcmrvl_platform_data config;
+
+	/* No configuration for USB */
+	memset(&config, 0, sizeof(config));
 
 	nfc_info(&udev->dev, "intf %p id %p\n", intf, id);
 
@@ -329,11 +343,12 @@ static int nfcmrvl_probe(struct usb_interface *intf,
 	init_usb_anchor(&drv_data->deferred);
 
 	priv = nfcmrvl_nci_register_dev(drv_data, &usb_ops,
-					&drv_data->udev->dev);
+					&drv_data->udev->dev, &config);
 	if (IS_ERR(priv))
 		return PTR_ERR(priv);
 
 	drv_data->priv = priv;
+	drv_data->priv->phy = NFCMRVL_PHY_USB;
 	priv->dev = &drv_data->udev->dev;
 
 	usb_set_intfdata(intf, drv_data);
diff --git a/drivers/nfc/nxp-nci/Makefile b/drivers/nfc/nxp-nci/Makefile
index c008be30bb18..c9ec7869dbd2 100644
--- a/drivers/nfc/nxp-nci/Makefile
+++ b/drivers/nfc/nxp-nci/Makefile
@@ -7,5 +7,3 @@ nxp-nci_i2c-objs = i2c.o
 
 obj-$(CONFIG_NFC_NXP_NCI) += nxp-nci.o
 obj-$(CONFIG_NFC_NXP_NCI_I2C) += nxp-nci_i2c.o
-
-ccflags-$(CONFIG_NFC_DEBUG) := -DDEBUG
diff --git a/drivers/nfc/nxp-nci/i2c.c b/drivers/nfc/nxp-nci/i2c.c
index 17bd67dbebf0..2f77f1d03638 100644
--- a/drivers/nfc/nxp-nci/i2c.c
+++ b/drivers/nfc/nxp-nci/i2c.c
@@ -2,8 +2,10 @@
  * I2C link layer for the NXP NCI driver
  *
  * Copyright (C) 2014  NXP Semiconductors  All rights reserved.
+ * Copyright (C) 2012-2015  Intel Corporation. All rights reserved.
  *
  * Authors: Clément Perrochaud <clement.perrochaud@nxp.com>
+ * Authors: Oleg Zhurakivskyy <oleg.zhurakivskyy@intel.com>
  *
  * Derived from PN544 device driver:
  * Copyright (C) 2012  Intel Corporation. All rights reserved.
@@ -23,12 +25,14 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/acpi.h>
 #include <linux/delay.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/miscdevice.h>
 #include <linux/module.h>
 #include <linux/nfc.h>
+#include <linux/gpio/consumer.h>
 #include <linux/of_gpio.h>
 #include <linux/of_irq.h>
 #include <linux/platform_data/nxp-nci.h>
@@ -48,6 +52,7 @@ struct nxp_nci_i2c_phy {
 
 	unsigned int gpio_en;
 	unsigned int gpio_fw;
+	unsigned int gpio_irq;
 
 	int hard_fault; /*
 			 * < 0 if hardware error occurred (e.g. i2c err)
@@ -308,6 +313,37 @@ static int nxp_nci_i2c_parse_devtree(struct i2c_client *client)
 
 #endif
 
+static int nxp_nci_i2c_acpi_config(struct nxp_nci_i2c_phy *phy)
+{
+	struct i2c_client *client = phy->i2c_dev;
+	struct gpio_desc *gpiod_en, *gpiod_fw, *gpiod_irq;
+
+	gpiod_en = devm_gpiod_get_index(&client->dev, NULL, 2);
+	gpiod_fw = devm_gpiod_get_index(&client->dev, NULL, 1);
+	gpiod_irq = devm_gpiod_get_index(&client->dev, NULL, 0);
+
+	if (IS_ERR(gpiod_en) || IS_ERR(gpiod_fw) || IS_ERR(gpiod_irq)) {
+		nfc_err(&client->dev, "No GPIOs\n");
+		return -EINVAL;
+	}
+
+	gpiod_direction_output(gpiod_en, 0);
+	gpiod_direction_output(gpiod_fw, 0);
+	gpiod_direction_input(gpiod_irq);
+
+	client->irq = gpiod_to_irq(gpiod_irq);
+	if (client->irq < 0) {
+		nfc_err(&client->dev, "No IRQ\n");
+		return -EINVAL;
+	}
+
+	phy->gpio_en = desc_to_gpio(gpiod_en);
+	phy->gpio_fw = desc_to_gpio(gpiod_fw);
+	phy->gpio_irq = desc_to_gpio(gpiod_irq);
+
+	return 0;
+}
+
 static int nxp_nci_i2c_probe(struct i2c_client *client,
 			    const struct i2c_device_id *id)
 {
@@ -343,6 +379,11 @@ static int nxp_nci_i2c_probe(struct i2c_client *client,
 		phy->gpio_en = pdata->gpio_en;
 		phy->gpio_fw = pdata->gpio_fw;
 		client->irq = pdata->irq;
+	} else if (ACPI_HANDLE(&client->dev)) {
+		r = nxp_nci_i2c_acpi_config(phy);
+		if (r < 0)
+			goto probe_exit;
+		goto nci_probe;
 	} else {
 		nfc_err(&client->dev, "No platform data\n");
 		r = -EINVAL;
@@ -359,6 +400,7 @@ static int nxp_nci_i2c_probe(struct i2c_client *client,
 	if (r < 0)
 		goto probe_exit;
 
+nci_probe:
 	r = nxp_nci_probe(phy, &client->dev, &i2c_phy_ops,
 			  NXP_NCI_I2C_MAX_PAYLOAD, &phy->ndev);
 	if (r < 0)
@@ -397,10 +439,19 @@ static const struct of_device_id of_nxp_nci_i2c_match[] = {
 };
 MODULE_DEVICE_TABLE(of, of_nxp_nci_i2c_match);
 
+#ifdef CONFIG_ACPI
+static struct acpi_device_id acpi_id[] = {
+	{ "NXP7471" },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, acpi_id);
+#endif
+
 static struct i2c_driver nxp_nci_i2c_driver = {
 	.driver = {
 		   .name = NXP_NCI_I2C_DRIVER_NAME,
 		   .owner  = THIS_MODULE,
+		   .acpi_match_table = ACPI_PTR(acpi_id),
 		   .of_match_table = of_match_ptr(of_nxp_nci_i2c_match),
 		  },
 	.probe = nxp_nci_i2c_probe,
@@ -413,3 +464,4 @@ module_i2c_driver(nxp_nci_i2c_driver);
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("I2C driver for NXP NCI NFC controllers");
 MODULE_AUTHOR("Clément Perrochaud <clement.perrochaud@nxp.com>");
+MODULE_AUTHOR("Oleg Zhurakivskyy <oleg.zhurakivskyy@intel.com>");
diff --git a/drivers/nfc/pn544/i2c.c b/drivers/nfc/pn544/i2c.c
index 6fd986f5ac3e..fa75c53f3fa5 100644
--- a/drivers/nfc/pn544/i2c.c
+++ b/drivers/nfc/pn544/i2c.c
@@ -895,56 +895,35 @@ static int pn544_hci_i2c_acpi_request_resources(struct i2c_client *client)
 		return -ENODEV;
 
 	/* Get EN GPIO from ACPI */
-	gpiod_en = devm_gpiod_get_index(dev, PN544_GPIO_NAME_EN, 1);
+	gpiod_en = devm_gpiod_get_index(dev, PN544_GPIO_NAME_EN, 1,
+					GPIOD_OUT_LOW);
 	if (IS_ERR(gpiod_en)) {
-		nfc_err(dev,
-			"Unable to get EN GPIO\n");
+		nfc_err(dev, "Unable to get EN GPIO\n");
 		return -ENODEV;
 	}
 
-	phy->gpio_en  = desc_to_gpio(gpiod_en);
-
-	/* Configuration EN GPIO */
-	ret = gpiod_direction_output(gpiod_en, 0);
-	if (ret) {
-		nfc_err(dev, "Fail EN pin direction\n");
-		return ret;
-	}
+	phy->gpio_en = desc_to_gpio(gpiod_en);
 
 	/* Get FW GPIO from ACPI */
-	gpiod_fw = devm_gpiod_get_index(dev, PN544_GPIO_NAME_FW, 2);
+	gpiod_fw = devm_gpiod_get_index(dev, PN544_GPIO_NAME_FW, 2,
+					GPIOD_OUT_LOW);
 	if (IS_ERR(gpiod_fw)) {
-		nfc_err(dev,
-			"Unable to get FW GPIO\n");
+		nfc_err(dev, "Unable to get FW GPIO\n");
 		return -ENODEV;
 	}
 
-	phy->gpio_fw  = desc_to_gpio(gpiod_fw);
-
-	/* Configuration FW GPIO */
-	ret = gpiod_direction_output(gpiod_fw, 0);
-	if (ret) {
-		nfc_err(dev, "Fail FW pin direction\n");
-		return ret;
-	}
+	phy->gpio_fw = desc_to_gpio(gpiod_fw);
 
 	/* Get IRQ GPIO */
-	gpiod_irq = devm_gpiod_get_index(dev, PN544_GPIO_NAME_IRQ, 0);
+	gpiod_irq = devm_gpiod_get_index(dev, PN544_GPIO_NAME_IRQ, 0,
+					 GPIOD_IN);
 	if (IS_ERR(gpiod_irq)) {
-		nfc_err(dev,
-			"Unable to get IRQ GPIO\n");
+		nfc_err(dev, "Unable to get IRQ GPIO\n");
 		return -ENODEV;
 	}
 
 	phy->gpio_irq = desc_to_gpio(gpiod_irq);
 
-	/* Configure IRQ GPIO */
-	ret = gpiod_direction_input(gpiod_irq);
-	if (ret) {
-		nfc_err(dev, "Fail IRQ pin direction\n");
-		return ret;
-	}
-
 	/* Map the pin to an IRQ */
 	ret = gpiod_to_irq(gpiod_irq);
 	if (ret < 0) {
diff --git a/drivers/nfc/pn544/mei.c b/drivers/nfc/pn544/mei.c
index 330cd4031009..101a37e12efa 100644
--- a/drivers/nfc/pn544/mei.c
+++ b/drivers/nfc/pn544/mei.c
@@ -67,7 +67,7 @@ static int pn544_mei_remove(struct mei_cl_device *device)
 }
 
 static struct mei_cl_device_id pn544_mei_tbl[] = {
-	{ PN544_DRIVER_NAME },
+	{ PN544_DRIVER_NAME, MEI_NFC_UUID},
 
 	/* required last entry */
 	{ }
diff --git a/drivers/nfc/st-nci/Kconfig b/drivers/nfc/st-nci/Kconfig
new file mode 100644
index 000000000000..fc3904c946ee
--- /dev/null
+++ b/drivers/nfc/st-nci/Kconfig
@@ -0,0 +1,23 @@
+config NFC_ST_NCI
+	tristate "STMicroelectronics ST NCI NFC driver"
+	depends on NFC_NCI
+	default n
+	---help---
+	  STMicroelectronics NFC NCI chips core driver. It implements the chipset
+	  NCI logic and hooks into the NFC kernel APIs. Physical layers will
+	  register against it.
+
+	  To compile this driver as a module, choose m here. The module will
+	  be called st-nci.
+	  Say N if unsure.
+
+config NFC_ST_NCI_I2C
+	tristate "NFC ST NCI i2c support"
+	depends on NFC_ST_NCI && I2C
+	---help---
+	  This module adds support for an I2C interface to the
+	  STMicroelectronics NFC NCI chips familly.
+	  Select this if your platform is using the i2c bus.
+
+	  If you choose to build a module, it'll be called st-nci_i2c.
+	  Say N if unsure.
diff --git a/drivers/nfc/st-nci/Makefile b/drivers/nfc/st-nci/Makefile
new file mode 100644
index 000000000000..0df157df3a94
--- /dev/null
+++ b/drivers/nfc/st-nci/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for ST21NFCB NCI based NFC driver
+#
+
+st-nci-objs = ndlc.o core.o st-nci_se.o
+obj-$(CONFIG_NFC_ST_NCI)     += st-nci.o
+
+st-nci_i2c-objs = i2c.o
+obj-$(CONFIG_NFC_ST_NCI_I2C) += st-nci_i2c.o
diff --git a/drivers/nfc/st-nci/core.c b/drivers/nfc/st-nci/core.c
new file mode 100644
index 000000000000..c419d3943973
--- /dev/null
+++ b/drivers/nfc/st-nci/core.c
@@ -0,0 +1,179 @@
+/*
+ * NCI based Driver for STMicroelectronics NFC Chip
+ *
+ * Copyright (C) 2014-2015  STMicroelectronics SAS. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/nfc.h>
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+
+#include "st-nci.h"
+#include "st-nci_se.h"
+
+#define DRIVER_DESC "NCI NFC driver for ST_NCI"
+
+#define ST_NCI1_X_PROPRIETARY_ISO15693 0x83
+
+static int st_nci_init(struct nci_dev *ndev)
+{
+	struct nci_mode_set_cmd cmd;
+
+	cmd.cmd_type = ST_NCI_SET_NFC_MODE;
+	cmd.mode = 1;
+
+	return nci_prop_cmd(ndev, ST_NCI_CORE_PROP,
+			sizeof(struct nci_mode_set_cmd), (__u8 *)&cmd);
+}
+
+static int st_nci_open(struct nci_dev *ndev)
+{
+	struct st_nci_info *info = nci_get_drvdata(ndev);
+	int r;
+
+	if (test_and_set_bit(ST_NCI_RUNNING, &info->flags))
+		return 0;
+
+	r = ndlc_open(info->ndlc);
+	if (r)
+		clear_bit(ST_NCI_RUNNING, &info->flags);
+
+	return r;
+}
+
+static int st_nci_close(struct nci_dev *ndev)
+{
+	struct st_nci_info *info = nci_get_drvdata(ndev);
+
+	if (!test_bit(ST_NCI_RUNNING, &info->flags))
+		return 0;
+
+	ndlc_close(info->ndlc);
+
+	clear_bit(ST_NCI_RUNNING, &info->flags);
+
+	return 0;
+}
+
+static int st_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct st_nci_info *info = nci_get_drvdata(ndev);
+
+	skb->dev = (void *)ndev;
+
+	if (!test_bit(ST_NCI_RUNNING, &info->flags))
+		return -EBUSY;
+
+	return ndlc_send(info->ndlc, skb);
+}
+
+static __u32 st_nci_get_rfprotocol(struct nci_dev *ndev,
+					 __u8 rf_protocol)
+{
+	return rf_protocol == ST_NCI1_X_PROPRIETARY_ISO15693 ?
+		NFC_PROTO_ISO15693_MASK : 0;
+}
+
+static int st_nci_prop_rsp_packet(struct nci_dev *ndev,
+					struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	nci_req_complete(ndev, status);
+	return 0;
+}
+
+static struct nci_prop_ops st_nci_prop_ops[] = {
+	{
+		.opcode = nci_opcode_pack(NCI_GID_PROPRIETARY,
+					  ST_NCI_CORE_PROP),
+		.rsp = st_nci_prop_rsp_packet,
+	},
+};
+
+static struct nci_ops st_nci_ops = {
+	.init = st_nci_init,
+	.open = st_nci_open,
+	.close = st_nci_close,
+	.send = st_nci_send,
+	.get_rfprotocol = st_nci_get_rfprotocol,
+	.discover_se = st_nci_discover_se,
+	.enable_se = st_nci_enable_se,
+	.disable_se = st_nci_disable_se,
+	.se_io = st_nci_se_io,
+	.hci_load_session = st_nci_hci_load_session,
+	.hci_event_received = st_nci_hci_event_received,
+	.hci_cmd_received = st_nci_hci_cmd_received,
+	.prop_ops = st_nci_prop_ops,
+	.n_prop_ops = ARRAY_SIZE(st_nci_prop_ops),
+};
+
+int st_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
+		       int phy_tailroom)
+{
+	struct st_nci_info *info;
+	int r;
+	u32 protocols;
+
+	info = devm_kzalloc(ndlc->dev,
+			sizeof(struct st_nci_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	protocols = NFC_PROTO_JEWEL_MASK
+		| NFC_PROTO_MIFARE_MASK
+		| NFC_PROTO_FELICA_MASK
+		| NFC_PROTO_ISO14443_MASK
+		| NFC_PROTO_ISO14443_B_MASK
+		| NFC_PROTO_ISO15693_MASK
+		| NFC_PROTO_NFC_DEP_MASK;
+
+	ndlc->ndev = nci_allocate_device(&st_nci_ops, protocols,
+					phy_headroom, phy_tailroom);
+	if (!ndlc->ndev) {
+		pr_err("Cannot allocate nfc ndev\n");
+		return -ENOMEM;
+	}
+	info->ndlc = ndlc;
+
+	nci_set_drvdata(ndlc->ndev, info);
+
+	r = nci_register_device(ndlc->ndev);
+	if (r) {
+		pr_err("Cannot register nfc device to nci core\n");
+		nci_free_device(ndlc->ndev);
+		return r;
+	}
+
+	return st_nci_se_init(ndlc->ndev);
+}
+EXPORT_SYMBOL_GPL(st_nci_probe);
+
+void st_nci_remove(struct nci_dev *ndev)
+{
+	struct st_nci_info *info = nci_get_drvdata(ndev);
+
+	ndlc_close(info->ndlc);
+
+	nci_unregister_device(ndev);
+	nci_free_device(ndev);
+}
+EXPORT_SYMBOL_GPL(st_nci_remove);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/nfc/st21nfcb/i2c.c b/drivers/nfc/st-nci/i2c.c
index 76a4cad41cec..06175ce769bb 100644
--- a/drivers/nfc/st21nfcb/i2c.c
+++ b/drivers/nfc/st-nci/i2c.c
@@ -1,6 +1,6 @@
 /*
- * I2C Link Layer for ST21NFCB NCI based Driver
- * Copyright (C) 2014  STMicroelectronics SAS. All rights reserved.
+ * I2C Link Layer for ST NCI NFC controller familly based Driver
+ * Copyright (C) 2014-2015 STMicroelectronics SAS. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -25,7 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/nfc.h>
-#include <linux/platform_data/st21nfcb.h>
+#include <linux/platform_data/st_nci.h>
 
 #include "ndlc.h"
 
@@ -35,25 +35,23 @@
 #define ST21NFCB_FRAME_HEADROOM	1
 #define ST21NFCB_FRAME_TAILROOM 0
 
-#define ST21NFCB_NCI_I2C_MIN_SIZE 4   /* PCB(1) + NCI Packet header(3) */
-#define ST21NFCB_NCI_I2C_MAX_SIZE 250 /* req 4.2.1 */
+#define ST_NCI_I2C_MIN_SIZE 4   /* PCB(1) + NCI Packet header(3) */
+#define ST_NCI_I2C_MAX_SIZE 250 /* req 4.2.1 */
 
-#define ST21NFCB_NCI_I2C_DRIVER_NAME "st21nfcb_nci_i2c"
+#define ST_NCI_I2C_DRIVER_NAME "st_nci_i2c"
 
-static struct i2c_device_id st21nfcb_nci_i2c_id_table[] = {
-	{ST21NFCB_NCI_DRIVER_NAME, 0},
+static struct i2c_device_id st_nci_i2c_id_table[] = {
+	{ST_NCI_DRIVER_NAME, 0},
 	{}
 };
-MODULE_DEVICE_TABLE(i2c, st21nfcb_nci_i2c_id_table);
+MODULE_DEVICE_TABLE(i2c, st_nci_i2c_id_table);
 
-struct st21nfcb_i2c_phy {
+struct st_nci_i2c_phy {
 	struct i2c_client *i2c_dev;
 	struct llt_ndlc *ndlc;
 
 	unsigned int gpio_reset;
 	unsigned int irq_polarity;
-
-	int powered;
 };
 
 #define I2C_DUMP_SKB(info, skb)					\
@@ -63,33 +61,26 @@ do {								\
 		       16, 1, (skb)->data, (skb)->len, 0);	\
 } while (0)
 
-static int st21nfcb_nci_i2c_enable(void *phy_id)
+static int st_nci_i2c_enable(void *phy_id)
 {
-	struct st21nfcb_i2c_phy *phy = phy_id;
+	struct st_nci_i2c_phy *phy = phy_id;
 
 	gpio_set_value(phy->gpio_reset, 0);
 	usleep_range(10000, 15000);
 	gpio_set_value(phy->gpio_reset, 1);
-	phy->powered = 1;
 	usleep_range(80000, 85000);
 
+	if (phy->ndlc->powered == 0)
+		enable_irq(phy->i2c_dev->irq);
+
 	return 0;
 }
 
-static void st21nfcb_nci_i2c_disable(void *phy_id)
+static void st_nci_i2c_disable(void *phy_id)
 {
-	struct st21nfcb_i2c_phy *phy = phy_id;
-
-	phy->powered = 0;
-	/* reset chip in order to flush clf */
-	gpio_set_value(phy->gpio_reset, 0);
-	usleep_range(10000, 15000);
-	gpio_set_value(phy->gpio_reset, 1);
-}
+	struct st_nci_i2c_phy *phy = phy_id;
 
-static void st21nfcb_nci_remove_header(struct sk_buff *skb)
-{
-	skb_pull(skb, ST21NFCB_FRAME_HEADROOM);
+	disable_irq_nosync(phy->i2c_dev->irq);
 }
 
 /*
@@ -97,13 +88,13 @@ static void st21nfcb_nci_remove_header(struct sk_buff *skb)
  * It must return either zero for success, or <0 for error.
  * In addition, it must not alter the skb
  */
-static int st21nfcb_nci_i2c_write(void *phy_id, struct sk_buff *skb)
+static int st_nci_i2c_write(void *phy_id, struct sk_buff *skb)
 {
 	int r = -1;
-	struct st21nfcb_i2c_phy *phy = phy_id;
+	struct st_nci_i2c_phy *phy = phy_id;
 	struct i2c_client *client = phy->i2c_dev;
 
-	I2C_DUMP_SKB("st21nfcb_nci_i2c_write", skb);
+	I2C_DUMP_SKB("st_nci_i2c_write", skb);
 
 	if (phy->ndlc->hard_fault != 0)
 		return phy->ndlc->hard_fault;
@@ -121,8 +112,6 @@ static int st21nfcb_nci_i2c_write(void *phy_id, struct sk_buff *skb)
 			r = 0;
 	}
 
-	st21nfcb_nci_remove_header(skb);
-
 	return r;
 }
 
@@ -135,40 +124,40 @@ static int st21nfcb_nci_i2c_write(void *phy_id, struct sk_buff *skb)
  * at end of read)
  * -EREMOTEIO : i2c read error (fatal)
  * -EBADMSG : frame was incorrect and discarded
- * (value returned from st21nfcb_nci_i2c_repack)
+ * (value returned from st_nci_i2c_repack)
  * -EIO : if no ST21NFCB_SOF_EOF is found after reaching
  * the read length end sequence
  */
-static int st21nfcb_nci_i2c_read(struct st21nfcb_i2c_phy *phy,
+static int st_nci_i2c_read(struct st_nci_i2c_phy *phy,
 				 struct sk_buff **skb)
 {
 	int r;
 	u8 len;
-	u8 buf[ST21NFCB_NCI_I2C_MAX_SIZE];
+	u8 buf[ST_NCI_I2C_MAX_SIZE];
 	struct i2c_client *client = phy->i2c_dev;
 
-	r = i2c_master_recv(client, buf, ST21NFCB_NCI_I2C_MIN_SIZE);
+	r = i2c_master_recv(client, buf, ST_NCI_I2C_MIN_SIZE);
 	if (r < 0) {  /* Retry, chip was in standby */
 		usleep_range(1000, 4000);
-		r = i2c_master_recv(client, buf, ST21NFCB_NCI_I2C_MIN_SIZE);
+		r = i2c_master_recv(client, buf, ST_NCI_I2C_MIN_SIZE);
 	}
 
-	if (r != ST21NFCB_NCI_I2C_MIN_SIZE)
+	if (r != ST_NCI_I2C_MIN_SIZE)
 		return -EREMOTEIO;
 
 	len = be16_to_cpu(*(__be16 *) (buf + 2));
-	if (len > ST21NFCB_NCI_I2C_MAX_SIZE) {
+	if (len > ST_NCI_I2C_MAX_SIZE) {
 		nfc_err(&client->dev, "invalid frame len\n");
 		return -EBADMSG;
 	}
 
-	*skb = alloc_skb(ST21NFCB_NCI_I2C_MIN_SIZE + len, GFP_KERNEL);
+	*skb = alloc_skb(ST_NCI_I2C_MIN_SIZE + len, GFP_KERNEL);
 	if (*skb == NULL)
 		return -ENOMEM;
 
-	skb_reserve(*skb, ST21NFCB_NCI_I2C_MIN_SIZE);
-	skb_put(*skb, ST21NFCB_NCI_I2C_MIN_SIZE);
-	memcpy((*skb)->data, buf, ST21NFCB_NCI_I2C_MIN_SIZE);
+	skb_reserve(*skb, ST_NCI_I2C_MIN_SIZE);
+	skb_put(*skb, ST_NCI_I2C_MIN_SIZE);
+	memcpy((*skb)->data, buf, ST_NCI_I2C_MIN_SIZE);
 
 	if (!len)
 		return 0;
@@ -180,7 +169,7 @@ static int st21nfcb_nci_i2c_read(struct st21nfcb_i2c_phy *phy,
 	}
 
 	skb_put(*skb, len);
-	memcpy((*skb)->data + ST21NFCB_NCI_I2C_MIN_SIZE, buf, len);
+	memcpy((*skb)->data + ST_NCI_I2C_MIN_SIZE, buf, len);
 
 	I2C_DUMP_SKB("i2c frame read", *skb);
 
@@ -192,9 +181,9 @@ static int st21nfcb_nci_i2c_read(struct st21nfcb_i2c_phy *phy,
  *
  * On ST21NFCB, IRQ goes in idle state when read starts.
  */
-static irqreturn_t st21nfcb_nci_irq_thread_fn(int irq, void *phy_id)
+static irqreturn_t st_nci_irq_thread_fn(int irq, void *phy_id)
 {
-	struct st21nfcb_i2c_phy *phy = phy_id;
+	struct st_nci_i2c_phy *phy = phy_id;
 	struct i2c_client *client;
 	struct sk_buff *skb = NULL;
 	int r;
@@ -210,12 +199,12 @@ static irqreturn_t st21nfcb_nci_irq_thread_fn(int irq, void *phy_id)
 	if (phy->ndlc->hard_fault)
 		return IRQ_HANDLED;
 
-	if (!phy->powered) {
-		st21nfcb_nci_i2c_disable(phy);
+	if (!phy->ndlc->powered) {
+		st_nci_i2c_disable(phy);
 		return IRQ_HANDLED;
 	}
 
-	r = st21nfcb_nci_i2c_read(phy, &skb);
+	r = st_nci_i2c_read(phy, &skb);
 	if (r == -EREMOTEIO || r == -ENOMEM || r == -EBADMSG)
 		return IRQ_HANDLED;
 
@@ -225,15 +214,15 @@ static irqreturn_t st21nfcb_nci_irq_thread_fn(int irq, void *phy_id)
 }
 
 static struct nfc_phy_ops i2c_phy_ops = {
-	.write = st21nfcb_nci_i2c_write,
-	.enable = st21nfcb_nci_i2c_enable,
-	.disable = st21nfcb_nci_i2c_disable,
+	.write = st_nci_i2c_write,
+	.enable = st_nci_i2c_enable,
+	.disable = st_nci_i2c_disable,
 };
 
 #ifdef CONFIG_OF
-static int st21nfcb_nci_i2c_of_request_resources(struct i2c_client *client)
+static int st_nci_i2c_of_request_resources(struct i2c_client *client)
 {
-	struct st21nfcb_i2c_phy *phy = i2c_get_clientdata(client);
+	struct st_nci_i2c_phy *phy = i2c_get_clientdata(client);
 	struct device_node *pp;
 	int gpio;
 	int r;
@@ -264,16 +253,16 @@ static int st21nfcb_nci_i2c_of_request_resources(struct i2c_client *client)
 	return 0;
 }
 #else
-static int st21nfcb_nci_i2c_of_request_resources(struct i2c_client *client)
+static int st_nci_i2c_of_request_resources(struct i2c_client *client)
 {
 	return -ENODEV;
 }
 #endif
 
-static int st21nfcb_nci_i2c_request_resources(struct i2c_client *client)
+static int st_nci_i2c_request_resources(struct i2c_client *client)
 {
-	struct st21nfcb_nfc_platform_data *pdata;
-	struct st21nfcb_i2c_phy *phy = i2c_get_clientdata(client);
+	struct st_nci_nfc_platform_data *pdata;
+	struct st_nci_i2c_phy *phy = i2c_get_clientdata(client);
 	int r;
 
 	pdata = client->dev.platform_data;
@@ -296,11 +285,11 @@ static int st21nfcb_nci_i2c_request_resources(struct i2c_client *client)
 	return 0;
 }
 
-static int st21nfcb_nci_i2c_probe(struct i2c_client *client,
+static int st_nci_i2c_probe(struct i2c_client *client,
 				  const struct i2c_device_id *id)
 {
-	struct st21nfcb_i2c_phy *phy;
-	struct st21nfcb_nfc_platform_data *pdata;
+	struct st_nci_i2c_phy *phy;
+	struct st_nci_nfc_platform_data *pdata;
 	int r;
 
 	dev_dbg(&client->dev, "%s\n", __func__);
@@ -311,7 +300,7 @@ static int st21nfcb_nci_i2c_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
-	phy = devm_kzalloc(&client->dev, sizeof(struct st21nfcb_i2c_phy),
+	phy = devm_kzalloc(&client->dev, sizeof(struct st_nci_i2c_phy),
 			   GFP_KERNEL);
 	if (!phy)
 		return -ENOMEM;
@@ -322,13 +311,13 @@ static int st21nfcb_nci_i2c_probe(struct i2c_client *client,
 
 	pdata = client->dev.platform_data;
 	if (!pdata && client->dev.of_node) {
-		r = st21nfcb_nci_i2c_of_request_resources(client);
+		r = st_nci_i2c_of_request_resources(client);
 		if (r) {
 			nfc_err(&client->dev, "No platform data\n");
 			return r;
 		}
 	} else if (pdata) {
-		r = st21nfcb_nci_i2c_request_resources(client);
+		r = st_nci_i2c_request_resources(client);
 		if (r) {
 			nfc_err(&client->dev,
 				"Cannot get platform resources\n");
@@ -349,50 +338,48 @@ static int st21nfcb_nci_i2c_probe(struct i2c_client *client,
 	}
 
 	r = devm_request_threaded_irq(&client->dev, client->irq, NULL,
-				st21nfcb_nci_irq_thread_fn,
+				st_nci_irq_thread_fn,
 				phy->irq_polarity | IRQF_ONESHOT,
-				ST21NFCB_NCI_DRIVER_NAME, phy);
+				ST_NCI_DRIVER_NAME, phy);
 	if (r < 0)
 		nfc_err(&client->dev, "Unable to register IRQ handler\n");
 
 	return r;
 }
 
-static int st21nfcb_nci_i2c_remove(struct i2c_client *client)
+static int st_nci_i2c_remove(struct i2c_client *client)
 {
-	struct st21nfcb_i2c_phy *phy = i2c_get_clientdata(client);
+	struct st_nci_i2c_phy *phy = i2c_get_clientdata(client);
 
 	dev_dbg(&client->dev, "%s\n", __func__);
 
 	ndlc_remove(phy->ndlc);
 
-	if (phy->powered)
-		st21nfcb_nci_i2c_disable(phy);
-
 	return 0;
 }
 
 #ifdef CONFIG_OF
-static const struct of_device_id of_st21nfcb_i2c_match[] = {
+static const struct of_device_id of_st_nci_i2c_match[] = {
 	{ .compatible = "st,st21nfcb-i2c", },
 	{ .compatible = "st,st21nfcb_i2c", },
+	{ .compatible = "st,st21nfcc-i2c", },
 	{}
 };
-MODULE_DEVICE_TABLE(of, of_st21nfcb_i2c_match);
+MODULE_DEVICE_TABLE(of, of_st_nci_i2c_match);
 #endif
 
-static struct i2c_driver st21nfcb_nci_i2c_driver = {
+static struct i2c_driver st_nci_i2c_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
-		.name = ST21NFCB_NCI_I2C_DRIVER_NAME,
-		.of_match_table = of_match_ptr(of_st21nfcb_i2c_match),
+		.name = ST_NCI_I2C_DRIVER_NAME,
+		.of_match_table = of_match_ptr(of_st_nci_i2c_match),
 	},
-	.probe = st21nfcb_nci_i2c_probe,
-	.id_table = st21nfcb_nci_i2c_id_table,
-	.remove = st21nfcb_nci_i2c_remove,
+	.probe = st_nci_i2c_probe,
+	.id_table = st_nci_i2c_id_table,
+	.remove = st_nci_i2c_remove,
 };
 
-module_i2c_driver(st21nfcb_nci_i2c_driver);
+module_i2c_driver(st_nci_i2c_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/nfc/st21nfcb/ndlc.c b/drivers/nfc/st-nci/ndlc.c
index 6014b5859465..56c6a4cb4c96 100644
--- a/drivers/nfc/st21nfcb/ndlc.c
+++ b/drivers/nfc/st-nci/ndlc.c
@@ -1,7 +1,7 @@
 /*
  * Low Level Transport (NDLC) Driver for STMicroelectronics NFC Chip
  *
- * Copyright (C) 2014  STMicroelectronics SAS. All rights reserved.
+ * Copyright (C) 2014-2015  STMicroelectronics SAS. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -20,7 +20,7 @@
 #include <net/nfc/nci_core.h>
 
 #include "ndlc.h"
-#include "st21nfcb.h"
+#include "st-nci.h"
 
 #define NDLC_TIMER_T1		100
 #define NDLC_TIMER_T1_WAIT	400
@@ -59,13 +59,25 @@ int ndlc_open(struct llt_ndlc *ndlc)
 {
 	/* toggle reset pin */
 	ndlc->ops->enable(ndlc->phy_id);
+	ndlc->powered = 1;
 	return 0;
 }
 EXPORT_SYMBOL(ndlc_open);
 
 void ndlc_close(struct llt_ndlc *ndlc)
 {
+	struct nci_mode_set_cmd cmd;
+
+	cmd.cmd_type = ST_NCI_SET_NFC_MODE;
+	cmd.mode = 0;
+
 	/* toggle reset pin */
+	ndlc->ops->enable(ndlc->phy_id);
+
+	nci_prop_cmd(ndlc->ndev, ST_NCI_CORE_PROP,
+		     sizeof(struct nci_mode_set_cmd), (__u8 *)&cmd);
+
+	ndlc->powered = 0;
 	ndlc->ops->disable(ndlc->phy_id);
 }
 EXPORT_SYMBOL(ndlc_close);
@@ -262,6 +274,7 @@ int ndlc_probe(void *phy_id, struct nfc_phy_ops *phy_ops, struct device *dev,
 	ndlc->ops = phy_ops;
 	ndlc->phy_id = phy_id;
 	ndlc->dev = dev;
+	ndlc->powered = 0;
 
 	*ndlc_id = ndlc;
 
@@ -280,12 +293,14 @@ int ndlc_probe(void *phy_id, struct nfc_phy_ops *phy_ops, struct device *dev,
 
 	INIT_WORK(&ndlc->sm_work, llt_ndlc_sm_work);
 
-	return st21nfcb_nci_probe(ndlc, phy_headroom, phy_tailroom);
+	return st_nci_probe(ndlc, phy_headroom, phy_tailroom);
 }
 EXPORT_SYMBOL(ndlc_probe);
 
 void ndlc_remove(struct llt_ndlc *ndlc)
 {
+	st_nci_remove(ndlc->ndev);
+
 	/* cancel timers */
 	del_timer_sync(&ndlc->t1_timer);
 	del_timer_sync(&ndlc->t2_timer);
@@ -294,7 +309,5 @@ void ndlc_remove(struct llt_ndlc *ndlc)
 
 	skb_queue_purge(&ndlc->rcv_q);
 	skb_queue_purge(&ndlc->send_q);
-
-	st21nfcb_nci_remove(ndlc->ndev);
 }
 EXPORT_SYMBOL(ndlc_remove);
diff --git a/drivers/nfc/st21nfcb/ndlc.h b/drivers/nfc/st-nci/ndlc.h
index b28140e0cd78..6361005ef003 100644
--- a/drivers/nfc/st21nfcb/ndlc.h
+++ b/drivers/nfc/st-nci/ndlc.h
@@ -1,7 +1,7 @@
 /*
  * NCI based Driver for STMicroelectronics NFC Chip
  *
- * Copyright (C) 2014  STMicroelectronics SAS. All rights reserved.
+ * Copyright (C) 2014-2015  STMicroelectronics SAS. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -43,10 +43,11 @@ struct llt_ndlc {
 	struct device *dev;
 
 	/*
-	 * < 0 if hardware error occured
+	 * < 0 if hardware error occurred
 	 * and prevents normal operation.
 	 */
 	int hard_fault;
+	int powered;
 };
 
 int ndlc_open(struct llt_ndlc *ndlc);
diff --git a/drivers/nfc/st21nfcb/st21nfcb.h b/drivers/nfc/st-nci/st-nci.h
index 5ef8a58c9839..850a2395deb7 100644
--- a/drivers/nfc/st21nfcb/st21nfcb.h
+++ b/drivers/nfc/st-nci/st-nci.h
@@ -16,23 +16,35 @@
  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef __LOCAL_ST21NFCB_H_
-#define __LOCAL_ST21NFCB_H_
+#ifndef __LOCAL_ST_NCI_H_
+#define __LOCAL_ST_NCI_H_
 
-#include "st21nfcb_se.h"
+#include "st-nci_se.h"
 #include "ndlc.h"
 
 /* Define private flags: */
-#define ST21NFCB_NCI_RUNNING			1
+#define ST_NCI_RUNNING			1
 
-struct st21nfcb_nci_info {
+#define ST_NCI_CORE_PROP                0x01
+#define ST_NCI_SET_NFC_MODE             0x02
+
+struct nci_mode_set_cmd {
+	u8 cmd_type;
+	u8 mode;
+} __packed;
+
+struct nci_mode_set_rsp {
+	u8 status;
+} __packed;
+
+struct st_nci_info {
 	struct llt_ndlc *ndlc;
 	unsigned long flags;
-	struct st21nfcb_se_info se_info;
+	struct st_nci_se_info se_info;
 };
 
-void st21nfcb_nci_remove(struct nci_dev *ndev);
-int st21nfcb_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
+void st_nci_remove(struct nci_dev *ndev);
+int st_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
 		int phy_tailroom);
 
-#endif /* __LOCAL_ST21NFCB_H_ */
+#endif /* __LOCAL_ST_NCI_H_ */
diff --git a/drivers/nfc/st21nfcb/st21nfcb_se.c b/drivers/nfc/st-nci/st-nci_se.c
index 24862a525fb5..97addfa96c6f 100644
--- a/drivers/nfc/st21nfcb/st21nfcb_se.c
+++ b/drivers/nfc/st-nci/st-nci_se.c
@@ -1,7 +1,7 @@
 /*
- * NCI based Driver for STMicroelectronics NFC Chip
+ * Secure Element driver for STMicroelectronics NFC NCI chip
  *
- * Copyright (C) 2014  STMicroelectronics SAS. All rights reserved.
+ * Copyright (C) 2014-2015 STMicroelectronics SAS. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -22,10 +22,10 @@
 #include <net/nfc/nci.h>
 #include <net/nfc/nci_core.h>
 
-#include "st21nfcb.h"
-#include "st21nfcb_se.h"
+#include "st-nci.h"
+#include "st-nci_se.h"
 
-struct st21nfcb_pipe_info {
+struct st_nci_pipe_info {
 	u8 pipe_state;
 	u8 src_host_id;
 	u8 src_gate_id;
@@ -34,166 +34,166 @@ struct st21nfcb_pipe_info {
 } __packed;
 
 /* Hosts */
-#define ST21NFCB_HOST_CONTROLLER_ID     0x00
-#define ST21NFCB_TERMINAL_HOST_ID       0x01
-#define ST21NFCB_UICC_HOST_ID           0x02
-#define ST21NFCB_ESE_HOST_ID            0xc0
+#define ST_NCI_HOST_CONTROLLER_ID     0x00
+#define ST_NCI_TERMINAL_HOST_ID       0x01
+#define ST_NCI_UICC_HOST_ID           0x02
+#define ST_NCI_ESE_HOST_ID            0xc0
 
 /* Gates */
-#define ST21NFCB_DEVICE_MGNT_GATE       0x01
-#define ST21NFCB_APDU_READER_GATE       0xf0
-#define ST21NFCB_CONNECTIVITY_GATE      0x41
+#define ST_NCI_DEVICE_MGNT_GATE       0x01
+#define ST_NCI_APDU_READER_GATE       0xf0
+#define ST_NCI_CONNECTIVITY_GATE      0x41
 
 /* Pipes */
-#define ST21NFCB_DEVICE_MGNT_PIPE               0x02
+#define ST_NCI_DEVICE_MGNT_PIPE               0x02
 
 /* Connectivity pipe only */
-#define ST21NFCB_SE_COUNT_PIPE_UICC             0x01
+#define ST_NCI_SE_COUNT_PIPE_UICC             0x01
 /* Connectivity + APDU Reader pipe */
-#define ST21NFCB_SE_COUNT_PIPE_EMBEDDED         0x02
+#define ST_NCI_SE_COUNT_PIPE_EMBEDDED         0x02
 
-#define ST21NFCB_SE_TO_HOT_PLUG			1000 /* msecs */
-#define ST21NFCB_SE_TO_PIPES			2000
+#define ST_NCI_SE_TO_HOT_PLUG			1000 /* msecs */
+#define ST_NCI_SE_TO_PIPES			2000
 
-#define ST21NFCB_EVT_HOT_PLUG_IS_INHIBITED(x)   (x->data[0] & 0x80)
+#define ST_NCI_EVT_HOT_PLUG_IS_INHIBITED(x)   (x->data[0] & 0x80)
 
 #define NCI_HCI_APDU_PARAM_ATR                     0x01
 #define NCI_HCI_ADMIN_PARAM_SESSION_IDENTITY       0x01
 #define NCI_HCI_ADMIN_PARAM_WHITELIST              0x03
 #define NCI_HCI_ADMIN_PARAM_HOST_LIST              0x04
 
-#define ST21NFCB_EVT_SE_HARD_RESET		0x20
-#define ST21NFCB_EVT_TRANSMIT_DATA		0x10
-#define ST21NFCB_EVT_WTX_REQUEST		0x11
-#define ST21NFCB_EVT_SE_SOFT_RESET		0x11
-#define ST21NFCB_EVT_SE_END_OF_APDU_TRANSFER	0x21
-#define ST21NFCB_EVT_HOT_PLUG			0x03
+#define ST_NCI_EVT_SE_HARD_RESET		0x20
+#define ST_NCI_EVT_TRANSMIT_DATA		0x10
+#define ST_NCI_EVT_WTX_REQUEST		0x11
+#define ST_NCI_EVT_SE_SOFT_RESET		0x11
+#define ST_NCI_EVT_SE_END_OF_APDU_TRANSFER	0x21
+#define ST_NCI_EVT_HOT_PLUG			0x03
 
-#define ST21NFCB_SE_MODE_OFF                    0x00
-#define ST21NFCB_SE_MODE_ON                     0x01
+#define ST_NCI_SE_MODE_OFF                    0x00
+#define ST_NCI_SE_MODE_ON                     0x01
 
-#define ST21NFCB_EVT_CONNECTIVITY       0x10
-#define ST21NFCB_EVT_TRANSACTION        0x12
+#define ST_NCI_EVT_CONNECTIVITY       0x10
+#define ST_NCI_EVT_TRANSACTION        0x12
 
-#define ST21NFCB_DM_GETINFO             0x13
-#define ST21NFCB_DM_GETINFO_PIPE_LIST   0x02
-#define ST21NFCB_DM_GETINFO_PIPE_INFO   0x01
-#define ST21NFCB_DM_PIPE_CREATED        0x02
-#define ST21NFCB_DM_PIPE_OPEN           0x04
-#define ST21NFCB_DM_RF_ACTIVE           0x80
-#define ST21NFCB_DM_DISCONNECT          0x30
+#define ST_NCI_DM_GETINFO             0x13
+#define ST_NCI_DM_GETINFO_PIPE_LIST   0x02
+#define ST_NCI_DM_GETINFO_PIPE_INFO   0x01
+#define ST_NCI_DM_PIPE_CREATED        0x02
+#define ST_NCI_DM_PIPE_OPEN           0x04
+#define ST_NCI_DM_RF_ACTIVE           0x80
+#define ST_NCI_DM_DISCONNECT          0x30
 
-#define ST21NFCB_DM_IS_PIPE_OPEN(p) \
-	((p & 0x0f) == (ST21NFCB_DM_PIPE_CREATED | ST21NFCB_DM_PIPE_OPEN))
+#define ST_NCI_DM_IS_PIPE_OPEN(p) \
+	((p & 0x0f) == (ST_NCI_DM_PIPE_CREATED | ST_NCI_DM_PIPE_OPEN))
 
-#define ST21NFCB_ATR_DEFAULT_BWI        0x04
+#define ST_NCI_ATR_DEFAULT_BWI        0x04
 
 /*
  * WT = 2^BWI/10[s], convert into msecs and add a secure
  * room by increasing by 2 this timeout
  */
-#define ST21NFCB_BWI_TO_TIMEOUT(x)      ((1 << x) * 200)
-#define ST21NFCB_ATR_GET_Y_FROM_TD(x)   (x >> 4)
+#define ST_NCI_BWI_TO_TIMEOUT(x)      ((1 << x) * 200)
+#define ST_NCI_ATR_GET_Y_FROM_TD(x)   (x >> 4)
 
 /* If TA is present bit 0 is set */
-#define ST21NFCB_ATR_TA_PRESENT(x) (x & 0x01)
+#define ST_NCI_ATR_TA_PRESENT(x) (x & 0x01)
 /* If TB is present bit 1 is set */
-#define ST21NFCB_ATR_TB_PRESENT(x) (x & 0x02)
+#define ST_NCI_ATR_TB_PRESENT(x) (x & 0x02)
 
-#define ST21NFCB_NUM_DEVICES           256
+#define ST_NCI_NUM_DEVICES           256
 
-static DECLARE_BITMAP(dev_mask, ST21NFCB_NUM_DEVICES);
+static DECLARE_BITMAP(dev_mask, ST_NCI_NUM_DEVICES);
 
-/* Here are the mandatory pipe for st21nfcb */
-static struct nci_hci_gate st21nfcb_gates[] = {
+/* Here are the mandatory pipe for st_nci */
+static struct nci_hci_gate st_nci_gates[] = {
 	{NCI_HCI_ADMIN_GATE, NCI_HCI_ADMIN_PIPE,
-					ST21NFCB_HOST_CONTROLLER_ID},
+					ST_NCI_HOST_CONTROLLER_ID},
 	{NCI_HCI_LINK_MGMT_GATE, NCI_HCI_LINK_MGMT_PIPE,
-					ST21NFCB_HOST_CONTROLLER_ID},
-	{ST21NFCB_DEVICE_MGNT_GATE, ST21NFCB_DEVICE_MGNT_PIPE,
-					ST21NFCB_HOST_CONTROLLER_ID},
+					ST_NCI_HOST_CONTROLLER_ID},
+	{ST_NCI_DEVICE_MGNT_GATE, ST_NCI_DEVICE_MGNT_PIPE,
+					ST_NCI_HOST_CONTROLLER_ID},
 
 	/* Secure element pipes are created by secure element host */
-	{ST21NFCB_CONNECTIVITY_GATE, NCI_HCI_DO_NOT_OPEN_PIPE,
-					ST21NFCB_HOST_CONTROLLER_ID},
-	{ST21NFCB_APDU_READER_GATE, NCI_HCI_DO_NOT_OPEN_PIPE,
-					ST21NFCB_HOST_CONTROLLER_ID},
+	{ST_NCI_CONNECTIVITY_GATE, NCI_HCI_DO_NOT_OPEN_PIPE,
+					ST_NCI_HOST_CONTROLLER_ID},
+	{ST_NCI_APDU_READER_GATE, NCI_HCI_DO_NOT_OPEN_PIPE,
+					ST_NCI_HOST_CONTROLLER_ID},
 };
 
-static u8 st21nfcb_se_get_bwi(struct nci_dev *ndev)
+static u8 st_nci_se_get_bwi(struct nci_dev *ndev)
 {
 	int i;
 	u8 td;
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 
 	/* Bits 8 to 5 of the first TB for T=1 encode BWI from zero to nine */
-	for (i = 1; i < ST21NFCB_ESE_MAX_LENGTH; i++) {
-		td = ST21NFCB_ATR_GET_Y_FROM_TD(info->se_info.atr[i]);
-		if (ST21NFCB_ATR_TA_PRESENT(td))
+	for (i = 1; i < ST_NCI_ESE_MAX_LENGTH; i++) {
+		td = ST_NCI_ATR_GET_Y_FROM_TD(info->se_info.atr[i]);
+		if (ST_NCI_ATR_TA_PRESENT(td))
 			i++;
-		if (ST21NFCB_ATR_TB_PRESENT(td)) {
+		if (ST_NCI_ATR_TB_PRESENT(td)) {
 			i++;
 			return info->se_info.atr[i] >> 4;
 		}
 	}
-	return ST21NFCB_ATR_DEFAULT_BWI;
+	return ST_NCI_ATR_DEFAULT_BWI;
 }
 
-static void st21nfcb_se_get_atr(struct nci_dev *ndev)
+static void st_nci_se_get_atr(struct nci_dev *ndev)
 {
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 	int r;
 	struct sk_buff *skb;
 
-	r = nci_hci_get_param(ndev, ST21NFCB_APDU_READER_GATE,
+	r = nci_hci_get_param(ndev, ST_NCI_APDU_READER_GATE,
 				NCI_HCI_APDU_PARAM_ATR, &skb);
 	if (r < 0)
 		return;
 
-	if (skb->len <= ST21NFCB_ESE_MAX_LENGTH) {
+	if (skb->len <= ST_NCI_ESE_MAX_LENGTH) {
 		memcpy(info->se_info.atr, skb->data, skb->len);
 
 		info->se_info.wt_timeout =
-			ST21NFCB_BWI_TO_TIMEOUT(st21nfcb_se_get_bwi(ndev));
+			ST_NCI_BWI_TO_TIMEOUT(st_nci_se_get_bwi(ndev));
 	}
 	kfree_skb(skb);
 }
 
-int st21nfcb_hci_load_session(struct nci_dev *ndev)
+int st_nci_hci_load_session(struct nci_dev *ndev)
 {
 	int i, j, r;
 	struct sk_buff *skb_pipe_list, *skb_pipe_info;
-	struct st21nfcb_pipe_info *dm_pipe_info;
-	u8 pipe_list[] = { ST21NFCB_DM_GETINFO_PIPE_LIST,
-			ST21NFCB_TERMINAL_HOST_ID};
-	u8 pipe_info[] = { ST21NFCB_DM_GETINFO_PIPE_INFO,
-			ST21NFCB_TERMINAL_HOST_ID, 0};
+	struct st_nci_pipe_info *dm_pipe_info;
+	u8 pipe_list[] = { ST_NCI_DM_GETINFO_PIPE_LIST,
+			ST_NCI_TERMINAL_HOST_ID};
+	u8 pipe_info[] = { ST_NCI_DM_GETINFO_PIPE_INFO,
+			ST_NCI_TERMINAL_HOST_ID, 0};
 
-	/* On ST21NFCB device pipes number are dynamics
+	/* On ST_NCI device pipes number are dynamics
 	 * If pipes are already created, hci_dev_up will fail.
 	 * Doing a clear all pipe is a bad idea because:
 	 * - It does useless EEPROM cycling
 	 * - It might cause issue for secure elements support
 	 * (such as removing connectivity or APDU reader pipe)
-	 * A better approach on ST21NFCB is to:
+	 * A better approach on ST_NCI is to:
 	 * - get a pipe list for each host.
-	 * (eg: ST21NFCB_HOST_CONTROLLER_ID for now).
+	 * (eg: ST_NCI_HOST_CONTROLLER_ID for now).
 	 * (TODO Later on UICC HOST and eSE HOST)
 	 * - get pipe information
-	 * - match retrieved pipe list in st21nfcb_gates
-	 * ST21NFCB_DEVICE_MGNT_GATE is a proprietary gate
-	 * with ST21NFCB_DEVICE_MGNT_PIPE.
+	 * - match retrieved pipe list in st_nci_gates
+	 * ST_NCI_DEVICE_MGNT_GATE is a proprietary gate
+	 * with ST_NCI_DEVICE_MGNT_PIPE.
 	 * Pipe can be closed and need to be open.
 	 */
-	r = nci_hci_connect_gate(ndev, ST21NFCB_HOST_CONTROLLER_ID,
-				ST21NFCB_DEVICE_MGNT_GATE,
-				ST21NFCB_DEVICE_MGNT_PIPE);
+	r = nci_hci_connect_gate(ndev, ST_NCI_HOST_CONTROLLER_ID,
+				ST_NCI_DEVICE_MGNT_GATE,
+				ST_NCI_DEVICE_MGNT_PIPE);
 	if (r < 0)
 		goto free_info;
 
 	/* Get pipe list */
-	r = nci_hci_send_cmd(ndev, ST21NFCB_DEVICE_MGNT_GATE,
-			ST21NFCB_DM_GETINFO, pipe_list, sizeof(pipe_list),
+	r = nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+			ST_NCI_DM_GETINFO, pipe_list, sizeof(pipe_list),
 			&skb_pipe_list);
 	if (r < 0)
 		goto free_info;
@@ -201,8 +201,8 @@ int st21nfcb_hci_load_session(struct nci_dev *ndev)
 	/* Complete the existing gate_pipe table */
 	for (i = 0; i < skb_pipe_list->len; i++) {
 		pipe_info[2] = skb_pipe_list->data[i];
-		r = nci_hci_send_cmd(ndev, ST21NFCB_DEVICE_MGNT_GATE,
-					ST21NFCB_DM_GETINFO, pipe_info,
+		r = nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+					ST_NCI_DM_GETINFO, pipe_info,
 					sizeof(pipe_info), &skb_pipe_info);
 
 		if (r)
@@ -217,81 +217,81 @@ int st21nfcb_hci_load_session(struct nci_dev *ndev)
 		 * - destination hid (1byte)
 		 * - destination gid (1byte)
 		 */
-		dm_pipe_info = (struct st21nfcb_pipe_info *)skb_pipe_info->data;
-		if (dm_pipe_info->dst_gate_id == ST21NFCB_APDU_READER_GATE &&
-		    dm_pipe_info->src_host_id != ST21NFCB_ESE_HOST_ID) {
+		dm_pipe_info = (struct st_nci_pipe_info *)skb_pipe_info->data;
+		if (dm_pipe_info->dst_gate_id == ST_NCI_APDU_READER_GATE &&
+		    dm_pipe_info->src_host_id != ST_NCI_ESE_HOST_ID) {
 			pr_err("Unexpected apdu_reader pipe on host %x\n",
 			       dm_pipe_info->src_host_id);
 			continue;
 		}
 
-		for (j = 0; (j < ARRAY_SIZE(st21nfcb_gates)) &&
-		     (st21nfcb_gates[j].gate != dm_pipe_info->dst_gate_id); j++)
+		for (j = 0; (j < ARRAY_SIZE(st_nci_gates)) &&
+		     (st_nci_gates[j].gate != dm_pipe_info->dst_gate_id); j++)
 			;
 
-		if (j < ARRAY_SIZE(st21nfcb_gates) &&
-		    st21nfcb_gates[j].gate == dm_pipe_info->dst_gate_id &&
-		    ST21NFCB_DM_IS_PIPE_OPEN(dm_pipe_info->pipe_state)) {
-			st21nfcb_gates[j].pipe = pipe_info[2];
+		if (j < ARRAY_SIZE(st_nci_gates) &&
+		    st_nci_gates[j].gate == dm_pipe_info->dst_gate_id &&
+		    ST_NCI_DM_IS_PIPE_OPEN(dm_pipe_info->pipe_state)) {
+			st_nci_gates[j].pipe = pipe_info[2];
 
-			ndev->hci_dev->gate2pipe[st21nfcb_gates[j].gate] =
-						st21nfcb_gates[j].pipe;
-			ndev->hci_dev->pipes[st21nfcb_gates[j].pipe].gate =
-						st21nfcb_gates[j].gate;
-			ndev->hci_dev->pipes[st21nfcb_gates[j].pipe].host =
+			ndev->hci_dev->gate2pipe[st_nci_gates[j].gate] =
+						st_nci_gates[j].pipe;
+			ndev->hci_dev->pipes[st_nci_gates[j].pipe].gate =
+						st_nci_gates[j].gate;
+			ndev->hci_dev->pipes[st_nci_gates[j].pipe].host =
 						dm_pipe_info->src_host_id;
 		}
 	}
 
-	memcpy(ndev->hci_dev->init_data.gates, st21nfcb_gates,
-	       sizeof(st21nfcb_gates));
+	memcpy(ndev->hci_dev->init_data.gates, st_nci_gates,
+	       sizeof(st_nci_gates));
 
 free_info:
 	kfree_skb(skb_pipe_info);
 	kfree_skb(skb_pipe_list);
 	return r;
 }
-EXPORT_SYMBOL_GPL(st21nfcb_hci_load_session);
+EXPORT_SYMBOL_GPL(st_nci_hci_load_session);
 
-static void st21nfcb_hci_admin_event_received(struct nci_dev *ndev,
+static void st_nci_hci_admin_event_received(struct nci_dev *ndev,
 					      u8 event, struct sk_buff *skb)
 {
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 
 	switch (event) {
-	case ST21NFCB_EVT_HOT_PLUG:
+	case ST_NCI_EVT_HOT_PLUG:
 		if (info->se_info.se_active) {
-			if (!ST21NFCB_EVT_HOT_PLUG_IS_INHIBITED(skb)) {
+			if (!ST_NCI_EVT_HOT_PLUG_IS_INHIBITED(skb)) {
 				del_timer_sync(&info->se_info.se_active_timer);
 				info->se_info.se_active = false;
 				complete(&info->se_info.req_completion);
 			} else {
 				mod_timer(&info->se_info.se_active_timer,
 				      jiffies +
-				      msecs_to_jiffies(ST21NFCB_SE_TO_PIPES));
+				      msecs_to_jiffies(ST_NCI_SE_TO_PIPES));
 			}
 		}
 	break;
 	}
 }
 
-static int st21nfcb_hci_apdu_reader_event_received(struct nci_dev *ndev,
+static int st_nci_hci_apdu_reader_event_received(struct nci_dev *ndev,
 						   u8 event,
 						   struct sk_buff *skb)
 {
 	int r = 0;
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 
 	pr_debug("apdu reader gate event: %x\n", event);
 
 	switch (event) {
-	case ST21NFCB_EVT_TRANSMIT_DATA:
+	case ST_NCI_EVT_TRANSMIT_DATA:
 		del_timer_sync(&info->se_info.bwi_timer);
 		info->se_info.bwi_active = false;
 		info->se_info.cb(info->se_info.cb_context,
 				 skb->data, skb->len, 0);
 	break;
-	case ST21NFCB_EVT_WTX_REQUEST:
+	case ST_NCI_EVT_WTX_REQUEST:
 		mod_timer(&info->se_info.bwi_timer, jiffies +
 			  msecs_to_jiffies(info->se_info.wt_timeout));
 	break;
@@ -306,7 +306,7 @@ static int st21nfcb_hci_apdu_reader_event_received(struct nci_dev *ndev,
  * <= 0: driver handled the event, skb consumed
  *    1: driver does not handle the event, please do standard processing
  */
-static int st21nfcb_hci_connectivity_event_received(struct nci_dev *ndev,
+static int st_nci_hci_connectivity_event_received(struct nci_dev *ndev,
 						u8 host, u8 event,
 						struct sk_buff *skb)
 {
@@ -317,10 +317,10 @@ static int st21nfcb_hci_connectivity_event_received(struct nci_dev *ndev,
 	pr_debug("connectivity gate event: %x\n", event);
 
 	switch (event) {
-	case ST21NFCB_EVT_CONNECTIVITY:
+	case ST_NCI_EVT_CONNECTIVITY:
 
 	break;
-	case ST21NFCB_EVT_TRANSACTION:
+	case ST_NCI_EVT_TRANSACTION:
 		/* According to specification etsi 102 622
 		 * 11.2.2.4 EVT_TRANSACTION Table 52
 		 * Description  Tag     Length
@@ -355,7 +355,7 @@ static int st21nfcb_hci_connectivity_event_received(struct nci_dev *ndev,
 	return r;
 }
 
-void st21nfcb_hci_event_received(struct nci_dev *ndev, u8 pipe,
+void st_nci_hci_event_received(struct nci_dev *ndev, u8 pipe,
 				 u8 event, struct sk_buff *skb)
 {
 	u8 gate = ndev->hci_dev->pipes[pipe].gate;
@@ -363,32 +363,32 @@ void st21nfcb_hci_event_received(struct nci_dev *ndev, u8 pipe,
 
 	switch (gate) {
 	case NCI_HCI_ADMIN_GATE:
-		st21nfcb_hci_admin_event_received(ndev, event, skb);
+		st_nci_hci_admin_event_received(ndev, event, skb);
 	break;
-	case ST21NFCB_APDU_READER_GATE:
-		st21nfcb_hci_apdu_reader_event_received(ndev, event, skb);
+	case ST_NCI_APDU_READER_GATE:
+		st_nci_hci_apdu_reader_event_received(ndev, event, skb);
 	break;
-	case ST21NFCB_CONNECTIVITY_GATE:
-		st21nfcb_hci_connectivity_event_received(ndev, host, event,
+	case ST_NCI_CONNECTIVITY_GATE:
+		st_nci_hci_connectivity_event_received(ndev, host, event,
 							 skb);
 	break;
 	}
 }
-EXPORT_SYMBOL_GPL(st21nfcb_hci_event_received);
+EXPORT_SYMBOL_GPL(st_nci_hci_event_received);
 
 
-void st21nfcb_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
+void st_nci_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
 			       struct sk_buff *skb)
 {
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 	u8 gate = ndev->hci_dev->pipes[pipe].gate;
 
 	pr_debug("cmd: %x\n", cmd);
 
 	switch (cmd) {
 	case NCI_HCI_ANY_OPEN_PIPE:
-		if (gate != ST21NFCB_APDU_READER_GATE &&
-		    ndev->hci_dev->pipes[pipe].host != ST21NFCB_UICC_HOST_ID)
+		if (gate != ST_NCI_APDU_READER_GATE &&
+		    ndev->hci_dev->pipes[pipe].host != ST_NCI_UICC_HOST_ID)
 			ndev->hci_dev->count_pipes++;
 
 		if (ndev->hci_dev->count_pipes ==
@@ -401,28 +401,28 @@ void st21nfcb_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
 	break;
 	}
 }
-EXPORT_SYMBOL_GPL(st21nfcb_hci_cmd_received);
+EXPORT_SYMBOL_GPL(st_nci_hci_cmd_received);
 
 /*
- * Remarks: On some early st21nfcb firmware, nci_nfcee_mode_set(0)
+ * Remarks: On some early st_nci firmware, nci_nfcee_mode_set(0)
  * is rejected
  */
-static int st21nfcb_nci_control_se(struct nci_dev *ndev, u8 se_idx,
+static int st_nci_control_se(struct nci_dev *ndev, u8 se_idx,
 				   u8 state)
 {
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 	int r;
 	struct sk_buff *sk_host_list;
 	u8 host_id;
 
 	switch (se_idx) {
-	case ST21NFCB_UICC_HOST_ID:
+	case ST_NCI_UICC_HOST_ID:
 		ndev->hci_dev->count_pipes = 0;
-		ndev->hci_dev->expected_pipes = ST21NFCB_SE_COUNT_PIPE_UICC;
+		ndev->hci_dev->expected_pipes = ST_NCI_SE_COUNT_PIPE_UICC;
 		break;
-	case ST21NFCB_ESE_HOST_ID:
+	case ST_NCI_ESE_HOST_ID:
 		ndev->hci_dev->count_pipes = 0;
-		ndev->hci_dev->expected_pipes = ST21NFCB_SE_COUNT_PIPE_EMBEDDED;
+		ndev->hci_dev->expected_pipes = ST_NCI_SE_COUNT_PIPE_EMBEDDED;
 		break;
 	default:
 		return -EINVAL;
@@ -438,7 +438,7 @@ static int st21nfcb_nci_control_se(struct nci_dev *ndev, u8 se_idx,
 		return r;
 
 	mod_timer(&info->se_info.se_active_timer, jiffies +
-		msecs_to_jiffies(ST21NFCB_SE_TO_HOT_PLUG));
+		msecs_to_jiffies(ST_NCI_SE_TO_HOT_PLUG));
 	info->se_info.se_active = true;
 
 	/* Ignore return value and check in any case the host_list */
@@ -458,49 +458,49 @@ static int st21nfcb_nci_control_se(struct nci_dev *ndev, u8 se_idx,
 
 	host_id = sk_host_list->data[sk_host_list->len - 1];
 	kfree_skb(sk_host_list);
-	if (state == ST21NFCB_SE_MODE_ON && host_id == se_idx)
+	if (state == ST_NCI_SE_MODE_ON && host_id == se_idx)
 		return se_idx;
-	else if (state == ST21NFCB_SE_MODE_OFF && host_id != se_idx)
+	else if (state == ST_NCI_SE_MODE_OFF && host_id != se_idx)
 		return se_idx;
 
 	return -1;
 }
 
-int st21nfcb_nci_disable_se(struct nci_dev *ndev, u32 se_idx)
+int st_nci_disable_se(struct nci_dev *ndev, u32 se_idx)
 {
 	int r;
 
-	pr_debug("st21nfcb_nci_disable_se\n");
+	pr_debug("st_nci_disable_se\n");
 
 	if (se_idx == NFC_SE_EMBEDDED) {
-		r = nci_hci_send_event(ndev, ST21NFCB_APDU_READER_GATE,
-				ST21NFCB_EVT_SE_END_OF_APDU_TRANSFER, NULL, 0);
+		r = nci_hci_send_event(ndev, ST_NCI_APDU_READER_GATE,
+				ST_NCI_EVT_SE_END_OF_APDU_TRANSFER, NULL, 0);
 		if (r < 0)
 			return r;
 	}
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(st21nfcb_nci_disable_se);
+EXPORT_SYMBOL_GPL(st_nci_disable_se);
 
-int st21nfcb_nci_enable_se(struct nci_dev *ndev, u32 se_idx)
+int st_nci_enable_se(struct nci_dev *ndev, u32 se_idx)
 {
 	int r;
 
-	pr_debug("st21nfcb_nci_enable_se\n");
+	pr_debug("st_nci_enable_se\n");
 
-	if (se_idx == ST21NFCB_HCI_HOST_ID_ESE) {
-		r = nci_hci_send_event(ndev, ST21NFCB_APDU_READER_GATE,
-				ST21NFCB_EVT_SE_SOFT_RESET, NULL, 0);
+	if (se_idx == ST_NCI_HCI_HOST_ID_ESE) {
+		r = nci_hci_send_event(ndev, ST_NCI_APDU_READER_GATE,
+				ST_NCI_EVT_SE_SOFT_RESET, NULL, 0);
 		if (r < 0)
 			return r;
 	}
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(st21nfcb_nci_enable_se);
+EXPORT_SYMBOL_GPL(st_nci_enable_se);
 
-static int st21nfcb_hci_network_init(struct nci_dev *ndev)
+static int st_nci_hci_network_init(struct nci_dev *ndev)
 {
 	struct core_conn_create_dest_spec_params *dest_params;
 	struct dest_spec_params spec_params;
@@ -519,7 +519,8 @@ static int st21nfcb_hci_network_init(struct nci_dev *ndev)
 	dest_params->length = sizeof(struct dest_spec_params);
 	spec_params.id = ndev->hci_dev->nfcee_id;
 	spec_params.protocol = NCI_NFCEE_INTERFACE_HCI_ACCESS;
-	memcpy(dest_params->value, &spec_params, sizeof(struct dest_spec_params));
+	memcpy(dest_params->value, &spec_params,
+	       sizeof(struct dest_spec_params));
 	r = nci_core_conn_create(ndev, NCI_DESTINATION_NFCEE, 1,
 				 sizeof(struct core_conn_create_dest_spec_params) +
 				 sizeof(struct dest_spec_params),
@@ -531,15 +532,15 @@ static int st21nfcb_hci_network_init(struct nci_dev *ndev)
 	if (!conn_info)
 		goto free_dest_params;
 
-	memcpy(ndev->hci_dev->init_data.gates, st21nfcb_gates,
-	       sizeof(st21nfcb_gates));
+	memcpy(ndev->hci_dev->init_data.gates, st_nci_gates,
+	       sizeof(st_nci_gates));
 
 	/*
 	 * Session id must include the driver name + i2c bus addr
 	 * persistent info to discriminate 2 identical chips
 	 */
-	dev_num = find_first_zero_bit(dev_mask, ST21NFCB_NUM_DEVICES);
-	if (dev_num >= ST21NFCB_NUM_DEVICES) {
+	dev_num = find_first_zero_bit(dev_mask, ST_NCI_NUM_DEVICES);
+	if (dev_num >= ST_NCI_NUM_DEVICES) {
 		r = -ENODEV;
 		goto free_dest_params;
 	}
@@ -564,72 +565,72 @@ exit:
 	return r;
 }
 
-int st21nfcb_nci_discover_se(struct nci_dev *ndev)
+int st_nci_discover_se(struct nci_dev *ndev)
 {
 	u8 param[2];
 	int r;
 	int se_count = 0;
 
-	pr_debug("st21nfcb_nci_discover_se\n");
+	pr_debug("st_nci_discover_se\n");
 
-	r = st21nfcb_hci_network_init(ndev);
+	r = st_nci_hci_network_init(ndev);
 	if (r != 0)
 		return r;
 
-	param[0] = ST21NFCB_UICC_HOST_ID;
-	param[1] = ST21NFCB_HCI_HOST_ID_ESE;
+	param[0] = ST_NCI_UICC_HOST_ID;
+	param[1] = ST_NCI_HCI_HOST_ID_ESE;
 	r = nci_hci_set_param(ndev, NCI_HCI_ADMIN_GATE,
 				NCI_HCI_ADMIN_PARAM_WHITELIST,
 				param, sizeof(param));
 	if (r != NCI_HCI_ANY_OK)
 		return r;
 
-	r = st21nfcb_nci_control_se(ndev, ST21NFCB_UICC_HOST_ID,
-				ST21NFCB_SE_MODE_ON);
-	if (r == ST21NFCB_UICC_HOST_ID) {
-		nfc_add_se(ndev->nfc_dev, ST21NFCB_UICC_HOST_ID, NFC_SE_UICC);
+	r = st_nci_control_se(ndev, ST_NCI_UICC_HOST_ID,
+				ST_NCI_SE_MODE_ON);
+	if (r == ST_NCI_UICC_HOST_ID) {
+		nfc_add_se(ndev->nfc_dev, ST_NCI_UICC_HOST_ID, NFC_SE_UICC);
 		se_count++;
 	}
 
 	/* Try to enable eSE in order to check availability */
-	r = st21nfcb_nci_control_se(ndev, ST21NFCB_HCI_HOST_ID_ESE,
-				ST21NFCB_SE_MODE_ON);
-	if (r == ST21NFCB_HCI_HOST_ID_ESE) {
-		nfc_add_se(ndev->nfc_dev, ST21NFCB_HCI_HOST_ID_ESE,
+	r = st_nci_control_se(ndev, ST_NCI_HCI_HOST_ID_ESE,
+				ST_NCI_SE_MODE_ON);
+	if (r == ST_NCI_HCI_HOST_ID_ESE) {
+		nfc_add_se(ndev->nfc_dev, ST_NCI_HCI_HOST_ID_ESE,
 			   NFC_SE_EMBEDDED);
 		se_count++;
-		st21nfcb_se_get_atr(ndev);
+		st_nci_se_get_atr(ndev);
 	}
 
 	return !se_count;
 }
-EXPORT_SYMBOL_GPL(st21nfcb_nci_discover_se);
+EXPORT_SYMBOL_GPL(st_nci_discover_se);
 
-int st21nfcb_nci_se_io(struct nci_dev *ndev, u32 se_idx,
+int st_nci_se_io(struct nci_dev *ndev, u32 se_idx,
 		       u8 *apdu, size_t apdu_length,
 		       se_io_cb_t cb, void *cb_context)
 {
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 
 	pr_debug("\n");
 
 	switch (se_idx) {
-	case ST21NFCB_HCI_HOST_ID_ESE:
+	case ST_NCI_HCI_HOST_ID_ESE:
 		info->se_info.cb = cb;
 		info->se_info.cb_context = cb_context;
 		mod_timer(&info->se_info.bwi_timer, jiffies +
 			  msecs_to_jiffies(info->se_info.wt_timeout));
 		info->se_info.bwi_active = true;
-		return nci_hci_send_event(ndev, ST21NFCB_APDU_READER_GATE,
-					ST21NFCB_EVT_TRANSMIT_DATA, apdu,
+		return nci_hci_send_event(ndev, ST_NCI_APDU_READER_GATE,
+					ST_NCI_EVT_TRANSMIT_DATA, apdu,
 					apdu_length);
 	default:
 		return -ENODEV;
 	}
 }
-EXPORT_SYMBOL(st21nfcb_nci_se_io);
+EXPORT_SYMBOL(st_nci_se_io);
 
-static void st21nfcb_se_wt_timeout(unsigned long data)
+static void st_nci_se_wt_timeout(unsigned long data)
 {
 	/*
 	 * No answer from the secure element
@@ -642,7 +643,7 @@ static void st21nfcb_se_wt_timeout(unsigned long data)
 	 */
 	/* hardware reset managed through VCC_UICC_OUT power supply */
 	u8 param = 0x01;
-	struct st21nfcb_nci_info *info = (struct st21nfcb_nci_info *) data;
+	struct st_nci_info *info = (struct st_nci_info *) data;
 
 	pr_debug("\n");
 
@@ -650,19 +651,19 @@ static void st21nfcb_se_wt_timeout(unsigned long data)
 
 	if (!info->se_info.xch_error) {
 		info->se_info.xch_error = true;
-		nci_hci_send_event(info->ndlc->ndev, ST21NFCB_APDU_READER_GATE,
-				ST21NFCB_EVT_SE_SOFT_RESET, NULL, 0);
+		nci_hci_send_event(info->ndlc->ndev, ST_NCI_APDU_READER_GATE,
+				ST_NCI_EVT_SE_SOFT_RESET, NULL, 0);
 	} else {
 		info->se_info.xch_error = false;
-		nci_hci_send_event(info->ndlc->ndev, ST21NFCB_DEVICE_MGNT_GATE,
-				ST21NFCB_EVT_SE_HARD_RESET, &param, 1);
+		nci_hci_send_event(info->ndlc->ndev, ST_NCI_DEVICE_MGNT_GATE,
+				ST_NCI_EVT_SE_HARD_RESET, &param, 1);
 	}
 	info->se_info.cb(info->se_info.cb_context, NULL, 0, -ETIME);
 }
 
-static void st21nfcb_se_activation_timeout(unsigned long data)
+static void st_nci_se_activation_timeout(unsigned long data)
 {
-	struct st21nfcb_nci_info *info = (struct st21nfcb_nci_info *) data;
+	struct st_nci_info *info = (struct st_nci_info *) data;
 
 	pr_debug("\n");
 
@@ -671,35 +672,35 @@ static void st21nfcb_se_activation_timeout(unsigned long data)
 	complete(&info->se_info.req_completion);
 }
 
-int st21nfcb_se_init(struct nci_dev *ndev)
+int st_nci_se_init(struct nci_dev *ndev)
 {
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 
 	init_completion(&info->se_info.req_completion);
 	/* initialize timers */
 	init_timer(&info->se_info.bwi_timer);
 	info->se_info.bwi_timer.data = (unsigned long)info;
-	info->se_info.bwi_timer.function = st21nfcb_se_wt_timeout;
+	info->se_info.bwi_timer.function = st_nci_se_wt_timeout;
 	info->se_info.bwi_active = false;
 
 	init_timer(&info->se_info.se_active_timer);
 	info->se_info.se_active_timer.data = (unsigned long)info;
 	info->se_info.se_active_timer.function =
-			st21nfcb_se_activation_timeout;
+			st_nci_se_activation_timeout;
 	info->se_info.se_active = false;
 
 	info->se_info.xch_error = false;
 
 	info->se_info.wt_timeout =
-		ST21NFCB_BWI_TO_TIMEOUT(ST21NFCB_ATR_DEFAULT_BWI);
+		ST_NCI_BWI_TO_TIMEOUT(ST_NCI_ATR_DEFAULT_BWI);
 
 	return 0;
 }
-EXPORT_SYMBOL(st21nfcb_se_init);
+EXPORT_SYMBOL(st_nci_se_init);
 
-void st21nfcb_se_deinit(struct nci_dev *ndev)
+void st_nci_se_deinit(struct nci_dev *ndev)
 {
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 
 	if (info->se_info.bwi_active)
 		del_timer_sync(&info->se_info.bwi_timer);
@@ -709,5 +710,5 @@ void st21nfcb_se_deinit(struct nci_dev *ndev)
 	info->se_info.se_active = false;
 	info->se_info.bwi_active = false;
 }
-EXPORT_SYMBOL(st21nfcb_se_deinit);
+EXPORT_SYMBOL(st_nci_se_deinit);
 
diff --git a/drivers/nfc/st-nci/st-nci_se.h b/drivers/nfc/st-nci/st-nci_se.h
new file mode 100644
index 000000000000..ea66e879d67f
--- /dev/null
+++ b/drivers/nfc/st-nci/st-nci_se.h
@@ -0,0 +1,61 @@
+/*
+ * Secure Element Driver for STMicroelectronics NFC NCI Chip
+ *
+ * Copyright (C) 2014-2015 STMicroelectronics SAS. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __LOCAL_ST_NCI_SE_H_
+#define __LOCAL_ST_NCI_SE_H_
+
+/*
+ * ref ISO7816-3 chap 8.1. the initial character TS is followed by a
+ * sequence of at most 32 characters.
+ */
+#define ST_NCI_ESE_MAX_LENGTH	33
+#define ST_NCI_HCI_HOST_ID_ESE	0xc0
+
+struct st_nci_se_info {
+	u8 atr[ST_NCI_ESE_MAX_LENGTH];
+	struct completion req_completion;
+
+	struct timer_list bwi_timer;
+	int wt_timeout; /* in msecs */
+	bool bwi_active;
+
+	struct timer_list se_active_timer;
+	bool se_active;
+
+	bool xch_error;
+
+	se_io_cb_t cb;
+	void *cb_context;
+};
+
+int st_nci_se_init(struct nci_dev *ndev);
+void st_nci_se_deinit(struct nci_dev *ndev);
+
+int st_nci_discover_se(struct nci_dev *ndev);
+int st_nci_enable_se(struct nci_dev *ndev, u32 se_idx);
+int st_nci_disable_se(struct nci_dev *ndev, u32 se_idx);
+int st_nci_se_io(struct nci_dev *ndev, u32 se_idx,
+		u8 *apdu, size_t apdu_length,
+		se_io_cb_t cb, void *cb_context);
+int st_nci_hci_load_session(struct nci_dev *ndev);
+void st_nci_hci_event_received(struct nci_dev *ndev, u8 pipe,
+			u8 event, struct sk_buff *skb);
+void st_nci_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
+			struct sk_buff *skb);
+
+
+#endif /* __LOCAL_ST_NCI_SE_H_ */
diff --git a/drivers/nfc/st21nfcb/Kconfig b/drivers/nfc/st21nfcb/Kconfig
deleted file mode 100644
index e0322dd03a70..000000000000
--- a/drivers/nfc/st21nfcb/Kconfig
+++ /dev/null
@@ -1,22 +0,0 @@
-config NFC_ST21NFCB
-	tristate "STMicroelectronics ST21NFCB NFC driver"
-	depends on NFC_NCI
-	default n
-	---help---
-	  STMicroelectronics ST21NFCB core driver. It implements the chipset
-	  NCI logic and hooks into the NFC kernel APIs. Physical layers will
-	  register against it.
-
-	  To compile this driver as a module, choose m here. The module will
-	  be called st21nfcb.
-	  Say N if unsure.
-
-config NFC_ST21NFCB_I2C
-	tristate "NFC ST21NFCB i2c support"
-	depends on NFC_ST21NFCB && I2C
-	---help---
-	  This module adds support for the STMicroelectronics st21nfcb i2c interface.
-	  Select this if your platform is using the i2c bus.
-
-	  If you choose to build a module, it'll be called st21nfcb_i2c.
-	  Say N if unsure.
diff --git a/drivers/nfc/st21nfcb/Makefile b/drivers/nfc/st21nfcb/Makefile
deleted file mode 100644
index ce659a9e5a1a..000000000000
--- a/drivers/nfc/st21nfcb/Makefile
+++ /dev/null
@@ -1,9 +0,0 @@
-#
-# Makefile for ST21NFCB NCI based NFC driver
-#
-
-st21nfcb_nci-objs = ndlc.o st21nfcb.o st21nfcb_se.o
-obj-$(CONFIG_NFC_ST21NFCB)     += st21nfcb_nci.o
-
-st21nfcb_i2c-objs = i2c.o
-obj-$(CONFIG_NFC_ST21NFCB_I2C) += st21nfcb_i2c.o
diff --git a/drivers/nfc/st21nfcb/st21nfcb.c b/drivers/nfc/st21nfcb/st21nfcb.c
deleted file mode 100644
index ca9871ab3fb3..000000000000
--- a/drivers/nfc/st21nfcb/st21nfcb.c
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * NCI based Driver for STMicroelectronics NFC Chip
- *
- * Copyright (C) 2014  STMicroelectronics SAS. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <linux/module.h>
-#include <linux/nfc.h>
-#include <net/nfc/nci.h>
-#include <net/nfc/nci_core.h>
-
-#include "st21nfcb.h"
-#include "st21nfcb_se.h"
-
-#define DRIVER_DESC "NCI NFC driver for ST21NFCB"
-
-#define ST21NFCB_NCI1_X_PROPRIETARY_ISO15693 0x83
-
-static int st21nfcb_nci_open(struct nci_dev *ndev)
-{
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
-	int r;
-
-	if (test_and_set_bit(ST21NFCB_NCI_RUNNING, &info->flags))
-		return 0;
-
-	r = ndlc_open(info->ndlc);
-	if (r)
-		clear_bit(ST21NFCB_NCI_RUNNING, &info->flags);
-
-	return r;
-}
-
-static int st21nfcb_nci_close(struct nci_dev *ndev)
-{
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
-
-	if (!test_and_clear_bit(ST21NFCB_NCI_RUNNING, &info->flags))
-		return 0;
-
-	ndlc_close(info->ndlc);
-
-	return 0;
-}
-
-static int st21nfcb_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
-{
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
-
-	skb->dev = (void *)ndev;
-
-	if (!test_bit(ST21NFCB_NCI_RUNNING, &info->flags))
-		return -EBUSY;
-
-	return ndlc_send(info->ndlc, skb);
-}
-
-static __u32 st21nfcb_nci_get_rfprotocol(struct nci_dev *ndev,
-					 __u8 rf_protocol)
-{
-	return rf_protocol == ST21NFCB_NCI1_X_PROPRIETARY_ISO15693 ?
-		NFC_PROTO_ISO15693_MASK : 0;
-}
-
-static struct nci_ops st21nfcb_nci_ops = {
-	.open = st21nfcb_nci_open,
-	.close = st21nfcb_nci_close,
-	.send = st21nfcb_nci_send,
-	.get_rfprotocol = st21nfcb_nci_get_rfprotocol,
-	.discover_se = st21nfcb_nci_discover_se,
-	.enable_se = st21nfcb_nci_enable_se,
-	.disable_se = st21nfcb_nci_disable_se,
-	.se_io = st21nfcb_nci_se_io,
-	.hci_load_session = st21nfcb_hci_load_session,
-	.hci_event_received = st21nfcb_hci_event_received,
-	.hci_cmd_received = st21nfcb_hci_cmd_received,
-};
-
-int st21nfcb_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
-		       int phy_tailroom)
-{
-	struct st21nfcb_nci_info *info;
-	int r;
-	u32 protocols;
-
-	info = devm_kzalloc(ndlc->dev,
-			sizeof(struct st21nfcb_nci_info), GFP_KERNEL);
-	if (!info)
-		return -ENOMEM;
-
-	protocols = NFC_PROTO_JEWEL_MASK
-		| NFC_PROTO_MIFARE_MASK
-		| NFC_PROTO_FELICA_MASK
-		| NFC_PROTO_ISO14443_MASK
-		| NFC_PROTO_ISO14443_B_MASK
-		| NFC_PROTO_ISO15693_MASK
-		| NFC_PROTO_NFC_DEP_MASK;
-
-	ndlc->ndev = nci_allocate_device(&st21nfcb_nci_ops, protocols,
-					phy_headroom, phy_tailroom);
-	if (!ndlc->ndev) {
-		pr_err("Cannot allocate nfc ndev\n");
-		return -ENOMEM;
-	}
-	info->ndlc = ndlc;
-
-	nci_set_drvdata(ndlc->ndev, info);
-
-	r = nci_register_device(ndlc->ndev);
-	if (r) {
-		pr_err("Cannot register nfc device to nci core\n");
-		nci_free_device(ndlc->ndev);
-		return r;
-	}
-
-	return st21nfcb_se_init(ndlc->ndev);
-}
-EXPORT_SYMBOL_GPL(st21nfcb_nci_probe);
-
-void st21nfcb_nci_remove(struct nci_dev *ndev)
-{
-	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
-
-	nci_unregister_device(ndev);
-	nci_free_device(ndev);
-	kfree(info);
-}
-EXPORT_SYMBOL_GPL(st21nfcb_nci_remove);
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/nfc/st21nfcb/st21nfcb_se.h b/drivers/nfc/st21nfcb/st21nfcb_se.h
deleted file mode 100644
index 52a323872bea..000000000000
--- a/drivers/nfc/st21nfcb/st21nfcb_se.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * NCI based Driver for STMicroelectronics NFC Chip
- *
- * Copyright (C) 2014  STMicroelectronics SAS. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see <http://www.gnu.org/licenses/>.
- */
-#ifndef __LOCAL_ST21NFCB_SE_H_
-#define __LOCAL_ST21NFCB_SE_H_
-
-/*
- * ref ISO7816-3 chap 8.1. the initial character TS is followed by a
- * sequence of at most 32 characters.
- */
-#define ST21NFCB_ESE_MAX_LENGTH		33
-#define ST21NFCB_HCI_HOST_ID_ESE	0xc0
-
-struct st21nfcb_se_info {
-	u8 atr[ST21NFCB_ESE_MAX_LENGTH];
-	struct completion req_completion;
-
-	struct timer_list bwi_timer;
-	int wt_timeout; /* in msecs */
-	bool bwi_active;
-
-	struct timer_list se_active_timer;
-	bool se_active;
-
-	bool xch_error;
-
-	se_io_cb_t cb;
-	void *cb_context;
-};
-
-int st21nfcb_se_init(struct nci_dev *ndev);
-void st21nfcb_se_deinit(struct nci_dev *ndev);
-
-int st21nfcb_nci_discover_se(struct nci_dev *ndev);
-int st21nfcb_nci_enable_se(struct nci_dev *ndev, u32 se_idx);
-int st21nfcb_nci_disable_se(struct nci_dev *ndev, u32 se_idx);
-int st21nfcb_nci_se_io(struct nci_dev *ndev, u32 se_idx,
-					u8 *apdu, size_t apdu_length,
-					se_io_cb_t cb, void *cb_context);
-int st21nfcb_hci_load_session(struct nci_dev *ndev);
-void st21nfcb_hci_event_received(struct nci_dev *ndev, u8 pipe,
-				 u8 event, struct sk_buff *skb);
-void st21nfcb_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
-			       struct sk_buff *skb);
-
-
-#endif /* __LOCAL_ST21NFCB_NCI_H_ */
diff --git a/drivers/nfc/trf7970a.c b/drivers/nfc/trf7970a.c
index aa6a333b2ead..85b4d86772d8 100644
--- a/drivers/nfc/trf7970a.c
+++ b/drivers/nfc/trf7970a.c
@@ -149,6 +149,7 @@
  */
 #define TRF7970A_QUIRK_IRQ_STATUS_READ		BIT(0)
 #define TRF7970A_QUIRK_EN2_MUST_STAY_LOW	BIT(1)
+#define TRF7970A_QUIRK_T5T_RMB_EXTRA_BYTE	BIT(2)
 
 /* Direct commands */
 #define TRF7970A_CMD_IDLE			0x00
@@ -446,6 +447,7 @@ struct trf7970a {
 	u8				md_rf_tech;
 	u8				tx_cmd;
 	bool				issue_eof;
+	bool				adjust_resp_len;
 	int				en2_gpio;
 	int				en_gpio;
 	struct mutex			lock;
@@ -626,6 +628,11 @@ static void trf7970a_send_upstream(struct trf7970a *trf)
 		trf->aborting = false;
 	}
 
+	if (trf->adjust_resp_len) {
+		skb_trim(trf->rx_skb, trf->rx_skb->len - 1);
+		trf->adjust_resp_len = false;
+	}
+
 	trf->cb(trf->ddev, trf->cb_arg, trf->rx_skb);
 
 	trf->rx_skb = NULL;
@@ -1429,10 +1436,15 @@ static int trf7970a_per_cmd_config(struct trf7970a *trf, struct sk_buff *skb)
 			trf->iso_ctrl = iso_ctrl;
 		}
 
-		if ((trf->framing == NFC_DIGITAL_FRAMING_ISO15693_T5T) &&
-				trf7970a_is_iso15693_write_or_lock(req[1]) &&
-				(req[0] & ISO15693_REQ_FLAG_OPTION))
-			trf->issue_eof = true;
+		if (trf->framing == NFC_DIGITAL_FRAMING_ISO15693_T5T) {
+			if (trf7970a_is_iso15693_write_or_lock(req[1]) &&
+					(req[0] & ISO15693_REQ_FLAG_OPTION))
+				trf->issue_eof = true;
+			else if ((trf->quirks &
+					TRF7970A_QUIRK_T5T_RMB_EXTRA_BYTE) &&
+				 (req[1] == ISO15693_CMD_READ_MULTIPLE_BLOCK))
+				trf->adjust_resp_len = true;
+		}
 	}
 
 	return 0;
@@ -1992,6 +2004,9 @@ static int trf7970a_probe(struct spi_device *spi)
 		return ret;
 	}
 
+	if (of_property_read_bool(np, "t5t-rmb-extra-byte-quirk"))
+		trf->quirks |= TRF7970A_QUIRK_T5T_RMB_EXTRA_BYTE;
+
 	if (of_property_read_bool(np, "irq-status-read-quirk"))
 		trf->quirks |= TRF7970A_QUIRK_IRQ_STATUS_READ;
 
diff --git a/drivers/ntb/Kconfig b/drivers/ntb/Kconfig
index f69df793dbe2..95944e52fa36 100644
--- a/drivers/ntb/Kconfig
+++ b/drivers/ntb/Kconfig
@@ -1,13 +1,28 @@
-config NTB
-       tristate "Intel Non-Transparent Bridge support"
-       depends on PCI
-       depends on X86
-       help
-        The PCI-E Non-transparent bridge hardware is a point-to-point PCI-E bus
-        connecting 2 systems.  When configured, writes to the device's PCI
-        mapped memory will be mirrored to a buffer on the remote system.  The
-        ntb Linux driver uses this point-to-point communication as a method to
-        transfer data from one system to the other.
-
-        If unsure, say N.
+menuconfig NTB
+	tristate "Non-Transparent Bridge support"
+	depends on PCI
+	help
+	 The PCI-E Non-transparent bridge hardware is a point-to-point PCI-E bus
+	 connecting 2 systems.  When configured, writes to the device's PCI
+	 mapped memory will be mirrored to a buffer on the remote system.  The
+	 ntb Linux driver uses this point-to-point communication as a method to
+	 transfer data from one system to the other.
 
+	 If unsure, say N.
+
+if NTB
+
+source "drivers/ntb/hw/Kconfig"
+
+source "drivers/ntb/test/Kconfig"
+
+config NTB_TRANSPORT
+	tristate "NTB Transport Client"
+	help
+	 This is a transport driver that enables connected systems to exchange
+	 messages over the ntb hardware.  The transport exposes a queue pair api
+	 to client drivers.
+
+	 If unsure, say N.
+
+endif # NTB
diff --git a/drivers/ntb/Makefile b/drivers/ntb/Makefile
index 15cb59fd354e..1921dec1949d 100644
--- a/drivers/ntb/Makefile
+++ b/drivers/ntb/Makefile
@@ -1,3 +1,2 @@
-obj-$(CONFIG_NTB) += ntb.o
-
-ntb-objs := ntb_hw.o ntb_transport.o
+obj-$(CONFIG_NTB) += ntb.o hw/ test/
+obj-$(CONFIG_NTB_TRANSPORT) += ntb_transport.o
diff --git a/drivers/ntb/hw/Kconfig b/drivers/ntb/hw/Kconfig
new file mode 100644
index 000000000000..4d5535c4cddf
--- /dev/null
+++ b/drivers/ntb/hw/Kconfig
@@ -0,0 +1 @@
+source "drivers/ntb/hw/intel/Kconfig"
diff --git a/drivers/ntb/hw/Makefile b/drivers/ntb/hw/Makefile
new file mode 100644
index 000000000000..175d7c92a569
--- /dev/null
+++ b/drivers/ntb/hw/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_NTB_INTEL)	+= intel/
diff --git a/drivers/ntb/hw/intel/Kconfig b/drivers/ntb/hw/intel/Kconfig
new file mode 100644
index 000000000000..91f995e33ac6
--- /dev/null
+++ b/drivers/ntb/hw/intel/Kconfig
@@ -0,0 +1,7 @@
+config NTB_INTEL
+	tristate "Intel Non-Transparent Bridge support"
+	depends on X86_64
+	help
+	 This driver supports Intel NTB on capable Xeon and Atom hardware.
+
+	 If unsure, say N.
diff --git a/drivers/ntb/hw/intel/Makefile b/drivers/ntb/hw/intel/Makefile
new file mode 100644
index 000000000000..1b434568d2ad
--- /dev/null
+++ b/drivers/ntb/hw/intel/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_NTB_INTEL) += ntb_hw_intel.o
diff --git a/drivers/ntb/hw/intel/ntb_hw_intel.c b/drivers/ntb/hw/intel/ntb_hw_intel.c
new file mode 100644
index 000000000000..87751cfd6f4f
--- /dev/null
+++ b/drivers/ntb/hw/intel/ntb_hw_intel.c
@@ -0,0 +1,2274 @@
+/*
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ *   redistributing this file, you may do so under either license.
+ *
+ *   GPL LICENSE SUMMARY
+ *
+ *   Copyright(c) 2012 Intel Corporation. All rights reserved.
+ *   Copyright (C) 2015 EMC 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.
+ *
+ *   BSD LICENSE
+ *
+ *   Copyright(c) 2012 Intel Corporation. All rights reserved.
+ *   Copyright (C) 2015 EMC Corporation. All Rights Reserved.
+ *
+ *   Redistribution and use in source and binary forms, with or without
+ *   modification, are permitted provided that the following conditions
+ *   are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copy
+ *       notice, this list of conditions and the following disclaimer in
+ *       the documentation and/or other materials provided with the
+ *       distribution.
+ *     * Neither the name of Intel Corporation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Intel PCIe NTB Linux driver
+ *
+ * Contact Information:
+ * Jon Mason <jon.mason@intel.com>
+ */
+
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/random.h>
+#include <linux/slab.h>
+#include <linux/ntb.h>
+
+#include "ntb_hw_intel.h"
+
+#define NTB_NAME	"ntb_hw_intel"
+#define NTB_DESC	"Intel(R) PCI-E Non-Transparent Bridge Driver"
+#define NTB_VER		"2.0"
+
+MODULE_DESCRIPTION(NTB_DESC);
+MODULE_VERSION(NTB_VER);
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Intel Corporation");
+
+#define bar0_off(base, bar) ((base) + ((bar) << 2))
+#define bar2_off(base, bar) bar0_off(base, (bar) - 2)
+
+static const struct intel_ntb_reg atom_reg;
+static const struct intel_ntb_alt_reg atom_pri_reg;
+static const struct intel_ntb_alt_reg atom_sec_reg;
+static const struct intel_ntb_alt_reg atom_b2b_reg;
+static const struct intel_ntb_xlat_reg atom_pri_xlat;
+static const struct intel_ntb_xlat_reg atom_sec_xlat;
+static const struct intel_ntb_reg xeon_reg;
+static const struct intel_ntb_alt_reg xeon_pri_reg;
+static const struct intel_ntb_alt_reg xeon_sec_reg;
+static const struct intel_ntb_alt_reg xeon_b2b_reg;
+static const struct intel_ntb_xlat_reg xeon_pri_xlat;
+static const struct intel_ntb_xlat_reg xeon_sec_xlat;
+static struct intel_b2b_addr xeon_b2b_usd_addr;
+static struct intel_b2b_addr xeon_b2b_dsd_addr;
+static const struct ntb_dev_ops intel_ntb_ops;
+
+static const struct file_operations intel_ntb_debugfs_info;
+static struct dentry *debugfs_dir;
+
+static int b2b_mw_idx = -1;
+module_param(b2b_mw_idx, int, 0644);
+MODULE_PARM_DESC(b2b_mw_idx, "Use this mw idx to access the peer ntb.  A "
+		 "value of zero or positive starts from first mw idx, and a "
+		 "negative value starts from last mw idx.  Both sides MUST "
+		 "set the same value here!");
+
+static unsigned int b2b_mw_share;
+module_param(b2b_mw_share, uint, 0644);
+MODULE_PARM_DESC(b2b_mw_share, "If the b2b mw is large enough, configure the "
+		 "ntb so that the peer ntb only occupies the first half of "
+		 "the mw, so the second half can still be used as a mw.  Both "
+		 "sides MUST set the same value here!");
+
+module_param_named(xeon_b2b_usd_bar2_addr64,
+		   xeon_b2b_usd_addr.bar2_addr64, ullong, 0644);
+MODULE_PARM_DESC(xeon_b2b_usd_bar2_addr64,
+		 "XEON B2B USD BAR 2 64-bit address");
+
+module_param_named(xeon_b2b_usd_bar4_addr64,
+		   xeon_b2b_usd_addr.bar4_addr64, ullong, 0644);
+MODULE_PARM_DESC(xeon_b2b_usd_bar2_addr64,
+		 "XEON B2B USD BAR 4 64-bit address");
+
+module_param_named(xeon_b2b_usd_bar4_addr32,
+		   xeon_b2b_usd_addr.bar4_addr32, ullong, 0644);
+MODULE_PARM_DESC(xeon_b2b_usd_bar2_addr64,
+		 "XEON B2B USD split-BAR 4 32-bit address");
+
+module_param_named(xeon_b2b_usd_bar5_addr32,
+		   xeon_b2b_usd_addr.bar5_addr32, ullong, 0644);
+MODULE_PARM_DESC(xeon_b2b_usd_bar2_addr64,
+		 "XEON B2B USD split-BAR 5 32-bit address");
+
+module_param_named(xeon_b2b_dsd_bar2_addr64,
+		   xeon_b2b_dsd_addr.bar2_addr64, ullong, 0644);
+MODULE_PARM_DESC(xeon_b2b_dsd_bar2_addr64,
+		 "XEON B2B DSD BAR 2 64-bit address");
+
+module_param_named(xeon_b2b_dsd_bar4_addr64,
+		   xeon_b2b_dsd_addr.bar4_addr64, ullong, 0644);
+MODULE_PARM_DESC(xeon_b2b_dsd_bar2_addr64,
+		 "XEON B2B DSD BAR 4 64-bit address");
+
+module_param_named(xeon_b2b_dsd_bar4_addr32,
+		   xeon_b2b_dsd_addr.bar4_addr32, ullong, 0644);
+MODULE_PARM_DESC(xeon_b2b_dsd_bar2_addr64,
+		 "XEON B2B DSD split-BAR 4 32-bit address");
+
+module_param_named(xeon_b2b_dsd_bar5_addr32,
+		   xeon_b2b_dsd_addr.bar5_addr32, ullong, 0644);
+MODULE_PARM_DESC(xeon_b2b_dsd_bar2_addr64,
+		 "XEON B2B DSD split-BAR 5 32-bit address");
+
+#ifndef ioread64
+#ifdef readq
+#define ioread64 readq
+#else
+#define ioread64 _ioread64
+static inline u64 _ioread64(void __iomem *mmio)
+{
+	u64 low, high;
+
+	low = ioread32(mmio);
+	high = ioread32(mmio + sizeof(u32));
+	return low | (high << 32);
+}
+#endif
+#endif
+
+#ifndef iowrite64
+#ifdef writeq
+#define iowrite64 writeq
+#else
+#define iowrite64 _iowrite64
+static inline void _iowrite64(u64 val, void __iomem *mmio)
+{
+	iowrite32(val, mmio);
+	iowrite32(val >> 32, mmio + sizeof(u32));
+}
+#endif
+#endif
+
+static inline int pdev_is_atom(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_BWD:
+		return 1;
+	}
+	return 0;
+}
+
+static inline int pdev_is_xeon(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PCI_DEVICE_ID_INTEL_NTB_SS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
+		return 1;
+	}
+	return 0;
+}
+
+static inline void ndev_reset_unsafe_flags(struct intel_ntb_dev *ndev)
+{
+	ndev->unsafe_flags = 0;
+	ndev->unsafe_flags_ignore = 0;
+
+	/* Only B2B has a workaround to avoid SDOORBELL */
+	if (ndev->hwerr_flags & NTB_HWERR_SDOORBELL_LOCKUP)
+		if (!ntb_topo_is_b2b(ndev->ntb.topo))
+			ndev->unsafe_flags |= NTB_UNSAFE_DB;
+
+	/* No low level workaround to avoid SB01BASE */
+	if (ndev->hwerr_flags & NTB_HWERR_SB01BASE_LOCKUP) {
+		ndev->unsafe_flags |= NTB_UNSAFE_DB;
+		ndev->unsafe_flags |= NTB_UNSAFE_SPAD;
+	}
+}
+
+static inline int ndev_is_unsafe(struct intel_ntb_dev *ndev,
+				 unsigned long flag)
+{
+	return !!(flag & ndev->unsafe_flags & ~ndev->unsafe_flags_ignore);
+}
+
+static inline int ndev_ignore_unsafe(struct intel_ntb_dev *ndev,
+				     unsigned long flag)
+{
+	flag &= ndev->unsafe_flags;
+	ndev->unsafe_flags_ignore |= flag;
+
+	return !!flag;
+}
+
+static int ndev_mw_to_bar(struct intel_ntb_dev *ndev, int idx)
+{
+	if (idx < 0 || idx > ndev->mw_count)
+		return -EINVAL;
+	return ndev->reg->mw_bar[idx];
+}
+
+static inline int ndev_db_addr(struct intel_ntb_dev *ndev,
+			       phys_addr_t *db_addr, resource_size_t *db_size,
+			       phys_addr_t reg_addr, unsigned long reg)
+{
+	if (ndev_is_unsafe(ndev, NTB_UNSAFE_DB))
+		pr_warn_once("%s: NTB unsafe doorbell access", __func__);
+
+	if (db_addr) {
+		*db_addr = reg_addr + reg;
+		dev_dbg(ndev_dev(ndev), "Peer db addr %llx\n", *db_addr);
+	}
+
+	if (db_size) {
+		*db_size = ndev->reg->db_size;
+		dev_dbg(ndev_dev(ndev), "Peer db size %llx\n", *db_size);
+	}
+
+	return 0;
+}
+
+static inline u64 ndev_db_read(struct intel_ntb_dev *ndev,
+			       void __iomem *mmio)
+{
+	if (ndev_is_unsafe(ndev, NTB_UNSAFE_DB))
+		pr_warn_once("%s: NTB unsafe doorbell access", __func__);
+
+	return ndev->reg->db_ioread(mmio);
+}
+
+static inline int ndev_db_write(struct intel_ntb_dev *ndev, u64 db_bits,
+				void __iomem *mmio)
+{
+	if (ndev_is_unsafe(ndev, NTB_UNSAFE_DB))
+		pr_warn_once("%s: NTB unsafe doorbell access", __func__);
+
+	if (db_bits & ~ndev->db_valid_mask)
+		return -EINVAL;
+
+	ndev->reg->db_iowrite(db_bits, mmio);
+
+	return 0;
+}
+
+static inline int ndev_db_set_mask(struct intel_ntb_dev *ndev, u64 db_bits,
+				   void __iomem *mmio)
+{
+	unsigned long irqflags;
+
+	if (ndev_is_unsafe(ndev, NTB_UNSAFE_DB))
+		pr_warn_once("%s: NTB unsafe doorbell access", __func__);
+
+	if (db_bits & ~ndev->db_valid_mask)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ndev->db_mask_lock, irqflags);
+	{
+		ndev->db_mask |= db_bits;
+		ndev->reg->db_iowrite(ndev->db_mask, mmio);
+	}
+	spin_unlock_irqrestore(&ndev->db_mask_lock, irqflags);
+
+	return 0;
+}
+
+static inline int ndev_db_clear_mask(struct intel_ntb_dev *ndev, u64 db_bits,
+				     void __iomem *mmio)
+{
+	unsigned long irqflags;
+
+	if (ndev_is_unsafe(ndev, NTB_UNSAFE_DB))
+		pr_warn_once("%s: NTB unsafe doorbell access", __func__);
+
+	if (db_bits & ~ndev->db_valid_mask)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ndev->db_mask_lock, irqflags);
+	{
+		ndev->db_mask &= ~db_bits;
+		ndev->reg->db_iowrite(ndev->db_mask, mmio);
+	}
+	spin_unlock_irqrestore(&ndev->db_mask_lock, irqflags);
+
+	return 0;
+}
+
+static inline int ndev_vec_mask(struct intel_ntb_dev *ndev, int db_vector)
+{
+	u64 shift, mask;
+
+	shift = ndev->db_vec_shift;
+	mask = BIT_ULL(shift) - 1;
+
+	return mask << (shift * db_vector);
+}
+
+static inline int ndev_spad_addr(struct intel_ntb_dev *ndev, int idx,
+				 phys_addr_t *spad_addr, phys_addr_t reg_addr,
+				 unsigned long reg)
+{
+	if (ndev_is_unsafe(ndev, NTB_UNSAFE_SPAD))
+		pr_warn_once("%s: NTB unsafe scratchpad access", __func__);
+
+	if (idx < 0 || idx >= ndev->spad_count)
+		return -EINVAL;
+
+	if (spad_addr) {
+		*spad_addr = reg_addr + reg + (idx << 2);
+		dev_dbg(ndev_dev(ndev), "Peer spad addr %llx\n", *spad_addr);
+	}
+
+	return 0;
+}
+
+static inline u32 ndev_spad_read(struct intel_ntb_dev *ndev, int idx,
+				 void __iomem *mmio)
+{
+	if (ndev_is_unsafe(ndev, NTB_UNSAFE_SPAD))
+		pr_warn_once("%s: NTB unsafe scratchpad access", __func__);
+
+	if (idx < 0 || idx >= ndev->spad_count)
+		return 0;
+
+	return ioread32(mmio + (idx << 2));
+}
+
+static inline int ndev_spad_write(struct intel_ntb_dev *ndev, int idx, u32 val,
+				  void __iomem *mmio)
+{
+	if (ndev_is_unsafe(ndev, NTB_UNSAFE_SPAD))
+		pr_warn_once("%s: NTB unsafe scratchpad access", __func__);
+
+	if (idx < 0 || idx >= ndev->spad_count)
+		return -EINVAL;
+
+	iowrite32(val, mmio + (idx << 2));
+
+	return 0;
+}
+
+static irqreturn_t ndev_interrupt(struct intel_ntb_dev *ndev, int vec)
+{
+	u64 vec_mask;
+
+	vec_mask = ndev_vec_mask(ndev, vec);
+
+	dev_dbg(ndev_dev(ndev), "vec %d vec_mask %llx\n", vec, vec_mask);
+
+	ndev->last_ts = jiffies;
+
+	if (vec_mask & ndev->db_link_mask) {
+		if (ndev->reg->poll_link(ndev))
+			ntb_link_event(&ndev->ntb);
+	}
+
+	if (vec_mask & ndev->db_valid_mask)
+		ntb_db_event(&ndev->ntb, vec);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ndev_vec_isr(int irq, void *dev)
+{
+	struct intel_ntb_vec *nvec = dev;
+
+	return ndev_interrupt(nvec->ndev, nvec->num);
+}
+
+static irqreturn_t ndev_irq_isr(int irq, void *dev)
+{
+	struct intel_ntb_dev *ndev = dev;
+
+	return ndev_interrupt(ndev, irq - ndev_pdev(ndev)->irq);
+}
+
+static int ndev_init_isr(struct intel_ntb_dev *ndev,
+			 int msix_min, int msix_max,
+			 int msix_shift, int total_shift)
+{
+	struct pci_dev *pdev;
+	int rc, i, msix_count, node;
+
+	pdev = ndev_pdev(ndev);
+
+	node = dev_to_node(&pdev->dev);
+
+	/* Mask all doorbell interrupts */
+	ndev->db_mask = ndev->db_valid_mask;
+	ndev->reg->db_iowrite(ndev->db_mask,
+			      ndev->self_mmio +
+			      ndev->self_reg->db_mask);
+
+	/* Try to set up msix irq */
+
+	ndev->vec = kzalloc_node(msix_max * sizeof(*ndev->vec),
+				 GFP_KERNEL, node);
+	if (!ndev->vec)
+		goto err_msix_vec_alloc;
+
+	ndev->msix = kzalloc_node(msix_max * sizeof(*ndev->msix),
+				  GFP_KERNEL, node);
+	if (!ndev->msix)
+		goto err_msix_alloc;
+
+	for (i = 0; i < msix_max; ++i)
+		ndev->msix[i].entry = i;
+
+	msix_count = pci_enable_msix_range(pdev, ndev->msix,
+					   msix_min, msix_max);
+	if (msix_count < 0)
+		goto err_msix_enable;
+
+	for (i = 0; i < msix_count; ++i) {
+		ndev->vec[i].ndev = ndev;
+		ndev->vec[i].num = i;
+		rc = request_irq(ndev->msix[i].vector, ndev_vec_isr, 0,
+				 "ndev_vec_isr", &ndev->vec[i]);
+		if (rc)
+			goto err_msix_request;
+	}
+
+	dev_dbg(ndev_dev(ndev), "Using msix interrupts\n");
+	ndev->db_vec_count = msix_count;
+	ndev->db_vec_shift = msix_shift;
+	return 0;
+
+err_msix_request:
+	while (i-- > 0)
+		free_irq(ndev->msix[i].vector, ndev);
+	pci_disable_msix(pdev);
+err_msix_enable:
+	kfree(ndev->msix);
+err_msix_alloc:
+	kfree(ndev->vec);
+err_msix_vec_alloc:
+	ndev->msix = NULL;
+	ndev->vec = NULL;
+
+	/* Try to set up msi irq */
+
+	rc = pci_enable_msi(pdev);
+	if (rc)
+		goto err_msi_enable;
+
+	rc = request_irq(pdev->irq, ndev_irq_isr, 0,
+			 "ndev_irq_isr", ndev);
+	if (rc)
+		goto err_msi_request;
+
+	dev_dbg(ndev_dev(ndev), "Using msi interrupts\n");
+	ndev->db_vec_count = 1;
+	ndev->db_vec_shift = total_shift;
+	return 0;
+
+err_msi_request:
+	pci_disable_msi(pdev);
+err_msi_enable:
+
+	/* Try to set up intx irq */
+
+	pci_intx(pdev, 1);
+
+	rc = request_irq(pdev->irq, ndev_irq_isr, IRQF_SHARED,
+			 "ndev_irq_isr", ndev);
+	if (rc)
+		goto err_intx_request;
+
+	dev_dbg(ndev_dev(ndev), "Using intx interrupts\n");
+	ndev->db_vec_count = 1;
+	ndev->db_vec_shift = total_shift;
+	return 0;
+
+err_intx_request:
+	return rc;
+}
+
+static void ndev_deinit_isr(struct intel_ntb_dev *ndev)
+{
+	struct pci_dev *pdev;
+	int i;
+
+	pdev = ndev_pdev(ndev);
+
+	/* Mask all doorbell interrupts */
+	ndev->db_mask = ndev->db_valid_mask;
+	ndev->reg->db_iowrite(ndev->db_mask,
+			      ndev->self_mmio +
+			      ndev->self_reg->db_mask);
+
+	if (ndev->msix) {
+		i = ndev->db_vec_count;
+		while (i--)
+			free_irq(ndev->msix[i].vector, &ndev->vec[i]);
+		pci_disable_msix(pdev);
+		kfree(ndev->msix);
+		kfree(ndev->vec);
+	} else {
+		free_irq(pdev->irq, ndev);
+		if (pci_dev_msi_enabled(pdev))
+			pci_disable_msi(pdev);
+	}
+}
+
+static ssize_t ndev_debugfs_read(struct file *filp, char __user *ubuf,
+				 size_t count, loff_t *offp)
+{
+	struct intel_ntb_dev *ndev;
+	void __iomem *mmio;
+	char *buf;
+	size_t buf_size;
+	ssize_t ret, off;
+	union { u64 v64; u32 v32; u16 v16; } u;
+
+	ndev = filp->private_data;
+	mmio = ndev->self_mmio;
+
+	buf_size = min(count, 0x800ul);
+
+	buf = kmalloc(buf_size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	off = 0;
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "NTB Device Information:\n");
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "Connection Topology -\t%s\n",
+			 ntb_topo_string(ndev->ntb.topo));
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "B2B Offset -\t\t%#lx\n", ndev->b2b_off);
+	off += scnprintf(buf + off, buf_size - off,
+			 "B2B MW Idx -\t\t%d\n", ndev->b2b_idx);
+	off += scnprintf(buf + off, buf_size - off,
+			 "BAR4 Split -\t\t%s\n",
+			 ndev->bar4_split ? "yes" : "no");
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "NTB CTL -\t\t%#06x\n", ndev->ntb_ctl);
+	off += scnprintf(buf + off, buf_size - off,
+			 "LNK STA -\t\t%#06x\n", ndev->lnk_sta);
+
+	if (!ndev->reg->link_is_up(ndev)) {
+		off += scnprintf(buf + off, buf_size - off,
+				 "Link Status -\t\tDown\n");
+	} else {
+		off += scnprintf(buf + off, buf_size - off,
+				 "Link Status -\t\tUp\n");
+		off += scnprintf(buf + off, buf_size - off,
+				 "Link Speed -\t\tPCI-E Gen %u\n",
+				 NTB_LNK_STA_SPEED(ndev->lnk_sta));
+		off += scnprintf(buf + off, buf_size - off,
+				 "Link Width -\t\tx%u\n",
+				 NTB_LNK_STA_WIDTH(ndev->lnk_sta));
+	}
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "Memory Window Count -\t%u\n", ndev->mw_count);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Scratchpad Count -\t%u\n", ndev->spad_count);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Count -\t%u\n", ndev->db_count);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Vector Count -\t%u\n", ndev->db_vec_count);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Vector Shift -\t%u\n", ndev->db_vec_shift);
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Valid Mask -\t%#llx\n", ndev->db_valid_mask);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Link Mask -\t%#llx\n", ndev->db_link_mask);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Mask Cached -\t%#llx\n", ndev->db_mask);
+
+	u.v64 = ndev_db_read(ndev, mmio + ndev->self_reg->db_mask);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Mask -\t\t%#llx\n", u.v64);
+
+	u.v64 = ndev_db_read(ndev, mmio + ndev->self_reg->db_bell);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Bell -\t\t%#llx\n", u.v64);
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "\nNTB Incoming XLAT:\n");
+
+	u.v64 = ioread64(mmio + bar2_off(ndev->xlat_reg->bar2_xlat, 2));
+	off += scnprintf(buf + off, buf_size - off,
+			 "XLAT23 -\t\t%#018llx\n", u.v64);
+
+	if (ndev->bar4_split) {
+		u.v32 = ioread32(mmio + bar2_off(ndev->xlat_reg->bar2_xlat, 4));
+		off += scnprintf(buf + off, buf_size - off,
+				 "XLAT4 -\t\t\t%#06x\n", u.v32);
+
+		u.v32 = ioread32(mmio + bar2_off(ndev->xlat_reg->bar2_xlat, 5));
+		off += scnprintf(buf + off, buf_size - off,
+				 "XLAT5 -\t\t\t%#06x\n", u.v32);
+	} else {
+		u.v64 = ioread64(mmio + bar2_off(ndev->xlat_reg->bar2_xlat, 4));
+		off += scnprintf(buf + off, buf_size - off,
+				 "XLAT45 -\t\t%#018llx\n", u.v64);
+	}
+
+	u.v64 = ioread64(mmio + bar2_off(ndev->xlat_reg->bar2_limit, 2));
+	off += scnprintf(buf + off, buf_size - off,
+			 "LMT23 -\t\t\t%#018llx\n", u.v64);
+
+	if (ndev->bar4_split) {
+		u.v32 = ioread32(mmio + bar2_off(ndev->xlat_reg->bar2_limit, 4));
+		off += scnprintf(buf + off, buf_size - off,
+				 "LMT4 -\t\t\t%#06x\n", u.v32);
+		u.v32 = ioread32(mmio + bar2_off(ndev->xlat_reg->bar2_limit, 5));
+		off += scnprintf(buf + off, buf_size - off,
+				 "LMT5 -\t\t\t%#06x\n", u.v32);
+	} else {
+		u.v64 = ioread64(mmio + bar2_off(ndev->xlat_reg->bar2_limit, 4));
+		off += scnprintf(buf + off, buf_size - off,
+				 "LMT45 -\t\t\t%#018llx\n", u.v64);
+	}
+
+	if (pdev_is_xeon(ndev->ntb.pdev)) {
+		if (ntb_topo_is_b2b(ndev->ntb.topo)) {
+			off += scnprintf(buf + off, buf_size - off,
+					 "\nNTB Outgoing B2B XLAT:\n");
+
+			u.v64 = ioread64(mmio + XEON_PBAR23XLAT_OFFSET);
+			off += scnprintf(buf + off, buf_size - off,
+					 "B2B XLAT23 -\t\t%#018llx\n", u.v64);
+
+			if (ndev->bar4_split) {
+				u.v32 = ioread32(mmio + XEON_PBAR4XLAT_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "B2B XLAT4 -\t\t%#06x\n",
+						 u.v32);
+				u.v32 = ioread32(mmio + XEON_PBAR5XLAT_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "B2B XLAT5 -\t\t%#06x\n",
+						 u.v32);
+			} else {
+				u.v64 = ioread64(mmio + XEON_PBAR45XLAT_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "B2B XLAT45 -\t\t%#018llx\n",
+						 u.v64);
+			}
+
+			u.v64 = ioread64(mmio + XEON_PBAR23LMT_OFFSET);
+			off += scnprintf(buf + off, buf_size - off,
+					 "B2B LMT23 -\t\t%#018llx\n", u.v64);
+
+			if (ndev->bar4_split) {
+				u.v32 = ioread32(mmio + XEON_PBAR4LMT_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "B2B LMT4 -\t\t%#06x\n",
+						 u.v32);
+				u.v32 = ioread32(mmio + XEON_PBAR5LMT_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "B2B LMT5 -\t\t%#06x\n",
+						 u.v32);
+			} else {
+				u.v64 = ioread64(mmio + XEON_PBAR45LMT_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "B2B LMT45 -\t\t%#018llx\n",
+						 u.v64);
+			}
+
+			off += scnprintf(buf + off, buf_size - off,
+					 "\nNTB Secondary BAR:\n");
+
+			u.v64 = ioread64(mmio + XEON_SBAR0BASE_OFFSET);
+			off += scnprintf(buf + off, buf_size - off,
+					 "SBAR01 -\t\t%#018llx\n", u.v64);
+
+			u.v64 = ioread64(mmio + XEON_SBAR23BASE_OFFSET);
+			off += scnprintf(buf + off, buf_size - off,
+					 "SBAR23 -\t\t%#018llx\n", u.v64);
+
+			if (ndev->bar4_split) {
+				u.v32 = ioread32(mmio + XEON_SBAR4BASE_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "SBAR4 -\t\t\t%#06x\n", u.v32);
+				u.v32 = ioread32(mmio + XEON_SBAR5BASE_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "SBAR5 -\t\t\t%#06x\n", u.v32);
+			} else {
+				u.v64 = ioread64(mmio + XEON_SBAR45BASE_OFFSET);
+				off += scnprintf(buf + off, buf_size - off,
+						 "SBAR45 -\t\t%#018llx\n",
+						 u.v64);
+			}
+		}
+
+		off += scnprintf(buf + off, buf_size - off,
+				 "\nXEON NTB Statistics:\n");
+
+		u.v16 = ioread16(mmio + XEON_USMEMMISS_OFFSET);
+		off += scnprintf(buf + off, buf_size - off,
+				 "Upstream Memory Miss -\t%u\n", u.v16);
+
+		off += scnprintf(buf + off, buf_size - off,
+				 "\nXEON NTB Hardware Errors:\n");
+
+		if (!pci_read_config_word(ndev->ntb.pdev,
+					  XEON_DEVSTS_OFFSET, &u.v16))
+			off += scnprintf(buf + off, buf_size - off,
+					 "DEVSTS -\t\t%#06x\n", u.v16);
+
+		if (!pci_read_config_word(ndev->ntb.pdev,
+					  XEON_LINK_STATUS_OFFSET, &u.v16))
+			off += scnprintf(buf + off, buf_size - off,
+					 "LNKSTS -\t\t%#06x\n", u.v16);
+
+		if (!pci_read_config_dword(ndev->ntb.pdev,
+					   XEON_UNCERRSTS_OFFSET, &u.v32))
+			off += scnprintf(buf + off, buf_size - off,
+					 "UNCERRSTS -\t\t%#06x\n", u.v32);
+
+		if (!pci_read_config_dword(ndev->ntb.pdev,
+					   XEON_CORERRSTS_OFFSET, &u.v32))
+			off += scnprintf(buf + off, buf_size - off,
+					 "CORERRSTS -\t\t%#06x\n", u.v32);
+	}
+
+	ret = simple_read_from_buffer(ubuf, count, offp, buf, off);
+	kfree(buf);
+	return ret;
+}
+
+static void ndev_init_debugfs(struct intel_ntb_dev *ndev)
+{
+	if (!debugfs_dir) {
+		ndev->debugfs_dir = NULL;
+		ndev->debugfs_info = NULL;
+	} else {
+		ndev->debugfs_dir =
+			debugfs_create_dir(ndev_name(ndev), debugfs_dir);
+		if (!ndev->debugfs_dir)
+			ndev->debugfs_info = NULL;
+		else
+			ndev->debugfs_info =
+				debugfs_create_file("info", S_IRUSR,
+						    ndev->debugfs_dir, ndev,
+						    &intel_ntb_debugfs_info);
+	}
+}
+
+static void ndev_deinit_debugfs(struct intel_ntb_dev *ndev)
+{
+	debugfs_remove_recursive(ndev->debugfs_dir);
+}
+
+static int intel_ntb_mw_count(struct ntb_dev *ntb)
+{
+	return ntb_ndev(ntb)->mw_count;
+}
+
+static int intel_ntb_mw_get_range(struct ntb_dev *ntb, int idx,
+				  phys_addr_t *base,
+				  resource_size_t *size,
+				  resource_size_t *align,
+				  resource_size_t *align_size)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+	int bar;
+
+	if (idx >= ndev->b2b_idx && !ndev->b2b_off)
+		idx += 1;
+
+	bar = ndev_mw_to_bar(ndev, idx);
+	if (bar < 0)
+		return bar;
+
+	if (base)
+		*base = pci_resource_start(ndev->ntb.pdev, bar) +
+			(idx == ndev->b2b_idx ? ndev->b2b_off : 0);
+
+	if (size)
+		*size = pci_resource_len(ndev->ntb.pdev, bar) -
+			(idx == ndev->b2b_idx ? ndev->b2b_off : 0);
+
+	if (align)
+		*align = pci_resource_len(ndev->ntb.pdev, bar);
+
+	if (align_size)
+		*align_size = 1;
+
+	return 0;
+}
+
+static int intel_ntb_mw_set_trans(struct ntb_dev *ntb, int idx,
+				  dma_addr_t addr, resource_size_t size)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+	unsigned long base_reg, xlat_reg, limit_reg;
+	resource_size_t bar_size, mw_size;
+	void __iomem *mmio;
+	u64 base, limit, reg_val;
+	int bar;
+
+	if (idx >= ndev->b2b_idx && !ndev->b2b_off)
+		idx += 1;
+
+	bar = ndev_mw_to_bar(ndev, idx);
+	if (bar < 0)
+		return bar;
+
+	bar_size = pci_resource_len(ndev->ntb.pdev, bar);
+
+	if (idx == ndev->b2b_idx)
+		mw_size = bar_size - ndev->b2b_off;
+	else
+		mw_size = bar_size;
+
+	/* hardware requires that addr is aligned to bar size */
+	if (addr & (bar_size - 1))
+		return -EINVAL;
+
+	/* make sure the range fits in the usable mw size */
+	if (size > mw_size)
+		return -EINVAL;
+
+	mmio = ndev->self_mmio;
+	base_reg = bar0_off(ndev->xlat_reg->bar0_base, bar);
+	xlat_reg = bar2_off(ndev->xlat_reg->bar2_xlat, bar);
+	limit_reg = bar2_off(ndev->xlat_reg->bar2_limit, bar);
+
+	if (bar < 4 || !ndev->bar4_split) {
+		base = ioread64(mmio + base_reg);
+
+		/* Set the limit if supported, if size is not mw_size */
+		if (limit_reg && size != mw_size)
+			limit = base + size;
+		else
+			limit = 0;
+
+		/* set and verify setting the translation address */
+		iowrite64(addr, mmio + xlat_reg);
+		reg_val = ioread64(mmio + xlat_reg);
+		if (reg_val != addr) {
+			iowrite64(0, mmio + xlat_reg);
+			return -EIO;
+		}
+
+		/* set and verify setting the limit */
+		iowrite64(limit, mmio + limit_reg);
+		reg_val = ioread64(mmio + limit_reg);
+		if (reg_val != limit) {
+			iowrite64(base, mmio + limit_reg);
+			iowrite64(0, mmio + xlat_reg);
+			return -EIO;
+		}
+	} else {
+		/* split bar addr range must all be 32 bit */
+		if (addr & (~0ull << 32))
+			return -EINVAL;
+		if ((addr + size) & (~0ull << 32))
+			return -EINVAL;
+
+		base = ioread32(mmio + base_reg);
+
+		/* Set the limit if supported, if size is not mw_size */
+		if (limit_reg && size != mw_size)
+			limit = base + size;
+		else
+			limit = 0;
+
+		/* set and verify setting the translation address */
+		iowrite32(addr, mmio + xlat_reg);
+		reg_val = ioread32(mmio + xlat_reg);
+		if (reg_val != addr) {
+			iowrite32(0, mmio + xlat_reg);
+			return -EIO;
+		}
+
+		/* set and verify setting the limit */
+		iowrite32(limit, mmio + limit_reg);
+		reg_val = ioread32(mmio + limit_reg);
+		if (reg_val != limit) {
+			iowrite32(base, mmio + limit_reg);
+			iowrite32(0, mmio + xlat_reg);
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int intel_ntb_link_is_up(struct ntb_dev *ntb,
+				enum ntb_speed *speed,
+				enum ntb_width *width)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	if (ndev->reg->link_is_up(ndev)) {
+		if (speed)
+			*speed = NTB_LNK_STA_SPEED(ndev->lnk_sta);
+		if (width)
+			*width = NTB_LNK_STA_WIDTH(ndev->lnk_sta);
+		return 1;
+	} else {
+		/* TODO MAYBE: is it possible to observe the link speed and
+		 * width while link is training? */
+		if (speed)
+			*speed = NTB_SPEED_NONE;
+		if (width)
+			*width = NTB_WIDTH_NONE;
+		return 0;
+	}
+}
+
+static int intel_ntb_link_enable(struct ntb_dev *ntb,
+				 enum ntb_speed max_speed,
+				 enum ntb_width max_width)
+{
+	struct intel_ntb_dev *ndev;
+	u32 ntb_ctl;
+
+	ndev = container_of(ntb, struct intel_ntb_dev, ntb);
+
+	if (ndev->ntb.topo == NTB_TOPO_SEC)
+		return -EINVAL;
+
+	dev_dbg(ndev_dev(ndev),
+		"Enabling link with max_speed %d max_width %d\n",
+		max_speed, max_width);
+	if (max_speed != NTB_SPEED_AUTO)
+		dev_dbg(ndev_dev(ndev), "ignoring max_speed %d\n", max_speed);
+	if (max_width != NTB_WIDTH_AUTO)
+		dev_dbg(ndev_dev(ndev), "ignoring max_width %d\n", max_width);
+
+	ntb_ctl = ioread32(ndev->self_mmio + ndev->reg->ntb_ctl);
+	ntb_ctl &= ~(NTB_CTL_DISABLE | NTB_CTL_CFG_LOCK);
+	ntb_ctl |= NTB_CTL_P2S_BAR2_SNOOP | NTB_CTL_S2P_BAR2_SNOOP;
+	ntb_ctl |= NTB_CTL_P2S_BAR4_SNOOP | NTB_CTL_S2P_BAR4_SNOOP;
+	if (ndev->bar4_split)
+		ntb_ctl |= NTB_CTL_P2S_BAR5_SNOOP | NTB_CTL_S2P_BAR5_SNOOP;
+	iowrite32(ntb_ctl, ndev->self_mmio + ndev->reg->ntb_ctl);
+
+	return 0;
+}
+
+static int intel_ntb_link_disable(struct ntb_dev *ntb)
+{
+	struct intel_ntb_dev *ndev;
+	u32 ntb_cntl;
+
+	ndev = container_of(ntb, struct intel_ntb_dev, ntb);
+
+	if (ndev->ntb.topo == NTB_TOPO_SEC)
+		return -EINVAL;
+
+	dev_dbg(ndev_dev(ndev), "Disabling link\n");
+
+	/* Bring NTB link down */
+	ntb_cntl = ioread32(ndev->self_mmio + ndev->reg->ntb_ctl);
+	ntb_cntl &= ~(NTB_CTL_P2S_BAR2_SNOOP | NTB_CTL_S2P_BAR2_SNOOP);
+	ntb_cntl &= ~(NTB_CTL_P2S_BAR4_SNOOP | NTB_CTL_S2P_BAR4_SNOOP);
+	if (ndev->bar4_split)
+		ntb_cntl &= ~(NTB_CTL_P2S_BAR5_SNOOP | NTB_CTL_S2P_BAR5_SNOOP);
+	ntb_cntl |= NTB_CTL_DISABLE | NTB_CTL_CFG_LOCK;
+	iowrite32(ntb_cntl, ndev->self_mmio + ndev->reg->ntb_ctl);
+
+	return 0;
+}
+
+static int intel_ntb_db_is_unsafe(struct ntb_dev *ntb)
+{
+	return ndev_ignore_unsafe(ntb_ndev(ntb), NTB_UNSAFE_DB);
+}
+
+static u64 intel_ntb_db_valid_mask(struct ntb_dev *ntb)
+{
+	return ntb_ndev(ntb)->db_valid_mask;
+}
+
+static int intel_ntb_db_vector_count(struct ntb_dev *ntb)
+{
+	struct intel_ntb_dev *ndev;
+
+	ndev = container_of(ntb, struct intel_ntb_dev, ntb);
+
+	return ndev->db_vec_count;
+}
+
+static u64 intel_ntb_db_vector_mask(struct ntb_dev *ntb, int db_vector)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	if (db_vector < 0 || db_vector > ndev->db_vec_count)
+		return 0;
+
+	return ndev->db_valid_mask & ndev_vec_mask(ndev, db_vector);
+}
+
+static u64 intel_ntb_db_read(struct ntb_dev *ntb)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_db_read(ndev,
+			    ndev->self_mmio +
+			    ndev->self_reg->db_bell);
+}
+
+static int intel_ntb_db_clear(struct ntb_dev *ntb, u64 db_bits)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_db_write(ndev, db_bits,
+			     ndev->self_mmio +
+			     ndev->self_reg->db_bell);
+}
+
+static int intel_ntb_db_set_mask(struct ntb_dev *ntb, u64 db_bits)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_db_set_mask(ndev, db_bits,
+				ndev->self_mmio +
+				ndev->self_reg->db_mask);
+}
+
+static int intel_ntb_db_clear_mask(struct ntb_dev *ntb, u64 db_bits)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_db_clear_mask(ndev, db_bits,
+				  ndev->self_mmio +
+				  ndev->self_reg->db_mask);
+}
+
+static int intel_ntb_peer_db_addr(struct ntb_dev *ntb,
+				  phys_addr_t *db_addr,
+				  resource_size_t *db_size)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_db_addr(ndev, db_addr, db_size, ndev->peer_addr,
+			    ndev->peer_reg->db_bell);
+}
+
+static int intel_ntb_peer_db_set(struct ntb_dev *ntb, u64 db_bits)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_db_write(ndev, db_bits,
+			     ndev->peer_mmio +
+			     ndev->peer_reg->db_bell);
+}
+
+static int intel_ntb_spad_is_unsafe(struct ntb_dev *ntb)
+{
+	return ndev_ignore_unsafe(ntb_ndev(ntb), NTB_UNSAFE_SPAD);
+}
+
+static int intel_ntb_spad_count(struct ntb_dev *ntb)
+{
+	struct intel_ntb_dev *ndev;
+
+	ndev = container_of(ntb, struct intel_ntb_dev, ntb);
+
+	return ndev->spad_count;
+}
+
+static u32 intel_ntb_spad_read(struct ntb_dev *ntb, int idx)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_spad_read(ndev, idx,
+			      ndev->self_mmio +
+			      ndev->self_reg->spad);
+}
+
+static int intel_ntb_spad_write(struct ntb_dev *ntb,
+				int idx, u32 val)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_spad_write(ndev, idx, val,
+			       ndev->self_mmio +
+			       ndev->self_reg->spad);
+}
+
+static int intel_ntb_peer_spad_addr(struct ntb_dev *ntb, int idx,
+				    phys_addr_t *spad_addr)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_spad_addr(ndev, idx, spad_addr, ndev->peer_addr,
+			      ndev->peer_reg->spad);
+}
+
+static u32 intel_ntb_peer_spad_read(struct ntb_dev *ntb, int idx)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_spad_read(ndev, idx,
+			      ndev->peer_mmio +
+			      ndev->peer_reg->spad);
+}
+
+static int intel_ntb_peer_spad_write(struct ntb_dev *ntb,
+				     int idx, u32 val)
+{
+	struct intel_ntb_dev *ndev = ntb_ndev(ntb);
+
+	return ndev_spad_write(ndev, idx, val,
+			       ndev->peer_mmio +
+			       ndev->peer_reg->spad);
+}
+
+/* ATOM */
+
+static u64 atom_db_ioread(void __iomem *mmio)
+{
+	return ioread64(mmio);
+}
+
+static void atom_db_iowrite(u64 bits, void __iomem *mmio)
+{
+	iowrite64(bits, mmio);
+}
+
+static int atom_poll_link(struct intel_ntb_dev *ndev)
+{
+	u32 ntb_ctl;
+
+	ntb_ctl = ioread32(ndev->self_mmio + ATOM_NTBCNTL_OFFSET);
+
+	if (ntb_ctl == ndev->ntb_ctl)
+		return 0;
+
+	ndev->ntb_ctl = ntb_ctl;
+
+	ndev->lnk_sta = ioread32(ndev->self_mmio + ATOM_LINK_STATUS_OFFSET);
+
+	return 1;
+}
+
+static int atom_link_is_up(struct intel_ntb_dev *ndev)
+{
+	return ATOM_NTB_CTL_ACTIVE(ndev->ntb_ctl);
+}
+
+static int atom_link_is_err(struct intel_ntb_dev *ndev)
+{
+	if (ioread32(ndev->self_mmio + ATOM_LTSSMSTATEJMP_OFFSET)
+	    & ATOM_LTSSMSTATEJMP_FORCEDETECT)
+		return 1;
+
+	if (ioread32(ndev->self_mmio + ATOM_IBSTERRRCRVSTS0_OFFSET)
+	    & ATOM_IBIST_ERR_OFLOW)
+		return 1;
+
+	return 0;
+}
+
+static inline enum ntb_topo atom_ppd_topo(struct intel_ntb_dev *ndev, u32 ppd)
+{
+	switch (ppd & ATOM_PPD_TOPO_MASK) {
+	case ATOM_PPD_TOPO_B2B_USD:
+		dev_dbg(ndev_dev(ndev), "PPD %d B2B USD\n", ppd);
+		return NTB_TOPO_B2B_USD;
+
+	case ATOM_PPD_TOPO_B2B_DSD:
+		dev_dbg(ndev_dev(ndev), "PPD %d B2B DSD\n", ppd);
+		return NTB_TOPO_B2B_DSD;
+
+	case ATOM_PPD_TOPO_PRI_USD:
+	case ATOM_PPD_TOPO_PRI_DSD: /* accept bogus PRI_DSD */
+	case ATOM_PPD_TOPO_SEC_USD:
+	case ATOM_PPD_TOPO_SEC_DSD: /* accept bogus SEC_DSD */
+		dev_dbg(ndev_dev(ndev), "PPD %d non B2B disabled\n", ppd);
+		return NTB_TOPO_NONE;
+	}
+
+	dev_dbg(ndev_dev(ndev), "PPD %d invalid\n", ppd);
+	return NTB_TOPO_NONE;
+}
+
+static void atom_link_hb(struct work_struct *work)
+{
+	struct intel_ntb_dev *ndev = hb_ndev(work);
+	unsigned long poll_ts;
+	void __iomem *mmio;
+	u32 status32;
+
+	poll_ts = ndev->last_ts + ATOM_LINK_HB_TIMEOUT;
+
+	/* Delay polling the link status if an interrupt was received,
+	 * unless the cached link status says the link is down.
+	 */
+	if (time_after(poll_ts, jiffies) && atom_link_is_up(ndev)) {
+		schedule_delayed_work(&ndev->hb_timer, poll_ts - jiffies);
+		return;
+	}
+
+	if (atom_poll_link(ndev))
+		ntb_link_event(&ndev->ntb);
+
+	if (atom_link_is_up(ndev) || !atom_link_is_err(ndev)) {
+		schedule_delayed_work(&ndev->hb_timer, ATOM_LINK_HB_TIMEOUT);
+		return;
+	}
+
+	/* Link is down with error: recover the link! */
+
+	mmio = ndev->self_mmio;
+
+	/* Driver resets the NTB ModPhy lanes - magic! */
+	iowrite8(0xe0, mmio + ATOM_MODPHY_PCSREG6);
+	iowrite8(0x40, mmio + ATOM_MODPHY_PCSREG4);
+	iowrite8(0x60, mmio + ATOM_MODPHY_PCSREG4);
+	iowrite8(0x60, mmio + ATOM_MODPHY_PCSREG6);
+
+	/* Driver waits 100ms to allow the NTB ModPhy to settle */
+	msleep(100);
+
+	/* Clear AER Errors, write to clear */
+	status32 = ioread32(mmio + ATOM_ERRCORSTS_OFFSET);
+	dev_dbg(ndev_dev(ndev), "ERRCORSTS = %x\n", status32);
+	status32 &= PCI_ERR_COR_REP_ROLL;
+	iowrite32(status32, mmio + ATOM_ERRCORSTS_OFFSET);
+
+	/* Clear unexpected electrical idle event in LTSSM, write to clear */
+	status32 = ioread32(mmio + ATOM_LTSSMERRSTS0_OFFSET);
+	dev_dbg(ndev_dev(ndev), "LTSSMERRSTS0 = %x\n", status32);
+	status32 |= ATOM_LTSSMERRSTS0_UNEXPECTEDEI;
+	iowrite32(status32, mmio + ATOM_LTSSMERRSTS0_OFFSET);
+
+	/* Clear DeSkew Buffer error, write to clear */
+	status32 = ioread32(mmio + ATOM_DESKEWSTS_OFFSET);
+	dev_dbg(ndev_dev(ndev), "DESKEWSTS = %x\n", status32);
+	status32 |= ATOM_DESKEWSTS_DBERR;
+	iowrite32(status32, mmio + ATOM_DESKEWSTS_OFFSET);
+
+	status32 = ioread32(mmio + ATOM_IBSTERRRCRVSTS0_OFFSET);
+	dev_dbg(ndev_dev(ndev), "IBSTERRRCRVSTS0 = %x\n", status32);
+	status32 &= ATOM_IBIST_ERR_OFLOW;
+	iowrite32(status32, mmio + ATOM_IBSTERRRCRVSTS0_OFFSET);
+
+	/* Releases the NTB state machine to allow the link to retrain */
+	status32 = ioread32(mmio + ATOM_LTSSMSTATEJMP_OFFSET);
+	dev_dbg(ndev_dev(ndev), "LTSSMSTATEJMP = %x\n", status32);
+	status32 &= ~ATOM_LTSSMSTATEJMP_FORCEDETECT;
+	iowrite32(status32, mmio + ATOM_LTSSMSTATEJMP_OFFSET);
+
+	/* There is a potential race between the 2 NTB devices recovering at the
+	 * same time.  If the times are the same, the link will not recover and
+	 * the driver will be stuck in this loop forever.  Add a random interval
+	 * to the recovery time to prevent this race.
+	 */
+	schedule_delayed_work(&ndev->hb_timer, ATOM_LINK_RECOVERY_TIME
+			      + prandom_u32() % ATOM_LINK_RECOVERY_TIME);
+}
+
+static int atom_init_isr(struct intel_ntb_dev *ndev)
+{
+	int rc;
+
+	rc = ndev_init_isr(ndev, 1, ATOM_DB_MSIX_VECTOR_COUNT,
+			   ATOM_DB_MSIX_VECTOR_SHIFT, ATOM_DB_TOTAL_SHIFT);
+	if (rc)
+		return rc;
+
+	/* ATOM doesn't have link status interrupt, poll on that platform */
+	ndev->last_ts = jiffies;
+	INIT_DELAYED_WORK(&ndev->hb_timer, atom_link_hb);
+	schedule_delayed_work(&ndev->hb_timer, ATOM_LINK_HB_TIMEOUT);
+
+	return 0;
+}
+
+static void atom_deinit_isr(struct intel_ntb_dev *ndev)
+{
+	cancel_delayed_work_sync(&ndev->hb_timer);
+	ndev_deinit_isr(ndev);
+}
+
+static int atom_init_ntb(struct intel_ntb_dev *ndev)
+{
+	ndev->mw_count = ATOM_MW_COUNT;
+	ndev->spad_count = ATOM_SPAD_COUNT;
+	ndev->db_count = ATOM_DB_COUNT;
+
+	switch (ndev->ntb.topo) {
+	case NTB_TOPO_B2B_USD:
+	case NTB_TOPO_B2B_DSD:
+		ndev->self_reg = &atom_pri_reg;
+		ndev->peer_reg = &atom_b2b_reg;
+		ndev->xlat_reg = &atom_sec_xlat;
+
+		/* Enable Bus Master and Memory Space on the secondary side */
+		iowrite16(PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER,
+			  ndev->self_mmio + ATOM_SPCICMD_OFFSET);
+
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	ndev->db_valid_mask = BIT_ULL(ndev->db_count) - 1;
+
+	return 0;
+}
+
+static int atom_init_dev(struct intel_ntb_dev *ndev)
+{
+	u32 ppd;
+	int rc;
+
+	rc = pci_read_config_dword(ndev->ntb.pdev, ATOM_PPD_OFFSET, &ppd);
+	if (rc)
+		return -EIO;
+
+	ndev->ntb.topo = atom_ppd_topo(ndev, ppd);
+	if (ndev->ntb.topo == NTB_TOPO_NONE)
+		return -EINVAL;
+
+	rc = atom_init_ntb(ndev);
+	if (rc)
+		return rc;
+
+	rc = atom_init_isr(ndev);
+	if (rc)
+		return rc;
+
+	if (ndev->ntb.topo != NTB_TOPO_SEC) {
+		/* Initiate PCI-E link training */
+		rc = pci_write_config_dword(ndev->ntb.pdev, ATOM_PPD_OFFSET,
+					    ppd | ATOM_PPD_INIT_LINK);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static void atom_deinit_dev(struct intel_ntb_dev *ndev)
+{
+	atom_deinit_isr(ndev);
+}
+
+/* XEON */
+
+static u64 xeon_db_ioread(void __iomem *mmio)
+{
+	return (u64)ioread16(mmio);
+}
+
+static void xeon_db_iowrite(u64 bits, void __iomem *mmio)
+{
+	iowrite16((u16)bits, mmio);
+}
+
+static int xeon_poll_link(struct intel_ntb_dev *ndev)
+{
+	u16 reg_val;
+	int rc;
+
+	ndev->reg->db_iowrite(ndev->db_link_mask,
+			      ndev->self_mmio +
+			      ndev->self_reg->db_bell);
+
+	rc = pci_read_config_word(ndev->ntb.pdev,
+				  XEON_LINK_STATUS_OFFSET, &reg_val);
+	if (rc)
+		return 0;
+
+	if (reg_val == ndev->lnk_sta)
+		return 0;
+
+	ndev->lnk_sta = reg_val;
+
+	return 1;
+}
+
+static int xeon_link_is_up(struct intel_ntb_dev *ndev)
+{
+	if (ndev->ntb.topo == NTB_TOPO_SEC)
+		return 1;
+
+	return NTB_LNK_STA_ACTIVE(ndev->lnk_sta);
+}
+
+static inline enum ntb_topo xeon_ppd_topo(struct intel_ntb_dev *ndev, u8 ppd)
+{
+	switch (ppd & XEON_PPD_TOPO_MASK) {
+	case XEON_PPD_TOPO_B2B_USD:
+		return NTB_TOPO_B2B_USD;
+
+	case XEON_PPD_TOPO_B2B_DSD:
+		return NTB_TOPO_B2B_DSD;
+
+	case XEON_PPD_TOPO_PRI_USD:
+	case XEON_PPD_TOPO_PRI_DSD: /* accept bogus PRI_DSD */
+		return NTB_TOPO_PRI;
+
+	case XEON_PPD_TOPO_SEC_USD:
+	case XEON_PPD_TOPO_SEC_DSD: /* accept bogus SEC_DSD */
+		return NTB_TOPO_SEC;
+	}
+
+	return NTB_TOPO_NONE;
+}
+
+static inline int xeon_ppd_bar4_split(struct intel_ntb_dev *ndev, u8 ppd)
+{
+	if (ppd & XEON_PPD_SPLIT_BAR_MASK) {
+		dev_dbg(ndev_dev(ndev), "PPD %d split bar\n", ppd);
+		return 1;
+	}
+	return 0;
+}
+
+static int xeon_init_isr(struct intel_ntb_dev *ndev)
+{
+	return ndev_init_isr(ndev, XEON_DB_MSIX_VECTOR_COUNT,
+			     XEON_DB_MSIX_VECTOR_COUNT,
+			     XEON_DB_MSIX_VECTOR_SHIFT,
+			     XEON_DB_TOTAL_SHIFT);
+}
+
+static void xeon_deinit_isr(struct intel_ntb_dev *ndev)
+{
+	ndev_deinit_isr(ndev);
+}
+
+static int xeon_setup_b2b_mw(struct intel_ntb_dev *ndev,
+			     const struct intel_b2b_addr *addr,
+			     const struct intel_b2b_addr *peer_addr)
+{
+	struct pci_dev *pdev;
+	void __iomem *mmio;
+	resource_size_t bar_size;
+	phys_addr_t bar_addr;
+	int b2b_bar;
+	u8 bar_sz;
+
+	pdev = ndev_pdev(ndev);
+	mmio = ndev->self_mmio;
+
+	if (ndev->b2b_idx >= ndev->mw_count) {
+		dev_dbg(ndev_dev(ndev), "not using b2b mw\n");
+		b2b_bar = 0;
+		ndev->b2b_off = 0;
+	} else {
+		b2b_bar = ndev_mw_to_bar(ndev, ndev->b2b_idx);
+		if (b2b_bar < 0)
+			return -EIO;
+
+		dev_dbg(ndev_dev(ndev), "using b2b mw bar %d\n", b2b_bar);
+
+		bar_size = pci_resource_len(ndev->ntb.pdev, b2b_bar);
+
+		dev_dbg(ndev_dev(ndev), "b2b bar size %#llx\n", bar_size);
+
+		if (b2b_mw_share && XEON_B2B_MIN_SIZE <= bar_size >> 1) {
+			dev_dbg(ndev_dev(ndev),
+				"b2b using first half of bar\n");
+			ndev->b2b_off = bar_size >> 1;
+		} else if (XEON_B2B_MIN_SIZE <= bar_size) {
+			dev_dbg(ndev_dev(ndev),
+				"b2b using whole bar\n");
+			ndev->b2b_off = 0;
+			--ndev->mw_count;
+		} else {
+			dev_dbg(ndev_dev(ndev),
+				"b2b bar size is too small\n");
+			return -EIO;
+		}
+	}
+
+	/* Reset the secondary bar sizes to match the primary bar sizes,
+	 * except disable or halve the size of the b2b secondary bar.
+	 *
+	 * Note: code for each specific bar size register, because the register
+	 * offsets are not in a consistent order (bar5sz comes after ppd, odd).
+	 */
+	pci_read_config_byte(pdev, XEON_PBAR23SZ_OFFSET, &bar_sz);
+	dev_dbg(ndev_dev(ndev), "PBAR23SZ %#x\n", bar_sz);
+	if (b2b_bar == 2) {
+		if (ndev->b2b_off)
+			bar_sz -= 1;
+		else
+			bar_sz = 0;
+	}
+	pci_write_config_byte(pdev, XEON_SBAR23SZ_OFFSET, bar_sz);
+	pci_read_config_byte(pdev, XEON_SBAR23SZ_OFFSET, &bar_sz);
+	dev_dbg(ndev_dev(ndev), "SBAR23SZ %#x\n", bar_sz);
+
+	if (!ndev->bar4_split) {
+		pci_read_config_byte(pdev, XEON_PBAR45SZ_OFFSET, &bar_sz);
+		dev_dbg(ndev_dev(ndev), "PBAR45SZ %#x\n", bar_sz);
+		if (b2b_bar == 4) {
+			if (ndev->b2b_off)
+				bar_sz -= 1;
+			else
+				bar_sz = 0;
+		}
+		pci_write_config_byte(pdev, XEON_SBAR45SZ_OFFSET, bar_sz);
+		pci_read_config_byte(pdev, XEON_SBAR45SZ_OFFSET, &bar_sz);
+		dev_dbg(ndev_dev(ndev), "SBAR45SZ %#x\n", bar_sz);
+	} else {
+		pci_read_config_byte(pdev, XEON_PBAR4SZ_OFFSET, &bar_sz);
+		dev_dbg(ndev_dev(ndev), "PBAR4SZ %#x\n", bar_sz);
+		if (b2b_bar == 4) {
+			if (ndev->b2b_off)
+				bar_sz -= 1;
+			else
+				bar_sz = 0;
+		}
+		pci_write_config_byte(pdev, XEON_SBAR4SZ_OFFSET, bar_sz);
+		pci_read_config_byte(pdev, XEON_SBAR4SZ_OFFSET, &bar_sz);
+		dev_dbg(ndev_dev(ndev), "SBAR4SZ %#x\n", bar_sz);
+
+		pci_read_config_byte(pdev, XEON_PBAR5SZ_OFFSET, &bar_sz);
+		dev_dbg(ndev_dev(ndev), "PBAR5SZ %#x\n", bar_sz);
+		if (b2b_bar == 5) {
+			if (ndev->b2b_off)
+				bar_sz -= 1;
+			else
+				bar_sz = 0;
+		}
+		pci_write_config_byte(pdev, XEON_SBAR5SZ_OFFSET, bar_sz);
+		pci_read_config_byte(pdev, XEON_SBAR5SZ_OFFSET, &bar_sz);
+		dev_dbg(ndev_dev(ndev), "SBAR5SZ %#x\n", bar_sz);
+	}
+
+	/* SBAR01 hit by first part of the b2b bar */
+	if (b2b_bar == 0)
+		bar_addr = addr->bar0_addr;
+	else if (b2b_bar == 2)
+		bar_addr = addr->bar2_addr64;
+	else if (b2b_bar == 4 && !ndev->bar4_split)
+		bar_addr = addr->bar4_addr64;
+	else if (b2b_bar == 4)
+		bar_addr = addr->bar4_addr32;
+	else if (b2b_bar == 5)
+		bar_addr = addr->bar5_addr32;
+	else
+		return -EIO;
+
+	dev_dbg(ndev_dev(ndev), "SBAR01 %#018llx\n", bar_addr);
+	iowrite64(bar_addr, mmio + XEON_SBAR0BASE_OFFSET);
+
+	/* Other SBAR are normally hit by the PBAR xlat, except for b2b bar.
+	 * The b2b bar is either disabled above, or configured half-size, and
+	 * it starts at the PBAR xlat + offset.
+	 */
+
+	bar_addr = addr->bar2_addr64 + (b2b_bar == 2 ? ndev->b2b_off : 0);
+	iowrite64(bar_addr, mmio + XEON_SBAR23BASE_OFFSET);
+	bar_addr = ioread64(mmio + XEON_SBAR23BASE_OFFSET);
+	dev_dbg(ndev_dev(ndev), "SBAR23 %#018llx\n", bar_addr);
+
+	if (!ndev->bar4_split) {
+		bar_addr = addr->bar4_addr64 +
+			(b2b_bar == 4 ? ndev->b2b_off : 0);
+		iowrite64(bar_addr, mmio + XEON_SBAR45BASE_OFFSET);
+		bar_addr = ioread64(mmio + XEON_SBAR45BASE_OFFSET);
+		dev_dbg(ndev_dev(ndev), "SBAR45 %#018llx\n", bar_addr);
+	} else {
+		bar_addr = addr->bar4_addr32 +
+			(b2b_bar == 4 ? ndev->b2b_off : 0);
+		iowrite32(bar_addr, mmio + XEON_SBAR4BASE_OFFSET);
+		bar_addr = ioread32(mmio + XEON_SBAR4BASE_OFFSET);
+		dev_dbg(ndev_dev(ndev), "SBAR4 %#010llx\n", bar_addr);
+
+		bar_addr = addr->bar5_addr32 +
+			(b2b_bar == 5 ? ndev->b2b_off : 0);
+		iowrite32(bar_addr, mmio + XEON_SBAR5BASE_OFFSET);
+		bar_addr = ioread32(mmio + XEON_SBAR5BASE_OFFSET);
+		dev_dbg(ndev_dev(ndev), "SBAR5 %#010llx\n", bar_addr);
+	}
+
+	/* setup incoming bar limits == base addrs (zero length windows) */
+
+	bar_addr = addr->bar2_addr64 + (b2b_bar == 2 ? ndev->b2b_off : 0);
+	iowrite64(bar_addr, mmio + XEON_SBAR23LMT_OFFSET);
+	bar_addr = ioread64(mmio + XEON_SBAR23LMT_OFFSET);
+	dev_dbg(ndev_dev(ndev), "SBAR23LMT %#018llx\n", bar_addr);
+
+	if (!ndev->bar4_split) {
+		bar_addr = addr->bar4_addr64 +
+			(b2b_bar == 4 ? ndev->b2b_off : 0);
+		iowrite64(bar_addr, mmio + XEON_SBAR45LMT_OFFSET);
+		bar_addr = ioread64(mmio + XEON_SBAR45LMT_OFFSET);
+		dev_dbg(ndev_dev(ndev), "SBAR45LMT %#018llx\n", bar_addr);
+	} else {
+		bar_addr = addr->bar4_addr32 +
+			(b2b_bar == 4 ? ndev->b2b_off : 0);
+		iowrite32(bar_addr, mmio + XEON_SBAR4LMT_OFFSET);
+		bar_addr = ioread32(mmio + XEON_SBAR4LMT_OFFSET);
+		dev_dbg(ndev_dev(ndev), "SBAR4LMT %#010llx\n", bar_addr);
+
+		bar_addr = addr->bar5_addr32 +
+			(b2b_bar == 5 ? ndev->b2b_off : 0);
+		iowrite32(bar_addr, mmio + XEON_SBAR5LMT_OFFSET);
+		bar_addr = ioread32(mmio + XEON_SBAR5LMT_OFFSET);
+		dev_dbg(ndev_dev(ndev), "SBAR5LMT %#05llx\n", bar_addr);
+	}
+
+	/* zero incoming translation addrs */
+	iowrite64(0, mmio + XEON_SBAR23XLAT_OFFSET);
+
+	if (!ndev->bar4_split) {
+		iowrite64(0, mmio + XEON_SBAR45XLAT_OFFSET);
+	} else {
+		iowrite32(0, mmio + XEON_SBAR4XLAT_OFFSET);
+		iowrite32(0, mmio + XEON_SBAR5XLAT_OFFSET);
+	}
+
+	/* zero outgoing translation limits (whole bar size windows) */
+	iowrite64(0, mmio + XEON_PBAR23LMT_OFFSET);
+	if (!ndev->bar4_split) {
+		iowrite64(0, mmio + XEON_PBAR45LMT_OFFSET);
+	} else {
+		iowrite32(0, mmio + XEON_PBAR4LMT_OFFSET);
+		iowrite32(0, mmio + XEON_PBAR5LMT_OFFSET);
+	}
+
+	/* set outgoing translation offsets */
+	bar_addr = peer_addr->bar2_addr64;
+	iowrite64(bar_addr, mmio + XEON_PBAR23XLAT_OFFSET);
+	bar_addr = ioread64(mmio + XEON_PBAR23XLAT_OFFSET);
+	dev_dbg(ndev_dev(ndev), "PBAR23XLAT %#018llx\n", bar_addr);
+
+	if (!ndev->bar4_split) {
+		bar_addr = peer_addr->bar4_addr64;
+		iowrite64(bar_addr, mmio + XEON_PBAR45XLAT_OFFSET);
+		bar_addr = ioread64(mmio + XEON_PBAR45XLAT_OFFSET);
+		dev_dbg(ndev_dev(ndev), "PBAR45XLAT %#018llx\n", bar_addr);
+	} else {
+		bar_addr = peer_addr->bar4_addr32;
+		iowrite32(bar_addr, mmio + XEON_PBAR4XLAT_OFFSET);
+		bar_addr = ioread32(mmio + XEON_PBAR4XLAT_OFFSET);
+		dev_dbg(ndev_dev(ndev), "PBAR4XLAT %#010llx\n", bar_addr);
+
+		bar_addr = peer_addr->bar5_addr32;
+		iowrite32(bar_addr, mmio + XEON_PBAR5XLAT_OFFSET);
+		bar_addr = ioread32(mmio + XEON_PBAR5XLAT_OFFSET);
+		dev_dbg(ndev_dev(ndev), "PBAR5XLAT %#010llx\n", bar_addr);
+	}
+
+	/* set the translation offset for b2b registers */
+	if (b2b_bar == 0)
+		bar_addr = peer_addr->bar0_addr;
+	else if (b2b_bar == 2)
+		bar_addr = peer_addr->bar2_addr64;
+	else if (b2b_bar == 4 && !ndev->bar4_split)
+		bar_addr = peer_addr->bar4_addr64;
+	else if (b2b_bar == 4)
+		bar_addr = peer_addr->bar4_addr32;
+	else if (b2b_bar == 5)
+		bar_addr = peer_addr->bar5_addr32;
+	else
+		return -EIO;
+
+	/* B2B_XLAT_OFFSET is 64bit, but can only take 32bit writes */
+	dev_dbg(ndev_dev(ndev), "B2BXLAT %#018llx\n", bar_addr);
+	iowrite32(bar_addr, mmio + XEON_B2B_XLAT_OFFSETL);
+	iowrite32(bar_addr >> 32, mmio + XEON_B2B_XLAT_OFFSETU);
+
+	if (b2b_bar) {
+		/* map peer ntb mmio config space registers */
+		ndev->peer_mmio = pci_iomap(pdev, b2b_bar,
+					    XEON_B2B_MIN_SIZE);
+		if (!ndev->peer_mmio)
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static int xeon_init_ntb(struct intel_ntb_dev *ndev)
+{
+	int rc;
+	u32 ntb_ctl;
+
+	if (ndev->bar4_split)
+		ndev->mw_count = HSX_SPLIT_BAR_MW_COUNT;
+	else
+		ndev->mw_count = XEON_MW_COUNT;
+
+	ndev->spad_count = XEON_SPAD_COUNT;
+	ndev->db_count = XEON_DB_COUNT;
+	ndev->db_link_mask = XEON_DB_LINK_BIT;
+
+	switch (ndev->ntb.topo) {
+	case NTB_TOPO_PRI:
+		if (ndev->hwerr_flags & NTB_HWERR_SDOORBELL_LOCKUP) {
+			dev_err(ndev_dev(ndev), "NTB Primary config disabled\n");
+			return -EINVAL;
+		}
+
+		/* enable link to allow secondary side device to appear */
+		ntb_ctl = ioread32(ndev->self_mmio + ndev->reg->ntb_ctl);
+		ntb_ctl &= ~NTB_CTL_DISABLE;
+		iowrite32(ntb_ctl, ndev->self_mmio + ndev->reg->ntb_ctl);
+
+		/* use half the spads for the peer */
+		ndev->spad_count >>= 1;
+		ndev->self_reg = &xeon_pri_reg;
+		ndev->peer_reg = &xeon_sec_reg;
+		ndev->xlat_reg = &xeon_sec_xlat;
+		break;
+
+	case NTB_TOPO_SEC:
+		if (ndev->hwerr_flags & NTB_HWERR_SDOORBELL_LOCKUP) {
+			dev_err(ndev_dev(ndev), "NTB Secondary config disabled\n");
+			return -EINVAL;
+		}
+		/* use half the spads for the peer */
+		ndev->spad_count >>= 1;
+		ndev->self_reg = &xeon_sec_reg;
+		ndev->peer_reg = &xeon_pri_reg;
+		ndev->xlat_reg = &xeon_pri_xlat;
+		break;
+
+	case NTB_TOPO_B2B_USD:
+	case NTB_TOPO_B2B_DSD:
+		ndev->self_reg = &xeon_pri_reg;
+		ndev->peer_reg = &xeon_b2b_reg;
+		ndev->xlat_reg = &xeon_sec_xlat;
+
+		if (ndev->hwerr_flags & NTB_HWERR_SDOORBELL_LOCKUP) {
+			ndev->peer_reg = &xeon_pri_reg;
+
+			if (b2b_mw_idx < 0)
+				ndev->b2b_idx = b2b_mw_idx + ndev->mw_count;
+			else
+				ndev->b2b_idx = b2b_mw_idx;
+
+			dev_dbg(ndev_dev(ndev),
+				"setting up b2b mw idx %d means %d\n",
+				b2b_mw_idx, ndev->b2b_idx);
+
+		} else if (ndev->hwerr_flags & NTB_HWERR_B2BDOORBELL_BIT14) {
+			dev_warn(ndev_dev(ndev), "Reduce doorbell count by 1\n");
+			ndev->db_count -= 1;
+		}
+
+		if (ndev->ntb.topo == NTB_TOPO_B2B_USD) {
+			rc = xeon_setup_b2b_mw(ndev,
+					       &xeon_b2b_dsd_addr,
+					       &xeon_b2b_usd_addr);
+		} else {
+			rc = xeon_setup_b2b_mw(ndev,
+					       &xeon_b2b_usd_addr,
+					       &xeon_b2b_dsd_addr);
+		}
+		if (rc)
+			return rc;
+
+		/* Enable Bus Master and Memory Space on the secondary side */
+		iowrite16(PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER,
+			  ndev->self_mmio + XEON_SPCICMD_OFFSET);
+
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	ndev->db_valid_mask = BIT_ULL(ndev->db_count) - 1;
+
+	ndev->reg->db_iowrite(ndev->db_valid_mask,
+			      ndev->self_mmio +
+			      ndev->self_reg->db_mask);
+
+	return 0;
+}
+
+static int xeon_init_dev(struct intel_ntb_dev *ndev)
+{
+	struct pci_dev *pdev;
+	u8 ppd;
+	int rc, mem;
+
+	pdev = ndev_pdev(ndev);
+
+	switch (pdev->device) {
+	/* There is a Xeon hardware errata related to writes to SDOORBELL or
+	 * B2BDOORBELL in conjunction with inbound access to NTB MMIO Space,
+	 * which may hang the system.  To workaround this use the second memory
+	 * window to access the interrupt and scratch pad registers on the
+	 * remote system.
+	 */
+	case PCI_DEVICE_ID_INTEL_NTB_SS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
+		ndev->hwerr_flags |= NTB_HWERR_SDOORBELL_LOCKUP;
+		break;
+	}
+
+	switch (pdev->device) {
+	/* There is a hardware errata related to accessing any register in
+	 * SB01BASE in the presence of bidirectional traffic crossing the NTB.
+	 */
+	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
+		ndev->hwerr_flags |= NTB_HWERR_SB01BASE_LOCKUP;
+		break;
+	}
+
+	switch (pdev->device) {
+	/* HW Errata on bit 14 of b2bdoorbell register.  Writes will not be
+	 * mirrored to the remote system.  Shrink the number of bits by one,
+	 * since bit 14 is the last bit.
+	 */
+	case PCI_DEVICE_ID_INTEL_NTB_SS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
+		ndev->hwerr_flags |= NTB_HWERR_B2BDOORBELL_BIT14;
+		break;
+	}
+
+	ndev->reg = &xeon_reg;
+
+	rc = pci_read_config_byte(pdev, XEON_PPD_OFFSET, &ppd);
+	if (rc)
+		return -EIO;
+
+	ndev->ntb.topo = xeon_ppd_topo(ndev, ppd);
+	dev_dbg(ndev_dev(ndev), "ppd %#x topo %s\n", ppd,
+		ntb_topo_string(ndev->ntb.topo));
+	if (ndev->ntb.topo == NTB_TOPO_NONE)
+		return -EINVAL;
+
+	if (ndev->ntb.topo != NTB_TOPO_SEC) {
+		ndev->bar4_split = xeon_ppd_bar4_split(ndev, ppd);
+		dev_dbg(ndev_dev(ndev), "ppd %#x bar4_split %d\n",
+			ppd, ndev->bar4_split);
+	} else {
+		/* This is a way for transparent BAR to figure out if we are
+		 * doing split BAR or not. There is no way for the hw on the
+		 * transparent side to know and set the PPD.
+		 */
+		mem = pci_select_bars(pdev, IORESOURCE_MEM);
+		ndev->bar4_split = hweight32(mem) ==
+			HSX_SPLIT_BAR_MW_COUNT + 1;
+		dev_dbg(ndev_dev(ndev), "mem %#x bar4_split %d\n",
+			mem, ndev->bar4_split);
+	}
+
+	rc = xeon_init_ntb(ndev);
+	if (rc)
+		return rc;
+
+	return xeon_init_isr(ndev);
+}
+
+static void xeon_deinit_dev(struct intel_ntb_dev *ndev)
+{
+	xeon_deinit_isr(ndev);
+}
+
+static int intel_ntb_init_pci(struct intel_ntb_dev *ndev, struct pci_dev *pdev)
+{
+	int rc;
+
+	pci_set_drvdata(pdev, ndev);
+
+	rc = pci_enable_device(pdev);
+	if (rc)
+		goto err_pci_enable;
+
+	rc = pci_request_regions(pdev, NTB_NAME);
+	if (rc)
+		goto err_pci_regions;
+
+	pci_set_master(pdev);
+
+	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (rc) {
+		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (rc)
+			goto err_dma_mask;
+		dev_warn(ndev_dev(ndev), "Cannot DMA highmem\n");
+	}
+
+	rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (rc) {
+		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (rc)
+			goto err_dma_mask;
+		dev_warn(ndev_dev(ndev), "Cannot DMA consistent highmem\n");
+	}
+
+	ndev->self_mmio = pci_iomap(pdev, 0, 0);
+	if (!ndev->self_mmio) {
+		rc = -EIO;
+		goto err_mmio;
+	}
+	ndev->peer_mmio = ndev->self_mmio;
+
+	return 0;
+
+err_mmio:
+err_dma_mask:
+	pci_clear_master(pdev);
+	pci_release_regions(pdev);
+err_pci_regions:
+	pci_disable_device(pdev);
+err_pci_enable:
+	pci_set_drvdata(pdev, NULL);
+	return rc;
+}
+
+static void intel_ntb_deinit_pci(struct intel_ntb_dev *ndev)
+{
+	struct pci_dev *pdev = ndev_pdev(ndev);
+
+	if (ndev->peer_mmio && ndev->peer_mmio != ndev->self_mmio)
+		pci_iounmap(pdev, ndev->peer_mmio);
+	pci_iounmap(pdev, ndev->self_mmio);
+
+	pci_clear_master(pdev);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+}
+
+static inline void ndev_init_struct(struct intel_ntb_dev *ndev,
+				    struct pci_dev *pdev)
+{
+	ndev->ntb.pdev = pdev;
+	ndev->ntb.topo = NTB_TOPO_NONE;
+	ndev->ntb.ops = &intel_ntb_ops;
+
+	ndev->b2b_off = 0;
+	ndev->b2b_idx = INT_MAX;
+
+	ndev->bar4_split = 0;
+
+	ndev->mw_count = 0;
+	ndev->spad_count = 0;
+	ndev->db_count = 0;
+	ndev->db_vec_count = 0;
+	ndev->db_vec_shift = 0;
+
+	ndev->ntb_ctl = 0;
+	ndev->lnk_sta = 0;
+
+	ndev->db_valid_mask = 0;
+	ndev->db_link_mask = 0;
+	ndev->db_mask = 0;
+
+	spin_lock_init(&ndev->db_mask_lock);
+}
+
+static int intel_ntb_pci_probe(struct pci_dev *pdev,
+			       const struct pci_device_id *id)
+{
+	struct intel_ntb_dev *ndev;
+	int rc, node;
+
+	node = dev_to_node(&pdev->dev);
+
+	if (pdev_is_atom(pdev)) {
+		ndev = kzalloc_node(sizeof(*ndev), GFP_KERNEL, node);
+		if (!ndev) {
+			rc = -ENOMEM;
+			goto err_ndev;
+		}
+
+		ndev_init_struct(ndev, pdev);
+
+		rc = intel_ntb_init_pci(ndev, pdev);
+		if (rc)
+			goto err_init_pci;
+
+		rc = atom_init_dev(ndev);
+		if (rc)
+			goto err_init_dev;
+
+	} else if (pdev_is_xeon(pdev)) {
+		ndev = kzalloc_node(sizeof(*ndev), GFP_KERNEL, node);
+		if (!ndev) {
+			rc = -ENOMEM;
+			goto err_ndev;
+		}
+
+		ndev_init_struct(ndev, pdev);
+
+		rc = intel_ntb_init_pci(ndev, pdev);
+		if (rc)
+			goto err_init_pci;
+
+		rc = xeon_init_dev(ndev);
+		if (rc)
+			goto err_init_dev;
+
+	} else {
+		rc = -EINVAL;
+		goto err_ndev;
+	}
+
+	ndev_reset_unsafe_flags(ndev);
+
+	ndev->reg->poll_link(ndev);
+
+	ndev_init_debugfs(ndev);
+
+	rc = ntb_register_device(&ndev->ntb);
+	if (rc)
+		goto err_register;
+
+	dev_info(&pdev->dev, "NTB device registered.\n");
+
+	return 0;
+
+err_register:
+	ndev_deinit_debugfs(ndev);
+	if (pdev_is_atom(pdev))
+		atom_deinit_dev(ndev);
+	else if (pdev_is_xeon(pdev))
+		xeon_deinit_dev(ndev);
+err_init_dev:
+	intel_ntb_deinit_pci(ndev);
+err_init_pci:
+	kfree(ndev);
+err_ndev:
+	return rc;
+}
+
+static void intel_ntb_pci_remove(struct pci_dev *pdev)
+{
+	struct intel_ntb_dev *ndev = pci_get_drvdata(pdev);
+
+	ntb_unregister_device(&ndev->ntb);
+	ndev_deinit_debugfs(ndev);
+	if (pdev_is_atom(pdev))
+		atom_deinit_dev(ndev);
+	else if (pdev_is_xeon(pdev))
+		xeon_deinit_dev(ndev);
+	intel_ntb_deinit_pci(ndev);
+	kfree(ndev);
+}
+
+static const struct intel_ntb_reg atom_reg = {
+	.poll_link		= atom_poll_link,
+	.link_is_up		= atom_link_is_up,
+	.db_ioread		= atom_db_ioread,
+	.db_iowrite		= atom_db_iowrite,
+	.db_size		= sizeof(u64),
+	.ntb_ctl		= ATOM_NTBCNTL_OFFSET,
+	.mw_bar			= {2, 4},
+};
+
+static const struct intel_ntb_alt_reg atom_pri_reg = {
+	.db_bell		= ATOM_PDOORBELL_OFFSET,
+	.db_mask		= ATOM_PDBMSK_OFFSET,
+	.spad			= ATOM_SPAD_OFFSET,
+};
+
+static const struct intel_ntb_alt_reg atom_b2b_reg = {
+	.db_bell		= ATOM_B2B_DOORBELL_OFFSET,
+	.spad			= ATOM_B2B_SPAD_OFFSET,
+};
+
+static const struct intel_ntb_xlat_reg atom_sec_xlat = {
+	/* FIXME : .bar0_base	= ATOM_SBAR0BASE_OFFSET, */
+	/* FIXME : .bar2_limit	= ATOM_SBAR2LMT_OFFSET, */
+	.bar2_xlat		= ATOM_SBAR2XLAT_OFFSET,
+};
+
+static const struct intel_ntb_reg xeon_reg = {
+	.poll_link		= xeon_poll_link,
+	.link_is_up		= xeon_link_is_up,
+	.db_ioread		= xeon_db_ioread,
+	.db_iowrite		= xeon_db_iowrite,
+	.db_size		= sizeof(u32),
+	.ntb_ctl		= XEON_NTBCNTL_OFFSET,
+	.mw_bar			= {2, 4, 5},
+};
+
+static const struct intel_ntb_alt_reg xeon_pri_reg = {
+	.db_bell		= XEON_PDOORBELL_OFFSET,
+	.db_mask		= XEON_PDBMSK_OFFSET,
+	.spad			= XEON_SPAD_OFFSET,
+};
+
+static const struct intel_ntb_alt_reg xeon_sec_reg = {
+	.db_bell		= XEON_SDOORBELL_OFFSET,
+	.db_mask		= XEON_SDBMSK_OFFSET,
+	/* second half of the scratchpads */
+	.spad			= XEON_SPAD_OFFSET + (XEON_SPAD_COUNT << 1),
+};
+
+static const struct intel_ntb_alt_reg xeon_b2b_reg = {
+	.db_bell		= XEON_B2B_DOORBELL_OFFSET,
+	.spad			= XEON_B2B_SPAD_OFFSET,
+};
+
+static const struct intel_ntb_xlat_reg xeon_pri_xlat = {
+	/* Note: no primary .bar0_base visible to the secondary side.
+	 *
+	 * The secondary side cannot get the base address stored in primary
+	 * bars.  The base address is necessary to set the limit register to
+	 * any value other than zero, or unlimited.
+	 *
+	 * WITHOUT THE BASE ADDRESS, THE SECONDARY SIDE CANNOT DISABLE the
+	 * window by setting the limit equal to base, nor can it limit the size
+	 * of the memory window by setting the limit to base + size.
+	 */
+	.bar2_limit		= XEON_PBAR23LMT_OFFSET,
+	.bar2_xlat		= XEON_PBAR23XLAT_OFFSET,
+};
+
+static const struct intel_ntb_xlat_reg xeon_sec_xlat = {
+	.bar0_base		= XEON_SBAR0BASE_OFFSET,
+	.bar2_limit		= XEON_SBAR23LMT_OFFSET,
+	.bar2_xlat		= XEON_SBAR23XLAT_OFFSET,
+};
+
+static struct intel_b2b_addr xeon_b2b_usd_addr = {
+	.bar2_addr64		= XEON_B2B_BAR2_USD_ADDR64,
+	.bar4_addr64		= XEON_B2B_BAR4_USD_ADDR64,
+	.bar4_addr32		= XEON_B2B_BAR4_USD_ADDR32,
+	.bar5_addr32		= XEON_B2B_BAR5_USD_ADDR32,
+};
+
+static struct intel_b2b_addr xeon_b2b_dsd_addr = {
+	.bar2_addr64		= XEON_B2B_BAR2_DSD_ADDR64,
+	.bar4_addr64		= XEON_B2B_BAR4_DSD_ADDR64,
+	.bar4_addr32		= XEON_B2B_BAR4_DSD_ADDR32,
+	.bar5_addr32		= XEON_B2B_BAR5_DSD_ADDR32,
+};
+
+/* operations for primary side of local ntb */
+static const struct ntb_dev_ops intel_ntb_ops = {
+	.mw_count		= intel_ntb_mw_count,
+	.mw_get_range		= intel_ntb_mw_get_range,
+	.mw_set_trans		= intel_ntb_mw_set_trans,
+	.link_is_up		= intel_ntb_link_is_up,
+	.link_enable		= intel_ntb_link_enable,
+	.link_disable		= intel_ntb_link_disable,
+	.db_is_unsafe		= intel_ntb_db_is_unsafe,
+	.db_valid_mask		= intel_ntb_db_valid_mask,
+	.db_vector_count	= intel_ntb_db_vector_count,
+	.db_vector_mask		= intel_ntb_db_vector_mask,
+	.db_read		= intel_ntb_db_read,
+	.db_clear		= intel_ntb_db_clear,
+	.db_set_mask		= intel_ntb_db_set_mask,
+	.db_clear_mask		= intel_ntb_db_clear_mask,
+	.peer_db_addr		= intel_ntb_peer_db_addr,
+	.peer_db_set		= intel_ntb_peer_db_set,
+	.spad_is_unsafe		= intel_ntb_spad_is_unsafe,
+	.spad_count		= intel_ntb_spad_count,
+	.spad_read		= intel_ntb_spad_read,
+	.spad_write		= intel_ntb_spad_write,
+	.peer_spad_addr		= intel_ntb_peer_spad_addr,
+	.peer_spad_read		= intel_ntb_peer_spad_read,
+	.peer_spad_write	= intel_ntb_peer_spad_write,
+};
+
+static const struct file_operations intel_ntb_debugfs_info = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = ndev_debugfs_read,
+};
+
+static const struct pci_device_id intel_ntb_pci_tbl[] = {
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_BWD)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_JSF)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_SNB)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_IVT)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_HSX)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_PS_JSF)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_PS_SNB)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_PS_IVT)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_PS_HSX)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_SS_JSF)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_SS_SNB)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_SS_IVT)},
+	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_SS_HSX)},
+	{0}
+};
+MODULE_DEVICE_TABLE(pci, intel_ntb_pci_tbl);
+
+static struct pci_driver intel_ntb_pci_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = intel_ntb_pci_tbl,
+	.probe = intel_ntb_pci_probe,
+	.remove = intel_ntb_pci_remove,
+};
+
+static int __init intel_ntb_pci_driver_init(void)
+{
+	pr_info("%s %s\n", NTB_DESC, NTB_VER);
+
+	if (debugfs_initialized())
+		debugfs_dir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+
+	return pci_register_driver(&intel_ntb_pci_driver);
+}
+module_init(intel_ntb_pci_driver_init);
+
+static void __exit intel_ntb_pci_driver_exit(void)
+{
+	pci_unregister_driver(&intel_ntb_pci_driver);
+
+	debugfs_remove_recursive(debugfs_dir);
+}
+module_exit(intel_ntb_pci_driver_exit);
+
diff --git a/drivers/ntb/hw/intel/ntb_hw_intel.h b/drivers/ntb/hw/intel/ntb_hw_intel.h
new file mode 100644
index 000000000000..7ddaf387b679
--- /dev/null
+++ b/drivers/ntb/hw/intel/ntb_hw_intel.h
@@ -0,0 +1,342 @@
+/*
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ *   redistributing this file, you may do so under either license.
+ *
+ *   GPL LICENSE SUMMARY
+ *
+ *   Copyright(c) 2012 Intel Corporation. All rights reserved.
+ *   Copyright (C) 2015 EMC 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.
+ *
+ *   BSD LICENSE
+ *
+ *   Copyright(c) 2012 Intel Corporation. All rights reserved.
+ *   Copyright (C) 2015 EMC Corporation. All Rights Reserved.
+ *
+ *   Redistribution and use in source and binary forms, with or without
+ *   modification, are permitted provided that the following conditions
+ *   are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copy
+ *       notice, this list of conditions and the following disclaimer in
+ *       the documentation and/or other materials provided with the
+ *       distribution.
+ *     * Neither the name of Intel Corporation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Intel PCIe NTB Linux driver
+ *
+ * Contact Information:
+ * Jon Mason <jon.mason@intel.com>
+ */
+
+#ifndef NTB_HW_INTEL_H
+#define NTB_HW_INTEL_H
+
+#include <linux/ntb.h>
+#include <linux/pci.h>
+
+#define PCI_DEVICE_ID_INTEL_NTB_B2B_JSF	0x3725
+#define PCI_DEVICE_ID_INTEL_NTB_PS_JSF	0x3726
+#define PCI_DEVICE_ID_INTEL_NTB_SS_JSF	0x3727
+#define PCI_DEVICE_ID_INTEL_NTB_B2B_SNB	0x3C0D
+#define PCI_DEVICE_ID_INTEL_NTB_PS_SNB	0x3C0E
+#define PCI_DEVICE_ID_INTEL_NTB_SS_SNB	0x3C0F
+#define PCI_DEVICE_ID_INTEL_NTB_B2B_IVT	0x0E0D
+#define PCI_DEVICE_ID_INTEL_NTB_PS_IVT	0x0E0E
+#define PCI_DEVICE_ID_INTEL_NTB_SS_IVT	0x0E0F
+#define PCI_DEVICE_ID_INTEL_NTB_B2B_HSX	0x2F0D
+#define PCI_DEVICE_ID_INTEL_NTB_PS_HSX	0x2F0E
+#define PCI_DEVICE_ID_INTEL_NTB_SS_HSX	0x2F0F
+#define PCI_DEVICE_ID_INTEL_NTB_B2B_BWD	0x0C4E
+
+/* Intel Xeon hardware */
+
+#define XEON_PBAR23LMT_OFFSET		0x0000
+#define XEON_PBAR45LMT_OFFSET		0x0008
+#define XEON_PBAR4LMT_OFFSET		0x0008
+#define XEON_PBAR5LMT_OFFSET		0x000c
+#define XEON_PBAR23XLAT_OFFSET		0x0010
+#define XEON_PBAR45XLAT_OFFSET		0x0018
+#define XEON_PBAR4XLAT_OFFSET		0x0018
+#define XEON_PBAR5XLAT_OFFSET		0x001c
+#define XEON_SBAR23LMT_OFFSET		0x0020
+#define XEON_SBAR45LMT_OFFSET		0x0028
+#define XEON_SBAR4LMT_OFFSET		0x0028
+#define XEON_SBAR5LMT_OFFSET		0x002c
+#define XEON_SBAR23XLAT_OFFSET		0x0030
+#define XEON_SBAR45XLAT_OFFSET		0x0038
+#define XEON_SBAR4XLAT_OFFSET		0x0038
+#define XEON_SBAR5XLAT_OFFSET		0x003c
+#define XEON_SBAR0BASE_OFFSET		0x0040
+#define XEON_SBAR23BASE_OFFSET		0x0048
+#define XEON_SBAR45BASE_OFFSET		0x0050
+#define XEON_SBAR4BASE_OFFSET		0x0050
+#define XEON_SBAR5BASE_OFFSET		0x0054
+#define XEON_SBDF_OFFSET		0x005c
+#define XEON_NTBCNTL_OFFSET		0x0058
+#define XEON_PDOORBELL_OFFSET		0x0060
+#define XEON_PDBMSK_OFFSET		0x0062
+#define XEON_SDOORBELL_OFFSET		0x0064
+#define XEON_SDBMSK_OFFSET		0x0066
+#define XEON_USMEMMISS_OFFSET		0x0070
+#define XEON_SPAD_OFFSET		0x0080
+#define XEON_PBAR23SZ_OFFSET		0x00d0
+#define XEON_PBAR45SZ_OFFSET		0x00d1
+#define XEON_PBAR4SZ_OFFSET		0x00d1
+#define XEON_SBAR23SZ_OFFSET		0x00d2
+#define XEON_SBAR45SZ_OFFSET		0x00d3
+#define XEON_SBAR4SZ_OFFSET		0x00d3
+#define XEON_PPD_OFFSET			0x00d4
+#define XEON_PBAR5SZ_OFFSET		0x00d5
+#define XEON_SBAR5SZ_OFFSET		0x00d6
+#define XEON_WCCNTRL_OFFSET		0x00e0
+#define XEON_UNCERRSTS_OFFSET		0x014c
+#define XEON_CORERRSTS_OFFSET		0x0158
+#define XEON_LINK_STATUS_OFFSET		0x01a2
+#define XEON_SPCICMD_OFFSET		0x0504
+#define XEON_DEVCTRL_OFFSET		0x0598
+#define XEON_DEVSTS_OFFSET		0x059a
+#define XEON_SLINK_STATUS_OFFSET	0x05a2
+#define XEON_B2B_SPAD_OFFSET		0x0100
+#define XEON_B2B_DOORBELL_OFFSET	0x0140
+#define XEON_B2B_XLAT_OFFSETL		0x0144
+#define XEON_B2B_XLAT_OFFSETU		0x0148
+#define XEON_PPD_CONN_MASK		0x03
+#define XEON_PPD_CONN_TRANSPARENT	0x00
+#define XEON_PPD_CONN_B2B		0x01
+#define XEON_PPD_CONN_RP		0x02
+#define XEON_PPD_DEV_MASK		0x10
+#define XEON_PPD_DEV_USD		0x00
+#define XEON_PPD_DEV_DSD		0x10
+#define XEON_PPD_SPLIT_BAR_MASK		0x40
+
+#define XEON_PPD_TOPO_MASK	(XEON_PPD_CONN_MASK | XEON_PPD_DEV_MASK)
+#define XEON_PPD_TOPO_PRI_USD	(XEON_PPD_CONN_RP | XEON_PPD_DEV_USD)
+#define XEON_PPD_TOPO_PRI_DSD	(XEON_PPD_CONN_RP | XEON_PPD_DEV_DSD)
+#define XEON_PPD_TOPO_SEC_USD	(XEON_PPD_CONN_TRANSPARENT | XEON_PPD_DEV_USD)
+#define XEON_PPD_TOPO_SEC_DSD	(XEON_PPD_CONN_TRANSPARENT | XEON_PPD_DEV_DSD)
+#define XEON_PPD_TOPO_B2B_USD	(XEON_PPD_CONN_B2B | XEON_PPD_DEV_USD)
+#define XEON_PPD_TOPO_B2B_DSD	(XEON_PPD_CONN_B2B | XEON_PPD_DEV_DSD)
+
+#define XEON_MW_COUNT			2
+#define HSX_SPLIT_BAR_MW_COUNT		3
+#define XEON_DB_COUNT			15
+#define XEON_DB_LINK			15
+#define XEON_DB_LINK_BIT			BIT_ULL(XEON_DB_LINK)
+#define XEON_DB_MSIX_VECTOR_COUNT	4
+#define XEON_DB_MSIX_VECTOR_SHIFT	5
+#define XEON_DB_TOTAL_SHIFT		16
+#define XEON_SPAD_COUNT			16
+
+/* Intel Atom hardware */
+
+#define ATOM_SBAR2XLAT_OFFSET		0x0008
+#define ATOM_PDOORBELL_OFFSET		0x0020
+#define ATOM_PDBMSK_OFFSET		0x0028
+#define ATOM_NTBCNTL_OFFSET		0x0060
+#define ATOM_SPAD_OFFSET			0x0080
+#define ATOM_PPD_OFFSET			0x00d4
+#define ATOM_PBAR2XLAT_OFFSET		0x8008
+#define ATOM_B2B_DOORBELL_OFFSET		0x8020
+#define ATOM_B2B_SPAD_OFFSET		0x8080
+#define ATOM_SPCICMD_OFFSET		0xb004
+#define ATOM_LINK_STATUS_OFFSET		0xb052
+#define ATOM_ERRCORSTS_OFFSET		0xb110
+#define ATOM_IP_BASE			0xc000
+#define ATOM_DESKEWSTS_OFFSET		(ATOM_IP_BASE + 0x3024)
+#define ATOM_LTSSMERRSTS0_OFFSET		(ATOM_IP_BASE + 0x3180)
+#define ATOM_LTSSMSTATEJMP_OFFSET	(ATOM_IP_BASE + 0x3040)
+#define ATOM_IBSTERRRCRVSTS0_OFFSET	(ATOM_IP_BASE + 0x3324)
+#define ATOM_MODPHY_PCSREG4		0x1c004
+#define ATOM_MODPHY_PCSREG6		0x1c006
+
+#define ATOM_PPD_INIT_LINK		0x0008
+#define ATOM_PPD_CONN_MASK		0x0300
+#define ATOM_PPD_CONN_TRANSPARENT	0x0000
+#define ATOM_PPD_CONN_B2B		0x0100
+#define ATOM_PPD_CONN_RP			0x0200
+#define ATOM_PPD_DEV_MASK		0x1000
+#define ATOM_PPD_DEV_USD			0x0000
+#define ATOM_PPD_DEV_DSD			0x1000
+#define ATOM_PPD_TOPO_MASK	(ATOM_PPD_CONN_MASK | ATOM_PPD_DEV_MASK)
+#define ATOM_PPD_TOPO_PRI_USD	(ATOM_PPD_CONN_TRANSPARENT | ATOM_PPD_DEV_USD)
+#define ATOM_PPD_TOPO_PRI_DSD	(ATOM_PPD_CONN_TRANSPARENT | ATOM_PPD_DEV_DSD)
+#define ATOM_PPD_TOPO_SEC_USD	(ATOM_PPD_CONN_RP | ATOM_PPD_DEV_USD)
+#define ATOM_PPD_TOPO_SEC_DSD	(ATOM_PPD_CONN_RP | ATOM_PPD_DEV_DSD)
+#define ATOM_PPD_TOPO_B2B_USD	(ATOM_PPD_CONN_B2B | ATOM_PPD_DEV_USD)
+#define ATOM_PPD_TOPO_B2B_DSD	(ATOM_PPD_CONN_B2B | ATOM_PPD_DEV_DSD)
+
+#define ATOM_MW_COUNT			2
+#define ATOM_DB_COUNT			34
+#define ATOM_DB_VALID_MASK		(BIT_ULL(ATOM_DB_COUNT) - 1)
+#define ATOM_DB_MSIX_VECTOR_COUNT	34
+#define ATOM_DB_MSIX_VECTOR_SHIFT	1
+#define ATOM_DB_TOTAL_SHIFT		34
+#define ATOM_SPAD_COUNT			16
+
+#define ATOM_NTB_CTL_DOWN_BIT		BIT(16)
+#define ATOM_NTB_CTL_ACTIVE(x)		!(x & ATOM_NTB_CTL_DOWN_BIT)
+
+#define ATOM_DESKEWSTS_DBERR		BIT(15)
+#define ATOM_LTSSMERRSTS0_UNEXPECTEDEI	BIT(20)
+#define ATOM_LTSSMSTATEJMP_FORCEDETECT	BIT(2)
+#define ATOM_IBIST_ERR_OFLOW		0x7FFF7FFF
+
+#define ATOM_LINK_HB_TIMEOUT		msecs_to_jiffies(1000)
+#define ATOM_LINK_RECOVERY_TIME		msecs_to_jiffies(500)
+
+/* Ntb control and link status */
+
+#define NTB_CTL_CFG_LOCK		BIT(0)
+#define NTB_CTL_DISABLE			BIT(1)
+#define NTB_CTL_S2P_BAR2_SNOOP		BIT(2)
+#define NTB_CTL_P2S_BAR2_SNOOP		BIT(4)
+#define NTB_CTL_S2P_BAR4_SNOOP		BIT(6)
+#define NTB_CTL_P2S_BAR4_SNOOP		BIT(8)
+#define NTB_CTL_S2P_BAR5_SNOOP		BIT(12)
+#define NTB_CTL_P2S_BAR5_SNOOP		BIT(14)
+
+#define NTB_LNK_STA_ACTIVE_BIT		0x2000
+#define NTB_LNK_STA_SPEED_MASK		0x000f
+#define NTB_LNK_STA_WIDTH_MASK		0x03f0
+#define NTB_LNK_STA_ACTIVE(x)		(!!((x) & NTB_LNK_STA_ACTIVE_BIT))
+#define NTB_LNK_STA_SPEED(x)		((x) & NTB_LNK_STA_SPEED_MASK)
+#define NTB_LNK_STA_WIDTH(x)		(((x) & NTB_LNK_STA_WIDTH_MASK) >> 4)
+
+/* Use the following addresses for translation between b2b ntb devices in case
+ * the hardware default values are not reliable. */
+#define XEON_B2B_BAR0_USD_ADDR		0x1000000000000000ull
+#define XEON_B2B_BAR2_USD_ADDR64	0x2000000000000000ull
+#define XEON_B2B_BAR4_USD_ADDR64	0x4000000000000000ull
+#define XEON_B2B_BAR4_USD_ADDR32	0x20000000u
+#define XEON_B2B_BAR5_USD_ADDR32	0x40000000u
+#define XEON_B2B_BAR0_DSD_ADDR		0x9000000000000000ull
+#define XEON_B2B_BAR2_DSD_ADDR64	0xa000000000000000ull
+#define XEON_B2B_BAR4_DSD_ADDR64	0xc000000000000000ull
+#define XEON_B2B_BAR4_DSD_ADDR32	0xa0000000u
+#define XEON_B2B_BAR5_DSD_ADDR32	0xc0000000u
+
+/* The peer ntb secondary config space is 32KB fixed size */
+#define XEON_B2B_MIN_SIZE		0x8000
+
+/* flags to indicate hardware errata */
+#define NTB_HWERR_SDOORBELL_LOCKUP	BIT_ULL(0)
+#define NTB_HWERR_SB01BASE_LOCKUP	BIT_ULL(1)
+#define NTB_HWERR_B2BDOORBELL_BIT14	BIT_ULL(2)
+
+/* flags to indicate unsafe api */
+#define NTB_UNSAFE_DB			BIT_ULL(0)
+#define NTB_UNSAFE_SPAD			BIT_ULL(1)
+
+struct intel_ntb_dev;
+
+struct intel_ntb_reg {
+	int (*poll_link)(struct intel_ntb_dev *ndev);
+	int (*link_is_up)(struct intel_ntb_dev *ndev);
+	u64 (*db_ioread)(void __iomem *mmio);
+	void (*db_iowrite)(u64 db_bits, void __iomem *mmio);
+	unsigned long			ntb_ctl;
+	resource_size_t			db_size;
+	int				mw_bar[];
+};
+
+struct intel_ntb_alt_reg {
+	unsigned long			db_bell;
+	unsigned long			db_mask;
+	unsigned long			spad;
+};
+
+struct intel_ntb_xlat_reg {
+	unsigned long			bar0_base;
+	unsigned long			bar2_xlat;
+	unsigned long			bar2_limit;
+};
+
+struct intel_b2b_addr {
+	phys_addr_t			bar0_addr;
+	phys_addr_t			bar2_addr64;
+	phys_addr_t			bar4_addr64;
+	phys_addr_t			bar4_addr32;
+	phys_addr_t			bar5_addr32;
+};
+
+struct intel_ntb_vec {
+	struct intel_ntb_dev		*ndev;
+	int				num;
+};
+
+struct intel_ntb_dev {
+	struct ntb_dev			ntb;
+
+	/* offset of peer bar0 in b2b bar */
+	unsigned long			b2b_off;
+	/* mw idx used to access peer bar0 */
+	unsigned int			b2b_idx;
+
+	/* BAR45 is split into BAR4 and BAR5 */
+	bool				bar4_split;
+
+	u32				ntb_ctl;
+	u32				lnk_sta;
+
+	unsigned char			mw_count;
+	unsigned char			spad_count;
+	unsigned char			db_count;
+	unsigned char			db_vec_count;
+	unsigned char			db_vec_shift;
+
+	u64				db_valid_mask;
+	u64				db_link_mask;
+	u64				db_mask;
+
+	/* synchronize rmw access of db_mask and hw reg */
+	spinlock_t			db_mask_lock;
+
+	struct msix_entry		*msix;
+	struct intel_ntb_vec		*vec;
+
+	const struct intel_ntb_reg	*reg;
+	const struct intel_ntb_alt_reg	*self_reg;
+	const struct intel_ntb_alt_reg	*peer_reg;
+	const struct intel_ntb_xlat_reg	*xlat_reg;
+	void				__iomem *self_mmio;
+	void				__iomem *peer_mmio;
+	phys_addr_t			peer_addr;
+
+	unsigned long			last_ts;
+	struct delayed_work		hb_timer;
+
+	unsigned long			hwerr_flags;
+	unsigned long			unsafe_flags;
+	unsigned long			unsafe_flags_ignore;
+
+	struct dentry			*debugfs_dir;
+	struct dentry			*debugfs_info;
+};
+
+#define ndev_pdev(ndev) ((ndev)->ntb.pdev)
+#define ndev_name(ndev) pci_name(ndev_pdev(ndev))
+#define ndev_dev(ndev) (&ndev_pdev(ndev)->dev)
+#define ntb_ndev(ntb) container_of(ntb, struct intel_ntb_dev, ntb)
+#define hb_ndev(work) container_of(work, struct intel_ntb_dev, hb_timer.work)
+
+#endif
diff --git a/drivers/ntb/ntb.c b/drivers/ntb/ntb.c
new file mode 100644
index 000000000000..23435f2a5486
--- /dev/null
+++ b/drivers/ntb/ntb.c
@@ -0,0 +1,251 @@
+/*
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ *   redistributing this file, you may do so under either license.
+ *
+ *   GPL LICENSE SUMMARY
+ *
+ *   Copyright (C) 2015 EMC 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.
+ *
+ *   BSD LICENSE
+ *
+ *   Copyright (C) 2015 EMC Corporation. All Rights Reserved.
+ *
+ *   Redistribution and use in source and binary forms, with or without
+ *   modification, are permitted provided that the following conditions
+ *   are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copy
+ *       notice, this list of conditions and the following disclaimer in
+ *       the documentation and/or other materials provided with the
+ *       distribution.
+ *     * Neither the name of Intel Corporation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * PCIe NTB Linux driver
+ *
+ * Contact Information:
+ * Allen Hubbe <Allen.Hubbe@emc.com>
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include <linux/ntb.h>
+#include <linux/pci.h>
+
+#define DRIVER_NAME			"ntb"
+#define DRIVER_DESCRIPTION		"PCIe NTB Driver Framework"
+
+#define DRIVER_LICENSE			"Dual BSD/GPL"
+#define DRIVER_VERSION			"1.0"
+#define DRIVER_RELDATE			"24 March 2015"
+#define DRIVER_AUTHOR			"Allen Hubbe <Allen.Hubbe@emc.com>"
+
+MODULE_LICENSE(DRIVER_LICENSE);
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESCRIPTION);
+
+static struct bus_type ntb_bus;
+static void ntb_dev_release(struct device *dev);
+
+int __ntb_register_client(struct ntb_client *client, struct module *mod,
+			  const char *mod_name)
+{
+	if (!client)
+		return -EINVAL;
+	if (!ntb_client_ops_is_valid(&client->ops))
+		return -EINVAL;
+
+	memset(&client->drv, 0, sizeof(client->drv));
+	client->drv.bus = &ntb_bus;
+	client->drv.name = mod_name;
+	client->drv.owner = mod;
+
+	return driver_register(&client->drv);
+}
+EXPORT_SYMBOL(__ntb_register_client);
+
+void ntb_unregister_client(struct ntb_client *client)
+{
+	driver_unregister(&client->drv);
+}
+EXPORT_SYMBOL(ntb_unregister_client);
+
+int ntb_register_device(struct ntb_dev *ntb)
+{
+	if (!ntb)
+		return -EINVAL;
+	if (!ntb->pdev)
+		return -EINVAL;
+	if (!ntb->ops)
+		return -EINVAL;
+	if (!ntb_dev_ops_is_valid(ntb->ops))
+		return -EINVAL;
+
+	init_completion(&ntb->released);
+
+	memset(&ntb->dev, 0, sizeof(ntb->dev));
+	ntb->dev.bus = &ntb_bus;
+	ntb->dev.parent = &ntb->pdev->dev;
+	ntb->dev.release = ntb_dev_release;
+	dev_set_name(&ntb->dev, pci_name(ntb->pdev));
+
+	ntb->ctx = NULL;
+	ntb->ctx_ops = NULL;
+	spin_lock_init(&ntb->ctx_lock);
+
+	return device_register(&ntb->dev);
+}
+EXPORT_SYMBOL(ntb_register_device);
+
+void ntb_unregister_device(struct ntb_dev *ntb)
+{
+	device_unregister(&ntb->dev);
+	wait_for_completion(&ntb->released);
+}
+EXPORT_SYMBOL(ntb_unregister_device);
+
+int ntb_set_ctx(struct ntb_dev *ntb, void *ctx,
+		const struct ntb_ctx_ops *ctx_ops)
+{
+	unsigned long irqflags;
+
+	if (!ntb_ctx_ops_is_valid(ctx_ops))
+		return -EINVAL;
+	if (ntb->ctx_ops)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ntb->ctx_lock, irqflags);
+	{
+		ntb->ctx = ctx;
+		ntb->ctx_ops = ctx_ops;
+	}
+	spin_unlock_irqrestore(&ntb->ctx_lock, irqflags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ntb_set_ctx);
+
+void ntb_clear_ctx(struct ntb_dev *ntb)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&ntb->ctx_lock, irqflags);
+	{
+		ntb->ctx_ops = NULL;
+		ntb->ctx = NULL;
+	}
+	spin_unlock_irqrestore(&ntb->ctx_lock, irqflags);
+}
+EXPORT_SYMBOL(ntb_clear_ctx);
+
+void ntb_link_event(struct ntb_dev *ntb)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&ntb->ctx_lock, irqflags);
+	{
+		if (ntb->ctx_ops && ntb->ctx_ops->link_event)
+			ntb->ctx_ops->link_event(ntb->ctx);
+	}
+	spin_unlock_irqrestore(&ntb->ctx_lock, irqflags);
+}
+EXPORT_SYMBOL(ntb_link_event);
+
+void ntb_db_event(struct ntb_dev *ntb, int vector)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&ntb->ctx_lock, irqflags);
+	{
+		if (ntb->ctx_ops && ntb->ctx_ops->db_event)
+			ntb->ctx_ops->db_event(ntb->ctx, vector);
+	}
+	spin_unlock_irqrestore(&ntb->ctx_lock, irqflags);
+}
+EXPORT_SYMBOL(ntb_db_event);
+
+static int ntb_probe(struct device *dev)
+{
+	struct ntb_dev *ntb;
+	struct ntb_client *client;
+	int rc;
+
+	get_device(dev);
+	ntb = dev_ntb(dev);
+	client = drv_ntb_client(dev->driver);
+
+	rc = client->ops.probe(client, ntb);
+	if (rc)
+		put_device(dev);
+
+	return rc;
+}
+
+static int ntb_remove(struct device *dev)
+{
+	struct ntb_dev *ntb;
+	struct ntb_client *client;
+
+	if (dev->driver) {
+		ntb = dev_ntb(dev);
+		client = drv_ntb_client(dev->driver);
+
+		client->ops.remove(client, ntb);
+		put_device(dev);
+	}
+
+	return 0;
+}
+
+static void ntb_dev_release(struct device *dev)
+{
+	struct ntb_dev *ntb = dev_ntb(dev);
+
+	complete(&ntb->released);
+}
+
+static struct bus_type ntb_bus = {
+	.name = "ntb",
+	.probe = ntb_probe,
+	.remove = ntb_remove,
+};
+
+static int __init ntb_driver_init(void)
+{
+	return bus_register(&ntb_bus);
+}
+module_init(ntb_driver_init);
+
+static void __exit ntb_driver_exit(void)
+{
+	bus_unregister(&ntb_bus);
+}
+module_exit(ntb_driver_exit);
+
diff --git a/drivers/ntb/ntb_hw.c b/drivers/ntb/ntb_hw.c
deleted file mode 100644
index cd29b1038c5e..000000000000
--- a/drivers/ntb/ntb_hw.c
+++ /dev/null
@@ -1,1896 +0,0 @@
-/*
- * This file is provided under a dual BSD/GPLv2 license.  When using or
- *   redistributing this file, you may do so under either license.
- *
- *   GPL LICENSE SUMMARY
- *
- *   Copyright(c) 2012 Intel Corporation. All rights reserved.
- *
- *   This program is free software; you can redistribute it and/or modify
- *   it under the terms of version 2 of the GNU General Public License as
- *   published by the Free Software Foundation.
- *
- *   BSD LICENSE
- *
- *   Copyright(c) 2012 Intel Corporation. All rights reserved.
- *
- *   Redistribution and use in source and binary forms, with or without
- *   modification, are permitted provided that the following conditions
- *   are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copy
- *       notice, this list of conditions and the following disclaimer in
- *       the documentation and/or other materials provided with the
- *       distribution.
- *     * Neither the name of Intel Corporation nor the names of its
- *       contributors may be used to endorse or promote products derived
- *       from this software without specific prior written permission.
- *
- *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * Intel PCIe NTB Linux driver
- *
- * Contact Information:
- * Jon Mason <jon.mason@intel.com>
- */
-#include <linux/debugfs.h>
-#include <linux/delay.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/module.h>
-#include <linux/pci.h>
-#include <linux/random.h>
-#include <linux/slab.h>
-#include "ntb_hw.h"
-#include "ntb_regs.h"
-
-#define NTB_NAME	"Intel(R) PCI-E Non-Transparent Bridge Driver"
-#define NTB_VER		"1.0"
-
-MODULE_DESCRIPTION(NTB_NAME);
-MODULE_VERSION(NTB_VER);
-MODULE_LICENSE("Dual BSD/GPL");
-MODULE_AUTHOR("Intel Corporation");
-
-enum {
-	NTB_CONN_TRANSPARENT = 0,
-	NTB_CONN_B2B,
-	NTB_CONN_RP,
-};
-
-enum {
-	NTB_DEV_USD = 0,
-	NTB_DEV_DSD,
-};
-
-enum {
-	SNB_HW = 0,
-	BWD_HW,
-};
-
-static struct dentry *debugfs_dir;
-
-#define BWD_LINK_RECOVERY_TIME	500
-
-/* Translate memory window 0,1,2 to BAR 2,4,5 */
-#define MW_TO_BAR(mw)	(mw == 0 ? 2 : (mw == 1 ? 4 : 5))
-
-static const struct pci_device_id ntb_pci_tbl[] = {
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_BWD)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_JSF)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_SNB)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_IVT)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_HSX)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_PS_JSF)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_PS_SNB)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_PS_IVT)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_PS_HSX)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_SS_JSF)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_SS_SNB)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_SS_IVT)},
-	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_SS_HSX)},
-	{0}
-};
-MODULE_DEVICE_TABLE(pci, ntb_pci_tbl);
-
-static int is_ntb_xeon(struct ntb_device *ndev)
-{
-	switch (ndev->pdev->device) {
-	case PCI_DEVICE_ID_INTEL_NTB_SS_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
-		return 1;
-	default:
-		return 0;
-	}
-
-	return 0;
-}
-
-static int is_ntb_atom(struct ntb_device *ndev)
-{
-	switch (ndev->pdev->device) {
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_BWD:
-		return 1;
-	default:
-		return 0;
-	}
-
-	return 0;
-}
-
-static void ntb_set_errata_flags(struct ntb_device *ndev)
-{
-	switch (ndev->pdev->device) {
-	/*
-	 * this workaround applies to all platform up to IvyBridge
-	 * Haswell has splitbar support and use a different workaround
-	 */
-	case PCI_DEVICE_ID_INTEL_NTB_SS_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
-		ndev->wa_flags |= WA_SNB_ERR;
-		break;
-	}
-}
-
-/**
- * ntb_register_event_callback() - register event callback
- * @ndev: pointer to ntb_device instance
- * @func: callback function to register
- *
- * This function registers a callback for any HW driver events such as link
- * up/down, power management notices and etc.
- *
- * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
- */
-int ntb_register_event_callback(struct ntb_device *ndev,
-				void (*func)(void *handle,
-					     enum ntb_hw_event event))
-{
-	if (ndev->event_cb)
-		return -EINVAL;
-
-	ndev->event_cb = func;
-
-	return 0;
-}
-
-/**
- * ntb_unregister_event_callback() - unregisters the event callback
- * @ndev: pointer to ntb_device instance
- *
- * This function unregisters the existing callback from transport
- */
-void ntb_unregister_event_callback(struct ntb_device *ndev)
-{
-	ndev->event_cb = NULL;
-}
-
-static void ntb_irq_work(unsigned long data)
-{
-	struct ntb_db_cb *db_cb = (struct ntb_db_cb *)data;
-	int rc;
-
-	rc = db_cb->callback(db_cb->data, db_cb->db_num);
-	if (rc)
-		tasklet_schedule(&db_cb->irq_work);
-	else {
-		struct ntb_device *ndev = db_cb->ndev;
-		unsigned long mask;
-
-		mask = readw(ndev->reg_ofs.ldb_mask);
-		clear_bit(db_cb->db_num * ndev->bits_per_vector, &mask);
-		writew(mask, ndev->reg_ofs.ldb_mask);
-	}
-}
-
-/**
- * ntb_register_db_callback() - register a callback for doorbell interrupt
- * @ndev: pointer to ntb_device instance
- * @idx: doorbell index to register callback, zero based
- * @data: pointer to be returned to caller with every callback
- * @func: callback function to register
- *
- * This function registers a callback function for the doorbell interrupt
- * on the primary side. The function will unmask the doorbell as well to
- * allow interrupt.
- *
- * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
- */
-int ntb_register_db_callback(struct ntb_device *ndev, unsigned int idx,
-			     void *data, int (*func)(void *data, int db_num))
-{
-	unsigned long mask;
-
-	if (idx >= ndev->max_cbs || ndev->db_cb[idx].callback) {
-		dev_warn(&ndev->pdev->dev, "Invalid Index.\n");
-		return -EINVAL;
-	}
-
-	ndev->db_cb[idx].callback = func;
-	ndev->db_cb[idx].data = data;
-	ndev->db_cb[idx].ndev = ndev;
-
-	tasklet_init(&ndev->db_cb[idx].irq_work, ntb_irq_work,
-		     (unsigned long) &ndev->db_cb[idx]);
-
-	/* unmask interrupt */
-	mask = readw(ndev->reg_ofs.ldb_mask);
-	clear_bit(idx * ndev->bits_per_vector, &mask);
-	writew(mask, ndev->reg_ofs.ldb_mask);
-
-	return 0;
-}
-
-/**
- * ntb_unregister_db_callback() - unregister a callback for doorbell interrupt
- * @ndev: pointer to ntb_device instance
- * @idx: doorbell index to register callback, zero based
- *
- * This function unregisters a callback function for the doorbell interrupt
- * on the primary side. The function will also mask the said doorbell.
- */
-void ntb_unregister_db_callback(struct ntb_device *ndev, unsigned int idx)
-{
-	unsigned long mask;
-
-	if (idx >= ndev->max_cbs || !ndev->db_cb[idx].callback)
-		return;
-
-	mask = readw(ndev->reg_ofs.ldb_mask);
-	set_bit(idx * ndev->bits_per_vector, &mask);
-	writew(mask, ndev->reg_ofs.ldb_mask);
-
-	tasklet_disable(&ndev->db_cb[idx].irq_work);
-
-	ndev->db_cb[idx].callback = NULL;
-}
-
-/**
- * ntb_find_transport() - find the transport pointer
- * @transport: pointer to pci device
- *
- * Given the pci device pointer, return the transport pointer passed in when
- * the transport attached when it was inited.
- *
- * RETURNS: pointer to transport.
- */
-void *ntb_find_transport(struct pci_dev *pdev)
-{
-	struct ntb_device *ndev = pci_get_drvdata(pdev);
-	return ndev->ntb_transport;
-}
-
-/**
- * ntb_register_transport() - Register NTB transport with NTB HW driver
- * @transport: transport identifier
- *
- * This function allows a transport to reserve the hardware driver for
- * NTB usage.
- *
- * RETURNS: pointer to ntb_device, NULL on error.
- */
-struct ntb_device *ntb_register_transport(struct pci_dev *pdev, void *transport)
-{
-	struct ntb_device *ndev = pci_get_drvdata(pdev);
-
-	if (ndev->ntb_transport)
-		return NULL;
-
-	ndev->ntb_transport = transport;
-	return ndev;
-}
-
-/**
- * ntb_unregister_transport() - Unregister the transport with the NTB HW driver
- * @ndev - ntb_device of the transport to be freed
- *
- * This function unregisters the transport from the HW driver and performs any
- * necessary cleanups.
- */
-void ntb_unregister_transport(struct ntb_device *ndev)
-{
-	int i;
-
-	if (!ndev->ntb_transport)
-		return;
-
-	for (i = 0; i < ndev->max_cbs; i++)
-		ntb_unregister_db_callback(ndev, i);
-
-	ntb_unregister_event_callback(ndev);
-	ndev->ntb_transport = NULL;
-}
-
-/**
- * ntb_write_local_spad() - write to the secondary scratchpad register
- * @ndev: pointer to ntb_device instance
- * @idx: index to the scratchpad register, 0 based
- * @val: the data value to put into the register
- *
- * This function allows writing of a 32bit value to the indexed scratchpad
- * register. This writes over the data mirrored to the local scratchpad register
- * by the remote system.
- *
- * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
- */
-int ntb_write_local_spad(struct ntb_device *ndev, unsigned int idx, u32 val)
-{
-	if (idx >= ndev->limits.max_spads)
-		return -EINVAL;
-
-	dev_dbg(&ndev->pdev->dev, "Writing %x to local scratch pad index %d\n",
-		val, idx);
-	writel(val, ndev->reg_ofs.spad_read + idx * 4);
-
-	return 0;
-}
-
-/**
- * ntb_read_local_spad() - read from the primary scratchpad register
- * @ndev: pointer to ntb_device instance
- * @idx: index to scratchpad register, 0 based
- * @val: pointer to 32bit integer for storing the register value
- *
- * This function allows reading of the 32bit scratchpad register on
- * the primary (internal) side.  This allows the local system to read data
- * written and mirrored to the scratchpad register by the remote system.
- *
- * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
- */
-int ntb_read_local_spad(struct ntb_device *ndev, unsigned int idx, u32 *val)
-{
-	if (idx >= ndev->limits.max_spads)
-		return -EINVAL;
-
-	*val = readl(ndev->reg_ofs.spad_write + idx * 4);
-	dev_dbg(&ndev->pdev->dev,
-		"Reading %x from local scratch pad index %d\n", *val, idx);
-
-	return 0;
-}
-
-/**
- * ntb_write_remote_spad() - write to the secondary scratchpad register
- * @ndev: pointer to ntb_device instance
- * @idx: index to the scratchpad register, 0 based
- * @val: the data value to put into the register
- *
- * This function allows writing of a 32bit value to the indexed scratchpad
- * register. The register resides on the secondary (external) side.  This allows
- * the local system to write data to be mirrored to the remote systems
- * scratchpad register.
- *
- * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
- */
-int ntb_write_remote_spad(struct ntb_device *ndev, unsigned int idx, u32 val)
-{
-	if (idx >= ndev->limits.max_spads)
-		return -EINVAL;
-
-	dev_dbg(&ndev->pdev->dev, "Writing %x to remote scratch pad index %d\n",
-		val, idx);
-	writel(val, ndev->reg_ofs.spad_write + idx * 4);
-
-	return 0;
-}
-
-/**
- * ntb_read_remote_spad() - read from the primary scratchpad register
- * @ndev: pointer to ntb_device instance
- * @idx: index to scratchpad register, 0 based
- * @val: pointer to 32bit integer for storing the register value
- *
- * This function allows reading of the 32bit scratchpad register on
- * the primary (internal) side.  This alloows the local system to read the data
- * it wrote to be mirrored on the remote system.
- *
- * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
- */
-int ntb_read_remote_spad(struct ntb_device *ndev, unsigned int idx, u32 *val)
-{
-	if (idx >= ndev->limits.max_spads)
-		return -EINVAL;
-
-	*val = readl(ndev->reg_ofs.spad_read + idx * 4);
-	dev_dbg(&ndev->pdev->dev,
-		"Reading %x from remote scratch pad index %d\n", *val, idx);
-
-	return 0;
-}
-
-/**
- * ntb_get_mw_base() - get addr for the NTB memory window
- * @ndev: pointer to ntb_device instance
- * @mw: memory window number
- *
- * This function provides the base address of the memory window specified.
- *
- * RETURNS: address, or NULL on error.
- */
-resource_size_t ntb_get_mw_base(struct ntb_device *ndev, unsigned int mw)
-{
-	if (mw >= ntb_max_mw(ndev))
-		return 0;
-
-	return pci_resource_start(ndev->pdev, MW_TO_BAR(mw));
-}
-
-/**
- * ntb_get_mw_vbase() - get virtual addr for the NTB memory window
- * @ndev: pointer to ntb_device instance
- * @mw: memory window number
- *
- * This function provides the base virtual address of the memory window
- * specified.
- *
- * RETURNS: pointer to virtual address, or NULL on error.
- */
-void __iomem *ntb_get_mw_vbase(struct ntb_device *ndev, unsigned int mw)
-{
-	if (mw >= ntb_max_mw(ndev))
-		return NULL;
-
-	return ndev->mw[mw].vbase;
-}
-
-/**
- * ntb_get_mw_size() - return size of NTB memory window
- * @ndev: pointer to ntb_device instance
- * @mw: memory window number
- *
- * This function provides the physical size of the memory window specified
- *
- * RETURNS: the size of the memory window or zero on error
- */
-u64 ntb_get_mw_size(struct ntb_device *ndev, unsigned int mw)
-{
-	if (mw >= ntb_max_mw(ndev))
-		return 0;
-
-	return ndev->mw[mw].bar_sz;
-}
-
-/**
- * ntb_set_mw_addr - set the memory window address
- * @ndev: pointer to ntb_device instance
- * @mw: memory window number
- * @addr: base address for data
- *
- * This function sets the base physical address of the memory window.  This
- * memory address is where data from the remote system will be transfered into
- * or out of depending on how the transport is configured.
- */
-void ntb_set_mw_addr(struct ntb_device *ndev, unsigned int mw, u64 addr)
-{
-	if (mw >= ntb_max_mw(ndev))
-		return;
-
-	dev_dbg(&ndev->pdev->dev, "Writing addr %Lx to BAR %d\n", addr,
-		MW_TO_BAR(mw));
-
-	ndev->mw[mw].phys_addr = addr;
-
-	switch (MW_TO_BAR(mw)) {
-	case NTB_BAR_23:
-		writeq(addr, ndev->reg_ofs.bar2_xlat);
-		break;
-	case NTB_BAR_4:
-		if (ndev->split_bar)
-			writel(addr, ndev->reg_ofs.bar4_xlat);
-		else
-			writeq(addr, ndev->reg_ofs.bar4_xlat);
-		break;
-	case NTB_BAR_5:
-		writel(addr, ndev->reg_ofs.bar5_xlat);
-		break;
-	}
-}
-
-/**
- * ntb_ring_doorbell() - Set the doorbell on the secondary/external side
- * @ndev: pointer to ntb_device instance
- * @db: doorbell to ring
- *
- * This function allows triggering of a doorbell on the secondary/external
- * side that will initiate an interrupt on the remote host
- *
- * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
- */
-void ntb_ring_doorbell(struct ntb_device *ndev, unsigned int db)
-{
-	dev_dbg(&ndev->pdev->dev, "%s: ringing doorbell %d\n", __func__, db);
-
-	if (ndev->hw_type == BWD_HW)
-		writeq((u64) 1 << db, ndev->reg_ofs.rdb);
-	else
-		writew(((1 << ndev->bits_per_vector) - 1) <<
-		       (db * ndev->bits_per_vector), ndev->reg_ofs.rdb);
-}
-
-static void bwd_recover_link(struct ntb_device *ndev)
-{
-	u32 status;
-
-	/* Driver resets the NTB ModPhy lanes - magic! */
-	writeb(0xe0, ndev->reg_base + BWD_MODPHY_PCSREG6);
-	writeb(0x40, ndev->reg_base + BWD_MODPHY_PCSREG4);
-	writeb(0x60, ndev->reg_base + BWD_MODPHY_PCSREG4);
-	writeb(0x60, ndev->reg_base + BWD_MODPHY_PCSREG6);
-
-	/* Driver waits 100ms to allow the NTB ModPhy to settle */
-	msleep(100);
-
-	/* Clear AER Errors, write to clear */
-	status = readl(ndev->reg_base + BWD_ERRCORSTS_OFFSET);
-	dev_dbg(&ndev->pdev->dev, "ERRCORSTS = %x\n", status);
-	status &= PCI_ERR_COR_REP_ROLL;
-	writel(status, ndev->reg_base + BWD_ERRCORSTS_OFFSET);
-
-	/* Clear unexpected electrical idle event in LTSSM, write to clear */
-	status = readl(ndev->reg_base + BWD_LTSSMERRSTS0_OFFSET);
-	dev_dbg(&ndev->pdev->dev, "LTSSMERRSTS0 = %x\n", status);
-	status |= BWD_LTSSMERRSTS0_UNEXPECTEDEI;
-	writel(status, ndev->reg_base + BWD_LTSSMERRSTS0_OFFSET);
-
-	/* Clear DeSkew Buffer error, write to clear */
-	status = readl(ndev->reg_base + BWD_DESKEWSTS_OFFSET);
-	dev_dbg(&ndev->pdev->dev, "DESKEWSTS = %x\n", status);
-	status |= BWD_DESKEWSTS_DBERR;
-	writel(status, ndev->reg_base + BWD_DESKEWSTS_OFFSET);
-
-	status = readl(ndev->reg_base + BWD_IBSTERRRCRVSTS0_OFFSET);
-	dev_dbg(&ndev->pdev->dev, "IBSTERRRCRVSTS0 = %x\n", status);
-	status &= BWD_IBIST_ERR_OFLOW;
-	writel(status, ndev->reg_base + BWD_IBSTERRRCRVSTS0_OFFSET);
-
-	/* Releases the NTB state machine to allow the link to retrain */
-	status = readl(ndev->reg_base + BWD_LTSSMSTATEJMP_OFFSET);
-	dev_dbg(&ndev->pdev->dev, "LTSSMSTATEJMP = %x\n", status);
-	status &= ~BWD_LTSSMSTATEJMP_FORCEDETECT;
-	writel(status, ndev->reg_base + BWD_LTSSMSTATEJMP_OFFSET);
-}
-
-static void ntb_link_event(struct ntb_device *ndev, int link_state)
-{
-	unsigned int event;
-
-	if (ndev->link_status == link_state)
-		return;
-
-	if (link_state == NTB_LINK_UP) {
-		u16 status;
-
-		dev_info(&ndev->pdev->dev, "Link Up\n");
-		ndev->link_status = NTB_LINK_UP;
-		event = NTB_EVENT_HW_LINK_UP;
-
-		if (is_ntb_atom(ndev) ||
-		    ndev->conn_type == NTB_CONN_TRANSPARENT)
-			status = readw(ndev->reg_ofs.lnk_stat);
-		else {
-			int rc = pci_read_config_word(ndev->pdev,
-						      SNB_LINK_STATUS_OFFSET,
-						      &status);
-			if (rc)
-				return;
-		}
-
-		ndev->link_width = (status & NTB_LINK_WIDTH_MASK) >> 4;
-		ndev->link_speed = (status & NTB_LINK_SPEED_MASK);
-		dev_info(&ndev->pdev->dev, "Link Width %d, Link Speed %d\n",
-			 ndev->link_width, ndev->link_speed);
-	} else {
-		dev_info(&ndev->pdev->dev, "Link Down\n");
-		ndev->link_status = NTB_LINK_DOWN;
-		event = NTB_EVENT_HW_LINK_DOWN;
-		/* Don't modify link width/speed, we need it in link recovery */
-	}
-
-	/* notify the upper layer if we have an event change */
-	if (ndev->event_cb)
-		ndev->event_cb(ndev->ntb_transport, event);
-}
-
-static int ntb_link_status(struct ntb_device *ndev)
-{
-	int link_state;
-
-	if (is_ntb_atom(ndev)) {
-		u32 ntb_cntl;
-
-		ntb_cntl = readl(ndev->reg_ofs.lnk_cntl);
-		if (ntb_cntl & BWD_CNTL_LINK_DOWN)
-			link_state = NTB_LINK_DOWN;
-		else
-			link_state = NTB_LINK_UP;
-	} else {
-		u16 status;
-		int rc;
-
-		rc = pci_read_config_word(ndev->pdev, SNB_LINK_STATUS_OFFSET,
-					  &status);
-		if (rc)
-			return rc;
-
-		if (status & NTB_LINK_STATUS_ACTIVE)
-			link_state = NTB_LINK_UP;
-		else
-			link_state = NTB_LINK_DOWN;
-	}
-
-	ntb_link_event(ndev, link_state);
-
-	return 0;
-}
-
-static void bwd_link_recovery(struct work_struct *work)
-{
-	struct ntb_device *ndev = container_of(work, struct ntb_device,
-					       lr_timer.work);
-	u32 status32;
-
-	bwd_recover_link(ndev);
-	/* There is a potential race between the 2 NTB devices recovering at the
-	 * same time.  If the times are the same, the link will not recover and
-	 * the driver will be stuck in this loop forever.  Add a random interval
-	 * to the recovery time to prevent this race.
-	 */
-	msleep(BWD_LINK_RECOVERY_TIME + prandom_u32() % BWD_LINK_RECOVERY_TIME);
-
-	status32 = readl(ndev->reg_base + BWD_LTSSMSTATEJMP_OFFSET);
-	if (status32 & BWD_LTSSMSTATEJMP_FORCEDETECT)
-		goto retry;
-
-	status32 = readl(ndev->reg_base + BWD_IBSTERRRCRVSTS0_OFFSET);
-	if (status32 & BWD_IBIST_ERR_OFLOW)
-		goto retry;
-
-	status32 = readl(ndev->reg_ofs.lnk_cntl);
-	if (!(status32 & BWD_CNTL_LINK_DOWN)) {
-		unsigned char speed, width;
-		u16 status16;
-
-		status16 = readw(ndev->reg_ofs.lnk_stat);
-		width = (status16 & NTB_LINK_WIDTH_MASK) >> 4;
-		speed = (status16 & NTB_LINK_SPEED_MASK);
-		if (ndev->link_width != width || ndev->link_speed != speed)
-			goto retry;
-	}
-
-	schedule_delayed_work(&ndev->hb_timer, NTB_HB_TIMEOUT);
-	return;
-
-retry:
-	schedule_delayed_work(&ndev->lr_timer, NTB_HB_TIMEOUT);
-}
-
-/* BWD doesn't have link status interrupt, poll on that platform */
-static void bwd_link_poll(struct work_struct *work)
-{
-	struct ntb_device *ndev = container_of(work, struct ntb_device,
-					       hb_timer.work);
-	unsigned long ts = jiffies;
-
-	/* If we haven't gotten an interrupt in a while, check the BWD link
-	 * status bit
-	 */
-	if (ts > ndev->last_ts + NTB_HB_TIMEOUT) {
-		int rc = ntb_link_status(ndev);
-		if (rc)
-			dev_err(&ndev->pdev->dev,
-				"Error determining link status\n");
-
-		/* Check to see if a link error is the cause of the link down */
-		if (ndev->link_status == NTB_LINK_DOWN) {
-			u32 status32 = readl(ndev->reg_base +
-					     BWD_LTSSMSTATEJMP_OFFSET);
-			if (status32 & BWD_LTSSMSTATEJMP_FORCEDETECT) {
-				schedule_delayed_work(&ndev->lr_timer, 0);
-				return;
-			}
-		}
-	}
-
-	schedule_delayed_work(&ndev->hb_timer, NTB_HB_TIMEOUT);
-}
-
-static int ntb_xeon_setup(struct ntb_device *ndev)
-{
-	switch (ndev->conn_type) {
-	case NTB_CONN_B2B:
-		ndev->reg_ofs.ldb = ndev->reg_base + SNB_PDOORBELL_OFFSET;
-		ndev->reg_ofs.ldb_mask = ndev->reg_base + SNB_PDBMSK_OFFSET;
-		ndev->reg_ofs.spad_read = ndev->reg_base + SNB_SPAD_OFFSET;
-		ndev->reg_ofs.bar2_xlat = ndev->reg_base + SNB_SBAR2XLAT_OFFSET;
-		ndev->reg_ofs.bar4_xlat = ndev->reg_base + SNB_SBAR4XLAT_OFFSET;
-		if (ndev->split_bar)
-			ndev->reg_ofs.bar5_xlat =
-				ndev->reg_base + SNB_SBAR5XLAT_OFFSET;
-		ndev->limits.max_spads = SNB_MAX_B2B_SPADS;
-
-		/* There is a Xeon hardware errata related to writes to
-		 * SDOORBELL or B2BDOORBELL in conjunction with inbound access
-		 * to NTB MMIO Space, which may hang the system.  To workaround
-		 * this use the second memory window to access the interrupt and
-		 * scratch pad registers on the remote system.
-		 */
-		if (ndev->wa_flags & WA_SNB_ERR) {
-			if (!ndev->mw[ndev->limits.max_mw - 1].bar_sz)
-				return -EINVAL;
-
-			ndev->limits.max_db_bits = SNB_MAX_DB_BITS;
-			ndev->reg_ofs.spad_write =
-				ndev->mw[ndev->limits.max_mw - 1].vbase +
-				SNB_SPAD_OFFSET;
-			ndev->reg_ofs.rdb =
-				ndev->mw[ndev->limits.max_mw - 1].vbase +
-				SNB_PDOORBELL_OFFSET;
-
-			/* Set the Limit register to 4k, the minimum size, to
-			 * prevent an illegal access
-			 */
-			writeq(ndev->mw[1].bar_sz + 0x1000, ndev->reg_base +
-			       SNB_PBAR4LMT_OFFSET);
-			/* HW errata on the Limit registers.  They can only be
-			 * written when the base register is 4GB aligned and
-			 * < 32bit.  This should already be the case based on
-			 * the driver defaults, but write the Limit registers
-			 * first just in case.
-			 */
-
-			ndev->limits.max_mw = SNB_ERRATA_MAX_MW;
-		} else {
-			/* HW Errata on bit 14 of b2bdoorbell register.  Writes
-			 * will not be mirrored to the remote system.  Shrink
-			 * the number of bits by one, since bit 14 is the last
-			 * bit.
-			 */
-			ndev->limits.max_db_bits = SNB_MAX_DB_BITS - 1;
-			ndev->reg_ofs.spad_write = ndev->reg_base +
-						   SNB_B2B_SPAD_OFFSET;
-			ndev->reg_ofs.rdb = ndev->reg_base +
-					    SNB_B2B_DOORBELL_OFFSET;
-
-			/* Disable the Limit register, just incase it is set to
-			 * something silly. A 64bit write should handle it
-			 * regardless of whether it has a split BAR or not.
-			 */
-			writeq(0, ndev->reg_base + SNB_PBAR4LMT_OFFSET);
-			/* HW errata on the Limit registers.  They can only be
-			 * written when the base register is 4GB aligned and
-			 * < 32bit.  This should already be the case based on
-			 * the driver defaults, but write the Limit registers
-			 * first just in case.
-			 */
-			if (ndev->split_bar)
-				ndev->limits.max_mw = HSX_SPLITBAR_MAX_MW;
-			else
-				ndev->limits.max_mw = SNB_MAX_MW;
-		}
-
-		/* The Xeon errata workaround requires setting SBAR Base
-		 * addresses to known values, so that the PBAR XLAT can be
-		 * pointed at SBAR0 of the remote system.
-		 */
-		if (ndev->dev_type == NTB_DEV_USD) {
-			writeq(SNB_MBAR23_DSD_ADDR, ndev->reg_base +
-			       SNB_PBAR2XLAT_OFFSET);
-			if (ndev->wa_flags & WA_SNB_ERR)
-				writeq(SNB_MBAR01_DSD_ADDR, ndev->reg_base +
-				       SNB_PBAR4XLAT_OFFSET);
-			else {
-				if (ndev->split_bar) {
-					writel(SNB_MBAR4_DSD_ADDR,
-					       ndev->reg_base +
-					       SNB_PBAR4XLAT_OFFSET);
-					writel(SNB_MBAR5_DSD_ADDR,
-					       ndev->reg_base +
-					       SNB_PBAR5XLAT_OFFSET);
-				} else
-					writeq(SNB_MBAR4_DSD_ADDR,
-					       ndev->reg_base +
-					       SNB_PBAR4XLAT_OFFSET);
-
-				/* B2B_XLAT_OFFSET is a 64bit register, but can
-				 * only take 32bit writes
-				 */
-				writel(SNB_MBAR01_DSD_ADDR & 0xffffffff,
-				       ndev->reg_base + SNB_B2B_XLAT_OFFSETL);
-				writel(SNB_MBAR01_DSD_ADDR >> 32,
-				       ndev->reg_base + SNB_B2B_XLAT_OFFSETU);
-			}
-
-			writeq(SNB_MBAR01_USD_ADDR, ndev->reg_base +
-			       SNB_SBAR0BASE_OFFSET);
-			writeq(SNB_MBAR23_USD_ADDR, ndev->reg_base +
-			       SNB_SBAR2BASE_OFFSET);
-			if (ndev->split_bar) {
-				writel(SNB_MBAR4_USD_ADDR, ndev->reg_base +
-				       SNB_SBAR4BASE_OFFSET);
-				writel(SNB_MBAR5_USD_ADDR, ndev->reg_base +
-				       SNB_SBAR5BASE_OFFSET);
-			} else
-				writeq(SNB_MBAR4_USD_ADDR, ndev->reg_base +
-				       SNB_SBAR4BASE_OFFSET);
-		} else {
-			writeq(SNB_MBAR23_USD_ADDR, ndev->reg_base +
-			       SNB_PBAR2XLAT_OFFSET);
-			if (ndev->wa_flags & WA_SNB_ERR)
-				writeq(SNB_MBAR01_USD_ADDR, ndev->reg_base +
-				       SNB_PBAR4XLAT_OFFSET);
-			else {
-				if (ndev->split_bar) {
-					writel(SNB_MBAR4_USD_ADDR,
-					       ndev->reg_base +
-					       SNB_PBAR4XLAT_OFFSET);
-					writel(SNB_MBAR5_USD_ADDR,
-					       ndev->reg_base +
-					       SNB_PBAR5XLAT_OFFSET);
-				} else
-					writeq(SNB_MBAR4_USD_ADDR,
-					       ndev->reg_base +
-					       SNB_PBAR4XLAT_OFFSET);
-
-				/*
-				 * B2B_XLAT_OFFSET is a 64bit register, but can
-				 * only take 32bit writes
-				 */
-				writel(SNB_MBAR01_USD_ADDR & 0xffffffff,
-				       ndev->reg_base + SNB_B2B_XLAT_OFFSETL);
-				writel(SNB_MBAR01_USD_ADDR >> 32,
-				       ndev->reg_base + SNB_B2B_XLAT_OFFSETU);
-			}
-			writeq(SNB_MBAR01_DSD_ADDR, ndev->reg_base +
-			       SNB_SBAR0BASE_OFFSET);
-			writeq(SNB_MBAR23_DSD_ADDR, ndev->reg_base +
-			       SNB_SBAR2BASE_OFFSET);
-			if (ndev->split_bar) {
-				writel(SNB_MBAR4_DSD_ADDR, ndev->reg_base +
-				       SNB_SBAR4BASE_OFFSET);
-				writel(SNB_MBAR5_DSD_ADDR, ndev->reg_base +
-				       SNB_SBAR5BASE_OFFSET);
-			} else
-				writeq(SNB_MBAR4_DSD_ADDR, ndev->reg_base +
-				       SNB_SBAR4BASE_OFFSET);
-
-		}
-		break;
-	case NTB_CONN_RP:
-		if (ndev->wa_flags & WA_SNB_ERR) {
-			dev_err(&ndev->pdev->dev,
-				"NTB-RP disabled due to hardware errata.\n");
-			return -EINVAL;
-		}
-
-		/* Scratch pads need to have exclusive access from the primary
-		 * or secondary side.  Halve the num spads so that each side can
-		 * have an equal amount.
-		 */
-		ndev->limits.max_spads = SNB_MAX_COMPAT_SPADS / 2;
-		ndev->limits.max_db_bits = SNB_MAX_DB_BITS;
-		/* Note: The SDOORBELL is the cause of the errata.  You REALLY
-		 * don't want to touch it.
-		 */
-		ndev->reg_ofs.rdb = ndev->reg_base + SNB_SDOORBELL_OFFSET;
-		ndev->reg_ofs.ldb = ndev->reg_base + SNB_PDOORBELL_OFFSET;
-		ndev->reg_ofs.ldb_mask = ndev->reg_base + SNB_PDBMSK_OFFSET;
-		/* Offset the start of the spads to correspond to whether it is
-		 * primary or secondary
-		 */
-		ndev->reg_ofs.spad_write = ndev->reg_base + SNB_SPAD_OFFSET +
-					   ndev->limits.max_spads * 4;
-		ndev->reg_ofs.spad_read = ndev->reg_base + SNB_SPAD_OFFSET;
-		ndev->reg_ofs.bar2_xlat = ndev->reg_base + SNB_SBAR2XLAT_OFFSET;
-		ndev->reg_ofs.bar4_xlat = ndev->reg_base + SNB_SBAR4XLAT_OFFSET;
-		if (ndev->split_bar) {
-			ndev->reg_ofs.bar5_xlat =
-				ndev->reg_base + SNB_SBAR5XLAT_OFFSET;
-			ndev->limits.max_mw = HSX_SPLITBAR_MAX_MW;
-		} else
-			ndev->limits.max_mw = SNB_MAX_MW;
-		break;
-	case NTB_CONN_TRANSPARENT:
-		if (ndev->wa_flags & WA_SNB_ERR) {
-			dev_err(&ndev->pdev->dev,
-				"NTB-TRANSPARENT disabled due to hardware errata.\n");
-			return -EINVAL;
-		}
-
-		/* Scratch pads need to have exclusive access from the primary
-		 * or secondary side.  Halve the num spads so that each side can
-		 * have an equal amount.
-		 */
-		ndev->limits.max_spads = SNB_MAX_COMPAT_SPADS / 2;
-		ndev->limits.max_db_bits = SNB_MAX_DB_BITS;
-		ndev->reg_ofs.rdb = ndev->reg_base + SNB_PDOORBELL_OFFSET;
-		ndev->reg_ofs.ldb = ndev->reg_base + SNB_SDOORBELL_OFFSET;
-		ndev->reg_ofs.ldb_mask = ndev->reg_base + SNB_SDBMSK_OFFSET;
-		ndev->reg_ofs.spad_write = ndev->reg_base + SNB_SPAD_OFFSET;
-		/* Offset the start of the spads to correspond to whether it is
-		 * primary or secondary
-		 */
-		ndev->reg_ofs.spad_read = ndev->reg_base + SNB_SPAD_OFFSET +
-					  ndev->limits.max_spads * 4;
-		ndev->reg_ofs.bar2_xlat = ndev->reg_base + SNB_PBAR2XLAT_OFFSET;
-		ndev->reg_ofs.bar4_xlat = ndev->reg_base + SNB_PBAR4XLAT_OFFSET;
-
-		if (ndev->split_bar) {
-			ndev->reg_ofs.bar5_xlat =
-				ndev->reg_base + SNB_PBAR5XLAT_OFFSET;
-			ndev->limits.max_mw = HSX_SPLITBAR_MAX_MW;
-		} else
-			ndev->limits.max_mw = SNB_MAX_MW;
-		break;
-	default:
-		/*
-		 * we should never hit this. the detect function should've
-		 * take cared of everything.
-		 */
-		return -EINVAL;
-	}
-
-	ndev->reg_ofs.lnk_cntl = ndev->reg_base + SNB_NTBCNTL_OFFSET;
-	ndev->reg_ofs.lnk_stat = ndev->reg_base + SNB_SLINK_STATUS_OFFSET;
-	ndev->reg_ofs.spci_cmd = ndev->reg_base + SNB_PCICMD_OFFSET;
-
-	ndev->limits.msix_cnt = SNB_MSIX_CNT;
-	ndev->bits_per_vector = SNB_DB_BITS_PER_VEC;
-
-	return 0;
-}
-
-static int ntb_bwd_setup(struct ntb_device *ndev)
-{
-	int rc;
-	u32 val;
-
-	ndev->hw_type = BWD_HW;
-
-	rc = pci_read_config_dword(ndev->pdev, NTB_PPD_OFFSET, &val);
-	if (rc)
-		return rc;
-
-	switch ((val & BWD_PPD_CONN_TYPE) >> 8) {
-	case NTB_CONN_B2B:
-		ndev->conn_type = NTB_CONN_B2B;
-		break;
-	case NTB_CONN_RP:
-	default:
-		dev_err(&ndev->pdev->dev, "Unsupported NTB configuration\n");
-		return -EINVAL;
-	}
-
-	if (val & BWD_PPD_DEV_TYPE)
-		ndev->dev_type = NTB_DEV_DSD;
-	else
-		ndev->dev_type = NTB_DEV_USD;
-
-	/* Initiate PCI-E link training */
-	rc = pci_write_config_dword(ndev->pdev, NTB_PPD_OFFSET,
-				    val | BWD_PPD_INIT_LINK);
-	if (rc)
-		return rc;
-
-	ndev->reg_ofs.ldb = ndev->reg_base + BWD_PDOORBELL_OFFSET;
-	ndev->reg_ofs.ldb_mask = ndev->reg_base + BWD_PDBMSK_OFFSET;
-	ndev->reg_ofs.rdb = ndev->reg_base + BWD_B2B_DOORBELL_OFFSET;
-	ndev->reg_ofs.bar2_xlat = ndev->reg_base + BWD_SBAR2XLAT_OFFSET;
-	ndev->reg_ofs.bar4_xlat = ndev->reg_base + BWD_SBAR4XLAT_OFFSET;
-	ndev->reg_ofs.lnk_cntl = ndev->reg_base + BWD_NTBCNTL_OFFSET;
-	ndev->reg_ofs.lnk_stat = ndev->reg_base + BWD_LINK_STATUS_OFFSET;
-	ndev->reg_ofs.spad_read = ndev->reg_base + BWD_SPAD_OFFSET;
-	ndev->reg_ofs.spad_write = ndev->reg_base + BWD_B2B_SPAD_OFFSET;
-	ndev->reg_ofs.spci_cmd = ndev->reg_base + BWD_PCICMD_OFFSET;
-	ndev->limits.max_mw = BWD_MAX_MW;
-	ndev->limits.max_spads = BWD_MAX_SPADS;
-	ndev->limits.max_db_bits = BWD_MAX_DB_BITS;
-	ndev->limits.msix_cnt = BWD_MSIX_CNT;
-	ndev->bits_per_vector = BWD_DB_BITS_PER_VEC;
-
-	/* Since bwd doesn't have a link interrupt, setup a poll timer */
-	INIT_DELAYED_WORK(&ndev->hb_timer, bwd_link_poll);
-	INIT_DELAYED_WORK(&ndev->lr_timer, bwd_link_recovery);
-	schedule_delayed_work(&ndev->hb_timer, NTB_HB_TIMEOUT);
-
-	return 0;
-}
-
-static int ntb_device_setup(struct ntb_device *ndev)
-{
-	int rc;
-
-	if (is_ntb_xeon(ndev))
-		rc = ntb_xeon_setup(ndev);
-	else if (is_ntb_atom(ndev))
-		rc = ntb_bwd_setup(ndev);
-	else
-		rc = -ENODEV;
-
-	if (rc)
-		return rc;
-
-	if (ndev->conn_type == NTB_CONN_B2B)
-		/* Enable Bus Master and Memory Space on the secondary side */
-		writew(PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER,
-		       ndev->reg_ofs.spci_cmd);
-
-	return 0;
-}
-
-static void ntb_device_free(struct ntb_device *ndev)
-{
-	if (is_ntb_atom(ndev)) {
-		cancel_delayed_work_sync(&ndev->hb_timer);
-		cancel_delayed_work_sync(&ndev->lr_timer);
-	}
-}
-
-static irqreturn_t bwd_callback_msix_irq(int irq, void *data)
-{
-	struct ntb_db_cb *db_cb = data;
-	struct ntb_device *ndev = db_cb->ndev;
-	unsigned long mask;
-
-	dev_dbg(&ndev->pdev->dev, "MSI-X irq %d received for DB %d\n", irq,
-		db_cb->db_num);
-
-	mask = readw(ndev->reg_ofs.ldb_mask);
-	set_bit(db_cb->db_num * ndev->bits_per_vector, &mask);
-	writew(mask, ndev->reg_ofs.ldb_mask);
-
-	tasklet_schedule(&db_cb->irq_work);
-
-	/* No need to check for the specific HB irq, any interrupt means
-	 * we're connected.
-	 */
-	ndev->last_ts = jiffies;
-
-	writeq((u64) 1 << db_cb->db_num, ndev->reg_ofs.ldb);
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t xeon_callback_msix_irq(int irq, void *data)
-{
-	struct ntb_db_cb *db_cb = data;
-	struct ntb_device *ndev = db_cb->ndev;
-	unsigned long mask;
-
-	dev_dbg(&ndev->pdev->dev, "MSI-X irq %d received for DB %d\n", irq,
-		db_cb->db_num);
-
-	mask = readw(ndev->reg_ofs.ldb_mask);
-	set_bit(db_cb->db_num * ndev->bits_per_vector, &mask);
-	writew(mask, ndev->reg_ofs.ldb_mask);
-
-	tasklet_schedule(&db_cb->irq_work);
-
-	/* On Sandybridge, there are 16 bits in the interrupt register
-	 * but only 4 vectors.  So, 5 bits are assigned to the first 3
-	 * vectors, with the 4th having a single bit for link
-	 * interrupts.
-	 */
-	writew(((1 << ndev->bits_per_vector) - 1) <<
-	       (db_cb->db_num * ndev->bits_per_vector), ndev->reg_ofs.ldb);
-
-	return IRQ_HANDLED;
-}
-
-/* Since we do not have a HW doorbell in BWD, this is only used in JF/JT */
-static irqreturn_t xeon_event_msix_irq(int irq, void *dev)
-{
-	struct ntb_device *ndev = dev;
-	int rc;
-
-	dev_dbg(&ndev->pdev->dev, "MSI-X irq %d received for Events\n", irq);
-
-	rc = ntb_link_status(ndev);
-	if (rc)
-		dev_err(&ndev->pdev->dev, "Error determining link status\n");
-
-	/* bit 15 is always the link bit */
-	writew(1 << SNB_LINK_DB, ndev->reg_ofs.ldb);
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t ntb_interrupt(int irq, void *dev)
-{
-	struct ntb_device *ndev = dev;
-	unsigned int i = 0;
-
-	if (is_ntb_atom(ndev)) {
-		u64 ldb = readq(ndev->reg_ofs.ldb);
-
-		dev_dbg(&ndev->pdev->dev, "irq %d - ldb = %Lx\n", irq, ldb);
-
-		while (ldb) {
-			i = __ffs(ldb);
-			ldb &= ldb - 1;
-			bwd_callback_msix_irq(irq, &ndev->db_cb[i]);
-		}
-	} else {
-		u16 ldb = readw(ndev->reg_ofs.ldb);
-
-		dev_dbg(&ndev->pdev->dev, "irq %d - ldb = %x\n", irq, ldb);
-
-		if (ldb & SNB_DB_HW_LINK) {
-			xeon_event_msix_irq(irq, dev);
-			ldb &= ~SNB_DB_HW_LINK;
-		}
-
-		while (ldb) {
-			i = __ffs(ldb);
-			ldb &= ldb - 1;
-			xeon_callback_msix_irq(irq, &ndev->db_cb[i]);
-		}
-	}
-
-	return IRQ_HANDLED;
-}
-
-static int ntb_setup_snb_msix(struct ntb_device *ndev, int msix_entries)
-{
-	struct pci_dev *pdev = ndev->pdev;
-	struct msix_entry *msix;
-	int rc, i;
-
-	if (msix_entries < ndev->limits.msix_cnt)
-		return -ENOSPC;
-
-	rc = pci_enable_msix_exact(pdev, ndev->msix_entries, msix_entries);
-	if (rc < 0)
-		return rc;
-
-	for (i = 0; i < msix_entries; i++) {
-		msix = &ndev->msix_entries[i];
-		WARN_ON(!msix->vector);
-
-		if (i == msix_entries - 1) {
-			rc = request_irq(msix->vector,
-					 xeon_event_msix_irq, 0,
-					 "ntb-event-msix", ndev);
-			if (rc)
-				goto err;
-		} else {
-			rc = request_irq(msix->vector,
-					 xeon_callback_msix_irq, 0,
-					 "ntb-callback-msix",
-					 &ndev->db_cb[i]);
-			if (rc)
-				goto err;
-		}
-	}
-
-	ndev->num_msix = msix_entries;
-	ndev->max_cbs = msix_entries - 1;
-
-	return 0;
-
-err:
-	while (--i >= 0) {
-		/* Code never reaches here for entry nr 'ndev->num_msix - 1' */
-		msix = &ndev->msix_entries[i];
-		free_irq(msix->vector, &ndev->db_cb[i]);
-	}
-
-	pci_disable_msix(pdev);
-	ndev->num_msix = 0;
-
-	return rc;
-}
-
-static int ntb_setup_bwd_msix(struct ntb_device *ndev, int msix_entries)
-{
-	struct pci_dev *pdev = ndev->pdev;
-	struct msix_entry *msix;
-	int rc, i;
-
-	msix_entries = pci_enable_msix_range(pdev, ndev->msix_entries,
-					     1, msix_entries);
-	if (msix_entries < 0)
-		return msix_entries;
-
-	for (i = 0; i < msix_entries; i++) {
-		msix = &ndev->msix_entries[i];
-		WARN_ON(!msix->vector);
-
-		rc = request_irq(msix->vector, bwd_callback_msix_irq, 0,
-				 "ntb-callback-msix", &ndev->db_cb[i]);
-		if (rc)
-			goto err;
-	}
-
-	ndev->num_msix = msix_entries;
-	ndev->max_cbs = msix_entries;
-
-	return 0;
-
-err:
-	while (--i >= 0)
-		free_irq(msix->vector, &ndev->db_cb[i]);
-
-	pci_disable_msix(pdev);
-	ndev->num_msix = 0;
-
-	return rc;
-}
-
-static int ntb_setup_msix(struct ntb_device *ndev)
-{
-	struct pci_dev *pdev = ndev->pdev;
-	int msix_entries;
-	int rc, i;
-
-	msix_entries = pci_msix_vec_count(pdev);
-	if (msix_entries < 0) {
-		rc = msix_entries;
-		goto err;
-	} else if (msix_entries > ndev->limits.msix_cnt) {
-		rc = -EINVAL;
-		goto err;
-	}
-
-	ndev->msix_entries = kmalloc(sizeof(struct msix_entry) * msix_entries,
-				     GFP_KERNEL);
-	if (!ndev->msix_entries) {
-		rc = -ENOMEM;
-		goto err;
-	}
-
-	for (i = 0; i < msix_entries; i++)
-		ndev->msix_entries[i].entry = i;
-
-	if (is_ntb_atom(ndev))
-		rc = ntb_setup_bwd_msix(ndev, msix_entries);
-	else
-		rc = ntb_setup_snb_msix(ndev, msix_entries);
-	if (rc)
-		goto err1;
-
-	return 0;
-
-err1:
-	kfree(ndev->msix_entries);
-err:
-	dev_err(&pdev->dev, "Error allocating MSI-X interrupt\n");
-	return rc;
-}
-
-static int ntb_setup_msi(struct ntb_device *ndev)
-{
-	struct pci_dev *pdev = ndev->pdev;
-	int rc;
-
-	rc = pci_enable_msi(pdev);
-	if (rc)
-		return rc;
-
-	rc = request_irq(pdev->irq, ntb_interrupt, 0, "ntb-msi", ndev);
-	if (rc) {
-		pci_disable_msi(pdev);
-		dev_err(&pdev->dev, "Error allocating MSI interrupt\n");
-		return rc;
-	}
-
-	return 0;
-}
-
-static int ntb_setup_intx(struct ntb_device *ndev)
-{
-	struct pci_dev *pdev = ndev->pdev;
-	int rc;
-
-	pci_msi_off(pdev);
-
-	/* Verify intx is enabled */
-	pci_intx(pdev, 1);
-
-	rc = request_irq(pdev->irq, ntb_interrupt, IRQF_SHARED, "ntb-intx",
-			 ndev);
-	if (rc)
-		return rc;
-
-	return 0;
-}
-
-static int ntb_setup_interrupts(struct ntb_device *ndev)
-{
-	int rc;
-
-	/* On BWD, disable all interrupts.  On SNB, disable all but Link
-	 * Interrupt.  The rest will be unmasked as callbacks are registered.
-	 */
-	if (is_ntb_atom(ndev))
-		writeq(~0, ndev->reg_ofs.ldb_mask);
-	else {
-		u16 var = 1 << SNB_LINK_DB;
-		writew(~var, ndev->reg_ofs.ldb_mask);
-	}
-
-	rc = ntb_setup_msix(ndev);
-	if (!rc)
-		goto done;
-
-	ndev->bits_per_vector = 1;
-	ndev->max_cbs = ndev->limits.max_db_bits;
-
-	rc = ntb_setup_msi(ndev);
-	if (!rc)
-		goto done;
-
-	rc = ntb_setup_intx(ndev);
-	if (rc) {
-		dev_err(&ndev->pdev->dev, "no usable interrupts\n");
-		return rc;
-	}
-
-done:
-	return 0;
-}
-
-static void ntb_free_interrupts(struct ntb_device *ndev)
-{
-	struct pci_dev *pdev = ndev->pdev;
-
-	/* mask interrupts */
-	if (is_ntb_atom(ndev))
-		writeq(~0, ndev->reg_ofs.ldb_mask);
-	else
-		writew(~0, ndev->reg_ofs.ldb_mask);
-
-	if (ndev->num_msix) {
-		struct msix_entry *msix;
-		u32 i;
-
-		for (i = 0; i < ndev->num_msix; i++) {
-			msix = &ndev->msix_entries[i];
-			if (is_ntb_xeon(ndev) && i == ndev->num_msix - 1)
-				free_irq(msix->vector, ndev);
-			else
-				free_irq(msix->vector, &ndev->db_cb[i]);
-		}
-		pci_disable_msix(pdev);
-		kfree(ndev->msix_entries);
-	} else {
-		free_irq(pdev->irq, ndev);
-
-		if (pci_dev_msi_enabled(pdev))
-			pci_disable_msi(pdev);
-	}
-}
-
-static int ntb_create_callbacks(struct ntb_device *ndev)
-{
-	int i;
-
-	/* Chicken-egg issue.  We won't know how many callbacks are necessary
-	 * until we see how many MSI-X vectors we get, but these pointers need
-	 * to be passed into the MSI-X register function.  So, we allocate the
-	 * max, knowing that they might not all be used, to work around this.
-	 */
-	ndev->db_cb = kcalloc(ndev->limits.max_db_bits,
-			      sizeof(struct ntb_db_cb),
-			      GFP_KERNEL);
-	if (!ndev->db_cb)
-		return -ENOMEM;
-
-	for (i = 0; i < ndev->limits.max_db_bits; i++) {
-		ndev->db_cb[i].db_num = i;
-		ndev->db_cb[i].ndev = ndev;
-	}
-
-	return 0;
-}
-
-static void ntb_free_callbacks(struct ntb_device *ndev)
-{
-	int i;
-
-	for (i = 0; i < ndev->limits.max_db_bits; i++)
-		ntb_unregister_db_callback(ndev, i);
-
-	kfree(ndev->db_cb);
-}
-
-static ssize_t ntb_debugfs_read(struct file *filp, char __user *ubuf,
-				size_t count, loff_t *offp)
-{
-	struct ntb_device *ndev;
-	char *buf;
-	ssize_t ret, offset, out_count;
-
-	out_count = 500;
-
-	buf = kmalloc(out_count, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	ndev = filp->private_data;
-	offset = 0;
-	offset += snprintf(buf + offset, out_count - offset,
-			   "NTB Device Information:\n");
-	offset += snprintf(buf + offset, out_count - offset,
-			   "Connection Type - \t\t%s\n",
-			   ndev->conn_type == NTB_CONN_TRANSPARENT ?
-			   "Transparent" : (ndev->conn_type == NTB_CONN_B2B) ?
-			   "Back to back" : "Root Port");
-	offset += snprintf(buf + offset, out_count - offset,
-			   "Device Type - \t\t\t%s\n",
-			   ndev->dev_type == NTB_DEV_USD ?
-			   "DSD/USP" : "USD/DSP");
-	offset += snprintf(buf + offset, out_count - offset,
-			   "Max Number of Callbacks - \t%u\n",
-			   ntb_max_cbs(ndev));
-	offset += snprintf(buf + offset, out_count - offset,
-			   "Link Status - \t\t\t%s\n",
-			   ntb_hw_link_status(ndev) ? "Up" : "Down");
-	if (ntb_hw_link_status(ndev)) {
-		offset += snprintf(buf + offset, out_count - offset,
-				   "Link Speed - \t\t\tPCI-E Gen %u\n",
-				   ndev->link_speed);
-		offset += snprintf(buf + offset, out_count - offset,
-				   "Link Width - \t\t\tx%u\n",
-				   ndev->link_width);
-	}
-
-	if (is_ntb_xeon(ndev)) {
-		u32 status32;
-		u16 status16;
-		int rc;
-
-		offset += snprintf(buf + offset, out_count - offset,
-				   "\nNTB Device Statistics:\n");
-		offset += snprintf(buf + offset, out_count - offset,
-				   "Upstream Memory Miss - \t%u\n",
-				   readw(ndev->reg_base +
-					 SNB_USMEMMISS_OFFSET));
-
-		offset += snprintf(buf + offset, out_count - offset,
-				   "\nNTB Hardware Errors:\n");
-
-		rc = pci_read_config_word(ndev->pdev, SNB_DEVSTS_OFFSET,
-					  &status16);
-		if (!rc)
-			offset += snprintf(buf + offset, out_count - offset,
-					   "DEVSTS - \t%#06x\n", status16);
-
-		rc = pci_read_config_word(ndev->pdev, SNB_LINK_STATUS_OFFSET,
-					  &status16);
-		if (!rc)
-			offset += snprintf(buf + offset, out_count - offset,
-					   "LNKSTS - \t%#06x\n", status16);
-
-		rc = pci_read_config_dword(ndev->pdev, SNB_UNCERRSTS_OFFSET,
-					   &status32);
-		if (!rc)
-			offset += snprintf(buf + offset, out_count - offset,
-					   "UNCERRSTS - \t%#010x\n", status32);
-
-		rc = pci_read_config_dword(ndev->pdev, SNB_CORERRSTS_OFFSET,
-					   &status32);
-		if (!rc)
-			offset += snprintf(buf + offset, out_count - offset,
-					   "CORERRSTS - \t%#010x\n", status32);
-	}
-
-	if (offset > out_count)
-		offset = out_count;
-
-	ret = simple_read_from_buffer(ubuf, count, offp, buf, offset);
-	kfree(buf);
-	return ret;
-}
-
-static const struct file_operations ntb_debugfs_info = {
-	.owner = THIS_MODULE,
-	.open = simple_open,
-	.read = ntb_debugfs_read,
-};
-
-static void ntb_setup_debugfs(struct ntb_device *ndev)
-{
-	if (!debugfs_initialized())
-		return;
-
-	if (!debugfs_dir)
-		debugfs_dir = debugfs_create_dir(KBUILD_MODNAME, NULL);
-
-	ndev->debugfs_dir = debugfs_create_dir(pci_name(ndev->pdev),
-					       debugfs_dir);
-	if (ndev->debugfs_dir)
-		ndev->debugfs_info = debugfs_create_file("info", S_IRUSR,
-							 ndev->debugfs_dir,
-							 ndev,
-							 &ntb_debugfs_info);
-}
-
-static void ntb_free_debugfs(struct ntb_device *ndev)
-{
-	debugfs_remove_recursive(ndev->debugfs_dir);
-
-	if (debugfs_dir && simple_empty(debugfs_dir)) {
-		debugfs_remove_recursive(debugfs_dir);
-		debugfs_dir = NULL;
-	}
-}
-
-static void ntb_hw_link_up(struct ntb_device *ndev)
-{
-	if (ndev->conn_type == NTB_CONN_TRANSPARENT)
-		ntb_link_event(ndev, NTB_LINK_UP);
-	else {
-		u32 ntb_cntl;
-
-		/* Let's bring the NTB link up */
-		ntb_cntl = readl(ndev->reg_ofs.lnk_cntl);
-		ntb_cntl &= ~(NTB_CNTL_LINK_DISABLE | NTB_CNTL_CFG_LOCK);
-		ntb_cntl |= NTB_CNTL_P2S_BAR23_SNOOP | NTB_CNTL_S2P_BAR23_SNOOP;
-		ntb_cntl |= NTB_CNTL_P2S_BAR4_SNOOP | NTB_CNTL_S2P_BAR4_SNOOP;
-		if (ndev->split_bar)
-			ntb_cntl |= NTB_CNTL_P2S_BAR5_SNOOP |
-				    NTB_CNTL_S2P_BAR5_SNOOP;
-
-		writel(ntb_cntl, ndev->reg_ofs.lnk_cntl);
-	}
-}
-
-static void ntb_hw_link_down(struct ntb_device *ndev)
-{
-	u32 ntb_cntl;
-
-	if (ndev->conn_type == NTB_CONN_TRANSPARENT) {
-		ntb_link_event(ndev, NTB_LINK_DOWN);
-		return;
-	}
-
-	/* Bring NTB link down */
-	ntb_cntl = readl(ndev->reg_ofs.lnk_cntl);
-	ntb_cntl &= ~(NTB_CNTL_P2S_BAR23_SNOOP | NTB_CNTL_S2P_BAR23_SNOOP);
-	ntb_cntl &= ~(NTB_CNTL_P2S_BAR4_SNOOP | NTB_CNTL_S2P_BAR4_SNOOP);
-	if (ndev->split_bar)
-		ntb_cntl &= ~(NTB_CNTL_P2S_BAR5_SNOOP |
-			      NTB_CNTL_S2P_BAR5_SNOOP);
-	ntb_cntl |= NTB_CNTL_LINK_DISABLE | NTB_CNTL_CFG_LOCK;
-	writel(ntb_cntl, ndev->reg_ofs.lnk_cntl);
-}
-
-static void ntb_max_mw_detect(struct ntb_device *ndev)
-{
-	if (ndev->split_bar)
-		ndev->limits.max_mw = HSX_SPLITBAR_MAX_MW;
-	else
-		ndev->limits.max_mw = SNB_MAX_MW;
-}
-
-static int ntb_xeon_detect(struct ntb_device *ndev)
-{
-	int rc, bars_mask;
-	u32 bars;
-	u8 ppd;
-
-	ndev->hw_type = SNB_HW;
-
-	rc = pci_read_config_byte(ndev->pdev, NTB_PPD_OFFSET, &ppd);
-	if (rc)
-		return -EIO;
-
-	if (ppd & SNB_PPD_DEV_TYPE)
-		ndev->dev_type = NTB_DEV_USD;
-	else
-		ndev->dev_type = NTB_DEV_DSD;
-
-	ndev->split_bar = (ppd & SNB_PPD_SPLIT_BAR) ? 1 : 0;
-
-	switch (ppd & SNB_PPD_CONN_TYPE) {
-	case NTB_CONN_B2B:
-		dev_info(&ndev->pdev->dev, "Conn Type = B2B\n");
-		ndev->conn_type = NTB_CONN_B2B;
-		break;
-	case NTB_CONN_RP:
-		dev_info(&ndev->pdev->dev, "Conn Type = RP\n");
-		ndev->conn_type = NTB_CONN_RP;
-		break;
-	case NTB_CONN_TRANSPARENT:
-		dev_info(&ndev->pdev->dev, "Conn Type = TRANSPARENT\n");
-		ndev->conn_type = NTB_CONN_TRANSPARENT;
-		/*
-		 * This mode is default to USD/DSP. HW does not report
-		 * properly in transparent mode as it has no knowledge of
-		 * NTB. We will just force correct here.
-		 */
-		ndev->dev_type = NTB_DEV_USD;
-
-		/*
-		 * This is a way for transparent BAR to figure out if we
-		 * are doing split BAR or not. There is no way for the hw
-		 * on the transparent side to know and set the PPD.
-		 */
-		bars_mask = pci_select_bars(ndev->pdev, IORESOURCE_MEM);
-		bars = hweight32(bars_mask);
-		if (bars == (HSX_SPLITBAR_MAX_MW + 1))
-			ndev->split_bar = 1;
-
-		break;
-	default:
-		dev_err(&ndev->pdev->dev, "Unknown PPD %x\n", ppd);
-		return -ENODEV;
-	}
-
-	ntb_max_mw_detect(ndev);
-
-	return 0;
-}
-
-static int ntb_atom_detect(struct ntb_device *ndev)
-{
-	int rc;
-	u32 ppd;
-
-	ndev->hw_type = BWD_HW;
-
-	rc = pci_read_config_dword(ndev->pdev, NTB_PPD_OFFSET, &ppd);
-	if (rc)
-		return rc;
-
-	switch ((ppd & BWD_PPD_CONN_TYPE) >> 8) {
-	case NTB_CONN_B2B:
-		dev_info(&ndev->pdev->dev, "Conn Type = B2B\n");
-		ndev->conn_type = NTB_CONN_B2B;
-		break;
-	case NTB_CONN_RP:
-	default:
-		dev_err(&ndev->pdev->dev, "Unsupported NTB configuration\n");
-		return -EINVAL;
-	}
-
-	if (ppd & BWD_PPD_DEV_TYPE)
-		ndev->dev_type = NTB_DEV_DSD;
-	else
-		ndev->dev_type = NTB_DEV_USD;
-
-	return 0;
-}
-
-static int ntb_device_detect(struct ntb_device *ndev)
-{
-	int rc;
-
-	if (is_ntb_xeon(ndev))
-		rc = ntb_xeon_detect(ndev);
-	else if (is_ntb_atom(ndev))
-		rc = ntb_atom_detect(ndev);
-	else
-		rc = -ENODEV;
-
-	dev_info(&ndev->pdev->dev, "Device Type = %s\n",
-		 ndev->dev_type == NTB_DEV_USD ? "USD/DSP" : "DSD/USP");
-
-	return 0;
-}
-
-static int ntb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
-{
-	struct ntb_device *ndev;
-	int rc, i;
-
-	ndev = kzalloc(sizeof(struct ntb_device), GFP_KERNEL);
-	if (!ndev)
-		return -ENOMEM;
-
-	ndev->pdev = pdev;
-
-	ntb_set_errata_flags(ndev);
-
-	ndev->link_status = NTB_LINK_DOWN;
-	pci_set_drvdata(pdev, ndev);
-	ntb_setup_debugfs(ndev);
-
-	rc = pci_enable_device(pdev);
-	if (rc)
-		goto err;
-
-	pci_set_master(ndev->pdev);
-
-	rc = ntb_device_detect(ndev);
-	if (rc)
-		goto err;
-
-	ndev->mw = kcalloc(ndev->limits.max_mw, sizeof(struct ntb_mw),
-			   GFP_KERNEL);
-	if (!ndev->mw) {
-		rc = -ENOMEM;
-		goto err1;
-	}
-
-	if (ndev->split_bar)
-		rc = pci_request_selected_regions(pdev, NTB_SPLITBAR_MASK,
-						  KBUILD_MODNAME);
-	else
-		rc = pci_request_selected_regions(pdev, NTB_BAR_MASK,
-						  KBUILD_MODNAME);
-
-	if (rc)
-		goto err2;
-
-	ndev->reg_base = pci_ioremap_bar(pdev, NTB_BAR_MMIO);
-	if (!ndev->reg_base) {
-		dev_warn(&pdev->dev, "Cannot remap BAR 0\n");
-		rc = -EIO;
-		goto err3;
-	}
-
-	for (i = 0; i < ndev->limits.max_mw; i++) {
-		ndev->mw[i].bar_sz = pci_resource_len(pdev, MW_TO_BAR(i));
-
-		/*
-		 * with the errata we need to steal last of the memory
-		 * windows for workarounds and they point to MMIO registers.
-		 */
-		if ((ndev->wa_flags & WA_SNB_ERR) &&
-		    (i == (ndev->limits.max_mw - 1))) {
-			ndev->mw[i].vbase =
-				ioremap_nocache(pci_resource_start(pdev,
-							MW_TO_BAR(i)),
-						ndev->mw[i].bar_sz);
-		} else {
-			ndev->mw[i].vbase =
-				ioremap_wc(pci_resource_start(pdev,
-							MW_TO_BAR(i)),
-					   ndev->mw[i].bar_sz);
-		}
-
-		dev_info(&pdev->dev, "MW %d size %llu\n", i,
-			 (unsigned long long) ndev->mw[i].bar_sz);
-		if (!ndev->mw[i].vbase) {
-			dev_warn(&pdev->dev, "Cannot remap BAR %d\n",
-				 MW_TO_BAR(i));
-			rc = -EIO;
-			goto err3;
-		}
-	}
-
-	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
-	if (rc) {
-		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
-		if (rc)
-			goto err4;
-
-		dev_warn(&pdev->dev, "Cannot DMA highmem\n");
-	}
-
-	rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
-	if (rc) {
-		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
-		if (rc)
-			goto err4;
-
-		dev_warn(&pdev->dev, "Cannot DMA consistent highmem\n");
-	}
-
-	rc = ntb_device_setup(ndev);
-	if (rc)
-		goto err4;
-
-	rc = ntb_create_callbacks(ndev);
-	if (rc)
-		goto err5;
-
-	rc = ntb_setup_interrupts(ndev);
-	if (rc)
-		goto err6;
-
-	/* The scratchpad registers keep the values between rmmod/insmod,
-	 * blast them now
-	 */
-	for (i = 0; i < ndev->limits.max_spads; i++) {
-		ntb_write_local_spad(ndev, i, 0);
-		ntb_write_remote_spad(ndev, i, 0);
-	}
-
-	rc = ntb_transport_init(pdev);
-	if (rc)
-		goto err7;
-
-	ntb_hw_link_up(ndev);
-
-	return 0;
-
-err7:
-	ntb_free_interrupts(ndev);
-err6:
-	ntb_free_callbacks(ndev);
-err5:
-	ntb_device_free(ndev);
-err4:
-	for (i--; i >= 0; i--)
-		iounmap(ndev->mw[i].vbase);
-	iounmap(ndev->reg_base);
-err3:
-	if (ndev->split_bar)
-		pci_release_selected_regions(pdev, NTB_SPLITBAR_MASK);
-	else
-		pci_release_selected_regions(pdev, NTB_BAR_MASK);
-err2:
-	kfree(ndev->mw);
-err1:
-	pci_disable_device(pdev);
-err:
-	ntb_free_debugfs(ndev);
-	kfree(ndev);
-
-	dev_err(&pdev->dev, "Error loading %s module\n", KBUILD_MODNAME);
-	return rc;
-}
-
-static void ntb_pci_remove(struct pci_dev *pdev)
-{
-	struct ntb_device *ndev = pci_get_drvdata(pdev);
-	int i;
-
-	ntb_hw_link_down(ndev);
-
-	ntb_transport_free(ndev->ntb_transport);
-
-	ntb_free_interrupts(ndev);
-	ntb_free_callbacks(ndev);
-	ntb_device_free(ndev);
-
-	/* need to reset max_mw limits so we can unmap properly */
-	if (ndev->hw_type == SNB_HW)
-		ntb_max_mw_detect(ndev);
-
-	for (i = 0; i < ndev->limits.max_mw; i++)
-		iounmap(ndev->mw[i].vbase);
-
-	kfree(ndev->mw);
-	iounmap(ndev->reg_base);
-	if (ndev->split_bar)
-		pci_release_selected_regions(pdev, NTB_SPLITBAR_MASK);
-	else
-		pci_release_selected_regions(pdev, NTB_BAR_MASK);
-	pci_disable_device(pdev);
-	ntb_free_debugfs(ndev);
-	kfree(ndev);
-}
-
-static struct pci_driver ntb_pci_driver = {
-	.name = KBUILD_MODNAME,
-	.id_table = ntb_pci_tbl,
-	.probe = ntb_pci_probe,
-	.remove = ntb_pci_remove,
-};
-
-module_pci_driver(ntb_pci_driver);
diff --git a/drivers/ntb/ntb_hw.h b/drivers/ntb/ntb_hw.h
deleted file mode 100644
index 96de5fc95f90..000000000000
--- a/drivers/ntb/ntb_hw.h
+++ /dev/null
@@ -1,256 +0,0 @@
-/*
- * This file is provided under a dual BSD/GPLv2 license.  When using or
- *   redistributing this file, you may do so under either license.
- *
- *   GPL LICENSE SUMMARY
- *
- *   Copyright(c) 2012 Intel Corporation. All rights reserved.
- *
- *   This program is free software; you can redistribute it and/or modify
- *   it under the terms of version 2 of the GNU General Public License as
- *   published by the Free Software Foundation.
- *
- *   BSD LICENSE
- *
- *   Copyright(c) 2012 Intel Corporation. All rights reserved.
- *
- *   Redistribution and use in source and binary forms, with or without
- *   modification, are permitted provided that the following conditions
- *   are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copy
- *       notice, this list of conditions and the following disclaimer in
- *       the documentation and/or other materials provided with the
- *       distribution.
- *     * Neither the name of Intel Corporation nor the names of its
- *       contributors may be used to endorse or promote products derived
- *       from this software without specific prior written permission.
- *
- *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * Intel PCIe NTB Linux driver
- *
- * Contact Information:
- * Jon Mason <jon.mason@intel.com>
- */
-#include <linux/ntb.h>
-
-#define PCI_DEVICE_ID_INTEL_NTB_B2B_JSF		0x3725
-#define PCI_DEVICE_ID_INTEL_NTB_PS_JSF		0x3726
-#define PCI_DEVICE_ID_INTEL_NTB_SS_JSF		0x3727
-#define PCI_DEVICE_ID_INTEL_NTB_B2B_SNB		0x3C0D
-#define PCI_DEVICE_ID_INTEL_NTB_PS_SNB		0x3C0E
-#define PCI_DEVICE_ID_INTEL_NTB_SS_SNB		0x3C0F
-#define PCI_DEVICE_ID_INTEL_NTB_B2B_IVT		0x0E0D
-#define PCI_DEVICE_ID_INTEL_NTB_PS_IVT		0x0E0E
-#define PCI_DEVICE_ID_INTEL_NTB_SS_IVT		0x0E0F
-#define PCI_DEVICE_ID_INTEL_NTB_B2B_HSX		0x2F0D
-#define PCI_DEVICE_ID_INTEL_NTB_PS_HSX		0x2F0E
-#define PCI_DEVICE_ID_INTEL_NTB_SS_HSX		0x2F0F
-#define PCI_DEVICE_ID_INTEL_NTB_B2B_BWD		0x0C4E
-
-#ifndef readq
-static inline u64 readq(void __iomem *addr)
-{
-	return readl(addr) | (((u64) readl(addr + 4)) << 32LL);
-}
-#endif
-
-#ifndef writeq
-static inline void writeq(u64 val, void __iomem *addr)
-{
-	writel(val & 0xffffffff, addr);
-	writel(val >> 32, addr + 4);
-}
-#endif
-
-#define NTB_BAR_MMIO		0
-#define NTB_BAR_23		2
-#define NTB_BAR_4		4
-#define NTB_BAR_5		5
-
-#define NTB_BAR_MASK		((1 << NTB_BAR_MMIO) | (1 << NTB_BAR_23) |\
-				 (1 << NTB_BAR_4))
-#define NTB_SPLITBAR_MASK	((1 << NTB_BAR_MMIO) | (1 << NTB_BAR_23) |\
-				 (1 << NTB_BAR_4) | (1 << NTB_BAR_5))
-
-#define NTB_HB_TIMEOUT		msecs_to_jiffies(1000)
-
-enum ntb_hw_event {
-	NTB_EVENT_SW_EVENT0 = 0,
-	NTB_EVENT_SW_EVENT1,
-	NTB_EVENT_SW_EVENT2,
-	NTB_EVENT_HW_ERROR,
-	NTB_EVENT_HW_LINK_UP,
-	NTB_EVENT_HW_LINK_DOWN,
-};
-
-struct ntb_mw {
-	dma_addr_t phys_addr;
-	void __iomem *vbase;
-	resource_size_t bar_sz;
-};
-
-struct ntb_db_cb {
-	int (*callback)(void *data, int db_num);
-	unsigned int db_num;
-	void *data;
-	struct ntb_device *ndev;
-	struct tasklet_struct irq_work;
-};
-
-#define WA_SNB_ERR	0x00000001
-
-struct ntb_device {
-	struct pci_dev *pdev;
-	struct msix_entry *msix_entries;
-	void __iomem *reg_base;
-	struct ntb_mw *mw;
-	struct {
-		unsigned char max_mw;
-		unsigned char max_spads;
-		unsigned char max_db_bits;
-		unsigned char msix_cnt;
-	} limits;
-	struct {
-		void __iomem *ldb;
-		void __iomem *ldb_mask;
-		void __iomem *rdb;
-		void __iomem *bar2_xlat;
-		void __iomem *bar4_xlat;
-		void __iomem *bar5_xlat;
-		void __iomem *spad_write;
-		void __iomem *spad_read;
-		void __iomem *lnk_cntl;
-		void __iomem *lnk_stat;
-		void __iomem *spci_cmd;
-	} reg_ofs;
-	struct ntb_transport *ntb_transport;
-	void (*event_cb)(void *handle, enum ntb_hw_event event);
-
-	struct ntb_db_cb *db_cb;
-	unsigned char hw_type;
-	unsigned char conn_type;
-	unsigned char dev_type;
-	unsigned char num_msix;
-	unsigned char bits_per_vector;
-	unsigned char max_cbs;
-	unsigned char link_width;
-	unsigned char link_speed;
-	unsigned char link_status;
-	unsigned char split_bar;
-
-	struct delayed_work hb_timer;
-	unsigned long last_ts;
-
-	struct delayed_work lr_timer;
-
-	struct dentry *debugfs_dir;
-	struct dentry *debugfs_info;
-
-	unsigned int wa_flags;
-};
-
-/**
- * ntb_max_cbs() - return the max callbacks
- * @ndev: pointer to ntb_device instance
- *
- * Given the ntb pointer, return the maximum number of callbacks
- *
- * RETURNS: the maximum number of callbacks
- */
-static inline unsigned char ntb_max_cbs(struct ntb_device *ndev)
-{
-	return ndev->max_cbs;
-}
-
-/**
- * ntb_max_mw() - return the max number of memory windows
- * @ndev: pointer to ntb_device instance
- *
- * Given the ntb pointer, return the maximum number of memory windows
- *
- * RETURNS: the maximum number of memory windows
- */
-static inline unsigned char ntb_max_mw(struct ntb_device *ndev)
-{
-	return ndev->limits.max_mw;
-}
-
-/**
- * ntb_hw_link_status() - return the hardware link status
- * @ndev: pointer to ntb_device instance
- *
- * Returns true if the hardware is connected to the remote system
- *
- * RETURNS: true or false based on the hardware link state
- */
-static inline bool ntb_hw_link_status(struct ntb_device *ndev)
-{
-	return ndev->link_status == NTB_LINK_UP;
-}
-
-/**
- * ntb_query_pdev() - return the pci_dev pointer
- * @ndev: pointer to ntb_device instance
- *
- * Given the ntb pointer, return the pci_dev pointer for the NTB hardware device
- *
- * RETURNS: a pointer to the ntb pci_dev
- */
-static inline struct pci_dev *ntb_query_pdev(struct ntb_device *ndev)
-{
-	return ndev->pdev;
-}
-
-/**
- * ntb_query_debugfs() - return the debugfs pointer
- * @ndev: pointer to ntb_device instance
- *
- * Given the ntb pointer, return the debugfs directory pointer for the NTB
- * hardware device
- *
- * RETURNS: a pointer to the debugfs directory
- */
-static inline struct dentry *ntb_query_debugfs(struct ntb_device *ndev)
-{
-	return ndev->debugfs_dir;
-}
-
-struct ntb_device *ntb_register_transport(struct pci_dev *pdev,
-					  void *transport);
-void ntb_unregister_transport(struct ntb_device *ndev);
-void ntb_set_mw_addr(struct ntb_device *ndev, unsigned int mw, u64 addr);
-int ntb_register_db_callback(struct ntb_device *ndev, unsigned int idx,
-			     void *data, int (*db_cb_func)(void *data,
-							   int db_num));
-void ntb_unregister_db_callback(struct ntb_device *ndev, unsigned int idx);
-int ntb_register_event_callback(struct ntb_device *ndev,
-				void (*event_cb_func)(void *handle,
-						      enum ntb_hw_event event));
-void ntb_unregister_event_callback(struct ntb_device *ndev);
-int ntb_get_max_spads(struct ntb_device *ndev);
-int ntb_write_local_spad(struct ntb_device *ndev, unsigned int idx, u32 val);
-int ntb_read_local_spad(struct ntb_device *ndev, unsigned int idx, u32 *val);
-int ntb_write_remote_spad(struct ntb_device *ndev, unsigned int idx, u32 val);
-int ntb_read_remote_spad(struct ntb_device *ndev, unsigned int idx, u32 *val);
-resource_size_t ntb_get_mw_base(struct ntb_device *ndev, unsigned int mw);
-void __iomem *ntb_get_mw_vbase(struct ntb_device *ndev, unsigned int mw);
-u64 ntb_get_mw_size(struct ntb_device *ndev, unsigned int mw);
-void ntb_ring_doorbell(struct ntb_device *ndev, unsigned int idx);
-void *ntb_find_transport(struct pci_dev *pdev);
-
-int ntb_transport_init(struct pci_dev *pdev);
-void ntb_transport_free(void *transport);
diff --git a/drivers/ntb/ntb_regs.h b/drivers/ntb/ntb_regs.h
deleted file mode 100644
index f028ff81fd77..000000000000
--- a/drivers/ntb/ntb_regs.h
+++ /dev/null
@@ -1,177 +0,0 @@
-/*
- * This file is provided under a dual BSD/GPLv2 license.  When using or
- *   redistributing this file, you may do so under either license.
- *
- *   GPL LICENSE SUMMARY
- *
- *   Copyright(c) 2012 Intel Corporation. All rights reserved.
- *
- *   This program is free software; you can redistribute it and/or modify
- *   it under the terms of version 2 of the GNU General Public License as
- *   published by the Free Software Foundation.
- *
- *   BSD LICENSE
- *
- *   Copyright(c) 2012 Intel Corporation. All rights reserved.
- *
- *   Redistribution and use in source and binary forms, with or without
- *   modification, are permitted provided that the following conditions
- *   are met:
- *
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copy
- *       notice, this list of conditions and the following disclaimer in
- *       the documentation and/or other materials provided with the
- *       distribution.
- *     * Neither the name of Intel Corporation nor the names of its
- *       contributors may be used to endorse or promote products derived
- *       from this software without specific prior written permission.
- *
- *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
- *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
- *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
- *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
- *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
- *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * Intel PCIe NTB Linux driver
- *
- * Contact Information:
- * Jon Mason <jon.mason@intel.com>
- */
-
-#define NTB_LINK_STATUS_ACTIVE	0x2000
-#define NTB_LINK_SPEED_MASK	0x000f
-#define NTB_LINK_WIDTH_MASK	0x03f0
-
-#define SNB_MSIX_CNT		4
-#define SNB_MAX_B2B_SPADS	16
-#define SNB_MAX_COMPAT_SPADS	16
-/* Reserve the uppermost bit for link interrupt */
-#define SNB_MAX_DB_BITS		15
-#define SNB_LINK_DB		15
-#define SNB_DB_BITS_PER_VEC	5
-#define HSX_SPLITBAR_MAX_MW	3
-#define SNB_MAX_MW		2
-#define SNB_ERRATA_MAX_MW	1
-
-#define SNB_DB_HW_LINK		0x8000
-
-#define SNB_UNCERRSTS_OFFSET	0x014C
-#define SNB_CORERRSTS_OFFSET	0x0158
-#define SNB_LINK_STATUS_OFFSET	0x01A2
-#define SNB_PCICMD_OFFSET	0x0504
-#define SNB_DEVCTRL_OFFSET	0x0598
-#define SNB_DEVSTS_OFFSET	0x059A
-#define SNB_SLINK_STATUS_OFFSET	0x05A2
-
-#define SNB_PBAR2LMT_OFFSET	0x0000
-#define SNB_PBAR4LMT_OFFSET	0x0008
-#define SNB_PBAR5LMT_OFFSET	0x000C
-#define SNB_PBAR2XLAT_OFFSET	0x0010
-#define SNB_PBAR4XLAT_OFFSET	0x0018
-#define SNB_PBAR5XLAT_OFFSET	0x001C
-#define SNB_SBAR2LMT_OFFSET	0x0020
-#define SNB_SBAR4LMT_OFFSET	0x0028
-#define SNB_SBAR5LMT_OFFSET	0x002C
-#define SNB_SBAR2XLAT_OFFSET	0x0030
-#define SNB_SBAR4XLAT_OFFSET	0x0038
-#define SNB_SBAR5XLAT_OFFSET	0x003C
-#define SNB_SBAR0BASE_OFFSET	0x0040
-#define SNB_SBAR2BASE_OFFSET	0x0048
-#define SNB_SBAR4BASE_OFFSET	0x0050
-#define SNB_SBAR5BASE_OFFSET	0x0054
-#define SNB_NTBCNTL_OFFSET	0x0058
-#define SNB_SBDF_OFFSET		0x005C
-#define SNB_PDOORBELL_OFFSET	0x0060
-#define SNB_PDBMSK_OFFSET	0x0062
-#define SNB_SDOORBELL_OFFSET	0x0064
-#define SNB_SDBMSK_OFFSET	0x0066
-#define SNB_USMEMMISS_OFFSET	0x0070
-#define SNB_SPAD_OFFSET		0x0080
-#define SNB_SPADSEMA4_OFFSET	0x00c0
-#define SNB_WCCNTRL_OFFSET	0x00e0
-#define SNB_B2B_SPAD_OFFSET	0x0100
-#define SNB_B2B_DOORBELL_OFFSET	0x0140
-#define SNB_B2B_XLAT_OFFSETL	0x0144
-#define SNB_B2B_XLAT_OFFSETU	0x0148
-
-/*
- * The addresses are setup so the 32bit BARs can function. Thus
- * the addresses are all in 32bit space
- */
-#define SNB_MBAR01_USD_ADDR	0x000000002100000CULL
-#define SNB_MBAR23_USD_ADDR	0x000000004100000CULL
-#define SNB_MBAR4_USD_ADDR	0x000000008100000CULL
-#define SNB_MBAR5_USD_ADDR	0x00000000A100000CULL
-#define SNB_MBAR01_DSD_ADDR	0x000000002000000CULL
-#define SNB_MBAR23_DSD_ADDR	0x000000004000000CULL
-#define SNB_MBAR4_DSD_ADDR	0x000000008000000CULL
-#define SNB_MBAR5_DSD_ADDR	0x00000000A000000CULL
-
-#define BWD_MSIX_CNT		34
-#define BWD_MAX_SPADS		16
-#define BWD_MAX_DB_BITS		34
-#define BWD_DB_BITS_PER_VEC	1
-#define BWD_MAX_MW		2
-
-#define BWD_PCICMD_OFFSET	0xb004
-#define BWD_MBAR23_OFFSET	0xb018
-#define BWD_MBAR45_OFFSET	0xb020
-#define BWD_DEVCTRL_OFFSET	0xb048
-#define BWD_LINK_STATUS_OFFSET	0xb052
-#define BWD_ERRCORSTS_OFFSET	0xb110
-
-#define BWD_SBAR2XLAT_OFFSET	0x0008
-#define BWD_SBAR4XLAT_OFFSET	0x0010
-#define BWD_PDOORBELL_OFFSET	0x0020
-#define BWD_PDBMSK_OFFSET	0x0028
-#define BWD_NTBCNTL_OFFSET	0x0060
-#define BWD_EBDF_OFFSET		0x0064
-#define BWD_SPAD_OFFSET		0x0080
-#define BWD_SPADSEMA_OFFSET	0x00c0
-#define BWD_STKYSPAD_OFFSET	0x00c4
-#define BWD_PBAR2XLAT_OFFSET	0x8008
-#define BWD_PBAR4XLAT_OFFSET	0x8010
-#define BWD_B2B_DOORBELL_OFFSET	0x8020
-#define BWD_B2B_SPAD_OFFSET	0x8080
-#define BWD_B2B_SPADSEMA_OFFSET	0x80c0
-#define BWD_B2B_STKYSPAD_OFFSET	0x80c4
-
-#define BWD_MODPHY_PCSREG4	0x1c004
-#define BWD_MODPHY_PCSREG6	0x1c006
-
-#define BWD_IP_BASE		0xC000
-#define BWD_DESKEWSTS_OFFSET	(BWD_IP_BASE + 0x3024)
-#define BWD_LTSSMERRSTS0_OFFSET (BWD_IP_BASE + 0x3180)
-#define BWD_LTSSMSTATEJMP_OFFSET	(BWD_IP_BASE + 0x3040)
-#define BWD_IBSTERRRCRVSTS0_OFFSET	(BWD_IP_BASE + 0x3324)
-
-#define BWD_DESKEWSTS_DBERR	(1 << 15)
-#define BWD_LTSSMERRSTS0_UNEXPECTEDEI	(1 << 20)
-#define BWD_LTSSMSTATEJMP_FORCEDETECT	(1 << 2)
-#define BWD_IBIST_ERR_OFLOW	0x7FFF7FFF
-
-#define NTB_CNTL_CFG_LOCK		(1 << 0)
-#define NTB_CNTL_LINK_DISABLE		(1 << 1)
-#define NTB_CNTL_S2P_BAR23_SNOOP	(1 << 2)
-#define NTB_CNTL_P2S_BAR23_SNOOP	(1 << 4)
-#define NTB_CNTL_S2P_BAR4_SNOOP	(1 << 6)
-#define NTB_CNTL_P2S_BAR4_SNOOP	(1 << 8)
-#define NTB_CNTL_S2P_BAR5_SNOOP	(1 << 12)
-#define NTB_CNTL_P2S_BAR5_SNOOP	(1 << 14)
-#define BWD_CNTL_LINK_DOWN		(1 << 16)
-
-#define NTB_PPD_OFFSET		0x00D4
-#define SNB_PPD_CONN_TYPE	0x0003
-#define SNB_PPD_DEV_TYPE	0x0010
-#define SNB_PPD_SPLIT_BAR	(1 << 6)
-#define BWD_PPD_INIT_LINK	0x0008
-#define BWD_PPD_CONN_TYPE	0x0300
-#define BWD_PPD_DEV_TYPE	0x1000
diff --git a/drivers/ntb/ntb_transport.c b/drivers/ntb/ntb_transport.c
index e9bf2f47b61a..efe3ad4122f2 100644
--- a/drivers/ntb/ntb_transport.c
+++ b/drivers/ntb/ntb_transport.c
@@ -5,6 +5,7 @@
  *   GPL LICENSE SUMMARY
  *
  *   Copyright(c) 2012 Intel Corporation. All rights reserved.
+ *   Copyright (C) 2015 EMC 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
@@ -13,6 +14,7 @@
  *   BSD LICENSE
  *
  *   Copyright(c) 2012 Intel Corporation. All rights reserved.
+ *   Copyright (C) 2015 EMC Corporation. All Rights Reserved.
  *
  *   Redistribution and use in source and binary forms, with or without
  *   modification, are permitted provided that the following conditions
@@ -40,7 +42,7 @@
  *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * Intel PCIe NTB Linux driver
+ * PCIe NTB Transport Linux driver
  *
  * Contact Information:
  * Jon Mason <jon.mason@intel.com>
@@ -56,11 +58,25 @@
 #include <linux/pci.h>
 #include <linux/slab.h>
 #include <linux/types.h>
-#include "ntb_hw.h"
+#include <linux/uaccess.h>
+#include "linux/ntb.h"
+#include "linux/ntb_transport.h"
 
-#define NTB_TRANSPORT_VERSION	3
+#define NTB_TRANSPORT_VERSION	4
+#define NTB_TRANSPORT_VER	"4"
+#define NTB_TRANSPORT_NAME	"ntb_transport"
+#define NTB_TRANSPORT_DESC	"Software Queue-Pair Transport over NTB"
 
-static unsigned int transport_mtu = 0x401E;
+MODULE_DESCRIPTION(NTB_TRANSPORT_DESC);
+MODULE_VERSION(NTB_TRANSPORT_VER);
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Intel Corporation");
+
+static unsigned long max_mw_size;
+module_param(max_mw_size, ulong, 0644);
+MODULE_PARM_DESC(max_mw_size, "Limit size of large memory windows");
+
+static unsigned int transport_mtu = 0x10000;
 module_param(transport_mtu, uint, 0644);
 MODULE_PARM_DESC(transport_mtu, "Maximum size of NTB transport packets");
 
@@ -72,10 +88,16 @@ static unsigned int copy_bytes = 1024;
 module_param(copy_bytes, uint, 0644);
 MODULE_PARM_DESC(copy_bytes, "Threshold under which NTB will use the CPU to copy instead of DMA");
 
+static bool use_dma;
+module_param(use_dma, bool, 0644);
+MODULE_PARM_DESC(use_dma, "Use DMA engine to perform large data copy");
+
+static struct dentry *nt_debugfs_dir;
+
 struct ntb_queue_entry {
 	/* ntb_queue list reference */
 	struct list_head entry;
-	/* pointers to data to be transfered */
+	/* pointers to data to be transferred */
 	void *cb_data;
 	void *buf;
 	unsigned int len;
@@ -94,14 +116,16 @@ struct ntb_rx_info {
 };
 
 struct ntb_transport_qp {
-	struct ntb_transport *transport;
-	struct ntb_device *ndev;
+	struct ntb_transport_ctx *transport;
+	struct ntb_dev *ndev;
 	void *cb_data;
 	struct dma_chan *dma_chan;
 
 	bool client_ready;
-	bool qp_link;
+	bool link_is_up;
+
 	u8 qp_num;	/* Only 64 QP's are allowed.  0-63 */
+	u64 qp_bit;
 
 	struct ntb_rx_info __iomem *rx_info;
 	struct ntb_rx_info *remote_rx_info;
@@ -127,6 +151,7 @@ struct ntb_transport_qp {
 	unsigned int rx_max_entry;
 	unsigned int rx_max_frame;
 	dma_cookie_t last_cookie;
+	struct tasklet_struct rxc_db_work;
 
 	void (*event_handler)(void *data, int status);
 	struct delayed_work link_work;
@@ -153,33 +178,44 @@ struct ntb_transport_qp {
 };
 
 struct ntb_transport_mw {
-	size_t size;
+	phys_addr_t phys_addr;
+	resource_size_t phys_size;
+	resource_size_t xlat_align;
+	resource_size_t xlat_align_size;
+	void __iomem *vbase;
+	size_t xlat_size;
+	size_t buff_size;
 	void *virt_addr;
 	dma_addr_t dma_addr;
 };
 
 struct ntb_transport_client_dev {
 	struct list_head entry;
+	struct ntb_transport_ctx *nt;
 	struct device dev;
 };
 
-struct ntb_transport {
+struct ntb_transport_ctx {
 	struct list_head entry;
 	struct list_head client_devs;
 
-	struct ntb_device *ndev;
-	struct ntb_transport_mw *mw;
-	struct ntb_transport_qp *qps;
-	unsigned int max_qps;
-	unsigned long qp_bitmap;
-	bool transport_link;
+	struct ntb_dev *ndev;
+
+	struct ntb_transport_mw *mw_vec;
+	struct ntb_transport_qp *qp_vec;
+	unsigned int mw_count;
+	unsigned int qp_count;
+	u64 qp_bitmap;
+	u64 qp_bitmap_free;
+
+	bool link_is_up;
 	struct delayed_work link_work;
 	struct work_struct link_cleanup;
 };
 
 enum {
-	DESC_DONE_FLAG = 1 << 0,
-	LINK_DOWN_FLAG = 1 << 1,
+	DESC_DONE_FLAG = BIT(0),
+	LINK_DOWN_FLAG = BIT(1),
 };
 
 struct ntb_payload_header {
@@ -200,68 +236,69 @@ enum {
 	MAX_SPAD,
 };
 
-#define QP_TO_MW(ndev, qp)	((qp) % ntb_max_mw(ndev))
+#define dev_client_dev(__dev) \
+	container_of((__dev), struct ntb_transport_client_dev, dev)
+
+#define drv_client(__drv) \
+	container_of((__drv), struct ntb_transport_client, driver)
+
+#define QP_TO_MW(nt, qp)	((qp) % nt->mw_count)
 #define NTB_QP_DEF_NUM_ENTRIES	100
 #define NTB_LINK_DOWN_TIMEOUT	10
 
-static int ntb_match_bus(struct device *dev, struct device_driver *drv)
+static void ntb_transport_rxc_db(unsigned long data);
+static const struct ntb_ctx_ops ntb_transport_ops;
+static struct ntb_client ntb_transport_client;
+
+static int ntb_transport_bus_match(struct device *dev,
+				   struct device_driver *drv)
 {
 	return !strncmp(dev_name(dev), drv->name, strlen(drv->name));
 }
 
-static int ntb_client_probe(struct device *dev)
+static int ntb_transport_bus_probe(struct device *dev)
 {
-	const struct ntb_client *drv = container_of(dev->driver,
-						    struct ntb_client, driver);
-	struct pci_dev *pdev = container_of(dev->parent, struct pci_dev, dev);
+	const struct ntb_transport_client *client;
 	int rc = -EINVAL;
 
 	get_device(dev);
-	if (drv && drv->probe)
-		rc = drv->probe(pdev);
+
+	client = drv_client(dev->driver);
+	rc = client->probe(dev);
 	if (rc)
 		put_device(dev);
 
 	return rc;
 }
 
-static int ntb_client_remove(struct device *dev)
+static int ntb_transport_bus_remove(struct device *dev)
 {
-	const struct ntb_client *drv = container_of(dev->driver,
-						    struct ntb_client, driver);
-	struct pci_dev *pdev = container_of(dev->parent, struct pci_dev, dev);
+	const struct ntb_transport_client *client;
 
-	if (drv && drv->remove)
-		drv->remove(pdev);
+	client = drv_client(dev->driver);
+	client->remove(dev);
 
 	put_device(dev);
 
 	return 0;
 }
 
-static struct bus_type ntb_bus_type = {
-	.name = "ntb_bus",
-	.match = ntb_match_bus,
-	.probe = ntb_client_probe,
-	.remove = ntb_client_remove,
+static struct bus_type ntb_transport_bus = {
+	.name = "ntb_transport",
+	.match = ntb_transport_bus_match,
+	.probe = ntb_transport_bus_probe,
+	.remove = ntb_transport_bus_remove,
 };
 
 static LIST_HEAD(ntb_transport_list);
 
-static int ntb_bus_init(struct ntb_transport *nt)
+static int ntb_bus_init(struct ntb_transport_ctx *nt)
 {
-	if (list_empty(&ntb_transport_list)) {
-		int rc = bus_register(&ntb_bus_type);
-		if (rc)
-			return rc;
-	}
-
 	list_add(&nt->entry, &ntb_transport_list);
-
 	return 0;
 }
 
-static void ntb_bus_remove(struct ntb_transport *nt)
+static void ntb_bus_remove(struct ntb_transport_ctx *nt)
 {
 	struct ntb_transport_client_dev *client_dev, *cd;
 
@@ -273,29 +310,26 @@ static void ntb_bus_remove(struct ntb_transport *nt)
 	}
 
 	list_del(&nt->entry);
-
-	if (list_empty(&ntb_transport_list))
-		bus_unregister(&ntb_bus_type);
 }
 
-static void ntb_client_release(struct device *dev)
+static void ntb_transport_client_release(struct device *dev)
 {
 	struct ntb_transport_client_dev *client_dev;
-	client_dev = container_of(dev, struct ntb_transport_client_dev, dev);
 
+	client_dev = dev_client_dev(dev);
 	kfree(client_dev);
 }
 
 /**
- * ntb_unregister_client_dev - Unregister NTB client device
+ * ntb_transport_unregister_client_dev - Unregister NTB client device
  * @device_name: Name of NTB client device
  *
  * Unregister an NTB client device with the NTB transport layer
  */
-void ntb_unregister_client_dev(char *device_name)
+void ntb_transport_unregister_client_dev(char *device_name)
 {
 	struct ntb_transport_client_dev *client, *cd;
-	struct ntb_transport *nt;
+	struct ntb_transport_ctx *nt;
 
 	list_for_each_entry(nt, &ntb_transport_list, entry)
 		list_for_each_entry_safe(client, cd, &nt->client_devs, entry)
@@ -305,18 +339,19 @@ void ntb_unregister_client_dev(char *device_name)
 				device_unregister(&client->dev);
 			}
 }
-EXPORT_SYMBOL_GPL(ntb_unregister_client_dev);
+EXPORT_SYMBOL_GPL(ntb_transport_unregister_client_dev);
 
 /**
- * ntb_register_client_dev - Register NTB client device
+ * ntb_transport_register_client_dev - Register NTB client device
  * @device_name: Name of NTB client device
  *
  * Register an NTB client device with the NTB transport layer
  */
-int ntb_register_client_dev(char *device_name)
+int ntb_transport_register_client_dev(char *device_name)
 {
 	struct ntb_transport_client_dev *client_dev;
-	struct ntb_transport *nt;
+	struct ntb_transport_ctx *nt;
+	int node;
 	int rc, i = 0;
 
 	if (list_empty(&ntb_transport_list))
@@ -325,8 +360,10 @@ int ntb_register_client_dev(char *device_name)
 	list_for_each_entry(nt, &ntb_transport_list, entry) {
 		struct device *dev;
 
-		client_dev = kzalloc(sizeof(struct ntb_transport_client_dev),
-				     GFP_KERNEL);
+		node = dev_to_node(&nt->ndev->dev);
+
+		client_dev = kzalloc_node(sizeof(*client_dev),
+					  GFP_KERNEL, node);
 		if (!client_dev) {
 			rc = -ENOMEM;
 			goto err;
@@ -336,9 +373,9 @@ int ntb_register_client_dev(char *device_name)
 
 		/* setup and register client devices */
 		dev_set_name(dev, "%s%d", device_name, i);
-		dev->bus = &ntb_bus_type;
-		dev->release = ntb_client_release;
-		dev->parent = &ntb_query_pdev(nt->ndev)->dev;
+		dev->bus = &ntb_transport_bus;
+		dev->release = ntb_transport_client_release;
+		dev->parent = &nt->ndev->dev;
 
 		rc = device_register(dev);
 		if (rc) {
@@ -353,44 +390,44 @@ int ntb_register_client_dev(char *device_name)
 	return 0;
 
 err:
-	ntb_unregister_client_dev(device_name);
+	ntb_transport_unregister_client_dev(device_name);
 
 	return rc;
 }
-EXPORT_SYMBOL_GPL(ntb_register_client_dev);
+EXPORT_SYMBOL_GPL(ntb_transport_register_client_dev);
 
 /**
- * ntb_register_client - Register NTB client driver
+ * ntb_transport_register_client - Register NTB client driver
  * @drv: NTB client driver to be registered
  *
  * Register an NTB client driver with the NTB transport layer
  *
  * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
  */
-int ntb_register_client(struct ntb_client *drv)
+int ntb_transport_register_client(struct ntb_transport_client *drv)
 {
-	drv->driver.bus = &ntb_bus_type;
+	drv->driver.bus = &ntb_transport_bus;
 
 	if (list_empty(&ntb_transport_list))
 		return -ENODEV;
 
 	return driver_register(&drv->driver);
 }
-EXPORT_SYMBOL_GPL(ntb_register_client);
+EXPORT_SYMBOL_GPL(ntb_transport_register_client);
 
 /**
- * ntb_unregister_client - Unregister NTB client driver
+ * ntb_transport_unregister_client - Unregister NTB client driver
  * @drv: NTB client driver to be unregistered
  *
  * Unregister an NTB client driver with the NTB transport layer
  *
  * RETURNS: An appropriate -ERRNO error value on error, or zero for success.
  */
-void ntb_unregister_client(struct ntb_client *drv)
+void ntb_transport_unregister_client(struct ntb_transport_client *drv)
 {
 	driver_unregister(&drv->driver);
 }
-EXPORT_SYMBOL_GPL(ntb_unregister_client);
+EXPORT_SYMBOL_GPL(ntb_transport_unregister_client);
 
 static ssize_t debugfs_read(struct file *filp, char __user *ubuf, size_t count,
 			    loff_t *offp)
@@ -452,8 +489,8 @@ static ssize_t debugfs_read(struct file *filp, char __user *ubuf, size_t count,
 			       "tx_max_entry - \t%u\n", qp->tx_max_entry);
 
 	out_offset += snprintf(buf + out_offset, out_count - out_offset,
-			       "\nQP Link %s\n", (qp->qp_link == NTB_LINK_UP) ?
-			       "Up" : "Down");
+			       "\nQP Link %s\n",
+			       qp->link_is_up ? "Up" : "Down");
 	if (out_offset > out_count)
 		out_offset = out_count;
 
@@ -497,26 +534,31 @@ out:
 	return entry;
 }
 
-static void ntb_transport_setup_qp_mw(struct ntb_transport *nt,
-				      unsigned int qp_num)
+static int ntb_transport_setup_qp_mw(struct ntb_transport_ctx *nt,
+				     unsigned int qp_num)
 {
-	struct ntb_transport_qp *qp = &nt->qps[qp_num];
+	struct ntb_transport_qp *qp = &nt->qp_vec[qp_num];
+	struct ntb_transport_mw *mw;
 	unsigned int rx_size, num_qps_mw;
-	u8 mw_num, mw_max;
+	unsigned int mw_num, mw_count, qp_count;
 	unsigned int i;
 
-	mw_max = ntb_max_mw(nt->ndev);
-	mw_num = QP_TO_MW(nt->ndev, qp_num);
+	mw_count = nt->mw_count;
+	qp_count = nt->qp_count;
+
+	mw_num = QP_TO_MW(nt, qp_num);
+	mw = &nt->mw_vec[mw_num];
 
-	WARN_ON(nt->mw[mw_num].virt_addr == NULL);
+	if (!mw->virt_addr)
+		return -ENOMEM;
 
-	if (nt->max_qps % mw_max && mw_num + 1 < nt->max_qps / mw_max)
-		num_qps_mw = nt->max_qps / mw_max + 1;
+	if (qp_count % mw_count && mw_num + 1 < qp_count / mw_count)
+		num_qps_mw = qp_count / mw_count + 1;
 	else
-		num_qps_mw = nt->max_qps / mw_max;
+		num_qps_mw = qp_count / mw_count;
 
-	rx_size = (unsigned int) nt->mw[mw_num].size / num_qps_mw;
-	qp->rx_buff = nt->mw[mw_num].virt_addr + qp_num / mw_max * rx_size;
+	rx_size = (unsigned int)mw->xlat_size / num_qps_mw;
+	qp->rx_buff = mw->virt_addr + rx_size * qp_num / mw_count;
 	rx_size -= sizeof(struct ntb_rx_info);
 
 	qp->remote_rx_info = qp->rx_buff + rx_size;
@@ -530,49 +572,63 @@ static void ntb_transport_setup_qp_mw(struct ntb_transport *nt,
 
 	/* setup the hdr offsets with 0's */
 	for (i = 0; i < qp->rx_max_entry; i++) {
-		void *offset = qp->rx_buff + qp->rx_max_frame * (i + 1) -
-			       sizeof(struct ntb_payload_header);
+		void *offset = (qp->rx_buff + qp->rx_max_frame * (i + 1) -
+				sizeof(struct ntb_payload_header));
 		memset(offset, 0, sizeof(struct ntb_payload_header));
 	}
 
 	qp->rx_pkts = 0;
 	qp->tx_pkts = 0;
 	qp->tx_index = 0;
+
+	return 0;
 }
 
-static void ntb_free_mw(struct ntb_transport *nt, int num_mw)
+static void ntb_free_mw(struct ntb_transport_ctx *nt, int num_mw)
 {
-	struct ntb_transport_mw *mw = &nt->mw[num_mw];
-	struct pci_dev *pdev = ntb_query_pdev(nt->ndev);
+	struct ntb_transport_mw *mw = &nt->mw_vec[num_mw];
+	struct pci_dev *pdev = nt->ndev->pdev;
 
 	if (!mw->virt_addr)
 		return;
 
-	dma_free_coherent(&pdev->dev, mw->size, mw->virt_addr, mw->dma_addr);
+	ntb_mw_clear_trans(nt->ndev, num_mw);
+	dma_free_coherent(&pdev->dev, mw->buff_size,
+			  mw->virt_addr, mw->dma_addr);
+	mw->xlat_size = 0;
+	mw->buff_size = 0;
 	mw->virt_addr = NULL;
 }
 
-static int ntb_set_mw(struct ntb_transport *nt, int num_mw, unsigned int size)
+static int ntb_set_mw(struct ntb_transport_ctx *nt, int num_mw,
+		      unsigned int size)
 {
-	struct ntb_transport_mw *mw = &nt->mw[num_mw];
-	struct pci_dev *pdev = ntb_query_pdev(nt->ndev);
+	struct ntb_transport_mw *mw = &nt->mw_vec[num_mw];
+	struct pci_dev *pdev = nt->ndev->pdev;
+	unsigned int xlat_size, buff_size;
+	int rc;
+
+	xlat_size = round_up(size, mw->xlat_align_size);
+	buff_size = round_up(size, mw->xlat_align);
 
 	/* No need to re-setup */
-	if (mw->size == ALIGN(size, 4096))
+	if (mw->xlat_size == xlat_size)
 		return 0;
 
-	if (mw->size != 0)
+	if (mw->buff_size)
 		ntb_free_mw(nt, num_mw);
 
-	/* Alloc memory for receiving data.  Must be 4k aligned */
-	mw->size = ALIGN(size, 4096);
+	/* Alloc memory for receiving data.  Must be aligned */
+	mw->xlat_size = xlat_size;
+	mw->buff_size = buff_size;
 
-	mw->virt_addr = dma_alloc_coherent(&pdev->dev, mw->size, &mw->dma_addr,
-					   GFP_KERNEL);
+	mw->virt_addr = dma_alloc_coherent(&pdev->dev, buff_size,
+					   &mw->dma_addr, GFP_KERNEL);
 	if (!mw->virt_addr) {
-		mw->size = 0;
-		dev_err(&pdev->dev, "Unable to allocate MW buffer of size %d\n",
-		       (int) mw->size);
+		mw->xlat_size = 0;
+		mw->buff_size = 0;
+		dev_err(&pdev->dev, "Unable to alloc MW buff of size %d\n",
+			buff_size);
 		return -ENOMEM;
 	}
 
@@ -582,34 +638,58 @@ static int ntb_set_mw(struct ntb_transport *nt, int num_mw, unsigned int size)
 	 * is a requirement of the hardware. It is recommended to setup CMA
 	 * for BAR sizes equal or greater than 4MB.
 	 */
-	if (!IS_ALIGNED(mw->dma_addr, mw->size)) {
-		dev_err(&pdev->dev, "DMA memory %pad not aligned to BAR size\n",
+	if (!IS_ALIGNED(mw->dma_addr, mw->xlat_align)) {
+		dev_err(&pdev->dev, "DMA memory %pad is not aligned\n",
 			&mw->dma_addr);
 		ntb_free_mw(nt, num_mw);
 		return -ENOMEM;
 	}
 
 	/* Notify HW the memory location of the receive buffer */
-	ntb_set_mw_addr(nt->ndev, num_mw, mw->dma_addr);
+	rc = ntb_mw_set_trans(nt->ndev, num_mw, mw->dma_addr, mw->xlat_size);
+	if (rc) {
+		dev_err(&pdev->dev, "Unable to set mw%d translation", num_mw);
+		ntb_free_mw(nt, num_mw);
+		return -EIO;
+	}
 
 	return 0;
 }
 
+static void ntb_qp_link_down_reset(struct ntb_transport_qp *qp)
+{
+	qp->link_is_up = false;
+
+	qp->tx_index = 0;
+	qp->rx_index = 0;
+	qp->rx_bytes = 0;
+	qp->rx_pkts = 0;
+	qp->rx_ring_empty = 0;
+	qp->rx_err_no_buf = 0;
+	qp->rx_err_oflow = 0;
+	qp->rx_err_ver = 0;
+	qp->rx_memcpy = 0;
+	qp->rx_async = 0;
+	qp->tx_bytes = 0;
+	qp->tx_pkts = 0;
+	qp->tx_ring_full = 0;
+	qp->tx_err_no_buf = 0;
+	qp->tx_memcpy = 0;
+	qp->tx_async = 0;
+}
+
 static void ntb_qp_link_cleanup(struct ntb_transport_qp *qp)
 {
-	struct ntb_transport *nt = qp->transport;
-	struct pci_dev *pdev = ntb_query_pdev(nt->ndev);
+	struct ntb_transport_ctx *nt = qp->transport;
+	struct pci_dev *pdev = nt->ndev->pdev;
 
-	if (qp->qp_link == NTB_LINK_DOWN) {
-		cancel_delayed_work_sync(&qp->link_work);
-		return;
-	}
+	dev_info(&pdev->dev, "qp %d: Link Cleanup\n", qp->qp_num);
 
-	if (qp->event_handler)
-		qp->event_handler(qp->cb_data, NTB_LINK_DOWN);
+	cancel_delayed_work_sync(&qp->link_work);
+	ntb_qp_link_down_reset(qp);
 
-	dev_info(&pdev->dev, "qp %d: Link Down\n", qp->qp_num);
-	qp->qp_link = NTB_LINK_DOWN;
+	if (qp->event_handler)
+		qp->event_handler(qp->cb_data, qp->link_is_up);
 }
 
 static void ntb_qp_link_cleanup_work(struct work_struct *work)
@@ -617,11 +697,11 @@ static void ntb_qp_link_cleanup_work(struct work_struct *work)
 	struct ntb_transport_qp *qp = container_of(work,
 						   struct ntb_transport_qp,
 						   link_cleanup);
-	struct ntb_transport *nt = qp->transport;
+	struct ntb_transport_ctx *nt = qp->transport;
 
 	ntb_qp_link_cleanup(qp);
 
-	if (nt->transport_link == NTB_LINK_UP)
+	if (nt->link_is_up)
 		schedule_delayed_work(&qp->link_work,
 				      msecs_to_jiffies(NTB_LINK_DOWN_TIMEOUT));
 }
@@ -631,180 +711,132 @@ static void ntb_qp_link_down(struct ntb_transport_qp *qp)
 	schedule_work(&qp->link_cleanup);
 }
 
-static void ntb_transport_link_cleanup(struct ntb_transport *nt)
+static void ntb_transport_link_cleanup(struct ntb_transport_ctx *nt)
 {
+	struct ntb_transport_qp *qp;
+	u64 qp_bitmap_alloc;
 	int i;
 
+	qp_bitmap_alloc = nt->qp_bitmap & ~nt->qp_bitmap_free;
+
 	/* Pass along the info to any clients */
-	for (i = 0; i < nt->max_qps; i++)
-		if (!test_bit(i, &nt->qp_bitmap))
-			ntb_qp_link_cleanup(&nt->qps[i]);
+	for (i = 0; i < nt->qp_count; i++)
+		if (qp_bitmap_alloc & BIT_ULL(i)) {
+			qp = &nt->qp_vec[i];
+			ntb_qp_link_cleanup(qp);
+			cancel_work_sync(&qp->link_cleanup);
+			cancel_delayed_work_sync(&qp->link_work);
+		}
 
-	if (nt->transport_link == NTB_LINK_DOWN)
+	if (!nt->link_is_up)
 		cancel_delayed_work_sync(&nt->link_work);
-	else
-		nt->transport_link = NTB_LINK_DOWN;
 
 	/* The scratchpad registers keep the values if the remote side
 	 * goes down, blast them now to give them a sane value the next
 	 * time they are accessed
 	 */
 	for (i = 0; i < MAX_SPAD; i++)
-		ntb_write_local_spad(nt->ndev, i, 0);
+		ntb_spad_write(nt->ndev, i, 0);
 }
 
 static void ntb_transport_link_cleanup_work(struct work_struct *work)
 {
-	struct ntb_transport *nt = container_of(work, struct ntb_transport,
-						link_cleanup);
+	struct ntb_transport_ctx *nt =
+		container_of(work, struct ntb_transport_ctx, link_cleanup);
 
 	ntb_transport_link_cleanup(nt);
 }
 
-static void ntb_transport_event_callback(void *data, enum ntb_hw_event event)
+static void ntb_transport_event_callback(void *data)
 {
-	struct ntb_transport *nt = data;
+	struct ntb_transport_ctx *nt = data;
 
-	switch (event) {
-	case NTB_EVENT_HW_LINK_UP:
+	if (ntb_link_is_up(nt->ndev, NULL, NULL) == 1)
 		schedule_delayed_work(&nt->link_work, 0);
-		break;
-	case NTB_EVENT_HW_LINK_DOWN:
+	else
 		schedule_work(&nt->link_cleanup);
-		break;
-	default:
-		BUG();
-	}
 }
 
 static void ntb_transport_link_work(struct work_struct *work)
 {
-	struct ntb_transport *nt = container_of(work, struct ntb_transport,
-						link_work.work);
-	struct ntb_device *ndev = nt->ndev;
-	struct pci_dev *pdev = ntb_query_pdev(ndev);
+	struct ntb_transport_ctx *nt =
+		container_of(work, struct ntb_transport_ctx, link_work.work);
+	struct ntb_dev *ndev = nt->ndev;
+	struct pci_dev *pdev = ndev->pdev;
+	resource_size_t size;
 	u32 val;
-	int rc, i;
+	int rc, i, spad;
 
 	/* send the local info, in the opposite order of the way we read it */
-	for (i = 0; i < ntb_max_mw(ndev); i++) {
-		rc = ntb_write_remote_spad(ndev, MW0_SZ_HIGH + (i * 2),
-					   ntb_get_mw_size(ndev, i) >> 32);
-		if (rc) {
-			dev_err(&pdev->dev, "Error writing %u to remote spad %d\n",
-				(u32)(ntb_get_mw_size(ndev, i) >> 32),
-				MW0_SZ_HIGH + (i * 2));
-			goto out;
-		}
+	for (i = 0; i < nt->mw_count; i++) {
+		size = nt->mw_vec[i].phys_size;
 
-		rc = ntb_write_remote_spad(ndev, MW0_SZ_LOW + (i * 2),
-					   (u32) ntb_get_mw_size(ndev, i));
-		if (rc) {
-			dev_err(&pdev->dev, "Error writing %u to remote spad %d\n",
-				(u32) ntb_get_mw_size(ndev, i),
-				MW0_SZ_LOW + (i * 2));
-			goto out;
-		}
-	}
+		if (max_mw_size && size > max_mw_size)
+			size = max_mw_size;
 
-	rc = ntb_write_remote_spad(ndev, NUM_MWS, ntb_max_mw(ndev));
-	if (rc) {
-		dev_err(&pdev->dev, "Error writing %x to remote spad %d\n",
-			ntb_max_mw(ndev), NUM_MWS);
-		goto out;
-	}
+		spad = MW0_SZ_HIGH + (i * 2);
+		ntb_peer_spad_write(ndev, spad, (u32)(size >> 32));
 
-	rc = ntb_write_remote_spad(ndev, NUM_QPS, nt->max_qps);
-	if (rc) {
-		dev_err(&pdev->dev, "Error writing %x to remote spad %d\n",
-			nt->max_qps, NUM_QPS);
-		goto out;
+		spad = MW0_SZ_LOW + (i * 2);
+		ntb_peer_spad_write(ndev, spad, (u32)size);
 	}
 
-	rc = ntb_write_remote_spad(ndev, VERSION, NTB_TRANSPORT_VERSION);
-	if (rc) {
-		dev_err(&pdev->dev, "Error writing %x to remote spad %d\n",
-			NTB_TRANSPORT_VERSION, VERSION);
-		goto out;
-	}
+	ntb_peer_spad_write(ndev, NUM_MWS, nt->mw_count);
 
-	/* Query the remote side for its info */
-	rc = ntb_read_remote_spad(ndev, VERSION, &val);
-	if (rc) {
-		dev_err(&pdev->dev, "Error reading remote spad %d\n", VERSION);
-		goto out;
-	}
+	ntb_peer_spad_write(ndev, NUM_QPS, nt->qp_count);
 
-	if (val != NTB_TRANSPORT_VERSION)
-		goto out;
-	dev_dbg(&pdev->dev, "Remote version = %d\n", val);
+	ntb_peer_spad_write(ndev, VERSION, NTB_TRANSPORT_VERSION);
 
-	rc = ntb_read_remote_spad(ndev, NUM_QPS, &val);
-	if (rc) {
-		dev_err(&pdev->dev, "Error reading remote spad %d\n", NUM_QPS);
+	/* Query the remote side for its info */
+	val = ntb_spad_read(ndev, VERSION);
+	dev_dbg(&pdev->dev, "Remote version = %d\n", val);
+	if (val != NTB_TRANSPORT_VERSION)
 		goto out;
-	}
 
-	if (val != nt->max_qps)
-		goto out;
+	val = ntb_spad_read(ndev, NUM_QPS);
 	dev_dbg(&pdev->dev, "Remote max number of qps = %d\n", val);
-
-	rc = ntb_read_remote_spad(ndev, NUM_MWS, &val);
-	if (rc) {
-		dev_err(&pdev->dev, "Error reading remote spad %d\n", NUM_MWS);
+	if (val != nt->qp_count)
 		goto out;
-	}
 
-	if (val != ntb_max_mw(ndev))
-		goto out;
+	val = ntb_spad_read(ndev, NUM_MWS);
 	dev_dbg(&pdev->dev, "Remote number of mws = %d\n", val);
+	if (val != nt->mw_count)
+		goto out;
 
-	for (i = 0; i < ntb_max_mw(ndev); i++) {
+	for (i = 0; i < nt->mw_count; i++) {
 		u64 val64;
 
-		rc = ntb_read_remote_spad(ndev, MW0_SZ_HIGH + (i * 2), &val);
-		if (rc) {
-			dev_err(&pdev->dev, "Error reading remote spad %d\n",
-				MW0_SZ_HIGH + (i * 2));
-			goto out1;
-		}
-
-		val64 = (u64) val << 32;
-
-		rc = ntb_read_remote_spad(ndev, MW0_SZ_LOW + (i * 2), &val);
-		if (rc) {
-			dev_err(&pdev->dev, "Error reading remote spad %d\n",
-				MW0_SZ_LOW + (i * 2));
-			goto out1;
-		}
+		val = ntb_spad_read(ndev, MW0_SZ_HIGH + (i * 2));
+		val64 = (u64)val << 32;
 
+		val = ntb_spad_read(ndev, MW0_SZ_LOW + (i * 2));
 		val64 |= val;
 
-		dev_dbg(&pdev->dev, "Remote MW%d size = %llu\n", i, val64);
+		dev_dbg(&pdev->dev, "Remote MW%d size = %#llx\n", i, val64);
 
 		rc = ntb_set_mw(nt, i, val64);
 		if (rc)
 			goto out1;
 	}
 
-	nt->transport_link = NTB_LINK_UP;
+	nt->link_is_up = true;
 
-	for (i = 0; i < nt->max_qps; i++) {
-		struct ntb_transport_qp *qp = &nt->qps[i];
+	for (i = 0; i < nt->qp_count; i++) {
+		struct ntb_transport_qp *qp = &nt->qp_vec[i];
 
 		ntb_transport_setup_qp_mw(nt, i);
 
-		if (qp->client_ready == NTB_LINK_UP)
+		if (qp->client_ready)
 			schedule_delayed_work(&qp->link_work, 0);
 	}
 
 	return;
 
 out1:
-	for (i = 0; i < ntb_max_mw(ndev); i++)
+	for (i = 0; i < nt->mw_count; i++)
 		ntb_free_mw(nt, i);
 out:
-	if (ntb_hw_link_status(ndev))
+	if (ntb_link_is_up(ndev, NULL, NULL) == 1)
 		schedule_delayed_work(&nt->link_work,
 				      msecs_to_jiffies(NTB_LINK_DOWN_TIMEOUT));
 }
@@ -814,73 +846,73 @@ static void ntb_qp_link_work(struct work_struct *work)
 	struct ntb_transport_qp *qp = container_of(work,
 						   struct ntb_transport_qp,
 						   link_work.work);
-	struct pci_dev *pdev = ntb_query_pdev(qp->ndev);
-	struct ntb_transport *nt = qp->transport;
-	int rc, val;
+	struct pci_dev *pdev = qp->ndev->pdev;
+	struct ntb_transport_ctx *nt = qp->transport;
+	int val;
 
-	WARN_ON(nt->transport_link != NTB_LINK_UP);
+	WARN_ON(!nt->link_is_up);
 
-	rc = ntb_read_local_spad(nt->ndev, QP_LINKS, &val);
-	if (rc) {
-		dev_err(&pdev->dev, "Error reading spad %d\n", QP_LINKS);
-		return;
-	}
+	val = ntb_spad_read(nt->ndev, QP_LINKS);
 
-	rc = ntb_write_remote_spad(nt->ndev, QP_LINKS, val | 1 << qp->qp_num);
-	if (rc)
-		dev_err(&pdev->dev, "Error writing %x to remote spad %d\n",
-			val | 1 << qp->qp_num, QP_LINKS);
+	ntb_peer_spad_write(nt->ndev, QP_LINKS, val | BIT(qp->qp_num));
 
 	/* query remote spad for qp ready bits */
-	rc = ntb_read_remote_spad(nt->ndev, QP_LINKS, &val);
-	if (rc)
-		dev_err(&pdev->dev, "Error reading remote spad %d\n", QP_LINKS);
-
-	dev_dbg(&pdev->dev, "Remote QP link status = %x\n", val);
+	ntb_peer_spad_read(nt->ndev, QP_LINKS);
+	dev_dbg_ratelimited(&pdev->dev, "Remote QP link status = %x\n", val);
 
 	/* See if the remote side is up */
-	if (1 << qp->qp_num & val) {
-		qp->qp_link = NTB_LINK_UP;
-
+	if (val & BIT(qp->qp_num)) {
 		dev_info(&pdev->dev, "qp %d: Link Up\n", qp->qp_num);
+		qp->link_is_up = true;
+
 		if (qp->event_handler)
-			qp->event_handler(qp->cb_data, NTB_LINK_UP);
-	} else if (nt->transport_link == NTB_LINK_UP)
+			qp->event_handler(qp->cb_data, qp->link_is_up);
+	} else if (nt->link_is_up)
 		schedule_delayed_work(&qp->link_work,
 				      msecs_to_jiffies(NTB_LINK_DOWN_TIMEOUT));
 }
 
-static int ntb_transport_init_queue(struct ntb_transport *nt,
+static int ntb_transport_init_queue(struct ntb_transport_ctx *nt,
 				    unsigned int qp_num)
 {
 	struct ntb_transport_qp *qp;
+	struct ntb_transport_mw *mw;
+	phys_addr_t mw_base;
+	resource_size_t mw_size;
 	unsigned int num_qps_mw, tx_size;
-	u8 mw_num, mw_max;
+	unsigned int mw_num, mw_count, qp_count;
 	u64 qp_offset;
 
-	mw_max = ntb_max_mw(nt->ndev);
-	mw_num = QP_TO_MW(nt->ndev, qp_num);
+	mw_count = nt->mw_count;
+	qp_count = nt->qp_count;
 
-	qp = &nt->qps[qp_num];
+	mw_num = QP_TO_MW(nt, qp_num);
+	mw = &nt->mw_vec[mw_num];
+
+	qp = &nt->qp_vec[qp_num];
 	qp->qp_num = qp_num;
 	qp->transport = nt;
 	qp->ndev = nt->ndev;
-	qp->qp_link = NTB_LINK_DOWN;
-	qp->client_ready = NTB_LINK_DOWN;
+	qp->client_ready = false;
 	qp->event_handler = NULL;
+	ntb_qp_link_down_reset(qp);
 
-	if (nt->max_qps % mw_max && mw_num + 1 < nt->max_qps / mw_max)
-		num_qps_mw = nt->max_qps / mw_max + 1;
+	if (qp_count % mw_count && mw_num + 1 < qp_count / mw_count)
+		num_qps_mw = qp_count / mw_count + 1;
 	else
-		num_qps_mw = nt->max_qps / mw_max;
+		num_qps_mw = qp_count / mw_count;
+
+	mw_base = nt->mw_vec[mw_num].phys_addr;
+	mw_size = nt->mw_vec[mw_num].phys_size;
 
-	tx_size = (unsigned int) ntb_get_mw_size(qp->ndev, mw_num) / num_qps_mw;
-	qp_offset = qp_num / mw_max * tx_size;
-	qp->tx_mw = ntb_get_mw_vbase(nt->ndev, mw_num) + qp_offset;
+	tx_size = (unsigned int)mw_size / num_qps_mw;
+	qp_offset = tx_size * qp_num / mw_count;
+
+	qp->tx_mw = nt->mw_vec[mw_num].vbase + qp_offset;
 	if (!qp->tx_mw)
 		return -EINVAL;
 
-	qp->tx_mw_phys = ntb_get_mw_base(qp->ndev, mw_num) + qp_offset;
+	qp->tx_mw_phys = mw_base + qp_offset;
 	if (!qp->tx_mw_phys)
 		return -EINVAL;
 
@@ -891,16 +923,19 @@ static int ntb_transport_init_queue(struct ntb_transport *nt,
 	qp->tx_max_frame = min(transport_mtu, tx_size / 2);
 	qp->tx_max_entry = tx_size / qp->tx_max_frame;
 
-	if (ntb_query_debugfs(nt->ndev)) {
+	if (nt_debugfs_dir) {
 		char debugfs_name[4];
 
 		snprintf(debugfs_name, 4, "qp%d", qp_num);
 		qp->debugfs_dir = debugfs_create_dir(debugfs_name,
-						 ntb_query_debugfs(nt->ndev));
+						     nt_debugfs_dir);
 
 		qp->debugfs_stats = debugfs_create_file("stats", S_IRUSR,
 							qp->debugfs_dir, qp,
 							&ntb_qp_debugfs_stats);
+	} else {
+		qp->debugfs_dir = NULL;
+		qp->debugfs_stats = NULL;
 	}
 
 	INIT_DELAYED_WORK(&qp->link_work, ntb_qp_link_work);
@@ -914,46 +949,89 @@ static int ntb_transport_init_queue(struct ntb_transport *nt,
 	INIT_LIST_HEAD(&qp->rx_free_q);
 	INIT_LIST_HEAD(&qp->tx_free_q);
 
+	tasklet_init(&qp->rxc_db_work, ntb_transport_rxc_db,
+		     (unsigned long)qp);
+
 	return 0;
 }
 
-int ntb_transport_init(struct pci_dev *pdev)
+static int ntb_transport_probe(struct ntb_client *self, struct ntb_dev *ndev)
 {
-	struct ntb_transport *nt;
+	struct ntb_transport_ctx *nt;
+	struct ntb_transport_mw *mw;
+	unsigned int mw_count, qp_count;
+	u64 qp_bitmap;
+	int node;
 	int rc, i;
 
-	nt = kzalloc(sizeof(struct ntb_transport), GFP_KERNEL);
+	if (ntb_db_is_unsafe(ndev))
+		dev_dbg(&ndev->dev,
+			"doorbell is unsafe, proceed anyway...\n");
+	if (ntb_spad_is_unsafe(ndev))
+		dev_dbg(&ndev->dev,
+			"scratchpad is unsafe, proceed anyway...\n");
+
+	node = dev_to_node(&ndev->dev);
+
+	nt = kzalloc_node(sizeof(*nt), GFP_KERNEL, node);
 	if (!nt)
 		return -ENOMEM;
 
-	nt->ndev = ntb_register_transport(pdev, nt);
-	if (!nt->ndev) {
-		rc = -EIO;
+	nt->ndev = ndev;
+
+	mw_count = ntb_mw_count(ndev);
+
+	nt->mw_count = mw_count;
+
+	nt->mw_vec = kzalloc_node(mw_count * sizeof(*nt->mw_vec),
+				  GFP_KERNEL, node);
+	if (!nt->mw_vec) {
+		rc = -ENOMEM;
 		goto err;
 	}
 
-	nt->mw = kcalloc(ntb_max_mw(nt->ndev), sizeof(struct ntb_transport_mw),
-			 GFP_KERNEL);
-	if (!nt->mw) {
-		rc = -ENOMEM;
-		goto err1;
+	for (i = 0; i < mw_count; i++) {
+		mw = &nt->mw_vec[i];
+
+		rc = ntb_mw_get_range(ndev, i, &mw->phys_addr, &mw->phys_size,
+				      &mw->xlat_align, &mw->xlat_align_size);
+		if (rc)
+			goto err1;
+
+		mw->vbase = ioremap_wc(mw->phys_addr, mw->phys_size);
+		if (!mw->vbase) {
+			rc = -ENOMEM;
+			goto err1;
+		}
+
+		mw->buff_size = 0;
+		mw->xlat_size = 0;
+		mw->virt_addr = NULL;
+		mw->dma_addr = 0;
 	}
 
-	if (max_num_clients)
-		nt->max_qps = min(ntb_max_cbs(nt->ndev), max_num_clients);
-	else
-		nt->max_qps = min(ntb_max_cbs(nt->ndev), ntb_max_mw(nt->ndev));
+	qp_bitmap = ntb_db_valid_mask(ndev);
+
+	qp_count = ilog2(qp_bitmap);
+	if (max_num_clients && max_num_clients < qp_count)
+		qp_count = max_num_clients;
+	else if (mw_count < qp_count)
+		qp_count = mw_count;
+
+	qp_bitmap &= BIT_ULL(qp_count) - 1;
 
-	nt->qps = kcalloc(nt->max_qps, sizeof(struct ntb_transport_qp),
-			  GFP_KERNEL);
-	if (!nt->qps) {
+	nt->qp_count = qp_count;
+	nt->qp_bitmap = qp_bitmap;
+	nt->qp_bitmap_free = qp_bitmap;
+
+	nt->qp_vec = kzalloc_node(qp_count * sizeof(*nt->qp_vec),
+				  GFP_KERNEL, node);
+	if (!nt->qp_vec) {
 		rc = -ENOMEM;
 		goto err2;
 	}
 
-	nt->qp_bitmap = ((u64) 1 << nt->max_qps) - 1;
-
-	for (i = 0; i < nt->max_qps; i++) {
+	for (i = 0; i < qp_count; i++) {
 		rc = ntb_transport_init_queue(nt, i);
 		if (rc)
 			goto err3;
@@ -962,8 +1040,7 @@ int ntb_transport_init(struct pci_dev *pdev)
 	INIT_DELAYED_WORK(&nt->link_work, ntb_transport_link_work);
 	INIT_WORK(&nt->link_cleanup, ntb_transport_link_cleanup_work);
 
-	rc = ntb_register_event_callback(nt->ndev,
-					 ntb_transport_event_callback);
+	rc = ntb_set_ctx(ndev, nt, &ntb_transport_ops);
 	if (rc)
 		goto err3;
 
@@ -972,51 +1049,61 @@ int ntb_transport_init(struct pci_dev *pdev)
 	if (rc)
 		goto err4;
 
-	if (ntb_hw_link_status(nt->ndev))
-		schedule_delayed_work(&nt->link_work, 0);
+	nt->link_is_up = false;
+	ntb_link_enable(ndev, NTB_SPEED_AUTO, NTB_WIDTH_AUTO);
+	ntb_link_event(ndev);
 
 	return 0;
 
 err4:
-	ntb_unregister_event_callback(nt->ndev);
+	ntb_clear_ctx(ndev);
 err3:
-	kfree(nt->qps);
+	kfree(nt->qp_vec);
 err2:
-	kfree(nt->mw);
+	kfree(nt->mw_vec);
 err1:
-	ntb_unregister_transport(nt->ndev);
+	while (i--) {
+		mw = &nt->mw_vec[i];
+		iounmap(mw->vbase);
+	}
 err:
 	kfree(nt);
 	return rc;
 }
 
-void ntb_transport_free(void *transport)
+static void ntb_transport_free(struct ntb_client *self, struct ntb_dev *ndev)
 {
-	struct ntb_transport *nt = transport;
-	struct ntb_device *ndev = nt->ndev;
+	struct ntb_transport_ctx *nt = ndev->ctx;
+	struct ntb_transport_qp *qp;
+	u64 qp_bitmap_alloc;
 	int i;
 
 	ntb_transport_link_cleanup(nt);
+	cancel_work_sync(&nt->link_cleanup);
+	cancel_delayed_work_sync(&nt->link_work);
+
+	qp_bitmap_alloc = nt->qp_bitmap & ~nt->qp_bitmap_free;
 
 	/* verify that all the qp's are freed */
-	for (i = 0; i < nt->max_qps; i++) {
-		if (!test_bit(i, &nt->qp_bitmap))
-			ntb_transport_free_queue(&nt->qps[i]);
-		debugfs_remove_recursive(nt->qps[i].debugfs_dir);
+	for (i = 0; i < nt->qp_count; i++) {
+		qp = &nt->qp_vec[i];
+		if (qp_bitmap_alloc & BIT_ULL(i))
+			ntb_transport_free_queue(qp);
+		debugfs_remove_recursive(qp->debugfs_dir);
 	}
 
-	ntb_bus_remove(nt);
+	ntb_link_disable(ndev);
+	ntb_clear_ctx(ndev);
 
-	cancel_delayed_work_sync(&nt->link_work);
-
-	ntb_unregister_event_callback(ndev);
+	ntb_bus_remove(nt);
 
-	for (i = 0; i < ntb_max_mw(ndev); i++)
+	for (i = nt->mw_count; i--; ) {
 		ntb_free_mw(nt, i);
+		iounmap(nt->mw_vec[i].vbase);
+	}
 
-	kfree(nt->qps);
-	kfree(nt->mw);
-	ntb_unregister_transport(ndev);
+	kfree(nt->qp_vec);
+	kfree(nt->mw_vec);
 	kfree(nt);
 }
 
@@ -1028,15 +1115,13 @@ static void ntb_rx_copy_callback(void *data)
 	unsigned int len = entry->len;
 	struct ntb_payload_header *hdr = entry->rx_hdr;
 
-	/* Ensure that the data is fully copied out before clearing the flag */
-	wmb();
 	hdr->flags = 0;
 
 	iowrite32(entry->index, &qp->rx_info->entry);
 
 	ntb_list_add(&qp->ntb_rx_free_q_lock, &entry->entry, &qp->rx_free_q);
 
-	if (qp->rx_handler && qp->client_ready == NTB_LINK_UP)
+	if (qp->rx_handler && qp->client_ready)
 		qp->rx_handler(qp, qp->cb_data, cb_data, len);
 }
 
@@ -1047,6 +1132,9 @@ static void ntb_memcpy_rx(struct ntb_queue_entry *entry, void *offset)
 
 	memcpy(buf, offset, len);
 
+	/* Ensure that the data is fully copied out before clearing the flag */
+	wmb();
+
 	ntb_rx_copy_callback(entry);
 }
 
@@ -1071,8 +1159,8 @@ static void ntb_async_rx(struct ntb_queue_entry *entry, void *offset,
 		goto err_wait;
 
 	device = chan->device;
-	pay_off = (size_t) offset & ~PAGE_MASK;
-	buff_off = (size_t) buf & ~PAGE_MASK;
+	pay_off = (size_t)offset & ~PAGE_MASK;
+	buff_off = (size_t)buf & ~PAGE_MASK;
 
 	if (!is_dma_copy_aligned(device, pay_off, buff_off, len))
 		goto err_wait;
@@ -1138,86 +1226,103 @@ static int ntb_process_rxc(struct ntb_transport_qp *qp)
 	struct ntb_payload_header *hdr;
 	struct ntb_queue_entry *entry;
 	void *offset;
+	int rc;
 
 	offset = qp->rx_buff + qp->rx_max_frame * qp->rx_index;
 	hdr = offset + qp->rx_max_frame - sizeof(struct ntb_payload_header);
 
-	entry = ntb_list_rm(&qp->ntb_rx_pend_q_lock, &qp->rx_pend_q);
-	if (!entry) {
-		dev_dbg(&ntb_query_pdev(qp->ndev)->dev,
-			"no buffer - HDR ver %u, len %d, flags %x\n",
-			hdr->ver, hdr->len, hdr->flags);
-		qp->rx_err_no_buf++;
-		return -ENOMEM;
-	}
+	dev_dbg(&qp->ndev->pdev->dev, "qp %d: RX ver %u len %d flags %x\n",
+		qp->qp_num, hdr->ver, hdr->len, hdr->flags);
 
 	if (!(hdr->flags & DESC_DONE_FLAG)) {
-		ntb_list_add(&qp->ntb_rx_pend_q_lock, &entry->entry,
-			     &qp->rx_pend_q);
+		dev_dbg(&qp->ndev->pdev->dev, "done flag not set\n");
 		qp->rx_ring_empty++;
 		return -EAGAIN;
 	}
 
-	if (hdr->ver != (u32) qp->rx_pkts) {
-		dev_dbg(&ntb_query_pdev(qp->ndev)->dev,
-			"qp %d: version mismatch, expected %llu - got %u\n",
-			qp->qp_num, qp->rx_pkts, hdr->ver);
-		ntb_list_add(&qp->ntb_rx_pend_q_lock, &entry->entry,
-			     &qp->rx_pend_q);
+	if (hdr->flags & LINK_DOWN_FLAG) {
+		dev_dbg(&qp->ndev->pdev->dev, "link down flag set\n");
+		ntb_qp_link_down(qp);
+		hdr->flags = 0;
+		return -EAGAIN;
+	}
+
+	if (hdr->ver != (u32)qp->rx_pkts) {
+		dev_dbg(&qp->ndev->pdev->dev,
+			"version mismatch, expected %llu - got %u\n",
+			qp->rx_pkts, hdr->ver);
 		qp->rx_err_ver++;
 		return -EIO;
 	}
 
-	if (hdr->flags & LINK_DOWN_FLAG) {
-		ntb_qp_link_down(qp);
+	entry = ntb_list_rm(&qp->ntb_rx_pend_q_lock, &qp->rx_pend_q);
+	if (!entry) {
+		dev_dbg(&qp->ndev->pdev->dev, "no receive buffer\n");
+		qp->rx_err_no_buf++;
 
+		rc = -ENOMEM;
 		goto err;
 	}
 
-	dev_dbg(&ntb_query_pdev(qp->ndev)->dev,
-		"rx offset %u, ver %u - %d payload received, buf size %d\n",
-		qp->rx_index, hdr->ver, hdr->len, entry->len);
-
-	qp->rx_bytes += hdr->len;
-	qp->rx_pkts++;
-
 	if (hdr->len > entry->len) {
-		qp->rx_err_oflow++;
-		dev_dbg(&ntb_query_pdev(qp->ndev)->dev,
-			"RX overflow! Wanted %d got %d\n",
+		dev_dbg(&qp->ndev->pdev->dev,
+			"receive buffer overflow! Wanted %d got %d\n",
 			hdr->len, entry->len);
+		qp->rx_err_oflow++;
 
+		rc = -EIO;
 		goto err;
 	}
 
+	dev_dbg(&qp->ndev->pdev->dev,
+		"RX OK index %u ver %u size %d into buf size %d\n",
+		qp->rx_index, hdr->ver, hdr->len, entry->len);
+
+	qp->rx_bytes += hdr->len;
+	qp->rx_pkts++;
+
 	entry->index = qp->rx_index;
 	entry->rx_hdr = hdr;
 
 	ntb_async_rx(entry, offset, hdr->len);
 
-out:
 	qp->rx_index++;
 	qp->rx_index %= qp->rx_max_entry;
 
 	return 0;
 
 err:
-	ntb_list_add(&qp->ntb_rx_pend_q_lock, &entry->entry, &qp->rx_pend_q);
-	/* Ensure that the data is fully copied out before clearing the flag */
-	wmb();
+	/* FIXME: if this syncrhonous update of the rx_index gets ahead of
+	 * asyncrhonous ntb_rx_copy_callback of previous entry, there are three
+	 * scenarios:
+	 *
+	 * 1) The peer might miss this update, but observe the update
+	 * from the memcpy completion callback.  In this case, the buffer will
+	 * not be freed on the peer to be reused for a different packet.  The
+	 * successful rx of a later packet would clear the condition, but the
+	 * condition could persist if several rx fail in a row.
+	 *
+	 * 2) The peer may observe this update before the asyncrhonous copy of
+	 * prior packets is completed.  The peer may overwrite the buffers of
+	 * the prior packets before they are copied.
+	 *
+	 * 3) Both: the peer may observe the update, and then observe the index
+	 * decrement by the asynchronous completion callback.  Who knows what
+	 * badness that will cause.
+	 */
 	hdr->flags = 0;
 	iowrite32(qp->rx_index, &qp->rx_info->entry);
 
-	goto out;
+	return rc;
 }
 
-static int ntb_transport_rxc_db(void *data, int db_num)
+static void ntb_transport_rxc_db(unsigned long data)
 {
-	struct ntb_transport_qp *qp = data;
+	struct ntb_transport_qp *qp = (void *)data;
 	int rc, i;
 
-	dev_dbg(&ntb_query_pdev(qp->ndev)->dev, "%s: doorbell %d received\n",
-		__func__, db_num);
+	dev_dbg(&qp->ndev->pdev->dev, "%s: doorbell %d received\n",
+		__func__, qp->qp_num);
 
 	/* Limit the number of packets processed in a single interrupt to
 	 * provide fairness to others
@@ -1231,7 +1336,21 @@ static int ntb_transport_rxc_db(void *data, int db_num)
 	if (qp->dma_chan)
 		dma_async_issue_pending(qp->dma_chan);
 
-	return i;
+	if (i == qp->rx_max_entry) {
+		/* there is more work to do */
+		tasklet_schedule(&qp->rxc_db_work);
+	} else if (ntb_db_read(qp->ndev) & BIT_ULL(qp->qp_num)) {
+		/* the doorbell bit is set: clear it */
+		ntb_db_clear(qp->ndev, BIT_ULL(qp->qp_num));
+		/* ntb_db_read ensures ntb_db_clear write is committed */
+		ntb_db_read(qp->ndev);
+
+		/* an interrupt may have arrived between finishing
+		 * ntb_process_rxc and clearing the doorbell bit:
+		 * there might be some more work to do.
+		 */
+		tasklet_schedule(&qp->rxc_db_work);
+	}
 }
 
 static void ntb_tx_copy_callback(void *data)
@@ -1240,11 +1359,9 @@ static void ntb_tx_copy_callback(void *data)
 	struct ntb_transport_qp *qp = entry->qp;
 	struct ntb_payload_header __iomem *hdr = entry->tx_hdr;
 
-	/* Ensure that the data is fully copied out before setting the flags */
-	wmb();
 	iowrite32(entry->flags | DESC_DONE_FLAG, &hdr->flags);
 
-	ntb_ring_doorbell(qp->ndev, qp->qp_num);
+	ntb_peer_db_set(qp->ndev, BIT_ULL(qp->qp_num));
 
 	/* The entry length can only be zero if the packet is intended to be a
 	 * "link down" or similar.  Since no payload is being sent in these
@@ -1263,7 +1380,18 @@ static void ntb_tx_copy_callback(void *data)
 
 static void ntb_memcpy_tx(struct ntb_queue_entry *entry, void __iomem *offset)
 {
+#ifdef ARCH_HAS_NOCACHE_UACCESS
+	/*
+	 * Using non-temporal mov to improve performance on non-cached
+	 * writes, even though we aren't actually copying from user space.
+	 */
+	__copy_from_user_inatomic_nocache(offset, entry->buf, entry->len);
+#else
 	memcpy_toio(offset, entry->buf, entry->len);
+#endif
+
+	/* Ensure that the data is fully copied out before setting the flags */
+	wmb();
 
 	ntb_tx_copy_callback(entry);
 }
@@ -1288,7 +1416,7 @@ static void ntb_async_tx(struct ntb_transport_qp *qp,
 	entry->tx_hdr = hdr;
 
 	iowrite32(entry->len, &hdr->len);
-	iowrite32((u32) qp->tx_pkts, &hdr->ver);
+	iowrite32((u32)qp->tx_pkts, &hdr->ver);
 
 	if (!chan)
 		goto err;
@@ -1298,8 +1426,8 @@ static void ntb_async_tx(struct ntb_transport_qp *qp,
 
 	device = chan->device;
 	dest = qp->tx_mw_phys + qp->tx_max_frame * qp->tx_index;
-	buff_off = (size_t) buf & ~PAGE_MASK;
-	dest_off = (size_t) dest & ~PAGE_MASK;
+	buff_off = (size_t)buf & ~PAGE_MASK;
+	dest_off = (size_t)dest & ~PAGE_MASK;
 
 	if (!is_dma_copy_aligned(device, buff_off, dest_off, len))
 		goto err;
@@ -1347,9 +1475,6 @@ err:
 static int ntb_process_tx(struct ntb_transport_qp *qp,
 			  struct ntb_queue_entry *entry)
 {
-	dev_dbg(&ntb_query_pdev(qp->ndev)->dev, "%lld - tx %u, entry len %d flags %x buff %p\n",
-		qp->tx_pkts, qp->tx_index, entry->len, entry->flags,
-		entry->buf);
 	if (qp->tx_index == qp->remote_rx_info->entry) {
 		qp->tx_ring_full++;
 		return -EAGAIN;
@@ -1376,15 +1501,14 @@ static int ntb_process_tx(struct ntb_transport_qp *qp,
 
 static void ntb_send_link_down(struct ntb_transport_qp *qp)
 {
-	struct pci_dev *pdev = ntb_query_pdev(qp->ndev);
+	struct pci_dev *pdev = qp->ndev->pdev;
 	struct ntb_queue_entry *entry;
 	int i, rc;
 
-	if (qp->qp_link == NTB_LINK_DOWN)
+	if (!qp->link_is_up)
 		return;
 
-	qp->qp_link = NTB_LINK_DOWN;
-	dev_info(&pdev->dev, "qp %d: Link Down\n", qp->qp_num);
+	dev_info(&pdev->dev, "qp %d: Send Link Down\n", qp->qp_num);
 
 	for (i = 0; i < NTB_LINK_DOWN_TIMEOUT; i++) {
 		entry = ntb_list_rm(&qp->ntb_tx_free_q_lock, &qp->tx_free_q);
@@ -1405,6 +1529,13 @@ static void ntb_send_link_down(struct ntb_transport_qp *qp)
 	if (rc)
 		dev_err(&pdev->dev, "ntb: QP%d unable to send linkdown msg\n",
 			qp->qp_num);
+
+	ntb_qp_link_down_reset(qp);
+}
+
+static bool ntb_dma_filter_fn(struct dma_chan *chan, void *node)
+{
+	return dev_to_node(&chan->dev->device) == (int)(unsigned long)node;
 }
 
 /**
@@ -1422,18 +1553,25 @@ static void ntb_send_link_down(struct ntb_transport_qp *qp)
  * RETURNS: pointer to newly created ntb_queue, NULL on error.
  */
 struct ntb_transport_qp *
-ntb_transport_create_queue(void *data, struct pci_dev *pdev,
+ntb_transport_create_queue(void *data, struct device *client_dev,
 			   const struct ntb_queue_handlers *handlers)
 {
+	struct ntb_dev *ndev;
+	struct pci_dev *pdev;
+	struct ntb_transport_ctx *nt;
 	struct ntb_queue_entry *entry;
 	struct ntb_transport_qp *qp;
-	struct ntb_transport *nt;
+	u64 qp_bit;
 	unsigned int free_queue;
-	int rc, i;
+	dma_cap_mask_t dma_mask;
+	int node;
+	int i;
 
-	nt = ntb_find_transport(pdev);
-	if (!nt)
-		goto err;
+	ndev = dev_ntb(client_dev->parent);
+	pdev = ndev->pdev;
+	nt = ndev->ctx;
+
+	node = dev_to_node(&ndev->dev);
 
 	free_queue = ffs(nt->qp_bitmap);
 	if (!free_queue)
@@ -1442,23 +1580,31 @@ ntb_transport_create_queue(void *data, struct pci_dev *pdev,
 	/* decrement free_queue to make it zero based */
 	free_queue--;
 
-	clear_bit(free_queue, &nt->qp_bitmap);
+	qp = &nt->qp_vec[free_queue];
+	qp_bit = BIT_ULL(qp->qp_num);
+
+	nt->qp_bitmap_free &= ~qp_bit;
 
-	qp = &nt->qps[free_queue];
 	qp->cb_data = data;
 	qp->rx_handler = handlers->rx_handler;
 	qp->tx_handler = handlers->tx_handler;
 	qp->event_handler = handlers->event_handler;
 
-	dmaengine_get();
-	qp->dma_chan = dma_find_channel(DMA_MEMCPY);
-	if (!qp->dma_chan) {
-		dmaengine_put();
-		dev_info(&pdev->dev, "Unable to allocate DMA channel, using CPU instead\n");
+	dma_cap_zero(dma_mask);
+	dma_cap_set(DMA_MEMCPY, dma_mask);
+
+	if (use_dma) {
+		qp->dma_chan = dma_request_channel(dma_mask, ntb_dma_filter_fn,
+						   (void *)(unsigned long)node);
+		if (!qp->dma_chan)
+			dev_info(&pdev->dev, "Unable to allocate DMA channel\n");
+	} else {
+		qp->dma_chan = NULL;
 	}
+	dev_dbg(&pdev->dev, "Using %s memcpy\n", qp->dma_chan ? "DMA" : "CPU");
 
 	for (i = 0; i < NTB_QP_DEF_NUM_ENTRIES; i++) {
-		entry = kzalloc(sizeof(struct ntb_queue_entry), GFP_ATOMIC);
+		entry = kzalloc_node(sizeof(*entry), GFP_ATOMIC, node);
 		if (!entry)
 			goto err1;
 
@@ -1468,7 +1614,7 @@ ntb_transport_create_queue(void *data, struct pci_dev *pdev,
 	}
 
 	for (i = 0; i < NTB_QP_DEF_NUM_ENTRIES; i++) {
-		entry = kzalloc(sizeof(struct ntb_queue_entry), GFP_ATOMIC);
+		entry = kzalloc_node(sizeof(*entry), GFP_ATOMIC, node);
 		if (!entry)
 			goto err2;
 
@@ -1477,10 +1623,8 @@ ntb_transport_create_queue(void *data, struct pci_dev *pdev,
 			     &qp->tx_free_q);
 	}
 
-	rc = ntb_register_db_callback(qp->ndev, free_queue, qp,
-				      ntb_transport_rxc_db);
-	if (rc)
-		goto err2;
+	ntb_db_clear(qp->ndev, qp_bit);
+	ntb_db_clear_mask(qp->ndev, qp_bit);
 
 	dev_info(&pdev->dev, "NTB Transport QP %d created\n", qp->qp_num);
 
@@ -1493,8 +1637,8 @@ err1:
 	while ((entry = ntb_list_rm(&qp->ntb_rx_free_q_lock, &qp->rx_free_q)))
 		kfree(entry);
 	if (qp->dma_chan)
-		dmaengine_put();
-	set_bit(free_queue, &nt->qp_bitmap);
+		dma_release_channel(qp->dma_chan);
+	nt->qp_bitmap_free |= qp_bit;
 err:
 	return NULL;
 }
@@ -1508,13 +1652,15 @@ EXPORT_SYMBOL_GPL(ntb_transport_create_queue);
  */
 void ntb_transport_free_queue(struct ntb_transport_qp *qp)
 {
+	struct ntb_transport_ctx *nt = qp->transport;
 	struct pci_dev *pdev;
 	struct ntb_queue_entry *entry;
+	u64 qp_bit;
 
 	if (!qp)
 		return;
 
-	pdev = ntb_query_pdev(qp->ndev);
+	pdev = qp->ndev->pdev;
 
 	if (qp->dma_chan) {
 		struct dma_chan *chan = qp->dma_chan;
@@ -1528,13 +1674,21 @@ void ntb_transport_free_queue(struct ntb_transport_qp *qp)
 		 */
 		dma_sync_wait(chan, qp->last_cookie);
 		dmaengine_terminate_all(chan);
-		dmaengine_put();
+		dma_release_channel(chan);
 	}
 
-	ntb_unregister_db_callback(qp->ndev, qp->qp_num);
+	qp_bit = BIT_ULL(qp->qp_num);
+
+	ntb_db_set_mask(qp->ndev, qp_bit);
+	tasklet_disable(&qp->rxc_db_work);
 
 	cancel_delayed_work_sync(&qp->link_work);
 
+	qp->cb_data = NULL;
+	qp->rx_handler = NULL;
+	qp->tx_handler = NULL;
+	qp->event_handler = NULL;
+
 	while ((entry = ntb_list_rm(&qp->ntb_rx_free_q_lock, &qp->rx_free_q)))
 		kfree(entry);
 
@@ -1546,7 +1700,7 @@ void ntb_transport_free_queue(struct ntb_transport_qp *qp)
 	while ((entry = ntb_list_rm(&qp->ntb_tx_free_q_lock, &qp->tx_free_q)))
 		kfree(entry);
 
-	set_bit(qp->qp_num, &qp->transport->qp_bitmap);
+	nt->qp_bitmap_free |= qp_bit;
 
 	dev_info(&pdev->dev, "NTB Transport QP %d freed\n", qp->qp_num);
 }
@@ -1567,7 +1721,7 @@ void *ntb_transport_rx_remove(struct ntb_transport_qp *qp, unsigned int *len)
 	struct ntb_queue_entry *entry;
 	void *buf;
 
-	if (!qp || qp->client_ready == NTB_LINK_UP)
+	if (!qp || qp->client_ready)
 		return NULL;
 
 	entry = ntb_list_rm(&qp->ntb_rx_pend_q_lock, &qp->rx_pend_q);
@@ -1636,7 +1790,7 @@ int ntb_transport_tx_enqueue(struct ntb_transport_qp *qp, void *cb, void *data,
 	struct ntb_queue_entry *entry;
 	int rc;
 
-	if (!qp || qp->qp_link != NTB_LINK_UP || !len)
+	if (!qp || !qp->link_is_up || !len)
 		return -EINVAL;
 
 	entry = ntb_list_rm(&qp->ntb_tx_free_q_lock, &qp->tx_free_q);
@@ -1670,9 +1824,9 @@ void ntb_transport_link_up(struct ntb_transport_qp *qp)
 	if (!qp)
 		return;
 
-	qp->client_ready = NTB_LINK_UP;
+	qp->client_ready = true;
 
-	if (qp->transport->transport_link == NTB_LINK_UP)
+	if (qp->transport->link_is_up)
 		schedule_delayed_work(&qp->link_work, 0);
 }
 EXPORT_SYMBOL_GPL(ntb_transport_link_up);
@@ -1688,27 +1842,20 @@ EXPORT_SYMBOL_GPL(ntb_transport_link_up);
 void ntb_transport_link_down(struct ntb_transport_qp *qp)
 {
 	struct pci_dev *pdev;
-	int rc, val;
+	int val;
 
 	if (!qp)
 		return;
 
-	pdev = ntb_query_pdev(qp->ndev);
-	qp->client_ready = NTB_LINK_DOWN;
+	pdev = qp->ndev->pdev;
+	qp->client_ready = false;
 
-	rc = ntb_read_local_spad(qp->ndev, QP_LINKS, &val);
-	if (rc) {
-		dev_err(&pdev->dev, "Error reading spad %d\n", QP_LINKS);
-		return;
-	}
+	val = ntb_spad_read(qp->ndev, QP_LINKS);
 
-	rc = ntb_write_remote_spad(qp->ndev, QP_LINKS,
-				   val & ~(1 << qp->qp_num));
-	if (rc)
-		dev_err(&pdev->dev, "Error writing %x to remote spad %d\n",
-			val & ~(1 << qp->qp_num), QP_LINKS);
+	ntb_peer_spad_write(qp->ndev, QP_LINKS,
+			    val & ~BIT(qp->qp_num));
 
-	if (qp->qp_link == NTB_LINK_UP)
+	if (qp->link_is_up)
 		ntb_send_link_down(qp);
 	else
 		cancel_delayed_work_sync(&qp->link_work);
@@ -1728,7 +1875,7 @@ bool ntb_transport_link_query(struct ntb_transport_qp *qp)
 	if (!qp)
 		return false;
 
-	return qp->qp_link == NTB_LINK_UP;
+	return qp->link_is_up;
 }
 EXPORT_SYMBOL_GPL(ntb_transport_link_query);
 
@@ -1774,3 +1921,71 @@ unsigned int ntb_transport_max_size(struct ntb_transport_qp *qp)
 	return max;
 }
 EXPORT_SYMBOL_GPL(ntb_transport_max_size);
+
+static void ntb_transport_doorbell_callback(void *data, int vector)
+{
+	struct ntb_transport_ctx *nt = data;
+	struct ntb_transport_qp *qp;
+	u64 db_bits;
+	unsigned int qp_num;
+
+	db_bits = (nt->qp_bitmap & ~nt->qp_bitmap_free &
+		   ntb_db_vector_mask(nt->ndev, vector));
+
+	while (db_bits) {
+		qp_num = __ffs(db_bits);
+		qp = &nt->qp_vec[qp_num];
+
+		tasklet_schedule(&qp->rxc_db_work);
+
+		db_bits &= ~BIT_ULL(qp_num);
+	}
+}
+
+static const struct ntb_ctx_ops ntb_transport_ops = {
+	.link_event = ntb_transport_event_callback,
+	.db_event = ntb_transport_doorbell_callback,
+};
+
+static struct ntb_client ntb_transport_client = {
+	.ops = {
+		.probe = ntb_transport_probe,
+		.remove = ntb_transport_free,
+	},
+};
+
+static int __init ntb_transport_init(void)
+{
+	int rc;
+
+	pr_info("%s, version %s\n", NTB_TRANSPORT_DESC, NTB_TRANSPORT_VER);
+
+	if (debugfs_initialized())
+		nt_debugfs_dir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+
+	rc = bus_register(&ntb_transport_bus);
+	if (rc)
+		goto err_bus;
+
+	rc = ntb_register_client(&ntb_transport_client);
+	if (rc)
+		goto err_client;
+
+	return 0;
+
+err_client:
+	bus_unregister(&ntb_transport_bus);
+err_bus:
+	debugfs_remove_recursive(nt_debugfs_dir);
+	return rc;
+}
+module_init(ntb_transport_init);
+
+static void __exit ntb_transport_exit(void)
+{
+	debugfs_remove_recursive(nt_debugfs_dir);
+
+	ntb_unregister_client(&ntb_transport_client);
+	bus_unregister(&ntb_transport_bus);
+}
+module_exit(ntb_transport_exit);
diff --git a/drivers/ntb/test/Kconfig b/drivers/ntb/test/Kconfig
new file mode 100644
index 000000000000..01852f98a843
--- /dev/null
+++ b/drivers/ntb/test/Kconfig
@@ -0,0 +1,19 @@
+config NTB_PINGPONG
+	tristate "NTB Ping Pong Test Client"
+	help
+	 This is a simple ping pong driver that exercises the scratchpads and
+	 doorbells of the ntb hardware.  This driver may be used to test that
+	 your ntb hardware and drivers are functioning at a basic level.
+
+	 If unsure, say N.
+
+config NTB_TOOL
+	tristate "NTB Debugging Tool Test Client"
+	help
+	 This is a simple debugging driver that enables the doorbell and
+	 scratchpad registers to be read and written from the debugfs.  This
+	 enables more complicated debugging to be scripted from user space.
+	 This driver may be used to test that your ntb hardware and drivers are
+	 functioning at a basic level.
+
+	 If unsure, say N.
diff --git a/drivers/ntb/test/Makefile b/drivers/ntb/test/Makefile
new file mode 100644
index 000000000000..0ea32a324b6c
--- /dev/null
+++ b/drivers/ntb/test/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_NTB_PINGPONG) += ntb_pingpong.o
+obj-$(CONFIG_NTB_TOOL) += ntb_tool.o
diff --git a/drivers/ntb/test/ntb_pingpong.c b/drivers/ntb/test/ntb_pingpong.c
new file mode 100644
index 000000000000..fe1600566981
--- /dev/null
+++ b/drivers/ntb/test/ntb_pingpong.c
@@ -0,0 +1,250 @@
+/*
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ *   redistributing this file, you may do so under either license.
+ *
+ *   GPL LICENSE SUMMARY
+ *
+ *   Copyright (C) 2015 EMC 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.
+ *
+ *   BSD LICENSE
+ *
+ *   Copyright (C) 2015 EMC Corporation. All Rights Reserved.
+ *
+ *   Redistribution and use in source and binary forms, with or without
+ *   modification, are permitted provided that the following conditions
+ *   are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copy
+ *       notice, this list of conditions and the following disclaimer in
+ *       the documentation and/or other materials provided with the
+ *       distribution.
+ *     * Neither the name of Intel Corporation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * PCIe NTB Pingpong Linux driver
+ *
+ * Contact Information:
+ * Allen Hubbe <Allen.Hubbe@emc.com>
+ */
+
+/* Note: load this module with option 'dyndbg=+p' */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include <linux/ntb.h>
+
+#define DRIVER_NAME			"ntb_pingpong"
+#define DRIVER_DESCRIPTION		"PCIe NTB Simple Pingpong Client"
+
+#define DRIVER_LICENSE			"Dual BSD/GPL"
+#define DRIVER_VERSION			"1.0"
+#define DRIVER_RELDATE			"24 March 2015"
+#define DRIVER_AUTHOR			"Allen Hubbe <Allen.Hubbe@emc.com>"
+
+MODULE_LICENSE(DRIVER_LICENSE);
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESCRIPTION);
+
+static unsigned int unsafe;
+module_param(unsafe, uint, 0644);
+MODULE_PARM_DESC(unsafe, "Run even though ntb operations may be unsafe");
+
+static unsigned int delay_ms = 1000;
+module_param(delay_ms, uint, 0644);
+MODULE_PARM_DESC(delay_ms, "Milliseconds to delay the response to peer");
+
+static unsigned long db_init = 0x7;
+module_param(db_init, ulong, 0644);
+MODULE_PARM_DESC(delay_ms, "Initial doorbell bits to ring on the peer");
+
+struct pp_ctx {
+	struct ntb_dev			*ntb;
+	u64				db_bits;
+	/* synchronize access to db_bits by ping and pong */
+	spinlock_t			db_lock;
+	struct timer_list		db_timer;
+	unsigned long			db_delay;
+};
+
+static void pp_ping(unsigned long ctx)
+{
+	struct pp_ctx *pp = (void *)ctx;
+	unsigned long irqflags;
+	u64 db_bits, db_mask;
+	u32 spad_rd, spad_wr;
+
+	spin_lock_irqsave(&pp->db_lock, irqflags);
+	{
+		db_mask = ntb_db_valid_mask(pp->ntb);
+		db_bits = ntb_db_read(pp->ntb);
+
+		if (db_bits) {
+			dev_dbg(&pp->ntb->dev,
+				"Masked pongs %#llx\n",
+				db_bits);
+			ntb_db_clear(pp->ntb, db_bits);
+		}
+
+		db_bits = ((pp->db_bits | db_bits) << 1) & db_mask;
+
+		if (!db_bits)
+			db_bits = db_init;
+
+		spad_rd = ntb_spad_read(pp->ntb, 0);
+		spad_wr = spad_rd + 1;
+
+		dev_dbg(&pp->ntb->dev,
+			"Ping bits %#llx read %#x write %#x\n",
+			db_bits, spad_rd, spad_wr);
+
+		ntb_peer_spad_write(pp->ntb, 0, spad_wr);
+		ntb_peer_db_set(pp->ntb, db_bits);
+		ntb_db_clear_mask(pp->ntb, db_mask);
+
+		pp->db_bits = 0;
+	}
+	spin_unlock_irqrestore(&pp->db_lock, irqflags);
+}
+
+static void pp_link_event(void *ctx)
+{
+	struct pp_ctx *pp = ctx;
+
+	if (ntb_link_is_up(pp->ntb, NULL, NULL) == 1) {
+		dev_dbg(&pp->ntb->dev, "link is up\n");
+		pp_ping((unsigned long)pp);
+	} else {
+		dev_dbg(&pp->ntb->dev, "link is down\n");
+		del_timer(&pp->db_timer);
+	}
+}
+
+static void pp_db_event(void *ctx, int vec)
+{
+	struct pp_ctx *pp = ctx;
+	u64 db_bits, db_mask;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&pp->db_lock, irqflags);
+	{
+		db_mask = ntb_db_vector_mask(pp->ntb, vec);
+		db_bits = db_mask & ntb_db_read(pp->ntb);
+		ntb_db_set_mask(pp->ntb, db_mask);
+		ntb_db_clear(pp->ntb, db_bits);
+
+		pp->db_bits |= db_bits;
+
+		mod_timer(&pp->db_timer, jiffies + pp->db_delay);
+
+		dev_dbg(&pp->ntb->dev,
+			"Pong vec %d bits %#llx\n",
+			vec, db_bits);
+	}
+	spin_unlock_irqrestore(&pp->db_lock, irqflags);
+}
+
+static const struct ntb_ctx_ops pp_ops = {
+	.link_event = pp_link_event,
+	.db_event = pp_db_event,
+};
+
+static int pp_probe(struct ntb_client *client,
+		    struct ntb_dev *ntb)
+{
+	struct pp_ctx *pp;
+	int rc;
+
+	if (ntb_db_is_unsafe(ntb)) {
+		dev_dbg(&ntb->dev, "doorbell is unsafe\n");
+		if (!unsafe) {
+			rc = -EINVAL;
+			goto err_pp;
+		}
+	}
+
+	if (ntb_spad_is_unsafe(ntb)) {
+		dev_dbg(&ntb->dev, "scratchpad is unsafe\n");
+		if (!unsafe) {
+			rc = -EINVAL;
+			goto err_pp;
+		}
+	}
+
+	pp = kmalloc(sizeof(*pp), GFP_KERNEL);
+	if (!pp) {
+		rc = -ENOMEM;
+		goto err_pp;
+	}
+
+	pp->ntb = ntb;
+	pp->db_bits = 0;
+	spin_lock_init(&pp->db_lock);
+	setup_timer(&pp->db_timer, pp_ping, (unsigned long)pp);
+	pp->db_delay = msecs_to_jiffies(delay_ms);
+
+	rc = ntb_set_ctx(ntb, pp, &pp_ops);
+	if (rc)
+		goto err_ctx;
+
+	ntb_link_enable(ntb, NTB_SPEED_AUTO, NTB_WIDTH_AUTO);
+	ntb_link_event(ntb);
+
+	return 0;
+
+err_ctx:
+	kfree(pp);
+err_pp:
+	return rc;
+}
+
+static void pp_remove(struct ntb_client *client,
+		      struct ntb_dev *ntb)
+{
+	struct pp_ctx *pp = ntb->ctx;
+
+	ntb_clear_ctx(ntb);
+	del_timer_sync(&pp->db_timer);
+	ntb_link_disable(ntb);
+
+	kfree(pp);
+}
+
+static struct ntb_client pp_client = {
+	.ops = {
+		.probe = pp_probe,
+		.remove = pp_remove,
+	},
+};
+module_ntb_client(pp_client);
diff --git a/drivers/ntb/test/ntb_tool.c b/drivers/ntb/test/ntb_tool.c
new file mode 100644
index 000000000000..6f5dc6ca673d
--- /dev/null
+++ b/drivers/ntb/test/ntb_tool.c
@@ -0,0 +1,556 @@
+/*
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ *   redistributing this file, you may do so under either license.
+ *
+ *   GPL LICENSE SUMMARY
+ *
+ *   Copyright (C) 2015 EMC 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.
+ *
+ *   BSD LICENSE
+ *
+ *   Copyright (C) 2015 EMC Corporation. All Rights Reserved.
+ *
+ *   Redistribution and use in source and binary forms, with or without
+ *   modification, are permitted provided that the following conditions
+ *   are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copy
+ *       notice, this list of conditions and the following disclaimer in
+ *       the documentation and/or other materials provided with the
+ *       distribution.
+ *     * Neither the name of Intel Corporation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * PCIe NTB Debugging Tool Linux driver
+ *
+ * Contact Information:
+ * Allen Hubbe <Allen.Hubbe@emc.com>
+ */
+
+/*
+ * How to use this tool, by example.
+ *
+ * Assuming $DBG_DIR is something like:
+ * '/sys/kernel/debug/ntb_tool/0000:00:03.0'
+ *
+ * Eg: check if clearing the doorbell mask generates an interrupt.
+ *
+ * # Set the doorbell mask
+ * root@self# echo 's 1' > $DBG_DIR/mask
+ *
+ * # Ring the doorbell from the peer
+ * root@peer# echo 's 1' > $DBG_DIR/peer_db
+ *
+ * # Clear the doorbell mask
+ * root@self# echo 'c 1' > $DBG_DIR/mask
+ *
+ * Observe debugging output in dmesg or your console.  You should see a
+ * doorbell event triggered by clearing the mask.  If not, this may indicate an
+ * issue with the hardware that needs to be worked around in the driver.
+ *
+ * Eg: read and write scratchpad registers
+ *
+ * root@peer# echo '0 0x01010101 1 0x7f7f7f7f' > $DBG_DIR/peer_spad
+ *
+ * root@self# cat $DBG_DIR/spad
+ *
+ * Observe that spad 0 and 1 have the values set by the peer.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include <linux/debugfs.h>
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include <linux/ntb.h>
+
+#define DRIVER_NAME			"ntb_tool"
+#define DRIVER_DESCRIPTION		"PCIe NTB Debugging Tool"
+
+#define DRIVER_LICENSE			"Dual BSD/GPL"
+#define DRIVER_VERSION			"1.0"
+#define DRIVER_RELDATE			"22 April 2015"
+#define DRIVER_AUTHOR			"Allen Hubbe <Allen.Hubbe@emc.com>"
+
+MODULE_LICENSE(DRIVER_LICENSE);
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESCRIPTION);
+
+static struct dentry *tool_dbgfs;
+
+struct tool_ctx {
+	struct ntb_dev *ntb;
+	struct dentry *dbgfs;
+};
+
+#define SPAD_FNAME_SIZE 0x10
+#define INT_PTR(x) ((void *)(unsigned long)x)
+#define PTR_INT(x) ((int)(unsigned long)x)
+
+#define TOOL_FOPS_RDWR(__name, __read, __write) \
+	const struct file_operations __name = {	\
+		.owner = THIS_MODULE,		\
+		.open = simple_open,		\
+		.read = __read,			\
+		.write = __write,		\
+	}
+
+static void tool_link_event(void *ctx)
+{
+	struct tool_ctx *tc = ctx;
+	enum ntb_speed speed;
+	enum ntb_width width;
+	int up;
+
+	up = ntb_link_is_up(tc->ntb, &speed, &width);
+
+	dev_dbg(&tc->ntb->dev, "link is %s speed %d width %d\n",
+		up ? "up" : "down", speed, width);
+}
+
+static void tool_db_event(void *ctx, int vec)
+{
+	struct tool_ctx *tc = ctx;
+	u64 db_bits, db_mask;
+
+	db_mask = ntb_db_vector_mask(tc->ntb, vec);
+	db_bits = ntb_db_read(tc->ntb);
+
+	dev_dbg(&tc->ntb->dev, "doorbell vec %d mask %#llx bits %#llx\n",
+		vec, db_mask, db_bits);
+}
+
+static const struct ntb_ctx_ops tool_ops = {
+	.link_event = tool_link_event,
+	.db_event = tool_db_event,
+};
+
+static ssize_t tool_dbfn_read(struct tool_ctx *tc, char __user *ubuf,
+			      size_t size, loff_t *offp,
+			      u64 (*db_read_fn)(struct ntb_dev *))
+{
+	size_t buf_size;
+	char *buf;
+	ssize_t pos, rc;
+
+	if (!db_read_fn)
+		return -EINVAL;
+
+	buf_size = min_t(size_t, size, 0x20);
+
+	buf = kmalloc(buf_size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	pos = scnprintf(buf, buf_size, "%#llx\n",
+			db_read_fn(tc->ntb));
+
+	rc = simple_read_from_buffer(ubuf, size, offp, buf, pos);
+
+	kfree(buf);
+
+	return rc;
+}
+
+static ssize_t tool_dbfn_write(struct tool_ctx *tc,
+			       const char __user *ubuf,
+			       size_t size, loff_t *offp,
+			       int (*db_set_fn)(struct ntb_dev *, u64),
+			       int (*db_clear_fn)(struct ntb_dev *, u64))
+{
+	u64 db_bits;
+	char *buf, cmd;
+	ssize_t rc;
+	int n;
+
+	buf = kmalloc(size + 1, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	rc = simple_write_to_buffer(buf, size, offp, ubuf, size);
+	if (rc < 0) {
+		kfree(buf);
+		return rc;
+	}
+
+	buf[size] = 0;
+
+	n = sscanf(buf, "%c %lli", &cmd, &db_bits);
+
+	kfree(buf);
+
+	if (n != 2) {
+		rc = -EINVAL;
+	} else if (cmd == 's') {
+		if (!db_set_fn)
+			rc = -EINVAL;
+		else
+			rc = db_set_fn(tc->ntb, db_bits);
+	} else if (cmd == 'c') {
+		if (!db_clear_fn)
+			rc = -EINVAL;
+		else
+			rc = db_clear_fn(tc->ntb, db_bits);
+	} else {
+		rc = -EINVAL;
+	}
+
+	return rc ? : size;
+}
+
+static ssize_t tool_spadfn_read(struct tool_ctx *tc, char __user *ubuf,
+				size_t size, loff_t *offp,
+				u32 (*spad_read_fn)(struct ntb_dev *, int))
+{
+	size_t buf_size;
+	char *buf;
+	ssize_t pos, rc;
+	int i, spad_count;
+
+	if (!spad_read_fn)
+		return -EINVAL;
+
+	buf_size = min_t(size_t, size, 0x100);
+
+	buf = kmalloc(buf_size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	pos = 0;
+
+	spad_count = ntb_spad_count(tc->ntb);
+	for (i = 0; i < spad_count; ++i) {
+		pos += scnprintf(buf + pos, buf_size - pos, "%d\t%#x\n",
+				 i, spad_read_fn(tc->ntb, i));
+	}
+
+	rc = simple_read_from_buffer(ubuf, size, offp, buf, pos);
+
+	kfree(buf);
+
+	return rc;
+}
+
+static ssize_t tool_spadfn_write(struct tool_ctx *tc,
+				 const char __user *ubuf,
+				 size_t size, loff_t *offp,
+				 int (*spad_write_fn)(struct ntb_dev *,
+						      int, u32))
+{
+	int spad_idx;
+	u32 spad_val;
+	char *buf;
+	int pos, n;
+	ssize_t rc;
+
+	if (!spad_write_fn) {
+		dev_dbg(&tc->ntb->dev, "no spad write fn\n");
+		return -EINVAL;
+	}
+
+	buf = kmalloc(size + 1, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	rc = simple_write_to_buffer(buf, size, offp, ubuf, size);
+	if (rc < 0) {
+		kfree(buf);
+		return rc;
+	}
+
+	buf[size] = 0;
+
+	n = sscanf(buf, "%d %i%n", &spad_idx, &spad_val, &pos);
+	while (n == 2) {
+		rc = spad_write_fn(tc->ntb, spad_idx, spad_val);
+		if (rc)
+			break;
+
+		n = sscanf(buf + pos, "%d %i%n", &spad_idx, &spad_val, &pos);
+	}
+
+	if (n < 0)
+		rc = n;
+
+	kfree(buf);
+
+	return rc ? : size;
+}
+
+static ssize_t tool_db_read(struct file *filep, char __user *ubuf,
+			    size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_dbfn_read(tc, ubuf, size, offp,
+			      tc->ntb->ops->db_read);
+}
+
+static ssize_t tool_db_write(struct file *filep, const char __user *ubuf,
+			     size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_dbfn_write(tc, ubuf, size, offp,
+			       tc->ntb->ops->db_set,
+			       tc->ntb->ops->db_clear);
+}
+
+static TOOL_FOPS_RDWR(tool_db_fops,
+		      tool_db_read,
+		      tool_db_write);
+
+static ssize_t tool_mask_read(struct file *filep, char __user *ubuf,
+			      size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_dbfn_read(tc, ubuf, size, offp,
+			      tc->ntb->ops->db_read_mask);
+}
+
+static ssize_t tool_mask_write(struct file *filep, const char __user *ubuf,
+			       size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_dbfn_write(tc, ubuf, size, offp,
+			       tc->ntb->ops->db_set_mask,
+			       tc->ntb->ops->db_clear_mask);
+}
+
+static TOOL_FOPS_RDWR(tool_mask_fops,
+		      tool_mask_read,
+		      tool_mask_write);
+
+static ssize_t tool_peer_db_read(struct file *filep, char __user *ubuf,
+				 size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_dbfn_read(tc, ubuf, size, offp,
+			      tc->ntb->ops->peer_db_read);
+}
+
+static ssize_t tool_peer_db_write(struct file *filep, const char __user *ubuf,
+				  size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_dbfn_write(tc, ubuf, size, offp,
+			       tc->ntb->ops->peer_db_set,
+			       tc->ntb->ops->peer_db_clear);
+}
+
+static TOOL_FOPS_RDWR(tool_peer_db_fops,
+		      tool_peer_db_read,
+		      tool_peer_db_write);
+
+static ssize_t tool_peer_mask_read(struct file *filep, char __user *ubuf,
+				   size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_dbfn_read(tc, ubuf, size, offp,
+			      tc->ntb->ops->peer_db_read_mask);
+}
+
+static ssize_t tool_peer_mask_write(struct file *filep, const char __user *ubuf,
+				    size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_dbfn_write(tc, ubuf, size, offp,
+			       tc->ntb->ops->peer_db_set_mask,
+			       tc->ntb->ops->peer_db_clear_mask);
+}
+
+static TOOL_FOPS_RDWR(tool_peer_mask_fops,
+		      tool_peer_mask_read,
+		      tool_peer_mask_write);
+
+static ssize_t tool_spad_read(struct file *filep, char __user *ubuf,
+			      size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_spadfn_read(tc, ubuf, size, offp,
+				tc->ntb->ops->spad_read);
+}
+
+static ssize_t tool_spad_write(struct file *filep, const char __user *ubuf,
+			       size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_spadfn_write(tc, ubuf, size, offp,
+				 tc->ntb->ops->spad_write);
+}
+
+static TOOL_FOPS_RDWR(tool_spad_fops,
+		      tool_spad_read,
+		      tool_spad_write);
+
+static ssize_t tool_peer_spad_read(struct file *filep, char __user *ubuf,
+				   size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_spadfn_read(tc, ubuf, size, offp,
+				tc->ntb->ops->peer_spad_read);
+}
+
+static ssize_t tool_peer_spad_write(struct file *filep, const char __user *ubuf,
+				    size_t size, loff_t *offp)
+{
+	struct tool_ctx *tc = filep->private_data;
+
+	return tool_spadfn_write(tc, ubuf, size, offp,
+				 tc->ntb->ops->peer_spad_write);
+}
+
+static TOOL_FOPS_RDWR(tool_peer_spad_fops,
+		      tool_peer_spad_read,
+		      tool_peer_spad_write);
+
+static void tool_setup_dbgfs(struct tool_ctx *tc)
+{
+	/* This modules is useless without dbgfs... */
+	if (!tool_dbgfs) {
+		tc->dbgfs = NULL;
+		return;
+	}
+
+	tc->dbgfs = debugfs_create_dir(dev_name(&tc->ntb->dev),
+				       tool_dbgfs);
+	if (!tc->dbgfs)
+		return;
+
+	debugfs_create_file("db", S_IRUSR | S_IWUSR, tc->dbgfs,
+			    tc, &tool_db_fops);
+
+	debugfs_create_file("mask", S_IRUSR | S_IWUSR, tc->dbgfs,
+			    tc, &tool_mask_fops);
+
+	debugfs_create_file("peer_db", S_IRUSR | S_IWUSR, tc->dbgfs,
+			    tc, &tool_peer_db_fops);
+
+	debugfs_create_file("peer_mask", S_IRUSR | S_IWUSR, tc->dbgfs,
+			    tc, &tool_peer_mask_fops);
+
+	debugfs_create_file("spad", S_IRUSR | S_IWUSR, tc->dbgfs,
+			    tc, &tool_spad_fops);
+
+	debugfs_create_file("peer_spad", S_IRUSR | S_IWUSR, tc->dbgfs,
+			    tc, &tool_peer_spad_fops);
+}
+
+static int tool_probe(struct ntb_client *self, struct ntb_dev *ntb)
+{
+	struct tool_ctx *tc;
+	int rc;
+
+	if (ntb_db_is_unsafe(ntb))
+		dev_dbg(&ntb->dev, "doorbell is unsafe\n");
+
+	if (ntb_spad_is_unsafe(ntb))
+		dev_dbg(&ntb->dev, "scratchpad is unsafe\n");
+
+	tc = kmalloc(sizeof(*tc), GFP_KERNEL);
+	if (!tc) {
+		rc = -ENOMEM;
+		goto err_tc;
+	}
+
+	tc->ntb = ntb;
+
+	tool_setup_dbgfs(tc);
+
+	rc = ntb_set_ctx(ntb, tc, &tool_ops);
+	if (rc)
+		goto err_ctx;
+
+	ntb_link_enable(ntb, NTB_SPEED_AUTO, NTB_WIDTH_AUTO);
+	ntb_link_event(ntb);
+
+	return 0;
+
+err_ctx:
+	debugfs_remove_recursive(tc->dbgfs);
+	kfree(tc);
+err_tc:
+	return rc;
+}
+
+static void tool_remove(struct ntb_client *self, struct ntb_dev *ntb)
+{
+	struct tool_ctx *tc = ntb->ctx;
+
+	ntb_clear_ctx(ntb);
+	ntb_link_disable(ntb);
+
+	debugfs_remove_recursive(tc->dbgfs);
+	kfree(tc);
+}
+
+static struct ntb_client tool_client = {
+	.ops = {
+		.probe = tool_probe,
+		.remove = tool_remove,
+	},
+};
+
+static int __init tool_init(void)
+{
+	int rc;
+
+	if (debugfs_initialized())
+		tool_dbgfs = debugfs_create_dir(KBUILD_MODNAME, NULL);
+
+	rc = ntb_register_client(&tool_client);
+	if (rc)
+		goto err_client;
+
+	return 0;
+
+err_client:
+	debugfs_remove_recursive(tool_dbgfs);
+	return rc;
+}
+module_init(tool_init);
+
+static void __exit tool_exit(void)
+{
+	ntb_unregister_client(&tool_client);
+	debugfs_remove_recursive(tool_dbgfs);
+}
+module_exit(tool_exit);
diff --git a/drivers/nvdimm/Kconfig b/drivers/nvdimm/Kconfig
new file mode 100644
index 000000000000..72226acb5c0f
--- /dev/null
+++ b/drivers/nvdimm/Kconfig
@@ -0,0 +1,68 @@
+menuconfig LIBNVDIMM
+	tristate "NVDIMM (Non-Volatile Memory Device) Support"
+	depends on PHYS_ADDR_T_64BIT
+	depends on BLK_DEV
+	help
+	  Generic support for non-volatile memory devices including
+	  ACPI-6-NFIT defined resources.  On platforms that define an
+	  NFIT, or otherwise can discover NVDIMM resources, a libnvdimm
+	  bus is registered to advertise PMEM (persistent memory)
+	  namespaces (/dev/pmemX) and BLK (sliding mmio window(s))
+	  namespaces (/dev/ndblkX.Y). A PMEM namespace refers to a
+	  memory resource that may span multiple DIMMs and support DAX
+	  (see CONFIG_DAX).  A BLK namespace refers to an NVDIMM control
+	  region which exposes an mmio register set for windowed access
+	  mode to non-volatile memory.
+
+if LIBNVDIMM
+
+config BLK_DEV_PMEM
+	tristate "PMEM: Persistent memory block device support"
+	default LIBNVDIMM
+	depends on HAS_IOMEM
+	select ND_BTT if BTT
+	help
+	  Memory ranges for PMEM are described by either an NFIT
+	  (NVDIMM Firmware Interface Table, see CONFIG_NFIT_ACPI), a
+	  non-standard OEM-specific E820 memory type (type-12, see
+	  CONFIG_X86_PMEM_LEGACY), or it is manually specified by the
+	  'memmap=nn[KMG]!ss[KMG]' kernel command line (see
+	  Documentation/kernel-parameters.txt).  This driver converts
+	  these persistent memory ranges into block devices that are
+	  capable of DAX (direct-access) file system mappings.  See
+	  Documentation/nvdimm/nvdimm.txt for more details.
+
+	  Say Y if you want to use an NVDIMM
+
+config ND_BLK
+	tristate "BLK: Block data window (aperture) device support"
+	default LIBNVDIMM
+	select ND_BTT if BTT
+	help
+	  Support NVDIMMs, or other devices, that implement a BLK-mode
+	  access capability.  BLK-mode access uses memory-mapped-i/o
+	  apertures to access persistent media.
+
+	  Say Y if your platform firmware emits an ACPI.NFIT table
+	  (CONFIG_ACPI_NFIT), or otherwise exposes BLK-mode
+	  capabilities.
+
+config ND_BTT
+	tristate
+
+config BTT
+	bool "BTT: Block Translation Table (atomic sector updates)"
+	default y if LIBNVDIMM
+	help
+	  The Block Translation Table (BTT) provides atomic sector
+	  update semantics for persistent memory devices, so that
+	  applications that rely on sector writes not being torn (a
+	  guarantee that typical disks provide) can continue to do so.
+	  The BTT manifests itself as an alternate personality for an
+	  NVDIMM namespace, i.e. a namespace can be in raw mode (pmemX,
+	  ndblkX.Y, etc...), or 'sectored' mode, (pmemXs, ndblkX.Ys,
+	  etc...).
+
+	  Select Y if unsure
+
+endif
diff --git a/drivers/nvdimm/Makefile b/drivers/nvdimm/Makefile
new file mode 100644
index 000000000000..594bb97c867a
--- /dev/null
+++ b/drivers/nvdimm/Makefile
@@ -0,0 +1,20 @@
+obj-$(CONFIG_LIBNVDIMM) += libnvdimm.o
+obj-$(CONFIG_BLK_DEV_PMEM) += nd_pmem.o
+obj-$(CONFIG_ND_BTT) += nd_btt.o
+obj-$(CONFIG_ND_BLK) += nd_blk.o
+
+nd_pmem-y := pmem.o
+
+nd_btt-y := btt.o
+
+nd_blk-y := blk.o
+
+libnvdimm-y := core.o
+libnvdimm-y += bus.o
+libnvdimm-y += dimm_devs.o
+libnvdimm-y += dimm.o
+libnvdimm-y += region_devs.o
+libnvdimm-y += region.o
+libnvdimm-y += namespace_devs.o
+libnvdimm-y += label.o
+libnvdimm-$(CONFIG_BTT) += btt_devs.o
diff --git a/drivers/nvdimm/blk.c b/drivers/nvdimm/blk.c
new file mode 100644
index 000000000000..4f97b248c236
--- /dev/null
+++ b/drivers/nvdimm/blk.c
@@ -0,0 +1,384 @@
+/*
+ * NVDIMM Block Window Driver
+ * 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/blkdev.h>
+#include <linux/fs.h>
+#include <linux/genhd.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/nd.h>
+#include <linux/sizes.h>
+#include "nd.h"
+
+struct nd_blk_device {
+	struct request_queue *queue;
+	struct gendisk *disk;
+	struct nd_namespace_blk *nsblk;
+	struct nd_blk_region *ndbr;
+	size_t disk_size;
+	u32 sector_size;
+	u32 internal_lbasize;
+};
+
+static int nd_blk_major;
+
+static u32 nd_blk_meta_size(struct nd_blk_device *blk_dev)
+{
+	return blk_dev->nsblk->lbasize - blk_dev->sector_size;
+}
+
+static resource_size_t to_dev_offset(struct nd_namespace_blk *nsblk,
+				resource_size_t ns_offset, unsigned int len)
+{
+	int i;
+
+	for (i = 0; i < nsblk->num_resources; i++) {
+		if (ns_offset < resource_size(nsblk->res[i])) {
+			if (ns_offset + len > resource_size(nsblk->res[i])) {
+				dev_WARN_ONCE(&nsblk->common.dev, 1,
+					"illegal request\n");
+				return SIZE_MAX;
+			}
+			return nsblk->res[i]->start + ns_offset;
+		}
+		ns_offset -= resource_size(nsblk->res[i]);
+	}
+
+	dev_WARN_ONCE(&nsblk->common.dev, 1, "request out of range\n");
+	return SIZE_MAX;
+}
+
+#ifdef CONFIG_BLK_DEV_INTEGRITY
+static int nd_blk_rw_integrity(struct nd_blk_device *blk_dev,
+				struct bio_integrity_payload *bip, u64 lba,
+				int rw)
+{
+	unsigned int len = nd_blk_meta_size(blk_dev);
+	resource_size_t	dev_offset, ns_offset;
+	struct nd_namespace_blk *nsblk;
+	struct nd_blk_region *ndbr;
+	int err = 0;
+
+	nsblk = blk_dev->nsblk;
+	ndbr = blk_dev->ndbr;
+	ns_offset = lba * blk_dev->internal_lbasize + blk_dev->sector_size;
+	dev_offset = to_dev_offset(nsblk, ns_offset, len);
+	if (dev_offset == SIZE_MAX)
+		return -EIO;
+
+	while (len) {
+		unsigned int cur_len;
+		struct bio_vec bv;
+		void *iobuf;
+
+		bv = bvec_iter_bvec(bip->bip_vec, bip->bip_iter);
+		/*
+		 * The 'bv' obtained from bvec_iter_bvec has its .bv_len and
+		 * .bv_offset already adjusted for iter->bi_bvec_done, and we
+		 * can use those directly
+		 */
+
+		cur_len = min(len, bv.bv_len);
+		iobuf = kmap_atomic(bv.bv_page);
+		err = ndbr->do_io(ndbr, dev_offset, iobuf + bv.bv_offset,
+				cur_len, rw);
+		kunmap_atomic(iobuf);
+		if (err)
+			return err;
+
+		len -= cur_len;
+		dev_offset += cur_len;
+		bvec_iter_advance(bip->bip_vec, &bip->bip_iter, cur_len);
+	}
+
+	return err;
+}
+
+#else /* CONFIG_BLK_DEV_INTEGRITY */
+static int nd_blk_rw_integrity(struct nd_blk_device *blk_dev,
+				struct bio_integrity_payload *bip, u64 lba,
+				int rw)
+{
+	return 0;
+}
+#endif
+
+static int nd_blk_do_bvec(struct nd_blk_device *blk_dev,
+			struct bio_integrity_payload *bip, struct page *page,
+			unsigned int len, unsigned int off, int rw,
+			sector_t sector)
+{
+	struct nd_blk_region *ndbr = blk_dev->ndbr;
+	resource_size_t	dev_offset, ns_offset;
+	int err = 0;
+	void *iobuf;
+	u64 lba;
+
+	while (len) {
+		unsigned int cur_len;
+
+		/*
+		 * If we don't have an integrity payload, we don't have to
+		 * split the bvec into sectors, as this would cause unnecessary
+		 * Block Window setup/move steps. the do_io routine is capable
+		 * of handling len <= PAGE_SIZE.
+		 */
+		cur_len = bip ? min(len, blk_dev->sector_size) : len;
+
+		lba = div_u64(sector << SECTOR_SHIFT, blk_dev->sector_size);
+		ns_offset = lba * blk_dev->internal_lbasize;
+		dev_offset = to_dev_offset(blk_dev->nsblk, ns_offset, cur_len);
+		if (dev_offset == SIZE_MAX)
+			return -EIO;
+
+		iobuf = kmap_atomic(page);
+		err = ndbr->do_io(ndbr, dev_offset, iobuf + off, cur_len, rw);
+		kunmap_atomic(iobuf);
+		if (err)
+			return err;
+
+		if (bip) {
+			err = nd_blk_rw_integrity(blk_dev, bip, lba, rw);
+			if (err)
+				return err;
+		}
+		len -= cur_len;
+		off += cur_len;
+		sector += blk_dev->sector_size >> SECTOR_SHIFT;
+	}
+
+	return err;
+}
+
+static void nd_blk_make_request(struct request_queue *q, struct bio *bio)
+{
+	struct block_device *bdev = bio->bi_bdev;
+	struct gendisk *disk = bdev->bd_disk;
+	struct bio_integrity_payload *bip;
+	struct nd_blk_device *blk_dev;
+	struct bvec_iter iter;
+	unsigned long start;
+	struct bio_vec bvec;
+	int err = 0, rw;
+	bool do_acct;
+
+	/*
+	 * bio_integrity_enabled also checks if the bio already has an
+	 * integrity payload attached. If it does, we *don't* do a
+	 * bio_integrity_prep here - the payload has been generated by
+	 * another kernel subsystem, and we just pass it through.
+	 */
+	if (bio_integrity_enabled(bio) && bio_integrity_prep(bio)) {
+		err = -EIO;
+		goto out;
+	}
+
+	bip = bio_integrity(bio);
+	blk_dev = disk->private_data;
+	rw = bio_data_dir(bio);
+	do_acct = nd_iostat_start(bio, &start);
+	bio_for_each_segment(bvec, bio, iter) {
+		unsigned int len = bvec.bv_len;
+
+		BUG_ON(len > PAGE_SIZE);
+		err = nd_blk_do_bvec(blk_dev, bip, bvec.bv_page, len,
+					bvec.bv_offset, rw, iter.bi_sector);
+		if (err) {
+			dev_info(&blk_dev->nsblk->common.dev,
+					"io error in %s sector %lld, len %d,\n",
+					(rw == READ) ? "READ" : "WRITE",
+					(unsigned long long) iter.bi_sector, len);
+			break;
+		}
+	}
+	if (do_acct)
+		nd_iostat_end(bio, start);
+
+ out:
+	bio_endio(bio, err);
+}
+
+static int nd_blk_rw_bytes(struct nd_namespace_common *ndns,
+		resource_size_t offset, void *iobuf, size_t n, int rw)
+{
+	struct nd_blk_device *blk_dev = dev_get_drvdata(ndns->claim);
+	struct nd_namespace_blk *nsblk = blk_dev->nsblk;
+	struct nd_blk_region *ndbr = blk_dev->ndbr;
+	resource_size_t	dev_offset;
+
+	dev_offset = to_dev_offset(nsblk, offset, n);
+
+	if (unlikely(offset + n > blk_dev->disk_size)) {
+		dev_WARN_ONCE(&ndns->dev, 1, "request out of range\n");
+		return -EFAULT;
+	}
+
+	if (dev_offset == SIZE_MAX)
+		return -EIO;
+
+	return ndbr->do_io(ndbr, dev_offset, iobuf, n, rw);
+}
+
+static const struct block_device_operations nd_blk_fops = {
+	.owner = THIS_MODULE,
+	.revalidate_disk = nvdimm_revalidate_disk,
+};
+
+static int nd_blk_attach_disk(struct nd_namespace_common *ndns,
+		struct nd_blk_device *blk_dev)
+{
+	resource_size_t available_disk_size;
+	struct gendisk *disk;
+	u64 internal_nlba;
+
+	internal_nlba = div_u64(blk_dev->disk_size, blk_dev->internal_lbasize);
+	available_disk_size = internal_nlba * blk_dev->sector_size;
+
+	blk_dev->queue = blk_alloc_queue(GFP_KERNEL);
+	if (!blk_dev->queue)
+		return -ENOMEM;
+
+	blk_queue_make_request(blk_dev->queue, nd_blk_make_request);
+	blk_queue_max_hw_sectors(blk_dev->queue, UINT_MAX);
+	blk_queue_bounce_limit(blk_dev->queue, BLK_BOUNCE_ANY);
+	blk_queue_logical_block_size(blk_dev->queue, blk_dev->sector_size);
+	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, blk_dev->queue);
+
+	disk = blk_dev->disk = alloc_disk(0);
+	if (!disk) {
+		blk_cleanup_queue(blk_dev->queue);
+		return -ENOMEM;
+	}
+
+	disk->driverfs_dev	= &ndns->dev;
+	disk->major		= nd_blk_major;
+	disk->first_minor	= 0;
+	disk->fops		= &nd_blk_fops;
+	disk->private_data	= blk_dev;
+	disk->queue		= blk_dev->queue;
+	disk->flags		= GENHD_FL_EXT_DEVT;
+	nvdimm_namespace_disk_name(ndns, disk->disk_name);
+	set_capacity(disk, 0);
+	add_disk(disk);
+
+	if (nd_blk_meta_size(blk_dev)) {
+		int rc = nd_integrity_init(disk, nd_blk_meta_size(blk_dev));
+
+		if (rc) {
+			del_gendisk(disk);
+			put_disk(disk);
+			blk_cleanup_queue(blk_dev->queue);
+			return rc;
+		}
+	}
+
+	set_capacity(disk, available_disk_size >> SECTOR_SHIFT);
+	revalidate_disk(disk);
+	return 0;
+}
+
+static int nd_blk_probe(struct device *dev)
+{
+	struct nd_namespace_common *ndns;
+	struct nd_namespace_blk *nsblk;
+	struct nd_blk_device *blk_dev;
+	int rc;
+
+	ndns = nvdimm_namespace_common_probe(dev);
+	if (IS_ERR(ndns))
+		return PTR_ERR(ndns);
+
+	blk_dev = kzalloc(sizeof(*blk_dev), GFP_KERNEL);
+	if (!blk_dev)
+		return -ENOMEM;
+
+	nsblk = to_nd_namespace_blk(&ndns->dev);
+	blk_dev->disk_size = nvdimm_namespace_capacity(ndns);
+	blk_dev->ndbr = to_nd_blk_region(dev->parent);
+	blk_dev->nsblk = to_nd_namespace_blk(&ndns->dev);
+	blk_dev->internal_lbasize = roundup(nsblk->lbasize,
+						INT_LBASIZE_ALIGNMENT);
+	blk_dev->sector_size = ((nsblk->lbasize >= 4096) ? 4096 : 512);
+	dev_set_drvdata(dev, blk_dev);
+
+	ndns->rw_bytes = nd_blk_rw_bytes;
+	if (is_nd_btt(dev))
+		rc = nvdimm_namespace_attach_btt(ndns);
+	else if (nd_btt_probe(ndns, blk_dev) == 0) {
+		/* we'll come back as btt-blk */
+		rc = -ENXIO;
+	} else
+		rc = nd_blk_attach_disk(ndns, blk_dev);
+	if (rc)
+		kfree(blk_dev);
+	return rc;
+}
+
+static void nd_blk_detach_disk(struct nd_blk_device *blk_dev)
+{
+	del_gendisk(blk_dev->disk);
+	put_disk(blk_dev->disk);
+	blk_cleanup_queue(blk_dev->queue);
+}
+
+static int nd_blk_remove(struct device *dev)
+{
+	struct nd_blk_device *blk_dev = dev_get_drvdata(dev);
+
+	if (is_nd_btt(dev))
+		nvdimm_namespace_detach_btt(to_nd_btt(dev)->ndns);
+	else
+		nd_blk_detach_disk(blk_dev);
+	kfree(blk_dev);
+
+	return 0;
+}
+
+static struct nd_device_driver nd_blk_driver = {
+	.probe = nd_blk_probe,
+	.remove = nd_blk_remove,
+	.drv = {
+		.name = "nd_blk",
+	},
+	.type = ND_DRIVER_NAMESPACE_BLK,
+};
+
+static int __init nd_blk_init(void)
+{
+	int rc;
+
+	rc = register_blkdev(0, "nd_blk");
+	if (rc < 0)
+		return rc;
+
+	nd_blk_major = rc;
+	rc = nd_driver_register(&nd_blk_driver);
+
+	if (rc < 0)
+		unregister_blkdev(nd_blk_major, "nd_blk");
+
+	return rc;
+}
+
+static void __exit nd_blk_exit(void)
+{
+	driver_unregister(&nd_blk_driver.drv);
+	unregister_blkdev(nd_blk_major, "nd_blk");
+}
+
+MODULE_AUTHOR("Ross Zwisler <ross.zwisler@linux.intel.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS_ND_DEVICE(ND_DEVICE_NAMESPACE_BLK);
+module_init(nd_blk_init);
+module_exit(nd_blk_exit);
diff --git a/drivers/nvdimm/btt.c b/drivers/nvdimm/btt.c
new file mode 100644
index 000000000000..411c7b2bb37a
--- /dev/null
+++ b/drivers/nvdimm/btt.c
@@ -0,0 +1,1479 @@
+/*
+ * Block Translation Table
+ * Copyright (c) 2014-2015, 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/highmem.h>
+#include <linux/debugfs.h>
+#include <linux/blkdev.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/hdreg.h>
+#include <linux/genhd.h>
+#include <linux/sizes.h>
+#include <linux/ndctl.h>
+#include <linux/fs.h>
+#include <linux/nd.h>
+#include "btt.h"
+#include "nd.h"
+
+enum log_ent_request {
+	LOG_NEW_ENT = 0,
+	LOG_OLD_ENT
+};
+
+static int btt_major;
+
+static int arena_read_bytes(struct arena_info *arena, resource_size_t offset,
+		void *buf, size_t n)
+{
+	struct nd_btt *nd_btt = arena->nd_btt;
+	struct nd_namespace_common *ndns = nd_btt->ndns;
+
+	/* arena offsets are 4K from the base of the device */
+	offset += SZ_4K;
+	return nvdimm_read_bytes(ndns, offset, buf, n);
+}
+
+static int arena_write_bytes(struct arena_info *arena, resource_size_t offset,
+		void *buf, size_t n)
+{
+	struct nd_btt *nd_btt = arena->nd_btt;
+	struct nd_namespace_common *ndns = nd_btt->ndns;
+
+	/* arena offsets are 4K from the base of the device */
+	offset += SZ_4K;
+	return nvdimm_write_bytes(ndns, offset, buf, n);
+}
+
+static int btt_info_write(struct arena_info *arena, struct btt_sb *super)
+{
+	int ret;
+
+	ret = arena_write_bytes(arena, arena->info2off, super,
+			sizeof(struct btt_sb));
+	if (ret)
+		return ret;
+
+	return arena_write_bytes(arena, arena->infooff, super,
+			sizeof(struct btt_sb));
+}
+
+static int btt_info_read(struct arena_info *arena, struct btt_sb *super)
+{
+	WARN_ON(!super);
+	return arena_read_bytes(arena, arena->infooff, super,
+			sizeof(struct btt_sb));
+}
+
+/*
+ * 'raw' version of btt_map write
+ * Assumptions:
+ *   mapping is in little-endian
+ *   mapping contains 'E' and 'Z' flags as desired
+ */
+static int __btt_map_write(struct arena_info *arena, u32 lba, __le32 mapping)
+{
+	u64 ns_off = arena->mapoff + (lba * MAP_ENT_SIZE);
+
+	WARN_ON(lba >= arena->external_nlba);
+	return arena_write_bytes(arena, ns_off, &mapping, MAP_ENT_SIZE);
+}
+
+static int btt_map_write(struct arena_info *arena, u32 lba, u32 mapping,
+			u32 z_flag, u32 e_flag)
+{
+	u32 ze;
+	__le32 mapping_le;
+
+	/*
+	 * This 'mapping' is supposed to be just the LBA mapping, without
+	 * any flags set, so strip the flag bits.
+	 */
+	mapping &= MAP_LBA_MASK;
+
+	ze = (z_flag << 1) + e_flag;
+	switch (ze) {
+	case 0:
+		/*
+		 * We want to set neither of the Z or E flags, and
+		 * in the actual layout, this means setting the bit
+		 * positions of both to '1' to indicate a 'normal'
+		 * map entry
+		 */
+		mapping |= MAP_ENT_NORMAL;
+		break;
+	case 1:
+		mapping |= (1 << MAP_ERR_SHIFT);
+		break;
+	case 2:
+		mapping |= (1 << MAP_TRIM_SHIFT);
+		break;
+	default:
+		/*
+		 * The case where Z and E are both sent in as '1' could be
+		 * construed as a valid 'normal' case, but we decide not to,
+		 * to avoid confusion
+		 */
+		WARN_ONCE(1, "Invalid use of Z and E flags\n");
+		return -EIO;
+	}
+
+	mapping_le = cpu_to_le32(mapping);
+	return __btt_map_write(arena, lba, mapping_le);
+}
+
+static int btt_map_read(struct arena_info *arena, u32 lba, u32 *mapping,
+			int *trim, int *error)
+{
+	int ret;
+	__le32 in;
+	u32 raw_mapping, postmap, ze, z_flag, e_flag;
+	u64 ns_off = arena->mapoff + (lba * MAP_ENT_SIZE);
+
+	WARN_ON(lba >= arena->external_nlba);
+
+	ret = arena_read_bytes(arena, ns_off, &in, MAP_ENT_SIZE);
+	if (ret)
+		return ret;
+
+	raw_mapping = le32_to_cpu(in);
+
+	z_flag = (raw_mapping & MAP_TRIM_MASK) >> MAP_TRIM_SHIFT;
+	e_flag = (raw_mapping & MAP_ERR_MASK) >> MAP_ERR_SHIFT;
+	ze = (z_flag << 1) + e_flag;
+	postmap = raw_mapping & MAP_LBA_MASK;
+
+	/* Reuse the {z,e}_flag variables for *trim and *error */
+	z_flag = 0;
+	e_flag = 0;
+
+	switch (ze) {
+	case 0:
+		/* Initial state. Return postmap = premap */
+		*mapping = lba;
+		break;
+	case 1:
+		*mapping = postmap;
+		e_flag = 1;
+		break;
+	case 2:
+		*mapping = postmap;
+		z_flag = 1;
+		break;
+	case 3:
+		*mapping = postmap;
+		break;
+	default:
+		return -EIO;
+	}
+
+	if (trim)
+		*trim = z_flag;
+	if (error)
+		*error = e_flag;
+
+	return ret;
+}
+
+static int btt_log_read_pair(struct arena_info *arena, u32 lane,
+			struct log_entry *ent)
+{
+	WARN_ON(!ent);
+	return arena_read_bytes(arena,
+			arena->logoff + (2 * lane * LOG_ENT_SIZE), ent,
+			2 * LOG_ENT_SIZE);
+}
+
+static struct dentry *debugfs_root;
+
+static void arena_debugfs_init(struct arena_info *a, struct dentry *parent,
+				int idx)
+{
+	char dirname[32];
+	struct dentry *d;
+
+	/* If for some reason, parent bttN was not created, exit */
+	if (!parent)
+		return;
+
+	snprintf(dirname, 32, "arena%d", idx);
+	d = debugfs_create_dir(dirname, parent);
+	if (IS_ERR_OR_NULL(d))
+		return;
+	a->debugfs_dir = d;
+
+	debugfs_create_x64("size", S_IRUGO, d, &a->size);
+	debugfs_create_x64("external_lba_start", S_IRUGO, d,
+				&a->external_lba_start);
+	debugfs_create_x32("internal_nlba", S_IRUGO, d, &a->internal_nlba);
+	debugfs_create_u32("internal_lbasize", S_IRUGO, d,
+				&a->internal_lbasize);
+	debugfs_create_x32("external_nlba", S_IRUGO, d, &a->external_nlba);
+	debugfs_create_u32("external_lbasize", S_IRUGO, d,
+				&a->external_lbasize);
+	debugfs_create_u32("nfree", S_IRUGO, d, &a->nfree);
+	debugfs_create_u16("version_major", S_IRUGO, d, &a->version_major);
+	debugfs_create_u16("version_minor", S_IRUGO, d, &a->version_minor);
+	debugfs_create_x64("nextoff", S_IRUGO, d, &a->nextoff);
+	debugfs_create_x64("infooff", S_IRUGO, d, &a->infooff);
+	debugfs_create_x64("dataoff", S_IRUGO, d, &a->dataoff);
+	debugfs_create_x64("mapoff", S_IRUGO, d, &a->mapoff);
+	debugfs_create_x64("logoff", S_IRUGO, d, &a->logoff);
+	debugfs_create_x64("info2off", S_IRUGO, d, &a->info2off);
+	debugfs_create_x32("flags", S_IRUGO, d, &a->flags);
+}
+
+static void btt_debugfs_init(struct btt *btt)
+{
+	int i = 0;
+	struct arena_info *arena;
+
+	btt->debugfs_dir = debugfs_create_dir(dev_name(&btt->nd_btt->dev),
+						debugfs_root);
+	if (IS_ERR_OR_NULL(btt->debugfs_dir))
+		return;
+
+	list_for_each_entry(arena, &btt->arena_list, list) {
+		arena_debugfs_init(arena, btt->debugfs_dir, i);
+		i++;
+	}
+}
+
+/*
+ * This function accepts two log entries, and uses the
+ * sequence number to find the 'older' entry.
+ * It also updates the sequence number in this old entry to
+ * make it the 'new' one if the mark_flag is set.
+ * Finally, it returns which of the entries was the older one.
+ *
+ * TODO The logic feels a bit kludge-y. make it better..
+ */
+static int btt_log_get_old(struct log_entry *ent)
+{
+	int old;
+
+	/*
+	 * the first ever time this is seen, the entry goes into [0]
+	 * the next time, the following logic works out to put this
+	 * (next) entry into [1]
+	 */
+	if (ent[0].seq == 0) {
+		ent[0].seq = cpu_to_le32(1);
+		return 0;
+	}
+
+	if (ent[0].seq == ent[1].seq)
+		return -EINVAL;
+	if (le32_to_cpu(ent[0].seq) + le32_to_cpu(ent[1].seq) > 5)
+		return -EINVAL;
+
+	if (le32_to_cpu(ent[0].seq) < le32_to_cpu(ent[1].seq)) {
+		if (le32_to_cpu(ent[1].seq) - le32_to_cpu(ent[0].seq) == 1)
+			old = 0;
+		else
+			old = 1;
+	} else {
+		if (le32_to_cpu(ent[0].seq) - le32_to_cpu(ent[1].seq) == 1)
+			old = 1;
+		else
+			old = 0;
+	}
+
+	return old;
+}
+
+static struct device *to_dev(struct arena_info *arena)
+{
+	return &arena->nd_btt->dev;
+}
+
+/*
+ * This function copies the desired (old/new) log entry into ent if
+ * it is not NULL. It returns the sub-slot number (0 or 1)
+ * where the desired log entry was found. Negative return values
+ * indicate errors.
+ */
+static int btt_log_read(struct arena_info *arena, u32 lane,
+			struct log_entry *ent, int old_flag)
+{
+	int ret;
+	int old_ent, ret_ent;
+	struct log_entry log[2];
+
+	ret = btt_log_read_pair(arena, lane, log);
+	if (ret)
+		return -EIO;
+
+	old_ent = btt_log_get_old(log);
+	if (old_ent < 0 || old_ent > 1) {
+		dev_info(to_dev(arena),
+				"log corruption (%d): lane %d seq [%d, %d]\n",
+			old_ent, lane, log[0].seq, log[1].seq);
+		/* TODO set error state? */
+		return -EIO;
+	}
+
+	ret_ent = (old_flag ? old_ent : (1 - old_ent));
+
+	if (ent != NULL)
+		memcpy(ent, &log[ret_ent], LOG_ENT_SIZE);
+
+	return ret_ent;
+}
+
+/*
+ * This function commits a log entry to media
+ * It does _not_ prepare the freelist entry for the next write
+ * btt_flog_write is the wrapper for updating the freelist elements
+ */
+static int __btt_log_write(struct arena_info *arena, u32 lane,
+			u32 sub, struct log_entry *ent)
+{
+	int ret;
+	/*
+	 * Ignore the padding in log_entry for calculating log_half.
+	 * The entry is 'committed' when we write the sequence number,
+	 * and we want to ensure that that is the last thing written.
+	 * We don't bother writing the padding as that would be extra
+	 * media wear and write amplification
+	 */
+	unsigned int log_half = (LOG_ENT_SIZE - 2 * sizeof(u64)) / 2;
+	u64 ns_off = arena->logoff + (((2 * lane) + sub) * LOG_ENT_SIZE);
+	void *src = ent;
+
+	/* split the 16B write into atomic, durable halves */
+	ret = arena_write_bytes(arena, ns_off, src, log_half);
+	if (ret)
+		return ret;
+
+	ns_off += log_half;
+	src += log_half;
+	return arena_write_bytes(arena, ns_off, src, log_half);
+}
+
+static int btt_flog_write(struct arena_info *arena, u32 lane, u32 sub,
+			struct log_entry *ent)
+{
+	int ret;
+
+	ret = __btt_log_write(arena, lane, sub, ent);
+	if (ret)
+		return ret;
+
+	/* prepare the next free entry */
+	arena->freelist[lane].sub = 1 - arena->freelist[lane].sub;
+	if (++(arena->freelist[lane].seq) == 4)
+		arena->freelist[lane].seq = 1;
+	arena->freelist[lane].block = le32_to_cpu(ent->old_map);
+
+	return ret;
+}
+
+/*
+ * This function initializes the BTT map to the initial state, which is
+ * all-zeroes, and indicates an identity mapping
+ */
+static int btt_map_init(struct arena_info *arena)
+{
+	int ret = -EINVAL;
+	void *zerobuf;
+	size_t offset = 0;
+	size_t chunk_size = SZ_2M;
+	size_t mapsize = arena->logoff - arena->mapoff;
+
+	zerobuf = kzalloc(chunk_size, GFP_KERNEL);
+	if (!zerobuf)
+		return -ENOMEM;
+
+	while (mapsize) {
+		size_t size = min(mapsize, chunk_size);
+
+		ret = arena_write_bytes(arena, arena->mapoff + offset, zerobuf,
+				size);
+		if (ret)
+			goto free;
+
+		offset += size;
+		mapsize -= size;
+		cond_resched();
+	}
+
+ free:
+	kfree(zerobuf);
+	return ret;
+}
+
+/*
+ * This function initializes the BTT log with 'fake' entries pointing
+ * to the initial reserved set of blocks as being free
+ */
+static int btt_log_init(struct arena_info *arena)
+{
+	int ret;
+	u32 i;
+	struct log_entry log, zerolog;
+
+	memset(&zerolog, 0, sizeof(zerolog));
+
+	for (i = 0; i < arena->nfree; i++) {
+		log.lba = cpu_to_le32(i);
+		log.old_map = cpu_to_le32(arena->external_nlba + i);
+		log.new_map = cpu_to_le32(arena->external_nlba + i);
+		log.seq = cpu_to_le32(LOG_SEQ_INIT);
+		ret = __btt_log_write(arena, i, 0, &log);
+		if (ret)
+			return ret;
+		ret = __btt_log_write(arena, i, 1, &zerolog);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int btt_freelist_init(struct arena_info *arena)
+{
+	int old, new, ret;
+	u32 i, map_entry;
+	struct log_entry log_new, log_old;
+
+	arena->freelist = kcalloc(arena->nfree, sizeof(struct free_entry),
+					GFP_KERNEL);
+	if (!arena->freelist)
+		return -ENOMEM;
+
+	for (i = 0; i < arena->nfree; i++) {
+		old = btt_log_read(arena, i, &log_old, LOG_OLD_ENT);
+		if (old < 0)
+			return old;
+
+		new = btt_log_read(arena, i, &log_new, LOG_NEW_ENT);
+		if (new < 0)
+			return new;
+
+		/* sub points to the next one to be overwritten */
+		arena->freelist[i].sub = 1 - new;
+		arena->freelist[i].seq = nd_inc_seq(le32_to_cpu(log_new.seq));
+		arena->freelist[i].block = le32_to_cpu(log_new.old_map);
+
+		/* This implies a newly created or untouched flog entry */
+		if (log_new.old_map == log_new.new_map)
+			continue;
+
+		/* Check if map recovery is needed */
+		ret = btt_map_read(arena, le32_to_cpu(log_new.lba), &map_entry,
+				NULL, NULL);
+		if (ret)
+			return ret;
+		if ((le32_to_cpu(log_new.new_map) != map_entry) &&
+				(le32_to_cpu(log_new.old_map) == map_entry)) {
+			/*
+			 * Last transaction wrote the flog, but wasn't able
+			 * to complete the map write. So fix up the map.
+			 */
+			ret = btt_map_write(arena, le32_to_cpu(log_new.lba),
+					le32_to_cpu(log_new.new_map), 0, 0);
+			if (ret)
+				return ret;
+		}
+
+	}
+
+	return 0;
+}
+
+static int btt_rtt_init(struct arena_info *arena)
+{
+	arena->rtt = kcalloc(arena->nfree, sizeof(u32), GFP_KERNEL);
+	if (arena->rtt == NULL)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int btt_maplocks_init(struct arena_info *arena)
+{
+	u32 i;
+
+	arena->map_locks = kcalloc(arena->nfree, sizeof(struct aligned_lock),
+				GFP_KERNEL);
+	if (!arena->map_locks)
+		return -ENOMEM;
+
+	for (i = 0; i < arena->nfree; i++)
+		spin_lock_init(&arena->map_locks[i].lock);
+
+	return 0;
+}
+
+static struct arena_info *alloc_arena(struct btt *btt, size_t size,
+				size_t start, size_t arena_off)
+{
+	struct arena_info *arena;
+	u64 logsize, mapsize, datasize;
+	u64 available = size;
+
+	arena = kzalloc(sizeof(struct arena_info), GFP_KERNEL);
+	if (!arena)
+		return NULL;
+	arena->nd_btt = btt->nd_btt;
+
+	if (!size)
+		return arena;
+
+	arena->size = size;
+	arena->external_lba_start = start;
+	arena->external_lbasize = btt->lbasize;
+	arena->internal_lbasize = roundup(arena->external_lbasize,
+					INT_LBASIZE_ALIGNMENT);
+	arena->nfree = BTT_DEFAULT_NFREE;
+	arena->version_major = 1;
+	arena->version_minor = 1;
+
+	if (available % BTT_PG_SIZE)
+		available -= (available % BTT_PG_SIZE);
+
+	/* Two pages are reserved for the super block and its copy */
+	available -= 2 * BTT_PG_SIZE;
+
+	/* The log takes a fixed amount of space based on nfree */
+	logsize = roundup(2 * arena->nfree * sizeof(struct log_entry),
+				BTT_PG_SIZE);
+	available -= logsize;
+
+	/* Calculate optimal split between map and data area */
+	arena->internal_nlba = div_u64(available - BTT_PG_SIZE,
+			arena->internal_lbasize + MAP_ENT_SIZE);
+	arena->external_nlba = arena->internal_nlba - arena->nfree;
+
+	mapsize = roundup((arena->external_nlba * MAP_ENT_SIZE), BTT_PG_SIZE);
+	datasize = available - mapsize;
+
+	/* 'Absolute' values, relative to start of storage space */
+	arena->infooff = arena_off;
+	arena->dataoff = arena->infooff + BTT_PG_SIZE;
+	arena->mapoff = arena->dataoff + datasize;
+	arena->logoff = arena->mapoff + mapsize;
+	arena->info2off = arena->logoff + logsize;
+	return arena;
+}
+
+static void free_arenas(struct btt *btt)
+{
+	struct arena_info *arena, *next;
+
+	list_for_each_entry_safe(arena, next, &btt->arena_list, list) {
+		list_del(&arena->list);
+		kfree(arena->rtt);
+		kfree(arena->map_locks);
+		kfree(arena->freelist);
+		debugfs_remove_recursive(arena->debugfs_dir);
+		kfree(arena);
+	}
+}
+
+/*
+ * This function checks if the metadata layout is valid and error free
+ */
+static int arena_is_valid(struct arena_info *arena, struct btt_sb *super,
+				u8 *uuid, u32 lbasize)
+{
+	u64 checksum;
+
+	if (memcmp(super->uuid, uuid, 16))
+		return 0;
+
+	checksum = le64_to_cpu(super->checksum);
+	super->checksum = 0;
+	if (checksum != nd_btt_sb_checksum(super))
+		return 0;
+	super->checksum = cpu_to_le64(checksum);
+
+	if (lbasize != le32_to_cpu(super->external_lbasize))
+		return 0;
+
+	/* TODO: figure out action for this */
+	if ((le32_to_cpu(super->flags) & IB_FLAG_ERROR_MASK) != 0)
+		dev_info(to_dev(arena), "Found arena with an error flag\n");
+
+	return 1;
+}
+
+/*
+ * This function reads an existing valid btt superblock and
+ * populates the corresponding arena_info struct
+ */
+static void parse_arena_meta(struct arena_info *arena, struct btt_sb *super,
+				u64 arena_off)
+{
+	arena->internal_nlba = le32_to_cpu(super->internal_nlba);
+	arena->internal_lbasize = le32_to_cpu(super->internal_lbasize);
+	arena->external_nlba = le32_to_cpu(super->external_nlba);
+	arena->external_lbasize = le32_to_cpu(super->external_lbasize);
+	arena->nfree = le32_to_cpu(super->nfree);
+	arena->version_major = le16_to_cpu(super->version_major);
+	arena->version_minor = le16_to_cpu(super->version_minor);
+
+	arena->nextoff = (super->nextoff == 0) ? 0 : (arena_off +
+			le64_to_cpu(super->nextoff));
+	arena->infooff = arena_off;
+	arena->dataoff = arena_off + le64_to_cpu(super->dataoff);
+	arena->mapoff = arena_off + le64_to_cpu(super->mapoff);
+	arena->logoff = arena_off + le64_to_cpu(super->logoff);
+	arena->info2off = arena_off + le64_to_cpu(super->info2off);
+
+	arena->size = (super->nextoff > 0) ? (le64_to_cpu(super->nextoff)) :
+			(arena->info2off - arena->infooff + BTT_PG_SIZE);
+
+	arena->flags = le32_to_cpu(super->flags);
+}
+
+static int discover_arenas(struct btt *btt)
+{
+	int ret = 0;
+	struct arena_info *arena;
+	struct btt_sb *super;
+	size_t remaining = btt->rawsize;
+	u64 cur_nlba = 0;
+	size_t cur_off = 0;
+	int num_arenas = 0;
+
+	super = kzalloc(sizeof(*super), GFP_KERNEL);
+	if (!super)
+		return -ENOMEM;
+
+	while (remaining) {
+		/* Alloc memory for arena */
+		arena = alloc_arena(btt, 0, 0, 0);
+		if (!arena) {
+			ret = -ENOMEM;
+			goto out_super;
+		}
+
+		arena->infooff = cur_off;
+		ret = btt_info_read(arena, super);
+		if (ret)
+			goto out;
+
+		if (!arena_is_valid(arena, super, btt->nd_btt->uuid,
+				btt->lbasize)) {
+			if (remaining == btt->rawsize) {
+				btt->init_state = INIT_NOTFOUND;
+				dev_info(to_dev(arena), "No existing arenas\n");
+				goto out;
+			} else {
+				dev_info(to_dev(arena),
+						"Found corrupted metadata!\n");
+				ret = -ENODEV;
+				goto out;
+			}
+		}
+
+		arena->external_lba_start = cur_nlba;
+		parse_arena_meta(arena, super, cur_off);
+
+		ret = btt_freelist_init(arena);
+		if (ret)
+			goto out;
+
+		ret = btt_rtt_init(arena);
+		if (ret)
+			goto out;
+
+		ret = btt_maplocks_init(arena);
+		if (ret)
+			goto out;
+
+		list_add_tail(&arena->list, &btt->arena_list);
+
+		remaining -= arena->size;
+		cur_off += arena->size;
+		cur_nlba += arena->external_nlba;
+		num_arenas++;
+
+		if (arena->nextoff == 0)
+			break;
+	}
+	btt->num_arenas = num_arenas;
+	btt->nlba = cur_nlba;
+	btt->init_state = INIT_READY;
+
+	kfree(super);
+	return ret;
+
+ out:
+	kfree(arena);
+	free_arenas(btt);
+ out_super:
+	kfree(super);
+	return ret;
+}
+
+static int create_arenas(struct btt *btt)
+{
+	size_t remaining = btt->rawsize;
+	size_t cur_off = 0;
+
+	while (remaining) {
+		struct arena_info *arena;
+		size_t arena_size = min_t(u64, ARENA_MAX_SIZE, remaining);
+
+		remaining -= arena_size;
+		if (arena_size < ARENA_MIN_SIZE)
+			break;
+
+		arena = alloc_arena(btt, arena_size, btt->nlba, cur_off);
+		if (!arena) {
+			free_arenas(btt);
+			return -ENOMEM;
+		}
+		btt->nlba += arena->external_nlba;
+		if (remaining >= ARENA_MIN_SIZE)
+			arena->nextoff = arena->size;
+		else
+			arena->nextoff = 0;
+		cur_off += arena_size;
+		list_add_tail(&arena->list, &btt->arena_list);
+	}
+
+	return 0;
+}
+
+/*
+ * This function completes arena initialization by writing
+ * all the metadata.
+ * It is only called for an uninitialized arena when a write
+ * to that arena occurs for the first time.
+ */
+static int btt_arena_write_layout(struct arena_info *arena, u8 *uuid)
+{
+	int ret;
+	struct btt_sb *super;
+
+	ret = btt_map_init(arena);
+	if (ret)
+		return ret;
+
+	ret = btt_log_init(arena);
+	if (ret)
+		return ret;
+
+	super = kzalloc(sizeof(struct btt_sb), GFP_NOIO);
+	if (!super)
+		return -ENOMEM;
+
+	strncpy(super->signature, BTT_SIG, BTT_SIG_LEN);
+	memcpy(super->uuid, uuid, 16);
+	super->flags = cpu_to_le32(arena->flags);
+	super->version_major = cpu_to_le16(arena->version_major);
+	super->version_minor = cpu_to_le16(arena->version_minor);
+	super->external_lbasize = cpu_to_le32(arena->external_lbasize);
+	super->external_nlba = cpu_to_le32(arena->external_nlba);
+	super->internal_lbasize = cpu_to_le32(arena->internal_lbasize);
+	super->internal_nlba = cpu_to_le32(arena->internal_nlba);
+	super->nfree = cpu_to_le32(arena->nfree);
+	super->infosize = cpu_to_le32(sizeof(struct btt_sb));
+	super->nextoff = cpu_to_le64(arena->nextoff);
+	/*
+	 * Subtract arena->infooff (arena start) so numbers are relative
+	 * to 'this' arena
+	 */
+	super->dataoff = cpu_to_le64(arena->dataoff - arena->infooff);
+	super->mapoff = cpu_to_le64(arena->mapoff - arena->infooff);
+	super->logoff = cpu_to_le64(arena->logoff - arena->infooff);
+	super->info2off = cpu_to_le64(arena->info2off - arena->infooff);
+
+	super->flags = 0;
+	super->checksum = cpu_to_le64(nd_btt_sb_checksum(super));
+
+	ret = btt_info_write(arena, super);
+
+	kfree(super);
+	return ret;
+}
+
+/*
+ * This function completes the initialization for the BTT namespace
+ * such that it is ready to accept IOs
+ */
+static int btt_meta_init(struct btt *btt)
+{
+	int ret = 0;
+	struct arena_info *arena;
+
+	mutex_lock(&btt->init_lock);
+	list_for_each_entry(arena, &btt->arena_list, list) {
+		ret = btt_arena_write_layout(arena, btt->nd_btt->uuid);
+		if (ret)
+			goto unlock;
+
+		ret = btt_freelist_init(arena);
+		if (ret)
+			goto unlock;
+
+		ret = btt_rtt_init(arena);
+		if (ret)
+			goto unlock;
+
+		ret = btt_maplocks_init(arena);
+		if (ret)
+			goto unlock;
+	}
+
+	btt->init_state = INIT_READY;
+
+ unlock:
+	mutex_unlock(&btt->init_lock);
+	return ret;
+}
+
+static u32 btt_meta_size(struct btt *btt)
+{
+	return btt->lbasize - btt->sector_size;
+}
+
+/*
+ * This function calculates the arena in which the given LBA lies
+ * by doing a linear walk. This is acceptable since we expect only
+ * a few arenas. If we have backing devices that get much larger,
+ * we can construct a balanced binary tree of arenas at init time
+ * so that this range search becomes faster.
+ */
+static int lba_to_arena(struct btt *btt, sector_t sector, __u32 *premap,
+				struct arena_info **arena)
+{
+	struct arena_info *arena_list;
+	__u64 lba = div_u64(sector << SECTOR_SHIFT, btt->sector_size);
+
+	list_for_each_entry(arena_list, &btt->arena_list, list) {
+		if (lba < arena_list->external_nlba) {
+			*arena = arena_list;
+			*premap = lba;
+			return 0;
+		}
+		lba -= arena_list->external_nlba;
+	}
+
+	return -EIO;
+}
+
+/*
+ * The following (lock_map, unlock_map) are mostly just to improve
+ * readability, since they index into an array of locks
+ */
+static void lock_map(struct arena_info *arena, u32 premap)
+		__acquires(&arena->map_locks[idx].lock)
+{
+	u32 idx = (premap * MAP_ENT_SIZE / L1_CACHE_BYTES) % arena->nfree;
+
+	spin_lock(&arena->map_locks[idx].lock);
+}
+
+static void unlock_map(struct arena_info *arena, u32 premap)
+		__releases(&arena->map_locks[idx].lock)
+{
+	u32 idx = (premap * MAP_ENT_SIZE / L1_CACHE_BYTES) % arena->nfree;
+
+	spin_unlock(&arena->map_locks[idx].lock);
+}
+
+static u64 to_namespace_offset(struct arena_info *arena, u64 lba)
+{
+	return arena->dataoff + ((u64)lba * arena->internal_lbasize);
+}
+
+static int btt_data_read(struct arena_info *arena, struct page *page,
+			unsigned int off, u32 lba, u32 len)
+{
+	int ret;
+	u64 nsoff = to_namespace_offset(arena, lba);
+	void *mem = kmap_atomic(page);
+
+	ret = arena_read_bytes(arena, nsoff, mem + off, len);
+	kunmap_atomic(mem);
+
+	return ret;
+}
+
+static int btt_data_write(struct arena_info *arena, u32 lba,
+			struct page *page, unsigned int off, u32 len)
+{
+	int ret;
+	u64 nsoff = to_namespace_offset(arena, lba);
+	void *mem = kmap_atomic(page);
+
+	ret = arena_write_bytes(arena, nsoff, mem + off, len);
+	kunmap_atomic(mem);
+
+	return ret;
+}
+
+static void zero_fill_data(struct page *page, unsigned int off, u32 len)
+{
+	void *mem = kmap_atomic(page);
+
+	memset(mem + off, 0, len);
+	kunmap_atomic(mem);
+}
+
+#ifdef CONFIG_BLK_DEV_INTEGRITY
+static int btt_rw_integrity(struct btt *btt, struct bio_integrity_payload *bip,
+			struct arena_info *arena, u32 postmap, int rw)
+{
+	unsigned int len = btt_meta_size(btt);
+	u64 meta_nsoff;
+	int ret = 0;
+
+	if (bip == NULL)
+		return 0;
+
+	meta_nsoff = to_namespace_offset(arena, postmap) + btt->sector_size;
+
+	while (len) {
+		unsigned int cur_len;
+		struct bio_vec bv;
+		void *mem;
+
+		bv = bvec_iter_bvec(bip->bip_vec, bip->bip_iter);
+		/*
+		 * The 'bv' obtained from bvec_iter_bvec has its .bv_len and
+		 * .bv_offset already adjusted for iter->bi_bvec_done, and we
+		 * can use those directly
+		 */
+
+		cur_len = min(len, bv.bv_len);
+		mem = kmap_atomic(bv.bv_page);
+		if (rw)
+			ret = arena_write_bytes(arena, meta_nsoff,
+					mem + bv.bv_offset, cur_len);
+		else
+			ret = arena_read_bytes(arena, meta_nsoff,
+					mem + bv.bv_offset, cur_len);
+
+		kunmap_atomic(mem);
+		if (ret)
+			return ret;
+
+		len -= cur_len;
+		meta_nsoff += cur_len;
+		bvec_iter_advance(bip->bip_vec, &bip->bip_iter, cur_len);
+	}
+
+	return ret;
+}
+
+#else /* CONFIG_BLK_DEV_INTEGRITY */
+static int btt_rw_integrity(struct btt *btt, struct bio_integrity_payload *bip,
+			struct arena_info *arena, u32 postmap, int rw)
+{
+	return 0;
+}
+#endif
+
+static int btt_read_pg(struct btt *btt, struct bio_integrity_payload *bip,
+			struct page *page, unsigned int off, sector_t sector,
+			unsigned int len)
+{
+	int ret = 0;
+	int t_flag, e_flag;
+	struct arena_info *arena = NULL;
+	u32 lane = 0, premap, postmap;
+
+	while (len) {
+		u32 cur_len;
+
+		lane = nd_region_acquire_lane(btt->nd_region);
+
+		ret = lba_to_arena(btt, sector, &premap, &arena);
+		if (ret)
+			goto out_lane;
+
+		cur_len = min(btt->sector_size, len);
+
+		ret = btt_map_read(arena, premap, &postmap, &t_flag, &e_flag);
+		if (ret)
+			goto out_lane;
+
+		/*
+		 * We loop to make sure that the post map LBA didn't change
+		 * from under us between writing the RTT and doing the actual
+		 * read.
+		 */
+		while (1) {
+			u32 new_map;
+
+			if (t_flag) {
+				zero_fill_data(page, off, cur_len);
+				goto out_lane;
+			}
+
+			if (e_flag) {
+				ret = -EIO;
+				goto out_lane;
+			}
+
+			arena->rtt[lane] = RTT_VALID | postmap;
+			/*
+			 * Barrier to make sure this write is not reordered
+			 * to do the verification map_read before the RTT store
+			 */
+			barrier();
+
+			ret = btt_map_read(arena, premap, &new_map, &t_flag,
+						&e_flag);
+			if (ret)
+				goto out_rtt;
+
+			if (postmap == new_map)
+				break;
+
+			postmap = new_map;
+		}
+
+		ret = btt_data_read(arena, page, off, postmap, cur_len);
+		if (ret)
+			goto out_rtt;
+
+		if (bip) {
+			ret = btt_rw_integrity(btt, bip, arena, postmap, READ);
+			if (ret)
+				goto out_rtt;
+		}
+
+		arena->rtt[lane] = RTT_INVALID;
+		nd_region_release_lane(btt->nd_region, lane);
+
+		len -= cur_len;
+		off += cur_len;
+		sector += btt->sector_size >> SECTOR_SHIFT;
+	}
+
+	return 0;
+
+ out_rtt:
+	arena->rtt[lane] = RTT_INVALID;
+ out_lane:
+	nd_region_release_lane(btt->nd_region, lane);
+	return ret;
+}
+
+static int btt_write_pg(struct btt *btt, struct bio_integrity_payload *bip,
+			sector_t sector, struct page *page, unsigned int off,
+			unsigned int len)
+{
+	int ret = 0;
+	struct arena_info *arena = NULL;
+	u32 premap = 0, old_postmap, new_postmap, lane = 0, i;
+	struct log_entry log;
+	int sub;
+
+	while (len) {
+		u32 cur_len;
+
+		lane = nd_region_acquire_lane(btt->nd_region);
+
+		ret = lba_to_arena(btt, sector, &premap, &arena);
+		if (ret)
+			goto out_lane;
+		cur_len = min(btt->sector_size, len);
+
+		if ((arena->flags & IB_FLAG_ERROR_MASK) != 0) {
+			ret = -EIO;
+			goto out_lane;
+		}
+
+		new_postmap = arena->freelist[lane].block;
+
+		/* Wait if the new block is being read from */
+		for (i = 0; i < arena->nfree; i++)
+			while (arena->rtt[i] == (RTT_VALID | new_postmap))
+				cpu_relax();
+
+
+		if (new_postmap >= arena->internal_nlba) {
+			ret = -EIO;
+			goto out_lane;
+		}
+
+		ret = btt_data_write(arena, new_postmap, page, off, cur_len);
+		if (ret)
+			goto out_lane;
+
+		if (bip) {
+			ret = btt_rw_integrity(btt, bip, arena, new_postmap,
+						WRITE);
+			if (ret)
+				goto out_lane;
+		}
+
+		lock_map(arena, premap);
+		ret = btt_map_read(arena, premap, &old_postmap, NULL, NULL);
+		if (ret)
+			goto out_map;
+		if (old_postmap >= arena->internal_nlba) {
+			ret = -EIO;
+			goto out_map;
+		}
+
+		log.lba = cpu_to_le32(premap);
+		log.old_map = cpu_to_le32(old_postmap);
+		log.new_map = cpu_to_le32(new_postmap);
+		log.seq = cpu_to_le32(arena->freelist[lane].seq);
+		sub = arena->freelist[lane].sub;
+		ret = btt_flog_write(arena, lane, sub, &log);
+		if (ret)
+			goto out_map;
+
+		ret = btt_map_write(arena, premap, new_postmap, 0, 0);
+		if (ret)
+			goto out_map;
+
+		unlock_map(arena, premap);
+		nd_region_release_lane(btt->nd_region, lane);
+
+		len -= cur_len;
+		off += cur_len;
+		sector += btt->sector_size >> SECTOR_SHIFT;
+	}
+
+	return 0;
+
+ out_map:
+	unlock_map(arena, premap);
+ out_lane:
+	nd_region_release_lane(btt->nd_region, lane);
+	return ret;
+}
+
+static int btt_do_bvec(struct btt *btt, struct bio_integrity_payload *bip,
+			struct page *page, unsigned int len, unsigned int off,
+			int rw, sector_t sector)
+{
+	int ret;
+
+	if (rw == READ) {
+		ret = btt_read_pg(btt, bip, page, off, sector, len);
+		flush_dcache_page(page);
+	} else {
+		flush_dcache_page(page);
+		ret = btt_write_pg(btt, bip, sector, page, off, len);
+	}
+
+	return ret;
+}
+
+static void btt_make_request(struct request_queue *q, struct bio *bio)
+{
+	struct bio_integrity_payload *bip = bio_integrity(bio);
+	struct btt *btt = q->queuedata;
+	struct bvec_iter iter;
+	unsigned long start;
+	struct bio_vec bvec;
+	int err = 0, rw;
+	bool do_acct;
+
+	/*
+	 * bio_integrity_enabled also checks if the bio already has an
+	 * integrity payload attached. If it does, we *don't* do a
+	 * bio_integrity_prep here - the payload has been generated by
+	 * another kernel subsystem, and we just pass it through.
+	 */
+	if (bio_integrity_enabled(bio) && bio_integrity_prep(bio)) {
+		err = -EIO;
+		goto out;
+	}
+
+	do_acct = nd_iostat_start(bio, &start);
+	rw = bio_data_dir(bio);
+	bio_for_each_segment(bvec, bio, iter) {
+		unsigned int len = bvec.bv_len;
+
+		BUG_ON(len > PAGE_SIZE);
+		/* Make sure len is in multiples of sector size. */
+		/* XXX is this right? */
+		BUG_ON(len < btt->sector_size);
+		BUG_ON(len % btt->sector_size);
+
+		err = btt_do_bvec(btt, bip, bvec.bv_page, len, bvec.bv_offset,
+				rw, iter.bi_sector);
+		if (err) {
+			dev_info(&btt->nd_btt->dev,
+					"io error in %s sector %lld, len %d,\n",
+					(rw == READ) ? "READ" : "WRITE",
+					(unsigned long long) iter.bi_sector, len);
+			break;
+		}
+	}
+	if (do_acct)
+		nd_iostat_end(bio, start);
+
+out:
+	bio_endio(bio, err);
+}
+
+static int btt_rw_page(struct block_device *bdev, sector_t sector,
+		struct page *page, int rw)
+{
+	struct btt *btt = bdev->bd_disk->private_data;
+
+	btt_do_bvec(btt, NULL, page, PAGE_CACHE_SIZE, 0, rw, sector);
+	page_endio(page, rw & WRITE, 0);
+	return 0;
+}
+
+
+static int btt_getgeo(struct block_device *bd, struct hd_geometry *geo)
+{
+	/* some standard values */
+	geo->heads = 1 << 6;
+	geo->sectors = 1 << 5;
+	geo->cylinders = get_capacity(bd->bd_disk) >> 11;
+	return 0;
+}
+
+static const struct block_device_operations btt_fops = {
+	.owner =		THIS_MODULE,
+	.rw_page =		btt_rw_page,
+	.getgeo =		btt_getgeo,
+	.revalidate_disk =	nvdimm_revalidate_disk,
+};
+
+static int btt_blk_init(struct btt *btt)
+{
+	struct nd_btt *nd_btt = btt->nd_btt;
+	struct nd_namespace_common *ndns = nd_btt->ndns;
+
+	/* create a new disk and request queue for btt */
+	btt->btt_queue = blk_alloc_queue(GFP_KERNEL);
+	if (!btt->btt_queue)
+		return -ENOMEM;
+
+	btt->btt_disk = alloc_disk(0);
+	if (!btt->btt_disk) {
+		blk_cleanup_queue(btt->btt_queue);
+		return -ENOMEM;
+	}
+
+	nvdimm_namespace_disk_name(ndns, btt->btt_disk->disk_name);
+	btt->btt_disk->driverfs_dev = &btt->nd_btt->dev;
+	btt->btt_disk->major = btt_major;
+	btt->btt_disk->first_minor = 0;
+	btt->btt_disk->fops = &btt_fops;
+	btt->btt_disk->private_data = btt;
+	btt->btt_disk->queue = btt->btt_queue;
+	btt->btt_disk->flags = GENHD_FL_EXT_DEVT;
+
+	blk_queue_make_request(btt->btt_queue, btt_make_request);
+	blk_queue_logical_block_size(btt->btt_queue, btt->sector_size);
+	blk_queue_max_hw_sectors(btt->btt_queue, UINT_MAX);
+	blk_queue_bounce_limit(btt->btt_queue, BLK_BOUNCE_ANY);
+	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, btt->btt_queue);
+	btt->btt_queue->queuedata = btt;
+
+	set_capacity(btt->btt_disk, 0);
+	add_disk(btt->btt_disk);
+	if (btt_meta_size(btt)) {
+		int rc = nd_integrity_init(btt->btt_disk, btt_meta_size(btt));
+
+		if (rc) {
+			del_gendisk(btt->btt_disk);
+			put_disk(btt->btt_disk);
+			blk_cleanup_queue(btt->btt_queue);
+			return rc;
+		}
+	}
+	set_capacity(btt->btt_disk, btt->nlba * btt->sector_size >> 9);
+	revalidate_disk(btt->btt_disk);
+
+	return 0;
+}
+
+static void btt_blk_cleanup(struct btt *btt)
+{
+	blk_integrity_unregister(btt->btt_disk);
+	del_gendisk(btt->btt_disk);
+	put_disk(btt->btt_disk);
+	blk_cleanup_queue(btt->btt_queue);
+}
+
+/**
+ * btt_init - initialize a block translation table for the given device
+ * @nd_btt:	device with BTT geometry and backing device info
+ * @rawsize:	raw size in bytes of the backing device
+ * @lbasize:	lba size of the backing device
+ * @uuid:	A uuid for the backing device - this is stored on media
+ * @maxlane:	maximum number of parallel requests the device can handle
+ *
+ * Initialize a Block Translation Table on a backing device to provide
+ * single sector power fail atomicity.
+ *
+ * Context:
+ * Might sleep.
+ *
+ * Returns:
+ * Pointer to a new struct btt on success, NULL on failure.
+ */
+static struct btt *btt_init(struct nd_btt *nd_btt, unsigned long long rawsize,
+		u32 lbasize, u8 *uuid, struct nd_region *nd_region)
+{
+	int ret;
+	struct btt *btt;
+	struct device *dev = &nd_btt->dev;
+
+	btt = kzalloc(sizeof(struct btt), GFP_KERNEL);
+	if (!btt)
+		return NULL;
+
+	btt->nd_btt = nd_btt;
+	btt->rawsize = rawsize;
+	btt->lbasize = lbasize;
+	btt->sector_size = ((lbasize >= 4096) ? 4096 : 512);
+	INIT_LIST_HEAD(&btt->arena_list);
+	mutex_init(&btt->init_lock);
+	btt->nd_region = nd_region;
+
+	ret = discover_arenas(btt);
+	if (ret) {
+		dev_err(dev, "init: error in arena_discover: %d\n", ret);
+		goto out_free;
+	}
+
+	if (btt->init_state != INIT_READY && nd_region->ro) {
+		dev_info(dev, "%s is read-only, unable to init btt metadata\n",
+				dev_name(&nd_region->dev));
+		goto out_free;
+	} else if (btt->init_state != INIT_READY) {
+		btt->num_arenas = (rawsize / ARENA_MAX_SIZE) +
+			((rawsize % ARENA_MAX_SIZE) ? 1 : 0);
+		dev_dbg(dev, "init: %d arenas for %llu rawsize\n",
+				btt->num_arenas, rawsize);
+
+		ret = create_arenas(btt);
+		if (ret) {
+			dev_info(dev, "init: create_arenas: %d\n", ret);
+			goto out_free;
+		}
+
+		ret = btt_meta_init(btt);
+		if (ret) {
+			dev_err(dev, "init: error in meta_init: %d\n", ret);
+			goto out_free;
+		}
+	}
+
+	ret = btt_blk_init(btt);
+	if (ret) {
+		dev_err(dev, "init: error in blk_init: %d\n", ret);
+		goto out_free;
+	}
+
+	btt_debugfs_init(btt);
+
+	return btt;
+
+ out_free:
+	kfree(btt);
+	return NULL;
+}
+
+/**
+ * btt_fini - de-initialize a BTT
+ * @btt:	the BTT handle that was generated by btt_init
+ *
+ * De-initialize a Block Translation Table on device removal
+ *
+ * Context:
+ * Might sleep.
+ */
+static void btt_fini(struct btt *btt)
+{
+	if (btt) {
+		btt_blk_cleanup(btt);
+		free_arenas(btt);
+		debugfs_remove_recursive(btt->debugfs_dir);
+		kfree(btt);
+	}
+}
+
+int nvdimm_namespace_attach_btt(struct nd_namespace_common *ndns)
+{
+	struct nd_btt *nd_btt = to_nd_btt(ndns->claim);
+	struct nd_region *nd_region;
+	struct btt *btt;
+	size_t rawsize;
+
+	if (!nd_btt->uuid || !nd_btt->ndns || !nd_btt->lbasize)
+		return -ENODEV;
+
+	rawsize = nvdimm_namespace_capacity(ndns) - SZ_4K;
+	if (rawsize < ARENA_MIN_SIZE) {
+		return -ENXIO;
+	}
+	nd_region = to_nd_region(nd_btt->dev.parent);
+	btt = btt_init(nd_btt, rawsize, nd_btt->lbasize, nd_btt->uuid,
+			nd_region);
+	if (!btt)
+		return -ENOMEM;
+	nd_btt->btt = btt;
+
+	return 0;
+}
+EXPORT_SYMBOL(nvdimm_namespace_attach_btt);
+
+int nvdimm_namespace_detach_btt(struct nd_namespace_common *ndns)
+{
+	struct nd_btt *nd_btt = to_nd_btt(ndns->claim);
+	struct btt *btt = nd_btt->btt;
+
+	btt_fini(btt);
+	nd_btt->btt = NULL;
+
+	return 0;
+}
+EXPORT_SYMBOL(nvdimm_namespace_detach_btt);
+
+static int __init nd_btt_init(void)
+{
+	int rc;
+
+	BUILD_BUG_ON(sizeof(struct btt_sb) != SZ_4K);
+
+	btt_major = register_blkdev(0, "btt");
+	if (btt_major < 0)
+		return btt_major;
+
+	debugfs_root = debugfs_create_dir("btt", NULL);
+	if (IS_ERR_OR_NULL(debugfs_root)) {
+		rc = -ENXIO;
+		goto err_debugfs;
+	}
+
+	return 0;
+
+ err_debugfs:
+	unregister_blkdev(btt_major, "btt");
+
+	return rc;
+}
+
+static void __exit nd_btt_exit(void)
+{
+	debugfs_remove_recursive(debugfs_root);
+	unregister_blkdev(btt_major, "btt");
+}
+
+MODULE_ALIAS_ND_DEVICE(ND_DEVICE_BTT);
+MODULE_AUTHOR("Vishal Verma <vishal.l.verma@linux.intel.com>");
+MODULE_LICENSE("GPL v2");
+module_init(nd_btt_init);
+module_exit(nd_btt_exit);
diff --git a/drivers/nvdimm/btt.h b/drivers/nvdimm/btt.h
new file mode 100644
index 000000000000..75b0d80a6bd9
--- /dev/null
+++ b/drivers/nvdimm/btt.h
@@ -0,0 +1,185 @@
+/*
+ * Block Translation Table library
+ * Copyright (c) 2014-2015, 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.
+ */
+
+#ifndef _LINUX_BTT_H
+#define _LINUX_BTT_H
+
+#include <linux/types.h>
+
+#define BTT_SIG_LEN 16
+#define BTT_SIG "BTT_ARENA_INFO\0"
+#define MAP_ENT_SIZE 4
+#define MAP_TRIM_SHIFT 31
+#define MAP_TRIM_MASK (1 << MAP_TRIM_SHIFT)
+#define MAP_ERR_SHIFT 30
+#define MAP_ERR_MASK (1 << MAP_ERR_SHIFT)
+#define MAP_LBA_MASK (~((1 << MAP_TRIM_SHIFT) | (1 << MAP_ERR_SHIFT)))
+#define MAP_ENT_NORMAL 0xC0000000
+#define LOG_ENT_SIZE sizeof(struct log_entry)
+#define ARENA_MIN_SIZE (1UL << 24)	/* 16 MB */
+#define ARENA_MAX_SIZE (1ULL << 39)	/* 512 GB */
+#define RTT_VALID (1UL << 31)
+#define RTT_INVALID 0
+#define BTT_PG_SIZE 4096
+#define BTT_DEFAULT_NFREE ND_MAX_LANES
+#define LOG_SEQ_INIT 1
+
+#define IB_FLAG_ERROR 0x00000001
+#define IB_FLAG_ERROR_MASK 0x00000001
+
+enum btt_init_state {
+	INIT_UNCHECKED = 0,
+	INIT_NOTFOUND,
+	INIT_READY
+};
+
+struct log_entry {
+	__le32 lba;
+	__le32 old_map;
+	__le32 new_map;
+	__le32 seq;
+	__le64 padding[2];
+};
+
+struct btt_sb {
+	u8 signature[BTT_SIG_LEN];
+	u8 uuid[16];
+	u8 parent_uuid[16];
+	__le32 flags;
+	__le16 version_major;
+	__le16 version_minor;
+	__le32 external_lbasize;
+	__le32 external_nlba;
+	__le32 internal_lbasize;
+	__le32 internal_nlba;
+	__le32 nfree;
+	__le32 infosize;
+	__le64 nextoff;
+	__le64 dataoff;
+	__le64 mapoff;
+	__le64 logoff;
+	__le64 info2off;
+	u8 padding[3968];
+	__le64 checksum;
+};
+
+struct free_entry {
+	u32 block;
+	u8 sub;
+	u8 seq;
+};
+
+struct aligned_lock {
+	union {
+		spinlock_t lock;
+		u8 cacheline_padding[L1_CACHE_BYTES];
+	};
+};
+
+/**
+ * struct arena_info - handle for an arena
+ * @size:		Size in bytes this arena occupies on the raw device.
+ *			This includes arena metadata.
+ * @external_lba_start:	The first external LBA in this arena.
+ * @internal_nlba:	Number of internal blocks available in the arena
+ *			including nfree reserved blocks
+ * @internal_lbasize:	Internal and external lba sizes may be different as
+ *			we can round up 'odd' external lbasizes such as 520B
+ *			to be aligned.
+ * @external_nlba:	Number of blocks contributed by the arena to the number
+ *			reported to upper layers. (internal_nlba - nfree)
+ * @external_lbasize:	LBA size as exposed to upper layers.
+ * @nfree:		A reserve number of 'free' blocks that is used to
+ *			handle incoming writes.
+ * @version_major:	Metadata layout version major.
+ * @version_minor:	Metadata layout version minor.
+ * @nextoff:		Offset in bytes to the start of the next arena.
+ * @infooff:		Offset in bytes to the info block of this arena.
+ * @dataoff:		Offset in bytes to the data area of this arena.
+ * @mapoff:		Offset in bytes to the map area of this arena.
+ * @logoff:		Offset in bytes to the log area of this arena.
+ * @info2off:		Offset in bytes to the backup info block of this arena.
+ * @freelist:		Pointer to in-memory list of free blocks
+ * @rtt:		Pointer to in-memory "Read Tracking Table"
+ * @map_locks:		Spinlocks protecting concurrent map writes
+ * @nd_btt:		Pointer to parent nd_btt structure.
+ * @list:		List head for list of arenas
+ * @debugfs_dir:	Debugfs dentry
+ * @flags:		Arena flags - may signify error states.
+ *
+ * arena_info is a per-arena handle. Once an arena is narrowed down for an
+ * IO, this struct is passed around for the duration of the IO.
+ */
+struct arena_info {
+	u64 size;			/* Total bytes for this arena */
+	u64 external_lba_start;
+	u32 internal_nlba;
+	u32 internal_lbasize;
+	u32 external_nlba;
+	u32 external_lbasize;
+	u32 nfree;
+	u16 version_major;
+	u16 version_minor;
+	/* Byte offsets to the different on-media structures */
+	u64 nextoff;
+	u64 infooff;
+	u64 dataoff;
+	u64 mapoff;
+	u64 logoff;
+	u64 info2off;
+	/* Pointers to other in-memory structures for this arena */
+	struct free_entry *freelist;
+	u32 *rtt;
+	struct aligned_lock *map_locks;
+	struct nd_btt *nd_btt;
+	struct list_head list;
+	struct dentry *debugfs_dir;
+	/* Arena flags */
+	u32 flags;
+};
+
+/**
+ * struct btt - handle for a BTT instance
+ * @btt_disk:		Pointer to the gendisk for BTT device
+ * @btt_queue:		Pointer to the request queue for the BTT device
+ * @arena_list:		Head of the list of arenas
+ * @debugfs_dir:	Debugfs dentry
+ * @nd_btt:		Parent nd_btt struct
+ * @nlba:		Number of logical blocks exposed to the	upper layers
+ *			after removing the amount of space needed by metadata
+ * @rawsize:		Total size in bytes of the available backing device
+ * @lbasize:		LBA size as requested and presented to upper layers.
+ *			This is sector_size + size of any metadata.
+ * @sector_size:	The Linux sector size - 512 or 4096
+ * @lanes:		Per-lane spinlocks
+ * @init_lock:		Mutex used for the BTT initialization
+ * @init_state:		Flag describing the initialization state for the BTT
+ * @num_arenas:		Number of arenas in the BTT instance
+ */
+struct btt {
+	struct gendisk *btt_disk;
+	struct request_queue *btt_queue;
+	struct list_head arena_list;
+	struct dentry *debugfs_dir;
+	struct nd_btt *nd_btt;
+	u64 nlba;
+	unsigned long long rawsize;
+	u32 lbasize;
+	u32 sector_size;
+	struct nd_region *nd_region;
+	struct mutex init_lock;
+	int init_state;
+	int num_arenas;
+};
+#endif
diff --git a/drivers/nvdimm/btt_devs.c b/drivers/nvdimm/btt_devs.c
new file mode 100644
index 000000000000..6ac8c0fea3ec
--- /dev/null
+++ b/drivers/nvdimm/btt_devs.c
@@ -0,0 +1,425 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#include <linux/blkdev.h>
+#include <linux/device.h>
+#include <linux/genhd.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include "nd-core.h"
+#include "btt.h"
+#include "nd.h"
+
+static void __nd_btt_detach_ndns(struct nd_btt *nd_btt)
+{
+	struct nd_namespace_common *ndns = nd_btt->ndns;
+
+	dev_WARN_ONCE(&nd_btt->dev, !mutex_is_locked(&ndns->dev.mutex)
+			|| ndns->claim != &nd_btt->dev,
+			"%s: invalid claim\n", __func__);
+	ndns->claim = NULL;
+	nd_btt->ndns = NULL;
+	put_device(&ndns->dev);
+}
+
+static void nd_btt_detach_ndns(struct nd_btt *nd_btt)
+{
+	struct nd_namespace_common *ndns = nd_btt->ndns;
+
+	if (!ndns)
+		return;
+	get_device(&ndns->dev);
+	device_lock(&ndns->dev);
+	__nd_btt_detach_ndns(nd_btt);
+	device_unlock(&ndns->dev);
+	put_device(&ndns->dev);
+}
+
+static bool __nd_btt_attach_ndns(struct nd_btt *nd_btt,
+		struct nd_namespace_common *ndns)
+{
+	if (ndns->claim)
+		return false;
+	dev_WARN_ONCE(&nd_btt->dev, !mutex_is_locked(&ndns->dev.mutex)
+			|| nd_btt->ndns,
+			"%s: invalid claim\n", __func__);
+	ndns->claim = &nd_btt->dev;
+	nd_btt->ndns = ndns;
+	get_device(&ndns->dev);
+	return true;
+}
+
+static bool nd_btt_attach_ndns(struct nd_btt *nd_btt,
+		struct nd_namespace_common *ndns)
+{
+	bool claimed;
+
+	device_lock(&ndns->dev);
+	claimed = __nd_btt_attach_ndns(nd_btt, ndns);
+	device_unlock(&ndns->dev);
+	return claimed;
+}
+
+static void nd_btt_release(struct device *dev)
+{
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	struct nd_btt *nd_btt = to_nd_btt(dev);
+
+	dev_dbg(dev, "%s\n", __func__);
+	nd_btt_detach_ndns(nd_btt);
+	ida_simple_remove(&nd_region->btt_ida, nd_btt->id);
+	kfree(nd_btt->uuid);
+	kfree(nd_btt);
+}
+
+static struct device_type nd_btt_device_type = {
+	.name = "nd_btt",
+	.release = nd_btt_release,
+};
+
+bool is_nd_btt(struct device *dev)
+{
+	return dev->type == &nd_btt_device_type;
+}
+EXPORT_SYMBOL(is_nd_btt);
+
+struct nd_btt *to_nd_btt(struct device *dev)
+{
+	struct nd_btt *nd_btt = container_of(dev, struct nd_btt, dev);
+
+	WARN_ON(!is_nd_btt(dev));
+	return nd_btt;
+}
+EXPORT_SYMBOL(to_nd_btt);
+
+static const unsigned long btt_lbasize_supported[] = { 512, 520, 528,
+	4096, 4104, 4160, 4224, 0 };
+
+static ssize_t sector_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_btt *nd_btt = to_nd_btt(dev);
+
+	return nd_sector_size_show(nd_btt->lbasize, btt_lbasize_supported, buf);
+}
+
+static ssize_t sector_size_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct nd_btt *nd_btt = to_nd_btt(dev);
+	ssize_t rc;
+
+	device_lock(dev);
+	nvdimm_bus_lock(dev);
+	rc = nd_sector_size_store(dev, buf, &nd_btt->lbasize,
+			btt_lbasize_supported);
+	dev_dbg(dev, "%s: result: %zd wrote: %s%s", __func__,
+			rc, buf, buf[len - 1] == '\n' ? "" : "\n");
+	nvdimm_bus_unlock(dev);
+	device_unlock(dev);
+
+	return rc ? rc : len;
+}
+static DEVICE_ATTR_RW(sector_size);
+
+static ssize_t uuid_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_btt *nd_btt = to_nd_btt(dev);
+
+	if (nd_btt->uuid)
+		return sprintf(buf, "%pUb\n", nd_btt->uuid);
+	return sprintf(buf, "\n");
+}
+
+static ssize_t uuid_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct nd_btt *nd_btt = to_nd_btt(dev);
+	ssize_t rc;
+
+	device_lock(dev);
+	rc = nd_uuid_store(dev, &nd_btt->uuid, buf, len);
+	dev_dbg(dev, "%s: result: %zd wrote: %s%s", __func__,
+			rc, buf, buf[len - 1] == '\n' ? "" : "\n");
+	device_unlock(dev);
+
+	return rc ? rc : len;
+}
+static DEVICE_ATTR_RW(uuid);
+
+static ssize_t namespace_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_btt *nd_btt = to_nd_btt(dev);
+	ssize_t rc;
+
+	nvdimm_bus_lock(dev);
+	rc = sprintf(buf, "%s\n", nd_btt->ndns
+			? dev_name(&nd_btt->ndns->dev) : "");
+	nvdimm_bus_unlock(dev);
+	return rc;
+}
+
+static int namespace_match(struct device *dev, void *data)
+{
+	char *name = data;
+
+	return strcmp(name, dev_name(dev)) == 0;
+}
+
+static bool is_nd_btt_idle(struct device *dev)
+{
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	struct nd_btt *nd_btt = to_nd_btt(dev);
+
+	if (nd_region->btt_seed == dev || nd_btt->ndns || dev->driver)
+		return false;
+	return true;
+}
+
+static ssize_t __namespace_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct nd_btt *nd_btt = to_nd_btt(dev);
+	struct nd_namespace_common *ndns;
+	struct device *found;
+	char *name;
+
+	if (dev->driver) {
+		dev_dbg(dev, "%s: -EBUSY\n", __func__);
+		return -EBUSY;
+	}
+
+	name = kstrndup(buf, len, GFP_KERNEL);
+	if (!name)
+		return -ENOMEM;
+	strim(name);
+
+	if (strncmp(name, "namespace", 9) == 0 || strcmp(name, "") == 0)
+		/* pass */;
+	else {
+		len = -EINVAL;
+		goto out;
+	}
+
+	ndns = nd_btt->ndns;
+	if (strcmp(name, "") == 0) {
+		/* detach the namespace and destroy / reset the btt device */
+		nd_btt_detach_ndns(nd_btt);
+		if (is_nd_btt_idle(dev))
+			nd_device_unregister(dev, ND_ASYNC);
+		else {
+			nd_btt->lbasize = 0;
+			kfree(nd_btt->uuid);
+			nd_btt->uuid = NULL;
+		}
+		goto out;
+	} else if (ndns) {
+		dev_dbg(dev, "namespace already set to: %s\n",
+				dev_name(&ndns->dev));
+		len = -EBUSY;
+		goto out;
+	}
+
+	found = device_find_child(dev->parent, name, namespace_match);
+	if (!found) {
+		dev_dbg(dev, "'%s' not found under %s\n", name,
+				dev_name(dev->parent));
+		len = -ENODEV;
+		goto out;
+	}
+
+	ndns = to_ndns(found);
+	if (__nvdimm_namespace_capacity(ndns) < SZ_16M) {
+		dev_dbg(dev, "%s too small to host btt\n", name);
+		len = -ENXIO;
+		goto out_attach;
+	}
+
+	WARN_ON_ONCE(!is_nvdimm_bus_locked(&nd_btt->dev));
+	if (!nd_btt_attach_ndns(nd_btt, ndns)) {
+		dev_dbg(dev, "%s already claimed\n",
+				dev_name(&ndns->dev));
+		len = -EBUSY;
+	}
+
+ out_attach:
+	put_device(&ndns->dev); /* from device_find_child */
+ out:
+	kfree(name);
+	return len;
+}
+
+static ssize_t namespace_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	ssize_t rc;
+
+	nvdimm_bus_lock(dev);
+	device_lock(dev);
+	rc = __namespace_store(dev, attr, buf, len);
+	dev_dbg(dev, "%s: result: %zd wrote: %s%s", __func__,
+			rc, buf, buf[len - 1] == '\n' ? "" : "\n");
+	device_unlock(dev);
+	nvdimm_bus_unlock(dev);
+
+	return rc;
+}
+static DEVICE_ATTR_RW(namespace);
+
+static struct attribute *nd_btt_attributes[] = {
+	&dev_attr_sector_size.attr,
+	&dev_attr_namespace.attr,
+	&dev_attr_uuid.attr,
+	NULL,
+};
+
+static struct attribute_group nd_btt_attribute_group = {
+	.attrs = nd_btt_attributes,
+};
+
+static const struct attribute_group *nd_btt_attribute_groups[] = {
+	&nd_btt_attribute_group,
+	&nd_device_attribute_group,
+	&nd_numa_attribute_group,
+	NULL,
+};
+
+static struct device *__nd_btt_create(struct nd_region *nd_region,
+		unsigned long lbasize, u8 *uuid,
+		struct nd_namespace_common *ndns)
+{
+	struct nd_btt *nd_btt;
+	struct device *dev;
+
+	nd_btt = kzalloc(sizeof(*nd_btt), GFP_KERNEL);
+	if (!nd_btt)
+		return NULL;
+
+	nd_btt->id = ida_simple_get(&nd_region->btt_ida, 0, 0, GFP_KERNEL);
+	if (nd_btt->id < 0) {
+		kfree(nd_btt);
+		return NULL;
+	}
+
+	nd_btt->lbasize = lbasize;
+	if (uuid)
+		uuid = kmemdup(uuid, 16, GFP_KERNEL);
+	nd_btt->uuid = uuid;
+	dev = &nd_btt->dev;
+	dev_set_name(dev, "btt%d.%d", nd_region->id, nd_btt->id);
+	dev->parent = &nd_region->dev;
+	dev->type = &nd_btt_device_type;
+	dev->groups = nd_btt_attribute_groups;
+	device_initialize(&nd_btt->dev);
+	if (ndns && !__nd_btt_attach_ndns(nd_btt, ndns)) {
+		dev_dbg(&ndns->dev, "%s failed, already claimed by %s\n",
+				__func__, dev_name(ndns->claim));
+		put_device(dev);
+		return NULL;
+	}
+	return dev;
+}
+
+struct device *nd_btt_create(struct nd_region *nd_region)
+{
+	struct device *dev = __nd_btt_create(nd_region, 0, NULL, NULL);
+
+	if (dev)
+		__nd_device_register(dev);
+	return dev;
+}
+
+/*
+ * nd_btt_sb_checksum: compute checksum for btt info block
+ *
+ * Returns a fletcher64 checksum of everything in the given info block
+ * except the last field (since that's where the checksum lives).
+ */
+u64 nd_btt_sb_checksum(struct btt_sb *btt_sb)
+{
+	u64 sum;
+	__le64 sum_save;
+
+	sum_save = btt_sb->checksum;
+	btt_sb->checksum = 0;
+	sum = nd_fletcher64(btt_sb, sizeof(*btt_sb), 1);
+	btt_sb->checksum = sum_save;
+	return sum;
+}
+EXPORT_SYMBOL(nd_btt_sb_checksum);
+
+static int __nd_btt_probe(struct nd_btt *nd_btt,
+		struct nd_namespace_common *ndns, struct btt_sb *btt_sb)
+{
+	u64 checksum;
+
+	if (!btt_sb || !ndns || !nd_btt)
+		return -ENODEV;
+
+	if (nvdimm_read_bytes(ndns, SZ_4K, btt_sb, sizeof(*btt_sb)))
+		return -ENXIO;
+
+	if (nvdimm_namespace_capacity(ndns) < SZ_16M)
+		return -ENXIO;
+
+	if (memcmp(btt_sb->signature, BTT_SIG, BTT_SIG_LEN) != 0)
+		return -ENODEV;
+
+	checksum = le64_to_cpu(btt_sb->checksum);
+	btt_sb->checksum = 0;
+	if (checksum != nd_btt_sb_checksum(btt_sb))
+		return -ENODEV;
+	btt_sb->checksum = cpu_to_le64(checksum);
+
+	nd_btt->lbasize = le32_to_cpu(btt_sb->external_lbasize);
+	nd_btt->uuid = kmemdup(btt_sb->uuid, 16, GFP_KERNEL);
+	if (!nd_btt->uuid)
+		return -ENOMEM;
+
+	__nd_device_register(&nd_btt->dev);
+
+	return 0;
+}
+
+int nd_btt_probe(struct nd_namespace_common *ndns, void *drvdata)
+{
+	int rc;
+	struct device *dev;
+	struct btt_sb *btt_sb;
+	struct nd_region *nd_region = to_nd_region(ndns->dev.parent);
+
+	if (ndns->force_raw)
+		return -ENODEV;
+
+	nvdimm_bus_lock(&ndns->dev);
+	dev = __nd_btt_create(nd_region, 0, NULL, ndns);
+	nvdimm_bus_unlock(&ndns->dev);
+	if (!dev)
+		return -ENOMEM;
+	dev_set_drvdata(dev, drvdata);
+	btt_sb = kzalloc(sizeof(*btt_sb), GFP_KERNEL);
+	rc = __nd_btt_probe(to_nd_btt(dev), ndns, btt_sb);
+	kfree(btt_sb);
+	dev_dbg(&ndns->dev, "%s: btt: %s\n", __func__,
+			rc == 0 ? dev_name(dev) : "<none>");
+	if (rc < 0) {
+		__nd_btt_detach_ndns(to_nd_btt(dev));
+		put_device(dev);
+	}
+
+	return rc;
+}
+EXPORT_SYMBOL(nd_btt_probe);
diff --git a/drivers/nvdimm/bus.c b/drivers/nvdimm/bus.c
new file mode 100644
index 000000000000..7e2c43f701bc
--- /dev/null
+++ b/drivers/nvdimm/bus.c
@@ -0,0 +1,725 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+#include <linux/vmalloc.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+#include <linux/blkdev.h>
+#include <linux/fcntl.h>
+#include <linux/async.h>
+#include <linux/genhd.h>
+#include <linux/ndctl.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/nd.h>
+#include "nd-core.h"
+#include "nd.h"
+
+int nvdimm_major;
+static int nvdimm_bus_major;
+static struct class *nd_class;
+
+static int to_nd_device_type(struct device *dev)
+{
+	if (is_nvdimm(dev))
+		return ND_DEVICE_DIMM;
+	else if (is_nd_pmem(dev))
+		return ND_DEVICE_REGION_PMEM;
+	else if (is_nd_blk(dev))
+		return ND_DEVICE_REGION_BLK;
+	else if (is_nd_pmem(dev->parent) || is_nd_blk(dev->parent))
+		return nd_region_to_nstype(to_nd_region(dev->parent));
+
+	return 0;
+}
+
+static int nvdimm_bus_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	/*
+	 * Ensure that region devices always have their numa node set as
+	 * early as possible.
+	 */
+	if (is_nd_pmem(dev) || is_nd_blk(dev))
+		set_dev_node(dev, to_nd_region(dev)->numa_node);
+	return add_uevent_var(env, "MODALIAS=" ND_DEVICE_MODALIAS_FMT,
+			to_nd_device_type(dev));
+}
+
+static int nvdimm_bus_match(struct device *dev, struct device_driver *drv)
+{
+	struct nd_device_driver *nd_drv = to_nd_device_driver(drv);
+
+	return test_bit(to_nd_device_type(dev), &nd_drv->type);
+}
+
+static struct module *to_bus_provider(struct device *dev)
+{
+	/* pin bus providers while regions are enabled */
+	if (is_nd_pmem(dev) || is_nd_blk(dev)) {
+		struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+
+		return nvdimm_bus->module;
+	}
+	return NULL;
+}
+
+static void nvdimm_bus_probe_start(struct nvdimm_bus *nvdimm_bus)
+{
+	nvdimm_bus_lock(&nvdimm_bus->dev);
+	nvdimm_bus->probe_active++;
+	nvdimm_bus_unlock(&nvdimm_bus->dev);
+}
+
+static void nvdimm_bus_probe_end(struct nvdimm_bus *nvdimm_bus)
+{
+	nvdimm_bus_lock(&nvdimm_bus->dev);
+	if (--nvdimm_bus->probe_active == 0)
+		wake_up(&nvdimm_bus->probe_wait);
+	nvdimm_bus_unlock(&nvdimm_bus->dev);
+}
+
+static int nvdimm_bus_probe(struct device *dev)
+{
+	struct nd_device_driver *nd_drv = to_nd_device_driver(dev->driver);
+	struct module *provider = to_bus_provider(dev);
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+	int rc;
+
+	if (!try_module_get(provider))
+		return -ENXIO;
+
+	nvdimm_bus_probe_start(nvdimm_bus);
+	rc = nd_drv->probe(dev);
+	if (rc == 0)
+		nd_region_probe_success(nvdimm_bus, dev);
+	else
+		nd_region_disable(nvdimm_bus, dev);
+	nvdimm_bus_probe_end(nvdimm_bus);
+
+	dev_dbg(&nvdimm_bus->dev, "%s.probe(%s) = %d\n", dev->driver->name,
+			dev_name(dev), rc);
+
+	if (rc != 0)
+		module_put(provider);
+	return rc;
+}
+
+static int nvdimm_bus_remove(struct device *dev)
+{
+	struct nd_device_driver *nd_drv = to_nd_device_driver(dev->driver);
+	struct module *provider = to_bus_provider(dev);
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+	int rc;
+
+	rc = nd_drv->remove(dev);
+	nd_region_disable(nvdimm_bus, dev);
+
+	dev_dbg(&nvdimm_bus->dev, "%s.remove(%s) = %d\n", dev->driver->name,
+			dev_name(dev), rc);
+	module_put(provider);
+	return rc;
+}
+
+static struct bus_type nvdimm_bus_type = {
+	.name = "nd",
+	.uevent = nvdimm_bus_uevent,
+	.match = nvdimm_bus_match,
+	.probe = nvdimm_bus_probe,
+	.remove = nvdimm_bus_remove,
+};
+
+static ASYNC_DOMAIN_EXCLUSIVE(nd_async_domain);
+
+void nd_synchronize(void)
+{
+	async_synchronize_full_domain(&nd_async_domain);
+}
+EXPORT_SYMBOL_GPL(nd_synchronize);
+
+static void nd_async_device_register(void *d, async_cookie_t cookie)
+{
+	struct device *dev = d;
+
+	if (device_add(dev) != 0) {
+		dev_err(dev, "%s: failed\n", __func__);
+		put_device(dev);
+	}
+	put_device(dev);
+}
+
+static void nd_async_device_unregister(void *d, async_cookie_t cookie)
+{
+	struct device *dev = d;
+
+	/* flush bus operations before delete */
+	nvdimm_bus_lock(dev);
+	nvdimm_bus_unlock(dev);
+
+	device_unregister(dev);
+	put_device(dev);
+}
+
+void __nd_device_register(struct device *dev)
+{
+	dev->bus = &nvdimm_bus_type;
+	get_device(dev);
+	async_schedule_domain(nd_async_device_register, dev,
+			&nd_async_domain);
+}
+
+void nd_device_register(struct device *dev)
+{
+	device_initialize(dev);
+	__nd_device_register(dev);
+}
+EXPORT_SYMBOL(nd_device_register);
+
+void nd_device_unregister(struct device *dev, enum nd_async_mode mode)
+{
+	switch (mode) {
+	case ND_ASYNC:
+		get_device(dev);
+		async_schedule_domain(nd_async_device_unregister, dev,
+				&nd_async_domain);
+		break;
+	case ND_SYNC:
+		nd_synchronize();
+		device_unregister(dev);
+		break;
+	}
+}
+EXPORT_SYMBOL(nd_device_unregister);
+
+/**
+ * __nd_driver_register() - register a region or a namespace driver
+ * @nd_drv: driver to register
+ * @owner: automatically set by nd_driver_register() macro
+ * @mod_name: automatically set by nd_driver_register() macro
+ */
+int __nd_driver_register(struct nd_device_driver *nd_drv, struct module *owner,
+		const char *mod_name)
+{
+	struct device_driver *drv = &nd_drv->drv;
+
+	if (!nd_drv->type) {
+		pr_debug("driver type bitmask not set (%pf)\n",
+				__builtin_return_address(0));
+		return -EINVAL;
+	}
+
+	if (!nd_drv->probe || !nd_drv->remove) {
+		pr_debug("->probe() and ->remove() must be specified\n");
+		return -EINVAL;
+	}
+
+	drv->bus = &nvdimm_bus_type;
+	drv->owner = owner;
+	drv->mod_name = mod_name;
+
+	return driver_register(drv);
+}
+EXPORT_SYMBOL(__nd_driver_register);
+
+int nvdimm_revalidate_disk(struct gendisk *disk)
+{
+	struct device *dev = disk->driverfs_dev;
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	const char *pol = nd_region->ro ? "only" : "write";
+
+	if (nd_region->ro == get_disk_ro(disk))
+		return 0;
+
+	dev_info(dev, "%s read-%s, marking %s read-%s\n",
+			dev_name(&nd_region->dev), pol, disk->disk_name, pol);
+	set_disk_ro(disk, nd_region->ro);
+
+	return 0;
+
+}
+EXPORT_SYMBOL(nvdimm_revalidate_disk);
+
+static ssize_t modalias_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	return sprintf(buf, ND_DEVICE_MODALIAS_FMT "\n",
+			to_nd_device_type(dev));
+}
+static DEVICE_ATTR_RO(modalias);
+
+static ssize_t devtype_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	return sprintf(buf, "%s\n", dev->type->name);
+}
+static DEVICE_ATTR_RO(devtype);
+
+static struct attribute *nd_device_attributes[] = {
+	&dev_attr_modalias.attr,
+	&dev_attr_devtype.attr,
+	NULL,
+};
+
+/**
+ * nd_device_attribute_group - generic attributes for all devices on an nd bus
+ */
+struct attribute_group nd_device_attribute_group = {
+	.attrs = nd_device_attributes,
+};
+EXPORT_SYMBOL_GPL(nd_device_attribute_group);
+
+static ssize_t numa_node_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d\n", dev_to_node(dev));
+}
+static DEVICE_ATTR_RO(numa_node);
+
+static struct attribute *nd_numa_attributes[] = {
+	&dev_attr_numa_node.attr,
+	NULL,
+};
+
+static umode_t nd_numa_attr_visible(struct kobject *kobj, struct attribute *a,
+		int n)
+{
+	if (!IS_ENABLED(CONFIG_NUMA))
+		return 0;
+
+	return a->mode;
+}
+
+/**
+ * nd_numa_attribute_group - NUMA attributes for all devices on an nd bus
+ */
+struct attribute_group nd_numa_attribute_group = {
+	.attrs = nd_numa_attributes,
+	.is_visible = nd_numa_attr_visible,
+};
+EXPORT_SYMBOL_GPL(nd_numa_attribute_group);
+
+int nvdimm_bus_create_ndctl(struct nvdimm_bus *nvdimm_bus)
+{
+	dev_t devt = MKDEV(nvdimm_bus_major, nvdimm_bus->id);
+	struct device *dev;
+
+	dev = device_create(nd_class, &nvdimm_bus->dev, devt, nvdimm_bus,
+			"ndctl%d", nvdimm_bus->id);
+
+	if (IS_ERR(dev)) {
+		dev_dbg(&nvdimm_bus->dev, "failed to register ndctl%d: %ld\n",
+				nvdimm_bus->id, PTR_ERR(dev));
+		return PTR_ERR(dev);
+	}
+	return 0;
+}
+
+void nvdimm_bus_destroy_ndctl(struct nvdimm_bus *nvdimm_bus)
+{
+	device_destroy(nd_class, MKDEV(nvdimm_bus_major, nvdimm_bus->id));
+}
+
+static const struct nd_cmd_desc __nd_cmd_dimm_descs[] = {
+	[ND_CMD_IMPLEMENTED] = { },
+	[ND_CMD_SMART] = {
+		.out_num = 2,
+		.out_sizes = { 4, 8, },
+	},
+	[ND_CMD_SMART_THRESHOLD] = {
+		.out_num = 2,
+		.out_sizes = { 4, 8, },
+	},
+	[ND_CMD_DIMM_FLAGS] = {
+		.out_num = 2,
+		.out_sizes = { 4, 4 },
+	},
+	[ND_CMD_GET_CONFIG_SIZE] = {
+		.out_num = 3,
+		.out_sizes = { 4, 4, 4, },
+	},
+	[ND_CMD_GET_CONFIG_DATA] = {
+		.in_num = 2,
+		.in_sizes = { 4, 4, },
+		.out_num = 2,
+		.out_sizes = { 4, UINT_MAX, },
+	},
+	[ND_CMD_SET_CONFIG_DATA] = {
+		.in_num = 3,
+		.in_sizes = { 4, 4, UINT_MAX, },
+		.out_num = 1,
+		.out_sizes = { 4, },
+	},
+	[ND_CMD_VENDOR] = {
+		.in_num = 3,
+		.in_sizes = { 4, 4, UINT_MAX, },
+		.out_num = 3,
+		.out_sizes = { 4, 4, UINT_MAX, },
+	},
+};
+
+const struct nd_cmd_desc *nd_cmd_dimm_desc(int cmd)
+{
+	if (cmd < ARRAY_SIZE(__nd_cmd_dimm_descs))
+		return &__nd_cmd_dimm_descs[cmd];
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(nd_cmd_dimm_desc);
+
+static const struct nd_cmd_desc __nd_cmd_bus_descs[] = {
+	[ND_CMD_IMPLEMENTED] = { },
+	[ND_CMD_ARS_CAP] = {
+		.in_num = 2,
+		.in_sizes = { 8, 8, },
+		.out_num = 2,
+		.out_sizes = { 4, 4, },
+	},
+	[ND_CMD_ARS_START] = {
+		.in_num = 4,
+		.in_sizes = { 8, 8, 2, 6, },
+		.out_num = 1,
+		.out_sizes = { 4, },
+	},
+	[ND_CMD_ARS_STATUS] = {
+		.out_num = 2,
+		.out_sizes = { 4, UINT_MAX, },
+	},
+};
+
+const struct nd_cmd_desc *nd_cmd_bus_desc(int cmd)
+{
+	if (cmd < ARRAY_SIZE(__nd_cmd_bus_descs))
+		return &__nd_cmd_bus_descs[cmd];
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(nd_cmd_bus_desc);
+
+u32 nd_cmd_in_size(struct nvdimm *nvdimm, int cmd,
+		const struct nd_cmd_desc *desc, int idx, void *buf)
+{
+	if (idx >= desc->in_num)
+		return UINT_MAX;
+
+	if (desc->in_sizes[idx] < UINT_MAX)
+		return desc->in_sizes[idx];
+
+	if (nvdimm && cmd == ND_CMD_SET_CONFIG_DATA && idx == 2) {
+		struct nd_cmd_set_config_hdr *hdr = buf;
+
+		return hdr->in_length;
+	} else if (nvdimm && cmd == ND_CMD_VENDOR && idx == 2) {
+		struct nd_cmd_vendor_hdr *hdr = buf;
+
+		return hdr->in_length;
+	}
+
+	return UINT_MAX;
+}
+EXPORT_SYMBOL_GPL(nd_cmd_in_size);
+
+u32 nd_cmd_out_size(struct nvdimm *nvdimm, int cmd,
+		const struct nd_cmd_desc *desc, int idx, const u32 *in_field,
+		const u32 *out_field)
+{
+	if (idx >= desc->out_num)
+		return UINT_MAX;
+
+	if (desc->out_sizes[idx] < UINT_MAX)
+		return desc->out_sizes[idx];
+
+	if (nvdimm && cmd == ND_CMD_GET_CONFIG_DATA && idx == 1)
+		return in_field[1];
+	else if (nvdimm && cmd == ND_CMD_VENDOR && idx == 2)
+		return out_field[1];
+	else if (!nvdimm && cmd == ND_CMD_ARS_STATUS && idx == 1)
+		return ND_CMD_ARS_STATUS_MAX;
+
+	return UINT_MAX;
+}
+EXPORT_SYMBOL_GPL(nd_cmd_out_size);
+
+void wait_nvdimm_bus_probe_idle(struct device *dev)
+{
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+
+	do {
+		if (nvdimm_bus->probe_active == 0)
+			break;
+		nvdimm_bus_unlock(&nvdimm_bus->dev);
+		wait_event(nvdimm_bus->probe_wait,
+				nvdimm_bus->probe_active == 0);
+		nvdimm_bus_lock(&nvdimm_bus->dev);
+	} while (true);
+}
+
+/* set_config requires an idle interleave set */
+static int nd_cmd_clear_to_send(struct nvdimm *nvdimm, unsigned int cmd)
+{
+	struct nvdimm_bus *nvdimm_bus;
+
+	if (!nvdimm || cmd != ND_CMD_SET_CONFIG_DATA)
+		return 0;
+
+	nvdimm_bus = walk_to_nvdimm_bus(&nvdimm->dev);
+	wait_nvdimm_bus_probe_idle(&nvdimm_bus->dev);
+
+	if (atomic_read(&nvdimm->busy))
+		return -EBUSY;
+	return 0;
+}
+
+static int __nd_ioctl(struct nvdimm_bus *nvdimm_bus, struct nvdimm *nvdimm,
+		int read_only, unsigned int ioctl_cmd, unsigned long arg)
+{
+	struct nvdimm_bus_descriptor *nd_desc = nvdimm_bus->nd_desc;
+	size_t buf_len = 0, in_len = 0, out_len = 0;
+	static char out_env[ND_CMD_MAX_ENVELOPE];
+	static char in_env[ND_CMD_MAX_ENVELOPE];
+	const struct nd_cmd_desc *desc = NULL;
+	unsigned int cmd = _IOC_NR(ioctl_cmd);
+	void __user *p = (void __user *) arg;
+	struct device *dev = &nvdimm_bus->dev;
+	const char *cmd_name, *dimm_name;
+	unsigned long dsm_mask;
+	void *buf;
+	int rc, i;
+
+	if (nvdimm) {
+		desc = nd_cmd_dimm_desc(cmd);
+		cmd_name = nvdimm_cmd_name(cmd);
+		dsm_mask = nvdimm->dsm_mask ? *(nvdimm->dsm_mask) : 0;
+		dimm_name = dev_name(&nvdimm->dev);
+	} else {
+		desc = nd_cmd_bus_desc(cmd);
+		cmd_name = nvdimm_bus_cmd_name(cmd);
+		dsm_mask = nd_desc->dsm_mask;
+		dimm_name = "bus";
+	}
+
+	if (!desc || (desc->out_num + desc->in_num == 0) ||
+			!test_bit(cmd, &dsm_mask))
+		return -ENOTTY;
+
+	/* fail write commands (when read-only) */
+	if (read_only)
+		switch (ioctl_cmd) {
+		case ND_IOCTL_VENDOR:
+		case ND_IOCTL_SET_CONFIG_DATA:
+		case ND_IOCTL_ARS_START:
+			dev_dbg(&nvdimm_bus->dev, "'%s' command while read-only.\n",
+					nvdimm ? nvdimm_cmd_name(cmd)
+					: nvdimm_bus_cmd_name(cmd));
+			return -EPERM;
+		default:
+			break;
+		}
+
+	/* process an input envelope */
+	for (i = 0; i < desc->in_num; i++) {
+		u32 in_size, copy;
+
+		in_size = nd_cmd_in_size(nvdimm, cmd, desc, i, in_env);
+		if (in_size == UINT_MAX) {
+			dev_err(dev, "%s:%s unknown input size cmd: %s field: %d\n",
+					__func__, dimm_name, cmd_name, i);
+			return -ENXIO;
+		}
+		if (in_len < sizeof(in_env))
+			copy = min_t(u32, sizeof(in_env) - in_len, in_size);
+		else
+			copy = 0;
+		if (copy && copy_from_user(&in_env[in_len], p + in_len, copy))
+			return -EFAULT;
+		in_len += in_size;
+	}
+
+	/* process an output envelope */
+	for (i = 0; i < desc->out_num; i++) {
+		u32 out_size = nd_cmd_out_size(nvdimm, cmd, desc, i,
+				(u32 *) in_env, (u32 *) out_env);
+		u32 copy;
+
+		if (out_size == UINT_MAX) {
+			dev_dbg(dev, "%s:%s unknown output size cmd: %s field: %d\n",
+					__func__, dimm_name, cmd_name, i);
+			return -EFAULT;
+		}
+		if (out_len < sizeof(out_env))
+			copy = min_t(u32, sizeof(out_env) - out_len, out_size);
+		else
+			copy = 0;
+		if (copy && copy_from_user(&out_env[out_len],
+					p + in_len + out_len, copy))
+			return -EFAULT;
+		out_len += out_size;
+	}
+
+	buf_len = out_len + in_len;
+	if (buf_len > ND_IOCTL_MAX_BUFLEN) {
+		dev_dbg(dev, "%s:%s cmd: %s buf_len: %zu > %d\n", __func__,
+				dimm_name, cmd_name, buf_len,
+				ND_IOCTL_MAX_BUFLEN);
+		return -EINVAL;
+	}
+
+	buf = vmalloc(buf_len);
+	if (!buf)
+		return -ENOMEM;
+
+	if (copy_from_user(buf, p, buf_len)) {
+		rc = -EFAULT;
+		goto out;
+	}
+
+	nvdimm_bus_lock(&nvdimm_bus->dev);
+	rc = nd_cmd_clear_to_send(nvdimm, cmd);
+	if (rc)
+		goto out_unlock;
+
+	rc = nd_desc->ndctl(nd_desc, nvdimm, cmd, buf, buf_len);
+	if (rc < 0)
+		goto out_unlock;
+	if (copy_to_user(p, buf, buf_len))
+		rc = -EFAULT;
+ out_unlock:
+	nvdimm_bus_unlock(&nvdimm_bus->dev);
+ out:
+	vfree(buf);
+	return rc;
+}
+
+static long nd_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	long id = (long) file->private_data;
+	int rc = -ENXIO, read_only;
+	struct nvdimm_bus *nvdimm_bus;
+
+	read_only = (O_RDWR != (file->f_flags & O_ACCMODE));
+	mutex_lock(&nvdimm_bus_list_mutex);
+	list_for_each_entry(nvdimm_bus, &nvdimm_bus_list, list) {
+		if (nvdimm_bus->id == id) {
+			rc = __nd_ioctl(nvdimm_bus, NULL, read_only, cmd, arg);
+			break;
+		}
+	}
+	mutex_unlock(&nvdimm_bus_list_mutex);
+
+	return rc;
+}
+
+static int match_dimm(struct device *dev, void *data)
+{
+	long id = (long) data;
+
+	if (is_nvdimm(dev)) {
+		struct nvdimm *nvdimm = to_nvdimm(dev);
+
+		return nvdimm->id == id;
+	}
+
+	return 0;
+}
+
+static long nvdimm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	int rc = -ENXIO, read_only;
+	struct nvdimm_bus *nvdimm_bus;
+
+	read_only = (O_RDWR != (file->f_flags & O_ACCMODE));
+	mutex_lock(&nvdimm_bus_list_mutex);
+	list_for_each_entry(nvdimm_bus, &nvdimm_bus_list, list) {
+		struct device *dev = device_find_child(&nvdimm_bus->dev,
+				file->private_data, match_dimm);
+		struct nvdimm *nvdimm;
+
+		if (!dev)
+			continue;
+
+		nvdimm = to_nvdimm(dev);
+		rc = __nd_ioctl(nvdimm_bus, nvdimm, read_only, cmd, arg);
+		put_device(dev);
+		break;
+	}
+	mutex_unlock(&nvdimm_bus_list_mutex);
+
+	return rc;
+}
+
+static int nd_open(struct inode *inode, struct file *file)
+{
+	long minor = iminor(inode);
+
+	file->private_data = (void *) minor;
+	return 0;
+}
+
+static const struct file_operations nvdimm_bus_fops = {
+	.owner = THIS_MODULE,
+	.open = nd_open,
+	.unlocked_ioctl = nd_ioctl,
+	.compat_ioctl = nd_ioctl,
+	.llseek = noop_llseek,
+};
+
+static const struct file_operations nvdimm_fops = {
+	.owner = THIS_MODULE,
+	.open = nd_open,
+	.unlocked_ioctl = nvdimm_ioctl,
+	.compat_ioctl = nvdimm_ioctl,
+	.llseek = noop_llseek,
+};
+
+int __init nvdimm_bus_init(void)
+{
+	int rc;
+
+	rc = bus_register(&nvdimm_bus_type);
+	if (rc)
+		return rc;
+
+	rc = register_chrdev(0, "ndctl", &nvdimm_bus_fops);
+	if (rc < 0)
+		goto err_bus_chrdev;
+	nvdimm_bus_major = rc;
+
+	rc = register_chrdev(0, "dimmctl", &nvdimm_fops);
+	if (rc < 0)
+		goto err_dimm_chrdev;
+	nvdimm_major = rc;
+
+	nd_class = class_create(THIS_MODULE, "nd");
+	if (IS_ERR(nd_class)) {
+		rc = PTR_ERR(nd_class);
+		goto err_class;
+	}
+
+	return 0;
+
+ err_class:
+	unregister_chrdev(nvdimm_major, "dimmctl");
+ err_dimm_chrdev:
+	unregister_chrdev(nvdimm_bus_major, "ndctl");
+ err_bus_chrdev:
+	bus_unregister(&nvdimm_bus_type);
+
+	return rc;
+}
+
+void nvdimm_bus_exit(void)
+{
+	class_destroy(nd_class);
+	unregister_chrdev(nvdimm_bus_major, "ndctl");
+	unregister_chrdev(nvdimm_major, "dimmctl");
+	bus_unregister(&nvdimm_bus_type);
+}
diff --git a/drivers/nvdimm/core.c b/drivers/nvdimm/core.c
new file mode 100644
index 000000000000..cb62ec6a12d0
--- /dev/null
+++ b/drivers/nvdimm/core.c
@@ -0,0 +1,465 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#include <linux/libnvdimm.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/blkdev.h>
+#include <linux/device.h>
+#include <linux/ctype.h>
+#include <linux/ndctl.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include "nd-core.h"
+#include "nd.h"
+
+LIST_HEAD(nvdimm_bus_list);
+DEFINE_MUTEX(nvdimm_bus_list_mutex);
+static DEFINE_IDA(nd_ida);
+
+void nvdimm_bus_lock(struct device *dev)
+{
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+
+	if (!nvdimm_bus)
+		return;
+	mutex_lock(&nvdimm_bus->reconfig_mutex);
+}
+EXPORT_SYMBOL(nvdimm_bus_lock);
+
+void nvdimm_bus_unlock(struct device *dev)
+{
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+
+	if (!nvdimm_bus)
+		return;
+	mutex_unlock(&nvdimm_bus->reconfig_mutex);
+}
+EXPORT_SYMBOL(nvdimm_bus_unlock);
+
+bool is_nvdimm_bus_locked(struct device *dev)
+{
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+
+	if (!nvdimm_bus)
+		return false;
+	return mutex_is_locked(&nvdimm_bus->reconfig_mutex);
+}
+EXPORT_SYMBOL(is_nvdimm_bus_locked);
+
+u64 nd_fletcher64(void *addr, size_t len, bool le)
+{
+	u32 *buf = addr;
+	u32 lo32 = 0;
+	u64 hi32 = 0;
+	int i;
+
+	for (i = 0; i < len / sizeof(u32); i++) {
+		lo32 += le ? le32_to_cpu((__le32) buf[i]) : buf[i];
+		hi32 += lo32;
+	}
+
+	return hi32 << 32 | lo32;
+}
+EXPORT_SYMBOL_GPL(nd_fletcher64);
+
+static void nvdimm_bus_release(struct device *dev)
+{
+	struct nvdimm_bus *nvdimm_bus;
+
+	nvdimm_bus = container_of(dev, struct nvdimm_bus, dev);
+	ida_simple_remove(&nd_ida, nvdimm_bus->id);
+	kfree(nvdimm_bus);
+}
+
+struct nvdimm_bus *to_nvdimm_bus(struct device *dev)
+{
+	struct nvdimm_bus *nvdimm_bus;
+
+	nvdimm_bus = container_of(dev, struct nvdimm_bus, dev);
+	WARN_ON(nvdimm_bus->dev.release != nvdimm_bus_release);
+	return nvdimm_bus;
+}
+EXPORT_SYMBOL_GPL(to_nvdimm_bus);
+
+struct nvdimm_bus_descriptor *to_nd_desc(struct nvdimm_bus *nvdimm_bus)
+{
+	/* struct nvdimm_bus definition is private to libnvdimm */
+	return nvdimm_bus->nd_desc;
+}
+EXPORT_SYMBOL_GPL(to_nd_desc);
+
+struct nvdimm_bus *walk_to_nvdimm_bus(struct device *nd_dev)
+{
+	struct device *dev;
+
+	for (dev = nd_dev; dev; dev = dev->parent)
+		if (dev->release == nvdimm_bus_release)
+			break;
+	dev_WARN_ONCE(nd_dev, !dev, "invalid dev, not on nd bus\n");
+	if (dev)
+		return to_nvdimm_bus(dev);
+	return NULL;
+}
+
+static bool is_uuid_sep(char sep)
+{
+	if (sep == '\n' || sep == '-' || sep == ':' || sep == '\0')
+		return true;
+	return false;
+}
+
+static int nd_uuid_parse(struct device *dev, u8 *uuid_out, const char *buf,
+		size_t len)
+{
+	const char *str = buf;
+	u8 uuid[16];
+	int i;
+
+	for (i = 0; i < 16; i++) {
+		if (!isxdigit(str[0]) || !isxdigit(str[1])) {
+			dev_dbg(dev, "%s: pos: %d buf[%zd]: %c buf[%zd]: %c\n",
+					__func__, i, str - buf, str[0],
+					str + 1 - buf, str[1]);
+			return -EINVAL;
+		}
+
+		uuid[i] = (hex_to_bin(str[0]) << 4) | hex_to_bin(str[1]);
+		str += 2;
+		if (is_uuid_sep(*str))
+			str++;
+	}
+
+	memcpy(uuid_out, uuid, sizeof(uuid));
+	return 0;
+}
+
+/**
+ * nd_uuid_store: common implementation for writing 'uuid' sysfs attributes
+ * @dev: container device for the uuid property
+ * @uuid_out: uuid buffer to replace
+ * @buf: raw sysfs buffer to parse
+ *
+ * Enforce that uuids can only be changed while the device is disabled
+ * (driver detached)
+ * LOCKING: expects device_lock() is held on entry
+ */
+int nd_uuid_store(struct device *dev, u8 **uuid_out, const char *buf,
+		size_t len)
+{
+	u8 uuid[16];
+	int rc;
+
+	if (dev->driver)
+		return -EBUSY;
+
+	rc = nd_uuid_parse(dev, uuid, buf, len);
+	if (rc)
+		return rc;
+
+	kfree(*uuid_out);
+	*uuid_out = kmemdup(uuid, sizeof(uuid), GFP_KERNEL);
+	if (!(*uuid_out))
+		return -ENOMEM;
+
+	return 0;
+}
+
+ssize_t nd_sector_size_show(unsigned long current_lbasize,
+		const unsigned long *supported, char *buf)
+{
+	ssize_t len = 0;
+	int i;
+
+	for (i = 0; supported[i]; i++)
+		if (current_lbasize == supported[i])
+			len += sprintf(buf + len, "[%ld] ", supported[i]);
+		else
+			len += sprintf(buf + len, "%ld ", supported[i]);
+	len += sprintf(buf + len, "\n");
+	return len;
+}
+
+ssize_t nd_sector_size_store(struct device *dev, const char *buf,
+		unsigned long *current_lbasize, const unsigned long *supported)
+{
+	unsigned long lbasize;
+	int rc, i;
+
+	if (dev->driver)
+		return -EBUSY;
+
+	rc = kstrtoul(buf, 0, &lbasize);
+	if (rc)
+		return rc;
+
+	for (i = 0; supported[i]; i++)
+		if (lbasize == supported[i])
+			break;
+
+	if (supported[i]) {
+		*current_lbasize = lbasize;
+		return 0;
+	} else {
+		return -EINVAL;
+	}
+}
+
+void __nd_iostat_start(struct bio *bio, unsigned long *start)
+{
+	struct gendisk *disk = bio->bi_bdev->bd_disk;
+	const int rw = bio_data_dir(bio);
+	int cpu = part_stat_lock();
+
+	*start = jiffies;
+	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();
+}
+EXPORT_SYMBOL(__nd_iostat_start);
+
+void nd_iostat_end(struct bio *bio, unsigned long start)
+{
+	struct gendisk *disk = bio->bi_bdev->bd_disk;
+	unsigned long duration = jiffies - start;
+	const int rw = bio_data_dir(bio);
+	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();
+}
+EXPORT_SYMBOL(nd_iostat_end);
+
+static ssize_t commands_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int cmd, len = 0;
+	struct nvdimm_bus *nvdimm_bus = to_nvdimm_bus(dev);
+	struct nvdimm_bus_descriptor *nd_desc = nvdimm_bus->nd_desc;
+
+	for_each_set_bit(cmd, &nd_desc->dsm_mask, BITS_PER_LONG)
+		len += sprintf(buf + len, "%s ", nvdimm_bus_cmd_name(cmd));
+	len += sprintf(buf + len, "\n");
+	return len;
+}
+static DEVICE_ATTR_RO(commands);
+
+static const char *nvdimm_bus_provider(struct nvdimm_bus *nvdimm_bus)
+{
+	struct nvdimm_bus_descriptor *nd_desc = nvdimm_bus->nd_desc;
+	struct device *parent = nvdimm_bus->dev.parent;
+
+	if (nd_desc->provider_name)
+		return nd_desc->provider_name;
+	else if (parent)
+		return dev_name(parent);
+	else
+		return "unknown";
+}
+
+static ssize_t provider_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nvdimm_bus *nvdimm_bus = to_nvdimm_bus(dev);
+
+	return sprintf(buf, "%s\n", nvdimm_bus_provider(nvdimm_bus));
+}
+static DEVICE_ATTR_RO(provider);
+
+static int flush_namespaces(struct device *dev, void *data)
+{
+	device_lock(dev);
+	device_unlock(dev);
+	return 0;
+}
+
+static int flush_regions_dimms(struct device *dev, void *data)
+{
+	device_lock(dev);
+	device_unlock(dev);
+	device_for_each_child(dev, NULL, flush_namespaces);
+	return 0;
+}
+
+static ssize_t wait_probe_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	nd_synchronize();
+	device_for_each_child(dev, NULL, flush_regions_dimms);
+	return sprintf(buf, "1\n");
+}
+static DEVICE_ATTR_RO(wait_probe);
+
+static struct attribute *nvdimm_bus_attributes[] = {
+	&dev_attr_commands.attr,
+	&dev_attr_wait_probe.attr,
+	&dev_attr_provider.attr,
+	NULL,
+};
+
+struct attribute_group nvdimm_bus_attribute_group = {
+	.attrs = nvdimm_bus_attributes,
+};
+EXPORT_SYMBOL_GPL(nvdimm_bus_attribute_group);
+
+struct nvdimm_bus *__nvdimm_bus_register(struct device *parent,
+		struct nvdimm_bus_descriptor *nd_desc, struct module *module)
+{
+	struct nvdimm_bus *nvdimm_bus;
+	int rc;
+
+	nvdimm_bus = kzalloc(sizeof(*nvdimm_bus), GFP_KERNEL);
+	if (!nvdimm_bus)
+		return NULL;
+	INIT_LIST_HEAD(&nvdimm_bus->list);
+	init_waitqueue_head(&nvdimm_bus->probe_wait);
+	nvdimm_bus->id = ida_simple_get(&nd_ida, 0, 0, GFP_KERNEL);
+	mutex_init(&nvdimm_bus->reconfig_mutex);
+	if (nvdimm_bus->id < 0) {
+		kfree(nvdimm_bus);
+		return NULL;
+	}
+	nvdimm_bus->nd_desc = nd_desc;
+	nvdimm_bus->module = module;
+	nvdimm_bus->dev.parent = parent;
+	nvdimm_bus->dev.release = nvdimm_bus_release;
+	nvdimm_bus->dev.groups = nd_desc->attr_groups;
+	dev_set_name(&nvdimm_bus->dev, "ndbus%d", nvdimm_bus->id);
+	rc = device_register(&nvdimm_bus->dev);
+	if (rc) {
+		dev_dbg(&nvdimm_bus->dev, "registration failed: %d\n", rc);
+		goto err;
+	}
+
+	rc = nvdimm_bus_create_ndctl(nvdimm_bus);
+	if (rc)
+		goto err;
+
+	mutex_lock(&nvdimm_bus_list_mutex);
+	list_add_tail(&nvdimm_bus->list, &nvdimm_bus_list);
+	mutex_unlock(&nvdimm_bus_list_mutex);
+
+	return nvdimm_bus;
+ err:
+	put_device(&nvdimm_bus->dev);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(__nvdimm_bus_register);
+
+static int child_unregister(struct device *dev, void *data)
+{
+	/*
+	 * the singular ndctl class device per bus needs to be
+	 * "device_destroy"ed, so skip it here
+	 *
+	 * i.e. remove classless children
+	 */
+	if (dev->class)
+		/* pass */;
+	else
+		nd_device_unregister(dev, ND_SYNC);
+	return 0;
+}
+
+void nvdimm_bus_unregister(struct nvdimm_bus *nvdimm_bus)
+{
+	if (!nvdimm_bus)
+		return;
+
+	mutex_lock(&nvdimm_bus_list_mutex);
+	list_del_init(&nvdimm_bus->list);
+	mutex_unlock(&nvdimm_bus_list_mutex);
+
+	nd_synchronize();
+	device_for_each_child(&nvdimm_bus->dev, NULL, child_unregister);
+	nvdimm_bus_destroy_ndctl(nvdimm_bus);
+
+	device_unregister(&nvdimm_bus->dev);
+}
+EXPORT_SYMBOL_GPL(nvdimm_bus_unregister);
+
+#ifdef CONFIG_BLK_DEV_INTEGRITY
+static int nd_pi_nop_generate_verify(struct blk_integrity_iter *iter)
+{
+	return 0;
+}
+
+int nd_integrity_init(struct gendisk *disk, unsigned long meta_size)
+{
+	struct blk_integrity integrity = {
+		.name = "ND-PI-NOP",
+		.generate_fn = nd_pi_nop_generate_verify,
+		.verify_fn = nd_pi_nop_generate_verify,
+		.tuple_size = meta_size,
+		.tag_size = meta_size,
+	};
+	int ret;
+
+	if (meta_size == 0)
+		return 0;
+
+	ret = blk_integrity_register(disk, &integrity);
+	if (ret)
+		return ret;
+
+	blk_queue_max_integrity_segments(disk->queue, 1);
+
+	return 0;
+}
+EXPORT_SYMBOL(nd_integrity_init);
+
+#else /* CONFIG_BLK_DEV_INTEGRITY */
+int nd_integrity_init(struct gendisk *disk, unsigned long meta_size)
+{
+	return 0;
+}
+EXPORT_SYMBOL(nd_integrity_init);
+
+#endif
+
+static __init int libnvdimm_init(void)
+{
+	int rc;
+
+	rc = nvdimm_bus_init();
+	if (rc)
+		return rc;
+	rc = nvdimm_init();
+	if (rc)
+		goto err_dimm;
+	rc = nd_region_init();
+	if (rc)
+		goto err_region;
+	return 0;
+ err_region:
+	nvdimm_exit();
+ err_dimm:
+	nvdimm_bus_exit();
+	return rc;
+}
+
+static __exit void libnvdimm_exit(void)
+{
+	WARN_ON(!list_empty(&nvdimm_bus_list));
+	nd_region_exit();
+	nvdimm_exit();
+	nvdimm_bus_exit();
+}
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Intel Corporation");
+subsys_initcall(libnvdimm_init);
+module_exit(libnvdimm_exit);
diff --git a/drivers/nvdimm/dimm.c b/drivers/nvdimm/dimm.c
new file mode 100644
index 000000000000..71d12bb67339
--- /dev/null
+++ b/drivers/nvdimm/dimm.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/sizes.h>
+#include <linux/ndctl.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/nd.h>
+#include "label.h"
+#include "nd.h"
+
+static int nvdimm_probe(struct device *dev)
+{
+	struct nvdimm_drvdata *ndd;
+	int rc;
+
+	ndd = kzalloc(sizeof(*ndd), GFP_KERNEL);
+	if (!ndd)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, ndd);
+	ndd->dpa.name = dev_name(dev);
+	ndd->ns_current = -1;
+	ndd->ns_next = -1;
+	ndd->dpa.start = 0;
+	ndd->dpa.end = -1;
+	ndd->dev = dev;
+	get_device(dev);
+	kref_init(&ndd->kref);
+
+	rc = nvdimm_init_nsarea(ndd);
+	if (rc)
+		goto err;
+
+	rc = nvdimm_init_config_data(ndd);
+	if (rc)
+		goto err;
+
+	dev_dbg(dev, "config data size: %d\n", ndd->nsarea.config_size);
+
+	nvdimm_bus_lock(dev);
+	ndd->ns_current = nd_label_validate(ndd);
+	ndd->ns_next = nd_label_next_nsindex(ndd->ns_current);
+	nd_label_copy(ndd, to_next_namespace_index(ndd),
+			to_current_namespace_index(ndd));
+	rc = nd_label_reserve_dpa(ndd);
+	nvdimm_bus_unlock(dev);
+
+	if (rc)
+		goto err;
+
+	return 0;
+
+ err:
+	put_ndd(ndd);
+	return rc;
+}
+
+static int nvdimm_remove(struct device *dev)
+{
+	struct nvdimm_drvdata *ndd = dev_get_drvdata(dev);
+
+	nvdimm_bus_lock(dev);
+	dev_set_drvdata(dev, NULL);
+	nvdimm_bus_unlock(dev);
+	put_ndd(ndd);
+
+	return 0;
+}
+
+static struct nd_device_driver nvdimm_driver = {
+	.probe = nvdimm_probe,
+	.remove = nvdimm_remove,
+	.drv = {
+		.name = "nvdimm",
+	},
+	.type = ND_DRIVER_DIMM,
+};
+
+int __init nvdimm_init(void)
+{
+	return nd_driver_register(&nvdimm_driver);
+}
+
+void nvdimm_exit(void)
+{
+	driver_unregister(&nvdimm_driver.drv);
+}
+
+MODULE_ALIAS_ND_DEVICE(ND_DEVICE_DIMM);
diff --git a/drivers/nvdimm/dimm_devs.c b/drivers/nvdimm/dimm_devs.c
new file mode 100644
index 000000000000..c05eb807d674
--- /dev/null
+++ b/drivers/nvdimm/dimm_devs.c
@@ -0,0 +1,551 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+#include <linux/vmalloc.h>
+#include <linux/device.h>
+#include <linux/ndctl.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include "nd-core.h"
+#include "label.h"
+#include "nd.h"
+
+static DEFINE_IDA(dimm_ida);
+
+/*
+ * Retrieve bus and dimm handle and return if this bus supports
+ * get_config_data commands
+ */
+static int __validate_dimm(struct nvdimm_drvdata *ndd)
+{
+	struct nvdimm *nvdimm;
+
+	if (!ndd)
+		return -EINVAL;
+
+	nvdimm = to_nvdimm(ndd->dev);
+
+	if (!nvdimm->dsm_mask)
+		return -ENXIO;
+	if (!test_bit(ND_CMD_GET_CONFIG_DATA, nvdimm->dsm_mask))
+		return -ENXIO;
+
+	return 0;
+}
+
+static int validate_dimm(struct nvdimm_drvdata *ndd)
+{
+	int rc = __validate_dimm(ndd);
+
+	if (rc && ndd)
+		dev_dbg(ndd->dev, "%pf: %s error: %d\n",
+				__builtin_return_address(0), __func__, rc);
+	return rc;
+}
+
+/**
+ * nvdimm_init_nsarea - determine the geometry of a dimm's namespace area
+ * @nvdimm: dimm to initialize
+ */
+int nvdimm_init_nsarea(struct nvdimm_drvdata *ndd)
+{
+	struct nd_cmd_get_config_size *cmd = &ndd->nsarea;
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(ndd->dev);
+	struct nvdimm_bus_descriptor *nd_desc;
+	int rc = validate_dimm(ndd);
+
+	if (rc)
+		return rc;
+
+	if (cmd->config_size)
+		return 0; /* already valid */
+
+	memset(cmd, 0, sizeof(*cmd));
+	nd_desc = nvdimm_bus->nd_desc;
+	return nd_desc->ndctl(nd_desc, to_nvdimm(ndd->dev),
+			ND_CMD_GET_CONFIG_SIZE, cmd, sizeof(*cmd));
+}
+
+int nvdimm_init_config_data(struct nvdimm_drvdata *ndd)
+{
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(ndd->dev);
+	struct nd_cmd_get_config_data_hdr *cmd;
+	struct nvdimm_bus_descriptor *nd_desc;
+	int rc = validate_dimm(ndd);
+	u32 max_cmd_size, config_size;
+	size_t offset;
+
+	if (rc)
+		return rc;
+
+	if (ndd->data)
+		return 0;
+
+	if (ndd->nsarea.status || ndd->nsarea.max_xfer == 0
+			|| ndd->nsarea.config_size < ND_LABEL_MIN_SIZE) {
+		dev_dbg(ndd->dev, "failed to init config data area: (%d:%d)\n",
+				ndd->nsarea.max_xfer, ndd->nsarea.config_size);
+		return -ENXIO;
+	}
+
+	ndd->data = kmalloc(ndd->nsarea.config_size, GFP_KERNEL);
+	if (!ndd->data)
+		ndd->data = vmalloc(ndd->nsarea.config_size);
+
+	if (!ndd->data)
+		return -ENOMEM;
+
+	max_cmd_size = min_t(u32, PAGE_SIZE, ndd->nsarea.max_xfer);
+	cmd = kzalloc(max_cmd_size + sizeof(*cmd), GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	nd_desc = nvdimm_bus->nd_desc;
+	for (config_size = ndd->nsarea.config_size, offset = 0;
+			config_size; config_size -= cmd->in_length,
+			offset += cmd->in_length) {
+		cmd->in_length = min(config_size, max_cmd_size);
+		cmd->in_offset = offset;
+		rc = nd_desc->ndctl(nd_desc, to_nvdimm(ndd->dev),
+				ND_CMD_GET_CONFIG_DATA, cmd,
+				cmd->in_length + sizeof(*cmd));
+		if (rc || cmd->status) {
+			rc = -ENXIO;
+			break;
+		}
+		memcpy(ndd->data + offset, cmd->out_buf, cmd->in_length);
+	}
+	dev_dbg(ndd->dev, "%s: len: %zu rc: %d\n", __func__, offset, rc);
+	kfree(cmd);
+
+	return rc;
+}
+
+int nvdimm_set_config_data(struct nvdimm_drvdata *ndd, size_t offset,
+		void *buf, size_t len)
+{
+	int rc = validate_dimm(ndd);
+	size_t max_cmd_size, buf_offset;
+	struct nd_cmd_set_config_hdr *cmd;
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(ndd->dev);
+	struct nvdimm_bus_descriptor *nd_desc = nvdimm_bus->nd_desc;
+
+	if (rc)
+		return rc;
+
+	if (!ndd->data)
+		return -ENXIO;
+
+	if (offset + len > ndd->nsarea.config_size)
+		return -ENXIO;
+
+	max_cmd_size = min_t(u32, PAGE_SIZE, len);
+	max_cmd_size = min_t(u32, max_cmd_size, ndd->nsarea.max_xfer);
+	cmd = kzalloc(max_cmd_size + sizeof(*cmd) + sizeof(u32), GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	for (buf_offset = 0; len; len -= cmd->in_length,
+			buf_offset += cmd->in_length) {
+		size_t cmd_size;
+		u32 *status;
+
+		cmd->in_offset = offset + buf_offset;
+		cmd->in_length = min(max_cmd_size, len);
+		memcpy(cmd->in_buf, buf + buf_offset, cmd->in_length);
+
+		/* status is output in the last 4-bytes of the command buffer */
+		cmd_size = sizeof(*cmd) + cmd->in_length + sizeof(u32);
+		status = ((void *) cmd) + cmd_size - sizeof(u32);
+
+		rc = nd_desc->ndctl(nd_desc, to_nvdimm(ndd->dev),
+				ND_CMD_SET_CONFIG_DATA, cmd, cmd_size);
+		if (rc || *status) {
+			rc = rc ? rc : -ENXIO;
+			break;
+		}
+	}
+	kfree(cmd);
+
+	return rc;
+}
+
+static void nvdimm_release(struct device *dev)
+{
+	struct nvdimm *nvdimm = to_nvdimm(dev);
+
+	ida_simple_remove(&dimm_ida, nvdimm->id);
+	kfree(nvdimm);
+}
+
+static struct device_type nvdimm_device_type = {
+	.name = "nvdimm",
+	.release = nvdimm_release,
+};
+
+bool is_nvdimm(struct device *dev)
+{
+	return dev->type == &nvdimm_device_type;
+}
+
+struct nvdimm *to_nvdimm(struct device *dev)
+{
+	struct nvdimm *nvdimm = container_of(dev, struct nvdimm, dev);
+
+	WARN_ON(!is_nvdimm(dev));
+	return nvdimm;
+}
+EXPORT_SYMBOL_GPL(to_nvdimm);
+
+struct nvdimm *nd_blk_region_to_dimm(struct nd_blk_region *ndbr)
+{
+	struct nd_region *nd_region = &ndbr->nd_region;
+	struct nd_mapping *nd_mapping = &nd_region->mapping[0];
+
+	return nd_mapping->nvdimm;
+}
+EXPORT_SYMBOL_GPL(nd_blk_region_to_dimm);
+
+struct nvdimm_drvdata *to_ndd(struct nd_mapping *nd_mapping)
+{
+	struct nvdimm *nvdimm = nd_mapping->nvdimm;
+
+	WARN_ON_ONCE(!is_nvdimm_bus_locked(&nvdimm->dev));
+
+	return dev_get_drvdata(&nvdimm->dev);
+}
+EXPORT_SYMBOL(to_ndd);
+
+void nvdimm_drvdata_release(struct kref *kref)
+{
+	struct nvdimm_drvdata *ndd = container_of(kref, typeof(*ndd), kref);
+	struct device *dev = ndd->dev;
+	struct resource *res, *_r;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	nvdimm_bus_lock(dev);
+	for_each_dpa_resource_safe(ndd, res, _r)
+		nvdimm_free_dpa(ndd, res);
+	nvdimm_bus_unlock(dev);
+
+	if (ndd->data && is_vmalloc_addr(ndd->data))
+		vfree(ndd->data);
+	else
+		kfree(ndd->data);
+	kfree(ndd);
+	put_device(dev);
+}
+
+void get_ndd(struct nvdimm_drvdata *ndd)
+{
+	kref_get(&ndd->kref);
+}
+
+void put_ndd(struct nvdimm_drvdata *ndd)
+{
+	if (ndd)
+		kref_put(&ndd->kref, nvdimm_drvdata_release);
+}
+
+const char *nvdimm_name(struct nvdimm *nvdimm)
+{
+	return dev_name(&nvdimm->dev);
+}
+EXPORT_SYMBOL_GPL(nvdimm_name);
+
+void *nvdimm_provider_data(struct nvdimm *nvdimm)
+{
+	if (nvdimm)
+		return nvdimm->provider_data;
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(nvdimm_provider_data);
+
+static ssize_t commands_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nvdimm *nvdimm = to_nvdimm(dev);
+	int cmd, len = 0;
+
+	if (!nvdimm->dsm_mask)
+		return sprintf(buf, "\n");
+
+	for_each_set_bit(cmd, nvdimm->dsm_mask, BITS_PER_LONG)
+		len += sprintf(buf + len, "%s ", nvdimm_cmd_name(cmd));
+	len += sprintf(buf + len, "\n");
+	return len;
+}
+static DEVICE_ATTR_RO(commands);
+
+static ssize_t state_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct nvdimm *nvdimm = to_nvdimm(dev);
+
+	/*
+	 * The state may be in the process of changing, userspace should
+	 * quiesce probing if it wants a static answer
+	 */
+	nvdimm_bus_lock(dev);
+	nvdimm_bus_unlock(dev);
+	return sprintf(buf, "%s\n", atomic_read(&nvdimm->busy)
+			? "active" : "idle");
+}
+static DEVICE_ATTR_RO(state);
+
+static ssize_t available_slots_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nvdimm_drvdata *ndd = dev_get_drvdata(dev);
+	ssize_t rc;
+	u32 nfree;
+
+	if (!ndd)
+		return -ENXIO;
+
+	nvdimm_bus_lock(dev);
+	nfree = nd_label_nfree(ndd);
+	if (nfree - 1 > nfree) {
+		dev_WARN_ONCE(dev, 1, "we ate our last label?\n");
+		nfree = 0;
+	} else
+		nfree--;
+	rc = sprintf(buf, "%d\n", nfree);
+	nvdimm_bus_unlock(dev);
+	return rc;
+}
+static DEVICE_ATTR_RO(available_slots);
+
+static struct attribute *nvdimm_attributes[] = {
+	&dev_attr_state.attr,
+	&dev_attr_commands.attr,
+	&dev_attr_available_slots.attr,
+	NULL,
+};
+
+struct attribute_group nvdimm_attribute_group = {
+	.attrs = nvdimm_attributes,
+};
+EXPORT_SYMBOL_GPL(nvdimm_attribute_group);
+
+struct nvdimm *nvdimm_create(struct nvdimm_bus *nvdimm_bus, void *provider_data,
+		const struct attribute_group **groups, unsigned long flags,
+		unsigned long *dsm_mask)
+{
+	struct nvdimm *nvdimm = kzalloc(sizeof(*nvdimm), GFP_KERNEL);
+	struct device *dev;
+
+	if (!nvdimm)
+		return NULL;
+
+	nvdimm->id = ida_simple_get(&dimm_ida, 0, 0, GFP_KERNEL);
+	if (nvdimm->id < 0) {
+		kfree(nvdimm);
+		return NULL;
+	}
+	nvdimm->provider_data = provider_data;
+	nvdimm->flags = flags;
+	nvdimm->dsm_mask = dsm_mask;
+	atomic_set(&nvdimm->busy, 0);
+	dev = &nvdimm->dev;
+	dev_set_name(dev, "nmem%d", nvdimm->id);
+	dev->parent = &nvdimm_bus->dev;
+	dev->type = &nvdimm_device_type;
+	dev->devt = MKDEV(nvdimm_major, nvdimm->id);
+	dev->groups = groups;
+	nd_device_register(dev);
+
+	return nvdimm;
+}
+EXPORT_SYMBOL_GPL(nvdimm_create);
+
+/**
+ * nd_blk_available_dpa - account the unused dpa of BLK region
+ * @nd_mapping: container of dpa-resource-root + labels
+ *
+ * Unlike PMEM, BLK namespaces can occupy discontiguous DPA ranges.
+ */
+resource_size_t nd_blk_available_dpa(struct nd_mapping *nd_mapping)
+{
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	resource_size_t map_end, busy = 0, available;
+	struct resource *res;
+
+	if (!ndd)
+		return 0;
+
+	map_end = nd_mapping->start + nd_mapping->size - 1;
+	for_each_dpa_resource(ndd, res)
+		if (res->start >= nd_mapping->start && res->start < map_end) {
+			resource_size_t end = min(map_end, res->end);
+
+			busy += end - res->start + 1;
+		} else if (res->end >= nd_mapping->start
+				&& res->end <= map_end) {
+			busy += res->end - nd_mapping->start;
+		} else if (nd_mapping->start > res->start
+				&& nd_mapping->start < res->end) {
+			/* total eclipse of the BLK region mapping */
+			busy += nd_mapping->size;
+		}
+
+	available = map_end - nd_mapping->start + 1;
+	if (busy < available)
+		return available - busy;
+	return 0;
+}
+
+/**
+ * nd_pmem_available_dpa - for the given dimm+region account unallocated dpa
+ * @nd_mapping: container of dpa-resource-root + labels
+ * @nd_region: constrain available space check to this reference region
+ * @overlap: calculate available space assuming this level of overlap
+ *
+ * Validate that a PMEM label, if present, aligns with the start of an
+ * interleave set and truncate the available size at the lowest BLK
+ * overlap point.
+ *
+ * The expectation is that this routine is called multiple times as it
+ * probes for the largest BLK encroachment for any single member DIMM of
+ * the interleave set.  Once that value is determined the PMEM-limit for
+ * the set can be established.
+ */
+resource_size_t nd_pmem_available_dpa(struct nd_region *nd_region,
+		struct nd_mapping *nd_mapping, resource_size_t *overlap)
+{
+	resource_size_t map_start, map_end, busy = 0, available, blk_start;
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	struct resource *res;
+	const char *reason;
+
+	if (!ndd)
+		return 0;
+
+	map_start = nd_mapping->start;
+	map_end = map_start + nd_mapping->size - 1;
+	blk_start = max(map_start, map_end + 1 - *overlap);
+	for_each_dpa_resource(ndd, res)
+		if (res->start >= map_start && res->start < map_end) {
+			if (strncmp(res->name, "blk", 3) == 0)
+				blk_start = min(blk_start, res->start);
+			else if (res->start != map_start) {
+				reason = "misaligned to iset";
+				goto err;
+			} else {
+				if (busy) {
+					reason = "duplicate overlapping PMEM reservations?";
+					goto err;
+				}
+				busy += resource_size(res);
+				continue;
+			}
+		} else if (res->end >= map_start && res->end <= map_end) {
+			if (strncmp(res->name, "blk", 3) == 0) {
+				/*
+				 * If a BLK allocation overlaps the start of
+				 * PMEM the entire interleave set may now only
+				 * be used for BLK.
+				 */
+				blk_start = map_start;
+			} else {
+				reason = "misaligned to iset";
+				goto err;
+			}
+		} else if (map_start > res->start && map_start < res->end) {
+			/* total eclipse of the mapping */
+			busy += nd_mapping->size;
+			blk_start = map_start;
+		}
+
+	*overlap = map_end + 1 - blk_start;
+	available = blk_start - map_start;
+	if (busy < available)
+		return available - busy;
+	return 0;
+
+ err:
+	/*
+	 * Something is wrong, PMEM must align with the start of the
+	 * interleave set, and there can only be one allocation per set.
+	 */
+	nd_dbg_dpa(nd_region, ndd, res, "%s\n", reason);
+	return 0;
+}
+
+void nvdimm_free_dpa(struct nvdimm_drvdata *ndd, struct resource *res)
+{
+	WARN_ON_ONCE(!is_nvdimm_bus_locked(ndd->dev));
+	kfree(res->name);
+	__release_region(&ndd->dpa, res->start, resource_size(res));
+}
+
+struct resource *nvdimm_allocate_dpa(struct nvdimm_drvdata *ndd,
+		struct nd_label_id *label_id, resource_size_t start,
+		resource_size_t n)
+{
+	char *name = kmemdup(label_id, sizeof(*label_id), GFP_KERNEL);
+	struct resource *res;
+
+	if (!name)
+		return NULL;
+
+	WARN_ON_ONCE(!is_nvdimm_bus_locked(ndd->dev));
+	res = __request_region(&ndd->dpa, start, n, name, 0);
+	if (!res)
+		kfree(name);
+	return res;
+}
+
+/**
+ * nvdimm_allocated_dpa - sum up the dpa currently allocated to this label_id
+ * @nvdimm: container of dpa-resource-root + labels
+ * @label_id: dpa resource name of the form {pmem|blk}-<human readable uuid>
+ */
+resource_size_t nvdimm_allocated_dpa(struct nvdimm_drvdata *ndd,
+		struct nd_label_id *label_id)
+{
+	resource_size_t allocated = 0;
+	struct resource *res;
+
+	for_each_dpa_resource(ndd, res)
+		if (strcmp(res->name, label_id->id) == 0)
+			allocated += resource_size(res);
+
+	return allocated;
+}
+
+static int count_dimms(struct device *dev, void *c)
+{
+	int *count = c;
+
+	if (is_nvdimm(dev))
+		(*count)++;
+	return 0;
+}
+
+int nvdimm_bus_check_dimm_count(struct nvdimm_bus *nvdimm_bus, int dimm_count)
+{
+	int count = 0;
+	/* Flush any possible dimm registration failures */
+	nd_synchronize();
+
+	device_for_each_child(&nvdimm_bus->dev, &count, count_dimms);
+	dev_dbg(&nvdimm_bus->dev, "%s: count: %d\n", __func__, count);
+	if (count != dimm_count)
+		return -ENXIO;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(nvdimm_bus_check_dimm_count);
diff --git a/drivers/nvdimm/label.c b/drivers/nvdimm/label.c
new file mode 100644
index 000000000000..96526dcfdd37
--- /dev/null
+++ b/drivers/nvdimm/label.c
@@ -0,0 +1,927 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#include <linux/device.h>
+#include <linux/ndctl.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/nd.h>
+#include "nd-core.h"
+#include "label.h"
+#include "nd.h"
+
+static u32 best_seq(u32 a, u32 b)
+{
+	a &= NSINDEX_SEQ_MASK;
+	b &= NSINDEX_SEQ_MASK;
+
+	if (a == 0 || a == b)
+		return b;
+	else if (b == 0)
+		return a;
+	else if (nd_inc_seq(a) == b)
+		return b;
+	else
+		return a;
+}
+
+size_t sizeof_namespace_index(struct nvdimm_drvdata *ndd)
+{
+	u32 index_span;
+
+	if (ndd->nsindex_size)
+		return ndd->nsindex_size;
+
+	/*
+	 * The minimum index space is 512 bytes, with that amount of
+	 * index we can describe ~1400 labels which is less than a byte
+	 * of overhead per label.  Round up to a byte of overhead per
+	 * label and determine the size of the index region.  Yes, this
+	 * starts to waste space at larger config_sizes, but it's
+	 * unlikely we'll ever see anything but 128K.
+	 */
+	index_span = ndd->nsarea.config_size / 129;
+	index_span /= NSINDEX_ALIGN * 2;
+	ndd->nsindex_size = index_span * NSINDEX_ALIGN;
+
+	return ndd->nsindex_size;
+}
+
+int nvdimm_num_label_slots(struct nvdimm_drvdata *ndd)
+{
+	return ndd->nsarea.config_size / 129;
+}
+
+int nd_label_validate(struct nvdimm_drvdata *ndd)
+{
+	/*
+	 * On media label format consists of two index blocks followed
+	 * by an array of labels.  None of these structures are ever
+	 * updated in place.  A sequence number tracks the current
+	 * active index and the next one to write, while labels are
+	 * written to free slots.
+	 *
+	 *     +------------+
+	 *     |            |
+	 *     |  nsindex0  |
+	 *     |            |
+	 *     +------------+
+	 *     |            |
+	 *     |  nsindex1  |
+	 *     |            |
+	 *     +------------+
+	 *     |   label0   |
+	 *     +------------+
+	 *     |   label1   |
+	 *     +------------+
+	 *     |            |
+	 *      ....nslot...
+	 *     |            |
+	 *     +------------+
+	 *     |   labelN   |
+	 *     +------------+
+	 */
+	struct nd_namespace_index *nsindex[] = {
+		to_namespace_index(ndd, 0),
+		to_namespace_index(ndd, 1),
+	};
+	const int num_index = ARRAY_SIZE(nsindex);
+	struct device *dev = ndd->dev;
+	bool valid[2] = { 0 };
+	int i, num_valid = 0;
+	u32 seq;
+
+	for (i = 0; i < num_index; i++) {
+		u32 nslot;
+		u8 sig[NSINDEX_SIG_LEN];
+		u64 sum_save, sum, size;
+
+		memcpy(sig, nsindex[i]->sig, NSINDEX_SIG_LEN);
+		if (memcmp(sig, NSINDEX_SIGNATURE, NSINDEX_SIG_LEN) != 0) {
+			dev_dbg(dev, "%s: nsindex%d signature invalid\n",
+					__func__, i);
+			continue;
+		}
+		sum_save = __le64_to_cpu(nsindex[i]->checksum);
+		nsindex[i]->checksum = __cpu_to_le64(0);
+		sum = nd_fletcher64(nsindex[i], sizeof_namespace_index(ndd), 1);
+		nsindex[i]->checksum = __cpu_to_le64(sum_save);
+		if (sum != sum_save) {
+			dev_dbg(dev, "%s: nsindex%d checksum invalid\n",
+					__func__, i);
+			continue;
+		}
+
+		seq = __le32_to_cpu(nsindex[i]->seq);
+		if ((seq & NSINDEX_SEQ_MASK) == 0) {
+			dev_dbg(dev, "%s: nsindex%d sequence: %#x invalid\n",
+					__func__, i, seq);
+			continue;
+		}
+
+		/* sanity check the index against expected values */
+		if (__le64_to_cpu(nsindex[i]->myoff)
+				!= i * sizeof_namespace_index(ndd)) {
+			dev_dbg(dev, "%s: nsindex%d myoff: %#llx invalid\n",
+					__func__, i, (unsigned long long)
+					__le64_to_cpu(nsindex[i]->myoff));
+			continue;
+		}
+		if (__le64_to_cpu(nsindex[i]->otheroff)
+				!= (!i) * sizeof_namespace_index(ndd)) {
+			dev_dbg(dev, "%s: nsindex%d otheroff: %#llx invalid\n",
+					__func__, i, (unsigned long long)
+					__le64_to_cpu(nsindex[i]->otheroff));
+			continue;
+		}
+
+		size = __le64_to_cpu(nsindex[i]->mysize);
+		if (size > sizeof_namespace_index(ndd)
+				|| size < sizeof(struct nd_namespace_index)) {
+			dev_dbg(dev, "%s: nsindex%d mysize: %#llx invalid\n",
+					__func__, i, size);
+			continue;
+		}
+
+		nslot = __le32_to_cpu(nsindex[i]->nslot);
+		if (nslot * sizeof(struct nd_namespace_label)
+				+ 2 * sizeof_namespace_index(ndd)
+				> ndd->nsarea.config_size) {
+			dev_dbg(dev, "%s: nsindex%d nslot: %u invalid, config_size: %#x\n",
+					__func__, i, nslot,
+					ndd->nsarea.config_size);
+			continue;
+		}
+		valid[i] = true;
+		num_valid++;
+	}
+
+	switch (num_valid) {
+	case 0:
+		break;
+	case 1:
+		for (i = 0; i < num_index; i++)
+			if (valid[i])
+				return i;
+		/* can't have num_valid > 0 but valid[] = { false, false } */
+		WARN_ON(1);
+		break;
+	default:
+		/* pick the best index... */
+		seq = best_seq(__le32_to_cpu(nsindex[0]->seq),
+				__le32_to_cpu(nsindex[1]->seq));
+		if (seq == (__le32_to_cpu(nsindex[1]->seq) & NSINDEX_SEQ_MASK))
+			return 1;
+		else
+			return 0;
+		break;
+	}
+
+	return -1;
+}
+
+void nd_label_copy(struct nvdimm_drvdata *ndd, struct nd_namespace_index *dst,
+		struct nd_namespace_index *src)
+{
+	if (dst && src)
+		/* pass */;
+	else
+		return;
+
+	memcpy(dst, src, sizeof_namespace_index(ndd));
+}
+
+static struct nd_namespace_label *nd_label_base(struct nvdimm_drvdata *ndd)
+{
+	void *base = to_namespace_index(ndd, 0);
+
+	return base + 2 * sizeof_namespace_index(ndd);
+}
+
+static int to_slot(struct nvdimm_drvdata *ndd,
+		struct nd_namespace_label *nd_label)
+{
+	return nd_label - nd_label_base(ndd);
+}
+
+#define for_each_clear_bit_le(bit, addr, size) \
+	for ((bit) = find_next_zero_bit_le((addr), (size), 0);  \
+	     (bit) < (size);                                    \
+	     (bit) = find_next_zero_bit_le((addr), (size), (bit) + 1))
+
+/**
+ * preamble_index - common variable initialization for nd_label_* routines
+ * @ndd: dimm container for the relevant label set
+ * @idx: namespace_index index
+ * @nsindex_out: on return set to the currently active namespace index
+ * @free: on return set to the free label bitmap in the index
+ * @nslot: on return set to the number of slots in the label space
+ */
+static bool preamble_index(struct nvdimm_drvdata *ndd, int idx,
+		struct nd_namespace_index **nsindex_out,
+		unsigned long **free, u32 *nslot)
+{
+	struct nd_namespace_index *nsindex;
+
+	nsindex = to_namespace_index(ndd, idx);
+	if (nsindex == NULL)
+		return false;
+
+	*free = (unsigned long *) nsindex->free;
+	*nslot = __le32_to_cpu(nsindex->nslot);
+	*nsindex_out = nsindex;
+
+	return true;
+}
+
+char *nd_label_gen_id(struct nd_label_id *label_id, u8 *uuid, u32 flags)
+{
+	if (!label_id || !uuid)
+		return NULL;
+	snprintf(label_id->id, ND_LABEL_ID_SIZE, "%s-%pUb",
+			flags & NSLABEL_FLAG_LOCAL ? "blk" : "pmem", uuid);
+	return label_id->id;
+}
+
+static bool preamble_current(struct nvdimm_drvdata *ndd,
+		struct nd_namespace_index **nsindex,
+		unsigned long **free, u32 *nslot)
+{
+	return preamble_index(ndd, ndd->ns_current, nsindex,
+			free, nslot);
+}
+
+static bool preamble_next(struct nvdimm_drvdata *ndd,
+		struct nd_namespace_index **nsindex,
+		unsigned long **free, u32 *nslot)
+{
+	return preamble_index(ndd, ndd->ns_next, nsindex,
+			free, nslot);
+}
+
+static bool slot_valid(struct nd_namespace_label *nd_label, u32 slot)
+{
+	/* check that we are written where we expect to be written */
+	if (slot != __le32_to_cpu(nd_label->slot))
+		return false;
+
+	/* check that DPA allocations are page aligned */
+	if ((__le64_to_cpu(nd_label->dpa)
+				| __le64_to_cpu(nd_label->rawsize)) % SZ_4K)
+		return false;
+
+	return true;
+}
+
+int nd_label_reserve_dpa(struct nvdimm_drvdata *ndd)
+{
+	struct nd_namespace_index *nsindex;
+	unsigned long *free;
+	u32 nslot, slot;
+
+	if (!preamble_current(ndd, &nsindex, &free, &nslot))
+		return 0; /* no label, nothing to reserve */
+
+	for_each_clear_bit_le(slot, free, nslot) {
+		struct nd_namespace_label *nd_label;
+		struct nd_region *nd_region = NULL;
+		u8 label_uuid[NSLABEL_UUID_LEN];
+		struct nd_label_id label_id;
+		struct resource *res;
+		u32 flags;
+
+		nd_label = nd_label_base(ndd) + slot;
+
+		if (!slot_valid(nd_label, slot))
+			continue;
+
+		memcpy(label_uuid, nd_label->uuid, NSLABEL_UUID_LEN);
+		flags = __le32_to_cpu(nd_label->flags);
+		nd_label_gen_id(&label_id, label_uuid, flags);
+		res = nvdimm_allocate_dpa(ndd, &label_id,
+				__le64_to_cpu(nd_label->dpa),
+				__le64_to_cpu(nd_label->rawsize));
+		nd_dbg_dpa(nd_region, ndd, res, "reserve\n");
+		if (!res)
+			return -EBUSY;
+	}
+
+	return 0;
+}
+
+int nd_label_active_count(struct nvdimm_drvdata *ndd)
+{
+	struct nd_namespace_index *nsindex;
+	unsigned long *free;
+	u32 nslot, slot;
+	int count = 0;
+
+	if (!preamble_current(ndd, &nsindex, &free, &nslot))
+		return 0;
+
+	for_each_clear_bit_le(slot, free, nslot) {
+		struct nd_namespace_label *nd_label;
+
+		nd_label = nd_label_base(ndd) + slot;
+
+		if (!slot_valid(nd_label, slot)) {
+			u32 label_slot = __le32_to_cpu(nd_label->slot);
+			u64 size = __le64_to_cpu(nd_label->rawsize);
+			u64 dpa = __le64_to_cpu(nd_label->dpa);
+
+			dev_dbg(ndd->dev,
+				"%s: slot%d invalid slot: %d dpa: %llx size: %llx\n",
+					__func__, slot, label_slot, dpa, size);
+			continue;
+		}
+		count++;
+	}
+	return count;
+}
+
+struct nd_namespace_label *nd_label_active(struct nvdimm_drvdata *ndd, int n)
+{
+	struct nd_namespace_index *nsindex;
+	unsigned long *free;
+	u32 nslot, slot;
+
+	if (!preamble_current(ndd, &nsindex, &free, &nslot))
+		return NULL;
+
+	for_each_clear_bit_le(slot, free, nslot) {
+		struct nd_namespace_label *nd_label;
+
+		nd_label = nd_label_base(ndd) + slot;
+		if (!slot_valid(nd_label, slot))
+			continue;
+
+		if (n-- == 0)
+			return nd_label_base(ndd) + slot;
+	}
+
+	return NULL;
+}
+
+u32 nd_label_alloc_slot(struct nvdimm_drvdata *ndd)
+{
+	struct nd_namespace_index *nsindex;
+	unsigned long *free;
+	u32 nslot, slot;
+
+	if (!preamble_next(ndd, &nsindex, &free, &nslot))
+		return UINT_MAX;
+
+	WARN_ON(!is_nvdimm_bus_locked(ndd->dev));
+
+	slot = find_next_bit_le(free, nslot, 0);
+	if (slot == nslot)
+		return UINT_MAX;
+
+	clear_bit_le(slot, free);
+
+	return slot;
+}
+
+bool nd_label_free_slot(struct nvdimm_drvdata *ndd, u32 slot)
+{
+	struct nd_namespace_index *nsindex;
+	unsigned long *free;
+	u32 nslot;
+
+	if (!preamble_next(ndd, &nsindex, &free, &nslot))
+		return false;
+
+	WARN_ON(!is_nvdimm_bus_locked(ndd->dev));
+
+	if (slot < nslot)
+		return !test_and_set_bit_le(slot, free);
+	return false;
+}
+
+u32 nd_label_nfree(struct nvdimm_drvdata *ndd)
+{
+	struct nd_namespace_index *nsindex;
+	unsigned long *free;
+	u32 nslot;
+
+	WARN_ON(!is_nvdimm_bus_locked(ndd->dev));
+
+	if (!preamble_next(ndd, &nsindex, &free, &nslot))
+		return nvdimm_num_label_slots(ndd);
+
+	return bitmap_weight(free, nslot);
+}
+
+static int nd_label_write_index(struct nvdimm_drvdata *ndd, int index, u32 seq,
+		unsigned long flags)
+{
+	struct nd_namespace_index *nsindex;
+	unsigned long offset;
+	u64 checksum;
+	u32 nslot;
+	int rc;
+
+	nsindex = to_namespace_index(ndd, index);
+	if (flags & ND_NSINDEX_INIT)
+		nslot = nvdimm_num_label_slots(ndd);
+	else
+		nslot = __le32_to_cpu(nsindex->nslot);
+
+	memcpy(nsindex->sig, NSINDEX_SIGNATURE, NSINDEX_SIG_LEN);
+	nsindex->flags = __cpu_to_le32(0);
+	nsindex->seq = __cpu_to_le32(seq);
+	offset = (unsigned long) nsindex
+		- (unsigned long) to_namespace_index(ndd, 0);
+	nsindex->myoff = __cpu_to_le64(offset);
+	nsindex->mysize = __cpu_to_le64(sizeof_namespace_index(ndd));
+	offset = (unsigned long) to_namespace_index(ndd,
+			nd_label_next_nsindex(index))
+		- (unsigned long) to_namespace_index(ndd, 0);
+	nsindex->otheroff = __cpu_to_le64(offset);
+	offset = (unsigned long) nd_label_base(ndd)
+		- (unsigned long) to_namespace_index(ndd, 0);
+	nsindex->labeloff = __cpu_to_le64(offset);
+	nsindex->nslot = __cpu_to_le32(nslot);
+	nsindex->major = __cpu_to_le16(1);
+	nsindex->minor = __cpu_to_le16(1);
+	nsindex->checksum = __cpu_to_le64(0);
+	if (flags & ND_NSINDEX_INIT) {
+		unsigned long *free = (unsigned long *) nsindex->free;
+		u32 nfree = ALIGN(nslot, BITS_PER_LONG);
+		int last_bits, i;
+
+		memset(nsindex->free, 0xff, nfree / 8);
+		for (i = 0, last_bits = nfree - nslot; i < last_bits; i++)
+			clear_bit_le(nslot + i, free);
+	}
+	checksum = nd_fletcher64(nsindex, sizeof_namespace_index(ndd), 1);
+	nsindex->checksum = __cpu_to_le64(checksum);
+	rc = nvdimm_set_config_data(ndd, __le64_to_cpu(nsindex->myoff),
+			nsindex, sizeof_namespace_index(ndd));
+	if (rc < 0)
+		return rc;
+
+	if (flags & ND_NSINDEX_INIT)
+		return 0;
+
+	/* copy the index we just wrote to the new 'next' */
+	WARN_ON(index != ndd->ns_next);
+	nd_label_copy(ndd, to_current_namespace_index(ndd), nsindex);
+	ndd->ns_current = nd_label_next_nsindex(ndd->ns_current);
+	ndd->ns_next = nd_label_next_nsindex(ndd->ns_next);
+	WARN_ON(ndd->ns_current == ndd->ns_next);
+
+	return 0;
+}
+
+static unsigned long nd_label_offset(struct nvdimm_drvdata *ndd,
+		struct nd_namespace_label *nd_label)
+{
+	return (unsigned long) nd_label
+		- (unsigned long) to_namespace_index(ndd, 0);
+}
+
+static int __pmem_label_update(struct nd_region *nd_region,
+		struct nd_mapping *nd_mapping, struct nd_namespace_pmem *nspm,
+		int pos)
+{
+	u64 cookie = nd_region_interleave_set_cookie(nd_region), rawsize;
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	struct nd_namespace_label *victim_label;
+	struct nd_namespace_label *nd_label;
+	struct nd_namespace_index *nsindex;
+	unsigned long *free;
+	u32 nslot, slot;
+	size_t offset;
+	int rc;
+
+	if (!preamble_next(ndd, &nsindex, &free, &nslot))
+		return -ENXIO;
+
+	/* allocate and write the label to the staging (next) index */
+	slot = nd_label_alloc_slot(ndd);
+	if (slot == UINT_MAX)
+		return -ENXIO;
+	dev_dbg(ndd->dev, "%s: allocated: %d\n", __func__, slot);
+
+	nd_label = nd_label_base(ndd) + slot;
+	memset(nd_label, 0, sizeof(struct nd_namespace_label));
+	memcpy(nd_label->uuid, nspm->uuid, NSLABEL_UUID_LEN);
+	if (nspm->alt_name)
+		memcpy(nd_label->name, nspm->alt_name, NSLABEL_NAME_LEN);
+	nd_label->flags = __cpu_to_le32(NSLABEL_FLAG_UPDATING);
+	nd_label->nlabel = __cpu_to_le16(nd_region->ndr_mappings);
+	nd_label->position = __cpu_to_le16(pos);
+	nd_label->isetcookie = __cpu_to_le64(cookie);
+	rawsize = div_u64(resource_size(&nspm->nsio.res),
+			nd_region->ndr_mappings);
+	nd_label->rawsize = __cpu_to_le64(rawsize);
+	nd_label->dpa = __cpu_to_le64(nd_mapping->start);
+	nd_label->slot = __cpu_to_le32(slot);
+
+	/* update label */
+	offset = nd_label_offset(ndd, nd_label);
+	rc = nvdimm_set_config_data(ndd, offset, nd_label,
+			sizeof(struct nd_namespace_label));
+	if (rc < 0)
+		return rc;
+
+	/* Garbage collect the previous label */
+	victim_label = nd_mapping->labels[0];
+	if (victim_label) {
+		slot = to_slot(ndd, victim_label);
+		nd_label_free_slot(ndd, slot);
+		dev_dbg(ndd->dev, "%s: free: %d\n", __func__, slot);
+	}
+
+	/* update index */
+	rc = nd_label_write_index(ndd, ndd->ns_next,
+			nd_inc_seq(__le32_to_cpu(nsindex->seq)), 0);
+	if (rc < 0)
+		return rc;
+
+	nd_mapping->labels[0] = nd_label;
+
+	return 0;
+}
+
+static void del_label(struct nd_mapping *nd_mapping, int l)
+{
+	struct nd_namespace_label *next_label, *nd_label;
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	unsigned int slot;
+	int j;
+
+	nd_label = nd_mapping->labels[l];
+	slot = to_slot(ndd, nd_label);
+	dev_vdbg(ndd->dev, "%s: clear: %d\n", __func__, slot);
+
+	for (j = l; (next_label = nd_mapping->labels[j + 1]); j++)
+		nd_mapping->labels[j] = next_label;
+	nd_mapping->labels[j] = NULL;
+}
+
+static bool is_old_resource(struct resource *res, struct resource **list, int n)
+{
+	int i;
+
+	if (res->flags & DPA_RESOURCE_ADJUSTED)
+		return false;
+	for (i = 0; i < n; i++)
+		if (res == list[i])
+			return true;
+	return false;
+}
+
+static struct resource *to_resource(struct nvdimm_drvdata *ndd,
+		struct nd_namespace_label *nd_label)
+{
+	struct resource *res;
+
+	for_each_dpa_resource(ndd, res) {
+		if (res->start != __le64_to_cpu(nd_label->dpa))
+			continue;
+		if (resource_size(res) != __le64_to_cpu(nd_label->rawsize))
+			continue;
+		return res;
+	}
+
+	return NULL;
+}
+
+/*
+ * 1/ Account all the labels that can be freed after this update
+ * 2/ Allocate and write the label to the staging (next) index
+ * 3/ Record the resources in the namespace device
+ */
+static int __blk_label_update(struct nd_region *nd_region,
+		struct nd_mapping *nd_mapping, struct nd_namespace_blk *nsblk,
+		int num_labels)
+{
+	int i, l, alloc, victims, nfree, old_num_resources, nlabel, rc = -ENXIO;
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	struct nd_namespace_label *nd_label;
+	struct nd_namespace_index *nsindex;
+	unsigned long *free, *victim_map = NULL;
+	struct resource *res, **old_res_list;
+	struct nd_label_id label_id;
+	u8 uuid[NSLABEL_UUID_LEN];
+	u32 nslot, slot;
+
+	if (!preamble_next(ndd, &nsindex, &free, &nslot))
+		return -ENXIO;
+
+	old_res_list = nsblk->res;
+	nfree = nd_label_nfree(ndd);
+	old_num_resources = nsblk->num_resources;
+	nd_label_gen_id(&label_id, nsblk->uuid, NSLABEL_FLAG_LOCAL);
+
+	/*
+	 * We need to loop over the old resources a few times, which seems a
+	 * bit inefficient, but we need to know that we have the label
+	 * space before we start mutating the tracking structures.
+	 * Otherwise the recovery method of last resort for userspace is
+	 * disable and re-enable the parent region.
+	 */
+	alloc = 0;
+	for_each_dpa_resource(ndd, res) {
+		if (strcmp(res->name, label_id.id) != 0)
+			continue;
+		if (!is_old_resource(res, old_res_list, old_num_resources))
+			alloc++;
+	}
+
+	victims = 0;
+	if (old_num_resources) {
+		/* convert old local-label-map to dimm-slot victim-map */
+		victim_map = kcalloc(BITS_TO_LONGS(nslot), sizeof(long),
+				GFP_KERNEL);
+		if (!victim_map)
+			return -ENOMEM;
+
+		/* mark unused labels for garbage collection */
+		for_each_clear_bit_le(slot, free, nslot) {
+			nd_label = nd_label_base(ndd) + slot;
+			memcpy(uuid, nd_label->uuid, NSLABEL_UUID_LEN);
+			if (memcmp(uuid, nsblk->uuid, NSLABEL_UUID_LEN) != 0)
+				continue;
+			res = to_resource(ndd, nd_label);
+			if (res && is_old_resource(res, old_res_list,
+						old_num_resources))
+				continue;
+			slot = to_slot(ndd, nd_label);
+			set_bit(slot, victim_map);
+			victims++;
+		}
+	}
+
+	/* don't allow updates that consume the last label */
+	if (nfree - alloc < 0 || nfree - alloc + victims < 1) {
+		dev_info(&nsblk->common.dev, "insufficient label space\n");
+		kfree(victim_map);
+		return -ENOSPC;
+	}
+	/* from here on we need to abort on error */
+
+
+	/* assign all resources to the namespace before writing the labels */
+	nsblk->res = NULL;
+	nsblk->num_resources = 0;
+	for_each_dpa_resource(ndd, res) {
+		if (strcmp(res->name, label_id.id) != 0)
+			continue;
+		if (!nsblk_add_resource(nd_region, ndd, nsblk, res->start)) {
+			rc = -ENOMEM;
+			goto abort;
+		}
+	}
+
+	for (i = 0; i < nsblk->num_resources; i++) {
+		size_t offset;
+
+		res = nsblk->res[i];
+		if (is_old_resource(res, old_res_list, old_num_resources))
+			continue; /* carry-over */
+		slot = nd_label_alloc_slot(ndd);
+		if (slot == UINT_MAX)
+			goto abort;
+		dev_dbg(ndd->dev, "%s: allocated: %d\n", __func__, slot);
+
+		nd_label = nd_label_base(ndd) + slot;
+		memset(nd_label, 0, sizeof(struct nd_namespace_label));
+		memcpy(nd_label->uuid, nsblk->uuid, NSLABEL_UUID_LEN);
+		if (nsblk->alt_name)
+			memcpy(nd_label->name, nsblk->alt_name,
+					NSLABEL_NAME_LEN);
+		nd_label->flags = __cpu_to_le32(NSLABEL_FLAG_LOCAL);
+		nd_label->nlabel = __cpu_to_le16(0); /* N/A */
+		nd_label->position = __cpu_to_le16(0); /* N/A */
+		nd_label->isetcookie = __cpu_to_le64(0); /* N/A */
+		nd_label->dpa = __cpu_to_le64(res->start);
+		nd_label->rawsize = __cpu_to_le64(resource_size(res));
+		nd_label->lbasize = __cpu_to_le64(nsblk->lbasize);
+		nd_label->slot = __cpu_to_le32(slot);
+
+		/* update label */
+		offset = nd_label_offset(ndd, nd_label);
+		rc = nvdimm_set_config_data(ndd, offset, nd_label,
+				sizeof(struct nd_namespace_label));
+		if (rc < 0)
+			goto abort;
+	}
+
+	/* free up now unused slots in the new index */
+	for_each_set_bit(slot, victim_map, victim_map ? nslot : 0) {
+		dev_dbg(ndd->dev, "%s: free: %d\n", __func__, slot);
+		nd_label_free_slot(ndd, slot);
+	}
+
+	/* update index */
+	rc = nd_label_write_index(ndd, ndd->ns_next,
+			nd_inc_seq(__le32_to_cpu(nsindex->seq)), 0);
+	if (rc)
+		goto abort;
+
+	/*
+	 * Now that the on-dimm labels are up to date, fix up the tracking
+	 * entries in nd_mapping->labels
+	 */
+	nlabel = 0;
+	for_each_label(l, nd_label, nd_mapping->labels) {
+		nlabel++;
+		memcpy(uuid, nd_label->uuid, NSLABEL_UUID_LEN);
+		if (memcmp(uuid, nsblk->uuid, NSLABEL_UUID_LEN) != 0)
+			continue;
+		nlabel--;
+		del_label(nd_mapping, l);
+		l--; /* retry with the new label at this index */
+	}
+	if (nlabel + nsblk->num_resources > num_labels) {
+		/*
+		 * Bug, we can't end up with more resources than
+		 * available labels
+		 */
+		WARN_ON_ONCE(1);
+		rc = -ENXIO;
+		goto out;
+	}
+
+	for_each_clear_bit_le(slot, free, nslot) {
+		nd_label = nd_label_base(ndd) + slot;
+		memcpy(uuid, nd_label->uuid, NSLABEL_UUID_LEN);
+		if (memcmp(uuid, nsblk->uuid, NSLABEL_UUID_LEN) != 0)
+			continue;
+		res = to_resource(ndd, nd_label);
+		res->flags &= ~DPA_RESOURCE_ADJUSTED;
+		dev_vdbg(&nsblk->common.dev, "assign label[%d] slot: %d\n",
+				l, slot);
+		nd_mapping->labels[l++] = nd_label;
+	}
+	nd_mapping->labels[l] = NULL;
+
+ out:
+	kfree(old_res_list);
+	kfree(victim_map);
+	return rc;
+
+ abort:
+	/*
+	 * 1/ repair the allocated label bitmap in the index
+	 * 2/ restore the resource list
+	 */
+	nd_label_copy(ndd, nsindex, to_current_namespace_index(ndd));
+	kfree(nsblk->res);
+	nsblk->res = old_res_list;
+	nsblk->num_resources = old_num_resources;
+	old_res_list = NULL;
+	goto out;
+}
+
+static int init_labels(struct nd_mapping *nd_mapping, int num_labels)
+{
+	int i, l, old_num_labels = 0;
+	struct nd_namespace_index *nsindex;
+	struct nd_namespace_label *nd_label;
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	size_t size = (num_labels + 1) * sizeof(struct nd_namespace_label *);
+
+	for_each_label(l, nd_label, nd_mapping->labels)
+		old_num_labels++;
+
+	/*
+	 * We need to preserve all the old labels for the mapping so
+	 * they can be garbage collected after writing the new labels.
+	 */
+	if (num_labels > old_num_labels) {
+		struct nd_namespace_label **labels;
+
+		labels = krealloc(nd_mapping->labels, size, GFP_KERNEL);
+		if (!labels)
+			return -ENOMEM;
+		nd_mapping->labels = labels;
+	}
+	if (!nd_mapping->labels)
+		return -ENOMEM;
+
+	for (i = old_num_labels; i <= num_labels; i++)
+		nd_mapping->labels[i] = NULL;
+
+	if (ndd->ns_current == -1 || ndd->ns_next == -1)
+		/* pass */;
+	else
+		return max(num_labels, old_num_labels);
+
+	nsindex = to_namespace_index(ndd, 0);
+	memset(nsindex, 0, ndd->nsarea.config_size);
+	for (i = 0; i < 2; i++) {
+		int rc = nd_label_write_index(ndd, i, i*2, ND_NSINDEX_INIT);
+
+		if (rc)
+			return rc;
+	}
+	ndd->ns_next = 1;
+	ndd->ns_current = 0;
+
+	return max(num_labels, old_num_labels);
+}
+
+static int del_labels(struct nd_mapping *nd_mapping, u8 *uuid)
+{
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	struct nd_namespace_label *nd_label;
+	struct nd_namespace_index *nsindex;
+	u8 label_uuid[NSLABEL_UUID_LEN];
+	int l, num_freed = 0;
+	unsigned long *free;
+	u32 nslot, slot;
+
+	if (!uuid)
+		return 0;
+
+	/* no index || no labels == nothing to delete */
+	if (!preamble_next(ndd, &nsindex, &free, &nslot)
+			|| !nd_mapping->labels)
+		return 0;
+
+	for_each_label(l, nd_label, nd_mapping->labels) {
+		memcpy(label_uuid, nd_label->uuid, NSLABEL_UUID_LEN);
+		if (memcmp(label_uuid, uuid, NSLABEL_UUID_LEN) != 0)
+			continue;
+		slot = to_slot(ndd, nd_label);
+		nd_label_free_slot(ndd, slot);
+		dev_dbg(ndd->dev, "%s: free: %d\n", __func__, slot);
+		del_label(nd_mapping, l);
+		num_freed++;
+		l--; /* retry with new label at this index */
+	}
+
+	if (num_freed > l) {
+		/*
+		 * num_freed will only ever be > l when we delete the last
+		 * label
+		 */
+		kfree(nd_mapping->labels);
+		nd_mapping->labels = NULL;
+		dev_dbg(ndd->dev, "%s: no more labels\n", __func__);
+	}
+
+	return nd_label_write_index(ndd, ndd->ns_next,
+			nd_inc_seq(__le32_to_cpu(nsindex->seq)), 0);
+}
+
+int nd_pmem_namespace_label_update(struct nd_region *nd_region,
+		struct nd_namespace_pmem *nspm, resource_size_t size)
+{
+	int i;
+
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		int rc;
+
+		if (size == 0) {
+			rc = del_labels(nd_mapping, nspm->uuid);
+			if (rc)
+				return rc;
+			continue;
+		}
+
+		rc = init_labels(nd_mapping, 1);
+		if (rc < 0)
+			return rc;
+
+		rc = __pmem_label_update(nd_region, nd_mapping, nspm, i);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+int nd_blk_namespace_label_update(struct nd_region *nd_region,
+		struct nd_namespace_blk *nsblk, resource_size_t size)
+{
+	struct nd_mapping *nd_mapping = &nd_region->mapping[0];
+	struct resource *res;
+	int count = 0;
+
+	if (size == 0)
+		return del_labels(nd_mapping, nsblk->uuid);
+
+	for_each_dpa_resource(to_ndd(nd_mapping), res)
+		count++;
+
+	count = init_labels(nd_mapping, count);
+	if (count < 0)
+		return count;
+
+	return __blk_label_update(nd_region, nd_mapping, nsblk, count);
+}
diff --git a/drivers/nvdimm/label.h b/drivers/nvdimm/label.h
new file mode 100644
index 000000000000..a59ef6eef2a3
--- /dev/null
+++ b/drivers/nvdimm/label.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#ifndef __LABEL_H__
+#define __LABEL_H__
+
+#include <linux/ndctl.h>
+#include <linux/sizes.h>
+#include <linux/io.h>
+
+enum {
+	NSINDEX_SIG_LEN = 16,
+	NSINDEX_ALIGN = 256,
+	NSINDEX_SEQ_MASK = 0x3,
+	NSLABEL_UUID_LEN = 16,
+	NSLABEL_NAME_LEN = 64,
+	NSLABEL_FLAG_ROLABEL = 0x1,  /* read-only label */
+	NSLABEL_FLAG_LOCAL = 0x2,    /* DIMM-local namespace */
+	NSLABEL_FLAG_BTT = 0x4,      /* namespace contains a BTT */
+	NSLABEL_FLAG_UPDATING = 0x8, /* label being updated */
+	BTT_ALIGN = 4096,            /* all btt structures */
+	BTTINFO_SIG_LEN = 16,
+	BTTINFO_UUID_LEN = 16,
+	BTTINFO_FLAG_ERROR = 0x1,    /* error state (read-only) */
+	BTTINFO_MAJOR_VERSION = 1,
+	ND_LABEL_MIN_SIZE = 512 * 129, /* see sizeof_namespace_index() */
+	ND_LABEL_ID_SIZE = 50,
+	ND_NSINDEX_INIT = 0x1,
+};
+
+static const char NSINDEX_SIGNATURE[] = "NAMESPACE_INDEX\0";
+
+/**
+ * struct nd_namespace_index - label set superblock
+ * @sig: NAMESPACE_INDEX\0
+ * @flags: placeholder
+ * @seq: sequence number for this index
+ * @myoff: offset of this index in label area
+ * @mysize: size of this index struct
+ * @otheroff: offset of other index
+ * @labeloff: offset of first label slot
+ * @nslot: total number of label slots
+ * @major: label area major version
+ * @minor: label area minor version
+ * @checksum: fletcher64 of all fields
+ * @free[0]: bitmap, nlabel bits
+ *
+ * The size of free[] is rounded up so the total struct size is a
+ * multiple of NSINDEX_ALIGN bytes.  Any bits this allocates beyond
+ * nlabel bits must be zero.
+ */
+struct nd_namespace_index {
+	u8 sig[NSINDEX_SIG_LEN];
+	__le32 flags;
+	__le32 seq;
+	__le64 myoff;
+	__le64 mysize;
+	__le64 otheroff;
+	__le64 labeloff;
+	__le32 nslot;
+	__le16 major;
+	__le16 minor;
+	__le64 checksum;
+	u8 free[0];
+};
+
+/**
+ * struct nd_namespace_label - namespace superblock
+ * @uuid: UUID per RFC 4122
+ * @name: optional name (NULL-terminated)
+ * @flags: see NSLABEL_FLAG_*
+ * @nlabel: num labels to describe this ns
+ * @position: labels position in set
+ * @isetcookie: interleave set cookie
+ * @lbasize: LBA size in bytes or 0 for pmem
+ * @dpa: DPA of NVM range on this DIMM
+ * @rawsize: size of namespace
+ * @slot: slot of this label in label area
+ * @unused: must be zero
+ */
+struct nd_namespace_label {
+	u8 uuid[NSLABEL_UUID_LEN];
+	u8 name[NSLABEL_NAME_LEN];
+	__le32 flags;
+	__le16 nlabel;
+	__le16 position;
+	__le64 isetcookie;
+	__le64 lbasize;
+	__le64 dpa;
+	__le64 rawsize;
+	__le32 slot;
+	__le32 unused;
+};
+
+/**
+ * struct nd_label_id - identifier string for dpa allocation
+ * @id: "{blk|pmem}-<namespace uuid>"
+ */
+struct nd_label_id {
+	char id[ND_LABEL_ID_SIZE];
+};
+
+/*
+ * If the 'best' index is invalid, so is the 'next' index.  Otherwise,
+ * the next index is MOD(index+1, 2)
+ */
+static inline int nd_label_next_nsindex(int index)
+{
+	if (index < 0)
+		return -1;
+
+	return (index + 1) % 2;
+}
+
+struct nvdimm_drvdata;
+int nd_label_validate(struct nvdimm_drvdata *ndd);
+void nd_label_copy(struct nvdimm_drvdata *ndd, struct nd_namespace_index *dst,
+		struct nd_namespace_index *src);
+size_t sizeof_namespace_index(struct nvdimm_drvdata *ndd);
+int nd_label_active_count(struct nvdimm_drvdata *ndd);
+struct nd_namespace_label *nd_label_active(struct nvdimm_drvdata *ndd, int n);
+u32 nd_label_alloc_slot(struct nvdimm_drvdata *ndd);
+bool nd_label_free_slot(struct nvdimm_drvdata *ndd, u32 slot);
+u32 nd_label_nfree(struct nvdimm_drvdata *ndd);
+struct nd_region;
+struct nd_namespace_pmem;
+struct nd_namespace_blk;
+int nd_pmem_namespace_label_update(struct nd_region *nd_region,
+		struct nd_namespace_pmem *nspm, resource_size_t size);
+int nd_blk_namespace_label_update(struct nd_region *nd_region,
+		struct nd_namespace_blk *nsblk, resource_size_t size);
+#endif /* __LABEL_H__ */
diff --git a/drivers/nvdimm/namespace_devs.c b/drivers/nvdimm/namespace_devs.c
new file mode 100644
index 000000000000..fef0dd80d4ad
--- /dev/null
+++ b/drivers/nvdimm/namespace_devs.c
@@ -0,0 +1,1870 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/nd.h>
+#include "nd-core.h"
+#include "nd.h"
+
+static void namespace_io_release(struct device *dev)
+{
+	struct nd_namespace_io *nsio = to_nd_namespace_io(dev);
+
+	kfree(nsio);
+}
+
+static void namespace_pmem_release(struct device *dev)
+{
+	struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+	kfree(nspm->alt_name);
+	kfree(nspm->uuid);
+	kfree(nspm);
+}
+
+static void namespace_blk_release(struct device *dev)
+{
+	struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+
+	if (nsblk->id >= 0)
+		ida_simple_remove(&nd_region->ns_ida, nsblk->id);
+	kfree(nsblk->alt_name);
+	kfree(nsblk->uuid);
+	kfree(nsblk->res);
+	kfree(nsblk);
+}
+
+static struct device_type namespace_io_device_type = {
+	.name = "nd_namespace_io",
+	.release = namespace_io_release,
+};
+
+static struct device_type namespace_pmem_device_type = {
+	.name = "nd_namespace_pmem",
+	.release = namespace_pmem_release,
+};
+
+static struct device_type namespace_blk_device_type = {
+	.name = "nd_namespace_blk",
+	.release = namespace_blk_release,
+};
+
+static bool is_namespace_pmem(struct device *dev)
+{
+	return dev ? dev->type == &namespace_pmem_device_type : false;
+}
+
+static bool is_namespace_blk(struct device *dev)
+{
+	return dev ? dev->type == &namespace_blk_device_type : false;
+}
+
+static bool is_namespace_io(struct device *dev)
+{
+	return dev ? dev->type == &namespace_io_device_type : false;
+}
+
+const char *nvdimm_namespace_disk_name(struct nd_namespace_common *ndns,
+		char *name)
+{
+	struct nd_region *nd_region = to_nd_region(ndns->dev.parent);
+	const char *suffix = "";
+
+	if (ndns->claim && is_nd_btt(ndns->claim))
+		suffix = "s";
+
+	if (is_namespace_pmem(&ndns->dev) || is_namespace_io(&ndns->dev))
+		sprintf(name, "pmem%d%s", nd_region->id, suffix);
+	else if (is_namespace_blk(&ndns->dev)) {
+		struct nd_namespace_blk *nsblk;
+
+		nsblk = to_nd_namespace_blk(&ndns->dev);
+		sprintf(name, "ndblk%d.%d%s", nd_region->id, nsblk->id, suffix);
+	} else {
+		return NULL;
+	}
+
+	return name;
+}
+EXPORT_SYMBOL(nvdimm_namespace_disk_name);
+
+static ssize_t nstype_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+
+	return sprintf(buf, "%d\n", nd_region_to_nstype(nd_region));
+}
+static DEVICE_ATTR_RO(nstype);
+
+static ssize_t __alt_name_store(struct device *dev, const char *buf,
+		const size_t len)
+{
+	char *input, *pos, *alt_name, **ns_altname;
+	ssize_t rc;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		ns_altname = &nspm->alt_name;
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		ns_altname = &nsblk->alt_name;
+	} else
+		return -ENXIO;
+
+	if (dev->driver || to_ndns(dev)->claim)
+		return -EBUSY;
+
+	input = kmemdup(buf, len + 1, GFP_KERNEL);
+	if (!input)
+		return -ENOMEM;
+
+	input[len] = '\0';
+	pos = strim(input);
+	if (strlen(pos) + 1 > NSLABEL_NAME_LEN) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	alt_name = kzalloc(NSLABEL_NAME_LEN, GFP_KERNEL);
+	if (!alt_name) {
+		rc = -ENOMEM;
+		goto out;
+	}
+	kfree(*ns_altname);
+	*ns_altname = alt_name;
+	sprintf(*ns_altname, "%s", pos);
+	rc = len;
+
+out:
+	kfree(input);
+	return rc;
+}
+
+static resource_size_t nd_namespace_blk_size(struct nd_namespace_blk *nsblk)
+{
+	struct nd_region *nd_region = to_nd_region(nsblk->common.dev.parent);
+	struct nd_mapping *nd_mapping = &nd_region->mapping[0];
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	struct nd_label_id label_id;
+	resource_size_t size = 0;
+	struct resource *res;
+
+	if (!nsblk->uuid)
+		return 0;
+	nd_label_gen_id(&label_id, nsblk->uuid, NSLABEL_FLAG_LOCAL);
+	for_each_dpa_resource(ndd, res)
+		if (strcmp(res->name, label_id.id) == 0)
+			size += resource_size(res);
+	return size;
+}
+
+static bool __nd_namespace_blk_validate(struct nd_namespace_blk *nsblk)
+{
+	struct nd_region *nd_region = to_nd_region(nsblk->common.dev.parent);
+	struct nd_mapping *nd_mapping = &nd_region->mapping[0];
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	struct nd_label_id label_id;
+	struct resource *res;
+	int count, i;
+
+	if (!nsblk->uuid || !nsblk->lbasize || !ndd)
+		return false;
+
+	count = 0;
+	nd_label_gen_id(&label_id, nsblk->uuid, NSLABEL_FLAG_LOCAL);
+	for_each_dpa_resource(ndd, res) {
+		if (strcmp(res->name, label_id.id) != 0)
+			continue;
+		/*
+		 * Resources with unacknoweldged adjustments indicate a
+		 * failure to update labels
+		 */
+		if (res->flags & DPA_RESOURCE_ADJUSTED)
+			return false;
+		count++;
+	}
+
+	/* These values match after a successful label update */
+	if (count != nsblk->num_resources)
+		return false;
+
+	for (i = 0; i < nsblk->num_resources; i++) {
+		struct resource *found = NULL;
+
+		for_each_dpa_resource(ndd, res)
+			if (res == nsblk->res[i]) {
+				found = res;
+				break;
+			}
+		/* stale resource */
+		if (!found)
+			return false;
+	}
+
+	return true;
+}
+
+resource_size_t nd_namespace_blk_validate(struct nd_namespace_blk *nsblk)
+{
+	resource_size_t size;
+
+	nvdimm_bus_lock(&nsblk->common.dev);
+	size = __nd_namespace_blk_validate(nsblk);
+	nvdimm_bus_unlock(&nsblk->common.dev);
+
+	return size;
+}
+EXPORT_SYMBOL(nd_namespace_blk_validate);
+
+
+static int nd_namespace_label_update(struct nd_region *nd_region,
+		struct device *dev)
+{
+	dev_WARN_ONCE(dev, dev->driver || to_ndns(dev)->claim,
+			"namespace must be idle during label update\n");
+	if (dev->driver || to_ndns(dev)->claim)
+		return 0;
+
+	/*
+	 * Only allow label writes that will result in a valid namespace
+	 * or deletion of an existing namespace.
+	 */
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+		resource_size_t size = resource_size(&nspm->nsio.res);
+
+		if (size == 0 && nspm->uuid)
+			/* delete allocation */;
+		else if (!nspm->uuid)
+			return 0;
+
+		return nd_pmem_namespace_label_update(nd_region, nspm, size);
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+		resource_size_t size = nd_namespace_blk_size(nsblk);
+
+		if (size == 0 && nsblk->uuid)
+			/* delete allocation */;
+		else if (!nsblk->uuid || !nsblk->lbasize)
+			return 0;
+
+		return nd_blk_namespace_label_update(nd_region, nsblk, size);
+	} else
+		return -ENXIO;
+}
+
+static ssize_t alt_name_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	ssize_t rc;
+
+	device_lock(dev);
+	nvdimm_bus_lock(dev);
+	wait_nvdimm_bus_probe_idle(dev);
+	rc = __alt_name_store(dev, buf, len);
+	if (rc >= 0)
+		rc = nd_namespace_label_update(nd_region, dev);
+	dev_dbg(dev, "%s: %s(%zd)\n", __func__, rc < 0 ? "fail " : "", rc);
+	nvdimm_bus_unlock(dev);
+	device_unlock(dev);
+
+	return rc < 0 ? rc : len;
+}
+
+static ssize_t alt_name_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	char *ns_altname;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		ns_altname = nspm->alt_name;
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		ns_altname = nsblk->alt_name;
+	} else
+		return -ENXIO;
+
+	return sprintf(buf, "%s\n", ns_altname ? ns_altname : "");
+}
+static DEVICE_ATTR_RW(alt_name);
+
+static int scan_free(struct nd_region *nd_region,
+		struct nd_mapping *nd_mapping, struct nd_label_id *label_id,
+		resource_size_t n)
+{
+	bool is_blk = strncmp(label_id->id, "blk", 3) == 0;
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	int rc = 0;
+
+	while (n) {
+		struct resource *res, *last;
+		resource_size_t new_start;
+
+		last = NULL;
+		for_each_dpa_resource(ndd, res)
+			if (strcmp(res->name, label_id->id) == 0)
+				last = res;
+		res = last;
+		if (!res)
+			return 0;
+
+		if (n >= resource_size(res)) {
+			n -= resource_size(res);
+			nd_dbg_dpa(nd_region, ndd, res, "delete %d\n", rc);
+			nvdimm_free_dpa(ndd, res);
+			/* retry with last resource deleted */
+			continue;
+		}
+
+		/*
+		 * Keep BLK allocations relegated to high DPA as much as
+		 * possible
+		 */
+		if (is_blk)
+			new_start = res->start + n;
+		else
+			new_start = res->start;
+
+		rc = adjust_resource(res, new_start, resource_size(res) - n);
+		if (rc == 0)
+			res->flags |= DPA_RESOURCE_ADJUSTED;
+		nd_dbg_dpa(nd_region, ndd, res, "shrink %d\n", rc);
+		break;
+	}
+
+	return rc;
+}
+
+/**
+ * shrink_dpa_allocation - for each dimm in region free n bytes for label_id
+ * @nd_region: the set of dimms to reclaim @n bytes from
+ * @label_id: unique identifier for the namespace consuming this dpa range
+ * @n: number of bytes per-dimm to release
+ *
+ * Assumes resources are ordered.  Starting from the end try to
+ * adjust_resource() the allocation to @n, but if @n is larger than the
+ * allocation delete it and find the 'new' last allocation in the label
+ * set.
+ */
+static int shrink_dpa_allocation(struct nd_region *nd_region,
+		struct nd_label_id *label_id, resource_size_t n)
+{
+	int i;
+
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		int rc;
+
+		rc = scan_free(nd_region, nd_mapping, label_id, n);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static resource_size_t init_dpa_allocation(struct nd_label_id *label_id,
+		struct nd_region *nd_region, struct nd_mapping *nd_mapping,
+		resource_size_t n)
+{
+	bool is_blk = strncmp(label_id->id, "blk", 3) == 0;
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	resource_size_t first_dpa;
+	struct resource *res;
+	int rc = 0;
+
+	/* allocate blk from highest dpa first */
+	if (is_blk)
+		first_dpa = nd_mapping->start + nd_mapping->size - n;
+	else
+		first_dpa = nd_mapping->start;
+
+	/* first resource allocation for this label-id or dimm */
+	res = nvdimm_allocate_dpa(ndd, label_id, first_dpa, n);
+	if (!res)
+		rc = -EBUSY;
+
+	nd_dbg_dpa(nd_region, ndd, res, "init %d\n", rc);
+	return rc ? n : 0;
+}
+
+static bool space_valid(bool is_pmem, bool is_reserve,
+		struct nd_label_id *label_id, struct resource *res)
+{
+	/*
+	 * For BLK-space any space is valid, for PMEM-space, it must be
+	 * contiguous with an existing allocation unless we are
+	 * reserving pmem.
+	 */
+	if (is_reserve || !is_pmem)
+		return true;
+	if (!res || strcmp(res->name, label_id->id) == 0)
+		return true;
+	return false;
+}
+
+enum alloc_loc {
+	ALLOC_ERR = 0, ALLOC_BEFORE, ALLOC_MID, ALLOC_AFTER,
+};
+
+static resource_size_t scan_allocate(struct nd_region *nd_region,
+		struct nd_mapping *nd_mapping, struct nd_label_id *label_id,
+		resource_size_t n)
+{
+	resource_size_t mapping_end = nd_mapping->start + nd_mapping->size - 1;
+	bool is_reserve = strcmp(label_id->id, "pmem-reserve") == 0;
+	bool is_pmem = strncmp(label_id->id, "pmem", 4) == 0;
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	const resource_size_t to_allocate = n;
+	struct resource *res;
+	int first;
+
+ retry:
+	first = 0;
+	for_each_dpa_resource(ndd, res) {
+		resource_size_t allocate, available = 0, free_start, free_end;
+		struct resource *next = res->sibling, *new_res = NULL;
+		enum alloc_loc loc = ALLOC_ERR;
+		const char *action;
+		int rc = 0;
+
+		/* ignore resources outside this nd_mapping */
+		if (res->start > mapping_end)
+			continue;
+		if (res->end < nd_mapping->start)
+			continue;
+
+		/* space at the beginning of the mapping */
+		if (!first++ && res->start > nd_mapping->start) {
+			free_start = nd_mapping->start;
+			available = res->start - free_start;
+			if (space_valid(is_pmem, is_reserve, label_id, NULL))
+				loc = ALLOC_BEFORE;
+		}
+
+		/* space between allocations */
+		if (!loc && next) {
+			free_start = res->start + resource_size(res);
+			free_end = min(mapping_end, next->start - 1);
+			if (space_valid(is_pmem, is_reserve, label_id, res)
+					&& free_start < free_end) {
+				available = free_end + 1 - free_start;
+				loc = ALLOC_MID;
+			}
+		}
+
+		/* space at the end of the mapping */
+		if (!loc && !next) {
+			free_start = res->start + resource_size(res);
+			free_end = mapping_end;
+			if (space_valid(is_pmem, is_reserve, label_id, res)
+					&& free_start < free_end) {
+				available = free_end + 1 - free_start;
+				loc = ALLOC_AFTER;
+			}
+		}
+
+		if (!loc || !available)
+			continue;
+		allocate = min(available, n);
+		switch (loc) {
+		case ALLOC_BEFORE:
+			if (strcmp(res->name, label_id->id) == 0) {
+				/* adjust current resource up */
+				if (is_pmem && !is_reserve)
+					return n;
+				rc = adjust_resource(res, res->start - allocate,
+						resource_size(res) + allocate);
+				action = "cur grow up";
+			} else
+				action = "allocate";
+			break;
+		case ALLOC_MID:
+			if (strcmp(next->name, label_id->id) == 0) {
+				/* adjust next resource up */
+				if (is_pmem && !is_reserve)
+					return n;
+				rc = adjust_resource(next, next->start
+						- allocate, resource_size(next)
+						+ allocate);
+				new_res = next;
+				action = "next grow up";
+			} else if (strcmp(res->name, label_id->id) == 0) {
+				action = "grow down";
+			} else
+				action = "allocate";
+			break;
+		case ALLOC_AFTER:
+			if (strcmp(res->name, label_id->id) == 0)
+				action = "grow down";
+			else
+				action = "allocate";
+			break;
+		default:
+			return n;
+		}
+
+		if (strcmp(action, "allocate") == 0) {
+			/* BLK allocate bottom up */
+			if (!is_pmem)
+				free_start += available - allocate;
+			else if (!is_reserve && free_start != nd_mapping->start)
+				return n;
+
+			new_res = nvdimm_allocate_dpa(ndd, label_id,
+					free_start, allocate);
+			if (!new_res)
+				rc = -EBUSY;
+		} else if (strcmp(action, "grow down") == 0) {
+			/* adjust current resource down */
+			rc = adjust_resource(res, res->start, resource_size(res)
+					+ allocate);
+			if (rc == 0)
+				res->flags |= DPA_RESOURCE_ADJUSTED;
+		}
+
+		if (!new_res)
+			new_res = res;
+
+		nd_dbg_dpa(nd_region, ndd, new_res, "%s(%d) %d\n",
+				action, loc, rc);
+
+		if (rc)
+			return n;
+
+		n -= allocate;
+		if (n) {
+			/*
+			 * Retry scan with newly inserted resources.
+			 * For example, if we did an ALLOC_BEFORE
+			 * insertion there may also have been space
+			 * available for an ALLOC_AFTER insertion, so we
+			 * need to check this same resource again
+			 */
+			goto retry;
+		} else
+			return 0;
+	}
+
+	/*
+	 * If we allocated nothing in the BLK case it may be because we are in
+	 * an initial "pmem-reserve pass".  Only do an initial BLK allocation
+	 * when none of the DPA space is reserved.
+	 */
+	if ((is_pmem || !ndd->dpa.child) && n == to_allocate)
+		return init_dpa_allocation(label_id, nd_region, nd_mapping, n);
+	return n;
+}
+
+static int merge_dpa(struct nd_region *nd_region,
+		struct nd_mapping *nd_mapping, struct nd_label_id *label_id)
+{
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	struct resource *res;
+
+	if (strncmp("pmem", label_id->id, 4) == 0)
+		return 0;
+ retry:
+	for_each_dpa_resource(ndd, res) {
+		int rc;
+		struct resource *next = res->sibling;
+		resource_size_t end = res->start + resource_size(res);
+
+		if (!next || strcmp(res->name, label_id->id) != 0
+				|| strcmp(next->name, label_id->id) != 0
+				|| end != next->start)
+			continue;
+		end += resource_size(next);
+		nvdimm_free_dpa(ndd, next);
+		rc = adjust_resource(res, res->start, end - res->start);
+		nd_dbg_dpa(nd_region, ndd, res, "merge %d\n", rc);
+		if (rc)
+			return rc;
+		res->flags |= DPA_RESOURCE_ADJUSTED;
+		goto retry;
+	}
+
+	return 0;
+}
+
+static int __reserve_free_pmem(struct device *dev, void *data)
+{
+	struct nvdimm *nvdimm = data;
+	struct nd_region *nd_region;
+	struct nd_label_id label_id;
+	int i;
+
+	if (!is_nd_pmem(dev))
+		return 0;
+
+	nd_region = to_nd_region(dev);
+	if (nd_region->ndr_mappings == 0)
+		return 0;
+
+	memset(&label_id, 0, sizeof(label_id));
+	strcat(label_id.id, "pmem-reserve");
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		resource_size_t n, rem = 0;
+
+		if (nd_mapping->nvdimm != nvdimm)
+			continue;
+
+		n = nd_pmem_available_dpa(nd_region, nd_mapping, &rem);
+		if (n == 0)
+			return 0;
+		rem = scan_allocate(nd_region, nd_mapping, &label_id, n);
+		dev_WARN_ONCE(&nd_region->dev, rem,
+				"pmem reserve underrun: %#llx of %#llx bytes\n",
+				(unsigned long long) n - rem,
+				(unsigned long long) n);
+		return rem ? -ENXIO : 0;
+	}
+
+	return 0;
+}
+
+static void release_free_pmem(struct nvdimm_bus *nvdimm_bus,
+		struct nd_mapping *nd_mapping)
+{
+	struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+	struct resource *res, *_res;
+
+	for_each_dpa_resource_safe(ndd, res, _res)
+		if (strcmp(res->name, "pmem-reserve") == 0)
+			nvdimm_free_dpa(ndd, res);
+}
+
+static int reserve_free_pmem(struct nvdimm_bus *nvdimm_bus,
+		struct nd_mapping *nd_mapping)
+{
+	struct nvdimm *nvdimm = nd_mapping->nvdimm;
+	int rc;
+
+	rc = device_for_each_child(&nvdimm_bus->dev, nvdimm,
+			__reserve_free_pmem);
+	if (rc)
+		release_free_pmem(nvdimm_bus, nd_mapping);
+	return rc;
+}
+
+/**
+ * grow_dpa_allocation - for each dimm allocate n bytes for @label_id
+ * @nd_region: the set of dimms to allocate @n more bytes from
+ * @label_id: unique identifier for the namespace consuming this dpa range
+ * @n: number of bytes per-dimm to add to the existing allocation
+ *
+ * Assumes resources are ordered.  For BLK regions, first consume
+ * BLK-only available DPA free space, then consume PMEM-aliased DPA
+ * space starting at the highest DPA.  For PMEM regions start
+ * allocations from the start of an interleave set and end at the first
+ * BLK allocation or the end of the interleave set, whichever comes
+ * first.
+ */
+static int grow_dpa_allocation(struct nd_region *nd_region,
+		struct nd_label_id *label_id, resource_size_t n)
+{
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(&nd_region->dev);
+	bool is_pmem = strncmp(label_id->id, "pmem", 4) == 0;
+	int i;
+
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		resource_size_t rem = n;
+		int rc, j;
+
+		/*
+		 * In the BLK case try once with all unallocated PMEM
+		 * reserved, and once without
+		 */
+		for (j = is_pmem; j < 2; j++) {
+			bool blk_only = j == 0;
+
+			if (blk_only) {
+				rc = reserve_free_pmem(nvdimm_bus, nd_mapping);
+				if (rc)
+					return rc;
+			}
+			rem = scan_allocate(nd_region, nd_mapping,
+					label_id, rem);
+			if (blk_only)
+				release_free_pmem(nvdimm_bus, nd_mapping);
+
+			/* try again and allow encroachments into PMEM */
+			if (rem == 0)
+				break;
+		}
+
+		dev_WARN_ONCE(&nd_region->dev, rem,
+				"allocation underrun: %#llx of %#llx bytes\n",
+				(unsigned long long) n - rem,
+				(unsigned long long) n);
+		if (rem)
+			return -ENXIO;
+
+		rc = merge_dpa(nd_region, nd_mapping, label_id);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static void nd_namespace_pmem_set_size(struct nd_region *nd_region,
+		struct nd_namespace_pmem *nspm, resource_size_t size)
+{
+	struct resource *res = &nspm->nsio.res;
+
+	res->start = nd_region->ndr_start;
+	res->end = nd_region->ndr_start + size - 1;
+}
+
+static ssize_t __size_store(struct device *dev, unsigned long long val)
+{
+	resource_size_t allocated = 0, available = 0;
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	struct nd_mapping *nd_mapping;
+	struct nvdimm_drvdata *ndd;
+	struct nd_label_id label_id;
+	u32 flags = 0, remainder;
+	u8 *uuid = NULL;
+	int rc, i;
+
+	if (dev->driver || to_ndns(dev)->claim)
+		return -EBUSY;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		uuid = nspm->uuid;
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		uuid = nsblk->uuid;
+		flags = NSLABEL_FLAG_LOCAL;
+	}
+
+	/*
+	 * We need a uuid for the allocation-label and dimm(s) on which
+	 * to store the label.
+	 */
+	if (!uuid || nd_region->ndr_mappings == 0)
+		return -ENXIO;
+
+	div_u64_rem(val, SZ_4K * nd_region->ndr_mappings, &remainder);
+	if (remainder) {
+		dev_dbg(dev, "%llu is not %dK aligned\n", val,
+				(SZ_4K * nd_region->ndr_mappings) / SZ_1K);
+		return -EINVAL;
+	}
+
+	nd_label_gen_id(&label_id, uuid, flags);
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		nd_mapping = &nd_region->mapping[i];
+		ndd = to_ndd(nd_mapping);
+
+		/*
+		 * All dimms in an interleave set, or the base dimm for a blk
+		 * region, need to be enabled for the size to be changed.
+		 */
+		if (!ndd)
+			return -ENXIO;
+
+		allocated += nvdimm_allocated_dpa(ndd, &label_id);
+	}
+	available = nd_region_available_dpa(nd_region);
+
+	if (val > available + allocated)
+		return -ENOSPC;
+
+	if (val == allocated)
+		return 0;
+
+	val = div_u64(val, nd_region->ndr_mappings);
+	allocated = div_u64(allocated, nd_region->ndr_mappings);
+	if (val < allocated)
+		rc = shrink_dpa_allocation(nd_region, &label_id,
+				allocated - val);
+	else
+		rc = grow_dpa_allocation(nd_region, &label_id, val - allocated);
+
+	if (rc)
+		return rc;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		nd_namespace_pmem_set_size(nd_region, nspm,
+				val * nd_region->ndr_mappings);
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		/*
+		 * Try to delete the namespace if we deleted all of its
+		 * allocation, this is not the seed device for the
+		 * region, and it is not actively claimed by a btt
+		 * instance.
+		 */
+		if (val == 0 && nd_region->ns_seed != dev
+				&& !nsblk->common.claim)
+			nd_device_unregister(dev, ND_ASYNC);
+	}
+
+	return rc;
+}
+
+static ssize_t size_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	unsigned long long val;
+	u8 **uuid = NULL;
+	int rc;
+
+	rc = kstrtoull(buf, 0, &val);
+	if (rc)
+		return rc;
+
+	device_lock(dev);
+	nvdimm_bus_lock(dev);
+	wait_nvdimm_bus_probe_idle(dev);
+	rc = __size_store(dev, val);
+	if (rc >= 0)
+		rc = nd_namespace_label_update(nd_region, dev);
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		uuid = &nspm->uuid;
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		uuid = &nsblk->uuid;
+	}
+
+	if (rc == 0 && val == 0 && uuid) {
+		/* setting size zero == 'delete namespace' */
+		kfree(*uuid);
+		*uuid = NULL;
+	}
+
+	dev_dbg(dev, "%s: %llx %s (%d)\n", __func__, val, rc < 0
+			? "fail" : "success", rc);
+
+	nvdimm_bus_unlock(dev);
+	device_unlock(dev);
+
+	return rc < 0 ? rc : len;
+}
+
+resource_size_t __nvdimm_namespace_capacity(struct nd_namespace_common *ndns)
+{
+	struct device *dev = &ndns->dev;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		return resource_size(&nspm->nsio.res);
+	} else if (is_namespace_blk(dev)) {
+		return nd_namespace_blk_size(to_nd_namespace_blk(dev));
+	} else if (is_namespace_io(dev)) {
+		struct nd_namespace_io *nsio = to_nd_namespace_io(dev);
+
+		return resource_size(&nsio->res);
+	} else
+		WARN_ONCE(1, "unknown namespace type\n");
+	return 0;
+}
+
+resource_size_t nvdimm_namespace_capacity(struct nd_namespace_common *ndns)
+{
+	resource_size_t size;
+
+	nvdimm_bus_lock(&ndns->dev);
+	size = __nvdimm_namespace_capacity(ndns);
+	nvdimm_bus_unlock(&ndns->dev);
+
+	return size;
+}
+EXPORT_SYMBOL(nvdimm_namespace_capacity);
+
+static ssize_t size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%llu\n", (unsigned long long)
+			nvdimm_namespace_capacity(to_ndns(dev)));
+}
+static DEVICE_ATTR(size, S_IRUGO, size_show, size_store);
+
+static ssize_t uuid_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	u8 *uuid;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		uuid = nspm->uuid;
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		uuid = nsblk->uuid;
+	} else
+		return -ENXIO;
+
+	if (uuid)
+		return sprintf(buf, "%pUb\n", uuid);
+	return sprintf(buf, "\n");
+}
+
+/**
+ * namespace_update_uuid - check for a unique uuid and whether we're "renaming"
+ * @nd_region: parent region so we can updates all dimms in the set
+ * @dev: namespace type for generating label_id
+ * @new_uuid: incoming uuid
+ * @old_uuid: reference to the uuid storage location in the namespace object
+ */
+static int namespace_update_uuid(struct nd_region *nd_region,
+		struct device *dev, u8 *new_uuid, u8 **old_uuid)
+{
+	u32 flags = is_namespace_blk(dev) ? NSLABEL_FLAG_LOCAL : 0;
+	struct nd_label_id old_label_id;
+	struct nd_label_id new_label_id;
+	int i;
+
+	if (!nd_is_uuid_unique(dev, new_uuid))
+		return -EINVAL;
+
+	if (*old_uuid == NULL)
+		goto out;
+
+	/*
+	 * If we've already written a label with this uuid, then it's
+	 * too late to rename because we can't reliably update the uuid
+	 * without losing the old namespace.  Userspace must delete this
+	 * namespace to abandon the old uuid.
+	 */
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+
+		/*
+		 * This check by itself is sufficient because old_uuid
+		 * would be NULL above if this uuid did not exist in the
+		 * currently written set.
+		 *
+		 * FIXME: can we delete uuid with zero dpa allocated?
+		 */
+		if (nd_mapping->labels)
+			return -EBUSY;
+	}
+
+	nd_label_gen_id(&old_label_id, *old_uuid, flags);
+	nd_label_gen_id(&new_label_id, new_uuid, flags);
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+		struct resource *res;
+
+		for_each_dpa_resource(ndd, res)
+			if (strcmp(res->name, old_label_id.id) == 0)
+				sprintf((void *) res->name, "%s",
+						new_label_id.id);
+	}
+	kfree(*old_uuid);
+ out:
+	*old_uuid = new_uuid;
+	return 0;
+}
+
+static ssize_t uuid_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	u8 *uuid = NULL;
+	ssize_t rc = 0;
+	u8 **ns_uuid;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		ns_uuid = &nspm->uuid;
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		ns_uuid = &nsblk->uuid;
+	} else
+		return -ENXIO;
+
+	device_lock(dev);
+	nvdimm_bus_lock(dev);
+	wait_nvdimm_bus_probe_idle(dev);
+	if (to_ndns(dev)->claim)
+		rc = -EBUSY;
+	if (rc >= 0)
+		rc = nd_uuid_store(dev, &uuid, buf, len);
+	if (rc >= 0)
+		rc = namespace_update_uuid(nd_region, dev, uuid, ns_uuid);
+	if (rc >= 0)
+		rc = nd_namespace_label_update(nd_region, dev);
+	else
+		kfree(uuid);
+	dev_dbg(dev, "%s: result: %zd wrote: %s%s", __func__,
+			rc, buf, buf[len - 1] == '\n' ? "" : "\n");
+	nvdimm_bus_unlock(dev);
+	device_unlock(dev);
+
+	return rc < 0 ? rc : len;
+}
+static DEVICE_ATTR_RW(uuid);
+
+static ssize_t resource_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct resource *res;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		res = &nspm->nsio.res;
+	} else if (is_namespace_io(dev)) {
+		struct nd_namespace_io *nsio = to_nd_namespace_io(dev);
+
+		res = &nsio->res;
+	} else
+		return -ENXIO;
+
+	/* no address to convey if the namespace has no allocation */
+	if (resource_size(res) == 0)
+		return -ENXIO;
+	return sprintf(buf, "%#llx\n", (unsigned long long) res->start);
+}
+static DEVICE_ATTR_RO(resource);
+
+static const unsigned long ns_lbasize_supported[] = { 512, 520, 528,
+	4096, 4104, 4160, 4224, 0 };
+
+static ssize_t sector_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+	if (!is_namespace_blk(dev))
+		return -ENXIO;
+
+	return nd_sector_size_show(nsblk->lbasize, ns_lbasize_supported, buf);
+}
+
+static ssize_t sector_size_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	ssize_t rc = 0;
+
+	if (!is_namespace_blk(dev))
+		return -ENXIO;
+
+	device_lock(dev);
+	nvdimm_bus_lock(dev);
+	if (to_ndns(dev)->claim)
+		rc = -EBUSY;
+	if (rc >= 0)
+		rc = nd_sector_size_store(dev, buf, &nsblk->lbasize,
+				ns_lbasize_supported);
+	if (rc >= 0)
+		rc = nd_namespace_label_update(nd_region, dev);
+	dev_dbg(dev, "%s: result: %zd %s: %s%s", __func__,
+			rc, rc < 0 ? "tried" : "wrote", buf,
+			buf[len - 1] == '\n' ? "" : "\n");
+	nvdimm_bus_unlock(dev);
+	device_unlock(dev);
+
+	return rc ? rc : len;
+}
+static DEVICE_ATTR_RW(sector_size);
+
+static ssize_t dpa_extents_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	struct nd_label_id label_id;
+	int count = 0, i;
+	u8 *uuid = NULL;
+	u32 flags = 0;
+
+	nvdimm_bus_lock(dev);
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		uuid = nspm->uuid;
+		flags = 0;
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		uuid = nsblk->uuid;
+		flags = NSLABEL_FLAG_LOCAL;
+	}
+
+	if (!uuid)
+		goto out;
+
+	nd_label_gen_id(&label_id, uuid, flags);
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+		struct resource *res;
+
+		for_each_dpa_resource(ndd, res)
+			if (strcmp(res->name, label_id.id) == 0)
+				count++;
+	}
+ out:
+	nvdimm_bus_unlock(dev);
+
+	return sprintf(buf, "%d\n", count);
+}
+static DEVICE_ATTR_RO(dpa_extents);
+
+static ssize_t holder_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_namespace_common *ndns = to_ndns(dev);
+	ssize_t rc;
+
+	device_lock(dev);
+	rc = sprintf(buf, "%s\n", ndns->claim ? dev_name(ndns->claim) : "");
+	device_unlock(dev);
+
+	return rc;
+}
+static DEVICE_ATTR_RO(holder);
+
+static ssize_t force_raw_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	bool force_raw;
+	int rc = strtobool(buf, &force_raw);
+
+	if (rc)
+		return rc;
+
+	to_ndns(dev)->force_raw = force_raw;
+	return len;
+}
+
+static ssize_t force_raw_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d\n", to_ndns(dev)->force_raw);
+}
+static DEVICE_ATTR_RW(force_raw);
+
+static struct attribute *nd_namespace_attributes[] = {
+	&dev_attr_nstype.attr,
+	&dev_attr_size.attr,
+	&dev_attr_uuid.attr,
+	&dev_attr_holder.attr,
+	&dev_attr_resource.attr,
+	&dev_attr_alt_name.attr,
+	&dev_attr_force_raw.attr,
+	&dev_attr_sector_size.attr,
+	&dev_attr_dpa_extents.attr,
+	NULL,
+};
+
+static umode_t namespace_visible(struct kobject *kobj,
+		struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+
+	if (a == &dev_attr_resource.attr) {
+		if (is_namespace_blk(dev))
+			return 0;
+		return a->mode;
+	}
+
+	if (is_namespace_pmem(dev) || is_namespace_blk(dev)) {
+		if (a == &dev_attr_size.attr)
+			return S_IWUSR | S_IRUGO;
+
+		if (is_namespace_pmem(dev) && a == &dev_attr_sector_size.attr)
+			return 0;
+
+		return a->mode;
+	}
+
+	if (a == &dev_attr_nstype.attr || a == &dev_attr_size.attr
+			|| a == &dev_attr_holder.attr
+			|| a == &dev_attr_force_raw.attr)
+		return a->mode;
+
+	return 0;
+}
+
+static struct attribute_group nd_namespace_attribute_group = {
+	.attrs = nd_namespace_attributes,
+	.is_visible = namespace_visible,
+};
+
+static const struct attribute_group *nd_namespace_attribute_groups[] = {
+	&nd_device_attribute_group,
+	&nd_namespace_attribute_group,
+	&nd_numa_attribute_group,
+	NULL,
+};
+
+struct nd_namespace_common *nvdimm_namespace_common_probe(struct device *dev)
+{
+	struct nd_btt *nd_btt = is_nd_btt(dev) ? to_nd_btt(dev) : NULL;
+	struct nd_namespace_common *ndns;
+	resource_size_t size;
+
+	if (nd_btt) {
+		ndns = nd_btt->ndns;
+		if (!ndns)
+			return ERR_PTR(-ENODEV);
+
+		/*
+		 * Flush any in-progess probes / removals in the driver
+		 * for the raw personality of this namespace.
+		 */
+		device_lock(&ndns->dev);
+		device_unlock(&ndns->dev);
+		if (ndns->dev.driver) {
+			dev_dbg(&ndns->dev, "is active, can't bind %s\n",
+					dev_name(&nd_btt->dev));
+			return ERR_PTR(-EBUSY);
+		}
+		if (dev_WARN_ONCE(&ndns->dev, ndns->claim != &nd_btt->dev,
+					"host (%s) vs claim (%s) mismatch\n",
+					dev_name(&nd_btt->dev),
+					dev_name(ndns->claim)))
+			return ERR_PTR(-ENXIO);
+	} else {
+		ndns = to_ndns(dev);
+		if (ndns->claim) {
+			dev_dbg(dev, "claimed by %s, failing probe\n",
+				dev_name(ndns->claim));
+
+			return ERR_PTR(-ENXIO);
+		}
+	}
+
+	size = nvdimm_namespace_capacity(ndns);
+	if (size < ND_MIN_NAMESPACE_SIZE) {
+		dev_dbg(&ndns->dev, "%pa, too small must be at least %#x\n",
+				&size, ND_MIN_NAMESPACE_SIZE);
+		return ERR_PTR(-ENODEV);
+	}
+
+	if (is_namespace_pmem(&ndns->dev)) {
+		struct nd_namespace_pmem *nspm;
+
+		nspm = to_nd_namespace_pmem(&ndns->dev);
+		if (!nspm->uuid) {
+			dev_dbg(&ndns->dev, "%s: uuid not set\n", __func__);
+			return ERR_PTR(-ENODEV);
+		}
+	} else if (is_namespace_blk(&ndns->dev)) {
+		struct nd_namespace_blk *nsblk;
+
+		nsblk = to_nd_namespace_blk(&ndns->dev);
+		if (!nd_namespace_blk_validate(nsblk))
+			return ERR_PTR(-ENODEV);
+	}
+
+	return ndns;
+}
+EXPORT_SYMBOL(nvdimm_namespace_common_probe);
+
+static struct device **create_namespace_io(struct nd_region *nd_region)
+{
+	struct nd_namespace_io *nsio;
+	struct device *dev, **devs;
+	struct resource *res;
+
+	nsio = kzalloc(sizeof(*nsio), GFP_KERNEL);
+	if (!nsio)
+		return NULL;
+
+	devs = kcalloc(2, sizeof(struct device *), GFP_KERNEL);
+	if (!devs) {
+		kfree(nsio);
+		return NULL;
+	}
+
+	dev = &nsio->common.dev;
+	dev->type = &namespace_io_device_type;
+	dev->parent = &nd_region->dev;
+	res = &nsio->res;
+	res->name = dev_name(&nd_region->dev);
+	res->flags = IORESOURCE_MEM;
+	res->start = nd_region->ndr_start;
+	res->end = res->start + nd_region->ndr_size - 1;
+
+	devs[0] = dev;
+	return devs;
+}
+
+static bool has_uuid_at_pos(struct nd_region *nd_region, u8 *uuid,
+		u64 cookie, u16 pos)
+{
+	struct nd_namespace_label *found = NULL;
+	int i;
+
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		struct nd_namespace_label *nd_label;
+		bool found_uuid = false;
+		int l;
+
+		for_each_label(l, nd_label, nd_mapping->labels) {
+			u64 isetcookie = __le64_to_cpu(nd_label->isetcookie);
+			u16 position = __le16_to_cpu(nd_label->position);
+			u16 nlabel = __le16_to_cpu(nd_label->nlabel);
+
+			if (isetcookie != cookie)
+				continue;
+
+			if (memcmp(nd_label->uuid, uuid, NSLABEL_UUID_LEN) != 0)
+				continue;
+
+			if (found_uuid) {
+				dev_dbg(to_ndd(nd_mapping)->dev,
+						"%s duplicate entry for uuid\n",
+						__func__);
+				return false;
+			}
+			found_uuid = true;
+			if (nlabel != nd_region->ndr_mappings)
+				continue;
+			if (position != pos)
+				continue;
+			found = nd_label;
+			break;
+		}
+		if (found)
+			break;
+	}
+	return found != NULL;
+}
+
+static int select_pmem_id(struct nd_region *nd_region, u8 *pmem_id)
+{
+	struct nd_namespace_label *select = NULL;
+	int i;
+
+	if (!pmem_id)
+		return -ENODEV;
+
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		struct nd_namespace_label *nd_label;
+		u64 hw_start, hw_end, pmem_start, pmem_end;
+		int l;
+
+		for_each_label(l, nd_label, nd_mapping->labels)
+			if (memcmp(nd_label->uuid, pmem_id, NSLABEL_UUID_LEN) == 0)
+				break;
+
+		if (!nd_label) {
+			WARN_ON(1);
+			return -EINVAL;
+		}
+
+		select = nd_label;
+		/*
+		 * Check that this label is compliant with the dpa
+		 * range published in NFIT
+		 */
+		hw_start = nd_mapping->start;
+		hw_end = hw_start + nd_mapping->size;
+		pmem_start = __le64_to_cpu(select->dpa);
+		pmem_end = pmem_start + __le64_to_cpu(select->rawsize);
+		if (pmem_start == hw_start && pmem_end <= hw_end)
+			/* pass */;
+		else
+			return -EINVAL;
+
+		nd_mapping->labels[0] = select;
+		nd_mapping->labels[1] = NULL;
+	}
+	return 0;
+}
+
+/**
+ * find_pmem_label_set - validate interleave set labelling, retrieve label0
+ * @nd_region: region with mappings to validate
+ */
+static int find_pmem_label_set(struct nd_region *nd_region,
+		struct nd_namespace_pmem *nspm)
+{
+	u64 cookie = nd_region_interleave_set_cookie(nd_region);
+	struct nd_namespace_label *nd_label;
+	u8 select_id[NSLABEL_UUID_LEN];
+	resource_size_t size = 0;
+	u8 *pmem_id = NULL;
+	int rc = -ENODEV, l;
+	u16 i;
+
+	if (cookie == 0)
+		return -ENXIO;
+
+	/*
+	 * Find a complete set of labels by uuid.  By definition we can start
+	 * with any mapping as the reference label
+	 */
+	for_each_label(l, nd_label, nd_region->mapping[0].labels) {
+		u64 isetcookie = __le64_to_cpu(nd_label->isetcookie);
+
+		if (isetcookie != cookie)
+			continue;
+
+		for (i = 0; nd_region->ndr_mappings; i++)
+			if (!has_uuid_at_pos(nd_region, nd_label->uuid,
+						cookie, i))
+				break;
+		if (i < nd_region->ndr_mappings) {
+			/*
+			 * Give up if we don't find an instance of a
+			 * uuid at each position (from 0 to
+			 * nd_region->ndr_mappings - 1), or if we find a
+			 * dimm with two instances of the same uuid.
+			 */
+			rc = -EINVAL;
+			goto err;
+		} else if (pmem_id) {
+			/*
+			 * If there is more than one valid uuid set, we
+			 * need userspace to clean this up.
+			 */
+			rc = -EBUSY;
+			goto err;
+		}
+		memcpy(select_id, nd_label->uuid, NSLABEL_UUID_LEN);
+		pmem_id = select_id;
+	}
+
+	/*
+	 * Fix up each mapping's 'labels' to have the validated pmem label for
+	 * that position at labels[0], and NULL at labels[1].  In the process,
+	 * check that the namespace aligns with interleave-set.  We know
+	 * that it does not overlap with any blk namespaces by virtue of
+	 * the dimm being enabled (i.e. nd_label_reserve_dpa()
+	 * succeeded).
+	 */
+	rc = select_pmem_id(nd_region, pmem_id);
+	if (rc)
+		goto err;
+
+	/* Calculate total size and populate namespace properties from label0 */
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		struct nd_namespace_label *label0 = nd_mapping->labels[0];
+
+		size += __le64_to_cpu(label0->rawsize);
+		if (__le16_to_cpu(label0->position) != 0)
+			continue;
+		WARN_ON(nspm->alt_name || nspm->uuid);
+		nspm->alt_name = kmemdup((void __force *) label0->name,
+				NSLABEL_NAME_LEN, GFP_KERNEL);
+		nspm->uuid = kmemdup((void __force *) label0->uuid,
+				NSLABEL_UUID_LEN, GFP_KERNEL);
+	}
+
+	if (!nspm->alt_name || !nspm->uuid) {
+		rc = -ENOMEM;
+		goto err;
+	}
+
+	nd_namespace_pmem_set_size(nd_region, nspm, size);
+
+	return 0;
+ err:
+	switch (rc) {
+	case -EINVAL:
+		dev_dbg(&nd_region->dev, "%s: invalid label(s)\n", __func__);
+		break;
+	case -ENODEV:
+		dev_dbg(&nd_region->dev, "%s: label not found\n", __func__);
+		break;
+	default:
+		dev_dbg(&nd_region->dev, "%s: unexpected err: %d\n",
+				__func__, rc);
+		break;
+	}
+	return rc;
+}
+
+static struct device **create_namespace_pmem(struct nd_region *nd_region)
+{
+	struct nd_namespace_pmem *nspm;
+	struct device *dev, **devs;
+	struct resource *res;
+	int rc;
+
+	nspm = kzalloc(sizeof(*nspm), GFP_KERNEL);
+	if (!nspm)
+		return NULL;
+
+	dev = &nspm->nsio.common.dev;
+	dev->type = &namespace_pmem_device_type;
+	dev->parent = &nd_region->dev;
+	res = &nspm->nsio.res;
+	res->name = dev_name(&nd_region->dev);
+	res->flags = IORESOURCE_MEM;
+	rc = find_pmem_label_set(nd_region, nspm);
+	if (rc == -ENODEV) {
+		int i;
+
+		/* Pass, try to permit namespace creation... */
+		for (i = 0; i < nd_region->ndr_mappings; i++) {
+			struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+
+			kfree(nd_mapping->labels);
+			nd_mapping->labels = NULL;
+		}
+
+		/* Publish a zero-sized namespace for userspace to configure. */
+		nd_namespace_pmem_set_size(nd_region, nspm, 0);
+
+		rc = 0;
+	} else if (rc)
+		goto err;
+
+	devs = kcalloc(2, sizeof(struct device *), GFP_KERNEL);
+	if (!devs)
+		goto err;
+
+	devs[0] = dev;
+	return devs;
+
+ err:
+	namespace_pmem_release(&nspm->nsio.common.dev);
+	return NULL;
+}
+
+struct resource *nsblk_add_resource(struct nd_region *nd_region,
+		struct nvdimm_drvdata *ndd, struct nd_namespace_blk *nsblk,
+		resource_size_t start)
+{
+	struct nd_label_id label_id;
+	struct resource *res;
+
+	nd_label_gen_id(&label_id, nsblk->uuid, NSLABEL_FLAG_LOCAL);
+	res = krealloc(nsblk->res,
+			sizeof(void *) * (nsblk->num_resources + 1),
+			GFP_KERNEL);
+	if (!res)
+		return NULL;
+	nsblk->res = (struct resource **) res;
+	for_each_dpa_resource(ndd, res)
+		if (strcmp(res->name, label_id.id) == 0
+				&& res->start == start) {
+			nsblk->res[nsblk->num_resources++] = res;
+			return res;
+		}
+	return NULL;
+}
+
+static struct device *nd_namespace_blk_create(struct nd_region *nd_region)
+{
+	struct nd_namespace_blk *nsblk;
+	struct device *dev;
+
+	if (!is_nd_blk(&nd_region->dev))
+		return NULL;
+
+	nsblk = kzalloc(sizeof(*nsblk), GFP_KERNEL);
+	if (!nsblk)
+		return NULL;
+
+	dev = &nsblk->common.dev;
+	dev->type = &namespace_blk_device_type;
+	nsblk->id = ida_simple_get(&nd_region->ns_ida, 0, 0, GFP_KERNEL);
+	if (nsblk->id < 0) {
+		kfree(nsblk);
+		return NULL;
+	}
+	dev_set_name(dev, "namespace%d.%d", nd_region->id, nsblk->id);
+	dev->parent = &nd_region->dev;
+	dev->groups = nd_namespace_attribute_groups;
+
+	return &nsblk->common.dev;
+}
+
+void nd_region_create_blk_seed(struct nd_region *nd_region)
+{
+	WARN_ON(!is_nvdimm_bus_locked(&nd_region->dev));
+	nd_region->ns_seed = nd_namespace_blk_create(nd_region);
+	/*
+	 * Seed creation failures are not fatal, provisioning is simply
+	 * disabled until memory becomes available
+	 */
+	if (!nd_region->ns_seed)
+		dev_err(&nd_region->dev, "failed to create blk namespace\n");
+	else
+		nd_device_register(nd_region->ns_seed);
+}
+
+void nd_region_create_btt_seed(struct nd_region *nd_region)
+{
+	WARN_ON(!is_nvdimm_bus_locked(&nd_region->dev));
+	nd_region->btt_seed = nd_btt_create(nd_region);
+	/*
+	 * Seed creation failures are not fatal, provisioning is simply
+	 * disabled until memory becomes available
+	 */
+	if (!nd_region->btt_seed)
+		dev_err(&nd_region->dev, "failed to create btt namespace\n");
+}
+
+static struct device **create_namespace_blk(struct nd_region *nd_region)
+{
+	struct nd_mapping *nd_mapping = &nd_region->mapping[0];
+	struct nd_namespace_label *nd_label;
+	struct device *dev, **devs = NULL;
+	struct nd_namespace_blk *nsblk;
+	struct nvdimm_drvdata *ndd;
+	int i, l, count = 0;
+	struct resource *res;
+
+	if (nd_region->ndr_mappings == 0)
+		return NULL;
+
+	ndd = to_ndd(nd_mapping);
+	for_each_label(l, nd_label, nd_mapping->labels) {
+		u32 flags = __le32_to_cpu(nd_label->flags);
+		char *name[NSLABEL_NAME_LEN];
+		struct device **__devs;
+
+		if (flags & NSLABEL_FLAG_LOCAL)
+			/* pass */;
+		else
+			continue;
+
+		for (i = 0; i < count; i++) {
+			nsblk = to_nd_namespace_blk(devs[i]);
+			if (memcmp(nsblk->uuid, nd_label->uuid,
+						NSLABEL_UUID_LEN) == 0) {
+				res = nsblk_add_resource(nd_region, ndd, nsblk,
+						__le64_to_cpu(nd_label->dpa));
+				if (!res)
+					goto err;
+				nd_dbg_dpa(nd_region, ndd, res, "%s assign\n",
+					dev_name(&nsblk->common.dev));
+				break;
+			}
+		}
+		if (i < count)
+			continue;
+		__devs = kcalloc(count + 2, sizeof(dev), GFP_KERNEL);
+		if (!__devs)
+			goto err;
+		memcpy(__devs, devs, sizeof(dev) * count);
+		kfree(devs);
+		devs = __devs;
+
+		nsblk = kzalloc(sizeof(*nsblk), GFP_KERNEL);
+		if (!nsblk)
+			goto err;
+		dev = &nsblk->common.dev;
+		dev->type = &namespace_blk_device_type;
+		dev->parent = &nd_region->dev;
+		dev_set_name(dev, "namespace%d.%d", nd_region->id, count);
+		devs[count++] = dev;
+		nsblk->id = -1;
+		nsblk->lbasize = __le64_to_cpu(nd_label->lbasize);
+		nsblk->uuid = kmemdup(nd_label->uuid, NSLABEL_UUID_LEN,
+				GFP_KERNEL);
+		if (!nsblk->uuid)
+			goto err;
+		memcpy(name, nd_label->name, NSLABEL_NAME_LEN);
+		if (name[0])
+			nsblk->alt_name = kmemdup(name, NSLABEL_NAME_LEN,
+					GFP_KERNEL);
+		res = nsblk_add_resource(nd_region, ndd, nsblk,
+				__le64_to_cpu(nd_label->dpa));
+		if (!res)
+			goto err;
+		nd_dbg_dpa(nd_region, ndd, res, "%s assign\n",
+				dev_name(&nsblk->common.dev));
+	}
+
+	dev_dbg(&nd_region->dev, "%s: discovered %d blk namespace%s\n",
+			__func__, count, count == 1 ? "" : "s");
+
+	if (count == 0) {
+		/* Publish a zero-sized namespace for userspace to configure. */
+		for (i = 0; i < nd_region->ndr_mappings; i++) {
+			struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+
+			kfree(nd_mapping->labels);
+			nd_mapping->labels = NULL;
+		}
+
+		devs = kcalloc(2, sizeof(dev), GFP_KERNEL);
+		if (!devs)
+			goto err;
+		nsblk = kzalloc(sizeof(*nsblk), GFP_KERNEL);
+		if (!nsblk)
+			goto err;
+		dev = &nsblk->common.dev;
+		dev->type = &namespace_blk_device_type;
+		dev->parent = &nd_region->dev;
+		devs[count++] = dev;
+	}
+
+	return devs;
+
+err:
+	for (i = 0; i < count; i++) {
+		nsblk = to_nd_namespace_blk(devs[i]);
+		namespace_blk_release(&nsblk->common.dev);
+	}
+	kfree(devs);
+	return NULL;
+}
+
+static int init_active_labels(struct nd_region *nd_region)
+{
+	int i;
+
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+		struct nvdimm *nvdimm = nd_mapping->nvdimm;
+		int count, j;
+
+		/*
+		 * If the dimm is disabled then prevent the region from
+		 * being activated if it aliases DPA.
+		 */
+		if (!ndd) {
+			if ((nvdimm->flags & NDD_ALIASING) == 0)
+				return 0;
+			dev_dbg(&nd_region->dev, "%s: is disabled, failing probe\n",
+					dev_name(&nd_mapping->nvdimm->dev));
+			return -ENXIO;
+		}
+		nd_mapping->ndd = ndd;
+		atomic_inc(&nvdimm->busy);
+		get_ndd(ndd);
+
+		count = nd_label_active_count(ndd);
+		dev_dbg(ndd->dev, "%s: %d\n", __func__, count);
+		if (!count)
+			continue;
+		nd_mapping->labels = kcalloc(count + 1, sizeof(void *),
+				GFP_KERNEL);
+		if (!nd_mapping->labels)
+			return -ENOMEM;
+		for (j = 0; j < count; j++) {
+			struct nd_namespace_label *label;
+
+			label = nd_label_active(ndd, j);
+			nd_mapping->labels[j] = label;
+		}
+	}
+
+	return 0;
+}
+
+int nd_region_register_namespaces(struct nd_region *nd_region, int *err)
+{
+	struct device **devs = NULL;
+	int i, rc = 0, type;
+
+	*err = 0;
+	nvdimm_bus_lock(&nd_region->dev);
+	rc = init_active_labels(nd_region);
+	if (rc) {
+		nvdimm_bus_unlock(&nd_region->dev);
+		return rc;
+	}
+
+	type = nd_region_to_nstype(nd_region);
+	switch (type) {
+	case ND_DEVICE_NAMESPACE_IO:
+		devs = create_namespace_io(nd_region);
+		break;
+	case ND_DEVICE_NAMESPACE_PMEM:
+		devs = create_namespace_pmem(nd_region);
+		break;
+	case ND_DEVICE_NAMESPACE_BLK:
+		devs = create_namespace_blk(nd_region);
+		break;
+	default:
+		break;
+	}
+	nvdimm_bus_unlock(&nd_region->dev);
+
+	if (!devs)
+		return -ENODEV;
+
+	for (i = 0; devs[i]; i++) {
+		struct device *dev = devs[i];
+		int id;
+
+		if (type == ND_DEVICE_NAMESPACE_BLK) {
+			struct nd_namespace_blk *nsblk;
+
+			nsblk = to_nd_namespace_blk(dev);
+			id = ida_simple_get(&nd_region->ns_ida, 0, 0,
+					GFP_KERNEL);
+			nsblk->id = id;
+		} else
+			id = i;
+
+		if (id < 0)
+			break;
+		dev_set_name(dev, "namespace%d.%d", nd_region->id, id);
+		dev->groups = nd_namespace_attribute_groups;
+		nd_device_register(dev);
+	}
+	if (i)
+		nd_region->ns_seed = devs[0];
+
+	if (devs[i]) {
+		int j;
+
+		for (j = i; devs[j]; j++) {
+			struct device *dev = devs[j];
+
+			device_initialize(dev);
+			put_device(dev);
+		}
+		*err = j - i;
+		/*
+		 * All of the namespaces we tried to register failed, so
+		 * fail region activation.
+		 */
+		if (*err == 0)
+			rc = -ENODEV;
+	}
+	kfree(devs);
+
+	if (rc == -ENODEV)
+		return rc;
+
+	return i;
+}
diff --git a/drivers/nvdimm/nd-core.h b/drivers/nvdimm/nd-core.h
new file mode 100644
index 000000000000..e1970c71ad1c
--- /dev/null
+++ b/drivers/nvdimm/nd-core.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#ifndef __ND_CORE_H__
+#define __ND_CORE_H__
+#include <linux/libnvdimm.h>
+#include <linux/device.h>
+#include <linux/libnvdimm.h>
+#include <linux/sizes.h>
+#include <linux/mutex.h>
+#include <linux/nd.h>
+
+extern struct list_head nvdimm_bus_list;
+extern struct mutex nvdimm_bus_list_mutex;
+extern int nvdimm_major;
+
+struct nvdimm_bus {
+	struct nvdimm_bus_descriptor *nd_desc;
+	wait_queue_head_t probe_wait;
+	struct module *module;
+	struct list_head list;
+	struct device dev;
+	int id, probe_active;
+	struct mutex reconfig_mutex;
+};
+
+struct nvdimm {
+	unsigned long flags;
+	void *provider_data;
+	unsigned long *dsm_mask;
+	struct device dev;
+	atomic_t busy;
+	int id;
+};
+
+bool is_nvdimm(struct device *dev);
+bool is_nd_pmem(struct device *dev);
+bool is_nd_blk(struct device *dev);
+struct nvdimm_bus *walk_to_nvdimm_bus(struct device *nd_dev);
+int __init nvdimm_bus_init(void);
+void nvdimm_bus_exit(void);
+void nd_region_probe_success(struct nvdimm_bus *nvdimm_bus, struct device *dev);
+struct nd_region;
+void nd_region_create_blk_seed(struct nd_region *nd_region);
+void nd_region_create_btt_seed(struct nd_region *nd_region);
+void nd_region_disable(struct nvdimm_bus *nvdimm_bus, struct device *dev);
+int nvdimm_bus_create_ndctl(struct nvdimm_bus *nvdimm_bus);
+void nvdimm_bus_destroy_ndctl(struct nvdimm_bus *nvdimm_bus);
+void nd_synchronize(void);
+int nvdimm_bus_register_dimms(struct nvdimm_bus *nvdimm_bus);
+int nvdimm_bus_register_regions(struct nvdimm_bus *nvdimm_bus);
+int nvdimm_bus_init_interleave_sets(struct nvdimm_bus *nvdimm_bus);
+void __nd_device_register(struct device *dev);
+int nd_match_dimm(struct device *dev, void *data);
+struct nd_label_id;
+char *nd_label_gen_id(struct nd_label_id *label_id, u8 *uuid, u32 flags);
+bool nd_is_uuid_unique(struct device *dev, u8 *uuid);
+struct nd_region;
+struct nvdimm_drvdata;
+struct nd_mapping;
+resource_size_t nd_pmem_available_dpa(struct nd_region *nd_region,
+		struct nd_mapping *nd_mapping, resource_size_t *overlap);
+resource_size_t nd_blk_available_dpa(struct nd_mapping *nd_mapping);
+resource_size_t nd_region_available_dpa(struct nd_region *nd_region);
+resource_size_t nvdimm_allocated_dpa(struct nvdimm_drvdata *ndd,
+		struct nd_label_id *label_id);
+struct nd_mapping;
+struct resource *nsblk_add_resource(struct nd_region *nd_region,
+		struct nvdimm_drvdata *ndd, struct nd_namespace_blk *nsblk,
+		resource_size_t start);
+int nvdimm_num_label_slots(struct nvdimm_drvdata *ndd);
+void get_ndd(struct nvdimm_drvdata *ndd);
+resource_size_t __nvdimm_namespace_capacity(struct nd_namespace_common *ndns);
+#endif /* __ND_CORE_H__ */
diff --git a/drivers/nvdimm/nd.h b/drivers/nvdimm/nd.h
new file mode 100644
index 000000000000..c41f53e74277
--- /dev/null
+++ b/drivers/nvdimm/nd.h
@@ -0,0 +1,220 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#ifndef __ND_H__
+#define __ND_H__
+#include <linux/libnvdimm.h>
+#include <linux/blkdev.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/ndctl.h>
+#include <linux/types.h>
+#include "label.h"
+
+enum {
+	/*
+	 * Limits the maximum number of block apertures a dimm can
+	 * support and is an input to the geometry/on-disk-format of a
+	 * BTT instance
+	 */
+	ND_MAX_LANES = 256,
+	SECTOR_SHIFT = 9,
+	INT_LBASIZE_ALIGNMENT = 64,
+};
+
+struct nvdimm_drvdata {
+	struct device *dev;
+	int nsindex_size;
+	struct nd_cmd_get_config_size nsarea;
+	void *data;
+	int ns_current, ns_next;
+	struct resource dpa;
+	struct kref kref;
+};
+
+struct nd_region_namespaces {
+	int count;
+	int active;
+};
+
+static inline struct nd_namespace_index *to_namespace_index(
+		struct nvdimm_drvdata *ndd, int i)
+{
+	if (i < 0)
+		return NULL;
+
+	return ndd->data + sizeof_namespace_index(ndd) * i;
+}
+
+static inline struct nd_namespace_index *to_current_namespace_index(
+		struct nvdimm_drvdata *ndd)
+{
+	return to_namespace_index(ndd, ndd->ns_current);
+}
+
+static inline struct nd_namespace_index *to_next_namespace_index(
+		struct nvdimm_drvdata *ndd)
+{
+	return to_namespace_index(ndd, ndd->ns_next);
+}
+
+#define nd_dbg_dpa(r, d, res, fmt, arg...) \
+	dev_dbg((r) ? &(r)->dev : (d)->dev, "%s: %.13s: %#llx @ %#llx " fmt, \
+		(r) ? dev_name((d)->dev) : "", res ? res->name : "null", \
+		(unsigned long long) (res ? resource_size(res) : 0), \
+		(unsigned long long) (res ? res->start : 0), ##arg)
+
+#define for_each_label(l, label, labels) \
+	for (l = 0; (label = labels ? labels[l] : NULL); l++)
+
+#define for_each_dpa_resource(ndd, res) \
+	for (res = (ndd)->dpa.child; res; res = res->sibling)
+
+#define for_each_dpa_resource_safe(ndd, res, next) \
+	for (res = (ndd)->dpa.child, next = res ? res->sibling : NULL; \
+			res; res = next, next = next ? next->sibling : NULL)
+
+struct nd_percpu_lane {
+	int count;
+	spinlock_t lock;
+};
+
+struct nd_region {
+	struct device dev;
+	struct ida ns_ida;
+	struct ida btt_ida;
+	struct device *ns_seed;
+	struct device *btt_seed;
+	u16 ndr_mappings;
+	u64 ndr_size;
+	u64 ndr_start;
+	int id, num_lanes, ro, numa_node;
+	void *provider_data;
+	struct nd_interleave_set *nd_set;
+	struct nd_percpu_lane __percpu *lane;
+	struct nd_mapping mapping[0];
+};
+
+struct nd_blk_region {
+	int (*enable)(struct nvdimm_bus *nvdimm_bus, struct device *dev);
+	void (*disable)(struct nvdimm_bus *nvdimm_bus, struct device *dev);
+	int (*do_io)(struct nd_blk_region *ndbr, resource_size_t dpa,
+			void *iobuf, u64 len, int rw);
+	void *blk_provider_data;
+	struct nd_region nd_region;
+};
+
+/*
+ * Lookup next in the repeating sequence of 01, 10, and 11.
+ */
+static inline unsigned nd_inc_seq(unsigned seq)
+{
+	static const unsigned next[] = { 0, 2, 3, 1 };
+
+	return next[seq & 3];
+}
+
+struct btt;
+struct nd_btt {
+	struct device dev;
+	struct nd_namespace_common *ndns;
+	struct btt *btt;
+	unsigned long lbasize;
+	u8 *uuid;
+	int id;
+};
+
+enum nd_async_mode {
+	ND_SYNC,
+	ND_ASYNC,
+};
+
+int nd_integrity_init(struct gendisk *disk, unsigned long meta_size);
+void wait_nvdimm_bus_probe_idle(struct device *dev);
+void nd_device_register(struct device *dev);
+void nd_device_unregister(struct device *dev, enum nd_async_mode mode);
+int nd_uuid_store(struct device *dev, u8 **uuid_out, const char *buf,
+		size_t len);
+ssize_t nd_sector_size_show(unsigned long current_lbasize,
+		const unsigned long *supported, char *buf);
+ssize_t nd_sector_size_store(struct device *dev, const char *buf,
+		unsigned long *current_lbasize, const unsigned long *supported);
+int __init nvdimm_init(void);
+int __init nd_region_init(void);
+void nvdimm_exit(void);
+void nd_region_exit(void);
+struct nvdimm;
+struct nvdimm_drvdata *to_ndd(struct nd_mapping *nd_mapping);
+int nvdimm_init_nsarea(struct nvdimm_drvdata *ndd);
+int nvdimm_init_config_data(struct nvdimm_drvdata *ndd);
+int nvdimm_set_config_data(struct nvdimm_drvdata *ndd, size_t offset,
+		void *buf, size_t len);
+struct nd_btt *to_nd_btt(struct device *dev);
+struct btt_sb;
+u64 nd_btt_sb_checksum(struct btt_sb *btt_sb);
+#if IS_ENABLED(CONFIG_BTT)
+int nd_btt_probe(struct nd_namespace_common *ndns, void *drvdata);
+bool is_nd_btt(struct device *dev);
+struct device *nd_btt_create(struct nd_region *nd_region);
+#else
+static inline nd_btt_probe(struct nd_namespace_common *ndns, void *drvdata)
+{
+	return -ENODEV;
+}
+
+static inline bool is_nd_btt(struct device *dev)
+{
+	return false;
+}
+
+static inline struct device *nd_btt_create(struct nd_region *nd_region)
+{
+	return NULL;
+}
+
+#endif
+struct nd_region *to_nd_region(struct device *dev);
+int nd_region_to_nstype(struct nd_region *nd_region);
+int nd_region_register_namespaces(struct nd_region *nd_region, int *err);
+u64 nd_region_interleave_set_cookie(struct nd_region *nd_region);
+void nvdimm_bus_lock(struct device *dev);
+void nvdimm_bus_unlock(struct device *dev);
+bool is_nvdimm_bus_locked(struct device *dev);
+int nvdimm_revalidate_disk(struct gendisk *disk);
+void nvdimm_drvdata_release(struct kref *kref);
+void put_ndd(struct nvdimm_drvdata *ndd);
+int nd_label_reserve_dpa(struct nvdimm_drvdata *ndd);
+void nvdimm_free_dpa(struct nvdimm_drvdata *ndd, struct resource *res);
+struct resource *nvdimm_allocate_dpa(struct nvdimm_drvdata *ndd,
+		struct nd_label_id *label_id, resource_size_t start,
+		resource_size_t n);
+resource_size_t nvdimm_namespace_capacity(struct nd_namespace_common *ndns);
+struct nd_namespace_common *nvdimm_namespace_common_probe(struct device *dev);
+int nvdimm_namespace_attach_btt(struct nd_namespace_common *ndns);
+int nvdimm_namespace_detach_btt(struct nd_namespace_common *ndns);
+const char *nvdimm_namespace_disk_name(struct nd_namespace_common *ndns,
+		char *name);
+int nd_blk_region_init(struct nd_region *nd_region);
+void __nd_iostat_start(struct bio *bio, unsigned long *start);
+static inline bool nd_iostat_start(struct bio *bio, unsigned long *start)
+{
+	struct gendisk *disk = bio->bi_bdev->bd_disk;
+
+	if (!blk_queue_io_stat(disk->queue))
+		return false;
+
+	__nd_iostat_start(bio, start);
+	return true;
+}
+void nd_iostat_end(struct bio *bio, unsigned long start);
+resource_size_t nd_namespace_blk_validate(struct nd_namespace_blk *nsblk);
+#endif /* __ND_H__ */
diff --git a/drivers/block/pmem.c b/drivers/nvdimm/pmem.c
index eabf4a8d0085..ade9eb917a4d 100644
--- a/drivers/block/pmem.c
+++ b/drivers/nvdimm/pmem.c
@@ -1,7 +1,7 @@
 /*
  * Persistent Memory Driver
  *
- * Copyright (c) 2014, Intel Corporation.
+ * Copyright (c) 2014-2015, Intel Corporation.
  * Copyright (c) 2015, Christoph Hellwig <hch@lst.de>.
  * Copyright (c) 2015, Boaz Harrosh <boaz@plexistor.com>.
  *
@@ -23,8 +23,9 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/slab.h>
-
-#define PMEM_MINORS		16
+#include <linux/pmem.h>
+#include <linux/nd.h>
+#include "nd.h"
 
 struct pmem_device {
 	struct request_queue	*pmem_queue;
@@ -32,12 +33,11 @@ struct pmem_device {
 
 	/* One contiguous memory region per device */
 	phys_addr_t		phys_addr;
-	void			*virt_addr;
+	void __pmem		*virt_addr;
 	size_t			size;
 };
 
 static int pmem_major;
-static atomic_t pmem_index;
 
 static void pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 			unsigned int len, unsigned int off, int rw,
@@ -45,13 +45,14 @@ static void pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 {
 	void *mem = kmap_atomic(page);
 	size_t pmem_off = sector << 9;
+	void __pmem *pmem_addr = pmem->virt_addr + pmem_off;
 
 	if (rw == READ) {
-		memcpy(mem + off, pmem->virt_addr + pmem_off, len);
+		memcpy_from_pmem(mem + off, pmem_addr, len);
 		flush_dcache_page(page);
 	} else {
 		flush_dcache_page(page);
-		memcpy(pmem->virt_addr + pmem_off, mem + off, len);
+		memcpy_to_pmem(pmem_addr, mem + off, len);
 	}
 
 	kunmap_atomic(mem);
@@ -59,31 +60,24 @@ static void pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 
 static void pmem_make_request(struct request_queue *q, struct bio *bio)
 {
-	struct block_device *bdev = bio->bi_bdev;
-	struct pmem_device *pmem = bdev->bd_disk->private_data;
-	int rw;
+	bool do_acct;
+	unsigned long start;
 	struct bio_vec bvec;
-	sector_t sector;
 	struct bvec_iter iter;
-	int err = 0;
-
-	if (bio_end_sector(bio) > get_capacity(bdev->bd_disk)) {
-		err = -EIO;
-		goto out;
-	}
-
-	BUG_ON(bio->bi_rw & REQ_DISCARD);
+	struct block_device *bdev = bio->bi_bdev;
+	struct pmem_device *pmem = bdev->bd_disk->private_data;
 
-	rw = bio_data_dir(bio);
-	sector = bio->bi_iter.bi_sector;
-	bio_for_each_segment(bvec, bio, iter) {
+	do_acct = nd_iostat_start(bio, &start);
+	bio_for_each_segment(bvec, bio, iter)
 		pmem_do_bvec(pmem, bvec.bv_page, bvec.bv_len, bvec.bv_offset,
-			     rw, sector);
-		sector += bvec.bv_len >> 9;
-	}
+				bio_data_dir(bio), iter.bi_sector);
+	if (do_acct)
+		nd_iostat_end(bio, start);
 
-out:
-	bio_endio(bio, err);
+	if (bio_data_dir(bio))
+		wmb_pmem();
+
+	bio_endio(bio, 0);
 }
 
 static int pmem_rw_page(struct block_device *bdev, sector_t sector,
@@ -106,7 +100,8 @@ static long pmem_direct_access(struct block_device *bdev, sector_t sector,
 	if (!pmem)
 		return -ENODEV;
 
-	*kaddr = pmem->virt_addr + offset;
+	/* FIXME convert DAX to comprehend that this mapping has a lifetime */
+	*kaddr = (void __force *) pmem->virt_addr + offset;
 	*pfn = (pmem->phys_addr + offset) >> PAGE_SHIFT;
 
 	return pmem->size - offset;
@@ -116,124 +111,165 @@ static const struct block_device_operations pmem_fops = {
 	.owner =		THIS_MODULE,
 	.rw_page =		pmem_rw_page,
 	.direct_access =	pmem_direct_access,
+	.revalidate_disk =	nvdimm_revalidate_disk,
 };
 
-static struct pmem_device *pmem_alloc(struct device *dev, struct resource *res)
+static struct pmem_device *pmem_alloc(struct device *dev,
+		struct resource *res, int id)
 {
 	struct pmem_device *pmem;
-	struct gendisk *disk;
-	int idx, err;
 
-	err = -ENOMEM;
 	pmem = kzalloc(sizeof(*pmem), GFP_KERNEL);
 	if (!pmem)
-		goto out;
+		return ERR_PTR(-ENOMEM);
 
 	pmem->phys_addr = res->start;
 	pmem->size = resource_size(res);
+	if (!arch_has_pmem_api())
+		dev_warn(dev, "unable to guarantee persistence of writes\n");
+
+	if (!request_mem_region(pmem->phys_addr, pmem->size, dev_name(dev))) {
+		dev_warn(dev, "could not reserve region [0x%pa:0x%zx]\n",
+				&pmem->phys_addr, pmem->size);
+		kfree(pmem);
+		return ERR_PTR(-EBUSY);
+	}
 
-	err = -EINVAL;
-	if (!request_mem_region(pmem->phys_addr, pmem->size, "pmem")) {
-		dev_warn(dev, "could not reserve region [0x%pa:0x%zx]\n", &pmem->phys_addr, pmem->size);
-		goto out_free_dev;
+	pmem->virt_addr = memremap_pmem(pmem->phys_addr, pmem->size);
+	if (!pmem->virt_addr) {
+		release_mem_region(pmem->phys_addr, pmem->size);
+		kfree(pmem);
+		return ERR_PTR(-ENXIO);
 	}
 
-	/*
-	 * Map the memory as non-cachable, as we can't write back the contents
-	 * of the CPU caches in case of a crash.
-	 */
-	err = -ENOMEM;
-	pmem->virt_addr = ioremap_nocache(pmem->phys_addr, pmem->size);
-	if (!pmem->virt_addr)
-		goto out_release_region;
+	return pmem;
+}
+
+static void pmem_detach_disk(struct pmem_device *pmem)
+{
+	del_gendisk(pmem->pmem_disk);
+	put_disk(pmem->pmem_disk);
+	blk_cleanup_queue(pmem->pmem_queue);
+}
+
+static int pmem_attach_disk(struct nd_namespace_common *ndns,
+		struct pmem_device *pmem)
+{
+	struct gendisk *disk;
 
 	pmem->pmem_queue = blk_alloc_queue(GFP_KERNEL);
 	if (!pmem->pmem_queue)
-		goto out_unmap;
+		return -ENOMEM;
 
 	blk_queue_make_request(pmem->pmem_queue, pmem_make_request);
-	blk_queue_max_hw_sectors(pmem->pmem_queue, 1024);
+	blk_queue_max_hw_sectors(pmem->pmem_queue, UINT_MAX);
 	blk_queue_bounce_limit(pmem->pmem_queue, BLK_BOUNCE_ANY);
+	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, pmem->pmem_queue);
 
-	disk = alloc_disk(PMEM_MINORS);
-	if (!disk)
-		goto out_free_queue;
-
-	idx = atomic_inc_return(&pmem_index) - 1;
+	disk = alloc_disk(0);
+	if (!disk) {
+		blk_cleanup_queue(pmem->pmem_queue);
+		return -ENOMEM;
+	}
 
 	disk->major		= pmem_major;
-	disk->first_minor	= PMEM_MINORS * idx;
+	disk->first_minor	= 0;
 	disk->fops		= &pmem_fops;
 	disk->private_data	= pmem;
 	disk->queue		= pmem->pmem_queue;
 	disk->flags		= GENHD_FL_EXT_DEVT;
-	sprintf(disk->disk_name, "pmem%d", idx);
-	disk->driverfs_dev = dev;
+	nvdimm_namespace_disk_name(ndns, disk->disk_name);
+	disk->driverfs_dev = &ndns->dev;
 	set_capacity(disk, pmem->size >> 9);
 	pmem->pmem_disk = disk;
 
 	add_disk(disk);
+	revalidate_disk(disk);
 
-	return pmem;
+	return 0;
+}
 
-out_free_queue:
-	blk_cleanup_queue(pmem->pmem_queue);
-out_unmap:
-	iounmap(pmem->virt_addr);
-out_release_region:
-	release_mem_region(pmem->phys_addr, pmem->size);
-out_free_dev:
-	kfree(pmem);
-out:
-	return ERR_PTR(err);
+static int pmem_rw_bytes(struct nd_namespace_common *ndns,
+		resource_size_t offset, void *buf, size_t size, int rw)
+{
+	struct pmem_device *pmem = dev_get_drvdata(ndns->claim);
+
+	if (unlikely(offset + size > pmem->size)) {
+		dev_WARN_ONCE(&ndns->dev, 1, "request out of range\n");
+		return -EFAULT;
+	}
+
+	if (rw == READ)
+		memcpy_from_pmem(buf, pmem->virt_addr + offset, size);
+	else {
+		memcpy_to_pmem(pmem->virt_addr + offset, buf, size);
+		wmb_pmem();
+	}
+
+	return 0;
 }
 
 static void pmem_free(struct pmem_device *pmem)
 {
-	del_gendisk(pmem->pmem_disk);
-	put_disk(pmem->pmem_disk);
-	blk_cleanup_queue(pmem->pmem_queue);
-	iounmap(pmem->virt_addr);
+	memunmap_pmem(pmem->virt_addr);
 	release_mem_region(pmem->phys_addr, pmem->size);
 	kfree(pmem);
 }
 
-static int pmem_probe(struct platform_device *pdev)
+static int nd_pmem_probe(struct device *dev)
 {
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	struct nd_namespace_common *ndns;
+	struct nd_namespace_io *nsio;
 	struct pmem_device *pmem;
-	struct resource *res;
-
-	if (WARN_ON(pdev->num_resources > 1))
-		return -ENXIO;
+	int rc;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENXIO;
+	ndns = nvdimm_namespace_common_probe(dev);
+	if (IS_ERR(ndns))
+		return PTR_ERR(ndns);
 
-	pmem = pmem_alloc(&pdev->dev, res);
+	nsio = to_nd_namespace_io(&ndns->dev);
+	pmem = pmem_alloc(dev, &nsio->res, nd_region->id);
 	if (IS_ERR(pmem))
 		return PTR_ERR(pmem);
 
-	platform_set_drvdata(pdev, pmem);
-
-	return 0;
+	dev_set_drvdata(dev, pmem);
+	ndns->rw_bytes = pmem_rw_bytes;
+	if (is_nd_btt(dev))
+		rc = nvdimm_namespace_attach_btt(ndns);
+	else if (nd_btt_probe(ndns, pmem) == 0) {
+		/* we'll come back as btt-pmem */
+		rc = -ENXIO;
+	} else
+		rc = pmem_attach_disk(ndns, pmem);
+	if (rc)
+		pmem_free(pmem);
+	return rc;
 }
 
-static int pmem_remove(struct platform_device *pdev)
+static int nd_pmem_remove(struct device *dev)
 {
-	struct pmem_device *pmem = platform_get_drvdata(pdev);
+	struct pmem_device *pmem = dev_get_drvdata(dev);
 
+	if (is_nd_btt(dev))
+		nvdimm_namespace_detach_btt(to_nd_btt(dev)->ndns);
+	else
+		pmem_detach_disk(pmem);
 	pmem_free(pmem);
+
 	return 0;
 }
 
-static struct platform_driver pmem_driver = {
-	.probe		= pmem_probe,
-	.remove		= pmem_remove,
-	.driver		= {
-		.owner	= THIS_MODULE,
-		.name	= "pmem",
+MODULE_ALIAS("pmem");
+MODULE_ALIAS_ND_DEVICE(ND_DEVICE_NAMESPACE_IO);
+MODULE_ALIAS_ND_DEVICE(ND_DEVICE_NAMESPACE_PMEM);
+static struct nd_device_driver nd_pmem_driver = {
+	.probe = nd_pmem_probe,
+	.remove = nd_pmem_remove,
+	.drv = {
+		.name = "nd_pmem",
 	},
+	.type = ND_DRIVER_NAMESPACE_IO | ND_DRIVER_NAMESPACE_PMEM,
 };
 
 static int __init pmem_init(void)
@@ -244,16 +280,19 @@ static int __init pmem_init(void)
 	if (pmem_major < 0)
 		return pmem_major;
 
-	error = platform_driver_register(&pmem_driver);
-	if (error)
+	error = nd_driver_register(&nd_pmem_driver);
+	if (error) {
 		unregister_blkdev(pmem_major, "pmem");
-	return error;
+		return error;
+	}
+
+	return 0;
 }
 module_init(pmem_init);
 
 static void pmem_exit(void)
 {
-	platform_driver_unregister(&pmem_driver);
+	driver_unregister(&nd_pmem_driver.drv);
 	unregister_blkdev(pmem_major, "pmem");
 }
 module_exit(pmem_exit);
diff --git a/drivers/nvdimm/region.c b/drivers/nvdimm/region.c
new file mode 100644
index 000000000000..f28f78ccff19
--- /dev/null
+++ b/drivers/nvdimm/region.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#include <linux/cpumask.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/nd.h>
+#include "nd.h"
+
+static int nd_region_probe(struct device *dev)
+{
+	int err, rc;
+	static unsigned long once;
+	struct nd_region_namespaces *num_ns;
+	struct nd_region *nd_region = to_nd_region(dev);
+
+	if (nd_region->num_lanes > num_online_cpus()
+			&& nd_region->num_lanes < num_possible_cpus()
+			&& !test_and_set_bit(0, &once)) {
+		dev_info(dev, "online cpus (%d) < concurrent i/o lanes (%d) < possible cpus (%d)\n",
+				num_online_cpus(), nd_region->num_lanes,
+				num_possible_cpus());
+		dev_info(dev, "setting nr_cpus=%d may yield better libnvdimm device performance\n",
+				nd_region->num_lanes);
+	}
+
+	rc = nd_blk_region_init(nd_region);
+	if (rc)
+		return rc;
+
+	rc = nd_region_register_namespaces(nd_region, &err);
+	num_ns = devm_kzalloc(dev, sizeof(*num_ns), GFP_KERNEL);
+	if (!num_ns)
+		return -ENOMEM;
+
+	if (rc < 0)
+		return rc;
+
+	num_ns->active = rc;
+	num_ns->count = rc + err;
+	dev_set_drvdata(dev, num_ns);
+
+	if (rc && err && rc == err)
+		return -ENODEV;
+
+	nd_region->btt_seed = nd_btt_create(nd_region);
+	if (err == 0)
+		return 0;
+
+	/*
+	 * Given multiple namespaces per region, we do not want to
+	 * disable all the successfully registered peer namespaces upon
+	 * a single registration failure.  If userspace is missing a
+	 * namespace that it expects it can disable/re-enable the region
+	 * to retry discovery after correcting the failure.
+	 * <regionX>/namespaces returns the current
+	 * "<async-registered>/<total>" namespace count.
+	 */
+	dev_err(dev, "failed to register %d namespace%s, continuing...\n",
+			err, err == 1 ? "" : "s");
+	return 0;
+}
+
+static int child_unregister(struct device *dev, void *data)
+{
+	nd_device_unregister(dev, ND_SYNC);
+	return 0;
+}
+
+static int nd_region_remove(struct device *dev)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+
+	/* flush attribute readers and disable */
+	nvdimm_bus_lock(dev);
+	nd_region->ns_seed = NULL;
+	nd_region->btt_seed = NULL;
+	dev_set_drvdata(dev, NULL);
+	nvdimm_bus_unlock(dev);
+
+	device_for_each_child(dev, NULL, child_unregister);
+	return 0;
+}
+
+static struct nd_device_driver nd_region_driver = {
+	.probe = nd_region_probe,
+	.remove = nd_region_remove,
+	.drv = {
+		.name = "nd_region",
+	},
+	.type = ND_DRIVER_REGION_BLK | ND_DRIVER_REGION_PMEM,
+};
+
+int __init nd_region_init(void)
+{
+	return nd_driver_register(&nd_region_driver);
+}
+
+void nd_region_exit(void)
+{
+	driver_unregister(&nd_region_driver.drv);
+}
+
+MODULE_ALIAS_ND_DEVICE(ND_DEVICE_REGION_PMEM);
+MODULE_ALIAS_ND_DEVICE(ND_DEVICE_REGION_BLK);
diff --git a/drivers/nvdimm/region_devs.c b/drivers/nvdimm/region_devs.c
new file mode 100644
index 000000000000..a5233422f9dc
--- /dev/null
+++ b/drivers/nvdimm/region_devs.c
@@ -0,0 +1,787 @@
+/*
+ * Copyright(c) 2013-2015 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+#include <linux/scatterlist.h>
+#include <linux/highmem.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/sort.h>
+#include <linux/io.h>
+#include <linux/nd.h>
+#include "nd-core.h"
+#include "nd.h"
+
+static DEFINE_IDA(region_ida);
+
+static void nd_region_release(struct device *dev)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+	u16 i;
+
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		struct nvdimm *nvdimm = nd_mapping->nvdimm;
+
+		put_device(&nvdimm->dev);
+	}
+	free_percpu(nd_region->lane);
+	ida_simple_remove(&region_ida, nd_region->id);
+	if (is_nd_blk(dev))
+		kfree(to_nd_blk_region(dev));
+	else
+		kfree(nd_region);
+}
+
+static struct device_type nd_blk_device_type = {
+	.name = "nd_blk",
+	.release = nd_region_release,
+};
+
+static struct device_type nd_pmem_device_type = {
+	.name = "nd_pmem",
+	.release = nd_region_release,
+};
+
+static struct device_type nd_volatile_device_type = {
+	.name = "nd_volatile",
+	.release = nd_region_release,
+};
+
+bool is_nd_pmem(struct device *dev)
+{
+	return dev ? dev->type == &nd_pmem_device_type : false;
+}
+
+bool is_nd_blk(struct device *dev)
+{
+	return dev ? dev->type == &nd_blk_device_type : false;
+}
+
+struct nd_region *to_nd_region(struct device *dev)
+{
+	struct nd_region *nd_region = container_of(dev, struct nd_region, dev);
+
+	WARN_ON(dev->type->release != nd_region_release);
+	return nd_region;
+}
+EXPORT_SYMBOL_GPL(to_nd_region);
+
+struct nd_blk_region *to_nd_blk_region(struct device *dev)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+
+	WARN_ON(!is_nd_blk(dev));
+	return container_of(nd_region, struct nd_blk_region, nd_region);
+}
+EXPORT_SYMBOL_GPL(to_nd_blk_region);
+
+void *nd_region_provider_data(struct nd_region *nd_region)
+{
+	return nd_region->provider_data;
+}
+EXPORT_SYMBOL_GPL(nd_region_provider_data);
+
+void *nd_blk_region_provider_data(struct nd_blk_region *ndbr)
+{
+	return ndbr->blk_provider_data;
+}
+EXPORT_SYMBOL_GPL(nd_blk_region_provider_data);
+
+void nd_blk_region_set_provider_data(struct nd_blk_region *ndbr, void *data)
+{
+	ndbr->blk_provider_data = data;
+}
+EXPORT_SYMBOL_GPL(nd_blk_region_set_provider_data);
+
+/**
+ * nd_region_to_nstype() - region to an integer namespace type
+ * @nd_region: region-device to interrogate
+ *
+ * This is the 'nstype' attribute of a region as well, an input to the
+ * MODALIAS for namespace devices, and bit number for a nvdimm_bus to match
+ * namespace devices with namespace drivers.
+ */
+int nd_region_to_nstype(struct nd_region *nd_region)
+{
+	if (is_nd_pmem(&nd_region->dev)) {
+		u16 i, alias;
+
+		for (i = 0, alias = 0; i < nd_region->ndr_mappings; i++) {
+			struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+			struct nvdimm *nvdimm = nd_mapping->nvdimm;
+
+			if (nvdimm->flags & NDD_ALIASING)
+				alias++;
+		}
+		if (alias)
+			return ND_DEVICE_NAMESPACE_PMEM;
+		else
+			return ND_DEVICE_NAMESPACE_IO;
+	} else if (is_nd_blk(&nd_region->dev)) {
+		return ND_DEVICE_NAMESPACE_BLK;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(nd_region_to_nstype);
+
+static int is_uuid_busy(struct device *dev, void *data)
+{
+	struct nd_region *nd_region = to_nd_region(dev->parent);
+	u8 *uuid = data;
+
+	switch (nd_region_to_nstype(nd_region)) {
+	case ND_DEVICE_NAMESPACE_PMEM: {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		if (!nspm->uuid)
+			break;
+		if (memcmp(uuid, nspm->uuid, NSLABEL_UUID_LEN) == 0)
+			return -EBUSY;
+		break;
+	}
+	case ND_DEVICE_NAMESPACE_BLK: {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		if (!nsblk->uuid)
+			break;
+		if (memcmp(uuid, nsblk->uuid, NSLABEL_UUID_LEN) == 0)
+			return -EBUSY;
+		break;
+	}
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int is_namespace_uuid_busy(struct device *dev, void *data)
+{
+	if (is_nd_pmem(dev) || is_nd_blk(dev))
+		return device_for_each_child(dev, data, is_uuid_busy);
+	return 0;
+}
+
+/**
+ * nd_is_uuid_unique - verify that no other namespace has @uuid
+ * @dev: any device on a nvdimm_bus
+ * @uuid: uuid to check
+ */
+bool nd_is_uuid_unique(struct device *dev, u8 *uuid)
+{
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+
+	if (!nvdimm_bus)
+		return false;
+	WARN_ON_ONCE(!is_nvdimm_bus_locked(&nvdimm_bus->dev));
+	if (device_for_each_child(&nvdimm_bus->dev, uuid,
+				is_namespace_uuid_busy) != 0)
+		return false;
+	return true;
+}
+
+static ssize_t size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+	unsigned long long size = 0;
+
+	if (is_nd_pmem(dev)) {
+		size = nd_region->ndr_size;
+	} else if (nd_region->ndr_mappings == 1) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[0];
+
+		size = nd_mapping->size;
+	}
+
+	return sprintf(buf, "%llu\n", size);
+}
+static DEVICE_ATTR_RO(size);
+
+static ssize_t mappings_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+
+	return sprintf(buf, "%d\n", nd_region->ndr_mappings);
+}
+static DEVICE_ATTR_RO(mappings);
+
+static ssize_t nstype_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+
+	return sprintf(buf, "%d\n", nd_region_to_nstype(nd_region));
+}
+static DEVICE_ATTR_RO(nstype);
+
+static ssize_t set_cookie_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+	struct nd_interleave_set *nd_set = nd_region->nd_set;
+
+	if (is_nd_pmem(dev) && nd_set)
+		/* pass, should be precluded by region_visible */;
+	else
+		return -ENXIO;
+
+	return sprintf(buf, "%#llx\n", nd_set->cookie);
+}
+static DEVICE_ATTR_RO(set_cookie);
+
+resource_size_t nd_region_available_dpa(struct nd_region *nd_region)
+{
+	resource_size_t blk_max_overlap = 0, available, overlap;
+	int i;
+
+	WARN_ON(!is_nvdimm_bus_locked(&nd_region->dev));
+
+ retry:
+	available = 0;
+	overlap = blk_max_overlap;
+	for (i = 0; i < nd_region->ndr_mappings; i++) {
+		struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+		struct nvdimm_drvdata *ndd = to_ndd(nd_mapping);
+
+		/* if a dimm is disabled the available capacity is zero */
+		if (!ndd)
+			return 0;
+
+		if (is_nd_pmem(&nd_region->dev)) {
+			available += nd_pmem_available_dpa(nd_region,
+					nd_mapping, &overlap);
+			if (overlap > blk_max_overlap) {
+				blk_max_overlap = overlap;
+				goto retry;
+			}
+		} else if (is_nd_blk(&nd_region->dev)) {
+			available += nd_blk_available_dpa(nd_mapping);
+		}
+	}
+
+	return available;
+}
+
+static ssize_t available_size_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+	unsigned long long available = 0;
+
+	/*
+	 * Flush in-flight updates and grab a snapshot of the available
+	 * size.  Of course, this value is potentially invalidated the
+	 * memory nvdimm_bus_lock() is dropped, but that's userspace's
+	 * problem to not race itself.
+	 */
+	nvdimm_bus_lock(dev);
+	wait_nvdimm_bus_probe_idle(dev);
+	available = nd_region_available_dpa(nd_region);
+	nvdimm_bus_unlock(dev);
+
+	return sprintf(buf, "%llu\n", available);
+}
+static DEVICE_ATTR_RO(available_size);
+
+static ssize_t init_namespaces_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region_namespaces *num_ns = dev_get_drvdata(dev);
+	ssize_t rc;
+
+	nvdimm_bus_lock(dev);
+	if (num_ns)
+		rc = sprintf(buf, "%d/%d\n", num_ns->active, num_ns->count);
+	else
+		rc = -ENXIO;
+	nvdimm_bus_unlock(dev);
+
+	return rc;
+}
+static DEVICE_ATTR_RO(init_namespaces);
+
+static ssize_t namespace_seed_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+	ssize_t rc;
+
+	nvdimm_bus_lock(dev);
+	if (nd_region->ns_seed)
+		rc = sprintf(buf, "%s\n", dev_name(nd_region->ns_seed));
+	else
+		rc = sprintf(buf, "\n");
+	nvdimm_bus_unlock(dev);
+	return rc;
+}
+static DEVICE_ATTR_RO(namespace_seed);
+
+static ssize_t btt_seed_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+	ssize_t rc;
+
+	nvdimm_bus_lock(dev);
+	if (nd_region->btt_seed)
+		rc = sprintf(buf, "%s\n", dev_name(nd_region->btt_seed));
+	else
+		rc = sprintf(buf, "\n");
+	nvdimm_bus_unlock(dev);
+
+	return rc;
+}
+static DEVICE_ATTR_RO(btt_seed);
+
+static ssize_t read_only_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+
+	return sprintf(buf, "%d\n", nd_region->ro);
+}
+
+static ssize_t read_only_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	bool ro;
+	int rc = strtobool(buf, &ro);
+	struct nd_region *nd_region = to_nd_region(dev);
+
+	if (rc)
+		return rc;
+
+	nd_region->ro = ro;
+	return len;
+}
+static DEVICE_ATTR_RW(read_only);
+
+static struct attribute *nd_region_attributes[] = {
+	&dev_attr_size.attr,
+	&dev_attr_nstype.attr,
+	&dev_attr_mappings.attr,
+	&dev_attr_btt_seed.attr,
+	&dev_attr_read_only.attr,
+	&dev_attr_set_cookie.attr,
+	&dev_attr_available_size.attr,
+	&dev_attr_namespace_seed.attr,
+	&dev_attr_init_namespaces.attr,
+	NULL,
+};
+
+static umode_t region_visible(struct kobject *kobj, struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, typeof(*dev), kobj);
+	struct nd_region *nd_region = to_nd_region(dev);
+	struct nd_interleave_set *nd_set = nd_region->nd_set;
+	int type = nd_region_to_nstype(nd_region);
+
+	if (a != &dev_attr_set_cookie.attr
+			&& a != &dev_attr_available_size.attr)
+		return a->mode;
+
+	if ((type == ND_DEVICE_NAMESPACE_PMEM
+				|| type == ND_DEVICE_NAMESPACE_BLK)
+			&& a == &dev_attr_available_size.attr)
+		return a->mode;
+	else if (is_nd_pmem(dev) && nd_set)
+		return a->mode;
+
+	return 0;
+}
+
+struct attribute_group nd_region_attribute_group = {
+	.attrs = nd_region_attributes,
+	.is_visible = region_visible,
+};
+EXPORT_SYMBOL_GPL(nd_region_attribute_group);
+
+u64 nd_region_interleave_set_cookie(struct nd_region *nd_region)
+{
+	struct nd_interleave_set *nd_set = nd_region->nd_set;
+
+	if (nd_set)
+		return nd_set->cookie;
+	return 0;
+}
+
+/*
+ * Upon successful probe/remove, take/release a reference on the
+ * associated interleave set (if present), and plant new btt + namespace
+ * seeds.  Also, on the removal of a BLK region, notify the provider to
+ * disable the region.
+ */
+static void nd_region_notify_driver_action(struct nvdimm_bus *nvdimm_bus,
+		struct device *dev, bool probe)
+{
+	struct nd_region *nd_region;
+
+	if (!probe && (is_nd_pmem(dev) || is_nd_blk(dev))) {
+		int i;
+
+		nd_region = to_nd_region(dev);
+		for (i = 0; i < nd_region->ndr_mappings; i++) {
+			struct nd_mapping *nd_mapping = &nd_region->mapping[i];
+			struct nvdimm_drvdata *ndd = nd_mapping->ndd;
+			struct nvdimm *nvdimm = nd_mapping->nvdimm;
+
+			kfree(nd_mapping->labels);
+			nd_mapping->labels = NULL;
+			put_ndd(ndd);
+			nd_mapping->ndd = NULL;
+			if (ndd)
+				atomic_dec(&nvdimm->busy);
+		}
+
+		if (is_nd_pmem(dev))
+			return;
+
+		to_nd_blk_region(dev)->disable(nvdimm_bus, dev);
+	}
+	if (dev->parent && is_nd_blk(dev->parent) && probe) {
+		nd_region = to_nd_region(dev->parent);
+		nvdimm_bus_lock(dev);
+		if (nd_region->ns_seed == dev)
+			nd_region_create_blk_seed(nd_region);
+		nvdimm_bus_unlock(dev);
+	}
+	if (is_nd_btt(dev) && probe) {
+		nd_region = to_nd_region(dev->parent);
+		nvdimm_bus_lock(dev);
+		if (nd_region->btt_seed == dev)
+			nd_region_create_btt_seed(nd_region);
+		nvdimm_bus_unlock(dev);
+	}
+}
+
+void nd_region_probe_success(struct nvdimm_bus *nvdimm_bus, struct device *dev)
+{
+	nd_region_notify_driver_action(nvdimm_bus, dev, true);
+}
+
+void nd_region_disable(struct nvdimm_bus *nvdimm_bus, struct device *dev)
+{
+	nd_region_notify_driver_action(nvdimm_bus, dev, false);
+}
+
+static ssize_t mappingN(struct device *dev, char *buf, int n)
+{
+	struct nd_region *nd_region = to_nd_region(dev);
+	struct nd_mapping *nd_mapping;
+	struct nvdimm *nvdimm;
+
+	if (n >= nd_region->ndr_mappings)
+		return -ENXIO;
+	nd_mapping = &nd_region->mapping[n];
+	nvdimm = nd_mapping->nvdimm;
+
+	return sprintf(buf, "%s,%llu,%llu\n", dev_name(&nvdimm->dev),
+			nd_mapping->start, nd_mapping->size);
+}
+
+#define REGION_MAPPING(idx) \
+static ssize_t mapping##idx##_show(struct device *dev,		\
+		struct device_attribute *attr, char *buf)	\
+{								\
+	return mappingN(dev, buf, idx);				\
+}								\
+static DEVICE_ATTR_RO(mapping##idx)
+
+/*
+ * 32 should be enough for a while, even in the presence of socket
+ * interleave a 32-way interleave set is a degenerate case.
+ */
+REGION_MAPPING(0);
+REGION_MAPPING(1);
+REGION_MAPPING(2);
+REGION_MAPPING(3);
+REGION_MAPPING(4);
+REGION_MAPPING(5);
+REGION_MAPPING(6);
+REGION_MAPPING(7);
+REGION_MAPPING(8);
+REGION_MAPPING(9);
+REGION_MAPPING(10);
+REGION_MAPPING(11);
+REGION_MAPPING(12);
+REGION_MAPPING(13);
+REGION_MAPPING(14);
+REGION_MAPPING(15);
+REGION_MAPPING(16);
+REGION_MAPPING(17);
+REGION_MAPPING(18);
+REGION_MAPPING(19);
+REGION_MAPPING(20);
+REGION_MAPPING(21);
+REGION_MAPPING(22);
+REGION_MAPPING(23);
+REGION_MAPPING(24);
+REGION_MAPPING(25);
+REGION_MAPPING(26);
+REGION_MAPPING(27);
+REGION_MAPPING(28);
+REGION_MAPPING(29);
+REGION_MAPPING(30);
+REGION_MAPPING(31);
+
+static umode_t mapping_visible(struct kobject *kobj, struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nd_region *nd_region = to_nd_region(dev);
+
+	if (n < nd_region->ndr_mappings)
+		return a->mode;
+	return 0;
+}
+
+static struct attribute *mapping_attributes[] = {
+	&dev_attr_mapping0.attr,
+	&dev_attr_mapping1.attr,
+	&dev_attr_mapping2.attr,
+	&dev_attr_mapping3.attr,
+	&dev_attr_mapping4.attr,
+	&dev_attr_mapping5.attr,
+	&dev_attr_mapping6.attr,
+	&dev_attr_mapping7.attr,
+	&dev_attr_mapping8.attr,
+	&dev_attr_mapping9.attr,
+	&dev_attr_mapping10.attr,
+	&dev_attr_mapping11.attr,
+	&dev_attr_mapping12.attr,
+	&dev_attr_mapping13.attr,
+	&dev_attr_mapping14.attr,
+	&dev_attr_mapping15.attr,
+	&dev_attr_mapping16.attr,
+	&dev_attr_mapping17.attr,
+	&dev_attr_mapping18.attr,
+	&dev_attr_mapping19.attr,
+	&dev_attr_mapping20.attr,
+	&dev_attr_mapping21.attr,
+	&dev_attr_mapping22.attr,
+	&dev_attr_mapping23.attr,
+	&dev_attr_mapping24.attr,
+	&dev_attr_mapping25.attr,
+	&dev_attr_mapping26.attr,
+	&dev_attr_mapping27.attr,
+	&dev_attr_mapping28.attr,
+	&dev_attr_mapping29.attr,
+	&dev_attr_mapping30.attr,
+	&dev_attr_mapping31.attr,
+	NULL,
+};
+
+struct attribute_group nd_mapping_attribute_group = {
+	.is_visible = mapping_visible,
+	.attrs = mapping_attributes,
+};
+EXPORT_SYMBOL_GPL(nd_mapping_attribute_group);
+
+int nd_blk_region_init(struct nd_region *nd_region)
+{
+	struct device *dev = &nd_region->dev;
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+
+	if (!is_nd_blk(dev))
+		return 0;
+
+	if (nd_region->ndr_mappings < 1) {
+		dev_err(dev, "invalid BLK region\n");
+		return -ENXIO;
+	}
+
+	return to_nd_blk_region(dev)->enable(nvdimm_bus, dev);
+}
+
+/**
+ * nd_region_acquire_lane - allocate and lock a lane
+ * @nd_region: region id and number of lanes possible
+ *
+ * A lane correlates to a BLK-data-window and/or a log slot in the BTT.
+ * We optimize for the common case where there are 256 lanes, one
+ * per-cpu.  For larger systems we need to lock to share lanes.  For now
+ * this implementation assumes the cost of maintaining an allocator for
+ * free lanes is on the order of the lock hold time, so it implements a
+ * static lane = cpu % num_lanes mapping.
+ *
+ * In the case of a BTT instance on top of a BLK namespace a lane may be
+ * acquired recursively.  We lock on the first instance.
+ *
+ * In the case of a BTT instance on top of PMEM, we only acquire a lane
+ * for the BTT metadata updates.
+ */
+unsigned int nd_region_acquire_lane(struct nd_region *nd_region)
+{
+	unsigned int cpu, lane;
+
+	cpu = get_cpu();
+	if (nd_region->num_lanes < nr_cpu_ids) {
+		struct nd_percpu_lane *ndl_lock, *ndl_count;
+
+		lane = cpu % nd_region->num_lanes;
+		ndl_count = per_cpu_ptr(nd_region->lane, cpu);
+		ndl_lock = per_cpu_ptr(nd_region->lane, lane);
+		if (ndl_count->count++ == 0)
+			spin_lock(&ndl_lock->lock);
+	} else
+		lane = cpu;
+
+	return lane;
+}
+EXPORT_SYMBOL(nd_region_acquire_lane);
+
+void nd_region_release_lane(struct nd_region *nd_region, unsigned int lane)
+{
+	if (nd_region->num_lanes < nr_cpu_ids) {
+		unsigned int cpu = get_cpu();
+		struct nd_percpu_lane *ndl_lock, *ndl_count;
+
+		ndl_count = per_cpu_ptr(nd_region->lane, cpu);
+		ndl_lock = per_cpu_ptr(nd_region->lane, lane);
+		if (--ndl_count->count == 0)
+			spin_unlock(&ndl_lock->lock);
+		put_cpu();
+	}
+	put_cpu();
+}
+EXPORT_SYMBOL(nd_region_release_lane);
+
+static struct nd_region *nd_region_create(struct nvdimm_bus *nvdimm_bus,
+		struct nd_region_desc *ndr_desc, struct device_type *dev_type,
+		const char *caller)
+{
+	struct nd_region *nd_region;
+	struct device *dev;
+	void *region_buf;
+	unsigned int i;
+	int ro = 0;
+
+	for (i = 0; i < ndr_desc->num_mappings; i++) {
+		struct nd_mapping *nd_mapping = &ndr_desc->nd_mapping[i];
+		struct nvdimm *nvdimm = nd_mapping->nvdimm;
+
+		if ((nd_mapping->start | nd_mapping->size) % SZ_4K) {
+			dev_err(&nvdimm_bus->dev, "%s: %s mapping%d is not 4K aligned\n",
+					caller, dev_name(&nvdimm->dev), i);
+
+			return NULL;
+		}
+
+		if (nvdimm->flags & NDD_UNARMED)
+			ro = 1;
+	}
+
+	if (dev_type == &nd_blk_device_type) {
+		struct nd_blk_region_desc *ndbr_desc;
+		struct nd_blk_region *ndbr;
+
+		ndbr_desc = to_blk_region_desc(ndr_desc);
+		ndbr = kzalloc(sizeof(*ndbr) + sizeof(struct nd_mapping)
+				* ndr_desc->num_mappings,
+				GFP_KERNEL);
+		if (ndbr) {
+			nd_region = &ndbr->nd_region;
+			ndbr->enable = ndbr_desc->enable;
+			ndbr->disable = ndbr_desc->disable;
+			ndbr->do_io = ndbr_desc->do_io;
+		}
+		region_buf = ndbr;
+	} else {
+		nd_region = kzalloc(sizeof(struct nd_region)
+				+ sizeof(struct nd_mapping)
+				* ndr_desc->num_mappings,
+				GFP_KERNEL);
+		region_buf = nd_region;
+	}
+
+	if (!region_buf)
+		return NULL;
+	nd_region->id = ida_simple_get(&region_ida, 0, 0, GFP_KERNEL);
+	if (nd_region->id < 0)
+		goto err_id;
+
+	nd_region->lane = alloc_percpu(struct nd_percpu_lane);
+	if (!nd_region->lane)
+		goto err_percpu;
+
+        for (i = 0; i < nr_cpu_ids; i++) {
+		struct nd_percpu_lane *ndl;
+
+		ndl = per_cpu_ptr(nd_region->lane, i);
+		spin_lock_init(&ndl->lock);
+		ndl->count = 0;
+	}
+
+	memcpy(nd_region->mapping, ndr_desc->nd_mapping,
+			sizeof(struct nd_mapping) * ndr_desc->num_mappings);
+	for (i = 0; i < ndr_desc->num_mappings; i++) {
+		struct nd_mapping *nd_mapping = &ndr_desc->nd_mapping[i];
+		struct nvdimm *nvdimm = nd_mapping->nvdimm;
+
+		get_device(&nvdimm->dev);
+	}
+	nd_region->ndr_mappings = ndr_desc->num_mappings;
+	nd_region->provider_data = ndr_desc->provider_data;
+	nd_region->nd_set = ndr_desc->nd_set;
+	nd_region->num_lanes = ndr_desc->num_lanes;
+	nd_region->ro = ro;
+	nd_region->numa_node = ndr_desc->numa_node;
+	ida_init(&nd_region->ns_ida);
+	ida_init(&nd_region->btt_ida);
+	dev = &nd_region->dev;
+	dev_set_name(dev, "region%d", nd_region->id);
+	dev->parent = &nvdimm_bus->dev;
+	dev->type = dev_type;
+	dev->groups = ndr_desc->attr_groups;
+	nd_region->ndr_size = resource_size(ndr_desc->res);
+	nd_region->ndr_start = ndr_desc->res->start;
+	nd_device_register(dev);
+
+	return nd_region;
+
+ err_percpu:
+	ida_simple_remove(&region_ida, nd_region->id);
+ err_id:
+	kfree(region_buf);
+	return NULL;
+}
+
+struct nd_region *nvdimm_pmem_region_create(struct nvdimm_bus *nvdimm_bus,
+		struct nd_region_desc *ndr_desc)
+{
+	ndr_desc->num_lanes = ND_MAX_LANES;
+	return nd_region_create(nvdimm_bus, ndr_desc, &nd_pmem_device_type,
+			__func__);
+}
+EXPORT_SYMBOL_GPL(nvdimm_pmem_region_create);
+
+struct nd_region *nvdimm_blk_region_create(struct nvdimm_bus *nvdimm_bus,
+		struct nd_region_desc *ndr_desc)
+{
+	if (ndr_desc->num_mappings > 1)
+		return NULL;
+	ndr_desc->num_lanes = min(ndr_desc->num_lanes, ND_MAX_LANES);
+	return nd_region_create(nvdimm_bus, ndr_desc, &nd_blk_device_type,
+			__func__);
+}
+EXPORT_SYMBOL_GPL(nvdimm_blk_region_create);
+
+struct nd_region *nvdimm_volatile_region_create(struct nvdimm_bus *nvdimm_bus,
+		struct nd_region_desc *ndr_desc)
+{
+	ndr_desc->num_lanes = ND_MAX_LANES;
+	return nd_region_create(nvdimm_bus, ndr_desc, &nd_volatile_device_type,
+			__func__);
+}
+EXPORT_SYMBOL_GPL(nvdimm_volatile_region_create);
diff --git a/drivers/of/Kconfig b/drivers/of/Kconfig
index 07bb3c8f191b..8df1b1777745 100644
--- a/drivers/of/Kconfig
+++ b/drivers/of/Kconfig
@@ -1,15 +1,20 @@
 config DTC
 	bool
 
-config OF
-	bool
+menuconfig OF
+	bool "Device Tree and Open Firmware support"
+	help
+	  This option enables the device tree infrastructure.
+	  It is automatically selected by platforms that need it or can
+	  be enabled manually for unittests, overlays or
+	  compile-coverage.
 
-menu "Device Tree and Open Firmware support"
-	depends on OF
+if OF
 
 config OF_UNITTEST
 	bool "Device Tree runtime unit tests"
-	depends on OF_IRQ && OF_EARLY_FLATTREE
+	depends on OF_IRQ
+	select OF_EARLY_FLATTREE
 	select OF_RESOLVE
 	help
 	  This option builds in test cases for the device tree infrastructure
@@ -97,4 +102,4 @@ config OF_OVERLAY
 	  While this option is selected automatically when needed, you can
 	  enable it manually to improve device tree unit test coverage.
 
-endmenu # OF
+endif # OF
diff --git a/drivers/of/Makefile b/drivers/of/Makefile
index fcacb186a67b..156c072b3117 100644
--- a/drivers/of/Makefile
+++ b/drivers/of/Makefile
@@ -16,6 +16,3 @@ obj-$(CONFIG_OF_RESOLVE)  += resolver.o
 obj-$(CONFIG_OF_OVERLAY) += overlay.o
 
 obj-$(CONFIG_OF_UNITTEST) += unittest-data/
-
-CFLAGS_fdt.o = -I$(src)/../../scripts/dtc/libfdt
-CFLAGS_fdt_address.o = -I$(src)/../../scripts/dtc/libfdt
diff --git a/drivers/of/address.c b/drivers/of/address.c
index 78a7dcbec7d8..8bfda6ade2c0 100644
--- a/drivers/of/address.c
+++ b/drivers/of/address.c
@@ -712,7 +712,7 @@ int __weak pci_register_io_range(phys_addr_t addr, resource_size_t size)
 	}
 
 	/* add the range to the list */
-	range = kzalloc(sizeof(*range), GFP_KERNEL);
+	range = kzalloc(sizeof(*range), GFP_ATOMIC);
 	if (!range) {
 		err = -ENOMEM;
 		goto end_register;
@@ -765,7 +765,7 @@ unsigned long __weak pci_address_to_pio(phys_addr_t address)
 	spin_lock(&io_range_lock);
 	list_for_each_entry(res, &io_range_list, list) {
 		if (address >= res->start && address < res->start + res->size) {
-			addr = res->start - address + offset;
+			addr = address - res->start + offset;
 			break;
 		}
 		offset += res->size;
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 62bb4ef96c0c..8b5a187a7682 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -89,7 +89,7 @@ EXPORT_SYMBOL(of_n_size_cells);
 #ifdef CONFIG_NUMA
 int __weak of_node_to_nid(struct device_node *np)
 {
-	return numa_node_id();
+	return NUMA_NO_NODE;
 }
 #endif
 
@@ -189,7 +189,7 @@ int __of_attach_node_sysfs(struct device_node *np)
 	return 0;
 }
 
-static int __init of_init(void)
+void __init of_core_init(void)
 {
 	struct device_node *np;
 
@@ -198,7 +198,8 @@ static int __init of_init(void)
 	of_kset = kset_create_and_add("devicetree", NULL, firmware_kobj);
 	if (!of_kset) {
 		mutex_unlock(&of_mutex);
-		return -ENOMEM;
+		pr_err("devicetree: failed to register existing nodes\n");
+		return;
 	}
 	for_each_of_allnodes(np)
 		__of_attach_node_sysfs(np);
@@ -207,10 +208,7 @@ static int __init of_init(void)
 	/* Symlink in /proc as required by userspace ABI */
 	if (of_root)
 		proc_symlink("device-tree", NULL, "/sys/firmware/devicetree/base");
-
-	return 0;
 }
-core_initcall(of_init);
 
 static struct property *__of_find_property(const struct device_node *np,
 					   const char *name, int *lenp)
diff --git a/drivers/of/device.c b/drivers/of/device.c
index 20c1332a0018..8b91ea241b10 100644
--- a/drivers/of/device.c
+++ b/drivers/of/device.c
@@ -163,6 +163,18 @@ void of_device_unregister(struct platform_device *ofdev)
 }
 EXPORT_SYMBOL(of_device_unregister);
 
+const void *of_device_get_match_data(const struct device *dev)
+{
+	const struct of_device_id *match;
+
+	match = of_match_device(dev->driver->of_match_table, dev);
+	if (!match)
+		return NULL;
+
+	return match->data;
+}
+EXPORT_SYMBOL(of_device_get_match_data);
+
 ssize_t of_device_get_modalias(struct device *dev, char *str, ssize_t len)
 {
 	const char *compat;
diff --git a/drivers/of/dynamic.c b/drivers/of/dynamic.c
index 3351ef408125..53826b84e0ec 100644
--- a/drivers/of/dynamic.c
+++ b/drivers/of/dynamic.c
@@ -225,7 +225,7 @@ void __of_attach_node(struct device_node *np)
 	phandle = __of_get_property(np, "phandle", &sz);
 	if (!phandle)
 		phandle = __of_get_property(np, "linux,phandle", &sz);
-	if (IS_ENABLED(PPC_PSERIES) && !phandle)
+	if (IS_ENABLED(CONFIG_PPC_PSERIES) && !phandle)
 		phandle = __of_get_property(np, "ibm,phandle", &sz);
 	np->phandle = (phandle && (sz >= 4)) ? be32_to_cpup(phandle) : 0;
 
diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
index cde35c5d0191..07496560e5b9 100644
--- a/drivers/of/fdt.c
+++ b/drivers/of/fdt.c
@@ -164,11 +164,14 @@ static void *unflatten_dt_alloc(void **mem, unsigned long size,
  * unflatten_dt_node - Alloc and populate a device_node from the flat tree
  * @blob: The parent device tree blob
  * @mem: Memory chunk to use for allocating device nodes and properties
- * @p: pointer to node in flat tree
+ * @poffset: pointer to node in flat tree
  * @dad: Parent struct device_node
+ * @nodepp: The device_node tree created by the call
  * @fpsize: Size of the node path up at the current depth.
+ * @dryrun: If true, do not allocate device nodes but still calculate needed
+ * memory size
  */
-static void * unflatten_dt_node(void *blob,
+static void * unflatten_dt_node(const void *blob,
 				void *mem,
 				int *poffset,
 				struct device_node *dad,
@@ -378,7 +381,7 @@ static void * unflatten_dt_node(void *blob,
  * @dt_alloc: An allocator that provides a virtual address to memory
  * for the resulting tree
  */
-static void __unflatten_device_tree(void *blob,
+static void __unflatten_device_tree(const void *blob,
 			     struct device_node **mynodes,
 			     void * (*dt_alloc)(u64 size, u64 align))
 {
@@ -441,7 +444,7 @@ static void *kernel_tree_alloc(u64 size, u64 align)
  * pointers of the nodes so the normal device-tree walking functions
  * can be used.
  */
-void of_fdt_unflatten_tree(unsigned long *blob,
+void of_fdt_unflatten_tree(const unsigned long *blob,
 			struct device_node **mynodes)
 {
 	__unflatten_device_tree(blob, mynodes, &kernel_tree_alloc);
@@ -580,11 +583,6 @@ void __init early_init_fdt_scan_reserved_mem(void)
 	if (!initial_boot_params)
 		return;
 
-	/* Reserve the dtb region */
-	early_init_dt_reserve_memory_arch(__pa(initial_boot_params),
-					  fdt_totalsize(initial_boot_params),
-					  0);
-
 	/* Process header /memreserve/ fields */
 	for (n = 0; ; n++) {
 		fdt_get_mem_rsv(initial_boot_params, n, &base, &size);
@@ -598,6 +596,20 @@ void __init early_init_fdt_scan_reserved_mem(void)
 }
 
 /**
+ * early_init_fdt_reserve_self() - reserve the memory used by the FDT blob
+ */
+void __init early_init_fdt_reserve_self(void)
+{
+	if (!initial_boot_params)
+		return;
+
+	/* Reserve the dtb region */
+	early_init_dt_reserve_memory_arch(__pa(initial_boot_params),
+					  fdt_totalsize(initial_boot_params),
+					  0);
+}
+
+/**
  * of_scan_flat_dt - scan flattened tree blob and call callback on each.
  * @it: callback function
  * @data: context data pointer
@@ -1015,6 +1027,11 @@ void * __init __weak early_init_dt_alloc_memory_arch(u64 size, u64 align)
 	return __va(memblock_alloc(size, align));
 }
 #else
+void __init __weak early_init_dt_add_memory_arch(u64 base, u64 size)
+{
+	WARN_ON(1);
+}
+
 int __init __weak early_init_dt_reserve_memory_arch(phys_addr_t base,
 					phys_addr_t size, bool nomap)
 {
@@ -1022,6 +1039,12 @@ int __init __weak early_init_dt_reserve_memory_arch(phys_addr_t base,
 		  &base, &size, nomap ? " (nomap)" : "");
 	return -ENOSYS;
 }
+
+void * __init __weak early_init_dt_alloc_memory_arch(u64 size, u64 align)
+{
+	WARN_ON(1);
+	return NULL;
+}
 #endif
 
 bool __init early_init_dt_verify(void *params)
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 1a7980692f25..3cf7a01f557f 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -252,8 +252,6 @@ int of_irq_parse_raw(const __be32 *addr, struct of_phandle_args *out_irq)
 		 * Successfully parsed an interrrupt-map translation; copy new
 		 * interrupt specifier into the out_irq structure
 		 */
-		out_irq->np = newpar;
-
 		match_array = imap - newaddrsize - newintsize;
 		for (i = 0; i < newintsize; i++)
 			out_irq->args[i] = be32_to_cpup(imap - newintsize + i);
@@ -262,6 +260,7 @@ int of_irq_parse_raw(const __be32 *addr, struct of_phandle_args *out_irq)
 
 	skiplevel:
 		/* Iterate again with new parent */
+		out_irq->np = newpar;
 		pr_debug(" -> new parent: %s\n", of_node_full_name(newpar));
 		of_node_put(ipar);
 		ipar = newpar;
@@ -469,7 +468,7 @@ int of_irq_to_resource_table(struct device_node *dev, struct resource *res,
 }
 EXPORT_SYMBOL_GPL(of_irq_to_resource_table);
 
-struct intc_desc {
+struct of_intc_desc {
 	struct list_head	list;
 	struct device_node	*dev;
 	struct device_node	*interrupt_parent;
@@ -485,7 +484,7 @@ struct intc_desc {
 void __init of_irq_init(const struct of_device_id *matches)
 {
 	struct device_node *np, *parent = NULL;
-	struct intc_desc *desc, *temp_desc;
+	struct of_intc_desc *desc, *temp_desc;
 	struct list_head intc_desc_list, intc_parent_list;
 
 	INIT_LIST_HEAD(&intc_desc_list);
@@ -496,7 +495,7 @@ void __init of_irq_init(const struct of_device_id *matches)
 				!of_device_is_available(np))
 			continue;
 		/*
-		 * Here, we allocate and populate an intc_desc with the node
+		 * Here, we allocate and populate an of_intc_desc with the node
 		 * pointer, interrupt-parent device_node etc.
 		 */
 		desc = kzalloc(sizeof(*desc), GFP_KERNEL);
diff --git a/drivers/of/of_mdio.c b/drivers/of/of_mdio.c
index 0c064485d1c2..fdc60db60829 100644
--- a/drivers/of/of_mdio.c
+++ b/drivers/of/of_mdio.c
@@ -68,6 +68,9 @@ static int of_mdiobus_register_phy(struct mii_bus *mdio, struct device_node *chi
 			phy->irq = mdio->irq[addr];
 	}
 
+	if (of_property_read_bool(child, "broken-turn-around"))
+		mdio->phy_ignore_ta_mask |= 1 << addr;
+
 	/* Associate the OF node with the device structure so it
 	 * can be looked up later */
 	of_node_get(child);
diff --git a/drivers/of/overlay.c b/drivers/of/overlay.c
index dee9270ba547..24e025f79299 100644
--- a/drivers/of/overlay.c
+++ b/drivers/of/overlay.c
@@ -333,7 +333,7 @@ static DEFINE_IDR(ov_idr);
  * of the overlay in a list. This list can be used to prevent
  * illegal overlay removals.
  *
- * Returns the id of the created overlay, or an negative error number
+ * Returns the id of the created overlay, or a negative error number
  */
 int of_overlay_create(struct device_node *tree)
 {
@@ -481,7 +481,7 @@ static int overlay_removal_is_ok(struct of_overlay *ov)
  *
  * Removes an overlay if it is permissible.
  *
- * Returns 0 on success, or an negative error number
+ * Returns 0 on success, or a negative error number
  */
 int of_overlay_destroy(int id)
 {
@@ -528,7 +528,7 @@ EXPORT_SYMBOL_GPL(of_overlay_destroy);
  *
  * Removes all overlays from the system in the correct order.
  *
- * Returns 0 on success, or an negative error number
+ * Returns 0 on success, or a negative error number
  */
 int of_overlay_destroy_all(void)
 {
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index a01f57c9e34e..ddf8e42c9367 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -25,6 +25,7 @@
 
 const struct of_device_id of_default_bus_match_table[] = {
 	{ .compatible = "simple-bus", },
+	{ .compatible = "simple-mfd", },
 #ifdef CONFIG_ARM_AMBA
 	{ .compatible = "arm,amba-bus", },
 #endif /* CONFIG_ARM_AMBA */
diff --git a/drivers/parport/parport_pc.c b/drivers/parport/parport_pc.c
index 53d15b30636a..78530d1714dc 100644
--- a/drivers/parport/parport_pc.c
+++ b/drivers/parport/parport_pc.c
@@ -2255,7 +2255,7 @@ out5:
 		release_region(base+0x3, 5);
 	release_region(base, 3);
 out4:
-	parport_put_port(p);
+	parport_del_port(p);
 out3:
 	kfree(priv);
 out2:
@@ -2294,7 +2294,7 @@ void parport_pc_unregister_port(struct parport *p)
 				    priv->dma_handle);
 #endif
 	kfree(p->private_data);
-	parport_put_port(p);
+	parport_del_port(p);
 	kfree(ops); /* hope no-one cached it */
 }
 EXPORT_SYMBOL(parport_pc_unregister_port);
diff --git a/drivers/parport/procfs.c b/drivers/parport/procfs.c
index 3b470801a04f..c776333a68bc 100644
--- a/drivers/parport/procfs.c
+++ b/drivers/parport/procfs.c
@@ -21,6 +21,7 @@
 #include <linux/parport.h>
 #include <linux/ctype.h>
 #include <linux/sysctl.h>
+#include <linux/device.h>
 
 #include <asm/uaccess.h>
 
@@ -558,8 +559,18 @@ int parport_device_proc_unregister(struct pardevice *device)
 
 static int __init parport_default_proc_register(void)
 {
+	int ret;
+
 	parport_default_sysctl_table.sysctl_header =
 		register_sysctl_table(parport_default_sysctl_table.dev_dir);
+	if (!parport_default_sysctl_table.sysctl_header)
+		return -ENOMEM;
+	ret = parport_bus_init();
+	if (ret) {
+		unregister_sysctl_table(parport_default_sysctl_table.
+					sysctl_header);
+		return ret;
+	}
 	return 0;
 }
 
@@ -570,6 +581,7 @@ static void __exit parport_default_proc_unregister(void)
 					sysctl_header);
 		parport_default_sysctl_table.sysctl_header = NULL;
 	}
+	parport_bus_exit();
 }
 
 #else /* no sysctl or no procfs*/
@@ -596,11 +608,12 @@ int parport_device_proc_unregister(struct pardevice *device)
 
 static int __init parport_default_proc_register (void)
 {
-	return 0;
+	return parport_bus_init();
 }
 
 static void __exit parport_default_proc_unregister (void)
 {
+	parport_bus_exit();
 }
 #endif
 
diff --git a/drivers/parport/share.c b/drivers/parport/share.c
index 3fa66244ce32..8067f54ce050 100644
--- a/drivers/parport/share.c
+++ b/drivers/parport/share.c
@@ -29,6 +29,7 @@
 #include <linux/slab.h>
 #include <linux/sched.h>
 #include <linux/kmod.h>
+#include <linux/device.h>
 
 #include <linux/spinlock.h>
 #include <linux/mutex.h>
@@ -100,13 +101,91 @@ static struct parport_operations dead_ops = {
 	.owner		= NULL,
 };
 
+static struct device_type parport_device_type = {
+	.name = "parport",
+};
+
+static int is_parport(struct device *dev)
+{
+	return dev->type == &parport_device_type;
+}
+
+static int parport_probe(struct device *dev)
+{
+	struct parport_driver *drv;
+
+	if (is_parport(dev))
+		return -ENODEV;
+
+	drv = to_parport_driver(dev->driver);
+	if (!drv->probe) {
+		/* if driver has not defined a custom probe */
+		struct pardevice *par_dev = to_pardevice(dev);
+
+		if (strcmp(par_dev->name, drv->name))
+			return -ENODEV;
+		return 0;
+	}
+	/* if driver defined its own probe */
+	return drv->probe(to_pardevice(dev));
+}
+
+static struct bus_type parport_bus_type = {
+	.name = "parport",
+	.probe = parport_probe,
+};
+
+int parport_bus_init(void)
+{
+	return bus_register(&parport_bus_type);
+}
+
+void parport_bus_exit(void)
+{
+	bus_unregister(&parport_bus_type);
+}
+
+/*
+ * iterates through all the drivers registered with the bus and sends the port
+ * details to the match_port callback of the driver, so that the driver can
+ * know about the new port that just regsitered with the bus and decide if it
+ * wants to use this new port.
+ */
+static int driver_check(struct device_driver *dev_drv, void *_port)
+{
+	struct parport *port = _port;
+	struct parport_driver *drv = to_parport_driver(dev_drv);
+
+	if (drv->match_port)
+		drv->match_port(port);
+	return 0;
+}
+
 /* Call attach(port) for each registered driver. */
 static void attach_driver_chain(struct parport *port)
 {
 	/* caller has exclusive registration_lock */
 	struct parport_driver *drv;
+
 	list_for_each_entry(drv, &drivers, list)
 		drv->attach(port);
+
+	/*
+	 * call the driver_check function of the drivers registered in
+	 * new device model
+	 */
+
+	bus_for_each_drv(&parport_bus_type, NULL, port, driver_check);
+}
+
+static int driver_detach(struct device_driver *_drv, void *_port)
+{
+	struct parport *port = _port;
+	struct parport_driver *drv = to_parport_driver(_drv);
+
+	if (drv->detach)
+		drv->detach(port);
+	return 0;
 }
 
 /* Call detach(port) for each registered driver. */
@@ -116,6 +195,13 @@ static void detach_driver_chain(struct parport *port)
 	/* caller has exclusive registration_lock */
 	list_for_each_entry(drv, &drivers, list)
 		drv->detach (port);
+
+	/*
+	 * call the detach function of the drivers registered in
+	 * new device model
+	 */
+
+	bus_for_each_drv(&parport_bus_type, NULL, port, driver_detach);
 }
 
 /* Ask kmod for some lowlevel drivers. */
@@ -126,17 +212,39 @@ static void get_lowlevel_driver (void)
 	request_module ("parport_lowlevel");
 }
 
+/*
+ * iterates through all the devices connected to the bus and sends the device
+ * details to the match_port callback of the driver, so that the driver can
+ * know what are all the ports that are connected to the bus and choose the
+ * port to which it wants to register its device.
+ */
+static int port_check(struct device *dev, void *dev_drv)
+{
+	struct parport_driver *drv = dev_drv;
+
+	/* only send ports, do not send other devices connected to bus */
+	if (is_parport(dev))
+		drv->match_port(to_parport_dev(dev));
+	return 0;
+}
+
 /**
  *	parport_register_driver - register a parallel port device driver
  *	@drv: structure describing the driver
+ *	@owner: owner module of drv
+ *	@mod_name: module name string
  *
  *	This can be called by a parallel port device driver in order
  *	to receive notifications about ports being found in the
  *	system, as well as ports no longer available.
  *
+ *	If devmodel is true then the new device model is used
+ *	for registration.
+ *
  *	The @drv structure is allocated by the caller and must not be
  *	deallocated until after calling parport_unregister_driver().
  *
+ *	If using the non device model:
  *	The driver's attach() function may block.  The port that
  *	attach() is given will be valid for the duration of the
  *	callback, but if the driver wants to take a copy of the
@@ -148,21 +256,57 @@ static void get_lowlevel_driver (void)
  *	callback, but if the driver wants to take a copy of the
  *	pointer it must call parport_get_port() to do so.
  *
- *	Returns 0 on success.  Currently it always succeeds.
+ *
+ *	Returns 0 on success. The non device model will always succeeds.
+ *	but the new device model can fail and will return the error code.
  **/
 
-int parport_register_driver (struct parport_driver *drv)
+int __parport_register_driver(struct parport_driver *drv, struct module *owner,
+			      const char *mod_name)
 {
-	struct parport *port;
-
 	if (list_empty(&portlist))
 		get_lowlevel_driver ();
 
-	mutex_lock(&registration_lock);
-	list_for_each_entry(port, &portlist, list)
-		drv->attach(port);
-	list_add(&drv->list, &drivers);
-	mutex_unlock(&registration_lock);
+	if (drv->devmodel) {
+		/* using device model */
+		int ret;
+
+		/* initialize common driver fields */
+		drv->driver.name = drv->name;
+		drv->driver.bus = &parport_bus_type;
+		drv->driver.owner = owner;
+		drv->driver.mod_name = mod_name;
+		ret = driver_register(&drv->driver);
+		if (ret)
+			return ret;
+
+		mutex_lock(&registration_lock);
+		if (drv->match_port)
+			bus_for_each_dev(&parport_bus_type, NULL, drv,
+					 port_check);
+		mutex_unlock(&registration_lock);
+	} else {
+		struct parport *port;
+
+		drv->devmodel = false;
+
+		mutex_lock(&registration_lock);
+		list_for_each_entry(port, &portlist, list)
+			drv->attach(port);
+		list_add(&drv->list, &drivers);
+		mutex_unlock(&registration_lock);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(__parport_register_driver);
+
+static int port_detach(struct device *dev, void *_drv)
+{
+	struct parport_driver *drv = _drv;
+
+	if (is_parport(dev) && drv->detach)
+		drv->detach(to_parport_dev(dev));
 
 	return 0;
 }
@@ -189,15 +333,22 @@ void parport_unregister_driver (struct parport_driver *drv)
 	struct parport *port;
 
 	mutex_lock(&registration_lock);
-	list_del_init(&drv->list);
-	list_for_each_entry(port, &portlist, list)
-		drv->detach(port);
+	if (drv->devmodel) {
+		bus_for_each_dev(&parport_bus_type, NULL, drv, port_detach);
+		driver_unregister(&drv->driver);
+	} else {
+		list_del_init(&drv->list);
+		list_for_each_entry(port, &portlist, list)
+			drv->detach(port);
+	}
 	mutex_unlock(&registration_lock);
 }
 
-static void free_port (struct parport *port)
+static void free_port(struct device *dev)
 {
 	int d;
+	struct parport *port = to_parport_dev(dev);
+
 	spin_lock(&full_list_lock);
 	list_del(&port->full_list);
 	spin_unlock(&full_list_lock);
@@ -223,25 +374,29 @@ static void free_port (struct parport *port)
 
 struct parport *parport_get_port (struct parport *port)
 {
-	atomic_inc (&port->ref_count);
-	return port;
+	struct device *dev = get_device(&port->bus_dev);
+
+	return to_parport_dev(dev);
+}
+
+void parport_del_port(struct parport *port)
+{
+	device_unregister(&port->bus_dev);
 }
+EXPORT_SYMBOL(parport_del_port);
 
 /**
  *	parport_put_port - decrement a port's reference count
  *	@port: the port
  *
  *	This should be called once for each call to parport_get_port(),
- *	once the port is no longer needed.
+ *	once the port is no longer needed. When the reference count reaches
+ *	zero (port is no longer used), free_port is called.
  **/
 
 void parport_put_port (struct parport *port)
 {
-	if (atomic_dec_and_test (&port->ref_count))
-		/* Can destroy it now. */
-		free_port (port);
-
-	return;
+	put_device(&port->bus_dev);
 }
 
 /**
@@ -281,6 +436,7 @@ struct parport *parport_register_port(unsigned long base, int irq, int dma,
 	int num;
 	int device;
 	char *name;
+	int ret;
 
 	tmp = kzalloc(sizeof(struct parport), GFP_KERNEL);
 	if (!tmp) {
@@ -333,6 +489,10 @@ struct parport *parport_register_port(unsigned long base, int irq, int dma,
 	 */
 	sprintf(name, "parport%d", tmp->portnum = tmp->number);
 	tmp->name = name;
+	tmp->bus_dev.bus = &parport_bus_type;
+	tmp->bus_dev.release = free_port;
+	dev_set_name(&tmp->bus_dev, name);
+	tmp->bus_dev.type = &parport_device_type;
 
 	for (device = 0; device < 5; device++)
 		/* assume the worst */
@@ -340,6 +500,12 @@ struct parport *parport_register_port(unsigned long base, int irq, int dma,
 
 	tmp->waithead = tmp->waittail = NULL;
 
+	ret = device_register(&tmp->bus_dev);
+	if (ret) {
+		put_device(&tmp->bus_dev);
+		return NULL;
+	}
+
 	return tmp;
 }
 
@@ -542,6 +708,20 @@ parport_register_device(struct parport *port, const char *name,
 		}
 	}
 
+	if (flags & PARPORT_DEV_EXCL) {
+		if (port->physport->devices) {
+			/*
+			 * If a device is already registered and this new
+			 * device wants exclusive access, then no need to
+			 * continue as we can not grant exclusive access to
+			 * this device.
+			 */
+			pr_err("%s: cannot grant exclusive access for device %s\n",
+			       port->name, name);
+			return NULL;
+		}
+	}
+
 	/* We up our own module reference count, and that of the port
            on which a device is to be registered, to ensure that
            neither of us gets unloaded while we sleep in (e.g.)
@@ -575,6 +755,7 @@ parport_register_device(struct parport *port, const char *name,
 	tmp->irq_func = irq_func;
 	tmp->waiting = 0;
 	tmp->timeout = 5 * HZ;
+	tmp->devmodel = false;
 
 	/* Chain this onto the list */
 	tmp->prev = NULL;
@@ -630,6 +811,150 @@ parport_register_device(struct parport *port, const char *name,
 	return NULL;
 }
 
+static void free_pardevice(struct device *dev)
+{
+	struct pardevice *par_dev = to_pardevice(dev);
+
+	kfree(par_dev->name);
+	kfree(par_dev);
+}
+
+struct pardevice *
+parport_register_dev_model(struct parport *port, const char *name,
+			   const struct pardev_cb *par_dev_cb, int id)
+{
+	struct pardevice *par_dev;
+	int ret;
+	char *devname;
+
+	if (port->physport->flags & PARPORT_FLAG_EXCL) {
+		/* An exclusive device is registered. */
+		pr_err("%s: no more devices allowed\n", port->name);
+		return NULL;
+	}
+
+	if (par_dev_cb->flags & PARPORT_DEV_LURK) {
+		if (!par_dev_cb->preempt || !par_dev_cb->wakeup) {
+			pr_info("%s: refused to register lurking device (%s) without callbacks\n",
+				port->name, name);
+			return NULL;
+		}
+	}
+
+	if (par_dev_cb->flags & PARPORT_DEV_EXCL) {
+		if (port->physport->devices) {
+			/*
+			 * If a device is already registered and this new
+			 * device wants exclusive access, then no need to
+			 * continue as we can not grant exclusive access to
+			 * this device.
+			 */
+			pr_err("%s: cannot grant exclusive access for device %s\n",
+			       port->name, name);
+			return NULL;
+		}
+	}
+
+	if (!try_module_get(port->ops->owner))
+		return NULL;
+
+	parport_get_port(port);
+
+	par_dev = kzalloc(sizeof(*par_dev), GFP_KERNEL);
+	if (!par_dev)
+		goto err_put_port;
+
+	par_dev->state = kzalloc(sizeof(*par_dev->state), GFP_KERNEL);
+	if (!par_dev->state)
+		goto err_put_par_dev;
+
+	devname = kstrdup(name, GFP_KERNEL);
+	if (!devname)
+		goto err_free_par_dev;
+
+	par_dev->name = devname;
+	par_dev->port = port;
+	par_dev->daisy = -1;
+	par_dev->preempt = par_dev_cb->preempt;
+	par_dev->wakeup = par_dev_cb->wakeup;
+	par_dev->private = par_dev_cb->private;
+	par_dev->flags = par_dev_cb->flags;
+	par_dev->irq_func = par_dev_cb->irq_func;
+	par_dev->waiting = 0;
+	par_dev->timeout = 5 * HZ;
+
+	par_dev->dev.parent = &port->bus_dev;
+	par_dev->dev.bus = &parport_bus_type;
+	ret = dev_set_name(&par_dev->dev, "%s.%d", devname, id);
+	if (ret)
+		goto err_free_devname;
+	par_dev->dev.release = free_pardevice;
+	par_dev->devmodel = true;
+	ret = device_register(&par_dev->dev);
+	if (ret)
+		goto err_put_dev;
+
+	/* Chain this onto the list */
+	par_dev->prev = NULL;
+	/*
+	 * This function must not run from an irq handler so we don' t need
+	 * to clear irq on the local CPU. -arca
+	 */
+	spin_lock(&port->physport->pardevice_lock);
+
+	if (par_dev_cb->flags & PARPORT_DEV_EXCL) {
+		if (port->physport->devices) {
+			spin_unlock(&port->physport->pardevice_lock);
+			pr_debug("%s: cannot grant exclusive access for device %s\n",
+				 port->name, name);
+			goto err_put_dev;
+		}
+		port->flags |= PARPORT_FLAG_EXCL;
+	}
+
+	par_dev->next = port->physport->devices;
+	wmb();	/*
+		 * Make sure that tmp->next is written before it's
+		 * added to the list; see comments marked 'no locking
+		 * required'
+		 */
+	if (port->physport->devices)
+		port->physport->devices->prev = par_dev;
+	port->physport->devices = par_dev;
+	spin_unlock(&port->physport->pardevice_lock);
+
+	init_waitqueue_head(&par_dev->wait_q);
+	par_dev->timeslice = parport_default_timeslice;
+	par_dev->waitnext = NULL;
+	par_dev->waitprev = NULL;
+
+	/*
+	 * This has to be run as last thing since init_state may need other
+	 * pardevice fields. -arca
+	 */
+	port->ops->init_state(par_dev, par_dev->state);
+	port->proc_device = par_dev;
+	parport_device_proc_register(par_dev);
+
+	return par_dev;
+
+err_put_dev:
+	put_device(&par_dev->dev);
+err_free_devname:
+	kfree(devname);
+err_free_par_dev:
+	kfree(par_dev->state);
+err_put_par_dev:
+	if (!par_dev->devmodel)
+		kfree(par_dev);
+err_put_port:
+	parport_put_port(port);
+	module_put(port->ops->owner);
+
+	return NULL;
+}
+EXPORT_SYMBOL(parport_register_dev_model);
+
 /**
  *	parport_unregister_device - deregister a device on a parallel port
  *	@dev: pointer to structure representing device
@@ -691,7 +1016,10 @@ void parport_unregister_device(struct pardevice *dev)
 	spin_unlock_irq(&port->waitlist_lock);
 
 	kfree(dev->state);
-	kfree(dev);
+	if (dev->devmodel)
+		device_unregister(&dev->dev);
+	else
+		kfree(dev);
 
 	module_put(port->ops->owner);
 	parport_put_port (port);
@@ -1019,7 +1347,6 @@ EXPORT_SYMBOL(parport_release);
 EXPORT_SYMBOL(parport_register_port);
 EXPORT_SYMBOL(parport_announce_port);
 EXPORT_SYMBOL(parport_remove_port);
-EXPORT_SYMBOL(parport_register_driver);
 EXPORT_SYMBOL(parport_unregister_driver);
 EXPORT_SYMBOL(parport_register_device);
 EXPORT_SYMBOL(parport_unregister_device);
diff --git a/drivers/pci/Kconfig b/drivers/pci/Kconfig
index 7a8f1c5e65af..73de4efcbe6e 100644
--- a/drivers/pci/Kconfig
+++ b/drivers/pci/Kconfig
@@ -1,6 +1,10 @@
 #
 # PCI configuration
 #
+config PCI_BUS_ADDR_T_64BIT
+	def_bool y if (ARCH_DMA_ADDR_T_64BIT || 64BIT)
+	depends on PCI
+
 config PCI_MSI
 	bool "Message Signaled Interrupts (MSI and MSI-X)"
 	depends on PCI
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index 90fa3a78fb7c..6fbd3f2b5992 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -92,11 +92,11 @@ void pci_bus_remove_resources(struct pci_bus *bus)
 }
 
 static struct pci_bus_region pci_32_bit = {0, 0xffffffffULL};
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+#ifdef CONFIG_PCI_BUS_ADDR_T_64BIT
 static struct pci_bus_region pci_64_bit = {0,
-				(dma_addr_t) 0xffffffffffffffffULL};
-static struct pci_bus_region pci_high = {(dma_addr_t) 0x100000000ULL,
-				(dma_addr_t) 0xffffffffffffffffULL};
+				(pci_bus_addr_t) 0xffffffffffffffffULL};
+static struct pci_bus_region pci_high = {(pci_bus_addr_t) 0x100000000ULL,
+				(pci_bus_addr_t) 0xffffffffffffffffULL};
 #endif
 
 /*
@@ -200,7 +200,7 @@ int pci_bus_alloc_resource(struct pci_bus *bus, struct resource *res,
 					  resource_size_t),
 		void *alignf_data)
 {
-#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
+#ifdef CONFIG_PCI_BUS_ADDR_T_64BIT
 	int rc;
 
 	if (res->flags & IORESOURCE_MEM_64) {
diff --git a/drivers/pci/host/Kconfig b/drivers/pci/host/Kconfig
index 1dfb567b3522..c132bddc03f3 100644
--- a/drivers/pci/host/Kconfig
+++ b/drivers/pci/host/Kconfig
@@ -89,11 +89,20 @@ config PCI_XGENE
 	depends on ARCH_XGENE
 	depends on OF
 	select PCIEPORTBUS
+	select PCI_MSI_IRQ_DOMAIN if PCI_MSI
 	help
 	  Say Y here if you want internal PCI support on APM X-Gene SoC.
 	  There are 5 internal PCIe ports available. Each port is GEN3 capable
 	  and have varied lanes from x1 to x8.
 
+config PCI_XGENE_MSI
+	bool "X-Gene v1 PCIe MSI feature"
+	depends on PCI_XGENE && PCI_MSI
+	default y
+	help
+	  Say Y here if you want PCIe MSI support for the APM X-Gene v1 SoC.
+	  This MSI driver supports 5 PCIe ports on the APM X-Gene v1 SoC.
+
 config PCI_LAYERSCAPE
 	bool "Freescale Layerscape PCIe controller"
 	depends on OF && ARM
@@ -125,4 +134,15 @@ config PCIE_IPROC_PLATFORM
 	  Say Y here if you want to use the Broadcom iProc PCIe controller
 	  through the generic platform bus interface
 
+config PCIE_IPROC_BCMA
+	bool "Broadcom iProc PCIe BCMA bus driver"
+	depends on ARCH_BCM_IPROC || (ARM && COMPILE_TEST)
+	select PCIE_IPROC
+	select BCMA
+	select PCI_DOMAINS
+	default ARCH_BCM_5301X
+	help
+	  Say Y here if you want to use the Broadcom iProc PCIe controller
+	  through the BCMA bus interface
+
 endmenu
diff --git a/drivers/pci/host/Makefile b/drivers/pci/host/Makefile
index f733b4e27642..140d66f796e4 100644
--- a/drivers/pci/host/Makefile
+++ b/drivers/pci/host/Makefile
@@ -11,7 +11,9 @@ obj-$(CONFIG_PCIE_SPEAR13XX) += pcie-spear13xx.o
 obj-$(CONFIG_PCI_KEYSTONE) += pci-keystone-dw.o pci-keystone.o
 obj-$(CONFIG_PCIE_XILINX) += pcie-xilinx.o
 obj-$(CONFIG_PCI_XGENE) += pci-xgene.o
+obj-$(CONFIG_PCI_XGENE_MSI) += pci-xgene-msi.o
 obj-$(CONFIG_PCI_LAYERSCAPE) += pci-layerscape.o
 obj-$(CONFIG_PCI_VERSATILE) += pci-versatile.o
 obj-$(CONFIG_PCIE_IPROC) += pcie-iproc.o
 obj-$(CONFIG_PCIE_IPROC_PLATFORM) += pcie-iproc-platform.o
+obj-$(CONFIG_PCIE_IPROC_BCMA) += pcie-iproc-bcma.o
diff --git a/drivers/pci/host/pci-dra7xx.c b/drivers/pci/host/pci-dra7xx.c
index 2d57e19a2cd4..80db09e47800 100644
--- a/drivers/pci/host/pci-dra7xx.c
+++ b/drivers/pci/host/pci-dra7xx.c
@@ -93,9 +93,9 @@ static int dra7xx_pcie_link_up(struct pcie_port *pp)
 
 static int dra7xx_pcie_establish_link(struct pcie_port *pp)
 {
-	u32 reg;
-	unsigned int retries = 1000;
 	struct dra7xx_pcie *dra7xx = to_dra7xx_pcie(pp);
+	u32 reg;
+	unsigned int retries;
 
 	if (dw_pcie_link_up(pp)) {
 		dev_err(pp->dev, "link is already up\n");
@@ -106,19 +106,14 @@ static int dra7xx_pcie_establish_link(struct pcie_port *pp)
 	reg |= LTSSM_EN;
 	dra7xx_pcie_writel(dra7xx, PCIECTRL_DRA7XX_CONF_DEVICE_CMD, reg);
 
-	while (retries--) {
-		reg = dra7xx_pcie_readl(dra7xx,	PCIECTRL_DRA7XX_CONF_PHY_CS);
-		if (reg & LINK_UP)
-			break;
+	for (retries = 0; retries < 1000; retries++) {
+		if (dw_pcie_link_up(pp))
+			return 0;
 		usleep_range(10, 20);
 	}
 
-	if (retries == 0) {
-		dev_err(pp->dev, "link is not up\n");
-		return -ETIMEDOUT;
-	}
-
-	return 0;
+	dev_err(pp->dev, "link is not up\n");
+	return -EINVAL;
 }
 
 static void dra7xx_pcie_enable_interrupts(struct pcie_port *pp)
diff --git a/drivers/pci/host/pci-exynos.c b/drivers/pci/host/pci-exynos.c
index c139237e0e52..f9f468d9a819 100644
--- a/drivers/pci/host/pci-exynos.c
+++ b/drivers/pci/host/pci-exynos.c
@@ -316,9 +316,9 @@ static void exynos_pcie_assert_reset(struct pcie_port *pp)
 
 static int exynos_pcie_establish_link(struct pcie_port *pp)
 {
-	u32 val;
-	int count = 0;
 	struct exynos_pcie *exynos_pcie = to_exynos_pcie(pp);
+	u32 val;
+	unsigned int retries;
 
 	if (dw_pcie_link_up(pp)) {
 		dev_err(pp->dev, "Link already up\n");
@@ -357,27 +357,23 @@ static int exynos_pcie_establish_link(struct pcie_port *pp)
 			  PCIE_APP_LTSSM_ENABLE);
 
 	/* check if the link is up or not */
-	while (!dw_pcie_link_up(pp)) {
-		mdelay(100);
-		count++;
-		if (count == 10) {
-			while (exynos_phy_readl(exynos_pcie,
-						PCIE_PHY_PLL_LOCKED) == 0) {
-				val = exynos_blk_readl(exynos_pcie,
-						       PCIE_PHY_PLL_LOCKED);
-				dev_info(pp->dev, "PLL Locked: 0x%x\n", val);
-			}
-			/* power off phy */
-			exynos_pcie_power_off_phy(pp);
-
-			dev_err(pp->dev, "PCIe Link Fail\n");
-			return -EINVAL;
+	for (retries = 0; retries < 10; retries++) {
+		if (dw_pcie_link_up(pp)) {
+			dev_info(pp->dev, "Link up\n");
+			return 0;
 		}
+		mdelay(100);
 	}
 
-	dev_info(pp->dev, "Link up\n");
+	while (exynos_phy_readl(exynos_pcie, PCIE_PHY_PLL_LOCKED) == 0) {
+		val = exynos_blk_readl(exynos_pcie, PCIE_PHY_PLL_LOCKED);
+		dev_info(pp->dev, "PLL Locked: 0x%x\n", val);
+	}
+	/* power off phy */
+	exynos_pcie_power_off_phy(pp);
 
-	return 0;
+	dev_err(pp->dev, "PCIe Link Fail\n");
+	return -EINVAL;
 }
 
 static void exynos_pcie_clear_irq_pulse(struct pcie_port *pp)
diff --git a/drivers/pci/host/pci-imx6.c b/drivers/pci/host/pci-imx6.c
index fdb95367721e..233a196c6e66 100644
--- a/drivers/pci/host/pci-imx6.c
+++ b/drivers/pci/host/pci-imx6.c
@@ -47,6 +47,8 @@ struct imx6_pcie {
 #define PCIE_RC_LCR_MAX_LINK_SPEEDS_GEN2	0x2
 #define PCIE_RC_LCR_MAX_LINK_SPEEDS_MASK	0xf
 
+#define PCIE_RC_LCSR				0x80
+
 /* PCIe Port Logic registers (memory-mapped) */
 #define PL_OFFSET 0x700
 #define PCIE_PL_PFLR (PL_OFFSET + 0x08)
@@ -335,21 +337,36 @@ static void imx6_pcie_init_phy(struct pcie_port *pp)
 
 static int imx6_pcie_wait_for_link(struct pcie_port *pp)
 {
-	int count = 200;
+	unsigned int retries;
 
-	while (!dw_pcie_link_up(pp)) {
+	for (retries = 0; retries < 200; retries++) {
+		if (dw_pcie_link_up(pp))
+			return 0;
 		usleep_range(100, 1000);
-		if (--count)
-			continue;
-
-		dev_err(pp->dev, "phy link never came up\n");
-		dev_dbg(pp->dev, "DEBUG_R0: 0x%08x, DEBUG_R1: 0x%08x\n",
-			readl(pp->dbi_base + PCIE_PHY_DEBUG_R0),
-			readl(pp->dbi_base + PCIE_PHY_DEBUG_R1));
-		return -EINVAL;
 	}
 
-	return 0;
+	dev_err(pp->dev, "phy link never came up\n");
+	dev_dbg(pp->dev, "DEBUG_R0: 0x%08x, DEBUG_R1: 0x%08x\n",
+		readl(pp->dbi_base + PCIE_PHY_DEBUG_R0),
+		readl(pp->dbi_base + PCIE_PHY_DEBUG_R1));
+	return -EINVAL;
+}
+
+static int imx6_pcie_wait_for_speed_change(struct pcie_port *pp)
+{
+	u32 tmp;
+	unsigned int retries;
+
+	for (retries = 0; retries < 200; retries++) {
+		tmp = readl(pp->dbi_base + PCIE_LINK_WIDTH_SPEED_CONTROL);
+		/* Test if the speed change finished. */
+		if (!(tmp & PORT_LOGIC_SPEED_CHANGE))
+			return 0;
+		usleep_range(100, 1000);
+	}
+
+	dev_err(pp->dev, "Speed change timeout\n");
+	return -EINVAL;
 }
 
 static irqreturn_t imx6_pcie_msi_handler(int irq, void *arg)
@@ -359,11 +376,11 @@ static irqreturn_t imx6_pcie_msi_handler(int irq, void *arg)
 	return dw_handle_msi_irq(pp);
 }
 
-static int imx6_pcie_start_link(struct pcie_port *pp)
+static int imx6_pcie_establish_link(struct pcie_port *pp)
 {
 	struct imx6_pcie *imx6_pcie = to_imx6_pcie(pp);
-	uint32_t tmp;
-	int ret, count;
+	u32 tmp;
+	int ret;
 
 	/*
 	 * Force Gen1 operation when starting the link.  In case the link is
@@ -397,29 +414,22 @@ static int imx6_pcie_start_link(struct pcie_port *pp)
 	tmp |= PORT_LOGIC_SPEED_CHANGE;
 	writel(tmp, pp->dbi_base + PCIE_LINK_WIDTH_SPEED_CONTROL);
 
-	count = 200;
-	while (count--) {
-		tmp = readl(pp->dbi_base + PCIE_LINK_WIDTH_SPEED_CONTROL);
-		/* Test if the speed change finished. */
-		if (!(tmp & PORT_LOGIC_SPEED_CHANGE))
-			break;
-		usleep_range(100, 1000);
+	ret = imx6_pcie_wait_for_speed_change(pp);
+	if (ret) {
+		dev_err(pp->dev, "Failed to bring link up!\n");
+		return ret;
 	}
 
 	/* Make sure link training is finished as well! */
-	if (count)
-		ret = imx6_pcie_wait_for_link(pp);
-	else
-		ret = -EINVAL;
-
+	ret = imx6_pcie_wait_for_link(pp);
 	if (ret) {
 		dev_err(pp->dev, "Failed to bring link up!\n");
-	} else {
-		tmp = readl(pp->dbi_base + 0x80);
-		dev_dbg(pp->dev, "Link up, Gen=%i\n", (tmp >> 16) & 0xf);
+		return ret;
 	}
 
-	return ret;
+	tmp = readl(pp->dbi_base + PCIE_RC_LCSR);
+	dev_dbg(pp->dev, "Link up, Gen=%i\n", (tmp >> 16) & 0xf);
+	return 0;
 }
 
 static void imx6_pcie_host_init(struct pcie_port *pp)
@@ -432,7 +442,7 @@ static void imx6_pcie_host_init(struct pcie_port *pp)
 
 	dw_pcie_setup_rc(pp);
 
-	imx6_pcie_start_link(pp);
+	imx6_pcie_establish_link(pp);
 
 	if (IS_ENABLED(CONFIG_PCI_MSI))
 		dw_pcie_msi_init(pp);
@@ -440,19 +450,19 @@ static void imx6_pcie_host_init(struct pcie_port *pp)
 
 static void imx6_pcie_reset_phy(struct pcie_port *pp)
 {
-	uint32_t temp;
+	u32 tmp;
 
-	pcie_phy_read(pp->dbi_base, PHY_RX_OVRD_IN_LO, &temp);
-	temp |= (PHY_RX_OVRD_IN_LO_RX_DATA_EN |
-		 PHY_RX_OVRD_IN_LO_RX_PLL_EN);
-	pcie_phy_write(pp->dbi_base, PHY_RX_OVRD_IN_LO, temp);
+	pcie_phy_read(pp->dbi_base, PHY_RX_OVRD_IN_LO, &tmp);
+	tmp |= (PHY_RX_OVRD_IN_LO_RX_DATA_EN |
+		PHY_RX_OVRD_IN_LO_RX_PLL_EN);
+	pcie_phy_write(pp->dbi_base, PHY_RX_OVRD_IN_LO, tmp);
 
 	usleep_range(2000, 3000);
 
-	pcie_phy_read(pp->dbi_base, PHY_RX_OVRD_IN_LO, &temp);
-	temp &= ~(PHY_RX_OVRD_IN_LO_RX_DATA_EN |
+	pcie_phy_read(pp->dbi_base, PHY_RX_OVRD_IN_LO, &tmp);
+	tmp &= ~(PHY_RX_OVRD_IN_LO_RX_DATA_EN |
 		  PHY_RX_OVRD_IN_LO_RX_PLL_EN);
-	pcie_phy_write(pp->dbi_base, PHY_RX_OVRD_IN_LO, temp);
+	pcie_phy_write(pp->dbi_base, PHY_RX_OVRD_IN_LO, tmp);
 }
 
 static int imx6_pcie_link_up(struct pcie_port *pp)
diff --git a/drivers/pci/host/pci-keystone.c b/drivers/pci/host/pci-keystone.c
index 75333b0c4f0a..734da589cdfb 100644
--- a/drivers/pci/host/pci-keystone.c
+++ b/drivers/pci/host/pci-keystone.c
@@ -88,7 +88,7 @@ DECLARE_PCI_FIXUP_ENABLE(PCI_ANY_ID, PCI_ANY_ID, quirk_limit_mrrs);
 static int ks_pcie_establish_link(struct keystone_pcie *ks_pcie)
 {
 	struct pcie_port *pp = &ks_pcie->pp;
-	int count = 200;
+	unsigned int retries;
 
 	dw_pcie_setup_rc(pp);
 
@@ -99,17 +99,15 @@ static int ks_pcie_establish_link(struct keystone_pcie *ks_pcie)
 
 	ks_dw_pcie_initiate_link_train(ks_pcie);
 	/* check if the link is up or not */
-	while (!dw_pcie_link_up(pp)) {
+	for (retries = 0; retries < 200; retries++) {
+		if (dw_pcie_link_up(pp))
+			return 0;
 		usleep_range(100, 1000);
-		if (--count) {
-			ks_dw_pcie_initiate_link_train(ks_pcie);
-			continue;
-		}
-		dev_err(pp->dev, "phy link never came up\n");
-		return -EINVAL;
+		ks_dw_pcie_initiate_link_train(ks_pcie);
 	}
 
-	return 0;
+	dev_err(pp->dev, "phy link never came up\n");
+	return -EINVAL;
 }
 
 static void ks_pcie_msi_irq_handler(unsigned int irq, struct irq_desc *desc)
@@ -223,10 +221,9 @@ static void ks_pcie_setup_interrupts(struct keystone_pcie *ks_pcie)
 	/* MSI IRQ */
 	if (IS_ENABLED(CONFIG_PCI_MSI)) {
 		for (i = 0; i < ks_pcie->num_msi_host_irqs; i++) {
-			irq_set_chained_handler(ks_pcie->msi_host_irqs[i],
-						ks_pcie_msi_irq_handler);
-			irq_set_handler_data(ks_pcie->msi_host_irqs[i],
-					     ks_pcie);
+			irq_set_chained_handler_and_data(ks_pcie->msi_host_irqs[i],
+							 ks_pcie_msi_irq_handler,
+							 ks_pcie);
 		}
 	}
 }
diff --git a/drivers/pci/host/pci-layerscape.c b/drivers/pci/host/pci-layerscape.c
index 4a6e62f67579..b2328ea13dcf 100644
--- a/drivers/pci/host/pci-layerscape.c
+++ b/drivers/pci/host/pci-layerscape.c
@@ -62,22 +62,27 @@ static int ls_pcie_link_up(struct pcie_port *pp)
 	return 1;
 }
 
+static int ls_pcie_establish_link(struct pcie_port *pp)
+{
+	unsigned int retries;
+
+	for (retries = 0; retries < 200; retries++) {
+		if (dw_pcie_link_up(pp))
+			return 0;
+		usleep_range(100, 1000);
+	}
+
+	dev_err(pp->dev, "phy link never came up\n");
+	return -EINVAL;
+}
+
 static void ls_pcie_host_init(struct pcie_port *pp)
 {
 	struct ls_pcie *pcie = to_ls_pcie(pp);
-	int count = 0;
 	u32 val;
 
 	dw_pcie_setup_rc(pp);
-
-	while (!ls_pcie_link_up(pp)) {
-		usleep_range(100, 1000);
-		count++;
-		if (count >= 200) {
-			dev_err(pp->dev, "phy link never came up\n");
-			return;
-		}
-	}
+	ls_pcie_establish_link(pp);
 
 	/*
 	 * LS1021A Workaround for internal TKT228622
diff --git a/drivers/pci/host/pci-mvebu.c b/drivers/pci/host/pci-mvebu.c
index 1ab863551920..70aa09556ec5 100644
--- a/drivers/pci/host/pci-mvebu.c
+++ b/drivers/pci/host/pci-mvebu.c
@@ -751,21 +751,6 @@ static int mvebu_pcie_setup(int nr, struct pci_sys_data *sys)
 	return 1;
 }
 
-static struct pci_bus *mvebu_pcie_scan_bus(int nr, struct pci_sys_data *sys)
-{
-	struct mvebu_pcie *pcie = sys_to_pcie(sys);
-	struct pci_bus *bus;
-
-	bus = pci_create_root_bus(&pcie->pdev->dev, sys->busnr,
-				  &mvebu_pcie_ops, sys, &sys->resources);
-	if (!bus)
-		return NULL;
-
-	pci_scan_child_bus(bus);
-
-	return bus;
-}
-
 static resource_size_t mvebu_pcie_align_resource(struct pci_dev *dev,
 						 const struct resource *res,
 						 resource_size_t start,
@@ -809,12 +794,11 @@ static void mvebu_pcie_enable(struct mvebu_pcie *pcie)
 	hw.nr_controllers = 1;
 	hw.private_data   = (void **)&pcie;
 	hw.setup          = mvebu_pcie_setup;
-	hw.scan           = mvebu_pcie_scan_bus;
 	hw.map_irq        = of_irq_parse_and_map_pci;
 	hw.ops            = &mvebu_pcie_ops;
 	hw.align_resource = mvebu_pcie_align_resource;
 
-	pci_common_init(&hw);
+	pci_common_init_dev(&pcie->pdev->dev, &hw);
 }
 
 /*
diff --git a/drivers/pci/host/pci-tegra.c b/drivers/pci/host/pci-tegra.c
index 00e92720d7f7..10c05718dbfd 100644
--- a/drivers/pci/host/pci-tegra.c
+++ b/drivers/pci/host/pci-tegra.c
@@ -630,21 +630,6 @@ static int tegra_pcie_map_irq(const struct pci_dev *pdev, u8 slot, u8 pin)
 	return irq;
 }
 
-static struct pci_bus *tegra_pcie_scan_bus(int nr, struct pci_sys_data *sys)
-{
-	struct tegra_pcie *pcie = sys_to_pcie(sys);
-	struct pci_bus *bus;
-
-	bus = pci_create_root_bus(pcie->dev, sys->busnr, &tegra_pcie_ops, sys,
-				  &sys->resources);
-	if (!bus)
-		return NULL;
-
-	pci_scan_child_bus(bus);
-
-	return bus;
-}
-
 static irqreturn_t tegra_pcie_isr(int irq, void *arg)
 {
 	const char *err_msg[] = {
@@ -1831,7 +1816,6 @@ static int tegra_pcie_enable(struct tegra_pcie *pcie)
 	hw.private_data = (void **)&pcie;
 	hw.setup = tegra_pcie_setup;
 	hw.map_irq = tegra_pcie_map_irq;
-	hw.scan = tegra_pcie_scan_bus;
 	hw.ops = &tegra_pcie_ops;
 
 	pci_common_init_dev(pcie->dev, &hw);
diff --git a/drivers/pci/host/pci-xgene-msi.c b/drivers/pci/host/pci-xgene-msi.c
new file mode 100644
index 000000000000..2d31d4d6fd08
--- /dev/null
+++ b/drivers/pci/host/pci-xgene-msi.c
@@ -0,0 +1,596 @@
+/*
+ * APM X-Gene MSI Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Author: Tanmay Inamdar <tinamdar@apm.com>
+ *	   Duc Dang <dhdang@apm.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/cpu.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/msi.h>
+#include <linux/of_irq.h>
+#include <linux/irqchip/chained_irq.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/of_pci.h>
+
+#define MSI_IR0			0x000000
+#define MSI_INT0		0x800000
+#define IDX_PER_GROUP		8
+#define IRQS_PER_IDX		16
+#define NR_HW_IRQS		16
+#define NR_MSI_VEC		(IDX_PER_GROUP * IRQS_PER_IDX * NR_HW_IRQS)
+
+struct xgene_msi_group {
+	struct xgene_msi	*msi;
+	int			gic_irq;
+	u32			msi_grp;
+};
+
+struct xgene_msi {
+	struct device_node	*node;
+	struct msi_controller	mchip;
+	struct irq_domain	*domain;
+	u64			msi_addr;
+	void __iomem		*msi_regs;
+	unsigned long		*bitmap;
+	struct mutex		bitmap_lock;
+	struct xgene_msi_group	*msi_groups;
+	int			num_cpus;
+};
+
+/* Global data */
+static struct xgene_msi xgene_msi_ctrl;
+
+static struct irq_chip xgene_msi_top_irq_chip = {
+	.name		= "X-Gene1 MSI",
+	.irq_enable	= pci_msi_unmask_irq,
+	.irq_disable	= pci_msi_mask_irq,
+	.irq_mask	= pci_msi_mask_irq,
+	.irq_unmask	= pci_msi_unmask_irq,
+};
+
+static struct  msi_domain_info xgene_msi_domain_info = {
+	.flags	= (MSI_FLAG_USE_DEF_DOM_OPS | MSI_FLAG_USE_DEF_CHIP_OPS |
+		  MSI_FLAG_PCI_MSIX),
+	.chip	= &xgene_msi_top_irq_chip,
+};
+
+/*
+ * X-Gene v1 has 16 groups of MSI termination registers MSInIRx, where
+ * n is group number (0..F), x is index of registers in each group (0..7)
+ * The register layout is as follows:
+ * MSI0IR0			base_addr
+ * MSI0IR1			base_addr +  0x10000
+ * ...				...
+ * MSI0IR6			base_addr +  0x60000
+ * MSI0IR7			base_addr +  0x70000
+ * MSI1IR0			base_addr +  0x80000
+ * MSI1IR1			base_addr +  0x90000
+ * ...				...
+ * MSI1IR7			base_addr +  0xF0000
+ * MSI2IR0			base_addr + 0x100000
+ * ...				...
+ * MSIFIR0			base_addr + 0x780000
+ * MSIFIR1			base_addr + 0x790000
+ * ...				...
+ * MSIFIR7			base_addr + 0x7F0000
+ * MSIINT0			base_addr + 0x800000
+ * MSIINT1			base_addr + 0x810000
+ * ...				...
+ * MSIINTF			base_addr + 0x8F0000
+ *
+ * Each index register supports 16 MSI vectors (0..15) to generate interrupt.
+ * There are total 16 GIC IRQs assigned for these 16 groups of MSI termination
+ * registers.
+ *
+ * Each MSI termination group has 1 MSIINTn register (n is 0..15) to indicate
+ * the MSI pending status caused by 1 of its 8 index registers.
+ */
+
+/* MSInIRx read helper */
+static u32 xgene_msi_ir_read(struct xgene_msi *msi,
+				    u32 msi_grp, u32 msir_idx)
+{
+	return readl_relaxed(msi->msi_regs + MSI_IR0 +
+			      (msi_grp << 19) + (msir_idx << 16));
+}
+
+/* MSIINTn read helper */
+static u32 xgene_msi_int_read(struct xgene_msi *msi, u32 msi_grp)
+{
+	return readl_relaxed(msi->msi_regs + MSI_INT0 + (msi_grp << 16));
+}
+
+/*
+ * With 2048 MSI vectors supported, the MSI message can be constructed using
+ * following scheme:
+ * - Divide into 8 256-vector groups
+ *		Group 0: 0-255
+ *		Group 1: 256-511
+ *		Group 2: 512-767
+ *		...
+ *		Group 7: 1792-2047
+ * - Each 256-vector group is divided into 16 16-vector groups
+ *	As an example: 16 16-vector groups for 256-vector group 0-255 is
+ *		Group 0: 0-15
+ *		Group 1: 16-32
+ *		...
+ *		Group 15: 240-255
+ * - The termination address of MSI vector in 256-vector group n and 16-vector
+ *   group x is the address of MSIxIRn
+ * - The data for MSI vector in 16-vector group x is x
+ */
+static u32 hwirq_to_reg_set(unsigned long hwirq)
+{
+	return (hwirq / (NR_HW_IRQS * IRQS_PER_IDX));
+}
+
+static u32 hwirq_to_group(unsigned long hwirq)
+{
+	return (hwirq % NR_HW_IRQS);
+}
+
+static u32 hwirq_to_msi_data(unsigned long hwirq)
+{
+	return ((hwirq / NR_HW_IRQS) % IRQS_PER_IDX);
+}
+
+static void xgene_compose_msi_msg(struct irq_data *data, struct msi_msg *msg)
+{
+	struct xgene_msi *msi = irq_data_get_irq_chip_data(data);
+	u32 reg_set = hwirq_to_reg_set(data->hwirq);
+	u32 group = hwirq_to_group(data->hwirq);
+	u64 target_addr = msi->msi_addr + (((8 * group) + reg_set) << 16);
+
+	msg->address_hi = upper_32_bits(target_addr);
+	msg->address_lo = lower_32_bits(target_addr);
+	msg->data = hwirq_to_msi_data(data->hwirq);
+}
+
+/*
+ * X-Gene v1 only has 16 MSI GIC IRQs for 2048 MSI vectors.  To maintain
+ * the expected behaviour of .set_affinity for each MSI interrupt, the 16
+ * MSI GIC IRQs are statically allocated to 8 X-Gene v1 cores (2 GIC IRQs
+ * for each core).  The MSI vector is moved fom 1 MSI GIC IRQ to another
+ * MSI GIC IRQ to steer its MSI interrupt to correct X-Gene v1 core.  As a
+ * consequence, the total MSI vectors that X-Gene v1 supports will be
+ * reduced to 256 (2048/8) vectors.
+ */
+static int hwirq_to_cpu(unsigned long hwirq)
+{
+	return (hwirq % xgene_msi_ctrl.num_cpus);
+}
+
+static unsigned long hwirq_to_canonical_hwirq(unsigned long hwirq)
+{
+	return (hwirq - hwirq_to_cpu(hwirq));
+}
+
+static int xgene_msi_set_affinity(struct irq_data *irqdata,
+				  const struct cpumask *mask, bool force)
+{
+	int target_cpu = cpumask_first(mask);
+	int curr_cpu;
+
+	curr_cpu = hwirq_to_cpu(irqdata->hwirq);
+	if (curr_cpu == target_cpu)
+		return IRQ_SET_MASK_OK_DONE;
+
+	/* Update MSI number to target the new CPU */
+	irqdata->hwirq = hwirq_to_canonical_hwirq(irqdata->hwirq) + target_cpu;
+
+	return IRQ_SET_MASK_OK;
+}
+
+static struct irq_chip xgene_msi_bottom_irq_chip = {
+	.name			= "MSI",
+	.irq_set_affinity       = xgene_msi_set_affinity,
+	.irq_compose_msi_msg	= xgene_compose_msi_msg,
+};
+
+static int xgene_irq_domain_alloc(struct irq_domain *domain, unsigned int virq,
+				  unsigned int nr_irqs, void *args)
+{
+	struct xgene_msi *msi = domain->host_data;
+	int msi_irq;
+
+	mutex_lock(&msi->bitmap_lock);
+
+	msi_irq = bitmap_find_next_zero_area(msi->bitmap, NR_MSI_VEC, 0,
+					     msi->num_cpus, 0);
+	if (msi_irq < NR_MSI_VEC)
+		bitmap_set(msi->bitmap, msi_irq, msi->num_cpus);
+	else
+		msi_irq = -ENOSPC;
+
+	mutex_unlock(&msi->bitmap_lock);
+
+	if (msi_irq < 0)
+		return msi_irq;
+
+	irq_domain_set_info(domain, virq, msi_irq,
+			    &xgene_msi_bottom_irq_chip, domain->host_data,
+			    handle_simple_irq, NULL, NULL);
+	set_irq_flags(virq, IRQF_VALID);
+
+	return 0;
+}
+
+static void xgene_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 xgene_msi *msi = irq_data_get_irq_chip_data(d);
+	u32 hwirq;
+
+	mutex_lock(&msi->bitmap_lock);
+
+	hwirq = hwirq_to_canonical_hwirq(d->hwirq);
+	bitmap_clear(msi->bitmap, hwirq, msi->num_cpus);
+
+	mutex_unlock(&msi->bitmap_lock);
+
+	irq_domain_free_irqs_parent(domain, virq, nr_irqs);
+}
+
+static const struct irq_domain_ops msi_domain_ops = {
+	.alloc  = xgene_irq_domain_alloc,
+	.free   = xgene_irq_domain_free,
+};
+
+static int xgene_allocate_domains(struct xgene_msi *msi)
+{
+	msi->domain = irq_domain_add_linear(NULL, NR_MSI_VEC,
+					    &msi_domain_ops, msi);
+	if (!msi->domain)
+		return -ENOMEM;
+
+	msi->mchip.domain = pci_msi_create_irq_domain(msi->mchip.of_node,
+						      &xgene_msi_domain_info,
+						      msi->domain);
+
+	if (!msi->mchip.domain) {
+		irq_domain_remove(msi->domain);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void xgene_free_domains(struct xgene_msi *msi)
+{
+	if (msi->mchip.domain)
+		irq_domain_remove(msi->mchip.domain);
+	if (msi->domain)
+		irq_domain_remove(msi->domain);
+}
+
+static int xgene_msi_init_allocator(struct xgene_msi *xgene_msi)
+{
+	int size = BITS_TO_LONGS(NR_MSI_VEC) * sizeof(long);
+
+	xgene_msi->bitmap = kzalloc(size, GFP_KERNEL);
+	if (!xgene_msi->bitmap)
+		return -ENOMEM;
+
+	mutex_init(&xgene_msi->bitmap_lock);
+
+	xgene_msi->msi_groups = kcalloc(NR_HW_IRQS,
+					sizeof(struct xgene_msi_group),
+					GFP_KERNEL);
+	if (!xgene_msi->msi_groups)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void xgene_msi_isr(unsigned int irq, struct irq_desc *desc)
+{
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	struct xgene_msi_group *msi_groups;
+	struct xgene_msi *xgene_msi;
+	unsigned int virq;
+	int msir_index, msir_val, hw_irq;
+	u32 intr_index, grp_select, msi_grp;
+
+	chained_irq_enter(chip, desc);
+
+	msi_groups = irq_desc_get_handler_data(desc);
+	xgene_msi = msi_groups->msi;
+	msi_grp = msi_groups->msi_grp;
+
+	/*
+	 * MSIINTn (n is 0..F) indicates if there is a pending MSI interrupt
+	 * If bit x of this register is set (x is 0..7), one or more interupts
+	 * corresponding to MSInIRx is set.
+	 */
+	grp_select = xgene_msi_int_read(xgene_msi, msi_grp);
+	while (grp_select) {
+		msir_index = ffs(grp_select) - 1;
+		/*
+		 * Calculate MSInIRx address to read to check for interrupts
+		 * (refer to termination address and data assignment
+		 * described in xgene_compose_msi_msg() )
+		 */
+		msir_val = xgene_msi_ir_read(xgene_msi, msi_grp, msir_index);
+		while (msir_val) {
+			intr_index = ffs(msir_val) - 1;
+			/*
+			 * Calculate MSI vector number (refer to the termination
+			 * address and data assignment described in
+			 * xgene_compose_msi_msg function)
+			 */
+			hw_irq = (((msir_index * IRQS_PER_IDX) + intr_index) *
+				 NR_HW_IRQS) + msi_grp;
+			/*
+			 * As we have multiple hw_irq that maps to single MSI,
+			 * always look up the virq using the hw_irq as seen from
+			 * CPU0
+			 */
+			hw_irq = hwirq_to_canonical_hwirq(hw_irq);
+			virq = irq_find_mapping(xgene_msi->domain, hw_irq);
+			WARN_ON(!virq);
+			if (virq != 0)
+				generic_handle_irq(virq);
+			msir_val &= ~(1 << intr_index);
+		}
+		grp_select &= ~(1 << msir_index);
+
+		if (!grp_select) {
+			/*
+			 * We handled all interrupts happened in this group,
+			 * resample this group MSI_INTx register in case
+			 * something else has been made pending in the meantime
+			 */
+			grp_select = xgene_msi_int_read(xgene_msi, msi_grp);
+		}
+	}
+
+	chained_irq_exit(chip, desc);
+}
+
+static int xgene_msi_remove(struct platform_device *pdev)
+{
+	int virq, i;
+	struct xgene_msi *msi = platform_get_drvdata(pdev);
+
+	for (i = 0; i < NR_HW_IRQS; i++) {
+		virq = msi->msi_groups[i].gic_irq;
+		if (virq != 0) {
+			irq_set_chained_handler(virq, NULL);
+			irq_set_handler_data(virq, NULL);
+		}
+	}
+	kfree(msi->msi_groups);
+
+	kfree(msi->bitmap);
+	msi->bitmap = NULL;
+
+	xgene_free_domains(msi);
+
+	return 0;
+}
+
+static int xgene_msi_hwirq_alloc(unsigned int cpu)
+{
+	struct xgene_msi *msi = &xgene_msi_ctrl;
+	struct xgene_msi_group *msi_group;
+	cpumask_var_t mask;
+	int i;
+	int err;
+
+	for (i = cpu; i < NR_HW_IRQS; i += msi->num_cpus) {
+		msi_group = &msi->msi_groups[i];
+		if (!msi_group->gic_irq)
+			continue;
+
+		irq_set_chained_handler(msi_group->gic_irq,
+					xgene_msi_isr);
+		err = irq_set_handler_data(msi_group->gic_irq, msi_group);
+		if (err) {
+			pr_err("failed to register GIC IRQ handler\n");
+			return -EINVAL;
+		}
+		/*
+		 * Statically allocate MSI GIC IRQs to each CPU core.
+		 * With 8-core X-Gene v1, 2 MSI GIC IRQs are allocated
+		 * to each core.
+		 */
+		if (alloc_cpumask_var(&mask, GFP_KERNEL)) {
+			cpumask_clear(mask);
+			cpumask_set_cpu(cpu, mask);
+			err = irq_set_affinity(msi_group->gic_irq, mask);
+			if (err)
+				pr_err("failed to set affinity for GIC IRQ");
+			free_cpumask_var(mask);
+		} else {
+			pr_err("failed to alloc CPU mask for affinity\n");
+			err = -EINVAL;
+		}
+
+		if (err) {
+			irq_set_chained_handler(msi_group->gic_irq, NULL);
+			irq_set_handler_data(msi_group->gic_irq, NULL);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+static void xgene_msi_hwirq_free(unsigned int cpu)
+{
+	struct xgene_msi *msi = &xgene_msi_ctrl;
+	struct xgene_msi_group *msi_group;
+	int i;
+
+	for (i = cpu; i < NR_HW_IRQS; i += msi->num_cpus) {
+		msi_group = &msi->msi_groups[i];
+		if (!msi_group->gic_irq)
+			continue;
+
+		irq_set_chained_handler(msi_group->gic_irq, NULL);
+		irq_set_handler_data(msi_group->gic_irq, NULL);
+	}
+}
+
+static int xgene_msi_cpu_callback(struct notifier_block *nfb,
+				  unsigned long action, void *hcpu)
+{
+	unsigned cpu = (unsigned long)hcpu;
+
+	switch (action) {
+	case CPU_ONLINE:
+	case CPU_ONLINE_FROZEN:
+		xgene_msi_hwirq_alloc(cpu);
+		break;
+	case CPU_DEAD:
+	case CPU_DEAD_FROZEN:
+		xgene_msi_hwirq_free(cpu);
+		break;
+	default:
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block xgene_msi_cpu_notifier = {
+	.notifier_call = xgene_msi_cpu_callback,
+};
+
+static const struct of_device_id xgene_msi_match_table[] = {
+	{.compatible = "apm,xgene1-msi"},
+	{},
+};
+
+static int xgene_msi_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int rc, irq_index;
+	struct xgene_msi *xgene_msi;
+	unsigned int cpu;
+	int virt_msir;
+	u32 msi_val, msi_idx;
+
+	xgene_msi = &xgene_msi_ctrl;
+
+	platform_set_drvdata(pdev, xgene_msi);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	xgene_msi->msi_regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(xgene_msi->msi_regs)) {
+		dev_err(&pdev->dev, "no reg space\n");
+		rc = -EINVAL;
+		goto error;
+	}
+	xgene_msi->msi_addr = res->start;
+
+	xgene_msi->num_cpus = num_possible_cpus();
+
+	rc = xgene_msi_init_allocator(xgene_msi);
+	if (rc) {
+		dev_err(&pdev->dev, "Error allocating MSI bitmap\n");
+		goto error;
+	}
+
+	rc = xgene_allocate_domains(xgene_msi);
+	if (rc) {
+		dev_err(&pdev->dev, "Failed to allocate MSI domain\n");
+		goto error;
+	}
+
+	for (irq_index = 0; irq_index < NR_HW_IRQS; irq_index++) {
+		virt_msir = platform_get_irq(pdev, irq_index);
+		if (virt_msir < 0) {
+			dev_err(&pdev->dev, "Cannot translate IRQ index %d\n",
+				irq_index);
+			rc = -EINVAL;
+			goto error;
+		}
+		xgene_msi->msi_groups[irq_index].gic_irq = virt_msir;
+		xgene_msi->msi_groups[irq_index].msi_grp = irq_index;
+		xgene_msi->msi_groups[irq_index].msi = xgene_msi;
+	}
+
+	/*
+	 * MSInIRx registers are read-to-clear; before registering
+	 * interrupt handlers, read all of them to clear spurious
+	 * interrupts that may occur before the driver is probed.
+	 */
+	for (irq_index = 0; irq_index < NR_HW_IRQS; irq_index++) {
+		for (msi_idx = 0; msi_idx < IDX_PER_GROUP; msi_idx++)
+			msi_val = xgene_msi_ir_read(xgene_msi, irq_index,
+						    msi_idx);
+		/* Read MSIINTn to confirm */
+		msi_val = xgene_msi_int_read(xgene_msi, irq_index);
+		if (msi_val) {
+			dev_err(&pdev->dev, "Failed to clear spurious IRQ\n");
+			rc = -EINVAL;
+			goto error;
+		}
+	}
+
+	cpu_notifier_register_begin();
+
+	for_each_online_cpu(cpu)
+		if (xgene_msi_hwirq_alloc(cpu)) {
+			dev_err(&pdev->dev, "failed to register MSI handlers\n");
+			cpu_notifier_register_done();
+			goto error;
+		}
+
+	rc = __register_hotcpu_notifier(&xgene_msi_cpu_notifier);
+	if (rc) {
+		dev_err(&pdev->dev, "failed to add CPU MSI notifier\n");
+		cpu_notifier_register_done();
+		goto error;
+	}
+
+	cpu_notifier_register_done();
+
+	xgene_msi->mchip.of_node = pdev->dev.of_node;
+	rc = of_pci_msi_chip_add(&xgene_msi->mchip);
+	if (rc) {
+		dev_err(&pdev->dev, "failed to add MSI controller chip\n");
+		goto error_notifier;
+	}
+
+	dev_info(&pdev->dev, "APM X-Gene PCIe MSI driver loaded\n");
+
+	return 0;
+
+error_notifier:
+	unregister_hotcpu_notifier(&xgene_msi_cpu_notifier);
+error:
+	xgene_msi_remove(pdev);
+	return rc;
+}
+
+static struct platform_driver xgene_msi_driver = {
+	.driver = {
+		.name = "xgene-msi",
+		.owner = THIS_MODULE,
+		.of_match_table = xgene_msi_match_table,
+	},
+	.probe = xgene_msi_probe,
+	.remove = xgene_msi_remove,
+};
+
+static int __init xgene_pcie_msi_init(void)
+{
+	return platform_driver_register(&xgene_msi_driver);
+}
+subsys_initcall(xgene_pcie_msi_init);
diff --git a/drivers/pci/host/pci-xgene.c b/drivers/pci/host/pci-xgene.c
index ee082c0366ec..a9dfb70d623a 100644
--- a/drivers/pci/host/pci-xgene.c
+++ b/drivers/pci/host/pci-xgene.c
@@ -59,6 +59,12 @@
 #define SZ_1T				(SZ_1G*1024ULL)
 #define PIPE_PHY_RATE_RD(src)		((0xc000 & (u32)(src)) >> 0xe)
 
+#define ROOT_CAP_AND_CTRL		0x5C
+
+/* PCIe IP version */
+#define XGENE_PCIE_IP_VER_UNKN		0
+#define XGENE_PCIE_IP_VER_1		1
+
 struct xgene_pcie_port {
 	struct device_node	*node;
 	struct device		*dev;
@@ -67,6 +73,7 @@ struct xgene_pcie_port {
 	void __iomem		*cfg_base;
 	unsigned long		cfg_addr;
 	bool			link_up;
+	u32			version;
 };
 
 static inline u32 pcie_bar_low_val(u32 addr, u32 flags)
@@ -130,9 +137,7 @@ static bool xgene_pcie_hide_rc_bars(struct pci_bus *bus, int offset)
 static void __iomem *xgene_pcie_map_bus(struct pci_bus *bus, unsigned int devfn,
 			      int offset)
 {
-	struct xgene_pcie_port *port = bus->sysdata;
-
-	if ((pci_is_root_bus(bus) && devfn != 0) || !port->link_up ||
+	if ((pci_is_root_bus(bus) && devfn != 0) ||
 	    xgene_pcie_hide_rc_bars(bus, offset))
 		return NULL;
 
@@ -140,9 +145,37 @@ static void __iomem *xgene_pcie_map_bus(struct pci_bus *bus, unsigned int devfn,
 	return xgene_pcie_get_cfg_base(bus) + offset;
 }
 
+static int xgene_pcie_config_read32(struct pci_bus *bus, unsigned int devfn,
+				    int where, int size, u32 *val)
+{
+	struct xgene_pcie_port *port = bus->sysdata;
+
+	if (pci_generic_config_read32(bus, devfn, where & ~0x3, 4, val) !=
+	    PCIBIOS_SUCCESSFUL)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	/*
+	 * The v1 controller has a bug in its Configuration Request
+	 * Retry Status (CRS) logic: when CRS is enabled and we read the
+	 * Vendor and Device ID of a non-existent device, the controller
+	 * fabricates return data of 0xFFFF0001 ("device exists but is not
+	 * ready") instead of 0xFFFFFFFF ("device does not exist").  This
+	 * causes the PCI core to retry the read until it times out.
+	 * Avoid this by not claiming to support CRS.
+	 */
+	if (pci_is_root_bus(bus) && (port->version == XGENE_PCIE_IP_VER_1) &&
+	    ((where & ~0x3) == ROOT_CAP_AND_CTRL))
+		*val &= ~(PCI_EXP_RTCAP_CRSVIS << 16);
+
+	if (size <= 2)
+		*val = (*val >> (8 * (where & 3))) & ((1 << (size * 8)) - 1);
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
 static struct pci_ops xgene_pcie_ops = {
 	.map_bus = xgene_pcie_map_bus,
-	.read = pci_generic_config_read32,
+	.read = xgene_pcie_config_read32,
 	.write = pci_generic_config_write32,
 };
 
@@ -468,6 +501,23 @@ static int xgene_pcie_setup(struct xgene_pcie_port *port,
 	return 0;
 }
 
+static int xgene_pcie_msi_enable(struct pci_bus *bus)
+{
+	struct device_node *msi_node;
+
+	msi_node = of_parse_phandle(bus->dev.of_node,
+					"msi-parent", 0);
+	if (!msi_node)
+		return -ENODEV;
+
+	bus->msi = of_pci_find_msi_chip_by_node(msi_node);
+	if (!bus->msi)
+		return -ENODEV;
+
+	bus->msi->dev = &bus->dev;
+	return 0;
+}
+
 static int xgene_pcie_probe_bridge(struct platform_device *pdev)
 {
 	struct device_node *dn = pdev->dev.of_node;
@@ -483,6 +533,10 @@ static int xgene_pcie_probe_bridge(struct platform_device *pdev)
 	port->node = of_node_get(pdev->dev.of_node);
 	port->dev = &pdev->dev;
 
+	port->version = XGENE_PCIE_IP_VER_UNKN;
+	if (of_device_is_compatible(port->node, "apm,xgene-pcie"))
+		port->version = XGENE_PCIE_IP_VER_1;
+
 	ret = xgene_pcie_map_reg(port, pdev);
 	if (ret)
 		return ret;
@@ -504,6 +558,10 @@ static int xgene_pcie_probe_bridge(struct platform_device *pdev)
 	if (!bus)
 		return -ENOMEM;
 
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		if (xgene_pcie_msi_enable(bus))
+			dev_info(port->dev, "failed to enable MSI\n");
+
 	pci_scan_child_bus(bus);
 	pci_assign_unassigned_bus_resources(bus);
 	pci_bus_add_devices(bus);
diff --git a/drivers/pci/host/pcie-designware.c b/drivers/pci/host/pcie-designware.c
index 2e9f84fdd9ce..69486be7181e 100644
--- a/drivers/pci/host/pcie-designware.c
+++ b/drivers/pci/host/pcie-designware.c
@@ -31,6 +31,7 @@
 #define PORT_LINK_MODE_1_LANES		(0x1 << 16)
 #define PORT_LINK_MODE_2_LANES		(0x3 << 16)
 #define PORT_LINK_MODE_4_LANES		(0x7 << 16)
+#define PORT_LINK_MODE_8_LANES		(0xf << 16)
 
 #define PCIE_LINK_WIDTH_SPEED_CONTROL	0x80C
 #define PORT_LOGIC_SPEED_CHANGE		(0x1 << 17)
@@ -38,6 +39,7 @@
 #define PORT_LOGIC_LINK_WIDTH_1_LANES	(0x1 << 8)
 #define PORT_LOGIC_LINK_WIDTH_2_LANES	(0x2 << 8)
 #define PORT_LOGIC_LINK_WIDTH_4_LANES	(0x4 << 8)
+#define PORT_LOGIC_LINK_WIDTH_8_LANES	(0x8 << 8)
 
 #define PCIE_MSI_ADDR_LO		0x820
 #define PCIE_MSI_ADDR_HI		0x824
@@ -150,6 +152,21 @@ static int dw_pcie_wr_own_conf(struct pcie_port *pp, int where, int size,
 	return ret;
 }
 
+static void dw_pcie_prog_outbound_atu(struct pcie_port *pp, int index,
+		int type, u64 cpu_addr, u64 pci_addr, u32 size)
+{
+	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | index,
+			  PCIE_ATU_VIEWPORT);
+	dw_pcie_writel_rc(pp, lower_32_bits(cpu_addr), PCIE_ATU_LOWER_BASE);
+	dw_pcie_writel_rc(pp, upper_32_bits(cpu_addr), PCIE_ATU_UPPER_BASE);
+	dw_pcie_writel_rc(pp, lower_32_bits(cpu_addr + size - 1),
+			  PCIE_ATU_LIMIT);
+	dw_pcie_writel_rc(pp, lower_32_bits(pci_addr), PCIE_ATU_LOWER_TARGET);
+	dw_pcie_writel_rc(pp, upper_32_bits(pci_addr), PCIE_ATU_UPPER_TARGET);
+	dw_pcie_writel_rc(pp, type, PCIE_ATU_CR1);
+	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
+}
+
 static struct irq_chip dw_msi_irq_chip = {
 	.name = "PCI-MSI",
 	.irq_enable = pci_msi_unmask_irq,
@@ -493,6 +510,11 @@ int dw_pcie_host_init(struct pcie_port *pp)
 	if (pp->ops->host_init)
 		pp->ops->host_init(pp);
 
+	if (!pp->ops->rd_other_conf)
+		dw_pcie_prog_outbound_atu(pp, PCIE_ATU_REGION_INDEX1,
+					  PCIE_ATU_TYPE_MEM, pp->mem_mod_base,
+					  pp->mem_bus_addr, pp->mem_size);
+
 	dw_pcie_wr_own_conf(pp, PCI_BASE_ADDRESS_0, 4, 0);
 
 	/* program correct class for RC */
@@ -515,115 +537,73 @@ int dw_pcie_host_init(struct pcie_port *pp)
 	return 0;
 }
 
-static void dw_pcie_prog_viewport_cfg0(struct pcie_port *pp, u32 busdev)
-{
-	/* Program viewport 0 : OUTBOUND : CFG0 */
-	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX0,
-			  PCIE_ATU_VIEWPORT);
-	dw_pcie_writel_rc(pp, pp->cfg0_mod_base, PCIE_ATU_LOWER_BASE);
-	dw_pcie_writel_rc(pp, (pp->cfg0_mod_base >> 32), PCIE_ATU_UPPER_BASE);
-	dw_pcie_writel_rc(pp, pp->cfg0_mod_base + pp->cfg0_size - 1,
-			  PCIE_ATU_LIMIT);
-	dw_pcie_writel_rc(pp, busdev, PCIE_ATU_LOWER_TARGET);
-	dw_pcie_writel_rc(pp, 0, PCIE_ATU_UPPER_TARGET);
-	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_CFG0, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
-}
-
-static void dw_pcie_prog_viewport_cfg1(struct pcie_port *pp, u32 busdev)
-{
-	/* Program viewport 1 : OUTBOUND : CFG1 */
-	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX1,
-			  PCIE_ATU_VIEWPORT);
-	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_CFG1, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, pp->cfg1_mod_base, PCIE_ATU_LOWER_BASE);
-	dw_pcie_writel_rc(pp, (pp->cfg1_mod_base >> 32), PCIE_ATU_UPPER_BASE);
-	dw_pcie_writel_rc(pp, pp->cfg1_mod_base + pp->cfg1_size - 1,
-			  PCIE_ATU_LIMIT);
-	dw_pcie_writel_rc(pp, busdev, PCIE_ATU_LOWER_TARGET);
-	dw_pcie_writel_rc(pp, 0, PCIE_ATU_UPPER_TARGET);
-	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
-}
-
-static void dw_pcie_prog_viewport_mem_outbound(struct pcie_port *pp)
-{
-	/* Program viewport 0 : OUTBOUND : MEM */
-	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX0,
-			  PCIE_ATU_VIEWPORT);
-	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_MEM, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, pp->mem_mod_base, PCIE_ATU_LOWER_BASE);
-	dw_pcie_writel_rc(pp, (pp->mem_mod_base >> 32), PCIE_ATU_UPPER_BASE);
-	dw_pcie_writel_rc(pp, pp->mem_mod_base + pp->mem_size - 1,
-			  PCIE_ATU_LIMIT);
-	dw_pcie_writel_rc(pp, pp->mem_bus_addr, PCIE_ATU_LOWER_TARGET);
-	dw_pcie_writel_rc(pp, upper_32_bits(pp->mem_bus_addr),
-			  PCIE_ATU_UPPER_TARGET);
-	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
-}
-
-static void dw_pcie_prog_viewport_io_outbound(struct pcie_port *pp)
-{
-	/* Program viewport 1 : OUTBOUND : IO */
-	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX1,
-			  PCIE_ATU_VIEWPORT);
-	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_IO, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, pp->io_mod_base, PCIE_ATU_LOWER_BASE);
-	dw_pcie_writel_rc(pp, (pp->io_mod_base >> 32), PCIE_ATU_UPPER_BASE);
-	dw_pcie_writel_rc(pp, pp->io_mod_base + pp->io_size - 1,
-			  PCIE_ATU_LIMIT);
-	dw_pcie_writel_rc(pp, pp->io_bus_addr, PCIE_ATU_LOWER_TARGET);
-	dw_pcie_writel_rc(pp, upper_32_bits(pp->io_bus_addr),
-			  PCIE_ATU_UPPER_TARGET);
-	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
-}
-
 static int dw_pcie_rd_other_conf(struct pcie_port *pp, struct pci_bus *bus,
 		u32 devfn, int where, int size, u32 *val)
 {
-	int ret = PCIBIOS_SUCCESSFUL;
-	u32 address, busdev;
+	int ret, type;
+	u32 address, busdev, cfg_size;
+	u64 cpu_addr;
+	void __iomem *va_cfg_base;
 
 	busdev = PCIE_ATU_BUS(bus->number) | PCIE_ATU_DEV(PCI_SLOT(devfn)) |
 		 PCIE_ATU_FUNC(PCI_FUNC(devfn));
 	address = where & ~0x3;
 
 	if (bus->parent->number == pp->root_bus_nr) {
-		dw_pcie_prog_viewport_cfg0(pp, busdev);
-		ret = dw_pcie_cfg_read(pp->va_cfg0_base + address, where, size,
-				val);
-		dw_pcie_prog_viewport_mem_outbound(pp);
+		type = PCIE_ATU_TYPE_CFG0;
+		cpu_addr = pp->cfg0_mod_base;
+		cfg_size = pp->cfg0_size;
+		va_cfg_base = pp->va_cfg0_base;
 	} else {
-		dw_pcie_prog_viewport_cfg1(pp, busdev);
-		ret = dw_pcie_cfg_read(pp->va_cfg1_base + address, where, size,
-				val);
-		dw_pcie_prog_viewport_io_outbound(pp);
+		type = PCIE_ATU_TYPE_CFG1;
+		cpu_addr = pp->cfg1_mod_base;
+		cfg_size = pp->cfg1_size;
+		va_cfg_base = pp->va_cfg1_base;
 	}
 
+	dw_pcie_prog_outbound_atu(pp, PCIE_ATU_REGION_INDEX0,
+				  type, cpu_addr,
+				  busdev, cfg_size);
+	ret = dw_pcie_cfg_read(va_cfg_base + address, where, size, val);
+	dw_pcie_prog_outbound_atu(pp, PCIE_ATU_REGION_INDEX0,
+				  PCIE_ATU_TYPE_IO, pp->io_mod_base,
+				  pp->io_bus_addr, pp->io_size);
+
 	return ret;
 }
 
 static int dw_pcie_wr_other_conf(struct pcie_port *pp, struct pci_bus *bus,
 		u32 devfn, int where, int size, u32 val)
 {
-	int ret = PCIBIOS_SUCCESSFUL;
-	u32 address, busdev;
+	int ret, type;
+	u32 address, busdev, cfg_size;
+	u64 cpu_addr;
+	void __iomem *va_cfg_base;
 
 	busdev = PCIE_ATU_BUS(bus->number) | PCIE_ATU_DEV(PCI_SLOT(devfn)) |
 		 PCIE_ATU_FUNC(PCI_FUNC(devfn));
 	address = where & ~0x3;
 
 	if (bus->parent->number == pp->root_bus_nr) {
-		dw_pcie_prog_viewport_cfg0(pp, busdev);
-		ret = dw_pcie_cfg_write(pp->va_cfg0_base + address, where, size,
-				val);
-		dw_pcie_prog_viewport_mem_outbound(pp);
+		type = PCIE_ATU_TYPE_CFG0;
+		cpu_addr = pp->cfg0_mod_base;
+		cfg_size = pp->cfg0_size;
+		va_cfg_base = pp->va_cfg0_base;
 	} else {
-		dw_pcie_prog_viewport_cfg1(pp, busdev);
-		ret = dw_pcie_cfg_write(pp->va_cfg1_base + address, where, size,
-				val);
-		dw_pcie_prog_viewport_io_outbound(pp);
+		type = PCIE_ATU_TYPE_CFG1;
+		cpu_addr = pp->cfg1_mod_base;
+		cfg_size = pp->cfg1_size;
+		va_cfg_base = pp->va_cfg1_base;
 	}
 
+	dw_pcie_prog_outbound_atu(pp, PCIE_ATU_REGION_INDEX0,
+				  type, cpu_addr,
+				  busdev, cfg_size);
+	ret = dw_pcie_cfg_write(va_cfg_base + address, where, size, val);
+	dw_pcie_prog_outbound_atu(pp, PCIE_ATU_REGION_INDEX0,
+				  PCIE_ATU_TYPE_IO, pp->io_mod_base,
+				  pp->io_bus_addr, pp->io_size);
+
 	return ret;
 }
 
@@ -728,13 +708,11 @@ static struct pci_bus *dw_pcie_scan_bus(int nr, struct pci_sys_data *sys)
 	struct pcie_port *pp = sys_to_pcie(sys);
 
 	pp->root_bus_nr = sys->busnr;
-	bus = pci_create_root_bus(pp->dev, sys->busnr,
+	bus = pci_scan_root_bus(pp->dev, sys->busnr,
 				  &dw_pcie_ops, sys, &sys->resources);
 	if (!bus)
 		return NULL;
 
-	pci_scan_child_bus(bus);
-
 	if (bus && pp->ops->scan_bus)
 		pp->ops->scan_bus(pp);
 
@@ -778,6 +756,9 @@ void dw_pcie_setup_rc(struct pcie_port *pp)
 	case 4:
 		val |= PORT_LINK_MODE_4_LANES;
 		break;
+	case 8:
+		val |= PORT_LINK_MODE_8_LANES;
+		break;
 	}
 	dw_pcie_writel_rc(pp, val, PCIE_PORT_LINK_CONTROL);
 
@@ -794,6 +775,9 @@ void dw_pcie_setup_rc(struct pcie_port *pp)
 	case 4:
 		val |= PORT_LOGIC_LINK_WIDTH_4_LANES;
 		break;
+	case 8:
+		val |= PORT_LOGIC_LINK_WIDTH_8_LANES;
+		break;
 	}
 	dw_pcie_writel_rc(pp, val, PCIE_LINK_WIDTH_SPEED_CONTROL);
 
diff --git a/drivers/pci/host/pcie-iproc-bcma.c b/drivers/pci/host/pcie-iproc-bcma.c
new file mode 100644
index 000000000000..96a7d999fd5e
--- /dev/null
+++ b/drivers/pci/host/pcie-iproc-bcma.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2015 Broadcom Corporation
+ * Copyright (C) 2015 Hauke Mehrtens <hauke@hauke-m.de>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/phy/phy.h>
+#include <linux/bcma/bcma.h>
+#include <linux/ioport.h>
+
+#include "pcie-iproc.h"
+
+
+/* NS: CLASS field is R/O, and set to wrong 0x200 value */
+static void bcma_pcie2_fixup_class(struct pci_dev *dev)
+{
+	dev->class = PCI_CLASS_BRIDGE_PCI << 8;
+}
+DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_BROADCOM, 0x8011, bcma_pcie2_fixup_class);
+DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_BROADCOM, 0x8012, bcma_pcie2_fixup_class);
+
+static int iproc_pcie_bcma_map_irq(const struct pci_dev *dev, u8 slot, u8 pin)
+{
+	struct pci_sys_data *sys = dev->sysdata;
+	struct iproc_pcie *pcie = sys->private_data;
+	struct bcma_device *bdev = container_of(pcie->dev, struct bcma_device, dev);
+
+	return bcma_core_irq(bdev, 5);
+}
+
+static int iproc_pcie_bcma_probe(struct bcma_device *bdev)
+{
+	struct iproc_pcie *pcie;
+	LIST_HEAD(res);
+	struct resource res_mem;
+	int ret;
+
+	pcie = devm_kzalloc(&bdev->dev, sizeof(*pcie), GFP_KERNEL);
+	if (!pcie)
+		return -ENOMEM;
+
+	pcie->dev = &bdev->dev;
+	bcma_set_drvdata(bdev, pcie);
+
+	pcie->base = bdev->io_addr;
+
+	res_mem.start = bdev->addr_s[0];
+	res_mem.end = bdev->addr_s[0] + SZ_128M - 1;
+	res_mem.name = "PCIe MEM space";
+	res_mem.flags = IORESOURCE_MEM;
+	pci_add_resource(&res, &res_mem);
+
+	pcie->map_irq = iproc_pcie_bcma_map_irq;
+
+	ret = iproc_pcie_setup(pcie, &res);
+	if (ret)
+		dev_err(pcie->dev, "PCIe controller setup failed\n");
+
+	pci_free_resource_list(&res);
+
+	return ret;
+}
+
+static void iproc_pcie_bcma_remove(struct bcma_device *bdev)
+{
+	struct iproc_pcie *pcie = bcma_get_drvdata(bdev);
+
+	iproc_pcie_remove(pcie);
+}
+
+static const struct bcma_device_id iproc_pcie_bcma_table[] = {
+	BCMA_CORE(BCMA_MANUF_BCM, BCMA_CORE_NS_PCIEG2, BCMA_ANY_REV, BCMA_ANY_CLASS),
+	{},
+};
+MODULE_DEVICE_TABLE(bcma, iproc_pcie_bcma_table);
+
+static struct bcma_driver iproc_pcie_bcma_driver = {
+	.name		= KBUILD_MODNAME,
+	.id_table	= iproc_pcie_bcma_table,
+	.probe		= iproc_pcie_bcma_probe,
+	.remove		= iproc_pcie_bcma_remove,
+};
+
+static int __init iproc_pcie_bcma_init(void)
+{
+	return bcma_driver_register(&iproc_pcie_bcma_driver);
+}
+module_init(iproc_pcie_bcma_init);
+
+static void __exit iproc_pcie_bcma_exit(void)
+{
+	bcma_driver_unregister(&iproc_pcie_bcma_driver);
+}
+module_exit(iproc_pcie_bcma_exit);
+
+MODULE_AUTHOR("Hauke Mehrtens");
+MODULE_DESCRIPTION("Broadcom iProc PCIe BCMA driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/host/pcie-iproc-platform.c b/drivers/pci/host/pcie-iproc-platform.c
index afad6c21fcfa..9aedc8eb2c6e 100644
--- a/drivers/pci/host/pcie-iproc-platform.c
+++ b/drivers/pci/host/pcie-iproc-platform.c
@@ -69,15 +69,15 @@ static int iproc_pcie_pltfm_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	pcie->resources = &res;
+	pcie->map_irq = of_irq_parse_and_map_pci;
 
-	ret = iproc_pcie_setup(pcie);
-	if (ret) {
+	ret = iproc_pcie_setup(pcie, &res);
+	if (ret)
 		dev_err(pcie->dev, "PCIe controller setup failed\n");
-		return ret;
-	}
 
-	return 0;
+	pci_free_resource_list(&res);
+
+	return ret;
 }
 
 static int iproc_pcie_pltfm_remove(struct platform_device *pdev)
diff --git a/drivers/pci/host/pcie-iproc.c b/drivers/pci/host/pcie-iproc.c
index 329e1b54528b..d77481ea553e 100644
--- a/drivers/pci/host/pcie-iproc.c
+++ b/drivers/pci/host/pcie-iproc.c
@@ -183,7 +183,7 @@ static void iproc_pcie_enable(struct iproc_pcie *pcie)
 	writel(SYS_RC_INTX_MASK, pcie->base + SYS_RC_INTX_EN);
 }
 
-int iproc_pcie_setup(struct iproc_pcie *pcie)
+int iproc_pcie_setup(struct iproc_pcie *pcie, struct list_head *res)
 {
 	int ret;
 	struct pci_bus *bus;
@@ -211,7 +211,7 @@ int iproc_pcie_setup(struct iproc_pcie *pcie)
 	pcie->sysdata.private_data = pcie;
 
 	bus = pci_create_root_bus(pcie->dev, 0, &iproc_pcie_ops,
-				  &pcie->sysdata, pcie->resources);
+				  &pcie->sysdata, res);
 	if (!bus) {
 		dev_err(pcie->dev, "unable to create PCI root bus\n");
 		ret = -ENOMEM;
@@ -229,7 +229,7 @@ int iproc_pcie_setup(struct iproc_pcie *pcie)
 
 	pci_scan_child_bus(bus);
 	pci_assign_unassigned_bus_resources(bus);
-	pci_fixup_irqs(pci_common_swizzle, of_irq_parse_and_map_pci);
+	pci_fixup_irqs(pci_common_swizzle, pcie->map_irq);
 	pci_bus_add_devices(bus);
 
 	return 0;
diff --git a/drivers/pci/host/pcie-iproc.h b/drivers/pci/host/pcie-iproc.h
index e28075ed1856..ba0a108309cc 100644
--- a/drivers/pci/host/pcie-iproc.h
+++ b/drivers/pci/host/pcie-iproc.h
@@ -29,14 +29,14 @@
 struct iproc_pcie {
 	struct device *dev;
 	void __iomem *base;
-	struct list_head *resources;
 	struct pci_sys_data sysdata;
 	struct pci_bus *root_bus;
 	struct phy *phy;
 	int irqs[IPROC_PCIE_MAX_NUM_IRQS];
+	int (*map_irq)(const struct pci_dev *, u8, u8);
 };
 
-int iproc_pcie_setup(struct iproc_pcie *pcie);
+int iproc_pcie_setup(struct iproc_pcie *pcie, struct list_head *res);
 int iproc_pcie_remove(struct iproc_pcie *pcie);
 
 #endif /* _PCIE_IPROC_H */
diff --git a/drivers/pci/host/pcie-spear13xx.c b/drivers/pci/host/pcie-spear13xx.c
index 020d78890719..c49fbdc0f6e4 100644
--- a/drivers/pci/host/pcie-spear13xx.c
+++ b/drivers/pci/host/pcie-spear13xx.c
@@ -4,8 +4,8 @@
  * SPEAr13xx PCIe Glue Layer Source Code
  *
  * Copyright (C) 2010-2014 ST Microelectronics
- * Pratyush Anand <pratyush.anand@st.com>
- * Mohit Kumar <mohit.kumar@st.com>
+ * Pratyush Anand <pratyush.anand@gmail.com>
+ * Mohit Kumar <mohit.kumar.dhaka@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
@@ -146,10 +146,10 @@ struct pcie_app_reg {
 static int spear13xx_pcie_establish_link(struct pcie_port *pp)
 {
 	u32 val;
-	int count = 0;
 	struct spear13xx_pcie *spear13xx_pcie = to_spear13xx_pcie(pp);
 	struct pcie_app_reg *app_reg = spear13xx_pcie->app_base;
 	u32 exp_cap_off = EXP_CAP_ID_OFFSET;
+	unsigned int retries;
 
 	if (dw_pcie_link_up(pp)) {
 		dev_err(pp->dev, "link already up\n");
@@ -201,17 +201,16 @@ static int spear13xx_pcie_establish_link(struct pcie_port *pp)
 			&app_reg->app_ctrl_0);
 
 	/* check if the link is up or not */
-	while (!dw_pcie_link_up(pp)) {
-		mdelay(100);
-		count++;
-		if (count == 10) {
-			dev_err(pp->dev, "link Fail\n");
-			return -EINVAL;
+	for (retries = 0; retries < 10; retries++) {
+		if (dw_pcie_link_up(pp)) {
+			dev_info(pp->dev, "link up\n");
+			return 0;
 		}
+		mdelay(100);
 	}
-	dev_info(pp->dev, "link up\n");
 
-	return 0;
+	dev_err(pp->dev, "link Fail\n");
+	return -EINVAL;
 }
 
 static irqreturn_t spear13xx_pcie_irq_handler(int irq, void *arg)
@@ -387,5 +386,5 @@ static int __init spear13xx_pcie_init(void)
 module_init(spear13xx_pcie_init);
 
 MODULE_DESCRIPTION("ST Microelectronics SPEAr13xx PCIe host controller driver");
-MODULE_AUTHOR("Pratyush Anand <pratyush.anand@st.com>");
+MODULE_AUTHOR("Pratyush Anand <pratyush.anand@gmail.com>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/hotplug/Makefile b/drivers/pci/hotplug/Makefile
index 4a9aa08b08f1..b616e7588ff4 100644
--- a/drivers/pci/hotplug/Makefile
+++ b/drivers/pci/hotplug/Makefile
@@ -61,9 +61,6 @@ pciehp-objs		:=	pciehp_core.o	\
 				pciehp_ctrl.o	\
 				pciehp_pci.o	\
 				pciehp_hpc.o
-ifdef CONFIG_ACPI
-pciehp-objs		+=	pciehp_acpi.o
-endif
 
 shpchp-objs		:=	shpchp_core.o	\
 				shpchp_ctrl.o	\
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index bcb90e4888dd..ff538568a617 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -632,15 +632,14 @@ static void trim_stale_devices(struct pci_dev *dev)
 {
 	struct acpi_device *adev = ACPI_COMPANION(&dev->dev);
 	struct pci_bus *bus = dev->subordinate;
-	bool alive = false;
+	bool alive = dev->ignore_hotplug;
 
 	if (adev) {
 		acpi_status status;
 		unsigned long long sta;
 
 		status = acpi_evaluate_integer(adev->handle, "_STA", NULL, &sta);
-		alive = (ACPI_SUCCESS(status) && device_status_valid(sta))
-			|| dev->ignore_hotplug;
+		alive = alive || (ACPI_SUCCESS(status) && device_status_valid(sta));
 	}
 	if (!alive)
 		alive = pci_device_is_present(dev);
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index b11521953485..57cd1327346f 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -132,11 +132,7 @@ struct controller {
 
 int pciehp_sysfs_enable_slot(struct slot *slot);
 int pciehp_sysfs_disable_slot(struct slot *slot);
-u8 pciehp_handle_attention_button(struct slot *p_slot);
-u8 pciehp_handle_switch_change(struct slot *p_slot);
-u8 pciehp_handle_presence_change(struct slot *p_slot);
-u8 pciehp_handle_power_fault(struct slot *p_slot);
-void pciehp_handle_linkstate_change(struct slot *p_slot);
+void pciehp_queue_interrupt_event(struct slot *slot, u32 event_type);
 int pciehp_configure_device(struct slot *p_slot);
 int pciehp_unconfigure_device(struct slot *p_slot);
 void pciehp_queue_pushbutton_work(struct work_struct *work);
@@ -167,21 +163,4 @@ static inline const char *slot_name(struct slot *slot)
 	return hotplug_slot_name(slot->hotplug_slot);
 }
 
-#ifdef CONFIG_ACPI
-#include <linux/pci-acpi.h>
-
-void __init pciehp_acpi_slot_detection_init(void);
-int pciehp_acpi_slot_detection_check(struct pci_dev *dev);
-
-static inline void pciehp_firmware_init(void)
-{
-	pciehp_acpi_slot_detection_init();
-}
-#else
-#define pciehp_firmware_init()				do {} while (0)
-static inline int pciehp_acpi_slot_detection_check(struct pci_dev *dev)
-{
-	return 0;
-}
-#endif				/* CONFIG_ACPI */
 #endif				/* _PCIEHP_H */
diff --git a/drivers/pci/hotplug/pciehp_acpi.c b/drivers/pci/hotplug/pciehp_acpi.c
deleted file mode 100644
index 93cc9266e8cb..000000000000
--- a/drivers/pci/hotplug/pciehp_acpi.c
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- * ACPI related functions for PCI Express Hot Plug driver.
- *
- * Copyright (C) 2008 Kenji Kaneshige
- * Copyright (C) 2008 Fujitsu Limited.
- *
- * 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, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- */
-
-#include <linux/acpi.h>
-#include <linux/pci.h>
-#include <linux/pci_hotplug.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include "pciehp.h"
-
-#define PCIEHP_DETECT_PCIE	(0)
-#define PCIEHP_DETECT_ACPI	(1)
-#define PCIEHP_DETECT_AUTO	(2)
-#define PCIEHP_DETECT_DEFAULT	PCIEHP_DETECT_AUTO
-
-struct dummy_slot {
-	u32 number;
-	struct list_head list;
-};
-
-static int slot_detection_mode;
-static char *pciehp_detect_mode;
-module_param(pciehp_detect_mode, charp, 0444);
-MODULE_PARM_DESC(pciehp_detect_mode,
-	 "Slot detection mode: pcie, acpi, auto\n"
-	 "  pcie          - Use PCIe based slot detection\n"
-	 "  acpi          - Use ACPI for slot detection\n"
-	 "  auto(default) - Auto select mode. Use acpi option if duplicate\n"
-	 "                  slot ids are found. Otherwise, use pcie option\n");
-
-int pciehp_acpi_slot_detection_check(struct pci_dev *dev)
-{
-	if (slot_detection_mode != PCIEHP_DETECT_ACPI)
-		return 0;
-	if (acpi_pci_detect_ejectable(ACPI_HANDLE(&dev->dev)))
-		return 0;
-	return -ENODEV;
-}
-
-static int __init parse_detect_mode(void)
-{
-	if (!pciehp_detect_mode)
-		return PCIEHP_DETECT_DEFAULT;
-	if (!strcmp(pciehp_detect_mode, "pcie"))
-		return PCIEHP_DETECT_PCIE;
-	if (!strcmp(pciehp_detect_mode, "acpi"))
-		return PCIEHP_DETECT_ACPI;
-	if (!strcmp(pciehp_detect_mode, "auto"))
-		return PCIEHP_DETECT_AUTO;
-	warn("bad specifier '%s' for pciehp_detect_mode. Use default\n",
-	     pciehp_detect_mode);
-	return PCIEHP_DETECT_DEFAULT;
-}
-
-static int __initdata dup_slot_id;
-static int __initdata acpi_slot_detected;
-static struct list_head __initdata dummy_slots = LIST_HEAD_INIT(dummy_slots);
-
-/* Dummy driver for duplicate name detection */
-static int __init dummy_probe(struct pcie_device *dev)
-{
-	u32 slot_cap;
-	acpi_handle handle;
-	struct dummy_slot *slot, *tmp;
-	struct pci_dev *pdev = dev->port;
-
-	pcie_capability_read_dword(pdev, PCI_EXP_SLTCAP, &slot_cap);
-	slot = kzalloc(sizeof(*slot), GFP_KERNEL);
-	if (!slot)
-		return -ENOMEM;
-	slot->number = (slot_cap & PCI_EXP_SLTCAP_PSN) >> 19;
-	list_for_each_entry(tmp, &dummy_slots, list) {
-		if (tmp->number == slot->number)
-			dup_slot_id++;
-	}
-	list_add_tail(&slot->list, &dummy_slots);
-	handle = ACPI_HANDLE(&pdev->dev);
-	if (!acpi_slot_detected && acpi_pci_detect_ejectable(handle))
-		acpi_slot_detected = 1;
-	return -ENODEV;         /* dummy driver always returns error */
-}
-
-static struct pcie_port_service_driver __initdata dummy_driver = {
-	.name		= "pciehp_dummy",
-	.port_type	= PCIE_ANY_PORT,
-	.service	= PCIE_PORT_SERVICE_HP,
-	.probe		= dummy_probe,
-};
-
-static int __init select_detection_mode(void)
-{
-	struct dummy_slot *slot, *tmp;
-
-	if (pcie_port_service_register(&dummy_driver))
-		return PCIEHP_DETECT_ACPI;
-	pcie_port_service_unregister(&dummy_driver);
-	list_for_each_entry_safe(slot, tmp, &dummy_slots, list) {
-		list_del(&slot->list);
-		kfree(slot);
-	}
-	if (acpi_slot_detected && dup_slot_id)
-		return PCIEHP_DETECT_ACPI;
-	return PCIEHP_DETECT_PCIE;
-}
-
-void __init pciehp_acpi_slot_detection_init(void)
-{
-	slot_detection_mode = parse_detect_mode();
-	if (slot_detection_mode != PCIEHP_DETECT_AUTO)
-		goto out;
-	slot_detection_mode = select_detection_mode();
-out:
-	if (slot_detection_mode == PCIEHP_DETECT_ACPI)
-		info("Using ACPI for slot detection.\n");
-}
diff --git a/drivers/pci/hotplug/pciehp_core.c b/drivers/pci/hotplug/pciehp_core.c
index 07aa722bb12c..612b21a14df5 100644
--- a/drivers/pci/hotplug/pciehp_core.c
+++ b/drivers/pci/hotplug/pciehp_core.c
@@ -77,11 +77,6 @@ static int reset_slot		(struct hotplug_slot *slot, int probe);
  */
 static void release_slot(struct hotplug_slot *hotplug_slot)
 {
-	struct slot *slot = hotplug_slot->private;
-
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		 __func__, hotplug_slot_name(hotplug_slot));
-
 	kfree(hotplug_slot->ops);
 	kfree(hotplug_slot->info);
 	kfree(hotplug_slot);
@@ -129,14 +124,10 @@ static int init_slot(struct controller *ctrl)
 	slot->hotplug_slot = hotplug;
 	snprintf(name, SLOT_NAME_SIZE, "%u", PSN(ctrl));
 
-	ctrl_dbg(ctrl, "Registering domain:bus:dev=%04x:%02x:00 sun=%x\n",
-		 pci_domain_nr(ctrl->pcie->port->subordinate),
-		 ctrl->pcie->port->subordinate->number, PSN(ctrl));
 	retval = pci_hp_register(hotplug,
 				 ctrl->pcie->port->subordinate, 0, name);
 	if (retval)
-		ctrl_err(ctrl,
-			 "pci_hp_register failed with error %d\n", retval);
+		ctrl_err(ctrl, "pci_hp_register failed: error %d\n", retval);
 out:
 	if (retval) {
 		kfree(ops);
@@ -158,9 +149,6 @@ static int set_attention_status(struct hotplug_slot *hotplug_slot, u8 status)
 {
 	struct slot *slot = hotplug_slot->private;
 
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		  __func__, slot_name(slot));
-
 	pciehp_set_attention_status(slot, status);
 	return 0;
 }
@@ -170,9 +158,6 @@ static int enable_slot(struct hotplug_slot *hotplug_slot)
 {
 	struct slot *slot = hotplug_slot->private;
 
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		 __func__, slot_name(slot));
-
 	return pciehp_sysfs_enable_slot(slot);
 }
 
@@ -181,9 +166,6 @@ static int disable_slot(struct hotplug_slot *hotplug_slot)
 {
 	struct slot *slot = hotplug_slot->private;
 
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		  __func__, slot_name(slot));
-
 	return pciehp_sysfs_disable_slot(slot);
 }
 
@@ -191,9 +173,6 @@ static int get_power_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
 	struct slot *slot = hotplug_slot->private;
 
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		  __func__, slot_name(slot));
-
 	pciehp_get_power_status(slot, value);
 	return 0;
 }
@@ -202,9 +181,6 @@ static int get_attention_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
 	struct slot *slot = hotplug_slot->private;
 
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		  __func__, slot_name(slot));
-
 	pciehp_get_attention_status(slot, value);
 	return 0;
 }
@@ -213,9 +189,6 @@ static int get_latch_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
 	struct slot *slot = hotplug_slot->private;
 
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		 __func__, slot_name(slot));
-
 	pciehp_get_latch_status(slot, value);
 	return 0;
 }
@@ -224,9 +197,6 @@ static int get_adapter_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
 	struct slot *slot = hotplug_slot->private;
 
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		 __func__, slot_name(slot));
-
 	pciehp_get_adapter_status(slot, value);
 	return 0;
 }
@@ -235,9 +205,6 @@ static int reset_slot(struct hotplug_slot *hotplug_slot, int probe)
 {
 	struct slot *slot = hotplug_slot->private;
 
-	ctrl_dbg(slot->ctrl, "%s: physical_slot = %s\n",
-		 __func__, slot_name(slot));
-
 	return pciehp_reset_slot(slot, probe);
 }
 
@@ -248,24 +215,21 @@ static int pciehp_probe(struct pcie_device *dev)
 	struct slot *slot;
 	u8 occupied, poweron;
 
-	if (pciehp_force)
-		dev_info(&dev->device,
-			 "Bypassing BIOS check for pciehp use on %s\n",
-			 pci_name(dev->port));
-	else if (pciehp_acpi_slot_detection_check(dev->port))
-		goto err_out_none;
+	/* If this is not a "hotplug" service, we have no business here. */
+	if (dev->service != PCIE_PORT_SERVICE_HP)
+		return -ENODEV;
 
 	if (!dev->port->subordinate) {
 		/* Can happen if we run out of bus numbers during probe */
 		dev_err(&dev->device,
 			"Hotplug bridge without secondary bus, ignoring\n");
-		goto err_out_none;
+		return -ENODEV;
 	}
 
 	ctrl = pcie_init(dev);
 	if (!ctrl) {
 		dev_err(&dev->device, "Controller initialization failed\n");
-		goto err_out_none;
+		return -ENODEV;
 	}
 	set_service_data(dev, ctrl);
 
@@ -275,14 +239,14 @@ static int pciehp_probe(struct pcie_device *dev)
 		if (rc == -EBUSY)
 			ctrl_warn(ctrl, "Slot already registered by another hotplug driver\n");
 		else
-			ctrl_err(ctrl, "Slot initialization failed\n");
+			ctrl_err(ctrl, "Slot initialization failed (%d)\n", rc);
 		goto err_out_release_ctlr;
 	}
 
 	/* Enable events after we have setup the data structures */
 	rc = pcie_init_notification(ctrl);
 	if (rc) {
-		ctrl_err(ctrl, "Notification initialization failed\n");
+		ctrl_err(ctrl, "Notification initialization failed (%d)\n", rc);
 		goto err_out_free_ctrl_slot;
 	}
 
@@ -305,7 +269,6 @@ err_out_free_ctrl_slot:
 	cleanup_slot(ctrl);
 err_out_release_ctlr:
 	pciehp_release_ctrl(ctrl);
-err_out_none:
 	return -ENODEV;
 }
 
@@ -366,7 +329,6 @@ static int __init pcied_init(void)
 {
 	int retval = 0;
 
-	pciehp_firmware_init();
 	retval = pcie_port_service_register(&hpdriver_portdrv);
 	dbg("pcie_port_service_register = %d\n", retval);
 	info(DRIVER_DESC " version: " DRIVER_VERSION "\n");
diff --git a/drivers/pci/hotplug/pciehp_ctrl.c b/drivers/pci/hotplug/pciehp_ctrl.c
index f052e951b23e..f3796124ad7c 100644
--- a/drivers/pci/hotplug/pciehp_ctrl.c
+++ b/drivers/pci/hotplug/pciehp_ctrl.c
@@ -37,138 +37,20 @@
 
 static void interrupt_event_handler(struct work_struct *work);
 
-static int queue_interrupt_event(struct slot *p_slot, u32 event_type)
+void pciehp_queue_interrupt_event(struct slot *p_slot, u32 event_type)
 {
 	struct event_info *info;
 
 	info = kmalloc(sizeof(*info), GFP_ATOMIC);
-	if (!info)
-		return -ENOMEM;
+	if (!info) {
+		ctrl_err(p_slot->ctrl, "dropped event %d (ENOMEM)\n", event_type);
+		return;
+	}
 
+	INIT_WORK(&info->work, interrupt_event_handler);
 	info->event_type = event_type;
 	info->p_slot = p_slot;
-	INIT_WORK(&info->work, interrupt_event_handler);
-
 	queue_work(p_slot->wq, &info->work);
-
-	return 0;
-}
-
-u8 pciehp_handle_attention_button(struct slot *p_slot)
-{
-	u32 event_type;
-	struct controller *ctrl = p_slot->ctrl;
-
-	/* Attention Button Change */
-	ctrl_dbg(ctrl, "Attention button interrupt received\n");
-
-	/*
-	 *  Button pressed - See if need to TAKE ACTION!!!
-	 */
-	ctrl_info(ctrl, "Button pressed on Slot(%s)\n", slot_name(p_slot));
-	event_type = INT_BUTTON_PRESS;
-
-	queue_interrupt_event(p_slot, event_type);
-
-	return 0;
-}
-
-u8 pciehp_handle_switch_change(struct slot *p_slot)
-{
-	u8 getstatus;
-	u32 event_type;
-	struct controller *ctrl = p_slot->ctrl;
-
-	/* Switch Change */
-	ctrl_dbg(ctrl, "Switch interrupt received\n");
-
-	pciehp_get_latch_status(p_slot, &getstatus);
-	if (getstatus) {
-		/*
-		 * Switch opened
-		 */
-		ctrl_info(ctrl, "Latch open on Slot(%s)\n", slot_name(p_slot));
-		event_type = INT_SWITCH_OPEN;
-	} else {
-		/*
-		 *  Switch closed
-		 */
-		ctrl_info(ctrl, "Latch close on Slot(%s)\n", slot_name(p_slot));
-		event_type = INT_SWITCH_CLOSE;
-	}
-
-	queue_interrupt_event(p_slot, event_type);
-
-	return 1;
-}
-
-u8 pciehp_handle_presence_change(struct slot *p_slot)
-{
-	u32 event_type;
-	u8 presence_save;
-	struct controller *ctrl = p_slot->ctrl;
-
-	/* Presence Change */
-	ctrl_dbg(ctrl, "Presence/Notify input change\n");
-
-	/* Switch is open, assume a presence change
-	 * Save the presence state
-	 */
-	pciehp_get_adapter_status(p_slot, &presence_save);
-	if (presence_save) {
-		/*
-		 * Card Present
-		 */
-		ctrl_info(ctrl, "Card present on Slot(%s)\n", slot_name(p_slot));
-		event_type = INT_PRESENCE_ON;
-	} else {
-		/*
-		 * Not Present
-		 */
-		ctrl_info(ctrl, "Card not present on Slot(%s)\n",
-			  slot_name(p_slot));
-		event_type = INT_PRESENCE_OFF;
-	}
-
-	queue_interrupt_event(p_slot, event_type);
-
-	return 1;
-}
-
-u8 pciehp_handle_power_fault(struct slot *p_slot)
-{
-	u32 event_type;
-	struct controller *ctrl = p_slot->ctrl;
-
-	/* power fault */
-	ctrl_dbg(ctrl, "Power fault interrupt received\n");
-	ctrl_err(ctrl, "Power fault on slot %s\n", slot_name(p_slot));
-	event_type = INT_POWER_FAULT;
-	ctrl_info(ctrl, "Power fault bit %x set\n", 0);
-	queue_interrupt_event(p_slot, event_type);
-
-	return 1;
-}
-
-void pciehp_handle_linkstate_change(struct slot *p_slot)
-{
-	u32 event_type;
-	struct controller *ctrl = p_slot->ctrl;
-
-	/* Link Status Change */
-	ctrl_dbg(ctrl, "Data Link Layer State change\n");
-
-	if (pciehp_check_link_active(ctrl)) {
-		ctrl_info(ctrl, "slot(%s): Link Up event\n",
-			  slot_name(p_slot));
-		event_type = INT_LINK_UP;
-	} else {
-		ctrl_info(ctrl, "slot(%s): Link Down event\n",
-			  slot_name(p_slot));
-		event_type = INT_LINK_DOWN;
-	}
-
-	queue_interrupt_event(p_slot, event_type);
 }
 
 /* The following routines constitute the bulk of the
@@ -298,10 +180,6 @@ static void pciehp_power_thread(struct work_struct *work)
 
 	switch (info->req) {
 	case DISABLE_REQ:
-		ctrl_dbg(p_slot->ctrl,
-			 "Disabling domain:bus:device=%04x:%02x:00\n",
-			 pci_domain_nr(p_slot->ctrl->pcie->port->subordinate),
-			 p_slot->ctrl->pcie->port->subordinate->number);
 		mutex_lock(&p_slot->hotplug_lock);
 		pciehp_disable_slot(p_slot);
 		mutex_unlock(&p_slot->hotplug_lock);
@@ -310,10 +188,6 @@ static void pciehp_power_thread(struct work_struct *work)
 		mutex_unlock(&p_slot->lock);
 		break;
 	case ENABLE_REQ:
-		ctrl_dbg(p_slot->ctrl,
-			 "Enabling domain:bus:device=%04x:%02x:00\n",
-			 pci_domain_nr(p_slot->ctrl->pcie->port->subordinate),
-			 p_slot->ctrl->pcie->port->subordinate->number);
 		mutex_lock(&p_slot->hotplug_lock);
 		ret = pciehp_enable_slot(p_slot);
 		mutex_unlock(&p_slot->hotplug_lock);
@@ -416,7 +290,7 @@ static void handle_button_press_event(struct slot *p_slot)
 		ctrl_info(ctrl, "Button ignore on Slot(%s)\n", slot_name(p_slot));
 		break;
 	default:
-		ctrl_warn(ctrl, "Not a valid state\n");
+		ctrl_warn(ctrl, "ignoring invalid state %#x\n", p_slot->state);
 		break;
 	}
 }
@@ -507,8 +381,8 @@ static void handle_link_event(struct slot *p_slot, u32 event)
 		}
 		break;
 	default:
-		ctrl_err(ctrl, "Not a valid state on slot(%s)\n",
-			 slot_name(p_slot));
+		ctrl_err(ctrl, "ignoring invalid state %#x on slot(%s)\n",
+			 p_slot->state, slot_name(p_slot));
 		kfree(info);
 		break;
 	}
@@ -532,7 +406,6 @@ static void interrupt_event_handler(struct work_struct *work)
 		pciehp_green_led_off(p_slot);
 		break;
 	case INT_PRESENCE_ON:
-		ctrl_dbg(ctrl, "Surprise Insertion\n");
 		handle_surprise_event(p_slot);
 		break;
 	case INT_PRESENCE_OFF:
@@ -540,7 +413,6 @@ static void interrupt_event_handler(struct work_struct *work)
 		 * Regardless of surprise capability, we need to
 		 * definitely remove a card that has been pulled out!
 		 */
-		ctrl_dbg(ctrl, "Surprise Removal\n");
 		handle_surprise_event(p_slot);
 		break;
 	case INT_LINK_UP:
@@ -647,8 +519,8 @@ int pciehp_sysfs_enable_slot(struct slot *p_slot)
 			  slot_name(p_slot));
 		break;
 	default:
-		ctrl_err(ctrl, "Not a valid state on slot %s\n",
-			 slot_name(p_slot));
+		ctrl_err(ctrl, "invalid state %#x on slot %s\n",
+			 p_slot->state, slot_name(p_slot));
 		break;
 	}
 	mutex_unlock(&p_slot->lock);
@@ -682,8 +554,8 @@ int pciehp_sysfs_disable_slot(struct slot *p_slot)
 			  slot_name(p_slot));
 		break;
 	default:
-		ctrl_err(ctrl, "Not a valid state on slot %s\n",
-			 slot_name(p_slot));
+		ctrl_err(ctrl, "invalid state %#x on slot %s\n",
+			 p_slot->state, slot_name(p_slot));
 		break;
 	}
 	mutex_unlock(&p_slot->lock);
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index 0ebf754fc177..2913f7e68a10 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -176,20 +176,17 @@ static void pcie_wait_cmd(struct controller *ctrl)
 			  jiffies_to_msecs(jiffies - ctrl->cmd_started));
 }
 
-/**
- * pcie_write_cmd - Issue controller command
- * @ctrl: controller to which the command is issued
- * @cmd:  command value written to slot control register
- * @mask: bitmask of slot control register to be modified
- */
-static void pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
+static void pcie_do_write_cmd(struct controller *ctrl, u16 cmd,
+			      u16 mask, bool wait)
 {
 	struct pci_dev *pdev = ctrl_dev(ctrl);
 	u16 slot_ctrl;
 
 	mutex_lock(&ctrl->ctrl_lock);
 
-	/* Wait for any previous command that might still be in progress */
+	/*
+	 * Always wait for any previous command that might still be in progress
+	 */
 	pcie_wait_cmd(ctrl);
 
 	pcie_capability_read_word(pdev, PCI_EXP_SLTCTL, &slot_ctrl);
@@ -201,9 +198,33 @@ static void pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
 	ctrl->cmd_started = jiffies;
 	ctrl->slot_ctrl = slot_ctrl;
 
+	/*
+	 * Optionally wait for the hardware to be ready for a new command,
+	 * indicating completion of the above issued command.
+	 */
+	if (wait)
+		pcie_wait_cmd(ctrl);
+
 	mutex_unlock(&ctrl->ctrl_lock);
 }
 
+/**
+ * pcie_write_cmd - Issue controller command
+ * @ctrl: controller to which the command is issued
+ * @cmd:  command value written to slot control register
+ * @mask: bitmask of slot control register to be modified
+ */
+static void pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
+{
+	pcie_do_write_cmd(ctrl, cmd, mask, true);
+}
+
+/* Same as above without waiting for the hardware to latch */
+static void pcie_write_cmd_nowait(struct controller *ctrl, u16 cmd, u16 mask)
+{
+	pcie_do_write_cmd(ctrl, cmd, mask, false);
+}
+
 bool pciehp_check_link_active(struct controller *ctrl)
 {
 	struct pci_dev *pdev = ctrl_dev(ctrl);
@@ -291,7 +312,8 @@ int pciehp_check_link_status(struct controller *ctrl)
 	ctrl_dbg(ctrl, "%s: lnk_status = %x\n", __func__, lnk_status);
 	if ((lnk_status & PCI_EXP_LNKSTA_LT) ||
 	    !(lnk_status & PCI_EXP_LNKSTA_NLW)) {
-		ctrl_err(ctrl, "Link Training Error occurs\n");
+		ctrl_err(ctrl, "link training error: status %#06x\n",
+			 lnk_status);
 		return -1;
 	}
 
@@ -422,7 +444,7 @@ void pciehp_set_attention_status(struct slot *slot, u8 value)
 	default:
 		return;
 	}
-	pcie_write_cmd(ctrl, slot_cmd, PCI_EXP_SLTCTL_AIC);
+	pcie_write_cmd_nowait(ctrl, slot_cmd, PCI_EXP_SLTCTL_AIC);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
 		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, slot_cmd);
 }
@@ -434,7 +456,8 @@ void pciehp_green_led_on(struct slot *slot)
 	if (!PWR_LED(ctrl))
 		return;
 
-	pcie_write_cmd(ctrl, PCI_EXP_SLTCTL_PWR_IND_ON, PCI_EXP_SLTCTL_PIC);
+	pcie_write_cmd_nowait(ctrl, PCI_EXP_SLTCTL_PWR_IND_ON,
+			      PCI_EXP_SLTCTL_PIC);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
 		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL,
 		 PCI_EXP_SLTCTL_PWR_IND_ON);
@@ -447,7 +470,8 @@ void pciehp_green_led_off(struct slot *slot)
 	if (!PWR_LED(ctrl))
 		return;
 
-	pcie_write_cmd(ctrl, PCI_EXP_SLTCTL_PWR_IND_OFF, PCI_EXP_SLTCTL_PIC);
+	pcie_write_cmd_nowait(ctrl, PCI_EXP_SLTCTL_PWR_IND_OFF,
+			      PCI_EXP_SLTCTL_PIC);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
 		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL,
 		 PCI_EXP_SLTCTL_PWR_IND_OFF);
@@ -460,7 +484,8 @@ void pciehp_green_led_blink(struct slot *slot)
 	if (!PWR_LED(ctrl))
 		return;
 
-	pcie_write_cmd(ctrl, PCI_EXP_SLTCTL_PWR_IND_BLINK, PCI_EXP_SLTCTL_PIC);
+	pcie_write_cmd_nowait(ctrl, PCI_EXP_SLTCTL_PWR_IND_BLINK,
+			      PCI_EXP_SLTCTL_PIC);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
 		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL,
 		 PCI_EXP_SLTCTL_PWR_IND_BLINK);
@@ -510,6 +535,8 @@ static irqreturn_t pcie_isr(int irq, void *dev_id)
 	struct pci_dev *dev;
 	struct slot *slot = ctrl->slot;
 	u16 detected, intr_loc;
+	u8 open, present;
+	bool link;
 
 	/*
 	 * In order to guarantee that all interrupt events are
@@ -532,7 +559,7 @@ static irqreturn_t pcie_isr(int irq, void *dev_id)
 						   intr_loc);
 	} while (detected);
 
-	ctrl_dbg(ctrl, "%s: intr_loc %x\n", __func__, intr_loc);
+	ctrl_dbg(ctrl, "pending interrupts %#06x from Slot Status\n", intr_loc);
 
 	/* Check Command Complete Interrupt Pending */
 	if (intr_loc & PCI_EXP_SLTSTA_CC) {
@@ -555,25 +582,44 @@ static irqreturn_t pcie_isr(int irq, void *dev_id)
 		return IRQ_HANDLED;
 
 	/* Check MRL Sensor Changed */
-	if (intr_loc & PCI_EXP_SLTSTA_MRLSC)
-		pciehp_handle_switch_change(slot);
+	if (intr_loc & PCI_EXP_SLTSTA_MRLSC) {
+		pciehp_get_latch_status(slot, &open);
+		ctrl_info(ctrl, "Latch %s on Slot(%s)\n",
+			  open ? "open" : "close", slot_name(slot));
+		pciehp_queue_interrupt_event(slot, open ? INT_SWITCH_OPEN :
+					     INT_SWITCH_CLOSE);
+	}
 
 	/* Check Attention Button Pressed */
-	if (intr_loc & PCI_EXP_SLTSTA_ABP)
-		pciehp_handle_attention_button(slot);
+	if (intr_loc & PCI_EXP_SLTSTA_ABP) {
+		ctrl_info(ctrl, "Button pressed on Slot(%s)\n",
+			  slot_name(slot));
+		pciehp_queue_interrupt_event(slot, INT_BUTTON_PRESS);
+	}
 
 	/* Check Presence Detect Changed */
-	if (intr_loc & PCI_EXP_SLTSTA_PDC)
-		pciehp_handle_presence_change(slot);
+	if (intr_loc & PCI_EXP_SLTSTA_PDC) {
+		pciehp_get_adapter_status(slot, &present);
+		ctrl_info(ctrl, "Card %spresent on Slot(%s)\n",
+			  present ? "" : "not ", slot_name(slot));
+		pciehp_queue_interrupt_event(slot, present ? INT_PRESENCE_ON :
+					     INT_PRESENCE_OFF);
+	}
 
 	/* Check Power Fault Detected */
 	if ((intr_loc & PCI_EXP_SLTSTA_PFD) && !ctrl->power_fault_detected) {
 		ctrl->power_fault_detected = 1;
-		pciehp_handle_power_fault(slot);
+		ctrl_err(ctrl, "Power fault on slot %s\n", slot_name(slot));
+		pciehp_queue_interrupt_event(slot, INT_POWER_FAULT);
 	}
 
-	if (intr_loc & PCI_EXP_SLTSTA_DLLSC)
-		pciehp_handle_linkstate_change(slot);
+	if (intr_loc & PCI_EXP_SLTSTA_DLLSC) {
+		link = pciehp_check_link_active(ctrl);
+		ctrl_info(ctrl, "slot(%s): Link %s event\n",
+			  slot_name(slot), link ? "Up" : "Down");
+		pciehp_queue_interrupt_event(slot, link ? INT_LINK_UP :
+					     INT_LINK_DOWN);
+	}
 
 	return IRQ_HANDLED;
 }
@@ -613,7 +659,7 @@ void pcie_enable_notification(struct controller *ctrl)
 		PCI_EXP_SLTCTL_HPIE | PCI_EXP_SLTCTL_CCIE |
 		PCI_EXP_SLTCTL_DLLSCE);
 
-	pcie_write_cmd(ctrl, cmd, mask);
+	pcie_write_cmd_nowait(ctrl, cmd, mask);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
 		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, cmd);
 }
@@ -664,7 +710,7 @@ int pciehp_reset_slot(struct slot *slot, int probe)
 	pci_reset_bridge_secondary_bus(ctrl->pcie->port);
 
 	pcie_capability_write_word(pdev, PCI_EXP_SLTSTA, stat_mask);
-	pcie_write_cmd(ctrl, ctrl_mask, ctrl_mask);
+	pcie_write_cmd_nowait(ctrl, ctrl_mask, ctrl_mask);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
 		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, ctrl_mask);
 	if (pciehp_poll_mode)
@@ -724,48 +770,13 @@ static void pcie_cleanup_slot(struct controller *ctrl)
 
 static inline void dbg_ctrl(struct controller *ctrl)
 {
-	int i;
-	u16 reg16;
 	struct pci_dev *pdev = ctrl->pcie->port;
+	u16 reg16;
 
 	if (!pciehp_debug)
 		return;
 
-	ctrl_info(ctrl, "Hotplug Controller:\n");
-	ctrl_info(ctrl, "  Seg/Bus/Dev/Func/IRQ : %s IRQ %d\n",
-		  pci_name(pdev), pdev->irq);
-	ctrl_info(ctrl, "  Vendor ID            : 0x%04x\n", pdev->vendor);
-	ctrl_info(ctrl, "  Device ID            : 0x%04x\n", pdev->device);
-	ctrl_info(ctrl, "  Subsystem ID         : 0x%04x\n",
-		  pdev->subsystem_device);
-	ctrl_info(ctrl, "  Subsystem Vendor ID  : 0x%04x\n",
-		  pdev->subsystem_vendor);
-	ctrl_info(ctrl, "  PCIe Cap offset      : 0x%02x\n",
-		  pci_pcie_cap(pdev));
-	for (i = 0; i < DEVICE_COUNT_RESOURCE; i++) {
-		if (!pci_resource_len(pdev, i))
-			continue;
-		ctrl_info(ctrl, "  PCI resource [%d]     : %pR\n",
-			  i, &pdev->resource[i]);
-	}
 	ctrl_info(ctrl, "Slot Capabilities      : 0x%08x\n", ctrl->slot_cap);
-	ctrl_info(ctrl, "  Physical Slot Number : %d\n", PSN(ctrl));
-	ctrl_info(ctrl, "  Attention Button     : %3s\n",
-		  ATTN_BUTTN(ctrl) ? "yes" : "no");
-	ctrl_info(ctrl, "  Power Controller     : %3s\n",
-		  POWER_CTRL(ctrl) ? "yes" : "no");
-	ctrl_info(ctrl, "  MRL Sensor           : %3s\n",
-		  MRL_SENS(ctrl)   ? "yes" : "no");
-	ctrl_info(ctrl, "  Attention Indicator  : %3s\n",
-		  ATTN_LED(ctrl)   ? "yes" : "no");
-	ctrl_info(ctrl, "  Power Indicator      : %3s\n",
-		  PWR_LED(ctrl)    ? "yes" : "no");
-	ctrl_info(ctrl, "  Hot-Plug Surprise    : %3s\n",
-		  HP_SUPR_RM(ctrl) ? "yes" : "no");
-	ctrl_info(ctrl, "  EMI Present          : %3s\n",
-		  EMI(ctrl)        ? "yes" : "no");
-	ctrl_info(ctrl, "  Command Completed    : %3s\n",
-		  NO_CMD_CMPL(ctrl) ? "no" : "yes");
 	pcie_capability_read_word(pdev, PCI_EXP_SLTSTA, &reg16);
 	ctrl_info(ctrl, "Slot Status            : 0x%04x\n", reg16);
 	pcie_capability_read_word(pdev, PCI_EXP_SLTCTL, &reg16);
@@ -794,10 +805,8 @@ struct controller *pcie_init(struct pcie_device *dev)
 
 	/* Check if Data Link Layer Link Active Reporting is implemented */
 	pcie_capability_read_dword(pdev, PCI_EXP_LNKCAP, &link_cap);
-	if (link_cap & PCI_EXP_LNKCAP_DLLLARC) {
-		ctrl_dbg(ctrl, "Link Active Reporting supported\n");
+	if (link_cap & PCI_EXP_LNKCAP_DLLLARC)
 		ctrl->link_active_reporting = 1;
-	}
 
 	/* Clear all remaining event bits in Slot Status register */
 	pcie_capability_write_word(pdev, PCI_EXP_SLTSTA,
@@ -805,13 +814,15 @@ struct controller *pcie_init(struct pcie_device *dev)
 		PCI_EXP_SLTSTA_MRLSC | PCI_EXP_SLTSTA_PDC |
 		PCI_EXP_SLTSTA_CC | PCI_EXP_SLTSTA_DLLSC);
 
-	ctrl_info(ctrl, "Slot #%d AttnBtn%c AttnInd%c PwrInd%c PwrCtrl%c MRL%c Interlock%c NoCompl%c LLActRep%c\n",
+	ctrl_info(ctrl, "Slot #%d AttnBtn%c PwrCtrl%c MRL%c AttnInd%c PwrInd%c HotPlug%c Surprise%c Interlock%c NoCompl%c LLActRep%c\n",
 		(slot_cap & PCI_EXP_SLTCAP_PSN) >> 19,
 		FLAG(slot_cap, PCI_EXP_SLTCAP_ABP),
-		FLAG(slot_cap, PCI_EXP_SLTCAP_AIP),
-		FLAG(slot_cap, PCI_EXP_SLTCAP_PIP),
 		FLAG(slot_cap, PCI_EXP_SLTCAP_PCP),
 		FLAG(slot_cap, PCI_EXP_SLTCAP_MRLSP),
+		FLAG(slot_cap, PCI_EXP_SLTCAP_AIP),
+		FLAG(slot_cap, PCI_EXP_SLTCAP_PIP),
+		FLAG(slot_cap, PCI_EXP_SLTCAP_HPC),
+		FLAG(slot_cap, PCI_EXP_SLTCAP_HPS),
 		FLAG(slot_cap, PCI_EXP_SLTCAP_EIP),
 		FLAG(slot_cap, PCI_EXP_SLTCAP_NCCS),
 		FLAG(link_cap, PCI_EXP_LNKCAP_DLLLARC));
diff --git a/drivers/pci/htirq.c b/drivers/pci/htirq.c
index a94dd2c4183a..7eb4109a3df4 100644
--- a/drivers/pci/htirq.c
+++ b/drivers/pci/htirq.c
@@ -23,20 +23,11 @@
  */
 static DEFINE_SPINLOCK(ht_irq_lock);
 
-struct ht_irq_cfg {
-	struct pci_dev *dev;
-	 /* Update callback used to cope with buggy hardware */
-	ht_irq_update_t *update;
-	unsigned pos;
-	unsigned idx;
-	struct ht_irq_msg msg;
-};
-
-
 void write_ht_irq_msg(unsigned int irq, struct ht_irq_msg *msg)
 {
 	struct ht_irq_cfg *cfg = irq_get_handler_data(irq);
 	unsigned long flags;
+
 	spin_lock_irqsave(&ht_irq_lock, flags);
 	if (cfg->msg.address_lo != msg->address_lo) {
 		pci_write_config_byte(cfg->dev, cfg->pos + 2, cfg->idx);
@@ -55,6 +46,7 @@ void write_ht_irq_msg(unsigned int irq, struct ht_irq_msg *msg)
 void fetch_ht_irq_msg(unsigned int irq, struct ht_irq_msg *msg)
 {
 	struct ht_irq_cfg *cfg = irq_get_handler_data(irq);
+
 	*msg = cfg->msg;
 }
 
@@ -86,7 +78,6 @@ void unmask_ht_irq(struct irq_data *data)
  */
 int __ht_create_irq(struct pci_dev *dev, int idx, ht_irq_update_t *update)
 {
-	struct ht_irq_cfg *cfg;
 	int max_irq, pos, irq;
 	unsigned long flags;
 	u32 data;
@@ -105,29 +96,9 @@ int __ht_create_irq(struct pci_dev *dev, int idx, ht_irq_update_t *update)
 	if (idx > max_irq)
 		return -EINVAL;
 
-	cfg = kmalloc(sizeof(*cfg), GFP_KERNEL);
-	if (!cfg)
-		return -ENOMEM;
-
-	cfg->dev = dev;
-	cfg->update = update;
-	cfg->pos = pos;
-	cfg->idx = 0x10 + (idx * 2);
-	/* Initialize msg to a value that will never match the first write. */
-	cfg->msg.address_lo = 0xffffffff;
-	cfg->msg.address_hi = 0xffffffff;
-
-	irq = irq_alloc_hwirq(dev_to_node(&dev->dev));
-	if (!irq) {
-		kfree(cfg);
-		return -EBUSY;
-	}
-	irq_set_handler_data(irq, cfg);
-
-	if (arch_setup_ht_irq(irq, dev) < 0) {
-		ht_destroy_irq(irq);
-		return -EBUSY;
-	}
+	irq = arch_setup_ht_irq(idx, pos, dev, update);
+	if (irq > 0)
+		dev_dbg(&dev->dev, "irq %d for HT\n", irq);
 
 	return irq;
 }
@@ -158,13 +129,6 @@ EXPORT_SYMBOL(ht_create_irq);
  */
 void ht_destroy_irq(unsigned int irq)
 {
-	struct ht_irq_cfg *cfg;
-
-	cfg = irq_get_handler_data(irq);
-	irq_set_chip(irq, NULL);
-	irq_set_handler_data(irq, NULL);
-	irq_free_hwirq(irq);
-
-	kfree(cfg);
+	arch_teardown_ht_irq(irq);
 }
 EXPORT_SYMBOL(ht_destroy_irq);
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index c3e7dfcf9ff5..f66be868ad21 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -185,27 +185,6 @@ void __weak arch_restore_msi_irqs(struct pci_dev *dev)
 	return default_restore_msi_irqs(dev);
 }
 
-static void msi_set_enable(struct pci_dev *dev, int enable)
-{
-	u16 control;
-
-	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &control);
-	control &= ~PCI_MSI_FLAGS_ENABLE;
-	if (enable)
-		control |= PCI_MSI_FLAGS_ENABLE;
-	pci_write_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, control);
-}
-
-static void msix_clear_and_set_ctrl(struct pci_dev *dev, u16 clear, u16 set)
-{
-	u16 ctrl;
-
-	pci_read_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, &ctrl);
-	ctrl &= ~clear;
-	ctrl |= set;
-	pci_write_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, ctrl);
-}
-
 static inline __attribute_const__ u32 msi_mask(unsigned x)
 {
 	/* Don't shift by >= width of type */
@@ -452,7 +431,7 @@ static void __pci_restore_msi_state(struct pci_dev *dev)
 	entry = irq_get_msi_desc(dev->irq);
 
 	pci_intx_for_msi(dev, 0);
-	msi_set_enable(dev, 0);
+	pci_msi_set_enable(dev, 0);
 	arch_restore_msi_irqs(dev);
 
 	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &control);
@@ -473,14 +452,14 @@ static void __pci_restore_msix_state(struct pci_dev *dev)
 
 	/* route the table */
 	pci_intx_for_msi(dev, 0);
-	msix_clear_and_set_ctrl(dev, 0,
+	pci_msix_clear_and_set_ctrl(dev, 0,
 				PCI_MSIX_FLAGS_ENABLE | PCI_MSIX_FLAGS_MASKALL);
 
 	arch_restore_msi_irqs(dev);
 	list_for_each_entry(entry, &dev->msi_list, list)
 		msix_mask_irq(entry, entry->masked);
 
-	msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_MASKALL, 0);
+	pci_msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_MASKALL, 0);
 }
 
 void pci_restore_msi_state(struct pci_dev *dev)
@@ -647,7 +626,7 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
 	int ret;
 	unsigned mask;
 
-	msi_set_enable(dev, 0);	/* Disable MSI during set up */
+	pci_msi_set_enable(dev, 0);	/* Disable MSI during set up */
 
 	entry = msi_setup_entry(dev, nvec);
 	if (!entry)
@@ -683,7 +662,7 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
 
 	/* Set MSI enabled bits	 */
 	pci_intx_for_msi(dev, 0);
-	msi_set_enable(dev, 1);
+	pci_msi_set_enable(dev, 1);
 	dev->msi_enabled = 1;
 
 	dev->irq = entry->irq;
@@ -775,7 +754,7 @@ static int msix_capability_init(struct pci_dev *dev,
 	void __iomem *base;
 
 	/* Ensure MSI-X is disabled while it is set up */
-	msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_ENABLE, 0);
+	pci_msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_ENABLE, 0);
 
 	pci_read_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, &control);
 	/* Request & Map MSI-X table region */
@@ -801,7 +780,7 @@ static int msix_capability_init(struct pci_dev *dev,
 	 * MSI-X registers.  We need to mask all the vectors to prevent
 	 * interrupts coming in before they're fully set up.
 	 */
-	msix_clear_and_set_ctrl(dev, 0,
+	pci_msix_clear_and_set_ctrl(dev, 0,
 				PCI_MSIX_FLAGS_MASKALL | PCI_MSIX_FLAGS_ENABLE);
 
 	msix_program_entries(dev, entries);
@@ -814,7 +793,7 @@ static int msix_capability_init(struct pci_dev *dev,
 	pci_intx_for_msi(dev, 0);
 	dev->msix_enabled = 1;
 
-	msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_MASKALL, 0);
+	pci_msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_MASKALL, 0);
 
 	return 0;
 
@@ -919,7 +898,7 @@ void pci_msi_shutdown(struct pci_dev *dev)
 	BUG_ON(list_empty(&dev->msi_list));
 	desc = list_first_entry(&dev->msi_list, struct msi_desc, list);
 
-	msi_set_enable(dev, 0);
+	pci_msi_set_enable(dev, 0);
 	pci_intx_for_msi(dev, 1);
 	dev->msi_enabled = 0;
 
@@ -1027,7 +1006,7 @@ void pci_msix_shutdown(struct pci_dev *dev)
 		__pci_msix_desc_mask_irq(entry, 1);
 	}
 
-	msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_ENABLE, 0);
+	pci_msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_ENABLE, 0);
 	pci_intx_for_msi(dev, 1);
 	dev->msix_enabled = 0;
 }
@@ -1062,18 +1041,6 @@ EXPORT_SYMBOL(pci_msi_enabled);
 void pci_msi_init_pci_dev(struct pci_dev *dev)
 {
 	INIT_LIST_HEAD(&dev->msi_list);
-
-	/* Disable the msi hardware to avoid screaming interrupts
-	 * during boot.  This is the power on reset default so
-	 * usually this should be a noop.
-	 */
-	dev->msi_cap = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	if (dev->msi_cap)
-		msi_set_enable(dev, 0);
-
-	dev->msix_cap = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	if (dev->msix_cap)
-		msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_ENABLE, 0);
 }
 
 /**
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index 6f6f175f51f7..314a625b78d6 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -420,7 +420,7 @@ static int acpi_pci_set_power_state(struct pci_dev *dev, pci_power_t state)
 		[PCI_D0] = ACPI_STATE_D0,
 		[PCI_D1] = ACPI_STATE_D1,
 		[PCI_D2] = ACPI_STATE_D2,
-		[PCI_D3hot] = ACPI_STATE_D3_COLD,
+		[PCI_D3hot] = ACPI_STATE_D3_HOT,
 		[PCI_D3cold] = ACPI_STATE_D3_COLD,
 	};
 	int error = -EINVAL;
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index acc4b6ef78c4..0008c950452c 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -3101,39 +3101,6 @@ bool pci_check_and_unmask_intx(struct pci_dev *dev)
 }
 EXPORT_SYMBOL_GPL(pci_check_and_unmask_intx);
 
-/**
- * pci_msi_off - disables any MSI or MSI-X capabilities
- * @dev: the PCI device to operate on
- *
- * If you want to use MSI, see pci_enable_msi() and friends.
- * This is a lower-level primitive that allows us to disable
- * MSI operation at the device level.
- */
-void pci_msi_off(struct pci_dev *dev)
-{
-	int pos;
-	u16 control;
-
-	/*
-	 * This looks like it could go in msi.c, but we need it even when
-	 * CONFIG_PCI_MSI=n.  For the same reason, we can't use
-	 * dev->msi_cap or dev->msix_cap here.
-	 */
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	if (pos) {
-		pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
-		control &= ~PCI_MSI_FLAGS_ENABLE;
-		pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
-	}
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	if (pos) {
-		pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
-		control &= ~PCI_MSIX_FLAGS_ENABLE;
-		pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
-	}
-}
-EXPORT_SYMBOL_GPL(pci_msi_off);
-
 int pci_set_dma_max_seg_size(struct pci_dev *dev, unsigned int size)
 {
 	return dma_set_max_seg_size(&dev->dev, size);
@@ -4324,6 +4291,17 @@ bool pci_device_is_present(struct pci_dev *pdev)
 }
 EXPORT_SYMBOL_GPL(pci_device_is_present);
 
+void pci_ignore_hotplug(struct pci_dev *dev)
+{
+	struct pci_dev *bridge = dev->bus->self;
+
+	dev->ignore_hotplug = 1;
+	/* Propagate the "ignore hotplug" setting to the parent bridge. */
+	if (bridge)
+		bridge->ignore_hotplug = 1;
+}
+EXPORT_SYMBOL_GPL(pci_ignore_hotplug);
+
 #define RESOURCE_ALIGNMENT_PARAM_SIZE COMMAND_LINE_SIZE
 static char resource_alignment_param[RESOURCE_ALIGNMENT_PARAM_SIZE] = {0};
 static DEFINE_SPINLOCK(resource_alignment_lock);
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index 9bd762c237ab..4ff0ff1c4088 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -146,6 +146,27 @@ static inline void pci_no_msi(void) { }
 static inline void pci_msi_init_pci_dev(struct pci_dev *dev) { }
 #endif
 
+static inline void pci_msi_set_enable(struct pci_dev *dev, int enable)
+{
+	u16 control;
+
+	pci_read_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, &control);
+	control &= ~PCI_MSI_FLAGS_ENABLE;
+	if (enable)
+		control |= PCI_MSI_FLAGS_ENABLE;
+	pci_write_config_word(dev, dev->msi_cap + PCI_MSI_FLAGS, control);
+}
+
+static inline void pci_msix_clear_and_set_ctrl(struct pci_dev *dev, u16 clear, u16 set)
+{
+	u16 ctrl;
+
+	pci_read_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, &ctrl);
+	ctrl &= ~clear;
+	ctrl |= set;
+	pci_write_config_word(dev, dev->msix_cap + PCI_MSIX_FLAGS, ctrl);
+}
+
 void pci_realloc_get_opt(char *);
 
 static inline int pci_no_d1d2(struct pci_dev *dev)
@@ -216,17 +237,6 @@ void __pci_bus_assign_resources(const struct pci_bus *bus,
 				struct list_head *fail_head);
 bool pci_bus_clip_resource(struct pci_dev *dev, int idx);
 
-/**
- * pci_ari_enabled - query ARI forwarding status
- * @bus: the PCI bus
- *
- * Returns 1 if ARI forwarding is enabled, or 0 if not enabled;
- */
-static inline int pci_ari_enabled(struct pci_bus *bus)
-{
-	return bus->self && bus->self->ari_enabled;
-}
-
 void pci_reassigndev_resource_alignment(struct pci_dev *dev);
 void pci_disable_bridge_window(struct pci_dev *dev);
 
diff --git a/drivers/pci/pcie/aer/aerdrv_core.c b/drivers/pci/pcie/aer/aerdrv_core.c
index 5653ea94547f..9803e3d039fe 100644
--- a/drivers/pci/pcie/aer/aerdrv_core.c
+++ b/drivers/pci/pcie/aer/aerdrv_core.c
@@ -425,8 +425,7 @@ static pci_ers_result_t reset_link(struct pci_dev *dev)
 
 	if (driver && driver->reset_link) {
 		status = driver->reset_link(udev);
-	} else if (pci_pcie_type(udev) == PCI_EXP_TYPE_DOWNSTREAM ||
-		pci_pcie_type(udev) == PCI_EXP_TYPE_ROOT_PORT) {
+	} else if (udev->has_secondary_link) {
 		status = default_reset_link(udev);
 	} else {
 		dev_printk(KERN_DEBUG, &dev->dev,
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index 7d4fcdc512aa..317e3558a35e 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -127,15 +127,12 @@ static void pcie_set_clkpm_nocheck(struct pcie_link_state *link, int enable)
 {
 	struct pci_dev *child;
 	struct pci_bus *linkbus = link->pdev->subordinate;
+	u32 val = enable ? PCI_EXP_LNKCTL_CLKREQ_EN : 0;
 
-	list_for_each_entry(child, &linkbus->devices, bus_list) {
-		if (enable)
-			pcie_capability_set_word(child, PCI_EXP_LNKCTL,
-						 PCI_EXP_LNKCTL_CLKREQ_EN);
-		else
-			pcie_capability_clear_word(child, PCI_EXP_LNKCTL,
-						   PCI_EXP_LNKCTL_CLKREQ_EN);
-	}
+	list_for_each_entry(child, &linkbus->devices, bus_list)
+		pcie_capability_clear_and_set_word(child, PCI_EXP_LNKCTL,
+						   PCI_EXP_LNKCTL_CLKREQ_EN,
+						   val);
 	link->clkpm_enabled = !!enable;
 }
 
@@ -525,7 +522,7 @@ static struct pcie_link_state *alloc_pcie_link_state(struct pci_dev *pdev)
 	INIT_LIST_HEAD(&link->children);
 	INIT_LIST_HEAD(&link->link);
 	link->pdev = pdev;
-	if (pci_pcie_type(pdev) == PCI_EXP_TYPE_DOWNSTREAM) {
+	if (pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT) {
 		struct pcie_link_state *parent;
 		parent = pdev->bus->parent->self->link_state;
 		if (!parent) {
@@ -559,10 +556,15 @@ void pcie_aspm_init_link_state(struct pci_dev *pdev)
 	if (!aspm_support_enabled)
 		return;
 
-	if (!pci_is_pcie(pdev) || pdev->link_state)
+	if (pdev->link_state)
 		return;
-	if (pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT &&
-	    pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM)
+
+	/*
+	 * We allocate pcie_link_state for the component on the upstream
+	 * end of a Link, so there's nothing to do unless this device has a
+	 * Link on its secondary side.
+	 */
+	if (!pdev->has_secondary_link)
 		return;
 
 	/* VIA has a strange chipset, root port is under a bridge */
@@ -675,10 +677,7 @@ void pcie_aspm_pm_state_change(struct pci_dev *pdev)
 {
 	struct pcie_link_state *link = pdev->link_state;
 
-	if (aspm_disabled || !pci_is_pcie(pdev) || !link)
-		return;
-	if ((pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT) &&
-	    (pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM))
+	if (aspm_disabled || !link)
 		return;
 	/*
 	 * Devices changed PM state, we should recheck if latency
@@ -696,16 +695,12 @@ void pcie_aspm_powersave_config_link(struct pci_dev *pdev)
 {
 	struct pcie_link_state *link = pdev->link_state;
 
-	if (aspm_disabled || !pci_is_pcie(pdev) || !link)
+	if (aspm_disabled || !link)
 		return;
 
 	if (aspm_policy != POLICY_POWERSAVE)
 		return;
 
-	if ((pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT) &&
-	    (pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM))
-		return;
-
 	down_read(&pci_bus_sem);
 	mutex_lock(&aspm_lock);
 	pcie_config_aspm_path(link);
@@ -714,8 +709,7 @@ void pcie_aspm_powersave_config_link(struct pci_dev *pdev)
 	up_read(&pci_bus_sem);
 }
 
-static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem,
-				     bool force)
+static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem)
 {
 	struct pci_dev *parent = pdev->bus->self;
 	struct pcie_link_state *link;
@@ -723,8 +717,7 @@ static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem,
 	if (!pci_is_pcie(pdev))
 		return;
 
-	if (pci_pcie_type(pdev) == PCI_EXP_TYPE_ROOT_PORT ||
-	    pci_pcie_type(pdev) == PCI_EXP_TYPE_DOWNSTREAM)
+	if (pdev->has_secondary_link)
 		parent = pdev;
 	if (!parent || !parent->link_state)
 		return;
@@ -737,7 +730,7 @@ static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem,
 	 * a similar mechanism using "PciASPMOptOut", which is also
 	 * ignored in this situation.
 	 */
-	if (aspm_disabled && !force) {
+	if (aspm_disabled) {
 		dev_warn(&pdev->dev, "can't disable ASPM; OS doesn't have ASPM control\n");
 		return;
 	}
@@ -763,7 +756,7 @@ static void __pci_disable_link_state(struct pci_dev *pdev, int state, bool sem,
 
 void pci_disable_link_state_locked(struct pci_dev *pdev, int state)
 {
-	__pci_disable_link_state(pdev, state, false, false);
+	__pci_disable_link_state(pdev, state, false);
 }
 EXPORT_SYMBOL(pci_disable_link_state_locked);
 
@@ -778,7 +771,7 @@ EXPORT_SYMBOL(pci_disable_link_state_locked);
  */
 void pci_disable_link_state(struct pci_dev *pdev, int state)
 {
-	__pci_disable_link_state(pdev, state, true, false);
+	__pci_disable_link_state(pdev, state, true);
 }
 EXPORT_SYMBOL(pci_disable_link_state);
 
@@ -907,9 +900,7 @@ void pcie_aspm_create_sysfs_dev_files(struct pci_dev *pdev)
 {
 	struct pcie_link_state *link_state = pdev->link_state;
 
-	if (!pci_is_pcie(pdev) ||
-	    (pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT &&
-	     pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM) || !link_state)
+	if (!link_state)
 		return;
 
 	if (link_state->aspm_support)
@@ -924,9 +915,7 @@ void pcie_aspm_remove_sysfs_dev_files(struct pci_dev *pdev)
 {
 	struct pcie_link_state *link_state = pdev->link_state;
 
-	if (!pci_is_pcie(pdev) ||
-	    (pci_pcie_type(pdev) != PCI_EXP_TYPE_ROOT_PORT &&
-	     pci_pcie_type(pdev) != PCI_EXP_TYPE_DOWNSTREAM) || !link_state)
+	if (!link_state)
 		return;
 
 	if (link_state->aspm_support)
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index 6675a7a1b9fc..cefd636681b6 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -254,8 +254,8 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 	}
 
 	if (res->flags & IORESOURCE_MEM_64) {
-		if ((sizeof(dma_addr_t) < 8 || sizeof(resource_size_t) < 8) &&
-		    sz64 > 0x100000000ULL) {
+		if ((sizeof(pci_bus_addr_t) < 8 || sizeof(resource_size_t) < 8)
+		    && sz64 > 0x100000000ULL) {
 			res->flags |= IORESOURCE_UNSET | IORESOURCE_DISABLED;
 			res->start = 0;
 			res->end = 0;
@@ -264,7 +264,7 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
 			goto out;
 		}
 
-		if ((sizeof(dma_addr_t) < 8) && l) {
+		if ((sizeof(pci_bus_addr_t) < 8) && l) {
 			/* Above 32-bit boundary; try to reallocate */
 			res->flags |= IORESOURCE_UNSET;
 			res->start = 0;
@@ -399,7 +399,7 @@ static void pci_read_bridge_mmio_pref(struct pci_bus *child)
 	struct pci_dev *dev = child->self;
 	u16 mem_base_lo, mem_limit_lo;
 	u64 base64, limit64;
-	dma_addr_t base, limit;
+	pci_bus_addr_t base, limit;
 	struct pci_bus_region region;
 	struct resource *res;
 
@@ -426,8 +426,8 @@ static void pci_read_bridge_mmio_pref(struct pci_bus *child)
 		}
 	}
 
-	base = (dma_addr_t) base64;
-	limit = (dma_addr_t) limit64;
+	base = (pci_bus_addr_t) base64;
+	limit = (pci_bus_addr_t) limit64;
 
 	if (base != base64) {
 		dev_err(&dev->dev, "can't handle bridge window above 4GB (bus address %#010llx)\n",
@@ -973,6 +973,8 @@ void set_pcie_port_type(struct pci_dev *pdev)
 {
 	int pos;
 	u16 reg16;
+	int type;
+	struct pci_dev *parent;
 
 	pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
 	if (!pos)
@@ -982,6 +984,22 @@ void set_pcie_port_type(struct pci_dev *pdev)
 	pdev->pcie_flags_reg = reg16;
 	pci_read_config_word(pdev, pos + PCI_EXP_DEVCAP, &reg16);
 	pdev->pcie_mpss = reg16 & PCI_EXP_DEVCAP_PAYLOAD;
+
+	/*
+	 * A Root Port is always the upstream end of a Link.  No PCIe
+	 * component has two Links.  Two Links are connected by a Switch
+	 * that has a Port on each Link and internal logic to connect the
+	 * two Ports.
+	 */
+	type = pci_pcie_type(pdev);
+	if (type == PCI_EXP_TYPE_ROOT_PORT)
+		pdev->has_secondary_link = 1;
+	else if (type == PCI_EXP_TYPE_UPSTREAM ||
+		 type == PCI_EXP_TYPE_DOWNSTREAM) {
+		parent = pci_upstream_bridge(pdev);
+		if (!parent->has_secondary_link)
+			pdev->has_secondary_link = 1;
+	}
 }
 
 void set_pcie_hotplug_bridge(struct pci_dev *pdev)
@@ -1085,6 +1103,22 @@ int pci_cfg_space_size(struct pci_dev *dev)
 
 #define LEGACY_IO_RESOURCE	(IORESOURCE_IO | IORESOURCE_PCI_FIXED)
 
+static void pci_msi_setup_pci_dev(struct pci_dev *dev)
+{
+	/*
+	 * Disable the MSI hardware to avoid screaming interrupts
+	 * during boot.  This is the power on reset default so
+	 * usually this should be a noop.
+	 */
+	dev->msi_cap = pci_find_capability(dev, PCI_CAP_ID_MSI);
+	if (dev->msi_cap)
+		pci_msi_set_enable(dev, 0);
+
+	dev->msix_cap = pci_find_capability(dev, PCI_CAP_ID_MSIX);
+	if (dev->msix_cap)
+		pci_msix_clear_and_set_ctrl(dev, PCI_MSIX_FLAGS_ENABLE, 0);
+}
+
 /**
  * pci_setup_device - fill in class and map information of a device
  * @dev: the device structure to fill
@@ -1140,6 +1174,8 @@ int pci_setup_device(struct pci_dev *dev)
 	/* "Unknown power state" */
 	dev->current_state = PCI_UNKNOWN;
 
+	pci_msi_setup_pci_dev(dev);
+
 	/* Early fixups, before probing the BARs */
 	pci_fixup_device(pci_fixup_early, dev);
 	/* device class may be changed after fixup */
@@ -1611,7 +1647,7 @@ static int only_one_child(struct pci_bus *bus)
 		return 0;
 	if (pci_pcie_type(parent) == PCI_EXP_TYPE_ROOT_PORT)
 		return 1;
-	if (pci_pcie_type(parent) == PCI_EXP_TYPE_DOWNSTREAM &&
+	if (parent->has_secondary_link &&
 	    !pci_has_flag(PCI_SCAN_ALL_PCIE_DEVS))
 		return 1;
 	return 0;
@@ -2094,25 +2130,6 @@ struct pci_bus *pci_scan_root_bus(struct device *parent, int bus,
 }
 EXPORT_SYMBOL(pci_scan_root_bus);
 
-/* Deprecated; use pci_scan_root_bus() instead */
-struct pci_bus *pci_scan_bus_parented(struct device *parent,
-		int bus, struct pci_ops *ops, void *sysdata)
-{
-	LIST_HEAD(resources);
-	struct pci_bus *b;
-
-	pci_add_resource(&resources, &ioport_resource);
-	pci_add_resource(&resources, &iomem_resource);
-	pci_add_resource(&resources, &busn_resource);
-	b = pci_create_root_bus(parent, bus, ops, sysdata, &resources);
-	if (b)
-		pci_scan_child_bus(b);
-	else
-		pci_free_resource_list(&resources);
-	return b;
-}
-EXPORT_SYMBOL(pci_scan_bus_parented);
-
 struct pci_bus *pci_scan_bus(int bus, struct pci_ops *ops,
 					void *sysdata)
 {
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index c6dc1dfd25d5..e9fd0e90fa3b 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -819,13 +819,6 @@ static void quirk_amd_ioapic(struct pci_dev *dev)
 	}
 }
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_AMD,	PCI_DEVICE_ID_AMD_VIPER_7410,	quirk_amd_ioapic);
-
-static void quirk_ioapic_rmw(struct pci_dev *dev)
-{
-	if (dev->devfn == 0 && dev->bus->number == 0)
-		sis_apic_bug = 1;
-}
-DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_SI,	PCI_ANY_ID,			quirk_ioapic_rmw);
 #endif /* CONFIG_X86_IO_APIC */
 
 /*
@@ -1600,7 +1593,6 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_EESSC,	quirk_a
 
 static void quirk_pcie_mch(struct pci_dev *pdev)
 {
-	pci_msi_off(pdev);
 	pdev->no_msi = 1;
 }
 DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_E7520_MCH,	quirk_pcie_mch);
@@ -1614,7 +1606,6 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL,	PCI_DEVICE_ID_INTEL_E7525_MCH,	quir
  */
 static void quirk_pcie_pxh(struct pci_dev *dev)
 {
-	pci_msi_off(dev);
 	dev->no_msi = 1;
 	dev_warn(&dev->dev, "PXH quirk detected; SHPC device MSI disabled\n");
 }
@@ -3572,6 +3563,8 @@ static void quirk_dma_func1_alias(struct pci_dev *dev)
  * SKUs this function is not present, making this a ghost requester.
  * https://bugzilla.kernel.org/show_bug.cgi?id=42679
  */
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_MARVELL_EXT, 0x9120,
+			 quirk_dma_func1_alias);
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_MARVELL_EXT, 0x9123,
 			 quirk_dma_func1_alias);
 /* https://bugzilla.kernel.org/show_bug.cgi?id=42679#c14 */
@@ -3740,6 +3733,8 @@ static const u16 pci_quirk_intel_pch_acs_ids[] = {
 	/* Wellsburg (X99) PCH */
 	0x8d10, 0x8d11, 0x8d12, 0x8d13, 0x8d14, 0x8d15, 0x8d16, 0x8d17,
 	0x8d18, 0x8d19, 0x8d1a, 0x8d1b, 0x8d1c, 0x8d1d, 0x8d1e,
+	/* Lynx Point (9 series) PCH */
+	0x8c90, 0x8c92, 0x8c94, 0x8c96, 0x8c98, 0x8c9a, 0x8c9c, 0x8c9e,
 };
 
 static bool pci_quirk_intel_pch_acs_match(struct pci_dev *dev)
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index 4fd0cacf7ca0..508cc56130e3 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -428,16 +428,19 @@ static void __assign_resources_sorted(struct list_head *head,
 		 * consistent.
 		 */
 		if (add_align > dev_res->res->start) {
+			resource_size_t r_size = resource_size(dev_res->res);
+
 			dev_res->res->start = add_align;
-			dev_res->res->end = add_align +
-				            resource_size(dev_res->res);
+			dev_res->res->end = add_align + r_size - 1;
 
 			list_for_each_entry(dev_res2, head, list) {
 				align = pci_resource_alignment(dev_res2->dev,
 							       dev_res2->res);
-				if (add_align > align)
+				if (add_align > align) {
 					list_move_tail(&dev_res->list,
 						       &dev_res2->list);
+					break;
+				}
 			}
                }
 
diff --git a/drivers/pci/vc.c b/drivers/pci/vc.c
index 7e1304d2e389..dfbab61a1b47 100644
--- a/drivers/pci/vc.c
+++ b/drivers/pci/vc.c
@@ -108,8 +108,7 @@ static void pci_vc_enable(struct pci_dev *dev, int pos, int res)
 	struct pci_dev *link = NULL;
 
 	/* Enable VCs from the downstream device */
-	if (pci_pcie_type(dev) == PCI_EXP_TYPE_ROOT_PORT ||
-	    pci_pcie_type(dev) == PCI_EXP_TYPE_DOWNSTREAM)
+	if (!dev->has_secondary_link)
 		return;
 
 	ctrl_pos = pos + PCI_VC_RES_CTRL + (res * PCI_CAP_VC_PER_VC_SIZEOF);
diff --git a/drivers/pci/xen-pcifront.c b/drivers/pci/xen-pcifront.c
index 7cfd2db02deb..8b7a900cd28b 100644
--- a/drivers/pci/xen-pcifront.c
+++ b/drivers/pci/xen-pcifront.c
@@ -20,6 +20,7 @@
 #include <linux/workqueue.h>
 #include <linux/bitops.h>
 #include <linux/time.h>
+#include <linux/ktime.h>
 #include <xen/platform_pci.h>
 
 #include <asm/xen/swiotlb-xen.h>
@@ -115,7 +116,6 @@ static int do_pci_op(struct pcifront_device *pdev, struct xen_pci_op *op)
 	evtchn_port_t port = pdev->evtchn;
 	unsigned irq = pdev->irq;
 	s64 ns, ns_timeout;
-	struct timeval tv;
 
 	spin_lock_irqsave(&pdev->sh_info_lock, irq_flags);
 
@@ -132,8 +132,7 @@ static int do_pci_op(struct pcifront_device *pdev, struct xen_pci_op *op)
 	 * (in the latter case we end up continually re-executing poll() with a
 	 * timeout in the past). 1s difference gives plenty of slack for error.
 	 */
-	do_gettimeofday(&tv);
-	ns_timeout = timeval_to_ns(&tv) + 2 * (s64)NSEC_PER_SEC;
+	ns_timeout = ktime_get_ns() + 2 * (s64)NSEC_PER_SEC;
 
 	xen_clear_irq_pending(irq);
 
@@ -141,8 +140,7 @@ static int do_pci_op(struct pcifront_device *pdev, struct xen_pci_op *op)
 			(unsigned long *)&pdev->sh_info->flags)) {
 		xen_poll_irq_timeout(irq, jiffies + 3*HZ);
 		xen_clear_irq_pending(irq);
-		do_gettimeofday(&tv);
-		ns = timeval_to_ns(&tv);
+		ns = ktime_get_ns();
 		if (ns > ns_timeout) {
 			dev_err(&pdev->xdev->dev,
 				"pciback not responding!!!\n");
@@ -446,9 +444,15 @@ static int pcifront_scan_root(struct pcifront_device *pdev,
 				 unsigned int domain, unsigned int bus)
 {
 	struct pci_bus *b;
+	LIST_HEAD(resources);
 	struct pcifront_sd *sd = NULL;
 	struct pci_bus_entry *bus_entry = NULL;
 	int err = 0;
+	static struct resource busn_res = {
+		.start = 0,
+		.end = 255,
+		.flags = IORESOURCE_BUS,
+	};
 
 #ifndef CONFIG_PCI_DOMAINS
 	if (domain != 0) {
@@ -470,17 +474,21 @@ static int pcifront_scan_root(struct pcifront_device *pdev,
 		err = -ENOMEM;
 		goto err_out;
 	}
+	pci_add_resource(&resources, &ioport_resource);
+	pci_add_resource(&resources, &iomem_resource);
+	pci_add_resource(&resources, &busn_res);
 	pcifront_init_sd(sd, domain, bus, pdev);
 
 	pci_lock_rescan_remove();
 
-	b = pci_scan_bus_parented(&pdev->xdev->dev, bus,
-				  &pcifront_bus_ops, sd);
+	b = pci_scan_root_bus(&pdev->xdev->dev, bus,
+				  &pcifront_bus_ops, sd, &resources);
 	if (!b) {
 		dev_err(&pdev->xdev->dev,
 			"Error creating PCI Frontend Bus!\n");
 		err = -ENOMEM;
 		pci_unlock_rescan_remove();
+		pci_free_resource_list(&resources);
 		goto err_out;
 	}
 
@@ -488,7 +496,7 @@ static int pcifront_scan_root(struct pcifront_device *pdev,
 
 	list_add(&bus_entry->list, &pdev->root_buses);
 
-	/* pci_scan_bus_parented skips devices which do not have a have
+	/* pci_scan_root_bus skips devices which do not have a
 	* devfn==0. The pcifront_scan_bus enumerates all devfn. */
 	err = pcifront_scan_bus(pdev, domain, bus, b);
 
diff --git a/drivers/pcmcia/Kconfig b/drivers/pcmcia/Kconfig
index a65f821f52eb..d3c378b4db6c 100644
--- a/drivers/pcmcia/Kconfig
+++ b/drivers/pcmcia/Kconfig
@@ -277,7 +277,6 @@ config AT91_CF
 	tristate "AT91 CompactFlash Controller"
 	depends on PCI
 	depends on PCMCIA && ARCH_AT91
-	depends on !ARCH_MULTIPLATFORM
 	help
 	  Say Y here to support the CompactFlash controller on AT91 chips.
 	  Or choose M to compile the driver as a module named "at91_cf".
diff --git a/drivers/pcmcia/at91_cf.c b/drivers/pcmcia/at91_cf.c
index e7775a41ae5d..87147bcd1655 100644
--- a/drivers/pcmcia/at91_cf.c
+++ b/drivers/pcmcia/at91_cf.c
@@ -20,16 +20,15 @@
 #include <linux/platform_data/atmel.h>
 #include <linux/io.h>
 #include <linux/sizes.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mfd/syscon/atmel-mc.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_gpio.h>
+#include <linux/regmap.h>
 
 #include <pcmcia/ss.h>
 
-#include <mach/at91rm9200_mc.h>
-#include <mach/at91_ramc.h>
-
-
 /*
  * A0..A10 work in each range; A23 indicates I/O space;  A25 is CFRNW;
  * some other bit in {A24,A22..A11} is nREG to flag memory access
@@ -40,6 +39,8 @@
 #define	CF_IO_PHYS	(1 << 23)
 #define	CF_MEM_PHYS	(0x017ff800)
 
+struct regmap *mc;
+
 /*--------------------------------------------------------------------------*/
 
 struct at91_cf_socket {
@@ -155,10 +156,7 @@ static int at91_cf_set_io_map(struct pcmcia_socket *s, struct pccard_io_map *io)
 
 	/*
 	 * Use 16 bit accesses unless/until we need 8-bit i/o space.
-	 */
-	csr = at91_ramc_read(0, AT91_SMC_CSR(cf->board->chipselect)) & ~AT91_SMC_DBW;
-
-	/*
+	 *
 	 * NOTE: this CF controller ignores IOIS16, so we can't really do
 	 * MAP_AUTOSZ.  The 16bit mode allows single byte access on either
 	 * D0-D7 (even addr) or D8-D15 (odd), so it's close enough for many
@@ -169,13 +167,14 @@ static int at91_cf_set_io_map(struct pcmcia_socket *s, struct pccard_io_map *io)
 	 * CF 3.0 spec table 35 also giving the D8-D15 option.
 	 */
 	if (!(io->flags & (MAP_16BIT | MAP_AUTOSZ))) {
-		csr |= AT91_SMC_DBW_8;
+		csr = AT91_MC_SMC_DBW_8;
 		dev_dbg(&cf->pdev->dev, "8bit i/o bus\n");
 	} else {
-		csr |= AT91_SMC_DBW_16;
+		csr = AT91_MC_SMC_DBW_16;
 		dev_dbg(&cf->pdev->dev, "16bit i/o bus\n");
 	}
-	at91_ramc_write(0, AT91_SMC_CSR(cf->board->chipselect), csr);
+	regmap_update_bits(mc, AT91_MC_SMC_CSR(cf->board->chipselect),
+			   AT91_MC_SMC_DBW, csr);
 
 	io->start = cf->socket.io_offset;
 	io->stop = io->start + SZ_2K - 1;
@@ -236,6 +235,10 @@ static int at91_cf_dt_init(struct platform_device *pdev)
 
 	pdev->dev.platform_data = board;
 
+	mc = syscon_regmap_lookup_by_compatible("atmel,at91rm9200-sdramc");
+	if (IS_ERR(mc))
+		return PTR_ERR(mc);
+
 	return 0;
 }
 #else
diff --git a/drivers/pcmcia/cistpl.c b/drivers/pcmcia/cistpl.c
index 64d0515b76bd..55ef7d1fd8da 100644
--- a/drivers/pcmcia/cistpl.c
+++ b/drivers/pcmcia/cistpl.c
@@ -94,8 +94,7 @@ static void __iomem *set_cis_map(struct pcmcia_socket *s,
 		mem->res = pcmcia_find_mem_region(0, s->map_size,
 						s->map_size, 0, s);
 		if (mem->res == NULL) {
-			dev_printk(KERN_NOTICE, &s->dev,
-				   "cs: unable to map card memory!\n");
+			dev_notice(&s->dev, "cs: unable to map card memory!\n");
 			return NULL;
 		}
 		s->cis_virt = NULL;
@@ -381,8 +380,7 @@ int verify_cis_cache(struct pcmcia_socket *s)
 
 	buf = kmalloc(256, GFP_KERNEL);
 	if (buf == NULL) {
-		dev_printk(KERN_WARNING, &s->dev,
-			   "no memory for verifying CIS\n");
+		dev_warn(&s->dev, "no memory for verifying CIS\n");
 		return -ENOMEM;
 	}
 	mutex_lock(&s->ops_mutex);
@@ -414,14 +412,14 @@ int pcmcia_replace_cis(struct pcmcia_socket *s,
 		       const u8 *data, const size_t len)
 {
 	if (len > CISTPL_MAX_CIS_SIZE) {
-		dev_printk(KERN_WARNING, &s->dev, "replacement CIS too big\n");
+		dev_warn(&s->dev, "replacement CIS too big\n");
 		return -EINVAL;
 	}
 	mutex_lock(&s->ops_mutex);
 	kfree(s->fake_cis);
 	s->fake_cis = kmalloc(len, GFP_KERNEL);
 	if (s->fake_cis == NULL) {
-		dev_printk(KERN_WARNING, &s->dev, "no memory to replace CIS\n");
+		dev_warn(&s->dev, "no memory to replace CIS\n");
 		mutex_unlock(&s->ops_mutex);
 		return -ENOMEM;
 	}
@@ -434,17 +432,17 @@ int pcmcia_replace_cis(struct pcmcia_socket *s,
 
 /* The high-level CIS tuple services */
 
-typedef struct tuple_flags {
+struct tuple_flags {
 	u_int		link_space:4;
 	u_int		has_link:1;
 	u_int		mfc_fn:3;
 	u_int		space:4;
-} tuple_flags;
+};
 
-#define LINK_SPACE(f)	(((tuple_flags *)(&(f)))->link_space)
-#define HAS_LINK(f)	(((tuple_flags *)(&(f)))->has_link)
-#define MFC_FN(f)	(((tuple_flags *)(&(f)))->mfc_fn)
-#define SPACE(f)	(((tuple_flags *)(&(f)))->space)
+#define LINK_SPACE(f)	(((struct tuple_flags *)(&(f)))->link_space)
+#define HAS_LINK(f)	(((struct tuple_flags *)(&(f)))->has_link)
+#define MFC_FN(f)	(((struct tuple_flags *)(&(f)))->mfc_fn)
+#define SPACE(f)	(((struct tuple_flags *)(&(f)))->space)
 
 int pccard_get_first_tuple(struct pcmcia_socket *s, unsigned int function,
 			tuple_t *tuple)
@@ -1451,26 +1449,16 @@ int pccard_validate_cis(struct pcmcia_socket *s, unsigned int *info)
 done:
 	/* invalidate CIS cache on failure */
 	if (!dev_ok || !ident_ok || !count) {
-#if defined(CONFIG_MTD_PCMCIA_ANONYMOUS)
-		/* Set up as an anonymous card. If we don't have anonymous
-		   memory support then just error the card as there is no
-		   point trying to second guess.
-
-		   Note: some cards have just a device entry, it may be
-		   worth extending support to cover these in future */
-		if (!dev_ok || !ident_ok) {
-			dev_info(&s->dev, "no CIS, assuming an anonymous memory card.\n");
-			pcmcia_replace_cis(s, "\xFF", 1);
-			count = 1;
-			ret = 0;
-		} else
-#endif
-		{
-			mutex_lock(&s->ops_mutex);
-			destroy_cis_cache(s);
-			mutex_unlock(&s->ops_mutex);
+		mutex_lock(&s->ops_mutex);
+		destroy_cis_cache(s);
+		mutex_unlock(&s->ops_mutex);
+		/* We differentiate between dev_ok, ident_ok and count
+		   failures to allow for an override for anonymous cards
+		   in ds.c */
+		if (!dev_ok || !ident_ok)
 			ret = -EIO;
-		}
+		else
+			ret = -EFAULT;
 	}
 
 	if (info)
diff --git a/drivers/pcmcia/cs.c b/drivers/pcmcia/cs.c
index 5292db69c426..c3b615c94b4b 100644
--- a/drivers/pcmcia/cs.c
+++ b/drivers/pcmcia/cs.c
@@ -177,8 +177,8 @@ int pcmcia_register_socket(struct pcmcia_socket *socket)
 
 	wait_for_completion(&socket->thread_done);
 	if (!socket->thread) {
-		dev_printk(KERN_WARNING, &socket->dev,
-			   "PCMCIA: warning: socket thread did not start\n");
+		dev_warn(&socket->dev,
+			 "PCMCIA: warning: socket thread did not start\n");
 		return -EIO;
 	}
 
@@ -275,7 +275,7 @@ static int socket_reset(struct pcmcia_socket *skt)
 		msleep(unreset_check * 10);
 	}
 
-	dev_printk(KERN_ERR, &skt->dev, "time out after reset.\n");
+	dev_err(&skt->dev, "time out after reset\n");
 	return -ETIMEDOUT;
 }
 
@@ -325,8 +325,8 @@ static void socket_shutdown(struct pcmcia_socket *s)
 
 	s->ops->get_status(s, &status);
 	if (status & SS_POWERON) {
-		dev_printk(KERN_ERR, &s->dev,
-			   "*** DANGER *** unable to remove socket power\n");
+		dev_err(&s->dev,
+			"*** DANGER *** unable to remove socket power\n");
 	}
 
 	s->state &= ~SOCKET_INUSE;
@@ -356,15 +356,13 @@ static int socket_setup(struct pcmcia_socket *skt, int initial_delay)
 	}
 
 	if (status & SS_PENDING) {
-		dev_printk(KERN_ERR, &skt->dev,
-			   "voltage interrogation timed out.\n");
+		dev_err(&skt->dev, "voltage interrogation timed out\n");
 		return -ETIMEDOUT;
 	}
 
 	if (status & SS_CARDBUS) {
 		if (!(skt->features & SS_CAP_CARDBUS)) {
-			dev_printk(KERN_ERR, &skt->dev,
-				"cardbus cards are not supported.\n");
+			dev_err(&skt->dev, "cardbus cards are not supported\n");
 			return -EINVAL;
 		}
 		skt->state |= SOCKET_CARDBUS;
@@ -379,7 +377,7 @@ static int socket_setup(struct pcmcia_socket *skt, int initial_delay)
 	else if (!(status & SS_XVCARD))
 		skt->socket.Vcc = skt->socket.Vpp = 50;
 	else {
-		dev_printk(KERN_ERR, &skt->dev, "unsupported voltage key.\n");
+		dev_err(&skt->dev, "unsupported voltage key\n");
 		return -EIO;
 	}
 
@@ -396,7 +394,7 @@ static int socket_setup(struct pcmcia_socket *skt, int initial_delay)
 
 	skt->ops->get_status(skt, &status);
 	if (!(status & SS_POWERON)) {
-		dev_printk(KERN_ERR, &skt->dev, "unable to apply power.\n");
+		dev_err(&skt->dev, "unable to apply power\n");
 		return -EIO;
 	}
 
@@ -429,8 +427,7 @@ static int socket_insert(struct pcmcia_socket *skt)
 	if (ret == 0) {
 		skt->state |= SOCKET_PRESENT;
 
-		dev_printk(KERN_NOTICE, &skt->dev,
-			   "pccard: %s card inserted into slot %d\n",
+		dev_notice(&skt->dev, "pccard: %s card inserted into slot %d\n",
 			   (skt->state & SOCKET_CARDBUS) ? "CardBus" : "PCMCIA",
 			   skt->sock);
 
@@ -558,8 +555,7 @@ static int socket_resume(struct pcmcia_socket *skt)
 
 static void socket_remove(struct pcmcia_socket *skt)
 {
-	dev_printk(KERN_NOTICE, &skt->dev,
-		   "pccard: card ejected from slot %d\n", skt->sock);
+	dev_notice(&skt->dev, "pccard: card ejected from slot %d\n", skt->sock);
 	socket_shutdown(skt);
 }
 
@@ -605,8 +601,7 @@ static int pccardd(void *__skt)
 	/* register with the device core */
 	ret = device_register(&skt->dev);
 	if (ret) {
-		dev_printk(KERN_WARNING, &skt->dev,
-			   "PCMCIA: unable to register socket\n");
+		dev_warn(&skt->dev, "PCMCIA: unable to register socket\n");
 		skt->thread = NULL;
 		complete(&skt->thread_done);
 		return 0;
@@ -626,8 +621,6 @@ static int pccardd(void *__skt)
 		unsigned int events;
 		unsigned int sysfs_events;
 
-		set_current_state(TASK_INTERRUPTIBLE);
-
 		spin_lock_irqsave(&skt->thread_lock, flags);
 		events = skt->thread_events;
 		skt->thread_events = 0;
@@ -675,11 +668,15 @@ static int pccardd(void *__skt)
 		if (kthread_should_stop())
 			break;
 
+		set_current_state(TASK_INTERRUPTIBLE);
+
 		schedule();
+
+		/* make sure we are running */
+		__set_current_state(TASK_RUNNING);
+
 		try_to_freeze();
 	}
-	/* make sure we are running before we exit */
-	set_current_state(TASK_RUNNING);
 
 	/* shut down socket, if a device is still present */
 	if (skt->state & SOCKET_PRESENT) {
diff --git a/drivers/pcmcia/ds.c b/drivers/pcmcia/ds.c
index d3baf0bfca9f..0decee6c556e 100644
--- a/drivers/pcmcia/ds.c
+++ b/drivers/pcmcia/ds.c
@@ -81,8 +81,8 @@ static void pcmcia_check_driver(struct pcmcia_driver *p_drv)
 
 
 struct pcmcia_dynid {
-	struct list_head 		node;
-	struct pcmcia_device_id 	id;
+	struct list_head		node;
+	struct pcmcia_device_id		id;
 };
 
 /**
@@ -284,8 +284,8 @@ static int pcmcia_device_probe(struct device *dev)
 		dev_dbg(dev, "base %x, regs %x", p_dev->config_base,
 			p_dev->config_regs);
 	} else {
-		dev_printk(KERN_INFO, dev,
-			   "pcmcia: could not parse base and rmask0 of CIS\n");
+		dev_info(dev,
+			 "pcmcia: could not parse base and rmask0 of CIS\n");
 		p_dev->config_base = 0;
 		p_dev->config_regs = 0;
 	}
@@ -382,15 +382,15 @@ static int pcmcia_device_remove(struct device *dev)
 
 	/* check for proper unloading */
 	if (p_dev->_irq || p_dev->_io || p_dev->_locked)
-		dev_printk(KERN_INFO, dev,
-			"pcmcia: driver %s did not release config properly\n",
-			p_drv->name);
+		dev_info(dev,
+			 "pcmcia: driver %s did not release config properly\n",
+			 p_drv->name);
 
 	for (i = 0; i < MAX_WIN; i++)
 		if (p_dev->_win & CLIENT_WIN_REQ(i))
-			dev_printk(KERN_INFO, dev,
-			  "pcmcia: driver %s did not release window properly\n",
-			   p_drv->name);
+			dev_info(dev,
+				 "pcmcia: driver %s did not release window properly\n",
+				 p_drv->name);
 
 	/* references from pcmcia_probe_device */
 	pcmcia_put_dev(p_dev);
@@ -566,7 +566,7 @@ static struct pcmcia_device *pcmcia_device_add(struct pcmcia_socket *s,
 			c->io[i].name = p_dev->devname;
 			c->io[i].flags = IORESOURCE_IO;
 		}
-		for (i = 0; i< MAX_WIN; i++) {
+		for (i = 0; i < MAX_WIN; i++) {
 			c->mem[i].name = p_dev->devname;
 			c->mem[i].flags = IORESOURCE_MEM;
 		}
@@ -578,8 +578,7 @@ static struct pcmcia_device *pcmcia_device_add(struct pcmcia_socket *s,
 
 	mutex_unlock(&s->ops_mutex);
 
-	dev_printk(KERN_NOTICE, &p_dev->dev,
-		   "pcmcia: registering new device %s (IRQ: %d)\n",
+	dev_notice(&p_dev->dev, "pcmcia: registering new device %s (IRQ: %d)\n",
 		   p_dev->devname, p_dev->irq);
 
 	pcmcia_device_query(p_dev);
@@ -634,8 +633,24 @@ static int pcmcia_card_add(struct pcmcia_socket *s)
 
 	ret = pccard_validate_cis(s, &no_chains);
 	if (ret || !no_chains) {
-		dev_dbg(&s->dev, "invalid CIS or invalid resources\n");
-		return -ENODEV;
+#if defined(CONFIG_MTD_PCMCIA_ANONYMOUS)
+		/* Set up as an anonymous card. If we don't have anonymous
+		   memory support then just error the card as there is no
+		   point trying to second guess.
+
+		   Note: some cards have just a device entry, it may be
+		   worth extending support to cover these in future */
+		if (ret == -EIO) {
+			dev_info(&s->dev, "no CIS, assuming an anonymous memory card.\n");
+			pcmcia_replace_cis(s, "\xFF", 1);
+			no_chains = 1;
+			ret = 0;
+		} else
+#endif
+		{
+			dev_dbg(&s->dev, "invalid CIS or invalid resources\n");
+			return -ENODEV;
+		}
 	}
 
 	if (!pccard_read_tuple(s, BIND_FN_ALL, CISTPL_LONGLINK_MFC, &mfc))
@@ -651,7 +666,7 @@ static int pcmcia_card_add(struct pcmcia_socket *s)
 }
 
 
-static int pcmcia_requery_callback(struct device *dev, void * _data)
+static int pcmcia_requery_callback(struct device *dev, void *_data)
 {
 	struct pcmcia_device *p_dev = to_pcmcia_dev(dev);
 	if (!p_dev->dev.driver) {
@@ -729,7 +744,7 @@ static void pcmcia_requery(struct pcmcia_socket *s)
  * the one provided by the card is broken. The firmware files reside in
  * /lib/firmware/ in userspace.
  */
-static int pcmcia_load_firmware(struct pcmcia_device *dev, char * filename)
+static int pcmcia_load_firmware(struct pcmcia_device *dev, char *filename)
 {
 	struct pcmcia_socket *s = dev->socket;
 	const struct firmware *fw;
@@ -745,16 +760,14 @@ static int pcmcia_load_firmware(struct pcmcia_device *dev, char * filename)
 	if (request_firmware(&fw, filename, &dev->dev) == 0) {
 		if (fw->size >= CISTPL_MAX_CIS_SIZE) {
 			ret = -EINVAL;
-			dev_printk(KERN_ERR, &dev->dev,
-				   "pcmcia: CIS override is too big\n");
+			dev_err(&dev->dev, "pcmcia: CIS override is too big\n");
 			goto release;
 		}
 
 		if (!pcmcia_replace_cis(s, fw->data, fw->size))
 			ret = 0;
 		else {
-			dev_printk(KERN_ERR, &dev->dev,
-				   "pcmcia: CIS override failed\n");
+			dev_err(&dev->dev, "pcmcia: CIS override failed\n");
 			goto release;
 		}
 
@@ -781,7 +794,8 @@ static int pcmcia_load_firmware(struct pcmcia_device *dev, char * filename)
 
 #else /* !CONFIG_PCMCIA_LOAD_CIS */
 
-static inline int pcmcia_load_firmware(struct pcmcia_device *dev, char * filename)
+static inline int pcmcia_load_firmware(struct pcmcia_device *dev,
+				       char *filename)
 {
 	return -ENODEV;
 }
@@ -1148,10 +1162,9 @@ static int pcmcia_dev_suspend(struct device *dev, pm_message_t state)
 	if (p_drv->suspend) {
 		ret = p_drv->suspend(p_dev);
 		if (ret) {
-			dev_printk(KERN_ERR, dev,
-				   "pcmcia: device %s (driver %s) did "
-				   "not want to go to sleep (%d)\n",
-				   p_dev->devname, p_drv->name, ret);
+			dev_err(dev,
+				"pcmcia: device %s (driver %s) did not want to go to sleep (%d)\n",
+				p_dev->devname, p_drv->name, ret);
 			mutex_lock(&p_dev->socket->ops_mutex);
 			p_dev->suspended = 0;
 			mutex_unlock(&p_dev->socket->ops_mutex);
@@ -1206,7 +1219,7 @@ static int pcmcia_dev_resume(struct device *dev)
 }
 
 
-static int pcmcia_bus_suspend_callback(struct device *dev, void * _data)
+static int pcmcia_bus_suspend_callback(struct device *dev, void *_data)
 {
 	struct pcmcia_socket *skt = _data;
 	struct pcmcia_device *p_dev = to_pcmcia_dev(dev);
@@ -1217,7 +1230,7 @@ static int pcmcia_bus_suspend_callback(struct device *dev, void * _data)
 	return runtime_suspend(dev);
 }
 
-static int pcmcia_bus_resume_callback(struct device *dev, void * _data)
+static int pcmcia_bus_resume_callback(struct device *dev, void *_data)
 {
 	struct pcmcia_socket *skt = _data;
 	struct pcmcia_device *p_dev = to_pcmcia_dev(dev);
@@ -1342,14 +1355,13 @@ static int pcmcia_bus_add_socket(struct device *dev,
 
 	socket = pcmcia_get_socket(socket);
 	if (!socket) {
-		dev_printk(KERN_ERR, dev,
-			   "PCMCIA obtaining reference to socket failed\n");
+		dev_err(dev, "PCMCIA obtaining reference to socket failed\n");
 		return -ENODEV;
 	}
 
 	ret = sysfs_create_bin_file(&dev->kobj, &pccard_cis_attr);
 	if (ret) {
-		dev_printk(KERN_ERR, dev, "PCMCIA registration failed\n");
+		dev_err(dev, "PCMCIA registration failed\n");
 		pcmcia_put_socket(socket);
 		return ret;
 	}
@@ -1361,7 +1373,7 @@ static int pcmcia_bus_add_socket(struct device *dev,
 
 	ret = pccard_register_pcmcia(socket, &pcmcia_bus_callback);
 	if (ret) {
-		dev_printk(KERN_ERR, dev, "PCMCIA registration failed\n");
+		dev_err(dev, "PCMCIA registration failed\n");
 		pcmcia_put_socket(socket);
 		return ret;
 	}
diff --git a/drivers/pcmcia/electra_cf.c b/drivers/pcmcia/electra_cf.c
index 7f9950d324df..61cf61ac621e 100644
--- a/drivers/pcmcia/electra_cf.c
+++ b/drivers/pcmcia/electra_cf.c
@@ -48,14 +48,14 @@ struct electra_cf_socket {
 
 	struct platform_device	*ofdev;
 	unsigned long		mem_phys;
-	void __iomem *		mem_base;
+	void __iomem		*mem_base;
 	unsigned long		mem_size;
-	void __iomem *		io_virt;
+	void __iomem		*io_virt;
 	unsigned int		io_base;
 	unsigned int		io_size;
 	u_int			irq;
 	struct resource		iomem;
-	void __iomem *		gpio_base;
+	void __iomem		*gpio_base;
 	int			gpio_detect;
 	int			gpio_vsense;
 	int			gpio_3v;
@@ -202,7 +202,7 @@ static int electra_cf_probe(struct platform_device *ofdev)
 	if (err)
 		return -EINVAL;
 
-	cf = kzalloc(sizeof *cf, GFP_KERNEL);
+	cf = kzalloc(sizeof(*cf), GFP_KERNEL);
 	if (!cf)
 		return -ENOMEM;
 
@@ -216,8 +216,10 @@ static int electra_cf_probe(struct platform_device *ofdev)
 	cf->io_size = PAGE_ALIGN(resource_size(&io));
 
 	area = __get_vm_area(cf->io_size, 0, PHB_IO_BASE, PHB_IO_END);
-	if (area == NULL)
-		return -ENOMEM;
+	if (area == NULL) {
+		status = -ENOMEM;
+		goto fail1;
+	}
 
 	cf->io_virt = (void __iomem *)(area->addr);
 
@@ -320,7 +322,8 @@ fail1:
 		iounmap(cf->mem_base);
 	if (cf->gpio_base)
 		iounmap(cf->gpio_base);
-	device_init_wakeup(&ofdev->dev, 0);
+	if (area)
+		device_init_wakeup(&ofdev->dev, 0);
 	kfree(cf);
 	return status;
 
@@ -369,5 +372,5 @@ static struct platform_driver electra_cf_driver = {
 module_platform_driver(electra_cf_driver);
 
 MODULE_LICENSE("GPL");
-MODULE_AUTHOR ("Olof Johansson <olof@lixom.net>");
+MODULE_AUTHOR("Olof Johansson <olof@lixom.net>");
 MODULE_DESCRIPTION("PA Semi Electra CF driver");
diff --git a/drivers/pcmcia/i82365.c b/drivers/pcmcia/i82365.c
index a2c138719bac..eb0d80a429e4 100644
--- a/drivers/pcmcia/i82365.c
+++ b/drivers/pcmcia/i82365.c
@@ -132,14 +132,14 @@ module_param(recov_time, int, 0444);
 
 /*====================================================================*/
 
-typedef struct cirrus_state_t {
+struct cirrus_state {
     u_char		misc1, misc2;
     u_char		timer[6];
-} cirrus_state_t;
+};
 
-typedef struct vg46x_state_t {
+struct vg46x_state {
     u_char		ctl, ema;
-} vg46x_state_t;
+};
 
 struct i82365_socket {
     u_short		type, flags;
@@ -149,8 +149,8 @@ struct i82365_socket {
     u_short		psock;
     u_char		cs_irq, intr;
     union {
-	cirrus_state_t		cirrus;
-	vg46x_state_t		vg46x;
+	struct cirrus_state		cirrus;
+	struct vg46x_state		vg46x;
     } state;
 };
 
@@ -173,11 +173,11 @@ static struct timer_list poll_timer;
 /*====================================================================*/
 
 /* These definitions must match the pcic table! */
-typedef enum pcic_id {
+enum pcic_id {
     IS_I82365A, IS_I82365B, IS_I82365DF,
     IS_IBM, IS_RF5Cx96, IS_VLSI, IS_VG468, IS_VG469,
     IS_PD6710, IS_PD672X, IS_VT83C469,
-} pcic_id;
+};
 
 /* Flags for classifying groups of controllers */
 #define IS_VADEM	0x0001
@@ -189,12 +189,12 @@ typedef enum pcic_id {
 #define IS_REGISTERED	0x2000
 #define IS_ALIVE	0x8000
 
-typedef struct pcic_t {
+struct pcic {
     char		*name;
     u_short		flags;
-} pcic_t;
+};
 
-static pcic_t pcic[] = {
+static struct pcic pcic[] = {
     { "Intel i82365sl A step", 0 },
     { "Intel i82365sl B step", 0 },
     { "Intel i82365sl DF", IS_DF_PWR },
@@ -208,7 +208,7 @@ static pcic_t pcic[] = {
     { "VIA VT83C469", IS_CIRRUS|IS_VIA },
 };
 
-#define PCIC_COUNT	(sizeof(pcic)/sizeof(pcic_t))
+#define PCIC_COUNT	ARRAY_SIZE(pcic)
 
 /*====================================================================*/
 
@@ -294,7 +294,7 @@ static void i365_set_pair(u_short sock, u_short reg, u_short data)
 static void cirrus_get_state(u_short s)
 {
     int i;
-    cirrus_state_t *p = &socket[s].state.cirrus;
+    struct cirrus_state *p = &socket[s].state.cirrus;
     p->misc1 = i365_get(s, PD67_MISC_CTL_1);
     p->misc1 &= (PD67_MC1_MEDIA_ENA | PD67_MC1_INPACK_ENA);
     p->misc2 = i365_get(s, PD67_MISC_CTL_2);
@@ -306,7 +306,7 @@ static void cirrus_set_state(u_short s)
 {
     int i;
     u_char misc;
-    cirrus_state_t *p = &socket[s].state.cirrus;
+    struct cirrus_state *p = &socket[s].state.cirrus;
 
     misc = i365_get(s, PD67_MISC_CTL_2);
     i365_set(s, PD67_MISC_CTL_2, p->misc2);
@@ -321,7 +321,7 @@ static void cirrus_set_state(u_short s)
 static u_int __init cirrus_set_opts(u_short s, char *buf)
 {
     struct i82365_socket *t = &socket[s];
-    cirrus_state_t *p = &socket[s].state.cirrus;
+    struct cirrus_state *p = &socket[s].state.cirrus;
     u_int mask = 0xffff;
 
     if (has_ring == -1) has_ring = 1;
@@ -377,7 +377,7 @@ static u_int __init cirrus_set_opts(u_short s, char *buf)
 
 static void vg46x_get_state(u_short s)
 {
-    vg46x_state_t *p = &socket[s].state.vg46x;
+    struct vg46x_state *p = &socket[s].state.vg46x;
     p->ctl = i365_get(s, VG468_CTL);
     if (socket[s].type == IS_VG469)
 	p->ema = i365_get(s, VG469_EXT_MODE);
@@ -385,7 +385,7 @@ static void vg46x_get_state(u_short s)
 
 static void vg46x_set_state(u_short s)
 {
-    vg46x_state_t *p = &socket[s].state.vg46x;
+    struct vg46x_state *p = &socket[s].state.vg46x;
     i365_set(s, VG468_CTL, p->ctl);
     if (socket[s].type == IS_VG469)
 	i365_set(s, VG469_EXT_MODE, p->ema);
@@ -393,7 +393,7 @@ static void vg46x_set_state(u_short s)
 
 static u_int __init vg46x_set_opts(u_short s, char *buf)
 {
-    vg46x_state_t *p = &socket[s].state.vg46x;
+    struct vg46x_state *p = &socket[s].state.vg46x;
     
     flip(p->ctl, VG468_CTL_ASYNC, async_clock);
     flip(p->ema, VG469_MODE_CABLE, cable_mode);
@@ -1285,13 +1285,6 @@ static int __init init_i82365(void)
 	    ret = pcmcia_register_socket(&socket[i].socket);
 	    if (!ret)
 		    socket[i].flags |= IS_REGISTERED;
-
-#if 0 /* driver model ordering issue */
-	   class_device_create_file(&socket[i].socket.dev,
-			   	    &class_device_attr_info);
-	   class_device_create_file(&socket[i].socket.dev,
-			   	    &class_device_attr_exca);
-#endif
     }
 
     /* Finally, schedule a polling interrupt */
diff --git a/drivers/pcmcia/m32r_cfc.c b/drivers/pcmcia/m32r_cfc.c
index 0075bd7162ed..70b089430fcc 100644
--- a/drivers/pcmcia/m32r_cfc.c
+++ b/drivers/pcmcia/m32r_cfc.c
@@ -754,13 +754,6 @@ static int __init init_m32r_pcc(void)
 		ret = pcmcia_register_socket(&socket[i].socket);
 		if (!ret)
 			socket[i].flags |= IS_REGISTERED;
-
-#if 0	/* driver model ordering issue */
-		class_device_create_file(&socket[i].socket.dev,
-					 &class_device_attr_info);
-		class_device_create_file(&socket[i].socket.dev,
-					 &class_device_attr_exca);
-#endif
 	}
 
 	/* Finally, schedule a polling interrupt */
diff --git a/drivers/pcmcia/m32r_pcc.c b/drivers/pcmcia/m32r_pcc.c
index a77e571b08b8..eb126b98ed8a 100644
--- a/drivers/pcmcia/m32r_pcc.c
+++ b/drivers/pcmcia/m32r_pcc.c
@@ -716,13 +716,6 @@ static int __init init_m32r_pcc(void)
 		ret = pcmcia_register_socket(&socket[i].socket);
 		if (!ret)
 			socket[i].flags |= IS_REGISTERED;
-
-#if 0	/* driver model ordering issue */
-		class_device_create_file(&socket[i].socket.dev,
-					 &class_device_attr_info);
-		class_device_create_file(&socket[i].socket.dev,
-					 &class_device_attr_exca);
-#endif
 	}
 
 	/* Finally, schedule a polling interrupt */
diff --git a/drivers/pcmcia/pcmcia_cis.c b/drivers/pcmcia/pcmcia_cis.c
index e2c92415b892..1c05d74e850d 100644
--- a/drivers/pcmcia/pcmcia_cis.c
+++ b/drivers/pcmcia/pcmcia_cis.c
@@ -44,7 +44,7 @@ int pccard_read_tuple(struct pcmcia_socket *s, unsigned int function,
 
 	buf = kmalloc(256, GFP_KERNEL);
 	if (buf == NULL) {
-		dev_printk(KERN_WARNING, &s->dev, "no memory to read tuple\n");
+		dev_warn(&s->dev, "no memory to read tuple\n");
 		return -ENOMEM;
 	}
 	tuple.DesiredTuple = code;
@@ -94,7 +94,7 @@ int pccard_loop_tuple(struct pcmcia_socket *s, unsigned int function,
 
 	buf = kzalloc(256, GFP_KERNEL);
 	if (buf == NULL) {
-		dev_printk(KERN_WARNING, &s->dev, "no memory to read tuple\n");
+		dev_warn(&s->dev, "no memory to read tuple\n");
 		return -ENOMEM;
 	}
 
diff --git a/drivers/pcmcia/pcmcia_resource.c b/drivers/pcmcia/pcmcia_resource.c
index e8c19def1b0f..34aad895a239 100644
--- a/drivers/pcmcia/pcmcia_resource.c
+++ b/drivers/pcmcia/pcmcia_resource.c
@@ -508,8 +508,7 @@ int pcmcia_enable_device(struct pcmcia_device *p_dev)
 	s->socket.Vpp = p_dev->vpp;
 	if (s->ops->set_socket(s, &s->socket)) {
 		mutex_unlock(&s->ops_mutex);
-		dev_printk(KERN_WARNING, &p_dev->dev,
-			   "Unable to set socket state\n");
+		dev_warn(&p_dev->dev, "Unable to set socket state\n");
 		return -EINVAL;
 	}
 
@@ -736,13 +735,11 @@ __pcmcia_request_exclusive_irq(struct pcmcia_device *p_dev,
 	ret = request_irq(p_dev->irq, handler, 0, p_dev->devname, p_dev->priv);
 	if (ret) {
 		ret = pcmcia_request_irq(p_dev, handler);
-		dev_printk(KERN_WARNING, &p_dev->dev, "pcmcia: "
-			"request for exclusive IRQ could not be fulfilled.\n");
-		dev_printk(KERN_WARNING, &p_dev->dev, "pcmcia: the driver "
-			"needs updating to supported shared IRQ lines.\n");
+		dev_warn(&p_dev->dev, "pcmcia: request for exclusive IRQ could not be fulfilled\n");
+		dev_warn(&p_dev->dev, "pcmcia: the driver needs updating to supported shared IRQ lines\n");
 	}
 	if (ret)
-		dev_printk(KERN_INFO, &p_dev->dev, "request_irq() failed\n");
+		dev_info(&p_dev->dev, "request_irq() failed\n");
 	else
 		p_dev->_irq = 1;
 
diff --git a/drivers/pcmcia/rsrc_nonstatic.c b/drivers/pcmcia/rsrc_nonstatic.c
index 065704c605d5..5ef7b46a2578 100644
--- a/drivers/pcmcia/rsrc_nonstatic.c
+++ b/drivers/pcmcia/rsrc_nonstatic.c
@@ -191,15 +191,13 @@ static void do_io_probe(struct pcmcia_socket *s, unsigned int base,
 	int any;
 	u_char *b, hole, most;
 
-	dev_printk(KERN_INFO, &s->dev, "cs: IO port probe %#x-%#x:",
-		base, base+num-1);
+	dev_info(&s->dev, "cs: IO port probe %#x-%#x:", base, base+num-1);
 
 	/* First, what does a floating port look like? */
 	b = kzalloc(256, GFP_KERNEL);
 	if (!b) {
-		printk("\n");
-		dev_printk(KERN_ERR, &s->dev,
-			"do_io_probe: unable to kmalloc 256 bytes");
+		pr_cont("\n");
+		dev_err(&s->dev, "do_io_probe: unable to kmalloc 256 bytes\n");
 		return;
 	}
 	for (i = base, most = 0; i < base+num; i += 8) {
@@ -223,7 +221,7 @@ static void do_io_probe(struct pcmcia_socket *s, unsigned int base,
 		res = claim_region(s, i, 8, IORESOURCE_IO, "PCMCIA ioprobe");
 		if (!res) {
 			if (!any)
-				printk(" excluding");
+				pr_cont(" excluding");
 			if (!bad)
 				bad = any = i;
 			continue;
@@ -234,13 +232,13 @@ static void do_io_probe(struct pcmcia_socket *s, unsigned int base,
 		free_region(res);
 		if (j < 8) {
 			if (!any)
-				printk(" excluding");
+				pr_cont(" excluding");
 			if (!bad)
 				bad = any = i;
 		} else {
 			if (bad) {
 				sub_interval(&s_data->io_db, bad, i-bad);
-				printk(" %#x-%#x", bad, i-1);
+				pr_cont(" %#x-%#x", bad, i-1);
 				bad = 0;
 			}
 		}
@@ -248,15 +246,15 @@ static void do_io_probe(struct pcmcia_socket *s, unsigned int base,
 	if (bad) {
 		if ((num > 16) && (bad == base) && (i == base+num)) {
 			sub_interval(&s_data->io_db, bad, i-bad);
-			printk(" nothing: probe failed.\n");
+			pr_cont(" nothing: probe failed.\n");
 			return;
 		} else {
 			sub_interval(&s_data->io_db, bad, i-bad);
-			printk(" %#x-%#x", bad, i-1);
+			pr_cont(" %#x-%#x", bad, i-1);
 		}
 	}
 
-	printk(any ? "\n" : " clean.\n");
+	pr_cont("%s\n", !any ? " clean" : "");
 }
 #endif
 
@@ -413,8 +411,8 @@ static int do_mem_probe(struct pcmcia_socket *s, u_long base, u_long num,
 	struct socket_data *s_data = s->resource_data;
 	u_long i, j, bad, fail, step;
 
-	dev_printk(KERN_INFO, &s->dev, "cs: memory probe 0x%06lx-0x%06lx:",
-		base, base+num-1);
+	dev_info(&s->dev, "cs: memory probe 0x%06lx-0x%06lx:",
+		 base, base+num-1);
 	bad = fail = 0;
 	step = (num < 0x20000) ? 0x2000 : ((num>>4) & ~0x1fff);
 	/* don't allow too large steps */
@@ -438,13 +436,13 @@ static int do_mem_probe(struct pcmcia_socket *s, u_long base, u_long num,
 		}
 		if (i != j) {
 			if (!bad)
-				printk(" excluding");
-			printk(" %#05lx-%#05lx", i, j-1);
+				pr_cont(" excluding");
+			pr_cont(" %#05lx-%#05lx", i, j-1);
 			sub_interval(&s_data->mem_db, i, j-i);
 			bad += j-i;
 		}
 	}
-	printk(bad ? "\n" : " clean.\n");
+	pr_cont("%s\n", !bad ? " clean" : "");
 	return num - bad;
 }
 
@@ -495,7 +493,7 @@ static int validate_mem(struct pcmcia_socket *s, unsigned int probe_mask)
 			return 0;
 		if (s_data->mem_db_valid.next != &s_data->mem_db_valid)
 			return 0;
-		dev_printk(KERN_NOTICE, &s->dev,
+		dev_notice(&s->dev,
 			   "cs: warning: no high memory space available!\n");
 		return -ENODEV;
 	}
@@ -975,9 +973,9 @@ static int nonstatic_autoadd_resources(struct pcmcia_socket *s)
 			if (res == &ioport_resource)
 				continue;
 
-			dev_printk(KERN_INFO, &s->cb_dev->dev,
-				   "pcmcia: parent PCI bridge window: %pR\n",
-				   res);
+			dev_info(&s->cb_dev->dev,
+				 "pcmcia: parent PCI bridge window: %pR\n",
+				 res);
 			if (!adjust_io(s, ADD_MANAGED_RESOURCE, res->start, res->end))
 				done |= IORESOURCE_IO;
 
@@ -990,9 +988,9 @@ static int nonstatic_autoadd_resources(struct pcmcia_socket *s)
 			if (res == &iomem_resource)
 				continue;
 
-			dev_printk(KERN_INFO, &s->cb_dev->dev,
-				   "pcmcia: parent PCI bridge window: %pR\n",
-				   res);
+			dev_info(&s->cb_dev->dev,
+				 "pcmcia: parent PCI bridge window: %pR\n",
+				 res);
 			if (!adjust_memory(s, ADD_MANAGED_RESOURCE, res->start, res->end))
 				done |= IORESOURCE_MEM;
 		}
diff --git a/drivers/pcmcia/ti113x.h b/drivers/pcmcia/ti113x.h
index a71789486cdf..5cb670e037a0 100644
--- a/drivers/pcmcia/ti113x.h
+++ b/drivers/pcmcia/ti113x.h
@@ -372,8 +372,8 @@ static void ti12xx_irqroute_func0(struct yenta_socket *socket)
 
 	mfunc = mfunc_old = config_readl(socket, TI122X_MFUNC);
 	devctl = config_readb(socket, TI113X_DEVICE_CONTROL);
-	dev_printk(KERN_INFO, &socket->dev->dev,
-		   "TI: mfunc 0x%08x, devctl 0x%02x\n", mfunc, devctl);
+	dev_info(&socket->dev->dev, "TI: mfunc 0x%08x, devctl 0x%02x\n",
+		 mfunc, devctl);
 
 	/* make sure PCI interrupts are enabled before probing */
 	ti_init(socket);
@@ -387,8 +387,8 @@ static void ti12xx_irqroute_func0(struct yenta_socket *socket)
 	 * We're here which means PCI interrupts are _not_ delivered. try to
 	 * find the right setting (all serial or parallel)
 	 */
-	dev_printk(KERN_INFO, &socket->dev->dev,
-		   "TI: probing PCI interrupt failed, trying to fix\n");
+	dev_info(&socket->dev->dev,
+		 "TI: probing PCI interrupt failed, trying to fix\n");
 
 	/* for serial PCI make sure MFUNC3 is set to IRQSER */
 	if ((devctl & TI113X_DCR_IMODE_MASK) == TI12XX_DCR_IMODE_ALL_SERIAL) {
@@ -412,8 +412,8 @@ static void ti12xx_irqroute_func0(struct yenta_socket *socket)
 
 				pci_irq_status = yenta_probe_cb_irq(socket);
 				if (pci_irq_status == 1) {
-					dev_printk(KERN_INFO, &socket->dev->dev,
-					    "TI: all-serial interrupts ok\n");
+					dev_info(&socket->dev->dev,
+						 "TI: all-serial interrupts ok\n");
 					mfunc_old = mfunc;
 					goto out;
 				}
@@ -428,8 +428,8 @@ static void ti12xx_irqroute_func0(struct yenta_socket *socket)
 		}
 
 		/* serial PCI interrupts not working fall back to parallel */
-		dev_printk(KERN_INFO, &socket->dev->dev,
-			   "TI: falling back to parallel PCI interrupts\n");
+		dev_info(&socket->dev->dev,
+			 "TI: falling back to parallel PCI interrupts\n");
 		devctl &= ~TI113X_DCR_IMODE_MASK;
 		devctl |= TI113X_DCR_IMODE_SERIAL; /* serial ISA could be right */
 		config_writeb(socket, TI113X_DEVICE_CONTROL, devctl);
@@ -460,8 +460,7 @@ static void ti12xx_irqroute_func0(struct yenta_socket *socket)
 	pci_irq_status = yenta_probe_cb_irq(socket);
 	if (pci_irq_status == 1) {
 		mfunc_old = mfunc;
-		dev_printk(KERN_INFO, &socket->dev->dev,
-			   "TI: parallel PCI interrupts ok\n");
+		dev_info(&socket->dev->dev, "TI: parallel PCI interrupts ok\n");
 	} else {
 		/* not working, back to old value */
 		mfunc = mfunc_old;
@@ -473,9 +472,8 @@ static void ti12xx_irqroute_func0(struct yenta_socket *socket)
 out:
 	if (pci_irq_status < 1) {
 		socket->cb_irq = 0;
-		dev_printk(KERN_INFO, &socket->dev->dev,
-			   "Yenta TI: no PCI interrupts. Fish. "
-			   "Please report.\n");
+		dev_info(&socket->dev->dev,
+			 "Yenta TI: no PCI interrupts. Fish. Please report.\n");
 	}
 }
 
@@ -547,9 +545,8 @@ static void ti12xx_irqroute_func1(struct yenta_socket *socket)
 
 	mfunc = mfunc_old = config_readl(socket, TI122X_MFUNC);
 	devctl = config_readb(socket, TI113X_DEVICE_CONTROL);
-	dev_printk(KERN_INFO, &socket->dev->dev,
-		   "TI: mfunc 0x%08x, devctl 0x%02x\n",
-		   mfunc, devctl);
+	dev_info(&socket->dev->dev, "TI: mfunc 0x%08x, devctl 0x%02x\n",
+		 mfunc, devctl);
 
 	/* if IRQs are configured as tied, align irq of func1 with func0 */
 	sysctl = config_readl(socket, TI113X_SYSTEM_CONTROL);
@@ -568,8 +565,8 @@ static void ti12xx_irqroute_func1(struct yenta_socket *socket)
 	 * We're here which means PCI interrupts are _not_ delivered. try to
 	 * find the right setting
 	 */
-	dev_printk(KERN_INFO, &socket->dev->dev,
-		   "TI: probing PCI interrupt failed, trying to fix\n");
+	dev_info(&socket->dev->dev,
+		 "TI: probing PCI interrupt failed, trying to fix\n");
 
 	/* if all serial: set INTRTIE, probe again */
 	if ((devctl & TI113X_DCR_IMODE_MASK) == TI12XX_DCR_IMODE_ALL_SERIAL) {
@@ -578,8 +575,8 @@ static void ti12xx_irqroute_func1(struct yenta_socket *socket)
 		if (ti12xx_tie_interrupts(socket, &old_irq)) {
 			pci_irq_status = yenta_probe_cb_irq(socket);
 			if (pci_irq_status == 1) {
-				dev_printk(KERN_INFO, &socket->dev->dev,
-					"TI: all-serial interrupts, tied ok\n");
+				dev_info(&socket->dev->dev,
+					 "TI: all-serial interrupts, tied ok\n");
 				goto out;
 			}
 
@@ -616,8 +613,8 @@ static void ti12xx_irqroute_func1(struct yenta_socket *socket)
 
 			pci_irq_status = yenta_probe_cb_irq(socket);
 			if (pci_irq_status == 1) {
-				dev_printk(KERN_INFO, &socket->dev->dev,
-					   "TI: parallel PCI interrupts ok\n");
+				dev_info(&socket->dev->dev,
+					 "TI: parallel PCI interrupts ok\n");
 				goto out;
 			}
 
@@ -632,8 +629,8 @@ static void ti12xx_irqroute_func1(struct yenta_socket *socket)
 		if (ti12xx_tie_interrupts(socket, &old_irq)) {
 			pci_irq_status = yenta_probe_cb_irq(socket);
 			if (pci_irq_status == 1) {
-				dev_printk(KERN_INFO, &socket->dev->dev,
-				    "TI: parallel PCI interrupts, tied ok\n");
+				dev_info(&socket->dev->dev,
+					 "TI: parallel PCI interrupts, tied ok\n");
 				goto out;
 			}
 
@@ -644,8 +641,8 @@ static void ti12xx_irqroute_func1(struct yenta_socket *socket)
 out:
 	if (pci_irq_status < 1) {
 		socket->cb_irq = 0;
-		dev_printk(KERN_INFO, &socket->dev->dev,
-			   "TI: no PCI interrupts. Fish. Please report.\n");
+		dev_info(&socket->dev->dev,
+			 "TI: no PCI interrupts. Fish. Please report.\n");
 	}
 }
 
@@ -849,13 +846,12 @@ static int ti12xx_override(struct yenta_socket *socket)
 	/* make sure that memory burst is active */
 	val_orig = val = config_readl(socket, TI113X_SYSTEM_CONTROL);
 	if (disable_clkrun && PCI_FUNC(socket->dev->devfn) == 0) {
-		dev_printk(KERN_INFO, &socket->dev->dev,
-			   "Disabling CLKRUN feature\n");
+		dev_info(&socket->dev->dev, "Disabling CLKRUN feature\n");
 		val |= TI113X_SCR_KEEPCLK;
 	}
 	if (!(val & TI122X_SCR_MRBURSTUP)) {
-		dev_printk(KERN_INFO, &socket->dev->dev,
-			   "Enabling burst memory read transactions\n");
+		dev_info(&socket->dev->dev,
+			 "Enabling burst memory read transactions\n");
 		val |= TI122X_SCR_MRBURSTUP;
 	}
 	if (val_orig != val)
@@ -866,12 +862,10 @@ static int ti12xx_override(struct yenta_socket *socket)
 	 * CSC interrupts to PCI rather than INTVAL.
 	 */
 	val = config_readb(socket, TI1250_DIAGNOSTIC);
-	dev_printk(KERN_INFO, &socket->dev->dev,
-		   "Using %s to route CSC interrupts to PCI\n",
-		   (val & TI1250_DIAG_PCI_CSC) ? "CSCINT" : "INTVAL");
-	dev_printk(KERN_INFO, &socket->dev->dev,
-		   "Routing CardBus interrupts to %s\n",
-		   (val & TI1250_DIAG_PCI_IREQ) ? "PCI" : "ISA");
+	dev_info(&socket->dev->dev, "Using %s to route CSC interrupts to PCI\n",
+		 (val & TI1250_DIAG_PCI_CSC) ? "CSCINT" : "INTVAL");
+	dev_info(&socket->dev->dev, "Routing CardBus interrupts to %s\n",
+		 (val & TI1250_DIAG_PCI_IREQ) ? "PCI" : "ISA");
 
 	/* do irqrouting, depending on function */
 	if (PCI_FUNC(socket->dev->devfn) == 0)
@@ -896,9 +890,9 @@ static int ti1250_override(struct yenta_socket *socket)
 		diag |= TI1250_DIAG_PCI_CSC | TI1250_DIAG_PCI_IREQ;
 
 	if (diag != old) {
-		dev_printk(KERN_INFO, &socket->dev->dev,
-			   "adjusting diagnostic: %02x -> %02x\n",
-			   old, diag);
+		dev_info(&socket->dev->dev,
+			 "adjusting diagnostic: %02x -> %02x\n",
+			 old, diag);
 		config_writeb(socket, TI1250_DIAGNOSTIC, diag);
 	}
 
@@ -963,9 +957,9 @@ static void ene_tune_bridge(struct pcmcia_socket *sock, struct pci_bus *bus)
 		/* default to clear TLTEnable bit, old behaviour */
 		test_c9 &= ~ENE_TEST_C9_TLTENABLE;
 
-	dev_printk(KERN_INFO, &socket->dev->dev,
-		   "EnE: chaning testregister 0xC9, %02x -> %02x\n",
-		   old_c9, test_c9);
+	dev_info(&socket->dev->dev,
+		 "EnE: changing testregister 0xC9, %02x -> %02x\n",
+		 old_c9, test_c9);
 	config_writeb(socket, ENE_TEST_C9, test_c9);
 }
 
diff --git a/drivers/pcmcia/topic.h b/drivers/pcmcia/topic.h
index 615a45a8fe86..582688fe7505 100644
--- a/drivers/pcmcia/topic.h
+++ b/drivers/pcmcia/topic.h
@@ -104,6 +104,9 @@
 #define TOPIC_EXCA_IF_CONTROL		0x3e	/* 8 bit */
 #define TOPIC_EXCA_IFC_33V_ENA		0x01
 
+#define TOPIC_PCI_CFG_PPBCN		0x3e	/* 16-bit */
+#define TOPIC_PCI_CFG_PPBCN_WBEN	0x0400
+
 static void topic97_zoom_video(struct pcmcia_socket *sock, int onoff)
 {
 	struct yenta_socket *socket = container_of(sock, struct yenta_socket, socket);
@@ -138,6 +141,7 @@ static int topic97_override(struct yenta_socket *socket)
 static int topic95_override(struct yenta_socket *socket)
 {
 	u8 fctrl;
+	u16 ppbcn;
 
 	/* enable 3.3V support for 16bit cards */
 	fctrl = exca_readb(socket, TOPIC_EXCA_IF_CONTROL);
@@ -146,6 +150,18 @@ static int topic95_override(struct yenta_socket *socket)
 	/* tell yenta to use exca registers to power 16bit cards */
 	socket->flags |= YENTA_16BIT_POWER_EXCA | YENTA_16BIT_POWER_DF;
 
+	/* Disable write buffers to prevent lockups under load with numerous
+	   Cardbus cards, observed on Tecra 500CDT and reported elsewhere on the
+	   net.  This is not a power-on default according to the datasheet
+	   but some BIOSes seem to set it. */
+	if (pci_read_config_word(socket->dev, TOPIC_PCI_CFG_PPBCN, &ppbcn) == 0
+	    && socket->dev->revision <= 7
+	    && (ppbcn & TOPIC_PCI_CFG_PPBCN_WBEN)) {
+		ppbcn &= ~TOPIC_PCI_CFG_PPBCN_WBEN;
+		pci_write_config_word(socket->dev, TOPIC_PCI_CFG_PPBCN, ppbcn);
+		dev_info(&socket->dev->dev, "Disabled ToPIC95 Cardbus write buffers.\n");
+	}
+
 	return 0;
 }
 
diff --git a/drivers/pcmcia/vrc4171_card.c b/drivers/pcmcia/vrc4171_card.c
index 21973d55a055..1e5fa211fb70 100644
--- a/drivers/pcmcia/vrc4171_card.c
+++ b/drivers/pcmcia/vrc4171_card.c
@@ -84,32 +84,32 @@ MODULE_LICENSE("GPL");
 #define IO_MAX_MAPS	2
 #define MEM_MAX_MAPS	5
 
-typedef enum {
+enum vrc4171_slot {
 	SLOT_PROBE = 0,
 	SLOT_NOPROBE_IO,
 	SLOT_NOPROBE_MEM,
 	SLOT_NOPROBE_ALL,
 	SLOT_INITIALIZED,
-} vrc4171_slot_t;
+};
 
-typedef enum {
+enum vrc4171_slotb {
 	SLOTB_IS_NONE,
 	SLOTB_IS_PCCARD,
 	SLOTB_IS_CF,
 	SLOTB_IS_FLASHROM,
-} vrc4171_slotb_t;
+};
 
-typedef struct vrc4171_socket {
-	vrc4171_slot_t slot;
+struct vrc4171_socket {
+	enum vrc4171_slot slot;
 	struct pcmcia_socket pcmcia_socket;
 	char name[24];
 	int csc_irq;
 	int io_irq;
 	spinlock_t lock;
-} vrc4171_socket_t;
+};
 
-static vrc4171_socket_t vrc4171_sockets[CARD_MAX_SLOTS];
-static vrc4171_slotb_t vrc4171_slotb = SLOTB_IS_NONE;
+static struct vrc4171_socket vrc4171_sockets[CARD_MAX_SLOTS];
+static enum vrc4171_slotb vrc4171_slotb = SLOTB_IS_NONE;
 static char vrc4171_card_name[] = "NEC VRC4171 Card Controller";
 static unsigned int vrc4171_irq;
 static uint16_t vrc4171_irq_mask = 0xdeb8;
@@ -141,7 +141,7 @@ static inline uint16_t vrc4171_get_irq_status(void)
 	return inw(INTERRUPT_STATUS);
 }
 
-static inline void vrc4171_set_multifunction_pin(vrc4171_slotb_t config)
+static inline void vrc4171_set_multifunction_pin(enum vrc4171_slotb config)
 {
 	uint16_t config1;
 
@@ -234,7 +234,7 @@ static inline int search_nonuse_irq(void)
 
 static int pccard_init(struct pcmcia_socket *sock)
 {
-	vrc4171_socket_t *socket;
+	struct vrc4171_socket *socket;
 	unsigned int slot;
 
 	sock->features |= SS_CAP_PCCARD | SS_CAP_PAGE_REGS;
@@ -317,7 +317,7 @@ static inline uint8_t set_Vcc_value(u_char Vcc)
 
 static int pccard_set_socket(struct pcmcia_socket *sock, socket_state_t *state)
 {
-	vrc4171_socket_t *socket;
+	struct vrc4171_socket *socket;
 	unsigned int slot;
 	uint8_t voltage, power, control, cscint;
 
@@ -517,7 +517,7 @@ static inline unsigned int get_events(int slot)
 
 static irqreturn_t pccard_interrupt(int irq, void *dev_id)
 {
-	vrc4171_socket_t *socket;
+	struct vrc4171_socket *socket;
 	unsigned int events;
 	irqreturn_t retval = IRQ_NONE;
 	uint16_t status;
@@ -567,7 +567,7 @@ static inline void reserve_using_irq(int slot)
 
 static int vrc4171_add_sockets(void)
 {
-	vrc4171_socket_t *socket;
+	struct vrc4171_socket *socket;
 	int slot, retval;
 
 	for (slot = 0; slot < CARD_MAX_SLOTS; slot++) {
@@ -617,7 +617,7 @@ static int vrc4171_add_sockets(void)
 
 static void vrc4171_remove_sockets(void)
 {
-	vrc4171_socket_t *socket;
+	struct vrc4171_socket *socket;
 	int slot;
 
 	for (slot = 0; slot < CARD_MAX_SLOTS; slot++) {
diff --git a/drivers/pcmcia/xxs1500_ss.c b/drivers/pcmcia/xxs1500_ss.c
index 4c04360f378b..b2a189507fc3 100644
--- a/drivers/pcmcia/xxs1500_ss.c
+++ b/drivers/pcmcia/xxs1500_ss.c
@@ -11,6 +11,7 @@
 #include <linux/io.h>
 #include <linux/ioport.h>
 #include <linux/mm.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/pm.h>
 #include <linux/resource.h>
diff --git a/drivers/pcmcia/yenta_socket.c b/drivers/pcmcia/yenta_socket.c
index 965bd8491233..5d6d9b1549bc 100644
--- a/drivers/pcmcia/yenta_socket.c
+++ b/drivers/pcmcia/yenta_socket.c
@@ -712,10 +712,9 @@ static int yenta_allocate_res(struct yenta_socket *socket, int nr, unsigned type
 		pcibios_bus_to_resource(dev->bus, res, &region);
 		if (pci_claim_resource(dev, PCI_BRIDGE_RESOURCES + nr) == 0)
 			return 0;
-		dev_printk(KERN_INFO, &dev->dev,
-			   "Preassigned resource %d busy or not available, "
-			   "reconfiguring...\n",
-			   nr);
+		dev_info(&dev->dev,
+			 "Preassigned resource %d busy or not available, reconfiguring...\n",
+			 nr);
 	}
 
 	if (type & IORESOURCE_IO) {
@@ -738,9 +737,9 @@ static int yenta_allocate_res(struct yenta_socket *socket, int nr, unsigned type
 			return 1;
 	}
 
-	dev_printk(KERN_INFO, &dev->dev,
-		   "no resource of type %x available, trying to continue...\n",
-		   type);
+	dev_info(&dev->dev,
+		 "no resource of type %x available, trying to continue...\n",
+		 type);
 	res->start = res->end = res->flags = 0;
 	return 0;
 }
@@ -802,13 +801,13 @@ static void yenta_close(struct pci_dev *dev)
 	else
 		del_timer_sync(&sock->poll_timer);
 
-	if (sock->base)
-		iounmap(sock->base);
+	iounmap(sock->base);
 	yenta_free_resources(sock);
 
 	pci_release_regions(dev);
 	pci_disable_device(dev);
 	pci_set_drvdata(dev, NULL);
+	kfree(sock);
 }
 
 
@@ -979,8 +978,8 @@ static int yenta_probe_cb_irq(struct yenta_socket *socket)
 	socket->probe_status = 0;
 
 	if (request_irq(socket->cb_irq, yenta_probe_handler, IRQF_SHARED, "yenta", socket)) {
-		dev_printk(KERN_WARNING, &socket->dev->dev,
-			   "request_irq() in yenta_probe_cb_irq() failed!\n");
+		dev_warn(&socket->dev->dev,
+			 "request_irq() in yenta_probe_cb_irq() failed!\n");
 		return -1;
 	}
 
@@ -1019,9 +1018,8 @@ static void yenta_get_socket_capabilities(struct yenta_socket *socket, u32 isa_i
 	else
 		socket->socket.irq_mask = 0;
 
-	dev_printk(KERN_INFO, &socket->dev->dev,
-		   "ISA IRQ mask 0x%04x, PCI irq %d\n",
-		   socket->socket.irq_mask, socket->cb_irq);
+	dev_info(&socket->dev->dev, "ISA IRQ mask 0x%04x, PCI irq %d\n",
+		 socket->socket.irq_mask, socket->cb_irq);
 }
 
 /*
@@ -1111,9 +1109,9 @@ static void yenta_fixup_parent_bridge(struct pci_bus *cardbus_bridge)
 
 	/* Show that the wanted subordinate number is not possible: */
 	if (cardbus_bridge->busn_res.end > upper_limit)
-		dev_printk(KERN_WARNING, &cardbus_bridge->dev,
-			   "Upper limit for fixing this "
-			   "bridge's parent bridge: #%02x\n", upper_limit);
+		dev_warn(&cardbus_bridge->dev,
+			 "Upper limit for fixing this bridge's parent bridge: #%02x\n",
+			 upper_limit);
 
 	/* If we have room to increase the bridge's subordinate number, */
 	if (bridge_to_fix->busn_res.end < upper_limit) {
@@ -1122,11 +1120,11 @@ static void yenta_fixup_parent_bridge(struct pci_bus *cardbus_bridge)
 		unsigned char subordinate_to_assign =
 			min_t(int, cardbus_bridge->busn_res.end, upper_limit);
 
-		dev_printk(KERN_INFO, &bridge_to_fix->dev,
-			   "Raising subordinate bus# of parent "
-			   "bus (#%02x) from #%02x to #%02x\n",
-			   bridge_to_fix->number,
-			   (int)bridge_to_fix->busn_res.end, subordinate_to_assign);
+		dev_info(&bridge_to_fix->dev,
+			 "Raising subordinate bus# of parent bus (#%02x) from #%02x to #%02x\n",
+			 bridge_to_fix->number,
+			 (int)bridge_to_fix->busn_res.end,
+			 subordinate_to_assign);
 
 		/* Save the new subordinate in the bus struct of the bridge */
 		bridge_to_fix->busn_res.end = subordinate_to_assign;
@@ -1153,8 +1151,7 @@ static int yenta_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	 * Bail out if so.
 	 */
 	if (!dev->subordinate) {
-		dev_printk(KERN_ERR, &dev->dev, "no bus associated! "
-			   "(try 'pci=assign-busses')\n");
+		dev_err(&dev->dev, "no bus associated! (try 'pci=assign-busses')\n");
 		return -ENODEV;
 	}
 
@@ -1189,7 +1186,7 @@ static int yenta_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		goto disable;
 
 	if (!pci_resource_start(dev, 0)) {
-		dev_printk(KERN_ERR, &dev->dev, "No cardbus resource!\n");
+		dev_err(&dev->dev, "No cardbus resource!\n");
 		ret = -ENODEV;
 		goto release;
 	}
@@ -1208,8 +1205,8 @@ static int yenta_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	 * report the subsystem vendor and device for help debugging
 	 * the irq stuff...
 	 */
-	dev_printk(KERN_INFO, &dev->dev, "CardBus bridge found [%04x:%04x]\n",
-		   dev->subsystem_vendor, dev->subsystem_device);
+	dev_info(&dev->dev, "CardBus bridge found [%04x:%04x]\n",
+		 dev->subsystem_vendor, dev->subsystem_device);
 
 	yenta_config_init(socket);
 
@@ -1239,12 +1236,10 @@ static int yenta_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		setup_timer(&socket->poll_timer, yenta_interrupt_wrapper,
 			    (unsigned long)socket);
 		mod_timer(&socket->poll_timer, jiffies + HZ);
-		dev_printk(KERN_INFO, &dev->dev,
-			   "no PCI IRQ, CardBus support disabled for this "
-			   "socket.\n");
-		dev_printk(KERN_INFO, &dev->dev,
-			   "check your BIOS CardBus, BIOS IRQ or ACPI "
-			   "settings.\n");
+		dev_info(&dev->dev,
+			 "no PCI IRQ, CardBus support disabled for this socket.\n");
+		dev_info(&dev->dev,
+			 "check your BIOS CardBus, BIOS IRQ or ACPI settings.\n");
 	} else {
 		socket->socket.features |= SS_CAP_CARDBUS;
 	}
@@ -1252,32 +1247,41 @@ static int yenta_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	/* Figure out what the dang thing can do for the PCMCIA layer... */
 	yenta_interrogate(socket);
 	yenta_get_socket_capabilities(socket, isa_interrupts);
-	dev_printk(KERN_INFO, &dev->dev,
-		   "Socket status: %08x\n", cb_readl(socket, CB_SOCKET_STATE));
+	dev_info(&dev->dev, "Socket status: %08x\n",
+		 cb_readl(socket, CB_SOCKET_STATE));
 
 	yenta_fixup_parent_bridge(dev->subordinate);
 
 	/* Register it with the pcmcia layer.. */
 	ret = pcmcia_register_socket(&socket->socket);
-	if (ret == 0) {
-		/* Add the yenta register attributes */
-		ret = device_create_file(&dev->dev, &dev_attr_yenta_registers);
-		if (ret == 0)
-			goto out;
-
-		/* error path... */
-		pcmcia_unregister_socket(&socket->socket);
-	}
+	if (ret)
+		goto free_irq;
+
+	/* Add the yenta register attributes */
+	ret = device_create_file(&dev->dev, &dev_attr_yenta_registers);
+	if (ret)
+		goto unregister_socket;
 
+	return ret;
+
+	/* error path... */
+ unregister_socket:
+	pcmcia_unregister_socket(&socket->socket);
+ free_irq:
+	if (socket->cb_irq)
+		free_irq(socket->cb_irq, socket);
+	else
+		del_timer_sync(&socket->poll_timer);
  unmap:
 	iounmap(socket->base);
+	yenta_free_resources(socket);
  release:
 	pci_release_regions(dev);
  disable:
 	pci_disable_device(dev);
  free:
+	pci_set_drvdata(dev, NULL);
 	kfree(socket);
- out:
 	return ret;
 }
 
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index a53bd5b52df9..c0e6ede3e27d 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -38,7 +38,9 @@ config ARMADA375_USBCLUSTER_PHY
 config PHY_DM816X_USB
 	tristate "TI dm816x USB PHY driver"
 	depends on ARCH_OMAP2PLUS
+	depends on USB_SUPPORT
 	select GENERIC_PHY
+	select USB_PHY
 	help
 	  Enable this for dm816x USB to work.
 
@@ -52,6 +54,26 @@ config PHY_EXYNOS_MIPI_VIDEO
 	  Support for MIPI CSI-2 and MIPI DSI DPHY found on Samsung S5P
 	  and EXYNOS SoCs.
 
+config PHY_PXA_28NM_HSIC
+	tristate "Marvell USB HSIC 28nm PHY Driver"
+	select GENERIC_PHY
+	help
+	  Enable this to support Marvell USB HSIC PHY driver for Marvell
+	  SoC. This driver will do the PHY initialization and shutdown.
+	  The PHY driver will be used by Marvell ehci driver.
+
+	  To compile this driver as a module, choose M here.
+
+config PHY_PXA_28NM_USB2
+	tristate "Marvell USB 2.0 28nm PHY Driver"
+	select GENERIC_PHY
+	help
+	  Enable this to support Marvell USB 2.0 PHY driver for Marvell
+	  SoC. This driver will do the PHY initialization and shutdown.
+	  The PHY driver will be used by Marvell udc/ehci/otg driver.
+
+	  To compile this driver as a module, choose M here.
+
 config PHY_MVEBU_SATA
 	def_bool y
 	depends on ARCH_DOVE || MACH_DOVE || MACH_KIRKWOOD
@@ -97,8 +119,9 @@ config OMAP_CONTROL_PHY
 config OMAP_USB2
 	tristate "OMAP USB2 PHY Driver"
 	depends on ARCH_OMAP2PLUS
-	depends on USB_PHY
+	depends on USB_SUPPORT
 	select GENERIC_PHY
+	select USB_PHY
 	select OMAP_CONTROL_PHY
 	depends on OMAP_OCP2SCP
 	help
@@ -122,8 +145,9 @@ config TI_PIPE3
 config TWL4030_USB
 	tristate "TWL4030 USB Transceiver Driver"
 	depends on TWL4030_CORE && REGULATOR_TWL4030 && USB_MUSB_OMAP2PLUS
-	depends on USB_PHY
+	depends on USB_SUPPORT
 	select GENERIC_PHY
+	select USB_PHY
 	help
 	  Enable this to support the USB OTG transceiver on TWL4030
 	  family chips (including the TWL5030 and TPS659x0 devices).
@@ -243,6 +267,13 @@ config PHY_EXYNOS5_USBDRD
 	  This driver provides PHY interface for USB 3.0 DRD controller
 	  present on Exynos5 SoC series.
 
+config PHY_PISTACHIO_USB
+	tristate "IMG Pistachio USB2.0 PHY driver"
+	depends on MACH_PISTACHIO
+	select GENERIC_PHY
+	help
+	  Enable this to support the USB2.0 PHY on the IMG Pistachio SoC.
+
 config PHY_QCOM_APQ8064_SATA
 	tristate "Qualcomm APQ8064 SATA SerDes/PHY driver"
 	depends on ARCH_QCOM
@@ -304,9 +335,25 @@ config PHY_STIH41X_USB
 
 config PHY_QCOM_UFS
 	tristate "Qualcomm UFS PHY driver"
-	depends on OF && ARCH_MSM
+	depends on OF && ARCH_QCOM
 	select GENERIC_PHY
 	help
 	  Support for UFS PHY on QCOM chipsets.
 
+config PHY_TUSB1210
+	tristate "TI TUSB1210 ULPI PHY module"
+	depends on USB_ULPI_BUS
+	select GENERIC_PHY
+	help
+	  Support for TI TUSB1210 USB ULPI PHY.
+
+config PHY_BRCMSTB_SATA
+	tristate "Broadcom STB SATA PHY driver"
+	depends on ARCH_BRCMSTB
+	depends on OF
+	select GENERIC_PHY
+	help
+	  Enable this to support the SATA3 PHY on 28nm Broadcom STB SoCs.
+	  Likely useful only with CONFIG_SATA_BRCMSTB enabled.
+
 endmenu
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index f12625178780..f344e1b2e825 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -10,6 +10,8 @@ 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_PXA_28NM_USB2)		+= phy-pxa-28nm-usb2.o
+obj-$(CONFIG_PHY_PXA_28NM_HSIC)		+= phy-pxa-28nm-hsic.o
 obj-$(CONFIG_PHY_MVEBU_SATA)		+= phy-mvebu-sata.o
 obj-$(CONFIG_PHY_MIPHY28LP) 		+= phy-miphy28lp.o
 obj-$(CONFIG_PHY_MIPHY365X)		+= phy-miphy365x.o
@@ -40,3 +42,6 @@ obj-$(CONFIG_PHY_STIH41X_USB)		+= phy-stih41x-usb.o
 obj-$(CONFIG_PHY_QCOM_UFS) 	+= phy-qcom-ufs.o
 obj-$(CONFIG_PHY_QCOM_UFS) 	+= phy-qcom-ufs-qmp-20nm.o
 obj-$(CONFIG_PHY_QCOM_UFS) 	+= phy-qcom-ufs-qmp-14nm.o
+obj-$(CONFIG_PHY_TUSB1210)		+= phy-tusb1210.o
+obj-$(CONFIG_PHY_BRCMSTB_SATA)		+= phy-brcmstb-sata.o
+obj-$(CONFIG_PHY_PISTACHIO_USB)		+= phy-pistachio-usb.o
diff --git a/drivers/phy/phy-brcmstb-sata.c b/drivers/phy/phy-brcmstb-sata.c
new file mode 100644
index 000000000000..b7e303d28caf
--- /dev/null
+++ b/drivers/phy/phy-brcmstb-sata.c
@@ -0,0 +1,216 @@
+/*
+ * Broadcom SATA3 AHCI Controller PHY Driver
+ *
+ * Copyright © 2009-2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+
+#define SATA_MDIO_BANK_OFFSET				0x23c
+#define SATA_MDIO_REG_OFFSET(ofs)			((ofs) * 4)
+#define SATA_MDIO_REG_SPACE_SIZE			0x1000
+#define SATA_MDIO_REG_LENGTH				0x1f00
+
+#define MAX_PORTS					2
+
+/* Register offset between PHYs in PCB space */
+#define SATA_MDIO_REG_SPACE_SIZE			0x1000
+
+struct brcm_sata_port {
+	int portnum;
+	struct phy *phy;
+	struct brcm_sata_phy *phy_priv;
+	bool ssc_en;
+};
+
+struct brcm_sata_phy {
+	struct device *dev;
+	void __iomem *phy_base;
+
+	struct brcm_sata_port phys[MAX_PORTS];
+};
+
+enum sata_mdio_phy_regs_28nm {
+	PLL_REG_BANK_0				= 0x50,
+	PLL_REG_BANK_0_PLLCONTROL_0		= 0x81,
+
+	TXPMD_REG_BANK				= 0x1a0,
+	TXPMD_CONTROL1				= 0x81,
+	TXPMD_CONTROL1_TX_SSC_EN_FRC		= BIT(0),
+	TXPMD_CONTROL1_TX_SSC_EN_FRC_VAL	= BIT(1),
+	TXPMD_TX_FREQ_CTRL_CONTROL1		= 0x82,
+	TXPMD_TX_FREQ_CTRL_CONTROL2		= 0x83,
+	TXPMD_TX_FREQ_CTRL_CONTROL2_FMIN_MASK	= 0x3ff,
+	TXPMD_TX_FREQ_CTRL_CONTROL3		= 0x84,
+	TXPMD_TX_FREQ_CTRL_CONTROL3_FMAX_MASK	= 0x3ff,
+};
+
+static inline void __iomem *brcm_sata_phy_base(struct brcm_sata_port *port)
+{
+	struct brcm_sata_phy *priv = port->phy_priv;
+
+	return priv->phy_base + (port->portnum * SATA_MDIO_REG_SPACE_SIZE);
+}
+
+static void brcm_sata_mdio_wr(void __iomem *addr, u32 bank, u32 ofs,
+			      u32 msk, u32 value)
+{
+	u32 tmp;
+
+	writel(bank, addr + SATA_MDIO_BANK_OFFSET);
+	tmp = readl(addr + SATA_MDIO_REG_OFFSET(ofs));
+	tmp = (tmp & msk) | value;
+	writel(tmp, addr + SATA_MDIO_REG_OFFSET(ofs));
+}
+
+/* These defaults were characterized by H/W group */
+#define FMIN_VAL_DEFAULT	0x3df
+#define FMAX_VAL_DEFAULT	0x3df
+#define FMAX_VAL_SSC		0x83
+
+static void brcm_sata_cfg_ssc_28nm(struct brcm_sata_port *port)
+{
+	void __iomem *base = brcm_sata_phy_base(port);
+	struct brcm_sata_phy *priv = port->phy_priv;
+	u32 tmp;
+
+	/* override the TX spread spectrum setting */
+	tmp = TXPMD_CONTROL1_TX_SSC_EN_FRC_VAL | TXPMD_CONTROL1_TX_SSC_EN_FRC;
+	brcm_sata_mdio_wr(base, TXPMD_REG_BANK, TXPMD_CONTROL1, ~tmp, tmp);
+
+	/* set fixed min freq */
+	brcm_sata_mdio_wr(base, TXPMD_REG_BANK, TXPMD_TX_FREQ_CTRL_CONTROL2,
+			  ~TXPMD_TX_FREQ_CTRL_CONTROL2_FMIN_MASK,
+			  FMIN_VAL_DEFAULT);
+
+	/* set fixed max freq depending on SSC config */
+	if (port->ssc_en) {
+		dev_info(priv->dev, "enabling SSC on port %d\n", port->portnum);
+		tmp = FMAX_VAL_SSC;
+	} else {
+		tmp = FMAX_VAL_DEFAULT;
+	}
+
+	brcm_sata_mdio_wr(base, TXPMD_REG_BANK, TXPMD_TX_FREQ_CTRL_CONTROL3,
+			  ~TXPMD_TX_FREQ_CTRL_CONTROL3_FMAX_MASK, tmp);
+}
+
+static int brcm_sata_phy_init(struct phy *phy)
+{
+	struct brcm_sata_port *port = phy_get_drvdata(phy);
+
+	brcm_sata_cfg_ssc_28nm(port);
+
+	return 0;
+}
+
+static struct phy_ops phy_ops_28nm = {
+	.init		= brcm_sata_phy_init,
+	.owner		= THIS_MODULE,
+};
+
+static const struct of_device_id brcm_sata_phy_of_match[] = {
+	{ .compatible	= "brcm,bcm7445-sata-phy" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, brcm_sata_phy_of_match);
+
+static int brcm_sata_phy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *dn = dev->of_node, *child;
+	struct brcm_sata_phy *priv;
+	struct resource *res;
+	struct phy_provider *provider;
+	int count = 0;
+
+	if (of_get_child_count(dn) == 0)
+		return -ENODEV;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+	dev_set_drvdata(dev, priv);
+	priv->dev = dev;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phy");
+	priv->phy_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->phy_base))
+		return PTR_ERR(priv->phy_base);
+
+	for_each_available_child_of_node(dn, child) {
+		unsigned int id;
+		struct brcm_sata_port *port;
+
+		if (of_property_read_u32(child, "reg", &id)) {
+			dev_err(dev, "missing reg property in node %s\n",
+					child->name);
+			return -EINVAL;
+		}
+
+		if (id >= MAX_PORTS) {
+			dev_err(dev, "invalid reg: %u\n", id);
+			return -EINVAL;
+		}
+		if (priv->phys[id].phy) {
+			dev_err(dev, "already registered port %u\n", id);
+			return -EINVAL;
+		}
+
+		port = &priv->phys[id];
+		port->portnum = id;
+		port->phy_priv = priv;
+		port->phy = devm_phy_create(dev, child, &phy_ops_28nm);
+		port->ssc_en = of_property_read_bool(child, "brcm,enable-ssc");
+		if (IS_ERR(port->phy)) {
+			dev_err(dev, "failed to create PHY\n");
+			return PTR_ERR(port->phy);
+		}
+
+		phy_set_drvdata(port->phy, port);
+		count++;
+	}
+
+	provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	if (IS_ERR(provider)) {
+		dev_err(dev, "could not register PHY provider\n");
+		return PTR_ERR(provider);
+	}
+
+	dev_info(dev, "registered %d port(s)\n", count);
+
+	return 0;
+}
+
+static struct platform_driver brcm_sata_phy_driver = {
+	.probe	= brcm_sata_phy_probe,
+	.driver	= {
+		.of_match_table	= brcm_sata_phy_of_match,
+		.name		= "brcmstb-sata-phy",
+	}
+};
+module_platform_driver(brcm_sata_phy_driver);
+
+MODULE_DESCRIPTION("Broadcom STB SATA PHY driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Marc Carino");
+MODULE_AUTHOR("Brian Norris");
+MODULE_ALIAS("platform:phy-brcmstb-sata");
diff --git a/drivers/phy/phy-core.c b/drivers/phy/phy-core.c
index 3791838f4bd4..fc48fac003a6 100644
--- a/drivers/phy/phy-core.c
+++ b/drivers/phy/phy-core.c
@@ -367,13 +367,21 @@ static struct phy *_of_phy_get(struct device_node *np, int index)
 	phy_provider = of_phy_provider_lookup(args.np);
 	if (IS_ERR(phy_provider) || !try_module_get(phy_provider->owner)) {
 		phy = ERR_PTR(-EPROBE_DEFER);
-		goto err0;
+		goto out_unlock;
+	}
+
+	if (!of_device_is_available(args.np)) {
+		dev_warn(phy_provider->dev, "Requested PHY is disabled\n");
+		phy = ERR_PTR(-ENODEV);
+		goto out_put_module;
 	}
 
 	phy = phy_provider->of_xlate(phy_provider->dev, &args);
+
+out_put_module:
 	module_put(phy_provider->owner);
 
-err0:
+out_unlock:
 	mutex_unlock(&phy_provider_mutex);
 	of_node_put(args.np);
 
@@ -530,7 +538,7 @@ struct phy *phy_optional_get(struct device *dev, const char *string)
 {
 	struct phy *phy = phy_get(dev, string);
 
-	if (PTR_ERR(phy) == -ENODEV)
+	if (IS_ERR(phy) && (PTR_ERR(phy) == -ENODEV))
 		phy = NULL;
 
 	return phy;
@@ -584,7 +592,7 @@ struct phy *devm_phy_optional_get(struct device *dev, const char *string)
 {
 	struct phy *phy = devm_phy_get(dev, string);
 
-	if (PTR_ERR(phy) == -ENODEV)
+	if (IS_ERR(phy) && (PTR_ERR(phy) == -ENODEV))
 		phy = NULL;
 
 	return phy;
@@ -623,6 +631,38 @@ struct phy *devm_of_phy_get(struct device *dev, struct device_node *np,
 EXPORT_SYMBOL_GPL(devm_of_phy_get);
 
 /**
+ * devm_of_phy_get_by_index() - lookup and obtain a reference to a phy by index.
+ * @dev: device that requests this phy
+ * @np: node containing the phy
+ * @index: index of the phy
+ *
+ * Gets the phy using _of_phy_get(), and associates a device with it using
+ * devres. On driver detach, release function is invoked on the devres data,
+ * then, devres data is freed.
+ *
+ */
+struct phy *devm_of_phy_get_by_index(struct device *dev, struct device_node *np,
+				     int index)
+{
+	struct phy **ptr, *phy;
+
+	ptr = devres_alloc(devm_phy_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	phy = _of_phy_get(np, index);
+	if (!IS_ERR(phy)) {
+		*ptr = phy;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return phy;
+}
+EXPORT_SYMBOL_GPL(devm_of_phy_get_by_index);
+
+/**
  * phy_create() - create a new phy
  * @dev: device that is creating the new phy
  * @node: device node of the phy
@@ -651,16 +691,6 @@ struct phy *phy_create(struct device *dev, struct device_node *node,
 		goto free_phy;
 	}
 
-	/* phy-supply */
-	phy->pwr = regulator_get_optional(dev, "phy");
-	if (IS_ERR(phy->pwr)) {
-		if (PTR_ERR(phy->pwr) == -EPROBE_DEFER) {
-			ret = -EPROBE_DEFER;
-			goto free_ida;
-		}
-		phy->pwr = NULL;
-	}
-
 	device_initialize(&phy->dev);
 	mutex_init(&phy->mutex);
 
@@ -674,6 +704,16 @@ struct phy *phy_create(struct device *dev, struct device_node *node,
 	if (ret)
 		goto put_dev;
 
+	/* phy-supply */
+	phy->pwr = regulator_get_optional(&phy->dev, "phy");
+	if (IS_ERR(phy->pwr)) {
+		ret = PTR_ERR(phy->pwr);
+		if (ret == -EPROBE_DEFER)
+			goto put_dev;
+
+		phy->pwr = NULL;
+	}
+
 	ret = device_add(&phy->dev);
 	if (ret)
 		goto put_dev;
@@ -689,9 +729,6 @@ put_dev:
 	put_device(&phy->dev);  /* calls phy_release() which frees resources */
 	return ERR_PTR(ret);
 
-free_ida:
-	ida_simple_remove(&phy_ida, phy->id);
-
 free_phy:
 	kfree(phy);
 	return ERR_PTR(ret);
diff --git a/drivers/phy/phy-miphy28lp.c b/drivers/phy/phy-miphy28lp.c
index c4cc11dcb2a2..5e257ef7ac05 100644
--- a/drivers/phy/phy-miphy28lp.c
+++ b/drivers/phy/phy-miphy28lp.c
@@ -367,7 +367,7 @@ static struct miphy28lp_pll_gen pcie_pll_gen[] = {
 
 static inline void miphy28lp_set_reset(struct miphy28lp_phy *miphy_phy)
 {
-	void *base = miphy_phy->base;
+	void __iomem *base = miphy_phy->base;
 	u8 val;
 
 	/* Putting Macro in reset */
@@ -391,7 +391,7 @@ static inline void miphy28lp_set_reset(struct miphy28lp_phy *miphy_phy)
 static inline void miphy28lp_pll_calibration(struct miphy28lp_phy *miphy_phy,
 		struct pll_ratio *pll_ratio)
 {
-	void *base = miphy_phy->base;
+	void __iomem *base = miphy_phy->base;
 	u8 val;
 
 	/* Applying PLL Settings */
@@ -1107,11 +1107,6 @@ static struct phy *miphy28lp_xlate(struct device *dev,
 	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);
diff --git a/drivers/phy/phy-miphy365x.c b/drivers/phy/phy-miphy365x.c
index 019c2d75344e..0ff354d6e183 100644
--- a/drivers/phy/phy-miphy365x.c
+++ b/drivers/phy/phy-miphy365x.c
@@ -441,8 +441,8 @@ static int miphy365x_init(struct phy *phy)
 	return ret;
 }
 
-int miphy365x_get_addr(struct device *dev, struct miphy365x_phy *miphy_phy,
-		       int index)
+static int miphy365x_get_addr(struct device *dev,
+		struct miphy365x_phy *miphy_phy, int index)
 {
 	struct device_node *phynode = miphy_phy->phy->dev.of_node;
 	const char *name;
@@ -476,11 +476,6 @@ static struct phy *miphy365x_xlate(struct device *dev,
 	struct device_node *phynode = args->np;
 	int ret, index;
 
-	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);
diff --git a/drivers/phy/phy-omap-usb2.c b/drivers/phy/phy-omap-usb2.c
index 183ef4368101..c1a468686bdc 100644
--- a/drivers/phy/phy-omap-usb2.c
+++ b/drivers/phy/phy-omap-usb2.c
@@ -275,6 +275,7 @@ static int omap_usb2_probe(struct platform_device *pdev)
 		phy->wkupclk = devm_clk_get(phy->dev, "usb_phy_cm_clk32k");
 		if (IS_ERR(phy->wkupclk)) {
 			dev_err(&pdev->dev, "unable to get usb_phy_cm_clk32k\n");
+			pm_runtime_disable(phy->dev);
 			return PTR_ERR(phy->wkupclk);
 		} else {
 			dev_warn(&pdev->dev,
diff --git a/drivers/phy/phy-pistachio-usb.c b/drivers/phy/phy-pistachio-usb.c
new file mode 100644
index 000000000000..c6db35e6bb63
--- /dev/null
+++ b/drivers/phy/phy-pistachio-usb.c
@@ -0,0 +1,206 @@
+/*
+ * IMG Pistachio USB PHY driver
+ *
+ * Copyright (C) 2015 Google, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include <dt-bindings/phy/phy-pistachio-usb.h>
+
+#define USB_PHY_CONTROL1				0x04
+#define USB_PHY_CONTROL1_FSEL_SHIFT			2
+#define USB_PHY_CONTROL1_FSEL_MASK			0x7
+
+#define USB_PHY_STRAP_CONTROL				0x10
+#define USB_PHY_STRAP_CONTROL_REFCLK_SHIFT		4
+#define USB_PHY_STRAP_CONTROL_REFCLK_MASK		0x3
+
+#define USB_PHY_STATUS					0x14
+#define USB_PHY_STATUS_RX_PHY_CLK			BIT(9)
+#define USB_PHY_STATUS_RX_UTMI_CLK			BIT(8)
+#define USB_PHY_STATUS_VBUS_FAULT			BIT(7)
+
+struct pistachio_usb_phy {
+	struct device *dev;
+	struct regmap *cr_top;
+	struct clk *phy_clk;
+	unsigned int refclk;
+};
+
+static const unsigned long fsel_rate_map[] = {
+	9600000,
+	10000000,
+	12000000,
+	19200000,
+	20000000,
+	24000000,
+	0,
+	50000000,
+};
+
+static int pistachio_usb_phy_power_on(struct phy *phy)
+{
+	struct pistachio_usb_phy *p_phy = phy_get_drvdata(phy);
+	unsigned long timeout, rate;
+	unsigned int i;
+	int ret;
+
+	ret = clk_prepare_enable(p_phy->phy_clk);
+	if (ret < 0) {
+		dev_err(p_phy->dev, "Failed to enable PHY clock: %d\n", ret);
+		return ret;
+	}
+
+	regmap_update_bits(p_phy->cr_top, USB_PHY_STRAP_CONTROL,
+			   USB_PHY_STRAP_CONTROL_REFCLK_MASK <<
+			   USB_PHY_STRAP_CONTROL_REFCLK_SHIFT,
+			   p_phy->refclk << USB_PHY_STRAP_CONTROL_REFCLK_SHIFT);
+
+	rate = clk_get_rate(p_phy->phy_clk);
+	if (p_phy->refclk == REFCLK_XO_CRYSTAL && rate != 12000000) {
+		dev_err(p_phy->dev, "Unsupported rate for XO crystal: %ld\n",
+			rate);
+		ret = -EINVAL;
+		goto disable_clk;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(fsel_rate_map); i++) {
+		if (rate == fsel_rate_map[i])
+			break;
+	}
+	if (i == ARRAY_SIZE(fsel_rate_map)) {
+		dev_err(p_phy->dev, "Unsupported clock rate: %lu\n", rate);
+		ret = -EINVAL;
+		goto disable_clk;
+	}
+
+	regmap_update_bits(p_phy->cr_top, USB_PHY_CONTROL1,
+			   USB_PHY_CONTROL1_FSEL_MASK <<
+			   USB_PHY_CONTROL1_FSEL_SHIFT,
+			   i << USB_PHY_CONTROL1_FSEL_SHIFT);
+
+	timeout = jiffies + msecs_to_jiffies(200);
+	while (time_before(jiffies, timeout)) {
+		unsigned int val;
+
+		regmap_read(p_phy->cr_top, USB_PHY_STATUS, &val);
+		if (val & USB_PHY_STATUS_VBUS_FAULT) {
+			dev_err(p_phy->dev, "VBUS fault detected\n");
+			ret = -EIO;
+			goto disable_clk;
+		}
+		if ((val & USB_PHY_STATUS_RX_PHY_CLK) &&
+		    (val & USB_PHY_STATUS_RX_UTMI_CLK))
+			return 0;
+		usleep_range(1000, 1500);
+	}
+
+	dev_err(p_phy->dev, "Timed out waiting for PHY to power on\n");
+	ret = -ETIMEDOUT;
+
+disable_clk:
+	clk_disable_unprepare(p_phy->phy_clk);
+	return ret;
+}
+
+static int pistachio_usb_phy_power_off(struct phy *phy)
+{
+	struct pistachio_usb_phy *p_phy = phy_get_drvdata(phy);
+
+	clk_disable_unprepare(p_phy->phy_clk);
+
+	return 0;
+}
+
+static const struct phy_ops pistachio_usb_phy_ops = {
+	.power_on = pistachio_usb_phy_power_on,
+	.power_off = pistachio_usb_phy_power_off,
+	.owner = THIS_MODULE,
+};
+
+static int pistachio_usb_phy_probe(struct platform_device *pdev)
+{
+	struct pistachio_usb_phy *p_phy;
+	struct phy_provider *provider;
+	struct phy *phy;
+	int ret;
+
+	p_phy = devm_kzalloc(&pdev->dev, sizeof(*p_phy), GFP_KERNEL);
+	if (!p_phy)
+		return -ENOMEM;
+	p_phy->dev = &pdev->dev;
+	platform_set_drvdata(pdev, p_phy);
+
+	p_phy->cr_top = syscon_regmap_lookup_by_phandle(p_phy->dev->of_node,
+							"img,cr-top");
+	if (IS_ERR(p_phy->cr_top)) {
+		dev_err(p_phy->dev, "Failed to get CR_TOP registers: %ld\n",
+			PTR_ERR(p_phy->cr_top));
+		return PTR_ERR(p_phy->cr_top);
+	}
+
+	p_phy->phy_clk = devm_clk_get(p_phy->dev, "usb_phy");
+	if (IS_ERR(p_phy->phy_clk)) {
+		dev_err(p_phy->dev, "Failed to get usb_phy clock: %ld\n",
+			PTR_ERR(p_phy->phy_clk));
+		return PTR_ERR(p_phy->phy_clk);
+	}
+
+	ret = of_property_read_u32(p_phy->dev->of_node, "img,refclk",
+				   &p_phy->refclk);
+	if (ret < 0) {
+		dev_err(p_phy->dev, "No reference clock selector specified\n");
+		return ret;
+	}
+
+	phy = devm_phy_create(p_phy->dev, NULL, &pistachio_usb_phy_ops);
+	if (IS_ERR(phy)) {
+		dev_err(p_phy->dev, "Failed to create PHY: %ld\n",
+			PTR_ERR(phy));
+		return PTR_ERR(phy);
+	}
+	phy_set_drvdata(phy, p_phy);
+
+	provider = devm_of_phy_provider_register(p_phy->dev,
+						 of_phy_simple_xlate);
+	if (IS_ERR(provider)) {
+		dev_err(p_phy->dev, "Failed to register PHY provider: %ld\n",
+			PTR_ERR(provider));
+		return PTR_ERR(provider);
+	}
+
+	return 0;
+}
+
+static const struct of_device_id pistachio_usb_phy_of_match[] = {
+	{ .compatible = "img,pistachio-usb-phy", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, pistachio_usb_phy_of_match);
+
+static struct platform_driver pistachio_usb_phy_driver = {
+	.probe		= pistachio_usb_phy_probe,
+	.driver		= {
+		.name	= "pistachio-usb-phy",
+		.of_match_table = pistachio_usb_phy_of_match,
+	},
+};
+module_platform_driver(pistachio_usb_phy_driver);
+
+MODULE_AUTHOR("Andrew Bresticker <abrestic@chromium.org>");
+MODULE_DESCRIPTION("IMG Pistachio USB2.0 PHY driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-pxa-28nm-hsic.c b/drivers/phy/phy-pxa-28nm-hsic.c
new file mode 100644
index 000000000000..234aacf4db20
--- /dev/null
+++ b/drivers/phy/phy-pxa-28nm-hsic.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright (C) 2015 Linaro, Ltd.
+ * Rob Herring <robh@kernel.org>
+ *
+ * Based on vendor driver:
+ * Copyright (C) 2013 Marvell Inc.
+ * Author: Chao Xie <xiechao.mail@gmail.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/phy/phy.h>
+
+#define PHY_28NM_HSIC_CTRL			0x08
+#define PHY_28NM_HSIC_IMPCAL_CAL		0x18
+#define PHY_28NM_HSIC_PLL_CTRL01		0x1c
+#define PHY_28NM_HSIC_PLL_CTRL2			0x20
+#define PHY_28NM_HSIC_INT			0x28
+
+#define PHY_28NM_HSIC_PLL_SELLPFR_SHIFT		26
+#define PHY_28NM_HSIC_PLL_FBDIV_SHIFT		0
+#define PHY_28NM_HSIC_PLL_REFDIV_SHIFT		9
+
+#define PHY_28NM_HSIC_S2H_PU_PLL		BIT(10)
+#define PHY_28NM_HSIC_H2S_PLL_LOCK		BIT(15)
+#define PHY_28NM_HSIC_S2H_HSIC_EN		BIT(7)
+#define S2H_DRV_SE0_4RESUME			BIT(14)
+#define PHY_28NM_HSIC_H2S_IMPCAL_DONE		BIT(27)
+
+#define PHY_28NM_HSIC_CONNECT_INT		BIT(1)
+#define PHY_28NM_HSIC_HS_READY_INT		BIT(2)
+
+struct mv_hsic_phy {
+	struct phy		*phy;
+	struct platform_device	*pdev;
+	void __iomem		*base;
+	struct clk		*clk;
+};
+
+static bool wait_for_reg(void __iomem *reg, u32 mask, unsigned long timeout)
+{
+	timeout += jiffies;
+	while (time_is_after_eq_jiffies(timeout)) {
+		if ((readl(reg) & mask) == mask)
+			return true;
+		msleep(1);
+	}
+	return false;
+}
+
+static int mv_hsic_phy_init(struct phy *phy)
+{
+	struct mv_hsic_phy *mv_phy = phy_get_drvdata(phy);
+	struct platform_device *pdev = mv_phy->pdev;
+	void __iomem *base = mv_phy->base;
+
+	clk_prepare_enable(mv_phy->clk);
+
+	/* Set reference clock */
+	writel(0x1 << PHY_28NM_HSIC_PLL_SELLPFR_SHIFT |
+		0xf0 << PHY_28NM_HSIC_PLL_FBDIV_SHIFT |
+		0xd << PHY_28NM_HSIC_PLL_REFDIV_SHIFT,
+		base + PHY_28NM_HSIC_PLL_CTRL01);
+
+	/* Turn on PLL */
+	writel(readl(base + PHY_28NM_HSIC_PLL_CTRL2) |
+		PHY_28NM_HSIC_S2H_PU_PLL,
+		base + PHY_28NM_HSIC_PLL_CTRL2);
+
+	/* Make sure PHY PLL is locked */
+	if (!wait_for_reg(base + PHY_28NM_HSIC_PLL_CTRL2,
+	    PHY_28NM_HSIC_H2S_PLL_LOCK, HZ / 10)) {
+		dev_err(&pdev->dev, "HSIC PHY PLL not locked after 100mS.");
+		clk_disable_unprepare(mv_phy->clk);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int mv_hsic_phy_power_on(struct phy *phy)
+{
+	struct mv_hsic_phy *mv_phy = phy_get_drvdata(phy);
+	struct platform_device *pdev = mv_phy->pdev;
+	void __iomem *base = mv_phy->base;
+	u32 reg;
+
+	reg = readl(base + PHY_28NM_HSIC_CTRL);
+	/* Avoid SE0 state when resume for some device will take it as reset */
+	reg &= ~S2H_DRV_SE0_4RESUME;
+	reg |= PHY_28NM_HSIC_S2H_HSIC_EN;	/* Enable HSIC PHY */
+	writel(reg, base + PHY_28NM_HSIC_CTRL);
+
+	/*
+	 *  Calibration Timing
+	 *		   ____________________________
+	 *  CAL START   ___|
+	 *			   ____________________
+	 *  CAL_DONE    ___________|
+	 *		   | 400us |
+	 */
+
+	/* Make sure PHY Calibration is ready */
+	if (!wait_for_reg(base + PHY_28NM_HSIC_IMPCAL_CAL,
+	    PHY_28NM_HSIC_H2S_IMPCAL_DONE, HZ / 10)) {
+		dev_warn(&pdev->dev, "HSIC PHY READY not set after 100mS.");
+		return -ETIMEDOUT;
+	}
+
+	/* Waiting for HSIC connect int*/
+	if (!wait_for_reg(base + PHY_28NM_HSIC_INT,
+	    PHY_28NM_HSIC_CONNECT_INT, HZ / 5)) {
+		dev_warn(&pdev->dev, "HSIC wait for connect interrupt timeout.");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int mv_hsic_phy_power_off(struct phy *phy)
+{
+	struct mv_hsic_phy *mv_phy = phy_get_drvdata(phy);
+	void __iomem *base = mv_phy->base;
+
+	writel(readl(base + PHY_28NM_HSIC_CTRL) & ~PHY_28NM_HSIC_S2H_HSIC_EN,
+		base + PHY_28NM_HSIC_CTRL);
+
+	return 0;
+}
+
+static int mv_hsic_phy_exit(struct phy *phy)
+{
+	struct mv_hsic_phy *mv_phy = phy_get_drvdata(phy);
+	void __iomem *base = mv_phy->base;
+
+	/* Turn off PLL */
+	writel(readl(base + PHY_28NM_HSIC_PLL_CTRL2) &
+		~PHY_28NM_HSIC_S2H_PU_PLL,
+		base + PHY_28NM_HSIC_PLL_CTRL2);
+
+	clk_disable_unprepare(mv_phy->clk);
+	return 0;
+}
+
+
+static const struct phy_ops hsic_ops = {
+	.init		= mv_hsic_phy_init,
+	.power_on	= mv_hsic_phy_power_on,
+	.power_off	= mv_hsic_phy_power_off,
+	.exit		= mv_hsic_phy_exit,
+	.owner		= THIS_MODULE,
+};
+
+static int mv_hsic_phy_probe(struct platform_device *pdev)
+{
+	struct phy_provider *phy_provider;
+	struct mv_hsic_phy *mv_phy;
+	struct resource *r;
+
+	mv_phy = devm_kzalloc(&pdev->dev, sizeof(*mv_phy), GFP_KERNEL);
+	if (!mv_phy)
+		return -ENOMEM;
+
+	mv_phy->pdev = pdev;
+
+	mv_phy->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(mv_phy->clk)) {
+		dev_err(&pdev->dev, "failed to get clock.\n");
+		return PTR_ERR(mv_phy->clk);
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mv_phy->base = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(mv_phy->base))
+		return PTR_ERR(mv_phy->base);
+
+	mv_phy->phy = devm_phy_create(&pdev->dev, pdev->dev.of_node, &hsic_ops);
+	if (IS_ERR(mv_phy->phy))
+		return PTR_ERR(mv_phy->phy);
+
+	phy_set_drvdata(mv_phy->phy, mv_phy);
+
+	phy_provider = devm_of_phy_provider_register(&pdev->dev, of_phy_simple_xlate);
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static const struct of_device_id mv_hsic_phy_dt_match[] = {
+	{ .compatible = "marvell,pxa1928-hsic-phy", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mv_hsic_phy_dt_match);
+
+static struct platform_driver mv_hsic_phy_driver = {
+	.probe	= mv_hsic_phy_probe,
+	.driver = {
+		.name   = "mv-hsic-phy",
+		.of_match_table = of_match_ptr(mv_hsic_phy_dt_match),
+	},
+};
+module_platform_driver(mv_hsic_phy_driver);
+
+MODULE_AUTHOR("Rob Herring <robh@kernel.org>");
+MODULE_DESCRIPTION("Marvell HSIC phy driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-pxa-28nm-usb2.c b/drivers/phy/phy-pxa-28nm-usb2.c
new file mode 100644
index 000000000000..37e9c8ca4983
--- /dev/null
+++ b/drivers/phy/phy-pxa-28nm-usb2.c
@@ -0,0 +1,355 @@
+/*
+ * Copyright (C) 2015 Linaro, Ltd.
+ * Rob Herring <robh@kernel.org>
+ *
+ * Based on vendor driver:
+ * Copyright (C) 2013 Marvell Inc.
+ * Author: Chao Xie <xiechao.mail@gmail.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/phy/phy.h>
+
+/* USB PXA1928 PHY mapping */
+#define PHY_28NM_PLL_REG0			0x0
+#define PHY_28NM_PLL_REG1			0x4
+#define PHY_28NM_CAL_REG			0x8
+#define PHY_28NM_TX_REG0			0x0c
+#define PHY_28NM_TX_REG1			0x10
+#define PHY_28NM_RX_REG0			0x14
+#define PHY_28NM_RX_REG1			0x18
+#define PHY_28NM_DIG_REG0			0x1c
+#define PHY_28NM_DIG_REG1			0x20
+#define PHY_28NM_TEST_REG0			0x24
+#define PHY_28NM_TEST_REG1			0x28
+#define PHY_28NM_MOC_REG			0x2c
+#define PHY_28NM_PHY_RESERVE			0x30
+#define PHY_28NM_OTG_REG			0x34
+#define PHY_28NM_CHRG_DET			0x38
+#define PHY_28NM_CTRL_REG0			0xc4
+#define PHY_28NM_CTRL_REG1			0xc8
+#define PHY_28NM_CTRL_REG2			0xd4
+#define PHY_28NM_CTRL_REG3			0xdc
+
+/* PHY_28NM_PLL_REG0 */
+#define PHY_28NM_PLL_READY			BIT(31)
+
+#define PHY_28NM_PLL_SELLPFR_SHIFT		28
+#define PHY_28NM_PLL_SELLPFR_MASK		(0x3 << 28)
+
+#define PHY_28NM_PLL_FBDIV_SHIFT		16
+#define PHY_28NM_PLL_FBDIV_MASK			(0x1ff << 16)
+
+#define PHY_28NM_PLL_ICP_SHIFT			8
+#define PHY_28NM_PLL_ICP_MASK			(0x7 << 8)
+
+#define PHY_28NM_PLL_REFDIV_SHIFT		0
+#define PHY_28NM_PLL_REFDIV_MASK		0x7f
+
+/* PHY_28NM_PLL_REG1 */
+#define PHY_28NM_PLL_PU_BY_REG			BIT(1)
+
+#define PHY_28NM_PLL_PU_PLL			BIT(0)
+
+/* PHY_28NM_CAL_REG */
+#define PHY_28NM_PLL_PLLCAL_DONE		BIT(31)
+
+#define PHY_28NM_PLL_IMPCAL_DONE		BIT(23)
+
+#define PHY_28NM_PLL_KVCO_SHIFT			16
+#define PHY_28NM_PLL_KVCO_MASK			(0x7 << 16)
+
+#define PHY_28NM_PLL_CAL12_SHIFT		20
+#define PHY_28NM_PLL_CAL12_MASK			(0x3 << 20)
+
+#define PHY_28NM_IMPCAL_VTH_SHIFT		8
+#define PHY_28NM_IMPCAL_VTH_MASK		(0x7 << 8)
+
+#define PHY_28NM_PLLCAL_START_SHIFT		22
+#define PHY_28NM_IMPCAL_START_SHIFT		13
+
+/* PHY_28NM_TX_REG0 */
+#define PHY_28NM_TX_PU_BY_REG			BIT(25)
+
+#define PHY_28NM_TX_PU_ANA			BIT(24)
+
+#define PHY_28NM_TX_AMP_SHIFT			20
+#define PHY_28NM_TX_AMP_MASK			(0x7 << 20)
+
+/* PHY_28NM_RX_REG0 */
+#define PHY_28NM_RX_SQ_THRESH_SHIFT		0
+#define PHY_28NM_RX_SQ_THRESH_MASK		(0xf << 0)
+
+/* PHY_28NM_RX_REG1 */
+#define PHY_28NM_RX_SQCAL_DONE			BIT(31)
+
+/* PHY_28NM_DIG_REG0 */
+#define PHY_28NM_DIG_BITSTAFFING_ERR		BIT(31)
+#define PHY_28NM_DIG_SYNC_ERR			BIT(30)
+
+#define PHY_28NM_DIG_SQ_FILT_SHIFT		16
+#define PHY_28NM_DIG_SQ_FILT_MASK		(0x7 << 16)
+
+#define PHY_28NM_DIG_SQ_BLK_SHIFT		12
+#define PHY_28NM_DIG_SQ_BLK_MASK		(0x7 << 12)
+
+#define PHY_28NM_DIG_SYNC_NUM_SHIFT		0
+#define PHY_28NM_DIG_SYNC_NUM_MASK		(0x3 << 0)
+
+#define PHY_28NM_PLL_LOCK_BYPASS		BIT(7)
+
+/* PHY_28NM_OTG_REG */
+#define PHY_28NM_OTG_CONTROL_BY_PIN		BIT(5)
+#define PHY_28NM_OTG_PU_OTG			BIT(4)
+
+#define PHY_28NM_CHGDTC_ENABLE_SWITCH_DM_SHIFT_28 13
+#define PHY_28NM_CHGDTC_ENABLE_SWITCH_DP_SHIFT_28 12
+#define PHY_28NM_CHGDTC_VSRC_CHARGE_SHIFT_28	10
+#define PHY_28NM_CHGDTC_VDAT_CHARGE_SHIFT_28	8
+#define PHY_28NM_CHGDTC_CDP_DM_AUTO_SWITCH_SHIFT_28 7
+#define PHY_28NM_CHGDTC_DP_DM_SWAP_SHIFT_28	6
+#define PHY_28NM_CHGDTC_PU_CHRG_DTC_SHIFT_28	5
+#define PHY_28NM_CHGDTC_PD_EN_SHIFT_28		4
+#define PHY_28NM_CHGDTC_DCP_EN_SHIFT_28		3
+#define PHY_28NM_CHGDTC_CDP_EN_SHIFT_28		2
+#define PHY_28NM_CHGDTC_TESTMON_CHRGDTC_SHIFT_28 0
+
+#define PHY_28NM_CTRL1_CHRG_DTC_OUT_SHIFT_28	4
+#define PHY_28NM_CTRL1_VBUSDTC_OUT_SHIFT_28	2
+
+#define PHY_28NM_CTRL3_OVERWRITE		BIT(0)
+#define PHY_28NM_CTRL3_VBUS_VALID		BIT(4)
+#define PHY_28NM_CTRL3_AVALID			BIT(5)
+#define PHY_28NM_CTRL3_BVALID			BIT(6)
+
+struct mv_usb2_phy {
+	struct phy		*phy;
+	struct platform_device	*pdev;
+	void __iomem		*base;
+	struct clk		*clk;
+};
+
+static bool wait_for_reg(void __iomem *reg, u32 mask, unsigned long timeout)
+{
+	timeout += jiffies;
+	while (time_is_after_eq_jiffies(timeout)) {
+		if ((readl(reg) & mask) == mask)
+			return true;
+		msleep(1);
+	}
+	return false;
+}
+
+static int mv_usb2_phy_28nm_init(struct phy *phy)
+{
+	struct mv_usb2_phy *mv_phy = phy_get_drvdata(phy);
+	struct platform_device *pdev = mv_phy->pdev;
+	void __iomem *base = mv_phy->base;
+	u32 reg;
+	int ret;
+
+	clk_prepare_enable(mv_phy->clk);
+
+	/* PHY_28NM_PLL_REG0 */
+	reg = readl(base + PHY_28NM_PLL_REG0) &
+		~(PHY_28NM_PLL_SELLPFR_MASK | PHY_28NM_PLL_FBDIV_MASK
+		| PHY_28NM_PLL_ICP_MASK	| PHY_28NM_PLL_REFDIV_MASK);
+	writel(reg | (0x1 << PHY_28NM_PLL_SELLPFR_SHIFT
+		| 0xf0 << PHY_28NM_PLL_FBDIV_SHIFT
+		| 0x3 << PHY_28NM_PLL_ICP_SHIFT
+		| 0xd << PHY_28NM_PLL_REFDIV_SHIFT),
+		base + PHY_28NM_PLL_REG0);
+
+	/* PHY_28NM_PLL_REG1 */
+	reg = readl(base + PHY_28NM_PLL_REG1);
+	writel(reg | PHY_28NM_PLL_PU_PLL | PHY_28NM_PLL_PU_BY_REG,
+		base + PHY_28NM_PLL_REG1);
+
+	/* PHY_28NM_TX_REG0 */
+	reg = readl(base + PHY_28NM_TX_REG0) & ~PHY_28NM_TX_AMP_MASK;
+	writel(reg | PHY_28NM_TX_PU_BY_REG | 0x3 << PHY_28NM_TX_AMP_SHIFT |
+		PHY_28NM_TX_PU_ANA,
+		base + PHY_28NM_TX_REG0);
+
+	/* PHY_28NM_RX_REG0 */
+	reg = readl(base + PHY_28NM_RX_REG0) & ~PHY_28NM_RX_SQ_THRESH_MASK;
+	writel(reg | 0xa << PHY_28NM_RX_SQ_THRESH_SHIFT,
+		base + PHY_28NM_RX_REG0);
+
+	/* PHY_28NM_DIG_REG0 */
+	reg = readl(base + PHY_28NM_DIG_REG0) &
+		~(PHY_28NM_DIG_BITSTAFFING_ERR | PHY_28NM_DIG_SYNC_ERR |
+		PHY_28NM_DIG_SQ_FILT_MASK | PHY_28NM_DIG_SQ_BLK_MASK |
+		PHY_28NM_DIG_SYNC_NUM_MASK);
+	writel(reg | (0x1 << PHY_28NM_DIG_SYNC_NUM_SHIFT |
+		PHY_28NM_PLL_LOCK_BYPASS),
+		base + PHY_28NM_DIG_REG0);
+
+	/* PHY_28NM_OTG_REG */
+	reg = readl(base + PHY_28NM_OTG_REG) | PHY_28NM_OTG_PU_OTG;
+	writel(reg & ~PHY_28NM_OTG_CONTROL_BY_PIN, base + PHY_28NM_OTG_REG);
+
+	/*
+	 *  Calibration Timing
+	 *		   ____________________________
+	 *  CAL START   ___|
+	 *			   ____________________
+	 *  CAL_DONE    ___________|
+	 *		   | 400us |
+	 */
+
+	/* Make sure PHY Calibration is ready */
+	if (!wait_for_reg(base + PHY_28NM_CAL_REG,
+	    PHY_28NM_PLL_PLLCAL_DONE | PHY_28NM_PLL_IMPCAL_DONE,
+	    HZ / 10)) {
+		dev_warn(&pdev->dev, "USB PHY PLL calibrate not done after 100mS.");
+		ret = -ETIMEDOUT;
+		goto err_clk;
+	}
+	if (!wait_for_reg(base + PHY_28NM_RX_REG1,
+	    PHY_28NM_RX_SQCAL_DONE, HZ / 10)) {
+		dev_warn(&pdev->dev, "USB PHY RX SQ calibrate not done after 100mS.");
+		ret = -ETIMEDOUT;
+		goto err_clk;
+	}
+	/* Make sure PHY PLL is ready */
+	if (!wait_for_reg(base + PHY_28NM_PLL_REG0,
+	    PHY_28NM_PLL_READY, HZ / 10)) {
+		dev_warn(&pdev->dev, "PLL_READY not set after 100mS.");
+		ret = -ETIMEDOUT;
+		goto err_clk;
+	}
+
+	return 0;
+err_clk:
+	clk_disable_unprepare(mv_phy->clk);
+	return ret;
+}
+
+static int mv_usb2_phy_28nm_power_on(struct phy *phy)
+{
+	struct mv_usb2_phy *mv_phy = phy_get_drvdata(phy);
+	void __iomem *base = mv_phy->base;
+
+	writel(readl(base + PHY_28NM_CTRL_REG3) |
+		(PHY_28NM_CTRL3_OVERWRITE | PHY_28NM_CTRL3_VBUS_VALID |
+		PHY_28NM_CTRL3_AVALID | PHY_28NM_CTRL3_BVALID),
+		base + PHY_28NM_CTRL_REG3);
+
+	return 0;
+}
+
+static int mv_usb2_phy_28nm_power_off(struct phy *phy)
+{
+	struct mv_usb2_phy *mv_phy = phy_get_drvdata(phy);
+	void __iomem *base = mv_phy->base;
+
+	writel(readl(base + PHY_28NM_CTRL_REG3) |
+		~(PHY_28NM_CTRL3_OVERWRITE | PHY_28NM_CTRL3_VBUS_VALID
+		| PHY_28NM_CTRL3_AVALID	| PHY_28NM_CTRL3_BVALID),
+		base + PHY_28NM_CTRL_REG3);
+
+	return 0;
+}
+
+static int mv_usb2_phy_28nm_exit(struct phy *phy)
+{
+	struct mv_usb2_phy *mv_phy = phy_get_drvdata(phy);
+	void __iomem *base = mv_phy->base;
+	unsigned int val;
+
+	val = readw(base + PHY_28NM_PLL_REG1);
+	val &= ~PHY_28NM_PLL_PU_PLL;
+	writew(val, base + PHY_28NM_PLL_REG1);
+
+	/* power down PHY Analog part */
+	val = readw(base + PHY_28NM_TX_REG0);
+	val &= ~PHY_28NM_TX_PU_ANA;
+	writew(val, base + PHY_28NM_TX_REG0);
+
+	/* power down PHY OTG part */
+	val = readw(base + PHY_28NM_OTG_REG);
+	val &= ~PHY_28NM_OTG_PU_OTG;
+	writew(val, base + PHY_28NM_OTG_REG);
+
+	clk_disable_unprepare(mv_phy->clk);
+	return 0;
+}
+
+static const struct phy_ops usb_ops = {
+	.init		= mv_usb2_phy_28nm_init,
+	.power_on	= mv_usb2_phy_28nm_power_on,
+	.power_off	= mv_usb2_phy_28nm_power_off,
+	.exit		= mv_usb2_phy_28nm_exit,
+	.owner		= THIS_MODULE,
+};
+
+static int mv_usb2_phy_probe(struct platform_device *pdev)
+{
+	struct phy_provider *phy_provider;
+	struct mv_usb2_phy *mv_phy;
+	struct resource *r;
+
+	mv_phy = devm_kzalloc(&pdev->dev, sizeof(*mv_phy), GFP_KERNEL);
+	if (!mv_phy)
+		return -ENOMEM;
+
+	mv_phy->pdev = pdev;
+
+	mv_phy->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(mv_phy->clk)) {
+		dev_err(&pdev->dev, "failed to get clock.\n");
+		return PTR_ERR(mv_phy->clk);
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mv_phy->base = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(mv_phy->base))
+		return PTR_ERR(mv_phy->base);
+
+	mv_phy->phy = devm_phy_create(&pdev->dev, pdev->dev.of_node, &usb_ops);
+	if (IS_ERR(mv_phy->phy))
+		return PTR_ERR(mv_phy->phy);
+
+	phy_set_drvdata(mv_phy->phy, mv_phy);
+
+	phy_provider = devm_of_phy_provider_register(&pdev->dev, of_phy_simple_xlate);
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static const struct of_device_id mv_usbphy_dt_match[] = {
+	{ .compatible = "marvell,pxa1928-usb-phy", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mv_usbphy_dt_match);
+
+static struct platform_driver mv_usb2_phy_driver = {
+	.probe	= mv_usb2_phy_probe,
+	.driver = {
+		.name   = "mv-usb2-phy",
+		.of_match_table = of_match_ptr(mv_usbphy_dt_match),
+	},
+};
+module_platform_driver(mv_usb2_phy_driver);
+
+MODULE_AUTHOR("Rob Herring <robh@kernel.org>");
+MODULE_DESCRIPTION("Marvell USB2 phy driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-rcar-gen2.c b/drivers/phy/phy-rcar-gen2.c
index 778276aba3aa..39d9b2995435 100644
--- a/drivers/phy/phy-rcar-gen2.c
+++ b/drivers/phy/phy-rcar-gen2.c
@@ -23,7 +23,7 @@
 #define USBHS_LPSTS			0x02
 #define USBHS_UGCTRL			0x80
 #define USBHS_UGCTRL2			0x84
-#define USBHS_UGSTS			0x88	/* The manuals have 0x90 */
+#define USBHS_UGSTS			0x88	/* From technical update */
 
 /* Low Power Status register (LPSTS) */
 #define USBHS_LPSTS_SUSPM		0x4000
@@ -41,7 +41,7 @@
 #define USBHS_UGCTRL2_USB0SEL_HS_USB	0x00000030
 
 /* USB General status register (UGSTS) */
-#define USBHS_UGSTS_LOCK		0x00000300 /* The manuals have 0x3 */
+#define USBHS_UGSTS_LOCK		0x00000100 /* From technical update */
 
 #define PHYS_PER_CHANNEL	2
 
@@ -195,6 +195,7 @@ static struct phy_ops rcar_gen2_phy_ops = {
 static const struct of_device_id rcar_gen2_phy_match_table[] = {
 	{ .compatible = "renesas,usb-phy-r8a7790" },
 	{ .compatible = "renesas,usb-phy-r8a7791" },
+	{ .compatible = "renesas,usb-phy-r8a7794" },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, rcar_gen2_phy_match_table);
@@ -206,11 +207,6 @@ static struct phy *rcar_gen2_phy_xlate(struct device *dev,
 	struct device_node *np = args->np;
 	int i;
 
-	if (!of_device_is_available(np)) {
-		dev_warn(dev, "Requested PHY is disabled\n");
-		return ERR_PTR(-ENODEV);
-	}
-
 	drv = dev_get_drvdata(dev);
 	if (!drv)
 		return ERR_PTR(-EINVAL);
diff --git a/drivers/phy/phy-spear1310-miphy.c b/drivers/phy/phy-spear1310-miphy.c
index 65ae640cfbd1..45d0005b2203 100644
--- a/drivers/phy/phy-spear1310-miphy.c
+++ b/drivers/phy/phy-spear1310-miphy.c
@@ -2,8 +2,8 @@
  * ST SPEAr1310-miphy driver
  *
  * Copyright (C) 2014 ST Microelectronics
- * Pratyush Anand <pratyush.anand@st.com>
- * Mohit Kumar <mohit.kumar@st.com>
+ * Pratyush Anand <pratyush.anand@gmail.com>
+ * Mohit Kumar <mohit.kumar.dhaka@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
@@ -257,5 +257,5 @@ static struct platform_driver spear1310_miphy_driver = {
 module_platform_driver(spear1310_miphy_driver);
 
 MODULE_DESCRIPTION("ST SPEAR1310-MIPHY driver");
-MODULE_AUTHOR("Pratyush Anand <pratyush.anand@st.com>");
+MODULE_AUTHOR("Pratyush Anand <pratyush.anand@gmail.com>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-spear1340-miphy.c b/drivers/phy/phy-spear1340-miphy.c
index 1a00c2817f34..494240da4a39 100644
--- a/drivers/phy/phy-spear1340-miphy.c
+++ b/drivers/phy/phy-spear1340-miphy.c
@@ -2,8 +2,8 @@
  * ST spear1340-miphy driver
  *
  * Copyright (C) 2014 ST Microelectronics
- * Pratyush Anand <pratyush.anand@st.com>
- * Mohit Kumar <mohit.kumar@st.com>
+ * Pratyush Anand <pratyush.anand@gmail.com>
+ * Mohit Kumar <mohit.kumar.dhaka@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
@@ -290,5 +290,5 @@ static struct platform_driver spear1340_miphy_driver = {
 module_platform_driver(spear1340_miphy_driver);
 
 MODULE_DESCRIPTION("ST SPEAR1340-MIPHY driver");
-MODULE_AUTHOR("Pratyush Anand <pratyush.anand@st.com>");
+MODULE_AUTHOR("Pratyush Anand <pratyush.anand@gmail.com>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-sun4i-usb.c b/drivers/phy/phy-sun4i-usb.c
index a2b08f3ccb03..e17c539e4f6f 100644
--- a/drivers/phy/phy-sun4i-usb.c
+++ b/drivers/phy/phy-sun4i-usb.c
@@ -30,6 +30,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/phy/phy.h>
+#include <linux/phy/phy-sun4i-usb.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
 #include <linux/reset.h>
@@ -58,6 +59,7 @@
 #define PHY_OTG_FUNC_EN			0x28
 #define PHY_VBUS_DET_EN			0x29
 #define PHY_DISCON_TH_SEL		0x2a
+#define PHY_SQUELCH_DETECT		0x3c
 
 #define MAX_PHYS			3
 
@@ -204,6 +206,13 @@ static int sun4i_usb_phy_power_off(struct phy *_phy)
 	return 0;
 }
 
+void sun4i_usb_phy_set_squelch_detect(struct phy *_phy, bool enabled)
+{
+	struct sun4i_usb_phy *phy = phy_get_drvdata(_phy);
+
+	sun4i_usb_phy_write(phy, PHY_SQUELCH_DETECT, enabled ? 0 : 2, 2);
+}
+
 static struct phy_ops sun4i_usb_phy_ops = {
 	.init		= sun4i_usb_phy_init,
 	.exit		= sun4i_usb_phy_exit,
diff --git a/drivers/phy/phy-tusb1210.c b/drivers/phy/phy-tusb1210.c
new file mode 100644
index 000000000000..07efdd318bdc
--- /dev/null
+++ b/drivers/phy/phy-tusb1210.c
@@ -0,0 +1,153 @@
+/**
+ * tusb1210.c - TUSB1210 USB ULPI PHY driver
+ *
+ * Copyright (C) 2015 Intel Corporation
+ *
+ * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/module.h>
+#include <linux/ulpi/driver.h>
+#include <linux/gpio/consumer.h>
+
+#include "ulpi_phy.h"
+
+#define TUSB1210_VENDOR_SPECIFIC2		0x80
+#define TUSB1210_VENDOR_SPECIFIC2_IHSTX_SHIFT	0
+#define TUSB1210_VENDOR_SPECIFIC2_ZHSDRV_SHIFT	4
+#define TUSB1210_VENDOR_SPECIFIC2_DP_SHIFT	6
+
+struct tusb1210 {
+	struct ulpi *ulpi;
+	struct phy *phy;
+	struct gpio_desc *gpio_reset;
+	struct gpio_desc *gpio_cs;
+	u8 vendor_specific2;
+};
+
+static int tusb1210_power_on(struct phy *phy)
+{
+	struct tusb1210 *tusb = phy_get_drvdata(phy);
+
+	gpiod_set_value_cansleep(tusb->gpio_reset, 1);
+	gpiod_set_value_cansleep(tusb->gpio_cs, 1);
+
+	/* Restore the optional eye diagram optimization value */
+	if (tusb->vendor_specific2)
+		ulpi_write(tusb->ulpi, TUSB1210_VENDOR_SPECIFIC2,
+			   tusb->vendor_specific2);
+
+	return 0;
+}
+
+static int tusb1210_power_off(struct phy *phy)
+{
+	struct tusb1210 *tusb = phy_get_drvdata(phy);
+
+	gpiod_set_value_cansleep(tusb->gpio_reset, 0);
+	gpiod_set_value_cansleep(tusb->gpio_cs, 0);
+
+	return 0;
+}
+
+static struct phy_ops phy_ops = {
+	.power_on = tusb1210_power_on,
+	.power_off = tusb1210_power_off,
+	.owner = THIS_MODULE,
+};
+
+static int tusb1210_probe(struct ulpi *ulpi)
+{
+	struct gpio_desc *gpio;
+	struct tusb1210 *tusb;
+	u8 val, reg;
+	int ret;
+
+	tusb = devm_kzalloc(&ulpi->dev, sizeof(*tusb), GFP_KERNEL);
+	if (!tusb)
+		return -ENOMEM;
+
+	gpio = devm_gpiod_get(&ulpi->dev, "reset");
+	if (!IS_ERR(gpio)) {
+		ret = gpiod_direction_output(gpio, 0);
+		if (ret)
+			return ret;
+		gpiod_set_value_cansleep(gpio, 1);
+		tusb->gpio_reset = gpio;
+	}
+
+	gpio = devm_gpiod_get(&ulpi->dev, "cs");
+	if (!IS_ERR(gpio)) {
+		ret = gpiod_direction_output(gpio, 0);
+		if (ret)
+			return ret;
+		gpiod_set_value_cansleep(gpio, 1);
+		tusb->gpio_cs = gpio;
+	}
+
+	/*
+	 * VENDOR_SPECIFIC2 register in TUSB1210 can be used for configuring eye
+	 * diagram optimization and DP/DM swap.
+	 */
+
+	/* High speed output drive strength configuration */
+	device_property_read_u8(&ulpi->dev, "ihstx", &val);
+	reg = val << TUSB1210_VENDOR_SPECIFIC2_IHSTX_SHIFT;
+
+	/* High speed output impedance configuration */
+	device_property_read_u8(&ulpi->dev, "zhsdrv", &val);
+	reg |= val << TUSB1210_VENDOR_SPECIFIC2_ZHSDRV_SHIFT;
+
+	/* DP/DM swap control */
+	device_property_read_u8(&ulpi->dev, "datapolarity", &val);
+	reg |= val << TUSB1210_VENDOR_SPECIFIC2_DP_SHIFT;
+
+	if (reg) {
+		ulpi_write(ulpi, TUSB1210_VENDOR_SPECIFIC2, reg);
+		tusb->vendor_specific2 = reg;
+	}
+
+	tusb->phy = ulpi_phy_create(ulpi, &phy_ops);
+	if (IS_ERR(tusb->phy))
+		return PTR_ERR(tusb->phy);
+
+	tusb->ulpi = ulpi;
+
+	phy_set_drvdata(tusb->phy, tusb);
+	ulpi_set_drvdata(ulpi, tusb);
+	return 0;
+}
+
+static void tusb1210_remove(struct ulpi *ulpi)
+{
+	struct tusb1210 *tusb = ulpi_get_drvdata(ulpi);
+
+	ulpi_phy_destroy(ulpi, tusb->phy);
+}
+
+#define TI_VENDOR_ID 0x0451
+
+static const struct ulpi_device_id tusb1210_ulpi_id[] = {
+	{ TI_VENDOR_ID, 0x1507, },
+	{ },
+};
+MODULE_DEVICE_TABLE(ulpi, tusb1210_ulpi_id);
+
+static struct ulpi_driver tusb1210_driver = {
+	.id_table = tusb1210_ulpi_id,
+	.probe = tusb1210_probe,
+	.remove = tusb1210_remove,
+	.driver = {
+		.name = "tusb1210",
+		.owner = THIS_MODULE,
+	},
+};
+
+module_ulpi_driver(tusb1210_driver);
+
+MODULE_AUTHOR("Intel Corporation");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TUSB1210 ULPI PHY driver");
diff --git a/drivers/phy/phy-twl4030-usb.c b/drivers/phy/phy-twl4030-usb.c
index bc42d6a8939f..3a707dd14238 100644
--- a/drivers/phy/phy-twl4030-usb.c
+++ b/drivers/phy/phy-twl4030-usb.c
@@ -144,6 +144,16 @@
 #define PMBR1				0x0D
 #define GPIO_USB_4PIN_ULPI_2430C	(3 << 0)
 
+/*
+ * If VBUS is valid or ID is ground, then we know a
+ * cable is present and we need to be runtime-enabled
+ */
+static inline bool cable_present(enum omap_musb_vbus_id_status stat)
+{
+	return stat == OMAP_MUSB_VBUS_VALID ||
+		stat == OMAP_MUSB_ID_GROUND;
+}
+
 struct twl4030_usb {
 	struct usb_phy		phy;
 	struct device		*dev;
@@ -386,8 +396,6 @@ static int twl4030_usb_runtime_suspend(struct device *dev)
 	struct twl4030_usb *twl = dev_get_drvdata(dev);
 
 	dev_dbg(twl->dev, "%s\n", __func__);
-	if (pm_runtime_suspended(dev))
-		return 0;
 
 	__twl4030_phy_power(twl, 0);
 	regulator_disable(twl->usb1v5);
@@ -403,8 +411,6 @@ static int twl4030_usb_runtime_resume(struct device *dev)
 	int res;
 
 	dev_dbg(twl->dev, "%s\n", __func__);
-	if (pm_runtime_active(dev))
-		return 0;
 
 	res = regulator_enable(twl->usb3v1);
 	if (res)
@@ -536,8 +542,10 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 
 	mutex_lock(&twl->lock);
 	if (status >= 0 && status != twl->linkstat) {
+		status_changed =
+			cable_present(twl->linkstat) !=
+			cable_present(status);
 		twl->linkstat = status;
-		status_changed = true;
 	}
 	mutex_unlock(&twl->lock);
 
@@ -553,15 +561,11 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 		 * USB_LINK_VBUS state.  musb_hdrc won't care until it
 		 * starts to handle softconnect right.
 		 */
-		if ((status == OMAP_MUSB_VBUS_VALID) ||
-		    (status == OMAP_MUSB_ID_GROUND)) {
-			if (pm_runtime_suspended(twl->dev))
-				pm_runtime_get_sync(twl->dev);
+		if (cable_present(status)) {
+			pm_runtime_get_sync(twl->dev);
 		} else {
-			if (pm_runtime_active(twl->dev)) {
-				pm_runtime_mark_last_busy(twl->dev);
-				pm_runtime_put_autosuspend(twl->dev);
-			}
+			pm_runtime_mark_last_busy(twl->dev);
+			pm_runtime_put_autosuspend(twl->dev);
 		}
 		omap_musb_mailbox(status);
 	}
@@ -711,7 +715,6 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev, 2000);
 	pm_runtime_enable(&pdev->dev);
-	pm_runtime_get_sync(&pdev->dev);
 
 	/* Our job is to use irqs and status from the power module
 	 * to keep the transceiver disabled when nothing's connected.
@@ -767,6 +770,9 @@ static int twl4030_usb_remove(struct platform_device *pdev)
 
 	/* disable complete OTG block */
 	twl4030_usb_clear_bits(twl, POWER_CTRL, POWER_CTRL_OTG_ENAB);
+
+	if (cable_present(twl->linkstat))
+		pm_runtime_put_noidle(twl->dev);
 	pm_runtime_mark_last_busy(twl->dev);
 	pm_runtime_put(twl->dev);
 
diff --git a/drivers/phy/ulpi_phy.h b/drivers/phy/ulpi_phy.h
new file mode 100644
index 000000000000..ac49fb6285ee
--- /dev/null
+++ b/drivers/phy/ulpi_phy.h
@@ -0,0 +1,31 @@
+#include <linux/phy/phy.h>
+
+/**
+ * Helper that registers PHY for a ULPI device and adds a lookup for binding it
+ * and it's controller, which is always the parent.
+ */
+static inline struct phy
+*ulpi_phy_create(struct ulpi *ulpi, struct phy_ops *ops)
+{
+	struct phy *phy;
+	int ret;
+
+	phy = phy_create(&ulpi->dev, NULL, ops);
+	if (IS_ERR(phy))
+		return phy;
+
+	ret = phy_create_lookup(phy, "usb2-phy", dev_name(ulpi->dev.parent));
+	if (ret) {
+		phy_destroy(phy);
+		return ERR_PTR(ret);
+	}
+
+	return phy;
+}
+
+/* Remove a PHY that was created with ulpi_phy_create() and it's lookup. */
+static inline void ulpi_phy_destroy(struct ulpi *ulpi, struct phy *phy)
+{
+	phy_remove_lookup(phy, "usb2-phy", dev_name(ulpi->dev.parent));
+	phy_destroy(phy);
+}
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index aeb5729fbda6..100d9ac2ae1f 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -88,6 +88,15 @@ config PINCTRL_LANTIQ
 	select PINMUX
 	select PINCONF
 
+config PINCTRL_LPC18XX
+	bool "NXP LPC18XX/43XX SCU pinctrl driver"
+	depends on OF && (ARCH_LPC18XX || COMPILE_TEST)
+	default ARCH_LPC18XX
+	select PINMUX
+	select GENERIC_PINCONF
+	help
+	  Pinctrl driver for NXP LPC18xx/43xx System Control Unit (SCU).
+
 config PINCTRL_FALCON
 	bool
 	depends on SOC_FALCON
@@ -123,7 +132,17 @@ config PINCTRL_SIRF
 	bool "CSR SiRFprimaII pin controller driver"
 	depends on ARCH_SIRF
 	select PINMUX
+	select PINCONF
+	select GENERIC_PINCONF
+	select GPIOLIB_IRQCHIP
+
+config PINCTRL_PISTACHIO
+	def_bool y if MACH_PISTACHIO
+	depends on GPIOLIB
+	select PINMUX
+	select GENERIC_PINCONF
 	select GPIOLIB_IRQCHIP
+	select OF_GPIO
 
 config PINCTRL_ST
 	bool
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 6eadf04a33b3..f4216d9347e2 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_PINCTRL_AMD)	+= pinctrl-amd.o
 obj-$(CONFIG_PINCTRL_FALCON)	+= pinctrl-falcon.o
 obj-$(CONFIG_PINCTRL_MESON)	+= meson/
 obj-$(CONFIG_PINCTRL_PALMAS)	+= pinctrl-palmas.o
+obj-$(CONFIG_PINCTRL_PISTACHIO)	+= pinctrl-pistachio.o
 obj-$(CONFIG_PINCTRL_ROCKCHIP)	+= pinctrl-rockchip.o
 obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
 obj-$(CONFIG_PINCTRL_SIRF)	+= sirf/
@@ -34,6 +35,7 @@ obj-$(CONFIG_PINCTRL_U300)	+= pinctrl-u300.o
 obj-$(CONFIG_PINCTRL_COH901)	+= pinctrl-coh901.o
 obj-$(CONFIG_PINCTRL_XWAY)	+= pinctrl-xway.o
 obj-$(CONFIG_PINCTRL_LANTIQ)	+= pinctrl-lantiq.o
+obj-$(CONFIG_PINCTRL_LPC18XX)	+= pinctrl-lpc18xx.o
 obj-$(CONFIG_PINCTRL_TB10X)	+= pinctrl-tb10x.o
 obj-$(CONFIG_PINCTRL_ST) 	+= pinctrl-st.o
 obj-$(CONFIG_PINCTRL_ZYNQ)	+= pinctrl-zynq.o
diff --git a/drivers/pinctrl/bcm/pinctrl-bcm281xx.c b/drivers/pinctrl/bcm/pinctrl-bcm281xx.c
index 9641f1c7617e..c3c692e508e8 100644
--- a/drivers/pinctrl/bcm/pinctrl-bcm281xx.c
+++ b/drivers/pinctrl/bcm/pinctrl-bcm281xx.c
@@ -1425,9 +1425,9 @@ static int __init bcm281xx_pinctrl_probe(struct platform_device *pdev)
 	pctl = pinctrl_register(&bcm281xx_pinctrl_desc,
 				&pdev->dev,
 				pdata);
-	if (!pctl) {
+	if (IS_ERR(pctl)) {
 		dev_err(&pdev->dev, "Failed to register pinctrl\n");
-		return -ENODEV;
+		return PTR_ERR(pctl);
 	}
 
 	platform_set_drvdata(pdev, pdata);
diff --git a/drivers/pinctrl/bcm/pinctrl-bcm2835.c b/drivers/pinctrl/bcm/pinctrl-bcm2835.c
index 8d908e3f42c3..efcf2a2b3975 100644
--- a/drivers/pinctrl/bcm/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/bcm/pinctrl-bcm2835.c
@@ -1036,9 +1036,9 @@ static int bcm2835_pinctrl_probe(struct platform_device *pdev)
 	}
 
 	pc->pctl_dev = pinctrl_register(&bcm2835_pinctrl_desc, dev, pc);
-	if (!pc->pctl_dev) {
+	if (IS_ERR(pc->pctl_dev)) {
 		gpiochip_remove(&pc->gpio_chip);
-		return -EINVAL;
+		return PTR_ERR(pc->pctl_dev);
 	}
 
 	pc->gpio_range = bcm2835_pinctrl_gpio_range;
diff --git a/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c b/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c
index e406e3d8c1c7..7d9482bf8252 100644
--- a/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c
+++ b/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c
@@ -38,7 +38,7 @@
 #define CYGNUS_GPIO_DATA_IN_OFFSET   0x00
 #define CYGNUS_GPIO_DATA_OUT_OFFSET  0x04
 #define CYGNUS_GPIO_OUT_EN_OFFSET    0x08
-#define CYGNUS_GPIO_IN_TYPE_OFFSET   0x0c
+#define CYGNUS_GPIO_INT_TYPE_OFFSET  0x0c
 #define CYGNUS_GPIO_INT_DE_OFFSET    0x10
 #define CYGNUS_GPIO_INT_EDGE_OFFSET  0x14
 #define CYGNUS_GPIO_INT_MSK_OFFSET   0x18
@@ -264,7 +264,7 @@ static int cygnus_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	}
 
 	spin_lock_irqsave(&chip->lock, flags);
-	cygnus_set_bit(chip, CYGNUS_GPIO_IN_TYPE_OFFSET, gpio,
+	cygnus_set_bit(chip, CYGNUS_GPIO_INT_TYPE_OFFSET, gpio,
 		       level_triggered);
 	cygnus_set_bit(chip, CYGNUS_GPIO_INT_DE_OFFSET, gpio, dual_edge);
 	cygnus_set_bit(chip, CYGNUS_GPIO_INT_EDGE_OFFSET, gpio,
@@ -750,9 +750,9 @@ static int cygnus_gpio_register_pinconf(struct cygnus_gpio *chip)
 	pctldesc->confops = &cygnus_pconf_ops;
 
 	chip->pctl = pinctrl_register(pctldesc, chip->dev, chip);
-	if (!chip->pctl) {
+	if (IS_ERR(chip->pctl)) {
 		dev_err(chip->dev, "unable to register pinctrl device\n");
-		return -EINVAL;
+		return PTR_ERR(chip->pctl);
 	}
 
 	return 0;
diff --git a/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c b/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c
index f9a9283caf81..9728f3db9126 100644
--- a/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c
+++ b/drivers/pinctrl/bcm/pinctrl-cygnus-mux.c
@@ -989,9 +989,9 @@ static int cygnus_pinmux_probe(struct platform_device *pdev)
 
 	pinctrl->pctl = pinctrl_register(&cygnus_pinctrl_desc, &pdev->dev,
 			pinctrl);
-	if (!pinctrl->pctl) {
+	if (IS_ERR(pinctrl->pctl)) {
 		dev_err(&pdev->dev, "unable to register Cygnus IOMUX pinctrl\n");
-		return -EINVAL;
+		return PTR_ERR(pinctrl->pctl);
 	}
 
 	return 0;
diff --git a/drivers/pinctrl/berlin/berlin-bg2.c b/drivers/pinctrl/berlin/berlin-bg2.c
index b71a6fffef1b..274c5535b531 100644
--- a/drivers/pinctrl/berlin/berlin-bg2.c
+++ b/drivers/pinctrl/berlin/berlin-bg2.c
@@ -20,24 +20,24 @@
 static const struct berlin_desc_group berlin2_soc_pinctrl_groups[] = {
 	/* G */
 	BERLIN_PINCTRL_GROUP("G0", 0x00, 0x1, 0x00,
-		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"), /* SS0n */
 		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
 	BERLIN_PINCTRL_GROUP("G1", 0x00, 0x2, 0x01,
-		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"), /* SS1n */
 		BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
 		BERLIN_PINCTRL_FUNCTION(0x2, "usb1")),
 	BERLIN_PINCTRL_GROUP("G2", 0x00, 0x2, 0x02,
 		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"), /* SS2n */
 		BERLIN_PINCTRL_FUNCTION(0x2, "pwm"),
 		BERLIN_PINCTRL_FUNCTION(0x3, "i2s0")),
 	BERLIN_PINCTRL_GROUP("G3", 0x00, 0x2, 0x04,
 		BERLIN_PINCTRL_FUNCTION(0x0, "soc"),
-		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"), /* SS3n */
 		BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
 		BERLIN_PINCTRL_FUNCTION(0x3, "i2s1")),
 	BERLIN_PINCTRL_GROUP("G4", 0x00, 0x2, 0x06,
-		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"), /* CLK/SDI/SDO */
 		BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
 		BERLIN_PINCTRL_FUNCTION(0x2, "pwm")),
 	BERLIN_PINCTRL_GROUP("G5", 0x00, 0x3, 0x08,
@@ -163,15 +163,15 @@ static const struct berlin_desc_group berlin2_sysmgr_pinctrl_groups[] = {
 	/* GSM */
 	BERLIN_PINCTRL_GROUP("GSM0", 0x40, 0x2, 0x00,
 		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"), /* SS0n */
 		BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
 	BERLIN_PINCTRL_GROUP("GSM1", 0x40, 0x2, 0x02,
 		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"), /* SS1n */
 		BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
 	BERLIN_PINCTRL_GROUP("GSM2", 0x40, 0x2, 0x04,
 		BERLIN_PINCTRL_FUNCTION(0x0, "twsi2"),
-		BERLIN_PINCTRL_FUNCTION(0x1, "spi2")),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2")), /* SS2n/SS3n */
 	BERLIN_PINCTRL_GROUP("GSM3", 0x40, 0x2, 0x06,
 		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
 		BERLIN_PINCTRL_FUNCTION(0x1, "uart0"),	/* CTS/RTS */
@@ -187,7 +187,7 @@ static const struct berlin_desc_group berlin2_sysmgr_pinctrl_groups[] = {
 		BERLIN_PINCTRL_FUNCTION(0x3, "twsi3")),
 	BERLIN_PINCTRL_GROUP("GSM6", 0x40, 0x2, 0x0c,
 		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi2"), /* CLK/SDO */
 		BERLIN_PINCTRL_FUNCTION(0x1, "clki")),
 	BERLIN_PINCTRL_GROUP("GSM7", 0x40, 0x1, 0x0e,
 		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
@@ -218,11 +218,11 @@ static const struct berlin_pinctrl_desc berlin2_sysmgr_pinctrl_data = {
 
 static const struct of_device_id berlin2_pinctrl_match[] = {
 	{
-		.compatible = "marvell,berlin2-chip-ctrl",
+		.compatible = "marvell,berlin2-soc-pinctrl",
 		.data = &berlin2_soc_pinctrl_data
 	},
 	{
-		.compatible = "marvell,berlin2-system-ctrl",
+		.compatible = "marvell,berlin2-system-pinctrl",
 		.data = &berlin2_sysmgr_pinctrl_data
 	},
 	{}
@@ -233,28 +233,6 @@ static int berlin2_pinctrl_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *match =
 		of_match_device(berlin2_pinctrl_match, &pdev->dev);
-	struct regmap_config *rmconfig;
-	struct regmap *regmap;
-	struct resource *res;
-	void __iomem *base;
-
-	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
-	if (!rmconfig)
-		return -ENOMEM;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	rmconfig->reg_bits = 32,
-	rmconfig->val_bits = 32,
-	rmconfig->reg_stride = 4,
-	rmconfig->max_register = resource_size(res);
-
-	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
-	if (IS_ERR(regmap))
-		return PTR_ERR(regmap);
 
 	return berlin_pinctrl_probe(pdev, match->data);
 }
diff --git a/drivers/pinctrl/berlin/berlin-bg2cd.c b/drivers/pinctrl/berlin/berlin-bg2cd.c
index 19ac5a22c947..0cb793a3552a 100644
--- a/drivers/pinctrl/berlin/berlin-bg2cd.c
+++ b/drivers/pinctrl/berlin/berlin-bg2cd.c
@@ -68,17 +68,17 @@ static const struct berlin_desc_group berlin2cd_soc_pinctrl_groups[] = {
 		BERLIN_PINCTRL_FUNCTION(0x1, "twsi1"),
 		BERLIN_PINCTRL_FUNCTION(0x2, "gpio")),
 	BERLIN_PINCTRL_GROUP("G8", 0x00, 0x3, 0x10,
-		BERLIN_PINCTRL_FUNCTION(0x0, "ss0"),
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"), /* SS0n */
 		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
 	BERLIN_PINCTRL_GROUP("G9", 0x00, 0x3, 0x13,
 		BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x1, "spi1"), /* SS1n/SS2n */
 		BERLIN_PINCTRL_FUNCTION(0x2, "twsi0")),
 	BERLIN_PINCTRL_GROUP("G10", 0x00, 0x2, 0x16,
-		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"), /* CLK */
 		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
 	BERLIN_PINCTRL_GROUP("G11", 0x00, 0x2, 0x18,
-		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+		BERLIN_PINCTRL_FUNCTION(0x0, "spi1"), /* SDI/SDO */
 		BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
 	BERLIN_PINCTRL_GROUP("G12", 0x00, 0x3, 0x1a,
 		BERLIN_PINCTRL_FUNCTION(0x0, "usb1"),
@@ -161,11 +161,11 @@ static const struct berlin_pinctrl_desc berlin2cd_sysmgr_pinctrl_data = {
 
 static const struct of_device_id berlin2cd_pinctrl_match[] = {
 	{
-		.compatible = "marvell,berlin2cd-chip-ctrl",
+		.compatible = "marvell,berlin2cd-soc-pinctrl",
 		.data = &berlin2cd_soc_pinctrl_data
 	},
 	{
-		.compatible = "marvell,berlin2cd-system-ctrl",
+		.compatible = "marvell,berlin2cd-system-pinctrl",
 		.data = &berlin2cd_sysmgr_pinctrl_data
 	},
 	{}
@@ -176,28 +176,6 @@ static int berlin2cd_pinctrl_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *match =
 		of_match_device(berlin2cd_pinctrl_match, &pdev->dev);
-	struct regmap_config *rmconfig;
-	struct regmap *regmap;
-	struct resource *res;
-	void __iomem *base;
-
-	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
-	if (!rmconfig)
-		return -ENOMEM;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	rmconfig->reg_bits = 32,
-	rmconfig->val_bits = 32,
-	rmconfig->reg_stride = 4,
-	rmconfig->max_register = resource_size(res);
-
-	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
-	if (IS_ERR(regmap))
-		return PTR_ERR(regmap);
 
 	return berlin_pinctrl_probe(pdev, match->data);
 }
diff --git a/drivers/pinctrl/berlin/berlin-bg2q.c b/drivers/pinctrl/berlin/berlin-bg2q.c
index bd9662e57ad3..a466054a8206 100644
--- a/drivers/pinctrl/berlin/berlin-bg2q.c
+++ b/drivers/pinctrl/berlin/berlin-bg2q.c
@@ -59,21 +59,21 @@ static const struct berlin_desc_group berlin2q_soc_pinctrl_groups[] = {
 			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
 			BERLIN_PINCTRL_FUNCTION(0x3, "eddc")),
 	BERLIN_PINCTRL_GROUP("G8", 0x18, 0x3, 0x18,
-			BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x0, "spi1"), /* CLK/SDI/SDO */
 			BERLIN_PINCTRL_FUNCTION(0x1, "gpio")),
 	BERLIN_PINCTRL_GROUP("G9", 0x18, 0x3, 0x1b,
-			BERLIN_PINCTRL_FUNCTION(0x0, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x0, "spi1"), /* SS0n/SS1n */
 			BERLIN_PINCTRL_FUNCTION(0x1, "gpio"),
 			BERLIN_PINCTRL_FUNCTION(0x5, "sata")),
 	BERLIN_PINCTRL_GROUP("G10", 0x1c, 0x3, 0x00,
 			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-			BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi1"), /* SS2n */
 			BERLIN_PINCTRL_FUNCTION(0x3, "i2s0"),
 			BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
 			BERLIN_PINCTRL_FUNCTION(0x5, "sata")),
 	BERLIN_PINCTRL_GROUP("G11", 0x1c, 0x3, 0x03,
 			BERLIN_PINCTRL_FUNCTION(0x0, "jtag"),
-			BERLIN_PINCTRL_FUNCTION(0x1, "spi1"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi1"), /* SS3n */
 			BERLIN_PINCTRL_FUNCTION(0x2, "gpio"),
 			BERLIN_PINCTRL_FUNCTION(0x3, "i2s1"),
 			BERLIN_PINCTRL_FUNCTION(0x4, "pwm"),
@@ -301,19 +301,19 @@ static const struct berlin_desc_group berlin2q_sysmgr_pinctrl_groups[] = {
 	/* GSM */
 	BERLIN_PINCTRL_GROUP("GSM0", 0x40, 0x2, 0x00,
 			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"), /* SS0n */
 			BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
 	BERLIN_PINCTRL_GROUP("GSM1", 0x40, 0x2, 0x02,
 			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"), /* SS1n */
 			BERLIN_PINCTRL_FUNCTION(0x2, "eth1")),
 	BERLIN_PINCTRL_GROUP("GSM2", 0x40, 0x2, 0x04,
 			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"), /* SS2n/SS3n */
 			BERLIN_PINCTRL_FUNCTION(0x2, "eddc")),
 	BERLIN_PINCTRL_GROUP("GSM3", 0x40, 0x2, 0x06,
 			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
-			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"),
+			BERLIN_PINCTRL_FUNCTION(0x1, "spi2"), /* CLK/SDO */
 			BERLIN_PINCTRL_FUNCTION(0x2, "eddc")),
 	BERLIN_PINCTRL_GROUP("GSM4", 0x40, 0x1, 0x08,
 			BERLIN_PINCTRL_FUNCTION(0x0, "gpio"),
@@ -380,11 +380,11 @@ static const struct berlin_pinctrl_desc berlin2q_sysmgr_pinctrl_data = {
 
 static const struct of_device_id berlin2q_pinctrl_match[] = {
 	{
-		.compatible = "marvell,berlin2q-chip-ctrl",
+		.compatible = "marvell,berlin2q-soc-pinctrl",
 		.data = &berlin2q_soc_pinctrl_data,
 	},
 	{
-		.compatible = "marvell,berlin2q-system-ctrl",
+		.compatible = "marvell,berlin2q-system-pinctrl",
 		.data = &berlin2q_sysmgr_pinctrl_data,
 	},
 	{}
@@ -395,28 +395,6 @@ static int berlin2q_pinctrl_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *match =
 		of_match_device(berlin2q_pinctrl_match, &pdev->dev);
-	struct regmap_config *rmconfig;
-	struct regmap *regmap;
-	struct resource *res;
-	void __iomem *base;
-
-	rmconfig = devm_kzalloc(&pdev->dev, sizeof(*rmconfig), GFP_KERNEL);
-	if (!rmconfig)
-		return -ENOMEM;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	rmconfig->reg_bits = 32,
-	rmconfig->val_bits = 32,
-	rmconfig->reg_stride = 4,
-	rmconfig->max_register = resource_size(res);
-
-	regmap = devm_regmap_init_mmio(&pdev->dev, base, rmconfig);
-	if (IS_ERR(regmap))
-		return PTR_ERR(regmap);
 
 	return berlin_pinctrl_probe(pdev, match->data);
 }
diff --git a/drivers/pinctrl/berlin/berlin.c b/drivers/pinctrl/berlin/berlin.c
index 7f0b0f93242b..f49580617055 100644
--- a/drivers/pinctrl/berlin/berlin.c
+++ b/drivers/pinctrl/berlin/berlin.c
@@ -11,6 +11,7 @@
  */
 
 #include <linux/io.h>
+#include <linux/mfd/syscon.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
@@ -295,13 +296,15 @@ int berlin_pinctrl_probe(struct platform_device *pdev,
 			 const struct berlin_pinctrl_desc *desc)
 {
 	struct device *dev = &pdev->dev;
+	struct device_node *parent_np = of_get_parent(dev->of_node);
 	struct berlin_pinctrl *pctrl;
 	struct regmap *regmap;
 	int ret;
 
-	regmap = dev_get_regmap(&pdev->dev, NULL);
-	if (!regmap)
-		return -ENODEV;
+	regmap = syscon_node_to_regmap(parent_np);
+	of_node_put(parent_np);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
 
 	pctrl = devm_kzalloc(dev, sizeof(*pctrl), GFP_KERNEL);
 	if (!pctrl)
@@ -320,9 +323,9 @@ int berlin_pinctrl_probe(struct platform_device *pdev,
 	}
 
 	pctrl->pctrl_dev = pinctrl_register(&berlin_pctrl_desc, dev, pctrl);
-	if (!pctrl->pctrl_dev) {
+	if (IS_ERR(pctrl->pctrl_dev)) {
 		dev_err(dev, "failed to register pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(pctrl->pctrl_dev);
 	}
 
 	return 0;
diff --git a/drivers/pinctrl/core.c b/drivers/pinctrl/core.c
index 18ee2089df4a..8b8f3a04c353 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -558,7 +558,7 @@ int pinctrl_get_group_selector(struct pinctrl_dev *pctldev,
 }
 
 /**
- * pinctrl_request_gpio() - request a single pin to be used in as GPIO
+ * pinctrl_request_gpio() - request a single pin to be used as GPIO
  * @gpio: the GPIO pin number from the GPIO subsystem number space
  *
  * This function should *ONLY* be used from gpiolib-based GPIO drivers,
@@ -1115,7 +1115,7 @@ int pinctrl_register_map(struct pinctrl_map const *maps, unsigned num_maps,
 	int i, ret;
 	struct pinctrl_maps *maps_node;
 
-	pr_debug("add %d pinmux maps\n", num_maps);
+	pr_debug("add %u pinctrl maps\n", num_maps);
 
 	/* First sanity check the new mapping */
 	for (i = 0; i < num_maps; i++) {
@@ -1704,14 +1704,14 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 	int ret;
 
 	if (!pctldesc)
-		return NULL;
+		return ERR_PTR(-EINVAL);
 	if (!pctldesc->name)
-		return NULL;
+		return ERR_PTR(-EINVAL);
 
 	pctldev = kzalloc(sizeof(*pctldev), GFP_KERNEL);
 	if (pctldev == NULL) {
 		dev_err(dev, "failed to alloc struct pinctrl_dev\n");
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 	}
 
 	/* Initialize pin control device struct */
@@ -1724,20 +1724,23 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 	mutex_init(&pctldev->mutex);
 
 	/* check core ops for sanity */
-	if (pinctrl_check_ops(pctldev)) {
+	ret = pinctrl_check_ops(pctldev);
+	if (ret) {
 		dev_err(dev, "pinctrl ops lacks necessary functions\n");
 		goto out_err;
 	}
 
 	/* If we're implementing pinmuxing, check the ops for sanity */
 	if (pctldesc->pmxops) {
-		if (pinmux_check_ops(pctldev))
+		ret = pinmux_check_ops(pctldev);
+		if (ret)
 			goto out_err;
 	}
 
 	/* If we're implementing pinconfig, check the ops for sanity */
 	if (pctldesc->confops) {
-		if (pinconf_check_ops(pctldev))
+		ret = pinconf_check_ops(pctldev);
+		if (ret)
 			goto out_err;
 	}
 
@@ -1783,7 +1786,7 @@ struct pinctrl_dev *pinctrl_register(struct pinctrl_desc *pctldesc,
 out_err:
 	mutex_destroy(&pctldev->mutex);
 	kfree(pctldev);
-	return NULL;
+	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL_GPL(pinctrl_register);
 
diff --git a/drivers/pinctrl/freescale/Kconfig b/drivers/pinctrl/freescale/Kconfig
index 16aac38793fe..12ef544b4894 100644
--- a/drivers/pinctrl/freescale/Kconfig
+++ b/drivers/pinctrl/freescale/Kconfig
@@ -87,6 +87,13 @@ config PINCTRL_IMX6SX
 	help
 	  Say Y here to enable the imx6sx pinctrl driver
 
+config PINCTRL_IMX7D
+	bool "IMX7D pinctrl driver"
+	depends on SOC_IMX7D
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx7d pinctrl driver
+
 config PINCTRL_VF610
 	bool "Freescale Vybrid VF610 pinctrl driver"
 	depends on SOC_VF610
diff --git a/drivers/pinctrl/freescale/Makefile b/drivers/pinctrl/freescale/Makefile
index bba73c22f043..343cb436ab17 100644
--- a/drivers/pinctrl/freescale/Makefile
+++ b/drivers/pinctrl/freescale/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6q.o
 obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6dl.o
 obj-$(CONFIG_PINCTRL_IMX6SL)	+= pinctrl-imx6sl.o
 obj-$(CONFIG_PINCTRL_IMX6SX)	+= pinctrl-imx6sx.o
+obj-$(CONFIG_PINCTRL_IMX7D)	+= pinctrl-imx7d.o
 obj-$(CONFIG_PINCTRL_VF610)	+= pinctrl-vf610.o
 obj-$(CONFIG_PINCTRL_MXS)	+= pinctrl-mxs.o
 obj-$(CONFIG_PINCTRL_IMX23)	+= pinctrl-imx23.o
diff --git a/drivers/pinctrl/freescale/pinctrl-imx.c b/drivers/pinctrl/freescale/pinctrl-imx.c
index e261f1cf85c6..d7b98ba36825 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx.c
@@ -606,6 +606,29 @@ static int imx_pinctrl_parse_functions(struct device_node *np,
 	return 0;
 }
 
+/*
+ * Check if the DT contains pins in the direct child nodes. This indicates the
+ * newer DT format to store pins. This function returns true if the first found
+ * fsl,pins property is in a child of np. Otherwise false is returned.
+ */
+static bool imx_pinctrl_dt_is_flat_functions(struct device_node *np)
+{
+	struct device_node *function_np;
+	struct device_node *pinctrl_np;
+
+	for_each_child_of_node(np, function_np) {
+		if (of_property_read_bool(function_np, "fsl,pins"))
+			return true;
+
+		for_each_child_of_node(function_np, pinctrl_np) {
+			if (of_property_read_bool(pinctrl_np, "fsl,pins"))
+				return false;
+		}
+	}
+
+	return true;
+}
+
 static int imx_pinctrl_probe_dt(struct platform_device *pdev,
 				struct imx_pinctrl_soc_info *info)
 {
@@ -613,14 +636,20 @@ static int imx_pinctrl_probe_dt(struct platform_device *pdev,
 	struct device_node *child;
 	u32 nfuncs = 0;
 	u32 i = 0;
+	bool flat_funcs;
 
 	if (!np)
 		return -ENODEV;
 
-	nfuncs = of_get_child_count(np);
-	if (nfuncs <= 0) {
-		dev_err(&pdev->dev, "no functions defined\n");
-		return -EINVAL;
+	flat_funcs = imx_pinctrl_dt_is_flat_functions(np);
+	if (flat_funcs) {
+		nfuncs = 1;
+	} else {
+		nfuncs = of_get_child_count(np);
+		if (nfuncs <= 0) {
+			dev_err(&pdev->dev, "no functions defined\n");
+			return -EINVAL;
+		}
 	}
 
 	info->nfunctions = nfuncs;
@@ -629,16 +658,24 @@ static int imx_pinctrl_probe_dt(struct platform_device *pdev,
 	if (!info->functions)
 		return -ENOMEM;
 
-	info->ngroups = 0;
-	for_each_child_of_node(np, child)
-		info->ngroups += of_get_child_count(child);
+	if (flat_funcs) {
+		info->ngroups = of_get_child_count(np);
+	} else {
+		info->ngroups = 0;
+		for_each_child_of_node(np, child)
+			info->ngroups += of_get_child_count(child);
+	}
 	info->groups = devm_kzalloc(&pdev->dev, info->ngroups * sizeof(struct imx_pin_group),
 					GFP_KERNEL);
 	if (!info->groups)
 		return -ENOMEM;
 
-	for_each_child_of_node(np, child)
-		imx_pinctrl_parse_functions(child, info, i++);
+	if (flat_funcs) {
+		imx_pinctrl_parse_functions(np, info, 0);
+	} else {
+		for_each_child_of_node(np, child)
+			imx_pinctrl_parse_functions(child, info, i++);
+	}
 
 	return 0;
 }
@@ -690,9 +727,9 @@ int imx_pinctrl_probe(struct platform_device *pdev,
 	ipctl->dev = info->dev;
 	platform_set_drvdata(pdev, ipctl);
 	ipctl->pctl = pinctrl_register(&imx_pinctrl_desc, &pdev->dev, ipctl);
-	if (!ipctl->pctl) {
+	if (IS_ERR(ipctl->pctl)) {
 		dev_err(&pdev->dev, "could not register IMX pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(ipctl->pctl);
 	}
 
 	dev_info(&pdev->dev, "initialized IMX pinctrl driver\n");
diff --git a/drivers/pinctrl/freescale/pinctrl-imx1-core.c b/drivers/pinctrl/freescale/pinctrl-imx1-core.c
index 5ac59fbb2440..5fd4437cee15 100644
--- a/drivers/pinctrl/freescale/pinctrl-imx1-core.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx1-core.c
@@ -633,9 +633,9 @@ int imx1_pinctrl_core_probe(struct platform_device *pdev,
 	ipctl->dev = info->dev;
 	platform_set_drvdata(pdev, ipctl);
 	ipctl->pctl = pinctrl_register(pctl_desc, &pdev->dev, ipctl);
-	if (!ipctl->pctl) {
+	if (IS_ERR(ipctl->pctl)) {
 		dev_err(&pdev->dev, "could not register IMX pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(ipctl->pctl);
 	}
 
 	ret = of_platform_populate(pdev->dev.of_node, NULL, NULL, &pdev->dev);
diff --git a/drivers/pinctrl/freescale/pinctrl-imx7d.c b/drivers/pinctrl/freescale/pinctrl-imx7d.c
new file mode 100644
index 000000000000..1fa7530530dd
--- /dev/null
+++ b/drivers/pinctrl/freescale/pinctrl-imx7d.c
@@ -0,0 +1,384 @@
+/*
+ * Copyright (C) 2014-2015 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-imx.h"
+
+enum imx7d_pads {
+	MX7D_PAD_RESERVE0 = 0,
+	MX7D_PAD_RESERVE1 = 1,
+	MX7D_PAD_RESERVE2 = 2,
+	MX7D_PAD_RESERVE3 = 3,
+	MX7D_PAD_RESERVE4 = 4,
+	MX7D_PAD_GPIO1_IO08 = 5,
+	MX7D_PAD_GPIO1_IO09 = 6,
+	MX7D_PAD_GPIO1_IO10 = 7,
+	MX7D_PAD_GPIO1_IO11 = 8,
+	MX7D_PAD_GPIO1_IO12 = 9,
+	MX7D_PAD_GPIO1_IO13 = 10,
+	MX7D_PAD_GPIO1_IO14 = 11,
+	MX7D_PAD_GPIO1_IO15 = 12,
+	MX7D_PAD_EPDC_DATA00 = 13,
+	MX7D_PAD_EPDC_DATA01 = 14,
+	MX7D_PAD_EPDC_DATA02 = 15,
+	MX7D_PAD_EPDC_DATA03 = 16,
+	MX7D_PAD_EPDC_DATA04 = 17,
+	MX7D_PAD_EPDC_DATA05 = 18,
+	MX7D_PAD_EPDC_DATA06 = 19,
+	MX7D_PAD_EPDC_DATA07 = 20,
+	MX7D_PAD_EPDC_DATA08 = 21,
+	MX7D_PAD_EPDC_DATA09 = 22,
+	MX7D_PAD_EPDC_DATA10 = 23,
+	MX7D_PAD_EPDC_DATA11 = 24,
+	MX7D_PAD_EPDC_DATA12 = 25,
+	MX7D_PAD_EPDC_DATA13 = 26,
+	MX7D_PAD_EPDC_DATA14 = 27,
+	MX7D_PAD_EPDC_DATA15 = 28,
+	MX7D_PAD_EPDC_SDCLK = 29,
+	MX7D_PAD_EPDC_SDLE = 30,
+	MX7D_PAD_EPDC_SDOE = 31,
+	MX7D_PAD_EPDC_SDSHR = 32,
+	MX7D_PAD_EPDC_SDCE0 = 33,
+	MX7D_PAD_EPDC_SDCE1 = 34,
+	MX7D_PAD_EPDC_SDCE2 = 35,
+	MX7D_PAD_EPDC_SDCE3 = 36,
+	MX7D_PAD_EPDC_GDCLK = 37,
+	MX7D_PAD_EPDC_GDOE = 38,
+	MX7D_PAD_EPDC_GDRL = 39,
+	MX7D_PAD_EPDC_GDSP = 40,
+	MX7D_PAD_EPDC_BDR0 = 41,
+	MX7D_PAD_EPDC_BDR1 = 42,
+	MX7D_PAD_EPDC_PWR_COM = 43,
+	MX7D_PAD_EPDC_PWR_STAT = 44,
+	MX7D_PAD_LCD_CLK = 45,
+	MX7D_PAD_LCD_ENABLE = 46,
+	MX7D_PAD_LCD_HSYNC = 47,
+	MX7D_PAD_LCD_VSYNC = 48,
+	MX7D_PAD_LCD_RESET = 49,
+	MX7D_PAD_LCD_DATA00 = 50,
+	MX7D_PAD_LCD_DATA01 = 51,
+	MX7D_PAD_LCD_DATA02 = 52,
+	MX7D_PAD_LCD_DATA03 = 53,
+	MX7D_PAD_LCD_DATA04 = 54,
+	MX7D_PAD_LCD_DATA05 = 55,
+	MX7D_PAD_LCD_DATA06 = 56,
+	MX7D_PAD_LCD_DATA07 = 57,
+	MX7D_PAD_LCD_DATA08 = 58,
+	MX7D_PAD_LCD_DATA09 = 59,
+	MX7D_PAD_LCD_DATA10 = 60,
+	MX7D_PAD_LCD_DATA11 = 61,
+	MX7D_PAD_LCD_DATA12 = 62,
+	MX7D_PAD_LCD_DATA13 = 63,
+	MX7D_PAD_LCD_DATA14 = 64,
+	MX7D_PAD_LCD_DATA15 = 65,
+	MX7D_PAD_LCD_DATA16 = 66,
+	MX7D_PAD_LCD_DATA17 = 67,
+	MX7D_PAD_LCD_DATA18 = 68,
+	MX7D_PAD_LCD_DATA19 = 69,
+	MX7D_PAD_LCD_DATA20 = 70,
+	MX7D_PAD_LCD_DATA21 = 71,
+	MX7D_PAD_LCD_DATA22 = 72,
+	MX7D_PAD_LCD_DATA23 = 73,
+	MX7D_PAD_UART1_RX_DATA = 74,
+	MX7D_PAD_UART1_TX_DATA = 75,
+	MX7D_PAD_UART2_RX_DATA = 76,
+	MX7D_PAD_UART2_TX_DATA = 77,
+	MX7D_PAD_UART3_RX_DATA = 78,
+	MX7D_PAD_UART3_TX_DATA = 79,
+	MX7D_PAD_UART3_RTS_B = 80,
+	MX7D_PAD_UART3_CTS_B = 81,
+	MX7D_PAD_I2C1_SCL = 82,
+	MX7D_PAD_I2C1_SDA = 83,
+	MX7D_PAD_I2C2_SCL = 84,
+	MX7D_PAD_I2C2_SDA = 85,
+	MX7D_PAD_I2C3_SCL = 86,
+	MX7D_PAD_I2C3_SDA = 87,
+	MX7D_PAD_I2C4_SCL = 88,
+	MX7D_PAD_I2C4_SDA = 89,
+	MX7D_PAD_ECSPI1_SCLK = 90,
+	MX7D_PAD_ECSPI1_MOSI = 91,
+	MX7D_PAD_ECSPI1_MISO = 92,
+	MX7D_PAD_ECSPI1_SS0 = 93,
+	MX7D_PAD_ECSPI2_SCLK = 94,
+	MX7D_PAD_ECSPI2_MOSI = 95,
+	MX7D_PAD_ECSPI2_MISO = 96,
+	MX7D_PAD_ECSPI2_SS0 = 97,
+	MX7D_PAD_SD1_CD_B = 98,
+	MX7D_PAD_SD1_WP = 99,
+	MX7D_PAD_SD1_RESET_B = 100,
+	MX7D_PAD_SD1_CLK = 101,
+	MX7D_PAD_SD1_CMD = 102,
+	MX7D_PAD_SD1_DATA0 = 103,
+	MX7D_PAD_SD1_DATA1 = 104,
+	MX7D_PAD_SD1_DATA2 = 105,
+	MX7D_PAD_SD1_DATA3 = 106,
+	MX7D_PAD_SD2_CD_B = 107,
+	MX7D_PAD_SD2_WP = 108,
+	MX7D_PAD_SD2_RESET_B = 109,
+	MX7D_PAD_SD2_CLK = 110,
+	MX7D_PAD_SD2_CMD = 111,
+	MX7D_PAD_SD2_DATA0 = 112,
+	MX7D_PAD_SD2_DATA1 = 113,
+	MX7D_PAD_SD2_DATA2 = 114,
+	MX7D_PAD_SD2_DATA3 = 115,
+	MX7D_PAD_SD3_CLK = 116,
+	MX7D_PAD_SD3_CMD = 117,
+	MX7D_PAD_SD3_DATA0 = 118,
+	MX7D_PAD_SD3_DATA1 = 119,
+	MX7D_PAD_SD3_DATA2 = 120,
+	MX7D_PAD_SD3_DATA3 = 121,
+	MX7D_PAD_SD3_DATA4 = 122,
+	MX7D_PAD_SD3_DATA5 = 123,
+	MX7D_PAD_SD3_DATA6 = 124,
+	MX7D_PAD_SD3_DATA7 = 125,
+	MX7D_PAD_SD3_STROBE = 126,
+	MX7D_PAD_SD3_RESET_B = 127,
+	MX7D_PAD_SAI1_RX_DATA = 128,
+	MX7D_PAD_SAI1_TX_BCLK = 129,
+	MX7D_PAD_SAI1_TX_SYNC = 130,
+	MX7D_PAD_SAI1_TX_DATA = 131,
+	MX7D_PAD_SAI1_RX_SYNC = 132,
+	MX7D_PAD_SAI1_RX_BCLK = 133,
+	MX7D_PAD_SAI1_MCLK = 134,
+	MX7D_PAD_SAI2_TX_SYNC = 135,
+	MX7D_PAD_SAI2_TX_BCLK = 136,
+	MX7D_PAD_SAI2_RX_DATA = 137,
+	MX7D_PAD_SAI2_TX_DATA = 138,
+	MX7D_PAD_ENET1_RGMII_RD0 = 139,
+	MX7D_PAD_ENET1_RGMII_RD1 = 140,
+	MX7D_PAD_ENET1_RGMII_RD2 = 141,
+	MX7D_PAD_ENET1_RGMII_RD3 = 142,
+	MX7D_PAD_ENET1_RGMII_RX_CTL = 143,
+	MX7D_PAD_ENET1_RGMII_RXC = 144,
+	MX7D_PAD_ENET1_RGMII_TD0 = 145,
+	MX7D_PAD_ENET1_RGMII_TD1 = 146,
+	MX7D_PAD_ENET1_RGMII_TD2 = 147,
+	MX7D_PAD_ENET1_RGMII_TD3 = 148,
+	MX7D_PAD_ENET1_RGMII_TX_CTL = 149,
+	MX7D_PAD_ENET1_RGMII_TXC = 150,
+	MX7D_PAD_ENET1_TX_CLK = 151,
+	MX7D_PAD_ENET1_RX_CLK = 152,
+	MX7D_PAD_ENET1_CRS = 153,
+	MX7D_PAD_ENET1_COL = 154,
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx7d_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX7D_PAD_RESERVE0),
+	IMX_PINCTRL_PIN(MX7D_PAD_RESERVE1),
+	IMX_PINCTRL_PIN(MX7D_PAD_RESERVE2),
+	IMX_PINCTRL_PIN(MX7D_PAD_RESERVE3),
+	IMX_PINCTRL_PIN(MX7D_PAD_RESERVE4),
+	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO08),
+	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO09),
+	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO10),
+	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO11),
+	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO12),
+	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO13),
+	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO14),
+	IMX_PINCTRL_PIN(MX7D_PAD_GPIO1_IO15),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA00),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA01),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA02),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA03),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA04),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA05),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA06),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA07),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA08),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA09),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA10),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA11),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA12),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA13),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA14),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_DATA15),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_SDCLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_SDLE),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_SDOE),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_SDSHR),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_SDCE0),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_SDCE1),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_SDCE2),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_SDCE3),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_GDCLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_GDOE),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_GDRL),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_GDSP),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_BDR0),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_BDR1),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_PWR_COM),
+	IMX_PINCTRL_PIN(MX7D_PAD_EPDC_PWR_STAT),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_CLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_ENABLE),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_HSYNC),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_VSYNC),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_RESET),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA00),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA01),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA02),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA03),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA04),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA05),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA06),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA07),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA08),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA09),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA10),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA11),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA12),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA13),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA14),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA15),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA16),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA17),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA18),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA19),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA20),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA21),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA22),
+	IMX_PINCTRL_PIN(MX7D_PAD_LCD_DATA23),
+	IMX_PINCTRL_PIN(MX7D_PAD_UART1_RX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_UART1_TX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_UART2_RX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_UART2_TX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_UART3_RX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_UART3_TX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_UART3_RTS_B),
+	IMX_PINCTRL_PIN(MX7D_PAD_UART3_CTS_B),
+	IMX_PINCTRL_PIN(MX7D_PAD_I2C1_SCL),
+	IMX_PINCTRL_PIN(MX7D_PAD_I2C1_SDA),
+	IMX_PINCTRL_PIN(MX7D_PAD_I2C2_SCL),
+	IMX_PINCTRL_PIN(MX7D_PAD_I2C2_SDA),
+	IMX_PINCTRL_PIN(MX7D_PAD_I2C3_SCL),
+	IMX_PINCTRL_PIN(MX7D_PAD_I2C3_SDA),
+	IMX_PINCTRL_PIN(MX7D_PAD_I2C4_SCL),
+	IMX_PINCTRL_PIN(MX7D_PAD_I2C4_SDA),
+	IMX_PINCTRL_PIN(MX7D_PAD_ECSPI1_SCLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_ECSPI1_MOSI),
+	IMX_PINCTRL_PIN(MX7D_PAD_ECSPI1_MISO),
+	IMX_PINCTRL_PIN(MX7D_PAD_ECSPI1_SS0),
+	IMX_PINCTRL_PIN(MX7D_PAD_ECSPI2_SCLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_ECSPI2_MOSI),
+	IMX_PINCTRL_PIN(MX7D_PAD_ECSPI2_MISO),
+	IMX_PINCTRL_PIN(MX7D_PAD_ECSPI2_SS0),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_CD_B),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_WP),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_RESET_B),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_CLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_CMD),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_DATA0),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_DATA1),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_DATA2),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD1_DATA3),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_CD_B),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_WP),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_RESET_B),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_CLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_CMD),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_DATA0),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_DATA1),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_DATA2),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD2_DATA3),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_CLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_CMD),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_DATA0),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_DATA1),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_DATA2),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_DATA3),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_DATA4),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_DATA5),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_DATA6),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_DATA7),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_STROBE),
+	IMX_PINCTRL_PIN(MX7D_PAD_SD3_RESET_B),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI1_RX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI1_TX_BCLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI1_TX_SYNC),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI1_TX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI1_RX_SYNC),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI1_RX_BCLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI1_MCLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI2_TX_SYNC),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI2_TX_BCLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI2_RX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_SAI2_TX_DATA),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_RD0),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_RD1),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_RD2),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_RD3),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_RX_CTL),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_RXC),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_TD0),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_TD1),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_TD2),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_TD3),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_TX_CTL),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RGMII_TXC),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_TX_CLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_RX_CLK),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_CRS),
+	IMX_PINCTRL_PIN(MX7D_PAD_ENET1_COL),
+};
+
+static struct imx_pinctrl_soc_info imx7d_pinctrl_info = {
+	.pins = imx7d_pinctrl_pads,
+	.npins = ARRAY_SIZE(imx7d_pinctrl_pads),
+};
+
+static struct of_device_id imx7d_pinctrl_of_match[] = {
+	{ .compatible = "fsl,imx7d-iomuxc", .data = &imx7d_pinctrl_info, },
+	{ /* sentinel */ }
+};
+
+static int imx7d_pinctrl_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	struct imx_pinctrl_soc_info *pinctrl_info;
+
+	match = of_match_device(imx7d_pinctrl_of_match, &pdev->dev);
+
+	if (!match)
+		return -ENODEV;
+
+	pinctrl_info = (struct imx_pinctrl_soc_info *) match->data;
+
+	return imx_pinctrl_probe(pdev, pinctrl_info);
+}
+
+static struct platform_driver imx7d_pinctrl_driver = {
+	.driver = {
+		.name = "imx7d-pinctrl",
+		.of_match_table = of_match_ptr(imx7d_pinctrl_of_match),
+	},
+	.probe = imx7d_pinctrl_probe,
+	.remove = imx_pinctrl_remove,
+};
+
+static int __init imx7d_pinctrl_init(void)
+{
+	return platform_driver_register(&imx7d_pinctrl_driver);
+}
+arch_initcall(imx7d_pinctrl_init);
+
+static void __exit imx7d_pinctrl_exit(void)
+{
+	platform_driver_unregister(&imx7d_pinctrl_driver);
+}
+module_exit(imx7d_pinctrl_exit);
+
+MODULE_AUTHOR("Anson Huang <Anson.Huang@freescale.com>");
+MODULE_DESCRIPTION("Freescale imx7d pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/freescale/pinctrl-mxs.c b/drivers/pinctrl/freescale/pinctrl-mxs.c
index 646d5c244af1..f64eecb24755 100644
--- a/drivers/pinctrl/freescale/pinctrl-mxs.c
+++ b/drivers/pinctrl/freescale/pinctrl-mxs.c
@@ -540,9 +540,9 @@ int mxs_pinctrl_probe(struct platform_device *pdev,
 	}
 
 	d->pctl = pinctrl_register(&mxs_pinctrl_desc, &pdev->dev, d);
-	if (!d->pctl) {
+	if (IS_ERR(d->pctl)) {
 		dev_err(&pdev->dev, "Couldn't register MXS pinctrl driver\n");
-		ret = -EINVAL;
+		ret = PTR_ERR(d->pctl);
 		goto err;
 	}
 
diff --git a/drivers/pinctrl/intel/pinctrl-cherryview.c b/drivers/pinctrl/intel/pinctrl-cherryview.c
index 732ff757a95f..3f737daa3fd2 100644
--- a/drivers/pinctrl/intel/pinctrl-cherryview.c
+++ b/drivers/pinctrl/intel/pinctrl-cherryview.c
@@ -1533,9 +1533,9 @@ static int chv_pinctrl_probe(struct platform_device *pdev)
 	pctrl->pctldesc.npins = pctrl->community->npins;
 
 	pctrl->pctldev = pinctrl_register(&pctrl->pctldesc, &pdev->dev, pctrl);
-	if (!pctrl->pctldev) {
+	if (IS_ERR(pctrl->pctldev)) {
 		dev_err(&pdev->dev, "failed to register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(pctrl->pctldev);
 	}
 
 	ret = chv_gpio_probe(pctrl, irq);
diff --git a/drivers/pinctrl/intel/pinctrl-intel.c b/drivers/pinctrl/intel/pinctrl-intel.c
index 00768e53deec..f9ee0d68b288 100644
--- a/drivers/pinctrl/intel/pinctrl-intel.c
+++ b/drivers/pinctrl/intel/pinctrl-intel.c
@@ -1021,9 +1021,9 @@ int intel_pinctrl_probe(struct platform_device *pdev,
 	pctrl->pctldesc.npins = pctrl->soc->npins;
 
 	pctrl->pctldev = pinctrl_register(&pctrl->pctldesc, &pdev->dev, pctrl);
-	if (!pctrl->pctldev) {
+	if (IS_ERR(pctrl->pctldev)) {
 		dev_err(&pdev->dev, "failed to register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(pctrl->pctldev);
 	}
 
 	ret = intel_gpio_probe(pctrl, irq);
diff --git a/drivers/pinctrl/intel/pinctrl-sunrisepoint.c b/drivers/pinctrl/intel/pinctrl-sunrisepoint.c
index 55d025dc89e8..1de9ae5010db 100644
--- a/drivers/pinctrl/intel/pinctrl-sunrisepoint.c
+++ b/drivers/pinctrl/intel/pinctrl-sunrisepoint.c
@@ -284,8 +284,271 @@ static const struct intel_pinctrl_soc_data sptlp_soc_data = {
 	.ncommunities = ARRAY_SIZE(sptlp_communities),
 };
 
+/* Sunrisepoint-H */
+static const struct pinctrl_pin_desc spth_pins[] = {
+	/* GPP_A */
+	PINCTRL_PIN(0, "RCINB"),
+	PINCTRL_PIN(1, "LAD_0"),
+	PINCTRL_PIN(2, "LAD_1"),
+	PINCTRL_PIN(3, "LAD_2"),
+	PINCTRL_PIN(4, "LAD_3"),
+	PINCTRL_PIN(5, "LFRAMEB"),
+	PINCTRL_PIN(6, "SERIQ"),
+	PINCTRL_PIN(7, "PIRQAB"),
+	PINCTRL_PIN(8, "CLKRUNB"),
+	PINCTRL_PIN(9, "CLKOUT_LPC_0"),
+	PINCTRL_PIN(10, "CLKOUT_LPC_1"),
+	PINCTRL_PIN(11, "PMEB"),
+	PINCTRL_PIN(12, "BM_BUSYB"),
+	PINCTRL_PIN(13, "SUSWARNB_SUS_PWRDNACK"),
+	PINCTRL_PIN(14, "SUS_STATB"),
+	PINCTRL_PIN(15, "SUSACKB"),
+	PINCTRL_PIN(16, "CLKOUT_48"),
+	PINCTRL_PIN(17, "ISH_GP_7"),
+	PINCTRL_PIN(18, "ISH_GP_0"),
+	PINCTRL_PIN(19, "ISH_GP_1"),
+	PINCTRL_PIN(20, "ISH_GP_2"),
+	PINCTRL_PIN(21, "ISH_GP_3"),
+	PINCTRL_PIN(22, "ISH_GP_4"),
+	PINCTRL_PIN(23, "ISH_GP_5"),
+	/* GPP_B */
+	PINCTRL_PIN(24, "CORE_VID_0"),
+	PINCTRL_PIN(25, "CORE_VID_1"),
+	PINCTRL_PIN(26, "VRALERTB"),
+	PINCTRL_PIN(27, "CPU_GP_2"),
+	PINCTRL_PIN(28, "CPU_GP_3"),
+	PINCTRL_PIN(29, "SRCCLKREQB_0"),
+	PINCTRL_PIN(30, "SRCCLKREQB_1"),
+	PINCTRL_PIN(31, "SRCCLKREQB_2"),
+	PINCTRL_PIN(32, "SRCCLKREQB_3"),
+	PINCTRL_PIN(33, "SRCCLKREQB_4"),
+	PINCTRL_PIN(34, "SRCCLKREQB_5"),
+	PINCTRL_PIN(35, "EXT_PWR_GATEB"),
+	PINCTRL_PIN(36, "SLP_S0B"),
+	PINCTRL_PIN(37, "PLTRSTB"),
+	PINCTRL_PIN(38, "SPKR"),
+	PINCTRL_PIN(39, "GSPI0_CSB"),
+	PINCTRL_PIN(40, "GSPI0_CLK"),
+	PINCTRL_PIN(41, "GSPI0_MISO"),
+	PINCTRL_PIN(42, "GSPI0_MOSI"),
+	PINCTRL_PIN(43, "GSPI1_CSB"),
+	PINCTRL_PIN(44, "GSPI1_CLK"),
+	PINCTRL_PIN(45, "GSPI1_MISO"),
+	PINCTRL_PIN(46, "GSPI1_MOSI"),
+	PINCTRL_PIN(47, "SML1ALERTB"),
+	/* GPP_C */
+	PINCTRL_PIN(48, "SMBCLK"),
+	PINCTRL_PIN(49, "SMBDATA"),
+	PINCTRL_PIN(50, "SMBALERTB"),
+	PINCTRL_PIN(51, "SML0CLK"),
+	PINCTRL_PIN(52, "SML0DATA"),
+	PINCTRL_PIN(53, "SML0ALERTB"),
+	PINCTRL_PIN(54, "SML1CLK"),
+	PINCTRL_PIN(55, "SML1DATA"),
+	PINCTRL_PIN(56, "UART0_RXD"),
+	PINCTRL_PIN(57, "UART0_TXD"),
+	PINCTRL_PIN(58, "UART0_RTSB"),
+	PINCTRL_PIN(59, "UART0_CTSB"),
+	PINCTRL_PIN(60, "UART1_RXD"),
+	PINCTRL_PIN(61, "UART1_TXD"),
+	PINCTRL_PIN(62, "UART1_RTSB"),
+	PINCTRL_PIN(63, "UART1_CTSB"),
+	PINCTRL_PIN(64, "I2C0_SDA"),
+	PINCTRL_PIN(65, "I2C0_SCL"),
+	PINCTRL_PIN(66, "I2C1_SDA"),
+	PINCTRL_PIN(67, "I2C1_SCL"),
+	PINCTRL_PIN(68, "UART2_RXD"),
+	PINCTRL_PIN(69, "UART2_TXD"),
+	PINCTRL_PIN(70, "UART2_RTSB"),
+	PINCTRL_PIN(71, "UART2_CTSB"),
+	/* GPP_D */
+	PINCTRL_PIN(72, "SPI1_CSB"),
+	PINCTRL_PIN(73, "SPI1_CLK"),
+	PINCTRL_PIN(74, "SPI1_MISO_IO_1"),
+	PINCTRL_PIN(75, "SPI1_MOSI_IO_0"),
+	PINCTRL_PIN(76, "ISH_I2C2_SDA"),
+	PINCTRL_PIN(77, "SSP0_SFRM"),
+	PINCTRL_PIN(78, "SSP0_TXD"),
+	PINCTRL_PIN(79, "SSP0_RXD"),
+	PINCTRL_PIN(80, "SSP0_SCLK"),
+	PINCTRL_PIN(81, "ISH_SPI_CSB"),
+	PINCTRL_PIN(82, "ISH_SPI_CLK"),
+	PINCTRL_PIN(83, "ISH_SPI_MISO"),
+	PINCTRL_PIN(84, "ISH_SPI_MOSI"),
+	PINCTRL_PIN(85, "ISH_UART0_RXD"),
+	PINCTRL_PIN(86, "ISH_UART0_TXD"),
+	PINCTRL_PIN(87, "ISH_UART0_RTSB"),
+	PINCTRL_PIN(88, "ISH_UART0_CTSB"),
+	PINCTRL_PIN(89, "DMIC_CLK_1"),
+	PINCTRL_PIN(90, "DMIC_DATA_1"),
+	PINCTRL_PIN(91, "DMIC_CLK_0"),
+	PINCTRL_PIN(92, "DMIC_DATA_0"),
+	PINCTRL_PIN(93, "SPI1_IO_2"),
+	PINCTRL_PIN(94, "SPI1_IO_3"),
+	PINCTRL_PIN(95, "ISH_I2C2_SCL"),
+	/* GPP_E */
+	PINCTRL_PIN(96, "SATAXPCIE_0"),
+	PINCTRL_PIN(97, "SATAXPCIE_1"),
+	PINCTRL_PIN(98, "SATAXPCIE_2"),
+	PINCTRL_PIN(99, "CPU_GP_0"),
+	PINCTRL_PIN(100, "SATA_DEVSLP_0"),
+	PINCTRL_PIN(101, "SATA_DEVSLP_1"),
+	PINCTRL_PIN(102, "SATA_DEVSLP_2"),
+	PINCTRL_PIN(103, "CPU_GP_1"),
+	PINCTRL_PIN(104, "SATA_LEDB"),
+	PINCTRL_PIN(105, "USB2_OCB_0"),
+	PINCTRL_PIN(106, "USB2_OCB_1"),
+	PINCTRL_PIN(107, "USB2_OCB_2"),
+	PINCTRL_PIN(108, "USB2_OCB_3"),
+	/* GPP_F */
+	PINCTRL_PIN(109, "SATAXPCIE_3"),
+	PINCTRL_PIN(110, "SATAXPCIE_4"),
+	PINCTRL_PIN(111, "SATAXPCIE_5"),
+	PINCTRL_PIN(112, "SATAXPCIE_6"),
+	PINCTRL_PIN(113, "SATAXPCIE_7"),
+	PINCTRL_PIN(114, "SATA_DEVSLP_3"),
+	PINCTRL_PIN(115, "SATA_DEVSLP_4"),
+	PINCTRL_PIN(116, "SATA_DEVSLP_5"),
+	PINCTRL_PIN(117, "SATA_DEVSLP_6"),
+	PINCTRL_PIN(118, "SATA_DEVSLP_7"),
+	PINCTRL_PIN(119, "SATA_SCLOCK"),
+	PINCTRL_PIN(120, "SATA_SLOAD"),
+	PINCTRL_PIN(121, "SATA_SDATAOUT1"),
+	PINCTRL_PIN(122, "SATA_SDATAOUT0"),
+	PINCTRL_PIN(123, "GPP_F_14"),
+	PINCTRL_PIN(124, "USB_OCB_4"),
+	PINCTRL_PIN(125, "USB_OCB_5"),
+	PINCTRL_PIN(126, "USB_OCB_6"),
+	PINCTRL_PIN(127, "USB_OCB_7"),
+	PINCTRL_PIN(128, "L_VDDEN"),
+	PINCTRL_PIN(129, "L_BKLTEN"),
+	PINCTRL_PIN(130, "L_BKLTCTL"),
+	PINCTRL_PIN(131, "GPP_F_22"),
+	PINCTRL_PIN(132, "GPP_F_23"),
+	/* GPP_G */
+	PINCTRL_PIN(133, "FAN_TACH_0"),
+	PINCTRL_PIN(134, "FAN_TACH_1"),
+	PINCTRL_PIN(135, "FAN_TACH_2"),
+	PINCTRL_PIN(136, "FAN_TACH_3"),
+	PINCTRL_PIN(137, "FAN_TACH_4"),
+	PINCTRL_PIN(138, "FAN_TACH_5"),
+	PINCTRL_PIN(139, "FAN_TACH_6"),
+	PINCTRL_PIN(140, "FAN_TACH_7"),
+	PINCTRL_PIN(141, "FAN_PWM_0"),
+	PINCTRL_PIN(142, "FAN_PWM_1"),
+	PINCTRL_PIN(143, "FAN_PWM_2"),
+	PINCTRL_PIN(144, "FAN_PWM_3"),
+	PINCTRL_PIN(145, "GSXDOUT"),
+	PINCTRL_PIN(146, "GSXSLOAD"),
+	PINCTRL_PIN(147, "GSXDIN"),
+	PINCTRL_PIN(148, "GSXRESETB"),
+	PINCTRL_PIN(149, "GSXCLK"),
+	PINCTRL_PIN(150, "ADR_COMPLETE"),
+	PINCTRL_PIN(151, "NMIB"),
+	PINCTRL_PIN(152, "SMIB"),
+	PINCTRL_PIN(153, "GPP_G_20"),
+	PINCTRL_PIN(154, "GPP_G_21"),
+	PINCTRL_PIN(155, "GPP_G_22"),
+	PINCTRL_PIN(156, "GPP_G_23"),
+	/* GPP_H */
+	PINCTRL_PIN(157, "SRCCLKREQB_6"),
+	PINCTRL_PIN(158, "SRCCLKREQB_7"),
+	PINCTRL_PIN(159, "SRCCLKREQB_8"),
+	PINCTRL_PIN(160, "SRCCLKREQB_9"),
+	PINCTRL_PIN(161, "SRCCLKREQB_10"),
+	PINCTRL_PIN(162, "SRCCLKREQB_11"),
+	PINCTRL_PIN(163, "SRCCLKREQB_12"),
+	PINCTRL_PIN(164, "SRCCLKREQB_13"),
+	PINCTRL_PIN(165, "SRCCLKREQB_14"),
+	PINCTRL_PIN(166, "SRCCLKREQB_15"),
+	PINCTRL_PIN(167, "SML2CLK"),
+	PINCTRL_PIN(168, "SML2DATA"),
+	PINCTRL_PIN(169, "SML2ALERTB"),
+	PINCTRL_PIN(170, "SML3CLK"),
+	PINCTRL_PIN(171, "SML3DATA"),
+	PINCTRL_PIN(172, "SML3ALERTB"),
+	PINCTRL_PIN(173, "SML4CLK"),
+	PINCTRL_PIN(174, "SML4DATA"),
+	PINCTRL_PIN(175, "SML4ALERTB"),
+	PINCTRL_PIN(176, "ISH_I2C0_SDA"),
+	PINCTRL_PIN(177, "ISH_I2C0_SCL"),
+	PINCTRL_PIN(178, "ISH_I2C1_SDA"),
+	PINCTRL_PIN(179, "ISH_I2C1_SCL"),
+	PINCTRL_PIN(180, "GPP_H_23"),
+	/* GPP_I */
+	PINCTRL_PIN(181, "DDSP_HDP_0"),
+	PINCTRL_PIN(182, "DDSP_HDP_1"),
+	PINCTRL_PIN(183, "DDSP_HDP_2"),
+	PINCTRL_PIN(184, "DDSP_HDP_3"),
+	PINCTRL_PIN(185, "EDP_HPD"),
+	PINCTRL_PIN(186, "DDPB_CTRLCLK"),
+	PINCTRL_PIN(187, "DDPB_CTRLDATA"),
+	PINCTRL_PIN(188, "DDPC_CTRLCLK"),
+	PINCTRL_PIN(189, "DDPC_CTRLDATA"),
+	PINCTRL_PIN(190, "DDPD_CTRLCLK"),
+	PINCTRL_PIN(191, "DDPD_CTRLDATA"),
+};
+
+static const unsigned spth_spi0_pins[] = { 39, 40, 41, 42 };
+static const unsigned spth_spi1_pins[] = { 43, 44, 45, 46 };
+static const unsigned spth_uart0_pins[] = { 56, 57, 58, 59 };
+static const unsigned spth_uart1_pins[] = { 60, 61, 62, 63 };
+static const unsigned spth_uart2_pins[] = { 68, 69, 71, 71 };
+static const unsigned spth_i2c0_pins[] = { 64, 65 };
+static const unsigned spth_i2c1_pins[] = { 66, 67 };
+static const unsigned spth_i2c2_pins[] = { 76, 95 };
+
+static const struct intel_pingroup spth_groups[] = {
+	PIN_GROUP("spi0_grp", spth_spi0_pins, 1),
+	PIN_GROUP("spi1_grp", spth_spi1_pins, 1),
+	PIN_GROUP("uart0_grp", spth_uart0_pins, 1),
+	PIN_GROUP("uart1_grp", spth_uart1_pins, 1),
+	PIN_GROUP("uart2_grp", spth_uart2_pins, 1),
+	PIN_GROUP("i2c0_grp", spth_i2c0_pins, 1),
+	PIN_GROUP("i2c1_grp", spth_i2c1_pins, 1),
+	PIN_GROUP("i2c2_grp", spth_i2c2_pins, 2),
+};
+
+static const char * const spth_spi0_groups[] = { "spi0_grp" };
+static const char * const spth_spi1_groups[] = { "spi0_grp" };
+static const char * const spth_uart0_groups[] = { "uart0_grp" };
+static const char * const spth_uart1_groups[] = { "uart1_grp" };
+static const char * const spth_uart2_groups[] = { "uart2_grp" };
+static const char * const spth_i2c0_groups[] = { "i2c0_grp" };
+static const char * const spth_i2c1_groups[] = { "i2c1_grp" };
+static const char * const spth_i2c2_groups[] = { "i2c2_grp" };
+
+static const struct intel_function spth_functions[] = {
+	FUNCTION("spi0", spth_spi0_groups),
+	FUNCTION("spi1", spth_spi1_groups),
+	FUNCTION("uart0", spth_uart0_groups),
+	FUNCTION("uart1", spth_uart1_groups),
+	FUNCTION("uart2", spth_uart2_groups),
+	FUNCTION("i2c0", spth_i2c0_groups),
+	FUNCTION("i2c1", spth_i2c1_groups),
+	FUNCTION("i2c2", spth_i2c2_groups),
+};
+
+static const struct intel_community spth_communities[] = {
+	SPT_COMMUNITY(0, 0, 47),
+	SPT_COMMUNITY(1, 48, 180),
+	SPT_COMMUNITY(2, 181, 191),
+};
+
+static const struct intel_pinctrl_soc_data spth_soc_data = {
+	.pins = spth_pins,
+	.npins = ARRAY_SIZE(spth_pins),
+	.groups = spth_groups,
+	.ngroups = ARRAY_SIZE(spth_groups),
+	.functions = spth_functions,
+	.nfunctions = ARRAY_SIZE(spth_functions),
+	.communities = spth_communities,
+	.ncommunities = ARRAY_SIZE(spth_communities),
+};
+
 static const struct acpi_device_id spt_pinctrl_acpi_match[] = {
 	{ "INT344B", (kernel_ulong_t)&sptlp_soc_data },
+	{ "INT345D", (kernel_ulong_t)&spth_soc_data },
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, spt_pinctrl_acpi_match);
diff --git a/drivers/pinctrl/mediatek/Kconfig b/drivers/pinctrl/mediatek/Kconfig
index 6b3551cad111..02f6f92df86c 100644
--- a/drivers/pinctrl/mediatek/Kconfig
+++ b/drivers/pinctrl/mediatek/Kconfig
@@ -15,6 +15,12 @@ config PINCTRL_MT8135
 	default MACH_MT8135
 	select PINCTRL_MTK_COMMON
 
+config PINCTRL_MT8127
+	bool "Mediatek MT8127 pin control" if COMPILE_TEST && !MACH_MT8127
+	depends on OF
+	default MACH_MT8127
+	select PINCTRL_MTK_COMMON
+
 # For ARMv8 SoCs
 config PINCTRL_MT8173
 	bool "Mediatek MT8173 pin control"
@@ -23,4 +29,11 @@ config PINCTRL_MT8173
 	default ARM64 && ARCH_MEDIATEK
 	select PINCTRL_MTK_COMMON
 
+# For PMIC
+config PINCTRL_MT6397
+	bool "Mediatek MT6397 pin control" if COMPILE_TEST && !MFD_MT6397
+	depends on OF
+	default MFD_MT6397
+	select PINCTRL_MTK_COMMON
+
 endif
diff --git a/drivers/pinctrl/mediatek/Makefile b/drivers/pinctrl/mediatek/Makefile
index d8606a2179cf..eb923d64d387 100644
--- a/drivers/pinctrl/mediatek/Makefile
+++ b/drivers/pinctrl/mediatek/Makefile
@@ -3,4 +3,6 @@ obj-$(CONFIG_PINCTRL_MTK_COMMON)	+= pinctrl-mtk-common.o
 
 # SoC Drivers
 obj-$(CONFIG_PINCTRL_MT8135)		+= pinctrl-mt8135.o
+obj-$(CONFIG_PINCTRL_MT8127)		+= pinctrl-mt8127.o
 obj-$(CONFIG_PINCTRL_MT8173)		+= pinctrl-mt8173.o
+obj-$(CONFIG_PINCTRL_MT6397)		+= pinctrl-mt6397.o
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt6397.c b/drivers/pinctrl/mediatek/pinctrl-mt6397.c
new file mode 100644
index 000000000000..f9751ae28e32
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mt6397.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2015 MediaTek Inc.
+ * Author: Hongzhou.Yang <hongzhou.yang@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/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/mfd/mt6397/core.h>
+
+#include "pinctrl-mtk-common.h"
+#include "pinctrl-mtk-mt6397.h"
+
+#define MT6397_PIN_REG_BASE  0xc000
+
+static const struct mtk_pinctrl_devdata mt6397_pinctrl_data = {
+	.pins = mtk_pins_mt6397,
+	.npins = ARRAY_SIZE(mtk_pins_mt6397),
+	.dir_offset = (MT6397_PIN_REG_BASE + 0x000),
+	.ies_offset = MTK_PINCTRL_NOT_SUPPORT,
+	.smt_offset = MTK_PINCTRL_NOT_SUPPORT,
+	.pullen_offset = (MT6397_PIN_REG_BASE + 0x020),
+	.pullsel_offset = (MT6397_PIN_REG_BASE + 0x040),
+	.dout_offset = (MT6397_PIN_REG_BASE + 0x080),
+	.din_offset = (MT6397_PIN_REG_BASE + 0x0a0),
+	.pinmux_offset = (MT6397_PIN_REG_BASE + 0x0c0),
+	.type1_start = 41,
+	.type1_end = 41,
+	.port_shf = 3,
+	.port_mask = 0x3,
+	.port_align = 2,
+};
+
+static int mt6397_pinctrl_probe(struct platform_device *pdev)
+{
+	struct mt6397_chip *mt6397;
+
+	mt6397 = dev_get_drvdata(pdev->dev.parent);
+	return mtk_pctrl_init(pdev, &mt6397_pinctrl_data, mt6397->regmap);
+}
+
+static const struct of_device_id mt6397_pctrl_match[] = {
+	{ .compatible = "mediatek,mt6397-pinctrl", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mt6397_pctrl_match);
+
+static struct platform_driver mtk_pinctrl_driver = {
+	.probe = mt6397_pinctrl_probe,
+	.driver = {
+		.name = "mediatek-mt6397-pinctrl",
+		.of_match_table = mt6397_pctrl_match,
+	},
+};
+
+static int __init mtk_pinctrl_init(void)
+{
+	return platform_driver_register(&mtk_pinctrl_driver);
+}
+
+module_init(mtk_pinctrl_init);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MediaTek MT6397 Pinctrl Driver");
+MODULE_AUTHOR("Hongzhou Yang <hongzhou.yang@mediatek.com>");
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt8127.c b/drivers/pinctrl/mediatek/pinctrl-mt8127.c
new file mode 100644
index 000000000000..b317b0b664ea
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mt8127.c
@@ -0,0 +1,358 @@
+/*
+ * Copyright (c) 2015 MediaTek Inc.
+ * Author: Hongzhou.Yang <hongzhou.yang@mediatek.com>
+ *         Yingjoe Chen <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/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/regmap.h>
+#include <dt-bindings/pinctrl/mt65xx.h>
+
+#include "pinctrl-mtk-common.h"
+#include "pinctrl-mtk-mt8127.h"
+
+static const struct mtk_drv_group_desc mt8127_drv_grp[] =  {
+	/* 0E4E8SR 4/8/12/16 */
+	MTK_DRV_GRP(4, 16, 1, 2, 4),
+	/* 0E2E4SR  2/4/6/8 */
+	MTK_DRV_GRP(2, 8, 1, 2, 2),
+	/* E8E4E2  2/4/6/8/10/12/14/16 */
+	MTK_DRV_GRP(2, 16, 0, 2, 2)
+};
+
+static const struct mtk_pin_drv_grp mt8127_pin_drv[] = {
+	MTK_PIN_DRV_GRP(0,   0xb00,  0, 1),
+	MTK_PIN_DRV_GRP(1,   0xb00,  0, 1),
+	MTK_PIN_DRV_GRP(2,   0xb00,  0, 1),
+	MTK_PIN_DRV_GRP(3,   0xb00,  0, 1),
+	MTK_PIN_DRV_GRP(4,   0xb00,  0, 1),
+	MTK_PIN_DRV_GRP(5,   0xb00,  0, 1),
+	MTK_PIN_DRV_GRP(6,   0xb00,  0, 1),
+	MTK_PIN_DRV_GRP(7,   0xb00, 12, 1),
+	MTK_PIN_DRV_GRP(8,   0xb00, 12, 1),
+	MTK_PIN_DRV_GRP(9,   0xb00, 12, 1),
+	MTK_PIN_DRV_GRP(10,  0xb00,  8, 1),
+	MTK_PIN_DRV_GRP(11,  0xb00,  8, 1),
+	MTK_PIN_DRV_GRP(12,  0xb00,  8, 1),
+	MTK_PIN_DRV_GRP(13,  0xb00,  8, 1),
+	MTK_PIN_DRV_GRP(14,  0xb10,  4, 0),
+	MTK_PIN_DRV_GRP(15,  0xb10,  4, 0),
+	MTK_PIN_DRV_GRP(16,  0xb10,  4, 0),
+	MTK_PIN_DRV_GRP(17,  0xb10,  4, 0),
+	MTK_PIN_DRV_GRP(18,  0xb10,  8, 0),
+	MTK_PIN_DRV_GRP(19,  0xb10,  8, 0),
+	MTK_PIN_DRV_GRP(20,  0xb10,  8, 0),
+	MTK_PIN_DRV_GRP(21,  0xb10,  8, 0),
+	MTK_PIN_DRV_GRP(22,  0xb20,  0, 0),
+	MTK_PIN_DRV_GRP(23,  0xb20,  0, 0),
+	MTK_PIN_DRV_GRP(24,  0xb20,  0, 0),
+	MTK_PIN_DRV_GRP(25,  0xb20,  0, 0),
+	MTK_PIN_DRV_GRP(26,  0xb20,  0, 0),
+	MTK_PIN_DRV_GRP(27,  0xb20,  4, 0),
+	MTK_PIN_DRV_GRP(28,  0xb20,  4, 0),
+	MTK_PIN_DRV_GRP(29,  0xb20,  4, 0),
+	MTK_PIN_DRV_GRP(30,  0xb20,  4, 0),
+	MTK_PIN_DRV_GRP(31,  0xb20,  4, 0),
+	MTK_PIN_DRV_GRP(32,  0xb20,  4, 0),
+	MTK_PIN_DRV_GRP(33,  0xb30,  4, 1),
+	MTK_PIN_DRV_GRP(34,  0xb30,  8, 1),
+	MTK_PIN_DRV_GRP(35,  0xb30,  8, 1),
+	MTK_PIN_DRV_GRP(36,  0xb30,  8, 1),
+	MTK_PIN_DRV_GRP(37,  0xb30,  8, 1),
+	MTK_PIN_DRV_GRP(38,  0xb30,  8, 1),
+	MTK_PIN_DRV_GRP(39,  0xb30, 12, 1),
+	MTK_PIN_DRV_GRP(40,  0xb30, 12, 1),
+	MTK_PIN_DRV_GRP(41,  0xb30, 12, 1),
+	MTK_PIN_DRV_GRP(42,  0xb30, 12, 1),
+	MTK_PIN_DRV_GRP(43,  0xb40, 12, 0),
+	MTK_PIN_DRV_GRP(44,  0xb40, 12, 0),
+	MTK_PIN_DRV_GRP(45,  0xb40, 12, 0),
+	MTK_PIN_DRV_GRP(46,  0xb50,  0, 2),
+	MTK_PIN_DRV_GRP(47,  0xb50,  0, 2),
+	MTK_PIN_DRV_GRP(48,  0xb50,  0, 2),
+	MTK_PIN_DRV_GRP(49,  0xb50,  0, 2),
+	MTK_PIN_DRV_GRP(50,  0xb70,  0, 1),
+	MTK_PIN_DRV_GRP(51,  0xb70,  0, 1),
+	MTK_PIN_DRV_GRP(52,  0xb70,  0, 1),
+	MTK_PIN_DRV_GRP(53,  0xb50, 12, 1),
+	MTK_PIN_DRV_GRP(54,  0xb50, 12, 1),
+	MTK_PIN_DRV_GRP(55,  0xb50, 12, 1),
+	MTK_PIN_DRV_GRP(56,  0xb50, 12, 1),
+	MTK_PIN_DRV_GRP(59,  0xb40,  4, 1),
+	MTK_PIN_DRV_GRP(60,  0xb40,  0, 1),
+	MTK_PIN_DRV_GRP(61,  0xb40,  0, 1),
+	MTK_PIN_DRV_GRP(62,  0xb40,  0, 1),
+	MTK_PIN_DRV_GRP(63,  0xb40,  4, 1),
+	MTK_PIN_DRV_GRP(64,  0xb40,  4, 1),
+	MTK_PIN_DRV_GRP(65,  0xb40,  4, 1),
+	MTK_PIN_DRV_GRP(66,  0xb40,  8, 1),
+	MTK_PIN_DRV_GRP(67,  0xb40,  8, 1),
+	MTK_PIN_DRV_GRP(68,  0xb40,  8, 1),
+	MTK_PIN_DRV_GRP(69,  0xb40,  8, 1),
+	MTK_PIN_DRV_GRP(70,  0xb40,  8, 1),
+	MTK_PIN_DRV_GRP(71,  0xb40,  8, 1),
+	MTK_PIN_DRV_GRP(72,  0xb50,  4, 1),
+	MTK_PIN_DRV_GRP(73,  0xb50,  4, 1),
+	MTK_PIN_DRV_GRP(74,  0xb50,  4, 1),
+	MTK_PIN_DRV_GRP(79,  0xb50,  8, 1),
+	MTK_PIN_DRV_GRP(80,  0xb50,  8, 1),
+	MTK_PIN_DRV_GRP(81,  0xb50,  8, 1),
+	MTK_PIN_DRV_GRP(82,  0xb50,  8, 1),
+	MTK_PIN_DRV_GRP(83,  0xb50,  8, 1),
+	MTK_PIN_DRV_GRP(84,  0xb50,  8, 1),
+	MTK_PIN_DRV_GRP(85,  0xce0,  0, 2),
+	MTK_PIN_DRV_GRP(86,  0xcd0,  0, 2),
+	MTK_PIN_DRV_GRP(87,  0xcf0,  0, 2),
+	MTK_PIN_DRV_GRP(88,  0xcf0,  0, 2),
+	MTK_PIN_DRV_GRP(89,  0xcf0,  0, 2),
+	MTK_PIN_DRV_GRP(90,  0xcf0,  0, 2),
+	MTK_PIN_DRV_GRP(117, 0xb60, 12, 1),
+	MTK_PIN_DRV_GRP(118, 0xb60, 12, 1),
+	MTK_PIN_DRV_GRP(119, 0xb60, 12, 1),
+	MTK_PIN_DRV_GRP(120, 0xb60, 12, 1),
+	MTK_PIN_DRV_GRP(121, 0xc80,  0, 2),
+	MTK_PIN_DRV_GRP(122, 0xc70,  0, 2),
+	MTK_PIN_DRV_GRP(123, 0xc90,  0, 2),
+	MTK_PIN_DRV_GRP(124, 0xc90,  0, 2),
+	MTK_PIN_DRV_GRP(125, 0xc90,  0, 2),
+	MTK_PIN_DRV_GRP(126, 0xc90,  0, 2),
+	MTK_PIN_DRV_GRP(127, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(128, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(129, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(130, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(131, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(132, 0xc10,  0, 2),
+	MTK_PIN_DRV_GRP(133, 0xc00,  0, 2),
+	MTK_PIN_DRV_GRP(134, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(135, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(136, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(137, 0xc20,  0, 2),
+	MTK_PIN_DRV_GRP(142, 0xb50,  0, 2),
+};
+
+static const struct mtk_pin_spec_pupd_set_samereg mt8127_spec_pupd[] = {
+	MTK_PIN_PUPD_SPEC_SR(33,  0xd90, 2, 0, 1),	/* KPROW0 */
+	MTK_PIN_PUPD_SPEC_SR(34,  0xd90, 6, 4, 5),	/* KPROW1 */
+	MTK_PIN_PUPD_SPEC_SR(35,  0xd90, 10, 8, 9),	/* KPROW2 */
+	MTK_PIN_PUPD_SPEC_SR(36,  0xda0, 2, 0, 1),	/* KPCOL0 */
+	MTK_PIN_PUPD_SPEC_SR(37,  0xda0, 6, 4, 5),	/* KPCOL1 */
+	MTK_PIN_PUPD_SPEC_SR(38,  0xda0, 10, 8, 9),	/* KPCOL2 */
+	MTK_PIN_PUPD_SPEC_SR(46,  0xdb0, 2, 0, 1),	/* EINT14 */
+	MTK_PIN_PUPD_SPEC_SR(47,  0xdb0, 6, 4, 5),	/* EINT15 */
+	MTK_PIN_PUPD_SPEC_SR(48,  0xdb0, 10, 8, 9),	/* EINT16 */
+	MTK_PIN_PUPD_SPEC_SR(49,  0xdb0, 14, 12, 13),	/* EINT17 */
+	MTK_PIN_PUPD_SPEC_SR(85,  0xce0, 8, 10, 9),	/* MSDC2_CMD */
+	MTK_PIN_PUPD_SPEC_SR(86,  0xcd0, 8, 10, 9),	/* MSDC2_CLK */
+	MTK_PIN_PUPD_SPEC_SR(87,  0xd00, 0, 2, 1),	/* MSDC2_DAT0 */
+	MTK_PIN_PUPD_SPEC_SR(88,  0xd00, 4, 6, 5),	/* MSDC2_DAT1 */
+	MTK_PIN_PUPD_SPEC_SR(89,  0xd00, 8, 10, 9),	/* MSDC2_DAT2 */
+	MTK_PIN_PUPD_SPEC_SR(90,  0xd00, 12, 14, 13),	/* MSDC2_DAT3 */
+	MTK_PIN_PUPD_SPEC_SR(121, 0xc80, 8, 10, 9),	/* MSDC1_CMD */
+	MTK_PIN_PUPD_SPEC_SR(122, 0xc70, 8, 10, 9),	/* MSDC1_CLK */
+	MTK_PIN_PUPD_SPEC_SR(123, 0xca0, 0, 2, 1),	/* MSDC1_DAT0 */
+	MTK_PIN_PUPD_SPEC_SR(124, 0xca0, 4, 6, 5),	/* MSDC1_DAT1 */
+	MTK_PIN_PUPD_SPEC_SR(125, 0xca0, 8, 10, 9),	/* MSDC1_DAT2 */
+	MTK_PIN_PUPD_SPEC_SR(126, 0xca0, 12, 14, 13),	/* MSDC1_DAT3 */
+	MTK_PIN_PUPD_SPEC_SR(127, 0xc40, 12, 14, 13),	/* MSDC0_DAT7 */
+	MTK_PIN_PUPD_SPEC_SR(128, 0xc40, 8, 10, 9),	/* MSDC0_DAT6 */
+	MTK_PIN_PUPD_SPEC_SR(129, 0xc40, 4, 6, 5),	/* MSDC0_DAT5 */
+	MTK_PIN_PUPD_SPEC_SR(130, 0xc40, 0, 2, 1),	/* MSDC0_DAT4 */
+	MTK_PIN_PUPD_SPEC_SR(131, 0xc50, 0, 2, 1),	/* MSDC0_RSTB */
+	MTK_PIN_PUPD_SPEC_SR(132, 0xc10, 8, 10, 9),	/* MSDC0_CMD */
+	MTK_PIN_PUPD_SPEC_SR(133, 0xc00, 8, 10, 9),	/* MSDC0_CLK */
+	MTK_PIN_PUPD_SPEC_SR(134, 0xc30, 12, 14, 13),	/* MSDC0_DAT3 */
+	MTK_PIN_PUPD_SPEC_SR(135, 0xc30, 8, 10, 9),	/* MSDC0_DAT2 */
+	MTK_PIN_PUPD_SPEC_SR(136, 0xc30, 4, 6, 5),	/* MSDC0_DAT1 */
+	MTK_PIN_PUPD_SPEC_SR(137, 0xc30, 0, 2, 1),	/* MSDC0_DAT0 */
+	MTK_PIN_PUPD_SPEC_SR(142, 0xdc0, 2, 0, 1),	/* EINT21 */
+};
+
+static int mt8127_spec_pull_set(struct regmap *regmap, unsigned int pin,
+		unsigned char align, bool isup, unsigned int r1r0)
+{
+	return mtk_pctrl_spec_pull_set_samereg(regmap, mt8127_spec_pupd,
+		ARRAY_SIZE(mt8127_spec_pupd), pin, align, isup, r1r0);
+}
+
+static const struct mtk_pin_ies_smt_set mt8127_ies_set[] = {
+	MTK_PIN_IES_SMT_SPEC(0, 9, 0x900, 0),
+	MTK_PIN_IES_SMT_SPEC(10, 13, 0x900, 1),
+	MTK_PIN_IES_SMT_SPEC(14, 28, 0x900, 2),
+	MTK_PIN_IES_SMT_SPEC(29, 32, 0x900, 3),
+	MTK_PIN_IES_SMT_SPEC(33, 33, 0x910, 11),
+	MTK_PIN_IES_SMT_SPEC(34, 38, 0x900, 10),
+	MTK_PIN_IES_SMT_SPEC(39, 42, 0x900, 11),
+	MTK_PIN_IES_SMT_SPEC(43, 45, 0x900, 12),
+	MTK_PIN_IES_SMT_SPEC(46, 49, 0x900, 13),
+	MTK_PIN_IES_SMT_SPEC(50, 52, 0x910, 10),
+	MTK_PIN_IES_SMT_SPEC(53, 56, 0x900, 14),
+	MTK_PIN_IES_SMT_SPEC(57, 58, 0x910, 0),
+	MTK_PIN_IES_SMT_SPEC(59, 65, 0x910, 2),
+	MTK_PIN_IES_SMT_SPEC(66, 71, 0x910, 3),
+	MTK_PIN_IES_SMT_SPEC(72, 74, 0x910, 4),
+	MTK_PIN_IES_SMT_SPEC(75, 76, 0x900, 15),
+	MTK_PIN_IES_SMT_SPEC(77, 78, 0x910, 1),
+	MTK_PIN_IES_SMT_SPEC(79, 82, 0x910, 5),
+	MTK_PIN_IES_SMT_SPEC(83, 84, 0x910, 6),
+	MTK_PIN_IES_SMT_SPEC(117, 120, 0x910, 7),
+	MTK_PIN_IES_SMT_SPEC(121, 121, 0xc80, 4),
+	MTK_PIN_IES_SMT_SPEC(122, 122, 0xc70, 4),
+	MTK_PIN_IES_SMT_SPEC(123, 126, 0xc90, 4),
+	MTK_PIN_IES_SMT_SPEC(127, 131, 0xc20, 4),
+	MTK_PIN_IES_SMT_SPEC(132, 132, 0xc10, 4),
+	MTK_PIN_IES_SMT_SPEC(133, 133, 0xc00, 4),
+	MTK_PIN_IES_SMT_SPEC(134, 137, 0xc20, 4),
+	MTK_PIN_IES_SMT_SPEC(138, 141, 0x910, 9),
+	MTK_PIN_IES_SMT_SPEC(142, 142, 0x900, 13),
+};
+
+static const struct mtk_pin_ies_smt_set mt8127_smt_set[] = {
+	MTK_PIN_IES_SMT_SPEC(0, 9, 0x920, 0),
+	MTK_PIN_IES_SMT_SPEC(10, 13, 0x920, 1),
+	MTK_PIN_IES_SMT_SPEC(14, 28, 0x920, 2),
+	MTK_PIN_IES_SMT_SPEC(29, 32, 0x920, 3),
+	MTK_PIN_IES_SMT_SPEC(33, 33, 0x930, 11),
+	MTK_PIN_IES_SMT_SPEC(34, 38, 0x920, 10),
+	MTK_PIN_IES_SMT_SPEC(39, 42, 0x920, 11),
+	MTK_PIN_IES_SMT_SPEC(43, 45, 0x920, 12),
+	MTK_PIN_IES_SMT_SPEC(46, 49, 0x920, 13),
+	MTK_PIN_IES_SMT_SPEC(50, 52, 0x930, 10),
+	MTK_PIN_IES_SMT_SPEC(53, 56, 0x920, 14),
+	MTK_PIN_IES_SMT_SPEC(57, 58, 0x930, 0),
+	MTK_PIN_IES_SMT_SPEC(59, 65, 0x930, 2),
+	MTK_PIN_IES_SMT_SPEC(66, 71, 0x930, 3),
+	MTK_PIN_IES_SMT_SPEC(72, 74, 0x930, 4),
+	MTK_PIN_IES_SMT_SPEC(75, 76, 0x920, 15),
+	MTK_PIN_IES_SMT_SPEC(77, 78, 0x930, 1),
+	MTK_PIN_IES_SMT_SPEC(79, 82, 0x930, 5),
+	MTK_PIN_IES_SMT_SPEC(83, 84, 0x930, 6),
+	MTK_PIN_IES_SMT_SPEC(85, 85, 0xce0, 11),
+	MTK_PIN_IES_SMT_SPEC(86, 86, 0xcd0, 11),
+	MTK_PIN_IES_SMT_SPEC(87, 87, 0xd00, 3),
+	MTK_PIN_IES_SMT_SPEC(88, 88, 0xd00, 7),
+	MTK_PIN_IES_SMT_SPEC(89, 89, 0xd00, 11),
+	MTK_PIN_IES_SMT_SPEC(90, 90, 0xd00, 15),
+	MTK_PIN_IES_SMT_SPEC(117, 120, 0x930, 7),
+	MTK_PIN_IES_SMT_SPEC(121, 121, 0xc80, 11),
+	MTK_PIN_IES_SMT_SPEC(122, 122, 0xc70, 11),
+	MTK_PIN_IES_SMT_SPEC(123, 123, 0xca0, 3),
+	MTK_PIN_IES_SMT_SPEC(124, 124, 0xca0, 7),
+	MTK_PIN_IES_SMT_SPEC(125, 125, 0xca0, 11),
+	MTK_PIN_IES_SMT_SPEC(126, 126, 0xca0, 15),
+	MTK_PIN_IES_SMT_SPEC(127, 127, 0xc40, 15),
+	MTK_PIN_IES_SMT_SPEC(128, 128, 0xc40, 11),
+	MTK_PIN_IES_SMT_SPEC(129, 129, 0xc40, 7),
+	MTK_PIN_IES_SMT_SPEC(130, 130, 0xc40, 3),
+	MTK_PIN_IES_SMT_SPEC(131, 131, 0xc50, 3),
+	MTK_PIN_IES_SMT_SPEC(132, 132, 0xc10, 11),
+	MTK_PIN_IES_SMT_SPEC(133, 133, 0xc00, 11),
+	MTK_PIN_IES_SMT_SPEC(134, 134, 0xc30, 15),
+	MTK_PIN_IES_SMT_SPEC(135, 135, 0xc30, 11),
+	MTK_PIN_IES_SMT_SPEC(136, 136, 0xc30, 7),
+	MTK_PIN_IES_SMT_SPEC(137, 137, 0xc30, 3),
+	MTK_PIN_IES_SMT_SPEC(138, 141, 0x930, 9),
+	MTK_PIN_IES_SMT_SPEC(142, 142, 0x920, 13),
+};
+
+static int mt8127_ies_smt_set(struct regmap *regmap, unsigned int pin,
+		unsigned char align, int value, enum pin_config_param arg)
+{
+	if (arg == PIN_CONFIG_INPUT_ENABLE)
+		return mtk_pconf_spec_set_ies_smt_range(regmap, mt8127_ies_set,
+			ARRAY_SIZE(mt8127_ies_set), pin, align, value);
+	else if (arg == PIN_CONFIG_INPUT_SCHMITT_ENABLE)
+		return mtk_pconf_spec_set_ies_smt_range(regmap, mt8127_smt_set,
+			ARRAY_SIZE(mt8127_smt_set), pin, align, value);
+	return -EINVAL;
+}
+
+
+static const struct mtk_pinctrl_devdata mt8127_pinctrl_data = {
+	.pins = mtk_pins_mt8127,
+	.npins = ARRAY_SIZE(mtk_pins_mt8127),
+	.grp_desc = mt8127_drv_grp,
+	.n_grp_cls = ARRAY_SIZE(mt8127_drv_grp),
+	.pin_drv_grp = mt8127_pin_drv,
+	.n_pin_drv_grps = ARRAY_SIZE(mt8127_pin_drv),
+	.spec_pull_set = mt8127_spec_pull_set,
+	.spec_ies_smt_set = mt8127_ies_smt_set,
+	.dir_offset = 0x0000,
+	.pullen_offset = 0x0100,
+	.pullsel_offset = 0x0200,
+	.dout_offset = 0x0400,
+	.din_offset = 0x0500,
+	.pinmux_offset = 0x0600,
+	.type1_start = 143,
+	.type1_end = 143,
+	.port_shf = 4,
+	.port_mask = 0xf,
+	.port_align = 4,
+	.eint_offsets = {
+		.name = "mt8127_eint",
+		.stat      = 0x000,
+		.ack       = 0x040,
+		.mask      = 0x080,
+		.mask_set  = 0x0c0,
+		.mask_clr  = 0x100,
+		.sens      = 0x140,
+		.sens_set  = 0x180,
+		.sens_clr  = 0x1c0,
+		.soft      = 0x200,
+		.soft_set  = 0x240,
+		.soft_clr  = 0x280,
+		.pol       = 0x300,
+		.pol_set   = 0x340,
+		.pol_clr   = 0x380,
+		.dom_en    = 0x400,
+		.dbnc_ctrl = 0x500,
+		.dbnc_set  = 0x600,
+		.dbnc_clr  = 0x700,
+		.port_mask = 7,
+		.ports     = 6,
+	},
+	.ap_num = 143,
+	.db_cnt = 16,
+};
+
+static int mt8127_pinctrl_probe(struct platform_device *pdev)
+{
+	return mtk_pctrl_init(pdev, &mt8127_pinctrl_data, NULL);
+}
+
+static const struct of_device_id mt8127_pctrl_match[] = {
+	{ .compatible = "mediatek,mt8127-pinctrl", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mt8127_pctrl_match);
+
+static struct platform_driver mtk_pinctrl_driver = {
+	.probe = mt8127_pinctrl_probe,
+	.driver = {
+		.name = "mediatek-mt8127-pinctrl",
+		.of_match_table = mt8127_pctrl_match,
+	},
+};
+
+static int __init mtk_pinctrl_init(void)
+{
+	return platform_driver_register(&mtk_pinctrl_driver);
+}
+
+module_init(mtk_pinctrl_init);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MediaTek MT8127 Pinctrl Driver");
+MODULE_AUTHOR("Yingjoe Chen <yingjoe.chen@mediatek.com>");
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt8135.c b/drivers/pinctrl/mediatek/pinctrl-mt8135.c
index f1e1e187ce96..404f1178511d 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mt8135.c
+++ b/drivers/pinctrl/mediatek/pinctrl-mt8135.c
@@ -32,12 +32,12 @@
 #define R1_BASE2				0x250
 
 struct mtk_spec_pull_set {
-	unsigned int pin;
-	unsigned int pupd_offset;
+	unsigned char pin;
 	unsigned char pupd_bit;
-	unsigned int r0_offset;
+	unsigned short pupd_offset;
+	unsigned short r0_offset;
+	unsigned short r1_offset;
 	unsigned char r0_bit;
-	unsigned int r1_offset;
 	unsigned char r1_bit;
 };
 
@@ -305,7 +305,6 @@ static const struct mtk_pinctrl_devdata mt8135_pinctrl_data = {
 	.pullen_offset = 0x0200,
 	.smt_offset = 0x0300,
 	.pullsel_offset = 0x0400,
-	.invser_offset = 0x0600,
 	.dout_offset = 0x0800,
 	.din_offset = 0x0A00,
 	.pinmux_offset = 0x0C00,
@@ -314,7 +313,6 @@ static const struct mtk_pinctrl_devdata mt8135_pinctrl_data = {
 	.port_shf = 4,
 	.port_mask = 0xf,
 	.port_align = 4,
-	.chip_type = MTK_CHIP_TYPE_BASE,
 	.eint_offsets = {
 		.name = "mt8135_eint",
 		.stat      = 0x000,
@@ -344,7 +342,7 @@ static const struct mtk_pinctrl_devdata mt8135_pinctrl_data = {
 
 static int mt8135_pinctrl_probe(struct platform_device *pdev)
 {
-	return mtk_pctrl_init(pdev, &mt8135_pinctrl_data);
+	return mtk_pctrl_init(pdev, &mt8135_pinctrl_data, NULL);
 }
 
 static const struct of_device_id mt8135_pctrl_match[] = {
@@ -359,7 +357,6 @@ static struct platform_driver mtk_pinctrl_driver = {
 	.probe = mt8135_pinctrl_probe,
 	.driver = {
 		.name = "mediatek-mt8135-pinctrl",
-		.owner = THIS_MODULE,
 		.of_match_table = mt8135_pctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt8173.c b/drivers/pinctrl/mediatek/pinctrl-mt8173.c
index 412ea84836a1..d0c811d5f07b 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mt8173.c
+++ b/drivers/pinctrl/mediatek/pinctrl-mt8173.c
@@ -18,6 +18,7 @@
 #include <linux/of_device.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/regmap.h>
+#include <linux/pinctrl/pinconf-generic.h>
 #include <dt-bindings/pinctrl/mt65xx.h>
 
 #include "pinctrl-mtk-common.h"
@@ -25,228 +26,172 @@
 
 #define DRV_BASE				0xb00
 
-/**
- * struct mtk_pin_ies_smt_set - For special pins' ies and smt setting.
- * @start: The start pin number of those special pins.
- * @end: The end pin number of those special pins.
- * @offset: The offset of special setting register.
- * @bit: The bit of special setting register.
- */
-struct mtk_pin_ies_smt_set {
-	unsigned int start;
-	unsigned int end;
-	unsigned int offset;
-	unsigned char bit;
+static const struct mtk_pin_spec_pupd_set_samereg mt8173_spec_pupd[] = {
+	MTK_PIN_PUPD_SPEC_SR(119, 0xe00, 2, 1, 0),  /* KROW0 */
+	MTK_PIN_PUPD_SPEC_SR(120, 0xe00, 6, 5, 4),  /* KROW1 */
+	MTK_PIN_PUPD_SPEC_SR(121, 0xe00, 10, 9, 8), /* KROW2 */
+	MTK_PIN_PUPD_SPEC_SR(122, 0xe10, 2, 1, 0),  /* KCOL0 */
+	MTK_PIN_PUPD_SPEC_SR(123, 0xe10, 6, 5, 4),  /* KCOL1 */
+	MTK_PIN_PUPD_SPEC_SR(124, 0xe10, 10, 9, 8), /* KCOL2 */
+
+	MTK_PIN_PUPD_SPEC_SR(67, 0xd10, 2, 1, 0),   /* ms0 DS */
+	MTK_PIN_PUPD_SPEC_SR(68, 0xd00, 2, 1, 0),   /* ms0 RST */
+	MTK_PIN_PUPD_SPEC_SR(66, 0xc10, 2, 1, 0),   /* ms0 cmd */
+	MTK_PIN_PUPD_SPEC_SR(65, 0xc00, 2, 1, 0),   /* ms0 clk */
+	MTK_PIN_PUPD_SPEC_SR(57, 0xc20, 2, 1, 0),   /* ms0 data0 */
+	MTK_PIN_PUPD_SPEC_SR(58, 0xc20, 2, 1, 0),   /* ms0 data1 */
+	MTK_PIN_PUPD_SPEC_SR(59, 0xc20, 2, 1, 0),   /* ms0 data2 */
+	MTK_PIN_PUPD_SPEC_SR(60, 0xc20, 2, 1, 0),   /* ms0 data3 */
+	MTK_PIN_PUPD_SPEC_SR(61, 0xc20, 2, 1, 0),   /* ms0 data4 */
+	MTK_PIN_PUPD_SPEC_SR(62, 0xc20, 2, 1, 0),   /* ms0 data5 */
+	MTK_PIN_PUPD_SPEC_SR(63, 0xc20, 2, 1, 0),   /* ms0 data6 */
+	MTK_PIN_PUPD_SPEC_SR(64, 0xc20, 2, 1, 0),   /* ms0 data7 */
+
+	MTK_PIN_PUPD_SPEC_SR(78, 0xc50, 2, 1, 0),    /* ms1 cmd */
+	MTK_PIN_PUPD_SPEC_SR(73, 0xd20, 2, 1, 0),    /* ms1 dat0 */
+	MTK_PIN_PUPD_SPEC_SR(74, 0xd20, 6, 5, 4),    /* ms1 dat1 */
+	MTK_PIN_PUPD_SPEC_SR(75, 0xd20, 10, 9, 8),   /* ms1 dat2 */
+	MTK_PIN_PUPD_SPEC_SR(76, 0xd20, 14, 13, 12), /* ms1 dat3 */
+	MTK_PIN_PUPD_SPEC_SR(77, 0xc40, 2, 1, 0),    /* ms1 clk */
+
+	MTK_PIN_PUPD_SPEC_SR(100, 0xd40, 2, 1, 0),    /* ms2 dat0 */
+	MTK_PIN_PUPD_SPEC_SR(101, 0xd40, 6, 5, 4),    /* ms2 dat1 */
+	MTK_PIN_PUPD_SPEC_SR(102, 0xd40, 10, 9, 8),   /* ms2 dat2 */
+	MTK_PIN_PUPD_SPEC_SR(103, 0xd40, 14, 13, 12), /* ms2 dat3 */
+	MTK_PIN_PUPD_SPEC_SR(104, 0xc80, 2, 1, 0),    /* ms2 clk */
+	MTK_PIN_PUPD_SPEC_SR(105, 0xc90, 2, 1, 0),    /* ms2 cmd */
+
+	MTK_PIN_PUPD_SPEC_SR(22, 0xd60, 2, 1, 0),    /* ms3 dat0 */
+	MTK_PIN_PUPD_SPEC_SR(23, 0xd60, 6, 5, 4),    /* ms3 dat1 */
+	MTK_PIN_PUPD_SPEC_SR(24, 0xd60, 10, 9, 8),   /* ms3 dat2 */
+	MTK_PIN_PUPD_SPEC_SR(25, 0xd60, 14, 13, 12), /* ms3 dat3 */
+	MTK_PIN_PUPD_SPEC_SR(26, 0xcc0, 2, 1, 0),    /* ms3 clk */
+	MTK_PIN_PUPD_SPEC_SR(27, 0xcd0, 2, 1, 0)     /* ms3 cmd */
 };
 
-#define MTK_PIN_IES_SMT_SET(_start, _end, _offset, _bit)	\
-	{	\
-		.start = _start,	\
-		.end = _end,	\
-		.bit = _bit,	\
-		.offset = _offset,	\
-	}
-
-/**
- * struct mtk_pin_spec_pupd_set - For special pins' pull up/down setting.
- * @pin: The pin number.
- * @offset: The offset of special pull up/down setting register.
- * @pupd_bit: The pull up/down bit in this register.
- * @r0_bit: The r0 bit of pull resistor.
- * @r1_bit: The r1 bit of pull resistor.
- */
-struct mtk_pin_spec_pupd_set {
-	unsigned int pin;
-	unsigned int offset;
-	unsigned char pupd_bit;
-	unsigned char r1_bit;
-	unsigned char r0_bit;
-};
-
-#define MTK_PIN_PUPD_SPEC(_pin, _offset, _pupd, _r1, _r0)	\
-	{	\
-		.pin = _pin,	\
-		.offset = _offset,	\
-		.pupd_bit = _pupd,	\
-		.r1_bit = _r1,		\
-		.r0_bit = _r0,		\
-	}
-
-static const struct mtk_pin_spec_pupd_set mt8173_spec_pupd[] = {
-	MTK_PIN_PUPD_SPEC(119, 0xe00, 2, 1, 0),  /* KROW0 */
-	MTK_PIN_PUPD_SPEC(120, 0xe00, 6, 5, 4),  /* KROW1 */
-	MTK_PIN_PUPD_SPEC(121, 0xe00, 10, 9, 8), /* KROW2 */
-	MTK_PIN_PUPD_SPEC(122, 0xe10, 2, 1, 0),  /* KCOL0 */
-	MTK_PIN_PUPD_SPEC(123, 0xe10, 6, 5, 4),  /* KCOL1 */
-	MTK_PIN_PUPD_SPEC(124, 0xe10, 10, 9, 8), /* KCOL2 */
-
-	MTK_PIN_PUPD_SPEC(67, 0xd10, 2, 1, 0),   /* ms0 DS */
-	MTK_PIN_PUPD_SPEC(68, 0xd00, 2, 1, 0),   /* ms0 RST */
-	MTK_PIN_PUPD_SPEC(66, 0xc10, 2, 1, 0),   /* ms0 cmd */
-	MTK_PIN_PUPD_SPEC(65, 0xc00, 2, 1, 0),   /* ms0 clk */
-	MTK_PIN_PUPD_SPEC(57, 0xc20, 2, 1, 0),   /* ms0 data0 */
-	MTK_PIN_PUPD_SPEC(58, 0xc20, 2, 1, 0),   /* ms0 data1 */
-	MTK_PIN_PUPD_SPEC(59, 0xc20, 2, 1, 0),   /* ms0 data2 */
-	MTK_PIN_PUPD_SPEC(60, 0xc20, 2, 1, 0),   /* ms0 data3 */
-	MTK_PIN_PUPD_SPEC(61, 0xc20, 2, 1, 0),   /* ms0 data4 */
-	MTK_PIN_PUPD_SPEC(62, 0xc20, 2, 1, 0),   /* ms0 data5 */
-	MTK_PIN_PUPD_SPEC(63, 0xc20, 2, 1, 0),   /* ms0 data6 */
-	MTK_PIN_PUPD_SPEC(64, 0xc20, 2, 1, 0),   /* ms0 data7 */
-
-	MTK_PIN_PUPD_SPEC(78, 0xc50, 2, 1, 0),    /* ms1 cmd */
-	MTK_PIN_PUPD_SPEC(73, 0xd20, 2, 1, 0),    /* ms1 dat0 */
-	MTK_PIN_PUPD_SPEC(74, 0xd20, 6, 5, 4),    /* ms1 dat1 */
-	MTK_PIN_PUPD_SPEC(75, 0xd20, 10, 9, 8),   /* ms1 dat2 */
-	MTK_PIN_PUPD_SPEC(76, 0xd20, 14, 13, 12), /* ms1 dat3 */
-	MTK_PIN_PUPD_SPEC(77, 0xc40, 2, 1, 0),    /* ms1 clk */
-
-	MTK_PIN_PUPD_SPEC(100, 0xd40, 2, 1, 0),    /* ms2 dat0 */
-	MTK_PIN_PUPD_SPEC(101, 0xd40, 6, 5, 4),    /* ms2 dat1 */
-	MTK_PIN_PUPD_SPEC(102, 0xd40, 10, 9, 8),   /* ms2 dat2 */
-	MTK_PIN_PUPD_SPEC(103, 0xd40, 14, 13, 12), /* ms2 dat3 */
-	MTK_PIN_PUPD_SPEC(104, 0xc80, 2, 1, 0),    /* ms2 clk */
-	MTK_PIN_PUPD_SPEC(105, 0xc90, 2, 1, 0),    /* ms2 cmd */
-
-	MTK_PIN_PUPD_SPEC(22, 0xd60, 2, 1, 0),    /* ms3 dat0 */
-	MTK_PIN_PUPD_SPEC(23, 0xd60, 6, 5, 4),    /* ms3 dat1 */
-	MTK_PIN_PUPD_SPEC(24, 0xd60, 10, 9, 8),   /* ms3 dat2 */
-	MTK_PIN_PUPD_SPEC(25, 0xd60, 14, 13, 12), /* ms3 dat3 */
-	MTK_PIN_PUPD_SPEC(26, 0xcc0, 2, 1, 0),    /* ms3 clk */
-	MTK_PIN_PUPD_SPEC(27, 0xcd0, 2, 1, 0)     /* ms3 cmd */
-};
-
-static int spec_pull_set(struct regmap *regmap, unsigned int pin,
+static int mt8173_spec_pull_set(struct regmap *regmap, unsigned int pin,
 		unsigned char align, bool isup, unsigned int r1r0)
 {
-	unsigned int i;
-	unsigned int reg_pupd, reg_set, reg_rst;
-	unsigned int bit_pupd, bit_r0, bit_r1;
-	const struct mtk_pin_spec_pupd_set *spec_pupd_pin;
-	bool find = false;
-
-	for (i = 0; i < ARRAY_SIZE(mt8173_spec_pupd); i++) {
-		if (pin == mt8173_spec_pupd[i].pin) {
-			find = true;
-			break;
-		}
-	}
-
-	if (!find)
-		return -EINVAL;
-
-	spec_pupd_pin = mt8173_spec_pupd + i;
-	reg_set = spec_pupd_pin->offset + align;
-	reg_rst = spec_pupd_pin->offset + (align << 1);
-
-	if (isup)
-		reg_pupd = reg_rst;
-	else
-		reg_pupd = reg_set;
-
-	bit_pupd = BIT(spec_pupd_pin->pupd_bit);
-	regmap_write(regmap, reg_pupd, bit_pupd);
-
-	bit_r0 = BIT(spec_pupd_pin->r0_bit);
-	bit_r1 = BIT(spec_pupd_pin->r1_bit);
-
-	switch (r1r0) {
-	case MTK_PUPD_SET_R1R0_00:
-		regmap_write(regmap, reg_rst, bit_r0);
-		regmap_write(regmap, reg_rst, bit_r1);
-		break;
-	case MTK_PUPD_SET_R1R0_01:
-		regmap_write(regmap, reg_set, bit_r0);
-		regmap_write(regmap, reg_rst, bit_r1);
-		break;
-	case MTK_PUPD_SET_R1R0_10:
-		regmap_write(regmap, reg_rst, bit_r0);
-		regmap_write(regmap, reg_set, bit_r1);
-		break;
-	case MTK_PUPD_SET_R1R0_11:
-		regmap_write(regmap, reg_set, bit_r0);
-		regmap_write(regmap, reg_set, bit_r1);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
+	return mtk_pctrl_spec_pull_set_samereg(regmap, mt8173_spec_pupd,
+		ARRAY_SIZE(mt8173_spec_pupd), pin, align, isup, r1r0);
 }
 
-static const struct mtk_pin_ies_smt_set mt8173_ies_smt_set[] = {
-	MTK_PIN_IES_SMT_SET(0, 4, 0x930, 1),
-	MTK_PIN_IES_SMT_SET(5, 9, 0x930, 2),
-	MTK_PIN_IES_SMT_SET(10, 13, 0x930, 10),
-	MTK_PIN_IES_SMT_SET(14, 15, 0x940, 10),
-	MTK_PIN_IES_SMT_SET(16, 16, 0x930, 0),
-	MTK_PIN_IES_SMT_SET(17, 17, 0x950, 2),
-	MTK_PIN_IES_SMT_SET(18, 21, 0x940, 3),
-	MTK_PIN_IES_SMT_SET(29, 32, 0x930, 3),
-	MTK_PIN_IES_SMT_SET(33, 33, 0x930, 4),
-	MTK_PIN_IES_SMT_SET(34, 36, 0x930, 5),
-	MTK_PIN_IES_SMT_SET(37, 38, 0x930, 6),
-	MTK_PIN_IES_SMT_SET(39, 39, 0x930, 7),
-	MTK_PIN_IES_SMT_SET(40, 41, 0x930, 9),
-	MTK_PIN_IES_SMT_SET(42, 42, 0x940, 0),
-	MTK_PIN_IES_SMT_SET(43, 44, 0x930, 11),
-	MTK_PIN_IES_SMT_SET(45, 46, 0x930, 12),
-	MTK_PIN_IES_SMT_SET(57, 64, 0xc20, 13),
-	MTK_PIN_IES_SMT_SET(65, 65, 0xc10, 13),
-	MTK_PIN_IES_SMT_SET(66, 66, 0xc00, 13),
-	MTK_PIN_IES_SMT_SET(67, 67, 0xd10, 13),
-	MTK_PIN_IES_SMT_SET(68, 68, 0xd00, 13),
-	MTK_PIN_IES_SMT_SET(69, 72, 0x940, 14),
-	MTK_PIN_IES_SMT_SET(73, 76, 0xc60, 13),
-	MTK_PIN_IES_SMT_SET(77, 77, 0xc40, 13),
-	MTK_PIN_IES_SMT_SET(78, 78, 0xc50, 13),
-	MTK_PIN_IES_SMT_SET(79, 82, 0x940, 15),
-	MTK_PIN_IES_SMT_SET(83, 83, 0x950, 0),
-	MTK_PIN_IES_SMT_SET(84, 85, 0x950, 1),
-	MTK_PIN_IES_SMT_SET(86, 91, 0x950, 2),
-	MTK_PIN_IES_SMT_SET(92, 92, 0x930, 13),
-	MTK_PIN_IES_SMT_SET(93, 95, 0x930, 14),
-	MTK_PIN_IES_SMT_SET(96, 99, 0x930, 15),
-	MTK_PIN_IES_SMT_SET(100, 103, 0xca0, 13),
-	MTK_PIN_IES_SMT_SET(104, 104, 0xc80, 13),
-	MTK_PIN_IES_SMT_SET(105, 105, 0xc90, 13),
-	MTK_PIN_IES_SMT_SET(106, 107, 0x940, 4),
-	MTK_PIN_IES_SMT_SET(108, 112, 0x940, 1),
-	MTK_PIN_IES_SMT_SET(113, 116, 0x940, 2),
-	MTK_PIN_IES_SMT_SET(117, 118, 0x940, 5),
-	MTK_PIN_IES_SMT_SET(119, 124, 0x940, 6),
-	MTK_PIN_IES_SMT_SET(125, 126, 0x940, 7),
-	MTK_PIN_IES_SMT_SET(127, 127, 0x940, 0),
-	MTK_PIN_IES_SMT_SET(128, 128, 0x950, 8),
-	MTK_PIN_IES_SMT_SET(129, 130, 0x950, 9),
-	MTK_PIN_IES_SMT_SET(131, 132, 0x950, 8),
-	MTK_PIN_IES_SMT_SET(133, 134, 0x910, 8)
+static const struct mtk_pin_ies_smt_set mt8173_smt_set[] = {
+	MTK_PIN_IES_SMT_SPEC(0, 4, 0x930, 1),
+	MTK_PIN_IES_SMT_SPEC(5, 9, 0x930, 2),
+	MTK_PIN_IES_SMT_SPEC(10, 13, 0x930, 10),
+	MTK_PIN_IES_SMT_SPEC(14, 15, 0x940, 10),
+	MTK_PIN_IES_SMT_SPEC(16, 16, 0x930, 0),
+	MTK_PIN_IES_SMT_SPEC(17, 17, 0x950, 2),
+	MTK_PIN_IES_SMT_SPEC(18, 21, 0x940, 3),
+	MTK_PIN_IES_SMT_SPEC(22, 25, 0xce0, 13),
+	MTK_PIN_IES_SMT_SPEC(26, 26, 0xcc0, 13),
+	MTK_PIN_IES_SMT_SPEC(27, 27, 0xcd0, 13),
+	MTK_PIN_IES_SMT_SPEC(28, 28, 0xd70, 13),
+	MTK_PIN_IES_SMT_SPEC(29, 32, 0x930, 3),
+	MTK_PIN_IES_SMT_SPEC(33, 33, 0x930, 4),
+	MTK_PIN_IES_SMT_SPEC(34, 36, 0x930, 5),
+	MTK_PIN_IES_SMT_SPEC(37, 38, 0x930, 6),
+	MTK_PIN_IES_SMT_SPEC(39, 39, 0x930, 7),
+	MTK_PIN_IES_SMT_SPEC(40, 41, 0x930, 9),
+	MTK_PIN_IES_SMT_SPEC(42, 42, 0x940, 0),
+	MTK_PIN_IES_SMT_SPEC(43, 44, 0x930, 11),
+	MTK_PIN_IES_SMT_SPEC(45, 46, 0x930, 12),
+	MTK_PIN_IES_SMT_SPEC(57, 64, 0xc20, 13),
+	MTK_PIN_IES_SMT_SPEC(65, 65, 0xc10, 13),
+	MTK_PIN_IES_SMT_SPEC(66, 66, 0xc00, 13),
+	MTK_PIN_IES_SMT_SPEC(67, 67, 0xd10, 13),
+	MTK_PIN_IES_SMT_SPEC(68, 68, 0xd00, 13),
+	MTK_PIN_IES_SMT_SPEC(69, 72, 0x940, 14),
+	MTK_PIN_IES_SMT_SPEC(73, 76, 0xc60, 13),
+	MTK_PIN_IES_SMT_SPEC(77, 77, 0xc40, 13),
+	MTK_PIN_IES_SMT_SPEC(78, 78, 0xc50, 13),
+	MTK_PIN_IES_SMT_SPEC(79, 82, 0x940, 15),
+	MTK_PIN_IES_SMT_SPEC(83, 83, 0x950, 0),
+	MTK_PIN_IES_SMT_SPEC(84, 85, 0x950, 1),
+	MTK_PIN_IES_SMT_SPEC(86, 91, 0x950, 2),
+	MTK_PIN_IES_SMT_SPEC(92, 92, 0x930, 13),
+	MTK_PIN_IES_SMT_SPEC(93, 95, 0x930, 14),
+	MTK_PIN_IES_SMT_SPEC(96, 99, 0x930, 15),
+	MTK_PIN_IES_SMT_SPEC(100, 103, 0xca0, 13),
+	MTK_PIN_IES_SMT_SPEC(104, 104, 0xc80, 13),
+	MTK_PIN_IES_SMT_SPEC(105, 105, 0xc90, 13),
+	MTK_PIN_IES_SMT_SPEC(106, 107, 0x940, 4),
+	MTK_PIN_IES_SMT_SPEC(108, 112, 0x940, 1),
+	MTK_PIN_IES_SMT_SPEC(113, 116, 0x940, 2),
+	MTK_PIN_IES_SMT_SPEC(117, 118, 0x940, 5),
+	MTK_PIN_IES_SMT_SPEC(119, 124, 0x940, 6),
+	MTK_PIN_IES_SMT_SPEC(125, 126, 0x940, 7),
+	MTK_PIN_IES_SMT_SPEC(127, 127, 0x940, 0),
+	MTK_PIN_IES_SMT_SPEC(128, 128, 0x950, 8),
+	MTK_PIN_IES_SMT_SPEC(129, 130, 0x950, 9),
+	MTK_PIN_IES_SMT_SPEC(131, 132, 0x950, 8),
+	MTK_PIN_IES_SMT_SPEC(133, 134, 0x910, 8)
 };
 
-static int spec_ies_smt_set(struct regmap *regmap, unsigned int pin,
-		unsigned char align, int value)
-{
-	unsigned int i, reg_addr, bit;
-	bool find = false;
-
-	for (i = 0; i < ARRAY_SIZE(mt8173_ies_smt_set); i++) {
-		if (pin >= mt8173_ies_smt_set[i].start &&
-				pin <= mt8173_ies_smt_set[i].end) {
-			find = true;
-			break;
-		}
-	}
-
-	if (!find)
-		return -EINVAL;
-
-	if (value)
-		reg_addr = mt8173_ies_smt_set[i].offset + align;
-	else
-		reg_addr = mt8173_ies_smt_set[i].offset + (align << 1);
+static const struct mtk_pin_ies_smt_set mt8173_ies_set[] = {
+	MTK_PIN_IES_SMT_SPEC(0, 4, 0x900, 1),
+	MTK_PIN_IES_SMT_SPEC(5, 9, 0x900, 2),
+	MTK_PIN_IES_SMT_SPEC(10, 13, 0x900, 10),
+	MTK_PIN_IES_SMT_SPEC(14, 15, 0x910, 10),
+	MTK_PIN_IES_SMT_SPEC(16, 16, 0x900, 0),
+	MTK_PIN_IES_SMT_SPEC(17, 17, 0x920, 2),
+	MTK_PIN_IES_SMT_SPEC(18, 21, 0x910, 3),
+	MTK_PIN_IES_SMT_SPEC(22, 25, 0xce0, 14),
+	MTK_PIN_IES_SMT_SPEC(26, 26, 0xcc0, 14),
+	MTK_PIN_IES_SMT_SPEC(27, 27, 0xcd0, 14),
+	MTK_PIN_IES_SMT_SPEC(28, 28, 0xd70, 14),
+	MTK_PIN_IES_SMT_SPEC(29, 32, 0x900, 3),
+	MTK_PIN_IES_SMT_SPEC(33, 33, 0x900, 4),
+	MTK_PIN_IES_SMT_SPEC(34, 36, 0x900, 5),
+	MTK_PIN_IES_SMT_SPEC(37, 38, 0x900, 6),
+	MTK_PIN_IES_SMT_SPEC(39, 39, 0x900, 7),
+	MTK_PIN_IES_SMT_SPEC(40, 41, 0x900, 9),
+	MTK_PIN_IES_SMT_SPEC(42, 42, 0x910, 0),
+	MTK_PIN_IES_SMT_SPEC(43, 44, 0x900, 11),
+	MTK_PIN_IES_SMT_SPEC(45, 46, 0x900, 12),
+	MTK_PIN_IES_SMT_SPEC(57, 64, 0xc20, 14),
+	MTK_PIN_IES_SMT_SPEC(65, 65, 0xc10, 14),
+	MTK_PIN_IES_SMT_SPEC(66, 66, 0xc00, 14),
+	MTK_PIN_IES_SMT_SPEC(67, 67, 0xd10, 14),
+	MTK_PIN_IES_SMT_SPEC(68, 68, 0xd00, 14),
+	MTK_PIN_IES_SMT_SPEC(69, 72, 0x910, 14),
+	MTK_PIN_IES_SMT_SPEC(73, 76, 0xc60, 14),
+	MTK_PIN_IES_SMT_SPEC(77, 77, 0xc40, 14),
+	MTK_PIN_IES_SMT_SPEC(78, 78, 0xc50, 14),
+	MTK_PIN_IES_SMT_SPEC(79, 82, 0x910, 15),
+	MTK_PIN_IES_SMT_SPEC(83, 83, 0x920, 0),
+	MTK_PIN_IES_SMT_SPEC(84, 85, 0x920, 1),
+	MTK_PIN_IES_SMT_SPEC(86, 91, 0x920, 2),
+	MTK_PIN_IES_SMT_SPEC(92, 92, 0x900, 13),
+	MTK_PIN_IES_SMT_SPEC(93, 95, 0x900, 14),
+	MTK_PIN_IES_SMT_SPEC(96, 99, 0x900, 15),
+	MTK_PIN_IES_SMT_SPEC(100, 103, 0xca0, 14),
+	MTK_PIN_IES_SMT_SPEC(104, 104, 0xc80, 14),
+	MTK_PIN_IES_SMT_SPEC(105, 105, 0xc90, 14),
+	MTK_PIN_IES_SMT_SPEC(106, 107, 0x910, 4),
+	MTK_PIN_IES_SMT_SPEC(108, 112, 0x910, 1),
+	MTK_PIN_IES_SMT_SPEC(113, 116, 0x910, 2),
+	MTK_PIN_IES_SMT_SPEC(117, 118, 0x910, 5),
+	MTK_PIN_IES_SMT_SPEC(119, 124, 0x910, 6),
+	MTK_PIN_IES_SMT_SPEC(125, 126, 0x910, 7),
+	MTK_PIN_IES_SMT_SPEC(127, 127, 0x910, 0),
+	MTK_PIN_IES_SMT_SPEC(128, 128, 0x920, 8),
+	MTK_PIN_IES_SMT_SPEC(129, 130, 0x920, 9),
+	MTK_PIN_IES_SMT_SPEC(131, 132, 0x920, 8),
+	MTK_PIN_IES_SMT_SPEC(133, 134, 0x910, 8)
+};
 
-	bit = BIT(mt8173_ies_smt_set[i].bit);
-	regmap_write(regmap, reg_addr, bit);
-	return 0;
+static int mt8173_ies_smt_set(struct regmap *regmap, unsigned int pin,
+		unsigned char align, int value, enum pin_config_param arg)
+{
+	if (arg == PIN_CONFIG_INPUT_ENABLE)
+		return mtk_pconf_spec_set_ies_smt_range(regmap, mt8173_ies_set,
+			ARRAY_SIZE(mt8173_ies_set), pin, align, value);
+	else if (arg == PIN_CONFIG_INPUT_SCHMITT_ENABLE)
+		return mtk_pconf_spec_set_ies_smt_range(regmap, mt8173_smt_set,
+			ARRAY_SIZE(mt8173_smt_set), pin, align, value);
+	return -EINVAL;
 }
 
 static const struct mtk_drv_group_desc mt8173_drv_grp[] =  {
@@ -382,8 +327,8 @@ static const struct mtk_pinctrl_devdata mt8173_pinctrl_data = {
 	.n_grp_cls = ARRAY_SIZE(mt8173_drv_grp),
 	.pin_drv_grp = mt8173_pin_drv,
 	.n_pin_drv_grps = ARRAY_SIZE(mt8173_pin_drv),
-	.spec_pull_set = spec_pull_set,
-	.spec_ies_smt_set = spec_ies_smt_set,
+	.spec_pull_set = mt8173_spec_pull_set,
+	.spec_ies_smt_set = mt8173_ies_smt_set,
 	.dir_offset = 0x0000,
 	.pullen_offset = 0x0100,
 	.pullsel_offset = 0x0200,
@@ -424,7 +369,7 @@ static const struct mtk_pinctrl_devdata mt8173_pinctrl_data = {
 
 static int mt8173_pinctrl_probe(struct platform_device *pdev)
 {
-	return mtk_pctrl_init(pdev, &mt8173_pinctrl_data);
+	return mtk_pctrl_init(pdev, &mt8173_pinctrl_data, NULL);
 }
 
 static const struct of_device_id mt8173_pctrl_match[] = {
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-common.c b/drivers/pinctrl/mediatek/pinctrl-mtk-common.c
index 474812e2b0cb..ad1ea1695b4a 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mtk-common.c
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-common.c
@@ -107,28 +107,38 @@ static void mtk_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 	regmap_write(mtk_get_regmap(pctl, offset), reg_addr, bit);
 }
 
-static void mtk_pconf_set_ies_smt(struct mtk_pinctrl *pctl, unsigned pin,
-		int value, enum pin_config_param param)
+static int mtk_pconf_set_ies_smt(struct mtk_pinctrl *pctl, unsigned pin,
+		int value, enum pin_config_param arg)
 {
 	unsigned int reg_addr, offset;
 	unsigned int bit;
-	int ret;
+
+	/**
+	 * Due to some soc are not support ies/smt config, add this special
+	 * control to handle it.
+	 */
+	if (!pctl->devdata->spec_ies_smt_set &&
+		pctl->devdata->ies_offset == MTK_PINCTRL_NOT_SUPPORT &&
+			arg == PIN_CONFIG_INPUT_ENABLE)
+		return -EINVAL;
+
+	if (!pctl->devdata->spec_ies_smt_set &&
+		pctl->devdata->smt_offset == MTK_PINCTRL_NOT_SUPPORT &&
+			arg == PIN_CONFIG_INPUT_SCHMITT_ENABLE)
+		return -EINVAL;
 
 	/*
 	 * Due to some pins are irregular, their input enable and smt
-	 * control register are discontinuous, but they are mapping together.
-	 * So we need this special handle.
+	 * control register are discontinuous, so we need this special handle.
 	 */
 	if (pctl->devdata->spec_ies_smt_set) {
-		ret = pctl->devdata->spec_ies_smt_set(mtk_get_regmap(pctl, pin),
-			pin, pctl->devdata->port_align, value);
-		if (!ret)
-			return;
+		return pctl->devdata->spec_ies_smt_set(mtk_get_regmap(pctl, pin),
+			pin, pctl->devdata->port_align, value, arg);
 	}
 
 	bit = BIT(pin & 0xf);
 
-	if (param == PIN_CONFIG_INPUT_ENABLE)
+	if (arg == PIN_CONFIG_INPUT_ENABLE)
 		offset = pctl->devdata->ies_offset;
 	else
 		offset = pctl->devdata->smt_offset;
@@ -139,6 +149,33 @@ static void mtk_pconf_set_ies_smt(struct mtk_pinctrl *pctl, unsigned pin,
 		reg_addr = CLR_ADDR(mtk_get_port(pctl, pin) + offset, pctl);
 
 	regmap_write(mtk_get_regmap(pctl, pin), reg_addr, bit);
+	return 0;
+}
+
+int mtk_pconf_spec_set_ies_smt_range(struct regmap *regmap,
+		const struct mtk_pin_ies_smt_set *ies_smt_infos, unsigned int info_num,
+		unsigned int pin, unsigned char align, int value)
+{
+	unsigned int i, reg_addr, bit;
+
+	for (i = 0; i < info_num; i++) {
+		if (pin >= ies_smt_infos[i].start &&
+				pin <= ies_smt_infos[i].end) {
+			break;
+		}
+	}
+
+	if (i == info_num)
+		return -EINVAL;
+
+	if (value)
+		reg_addr = ies_smt_infos[i].offset + align;
+	else
+		reg_addr = ies_smt_infos[i].offset + (align << 1);
+
+	bit = BIT(ies_smt_infos[i].bit);
+	regmap_write(regmap, reg_addr, bit);
+	return 0;
 }
 
 static const struct mtk_pin_drv_grp *mtk_find_pin_drv_grp_by_pin(
@@ -186,6 +223,66 @@ static int mtk_pconf_set_driving(struct mtk_pinctrl *pctl,
 	return -EINVAL;
 }
 
+int mtk_pctrl_spec_pull_set_samereg(struct regmap *regmap,
+		const struct mtk_pin_spec_pupd_set_samereg *pupd_infos,
+		unsigned int info_num, unsigned int pin,
+		unsigned char align, bool isup, unsigned int r1r0)
+{
+	unsigned int i;
+	unsigned int reg_pupd, reg_set, reg_rst;
+	unsigned int bit_pupd, bit_r0, bit_r1;
+	const struct mtk_pin_spec_pupd_set_samereg *spec_pupd_pin;
+	bool find = false;
+
+	for (i = 0; i < info_num; i++) {
+		if (pin == pupd_infos[i].pin) {
+			find = true;
+			break;
+		}
+	}
+
+	if (!find)
+		return -EINVAL;
+
+	spec_pupd_pin = pupd_infos + i;
+	reg_set = spec_pupd_pin->offset + align;
+	reg_rst = spec_pupd_pin->offset + (align << 1);
+
+	if (isup)
+		reg_pupd = reg_rst;
+	else
+		reg_pupd = reg_set;
+
+	bit_pupd = BIT(spec_pupd_pin->pupd_bit);
+	regmap_write(regmap, reg_pupd, bit_pupd);
+
+	bit_r0 = BIT(spec_pupd_pin->r0_bit);
+	bit_r1 = BIT(spec_pupd_pin->r1_bit);
+
+	switch (r1r0) {
+	case MTK_PUPD_SET_R1R0_00:
+		regmap_write(regmap, reg_rst, bit_r0);
+		regmap_write(regmap, reg_rst, bit_r1);
+		break;
+	case MTK_PUPD_SET_R1R0_01:
+		regmap_write(regmap, reg_set, bit_r0);
+		regmap_write(regmap, reg_rst, bit_r1);
+		break;
+	case MTK_PUPD_SET_R1R0_10:
+		regmap_write(regmap, reg_rst, bit_r0);
+		regmap_write(regmap, reg_set, bit_r1);
+		break;
+	case MTK_PUPD_SET_R1R0_11:
+		regmap_write(regmap, reg_set, bit_r0);
+		regmap_write(regmap, reg_set, bit_r1);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int mtk_pconf_set_pull_select(struct mtk_pinctrl *pctl,
 		unsigned int pin, bool enable, bool isup, unsigned int arg)
 {
@@ -235,36 +332,37 @@ static int mtk_pconf_parse_conf(struct pinctrl_dev *pctldev,
 		unsigned int pin, enum pin_config_param param,
 		enum pin_config_param arg)
 {
+	int ret = 0;
 	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
 
 	switch (param) {
 	case PIN_CONFIG_BIAS_DISABLE:
-		mtk_pconf_set_pull_select(pctl, pin, false, false, arg);
+		ret = mtk_pconf_set_pull_select(pctl, pin, false, false, arg);
 		break;
 	case PIN_CONFIG_BIAS_PULL_UP:
-		mtk_pconf_set_pull_select(pctl, pin, true, true, arg);
+		ret = mtk_pconf_set_pull_select(pctl, pin, true, true, arg);
 		break;
 	case PIN_CONFIG_BIAS_PULL_DOWN:
-		mtk_pconf_set_pull_select(pctl, pin, true, false, arg);
+		ret = mtk_pconf_set_pull_select(pctl, pin, true, false, arg);
 		break;
 	case PIN_CONFIG_INPUT_ENABLE:
-		mtk_pconf_set_ies_smt(pctl, pin, arg, param);
+		ret = mtk_pconf_set_ies_smt(pctl, pin, arg, param);
 		break;
 	case PIN_CONFIG_OUTPUT:
 		mtk_gpio_set(pctl->chip, pin, arg);
-		mtk_pmx_gpio_set_direction(pctldev, NULL, pin, false);
+		ret = mtk_pmx_gpio_set_direction(pctldev, NULL, pin, false);
 		break;
 	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
-		mtk_pconf_set_ies_smt(pctl, pin, arg, param);
+		ret = mtk_pconf_set_ies_smt(pctl, pin, arg, param);
 		break;
 	case PIN_CONFIG_DRIVE_STRENGTH:
-		mtk_pconf_set_driving(pctl, pin, arg);
+		ret = mtk_pconf_set_driving(pctl, pin, arg);
 		break;
 	default:
-		return -EINVAL;
+		ret = -EINVAL;
 	}
 
-	return 0;
+	return ret;
 }
 
 static int mtk_pconf_group_get(struct pinctrl_dev *pctldev,
@@ -283,12 +381,14 @@ static int mtk_pconf_group_set(struct pinctrl_dev *pctldev, unsigned group,
 {
 	struct mtk_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
 	struct mtk_pinctrl_group *g = &pctl->groups[group];
-	int i;
+	int i, ret;
 
 	for (i = 0; i < num_configs; i++) {
-		mtk_pconf_parse_conf(pctldev, g->pin,
+		ret = mtk_pconf_parse_conf(pctldev, g->pin,
 			pinconf_to_config_param(configs[i]),
 			pinconf_to_config_argument(configs[i]));
+		if (ret < 0)
+			return ret;
 
 		g->config = configs[i];
 	}
@@ -1109,7 +1209,8 @@ static struct pinctrl_desc mtk_pctrl_desc = {
 };
 
 int mtk_pctrl_init(struct platform_device *pdev,
-		const struct mtk_pinctrl_devdata *data)
+		const struct mtk_pinctrl_devdata *data,
+		struct regmap *regmap)
 {
 	struct pinctrl_pin_desc *pins;
 	struct mtk_pinctrl *pctl;
@@ -1135,6 +1236,11 @@ int mtk_pctrl_init(struct platform_device *pdev,
 		pctl->regmap1 = syscon_node_to_regmap(node);
 		if (IS_ERR(pctl->regmap1))
 			return PTR_ERR(pctl->regmap1);
+	} else if (regmap) {
+		pctl->regmap1  = regmap;
+	} else {
+		dev_err(&pdev->dev, "Pinctrl node has not register regmap.\n");
+		return -EINVAL;
 	}
 
 	/* Only 8135 has two base addr, other SoCs have only one. */
@@ -1165,9 +1271,9 @@ int mtk_pctrl_init(struct platform_device *pdev,
 	mtk_pctrl_desc.npins = pctl->devdata->npins;
 	pctl->dev = &pdev->dev;
 	pctl->pctl_dev = pinctrl_register(&mtk_pctrl_desc, &pdev->dev, pctl);
-	if (!pctl->pctl_dev) {
+	if (IS_ERR(pctl->pctl_dev)) {
 		dev_err(&pdev->dev, "couldn't register pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(pctl->pctl_dev);
 	}
 
 	pctl->chip = devm_kzalloc(&pdev->dev, sizeof(*pctl->chip), GFP_KERNEL);
@@ -1176,11 +1282,11 @@ int mtk_pctrl_init(struct platform_device *pdev,
 		goto pctrl_error;
 	}
 
-	pctl->chip = &mtk_gpio_chip;
+	*pctl->chip = mtk_gpio_chip;
 	pctl->chip->ngpio = pctl->devdata->npins;
 	pctl->chip->label = dev_name(&pdev->dev);
 	pctl->chip->dev = &pdev->dev;
-	pctl->chip->base = 0;
+	pctl->chip->base = -1;
 
 	ret = gpiochip_add(pctl->chip);
 	if (ret) {
@@ -1196,6 +1302,9 @@ int mtk_pctrl_init(struct platform_device *pdev,
 		goto chip_error;
 	}
 
+	if (!of_property_read_bool(np, "interrupt-controller"))
+		return 0;
+
 	/* Get EINT register base from dts. */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
@@ -1242,8 +1351,7 @@ int mtk_pctrl_init(struct platform_device *pdev,
 		set_irq_flags(virq, IRQF_VALID);
 	};
 
-	irq_set_chained_handler(irq, mtk_eint_irq_handler);
-	irq_set_handler_data(irq, pctl);
+	irq_set_chained_handler_and_data(irq, mtk_eint_irq_handler, pctl);
 	set_irq_flags(irq, IRQF_VALID);
 	return 0;
 
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-common.h b/drivers/pinctrl/mediatek/pinctrl-mtk-common.h
index 375771db9bd0..30213e514c2f 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mtk-common.h
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-common.h
@@ -17,16 +17,17 @@
 
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/regmap.h>
+#include <linux/pinctrl/pinconf-generic.h>
 
 #define NO_EINT_SUPPORT    255
-#define MTK_CHIP_TYPE_BASE     0
-#define MTK_CHIP_TYPE_PMIC     1
 #define MT_EDGE_SENSITIVE           0
 #define MT_LEVEL_SENSITIVE          1
 #define EINT_DBNC_SET_DBNC_BITS     4
 #define EINT_DBNC_RST_BIT           (0x1 << 1)
 #define EINT_DBNC_SET_EN            (0x1 << 0)
 
+#define MTK_PINCTRL_NOT_SUPPORT	(0xffff)
+
 struct mtk_desc_function {
 	const char *name;
 	unsigned char muxval;
@@ -39,7 +40,6 @@ struct mtk_desc_eint {
 
 struct mtk_desc_pin {
 	struct pinctrl_pin_desc	pin;
-	const char *chip;
 	const struct mtk_desc_eint eint;
 	const struct mtk_desc_function	*functions;
 };
@@ -47,7 +47,6 @@ struct mtk_desc_pin {
 #define MTK_PIN(_pin, _pad, _chip, _eint, ...)		\
 	{							\
 		.pin = _pin,					\
-		.chip = _chip,					\
 		.eint = _eint,					\
 		.functions = (struct mtk_desc_function[]){	\
 			__VA_ARGS__, { } },			\
@@ -107,8 +106,8 @@ struct mtk_drv_group_desc {
  * @grp: The group for this pin belongs to.
  */
 struct mtk_pin_drv_grp {
-	unsigned int pin;
-	unsigned int offset;
+	unsigned short pin;
+	unsigned short offset;
 	unsigned char bit;
 	unsigned char grp;
 };
@@ -121,6 +120,54 @@ struct mtk_pin_drv_grp {
 		.grp = _grp,	\
 	}
 
+/**
+ * struct mtk_pin_spec_pupd_set_samereg
+ * - For special pins' pull up/down setting which resides in same register
+ * @pin: The pin number.
+ * @offset: The offset of special pull up/down setting register.
+ * @pupd_bit: The pull up/down bit in this register.
+ * @r0_bit: The r0 bit of pull resistor.
+ * @r1_bit: The r1 bit of pull resistor.
+ */
+struct mtk_pin_spec_pupd_set_samereg {
+	unsigned short pin;
+	unsigned short offset;
+	unsigned char pupd_bit;
+	unsigned char r1_bit;
+	unsigned char r0_bit;
+};
+
+#define MTK_PIN_PUPD_SPEC_SR(_pin, _offset, _pupd, _r1, _r0)	\
+	{	\
+		.pin = _pin,	\
+		.offset = _offset,	\
+		.pupd_bit = _pupd,	\
+		.r1_bit = _r1,		\
+		.r0_bit = _r0,		\
+	}
+
+/**
+ * struct mtk_pin_ies_set - For special pins' ies and smt setting.
+ * @start: The start pin number of those special pins.
+ * @end: The end pin number of those special pins.
+ * @offset: The offset of special setting register.
+ * @bit: The bit of special setting register.
+ */
+struct mtk_pin_ies_smt_set {
+	unsigned short start;
+	unsigned short end;
+	unsigned short offset;
+	unsigned char bit;
+};
+
+#define MTK_PIN_IES_SMT_SPEC(_start, _end, _offset, _bit)	\
+	{	\
+		.start = _start,	\
+		.end = _end,	\
+		.bit = _bit,	\
+		.offset = _offset,	\
+	}
+
 struct mtk_eint_offsets {
 	const char *name;
 	unsigned int  stat;
@@ -186,14 +233,13 @@ struct mtk_pinctrl_devdata {
 	int (*spec_pull_set)(struct regmap *reg, unsigned int pin,
 			unsigned char align, bool isup, unsigned int arg);
 	int (*spec_ies_smt_set)(struct regmap *reg, unsigned int pin,
-			unsigned char align, int value);
+			unsigned char align, int value, enum pin_config_param arg);
 	unsigned int dir_offset;
 	unsigned int ies_offset;
 	unsigned int smt_offset;
 	unsigned int pullen_offset;
 	unsigned int pullsel_offset;
 	unsigned int drv_offset;
-	unsigned int invser_offset;
 	unsigned int dout_offset;
 	unsigned int din_offset;
 	unsigned int pinmux_offset;
@@ -202,7 +248,6 @@ struct mtk_pinctrl_devdata {
 	unsigned char  port_shf;
 	unsigned char  port_mask;
 	unsigned char  port_align;
-	unsigned char	chip_type;
 	struct mtk_eint_offsets eint_offsets;
 	unsigned int	ap_num;
 	unsigned int	db_cnt;
@@ -224,6 +269,16 @@ struct mtk_pinctrl {
 };
 
 int mtk_pctrl_init(struct platform_device *pdev,
-		const struct mtk_pinctrl_devdata *data);
+		const struct mtk_pinctrl_devdata *data,
+		struct regmap *regmap);
+
+int mtk_pctrl_spec_pull_set_samereg(struct regmap *regmap,
+		const struct mtk_pin_spec_pupd_set_samereg *pupd_infos,
+		unsigned int info_num, unsigned int pin,
+		unsigned char align, bool isup, unsigned int r1r0);
+
+int mtk_pconf_spec_set_ies_smt_range(struct regmap *regmap,
+		const struct mtk_pin_ies_smt_set *ies_smt_infos, unsigned int info_num,
+		unsigned int pin, unsigned char align, int value);
 
 #endif /* __PINCTRL_MTK_COMMON_H */
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-mt6397.h b/drivers/pinctrl/mediatek/pinctrl-mtk-mt6397.h
new file mode 100644
index 000000000000..4eb98ddb40a4
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-mt6397.h
@@ -0,0 +1,424 @@
+#ifndef __PINCTRL_MTK_MT6397_H
+#define __PINCTRL_MTK_MT6397_H
+
+#include <linux/pinctrl/pinctrl.h>
+#include "pinctrl-mtk-common.h"
+
+static const struct mtk_desc_pin mtk_pins_mt6397[] = {
+	MTK_PIN(
+		PINCTRL_PIN(0, "INT"),
+		"N2", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO0"),
+		MTK_FUNCTION(1, "INT")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(1, "SRCVOLTEN"),
+		"M4", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO1"),
+		MTK_FUNCTION(1, "SRCVOLTEN"),
+		MTK_FUNCTION(6, "TEST_CK1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(2, "SRCLKEN_PERI"),
+		"M2", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO2"),
+		MTK_FUNCTION(1, "SRCLKEN_PERI"),
+		MTK_FUNCTION(6, "TEST_CK2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(3, "RTC_32K1V8"),
+		"K3", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO3"),
+		MTK_FUNCTION(1, "RTC_32K1V8"),
+		MTK_FUNCTION(6, "TEST_CK3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(4, "WRAP_EVENT"),
+		"J2", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO4"),
+		MTK_FUNCTION(1, "WRAP_EVENT")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(5, "SPI_CLK"),
+		"L4", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO5"),
+		MTK_FUNCTION(1, "SPI_CLK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(6, "SPI_CSN"),
+		"J3", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO6"),
+		MTK_FUNCTION(1, "SPI_CSN")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(7, "SPI_MOSI"),
+		"J1", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO7"),
+		MTK_FUNCTION(1, "SPI_MOSI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(8, "SPI_MISO"),
+		"L3", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO8"),
+		MTK_FUNCTION(1, "SPI_MISO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(9, "AUD_CLK_MOSI"),
+		"H2", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO9"),
+		MTK_FUNCTION(1, "AUD_CLK"),
+		MTK_FUNCTION(6, "TEST_IN0"),
+		MTK_FUNCTION(7, "TEST_OUT0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(10, "AUD_DAT_MISO"),
+		"H3", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO10"),
+		MTK_FUNCTION(1, "AUD_MISO"),
+		MTK_FUNCTION(6, "TEST_IN1"),
+		MTK_FUNCTION(7, "TEST_OUT1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(11, "AUD_DAT_MOSI"),
+		"H1", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO11"),
+		MTK_FUNCTION(1, "AUD_MOSI"),
+		MTK_FUNCTION(6, "TEST_IN2"),
+		MTK_FUNCTION(7, "TEST_OUT2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(12, "COL0"),
+		"F3", "mt6397",
+		MTK_EINT_FUNCTION(2, 10),
+		MTK_FUNCTION(0, "GPIO12"),
+		MTK_FUNCTION(1, "COL0_USBDL"),
+		MTK_FUNCTION(2, "EINT10_1X"),
+		MTK_FUNCTION(3, "PWM1_3X"),
+		MTK_FUNCTION(6, "TEST_IN3"),
+		MTK_FUNCTION(7, "TEST_OUT3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(13, "COL1"),
+		"G8", "mt6397",
+		MTK_EINT_FUNCTION(2, 11),
+		MTK_FUNCTION(0, "GPIO13"),
+		MTK_FUNCTION(1, "COL1"),
+		MTK_FUNCTION(2, "EINT11_1X"),
+		MTK_FUNCTION(3, "SCL0_2X"),
+		MTK_FUNCTION(6, "TEST_IN4"),
+		MTK_FUNCTION(7, "TEST_OUT4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(14, "COL2"),
+		"H4", "mt6397",
+		MTK_EINT_FUNCTION(2, 12),
+		MTK_FUNCTION(0, "GPIO14"),
+		MTK_FUNCTION(1, "COL2"),
+		MTK_FUNCTION(2, "EINT12_1X"),
+		MTK_FUNCTION(3, "SDA0_2X"),
+		MTK_FUNCTION(6, "TEST_IN5"),
+		MTK_FUNCTION(7, "TEST_OUT5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(15, "COL3"),
+		"G2", "mt6397",
+		MTK_EINT_FUNCTION(2, 13),
+		MTK_FUNCTION(0, "GPIO15"),
+		MTK_FUNCTION(1, "COL3"),
+		MTK_FUNCTION(2, "EINT13_1X"),
+		MTK_FUNCTION(3, "SCL1_2X"),
+		MTK_FUNCTION(6, "TEST_IN6"),
+		MTK_FUNCTION(7, "TEST_OUT6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(16, "COL4"),
+		"F2", "mt6397",
+		MTK_EINT_FUNCTION(2, 14),
+		MTK_FUNCTION(0, "GPIO16"),
+		MTK_FUNCTION(1, "COL4"),
+		MTK_FUNCTION(2, "EINT14_1X"),
+		MTK_FUNCTION(3, "SDA1_2X"),
+		MTK_FUNCTION(6, "TEST_IN7"),
+		MTK_FUNCTION(7, "TEST_OUT7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(17, "COL5"),
+		"G7", "mt6397",
+		MTK_EINT_FUNCTION(2, 15),
+		MTK_FUNCTION(0, "GPIO17"),
+		MTK_FUNCTION(1, "COL5"),
+		MTK_FUNCTION(2, "EINT15_1X"),
+		MTK_FUNCTION(3, "SCL2_2X"),
+		MTK_FUNCTION(6, "TEST_IN8"),
+		MTK_FUNCTION(7, "TEST_OUT8")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(18, "COL6"),
+		"J6", "mt6397",
+		MTK_EINT_FUNCTION(2, 16),
+		MTK_FUNCTION(0, "GPIO18"),
+		MTK_FUNCTION(1, "COL6"),
+		MTK_FUNCTION(2, "EINT16_1X"),
+		MTK_FUNCTION(3, "SDA2_2X"),
+		MTK_FUNCTION(4, "GPIO32K_0"),
+		MTK_FUNCTION(5, "GPIO26M_0"),
+		MTK_FUNCTION(6, "TEST_IN9"),
+		MTK_FUNCTION(7, "TEST_OUT9")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(19, "COL7"),
+		"J5", "mt6397",
+		MTK_EINT_FUNCTION(2, 17),
+		MTK_FUNCTION(0, "GPIO19"),
+		MTK_FUNCTION(1, "COL7"),
+		MTK_FUNCTION(2, "EINT17_1X"),
+		MTK_FUNCTION(3, "PWM2_3X"),
+		MTK_FUNCTION(4, "GPIO32K_1"),
+		MTK_FUNCTION(5, "GPIO26M_1"),
+		MTK_FUNCTION(6, "TEST_IN10"),
+		MTK_FUNCTION(7, "TEST_OUT10")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(20, "ROW0"),
+		"L7", "mt6397",
+		MTK_EINT_FUNCTION(2, 18),
+		MTK_FUNCTION(0, "GPIO20"),
+		MTK_FUNCTION(1, "ROW0"),
+		MTK_FUNCTION(2, "EINT18_1X"),
+		MTK_FUNCTION(3, "SCL0_3X"),
+		MTK_FUNCTION(6, "TEST_IN11"),
+		MTK_FUNCTION(7, "TEST_OUT11")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(21, "ROW1"),
+		"P1", "mt6397",
+		MTK_EINT_FUNCTION(2, 19),
+		MTK_FUNCTION(0, "GPIO21"),
+		MTK_FUNCTION(1, "ROW1"),
+		MTK_FUNCTION(2, "EINT19_1X"),
+		MTK_FUNCTION(3, "SDA0_3X"),
+		MTK_FUNCTION(4, "AUD_TSTCK"),
+		MTK_FUNCTION(6, "TEST_IN12"),
+		MTK_FUNCTION(7, "TEST_OUT12")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(22, "ROW2"),
+		"J8", "mt6397",
+		MTK_EINT_FUNCTION(2, 20),
+		MTK_FUNCTION(0, "GPIO22"),
+		MTK_FUNCTION(1, "ROW2"),
+		MTK_FUNCTION(2, "EINT20_1X"),
+		MTK_FUNCTION(3, "SCL1_3X"),
+		MTK_FUNCTION(6, "TEST_IN13"),
+		MTK_FUNCTION(7, "TEST_OUT13")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(23, "ROW3"),
+		"J7", "mt6397",
+		MTK_EINT_FUNCTION(2, 21),
+		MTK_FUNCTION(0, "GPIO23"),
+		MTK_FUNCTION(1, "ROW3"),
+		MTK_FUNCTION(2, "EINT21_1X"),
+		MTK_FUNCTION(3, "SDA1_3X"),
+		MTK_FUNCTION(6, "TEST_IN14"),
+		MTK_FUNCTION(7, "TEST_OUT14")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(24, "ROW4"),
+		"L5", "mt6397",
+		MTK_EINT_FUNCTION(2, 22),
+		MTK_FUNCTION(0, "GPIO24"),
+		MTK_FUNCTION(1, "ROW4"),
+		MTK_FUNCTION(2, "EINT22_1X"),
+		MTK_FUNCTION(3, "SCL2_3X"),
+		MTK_FUNCTION(6, "TEST_IN15"),
+		MTK_FUNCTION(7, "TEST_OUT15")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(25, "ROW5"),
+		"N6", "mt6397",
+		MTK_EINT_FUNCTION(2, 23),
+		MTK_FUNCTION(0, "GPIO25"),
+		MTK_FUNCTION(1, "ROW5"),
+		MTK_FUNCTION(2, "EINT23_1X"),
+		MTK_FUNCTION(3, "SDA2_3X"),
+		MTK_FUNCTION(6, "TEST_IN16"),
+		MTK_FUNCTION(7, "TEST_OUT16")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(26, "ROW6"),
+		"L6", "mt6397",
+		MTK_EINT_FUNCTION(2, 24),
+		MTK_FUNCTION(0, "GPIO26"),
+		MTK_FUNCTION(1, "ROW6"),
+		MTK_FUNCTION(2, "EINT24_1X"),
+		MTK_FUNCTION(3, "PWM3_3X"),
+		MTK_FUNCTION(4, "GPIO32K_2"),
+		MTK_FUNCTION(5, "GPIO26M_2"),
+		MTK_FUNCTION(6, "TEST_IN17"),
+		MTK_FUNCTION(7, "TEST_OUT17")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(27, "ROW7"),
+		"P2", "mt6397",
+		MTK_EINT_FUNCTION(2, 3),
+		MTK_FUNCTION(0, "GPIO27"),
+		MTK_FUNCTION(1, "ROW7"),
+		MTK_FUNCTION(2, "EINT3_1X"),
+		MTK_FUNCTION(3, "CBUS"),
+		MTK_FUNCTION(4, "GPIO32K_3"),
+		MTK_FUNCTION(5, "GPIO26M_3"),
+		MTK_FUNCTION(6, "TEST_IN18"),
+		MTK_FUNCTION(7, "TEST_OUT18")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(28, "PWM1(VMSEL1)"),
+		"J4", "mt6397",
+		MTK_EINT_FUNCTION(2, 4),
+		MTK_FUNCTION(0, "GPIO28"),
+		MTK_FUNCTION(1, "PWM1"),
+		MTK_FUNCTION(2, "EINT4_1X"),
+		MTK_FUNCTION(4, "GPIO32K_4"),
+		MTK_FUNCTION(5, "GPIO26M_4"),
+		MTK_FUNCTION(6, "TEST_IN19"),
+		MTK_FUNCTION(7, "TEST_OUT19")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(29, "PWM2(VMSEL2)"),
+		"N5", "mt6397",
+		MTK_EINT_FUNCTION(2, 5),
+		MTK_FUNCTION(0, "GPIO29"),
+		MTK_FUNCTION(1, "PWM2"),
+		MTK_FUNCTION(2, "EINT5_1X"),
+		MTK_FUNCTION(4, "GPIO32K_5"),
+		MTK_FUNCTION(5, "GPIO26M_5"),
+		MTK_FUNCTION(6, "TEST_IN20"),
+		MTK_FUNCTION(7, "TEST_OUT20")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(30, "PWM3(PWM)"),
+		"R3", "mt6397",
+		MTK_EINT_FUNCTION(2, 6),
+		MTK_FUNCTION(0, "GPIO30"),
+		MTK_FUNCTION(1, "PWM3"),
+		MTK_FUNCTION(2, "EINT6_1X"),
+		MTK_FUNCTION(3, "COL0"),
+		MTK_FUNCTION(4, "GPIO32K_6"),
+		MTK_FUNCTION(5, "GPIO26M_6"),
+		MTK_FUNCTION(6, "TEST_IN21"),
+		MTK_FUNCTION(7, "TEST_OUT21")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(31, "SCL0"),
+		"N1", "mt6397",
+		MTK_EINT_FUNCTION(2, 7),
+		MTK_FUNCTION(0, "GPIO31"),
+		MTK_FUNCTION(1, "SCL0"),
+		MTK_FUNCTION(2, "EINT7_1X"),
+		MTK_FUNCTION(3, "PWM1_2X"),
+		MTK_FUNCTION(6, "TEST_IN22"),
+		MTK_FUNCTION(7, "TEST_OUT22")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(32, "SDA0"),
+		"N3", "mt6397",
+		MTK_EINT_FUNCTION(2, 8),
+		MTK_FUNCTION(0, "GPIO32"),
+		MTK_FUNCTION(1, "SDA0"),
+		MTK_FUNCTION(2, "EINT8_1X"),
+		MTK_FUNCTION(6, "TEST_IN23"),
+		MTK_FUNCTION(7, "TEST_OUT23")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(33, "SCL1"),
+		"T1", "mt6397",
+		MTK_EINT_FUNCTION(2, 9),
+		MTK_FUNCTION(0, "GPIO33"),
+		MTK_FUNCTION(1, "SCL1"),
+		MTK_FUNCTION(2, "EINT9_1X"),
+		MTK_FUNCTION(3, "PWM2_2X"),
+		MTK_FUNCTION(6, "TEST_IN24"),
+		MTK_FUNCTION(7, "TEST_OUT24")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(34, "SDA1"),
+		"T2", "mt6397",
+		MTK_EINT_FUNCTION(2, 0),
+		MTK_FUNCTION(0, "GPIO34"),
+		MTK_FUNCTION(1, "SDA1"),
+		MTK_FUNCTION(2, "EINT0_1X"),
+		MTK_FUNCTION(6, "TEST_IN25"),
+		MTK_FUNCTION(7, "TEST_OUT25")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(35, "SCL2"),
+		"T3", "mt6397",
+		MTK_EINT_FUNCTION(2, 1),
+		MTK_FUNCTION(0, "GPIO35"),
+		MTK_FUNCTION(1, "SCL2"),
+		MTK_FUNCTION(2, "EINT1_1X"),
+		MTK_FUNCTION(3, "PWM3_2X"),
+		MTK_FUNCTION(6, "TEST_IN26"),
+		MTK_FUNCTION(7, "TEST_OUT26")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(36, "SDA2"),
+		"U2", "mt6397",
+		MTK_EINT_FUNCTION(2, 2),
+		MTK_FUNCTION(0, "GPIO36"),
+		MTK_FUNCTION(1, "SDA2"),
+		MTK_FUNCTION(2, "EINT2_1X"),
+		MTK_FUNCTION(6, "TEST_IN27"),
+		MTK_FUNCTION(7, "TEST_OUT27")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(37, "HDMISD"),
+		"H6", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO37"),
+		MTK_FUNCTION(1, "HDMISD"),
+		MTK_FUNCTION(6, "TEST_IN28"),
+		MTK_FUNCTION(7, "TEST_OUT28")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(38, "HDMISCK"),
+		"H5", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO38"),
+		MTK_FUNCTION(1, "HDMISCK"),
+		MTK_FUNCTION(6, "TEST_IN29"),
+		MTK_FUNCTION(7, "TEST_OUT29")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(39, "HTPLG"),
+		"H7", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO39"),
+		MTK_FUNCTION(1, "HTPLG"),
+		MTK_FUNCTION(6, "TEST_IN30"),
+		MTK_FUNCTION(7, "TEST_OUT30")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(40, "CEC"),
+		"J9", "mt6397",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO40"),
+		MTK_FUNCTION(1, "CEC"),
+		MTK_FUNCTION(6, "TEST_IN31"),
+		MTK_FUNCTION(7, "TEST_OUT31")
+	),
+};
+
+#endif /* __PINCTRL_MTK_MT6397_H */
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-mt8127.h b/drivers/pinctrl/mediatek/pinctrl-mtk-mt8127.h
new file mode 100644
index 000000000000..212559c147f8
--- /dev/null
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-mt8127.h
@@ -0,0 +1,1318 @@
+#ifndef __PINCTRL_MTK_MT8127_H
+#define __PINCTRL_MTK_MT8127_H
+
+#include <linux/pinctrl/pinctrl.h>
+#include "pinctrl-mtk-common.h"
+
+static const struct mtk_desc_pin mtk_pins_mt8127[] = {
+	MTK_PIN(
+		PINCTRL_PIN(0, "PWRAP_SPI0_MI"),
+		"P22", "mt8127",
+		MTK_EINT_FUNCTION(0, 22),
+		MTK_FUNCTION(0, "GPIO0"),
+		MTK_FUNCTION(1, "PWRAP_SPIDO"),
+		MTK_FUNCTION(2, "PWRAP_SPIDI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(1, "PWRAP_SPI0_MO"),
+		"M22", "mt8127",
+		MTK_EINT_FUNCTION(0, 23),
+		MTK_FUNCTION(0, "GPIO1"),
+		MTK_FUNCTION(1, "PWRAP_SPIDI"),
+		MTK_FUNCTION(2, "PWRAP_SPIDO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(2, "PWRAP_INT"),
+		"L23", "mt8127",
+		MTK_EINT_FUNCTION(0, 24),
+		MTK_FUNCTION(0, "GPIO2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(3, "PWRAP_SPI0_CK"),
+		"N23", "mt8127",
+		MTK_EINT_FUNCTION(0, 25),
+		MTK_FUNCTION(0, "GPIO3"),
+		MTK_FUNCTION(1, "PWRAP_SPICK_I")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(4, "PWRAP_SPI0_CSN"),
+		"N22", "mt8127",
+		MTK_EINT_FUNCTION(0, 26),
+		MTK_FUNCTION(0, "GPIO4"),
+		MTK_FUNCTION(1, "PWRAP_SPICS_B_I")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(5, "PWRAP_SPI0_CK2"),
+		"L19", "mt8127",
+		MTK_EINT_FUNCTION(0, 27),
+		MTK_FUNCTION(0, "GPIO5"),
+		MTK_FUNCTION(1, "PWRAP_SPICK2_I"),
+		MTK_FUNCTION(2, "ANT_SEL1"),
+		MTK_FUNCTION(3, "VDEC_TEST_CK"),
+		MTK_FUNCTION(7, "DBG_MON_B[0]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(6, "PWRAP_SPI0_CSN2"),
+		"M23", "mt8127",
+		MTK_EINT_FUNCTION(0, 28),
+		MTK_FUNCTION(0, "GPIO6"),
+		MTK_FUNCTION(1, "PWRAP_SPICS2_B_I"),
+		MTK_FUNCTION(2, "ANT_SEL0"),
+		MTK_FUNCTION(3, "MM_TEST_CK"),
+		MTK_FUNCTION(7, "DBG_MON_B[1]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(7, "AUD_CLK_MOSI"),
+		"K23", "mt8127",
+		MTK_EINT_FUNCTION(0, 29),
+		MTK_FUNCTION(0, "GPIO7"),
+		MTK_FUNCTION(1, "AUD_CLK"),
+		MTK_FUNCTION(2, "ADC_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(8, "AUD_DAT_MISO"),
+		"K24", "mt8127",
+		MTK_EINT_FUNCTION(0, 30),
+		MTK_FUNCTION(0, "GPIO8"),
+		MTK_FUNCTION(1, "AUD_MISO"),
+		MTK_FUNCTION(2, "ADC_DAT_IN"),
+		MTK_FUNCTION(3, "AUD_MOSI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(9, "AUD_DAT_MOSI"),
+		"K22", "mt8127",
+		MTK_EINT_FUNCTION(0, 31),
+		MTK_FUNCTION(0, "GPIO9"),
+		MTK_FUNCTION(1, "AUD_MOSI"),
+		MTK_FUNCTION(2, "ADC_WS"),
+		MTK_FUNCTION(3, "AUD_MISO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(10, "RTC32K_CK"),
+		"R21", "mt8127",
+		MTK_EINT_FUNCTION(0, 32),
+		MTK_FUNCTION(0, "GPIO10"),
+		MTK_FUNCTION(1, "RTC32K_CK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(11, "WATCHDOG"),
+		"P24", "mt8127",
+		MTK_EINT_FUNCTION(0, 33),
+		MTK_FUNCTION(0, "GPIO11"),
+		MTK_FUNCTION(1, "WATCHDOG")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(12, "SRCLKENA"),
+		"R22", "mt8127",
+		MTK_EINT_FUNCTION(0, 34),
+		MTK_FUNCTION(0, "GPIO12"),
+		MTK_FUNCTION(1, "SRCLKENA")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(13, "SRCLKENAI"),
+		"P23", "mt8127",
+		MTK_EINT_FUNCTION(0, 35),
+		MTK_FUNCTION(0, "GPIO13"),
+		MTK_FUNCTION(1, "SRCLKENAI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(14, "URXD2"),
+		"U19", "mt8127",
+		MTK_EINT_FUNCTION(0, 36),
+		MTK_FUNCTION(0, "GPIO14"),
+		MTK_FUNCTION(1, "URXD2"),
+		MTK_FUNCTION(2, "DPI_D5"),
+		MTK_FUNCTION(3, "UTXD2"),
+		MTK_FUNCTION(5, "SRCCLKENAI2"),
+		MTK_FUNCTION(6, "KROW4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(15, "UTXD2"),
+		"U20", "mt8127",
+		MTK_EINT_FUNCTION(0, 37),
+		MTK_FUNCTION(0, "GPIO15"),
+		MTK_FUNCTION(1, "UTXD2"),
+		MTK_FUNCTION(2, "DPI_HSYNC"),
+		MTK_FUNCTION(3, "URXD2"),
+		MTK_FUNCTION(6, "KROW5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(16, "URXD3"),
+		"U18", "mt8127",
+		MTK_EINT_FUNCTION(0, 38),
+		MTK_FUNCTION(0, "GPIO16"),
+		MTK_FUNCTION(1, "URXD3"),
+		MTK_FUNCTION(2, "DPI_DE"),
+		MTK_FUNCTION(3, "UTXD3"),
+		MTK_FUNCTION(4, "UCTS2"),
+		MTK_FUNCTION(5, "PWM3"),
+		MTK_FUNCTION(6, "KROW6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(17, "UTXD3"),
+		"R18", "mt8127",
+		MTK_EINT_FUNCTION(0, 39),
+		MTK_FUNCTION(0, "GPIO17"),
+		MTK_FUNCTION(1, "UTXD3"),
+		MTK_FUNCTION(2, "DPI_VSYNC"),
+		MTK_FUNCTION(3, "URXD3"),
+		MTK_FUNCTION(4, "URTS2"),
+		MTK_FUNCTION(5, "PWM4"),
+		MTK_FUNCTION(6, "KROW7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(18, "PCM_CLK"),
+		"U22", "mt8127",
+		MTK_EINT_FUNCTION(0, 40),
+		MTK_FUNCTION(0, "GPIO18"),
+		MTK_FUNCTION(1, "PCM_CLK0"),
+		MTK_FUNCTION(2, "DPI_D4"),
+		MTK_FUNCTION(3, "I2SIN1_BCK0"),
+		MTK_FUNCTION(4, "I2SOUT_BCK"),
+		MTK_FUNCTION(5, "CONN_DSP_JCK"),
+		MTK_FUNCTION(6, "IR"),
+		MTK_FUNCTION(7, "DBG_MON_A[0]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(19, "PCM_SYNC"),
+		"U23", "mt8127",
+		MTK_EINT_FUNCTION(0, 41),
+		MTK_FUNCTION(0, "GPIO19"),
+		MTK_FUNCTION(1, "PCM_SYNC"),
+		MTK_FUNCTION(2, "DPI_D3"),
+		MTK_FUNCTION(3, "I2SIN1_LRCK"),
+		MTK_FUNCTION(4, "I2SOUT_LRCK"),
+		MTK_FUNCTION(5, "CONN_DSP_JINTP"),
+		MTK_FUNCTION(6, "EXT_COL"),
+		MTK_FUNCTION(7, "DBG_MON_A[1]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(20, "PCM_RX"),
+		"V22", "mt8127",
+		MTK_EINT_FUNCTION(0, 42),
+		MTK_FUNCTION(0, "GPIO20"),
+		MTK_FUNCTION(1, "PCM_RX"),
+		MTK_FUNCTION(2, "DPI_D1"),
+		MTK_FUNCTION(3, "I2SIN1_DATA_IN"),
+		MTK_FUNCTION(4, "PCM_TX"),
+		MTK_FUNCTION(5, "CONN_DSP_JDI"),
+		MTK_FUNCTION(6, "EXT_MDIO"),
+		MTK_FUNCTION(7, "DBG_MON_A[2]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(21, "PCM_TX"),
+		"U21", "mt8127",
+		MTK_EINT_FUNCTION(0, 43),
+		MTK_FUNCTION(0, "GPIO21"),
+		MTK_FUNCTION(1, "PCM_TX"),
+		MTK_FUNCTION(2, "DPI_D2"),
+		MTK_FUNCTION(3, "I2SOUT_DATA_OUT"),
+		MTK_FUNCTION(4, "PCM_RX"),
+		MTK_FUNCTION(5, "CONN_DSP_JMS"),
+		MTK_FUNCTION(6, "EXT_MDC"),
+		MTK_FUNCTION(7, "DBG_MON_A[3]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(22, "EINT0"),
+		"AB19", "mt8127",
+		MTK_EINT_FUNCTION(0, 0),
+		MTK_FUNCTION(0, "GPIO22"),
+		MTK_FUNCTION(1, "PWM1"),
+		MTK_FUNCTION(2, "DPI_CK"),
+		MTK_FUNCTION(4, "EXT_TXD0"),
+		MTK_FUNCTION(5, "CONN_DSP_JDO"),
+		MTK_FUNCTION(7, "DBG_MON_A[4]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(23, "EINT1"),
+		"AA21", "mt8127",
+		MTK_EINT_FUNCTION(0, 1),
+		MTK_FUNCTION(0, "GPIO23"),
+		MTK_FUNCTION(1, "PWM2"),
+		MTK_FUNCTION(2, "DPI_D12"),
+		MTK_FUNCTION(4, "EXT_TXD1"),
+		MTK_FUNCTION(5, "CONN_MCU_TDO"),
+		MTK_FUNCTION(7, "DBG_MON_A[5]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(24, "EINT2"),
+		"AA19", "mt8127",
+		MTK_EINT_FUNCTION(0, 2),
+		MTK_FUNCTION(0, "GPIO24"),
+		MTK_FUNCTION(1, "CLKM0"),
+		MTK_FUNCTION(2, "DPI_D13"),
+		MTK_FUNCTION(4, "EXT_TXD2"),
+		MTK_FUNCTION(5, "CONN_MCU_DBGACK_N"),
+		MTK_FUNCTION(6, "KCOL4"),
+		MTK_FUNCTION(7, "DBG_MON_A[6]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(25, "EINT3"),
+		"Y19", "mt8127",
+		MTK_EINT_FUNCTION(0, 3),
+		MTK_FUNCTION(0, "GPIO25"),
+		MTK_FUNCTION(1, "CLKM1"),
+		MTK_FUNCTION(2, "DPI_D14"),
+		MTK_FUNCTION(3, "SPI_MI"),
+		MTK_FUNCTION(4, "EXT_TXD3"),
+		MTK_FUNCTION(5, "CONN_MCU_DBGI_N"),
+		MTK_FUNCTION(6, "KCOL5"),
+		MTK_FUNCTION(7, "DBG_MON_A[7]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(26, "EINT4"),
+		"V21", "mt8127",
+		MTK_EINT_FUNCTION(0, 4),
+		MTK_FUNCTION(0, "GPIO26"),
+		MTK_FUNCTION(1, "CLKM2"),
+		MTK_FUNCTION(2, "DPI_D15"),
+		MTK_FUNCTION(3, "SPI_MO"),
+		MTK_FUNCTION(4, "EXT_TXC"),
+		MTK_FUNCTION(5, "CONN_MCU_TCK0"),
+		MTK_FUNCTION(6, "CONN_MCU_AICE_JCKC"),
+		MTK_FUNCTION(7, "DBG_MON_A[8]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(27, "EINT5"),
+		"AB22", "mt8127",
+		MTK_EINT_FUNCTION(0, 5),
+		MTK_FUNCTION(0, "GPIO27"),
+		MTK_FUNCTION(1, "UCTS2"),
+		MTK_FUNCTION(2, "DPI_D16"),
+		MTK_FUNCTION(3, "SPI_CS"),
+		MTK_FUNCTION(4, "EXT_RXER"),
+		MTK_FUNCTION(5, "CONN_MCU_TDI"),
+		MTK_FUNCTION(6, "KCOL6"),
+		MTK_FUNCTION(7, "DBG_MON_A[9]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(28, "EINT6"),
+		"AA23", "mt8127",
+		MTK_EINT_FUNCTION(0, 6),
+		MTK_FUNCTION(0, "GPIO28"),
+		MTK_FUNCTION(1, "URTS2"),
+		MTK_FUNCTION(2, "DPI_D17"),
+		MTK_FUNCTION(3, "SPI_CK"),
+		MTK_FUNCTION(4, "EXT_RXC"),
+		MTK_FUNCTION(5, "CONN_MCU_TRST_B"),
+		MTK_FUNCTION(6, "KCOL7"),
+		MTK_FUNCTION(7, "DBG_MON_A[10]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(29, "EINT7"),
+		"Y23", "mt8127",
+		MTK_EINT_FUNCTION(0, 7),
+		MTK_FUNCTION(0, "GPIO29"),
+		MTK_FUNCTION(1, "UCTS3"),
+		MTK_FUNCTION(2, "DPI_D6"),
+		MTK_FUNCTION(3, "SDA1"),
+		MTK_FUNCTION(4, "EXT_RXDV"),
+		MTK_FUNCTION(5, "CONN_MCU_TMS"),
+		MTK_FUNCTION(6, "CONN_MCU_AICE_JMSC"),
+		MTK_FUNCTION(7, "DBG_MON_A[11]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(30, "EINT8"),
+		"Y24", "mt8127",
+		MTK_EINT_FUNCTION(0, 8),
+		MTK_FUNCTION(0, "GPIO30"),
+		MTK_FUNCTION(1, "URTS3"),
+		MTK_FUNCTION(2, "CLKM3"),
+		MTK_FUNCTION(3, "SCL1"),
+		MTK_FUNCTION(4, "EXT_RXD0"),
+		MTK_FUNCTION(5, "ANT_SEL0"),
+		MTK_FUNCTION(6, "DPI_D7"),
+		MTK_FUNCTION(7, "DBG_MON_B[2]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(31, "EINT9"),
+		"W23", "mt8127",
+		MTK_EINT_FUNCTION(0, 9),
+		MTK_FUNCTION(0, "GPIO31"),
+		MTK_FUNCTION(1, "CLKM4"),
+		MTK_FUNCTION(2, "SDA2"),
+		MTK_FUNCTION(3, "EXT_FRAME_SYNC"),
+		MTK_FUNCTION(4, "EXT_RXD1"),
+		MTK_FUNCTION(5, "ANT_SEL1"),
+		MTK_FUNCTION(6, "DPI_D8"),
+		MTK_FUNCTION(7, "DBG_MON_B[3]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(32, "EINT10"),
+		"W24", "mt8127",
+		MTK_EINT_FUNCTION(0, 10),
+		MTK_FUNCTION(0, "GPIO32"),
+		MTK_FUNCTION(1, "CLKM5"),
+		MTK_FUNCTION(2, "SCL2"),
+		MTK_FUNCTION(3, "EXT_FRAME_SYNC"),
+		MTK_FUNCTION(4, "EXT_RXD2"),
+		MTK_FUNCTION(5, "ANT_SEL2"),
+		MTK_FUNCTION(6, "DPI_D9"),
+		MTK_FUNCTION(7, "DBG_MON_B[4]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(33, "KPROW0"),
+		"AB24", "mt8127",
+		MTK_EINT_FUNCTION(0, 44),
+		MTK_FUNCTION(0, "GPIO33"),
+		MTK_FUNCTION(1, "KROW0"),
+		MTK_FUNCTION(4, "IMG_TEST_CK"),
+		MTK_FUNCTION(7, "DBG_MON_A[12]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(34, "KPROW1"),
+		"AC24", "mt8127",
+		MTK_EINT_FUNCTION(0, 45),
+		MTK_FUNCTION(0, "GPIO34"),
+		MTK_FUNCTION(1, "KROW1"),
+		MTK_FUNCTION(2, "IDDIG"),
+		MTK_FUNCTION(3, "EXT_FRAME_SYNC"),
+		MTK_FUNCTION(4, "MFG_TEST_CK"),
+		MTK_FUNCTION(7, "DBG_MON_B[5]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(35, "KPROW2"),
+		"AD24", "mt8127",
+		MTK_EINT_FUNCTION(0, 46),
+		MTK_FUNCTION(0, "GPIO35"),
+		MTK_FUNCTION(1, "KROW2"),
+		MTK_FUNCTION(2, "DRV_VBUS"),
+		MTK_FUNCTION(3, "EXT_FRAME_SYNC"),
+		MTK_FUNCTION(4, "CONN_TEST_CK"),
+		MTK_FUNCTION(7, "DBG_MON_B[6]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(36, "KPCOL0"),
+		"AB23", "mt8127",
+		MTK_EINT_FUNCTION(0, 47),
+		MTK_FUNCTION(0, "GPIO36"),
+		MTK_FUNCTION(1, "KCOL0"),
+		MTK_FUNCTION(7, "DBG_MON_A[13]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(37, "KPCOL1"),
+		"AC22", "mt8127",
+		MTK_EINT_FUNCTION(0, 48),
+		MTK_FUNCTION(0, "GPIO37"),
+		MTK_FUNCTION(1, "KCOL1"),
+		MTK_FUNCTION(7, "DBG_MON_B[7]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(38, "KPCOL2"),
+		"AC23", "mt8127",
+		MTK_EINT_FUNCTION(0, 49),
+		MTK_FUNCTION(0, "GPIO38"),
+		MTK_FUNCTION(1, "KCOL2"),
+		MTK_FUNCTION(2, "IDDIG"),
+		MTK_FUNCTION(3, "EXT_FRAME_SYNC"),
+		MTK_FUNCTION(7, "DBG_MON_B[8]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(39, "JTMS"),
+		"V18", "mt8127",
+		MTK_EINT_FUNCTION(0, 50),
+		MTK_FUNCTION(0, "GPIO39"),
+		MTK_FUNCTION(1, "JTMS"),
+		MTK_FUNCTION(2, "CONN_MCU_TMS"),
+		MTK_FUNCTION(3, "CONN_MCU_AICE_JMSC")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(40, "JTCK"),
+		"AA18", "mt8127",
+		MTK_EINT_FUNCTION(0, 51),
+		MTK_FUNCTION(0, "GPIO40"),
+		MTK_FUNCTION(1, "JTCK"),
+		MTK_FUNCTION(2, "CONN_MCU_TCK1"),
+		MTK_FUNCTION(3, "CONN_MCU_AICE_JCKC")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(41, "JTDI"),
+		"W18", "mt8127",
+		MTK_EINT_FUNCTION(0, 52),
+		MTK_FUNCTION(0, "GPIO41"),
+		MTK_FUNCTION(1, "JTDI"),
+		MTK_FUNCTION(2, "CONN_MCU_TDI")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(42, "JTDO"),
+		"Y18", "mt8127",
+		MTK_EINT_FUNCTION(0, 53),
+		MTK_FUNCTION(0, "GPIO42"),
+		MTK_FUNCTION(1, "JTDO"),
+		MTK_FUNCTION(2, "CONN_MCU_TDO")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(43, "EINT11"),
+		"W22", "mt8127",
+		MTK_EINT_FUNCTION(0, 11),
+		MTK_FUNCTION(0, "GPIO43"),
+		MTK_FUNCTION(1, "CLKM4"),
+		MTK_FUNCTION(2, "PWM2"),
+		MTK_FUNCTION(3, "KROW3"),
+		MTK_FUNCTION(4, "ANT_SEL3"),
+		MTK_FUNCTION(5, "DPI_D10"),
+		MTK_FUNCTION(6, "EXT_RXD3"),
+		MTK_FUNCTION(7, "DBG_MON_B[9]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(44, "EINT12"),
+		"V23", "mt8127",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO44"),
+		MTK_FUNCTION(1, "CLKM5"),
+		MTK_FUNCTION(2, "PWM0"),
+		MTK_FUNCTION(3, "KCOL3"),
+		MTK_FUNCTION(4, "ANT_SEL4"),
+		MTK_FUNCTION(5, "DPI_D11"),
+		MTK_FUNCTION(6, "EXT_TXEN"),
+		MTK_FUNCTION(7, "DBG_MON_B[10]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(45, "EINT13"),
+		"Y21", "mt8127",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO45"),
+		MTK_FUNCTION(4, "ANT_SEL5"),
+		MTK_FUNCTION(5, "DPI_D0"),
+		MTK_FUNCTION(6, "SPDIF"),
+		MTK_FUNCTION(7, "DBG_MON_B[11]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(46, "EINT14"),
+		"F23", "mt8127",
+		MTK_EINT_FUNCTION(0, 14),
+		MTK_FUNCTION(0, "GPIO46"),
+		MTK_FUNCTION(2, "DAC_DAT_OUT"),
+		MTK_FUNCTION(4, "ANT_SEL1"),
+		MTK_FUNCTION(5, "CONN_MCU_DBGACK_N"),
+		MTK_FUNCTION(6, "NCLE"),
+		MTK_FUNCTION(7, "DBG_MON_A[14]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(47, "EINT15"),
+		"G23", "mt8127",
+		MTK_EINT_FUNCTION(0, 15),
+		MTK_FUNCTION(0, "GPIO47"),
+		MTK_FUNCTION(2, "DAC_WS"),
+		MTK_FUNCTION(4, "ANT_SEL2"),
+		MTK_FUNCTION(5, "CONN_MCU_DBGI_N"),
+		MTK_FUNCTION(6, "NCEB1"),
+		MTK_FUNCTION(7, "DBG_MON_A[15]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(48, "EINT16"),
+		"H23", "mt8127",
+		MTK_EINT_FUNCTION(0, 16),
+		MTK_FUNCTION(0, "GPIO48"),
+		MTK_FUNCTION(2, "DAC_CK"),
+		MTK_FUNCTION(4, "ANT_SEL3"),
+		MTK_FUNCTION(5, "CONN_MCU_TRST_B"),
+		MTK_FUNCTION(6, "NCEB0"),
+		MTK_FUNCTION(7, "DBG_MON_A[16]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(49, "EINT17"),
+		"J22", "mt8127",
+		MTK_EINT_FUNCTION(0, 17),
+		MTK_FUNCTION(0, "GPIO49"),
+		MTK_FUNCTION(1, "UCTS0"),
+		MTK_FUNCTION(3, "CLKM0"),
+		MTK_FUNCTION(4, "IDDIG"),
+		MTK_FUNCTION(5, "ANT_SEL4"),
+		MTK_FUNCTION(6, "NREB"),
+		MTK_FUNCTION(7, "DBG_MON_A[17]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(50, "EINT18"),
+		"AD20", "mt8127",
+		MTK_EINT_FUNCTION(0, 18),
+		MTK_FUNCTION(0, "GPIO50"),
+		MTK_FUNCTION(1, "URTS0"),
+		MTK_FUNCTION(2, "CLKM3"),
+		MTK_FUNCTION(3, "I2SOUT_LRCK"),
+		MTK_FUNCTION(4, "DRV_VBUS"),
+		MTK_FUNCTION(5, "ANT_SEL3"),
+		MTK_FUNCTION(6, "ADC_CK"),
+		MTK_FUNCTION(7, "DBG_MON_B[12]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(51, "EINT19"),
+		"AC21", "mt8127",
+		MTK_EINT_FUNCTION(0, 19),
+		MTK_FUNCTION(0, "GPIO51"),
+		MTK_FUNCTION(1, "UCTS1"),
+		MTK_FUNCTION(3, "I2SOUT_BCK"),
+		MTK_FUNCTION(4, "CLKM1"),
+		MTK_FUNCTION(5, "ANT_SEL4"),
+		MTK_FUNCTION(6, "ADC_DAT_IN"),
+		MTK_FUNCTION(7, "DBG_MON_B[13]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(52, "EINT20"),
+		"V20", "mt8127",
+		MTK_EINT_FUNCTION(0, 20),
+		MTK_FUNCTION(0, "GPIO52"),
+		MTK_FUNCTION(1, "URTS1"),
+		MTK_FUNCTION(2, "PCM_TX"),
+		MTK_FUNCTION(3, "I2SOUT_DATA_OUT"),
+		MTK_FUNCTION(4, "CLKM2"),
+		MTK_FUNCTION(5, "ANT_SEL5"),
+		MTK_FUNCTION(6, "ADC_WS"),
+		MTK_FUNCTION(7, "DBG_MON_B[14]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(53, "SPI_CS"),
+		"AD19", "mt8127",
+		MTK_EINT_FUNCTION(0, 54),
+		MTK_FUNCTION(0, "GPIO53"),
+		MTK_FUNCTION(1, "SPI_CS"),
+		MTK_FUNCTION(3, "I2SIN1_DATA_IN"),
+		MTK_FUNCTION(4, "ADC_CK"),
+		MTK_FUNCTION(7, "DBG_MON_B[15]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(54, "SPI_CK"),
+		"AC18", "mt8127",
+		MTK_EINT_FUNCTION(0, 55),
+		MTK_FUNCTION(0, "GPIO54"),
+		MTK_FUNCTION(1, "SPI_CK"),
+		MTK_FUNCTION(3, "I2SIN1_LRCK"),
+		MTK_FUNCTION(4, "ADC_DAT_IN"),
+		MTK_FUNCTION(7, "DBG_MON_B[16]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(55, "SPI_MI"),
+		"AC19", "mt8127",
+		MTK_EINT_FUNCTION(0, 56),
+		MTK_FUNCTION(0, "GPIO55"),
+		MTK_FUNCTION(1, "SPI_MI"),
+		MTK_FUNCTION(2, "SPI_MO"),
+		MTK_FUNCTION(3, "I2SIN1_BCK1"),
+		MTK_FUNCTION(4, "ADC_WS"),
+		MTK_FUNCTION(7, "DBG_MON_B[17]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(56, "SPI_MO"),
+		"AD18", "mt8127",
+		MTK_EINT_FUNCTION(0, 57),
+		MTK_FUNCTION(0, "GPIO56"),
+		MTK_FUNCTION(1, "SPI_MO"),
+		MTK_FUNCTION(2, "SPI_MI"),
+		MTK_FUNCTION(7, "DBG_MON_B[18]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(57, "SDA1"),
+		"AE23", "mt8127",
+		MTK_EINT_FUNCTION(0, 58),
+		MTK_FUNCTION(0, "GPIO57"),
+		MTK_FUNCTION(1, "SDA1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(58, "SCL1"),
+		"AD23", "mt8127",
+		MTK_EINT_FUNCTION(0, 59),
+		MTK_FUNCTION(0, "GPIO58"),
+		MTK_FUNCTION(1, "SCL1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(59, "DISP_PWM"),
+		"AC20", "mt8127",
+		MTK_EINT_FUNCTION(0, 60),
+		MTK_FUNCTION(0, "GPIO59"),
+		MTK_FUNCTION(1, "DISP_PWM"),
+		MTK_FUNCTION(2, "PWM1"),
+		MTK_FUNCTION(7, "DBG_MON_A[18]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(60, "WB_RSTB"),
+		"AD7", "mt8127",
+		MTK_EINT_FUNCTION(0, 61),
+		MTK_FUNCTION(0, "GPIO60"),
+		MTK_FUNCTION(1, "WB_RSTB"),
+		MTK_FUNCTION(7, "DBG_MON_A[19]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(61, "F2W_DATA"),
+		"Y10", "mt8127",
+		MTK_EINT_FUNCTION(0, 62),
+		MTK_FUNCTION(0, "GPIO61"),
+		MTK_FUNCTION(1, "F2W_DATA"),
+		MTK_FUNCTION(7, "DBG_MON_A[20]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(62, "F2W_CLK"),
+		"W10", "mt8127",
+		MTK_EINT_FUNCTION(0, 63),
+		MTK_FUNCTION(0, "GPIO62"),
+		MTK_FUNCTION(1, "F2W_CK"),
+		MTK_FUNCTION(7, "DBG_MON_A[21]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(63, "WB_SCLK"),
+		"AB7", "mt8127",
+		MTK_EINT_FUNCTION(0, 64),
+		MTK_FUNCTION(0, "GPIO63"),
+		MTK_FUNCTION(1, "WB_SCLK"),
+		MTK_FUNCTION(7, "DBG_MON_A[22]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(64, "WB_SDATA"),
+		"AA7", "mt8127",
+		MTK_EINT_FUNCTION(0, 65),
+		MTK_FUNCTION(0, "GPIO64"),
+		MTK_FUNCTION(1, "WB_SDATA"),
+		MTK_FUNCTION(7, "DBG_MON_A[23]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(65, "WB_SEN"),
+		"Y7", "mt8127",
+		MTK_EINT_FUNCTION(0, 66),
+		MTK_FUNCTION(0, "GPIO65"),
+		MTK_FUNCTION(1, "WB_SEN"),
+		MTK_FUNCTION(7, "DBG_MON_A[24]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(66, "WB_CRTL0"),
+		"AA1", "mt8127",
+		MTK_EINT_FUNCTION(0, 67),
+		MTK_FUNCTION(0, "GPIO66"),
+		MTK_FUNCTION(1, "WB_CRTL0"),
+		MTK_FUNCTION(2, "DFD_NTRST_XI"),
+		MTK_FUNCTION(7, "DBG_MON_A[25]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(67, "WB_CRTL1"),
+		"AA2", "mt8127",
+		MTK_EINT_FUNCTION(0, 68),
+		MTK_FUNCTION(0, "GPIO67"),
+		MTK_FUNCTION(1, "WB_CRTL1"),
+		MTK_FUNCTION(2, "DFD_TMS_XI"),
+		MTK_FUNCTION(7, "DBG_MON_A[26]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(68, "WB_CRTL2"),
+		"Y1", "mt8127",
+		MTK_EINT_FUNCTION(0, 69),
+		MTK_FUNCTION(0, "GPIO68"),
+		MTK_FUNCTION(1, "WB_CRTL2"),
+		MTK_FUNCTION(2, "DFD_TCK_XI"),
+		MTK_FUNCTION(7, "DBG_MON_A[27]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(69, "WB_CRTL3"),
+		"Y2", "mt8127",
+		MTK_EINT_FUNCTION(0, 70),
+		MTK_FUNCTION(0, "GPIO69"),
+		MTK_FUNCTION(1, "WB_CRTL3"),
+		MTK_FUNCTION(2, "DFD_TDI_XI"),
+		MTK_FUNCTION(7, "DBG_MON_A[28]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(70, "WB_CRTL4"),
+		"Y3", "mt8127",
+		MTK_EINT_FUNCTION(0, 71),
+		MTK_FUNCTION(0, "GPIO70"),
+		MTK_FUNCTION(1, "WB_CRTL4"),
+		MTK_FUNCTION(2, "DFD_TDO"),
+		MTK_FUNCTION(7, "DBG_MON_A[29]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(71, "WB_CRTL5"),
+		"Y4", "mt8127",
+		MTK_EINT_FUNCTION(0, 72),
+		MTK_FUNCTION(0, "GPIO71"),
+		MTK_FUNCTION(1, "WB_CRTL5"),
+		MTK_FUNCTION(7, "DBG_MON_A[30]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(72, "I2S_DATA_IN"),
+		"K21", "mt8127",
+		MTK_EINT_FUNCTION(0, 73),
+		MTK_FUNCTION(0, "GPIO72"),
+		MTK_FUNCTION(1, "I2SIN1_DATA_IN"),
+		MTK_FUNCTION(2, "PCM_RX"),
+		MTK_FUNCTION(3, "I2SOUT_DATA_OUT"),
+		MTK_FUNCTION(4, "DAC_DAT_OUT"),
+		MTK_FUNCTION(5, "PWM0"),
+		MTK_FUNCTION(6, "ADC_CK"),
+		MTK_FUNCTION(7, "DBG_MON_B[19]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(73, "I2S_LRCK"),
+		"L21", "mt8127",
+		MTK_EINT_FUNCTION(0, 74),
+		MTK_FUNCTION(0, "GPIO73"),
+		MTK_FUNCTION(1, "I2SIN1_LRCK"),
+		MTK_FUNCTION(2, "PCM_SYNC"),
+		MTK_FUNCTION(3, "I2SOUT_LRCK"),
+		MTK_FUNCTION(4, "DAC_WS"),
+		MTK_FUNCTION(5, "PWM3"),
+		MTK_FUNCTION(6, "ADC_DAT_IN"),
+		MTK_FUNCTION(7, "DBG_MON_B[20]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(74, "I2S_BCK"),
+		"L20", "mt8127",
+		MTK_EINT_FUNCTION(0, 75),
+		MTK_FUNCTION(0, "GPIO74"),
+		MTK_FUNCTION(1, "I2SIN1_BCK2"),
+		MTK_FUNCTION(2, "PCM_CLK1"),
+		MTK_FUNCTION(3, "I2SOUT_BCK"),
+		MTK_FUNCTION(4, "DAC_CK"),
+		MTK_FUNCTION(5, "PWM4"),
+		MTK_FUNCTION(6, "ADC_WS"),
+		MTK_FUNCTION(7, "DBG_MON_B[21]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(75, "SDA0"),
+		"W3", "mt8127",
+		MTK_EINT_FUNCTION(0, 76),
+		MTK_FUNCTION(0, "GPIO75"),
+		MTK_FUNCTION(1, "SDA0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(76, "SCL0"),
+		"W4", "mt8127",
+		MTK_EINT_FUNCTION(0, 77),
+		MTK_FUNCTION(0, "GPIO76"),
+		MTK_FUNCTION(1, "SCL0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(77, "SDA2"),
+		"K19", "mt8127",
+		MTK_EINT_FUNCTION(0, 78),
+		MTK_FUNCTION(0, "GPIO77"),
+		MTK_FUNCTION(1, "SDA2"),
+		MTK_FUNCTION(2, "PWM1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(78, "SCL2"),
+		"K20", "mt8127",
+		MTK_EINT_FUNCTION(0, 79),
+		MTK_FUNCTION(0, "GPIO78"),
+		MTK_FUNCTION(1, "SCL2"),
+		MTK_FUNCTION(2, "PWM2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(79, "URXD0"),
+		"K18", "mt8127",
+		MTK_EINT_FUNCTION(0, 80),
+		MTK_FUNCTION(0, "GPIO79"),
+		MTK_FUNCTION(1, "URXD0"),
+		MTK_FUNCTION(2, "UTXD0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(80, "UTXD0"),
+		"K17", "mt8127",
+		MTK_EINT_FUNCTION(0, 81),
+		MTK_FUNCTION(0, "GPIO80"),
+		MTK_FUNCTION(1, "UTXD0"),
+		MTK_FUNCTION(2, "URXD0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(81, "URXD1"),
+		"L17", "mt8127",
+		MTK_EINT_FUNCTION(0, 82),
+		MTK_FUNCTION(0, "GPIO81"),
+		MTK_FUNCTION(1, "URXD1"),
+		MTK_FUNCTION(2, "UTXD1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(82, "UTXD1"),
+		"L18", "mt8127",
+		MTK_EINT_FUNCTION(0, 83),
+		MTK_FUNCTION(0, "GPIO82"),
+		MTK_FUNCTION(1, "UTXD1"),
+		MTK_FUNCTION(2, "URXD1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(83, "LCM_RST"),
+		"W5", "mt8127",
+		MTK_EINT_FUNCTION(0, 84),
+		MTK_FUNCTION(0, "GPIO83"),
+		MTK_FUNCTION(1, "LCM_RST"),
+		MTK_FUNCTION(2, "VDAC_CK_XI"),
+		MTK_FUNCTION(7, "DBG_MON_A[31]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(84, "DSI_TE"),
+		"W6", "mt8127",
+		MTK_EINT_FUNCTION(0, 85),
+		MTK_FUNCTION(0, "GPIO84"),
+		MTK_FUNCTION(1, "DSI_TE"),
+		MTK_FUNCTION(7, "DBG_MON_A[32]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(85, "MSDC2_CMD"),
+		"U7", "mt8127",
+		MTK_EINT_FUNCTION(0, 86),
+		MTK_FUNCTION(0, "GPIO85"),
+		MTK_FUNCTION(1, "MSDC2_CMD"),
+		MTK_FUNCTION(2, "ANT_SEL0"),
+		MTK_FUNCTION(3, "SDA1"),
+		MTK_FUNCTION(6, "I2SOUT_BCK"),
+		MTK_FUNCTION(7, "DBG_MON_B[22]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(86, "MSDC2_CLK"),
+		"T8", "mt8127",
+		MTK_EINT_FUNCTION(0, 87),
+		MTK_FUNCTION(0, "GPIO86"),
+		MTK_FUNCTION(1, "MSDC2_CLK"),
+		MTK_FUNCTION(2, "ANT_SEL1"),
+		MTK_FUNCTION(3, "SCL1"),
+		MTK_FUNCTION(6, "I2SOUT_LRCK"),
+		MTK_FUNCTION(7, "DBG_MON_B[23]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(87, "MSDC2_DAT0"),
+		"V3", "mt8127",
+		MTK_EINT_FUNCTION(0, 88),
+		MTK_FUNCTION(0, "GPIO87"),
+		MTK_FUNCTION(1, "MSDC2_DAT0"),
+		MTK_FUNCTION(2, "ANT_SEL2"),
+		MTK_FUNCTION(5, "UTXD0"),
+		MTK_FUNCTION(6, "I2SOUT_DATA_OUT"),
+		MTK_FUNCTION(7, "DBG_MON_B[24]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(88, "MSDC2_DAT1"),
+		"V4", "mt8127",
+		MTK_EINT_FUNCTION(0, 89),
+		MTK_FUNCTION(0, "GPIO88"),
+		MTK_FUNCTION(1, "MSDC2_DAT1"),
+		MTK_FUNCTION(2, "ANT_SEL3"),
+		MTK_FUNCTION(3, "PWM0"),
+		MTK_FUNCTION(5, "URXD0"),
+		MTK_FUNCTION(6, "PWM1"),
+		MTK_FUNCTION(7, "DBG_MON_B[25]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(89, "MSDC2_DAT2"),
+		"U5", "mt8127",
+		MTK_EINT_FUNCTION(0, 90),
+		MTK_FUNCTION(0, "GPIO89"),
+		MTK_FUNCTION(1, "MSDC2_DAT2"),
+		MTK_FUNCTION(2, "ANT_SEL4"),
+		MTK_FUNCTION(3, "SDA2"),
+		MTK_FUNCTION(5, "UTXD1"),
+		MTK_FUNCTION(6, "PWM2"),
+		MTK_FUNCTION(7, "DBG_MON_B[26]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(90, "MSDC2_DAT3"),
+		"U6", "mt8127",
+		MTK_EINT_FUNCTION(0, 91),
+		MTK_FUNCTION(0, "GPIO90"),
+		MTK_FUNCTION(1, "MSDC2_DAT3"),
+		MTK_FUNCTION(2, "ANT_SEL5"),
+		MTK_FUNCTION(3, "SCL2"),
+		MTK_FUNCTION(4, "EXT_FRAME_SYNC"),
+		MTK_FUNCTION(5, "URXD1"),
+		MTK_FUNCTION(6, "PWM3"),
+		MTK_FUNCTION(7, "DBG_MON_B[27]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(91, "TDN3"),
+		"U2", "mt8127",
+		MTK_EINT_FUNCTION(0, 92),
+		MTK_FUNCTION(0, "GPI91"),
+		MTK_FUNCTION(1, "TDN3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(92, "TDP3"),
+		"U1", "mt8127",
+		MTK_EINT_FUNCTION(0, 93),
+		MTK_FUNCTION(0, "GPI92"),
+		MTK_FUNCTION(1, "TDP3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(93, "TDN2"),
+		"T2", "mt8127",
+		MTK_EINT_FUNCTION(0, 94),
+		MTK_FUNCTION(0, "GPI93"),
+		MTK_FUNCTION(1, "TDN2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(94, "TDP2"),
+		"T1", "mt8127",
+		MTK_EINT_FUNCTION(0, 95),
+		MTK_FUNCTION(0, "GPI94"),
+		MTK_FUNCTION(1, "TDP2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(95, "TCN"),
+		"R5", "mt8127",
+		MTK_EINT_FUNCTION(0, 96),
+		MTK_FUNCTION(0, "GPI95"),
+		MTK_FUNCTION(1, "TCN")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(96, "TCP"),
+		"R4", "mt8127",
+		MTK_EINT_FUNCTION(0, 97),
+		MTK_FUNCTION(0, "GPI96"),
+		MTK_FUNCTION(1, "TCP")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(97, "TDN1"),
+		"R3", "mt8127",
+		MTK_EINT_FUNCTION(0, 98),
+		MTK_FUNCTION(0, "GPI97"),
+		MTK_FUNCTION(1, "TDN1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(98, "TDP1"),
+		"R2", "mt8127",
+		MTK_EINT_FUNCTION(0, 99),
+		MTK_FUNCTION(0, "GPI98"),
+		MTK_FUNCTION(1, "TDP1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(99, "TDN0"),
+		"P3", "mt8127",
+		MTK_EINT_FUNCTION(0, 100),
+		MTK_FUNCTION(0, "GPI99"),
+		MTK_FUNCTION(1, "TDN0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(100, "TDP0"),
+		"P2", "mt8127",
+		MTK_EINT_FUNCTION(0, 101),
+		MTK_FUNCTION(0, "GPI100"),
+		MTK_FUNCTION(1, "TDP0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(101, "RDN0"),
+		"K1", "mt8127",
+		MTK_EINT_FUNCTION(0, 102),
+		MTK_FUNCTION(0, "GPI101"),
+		MTK_FUNCTION(1, "RDN0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(102, "RDP0"),
+		"K2", "mt8127",
+		MTK_EINT_FUNCTION(0, 103),
+		MTK_FUNCTION(0, "GPI102"),
+		MTK_FUNCTION(1, "RDP0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(103, "RDN1"),
+		"L2", "mt8127",
+		MTK_EINT_FUNCTION(0, 104),
+		MTK_FUNCTION(0, "GPI103"),
+		MTK_FUNCTION(1, "RDN1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(104, "RDP1"),
+		"L3", "mt8127",
+		MTK_EINT_FUNCTION(0, 105),
+		MTK_FUNCTION(0, "GPI104"),
+		MTK_FUNCTION(1, "RDP1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(105, "RCN"),
+		"M4", "mt8127",
+		MTK_EINT_FUNCTION(0, 106),
+		MTK_FUNCTION(0, "GPI105"),
+		MTK_FUNCTION(1, "RCN")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(106, "RCP"),
+		"M5", "mt8127",
+		MTK_EINT_FUNCTION(0, 107),
+		MTK_FUNCTION(0, "GPI106"),
+		MTK_FUNCTION(1, "RCP")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(107, "RDN2"),
+		"M2", "mt8127",
+		MTK_EINT_FUNCTION(0, 108),
+		MTK_FUNCTION(0, "GPI107"),
+		MTK_FUNCTION(1, "RDN2"),
+		MTK_FUNCTION(2, "CMDAT8")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(108, "RDP2"),
+		"M3", "mt8127",
+		MTK_EINT_FUNCTION(0, 109),
+		MTK_FUNCTION(0, "GPI108"),
+		MTK_FUNCTION(1, "RDP2"),
+		MTK_FUNCTION(2, "CMDAT9")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(109, "RDN3"),
+		"N2", "mt8127",
+		MTK_EINT_FUNCTION(0, 110),
+		MTK_FUNCTION(0, "GPI109"),
+		MTK_FUNCTION(1, "RDN3"),
+		MTK_FUNCTION(2, "CMDAT4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(110, "RDP3"),
+		"N3", "mt8127",
+		MTK_EINT_FUNCTION(0, 111),
+		MTK_FUNCTION(0, "GPI110"),
+		MTK_FUNCTION(1, "RDP3"),
+		MTK_FUNCTION(2, "CMDAT5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(111, "RCN_A"),
+		"J5", "mt8127",
+		MTK_EINT_FUNCTION(0, 112),
+		MTK_FUNCTION(0, "GPI111"),
+		MTK_FUNCTION(1, "RCN_A"),
+		MTK_FUNCTION(2, "CMDAT6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(112, "RCP_A"),
+		"J4", "mt8127",
+		MTK_EINT_FUNCTION(0, 113),
+		MTK_FUNCTION(0, "GPI112"),
+		MTK_FUNCTION(1, "RCP_A"),
+		MTK_FUNCTION(2, "CMDAT7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(113, "RDN1_A"),
+		"J2", "mt8127",
+		MTK_EINT_FUNCTION(0, 114),
+		MTK_FUNCTION(0, "GPI113"),
+		MTK_FUNCTION(1, "RDN1_A"),
+		MTK_FUNCTION(2, "CMDAT2"),
+		MTK_FUNCTION(3, "CMCSD2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(114, "RDP1_A"),
+		"J3", "mt8127",
+		MTK_EINT_FUNCTION(0, 115),
+		MTK_FUNCTION(0, "GPI114"),
+		MTK_FUNCTION(1, "RDP1_A"),
+		MTK_FUNCTION(2, "CMDAT3"),
+		MTK_FUNCTION(3, "CMCSD3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(115, "RDN0_A"),
+		"H2", "mt8127",
+		MTK_EINT_FUNCTION(0, 116),
+		MTK_FUNCTION(0, "GPI115"),
+		MTK_FUNCTION(1, "RDN0_A"),
+		MTK_FUNCTION(2, "CMHSYNC")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(116, "RDP0_A"),
+		"H3", "mt8127",
+		MTK_EINT_FUNCTION(0, 117),
+		MTK_FUNCTION(0, "GPI116"),
+		MTK_FUNCTION(1, "RDP0_A"),
+		MTK_FUNCTION(2, "CMVSYNC")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(117, "CMDAT0"),
+		"G5", "mt8127",
+		MTK_EINT_FUNCTION(0, 118),
+		MTK_FUNCTION(0, "GPIO117"),
+		MTK_FUNCTION(1, "CMDAT0"),
+		MTK_FUNCTION(2, "CMCSD0"),
+		MTK_FUNCTION(3, "ANT_SEL2"),
+		MTK_FUNCTION(7, "DBG_MON_B[28]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(118, "CMDAT1"),
+		"G4", "mt8127",
+		MTK_EINT_FUNCTION(0, 119),
+		MTK_FUNCTION(0, "GPIO118"),
+		MTK_FUNCTION(1, "CMDAT1"),
+		MTK_FUNCTION(2, "CMCSD1"),
+		MTK_FUNCTION(3, "ANT_SEL3"),
+		MTK_FUNCTION(7, "DBG_MON_B[29]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(119, "CMMCLK"),
+		"F3", "mt8127",
+		MTK_EINT_FUNCTION(0, 120),
+		MTK_FUNCTION(0, "GPIO119"),
+		MTK_FUNCTION(1, "CMMCLK"),
+		MTK_FUNCTION(3, "ANT_SEL4"),
+		MTK_FUNCTION(7, "DBG_MON_B[30]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(120, "CMPCLK"),
+		"G6", "mt8127",
+		MTK_EINT_FUNCTION(0, 121),
+		MTK_FUNCTION(0, "GPIO120"),
+		MTK_FUNCTION(1, "CMPCLK"),
+		MTK_FUNCTION(2, "CMCSK"),
+		MTK_FUNCTION(3, "ANT_SEL5"),
+		MTK_FUNCTION(7, "DBG_MON_B[31]")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(121, "MSDC1_CMD"),
+		"E3", "mt8127",
+		MTK_EINT_FUNCTION(0, 122),
+		MTK_FUNCTION(0, "GPIO121"),
+		MTK_FUNCTION(1, "MSDC1_CMD")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(122, "MSDC1_CLK"),
+		"D1", "mt8127",
+		MTK_EINT_FUNCTION(0, 123),
+		MTK_FUNCTION(0, "GPIO122"),
+		MTK_FUNCTION(1, "MSDC1_CLK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(123, "MSDC1_DAT0"),
+		"D2", "mt8127",
+		MTK_EINT_FUNCTION(0, 124),
+		MTK_FUNCTION(0, "GPIO123"),
+		MTK_FUNCTION(1, "MSDC1_DAT0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(124, "MSDC1_DAT1"),
+		"D3", "mt8127",
+		MTK_EINT_FUNCTION(0, 125),
+		MTK_FUNCTION(0, "GPIO124"),
+		MTK_FUNCTION(1, "MSDC1_DAT1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(125, "MSDC1_DAT2"),
+		"F2", "mt8127",
+		MTK_EINT_FUNCTION(0, 126),
+		MTK_FUNCTION(0, "GPIO125"),
+		MTK_FUNCTION(1, "MSDC1_DAT2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(126, "MSDC1_DAT3"),
+		"E2", "mt8127",
+		MTK_EINT_FUNCTION(0, 127),
+		MTK_FUNCTION(0, "GPIO126"),
+		MTK_FUNCTION(1, "MSDC1_DAT3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(127, "MSDC0_DAT7"),
+		"C23", "mt8127",
+		MTK_EINT_FUNCTION(0, 128),
+		MTK_FUNCTION(0, "GPIO127"),
+		MTK_FUNCTION(1, "MSDC0_DAT7"),
+		MTK_FUNCTION(4, "NLD7")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(128, "MSDC0_DAT6"),
+		"C24", "mt8127",
+		MTK_EINT_FUNCTION(0, 129),
+		MTK_FUNCTION(0, "GPIO128"),
+		MTK_FUNCTION(1, "MSDC0_DAT6"),
+		MTK_FUNCTION(4, "NLD6")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(129, "MSDC0_DAT5"),
+		"D22", "mt8127",
+		MTK_EINT_FUNCTION(0, 130),
+		MTK_FUNCTION(0, "GPIO129"),
+		MTK_FUNCTION(1, "MSDC0_DAT5"),
+		MTK_FUNCTION(4, "NLD4")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(130, "MSDC0_DAT4"),
+		"D24", "mt8127",
+		MTK_EINT_FUNCTION(0, 131),
+		MTK_FUNCTION(0, "GPIO130"),
+		MTK_FUNCTION(1, "MSDC0_DAT4"),
+		MTK_FUNCTION(4, "NLD3")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(131, "MSDC0_RSTB"),
+		"F24", "mt8127",
+		MTK_EINT_FUNCTION(0, 132),
+		MTK_FUNCTION(0, "GPIO131"),
+		MTK_FUNCTION(1, "MSDC0_RSTB"),
+		MTK_FUNCTION(4, "NLD0")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(132, "MSDC0_CMD"),
+		"G20", "mt8127",
+		MTK_EINT_FUNCTION(0, 133),
+		MTK_FUNCTION(0, "GPIO132"),
+		MTK_FUNCTION(1, "MSDC0_CMD"),
+		MTK_FUNCTION(4, "NALE")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(133, "MSDC0_CLK"),
+		"G21", "mt8127",
+		MTK_EINT_FUNCTION(0, 134),
+		MTK_FUNCTION(0, "GPIO133"),
+		MTK_FUNCTION(1, "MSDC0_CLK"),
+		MTK_FUNCTION(4, "NWEB")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(134, "MSDC0_DAT3"),
+		"D23", "mt8127",
+		MTK_EINT_FUNCTION(0, 135),
+		MTK_FUNCTION(0, "GPIO134"),
+		MTK_FUNCTION(1, "MSDC0_DAT3"),
+		MTK_FUNCTION(4, "NLD1")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(135, "MSDC0_DAT2"),
+		"E22", "mt8127",
+		MTK_EINT_FUNCTION(0, 136),
+		MTK_FUNCTION(0, "GPIO135"),
+		MTK_FUNCTION(1, "MSDC0_DAT2"),
+		MTK_FUNCTION(4, "NLD5")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(136, "MSDC0_DAT1"),
+		"E23", "mt8127",
+		MTK_EINT_FUNCTION(0, 137),
+		MTK_FUNCTION(0, "GPIO136"),
+		MTK_FUNCTION(1, "MSDC0_DAT1"),
+		MTK_FUNCTION(4, "NLD8")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(137, "MSDC0_DAT0"),
+		"F22", "mt8127",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO137"),
+		MTK_FUNCTION(1, "MSDC0_DAT0"),
+		MTK_FUNCTION(4, "WATCHDOG"),
+		MTK_FUNCTION(5, "NLD2")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(138, "CEC"),
+		"AE21", "mt8127",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO138"),
+		MTK_FUNCTION(1, "CEC")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(139, "HTPLG"),
+		"AD21", "mt8127",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO139"),
+		MTK_FUNCTION(1, "HTPLG")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(140, "HDMISCK"),
+		"AE22", "mt8127",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO140"),
+		MTK_FUNCTION(1, "HDMISCK")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(141, "HDMISD"),
+		"AD22", "mt8127",
+		MTK_EINT_FUNCTION(NO_EINT_SUPPORT, NO_EINT_SUPPORT),
+		MTK_FUNCTION(0, "GPIO141"),
+		MTK_FUNCTION(1, "HDMISD")
+	),
+	MTK_PIN(
+		PINCTRL_PIN(142, "EINT21"),
+		"J23", "mt8127",
+		MTK_EINT_FUNCTION(0, 21),
+		MTK_FUNCTION(0, "GPIO142"),
+		MTK_FUNCTION(1, "NRNB"),
+		MTK_FUNCTION(2, "ANT_SEL0"),
+		MTK_FUNCTION(7, "DBG_MON_B[32]")
+	),
+};
+
+#endif /* __PINCTRL_MTK_MT8127_H */
diff --git a/drivers/pinctrl/meson/pinctrl-meson.c b/drivers/pinctrl/meson/pinctrl-meson.c
index a70a5fe79d44..84943e4cff09 100644
--- a/drivers/pinctrl/meson/pinctrl-meson.c
+++ b/drivers/pinctrl/meson/pinctrl-meson.c
@@ -738,9 +738,9 @@ static int meson_pinctrl_probe(struct platform_device *pdev)
 	pc->desc.npins		= pc->data->num_pins;
 
 	pc->pcdev = pinctrl_register(&pc->desc, pc->dev, pc);
-	if (!pc->pcdev) {
+	if (IS_ERR(pc->pcdev)) {
 		dev_err(pc->dev, "can't register pinctrl device");
-		return -EINVAL;
+		return PTR_ERR(pc->pcdev);
 	}
 
 	ret = meson_gpiolib_register(pc);
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-370.c b/drivers/pinctrl/mvebu/pinctrl-armada-370.c
index 03aa58c4cb85..73dc1bc5f32c 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-370.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-370.c
@@ -52,12 +52,12 @@ static struct mvebu_mpp_mode mv88f6710_mpp_modes[] = {
 	   MPP_FUNCTION(0x2, "uart0", "rxd")),
 	MPP_MODE(4,
 	   MPP_FUNCTION(0x0, "gpio", NULL),
-	   MPP_FUNCTION(0x1, "cpu_pd", "vdd")),
+	   MPP_FUNCTION(0x1, "vdd", "cpu-pd")),
 	MPP_MODE(5,
 	   MPP_FUNCTION(0x0, "gpo", NULL),
-	   MPP_FUNCTION(0x1, "ge0", "txclko"),
+	   MPP_FUNCTION(0x1, "ge0", "txclkout"),
 	   MPP_FUNCTION(0x2, "uart1", "txd"),
-	   MPP_FUNCTION(0x4, "spi1", "clk"),
+	   MPP_FUNCTION(0x4, "spi1", "sck"),
 	   MPP_FUNCTION(0x5, "audio", "mclk")),
 	MPP_MODE(6,
 	   MPP_FUNCTION(0x0, "gpio", NULL),
@@ -68,7 +68,7 @@ static struct mvebu_mpp_mode mv88f6710_mpp_modes[] = {
 	MPP_MODE(7,
 	   MPP_FUNCTION(0x0, "gpo", NULL),
 	   MPP_FUNCTION(0x1, "ge0", "txd1"),
-	   MPP_FUNCTION(0x4, "tdm", "tdx"),
+	   MPP_FUNCTION(0x4, "tdm", "dtx"),
 	   MPP_FUNCTION(0x5, "audio", "lrclk")),
 	MPP_MODE(8,
 	   MPP_FUNCTION(0x0, "gpio", NULL),
@@ -207,11 +207,11 @@ static struct mvebu_mpp_mode mv88f6710_mpp_modes[] = {
 	   MPP_FUNCTION(0x2, "spi0", "cs0")),
 	MPP_MODE(34,
 	   MPP_FUNCTION(0x0, "gpo", NULL),
-	   MPP_FUNCTION(0x1, "dev", "wen0"),
+	   MPP_FUNCTION(0x1, "dev", "we0"),
 	   MPP_FUNCTION(0x2, "spi0", "mosi")),
 	MPP_MODE(35,
 	   MPP_FUNCTION(0x0, "gpo", NULL),
-	   MPP_FUNCTION(0x1, "dev", "oen"),
+	   MPP_FUNCTION(0x1, "dev", "oe"),
 	   MPP_FUNCTION(0x2, "spi0", "sck")),
 	MPP_MODE(36,
 	   MPP_FUNCTION(0x0, "gpo", NULL),
@@ -348,13 +348,13 @@ static struct mvebu_mpp_mode mv88f6710_mpp_modes[] = {
 	   MPP_FUNCTION(0x1, "dev", "ale1"),
 	   MPP_FUNCTION(0x2, "uart1", "rxd"),
 	   MPP_FUNCTION(0x3, "sata0", "prsnt"),
-	   MPP_FUNCTION(0x4, "pcie", "rst-out"),
+	   MPP_FUNCTION(0x4, "pcie", "rstout"),
 	   MPP_FUNCTION(0x5, "audio", "sdi")),
 	MPP_MODE(61,
 	   MPP_FUNCTION(0x0, "gpo", NULL),
-	   MPP_FUNCTION(0x1, "dev", "wen1"),
+	   MPP_FUNCTION(0x1, "dev", "we1"),
 	   MPP_FUNCTION(0x2, "uart1", "txd"),
-	   MPP_FUNCTION(0x5, "audio", "rclk")),
+	   MPP_FUNCTION(0x5, "audio", "lrclk")),
 	MPP_MODE(62,
 	   MPP_FUNCTION(0x0, "gpio", NULL),
 	   MPP_FUNCTION(0x1, "dev", "a2"),
@@ -370,11 +370,11 @@ static struct mvebu_mpp_mode mv88f6710_mpp_modes[] = {
 	MPP_MODE(64,
 	   MPP_FUNCTION(0x0, "gpio", NULL),
 	   MPP_FUNCTION(0x1, "spi0", "miso"),
-	   MPP_FUNCTION(0x2, "spi0-1", "cs1")),
+	   MPP_FUNCTION(0x2, "spi0", "cs1")),
 	MPP_MODE(65,
 	   MPP_FUNCTION(0x0, "gpio", NULL),
 	   MPP_FUNCTION(0x1, "spi0", "mosi"),
-	   MPP_FUNCTION(0x2, "spi0-1", "cs2")),
+	   MPP_FUNCTION(0x2, "spi0", "cs2")),
 };
 
 static struct mvebu_pinctrl_soc_info armada_370_pinctrl_info;
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-375.c b/drivers/pinctrl/mvebu/pinctrl-armada-375.c
index ca1e7571fedb..54e9fbd0121f 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-375.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-375.c
@@ -51,7 +51,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 	MPP_MODE(2,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x1, "dev", "ad4"),
-		 MPP_FUNCTION(0x2, "ptp", "eventreq"),
+		 MPP_FUNCTION(0x2, "ptp", "evreq"),
 		 MPP_FUNCTION(0x3, "led", "c0"),
 		 MPP_FUNCTION(0x4, "audio", "sdi"),
 		 MPP_FUNCTION(0x5, "nand", "io4"),
@@ -59,7 +59,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 	MPP_MODE(3,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x1, "dev", "ad5"),
-		 MPP_FUNCTION(0x2, "ptp", "triggen"),
+		 MPP_FUNCTION(0x2, "ptp", "trig"),
 		 MPP_FUNCTION(0x3, "led", "p3"),
 		 MPP_FUNCTION(0x4, "audio", "mclk"),
 		 MPP_FUNCTION(0x5, "nand", "io5"),
@@ -81,7 +81,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x1, "dev", "ad0"),
 		 MPP_FUNCTION(0x3, "led", "p1"),
-		 MPP_FUNCTION(0x4, "audio", "rclk"),
+		 MPP_FUNCTION(0x4, "audio", "lrclk"),
 		 MPP_FUNCTION(0x5, "nand", "io0")),
 	MPP_MODE(7,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
@@ -92,19 +92,17 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x5, "nand", "io1")),
 	MPP_MODE(8,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x1, "dev ", "bootcs"),
+		 MPP_FUNCTION(0x1, "dev", "bootcs"),
 		 MPP_FUNCTION(0x2, "spi0", "cs0"),
 		 MPP_FUNCTION(0x3, "spi1", "cs0"),
 		 MPP_FUNCTION(0x5, "nand", "ce")),
 	MPP_MODE(9,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x1, "nf", "wen"),
 		 MPP_FUNCTION(0x2, "spi0", "sck"),
 		 MPP_FUNCTION(0x3, "spi1", "sck"),
 		 MPP_FUNCTION(0x5, "nand", "we")),
 	MPP_MODE(10,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x1, "nf", "ren"),
 		 MPP_FUNCTION(0x2, "dram", "vttctrl"),
 		 MPP_FUNCTION(0x3, "led", "c1"),
 		 MPP_FUNCTION(0x5, "nand", "re"),
@@ -122,9 +120,9 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x5, "nand", "ale")),
 	MPP_MODE(13,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x1, "dev", "readyn"),
-		 MPP_FUNCTION(0x2, "pcie0", "rstoutn"),
-		 MPP_FUNCTION(0x3, "pcie1", "rstoutn"),
+		 MPP_FUNCTION(0x1, "dev", "ready"),
+		 MPP_FUNCTION(0x2, "pcie0", "rstout"),
+		 MPP_FUNCTION(0x3, "pcie1", "rstout"),
 		 MPP_FUNCTION(0x5, "nand", "rb"),
 		 MPP_FUNCTION(0x6, "spi1", "mosi")),
 	MPP_MODE(14,
@@ -143,10 +141,10 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x2, "uart0", "rxd")),
 	MPP_MODE(18,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x2, "tdm", "intn")),
+		 MPP_FUNCTION(0x2, "tdm", "int")),
 	MPP_MODE(19,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x2, "tdm", "rstn")),
+		 MPP_FUNCTION(0x2, "tdm", "rst")),
 	MPP_MODE(20,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x2, "tdm", "pclk")),
@@ -203,13 +201,13 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x2, "ge1", "rxclk"),
 		 MPP_FUNCTION(0x3, "sd", "d3"),
 		 MPP_FUNCTION(0x5, "spi0", "sck"),
-		 MPP_FUNCTION(0x6, "pcie0", "rstoutn")),
+		 MPP_FUNCTION(0x6, "pcie0", "rstout")),
 	MPP_MODE(30,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x2, "ge1", "txd0"),
 		 MPP_FUNCTION(0x3, "spi1", "cs0"),
 		 MPP_FUNCTION(0x5, "led", "p3"),
-		 MPP_FUNCTION(0x6, "ptp", "eventreq")),
+		 MPP_FUNCTION(0x6, "ptp", "evreq")),
 	MPP_MODE(31,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x2, "ge1", "txd1"),
@@ -219,7 +217,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x2, "ge1", "txd2"),
 		 MPP_FUNCTION(0x3, "spi1", "sck"),
-		 MPP_FUNCTION(0x4, "ptp", "triggen"),
+		 MPP_FUNCTION(0x4, "ptp", "trig"),
 		 MPP_FUNCTION(0x5, "led", "c0")),
 	MPP_MODE(33,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
@@ -244,7 +242,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 	MPP_MODE(37,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x1, "pcie0", "clkreq"),
-		 MPP_FUNCTION(0x2, "tdm", "intn"),
+		 MPP_FUNCTION(0x2, "tdm", "int"),
 		 MPP_FUNCTION(0x4, "ge", "mdc")),
 	MPP_MODE(38,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
@@ -278,7 +276,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 	MPP_MODE(45,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x2, "spi0", "cs2"),
-		 MPP_FUNCTION(0x4, "pcie0", "rstoutn"),
+		 MPP_FUNCTION(0x4, "pcie0", "rstout"),
 		 MPP_FUNCTION(0x5, "led", "c2"),
 		 MPP_FUNCTION(0x6, "spi1", "cs2")),
 	MPP_MODE(46,
@@ -286,13 +284,13 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x1, "led", "p0"),
 		 MPP_FUNCTION(0x2, "ge0", "txd0"),
 		 MPP_FUNCTION(0x3, "ge1", "txd0"),
-		 MPP_FUNCTION(0x6, "dev", "wen1")),
+		 MPP_FUNCTION(0x6, "dev", "we1")),
 	MPP_MODE(47,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x1, "led", "p1"),
 		 MPP_FUNCTION(0x2, "ge0", "txd1"),
 		 MPP_FUNCTION(0x3, "ge1", "txd1"),
-		 MPP_FUNCTION(0x5, "ptp", "triggen"),
+		 MPP_FUNCTION(0x5, "ptp", "trig"),
 		 MPP_FUNCTION(0x6, "dev", "ale0")),
 	MPP_MODE(48,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
@@ -311,7 +309,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x1, "led", "c0"),
 		 MPP_FUNCTION(0x2, "ge0", "rxd0"),
 		 MPP_FUNCTION(0x3, "ge1", "rxd0"),
-		 MPP_FUNCTION(0x5, "ptp", "eventreq"),
+		 MPP_FUNCTION(0x5, "ptp", "evreq"),
 		 MPP_FUNCTION(0x6, "dev", "ad12")),
 	MPP_MODE(51,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
@@ -328,14 +326,14 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x6, "dev", "ad9")),
 	MPP_MODE(53,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x1, "pcie1", "rstoutn"),
+		 MPP_FUNCTION(0x1, "pcie1", "rstout"),
 		 MPP_FUNCTION(0x2, "ge0", "rxd3"),
 		 MPP_FUNCTION(0x3, "ge1", "rxd3"),
 		 MPP_FUNCTION(0x5, "i2c0", "sck"),
 		 MPP_FUNCTION(0x6, "dev", "ad10")),
 	MPP_MODE(54,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x1, "pcie0", "rstoutn"),
+		 MPP_FUNCTION(0x1, "pcie0", "rstout"),
 		 MPP_FUNCTION(0x2, "ge0", "rxctl"),
 		 MPP_FUNCTION(0x3, "ge1", "rxctl"),
 		 MPP_FUNCTION(0x6, "dev", "ad11")),
@@ -353,7 +351,7 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x2, "ge0", "txctl"),
 		 MPP_FUNCTION(0x3, "ge1", "txctl"),
-		 MPP_FUNCTION(0x6, "dev", "wen0")),
+		 MPP_FUNCTION(0x6, "dev", "we0")),
 	MPP_MODE(58,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x4, "led", "c0")),
@@ -379,9 +377,9 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x6, "dev", "ad15")),
 	MPP_MODE(63,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x2, "ptp", "triggen"),
+		 MPP_FUNCTION(0x2, "ptp", "trig"),
 		 MPP_FUNCTION(0x4, "led", "p2"),
-		 MPP_FUNCTION(0x6, "dev", "burst")),
+		 MPP_FUNCTION(0x6, "dev", "burst/last")),
 	MPP_MODE(64,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
 		 MPP_FUNCTION(0x2, "dram", "vttctrl"),
@@ -391,9 +389,9 @@ static struct mvebu_mpp_mode mv88f6720_mpp_modes[] = {
 		 MPP_FUNCTION(0x1, "sata1", "prsnt")),
 	MPP_MODE(66,
 		 MPP_FUNCTION(0x0, "gpio", NULL),
-		 MPP_FUNCTION(0x2, "ptp", "eventreq"),
+		 MPP_FUNCTION(0x2, "ptp", "evreq"),
 		 MPP_FUNCTION(0x4, "spi1", "cs3"),
-		 MPP_FUNCTION(0x5, "pcie0", "rstoutn"),
+		 MPP_FUNCTION(0x5, "pcie0", "rstout"),
 		 MPP_FUNCTION(0x6, "dev", "cs3")),
 };
 
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-38x.c b/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
index 83bbcc72be1f..6ec82c62dff7 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-38x.c
@@ -94,56 +94,58 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "rxd0",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "pcie0", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "pcie1", "rstout",     V_88F6820_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi0",  "cs1",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",   "ad14",       V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "dev",   "ad14",       V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "pcie3", "clkreq",     V_88F6810_PLUS)),
 	MPP_MODE(13,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "rxd1",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "pcie0", "clkreq",     V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "pcie1", "clkreq",     V_88F6820_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi0",  "cs2",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",   "ad15",       V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "dev",   "ad15",       V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "pcie2", "clkreq",     V_88F6810_PLUS)),
 	MPP_MODE(14,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "rxd2",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ptp",   "clk",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "m",     "vtt_ctrl",   V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "dram",  "vttctrl",    V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi0",  "cs3",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",   "wen1",       V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "dev",   "we1",        V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "pcie3", "clkreq",     V_88F6810_PLUS)),
 	MPP_MODE(15,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "rxd3",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ge",    "mdc slave",  V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "pcie0", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(4, "spi0",  "mosi",       V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "pcie1", "rstout",     V_88F6820_PLUS)),
+		 MPP_VAR_FUNCTION(4, "spi0",  "mosi",       V_88F6810_PLUS)),
 	MPP_MODE(16,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "rxctl",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ge",    "mdio slave", V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "m",     "decc_err",   V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "dram",  "deccerr",    V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi0",  "miso",       V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "pcie0", "clkreq",     V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "pcie0", "clkreq",     V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "pcie1", "clkreq",     V_88F6820_PLUS)),
 	MPP_MODE(17,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "rxclk",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ptp",   "clk",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua1",   "rxd",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi0",  "sck",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "sata1", "prsnt",      V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sata1", "prsnt",      V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "sata0", "prsnt",      V_88F6810_PLUS)),
 	MPP_MODE(18,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "rxerr",      V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(2, "ptp",   "trig_gen",   V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(2, "ptp",   "trig",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua1",   "txd",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(4, "spi0",  "cs0",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "pcie1", "rstout",     V_88F6820_PLUS)),
+		 MPP_VAR_FUNCTION(4, "spi0",  "cs0",        V_88F6810_PLUS)),
 	MPP_MODE(19,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "col",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(2, "ptp",   "event_req",  V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "pcie0", "clkreq",     V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(2, "ptp",   "evreq",      V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "ge0",   "txerr",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "sata1", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(5, "ua0",   "cts",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",   "rxd",        V_88F6810_PLUS)),
@@ -151,7 +153,6 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ge0",   "txclk",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ptp",   "clk",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "pcie1", "rstout",     V_88F6820_PLUS),
 		 MPP_VAR_FUNCTION(4, "sata0", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(5, "ua0",   "rts",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",   "txd",        V_88F6810_PLUS)),
@@ -161,7 +162,8 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(2, "ge1",   "rxd0",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "sata0", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "sd0",   "cmd",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",   "bootcs",     V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "dev",   "bootcs",     V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "sata1", "prsnt",      V_88F6810_PLUS)),
 	MPP_MODE(22,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "spi0",  "mosi",       V_88F6810_PLUS),
@@ -209,7 +211,7 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 	MPP_MODE(30,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ge1",   "txd2",       V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",   "oen",        V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "dev",   "oe",         V_88F6810_PLUS)),
 	MPP_MODE(31,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ge1",   "txd3",       V_88F6810_PLUS),
@@ -217,10 +219,10 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 	MPP_MODE(32,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ge1",   "txctl",      V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",   "wen0",       V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "dev",   "we0",        V_88F6810_PLUS)),
 	MPP_MODE(33,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(1, "m",     "decc_err",   V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(1, "dram",  "deccerr",    V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",   "ad3",        V_88F6810_PLUS)),
 	MPP_MODE(34,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
@@ -231,7 +233,7 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(5, "dev",   "a1",         V_88F6810_PLUS)),
 	MPP_MODE(36,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(1, "ptp",   "trig_gen",   V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(1, "ptp",   "trig",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",   "a0",         V_88F6810_PLUS)),
 	MPP_MODE(37,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
@@ -241,7 +243,7 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(5, "dev",   "ad8",        V_88F6810_PLUS)),
 	MPP_MODE(38,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(1, "ptp",   "event_req",  V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(1, "ptp",   "evreq",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ge1",   "rxd1",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "ref",   "clk_out0",   V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "sd0",   "d0",         V_88F6810_PLUS),
@@ -266,7 +268,8 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(2, "ge1",   "rxctl",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua0",   "cts",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",  "cs3",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",   "burst/last", V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "dev",   "burst/last", V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "nand",  "rb0",        V_88F6810_PLUS)),
 	MPP_MODE(42,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ua1",   "txd",        V_88F6810_PLUS),
@@ -275,84 +278,82 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 	MPP_MODE(43,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie0", "clkreq",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(2, "m",     "vtt_ctrl",   V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "m",     "decc_err",   V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(4, "pcie0", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",   "clkout",     V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(2, "dram",  "vttctrl",    V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "dram",  "deccerr",    V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",  "cs2",        V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",   "clkout",     V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "nand",  "rb1",        V_88F6810_PLUS)),
 	MPP_MODE(44,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "sata0", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "sata1", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "sata2", "prsnt",      V_88F6828),
-		 MPP_VAR_FUNCTION(4, "sata3", "prsnt",      V_88F6828),
-		 MPP_VAR_FUNCTION(5, "pcie0", "rstout",     V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(4, "sata3", "prsnt",      V_88F6828)),
 	MPP_MODE(45,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ref",   "clk_out0",   V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "pcie0", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "pcie1", "rstout",     V_88F6820_PLUS),
-		 MPP_VAR_FUNCTION(4, "pcie2", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "pcie3", "rstout",     V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(6, "ua1",   "rxd",        V_88F6810_PLUS)),
 	MPP_MODE(46,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ref",   "clk_out1",   V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "pcie0", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "pcie1", "rstout",     V_88F6820_PLUS),
-		 MPP_VAR_FUNCTION(4, "pcie2", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "pcie3", "rstout",     V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(6, "ua1",   "txd",        V_88F6810_PLUS)),
 	MPP_MODE(47,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "sata0", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "sata1", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "sata2", "prsnt",      V_88F6828),
-		 MPP_VAR_FUNCTION(4, "spi1",  "cs2",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(5, "sata3", "prsnt",      V_88F6828)),
 	MPP_MODE(48,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "sata0", "prsnt",      V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(2, "m",     "vtt_ctrl",   V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "tdm2c", "pclk",       V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(2, "dram",  "vttctrl",    V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",   "pclk",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "audio", "mclk",       V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd0",   "d4",         V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sd0",   "d4",         V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "pcie0", "clkreq",     V_88F6810_PLUS)),
 	MPP_MODE(49,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "sata2", "prsnt",      V_88F6828),
 		 MPP_VAR_FUNCTION(2, "sata3", "prsnt",      V_88F6828),
-		 MPP_VAR_FUNCTION(3, "tdm2c", "fsync",      V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",   "fsync",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "audio", "lrclk",      V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd0",   "d5",         V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sd0",   "d5",         V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "pcie1", "clkreq",     V_88F6820_PLUS)),
 	MPP_MODE(50,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie0", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(2, "pcie1", "rstout",     V_88F6820_PLUS),
-		 MPP_VAR_FUNCTION(3, "tdm2c", "drx",        V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",   "drx",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "audio", "extclk",     V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(5, "sd0",   "cmd",        V_88F6810_PLUS)),
 	MPP_MODE(51,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "tdm2c", "dtx",        V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",   "dtx",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "audio", "sdo",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "m",     "decc_err",   V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "dram",  "deccerr",    V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "ptp",   "trig",       V_88F6810_PLUS)),
 	MPP_MODE(52,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie0", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(2, "pcie1", "rstout",     V_88F6820_PLUS),
-		 MPP_VAR_FUNCTION(3, "tdm2c", "intn",       V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",   "int",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "audio", "sdi",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd0",   "d6",         V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sd0",   "d6",         V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "ptp",   "clk",        V_88F6810_PLUS)),
 	MPP_MODE(53,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "sata1", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "sata0", "prsnt",      V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "tdm2c", "rstn",       V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",   "rst",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "audio", "bclk",       V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd0",   "d7",         V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sd0",   "d7",         V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "ptp",   "evreq",      V_88F6810_PLUS)),
 	MPP_MODE(54,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "sata0", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "sata1", "prsnt",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "pcie0", "rstout",     V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(4, "pcie1", "rstout",     V_88F6820_PLUS),
+		 MPP_VAR_FUNCTION(4, "ge0",   "txerr",      V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(5, "sd0",   "d3",         V_88F6810_PLUS)),
 	MPP_MODE(55,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
@@ -360,29 +361,32 @@ static struct mvebu_mpp_mode armada_38x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(2, "ge",    "mdio",       V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "pcie1", "clkreq",     V_88F6820_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",  "cs1",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd0",   "d0",         V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sd0",   "d0",         V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",   "rxd",        V_88F6810_PLUS)),
 	MPP_MODE(56,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "ua1",   "rts",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "ge",    "mdc",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "m",     "decc_err",   V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(4, "spi1",  "mosi",       V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(3, "dram",  "deccerr",    V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(4, "spi1",  "mosi",       V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",   "txd",        V_88F6810_PLUS)),
 	MPP_MODE(57,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",  "sck",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd0",   "clk",        V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sd0",   "clk",        V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",   "txd",        V_88F6810_PLUS)),
 	MPP_MODE(58,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie1", "clkreq",     V_88F6820_PLUS),
 		 MPP_VAR_FUNCTION(2, "i2c1",  "sck",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(3, "pcie2", "clkreq",     V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",  "miso",       V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd0",   "d1",         V_88F6810_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sd0",   "d1",         V_88F6810_PLUS),
+		 MPP_VAR_FUNCTION(6, "ua1",   "rxd",        V_88F6810_PLUS)),
 	MPP_MODE(59,
 		 MPP_VAR_FUNCTION(0, "gpio",  NULL,         V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie0", "rstout",     V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(2, "i2c1",  "sda",        V_88F6810_PLUS),
-		 MPP_VAR_FUNCTION(3, "pcie1", "rstout",     V_88F6820_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",  "cs0",        V_88F6810_PLUS),
 		 MPP_VAR_FUNCTION(5, "sd0",   "d2",         V_88F6810_PLUS)),
 };
@@ -411,7 +415,7 @@ static struct mvebu_mpp_ctrl armada_38x_mpp_controls[] = {
 
 static struct pinctrl_gpio_range armada_38x_mpp_gpio_ranges[] = {
 	MPP_GPIO_RANGE(0,   0,  0, 32),
-	MPP_GPIO_RANGE(1,  32, 32, 27),
+	MPP_GPIO_RANGE(1,  32, 32, 28),
 };
 
 static int armada_38x_pinctrl_probe(struct platform_device *pdev)
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-39x.c b/drivers/pinctrl/mvebu/pinctrl-armada-39x.c
index 42491624d660..fcfe9b478a2e 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-39x.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-39x.c
@@ -36,8 +36,10 @@ static int armada_39x_mpp_ctrl_set(unsigned pid, unsigned long config)
 
 enum {
 	V_88F6920 = BIT(0),
-	V_88F6928 = BIT(1),
-	V_88F6920_PLUS = (V_88F6920 | V_88F6928),
+	V_88F6925 = BIT(1),
+	V_88F6928 = BIT(2),
+	V_88F6920_PLUS = (V_88F6920 | V_88F6925 | V_88F6928),
+	V_88F6925_PLUS = (V_88F6925 | V_88F6928),
 };
 
 static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
@@ -82,7 +84,7 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 	MPP_MODE(10,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad12",     V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(7, "ptp",     "event",    V_88F6920_PLUS)),
+		 MPP_VAR_FUNCTION(7, "ptp",     "evreq",    V_88F6920_PLUS)),
 	MPP_MODE(11,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad13",     V_88F6920_PLUS),
@@ -95,11 +97,12 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 	MPP_MODE(13,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad15",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "pcie2",   "clkreq",   V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "led",     "data",     V_88F6920_PLUS)),
 	MPP_MODE(14,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(3, "m",       "vtt",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",     "wen1",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "dram",    "vttctrl",  V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "we1",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "ua1",     "txd",      V_88F6920_PLUS)),
 	MPP_MODE(15,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
@@ -108,13 +111,16 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(7, "i2c1",    "sck",      V_88F6920_PLUS)),
 	MPP_MODE(16,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(3, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "dram",    "deccerr",  V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi0",    "miso",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "pcie0",   "clkreq",   V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "i2c1",    "sda",      V_88F6920_PLUS)),
 	MPP_MODE(17,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua1",     "rxd",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi0",    "sck",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sata1",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(6, "sata0",   "prsnt",    V_88F6925_PLUS),
 		 MPP_VAR_FUNCTION(7, "smi",     "mdio",     V_88F6920_PLUS)),
 	MPP_MODE(18,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
@@ -123,22 +129,23 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(7, "i2c2",    "sck",      V_88F6920_PLUS)),
 	MPP_MODE(19,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(4, "sata1",   "prsnt",    V_88F6925_PLUS),
 		 MPP_VAR_FUNCTION(5, "ua0",     "cts",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",     "rxd",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "i2c2",    "sda",      V_88F6920_PLUS)),
 	MPP_MODE(20,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sata0",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(4, "sata0",   "prsnt",    V_88F6925_PLUS),
 		 MPP_VAR_FUNCTION(5, "ua0",     "rts",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",     "txd",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "smi",     "mdc",      V_88F6920_PLUS)),
 	MPP_MODE(21,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "spi0",    "cs1",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(3, "sata0",   "present",  V_88F6928),
-		 MPP_VAR_FUNCTION(4, "sd",      "cmd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "sata0",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "cmd",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "bootcs",   V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "sata1",   "prsnt",    V_88F6925_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "rxd0",     V_88F6920_PLUS)),
 	MPP_MODE(22,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
@@ -153,31 +160,31 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(1, "spi0",    "miso",     V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(2, "ua0",     "cts",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua1",     "rxd",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "d4",       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",     "readyn",   V_88F6920_PLUS)),
+		 MPP_VAR_FUNCTION(4, "sd0",     "d4",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "ready",    V_88F6920_PLUS)),
 	MPP_MODE(25,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "spi0",    "cs0",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(2, "ua0",     "rts",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua1",     "txd",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "d5",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "d5",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "cs0",      V_88F6920_PLUS)),
 	MPP_MODE(26,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "spi0",    "cs2",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "i2c1",    "sck",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "d6",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "d6",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "cs1",      V_88F6920_PLUS)),
 	MPP_MODE(27,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "spi0",    "cs3",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "i2c1",    "sda",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "d7",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "d7",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "cs2",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "txclkout", V_88F6920_PLUS)),
 	MPP_MODE(28,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "clk",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad5",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "txd0",     V_88F6920_PLUS)),
 	MPP_MODE(29,
@@ -186,7 +193,7 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(8, "ge",      "txd1",     V_88F6920_PLUS)),
 	MPP_MODE(30,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",     "oen",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "oe",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "txd2",     V_88F6920_PLUS)),
 	MPP_MODE(31,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
@@ -194,45 +201,45 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(8, "ge",      "txd3",     V_88F6920_PLUS)),
 	MPP_MODE(32,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",     "wen0",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "we0",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "txctl",    V_88F6920_PLUS)),
 	MPP_MODE(33,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "dram",    "deccerr",  V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad3",      V_88F6920_PLUS)),
 	MPP_MODE(34,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad1",      V_88F6920_PLUS)),
 	MPP_MODE(35,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "ref",     "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ref",     "clk_out1", V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "a1",       V_88F6920_PLUS)),
 	MPP_MODE(36,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "a0",       V_88F6920_PLUS)),
 	MPP_MODE(37,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "d3",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "d3",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad8",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "rxclk",    V_88F6920_PLUS)),
 	MPP_MODE(38,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(3, "ref",     "clk",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "d0",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "ref",     "clk_out0", V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "d0",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad4",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "rxd1",     V_88F6920_PLUS)),
 	MPP_MODE(39,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "i2c1",    "sck",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua0",     "cts",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "d1",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "d1",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "a2",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "rxd2",     V_88F6920_PLUS)),
 	MPP_MODE(40,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "i2c1",    "sda",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua0",     "rts",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(4, "sd",      "d2",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(4, "sd0",     "d2",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "ad6",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "rxd3",     V_88F6920_PLUS)),
 	MPP_MODE(41,
@@ -240,8 +247,8 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(1, "ua1",     "rxd",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "ua0",     "cts",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",    "cs3",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "dev",     "burstn",   V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(6, "nd",      "rbn0",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dev",     "burst/last", V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(6, "nand",    "rb0",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(8, "ge",      "rxctl",    V_88F6920_PLUS)),
 	MPP_MODE(42,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
@@ -251,113 +258,119 @@ static struct mvebu_mpp_mode armada_39x_mpp_modes[] = {
 	MPP_MODE(43,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie0",   "clkreq",   V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(2, "m",       "vtt",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(3, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(2, "dram",    "vttctrl",  V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "dram",    "deccerr",  V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",    "cs2",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(5, "dev",     "clkout",   V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(6, "nd",      "rbn1",     V_88F6920_PLUS)),
+		 MPP_VAR_FUNCTION(6, "nand",    "rb1",      V_88F6920_PLUS)),
 	MPP_MODE(44,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "sata0",   "present",  V_88F6928),
-		 MPP_VAR_FUNCTION(2, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(1, "sata0",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(2, "sata1",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(3, "sata2",   "prsnt",    V_88F6928),
+		 MPP_VAR_FUNCTION(4, "sata3",   "prsnt",    V_88F6928),
 		 MPP_VAR_FUNCTION(7, "led",     "clk",      V_88F6920_PLUS)),
 	MPP_MODE(45,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "ref",     "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ref",     "clk_out0", V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(2, "pcie0",   "rstout",   V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",     "rxd",      V_88F6920_PLUS)),
 	MPP_MODE(46,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "ref",     "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "ref",     "clk_out1", V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(2, "pcie0",   "rstout",   V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",     "txd",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "led",     "stb",      V_88F6920_PLUS)),
 	MPP_MODE(47,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "sata0",   "present",  V_88F6928),
-		 MPP_VAR_FUNCTION(2, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(1, "sata0",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(2, "sata1",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(3, "sata2",   "prsnt",    V_88F6928),
+		 MPP_VAR_FUNCTION(5, "sata3",   "prsnt",    V_88F6928),
 		 MPP_VAR_FUNCTION(7, "led",     "data",     V_88F6920_PLUS)),
 	MPP_MODE(48,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "sata0",   "present",  V_88F6928),
-		 MPP_VAR_FUNCTION(2, "m",       "vtt",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "sata0",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(2, "dram",    "vttctrl",  V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "tdm",     "pclk",     V_88F6928),
 		 MPP_VAR_FUNCTION(4, "audio",   "mclk",     V_88F6928),
-		 MPP_VAR_FUNCTION(5, "sd",      "d4",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "d4",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "pcie0",   "clkreq",   V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "ua1",     "txd",      V_88F6920_PLUS)),
 	MPP_MODE(49,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(1, "sata2",   "prsnt",    V_88F6928),
+		 MPP_VAR_FUNCTION(2, "sata3",   "prsnt",    V_88F6928),
 		 MPP_VAR_FUNCTION(3, "tdm",     "fsync",    V_88F6928),
 		 MPP_VAR_FUNCTION(4, "audio",   "lrclk",    V_88F6928),
-		 MPP_VAR_FUNCTION(5, "sd",      "d5",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "d5",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "ua2",     "rxd",      V_88F6920_PLUS)),
 	MPP_MODE(50,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie0",   "rstout",   V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "tdm",     "drx",      V_88F6928),
 		 MPP_VAR_FUNCTION(4, "audio",   "extclk",   V_88F6928),
-		 MPP_VAR_FUNCTION(5, "sd",      "cmd",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "cmd",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "ua2",     "rxd",      V_88F6920_PLUS)),
 	MPP_MODE(51,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "tdm",     "dtx",      V_88F6928),
 		 MPP_VAR_FUNCTION(4, "audio",   "sdo",      V_88F6928),
-		 MPP_VAR_FUNCTION(5, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "dram",    "deccerr",  V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "ua2",     "txd",      V_88F6920_PLUS)),
 	MPP_MODE(52,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie0",   "rstout",   V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(3, "tdm",     "intn",     V_88F6928),
+		 MPP_VAR_FUNCTION(3, "tdm",     "int",      V_88F6928),
 		 MPP_VAR_FUNCTION(4, "audio",   "sdi",      V_88F6928),
-		 MPP_VAR_FUNCTION(5, "sd",      "d6",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "d6",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "i2c3",    "sck",      V_88F6920_PLUS)),
 	MPP_MODE(53,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "sata1",   "present",  V_88F6928),
-		 MPP_VAR_FUNCTION(2, "sata0",   "present",  V_88F6928),
-		 MPP_VAR_FUNCTION(3, "tdm",     "rstn",     V_88F6928),
+		 MPP_VAR_FUNCTION(1, "sata1",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(2, "sata0",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(3, "tdm",     "rst",      V_88F6928),
 		 MPP_VAR_FUNCTION(4, "audio",   "bclk",     V_88F6928),
-		 MPP_VAR_FUNCTION(5, "sd",      "d7",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "d7",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "i2c3",    "sda",      V_88F6920_PLUS)),
 	MPP_MODE(54,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(1, "sata0",   "present",  V_88F6928),
-		 MPP_VAR_FUNCTION(2, "sata1",   "present",  V_88F6928),
+		 MPP_VAR_FUNCTION(1, "sata0",   "prsnt",    V_88F6925_PLUS),
+		 MPP_VAR_FUNCTION(2, "sata1",   "prsnt",    V_88F6925_PLUS),
 		 MPP_VAR_FUNCTION(3, "pcie0",   "rstout",   V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd",      "d3",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "d3",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "ua3",     "txd",      V_88F6920_PLUS)),
 	MPP_MODE(55,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "ua1",     "cts",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",    "cs1",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd",      "d0",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "d0",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",     "rxd",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(7, "ua3",     "rxd",      V_88F6920_PLUS)),
 	MPP_MODE(56,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "ua1",     "rts",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(3, "m",       "decc",     V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(3, "dram",    "deccerr",  V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",    "mosi",     V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",     "txd",      V_88F6920_PLUS)),
 	MPP_MODE(57,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",    "sck",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd",      "clk",      V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "clk",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",     "txd",      V_88F6920_PLUS)),
 	MPP_MODE(58,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(2, "i2c1",    "sck",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(3, "pcie2",   "clkreq",   V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",    "miso",     V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd",      "d1",       V_88F6920_PLUS),
+		 MPP_VAR_FUNCTION(5, "sd0",     "d1",       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(6, "ua1",     "rxd",      V_88F6920_PLUS)),
 	MPP_MODE(59,
 		 MPP_VAR_FUNCTION(0, "gpio",    NULL,       V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(1, "pcie0",   "rstout",   V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(2, "i2c1",    "sda",      V_88F6920_PLUS),
 		 MPP_VAR_FUNCTION(4, "spi1",    "cs0",      V_88F6920_PLUS),
-		 MPP_VAR_FUNCTION(5, "sd",      "d2",       V_88F6920_PLUS)),
+		 MPP_VAR_FUNCTION(5, "sd0",     "d2",       V_88F6920_PLUS)),
 };
 
 static struct mvebu_pinctrl_soc_info armada_39x_pinctrl_info;
@@ -368,6 +381,10 @@ static const struct of_device_id armada_39x_pinctrl_of_match[] = {
 		.data       = (void *) V_88F6920,
 	},
 	{
+		.compatible = "marvell,mv88f6925-pinctrl",
+		.data       = (void *) V_88F6925,
+	},
+	{
 		.compatible = "marvell,mv88f6928-pinctrl",
 		.data       = (void *) V_88F6928,
 	},
@@ -380,7 +397,7 @@ static struct mvebu_mpp_ctrl armada_39x_mpp_controls[] = {
 
 static struct pinctrl_gpio_range armada_39x_mpp_gpio_ranges[] = {
 	MPP_GPIO_RANGE(0,   0,  0, 32),
-	MPP_GPIO_RANGE(1,  32, 32, 27),
+	MPP_GPIO_RANGE(1,  32, 32, 28),
 };
 
 static int armada_39x_pinctrl_probe(struct platform_device *pdev)
diff --git a/drivers/pinctrl/mvebu/pinctrl-armada-xp.c b/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
index 578db9f033b2..bf70e0953576 100644
--- a/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
+++ b/drivers/pinctrl/mvebu/pinctrl-armada-xp.c
@@ -14,10 +14,7 @@
  * available: mv78230, mv78260 and mv78460. From a pin muxing
  * perspective, the mv78230 has 49 MPP pins. The mv78260 and mv78460
  * both have 67 MPP pins (more GPIOs and address lines for the memory
- * bus mainly). The only difference between the mv78260 and the
- * mv78460 in terms of pin muxing is the addition of two functions on
- * pins 43 and 56 to access the VDD of the CPU2 and 3 (mv78260 has two
- * cores, mv78460 has four cores).
+ * bus mainly).
  */
 
 #include <linux/err.h>
@@ -57,7 +54,7 @@ enum armada_xp_variant {
 static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 	MPP_MODE(0,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "ge0", "txclko",     V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x1, "ge0", "txclkout",   V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "d0",         V_MV78230_PLUS)),
 	MPP_MODE(1,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
@@ -106,17 +103,19 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 	MPP_MODE(12,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "ge0", "txd4",       V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x2, "ge1", "clkout",     V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x2, "ge1", "txclkout",   V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "d12",        V_MV78230_PLUS)),
 	MPP_MODE(13,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "ge0", "txd5",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "ge1", "txd0",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x3, "spi1", "mosi",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "d13",        V_MV78230_PLUS)),
 	MPP_MODE(14,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "ge0", "txd6",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "ge1", "txd1",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x3, "spi1", "sck",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "d14",        V_MV78230_PLUS)),
 	MPP_MODE(15,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
@@ -127,11 +126,13 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "ge0", "txclk",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "ge1", "txd3",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x3, "spi1", "cs0",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "d16",        V_MV78230_PLUS)),
 	MPP_MODE(17,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "ge0", "col",        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "ge1", "txctl",      V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x3, "spi1", "miso",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "d17",        V_MV78230_PLUS)),
 	MPP_MODE(18,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
@@ -155,7 +156,7 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "ge0", "rxd5",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "ge1", "rxd3",       V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "mem", "bat",        V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x3, "dram", "bat",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "d21",        V_MV78230_PLUS)),
 	MPP_MODE(22,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
@@ -172,20 +173,17 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 	MPP_MODE(24,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "sata1", "prsnt",    V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x2, "nf", "bootcs-re",   V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x3, "tdm", "rst",        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "hsync",      V_MV78230_PLUS)),
 	MPP_MODE(25,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "sata0", "prsnt",    V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x2, "nf", "bootcs-we",   V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x3, "tdm", "pclk",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "vsync",      V_MV78230_PLUS)),
 	MPP_MODE(26,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x3, "tdm", "fsync",      V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x4, "lcd", "clk",        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "vdd", "cpu1-pd",    V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x4, "lcd", "clk",        V_MV78230_PLUS)),
 	MPP_MODE(27,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "ptp", "trig",       V_MV78230_PLUS),
@@ -200,8 +198,7 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "ptp", "clk",        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x3, "tdm", "int0",       V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x4, "lcd", "ref-clk",    V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "vdd", "cpu0-pd",    V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x4, "lcd", "ref-clk",    V_MV78230_PLUS)),
 	MPP_MODE(30,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "sd0", "clk",        V_MV78230_PLUS),
@@ -209,23 +206,23 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 	MPP_MODE(31,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "sd0", "cmd",        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "tdm", "int2",       V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "vdd", "cpu0-pd",    V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x3, "tdm", "int2",       V_MV78230_PLUS)),
 	MPP_MODE(32,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "sd0", "d0",         V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "tdm", "int3",       V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "vdd", "cpu1-pd",    V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x3, "tdm", "int3",       V_MV78230_PLUS)),
 	MPP_MODE(33,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "sd0", "d1",         V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x3, "tdm", "int4",       V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x4, "mem", "bat",        V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x4, "dram", "bat",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x5, "dram", "vttctrl",   V_MV78230_PLUS)),
 	MPP_MODE(34,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "sd0", "d2",         V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "sata0", "prsnt",    V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "tdm", "int5",       V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x3, "tdm", "int5",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x4, "dram", "deccerr",   V_MV78230_PLUS)),
 	MPP_MODE(35,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "sd0", "d3",         V_MV78230_PLUS),
@@ -233,74 +230,80 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(0x3, "tdm", "int6",       V_MV78230_PLUS)),
 	MPP_MODE(36,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "spi", "mosi",       V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x1, "spi0", "mosi",      V_MV78230_PLUS)),
 	MPP_MODE(37,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "spi", "miso",       V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x1, "spi0", "miso",      V_MV78230_PLUS)),
 	MPP_MODE(38,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "spi", "sck",        V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x1, "spi0", "sck",       V_MV78230_PLUS)),
 	MPP_MODE(39,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "spi", "cs0",        V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x1, "spi0", "cs0",       V_MV78230_PLUS)),
 	MPP_MODE(40,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "spi", "cs1",        V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x1, "spi0", "cs1",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "uart2", "cts",      V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "vdd", "cpu1-pd",    V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "vga-hsync",  V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "pcie", "clkreq0",   V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x5, "pcie", "clkreq0",   V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x6, "spi1", "cs1",       V_MV78230_PLUS)),
 	MPP_MODE(41,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "spi", "cs2",        V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x1, "spi0", "cs2",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "uart2", "rts",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x3, "sata1", "prsnt",    V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "lcd", "vga-vsync",  V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "pcie", "clkreq1",   V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x5, "pcie", "clkreq1",   V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x6, "spi1", "cs2",       V_MV78230_PLUS)),
 	MPP_MODE(42,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "uart2", "rxd",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "uart0", "cts",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x3, "tdm", "int7",       V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x4, "tdm-1", "timer",    V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "vdd", "cpu0-pd",    V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x4, "tdm", "timer",      V_MV78230_PLUS)),
 	MPP_MODE(43,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "uart2", "txd",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "uart0", "rts",      V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "spi", "cs3",        V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x3, "spi0", "cs3",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "pcie", "rstout",    V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "vdd", "cpu2-3-pd",  V_MV78460)),
+		 MPP_VAR_FUNCTION(0x6, "spi1", "cs3",       V_MV78230_PLUS)),
 	MPP_MODE(44,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "uart2", "cts",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "uart3", "rxd",      V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "spi", "cs4",        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x4, "mem", "bat",        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "pcie", "clkreq2",   V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x3, "spi0", "cs4",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x4, "dram", "bat",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x5, "pcie", "clkreq2",   V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x6, "spi1", "cs4",       V_MV78230_PLUS)),
 	MPP_MODE(45,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "uart2", "rts",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "uart3", "txd",      V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "spi", "cs5",        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x4, "sata1", "prsnt",    V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x3, "spi0", "cs5",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x4, "sata1", "prsnt",    V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x5, "dram", "vttctrl",   V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x6, "spi1", "cs5",       V_MV78230_PLUS)),
 	MPP_MODE(46,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "uart3", "rts",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "uart1", "rts",      V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "spi", "cs6",        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x4, "sata0", "prsnt",    V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x3, "spi0", "cs6",       V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x4, "sata0", "prsnt",    V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x6, "spi1", "cs6",       V_MV78230_PLUS)),
 	MPP_MODE(47,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "uart3", "cts",      V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x2, "uart1", "cts",      V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x3, "spi", "cs7",        V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x3, "spi0", "cs7",       V_MV78230_PLUS),
 		 MPP_VAR_FUNCTION(0x4, "ref", "clkout",     V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x5, "pcie", "clkreq3",   V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x5, "pcie", "clkreq3",   V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x6, "spi1", "cs7",       V_MV78230_PLUS)),
 	MPP_MODE(48,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "tclk", NULL,        V_MV78230_PLUS),
-		 MPP_VAR_FUNCTION(0x2, "dev", "burst/last", V_MV78230_PLUS)),
+		 MPP_VAR_FUNCTION(0x1, "dev", "clkout",     V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x2, "dev", "burst/last", V_MV78230_PLUS),
+		 MPP_VAR_FUNCTION(0x3, "nand", "rb",        V_MV78230_PLUS)),
 	MPP_MODE(49,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78260_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "dev", "we3",        V_MV78260_PLUS)),
@@ -321,16 +324,13 @@ static struct mvebu_mpp_mode armada_xp_mpp_modes[] = {
 		 MPP_VAR_FUNCTION(0x1, "dev", "ad19",       V_MV78260_PLUS)),
 	MPP_MODE(55,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78260_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "dev", "ad20",       V_MV78260_PLUS),
-		 MPP_VAR_FUNCTION(0x2, "vdd", "cpu0-pd",    V_MV78260_PLUS)),
+		 MPP_VAR_FUNCTION(0x1, "dev", "ad20",       V_MV78260_PLUS)),
 	MPP_MODE(56,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78260_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "dev", "ad21",       V_MV78260_PLUS),
-		 MPP_VAR_FUNCTION(0x2, "vdd", "cpu1-pd",    V_MV78260_PLUS)),
+		 MPP_VAR_FUNCTION(0x1, "dev", "ad21",       V_MV78260_PLUS)),
 	MPP_MODE(57,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78260_PLUS),
-		 MPP_VAR_FUNCTION(0x1, "dev", "ad22",       V_MV78260_PLUS),
-		 MPP_VAR_FUNCTION(0x2, "vdd", "cpu2-3-pd",  V_MV78460)),
+		 MPP_VAR_FUNCTION(0x1, "dev", "ad22",       V_MV78260_PLUS)),
 	MPP_MODE(58,
 		 MPP_VAR_FUNCTION(0x0, "gpio", NULL,        V_MV78260_PLUS),
 		 MPP_VAR_FUNCTION(0x1, "dev", "ad23",       V_MV78260_PLUS)),
diff --git a/drivers/pinctrl/mvebu/pinctrl-mvebu.c b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
index f3b426cdaf8f..77d2221d379d 100644
--- a/drivers/pinctrl/mvebu/pinctrl-mvebu.c
+++ b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
@@ -706,9 +706,9 @@ int mvebu_pinctrl_probe(struct platform_device *pdev)
 	}
 
 	pctl->pctldev = pinctrl_register(&pctl->desc, &pdev->dev, pctl);
-	if (!pctl->pctldev) {
+	if (IS_ERR(pctl->pctldev)) {
 		dev_err(&pdev->dev, "unable to register pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(pctl->pctldev);
 	}
 
 	dev_info(&pdev->dev, "registered pinctrl driver\n");
diff --git a/drivers/pinctrl/nomadik/pinctrl-ab8505.c b/drivers/pinctrl/nomadik/pinctrl-ab8505.c
index bf0ef4ac376f..42c6e1f7886b 100644
--- a/drivers/pinctrl/nomadik/pinctrl-ab8505.c
+++ b/drivers/pinctrl/nomadik/pinctrl-ab8505.c
@@ -286,7 +286,7 @@ alternate_functions ab8505_alternate_functions[AB8505_GPIO_MAX_NUMBER + 1] = {
 	ALTERNATE_FUNCTIONS(9, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO9, bit 0 reserved */
 	ALTERNATE_FUNCTIONS(10,      1,      0, UNUSED, 1, 0, 0), /* GPIO10, altA and altB controlled by bit 0 */
 	ALTERNATE_FUNCTIONS(11,      2,      1, UNUSED, 0, 0, 0), /* GPIO11, altA controlled by bit 2 */
-	ALTERNATE_FUNCTIONS(12, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO12, bit3 reseved */
+	ALTERNATE_FUNCTIONS(12, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO12, bit3 reserved */
 	ALTERNATE_FUNCTIONS(13,      4,      3,      4, 1, 0, 2), /* GPIO13, altA altB and altC controlled by bit 3 and 4 */
 	ALTERNATE_FUNCTIONS(14,      5, UNUSED, UNUSED, 0, 0, 0), /* GPIO14, altA controlled by bit 5 */
 	ALTERNATE_FUNCTIONS(15, UNUSED, UNUSED, UNUSED, 0, 0, 0), /* no GPIO15, bit 6 reserved */
diff --git a/drivers/pinctrl/nomadik/pinctrl-abx500.c b/drivers/pinctrl/nomadik/pinctrl-abx500.c
index 23db4c9ac76c..557d0f2a3031 100644
--- a/drivers/pinctrl/nomadik/pinctrl-abx500.c
+++ b/drivers/pinctrl/nomadik/pinctrl-abx500.c
@@ -787,6 +787,7 @@ static const struct pinmux_ops abx500_pinmux_ops = {
 	.set_mux = abx500_pmx_set,
 	.gpio_request_enable = abx500_gpio_request_enable,
 	.gpio_disable_free = abx500_gpio_disable_free,
+	.strict = true,
 };
 
 static int abx500_get_groups_cnt(struct pinctrl_dev *pctldev)
@@ -1234,10 +1235,10 @@ static int abx500_gpio_probe(struct platform_device *pdev)
 	abx500_pinctrl_desc.pins = pct->soc->pins;
 	abx500_pinctrl_desc.npins = pct->soc->npins;
 	pct->pctldev = pinctrl_register(&abx500_pinctrl_desc, &pdev->dev, pct);
-	if (!pct->pctldev) {
+	if (IS_ERR(pct->pctldev)) {
 		dev_err(&pdev->dev,
 			"could not register abx500 pinctrl driver\n");
-		ret = -EINVAL;
+		ret = PTR_ERR(pct->pctldev);
 		goto out_rem_chip;
 	}
 	dev_info(&pdev->dev, "registered pin controller\n");
diff --git a/drivers/pinctrl/nomadik/pinctrl-nomadik.c b/drivers/pinctrl/nomadik/pinctrl-nomadik.c
index a6a22054c0ba..809d88445db5 100644
--- a/drivers/pinctrl/nomadik/pinctrl-nomadik.c
+++ b/drivers/pinctrl/nomadik/pinctrl-nomadik.c
@@ -246,6 +246,7 @@ enum nmk_gpio_slpm {
 
 struct nmk_gpio_chip {
 	struct gpio_chip chip;
+	struct irq_chip irqchip;
 	void __iomem *addr;
 	struct clk *clk;
 	unsigned int bank;
@@ -842,18 +843,6 @@ static void nmk_gpio_irq_shutdown(struct irq_data *d)
 	clk_disable(nmk_chip->clk);
 }
 
-static struct irq_chip nmk_gpio_irq_chip = {
-	.name		= "Nomadik-GPIO",
-	.irq_ack	= nmk_gpio_irq_ack,
-	.irq_mask	= nmk_gpio_irq_mask,
-	.irq_unmask	= nmk_gpio_irq_unmask,
-	.irq_set_type	= nmk_gpio_irq_set_type,
-	.irq_set_wake	= nmk_gpio_irq_set_wake,
-	.irq_startup	= nmk_gpio_irq_startup,
-	.irq_shutdown	= nmk_gpio_irq_shutdown,
-	.flags		= IRQCHIP_MASK_ON_SUSPEND,
-};
-
 static void __nmk_gpio_irq_handler(unsigned int irq, struct irq_desc *desc,
 				   u32 status)
 {
@@ -1077,18 +1066,6 @@ static inline void nmk_gpio_dbg_show_one(struct seq_file *s,
 #define nmk_gpio_dbg_show	NULL
 #endif
 
-/* This structure is replicated for each GPIO block allocated at probe time */
-static struct gpio_chip nmk_gpio_template = {
-	.request		= nmk_gpio_request,
-	.free			= nmk_gpio_free,
-	.direction_input	= nmk_gpio_make_input,
-	.get			= nmk_gpio_get_input,
-	.direction_output	= nmk_gpio_make_output,
-	.set			= nmk_gpio_set_output,
-	.dbg_show		= nmk_gpio_dbg_show,
-	.can_sleep		= false,
-};
-
 void nmk_gpio_clocks_enable(void)
 {
 	int i;
@@ -1190,6 +1167,7 @@ static int nmk_gpio_probe(struct platform_device *dev)
 	struct device_node *np = dev->dev.of_node;
 	struct nmk_gpio_chip *nmk_chip;
 	struct gpio_chip *chip;
+	struct irq_chip *irqchip;
 	struct resource *res;
 	struct clk *clk;
 	int latent_irq;
@@ -1236,19 +1214,40 @@ static int nmk_gpio_probe(struct platform_device *dev)
 	nmk_chip->bank = dev->id;
 	nmk_chip->clk = clk;
 	nmk_chip->addr = base;
-	nmk_chip->chip = nmk_gpio_template;
 	nmk_chip->parent_irq = irq;
 	nmk_chip->latent_parent_irq = latent_irq;
 	nmk_chip->sleepmode = supports_sleepmode;
 	spin_lock_init(&nmk_chip->lock);
 
 	chip = &nmk_chip->chip;
+	chip->request = nmk_gpio_request;
+	chip->free = nmk_gpio_free;
+	chip->direction_input = nmk_gpio_make_input;
+	chip->get = nmk_gpio_get_input;
+	chip->direction_output = nmk_gpio_make_output;
+	chip->set = nmk_gpio_set_output;
+	chip->dbg_show = nmk_gpio_dbg_show;
+	chip->can_sleep = false;
 	chip->base = dev->id * NMK_GPIO_PER_CHIP;
 	chip->ngpio = NMK_GPIO_PER_CHIP;
 	chip->label = dev_name(&dev->dev);
 	chip->dev = &dev->dev;
 	chip->owner = THIS_MODULE;
 
+	irqchip = &nmk_chip->irqchip;
+	irqchip->irq_ack = nmk_gpio_irq_ack;
+	irqchip->irq_mask = nmk_gpio_irq_mask;
+	irqchip->irq_unmask = nmk_gpio_irq_unmask;
+	irqchip->irq_set_type = nmk_gpio_irq_set_type;
+	irqchip->irq_set_wake = nmk_gpio_irq_set_wake;
+	irqchip->irq_startup = nmk_gpio_irq_startup;
+	irqchip->irq_shutdown = nmk_gpio_irq_shutdown;
+	irqchip->flags = IRQCHIP_MASK_ON_SUSPEND;
+	irqchip->name = kasprintf(GFP_KERNEL, "nmk%u-%u-%u",
+				  dev->id,
+				  chip->base,
+				  chip->base + chip->ngpio - 1);
+
 	clk_enable(nmk_chip->clk);
 	nmk_chip->lowemi = readl_relaxed(nmk_chip->addr + NMK_GPIO_LOWEMI);
 	clk_disable(nmk_chip->clk);
@@ -1269,8 +1268,8 @@ static int nmk_gpio_probe(struct platform_device *dev)
 	 * handler will perform the actual work of handling the parent
 	 * interrupt.
 	 */
-	ret = gpiochip_irqchip_add(&nmk_chip->chip,
-				   &nmk_gpio_irq_chip,
+	ret = gpiochip_irqchip_add(chip,
+				   irqchip,
 				   0,
 				   handle_edge_irq,
 				   IRQ_TYPE_EDGE_FALLING);
@@ -1280,13 +1279,13 @@ static int nmk_gpio_probe(struct platform_device *dev)
 		return -ENODEV;
 	}
 	/* Then register the chain on the parent IRQ */
-	gpiochip_set_chained_irqchip(&nmk_chip->chip,
-				     &nmk_gpio_irq_chip,
+	gpiochip_set_chained_irqchip(chip,
+				     irqchip,
 				     nmk_chip->parent_irq,
 				     nmk_gpio_irq_handler);
 	if (nmk_chip->latent_parent_irq > 0)
-		gpiochip_set_chained_irqchip(&nmk_chip->chip,
-					     &nmk_gpio_irq_chip,
+		gpiochip_set_chained_irqchip(chip,
+					     irqchip,
 					     nmk_chip->latent_parent_irq,
 					     nmk_gpio_latent_irq_handler);
 
@@ -1803,6 +1802,7 @@ static const struct pinmux_ops nmk_pinmux_ops = {
 	.set_mux = nmk_pmx_set,
 	.gpio_request_enable = nmk_gpio_request_enable,
 	.gpio_disable_free = nmk_gpio_disable_free,
+	.strict = true,
 };
 
 static int nmk_pin_config_get(struct pinctrl_dev *pctldev, unsigned pin,
@@ -2029,9 +2029,9 @@ static int nmk_pinctrl_probe(struct platform_device *pdev)
 	npct->dev = &pdev->dev;
 
 	npct->pctl = pinctrl_register(&nmk_pinctrl_desc, &pdev->dev, npct);
-	if (!npct->pctl) {
+	if (IS_ERR(npct->pctl)) {
 		dev_err(&pdev->dev, "could not register Nomadik pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(npct->pctl);
 	}
 
 	/* We will handle a range of GPIO pins */
diff --git a/drivers/pinctrl/pinctrl-adi2.c b/drivers/pinctrl/pinctrl-adi2.c
index 8434439c5017..c3c3d2345fc6 100644
--- a/drivers/pinctrl/pinctrl-adi2.c
+++ b/drivers/pinctrl/pinctrl-adi2.c
@@ -703,6 +703,7 @@ static struct pinmux_ops adi_pinmux_ops = {
 	.get_function_name = adi_pinmux_get_func_name,
 	.get_function_groups = adi_pinmux_get_groups,
 	.gpio_request_enable = adi_pinmux_request_gpio,
+	.strict = true,
 };
 
 
@@ -864,8 +865,8 @@ static int adi_gpio_pint_probe(struct platform_device *pdev)
 	pint->pint_map_port = adi_pint_map_port;
 	platform_set_drvdata(pdev, pint);
 
-	irq_set_chained_handler(pint->irq, adi_gpio_handle_pint_irq);
-	irq_set_handler_data(pint->irq, pint);
+	irq_set_chained_handler_and_data(pint->irq, adi_gpio_handle_pint_irq,
+					 pint);
 
 	list_add_tail(&pint->node, &adi_pint_list);
 
@@ -1069,9 +1070,9 @@ static int adi_pinctrl_probe(struct platform_device *pdev)
 
 	/* Now register the pin controller and all pins it handles */
 	pinctrl->pctl = pinctrl_register(&adi_pinmux_desc, &pdev->dev, pinctrl);
-	if (!pinctrl->pctl) {
+	if (IS_ERR(pinctrl->pctl)) {
 		dev_err(&pdev->dev, "could not register pinctrl ADI2 driver\n");
-		return -EINVAL;
+		return PTR_ERR(pinctrl->pctl);
 	}
 
 	platform_set_drvdata(pdev, pinctrl);
diff --git a/drivers/pinctrl/pinctrl-amd.c b/drivers/pinctrl/pinctrl-amd.c
index 7de3b64bf142..d8e3f7c7fea3 100644
--- a/drivers/pinctrl/pinctrl-amd.c
+++ b/drivers/pinctrl/pinctrl-amd.c
@@ -789,9 +789,9 @@ static int amd_gpio_probe(struct platform_device *pdev)
 	amd_pinctrl_desc.name = dev_name(&pdev->dev);
 	gpio_dev->pctrl = pinctrl_register(&amd_pinctrl_desc,
 					&pdev->dev, gpio_dev);
-	if (!gpio_dev->pctrl) {
+	if (IS_ERR(gpio_dev->pctrl)) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(gpio_dev->pctrl);
 	}
 
 	ret = gpiochip_add(&gpio_dev->gc);
@@ -855,7 +855,6 @@ MODULE_DEVICE_TABLE(acpi, amd_gpio_acpi_match);
 static struct platform_driver amd_gpio_driver = {
 	.driver		= {
 		.name	= "amd_gpio",
-		.owner	= THIS_MODULE,
 		.acpi_match_table = ACPI_PTR(amd_gpio_acpi_match),
 	},
 	.probe		= amd_gpio_probe,
diff --git a/drivers/pinctrl/pinctrl-as3722.c b/drivers/pinctrl/pinctrl-as3722.c
index db0571ffbe99..4747e08f5389 100644
--- a/drivers/pinctrl/pinctrl-as3722.c
+++ b/drivers/pinctrl/pinctrl-as3722.c
@@ -586,9 +586,9 @@ static int as3722_pinctrl_probe(struct platform_device *pdev)
 	as3722_pinctrl_desc.npins = ARRAY_SIZE(as3722_pins_desc);
 	as_pci->pctl = pinctrl_register(&as3722_pinctrl_desc,
 					&pdev->dev, as_pci);
-	if (!as_pci->pctl) {
+	if (IS_ERR(as_pci->pctl)) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(as_pci->pctl);
 	}
 
 	as_pci->gpio_chip = as3722_gpio_chip;
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index 2f797cb7e205..a0824477072b 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -1238,9 +1238,9 @@ static int at91_pinctrl_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, info);
 	info->pctl = pinctrl_register(&at91_pinctrl_desc, &pdev->dev, info);
 
-	if (!info->pctl) {
+	if (IS_ERR(info->pctl)) {
 		dev_err(&pdev->dev, "could not register AT91 pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(info->pctl);
 	}
 
 	/* We will handle a range of GPIO pins */
@@ -1326,6 +1326,21 @@ static void at91_gpio_set(struct gpio_chip *chip, unsigned offset,
 	writel_relaxed(mask, pio + (val ? PIO_SODR : PIO_CODR));
 }
 
+static void at91_gpio_set_multiple(struct gpio_chip *chip,
+				      unsigned long *mask, unsigned long *bits)
+{
+	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	void __iomem *pio = at91_gpio->regbase;
+
+#define BITS_MASK(bits) (((bits) == 32) ? ~0U : (BIT(bits) - 1))
+	/* Mask additionally to ngpio as not all GPIO controllers have 32 pins */
+	uint32_t set_mask = (*mask & *bits) & BITS_MASK(chip->ngpio);
+	uint32_t clear_mask = (*mask & ~(*bits)) & BITS_MASK(chip->ngpio);
+
+	writel_relaxed(set_mask, pio + PIO_SODR);
+	writel_relaxed(clear_mask, pio + PIO_CODR);
+}
+
 static int at91_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 				int val)
 {
@@ -1685,6 +1700,7 @@ static struct gpio_chip at91_gpio_template = {
 	.get			= at91_gpio_get,
 	.direction_output	= at91_gpio_direction_output,
 	.set			= at91_gpio_set,
+	.set_multiple		= at91_gpio_set_multiple,
 	.dbg_show		= at91_gpio_dbg_show,
 	.can_sleep		= false,
 	.ngpio			= MAX_NB_GPIO_PER_BANK,
diff --git a/drivers/pinctrl/pinctrl-lantiq.c b/drivers/pinctrl/pinctrl-lantiq.c
index 296e5b37f768..fc38a8540544 100644
--- a/drivers/pinctrl/pinctrl-lantiq.c
+++ b/drivers/pinctrl/pinctrl-lantiq.c
@@ -337,9 +337,9 @@ int ltq_pinctrl_register(struct platform_device *pdev,
 	info->dev = &pdev->dev;
 
 	info->pctrl = pinctrl_register(desc, &pdev->dev, info);
-	if (!info->pctrl) {
+	if (IS_ERR(info->pctrl)) {
 		dev_err(&pdev->dev, "failed to register LTQ pinmux driver\n");
-		return -EINVAL;
+		return PTR_ERR(info->pctrl);
 	}
 	platform_set_drvdata(pdev, info);
 	return 0;
diff --git a/drivers/pinctrl/pinctrl-lpc18xx.c b/drivers/pinctrl/pinctrl-lpc18xx.c
new file mode 100644
index 000000000000..ef0b697639a7
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-lpc18xx.c
@@ -0,0 +1,1220 @@
+/*
+ * Pinctrl driver for NXP LPC18xx/LPC43xx System Control Unit (SCU)
+ *
+ * Copyright (C) 2015 Joachim Eastwood <manabian@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/bitops.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinconf-generic.h>
+
+#include "core.h"
+#include "pinctrl-utils.h"
+
+/* LPC18XX SCU analog function registers */
+#define LPC18XX_SCU_REG_ENAIO0		0xc88
+#define LPC18XX_SCU_REG_ENAIO1		0xc8c
+#define LPC18XX_SCU_REG_ENAIO2		0xc90
+#define LPC18XX_SCU_REG_ENAIO2_DAC	BIT(0)
+
+/* LPC18XX SCU pin register definitions */
+#define LPC18XX_SCU_PIN_MODE_MASK	0x7
+#define LPC18XX_SCU_PIN_EPD		BIT(3)
+#define LPC18XX_SCU_PIN_EPUN		BIT(4)
+#define LPC18XX_SCU_PIN_EHS		BIT(5)
+#define LPC18XX_SCU_PIN_EZI		BIT(6)
+#define LPC18XX_SCU_PIN_ZIF		BIT(7)
+#define LPC18XX_SCU_PIN_EHD_MASK	0x300
+#define LPC18XX_SCU_PIN_EHD_POS		8
+
+#define LPC18XX_SCU_I2C0_EFP		BIT(0)
+#define LPC18XX_SCU_I2C0_EHD		BIT(2)
+#define LPC18XX_SCU_I2C0_EZI		BIT(3)
+#define LPC18XX_SCU_I2C0_ZIF		BIT(7)
+#define LPC18XX_SCU_I2C0_SCL_SHIFT	0
+#define LPC18XX_SCU_I2C0_SDA_SHIFT	8
+
+#define LPC18XX_SCU_FUNC_PER_PIN	8
+
+/* LPC18xx pin types */
+enum {
+	TYPE_ND,	/* Normal-drive */
+	TYPE_HD,	/* High-drive */
+	TYPE_HS,	/* High-speed */
+	TYPE_I2C0,
+	TYPE_USB1,
+};
+
+/* LPC18xx pin functions */
+enum {
+	FUNC_R,		/* Reserved */
+	FUNC_ADC,
+	FUNC_ADCTRIG,
+	FUNC_CAN0,
+	FUNC_CAN1,
+	FUNC_CGU_OUT,
+	FUNC_CLKIN,
+	FUNC_CLKOUT,
+	FUNC_CTIN,
+	FUNC_CTOUT,
+	FUNC_DAC,
+	FUNC_EMC,
+	FUNC_EMC_ALT,
+	FUNC_ENET,
+	FUNC_ENET_ALT,
+	FUNC_GPIO,
+	FUNC_I2C0,
+	FUNC_I2C1,
+	FUNC_I2S0_RX_MCLK,
+	FUNC_I2S0_RX_SCK,
+	FUNC_I2S0_RX_SDA,
+	FUNC_I2S0_RX_WS,
+	FUNC_I2S0_TX_MCLK,
+	FUNC_I2S0_TX_SCK,
+	FUNC_I2S0_TX_SDA,
+	FUNC_I2S0_TX_WS,
+	FUNC_I2S1,
+	FUNC_LCD,
+	FUNC_LCD_ALT,
+	FUNC_MCTRL,
+	FUNC_NMI,
+	FUNC_QEI,
+	FUNC_SDMMC,
+	FUNC_SGPIO,
+	FUNC_SPI,
+	FUNC_SPIFI,
+	FUNC_SSP0,
+	FUNC_SSP0_ALT,
+	FUNC_SSP1,
+	FUNC_TIMER0,
+	FUNC_TIMER1,
+	FUNC_TIMER2,
+	FUNC_TIMER3,
+	FUNC_TRACE,
+	FUNC_UART0,
+	FUNC_UART1,
+	FUNC_UART2,
+	FUNC_UART3,
+	FUNC_USB0,
+	FUNC_USB1,
+	FUNC_MAX
+};
+
+static const char *const lpc18xx_function_names[] = {
+	[FUNC_R]		= "reserved",
+	[FUNC_ADC]		= "adc",
+	[FUNC_ADCTRIG]		= "adctrig",
+	[FUNC_CAN0]		= "can0",
+	[FUNC_CAN1]		= "can1",
+	[FUNC_CGU_OUT]		= "cgu_out",
+	[FUNC_CLKIN]		= "clkin",
+	[FUNC_CLKOUT]		= "clkout",
+	[FUNC_CTIN]		= "ctin",
+	[FUNC_CTOUT]		= "ctout",
+	[FUNC_DAC]		= "dac",
+	[FUNC_EMC]		= "emc",
+	[FUNC_EMC_ALT]		= "emc_alt",
+	[FUNC_ENET]		= "enet",
+	[FUNC_ENET_ALT]		= "enet_alt",
+	[FUNC_GPIO]		= "gpio",
+	[FUNC_I2C0]		= "i2c0",
+	[FUNC_I2C1]		= "i2c1",
+	[FUNC_I2S0_RX_MCLK]	= "i2s0_rx_mclk",
+	[FUNC_I2S0_RX_SCK]	= "i2s0_rx_sck",
+	[FUNC_I2S0_RX_SDA]	= "i2s0_rx_sda",
+	[FUNC_I2S0_RX_WS]	= "i2s0_rx_ws",
+	[FUNC_I2S0_TX_MCLK]	= "i2s0_tx_mclk",
+	[FUNC_I2S0_TX_SCK]	= "i2s0_tx_sck",
+	[FUNC_I2S0_TX_SDA]	= "i2s0_tx_sda",
+	[FUNC_I2S0_TX_WS]	= "i2s0_tx_ws",
+	[FUNC_I2S1]		= "i2s1",
+	[FUNC_LCD]		= "lcd",
+	[FUNC_LCD_ALT]		= "lcd_alt",
+	[FUNC_MCTRL]		= "mctrl",
+	[FUNC_NMI]		= "nmi",
+	[FUNC_QEI]		= "qei",
+	[FUNC_SDMMC]		= "sdmmc",
+	[FUNC_SGPIO]		= "sgpio",
+	[FUNC_SPI]		= "spi",
+	[FUNC_SPIFI]		= "spifi",
+	[FUNC_SSP0]		= "ssp0",
+	[FUNC_SSP0_ALT]		= "ssp0_alt",
+	[FUNC_SSP1]		= "ssp1",
+	[FUNC_TIMER0]		= "timer0",
+	[FUNC_TIMER1]		= "timer1",
+	[FUNC_TIMER2]		= "timer2",
+	[FUNC_TIMER3]		= "timer3",
+	[FUNC_TRACE]		= "trace",
+	[FUNC_UART0]		= "uart0",
+	[FUNC_UART1]		= "uart1",
+	[FUNC_UART2]		= "uart2",
+	[FUNC_UART3]		= "uart3",
+	[FUNC_USB0]		= "usb0",
+	[FUNC_USB1]		= "usb1",
+};
+
+struct lpc18xx_pmx_func {
+	const char **groups;
+	unsigned ngroups;
+};
+
+struct lpc18xx_scu_data {
+	struct pinctrl_dev *pctl;
+	void __iomem *base;
+	struct clk *clk;
+	struct lpc18xx_pmx_func func[FUNC_MAX];
+};
+
+struct lpc18xx_pin_caps {
+	unsigned int offset;
+	unsigned char functions[LPC18XX_SCU_FUNC_PER_PIN];
+	unsigned char analog;
+	unsigned char type;
+};
+
+/* Analog pins are required to have both bias and input disabled */
+#define LPC18XX_SCU_ANALOG_PIN_CFG	0x10
+
+/* Macros to maniupluate analog member in lpc18xx_pin_caps */
+#define LPC18XX_ANALOG_PIN		BIT(7)
+#define LPC18XX_ANALOG_ADC(a)		((a >> 5) & 0x3)
+#define LPC18XX_ANALOG_BIT_MASK		0x1f
+#define ADC0				(LPC18XX_ANALOG_PIN | (0x00 << 5))
+#define ADC1				(LPC18XX_ANALOG_PIN | (0x01 << 5))
+#define DAC				LPC18XX_ANALOG_PIN
+
+#define LPC_P(port, pin, f0, f1, f2, f3, f4, f5, f6, f7, a, t)	\
+static struct lpc18xx_pin_caps lpc18xx_pin_p##port##_##pin = {	\
+	.offset = 0x##port * 32 * 4 + pin * 4,			\
+	.functions = {						\
+			FUNC_##f0, FUNC_##f1, FUNC_##f2,	\
+			FUNC_##f3, FUNC_##f4, FUNC_##f5,	\
+			FUNC_##f6, FUNC_##f7,			\
+	},							\
+	.analog = a,						\
+	.type = TYPE_##t,					\
+}
+
+#define LPC_N(pname, off, f0, f1, f2, f3, f4, f5, f6, f7, a, t)	\
+static struct lpc18xx_pin_caps lpc18xx_pin_##pname = {		\
+	.offset = off,						\
+	.functions = {						\
+			FUNC_##f0, FUNC_##f1, FUNC_##f2,	\
+			FUNC_##f3, FUNC_##f4, FUNC_##f5,	\
+			FUNC_##f6, FUNC_##f7,			\
+	},							\
+	.analog = a,						\
+	.type = TYPE_##t,					\
+}
+
+
+/* Pinmuxing table taken from data sheet */
+/*    Pin    FUNC0  FUNC1  FUNC2  FUNC3   FUNC4   FUNC5   FUNC6    FUNC7 ANALOG TYPE */
+LPC_P(0,0,   GPIO,  SSP1,  ENET,  SGPIO,      R,      R, I2S0_TX_WS,I2S1,     0, ND);
+LPC_P(0,1,   GPIO,  SSP1,ENET_ALT,SGPIO,      R,      R,   ENET,    I2S1,     0, ND);
+LPC_P(1,0,   GPIO,  CTIN,   EMC,      R,      R,   SSP0,  SGPIO,       R,     0, ND);
+LPC_P(1,1,   GPIO, CTOUT,   EMC,  SGPIO,      R,   SSP0,      R,       R,     0, ND);
+LPC_P(1,2,   GPIO, CTOUT,   EMC,  SGPIO,      R,   SSP0,      R,       R,     0, ND);
+LPC_P(1,3,   GPIO, CTOUT, SGPIO,    EMC,   USB0,   SSP1,      R,   SDMMC,     0, ND);
+LPC_P(1,4,   GPIO, CTOUT, SGPIO,    EMC,   USB0,   SSP1,      R,   SDMMC,     0, ND);
+LPC_P(1,5,   GPIO, CTOUT,     R,    EMC,   USB0,   SSP1,  SGPIO,   SDMMC,     0, ND);
+LPC_P(1,6,   GPIO,  CTIN,     R,    EMC,      R,      R,  SGPIO,   SDMMC,     0, ND);
+LPC_P(1,7,   GPIO, UART1, CTOUT,    EMC,   USB0,      R,      R,       R,     0, ND);
+LPC_P(1,8,   GPIO, UART1, CTOUT,    EMC,      R,      R,      R,   SDMMC,     0, ND);
+LPC_P(1,9,   GPIO, UART1, CTOUT,    EMC,      R,      R,      R,   SDMMC,     0, ND);
+LPC_P(1,10,  GPIO, UART1, CTOUT,    EMC,      R,      R,      R,   SDMMC,     0, ND);
+LPC_P(1,11,  GPIO, UART1, CTOUT,    EMC,      R,      R,      R,   SDMMC,     0, ND);
+LPC_P(1,12,  GPIO, UART1,     R,    EMC, TIMER0,      R,  SGPIO,   SDMMC,     0, ND);
+LPC_P(1,13,  GPIO, UART1,     R,    EMC, TIMER0,      R,  SGPIO,   SDMMC,     0, ND);
+LPC_P(1,14,  GPIO, UART1,     R,    EMC, TIMER0,      R,  SGPIO,       R,     0, ND);
+LPC_P(1,15,  GPIO, UART2, SGPIO,   ENET, TIMER0,      R,      R,       R,     0, ND);
+LPC_P(1,16,  GPIO, UART2, SGPIO,ENET_ALT,TIMER0,      R,      R,    ENET,     0, ND);
+LPC_P(1,17,  GPIO, UART2,     R,   ENET, TIMER0,   CAN1,  SGPIO,       R,     0, HD);
+LPC_P(1,18,  GPIO, UART2,     R,   ENET, TIMER0,   CAN1,  SGPIO,       R,     0, ND);
+LPC_P(1,19,  ENET,  SSP1,     R,      R, CLKOUT,      R, I2S0_RX_MCLK,I2S1,   0, ND);
+LPC_P(1,20,  GPIO,  SSP1,     R,   ENET, TIMER0,      R,  SGPIO,       R,     0, ND);
+LPC_P(2,0,  SGPIO, UART0,   EMC,   USB0,   GPIO,      R, TIMER3,    ENET,     0, ND);
+LPC_P(2,1,  SGPIO, UART0,   EMC,   USB0,   GPIO,      R, TIMER3,       R,     0, ND);
+LPC_P(2,2,  SGPIO, UART0,   EMC,   USB0,   GPIO,   CTIN, TIMER3,       R,     0, ND);
+LPC_P(2,3,  SGPIO,  I2C1, UART3,   CTIN,   GPIO,      R, TIMER3,    USB0,     0, HD);
+LPC_P(2,4,  SGPIO,  I2C1, UART3,   CTIN,   GPIO,      R, TIMER3,    USB0,     0, HD);
+LPC_P(2,5,  SGPIO,  CTIN,  USB1, ADCTRIG,  GPIO,      R, TIMER3,    USB0,     0, HD);
+LPC_P(2,6,  SGPIO, UART0,   EMC,   USB0,   GPIO,   CTIN, TIMER3,       R,     0, ND);
+LPC_P(2,7,   GPIO, CTOUT, UART3,    EMC,      R,      R, TIMER3,       R,     0, ND);
+LPC_P(2,8,  SGPIO, CTOUT, UART3,    EMC,   GPIO,      R,      R,       R,     0, ND);
+LPC_P(2,9,   GPIO, CTOUT, UART3,    EMC,      R,      R,      R,       R,     0, ND);
+LPC_P(2,10,  GPIO, CTOUT, UART2,    EMC,      R,      R,      R,       R,     0, ND);
+LPC_P(2,11,  GPIO, CTOUT, UART2,    EMC,      R,      R,      R,       R,     0, ND);
+LPC_P(2,12,  GPIO, CTOUT,     R,    EMC,      R,      R,      R,   UART2,     0, ND);
+LPC_P(2,13,  GPIO,  CTIN,     R,    EMC,      R,      R,      R,   UART2,     0, ND);
+LPC_P(3,0,  I2S0_RX_SCK, I2S0_RX_MCLK, I2S0_TX_SCK, I2S0_TX_MCLK,SSP0,R,R,R,  0, ND);
+LPC_P(3,1,  I2S0_TX_WS, I2S0_RX_WS,CAN0,USB1,GPIO,    R,    LCD,       R,     0, ND);
+LPC_P(3,2,  I2S0_TX_SDA, I2S0_RX_SDA,CAN0,USB1,GPIO,  R,    LCD,      R,      0, ND);
+LPC_P(3,3,      R,   SPI,  SSP0,  SPIFI, CGU_OUT,R, I2S0_TX_MCLK,  I2S1,      0, HS);
+LPC_P(3,4,   GPIO,     R,     R,  SPIFI,  UART1, I2S0_TX_WS, I2S1,  LCD,      0, ND);
+LPC_P(3,5,   GPIO,     R,     R,  SPIFI,  UART1, I2S0_TX_SDA,I2S1,  LCD,      0, ND);
+LPC_P(3,6,   GPIO,   SPI,  SSP0,  SPIFI,      R,  SSP0_ALT,   R,      R,      0, ND);
+LPC_P(3,7,      R,   SPI,  SSP0,  SPIFI,   GPIO,  SSP0_ALT,   R,      R,      0, ND);
+LPC_P(3,8,      R,   SPI,  SSP0,  SPIFI,   GPIO,  SSP0_ALT,   R,      R,      0, ND);
+LPC_P(4,0,   GPIO, MCTRL,   NMI,      R,      R,    LCD,  UART3,      R,      0, ND);
+LPC_P(4,1,   GPIO, CTOUT,   LCD,      R,      R, LCD_ALT, UART3,   ENET, ADC0|1, ND);
+LPC_P(4,2,   GPIO, CTOUT,   LCD,      R,      R, LCD_ALT, UART3,  SGPIO,      0, ND);
+LPC_P(4,3,   GPIO, CTOUT,   LCD,      R,      R, LCD_ALT, UART3,  SGPIO, ADC0|0, ND);
+LPC_P(4,4,   GPIO, CTOUT,   LCD,      R,      R, LCD_ALT, UART3,  SGPIO,    DAC, ND);
+LPC_P(4,5,   GPIO, CTOUT,   LCD,      R,      R,      R,      R,  SGPIO,      0, ND);
+LPC_P(4,6,   GPIO, CTOUT,   LCD,      R,      R,      R,      R,  SGPIO,      0, ND);
+LPC_P(4,7,    LCD, CLKIN,     R,      R,      R,      R,   I2S1,I2S0_TX_SCK,  0, ND);
+LPC_P(4,8,      R,  CTIN,   LCD,      R,   GPIO, LCD_ALT,  CAN1,  SGPIO,      0, ND);
+LPC_P(4,9,      R,  CTIN,   LCD,      R,   GPIO, LCD_ALT,  CAN1,  SGPIO,      0, ND);
+LPC_P(4,10,     R,  CTIN,   LCD,      R,   GPIO, LCD_ALT,     R,  SGPIO,      0, ND);
+LPC_P(5,0,   GPIO, MCTRL,   EMC,      R,  UART1, TIMER1,      R,      R,      0, ND);
+LPC_P(5,1,   GPIO, MCTRL,   EMC,      R,  UART1, TIMER1,      R,      R,      0, ND);
+LPC_P(5,2,   GPIO, MCTRL,   EMC,      R,  UART1, TIMER1,      R,      R,      0, ND);
+LPC_P(5,3,   GPIO, MCTRL,   EMC,      R,  UART1, TIMER1,      R,      R,      0, ND);
+LPC_P(5,4,   GPIO, MCTRL,   EMC,      R,  UART1, TIMER1,      R,      R,      0, ND);
+LPC_P(5,5,   GPIO, MCTRL,   EMC,      R,  UART1, TIMER1,      R,      R,      0, ND);
+LPC_P(5,6,   GPIO, MCTRL,   EMC,      R,  UART1, TIMER1,      R,      R,      0, ND);
+LPC_P(5,7,   GPIO, MCTRL,   EMC,      R,  UART1, TIMER1,      R,      R,      0, ND);
+LPC_P(6,0,      R, I2S0_RX_MCLK,R,    R, I2S0_RX_SCK, R,      R,      R,      0, ND);
+LPC_P(6,1,   GPIO,   EMC, UART0, I2S0_RX_WS,  R, TIMER2,      R,      R,      0, ND);
+LPC_P(6,2,   GPIO,   EMC, UART0, I2S0_RX_SDA, R, TIMER2,      R,      R,      0, ND);
+LPC_P(6,3,   GPIO,  USB0, SGPIO,    EMC,      R, TIMER2,      R,      R,      0, ND);
+LPC_P(6,4,   GPIO,  CTIN, UART0,    EMC,      R,      R,      R,      R,      0, ND);
+LPC_P(6,5,   GPIO, CTOUT, UART0,    EMC,      R,      R,      R,      R,      0, ND);
+LPC_P(6,6,   GPIO,   EMC, SGPIO,   USB0,      R, TIMER2,      R,      R,      0, ND);
+LPC_P(6,7,      R,   EMC, SGPIO,   USB0,   GPIO, TIMER2,      R,      R,      0, ND);
+LPC_P(6,8,      R,   EMC, SGPIO,   USB0,   GPIO, TIMER2,      R,      R,      0, ND);
+LPC_P(6,9,   GPIO,     R,     R,    EMC,      R, TIMER2,      R,      R,      0, ND);
+LPC_P(6,10,  GPIO, MCTRL,     R,    EMC,      R,      R,      R,      R,      0, ND);
+LPC_P(6,11,  GPIO,     R,     R,    EMC,      R, TIMER2,      R,      R,      0, ND);
+LPC_P(6,12,  GPIO, CTOUT,     R,    EMC,      R,      R,      R,      R,      0, ND);
+LPC_P(7,0,   GPIO, CTOUT,     R,    LCD,      R,      R,      R,  SGPIO,      0, ND);
+LPC_P(7,1,   GPIO, CTOUT,I2S0_TX_WS,LCD,LCD_ALT,      R,  UART2,  SGPIO,      0, ND);
+LPC_P(7,2,   GPIO, CTIN,I2S0_TX_SDA,LCD,LCD_ALT,      R,  UART2,  SGPIO,      0, ND);
+LPC_P(7,3,   GPIO, CTIN,      R,    LCD,LCD_ALT,      R,      R,      R,      0, ND);
+LPC_P(7,4,   GPIO, CTOUT,     R,    LCD,LCD_ALT,  TRACE,      R,      R, ADC0|4, ND);
+LPC_P(7,5,   GPIO, CTOUT,     R,    LCD,LCD_ALT,  TRACE,      R,      R, ADC0|3, ND);
+LPC_P(7,6,   GPIO, CTOUT,     R,    LCD,      R,  TRACE,      R,      R,      0, ND);
+LPC_P(7,7,   GPIO, CTOUT,     R,    LCD,      R,  TRACE,   ENET,  SGPIO, ADC1|6, ND);
+LPC_P(8,0,   GPIO,  USB0,     R,  MCTRL,  SGPIO,      R,      R, TIMER0,      0, HD);
+LPC_P(8,1,   GPIO,  USB0,     R,  MCTRL,  SGPIO,      R,      R, TIMER0,      0, HD);
+LPC_P(8,2,   GPIO,  USB0,     R,  MCTRL,  SGPIO,      R,      R, TIMER0,      0, HD);
+LPC_P(8,3,   GPIO,  USB1,     R,    LCD, LCD_ALT,     R,      R, TIMER0,      0, ND);
+LPC_P(8,4,   GPIO,  USB1,     R,    LCD, LCD_ALT,     R,      R, TIMER0,      0, ND);
+LPC_P(8,5,   GPIO,  USB1,     R,    LCD, LCD_ALT,     R,      R, TIMER0,      0, ND);
+LPC_P(8,6,   GPIO,  USB1,     R,    LCD, LCD_ALT,     R,      R, TIMER0,      0, ND);
+LPC_P(8,7,   GPIO,  USB1,     R,    LCD, LCD_ALT,     R,      R, TIMER0,      0, ND);
+LPC_P(8,8,      R,  USB1,     R,      R,      R,      R,CGU_OUT,   I2S1,      0, ND);
+LPC_P(9,0,   GPIO, MCTRL,     R,      R,      R,   ENET,  SGPIO,   SSP0,      0, ND);
+LPC_P(9,1,   GPIO, MCTRL,     R,      R, I2S0_TX_WS,ENET, SGPIO,   SSP0,      0, ND);
+LPC_P(9,2,   GPIO, MCTRL,     R,      R, I2S0_TX_SDA,ENET,SGPIO,   SSP0,      0, ND);
+LPC_P(9,3,   GPIO, MCTRL,  USB1,      R,      R,   ENET,  SGPIO,  UART3,      0, ND);
+LPC_P(9,4,      R, MCTRL,  USB1,      R,   GPIO,   ENET,  SGPIO,  UART3,      0, ND);
+LPC_P(9,5,      R, MCTRL,  USB1,      R,   GPIO,   ENET,  SGPIO,  UART0,      0, ND);
+LPC_P(9,6,   GPIO, MCTRL,  USB1,      R,      R,   ENET,  SGPIO,  UART0,      0, ND);
+LPC_P(a,0,      R,     R,     R,      R,      R,   I2S1, CGU_OUT,     R,      0, ND);
+LPC_P(a,1,   GPIO,   QEI,     R,  UART2,      R,      R,      R,      R,      0, HD);
+LPC_P(a,2,   GPIO,   QEI,     R,  UART2,      R,      R,      R,      R,      0, HD);
+LPC_P(a,3,   GPIO,   QEI,     R,      R,      R,      R,      R,      R,      0, HD);
+LPC_P(a,4,      R, CTOUT,     R,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(b,0,      R, CTOUT,   LCD,      R,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(b,1,      R,  USB1,   LCD,      R,   GPIO,  CTOUT,      R,      R,      0, ND);
+LPC_P(b,2,      R,  USB1,   LCD,      R,   GPIO,  CTOUT,      R,      R,      0, ND);
+LPC_P(b,3,      R,  USB1,   LCD,      R,   GPIO,  CTOUT,      R,      R,      0, ND);
+LPC_P(b,4,      R,  USB1,   LCD,      R,   GPIO,   CTIN,      R,      R,      0, ND);
+LPC_P(b,5,      R,  USB1,   LCD,      R,   GPIO,   CTIN, LCD_ALT,     R,      0, ND);
+LPC_P(b,6,      R,  USB1,   LCD,      R,   GPIO,   CTIN, LCD_ALT,     R, ADC0|6, ND);
+LPC_P(c,0,      R,  USB1,     R,   ENET,    LCD,      R,      R,  SDMMC, ADC1|1, ND);
+LPC_P(c,1,   USB1,     R, UART1,   ENET,   GPIO,      R, TIMER3,  SDMMC,      0, ND);
+LPC_P(c,2,   USB1,     R, UART1,   ENET,   GPIO,      R,      R,  SDMMC,      0, ND);
+LPC_P(c,3,   USB1,     R, UART1,   ENET,   GPIO,      R,      R,  SDMMC, ADC1|0, ND);
+LPC_P(c,4,      R,  USB1,     R,   ENET,   GPIO,      R, TIMER3,  SDMMC,      0, ND);
+LPC_P(c,5,      R,  USB1,     R,   ENET,   GPIO,      R, TIMER3,  SDMMC,      0, ND);
+LPC_P(c,6,      R,  USB1,     R,   ENET,   GPIO,      R, TIMER3,  SDMMC,      0, ND);
+LPC_P(c,7,      R,  USB1,     R,   ENET,   GPIO,      R, TIMER3,  SDMMC,      0, ND);
+LPC_P(c,8,      R,  USB1,     R,   ENET,   GPIO,      R, TIMER3,  SDMMC,      0, ND);
+LPC_P(c,9,      R,  USB1,     R,   ENET,   GPIO,      R, TIMER3,  SDMMC,      0, ND);
+LPC_P(c,10,     R,  USB1, UART1,      R,   GPIO,      R, TIMER3,  SDMMC,      0, ND);
+LPC_P(c,11,     R,  USB1, UART1,      R,   GPIO,      R,      R,  SDMMC,      0, ND);
+LPC_P(c,12,     R,     R, UART1,      R,   GPIO,  SGPIO, I2S0_TX_SDA,SDMMC,   0, ND);
+LPC_P(c,13,     R,     R, UART1,      R,   GPIO,  SGPIO, I2S0_TX_WS, SDMMC,   0, ND);
+LPC_P(c,14,     R,     R, UART1,      R,   GPIO,  SGPIO,   ENET,  SDMMC,      0, ND);
+LPC_P(d,0,      R, CTOUT,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,1,      R,     R,   EMC,      R,   GPIO,  SDMMC,      R,  SGPIO,      0, ND);
+LPC_P(d,2,      R, CTOUT,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,3,      R, CTOUT,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,4,      R, CTOUT,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,5,      R, CTOUT,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,6,      R, CTOUT,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,7,      R,  CTIN,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,8,      R,  CTIN,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,9,      R, CTOUT,   EMC,      R,   GPIO,      R,      R,  SGPIO,      0, ND);
+LPC_P(d,10,     R,  CTIN,   EMC,      R,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(d,11,     R,     R,   EMC,      R,   GPIO,   USB1,  CTOUT,      R,      0, ND);
+LPC_P(d,12,     R,     R,   EMC,      R,   GPIO,      R,  CTOUT,      R,      0, ND);
+LPC_P(d,13,     R,  CTIN,   EMC,      R,   GPIO,      R,  CTOUT,      R,      0, ND);
+LPC_P(d,14,     R,     R,   EMC,      R,   GPIO,      R,  CTOUT,      R,      0, ND);
+LPC_P(d,15,     R,     R,   EMC,      R,   GPIO,  SDMMC,  CTOUT,      R,      0, ND);
+LPC_P(d,16,     R,     R,   EMC,      R,   GPIO,  SDMMC,  CTOUT,      R,      0, ND);
+LPC_P(e,0,      R,     R,     R,    EMC,   GPIO,   CAN1,      R,      R,      0, ND);
+LPC_P(e,1,      R,     R,     R,    EMC,   GPIO,   CAN1,      R,      R,      0, ND);
+LPC_P(e,2,ADCTRIG,  CAN0,     R,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,3,      R,  CAN0,ADCTRIG,   EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,4,      R,   NMI,     R,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,5,      R, CTOUT, UART1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,6,      R, CTOUT, UART1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,7,      R, CTOUT, UART1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,8,      R, CTOUT, UART1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,9,      R,  CTIN, UART1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,10,     R,  CTIN, UART1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,11,     R, CTOUT, UART1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,12,     R, CTOUT, UART1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,13,     R, CTOUT,  I2C1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,14,     R,     R,     R,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(e,15,     R, CTOUT,  I2C1,    EMC,   GPIO,      R,      R,      R,      0, ND);
+LPC_P(f,0,   SSP0, CLKIN,     R,      R,      R,      R,      R,   I2S1,      0, ND);
+LPC_P(f,1,      R,     R,  SSP0,      R,   GPIO,      R,  SGPIO,      R,      0, ND);
+LPC_P(f,2,      R, UART3,  SSP0,      R,   GPIO,      R,  SGPIO,      R,      0, ND);
+LPC_P(f,3,      R, UART3,  SSP0,      R,   GPIO,      R,  SGPIO,      R,      0, ND);
+LPC_P(f,4,   SSP1, CLKIN, TRACE,      R,      R, R, I2S0_TX_MCLK,I2S0_RX_SCK, 0, ND);
+LPC_P(f,5,      R, UART3,  SSP1,  TRACE,   GPIO,      R,  SGPIO,      R, ADC1|4, ND);
+LPC_P(f,6,      R, UART3,  SSP1,  TRACE,   GPIO,      R,  SGPIO,   I2S1, ADC1|3, ND);
+LPC_P(f,7,      R, UART3,  SSP1,  TRACE,   GPIO,      R,  SGPIO,   I2S1, ADC1|7, ND);
+LPC_P(f,8,      R, UART0,  CTIN,  TRACE,   GPIO,      R,  SGPIO,      R, ADC0|2, ND);
+LPC_P(f,9,      R, UART0, CTOUT,      R,   GPIO,      R,  SGPIO,      R, ADC1|2, ND);
+LPC_P(f,10,     R, UART0,     R,      R,   GPIO,      R,  SDMMC,      R, ADC0|5, ND);
+LPC_P(f,11,     R, UART0,     R,      R,   GPIO,      R,  SDMMC,      R, ADC1|5, ND);
+
+/*    Pin      Offset FUNC0  FUNC1  FUNC2  FUNC3  FUNC4    FUNC5   FUNC6      FUNC7 ANALOG TYPE */
+LPC_N(clk0,     0xc00, EMC, CLKOUT,   R,     R,  SDMMC,   EMC_ALT,  SSP1,      ENET,  0, HS);
+LPC_N(clk1,     0xc04, EMC, CLKOUT,   R,     R,      R,   CGU_OUT,   R,        I2S1,  0, HS);
+LPC_N(clk2,     0xc08, EMC, CLKOUT,   R,     R,  SDMMC,   EMC_ALT,I2S0_TX_MCLK,I2S1,  0, HS);
+LPC_N(clk3,     0xc0c, EMC, CLKOUT,   R,     R,      R,   CGU_OUT,   R,        I2S1,  0, HS);
+LPC_N(usb1_dm,  0xc80, R,      R,     R,     R,      R,      R,      R,          R,   0, USB1);
+LPC_N(usb1_dp,  0xc80, R,      R,     R,     R,      R,      R,      R,          R,   0, USB1);
+LPC_N(i2c0_scl, 0xc84, R,      R,     R,     R,      R,      R,      R,          R,   0, I2C0);
+LPC_N(i2c0_sda, 0xc84, R,      R,     R,     R,      R,      R,      R,          R,   0, I2C0);
+
+#define LPC18XX_PIN_P(port, pin) {			\
+	.number = 0x##port * 32 + pin,			\
+	.name = "p"#port"_"#pin,			\
+	.drv_data = &lpc18xx_pin_p##port##_##pin 	\
+}
+
+/* Pin numbers for special pins */
+enum {
+	PIN_CLK0 = 600,
+	PIN_CLK1,
+	PIN_CLK2,
+	PIN_CLK3,
+	PIN_USB1_DM,
+	PIN_USB1_DP,
+	PIN_I2C0_SCL,
+	PIN_I2C0_SDA,
+};
+
+#define LPC18XX_PIN(pname, n) {				\
+	.number = n,					\
+	.name = #pname,					\
+	.drv_data = &lpc18xx_pin_##pname 		\
+}
+
+static const struct pinctrl_pin_desc lpc18xx_pins[] = {
+	LPC18XX_PIN_P(0,0),
+	LPC18XX_PIN_P(0,1),
+	LPC18XX_PIN_P(1,0),
+	LPC18XX_PIN_P(1,1),
+	LPC18XX_PIN_P(1,2),
+	LPC18XX_PIN_P(1,3),
+	LPC18XX_PIN_P(1,4),
+	LPC18XX_PIN_P(1,5),
+	LPC18XX_PIN_P(1,6),
+	LPC18XX_PIN_P(1,7),
+	LPC18XX_PIN_P(1,8),
+	LPC18XX_PIN_P(1,9),
+	LPC18XX_PIN_P(1,10),
+	LPC18XX_PIN_P(1,11),
+	LPC18XX_PIN_P(1,12),
+	LPC18XX_PIN_P(1,13),
+	LPC18XX_PIN_P(1,14),
+	LPC18XX_PIN_P(1,15),
+	LPC18XX_PIN_P(1,16),
+	LPC18XX_PIN_P(1,17),
+	LPC18XX_PIN_P(1,18),
+	LPC18XX_PIN_P(1,19),
+	LPC18XX_PIN_P(1,20),
+	LPC18XX_PIN_P(2,0),
+	LPC18XX_PIN_P(2,1),
+	LPC18XX_PIN_P(2,2),
+	LPC18XX_PIN_P(2,3),
+	LPC18XX_PIN_P(2,4),
+	LPC18XX_PIN_P(2,5),
+	LPC18XX_PIN_P(2,6),
+	LPC18XX_PIN_P(2,7),
+	LPC18XX_PIN_P(2,8),
+	LPC18XX_PIN_P(2,9),
+	LPC18XX_PIN_P(2,10),
+	LPC18XX_PIN_P(2,11),
+	LPC18XX_PIN_P(2,12),
+	LPC18XX_PIN_P(2,13),
+	LPC18XX_PIN_P(3,0),
+	LPC18XX_PIN_P(3,1),
+	LPC18XX_PIN_P(3,2),
+	LPC18XX_PIN_P(3,3),
+	LPC18XX_PIN_P(3,4),
+	LPC18XX_PIN_P(3,5),
+	LPC18XX_PIN_P(3,6),
+	LPC18XX_PIN_P(3,7),
+	LPC18XX_PIN_P(3,8),
+	LPC18XX_PIN_P(4,0),
+	LPC18XX_PIN_P(4,1),
+	LPC18XX_PIN_P(4,2),
+	LPC18XX_PIN_P(4,3),
+	LPC18XX_PIN_P(4,4),
+	LPC18XX_PIN_P(4,5),
+	LPC18XX_PIN_P(4,6),
+	LPC18XX_PIN_P(4,7),
+	LPC18XX_PIN_P(4,8),
+	LPC18XX_PIN_P(4,9),
+	LPC18XX_PIN_P(4,10),
+	LPC18XX_PIN_P(5,0),
+	LPC18XX_PIN_P(5,1),
+	LPC18XX_PIN_P(5,2),
+	LPC18XX_PIN_P(5,3),
+	LPC18XX_PIN_P(5,4),
+	LPC18XX_PIN_P(5,5),
+	LPC18XX_PIN_P(5,6),
+	LPC18XX_PIN_P(5,7),
+	LPC18XX_PIN_P(6,0),
+	LPC18XX_PIN_P(6,1),
+	LPC18XX_PIN_P(6,2),
+	LPC18XX_PIN_P(6,3),
+	LPC18XX_PIN_P(6,4),
+	LPC18XX_PIN_P(6,5),
+	LPC18XX_PIN_P(6,6),
+	LPC18XX_PIN_P(6,7),
+	LPC18XX_PIN_P(6,8),
+	LPC18XX_PIN_P(6,9),
+	LPC18XX_PIN_P(6,10),
+	LPC18XX_PIN_P(6,11),
+	LPC18XX_PIN_P(6,12),
+	LPC18XX_PIN_P(7,0),
+	LPC18XX_PIN_P(7,1),
+	LPC18XX_PIN_P(7,2),
+	LPC18XX_PIN_P(7,3),
+	LPC18XX_PIN_P(7,4),
+	LPC18XX_PIN_P(7,5),
+	LPC18XX_PIN_P(7,6),
+	LPC18XX_PIN_P(7,7),
+	LPC18XX_PIN_P(8,0),
+	LPC18XX_PIN_P(8,1),
+	LPC18XX_PIN_P(8,2),
+	LPC18XX_PIN_P(8,3),
+	LPC18XX_PIN_P(8,4),
+	LPC18XX_PIN_P(8,5),
+	LPC18XX_PIN_P(8,6),
+	LPC18XX_PIN_P(8,7),
+	LPC18XX_PIN_P(8,8),
+	LPC18XX_PIN_P(9,0),
+	LPC18XX_PIN_P(9,1),
+	LPC18XX_PIN_P(9,2),
+	LPC18XX_PIN_P(9,3),
+	LPC18XX_PIN_P(9,4),
+	LPC18XX_PIN_P(9,5),
+	LPC18XX_PIN_P(9,6),
+	LPC18XX_PIN_P(a,0),
+	LPC18XX_PIN_P(a,1),
+	LPC18XX_PIN_P(a,2),
+	LPC18XX_PIN_P(a,3),
+	LPC18XX_PIN_P(a,4),
+	LPC18XX_PIN_P(b,0),
+	LPC18XX_PIN_P(b,1),
+	LPC18XX_PIN_P(b,2),
+	LPC18XX_PIN_P(b,3),
+	LPC18XX_PIN_P(b,4),
+	LPC18XX_PIN_P(b,5),
+	LPC18XX_PIN_P(b,6),
+	LPC18XX_PIN_P(c,0),
+	LPC18XX_PIN_P(c,1),
+	LPC18XX_PIN_P(c,2),
+	LPC18XX_PIN_P(c,3),
+	LPC18XX_PIN_P(c,4),
+	LPC18XX_PIN_P(c,5),
+	LPC18XX_PIN_P(c,6),
+	LPC18XX_PIN_P(c,7),
+	LPC18XX_PIN_P(c,8),
+	LPC18XX_PIN_P(c,9),
+	LPC18XX_PIN_P(c,10),
+	LPC18XX_PIN_P(c,11),
+	LPC18XX_PIN_P(c,12),
+	LPC18XX_PIN_P(c,13),
+	LPC18XX_PIN_P(c,14),
+	LPC18XX_PIN_P(d,0),
+	LPC18XX_PIN_P(d,1),
+	LPC18XX_PIN_P(d,2),
+	LPC18XX_PIN_P(d,3),
+	LPC18XX_PIN_P(d,4),
+	LPC18XX_PIN_P(d,5),
+	LPC18XX_PIN_P(d,6),
+	LPC18XX_PIN_P(d,7),
+	LPC18XX_PIN_P(d,8),
+	LPC18XX_PIN_P(d,9),
+	LPC18XX_PIN_P(d,10),
+	LPC18XX_PIN_P(d,11),
+	LPC18XX_PIN_P(d,12),
+	LPC18XX_PIN_P(d,13),
+	LPC18XX_PIN_P(d,14),
+	LPC18XX_PIN_P(d,15),
+	LPC18XX_PIN_P(d,16),
+	LPC18XX_PIN_P(e,0),
+	LPC18XX_PIN_P(e,1),
+	LPC18XX_PIN_P(e,2),
+	LPC18XX_PIN_P(e,3),
+	LPC18XX_PIN_P(e,4),
+	LPC18XX_PIN_P(e,5),
+	LPC18XX_PIN_P(e,6),
+	LPC18XX_PIN_P(e,7),
+	LPC18XX_PIN_P(e,8),
+	LPC18XX_PIN_P(e,9),
+	LPC18XX_PIN_P(e,10),
+	LPC18XX_PIN_P(e,11),
+	LPC18XX_PIN_P(e,12),
+	LPC18XX_PIN_P(e,13),
+	LPC18XX_PIN_P(e,14),
+	LPC18XX_PIN_P(e,15),
+	LPC18XX_PIN_P(f,0),
+	LPC18XX_PIN_P(f,1),
+	LPC18XX_PIN_P(f,2),
+	LPC18XX_PIN_P(f,3),
+	LPC18XX_PIN_P(f,4),
+	LPC18XX_PIN_P(f,5),
+	LPC18XX_PIN_P(f,6),
+	LPC18XX_PIN_P(f,7),
+	LPC18XX_PIN_P(f,8),
+	LPC18XX_PIN_P(f,9),
+	LPC18XX_PIN_P(f,10),
+	LPC18XX_PIN_P(f,11),
+
+	LPC18XX_PIN(clk0, PIN_CLK0),
+	LPC18XX_PIN(clk1, PIN_CLK1),
+	LPC18XX_PIN(clk2, PIN_CLK2),
+	LPC18XX_PIN(clk3, PIN_CLK3),
+	LPC18XX_PIN(usb1_dm,  PIN_USB1_DM),
+	LPC18XX_PIN(usb1_dp,  PIN_USB1_DP),
+	LPC18XX_PIN(i2c0_scl, PIN_I2C0_SCL),
+	LPC18XX_PIN(i2c0_sda, PIN_I2C0_SDA),
+};
+
+static int lpc18xx_pconf_get_usb1(enum pin_config_param param, int *arg, u32 reg)
+{
+	/* TODO */
+	return -ENOTSUPP;
+}
+
+static int lpc18xx_pconf_get_i2c0(enum pin_config_param param, int *arg, u32 reg,
+				  unsigned pin)
+{
+	u8 shift;
+
+	if (pin == PIN_I2C0_SCL)
+		shift = LPC18XX_SCU_I2C0_SCL_SHIFT;
+	else
+		shift = LPC18XX_SCU_I2C0_SDA_SHIFT;
+
+	switch (param) {
+	case PIN_CONFIG_INPUT_ENABLE:
+		if (reg & (LPC18XX_SCU_I2C0_EZI << shift))
+			*arg = 1;
+		else
+			return -EINVAL;
+		break;
+
+	case PIN_CONFIG_SLEW_RATE:
+		if (reg & (LPC18XX_SCU_I2C0_EHD << shift))
+			*arg = 1;
+		else
+			*arg = 0;
+		break;
+
+	case PIN_CONFIG_INPUT_SCHMITT:
+		if (reg & (LPC18XX_SCU_I2C0_EFP << shift))
+			*arg = 3;
+		else
+			*arg = 50;
+		break;
+
+	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+		if (reg & (LPC18XX_SCU_I2C0_ZIF << shift))
+			return -EINVAL;
+		else
+			*arg = 1;
+		break;
+
+	default:
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+static int lpc18xx_pconf_get_pin(enum pin_config_param param, int *arg, u32 reg,
+				 struct lpc18xx_pin_caps *pin_cap)
+{
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		if ((!(reg & LPC18XX_SCU_PIN_EPD)) && (reg & LPC18XX_SCU_PIN_EPUN))
+			;
+		else
+			return -EINVAL;
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_UP:
+		if (reg & LPC18XX_SCU_PIN_EPUN)
+			return -EINVAL;
+		else
+			*arg = 1;
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		if (reg & LPC18XX_SCU_PIN_EPD)
+			*arg = 1;
+		else
+			return -EINVAL;
+		break;
+
+	case PIN_CONFIG_INPUT_ENABLE:
+		if (reg & LPC18XX_SCU_PIN_EZI)
+			*arg = 1;
+		else
+			return -EINVAL;
+		break;
+
+	case PIN_CONFIG_SLEW_RATE:
+		if (pin_cap->type == TYPE_HD)
+			return -ENOTSUPP;
+
+		if (reg & LPC18XX_SCU_PIN_EHS)
+			*arg = 1;
+		else
+			*arg = 0;
+		break;
+
+	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+		if (reg & LPC18XX_SCU_PIN_ZIF)
+			return -EINVAL;
+		else
+			*arg = 1;
+		break;
+
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		if (pin_cap->type != TYPE_HD)
+			return -ENOTSUPP;
+
+		*arg = (reg & LPC18XX_SCU_PIN_EHD_MASK) >> LPC18XX_SCU_PIN_EHD_POS;
+		switch (*arg) {
+		case 3: *arg += 5;
+		case 2: *arg += 5;
+		case 1: *arg += 3;
+		case 0: *arg += 4;
+		}
+		break;
+
+	default:
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+static struct lpc18xx_pin_caps *lpc18xx_get_pin_caps(unsigned pin)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(lpc18xx_pins); i++) {
+		if (lpc18xx_pins[i].number == pin)
+			return lpc18xx_pins[i].drv_data;
+	}
+
+	return NULL;
+}
+
+static int lpc18xx_pconf_get(struct pinctrl_dev *pctldev, unsigned pin,
+			     unsigned long *config)
+{
+	struct lpc18xx_scu_data *scu = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	struct lpc18xx_pin_caps *pin_cap;
+	int ret, arg = 0;
+	u32 reg;
+
+	pin_cap = lpc18xx_get_pin_caps(pin);
+	if (!pin_cap)
+		return -EINVAL;
+
+	reg = readl(scu->base + pin_cap->offset);
+
+	if (pin_cap->type == TYPE_I2C0)
+		ret = lpc18xx_pconf_get_i2c0(param, &arg, reg, pin);
+	else if (pin_cap->type == TYPE_USB1)
+		ret = lpc18xx_pconf_get_usb1(param, &arg, reg);
+	else
+		ret = lpc18xx_pconf_get_pin(param, &arg, reg, pin_cap);
+
+	if (ret < 0)
+		return ret;
+
+	*config = pinconf_to_config_packed(param, (u16)arg);
+
+	return 0;
+}
+
+static int lpc18xx_pconf_set_usb1(struct pinctrl_dev *pctldev,
+				  enum pin_config_param param,
+				  u16 param_val, u32 *reg)
+{
+	/* TODO */
+	return -ENOTSUPP;
+}
+
+static int lpc18xx_pconf_set_i2c0(struct pinctrl_dev *pctldev,
+				  enum pin_config_param param,
+				  u16 param_val, u32 *reg,
+				  unsigned pin)
+{
+	u8 shift;
+
+	if (pin == PIN_I2C0_SCL)
+		shift = LPC18XX_SCU_I2C0_SCL_SHIFT;
+	else
+		shift = LPC18XX_SCU_I2C0_SDA_SHIFT;
+
+	switch (param) {
+	case PIN_CONFIG_INPUT_ENABLE:
+		if (param_val)
+			*reg |= (LPC18XX_SCU_I2C0_EZI << shift);
+		else
+			*reg &= ~(LPC18XX_SCU_I2C0_EZI << shift);
+		break;
+
+	case PIN_CONFIG_SLEW_RATE:
+		if (param_val)
+			*reg |= (LPC18XX_SCU_I2C0_EHD << shift);
+		else
+			*reg &= ~(LPC18XX_SCU_I2C0_EHD << shift);
+		break;
+
+	case PIN_CONFIG_INPUT_SCHMITT:
+		if (param_val == 3)
+			*reg |= (LPC18XX_SCU_I2C0_EFP << shift);
+		else if (param_val == 50)
+			*reg &= ~(LPC18XX_SCU_I2C0_EFP << shift);
+		else
+			return -ENOTSUPP;
+		break;
+
+	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+		if (param)
+			*reg &= ~(LPC18XX_SCU_I2C0_ZIF << shift);
+		else
+			*reg |= (LPC18XX_SCU_I2C0_ZIF << shift);
+		break;
+
+	default:
+		dev_err(pctldev->dev, "Property not supported\n");
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+static int lpc18xx_pconf_set_pin(struct pinctrl_dev *pctldev,
+				 enum pin_config_param param,
+				 u16 param_val, u32 *reg,
+				 struct lpc18xx_pin_caps *pin_cap)
+{
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		*reg &= ~LPC18XX_SCU_PIN_EPD;
+		*reg |= LPC18XX_SCU_PIN_EPUN;
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_UP:
+		*reg &= ~LPC18XX_SCU_PIN_EPUN;
+		break;
+
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		*reg |= LPC18XX_SCU_PIN_EPD;
+		break;
+
+	case PIN_CONFIG_INPUT_ENABLE:
+		if (param_val)
+			*reg |= LPC18XX_SCU_PIN_EZI;
+		else
+			*reg &= ~LPC18XX_SCU_PIN_EZI;
+		break;
+
+	case PIN_CONFIG_SLEW_RATE:
+		if (pin_cap->type == TYPE_HD) {
+			dev_err(pctldev->dev, "Slew rate unsupported on high-drive pins\n");
+			return -ENOTSUPP;
+		}
+
+		if (param_val == 0)
+			*reg &= ~LPC18XX_SCU_PIN_EHS;
+		else
+			*reg |= LPC18XX_SCU_PIN_EHS;
+		break;
+
+	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+		if (param)
+			*reg &= ~LPC18XX_SCU_PIN_ZIF;
+		else
+			*reg |= LPC18XX_SCU_PIN_ZIF;
+		break;
+
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		if (pin_cap->type != TYPE_HD) {
+			dev_err(pctldev->dev, "Drive strength available only on high-drive pins\n");
+			return -ENOTSUPP;
+		}
+		*reg &= ~LPC18XX_SCU_PIN_EHD_MASK;
+
+		switch (param_val) {
+		case 20: param_val -= 5;
+		case 14: param_val -= 5;
+		case  8: param_val -= 3;
+		case  4: param_val -= 4;
+			 break;
+		default:
+			dev_err(pctldev->dev, "Drive strength %u unsupported\n", param_val);
+			return -ENOTSUPP;
+		}
+		*reg |= param_val << LPC18XX_SCU_PIN_EHD_POS;
+		break;
+
+	default:
+		dev_err(pctldev->dev, "Property not supported\n");
+		return -ENOTSUPP;
+	}
+
+	return 0;
+}
+
+static int lpc18xx_pconf_set(struct pinctrl_dev *pctldev, unsigned pin,
+			     unsigned long *configs, unsigned num_configs)
+{
+	struct lpc18xx_scu_data *scu = pinctrl_dev_get_drvdata(pctldev);
+	struct lpc18xx_pin_caps *pin_cap;
+	enum pin_config_param param;
+	u16 param_val;
+	u32 reg;
+	int ret;
+	int i;
+
+	pin_cap = lpc18xx_get_pin_caps(pin);
+	if (!pin_cap)
+		return -EINVAL;
+
+	reg = readl(scu->base + pin_cap->offset);
+
+	for (i = 0; i < num_configs; i++) {
+		param = pinconf_to_config_param(configs[i]);
+		param_val = pinconf_to_config_argument(configs[i]);
+
+		if (pin_cap->type == TYPE_I2C0)
+			ret = lpc18xx_pconf_set_i2c0(pctldev, param, param_val, &reg, pin);
+		else if (pin_cap->type == TYPE_USB1)
+			ret = lpc18xx_pconf_set_usb1(pctldev, param, param_val, &reg);
+		else
+			ret = lpc18xx_pconf_set_pin(pctldev, param, param_val, &reg, pin_cap);
+
+		if (ret)
+			return ret;
+	}
+
+	writel(reg, scu->base + pin_cap->offset);
+
+	return 0;
+}
+
+static const struct pinconf_ops lpc18xx_pconf_ops = {
+	.is_generic	= true,
+	.pin_config_get	= lpc18xx_pconf_get,
+	.pin_config_set	= lpc18xx_pconf_set,
+};
+
+static int lpc18xx_pmx_get_funcs_count(struct pinctrl_dev *pctldev)
+{
+	return ARRAY_SIZE(lpc18xx_function_names);
+}
+
+static const char *lpc18xx_pmx_get_func_name(struct pinctrl_dev *pctldev,
+					     unsigned function)
+{
+	return lpc18xx_function_names[function];
+}
+
+static int lpc18xx_pmx_get_func_groups(struct pinctrl_dev *pctldev,
+				       unsigned function,
+				       const char *const **groups,
+				       unsigned *const num_groups)
+{
+	struct lpc18xx_scu_data *scu = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups  = scu->func[function].groups;
+	*num_groups = scu->func[function].ngroups;
+
+	return 0;
+}
+
+static int lpc18xx_pmx_set(struct pinctrl_dev *pctldev, unsigned function,
+			   unsigned group)
+{
+	struct lpc18xx_scu_data *scu = pinctrl_dev_get_drvdata(pctldev);
+	struct lpc18xx_pin_caps *pin = lpc18xx_pins[group].drv_data;
+	int func;
+	u32 reg;
+
+	/* Dedicated USB1 and I2C0 pins doesn't support muxing */
+	if (pin->type == TYPE_USB1) {
+		if (function == FUNC_USB1)
+			return 0;
+
+		goto fail;
+	}
+
+	if (pin->type == TYPE_I2C0) {
+		if (function == FUNC_I2C0)
+			return 0;
+
+		goto fail;
+	}
+
+	if (function == FUNC_ADC && (pin->analog & LPC18XX_ANALOG_PIN)) {
+		u32 offset;
+
+		writel(LPC18XX_SCU_ANALOG_PIN_CFG, scu->base + pin->offset);
+
+		if (LPC18XX_ANALOG_ADC(pin->analog) == 0)
+			offset = LPC18XX_SCU_REG_ENAIO0;
+		else
+			offset = LPC18XX_SCU_REG_ENAIO1;
+
+		reg = readl(scu->base + offset);
+		reg |= pin->analog & LPC18XX_ANALOG_BIT_MASK;
+		writel(reg, scu->base + offset);
+
+		return 0;
+	}
+
+	if (function == FUNC_DAC && (pin->analog & LPC18XX_ANALOG_PIN)) {
+		writel(LPC18XX_SCU_ANALOG_PIN_CFG, scu->base + pin->offset);
+
+		reg = readl(scu->base + LPC18XX_SCU_REG_ENAIO2);
+		reg |= LPC18XX_SCU_REG_ENAIO2_DAC;
+		writel(reg, scu->base + LPC18XX_SCU_REG_ENAIO2);
+
+		return 0;
+	}
+
+	for (func = 0; func < LPC18XX_SCU_FUNC_PER_PIN; func++) {
+		if (function == pin->functions[func])
+			break;
+	}
+
+	if (func >= LPC18XX_SCU_FUNC_PER_PIN)
+		goto fail;
+
+	reg = readl(scu->base + pin->offset);
+	reg &= ~LPC18XX_SCU_PIN_MODE_MASK;
+	writel(reg | func, scu->base + pin->offset);
+
+	return 0;
+fail:
+	dev_err(pctldev->dev, "Pin %s can't be %s\n", lpc18xx_pins[group].name,
+						      lpc18xx_function_names[function]);
+	return -EINVAL;
+}
+
+static const struct pinmux_ops lpc18xx_pmx_ops = {
+	.get_functions_count	= lpc18xx_pmx_get_funcs_count,
+	.get_function_name	= lpc18xx_pmx_get_func_name,
+	.get_function_groups	= lpc18xx_pmx_get_func_groups,
+	.set_mux		= lpc18xx_pmx_set,
+};
+
+static int lpc18xx_pctl_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	return ARRAY_SIZE(lpc18xx_pins);
+}
+
+static const char *lpc18xx_pctl_get_group_name(struct pinctrl_dev *pctldev,
+					       unsigned group)
+{
+	return lpc18xx_pins[group].name;
+}
+
+static int lpc18xx_pctl_get_group_pins(struct pinctrl_dev *pctldev,
+				       unsigned group,
+				       const unsigned **pins,
+				       unsigned *num_pins)
+{
+	*pins = &lpc18xx_pins[group].number;
+	*num_pins = 1;
+
+	return 0;
+}
+
+static const struct pinctrl_ops lpc18xx_pctl_ops = {
+	.get_groups_count	= lpc18xx_pctl_get_groups_count,
+	.get_group_name		= lpc18xx_pctl_get_group_name,
+	.get_group_pins		= lpc18xx_pctl_get_group_pins,
+	.dt_node_to_map		= pinconf_generic_dt_node_to_map_pin,
+	.dt_free_map		= pinctrl_utils_dt_free_map,
+};
+
+static struct pinctrl_desc lpc18xx_scu_desc = {
+	.name = "lpc18xx/43xx-scu",
+	.pins = lpc18xx_pins,
+	.npins = ARRAY_SIZE(lpc18xx_pins),
+	.pctlops = &lpc18xx_pctl_ops,
+	.pmxops = &lpc18xx_pmx_ops,
+	.confops = &lpc18xx_pconf_ops,
+	.owner = THIS_MODULE,
+};
+
+static bool lpc18xx_valid_pin_function(unsigned pin, unsigned function)
+{
+	struct lpc18xx_pin_caps *p = lpc18xx_pins[pin].drv_data;
+	int i;
+
+	if (function == FUNC_DAC && p->analog == DAC)
+		return true;
+
+	if (function == FUNC_ADC && p->analog)
+		return true;
+
+	if (function == FUNC_I2C0 && p->type == TYPE_I2C0)
+		return true;
+
+	if (function == FUNC_USB1 && p->type == TYPE_USB1)
+		return true;
+
+	for (i = 0; i < LPC18XX_SCU_FUNC_PER_PIN; i++) {
+		if (function == p->functions[i])
+			return true;
+	}
+
+	return false;
+}
+
+static int lpc18xx_create_group_func_map(struct device *dev,
+					 struct lpc18xx_scu_data *scu)
+{
+	u16 pins[ARRAY_SIZE(lpc18xx_pins)];
+	int func, ngroups, i;
+
+	for (func = 0; func < FUNC_MAX; ngroups = 0, func++) {
+
+		for (i = 0; i < ARRAY_SIZE(lpc18xx_pins); i++) {
+			if (lpc18xx_valid_pin_function(i, func))
+				pins[ngroups++] = i;
+		}
+
+		scu->func[func].ngroups = ngroups;
+		scu->func[func].groups = devm_kzalloc(dev, ngroups *
+						      sizeof(char *), GFP_KERNEL);
+		if (!scu->func[func].groups)
+			return -ENOMEM;
+
+		for (i = 0; i < ngroups; i++)
+			scu->func[func].groups[i] = lpc18xx_pins[pins[i]].name;
+	}
+
+	return 0;
+}
+
+static int lpc18xx_scu_probe(struct platform_device *pdev)
+{
+	struct lpc18xx_scu_data *scu;
+	struct resource *res;
+	int ret;
+
+	scu = devm_kzalloc(&pdev->dev, sizeof(*scu), GFP_KERNEL);
+	if (!scu)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	scu->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(scu->base))
+		return PTR_ERR(scu->base);
+
+	scu->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(scu->clk)) {
+		dev_err(&pdev->dev, "Input clock not found.\n");
+		return PTR_ERR(scu->clk);
+	}
+
+	ret = lpc18xx_create_group_func_map(&pdev->dev, scu);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to create group func map.\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(scu->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to enable clock.\n");
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, scu);
+
+	scu->pctl = pinctrl_register(&lpc18xx_scu_desc, &pdev->dev, scu);
+	if (IS_ERR(scu->pctl)) {
+		dev_err(&pdev->dev, "Could not register pinctrl driver\n");
+		clk_disable_unprepare(scu->clk);
+		return PTR_ERR(scu->pctl);
+	}
+
+	return 0;
+}
+
+static int lpc18xx_scu_remove(struct platform_device *pdev)
+{
+	struct lpc18xx_scu_data *scu = platform_get_drvdata(pdev);
+
+	pinctrl_unregister(scu->pctl);
+	clk_disable_unprepare(scu->clk);
+
+	return 0;
+}
+
+static const struct of_device_id lpc18xx_scu_match[] = {
+	{ .compatible = "nxp,lpc1850-scu" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, lpc18xx_scu_match);
+
+static struct platform_driver lpc18xx_scu_driver = {
+	.probe		= lpc18xx_scu_probe,
+	.remove		= lpc18xx_scu_remove,
+	.driver = {
+		.name		= "lpc18xx-scu",
+		.of_match_table	= lpc18xx_scu_match,
+	},
+};
+module_platform_driver(lpc18xx_scu_driver);
+
+MODULE_AUTHOR("Joachim Eastwood <manabian@gmail.com>");
+MODULE_DESCRIPTION("Pinctrl driver for NXP LPC18xx/43xx SCU");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pinctrl-palmas.c b/drivers/pinctrl/pinctrl-palmas.c
index 2631df0504bd..f7e168044baf 100644
--- a/drivers/pinctrl/pinctrl-palmas.c
+++ b/drivers/pinctrl/pinctrl-palmas.c
@@ -1044,9 +1044,9 @@ static int palmas_pinctrl_probe(struct platform_device *pdev)
 	palmas_pinctrl_desc.pins = palmas_pins_desc;
 	palmas_pinctrl_desc.npins = ARRAY_SIZE(palmas_pins_desc);
 	pci->pctl = pinctrl_register(&palmas_pinctrl_desc, &pdev->dev, pci);
-	if (!pci->pctl) {
+	if (IS_ERR(pci->pctl)) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(pci->pctl);
 	}
 	return 0;
 }
diff --git a/drivers/pinctrl/pinctrl-pistachio.c b/drivers/pinctrl/pinctrl-pistachio.c
new file mode 100644
index 000000000000..63100be81015
--- /dev/null
+++ b/drivers/pinctrl/pinctrl-pistachio.c
@@ -0,0 +1,1504 @@
+/*
+ * Pistachio SoC pinctrl driver
+ *
+ * Copyright (C) 2014 Imagination Technologies Ltd.
+ * Copyright (C) 2014 Google, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ */
+
+#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include "pinctrl-utils.h"
+
+#define PADS_SCHMITT_EN0		0x000
+#define PADS_SCHMITT_EN_REG(pin)	(PADS_SCHMITT_EN0 + 0x4 * ((pin) / 32))
+#define PADS_SCHMITT_EN_BIT(pin)	BIT((pin) % 32)
+
+#define PADS_PU_PD0			0x040
+#define PADS_PU_PD_REG(pin)		(PADS_PU_PD0 + 0x4 * ((pin) / 16))
+#define PADS_PU_PD_SHIFT(pin)		(2 * ((pin) % 16))
+#define PADS_PU_PD_MASK			0x3
+#define PADS_PU_PD_HIGHZ		0x0
+#define PADS_PU_PD_UP			0x1
+#define PADS_PU_PD_DOWN			0x2
+#define PADS_PU_PD_BUS			0x3
+
+#define PADS_FUNCTION_SELECT0		0x0c0
+#define PADS_FUNCTION_SELECT1		0x0c4
+#define PADS_FUNCTION_SELECT2		0x0c8
+#define PADS_SCENARIO_SELECT		0x0f8
+
+#define PADS_SLEW_RATE0			0x100
+#define PADS_SLEW_RATE_REG(pin)		(PADS_SLEW_RATE0 + 0x4 * ((pin) / 32))
+#define PADS_SLEW_RATE_BIT(pin)		BIT((pin) % 32)
+
+#define PADS_DRIVE_STRENGTH0		0x120
+#define PADS_DRIVE_STRENGTH_REG(pin)					\
+	(PADS_DRIVE_STRENGTH0 + 0x4 * ((pin) / 16))
+#define PADS_DRIVE_STRENGTH_SHIFT(pin)	(2 * ((pin) % 16))
+#define PADS_DRIVE_STRENGTH_MASK	0x3
+#define PADS_DRIVE_STRENGTH_2MA		0x0
+#define PADS_DRIVE_STRENGTH_4MA		0x1
+#define PADS_DRIVE_STRENGTH_8MA		0x2
+#define PADS_DRIVE_STRENGTH_12MA	0x3
+
+#define GPIO_BANK_BASE(bank)		(0x200 + 0x24 * (bank))
+
+#define GPIO_BIT_EN			0x00
+#define GPIO_OUTPUT_EN			0x04
+#define GPIO_OUTPUT			0x08
+#define GPIO_INPUT			0x0c
+#define GPIO_INPUT_POLARITY		0x10
+#define GPIO_INTERRUPT_TYPE		0x14
+#define GPIO_INTERRUPT_TYPE_LEVEL	0x0
+#define GPIO_INTERRUPT_TYPE_EDGE	0x1
+#define GPIO_INTERRUPT_EDGE		0x18
+#define GPIO_INTERRUPT_EDGE_SINGLE	0x0
+#define GPIO_INTERRUPT_EDGE_DUAL	0x1
+#define GPIO_INTERRUPT_EN		0x1c
+#define GPIO_INTERRUPT_STATUS		0x20
+
+struct pistachio_function {
+	const char *name;
+	const char * const *groups;
+	unsigned int ngroups;
+	const int *scenarios;
+	unsigned int nscenarios;
+	unsigned int scenario_reg;
+	unsigned int scenario_shift;
+	unsigned int scenario_mask;
+};
+
+struct pistachio_pin_group {
+	const char *name;
+	unsigned int pin;
+	int mux_option[3];
+	int mux_reg;
+	int mux_shift;
+	int mux_mask;
+};
+
+struct pistachio_gpio_bank {
+	struct pistachio_pinctrl *pctl;
+	void __iomem *base;
+	unsigned int pin_base;
+	unsigned int npins;
+	struct gpio_chip gpio_chip;
+	struct irq_chip irq_chip;
+};
+
+struct pistachio_pinctrl {
+	struct device *dev;
+	void __iomem *base;
+	struct pinctrl_dev *pctldev;
+	const struct pinctrl_pin_desc *pins;
+	unsigned int npins;
+	const struct pistachio_function *functions;
+	unsigned int nfunctions;
+	const struct pistachio_pin_group *groups;
+	unsigned int ngroups;
+	struct pistachio_gpio_bank *gpio_banks;
+	unsigned int nbanks;
+};
+
+#define PISTACHIO_PIN_MFIO(p)		(p)
+#define PISTACHIO_PIN_TCK		90
+#define PISTACHIO_PIN_TRSTN		91
+#define PISTACHIO_PIN_TDI		92
+#define PISTACHIO_PIN_TMS		93
+#define PISTACHIO_PIN_TDO		94
+#define PISTACHIO_PIN_JTAG_COMPLY	95
+#define PISTACHIO_PIN_SAFE_MODE		96
+#define PISTACHIO_PIN_POR_DISABLE	97
+#define PISTACHIO_PIN_RESETN		98
+
+#define MFIO_PIN_DESC(p)	PINCTRL_PIN(PISTACHIO_PIN_MFIO(p), "mfio" #p)
+
+static const struct pinctrl_pin_desc pistachio_pins[] = {
+	MFIO_PIN_DESC(0),
+	MFIO_PIN_DESC(1),
+	MFIO_PIN_DESC(2),
+	MFIO_PIN_DESC(3),
+	MFIO_PIN_DESC(4),
+	MFIO_PIN_DESC(5),
+	MFIO_PIN_DESC(6),
+	MFIO_PIN_DESC(7),
+	MFIO_PIN_DESC(8),
+	MFIO_PIN_DESC(9),
+	MFIO_PIN_DESC(10),
+	MFIO_PIN_DESC(11),
+	MFIO_PIN_DESC(12),
+	MFIO_PIN_DESC(13),
+	MFIO_PIN_DESC(14),
+	MFIO_PIN_DESC(15),
+	MFIO_PIN_DESC(16),
+	MFIO_PIN_DESC(17),
+	MFIO_PIN_DESC(18),
+	MFIO_PIN_DESC(19),
+	MFIO_PIN_DESC(20),
+	MFIO_PIN_DESC(21),
+	MFIO_PIN_DESC(22),
+	MFIO_PIN_DESC(23),
+	MFIO_PIN_DESC(24),
+	MFIO_PIN_DESC(25),
+	MFIO_PIN_DESC(26),
+	MFIO_PIN_DESC(27),
+	MFIO_PIN_DESC(28),
+	MFIO_PIN_DESC(29),
+	MFIO_PIN_DESC(30),
+	MFIO_PIN_DESC(31),
+	MFIO_PIN_DESC(32),
+	MFIO_PIN_DESC(33),
+	MFIO_PIN_DESC(34),
+	MFIO_PIN_DESC(35),
+	MFIO_PIN_DESC(36),
+	MFIO_PIN_DESC(37),
+	MFIO_PIN_DESC(38),
+	MFIO_PIN_DESC(39),
+	MFIO_PIN_DESC(40),
+	MFIO_PIN_DESC(41),
+	MFIO_PIN_DESC(42),
+	MFIO_PIN_DESC(43),
+	MFIO_PIN_DESC(44),
+	MFIO_PIN_DESC(45),
+	MFIO_PIN_DESC(46),
+	MFIO_PIN_DESC(47),
+	MFIO_PIN_DESC(48),
+	MFIO_PIN_DESC(49),
+	MFIO_PIN_DESC(50),
+	MFIO_PIN_DESC(51),
+	MFIO_PIN_DESC(52),
+	MFIO_PIN_DESC(53),
+	MFIO_PIN_DESC(54),
+	MFIO_PIN_DESC(55),
+	MFIO_PIN_DESC(56),
+	MFIO_PIN_DESC(57),
+	MFIO_PIN_DESC(58),
+	MFIO_PIN_DESC(59),
+	MFIO_PIN_DESC(60),
+	MFIO_PIN_DESC(61),
+	MFIO_PIN_DESC(62),
+	MFIO_PIN_DESC(63),
+	MFIO_PIN_DESC(64),
+	MFIO_PIN_DESC(65),
+	MFIO_PIN_DESC(66),
+	MFIO_PIN_DESC(67),
+	MFIO_PIN_DESC(68),
+	MFIO_PIN_DESC(69),
+	MFIO_PIN_DESC(70),
+	MFIO_PIN_DESC(71),
+	MFIO_PIN_DESC(72),
+	MFIO_PIN_DESC(73),
+	MFIO_PIN_DESC(74),
+	MFIO_PIN_DESC(75),
+	MFIO_PIN_DESC(76),
+	MFIO_PIN_DESC(77),
+	MFIO_PIN_DESC(78),
+	MFIO_PIN_DESC(79),
+	MFIO_PIN_DESC(80),
+	MFIO_PIN_DESC(81),
+	MFIO_PIN_DESC(82),
+	MFIO_PIN_DESC(83),
+	MFIO_PIN_DESC(84),
+	MFIO_PIN_DESC(85),
+	MFIO_PIN_DESC(86),
+	MFIO_PIN_DESC(87),
+	MFIO_PIN_DESC(88),
+	MFIO_PIN_DESC(89),
+	PINCTRL_PIN(PISTACHIO_PIN_TCK, "tck"),
+	PINCTRL_PIN(PISTACHIO_PIN_TRSTN, "trstn"),
+	PINCTRL_PIN(PISTACHIO_PIN_TDI, "tdi"),
+	PINCTRL_PIN(PISTACHIO_PIN_TMS, "tms"),
+	PINCTRL_PIN(PISTACHIO_PIN_TDO, "tdo"),
+	PINCTRL_PIN(PISTACHIO_PIN_JTAG_COMPLY, "jtag_comply"),
+	PINCTRL_PIN(PISTACHIO_PIN_SAFE_MODE, "safe_mode"),
+	PINCTRL_PIN(PISTACHIO_PIN_POR_DISABLE, "por_disable"),
+	PINCTRL_PIN(PISTACHIO_PIN_RESETN, "resetn"),
+};
+
+static const char * const pistachio_spim0_groups[] = {
+	"mfio1", "mfio2", "mfio8", "mfio9", "mfio10", "mfio28", "mfio29",
+	"mfio30", "mfio55", "mfio56", "mfio57",
+};
+
+static const char * const pistachio_spim1_groups[] = {
+	"mfio0", "mfio1", "mfio2", "mfio3", "mfio4", "mfio5", "mfio6",
+	"mfio7", "mfio31", "mfio55", "mfio56", "mfio57", "mfio58",
+};
+
+static const char * const pistachio_spis_groups[] = {
+	"mfio11", "mfio12", "mfio13", "mfio14",
+};
+
+static const char *const pistachio_sdhost_groups[] = {
+	"mfio15", "mfio16", "mfio17", "mfio18", "mfio19", "mfio20",
+	"mfio21", "mfio22", "mfio23", "mfio24", "mfio25", "mfio26",
+	"mfio27",
+};
+
+static const char * const pistachio_i2c0_groups[] = {
+	"mfio28", "mfio29",
+};
+
+static const char * const pistachio_i2c1_groups[] = {
+	"mfio30", "mfio31",
+};
+
+static const char * const pistachio_i2c2_groups[] = {
+	"mfio32", "mfio33",
+};
+
+static const char * const pistachio_i2c3_groups[] = {
+	"mfio34", "mfio35",
+};
+
+static const char * const pistachio_audio_clk_in_groups[] = {
+	"mfio36",
+};
+
+static const char * const pistachio_i2s_out_groups[] = {
+	"mfio36", "mfio37", "mfio38", "mfio39", "mfio40", "mfio41",
+	"mfio42", "mfio43", "mfio44",
+};
+
+static const char * const pistachio_debug_raw_cca_ind_groups[] = {
+	"mfio37",
+};
+
+static const char * const pistachio_debug_ed_sec20_cca_ind_groups[] = {
+	"mfio38",
+};
+
+static const char * const pistachio_debug_ed_sec40_cca_ind_groups[] = {
+	"mfio39",
+};
+
+static const char * const pistachio_debug_agc_done_0_groups[] = {
+	"mfio40",
+};
+
+static const char * const pistachio_debug_agc_done_1_groups[] = {
+	"mfio41",
+};
+
+static const char * const pistachio_debug_ed_cca_ind_groups[] = {
+	"mfio42",
+};
+
+static const char * const pistachio_debug_s2l_done_groups[] = {
+	"mfio43",
+};
+
+static const char * const pistachio_i2s_dac_clk_groups[] = {
+	"mfio45",
+};
+
+static const char * const pistachio_audio_sync_groups[] = {
+	"mfio45",
+};
+
+static const char * const pistachio_audio_trigger_groups[] = {
+	"mfio46",
+};
+
+static const char * const pistachio_i2s_in_groups[] = {
+	"mfio47", "mfio48", "mfio49", "mfio50", "mfio51", "mfio52",
+	"mfio53", "mfio54",
+};
+
+static const char * const pistachio_uart0_groups[] = {
+	"mfio55", "mfio56", "mfio57", "mfio58",
+};
+
+static const char * const pistachio_uart1_groups[] = {
+	"mfio59", "mfio60", "mfio1", "mfio2",
+};
+
+static const char * const pistachio_spdif_out_groups[] = {
+	"mfio61",
+};
+
+static const char * const pistachio_spdif_in_groups[] = {
+	"mfio62", "mfio54",
+};
+static const int pistachio_spdif_in_scenarios[] = {
+	PISTACHIO_PIN_MFIO(62),
+	PISTACHIO_PIN_MFIO(54),
+};
+
+static const char * const pistachio_eth_groups[] = {
+	"mfio63", "mfio64", "mfio65", "mfio66", "mfio67", "mfio68",
+	"mfio69", "mfio70", "mfio71",
+};
+
+static const char * const pistachio_ir_groups[] = {
+	"mfio72",
+};
+
+static const char * const pistachio_pwmpdm_groups[] = {
+	"mfio73", "mfio74", "mfio75", "mfio76",
+};
+
+static const char * const pistachio_mips_trace_clk_groups[] = {
+	"mfio15", "mfio63", "mfio73",
+};
+
+static const char * const pistachio_mips_trace_dint_groups[] = {
+	"mfio16", "mfio64", "mfio74",
+};
+static const int pistachio_mips_trace_dint_scenarios[] = {
+	PISTACHIO_PIN_MFIO(16),
+	PISTACHIO_PIN_MFIO(64),
+	PISTACHIO_PIN_MFIO(74),
+};
+
+static const char * const pistachio_mips_trace_trigout_groups[] = {
+	"mfio17", "mfio65", "mfio75",
+};
+
+static const char * const pistachio_mips_trace_trigin_groups[] = {
+	"mfio18", "mfio66", "mfio76",
+};
+static const int pistachio_mips_trace_trigin_scenarios[] = {
+	PISTACHIO_PIN_MFIO(18),
+	PISTACHIO_PIN_MFIO(66),
+	PISTACHIO_PIN_MFIO(76),
+};
+
+static const char * const pistachio_mips_trace_dm_groups[] = {
+	"mfio19", "mfio67", "mfio77",
+};
+
+static const char * const pistachio_mips_probe_n_groups[] = {
+	"mfio20", "mfio68", "mfio78",
+};
+static const int pistachio_mips_probe_n_scenarios[] = {
+	PISTACHIO_PIN_MFIO(20),
+	PISTACHIO_PIN_MFIO(68),
+	PISTACHIO_PIN_MFIO(78),
+};
+
+static const char * const pistachio_mips_trace_data_groups[] = {
+	"mfio15", "mfio16", "mfio17", "mfio18", "mfio19", "mfio20",
+	"mfio21", "mfio22", "mfio63", "mfio64", "mfio65", "mfio66",
+	"mfio67", "mfio68", "mfio69", "mfio70", "mfio79", "mfio80",
+	"mfio81", "mfio82", "mfio83", "mfio84", "mfio85", "mfio86",
+};
+
+static const char * const pistachio_sram_debug_groups[] = {
+	"mfio73", "mfio74",
+};
+
+static const char * const pistachio_rom_debug_groups[] = {
+	"mfio75", "mfio76",
+};
+
+static const char * const pistachio_rpu_debug_groups[] = {
+	"mfio77", "mfio78",
+};
+
+static const char * const pistachio_mips_debug_groups[] = {
+	"mfio79", "mfio80",
+};
+
+static const char * const pistachio_eth_debug_groups[] = {
+	"mfio81", "mfio82",
+};
+
+static const char * const pistachio_usb_debug_groups[] = {
+	"mfio83", "mfio84",
+};
+
+static const char * const pistachio_sdhost_debug_groups[] = {
+	"mfio85", "mfio86",
+};
+
+static const char * const pistachio_socif_debug_groups[] = {
+	"mfio87", "mfio88",
+};
+
+static const char * const pistachio_mdc_debug_groups[] = {
+	"mfio77", "mfio78",
+};
+
+static const char * const pistachio_ddr_debug_groups[] = {
+	"mfio79", "mfio80",
+};
+
+static const char * const pistachio_dreq0_groups[] = {
+	"mfio81",
+};
+
+static const char * const pistachio_dreq1_groups[] = {
+	"mfio82",
+};
+
+static const char * const pistachio_dreq2_groups[] = {
+	"mfio87",
+};
+
+static const char * const pistachio_dreq3_groups[] = {
+	"mfio88",
+};
+
+static const char * const pistachio_dreq4_groups[] = {
+	"mfio89",
+};
+
+static const char * const pistachio_dreq5_groups[] = {
+	"mfio89",
+};
+
+static const char * const pistachio_mips_pll_lock_groups[] = {
+	"mfio83",
+};
+
+static const char * const pistachio_sys_pll_lock_groups[] = {
+	"mfio84",
+};
+
+static const char * const pistachio_wifi_pll_lock_groups[] = {
+	"mfio85",
+};
+
+static const char * const pistachio_bt_pll_lock_groups[] = {
+	"mfio86",
+};
+
+static const char * const pistachio_rpu_v_pll_lock_groups[] = {
+	"mfio87",
+};
+
+static const char * const pistachio_rpu_l_pll_lock_groups[] = {
+	"mfio88",
+};
+
+static const char * const pistachio_audio_pll_lock_groups[] = {
+	"mfio89",
+};
+
+#define FUNCTION(_name)							\
+	{								\
+		.name = #_name,						\
+		.groups = pistachio_##_name##_groups,			\
+		.ngroups = ARRAY_SIZE(pistachio_##_name##_groups),	\
+	}
+
+#define FUNCTION_SCENARIO(_name, _reg, _shift, _mask)			\
+	{								\
+		.name = #_name,						\
+		.groups = pistachio_##_name##_groups,			\
+		.ngroups = ARRAY_SIZE(pistachio_##_name##_groups),	\
+		.scenarios = pistachio_##_name##_scenarios,		\
+		.nscenarios = ARRAY_SIZE(pistachio_##_name##_scenarios),\
+		.scenario_reg = _reg,					\
+		.scenario_shift = _shift,				\
+		.scenario_mask = _mask,					\
+	}
+
+enum pistachio_mux_option {
+	PISTACHIO_FUNCTION_NONE = -1,
+	PISTACHIO_FUNCTION_SPIM0,
+	PISTACHIO_FUNCTION_SPIM1,
+	PISTACHIO_FUNCTION_SPIS,
+	PISTACHIO_FUNCTION_SDHOST,
+	PISTACHIO_FUNCTION_I2C0,
+	PISTACHIO_FUNCTION_I2C1,
+	PISTACHIO_FUNCTION_I2C2,
+	PISTACHIO_FUNCTION_I2C3,
+	PISTACHIO_FUNCTION_AUDIO_CLK_IN,
+	PISTACHIO_FUNCTION_I2S_OUT,
+	PISTACHIO_FUNCTION_I2S_DAC_CLK,
+	PISTACHIO_FUNCTION_AUDIO_SYNC,
+	PISTACHIO_FUNCTION_AUDIO_TRIGGER,
+	PISTACHIO_FUNCTION_I2S_IN,
+	PISTACHIO_FUNCTION_UART0,
+	PISTACHIO_FUNCTION_UART1,
+	PISTACHIO_FUNCTION_SPDIF_OUT,
+	PISTACHIO_FUNCTION_SPDIF_IN,
+	PISTACHIO_FUNCTION_ETH,
+	PISTACHIO_FUNCTION_IR,
+	PISTACHIO_FUNCTION_PWMPDM,
+	PISTACHIO_FUNCTION_MIPS_TRACE_CLK,
+	PISTACHIO_FUNCTION_MIPS_TRACE_DINT,
+	PISTACHIO_FUNCTION_MIPS_TRACE_TRIGOUT,
+	PISTACHIO_FUNCTION_MIPS_TRACE_TRIGIN,
+	PISTACHIO_FUNCTION_MIPS_TRACE_DM,
+	PISTACHIO_FUNCTION_MIPS_TRACE_PROBE_N,
+	PISTACHIO_FUNCTION_MIPS_TRACE_DATA,
+	PISTACHIO_FUNCTION_SRAM_DEBUG,
+	PISTACHIO_FUNCTION_ROM_DEBUG,
+	PISTACHIO_FUNCTION_RPU_DEBUG,
+	PISTACHIO_FUNCTION_MIPS_DEBUG,
+	PISTACHIO_FUNCTION_ETH_DEBUG,
+	PISTACHIO_FUNCTION_USB_DEBUG,
+	PISTACHIO_FUNCTION_SDHOST_DEBUG,
+	PISTACHIO_FUNCTION_SOCIF_DEBUG,
+	PISTACHIO_FUNCTION_MDC_DEBUG,
+	PISTACHIO_FUNCTION_DDR_DEBUG,
+	PISTACHIO_FUNCTION_DREQ0,
+	PISTACHIO_FUNCTION_DREQ1,
+	PISTACHIO_FUNCTION_DREQ2,
+	PISTACHIO_FUNCTION_DREQ3,
+	PISTACHIO_FUNCTION_DREQ4,
+	PISTACHIO_FUNCTION_DREQ5,
+	PISTACHIO_FUNCTION_MIPS_PLL_LOCK,
+	PISTACHIO_FUNCTION_SYS_PLL_LOCK,
+	PISTACHIO_FUNCTION_WIFI_PLL_LOCK,
+	PISTACHIO_FUNCTION_BT_PLL_LOCK,
+	PISTACHIO_FUNCTION_RPU_V_PLL_LOCK,
+	PISTACHIO_FUNCTION_RPU_L_PLL_LOCK,
+	PISTACHIO_FUNCTION_AUDIO_PLL_LOCK,
+	PISTACHIO_FUNCTION_DEBUG_RAW_CCA_IND,
+	PISTACHIO_FUNCTION_DEBUG_ED_SEC20_CCA_IND,
+	PISTACHIO_FUNCTION_DEBUG_ED_SEC40_CCA_IND,
+	PISTACHIO_FUNCTION_DEBUG_AGC_DONE_0,
+	PISTACHIO_FUNCTION_DEBUG_AGC_DONE_1,
+	PISTACHIO_FUNCTION_DEBUG_ED_CCA_IND,
+	PISTACHIO_FUNCTION_DEBUG_S2L_DONE,
+};
+
+static const struct pistachio_function pistachio_functions[] = {
+	FUNCTION(spim0),
+	FUNCTION(spim1),
+	FUNCTION(spis),
+	FUNCTION(sdhost),
+	FUNCTION(i2c0),
+	FUNCTION(i2c1),
+	FUNCTION(i2c2),
+	FUNCTION(i2c3),
+	FUNCTION(audio_clk_in),
+	FUNCTION(i2s_out),
+	FUNCTION(i2s_dac_clk),
+	FUNCTION(audio_sync),
+	FUNCTION(audio_trigger),
+	FUNCTION(i2s_in),
+	FUNCTION(uart0),
+	FUNCTION(uart1),
+	FUNCTION(spdif_out),
+	FUNCTION_SCENARIO(spdif_in, PADS_SCENARIO_SELECT, 0, 0x1),
+	FUNCTION(eth),
+	FUNCTION(ir),
+	FUNCTION(pwmpdm),
+	FUNCTION(mips_trace_clk),
+	FUNCTION_SCENARIO(mips_trace_dint, PADS_SCENARIO_SELECT, 1, 0x3),
+	FUNCTION(mips_trace_trigout),
+	FUNCTION_SCENARIO(mips_trace_trigin, PADS_SCENARIO_SELECT, 3, 0x3),
+	FUNCTION(mips_trace_dm),
+	FUNCTION_SCENARIO(mips_probe_n, PADS_SCENARIO_SELECT, 5, 0x3),
+	FUNCTION(mips_trace_data),
+	FUNCTION(sram_debug),
+	FUNCTION(rom_debug),
+	FUNCTION(rpu_debug),
+	FUNCTION(mips_debug),
+	FUNCTION(eth_debug),
+	FUNCTION(usb_debug),
+	FUNCTION(sdhost_debug),
+	FUNCTION(socif_debug),
+	FUNCTION(mdc_debug),
+	FUNCTION(ddr_debug),
+	FUNCTION(dreq0),
+	FUNCTION(dreq1),
+	FUNCTION(dreq2),
+	FUNCTION(dreq3),
+	FUNCTION(dreq4),
+	FUNCTION(dreq5),
+	FUNCTION(mips_pll_lock),
+	FUNCTION(sys_pll_lock),
+	FUNCTION(wifi_pll_lock),
+	FUNCTION(bt_pll_lock),
+	FUNCTION(rpu_v_pll_lock),
+	FUNCTION(rpu_l_pll_lock),
+	FUNCTION(audio_pll_lock),
+	FUNCTION(debug_raw_cca_ind),
+	FUNCTION(debug_ed_sec20_cca_ind),
+	FUNCTION(debug_ed_sec40_cca_ind),
+	FUNCTION(debug_agc_done_0),
+	FUNCTION(debug_agc_done_1),
+	FUNCTION(debug_ed_cca_ind),
+	FUNCTION(debug_s2l_done),
+};
+
+#define PIN_GROUP(_pin, _name)					\
+	{							\
+		.name = #_name,					\
+		.pin = PISTACHIO_PIN_##_pin,			\
+		.mux_option = {					\
+			PISTACHIO_FUNCTION_NONE,		\
+			PISTACHIO_FUNCTION_NONE,		\
+			PISTACHIO_FUNCTION_NONE,		\
+		},						\
+		.mux_reg = -1,					\
+		.mux_shift = -1,				\
+		.mux_mask = -1,					\
+	}
+
+#define MFIO_PIN_GROUP(_pin, _func)				\
+	{							\
+		.name = "mfio" #_pin,				\
+		.pin = PISTACHIO_PIN_MFIO(_pin),		\
+		.mux_option = {					\
+			PISTACHIO_FUNCTION_##_func,		\
+			PISTACHIO_FUNCTION_NONE,		\
+			PISTACHIO_FUNCTION_NONE,		\
+		},						\
+		.mux_reg = -1,					\
+		.mux_shift = -1,				\
+		.mux_mask = -1,					\
+	}
+
+#define MFIO_MUX_PIN_GROUP(_pin, _f0, _f1, _f2, _reg, _shift, _mask)	\
+	{								\
+		.name = "mfio" #_pin,					\
+		.pin = PISTACHIO_PIN_MFIO(_pin),			\
+		.mux_option = {						\
+			PISTACHIO_FUNCTION_##_f0,			\
+			PISTACHIO_FUNCTION_##_f1,			\
+			PISTACHIO_FUNCTION_##_f2,			\
+		},							\
+		.mux_reg = _reg,					\
+		.mux_shift = _shift,					\
+		.mux_mask = _mask,					\
+	}
+
+static const struct pistachio_pin_group pistachio_groups[] = {
+	MFIO_PIN_GROUP(0, SPIM1),
+	MFIO_MUX_PIN_GROUP(1, SPIM1, SPIM0, UART1,
+			   PADS_FUNCTION_SELECT0, 0, 0x3),
+	MFIO_MUX_PIN_GROUP(2, SPIM1, SPIM0, UART1,
+			   PADS_FUNCTION_SELECT0, 2, 0x3),
+	MFIO_PIN_GROUP(3, SPIM1),
+	MFIO_PIN_GROUP(4, SPIM1),
+	MFIO_PIN_GROUP(5, SPIM1),
+	MFIO_PIN_GROUP(6, SPIM1),
+	MFIO_PIN_GROUP(7, SPIM1),
+	MFIO_PIN_GROUP(8, SPIM0),
+	MFIO_PIN_GROUP(9, SPIM0),
+	MFIO_PIN_GROUP(10, SPIM0),
+	MFIO_PIN_GROUP(11, SPIS),
+	MFIO_PIN_GROUP(12, SPIS),
+	MFIO_PIN_GROUP(13, SPIS),
+	MFIO_PIN_GROUP(14, SPIS),
+	MFIO_MUX_PIN_GROUP(15, SDHOST, MIPS_TRACE_CLK, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT0, 4, 0x3),
+	MFIO_MUX_PIN_GROUP(16, SDHOST, MIPS_TRACE_DINT, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT0, 6, 0x3),
+	MFIO_MUX_PIN_GROUP(17, SDHOST, MIPS_TRACE_TRIGOUT, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT0, 8, 0x3),
+	MFIO_MUX_PIN_GROUP(18, SDHOST, MIPS_TRACE_TRIGIN, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT0, 10, 0x3),
+	MFIO_MUX_PIN_GROUP(19, SDHOST, MIPS_TRACE_DM, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT0, 12, 0x3),
+	MFIO_MUX_PIN_GROUP(20, SDHOST, MIPS_TRACE_PROBE_N, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT0, 14, 0x3),
+	MFIO_MUX_PIN_GROUP(21, SDHOST, NONE, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT0, 16, 0x3),
+	MFIO_MUX_PIN_GROUP(22, SDHOST, NONE, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT0, 18, 0x3),
+	MFIO_PIN_GROUP(23, SDHOST),
+	MFIO_PIN_GROUP(24, SDHOST),
+	MFIO_PIN_GROUP(25, SDHOST),
+	MFIO_PIN_GROUP(26, SDHOST),
+	MFIO_PIN_GROUP(27, SDHOST),
+	MFIO_MUX_PIN_GROUP(28, I2C0, SPIM0, NONE,
+			   PADS_FUNCTION_SELECT0, 20, 0x1),
+	MFIO_MUX_PIN_GROUP(29, I2C0, SPIM0, NONE,
+			   PADS_FUNCTION_SELECT0, 21, 0x1),
+	MFIO_MUX_PIN_GROUP(30, I2C1, SPIM0, NONE,
+			   PADS_FUNCTION_SELECT0, 22, 0x1),
+	MFIO_MUX_PIN_GROUP(31, I2C1, SPIM1, NONE,
+			   PADS_FUNCTION_SELECT0, 23, 0x1),
+	MFIO_PIN_GROUP(32, I2C2),
+	MFIO_PIN_GROUP(33, I2C2),
+	MFIO_PIN_GROUP(34, I2C3),
+	MFIO_PIN_GROUP(35, I2C3),
+	MFIO_MUX_PIN_GROUP(36, I2S_OUT, AUDIO_CLK_IN, NONE,
+			   PADS_FUNCTION_SELECT0, 24, 0x1),
+	MFIO_MUX_PIN_GROUP(37, I2S_OUT, DEBUG_RAW_CCA_IND, NONE,
+			   PADS_FUNCTION_SELECT0, 25, 0x1),
+	MFIO_MUX_PIN_GROUP(38, I2S_OUT, DEBUG_ED_SEC20_CCA_IND, NONE,
+			   PADS_FUNCTION_SELECT0, 26, 0x1),
+	MFIO_MUX_PIN_GROUP(39, I2S_OUT, DEBUG_ED_SEC40_CCA_IND, NONE,
+			   PADS_FUNCTION_SELECT0, 27, 0x1),
+	MFIO_MUX_PIN_GROUP(40, I2S_OUT, DEBUG_AGC_DONE_0, NONE,
+			   PADS_FUNCTION_SELECT0, 28, 0x1),
+	MFIO_MUX_PIN_GROUP(41, I2S_OUT, DEBUG_AGC_DONE_1, NONE,
+			   PADS_FUNCTION_SELECT0, 29, 0x1),
+	MFIO_MUX_PIN_GROUP(42, I2S_OUT, DEBUG_ED_CCA_IND, NONE,
+			   PADS_FUNCTION_SELECT0, 30, 0x1),
+	MFIO_MUX_PIN_GROUP(43, I2S_OUT, DEBUG_S2L_DONE, NONE,
+			   PADS_FUNCTION_SELECT0, 31, 0x1),
+	MFIO_PIN_GROUP(44, I2S_OUT),
+	MFIO_MUX_PIN_GROUP(45, I2S_DAC_CLK, AUDIO_SYNC, NONE,
+			   PADS_FUNCTION_SELECT1, 0, 0x1),
+	MFIO_PIN_GROUP(46, AUDIO_TRIGGER),
+	MFIO_PIN_GROUP(47, I2S_IN),
+	MFIO_PIN_GROUP(48, I2S_IN),
+	MFIO_PIN_GROUP(49, I2S_IN),
+	MFIO_PIN_GROUP(50, I2S_IN),
+	MFIO_PIN_GROUP(51, I2S_IN),
+	MFIO_PIN_GROUP(52, I2S_IN),
+	MFIO_PIN_GROUP(53, I2S_IN),
+	MFIO_MUX_PIN_GROUP(54, I2S_IN, NONE, SPDIF_IN,
+			   PADS_FUNCTION_SELECT1, 1, 0x3),
+	MFIO_MUX_PIN_GROUP(55, UART0, SPIM0, SPIM1,
+			   PADS_FUNCTION_SELECT1, 3, 0x3),
+	MFIO_MUX_PIN_GROUP(56, UART0, SPIM0, SPIM1,
+			   PADS_FUNCTION_SELECT1, 5, 0x3),
+	MFIO_MUX_PIN_GROUP(57, UART0, SPIM0, SPIM1,
+			   PADS_FUNCTION_SELECT1, 7, 0x3),
+	MFIO_MUX_PIN_GROUP(58, UART0, SPIM1, NONE,
+			   PADS_FUNCTION_SELECT1, 9, 0x1),
+	MFIO_PIN_GROUP(59, UART1),
+	MFIO_PIN_GROUP(60, UART1),
+	MFIO_PIN_GROUP(61, SPDIF_OUT),
+	MFIO_PIN_GROUP(62, SPDIF_IN),
+	MFIO_MUX_PIN_GROUP(63, ETH, MIPS_TRACE_CLK, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT1, 10, 0x3),
+	MFIO_MUX_PIN_GROUP(64, ETH, MIPS_TRACE_DINT, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT1, 12, 0x3),
+	MFIO_MUX_PIN_GROUP(65, ETH, MIPS_TRACE_TRIGOUT, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT1, 14, 0x3),
+	MFIO_MUX_PIN_GROUP(66, ETH, MIPS_TRACE_TRIGIN, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT1, 16, 0x3),
+	MFIO_MUX_PIN_GROUP(67, ETH, MIPS_TRACE_DM, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT1, 18, 0x3),
+	MFIO_MUX_PIN_GROUP(68, ETH, MIPS_TRACE_PROBE_N, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT1, 20, 0x3),
+	MFIO_MUX_PIN_GROUP(69, ETH, NONE, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT1, 22, 0x3),
+	MFIO_MUX_PIN_GROUP(70, ETH, NONE, MIPS_TRACE_DATA,
+			   PADS_FUNCTION_SELECT1, 24, 0x3),
+	MFIO_PIN_GROUP(71, ETH),
+	MFIO_PIN_GROUP(72, IR),
+	MFIO_MUX_PIN_GROUP(73, PWMPDM, MIPS_TRACE_CLK, SRAM_DEBUG,
+			   PADS_FUNCTION_SELECT1, 26, 0x3),
+	MFIO_MUX_PIN_GROUP(74, PWMPDM, MIPS_TRACE_DINT, SRAM_DEBUG,
+			   PADS_FUNCTION_SELECT1, 28, 0x3),
+	MFIO_MUX_PIN_GROUP(75, PWMPDM, MIPS_TRACE_TRIGOUT, ROM_DEBUG,
+			   PADS_FUNCTION_SELECT1, 30, 0x3),
+	MFIO_MUX_PIN_GROUP(76, PWMPDM, MIPS_TRACE_TRIGIN, ROM_DEBUG,
+			   PADS_FUNCTION_SELECT2, 0, 0x3),
+	MFIO_MUX_PIN_GROUP(77, MDC_DEBUG, MIPS_TRACE_DM, RPU_DEBUG,
+			   PADS_FUNCTION_SELECT2, 2, 0x3),
+	MFIO_MUX_PIN_GROUP(78, MDC_DEBUG, MIPS_TRACE_PROBE_N, RPU_DEBUG,
+			   PADS_FUNCTION_SELECT2, 4, 0x3),
+	MFIO_MUX_PIN_GROUP(79, DDR_DEBUG, MIPS_TRACE_DATA, MIPS_DEBUG,
+			   PADS_FUNCTION_SELECT2, 6, 0x3),
+	MFIO_MUX_PIN_GROUP(80, DDR_DEBUG, MIPS_TRACE_DATA, MIPS_DEBUG,
+			   PADS_FUNCTION_SELECT2, 8, 0x3),
+	MFIO_MUX_PIN_GROUP(81, DREQ0, MIPS_TRACE_DATA, ETH_DEBUG,
+			   PADS_FUNCTION_SELECT2, 10, 0x3),
+	MFIO_MUX_PIN_GROUP(82, DREQ1, MIPS_TRACE_DATA, ETH_DEBUG,
+			   PADS_FUNCTION_SELECT2, 12, 0x3),
+	MFIO_MUX_PIN_GROUP(83, MIPS_PLL_LOCK, MIPS_TRACE_DATA, USB_DEBUG,
+			   PADS_FUNCTION_SELECT2, 14, 0x3),
+	MFIO_MUX_PIN_GROUP(84, SYS_PLL_LOCK, MIPS_TRACE_DATA, USB_DEBUG,
+			   PADS_FUNCTION_SELECT2, 16, 0x3),
+	MFIO_MUX_PIN_GROUP(85, WIFI_PLL_LOCK, MIPS_TRACE_DATA, SDHOST_DEBUG,
+			   PADS_FUNCTION_SELECT2, 18, 0x3),
+	MFIO_MUX_PIN_GROUP(86, BT_PLL_LOCK, MIPS_TRACE_DATA, SDHOST_DEBUG,
+			   PADS_FUNCTION_SELECT2, 20, 0x3),
+	MFIO_MUX_PIN_GROUP(87, RPU_V_PLL_LOCK, DREQ2, SOCIF_DEBUG,
+			   PADS_FUNCTION_SELECT2, 22, 0x3),
+	MFIO_MUX_PIN_GROUP(88, RPU_L_PLL_LOCK, DREQ3, SOCIF_DEBUG,
+			   PADS_FUNCTION_SELECT2, 24, 0x3),
+	MFIO_MUX_PIN_GROUP(89, AUDIO_PLL_LOCK, DREQ4, DREQ5,
+			   PADS_FUNCTION_SELECT2, 26, 0x3),
+	PIN_GROUP(TCK, "tck"),
+	PIN_GROUP(TRSTN, "trstn"),
+	PIN_GROUP(TDI, "tdi"),
+	PIN_GROUP(TMS, "tms"),
+	PIN_GROUP(TDO, "tdo"),
+	PIN_GROUP(JTAG_COMPLY, "jtag_comply"),
+	PIN_GROUP(SAFE_MODE, "safe_mode"),
+	PIN_GROUP(POR_DISABLE, "por_disable"),
+	PIN_GROUP(RESETN, "resetn"),
+};
+
+static inline u32 pctl_readl(struct pistachio_pinctrl *pctl, u32 reg)
+{
+	return readl(pctl->base + reg);
+}
+
+static inline void pctl_writel(struct pistachio_pinctrl *pctl, u32 val, u32 reg)
+{
+	writel(val, pctl->base + reg);
+}
+
+static inline struct pistachio_gpio_bank *gc_to_bank(struct gpio_chip *gc)
+{
+	return container_of(gc, struct pistachio_gpio_bank, gpio_chip);
+}
+
+static inline struct pistachio_gpio_bank *irqd_to_bank(struct irq_data *d)
+{
+	return gc_to_bank(irq_data_get_irq_chip_data(d));
+}
+
+static inline u32 gpio_readl(struct pistachio_gpio_bank *bank, u32 reg)
+{
+	return readl(bank->base + reg);
+}
+
+static inline void gpio_writel(struct pistachio_gpio_bank *bank, u32 val,
+			       u32 reg)
+{
+	writel(val, bank->base + reg);
+}
+
+static inline void gpio_mask_writel(struct pistachio_gpio_bank *bank,
+				    u32 reg, unsigned int bit, u32 val)
+{
+	/*
+	 * For most of the GPIO registers, bit 16 + X must be set in order to
+	 * write bit X.
+	 */
+	gpio_writel(bank, (0x10000 | val) << bit, reg);
+}
+
+static inline void gpio_enable(struct pistachio_gpio_bank *bank,
+			       unsigned offset)
+{
+	gpio_mask_writel(bank, GPIO_BIT_EN, offset, 1);
+}
+
+static inline void gpio_disable(struct pistachio_gpio_bank *bank,
+				unsigned offset)
+{
+	gpio_mask_writel(bank, GPIO_BIT_EN, offset, 0);
+}
+
+static int pistachio_pinctrl_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctl->ngroups;
+}
+
+static const char *pistachio_pinctrl_get_group_name(struct pinctrl_dev *pctldev,
+						    unsigned group)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctl->groups[group].name;
+}
+
+static int pistachio_pinctrl_get_group_pins(struct pinctrl_dev *pctldev,
+					    unsigned group,
+					    const unsigned **pins,
+					    unsigned *num_pins)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	*pins = &pctl->groups[group].pin;
+	*num_pins = 1;
+
+	return 0;
+}
+
+static const struct pinctrl_ops pistachio_pinctrl_ops = {
+	.get_groups_count = pistachio_pinctrl_get_groups_count,
+	.get_group_name = pistachio_pinctrl_get_group_name,
+	.get_group_pins = pistachio_pinctrl_get_group_pins,
+	.dt_node_to_map = pinconf_generic_dt_node_to_map_pin,
+	.dt_free_map = pinctrl_utils_dt_free_map,
+};
+
+static int pistachio_pinmux_get_functions_count(struct pinctrl_dev *pctldev)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctl->nfunctions;
+}
+
+static const char *
+pistachio_pinmux_get_function_name(struct pinctrl_dev *pctldev, unsigned func)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctl->functions[func].name;
+}
+
+static int pistachio_pinmux_get_function_groups(struct pinctrl_dev *pctldev,
+						unsigned func,
+						const char * const **groups,
+						unsigned * const num_groups)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = pctl->functions[func].groups;
+	*num_groups = pctl->functions[func].ngroups;
+
+	return 0;
+}
+
+static int pistachio_pinmux_enable(struct pinctrl_dev *pctldev,
+				   unsigned func, unsigned group)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	const struct pistachio_pin_group *pg = &pctl->groups[group];
+	const struct pistachio_function *pf = &pctl->functions[func];
+	struct pinctrl_gpio_range *range;
+	unsigned int i;
+	u32 val;
+
+	if (pg->mux_reg > 0) {
+		for (i = 0; i < ARRAY_SIZE(pg->mux_option); i++) {
+			if (pg->mux_option[i] == func)
+				break;
+		}
+		if (i == ARRAY_SIZE(pg->mux_option)) {
+			dev_err(pctl->dev, "Cannot mux pin %u to function %u\n",
+				group, func);
+			return -EINVAL;
+		}
+
+		val = pctl_readl(pctl, pg->mux_reg);
+		val &= ~(pg->mux_mask << pg->mux_shift);
+		val |= i << pg->mux_shift;
+		pctl_writel(pctl, val, pg->mux_reg);
+
+		if (pf->scenarios) {
+			for (i = 0; i < pf->nscenarios; i++) {
+				if (pf->scenarios[i] == group)
+					break;
+			}
+			if (WARN_ON(i == pf->nscenarios))
+				return -EINVAL;
+
+			val = pctl_readl(pctl, pf->scenario_reg);
+			val &= ~(pf->scenario_mask << pf->scenario_shift);
+			val |= i << pf->scenario_shift;
+			pctl_writel(pctl, val, pf->scenario_reg);
+		}
+	}
+
+	range = pinctrl_find_gpio_range_from_pin(pctl->pctldev, pg->pin);
+	if (range)
+		gpio_disable(gc_to_bank(range->gc), pg->pin - range->pin_base);
+
+	return 0;
+}
+
+static const struct pinmux_ops pistachio_pinmux_ops = {
+	.get_functions_count = pistachio_pinmux_get_functions_count,
+	.get_function_name = pistachio_pinmux_get_function_name,
+	.get_function_groups = pistachio_pinmux_get_function_groups,
+	.set_mux = pistachio_pinmux_enable,
+};
+
+static int pistachio_pinconf_get(struct pinctrl_dev *pctldev, unsigned pin,
+				 unsigned long *config)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	u32 val, arg;
+
+	switch (param) {
+	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+		val = pctl_readl(pctl, PADS_SCHMITT_EN_REG(pin));
+		arg = !!(val & PADS_SCHMITT_EN_BIT(pin));
+		break;
+	case PIN_CONFIG_BIAS_HIGH_IMPEDANCE:
+		val = pctl_readl(pctl, PADS_PU_PD_REG(pin)) >>
+			PADS_PU_PD_SHIFT(pin);
+		arg = (val & PADS_PU_PD_MASK) == PADS_PU_PD_HIGHZ;
+		break;
+	case PIN_CONFIG_BIAS_PULL_UP:
+		val = pctl_readl(pctl, PADS_PU_PD_REG(pin)) >>
+			PADS_PU_PD_SHIFT(pin);
+		arg = (val & PADS_PU_PD_MASK) == PADS_PU_PD_UP;
+		break;
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		val = pctl_readl(pctl, PADS_PU_PD_REG(pin)) >>
+			PADS_PU_PD_SHIFT(pin);
+		arg = (val & PADS_PU_PD_MASK) == PADS_PU_PD_DOWN;
+		break;
+	case PIN_CONFIG_BIAS_BUS_HOLD:
+		val = pctl_readl(pctl, PADS_PU_PD_REG(pin)) >>
+			PADS_PU_PD_SHIFT(pin);
+		arg = (val & PADS_PU_PD_MASK) == PADS_PU_PD_BUS;
+		break;
+	case PIN_CONFIG_SLEW_RATE:
+		val = pctl_readl(pctl, PADS_SLEW_RATE_REG(pin));
+		arg = !!(val & PADS_SLEW_RATE_BIT(pin));
+		break;
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		val = pctl_readl(pctl, PADS_DRIVE_STRENGTH_REG(pin)) >>
+			PADS_DRIVE_STRENGTH_SHIFT(pin);
+		switch (val & PADS_DRIVE_STRENGTH_MASK) {
+		case PADS_DRIVE_STRENGTH_2MA:
+			arg = 2;
+			break;
+		case PADS_DRIVE_STRENGTH_4MA:
+			arg = 4;
+			break;
+		case PADS_DRIVE_STRENGTH_8MA:
+			arg = 8;
+			break;
+		case PADS_DRIVE_STRENGTH_12MA:
+		default:
+			arg = 12;
+			break;
+		}
+		break;
+	default:
+		dev_dbg(pctl->dev, "Property %u not supported\n", param);
+		return -ENOTSUPP;
+	}
+
+	*config = pinconf_to_config_packed(param, arg);
+
+	return 0;
+}
+
+static int pistachio_pinconf_set(struct pinctrl_dev *pctldev, unsigned pin,
+				 unsigned long *configs, unsigned num_configs)
+{
+	struct pistachio_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param;
+	u32 drv, val, arg;
+	unsigned int i;
+
+	for (i = 0; i < num_configs; i++) {
+		param = pinconf_to_config_param(configs[i]);
+		arg = pinconf_to_config_argument(configs[i]);
+
+		switch (param) {
+		case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+			val = pctl_readl(pctl, PADS_SCHMITT_EN_REG(pin));
+			if (arg)
+				val |= PADS_SCHMITT_EN_BIT(pin);
+			else
+				val &= ~PADS_SCHMITT_EN_BIT(pin);
+			pctl_writel(pctl, val, PADS_SCHMITT_EN_REG(pin));
+			break;
+		case PIN_CONFIG_BIAS_HIGH_IMPEDANCE:
+			val = pctl_readl(pctl, PADS_PU_PD_REG(pin));
+			val &= ~(PADS_PU_PD_MASK << PADS_PU_PD_SHIFT(pin));
+			val |= PADS_PU_PD_HIGHZ << PADS_PU_PD_SHIFT(pin);
+			pctl_writel(pctl, val, PADS_PU_PD_REG(pin));
+			break;
+		case PIN_CONFIG_BIAS_PULL_UP:
+			val = pctl_readl(pctl, PADS_PU_PD_REG(pin));
+			val &= ~(PADS_PU_PD_MASK << PADS_PU_PD_SHIFT(pin));
+			val |= PADS_PU_PD_UP << PADS_PU_PD_SHIFT(pin);
+			pctl_writel(pctl, val, PADS_PU_PD_REG(pin));
+			break;
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			val = pctl_readl(pctl, PADS_PU_PD_REG(pin));
+			val &= ~(PADS_PU_PD_MASK << PADS_PU_PD_SHIFT(pin));
+			val |= PADS_PU_PD_DOWN << PADS_PU_PD_SHIFT(pin);
+			pctl_writel(pctl, val, PADS_PU_PD_REG(pin));
+			break;
+		case PIN_CONFIG_BIAS_BUS_HOLD:
+			val = pctl_readl(pctl, PADS_PU_PD_REG(pin));
+			val &= ~(PADS_PU_PD_MASK << PADS_PU_PD_SHIFT(pin));
+			val |= PADS_PU_PD_BUS << PADS_PU_PD_SHIFT(pin);
+			pctl_writel(pctl, val, PADS_PU_PD_REG(pin));
+			break;
+		case PIN_CONFIG_SLEW_RATE:
+			val = pctl_readl(pctl, PADS_SLEW_RATE_REG(pin));
+			if (arg)
+				val |= PADS_SLEW_RATE_BIT(pin);
+			else
+				val &= ~PADS_SLEW_RATE_BIT(pin);
+			pctl_writel(pctl, val, PADS_SLEW_RATE_REG(pin));
+			break;
+		case PIN_CONFIG_DRIVE_STRENGTH:
+			val = pctl_readl(pctl, PADS_DRIVE_STRENGTH_REG(pin));
+			val &= ~(PADS_DRIVE_STRENGTH_MASK <<
+				 PADS_DRIVE_STRENGTH_SHIFT(pin));
+			switch (arg) {
+			case 2:
+				drv = PADS_DRIVE_STRENGTH_2MA;
+				break;
+			case 4:
+				drv = PADS_DRIVE_STRENGTH_4MA;
+				break;
+			case 8:
+				drv = PADS_DRIVE_STRENGTH_8MA;
+				break;
+			case 12:
+				drv = PADS_DRIVE_STRENGTH_12MA;
+				break;
+			default:
+				dev_err(pctl->dev,
+					"Drive strength %umA not supported\n",
+					arg);
+				return -EINVAL;
+			}
+			val |= drv << PADS_DRIVE_STRENGTH_SHIFT(pin);
+			pctl_writel(pctl, val, PADS_DRIVE_STRENGTH_REG(pin));
+			break;
+		default:
+			dev_err(pctl->dev, "Property %u not supported\n",
+				param);
+			return -ENOTSUPP;
+		}
+	}
+
+	return 0;
+}
+
+static const struct pinconf_ops pistachio_pinconf_ops = {
+	.pin_config_get = pistachio_pinconf_get,
+	.pin_config_set = pistachio_pinconf_set,
+	.is_generic = true,
+};
+
+static struct pinctrl_desc pistachio_pinctrl_desc = {
+	.name = "pistachio-pinctrl",
+	.pctlops = &pistachio_pinctrl_ops,
+	.pmxops = &pistachio_pinmux_ops,
+	.confops = &pistachio_pinconf_ops,
+};
+
+static int pistachio_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	return pinctrl_request_gpio(chip->base + offset);
+}
+
+static void pistachio_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	pinctrl_free_gpio(chip->base + offset);
+}
+
+static int pistachio_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+
+	return !(gpio_readl(bank, GPIO_OUTPUT_EN) & BIT(offset));
+}
+
+static int pistachio_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+	u32 reg;
+
+	if (gpio_readl(bank, GPIO_OUTPUT_EN) & BIT(offset))
+		reg = GPIO_OUTPUT;
+	else
+		reg = GPIO_INPUT;
+
+	return !!(gpio_readl(bank, reg) & BIT(offset));
+}
+
+static void pistachio_gpio_set(struct gpio_chip *chip, unsigned offset,
+			       int value)
+{
+	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+
+	gpio_mask_writel(bank, GPIO_OUTPUT, offset, !!value);
+}
+
+static int pistachio_gpio_direction_input(struct gpio_chip *chip,
+					  unsigned offset)
+{
+	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+
+	gpio_mask_writel(bank, GPIO_OUTPUT_EN, offset, 0);
+	gpio_enable(bank, offset);
+
+	return 0;
+}
+
+static int pistachio_gpio_direction_output(struct gpio_chip *chip,
+					   unsigned offset, int value)
+{
+	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+
+	pistachio_gpio_set(chip, offset, value);
+	gpio_mask_writel(bank, GPIO_OUTPUT_EN, offset, 1);
+	gpio_enable(bank, offset);
+
+	return 0;
+}
+
+static void pistachio_gpio_irq_ack(struct irq_data *data)
+{
+	struct pistachio_gpio_bank *bank = irqd_to_bank(data);
+
+	gpio_mask_writel(bank, GPIO_INTERRUPT_STATUS, data->hwirq, 0);
+}
+
+static void pistachio_gpio_irq_mask(struct irq_data *data)
+{
+	struct pistachio_gpio_bank *bank = irqd_to_bank(data);
+
+	gpio_mask_writel(bank, GPIO_INTERRUPT_EN, data->hwirq, 0);
+}
+
+static void pistachio_gpio_irq_unmask(struct irq_data *data)
+{
+	struct pistachio_gpio_bank *bank = irqd_to_bank(data);
+
+	gpio_mask_writel(bank, GPIO_INTERRUPT_EN, data->hwirq, 1);
+}
+
+static unsigned int pistachio_gpio_irq_startup(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+
+	pistachio_gpio_direction_input(chip, data->hwirq);
+	pistachio_gpio_irq_unmask(data);
+
+	return 0;
+}
+
+static int pistachio_gpio_irq_set_type(struct irq_data *data, unsigned int type)
+{
+	struct pistachio_gpio_bank *bank = irqd_to_bank(data);
+
+	switch (type & IRQ_TYPE_SENSE_MASK) {
+	case IRQ_TYPE_EDGE_RISING:
+		gpio_mask_writel(bank, GPIO_INPUT_POLARITY, data->hwirq, 1);
+		gpio_mask_writel(bank, GPIO_INTERRUPT_TYPE, data->hwirq,
+				 GPIO_INTERRUPT_TYPE_EDGE);
+		gpio_mask_writel(bank, GPIO_INTERRUPT_EDGE, data->hwirq,
+				 GPIO_INTERRUPT_EDGE_SINGLE);
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		gpio_mask_writel(bank, GPIO_INPUT_POLARITY, data->hwirq, 0);
+		gpio_mask_writel(bank, GPIO_INTERRUPT_TYPE, data->hwirq,
+				 GPIO_INTERRUPT_TYPE_EDGE);
+		gpio_mask_writel(bank, GPIO_INTERRUPT_EDGE, data->hwirq,
+				 GPIO_INTERRUPT_EDGE_SINGLE);
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		gpio_mask_writel(bank, GPIO_INTERRUPT_TYPE, data->hwirq,
+				 GPIO_INTERRUPT_TYPE_EDGE);
+		gpio_mask_writel(bank, GPIO_INTERRUPT_EDGE, data->hwirq,
+				 GPIO_INTERRUPT_EDGE_DUAL);
+		break;
+	case IRQ_TYPE_LEVEL_HIGH:
+		gpio_mask_writel(bank, GPIO_INPUT_POLARITY, data->hwirq, 1);
+		gpio_mask_writel(bank, GPIO_INTERRUPT_TYPE, data->hwirq,
+				 GPIO_INTERRUPT_TYPE_LEVEL);
+		break;
+	case IRQ_TYPE_LEVEL_LOW:
+		gpio_mask_writel(bank, GPIO_INPUT_POLARITY, data->hwirq, 0);
+		gpio_mask_writel(bank, GPIO_INTERRUPT_TYPE, data->hwirq,
+				 GPIO_INTERRUPT_TYPE_LEVEL);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (type & IRQ_TYPE_LEVEL_MASK)
+		__irq_set_handler_locked(data->irq, handle_level_irq);
+	else
+		__irq_set_handler_locked(data->irq, handle_edge_irq);
+
+	return 0;
+}
+
+static void pistachio_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
+{
+	struct gpio_chip *gc = irq_get_handler_data(irq);
+	struct pistachio_gpio_bank *bank = gc_to_bank(gc);
+	struct irq_chip *chip = irq_get_chip(irq);
+	unsigned long pending;
+	unsigned int pin;
+
+	chained_irq_enter(chip, desc);
+	pending = gpio_readl(bank, GPIO_INTERRUPT_STATUS) &
+		gpio_readl(bank, GPIO_INTERRUPT_EN);
+	for_each_set_bit(pin, &pending, 16)
+		generic_handle_irq(irq_linear_revmap(gc->irqdomain, pin));
+	chained_irq_exit(chip, desc);
+}
+
+#define GPIO_BANK(_bank, _pin_base, _npins)				\
+	{								\
+		.pin_base = _pin_base,					\
+		.npins = _npins,					\
+		.gpio_chip = {						\
+			.label = "GPIO" #_bank,				\
+			.request = pistachio_gpio_request,		\
+			.free = pistachio_gpio_free,			\
+			.get_direction = pistachio_gpio_get_direction,	\
+			.direction_input = pistachio_gpio_direction_input, \
+			.direction_output = pistachio_gpio_direction_output, \
+			.get = pistachio_gpio_get,			\
+			.set = pistachio_gpio_set,			\
+			.base = _pin_base,				\
+			.ngpio = _npins,				\
+		},							\
+		.irq_chip = {						\
+			.name = "GPIO" #_bank,				\
+			.irq_startup = pistachio_gpio_irq_startup,	\
+			.irq_ack = pistachio_gpio_irq_ack,		\
+			.irq_mask = pistachio_gpio_irq_mask,		\
+			.irq_unmask = pistachio_gpio_irq_unmask,	\
+			.irq_set_type = pistachio_gpio_irq_set_type,	\
+		},							\
+	}
+
+static struct pistachio_gpio_bank pistachio_gpio_banks[] = {
+	GPIO_BANK(0, PISTACHIO_PIN_MFIO(0), 16),
+	GPIO_BANK(1, PISTACHIO_PIN_MFIO(16), 16),
+	GPIO_BANK(2, PISTACHIO_PIN_MFIO(32), 16),
+	GPIO_BANK(3, PISTACHIO_PIN_MFIO(48), 16),
+	GPIO_BANK(4, PISTACHIO_PIN_MFIO(64), 16),
+	GPIO_BANK(5, PISTACHIO_PIN_MFIO(80), 10),
+};
+
+static int pistachio_gpio_register(struct pistachio_pinctrl *pctl)
+{
+	struct device_node *node = pctl->dev->of_node;
+	struct pistachio_gpio_bank *bank;
+	unsigned int i;
+	int irq, ret = 0;
+
+	for (i = 0; i < pctl->nbanks; i++) {
+		char child_name[sizeof("gpioXX")];
+		struct device_node *child;
+
+		snprintf(child_name, sizeof(child_name), "gpio%d", i);
+		child = of_get_child_by_name(node, child_name);
+		if (!child) {
+			dev_err(pctl->dev, "No node for bank %u\n", i);
+			ret = -ENODEV;
+			goto err;
+		}
+
+		if (!of_find_property(child, "gpio-controller", NULL)) {
+			dev_err(pctl->dev,
+				"No gpio-controller property for bank %u\n", i);
+			ret = -ENODEV;
+			goto err;
+		}
+
+		irq = irq_of_parse_and_map(child, 0);
+		if (irq < 0) {
+			dev_err(pctl->dev, "No IRQ for bank %u: %d\n", i, irq);
+			ret = irq;
+			goto err;
+		}
+
+		bank = &pctl->gpio_banks[i];
+		bank->pctl = pctl;
+		bank->base = pctl->base + GPIO_BANK_BASE(i);
+
+		bank->gpio_chip.dev = pctl->dev;
+		bank->gpio_chip.of_node = child;
+		ret = gpiochip_add(&bank->gpio_chip);
+		if (ret < 0) {
+			dev_err(pctl->dev, "Failed to add GPIO chip %u: %d\n",
+				i, ret);
+			goto err;
+		}
+
+		ret = gpiochip_irqchip_add(&bank->gpio_chip, &bank->irq_chip,
+					   0, handle_level_irq, IRQ_TYPE_NONE);
+		if (ret < 0) {
+			dev_err(pctl->dev, "Failed to add IRQ chip %u: %d\n",
+				i, ret);
+			gpiochip_remove(&bank->gpio_chip);
+			goto err;
+		}
+		gpiochip_set_chained_irqchip(&bank->gpio_chip, &bank->irq_chip,
+					     irq, pistachio_gpio_irq_handler);
+
+		ret = gpiochip_add_pin_range(&bank->gpio_chip,
+					     dev_name(pctl->dev), 0,
+					     bank->pin_base, bank->npins);
+		if (ret < 0) {
+			dev_err(pctl->dev, "Failed to add GPIO range %u: %d\n",
+				i, ret);
+			gpiochip_remove(&bank->gpio_chip);
+			goto err;
+		}
+	}
+
+	return 0;
+err:
+	for (; i > 0; i--) {
+		bank = &pctl->gpio_banks[i - 1];
+		gpiochip_remove(&bank->gpio_chip);
+	}
+	return ret;
+}
+
+static const struct of_device_id pistachio_pinctrl_of_match[] = {
+	{ .compatible = "img,pistachio-system-pinctrl", },
+	{ },
+};
+
+static int pistachio_pinctrl_probe(struct platform_device *pdev)
+{
+	struct pistachio_pinctrl *pctl;
+	struct resource *res;
+	int ret;
+
+	pctl = devm_kzalloc(&pdev->dev, sizeof(*pctl), GFP_KERNEL);
+	if (!pctl)
+		return -ENOMEM;
+	pctl->dev = &pdev->dev;
+	dev_set_drvdata(&pdev->dev, pctl);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pctl->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pctl->base))
+		return PTR_ERR(pctl->base);
+
+	pctl->pins = pistachio_pins;
+	pctl->npins = ARRAY_SIZE(pistachio_pins);
+	pctl->functions = pistachio_functions;
+	pctl->nfunctions = ARRAY_SIZE(pistachio_functions);
+	pctl->groups = pistachio_groups;
+	pctl->ngroups = ARRAY_SIZE(pistachio_groups);
+	pctl->gpio_banks = pistachio_gpio_banks;
+	pctl->nbanks = ARRAY_SIZE(pistachio_gpio_banks);
+
+	pistachio_pinctrl_desc.pins = pctl->pins;
+	pistachio_pinctrl_desc.npins = pctl->npins;
+
+	pctl->pctldev = pinctrl_register(&pistachio_pinctrl_desc, &pdev->dev,
+					 pctl);
+	if (IS_ERR(pctl->pctldev)) {
+		dev_err(&pdev->dev, "Failed to register pinctrl device\n");
+		return PTR_ERR(pctl->pctldev);
+	}
+
+	ret = pistachio_gpio_register(pctl);
+	if (ret < 0) {
+		pinctrl_unregister(pctl->pctldev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct platform_driver pistachio_pinctrl_driver = {
+	.driver = {
+		.name = "pistachio-pinctrl",
+		.of_match_table = pistachio_pinctrl_of_match,
+		.suppress_bind_attrs = true,
+	},
+	.probe = pistachio_pinctrl_probe,
+};
+
+static int __init pistachio_pinctrl_register(void)
+{
+	return platform_driver_register(&pistachio_pinctrl_driver);
+}
+arch_initcall(pistachio_pinctrl_register);
diff --git a/drivers/pinctrl/pinctrl-rockchip.c b/drivers/pinctrl/pinctrl-rockchip.c
index dee7d5f06c60..9affcd725776 100644
--- a/drivers/pinctrl/pinctrl-rockchip.c
+++ b/drivers/pinctrl/pinctrl-rockchip.c
@@ -63,6 +63,7 @@ enum rockchip_pinctrl_type {
 	RK3066B,
 	RK3188,
 	RK3288,
+	RK3368,
 };
 
 /**
@@ -163,6 +164,9 @@ struct rockchip_pin_ctrl {
 	void	(*pull_calc_reg)(struct rockchip_pin_bank *bank,
 				    int pin_num, struct regmap **regmap,
 				    int *reg, u8 *bit);
+	void	(*drv_calc_reg)(struct rockchip_pin_bank *bank,
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit);
 };
 
 struct rockchip_pin_config {
@@ -581,7 +585,6 @@ static void rk3288_calc_pull_reg_and_bit(struct rockchip_pin_bank *bank,
 #define RK3288_DRV_BITS_PER_PIN		2
 #define RK3288_DRV_PINS_PER_REG		8
 #define RK3288_DRV_BANK_STRIDE		16
-static int rk3288_drv_list[] = { 2, 4, 8, 12 };
 
 static void rk3288_calc_drv_reg_and_bit(struct rockchip_pin_bank *bank,
 				    int pin_num, struct regmap **regmap,
@@ -611,14 +614,81 @@ static void rk3288_calc_drv_reg_and_bit(struct rockchip_pin_bank *bank,
 	}
 }
 
-static int rk3288_get_drive(struct rockchip_pin_bank *bank, int pin_num)
+#define RK3368_PULL_GRF_OFFSET		0x100
+#define RK3368_PULL_PMU_OFFSET		0x10
+
+static void rk3368_calc_pull_reg_and_bit(struct rockchip_pin_bank *bank,
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit)
 {
+	struct rockchip_pinctrl *info = bank->drvdata;
+
+	/* The first 32 pins of the first bank are located in PMU */
+	if (bank->bank_num == 0) {
+		*regmap = info->regmap_pmu;
+		*reg = RK3368_PULL_PMU_OFFSET;
+
+		*reg += ((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
+		*bit = pin_num % RK3188_PULL_PINS_PER_REG;
+		*bit *= RK3188_PULL_BITS_PER_PIN;
+	} else {
+		*regmap = info->regmap_base;
+		*reg = RK3368_PULL_GRF_OFFSET;
+
+		/* correct the offset, as we're starting with the 2nd bank */
+		*reg -= 0x10;
+		*reg += bank->bank_num * RK3188_PULL_BANK_STRIDE;
+		*reg += ((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
+
+		*bit = (pin_num % RK3188_PULL_PINS_PER_REG);
+		*bit *= RK3188_PULL_BITS_PER_PIN;
+	}
+}
+
+#define RK3368_DRV_PMU_OFFSET		0x20
+#define RK3368_DRV_GRF_OFFSET		0x200
+
+static void rk3368_calc_drv_reg_and_bit(struct rockchip_pin_bank *bank,
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit)
+{
+	struct rockchip_pinctrl *info = bank->drvdata;
+
+	/* The first 32 pins of the first bank are located in PMU */
+	if (bank->bank_num == 0) {
+		*regmap = info->regmap_pmu;
+		*reg = RK3368_DRV_PMU_OFFSET;
+
+		*reg += ((pin_num / RK3288_DRV_PINS_PER_REG) * 4);
+		*bit = pin_num % RK3288_DRV_PINS_PER_REG;
+		*bit *= RK3288_DRV_BITS_PER_PIN;
+	} else {
+		*regmap = info->regmap_base;
+		*reg = RK3368_DRV_GRF_OFFSET;
+
+		/* correct the offset, as we're starting with the 2nd bank */
+		*reg -= 0x10;
+		*reg += bank->bank_num * RK3288_DRV_BANK_STRIDE;
+		*reg += ((pin_num / RK3288_DRV_PINS_PER_REG) * 4);
+
+		*bit = (pin_num % RK3288_DRV_PINS_PER_REG);
+		*bit *= RK3288_DRV_BITS_PER_PIN;
+	}
+}
+
+static int rockchip_perpin_drv_list[] = { 2, 4, 8, 12 };
+
+static int rockchip_get_drive_perpin(struct rockchip_pin_bank *bank,
+				     int pin_num)
+{
+	struct rockchip_pinctrl *info = bank->drvdata;
+	struct rockchip_pin_ctrl *ctrl = info->ctrl;
 	struct regmap *regmap;
 	int reg, ret;
 	u32 data;
 	u8 bit;
 
-	rk3288_calc_drv_reg_and_bit(bank, pin_num, &regmap, &reg, &bit);
+	ctrl->drv_calc_reg(bank, pin_num, &regmap, &reg, &bit);
 
 	ret = regmap_read(regmap, reg, &data);
 	if (ret)
@@ -627,24 +697,25 @@ static int rk3288_get_drive(struct rockchip_pin_bank *bank, int pin_num)
 	data >>= bit;
 	data &= (1 << RK3288_DRV_BITS_PER_PIN) - 1;
 
-	return rk3288_drv_list[data];
+	return rockchip_perpin_drv_list[data];
 }
 
-static int rk3288_set_drive(struct rockchip_pin_bank *bank, int pin_num,
-			    int strength)
+static int rockchip_set_drive_perpin(struct rockchip_pin_bank *bank,
+				     int pin_num, int strength)
 {
 	struct rockchip_pinctrl *info = bank->drvdata;
+	struct rockchip_pin_ctrl *ctrl = info->ctrl;
 	struct regmap *regmap;
 	unsigned long flags;
 	int reg, ret, i;
 	u32 data, rmask;
 	u8 bit;
 
-	rk3288_calc_drv_reg_and_bit(bank, pin_num, &regmap, &reg, &bit);
+	ctrl->drv_calc_reg(bank, pin_num, &regmap, &reg, &bit);
 
 	ret = -EINVAL;
-	for (i = 0; i < ARRAY_SIZE(rk3288_drv_list); i++) {
-		if (rk3288_drv_list[i] == strength) {
+	for (i = 0; i < ARRAY_SIZE(rockchip_perpin_drv_list); i++) {
+		if (rockchip_perpin_drv_list[i] == strength) {
 			ret = i;
 			break;
 		}
@@ -695,6 +766,7 @@ static int rockchip_get_pull(struct rockchip_pin_bank *bank, int pin_num)
 				: PIN_CONFIG_BIAS_DISABLE;
 	case RK3188:
 	case RK3288:
+	case RK3368:
 		data >>= bit;
 		data &= (1 << RK3188_PULL_BITS_PER_PIN) - 1;
 
@@ -750,6 +822,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 		break;
 	case RK3188:
 	case RK3288:
+	case RK3368:
 		spin_lock_irqsave(&bank->slock, flags);
 
 		/* enable the write to the equivalent lower bits */
@@ -927,6 +1000,7 @@ static bool rockchip_pinconf_pull_valid(struct rockchip_pin_ctrl *ctrl,
 		return pull ? false : true;
 	case RK3188:
 	case RK3288:
+	case RK3368:
 		return (pull != PIN_CONFIG_BIAS_PULL_PIN_DEFAULT);
 	}
 
@@ -983,10 +1057,11 @@ static int rockchip_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
 			break;
 		case PIN_CONFIG_DRIVE_STRENGTH:
 			/* rk3288 is the first with per-pin drive-strength */
-			if (info->ctrl->type != RK3288)
+			if (!info->ctrl->drv_calc_reg)
 				return -ENOTSUPP;
 
-			rc = rk3288_set_drive(bank, pin - bank->pin_base, arg);
+			rc = rockchip_set_drive_perpin(bank,
+						pin - bank->pin_base, arg);
 			if (rc < 0)
 				return rc;
 			break;
@@ -1041,10 +1116,10 @@ static int rockchip_pinconf_get(struct pinctrl_dev *pctldev, unsigned int pin,
 		break;
 	case PIN_CONFIG_DRIVE_STRENGTH:
 		/* rk3288 is the first with per-pin drive-strength */
-		if (info->ctrl->type != RK3288)
+		if (!info->ctrl->drv_calc_reg)
 			return -ENOTSUPP;
 
-		rc = rk3288_get_drive(bank, pin - bank->pin_base);
+		rc = rockchip_get_drive_perpin(bank, pin - bank->pin_base);
 		if (rc < 0)
 			return rc;
 
@@ -1274,9 +1349,9 @@ static int rockchip_pinctrl_register(struct platform_device *pdev,
 		return ret;
 
 	info->pctl_dev = pinctrl_register(ctrldesc, &pdev->dev, info);
-	if (!info->pctl_dev) {
+	if (IS_ERR(info->pctl_dev)) {
 		dev_err(&pdev->dev, "could not register pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(info->pctl_dev);
 	}
 
 	for (bank = 0; bank < info->ctrl->nr_banks; ++bank) {
@@ -2056,8 +2131,32 @@ static struct rockchip_pin_ctrl rk3288_pin_ctrl = {
 		.grf_mux_offset		= 0x0,
 		.pmu_mux_offset		= 0x84,
 		.pull_calc_reg		= rk3288_calc_pull_reg_and_bit,
+		.drv_calc_reg		= rk3288_calc_drv_reg_and_bit,
 };
 
+static struct rockchip_pin_bank rk3368_pin_banks[] = {
+	PIN_BANK_IOMUX_FLAGS(0, 32, "gpio0", IOMUX_SOURCE_PMU,
+					     IOMUX_SOURCE_PMU,
+					     IOMUX_SOURCE_PMU,
+					     IOMUX_SOURCE_PMU
+			    ),
+	PIN_BANK(1, 32, "gpio1"),
+	PIN_BANK(2, 32, "gpio2"),
+	PIN_BANK(3, 32, "gpio3"),
+};
+
+static struct rockchip_pin_ctrl rk3368_pin_ctrl = {
+		.pin_banks		= rk3368_pin_banks,
+		.nr_banks		= ARRAY_SIZE(rk3368_pin_banks),
+		.label			= "RK3368-GPIO",
+		.type			= RK3368,
+		.grf_mux_offset		= 0x0,
+		.pmu_mux_offset		= 0x0,
+		.pull_calc_reg		= rk3368_calc_pull_reg_and_bit,
+		.drv_calc_reg		= rk3368_calc_drv_reg_and_bit,
+};
+
+
 static const struct of_device_id rockchip_pinctrl_dt_match[] = {
 	{ .compatible = "rockchip,rk2928-pinctrl",
 		.data = (void *)&rk2928_pin_ctrl },
@@ -2069,6 +2168,8 @@ static const struct of_device_id rockchip_pinctrl_dt_match[] = {
 		.data = (void *)&rk3188_pin_ctrl },
 	{ .compatible = "rockchip,rk3288-pinctrl",
 		.data = (void *)&rk3288_pin_ctrl },
+	{ .compatible = "rockchip,rk3368-pinctrl",
+		.data = (void *)&rk3368_pin_ctrl },
 	{},
 };
 MODULE_DEVICE_TABLE(of, rockchip_pinctrl_dt_match);
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
index 13b45f297727..b2de09d3b1a0 100644
--- a/drivers/pinctrl/pinctrl-single.c
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -1726,7 +1726,7 @@ static int pcs_irqdomain_map(struct irq_domain *d, unsigned int irq,
 	return 0;
 }
 
-static struct irq_domain_ops pcs_irqdomain_ops = {
+static const struct irq_domain_ops pcs_irqdomain_ops = {
 	.map = pcs_irqdomain_map,
 	.xlate = irq_domain_xlate_onecell,
 };
@@ -1921,9 +1921,9 @@ static int pcs_probe(struct platform_device *pdev)
 		goto free;
 
 	pcs->pctl = pinctrl_register(&pcs->desc, pcs->dev, pcs);
-	if (!pcs->pctl) {
+	if (IS_ERR(pcs->pctl)) {
 		dev_err(pcs->dev, "could not register single pinctrl driver\n");
-		ret = -EINVAL;
+		ret = PTR_ERR(pcs->pctl);
 		goto free;
 	}
 
diff --git a/drivers/pinctrl/pinctrl-st.c b/drivers/pinctrl/pinctrl-st.c
index 65bf73b70e34..c262e5f35c28 100644
--- a/drivers/pinctrl/pinctrl-st.c
+++ b/drivers/pinctrl/pinctrl-st.c
@@ -1661,8 +1661,8 @@ static int st_pctl_probe_dt(struct platform_device *pdev,
 		if (IS_ERR(info->irqmux_base))
 			return PTR_ERR(info->irqmux_base);
 
-		irq_set_chained_handler(irq, st_gpio_irqmux_handler);
-		irq_set_handler_data(irq, info);
+		irq_set_chained_handler_and_data(irq, st_gpio_irqmux_handler,
+						 info);
 
 	}
 
@@ -1737,9 +1737,9 @@ static int st_pctl_probe(struct platform_device *pdev)
 	pctl_desc->name		= dev_name(&pdev->dev);
 
 	info->pctl = pinctrl_register(pctl_desc, &pdev->dev, info);
-	if (!info->pctl) {
+	if (IS_ERR(info->pctl)) {
 		dev_err(&pdev->dev, "Failed pinctrl registration\n");
-		return -EINVAL;
+		return PTR_ERR(info->pctl);
 	}
 
 	for (i = 0; i < info->nbanks; i++)
diff --git a/drivers/pinctrl/pinctrl-tb10x.c b/drivers/pinctrl/pinctrl-tb10x.c
index 160a1f5e9896..6546b9bb2e06 100644
--- a/drivers/pinctrl/pinctrl-tb10x.c
+++ b/drivers/pinctrl/pinctrl-tb10x.c
@@ -807,9 +807,9 @@ static int tb10x_pinctrl_probe(struct platform_device *pdev)
 	}
 
 	state->pctl = pinctrl_register(&tb10x_pindesc, dev, state);
-	if (!state->pctl) {
+	if (IS_ERR(state->pctl)) {
 		dev_err(dev, "could not register TB10x pin driver\n");
-		ret = -EINVAL;
+		ret = PTR_ERR(state->pctl);
 		goto fail;
 	}
 
diff --git a/drivers/pinctrl/pinctrl-tegra-xusb.c b/drivers/pinctrl/pinctrl-tegra-xusb.c
index 753d747d4261..2651d04bd1be 100644
--- a/drivers/pinctrl/pinctrl-tegra-xusb.c
+++ b/drivers/pinctrl/pinctrl-tegra-xusb.c
@@ -59,11 +59,6 @@ struct tegra_xusb_padctl_function {
 	unsigned int num_groups;
 };
 
-struct tegra_xusb_padctl_group {
-	const unsigned int *funcs;
-	unsigned int num_funcs;
-};
-
 struct tegra_xusb_padctl_soc {
 	const struct pinctrl_pin_desc *pins;
 	unsigned int num_pins;
@@ -130,6 +125,21 @@ static const char *tegra_xusb_padctl_get_group_name(struct pinctrl_dev *pinctrl,
 	return padctl->soc->pins[group].name;
 }
 
+static int tegra_xusb_padctl_get_group_pins(struct pinctrl_dev *pinctrl,
+					    unsigned group,
+					    const unsigned **pins,
+					    unsigned *num_pins)
+{
+	/*
+	 * For the tegra-xusb pad controller groups are synonomous
+	 * with lanes/pins and there is always one lane/pin per group.
+	 */
+	*pins = &pinctrl->desc->pins[group].number;
+	*num_pins = 1;
+
+	return 0;
+}
+
 enum tegra_xusb_padctl_param {
 	TEGRA_XUSB_PADCTL_IDDQ,
 };
@@ -253,6 +263,7 @@ static int tegra_xusb_padctl_dt_node_to_map(struct pinctrl_dev *pinctrl,
 static const struct pinctrl_ops tegra_xusb_padctl_pinctrl_ops = {
 	.get_groups_count = tegra_xusb_padctl_get_groups_count,
 	.get_group_name = tegra_xusb_padctl_get_group_name,
+	.get_group_pins = tegra_xusb_padctl_get_group_pins,
 	.dt_node_to_map = tegra_xusb_padctl_dt_node_to_map,
 	.dt_free_map = pinctrl_utils_dt_free_map,
 };
@@ -903,15 +914,17 @@ static int tegra_xusb_padctl_probe(struct platform_device *pdev)
 
 	memset(&padctl->desc, 0, sizeof(padctl->desc));
 	padctl->desc.name = dev_name(padctl->dev);
+	padctl->desc.pins = tegra124_pins;
+	padctl->desc.npins = ARRAY_SIZE(tegra124_pins);
 	padctl->desc.pctlops = &tegra_xusb_padctl_pinctrl_ops;
 	padctl->desc.pmxops = &tegra_xusb_padctl_pinmux_ops;
 	padctl->desc.confops = &tegra_xusb_padctl_pinconf_ops;
 	padctl->desc.owner = THIS_MODULE;
 
 	padctl->pinctrl = pinctrl_register(&padctl->desc, &pdev->dev, padctl);
-	if (!padctl->pinctrl) {
+	if (IS_ERR(padctl->pinctrl)) {
 		dev_err(&pdev->dev, "failed to register pincontrol\n");
-		err = -ENODEV;
+		err = PTR_ERR(padctl->pinctrl);
 		goto reset;
 	}
 
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index 4c95c2024a1c..0f982b829be1 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -703,9 +703,9 @@ int tegra_pinctrl_probe(struct platform_device *pdev,
 	}
 
 	pmx->pctl = pinctrl_register(&tegra_pinctrl_desc, &pdev->dev, pmx);
-	if (!pmx->pctl) {
+	if (IS_ERR(pmx->pctl)) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(pmx->pctl);
 	}
 
 	pinctrl_add_gpio_range(pmx->pctl, &tegra_pinctrl_gpio_range);
diff --git a/drivers/pinctrl/pinctrl-tz1090-pdc.c b/drivers/pinctrl/pinctrl-tz1090-pdc.c
index 8a8911bb883a..c349911708ef 100644
--- a/drivers/pinctrl/pinctrl-tz1090-pdc.c
+++ b/drivers/pinctrl/pinctrl-tz1090-pdc.c
@@ -948,9 +948,9 @@ static int tz1090_pdc_pinctrl_probe(struct platform_device *pdev)
 		return PTR_ERR(pmx->regs);
 
 	pmx->pctl = pinctrl_register(&tz1090_pdc_pinctrl_desc, &pdev->dev, pmx);
-	if (!pmx->pctl) {
+	if (IS_ERR(pmx->pctl)) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(pmx->pctl);
 	}
 
 	platform_set_drvdata(pdev, pmx);
diff --git a/drivers/pinctrl/pinctrl-tz1090.c b/drivers/pinctrl/pinctrl-tz1090.c
index fc5594a530c2..6d07a2f64d97 100644
--- a/drivers/pinctrl/pinctrl-tz1090.c
+++ b/drivers/pinctrl/pinctrl-tz1090.c
@@ -1963,9 +1963,9 @@ static int tz1090_pinctrl_probe(struct platform_device *pdev)
 		return PTR_ERR(pmx->regs);
 
 	pmx->pctl = pinctrl_register(&tz1090_pinctrl_desc, &pdev->dev, pmx);
-	if (!pmx->pctl) {
+	if (IS_ERR(pmx->pctl)) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(pmx->pctl);
 	}
 
 	platform_set_drvdata(pdev, pmx);
diff --git a/drivers/pinctrl/pinctrl-u300.c b/drivers/pinctrl/pinctrl-u300.c
index f931e65aba3a..c076021f37d2 100644
--- a/drivers/pinctrl/pinctrl-u300.c
+++ b/drivers/pinctrl/pinctrl-u300.c
@@ -1068,9 +1068,9 @@ static int u300_pmx_probe(struct platform_device *pdev)
 		return PTR_ERR(upmx->virtbase);
 
 	upmx->pctl = pinctrl_register(&u300_pmx_desc, &pdev->dev, upmx);
-	if (!upmx->pctl) {
+	if (IS_ERR(upmx->pctl)) {
 		dev_err(&pdev->dev, "could not register U300 pinmux driver\n");
-		return -EINVAL;
+		return PTR_ERR(upmx->pctl);
 	}
 
 	platform_set_drvdata(pdev, upmx);
diff --git a/drivers/pinctrl/pinctrl-zynq.c b/drivers/pinctrl/pinctrl-zynq.c
index 22280bddb9e2..7ce23b6282ad 100644
--- a/drivers/pinctrl/pinctrl-zynq.c
+++ b/drivers/pinctrl/pinctrl-zynq.c
@@ -101,6 +101,8 @@ enum zynq_pinmux_functions {
 	ZYNQ_PMUX_qspi_cs1,
 	ZYNQ_PMUX_spi0,
 	ZYNQ_PMUX_spi1,
+	ZYNQ_PMUX_spi0_ss,
+	ZYNQ_PMUX_spi1_ss,
 	ZYNQ_PMUX_sdio0,
 	ZYNQ_PMUX_sdio0_pc,
 	ZYNQ_PMUX_sdio0_cd,
@@ -123,7 +125,7 @@ enum zynq_pinmux_functions {
 	ZYNQ_PMUX_MAX_FUNC
 };
 
-const struct pinctrl_pin_desc zynq_pins[] = {
+static const struct pinctrl_pin_desc zynq_pins[] = {
 	PINCTRL_PIN(0,  "MIO0"),
 	PINCTRL_PIN(1,  "MIO1"),
 	PINCTRL_PIN(2,  "MIO2"),
@@ -196,13 +198,35 @@ static const unsigned int qspi0_0_pins[] = {1, 2, 3, 4, 5, 6};
 static const unsigned int qspi1_0_pins[] = {9, 10, 11, 12, 13};
 static const unsigned int qspi_cs1_pins[] = {0};
 static const unsigned int qspi_fbclk_pins[] = {8};
-static const unsigned int spi0_0_pins[] = {16, 17, 18, 19, 20, 21};
-static const unsigned int spi0_1_pins[] = {28, 29, 30, 31, 32, 33};
-static const unsigned int spi0_2_pins[] = {40, 41, 42, 43, 44, 45};
-static const unsigned int spi1_0_pins[] = {10, 11, 12, 13, 14, 15};
-static const unsigned int spi1_1_pins[] = {22, 23, 24, 25, 26, 27};
-static const unsigned int spi1_2_pins[] = {34, 35, 36, 37, 38, 39};
-static const unsigned int spi1_3_pins[] = {46, 47, 48, 49, 40, 51};
+static const unsigned int spi0_0_pins[] = {16, 17, 21};
+static const unsigned int spi0_0_ss0_pins[] = {18};
+static const unsigned int spi0_0_ss1_pins[] = {19};
+static const unsigned int spi0_0_ss2_pins[] = {20,};
+static const unsigned int spi0_1_pins[] = {28, 29, 33};
+static const unsigned int spi0_1_ss0_pins[] = {30};
+static const unsigned int spi0_1_ss1_pins[] = {31};
+static const unsigned int spi0_1_ss2_pins[] = {32};
+static const unsigned int spi0_2_pins[] = {40, 41, 45};
+static const unsigned int spi0_2_ss0_pins[] = {42};
+static const unsigned int spi0_2_ss1_pins[] = {43};
+static const unsigned int spi0_2_ss2_pins[] = {44};
+static const unsigned int spi1_0_pins[] = {10, 11, 12};
+static const unsigned int spi1_0_ss0_pins[] = {13};
+static const unsigned int spi1_0_ss1_pins[] = {14};
+static const unsigned int spi1_0_ss2_pins[] = {15};
+static const unsigned int spi1_1_pins[] = {22, 23, 24};
+static const unsigned int spi1_1_ss0_pins[] = {25};
+static const unsigned int spi1_1_ss1_pins[] = {26};
+static const unsigned int spi1_1_ss2_pins[] = {27};
+static const unsigned int spi1_2_pins[] = {34, 35, 36};
+static const unsigned int spi1_2_ss0_pins[] = {37};
+static const unsigned int spi1_2_ss1_pins[] = {38};
+static const unsigned int spi1_2_ss2_pins[] = {39};
+static const unsigned int spi1_3_pins[] = {46, 47, 48, 49};
+static const unsigned int spi1_3_ss0_pins[] = {49};
+static const unsigned int spi1_3_ss1_pins[] = {50};
+static const unsigned int spi1_3_ss2_pins[] = {51};
+
 static const unsigned int sdio0_0_pins[] = {16, 17, 18, 19, 20, 21};
 static const unsigned int sdio0_1_pins[] = {28, 29, 30, 31, 32, 33};
 static const unsigned int sdio0_2_pins[] = {40, 41, 42, 43, 44, 45};
@@ -369,7 +393,7 @@ static const unsigned int usb1_0_pins[] = {40, 41, 42, 43, 44, 45, 46, 47, 48,
 		.npins = ARRAY_SIZE(nm ## _pins), \
 	}
 
-struct zynq_pctrl_group zynq_pctrl_groups[] = {
+static const struct zynq_pctrl_group zynq_pctrl_groups[] = {
 	DEFINE_ZYNQ_PINCTRL_GRP(ethernet0_0),
 	DEFINE_ZYNQ_PINCTRL_GRP(ethernet1_0),
 	DEFINE_ZYNQ_PINCTRL_GRP(mdio0_0),
@@ -379,12 +403,33 @@ struct zynq_pctrl_group zynq_pctrl_groups[] = {
 	DEFINE_ZYNQ_PINCTRL_GRP(qspi_fbclk),
 	DEFINE_ZYNQ_PINCTRL_GRP(qspi_cs1),
 	DEFINE_ZYNQ_PINCTRL_GRP(spi0_0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_0_ss0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_0_ss1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_0_ss2),
 	DEFINE_ZYNQ_PINCTRL_GRP(spi0_1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_1_ss0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_1_ss1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_1_ss2),
 	DEFINE_ZYNQ_PINCTRL_GRP(spi0_2),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_2_ss0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_2_ss1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi0_2_ss2),
 	DEFINE_ZYNQ_PINCTRL_GRP(spi1_0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_0_ss0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_0_ss1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_0_ss2),
 	DEFINE_ZYNQ_PINCTRL_GRP(spi1_1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_1_ss0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_1_ss1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_1_ss2),
 	DEFINE_ZYNQ_PINCTRL_GRP(spi1_2),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_2_ss0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_2_ss1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_2_ss2),
 	DEFINE_ZYNQ_PINCTRL_GRP(spi1_3),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_3_ss0),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_3_ss1),
+	DEFINE_ZYNQ_PINCTRL_GRP(spi1_3_ss2),
 	DEFINE_ZYNQ_PINCTRL_GRP(sdio0_0),
 	DEFINE_ZYNQ_PINCTRL_GRP(sdio0_1),
 	DEFINE_ZYNQ_PINCTRL_GRP(sdio0_2),
@@ -552,6 +597,15 @@ static const char * const spi0_groups[] = {"spi0_0_grp", "spi0_1_grp",
 					   "spi0_2_grp"};
 static const char * const spi1_groups[] = {"spi1_0_grp", "spi1_1_grp",
 					   "spi1_2_grp", "spi1_3_grp"};
+static const char * const spi0_ss_groups[] = {"spi0_0_ss0_grp",
+		"spi0_0_ss1_grp", "spi0_0_ss2_grp", "spi0_1_ss0_grp",
+		"spi0_1_ss1_grp", "spi0_1_ss2_grp", "spi0_2_ss0_grp",
+		"spi0_2_ss1_grp", "spi0_2_ss2_grp"};
+static const char * const spi1_ss_groups[] = {"spi1_0_ss0_grp",
+		"spi1_0_ss1_grp", "spi1_0_ss2_grp", "spi1_1_ss0_grp",
+		"spi1_1_ss1_grp", "spi1_1_ss2_grp", "spi1_2_ss0_grp",
+		"spi1_2_ss1_grp", "spi1_2_ss2_grp", "spi1_3_ss0_grp",
+		"spi1_3_ss1_grp", "spi1_3_ss2_grp"};
 static const char * const sdio0_groups[] = {"sdio0_0_grp", "sdio0_1_grp",
 					    "sdio0_2_grp"};
 static const char * const sdio1_groups[] = {"sdio1_0_grp", "sdio1_1_grp",
@@ -714,12 +768,13 @@ static const char * const gpio0_groups[] = {"gpio0_0_grp",
 		.mux_val = mval,			\
 	}
 
-#define DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(fname, mval, mux, mask, shift)	\
+#define DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(fname, mval, offset, mask, shift)\
 	[ZYNQ_PMUX_##fname] = {				\
 		.name = #fname,				\
 		.groups = fname##_groups,		\
 		.ngroups = ARRAY_SIZE(fname##_groups),	\
 		.mux_val = mval,			\
+		.mux = offset,				\
 		.mux_mask = mask,			\
 		.mux_shift = shift,			\
 	}
@@ -742,17 +797,19 @@ static const struct zynq_pinmux_function zynq_pmux_functions[] = {
 	DEFINE_ZYNQ_PINMUX_FUNCTION(qspi_cs1, 1),
 	DEFINE_ZYNQ_PINMUX_FUNCTION(spi0, 0x50),
 	DEFINE_ZYNQ_PINMUX_FUNCTION(spi1, 0x50),
+	DEFINE_ZYNQ_PINMUX_FUNCTION(spi0_ss, 0x50),
+	DEFINE_ZYNQ_PINMUX_FUNCTION(spi1_ss, 0x50),
 	DEFINE_ZYNQ_PINMUX_FUNCTION(sdio0, 0x40),
 	DEFINE_ZYNQ_PINMUX_FUNCTION(sdio0_pc, 0xc),
-	DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(sdio0_wp, 0, 130, ZYNQ_SDIO_WP_MASK,
+	DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(sdio0_wp, 0, 0x130, ZYNQ_SDIO_WP_MASK,
 					ZYNQ_SDIO_WP_SHIFT),
-	DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(sdio0_cd, 0, 130, ZYNQ_SDIO_CD_MASK,
+	DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(sdio0_cd, 0, 0x130, ZYNQ_SDIO_CD_MASK,
 					ZYNQ_SDIO_CD_SHIFT),
 	DEFINE_ZYNQ_PINMUX_FUNCTION(sdio1, 0x40),
 	DEFINE_ZYNQ_PINMUX_FUNCTION(sdio1_pc, 0xc),
-	DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(sdio1_wp, 0, 134, ZYNQ_SDIO_WP_MASK,
+	DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(sdio1_wp, 0, 0x134, ZYNQ_SDIO_WP_MASK,
 					ZYNQ_SDIO_WP_SHIFT),
-	DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(sdio1_cd, 0, 134, ZYNQ_SDIO_CD_MASK,
+	DEFINE_ZYNQ_PINMUX_FUNCTION_MUX(sdio1_cd, 0, 0x134, ZYNQ_SDIO_CD_MASK,
 					ZYNQ_SDIO_CD_SHIFT),
 	DEFINE_ZYNQ_PINMUX_FUNCTION(smc0_nor, 4),
 	DEFINE_ZYNQ_PINMUX_FUNCTION(smc0_nor_cs1, 8),
@@ -1139,8 +1196,8 @@ static int zynq_pinctrl_probe(struct platform_device *pdev)
 	pctrl->nfuncs = ARRAY_SIZE(zynq_pmux_functions);
 
 	pctrl->pctrl = pinctrl_register(&zynq_desc, &pdev->dev, pctrl);
-	if (!pctrl->pctrl)
-		return -ENOMEM;
+	if (IS_ERR(pctrl->pctrl))
+		return PTR_ERR(pctrl->pctrl);
 
 	platform_set_drvdata(pdev, pctrl);
 
@@ -1149,7 +1206,7 @@ static int zynq_pinctrl_probe(struct platform_device *pdev)
 	return 0;
 }
 
-int zynq_pinctrl_remove(struct platform_device *pdev)
+static int zynq_pinctrl_remove(struct platform_device *pdev)
 {
 	struct zynq_pinctrl *pctrl = platform_get_drvdata(pdev);
 
diff --git a/drivers/pinctrl/pinmux.c b/drivers/pinctrl/pinmux.c
index b874458dcb88..e7ae890dcf1a 100644
--- a/drivers/pinctrl/pinmux.c
+++ b/drivers/pinctrl/pinmux.c
@@ -107,6 +107,13 @@ static int pin_request(struct pinctrl_dev *pctldev,
 				desc->name, desc->gpio_owner, owner);
 			goto out;
 		}
+		if (ops->strict && desc->mux_usecount &&
+		    strcmp(desc->mux_owner, owner)) {
+			dev_err(pctldev->dev,
+				"pin %s already requested by %s; cannot claim for %s\n",
+				desc->name, desc->mux_owner, owner);
+			goto out;
+		}
 
 		desc->gpio_owner = owner;
 	} else {
@@ -116,6 +123,12 @@ static int pin_request(struct pinctrl_dev *pctldev,
 				desc->name, desc->mux_owner, owner);
 			goto out;
 		}
+		if (ops->strict && desc->gpio_owner) {
+			dev_err(pctldev->dev,
+				"pin %s already requested by %s; cannot claim for %s\n",
+				desc->name, desc->gpio_owner, owner);
+			goto out;
+		}
 
 		desc->mux_usecount++;
 		if (desc->mux_usecount > 1)
@@ -544,9 +557,12 @@ static int pinmux_functions_show(struct seq_file *s, void *what)
 
 		ret = pmxops->get_function_groups(pctldev, func_selector,
 						  &groups, &num_groups);
-		if (ret)
+		if (ret) {
 			seq_printf(s, "function %s: COULD NOT GET GROUPS\n",
 				   func);
+			func_selector++;
+			continue;
+		}
 
 		seq_printf(s, "function: %s, groups = [ ", func);
 		for (i = 0; i < num_groups; i++)
@@ -572,7 +588,12 @@ static int pinmux_pins_show(struct seq_file *s, void *what)
 		return 0;
 
 	seq_puts(s, "Pinmux settings per pin\n");
-	seq_puts(s, "Format: pin (name): mux_owner gpio_owner hog?\n");
+	if (pmxops->strict)
+		seq_puts(s,
+		 "Format: pin (name): mux_owner|gpio_owner (strict) hog?\n");
+	else
+		seq_puts(s,
+		"Format: pin (name): mux_owner gpio_owner hog?\n");
 
 	mutex_lock(&pctldev->mutex);
 
@@ -591,14 +612,34 @@ static int pinmux_pins_show(struct seq_file *s, void *what)
 		    !strcmp(desc->mux_owner, pinctrl_dev_get_name(pctldev)))
 			is_hog = true;
 
-		seq_printf(s, "pin %d (%s): %s %s%s", pin,
-			   desc->name ? desc->name : "unnamed",
-			   desc->mux_owner ? desc->mux_owner
-				: "(MUX UNCLAIMED)",
-			   desc->gpio_owner ? desc->gpio_owner
-				: "(GPIO UNCLAIMED)",
-			   is_hog ? " (HOG)" : "");
+		if (pmxops->strict) {
+			if (desc->mux_owner)
+				seq_printf(s, "pin %d (%s): device %s%s",
+					   pin,
+					   desc->name ? desc->name : "unnamed",
+					   desc->mux_owner,
+					   is_hog ? " (HOG)" : "");
+			else if (desc->gpio_owner)
+				seq_printf(s, "pin %d (%s): GPIO %s",
+					   pin,
+					   desc->name ? desc->name : "unnamed",
+					   desc->gpio_owner);
+			else
+				seq_printf(s, "pin %d (%s): UNCLAIMED",
+					   pin,
+					   desc->name ? desc->name : "unnamed");
+		} else {
+			/* For non-strict controllers */
+			seq_printf(s, "pin %d (%s): %s %s%s", pin,
+				   desc->name ? desc->name : "unnamed",
+				   desc->mux_owner ? desc->mux_owner
+				   : "(MUX UNCLAIMED)",
+				   desc->gpio_owner ? desc->gpio_owner
+				   : "(GPIO UNCLAIMED)",
+				   is_hog ? " (HOG)" : "");
+		}
 
+		/* If mux: print function+group claiming the pin */
 		if (desc->mux_setting)
 			seq_printf(s, " function %s group %s\n",
 				   pmxops->get_function_name(pctldev,
diff --git a/drivers/pinctrl/qcom/Kconfig b/drivers/pinctrl/qcom/Kconfig
index ea575f60f001..58f5632b27f4 100644
--- a/drivers/pinctrl/qcom/Kconfig
+++ b/drivers/pinctrl/qcom/Kconfig
@@ -31,6 +31,14 @@ config PINCTRL_IPQ8064
 	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
 	  Qualcomm TLMM block found in the Qualcomm IPQ8064 platform.
 
+config PINCTRL_MSM8660
+	tristate "Qualcomm 8660 pin controller driver"
+	depends on GPIOLIB && OF
+	select PINCTRL_MSM
+	help
+	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
+	  Qualcomm TLMM block found in the Qualcomm 8660 platform.
+
 config PINCTRL_MSM8960
 	tristate "Qualcomm 8960 pin controller driver"
 	depends on GPIOLIB && OF
diff --git a/drivers/pinctrl/qcom/Makefile b/drivers/pinctrl/qcom/Makefile
index 68958702917d..3666c703ce88 100644
--- a/drivers/pinctrl/qcom/Makefile
+++ b/drivers/pinctrl/qcom/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_PINCTRL_MSM)	+= pinctrl-msm.o
 obj-$(CONFIG_PINCTRL_APQ8064)	+= pinctrl-apq8064.o
 obj-$(CONFIG_PINCTRL_APQ8084)	+= pinctrl-apq8084.o
 obj-$(CONFIG_PINCTRL_IPQ8064)	+= pinctrl-ipq8064.o
+obj-$(CONFIG_PINCTRL_MSM8660)	+= pinctrl-msm8660.o
 obj-$(CONFIG_PINCTRL_MSM8960)	+= pinctrl-msm8960.o
 obj-$(CONFIG_PINCTRL_MSM8X74)	+= pinctrl-msm8x74.o
 obj-$(CONFIG_PINCTRL_MSM8916)	+= pinctrl-msm8916.o
diff --git a/drivers/pinctrl/qcom/pinctrl-msm.c b/drivers/pinctrl/qcom/pinctrl-msm.c
index f3d800f796c2..e457d52302a2 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm.c
@@ -906,9 +906,9 @@ int msm_pinctrl_probe(struct platform_device *pdev,
 	msm_pinctrl_desc.pins = pctrl->soc->pins;
 	msm_pinctrl_desc.npins = pctrl->soc->npins;
 	pctrl->pctrl = pinctrl_register(&msm_pinctrl_desc, &pdev->dev, pctrl);
-	if (!pctrl->pctrl) {
+	if (IS_ERR(pctrl->pctrl)) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(pctrl->pctrl);
 	}
 
 	ret = msm_gpio_init(pctrl);
diff --git a/drivers/pinctrl/qcom/pinctrl-msm8660.c b/drivers/pinctrl/qcom/pinctrl-msm8660.c
new file mode 100644
index 000000000000..3e8f7ac2ac8a
--- /dev/null
+++ b/drivers/pinctrl/qcom/pinctrl-msm8660.c
@@ -0,0 +1,984 @@
+/*
+ * Copyright (c) 2015, Sony Mobile Communications AB.
+ *
+ * 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/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-msm.h"
+
+static const struct pinctrl_pin_desc msm8660_pins[] = {
+	PINCTRL_PIN(0, "GPIO_0"),
+	PINCTRL_PIN(1, "GPIO_1"),
+	PINCTRL_PIN(2, "GPIO_2"),
+	PINCTRL_PIN(3, "GPIO_3"),
+	PINCTRL_PIN(4, "GPIO_4"),
+	PINCTRL_PIN(5, "GPIO_5"),
+	PINCTRL_PIN(6, "GPIO_6"),
+	PINCTRL_PIN(7, "GPIO_7"),
+	PINCTRL_PIN(8, "GPIO_8"),
+	PINCTRL_PIN(9, "GPIO_9"),
+	PINCTRL_PIN(10, "GPIO_10"),
+	PINCTRL_PIN(11, "GPIO_11"),
+	PINCTRL_PIN(12, "GPIO_12"),
+	PINCTRL_PIN(13, "GPIO_13"),
+	PINCTRL_PIN(14, "GPIO_14"),
+	PINCTRL_PIN(15, "GPIO_15"),
+	PINCTRL_PIN(16, "GPIO_16"),
+	PINCTRL_PIN(17, "GPIO_17"),
+	PINCTRL_PIN(18, "GPIO_18"),
+	PINCTRL_PIN(19, "GPIO_19"),
+	PINCTRL_PIN(20, "GPIO_20"),
+	PINCTRL_PIN(21, "GPIO_21"),
+	PINCTRL_PIN(22, "GPIO_22"),
+	PINCTRL_PIN(23, "GPIO_23"),
+	PINCTRL_PIN(24, "GPIO_24"),
+	PINCTRL_PIN(25, "GPIO_25"),
+	PINCTRL_PIN(26, "GPIO_26"),
+	PINCTRL_PIN(27, "GPIO_27"),
+	PINCTRL_PIN(28, "GPIO_28"),
+	PINCTRL_PIN(29, "GPIO_29"),
+	PINCTRL_PIN(30, "GPIO_30"),
+	PINCTRL_PIN(31, "GPIO_31"),
+	PINCTRL_PIN(32, "GPIO_32"),
+	PINCTRL_PIN(33, "GPIO_33"),
+	PINCTRL_PIN(34, "GPIO_34"),
+	PINCTRL_PIN(35, "GPIO_35"),
+	PINCTRL_PIN(36, "GPIO_36"),
+	PINCTRL_PIN(37, "GPIO_37"),
+	PINCTRL_PIN(38, "GPIO_38"),
+	PINCTRL_PIN(39, "GPIO_39"),
+	PINCTRL_PIN(40, "GPIO_40"),
+	PINCTRL_PIN(41, "GPIO_41"),
+	PINCTRL_PIN(42, "GPIO_42"),
+	PINCTRL_PIN(43, "GPIO_43"),
+	PINCTRL_PIN(44, "GPIO_44"),
+	PINCTRL_PIN(45, "GPIO_45"),
+	PINCTRL_PIN(46, "GPIO_46"),
+	PINCTRL_PIN(47, "GPIO_47"),
+	PINCTRL_PIN(48, "GPIO_48"),
+	PINCTRL_PIN(49, "GPIO_49"),
+	PINCTRL_PIN(50, "GPIO_50"),
+	PINCTRL_PIN(51, "GPIO_51"),
+	PINCTRL_PIN(52, "GPIO_52"),
+	PINCTRL_PIN(53, "GPIO_53"),
+	PINCTRL_PIN(54, "GPIO_54"),
+	PINCTRL_PIN(55, "GPIO_55"),
+	PINCTRL_PIN(56, "GPIO_56"),
+	PINCTRL_PIN(57, "GPIO_57"),
+	PINCTRL_PIN(58, "GPIO_58"),
+	PINCTRL_PIN(59, "GPIO_59"),
+	PINCTRL_PIN(60, "GPIO_60"),
+	PINCTRL_PIN(61, "GPIO_61"),
+	PINCTRL_PIN(62, "GPIO_62"),
+	PINCTRL_PIN(63, "GPIO_63"),
+	PINCTRL_PIN(64, "GPIO_64"),
+	PINCTRL_PIN(65, "GPIO_65"),
+	PINCTRL_PIN(66, "GPIO_66"),
+	PINCTRL_PIN(67, "GPIO_67"),
+	PINCTRL_PIN(68, "GPIO_68"),
+	PINCTRL_PIN(69, "GPIO_69"),
+	PINCTRL_PIN(70, "GPIO_70"),
+	PINCTRL_PIN(71, "GPIO_71"),
+	PINCTRL_PIN(72, "GPIO_72"),
+	PINCTRL_PIN(73, "GPIO_73"),
+	PINCTRL_PIN(74, "GPIO_74"),
+	PINCTRL_PIN(75, "GPIO_75"),
+	PINCTRL_PIN(76, "GPIO_76"),
+	PINCTRL_PIN(77, "GPIO_77"),
+	PINCTRL_PIN(78, "GPIO_78"),
+	PINCTRL_PIN(79, "GPIO_79"),
+	PINCTRL_PIN(80, "GPIO_80"),
+	PINCTRL_PIN(81, "GPIO_81"),
+	PINCTRL_PIN(82, "GPIO_82"),
+	PINCTRL_PIN(83, "GPIO_83"),
+	PINCTRL_PIN(84, "GPIO_84"),
+	PINCTRL_PIN(85, "GPIO_85"),
+	PINCTRL_PIN(86, "GPIO_86"),
+	PINCTRL_PIN(87, "GPIO_87"),
+	PINCTRL_PIN(88, "GPIO_88"),
+	PINCTRL_PIN(89, "GPIO_89"),
+	PINCTRL_PIN(90, "GPIO_90"),
+	PINCTRL_PIN(91, "GPIO_91"),
+	PINCTRL_PIN(92, "GPIO_92"),
+	PINCTRL_PIN(93, "GPIO_93"),
+	PINCTRL_PIN(94, "GPIO_94"),
+	PINCTRL_PIN(95, "GPIO_95"),
+	PINCTRL_PIN(96, "GPIO_96"),
+	PINCTRL_PIN(97, "GPIO_97"),
+	PINCTRL_PIN(98, "GPIO_98"),
+	PINCTRL_PIN(99, "GPIO_99"),
+	PINCTRL_PIN(100, "GPIO_100"),
+	PINCTRL_PIN(101, "GPIO_101"),
+	PINCTRL_PIN(102, "GPIO_102"),
+	PINCTRL_PIN(103, "GPIO_103"),
+	PINCTRL_PIN(104, "GPIO_104"),
+	PINCTRL_PIN(105, "GPIO_105"),
+	PINCTRL_PIN(106, "GPIO_106"),
+	PINCTRL_PIN(107, "GPIO_107"),
+	PINCTRL_PIN(108, "GPIO_108"),
+	PINCTRL_PIN(109, "GPIO_109"),
+	PINCTRL_PIN(110, "GPIO_110"),
+	PINCTRL_PIN(111, "GPIO_111"),
+	PINCTRL_PIN(112, "GPIO_112"),
+	PINCTRL_PIN(113, "GPIO_113"),
+	PINCTRL_PIN(114, "GPIO_114"),
+	PINCTRL_PIN(115, "GPIO_115"),
+	PINCTRL_PIN(116, "GPIO_116"),
+	PINCTRL_PIN(117, "GPIO_117"),
+	PINCTRL_PIN(118, "GPIO_118"),
+	PINCTRL_PIN(119, "GPIO_119"),
+	PINCTRL_PIN(120, "GPIO_120"),
+	PINCTRL_PIN(121, "GPIO_121"),
+	PINCTRL_PIN(122, "GPIO_122"),
+	PINCTRL_PIN(123, "GPIO_123"),
+	PINCTRL_PIN(124, "GPIO_124"),
+	PINCTRL_PIN(125, "GPIO_125"),
+	PINCTRL_PIN(126, "GPIO_126"),
+	PINCTRL_PIN(127, "GPIO_127"),
+	PINCTRL_PIN(128, "GPIO_128"),
+	PINCTRL_PIN(129, "GPIO_129"),
+	PINCTRL_PIN(130, "GPIO_130"),
+	PINCTRL_PIN(131, "GPIO_131"),
+	PINCTRL_PIN(132, "GPIO_132"),
+	PINCTRL_PIN(133, "GPIO_133"),
+	PINCTRL_PIN(134, "GPIO_134"),
+	PINCTRL_PIN(135, "GPIO_135"),
+	PINCTRL_PIN(136, "GPIO_136"),
+	PINCTRL_PIN(137, "GPIO_137"),
+	PINCTRL_PIN(138, "GPIO_138"),
+	PINCTRL_PIN(139, "GPIO_139"),
+	PINCTRL_PIN(140, "GPIO_140"),
+	PINCTRL_PIN(141, "GPIO_141"),
+	PINCTRL_PIN(142, "GPIO_142"),
+	PINCTRL_PIN(143, "GPIO_143"),
+	PINCTRL_PIN(144, "GPIO_144"),
+	PINCTRL_PIN(145, "GPIO_145"),
+	PINCTRL_PIN(146, "GPIO_146"),
+	PINCTRL_PIN(147, "GPIO_147"),
+	PINCTRL_PIN(148, "GPIO_148"),
+	PINCTRL_PIN(149, "GPIO_149"),
+	PINCTRL_PIN(150, "GPIO_150"),
+	PINCTRL_PIN(151, "GPIO_151"),
+	PINCTRL_PIN(152, "GPIO_152"),
+	PINCTRL_PIN(153, "GPIO_153"),
+	PINCTRL_PIN(154, "GPIO_154"),
+	PINCTRL_PIN(155, "GPIO_155"),
+	PINCTRL_PIN(156, "GPIO_156"),
+	PINCTRL_PIN(157, "GPIO_157"),
+	PINCTRL_PIN(158, "GPIO_158"),
+	PINCTRL_PIN(159, "GPIO_159"),
+	PINCTRL_PIN(160, "GPIO_160"),
+	PINCTRL_PIN(161, "GPIO_161"),
+	PINCTRL_PIN(162, "GPIO_162"),
+	PINCTRL_PIN(163, "GPIO_163"),
+	PINCTRL_PIN(164, "GPIO_164"),
+	PINCTRL_PIN(165, "GPIO_165"),
+	PINCTRL_PIN(166, "GPIO_166"),
+	PINCTRL_PIN(167, "GPIO_167"),
+	PINCTRL_PIN(168, "GPIO_168"),
+	PINCTRL_PIN(169, "GPIO_169"),
+	PINCTRL_PIN(170, "GPIO_170"),
+	PINCTRL_PIN(171, "GPIO_171"),
+	PINCTRL_PIN(172, "GPIO_172"),
+
+	PINCTRL_PIN(173, "SDC1_CLK"),
+	PINCTRL_PIN(174, "SDC1_CMD"),
+	PINCTRL_PIN(175, "SDC1_DATA"),
+	PINCTRL_PIN(176, "SDC3_CLK"),
+	PINCTRL_PIN(177, "SDC3_CMD"),
+	PINCTRL_PIN(178, "SDC3_DATA"),
+};
+
+#define DECLARE_MSM_GPIO_PIN(pin) static const unsigned int gpio##pin##_pins[] = { pin }
+DECLARE_MSM_GPIO_PIN(0);
+DECLARE_MSM_GPIO_PIN(1);
+DECLARE_MSM_GPIO_PIN(2);
+DECLARE_MSM_GPIO_PIN(3);
+DECLARE_MSM_GPIO_PIN(4);
+DECLARE_MSM_GPIO_PIN(5);
+DECLARE_MSM_GPIO_PIN(6);
+DECLARE_MSM_GPIO_PIN(7);
+DECLARE_MSM_GPIO_PIN(8);
+DECLARE_MSM_GPIO_PIN(9);
+DECLARE_MSM_GPIO_PIN(10);
+DECLARE_MSM_GPIO_PIN(11);
+DECLARE_MSM_GPIO_PIN(12);
+DECLARE_MSM_GPIO_PIN(13);
+DECLARE_MSM_GPIO_PIN(14);
+DECLARE_MSM_GPIO_PIN(15);
+DECLARE_MSM_GPIO_PIN(16);
+DECLARE_MSM_GPIO_PIN(17);
+DECLARE_MSM_GPIO_PIN(18);
+DECLARE_MSM_GPIO_PIN(19);
+DECLARE_MSM_GPIO_PIN(20);
+DECLARE_MSM_GPIO_PIN(21);
+DECLARE_MSM_GPIO_PIN(22);
+DECLARE_MSM_GPIO_PIN(23);
+DECLARE_MSM_GPIO_PIN(24);
+DECLARE_MSM_GPIO_PIN(25);
+DECLARE_MSM_GPIO_PIN(26);
+DECLARE_MSM_GPIO_PIN(27);
+DECLARE_MSM_GPIO_PIN(28);
+DECLARE_MSM_GPIO_PIN(29);
+DECLARE_MSM_GPIO_PIN(30);
+DECLARE_MSM_GPIO_PIN(31);
+DECLARE_MSM_GPIO_PIN(32);
+DECLARE_MSM_GPIO_PIN(33);
+DECLARE_MSM_GPIO_PIN(34);
+DECLARE_MSM_GPIO_PIN(35);
+DECLARE_MSM_GPIO_PIN(36);
+DECLARE_MSM_GPIO_PIN(37);
+DECLARE_MSM_GPIO_PIN(38);
+DECLARE_MSM_GPIO_PIN(39);
+DECLARE_MSM_GPIO_PIN(40);
+DECLARE_MSM_GPIO_PIN(41);
+DECLARE_MSM_GPIO_PIN(42);
+DECLARE_MSM_GPIO_PIN(43);
+DECLARE_MSM_GPIO_PIN(44);
+DECLARE_MSM_GPIO_PIN(45);
+DECLARE_MSM_GPIO_PIN(46);
+DECLARE_MSM_GPIO_PIN(47);
+DECLARE_MSM_GPIO_PIN(48);
+DECLARE_MSM_GPIO_PIN(49);
+DECLARE_MSM_GPIO_PIN(50);
+DECLARE_MSM_GPIO_PIN(51);
+DECLARE_MSM_GPIO_PIN(52);
+DECLARE_MSM_GPIO_PIN(53);
+DECLARE_MSM_GPIO_PIN(54);
+DECLARE_MSM_GPIO_PIN(55);
+DECLARE_MSM_GPIO_PIN(56);
+DECLARE_MSM_GPIO_PIN(57);
+DECLARE_MSM_GPIO_PIN(58);
+DECLARE_MSM_GPIO_PIN(59);
+DECLARE_MSM_GPIO_PIN(60);
+DECLARE_MSM_GPIO_PIN(61);
+DECLARE_MSM_GPIO_PIN(62);
+DECLARE_MSM_GPIO_PIN(63);
+DECLARE_MSM_GPIO_PIN(64);
+DECLARE_MSM_GPIO_PIN(65);
+DECLARE_MSM_GPIO_PIN(66);
+DECLARE_MSM_GPIO_PIN(67);
+DECLARE_MSM_GPIO_PIN(68);
+DECLARE_MSM_GPIO_PIN(69);
+DECLARE_MSM_GPIO_PIN(70);
+DECLARE_MSM_GPIO_PIN(71);
+DECLARE_MSM_GPIO_PIN(72);
+DECLARE_MSM_GPIO_PIN(73);
+DECLARE_MSM_GPIO_PIN(74);
+DECLARE_MSM_GPIO_PIN(75);
+DECLARE_MSM_GPIO_PIN(76);
+DECLARE_MSM_GPIO_PIN(77);
+DECLARE_MSM_GPIO_PIN(78);
+DECLARE_MSM_GPIO_PIN(79);
+DECLARE_MSM_GPIO_PIN(80);
+DECLARE_MSM_GPIO_PIN(81);
+DECLARE_MSM_GPIO_PIN(82);
+DECLARE_MSM_GPIO_PIN(83);
+DECLARE_MSM_GPIO_PIN(84);
+DECLARE_MSM_GPIO_PIN(85);
+DECLARE_MSM_GPIO_PIN(86);
+DECLARE_MSM_GPIO_PIN(87);
+DECLARE_MSM_GPIO_PIN(88);
+DECLARE_MSM_GPIO_PIN(89);
+DECLARE_MSM_GPIO_PIN(90);
+DECLARE_MSM_GPIO_PIN(91);
+DECLARE_MSM_GPIO_PIN(92);
+DECLARE_MSM_GPIO_PIN(93);
+DECLARE_MSM_GPIO_PIN(94);
+DECLARE_MSM_GPIO_PIN(95);
+DECLARE_MSM_GPIO_PIN(96);
+DECLARE_MSM_GPIO_PIN(97);
+DECLARE_MSM_GPIO_PIN(98);
+DECLARE_MSM_GPIO_PIN(99);
+DECLARE_MSM_GPIO_PIN(100);
+DECLARE_MSM_GPIO_PIN(101);
+DECLARE_MSM_GPIO_PIN(102);
+DECLARE_MSM_GPIO_PIN(103);
+DECLARE_MSM_GPIO_PIN(104);
+DECLARE_MSM_GPIO_PIN(105);
+DECLARE_MSM_GPIO_PIN(106);
+DECLARE_MSM_GPIO_PIN(107);
+DECLARE_MSM_GPIO_PIN(108);
+DECLARE_MSM_GPIO_PIN(109);
+DECLARE_MSM_GPIO_PIN(110);
+DECLARE_MSM_GPIO_PIN(111);
+DECLARE_MSM_GPIO_PIN(112);
+DECLARE_MSM_GPIO_PIN(113);
+DECLARE_MSM_GPIO_PIN(114);
+DECLARE_MSM_GPIO_PIN(115);
+DECLARE_MSM_GPIO_PIN(116);
+DECLARE_MSM_GPIO_PIN(117);
+DECLARE_MSM_GPIO_PIN(118);
+DECLARE_MSM_GPIO_PIN(119);
+DECLARE_MSM_GPIO_PIN(120);
+DECLARE_MSM_GPIO_PIN(121);
+DECLARE_MSM_GPIO_PIN(122);
+DECLARE_MSM_GPIO_PIN(123);
+DECLARE_MSM_GPIO_PIN(124);
+DECLARE_MSM_GPIO_PIN(125);
+DECLARE_MSM_GPIO_PIN(126);
+DECLARE_MSM_GPIO_PIN(127);
+DECLARE_MSM_GPIO_PIN(128);
+DECLARE_MSM_GPIO_PIN(129);
+DECLARE_MSM_GPIO_PIN(130);
+DECLARE_MSM_GPIO_PIN(131);
+DECLARE_MSM_GPIO_PIN(132);
+DECLARE_MSM_GPIO_PIN(133);
+DECLARE_MSM_GPIO_PIN(134);
+DECLARE_MSM_GPIO_PIN(135);
+DECLARE_MSM_GPIO_PIN(136);
+DECLARE_MSM_GPIO_PIN(137);
+DECLARE_MSM_GPIO_PIN(138);
+DECLARE_MSM_GPIO_PIN(139);
+DECLARE_MSM_GPIO_PIN(140);
+DECLARE_MSM_GPIO_PIN(141);
+DECLARE_MSM_GPIO_PIN(142);
+DECLARE_MSM_GPIO_PIN(143);
+DECLARE_MSM_GPIO_PIN(144);
+DECLARE_MSM_GPIO_PIN(145);
+DECLARE_MSM_GPIO_PIN(146);
+DECLARE_MSM_GPIO_PIN(147);
+DECLARE_MSM_GPIO_PIN(148);
+DECLARE_MSM_GPIO_PIN(149);
+DECLARE_MSM_GPIO_PIN(150);
+DECLARE_MSM_GPIO_PIN(151);
+DECLARE_MSM_GPIO_PIN(152);
+DECLARE_MSM_GPIO_PIN(153);
+DECLARE_MSM_GPIO_PIN(154);
+DECLARE_MSM_GPIO_PIN(155);
+DECLARE_MSM_GPIO_PIN(156);
+DECLARE_MSM_GPIO_PIN(157);
+DECLARE_MSM_GPIO_PIN(158);
+DECLARE_MSM_GPIO_PIN(159);
+DECLARE_MSM_GPIO_PIN(160);
+DECLARE_MSM_GPIO_PIN(161);
+DECLARE_MSM_GPIO_PIN(162);
+DECLARE_MSM_GPIO_PIN(163);
+DECLARE_MSM_GPIO_PIN(164);
+DECLARE_MSM_GPIO_PIN(165);
+DECLARE_MSM_GPIO_PIN(166);
+DECLARE_MSM_GPIO_PIN(167);
+DECLARE_MSM_GPIO_PIN(168);
+DECLARE_MSM_GPIO_PIN(169);
+DECLARE_MSM_GPIO_PIN(170);
+DECLARE_MSM_GPIO_PIN(171);
+DECLARE_MSM_GPIO_PIN(172);
+
+static const unsigned int sdc4_clk_pins[] = { 173 };
+static const unsigned int sdc4_cmd_pins[] = { 174 };
+static const unsigned int sdc4_data_pins[] = { 175 };
+static const unsigned int sdc3_clk_pins[] = { 176 };
+static const unsigned int sdc3_cmd_pins[] = { 177 };
+static const unsigned int sdc3_data_pins[] = { 178 };
+
+#define FUNCTION(fname)					\
+	[MSM_MUX_##fname] = {				\
+		.name = #fname,				\
+		.groups = fname##_groups,		\
+		.ngroups = ARRAY_SIZE(fname##_groups),	\
+	}
+
+#define PINGROUP(id, f1, f2, f3, f4, f5, f6, f7) \
+	{						\
+		.name = "gpio" #id,			\
+		.pins = gpio##id##_pins,		\
+		.npins = ARRAY_SIZE(gpio##id##_pins),	\
+		.funcs = (int[]){			\
+			MSM_MUX_gpio,			\
+			MSM_MUX_##f1,			\
+			MSM_MUX_##f2,			\
+			MSM_MUX_##f3,			\
+			MSM_MUX_##f4,			\
+			MSM_MUX_##f5,			\
+			MSM_MUX_##f6,			\
+			MSM_MUX_##f7,			\
+		},					\
+		.nfuncs = 8,				\
+		.ctl_reg = 0x1000 + 0x10 * id,		\
+		.io_reg = 0x1004 + 0x10 * id,		\
+		.intr_cfg_reg = 0x1008 + 0x10 * id,	\
+		.intr_status_reg = 0x100c + 0x10 * id,	\
+		.intr_target_reg = 0x400 + 0x4 * id,	\
+		.mux_bit = 2,				\
+		.pull_bit = 0,				\
+		.drv_bit = 6,				\
+		.oe_bit = 9,				\
+		.in_bit = 0,				\
+		.out_bit = 1,				\
+		.intr_enable_bit = 0,			\
+		.intr_status_bit = 0,			\
+		.intr_ack_high = 1,			\
+		.intr_target_bit = 0,			\
+		.intr_target_kpss_val = 4,		\
+		.intr_raw_status_bit = 3,		\
+		.intr_polarity_bit = 1,			\
+		.intr_detection_bit = 2,		\
+		.intr_detection_width = 1,		\
+	}
+
+#define SDC_PINGROUP(pg_name, ctl, pull, drv)		\
+	{						\
+		.name = #pg_name,			\
+		.pins = pg_name##_pins,			\
+		.npins = ARRAY_SIZE(pg_name##_pins),	\
+		.ctl_reg = ctl,				\
+		.io_reg = 0,				\
+		.intr_cfg_reg = 0,			\
+		.intr_status_reg = 0,			\
+		.intr_target_reg = 0,			\
+		.mux_bit = -1,				\
+		.pull_bit = pull,			\
+		.drv_bit = drv,				\
+		.oe_bit = -1,				\
+		.in_bit = -1,				\
+		.out_bit = -1,				\
+		.intr_enable_bit = -1,			\
+		.intr_status_bit = -1,			\
+		.intr_target_bit = -1,			\
+		.intr_target_kpss_val = -1,		\
+		.intr_raw_status_bit = -1,		\
+		.intr_polarity_bit = -1,		\
+		.intr_detection_bit = -1,		\
+		.intr_detection_width = -1,		\
+	}
+
+enum msm8660_functions {
+	MSM_MUX_gpio,
+	MSM_MUX_cam_mclk,
+	MSM_MUX_dsub,
+	MSM_MUX_ext_gps,
+	MSM_MUX_gp_clk_0a,
+	MSM_MUX_gp_clk_0b,
+	MSM_MUX_gp_clk_1a,
+	MSM_MUX_gp_clk_1b,
+	MSM_MUX_gp_clk_2a,
+	MSM_MUX_gp_clk_2b,
+	MSM_MUX_gp_mn,
+	MSM_MUX_gsbi1,
+	MSM_MUX_gsbi1_spi_cs1_n,
+	MSM_MUX_gsbi1_spi_cs2a_n,
+	MSM_MUX_gsbi1_spi_cs2b_n,
+	MSM_MUX_gsbi1_spi_cs3_n,
+	MSM_MUX_gsbi2,
+	MSM_MUX_gsbi2_spi_cs1_n,
+	MSM_MUX_gsbi2_spi_cs2_n,
+	MSM_MUX_gsbi2_spi_cs3_n,
+	MSM_MUX_gsbi3,
+	MSM_MUX_gsbi3_spi_cs1_n,
+	MSM_MUX_gsbi3_spi_cs2_n,
+	MSM_MUX_gsbi3_spi_cs3_n,
+	MSM_MUX_gsbi4,
+	MSM_MUX_gsbi5,
+	MSM_MUX_gsbi6,
+	MSM_MUX_gsbi7,
+	MSM_MUX_gsbi8,
+	MSM_MUX_gsbi9,
+	MSM_MUX_gsbi10,
+	MSM_MUX_gsbi11,
+	MSM_MUX_gsbi12,
+	MSM_MUX_hdmi,
+	MSM_MUX_i2s,
+	MSM_MUX_lcdc,
+	MSM_MUX_mdp_vsync,
+	MSM_MUX_mi2s,
+	MSM_MUX_pcm,
+	MSM_MUX_ps_hold,
+	MSM_MUX_sdc1,
+	MSM_MUX_sdc2,
+	MSM_MUX_sdc5,
+	MSM_MUX_tsif1,
+	MSM_MUX_tsif2,
+	MSM_MUX_usb_fs1,
+	MSM_MUX_usb_fs1_oe_n,
+	MSM_MUX_usb_fs2,
+	MSM_MUX_usb_fs2_oe_n,
+	MSM_MUX_vfe,
+	MSM_MUX_vsens_alarm,
+	MSM_MUX__,
+};
+
+static const char * const gpio_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3", "gpio4", "gpio5", "gpio6", "gpio7",
+	"gpio8", "gpio9", "gpio10", "gpio11", "gpio12", "gpio13", "gpio14",
+	"gpio15", "gpio16", "gpio17", "gpio18", "gpio19", "gpio20", "gpio21",
+	"gpio22", "gpio23", "gpio24", "gpio25", "gpio26", "gpio27", "gpio28",
+	"gpio29", "gpio30", "gpio31", "gpio32", "gpio33", "gpio34", "gpio35",
+	"gpio36", "gpio37", "gpio38", "gpio39", "gpio40", "gpio41", "gpio42",
+	"gpio43", "gpio44", "gpio45", "gpio46", "gpio47", "gpio48", "gpio49",
+	"gpio50", "gpio51", "gpio52", "gpio53", "gpio54", "gpio55", "gpio56",
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62", "gpio63",
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68", "gpio69", "gpio70",
+	"gpio71", "gpio72", "gpio73", "gpio74", "gpio75", "gpio76", "gpio77",
+	"gpio78", "gpio79", "gpio80", "gpio81", "gpio82", "gpio83", "gpio84",
+	"gpio85", "gpio86", "gpio87", "gpio88", "gpio89", "gpio90", "gpio91",
+	"gpio92", "gpio93", "gpio94", "gpio95", "gpio96", "gpio97", "gpio98",
+	"gpio99", "gpio100", "gpio101", "gpio102", "gpio103", "gpio104",
+	"gpio105", "gpio106", "gpio107", "gpio108", "gpio109", "gpio110",
+	"gpio111", "gpio112", "gpio113", "gpio114", "gpio115", "gpio116",
+	"gpio117", "gpio118", "gpio119", "gpio120", "gpio121", "gpio122",
+	"gpio123", "gpio124", "gpio125", "gpio126", "gpio127", "gpio128",
+	"gpio129", "gpio130", "gpio131", "gpio132", "gpio133", "gpio134",
+	"gpio135", "gpio136", "gpio137", "gpio138", "gpio139", "gpio140",
+	"gpio141", "gpio142", "gpio143", "gpio144", "gpio145", "gpio146",
+	"gpio147", "gpio148", "gpio149", "gpio150", "gpio151", "gpio152",
+	"gpio153", "gpio154", "gpio155", "gpio156", "gpio157", "gpio158",
+	"gpio159", "gpio160", "gpio161", "gpio162", "gpio163", "gpio164",
+	"gpio165", "gpio166", "gpio167", "gpio168", "gpio169", "gpio170",
+	"gpio171", "gpio172"
+};
+
+static const char * const cam_mclk_groups[] = {
+	"gpio32"
+};
+static const char * const dsub_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3", "gpio4", "gpio5", "gpio6", "gpio7",
+	"gpio8", "gpio9", "gpio10", "gpio11", "gpio12", "gpio13", "gpio14",
+	"gpio15", "gpio16", "gpio17", "gpio18", "gpio19", "gpio20", "gpio21",
+	"gpio22", "gpio23", "gpio24", "gpio25", "gpio26", "gpio27"
+};
+static const char * const ext_gps_groups[] = {
+	"gpio66", "gpio67", "gpio68", "gpio69"
+};
+static const char * const gp_clk_0a_groups[] = {
+	"gpio30"
+};
+static const char * const gp_clk_0b_groups[] = {
+	"gpio115"
+};
+static const char * const gp_clk_1a_groups[] = {
+	"gpio31"
+};
+static const char * const gp_clk_1b_groups[] = {
+	"gpio122"
+};
+static const char * const gp_clk_2a_groups[] = {
+	"gpio103"
+};
+static const char * const gp_clk_2b_groups[] = {
+	"gpio70"
+};
+static const char * const gp_mn_groups[] = {
+	"gpio29"
+};
+static const char * const gsbi1_groups[] = {
+	"gpio33", "gpio34", "gpio35", "gpio36"
+};
+static const char * const gsbi1_spi_cs1_n_groups[] = {
+};
+static const char * const gsbi1_spi_cs2a_n_groups[] = {
+};
+static const char * const gsbi1_spi_cs2b_n_groups[] = {
+};
+static const char * const gsbi1_spi_cs3_n_groups[] = {
+};
+static const char * const gsbi2_groups[] = {
+	"gpio37", "gpio38", "gpio39", "gpio40"
+};
+static const char * const gsbi2_spi_cs1_n_groups[] = {
+	"gpio123"
+};
+static const char * const gsbi2_spi_cs2_n_groups[] = {
+	"gpio124"
+};
+static const char * const gsbi2_spi_cs3_n_groups[] = {
+	"gpio125"
+};
+static const char * const gsbi3_groups[] = {
+	"gpio41", "gpio42", "gpio43", "gpio44"
+};
+static const char * const gsbi3_spi_cs1_n_groups[] = {
+	"gpio62"
+};
+static const char * const gsbi3_spi_cs2_n_groups[] = {
+	"gpio45"
+};
+static const char * const gsbi3_spi_cs3_n_groups[] = {
+	"gpio46"
+};
+static const char * const gsbi4_groups[] = {
+	"gpio45", "gpio56", "gpio47", "gpio48"
+};
+static const char * const gsbi5_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52"
+};
+static const char * const gsbi6_groups[] = {
+	"gpio53", "gpio54", "gpio55", "gpio56"
+};
+static const char * const gsbi7_groups[] = {
+	"gpio57", "gpio58", "gpio59", "gpio60"
+};
+static const char * const gsbi8_groups[] = {
+	"gpio62", "gpio63", "gpio64", "gpio65"
+};
+static const char * const gsbi9_groups[] = {
+	"gpio66", "gpio67", "gpio68", "gpio69"
+};
+static const char * const gsbi10_groups[] = {
+	"gpio70", "gpio71", "gpio72", "gpio73"
+};
+static const char * const gsbi11_groups[] = {
+	"gpio103", "gpio104", "gpio105", "gpio106"
+};
+static const char * const gsbi12_groups[] = {
+	"gpio115", "gpio116", "gpio117", "gpio118"
+};
+static const char * const hdmi_groups[] = {
+	"gpio169", "gpio170", "gpio171", "gpio172"
+};
+static const char * const i2s_groups[] = {
+	"gpio108", "gpio109", "gpio110", "gpio115", "gpio116", "gpio117",
+	"gpio118", "gpio119", "gpio120", "gpio121", "gpio122"
+};
+static const char * const lcdc_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3", "gpio4", "gpio5", "gpio6", "gpio7",
+	"gpio8", "gpio9", "gpio10", "gpio11", "gpio12", "gpio13", "gpio14",
+	"gpio15", "gpio16", "gpio17", "gpio18", "gpio19", "gpio20", "gpio21",
+	"gpio22", "gpio23", "gpio24", "gpio25", "gpio26", "gpio27"
+};
+static const char * const mdp_vsync_groups[] = {
+	"gpio28", "gpio39", "gpio41"
+};
+static const char * const mi2s_groups[] = {
+	"gpio101", "gpio102", "gpio103", "gpio104", "gpio105", "gpio106",
+	"gpio107"
+};
+static const char * const pcm_groups[] = {
+	"gpio111", "gpio112", "gpio113", "gpio114"
+};
+static const char * const ps_hold_groups[] = {
+	"gpio92"
+};
+static const char * const sdc1_groups[] = {
+	"gpio159", "gpio160", "gpio161", "gpio162", "gpio163", "gpio164",
+	"gpio165", "gpio166", "gpio167", "gpio168"
+};
+static const char * const sdc2_groups[] = {
+	"gpio143", "gpio144", "gpio145", "gpio146", "gpio147", "gpio148",
+	"gpio149", "gpio150", "gpio151", "gpio152"
+};
+static const char * const sdc5_groups[] = {
+	"gpio95", "gpio96", "gpio97", "gpio98", "gpio99", "gpio100"
+};
+static const char * const tsif1_groups[] = {
+	"gpio93", "gpio94", "gpio95", "gpio96"
+};
+static const char * const tsif2_groups[] = {
+	"gpio97", "gpio98", "gpio99", "gpio100"
+};
+static const char * const usb_fs1_groups[] = {
+	"gpio49", "gpio50", "gpio51"
+};
+static const char * const usb_fs1_oe_n_groups[] = {
+	"gpio51"
+};
+static const char * const usb_fs2_groups[] = {
+	"gpio71", "gpio72", "gpio73"
+};
+static const char * const usb_fs2_oe_n_groups[] = {
+	"gpio73"
+};
+static const char * const vfe_groups[] = {
+	"gpio29", "gpio30", "gpio31", "gpio42", "gpio46", "gpio105", "gpio106",
+	"gpio117"
+};
+static const char * const vsens_alarm_groups[] = {
+	"gpio127"
+};
+
+static const struct msm_function msm8660_functions[] = {
+	FUNCTION(gpio),
+	FUNCTION(cam_mclk),
+	FUNCTION(dsub),
+	FUNCTION(ext_gps),
+	FUNCTION(gp_clk_0a),
+	FUNCTION(gp_clk_0b),
+	FUNCTION(gp_clk_1a),
+	FUNCTION(gp_clk_1b),
+	FUNCTION(gp_clk_2a),
+	FUNCTION(gp_clk_2b),
+	FUNCTION(gp_mn),
+	FUNCTION(gsbi1),
+	FUNCTION(gsbi1_spi_cs1_n),
+	FUNCTION(gsbi1_spi_cs2a_n),
+	FUNCTION(gsbi1_spi_cs2b_n),
+	FUNCTION(gsbi1_spi_cs3_n),
+	FUNCTION(gsbi2),
+	FUNCTION(gsbi2_spi_cs1_n),
+	FUNCTION(gsbi2_spi_cs2_n),
+	FUNCTION(gsbi2_spi_cs3_n),
+	FUNCTION(gsbi3),
+	FUNCTION(gsbi3_spi_cs1_n),
+	FUNCTION(gsbi3_spi_cs2_n),
+	FUNCTION(gsbi3_spi_cs3_n),
+	FUNCTION(gsbi4),
+	FUNCTION(gsbi5),
+	FUNCTION(gsbi6),
+	FUNCTION(gsbi7),
+	FUNCTION(gsbi8),
+	FUNCTION(gsbi9),
+	FUNCTION(gsbi10),
+	FUNCTION(gsbi11),
+	FUNCTION(gsbi12),
+	FUNCTION(hdmi),
+	FUNCTION(i2s),
+	FUNCTION(lcdc),
+	FUNCTION(mdp_vsync),
+	FUNCTION(mi2s),
+	FUNCTION(pcm),
+	FUNCTION(ps_hold),
+	FUNCTION(sdc1),
+	FUNCTION(sdc2),
+	FUNCTION(sdc5),
+	FUNCTION(tsif1),
+	FUNCTION(tsif2),
+	FUNCTION(usb_fs1),
+	FUNCTION(usb_fs1_oe_n),
+	FUNCTION(usb_fs2),
+	FUNCTION(usb_fs2_oe_n),
+	FUNCTION(vfe),
+	FUNCTION(vsens_alarm),
+};
+
+static const struct msm_pingroup msm8660_groups[] = {
+	PINGROUP(0, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(1, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(2, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(3, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(4, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(5, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(6, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(7, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(8, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(9, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(10, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(11, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(12, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(13, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(14, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(15, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(16, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(17, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(18, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(19, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(20, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(21, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(22, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(23, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(24, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(25, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(26, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(27, lcdc, dsub, _, _, _, _, _),
+	PINGROUP(28, mdp_vsync, _, _, _, _, _, _),
+	PINGROUP(29, vfe, gp_mn, _, _, _, _, _),
+	PINGROUP(30, vfe, gp_clk_0a, _, _, _, _, _),
+	PINGROUP(31, vfe, gp_clk_1a, _, _, _, _, _),
+	PINGROUP(32, cam_mclk, _, _, _, _, _, _),
+	PINGROUP(33, gsbi1, _, _, _, _, _, _),
+	PINGROUP(34, gsbi1, _, _, _, _, _, _),
+	PINGROUP(35, gsbi1, _, _, _, _, _, _),
+	PINGROUP(36, gsbi1, _, _, _, _, _, _),
+	PINGROUP(37, gsbi2, _, _, _, _, _, _),
+	PINGROUP(38, gsbi2, _, _, _, _, _, _),
+	PINGROUP(39, gsbi2, _, mdp_vsync, _, _, _, _),
+	PINGROUP(40, gsbi2, _, _, _, _, _, _),
+	PINGROUP(41, gsbi3, mdp_vsync, _, _, _, _, _),
+	PINGROUP(42, gsbi3, vfe, _, _, _, _, _),
+	PINGROUP(43, gsbi3, _, _, _, _, _, _),
+	PINGROUP(44, gsbi3, _, _, _, _, _, _),
+	PINGROUP(45, gsbi4, gsbi3_spi_cs2_n, _, _, _, _, _),
+	PINGROUP(46, gsbi4, gsbi3_spi_cs3_n, vfe, _, _, _, _),
+	PINGROUP(47, gsbi4, _, _, _, _, _, _),
+	PINGROUP(48, gsbi4, _, _, _, _, _, _),
+	PINGROUP(49, gsbi5, usb_fs1, _, _, _, _, _),
+	PINGROUP(50, gsbi5, usb_fs1, _, _, _, _, _),
+	PINGROUP(51, gsbi5, usb_fs1, usb_fs1_oe_n, _, _, _, _),
+	PINGROUP(52, gsbi5, _, _, _, _, _, _),
+	PINGROUP(53, gsbi6, _, _, _, _, _, _),
+	PINGROUP(54, gsbi6, _, _, _, _, _, _),
+	PINGROUP(55, gsbi6, _, _, _, _, _, _),
+	PINGROUP(56, gsbi6, _, _, _, _, _, _),
+	PINGROUP(57, gsbi7, _, _, _, _, _, _),
+	PINGROUP(58, gsbi7, _, _, _, _, _, _),
+	PINGROUP(59, gsbi7, _, _, _, _, _, _),
+	PINGROUP(60, gsbi7, _, _, _, _, _, _),
+	PINGROUP(61, _, _, _, _, _, _, _),
+	PINGROUP(62, gsbi8, gsbi3_spi_cs1_n, gsbi1_spi_cs2a_n, _, _, _, _),
+	PINGROUP(63, gsbi8, gsbi1_spi_cs1_n, _, _, _, _, _),
+	PINGROUP(64, gsbi8, gsbi1_spi_cs2b_n, _, _, _, _, _),
+	PINGROUP(65, gsbi8, gsbi1_spi_cs3_n, _, _, _, _, _),
+	PINGROUP(66, gsbi9, ext_gps, _, _, _, _, _),
+	PINGROUP(67, gsbi9, ext_gps, _, _, _, _, _),
+	PINGROUP(68, gsbi9, ext_gps, _, _, _, _, _),
+	PINGROUP(69, gsbi9, ext_gps, _, _, _, _, _),
+	PINGROUP(70, gsbi10, gp_clk_2b, _, _, _, _, _),
+	PINGROUP(71, gsbi10, usb_fs2, _, _, _, _, _),
+	PINGROUP(72, gsbi10, usb_fs2, _, _, _, _, _),
+	PINGROUP(73, gsbi10, usb_fs2, usb_fs2_oe_n, _, _, _, _),
+	PINGROUP(74, _, _, _, _, _, _, _),
+	PINGROUP(75, _, _, _, _, _, _, _),
+	PINGROUP(76, _, _, _, _, _, _, _),
+	PINGROUP(77, _, _, _, _, _, _, _),
+	PINGROUP(78, _, _, _, _, _, _, _),
+	PINGROUP(79, _, _, _, _, _, _, _),
+	PINGROUP(80, _, _, _, _, _, _, _),
+	PINGROUP(81, _, _, _, _, _, _, _),
+	PINGROUP(82, _, _, _, _, _, _, _),
+	PINGROUP(83, _, _, _, _, _, _, _),
+	PINGROUP(84, _, _, _, _, _, _, _),
+	PINGROUP(85, _, _, _, _, _, _, _),
+	PINGROUP(86, _, _, _, _, _, _, _),
+	PINGROUP(87, _, _, _, _, _, _, _),
+	PINGROUP(88, _, _, _, _, _, _, _),
+	PINGROUP(89, _, _, _, _, _, _, _),
+	PINGROUP(90, _, _, _, _, _, _, _),
+	PINGROUP(91, _, _, _, _, _, _, _),
+	PINGROUP(92, ps_hold, _, _, _, _, _, _),
+	PINGROUP(93, tsif1, _, _, _, _, _, _),
+	PINGROUP(94, tsif1, _, _, _, _, _, _),
+	PINGROUP(95, tsif1, sdc5, _, _, _, _, _),
+	PINGROUP(96, tsif1, sdc5, _, _, _, _, _),
+	PINGROUP(97, tsif2, sdc5, _, _, _, _, _),
+	PINGROUP(98, tsif2, sdc5, _, _, _, _, _),
+	PINGROUP(99, tsif2, sdc5, _, _, _, _, _),
+	PINGROUP(100, tsif2, sdc5, _, _, _, _, _),
+	PINGROUP(101, mi2s, _, _, _, _, _, _),
+	PINGROUP(102, mi2s, _, _, _, _, _, _),
+	PINGROUP(103, mi2s, gsbi11, gp_clk_2a, _, _, _, _),
+	PINGROUP(104, mi2s, gsbi11, _, _, _, _, _),
+	PINGROUP(105, mi2s, gsbi11, vfe, _, _, _, _),
+	PINGROUP(106, mi2s, gsbi11, vfe, _, _, _, _),
+	PINGROUP(107, mi2s, _, _, _, _, _, _),
+	PINGROUP(108, i2s, _, _, _, _, _, _),
+	PINGROUP(109, i2s, _, _, _, _, _, _),
+	PINGROUP(110, i2s, _, _, _, _, _, _),
+	PINGROUP(111, pcm, _, _, _, _, _, _),
+	PINGROUP(112, pcm, _, _, _, _, _, _),
+	PINGROUP(113, pcm, _, _, _, _, _, _),
+	PINGROUP(114, pcm, _, _, _, _, _, _),
+	PINGROUP(115, i2s, gsbi12, gp_clk_0b, _, _, _, _),
+	PINGROUP(116, i2s, gsbi12, _, _, _, _, _),
+	PINGROUP(117, i2s, gsbi12, vfe, _, _, _, _),
+	PINGROUP(118, i2s, gsbi12, _, _, _, _, _),
+	PINGROUP(119, i2s, _, _, _, _, _, _),
+	PINGROUP(120, i2s, _, _, _, _, _, _),
+	PINGROUP(121, i2s, _, _, _, _, _, _),
+	PINGROUP(122, i2s, gp_clk_1b, _, _, _, _, _),
+	PINGROUP(123, _, gsbi2_spi_cs1_n, _, _, _, _, _),
+	PINGROUP(124, _, gsbi2_spi_cs2_n, _, _, _, _, _),
+	PINGROUP(125, _, gsbi2_spi_cs3_n, _, _, _, _, _),
+	PINGROUP(126, _, _, _, _, _, _, _),
+	PINGROUP(127, _, vsens_alarm, _, _, _, _, _),
+	PINGROUP(128, _, _, _, _, _, _, _),
+	PINGROUP(129, _, _, _, _, _, _, _),
+	PINGROUP(130, _, _, _, _, _, _, _),
+	PINGROUP(131, _, _, _, _, _, _, _),
+	PINGROUP(132, _, _, _, _, _, _, _),
+	PINGROUP(133, _, _, _, _, _, _, _),
+	PINGROUP(134, _, _, _, _, _, _, _),
+	PINGROUP(135, _, _, _, _, _, _, _),
+	PINGROUP(136, _, _, _, _, _, _, _),
+	PINGROUP(137, _, _, _, _, _, _, _),
+	PINGROUP(138, _, _, _, _, _, _, _),
+	PINGROUP(139, _, _, _, _, _, _, _),
+	PINGROUP(140, _, _, _, _, _, _, _),
+	PINGROUP(141, _, _, _, _, _, _, _),
+	PINGROUP(142, _, _, _, _, _, _, _),
+	PINGROUP(143, _, sdc2, _, _, _, _, _),
+	PINGROUP(144, _, sdc2, _, _, _, _, _),
+	PINGROUP(145, _, sdc2, _, _, _, _, _),
+	PINGROUP(146, _, sdc2, _, _, _, _, _),
+	PINGROUP(147, _, sdc2, _, _, _, _, _),
+	PINGROUP(148, _, sdc2, _, _, _, _, _),
+	PINGROUP(149, _, sdc2, _, _, _, _, _),
+	PINGROUP(150, _, sdc2, _, _, _, _, _),
+	PINGROUP(151, _, sdc2, _, _, _, _, _),
+	PINGROUP(152, _, sdc2, _, _, _, _, _),
+	PINGROUP(153, _, _, _, _, _, _, _),
+	PINGROUP(154, _, _, _, _, _, _, _),
+	PINGROUP(155, _, _, _, _, _, _, _),
+	PINGROUP(156, _, _, _, _, _, _, _),
+	PINGROUP(157, _, _, _, _, _, _, _),
+	PINGROUP(158, _, _, _, _, _, _, _),
+	PINGROUP(159, sdc1, _, _, _, _, _, _),
+	PINGROUP(160, sdc1, _, _, _, _, _, _),
+	PINGROUP(161, sdc1, _, _, _, _, _, _),
+	PINGROUP(162, sdc1, _, _, _, _, _, _),
+	PINGROUP(163, sdc1, _, _, _, _, _, _),
+	PINGROUP(164, sdc1, _, _, _, _, _, _),
+	PINGROUP(165, sdc1, _, _, _, _, _, _),
+	PINGROUP(166, sdc1, _, _, _, _, _, _),
+	PINGROUP(167, sdc1, _, _, _, _, _, _),
+	PINGROUP(168, sdc1, _, _, _, _, _, _),
+	PINGROUP(169, hdmi, _, _, _, _, _, _),
+	PINGROUP(170, hdmi, _, _, _, _, _, _),
+	PINGROUP(171, hdmi, _, _, _, _, _, _),
+	PINGROUP(172, hdmi, _, _, _, _, _, _),
+
+	SDC_PINGROUP(sdc4_clk, 0x20a0, -1, 6),
+	SDC_PINGROUP(sdc4_cmd, 0x20a0, 11, 3),
+	SDC_PINGROUP(sdc4_data, 0x20a0, 9, 0),
+
+	SDC_PINGROUP(sdc3_clk, 0x20a4, -1, 6),
+	SDC_PINGROUP(sdc3_cmd, 0x20a4, 11, 3),
+	SDC_PINGROUP(sdc3_data, 0x20a4, 9, 0),
+};
+
+#define NUM_GPIO_PINGROUPS 173
+
+static const struct msm_pinctrl_soc_data msm8660_pinctrl = {
+	.pins = msm8660_pins,
+	.npins = ARRAY_SIZE(msm8660_pins),
+	.functions = msm8660_functions,
+	.nfunctions = ARRAY_SIZE(msm8660_functions),
+	.groups = msm8660_groups,
+	.ngroups = ARRAY_SIZE(msm8660_groups),
+	.ngpios = NUM_GPIO_PINGROUPS,
+};
+
+static int msm8660_pinctrl_probe(struct platform_device *pdev)
+{
+	return msm_pinctrl_probe(pdev, &msm8660_pinctrl);
+}
+
+static const struct of_device_id msm8660_pinctrl_of_match[] = {
+	{ .compatible = "qcom,msm8660-pinctrl", },
+	{ },
+};
+
+static struct platform_driver msm8660_pinctrl_driver = {
+	.driver = {
+		.name = "msm8660-pinctrl",
+		.of_match_table = msm8660_pinctrl_of_match,
+	},
+	.probe = msm8660_pinctrl_probe,
+	.remove = msm_pinctrl_remove,
+};
+
+static int __init msm8660_pinctrl_init(void)
+{
+	return platform_driver_register(&msm8660_pinctrl_driver);
+}
+arch_initcall(msm8660_pinctrl_init);
+
+static void __exit msm8660_pinctrl_exit(void)
+{
+	platform_driver_unregister(&msm8660_pinctrl_driver);
+}
+module_exit(msm8660_pinctrl_exit);
+
+MODULE_AUTHOR("Bjorn Andersson <bjorn.andersson@sonymobile.com>");
+MODULE_DESCRIPTION("Qualcomm MSM8660 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, msm8660_pinctrl_of_match);
diff --git a/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c b/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
index ae4115e4b4ef..bd1e24598e12 100644
--- a/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
+++ b/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
@@ -778,8 +778,8 @@ static int pmic_gpio_probe(struct platform_device *pdev)
 	state->chip.can_sleep = false;
 
 	state->ctrl = pinctrl_register(pctrldesc, dev, state);
-	if (!state->ctrl)
-		return -ENODEV;
+	if (IS_ERR(state->ctrl))
+		return PTR_ERR(state->ctrl);
 
 	ret = gpiochip_add(&state->chip);
 	if (ret) {
diff --git a/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c b/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
index 211b942ad6d5..3121de9b6331 100644
--- a/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
+++ b/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
@@ -892,8 +892,8 @@ static int pmic_mpp_probe(struct platform_device *pdev)
 	state->chip.can_sleep = false;
 
 	state->ctrl = pinctrl_register(pctrldesc, dev, state);
-	if (!state->ctrl)
-		return -ENODEV;
+	if (IS_ERR(state->ctrl))
+		return PTR_ERR(state->ctrl);
 
 	ret = gpiochip_add(&state->chip);
 	if (ret) {
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos.c b/drivers/pinctrl/samsung/pinctrl-exynos.c
index 0b7afa50121a..b18dabba03a4 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos.c
@@ -563,8 +563,8 @@ static int exynos_eint_wkup_init(struct samsung_pinctrl_drv_data *d)
 		return -ENOMEM;
 	}
 
-	irq_set_chained_handler(irq, exynos_irq_demux_eint16_31);
-	irq_set_handler_data(irq, muxed_data);
+	irq_set_chained_handler_and_data(irq, exynos_irq_demux_eint16_31,
+					 muxed_data);
 
 	bank = d->pin_banks;
 	idx = 0;
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos5440.c b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
index 86192be3b679..f5619fb50447 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos5440.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
@@ -822,9 +822,9 @@ static int exynos5440_pinctrl_register(struct platform_device *pdev,
 		return ret;
 
 	pctl_dev = pinctrl_register(ctrldesc, &pdev->dev, priv);
-	if (!pctl_dev) {
+	if (IS_ERR(pctl_dev)) {
 		dev_err(&pdev->dev, "could not register pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(pctl_dev);
 	}
 
 	grange.name = "exynos5440-pctrl-gpio-range";
diff --git a/drivers/pinctrl/samsung/pinctrl-s3c24xx.c b/drivers/pinctrl/samsung/pinctrl-s3c24xx.c
index f1993f42114c..01b43dbfb795 100644
--- a/drivers/pinctrl/samsung/pinctrl-s3c24xx.c
+++ b/drivers/pinctrl/samsung/pinctrl-s3c24xx.c
@@ -514,8 +514,7 @@ static int s3c24xx_eint_init(struct samsung_pinctrl_drv_data *d)
 		}
 
 		eint_data->parents[i] = irq;
-		irq_set_chained_handler(irq, handlers[i]);
-		irq_set_handler_data(irq, eint_data);
+		irq_set_chained_handler_and_data(irq, handlers[i], eint_data);
 	}
 
 	bank = d->pin_banks;
diff --git a/drivers/pinctrl/samsung/pinctrl-s3c64xx.c b/drivers/pinctrl/samsung/pinctrl-s3c64xx.c
index 7756c1e9e763..ec8cc3b47621 100644
--- a/drivers/pinctrl/samsung/pinctrl-s3c64xx.c
+++ b/drivers/pinctrl/samsung/pinctrl-s3c64xx.c
@@ -506,8 +506,7 @@ static int s3c64xx_eint_gpio_init(struct samsung_pinctrl_drv_data *d)
 		data->domains[nr_domains++] = bank->irq_domain;
 	}
 
-	irq_set_chained_handler(d->irq, s3c64xx_eint_gpio_irq);
-	irq_set_handler_data(d->irq, data);
+	irq_set_chained_handler_and_data(d->irq, s3c64xx_eint_gpio_irq, data);
 
 	return 0;
 }
@@ -731,8 +730,9 @@ static int s3c64xx_eint_eint0_init(struct samsung_pinctrl_drv_data *d)
 			return -ENXIO;
 		}
 
-		irq_set_chained_handler(irq, s3c64xx_eint0_handlers[i]);
-		irq_set_handler_data(irq, data);
+		irq_set_chained_handler_and_data(irq,
+						 s3c64xx_eint0_handlers[i],
+						 data);
 	}
 
 	bank = d->pin_banks;
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.c b/drivers/pinctrl/samsung/pinctrl-samsung.c
index ed165ba2eb2f..3dd5a3b2ac62 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.c
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.c
@@ -806,7 +806,7 @@ static int samsung_pinctrl_parse_dt(struct platform_device *pdev,
 	functions = samsung_pinctrl_create_functions(dev, drvdata, &func_cnt);
 	if (IS_ERR(functions)) {
 		dev_err(dev, "failed to parse pin functions\n");
-		return PTR_ERR(groups);
+		return PTR_ERR(functions);
 	}
 
 	drvdata->pin_groups = groups;
@@ -873,9 +873,9 @@ static int samsung_pinctrl_register(struct platform_device *pdev,
 		return ret;
 
 	drvdata->pctl_dev = pinctrl_register(ctrldesc, &pdev->dev, drvdata);
-	if (!drvdata->pctl_dev) {
+	if (IS_ERR(drvdata->pctl_dev)) {
 		dev_err(&pdev->dev, "could not register pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(drvdata->pctl_dev);
 	}
 
 	for (bank = 0; bank < drvdata->nr_banks; ++bank) {
diff --git a/drivers/pinctrl/sh-pfc/Kconfig b/drivers/pinctrl/sh-pfc/Kconfig
index 8c4b3d391823..8e024c9c9115 100644
--- a/drivers/pinctrl/sh-pfc/Kconfig
+++ b/drivers/pinctrl/sh-pfc/Kconfig
@@ -55,6 +55,16 @@ config PINCTRL_PFC_R8A7791
 	depends on ARCH_R8A7791
 	select PINCTRL_SH_PFC
 
+config PINCTRL_PFC_R8A7793
+	def_bool y
+	depends on ARCH_R8A7793
+	select PINCTRL_SH_PFC
+
+config PINCTRL_PFC_R8A7794
+	def_bool y
+	depends on ARCH_R8A7794
+	select PINCTRL_SH_PFC
+
 config PINCTRL_PFC_SH7203
 	def_bool y
 	depends on CPU_SUBTYPE_SH7203
diff --git a/drivers/pinctrl/sh-pfc/Makefile b/drivers/pinctrl/sh-pfc/Makefile
index f4074e166bcf..ea2a60ef122a 100644
--- a/drivers/pinctrl/sh-pfc/Makefile
+++ b/drivers/pinctrl/sh-pfc/Makefile
@@ -10,6 +10,8 @@ obj-$(CONFIG_PINCTRL_PFC_R8A7778)	+= pfc-r8a7778.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7779)	+= pfc-r8a7779.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7790)	+= pfc-r8a7790.o
 obj-$(CONFIG_PINCTRL_PFC_R8A7791)	+= pfc-r8a7791.o
+obj-$(CONFIG_PINCTRL_PFC_R8A7793)	+= pfc-r8a7791.o
+obj-$(CONFIG_PINCTRL_PFC_R8A7794)	+= pfc-r8a7794.o
 obj-$(CONFIG_PINCTRL_PFC_SH7203)	+= pfc-sh7203.o
 obj-$(CONFIG_PINCTRL_PFC_SH7264)	+= pfc-sh7264.o
 obj-$(CONFIG_PINCTRL_PFC_SH7269)	+= pfc-sh7269.o
diff --git a/drivers/pinctrl/sh-pfc/core.c b/drivers/pinctrl/sh-pfc/core.c
index 7b2c9495c383..865d235612c5 100644
--- a/drivers/pinctrl/sh-pfc/core.c
+++ b/drivers/pinctrl/sh-pfc/core.c
@@ -481,6 +481,18 @@ static const struct of_device_id sh_pfc_of_table[] = {
 		.data = &r8a7791_pinmux_info,
 	},
 #endif
+#ifdef CONFIG_PINCTRL_PFC_R8A7793
+	{
+		.compatible = "renesas,pfc-r8a7793",
+		.data = &r8a7793_pinmux_info,
+	},
+#endif
+#ifdef CONFIG_PINCTRL_PFC_R8A7794
+	{
+		.compatible = "renesas,pfc-r8a7794",
+		.data = &r8a7794_pinmux_info,
+	},
+#endif
 #ifdef CONFIG_PINCTRL_PFC_SH73A0
 	{
 		.compatible = "renesas,pfc-sh73a0",
@@ -579,9 +591,6 @@ static int sh_pfc_remove(struct platform_device *pdev)
 }
 
 static const struct platform_device_id sh_pfc_id_table[] = {
-#ifdef CONFIG_PINCTRL_PFC_R8A73A4
-	{ "pfc-r8a73a4", (kernel_ulong_t)&r8a73a4_pinmux_info },
-#endif
 #ifdef CONFIG_PINCTRL_PFC_R8A7740
 	{ "pfc-r8a7740", (kernel_ulong_t)&r8a7740_pinmux_info },
 #endif
diff --git a/drivers/pinctrl/sh-pfc/core.h b/drivers/pinctrl/sh-pfc/core.h
index 6dc8a6fc2746..4c3c37bf7161 100644
--- a/drivers/pinctrl/sh-pfc/core.h
+++ b/drivers/pinctrl/sh-pfc/core.h
@@ -71,6 +71,8 @@ extern const struct sh_pfc_soc_info r8a7778_pinmux_info;
 extern const struct sh_pfc_soc_info r8a7779_pinmux_info;
 extern const struct sh_pfc_soc_info r8a7790_pinmux_info;
 extern const struct sh_pfc_soc_info r8a7791_pinmux_info;
+extern const struct sh_pfc_soc_info r8a7793_pinmux_info;
+extern const struct sh_pfc_soc_info r8a7794_pinmux_info;
 extern const struct sh_pfc_soc_info sh7203_pinmux_info;
 extern const struct sh_pfc_soc_info sh7264_pinmux_info;
 extern const struct sh_pfc_soc_info sh7269_pinmux_info;
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
index 280a56f97786..ba18d2e65e67 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
@@ -21,10 +21,6 @@
 #include <linux/kernel.h>
 #include <linux/pinctrl/pinconf-generic.h>
 
-#ifndef CONFIG_ARCH_MULTIPLATFORM
-#include <mach/irqs.h>
-#endif
-
 #include "core.h"
 #include "sh_pfc.h"
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
index b486e9d20cc2..d0bb1459783a 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
@@ -258,7 +258,7 @@ enum {
 	/* SCIFA7 */
 	SCIFA7_TXD_MARK,	SCIFA7_RXD_MARK,
 
-	/* SCIFAB */
+	/* SCIFB */
 	SCIFB_SCK_PORT190_MARK, /* MSEL5CR_17_0 */
 	SCIFB_RXD_PORT191_MARK,
 	SCIFB_TXD_PORT192_MARK,
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
index 22a5470889f5..baab81ead9ff 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
@@ -2664,6 +2664,61 @@ static const unsigned int msiof3_tx_b_pins[] = {
 static const unsigned int msiof3_tx_b_mux[] = {
 	MSIOF3_TXD_B_MARK,
 };
+/* - PWM -------------------------------------------------------------------- */
+static const unsigned int pwm0_pins[] = {
+	RCAR_GP_PIN(5, 29),
+};
+static const unsigned int pwm0_mux[] = {
+	PWM0_MARK,
+};
+static const unsigned int pwm0_b_pins[] = {
+	RCAR_GP_PIN(4, 30),
+};
+static const unsigned int pwm0_b_mux[] = {
+	PWM0_B_MARK,
+};
+static const unsigned int pwm1_pins[] = {
+	RCAR_GP_PIN(5, 30),
+};
+static const unsigned int pwm1_mux[] = {
+	PWM1_MARK,
+};
+static const unsigned int pwm1_b_pins[] = {
+	RCAR_GP_PIN(4, 31),
+};
+static const unsigned int pwm1_b_mux[] = {
+	PWM1_B_MARK,
+};
+static const unsigned int pwm2_pins[] = {
+	RCAR_GP_PIN(5, 31),
+};
+static const unsigned int pwm2_mux[] = {
+	PWM2_MARK,
+};
+static const unsigned int pwm3_pins[] = {
+	RCAR_GP_PIN(0, 16),
+};
+static const unsigned int pwm3_mux[] = {
+	PWM3_MARK,
+};
+static const unsigned int pwm4_pins[] = {
+	RCAR_GP_PIN(0, 17),
+};
+static const unsigned int pwm4_mux[] = {
+	PWM4_MARK,
+};
+static const unsigned int pwm5_pins[] = {
+	RCAR_GP_PIN(0, 18),
+};
+static const unsigned int pwm5_mux[] = {
+	PWM5_MARK,
+};
+static const unsigned int pwm6_pins[] = {
+	RCAR_GP_PIN(0, 19),
+};
+static const unsigned int pwm6_mux[] = {
+	PWM6_MARK,
+};
 /* - QSPI ------------------------------------------------------------------- */
 static const unsigned int qspi_ctrl_pins[] = {
 	/* SPCLK, SSL */
@@ -4008,6 +4063,15 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(msiof3_sync_b),
 	SH_PFC_PIN_GROUP(msiof3_rx_b),
 	SH_PFC_PIN_GROUP(msiof3_tx_b),
+	SH_PFC_PIN_GROUP(pwm0),
+	SH_PFC_PIN_GROUP(pwm0_b),
+	SH_PFC_PIN_GROUP(pwm1),
+	SH_PFC_PIN_GROUP(pwm1_b),
+	SH_PFC_PIN_GROUP(pwm2),
+	SH_PFC_PIN_GROUP(pwm3),
+	SH_PFC_PIN_GROUP(pwm4),
+	SH_PFC_PIN_GROUP(pwm5),
+	SH_PFC_PIN_GROUP(pwm6),
 	SH_PFC_PIN_GROUP(qspi_ctrl),
 	SH_PFC_PIN_GROUP(qspi_data2),
 	SH_PFC_PIN_GROUP(qspi_data4),
@@ -4364,6 +4428,36 @@ static const char * const msiof3_groups[] = {
 	"msiof3_tx_b",
 };
 
+static const char * const pwm0_groups[] = {
+	"pwm0",
+	"pwm0_b",
+};
+
+static const char * const pwm1_groups[] = {
+	"pwm1",
+	"pwm1_b",
+};
+
+static const char * const pwm2_groups[] = {
+	"pwm2",
+};
+
+static const char * const pwm3_groups[] = {
+	"pwm3",
+};
+
+static const char * const pwm4_groups[] = {
+	"pwm4",
+};
+
+static const char * const pwm5_groups[] = {
+	"pwm5",
+};
+
+static const char * const pwm6_groups[] = {
+	"pwm6",
+};
+
 static const char * const qspi_groups[] = {
 	"qspi_ctrl",
 	"qspi_data2",
@@ -4621,6 +4715,13 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(msiof1),
 	SH_PFC_FUNCTION(msiof2),
 	SH_PFC_FUNCTION(msiof3),
+	SH_PFC_FUNCTION(pwm0),
+	SH_PFC_FUNCTION(pwm1),
+	SH_PFC_FUNCTION(pwm2),
+	SH_PFC_FUNCTION(pwm3),
+	SH_PFC_FUNCTION(pwm4),
+	SH_PFC_FUNCTION(pwm5),
+	SH_PFC_FUNCTION(pwm6),
 	SH_PFC_FUNCTION(qspi),
 	SH_PFC_FUNCTION(scif0),
 	SH_PFC_FUNCTION(scif1),
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
index fdd2c8729791..3ddf23ec9f0b 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
@@ -2928,6 +2928,79 @@ static const unsigned int msiof2_tx_e_pins[] = {
 static const unsigned int msiof2_tx_e_mux[] = {
 	MSIOF2_TXD_E_MARK,
 };
+/* - PWM -------------------------------------------------------------------- */
+static const unsigned int pwm0_pins[] = {
+	RCAR_GP_PIN(6, 14),
+};
+static const unsigned int pwm0_mux[] = {
+	PWM0_MARK,
+};
+static const unsigned int pwm0_b_pins[] = {
+	RCAR_GP_PIN(5, 30),
+};
+static const unsigned int pwm0_b_mux[] = {
+	PWM0_B_MARK,
+};
+static const unsigned int pwm1_pins[] = {
+	RCAR_GP_PIN(1, 17),
+};
+static const unsigned int pwm1_mux[] = {
+	PWM1_MARK,
+};
+static const unsigned int pwm1_b_pins[] = {
+	RCAR_GP_PIN(6, 15),
+};
+static const unsigned int pwm1_b_mux[] = {
+	PWM1_B_MARK,
+};
+static const unsigned int pwm2_pins[] = {
+	RCAR_GP_PIN(1, 18),
+};
+static const unsigned int pwm2_mux[] = {
+	PWM2_MARK,
+};
+static const unsigned int pwm2_b_pins[] = {
+	RCAR_GP_PIN(0, 16),
+};
+static const unsigned int pwm2_b_mux[] = {
+	PWM2_B_MARK,
+};
+static const unsigned int pwm3_pins[] = {
+	RCAR_GP_PIN(1, 24),
+};
+static const unsigned int pwm3_mux[] = {
+	PWM3_MARK,
+};
+static const unsigned int pwm4_pins[] = {
+	RCAR_GP_PIN(3, 26),
+};
+static const unsigned int pwm4_mux[] = {
+	PWM4_MARK,
+};
+static const unsigned int pwm4_b_pins[] = {
+	RCAR_GP_PIN(3, 31),
+};
+static const unsigned int pwm4_b_mux[] = {
+	PWM4_B_MARK,
+};
+static const unsigned int pwm5_pins[] = {
+	RCAR_GP_PIN(7, 21),
+};
+static const unsigned int pwm5_mux[] = {
+	PWM5_MARK,
+};
+static const unsigned int pwm5_b_pins[] = {
+	RCAR_GP_PIN(7, 20),
+};
+static const unsigned int pwm5_b_mux[] = {
+	PWM5_B_MARK,
+};
+static const unsigned int pwm6_pins[] = {
+	RCAR_GP_PIN(7, 22),
+};
+static const unsigned int pwm6_mux[] = {
+	PWM6_MARK,
+};
 /* - QSPI ------------------------------------------------------------------- */
 static const unsigned int qspi_ctrl_pins[] = {
 	/* SPCLK, SSL */
@@ -4348,6 +4421,18 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(msiof2_sync_e),
 	SH_PFC_PIN_GROUP(msiof2_rx_e),
 	SH_PFC_PIN_GROUP(msiof2_tx_e),
+	SH_PFC_PIN_GROUP(pwm0),
+	SH_PFC_PIN_GROUP(pwm0_b),
+	SH_PFC_PIN_GROUP(pwm1),
+	SH_PFC_PIN_GROUP(pwm1_b),
+	SH_PFC_PIN_GROUP(pwm2),
+	SH_PFC_PIN_GROUP(pwm2_b),
+	SH_PFC_PIN_GROUP(pwm3),
+	SH_PFC_PIN_GROUP(pwm4),
+	SH_PFC_PIN_GROUP(pwm4_b),
+	SH_PFC_PIN_GROUP(pwm5),
+	SH_PFC_PIN_GROUP(pwm5_b),
+	SH_PFC_PIN_GROUP(pwm6),
 	SH_PFC_PIN_GROUP(qspi_ctrl),
 	SH_PFC_PIN_GROUP(qspi_data2),
 	SH_PFC_PIN_GROUP(qspi_data4),
@@ -4745,6 +4830,39 @@ static const char * const msiof2_groups[] = {
 	"msiof2_tx_e",
 };
 
+static const char * const pwm0_groups[] = {
+	"pwm0",
+	"pwm0_b",
+};
+
+static const char * const pwm1_groups[] = {
+	"pwm1",
+	"pwm1_b",
+};
+
+static const char * const pwm2_groups[] = {
+	"pwm2",
+	"pwm2_b",
+};
+
+static const char * const pwm3_groups[] = {
+	"pwm3",
+};
+
+static const char * const pwm4_groups[] = {
+	"pwm4",
+	"pwm4_b",
+};
+
+static const char * const pwm5_groups[] = {
+	"pwm5",
+	"pwm5_b",
+};
+
+static const char * const pwm6_groups[] = {
+	"pwm6",
+};
+
 static const char * const qspi_groups[] = {
 	"qspi_ctrl",
 	"qspi_data2",
@@ -4989,6 +5107,13 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(msiof0),
 	SH_PFC_FUNCTION(msiof1),
 	SH_PFC_FUNCTION(msiof2),
+	SH_PFC_FUNCTION(pwm0),
+	SH_PFC_FUNCTION(pwm1),
+	SH_PFC_FUNCTION(pwm2),
+	SH_PFC_FUNCTION(pwm3),
+	SH_PFC_FUNCTION(pwm4),
+	SH_PFC_FUNCTION(pwm5),
+	SH_PFC_FUNCTION(pwm6),
 	SH_PFC_FUNCTION(qspi),
 	SH_PFC_FUNCTION(scif0),
 	SH_PFC_FUNCTION(scif1),
@@ -6000,7 +6125,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 	{ PINMUX_CFG_REG_VAR("MOD_SEL", 0xE6060090, 32,
 			     1, 2, 2, 2, 3, 2, 1, 1, 1, 1,
 			     3, 2, 2, 2, 1, 2, 2, 2) {
-		/* RESEVED [1] */
+		/* RESERVED [1] */
 		0, 0,
 		/* SEL_SCIF1 [2] */
 		FN_SEL_SCIF1_0, FN_SEL_SCIF1_1, FN_SEL_SCIF1_2, FN_SEL_SCIF1_3,
@@ -6027,11 +6152,11 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_HSCIF1_0, FN_SEL_HSCIF1_1, FN_SEL_HSCIF1_2,
 		FN_SEL_HSCIF1_3, FN_SEL_HSCIF1_4,
 		0, 0, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
 		/* SEL_VI1 [2] */
 		FN_SEL_VI1_0, FN_SEL_VI1_1, FN_SEL_VI1_2, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
 		/* SEL_TMU [1] */
 		FN_SEL_TMU1_0, FN_SEL_TMU1_1,
@@ -6049,7 +6174,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_SCIF0_0, FN_SEL_SCIF0_1, FN_SEL_SCIF0_2,
 		FN_SEL_SCIF0_3, FN_SEL_SCIF0_4,
 		0, 0, 0,
-		/* RESEVED [1] */
+		/* RESERVED [1] */
 		0, 0,
 		/* SEL_SCIF [1] */
 		FN_SEL_SCIF_0, FN_SEL_SCIF_1,
@@ -6059,13 +6184,13 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0,
 		/* SEL_CAN1 [2] */
 		FN_SEL_CAN1_0, FN_SEL_CAN1_1, FN_SEL_CAN1_2, FN_SEL_CAN1_3,
-		/* RESEVED [1] */
+		/* RESERVED [1] */
 		0, 0,
 		/* SEL_SCIFA2 [1] */
 		FN_SEL_SCIFA2_0, FN_SEL_SCIFA2_1,
 		/* SEL_SCIF4 [2] */
 		FN_SEL_SCIF4_0, FN_SEL_SCIF4_1, FN_SEL_SCIF4_2, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
 		/* SEL_ADG [1] */
 		FN_SEL_ADG_0, FN_SEL_ADG_1,
@@ -6075,7 +6200,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0, 0,
 		/* SEL_SCIFA5 [2] */
 		FN_SEL_SCIFA5_0, FN_SEL_SCIFA5_1, FN_SEL_SCIFA5_2, 0,
-		/* RESEVED [1] */
+		/* RESERVED [1] */
 		0, 0,
 		/* SEL_GPS [2] */
 		FN_SEL_GPS_0, FN_SEL_GPS_1, FN_SEL_GPS_2, FN_SEL_GPS_3,
@@ -6085,7 +6210,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_SCIFA3_0, FN_SEL_SCIFA3_1, FN_SEL_SCIFA3_2, 0,
 		/* SEL_SIM [1] */
 		FN_SEL_SIM_0, FN_SEL_SIM_1,
-		/* RESEVED [1] */
+		/* RESERVED [1] */
 		0, 0,
 		/* SEL_SSI8 [1] */
 		FN_SEL_SSI8_0, FN_SEL_SSI8_1, }
@@ -6115,7 +6240,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_MMC_0, FN_SEL_MMC_1,
 		/* SEL_SCIF5 [1] */
 		FN_SEL_SCIF5_0, FN_SEL_SCIF5_1,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
 		/* SEL_IIC2 [2] */
 		FN_SEL_IIC2_0, FN_SEL_IIC2_1, FN_SEL_IIC2_2, FN_SEL_IIC2_3,
@@ -6125,11 +6250,11 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		0, 0, 0,
 		/* SEL_IIC0 [2] */
 		FN_SEL_IIC0_0, FN_SEL_IIC0_1, FN_SEL_IIC0_2, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
-		/* RESEVED [1] */
+		/* RESERVED [1] */
 		0, 0, }
 	},
 	{ PINMUX_CFG_REG_VAR("MOD_SEL4", 0xE606009C, 32,
@@ -6143,7 +6268,7 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_HSCIF0_0, FN_SEL_HSCIF0_1, FN_SEL_HSCIF0_2, 0,
 		/* SEL_DIS [2] */
 		FN_SEL_DIS_0, FN_SEL_DIS_1, FN_SEL_DIS_2, 0,
-		/* RESEVED [1] */
+		/* RESERVED [1] */
 		0, 0,
 		/* SEL_RAD [1] */
 		FN_SEL_RAD_0, FN_SEL_RAD_1,
@@ -6155,15 +6280,15 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_SCIF2_0, FN_SEL_SCIF2_1, FN_SEL_SCIF2_2,
 		FN_SEL_SCIF2_3, FN_SEL_SCIF2_4,
 		0, 0, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
 		/* SEL_SOF2 [3] */
 		FN_SEL_SOF2_0, FN_SEL_SOF2_1, FN_SEL_SOF2_2,
 		FN_SEL_SOF2_3, FN_SEL_SOF2_4,
 		0, 0, 0,
-		/* RESEVED [1] */
+		/* RESERVED [1] */
 		0, 0,
 		/* SEL_SSI1 [1] */
 		FN_SEL_SSI1_0, FN_SEL_SSI1_1,
@@ -6171,16 +6296,17 @@ static const struct pinmux_cfg_reg pinmux_config_regs[] = {
 		FN_SEL_SSI0_0, FN_SEL_SSI0_1,
 		/* SEL_SSP [2] */
 		FN_SEL_SSP_0, FN_SEL_SSP_1, FN_SEL_SSP_2, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0,
-		/* RESEVED [2] */
+		/* RESERVED [2] */
 		0, 0, 0, 0, }
 	},
 	{ },
 };
 
+#ifdef CONFIG_PINCTRL_PFC_R8A7791
 const struct sh_pfc_soc_info r8a7791_pinmux_info = {
 	.name = "r8a77910_pfc",
 	.unlock_reg = 0xe6060000, /* PMMR */
@@ -6199,3 +6325,25 @@ const struct sh_pfc_soc_info r8a7791_pinmux_info = {
 	.gpio_data = pinmux_data,
 	.gpio_data_size = ARRAY_SIZE(pinmux_data),
 };
+#endif
+
+#ifdef CONFIG_PINCTRL_PFC_R8A7793
+const struct sh_pfc_soc_info r8a7793_pinmux_info = {
+	.name = "r8a77930_pfc",
+	.unlock_reg = 0xe6060000, /* PMMR */
+
+	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
+
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.groups = pinmux_groups,
+	.nr_groups = ARRAY_SIZE(pinmux_groups),
+	.functions = pinmux_functions,
+	.nr_functions = ARRAY_SIZE(pinmux_functions),
+
+	.cfg_regs = pinmux_config_regs,
+
+	.gpio_data = pinmux_data,
+	.gpio_data_size = ARRAY_SIZE(pinmux_data),
+};
+#endif
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7794.c b/drivers/pinctrl/sh-pfc/pfc-r8a7794.c
new file mode 100644
index 000000000000..bfdcac4b3bc4
--- /dev/null
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7794.c
@@ -0,0 +1,4207 @@
+/*
+ * r8a7794 processor support - PFC hardware block.
+ *
+ * Copyright (C) 2014 Renesas Electronics Corporation
+ * Copyright (C) 2015 Renesas Solutions Corp.
+ * Copyright (C) 2015 Cogent  Embedded, Inc., <source@cogentembedded.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/platform_data/gpio-rcar.h>
+
+#include "core.h"
+#include "sh_pfc.h"
+
+#define PORT_GP_26(bank, fn, sfx)					\
+	PORT_GP_1(bank, 0,  fn, sfx), PORT_GP_1(bank, 1,  fn, sfx),	\
+	PORT_GP_1(bank, 2,  fn, sfx), PORT_GP_1(bank, 3,  fn, sfx),	\
+	PORT_GP_1(bank, 4,  fn, sfx), PORT_GP_1(bank, 5,  fn, sfx),	\
+	PORT_GP_1(bank, 6,  fn, sfx), PORT_GP_1(bank, 7,  fn, sfx),	\
+	PORT_GP_1(bank, 8,  fn, sfx), PORT_GP_1(bank, 9,  fn, sfx),	\
+	PORT_GP_1(bank, 10, fn, sfx), PORT_GP_1(bank, 11, fn, sfx),	\
+	PORT_GP_1(bank, 12, fn, sfx), PORT_GP_1(bank, 13, fn, sfx),	\
+	PORT_GP_1(bank, 14, fn, sfx), PORT_GP_1(bank, 15, fn, sfx),	\
+	PORT_GP_1(bank, 16, fn, sfx), PORT_GP_1(bank, 17, fn, sfx),	\
+	PORT_GP_1(bank, 18, fn, sfx), PORT_GP_1(bank, 19, fn, sfx),	\
+	PORT_GP_1(bank, 20, fn, sfx), PORT_GP_1(bank, 21, fn, sfx),	\
+	PORT_GP_1(bank, 22, fn, sfx), PORT_GP_1(bank, 23, fn, sfx),	\
+	PORT_GP_1(bank, 24, fn, sfx), PORT_GP_1(bank, 25, fn, sfx)
+
+#define PORT_GP_28(bank, fn, sfx)					\
+	PORT_GP_26(bank, fn, sfx),					\
+	PORT_GP_1(bank, 26, fn, sfx), PORT_GP_1(bank, 27, fn, sfx)
+
+#define CPU_ALL_PORT(fn, sfx)						\
+	PORT_GP_32(0, fn, sfx),						\
+	PORT_GP_26(1, fn, sfx),						\
+	PORT_GP_32(2, fn, sfx),						\
+	PORT_GP_32(3, fn, sfx),						\
+	PORT_GP_32(4, fn, sfx),						\
+	PORT_GP_28(5, fn, sfx),						\
+	PORT_GP_26(6, fn, sfx)
+
+enum {
+	PINMUX_RESERVED = 0,
+
+	PINMUX_DATA_BEGIN,
+	GP_ALL(DATA),
+	PINMUX_DATA_END,
+
+	PINMUX_FUNCTION_BEGIN,
+	GP_ALL(FN),
+
+	/* GPSR0 */
+	FN_IP0_23_22, FN_IP0_24, FN_IP0_25, FN_IP0_27_26, FN_IP0_29_28,
+	FN_IP0_31_30, FN_IP1_1_0, FN_IP1_3_2, FN_IP1_5_4, FN_IP1_7_6,
+	FN_IP1_10_8, FN_IP1_12_11, FN_IP1_14_13, FN_IP1_17_15, FN_IP1_19_18,
+	FN_IP1_21_20, FN_IP1_23_22, FN_IP1_24, FN_A2, FN_IP1_26, FN_IP1_27,
+	FN_IP1_29_28, FN_IP1_31_30, FN_IP2_1_0, FN_IP2_3_2, FN_IP2_5_4,
+	FN_IP2_7_6, FN_IP2_9_8, FN_IP2_11_10, FN_IP2_13_12, FN_IP2_15_14,
+	FN_IP2_17_16,
+
+	/* GPSR1 */
+	FN_IP2_20_18, FN_IP2_23_21, FN_IP2_26_24, FN_IP2_29_27, FN_IP2_31_30,
+	FN_IP3_1_0, FN_IP3_3_2, FN_IP3_5_4, FN_IP3_7_6, FN_IP3_9_8, FN_IP3_10,
+	FN_IP3_11, FN_IP3_12, FN_IP3_14_13, FN_IP3_17_15, FN_IP3_20_18,
+	FN_IP3_23_21, FN_IP3_26_24, FN_IP3_29_27, FN_IP3_30, FN_IP3_31,
+	FN_WE0_N, FN_WE1_N, FN_IP4_1_0 , FN_IP7_31, FN_DACK0,
+
+	/* GPSR2 */
+	FN_IP4_4_2, FN_IP4_7_5, FN_IP4_9_8, FN_IP4_11_10, FN_IP4_13_12,
+	FN_IP4_15_14, FN_IP4_17_16, FN_IP4_19_18, FN_IP4_22_20, FN_IP4_25_23,
+	FN_IP4_27_26, FN_IP4_29_28, FN_IP4_31_30, FN_IP5_1_0, FN_IP5_3_2,
+	FN_IP5_5_4, FN_IP5_8_6, FN_IP5_11_9, FN_IP5_13_12, FN_IP5_15_14,
+	FN_IP5_17_16, FN_IP5_19_18, FN_IP5_21_20, FN_IP5_23_22, FN_IP5_25_24,
+	FN_IP5_27_26, FN_IP5_29_28, FN_IP5_31_30, FN_IP6_1_0, FN_IP6_3_2,
+	FN_IP6_5_4, FN_IP6_7_6,
+
+	/* GPSR3 */
+	FN_IP6_8, FN_IP6_9, FN_IP6_10, FN_IP6_11, FN_IP6_12, FN_IP6_13,
+	FN_IP6_14, FN_IP6_15, FN_IP6_16, FN_IP6_19_17, FN_IP6_22_20,
+	FN_IP6_25_23, FN_IP6_28_26, FN_IP6_31_29, FN_IP7_2_0, FN_IP7_5_3,
+	FN_IP7_8_6, FN_IP7_11_9, FN_IP7_14_12, FN_IP7_17_15, FN_IP7_20_18,
+	FN_IP7_23_21, FN_IP7_26_24, FN_IP7_29_27, FN_IP8_2_0, FN_IP8_5_3,
+	FN_IP8_8_6, FN_IP8_11_9, FN_IP8_14_12, FN_IP8_16_15, FN_IP8_19_17,
+	FN_IP8_22_20,
+
+	/* GPSR4 */
+	FN_IP8_25_23, FN_IP8_28_26, FN_IP8_31_29, FN_IP9_2_0, FN_IP9_5_3,
+	FN_IP9_8_6, FN_IP9_11_9, FN_IP9_14_12, FN_IP9_16_15, FN_IP9_18_17,
+	FN_IP9_21_19, FN_IP9_24_22, FN_IP9_27_25, FN_IP9_30_28, FN_IP10_2_0,
+	FN_IP10_5_3, FN_IP10_8_6, FN_IP10_11_9, FN_IP10_14_12, FN_IP10_17_15,
+	FN_IP10_20_18, FN_IP10_23_21, FN_IP10_26_24, FN_IP10_29_27,
+	FN_IP10_31_30, FN_IP11_2_0, FN_IP11_5_3, FN_IP11_7_6, FN_IP11_10_8,
+	FN_IP11_13_11, FN_IP11_15_14, FN_IP11_17_16,
+
+	/* GPSR5 */
+	FN_IP11_20_18, FN_IP11_23_21, FN_IP11_26_24, FN_IP11_29_27, FN_IP12_2_0,
+	FN_IP12_5_3, FN_IP12_8_6, FN_IP12_10_9, FN_IP12_12_11, FN_IP12_14_13,
+	FN_IP12_17_15, FN_IP12_20_18, FN_IP12_23_21, FN_IP12_26_24,
+	FN_IP12_29_27, FN_IP13_2_0, FN_IP13_5_3, FN_IP13_8_6, FN_IP13_11_9,
+	FN_IP13_14_12, FN_IP13_17_15, FN_IP13_20_18, FN_IP13_23_21,
+	FN_IP13_26_24, FN_USB0_PWEN, FN_USB0_OVC, FN_USB1_PWEN, FN_USB1_OVC,
+
+	/* GPSR6 */
+	FN_SD0_CLK, FN_SD0_CMD, FN_SD0_DATA0, FN_SD0_DATA1, FN_SD0_DATA2,
+	FN_SD0_DATA3, FN_SD0_CD, FN_SD0_WP, FN_SD1_CLK, FN_SD1_CMD,
+	FN_SD1_DATA0, FN_SD1_DATA1, FN_SD1_DATA2, FN_SD1_DATA3, FN_IP0_0,
+	FN_IP0_9_8, FN_IP0_10, FN_IP0_11, FN_IP0_12, FN_IP0_13, FN_IP0_14,
+	FN_IP0_15, FN_IP0_16, FN_IP0_17, FN_IP0_19_18, FN_IP0_21_20,
+
+	/* IPSR0 */
+	FN_SD1_CD, FN_CAN0_RX, FN_SD1_WP, FN_IRQ7, FN_CAN0_TX, FN_MMC_CLK,
+	FN_SD2_CLK, FN_MMC_CMD, FN_SD2_CMD, FN_MMC_D0, FN_SD2_DATA0, FN_MMC_D1,
+	FN_SD2_DATA1, FN_MMC_D2, FN_SD2_DATA2, FN_MMC_D3, FN_SD2_DATA3,
+	FN_MMC_D4, FN_SD2_CD, FN_MMC_D5, FN_SD2_WP, FN_MMC_D6, FN_SCIF0_RXD,
+	FN_I2C2_SCL_B, FN_CAN1_RX, FN_MMC_D7, FN_SCIF0_TXD, FN_I2C2_SDA_B,
+	FN_CAN1_TX, FN_D0, FN_SCIFA3_SCK_B, FN_IRQ4, FN_D1, FN_SCIFA3_RXD_B,
+	FN_D2, FN_SCIFA3_TXD_B, FN_D3, FN_I2C3_SCL_B, FN_SCIF5_RXD_B, FN_D4,
+	FN_I2C3_SDA_B, FN_SCIF5_TXD_B, FN_D5, FN_SCIF4_RXD_B, FN_I2C0_SCL_D,
+
+	/* IPSR1 */
+	FN_D6, FN_SCIF4_TXD_B, FN_I2C0_SDA_D, FN_D7, FN_IRQ3, FN_TCLK1,
+	FN_PWM6_B, FN_D8, FN_HSCIF2_HRX, FN_I2C1_SCL_B, FN_D9, FN_HSCIF2_HTX,
+	FN_I2C1_SDA_B, FN_D10, FN_HSCIF2_HSCK, FN_SCIF1_SCK_C, FN_IRQ6,
+	FN_PWM5_C, FN_D11, FN_HSCIF2_HCTS_N, FN_SCIF1_RXD_C, FN_I2C1_SCL_D,
+	FN_D12, FN_HSCIF2_HRTS_N, FN_SCIF1_TXD_C, FN_I2C1_SDA_D, FN_D13,
+	FN_SCIFA1_SCK, FN_TANS1, FN_PWM2_C, FN_TCLK2_B, FN_D14, FN_SCIFA1_RXD,
+	FN_IIC0_SCL_B, FN_D15, FN_SCIFA1_TXD, FN_IIC0_SDA_B, FN_A0,
+	FN_SCIFB1_SCK, FN_PWM3_B, FN_A1, FN_SCIFB1_TXD, FN_A3, FN_SCIFB0_SCK,
+	FN_A4, FN_SCIFB0_TXD, FN_A5, FN_SCIFB0_RXD, FN_PWM4_B, FN_TPUTO3_C,
+	FN_A6, FN_SCIFB0_CTS_N, FN_SCIFA4_RXD_B, FN_TPUTO2_C,
+
+	/* IPSR2 */
+	FN_A7, FN_SCIFB0_RTS_N, FN_SCIFA4_TXD_B, FN_A8, FN_MSIOF1_RXD,
+	FN_SCIFA0_RXD_B, FN_A9, FN_MSIOF1_TXD, FN_SCIFA0_TXD_B, FN_A10,
+	FN_MSIOF1_SCK, FN_IIC1_SCL_B, FN_A11, FN_MSIOF1_SYNC, FN_IIC1_SDA_B,
+	FN_A12, FN_MSIOF1_SS1, FN_SCIFA5_RXD_B, FN_A13, FN_MSIOF1_SS2,
+	FN_SCIFA5_TXD_B, FN_A14, FN_MSIOF2_RXD, FN_HSCIF0_HRX_B, FN_DREQ1_N,
+	FN_A15, FN_MSIOF2_TXD, FN_HSCIF0_HTX_B, FN_DACK1, FN_A16,
+	FN_MSIOF2_SCK, FN_HSCIF0_HSCK_B, FN_SPEEDIN, FN_VSP, FN_CAN_CLK_C,
+	FN_TPUTO2_B, FN_A17, FN_MSIOF2_SYNC, FN_SCIF4_RXD_E, FN_CAN1_RX_B,
+	FN_AVB_AVTP_CAPTURE_B, FN_A18, FN_MSIOF2_SS1, FN_SCIF4_TXD_E,
+	FN_CAN1_TX_B, FN_AVB_AVTP_MATCH_B, FN_A19, FN_MSIOF2_SS2, FN_PWM4,
+	FN_TPUTO2, FN_MOUT0, FN_A20, FN_SPCLK, FN_MOUT1,
+
+	/* IPSR3 */
+	FN_A21, FN_MOSI_IO0, FN_MOUT2, FN_A22, FN_MISO_IO1, FN_MOUT5,
+	FN_ATADIR1_N, FN_A23, FN_IO2, FN_MOUT6, FN_ATAWR1_N, FN_A24, FN_IO3,
+	FN_EX_WAIT2, FN_A25, FN_SSL, FN_ATARD1_N, FN_CS0_N, FN_VI1_DATA8,
+	FN_CS1_N_A26, FN_VI1_DATA9, FN_EX_CS0_N, FN_VI1_DATA10, FN_EX_CS1_N,
+	FN_TPUTO3_B, FN_SCIFB2_RXD, FN_VI1_DATA11, FN_EX_CS2_N, FN_PWM0,
+	FN_SCIF4_RXD_C, FN_TS_SDATA_B, FN_RIF0_SYNC, FN_TPUTO3, FN_SCIFB2_TXD,
+	FN_SDATA_B, FN_EX_CS3_N, FN_SCIFA2_SCK, FN_SCIF4_TXD_C, FN_TS_SCK_B,
+	FN_RIF0_CLK, FN_BPFCLK, FN_SCIFB2_SCK, FN_MDATA_B, FN_EX_CS4_N,
+	FN_SCIFA2_RXD, FN_I2C2_SCL_E, FN_TS_SDEN_B, FN_RIF0_D0, FN_FMCLK,
+	FN_SCIFB2_CTS_N, FN_SCKZ_B, FN_EX_CS5_N, FN_SCIFA2_TXD, FN_I2C2_SDA_E,
+	FN_TS_SPSYNC_B, FN_RIF0_D1, FN_FMIN, FN_SCIFB2_RTS_N, FN_STM_N_B,
+	FN_BS_N, FN_DRACK0, FN_PWM1_C, FN_TPUTO0_C, FN_ATACS01_N, FN_MTS_N_B,
+	FN_RD_N, FN_ATACS11_N, FN_RD_WR_N, FN_ATAG1_N,
+
+	/* IPSR4 */
+	FN_EX_WAIT0, FN_CAN_CLK_B, FN_SCIF_CLK, FN_PWMFSW0, FN_DU0_DR0,
+	FN_LCDOUT16, FN_SCIF5_RXD_C, FN_I2C2_SCL_D, FN_CC50_STATE0,
+	FN_DU0_DR1, FN_LCDOUT17, FN_SCIF5_TXD_C, FN_I2C2_SDA_D, FN_CC50_STATE1,
+	FN_DU0_DR2, FN_LCDOUT18, FN_CC50_STATE2, FN_DU0_DR3, FN_LCDOUT19,
+	FN_CC50_STATE3, FN_DU0_DR4, FN_LCDOUT20, FN_CC50_STATE4, FN_DU0_DR5,
+	FN_LCDOUT21, FN_CC50_STATE5, FN_DU0_DR6, FN_LCDOUT22, FN_CC50_STATE6,
+	FN_DU0_DR7, FN_LCDOUT23, FN_CC50_STATE7, FN_DU0_DG0, FN_LCDOUT8,
+	FN_SCIFA0_RXD_C, FN_I2C3_SCL_D, FN_CC50_STATE8, FN_DU0_DG1, FN_LCDOUT9,
+	FN_SCIFA0_TXD_C, FN_I2C3_SDA_D, FN_CC50_STATE9, FN_DU0_DG2, FN_LCDOUT10,
+	FN_CC50_STATE10, FN_DU0_DG3, FN_LCDOUT11, FN_CC50_STATE11, FN_DU0_DG4,
+	FN_LCDOUT12, FN_CC50_STATE12,
+
+	/* IPSR5 */
+	FN_DU0_DG5, FN_LCDOUT13, FN_CC50_STATE13, FN_DU0_DG6, FN_LCDOUT14,
+	FN_CC50_STATE14, FN_DU0_DG7, FN_LCDOUT15, FN_CC50_STATE15, FN_DU0_DB0,
+	FN_LCDOUT0, FN_SCIFA4_RXD_C, FN_I2C4_SCL_D, FN_CAN0_RX_C,
+	FN_CC50_STATE16, FN_DU0_DB1, FN_LCDOUT1, FN_SCIFA4_TXD_C, FN_I2C4_SDA_D,
+	FN_CAN0_TX_C, FN_CC50_STATE17, FN_DU0_DB2, FN_LCDOUT2, FN_CC50_STATE18,
+	FN_DU0_DB3, FN_LCDOUT3, FN_CC50_STATE19, FN_DU0_DB4, FN_LCDOUT4,
+	FN_CC50_STATE20, FN_DU0_DB5, FN_LCDOUT5, FN_CC50_STATE21, FN_DU0_DB6,
+	FN_LCDOUT6, FN_CC50_STATE22, FN_DU0_DB7, FN_LCDOUT7, FN_CC50_STATE23,
+	FN_DU0_DOTCLKIN, FN_QSTVA_QVS, FN_CC50_STATE24, FN_DU0_DOTCLKOUT0,
+	FN_QCLK, FN_CC50_STATE25, FN_DU0_DOTCLKOUT1, FN_QSTVB_QVE,
+	FN_CC50_STATE26, FN_DU0_EXHSYNC_DU0_HSYNC, FN_QSTH_QHS, FN_CC50_STATE27,
+
+	/* IPSR6 */
+	FN_DU0_EXVSYNC_DU0_VSYNC, FN_QSTB_QHE, FN_CC50_STATE28,
+	FN_DU0_EXODDF_DU0_ODDF_DISP_CDE, FN_QCPV_QDE, FN_CC50_STATE29,
+	FN_DU0_DISP, FN_QPOLA, FN_CC50_STATE30, FN_DU0_CDE, FN_QPOLB,
+	FN_CC50_STATE31, FN_VI0_CLK, FN_AVB_RX_CLK, FN_VI0_DATA0_VI0_B0,
+	FN_AVB_RX_DV, FN_VI0_DATA1_VI0_B1, FN_AVB_RXD0, FN_VI0_DATA2_VI0_B2,
+	FN_AVB_RXD1, FN_VI0_DATA3_VI0_B3, FN_AVB_RXD2, FN_VI0_DATA4_VI0_B4,
+	FN_AVB_RXD3, FN_VI0_DATA5_VI0_B5, FN_AVB_RXD4, FN_VI0_DATA6_VI0_B6,
+	FN_AVB_RXD5, FN_VI0_DATA7_VI0_B7, FN_AVB_RXD6, FN_VI0_CLKENB,
+	FN_I2C3_SCL, FN_SCIFA5_RXD_C, FN_IETX_C, FN_AVB_RXD7, FN_VI0_FIELD,
+	FN_I2C3_SDA, FN_SCIFA5_TXD_C, FN_IECLK_C, FN_AVB_RX_ER, FN_VI0_HSYNC_N,
+	FN_SCIF0_RXD_B, FN_I2C0_SCL_C, FN_IERX_C, FN_AVB_COL, FN_VI0_VSYNC_N,
+	FN_SCIF0_TXD_B, FN_I2C0_SDA_C, FN_AUDIO_CLKOUT_B, FN_AVB_TX_EN,
+	FN_ETH_MDIO, FN_VI0_G0, FN_MSIOF2_RXD_B, FN_IIC0_SCL_D, FN_AVB_TX_CLK,
+	FN_ADIDATA, FN_AD_DI,
+
+	/* IPSR7 */
+	FN_ETH_CRS_DV, FN_VI0_G1, FN_MSIOF2_TXD_B, FN_IIC0_SDA_D, FN_AVB_TXD0,
+	FN_ADICS_SAMP, FN_AD_DO, FN_ETH_RX_ER, FN_VI0_G2, FN_MSIOF2_SCK_B,
+	FN_CAN0_RX_B, FN_AVB_TXD1, FN_ADICLK, FN_AD_CLK, FN_ETH_RXD0, FN_VI0_G3,
+	FN_MSIOF2_SYNC_B, FN_CAN0_TX_B, FN_AVB_TXD2, FN_ADICHS0, FN_AD_NCS_N,
+	FN_ETH_RXD1, FN_VI0_G4, FN_MSIOF2_SS1_B, FN_SCIF4_RXD_D, FN_AVB_TXD3,
+	FN_ADICHS1, FN_ETH_LINK, FN_VI0_G5, FN_MSIOF2_SS2_B, FN_SCIF4_TXD_D,
+	FN_AVB_TXD4, FN_ADICHS2, FN_ETH_REFCLK, FN_VI0_G6, FN_SCIF2_SCK_C,
+	FN_AVB_TXD5, FN_SSI_SCK5_B, FN_ETH_TXD1, FN_VI0_G7, FN_SCIF2_RXD_C,
+	FN_IIC1_SCL_D, FN_AVB_TXD6, FN_SSI_WS5_B, FN_ETH_TX_EN, FN_VI0_R0,
+	FN_SCIF2_TXD_C, FN_IIC1_SDA_D, FN_AVB_TXD7, FN_SSI_SDATA5_B,
+	FN_ETH_MAGIC, FN_VI0_R1, FN_SCIF3_SCK_B, FN_AVB_TX_ER, FN_SSI_SCK6_B,
+	FN_ETH_TXD0, FN_VI0_R2, FN_SCIF3_RXD_B, FN_I2C4_SCL_E, FN_AVB_GTX_CLK,
+	FN_SSI_WS6_B, FN_DREQ0_N, FN_SCIFB1_RXD,
+
+	/* IPSR8 */
+	FN_ETH_MDC, FN_VI0_R3, FN_SCIF3_TXD_B, FN_I2C4_SDA_E, FN_AVB_MDC,
+	FN_SSI_SDATA6_B, FN_HSCIF0_HRX, FN_VI0_R4, FN_I2C1_SCL_C,
+	FN_AUDIO_CLKA_B, FN_AVB_MDIO, FN_SSI_SCK78_B, FN_HSCIF0_HTX,
+	FN_VI0_R5, FN_I2C1_SDA_C, FN_AUDIO_CLKB_B, FN_AVB_LINK, FN_SSI_WS78_B,
+	FN_HSCIF0_HCTS_N, FN_VI0_R6, FN_SCIF0_RXD_D, FN_I2C0_SCL_E,
+	FN_AVB_MAGIC, FN_SSI_SDATA7_B, FN_HSCIF0_HRTS_N, FN_VI0_R7,
+	FN_SCIF0_TXD_D, FN_I2C0_SDA_E, FN_AVB_PHY_INT, FN_SSI_SDATA8_B,
+	FN_HSCIF0_HSCK, FN_SCIF_CLK_B, FN_AVB_CRS, FN_AUDIO_CLKC_B,
+	FN_I2C0_SCL, FN_SCIF0_RXD_C, FN_PWM5, FN_TCLK1_B, FN_AVB_GTXREFCLK,
+	FN_CAN1_RX_D, FN_TPUTO0_B, FN_I2C0_SDA, FN_SCIF0_TXD_C, FN_TPUTO0,
+	FN_CAN_CLK, FN_DVC_MUTE, FN_CAN1_TX_D, FN_I2C1_SCL, FN_SCIF4_RXD,
+	FN_PWM5_B, FN_DU1_DR0, FN_RIF1_SYNC_B, FN_TS_SDATA_D, FN_TPUTO1_B,
+	FN_I2C1_SDA, FN_SCIF4_TXD, FN_IRQ5, FN_DU1_DR1, FN_RIF1_CLK_B,
+	FN_TS_SCK_D, FN_BPFCLK_C, FN_MSIOF0_RXD, FN_SCIF5_RXD, FN_I2C2_SCL_C,
+	FN_DU1_DR2, FN_RIF1_D0_B, FN_TS_SDEN_D, FN_FMCLK_C, FN_RDS_CLK,
+
+	/* IPSR9 */
+	FN_MSIOF0_TXD, FN_SCIF5_TXD, FN_I2C2_SDA_C, FN_DU1_DR3, FN_RIF1_D1_B,
+	FN_TS_SPSYNC_D, FN_FMIN_C, FN_RDS_DATA, FN_MSIOF0_SCK, FN_IRQ0,
+	FN_TS_SDATA, FN_DU1_DR4, FN_RIF1_SYNC, FN_TPUTO1_C, FN_MSIOF0_SYNC,
+	FN_PWM1, FN_TS_SCK, FN_DU1_DR5, FN_RIF1_CLK, FN_BPFCLK_B, FN_MSIOF0_SS1,
+	FN_SCIFA0_RXD, FN_TS_SDEN, FN_DU1_DR6, FN_RIF1_D0, FN_FMCLK_B,
+	FN_RDS_CLK_B, FN_MSIOF0_SS2, FN_SCIFA0_TXD, FN_TS_SPSYNC, FN_DU1_DR7,
+	FN_RIF1_D1, FN_FMIN_B, FN_RDS_DATA_B, FN_HSCIF1_HRX, FN_I2C4_SCL,
+	FN_PWM6, FN_DU1_DG0, FN_HSCIF1_HTX, FN_I2C4_SDA, FN_TPUTO1, FN_DU1_DG1,
+	FN_HSCIF1_HSCK, FN_PWM2, FN_IETX, FN_DU1_DG2, FN_REMOCON_B,
+	FN_SPEEDIN_B, FN_VSP_B, FN_HSCIF1_HCTS_N, FN_SCIFA4_RXD, FN_IECLK,
+	FN_DU1_DG3, FN_SSI_SCK1_B, FN_CAN_DEBUG_HW_TRIGGER, FN_CC50_STATE32,
+	FN_HSCIF1_HRTS_N, FN_SCIFA4_TXD, FN_IERX, FN_DU1_DG4, FN_SSI_WS1_B,
+	FN_CAN_STEP0, FN_CC50_STATE33, FN_SCIF1_SCK, FN_PWM3, FN_TCLK2,
+	FN_DU1_DG5, FN_SSI_SDATA1_B, FN_CAN_TXCLK, FN_CC50_STATE34,
+
+	/* IPSR10 */
+	FN_SCIF1_RXD, FN_IIC0_SCL, FN_DU1_DG6, FN_SSI_SCK2_B, FN_CAN_DEBUGOUT0,
+	FN_CC50_STATE35, FN_SCIF1_TXD, FN_IIC0_SDA, FN_DU1_DG7, FN_SSI_WS2_B,
+	FN_CAN_DEBUGOUT1, FN_CC50_STATE36, FN_SCIF2_RXD, FN_IIC1_SCL,
+	FN_DU1_DB0, FN_SSI_SDATA2_B, FN_USB0_EXTLP, FN_CAN_DEBUGOUT2,
+	FN_CC50_STATE37, FN_SCIF2_TXD, FN_IIC1_SDA, FN_DU1_DB1, FN_SSI_SCK9_B,
+	FN_USB0_OVC1, FN_CAN_DEBUGOUT3, FN_CC50_STATE38, FN_SCIF2_SCK, FN_IRQ1,
+	FN_DU1_DB2, FN_SSI_WS9_B, FN_USB0_IDIN, FN_CAN_DEBUGOUT4,
+	FN_CC50_STATE39, FN_SCIF3_SCK, FN_IRQ2, FN_BPFCLK_D, FN_DU1_DB3,
+	FN_SSI_SDATA9_B, FN_TANS2, FN_CAN_DEBUGOUT5, FN_CC50_OSCOUT,
+	FN_SCIF3_RXD, FN_I2C1_SCL_E, FN_FMCLK_D, FN_DU1_DB4, FN_AUDIO_CLKA_C,
+	FN_SSI_SCK4_B, FN_CAN_DEBUGOUT6, FN_RDS_CLK_C, FN_SCIF3_TXD,
+	FN_I2C1_SDA_E, FN_FMIN_D, FN_DU1_DB5, FN_AUDIO_CLKB_C, FN_SSI_WS4_B,
+	FN_CAN_DEBUGOUT7, FN_RDS_DATA_C, FN_I2C2_SCL, FN_SCIFA5_RXD, FN_DU1_DB6,
+	FN_AUDIO_CLKC_C, FN_SSI_SDATA4_B, FN_CAN_DEBUGOUT8, FN_I2C2_SDA,
+	FN_SCIFA5_TXD, FN_DU1_DB7, FN_AUDIO_CLKOUT_C, FN_CAN_DEBUGOUT9,
+	FN_SSI_SCK5, FN_SCIFA3_SCK, FN_DU1_DOTCLKIN, FN_CAN_DEBUGOUT10,
+
+	/* IPSR11 */
+	FN_SSI_WS5, FN_SCIFA3_RXD, FN_I2C3_SCL_C, FN_DU1_DOTCLKOUT0,
+	FN_CAN_DEBUGOUT11, FN_SSI_SDATA5, FN_SCIFA3_TXD, FN_I2C3_SDA_C,
+	FN_DU1_DOTCLKOUT1, FN_CAN_DEBUGOUT12, FN_SSI_SCK6, FN_SCIFA1_SCK_B,
+	FN_DU1_EXHSYNC_DU1_HSYNC, FN_CAN_DEBUGOUT13, FN_SSI_WS6,
+	FN_SCIFA1_RXD_B, FN_I2C4_SCL_C, FN_DU1_EXVSYNC_DU1_VSYNC,
+	FN_CAN_DEBUGOUT14, FN_SSI_SDATA6, FN_SCIFA1_TXD_B, FN_I2C4_SDA_C,
+	FN_DU1_EXODDF_DU1_ODDF_DISP_CDE, FN_CAN_DEBUGOUT15, FN_SSI_SCK78,
+	FN_SCIFA2_SCK_B, FN_IIC0_SDA_C, FN_DU1_DISP, FN_SSI_WS78,
+	FN_SCIFA2_RXD_B, FN_IIC0_SCL_C, FN_DU1_CDE, FN_SSI_SDATA7,
+	FN_SCIFA2_TXD_B, FN_IRQ8, FN_AUDIO_CLKA_D, FN_CAN_CLK_D, FN_PCMOE_N,
+	FN_SSI_SCK0129, FN_MSIOF1_RXD_B, FN_SCIF5_RXD_D, FN_ADIDATA_B,
+	FN_AD_DI_B, FN_PCMWE_N, FN_SSI_WS0129, FN_MSIOF1_TXD_B, FN_SCIF5_TXD_D,
+	FN_ADICS_SAMP_B, FN_AD_DO_B, FN_SSI_SDATA0, FN_MSIOF1_SCK_B, FN_PWM0_B,
+	FN_ADICLK_B, FN_AD_CLK_B,
+
+	/* IPSR12 */
+	FN_SSI_SCK34, FN_MSIOF1_SYNC_B, FN_SCIFA1_SCK_C, FN_ADICHS0_B,
+	FN_AD_NCS_N_B, FN_DREQ1_N_B, FN_SSI_WS34, FN_MSIOF1_SS1_B,
+	FN_SCIFA1_RXD_C, FN_ADICHS1_B, FN_CAN1_RX_C, FN_DACK1_B, FN_SSI_SDATA3,
+	FN_MSIOF1_SS2_B, FN_SCIFA1_TXD_C, FN_ADICHS2_B, FN_CAN1_TX_C,
+	FN_DREQ2_N, FN_SSI_SCK4, FN_MLB_CLK, FN_IETX_B, FN_IRD_TX, FN_SSI_WS4,
+	FN_MLB_SIG, FN_IECLK_B, FN_IRD_RX, FN_SSI_SDATA4, FN_MLB_DAT,
+	FN_IERX_B, FN_IRD_SCK, FN_SSI_SDATA8, FN_SCIF1_SCK_B,
+	FN_PWM1_B, FN_IRQ9, FN_REMOCON, FN_DACK2, FN_ETH_MDIO_B, FN_SSI_SCK1,
+	FN_SCIF1_RXD_B, FN_IIC1_SCL_C, FN_VI1_CLK, FN_CAN0_RX_D,
+	FN_AVB_AVTP_CAPTURE, FN_ETH_CRS_DV_B, FN_SSI_WS1, FN_SCIF1_TXD_B,
+	FN_IIC1_SDA_C, FN_VI1_DATA0, FN_CAN0_TX_D, FN_AVB_AVTP_MATCH,
+	FN_ETH_RX_ER_B, FN_SSI_SDATA1, FN_HSCIF1_HRX_B, FN_SDATA, FN_VI1_DATA1,
+	FN_ATAG0_N, FN_ETH_RXD0_B, FN_SSI_SCK2, FN_HSCIF1_HTX_B, FN_VI1_DATA2,
+	FN_MDATA, FN_ATAWR0_N, FN_ETH_RXD1_B,
+
+	/* IPSR13 */
+	FN_SSI_WS2, FN_HSCIF1_HCTS_N_B, FN_SCIFA0_RXD_D, FN_VI1_DATA3, FN_SCKZ,
+	FN_ATACS00_N, FN_ETH_LINK_B, FN_SSI_SDATA2, FN_HSCIF1_HRTS_N_B,
+	FN_SCIFA0_TXD_D, FN_VI1_DATA4, FN_STM_N, FN_ATACS10_N, FN_ETH_REFCLK_B,
+	FN_SSI_SCK9, FN_SCIF2_SCK_B, FN_PWM2_B, FN_VI1_DATA5, FN_MTS_N,
+	FN_EX_WAIT1, FN_ETH_TXD1_B, FN_SSI_WS9, FN_SCIF2_RXD_B, FN_I2C3_SCL_E,
+	FN_VI1_DATA6, FN_ATARD0_N, FN_ETH_TX_EN_B, FN_SSI_SDATA9,
+	FN_SCIF2_TXD_B, FN_I2C3_SDA_E, FN_VI1_DATA7, FN_ATADIR0_N,
+	FN_ETH_MAGIC_B, FN_AUDIO_CLKA, FN_I2C0_SCL_B, FN_SCIFA4_RXD_D,
+	FN_VI1_CLKENB, FN_TS_SDATA_C, FN_RIF0_SYNC_B, FN_ETH_TXD0_B,
+	FN_AUDIO_CLKB, FN_I2C0_SDA_B, FN_SCIFA4_TXD_D, FN_VI1_FIELD,
+	FN_TS_SCK_C, FN_RIF0_CLK_B, FN_BPFCLK_E, FN_ETH_MDC_B, FN_AUDIO_CLKC,
+	FN_I2C4_SCL_B, FN_SCIFA5_RXD_D, FN_VI1_HSYNC_N, FN_TS_SDEN_C,
+	FN_RIF0_D0_B, FN_FMCLK_E, FN_RDS_CLK_D, FN_AUDIO_CLKOUT, FN_I2C4_SDA_B,
+	FN_SCIFA5_TXD_D, FN_VI1_VSYNC_N, FN_TS_SPSYNC_C, FN_RIF0_D1_B,
+	FN_FMIN_E, FN_RDS_DATA_D,
+
+	/* MOD_SEL */
+	FN_SEL_ADG_0, FN_SEL_ADG_1, FN_SEL_ADG_2, FN_SEL_ADG_3,
+	FN_SEL_ADI_0, FN_SEL_ADI_1, FN_SEL_CAN_0, FN_SEL_CAN_1,
+	FN_SEL_CAN_2, FN_SEL_CAN_3, FN_SEL_DARC_0, FN_SEL_DARC_1,
+	FN_SEL_DARC_2, FN_SEL_DARC_3, FN_SEL_DARC_4, FN_SEL_DR0_0,
+	FN_SEL_DR0_1, FN_SEL_DR1_0, FN_SEL_DR1_1, FN_SEL_DR2_0, FN_SEL_DR2_1,
+	FN_SEL_DR3_0, FN_SEL_DR3_1, FN_SEL_ETH_0, FN_SEL_ETH_1, FN_SEL_FSN_0,
+	FN_SEL_FSN_1, FN_SEL_I2C00_0, FN_SEL_I2C00_1, FN_SEL_I2C00_2,
+	FN_SEL_I2C00_3, FN_SEL_I2C00_4, FN_SEL_I2C01_0, FN_SEL_I2C01_1,
+	FN_SEL_I2C01_2, FN_SEL_I2C01_3, FN_SEL_I2C01_4, FN_SEL_I2C02_0,
+	FN_SEL_I2C02_1, FN_SEL_I2C02_2, FN_SEL_I2C02_3, FN_SEL_I2C02_4,
+	FN_SEL_I2C03_0, FN_SEL_I2C03_1, FN_SEL_I2C03_2, FN_SEL_I2C03_3,
+	FN_SEL_I2C03_4, FN_SEL_I2C04_0, FN_SEL_I2C04_1, FN_SEL_I2C04_2,
+	FN_SEL_I2C04_3, FN_SEL_I2C04_4, FN_SEL_IIC00_0, FN_SEL_IIC00_1,
+	FN_SEL_IIC00_2, FN_SEL_IIC00_3, FN_SEL_AVB_0, FN_SEL_AVB_1,
+
+	/* MOD_SEL2 */
+	FN_SEL_IEB_0, FN_SEL_IEB_1, FN_SEL_IEB_2, FN_SEL_IIC01_0,
+	FN_SEL_IIC01_1, FN_SEL_IIC01_2, FN_SEL_IIC01_3, FN_SEL_LBS_0,
+	FN_SEL_LBS_1, FN_SEL_MSI1_0, FN_SEL_MSI1_1, FN_SEL_MSI2_0,
+	FN_SEL_MSI2_1, FN_SEL_RAD_0, FN_SEL_RAD_1, FN_SEL_RCN_0,
+	FN_SEL_RCN_1, FN_SEL_RSP_0, FN_SEL_RSP_1, FN_SEL_SCIFA0_0,
+	FN_SEL_SCIFA0_1, FN_SEL_SCIFA0_2, FN_SEL_SCIFA0_3, FN_SEL_SCIFA1_0,
+	FN_SEL_SCIFA1_1, FN_SEL_SCIFA1_2, FN_SEL_SCIFA2_0, FN_SEL_SCIFA2_1,
+	FN_SEL_SCIFA3_0, FN_SEL_SCIFA3_1, FN_SEL_SCIFA4_0, FN_SEL_SCIFA4_1,
+	FN_SEL_SCIFA4_2, FN_SEL_SCIFA4_3, FN_SEL_SCIFA5_0, FN_SEL_SCIFA5_1,
+	FN_SEL_SCIFA5_2, FN_SEL_SCIFA5_3, FN_SEL_SPDM_0, FN_SEL_SPDM_1,
+	FN_SEL_TMU_0, FN_SEL_TMU_1, FN_SEL_TSIF0_0, FN_SEL_TSIF0_1,
+	FN_SEL_TSIF0_2, FN_SEL_TSIF0_3, FN_SEL_CAN0_0, FN_SEL_CAN0_1,
+	FN_SEL_CAN0_2, FN_SEL_CAN0_3, FN_SEL_CAN1_0, FN_SEL_CAN1_1,
+	FN_SEL_CAN1_2, FN_SEL_CAN1_3, FN_SEL_HSCIF0_0, FN_SEL_HSCIF0_1,
+	FN_SEL_HSCIF1_0, FN_SEL_HSCIF1_1, FN_SEL_RDS_0, FN_SEL_RDS_1,
+	FN_SEL_RDS_2, FN_SEL_RDS_3,
+
+	/* MOD_SEL3 */
+	FN_SEL_SCIF0_0, FN_SEL_SCIF0_1, FN_SEL_SCIF0_2, FN_SEL_SCIF0_3,
+	FN_SEL_SCIF1_0, FN_SEL_SCIF1_1, FN_SEL_SCIF1_2, FN_SEL_SCIF2_0,
+	FN_SEL_SCIF2_1, FN_SEL_SCIF2_2, FN_SEL_SCIF3_0, FN_SEL_SCIF3_1,
+	FN_SEL_SCIF4_0, FN_SEL_SCIF4_1, FN_SEL_SCIF4_2, FN_SEL_SCIF4_3,
+	FN_SEL_SCIF4_4, FN_SEL_SCIF5_0, FN_SEL_SCIF5_1, FN_SEL_SCIF5_2,
+	FN_SEL_SCIF5_3, FN_SEL_SSI1_0, FN_SEL_SSI1_1, FN_SEL_SSI2_0,
+	FN_SEL_SSI2_1, FN_SEL_SSI4_0, FN_SEL_SSI4_1, FN_SEL_SSI5_0,
+	FN_SEL_SSI5_1, FN_SEL_SSI6_0, FN_SEL_SSI6_1, FN_SEL_SSI7_0,
+	FN_SEL_SSI7_1, FN_SEL_SSI8_0, FN_SEL_SSI8_1, FN_SEL_SSI9_0,
+	FN_SEL_SSI9_1,
+	PINMUX_FUNCTION_END,
+
+	PINMUX_MARK_BEGIN,
+	A2_MARK, WE0_N_MARK, WE1_N_MARK, DACK0_MARK,
+
+	USB0_PWEN_MARK, USB0_OVC_MARK, USB1_PWEN_MARK, USB1_OVC_MARK,
+
+	SD0_CLK_MARK, SD0_CMD_MARK, SD0_DATA0_MARK, SD0_DATA1_MARK,
+	SD0_DATA2_MARK, SD0_DATA3_MARK, SD0_CD_MARK, SD0_WP_MARK,
+
+	SD1_CLK_MARK, SD1_CMD_MARK, SD1_DATA0_MARK, SD1_DATA1_MARK,
+	SD1_DATA2_MARK, SD1_DATA3_MARK,
+
+	/* IPSR0 */
+	SD1_CD_MARK, CAN0_RX_MARK, SD1_WP_MARK, IRQ7_MARK, CAN0_TX_MARK,
+	MMC_CLK_MARK, SD2_CLK_MARK, MMC_CMD_MARK, SD2_CMD_MARK, MMC_D0_MARK,
+	SD2_DATA0_MARK, MMC_D1_MARK, SD2_DATA1_MARK, MMC_D2_MARK,
+	SD2_DATA2_MARK, MMC_D3_MARK, SD2_DATA3_MARK, MMC_D4_MARK, SD2_CD_MARK,
+	MMC_D5_MARK, SD2_WP_MARK, MMC_D6_MARK, SCIF0_RXD_MARK, I2C2_SCL_B_MARK,
+	CAN1_RX_MARK, MMC_D7_MARK, SCIF0_TXD_MARK, I2C2_SDA_B_MARK,
+	CAN1_TX_MARK, D0_MARK, SCIFA3_SCK_B_MARK, IRQ4_MARK, D1_MARK,
+	SCIFA3_RXD_B_MARK, D2_MARK, SCIFA3_TXD_B_MARK, D3_MARK, I2C3_SCL_B_MARK,
+	SCIF5_RXD_B_MARK, D4_MARK, I2C3_SDA_B_MARK, SCIF5_TXD_B_MARK, D5_MARK,
+	SCIF4_RXD_B_MARK, I2C0_SCL_D_MARK,
+
+	/* IPSR1 */
+	D6_MARK, SCIF4_TXD_B_MARK, I2C0_SDA_D_MARK, D7_MARK, IRQ3_MARK,
+	TCLK1_MARK, PWM6_B_MARK, D8_MARK, HSCIF2_HRX_MARK, I2C1_SCL_B_MARK,
+	D9_MARK, HSCIF2_HTX_MARK, I2C1_SDA_B_MARK, D10_MARK,
+	HSCIF2_HSCK_MARK, SCIF1_SCK_C_MARK, IRQ6_MARK, PWM5_C_MARK,
+	D11_MARK, HSCIF2_HCTS_N_MARK, SCIF1_RXD_C_MARK, I2C1_SCL_D_MARK,
+	D12_MARK, HSCIF2_HRTS_N_MARK, SCIF1_TXD_C_MARK, I2C1_SDA_D_MARK,
+	D13_MARK, SCIFA1_SCK_MARK, TANS1_MARK, PWM2_C_MARK, TCLK2_B_MARK,
+	D14_MARK, SCIFA1_RXD_MARK, IIC0_SCL_B_MARK, D15_MARK, SCIFA1_TXD_MARK,
+	IIC0_SDA_B_MARK, A0_MARK, SCIFB1_SCK_MARK, PWM3_B_MARK, A1_MARK,
+	SCIFB1_TXD_MARK, A3_MARK, SCIFB0_SCK_MARK, A4_MARK, SCIFB0_TXD_MARK,
+	A5_MARK, SCIFB0_RXD_MARK, PWM4_B_MARK, TPUTO3_C_MARK, A6_MARK,
+	SCIFB0_CTS_N_MARK, SCIFA4_RXD_B_MARK, TPUTO2_C_MARK,
+
+	/* IPSR2 */
+	A7_MARK, SCIFB0_RTS_N_MARK, SCIFA4_TXD_B_MARK, A8_MARK, MSIOF1_RXD_MARK,
+	SCIFA0_RXD_B_MARK, A9_MARK, MSIOF1_TXD_MARK, SCIFA0_TXD_B_MARK,
+	A10_MARK, MSIOF1_SCK_MARK, IIC1_SCL_B_MARK, A11_MARK, MSIOF1_SYNC_MARK,
+	IIC1_SDA_B_MARK, A12_MARK, MSIOF1_SS1_MARK, SCIFA5_RXD_B_MARK,
+	A13_MARK, MSIOF1_SS2_MARK, SCIFA5_TXD_B_MARK, A14_MARK, MSIOF2_RXD_MARK,
+	HSCIF0_HRX_B_MARK, DREQ1_N_MARK, A15_MARK, MSIOF2_TXD_MARK,
+	HSCIF0_HTX_B_MARK, DACK1_MARK, A16_MARK, MSIOF2_SCK_MARK,
+	HSCIF0_HSCK_B_MARK, SPEEDIN_MARK, VSP_MARK, CAN_CLK_C_MARK,
+	TPUTO2_B_MARK, A17_MARK, MSIOF2_SYNC_MARK, SCIF4_RXD_E_MARK,
+	CAN1_RX_B_MARK, AVB_AVTP_CAPTURE_B_MARK, A18_MARK, MSIOF2_SS1_MARK,
+	SCIF4_TXD_E_MARK, CAN1_TX_B_MARK, AVB_AVTP_MATCH_B_MARK, A19_MARK,
+	MSIOF2_SS2_MARK, PWM4_MARK, TPUTO2_MARK, MOUT0_MARK, A20_MARK,
+	SPCLK_MARK, MOUT1_MARK,
+
+	/* IPSR3 */
+	A21_MARK, MOSI_IO0_MARK, MOUT2_MARK, A22_MARK, MISO_IO1_MARK,
+	MOUT5_MARK, ATADIR1_N_MARK, A23_MARK, IO2_MARK, MOUT6_MARK,
+	ATAWR1_N_MARK, A24_MARK, IO3_MARK, EX_WAIT2_MARK, A25_MARK, SSL_MARK,
+	ATARD1_N_MARK, CS0_N_MARK, VI1_DATA8_MARK, CS1_N_A26_MARK,
+	VI1_DATA9_MARK, EX_CS0_N_MARK, VI1_DATA10_MARK, EX_CS1_N_MARK,
+	TPUTO3_B_MARK, SCIFB2_RXD_MARK, VI1_DATA11_MARK, EX_CS2_N_MARK,
+	PWM0_MARK, SCIF4_RXD_C_MARK, TS_SDATA_B_MARK, RIF0_SYNC_MARK,
+	TPUTO3_MARK, SCIFB2_TXD_MARK, SDATA_B_MARK, EX_CS3_N_MARK,
+	SCIFA2_SCK_MARK, SCIF4_TXD_C_MARK, TS_SCK_B_MARK, RIF0_CLK_MARK,
+	BPFCLK_MARK, SCIFB2_SCK_MARK, MDATA_B_MARK, EX_CS4_N_MARK,
+	SCIFA2_RXD_MARK, I2C2_SCL_E_MARK, TS_SDEN_B_MARK, RIF0_D0_MARK,
+	FMCLK_MARK, SCIFB2_CTS_N_MARK, SCKZ_B_MARK, EX_CS5_N_MARK,
+	SCIFA2_TXD_MARK, I2C2_SDA_E_MARK, TS_SPSYNC_B_MARK, RIF0_D1_MARK,
+	FMIN_MARK, SCIFB2_RTS_N_MARK, STM_N_B_MARK, BS_N_MARK, DRACK0_MARK,
+	PWM1_C_MARK, TPUTO0_C_MARK, ATACS01_N_MARK, MTS_N_B_MARK, RD_N_MARK,
+	ATACS11_N_MARK, RD_WR_N_MARK, ATAG1_N_MARK,
+
+	/* IPSR4 */
+	EX_WAIT0_MARK, CAN_CLK_B_MARK, SCIF_CLK_MARK, PWMFSW0_MARK,
+	DU0_DR0_MARK, LCDOUT16_MARK, SCIF5_RXD_C_MARK, I2C2_SCL_D_MARK,
+	CC50_STATE0_MARK, DU0_DR1_MARK, LCDOUT17_MARK, SCIF5_TXD_C_MARK,
+	I2C2_SDA_D_MARK, CC50_STATE1_MARK, DU0_DR2_MARK, LCDOUT18_MARK,
+	CC50_STATE2_MARK, DU0_DR3_MARK, LCDOUT19_MARK, CC50_STATE3_MARK,
+	DU0_DR4_MARK, LCDOUT20_MARK, CC50_STATE4_MARK, DU0_DR5_MARK,
+	LCDOUT21_MARK, CC50_STATE5_MARK, DU0_DR6_MARK, LCDOUT22_MARK,
+	CC50_STATE6_MARK, DU0_DR7_MARK, LCDOUT23_MARK, CC50_STATE7_MARK,
+	DU0_DG0_MARK, LCDOUT8_MARK, SCIFA0_RXD_C_MARK, I2C3_SCL_D_MARK,
+	CC50_STATE8_MARK, DU0_DG1_MARK, LCDOUT9_MARK, SCIFA0_TXD_C_MARK,
+	I2C3_SDA_D_MARK, CC50_STATE9_MARK, DU0_DG2_MARK, LCDOUT10_MARK,
+	CC50_STATE10_MARK, DU0_DG3_MARK, LCDOUT11_MARK, CC50_STATE11_MARK,
+	DU0_DG4_MARK, LCDOUT12_MARK, CC50_STATE12_MARK,
+
+	/* IPSR5 */
+	DU0_DG5_MARK, LCDOUT13_MARK, CC50_STATE13_MARK, DU0_DG6_MARK,
+	LCDOUT14_MARK, CC50_STATE14_MARK, DU0_DG7_MARK, LCDOUT15_MARK,
+	CC50_STATE15_MARK, DU0_DB0_MARK, LCDOUT0_MARK, SCIFA4_RXD_C_MARK,
+	I2C4_SCL_D_MARK, CAN0_RX_C_MARK, CC50_STATE16_MARK, DU0_DB1_MARK,
+	LCDOUT1_MARK, SCIFA4_TXD_C_MARK, I2C4_SDA_D_MARK, CAN0_TX_C_MARK,
+	CC50_STATE17_MARK, DU0_DB2_MARK, LCDOUT2_MARK, CC50_STATE18_MARK,
+	DU0_DB3_MARK, LCDOUT3_MARK, CC50_STATE19_MARK, DU0_DB4_MARK,
+	LCDOUT4_MARK, CC50_STATE20_MARK, DU0_DB5_MARK, LCDOUT5_MARK,
+	CC50_STATE21_MARK, DU0_DB6_MARK, LCDOUT6_MARK, CC50_STATE22_MARK,
+	DU0_DB7_MARK, LCDOUT7_MARK, CC50_STATE23_MARK, DU0_DOTCLKIN_MARK,
+	QSTVA_QVS_MARK, CC50_STATE24_MARK, DU0_DOTCLKOUT0_MARK,
+	QCLK_MARK, CC50_STATE25_MARK, DU0_DOTCLKOUT1_MARK, QSTVB_QVE_MARK,
+	CC50_STATE26_MARK, DU0_EXHSYNC_DU0_HSYNC_MARK, QSTH_QHS_MARK,
+	CC50_STATE27_MARK,
+
+	/* IPSR6 */
+	DU0_EXVSYNC_DU0_VSYNC_MARK, QSTB_QHE_MARK, CC50_STATE28_MARK,
+	DU0_EXODDF_DU0_ODDF_DISP_CDE_MARK, QCPV_QDE_MARK, CC50_STATE29_MARK,
+	DU0_DISP_MARK, QPOLA_MARK, CC50_STATE30_MARK, DU0_CDE_MARK, QPOLB_MARK,
+	CC50_STATE31_MARK, VI0_CLK_MARK, AVB_RX_CLK_MARK, VI0_DATA0_VI0_B0_MARK,
+	AVB_RX_DV_MARK, VI0_DATA1_VI0_B1_MARK, AVB_RXD0_MARK,
+	VI0_DATA2_VI0_B2_MARK, AVB_RXD1_MARK, VI0_DATA3_VI0_B3_MARK,
+	AVB_RXD2_MARK, VI0_DATA4_VI0_B4_MARK, AVB_RXD3_MARK,
+	VI0_DATA5_VI0_B5_MARK, AVB_RXD4_MARK, VI0_DATA6_VI0_B6_MARK,
+	AVB_RXD5_MARK, VI0_DATA7_VI0_B7_MARK, AVB_RXD6_MARK, VI0_CLKENB_MARK,
+	I2C3_SCL_MARK, SCIFA5_RXD_C_MARK, IETX_C_MARK, AVB_RXD7_MARK,
+	VI0_FIELD_MARK, I2C3_SDA_MARK, SCIFA5_TXD_C_MARK, IECLK_C_MARK,
+	AVB_RX_ER_MARK, VI0_HSYNC_N_MARK, SCIF0_RXD_B_MARK, I2C0_SCL_C_MARK,
+	IERX_C_MARK, AVB_COL_MARK, VI0_VSYNC_N_MARK, SCIF0_TXD_B_MARK,
+	I2C0_SDA_C_MARK, AUDIO_CLKOUT_B_MARK, AVB_TX_EN_MARK, ETH_MDIO_MARK,
+	VI0_G0_MARK, MSIOF2_RXD_B_MARK, IIC0_SCL_D_MARK, AVB_TX_CLK_MARK,
+	ADIDATA_MARK, AD_DI_MARK,
+
+	/* IPSR7 */
+	ETH_CRS_DV_MARK, VI0_G1_MARK, MSIOF2_TXD_B_MARK, IIC0_SDA_D_MARK,
+	AVB_TXD0_MARK, ADICS_SAMP_MARK, AD_DO_MARK, ETH_RX_ER_MARK, VI0_G2_MARK,
+	MSIOF2_SCK_B_MARK, CAN0_RX_B_MARK, AVB_TXD1_MARK, ADICLK_MARK,
+	AD_CLK_MARK, ETH_RXD0_MARK, VI0_G3_MARK, MSIOF2_SYNC_B_MARK,
+	CAN0_TX_B_MARK, AVB_TXD2_MARK, ADICHS0_MARK, AD_NCS_N_MARK,
+	ETH_RXD1_MARK, VI0_G4_MARK, MSIOF2_SS1_B_MARK, SCIF4_RXD_D_MARK,
+	AVB_TXD3_MARK, ADICHS1_MARK, ETH_LINK_MARK, VI0_G5_MARK,
+	MSIOF2_SS2_B_MARK, SCIF4_TXD_D_MARK, AVB_TXD4_MARK, ADICHS2_MARK,
+	ETH_REFCLK_MARK, VI0_G6_MARK, SCIF2_SCK_C_MARK, AVB_TXD5_MARK,
+	SSI_SCK5_B_MARK, ETH_TXD1_MARK, VI0_G7_MARK, SCIF2_RXD_C_MARK,
+	IIC1_SCL_D_MARK, AVB_TXD6_MARK, SSI_WS5_B_MARK, ETH_TX_EN_MARK,
+	VI0_R0_MARK, SCIF2_TXD_C_MARK, IIC1_SDA_D_MARK, AVB_TXD7_MARK,
+	SSI_SDATA5_B_MARK, ETH_MAGIC_MARK, VI0_R1_MARK, SCIF3_SCK_B_MARK,
+	AVB_TX_ER_MARK, SSI_SCK6_B_MARK, ETH_TXD0_MARK, VI0_R2_MARK,
+	SCIF3_RXD_B_MARK, I2C4_SCL_E_MARK, AVB_GTX_CLK_MARK, SSI_WS6_B_MARK,
+	DREQ0_N_MARK, SCIFB1_RXD_MARK,
+
+	/* IPSR8 */
+	ETH_MDC_MARK, VI0_R3_MARK, SCIF3_TXD_B_MARK, I2C4_SDA_E_MARK,
+	AVB_MDC_MARK, SSI_SDATA6_B_MARK, HSCIF0_HRX_MARK, VI0_R4_MARK,
+	I2C1_SCL_C_MARK, AUDIO_CLKA_B_MARK, AVB_MDIO_MARK, SSI_SCK78_B_MARK,
+	HSCIF0_HTX_MARK, VI0_R5_MARK, I2C1_SDA_C_MARK, AUDIO_CLKB_B_MARK,
+	AVB_LINK_MARK, SSI_WS78_B_MARK, HSCIF0_HCTS_N_MARK, VI0_R6_MARK,
+	SCIF0_RXD_D_MARK, I2C0_SCL_E_MARK, AVB_MAGIC_MARK, SSI_SDATA7_B_MARK,
+	HSCIF0_HRTS_N_MARK, VI0_R7_MARK, SCIF0_TXD_D_MARK, I2C0_SDA_E_MARK,
+	AVB_PHY_INT_MARK, SSI_SDATA8_B_MARK,
+	HSCIF0_HSCK_MARK, SCIF_CLK_B_MARK, AVB_CRS_MARK, AUDIO_CLKC_B_MARK,
+	I2C0_SCL_MARK, SCIF0_RXD_C_MARK, PWM5_MARK, TCLK1_B_MARK,
+	AVB_GTXREFCLK_MARK, CAN1_RX_D_MARK, TPUTO0_B_MARK, I2C0_SDA_MARK,
+	SCIF0_TXD_C_MARK, TPUTO0_MARK, CAN_CLK_MARK, DVC_MUTE_MARK,
+	CAN1_TX_D_MARK, I2C1_SCL_MARK, SCIF4_RXD_MARK, PWM5_B_MARK,
+	DU1_DR0_MARK, RIF1_SYNC_B_MARK, TS_SDATA_D_MARK, TPUTO1_B_MARK,
+	I2C1_SDA_MARK, SCIF4_TXD_MARK, IRQ5_MARK, DU1_DR1_MARK, RIF1_CLK_B_MARK,
+	TS_SCK_D_MARK, BPFCLK_C_MARK, MSIOF0_RXD_MARK, SCIF5_RXD_MARK,
+	I2C2_SCL_C_MARK, DU1_DR2_MARK, RIF1_D0_B_MARK, TS_SDEN_D_MARK,
+	FMCLK_C_MARK, RDS_CLK_MARK,
+
+	/* IPSR9 */
+	MSIOF0_TXD_MARK, SCIF5_TXD_MARK, I2C2_SDA_C_MARK, DU1_DR3_MARK,
+	RIF1_D1_B_MARK, TS_SPSYNC_D_MARK, FMIN_C_MARK, RDS_DATA_MARK,
+	MSIOF0_SCK_MARK, IRQ0_MARK, TS_SDATA_MARK, DU1_DR4_MARK, RIF1_SYNC_MARK,
+	TPUTO1_C_MARK, MSIOF0_SYNC_MARK, PWM1_MARK, TS_SCK_MARK, DU1_DR5_MARK,
+	RIF1_CLK_MARK, BPFCLK_B_MARK, MSIOF0_SS1_MARK, SCIFA0_RXD_MARK,
+	TS_SDEN_MARK, DU1_DR6_MARK, RIF1_D0_MARK, FMCLK_B_MARK, RDS_CLK_B_MARK,
+	MSIOF0_SS2_MARK, SCIFA0_TXD_MARK, TS_SPSYNC_MARK, DU1_DR7_MARK,
+	RIF1_D1_MARK, FMIN_B_MARK, RDS_DATA_B_MARK, HSCIF1_HRX_MARK,
+	I2C4_SCL_MARK, PWM6_MARK, DU1_DG0_MARK, HSCIF1_HTX_MARK,
+	I2C4_SDA_MARK, TPUTO1_MARK, DU1_DG1_MARK, HSCIF1_HSCK_MARK,
+	PWM2_MARK, IETX_MARK, DU1_DG2_MARK, REMOCON_B_MARK, SPEEDIN_B_MARK,
+	VSP_B_MARK, HSCIF1_HCTS_N_MARK, SCIFA4_RXD_MARK, IECLK_MARK,
+	DU1_DG3_MARK, SSI_SCK1_B_MARK, CAN_DEBUG_HW_TRIGGER_MARK,
+	CC50_STATE32_MARK, HSCIF1_HRTS_N_MARK, SCIFA4_TXD_MARK, IERX_MARK,
+	DU1_DG4_MARK, SSI_WS1_B_MARK, CAN_STEP0_MARK, CC50_STATE33_MARK,
+	SCIF1_SCK_MARK, PWM3_MARK, TCLK2_MARK, DU1_DG5_MARK, SSI_SDATA1_B_MARK,
+	CAN_TXCLK_MARK, CC50_STATE34_MARK,
+
+	/* IPSR10 */
+	SCIF1_RXD_MARK, IIC0_SCL_MARK, DU1_DG6_MARK, SSI_SCK2_B_MARK,
+	CAN_DEBUGOUT0_MARK, CC50_STATE35_MARK, SCIF1_TXD_MARK, IIC0_SDA_MARK,
+	DU1_DG7_MARK, SSI_WS2_B_MARK, CAN_DEBUGOUT1_MARK, CC50_STATE36_MARK,
+	SCIF2_RXD_MARK, IIC1_SCL_MARK, DU1_DB0_MARK, SSI_SDATA2_B_MARK,
+	USB0_EXTLP_MARK, CAN_DEBUGOUT2_MARK, CC50_STATE37_MARK, SCIF2_TXD_MARK,
+	IIC1_SDA_MARK, DU1_DB1_MARK, SSI_SCK9_B_MARK, USB0_OVC1_MARK,
+	CAN_DEBUGOUT3_MARK, CC50_STATE38_MARK, SCIF2_SCK_MARK, IRQ1_MARK,
+	DU1_DB2_MARK, SSI_WS9_B_MARK, USB0_IDIN_MARK, CAN_DEBUGOUT4_MARK,
+	CC50_STATE39_MARK, SCIF3_SCK_MARK, IRQ2_MARK, BPFCLK_D_MARK,
+	DU1_DB3_MARK, SSI_SDATA9_B_MARK, TANS2_MARK, CAN_DEBUGOUT5_MARK,
+	CC50_OSCOUT_MARK, SCIF3_RXD_MARK, I2C1_SCL_E_MARK, FMCLK_D_MARK,
+	DU1_DB4_MARK, AUDIO_CLKA_C_MARK, SSI_SCK4_B_MARK, CAN_DEBUGOUT6_MARK,
+	RDS_CLK_C_MARK, SCIF3_TXD_MARK, I2C1_SDA_E_MARK, FMIN_D_MARK,
+	DU1_DB5_MARK, AUDIO_CLKB_C_MARK, SSI_WS4_B_MARK, CAN_DEBUGOUT7_MARK,
+	RDS_DATA_C_MARK, I2C2_SCL_MARK, SCIFA5_RXD_MARK, DU1_DB6_MARK,
+	AUDIO_CLKC_C_MARK, SSI_SDATA4_B_MARK, CAN_DEBUGOUT8_MARK, I2C2_SDA_MARK,
+	SCIFA5_TXD_MARK, DU1_DB7_MARK, AUDIO_CLKOUT_C_MARK, CAN_DEBUGOUT9_MARK,
+	SSI_SCK5_MARK, SCIFA3_SCK_MARK, DU1_DOTCLKIN_MARK, CAN_DEBUGOUT10_MARK,
+
+	/* IPSR11 */
+	SSI_WS5_MARK, SCIFA3_RXD_MARK, I2C3_SCL_C_MARK, DU1_DOTCLKOUT0_MARK,
+	CAN_DEBUGOUT11_MARK, SSI_SDATA5_MARK, SCIFA3_TXD_MARK, I2C3_SDA_C_MARK,
+	DU1_DOTCLKOUT1_MARK, CAN_DEBUGOUT12_MARK, SSI_SCK6_MARK,
+	SCIFA1_SCK_B_MARK, DU1_EXHSYNC_DU1_HSYNC_MARK, CAN_DEBUGOUT13_MARK,
+	SSI_WS6_MARK, SCIFA1_RXD_B_MARK, I2C4_SCL_C_MARK,
+	DU1_EXVSYNC_DU1_VSYNC_MARK, CAN_DEBUGOUT14_MARK, SSI_SDATA6_MARK,
+	SCIFA1_TXD_B_MARK, I2C4_SDA_C_MARK, DU1_EXODDF_DU1_ODDF_DISP_CDE_MARK,
+	CAN_DEBUGOUT15_MARK, SSI_SCK78_MARK, SCIFA2_SCK_B_MARK, IIC0_SDA_C_MARK,
+	DU1_DISP_MARK, SSI_WS78_MARK, SCIFA2_RXD_B_MARK, IIC0_SCL_C_MARK,
+	DU1_CDE_MARK, SSI_SDATA7_MARK, SCIFA2_TXD_B_MARK, IRQ8_MARK,
+	AUDIO_CLKA_D_MARK, CAN_CLK_D_MARK, PCMOE_N_MARK, SSI_SCK0129_MARK,
+	MSIOF1_RXD_B_MARK, SCIF5_RXD_D_MARK, ADIDATA_B_MARK, AD_DI_B_MARK,
+	PCMWE_N_MARK, SSI_WS0129_MARK, MSIOF1_TXD_B_MARK, SCIF5_TXD_D_MARK,
+	ADICS_SAMP_B_MARK, AD_DO_B_MARK, SSI_SDATA0_MARK, MSIOF1_SCK_B_MARK,
+	PWM0_B_MARK, ADICLK_B_MARK, AD_CLK_B_MARK,
+
+	/* IPSR12 */
+	SSI_SCK34_MARK, MSIOF1_SYNC_B_MARK, SCIFA1_SCK_C_MARK, ADICHS0_B_MARK,
+	AD_NCS_N_B_MARK, DREQ1_N_B_MARK, SSI_WS34_MARK, MSIOF1_SS1_B_MARK,
+	SCIFA1_RXD_C_MARK, ADICHS1_B_MARK, CAN1_RX_C_MARK, DACK1_B_MARK,
+	SSI_SDATA3_MARK, MSIOF1_SS2_B_MARK, SCIFA1_TXD_C_MARK, ADICHS2_B_MARK,
+	CAN1_TX_C_MARK, DREQ2_N_MARK, SSI_SCK4_MARK, MLB_CLK_MARK, IETX_B_MARK,
+	IRD_TX_MARK, SSI_WS4_MARK, MLB_SIG_MARK, IECLK_B_MARK, IRD_RX_MARK,
+	SSI_SDATA4_MARK, MLB_DAT_MARK, IERX_B_MARK, IRD_SCK_MARK,
+	SSI_SDATA8_MARK, SCIF1_SCK_B_MARK, PWM1_B_MARK, IRQ9_MARK, REMOCON_MARK,
+	DACK2_MARK, ETH_MDIO_B_MARK, SSI_SCK1_MARK, SCIF1_RXD_B_MARK,
+	IIC1_SCL_C_MARK, VI1_CLK_MARK, CAN0_RX_D_MARK, AVB_AVTP_CAPTURE_MARK,
+	ETH_CRS_DV_B_MARK, SSI_WS1_MARK, SCIF1_TXD_B_MARK, IIC1_SDA_C_MARK,
+	VI1_DATA0_MARK, CAN0_TX_D_MARK, AVB_AVTP_MATCH_MARK, ETH_RX_ER_B_MARK,
+	SSI_SDATA1_MARK, HSCIF1_HRX_B_MARK, VI1_DATA1_MARK, SDATA_MARK,
+	ATAG0_N_MARK, ETH_RXD0_B_MARK, SSI_SCK2_MARK, HSCIF1_HTX_B_MARK,
+	VI1_DATA2_MARK, MDATA_MARK, ATAWR0_N_MARK, ETH_RXD1_B_MARK,
+
+	/* IPSR13 */
+	SSI_WS2_MARK, HSCIF1_HCTS_N_B_MARK, SCIFA0_RXD_D_MARK, VI1_DATA3_MARK,
+	SCKZ_MARK, ATACS00_N_MARK, ETH_LINK_B_MARK, SSI_SDATA2_MARK,
+	HSCIF1_HRTS_N_B_MARK, SCIFA0_TXD_D_MARK, VI1_DATA4_MARK, STM_N_MARK,
+	ATACS10_N_MARK, ETH_REFCLK_B_MARK, SSI_SCK9_MARK, SCIF2_SCK_B_MARK,
+	PWM2_B_MARK, VI1_DATA5_MARK, MTS_N_MARK, EX_WAIT1_MARK,
+	ETH_TXD1_B_MARK, SSI_WS9_MARK, SCIF2_RXD_B_MARK, I2C3_SCL_E_MARK,
+	VI1_DATA6_MARK, ATARD0_N_MARK, ETH_TX_EN_B_MARK, SSI_SDATA9_MARK,
+	SCIF2_TXD_B_MARK, I2C3_SDA_E_MARK, VI1_DATA7_MARK, ATADIR0_N_MARK,
+	ETH_MAGIC_B_MARK, AUDIO_CLKA_MARK, I2C0_SCL_B_MARK, SCIFA4_RXD_D_MARK,
+	VI1_CLKENB_MARK, TS_SDATA_C_MARK, RIF0_SYNC_B_MARK, ETH_TXD0_B_MARK,
+	AUDIO_CLKB_MARK, I2C0_SDA_B_MARK, SCIFA4_TXD_D_MARK, VI1_FIELD_MARK,
+	TS_SCK_C_MARK, RIF0_CLK_B_MARK, BPFCLK_E_MARK, ETH_MDC_B_MARK,
+	AUDIO_CLKC_MARK, I2C4_SCL_B_MARK, SCIFA5_RXD_D_MARK, VI1_HSYNC_N_MARK,
+	TS_SDEN_C_MARK, RIF0_D0_B_MARK, FMCLK_E_MARK, RDS_CLK_D_MARK,
+	AUDIO_CLKOUT_MARK, I2C4_SDA_B_MARK, SCIFA5_TXD_D_MARK, VI1_VSYNC_N_MARK,
+	TS_SPSYNC_C_MARK, RIF0_D1_B_MARK, FMIN_E_MARK, RDS_DATA_D_MARK,
+	PINMUX_MARK_END,
+};
+
+static const u16 pinmux_data[] = {
+	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
+
+	PINMUX_DATA(A2_MARK, FN_A2),
+	PINMUX_DATA(WE0_N_MARK, FN_WE0_N),
+	PINMUX_DATA(WE1_N_MARK, FN_WE1_N),
+	PINMUX_DATA(DACK0_MARK, FN_DACK0),
+	PINMUX_DATA(USB0_PWEN_MARK, FN_USB0_PWEN),
+	PINMUX_DATA(USB0_OVC_MARK, FN_USB0_OVC),
+	PINMUX_DATA(USB1_PWEN_MARK, FN_USB1_PWEN),
+	PINMUX_DATA(USB1_OVC_MARK, FN_USB1_OVC),
+	PINMUX_DATA(SD0_CLK_MARK, FN_SD0_CLK),
+	PINMUX_DATA(SD0_CMD_MARK, FN_SD0_CMD),
+	PINMUX_DATA(SD0_DATA0_MARK, FN_SD0_DATA0),
+	PINMUX_DATA(SD0_DATA1_MARK, FN_SD0_DATA1),
+	PINMUX_DATA(SD0_DATA2_MARK, FN_SD0_DATA2),
+	PINMUX_DATA(SD0_DATA3_MARK, FN_SD0_DATA3),
+	PINMUX_DATA(SD0_CD_MARK, FN_SD0_CD),
+	PINMUX_DATA(SD0_WP_MARK, FN_SD0_WP),
+	PINMUX_DATA(SD1_CLK_MARK, FN_SD1_CLK),
+	PINMUX_DATA(SD1_CMD_MARK, FN_SD1_CMD),
+	PINMUX_DATA(SD1_DATA0_MARK, FN_SD1_DATA0),
+	PINMUX_DATA(SD1_DATA1_MARK, FN_SD1_DATA1),
+	PINMUX_DATA(SD1_DATA2_MARK, FN_SD1_DATA2),
+	PINMUX_DATA(SD1_DATA3_MARK, FN_SD1_DATA3),
+
+	/* IPSR0 */
+	PINMUX_IPSR_DATA(IP0_0, SD1_CD),
+	PINMUX_IPSR_MODSEL_DATA(IP0_0, CAN0_RX, SEL_CAN0_0),
+	PINMUX_IPSR_DATA(IP0_9_8, SD1_WP),
+	PINMUX_IPSR_DATA(IP0_9_8, IRQ7),
+	PINMUX_IPSR_MODSEL_DATA(IP0_9_8, CAN0_TX, SEL_CAN0_0),
+	PINMUX_IPSR_DATA(IP0_10, MMC_CLK),
+	PINMUX_IPSR_DATA(IP0_10, SD2_CLK),
+	PINMUX_IPSR_DATA(IP0_11, MMC_CMD),
+	PINMUX_IPSR_DATA(IP0_11, SD2_CMD),
+	PINMUX_IPSR_DATA(IP0_12, MMC_D0),
+	PINMUX_IPSR_DATA(IP0_12, SD2_DATA0),
+	PINMUX_IPSR_DATA(IP0_13, MMC_D1),
+	PINMUX_IPSR_DATA(IP0_13, SD2_DATA1),
+	PINMUX_IPSR_DATA(IP0_14, MMC_D2),
+	PINMUX_IPSR_DATA(IP0_14, SD2_DATA2),
+	PINMUX_IPSR_DATA(IP0_15, MMC_D3),
+	PINMUX_IPSR_DATA(IP0_15, SD2_DATA3),
+	PINMUX_IPSR_DATA(IP0_16, MMC_D4),
+	PINMUX_IPSR_DATA(IP0_16, SD2_CD),
+	PINMUX_IPSR_DATA(IP0_17, MMC_D5),
+	PINMUX_IPSR_DATA(IP0_17, SD2_WP),
+	PINMUX_IPSR_DATA(IP0_19_18, MMC_D6),
+	PINMUX_IPSR_MODSEL_DATA(IP0_19_18, SCIF0_RXD, SEL_SCIF0_0),
+	PINMUX_IPSR_MODSEL_DATA(IP0_19_18, I2C2_SCL_B, SEL_I2C02_1),
+	PINMUX_IPSR_MODSEL_DATA(IP0_19_18, CAN1_RX, SEL_CAN1_0),
+	PINMUX_IPSR_DATA(IP0_21_20, MMC_D7),
+	PINMUX_IPSR_MODSEL_DATA(IP0_21_20, SCIF0_TXD, SEL_SCIF0_0),
+	PINMUX_IPSR_MODSEL_DATA(IP0_21_20, I2C2_SDA_B, SEL_I2C02_1),
+	PINMUX_IPSR_MODSEL_DATA(IP0_21_20, CAN1_TX, SEL_CAN1_0),
+	PINMUX_IPSR_DATA(IP0_23_22, D0),
+	PINMUX_IPSR_MODSEL_DATA(IP0_23_22, SCIFA3_SCK_B, SEL_SCIFA3_1),
+	PINMUX_IPSR_DATA(IP0_23_22, IRQ4),
+	PINMUX_IPSR_DATA(IP0_24, D1),
+	PINMUX_IPSR_MODSEL_DATA(IP0_24, SCIFA3_RXD_B, SEL_SCIFA3_1),
+	PINMUX_IPSR_DATA(IP0_25, D2),
+	PINMUX_IPSR_MODSEL_DATA(IP0_25, SCIFA3_TXD_B, SEL_SCIFA3_1),
+	PINMUX_IPSR_DATA(IP0_27_26, D3),
+	PINMUX_IPSR_MODSEL_DATA(IP0_27_26, I2C3_SCL_B, SEL_I2C03_1),
+	PINMUX_IPSR_MODSEL_DATA(IP0_27_26, SCIF5_RXD_B, SEL_SCIF5_1),
+	PINMUX_IPSR_DATA(IP0_29_28, D4),
+	PINMUX_IPSR_MODSEL_DATA(IP0_29_28, I2C3_SDA_B, SEL_I2C03_1),
+	PINMUX_IPSR_MODSEL_DATA(IP0_29_28, SCIF5_TXD_B, SEL_SCIF5_1),
+	PINMUX_IPSR_DATA(IP0_31_30, D5),
+	PINMUX_IPSR_MODSEL_DATA(IP0_31_30, SCIF4_RXD_B, SEL_SCIF4_1),
+	PINMUX_IPSR_MODSEL_DATA(IP0_31_30, I2C0_SCL_D, SEL_I2C00_3),
+
+	/* IPSR1 */
+	PINMUX_IPSR_DATA(IP1_1_0, D6),
+	PINMUX_IPSR_MODSEL_DATA(IP1_1_0, SCIF4_TXD_B, SEL_SCIF4_1),
+	PINMUX_IPSR_MODSEL_DATA(IP1_1_0, I2C0_SDA_D, SEL_I2C00_3),
+	PINMUX_IPSR_DATA(IP1_3_2, D7),
+	PINMUX_IPSR_DATA(IP1_3_2, IRQ3),
+	PINMUX_IPSR_MODSEL_DATA(IP1_3_2, TCLK1, SEL_TMU_0),
+	PINMUX_IPSR_DATA(IP1_3_2, PWM6_B),
+	PINMUX_IPSR_DATA(IP1_5_4, D8),
+	PINMUX_IPSR_DATA(IP1_5_4, HSCIF2_HRX),
+	PINMUX_IPSR_MODSEL_DATA(IP1_5_4, I2C1_SCL_B, SEL_I2C01_1),
+	PINMUX_IPSR_DATA(IP1_7_6, D9),
+	PINMUX_IPSR_DATA(IP1_7_6, HSCIF2_HTX),
+	PINMUX_IPSR_MODSEL_DATA(IP1_7_6, I2C1_SDA_B, SEL_I2C01_1),
+	PINMUX_IPSR_DATA(IP1_10_8, D10),
+	PINMUX_IPSR_DATA(IP1_10_8, HSCIF2_HSCK),
+	PINMUX_IPSR_MODSEL_DATA(IP1_10_8, SCIF1_SCK_C, SEL_SCIF1_2),
+	PINMUX_IPSR_DATA(IP1_10_8, IRQ6),
+	PINMUX_IPSR_DATA(IP1_10_8, PWM5_C),
+	PINMUX_IPSR_DATA(IP1_12_11, D11),
+	PINMUX_IPSR_DATA(IP1_12_11, HSCIF2_HCTS_N),
+	PINMUX_IPSR_MODSEL_DATA(IP1_12_11, SCIF1_RXD_C, SEL_SCIF1_2),
+	PINMUX_IPSR_MODSEL_DATA(IP1_12_11, I2C1_SCL_D, SEL_I2C01_3),
+	PINMUX_IPSR_DATA(IP1_14_13, D12),
+	PINMUX_IPSR_DATA(IP1_14_13, HSCIF2_HRTS_N),
+	PINMUX_IPSR_MODSEL_DATA(IP1_14_13, SCIF1_TXD_C, SEL_SCIF1_2),
+	PINMUX_IPSR_MODSEL_DATA(IP1_14_13, I2C1_SDA_D, SEL_I2C01_3),
+	PINMUX_IPSR_DATA(IP1_17_15, D13),
+	PINMUX_IPSR_MODSEL_DATA(IP1_17_15, SCIFA1_SCK, SEL_SCIFA1_0),
+	PINMUX_IPSR_DATA(IP1_17_15, TANS1),
+	PINMUX_IPSR_DATA(IP1_17_15, PWM2_C),
+	PINMUX_IPSR_MODSEL_DATA(IP1_17_15, TCLK2_B, SEL_TMU_1),
+	PINMUX_IPSR_DATA(IP1_19_18, D14),
+	PINMUX_IPSR_MODSEL_DATA(IP1_19_18, SCIFA1_RXD, SEL_SCIFA1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP1_19_18, IIC0_SCL_B, SEL_IIC00_1),
+	PINMUX_IPSR_DATA(IP1_21_20, D15),
+	PINMUX_IPSR_MODSEL_DATA(IP1_21_20, SCIFA1_TXD, SEL_SCIFA1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP1_21_20, IIC0_SDA_B, SEL_IIC00_1),
+	PINMUX_IPSR_DATA(IP1_23_22, A0),
+	PINMUX_IPSR_DATA(IP1_23_22, SCIFB1_SCK),
+	PINMUX_IPSR_DATA(IP1_23_22, PWM3_B),
+	PINMUX_IPSR_DATA(IP1_24, A1),
+	PINMUX_IPSR_DATA(IP1_24, SCIFB1_TXD),
+	PINMUX_IPSR_DATA(IP1_26, A3),
+	PINMUX_IPSR_DATA(IP1_26, SCIFB0_SCK),
+	PINMUX_IPSR_DATA(IP1_27, A4),
+	PINMUX_IPSR_DATA(IP1_27, SCIFB0_TXD),
+	PINMUX_IPSR_DATA(IP1_29_28, A5),
+	PINMUX_IPSR_DATA(IP1_29_28, SCIFB0_RXD),
+	PINMUX_IPSR_DATA(IP1_29_28, PWM4_B),
+	PINMUX_IPSR_DATA(IP1_29_28, TPUTO3_C),
+	PINMUX_IPSR_DATA(IP1_31_30, A6),
+	PINMUX_IPSR_DATA(IP1_31_30, SCIFB0_CTS_N),
+	PINMUX_IPSR_MODSEL_DATA(IP1_31_30, SCIFA4_RXD_B, SEL_SCIFA4_1),
+	PINMUX_IPSR_DATA(IP1_31_30, TPUTO2_C),
+
+	/* IPSR2 */
+	PINMUX_IPSR_DATA(IP2_1_0, A7),
+	PINMUX_IPSR_DATA(IP2_1_0, SCIFB0_RTS_N),
+	PINMUX_IPSR_MODSEL_DATA(IP2_1_0, SCIFA4_TXD_B, SEL_SCIFA4_1),
+	PINMUX_IPSR_DATA(IP2_3_2, A8),
+	PINMUX_IPSR_MODSEL_DATA(IP2_3_2, MSIOF1_RXD, SEL_MSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_3_2, SCIFA0_RXD_B, SEL_SCIFA0_1),
+	PINMUX_IPSR_DATA(IP2_5_4, A9),
+	PINMUX_IPSR_MODSEL_DATA(IP2_5_4, MSIOF1_TXD, SEL_MSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_5_4, SCIFA0_TXD_B, SEL_SCIFA0_1),
+	PINMUX_IPSR_DATA(IP2_7_6, A10),
+	PINMUX_IPSR_MODSEL_DATA(IP2_7_6, MSIOF1_SCK, SEL_MSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_7_6, IIC1_SCL_B, SEL_IIC01_1),
+	PINMUX_IPSR_DATA(IP2_9_8, A11),
+	PINMUX_IPSR_MODSEL_DATA(IP2_9_8, MSIOF1_SYNC, SEL_MSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_9_8, IIC1_SDA_B, SEL_IIC01_1),
+	PINMUX_IPSR_DATA(IP2_11_10, A12),
+	PINMUX_IPSR_MODSEL_DATA(IP2_11_10, MSIOF1_SS1, SEL_MSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_11_10, SCIFA5_RXD_B, SEL_SCIFA5_1),
+	PINMUX_IPSR_DATA(IP2_13_12, A13),
+	PINMUX_IPSR_MODSEL_DATA(IP2_13_12, MSIOF1_SS2, SEL_MSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_13_12, SCIFA5_TXD_B, SEL_SCIFA5_1),
+	PINMUX_IPSR_DATA(IP2_15_14, A14),
+	PINMUX_IPSR_MODSEL_DATA(IP2_15_14, MSIOF2_RXD, SEL_MSI2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_15_14, HSCIF0_HRX_B, SEL_HSCIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP2_15_14, DREQ1_N, SEL_LBS_0),
+	PINMUX_IPSR_DATA(IP2_17_16, A15),
+	PINMUX_IPSR_MODSEL_DATA(IP2_17_16, MSIOF2_TXD, SEL_MSI2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_17_16, HSCIF0_HTX_B, SEL_HSCIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP2_17_16, DACK1, SEL_LBS_0),
+	PINMUX_IPSR_DATA(IP2_20_18, A16),
+	PINMUX_IPSR_MODSEL_DATA(IP2_20_18, MSIOF2_SCK, SEL_MSI2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_20_18, HSCIF0_HSCK_B, SEL_HSCIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP2_20_18, SPEEDIN, SEL_RSP_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_20_18, VSP, SEL_SPDM_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_20_18, CAN_CLK_C, SEL_CAN_2),
+	PINMUX_IPSR_DATA(IP2_20_18, TPUTO2_B),
+	PINMUX_IPSR_DATA(IP2_23_21, A17),
+	PINMUX_IPSR_MODSEL_DATA(IP2_23_21, MSIOF2_SYNC, SEL_MSI2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_23_21, SCIF4_RXD_E, SEL_SCIF4_4),
+	PINMUX_IPSR_MODSEL_DATA(IP2_23_21, CAN1_RX_B, SEL_CAN1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP2_23_21, AVB_AVTP_CAPTURE_B, SEL_AVB_1),
+	PINMUX_IPSR_DATA(IP2_26_24, A18),
+	PINMUX_IPSR_MODSEL_DATA(IP2_26_24, MSIOF2_SS1, SEL_MSI2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP2_26_24, SCIF4_TXD_E, SEL_SCIF4_4),
+	PINMUX_IPSR_MODSEL_DATA(IP2_26_24, CAN1_TX_B, SEL_CAN1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP2_26_24, AVB_AVTP_MATCH_B, SEL_AVB_1),
+	PINMUX_IPSR_DATA(IP2_29_27, A19),
+	PINMUX_IPSR_MODSEL_DATA(IP2_29_27, MSIOF2_SS2, SEL_MSI2_0),
+	PINMUX_IPSR_DATA(IP2_29_27, PWM4),
+	PINMUX_IPSR_DATA(IP2_29_27, TPUTO2),
+	PINMUX_IPSR_DATA(IP2_29_27, MOUT0),
+	PINMUX_IPSR_DATA(IP2_31_30, A20),
+	PINMUX_IPSR_DATA(IP2_31_30, SPCLK),
+	PINMUX_IPSR_DATA(IP2_29_27, MOUT1),
+
+	/* IPSR3 */
+	PINMUX_IPSR_DATA(IP3_1_0, A21),
+	PINMUX_IPSR_DATA(IP3_1_0, MOSI_IO0),
+	PINMUX_IPSR_DATA(IP3_1_0, MOUT2),
+	PINMUX_IPSR_DATA(IP3_3_2, A22),
+	PINMUX_IPSR_DATA(IP3_3_2, MISO_IO1),
+	PINMUX_IPSR_DATA(IP3_3_2, MOUT5),
+	PINMUX_IPSR_DATA(IP3_3_2, ATADIR1_N),
+	PINMUX_IPSR_DATA(IP3_5_4, A23),
+	PINMUX_IPSR_DATA(IP3_5_4, IO2),
+	PINMUX_IPSR_DATA(IP3_5_4, MOUT6),
+	PINMUX_IPSR_DATA(IP3_5_4, ATAWR1_N),
+	PINMUX_IPSR_DATA(IP3_7_6, A24),
+	PINMUX_IPSR_DATA(IP3_7_6, IO3),
+	PINMUX_IPSR_DATA(IP3_7_6, EX_WAIT2),
+	PINMUX_IPSR_DATA(IP3_9_8, A25),
+	PINMUX_IPSR_DATA(IP3_9_8, SSL),
+	PINMUX_IPSR_DATA(IP3_9_8, ATARD1_N),
+	PINMUX_IPSR_DATA(IP3_10, CS0_N),
+	PINMUX_IPSR_DATA(IP3_10, VI1_DATA8),
+	PINMUX_IPSR_DATA(IP3_11, CS1_N_A26),
+	PINMUX_IPSR_DATA(IP3_11, VI1_DATA9),
+	PINMUX_IPSR_DATA(IP3_12, EX_CS0_N),
+	PINMUX_IPSR_DATA(IP3_12, VI1_DATA10),
+	PINMUX_IPSR_DATA(IP3_14_13, EX_CS1_N),
+	PINMUX_IPSR_DATA(IP3_14_13, TPUTO3_B),
+	PINMUX_IPSR_DATA(IP3_14_13, SCIFB2_RXD),
+	PINMUX_IPSR_DATA(IP3_14_13, VI1_DATA11),
+	PINMUX_IPSR_DATA(IP3_17_15, EX_CS2_N),
+	PINMUX_IPSR_DATA(IP3_17_15, PWM0),
+	PINMUX_IPSR_MODSEL_DATA(IP3_17_15, SCIF4_RXD_C, SEL_SCIF4_2),
+	PINMUX_IPSR_MODSEL_DATA(IP3_17_15, TS_SDATA_B, SEL_TSIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP3_17_15, RIF0_SYNC, SEL_DR0_0),
+	PINMUX_IPSR_DATA(IP3_17_15, TPUTO3),
+	PINMUX_IPSR_DATA(IP3_17_15, SCIFB2_TXD),
+	PINMUX_IPSR_MODSEL_DATA(IP3_17_15, SDATA_B, SEL_FSN_1),
+	PINMUX_IPSR_DATA(IP3_20_18, EX_CS3_N),
+	PINMUX_IPSR_MODSEL_DATA(IP3_20_18, SCIFA2_SCK, SEL_SCIFA2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP3_20_18, SCIF4_TXD_C, SEL_SCIF4_2),
+	PINMUX_IPSR_MODSEL_DATA(IP3_20_18, TS_SCK_B, SEL_TSIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP3_20_18, RIF0_CLK, SEL_DR0_0),
+	PINMUX_IPSR_MODSEL_DATA(IP3_20_18, BPFCLK, SEL_DARC_0),
+	PINMUX_IPSR_DATA(IP3_20_18, SCIFB2_SCK),
+	PINMUX_IPSR_MODSEL_DATA(IP3_20_18, MDATA_B, SEL_FSN_1),
+	PINMUX_IPSR_DATA(IP3_23_21, EX_CS4_N),
+	PINMUX_IPSR_MODSEL_DATA(IP3_23_21, SCIFA2_RXD, SEL_SCIFA2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP3_23_21, I2C2_SCL_E, SEL_I2C02_4),
+	PINMUX_IPSR_MODSEL_DATA(IP3_23_21, TS_SDEN_B, SEL_TSIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP3_23_21, RIF0_D0, SEL_DR0_0),
+	PINMUX_IPSR_MODSEL_DATA(IP3_23_21, FMCLK, SEL_DARC_0),
+	PINMUX_IPSR_DATA(IP3_23_21, SCIFB2_CTS_N),
+	PINMUX_IPSR_MODSEL_DATA(IP3_23_21, SCKZ_B, SEL_FSN_1),
+	PINMUX_IPSR_DATA(IP3_26_24, EX_CS5_N),
+	PINMUX_IPSR_MODSEL_DATA(IP3_26_24, SCIFA2_TXD, SEL_SCIFA2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP3_26_24, I2C2_SDA_E, SEL_I2C02_4),
+	PINMUX_IPSR_MODSEL_DATA(IP3_26_24, TS_SPSYNC_B, SEL_TSIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP3_26_24, RIF0_D1, SEL_DR1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP3_26_24, FMIN, SEL_DARC_0),
+	PINMUX_IPSR_DATA(IP3_26_24, SCIFB2_RTS_N),
+	PINMUX_IPSR_MODSEL_DATA(IP3_26_24, STM_N_B, SEL_FSN_1),
+	PINMUX_IPSR_DATA(IP3_29_27, BS_N),
+	PINMUX_IPSR_DATA(IP3_29_27, DRACK0),
+	PINMUX_IPSR_DATA(IP3_29_27, PWM1_C),
+	PINMUX_IPSR_DATA(IP3_29_27, TPUTO0_C),
+	PINMUX_IPSR_DATA(IP3_29_27, ATACS01_N),
+	PINMUX_IPSR_MODSEL_DATA(IP3_29_27, MTS_N_B, SEL_FSN_1),
+	PINMUX_IPSR_DATA(IP3_30, RD_N),
+	PINMUX_IPSR_DATA(IP3_30, ATACS11_N),
+	PINMUX_IPSR_DATA(IP3_31, RD_WR_N),
+	PINMUX_IPSR_DATA(IP3_31, ATAG1_N),
+
+	/* IPSR4 */
+	PINMUX_IPSR_DATA(IP4_1_0, EX_WAIT0),
+	PINMUX_IPSR_MODSEL_DATA(IP4_1_0, CAN_CLK_B, SEL_CAN_1),
+	PINMUX_IPSR_MODSEL_DATA(IP4_1_0, SCIF_CLK, SEL_SCIF0_0),
+	PINMUX_IPSR_DATA(IP4_1_0, PWMFSW0),
+	PINMUX_IPSR_DATA(IP4_4_2, DU0_DR0),
+	PINMUX_IPSR_DATA(IP4_4_2, LCDOUT16),
+	PINMUX_IPSR_MODSEL_DATA(IP4_4_2, SCIF5_RXD_C, SEL_SCIF5_2),
+	PINMUX_IPSR_MODSEL_DATA(IP4_4_2, I2C2_SCL_D, SEL_I2C02_3),
+	PINMUX_IPSR_DATA(IP4_4_2, CC50_STATE0),
+	PINMUX_IPSR_DATA(IP4_7_5, DU0_DR1),
+	PINMUX_IPSR_DATA(IP4_7_5, LCDOUT17),
+	PINMUX_IPSR_MODSEL_DATA(IP4_7_5, SCIF5_TXD_C, SEL_SCIF5_2),
+	PINMUX_IPSR_MODSEL_DATA(IP4_7_5, I2C2_SDA_D, SEL_I2C02_3),
+	PINMUX_IPSR_DATA(IP4_9_8, CC50_STATE1),
+	PINMUX_IPSR_DATA(IP4_9_8, DU0_DR2),
+	PINMUX_IPSR_DATA(IP4_9_8, LCDOUT18),
+	PINMUX_IPSR_DATA(IP4_9_8, CC50_STATE2),
+	PINMUX_IPSR_DATA(IP4_11_10, DU0_DR3),
+	PINMUX_IPSR_DATA(IP4_11_10, LCDOUT19),
+	PINMUX_IPSR_DATA(IP4_11_10, CC50_STATE3),
+	PINMUX_IPSR_DATA(IP4_13_12, DU0_DR4),
+	PINMUX_IPSR_DATA(IP4_13_12, LCDOUT20),
+	PINMUX_IPSR_DATA(IP4_13_12, CC50_STATE4),
+	PINMUX_IPSR_DATA(IP4_15_14, DU0_DR5),
+	PINMUX_IPSR_DATA(IP4_15_14, LCDOUT21),
+	PINMUX_IPSR_DATA(IP4_15_14, CC50_STATE5),
+	PINMUX_IPSR_DATA(IP4_17_16, DU0_DR6),
+	PINMUX_IPSR_DATA(IP4_17_16, LCDOUT22),
+	PINMUX_IPSR_DATA(IP4_17_16, CC50_STATE6),
+	PINMUX_IPSR_DATA(IP4_19_18, DU0_DR7),
+	PINMUX_IPSR_DATA(IP4_19_18, LCDOUT23),
+	PINMUX_IPSR_DATA(IP4_19_18, CC50_STATE7),
+	PINMUX_IPSR_DATA(IP4_22_20, DU0_DG0),
+	PINMUX_IPSR_DATA(IP4_22_20, LCDOUT8),
+	PINMUX_IPSR_MODSEL_DATA(IP4_22_20, SCIFA0_RXD_C, SEL_SCIFA0_2),
+	PINMUX_IPSR_MODSEL_DATA(IP4_22_20, I2C3_SCL_D, SEL_I2C03_3),
+	PINMUX_IPSR_DATA(IP4_22_20, CC50_STATE8),
+	PINMUX_IPSR_DATA(IP4_25_23, DU0_DG1),
+	PINMUX_IPSR_DATA(IP4_25_23, LCDOUT9),
+	PINMUX_IPSR_MODSEL_DATA(IP4_25_23, SCIFA0_TXD_C, SEL_SCIFA0_2),
+	PINMUX_IPSR_MODSEL_DATA(IP4_25_23, I2C3_SDA_D, SEL_I2C03_3),
+	PINMUX_IPSR_DATA(IP4_25_23, CC50_STATE9),
+	PINMUX_IPSR_DATA(IP4_27_26, DU0_DG2),
+	PINMUX_IPSR_DATA(IP4_27_26, LCDOUT10),
+	PINMUX_IPSR_DATA(IP4_27_26, CC50_STATE10),
+	PINMUX_IPSR_DATA(IP4_29_28, DU0_DG3),
+	PINMUX_IPSR_DATA(IP4_29_28, LCDOUT11),
+	PINMUX_IPSR_DATA(IP4_29_28, CC50_STATE11),
+	PINMUX_IPSR_DATA(IP4_31_30, DU0_DG4),
+	PINMUX_IPSR_DATA(IP4_31_30, LCDOUT12),
+	PINMUX_IPSR_DATA(IP4_31_30, CC50_STATE12),
+
+	/* IPSR5 */
+	PINMUX_IPSR_DATA(IP5_1_0, DU0_DG5),
+	PINMUX_IPSR_DATA(IP5_1_0, LCDOUT13),
+	PINMUX_IPSR_DATA(IP5_1_0, CC50_STATE13),
+	PINMUX_IPSR_DATA(IP5_3_2, DU0_DG6),
+	PINMUX_IPSR_DATA(IP5_3_2, LCDOUT14),
+	PINMUX_IPSR_DATA(IP5_3_2, CC50_STATE14),
+	PINMUX_IPSR_DATA(IP5_5_4, DU0_DG7),
+	PINMUX_IPSR_DATA(IP5_5_4, LCDOUT15),
+	PINMUX_IPSR_DATA(IP5_5_4, CC50_STATE15),
+	PINMUX_IPSR_DATA(IP5_8_6, DU0_DB0),
+	PINMUX_IPSR_DATA(IP5_8_6, LCDOUT0),
+	PINMUX_IPSR_MODSEL_DATA(IP5_8_6, SCIFA4_RXD_C, SEL_SCIFA4_2),
+	PINMUX_IPSR_MODSEL_DATA(IP5_8_6, I2C4_SCL_D, SEL_I2C04_3),
+	PINMUX_IPSR_MODSEL_DATA(IP7_8_6, CAN0_RX_C, SEL_CAN0_2),
+	PINMUX_IPSR_DATA(IP5_8_6, CC50_STATE16),
+	PINMUX_IPSR_DATA(IP5_11_9, DU0_DB1),
+	PINMUX_IPSR_DATA(IP5_11_9, LCDOUT1),
+	PINMUX_IPSR_MODSEL_DATA(IP5_11_9, SCIFA4_TXD_C, SEL_SCIFA4_2),
+	PINMUX_IPSR_MODSEL_DATA(IP5_11_9, I2C4_SDA_D, SEL_I2C04_3),
+	PINMUX_IPSR_MODSEL_DATA(IP5_11_9, CAN0_TX_C, SEL_CAN0_2),
+	PINMUX_IPSR_DATA(IP5_11_9, CC50_STATE17),
+	PINMUX_IPSR_DATA(IP5_13_12, DU0_DB2),
+	PINMUX_IPSR_DATA(IP5_13_12, LCDOUT2),
+	PINMUX_IPSR_DATA(IP5_13_12, CC50_STATE18),
+	PINMUX_IPSR_DATA(IP5_15_14, DU0_DB3),
+	PINMUX_IPSR_DATA(IP5_15_14, LCDOUT3),
+	PINMUX_IPSR_DATA(IP5_15_14, CC50_STATE19),
+	PINMUX_IPSR_DATA(IP5_17_16, DU0_DB4),
+	PINMUX_IPSR_DATA(IP5_17_16, LCDOUT4),
+	PINMUX_IPSR_DATA(IP5_17_16, CC50_STATE20),
+	PINMUX_IPSR_DATA(IP5_19_18, DU0_DB5),
+	PINMUX_IPSR_DATA(IP5_19_18, LCDOUT5),
+	PINMUX_IPSR_DATA(IP5_19_18, CC50_STATE21),
+	PINMUX_IPSR_DATA(IP5_21_20, DU0_DB6),
+	PINMUX_IPSR_DATA(IP5_21_20, LCDOUT6),
+	PINMUX_IPSR_DATA(IP5_21_20, CC50_STATE22),
+	PINMUX_IPSR_DATA(IP5_23_22, DU0_DB7),
+	PINMUX_IPSR_DATA(IP5_23_22, LCDOUT7),
+	PINMUX_IPSR_DATA(IP5_23_22, CC50_STATE23),
+	PINMUX_IPSR_DATA(IP5_25_24, DU0_DOTCLKIN),
+	PINMUX_IPSR_DATA(IP5_25_24, QSTVA_QVS),
+	PINMUX_IPSR_DATA(IP5_25_24, CC50_STATE24),
+	PINMUX_IPSR_DATA(IP5_27_26, DU0_DOTCLKOUT0),
+	PINMUX_IPSR_DATA(IP5_27_26, QCLK),
+	PINMUX_IPSR_DATA(IP5_27_26, CC50_STATE25),
+	PINMUX_IPSR_DATA(IP5_29_28, DU0_DOTCLKOUT1),
+	PINMUX_IPSR_DATA(IP5_29_28, QSTVB_QVE),
+	PINMUX_IPSR_DATA(IP5_29_28, CC50_STATE26),
+	PINMUX_IPSR_DATA(IP5_31_30, DU0_EXHSYNC_DU0_HSYNC),
+	PINMUX_IPSR_DATA(IP5_31_30, QSTH_QHS),
+	PINMUX_IPSR_DATA(IP5_31_30, CC50_STATE27),
+
+	/* IPSR6 */
+	PINMUX_IPSR_DATA(IP6_1_0, DU0_EXVSYNC_DU0_VSYNC),
+	PINMUX_IPSR_DATA(IP6_1_0, QSTB_QHE),
+	PINMUX_IPSR_DATA(IP6_1_0, CC50_STATE28),
+	PINMUX_IPSR_DATA(IP6_3_2, DU0_EXODDF_DU0_ODDF_DISP_CDE),
+	PINMUX_IPSR_DATA(IP6_3_2, QCPV_QDE),
+	PINMUX_IPSR_DATA(IP6_3_2, CC50_STATE29),
+	PINMUX_IPSR_DATA(IP6_5_4, DU0_DISP),
+	PINMUX_IPSR_DATA(IP6_5_4, QPOLA),
+	PINMUX_IPSR_DATA(IP6_5_4, CC50_STATE30),
+	PINMUX_IPSR_DATA(IP6_7_6, DU0_CDE),
+	PINMUX_IPSR_DATA(IP6_7_6, QPOLB),
+	PINMUX_IPSR_DATA(IP6_7_6, CC50_STATE31),
+	PINMUX_IPSR_DATA(IP6_8, VI0_CLK),
+	PINMUX_IPSR_DATA(IP6_8, AVB_RX_CLK),
+	PINMUX_IPSR_DATA(IP6_9, VI0_DATA0_VI0_B0),
+	PINMUX_IPSR_DATA(IP6_9, AVB_RX_DV),
+	PINMUX_IPSR_DATA(IP6_10, VI0_DATA1_VI0_B1),
+	PINMUX_IPSR_DATA(IP6_10, AVB_RXD0),
+	PINMUX_IPSR_DATA(IP6_11, VI0_DATA2_VI0_B2),
+	PINMUX_IPSR_DATA(IP6_11, AVB_RXD1),
+	PINMUX_IPSR_DATA(IP6_12, VI0_DATA3_VI0_B3),
+	PINMUX_IPSR_DATA(IP6_12, AVB_RXD2),
+	PINMUX_IPSR_DATA(IP6_13, VI0_DATA4_VI0_B4),
+	PINMUX_IPSR_DATA(IP6_13, AVB_RXD3),
+	PINMUX_IPSR_DATA(IP6_14, VI0_DATA5_VI0_B5),
+	PINMUX_IPSR_DATA(IP6_14, AVB_RXD4),
+	PINMUX_IPSR_DATA(IP6_15, VI0_DATA6_VI0_B6),
+	PINMUX_IPSR_DATA(IP6_15, AVB_RXD5),
+	PINMUX_IPSR_DATA(IP6_16, VI0_DATA7_VI0_B7),
+	PINMUX_IPSR_DATA(IP6_16, AVB_RXD6),
+	PINMUX_IPSR_DATA(IP6_19_17, VI0_CLKENB),
+	PINMUX_IPSR_MODSEL_DATA(IP6_19_17, I2C3_SCL, SEL_I2C03_0),
+	PINMUX_IPSR_MODSEL_DATA(IP6_19_17, SCIFA5_RXD_C, SEL_SCIFA5_2),
+	PINMUX_IPSR_MODSEL_DATA(IP6_19_17, IETX_C, SEL_IEB_2),
+	PINMUX_IPSR_DATA(IP6_19_17, AVB_RXD7),
+	PINMUX_IPSR_DATA(IP6_22_20, VI0_FIELD),
+	PINMUX_IPSR_MODSEL_DATA(IP6_22_20, I2C3_SDA, SEL_I2C03_0),
+	PINMUX_IPSR_MODSEL_DATA(IP6_22_20, SCIFA5_TXD_C, SEL_SCIFA5_2),
+	PINMUX_IPSR_MODSEL_DATA(IP6_22_20, IECLK_C, SEL_IEB_2),
+	PINMUX_IPSR_DATA(IP6_22_20, AVB_RX_ER),
+	PINMUX_IPSR_DATA(IP6_25_23, VI0_HSYNC_N),
+	PINMUX_IPSR_MODSEL_DATA(IP6_25_23, SCIF0_RXD_B, SEL_SCIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP6_25_23, I2C0_SCL_C, SEL_I2C00_2),
+	PINMUX_IPSR_MODSEL_DATA(IP6_25_23, IERX_C, SEL_IEB_2),
+	PINMUX_IPSR_DATA(IP6_25_23, AVB_COL),
+	PINMUX_IPSR_DATA(IP6_28_26, VI0_VSYNC_N),
+	PINMUX_IPSR_MODSEL_DATA(IP6_28_26, SCIF0_TXD_B, SEL_SCIF0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP6_28_26, I2C0_SDA_C, SEL_I2C00_2),
+	PINMUX_IPSR_MODSEL_DATA(IP6_28_26, AUDIO_CLKOUT_B, SEL_ADG_1),
+	PINMUX_IPSR_DATA(IP6_28_26, AVB_TX_EN),
+	PINMUX_IPSR_MODSEL_DATA(IP6_31_29, ETH_MDIO, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP6_31_29, VI0_G0),
+	PINMUX_IPSR_MODSEL_DATA(IP6_31_29, MSIOF2_RXD_B, SEL_MSI2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP6_31_29, IIC0_SCL_D, SEL_IIC00_3),
+	PINMUX_IPSR_DATA(IP6_31_29, AVB_TX_CLK),
+	PINMUX_IPSR_MODSEL_DATA(IP6_31_29, ADIDATA, SEL_RAD_0),
+	PINMUX_IPSR_MODSEL_DATA(IP6_31_29, AD_DI, SEL_ADI_0),
+
+	/* IPSR7 */
+	PINMUX_IPSR_MODSEL_DATA(IP7_2_0, ETH_CRS_DV, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_2_0, VI0_G1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_2_0, MSIOF2_TXD_B, SEL_MSI2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_2_0, IIC0_SDA_D, SEL_IIC00_3),
+	PINMUX_IPSR_DATA(IP7_2_0, AVB_TXD0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_2_0, ADICS_SAMP, SEL_RAD_0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_2_0, AD_DO, SEL_ADI_0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_5_3, ETH_RX_ER, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_5_3, VI0_G2),
+	PINMUX_IPSR_MODSEL_DATA(IP7_5_3, MSIOF2_SCK_B, SEL_MSI2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_5_3, CAN0_RX_B, SEL_CAN0_1),
+	PINMUX_IPSR_DATA(IP7_5_3, AVB_TXD1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_5_3, ADICLK, SEL_RAD_0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_5_3, AD_CLK, SEL_ADI_0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_8_6, ETH_RXD0, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_8_6, VI0_G3),
+	PINMUX_IPSR_MODSEL_DATA(IP7_8_6, MSIOF2_SYNC_B, SEL_MSI2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_8_6, CAN0_TX_B, SEL_CAN0_1),
+	PINMUX_IPSR_DATA(IP7_8_6, AVB_TXD2),
+	PINMUX_IPSR_MODSEL_DATA(IP7_8_6, ADICHS0, SEL_RAD_0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_8_6, AD_NCS_N, SEL_ADI_0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_11_9, ETH_RXD1, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_11_9, VI0_G4),
+	PINMUX_IPSR_MODSEL_DATA(IP7_11_9, MSIOF2_SS1_B, SEL_MSI2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_11_9, SCIF4_RXD_D, SEL_SCIF4_3),
+	PINMUX_IPSR_DATA(IP7_11_9, AVB_TXD3),
+	PINMUX_IPSR_MODSEL_DATA(IP7_11_9, ADICHS1, SEL_RAD_0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_14_12, ETH_LINK, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_14_12, VI0_G5),
+	PINMUX_IPSR_MODSEL_DATA(IP7_14_12, MSIOF2_SS2_B, SEL_MSI2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_14_12, SCIF4_TXD_D, SEL_SCIF4_3),
+	PINMUX_IPSR_DATA(IP7_14_12, AVB_TXD4),
+	PINMUX_IPSR_MODSEL_DATA(IP7_14_12, ADICHS2, SEL_RAD_0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_17_15, ETH_REFCLK, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_17_15, VI0_G6),
+	PINMUX_IPSR_MODSEL_DATA(IP7_17_15, SCIF2_SCK_C, SEL_SCIF2_2),
+	PINMUX_IPSR_DATA(IP7_17_15, AVB_TXD5),
+	PINMUX_IPSR_MODSEL_DATA(IP7_17_15, SSI_SCK5_B, SEL_SSI5_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_20_18, ETH_TXD1, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_20_18, VI0_G7),
+	PINMUX_IPSR_MODSEL_DATA(IP7_20_18, SCIF2_RXD_C, SEL_SCIF2_2),
+	PINMUX_IPSR_MODSEL_DATA(IP7_20_18, IIC1_SCL_D, SEL_IIC01_3),
+	PINMUX_IPSR_DATA(IP7_20_18, AVB_TXD6),
+	PINMUX_IPSR_MODSEL_DATA(IP7_20_18, SSI_WS5_B, SEL_SSI5_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_23_21, ETH_TX_EN, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_23_21, VI0_R0),
+	PINMUX_IPSR_MODSEL_DATA(IP7_23_21, SCIF2_TXD_C, SEL_SCIF2_2),
+	PINMUX_IPSR_MODSEL_DATA(IP7_23_21, IIC1_SDA_D, SEL_IIC01_3),
+	PINMUX_IPSR_DATA(IP7_23_21, AVB_TXD7),
+	PINMUX_IPSR_MODSEL_DATA(IP7_23_21, SSI_SDATA5_B, SEL_SSI5_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_26_24, ETH_MAGIC, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_26_24, VI0_R1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_26_24, SCIF3_SCK_B, SEL_SCIF3_1),
+	PINMUX_IPSR_DATA(IP7_26_24, AVB_TX_ER),
+	PINMUX_IPSR_MODSEL_DATA(IP7_26_24, SSI_SCK6_B, SEL_SSI6_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_29_27, ETH_TXD0, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP7_29_27, VI0_R2),
+	PINMUX_IPSR_MODSEL_DATA(IP7_29_27, SCIF3_RXD_B, SEL_SCIF3_1),
+	PINMUX_IPSR_MODSEL_DATA(IP7_29_27, I2C4_SCL_E, SEL_I2C04_4),
+	PINMUX_IPSR_DATA(IP7_29_27, AVB_GTX_CLK),
+	PINMUX_IPSR_MODSEL_DATA(IP7_29_27, SSI_WS6_B, SEL_SSI6_1),
+	PINMUX_IPSR_DATA(IP7_31, DREQ0_N),
+	PINMUX_IPSR_DATA(IP7_31, SCIFB1_RXD),
+
+	/* IPSR8 */
+	PINMUX_IPSR_MODSEL_DATA(IP8_2_0, ETH_MDC, SEL_ETH_0),
+	PINMUX_IPSR_DATA(IP8_2_0, VI0_R3),
+	PINMUX_IPSR_MODSEL_DATA(IP8_2_0, SCIF3_TXD_B, SEL_SCIF3_1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_2_0, I2C4_SDA_E, SEL_I2C04_4),
+	PINMUX_IPSR_DATA(IP8_2_0, AVB_MDC),
+	PINMUX_IPSR_MODSEL_DATA(IP8_2_0, SSI_SDATA6_B, SEL_SSI6_1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_5_3, HSCIF0_HRX, SEL_HSCIF0_0),
+	PINMUX_IPSR_DATA(IP8_5_3, VI0_R4),
+	PINMUX_IPSR_MODSEL_DATA(IP8_5_3, I2C1_SCL_C, SEL_I2C01_2),
+	PINMUX_IPSR_MODSEL_DATA(IP8_5_3, AUDIO_CLKA_B, SEL_ADG_1),
+	PINMUX_IPSR_DATA(IP8_5_3, AVB_MDIO),
+	PINMUX_IPSR_MODSEL_DATA(IP8_5_3, SSI_SCK78_B, SEL_SSI7_1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_8_6, HSCIF0_HTX, SEL_HSCIF0_0),
+	PINMUX_IPSR_DATA(IP8_8_6, VI0_R5),
+	PINMUX_IPSR_MODSEL_DATA(IP8_8_6, I2C1_SDA_C, SEL_I2C01_2),
+	PINMUX_IPSR_MODSEL_DATA(IP8_8_6, AUDIO_CLKB_B, SEL_ADG_1),
+	PINMUX_IPSR_DATA(IP8_5_3, AVB_LINK),
+	PINMUX_IPSR_MODSEL_DATA(IP8_8_6, SSI_WS78_B, SEL_SSI7_1),
+	PINMUX_IPSR_DATA(IP8_11_9, HSCIF0_HCTS_N),
+	PINMUX_IPSR_DATA(IP8_11_9, VI0_R6),
+	PINMUX_IPSR_MODSEL_DATA(IP8_11_9, SCIF0_RXD_D, SEL_SCIF0_3),
+	PINMUX_IPSR_MODSEL_DATA(IP8_11_9, I2C0_SCL_E, SEL_I2C00_4),
+	PINMUX_IPSR_DATA(IP8_11_9, AVB_MAGIC),
+	PINMUX_IPSR_MODSEL_DATA(IP8_11_9, SSI_SDATA7_B, SEL_SSI7_1),
+	PINMUX_IPSR_DATA(IP8_14_12, HSCIF0_HRTS_N),
+	PINMUX_IPSR_DATA(IP8_14_12, VI0_R7),
+	PINMUX_IPSR_MODSEL_DATA(IP8_14_12, SCIF0_TXD_D, SEL_SCIF0_3),
+	PINMUX_IPSR_MODSEL_DATA(IP8_14_12, I2C0_SDA_E, SEL_I2C00_4),
+	PINMUX_IPSR_DATA(IP8_14_12, AVB_PHY_INT),
+	PINMUX_IPSR_MODSEL_DATA(IP8_14_12, SSI_SDATA8_B, SEL_SSI8_1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_16_15, HSCIF0_HSCK, SEL_HSCIF0_0),
+	PINMUX_IPSR_MODSEL_DATA(IP8_16_15, SCIF_CLK_B, SEL_SCIF0_1),
+	PINMUX_IPSR_DATA(IP8_16_15, AVB_CRS),
+	PINMUX_IPSR_MODSEL_DATA(IP8_16_15, AUDIO_CLKC_B, SEL_ADG_1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_19_17, I2C0_SCL, SEL_I2C00_0),
+	PINMUX_IPSR_MODSEL_DATA(IP8_19_17, SCIF0_RXD_C, SEL_SCIF0_2),
+	PINMUX_IPSR_DATA(IP8_19_17, PWM5),
+	PINMUX_IPSR_MODSEL_DATA(IP8_19_17, TCLK1_B, SEL_TMU_1),
+	PINMUX_IPSR_DATA(IP8_19_17, AVB_GTXREFCLK),
+	PINMUX_IPSR_MODSEL_DATA(IP8_19_17, CAN1_RX_D, SEL_CAN1_3),
+	PINMUX_IPSR_DATA(IP8_19_17, TPUTO0_B),
+	PINMUX_IPSR_MODSEL_DATA(IP8_22_20, I2C0_SDA, SEL_I2C00_0),
+	PINMUX_IPSR_MODSEL_DATA(IP8_22_20, SCIF0_TXD_C, SEL_SCIF0_2),
+	PINMUX_IPSR_DATA(IP8_22_20, TPUTO0),
+	PINMUX_IPSR_MODSEL_DATA(IP8_22_20, CAN_CLK, SEL_CAN_0),
+	PINMUX_IPSR_DATA(IP8_22_20, DVC_MUTE),
+	PINMUX_IPSR_MODSEL_DATA(IP8_22_20, CAN1_TX_D, SEL_CAN1_3),
+	PINMUX_IPSR_MODSEL_DATA(IP8_25_23, I2C1_SCL, SEL_I2C01_0),
+	PINMUX_IPSR_MODSEL_DATA(IP8_25_23, SCIF4_RXD, SEL_SCIF4_0),
+	PINMUX_IPSR_DATA(IP8_25_23, PWM5_B),
+	PINMUX_IPSR_DATA(IP8_25_23, DU1_DR0),
+	PINMUX_IPSR_MODSEL_DATA(IP8_25_23, RIF1_SYNC_B, SEL_DR2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_25_23, TS_SDATA_D, SEL_TSIF0_3),
+	PINMUX_IPSR_DATA(IP8_25_23, TPUTO1_B),
+	PINMUX_IPSR_MODSEL_DATA(IP8_28_26, I2C1_SDA, SEL_I2C01_0),
+	PINMUX_IPSR_MODSEL_DATA(IP8_28_26, SCIF4_TXD, SEL_SCIF4_0),
+	PINMUX_IPSR_DATA(IP8_28_26, IRQ5),
+	PINMUX_IPSR_DATA(IP8_28_26, DU1_DR1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_28_26, RIF1_CLK_B, SEL_DR2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_28_26, TS_SCK_D, SEL_TSIF0_3),
+	PINMUX_IPSR_MODSEL_DATA(IP8_28_26, BPFCLK_C, SEL_DARC_2),
+	PINMUX_IPSR_DATA(IP8_31_29, MSIOF0_RXD),
+	PINMUX_IPSR_MODSEL_DATA(IP8_31_29, SCIF5_RXD, SEL_SCIF5_0),
+	PINMUX_IPSR_MODSEL_DATA(IP8_31_29, I2C2_SCL_C, SEL_I2C02_2),
+	PINMUX_IPSR_DATA(IP8_31_29, DU1_DR2),
+	PINMUX_IPSR_MODSEL_DATA(IP8_31_29, RIF1_D0_B, SEL_DR2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP8_31_29, TS_SDEN_D, SEL_TSIF0_3),
+	PINMUX_IPSR_MODSEL_DATA(IP8_31_29, FMCLK_C, SEL_DARC_2),
+	PINMUX_IPSR_MODSEL_DATA(IP8_31_29, RDS_CLK, SEL_RDS_0),
+
+	/* IPSR9 */
+	PINMUX_IPSR_DATA(IP9_2_0, MSIOF0_TXD),
+	PINMUX_IPSR_MODSEL_DATA(IP9_2_0, SCIF5_TXD, SEL_SCIF5_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_2_0, I2C2_SDA_C, SEL_I2C02_2),
+	PINMUX_IPSR_DATA(IP9_2_0, DU1_DR3),
+	PINMUX_IPSR_MODSEL_DATA(IP9_2_0, RIF1_D1_B, SEL_DR3_1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_2_0, TS_SPSYNC_D, SEL_TSIF0_3),
+	PINMUX_IPSR_MODSEL_DATA(IP9_2_0, FMIN_C, SEL_DARC_2),
+	PINMUX_IPSR_MODSEL_DATA(IP9_2_0, RDS_DATA, SEL_RDS_0),
+	PINMUX_IPSR_DATA(IP9_5_3, MSIOF0_SCK),
+	PINMUX_IPSR_DATA(IP9_5_3, IRQ0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_5_3, TS_SDATA, SEL_TSIF0_0),
+	PINMUX_IPSR_DATA(IP9_5_3, DU1_DR4),
+	PINMUX_IPSR_MODSEL_DATA(IP9_5_3, RIF1_SYNC, SEL_DR2_0),
+	PINMUX_IPSR_DATA(IP9_5_3, TPUTO1_C),
+	PINMUX_IPSR_DATA(IP9_8_6, MSIOF0_SYNC),
+	PINMUX_IPSR_DATA(IP9_8_6, PWM1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_8_6, TS_SCK, SEL_TSIF0_0),
+	PINMUX_IPSR_DATA(IP9_8_6, DU1_DR5),
+	PINMUX_IPSR_MODSEL_DATA(IP9_8_6, RIF1_CLK, SEL_DR2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_8_6, BPFCLK_B, SEL_DARC_1),
+	PINMUX_IPSR_DATA(IP9_11_9, MSIOF0_SS1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_11_9, SCIFA0_RXD, SEL_SCIFA0_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_11_9, TS_SDEN, SEL_TSIF0_0),
+	PINMUX_IPSR_DATA(IP9_11_9, DU1_DR6),
+	PINMUX_IPSR_MODSEL_DATA(IP9_11_9, RIF1_D0, SEL_DR2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_11_9, FMCLK_B, SEL_DARC_1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_11_9, RDS_CLK_B, SEL_RDS_1),
+	PINMUX_IPSR_DATA(IP9_14_12, MSIOF0_SS2),
+	PINMUX_IPSR_MODSEL_DATA(IP9_14_12, SCIFA0_TXD, SEL_SCIFA0_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_14_12, TS_SPSYNC, SEL_TSIF0_0),
+	PINMUX_IPSR_DATA(IP9_14_12, DU1_DR7),
+	PINMUX_IPSR_MODSEL_DATA(IP9_14_12, RIF1_D1, SEL_DR3_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_14_12, FMIN_B, SEL_DARC_1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_14_12, RDS_DATA_B, SEL_RDS_1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_16_15, HSCIF1_HRX, SEL_HSCIF1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_16_15, I2C4_SCL, SEL_I2C04_0),
+	PINMUX_IPSR_DATA(IP9_16_15, PWM6),
+	PINMUX_IPSR_DATA(IP9_16_15, DU1_DG0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_18_17, HSCIF1_HTX, SEL_HSCIF1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_18_17, I2C4_SDA, SEL_I2C04_0),
+	PINMUX_IPSR_DATA(IP9_18_17, TPUTO1),
+	PINMUX_IPSR_DATA(IP9_18_17, DU1_DG1),
+	PINMUX_IPSR_DATA(IP9_21_19, HSCIF1_HSCK),
+	PINMUX_IPSR_DATA(IP9_21_19, PWM2),
+	PINMUX_IPSR_MODSEL_DATA(IP9_21_19, IETX, SEL_IEB_0),
+	PINMUX_IPSR_DATA(IP9_21_19, DU1_DG2),
+	PINMUX_IPSR_MODSEL_DATA(IP9_21_19, REMOCON_B, SEL_RCN_1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_21_19, SPEEDIN_B, SEL_RSP_1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_21_19, VSP_B, SEL_SPDM_1),
+	PINMUX_IPSR_MODSEL_DATA(IP9_24_22, HSCIF1_HCTS_N, SEL_HSCIF1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_24_22, SCIFA4_RXD, SEL_SCIFA4_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_24_22, IECLK, SEL_IEB_0),
+	PINMUX_IPSR_DATA(IP9_24_22, DU1_DG3),
+	PINMUX_IPSR_MODSEL_DATA(IP9_24_22, SSI_SCK1_B, SEL_SSI1_1),
+	PINMUX_IPSR_DATA(IP9_24_22, CAN_DEBUG_HW_TRIGGER),
+	PINMUX_IPSR_DATA(IP9_24_22, CC50_STATE32),
+	PINMUX_IPSR_MODSEL_DATA(IP9_27_25, HSCIF1_HRTS_N, SEL_HSCIF1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_27_25, SCIFA4_TXD, SEL_SCIFA4_0),
+	PINMUX_IPSR_MODSEL_DATA(IP9_27_25, IERX, SEL_IEB_0),
+	PINMUX_IPSR_DATA(IP9_27_25, DU1_DG4),
+	PINMUX_IPSR_MODSEL_DATA(IP9_27_25, SSI_WS1_B, SEL_SSI1_1),
+	PINMUX_IPSR_DATA(IP9_27_25, CAN_STEP0),
+	PINMUX_IPSR_DATA(IP9_27_25, CC50_STATE33),
+	PINMUX_IPSR_MODSEL_DATA(IP9_30_28, SCIF1_SCK, SEL_SCIF1_0),
+	PINMUX_IPSR_DATA(IP9_30_28, PWM3),
+	PINMUX_IPSR_MODSEL_DATA(IP9_30_28, TCLK2, SEL_TMU_0),
+	PINMUX_IPSR_DATA(IP9_30_28, DU1_DG5),
+	PINMUX_IPSR_MODSEL_DATA(IP9_30_28, SSI_SDATA1_B, SEL_SSI1_1),
+	PINMUX_IPSR_DATA(IP9_30_28, CAN_TXCLK),
+	PINMUX_IPSR_DATA(IP9_30_28, CC50_STATE34),
+
+	/* IPSR10 */
+	PINMUX_IPSR_MODSEL_DATA(IP10_2_0, SCIF1_RXD, SEL_SCIF1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_2_0, IIC0_SCL, SEL_IIC00_0),
+	PINMUX_IPSR_DATA(IP10_2_0, DU1_DG6),
+	PINMUX_IPSR_MODSEL_DATA(IP10_2_0, SSI_SCK2_B, SEL_SSI2_1),
+	PINMUX_IPSR_DATA(IP10_2_0, CAN_DEBUGOUT0),
+	PINMUX_IPSR_DATA(IP10_2_0, CC50_STATE35),
+	PINMUX_IPSR_MODSEL_DATA(IP10_5_3, SCIF1_TXD, SEL_SCIF1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_5_3, IIC0_SDA, SEL_IIC00_0),
+	PINMUX_IPSR_DATA(IP10_5_3, DU1_DG7),
+	PINMUX_IPSR_MODSEL_DATA(IP10_5_3, SSI_WS2_B, SEL_SSI2_1),
+	PINMUX_IPSR_DATA(IP10_5_3, CAN_DEBUGOUT1),
+	PINMUX_IPSR_DATA(IP10_5_3, CC50_STATE36),
+	PINMUX_IPSR_MODSEL_DATA(IP10_8_6, SCIF2_RXD, SEL_SCIF2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_8_6, IIC1_SCL, SEL_IIC01_0),
+	PINMUX_IPSR_DATA(IP10_8_6, DU1_DB0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_8_6, SSI_SDATA2_B, SEL_SSI2_1),
+	PINMUX_IPSR_DATA(IP10_8_6, USB0_EXTLP),
+	PINMUX_IPSR_DATA(IP10_8_6, CAN_DEBUGOUT2),
+	PINMUX_IPSR_DATA(IP10_8_6, CC50_STATE37),
+	PINMUX_IPSR_MODSEL_DATA(IP10_11_9, SCIF2_TXD, SEL_SCIF2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_11_9, IIC1_SDA, SEL_IIC01_0),
+	PINMUX_IPSR_DATA(IP10_11_9, DU1_DB1),
+	PINMUX_IPSR_MODSEL_DATA(IP10_11_9, SSI_SCK9_B, SEL_SSI9_1),
+	PINMUX_IPSR_DATA(IP10_11_9, USB0_OVC1),
+	PINMUX_IPSR_DATA(IP10_11_9, CAN_DEBUGOUT3),
+	PINMUX_IPSR_DATA(IP10_11_9, CC50_STATE38),
+	PINMUX_IPSR_MODSEL_DATA(IP10_14_12, SCIF2_SCK, SEL_SCIF2_0),
+	PINMUX_IPSR_DATA(IP10_14_12, IRQ1),
+	PINMUX_IPSR_DATA(IP10_14_12, DU1_DB2),
+	PINMUX_IPSR_MODSEL_DATA(IP10_14_12, SSI_WS9_B, SEL_SSI9_1),
+	PINMUX_IPSR_DATA(IP10_14_12, USB0_IDIN),
+	PINMUX_IPSR_DATA(IP10_14_12, CAN_DEBUGOUT4),
+	PINMUX_IPSR_DATA(IP10_14_12, CC50_STATE39),
+	PINMUX_IPSR_MODSEL_DATA(IP10_17_15, SCIF3_SCK, SEL_SCIF3_0),
+	PINMUX_IPSR_DATA(IP10_17_15, IRQ2),
+	PINMUX_IPSR_MODSEL_DATA(IP10_17_15, BPFCLK_D, SEL_DARC_3),
+	PINMUX_IPSR_DATA(IP10_17_15, DU1_DB3),
+	PINMUX_IPSR_MODSEL_DATA(IP10_17_15, SSI_SDATA9_B, SEL_SSI9_1),
+	PINMUX_IPSR_DATA(IP10_17_15, TANS2),
+	PINMUX_IPSR_DATA(IP10_17_15, CAN_DEBUGOUT5),
+	PINMUX_IPSR_DATA(IP10_17_15, CC50_OSCOUT),
+	PINMUX_IPSR_MODSEL_DATA(IP10_20_18, SCIF3_RXD, SEL_SCIF3_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_20_18, I2C1_SCL_E, SEL_I2C01_4),
+	PINMUX_IPSR_MODSEL_DATA(IP10_20_18, FMCLK_D, SEL_DARC_3),
+	PINMUX_IPSR_DATA(IP10_20_18, DU1_DB4),
+	PINMUX_IPSR_MODSEL_DATA(IP10_20_18, AUDIO_CLKA_C, SEL_ADG_2),
+	PINMUX_IPSR_MODSEL_DATA(IP10_20_18, SSI_SCK4_B, SEL_SSI4_1),
+	PINMUX_IPSR_DATA(IP10_20_18, CAN_DEBUGOUT6),
+	PINMUX_IPSR_MODSEL_DATA(IP10_20_18, RDS_CLK_C, SEL_RDS_2),
+	PINMUX_IPSR_MODSEL_DATA(IP10_23_21, SCIF3_TXD, SEL_SCIF3_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_23_21, I2C1_SDA_E, SEL_I2C01_4),
+	PINMUX_IPSR_MODSEL_DATA(IP10_23_21, FMIN_D, SEL_DARC_3),
+	PINMUX_IPSR_DATA(IP10_23_21, DU1_DB5),
+	PINMUX_IPSR_MODSEL_DATA(IP10_23_21, AUDIO_CLKB_C, SEL_ADG_2),
+	PINMUX_IPSR_MODSEL_DATA(IP10_23_21, SSI_WS4_B, SEL_SSI4_1),
+	PINMUX_IPSR_DATA(IP10_23_21, CAN_DEBUGOUT7),
+	PINMUX_IPSR_MODSEL_DATA(IP10_23_21, RDS_DATA_C, SEL_RDS_2),
+	PINMUX_IPSR_MODSEL_DATA(IP10_26_24, I2C2_SCL, SEL_I2C02_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_26_24, SCIFA5_RXD, SEL_SCIFA5_0),
+	PINMUX_IPSR_DATA(IP10_26_24, DU1_DB6),
+	PINMUX_IPSR_MODSEL_DATA(IP10_26_24, AUDIO_CLKC_C, SEL_ADG_2),
+	PINMUX_IPSR_MODSEL_DATA(IP10_26_24, SSI_SDATA4_B, SEL_SSI4_1),
+	PINMUX_IPSR_DATA(IP10_26_24, CAN_DEBUGOUT8),
+	PINMUX_IPSR_MODSEL_DATA(IP10_29_27, I2C2_SDA, SEL_I2C02_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_29_27, SCIFA5_TXD, SEL_SCIFA5_0),
+	PINMUX_IPSR_DATA(IP10_29_27, DU1_DB7),
+	PINMUX_IPSR_MODSEL_DATA(IP10_29_27, AUDIO_CLKOUT_C, SEL_ADG_2),
+	PINMUX_IPSR_DATA(IP10_29_27, CAN_DEBUGOUT9),
+	PINMUX_IPSR_MODSEL_DATA(IP10_31_30, SSI_SCK5, SEL_SSI5_0),
+	PINMUX_IPSR_MODSEL_DATA(IP10_31_30, SCIFA3_SCK, SEL_SCIFA3_0),
+	PINMUX_IPSR_DATA(IP10_31_30, DU1_DOTCLKIN),
+	PINMUX_IPSR_DATA(IP10_31_30, CAN_DEBUGOUT10),
+
+	/* IPSR11 */
+	PINMUX_IPSR_MODSEL_DATA(IP11_2_0, SSI_WS5, SEL_SSI5_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_2_0, SCIFA3_RXD, SEL_SCIFA3_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_2_0, I2C3_SCL_C, SEL_I2C03_2),
+	PINMUX_IPSR_DATA(IP11_2_0, DU1_DOTCLKOUT0),
+	PINMUX_IPSR_DATA(IP11_2_0, CAN_DEBUGOUT11),
+	PINMUX_IPSR_MODSEL_DATA(IP11_5_3, SSI_SDATA5, SEL_SSI5_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_5_3, SCIFA3_TXD, SEL_SCIFA3_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_5_3, I2C3_SDA_C, SEL_I2C03_2),
+	PINMUX_IPSR_DATA(IP11_5_3, DU1_DOTCLKOUT1),
+	PINMUX_IPSR_DATA(IP11_5_3, CAN_DEBUGOUT12),
+	PINMUX_IPSR_MODSEL_DATA(IP11_7_6, SSI_SCK6, SEL_SSI6_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_7_6, SCIFA1_SCK_B, SEL_SCIFA1_1),
+	PINMUX_IPSR_DATA(IP11_7_6, DU1_EXHSYNC_DU1_HSYNC),
+	PINMUX_IPSR_DATA(IP11_7_6, CAN_DEBUGOUT13),
+	PINMUX_IPSR_MODSEL_DATA(IP11_10_8, SSI_WS6, SEL_SSI6_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_10_8, SCIFA1_RXD_B, SEL_SCIFA1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_10_8, I2C4_SCL_C, SEL_I2C04_2),
+	PINMUX_IPSR_DATA(IP11_10_8, DU1_EXVSYNC_DU1_VSYNC),
+	PINMUX_IPSR_DATA(IP11_10_8, CAN_DEBUGOUT14),
+	PINMUX_IPSR_MODSEL_DATA(IP11_13_11, SSI_SDATA6, SEL_SSI6_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_13_11, SCIFA1_TXD_B, SEL_SCIFA1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_13_11, I2C4_SDA_C, SEL_I2C04_2),
+	PINMUX_IPSR_DATA(IP11_13_11, DU1_EXODDF_DU1_ODDF_DISP_CDE),
+	PINMUX_IPSR_DATA(IP11_13_11, CAN_DEBUGOUT15),
+	PINMUX_IPSR_MODSEL_DATA(IP11_15_14, SSI_SCK78, SEL_SSI7_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_15_14, SCIFA2_SCK_B, SEL_SCIFA2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_15_14, IIC0_SDA_C, SEL_IIC00_2),
+	PINMUX_IPSR_DATA(IP11_15_14, DU1_DISP),
+	PINMUX_IPSR_MODSEL_DATA(IP11_17_16, SSI_WS78, SEL_SSI7_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_17_16, SCIFA2_RXD_B, SEL_SCIFA2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_17_16, IIC0_SCL_C, SEL_IIC00_2),
+	PINMUX_IPSR_DATA(IP11_17_16, DU1_CDE),
+	PINMUX_IPSR_MODSEL_DATA(IP11_20_18, SSI_SDATA7, SEL_SSI7_0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_20_18, SCIFA2_TXD_B, SEL_SCIFA2_1),
+	PINMUX_IPSR_DATA(IP11_20_18, IRQ8),
+	PINMUX_IPSR_MODSEL_DATA(IP11_20_18, AUDIO_CLKA_D, SEL_ADG_3),
+	PINMUX_IPSR_MODSEL_DATA(IP11_20_18, CAN_CLK_D, SEL_CAN_3),
+	PINMUX_IPSR_DATA(IP11_20_18, PCMOE_N),
+	PINMUX_IPSR_DATA(IP11_23_21, SSI_SCK0129),
+	PINMUX_IPSR_MODSEL_DATA(IP11_23_21, MSIOF1_RXD_B, SEL_MSI1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_23_21, SCIF5_RXD_D, SEL_SCIF5_3),
+	PINMUX_IPSR_MODSEL_DATA(IP11_23_21, ADIDATA_B, SEL_RAD_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_23_21, AD_DI_B, SEL_ADI_1),
+	PINMUX_IPSR_DATA(IP11_23_21, PCMWE_N),
+	PINMUX_IPSR_DATA(IP11_26_24, SSI_WS0129),
+	PINMUX_IPSR_MODSEL_DATA(IP11_26_24, MSIOF1_TXD_B, SEL_MSI1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_26_24, SCIF5_TXD_D, SEL_SCIF5_3),
+	PINMUX_IPSR_MODSEL_DATA(IP11_26_24, ADICS_SAMP_B, SEL_RAD_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_26_24, AD_DO_B, SEL_ADI_1),
+	PINMUX_IPSR_DATA(IP11_29_27, SSI_SDATA0),
+	PINMUX_IPSR_MODSEL_DATA(IP11_29_27, MSIOF1_SCK_B, SEL_MSI1_1),
+	PINMUX_IPSR_DATA(IP11_29_27, PWM0_B),
+	PINMUX_IPSR_MODSEL_DATA(IP11_29_27, ADICLK_B, SEL_RAD_1),
+	PINMUX_IPSR_MODSEL_DATA(IP11_29_27, AD_CLK_B, SEL_ADI_1),
+
+	/* IPSR12 */
+	PINMUX_IPSR_DATA(IP12_2_0, SSI_SCK34),
+	PINMUX_IPSR_MODSEL_DATA(IP12_2_0, MSIOF1_SYNC_B, SEL_MSI1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_2_0, SCIFA1_SCK_C, SEL_SCIFA1_2),
+	PINMUX_IPSR_MODSEL_DATA(IP12_2_0, ADICHS0_B, SEL_RAD_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_2_0, AD_NCS_N_B, SEL_ADI_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_2_0, DREQ1_N_B, SEL_LBS_1),
+	PINMUX_IPSR_DATA(IP12_5_3, SSI_WS34),
+	PINMUX_IPSR_MODSEL_DATA(IP12_5_3, MSIOF1_SS1_B, SEL_MSI1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_5_3, SCIFA1_RXD_C, SEL_SCIFA1_2),
+	PINMUX_IPSR_MODSEL_DATA(IP12_5_3, ADICHS1_B, SEL_RAD_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_5_3, CAN1_RX_C, SEL_CAN1_2),
+	PINMUX_IPSR_MODSEL_DATA(IP12_5_3, DACK1_B, SEL_LBS_1),
+	PINMUX_IPSR_DATA(IP12_8_6, SSI_SDATA3),
+	PINMUX_IPSR_MODSEL_DATA(IP12_8_6, MSIOF1_SS2_B, SEL_MSI1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_8_6, SCIFA1_TXD_C, SEL_SCIFA1_2),
+	PINMUX_IPSR_MODSEL_DATA(IP12_8_6, ADICHS2_B, SEL_RAD_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_8_6, CAN1_TX_C, SEL_CAN1_2),
+	PINMUX_IPSR_DATA(IP12_8_6, DREQ2_N),
+	PINMUX_IPSR_MODSEL_DATA(IP12_10_9, SSI_SCK4, SEL_SSI4_0),
+	PINMUX_IPSR_DATA(IP12_10_9, MLB_CLK),
+	PINMUX_IPSR_MODSEL_DATA(IP12_10_9, IETX_B, SEL_IEB_1),
+	PINMUX_IPSR_DATA(IP12_10_9, IRD_TX),
+	PINMUX_IPSR_MODSEL_DATA(IP12_12_11, SSI_WS4, SEL_SSI4_0),
+	PINMUX_IPSR_DATA(IP12_12_11, MLB_SIG),
+	PINMUX_IPSR_MODSEL_DATA(IP12_12_11, IECLK_B, SEL_IEB_1),
+	PINMUX_IPSR_DATA(IP12_12_11, IRD_RX),
+	PINMUX_IPSR_MODSEL_DATA(IP12_14_13, SSI_SDATA4, SEL_SSI4_0),
+	PINMUX_IPSR_DATA(IP12_14_13, MLB_DAT),
+	PINMUX_IPSR_MODSEL_DATA(IP12_14_13, IERX_B, SEL_IEB_1),
+	PINMUX_IPSR_DATA(IP12_14_13, IRD_SCK),
+	PINMUX_IPSR_MODSEL_DATA(IP12_17_15, SSI_SDATA8, SEL_SSI8_0),
+	PINMUX_IPSR_MODSEL_DATA(IP12_17_15, SCIF1_SCK_B, SEL_SCIF1_1),
+	PINMUX_IPSR_DATA(IP12_17_15, PWM1_B),
+	PINMUX_IPSR_DATA(IP12_17_15, IRQ9),
+	PINMUX_IPSR_MODSEL_DATA(IP12_17_15, REMOCON, SEL_RCN_0),
+	PINMUX_IPSR_DATA(IP12_17_15, DACK2),
+	PINMUX_IPSR_MODSEL_DATA(IP12_17_15, ETH_MDIO_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_20_18, SSI_SCK1, SEL_SSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP12_20_18, SCIF1_RXD_B, SEL_SCIF1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_20_18, IIC1_SCL_C, SEL_IIC01_2),
+	PINMUX_IPSR_DATA(IP12_20_18, VI1_CLK),
+	PINMUX_IPSR_MODSEL_DATA(IP12_20_18, CAN0_RX_D, SEL_CAN0_3),
+	PINMUX_IPSR_MODSEL_DATA(IP12_20_18, AVB_AVTP_CAPTURE, SEL_AVB_0),
+	PINMUX_IPSR_MODSEL_DATA(IP12_20_18, ETH_CRS_DV_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_23_21, SSI_WS1, SEL_SSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP12_23_21, SCIF1_TXD_B, SEL_SCIF1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_23_21, IIC1_SDA_C, SEL_IIC01_2),
+	PINMUX_IPSR_DATA(IP12_23_21, VI1_DATA0),
+	PINMUX_IPSR_MODSEL_DATA(IP12_23_21, CAN0_TX_D, SEL_CAN0_3),
+	PINMUX_IPSR_MODSEL_DATA(IP12_23_21, AVB_AVTP_MATCH, SEL_AVB_0),
+	PINMUX_IPSR_MODSEL_DATA(IP12_23_21, ETH_RX_ER_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_26_24, SSI_SDATA1, SEL_SSI1_0),
+	PINMUX_IPSR_MODSEL_DATA(IP12_26_24, HSCIF1_HRX_B, SEL_HSCIF1_1),
+	PINMUX_IPSR_DATA(IP12_26_24, VI1_DATA1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_26_24, SDATA, SEL_FSN_0),
+	PINMUX_IPSR_DATA(IP12_26_24, ATAG0_N),
+	PINMUX_IPSR_MODSEL_DATA(IP12_26_24, ETH_RXD0_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP12_29_27, SSI_SCK2, SEL_SSI2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP12_29_27, HSCIF1_HTX_B, SEL_HSCIF1_1),
+	PINMUX_IPSR_DATA(IP12_29_27, VI1_DATA2),
+	PINMUX_IPSR_MODSEL_DATA(IP12_29_27, MDATA, SEL_FSN_0),
+	PINMUX_IPSR_DATA(IP12_29_27, ATAWR0_N),
+	PINMUX_IPSR_MODSEL_DATA(IP12_29_27, ETH_RXD1_B, SEL_ETH_1),
+
+	/* IPSR13 */
+	PINMUX_IPSR_MODSEL_DATA(IP13_2_0, SSI_WS2, SEL_SSI2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_2_0, HSCIF1_HCTS_N_B, SEL_HSCIF1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_2_0, SCIFA0_RXD_D, SEL_SCIFA0_3),
+	PINMUX_IPSR_DATA(IP13_2_0, VI1_DATA3),
+	PINMUX_IPSR_MODSEL_DATA(IP13_2_0, SCKZ, SEL_FSN_0),
+	PINMUX_IPSR_DATA(IP13_2_0, ATACS00_N),
+	PINMUX_IPSR_MODSEL_DATA(IP13_2_0, ETH_LINK_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_5_3, SSI_SDATA2, SEL_SSI2_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_5_3, HSCIF1_HRTS_N_B, SEL_HSCIF1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_5_3, SCIFA0_TXD_D, SEL_SCIFA0_3),
+	PINMUX_IPSR_DATA(IP13_5_3, VI1_DATA4),
+	PINMUX_IPSR_MODSEL_DATA(IP13_5_3, STM_N, SEL_FSN_0),
+	PINMUX_IPSR_DATA(IP13_5_3, ATACS10_N),
+	PINMUX_IPSR_MODSEL_DATA(IP13_5_3, ETH_REFCLK_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_8_6, SSI_SCK9, SEL_SSI9_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_8_6, SCIF2_SCK_B, SEL_SCIF2_1),
+	PINMUX_IPSR_DATA(IP13_8_6, PWM2_B),
+	PINMUX_IPSR_DATA(IP13_8_6, VI1_DATA5),
+	PINMUX_IPSR_MODSEL_DATA(IP13_8_6, MTS_N, SEL_FSN_0),
+	PINMUX_IPSR_DATA(IP13_8_6, EX_WAIT1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_8_6, ETH_TXD1_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_11_9, SSI_WS9, SEL_SSI9_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_11_9, SCIF2_RXD_B, SEL_SCIF2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_11_9, I2C3_SCL_E, SEL_I2C03_4),
+	PINMUX_IPSR_DATA(IP13_11_9, VI1_DATA6),
+	PINMUX_IPSR_DATA(IP13_11_9, ATARD0_N),
+	PINMUX_IPSR_MODSEL_DATA(IP13_11_9, ETH_TX_EN_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_14_12, SSI_SDATA9, SEL_SSI9_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_14_12, SCIF2_TXD_B, SEL_SCIF2_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_14_12, I2C3_SDA_E, SEL_I2C03_4),
+	PINMUX_IPSR_DATA(IP13_14_12, VI1_DATA7),
+	PINMUX_IPSR_DATA(IP13_14_12, ATADIR0_N),
+	PINMUX_IPSR_MODSEL_DATA(IP13_14_12, ETH_MAGIC_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_17_15, AUDIO_CLKA, SEL_ADG_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_17_15, I2C0_SCL_B, SEL_I2C00_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_17_15, SCIFA4_RXD_D, SEL_SCIFA4_3),
+	PINMUX_IPSR_DATA(IP13_17_15, VI1_CLKENB),
+	PINMUX_IPSR_MODSEL_DATA(IP13_17_15, TS_SDATA_C, SEL_TSIF0_2),
+	PINMUX_IPSR_MODSEL_DATA(IP13_17_15, RIF0_SYNC_B, SEL_DR0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_17_15, ETH_TXD0_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_20_18, AUDIO_CLKB, SEL_ADG_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_20_18, I2C0_SDA_B, SEL_I2C00_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_20_18, SCIFA4_TXD_D, SEL_SCIFA4_3),
+	PINMUX_IPSR_DATA(IP13_20_18, VI1_FIELD),
+	PINMUX_IPSR_MODSEL_DATA(IP13_20_18, TS_SCK_C, SEL_TSIF0_2),
+	PINMUX_IPSR_MODSEL_DATA(IP13_20_18, RIF0_CLK_B, SEL_DR0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_20_18, BPFCLK_E, SEL_DARC_4),
+	PINMUX_IPSR_MODSEL_DATA(IP13_20_18, ETH_MDC_B, SEL_ETH_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_23_21, AUDIO_CLKC, SEL_ADG_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_23_21, I2C4_SCL_B, SEL_I2C04_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_23_21, SCIFA5_RXD_D, SEL_SCIFA5_3),
+	PINMUX_IPSR_DATA(IP13_23_21, VI1_HSYNC_N),
+	PINMUX_IPSR_MODSEL_DATA(IP13_23_21, TS_SDEN_C, SEL_TSIF0_2),
+	PINMUX_IPSR_MODSEL_DATA(IP13_23_21, RIF0_D0_B, SEL_DR0_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_23_21, FMCLK_E, SEL_DARC_4),
+	PINMUX_IPSR_MODSEL_DATA(IP13_23_21, RDS_CLK_D, SEL_RDS_3),
+	PINMUX_IPSR_MODSEL_DATA(IP13_26_24, AUDIO_CLKOUT, SEL_ADG_0),
+	PINMUX_IPSR_MODSEL_DATA(IP13_26_24, I2C4_SDA_B, SEL_I2C04_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_26_24, SCIFA5_TXD_D, SEL_SCIFA5_3),
+	PINMUX_IPSR_DATA(IP13_26_24, VI1_VSYNC_N),
+	PINMUX_IPSR_MODSEL_DATA(IP13_26_24, TS_SPSYNC_C, SEL_TSIF0_2),
+	PINMUX_IPSR_MODSEL_DATA(IP13_26_24, RIF0_D1_B, SEL_DR1_1),
+	PINMUX_IPSR_MODSEL_DATA(IP13_26_24, FMIN_E, SEL_DARC_4),
+	PINMUX_IPSR_MODSEL_DATA(IP13_26_24, RDS_DATA_D, SEL_RDS_3),
+};
+
+static const struct sh_pfc_pin pinmux_pins[] = {
+	PINMUX_GPIO_GP_ALL(),
+};
+
+/* - ETH -------------------------------------------------------------------- */
+static const unsigned int eth_link_pins[] = {
+	/* LINK */
+	RCAR_GP_PIN(3, 18),
+};
+static const unsigned int eth_link_mux[] = {
+	ETH_LINK_MARK,
+};
+static const unsigned int eth_magic_pins[] = {
+	/* MAGIC */
+	RCAR_GP_PIN(3, 22),
+};
+static const unsigned int eth_magic_mux[] = {
+	ETH_MAGIC_MARK,
+};
+static const unsigned int eth_mdio_pins[] = {
+	/* MDC, MDIO */
+	RCAR_GP_PIN(3, 24), RCAR_GP_PIN(3, 13),
+};
+static const unsigned int eth_mdio_mux[] = {
+	ETH_MDC_MARK, ETH_MDIO_MARK,
+};
+static const unsigned int eth_rmii_pins[] = {
+	/* RXD[0:1], RX_ER, CRS_DV, TXD[0:1], TX_EN, REF_CLK */
+	RCAR_GP_PIN(3, 16), RCAR_GP_PIN(3, 17), RCAR_GP_PIN(3, 15),
+	RCAR_GP_PIN(3, 14), RCAR_GP_PIN(3, 23), RCAR_GP_PIN(3, 20),
+	RCAR_GP_PIN(3, 21), RCAR_GP_PIN(3, 19),
+};
+static const unsigned int eth_rmii_mux[] = {
+	ETH_RXD0_MARK, ETH_RXD1_MARK, ETH_RX_ER_MARK, ETH_CRS_DV_MARK,
+	ETH_TXD0_MARK, ETH_TXD1_MARK, ETH_TX_EN_MARK, ETH_REFCLK_MARK,
+};
+static const unsigned int eth_link_b_pins[] = {
+	/* LINK */
+	RCAR_GP_PIN(5, 15),
+};
+static const unsigned int eth_link_b_mux[] = {
+	ETH_LINK_B_MARK,
+};
+static const unsigned int eth_magic_b_pins[] = {
+	/* MAGIC */
+	RCAR_GP_PIN(5, 19),
+};
+static const unsigned int eth_magic_b_mux[] = {
+	ETH_MAGIC_B_MARK,
+};
+static const unsigned int eth_mdio_b_pins[] = {
+	/* MDC, MDIO */
+	RCAR_GP_PIN(5, 21), RCAR_GP_PIN(5, 10),
+};
+static const unsigned int eth_mdio_b_mux[] = {
+	ETH_MDC_B_MARK, ETH_MDIO_B_MARK,
+};
+static const unsigned int eth_rmii_b_pins[] = {
+	/* RXD[0:1], RX_ER, CRS_DV, TXD[0:1], TX_EN, REF_CLK */
+	RCAR_GP_PIN(5, 13), RCAR_GP_PIN(5, 14), RCAR_GP_PIN(5, 12),
+	RCAR_GP_PIN(5, 11), RCAR_GP_PIN(5, 20), RCAR_GP_PIN(5, 17),
+	RCAR_GP_PIN(5, 18), RCAR_GP_PIN(5, 16),
+};
+static const unsigned int eth_rmii_b_mux[] = {
+	ETH_RXD0_B_MARK, ETH_RXD1_B_MARK, ETH_RX_ER_B_MARK, ETH_CRS_DV_B_MARK,
+	ETH_TXD0_B_MARK, ETH_TXD1_B_MARK, ETH_TX_EN_B_MARK, ETH_REFCLK_B_MARK,
+};
+/* - HSCIF0 ----------------------------------------------------------------- */
+static const unsigned int hscif0_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(3, 25), RCAR_GP_PIN(3, 26),
+};
+static const unsigned int hscif0_data_mux[] = {
+	HSCIF0_HRX_MARK, HSCIF0_HTX_MARK,
+};
+static const unsigned int hscif0_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(3, 29),
+};
+static const unsigned int hscif0_clk_mux[] = {
+	HSCIF0_HSCK_MARK,
+};
+static const unsigned int hscif0_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(3, 28), RCAR_GP_PIN(3, 27),
+};
+static const unsigned int hscif0_ctrl_mux[] = {
+	HSCIF0_HRTS_N_MARK, HSCIF0_HCTS_N_MARK,
+};
+static const unsigned int hscif0_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(0, 30), RCAR_GP_PIN(0, 31),
+};
+static const unsigned int hscif0_data_b_mux[] = {
+	HSCIF0_HRX_B_MARK, HSCIF0_HTX_B_MARK,
+};
+static const unsigned int hscif0_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 0),
+};
+static const unsigned int hscif0_clk_b_mux[] = {
+	HSCIF0_HSCK_B_MARK,
+};
+/* - HSCIF1 ----------------------------------------------------------------- */
+static const unsigned int hscif1_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(4, 8), RCAR_GP_PIN(4, 9),
+};
+static const unsigned int hscif1_data_mux[] = {
+	HSCIF1_HRX_MARK, HSCIF1_HTX_MARK,
+};
+static const unsigned int hscif1_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(4, 10),
+};
+static const unsigned int hscif1_clk_mux[] = {
+	HSCIF1_HSCK_MARK,
+};
+static const unsigned int hscif1_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(4, 12), RCAR_GP_PIN(4, 11),
+};
+static const unsigned int hscif1_ctrl_mux[] = {
+	HSCIF1_HRTS_N_MARK, HSCIF1_HCTS_N_MARK,
+};
+static const unsigned int hscif1_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(5, 13), RCAR_GP_PIN(5, 14),
+};
+static const unsigned int hscif1_data_b_mux[] = {
+	HSCIF1_HRX_B_MARK, HSCIF1_HTX_B_MARK,
+};
+static const unsigned int hscif1_ctrl_b_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(5, 16), RCAR_GP_PIN(5, 15),
+};
+static const unsigned int hscif1_ctrl_b_mux[] = {
+	HSCIF1_HRTS_N_B_MARK, HSCIF1_HCTS_N_B_MARK,
+};
+/* - HSCIF2 ----------------------------------------------------------------- */
+static const unsigned int hscif2_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(0, 8), RCAR_GP_PIN(0, 9),
+};
+static const unsigned int hscif2_data_mux[] = {
+	HSCIF2_HRX_MARK, HSCIF2_HTX_MARK,
+};
+static const unsigned int hscif2_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 10),
+};
+static const unsigned int hscif2_clk_mux[] = {
+	HSCIF2_HSCK_MARK,
+};
+static const unsigned int hscif2_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(0, 12), RCAR_GP_PIN(0, 11),
+};
+static const unsigned int hscif2_ctrl_mux[] = {
+	HSCIF2_HRTS_N_MARK, HSCIF2_HCTS_N_MARK,
+};
+/* - I2C0 ------------------------------------------------------------------- */
+static const unsigned int i2c0_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 30), RCAR_GP_PIN(3, 31),
+};
+static const unsigned int i2c0_mux[] = {
+	I2C0_SCL_MARK, I2C0_SDA_MARK,
+};
+static const unsigned int i2c0_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(5, 20), RCAR_GP_PIN(5, 21),
+};
+static const unsigned int i2c0_b_mux[] = {
+	I2C0_SCL_B_MARK, I2C0_SDA_B_MARK,
+};
+static const unsigned int i2c0_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 11), RCAR_GP_PIN(3, 12),
+};
+static const unsigned int i2c0_c_mux[] = {
+	I2C0_SCL_C_MARK, I2C0_SDA_C_MARK,
+};
+static const unsigned int i2c0_d_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(0, 5), RCAR_GP_PIN(0, 6),
+};
+static const unsigned int i2c0_d_mux[] = {
+	I2C0_SCL_D_MARK, I2C0_SDA_D_MARK,
+};
+static const unsigned int i2c0_e_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 27), RCAR_GP_PIN(3, 28),
+};
+static const unsigned int i2c0_e_mux[] = {
+	I2C0_SCL_E_MARK, I2C0_SDA_E_MARK,
+};
+/* - I2C1 ------------------------------------------------------------------- */
+static const unsigned int i2c1_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 0), RCAR_GP_PIN(4, 1),
+};
+static const unsigned int i2c1_mux[] = {
+	I2C1_SCL_MARK, I2C1_SDA_MARK,
+};
+static const unsigned int i2c1_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(0, 8), RCAR_GP_PIN(0, 9),
+};
+static const unsigned int i2c1_b_mux[] = {
+	I2C1_SCL_B_MARK, I2C1_SDA_B_MARK,
+};
+static const unsigned int i2c1_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 25), RCAR_GP_PIN(3, 26),
+};
+static const unsigned int i2c1_c_mux[] = {
+	I2C1_SCL_C_MARK, I2C1_SDA_C_MARK,
+};
+static const unsigned int i2c1_d_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(0, 11), RCAR_GP_PIN(0, 12),
+};
+static const unsigned int i2c1_d_mux[] = {
+	I2C1_SCL_D_MARK, I2C1_SDA_D_MARK,
+};
+static const unsigned int i2c1_e_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 20), RCAR_GP_PIN(4, 21),
+};
+static const unsigned int i2c1_e_mux[] = {
+	I2C1_SCL_E_MARK, I2C1_SDA_E_MARK,
+};
+/* - I2C2 ------------------------------------------------------------------- */
+static const unsigned int i2c2_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 22), RCAR_GP_PIN(4, 23),
+};
+static const unsigned int i2c2_mux[] = {
+	I2C2_SCL_MARK, I2C2_SDA_MARK,
+};
+static const unsigned int i2c2_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(6, 24), RCAR_GP_PIN(6, 25),
+};
+static const unsigned int i2c2_b_mux[] = {
+	I2C2_SCL_B_MARK, I2C2_SDA_B_MARK,
+};
+static const unsigned int i2c2_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 2), RCAR_GP_PIN(4, 3),
+};
+static const unsigned int i2c2_c_mux[] = {
+	I2C2_SCL_C_MARK, I2C2_SDA_C_MARK,
+};
+static const unsigned int i2c2_d_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(2, 0), RCAR_GP_PIN(2, 1),
+};
+static const unsigned int i2c2_d_mux[] = {
+	I2C2_SCL_D_MARK, I2C2_SDA_D_MARK,
+};
+static const unsigned int i2c2_e_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(1, 16), RCAR_GP_PIN(1, 17),
+};
+static const unsigned int i2c2_e_mux[] = {
+	I2C2_SCL_E_MARK, I2C2_SDA_E_MARK,
+};
+/* - I2C3 ------------------------------------------------------------------- */
+static const unsigned int i2c3_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 9), RCAR_GP_PIN(3, 10),
+};
+static const unsigned int i2c3_mux[] = {
+	I2C3_SCL_MARK, I2C3_SDA_MARK,
+};
+static const unsigned int i2c3_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(0, 3), RCAR_GP_PIN(0, 4),
+};
+static const unsigned int i2c3_b_mux[] = {
+	I2C3_SCL_B_MARK, I2C3_SDA_B_MARK,
+};
+static const unsigned int i2c3_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 25), RCAR_GP_PIN(4, 26),
+};
+static const unsigned int i2c3_c_mux[] = {
+	I2C3_SCL_C_MARK, I2C3_SDA_C_MARK,
+};
+static const unsigned int i2c3_d_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(2, 8), RCAR_GP_PIN(2, 9),
+};
+static const unsigned int i2c3_d_mux[] = {
+	I2C3_SCL_D_MARK, I2C3_SDA_D_MARK,
+};
+static const unsigned int i2c3_e_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(5, 18), RCAR_GP_PIN(5, 19),
+};
+static const unsigned int i2c3_e_mux[] = {
+	I2C3_SCL_E_MARK, I2C3_SDA_E_MARK,
+};
+/* - I2C4 ------------------------------------------------------------------- */
+static const unsigned int i2c4_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 8), RCAR_GP_PIN(4, 9),
+};
+static const unsigned int i2c4_mux[] = {
+	I2C4_SCL_MARK, I2C4_SDA_MARK,
+};
+static const unsigned int i2c4_b_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(5, 22), RCAR_GP_PIN(5, 23),
+};
+static const unsigned int i2c4_b_mux[] = {
+	I2C4_SCL_B_MARK, I2C4_SDA_B_MARK,
+};
+static const unsigned int i2c4_c_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(4, 28), RCAR_GP_PIN(4, 29),
+};
+static const unsigned int i2c4_c_mux[] = {
+	I2C4_SCL_C_MARK, I2C4_SDA_C_MARK,
+};
+static const unsigned int i2c4_d_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(2, 16), RCAR_GP_PIN(2, 17),
+};
+static const unsigned int i2c4_d_mux[] = {
+	I2C4_SCL_D_MARK, I2C4_SDA_D_MARK,
+};
+static const unsigned int i2c4_e_pins[] = {
+	/* SCL, SDA */
+	RCAR_GP_PIN(3, 23), RCAR_GP_PIN(3, 24),
+};
+static const unsigned int i2c4_e_mux[] = {
+	I2C4_SCL_E_MARK, I2C4_SDA_E_MARK,
+};
+/* - INTC ------------------------------------------------------------------- */
+static const unsigned int intc_irq0_pins[] = {
+	/* IRQ0 */
+	RCAR_GP_PIN(4, 4),
+};
+static const unsigned int intc_irq0_mux[] = {
+	IRQ0_MARK,
+};
+static const unsigned int intc_irq1_pins[] = {
+	/* IRQ1 */
+	RCAR_GP_PIN(4, 18),
+};
+static const unsigned int intc_irq1_mux[] = {
+	IRQ1_MARK,
+};
+static const unsigned int intc_irq2_pins[] = {
+	/* IRQ2 */
+	RCAR_GP_PIN(4, 19),
+};
+static const unsigned int intc_irq2_mux[] = {
+	IRQ2_MARK,
+};
+static const unsigned int intc_irq3_pins[] = {
+	/* IRQ3 */
+	RCAR_GP_PIN(0, 7),
+};
+static const unsigned int intc_irq3_mux[] = {
+	IRQ3_MARK,
+};
+static const unsigned int intc_irq4_pins[] = {
+	/* IRQ4 */
+	RCAR_GP_PIN(0, 0),
+};
+static const unsigned int intc_irq4_mux[] = {
+	IRQ4_MARK,
+};
+static const unsigned int intc_irq5_pins[] = {
+	/* IRQ5 */
+	RCAR_GP_PIN(4, 1),
+};
+static const unsigned int intc_irq5_mux[] = {
+	IRQ5_MARK,
+};
+static const unsigned int intc_irq6_pins[] = {
+	/* IRQ6 */
+	RCAR_GP_PIN(0, 10),
+};
+static const unsigned int intc_irq6_mux[] = {
+	IRQ6_MARK,
+};
+static const unsigned int intc_irq7_pins[] = {
+	/* IRQ7 */
+	RCAR_GP_PIN(6, 15),
+};
+static const unsigned int intc_irq7_mux[] = {
+	IRQ7_MARK,
+};
+static const unsigned int intc_irq8_pins[] = {
+	/* IRQ8 */
+	RCAR_GP_PIN(5, 0),
+};
+static const unsigned int intc_irq8_mux[] = {
+	IRQ8_MARK,
+};
+static const unsigned int intc_irq9_pins[] = {
+	/* IRQ9 */
+	RCAR_GP_PIN(5, 10),
+};
+static const unsigned int intc_irq9_mux[] = {
+	IRQ9_MARK,
+};
+/* - MMCIF ------------------------------------------------------------------ */
+static const unsigned int mmc_data1_pins[] = {
+	/* D[0] */
+	RCAR_GP_PIN(6, 18),
+};
+static const unsigned int mmc_data1_mux[] = {
+	MMC_D0_MARK,
+};
+static const unsigned int mmc_data4_pins[] = {
+	/* D[0:3] */
+	RCAR_GP_PIN(6, 18), RCAR_GP_PIN(6, 19),
+	RCAR_GP_PIN(6, 20), RCAR_GP_PIN(6, 21),
+};
+static const unsigned int mmc_data4_mux[] = {
+	MMC_D0_MARK, MMC_D1_MARK, MMC_D2_MARK, MMC_D3_MARK,
+};
+static const unsigned int mmc_data8_pins[] = {
+	/* D[0:7] */
+	RCAR_GP_PIN(6, 18), RCAR_GP_PIN(6, 19),
+	RCAR_GP_PIN(6, 20), RCAR_GP_PIN(6, 21),
+	RCAR_GP_PIN(6, 22), RCAR_GP_PIN(6, 23),
+	RCAR_GP_PIN(6, 24), RCAR_GP_PIN(6, 25),
+};
+static const unsigned int mmc_data8_mux[] = {
+	MMC_D0_MARK, MMC_D1_MARK, MMC_D2_MARK, MMC_D3_MARK,
+	MMC_D4_MARK, MMC_D5_MARK, MMC_D6_MARK, MMC_D7_MARK,
+};
+static const unsigned int mmc_ctrl_pins[] = {
+	/* CLK, CMD */
+	RCAR_GP_PIN(6, 16), RCAR_GP_PIN(6, 17),
+};
+static const unsigned int mmc_ctrl_mux[] = {
+	MMC_CLK_MARK, MMC_CMD_MARK,
+};
+/* - MSIOF0 ----------------------------------------------------------------- */
+static const unsigned int msiof0_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(4, 4),
+};
+static const unsigned int msiof0_clk_mux[] = {
+	MSIOF0_SCK_MARK,
+};
+static const unsigned int msiof0_sync_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(4, 5),
+};
+static const unsigned int msiof0_sync_mux[] = {
+	MSIOF0_SYNC_MARK,
+};
+static const unsigned int msiof0_ss1_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(4, 6),
+};
+static const unsigned int msiof0_ss1_mux[] = {
+	MSIOF0_SS1_MARK,
+};
+static const unsigned int msiof0_ss2_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(4, 7),
+};
+static const unsigned int msiof0_ss2_mux[] = {
+	MSIOF0_SS2_MARK,
+};
+static const unsigned int msiof0_rx_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(4, 2),
+};
+static const unsigned int msiof0_rx_mux[] = {
+	MSIOF0_RXD_MARK,
+};
+static const unsigned int msiof0_tx_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(4, 3),
+};
+static const unsigned int msiof0_tx_mux[] = {
+	MSIOF0_TXD_MARK,
+};
+/* - MSIOF1 ----------------------------------------------------------------- */
+static const unsigned int msiof1_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 26),
+};
+static const unsigned int msiof1_clk_mux[] = {
+	MSIOF1_SCK_MARK,
+};
+static const unsigned int msiof1_sync_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(0, 27),
+};
+static const unsigned int msiof1_sync_mux[] = {
+	MSIOF1_SYNC_MARK,
+};
+static const unsigned int msiof1_ss1_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(0, 28),
+};
+static const unsigned int msiof1_ss1_mux[] = {
+	MSIOF1_SS1_MARK,
+};
+static const unsigned int msiof1_ss2_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(0, 29),
+};
+static const unsigned int msiof1_ss2_mux[] = {
+	MSIOF1_SS2_MARK,
+};
+static const unsigned int msiof1_rx_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(0, 24),
+};
+static const unsigned int msiof1_rx_mux[] = {
+	MSIOF1_RXD_MARK,
+};
+static const unsigned int msiof1_tx_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(0, 25),
+};
+static const unsigned int msiof1_tx_mux[] = {
+	MSIOF1_TXD_MARK,
+};
+static const unsigned int msiof1_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 3),
+};
+static const unsigned int msiof1_clk_b_mux[] = {
+	MSIOF1_SCK_B_MARK,
+};
+static const unsigned int msiof1_sync_b_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(5, 4),
+};
+static const unsigned int msiof1_sync_b_mux[] = {
+	MSIOF1_SYNC_B_MARK,
+};
+static const unsigned int msiof1_ss1_b_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(5, 5),
+};
+static const unsigned int msiof1_ss1_b_mux[] = {
+	MSIOF1_SS1_B_MARK,
+};
+static const unsigned int msiof1_ss2_b_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(5, 6),
+};
+static const unsigned int msiof1_ss2_b_mux[] = {
+	MSIOF1_SS2_B_MARK,
+};
+static const unsigned int msiof1_rx_b_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(5, 1),
+};
+static const unsigned int msiof1_rx_b_mux[] = {
+	MSIOF1_RXD_B_MARK,
+};
+static const unsigned int msiof1_tx_b_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(5, 2),
+};
+static const unsigned int msiof1_tx_b_mux[] = {
+	MSIOF1_TXD_B_MARK,
+};
+/* - MSIOF2 ----------------------------------------------------------------- */
+static const unsigned int msiof2_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 0),
+};
+static const unsigned int msiof2_clk_mux[] = {
+	MSIOF2_SCK_MARK,
+};
+static const unsigned int msiof2_sync_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(1, 1),
+};
+static const unsigned int msiof2_sync_mux[] = {
+	MSIOF2_SYNC_MARK,
+};
+static const unsigned int msiof2_ss1_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(1, 2),
+};
+static const unsigned int msiof2_ss1_mux[] = {
+	MSIOF2_SS1_MARK,
+};
+static const unsigned int msiof2_ss2_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(1, 3),
+};
+static const unsigned int msiof2_ss2_mux[] = {
+	MSIOF2_SS2_MARK,
+};
+static const unsigned int msiof2_rx_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(0, 30),
+};
+static const unsigned int msiof2_rx_mux[] = {
+	MSIOF2_RXD_MARK,
+};
+static const unsigned int msiof2_tx_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(0, 31),
+};
+static const unsigned int msiof2_tx_mux[] = {
+	MSIOF2_TXD_MARK,
+};
+static const unsigned int msiof2_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(3, 15),
+};
+static const unsigned int msiof2_clk_b_mux[] = {
+	MSIOF2_SCK_B_MARK,
+};
+static const unsigned int msiof2_sync_b_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(3, 16),
+};
+static const unsigned int msiof2_sync_b_mux[] = {
+	MSIOF2_SYNC_B_MARK,
+};
+static const unsigned int msiof2_ss1_b_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(3, 17),
+};
+static const unsigned int msiof2_ss1_b_mux[] = {
+	MSIOF2_SS1_B_MARK,
+};
+static const unsigned int msiof2_ss2_b_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(3, 18),
+};
+static const unsigned int msiof2_ss2_b_mux[] = {
+	MSIOF2_SS2_B_MARK,
+};
+static const unsigned int msiof2_rx_b_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(3, 13),
+};
+static const unsigned int msiof2_rx_b_mux[] = {
+	MSIOF2_RXD_B_MARK,
+};
+static const unsigned int msiof2_tx_b_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(3, 14),
+};
+static const unsigned int msiof2_tx_b_mux[] = {
+	MSIOF2_TXD_B_MARK,
+};
+/* - QSPI ------------------------------------------------------------------- */
+static const unsigned int qspi_ctrl_pins[] = {
+	/* SPCLK, SSL */
+	RCAR_GP_PIN(1, 4), RCAR_GP_PIN(1, 9),
+};
+static const unsigned int qspi_ctrl_mux[] = {
+	SPCLK_MARK, SSL_MARK,
+};
+static const unsigned int qspi_data2_pins[] = {
+	/* MOSI_IO0, MISO_IO1 */
+	RCAR_GP_PIN(1, 5), RCAR_GP_PIN(1, 6),
+};
+static const unsigned int qspi_data2_mux[] = {
+	MOSI_IO0_MARK, MISO_IO1_MARK,
+};
+static const unsigned int qspi_data4_pins[] = {
+	/* MOSI_IO0, MISO_IO1, IO2, IO3 */
+	RCAR_GP_PIN(1, 5), RCAR_GP_PIN(1, 6), RCAR_GP_PIN(1, 7),
+	RCAR_GP_PIN(1, 8),
+};
+static const unsigned int qspi_data4_mux[] = {
+	MOSI_IO0_MARK, MISO_IO1_MARK, IO2_MARK, IO3_MARK,
+};
+/* - SCIF0 ------------------------------------------------------------------ */
+static const unsigned int scif0_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(6, 24), RCAR_GP_PIN(6, 25),
+};
+static const unsigned int scif0_data_mux[] = {
+	SCIF0_RXD_MARK, SCIF0_TXD_MARK,
+};
+static const unsigned int scif0_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 23),
+};
+static const unsigned int scif0_clk_mux[] = {
+	SCIF_CLK_MARK,
+};
+static const unsigned int scif0_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(3, 11), RCAR_GP_PIN(3, 12),
+};
+static const unsigned int scif0_data_b_mux[] = {
+	SCIF0_RXD_B_MARK, SCIF0_TXD_B_MARK,
+};
+static const unsigned int scif0_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(3, 29),
+};
+static const unsigned int scif0_clk_b_mux[] = {
+	SCIF_CLK_B_MARK,
+};
+static const unsigned int scif0_data_c_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(3, 30), RCAR_GP_PIN(3, 31),
+};
+static const unsigned int scif0_data_c_mux[] = {
+	SCIF0_RXD_C_MARK, SCIF0_TXD_C_MARK,
+};
+static const unsigned int scif0_data_d_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(3, 27), RCAR_GP_PIN(3, 28),
+};
+static const unsigned int scif0_data_d_mux[] = {
+	SCIF0_RXD_D_MARK, SCIF0_TXD_D_MARK,
+};
+/* - SCIF1 ------------------------------------------------------------------ */
+static const unsigned int scif1_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(4, 14), RCAR_GP_PIN(4, 15),
+};
+static const unsigned int scif1_data_mux[] = {
+	SCIF1_RXD_MARK, SCIF1_TXD_MARK,
+};
+static const unsigned int scif1_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(4, 13),
+};
+static const unsigned int scif1_clk_mux[] = {
+	SCIF1_SCK_MARK,
+};
+static const unsigned int scif1_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(5, 11), RCAR_GP_PIN(5, 12),
+};
+static const unsigned int scif1_data_b_mux[] = {
+	SCIF1_RXD_B_MARK, SCIF1_TXD_B_MARK,
+};
+static const unsigned int scif1_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 10),
+};
+static const unsigned int scif1_clk_b_mux[] = {
+	SCIF1_SCK_B_MARK,
+};
+static const unsigned int scif1_data_c_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(0, 11), RCAR_GP_PIN(0, 12),
+};
+static const unsigned int scif1_data_c_mux[] = {
+	SCIF1_RXD_C_MARK, SCIF1_TXD_C_MARK,
+};
+static const unsigned int scif1_clk_c_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 10),
+};
+static const unsigned int scif1_clk_c_mux[] = {
+	SCIF1_SCK_C_MARK,
+};
+/* - SCIF2 ------------------------------------------------------------------ */
+static const unsigned int scif2_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(4, 16), RCAR_GP_PIN(4, 17),
+};
+static const unsigned int scif2_data_mux[] = {
+	SCIF2_RXD_MARK, SCIF2_TXD_MARK,
+};
+static const unsigned int scif2_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(4, 18),
+};
+static const unsigned int scif2_clk_mux[] = {
+	SCIF2_SCK_MARK,
+};
+static const unsigned int scif2_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(5, 18), RCAR_GP_PIN(5, 19),
+};
+static const unsigned int scif2_data_b_mux[] = {
+	SCIF2_RXD_B_MARK, SCIF2_TXD_B_MARK,
+};
+static const unsigned int scif2_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 17),
+};
+static const unsigned int scif2_clk_b_mux[] = {
+	SCIF2_SCK_B_MARK,
+};
+static const unsigned int scif2_data_c_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(3, 20), RCAR_GP_PIN(3, 21),
+};
+static const unsigned int scif2_data_c_mux[] = {
+	SCIF2_RXD_C_MARK, SCIF2_TXD_C_MARK,
+};
+static const unsigned int scif2_clk_c_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(3, 19),
+};
+static const unsigned int scif2_clk_c_mux[] = {
+	SCIF2_SCK_C_MARK,
+};
+/* - SCIF3 ------------------------------------------------------------------ */
+static const unsigned int scif3_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(4, 20), RCAR_GP_PIN(4, 21),
+};
+static const unsigned int scif3_data_mux[] = {
+	SCIF3_RXD_MARK, SCIF3_TXD_MARK,
+};
+static const unsigned int scif3_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(4, 19),
+};
+static const unsigned int scif3_clk_mux[] = {
+	SCIF3_SCK_MARK,
+};
+static const unsigned int scif3_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(3, 23), RCAR_GP_PIN(3, 24),
+};
+static const unsigned int scif3_data_b_mux[] = {
+	SCIF3_RXD_B_MARK, SCIF3_TXD_B_MARK,
+};
+static const unsigned int scif3_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(3, 22),
+};
+static const unsigned int scif3_clk_b_mux[] = {
+	SCIF3_SCK_B_MARK,
+};
+/* - SCIF4 ------------------------------------------------------------------ */
+static const unsigned int scif4_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(4, 0), RCAR_GP_PIN(4, 1),
+};
+static const unsigned int scif4_data_mux[] = {
+	SCIF4_RXD_MARK, SCIF4_TXD_MARK,
+};
+static const unsigned int scif4_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(0, 5), RCAR_GP_PIN(0, 6),
+};
+static const unsigned int scif4_data_b_mux[] = {
+	SCIF4_RXD_B_MARK, SCIF4_TXD_B_MARK,
+};
+static const unsigned int scif4_data_c_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(1, 14), RCAR_GP_PIN(1, 15),
+};
+static const unsigned int scif4_data_c_mux[] = {
+	SCIF4_RXD_C_MARK, SCIF4_TXD_C_MARK,
+};
+static const unsigned int scif4_data_d_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(3, 17), RCAR_GP_PIN(3, 18),
+};
+static const unsigned int scif4_data_d_mux[] = {
+	SCIF4_RXD_D_MARK, SCIF4_TXD_D_MARK,
+};
+static const unsigned int scif4_data_e_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(1, 1), RCAR_GP_PIN(1, 2),
+};
+static const unsigned int scif4_data_e_mux[] = {
+	SCIF4_RXD_E_MARK, SCIF4_TXD_E_MARK,
+};
+/* - SCIF5 ------------------------------------------------------------------ */
+static const unsigned int scif5_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(4, 2), RCAR_GP_PIN(4, 3),
+};
+static const unsigned int scif5_data_mux[] = {
+	SCIF5_RXD_MARK, SCIF5_TXD_MARK,
+};
+static const unsigned int scif5_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(0, 3), RCAR_GP_PIN(0, 4),
+};
+static const unsigned int scif5_data_b_mux[] = {
+	SCIF5_RXD_B_MARK, SCIF5_TXD_B_MARK,
+};
+static const unsigned int scif5_data_c_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(2, 0), RCAR_GP_PIN(2, 11),
+};
+static const unsigned int scif5_data_c_mux[] = {
+	SCIF5_RXD_C_MARK, SCIF5_TXD_C_MARK,
+};
+static const unsigned int scif5_data_d_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(5, 1), RCAR_GP_PIN(5, 2),
+};
+static const unsigned int scif5_data_d_mux[] = {
+	SCIF5_RXD_D_MARK, SCIF5_TXD_D_MARK,
+};
+/* - SCIFA0 ----------------------------------------------------------------- */
+static const unsigned int scifa0_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(4, 6), RCAR_GP_PIN(4, 7),
+};
+static const unsigned int scifa0_data_mux[] = {
+	SCIFA0_RXD_MARK, SCIFA0_TXD_MARK,
+};
+static const unsigned int scifa0_data_b_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(0, 24), RCAR_GP_PIN(0, 25),
+};
+static const unsigned int scifa0_data_b_mux[] = {
+	SCIFA0_RXD_B_MARK, SCIFA0_TXD_B_MARK
+};
+static const unsigned int scifa0_data_c_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(2, 8), RCAR_GP_PIN(2, 9),
+};
+static const unsigned int scifa0_data_c_mux[] = {
+	SCIFA0_RXD_C_MARK, SCIFA0_TXD_C_MARK
+};
+static const unsigned int scifa0_data_d_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(5, 15), RCAR_GP_PIN(5, 16),
+};
+static const unsigned int scifa0_data_d_mux[] = {
+	SCIFA0_RXD_D_MARK, SCIFA0_TXD_D_MARK
+};
+/* - SCIFA1 ----------------------------------------------------------------- */
+static const unsigned int scifa1_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(0, 14), RCAR_GP_PIN(0, 15),
+};
+static const unsigned int scifa1_data_mux[] = {
+	SCIFA1_RXD_MARK, SCIFA1_TXD_MARK,
+};
+static const unsigned int scifa1_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 13),
+};
+static const unsigned int scifa1_clk_mux[] = {
+	SCIFA1_SCK_MARK,
+};
+static const unsigned int scifa1_data_b_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(4, 28), RCAR_GP_PIN(4, 29),
+};
+static const unsigned int scifa1_data_b_mux[] = {
+	SCIFA1_RXD_B_MARK, SCIFA1_TXD_B_MARK,
+};
+static const unsigned int scifa1_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(4, 27),
+};
+static const unsigned int scifa1_clk_b_mux[] = {
+	SCIFA1_SCK_B_MARK,
+};
+static const unsigned int scifa1_data_c_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(5, 5), RCAR_GP_PIN(5, 6),
+};
+static const unsigned int scifa1_data_c_mux[] = {
+	SCIFA1_RXD_C_MARK, SCIFA1_TXD_C_MARK,
+};
+static const unsigned int scifa1_clk_c_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 4),
+};
+static const unsigned int scifa1_clk_c_mux[] = {
+	SCIFA1_SCK_C_MARK,
+};
+/* - SCIFA2 ----------------------------------------------------------------- */
+static const unsigned int scifa2_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(1, 16), RCAR_GP_PIN(1, 17),
+};
+static const unsigned int scifa2_data_mux[] = {
+	SCIFA2_RXD_MARK, SCIFA2_TXD_MARK,
+};
+static const unsigned int scifa2_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 15),
+};
+static const unsigned int scifa2_clk_mux[] = {
+	SCIFA2_SCK_MARK,
+};
+static const unsigned int scifa2_data_b_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(4, 31), RCAR_GP_PIN(5, 0),
+};
+static const unsigned int scifa2_data_b_mux[] = {
+	SCIFA2_RXD_B_MARK, SCIFA2_TXD_B_MARK,
+};
+static const unsigned int scifa2_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(4, 30),
+};
+static const unsigned int scifa2_clk_b_mux[] = {
+	SCIFA2_SCK_B_MARK,
+};
+/* - SCIFA3 ----------------------------------------------------------------- */
+static const unsigned int scifa3_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(4, 25), RCAR_GP_PIN(4, 26),
+};
+static const unsigned int scifa3_data_mux[] = {
+	SCIFA3_RXD_MARK, SCIFA3_TXD_MARK,
+};
+static const unsigned int scifa3_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(4, 24),
+};
+static const unsigned int scifa3_clk_mux[] = {
+	SCIFA3_SCK_MARK,
+};
+static const unsigned int scifa3_data_b_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(0, 1), RCAR_GP_PIN(0, 2),
+};
+static const unsigned int scifa3_data_b_mux[] = {
+	SCIFA3_RXD_B_MARK, SCIFA3_TXD_B_MARK,
+};
+static const unsigned int scifa3_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 0),
+};
+static const unsigned int scifa3_clk_b_mux[] = {
+	SCIFA3_SCK_B_MARK,
+};
+/* - SCIFA4 ----------------------------------------------------------------- */
+static const unsigned int scifa4_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(4, 12), RCAR_GP_PIN(4, 12),
+};
+static const unsigned int scifa4_data_mux[] = {
+	SCIFA4_RXD_MARK, SCIFA4_TXD_MARK,
+};
+static const unsigned int scifa4_data_b_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(0, 22), RCAR_GP_PIN(0, 23),
+};
+static const unsigned int scifa4_data_b_mux[] = {
+	SCIFA4_RXD_B_MARK, SCIFA4_TXD_B_MARK,
+};
+static const unsigned int scifa4_data_c_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(2, 16), RCAR_GP_PIN(2, 17),
+};
+static const unsigned int scifa4_data_c_mux[] = {
+	SCIFA4_RXD_C_MARK, SCIFA4_TXD_C_MARK,
+};
+static const unsigned int scifa4_data_d_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(5, 20), RCAR_GP_PIN(5, 21),
+};
+static const unsigned int scifa4_data_d_mux[] = {
+	SCIFA4_RXD_D_MARK, SCIFA4_TXD_D_MARK,
+};
+/* - SCIFA5 ----------------------------------------------------------------- */
+static const unsigned int scifa5_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(4, 22), RCAR_GP_PIN(4, 23),
+};
+static const unsigned int scifa5_data_mux[] = {
+	SCIFA5_RXD_MARK, SCIFA5_TXD_MARK,
+};
+static const unsigned int scifa5_data_b_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(0, 28), RCAR_GP_PIN(0, 29),
+};
+static const unsigned int scifa5_data_b_mux[] = {
+	SCIFA5_RXD_B_MARK, SCIFA5_TXD_B_MARK,
+};
+static const unsigned int scifa5_data_c_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(3, 9), RCAR_GP_PIN(3, 10),
+};
+static const unsigned int scifa5_data_c_mux[] = {
+	SCIFA5_RXD_C_MARK, SCIFA5_TXD_C_MARK,
+};
+static const unsigned int scifa5_data_d_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(5, 22), RCAR_GP_PIN(5, 23),
+};
+static const unsigned int scifa5_data_d_mux[] = {
+	SCIFA5_RXD_D_MARK, SCIFA5_TXD_D_MARK,
+};
+/* - SCIFB0 ----------------------------------------------------------------- */
+static const unsigned int scifb0_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(0, 21), RCAR_GP_PIN(0, 20),
+};
+static const unsigned int scifb0_data_mux[] = {
+	SCIFB0_RXD_MARK, SCIFB0_TXD_MARK,
+};
+static const unsigned int scifb0_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 19),
+};
+static const unsigned int scifb0_clk_mux[] = {
+	SCIFB0_SCK_MARK,
+};
+static const unsigned int scifb0_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(0, 23), RCAR_GP_PIN(0, 22),
+};
+static const unsigned int scifb0_ctrl_mux[] = {
+	SCIFB0_RTS_N_MARK, SCIFB0_CTS_N_MARK,
+};
+/* - SCIFB1 ----------------------------------------------------------------- */
+static const unsigned int scifb1_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(1, 24), RCAR_GP_PIN(0, 17),
+};
+static const unsigned int scifb1_data_mux[] = {
+	SCIFB1_RXD_MARK, SCIFB1_TXD_MARK,
+};
+static const unsigned int scifb1_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 16),
+};
+static const unsigned int scifb1_clk_mux[] = {
+	SCIFB1_SCK_MARK,
+};
+/* - SCIFB2 ----------------------------------------------------------------- */
+static const unsigned int scifb2_data_pins[] = {
+	/* RXD, TXD */
+	RCAR_GP_PIN(1, 13), RCAR_GP_PIN(1, 14),
+};
+static const unsigned int scifb2_data_mux[] = {
+	SCIFB2_RXD_MARK, SCIFB2_TXD_MARK,
+};
+static const unsigned int scifb2_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 15),
+};
+static const unsigned int scifb2_clk_mux[] = {
+	SCIFB2_SCK_MARK,
+};
+static const unsigned int scifb2_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(1, 17), RCAR_GP_PIN(1, 16),
+};
+static const unsigned int scifb2_ctrl_mux[] = {
+	SCIFB2_RTS_N_MARK, SCIFB2_CTS_N_MARK,
+};
+/* - SDHI0 ------------------------------------------------------------------ */
+static const unsigned int sdhi0_data1_pins[] = {
+	/* D0 */
+	RCAR_GP_PIN(6, 2),
+};
+static const unsigned int sdhi0_data1_mux[] = {
+	SD0_DATA0_MARK,
+};
+static const unsigned int sdhi0_data4_pins[] = {
+	/* D[0:3] */
+	RCAR_GP_PIN(6, 2), RCAR_GP_PIN(6, 3),
+	RCAR_GP_PIN(6, 4), RCAR_GP_PIN(6, 5),
+};
+static const unsigned int sdhi0_data4_mux[] = {
+	SD0_DATA0_MARK, SD0_DATA1_MARK, SD0_DATA2_MARK, SD0_DATA3_MARK,
+};
+static const unsigned int sdhi0_ctrl_pins[] = {
+	/* CLK, CMD */
+	RCAR_GP_PIN(6, 0), RCAR_GP_PIN(6, 1),
+};
+static const unsigned int sdhi0_ctrl_mux[] = {
+	SD0_CLK_MARK, SD0_CMD_MARK,
+};
+static const unsigned int sdhi0_cd_pins[] = {
+	/* CD */
+	RCAR_GP_PIN(6, 6),
+};
+static const unsigned int sdhi0_cd_mux[] = {
+	SD0_CD_MARK,
+};
+static const unsigned int sdhi0_wp_pins[] = {
+	/* WP */
+	RCAR_GP_PIN(6, 7),
+};
+static const unsigned int sdhi0_wp_mux[] = {
+	SD0_WP_MARK,
+};
+/* - SDHI1 ------------------------------------------------------------------ */
+static const unsigned int sdhi1_data1_pins[] = {
+	/* D0 */
+	RCAR_GP_PIN(6, 10),
+};
+static const unsigned int sdhi1_data1_mux[] = {
+	SD1_DATA0_MARK,
+};
+static const unsigned int sdhi1_data4_pins[] = {
+	/* D[0:3] */
+	RCAR_GP_PIN(6, 10), RCAR_GP_PIN(6, 11),
+	RCAR_GP_PIN(6, 12), RCAR_GP_PIN(6, 13),
+};
+static const unsigned int sdhi1_data4_mux[] = {
+	SD1_DATA0_MARK, SD1_DATA1_MARK, SD1_DATA2_MARK, SD1_DATA3_MARK,
+};
+static const unsigned int sdhi1_ctrl_pins[] = {
+	/* CLK, CMD */
+	RCAR_GP_PIN(6, 8), RCAR_GP_PIN(6, 9),
+};
+static const unsigned int sdhi1_ctrl_mux[] = {
+	SD1_CLK_MARK, SD1_CMD_MARK,
+};
+static const unsigned int sdhi1_cd_pins[] = {
+	/* CD */
+	RCAR_GP_PIN(6, 14),
+};
+static const unsigned int sdhi1_cd_mux[] = {
+	SD1_CD_MARK,
+};
+static const unsigned int sdhi1_wp_pins[] = {
+	/* WP */
+	RCAR_GP_PIN(6, 15),
+};
+static const unsigned int sdhi1_wp_mux[] = {
+	SD1_WP_MARK,
+};
+/* - SDHI2 ------------------------------------------------------------------ */
+static const unsigned int sdhi2_data1_pins[] = {
+	/* D0 */
+	RCAR_GP_PIN(6, 18),
+};
+static const unsigned int sdhi2_data1_mux[] = {
+	SD2_DATA0_MARK,
+};
+static const unsigned int sdhi2_data4_pins[] = {
+	/* D[0:3] */
+	RCAR_GP_PIN(6, 18), RCAR_GP_PIN(6, 19),
+	RCAR_GP_PIN(6, 20), RCAR_GP_PIN(6, 21),
+};
+static const unsigned int sdhi2_data4_mux[] = {
+	SD2_DATA0_MARK, SD2_DATA1_MARK, SD2_DATA2_MARK, SD2_DATA3_MARK,
+};
+static const unsigned int sdhi2_ctrl_pins[] = {
+	/* CLK, CMD */
+	RCAR_GP_PIN(6, 16), RCAR_GP_PIN(6, 17),
+};
+static const unsigned int sdhi2_ctrl_mux[] = {
+	SD2_CLK_MARK, SD2_CMD_MARK,
+};
+static const unsigned int sdhi2_cd_pins[] = {
+	/* CD */
+	RCAR_GP_PIN(6, 22),
+};
+static const unsigned int sdhi2_cd_mux[] = {
+	SD2_CD_MARK,
+};
+static const unsigned int sdhi2_wp_pins[] = {
+	/* WP */
+	RCAR_GP_PIN(6, 23),
+};
+static const unsigned int sdhi2_wp_mux[] = {
+	SD2_WP_MARK,
+};
+
+static const struct sh_pfc_pin_group pinmux_groups[] = {
+	SH_PFC_PIN_GROUP(eth_link),
+	SH_PFC_PIN_GROUP(eth_magic),
+	SH_PFC_PIN_GROUP(eth_mdio),
+	SH_PFC_PIN_GROUP(eth_rmii),
+	SH_PFC_PIN_GROUP(eth_link_b),
+	SH_PFC_PIN_GROUP(eth_magic_b),
+	SH_PFC_PIN_GROUP(eth_mdio_b),
+	SH_PFC_PIN_GROUP(eth_rmii_b),
+	SH_PFC_PIN_GROUP(hscif0_data),
+	SH_PFC_PIN_GROUP(hscif0_clk),
+	SH_PFC_PIN_GROUP(hscif0_ctrl),
+	SH_PFC_PIN_GROUP(hscif0_data_b),
+	SH_PFC_PIN_GROUP(hscif0_clk_b),
+	SH_PFC_PIN_GROUP(hscif1_data),
+	SH_PFC_PIN_GROUP(hscif1_clk),
+	SH_PFC_PIN_GROUP(hscif1_ctrl),
+	SH_PFC_PIN_GROUP(hscif1_data_b),
+	SH_PFC_PIN_GROUP(hscif1_ctrl_b),
+	SH_PFC_PIN_GROUP(hscif2_data),
+	SH_PFC_PIN_GROUP(hscif2_clk),
+	SH_PFC_PIN_GROUP(hscif2_ctrl),
+	SH_PFC_PIN_GROUP(i2c0),
+	SH_PFC_PIN_GROUP(i2c0_b),
+	SH_PFC_PIN_GROUP(i2c0_c),
+	SH_PFC_PIN_GROUP(i2c0_d),
+	SH_PFC_PIN_GROUP(i2c0_e),
+	SH_PFC_PIN_GROUP(i2c1),
+	SH_PFC_PIN_GROUP(i2c1_b),
+	SH_PFC_PIN_GROUP(i2c1_c),
+	SH_PFC_PIN_GROUP(i2c1_d),
+	SH_PFC_PIN_GROUP(i2c1_e),
+	SH_PFC_PIN_GROUP(i2c2),
+	SH_PFC_PIN_GROUP(i2c2_b),
+	SH_PFC_PIN_GROUP(i2c2_c),
+	SH_PFC_PIN_GROUP(i2c2_d),
+	SH_PFC_PIN_GROUP(i2c2_e),
+	SH_PFC_PIN_GROUP(i2c3),
+	SH_PFC_PIN_GROUP(i2c3_b),
+	SH_PFC_PIN_GROUP(i2c3_c),
+	SH_PFC_PIN_GROUP(i2c3_d),
+	SH_PFC_PIN_GROUP(i2c3_e),
+	SH_PFC_PIN_GROUP(i2c4),
+	SH_PFC_PIN_GROUP(i2c4_b),
+	SH_PFC_PIN_GROUP(i2c4_c),
+	SH_PFC_PIN_GROUP(i2c4_d),
+	SH_PFC_PIN_GROUP(i2c4_e),
+	SH_PFC_PIN_GROUP(intc_irq0),
+	SH_PFC_PIN_GROUP(intc_irq1),
+	SH_PFC_PIN_GROUP(intc_irq2),
+	SH_PFC_PIN_GROUP(intc_irq3),
+	SH_PFC_PIN_GROUP(intc_irq4),
+	SH_PFC_PIN_GROUP(intc_irq5),
+	SH_PFC_PIN_GROUP(intc_irq6),
+	SH_PFC_PIN_GROUP(intc_irq7),
+	SH_PFC_PIN_GROUP(intc_irq8),
+	SH_PFC_PIN_GROUP(intc_irq9),
+	SH_PFC_PIN_GROUP(mmc_data1),
+	SH_PFC_PIN_GROUP(mmc_data4),
+	SH_PFC_PIN_GROUP(mmc_data8),
+	SH_PFC_PIN_GROUP(mmc_ctrl),
+	SH_PFC_PIN_GROUP(msiof0_clk),
+	SH_PFC_PIN_GROUP(msiof0_sync),
+	SH_PFC_PIN_GROUP(msiof0_ss1),
+	SH_PFC_PIN_GROUP(msiof0_ss2),
+	SH_PFC_PIN_GROUP(msiof0_rx),
+	SH_PFC_PIN_GROUP(msiof0_tx),
+	SH_PFC_PIN_GROUP(msiof1_clk),
+	SH_PFC_PIN_GROUP(msiof1_sync),
+	SH_PFC_PIN_GROUP(msiof1_ss1),
+	SH_PFC_PIN_GROUP(msiof1_ss2),
+	SH_PFC_PIN_GROUP(msiof1_rx),
+	SH_PFC_PIN_GROUP(msiof1_tx),
+	SH_PFC_PIN_GROUP(msiof1_clk_b),
+	SH_PFC_PIN_GROUP(msiof1_sync_b),
+	SH_PFC_PIN_GROUP(msiof1_ss1_b),
+	SH_PFC_PIN_GROUP(msiof1_ss2_b),
+	SH_PFC_PIN_GROUP(msiof1_rx_b),
+	SH_PFC_PIN_GROUP(msiof1_tx_b),
+	SH_PFC_PIN_GROUP(msiof2_clk),
+	SH_PFC_PIN_GROUP(msiof2_sync),
+	SH_PFC_PIN_GROUP(msiof2_ss1),
+	SH_PFC_PIN_GROUP(msiof2_ss2),
+	SH_PFC_PIN_GROUP(msiof2_rx),
+	SH_PFC_PIN_GROUP(msiof2_tx),
+	SH_PFC_PIN_GROUP(msiof2_clk_b),
+	SH_PFC_PIN_GROUP(msiof2_sync_b),
+	SH_PFC_PIN_GROUP(msiof2_ss1_b),
+	SH_PFC_PIN_GROUP(msiof2_ss2_b),
+	SH_PFC_PIN_GROUP(msiof2_rx_b),
+	SH_PFC_PIN_GROUP(msiof2_tx_b),
+	SH_PFC_PIN_GROUP(qspi_ctrl),
+	SH_PFC_PIN_GROUP(qspi_data2),
+	SH_PFC_PIN_GROUP(qspi_data4),
+	SH_PFC_PIN_GROUP(scif0_data),
+	SH_PFC_PIN_GROUP(scif0_clk),
+	SH_PFC_PIN_GROUP(scif0_data_b),
+	SH_PFC_PIN_GROUP(scif0_clk_b),
+	SH_PFC_PIN_GROUP(scif0_data_c),
+	SH_PFC_PIN_GROUP(scif0_data_d),
+	SH_PFC_PIN_GROUP(scif1_data),
+	SH_PFC_PIN_GROUP(scif1_clk),
+	SH_PFC_PIN_GROUP(scif1_data_b),
+	SH_PFC_PIN_GROUP(scif1_clk_b),
+	SH_PFC_PIN_GROUP(scif1_data_c),
+	SH_PFC_PIN_GROUP(scif1_clk_c),
+	SH_PFC_PIN_GROUP(scif2_data),
+	SH_PFC_PIN_GROUP(scif2_clk),
+	SH_PFC_PIN_GROUP(scif2_data_b),
+	SH_PFC_PIN_GROUP(scif2_clk_b),
+	SH_PFC_PIN_GROUP(scif2_data_c),
+	SH_PFC_PIN_GROUP(scif2_clk_c),
+	SH_PFC_PIN_GROUP(scif3_data),
+	SH_PFC_PIN_GROUP(scif3_clk),
+	SH_PFC_PIN_GROUP(scif3_data_b),
+	SH_PFC_PIN_GROUP(scif3_clk_b),
+	SH_PFC_PIN_GROUP(scif4_data),
+	SH_PFC_PIN_GROUP(scif4_data_b),
+	SH_PFC_PIN_GROUP(scif4_data_c),
+	SH_PFC_PIN_GROUP(scif4_data_d),
+	SH_PFC_PIN_GROUP(scif4_data_e),
+	SH_PFC_PIN_GROUP(scif5_data),
+	SH_PFC_PIN_GROUP(scif5_data_b),
+	SH_PFC_PIN_GROUP(scif5_data_c),
+	SH_PFC_PIN_GROUP(scif5_data_d),
+	SH_PFC_PIN_GROUP(scifa0_data),
+	SH_PFC_PIN_GROUP(scifa0_data_b),
+	SH_PFC_PIN_GROUP(scifa0_data_c),
+	SH_PFC_PIN_GROUP(scifa0_data_d),
+	SH_PFC_PIN_GROUP(scifa1_data),
+	SH_PFC_PIN_GROUP(scifa1_clk),
+	SH_PFC_PIN_GROUP(scifa1_data_b),
+	SH_PFC_PIN_GROUP(scifa1_clk_b),
+	SH_PFC_PIN_GROUP(scifa1_data_c),
+	SH_PFC_PIN_GROUP(scifa1_clk_c),
+	SH_PFC_PIN_GROUP(scifa2_data),
+	SH_PFC_PIN_GROUP(scifa2_clk),
+	SH_PFC_PIN_GROUP(scifa2_data_b),
+	SH_PFC_PIN_GROUP(scifa2_clk_b),
+	SH_PFC_PIN_GROUP(scifa3_data),
+	SH_PFC_PIN_GROUP(scifa3_clk),
+	SH_PFC_PIN_GROUP(scifa3_data_b),
+	SH_PFC_PIN_GROUP(scifa3_clk_b),
+	SH_PFC_PIN_GROUP(scifa4_data),
+	SH_PFC_PIN_GROUP(scifa4_data_b),
+	SH_PFC_PIN_GROUP(scifa4_data_c),
+	SH_PFC_PIN_GROUP(scifa4_data_d),
+	SH_PFC_PIN_GROUP(scifa5_data),
+	SH_PFC_PIN_GROUP(scifa5_data_b),
+	SH_PFC_PIN_GROUP(scifa5_data_c),
+	SH_PFC_PIN_GROUP(scifa5_data_d),
+	SH_PFC_PIN_GROUP(scifb0_data),
+	SH_PFC_PIN_GROUP(scifb0_clk),
+	SH_PFC_PIN_GROUP(scifb0_ctrl),
+	SH_PFC_PIN_GROUP(scifb1_data),
+	SH_PFC_PIN_GROUP(scifb1_clk),
+	SH_PFC_PIN_GROUP(scifb2_data),
+	SH_PFC_PIN_GROUP(scifb2_clk),
+	SH_PFC_PIN_GROUP(scifb2_ctrl),
+	SH_PFC_PIN_GROUP(sdhi0_data1),
+	SH_PFC_PIN_GROUP(sdhi0_data4),
+	SH_PFC_PIN_GROUP(sdhi0_ctrl),
+	SH_PFC_PIN_GROUP(sdhi0_cd),
+	SH_PFC_PIN_GROUP(sdhi0_wp),
+	SH_PFC_PIN_GROUP(sdhi1_data1),
+	SH_PFC_PIN_GROUP(sdhi1_data4),
+	SH_PFC_PIN_GROUP(sdhi1_ctrl),
+	SH_PFC_PIN_GROUP(sdhi1_cd),
+	SH_PFC_PIN_GROUP(sdhi1_wp),
+	SH_PFC_PIN_GROUP(sdhi2_data1),
+	SH_PFC_PIN_GROUP(sdhi2_data4),
+	SH_PFC_PIN_GROUP(sdhi2_ctrl),
+	SH_PFC_PIN_GROUP(sdhi2_cd),
+	SH_PFC_PIN_GROUP(sdhi2_wp),
+};
+
+static const char * const eth_groups[] = {
+	"eth_link",
+	"eth_magic",
+	"eth_mdio",
+	"eth_rmii",
+	"eth_link_b",
+	"eth_magic_b",
+	"eth_mdio_b",
+	"eth_rmii_b",
+};
+
+static const char * const hscif0_groups[] = {
+	"hscif0_data",
+	"hscif0_clk",
+	"hscif0_ctrl",
+	"hscif0_data_b",
+	"hscif0_clk_b",
+};
+
+static const char * const hscif1_groups[] = {
+	"hscif1_data",
+	"hscif1_clk",
+	"hscif1_ctrl",
+	"hscif1_data_b",
+	"hscif1_ctrl_b",
+};
+
+static const char * const hscif2_groups[] = {
+	"hscif2_data",
+	"hscif2_clk",
+	"hscif2_ctrl",
+};
+
+static const char * const i2c0_groups[] = {
+	"i2c0",
+	"i2c0_b",
+	"i2c0_c",
+	"i2c0_d",
+	"i2c0_e",
+};
+
+static const char * const i2c1_groups[] = {
+	"i2c1",
+	"i2c1_b",
+	"i2c1_c",
+	"i2c1_d",
+	"i2c1_e",
+};
+
+static const char * const i2c2_groups[] = {
+	"i2c2",
+	"i2c2_b",
+	"i2c2_c",
+	"i2c2_d",
+	"i2c2_e",
+};
+
+static const char * const i2c3_groups[] = {
+	"i2c3",
+	"i2c3_b",
+	"i2c3_c",
+	"i2c3_d",
+	"i2c3_e",
+};
+
+static const char * const i2c4_groups[] = {
+	"i2c4",
+	"i2c4_b",
+	"i2c4_c",
+	"i2c4_d",
+	"i2c4_e",
+};
+
+static const char * const intc_groups[] = {
+	"intc_irq0",
+	"intc_irq1",
+	"intc_irq2",
+	"intc_irq3",
+	"intc_irq4",
+	"intc_irq5",
+	"intc_irq6",
+	"intc_irq7",
+	"intc_irq8",
+	"intc_irq9",
+};
+
+static const char * const mmc_groups[] = {
+	"mmc_data1",
+	"mmc_data4",
+	"mmc_data8",
+	"mmc_ctrl",
+};
+
+static const char * const msiof0_groups[] = {
+	"msiof0_clk",
+	"msiof0_sync",
+	"msiof0_ss1",
+	"msiof0_ss2",
+	"msiof0_rx",
+	"msiof0_tx",
+};
+
+static const char * const msiof1_groups[] = {
+	"msiof1_clk",
+	"msiof1_sync",
+	"msiof1_ss1",
+	"msiof1_ss2",
+	"msiof1_rx",
+	"msiof1_tx",
+	"msiof1_clk_b",
+	"msiof1_sync_b",
+	"msiof1_ss1_b",
+	"msiof1_ss2_b",
+	"msiof1_rx_b",
+	"msiof1_tx_b",
+};
+
+static const char * const msiof2_groups[] = {
+	"msiof2_clk",
+	"msiof2_sync",
+	"msiof2_ss1",
+	"msiof2_ss2",
+	"msiof2_rx",
+	"msiof2_tx",
+	"msiof2_clk_b",
+	"msiof2_sync_b",
+	"msiof2_ss1_b",
+	"msiof2_ss2_b",
+	"msiof2_rx_b",
+	"msiof2_tx_b",
+};
+
+static const char * const qspi_groups[] = {
+	"qspi_ctrl",
+	"qspi_data2",
+	"qspi_data4",
+};
+
+static const char * const scif0_groups[] = {
+	"scif0_data",
+	"scif0_clk",
+	"scif0_data_b",
+	"scif0_clk_b",
+	"scif0_data_c",
+	"scif0_data_d",
+};
+
+static const char * const scif1_groups[] = {
+	"scif1_data",
+	"scif1_clk",
+	"scif1_data_b",
+	"scif1_clk_b",
+	"scif1_data_c",
+	"scif1_clk_c",
+};
+
+static const char * const scif2_groups[] = {
+	"scif2_data",
+	"scif2_clk",
+	"scif2_data_b",
+	"scif2_clk_b",
+	"scif2_data_c",
+	"scif2_clk_c",
+};
+
+static const char * const scif3_groups[] = {
+	"scif3_data",
+	"scif3_clk",
+	"scif3_data_b",
+	"scif3_clk_b",
+};
+
+static const char * const scif4_groups[] = {
+	"scif4_data",
+	"scif4_data_b",
+	"scif4_data_c",
+	"scif4_data_d",
+	"scif4_data_e",
+};
+
+static const char * const scif5_groups[] = {
+	"scif5_data",
+	"scif5_data_b",
+	"scif5_data_c",
+	"scif5_data_d",
+};
+
+static const char * const scifa0_groups[] = {
+	"scifa0_data",
+	"scifa0_data_b",
+	"scifa0_data_c",
+	"scifa0_data_d",
+};
+
+static const char * const scifa1_groups[] = {
+	"scifa1_data",
+	"scifa1_clk",
+	"scifa1_data_b",
+	"scifa1_clk_b",
+	"scifa1_data_c",
+	"scifa1_clk_c",
+};
+
+static const char * const scifa2_groups[] = {
+	"scifa2_data",
+	"scifa2_clk",
+	"scifa2_data_b",
+	"scifa2_clk_b",
+};
+
+static const char * const scifa3_groups[] = {
+	"scifa3_data",
+	"scifa3_clk",
+	"scifa3_data_b",
+	"scifa3_clk_b",
+};
+
+static const char * const scifa4_groups[] = {
+	"scifa4_data",
+	"scifa4_data_b",
+	"scifa4_data_c",
+	"scifa4_data_d",
+};
+
+static const char * const scifa5_groups[] = {
+	"scifa5_data",
+	"scifa5_data_b",
+	"scifa5_data_c",
+	"scifa5_data_d",
+};
+
+static const char * const scifb0_groups[] = {
+	"scifb0_data",
+	"scifb0_clk",
+	"scifb0_ctrl",
+};
+
+static const char * const scifb1_groups[] = {
+	"scifb1_data",
+	"scifb1_clk",
+};
+
+static const char * const scifb2_groups[] = {
+	"scifb2_data",
+	"scifb2_clk",
+	"scifb2_ctrl",
+};
+
+static const char * const sdhi0_groups[] = {
+	"sdhi0_data1",
+	"sdhi0_data4",
+	"sdhi0_ctrl",
+	"sdhi0_cd",
+	"sdhi0_wp",
+};
+
+static const char * const sdhi1_groups[] = {
+	"sdhi1_data1",
+	"sdhi1_data4",
+	"sdhi1_ctrl",
+	"sdhi1_cd",
+	"sdhi1_wp",
+};
+
+static const char * const sdhi2_groups[] = {
+	"sdhi2_data1",
+	"sdhi2_data4",
+	"sdhi2_ctrl",
+	"sdhi2_cd",
+	"sdhi2_wp",
+};
+
+static const struct sh_pfc_function pinmux_functions[] = {
+	SH_PFC_FUNCTION(eth),
+	SH_PFC_FUNCTION(hscif0),
+	SH_PFC_FUNCTION(hscif1),
+	SH_PFC_FUNCTION(hscif2),
+	SH_PFC_FUNCTION(i2c0),
+	SH_PFC_FUNCTION(i2c1),
+	SH_PFC_FUNCTION(i2c2),
+	SH_PFC_FUNCTION(i2c3),
+	SH_PFC_FUNCTION(i2c4),
+	SH_PFC_FUNCTION(intc),
+	SH_PFC_FUNCTION(mmc),
+	SH_PFC_FUNCTION(msiof0),
+	SH_PFC_FUNCTION(msiof1),
+	SH_PFC_FUNCTION(msiof2),
+	SH_PFC_FUNCTION(qspi),
+	SH_PFC_FUNCTION(scif0),
+	SH_PFC_FUNCTION(scif1),
+	SH_PFC_FUNCTION(scif2),
+	SH_PFC_FUNCTION(scif3),
+	SH_PFC_FUNCTION(scif4),
+	SH_PFC_FUNCTION(scif5),
+	SH_PFC_FUNCTION(scifa0),
+	SH_PFC_FUNCTION(scifa1),
+	SH_PFC_FUNCTION(scifa2),
+	SH_PFC_FUNCTION(scifa3),
+	SH_PFC_FUNCTION(scifa4),
+	SH_PFC_FUNCTION(scifa5),
+	SH_PFC_FUNCTION(scifb0),
+	SH_PFC_FUNCTION(scifb1),
+	SH_PFC_FUNCTION(scifb2),
+	SH_PFC_FUNCTION(sdhi0),
+	SH_PFC_FUNCTION(sdhi1),
+	SH_PFC_FUNCTION(sdhi2),
+};
+
+static const struct pinmux_cfg_reg pinmux_config_regs[] = {
+	{ PINMUX_CFG_REG("GPSR0", 0xE6060004, 32, 1) {
+		GP_0_31_FN, FN_IP2_17_16,
+		GP_0_30_FN, FN_IP2_15_14,
+		GP_0_29_FN, FN_IP2_13_12,
+		GP_0_28_FN, FN_IP2_11_10,
+		GP_0_27_FN, FN_IP2_9_8,
+		GP_0_26_FN, FN_IP2_7_6,
+		GP_0_25_FN, FN_IP2_5_4,
+		GP_0_24_FN, FN_IP2_3_2,
+		GP_0_23_FN, FN_IP2_1_0,
+		GP_0_22_FN, FN_IP1_31_30,
+		GP_0_21_FN, FN_IP1_29_28,
+		GP_0_20_FN, FN_IP1_27,
+		GP_0_19_FN, FN_IP1_26,
+		GP_0_18_FN, FN_A2,
+		GP_0_17_FN, FN_IP1_24,
+		GP_0_16_FN, FN_IP1_23_22,
+		GP_0_15_FN, FN_IP1_21_20,
+		GP_0_14_FN, FN_IP1_19_18,
+		GP_0_13_FN, FN_IP1_17_15,
+		GP_0_12_FN, FN_IP1_14_13,
+		GP_0_11_FN, FN_IP1_12_11,
+		GP_0_10_FN, FN_IP1_10_8,
+		GP_0_9_FN, FN_IP1_7_6,
+		GP_0_8_FN, FN_IP1_5_4,
+		GP_0_7_FN, FN_IP1_3_2,
+		GP_0_6_FN, FN_IP1_1_0,
+		GP_0_5_FN, FN_IP0_31_30,
+		GP_0_4_FN, FN_IP0_29_28,
+		GP_0_3_FN, FN_IP0_27_26,
+		GP_0_2_FN, FN_IP0_25,
+		GP_0_1_FN, FN_IP0_24,
+		GP_0_0_FN, FN_IP0_23_22, }
+	},
+	{ PINMUX_CFG_REG("GPSR1", 0xE6060008, 32, 1) {
+		0, 0,
+		0, 0,
+		0, 0,
+		0, 0,
+		0, 0,
+		0, 0,
+		GP_1_25_FN, FN_DACK0,
+		GP_1_24_FN, FN_IP7_31,
+		GP_1_23_FN, FN_IP4_1_0,
+		GP_1_22_FN, FN_WE1_N,
+		GP_1_21_FN, FN_WE0_N,
+		GP_1_20_FN, FN_IP3_31,
+		GP_1_19_FN, FN_IP3_30,
+		GP_1_18_FN, FN_IP3_29_27,
+		GP_1_17_FN, FN_IP3_26_24,
+		GP_1_16_FN, FN_IP3_23_21,
+		GP_1_15_FN, FN_IP3_20_18,
+		GP_1_14_FN, FN_IP3_17_15,
+		GP_1_13_FN, FN_IP3_14_13,
+		GP_1_12_FN, FN_IP3_12,
+		GP_1_11_FN, FN_IP3_11,
+		GP_1_10_FN, FN_IP3_10,
+		GP_1_9_FN, FN_IP3_9_8,
+		GP_1_8_FN, FN_IP3_7_6,
+		GP_1_7_FN, FN_IP3_5_4,
+		GP_1_6_FN, FN_IP3_3_2,
+		GP_1_5_FN, FN_IP3_1_0,
+		GP_1_4_FN, FN_IP2_31_30,
+		GP_1_3_FN, FN_IP2_29_27,
+		GP_1_2_FN, FN_IP2_26_24,
+		GP_1_1_FN, FN_IP2_23_21,
+		GP_1_0_FN, FN_IP2_20_18, }
+	},
+	{ PINMUX_CFG_REG("GPSR2", 0xE606000C, 32, 1) {
+		GP_2_31_FN, FN_IP6_7_6,
+		GP_2_30_FN, FN_IP6_5_4,
+		GP_2_29_FN, FN_IP6_3_2,
+		GP_2_28_FN, FN_IP6_1_0,
+		GP_2_27_FN, FN_IP5_31_30,
+		GP_2_26_FN, FN_IP5_29_28,
+		GP_2_25_FN, FN_IP5_27_26,
+		GP_2_24_FN, FN_IP5_25_24,
+		GP_2_23_FN, FN_IP5_23_22,
+		GP_2_22_FN, FN_IP5_21_20,
+		GP_2_21_FN, FN_IP5_19_18,
+		GP_2_20_FN, FN_IP5_17_16,
+		GP_2_19_FN, FN_IP5_15_14,
+		GP_2_18_FN, FN_IP5_13_12,
+		GP_2_17_FN, FN_IP5_11_9,
+		GP_2_16_FN, FN_IP5_8_6,
+		GP_2_15_FN, FN_IP5_5_4,
+		GP_2_14_FN, FN_IP5_3_2,
+		GP_2_13_FN, FN_IP5_1_0,
+		GP_2_12_FN, FN_IP4_31_30,
+		GP_2_11_FN, FN_IP4_29_28,
+		GP_2_10_FN, FN_IP4_27_26,
+		GP_2_9_FN, FN_IP4_25_23,
+		GP_2_8_FN, FN_IP4_22_20,
+		GP_2_7_FN, FN_IP4_19_18,
+		GP_2_6_FN, FN_IP4_17_16,
+		GP_2_5_FN, FN_IP4_15_14,
+		GP_2_4_FN, FN_IP4_13_12,
+		GP_2_3_FN, FN_IP4_11_10,
+		GP_2_2_FN, FN_IP4_9_8,
+		GP_2_1_FN, FN_IP4_7_5,
+		GP_2_0_FN, FN_IP4_4_2 }
+	},
+	{ PINMUX_CFG_REG("GPSR3", 0xE6060010, 32, 1) {
+		GP_3_31_FN, FN_IP8_22_20,
+		GP_3_30_FN, FN_IP8_19_17,
+		GP_3_29_FN, FN_IP8_16_15,
+		GP_3_28_FN, FN_IP8_14_12,
+		GP_3_27_FN, FN_IP8_11_9,
+		GP_3_26_FN, FN_IP8_8_6,
+		GP_3_25_FN, FN_IP8_5_3,
+		GP_3_24_FN, FN_IP8_2_0,
+		GP_3_23_FN, FN_IP7_29_27,
+		GP_3_22_FN, FN_IP7_26_24,
+		GP_3_21_FN, FN_IP7_23_21,
+		GP_3_20_FN, FN_IP7_20_18,
+		GP_3_19_FN, FN_IP7_17_15,
+		GP_3_18_FN, FN_IP7_14_12,
+		GP_3_17_FN, FN_IP7_11_9,
+		GP_3_16_FN, FN_IP7_8_6,
+		GP_3_15_FN, FN_IP7_5_3,
+		GP_3_14_FN, FN_IP7_2_0,
+		GP_3_13_FN, FN_IP6_31_29,
+		GP_3_12_FN, FN_IP6_28_26,
+		GP_3_11_FN, FN_IP6_25_23,
+		GP_3_10_FN, FN_IP6_22_20,
+		GP_3_9_FN, FN_IP6_19_17,
+		GP_3_8_FN, FN_IP6_16,
+		GP_3_7_FN, FN_IP6_15,
+		GP_3_6_FN, FN_IP6_14,
+		GP_3_5_FN, FN_IP6_13,
+		GP_3_4_FN, FN_IP6_12,
+		GP_3_3_FN, FN_IP6_11,
+		GP_3_2_FN, FN_IP6_10,
+		GP_3_1_FN, FN_IP6_9,
+		GP_3_0_FN, FN_IP6_8 }
+	},
+	{ PINMUX_CFG_REG("GPSR4", 0xE6060014, 32, 1) {
+		GP_4_31_FN, FN_IP11_17_16,
+		GP_4_30_FN, FN_IP11_15_14,
+		GP_4_29_FN, FN_IP11_13_11,
+		GP_4_28_FN, FN_IP11_10_8,
+		GP_4_27_FN, FN_IP11_7_6,
+		GP_4_26_FN, FN_IP11_5_3,
+		GP_4_25_FN, FN_IP11_2_0,
+		GP_4_24_FN, FN_IP10_31_30,
+		GP_4_23_FN, FN_IP10_29_27,
+		GP_4_22_FN, FN_IP10_26_24,
+		GP_4_21_FN, FN_IP10_23_21,
+		GP_4_20_FN, FN_IP10_20_18,
+		GP_4_19_FN, FN_IP10_17_15,
+		GP_4_18_FN, FN_IP10_14_12,
+		GP_4_17_FN, FN_IP10_11_9,
+		GP_4_16_FN, FN_IP10_8_6,
+		GP_4_15_FN, FN_IP10_5_3,
+		GP_4_14_FN, FN_IP10_2_0,
+		GP_4_13_FN, FN_IP9_30_28,
+		GP_4_12_FN, FN_IP9_27_25,
+		GP_4_11_FN, FN_IP9_24_22,
+		GP_4_10_FN, FN_IP9_21_19,
+		GP_4_9_FN, FN_IP9_18_17,
+		GP_4_8_FN, FN_IP9_16_15,
+		GP_4_7_FN, FN_IP9_14_12,
+		GP_4_6_FN, FN_IP9_11_9,
+		GP_4_5_FN, FN_IP9_8_6,
+		GP_4_4_FN, FN_IP9_5_3,
+		GP_4_3_FN, FN_IP9_2_0,
+		GP_4_2_FN, FN_IP8_31_29,
+		GP_4_1_FN, FN_IP8_28_26,
+		GP_4_0_FN, FN_IP8_25_23 }
+	},
+	{ PINMUX_CFG_REG("GPSR5", 0xE6060018, 32, 1) {
+		0, 0,
+		0, 0,
+		0, 0,
+		0, 0,
+		GP_5_27_FN, FN_USB1_OVC,
+		GP_5_26_FN, FN_USB1_PWEN,
+		GP_5_25_FN, FN_USB0_OVC,
+		GP_5_24_FN, FN_USB0_PWEN,
+		GP_5_23_FN, FN_IP13_26_24,
+		GP_5_22_FN, FN_IP13_23_21,
+		GP_5_21_FN, FN_IP13_20_18,
+		GP_5_20_FN, FN_IP13_17_15,
+		GP_5_19_FN, FN_IP13_14_12,
+		GP_5_18_FN, FN_IP13_11_9,
+		GP_5_17_FN, FN_IP13_8_6,
+		GP_5_16_FN, FN_IP13_5_3,
+		GP_5_15_FN, FN_IP13_2_0,
+		GP_5_14_FN, FN_IP12_29_27,
+		GP_5_13_FN, FN_IP12_26_24,
+		GP_5_12_FN, FN_IP12_23_21,
+		GP_5_11_FN, FN_IP12_20_18,
+		GP_5_10_FN, FN_IP12_17_15,
+		GP_5_9_FN, FN_IP12_14_13,
+		GP_5_8_FN, FN_IP12_12_11,
+		GP_5_7_FN, FN_IP12_10_9,
+		GP_5_6_FN, FN_IP12_8_6,
+		GP_5_5_FN, FN_IP12_5_3,
+		GP_5_4_FN, FN_IP12_2_0,
+		GP_5_3_FN, FN_IP11_29_27,
+		GP_5_2_FN, FN_IP11_26_24,
+		GP_5_1_FN, FN_IP11_23_21,
+		GP_5_0_FN, FN_IP11_20_18 }
+	},
+	{ PINMUX_CFG_REG("GPSR6", 0xE606001C, 32, 1) {
+		0, 0,
+		0, 0,
+		0, 0,
+		0, 0,
+		0, 0,
+		0, 0,
+		GP_6_25_FN, FN_IP0_21_20,
+		GP_6_24_FN, FN_IP0_19_18,
+		GP_6_23_FN, FN_IP0_17,
+		GP_6_22_FN, FN_IP0_16,
+		GP_6_21_FN, FN_IP0_15,
+		GP_6_20_FN, FN_IP0_14,
+		GP_6_19_FN, FN_IP0_13,
+		GP_6_18_FN, FN_IP0_12,
+		GP_6_17_FN, FN_IP0_11,
+		GP_6_16_FN, FN_IP0_10,
+		GP_6_15_FN, FN_IP0_9_8,
+		GP_6_14_FN, FN_IP0_0,
+		GP_6_13_FN, FN_SD1_DATA3,
+		GP_6_12_FN, FN_SD1_DATA2,
+		GP_6_11_FN, FN_SD1_DATA1,
+		GP_6_10_FN, FN_SD1_DATA0,
+		GP_6_9_FN, FN_SD1_CMD,
+		GP_6_8_FN, FN_SD1_CLK,
+		GP_6_7_FN, FN_SD0_WP,
+		GP_6_6_FN, FN_SD0_CD,
+		GP_6_5_FN, FN_SD0_DATA3,
+		GP_6_4_FN, FN_SD0_DATA2,
+		GP_6_3_FN, FN_SD0_DATA1,
+		GP_6_2_FN, FN_SD0_DATA0,
+		GP_6_1_FN, FN_SD0_CMD,
+		GP_6_0_FN, FN_SD0_CLK }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR0", 0xE6060020, 32,
+			     2, 2, 2, 1, 1, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1,
+			     2, 1, 1, 1, 1, 1, 1, 1, 1) {
+		/* IP0_31_30 [2] */
+		FN_D5, FN_SCIF4_RXD_B, FN_I2C0_SCL_D, 0,
+		/* IP0_29_28 [2] */
+		FN_D4, FN_I2C3_SDA_B, FN_SCIF5_TXD_B, 0,
+		/* IP0_27_26 [2] */
+		FN_D3, FN_I2C3_SCL_B, FN_SCIF5_RXD_B, 0,
+		/* IP0_25 [1] */
+		FN_D2, FN_SCIFA3_TXD_B,
+		/* IP0_24 [1] */
+		FN_D1, FN_SCIFA3_RXD_B,
+		/* IP0_23_22 [2] */
+		FN_D0, FN_SCIFA3_SCK_B, FN_IRQ4, 0,
+		/* IP0_21_20 [2] */
+		FN_MMC_D7, FN_SCIF0_TXD, FN_I2C2_SDA_B, FN_CAN1_TX,
+		/* IP0_19_18 [2] */
+		FN_MMC_D6, FN_SCIF0_RXD, FN_I2C2_SCL_B,	FN_CAN1_RX,
+		/* IP0_17 [1] */
+		FN_MMC_D5, FN_SD2_WP,
+		/* IP0_16 [1] */
+		FN_MMC_D4, FN_SD2_CD,
+		/* IP0_15 [1] */
+		FN_MMC_D3, FN_SD2_DATA3,
+		/* IP0_14 [1] */
+		FN_MMC_D2, FN_SD2_DATA2,
+		/* IP0_13 [1] */
+		FN_MMC_D1, FN_SD2_DATA1,
+		/* IP0_12 [1] */
+		FN_MMC_D0, FN_SD2_DATA0,
+		/* IP0_11 [1] */
+		FN_MMC_CMD, FN_SD2_CMD,
+		/* IP0_10 [1] */
+		FN_MMC_CLK, FN_SD2_CLK,
+		/* IP0_9_8 [2] */
+		FN_SD1_WP, FN_IRQ7, FN_CAN0_TX, 0,
+		/* IP0_7 [1] */
+		0, 0,
+		/* IP0_6 [1] */
+		0, 0,
+		/* IP0_5 [1] */
+		0, 0,
+		/* IP0_4 [1] */
+		0, 0,
+		/* IP0_3 [1] */
+		0, 0,
+		/* IP0_2 [1] */
+		0, 0,
+		/* IP0_1 [1] */
+		0, 0,
+		/* IP0_0 [1] */
+		FN_SD1_CD, FN_CAN0_RX, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR1", 0xE6060024, 32,
+			     2, 2, 1, 1, 1, 1, 2, 2, 2, 3, 2, 2, 3, 2, 2,
+			     2, 2) {
+		/* IP1_31_30 [2] */
+		FN_A6, FN_SCIFB0_CTS_N, FN_SCIFA4_RXD_B, FN_TPUTO2_C,
+		/* IP1_29_28 [2] */
+		FN_A5, FN_SCIFB0_RXD, FN_PWM4_B, FN_TPUTO3_C,
+		/* IP1_27 [1] */
+		FN_A4, FN_SCIFB0_TXD,
+		/* IP1_26 [1] */
+		FN_A3, FN_SCIFB0_SCK,
+		/* IP1_25 [1] */
+		0, 0,
+		/* IP1_24 [1] */
+		FN_A1, FN_SCIFB1_TXD,
+		/* IP1_23_22 [2] */
+		FN_A0, FN_SCIFB1_SCK, FN_PWM3_B, 0,
+		/* IP1_21_20 [2] */
+		FN_D15, FN_SCIFA1_TXD, FN_IIC0_SDA_B, 0,
+		/* IP1_19_18 [2] */
+		FN_D14, FN_SCIFA1_RXD, FN_IIC0_SCL_B, 0,
+		/* IP1_17_15 [3] */
+		FN_D13, FN_SCIFA1_SCK, FN_TANS1, FN_PWM2_C, FN_TCLK2_B,
+		0, 0, 0,
+		/* IP1_14_13 [2] */
+		FN_D12, FN_HSCIF2_HRTS_N, FN_SCIF1_TXD_C, FN_I2C1_SDA_D,
+		/* IP1_12_11 [2] */
+		FN_D11, FN_HSCIF2_HCTS_N, FN_SCIF1_RXD_C, FN_I2C1_SCL_D,
+		/* IP1_10_8 [3] */
+		FN_D10, FN_HSCIF2_HSCK, FN_SCIF1_SCK_C, FN_IRQ6, FN_PWM5_C,
+		0, 0, 0,
+		/* IP1_7_6 [2] */
+		FN_D9, FN_HSCIF2_HTX, FN_I2C1_SDA_B, 0,
+		/* IP1_5_4 [2] */
+		FN_D8, FN_HSCIF2_HRX, FN_I2C1_SCL_B, 0,
+		/* IP1_3_2 [2] */
+		FN_D7, FN_IRQ3, FN_TCLK1, FN_PWM6_B,
+		/* IP1_1_0 [2] */
+		FN_D6, FN_SCIF4_TXD_B, FN_I2C0_SDA_D, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR2", 0xE6060028, 32,
+			     2, 3, 3, 3, 3, 2, 2, 2, 2, 2, 2, 2, 2, 2) {
+		/* IP2_31_30 [2] */
+		FN_A20, FN_SPCLK, FN_MOUT1, 0,
+		/* IP2_29_27 [3] */
+		FN_A19, FN_MSIOF2_SS2, FN_PWM4, FN_TPUTO2,
+		FN_MOUT0, 0, 0, 0,
+		/* IP2_26_24 [3] */
+		FN_A18, FN_MSIOF2_SS1, FN_SCIF4_TXD_E, FN_CAN1_TX_B,
+		FN_AVB_AVTP_MATCH_B, 0, 0, 0,
+		/* IP2_23_21 [3] */
+		FN_A17, FN_MSIOF2_SYNC, FN_SCIF4_RXD_E, FN_CAN1_RX_B,
+		FN_AVB_AVTP_CAPTURE_B, 0, 0, 0,
+		/* IP2_20_18 [3] */
+		FN_A16, FN_MSIOF2_SCK, FN_HSCIF0_HSCK_B, FN_SPEEDIN,
+		FN_VSP, FN_CAN_CLK_C, FN_TPUTO2_B, 0,
+		/* IP2_17_16 [2] */
+		FN_A15, FN_MSIOF2_TXD, FN_HSCIF0_HTX_B, FN_DACK1,
+		/* IP2_15_14 [2] */
+		FN_A14, FN_MSIOF2_RXD, FN_HSCIF0_HRX_B, FN_DREQ1_N,
+		/* IP2_13_12 [2] */
+		FN_A13, FN_MSIOF1_SS2, FN_SCIFA5_TXD_B, 0,
+		/* IP2_11_10 [2] */
+		FN_A12, FN_MSIOF1_SS1, FN_SCIFA5_RXD_B, 0,
+		/* IP2_9_8 [2] */
+		FN_A11, FN_MSIOF1_SYNC, FN_IIC1_SDA_B, 0,
+		/* IP2_7_6 [2] */
+		FN_A10, FN_MSIOF1_SCK, FN_IIC1_SCL_B, 0,
+		/* IP2_5_4 [2] */
+		FN_A9, FN_MSIOF1_TXD, FN_SCIFA0_TXD_B, 0,
+		/* IP2_3_2 [2] */
+		FN_A8, FN_MSIOF1_RXD, FN_SCIFA0_RXD_B, 0,
+		/* IP2_1_0 [2] */
+		FN_A7, FN_SCIFB0_RTS_N, FN_SCIFA4_TXD_B, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR3", 0xE606002C, 32,
+			     1, 1, 3, 3, 3, 3, 3, 2, 1, 1, 1, 2, 2, 2, 2, 2) {
+		/* IP3_31 [1] */
+		FN_RD_WR_N, FN_ATAG1_N,
+		/* IP3_30 [1] */
+		FN_RD_N, FN_ATACS11_N,
+		/* IP3_29_27 [3] */
+		FN_BS_N, FN_DRACK0, FN_PWM1_C, FN_TPUTO0_C, FN_ATACS01_N,
+		FN_MTS_N_B, 0, 0,
+		/* IP3_26_24 [3] */
+		FN_EX_CS5_N, FN_SCIFA2_TXD, FN_I2C2_SDA_E, FN_TS_SPSYNC_B,
+		FN_RIF0_D1, FN_FMIN, FN_SCIFB2_RTS_N, FN_STM_N_B,
+		/* IP3_23_21 [3] */
+		FN_EX_CS4_N, FN_SCIFA2_RXD, FN_I2C2_SCL_E, FN_TS_SDEN_B,
+		FN_RIF0_D0, FN_FMCLK, FN_SCIFB2_CTS_N, FN_SCKZ_B,
+		/* IP3_20_18 [3] */
+		FN_EX_CS3_N, FN_SCIFA2_SCK, FN_SCIF4_TXD_C, FN_TS_SCK_B,
+		FN_RIF0_CLK, FN_BPFCLK, FN_SCIFB2_SCK, FN_MDATA_B,
+		/* IP3_17_15 [3] */
+		FN_EX_CS2_N, FN_PWM0, FN_SCIF4_RXD_C, FN_TS_SDATA_B,
+		FN_RIF0_SYNC, FN_TPUTO3, FN_SCIFB2_TXD, FN_SDATA_B,
+		/* IP3_14_13 [2] */
+		FN_EX_CS1_N, FN_TPUTO3_B, FN_SCIFB2_RXD, FN_VI1_DATA11,
+		/* IP3_12 [1] */
+		FN_EX_CS0_N, FN_VI1_DATA10,
+		/* IP3_11 [1] */
+		FN_CS1_N_A26, FN_VI1_DATA9,
+		/* IP3_10 [1] */
+		FN_CS0_N, FN_VI1_DATA8,
+		/* IP3_9_8 [2] */
+		FN_A25, FN_SSL, FN_ATARD1_N, 0,
+		/* IP3_7_6 [2] */
+		FN_A24, FN_IO3, FN_EX_WAIT2, 0,
+		/* IP3_5_4 [2] */
+		FN_A23, FN_IO2, FN_MOUT6, FN_ATAWR1_N,
+		/* IP3_3_2 [2] */
+		FN_A22, FN_MISO_IO1, FN_MOUT5, FN_ATADIR1_N,
+		/* IP3_1_0 [2] */
+		FN_A21, FN_MOSI_IO0, FN_MOUT2, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR4", 0xE6060030, 32,
+			     2, 2, 2, 3, 3, 2, 2, 2, 2, 2, 2, 3, 3, 2) {
+		/* IP4_31_30 [2] */
+		FN_DU0_DG4, FN_LCDOUT12, FN_CC50_STATE12, 0,
+		/* IP4_29_28 [2] */
+		FN_DU0_DG3, FN_LCDOUT11, FN_CC50_STATE11, 0,
+		/* IP4_27_26 [2] */
+		FN_DU0_DG2, FN_LCDOUT10, FN_CC50_STATE10, 0,
+		/* IP4_25_23 [3] */
+		FN_DU0_DG1, FN_LCDOUT9, FN_SCIFA0_TXD_C, FN_I2C3_SDA_D,
+		FN_CC50_STATE9, 0, 0, 0,
+		/* IP4_22_20 [3] */
+		FN_DU0_DG0, FN_LCDOUT8, FN_SCIFA0_RXD_C, FN_I2C3_SCL_D,
+		FN_CC50_STATE8, 0, 0, 0,
+		/* IP4_19_18 [2] */
+		FN_DU0_DR7, FN_LCDOUT23, FN_CC50_STATE7, 0,
+		/* IP4_17_16 [2] */
+		FN_DU0_DR6, FN_LCDOUT22, FN_CC50_STATE6, 0,
+		/* IP4_15_14 [2] */
+		FN_DU0_DR5, FN_LCDOUT21, FN_CC50_STATE5, 0,
+		/* IP4_13_12 [2] */
+		FN_DU0_DR4, FN_LCDOUT20, FN_CC50_STATE4, 0,
+		/* IP4_11_10 [2] */
+		FN_DU0_DR3, FN_LCDOUT19, FN_CC50_STATE3, 0,
+		/* IP4_9_8 [2] */
+		FN_DU0_DR2, FN_LCDOUT18, FN_CC50_STATE2, 0,
+		/* IP4_7_5 [3] */
+		FN_DU0_DR1, FN_LCDOUT17, FN_SCIF5_TXD_C, FN_I2C2_SDA_D,
+		FN_CC50_STATE1, 0, 0, 0,
+		/* IP4_4_2 [3] */
+		FN_DU0_DR0, FN_LCDOUT16, FN_SCIF5_RXD_C, FN_I2C2_SCL_D,
+		FN_CC50_STATE0, 0, 0, 0,
+		/* IP4_1_0 [2] */
+		FN_EX_WAIT0, FN_CAN_CLK_B, FN_SCIF_CLK, FN_PWMFSW0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR5", 0xE6060034, 32,
+			     2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 2, 2, 2) {
+		/* IP5_31_30 [2] */
+		FN_DU0_EXHSYNC_DU0_HSYNC, FN_QSTH_QHS, FN_CC50_STATE27, 0,
+		/* IP5_29_28 [2] */
+		FN_DU0_DOTCLKOUT1, FN_QSTVB_QVE, FN_CC50_STATE26, 0,
+		/* IP5_27_26 [2] */
+		FN_DU0_DOTCLKOUT0, FN_QCLK, FN_CC50_STATE25, 0,
+		/* IP5_25_24 [2] */
+		FN_DU0_DOTCLKIN, FN_QSTVA_QVS, FN_CC50_STATE24, 0,
+		/* IP5_23_22 [2] */
+		FN_DU0_DB7, FN_LCDOUT7, FN_CC50_STATE23, 0,
+		/* IP5_21_20 [2] */
+		FN_DU0_DB6, FN_LCDOUT6, FN_CC50_STATE22, 0,
+		/* IP5_19_18 [2] */
+		FN_DU0_DB5, FN_LCDOUT5, FN_CC50_STATE21, 0,
+		/* IP5_17_16 [2] */
+		FN_DU0_DB4, FN_LCDOUT4, FN_CC50_STATE20, 0,
+		/* IP5_15_14 [2] */
+		FN_DU0_DB3, FN_LCDOUT3, FN_CC50_STATE19, 0,
+		/* IP5_13_12 [2] */
+		FN_DU0_DB2, FN_LCDOUT2, FN_CC50_STATE18, 0,
+		/* IP5_11_9 [3] */
+		FN_DU0_DB1, FN_LCDOUT1, FN_SCIFA4_TXD_C, FN_I2C4_SDA_D,
+		FN_CAN0_TX_C, FN_CC50_STATE17, 0, 0,
+		/* IP5_8_6 [3] */
+		FN_DU0_DB0, FN_LCDOUT0, FN_SCIFA4_RXD_C, FN_I2C4_SCL_D,
+		FN_CAN0_RX_C, FN_CC50_STATE16, 0, 0,
+		/* IP5_5_4 [2] */
+		FN_DU0_DG7, FN_LCDOUT15, FN_CC50_STATE15, 0,
+		/* IP5_3_2 [2] */
+		FN_DU0_DG6, FN_LCDOUT14, FN_CC50_STATE14, 0,
+		/* IP5_1_0 [2] */
+		FN_DU0_DG5, FN_LCDOUT13, FN_CC50_STATE13, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR6", 0xE6060038, 32,
+			     3, 3, 3, 3, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2,
+			     2, 2) {
+		/* IP6_31_29 [3] */
+		FN_ETH_MDIO, FN_VI0_G0, FN_MSIOF2_RXD_B, FN_IIC0_SCL_D,
+		FN_AVB_TX_CLK, FN_ADIDATA, FN_AD_DI, 0,
+		/* IP6_28_26 [3] */
+		FN_VI0_VSYNC_N, FN_SCIF0_TXD_B, FN_I2C0_SDA_C,
+		FN_AUDIO_CLKOUT_B, FN_AVB_TX_EN, 0, 0, 0,
+		/* IP6_25_23 [3] */
+		FN_VI0_HSYNC_N, FN_SCIF0_RXD_B, FN_I2C0_SCL_C, FN_IERX_C,
+		FN_AVB_COL, 0, 0, 0,
+		/* IP6_22_20 [3] */
+		FN_VI0_FIELD, FN_I2C3_SDA, FN_SCIFA5_TXD_C, FN_IECLK_C,
+		FN_AVB_RX_ER, 0, 0, 0,
+		/* IP6_19_17 [3] */
+		FN_VI0_CLKENB, FN_I2C3_SCL, FN_SCIFA5_RXD_C, FN_IETX_C,
+		FN_AVB_RXD7, 0, 0, 0,
+		/* IP6_16 [1] */
+		FN_VI0_DATA7_VI0_B7, FN_AVB_RXD6,
+		/* IP6_15 [1] */
+		FN_VI0_DATA6_VI0_B6, FN_AVB_RXD5,
+		/* IP6_14 [1] */
+		FN_VI0_DATA5_VI0_B5, FN_AVB_RXD4,
+		/* IP6_13 [1] */
+		FN_VI0_DATA4_VI0_B4, FN_AVB_RXD3,
+		/* IP6_12 [1] */
+		FN_VI0_DATA3_VI0_B3, FN_AVB_RXD2,
+		/* IP6_11 [1] */
+		FN_VI0_DATA2_VI0_B2, FN_AVB_RXD1,
+		/* IP6_10 [1] */
+		FN_VI0_DATA1_VI0_B1, FN_AVB_RXD0,
+		/* IP6_9 [1] */
+		FN_VI0_DATA0_VI0_B0, FN_AVB_RX_DV,
+		/* IP6_8 [1] */
+		FN_VI0_CLK, FN_AVB_RX_CLK,
+		/* IP6_7_6 [2] */
+		FN_DU0_CDE, FN_QPOLB, FN_CC50_STATE31, 0,
+		/* IP6_5_4 [2] */
+		FN_DU0_DISP, FN_QPOLA, FN_CC50_STATE30, 0,
+		/* IP6_3_2 [2] */
+		FN_DU0_EXODDF_DU0_ODDF_DISP_CDE, FN_QCPV_QDE, FN_CC50_STATE29,
+		/* IP6_1_0 [2] */
+		FN_DU0_EXVSYNC_DU0_VSYNC, FN_QSTB_QHE, FN_CC50_STATE28, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR7", 0xE606003C, 32,
+			     1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3) {
+		/* IP7_31 [1] */
+		FN_DREQ0_N, FN_SCIFB1_RXD,
+		/* IP7_30 [1] */
+		0, 0,
+		/* IP7_29_27 [3] */
+		FN_ETH_TXD0, FN_VI0_R2, FN_SCIF3_RXD_B, FN_I2C4_SCL_E,
+		FN_AVB_GTX_CLK, FN_SSI_WS6_B, 0, 0,
+		/* IP7_26_24 [3] */
+		FN_ETH_MAGIC, FN_VI0_R1, FN_SCIF3_SCK_B, FN_AVB_TX_ER,
+		FN_SSI_SCK6_B, 0, 0, 0,
+		/* IP7_23_21 [3] */
+		FN_ETH_TX_EN, FN_VI0_R0, FN_SCIF2_TXD_C, FN_IIC1_SDA_D,
+		FN_AVB_TXD7, FN_SSI_SDATA5_B, 0, 0,
+		/* IP7_20_18 [3] */
+		FN_ETH_TXD1, FN_VI0_G7, FN_SCIF2_RXD_C, FN_IIC1_SCL_D,
+		FN_AVB_TXD6, FN_SSI_WS5_B, 0, 0,
+		/* IP7_17_15 [3] */
+		FN_ETH_REFCLK, FN_VI0_G6, FN_SCIF2_SCK_C, FN_AVB_TXD5,
+		FN_SSI_SCK5_B, 0, 0, 0,
+		/* IP7_14_12 [3] */
+		FN_ETH_LINK, FN_VI0_G5, FN_MSIOF2_SS2_B, FN_SCIF4_TXD_D,
+		FN_AVB_TXD4, FN_ADICHS2, 0, 0,
+		/* IP7_11_9 [3] */
+		FN_ETH_RXD1, FN_VI0_G4, FN_MSIOF2_SS1_B, FN_SCIF4_RXD_D,
+		FN_AVB_TXD3, FN_ADICHS1, 0, 0,
+		/* IP7_8_6 [3] */
+		FN_ETH_RXD0, FN_VI0_G3, FN_MSIOF2_SYNC_B, FN_CAN0_TX_B,
+		FN_AVB_TXD2, FN_ADICHS0, FN_AD_NCS_N, 0,
+		/* IP7_5_3 [3] */
+		FN_ETH_RX_ER, FN_VI0_G2, FN_MSIOF2_SCK_B, FN_CAN0_RX_B,
+		FN_AVB_TXD1, FN_ADICLK, FN_AD_CLK, 0,
+		/* IP7_2_0 [3] */
+		FN_ETH_CRS_DV, FN_VI0_G1, FN_MSIOF2_TXD_B, FN_IIC0_SDA_D,
+		FN_AVB_TXD0, FN_ADICS_SAMP, FN_AD_DO, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR8", 0xE6060040, 32,
+			     3, 3, 3, 3, 3, 2, 3, 3, 3, 3, 3) {
+		/* IP8_31_29 [3] */
+		FN_MSIOF0_RXD, FN_SCIF5_RXD, FN_I2C2_SCL_C, FN_DU1_DR2,
+		FN_RIF1_D0_B, FN_TS_SDEN_D, FN_FMCLK_C, FN_RDS_CLK,
+		/* IP8_28_26 [3] */
+		FN_I2C1_SDA, FN_SCIF4_TXD, FN_IRQ5, FN_DU1_DR1,
+		FN_RIF1_CLK_B, FN_TS_SCK_D, FN_BPFCLK_C, 0,
+		/* IP8_25_23 [3] */
+		FN_I2C1_SCL, FN_SCIF4_RXD, FN_PWM5_B, FN_DU1_DR0,
+		FN_RIF1_SYNC_B, FN_TS_SDATA_D, FN_TPUTO1_B, 0,
+		/* IP8_22_20 [3] */
+		FN_I2C0_SDA, FN_SCIF0_TXD_C, FN_TPUTO0, FN_CAN_CLK,
+		FN_DVC_MUTE, FN_CAN1_TX_D, 0, 0,
+		/* IP8_19_17 [3] */
+		FN_I2C0_SCL, FN_SCIF0_RXD_C, FN_PWM5, FN_TCLK1_B,
+		FN_AVB_GTXREFCLK, FN_CAN1_RX_D, FN_TPUTO0_B, 0,
+		/* IP8_16_15 [2] */
+		FN_HSCIF0_HSCK, FN_SCIF_CLK_B, FN_AVB_CRS, FN_AUDIO_CLKC_B,
+		/* IP8_14_12 [3] */
+		FN_HSCIF0_HRTS_N, FN_VI0_R7, FN_SCIF0_TXD_D, FN_I2C0_SDA_E,
+		FN_AVB_PHY_INT, FN_SSI_SDATA8_B, 0, 0,
+		/* IP8_11_9 [3] */
+		FN_HSCIF0_HCTS_N, FN_VI0_R6, FN_SCIF0_RXD_D, FN_I2C0_SCL_E,
+		FN_AVB_MAGIC, FN_SSI_SDATA7_B, 0, 0,
+		/* IP8_8_6 [3] */
+		FN_HSCIF0_HTX, FN_VI0_R5, FN_I2C1_SDA_C, FN_AUDIO_CLKB_B,
+		FN_AVB_LINK, FN_SSI_WS78_B, 0, 0,
+		/* IP8_5_3 [3] */
+		FN_HSCIF0_HRX, FN_VI0_R4, FN_I2C1_SCL_C, FN_AUDIO_CLKA_B,
+		FN_AVB_MDIO, FN_SSI_SCK78_B, 0, 0,
+		/* IP8_2_0 [3] */
+		FN_ETH_MDC, FN_VI0_R3, FN_SCIF3_TXD_B, FN_I2C4_SDA_E,
+		FN_AVB_MDC, FN_SSI_SDATA6_B, 0, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR9", 0xE6060044, 32,
+			     1, 3, 3, 3, 3, 2, 2, 3, 3, 3, 3, 3, 3) {
+		/* IP9_31 [1] */
+		0, 0,
+		/* IP9_30_28 [3] */
+		FN_SCIF1_SCK, FN_PWM3, FN_TCLK2, FN_DU1_DG5,
+		FN_SSI_SDATA1_B, FN_CAN_TXCLK, FN_CC50_STATE34, 0,
+		/* IP9_27_25 [3] */
+		FN_HSCIF1_HRTS_N, FN_SCIFA4_TXD, FN_IERX, FN_DU1_DG4,
+		FN_SSI_WS1_B, FN_CAN_STEP0, FN_CC50_STATE33, 0,
+		/* IP9_24_22 [3] */
+		FN_HSCIF1_HCTS_N, FN_SCIFA4_RXD, FN_IECLK, FN_DU1_DG3,
+		FN_SSI_SCK1_B, FN_CAN_DEBUG_HW_TRIGGER, FN_CC50_STATE32, 0,
+		/* IP9_21_19 [3] */
+		FN_HSCIF1_HSCK, FN_PWM2, FN_IETX, FN_DU1_DG2,
+		FN_REMOCON_B, FN_SPEEDIN_B, FN_VSP_B, 0,
+		/* IP9_18_17 [2] */
+		FN_HSCIF1_HTX, FN_I2C4_SDA, FN_TPUTO1, FN_DU1_DG1,
+		/* IP9_16_15 [2] */
+		FN_HSCIF1_HRX, FN_I2C4_SCL, FN_PWM6, FN_DU1_DG0,
+		/* IP9_14_12 [3] */
+		FN_MSIOF0_SS2, FN_SCIFA0_TXD, FN_TS_SPSYNC, FN_DU1_DR7,
+		FN_RIF1_D1, FN_FMIN_B, FN_RDS_DATA_B, 0,
+		/* IP9_11_9 [3] */
+		FN_MSIOF0_SS1, FN_SCIFA0_RXD, FN_TS_SDEN, FN_DU1_DR6,
+		FN_RIF1_D0, FN_FMCLK_B, FN_RDS_CLK_B, 0,
+		/* IP9_8_6 [3] */
+		FN_MSIOF0_SYNC, FN_PWM1, FN_TS_SCK, FN_DU1_DR5,
+		FN_RIF1_CLK, FN_BPFCLK_B, 0, 0,
+		/* IP9_5_3 [3] */
+		FN_MSIOF0_SCK, FN_IRQ0, FN_TS_SDATA, FN_DU1_DR4,
+		FN_RIF1_SYNC, FN_TPUTO1_C, 0, 0,
+		/* IP9_2_0 [3] */
+		FN_MSIOF0_TXD, FN_SCIF5_TXD, FN_I2C2_SDA_C, FN_DU1_DR3,
+		FN_RIF1_D1_B, FN_TS_SPSYNC_D, FN_FMIN_C, FN_RDS_DATA, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR10", 0xE6060048, 32,
+			     2, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3) {
+		/* IP10_31_30 [2] */
+		FN_SSI_SCK5, FN_SCIFA3_SCK, FN_DU1_DOTCLKIN, FN_CAN_DEBUGOUT10,
+		/* IP10_29_27 [3] */
+		FN_I2C2_SDA, FN_SCIFA5_TXD, FN_DU1_DB7, FN_AUDIO_CLKOUT_C,
+		FN_CAN_DEBUGOUT9, 0, 0, 0,
+		/* IP10_26_24 [3] */
+		FN_I2C2_SCL, FN_SCIFA5_RXD, FN_DU1_DB6, FN_AUDIO_CLKC_C,
+		FN_SSI_SDATA4_B, FN_CAN_DEBUGOUT8, 0, 0,
+		/* IP10_23_21 [3] */
+		FN_SCIF3_TXD, FN_I2C1_SDA_E, FN_FMIN_D, FN_DU1_DB5,
+		FN_AUDIO_CLKB_C, FN_SSI_WS4_B, FN_CAN_DEBUGOUT7, FN_RDS_DATA_C,
+		/* IP10_20_18 [3] */
+		FN_SCIF3_RXD, FN_I2C1_SCL_E, FN_FMCLK_D, FN_DU1_DB4,
+		FN_AUDIO_CLKA_C, FN_SSI_SCK4_B, FN_CAN_DEBUGOUT6, FN_RDS_CLK_C,
+		/* IP10_17_15 [3] */
+		FN_SCIF3_SCK, FN_IRQ2, FN_BPFCLK_D, FN_DU1_DB3,
+		FN_SSI_SDATA9_B, FN_TANS2, FN_CAN_DEBUGOUT5, FN_CC50_OSCOUT,
+		/* IP10_14_12 [3] */
+		FN_SCIF2_SCK, FN_IRQ1, FN_DU1_DB2, FN_SSI_WS9_B,
+		FN_USB0_IDIN, FN_CAN_DEBUGOUT4, FN_CC50_STATE39, 0,
+		/* IP10_11_9 [3] */
+		FN_SCIF2_TXD, FN_IIC1_SDA, FN_DU1_DB1, FN_SSI_SCK9_B,
+		FN_USB0_OVC1, FN_CAN_DEBUGOUT3, FN_CC50_STATE38, 0,
+		/* IP10_8_6 [3] */
+		FN_SCIF2_RXD, FN_IIC1_SCL, FN_DU1_DB0, FN_SSI_SDATA2_B,
+		FN_USB0_EXTLP, FN_CAN_DEBUGOUT2, FN_CC50_STATE37, 0,
+		/* IP10_5_3 [3] */
+		FN_SCIF1_TXD, FN_IIC0_SDA, FN_DU1_DG7, FN_SSI_WS2_B,
+		FN_CAN_DEBUGOUT1, FN_CC50_STATE36, 0, 0,
+		/* IP10_2_0 [3] */
+		FN_SCIF1_RXD, FN_IIC0_SCL, FN_DU1_DG6, FN_SSI_SCK2_B,
+		FN_CAN_DEBUGOUT0, FN_CC50_STATE35, 0, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR11", 0xE606004C, 32,
+			     2, 3, 3, 3, 3, 2, 2, 3, 3, 2, 3, 3) {
+		/* IP11_31_30 [2] */
+		0, 0, 0, 0,
+		/* IP11_29_27 [3] */
+		FN_SSI_SDATA0, FN_MSIOF1_SCK_B, FN_PWM0_B, FN_ADICLK_B,
+		FN_AD_CLK_B, 0, 0, 0,
+		/* IP11_26_24 [3] */
+		FN_SSI_WS0129, FN_MSIOF1_TXD_B, FN_SCIF5_TXD_D, FN_ADICS_SAMP_B,
+		FN_AD_DO_B, 0, 0, 0,
+		/* IP11_23_21 [3] */
+		FN_SSI_SCK0129, FN_MSIOF1_RXD_B, FN_SCIF5_RXD_D, FN_ADIDATA_B,
+		FN_AD_DI_B, FN_PCMWE_N, 0, 0,
+		/* IP11_20_18 [3] */
+		FN_SSI_SDATA7, FN_SCIFA2_TXD_B, FN_IRQ8, FN_AUDIO_CLKA_D,
+		FN_CAN_CLK_D, FN_PCMOE_N, 0, 0,
+		/* IP11_17_16 [2] */
+		FN_SSI_WS78, FN_SCIFA2_RXD_B, FN_IIC0_SCL_C, FN_DU1_CDE,
+		/* IP11_15_14 [2] */
+		FN_SSI_SCK78, FN_SCIFA2_SCK_B, FN_IIC0_SDA_C, FN_DU1_DISP,
+		/* IP11_13_11 [3] */
+		FN_SSI_SDATA6, FN_SCIFA1_TXD_B, FN_I2C4_SDA_C,
+		FN_DU1_EXODDF_DU1_ODDF_DISP_CDE, FN_CAN_DEBUGOUT15, 0, 0, 0,
+		/* IP11_10_8 [3] */
+		FN_SSI_WS6, FN_SCIFA1_RXD_B, FN_I2C4_SCL_C,
+		FN_DU1_EXVSYNC_DU1_VSYNC, FN_CAN_DEBUGOUT14, 0, 0, 0,
+		/* IP11_7_6 [2] */
+		FN_SSI_SCK6, FN_SCIFA1_SCK_B, FN_DU1_EXHSYNC_DU1_HSYNC,
+		FN_CAN_DEBUGOUT13,
+		/* IP11_5_3 [3] */
+		FN_SSI_SDATA5, FN_SCIFA3_TXD, FN_I2C3_SDA_C, FN_DU1_DOTCLKOUT1,
+		FN_CAN_DEBUGOUT12, 0, 0, 0,
+		/* IP11_2_0 [3] */
+		FN_SSI_WS5, FN_SCIFA3_RXD, FN_I2C3_SCL_C, FN_DU1_DOTCLKOUT0,
+		FN_CAN_DEBUGOUT11, 0, 0, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR12", 0xE6060050, 32,
+			     2, 3, 3, 3, 3, 3, 2, 2, 2, 3, 3, 3) {
+		/* IP12_31_30 [2] */
+		0, 0, 0, 0,
+		/* IP12_29_27 [3] */
+		FN_SSI_SCK2, FN_HSCIF1_HTX_B, FN_VI1_DATA2, FN_MDATA,
+		FN_ATAWR0_N, FN_ETH_RXD1_B, 0, 0,
+		/* IP12_26_24 [3] */
+		FN_SSI_SDATA1, FN_HSCIF1_HRX_B, FN_VI1_DATA1, FN_SDATA,
+		FN_ATAG0_N, FN_ETH_RXD0_B, 0, 0,
+		/* IP12_23_21 [3] */
+		FN_SSI_WS1, FN_SCIF1_TXD_B, FN_IIC1_SDA_C, FN_VI1_DATA0,
+		FN_CAN0_TX_D, FN_AVB_AVTP_MATCH, FN_ETH_RX_ER_B, 0,
+		/* IP12_20_18 [3] */
+		FN_SSI_SCK1, FN_SCIF1_RXD_B, FN_IIC1_SCL_C, FN_VI1_CLK,
+		FN_CAN0_RX_D, FN_AVB_AVTP_CAPTURE, FN_ETH_CRS_DV_B, 0,
+		/* IP12_17_15 [3] */
+		FN_SSI_SDATA8, FN_SCIF1_SCK_B, FN_PWM1_B, FN_IRQ9,
+		FN_REMOCON, FN_DACK2, FN_ETH_MDIO_B, 0,
+		/* IP12_14_13 [2] */
+		FN_SSI_SDATA4, FN_MLB_DAT, FN_IERX_B, FN_IRD_SCK,
+		/* IP12_12_11 [2] */
+		FN_SSI_WS4, FN_MLB_SIG, FN_IECLK_B, FN_IRD_RX,
+		/* IP12_10_9 [2] */
+		FN_SSI_SCK4, FN_MLB_CLK, FN_IETX_B, FN_IRD_TX,
+		/* IP12_8_6 [3] */
+		FN_SSI_SDATA3, FN_MSIOF1_SS2_B, FN_SCIFA1_TXD_C, FN_ADICHS2_B,
+		FN_CAN1_TX_C, FN_DREQ2_N, 0, 0,
+		/* IP12_5_3 [3] */
+		FN_SSI_WS34, FN_MSIOF1_SS1_B, FN_SCIFA1_RXD_C, FN_ADICHS1_B,
+		FN_CAN1_RX_C, FN_DACK1_B, 0, 0,
+		/* IP12_2_0 [3] */
+		FN_SSI_SCK34, FN_MSIOF1_SYNC_B, FN_SCIFA1_SCK_C, FN_ADICHS0_B,
+		FN_AD_NCS_N_B, FN_DREQ1_N_B, 0, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("IPSR13", 0xE6060054, 32,
+			     1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3) {
+		/* IP13_31 [1] */
+		0, 0,
+		/* IP13_30 [1] */
+		0, 0,
+		/* IP13_29 [1] */
+		0, 0,
+		/* IP13_28 [1] */
+		0, 0,
+		/* IP13_27 [1] */
+		0, 0,
+		/* IP13_26_24 [3] */
+		FN_AUDIO_CLKOUT, FN_I2C4_SDA_B, FN_SCIFA5_TXD_D, FN_VI1_VSYNC_N,
+		FN_TS_SPSYNC_C, FN_RIF0_D1_B, FN_FMIN_E, FN_RDS_DATA_D,
+		/* IP13_23_21 [3] */
+		FN_AUDIO_CLKC, FN_I2C4_SCL_B, FN_SCIFA5_RXD_D, FN_VI1_HSYNC_N,
+		FN_TS_SDEN_C, FN_RIF0_D0_B, FN_FMCLK_E, FN_RDS_CLK_D,
+		/* IP13_20_18 [3] */
+		FN_AUDIO_CLKB, FN_I2C0_SDA_B, FN_SCIFA4_TXD_D, FN_VI1_FIELD,
+		FN_TS_SCK_C, FN_RIF0_CLK_B, FN_BPFCLK_E, FN_ETH_MDC_B,
+		/* IP13_17_15 [3] */
+		FN_AUDIO_CLKA, FN_I2C0_SCL_B, FN_SCIFA4_RXD_D, FN_VI1_CLKENB,
+		FN_TS_SDATA_C, FN_RIF0_SYNC_B, FN_ETH_TXD0_B, 0,
+		/* IP13_14_12 [3] */
+		FN_SSI_SDATA9, FN_SCIF2_TXD_B, FN_I2C3_SDA_E, FN_VI1_DATA7,
+		FN_ATADIR0_N, FN_ETH_MAGIC_B, 0, 0,
+		/* IP13_11_9 [3] */
+		FN_SSI_WS9, FN_SCIF2_RXD_B, FN_I2C3_SCL_E, FN_VI1_DATA6,
+		FN_ATARD0_N, FN_ETH_TX_EN_B, 0, 0,
+		/* IP13_8_6 [3] */
+		FN_SSI_SCK9, FN_SCIF2_SCK_B, FN_PWM2_B, FN_VI1_DATA5,
+		FN_MTS_N, FN_EX_WAIT1, FN_ETH_TXD1_B, 0,
+		/* IP13_5_3 [2] */
+		FN_SSI_SDATA2, FN_HSCIF1_HRTS_N_B, FN_SCIFA0_TXD_D,
+		FN_VI1_DATA4, FN_STM_N, FN_ATACS10_N, FN_ETH_REFCLK_B, 0,
+		/* IP13_2_0 [3] */
+		FN_SSI_WS2, FN_HSCIF1_HCTS_N_B, FN_SCIFA0_RXD_D, FN_VI1_DATA3,
+		FN_SCKZ, FN_ATACS00_N, FN_ETH_LINK_B, 0, }
+	},
+	{ PINMUX_CFG_REG_VAR("MOD_SEL", 0xE6060090, 32,
+			     2, 1, 2, 3, 1, 1, 1, 1, 1, 1, 3, 3, 3, 3, 3,
+			     2, 1) {
+		/* SEL_ADG [2] */
+		FN_SEL_ADG_0, FN_SEL_ADG_1, FN_SEL_ADG_2, FN_SEL_ADG_3,
+		/* SEL_ADI [1] */
+		FN_SEL_ADI_0, FN_SEL_ADI_1,
+		/* SEL_CAN [2] */
+		FN_SEL_CAN_0, FN_SEL_CAN_1, FN_SEL_CAN_2, FN_SEL_CAN_3,
+		/* SEL_DARC [3] */
+		FN_SEL_DARC_0, FN_SEL_DARC_1, FN_SEL_DARC_2, FN_SEL_DARC_3,
+		FN_SEL_DARC_4, 0, 0, 0,
+		/* SEL_DR0 [1] */
+		FN_SEL_DR0_0, FN_SEL_DR0_1,
+		/* SEL_DR1 [1] */
+		FN_SEL_DR1_0, FN_SEL_DR1_1,
+		/* SEL_DR2 [1] */
+		FN_SEL_DR2_0, FN_SEL_DR2_1,
+		/* SEL_DR3 [1] */
+		FN_SEL_DR3_0, FN_SEL_DR3_1,
+		/* SEL_ETH [1] */
+		FN_SEL_ETH_0, FN_SEL_ETH_1,
+		/* SLE_FSN [1] */
+		FN_SEL_FSN_0, FN_SEL_FSN_1,
+		/* SEL_IC200 [3] */
+		FN_SEL_I2C00_0, FN_SEL_I2C00_1, FN_SEL_I2C00_2, FN_SEL_I2C00_3,
+		FN_SEL_I2C00_4, 0, 0, 0,
+		/* SEL_I2C01 [3] */
+		FN_SEL_I2C01_0, FN_SEL_I2C01_1, FN_SEL_I2C01_2, FN_SEL_I2C01_3,
+		FN_SEL_I2C01_4, 0, 0, 0,
+		/* SEL_I2C02 [3] */
+		FN_SEL_I2C02_0, FN_SEL_I2C02_1, FN_SEL_I2C02_2, FN_SEL_I2C02_3,
+		FN_SEL_I2C02_4, 0, 0, 0,
+		/* SEL_I2C03 [3] */
+		FN_SEL_I2C03_0, FN_SEL_I2C03_1, FN_SEL_I2C03_2, FN_SEL_I2C03_3,
+		FN_SEL_I2C03_4, 0, 0, 0,
+		/* SEL_I2C04 [3] */
+		FN_SEL_I2C04_0, FN_SEL_I2C04_1, FN_SEL_I2C04_2, FN_SEL_I2C04_3,
+		FN_SEL_I2C04_4, 0, 0, 0,
+		/* SEL_IIC00 [2] */
+		FN_SEL_IIC00_0, FN_SEL_IIC00_1, FN_SEL_IIC00_2, FN_SEL_IIC00_3,
+		/* SEL_AVB [1] */
+		FN_SEL_AVB_0, FN_SEL_AVB_1, }
+	},
+	{ PINMUX_CFG_REG_VAR("MOD_SEL2", 0xE6060094, 32,
+			     2, 2, 1, 1, 1, 1, 1, 1, 2, 2, 1, 1, 2, 2, 1, 1,
+			     2, 2, 2, 1, 1, 2) {
+		/* SEL_IEB [2] */
+		FN_SEL_IEB_0, FN_SEL_IEB_1, FN_SEL_IEB_2, 0,
+		/* SEL_IIC0 [2] */
+		FN_SEL_IIC01_0, FN_SEL_IIC01_1, FN_SEL_IIC01_2, FN_SEL_IIC01_3,
+		/* SEL_LBS [1] */
+		FN_SEL_LBS_0, FN_SEL_LBS_1,
+		/* SEL_MSI1 [1] */
+		FN_SEL_MSI1_0, FN_SEL_MSI1_1,
+		/* SEL_MSI2 [1] */
+		FN_SEL_MSI2_0, FN_SEL_MSI2_1,
+		/* SEL_RAD [1] */
+		FN_SEL_RAD_0, FN_SEL_RAD_1,
+		/* SEL_RCN [1] */
+		FN_SEL_RCN_0, FN_SEL_RCN_1,
+		/* SEL_RSP [1] */
+		FN_SEL_RSP_0, FN_SEL_RSP_1,
+		/* SEL_SCIFA0 [2] */
+		FN_SEL_SCIFA0_0, FN_SEL_SCIFA0_1, FN_SEL_SCIFA0_2,
+		FN_SEL_SCIFA0_3,
+		/* SEL_SCIFA1 [2] */
+		FN_SEL_SCIFA1_0, FN_SEL_SCIFA1_1, FN_SEL_SCIFA1_2, 0,
+		/* SEL_SCIFA2 [1] */
+		FN_SEL_SCIFA2_0, FN_SEL_SCIFA2_1,
+		/* SEL_SCIFA3 [1] */
+		FN_SEL_SCIFA3_0, FN_SEL_SCIFA3_1,
+		/* SEL_SCIFA4 [2] */
+		FN_SEL_SCIFA4_0, FN_SEL_SCIFA4_1, FN_SEL_SCIFA4_2,
+		FN_SEL_SCIFA4_3,
+		/* SEL_SCIFA5 [2] */
+		FN_SEL_SCIFA5_0, FN_SEL_SCIFA5_1, FN_SEL_SCIFA5_2,
+		FN_SEL_SCIFA5_3,
+		/* SEL_SPDM [1] */
+		FN_SEL_SPDM_0, FN_SEL_SPDM_1,
+		/* SEL_TMU [1] */
+		FN_SEL_TMU_0, FN_SEL_TMU_1,
+		/* SEL_TSIF0 [2] */
+		FN_SEL_TSIF0_0, FN_SEL_TSIF0_1, FN_SEL_TSIF0_2, FN_SEL_TSIF0_3,
+		/* SEL_CAN0 [2] */
+		FN_SEL_CAN0_0, FN_SEL_CAN0_1, FN_SEL_CAN0_2, FN_SEL_CAN0_3,
+		/* SEL_CAN1 [2] */
+		FN_SEL_CAN1_0, FN_SEL_CAN1_1, FN_SEL_CAN1_2, FN_SEL_CAN1_3,
+		/* SEL_HSCIF0 [1] */
+		FN_SEL_HSCIF0_0, FN_SEL_HSCIF0_1,
+		/* SEL_HSCIF1 [1] */
+		FN_SEL_HSCIF1_0, FN_SEL_HSCIF1_1,
+		/* SEL_RDS [2] */
+		FN_SEL_RDS_0, FN_SEL_RDS_1, FN_SEL_RDS_2, FN_SEL_RDS_3, }
+	},
+	{ PINMUX_CFG_REG_VAR("MOD_SEL3", 0xE6060098, 32,
+			     2, 2, 2, 1, 3, 2, 1, 1, 1, 1, 1, 1, 1, 1,
+			     1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1) {
+		/* SEL_SCIF0 [2] */
+		FN_SEL_SCIF0_0, FN_SEL_SCIF0_1, FN_SEL_SCIF0_2, FN_SEL_SCIF0_3,
+		/* SEL_SCIF1 [2] */
+		FN_SEL_SCIF1_0, FN_SEL_SCIF1_1, FN_SEL_SCIF1_2, 0,
+		/* SEL_SCIF2 [2] */
+		FN_SEL_SCIF2_0, FN_SEL_SCIF2_1, FN_SEL_SCIF2_2, 0,
+		/* SEL_SCIF3 [1] */
+		FN_SEL_SCIF3_0, FN_SEL_SCIF3_1,
+		/* SEL_SCIF4 [3] */
+		FN_SEL_SCIF4_0, FN_SEL_SCIF4_1, FN_SEL_SCIF4_2, FN_SEL_SCIF4_3,
+		FN_SEL_SCIF4_4, 0, 0, 0,
+		/* SEL_SCIF5 [2] */
+		FN_SEL_SCIF5_0, FN_SEL_SCIF5_1, FN_SEL_SCIF5_2, FN_SEL_SCIF5_3,
+		/* SEL_SSI1 [1] */
+		FN_SEL_SSI1_0, FN_SEL_SSI1_1,
+		/* SEL_SSI2 [1] */
+		FN_SEL_SSI2_0, FN_SEL_SSI2_1,
+		/* SEL_SSI4 [1] */
+		FN_SEL_SSI4_0, FN_SEL_SSI4_1,
+		/* SEL_SSI5 [1] */
+		FN_SEL_SSI5_0, FN_SEL_SSI5_1,
+		/* SEL_SSI6 [1] */
+		FN_SEL_SSI6_0, FN_SEL_SSI6_1,
+		/* SEL_SSI7 [1] */
+		FN_SEL_SSI7_0, FN_SEL_SSI7_1,
+		/* SEL_SSI8 [1] */
+		FN_SEL_SSI8_0, FN_SEL_SSI8_1,
+		/* SEL_SSI9 [1] */
+		FN_SEL_SSI9_0, FN_SEL_SSI9_1,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0,
+		/* RESERVED [1] */
+		0, 0, }
+	},
+	{ },
+};
+
+const struct sh_pfc_soc_info r8a7794_pinmux_info = {
+	.name = "r8a77940_pfc",
+	.unlock_reg = 0xe6060000, /* PMMR */
+
+	.function = { PINMUX_FUNCTION_BEGIN, PINMUX_FUNCTION_END },
+
+	.pins = pinmux_pins,
+	.nr_pins = ARRAY_SIZE(pinmux_pins),
+	.groups = pinmux_groups,
+	.nr_groups = ARRAY_SIZE(pinmux_groups),
+	.functions = pinmux_functions,
+	.nr_functions = ARRAY_SIZE(pinmux_functions),
+
+	.cfg_regs = pinmux_config_regs,
+
+	.gpio_data = pinmux_data,
+	.gpio_data_size = ARRAY_SIZE(pinmux_data),
+};
diff --git a/drivers/pinctrl/sh-pfc/pinctrl.c b/drivers/pinctrl/sh-pfc/pinctrl.c
index 072e7c62cab7..ff678966008b 100644
--- a/drivers/pinctrl/sh-pfc/pinctrl.c
+++ b/drivers/pinctrl/sh-pfc/pinctrl.c
@@ -625,8 +625,8 @@ int sh_pfc_register_pinctrl(struct sh_pfc *pfc)
 	pmx->pctl_desc.npins = pfc->info->nr_pins;
 
 	pmx->pctl = pinctrl_register(&pmx->pctl_desc, pfc->dev, pmx);
-	if (pmx->pctl == NULL)
-		return -EINVAL;
+	if (IS_ERR(pmx->pctl))
+		return PTR_ERR(pmx->pctl);
 
 	return 0;
 }
diff --git a/drivers/pinctrl/sirf/Makefile b/drivers/pinctrl/sirf/Makefile
index 3ffc475ce40c..fd58e0bacb2a 100644
--- a/drivers/pinctrl/sirf/Makefile
+++ b/drivers/pinctrl/sirf/Makefile
@@ -3,3 +3,4 @@
 obj-y	+= pinctrl-sirf.o
 obj-y	+= pinctrl-prima2.o
 obj-y	+= pinctrl-atlas6.o
+obj-y	+= pinctrl-atlas7.o
diff --git a/drivers/pinctrl/sirf/pinctrl-atlas7.c b/drivers/pinctrl/sirf/pinctrl-atlas7.c
new file mode 100644
index 000000000000..9384e0aa3996
--- /dev/null
+++ b/drivers/pinctrl/sirf/pinctrl-atlas7.c
@@ -0,0 +1,4637 @@
+/*
+ * pinctrl pads, groups, functions for CSR SiRFatlasVII
+ *
+ * Copyright (c) 2011 - 2014 Cambridge Silicon Radio Limited, a CSR plc group
+ * company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/bitops.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <linux/pinctrl/machine.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/gpio.h>
+
+/* Definition of Pad&Mux Properties */
+#define N 0
+
+/* The Bank contains input-disable regisgers */
+#define BANK_DS	0
+
+/* Clear Register offset */
+#define CLR_REG(r)	((r) + 0x04)
+
+/* Definition of multiple function select register */
+#define FUNC_CLEAR_MASK		0x7
+#define FUNC_GPIO		0
+#define FUNC_ANALOGUE		0x8
+#define ANA_CLEAR_MASK		0x1
+
+/* The Atlas7's Pad Type List */
+enum altas7_pad_type {
+	PAD_T_4WE_PD = 0,	/* ZIO_PAD3V_4WE_PD */
+	PAD_T_4WE_PU,		/* ZIO_PAD3V_4WE_PD */
+	PAD_T_16ST,		/* ZIO_PAD3V_SDCLK_PD */
+	PAD_T_M31_0204_PD,	/* PRDW0204SDGZ_M311311_PD */
+	PAD_T_M31_0204_PU,	/* PRDW0204SDGZ_M311311_PU */
+	PAD_T_M31_0610_PD,	/* PRUW0610SDGZ_M311311_PD */
+	PAD_T_M31_0610_PU,	/* PRUW0610SDGZ_M311311_PU */
+	PAD_T_AD,		/* PRDWUWHW08SCDG_HZ */
+};
+
+/* Raw value of Driver-Strength Bits */
+#define DS3	BIT(3)
+#define DS2	BIT(2)
+#define DS1	BIT(1)
+#define DS0	BIT(0)
+#define DSZ	0
+
+/* Drive-Strength Intermediate Values */
+#define DS_NULL		-1
+#define DS_1BIT_IM_VAL  DS0
+#define DS_1BIT_MASK	0x1
+#define DS_2BIT_IM_VAL  (DS1 | DS0)
+#define DS_2BIT_MASK	0x3
+#define DS_4BIT_IM_VAL	(DS3 | DS2 | DS1 | DS0)
+#define DS_4BIT_MASK	0xf
+
+/* The Drive-Strength of 4WE Pad		 DS1  0  CO */
+#define DS_4WE_3   (DS1 | DS0)			/* 1  1  3  */
+#define DS_4WE_2   (DS1)			/* 1  0  2  */
+#define DS_4WE_1   (DS0)			/* 0  1  1  */
+#define DS_4WE_0   (DSZ)			/* 0  0  0  */
+
+/* The Drive-Strength of 16st Pad		 DS3  2  1  0  CO */
+#define DS_16ST_15  (DS3 | DS2 | DS1 | DS0)	/* 1  1  1  1  15 */
+#define DS_16ST_14  (DS3 | DS2 | DS0)		/* 1  1  0  1  13 */
+#define DS_16ST_13  (DS3 | DS2 | DS1)		/* 1  1  1  0  14 */
+#define DS_16ST_12  (DS2 | DS1 | DS0)		/* 0  1  1  1  7  */
+#define DS_16ST_11  (DS2 | DS0)			/* 0  1  0  1  5  */
+#define DS_16ST_10  (DS3 | DS1 | DS0)		/* 1  0  1  1  11 */
+#define DS_16ST_9   (DS3 | DS0)			/* 1  0  0  1  9  */
+#define DS_16ST_8   (DS1 | DS0)			/* 0  0  1  1  3  */
+#define DS_16ST_7   (DS2 | DS1)			/* 0  1  1  0  6  */
+#define DS_16ST_6   (DS3 | DS2)			/* 1  1  0  0  12 */
+#define DS_16ST_5   (DS2)			/* 0  1  0  0  4  */
+#define DS_16ST_4   (DS3 | DS1)			/* 1  0  1  0  10 */
+#define DS_16ST_3   (DS1)			/* 0  0  1  0  2  */
+#define DS_16ST_2   (DS0)			/* 0  0  0  1  1  */
+#define DS_16ST_1   (DSZ)			/* 0  0  0  0  0  */
+#define DS_16ST_0   (DS3)			/* 1  0  0  0  8  */
+
+/* The Drive-Strength of M31 Pad		 DS0  CO */
+#define DS_M31_0   (DSZ)			/* 0  0  */
+#define DS_M31_1   (DS0)			/* 1  1  */
+
+/* Raw values of Pull Option Bits */
+#define PUN	BIT(1)
+#define PD	BIT(0)
+#define PE	BIT(0)
+#define PZ	0
+
+/* Definition of Pull Types */
+#define PULL_UP		0
+#define HIGH_HYSTERESIS 1
+#define HIGH_Z		2
+#define PULL_DOWN	3
+#define PULL_DISABLE	4
+#define PULL_ENABLE	5
+#define PULL_UNKNOWN	-1
+
+/* Pull Options for 4WE Pad			  PUN  PD  CO */
+#define P4WE_PULL_MASK		0x3
+#define P4WE_PULL_DOWN		(PUN | PD)	/* 1   1   3  */
+#define P4WE_HIGH_Z		(PUN)		/* 1   0   2  */
+#define P4WE_HIGH_HYSTERESIS	(PD)		/* 0   1   1  */
+#define P4WE_PULL_UP		(PZ)		/* 0   0   0  */
+
+/* Pull Options for 16ST Pad			  PUN  PD  CO */
+#define P16ST_PULL_MASK		0x3
+#define P16ST_PULL_DOWN		(PUN | PD)	/* 1   1   3  */
+#define P16ST_HIGH_Z		(PUN)		/* 1   0   2  */
+#define P16ST_PULL_UP		(PZ)		/* 0   0   0  */
+
+/* Pull Options for M31 Pad			  PE */
+#define PM31_PULL_MASK		0x1
+#define PM31_PULL_ENABLED	(PE)		/* 1 */
+#define PM31_PULL_DISABLED	(PZ)		/* 0 */
+
+/* Pull Options for A/D Pad			  PUN  PD  CO */
+#define PANGD_PULL_MASK		0x3
+#define PANGD_PULL_DOWN		(PUN | PD)	/* 1   1   3  */
+#define PANGD_HIGH_Z		(PUN)		/* 1   0   2  */
+#define PANGD_PULL_UP		(PZ)		/* 0   0   0  */
+
+/* Definition of Input Disable */
+#define DI_MASK		0x1
+#define DI_DISABLE	0x1
+#define DI_ENABLE	0x0
+
+/* Definition of Input Disable Value */
+#define DIV_MASK	0x1
+#define DIV_DISABLE	0x1
+#define DIV_ENABLE	0x0
+
+struct dt_params {
+	const char *property;
+	int value;
+};
+
+/**
+ * struct atlas7_pad_conf - Atlas7 Pad Configuration
+ * @id			The ID of this Pad.
+ * @type:		The type of this Pad.
+ * @mux_reg:		The mux register offset.
+ *			This register contains the mux.
+ * @pupd_reg:		The pull-up/down register offset.
+ * @drvstr_reg:		The drive-strength register offset.
+ * @ad_ctrl_reg:	The Analogue/Digital Control register.
+ *
+ * @mux_bit:		The start bit of mux register.
+ * @pupd_bit:		The start bit of pull-up/down register.
+ * @drvstr_bit:		The start bit of drive-strength register.
+ * @ad_ctrl_bit:	The start bit of analogue/digital register.
+ */
+struct atlas7_pad_config {
+	const u32 id;
+	u32 type;
+	u32 mux_reg;
+	u32 pupd_reg;
+	u32 drvstr_reg;
+	u32 ad_ctrl_reg;
+	/* bits in register */
+	u8 mux_bit;
+	u8 pupd_bit;
+	u8 drvstr_bit;
+	u8 ad_ctrl_bit;
+};
+
+#define PADCONF(pad, t, mr, pr, dsr, adr, mb, pb, dsb, adb)	\
+	{							\
+		.id = pad,					\
+		.type = t,					\
+		.mux_reg = mr,					\
+		.pupd_reg = pr,					\
+		.drvstr_reg = dsr,				\
+		.ad_ctrl_reg = adr,				\
+		.mux_bit = mb,					\
+		.pupd_bit = pb,					\
+		.drvstr_bit = dsb,				\
+		.ad_ctrl_bit = adb,				\
+	}
+
+/**
+ * struct atlas7_pad_mux - Atlas7 mux
+ * @bank:		The bank of this pad's registers on.
+ * @pin	:		The ID of this Pad.
+ * @func:		The mux func on this Pad.
+ * @dinput_reg:		The Input-Disable register offset.
+ * @dinput_bit:		The start bit of Input-Disable register.
+ * @dinput_val_reg:	The Input-Disable-value register offset.
+ *			This register is used to set the value of this pad
+ *			if this pad was disabled.
+ * @dinput_val_bit:	The start bit of Input-Disable Value register.
+ */
+struct atlas7_pad_mux {
+	u32 bank;
+	u32 pin;
+	u32 func;
+	u32 dinput_reg;
+	u32 dinput_bit;
+	u32 dinput_val_reg;
+	u32 dinput_val_bit;
+};
+
+#define MUX(b, pad, f, dr, db, dvr, dvb)	\
+	{					\
+		.bank = b,			\
+		.pin = pad,			\
+		.func = f,			\
+		.dinput_reg = dr,		\
+		.dinput_bit = db,		\
+		.dinput_val_reg = dvr,		\
+		.dinput_val_bit = dvb,		\
+	}
+
+struct atlas7_grp_mux {
+	unsigned int group;
+	unsigned int pad_mux_count;
+	const struct atlas7_pad_mux *pad_mux_list;
+};
+
+ /**
+ * struct sirfsoc_pin_group - describes a SiRFprimaII pin group
+ * @name: the name of this specific pin group
+ * @pins: an array of discrete physical pins used in this group, taken
+ *	from the driver-local pin enumeration space
+ * @num_pins: the number of pins in this group array, i.e. the number of
+ *	elements in .pins so we can iterate over that array
+ */
+struct atlas7_pin_group {
+	const char *name;
+	const unsigned int *pins;
+	const unsigned num_pins;
+};
+
+#define GROUP(n, p)  \
+	{			\
+		.name = n,	\
+		.pins = p,	\
+		.num_pins = ARRAY_SIZE(p),	\
+	}
+
+struct atlas7_pmx_func {
+	const char *name;
+	const char * const *groups;
+	const unsigned num_groups;
+	const struct atlas7_grp_mux *grpmux;
+};
+
+#define FUNCTION(n, g, m)		\
+	{					\
+		.name = n,			\
+		.groups = g,			\
+		.num_groups = ARRAY_SIZE(g),	\
+		.grpmux = m,			\
+	}
+
+struct atlas7_pinctrl_data {
+	struct pinctrl_pin_desc *pads;
+	int pads_cnt;
+	struct atlas7_pin_group *grps;
+	int grps_cnt;
+	struct atlas7_pmx_func *funcs;
+	int funcs_cnt;
+	struct atlas7_pad_config *confs;
+	int confs_cnt;
+};
+
+/* Platform info of atlas7 pinctrl */
+#define ATLAS7_PINCTRL_REG_BANKS	2
+#define ATLAS7_PINCTRL_BANK_0_PINS	18
+
+/**
+ * Atlas7 GPIO Chip
+ */
+
+#define NGPIO_OF_BANK		32
+#define GPIO_TO_BANK(gpio)	((gpio) / NGPIO_OF_BANK)
+
+/* Registers of GPIO Controllers */
+#define ATLAS7_GPIO_BASE(g, b)		((g)->reg + 0x100 * (b))
+#define ATLAS7_GPIO_CTRL(b, i)		((b)->base + 4 * (i))
+#define ATLAS7_GPIO_INT_STATUS(b)	((b)->base + 0x8C)
+
+/* Definition bits of GPIO Control Registers */
+#define ATLAS7_GPIO_CTL_INTR_LOW_MASK		BIT(0)
+#define ATLAS7_GPIO_CTL_INTR_HIGH_MASK		BIT(1)
+#define ATLAS7_GPIO_CTL_INTR_TYPE_MASK		BIT(2)
+#define ATLAS7_GPIO_CTL_INTR_EN_MASK		BIT(3)
+#define ATLAS7_GPIO_CTL_INTR_STATUS_MASK	BIT(4)
+#define ATLAS7_GPIO_CTL_OUT_EN_MASK		BIT(5)
+#define ATLAS7_GPIO_CTL_DATAOUT_MASK		BIT(6)
+#define ATLAS7_GPIO_CTL_DATAIN_MASK		BIT(7)
+
+struct atlas7_gpio_bank {
+	struct pinctrl_dev *pctldev;
+	int id;
+	int irq;
+	void __iomem *base;
+	unsigned int gpio_offset;
+	unsigned int ngpio;
+	const unsigned int *gpio_pins;
+};
+
+struct atlas7_gpio_chip {
+	const char *name;
+	void __iomem *reg;
+	struct clk *clk;
+	int nbank;
+	spinlock_t lock;
+	struct gpio_chip chip;
+	struct atlas7_gpio_bank banks[0];
+};
+
+static inline struct atlas7_gpio_chip *to_atlas7_gpio(struct gpio_chip *gc)
+{
+	return container_of(gc, struct atlas7_gpio_chip, chip);
+}
+
+/**
+ * @dev: a pointer back to containing device
+ * @virtbase: the offset to the controller in virtual memory
+ */
+struct atlas7_pmx {
+	struct device *dev;
+	struct pinctrl_dev *pctl;
+	struct pinctrl_desc pctl_desc;
+	struct atlas7_pinctrl_data *pctl_data;
+	void __iomem *regs[ATLAS7_PINCTRL_REG_BANKS];
+};
+
+/*
+ * Pad list for the pinmux subsystem
+ * refer to A7DA IO Summary - CS-314158-DD-4E.xls
+ */
+
+/*Pads in IOC RTC & TOP */
+static const struct pinctrl_pin_desc atlas7_ioc_pads[] = {
+	/* RTC PADs */
+	PINCTRL_PIN(0, "rtc_gpio_0"),
+	PINCTRL_PIN(1, "rtc_gpio_1"),
+	PINCTRL_PIN(2, "rtc_gpio_2"),
+	PINCTRL_PIN(3, "rtc_gpio_3"),
+	PINCTRL_PIN(4, "low_bat_ind_b"),
+	PINCTRL_PIN(5, "on_key_b"),
+	PINCTRL_PIN(6, "ext_on"),
+	PINCTRL_PIN(7, "mem_on"),
+	PINCTRL_PIN(8, "core_on"),
+	PINCTRL_PIN(9, "io_on"),
+	PINCTRL_PIN(10, "can0_tx"),
+	PINCTRL_PIN(11, "can0_rx"),
+	PINCTRL_PIN(12, "spi0_clk"),
+	PINCTRL_PIN(13, "spi0_cs_b"),
+	PINCTRL_PIN(14, "spi0_io_0"),
+	PINCTRL_PIN(15, "spi0_io_1"),
+	PINCTRL_PIN(16, "spi0_io_2"),
+	PINCTRL_PIN(17, "spi0_io_3"),
+
+	/* TOP PADs */
+	PINCTRL_PIN(18, "spi1_en"),
+	PINCTRL_PIN(19, "spi1_clk"),
+	PINCTRL_PIN(20, "spi1_din"),
+	PINCTRL_PIN(21, "spi1_dout"),
+	PINCTRL_PIN(22, "trg_spi_clk"),
+	PINCTRL_PIN(23, "trg_spi_di"),
+	PINCTRL_PIN(24, "trg_spi_do"),
+	PINCTRL_PIN(25, "trg_spi_cs_b"),
+	PINCTRL_PIN(26, "trg_acq_d1"),
+	PINCTRL_PIN(27, "trg_irq_b"),
+	PINCTRL_PIN(28, "trg_acq_d0"),
+	PINCTRL_PIN(29, "trg_acq_clk"),
+	PINCTRL_PIN(30, "trg_shutdown_b_out"),
+	PINCTRL_PIN(31, "sdio2_clk"),
+	PINCTRL_PIN(32, "sdio2_cmd"),
+	PINCTRL_PIN(33, "sdio2_dat_0"),
+	PINCTRL_PIN(34, "sdio2_dat_1"),
+	PINCTRL_PIN(35, "sdio2_dat_2"),
+	PINCTRL_PIN(36, "sdio2_dat_3"),
+	PINCTRL_PIN(37, "df_ad_7"),
+	PINCTRL_PIN(38, "df_ad_6"),
+	PINCTRL_PIN(39, "df_ad_5"),
+	PINCTRL_PIN(40, "df_ad_4"),
+	PINCTRL_PIN(41, "df_ad_3"),
+	PINCTRL_PIN(42, "df_ad_2"),
+	PINCTRL_PIN(43, "df_ad_1"),
+	PINCTRL_PIN(44, "df_ad_0"),
+	PINCTRL_PIN(45, "df_dqs"),
+	PINCTRL_PIN(46, "df_cle"),
+	PINCTRL_PIN(47, "df_ale"),
+	PINCTRL_PIN(48, "df_we_b"),
+	PINCTRL_PIN(49, "df_re_b"),
+	PINCTRL_PIN(50, "df_ry_by"),
+	PINCTRL_PIN(51, "df_cs_b_1"),
+	PINCTRL_PIN(52, "df_cs_b_0"),
+	PINCTRL_PIN(53, "l_pclk"),
+	PINCTRL_PIN(54, "l_lck"),
+	PINCTRL_PIN(55, "l_fck"),
+	PINCTRL_PIN(56, "l_de"),
+	PINCTRL_PIN(57, "ldd_0"),
+	PINCTRL_PIN(58, "ldd_1"),
+	PINCTRL_PIN(59, "ldd_2"),
+	PINCTRL_PIN(60, "ldd_3"),
+	PINCTRL_PIN(61, "ldd_4"),
+	PINCTRL_PIN(62, "ldd_5"),
+	PINCTRL_PIN(63, "ldd_6"),
+	PINCTRL_PIN(64, "ldd_7"),
+	PINCTRL_PIN(65, "ldd_8"),
+	PINCTRL_PIN(66, "ldd_9"),
+	PINCTRL_PIN(67, "ldd_10"),
+	PINCTRL_PIN(68, "ldd_11"),
+	PINCTRL_PIN(69, "ldd_12"),
+	PINCTRL_PIN(70, "ldd_13"),
+	PINCTRL_PIN(71, "ldd_14"),
+	PINCTRL_PIN(72, "ldd_15"),
+	PINCTRL_PIN(73, "lcd_gpio_20"),
+	PINCTRL_PIN(74, "vip_0"),
+	PINCTRL_PIN(75, "vip_1"),
+	PINCTRL_PIN(76, "vip_2"),
+	PINCTRL_PIN(77, "vip_3"),
+	PINCTRL_PIN(78, "vip_4"),
+	PINCTRL_PIN(79, "vip_5"),
+	PINCTRL_PIN(80, "vip_6"),
+	PINCTRL_PIN(81, "vip_7"),
+	PINCTRL_PIN(82, "vip_pxclk"),
+	PINCTRL_PIN(83, "vip_hsync"),
+	PINCTRL_PIN(84, "vip_vsync"),
+	PINCTRL_PIN(85, "sdio3_clk"),
+	PINCTRL_PIN(86, "sdio3_cmd"),
+	PINCTRL_PIN(87, "sdio3_dat_0"),
+	PINCTRL_PIN(88, "sdio3_dat_1"),
+	PINCTRL_PIN(89, "sdio3_dat_2"),
+	PINCTRL_PIN(90, "sdio3_dat_3"),
+	PINCTRL_PIN(91, "sdio5_clk"),
+	PINCTRL_PIN(92, "sdio5_cmd"),
+	PINCTRL_PIN(93, "sdio5_dat_0"),
+	PINCTRL_PIN(94, "sdio5_dat_1"),
+	PINCTRL_PIN(95, "sdio5_dat_2"),
+	PINCTRL_PIN(96, "sdio5_dat_3"),
+	PINCTRL_PIN(97, "rgmii_txd_0"),
+	PINCTRL_PIN(98, "rgmii_txd_1"),
+	PINCTRL_PIN(99, "rgmii_txd_2"),
+	PINCTRL_PIN(100, "rgmii_txd_3"),
+	PINCTRL_PIN(101, "rgmii_txclk"),
+	PINCTRL_PIN(102, "rgmii_tx_ctl"),
+	PINCTRL_PIN(103, "rgmii_rxd_0"),
+	PINCTRL_PIN(104, "rgmii_rxd_1"),
+	PINCTRL_PIN(105, "rgmii_rxd_2"),
+	PINCTRL_PIN(106, "rgmii_rxd_3"),
+	PINCTRL_PIN(107, "rgmii_rx_clk"),
+	PINCTRL_PIN(108, "rgmii_rxc_ctl"),
+	PINCTRL_PIN(109, "rgmii_mdio"),
+	PINCTRL_PIN(110, "rgmii_mdc"),
+	PINCTRL_PIN(111, "rgmii_intr_n"),
+	PINCTRL_PIN(112, "i2s_mclk"),
+	PINCTRL_PIN(113, "i2s_bclk"),
+	PINCTRL_PIN(114, "i2s_ws"),
+	PINCTRL_PIN(115, "i2s_dout0"),
+	PINCTRL_PIN(116, "i2s_dout1"),
+	PINCTRL_PIN(117, "i2s_dout2"),
+	PINCTRL_PIN(118, "i2s_din"),
+	PINCTRL_PIN(119, "gpio_0"),
+	PINCTRL_PIN(120, "gpio_1"),
+	PINCTRL_PIN(121, "gpio_2"),
+	PINCTRL_PIN(122, "gpio_3"),
+	PINCTRL_PIN(123, "gpio_4"),
+	PINCTRL_PIN(124, "gpio_5"),
+	PINCTRL_PIN(125, "gpio_6"),
+	PINCTRL_PIN(126, "gpio_7"),
+	PINCTRL_PIN(127, "sda_0"),
+	PINCTRL_PIN(128, "scl_0"),
+	PINCTRL_PIN(129, "coex_pio_0"),
+	PINCTRL_PIN(130, "coex_pio_1"),
+	PINCTRL_PIN(131, "coex_pio_2"),
+	PINCTRL_PIN(132, "coex_pio_3"),
+	PINCTRL_PIN(133, "uart0_tx"),
+	PINCTRL_PIN(134, "uart0_rx"),
+	PINCTRL_PIN(135, "uart1_tx"),
+	PINCTRL_PIN(136, "uart1_rx"),
+	PINCTRL_PIN(137, "uart3_tx"),
+	PINCTRL_PIN(138, "uart3_rx"),
+	PINCTRL_PIN(139, "uart4_tx"),
+	PINCTRL_PIN(140, "uart4_rx"),
+	PINCTRL_PIN(141, "usp0_clk"),
+	PINCTRL_PIN(142, "usp0_tx"),
+	PINCTRL_PIN(143, "usp0_rx"),
+	PINCTRL_PIN(144, "usp0_fs"),
+	PINCTRL_PIN(145, "usp1_clk"),
+	PINCTRL_PIN(146, "usp1_tx"),
+	PINCTRL_PIN(147, "usp1_rx"),
+	PINCTRL_PIN(148, "usp1_fs"),
+	PINCTRL_PIN(149, "lvds_tx0d4p"),
+	PINCTRL_PIN(150, "lvds_tx0d4n"),
+	PINCTRL_PIN(151, "lvds_tx0d3p"),
+	PINCTRL_PIN(152, "lvds_tx0d3n"),
+	PINCTRL_PIN(153, "lvds_tx0d2p"),
+	PINCTRL_PIN(154, "lvds_tx0d2n"),
+	PINCTRL_PIN(155, "lvds_tx0d1p"),
+	PINCTRL_PIN(156, "lvds_tx0d1n"),
+	PINCTRL_PIN(157, "lvds_tx0d0p"),
+	PINCTRL_PIN(158, "lvds_tx0d0n"),
+};
+
+struct atlas7_pad_config atlas7_ioc_pad_confs[] = {
+	/* The Configuration of IOC_RTC Pads */
+	PADCONF(0, 3, 0x0, 0x100, 0x200, -1, 0, 0, 0, 0),
+	PADCONF(1, 3, 0x0, 0x100, 0x200, -1, 4, 2, 2, 0),
+	PADCONF(2, 3, 0x0, 0x100, 0x200, -1, 8, 4, 4, 0),
+	PADCONF(3, 5, 0x0, 0x100, 0x200, -1, 12, 6, 6, 0),
+	PADCONF(4, 4, 0x0, 0x100, 0x200, -1, 16, 8, 8, 0),
+	PADCONF(5, 4, 0x0, 0x100, 0x200, -1, 20, 10, 10, 0),
+	PADCONF(6, 3, 0x0, 0x100, 0x200, -1, 24, 12, 12, 0),
+	PADCONF(7, 3, 0x0, 0x100, 0x200, -1, 28, 14, 14, 0),
+	PADCONF(8, 3, 0x8, 0x100, 0x200, -1, 0, 16, 16, 0),
+	PADCONF(9, 3, 0x8, 0x100, 0x200, -1, 4, 18, 18, 0),
+	PADCONF(10, 4, 0x8, 0x100, 0x200, -1, 8, 20, 20, 0),
+	PADCONF(11, 4, 0x8, 0x100, 0x200, -1, 12, 22, 22, 0),
+	PADCONF(12, 5, 0x8, 0x100, 0x200, -1, 16, 24, 24, 0),
+	PADCONF(13, 6, 0x8, 0x100, 0x200, -1, 20, 26, 26, 0),
+	PADCONF(14, 5, 0x8, 0x100, 0x200, -1, 24, 28, 28, 0),
+	PADCONF(15, 5, 0x8, 0x100, 0x200, -1, 28, 30, 30, 0),
+	PADCONF(16, 5, 0x10, 0x108, 0x208, -1, 0, 0, 0, 0),
+	PADCONF(17, 5, 0x10, 0x108, 0x208, -1, 4, 2, 2, 0),
+	/* The Configuration of IOC_TOP Pads */
+	PADCONF(18, 5, 0x80, 0x180, 0x300, -1, 0, 0, 0, 0),
+	PADCONF(19, 5, 0x80, 0x180, 0x300, -1, 4, 2, 2, 0),
+	PADCONF(20, 5, 0x80, 0x180, 0x300, -1, 8, 4, 4, 0),
+	PADCONF(21, 5, 0x80, 0x180, 0x300, -1, 12, 6, 6, 0),
+	PADCONF(22, 5, 0x88, 0x188, 0x308, -1, 0, 0, 0, 0),
+	PADCONF(23, 5, 0x88, 0x188, 0x308, -1, 4, 2, 2, 0),
+	PADCONF(24, 5, 0x88, 0x188, 0x308, -1, 8, 4, 4, 0),
+	PADCONF(25, 6, 0x88, 0x188, 0x308, -1, 12, 6, 6, 0),
+	PADCONF(26, 5, 0x88, 0x188, 0x308, -1, 16, 8, 8, 0),
+	PADCONF(27, 6, 0x88, 0x188, 0x308, -1, 20, 10, 10, 0),
+	PADCONF(28, 5, 0x88, 0x188, 0x308, -1, 24, 12, 12, 0),
+	PADCONF(29, 5, 0x88, 0x188, 0x308, -1, 28, 14, 14, 0),
+	PADCONF(30, 5, 0x90, 0x188, 0x308, -1, 0, 16, 16, 0),
+	PADCONF(31, 2, 0x98, 0x190, 0x310, -1, 0, 0, 0, 0),
+	PADCONF(32, 1, 0x98, 0x190, 0x310, -1, 4, 2, 4, 0),
+	PADCONF(33, 1, 0x98, 0x190, 0x310, -1, 8, 4, 6, 0),
+	PADCONF(34, 1, 0x98, 0x190, 0x310, -1, 12, 6, 8, 0),
+	PADCONF(35, 1, 0x98, 0x190, 0x310, -1, 16, 8, 10, 0),
+	PADCONF(36, 1, 0x98, 0x190, 0x310, -1, 20, 10, 12, 0),
+	PADCONF(37, 1, 0xa0, 0x198, 0x318, -1, 0, 0, 0, 0),
+	PADCONF(38, 1, 0xa0, 0x198, 0x318, -1, 4, 2, 2, 0),
+	PADCONF(39, 1, 0xa0, 0x198, 0x318, -1, 8, 4, 4, 0),
+	PADCONF(40, 1, 0xa0, 0x198, 0x318, -1, 12, 6, 6, 0),
+	PADCONF(41, 1, 0xa0, 0x198, 0x318, -1, 16, 8, 8, 0),
+	PADCONF(42, 1, 0xa0, 0x198, 0x318, -1, 20, 10, 10, 0),
+	PADCONF(43, 1, 0xa0, 0x198, 0x318, -1, 24, 12, 12, 0),
+	PADCONF(44, 1, 0xa0, 0x198, 0x318, -1, 28, 14, 14, 0),
+	PADCONF(45, 0, 0xa8, 0x198, 0x318, -1, 0, 16, 16, 0),
+	PADCONF(46, 0, 0xa8, 0x198, 0x318, -1, 4, 18, 18, 0),
+	PADCONF(47, 1, 0xa8, 0x198, 0x318, -1, 8, 20, 20, 0),
+	PADCONF(48, 1, 0xa8, 0x198, 0x318, -1, 12, 22, 22, 0),
+	PADCONF(49, 1, 0xa8, 0x198, 0x318, -1, 16, 24, 24, 0),
+	PADCONF(50, 1, 0xa8, 0x198, 0x318, -1, 20, 26, 26, 0),
+	PADCONF(51, 1, 0xa8, 0x198, 0x318, -1, 24, 28, 28, 0),
+	PADCONF(52, 1, 0xa8, 0x198, 0x318, -1, 28, 30, 30, 0),
+	PADCONF(53, 0, 0xb0, 0x1a0, 0x320, -1, 0, 0, 0, 0),
+	PADCONF(54, 0, 0xb0, 0x1a0, 0x320, -1, 4, 2, 2, 0),
+	PADCONF(55, 0, 0xb0, 0x1a0, 0x320, -1, 8, 4, 4, 0),
+	PADCONF(56, 0, 0xb0, 0x1a0, 0x320, -1, 12, 6, 6, 0),
+	PADCONF(57, 0, 0xb0, 0x1a0, 0x320, -1, 16, 8, 8, 0),
+	PADCONF(58, 0, 0xb0, 0x1a0, 0x320, -1, 20, 10, 10, 0),
+	PADCONF(59, 0, 0xb0, 0x1a0, 0x320, -1, 24, 12, 12, 0),
+	PADCONF(60, 0, 0xb0, 0x1a0, 0x320, -1, 28, 14, 14, 0),
+	PADCONF(61, 0, 0xb8, 0x1a0, 0x320, -1, 0, 16, 16, 0),
+	PADCONF(62, 0, 0xb8, 0x1a0, 0x320, -1, 4, 18, 18, 0),
+	PADCONF(63, 0, 0xb8, 0x1a0, 0x320, -1, 8, 20, 20, 0),
+	PADCONF(64, 0, 0xb8, 0x1a0, 0x320, -1, 12, 22, 22, 0),
+	PADCONF(65, 0, 0xb8, 0x1a0, 0x320, -1, 16, 24, 24, 0),
+	PADCONF(66, 0, 0xb8, 0x1a0, 0x320, -1, 20, 26, 26, 0),
+	PADCONF(67, 0, 0xb8, 0x1a0, 0x320, -1, 24, 28, 28, 0),
+	PADCONF(68, 0, 0xb8, 0x1a0, 0x320, -1, 28, 30, 30, 0),
+	PADCONF(69, 0, 0xc0, 0x1a8, 0x328, -1, 0, 0, 0, 0),
+	PADCONF(70, 0, 0xc0, 0x1a8, 0x328, -1, 4, 2, 2, 0),
+	PADCONF(71, 0, 0xc0, 0x1a8, 0x328, -1, 8, 4, 4, 0),
+	PADCONF(72, 0, 0xc0, 0x1a8, 0x328, -1, 12, 6, 6, 0),
+	PADCONF(73, 0, 0xc0, 0x1a8, 0x328, -1, 16, 8, 8, 0),
+	PADCONF(74, 0, 0xc8, 0x1b0, 0x330, -1, 0, 0, 0, 0),
+	PADCONF(75, 0, 0xc8, 0x1b0, 0x330, -1, 4, 2, 2, 0),
+	PADCONF(76, 0, 0xc8, 0x1b0, 0x330, -1, 8, 4, 4, 0),
+	PADCONF(77, 0, 0xc8, 0x1b0, 0x330, -1, 12, 6, 6, 0),
+	PADCONF(78, 0, 0xc8, 0x1b0, 0x330, -1, 16, 8, 8, 0),
+	PADCONF(79, 0, 0xc8, 0x1b0, 0x330, -1, 20, 10, 10, 0),
+	PADCONF(80, 0, 0xc8, 0x1b0, 0x330, -1, 24, 12, 12, 0),
+	PADCONF(81, 0, 0xc8, 0x1b0, 0x330, -1, 28, 14, 14, 0),
+	PADCONF(82, 0, 0xd0, 0x1b0, 0x330, -1, 0, 16, 16, 0),
+	PADCONF(83, 0, 0xd0, 0x1b0, 0x330, -1, 4, 18, 18, 0),
+	PADCONF(84, 0, 0xd0, 0x1b0, 0x330, -1, 8, 20, 20, 0),
+	PADCONF(85, 2, 0xd8, 0x1b8, 0x338, -1, 0, 0, 0, 0),
+	PADCONF(86, 1, 0xd8, 0x1b8, 0x338, -1, 4, 4, 4, 0),
+	PADCONF(87, 1, 0xd8, 0x1b8, 0x338, -1, 8, 6, 6, 0),
+	PADCONF(88, 1, 0xd8, 0x1b8, 0x338, -1, 12, 8, 8, 0),
+	PADCONF(89, 1, 0xd8, 0x1b8, 0x338, -1, 16, 10, 10, 0),
+	PADCONF(90, 1, 0xd8, 0x1b8, 0x338, -1, 20, 12, 12, 0),
+	PADCONF(91, 2, 0xe0, 0x1c0, 0x340, -1, 0, 0, 0, 0),
+	PADCONF(92, 1, 0xe0, 0x1c0, 0x340, -1, 4, 4, 4, 0),
+	PADCONF(93, 1, 0xe0, 0x1c0, 0x340, -1, 8, 6, 6, 0),
+	PADCONF(94, 1, 0xe0, 0x1c0, 0x340, -1, 12, 8, 8, 0),
+	PADCONF(95, 1, 0xe0, 0x1c0, 0x340, -1, 16, 10, 10, 0),
+	PADCONF(96, 1, 0xe0, 0x1c0, 0x340, -1, 20, 12, 12, 0),
+	PADCONF(97, 0, 0xe8, 0x1c8, 0x348, -1, 0, 0, 0, 0),
+	PADCONF(98, 0, 0xe8, 0x1c8, 0x348, -1, 4, 2, 2, 0),
+	PADCONF(99, 0, 0xe8, 0x1c8, 0x348, -1, 8, 4, 4, 0),
+	PADCONF(100, 0, 0xe8, 0x1c8, 0x348, -1, 12, 6, 6, 0),
+	PADCONF(101, 2, 0xe8, 0x1c8, 0x348, -1, 16, 8, 8, 0),
+	PADCONF(102, 0, 0xe8, 0x1c8, 0x348, -1, 20, 12, 12, 0),
+	PADCONF(103, 0, 0xe8, 0x1c8, 0x348, -1, 24, 14, 14, 0),
+	PADCONF(104, 0, 0xe8, 0x1c8, 0x348, -1, 28, 16, 16, 0),
+	PADCONF(105, 0, 0xf0, 0x1c8, 0x348, -1, 0, 18, 18, 0),
+	PADCONF(106, 0, 0xf0, 0x1c8, 0x348, -1, 4, 20, 20, 0),
+	PADCONF(107, 0, 0xf0, 0x1c8, 0x348, -1, 8, 22, 22, 0),
+	PADCONF(108, 0, 0xf0, 0x1c8, 0x348, -1, 12, 24, 24, 0),
+	PADCONF(109, 1, 0xf0, 0x1c8, 0x348, -1, 16, 26, 26, 0),
+	PADCONF(110, 0, 0xf0, 0x1c8, 0x348, -1, 20, 28, 28, 0),
+	PADCONF(111, 1, 0xf0, 0x1c8, 0x348, -1, 24, 30, 30, 0),
+	PADCONF(112, 5, 0xf8, 0x200, 0x350, -1, 0, 0, 0, 0),
+	PADCONF(113, 5, 0xf8, 0x200, 0x350, -1, 4, 2, 2, 0),
+	PADCONF(114, 5, 0xf8, 0x200, 0x350, -1, 8, 4, 4, 0),
+	PADCONF(115, 5, 0xf8, 0x200, 0x350, -1, 12, 6, 6, 0),
+	PADCONF(116, 5, 0xf8, 0x200, 0x350, -1, 16, 8, 8, 0),
+	PADCONF(117, 5, 0xf8, 0x200, 0x350, -1, 20, 10, 10, 0),
+	PADCONF(118, 5, 0xf8, 0x200, 0x350, -1, 24, 12, 12, 0),
+	PADCONF(119, 5, 0x100, 0x250, 0x358, -1, 0, 0, 0, 0),
+	PADCONF(120, 5, 0x100, 0x250, 0x358, -1, 4, 2, 2, 0),
+	PADCONF(121, 5, 0x100, 0x250, 0x358, -1, 8, 4, 4, 0),
+	PADCONF(122, 5, 0x100, 0x250, 0x358, -1, 12, 6, 6, 0),
+	PADCONF(123, 6, 0x100, 0x250, 0x358, -1, 16, 8, 8, 0),
+	PADCONF(124, 6, 0x100, 0x250, 0x358, -1, 20, 10, 10, 0),
+	PADCONF(125, 6, 0x100, 0x250, 0x358, -1, 24, 12, 12, 0),
+	PADCONF(126, 6, 0x100, 0x250, 0x358, -1, 28, 14, 14, 0),
+	PADCONF(127, 6, 0x108, 0x250, 0x358, -1, 16, 24, 24, 0),
+	PADCONF(128, 6, 0x108, 0x250, 0x358, -1, 20, 26, 26, 0),
+	PADCONF(129, 0, 0x110, 0x258, 0x360, -1, 0, 0, 0, 0),
+	PADCONF(130, 0, 0x110, 0x258, 0x360, -1, 4, 2, 2, 0),
+	PADCONF(131, 0, 0x110, 0x258, 0x360, -1, 8, 4, 4, 0),
+	PADCONF(132, 0, 0x110, 0x258, 0x360, -1, 12, 6, 6, 0),
+	PADCONF(133, 6, 0x118, 0x260, 0x368, -1, 0, 0, 0, 0),
+	PADCONF(134, 6, 0x118, 0x260, 0x368, -1, 4, 2, 2, 0),
+	PADCONF(135, 6, 0x118, 0x260, 0x368, -1, 16, 8, 8, 0),
+	PADCONF(136, 6, 0x118, 0x260, 0x368, -1, 20, 10, 10, 0),
+	PADCONF(137, 6, 0x118, 0x260, 0x368, -1, 24, 12, 12, 0),
+	PADCONF(138, 6, 0x118, 0x260, 0x368, -1, 28, 14, 14, 0),
+	PADCONF(139, 6, 0x120, 0x260, 0x368, -1, 0, 16, 16, 0),
+	PADCONF(140, 6, 0x120, 0x260, 0x368, -1, 4, 18, 18, 0),
+	PADCONF(141, 5, 0x128, 0x268, 0x378, -1, 0, 0, 0, 0),
+	PADCONF(142, 5, 0x128, 0x268, 0x378, -1, 4, 2, 2, 0),
+	PADCONF(143, 5, 0x128, 0x268, 0x378, -1, 8, 4, 4, 0),
+	PADCONF(144, 5, 0x128, 0x268, 0x378, -1, 12, 6, 6, 0),
+	PADCONF(145, 5, 0x128, 0x268, 0x378, -1, 16, 8, 8, 0),
+	PADCONF(146, 5, 0x128, 0x268, 0x378, -1, 20, 10, 10, 0),
+	PADCONF(147, 5, 0x128, 0x268, 0x378, -1, 24, 12, 12, 0),
+	PADCONF(148, 5, 0x128, 0x268, 0x378, -1, 28, 14, 14, 0),
+	PADCONF(149, 7, 0x130, 0x270, -1, 0x480, 0, 0, 0, 0),
+	PADCONF(150, 7, 0x130, 0x270, -1, 0x480, 4, 2, 0, 1),
+	PADCONF(151, 7, 0x130, 0x270, -1, 0x480, 8, 4, 0, 2),
+	PADCONF(152, 7, 0x130, 0x270, -1, 0x480, 12, 6, 0, 3),
+	PADCONF(153, 7, 0x130, 0x270, -1, 0x480, 16, 8, 0, 4),
+	PADCONF(154, 7, 0x130, 0x270, -1, 0x480, 20, 10, 0, 5),
+	PADCONF(155, 7, 0x130, 0x270, -1, 0x480, 24, 12, 0, 6),
+	PADCONF(156, 7, 0x130, 0x270, -1, 0x480, 28, 14, 0, 7),
+	PADCONF(157, 7, 0x138, 0x278, -1, 0x480, 0, 0, 0, 8),
+	PADCONF(158, 7, 0x138, 0x278, -1, 0x480, 4, 2, 0, 9),
+};
+
+/* pin list of each pin group */
+static const unsigned int gnss_gpio_pins[] = { 119, 120, 121, 122, 123, 124,
+		125, 126, 127, 128, 22, 23, 24, 25, 26, 27, 28, 29, 30, };
+static const unsigned int lcd_vip_gpio_pins[] = { 74, 75, 76, 77, 78, 79, 80,
+		81, 82, 83, 84, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63,
+		64, 65, 66, 67, 68, 69, 70, 71, 72, 73, };
+static const unsigned int sdio_i2s_gpio_pins[] = { 31, 32, 33, 34, 35, 36,
+		85, 86, 87, 88, 89, 90, 129, 130, 131, 132, 91, 92, 93, 94,
+		95, 96, 112, 113, 114, 115, 116, 117, 118, };
+static const unsigned int sp_rgmii_gpio_pins[] = { 97, 98, 99, 100, 101, 102,
+		103, 104, 105, 106, 107, 108, 109, 110, 111, 18, 19, 20, 21,
+		141, 142, 143, 144, 145, 146, 147, 148, };
+static const unsigned int lvds_gpio_pins[] = { 157, 158, 155, 156, 153, 154,
+		151, 152, 149, 150, };
+static const unsigned int uart_nand_gpio_pins[] = { 44, 43, 42, 41, 40, 39,
+		38, 37, 46, 47, 48, 49, 50, 52, 51, 45, 133, 134, 135, 136,
+		137, 138, 139, 140, };
+static const unsigned int rtc_gpio_pins[] = { 0, 1, 2, 3, 4, 10, 11, 12, 13,
+		14, 15, 16, 17, };
+static const unsigned int audio_ac97_pins[] = { 113, 118, 115, 114, };
+static const unsigned int audio_func_dbg_pins[] = { 141, 144, 44, 43, 42, 41,
+		40, 39, 38, 37, 74, 75, 76, 77, 78, 79, 81, 113, 114, 118,
+		115, 49, 50, 142, 143, 80, };
+static const unsigned int audio_i2s_pins[] = { 118, 115, 116, 117, 112, 113,
+		114, };
+static const unsigned int audio_i2s_2ch_pins[] = { 118, 115, 112, 113, 114, };
+static const unsigned int audio_i2s_extclk_pins[] = { 112, };
+static const unsigned int audio_uart0_pins[] = { 143, 142, 141, 144, };
+static const unsigned int audio_uart1_pins[] = { 147, 146, 145, 148, };
+static const unsigned int audio_uart2_pins0[] = { 20, 21, 19, 18, };
+static const unsigned int audio_uart2_pins1[] = { 109, 110, 101, 111, };
+static const unsigned int c_can_trnsvr_pins[] = { 1, };
+static const unsigned int c0_can_pins0[] = { 11, 10, };
+static const unsigned int c0_can_pins1[] = { 2, 3, };
+static const unsigned int c1_can_pins0[] = { 138, 137, };
+static const unsigned int c1_can_pins1[] = { 147, 146, };
+static const unsigned int c1_can_pins2[] = { 2, 3, };
+static const unsigned int ca_audio_lpc_pins[] = { 62, 63, 64, 65, 66, 67, 68,
+		69, 70, 71, };
+static const unsigned int ca_bt_lpc_pins[] = { 85, 86, 87, 88, 89, 90, };
+static const unsigned int ca_coex_pins[] = { 129, 130, 131, 132, };
+static const unsigned int ca_curator_lpc_pins[] = { 57, 58, 59, 60, };
+static const unsigned int ca_pcm_debug_pins[] = { 91, 93, 94, 92, };
+static const unsigned int ca_pio_pins[] = { 121, 122, 125, 126, 38, 37, 47,
+		49, 50, 54, 55, 56, };
+static const unsigned int ca_sdio_debug_pins[] = { 40, 39, 44, 43, 42, 41, };
+static const unsigned int ca_spi_pins[] = { 82, 79, 80, 81, };
+static const unsigned int ca_trb_pins[] = { 91, 93, 94, 95, 96, 78, 74, 75,
+		76, 77, };
+static const unsigned int ca_uart_debug_pins[] = { 136, 135, 134, 133, };
+static const unsigned int clkc_pins0[] = { 30, 47, };
+static const unsigned int clkc_pins1[] = { 78, 54, };
+static const unsigned int gn_gnss_i2c_pins[] = { 128, 127, };
+static const unsigned int gn_gnss_uart_nopause_pins[] = { 134, 133, };
+static const unsigned int gn_gnss_uart_pins[] = { 134, 133, 136, 135, };
+static const unsigned int gn_trg_spi_pins0[] = { 22, 25, 23, 24, };
+static const unsigned int gn_trg_spi_pins1[] = { 82, 79, 80, 81, };
+static const unsigned int cvbs_dbg_pins[] = { 54, 53, 82, 74, 75, 76, 77, 78,
+		79, 80, 81, 83, 84, 73, 55, 56, };
+static const unsigned int cvbs_dbg_test_pins0[] = { 57, };
+static const unsigned int cvbs_dbg_test_pins1[] = { 58, };
+static const unsigned int cvbs_dbg_test_pins2[] = { 59, };
+static const unsigned int cvbs_dbg_test_pins3[] = { 60, };
+static const unsigned int cvbs_dbg_test_pins4[] = { 61, };
+static const unsigned int cvbs_dbg_test_pins5[] = { 62, };
+static const unsigned int cvbs_dbg_test_pins6[] = { 63, };
+static const unsigned int cvbs_dbg_test_pins7[] = { 64, };
+static const unsigned int cvbs_dbg_test_pins8[] = { 65, };
+static const unsigned int cvbs_dbg_test_pins9[] = { 66, };
+static const unsigned int cvbs_dbg_test_pins10[] = { 67, };
+static const unsigned int cvbs_dbg_test_pins11[] = { 68, };
+static const unsigned int cvbs_dbg_test_pins12[] = { 69, };
+static const unsigned int cvbs_dbg_test_pins13[] = { 70, };
+static const unsigned int cvbs_dbg_test_pins14[] = { 71, };
+static const unsigned int cvbs_dbg_test_pins15[] = { 72, };
+static const unsigned int gn_gnss_power_pins[] = { 123, 124, 121, 122, 125,
+		120, };
+static const unsigned int gn_gnss_sw_status_pins[] = { 57, 58, 59, 60, 61,
+		62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 53, 55, 56, 54, };
+static const unsigned int gn_gnss_eclk_pins[] = { 113, };
+static const unsigned int gn_gnss_irq1_pins0[] = { 112, };
+static const unsigned int gn_gnss_irq2_pins0[] = { 118, };
+static const unsigned int gn_gnss_tm_pins[] = { 115, };
+static const unsigned int gn_gnss_tsync_pins[] = { 114, };
+static const unsigned int gn_io_gnsssys_sw_cfg_pins[] = { 44, 43, 42, 41, 40,
+		39, 38, 37, 49, 50, 91, 92, 93, 94, 95, 96, };
+static const unsigned int gn_trg_pins0[] = { 29, 28, 26, 27, };
+static const unsigned int gn_trg_pins1[] = { 77, 76, 74, 75, };
+static const unsigned int gn_trg_shutdown_pins0[] = { 30, };
+static const unsigned int gn_trg_shutdown_pins1[] = { 83, };
+static const unsigned int gn_trg_shutdown_pins2[] = { 117, };
+static const unsigned int gn_trg_shutdown_pins3[] = { 123, };
+static const unsigned int i2c0_pins[] = { 128, 127, };
+static const unsigned int i2c1_pins[] = { 126, 125, };
+static const unsigned int jtag_pins0[] = { 125, 4, 2, 0, 1, 3, };
+static const unsigned int ks_kas_spi_pins0[] = { 141, 144, 143, 142, };
+static const unsigned int ld_ldd_pins[] = { 57, 58, 59, 60, 61, 62, 63, 64,
+		65, 66, 67, 68, 69, 70, 71, 72, 74, 75, 76, 77, 78, 79, 80,
+		81, 56, 53, };
+static const unsigned int ld_ldd_16bit_pins[] = { 57, 58, 59, 60, 61, 62, 63,
+		64, 65, 66, 67, 68, 69, 70, 71, 72, 56, 53, };
+static const unsigned int ld_ldd_fck_pins[] = { 55, };
+static const unsigned int ld_ldd_lck_pins[] = { 54, };
+static const unsigned int lr_lcdrom_pins[] = { 73, 54, 57, 58, 59, 60, 61,
+		62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 56, 53, 55, };
+static const unsigned int lvds_analog_pins[] = { 149, 150, 151, 152, 153, 154,
+		155, 156, 157, 158, };
+static const unsigned int nd_df_pins[] = { 44, 43, 42, 41, 40, 39, 38, 37,
+		47, 46, 52, 51, 45, 49, 50, 48, 124, };
+static const unsigned int nd_df_nowp_pins[] = { 44, 43, 42, 41, 40, 39, 38,
+		37, 47, 46, 52, 51, 45, 49, 50, 48, };
+static const unsigned int ps_pins[] = { 120, 119, };
+static const unsigned int pwc_core_on_pins[] = { 8, };
+static const unsigned int pwc_ext_on_pins[] = { 6, };
+static const unsigned int pwc_gpio3_clk_pins[] = { 3, };
+static const unsigned int pwc_io_on_pins[] = { 9, };
+static const unsigned int pwc_lowbatt_b_pins0[] = { 4, };
+static const unsigned int pwc_mem_on_pins[] = { 7, };
+static const unsigned int pwc_on_key_b_pins0[] = { 5, };
+static const unsigned int pwc_wakeup_src0_pins[] = { 0, };
+static const unsigned int pwc_wakeup_src1_pins[] = { 1, };
+static const unsigned int pwc_wakeup_src2_pins[] = { 2, };
+static const unsigned int pwc_wakeup_src3_pins[] = { 3, };
+static const unsigned int pw_cko0_pins0[] = { 123, };
+static const unsigned int pw_cko0_pins1[] = { 101, };
+static const unsigned int pw_cko0_pins2[] = { 82, };
+static const unsigned int pw_cko1_pins0[] = { 124, };
+static const unsigned int pw_cko1_pins1[] = { 110, };
+static const unsigned int pw_i2s01_clk_pins0[] = { 125, };
+static const unsigned int pw_i2s01_clk_pins1[] = { 117, };
+static const unsigned int pw_pwm0_pins[] = { 119, };
+static const unsigned int pw_pwm1_pins[] = { 120, };
+static const unsigned int pw_pwm2_pins0[] = { 121, };
+static const unsigned int pw_pwm2_pins1[] = { 98, };
+static const unsigned int pw_pwm3_pins0[] = { 122, };
+static const unsigned int pw_pwm3_pins1[] = { 73, };
+static const unsigned int pw_pwm_cpu_vol_pins0[] = { 121, };
+static const unsigned int pw_pwm_cpu_vol_pins1[] = { 98, };
+static const unsigned int pw_backlight_pins0[] = { 122, };
+static const unsigned int pw_backlight_pins1[] = { 73, };
+static const unsigned int rg_eth_mac_pins[] = { 108, 103, 104, 105, 106, 107,
+		102, 97, 98, 99, 100, 101, };
+static const unsigned int rg_gmac_phy_intr_n_pins[] = { 111, };
+static const unsigned int rg_rgmii_mac_pins[] = { 109, 110, };
+static const unsigned int rg_rgmii_phy_ref_clk_pins0[] = { 111, };
+static const unsigned int rg_rgmii_phy_ref_clk_pins1[] = { 53, };
+static const unsigned int sd0_pins[] = { 46, 47, 44, 43, 42, 41, 40, 39, 38,
+		37, };
+static const unsigned int sd0_4bit_pins[] = { 46, 47, 44, 43, 42, 41, };
+static const unsigned int sd1_pins[] = { 48, 49, 44, 43, 42, 41, 40, 39, 38,
+		37, };
+static const unsigned int sd1_4bit_pins0[] = { 48, 49, 44, 43, 42, 41, };
+static const unsigned int sd1_4bit_pins1[] = { 48, 49, 40, 39, 38, 37, };
+static const unsigned int sd2_pins0[] = { 124, 31, 32, 33, 34, 35, 36, 123, };
+static const unsigned int sd2_no_cdb_pins0[] = { 31, 32, 33, 34, 35, 36, 123, };
+static const unsigned int sd3_pins[] = { 85, 86, 87, 88, 89, 90, };
+static const unsigned int sd5_pins[] = { 91, 92, 93, 94, 95, 96, };
+static const unsigned int sd6_pins0[] = { 79, 78, 74, 75, 76, 77, };
+static const unsigned int sd6_pins1[] = { 101, 99, 100, 110, 109, 111, };
+static const unsigned int sp0_ext_ldo_on_pins[] = { 4, };
+static const unsigned int sp0_qspi_pins[] = { 12, 13, 14, 15, 16, 17, };
+static const unsigned int sp1_spi_pins[] = { 19, 20, 21, 18, };
+static const unsigned int tpiu_trace_pins[] = { 53, 56, 57, 58, 59, 60, 61,
+		62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, };
+static const unsigned int uart0_pins[] = { 121, 120, 134, 133, };
+static const unsigned int uart0_nopause_pins[] = { 134, 133, };
+static const unsigned int uart1_pins[] = { 136, 135, };
+static const unsigned int uart2_pins[] = { 11, 10, };
+static const unsigned int uart3_pins0[] = { 125, 126, 138, 137, };
+static const unsigned int uart3_pins1[] = { 111, 109, 84, 83, };
+static const unsigned int uart3_pins2[] = { 140, 139, 138, 137, };
+static const unsigned int uart3_pins3[] = { 139, 140, 84, 83, };
+static const unsigned int uart3_nopause_pins0[] = { 138, 137, };
+static const unsigned int uart3_nopause_pins1[] = { 84, 83, };
+static const unsigned int uart4_pins0[] = { 122, 123, 140, 139, };
+static const unsigned int uart4_pins1[] = { 100, 99, 140, 139, };
+static const unsigned int uart4_pins2[] = { 117, 116, 140, 139, };
+static const unsigned int uart4_nopause_pins[] = { 140, 139, };
+static const unsigned int usb0_drvvbus_pins[] = { 51, };
+static const unsigned int usb1_drvvbus_pins[] = { 134, };
+static const unsigned int visbus_dout_pins[] = { 57, 58, 59, 60, 61, 62, 63,
+		64, 65, 66, 67, 68, 69, 70, 71, 72, 53, 54, 55, 56, 85, 86,
+		87, 88, 89, 90, 91, 92, 93, 94, 95, 96, };
+static const unsigned int vi_vip1_pins[] = { 74, 75, 76, 77, 78, 79, 80, 81,
+		82, 83, 84, 103, 104, 105, 106, 107, 102, 97, 98, };
+static const unsigned int vi_vip1_ext_pins[] = { 74, 75, 76, 77, 78, 79, 80,
+		81, 82, 83, 84, 108, 103, 104, 105, 106, 107, 102, 97, 98,
+		99, 100, };
+static const unsigned int vi_vip1_low8bit_pins[] = { 74, 75, 76, 77, 78, 79,
+		80, 81, };
+static const unsigned int vi_vip1_high8bit_pins[] = { 82, 83, 84, 108, 103,
+		104, 105, 106, };
+
+/* definition of pin group table */
+struct atlas7_pin_group altas7_pin_groups[] = {
+	GROUP("gnss_gpio_grp", gnss_gpio_pins),
+	GROUP("lcd_vip_gpio_grp", lcd_vip_gpio_pins),
+	GROUP("sdio_i2s_gpio_grp", sdio_i2s_gpio_pins),
+	GROUP("sp_rgmii_gpio_grp", sp_rgmii_gpio_pins),
+	GROUP("lvds_gpio_grp", lvds_gpio_pins),
+	GROUP("uart_nand_gpio_grp", uart_nand_gpio_pins),
+	GROUP("rtc_gpio_grp", rtc_gpio_pins),
+	GROUP("audio_ac97_grp", audio_ac97_pins),
+	GROUP("audio_func_dbg_grp", audio_func_dbg_pins),
+	GROUP("audio_i2s_grp", audio_i2s_pins),
+	GROUP("audio_i2s_2ch_grp", audio_i2s_2ch_pins),
+	GROUP("audio_i2s_extclk_grp", audio_i2s_extclk_pins),
+	GROUP("audio_uart0_grp", audio_uart0_pins),
+	GROUP("audio_uart1_grp", audio_uart1_pins),
+	GROUP("audio_uart2_grp0", audio_uart2_pins0),
+	GROUP("audio_uart2_grp1", audio_uart2_pins1),
+	GROUP("c_can_trnsvr_grp", c_can_trnsvr_pins),
+	GROUP("c0_can_grp0", c0_can_pins0),
+	GROUP("c0_can_grp1", c0_can_pins1),
+	GROUP("c1_can_grp0", c1_can_pins0),
+	GROUP("c1_can_grp1", c1_can_pins1),
+	GROUP("c1_can_grp2", c1_can_pins2),
+	GROUP("ca_audio_lpc_grp", ca_audio_lpc_pins),
+	GROUP("ca_bt_lpc_grp", ca_bt_lpc_pins),
+	GROUP("ca_coex_grp", ca_coex_pins),
+	GROUP("ca_curator_lpc_grp", ca_curator_lpc_pins),
+	GROUP("ca_pcm_debug_grp", ca_pcm_debug_pins),
+	GROUP("ca_pio_grp", ca_pio_pins),
+	GROUP("ca_sdio_debug_grp", ca_sdio_debug_pins),
+	GROUP("ca_spi_grp", ca_spi_pins),
+	GROUP("ca_trb_grp", ca_trb_pins),
+	GROUP("ca_uart_debug_grp", ca_uart_debug_pins),
+	GROUP("clkc_grp0", clkc_pins0),
+	GROUP("clkc_grp1", clkc_pins1),
+	GROUP("gn_gnss_i2c_grp", gn_gnss_i2c_pins),
+	GROUP("gn_gnss_uart_nopause_grp", gn_gnss_uart_nopause_pins),
+	GROUP("gn_gnss_uart_grp", gn_gnss_uart_pins),
+	GROUP("gn_trg_spi_grp0", gn_trg_spi_pins0),
+	GROUP("gn_trg_spi_grp1", gn_trg_spi_pins1),
+	GROUP("cvbs_dbg_grp", cvbs_dbg_pins),
+	GROUP("cvbs_dbg_test_grp0", cvbs_dbg_test_pins0),
+	GROUP("cvbs_dbg_test_grp1", cvbs_dbg_test_pins1),
+	GROUP("cvbs_dbg_test_grp2", cvbs_dbg_test_pins2),
+	GROUP("cvbs_dbg_test_grp3", cvbs_dbg_test_pins3),
+	GROUP("cvbs_dbg_test_grp4", cvbs_dbg_test_pins4),
+	GROUP("cvbs_dbg_test_grp5", cvbs_dbg_test_pins5),
+	GROUP("cvbs_dbg_test_grp6", cvbs_dbg_test_pins6),
+	GROUP("cvbs_dbg_test_grp7", cvbs_dbg_test_pins7),
+	GROUP("cvbs_dbg_test_grp8", cvbs_dbg_test_pins8),
+	GROUP("cvbs_dbg_test_grp9", cvbs_dbg_test_pins9),
+	GROUP("cvbs_dbg_test_grp10", cvbs_dbg_test_pins10),
+	GROUP("cvbs_dbg_test_grp11", cvbs_dbg_test_pins11),
+	GROUP("cvbs_dbg_test_grp12", cvbs_dbg_test_pins12),
+	GROUP("cvbs_dbg_test_grp13", cvbs_dbg_test_pins13),
+	GROUP("cvbs_dbg_test_grp14", cvbs_dbg_test_pins14),
+	GROUP("cvbs_dbg_test_grp15", cvbs_dbg_test_pins15),
+	GROUP("gn_gnss_power_grp", gn_gnss_power_pins),
+	GROUP("gn_gnss_sw_status_grp", gn_gnss_sw_status_pins),
+	GROUP("gn_gnss_eclk_grp", gn_gnss_eclk_pins),
+	GROUP("gn_gnss_irq1_grp0", gn_gnss_irq1_pins0),
+	GROUP("gn_gnss_irq2_grp0", gn_gnss_irq2_pins0),
+	GROUP("gn_gnss_tm_grp", gn_gnss_tm_pins),
+	GROUP("gn_gnss_tsync_grp", gn_gnss_tsync_pins),
+	GROUP("gn_io_gnsssys_sw_cfg_grp", gn_io_gnsssys_sw_cfg_pins),
+	GROUP("gn_trg_grp0", gn_trg_pins0),
+	GROUP("gn_trg_grp1", gn_trg_pins1),
+	GROUP("gn_trg_shutdown_grp0", gn_trg_shutdown_pins0),
+	GROUP("gn_trg_shutdown_grp1", gn_trg_shutdown_pins1),
+	GROUP("gn_trg_shutdown_grp2", gn_trg_shutdown_pins2),
+	GROUP("gn_trg_shutdown_grp3", gn_trg_shutdown_pins3),
+	GROUP("i2c0_grp", i2c0_pins),
+	GROUP("i2c1_grp", i2c1_pins),
+	GROUP("jtag_grp0", jtag_pins0),
+	GROUP("ks_kas_spi_grp0", ks_kas_spi_pins0),
+	GROUP("ld_ldd_grp", ld_ldd_pins),
+	GROUP("ld_ldd_16bit_grp", ld_ldd_16bit_pins),
+	GROUP("ld_ldd_fck_grp", ld_ldd_fck_pins),
+	GROUP("ld_ldd_lck_grp", ld_ldd_lck_pins),
+	GROUP("lr_lcdrom_grp", lr_lcdrom_pins),
+	GROUP("lvds_analog_grp", lvds_analog_pins),
+	GROUP("nd_df_grp", nd_df_pins),
+	GROUP("nd_df_nowp_grp", nd_df_nowp_pins),
+	GROUP("ps_grp", ps_pins),
+	GROUP("pwc_core_on_grp", pwc_core_on_pins),
+	GROUP("pwc_ext_on_grp", pwc_ext_on_pins),
+	GROUP("pwc_gpio3_clk_grp", pwc_gpio3_clk_pins),
+	GROUP("pwc_io_on_grp", pwc_io_on_pins),
+	GROUP("pwc_lowbatt_b_grp0", pwc_lowbatt_b_pins0),
+	GROUP("pwc_mem_on_grp", pwc_mem_on_pins),
+	GROUP("pwc_on_key_b_grp0", pwc_on_key_b_pins0),
+	GROUP("pwc_wakeup_src0_grp", pwc_wakeup_src0_pins),
+	GROUP("pwc_wakeup_src1_grp", pwc_wakeup_src1_pins),
+	GROUP("pwc_wakeup_src2_grp", pwc_wakeup_src2_pins),
+	GROUP("pwc_wakeup_src3_grp", pwc_wakeup_src3_pins),
+	GROUP("pw_cko0_grp0", pw_cko0_pins0),
+	GROUP("pw_cko0_grp1", pw_cko0_pins1),
+	GROUP("pw_cko0_grp2", pw_cko0_pins2),
+	GROUP("pw_cko1_grp0", pw_cko1_pins0),
+	GROUP("pw_cko1_grp1", pw_cko1_pins1),
+	GROUP("pw_i2s01_clk_grp0", pw_i2s01_clk_pins0),
+	GROUP("pw_i2s01_clk_grp1", pw_i2s01_clk_pins1),
+	GROUP("pw_pwm0_grp", pw_pwm0_pins),
+	GROUP("pw_pwm1_grp", pw_pwm1_pins),
+	GROUP("pw_pwm2_grp0", pw_pwm2_pins0),
+	GROUP("pw_pwm2_grp1", pw_pwm2_pins1),
+	GROUP("pw_pwm3_grp0", pw_pwm3_pins0),
+	GROUP("pw_pwm3_grp1", pw_pwm3_pins1),
+	GROUP("pw_pwm_cpu_vol_grp0", pw_pwm_cpu_vol_pins0),
+	GROUP("pw_pwm_cpu_vol_grp1", pw_pwm_cpu_vol_pins1),
+	GROUP("pw_backlight_grp0", pw_backlight_pins0),
+	GROUP("pw_backlight_grp1", pw_backlight_pins1),
+	GROUP("rg_eth_mac_grp", rg_eth_mac_pins),
+	GROUP("rg_gmac_phy_intr_n_grp", rg_gmac_phy_intr_n_pins),
+	GROUP("rg_rgmii_mac_grp", rg_rgmii_mac_pins),
+	GROUP("rg_rgmii_phy_ref_clk_grp0", rg_rgmii_phy_ref_clk_pins0),
+	GROUP("rg_rgmii_phy_ref_clk_grp1", rg_rgmii_phy_ref_clk_pins1),
+	GROUP("sd0_grp", sd0_pins),
+	GROUP("sd0_4bit_grp", sd0_4bit_pins),
+	GROUP("sd1_grp", sd1_pins),
+	GROUP("sd1_4bit_grp0", sd1_4bit_pins0),
+	GROUP("sd1_4bit_grp1", sd1_4bit_pins1),
+	GROUP("sd2_grp0", sd2_pins0),
+	GROUP("sd2_no_cdb_grp0", sd2_no_cdb_pins0),
+	GROUP("sd3_grp", sd3_pins),
+	GROUP("sd5_grp", sd5_pins),
+	GROUP("sd6_grp0", sd6_pins0),
+	GROUP("sd6_grp1", sd6_pins1),
+	GROUP("sp0_ext_ldo_on_grp", sp0_ext_ldo_on_pins),
+	GROUP("sp0_qspi_grp", sp0_qspi_pins),
+	GROUP("sp1_spi_grp", sp1_spi_pins),
+	GROUP("tpiu_trace_grp", tpiu_trace_pins),
+	GROUP("uart0_grp", uart0_pins),
+	GROUP("uart0_nopause_grp", uart0_nopause_pins),
+	GROUP("uart1_grp", uart1_pins),
+	GROUP("uart2_grp", uart2_pins),
+	GROUP("uart3_grp0", uart3_pins0),
+	GROUP("uart3_grp1", uart3_pins1),
+	GROUP("uart3_grp2", uart3_pins2),
+	GROUP("uart3_grp3", uart3_pins3),
+	GROUP("uart3_nopause_grp0", uart3_nopause_pins0),
+	GROUP("uart3_nopause_grp1", uart3_nopause_pins1),
+	GROUP("uart4_grp0", uart4_pins0),
+	GROUP("uart4_grp1", uart4_pins1),
+	GROUP("uart4_grp2", uart4_pins2),
+	GROUP("uart4_nopause_grp", uart4_nopause_pins),
+	GROUP("usb0_drvvbus_grp", usb0_drvvbus_pins),
+	GROUP("usb1_drvvbus_grp", usb1_drvvbus_pins),
+	GROUP("visbus_dout_grp", visbus_dout_pins),
+	GROUP("vi_vip1_grp", vi_vip1_pins),
+	GROUP("vi_vip1_ext_grp", vi_vip1_ext_pins),
+	GROUP("vi_vip1_low8bit_grp", vi_vip1_low8bit_pins),
+	GROUP("vi_vip1_high8bit_grp", vi_vip1_high8bit_pins),
+};
+
+/* How many groups that a function can use */
+static const char * const gnss_gpio_grp[] = { "gnss_gpio_grp", };
+static const char * const lcd_vip_gpio_grp[] = { "lcd_vip_gpio_grp", };
+static const char * const sdio_i2s_gpio_grp[] = { "sdio_i2s_gpio_grp", };
+static const char * const sp_rgmii_gpio_grp[] = { "sp_rgmii_gpio_grp", };
+static const char * const lvds_gpio_grp[] = { "lvds_gpio_grp", };
+static const char * const uart_nand_gpio_grp[] = { "uart_nand_gpio_grp", };
+static const char * const rtc_gpio_grp[] = { "rtc_gpio_grp", };
+static const char * const audio_ac97_grp[] = { "audio_ac97_grp", };
+static const char * const audio_func_dbg_grp[] = { "audio_func_dbg_grp", };
+static const char * const audio_i2s_grp[] = { "audio_i2s_grp", };
+static const char * const audio_i2s_2ch_grp[] = { "audio_i2s_2ch_grp", };
+static const char * const audio_i2s_extclk_grp[] = { "audio_i2s_extclk_grp", };
+static const char * const audio_uart0_grp[] = { "audio_uart0_grp", };
+static const char * const audio_uart1_grp[] = { "audio_uart1_grp", };
+static const char * const audio_uart2_grp0[] = { "audio_uart2_grp0", };
+static const char * const audio_uart2_grp1[] = { "audio_uart2_grp1", };
+static const char * const c_can_trnsvr_grp[] = { "c_can_trnsvr_grp", };
+static const char * const c0_can_grp0[] = { "c0_can_grp0", };
+static const char * const c0_can_grp1[] = { "c0_can_grp1", };
+static const char * const c1_can_grp0[] = { "c1_can_grp0", };
+static const char * const c1_can_grp1[] = { "c1_can_grp1", };
+static const char * const c1_can_grp2[] = { "c1_can_grp2", };
+static const char * const ca_audio_lpc_grp[] = { "ca_audio_lpc_grp", };
+static const char * const ca_bt_lpc_grp[] = { "ca_bt_lpc_grp", };
+static const char * const ca_coex_grp[] = { "ca_coex_grp", };
+static const char * const ca_curator_lpc_grp[] = { "ca_curator_lpc_grp", };
+static const char * const ca_pcm_debug_grp[] = { "ca_pcm_debug_grp", };
+static const char * const ca_pio_grp[] = { "ca_pio_grp", };
+static const char * const ca_sdio_debug_grp[] = { "ca_sdio_debug_grp", };
+static const char * const ca_spi_grp[] = { "ca_spi_grp", };
+static const char * const ca_trb_grp[] = { "ca_trb_grp", };
+static const char * const ca_uart_debug_grp[] = { "ca_uart_debug_grp", };
+static const char * const clkc_grp0[] = { "clkc_grp0", };
+static const char * const clkc_grp1[] = { "clkc_grp1", };
+static const char * const gn_gnss_i2c_grp[] = { "gn_gnss_i2c_grp", };
+static const char * const gn_gnss_uart_nopause_grp[] = {
+				"gn_gnss_uart_nopause_grp", };
+static const char * const gn_gnss_uart_grp[] = { "gn_gnss_uart_grp", };
+static const char * const gn_trg_spi_grp0[] = { "gn_trg_spi_grp0", };
+static const char * const gn_trg_spi_grp1[] = { "gn_trg_spi_grp1", };
+static const char * const cvbs_dbg_grp[] = { "cvbs_dbg_grp", };
+static const char * const cvbs_dbg_test_grp0[] = { "cvbs_dbg_test_grp0", };
+static const char * const cvbs_dbg_test_grp1[] = { "cvbs_dbg_test_grp1", };
+static const char * const cvbs_dbg_test_grp2[] = { "cvbs_dbg_test_grp2", };
+static const char * const cvbs_dbg_test_grp3[] = { "cvbs_dbg_test_grp3", };
+static const char * const cvbs_dbg_test_grp4[] = { "cvbs_dbg_test_grp4", };
+static const char * const cvbs_dbg_test_grp5[] = { "cvbs_dbg_test_grp5", };
+static const char * const cvbs_dbg_test_grp6[] = { "cvbs_dbg_test_grp6", };
+static const char * const cvbs_dbg_test_grp7[] = { "cvbs_dbg_test_grp7", };
+static const char * const cvbs_dbg_test_grp8[] = { "cvbs_dbg_test_grp8", };
+static const char * const cvbs_dbg_test_grp9[] = { "cvbs_dbg_test_grp9", };
+static const char * const cvbs_dbg_test_grp10[] = { "cvbs_dbg_test_grp10", };
+static const char * const cvbs_dbg_test_grp11[] = { "cvbs_dbg_test_grp11", };
+static const char * const cvbs_dbg_test_grp12[] = { "cvbs_dbg_test_grp12", };
+static const char * const cvbs_dbg_test_grp13[] = { "cvbs_dbg_test_grp13", };
+static const char * const cvbs_dbg_test_grp14[] = { "cvbs_dbg_test_grp14", };
+static const char * const cvbs_dbg_test_grp15[] = { "cvbs_dbg_test_grp15", };
+static const char * const gn_gnss_power_grp[] = { "gn_gnss_power_grp", };
+static const char * const gn_gnss_sw_status_grp[] = {
+				"gn_gnss_sw_status_grp", };
+static const char * const gn_gnss_eclk_grp[] = { "gn_gnss_eclk_grp", };
+static const char * const gn_gnss_irq1_grp0[] = { "gn_gnss_irq1_grp0", };
+static const char * const gn_gnss_irq2_grp0[] = { "gn_gnss_irq2_grp0", };
+static const char * const gn_gnss_tm_grp[] = { "gn_gnss_tm_grp", };
+static const char * const gn_gnss_tsync_grp[] = { "gn_gnss_tsync_grp", };
+static const char * const gn_io_gnsssys_sw_cfg_grp[] = {
+				"gn_io_gnsssys_sw_cfg_grp", };
+static const char * const gn_trg_grp0[] = { "gn_trg_grp0", };
+static const char * const gn_trg_grp1[] = { "gn_trg_grp1", };
+static const char * const gn_trg_shutdown_grp0[] = { "gn_trg_shutdown_grp0", };
+static const char * const gn_trg_shutdown_grp1[] = { "gn_trg_shutdown_grp1", };
+static const char * const gn_trg_shutdown_grp2[] = { "gn_trg_shutdown_grp2", };
+static const char * const gn_trg_shutdown_grp3[] = { "gn_trg_shutdown_grp3", };
+static const char * const i2c0_grp[] = { "i2c0_grp", };
+static const char * const i2c1_grp[] = { "i2c1_grp", };
+static const char * const jtag_grp0[] = { "jtag_grp0", };
+static const char * const ks_kas_spi_grp0[] = { "ks_kas_spi_grp0", };
+static const char * const ld_ldd_grp[] = { "ld_ldd_grp", };
+static const char * const ld_ldd_16bit_grp[] = { "ld_ldd_16bit_grp", };
+static const char * const ld_ldd_fck_grp[] = { "ld_ldd_fck_grp", };
+static const char * const ld_ldd_lck_grp[] = { "ld_ldd_lck_grp", };
+static const char * const lr_lcdrom_grp[] = { "lr_lcdrom_grp", };
+static const char * const lvds_analog_grp[] = { "lvds_analog_grp", };
+static const char * const nd_df_grp[] = { "nd_df_grp", };
+static const char * const nd_df_nowp_grp[] = { "nd_df_nowp_grp", };
+static const char * const ps_grp[] = { "ps_grp", };
+static const char * const pwc_core_on_grp[] = { "pwc_core_on_grp", };
+static const char * const pwc_ext_on_grp[] = { "pwc_ext_on_grp", };
+static const char * const pwc_gpio3_clk_grp[] = { "pwc_gpio3_clk_grp", };
+static const char * const pwc_io_on_grp[] = { "pwc_io_on_grp", };
+static const char * const pwc_lowbatt_b_grp0[] = { "pwc_lowbatt_b_grp0", };
+static const char * const pwc_mem_on_grp[] = { "pwc_mem_on_grp", };
+static const char * const pwc_on_key_b_grp0[] = { "pwc_on_key_b_grp0", };
+static const char * const pwc_wakeup_src0_grp[] = { "pwc_wakeup_src0_grp", };
+static const char * const pwc_wakeup_src1_grp[] = { "pwc_wakeup_src1_grp", };
+static const char * const pwc_wakeup_src2_grp[] = { "pwc_wakeup_src2_grp", };
+static const char * const pwc_wakeup_src3_grp[] = { "pwc_wakeup_src3_grp", };
+static const char * const pw_cko0_grp0[] = { "pw_cko0_grp0", };
+static const char * const pw_cko0_grp1[] = { "pw_cko0_grp1", };
+static const char * const pw_cko0_grp2[] = { "pw_cko0_grp2", };
+static const char * const pw_cko1_grp0[] = { "pw_cko1_grp0", };
+static const char * const pw_cko1_grp1[] = { "pw_cko1_grp1", };
+static const char * const pw_i2s01_clk_grp0[] = { "pw_i2s01_clk_grp0", };
+static const char * const pw_i2s01_clk_grp1[] = { "pw_i2s01_clk_grp1", };
+static const char * const pw_pwm0_grp[] = { "pw_pwm0_grp", };
+static const char * const pw_pwm1_grp[] = { "pw_pwm1_grp", };
+static const char * const pw_pwm2_grp0[] = { "pw_pwm2_grp0", };
+static const char * const pw_pwm2_grp1[] = { "pw_pwm2_grp1", };
+static const char * const pw_pwm3_grp0[] = { "pw_pwm3_grp0", };
+static const char * const pw_pwm3_grp1[] = { "pw_pwm3_grp1", };
+static const char * const pw_pwm_cpu_vol_grp0[] = { "pw_pwm_cpu_vol_grp0", };
+static const char * const pw_pwm_cpu_vol_grp1[] = { "pw_pwm_cpu_vol_grp1", };
+static const char * const pw_backlight_grp0[] = { "pw_backlight_grp0", };
+static const char * const pw_backlight_grp1[] = { "pw_backlight_grp1", };
+static const char * const rg_eth_mac_grp[] = { "rg_eth_mac_grp", };
+static const char * const rg_gmac_phy_intr_n_grp[] = {
+				"rg_gmac_phy_intr_n_grp", };
+static const char * const rg_rgmii_mac_grp[] = { "rg_rgmii_mac_grp", };
+static const char * const rg_rgmii_phy_ref_clk_grp0[] = {
+				"rg_rgmii_phy_ref_clk_grp0", };
+static const char * const rg_rgmii_phy_ref_clk_grp1[] = {
+				"rg_rgmii_phy_ref_clk_grp1", };
+static const char * const sd0_grp[] = { "sd0_grp", };
+static const char * const sd0_4bit_grp[] = { "sd0_4bit_grp", };
+static const char * const sd1_grp[] = { "sd1_grp", };
+static const char * const sd1_4bit_grp0[] = { "sd1_4bit_grp0", };
+static const char * const sd1_4bit_grp1[] = { "sd1_4bit_grp1", };
+static const char * const sd2_grp0[] = { "sd2_grp0", };
+static const char * const sd2_no_cdb_grp0[] = { "sd2_no_cdb_grp0", };
+static const char * const sd3_grp[] = { "sd3_grp", };
+static const char * const sd5_grp[] = { "sd5_grp", };
+static const char * const sd6_grp0[] = { "sd6_grp0", };
+static const char * const sd6_grp1[] = { "sd6_grp1", };
+static const char * const sp0_ext_ldo_on_grp[] = { "sp0_ext_ldo_on_grp", };
+static const char * const sp0_qspi_grp[] = { "sp0_qspi_grp", };
+static const char * const sp1_spi_grp[] = { "sp1_spi_grp", };
+static const char * const tpiu_trace_grp[] = { "tpiu_trace_grp", };
+static const char * const uart0_grp[] = { "uart0_grp", };
+static const char * const uart0_nopause_grp[] = { "uart0_nopause_grp", };
+static const char * const uart1_grp[] = { "uart1_grp", };
+static const char * const uart2_grp[] = { "uart2_grp", };
+static const char * const uart3_grp0[] = { "uart3_grp0", };
+static const char * const uart3_grp1[] = { "uart3_grp1", };
+static const char * const uart3_grp2[] = { "uart3_grp2", };
+static const char * const uart3_grp3[] = { "uart3_grp3", };
+static const char * const uart3_nopause_grp0[] = { "uart3_nopause_grp0", };
+static const char * const uart3_nopause_grp1[] = { "uart3_nopause_grp1", };
+static const char * const uart4_grp0[] = { "uart4_grp0", };
+static const char * const uart4_grp1[] = { "uart4_grp1", };
+static const char * const uart4_grp2[] = { "uart4_grp2", };
+static const char * const uart4_nopause_grp[] = { "uart4_nopause_grp", };
+static const char * const usb0_drvvbus_grp[] = { "usb0_drvvbus_grp", };
+static const char * const usb1_drvvbus_grp[] = { "usb1_drvvbus_grp", };
+static const char * const visbus_dout_grp[] = { "visbus_dout_grp", };
+static const char * const vi_vip1_grp[] = { "vi_vip1_grp", };
+static const char * const vi_vip1_ext_grp[] = { "vi_vip1_ext_grp", };
+static const char * const vi_vip1_low8bit_grp[] = { "vi_vip1_low8bit_grp", };
+static const char * const vi_vip1_high8bit_grp[] = { "vi_vip1_high8bit_grp", };
+
+static struct atlas7_pad_mux gnss_gpio_grp_pad_mux[] = {
+	MUX(1, 119, 0, N, N, N, N),
+	MUX(1, 120, 0, N, N, N, N),
+	MUX(1, 121, 0, N, N, N, N),
+	MUX(1, 122, 0, N, N, N, N),
+	MUX(1, 123, 0, N, N, N, N),
+	MUX(1, 124, 0, N, N, N, N),
+	MUX(1, 125, 0, N, N, N, N),
+	MUX(1, 126, 0, N, N, N, N),
+	MUX(1, 127, 0, N, N, N, N),
+	MUX(1, 128, 0, N, N, N, N),
+	MUX(1, 22, 0, N, N, N, N),
+	MUX(1, 23, 0, N, N, N, N),
+	MUX(1, 24, 0, N, N, N, N),
+	MUX(1, 25, 0, N, N, N, N),
+	MUX(1, 26, 0, N, N, N, N),
+	MUX(1, 27, 0, N, N, N, N),
+	MUX(1, 28, 0, N, N, N, N),
+	MUX(1, 29, 0, N, N, N, N),
+	MUX(1, 30, 0, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gnss_gpio_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gnss_gpio_grp_pad_mux),
+	.pad_mux_list = gnss_gpio_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux lcd_vip_gpio_grp_pad_mux[] = {
+	MUX(1, 74, 0, N, N, N, N),
+	MUX(1, 75, 0, N, N, N, N),
+	MUX(1, 76, 0, N, N, N, N),
+	MUX(1, 77, 0, N, N, N, N),
+	MUX(1, 78, 0, N, N, N, N),
+	MUX(1, 79, 0, N, N, N, N),
+	MUX(1, 80, 0, N, N, N, N),
+	MUX(1, 81, 0, N, N, N, N),
+	MUX(1, 82, 0, N, N, N, N),
+	MUX(1, 83, 0, N, N, N, N),
+	MUX(1, 84, 0, N, N, N, N),
+	MUX(1, 53, 0, N, N, N, N),
+	MUX(1, 54, 0, N, N, N, N),
+	MUX(1, 55, 0, N, N, N, N),
+	MUX(1, 56, 0, N, N, N, N),
+	MUX(1, 57, 0, N, N, N, N),
+	MUX(1, 58, 0, N, N, N, N),
+	MUX(1, 59, 0, N, N, N, N),
+	MUX(1, 60, 0, N, N, N, N),
+	MUX(1, 61, 0, N, N, N, N),
+	MUX(1, 62, 0, N, N, N, N),
+	MUX(1, 63, 0, N, N, N, N),
+	MUX(1, 64, 0, N, N, N, N),
+	MUX(1, 65, 0, N, N, N, N),
+	MUX(1, 66, 0, N, N, N, N),
+	MUX(1, 67, 0, N, N, N, N),
+	MUX(1, 68, 0, N, N, N, N),
+	MUX(1, 69, 0, N, N, N, N),
+	MUX(1, 70, 0, N, N, N, N),
+	MUX(1, 71, 0, N, N, N, N),
+	MUX(1, 72, 0, N, N, N, N),
+	MUX(1, 73, 0, N, N, N, N),
+};
+
+static struct atlas7_grp_mux lcd_vip_gpio_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(lcd_vip_gpio_grp_pad_mux),
+	.pad_mux_list = lcd_vip_gpio_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sdio_i2s_gpio_grp_pad_mux[] = {
+	MUX(1, 31, 0, N, N, N, N),
+	MUX(1, 32, 0, N, N, N, N),
+	MUX(1, 33, 0, N, N, N, N),
+	MUX(1, 34, 0, N, N, N, N),
+	MUX(1, 35, 0, N, N, N, N),
+	MUX(1, 36, 0, N, N, N, N),
+	MUX(1, 85, 0, N, N, N, N),
+	MUX(1, 86, 0, N, N, N, N),
+	MUX(1, 87, 0, N, N, N, N),
+	MUX(1, 88, 0, N, N, N, N),
+	MUX(1, 89, 0, N, N, N, N),
+	MUX(1, 90, 0, N, N, N, N),
+	MUX(1, 129, 0, N, N, N, N),
+	MUX(1, 130, 0, N, N, N, N),
+	MUX(1, 131, 0, N, N, N, N),
+	MUX(1, 132, 0, N, N, N, N),
+	MUX(1, 91, 0, N, N, N, N),
+	MUX(1, 92, 0, N, N, N, N),
+	MUX(1, 93, 0, N, N, N, N),
+	MUX(1, 94, 0, N, N, N, N),
+	MUX(1, 95, 0, N, N, N, N),
+	MUX(1, 96, 0, N, N, N, N),
+	MUX(1, 112, 0, N, N, N, N),
+	MUX(1, 113, 0, N, N, N, N),
+	MUX(1, 114, 0, N, N, N, N),
+	MUX(1, 115, 0, N, N, N, N),
+	MUX(1, 116, 0, N, N, N, N),
+	MUX(1, 117, 0, N, N, N, N),
+	MUX(1, 118, 0, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sdio_i2s_gpio_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sdio_i2s_gpio_grp_pad_mux),
+	.pad_mux_list = sdio_i2s_gpio_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sp_rgmii_gpio_grp_pad_mux[] = {
+	MUX(1, 97, 0, N, N, N, N),
+	MUX(1, 98, 0, N, N, N, N),
+	MUX(1, 99, 0, N, N, N, N),
+	MUX(1, 100, 0, N, N, N, N),
+	MUX(1, 101, 0, N, N, N, N),
+	MUX(1, 102, 0, N, N, N, N),
+	MUX(1, 103, 0, N, N, N, N),
+	MUX(1, 104, 0, N, N, N, N),
+	MUX(1, 105, 0, N, N, N, N),
+	MUX(1, 106, 0, N, N, N, N),
+	MUX(1, 107, 0, N, N, N, N),
+	MUX(1, 108, 0, N, N, N, N),
+	MUX(1, 109, 0, N, N, N, N),
+	MUX(1, 110, 0, N, N, N, N),
+	MUX(1, 111, 0, N, N, N, N),
+	MUX(1, 18, 0, N, N, N, N),
+	MUX(1, 19, 0, N, N, N, N),
+	MUX(1, 20, 0, N, N, N, N),
+	MUX(1, 21, 0, N, N, N, N),
+	MUX(1, 141, 0, N, N, N, N),
+	MUX(1, 142, 0, N, N, N, N),
+	MUX(1, 143, 0, N, N, N, N),
+	MUX(1, 144, 0, N, N, N, N),
+	MUX(1, 145, 0, N, N, N, N),
+	MUX(1, 146, 0, N, N, N, N),
+	MUX(1, 147, 0, N, N, N, N),
+	MUX(1, 148, 0, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sp_rgmii_gpio_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sp_rgmii_gpio_grp_pad_mux),
+	.pad_mux_list = sp_rgmii_gpio_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux lvds_gpio_grp_pad_mux[] = {
+	MUX(1, 157, 0, N, N, N, N),
+	MUX(1, 158, 0, N, N, N, N),
+	MUX(1, 155, 0, N, N, N, N),
+	MUX(1, 156, 0, N, N, N, N),
+	MUX(1, 153, 0, N, N, N, N),
+	MUX(1, 154, 0, N, N, N, N),
+	MUX(1, 151, 0, N, N, N, N),
+	MUX(1, 152, 0, N, N, N, N),
+	MUX(1, 149, 0, N, N, N, N),
+	MUX(1, 150, 0, N, N, N, N),
+};
+
+static struct atlas7_grp_mux lvds_gpio_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(lvds_gpio_grp_pad_mux),
+	.pad_mux_list = lvds_gpio_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux uart_nand_gpio_grp_pad_mux[] = {
+	MUX(1, 44, 0, N, N, N, N),
+	MUX(1, 43, 0, N, N, N, N),
+	MUX(1, 42, 0, N, N, N, N),
+	MUX(1, 41, 0, N, N, N, N),
+	MUX(1, 40, 0, N, N, N, N),
+	MUX(1, 39, 0, N, N, N, N),
+	MUX(1, 38, 0, N, N, N, N),
+	MUX(1, 37, 0, N, N, N, N),
+	MUX(1, 46, 0, N, N, N, N),
+	MUX(1, 47, 0, N, N, N, N),
+	MUX(1, 48, 0, N, N, N, N),
+	MUX(1, 49, 0, N, N, N, N),
+	MUX(1, 50, 0, N, N, N, N),
+	MUX(1, 52, 0, N, N, N, N),
+	MUX(1, 51, 0, N, N, N, N),
+	MUX(1, 45, 0, N, N, N, N),
+	MUX(1, 133, 0, N, N, N, N),
+	MUX(1, 134, 0, N, N, N, N),
+	MUX(1, 135, 0, N, N, N, N),
+	MUX(1, 136, 0, N, N, N, N),
+	MUX(1, 137, 0, N, N, N, N),
+	MUX(1, 138, 0, N, N, N, N),
+	MUX(1, 139, 0, N, N, N, N),
+	MUX(1, 140, 0, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart_nand_gpio_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart_nand_gpio_grp_pad_mux),
+	.pad_mux_list = uart_nand_gpio_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux rtc_gpio_grp_pad_mux[] = {
+	MUX(0, 0, 0, N, N, N, N),
+	MUX(0, 1, 0, N, N, N, N),
+	MUX(0, 2, 0, N, N, N, N),
+	MUX(0, 3, 0, N, N, N, N),
+	MUX(0, 4, 0, N, N, N, N),
+	MUX(0, 10, 0, N, N, N, N),
+	MUX(0, 11, 0, N, N, N, N),
+	MUX(0, 12, 0, N, N, N, N),
+	MUX(0, 13, 0, N, N, N, N),
+	MUX(0, 14, 0, N, N, N, N),
+	MUX(0, 15, 0, N, N, N, N),
+	MUX(0, 16, 0, N, N, N, N),
+	MUX(0, 17, 0, N, N, N, N),
+};
+
+static struct atlas7_grp_mux rtc_gpio_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(rtc_gpio_grp_pad_mux),
+	.pad_mux_list = rtc_gpio_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_ac97_grp_pad_mux[] = {
+	MUX(1, 113, 2, N, N, N, N),
+	MUX(1, 118, 2, N, N, N, N),
+	MUX(1, 115, 2, N, N, N, N),
+	MUX(1, 114, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux audio_ac97_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_ac97_grp_pad_mux),
+	.pad_mux_list = audio_ac97_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_func_dbg_grp_pad_mux[] = {
+	MUX(1, 141, 4, N, N, N, N),
+	MUX(1, 144, 4, N, N, N, N),
+	MUX(1, 44, 6, N, N, N, N),
+	MUX(1, 43, 6, N, N, N, N),
+	MUX(1, 42, 6, N, N, N, N),
+	MUX(1, 41, 6, N, N, N, N),
+	MUX(1, 40, 6, N, N, N, N),
+	MUX(1, 39, 6, N, N, N, N),
+	MUX(1, 38, 6, N, N, N, N),
+	MUX(1, 37, 6, N, N, N, N),
+	MUX(1, 74, 6, N, N, N, N),
+	MUX(1, 75, 6, N, N, N, N),
+	MUX(1, 76, 6, N, N, N, N),
+	MUX(1, 77, 6, N, N, N, N),
+	MUX(1, 78, 6, N, N, N, N),
+	MUX(1, 79, 6, N, N, N, N),
+	MUX(1, 81, 6, N, N, N, N),
+	MUX(1, 113, 6, N, N, N, N),
+	MUX(1, 114, 6, N, N, N, N),
+	MUX(1, 118, 6, N, N, N, N),
+	MUX(1, 115, 6, N, N, N, N),
+	MUX(1, 49, 6, N, N, N, N),
+	MUX(1, 50, 6, N, N, N, N),
+	MUX(1, 142, 4, N, N, N, N),
+	MUX(1, 143, 4, N, N, N, N),
+	MUX(1, 80, 6, N, N, N, N),
+};
+
+static struct atlas7_grp_mux audio_func_dbg_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_func_dbg_grp_pad_mux),
+	.pad_mux_list = audio_func_dbg_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_i2s_grp_pad_mux[] = {
+	MUX(1, 118, 1, N, N, N, N),
+	MUX(1, 115, 1, N, N, N, N),
+	MUX(1, 116, 1, N, N, N, N),
+	MUX(1, 117, 1, N, N, N, N),
+	MUX(1, 112, 1, N, N, N, N),
+	MUX(1, 113, 1, N, N, N, N),
+	MUX(1, 114, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux audio_i2s_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_i2s_grp_pad_mux),
+	.pad_mux_list = audio_i2s_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_i2s_2ch_grp_pad_mux[] = {
+	MUX(1, 118, 1, N, N, N, N),
+	MUX(1, 115, 1, N, N, N, N),
+	MUX(1, 112, 1, N, N, N, N),
+	MUX(1, 113, 1, N, N, N, N),
+	MUX(1, 114, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux audio_i2s_2ch_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_i2s_2ch_grp_pad_mux),
+	.pad_mux_list = audio_i2s_2ch_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_i2s_extclk_grp_pad_mux[] = {
+	MUX(1, 112, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux audio_i2s_extclk_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_i2s_extclk_grp_pad_mux),
+	.pad_mux_list = audio_i2s_extclk_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_uart0_grp_pad_mux[] = {
+	MUX(1, 143, 1, N, N, N, N),
+	MUX(1, 142, 1, N, N, N, N),
+	MUX(1, 141, 1, N, N, N, N),
+	MUX(1, 144, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux audio_uart0_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_uart0_grp_pad_mux),
+	.pad_mux_list = audio_uart0_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_uart1_grp_pad_mux[] = {
+	MUX(1, 147, 1, N, N, N, N),
+	MUX(1, 146, 1, N, N, N, N),
+	MUX(1, 145, 1, N, N, N, N),
+	MUX(1, 148, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux audio_uart1_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_uart1_grp_pad_mux),
+	.pad_mux_list = audio_uart1_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_uart2_grp0_pad_mux[] = {
+	MUX(1, 20, 2, 0xa00, 24, 0xa80, 24),
+	MUX(1, 21, 2, 0xa00, 25, 0xa80, 25),
+	MUX(1, 19, 2, 0xa00, 23, 0xa80, 23),
+	MUX(1, 18, 2, 0xa00, 22, 0xa80, 22),
+};
+
+static struct atlas7_grp_mux audio_uart2_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_uart2_grp0_pad_mux),
+	.pad_mux_list = audio_uart2_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux audio_uart2_grp1_pad_mux[] = {
+	MUX(1, 109, 2, 0xa00, 24, 0xa80, 24),
+	MUX(1, 110, 2, 0xa00, 25, 0xa80, 25),
+	MUX(1, 101, 2, 0xa00, 23, 0xa80, 23),
+	MUX(1, 111, 2, 0xa00, 22, 0xa80, 22),
+};
+
+static struct atlas7_grp_mux audio_uart2_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(audio_uart2_grp1_pad_mux),
+	.pad_mux_list = audio_uart2_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux c_can_trnsvr_grp_pad_mux[] = {
+	MUX(0, 1, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux c_can_trnsvr_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(c_can_trnsvr_grp_pad_mux),
+	.pad_mux_list = c_can_trnsvr_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux c0_can_grp0_pad_mux[] = {
+	MUX(0, 11, 1, 0xa08, 9, 0xa88, 9),
+	MUX(0, 10, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux c0_can_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(c0_can_grp0_pad_mux),
+	.pad_mux_list = c0_can_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux c0_can_grp1_pad_mux[] = {
+	MUX(0, 2, 5, 0xa08, 9, 0xa88, 9),
+	MUX(0, 3, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux c0_can_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(c0_can_grp1_pad_mux),
+	.pad_mux_list = c0_can_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux c1_can_grp0_pad_mux[] = {
+	MUX(1, 138, 2, 0xa00, 4, 0xa80, 4),
+	MUX(1, 137, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux c1_can_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(c1_can_grp0_pad_mux),
+	.pad_mux_list = c1_can_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux c1_can_grp1_pad_mux[] = {
+	MUX(1, 147, 2, 0xa00, 4, 0xa80, 4),
+	MUX(1, 146, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux c1_can_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(c1_can_grp1_pad_mux),
+	.pad_mux_list = c1_can_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux c1_can_grp2_pad_mux[] = {
+	MUX(0, 2, 2, 0xa00, 4, 0xa80, 4),
+	MUX(0, 3, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux c1_can_grp2_mux = {
+	.pad_mux_count = ARRAY_SIZE(c1_can_grp2_pad_mux),
+	.pad_mux_list = c1_can_grp2_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_audio_lpc_grp_pad_mux[] = {
+	MUX(1, 62, 4, N, N, N, N),
+	MUX(1, 63, 4, N, N, N, N),
+	MUX(1, 64, 4, N, N, N, N),
+	MUX(1, 65, 4, N, N, N, N),
+	MUX(1, 66, 4, N, N, N, N),
+	MUX(1, 67, 4, N, N, N, N),
+	MUX(1, 68, 4, N, N, N, N),
+	MUX(1, 69, 4, N, N, N, N),
+	MUX(1, 70, 4, N, N, N, N),
+	MUX(1, 71, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_audio_lpc_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_audio_lpc_grp_pad_mux),
+	.pad_mux_list = ca_audio_lpc_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_bt_lpc_grp_pad_mux[] = {
+	MUX(1, 85, 5, N, N, N, N),
+	MUX(1, 86, 5, N, N, N, N),
+	MUX(1, 87, 5, N, N, N, N),
+	MUX(1, 88, 5, N, N, N, N),
+	MUX(1, 89, 5, N, N, N, N),
+	MUX(1, 90, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_bt_lpc_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_bt_lpc_grp_pad_mux),
+	.pad_mux_list = ca_bt_lpc_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_coex_grp_pad_mux[] = {
+	MUX(1, 129, 1, N, N, N, N),
+	MUX(1, 130, 1, N, N, N, N),
+	MUX(1, 131, 1, N, N, N, N),
+	MUX(1, 132, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_coex_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_coex_grp_pad_mux),
+	.pad_mux_list = ca_coex_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_curator_lpc_grp_pad_mux[] = {
+	MUX(1, 57, 4, N, N, N, N),
+	MUX(1, 58, 4, N, N, N, N),
+	MUX(1, 59, 4, N, N, N, N),
+	MUX(1, 60, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_curator_lpc_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_curator_lpc_grp_pad_mux),
+	.pad_mux_list = ca_curator_lpc_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_pcm_debug_grp_pad_mux[] = {
+	MUX(1, 91, 5, N, N, N, N),
+	MUX(1, 93, 5, N, N, N, N),
+	MUX(1, 94, 5, N, N, N, N),
+	MUX(1, 92, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_pcm_debug_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_pcm_debug_grp_pad_mux),
+	.pad_mux_list = ca_pcm_debug_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_pio_grp_pad_mux[] = {
+	MUX(1, 121, 2, N, N, N, N),
+	MUX(1, 122, 2, N, N, N, N),
+	MUX(1, 125, 6, N, N, N, N),
+	MUX(1, 126, 6, N, N, N, N),
+	MUX(1, 38, 5, N, N, N, N),
+	MUX(1, 37, 5, N, N, N, N),
+	MUX(1, 47, 5, N, N, N, N),
+	MUX(1, 49, 5, N, N, N, N),
+	MUX(1, 50, 5, N, N, N, N),
+	MUX(1, 54, 4, N, N, N, N),
+	MUX(1, 55, 4, N, N, N, N),
+	MUX(1, 56, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_pio_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_pio_grp_pad_mux),
+	.pad_mux_list = ca_pio_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_sdio_debug_grp_pad_mux[] = {
+	MUX(1, 40, 5, N, N, N, N),
+	MUX(1, 39, 5, N, N, N, N),
+	MUX(1, 44, 5, N, N, N, N),
+	MUX(1, 43, 5, N, N, N, N),
+	MUX(1, 42, 5, N, N, N, N),
+	MUX(1, 41, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_sdio_debug_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_sdio_debug_grp_pad_mux),
+	.pad_mux_list = ca_sdio_debug_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_spi_grp_pad_mux[] = {
+	MUX(1, 82, 5, N, N, N, N),
+	MUX(1, 79, 5, 0xa08, 6, 0xa88, 6),
+	MUX(1, 80, 5, N, N, N, N),
+	MUX(1, 81, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_spi_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_spi_grp_pad_mux),
+	.pad_mux_list = ca_spi_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_trb_grp_pad_mux[] = {
+	MUX(1, 91, 4, N, N, N, N),
+	MUX(1, 93, 4, N, N, N, N),
+	MUX(1, 94, 4, N, N, N, N),
+	MUX(1, 95, 4, N, N, N, N),
+	MUX(1, 96, 4, N, N, N, N),
+	MUX(1, 78, 5, N, N, N, N),
+	MUX(1, 74, 5, N, N, N, N),
+	MUX(1, 75, 5, N, N, N, N),
+	MUX(1, 76, 5, N, N, N, N),
+	MUX(1, 77, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_trb_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_trb_grp_pad_mux),
+	.pad_mux_list = ca_trb_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ca_uart_debug_grp_pad_mux[] = {
+	MUX(1, 136, 3, N, N, N, N),
+	MUX(1, 135, 3, N, N, N, N),
+	MUX(1, 134, 3, N, N, N, N),
+	MUX(1, 133, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ca_uart_debug_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ca_uart_debug_grp_pad_mux),
+	.pad_mux_list = ca_uart_debug_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux clkc_grp0_pad_mux[] = {
+	MUX(1, 30, 2, 0xa08, 14, 0xa88, 14),
+	MUX(1, 47, 6, N, N, N, N),
+};
+
+static struct atlas7_grp_mux clkc_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(clkc_grp0_pad_mux),
+	.pad_mux_list = clkc_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux clkc_grp1_pad_mux[] = {
+	MUX(1, 78, 3, 0xa08, 14, 0xa88, 14),
+	MUX(1, 54, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux clkc_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(clkc_grp1_pad_mux),
+	.pad_mux_list = clkc_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_i2c_grp_pad_mux[] = {
+	MUX(1, 128, 2, N, N, N, N),
+	MUX(1, 127, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_gnss_i2c_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_i2c_grp_pad_mux),
+	.pad_mux_list = gn_gnss_i2c_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_uart_nopause_grp_pad_mux[] = {
+	MUX(1, 134, 4, N, N, N, N),
+	MUX(1, 133, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_gnss_uart_nopause_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_uart_nopause_grp_pad_mux),
+	.pad_mux_list = gn_gnss_uart_nopause_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_uart_grp_pad_mux[] = {
+	MUX(1, 134, 4, N, N, N, N),
+	MUX(1, 133, 4, N, N, N, N),
+	MUX(1, 136, 4, N, N, N, N),
+	MUX(1, 135, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_gnss_uart_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_uart_grp_pad_mux),
+	.pad_mux_list = gn_gnss_uart_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_trg_spi_grp0_pad_mux[] = {
+	MUX(1, 22, 1, N, N, N, N),
+	MUX(1, 25, 1, N, N, N, N),
+	MUX(1, 23, 1, 0xa00, 10, 0xa80, 10),
+	MUX(1, 24, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_trg_spi_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_trg_spi_grp0_pad_mux),
+	.pad_mux_list = gn_trg_spi_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_trg_spi_grp1_pad_mux[] = {
+	MUX(1, 82, 3, N, N, N, N),
+	MUX(1, 79, 3, N, N, N, N),
+	MUX(1, 80, 3, 0xa00, 10, 0xa80, 10),
+	MUX(1, 81, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_trg_spi_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_trg_spi_grp1_pad_mux),
+	.pad_mux_list = gn_trg_spi_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_grp_pad_mux[] = {
+	MUX(1, 54, 3, N, N, N, N),
+	MUX(1, 53, 3, N, N, N, N),
+	MUX(1, 82, 7, N, N, N, N),
+	MUX(1, 74, 7, N, N, N, N),
+	MUX(1, 75, 7, N, N, N, N),
+	MUX(1, 76, 7, N, N, N, N),
+	MUX(1, 77, 7, N, N, N, N),
+	MUX(1, 78, 7, N, N, N, N),
+	MUX(1, 79, 7, N, N, N, N),
+	MUX(1, 80, 7, N, N, N, N),
+	MUX(1, 81, 7, N, N, N, N),
+	MUX(1, 83, 7, N, N, N, N),
+	MUX(1, 84, 7, N, N, N, N),
+	MUX(1, 73, 3, N, N, N, N),
+	MUX(1, 55, 3, N, N, N, N),
+	MUX(1, 56, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_grp_pad_mux),
+	.pad_mux_list = cvbs_dbg_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp0_pad_mux[] = {
+	MUX(1, 57, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp0_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp1_pad_mux[] = {
+	MUX(1, 58, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp1_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp2_pad_mux[] = {
+	MUX(1, 59, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp2_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp2_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp2_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp3_pad_mux[] = {
+	MUX(1, 60, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp3_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp3_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp3_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp4_pad_mux[] = {
+	MUX(1, 61, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp4_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp4_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp4_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp5_pad_mux[] = {
+	MUX(1, 62, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp5_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp5_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp5_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp6_pad_mux[] = {
+	MUX(1, 63, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp6_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp6_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp6_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp7_pad_mux[] = {
+	MUX(1, 64, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp7_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp7_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp7_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp8_pad_mux[] = {
+	MUX(1, 65, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp8_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp8_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp8_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp9_pad_mux[] = {
+	MUX(1, 66, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp9_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp9_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp9_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp10_pad_mux[] = {
+	MUX(1, 67, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp10_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp10_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp10_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp11_pad_mux[] = {
+	MUX(1, 68, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp11_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp11_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp11_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp12_pad_mux[] = {
+	MUX(1, 69, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp12_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp12_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp12_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp13_pad_mux[] = {
+	MUX(1, 70, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp13_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp13_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp13_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp14_pad_mux[] = {
+	MUX(1, 71, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp14_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp14_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp14_pad_mux,
+};
+
+static struct atlas7_pad_mux cvbs_dbg_test_grp15_pad_mux[] = {
+	MUX(1, 72, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux cvbs_dbg_test_grp15_mux = {
+	.pad_mux_count = ARRAY_SIZE(cvbs_dbg_test_grp15_pad_mux),
+	.pad_mux_list = cvbs_dbg_test_grp15_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_power_grp_pad_mux[] = {
+	MUX(1, 123, 7, N, N, N, N),
+	MUX(1, 124, 7, N, N, N, N),
+	MUX(1, 121, 7, N, N, N, N),
+	MUX(1, 122, 7, N, N, N, N),
+	MUX(1, 125, 7, N, N, N, N),
+	MUX(1, 120, 7, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_gnss_power_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_power_grp_pad_mux),
+	.pad_mux_list = gn_gnss_power_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_sw_status_grp_pad_mux[] = {
+	MUX(1, 57, 7, N, N, N, N),
+	MUX(1, 58, 7, N, N, N, N),
+	MUX(1, 59, 7, N, N, N, N),
+	MUX(1, 60, 7, N, N, N, N),
+	MUX(1, 61, 7, N, N, N, N),
+	MUX(1, 62, 7, N, N, N, N),
+	MUX(1, 63, 7, N, N, N, N),
+	MUX(1, 64, 7, N, N, N, N),
+	MUX(1, 65, 7, N, N, N, N),
+	MUX(1, 66, 7, N, N, N, N),
+	MUX(1, 67, 7, N, N, N, N),
+	MUX(1, 68, 7, N, N, N, N),
+	MUX(1, 69, 7, N, N, N, N),
+	MUX(1, 70, 7, N, N, N, N),
+	MUX(1, 71, 7, N, N, N, N),
+	MUX(1, 72, 7, N, N, N, N),
+	MUX(1, 53, 7, N, N, N, N),
+	MUX(1, 55, 7, N, N, N, N),
+	MUX(1, 56, 7, 0xa08, 12, 0xa88, 12),
+	MUX(1, 54, 7, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_gnss_sw_status_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_sw_status_grp_pad_mux),
+	.pad_mux_list = gn_gnss_sw_status_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_eclk_grp_pad_mux[] = {
+	MUX(1, 113, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_gnss_eclk_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_eclk_grp_pad_mux),
+	.pad_mux_list = gn_gnss_eclk_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_irq1_grp0_pad_mux[] = {
+	MUX(1, 112, 4, 0xa08, 10, 0xa88, 10),
+};
+
+static struct atlas7_grp_mux gn_gnss_irq1_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_irq1_grp0_pad_mux),
+	.pad_mux_list = gn_gnss_irq1_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_irq2_grp0_pad_mux[] = {
+	MUX(1, 118, 4, 0xa08, 11, 0xa88, 11),
+};
+
+static struct atlas7_grp_mux gn_gnss_irq2_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_irq2_grp0_pad_mux),
+	.pad_mux_list = gn_gnss_irq2_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_tm_grp_pad_mux[] = {
+	MUX(1, 115, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_gnss_tm_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_tm_grp_pad_mux),
+	.pad_mux_list = gn_gnss_tm_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_gnss_tsync_grp_pad_mux[] = {
+	MUX(1, 114, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_gnss_tsync_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_gnss_tsync_grp_pad_mux),
+	.pad_mux_list = gn_gnss_tsync_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_io_gnsssys_sw_cfg_grp_pad_mux[] = {
+	MUX(1, 44, 7, N, N, N, N),
+	MUX(1, 43, 7, N, N, N, N),
+	MUX(1, 42, 7, N, N, N, N),
+	MUX(1, 41, 7, N, N, N, N),
+	MUX(1, 40, 7, N, N, N, N),
+	MUX(1, 39, 7, N, N, N, N),
+	MUX(1, 38, 7, N, N, N, N),
+	MUX(1, 37, 7, N, N, N, N),
+	MUX(1, 49, 7, N, N, N, N),
+	MUX(1, 50, 7, N, N, N, N),
+	MUX(1, 91, 7, N, N, N, N),
+	MUX(1, 92, 7, N, N, N, N),
+	MUX(1, 93, 7, N, N, N, N),
+	MUX(1, 94, 7, N, N, N, N),
+	MUX(1, 95, 7, N, N, N, N),
+	MUX(1, 96, 7, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_io_gnsssys_sw_cfg_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_io_gnsssys_sw_cfg_grp_pad_mux),
+	.pad_mux_list = gn_io_gnsssys_sw_cfg_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_trg_grp0_pad_mux[] = {
+	MUX(1, 29, 1, 0xa00, 6, 0xa80, 6),
+	MUX(1, 28, 1, 0xa00, 7, 0xa80, 7),
+	MUX(1, 26, 1, 0xa00, 8, 0xa80, 8),
+	MUX(1, 27, 1, 0xa00, 9, 0xa80, 9),
+};
+
+static struct atlas7_grp_mux gn_trg_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_trg_grp0_pad_mux),
+	.pad_mux_list = gn_trg_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_trg_grp1_pad_mux[] = {
+	MUX(1, 77, 3, 0xa00, 6, 0xa80, 6),
+	MUX(1, 76, 3, 0xa00, 7, 0xa80, 7),
+	MUX(1, 74, 3, 0xa00, 8, 0xa80, 8),
+	MUX(1, 75, 3, 0xa00, 9, 0xa80, 9),
+};
+
+static struct atlas7_grp_mux gn_trg_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_trg_grp1_pad_mux),
+	.pad_mux_list = gn_trg_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_trg_shutdown_grp0_pad_mux[] = {
+	MUX(1, 30, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_trg_shutdown_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_trg_shutdown_grp0_pad_mux),
+	.pad_mux_list = gn_trg_shutdown_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_trg_shutdown_grp1_pad_mux[] = {
+	MUX(1, 83, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_trg_shutdown_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_trg_shutdown_grp1_pad_mux),
+	.pad_mux_list = gn_trg_shutdown_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_trg_shutdown_grp2_pad_mux[] = {
+	MUX(1, 117, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_trg_shutdown_grp2_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_trg_shutdown_grp2_pad_mux),
+	.pad_mux_list = gn_trg_shutdown_grp2_pad_mux,
+};
+
+static struct atlas7_pad_mux gn_trg_shutdown_grp3_pad_mux[] = {
+	MUX(1, 123, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux gn_trg_shutdown_grp3_mux = {
+	.pad_mux_count = ARRAY_SIZE(gn_trg_shutdown_grp3_pad_mux),
+	.pad_mux_list = gn_trg_shutdown_grp3_pad_mux,
+};
+
+static struct atlas7_pad_mux i2c0_grp_pad_mux[] = {
+	MUX(1, 128, 1, N, N, N, N),
+	MUX(1, 127, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux i2c0_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(i2c0_grp_pad_mux),
+	.pad_mux_list = i2c0_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux i2c1_grp_pad_mux[] = {
+	MUX(1, 126, 4, N, N, N, N),
+	MUX(1, 125, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux i2c1_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(i2c1_grp_pad_mux),
+	.pad_mux_list = i2c1_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux jtag_grp0_pad_mux[] = {
+	MUX(1, 125, 5, 0xa08, 2, 0xa88, 2),
+	MUX(0, 4, 3, 0xa08, 3, 0xa88, 3),
+	MUX(0, 2, 3, N, N, N, N),
+	MUX(0, 0, 3, N, N, N, N),
+	MUX(0, 1, 3, N, N, N, N),
+	MUX(0, 3, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux jtag_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(jtag_grp0_pad_mux),
+	.pad_mux_list = jtag_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux ks_kas_spi_grp0_pad_mux[] = {
+	MUX(1, 141, 2, N, N, N, N),
+	MUX(1, 144, 2, 0xa08, 8, 0xa88, 8),
+	MUX(1, 143, 2, N, N, N, N),
+	MUX(1, 142, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ks_kas_spi_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(ks_kas_spi_grp0_pad_mux),
+	.pad_mux_list = ks_kas_spi_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux ld_ldd_grp_pad_mux[] = {
+	MUX(1, 57, 1, N, N, N, N),
+	MUX(1, 58, 1, N, N, N, N),
+	MUX(1, 59, 1, N, N, N, N),
+	MUX(1, 60, 1, N, N, N, N),
+	MUX(1, 61, 1, N, N, N, N),
+	MUX(1, 62, 1, N, N, N, N),
+	MUX(1, 63, 1, N, N, N, N),
+	MUX(1, 64, 1, N, N, N, N),
+	MUX(1, 65, 1, N, N, N, N),
+	MUX(1, 66, 1, N, N, N, N),
+	MUX(1, 67, 1, N, N, N, N),
+	MUX(1, 68, 1, N, N, N, N),
+	MUX(1, 69, 1, N, N, N, N),
+	MUX(1, 70, 1, N, N, N, N),
+	MUX(1, 71, 1, N, N, N, N),
+	MUX(1, 72, 1, N, N, N, N),
+	MUX(1, 74, 2, N, N, N, N),
+	MUX(1, 75, 2, N, N, N, N),
+	MUX(1, 76, 2, N, N, N, N),
+	MUX(1, 77, 2, N, N, N, N),
+	MUX(1, 78, 2, N, N, N, N),
+	MUX(1, 79, 2, N, N, N, N),
+	MUX(1, 80, 2, N, N, N, N),
+	MUX(1, 81, 2, N, N, N, N),
+	MUX(1, 56, 1, N, N, N, N),
+	MUX(1, 53, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ld_ldd_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ld_ldd_grp_pad_mux),
+	.pad_mux_list = ld_ldd_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ld_ldd_16bit_grp_pad_mux[] = {
+	MUX(1, 57, 1, N, N, N, N),
+	MUX(1, 58, 1, N, N, N, N),
+	MUX(1, 59, 1, N, N, N, N),
+	MUX(1, 60, 1, N, N, N, N),
+	MUX(1, 61, 1, N, N, N, N),
+	MUX(1, 62, 1, N, N, N, N),
+	MUX(1, 63, 1, N, N, N, N),
+	MUX(1, 64, 1, N, N, N, N),
+	MUX(1, 65, 1, N, N, N, N),
+	MUX(1, 66, 1, N, N, N, N),
+	MUX(1, 67, 1, N, N, N, N),
+	MUX(1, 68, 1, N, N, N, N),
+	MUX(1, 69, 1, N, N, N, N),
+	MUX(1, 70, 1, N, N, N, N),
+	MUX(1, 71, 1, N, N, N, N),
+	MUX(1, 72, 1, N, N, N, N),
+	MUX(1, 56, 1, N, N, N, N),
+	MUX(1, 53, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ld_ldd_16bit_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ld_ldd_16bit_grp_pad_mux),
+	.pad_mux_list = ld_ldd_16bit_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ld_ldd_fck_grp_pad_mux[] = {
+	MUX(1, 55, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ld_ldd_fck_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ld_ldd_fck_grp_pad_mux),
+	.pad_mux_list = ld_ldd_fck_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ld_ldd_lck_grp_pad_mux[] = {
+	MUX(1, 54, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ld_ldd_lck_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ld_ldd_lck_grp_pad_mux),
+	.pad_mux_list = ld_ldd_lck_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux lr_lcdrom_grp_pad_mux[] = {
+	MUX(1, 73, 2, N, N, N, N),
+	MUX(1, 54, 2, N, N, N, N),
+	MUX(1, 57, 2, N, N, N, N),
+	MUX(1, 58, 2, N, N, N, N),
+	MUX(1, 59, 2, N, N, N, N),
+	MUX(1, 60, 2, N, N, N, N),
+	MUX(1, 61, 2, N, N, N, N),
+	MUX(1, 62, 2, N, N, N, N),
+	MUX(1, 63, 2, N, N, N, N),
+	MUX(1, 64, 2, N, N, N, N),
+	MUX(1, 65, 2, N, N, N, N),
+	MUX(1, 66, 2, N, N, N, N),
+	MUX(1, 67, 2, N, N, N, N),
+	MUX(1, 68, 2, N, N, N, N),
+	MUX(1, 69, 2, N, N, N, N),
+	MUX(1, 70, 2, N, N, N, N),
+	MUX(1, 71, 2, N, N, N, N),
+	MUX(1, 72, 2, N, N, N, N),
+	MUX(1, 56, 2, N, N, N, N),
+	MUX(1, 53, 2, N, N, N, N),
+	MUX(1, 55, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux lr_lcdrom_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(lr_lcdrom_grp_pad_mux),
+	.pad_mux_list = lr_lcdrom_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux lvds_analog_grp_pad_mux[] = {
+	MUX(1, 149, 8, N, N, N, N),
+	MUX(1, 150, 8, N, N, N, N),
+	MUX(1, 151, 8, N, N, N, N),
+	MUX(1, 152, 8, N, N, N, N),
+	MUX(1, 153, 8, N, N, N, N),
+	MUX(1, 154, 8, N, N, N, N),
+	MUX(1, 155, 8, N, N, N, N),
+	MUX(1, 156, 8, N, N, N, N),
+	MUX(1, 157, 8, N, N, N, N),
+	MUX(1, 158, 8, N, N, N, N),
+};
+
+static struct atlas7_grp_mux lvds_analog_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(lvds_analog_grp_pad_mux),
+	.pad_mux_list = lvds_analog_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux nd_df_grp_pad_mux[] = {
+	MUX(1, 44, 1, N, N, N, N),
+	MUX(1, 43, 1, N, N, N, N),
+	MUX(1, 42, 1, N, N, N, N),
+	MUX(1, 41, 1, N, N, N, N),
+	MUX(1, 40, 1, N, N, N, N),
+	MUX(1, 39, 1, N, N, N, N),
+	MUX(1, 38, 1, N, N, N, N),
+	MUX(1, 37, 1, N, N, N, N),
+	MUX(1, 47, 1, N, N, N, N),
+	MUX(1, 46, 1, N, N, N, N),
+	MUX(1, 52, 1, N, N, N, N),
+	MUX(1, 51, 1, N, N, N, N),
+	MUX(1, 45, 1, N, N, N, N),
+	MUX(1, 49, 1, N, N, N, N),
+	MUX(1, 50, 1, N, N, N, N),
+	MUX(1, 48, 1, N, N, N, N),
+	MUX(1, 124, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux nd_df_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(nd_df_grp_pad_mux),
+	.pad_mux_list = nd_df_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux nd_df_nowp_grp_pad_mux[] = {
+	MUX(1, 44, 1, N, N, N, N),
+	MUX(1, 43, 1, N, N, N, N),
+	MUX(1, 42, 1, N, N, N, N),
+	MUX(1, 41, 1, N, N, N, N),
+	MUX(1, 40, 1, N, N, N, N),
+	MUX(1, 39, 1, N, N, N, N),
+	MUX(1, 38, 1, N, N, N, N),
+	MUX(1, 37, 1, N, N, N, N),
+	MUX(1, 47, 1, N, N, N, N),
+	MUX(1, 46, 1, N, N, N, N),
+	MUX(1, 52, 1, N, N, N, N),
+	MUX(1, 51, 1, N, N, N, N),
+	MUX(1, 45, 1, N, N, N, N),
+	MUX(1, 49, 1, N, N, N, N),
+	MUX(1, 50, 1, N, N, N, N),
+	MUX(1, 48, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux nd_df_nowp_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(nd_df_nowp_grp_pad_mux),
+	.pad_mux_list = nd_df_nowp_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux ps_grp_pad_mux[] = {
+	MUX(1, 120, 2, N, N, N, N),
+	MUX(1, 119, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ps_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ps_grp_pad_mux),
+	.pad_mux_list = ps_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_core_on_grp_pad_mux[] = {
+	MUX(0, 8, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_core_on_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_core_on_grp_pad_mux),
+	.pad_mux_list = pwc_core_on_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_ext_on_grp_pad_mux[] = {
+	MUX(0, 6, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_ext_on_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_ext_on_grp_pad_mux),
+	.pad_mux_list = pwc_ext_on_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_gpio3_clk_grp_pad_mux[] = {
+	MUX(0, 3, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_gpio3_clk_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_gpio3_clk_grp_pad_mux),
+	.pad_mux_list = pwc_gpio3_clk_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_io_on_grp_pad_mux[] = {
+	MUX(0, 9, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_io_on_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_io_on_grp_pad_mux),
+	.pad_mux_list = pwc_io_on_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_lowbatt_b_grp0_pad_mux[] = {
+	MUX(0, 4, 1, 0xa08, 4, 0xa88, 4),
+};
+
+static struct atlas7_grp_mux pwc_lowbatt_b_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_lowbatt_b_grp0_pad_mux),
+	.pad_mux_list = pwc_lowbatt_b_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_mem_on_grp_pad_mux[] = {
+	MUX(0, 7, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_mem_on_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_mem_on_grp_pad_mux),
+	.pad_mux_list = pwc_mem_on_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_on_key_b_grp0_pad_mux[] = {
+	MUX(0, 5, 1, 0xa08, 5, 0xa88, 5),
+};
+
+static struct atlas7_grp_mux pwc_on_key_b_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_on_key_b_grp0_pad_mux),
+	.pad_mux_list = pwc_on_key_b_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_wakeup_src0_grp_pad_mux[] = {
+	MUX(0, 0, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_wakeup_src0_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_wakeup_src0_grp_pad_mux),
+	.pad_mux_list = pwc_wakeup_src0_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_wakeup_src1_grp_pad_mux[] = {
+	MUX(0, 1, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_wakeup_src1_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_wakeup_src1_grp_pad_mux),
+	.pad_mux_list = pwc_wakeup_src1_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_wakeup_src2_grp_pad_mux[] = {
+	MUX(0, 2, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_wakeup_src2_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_wakeup_src2_grp_pad_mux),
+	.pad_mux_list = pwc_wakeup_src2_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pwc_wakeup_src3_grp_pad_mux[] = {
+	MUX(0, 3, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pwc_wakeup_src3_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pwc_wakeup_src3_grp_pad_mux),
+	.pad_mux_list = pwc_wakeup_src3_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_cko0_grp0_pad_mux[] = {
+	MUX(1, 123, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_cko0_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_cko0_grp0_pad_mux),
+	.pad_mux_list = pw_cko0_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_cko0_grp1_pad_mux[] = {
+	MUX(1, 101, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_cko0_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_cko0_grp1_pad_mux),
+	.pad_mux_list = pw_cko0_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_cko0_grp2_pad_mux[] = {
+	MUX(1, 82, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_cko0_grp2_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_cko0_grp2_pad_mux),
+	.pad_mux_list = pw_cko0_grp2_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_cko1_grp0_pad_mux[] = {
+	MUX(1, 124, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_cko1_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_cko1_grp0_pad_mux),
+	.pad_mux_list = pw_cko1_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_cko1_grp1_pad_mux[] = {
+	MUX(1, 110, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_cko1_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_cko1_grp1_pad_mux),
+	.pad_mux_list = pw_cko1_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_i2s01_clk_grp0_pad_mux[] = {
+	MUX(1, 125, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_i2s01_clk_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_i2s01_clk_grp0_pad_mux),
+	.pad_mux_list = pw_i2s01_clk_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_i2s01_clk_grp1_pad_mux[] = {
+	MUX(1, 117, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_i2s01_clk_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_i2s01_clk_grp1_pad_mux),
+	.pad_mux_list = pw_i2s01_clk_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_pwm0_grp_pad_mux[] = {
+	MUX(1, 119, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_pwm0_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_pwm0_grp_pad_mux),
+	.pad_mux_list = pw_pwm0_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_pwm1_grp_pad_mux[] = {
+	MUX(1, 120, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_pwm1_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_pwm1_grp_pad_mux),
+	.pad_mux_list = pw_pwm1_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_pwm2_grp0_pad_mux[] = {
+	MUX(1, 121, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_pwm2_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_pwm2_grp0_pad_mux),
+	.pad_mux_list = pw_pwm2_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_pwm2_grp1_pad_mux[] = {
+	MUX(1, 98, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_pwm2_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_pwm2_grp1_pad_mux),
+	.pad_mux_list = pw_pwm2_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_pwm3_grp0_pad_mux[] = {
+	MUX(1, 122, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_pwm3_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_pwm3_grp0_pad_mux),
+	.pad_mux_list = pw_pwm3_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_pwm3_grp1_pad_mux[] = {
+	MUX(1, 73, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_pwm3_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_pwm3_grp1_pad_mux),
+	.pad_mux_list = pw_pwm3_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_pwm_cpu_vol_grp0_pad_mux[] = {
+	MUX(1, 121, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_pwm_cpu_vol_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_pwm_cpu_vol_grp0_pad_mux),
+	.pad_mux_list = pw_pwm_cpu_vol_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_pwm_cpu_vol_grp1_pad_mux[] = {
+	MUX(1, 98, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_pwm_cpu_vol_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_pwm_cpu_vol_grp1_pad_mux),
+	.pad_mux_list = pw_pwm_cpu_vol_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_backlight_grp0_pad_mux[] = {
+	MUX(1, 122, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_backlight_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_backlight_grp0_pad_mux),
+	.pad_mux_list = pw_backlight_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux pw_backlight_grp1_pad_mux[] = {
+	MUX(1, 73, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux pw_backlight_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(pw_backlight_grp1_pad_mux),
+	.pad_mux_list = pw_backlight_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux rg_eth_mac_grp_pad_mux[] = {
+	MUX(1, 108, 1, N, N, N, N),
+	MUX(1, 103, 1, N, N, N, N),
+	MUX(1, 104, 1, N, N, N, N),
+	MUX(1, 105, 1, N, N, N, N),
+	MUX(1, 106, 1, N, N, N, N),
+	MUX(1, 107, 1, N, N, N, N),
+	MUX(1, 102, 1, N, N, N, N),
+	MUX(1, 97, 1, N, N, N, N),
+	MUX(1, 98, 1, N, N, N, N),
+	MUX(1, 99, 1, N, N, N, N),
+	MUX(1, 100, 1, N, N, N, N),
+	MUX(1, 101, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux rg_eth_mac_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(rg_eth_mac_grp_pad_mux),
+	.pad_mux_list = rg_eth_mac_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux rg_gmac_phy_intr_n_grp_pad_mux[] = {
+	MUX(1, 111, 1, 0xa08, 13, 0xa88, 13),
+};
+
+static struct atlas7_grp_mux rg_gmac_phy_intr_n_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(rg_gmac_phy_intr_n_grp_pad_mux),
+	.pad_mux_list = rg_gmac_phy_intr_n_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux rg_rgmii_mac_grp_pad_mux[] = {
+	MUX(1, 109, 1, N, N, N, N),
+	MUX(1, 110, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux rg_rgmii_mac_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(rg_rgmii_mac_grp_pad_mux),
+	.pad_mux_list = rg_rgmii_mac_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux rg_rgmii_phy_ref_clk_grp0_pad_mux[] = {
+	MUX(1, 111, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux rg_rgmii_phy_ref_clk_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(rg_rgmii_phy_ref_clk_grp0_pad_mux),
+	.pad_mux_list = rg_rgmii_phy_ref_clk_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux rg_rgmii_phy_ref_clk_grp1_pad_mux[] = {
+	MUX(1, 53, 4, N, N, N, N),
+};
+
+static struct atlas7_grp_mux rg_rgmii_phy_ref_clk_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(rg_rgmii_phy_ref_clk_grp1_pad_mux),
+	.pad_mux_list = rg_rgmii_phy_ref_clk_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux sd0_grp_pad_mux[] = {
+	MUX(1, 46, 2, N, N, N, N),
+	MUX(1, 47, 2, N, N, N, N),
+	MUX(1, 44, 2, N, N, N, N),
+	MUX(1, 43, 2, N, N, N, N),
+	MUX(1, 42, 2, N, N, N, N),
+	MUX(1, 41, 2, N, N, N, N),
+	MUX(1, 40, 2, N, N, N, N),
+	MUX(1, 39, 2, N, N, N, N),
+	MUX(1, 38, 2, N, N, N, N),
+	MUX(1, 37, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sd0_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd0_grp_pad_mux),
+	.pad_mux_list = sd0_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sd0_4bit_grp_pad_mux[] = {
+	MUX(1, 46, 2, N, N, N, N),
+	MUX(1, 47, 2, N, N, N, N),
+	MUX(1, 44, 2, N, N, N, N),
+	MUX(1, 43, 2, N, N, N, N),
+	MUX(1, 42, 2, N, N, N, N),
+	MUX(1, 41, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sd0_4bit_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd0_4bit_grp_pad_mux),
+	.pad_mux_list = sd0_4bit_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sd1_grp_pad_mux[] = {
+	MUX(1, 48, 3, N, N, N, N),
+	MUX(1, 49, 3, N, N, N, N),
+	MUX(1, 44, 3, 0xa00, 0, 0xa80, 0),
+	MUX(1, 43, 3, 0xa00, 1, 0xa80, 1),
+	MUX(1, 42, 3, 0xa00, 2, 0xa80, 2),
+	MUX(1, 41, 3, 0xa00, 3, 0xa80, 3),
+	MUX(1, 40, 3, N, N, N, N),
+	MUX(1, 39, 3, N, N, N, N),
+	MUX(1, 38, 3, N, N, N, N),
+	MUX(1, 37, 3, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sd1_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd1_grp_pad_mux),
+	.pad_mux_list = sd1_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sd1_4bit_grp0_pad_mux[] = {
+	MUX(1, 48, 3, N, N, N, N),
+	MUX(1, 49, 3, N, N, N, N),
+	MUX(1, 44, 3, 0xa00, 0, 0xa80, 0),
+	MUX(1, 43, 3, 0xa00, 1, 0xa80, 1),
+	MUX(1, 42, 3, 0xa00, 2, 0xa80, 2),
+	MUX(1, 41, 3, 0xa00, 3, 0xa80, 3),
+};
+
+static struct atlas7_grp_mux sd1_4bit_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd1_4bit_grp0_pad_mux),
+	.pad_mux_list = sd1_4bit_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux sd1_4bit_grp1_pad_mux[] = {
+	MUX(1, 48, 3, N, N, N, N),
+	MUX(1, 49, 3, N, N, N, N),
+	MUX(1, 40, 4, 0xa00, 0, 0xa80, 0),
+	MUX(1, 39, 4, 0xa00, 1, 0xa80, 1),
+	MUX(1, 38, 4, 0xa00, 2, 0xa80, 2),
+	MUX(1, 37, 4, 0xa00, 3, 0xa80, 3),
+};
+
+static struct atlas7_grp_mux sd1_4bit_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd1_4bit_grp1_pad_mux),
+	.pad_mux_list = sd1_4bit_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux sd2_grp0_pad_mux[] = {
+	MUX(1, 124, 2, 0xa08, 7, 0xa88, 7),
+	MUX(1, 31, 1, N, N, N, N),
+	MUX(1, 32, 1, N, N, N, N),
+	MUX(1, 33, 1, N, N, N, N),
+	MUX(1, 34, 1, N, N, N, N),
+	MUX(1, 35, 1, N, N, N, N),
+	MUX(1, 36, 1, N, N, N, N),
+	MUX(1, 123, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sd2_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd2_grp0_pad_mux),
+	.pad_mux_list = sd2_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux sd2_no_cdb_grp0_pad_mux[] = {
+	MUX(1, 31, 1, N, N, N, N),
+	MUX(1, 32, 1, N, N, N, N),
+	MUX(1, 33, 1, N, N, N, N),
+	MUX(1, 34, 1, N, N, N, N),
+	MUX(1, 35, 1, N, N, N, N),
+	MUX(1, 36, 1, N, N, N, N),
+	MUX(1, 123, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sd2_no_cdb_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd2_no_cdb_grp0_pad_mux),
+	.pad_mux_list = sd2_no_cdb_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux sd3_grp_pad_mux[] = {
+	MUX(1, 85, 1, N, N, N, N),
+	MUX(1, 86, 1, N, N, N, N),
+	MUX(1, 87, 1, N, N, N, N),
+	MUX(1, 88, 1, N, N, N, N),
+	MUX(1, 89, 1, N, N, N, N),
+	MUX(1, 90, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sd3_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd3_grp_pad_mux),
+	.pad_mux_list = sd3_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sd5_grp_pad_mux[] = {
+	MUX(1, 91, 1, N, N, N, N),
+	MUX(1, 92, 1, N, N, N, N),
+	MUX(1, 93, 1, N, N, N, N),
+	MUX(1, 94, 1, N, N, N, N),
+	MUX(1, 95, 1, N, N, N, N),
+	MUX(1, 96, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sd5_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd5_grp_pad_mux),
+	.pad_mux_list = sd5_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sd6_grp0_pad_mux[] = {
+	MUX(1, 79, 4, 0xa00, 27, 0xa80, 27),
+	MUX(1, 78, 4, 0xa00, 26, 0xa80, 26),
+	MUX(1, 74, 4, 0xa00, 28, 0xa80, 28),
+	MUX(1, 75, 4, 0xa00, 29, 0xa80, 29),
+	MUX(1, 76, 4, 0xa00, 30, 0xa80, 30),
+	MUX(1, 77, 4, 0xa00, 31, 0xa80, 31),
+};
+
+static struct atlas7_grp_mux sd6_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd6_grp0_pad_mux),
+	.pad_mux_list = sd6_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux sd6_grp1_pad_mux[] = {
+	MUX(1, 101, 3, 0xa00, 27, 0xa80, 27),
+	MUX(1, 99, 3, 0xa00, 26, 0xa80, 26),
+	MUX(1, 100, 3, 0xa00, 28, 0xa80, 28),
+	MUX(1, 110, 3, 0xa00, 29, 0xa80, 29),
+	MUX(1, 109, 3, 0xa00, 30, 0xa80, 30),
+	MUX(1, 111, 3, 0xa00, 31, 0xa80, 31),
+};
+
+static struct atlas7_grp_mux sd6_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd6_grp1_pad_mux),
+	.pad_mux_list = sd6_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux sp0_ext_ldo_on_grp_pad_mux[] = {
+	MUX(0, 4, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sp0_ext_ldo_on_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sp0_ext_ldo_on_grp_pad_mux),
+	.pad_mux_list = sp0_ext_ldo_on_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sp0_qspi_grp_pad_mux[] = {
+	MUX(0, 12, 1, N, N, N, N),
+	MUX(0, 13, 1, N, N, N, N),
+	MUX(0, 14, 1, N, N, N, N),
+	MUX(0, 15, 1, N, N, N, N),
+	MUX(0, 16, 1, N, N, N, N),
+	MUX(0, 17, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sp0_qspi_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sp0_qspi_grp_pad_mux),
+	.pad_mux_list = sp0_qspi_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux sp1_spi_grp_pad_mux[] = {
+	MUX(1, 19, 1, N, N, N, N),
+	MUX(1, 20, 1, N, N, N, N),
+	MUX(1, 21, 1, N, N, N, N),
+	MUX(1, 18, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux sp1_spi_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sp1_spi_grp_pad_mux),
+	.pad_mux_list = sp1_spi_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux tpiu_trace_grp_pad_mux[] = {
+	MUX(1, 53, 5, N, N, N, N),
+	MUX(1, 56, 5, N, N, N, N),
+	MUX(1, 57, 5, N, N, N, N),
+	MUX(1, 58, 5, N, N, N, N),
+	MUX(1, 59, 5, N, N, N, N),
+	MUX(1, 60, 5, N, N, N, N),
+	MUX(1, 61, 5, N, N, N, N),
+	MUX(1, 62, 5, N, N, N, N),
+	MUX(1, 63, 5, N, N, N, N),
+	MUX(1, 64, 5, N, N, N, N),
+	MUX(1, 65, 5, N, N, N, N),
+	MUX(1, 66, 5, N, N, N, N),
+	MUX(1, 67, 5, N, N, N, N),
+	MUX(1, 68, 5, N, N, N, N),
+	MUX(1, 69, 5, N, N, N, N),
+	MUX(1, 70, 5, N, N, N, N),
+	MUX(1, 71, 5, N, N, N, N),
+	MUX(1, 72, 5, N, N, N, N),
+};
+
+static struct atlas7_grp_mux tpiu_trace_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(tpiu_trace_grp_pad_mux),
+	.pad_mux_list = tpiu_trace_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux uart0_grp_pad_mux[] = {
+	MUX(1, 121, 4, N, N, N, N),
+	MUX(1, 120, 4, N, N, N, N),
+	MUX(1, 134, 1, N, N, N, N),
+	MUX(1, 133, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart0_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart0_grp_pad_mux),
+	.pad_mux_list = uart0_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux uart0_nopause_grp_pad_mux[] = {
+	MUX(1, 134, 1, N, N, N, N),
+	MUX(1, 133, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart0_nopause_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart0_nopause_grp_pad_mux),
+	.pad_mux_list = uart0_nopause_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux uart1_grp_pad_mux[] = {
+	MUX(1, 136, 1, N, N, N, N),
+	MUX(1, 135, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart1_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart1_grp_pad_mux),
+	.pad_mux_list = uart1_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux uart2_grp_pad_mux[] = {
+	MUX(0, 11, 2, N, N, N, N),
+	MUX(0, 10, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart2_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart2_grp_pad_mux),
+	.pad_mux_list = uart2_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux uart3_grp0_pad_mux[] = {
+	MUX(1, 125, 2, 0xa08, 0, 0xa88, 0),
+	MUX(1, 126, 2, N, N, N, N),
+	MUX(1, 138, 1, 0xa00, 5, 0xa80, 5),
+	MUX(1, 137, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart3_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart3_grp0_pad_mux),
+	.pad_mux_list = uart3_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux uart3_grp1_pad_mux[] = {
+	MUX(1, 111, 4, 0xa08, 0, 0xa88, 0),
+	MUX(1, 109, 4, N, N, N, N),
+	MUX(1, 84, 2, 0xa00, 5, 0xa80, 5),
+	MUX(1, 83, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart3_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart3_grp1_pad_mux),
+	.pad_mux_list = uart3_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux uart3_grp2_pad_mux[] = {
+	MUX(1, 140, 2, 0xa08, 0, 0xa88, 0),
+	MUX(1, 139, 2, N, N, N, N),
+	MUX(1, 138, 1, 0xa00, 5, 0xa80, 5),
+	MUX(1, 137, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart3_grp2_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart3_grp2_pad_mux),
+	.pad_mux_list = uart3_grp2_pad_mux,
+};
+
+static struct atlas7_pad_mux uart3_grp3_pad_mux[] = {
+	MUX(1, 139, 2, N, N, N, N),
+	MUX(1, 140, 2, 0xa08, 0, 0xa88, 0),
+	MUX(1, 84, 2, 0xa00, 5, 0xa80, 5),
+	MUX(1, 83, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart3_grp3_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart3_grp3_pad_mux),
+	.pad_mux_list = uart3_grp3_pad_mux,
+};
+
+static struct atlas7_pad_mux uart3_nopause_grp0_pad_mux[] = {
+	MUX(1, 138, 1, 0xa00, 5, 0xa80, 5),
+	MUX(1, 137, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart3_nopause_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart3_nopause_grp0_pad_mux),
+	.pad_mux_list = uart3_nopause_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux uart3_nopause_grp1_pad_mux[] = {
+	MUX(1, 84, 2, 0xa00, 5, 0xa80, 5),
+	MUX(1, 83, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart3_nopause_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart3_nopause_grp1_pad_mux),
+	.pad_mux_list = uart3_nopause_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux uart4_grp0_pad_mux[] = {
+	MUX(1, 122, 4, 0xa08, 1, 0xa88, 1),
+	MUX(1, 123, 4, N, N, N, N),
+	MUX(1, 140, 1, N, N, N, N),
+	MUX(1, 139, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart4_grp0_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart4_grp0_pad_mux),
+	.pad_mux_list = uart4_grp0_pad_mux,
+};
+
+static struct atlas7_pad_mux uart4_grp1_pad_mux[] = {
+	MUX(1, 100, 4, 0xa08, 1, 0xa88, 1),
+	MUX(1, 99, 4, N, N, N, N),
+	MUX(1, 140, 1, N, N, N, N),
+	MUX(1, 139, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart4_grp1_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart4_grp1_pad_mux),
+	.pad_mux_list = uart4_grp1_pad_mux,
+};
+
+static struct atlas7_pad_mux uart4_grp2_pad_mux[] = {
+	MUX(1, 117, 2, 0xa08, 1, 0xa88, 1),
+	MUX(1, 116, 2, N, N, N, N),
+	MUX(1, 140, 1, N, N, N, N),
+	MUX(1, 139, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart4_grp2_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart4_grp2_pad_mux),
+	.pad_mux_list = uart4_grp2_pad_mux,
+};
+
+static struct atlas7_pad_mux uart4_nopause_grp_pad_mux[] = {
+	MUX(1, 140, 1, N, N, N, N),
+	MUX(1, 139, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux uart4_nopause_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(uart4_nopause_grp_pad_mux),
+	.pad_mux_list = uart4_nopause_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux usb0_drvvbus_grp_pad_mux[] = {
+	MUX(1, 51, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux usb0_drvvbus_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(usb0_drvvbus_grp_pad_mux),
+	.pad_mux_list = usb0_drvvbus_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux usb1_drvvbus_grp_pad_mux[] = {
+	MUX(1, 134, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux usb1_drvvbus_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(usb1_drvvbus_grp_pad_mux),
+	.pad_mux_list = usb1_drvvbus_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux visbus_dout_grp_pad_mux[] = {
+	MUX(1, 57, 6, N, N, N, N),
+	MUX(1, 58, 6, N, N, N, N),
+	MUX(1, 59, 6, N, N, N, N),
+	MUX(1, 60, 6, N, N, N, N),
+	MUX(1, 61, 6, N, N, N, N),
+	MUX(1, 62, 6, N, N, N, N),
+	MUX(1, 63, 6, N, N, N, N),
+	MUX(1, 64, 6, N, N, N, N),
+	MUX(1, 65, 6, N, N, N, N),
+	MUX(1, 66, 6, N, N, N, N),
+	MUX(1, 67, 6, N, N, N, N),
+	MUX(1, 68, 6, N, N, N, N),
+	MUX(1, 69, 6, N, N, N, N),
+	MUX(1, 70, 6, N, N, N, N),
+	MUX(1, 71, 6, N, N, N, N),
+	MUX(1, 72, 6, N, N, N, N),
+	MUX(1, 53, 6, N, N, N, N),
+	MUX(1, 54, 6, N, N, N, N),
+	MUX(1, 55, 6, N, N, N, N),
+	MUX(1, 56, 6, N, N, N, N),
+	MUX(1, 85, 6, N, N, N, N),
+	MUX(1, 86, 6, N, N, N, N),
+	MUX(1, 87, 6, N, N, N, N),
+	MUX(1, 88, 6, N, N, N, N),
+	MUX(1, 89, 6, N, N, N, N),
+	MUX(1, 90, 6, N, N, N, N),
+	MUX(1, 91, 6, N, N, N, N),
+	MUX(1, 92, 6, N, N, N, N),
+	MUX(1, 93, 6, N, N, N, N),
+	MUX(1, 94, 6, N, N, N, N),
+	MUX(1, 95, 6, N, N, N, N),
+	MUX(1, 96, 6, N, N, N, N),
+};
+
+static struct atlas7_grp_mux visbus_dout_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(visbus_dout_grp_pad_mux),
+	.pad_mux_list = visbus_dout_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux vi_vip1_grp_pad_mux[] = {
+	MUX(1, 74, 1, N, N, N, N),
+	MUX(1, 75, 1, N, N, N, N),
+	MUX(1, 76, 1, N, N, N, N),
+	MUX(1, 77, 1, N, N, N, N),
+	MUX(1, 78, 1, N, N, N, N),
+	MUX(1, 79, 1, N, N, N, N),
+	MUX(1, 80, 1, N, N, N, N),
+	MUX(1, 81, 1, N, N, N, N),
+	MUX(1, 82, 1, N, N, N, N),
+	MUX(1, 83, 1, N, N, N, N),
+	MUX(1, 84, 1, N, N, N, N),
+	MUX(1, 103, 2, N, N, N, N),
+	MUX(1, 104, 2, N, N, N, N),
+	MUX(1, 105, 2, N, N, N, N),
+	MUX(1, 106, 2, N, N, N, N),
+	MUX(1, 107, 2, N, N, N, N),
+	MUX(1, 102, 2, N, N, N, N),
+	MUX(1, 97, 2, N, N, N, N),
+	MUX(1, 98, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux vi_vip1_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(vi_vip1_grp_pad_mux),
+	.pad_mux_list = vi_vip1_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux vi_vip1_ext_grp_pad_mux[] = {
+	MUX(1, 74, 1, N, N, N, N),
+	MUX(1, 75, 1, N, N, N, N),
+	MUX(1, 76, 1, N, N, N, N),
+	MUX(1, 77, 1, N, N, N, N),
+	MUX(1, 78, 1, N, N, N, N),
+	MUX(1, 79, 1, N, N, N, N),
+	MUX(1, 80, 1, N, N, N, N),
+	MUX(1, 81, 1, N, N, N, N),
+	MUX(1, 82, 1, N, N, N, N),
+	MUX(1, 83, 1, N, N, N, N),
+	MUX(1, 84, 1, N, N, N, N),
+	MUX(1, 108, 2, N, N, N, N),
+	MUX(1, 103, 2, N, N, N, N),
+	MUX(1, 104, 2, N, N, N, N),
+	MUX(1, 105, 2, N, N, N, N),
+	MUX(1, 106, 2, N, N, N, N),
+	MUX(1, 107, 2, N, N, N, N),
+	MUX(1, 102, 2, N, N, N, N),
+	MUX(1, 97, 2, N, N, N, N),
+	MUX(1, 98, 2, N, N, N, N),
+	MUX(1, 99, 2, N, N, N, N),
+	MUX(1, 100, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux vi_vip1_ext_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(vi_vip1_ext_grp_pad_mux),
+	.pad_mux_list = vi_vip1_ext_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux vi_vip1_low8bit_grp_pad_mux[] = {
+	MUX(1, 74, 1, N, N, N, N),
+	MUX(1, 75, 1, N, N, N, N),
+	MUX(1, 76, 1, N, N, N, N),
+	MUX(1, 77, 1, N, N, N, N),
+	MUX(1, 78, 1, N, N, N, N),
+	MUX(1, 79, 1, N, N, N, N),
+	MUX(1, 80, 1, N, N, N, N),
+	MUX(1, 81, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux vi_vip1_low8bit_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(vi_vip1_low8bit_grp_pad_mux),
+	.pad_mux_list = vi_vip1_low8bit_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux vi_vip1_high8bit_grp_pad_mux[] = {
+	MUX(1, 82, 1, N, N, N, N),
+	MUX(1, 83, 1, N, N, N, N),
+	MUX(1, 84, 1, N, N, N, N),
+	MUX(1, 108, 2, N, N, N, N),
+	MUX(1, 103, 2, N, N, N, N),
+	MUX(1, 104, 2, N, N, N, N),
+	MUX(1, 105, 2, N, N, N, N),
+	MUX(1, 106, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux vi_vip1_high8bit_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(vi_vip1_high8bit_grp_pad_mux),
+	.pad_mux_list = vi_vip1_high8bit_grp_pad_mux,
+};
+
+static struct atlas7_pmx_func atlas7_pmx_functions[] = {
+	FUNCTION("gnss_gpio", gnss_gpio_grp, &gnss_gpio_grp_mux),
+	FUNCTION("lcd_vip_gpio", lcd_vip_gpio_grp, &lcd_vip_gpio_grp_mux),
+	FUNCTION("sdio_i2s_gpio", sdio_i2s_gpio_grp, &sdio_i2s_gpio_grp_mux),
+	FUNCTION("sp_rgmii_gpio", sp_rgmii_gpio_grp, &sp_rgmii_gpio_grp_mux),
+	FUNCTION("lvds_gpio", lvds_gpio_grp, &lvds_gpio_grp_mux),
+	FUNCTION("uart_nand_gpio",
+			uart_nand_gpio_grp,
+			&uart_nand_gpio_grp_mux),
+	FUNCTION("rtc_gpio", rtc_gpio_grp, &rtc_gpio_grp_mux),
+	FUNCTION("audio_ac97", audio_ac97_grp, &audio_ac97_grp_mux),
+	FUNCTION("audio_func_dbg",
+			audio_func_dbg_grp,
+			&audio_func_dbg_grp_mux),
+	FUNCTION("audio_i2s", audio_i2s_grp, &audio_i2s_grp_mux),
+	FUNCTION("audio_i2s_2ch", audio_i2s_2ch_grp, &audio_i2s_2ch_grp_mux),
+	FUNCTION("audio_i2s_extclk",
+			audio_i2s_extclk_grp,
+			&audio_i2s_extclk_grp_mux),
+	FUNCTION("audio_uart0", audio_uart0_grp, &audio_uart0_grp_mux),
+	FUNCTION("audio_uart1", audio_uart1_grp, &audio_uart1_grp_mux),
+	FUNCTION("audio_uart2_m0", audio_uart2_grp0, &audio_uart2_grp0_mux),
+	FUNCTION("audio_uart2_m1", audio_uart2_grp1, &audio_uart2_grp1_mux),
+	FUNCTION("c_can_trnsvr", c_can_trnsvr_grp, &c_can_trnsvr_grp_mux),
+	FUNCTION("c0_can_m0", c0_can_grp0, &c0_can_grp0_mux),
+	FUNCTION("c0_can_m1", c0_can_grp1, &c0_can_grp1_mux),
+	FUNCTION("c1_can_m0", c1_can_grp0, &c1_can_grp0_mux),
+	FUNCTION("c1_can_m1", c1_can_grp1, &c1_can_grp1_mux),
+	FUNCTION("c1_can_m2", c1_can_grp2, &c1_can_grp2_mux),
+	FUNCTION("ca_audio_lpc", ca_audio_lpc_grp, &ca_audio_lpc_grp_mux),
+	FUNCTION("ca_bt_lpc", ca_bt_lpc_grp, &ca_bt_lpc_grp_mux),
+	FUNCTION("ca_coex", ca_coex_grp, &ca_coex_grp_mux),
+	FUNCTION("ca_curator_lpc",
+			ca_curator_lpc_grp,
+			&ca_curator_lpc_grp_mux),
+	FUNCTION("ca_pcm_debug", ca_pcm_debug_grp, &ca_pcm_debug_grp_mux),
+	FUNCTION("ca_pio", ca_pio_grp, &ca_pio_grp_mux),
+	FUNCTION("ca_sdio_debug", ca_sdio_debug_grp, &ca_sdio_debug_grp_mux),
+	FUNCTION("ca_spi", ca_spi_grp, &ca_spi_grp_mux),
+	FUNCTION("ca_trb", ca_trb_grp, &ca_trb_grp_mux),
+	FUNCTION("ca_uart_debug", ca_uart_debug_grp, &ca_uart_debug_grp_mux),
+	FUNCTION("clkc_m0", clkc_grp0, &clkc_grp0_mux),
+	FUNCTION("clkc_m1", clkc_grp1, &clkc_grp1_mux),
+	FUNCTION("gn_gnss_i2c", gn_gnss_i2c_grp, &gn_gnss_i2c_grp_mux),
+	FUNCTION("gn_gnss_uart_nopause",
+			gn_gnss_uart_nopause_grp,
+			&gn_gnss_uart_nopause_grp_mux),
+	FUNCTION("gn_gnss_uart", gn_gnss_uart_grp, &gn_gnss_uart_grp_mux),
+	FUNCTION("gn_trg_spi_m0", gn_trg_spi_grp0, &gn_trg_spi_grp0_mux),
+	FUNCTION("gn_trg_spi_m1", gn_trg_spi_grp1, &gn_trg_spi_grp1_mux),
+	FUNCTION("cvbs_dbg", cvbs_dbg_grp, &cvbs_dbg_grp_mux),
+	FUNCTION("cvbs_dbg_test_m0",
+			cvbs_dbg_test_grp0,
+			&cvbs_dbg_test_grp0_mux),
+	FUNCTION("cvbs_dbg_test_m1",
+			cvbs_dbg_test_grp1,
+			&cvbs_dbg_test_grp1_mux),
+	FUNCTION("cvbs_dbg_test_m2",
+			cvbs_dbg_test_grp2,
+			&cvbs_dbg_test_grp2_mux),
+	FUNCTION("cvbs_dbg_test_m3",
+			cvbs_dbg_test_grp3,
+			&cvbs_dbg_test_grp3_mux),
+	FUNCTION("cvbs_dbg_test_m4",
+			cvbs_dbg_test_grp4,
+			&cvbs_dbg_test_grp4_mux),
+	FUNCTION("cvbs_dbg_test_m5",
+			cvbs_dbg_test_grp5,
+			&cvbs_dbg_test_grp5_mux),
+	FUNCTION("cvbs_dbg_test_m6",
+			cvbs_dbg_test_grp6,
+			&cvbs_dbg_test_grp6_mux),
+	FUNCTION("cvbs_dbg_test_m7",
+			cvbs_dbg_test_grp7,
+			&cvbs_dbg_test_grp7_mux),
+	FUNCTION("cvbs_dbg_test_m8",
+			cvbs_dbg_test_grp8,
+			&cvbs_dbg_test_grp8_mux),
+	FUNCTION("cvbs_dbg_test_m9",
+			cvbs_dbg_test_grp9,
+			&cvbs_dbg_test_grp9_mux),
+	FUNCTION("cvbs_dbg_test_m10",
+			cvbs_dbg_test_grp10,
+			&cvbs_dbg_test_grp10_mux),
+	FUNCTION("cvbs_dbg_test_m11",
+			cvbs_dbg_test_grp11,
+			&cvbs_dbg_test_grp11_mux),
+	FUNCTION("cvbs_dbg_test_m12",
+			cvbs_dbg_test_grp12,
+			&cvbs_dbg_test_grp12_mux),
+	FUNCTION("cvbs_dbg_test_m13",
+			cvbs_dbg_test_grp13,
+			&cvbs_dbg_test_grp13_mux),
+	FUNCTION("cvbs_dbg_test_m14",
+			cvbs_dbg_test_grp14,
+			&cvbs_dbg_test_grp14_mux),
+	FUNCTION("cvbs_dbg_test_m15",
+			cvbs_dbg_test_grp15,
+			&cvbs_dbg_test_grp15_mux),
+	FUNCTION("gn_gnss_power", gn_gnss_power_grp, &gn_gnss_power_grp_mux),
+	FUNCTION("gn_gnss_sw_status",
+			gn_gnss_sw_status_grp,
+			&gn_gnss_sw_status_grp_mux),
+	FUNCTION("gn_gnss_eclk", gn_gnss_eclk_grp, &gn_gnss_eclk_grp_mux),
+	FUNCTION("gn_gnss_irq1_m0",
+			gn_gnss_irq1_grp0,
+			&gn_gnss_irq1_grp0_mux),
+	FUNCTION("gn_gnss_irq2_m0",
+			gn_gnss_irq2_grp0,
+			&gn_gnss_irq2_grp0_mux),
+	FUNCTION("gn_gnss_tm", gn_gnss_tm_grp, &gn_gnss_tm_grp_mux),
+	FUNCTION("gn_gnss_tsync", gn_gnss_tsync_grp, &gn_gnss_tsync_grp_mux),
+	FUNCTION("gn_io_gnsssys_sw_cfg",
+			gn_io_gnsssys_sw_cfg_grp,
+			&gn_io_gnsssys_sw_cfg_grp_mux),
+	FUNCTION("gn_trg_m0", gn_trg_grp0, &gn_trg_grp0_mux),
+	FUNCTION("gn_trg_m1", gn_trg_grp1, &gn_trg_grp1_mux),
+	FUNCTION("gn_trg_shutdown_m0",
+			gn_trg_shutdown_grp0,
+			&gn_trg_shutdown_grp0_mux),
+	FUNCTION("gn_trg_shutdown_m1",
+			gn_trg_shutdown_grp1,
+			&gn_trg_shutdown_grp1_mux),
+	FUNCTION("gn_trg_shutdown_m2",
+			gn_trg_shutdown_grp2,
+			&gn_trg_shutdown_grp2_mux),
+	FUNCTION("gn_trg_shutdown_m3",
+			gn_trg_shutdown_grp3,
+			&gn_trg_shutdown_grp3_mux),
+	FUNCTION("i2c0", i2c0_grp, &i2c0_grp_mux),
+	FUNCTION("i2c1", i2c1_grp, &i2c1_grp_mux),
+	FUNCTION("jtag_m0", jtag_grp0, &jtag_grp0_mux),
+	FUNCTION("ks_kas_spi_m0", ks_kas_spi_grp0, &ks_kas_spi_grp0_mux),
+	FUNCTION("ld_ldd", ld_ldd_grp, &ld_ldd_grp_mux),
+	FUNCTION("ld_ldd_16bit", ld_ldd_16bit_grp, &ld_ldd_16bit_grp_mux),
+	FUNCTION("ld_ldd_fck", ld_ldd_fck_grp, &ld_ldd_fck_grp_mux),
+	FUNCTION("ld_ldd_lck", ld_ldd_lck_grp, &ld_ldd_lck_grp_mux),
+	FUNCTION("lr_lcdrom", lr_lcdrom_grp, &lr_lcdrom_grp_mux),
+	FUNCTION("lvds_analog", lvds_analog_grp, &lvds_analog_grp_mux),
+	FUNCTION("nd_df", nd_df_grp, &nd_df_grp_mux),
+	FUNCTION("nd_df_nowp", nd_df_nowp_grp, &nd_df_nowp_grp_mux),
+	FUNCTION("ps", ps_grp, &ps_grp_mux),
+	FUNCTION("pwc_core_on", pwc_core_on_grp, &pwc_core_on_grp_mux),
+	FUNCTION("pwc_ext_on", pwc_ext_on_grp, &pwc_ext_on_grp_mux),
+	FUNCTION("pwc_gpio3_clk", pwc_gpio3_clk_grp, &pwc_gpio3_clk_grp_mux),
+	FUNCTION("pwc_io_on", pwc_io_on_grp, &pwc_io_on_grp_mux),
+	FUNCTION("pwc_lowbatt_b_m0",
+			pwc_lowbatt_b_grp0,
+			&pwc_lowbatt_b_grp0_mux),
+	FUNCTION("pwc_mem_on", pwc_mem_on_grp, &pwc_mem_on_grp_mux),
+	FUNCTION("pwc_on_key_b_m0",
+			pwc_on_key_b_grp0,
+			&pwc_on_key_b_grp0_mux),
+	FUNCTION("pwc_wakeup_src0",
+			pwc_wakeup_src0_grp,
+			&pwc_wakeup_src0_grp_mux),
+	FUNCTION("pwc_wakeup_src1",
+			pwc_wakeup_src1_grp,
+			&pwc_wakeup_src1_grp_mux),
+	FUNCTION("pwc_wakeup_src2",
+			pwc_wakeup_src2_grp,
+			&pwc_wakeup_src2_grp_mux),
+	FUNCTION("pwc_wakeup_src3",
+			pwc_wakeup_src3_grp,
+			&pwc_wakeup_src3_grp_mux),
+	FUNCTION("pw_cko0_m0", pw_cko0_grp0, &pw_cko0_grp0_mux),
+	FUNCTION("pw_cko0_m1", pw_cko0_grp1, &pw_cko0_grp1_mux),
+	FUNCTION("pw_cko0_m2", pw_cko0_grp2, &pw_cko0_grp2_mux),
+	FUNCTION("pw_cko1_m0", pw_cko1_grp0, &pw_cko1_grp0_mux),
+	FUNCTION("pw_cko1_m1", pw_cko1_grp1, &pw_cko1_grp1_mux),
+	FUNCTION("pw_i2s01_clk_m0",
+			pw_i2s01_clk_grp0,
+			&pw_i2s01_clk_grp0_mux),
+	FUNCTION("pw_i2s01_clk_m1",
+			pw_i2s01_clk_grp1,
+			&pw_i2s01_clk_grp1_mux),
+	FUNCTION("pw_pwm0", pw_pwm0_grp, &pw_pwm0_grp_mux),
+	FUNCTION("pw_pwm1", pw_pwm1_grp, &pw_pwm1_grp_mux),
+	FUNCTION("pw_pwm2_m0", pw_pwm2_grp0, &pw_pwm2_grp0_mux),
+	FUNCTION("pw_pwm2_m1", pw_pwm2_grp1, &pw_pwm2_grp1_mux),
+	FUNCTION("pw_pwm3_m0", pw_pwm3_grp0, &pw_pwm3_grp0_mux),
+	FUNCTION("pw_pwm3_m1", pw_pwm3_grp1, &pw_pwm3_grp1_mux),
+	FUNCTION("pw_pwm_cpu_vol_m0",
+			pw_pwm_cpu_vol_grp0,
+			&pw_pwm_cpu_vol_grp0_mux),
+	FUNCTION("pw_pwm_cpu_vol_m1",
+			pw_pwm_cpu_vol_grp1,
+			&pw_pwm_cpu_vol_grp1_mux),
+	FUNCTION("pw_backlight_m0",
+			pw_backlight_grp0,
+			&pw_backlight_grp0_mux),
+	FUNCTION("pw_backlight_m1",
+			pw_backlight_grp1,
+			&pw_backlight_grp1_mux),
+	FUNCTION("rg_eth_mac", rg_eth_mac_grp, &rg_eth_mac_grp_mux),
+	FUNCTION("rg_gmac_phy_intr_n",
+			rg_gmac_phy_intr_n_grp,
+			&rg_gmac_phy_intr_n_grp_mux),
+	FUNCTION("rg_rgmii_mac", rg_rgmii_mac_grp, &rg_rgmii_mac_grp_mux),
+	FUNCTION("rg_rgmii_phy_ref_clk_m0",
+			rg_rgmii_phy_ref_clk_grp0,
+			&rg_rgmii_phy_ref_clk_grp0_mux),
+	FUNCTION("rg_rgmii_phy_ref_clk_m1",
+			rg_rgmii_phy_ref_clk_grp1,
+			&rg_rgmii_phy_ref_clk_grp1_mux),
+	FUNCTION("sd0", sd0_grp, &sd0_grp_mux),
+	FUNCTION("sd0_4bit", sd0_4bit_grp, &sd0_4bit_grp_mux),
+	FUNCTION("sd1", sd1_grp, &sd1_grp_mux),
+	FUNCTION("sd1_4bit_m0", sd1_4bit_grp0, &sd1_4bit_grp0_mux),
+	FUNCTION("sd1_4bit_m1", sd1_4bit_grp1, &sd1_4bit_grp1_mux),
+	FUNCTION("sd2_m0", sd2_grp0, &sd2_grp0_mux),
+	FUNCTION("sd2_no_cdb_m0", sd2_no_cdb_grp0, &sd2_no_cdb_grp0_mux),
+	FUNCTION("sd3", sd3_grp, &sd3_grp_mux),
+	FUNCTION("sd5", sd5_grp, &sd5_grp_mux),
+	FUNCTION("sd6_m0", sd6_grp0, &sd6_grp0_mux),
+	FUNCTION("sd6_m1", sd6_grp1, &sd6_grp1_mux),
+	FUNCTION("sp0_ext_ldo_on",
+			sp0_ext_ldo_on_grp,
+			&sp0_ext_ldo_on_grp_mux),
+	FUNCTION("sp0_qspi", sp0_qspi_grp, &sp0_qspi_grp_mux),
+	FUNCTION("sp1_spi", sp1_spi_grp, &sp1_spi_grp_mux),
+	FUNCTION("tpiu_trace", tpiu_trace_grp, &tpiu_trace_grp_mux),
+	FUNCTION("uart0", uart0_grp, &uart0_grp_mux),
+	FUNCTION("uart0_nopause", uart0_nopause_grp, &uart0_nopause_grp_mux),
+	FUNCTION("uart1", uart1_grp, &uart1_grp_mux),
+	FUNCTION("uart2", uart2_grp, &uart2_grp_mux),
+	FUNCTION("uart3_m0", uart3_grp0, &uart3_grp0_mux),
+	FUNCTION("uart3_m1", uart3_grp1, &uart3_grp1_mux),
+	FUNCTION("uart3_m2", uart3_grp2, &uart3_grp2_mux),
+	FUNCTION("uart3_m3", uart3_grp3, &uart3_grp3_mux),
+	FUNCTION("uart3_nopause_m0",
+			uart3_nopause_grp0,
+			&uart3_nopause_grp0_mux),
+	FUNCTION("uart3_nopause_m1",
+			uart3_nopause_grp1,
+			&uart3_nopause_grp1_mux),
+	FUNCTION("uart4_m0", uart4_grp0, &uart4_grp0_mux),
+	FUNCTION("uart4_m1", uart4_grp1, &uart4_grp1_mux),
+	FUNCTION("uart4_m2", uart4_grp2, &uart4_grp2_mux),
+	FUNCTION("uart4_nopause", uart4_nopause_grp, &uart4_nopause_grp_mux),
+	FUNCTION("usb0_drvvbus", usb0_drvvbus_grp, &usb0_drvvbus_grp_mux),
+	FUNCTION("usb1_drvvbus", usb1_drvvbus_grp, &usb1_drvvbus_grp_mux),
+	FUNCTION("visbus_dout", visbus_dout_grp, &visbus_dout_grp_mux),
+	FUNCTION("vi_vip1", vi_vip1_grp, &vi_vip1_grp_mux),
+	FUNCTION("vi_vip1_ext", vi_vip1_ext_grp, &vi_vip1_ext_grp_mux),
+	FUNCTION("vi_vip1_low8bit",
+			vi_vip1_low8bit_grp,
+			&vi_vip1_low8bit_grp_mux),
+	FUNCTION("vi_vip1_high8bit",
+			vi_vip1_high8bit_grp,
+			&vi_vip1_high8bit_grp_mux),
+};
+
+struct atlas7_pinctrl_data atlas7_ioc_data = {
+	.pads = (struct pinctrl_pin_desc *)atlas7_ioc_pads,
+	.pads_cnt = ARRAY_SIZE(atlas7_ioc_pads),
+	.grps = (struct atlas7_pin_group *)altas7_pin_groups,
+	.grps_cnt = ARRAY_SIZE(altas7_pin_groups),
+	.funcs = (struct atlas7_pmx_func *)atlas7_pmx_functions,
+	.funcs_cnt = ARRAY_SIZE(atlas7_pmx_functions),
+	.confs = (struct atlas7_pad_config *)atlas7_ioc_pad_confs,
+	.confs_cnt = ARRAY_SIZE(atlas7_ioc_pad_confs),
+};
+
+static inline u32 atlas7_pin_to_bank(u32 pin)
+{
+	return (pin >= ATLAS7_PINCTRL_BANK_0_PINS) ? 1 : 0;
+}
+
+static int atlas7_pmx_get_funcs_count(struct pinctrl_dev *pctldev)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->pctl_data->funcs_cnt;
+}
+
+static const char *atlas7_pmx_get_func_name(struct pinctrl_dev *pctldev,
+					u32 selector)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->pctl_data->funcs[selector].name;
+}
+
+static int atlas7_pmx_get_func_groups(struct pinctrl_dev *pctldev,
+		u32 selector, const char * const **groups,
+		u32 * const num_groups)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	*groups = pmx->pctl_data->funcs[selector].groups;
+	*num_groups = pmx->pctl_data->funcs[selector].num_groups;
+
+	return 0;
+}
+
+static void __atlas7_pmx_pin_input_disable_set(struct atlas7_pmx *pmx,
+				const struct atlas7_pad_mux *mux)
+{
+	/* Set Input Disable to avoid input glitches
+	 *
+	 * All Input-Disable Control registers are located on IOCRTC.
+	 * So the regs bank is always 0.
+	 *
+	 */
+	if (mux->dinput_reg && mux->dinput_val_reg) {
+		writel(DI_MASK << mux->dinput_bit,
+			pmx->regs[BANK_DS] + CLR_REG(mux->dinput_reg));
+		writel(DI_DISABLE << mux->dinput_bit,
+			pmx->regs[BANK_DS] + mux->dinput_reg);
+
+
+		writel(DIV_MASK << mux->dinput_val_bit,
+			pmx->regs[BANK_DS] + CLR_REG(mux->dinput_val_reg));
+		writel(DIV_DISABLE << mux->dinput_val_bit,
+			pmx->regs[BANK_DS] + mux->dinput_val_reg);
+	}
+}
+
+static void __atlas7_pmx_pin_input_disable_clr(struct atlas7_pmx *pmx,
+				const struct atlas7_pad_mux *mux)
+{
+	/* Clear Input Disable to avoid input glitches */
+	if (mux->dinput_reg && mux->dinput_val_reg) {
+		writel(DI_MASK << mux->dinput_bit,
+			pmx->regs[BANK_DS] + CLR_REG(mux->dinput_reg));
+		writel(DI_ENABLE << mux->dinput_bit,
+			pmx->regs[BANK_DS] + mux->dinput_reg);
+
+		writel(DIV_MASK << mux->dinput_val_bit,
+			pmx->regs[BANK_DS] + CLR_REG(mux->dinput_val_reg));
+		writel(DIV_ENABLE << mux->dinput_val_bit,
+			pmx->regs[BANK_DS] + mux->dinput_val_reg);
+	}
+}
+
+static int __atlas7_pmx_pin_ad_sel(struct atlas7_pmx *pmx,
+			struct atlas7_pad_config *conf,
+			u32 bank, u32 ad_sel)
+{
+	unsigned long regv;
+
+	/* Write to clear register to clear A/D selector */
+	writel(ANA_CLEAR_MASK << conf->ad_ctrl_bit,
+		pmx->regs[bank] + CLR_REG(conf->ad_ctrl_reg));
+
+	/* Set target pad A/D selector */
+	regv = readl(pmx->regs[bank] + conf->ad_ctrl_reg);
+	regv &= ~(ANA_CLEAR_MASK << conf->ad_ctrl_bit);
+	writel(regv | (ad_sel << conf->ad_ctrl_bit),
+			pmx->regs[bank] + conf->ad_ctrl_reg);
+
+	regv = readl(pmx->regs[bank] + conf->ad_ctrl_reg);
+	pr_debug("bank:%d reg:0x%04x val:0x%08lx\n",
+			bank, conf->ad_ctrl_reg, regv);
+	return 0;
+}
+
+static int  __atlas7_pmx_pin_analog_enable(struct atlas7_pmx *pmx,
+			struct atlas7_pad_config *conf, u32 bank)
+{
+	/* Only PAD_T_AD pins can change between Analogue&Digital */
+	if (conf->type != PAD_T_AD)
+		return -EINVAL;
+
+	return __atlas7_pmx_pin_ad_sel(pmx, conf, bank, 0);
+}
+
+static int __atlas7_pmx_pin_digital_enable(struct atlas7_pmx *pmx,
+			struct atlas7_pad_config *conf, u32 bank)
+{
+	/* Other type pads are always digital */
+	if (conf->type != PAD_T_AD)
+		return 0;
+
+	return __atlas7_pmx_pin_ad_sel(pmx, conf, bank, 1);
+}
+
+static int __atlas7_pmx_pin_enable(struct atlas7_pmx *pmx,
+				u32 pin, u32 func)
+{
+	struct atlas7_pad_config *conf;
+	u32 bank;
+	int ret;
+	unsigned long regv;
+
+	pr_debug("PMX DUMP ### pin#%d func:%d #### START >>>\n",
+			pin, func);
+
+	/* Get this Pad's descriptor from PINCTRL */
+	conf = &pmx->pctl_data->confs[pin];
+	bank = atlas7_pin_to_bank(pin);
+
+	/* Just enable the analog function of this pad */
+	if (FUNC_ANALOGUE == func) {
+		ret = __atlas7_pmx_pin_analog_enable(pmx, conf, bank);
+		if (ret)
+			dev_err(pmx->dev,
+				"Convert pad#%d to analog failed, ret=%d\n",
+				pin, ret);
+		return ret;
+	}
+
+	/* Set Pads from analog to digital */
+	ret = __atlas7_pmx_pin_digital_enable(pmx, conf, bank);
+	if (ret) {
+		dev_err(pmx->dev,
+			"Convert pad#%d to digital failed, ret=%d\n",
+			pin, ret);
+		return ret;
+	}
+
+	/* Write to clear register to clear current function */
+	writel(FUNC_CLEAR_MASK << conf->mux_bit,
+		pmx->regs[bank] + CLR_REG(conf->mux_reg));
+
+	/* Set target pad mux function */
+	regv = readl(pmx->regs[bank] + conf->mux_reg);
+	regv &= ~(FUNC_CLEAR_MASK << conf->mux_bit);
+	writel(regv | (func << conf->mux_bit),
+			pmx->regs[bank] + conf->mux_reg);
+
+	regv = readl(pmx->regs[bank] + conf->mux_reg);
+	pr_debug("bank:%d reg:0x%04x val:0x%08lx\n",
+		bank, conf->mux_reg, regv);
+
+	return 0;
+}
+
+static int atlas7_pmx_set_mux(struct pinctrl_dev *pctldev,
+			u32 func_selector, u32 group_selector)
+{
+	int idx, ret;
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct atlas7_pmx_func *pmx_func;
+	struct atlas7_pin_group *pin_grp;
+	const struct atlas7_grp_mux *grp_mux;
+	const struct atlas7_pad_mux *mux;
+
+	pmx_func = &pmx->pctl_data->funcs[func_selector];
+	pin_grp = &pmx->pctl_data->grps[group_selector];
+
+	pr_debug("PMX DUMP ### Function:[%s] Group:[%s] #### START >>>\n",
+			pmx_func->name, pin_grp->name);
+
+	grp_mux = pmx_func->grpmux;
+
+	for (idx = 0; idx < grp_mux->pad_mux_count; idx++) {
+		mux = &grp_mux->pad_mux_list[idx];
+		__atlas7_pmx_pin_input_disable_set(pmx, mux);
+		ret = __atlas7_pmx_pin_enable(pmx, mux->pin, mux->func);
+		if (ret) {
+			dev_err(pmx->dev,
+				"FUNC:%s GRP:%s PIN#%d.%d failed, ret=%d\n",
+				pmx_func->name, pin_grp->name,
+				mux->pin, mux->func, ret);
+			BUG_ON(1);
+		}
+		__atlas7_pmx_pin_input_disable_clr(pmx, mux);
+	}
+	pr_debug("PMX DUMP ### Function:[%s] Group:[%s] #### END <<<\n",
+			pmx_func->name, pin_grp->name);
+
+	return 0;
+}
+
+struct atlas7_ds_info {
+	u32 ma;
+	u32 ds_16st;
+	u32 ds_4we;
+	u32 ds_0204m31;
+	u32 ds_0610m31;
+};
+
+const struct atlas7_ds_info atlas7_ds_map[] = {
+	{ 2, DS_16ST_0, DS_4WE_0, DS_M31_0, DS_NULL},
+	{ 4, DS_16ST_1, DS_NULL, DS_M31_1, DS_NULL},
+	{ 6, DS_16ST_2, DS_NULL, DS_NULL, DS_M31_0},
+	{ 8, DS_16ST_3, DS_4WE_1, DS_NULL, DS_NULL},
+	{ 10, DS_16ST_4, DS_NULL, DS_NULL, DS_M31_1},
+	{ 12, DS_16ST_5, DS_NULL, DS_NULL, DS_NULL},
+	{ 14, DS_16ST_6, DS_NULL, DS_NULL, DS_NULL},
+	{ 16, DS_16ST_7, DS_4WE_2, DS_NULL, DS_NULL},
+	{ 18, DS_16ST_8, DS_NULL, DS_NULL, DS_NULL},
+	{ 20, DS_16ST_9, DS_NULL, DS_NULL, DS_NULL},
+	{ 22, DS_16ST_10, DS_NULL, DS_NULL, DS_NULL},
+	{ 24, DS_16ST_11, DS_NULL, DS_NULL, DS_NULL},
+	{ 26, DS_16ST_12, DS_NULL, DS_NULL, DS_NULL},
+	{ 28, DS_16ST_13, DS_4WE_3, DS_NULL, DS_NULL},
+	{ 30, DS_16ST_14, DS_NULL, DS_NULL, DS_NULL},
+	{ 32, DS_16ST_15, DS_NULL, DS_NULL, DS_NULL},
+};
+
+static u32 convert_current_to_drive_strength(u32 type, u32 ma)
+{
+	int idx;
+
+	for (idx = 0; idx < ARRAY_SIZE(atlas7_ds_map); idx++) {
+		if (atlas7_ds_map[idx].ma != ma)
+			continue;
+
+		if (type == PAD_T_4WE_PD || type == PAD_T_4WE_PU)
+			return atlas7_ds_map[idx].ds_4we;
+		else if (type == PAD_T_16ST)
+			return atlas7_ds_map[idx].ds_16st;
+		else if (type == PAD_T_M31_0204_PD || type == PAD_T_M31_0204_PU)
+			return atlas7_ds_map[idx].ds_0204m31;
+		else if (type == PAD_T_M31_0610_PD || type == PAD_T_M31_0610_PU)
+			return atlas7_ds_map[idx].ds_0610m31;
+	}
+
+	return DS_NULL;
+}
+
+static int altas7_pinctrl_set_pull_sel(struct pinctrl_dev *pctldev,
+					u32 pin, u32 sel)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct atlas7_pad_config *conf = &pmx->pctl_data->confs[pin];
+	u32 type = conf->type;
+	u32 shift = conf->pupd_bit;
+	u32 bank = atlas7_pin_to_bank(pin);
+	void __iomem *pull_sel_reg, *pull_clr_reg;
+
+	pull_sel_reg = pmx->regs[bank] + conf->pupd_reg;
+	pull_clr_reg = CLR_REG(pull_sel_reg);
+
+	if (type == PAD_T_4WE_PD || type == PAD_T_4WE_PU) {
+		writel(P4WE_PULL_MASK << shift, pull_clr_reg);
+
+		if (sel == PULL_UP)
+			writel(P4WE_PULL_UP << shift, pull_sel_reg);
+		else if (sel == HIGH_HYSTERESIS)
+			writel(P4WE_HIGH_HYSTERESIS << shift, pull_sel_reg);
+		else if (sel == HIGH_Z)
+			writel(P4WE_HIGH_Z << shift, pull_sel_reg);
+		else if (sel == PULL_DOWN)
+			writel(P4WE_PULL_DOWN << shift, pull_sel_reg);
+		else {
+			pr_err("Unknown Pull select type for 4WEPAD#%d\n",
+				pin);
+			return -ENOTSUPP;
+		}
+	} else if (type == PAD_T_16ST) {
+		writel(P16ST_PULL_MASK << shift, pull_clr_reg);
+
+		if (sel == PULL_UP)
+			writel(P16ST_PULL_UP << shift, pull_sel_reg);
+		else if (sel == HIGH_Z)
+			writel(P16ST_HIGH_Z << shift, pull_sel_reg);
+		else if (sel == PULL_DOWN)
+			writel(P16ST_PULL_DOWN << shift, pull_sel_reg);
+		else {
+			pr_err("Unknown Pull select type for 16STPAD#%d\n",
+				pin);
+			return -ENOTSUPP;
+		}
+	} else if (type == PAD_T_M31_0204_PD ||
+		type == PAD_T_M31_0204_PU ||
+		type == PAD_T_M31_0610_PD ||
+		type == PAD_T_M31_0610_PU) {
+		writel(PM31_PULL_MASK << shift, pull_clr_reg);
+
+		if (sel == PULL_UP)
+			writel(PM31_PULL_ENABLED << shift, pull_sel_reg);
+		else if (sel == PULL_DOWN)
+			writel(PM31_PULL_DISABLED << shift, pull_sel_reg);
+		else {
+			pr_err("Unknown Pull select type for M31PAD#%d\n",
+				pin);
+			return -ENOTSUPP;
+		}
+	} else if (type == PAD_T_AD) {
+		writel(PANGD_PULL_MASK << shift, pull_clr_reg);
+
+		if (sel == PULL_UP)
+			writel(PANGD_PULL_UP << shift, pull_sel_reg);
+		else if (sel == HIGH_Z)
+			writel(PANGD_HIGH_Z << shift, pull_sel_reg);
+		else if (sel == PULL_DOWN)
+			writel(PANGD_PULL_DOWN << shift, pull_sel_reg);
+		else {
+			pr_err("Unknown Pull select type for A/D PAD#%d\n",
+				pin);
+			return -ENOTSUPP;
+		}
+	} else {
+			pr_err("Unknown Pad type[%d] for pull select PAD#%d\n",
+				type, pin);
+			return -ENOTSUPP;
+	}
+
+	pr_debug("PIN_CFG ### SET PIN#%d PULL SELECTOR:%d == OK ####\n",
+		pin, sel);
+	return 0;
+}
+
+static int __altas7_pinctrl_set_drive_strength_sel(struct pinctrl_dev *pctldev,
+						u32 pin, u32 sel)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct atlas7_pad_config *conf = &pmx->pctl_data->confs[pin];
+	u32 type = conf->type;
+	u32 shift = conf->drvstr_bit;
+	u32 bank = atlas7_pin_to_bank(pin);
+	void __iomem *ds_sel_reg, *ds_clr_reg;
+
+	ds_sel_reg = pmx->regs[bank] + conf->drvstr_reg;
+	ds_clr_reg = CLR_REG(ds_sel_reg);
+	if (type == PAD_T_4WE_PD || type == PAD_T_4WE_PU) {
+		if (sel & (~DS_2BIT_MASK))
+			goto unsupport;
+
+		writel(DS_2BIT_IM_VAL << shift, ds_clr_reg);
+		writel(sel << shift, ds_sel_reg);
+
+		return 0;
+	} else if (type == PAD_T_16ST) {
+		if (sel & (~DS_4BIT_MASK))
+			goto unsupport;
+
+		writel(DS_4BIT_IM_VAL << shift, ds_clr_reg);
+		writel(sel << shift, ds_sel_reg);
+
+		return 0;
+	} else if (type == PAD_T_M31_0204_PD ||	type == PAD_T_M31_0204_PU ||
+		type == PAD_T_M31_0610_PD || type == PAD_T_M31_0610_PU) {
+		if (sel & (~DS_1BIT_MASK))
+			goto unsupport;
+
+		writel(DS_1BIT_IM_VAL << shift, ds_clr_reg);
+		writel(sel << shift, ds_sel_reg);
+
+		return 0;
+	}
+
+unsupport:
+	pr_err("Pad#%d type[%d] doesn't support ds code[%d]!\n",
+		pin, type, sel);
+	return -ENOTSUPP;
+}
+
+static int altas7_pinctrl_set_drive_strength_sel(struct pinctrl_dev *pctldev,
+						u32 pin, u32 ma)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	struct atlas7_pad_config *conf = &pmx->pctl_data->confs[pin];
+	u32 type = conf->type;
+	u32 sel;
+	int ret;
+
+	sel = convert_current_to_drive_strength(conf->type, ma);
+	if (DS_NULL == sel) {
+		pr_err("Pad#%d type[%d] doesn't support ds current[%d]!\n",
+		pin, type, ma);
+		return -ENOTSUPP;
+	}
+
+	ret =  __altas7_pinctrl_set_drive_strength_sel(pctldev,
+						pin, sel);
+	pr_debug("PIN_CFG ### SET PIN#%d DS:%d MA:%d == %s ####\n",
+		pin, sel, ma, ret?"FAILED":"OK");
+	return ret;
+}
+
+static int atlas7_pmx_gpio_request_enable(struct pinctrl_dev *pctldev,
+		struct pinctrl_gpio_range *range, u32 pin)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+	u32 idx;
+
+	dev_dbg(pmx->dev,
+		"atlas7_pmx_gpio_request_enable: pin=%d\n", pin);
+	for (idx = 0; idx < range->npins; idx++) {
+		if (pin == range->pins[idx])
+			break;
+	}
+
+	if (idx >= range->npins) {
+		dev_err(pmx->dev,
+			"The pin#%d could not be requested as GPIO!!\n",
+			pin);
+		return -EPERM;
+	}
+
+	__atlas7_pmx_pin_enable(pmx, pin, FUNC_GPIO);
+
+	return 0;
+}
+
+static struct pinmux_ops atlas7_pinmux_ops = {
+	.get_functions_count = atlas7_pmx_get_funcs_count,
+	.get_function_name = atlas7_pmx_get_func_name,
+	.get_function_groups = atlas7_pmx_get_func_groups,
+	.set_mux = atlas7_pmx_set_mux,
+	.gpio_request_enable = atlas7_pmx_gpio_request_enable,
+};
+
+static int atlas7_pinctrl_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->pctl_data->grps_cnt;
+}
+
+static const char *atlas7_pinctrl_get_group_name(struct pinctrl_dev *pctldev,
+						u32 group)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	return pmx->pctl_data->grps[group].name;
+}
+
+static int atlas7_pinctrl_get_group_pins(struct pinctrl_dev *pctldev,
+		u32 group, const u32 **pins, u32 *num_pins)
+{
+	struct atlas7_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
+
+	*num_pins = pmx->pctl_data->grps[group].num_pins;
+	*pins = pmx->pctl_data->grps[group].pins;
+
+	return 0;
+}
+
+static int atlas7_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
+					struct device_node *np_config,
+					struct pinctrl_map **map,
+					u32 *num_maps)
+{
+	return pinconf_generic_dt_node_to_map(pctldev, np_config, map,
+				num_maps, PIN_MAP_TYPE_INVALID);
+}
+
+static void atlas7_pinctrl_dt_free_map(struct pinctrl_dev *pctldev,
+		struct pinctrl_map *map, u32 num_maps)
+{
+	kfree(map);
+}
+
+static const struct pinctrl_ops atlas7_pinctrl_ops = {
+	.get_groups_count = atlas7_pinctrl_get_groups_count,
+	.get_group_name = atlas7_pinctrl_get_group_name,
+	.get_group_pins = atlas7_pinctrl_get_group_pins,
+	.dt_node_to_map = atlas7_pinctrl_dt_node_to_map,
+	.dt_free_map = atlas7_pinctrl_dt_free_map,
+};
+
+static int atlas7_pin_config_set(struct pinctrl_dev *pctldev,
+				unsigned pin, unsigned long *configs,
+				unsigned num_configs)
+{
+	u16 param, arg;
+	int idx, err;
+
+	for (idx = 0; idx < num_configs; idx++) {
+		param = pinconf_to_config_param(configs[idx]);
+		arg = pinconf_to_config_argument(configs[idx]);
+
+		pr_debug("PMX CFG###### ATLAS7 PIN#%d [%s] CONFIG PARAM:%d ARG:%d >>>>>\n",
+			pin, atlas7_ioc_pads[pin].name, param, arg);
+		switch (param) {
+		case PIN_CONFIG_BIAS_PULL_UP:
+			err = altas7_pinctrl_set_pull_sel(pctldev,
+							pin, PULL_UP);
+			if (err)
+				return err;
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			err = altas7_pinctrl_set_pull_sel(pctldev,
+							pin, PULL_DOWN);
+			if (err)
+				return err;
+			break;
+
+		case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+			err = altas7_pinctrl_set_pull_sel(pctldev,
+							pin, HIGH_HYSTERESIS);
+			if (err)
+				return err;
+			break;
+		case PIN_CONFIG_BIAS_HIGH_IMPEDANCE:
+			err = altas7_pinctrl_set_pull_sel(pctldev,
+							pin, HIGH_Z);
+			if (err)
+				return err;
+			break;
+
+		case PIN_CONFIG_DRIVE_STRENGTH:
+			err = altas7_pinctrl_set_drive_strength_sel(pctldev,
+							pin, arg);
+			if (err)
+				return err;
+			break;
+		default:
+			return -ENOTSUPP;
+		}
+		pr_debug("PMX CFG###### ATLAS7 PIN#%d [%s] CONFIG PARAM:%d ARG:%d <<<<\n",
+			pin, atlas7_ioc_pads[pin].name, param, arg);
+	}
+
+	return 0;
+}
+
+static int atlas7_pin_config_group_set(struct pinctrl_dev *pctldev,
+				unsigned group, unsigned long *configs,
+				unsigned num_configs)
+{
+	const unsigned *pins;
+	unsigned npins;
+	int i, ret;
+
+	ret = atlas7_pinctrl_get_group_pins(pctldev, group, &pins, &npins);
+	if (ret)
+		return ret;
+	for (i = 0; i < npins; i++) {
+		if (atlas7_pin_config_set(pctldev, pins[i],
+					  configs, num_configs))
+			return -ENOTSUPP;
+	}
+	return 0;
+}
+
+static const struct pinconf_ops atlas7_pinconf_ops = {
+	.pin_config_set = atlas7_pin_config_set,
+	.pin_config_group_set = atlas7_pin_config_group_set,
+	.is_generic = true,
+};
+
+static int atlas7_pinmux_probe(struct platform_device *pdev)
+{
+	int ret, idx;
+	struct atlas7_pmx *pmx;
+	struct device_node *np = pdev->dev.of_node;
+	u32 banks = ATLAS7_PINCTRL_REG_BANKS;
+
+	/* Create state holders etc for this driver */
+	pmx = devm_kzalloc(&pdev->dev, sizeof(*pmx), GFP_KERNEL);
+	if (!pmx)
+		return -ENOMEM;
+
+	pmx->dev = &pdev->dev;
+
+	pmx->pctl_data = &atlas7_ioc_data;
+	pmx->pctl_desc.name = "pinctrl-atlas7";
+	pmx->pctl_desc.pins = pmx->pctl_data->pads;
+	pmx->pctl_desc.npins = pmx->pctl_data->pads_cnt;
+	pmx->pctl_desc.pctlops = &atlas7_pinctrl_ops;
+	pmx->pctl_desc.pmxops = &atlas7_pinmux_ops;
+	pmx->pctl_desc.confops = &atlas7_pinconf_ops;
+
+	for (idx = 0; idx < banks; idx++) {
+		pmx->regs[idx] = of_iomap(np, idx);
+		if (!pmx->regs[idx]) {
+			dev_err(&pdev->dev,
+			"can't map ioc bank#%d registers\n", idx);
+			ret = -ENOMEM;
+			goto unmap_io;
+		}
+	}
+
+	/* Now register the pin controller and all pins it handles */
+	pmx->pctl = pinctrl_register(&pmx->pctl_desc, &pdev->dev, pmx);
+	if (IS_ERR(pmx->pctl)) {
+		dev_err(&pdev->dev, "could not register atlas7 pinmux driver\n");
+		ret = PTR_ERR(pmx->pctl);
+		goto unmap_io;
+	}
+
+	platform_set_drvdata(pdev, pmx);
+
+	dev_info(&pdev->dev, "initialized atlas7 pinmux driver\n");
+
+	return 0;
+
+unmap_io:
+	for (idx = 0; idx < banks; idx++) {
+		if (!pmx->regs[idx])
+			break;
+		iounmap(pmx->regs[idx]);
+	}
+
+	return ret;
+}
+
+static const struct of_device_id atlas7_pinmux_ids[] = {
+	{ .compatible = "sirf,atlas7-ioc",},
+};
+
+static struct platform_driver atlas7_pinmux_driver = {
+	.driver = {
+		.name = "atlas7-ioc",
+		.of_match_table = atlas7_pinmux_ids,
+	},
+	.probe = atlas7_pinmux_probe,
+};
+
+static int __init atlas7_pinmux_init(void)
+{
+	return platform_driver_register(&atlas7_pinmux_driver);
+}
+arch_initcall(atlas7_pinmux_init);
+
+
+/**
+ * The Following is GPIO Code
+ */
+static inline struct
+atlas7_gpio_bank *atlas7_gpio_to_bank(struct atlas7_gpio_chip *a7gc, u32 gpio)
+{
+	return &a7gc->banks[GPIO_TO_BANK(gpio)];
+}
+
+static int __atlas7_gpio_to_pin(struct atlas7_gpio_chip *a7gc, u32 gpio)
+{
+	struct atlas7_gpio_bank *bank;
+	u32 ofs;
+
+	bank = atlas7_gpio_to_bank(a7gc, gpio);
+	ofs = gpio - bank->gpio_offset;
+	if (ofs >= bank->ngpio)
+		return -ENODEV;
+
+	return bank->gpio_pins[ofs];
+}
+
+static void atlas7_gpio_irq_ack(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_bank *bank;
+	void __iomem *ctrl_reg;
+	u32 val, pin_in_bank;
+	unsigned long flags;
+
+	bank = atlas7_gpio_to_bank(a7gc, d->hwirq);
+	pin_in_bank = d->hwirq - bank->gpio_offset;
+	ctrl_reg = ATLAS7_GPIO_CTRL(bank, pin_in_bank);
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	val = readl(ctrl_reg);
+	/* clear interrupt status */
+	writel(val, ctrl_reg);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+}
+
+static void __atlas7_gpio_irq_mask(struct atlas7_gpio_chip *a7gc, int idx)
+{
+	struct atlas7_gpio_bank *bank;
+	void __iomem *ctrl_reg;
+	u32 val, pin_in_bank;
+
+	bank = atlas7_gpio_to_bank(a7gc, idx);
+	pin_in_bank = idx - bank->gpio_offset;
+	ctrl_reg = ATLAS7_GPIO_CTRL(bank, pin_in_bank);
+
+	val = readl(ctrl_reg);
+	val &= ~(ATLAS7_GPIO_CTL_INTR_EN_MASK |
+		ATLAS7_GPIO_CTL_INTR_STATUS_MASK);
+	writel(val, ctrl_reg);
+}
+
+static void atlas7_gpio_irq_mask(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	__atlas7_gpio_irq_mask(a7gc, d->hwirq);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+}
+
+static void atlas7_gpio_irq_unmask(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_bank *bank;
+	void __iomem *ctrl_reg;
+	u32 val, pin_in_bank;
+	unsigned long flags;
+
+	bank = atlas7_gpio_to_bank(a7gc, d->hwirq);
+	pin_in_bank = d->hwirq - bank->gpio_offset;
+	ctrl_reg = ATLAS7_GPIO_CTRL(bank, pin_in_bank);
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	val = readl(ctrl_reg);
+	val &= ~ATLAS7_GPIO_CTL_INTR_STATUS_MASK;
+	val |= ATLAS7_GPIO_CTL_INTR_EN_MASK;
+	writel(val, ctrl_reg);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+}
+
+static int atlas7_gpio_irq_type(struct irq_data *d,
+				unsigned int type)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_bank *bank;
+	void __iomem *ctrl_reg;
+	u32 val, pin_in_bank;
+	unsigned long flags;
+
+	bank = atlas7_gpio_to_bank(a7gc, d->hwirq);
+	pin_in_bank = d->hwirq - bank->gpio_offset;
+	ctrl_reg = ATLAS7_GPIO_CTRL(bank, pin_in_bank);
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	val = readl(ctrl_reg);
+	val &= ~(ATLAS7_GPIO_CTL_INTR_STATUS_MASK |
+		ATLAS7_GPIO_CTL_INTR_EN_MASK);
+
+	switch (type) {
+	case IRQ_TYPE_NONE:
+		break;
+
+	case IRQ_TYPE_EDGE_RISING:
+		val |= ATLAS7_GPIO_CTL_INTR_HIGH_MASK |
+			ATLAS7_GPIO_CTL_INTR_TYPE_MASK;
+		val &= ~ATLAS7_GPIO_CTL_INTR_LOW_MASK;
+		break;
+
+	case IRQ_TYPE_EDGE_FALLING:
+		val &= ~ATLAS7_GPIO_CTL_INTR_HIGH_MASK;
+		val |= ATLAS7_GPIO_CTL_INTR_LOW_MASK |
+			ATLAS7_GPIO_CTL_INTR_TYPE_MASK;
+		break;
+
+	case IRQ_TYPE_EDGE_BOTH:
+		val |= ATLAS7_GPIO_CTL_INTR_HIGH_MASK |
+			ATLAS7_GPIO_CTL_INTR_LOW_MASK |
+			ATLAS7_GPIO_CTL_INTR_TYPE_MASK;
+		break;
+
+	case IRQ_TYPE_LEVEL_LOW:
+		val &= ~(ATLAS7_GPIO_CTL_INTR_HIGH_MASK |
+			ATLAS7_GPIO_CTL_INTR_TYPE_MASK);
+		val |= ATLAS7_GPIO_CTL_INTR_LOW_MASK;
+		break;
+
+	case IRQ_TYPE_LEVEL_HIGH:
+		val |= ATLAS7_GPIO_CTL_INTR_HIGH_MASK;
+		val &= ~(ATLAS7_GPIO_CTL_INTR_LOW_MASK |
+			ATLAS7_GPIO_CTL_INTR_TYPE_MASK);
+		break;
+	}
+
+	writel(val, ctrl_reg);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+
+	return 0;
+}
+
+static struct irq_chip atlas7_gpio_irq_chip = {
+	.name = "atlas7-gpio-irq",
+	.irq_ack = atlas7_gpio_irq_ack,
+	.irq_mask = atlas7_gpio_irq_mask,
+	.irq_unmask = atlas7_gpio_irq_unmask,
+	.irq_set_type = atlas7_gpio_irq_type,
+};
+
+static void atlas7_gpio_handle_irq(unsigned int irq, struct irq_desc *desc)
+{
+	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_bank *bank = NULL;
+	u32 status, ctrl;
+	int pin_in_bank = 0, idx;
+	struct irq_chip *chip = irq_get_chip(irq);
+
+	for (idx = 0; idx < a7gc->nbank; idx++) {
+		bank = &a7gc->banks[idx];
+		if (bank->irq == irq)
+			break;
+	}
+	BUG_ON(idx == a7gc->nbank);
+
+	chained_irq_enter(chip, desc);
+
+	status = readl(ATLAS7_GPIO_INT_STATUS(bank));
+	if (!status) {
+		pr_warn("%s: gpio [%s] status %#x no interrupt is flaged\n",
+			__func__, gc->label, status);
+		handle_bad_irq(irq, desc);
+		return;
+	}
+
+	while (status) {
+		ctrl = readl(ATLAS7_GPIO_CTRL(bank, pin_in_bank));
+
+		/*
+		 * Here we must check whether the corresponding GPIO's
+		 * interrupt has been enabled, otherwise just skip it
+		 */
+		if ((status & 0x1) && (ctrl & ATLAS7_GPIO_CTL_INTR_EN_MASK)) {
+			pr_debug("%s: chip[%s] gpio:%d happens\n",
+				__func__, gc->label,
+				bank->gpio_offset + pin_in_bank);
+			generic_handle_irq(
+				irq_find_mapping(gc->irqdomain,
+					bank->gpio_offset + pin_in_bank));
+		}
+
+		if (++pin_in_bank >= bank->ngpio)
+			break;
+
+		status = status >> 1;
+	}
+
+	chained_irq_exit(chip, desc);
+}
+
+static void __atlas7_gpio_set_input(struct atlas7_gpio_chip *a7gc,
+				unsigned int gpio)
+{
+	struct atlas7_gpio_bank *bank;
+	void __iomem *ctrl_reg;
+	u32 val, pin_in_bank;
+
+	bank = atlas7_gpio_to_bank(a7gc, gpio);
+	pin_in_bank = gpio - bank->gpio_offset;
+	ctrl_reg = ATLAS7_GPIO_CTRL(bank, pin_in_bank);
+
+	val = readl(ctrl_reg);
+	val &= ~ATLAS7_GPIO_CTL_OUT_EN_MASK;
+	writel(val, ctrl_reg);
+}
+
+static int atlas7_gpio_request(struct gpio_chip *chip,
+				unsigned int gpio)
+{
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	int ret;
+	unsigned long flags;
+
+	ret = __atlas7_gpio_to_pin(a7gc, gpio);
+	if (ret < 0)
+		return ret;
+
+	if (pinctrl_request_gpio(chip->base + gpio))
+		return -ENODEV;
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	/*
+	 * default status:
+	 * set direction as input and mask irq
+	 */
+	__atlas7_gpio_set_input(a7gc, gpio);
+	__atlas7_gpio_irq_mask(a7gc, gpio);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+
+	return 0;
+}
+
+static void atlas7_gpio_free(struct gpio_chip *chip,
+				unsigned int gpio)
+{
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	unsigned long flags;
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	__atlas7_gpio_irq_mask(a7gc, gpio);
+	__atlas7_gpio_set_input(a7gc, gpio);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+
+	pinctrl_free_gpio(chip->base + gpio);
+}
+
+static int atlas7_gpio_direction_input(struct gpio_chip *chip,
+					unsigned int gpio)
+{
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	unsigned long flags;
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	__atlas7_gpio_set_input(a7gc, gpio);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+
+	return 0;
+}
+
+static void __atlas7_gpio_set_output(struct atlas7_gpio_chip *a7gc,
+			   unsigned int gpio, int value)
+{
+	struct atlas7_gpio_bank *bank;
+	void __iomem *ctrl_reg;
+	u32 out_ctrl, pin_in_bank;
+
+	bank = atlas7_gpio_to_bank(a7gc, gpio);
+	pin_in_bank = gpio - bank->gpio_offset;
+	ctrl_reg = ATLAS7_GPIO_CTRL(bank, pin_in_bank);
+
+	out_ctrl = readl(ctrl_reg);
+	if (value)
+		out_ctrl |= ATLAS7_GPIO_CTL_DATAOUT_MASK;
+	else
+		out_ctrl &= ~ATLAS7_GPIO_CTL_DATAOUT_MASK;
+
+	out_ctrl &= ~ATLAS7_GPIO_CTL_INTR_EN_MASK;
+	out_ctrl |= ATLAS7_GPIO_CTL_OUT_EN_MASK;
+	writel(out_ctrl, ctrl_reg);
+}
+
+static int atlas7_gpio_direction_output(struct gpio_chip *chip,
+				unsigned int gpio, int value)
+{
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	unsigned long flags;
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	__atlas7_gpio_set_output(a7gc, gpio, value);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+
+	return 0;
+}
+
+static int atlas7_gpio_get_value(struct gpio_chip *chip,
+					unsigned int gpio)
+{
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	struct atlas7_gpio_bank *bank;
+	u32 val, pin_in_bank;
+	unsigned long flags;
+
+	bank = atlas7_gpio_to_bank(a7gc, gpio);
+	pin_in_bank = gpio - bank->gpio_offset;
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	val = readl(ATLAS7_GPIO_CTRL(bank, pin_in_bank));
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+
+	return !!(val & ATLAS7_GPIO_CTL_DATAIN_MASK);
+}
+
+static void atlas7_gpio_set_value(struct gpio_chip *chip,
+				unsigned int gpio, int value)
+{
+	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	struct atlas7_gpio_bank *bank;
+	void __iomem *ctrl_reg;
+	u32 ctrl, pin_in_bank;
+	unsigned long flags;
+
+	bank = atlas7_gpio_to_bank(a7gc, gpio);
+	pin_in_bank = gpio - bank->gpio_offset;
+	ctrl_reg = ATLAS7_GPIO_CTRL(bank, pin_in_bank);
+
+	spin_lock_irqsave(&a7gc->lock, flags);
+
+	ctrl = readl(ctrl_reg);
+	if (value)
+		ctrl |= ATLAS7_GPIO_CTL_DATAOUT_MASK;
+	else
+		ctrl &= ~ATLAS7_GPIO_CTL_DATAOUT_MASK;
+	writel(ctrl, ctrl_reg);
+
+	spin_unlock_irqrestore(&a7gc->lock, flags);
+}
+
+static const struct of_device_id atlas7_gpio_ids[] = {
+	{ .compatible = "sirf,atlas7-gpio", },
+};
+
+static int atlas7_gpio_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct atlas7_gpio_chip *a7gc;
+	struct gpio_chip *chip;
+	u32 nbank;
+	int ret, idx;
+
+	ret = of_property_read_u32(np, "gpio-banks", &nbank);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Could not find GPIO bank info,ret=%d!\n",
+			ret);
+		return ret;
+	}
+
+	/* retrieve gpio descriptor data */
+	a7gc = devm_kzalloc(&pdev->dev, sizeof(*a7gc) +
+			sizeof(struct atlas7_gpio_bank) * nbank, GFP_KERNEL);
+	if (!a7gc)
+		return -ENOMEM;
+
+	/* Get Gpio clk */
+	a7gc->clk = of_clk_get(np, 0);
+	if (!IS_ERR(a7gc->clk)) {
+		ret = clk_prepare_enable(a7gc->clk);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"Could not enable clock!\n");
+			return ret;
+		}
+	}
+
+	/* Get Gpio Registers */
+	a7gc->reg = of_iomap(np, 0);
+	if (!a7gc->reg) {
+		dev_err(&pdev->dev, "Could not map GPIO Registers!\n");
+		return -ENOMEM;
+	}
+
+	a7gc->nbank = nbank;
+	spin_lock_init(&a7gc->lock);
+
+	/* Setup GPIO Chip */
+	chip = &a7gc->chip;
+	chip->request = atlas7_gpio_request;
+	chip->free = atlas7_gpio_free;
+	chip->direction_input = atlas7_gpio_direction_input;
+	chip->get = atlas7_gpio_get_value;
+	chip->direction_output = atlas7_gpio_direction_output;
+	chip->set = atlas7_gpio_set_value;
+	chip->base = -1;
+	/* Each chip can support 32 pins at one bank */
+	chip->ngpio = NGPIO_OF_BANK * nbank;
+	chip->label = kstrdup(np->name, GFP_KERNEL);
+	chip->of_node = np;
+	chip->of_gpio_n_cells = 2;
+	chip->dev = &pdev->dev;
+
+	/* Add gpio chip to system */
+	ret = gpiochip_add(chip);
+	if (ret) {
+		dev_err(&pdev->dev,
+		"%s: error in probe function with status %d\n",
+		np->name, ret);
+		goto failed;
+	}
+
+	/* Add gpio chip to irq subsystem */
+	ret =  gpiochip_irqchip_add(chip, &atlas7_gpio_irq_chip,
+			0, handle_level_irq, IRQ_TYPE_NONE);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"could not connect irqchip to gpiochip\n");
+		goto failed;
+	}
+
+	for (idx = 0; idx < nbank; idx++) {
+		struct gpio_pin_range *pin_range;
+		struct atlas7_gpio_bank *bank;
+
+		bank = &a7gc->banks[idx];
+		/* Set ctrl registers' base of this bank */
+		bank->base = ATLAS7_GPIO_BASE(a7gc, idx);
+
+		/* Get interrupt number from DTS */
+		ret = of_irq_get(np, idx);
+		if (ret == -EPROBE_DEFER) {
+			dev_err(&pdev->dev,
+				"Unable to find IRQ number. ret=%d\n", ret);
+			goto failed;
+		}
+		bank->irq = ret;
+
+		gpiochip_set_chained_irqchip(chip, &atlas7_gpio_irq_chip,
+					bank->irq, atlas7_gpio_handle_irq);
+
+		/* Records gpio_pin_range to a7gc */
+		list_for_each_entry(pin_range, &chip->pin_ranges, node) {
+			struct pinctrl_gpio_range *range;
+
+			range = &pin_range->range;
+			if (range->id == NGPIO_OF_BANK * idx) {
+				bank->gpio_offset = range->id;
+				bank->ngpio = range->npins;
+				bank->gpio_pins = range->pins;
+				bank->pctldev = pin_range->pctldev;
+				break;
+			}
+		}
+
+		BUG_ON(!bank->pctldev);
+	}
+
+	dev_info(&pdev->dev, "add to system.\n");
+	return 0;
+failed:
+	return ret;
+}
+
+static struct platform_driver atlas7_gpio_driver = {
+	.driver = {
+		.name = "atlas7-gpio",
+		.owner = THIS_MODULE,
+		.of_match_table = atlas7_gpio_ids,
+	},
+	.probe = atlas7_gpio_probe,
+};
+
+static int __init atlas7_gpio_init(void)
+{
+	return platform_driver_register(&atlas7_gpio_driver);
+}
+subsys_initcall(atlas7_gpio_init);
+
+MODULE_DESCRIPTION("SIRFSOC Atlas7 pin control driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sirf/pinctrl-sirf.c b/drivers/pinctrl/sirf/pinctrl-sirf.c
index e2efbbae4061..8ba26e45499a 100644
--- a/drivers/pinctrl/sirf/pinctrl-sirf.c
+++ b/drivers/pinctrl/sirf/pinctrl-sirf.c
@@ -310,9 +310,9 @@ static int sirfsoc_pinmux_probe(struct platform_device *pdev)
 
 	/* Now register the pin controller and all pins it handles */
 	spmx->pmx = pinctrl_register(&sirfsoc_pinmux_desc, &pdev->dev, spmx);
-	if (!spmx->pmx) {
+	if (IS_ERR(spmx->pmx)) {
 		dev_err(&pdev->dev, "could not register SIRFSOC pinmux driver\n");
-		ret = -EINVAL;
+		ret = PTR_ERR(spmx->pmx);
 		goto out_no_pmx;
 	}
 
diff --git a/drivers/pinctrl/spear/pinctrl-spear.c b/drivers/pinctrl/spear/pinctrl-spear.c
index abdb05ac43dc..f87a5eaf75da 100644
--- a/drivers/pinctrl/spear/pinctrl-spear.c
+++ b/drivers/pinctrl/spear/pinctrl-spear.c
@@ -396,9 +396,9 @@ int spear_pinctrl_probe(struct platform_device *pdev,
 	spear_pinctrl_desc.npins = machdata->npins;
 
 	pmx->pctl = pinctrl_register(&spear_pinctrl_desc, &pdev->dev, pmx);
-	if (!pmx->pctl) {
+	if (IS_ERR(pmx->pctl)) {
 		dev_err(&pdev->dev, "Couldn't register pinctrl driver\n");
-		return -ENODEV;
+		return PTR_ERR(pmx->pctl);
 	}
 
 	return 0;
diff --git a/drivers/pinctrl/sunxi/Kconfig b/drivers/pinctrl/sunxi/Kconfig
index 2eb893e0ea1e..ae27872ff3a6 100644
--- a/drivers/pinctrl/sunxi/Kconfig
+++ b/drivers/pinctrl/sunxi/Kconfig
@@ -38,6 +38,10 @@ config PINCTRL_SUN8I_A23
 	def_bool MACH_SUN8I
 	select PINCTRL_SUNXI_COMMON
 
+config PINCTRL_SUN8I_A33
+	def_bool MACH_SUN8I
+	select PINCTRL_SUNXI_COMMON
+
 config PINCTRL_SUN8I_A23_R
 	def_bool MACH_SUN8I
 	depends on RESET_CONTROLLER
diff --git a/drivers/pinctrl/sunxi/Makefile b/drivers/pinctrl/sunxi/Makefile
index b796d579dce6..227a1213947c 100644
--- a/drivers/pinctrl/sunxi/Makefile
+++ b/drivers/pinctrl/sunxi/Makefile
@@ -11,4 +11,5 @@ obj-$(CONFIG_PINCTRL_SUN6I_A31_R)	+= pinctrl-sun6i-a31-r.o
 obj-$(CONFIG_PINCTRL_SUN7I_A20)		+= pinctrl-sun7i-a20.o
 obj-$(CONFIG_PINCTRL_SUN8I_A23)		+= pinctrl-sun8i-a23.o
 obj-$(CONFIG_PINCTRL_SUN8I_A23_R)	+= pinctrl-sun8i-a23-r.o
+obj-$(CONFIG_PINCTRL_SUN8I_A33)		+= pinctrl-sun8i-a33.o
 obj-$(CONFIG_PINCTRL_SUN9I_A80)		+= pinctrl-sun9i-a80.o
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31s.c b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31s.c
index d3725dcd6979..e570d5c93ecc 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun6i-a31s.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun6i-a31s.c
@@ -804,7 +804,6 @@ static struct platform_driver sun6i_a31s_pinctrl_driver = {
 	.probe	= sun6i_a31s_pinctrl_probe,
 	.driver	= {
 		.name		= "sun6i-a31s-pinctrl",
-		.owner		= THIS_MODULE,
 		.of_match_table	= sun6i_a31s_pinctrl_match,
 	},
 };
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun8i-a33.c b/drivers/pinctrl/sunxi/pinctrl-sun8i-a33.c
new file mode 100644
index 000000000000..00265f0435a7
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun8i-a33.c
@@ -0,0 +1,513 @@
+/*
+ * Allwinner a33 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2015 Vishnu Patekar <vishnupatekar0510@gmail.com>
+ *
+ * Based on pinctrl-sun8i-a23.c, which is:
+ * Copyright (C) 2014 Chen-Yu Tsai <wens@csie.org>
+ * Copyright (C) 2014 Maxime Ripard <maxime.ripard@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/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun8i_a33_pins[] = {
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* TX */
+		  SUNXI_FUNCTION(0x3, "uart0"),		/* TX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 0, 0)),	/* PB_EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* RX */
+		  SUNXI_FUNCTION(0x3, "uart0"),		/* RX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 0, 1)),	/* PB_EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 0, 2)),	/* PB_EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 0, 3)),	/* PB_EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "aif2"),		/* SYNC */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 0, 4)),	/* PB_EINT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* BCLK */
+		  SUNXI_FUNCTION(0x3, "aif2"),		/* BCLK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 0, 5)),	/* PB_EINT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DOUT */
+		  SUNXI_FUNCTION(0x3, "aif2"),		/* DOUT */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 0, 6)),	/* PB_EINT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(B, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DIN */
+		  SUNXI_FUNCTION(0x3, "aif2"),		/* DIN */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 0, 7)),	/* PB_EINT7 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* WE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* ALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* CLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* CE1 */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* RE */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* RB1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ4 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ5 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand"),		/* DQ6 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand"),		/* DQ7 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand"),		/* DQS */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* RST */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "mmc1")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "mmc1")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "mmc1")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "mmc1")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "mmc1")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "mmc1")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D10 */
+		  SUNXI_FUNCTION(0x3, "uart1")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D11 */
+		  SUNXI_FUNCTION(0x3, "uart1")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D12 */
+		  SUNXI_FUNCTION(0x3, "uart1")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D13 */
+		  SUNXI_FUNCTION(0x3, "uart1")),	/* CTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0")),		/* D15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D18 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D19 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D20 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D21 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 22),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D22 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 23),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* D23 */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 24),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VPC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 25),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* DE */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 26),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VP3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 27),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "lcd0"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "lvds0")),	/* VN3 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* PCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* HSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* VSYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* SCK */
+		  SUNXI_FUNCTION(0x3, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* SDA */
+		  SUNXI_FUNCTION(0x3, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart0")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "uart0")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK1 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 0)),	/* PG_EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 1)),	/* PG_EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D0 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 2)),	/* PG_EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D1 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 3)),	/* PG_EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 4)),	/* PG_EINT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 5)),	/* PG_EINT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart1"),		/* TX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 6)),	/* PG_EINT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart1"),		/* RX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 7)),	/* PG_EINT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 8)),	/* PG_EINT8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 9)),	/* PG_EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s1"),		/* SYNC */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 10)),	/* PG_EINT10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s1"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 11)),	/* PG_EINT11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s1"),		/* DOUT */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 12)),	/* PG_EINT12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s1"),		/* DIN */
+		  SUNXI_FUNCTION_IRQ_BANK(0x4, 1, 13)),	/* PG_EINT13 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm0")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "pwm1")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c1")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CS */
+		  SUNXI_FUNCTION(0x3, "uart3")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart3")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* DOUT */
+		  SUNXI_FUNCTION(0x3, "uart3")),	/* RTS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(H, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi0"),		/* DIN */
+		  SUNXI_FUNCTION(0x3, "uart3")),	/* CTS */
+};
+
+static const struct sunxi_pinctrl_desc sun8i_a33_pinctrl_data = {
+	.pins = sun8i_a33_pins,
+	.npins = ARRAY_SIZE(sun8i_a33_pins),
+	.irq_banks = 2,
+};
+
+static int sun8i_a33_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun8i_a33_pinctrl_data);
+}
+
+static const struct of_device_id sun8i_a33_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun8i-a33-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun8i_a33_pinctrl_match);
+
+static struct platform_driver sun8i_a33_pinctrl_driver = {
+	.probe	= sun8i_a33_pinctrl_probe,
+	.driver	= {
+		.name		= "sun8i-a33-pinctrl",
+		.of_match_table	= sun8i_a33_pinctrl_match,
+	},
+};
+module_platform_driver(sun8i_a33_pinctrl_driver);
+
+MODULE_AUTHOR("Vishnu Patekar <vishnupatekar0510@gmail.com>");
+MODULE_DESCRIPTION("Allwinner a33 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.c b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
index f8e171b76693..f09573e13203 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sunxi.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
@@ -911,9 +911,9 @@ int sunxi_pinctrl_init(struct platform_device *pdev,
 
 	pctl->pctl_dev = pinctrl_register(pctrl_desc,
 					  &pdev->dev, pctl);
-	if (!pctl->pctl_dev) {
+	if (IS_ERR(pctl->pctl_dev)) {
 		dev_err(&pdev->dev, "couldn't register pinctrl driver\n");
-		return -EINVAL;
+		return PTR_ERR(pctl->pctl_dev);
 	}
 
 	pctl->chip = devm_kzalloc(&pdev->dev, sizeof(*pctl->chip), GFP_KERNEL);
@@ -1005,9 +1005,9 @@ int sunxi_pinctrl_init(struct platform_device *pdev,
 		writel(0xffffffff,
 			pctl->membase + sunxi_irq_status_reg_from_bank(i));
 
-		irq_set_chained_handler(pctl->irq[i],
-					sunxi_pinctrl_irq_handler);
-		irq_set_handler_data(pctl->irq[i], pctl);
+		irq_set_chained_handler_and_data(pctl->irq[i],
+						 sunxi_pinctrl_irq_handler,
+						 pctl);
 	}
 
 	dev_info(&pdev->dev, "initialized sunXi PIO driver\n");
diff --git a/drivers/pinctrl/vt8500/pinctrl-wmt.c b/drivers/pinctrl/vt8500/pinctrl-wmt.c
index d055d63309e4..c15316b003c5 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wmt.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wmt.c
@@ -594,9 +594,9 @@ int wmt_pinctrl_probe(struct platform_device *pdev,
 	data->dev = &pdev->dev;
 
 	data->pctl_dev = pinctrl_register(&wmt_desc, &pdev->dev, data);
-	if (!data->pctl_dev) {
+	if (IS_ERR(data->pctl_dev)) {
 		dev_err(&pdev->dev, "Failed to register pinctrl\n");
-		return -EINVAL;
+		return PTR_ERR(data->pctl_dev);
 	}
 
 	err = gpiochip_add(&data->gpio_chip);
diff --git a/drivers/platform/chrome/Kconfig b/drivers/platform/chrome/Kconfig
index 2a6531a5fde8..cb1329919527 100644
--- a/drivers/platform/chrome/Kconfig
+++ b/drivers/platform/chrome/Kconfig
@@ -40,7 +40,7 @@ config CHROMEOS_PSTORE
 
 config CROS_EC_CHARDEV
         tristate "Chrome OS Embedded Controller userspace device interface"
-        depends on MFD_CROS_EC
+        depends on CROS_EC_PROTO
         ---help---
           This driver adds support to talk with the ChromeOS EC from userspace.
 
@@ -49,7 +49,7 @@ config CROS_EC_CHARDEV
 
 config CROS_EC_LPC
         tristate "ChromeOS Embedded Controller (LPC)"
-        depends on MFD_CROS_EC && (X86 || COMPILE_TEST)
+        depends on MFD_CROS_EC && CROS_EC_PROTO && (X86 || COMPILE_TEST)
         help
           If you say Y here, you get support for talking to the ChromeOS EC
           over an LPC bus. This uses a simple byte-level protocol with a
@@ -59,4 +59,9 @@ config CROS_EC_LPC
           To compile this driver as a module, choose M here: the
           module will be called cros_ec_lpc.
 
+config CROS_EC_PROTO
+        bool
+        help
+          ChromeOS EC communication protocol helpers.
+
 endif # CHROMEOS_PLATFORMS
diff --git a/drivers/platform/chrome/Makefile b/drivers/platform/chrome/Makefile
index bd8d8601e875..4a11b010f5d8 100644
--- a/drivers/platform/chrome/Makefile
+++ b/drivers/platform/chrome/Makefile
@@ -4,3 +4,4 @@ obj-$(CONFIG_CHROMEOS_PSTORE)	+= chromeos_pstore.o
 cros_ec_devs-objs               := cros_ec_dev.o cros_ec_sysfs.o cros_ec_lightbar.o
 obj-$(CONFIG_CROS_EC_CHARDEV)   += cros_ec_devs.o
 obj-$(CONFIG_CROS_EC_LPC)       += cros_ec_lpc.o
+obj-$(CONFIG_CROS_EC_PROTO)	+= cros_ec_proto.o
diff --git a/drivers/platform/chrome/cros_ec_dev.c b/drivers/platform/chrome/cros_ec_dev.c
index 6090d0b2826f..e8fcdc237029 100644
--- a/drivers/platform/chrome/cros_ec_dev.c
+++ b/drivers/platform/chrome/cros_ec_dev.c
@@ -20,44 +20,59 @@
 #include <linux/fs.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <linux/uaccess.h>
 
 #include "cros_ec_dev.h"
 
 /* Device variables */
 #define CROS_MAX_DEV 128
-static struct class *cros_class;
 static int ec_major;
 
+static const struct attribute_group *cros_ec_groups[] = {
+	&cros_ec_attr_group,
+	&cros_ec_lightbar_attr_group,
+	NULL,
+};
+
+static struct class cros_class = {
+	.owner          = THIS_MODULE,
+	.name           = "chromeos",
+	.dev_groups     = cros_ec_groups,
+};
+
 /* Basic communication */
-static int ec_get_version(struct cros_ec_device *ec, char *str, int maxlen)
+static int ec_get_version(struct cros_ec_dev *ec, char *str, int maxlen)
 {
 	struct ec_response_get_version *resp;
 	static const char * const current_image_name[] = {
 		"unknown", "read-only", "read-write", "invalid",
 	};
-	struct cros_ec_command msg = {
-		.version = 0,
-		.command = EC_CMD_GET_VERSION,
-		.outdata = { 0 },
-		.outsize = 0,
-		.indata = { 0 },
-		.insize = sizeof(*resp),
-	};
+	struct cros_ec_command *msg;
 	int ret;
 
-	ret = cros_ec_cmd_xfer(ec, &msg);
+	msg = kmalloc(sizeof(*msg) + sizeof(*resp), GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	msg->version = 0;
+	msg->command = EC_CMD_GET_VERSION + ec->cmd_offset;
+	msg->insize = sizeof(*resp);
+	msg->outsize = 0;
+
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 	if (ret < 0)
-		return ret;
+		goto exit;
 
-	if (msg.result != EC_RES_SUCCESS) {
+	if (msg->result != EC_RES_SUCCESS) {
 		snprintf(str, maxlen,
 			 "%s\nUnknown EC version: EC returned %d\n",
-			 CROS_EC_DEV_VERSION, msg.result);
-		return 0;
+			 CROS_EC_DEV_VERSION, msg->result);
+		ret = -EINVAL;
+		goto exit;
 	}
 
-	resp = (struct ec_response_get_version *)msg.indata;
+	resp = (struct ec_response_get_version *)msg->data;
 	if (resp->current_image >= ARRAY_SIZE(current_image_name))
 		resp->current_image = 3; /* invalid */
 
@@ -65,14 +80,19 @@ static int ec_get_version(struct cros_ec_device *ec, char *str, int maxlen)
 		 resp->version_string_ro, resp->version_string_rw,
 		 current_image_name[resp->current_image]);
 
-	return 0;
+	ret = 0;
+exit:
+	kfree(msg);
+	return ret;
 }
 
 /* Device file ops */
 static int ec_device_open(struct inode *inode, struct file *filp)
 {
-	filp->private_data = container_of(inode->i_cdev,
-					  struct cros_ec_device, cdev);
+	struct cros_ec_dev *ec = container_of(inode->i_cdev,
+					      struct cros_ec_dev, cdev);
+	filp->private_data = ec;
+	nonseekable_open(inode, filp);
 	return 0;
 }
 
@@ -84,7 +104,7 @@ static int ec_device_release(struct inode *inode, struct file *filp)
 static ssize_t ec_device_read(struct file *filp, char __user *buffer,
 			      size_t length, loff_t *offset)
 {
-	struct cros_ec_device *ec = filp->private_data;
+	struct cros_ec_dev *ec = filp->private_data;
 	char msg[sizeof(struct ec_response_get_version) +
 		 sizeof(CROS_EC_DEV_VERSION)];
 	size_t count;
@@ -107,38 +127,53 @@ static ssize_t ec_device_read(struct file *filp, char __user *buffer,
 }
 
 /* Ioctls */
-static long ec_device_ioctl_xcmd(struct cros_ec_device *ec, void __user *arg)
+static long ec_device_ioctl_xcmd(struct cros_ec_dev *ec, void __user *arg)
 {
 	long ret;
-	struct cros_ec_command s_cmd = { };
+	struct cros_ec_command u_cmd;
+	struct cros_ec_command *s_cmd;
 
-	if (copy_from_user(&s_cmd, arg, sizeof(s_cmd)))
+	if (copy_from_user(&u_cmd, arg, sizeof(u_cmd)))
 		return -EFAULT;
 
-	ret = cros_ec_cmd_xfer(ec, &s_cmd);
+	s_cmd = kmalloc(sizeof(*s_cmd) + max(u_cmd.outsize, u_cmd.insize),
+			GFP_KERNEL);
+	if (!s_cmd)
+		return -ENOMEM;
+
+	if (copy_from_user(s_cmd, arg, sizeof(*s_cmd) + u_cmd.outsize)) {
+		ret = -EFAULT;
+		goto exit;
+	}
+
+	s_cmd->command += ec->cmd_offset;
+	ret = cros_ec_cmd_xfer(ec->ec_dev, s_cmd);
 	/* Only copy data to userland if data was received. */
 	if (ret < 0)
-		return ret;
-
-	if (copy_to_user(arg, &s_cmd, sizeof(s_cmd)))
-		return -EFAULT;
+		goto exit;
 
-	return 0;
+	if (copy_to_user(arg, s_cmd, sizeof(*s_cmd) + u_cmd.insize))
+		ret = -EFAULT;
+exit:
+	kfree(s_cmd);
+	return ret;
 }
 
-static long ec_device_ioctl_readmem(struct cros_ec_device *ec, void __user *arg)
+static long ec_device_ioctl_readmem(struct cros_ec_dev *ec, void __user *arg)
 {
+	struct cros_ec_device *ec_dev = ec->ec_dev;
 	struct cros_ec_readmem s_mem = { };
 	long num;
 
 	/* Not every platform supports direct reads */
-	if (!ec->cmd_readmem)
+	if (!ec_dev->cmd_readmem)
 		return -ENOTTY;
 
 	if (copy_from_user(&s_mem, arg, sizeof(s_mem)))
 		return -EFAULT;
 
-	num = ec->cmd_readmem(ec, s_mem.offset, s_mem.bytes, s_mem.buffer);
+	num = ec_dev->cmd_readmem(ec_dev, s_mem.offset, s_mem.bytes,
+				  s_mem.buffer);
 	if (num <= 0)
 		return num;
 
@@ -151,7 +186,7 @@ static long ec_device_ioctl_readmem(struct cros_ec_device *ec, void __user *arg)
 static long ec_device_ioctl(struct file *filp, unsigned int cmd,
 			    unsigned long arg)
 {
-	struct cros_ec_device *ec = filp->private_data;
+	struct cros_ec_dev *ec = filp->private_data;
 
 	if (_IOC_TYPE(cmd) != CROS_EC_DEV_IOC)
 		return -ENOTTY;
@@ -174,45 +209,81 @@ static const struct file_operations fops = {
 	.unlocked_ioctl = ec_device_ioctl,
 };
 
+static void __remove(struct device *dev)
+{
+	struct cros_ec_dev *ec = container_of(dev, struct cros_ec_dev,
+					      class_dev);
+	kfree(ec);
+}
+
 static int ec_device_probe(struct platform_device *pdev)
 {
-	struct cros_ec_device *ec = dev_get_drvdata(pdev->dev.parent);
-	int retval = -ENOTTY;
-	dev_t devno = MKDEV(ec_major, 0);
+	int retval = -ENOMEM;
+	struct device *dev = &pdev->dev;
+	struct cros_ec_platform *ec_platform = dev_get_platdata(dev);
+	dev_t devno = MKDEV(ec_major, pdev->id);
+	struct cros_ec_dev *ec = kzalloc(sizeof(*ec), GFP_KERNEL);
+
+	if (!ec)
+		return retval;
 
-	/* Instantiate it (and remember the EC) */
+	dev_set_drvdata(dev, ec);
+	ec->ec_dev = dev_get_drvdata(dev->parent);
+	ec->dev = dev;
+	ec->cmd_offset = ec_platform->cmd_offset;
+	device_initialize(&ec->class_dev);
 	cdev_init(&ec->cdev, &fops);
 
+	/*
+	 * Add the character device
+	 * Link cdev to the class device to be sure device is not used
+	 * before unbinding it.
+	 */
+	ec->cdev.kobj.parent = &ec->class_dev.kobj;
 	retval = cdev_add(&ec->cdev, devno, 1);
 	if (retval) {
-		dev_err(&pdev->dev, ": failed to add character device\n");
-		return retval;
+		dev_err(dev, ": failed to add character device\n");
+		goto cdev_add_failed;
 	}
 
-	ec->vdev = device_create(cros_class, NULL, devno, ec,
-				 CROS_EC_DEV_NAME);
-	if (IS_ERR(ec->vdev)) {
-		retval = PTR_ERR(ec->vdev);
-		dev_err(&pdev->dev, ": failed to create device\n");
-		cdev_del(&ec->cdev);
-		return retval;
+	/*
+	 * Add the class device
+	 * Link to the character device for creating the /dev entry
+	 * in devtmpfs.
+	 */
+	ec->class_dev.devt = ec->cdev.dev;
+	ec->class_dev.class = &cros_class;
+	ec->class_dev.parent = dev;
+	ec->class_dev.release = __remove;
+
+	retval = dev_set_name(&ec->class_dev, "%s", ec_platform->ec_name);
+	if (retval) {
+		dev_err(dev, "dev_set_name failed => %d\n", retval);
+		goto set_named_failed;
 	}
 
-	/* Initialize extra interfaces */
-	ec_dev_sysfs_init(ec);
-	ec_dev_lightbar_init(ec);
+	retval = device_add(&ec->class_dev);
+	if (retval) {
+		dev_err(dev, "device_register failed => %d\n", retval);
+		goto dev_reg_failed;
+	}
 
 	return 0;
+
+dev_reg_failed:
+set_named_failed:
+	dev_set_drvdata(dev, NULL);
+	cdev_del(&ec->cdev);
+cdev_add_failed:
+	kfree(ec);
+	return retval;
 }
 
 static int ec_device_remove(struct platform_device *pdev)
 {
-	struct cros_ec_device *ec = dev_get_drvdata(pdev->dev.parent);
-
-	ec_dev_lightbar_remove(ec);
-	ec_dev_sysfs_remove(ec);
-	device_destroy(cros_class, MKDEV(ec_major, 0));
+	struct cros_ec_dev *ec = dev_get_drvdata(&pdev->dev);
 	cdev_del(&ec->cdev);
+	device_unregister(&ec->class_dev);
 	return 0;
 }
 
@@ -229,10 +300,10 @@ static int __init cros_ec_dev_init(void)
 	int ret;
 	dev_t dev = 0;
 
-	cros_class = class_create(THIS_MODULE, "chromeos");
-	if (IS_ERR(cros_class)) {
+	ret  = class_register(&cros_class);
+	if (ret) {
 		pr_err(CROS_EC_DEV_NAME ": failed to register device class\n");
-		return PTR_ERR(cros_class);
+		return ret;
 	}
 
 	/* Get a range of minor numbers (starting with 0) to work with */
@@ -254,7 +325,7 @@ static int __init cros_ec_dev_init(void)
 failed_devreg:
 	unregister_chrdev_region(MKDEV(ec_major, 0), CROS_MAX_DEV);
 failed_chrdevreg:
-	class_destroy(cros_class);
+	class_unregister(&cros_class);
 	return ret;
 }
 
@@ -262,7 +333,7 @@ static void __exit cros_ec_dev_exit(void)
 {
 	platform_driver_unregister(&cros_ec_dev_driver);
 	unregister_chrdev(ec_major, CROS_EC_DEV_NAME);
-	class_destroy(cros_class);
+	class_unregister(&cros_class);
 }
 
 module_init(cros_ec_dev_init);
diff --git a/drivers/platform/chrome/cros_ec_dev.h b/drivers/platform/chrome/cros_ec_dev.h
index 45d67f7e518c..bfd2c84c3571 100644
--- a/drivers/platform/chrome/cros_ec_dev.h
+++ b/drivers/platform/chrome/cros_ec_dev.h
@@ -24,7 +24,6 @@
 #include <linux/types.h>
 #include <linux/mfd/cros_ec.h>
 
-#define CROS_EC_DEV_NAME "cros_ec"
 #define CROS_EC_DEV_VERSION "1.0.0"
 
 /*
@@ -44,10 +43,4 @@ struct cros_ec_readmem {
 #define CROS_EC_DEV_IOCXCMD   _IOWR(CROS_EC_DEV_IOC, 0, struct cros_ec_command)
 #define CROS_EC_DEV_IOCRDMEM  _IOWR(CROS_EC_DEV_IOC, 1, struct cros_ec_readmem)
 
-void ec_dev_sysfs_init(struct cros_ec_device *);
-void ec_dev_sysfs_remove(struct cros_ec_device *);
-
-void ec_dev_lightbar_init(struct cros_ec_device *);
-void ec_dev_lightbar_remove(struct cros_ec_device *);
-
 #endif /* _CROS_EC_DEV_H_ */
diff --git a/drivers/platform/chrome/cros_ec_lightbar.c b/drivers/platform/chrome/cros_ec_lightbar.c
index b4ff47a9069a..144e09df9b84 100644
--- a/drivers/platform/chrome/cros_ec_lightbar.c
+++ b/drivers/platform/chrome/cros_ec_lightbar.c
@@ -31,6 +31,7 @@
 #include <linux/sched.h>
 #include <linux/types.h>
 #include <linux/uaccess.h>
+#include <linux/slab.h>
 
 #include "cros_ec_dev.h"
 
@@ -91,55 +92,81 @@ out:
 	return ret;
 }
 
-#define INIT_MSG(P, R) { \
-		.command = EC_CMD_LIGHTBAR_CMD, \
-		.outsize = sizeof(*P), \
-		.insize = sizeof(*R), \
-	}
+static struct cros_ec_command *alloc_lightbar_cmd_msg(struct cros_ec_dev *ec)
+{
+	struct cros_ec_command *msg;
+	int len;
+
+	len = max(sizeof(struct ec_params_lightbar),
+		  sizeof(struct ec_response_lightbar));
+
+	msg = kmalloc(sizeof(*msg) + len, GFP_KERNEL);
+	if (!msg)
+		return NULL;
+
+	msg->version = 0;
+	msg->command = EC_CMD_LIGHTBAR_CMD + ec->cmd_offset;
+	msg->outsize = sizeof(struct ec_params_lightbar);
+	msg->insize = sizeof(struct ec_response_lightbar);
+
+	return msg;
+}
 
-static int get_lightbar_version(struct cros_ec_device *ec,
+static int get_lightbar_version(struct cros_ec_dev *ec,
 				uint32_t *ver_ptr, uint32_t *flg_ptr)
 {
 	struct ec_params_lightbar *param;
 	struct ec_response_lightbar *resp;
-	struct cros_ec_command msg = INIT_MSG(param, resp);
+	struct cros_ec_command *msg;
 	int ret;
 
-	param = (struct ec_params_lightbar *)msg.outdata;
-	param->cmd = LIGHTBAR_CMD_VERSION;
-	ret = cros_ec_cmd_xfer(ec, &msg);
-	if (ret < 0)
+	msg = alloc_lightbar_cmd_msg(ec);
+	if (!msg)
 		return 0;
 
-	switch (msg.result) {
+	param = (struct ec_params_lightbar *)msg->data;
+	param->cmd = LIGHTBAR_CMD_VERSION;
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
+	if (ret < 0) {
+		ret = 0;
+		goto exit;
+	}
+
+	switch (msg->result) {
 	case EC_RES_INVALID_PARAM:
 		/* Pixel had no version command. */
 		if (ver_ptr)
 			*ver_ptr = 0;
 		if (flg_ptr)
 			*flg_ptr = 0;
-		return 1;
+		ret = 1;
+		goto exit;
 
 	case EC_RES_SUCCESS:
-		resp = (struct ec_response_lightbar *)msg.indata;
+		resp = (struct ec_response_lightbar *)msg->data;
 
 		/* Future devices w/lightbars should implement this command */
 		if (ver_ptr)
 			*ver_ptr = resp->version.num;
 		if (flg_ptr)
 			*flg_ptr = resp->version.flags;
-		return 1;
+		ret = 1;
+		goto exit;
 	}
 
 	/* Anything else (ie, EC_RES_INVALID_COMMAND) - no lightbar */
-	return 0;
+	ret = 0;
+exit:
+	kfree(msg);
+	return ret;
 }
 
 static ssize_t version_show(struct device *dev,
 			    struct device_attribute *attr, char *buf)
 {
-	uint32_t version, flags;
-	struct cros_ec_device *ec = dev_get_drvdata(dev);
+	uint32_t version = 0, flags = 0;
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
 	int ret;
 
 	ret = lb_throttle();
@@ -158,30 +185,39 @@ static ssize_t brightness_store(struct device *dev,
 				const char *buf, size_t count)
 {
 	struct ec_params_lightbar *param;
-	struct ec_response_lightbar *resp;
-	struct cros_ec_command msg = INIT_MSG(param, resp);
+	struct cros_ec_command *msg;
 	int ret;
 	unsigned int val;
-	struct cros_ec_device *ec = dev_get_drvdata(dev);
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
 
 	if (kstrtouint(buf, 0, &val))
 		return -EINVAL;
 
-	param = (struct ec_params_lightbar *)msg.outdata;
-	param->cmd = LIGHTBAR_CMD_BRIGHTNESS;
-	param->brightness.num = val;
+	msg = alloc_lightbar_cmd_msg(ec);
+	if (!msg)
+		return -ENOMEM;
+
+	param = (struct ec_params_lightbar *)msg->data;
+	param->cmd = LIGHTBAR_CMD_SET_BRIGHTNESS;
+	param->set_brightness.num = val;
 	ret = lb_throttle();
 	if (ret)
-		return ret;
+		goto exit;
 
-	ret = cros_ec_cmd_xfer(ec, &msg);
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 	if (ret < 0)
-		return ret;
+		goto exit;
 
-	if (msg.result != EC_RES_SUCCESS)
-		return -EINVAL;
+	if (msg->result != EC_RES_SUCCESS) {
+		ret = -EINVAL;
+		goto exit;
+	}
 
-	return count;
+	ret = count;
+exit:
+	kfree(msg);
+	return ret;
 }
 
 
@@ -196,12 +232,16 @@ static ssize_t led_rgb_store(struct device *dev, struct device_attribute *attr,
 			     const char *buf, size_t count)
 {
 	struct ec_params_lightbar *param;
-	struct ec_response_lightbar *resp;
-	struct cros_ec_command msg = INIT_MSG(param, resp);
-	struct cros_ec_device *ec = dev_get_drvdata(dev);
+	struct cros_ec_command *msg;
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
 	unsigned int val[4];
 	int ret, i = 0, j = 0, ok = 0;
 
+	msg = alloc_lightbar_cmd_msg(ec);
+	if (!msg)
+		return -ENOMEM;
+
 	do {
 		/* Skip any whitespace */
 		while (*buf && isspace(*buf))
@@ -215,12 +255,12 @@ static ssize_t led_rgb_store(struct device *dev, struct device_attribute *attr,
 			return -EINVAL;
 
 		if (i == 4) {
-			param = (struct ec_params_lightbar *)msg.outdata;
-			param->cmd = LIGHTBAR_CMD_RGB;
-			param->rgb.led = val[0];
-			param->rgb.red = val[1];
-			param->rgb.green = val[2];
-			param->rgb.blue = val[3];
+			param = (struct ec_params_lightbar *)msg->data;
+			param->cmd = LIGHTBAR_CMD_SET_RGB;
+			param->set_rgb.led = val[0];
+			param->set_rgb.red = val[1];
+			param->set_rgb.green = val[2];
+			param->set_rgb.blue = val[3];
 			/*
 			 * Throttle only the first of every four transactions,
 			 * so that the user can update all four LEDs at once.
@@ -231,12 +271,14 @@ static ssize_t led_rgb_store(struct device *dev, struct device_attribute *attr,
 					return ret;
 			}
 
-			ret = cros_ec_cmd_xfer(ec, &msg);
+			ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 			if (ret < 0)
-				return ret;
+				goto exit;
 
-			if (msg.result != EC_RES_SUCCESS)
-				return -EINVAL;
+			if (msg->result != EC_RES_SUCCESS) {
+				ret = -EINVAL;
+				goto exit;
+			}
 
 			i = 0;
 			ok = 1;
@@ -248,6 +290,8 @@ static ssize_t led_rgb_store(struct device *dev, struct device_attribute *attr,
 
 	} while (*buf);
 
+exit:
+	kfree(msg);
 	return (ok && i == 0) ? count : -EINVAL;
 }
 
@@ -261,41 +305,56 @@ static ssize_t sequence_show(struct device *dev,
 {
 	struct ec_params_lightbar *param;
 	struct ec_response_lightbar *resp;
-	struct cros_ec_command msg = INIT_MSG(param, resp);
+	struct cros_ec_command *msg;
 	int ret;
-	struct cros_ec_device *ec = dev_get_drvdata(dev);
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
+
+	msg = alloc_lightbar_cmd_msg(ec);
+	if (!msg)
+		return -ENOMEM;
 
-	param = (struct ec_params_lightbar *)msg.outdata;
+	param = (struct ec_params_lightbar *)msg->data;
 	param->cmd = LIGHTBAR_CMD_GET_SEQ;
 	ret = lb_throttle();
 	if (ret)
-		return ret;
+		goto exit;
 
-	ret = cros_ec_cmd_xfer(ec, &msg);
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 	if (ret < 0)
-		return ret;
+		goto exit;
 
-	if (msg.result != EC_RES_SUCCESS)
-		return scnprintf(buf, PAGE_SIZE,
-				 "ERROR: EC returned %d\n", msg.result);
+	if (msg->result != EC_RES_SUCCESS) {
+		ret = scnprintf(buf, PAGE_SIZE,
+				"ERROR: EC returned %d\n", msg->result);
+		goto exit;
+	}
 
-	resp = (struct ec_response_lightbar *)msg.indata;
+	resp = (struct ec_response_lightbar *)msg->data;
 	if (resp->get_seq.num >= ARRAY_SIZE(seqname))
-		return scnprintf(buf, PAGE_SIZE, "%d\n", resp->get_seq.num);
+		ret = scnprintf(buf, PAGE_SIZE, "%d\n", resp->get_seq.num);
 	else
-		return scnprintf(buf, PAGE_SIZE, "%s\n",
-				 seqname[resp->get_seq.num]);
+		ret = scnprintf(buf, PAGE_SIZE, "%s\n",
+				seqname[resp->get_seq.num]);
+
+exit:
+	kfree(msg);
+	return ret;
 }
 
 static ssize_t sequence_store(struct device *dev, struct device_attribute *attr,
 			      const char *buf, size_t count)
 {
 	struct ec_params_lightbar *param;
-	struct ec_response_lightbar *resp;
-	struct cros_ec_command msg = INIT_MSG(param, resp);
+	struct cros_ec_command *msg;
 	unsigned int num;
 	int ret, len;
-	struct cros_ec_device *ec = dev_get_drvdata(dev);
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
+
+	msg = alloc_lightbar_cmd_msg(ec);
+	if (!msg)
+		return -ENOMEM;
 
 	for (len = 0; len < count; len++)
 		if (!isalnum(buf[len]))
@@ -311,18 +370,18 @@ static ssize_t sequence_store(struct device *dev, struct device_attribute *attr,
 			return ret;
 	}
 
-	param = (struct ec_params_lightbar *)msg.outdata;
+	param = (struct ec_params_lightbar *)msg->data;
 	param->cmd = LIGHTBAR_CMD_SEQ;
 	param->seq.num = num;
 	ret = lb_throttle();
 	if (ret)
 		return ret;
 
-	ret = cros_ec_cmd_xfer(ec, &msg);
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 	if (ret < 0)
 		return ret;
 
-	if (msg.result != EC_RES_SUCCESS)
+	if (msg->result != EC_RES_SUCCESS)
 		return -EINVAL;
 
 	return count;
@@ -343,25 +402,27 @@ static struct attribute *__lb_cmds_attrs[] = {
 	&dev_attr_sequence.attr,
 	NULL,
 };
-static struct attribute_group lb_cmds_attr_group = {
-	.name = "lightbar",
-	.attrs = __lb_cmds_attrs,
-};
 
-void ec_dev_lightbar_init(struct cros_ec_device *ec)
+static umode_t cros_ec_lightbar_attrs_are_visible(struct kobject *kobj,
+						  struct attribute *a, int n)
 {
-	int ret = 0;
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
+	struct platform_device *pdev = container_of(ec->dev,
+						   struct platform_device, dev);
+	if (pdev->id != 0)
+		return 0;
 
 	/* Only instantiate this stuff if the EC has a lightbar */
-	if (!get_lightbar_version(ec, NULL, NULL))
-		return;
-
-	ret = sysfs_create_group(&ec->vdev->kobj, &lb_cmds_attr_group);
-	if (ret)
-		pr_warn("sysfs_create_group() failed: %d\n", ret);
+	if (get_lightbar_version(ec, NULL, NULL))
+		return a->mode;
+	else
+		return 0;
 }
 
-void ec_dev_lightbar_remove(struct cros_ec_device *ec)
-{
-	sysfs_remove_group(&ec->vdev->kobj, &lb_cmds_attr_group);
-}
+struct attribute_group cros_ec_lightbar_attr_group = {
+	.name = "lightbar",
+	.attrs = __lb_cmds_attrs,
+	.is_visible = cros_ec_lightbar_attrs_are_visible,
+};
diff --git a/drivers/platform/chrome/cros_ec_lpc.c b/drivers/platform/chrome/cros_ec_lpc.c
index 8f9ac4d7bbd0..bdd77ce45f05 100644
--- a/drivers/platform/chrome/cros_ec_lpc.c
+++ b/drivers/platform/chrome/cros_ec_lpc.c
@@ -46,6 +46,77 @@ static int ec_response_timed_out(void)
 	return 1;
 }
 
+static int cros_ec_pkt_xfer_lpc(struct cros_ec_device *ec,
+				struct cros_ec_command *msg)
+{
+	struct ec_host_request *request;
+	struct ec_host_response response;
+	u8 sum = 0;
+	int i;
+	int ret = 0;
+	u8 *dout;
+
+	ret = cros_ec_prepare_tx(ec, msg);
+
+	/* Write buffer */
+	for (i = 0; i < ret; i++)
+		outb(ec->dout[i], EC_LPC_ADDR_HOST_PACKET + i);
+
+	request = (struct ec_host_request *)ec->dout;
+
+	/* Here we go */
+	outb(EC_COMMAND_PROTOCOL_3, EC_LPC_ADDR_HOST_CMD);
+
+	if (ec_response_timed_out()) {
+		dev_warn(ec->dev, "EC responsed timed out\n");
+		ret = -EIO;
+		goto done;
+	}
+
+	/* Check result */
+	msg->result = inb(EC_LPC_ADDR_HOST_DATA);
+	ret = cros_ec_check_result(ec, msg);
+	if (ret)
+		goto done;
+
+	/* Read back response */
+	dout = (u8 *)&response;
+	for (i = 0; i < sizeof(response); i++) {
+		dout[i] = inb(EC_LPC_ADDR_HOST_PACKET + i);
+		sum += dout[i];
+	}
+
+	msg->result = response.result;
+
+	if (response.data_len > msg->insize) {
+		dev_err(ec->dev,
+			"packet too long (%d bytes, expected %d)",
+			response.data_len, msg->insize);
+		ret = -EMSGSIZE;
+		goto done;
+	}
+
+	/* Read response and process checksum */
+	for (i = 0; i < response.data_len; i++) {
+		msg->data[i] =
+			inb(EC_LPC_ADDR_HOST_PACKET + sizeof(response) + i);
+		sum += msg->data[i];
+	}
+
+	if (sum) {
+		dev_err(ec->dev,
+			"bad packet checksum %02x\n",
+			response.checksum);
+		ret = -EBADMSG;
+		goto done;
+	}
+
+	/* Return actual amount of data received */
+	ret = response.data_len;
+done:
+	return ret;
+}
+
 static int cros_ec_cmd_xfer_lpc(struct cros_ec_device *ec,
 				struct cros_ec_command *msg)
 {
@@ -73,8 +144,8 @@ static int cros_ec_cmd_xfer_lpc(struct cros_ec_device *ec,
 
 	/* Copy data and update checksum */
 	for (i = 0; i < msg->outsize; i++) {
-		outb(msg->outdata[i], EC_LPC_ADDR_HOST_PARAM + i);
-		csum += msg->outdata[i];
+		outb(msg->data[i], EC_LPC_ADDR_HOST_PARAM + i);
+		csum += msg->data[i];
 	}
 
 	/* Finalize checksum and write args */
@@ -129,8 +200,8 @@ static int cros_ec_cmd_xfer_lpc(struct cros_ec_device *ec,
 
 	/* Read response and update checksum */
 	for (i = 0; i < args.data_size; i++) {
-		msg->indata[i] = inb(EC_LPC_ADDR_HOST_PARAM + i);
-		csum += msg->indata[i];
+		msg->data[i] = inb(EC_LPC_ADDR_HOST_PARAM + i);
+		csum += msg->data[i];
 	}
 
 	/* Verify checksum */
@@ -212,11 +283,13 @@ static int cros_ec_lpc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, ec_dev);
 	ec_dev->dev = dev;
-	ec_dev->ec_name = pdev->name;
 	ec_dev->phys_name = dev_name(dev);
-	ec_dev->parent = dev;
 	ec_dev->cmd_xfer = cros_ec_cmd_xfer_lpc;
+	ec_dev->pkt_xfer = cros_ec_pkt_xfer_lpc;
 	ec_dev->cmd_readmem = cros_ec_lpc_readmem;
+	ec_dev->din_size = sizeof(struct ec_host_response) +
+			   sizeof(struct ec_response_get_protocol_info);
+	ec_dev->dout_size = sizeof(struct ec_host_request);
 
 	ret = cros_ec_register(ec_dev);
 	if (ret) {
diff --git a/drivers/platform/chrome/cros_ec_proto.c b/drivers/platform/chrome/cros_ec_proto.c
new file mode 100644
index 000000000000..990308ca384f
--- /dev/null
+++ b/drivers/platform/chrome/cros_ec_proto.c
@@ -0,0 +1,382 @@
+/*
+ * ChromeOS EC communication protocol helper functions
+ *
+ * Copyright (C) 2015 Google, Inc
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/mfd/cros_ec.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#define EC_COMMAND_RETRIES	50
+
+static int prepare_packet(struct cros_ec_device *ec_dev,
+			  struct cros_ec_command *msg)
+{
+	struct ec_host_request *request;
+	u8 *out;
+	int i;
+	u8 csum = 0;
+
+	BUG_ON(ec_dev->proto_version != EC_HOST_REQUEST_VERSION);
+	BUG_ON(msg->outsize + sizeof(*request) > ec_dev->dout_size);
+
+	out = ec_dev->dout;
+	request = (struct ec_host_request *)out;
+	request->struct_version = EC_HOST_REQUEST_VERSION;
+	request->checksum = 0;
+	request->command = msg->command;
+	request->command_version = msg->version;
+	request->reserved = 0;
+	request->data_len = msg->outsize;
+
+	for (i = 0; i < sizeof(*request); i++)
+		csum += out[i];
+
+	/* Copy data and update checksum */
+	memcpy(out + sizeof(*request), msg->data, msg->outsize);
+	for (i = 0; i < msg->outsize; i++)
+		csum += msg->data[i];
+
+	request->checksum = -csum;
+
+	return sizeof(*request) + msg->outsize;
+}
+
+static int send_command(struct cros_ec_device *ec_dev,
+			struct cros_ec_command *msg)
+{
+	int ret;
+
+	if (ec_dev->proto_version > 2)
+		ret = ec_dev->pkt_xfer(ec_dev, msg);
+	else
+		ret = ec_dev->cmd_xfer(ec_dev, msg);
+
+	if (msg->result == EC_RES_IN_PROGRESS) {
+		int i;
+		struct cros_ec_command *status_msg;
+		struct ec_response_get_comms_status *status;
+
+		status_msg = kmalloc(sizeof(*status_msg) + sizeof(*status),
+				     GFP_KERNEL);
+		if (!status_msg)
+			return -ENOMEM;
+
+		status_msg->version = 0;
+		status_msg->command = EC_CMD_GET_COMMS_STATUS;
+		status_msg->insize = sizeof(*status);
+		status_msg->outsize = 0;
+
+		/*
+		 * Query the EC's status until it's no longer busy or
+		 * we encounter an error.
+		 */
+		for (i = 0; i < EC_COMMAND_RETRIES; i++) {
+			usleep_range(10000, 11000);
+
+			ret = ec_dev->cmd_xfer(ec_dev, status_msg);
+			if (ret < 0)
+				break;
+
+			msg->result = status_msg->result;
+			if (status_msg->result != EC_RES_SUCCESS)
+				break;
+
+			status = (struct ec_response_get_comms_status *)
+				 status_msg->data;
+			if (!(status->flags & EC_COMMS_STATUS_PROCESSING))
+				break;
+		}
+
+		kfree(status_msg);
+	}
+
+	return ret;
+}
+
+int cros_ec_prepare_tx(struct cros_ec_device *ec_dev,
+		       struct cros_ec_command *msg)
+{
+	u8 *out;
+	u8 csum;
+	int i;
+
+	if (ec_dev->proto_version > 2)
+		return prepare_packet(ec_dev, msg);
+
+	BUG_ON(msg->outsize > EC_PROTO2_MAX_PARAM_SIZE);
+	out = ec_dev->dout;
+	out[0] = EC_CMD_VERSION0 + msg->version;
+	out[1] = msg->command;
+	out[2] = msg->outsize;
+	csum = out[0] + out[1] + out[2];
+	for (i = 0; i < msg->outsize; i++)
+		csum += out[EC_MSG_TX_HEADER_BYTES + i] = msg->data[i];
+	out[EC_MSG_TX_HEADER_BYTES + msg->outsize] = csum;
+
+	return EC_MSG_TX_PROTO_BYTES + msg->outsize;
+}
+EXPORT_SYMBOL(cros_ec_prepare_tx);
+
+int cros_ec_check_result(struct cros_ec_device *ec_dev,
+			 struct cros_ec_command *msg)
+{
+	switch (msg->result) {
+	case EC_RES_SUCCESS:
+		return 0;
+	case EC_RES_IN_PROGRESS:
+		dev_dbg(ec_dev->dev, "command 0x%02x in progress\n",
+			msg->command);
+		return -EAGAIN;
+	default:
+		dev_dbg(ec_dev->dev, "command 0x%02x returned %d\n",
+			msg->command, msg->result);
+		return 0;
+	}
+}
+EXPORT_SYMBOL(cros_ec_check_result);
+
+static int cros_ec_host_command_proto_query(struct cros_ec_device *ec_dev,
+					    int devidx,
+					    struct cros_ec_command *msg)
+{
+	/*
+	 * Try using v3+ to query for supported protocols. If this
+	 * command fails, fall back to v2. Returns the highest protocol
+	 * supported by the EC.
+	 * Also sets the max request/response/passthru size.
+	 */
+	int ret;
+
+	if (!ec_dev->pkt_xfer)
+		return -EPROTONOSUPPORT;
+
+	memset(msg, 0, sizeof(*msg));
+	msg->command = EC_CMD_PASSTHRU_OFFSET(devidx) | EC_CMD_GET_PROTOCOL_INFO;
+	msg->insize = sizeof(struct ec_response_get_protocol_info);
+
+	ret = send_command(ec_dev, msg);
+
+	if (ret < 0) {
+		dev_dbg(ec_dev->dev,
+			"failed to check for EC[%d] protocol version: %d\n",
+			devidx, ret);
+		return ret;
+	}
+
+	if (devidx > 0 && msg->result == EC_RES_INVALID_COMMAND)
+		return -ENODEV;
+	else if (msg->result != EC_RES_SUCCESS)
+		return msg->result;
+
+	return 0;
+}
+
+static int cros_ec_host_command_proto_query_v2(struct cros_ec_device *ec_dev)
+{
+	struct cros_ec_command *msg;
+	struct ec_params_hello *hello_params;
+	struct ec_response_hello *hello_response;
+	int ret;
+	int len = max(sizeof(*hello_params), sizeof(*hello_response));
+
+	msg = kmalloc(sizeof(*msg) + len, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	msg->version = 0;
+	msg->command = EC_CMD_HELLO;
+	hello_params = (struct ec_params_hello *)msg->data;
+	msg->outsize = sizeof(*hello_params);
+	hello_response = (struct ec_response_hello *)msg->data;
+	msg->insize = sizeof(*hello_response);
+
+	hello_params->in_data = 0xa0b0c0d0;
+
+	ret = send_command(ec_dev, msg);
+
+	if (ret < 0) {
+		dev_dbg(ec_dev->dev,
+			"EC failed to respond to v2 hello: %d\n",
+			ret);
+		goto exit;
+	} else if (msg->result != EC_RES_SUCCESS) {
+		dev_err(ec_dev->dev,
+			"EC responded to v2 hello with error: %d\n",
+			msg->result);
+		ret = msg->result;
+		goto exit;
+	} else if (hello_response->out_data != 0xa1b2c3d4) {
+		dev_err(ec_dev->dev,
+			"EC responded to v2 hello with bad result: %u\n",
+			hello_response->out_data);
+		ret = -EBADMSG;
+		goto exit;
+	}
+
+	ret = 0;
+
+ exit:
+	kfree(msg);
+	return ret;
+}
+
+int cros_ec_query_all(struct cros_ec_device *ec_dev)
+{
+	struct device *dev = ec_dev->dev;
+	struct cros_ec_command *proto_msg;
+	struct ec_response_get_protocol_info *proto_info;
+	int ret;
+
+	proto_msg = kzalloc(sizeof(*proto_msg) + sizeof(*proto_info),
+			    GFP_KERNEL);
+	if (!proto_msg)
+		return -ENOMEM;
+
+	/* First try sending with proto v3. */
+	ec_dev->proto_version = 3;
+	ret = cros_ec_host_command_proto_query(ec_dev, 0, proto_msg);
+
+	if (ret == 0) {
+		proto_info = (struct ec_response_get_protocol_info *)
+			proto_msg->data;
+		ec_dev->max_request = proto_info->max_request_packet_size -
+			sizeof(struct ec_host_request);
+		ec_dev->max_response = proto_info->max_response_packet_size -
+			sizeof(struct ec_host_response);
+		ec_dev->proto_version =
+			min(EC_HOST_REQUEST_VERSION,
+					fls(proto_info->protocol_versions) - 1);
+		dev_dbg(ec_dev->dev,
+			"using proto v%u\n",
+			ec_dev->proto_version);
+
+		ec_dev->din_size = ec_dev->max_response +
+			sizeof(struct ec_host_response) +
+			EC_MAX_RESPONSE_OVERHEAD;
+		ec_dev->dout_size = ec_dev->max_request +
+			sizeof(struct ec_host_request) +
+			EC_MAX_REQUEST_OVERHEAD;
+
+		/*
+		 * Check for PD
+		 */
+		ret = cros_ec_host_command_proto_query(ec_dev, 1, proto_msg);
+
+		if (ret) {
+			dev_dbg(ec_dev->dev, "no PD chip found: %d\n", ret);
+			ec_dev->max_passthru = 0;
+		} else {
+			dev_dbg(ec_dev->dev, "found PD chip\n");
+			ec_dev->max_passthru =
+				proto_info->max_request_packet_size -
+				sizeof(struct ec_host_request);
+		}
+	} else {
+		/* Try querying with a v2 hello message. */
+		ec_dev->proto_version = 2;
+		ret = cros_ec_host_command_proto_query_v2(ec_dev);
+
+		if (ret == 0) {
+			/* V2 hello succeeded. */
+			dev_dbg(ec_dev->dev, "falling back to proto v2\n");
+
+			ec_dev->max_request = EC_PROTO2_MAX_PARAM_SIZE;
+			ec_dev->max_response = EC_PROTO2_MAX_PARAM_SIZE;
+			ec_dev->max_passthru = 0;
+			ec_dev->pkt_xfer = NULL;
+			ec_dev->din_size = EC_MSG_BYTES;
+			ec_dev->dout_size = EC_MSG_BYTES;
+		} else {
+			/*
+			 * It's possible for a test to occur too early when
+			 * the EC isn't listening. If this happens, we'll
+			 * test later when the first command is run.
+			 */
+			ec_dev->proto_version = EC_PROTO_VERSION_UNKNOWN;
+			dev_dbg(ec_dev->dev, "EC query failed: %d\n", ret);
+			goto exit;
+		}
+	}
+
+	devm_kfree(dev, ec_dev->din);
+	devm_kfree(dev, ec_dev->dout);
+
+	ec_dev->din = devm_kzalloc(dev, ec_dev->din_size, GFP_KERNEL);
+	if (!ec_dev->din) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	ec_dev->dout = devm_kzalloc(dev, ec_dev->dout_size, GFP_KERNEL);
+	if (!ec_dev->dout) {
+		devm_kfree(dev, ec_dev->din);
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+exit:
+	kfree(proto_msg);
+	return ret;
+}
+EXPORT_SYMBOL(cros_ec_query_all);
+
+int cros_ec_cmd_xfer(struct cros_ec_device *ec_dev,
+		     struct cros_ec_command *msg)
+{
+	int ret;
+
+	mutex_lock(&ec_dev->lock);
+	if (ec_dev->proto_version == EC_PROTO_VERSION_UNKNOWN) {
+		ret = cros_ec_query_all(ec_dev);
+		if (ret) {
+			dev_err(ec_dev->dev,
+				"EC version unknown and query failed; aborting command\n");
+			mutex_unlock(&ec_dev->lock);
+			return ret;
+		}
+	}
+
+	if (msg->insize > ec_dev->max_response) {
+		dev_dbg(ec_dev->dev, "clamping message receive buffer\n");
+		msg->insize = ec_dev->max_response;
+	}
+
+	if (msg->command < EC_CMD_PASSTHRU_OFFSET(1)) {
+		if (msg->outsize > ec_dev->max_request) {
+			dev_err(ec_dev->dev,
+				"request of size %u is too big (max: %u)\n",
+				msg->outsize,
+				ec_dev->max_request);
+			mutex_unlock(&ec_dev->lock);
+			return -EMSGSIZE;
+		}
+	} else {
+		if (msg->outsize > ec_dev->max_passthru) {
+			dev_err(ec_dev->dev,
+				"passthru rq of size %u is too big (max: %u)\n",
+				msg->outsize,
+				ec_dev->max_passthru);
+			mutex_unlock(&ec_dev->lock);
+			return -EMSGSIZE;
+		}
+	}
+	ret = send_command(ec_dev, msg);
+	mutex_unlock(&ec_dev->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(cros_ec_cmd_xfer);
diff --git a/drivers/platform/chrome/cros_ec_sysfs.c b/drivers/platform/chrome/cros_ec_sysfs.c
index fb62ab6cc659..f3baf9973989 100644
--- a/drivers/platform/chrome/cros_ec_sysfs.c
+++ b/drivers/platform/chrome/cros_ec_sysfs.c
@@ -29,6 +29,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/printk.h>
+#include <linux/slab.h>
 #include <linux/stat.h>
 #include <linux/types.h>
 #include <linux/uaccess.h>
@@ -66,13 +67,19 @@ static ssize_t store_ec_reboot(struct device *dev,
 		{"hibernate",    EC_REBOOT_HIBERNATE, 0},
 		{"at-shutdown",  -1, EC_REBOOT_FLAG_ON_AP_SHUTDOWN},
 	};
-	struct cros_ec_command msg = { 0 };
-	struct ec_params_reboot_ec *param =
-		(struct ec_params_reboot_ec *)msg.outdata;
+	struct cros_ec_command *msg;
+	struct ec_params_reboot_ec *param;
 	int got_cmd = 0, offset = 0;
 	int i;
 	int ret;
-	struct cros_ec_device *ec = dev_get_drvdata(dev);
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
+
+	msg = kmalloc(sizeof(*msg) + sizeof(*param), GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	param = (struct ec_params_reboot_ec *)msg->data;
 
 	param->flags = 0;
 	while (1) {
@@ -100,19 +107,26 @@ static ssize_t store_ec_reboot(struct device *dev,
 			offset++;
 	}
 
-	if (!got_cmd)
-		return -EINVAL;
-
-	msg.command = EC_CMD_REBOOT_EC;
-	msg.outsize = sizeof(param);
-	ret = cros_ec_cmd_xfer(ec, &msg);
-	if (ret < 0)
-		return ret;
-	if (msg.result != EC_RES_SUCCESS) {
-		dev_dbg(ec->dev, "EC result %d\n", msg.result);
-		return -EINVAL;
+	if (!got_cmd) {
+		count = -EINVAL;
+		goto exit;
 	}
 
+	msg->version = 0;
+	msg->command = EC_CMD_REBOOT_EC + ec->cmd_offset;
+	msg->outsize = sizeof(*param);
+	msg->insize = 0;
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
+	if (ret < 0) {
+		count = ret;
+		goto exit;
+	}
+	if (msg->result != EC_RES_SUCCESS) {
+		dev_dbg(ec->dev, "EC result %d\n", msg->result);
+		count = -EINVAL;
+	}
+exit:
+	kfree(msg);
 	return count;
 }
 
@@ -123,22 +137,33 @@ static ssize_t show_ec_version(struct device *dev,
 	struct ec_response_get_version *r_ver;
 	struct ec_response_get_chip_info *r_chip;
 	struct ec_response_board_version *r_board;
-	struct cros_ec_command msg = { 0 };
+	struct cros_ec_command *msg;
 	int ret;
 	int count = 0;
-	struct cros_ec_device *ec = dev_get_drvdata(dev);
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
+
+	msg = kmalloc(sizeof(*msg) + EC_HOST_PARAM_SIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
 
 	/* Get versions. RW may change. */
-	msg.command = EC_CMD_GET_VERSION;
-	msg.insize = sizeof(*r_ver);
-	ret = cros_ec_cmd_xfer(ec, &msg);
-	if (ret < 0)
-		return ret;
-	if (msg.result != EC_RES_SUCCESS)
-		return scnprintf(buf, PAGE_SIZE,
-				 "ERROR: EC returned %d\n", msg.result);
+	msg->version = 0;
+	msg->command = EC_CMD_GET_VERSION + ec->cmd_offset;
+	msg->insize = sizeof(*r_ver);
+	msg->outsize = 0;
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
+	if (ret < 0) {
+		count = ret;
+		goto exit;
+	}
+	if (msg->result != EC_RES_SUCCESS) {
+		count = scnprintf(buf, PAGE_SIZE,
+				  "ERROR: EC returned %d\n", msg->result);
+		goto exit;
+	}
 
-	r_ver = (struct ec_response_get_version *)msg.indata;
+	r_ver = (struct ec_response_get_version *)msg->data;
 	/* Strings should be null-terminated, but let's be sure. */
 	r_ver->version_string_ro[sizeof(r_ver->version_string_ro) - 1] = '\0';
 	r_ver->version_string_rw[sizeof(r_ver->version_string_rw) - 1] = '\0';
@@ -152,33 +177,33 @@ static ssize_t show_ec_version(struct device *dev,
 			    image_names[r_ver->current_image] : "?"));
 
 	/* Get build info. */
-	msg.command = EC_CMD_GET_BUILD_INFO;
-	msg.insize = sizeof(msg.indata);
-	ret = cros_ec_cmd_xfer(ec, &msg);
+	msg->command = EC_CMD_GET_BUILD_INFO + ec->cmd_offset;
+	msg->insize = EC_HOST_PARAM_SIZE;
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 	if (ret < 0)
 		count += scnprintf(buf + count, PAGE_SIZE - count,
 				   "Build info:    XFER ERROR %d\n", ret);
-	else if (msg.result != EC_RES_SUCCESS)
+	else if (msg->result != EC_RES_SUCCESS)
 		count += scnprintf(buf + count, PAGE_SIZE - count,
-				   "Build info:    EC error %d\n", msg.result);
+				   "Build info:    EC error %d\n", msg->result);
 	else {
-		msg.indata[sizeof(msg.indata) - 1] = '\0';
+		msg->data[sizeof(msg->data) - 1] = '\0';
 		count += scnprintf(buf + count, PAGE_SIZE - count,
-				   "Build info:    %s\n", msg.indata);
+				   "Build info:    %s\n", msg->data);
 	}
 
 	/* Get chip info. */
-	msg.command = EC_CMD_GET_CHIP_INFO;
-	msg.insize = sizeof(*r_chip);
-	ret = cros_ec_cmd_xfer(ec, &msg);
+	msg->command = EC_CMD_GET_CHIP_INFO + ec->cmd_offset;
+	msg->insize = sizeof(*r_chip);
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 	if (ret < 0)
 		count += scnprintf(buf + count, PAGE_SIZE - count,
 				   "Chip info:     XFER ERROR %d\n", ret);
-	else if (msg.result != EC_RES_SUCCESS)
+	else if (msg->result != EC_RES_SUCCESS)
 		count += scnprintf(buf + count, PAGE_SIZE - count,
-				   "Chip info:     EC error %d\n", msg.result);
+				   "Chip info:     EC error %d\n", msg->result);
 	else {
-		r_chip = (struct ec_response_get_chip_info *)msg.indata;
+		r_chip = (struct ec_response_get_chip_info *)msg->data;
 
 		r_chip->vendor[sizeof(r_chip->vendor) - 1] = '\0';
 		r_chip->name[sizeof(r_chip->name) - 1] = '\0';
@@ -192,23 +217,25 @@ static ssize_t show_ec_version(struct device *dev,
 	}
 
 	/* Get board version */
-	msg.command = EC_CMD_GET_BOARD_VERSION;
-	msg.insize = sizeof(*r_board);
-	ret = cros_ec_cmd_xfer(ec, &msg);
+	msg->command = EC_CMD_GET_BOARD_VERSION + ec->cmd_offset;
+	msg->insize = sizeof(*r_board);
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 	if (ret < 0)
 		count += scnprintf(buf + count, PAGE_SIZE - count,
 				   "Board version: XFER ERROR %d\n", ret);
-	else if (msg.result != EC_RES_SUCCESS)
+	else if (msg->result != EC_RES_SUCCESS)
 		count += scnprintf(buf + count, PAGE_SIZE - count,
-				   "Board version: EC error %d\n", msg.result);
+				   "Board version: EC error %d\n", msg->result);
 	else {
-		r_board = (struct ec_response_board_version *)msg.indata;
+		r_board = (struct ec_response_board_version *)msg->data;
 
 		count += scnprintf(buf + count, PAGE_SIZE - count,
 				   "Board version: %d\n",
 				   r_board->board_version);
 	}
 
+exit:
+	kfree(msg);
 	return count;
 }
 
@@ -216,27 +243,39 @@ static ssize_t show_ec_flashinfo(struct device *dev,
 				 struct device_attribute *attr, char *buf)
 {
 	struct ec_response_flash_info *resp;
-	struct cros_ec_command msg = { 0 };
+	struct cros_ec_command *msg;
 	int ret;
-	struct cros_ec_device *ec = dev_get_drvdata(dev);
+	struct cros_ec_dev *ec = container_of(dev,
+					      struct cros_ec_dev, class_dev);
+
+	msg = kmalloc(sizeof(*msg) + sizeof(*resp), GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
 
 	/* The flash info shouldn't ever change, but ask each time anyway. */
-	msg.command = EC_CMD_FLASH_INFO;
-	msg.insize = sizeof(*resp);
-	ret = cros_ec_cmd_xfer(ec, &msg);
+	msg->version = 0;
+	msg->command = EC_CMD_FLASH_INFO + ec->cmd_offset;
+	msg->insize = sizeof(*resp);
+	msg->outsize = 0;
+	ret = cros_ec_cmd_xfer(ec->ec_dev, msg);
 	if (ret < 0)
-		return ret;
-	if (msg.result != EC_RES_SUCCESS)
-		return scnprintf(buf, PAGE_SIZE,
-				 "ERROR: EC returned %d\n", msg.result);
-
-	resp = (struct ec_response_flash_info *)msg.indata;
-
-	return scnprintf(buf, PAGE_SIZE,
-			 "FlashSize %d\nWriteSize %d\n"
-			 "EraseSize %d\nProtectSize %d\n",
-			 resp->flash_size, resp->write_block_size,
-			 resp->erase_block_size, resp->protect_block_size);
+		goto exit;
+	if (msg->result != EC_RES_SUCCESS) {
+		ret = scnprintf(buf, PAGE_SIZE,
+				"ERROR: EC returned %d\n", msg->result);
+		goto exit;
+	}
+
+	resp = (struct ec_response_flash_info *)msg->data;
+
+	ret = scnprintf(buf, PAGE_SIZE,
+			"FlashSize %d\nWriteSize %d\n"
+			"EraseSize %d\nProtectSize %d\n",
+			resp->flash_size, resp->write_block_size,
+			resp->erase_block_size, resp->protect_block_size);
+exit:
+	kfree(msg);
+	return ret;
 }
 
 /* Module initialization */
@@ -252,20 +291,7 @@ static struct attribute *__ec_attrs[] = {
 	NULL,
 };
 
-static struct attribute_group ec_attr_group = {
+struct attribute_group cros_ec_attr_group = {
 	.attrs = __ec_attrs,
 };
 
-void ec_dev_sysfs_init(struct cros_ec_device *ec)
-{
-	int error;
-
-	error = sysfs_create_group(&ec->vdev->kobj, &ec_attr_group);
-	if (error)
-		pr_warn("failed to create group: %d\n", error);
-}
-
-void ec_dev_sysfs_remove(struct cros_ec_device *ec)
-{
-	sysfs_remove_group(&ec->vdev->kobj, &ec_attr_group);
-}
diff --git a/drivers/platform/goldfish/goldfish_pipe.c b/drivers/platform/goldfish/goldfish_pipe.c
index d9a09d9637d9..e7a29e2750c6 100644
--- a/drivers/platform/goldfish/goldfish_pipe.c
+++ b/drivers/platform/goldfish/goldfish_pipe.c
@@ -158,8 +158,8 @@ static u32 goldfish_cmd_status(struct goldfish_pipe *pipe, u32 cmd)
 	struct goldfish_pipe_dev *dev = pipe->dev;
 
 	spin_lock_irqsave(&dev->lock, flags);
-	gf_write64((u64)(unsigned long)pipe, dev->base + PIPE_REG_CHANNEL,
-				dev->base + PIPE_REG_CHANNEL_HIGH);
+	gf_write_ptr(pipe, dev->base + PIPE_REG_CHANNEL,
+		     dev->base + PIPE_REG_CHANNEL_HIGH);
 	writel(cmd, dev->base + PIPE_REG_COMMAND);
 	status = readl(dev->base + PIPE_REG_STATUS);
 	spin_unlock_irqrestore(&dev->lock, flags);
@@ -172,8 +172,8 @@ static void goldfish_cmd(struct goldfish_pipe *pipe, u32 cmd)
 	struct goldfish_pipe_dev *dev = pipe->dev;
 
 	spin_lock_irqsave(&dev->lock, flags);
-	gf_write64((u64)(unsigned long)pipe, dev->base + PIPE_REG_CHANNEL,
-				dev->base + PIPE_REG_CHANNEL_HIGH);
+	gf_write_ptr(pipe, dev->base + PIPE_REG_CHANNEL,
+		     dev->base + PIPE_REG_CHANNEL_HIGH);
 	writel(cmd, dev->base + PIPE_REG_COMMAND);
 	spin_unlock_irqrestore(&dev->lock, flags);
 }
@@ -282,7 +282,7 @@ static ssize_t goldfish_pipe_read_write(struct file *filp, char __user *buffer,
 		return -EIO;
 
 	/* Null reads or writes succeeds */
-	if (unlikely(bufflen) == 0)
+	if (unlikely(bufflen == 0))
 		return 0;
 
 	/* Check the buffer range for access */
@@ -327,12 +327,12 @@ static ssize_t goldfish_pipe_read_write(struct file *filp, char __user *buffer,
 		spin_lock_irqsave(&dev->lock, irq_flags);
 		if (access_with_param(dev, CMD_WRITE_BUFFER + cmd_offset,
 				address, avail, pipe, &status)) {
-			gf_write64((u64)(unsigned long)pipe,
-				   dev->base + PIPE_REG_CHANNEL,
-				   dev->base + PIPE_REG_CHANNEL_HIGH);
+			gf_write_ptr(pipe, dev->base + PIPE_REG_CHANNEL,
+				     dev->base + PIPE_REG_CHANNEL_HIGH);
 			writel(avail, dev->base + PIPE_REG_SIZE);
-			gf_write64(address, dev->base + PIPE_REG_ADDRESS,
-				dev->base + PIPE_REG_ADDRESS_HIGH);
+			gf_write_ptr((void *)address,
+				     dev->base + PIPE_REG_ADDRESS,
+				     dev->base + PIPE_REG_ADDRESS_HIGH);
 			writel(CMD_WRITE_BUFFER + cmd_offset,
 					dev->base + PIPE_REG_COMMAND);
 			status = readl(dev->base + PIPE_REG_STATUS);
diff --git a/drivers/platform/goldfish/pdev_bus.c b/drivers/platform/goldfish/pdev_bus.c
index 8c43589c3edb..1f52462f4cdd 100644
--- a/drivers/platform/goldfish/pdev_bus.c
+++ b/drivers/platform/goldfish/pdev_bus.c
@@ -220,20 +220,10 @@ free_resources:
 	return ret;
 }
 
-static int goldfish_pdev_bus_remove(struct platform_device *pdev)
-{
-	iounmap(pdev_bus_base);
-	free_irq(pdev_bus_irq, pdev);
-	release_mem_region(pdev_bus_addr, pdev_bus_len);
-	return 0;
-}
-
 static struct platform_driver goldfish_pdev_bus_driver = {
 	.probe = goldfish_pdev_bus_probe,
-	.remove = goldfish_pdev_bus_remove,
 	.driver = {
 		.name = "goldfish_pdev_bus"
 	}
 };
-
-module_platform_driver(goldfish_pdev_bus_driver);
+builtin_platform_driver(goldfish_pdev_bus_driver);
diff --git a/drivers/platform/x86/Kconfig b/drivers/platform/x86/Kconfig
index f9f205cb1f11..6dc13e4de396 100644
--- a/drivers/platform/x86/Kconfig
+++ b/drivers/platform/x86/Kconfig
@@ -71,9 +71,10 @@ config ASUS_LAPTOP
 	depends on ACPI
 	select LEDS_CLASS
 	select NEW_LEDS
-	select BACKLIGHT_CLASS_DEVICE
+	depends on BACKLIGHT_CLASS_DEVICE
 	depends on INPUT
 	depends on RFKILL || RFKILL = n
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	select INPUT_SPARSEKMAP
 	select INPUT_POLLDEV
 	---help---
@@ -95,6 +96,7 @@ config DELL_LAPTOP
 	depends on X86
 	depends on DCDBAS
 	depends on BACKLIGHT_CLASS_DEVICE
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	depends on RFKILL || RFKILL = n
 	depends on SERIO_I8042
 	select POWER_SUPPLY
@@ -109,6 +111,7 @@ config DELL_WMI
 	tristate "Dell WMI extras"
 	depends on ACPI_WMI
 	depends on INPUT
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	select INPUT_SPARSEKMAP
 	---help---
 	  Say Y here if you want to support WMI-based hotkeys on Dell laptops.
@@ -138,12 +141,29 @@ config DELL_SMO8800
 	  To compile this driver as a module, choose M here: the module will
 	  be called dell-smo8800.
 
+config DELL_RBTN
+	tristate "Dell Airplane Mode Switch driver"
+	depends on ACPI
+	depends on INPUT
+	depends on RFKILL
+	---help---
+	  Say Y here if you want to support Dell Airplane Mode Switch ACPI
+	  device on Dell laptops. Sometimes it has names: DELLABCE or DELRBTN.
+	  This driver register rfkill device or input hotkey device depending
+	  on hardware type (hw switch slider or keyboard toggle button). For
+	  rfkill devices it receive HW switch events and set correct hard
+	  rfkill state.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called dell-rbtn.
+
 
 config FUJITSU_LAPTOP
 	tristate "Fujitsu Laptop Extras"
 	depends on ACPI
 	depends on INPUT
 	depends on BACKLIGHT_CLASS_DEVICE
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	depends on LEDS_CLASS || LEDS_CLASS=n
 	---help---
 	  This is a driver for laptops built by Fujitsu:
@@ -247,6 +267,7 @@ config MSI_LAPTOP
 	tristate "MSI Laptop Extras"
 	depends on ACPI
 	depends on BACKLIGHT_CLASS_DEVICE
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	depends on RFKILL
 	depends on INPUT && SERIO_I8042
 	select INPUT_SPARSEKMAP
@@ -280,6 +301,7 @@ config COMPAL_LAPTOP
 	tristate "Compal (and others) Laptop Extras"
 	depends on ACPI
 	depends on BACKLIGHT_CLASS_DEVICE
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	depends on RFKILL
 	depends on HWMON
 	depends on POWER_SUPPLY
@@ -296,7 +318,8 @@ config COMPAL_LAPTOP
 config SONY_LAPTOP
 	tristate "Sony Laptop Extras"
 	depends on ACPI
-	select BACKLIGHT_CLASS_DEVICE
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
+	depends on BACKLIGHT_CLASS_DEVICE
 	depends on INPUT
 	depends on RFKILL
 	  ---help---
@@ -321,6 +344,7 @@ config IDEAPAD_LAPTOP
 	depends on RFKILL && INPUT
 	depends on SERIO_I8042
 	depends on BACKLIGHT_CLASS_DEVICE
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	select INPUT_SPARSEKMAP
 	help
 	  This is a driver for Lenovo IdeaPad netbooks contains drivers for
@@ -331,8 +355,8 @@ config THINKPAD_ACPI
 	depends on ACPI
 	depends on INPUT
 	depends on RFKILL || RFKILL = n
-	select BACKLIGHT_LCD_SUPPORT
-	select BACKLIGHT_CLASS_DEVICE
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
+	depends on BACKLIGHT_CLASS_DEVICE
 	select HWMON
 	select NVRAM
 	select NEW_LEDS
@@ -500,8 +524,9 @@ config EEEPC_LAPTOP
 	depends on ACPI
 	depends on INPUT
 	depends on RFKILL || RFKILL = n
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	depends on HOTPLUG_PCI
-	select BACKLIGHT_CLASS_DEVICE
+	depends on BACKLIGHT_CLASS_DEVICE
 	select HWMON
 	select LEDS_CLASS
 	select NEW_LEDS
@@ -587,6 +612,7 @@ config MSI_WMI
 	depends on ACPI_WMI
 	depends on INPUT
 	depends on BACKLIGHT_CLASS_DEVICE
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	select INPUT_SPARSEKMAP
 	help
 	 Say Y here if you want to support WMI-based hotkeys on MSI laptops.
@@ -612,7 +638,6 @@ config ACPI_TOSHIBA
 	select NEW_LEDS
 	depends on BACKLIGHT_CLASS_DEVICE
 	depends on INPUT
-	depends on RFKILL || RFKILL = n
 	depends on SERIO_I8042 || SERIO_I8042 = n
 	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	select INPUT_POLLDEV
@@ -643,6 +668,7 @@ config ACPI_TOSHIBA
 config TOSHIBA_BT_RFKILL
 	tristate "Toshiba Bluetooth RFKill switch support"
 	depends on ACPI
+	depends on RFKILL || RFKILL = n
 	---help---
 	  This driver adds support for Bluetooth events for the RFKill
 	  switch on modern Toshiba laptops with full ACPI support and
@@ -660,7 +686,7 @@ config TOSHIBA_HAPS
 	depends on ACPI
 	---help---
 	  This driver adds support for the built-in accelerometer
-	  found on recent Toshiba laptops equiped with HID TOS620A
+	  found on recent Toshiba laptops equipped with HID TOS620A
 	  device.
 
 	  This driver receives ACPI notify events 0x80 when the sensor
@@ -669,7 +695,7 @@ config TOSHIBA_HAPS
 	  been stabilized.
 
 	  Also provides sysfs entries to get/set the desired protection
-	  level and reseting the HDD protection interface.
+	  level and resetting the HDD protection interface.
 
 	  If you have a recent Toshiba laptop with a built-in accelerometer
 	  device, say Y.
@@ -824,6 +850,7 @@ config MXM_WMI
 config INTEL_OAKTRAIL
 	tristate "Intel Oaktrail Platform Extras"
 	depends on ACPI
+	depends on ACPI_VIDEO || ACPI_VIDEO = n
 	depends on RFKILL && BACKLIGHT_CLASS_DEVICE && ACPI
 	---help---
 	  Intel Oaktrail platform need this driver to provide interfaces to
@@ -885,4 +912,11 @@ config PVPANIC
 	  a paravirtualized device provided by QEMU; it lets a virtual machine
 	  (guest) communicate panic events to the host.
 
+config INTEL_PMC_IPC
+	tristate "Intel PMC IPC Driver"
+	---help---
+	This driver provides support for PMC control on some Intel platforms.
+	The PMC is an ARC processor which defines IPC commands for communication
+	with other entities in the CPU.
+
 endif # X86_PLATFORM_DEVICES
diff --git a/drivers/platform/x86/Makefile b/drivers/platform/x86/Makefile
index f82232b1fc4d..dda95a985321 100644
--- a/drivers/platform/x86/Makefile
+++ b/drivers/platform/x86/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_DELL_LAPTOP)	+= dell-laptop.o
 obj-$(CONFIG_DELL_WMI)		+= dell-wmi.o
 obj-$(CONFIG_DELL_WMI_AIO)	+= dell-wmi-aio.o
 obj-$(CONFIG_DELL_SMO8800)	+= dell-smo8800.o
+obj-$(CONFIG_DELL_RBTN)		+= dell-rbtn.o
 obj-$(CONFIG_ACER_WMI)		+= acer-wmi.o
 obj-$(CONFIG_ACERHDF)		+= acerhdf.o
 obj-$(CONFIG_HP_ACCEL)		+= hp_accel.o
@@ -58,3 +59,4 @@ obj-$(CONFIG_INTEL_SMARTCONNECT)	+= intel-smartconnect.o
 
 obj-$(CONFIG_PVPANIC)           += pvpanic.o
 obj-$(CONFIG_ALIENWARE_WMI)	+= alienware-wmi.o
+obj-$(CONFIG_INTEL_PMC_IPC)	+= intel_pmc_ipc.o
diff --git a/drivers/platform/x86/acer-wmi.c b/drivers/platform/x86/acer-wmi.c
index 3ac29a1e8f92..f6b280dbfb33 100644
--- a/drivers/platform/x86/acer-wmi.c
+++ b/drivers/platform/x86/acer-wmi.c
@@ -2246,14 +2246,10 @@ static int __init acer_wmi_init(void)
 	set_quirks();
 
 	if (dmi_check_system(video_vendor_dmi_table))
-		acpi_video_dmi_promote_vendor();
-	if (acpi_video_backlight_support()) {
+		acpi_video_set_dmi_backlight_type(acpi_backlight_vendor);
+
+	if (acpi_video_get_backlight_type() != acpi_backlight_vendor)
 		interface->capability &= ~ACER_CAP_BRIGHTNESS;
-		pr_info("Brightness must be controlled by acpi video driver\n");
-	} else {
-		pr_info("Disabling ACPI video driver\n");
-		acpi_video_unregister_backlight();
-	}
 
 	if (wmi_has_guid(WMID_GUID3)) {
 		if (ec_raw_mode) {
diff --git a/drivers/platform/x86/acerhdf.c b/drivers/platform/x86/acerhdf.c
index 594c918b553d..1ef02daddb60 100644
--- a/drivers/platform/x86/acerhdf.c
+++ b/drivers/platform/x86/acerhdf.c
@@ -372,7 +372,8 @@ static int acerhdf_bind(struct thermal_zone_device *thermal,
 		return 0;
 
 	if (thermal_zone_bind_cooling_device(thermal, 0, cdev,
-			THERMAL_NO_LIMIT, THERMAL_NO_LIMIT)) {
+			THERMAL_NO_LIMIT, THERMAL_NO_LIMIT,
+			THERMAL_WEIGHT_DEFAULT)) {
 		pr_err("error binding cooling dev\n");
 		return -EINVAL;
 	}
diff --git a/drivers/platform/x86/apple-gmux.c b/drivers/platform/x86/apple-gmux.c
index 6808715003f6..0dec3f59917a 100644
--- a/drivers/platform/x86/apple-gmux.c
+++ b/drivers/platform/x86/apple-gmux.c
@@ -550,8 +550,7 @@ static int gmux_probe(struct pnp_dev *pnp, const struct pnp_device_id *id)
 	 * backlight control and supports more levels than other options.
 	 * Disable the other backlight choices.
 	 */
-	acpi_video_dmi_promote_vendor();
-	acpi_video_unregister();
+	acpi_video_set_dmi_backlight_type(acpi_backlight_vendor);
 	apple_bl_unregister();
 
 	gmux_data->power_state = VGA_SWITCHEROO_ON;
@@ -645,7 +644,6 @@ static void gmux_remove(struct pnp_dev *pnp)
 	apple_gmux_data = NULL;
 	kfree(gmux_data);
 
-	acpi_video_dmi_demote_vendor();
 	acpi_video_register();
 	apple_bl_register();
 }
diff --git a/drivers/platform/x86/asus-laptop.c b/drivers/platform/x86/asus-laptop.c
index 46b274693872..58d29c4f2840 100644
--- a/drivers/platform/x86/asus-laptop.c
+++ b/drivers/platform/x86/asus-laptop.c
@@ -54,6 +54,7 @@
 #include <linux/slab.h>
 #include <linux/dmi.h>
 #include <linux/acpi.h>
+#include <acpi/video.h>
 
 #define ASUS_LAPTOP_VERSION	"0.42"
 
@@ -1884,12 +1885,11 @@ static int asus_acpi_add(struct acpi_device *device)
 	if (result)
 		goto fail_platform;
 
-	if (!acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		result = asus_backlight_init(asus);
 		if (result)
 			goto fail_backlight;
-	} else
-		pr_info("Backlight controlled by ACPI video driver\n");
+	}
 
 	result = asus_input_init(asus);
 	if (result)
diff --git a/drivers/platform/x86/asus-wmi.c b/drivers/platform/x86/asus-wmi.c
index 7543a56e0f45..efbc3f0c592b 100644
--- a/drivers/platform/x86/asus-wmi.c
+++ b/drivers/platform/x86/asus-wmi.c
@@ -78,6 +78,7 @@ MODULE_LICENSE("GPL");
 #define ASUS_WMI_METHODID_GPID		0x44495047 /* Get Panel ID?? (Resol) */
 #define ASUS_WMI_METHODID_QMOD		0x444F4D51 /* Quiet MODe */
 #define ASUS_WMI_METHODID_SPLV		0x4C425053 /* Set Panel Light Value */
+#define ASUS_WMI_METHODID_AGFN		0x4E464741 /* FaN? */
 #define ASUS_WMI_METHODID_SFUN		0x4E554653 /* FUNCtionalities */
 #define ASUS_WMI_METHODID_SDSP		0x50534453 /* Set DiSPlay output */
 #define ASUS_WMI_METHODID_GDSP		0x50534447 /* Get DiSPlay output */
@@ -150,12 +151,38 @@ MODULE_LICENSE("GPL");
 #define ASUS_WMI_DSTS_BRIGHTNESS_MASK	0x000000FF
 #define ASUS_WMI_DSTS_MAX_BRIGTH_MASK	0x0000FF00
 
+#define ASUS_FAN_DESC			"cpu_fan"
+#define ASUS_FAN_MFUN			0x13
+#define ASUS_FAN_SFUN_READ		0x06
+#define ASUS_FAN_SFUN_WRITE		0x07
+#define ASUS_FAN_CTRL_MANUAL		1
+#define ASUS_FAN_CTRL_AUTO		2
+
 struct bios_args {
 	u32 arg0;
 	u32 arg1;
 } __packed;
 
 /*
+ * Struct that's used for all methods called via AGFN. Naming is
+ * identically to the AML code.
+ */
+struct agfn_args {
+	u16 mfun; /* probably "Multi-function" to be called */
+	u16 sfun; /* probably "Sub-function" to be called */
+	u16 len;  /* size of the hole struct, including subfunction fields */
+	u8 stas;  /* not used by now */
+	u8 err;   /* zero on success */
+} __packed;
+
+/* struct used for calling fan read and write methods */
+struct fan_args {
+	struct agfn_args agfn;	/* common fields */
+	u8 fan;			/* fan number: 0: set auto mode 1: 1st fan */
+	u32 speed;		/* read: RPM/100 - write: 0-255 */
+} __packed;
+
+/*
  * <platform>/    - debugfs root directory
  *   dev_id      - current dev_id
  *   ctrl_param  - current ctrl_param
@@ -204,6 +231,10 @@ struct asus_wmi {
 	struct asus_rfkill gps;
 	struct asus_rfkill uwb;
 
+	bool asus_hwmon_fan_manual_mode;
+	int asus_hwmon_num_fans;
+	int asus_hwmon_pwm;
+
 	struct hotplug_slot *hotplug_slot;
 	struct mutex hotplug_lock;
 	struct mutex wmi_lock;
@@ -294,6 +325,36 @@ exit:
 	return 0;
 }
 
+static int asus_wmi_evaluate_method_agfn(const struct acpi_buffer args)
+{
+	struct acpi_buffer input;
+	u64 phys_addr;
+	u32 retval;
+	u32 status = -1;
+
+	/*
+	 * Copy to dma capable address otherwise memory corruption occurs as
+	 * bios has to be able to access it.
+	 */
+	input.pointer = kzalloc(args.length, GFP_DMA | GFP_KERNEL);
+	input.length = args.length;
+	if (!input.pointer)
+		return -ENOMEM;
+	phys_addr = virt_to_phys(input.pointer);
+	memcpy(input.pointer, args.pointer, args.length);
+
+	status = asus_wmi_evaluate_method(ASUS_WMI_METHODID_AGFN,
+					phys_addr, 0, &retval);
+	if (!status)
+		memcpy(args.pointer, input.pointer, args.length);
+
+	kfree(input.pointer);
+	if (status)
+		return -ENXIO;
+
+	return retval;
+}
+
 static int asus_wmi_get_devstate(struct asus_wmi *asus, u32 dev_id, u32 *retval)
 {
 	return asus_wmi_evaluate_method(asus->dsts_id, dev_id, 0, retval);
@@ -1022,35 +1083,228 @@ exit:
 /*
  * Hwmon device
  */
-static ssize_t asus_hwmon_pwm1(struct device *dev,
-			       struct device_attribute *attr,
-			       char *buf)
+static int asus_hwmon_agfn_fan_speed_read(struct asus_wmi *asus, int fan,
+					  int *speed)
+{
+	struct fan_args args = {
+		.agfn.len = sizeof(args),
+		.agfn.mfun = ASUS_FAN_MFUN,
+		.agfn.sfun = ASUS_FAN_SFUN_READ,
+		.fan = fan,
+		.speed = 0,
+	};
+	struct acpi_buffer input = { (acpi_size) sizeof(args), &args };
+	int status;
+
+	if (fan != 1)
+		return -EINVAL;
+
+	status = asus_wmi_evaluate_method_agfn(input);
+
+	if (status || args.agfn.err)
+		return -ENXIO;
+
+	if (speed)
+		*speed = args.speed;
+
+	return 0;
+}
+
+static int asus_hwmon_agfn_fan_speed_write(struct asus_wmi *asus, int fan,
+				     int *speed)
+{
+	struct fan_args args = {
+		.agfn.len = sizeof(args),
+		.agfn.mfun = ASUS_FAN_MFUN,
+		.agfn.sfun = ASUS_FAN_SFUN_WRITE,
+		.fan = fan,
+		.speed = speed ?  *speed : 0,
+	};
+	struct acpi_buffer input = { (acpi_size) sizeof(args), &args };
+	int status;
+
+	/* 1: for setting 1st fan's speed 0: setting auto mode */
+	if (fan != 1 && fan != 0)
+		return -EINVAL;
+
+	status = asus_wmi_evaluate_method_agfn(input);
+
+	if (status || args.agfn.err)
+		return -ENXIO;
+
+	if (speed && fan == 1)
+		asus->asus_hwmon_pwm = *speed;
+
+	return 0;
+}
+
+/*
+ * Check if we can read the speed of one fan. If true we assume we can also
+ * control it.
+ */
+static int asus_hwmon_get_fan_number(struct asus_wmi *asus, int *num_fans)
+{
+	int status;
+	int speed = 0;
+
+	*num_fans = 0;
+
+	status = asus_hwmon_agfn_fan_speed_read(asus, 1, &speed);
+	if (!status)
+		*num_fans = 1;
+
+	return 0;
+}
+
+static int asus_hwmon_fan_set_auto(struct asus_wmi *asus)
+{
+	int status;
+
+	status = asus_hwmon_agfn_fan_speed_write(asus, 0, NULL);
+	if (status)
+		return -ENXIO;
+
+	asus->asus_hwmon_fan_manual_mode = false;
+
+	return 0;
+}
+
+static int asus_hwmon_fan_rpm_show(struct device *dev, int fan)
 {
 	struct asus_wmi *asus = dev_get_drvdata(dev);
-	u32 value;
+	int value;
+	int ret;
+
+	/* no speed readable on manual mode */
+	if (asus->asus_hwmon_fan_manual_mode)
+		return -ENXIO;
+
+	ret = asus_hwmon_agfn_fan_speed_read(asus, fan+1, &value);
+	if (ret) {
+		pr_warn("reading fan speed failed: %d\n", ret);
+		return -ENXIO;
+	}
+
+	return value;
+}
+
+static void asus_hwmon_pwm_show(struct asus_wmi *asus, int fan, int *value)
+{
 	int err;
 
-	err = asus_wmi_get_devstate(asus, ASUS_WMI_DEVID_FAN_CTRL, &value);
+	if (asus->asus_hwmon_pwm >= 0) {
+		*value = asus->asus_hwmon_pwm;
+		return;
+	}
 
+	err = asus_wmi_get_devstate(asus, ASUS_WMI_DEVID_FAN_CTRL, value);
 	if (err < 0)
-		return err;
+		return;
 
-	value &= 0xFF;
-
-	if (value == 1) /* Low Speed */
-		value = 85;
-	else if (value == 2)
-		value = 170;
-	else if (value == 3)
-		value = 255;
-	else if (value != 0) {
-		pr_err("Unknown fan speed %#x\n", value);
-		value = -1;
+	*value &= 0xFF;
+
+	if (*value == 1) /* Low Speed */
+		*value = 85;
+	else if (*value == 2)
+		*value = 170;
+	else if (*value == 3)
+		*value = 255;
+	else if (*value) {
+		pr_err("Unknown fan speed %#x\n", *value);
+		*value = -1;
 	}
+}
+
+static ssize_t pwm1_show(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	struct asus_wmi *asus = dev_get_drvdata(dev);
+	int value;
+
+	asus_hwmon_pwm_show(asus, 0, &value);
 
 	return sprintf(buf, "%d\n", value);
 }
 
+static ssize_t pwm1_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count) {
+	struct asus_wmi *asus = dev_get_drvdata(dev);
+	int value;
+	int state;
+	int ret;
+
+	ret = kstrtouint(buf, 10, &value);
+
+	if (ret)
+		return ret;
+
+	value = clamp(value, 0, 255);
+
+	state = asus_hwmon_agfn_fan_speed_write(asus, 1, &value);
+	if (state)
+		pr_warn("Setting fan speed failed: %d\n", state);
+	else
+		asus->asus_hwmon_fan_manual_mode = true;
+
+	return count;
+}
+
+static ssize_t fan1_input_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	int value = asus_hwmon_fan_rpm_show(dev, 0);
+
+	return sprintf(buf, "%d\n", value < 0 ? -1 : value*100);
+
+}
+
+static ssize_t pwm1_enable_show(struct device *dev,
+						 struct device_attribute *attr,
+						 char *buf)
+{
+	struct asus_wmi *asus = dev_get_drvdata(dev);
+
+	if (asus->asus_hwmon_fan_manual_mode)
+		return sprintf(buf, "%d\n", ASUS_FAN_CTRL_MANUAL);
+
+	return sprintf(buf, "%d\n", ASUS_FAN_CTRL_AUTO);
+}
+
+static ssize_t pwm1_enable_store(struct device *dev,
+						  struct device_attribute *attr,
+						  const char *buf, size_t count)
+{
+	struct asus_wmi *asus = dev_get_drvdata(dev);
+	int status = 0;
+	int state;
+	int ret;
+
+	ret = kstrtouint(buf, 10, &state);
+
+	if (ret)
+		return ret;
+
+	if (state == ASUS_FAN_CTRL_MANUAL)
+		asus->asus_hwmon_fan_manual_mode = true;
+	else
+		status = asus_hwmon_fan_set_auto(asus);
+
+	if (status)
+		return status;
+
+	return count;
+}
+
+static ssize_t fan1_label_show(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	return sprintf(buf, "%s\n", ASUS_FAN_DESC);
+}
+
 static ssize_t asus_hwmon_temp1(struct device *dev,
 				struct device_attribute *attr,
 				char *buf)
@@ -1069,11 +1323,21 @@ static ssize_t asus_hwmon_temp1(struct device *dev,
 	return sprintf(buf, "%d\n", value);
 }
 
-static DEVICE_ATTR(pwm1, S_IRUGO, asus_hwmon_pwm1, NULL);
+/* Fan1 */
+static DEVICE_ATTR_RW(pwm1);
+static DEVICE_ATTR_RW(pwm1_enable);
+static DEVICE_ATTR_RO(fan1_input);
+static DEVICE_ATTR_RO(fan1_label);
+
+/* Temperature */
 static DEVICE_ATTR(temp1_input, S_IRUGO, asus_hwmon_temp1, NULL);
 
 static struct attribute *hwmon_attributes[] = {
 	&dev_attr_pwm1.attr,
+	&dev_attr_pwm1_enable.attr,
+	&dev_attr_fan1_input.attr,
+	&dev_attr_fan1_label.attr,
+
 	&dev_attr_temp1_input.attr,
 	NULL
 };
@@ -1084,19 +1348,28 @@ static umode_t asus_hwmon_sysfs_is_visible(struct kobject *kobj,
 	struct device *dev = container_of(kobj, struct device, kobj);
 	struct platform_device *pdev = to_platform_device(dev->parent);
 	struct asus_wmi *asus = platform_get_drvdata(pdev);
-	bool ok = true;
 	int dev_id = -1;
+	int fan_attr = -1;
 	u32 value = ASUS_WMI_UNSUPPORTED_METHOD;
+	bool ok = true;
 
 	if (attr == &dev_attr_pwm1.attr)
 		dev_id = ASUS_WMI_DEVID_FAN_CTRL;
 	else if (attr == &dev_attr_temp1_input.attr)
 		dev_id = ASUS_WMI_DEVID_THERMAL_CTRL;
 
+
+	if (attr == &dev_attr_fan1_input.attr
+	    || attr == &dev_attr_fan1_label.attr
+	    || attr == &dev_attr_pwm1.attr
+	    || attr == &dev_attr_pwm1_enable.attr) {
+		fan_attr = 1;
+	}
+
 	if (dev_id != -1) {
 		int err = asus_wmi_get_devstate(asus, dev_id, &value);
 
-		if (err < 0)
+		if (err < 0 && fan_attr == -1)
 			return 0; /* can't return negative here */
 	}
 
@@ -1112,10 +1385,16 @@ static umode_t asus_hwmon_sysfs_is_visible(struct kobject *kobj,
 		if (value == ASUS_WMI_UNSUPPORTED_METHOD || value & 0xFFF80000
 		    || (!asus->sfun && !(value & ASUS_WMI_DSTS_PRESENCE_BIT)))
 			ok = false;
+		else
+			ok = fan_attr <= asus->asus_hwmon_num_fans;
 	} else if (dev_id == ASUS_WMI_DEVID_THERMAL_CTRL) {
 		/* If value is zero, something is clearly wrong */
-		if (value == 0)
+		if (!value)
 			ok = false;
+	} else if (fan_attr <= asus->asus_hwmon_num_fans && fan_attr != -1) {
+		ok = true;
+	} else {
+		ok = false;
 	}
 
 	return ok ? attr->mode : 0;
@@ -1364,7 +1643,7 @@ static void asus_wmi_notify(u32 value, void *context)
 		code = ASUS_WMI_BRN_DOWN;
 
 	if (code == ASUS_WMI_BRN_DOWN || code == ASUS_WMI_BRN_UP) {
-		if (!acpi_video_backlight_support()) {
+		if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 			asus_wmi_backlight_notify(asus, orig_code);
 			goto exit;
 		}
@@ -1723,6 +2002,25 @@ error_debugfs:
 	return -ENOMEM;
 }
 
+static int asus_wmi_fan_init(struct asus_wmi *asus)
+{
+	int status;
+
+	asus->asus_hwmon_pwm = -1;
+	asus->asus_hwmon_num_fans = -1;
+	asus->asus_hwmon_fan_manual_mode = false;
+
+	status = asus_hwmon_get_fan_number(asus, &asus->asus_hwmon_num_fans);
+	if (status) {
+		asus->asus_hwmon_num_fans = 0;
+		pr_warn("Could not determine number of fans: %d\n", status);
+		return -ENXIO;
+	}
+
+	pr_info("Number of fans: %d\n", asus->asus_hwmon_num_fans);
+	return 0;
+}
+
 /*
  * WMI Driver
  */
@@ -1756,6 +2054,9 @@ static int asus_wmi_add(struct platform_device *pdev)
 	if (err)
 		goto fail_input;
 
+	err = asus_wmi_fan_init(asus); /* probably no problems on error */
+	asus_hwmon_fan_set_auto(asus);
+
 	err = asus_wmi_hwmon_init(asus);
 	if (err)
 		goto fail_hwmon;
@@ -1772,17 +2073,16 @@ static int asus_wmi_add(struct platform_device *pdev)
 	   stop this from showing up */
 	chassis_type = dmi_get_system_info(DMI_CHASSIS_TYPE);
 	if (chassis_type && !strcmp(chassis_type, "3"))
-		acpi_video_dmi_promote_vendor();
+		acpi_video_set_dmi_backlight_type(acpi_backlight_vendor);
+
 	if (asus->driver->quirks->wmi_backlight_power)
-		acpi_video_dmi_promote_vendor();
-	if (!acpi_video_backlight_support()) {
-		pr_info("Disabling ACPI video driver\n");
-		acpi_video_unregister();
+		acpi_video_set_dmi_backlight_type(acpi_backlight_vendor);
+
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		err = asus_wmi_backlight_init(asus);
 		if (err && err != -ENODEV)
 			goto fail_backlight;
-	} else
-		pr_info("Backlight controlled by ACPI video driver\n");
+	}
 
 	status = wmi_install_notify_handler(asus->driver->event_guid,
 					    asus_wmi_notify, asus);
@@ -1832,6 +2132,7 @@ static int asus_wmi_remove(struct platform_device *device)
 	asus_wmi_rfkill_exit(asus);
 	asus_wmi_debugfs_exit(asus);
 	asus_wmi_platform_exit(asus);
+	asus_hwmon_fan_set_auto(asus);
 
 	kfree(asus);
 	return 0;
diff --git a/drivers/platform/x86/compal-laptop.c b/drivers/platform/x86/compal-laptop.c
index b4e94471f3d5..f2706d27adff 100644
--- a/drivers/platform/x86/compal-laptop.c
+++ b/drivers/platform/x86/compal-laptop.c
@@ -82,7 +82,7 @@
 #include <linux/hwmon-sysfs.h>
 #include <linux/power_supply.h>
 #include <linux/fb.h>
-
+#include <acpi/video.h>
 
 /* ======= */
 /* Defines */
@@ -959,7 +959,7 @@ static int __init compal_init(void)
 		return -ENODEV;
 	}
 
-	if (!acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		struct backlight_properties props;
 		memset(&props, 0, sizeof(struct backlight_properties));
 		props.type = BACKLIGHT_PLATFORM;
diff --git a/drivers/platform/x86/dell-laptop.c b/drivers/platform/x86/dell-laptop.c
index d688d806a8a5..ed317ccac4a2 100644
--- a/drivers/platform/x86/dell-laptop.c
+++ b/drivers/platform/x86/dell-laptop.c
@@ -31,7 +31,9 @@
 #include <linux/slab.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
+#include <acpi/video.h>
 #include "../../firmware/dcdbas.h"
+#include "dell-rbtn.h"
 
 #define BRIGHTNESS_TOKEN 0x7d
 #define KBD_LED_OFF_TOKEN 0x01E1
@@ -305,7 +307,6 @@ static const struct dmi_system_id dell_quirks[] __initconst = {
 };
 
 static struct calling_interface_buffer *buffer;
-static struct page *bufferpage;
 static DEFINE_MUTEX(buffer_mutex);
 
 static int hwswitch_state;
@@ -422,45 +423,125 @@ static inline int dell_smi_error(int value)
 	}
 }
 
-/* Derived from information in DellWirelessCtl.cpp:
-   Class 17, select 11 is radio control. It returns an array of 32-bit values.
-
-   Input byte 0 = 0: Wireless information
-
-   result[0]: return code
-   result[1]:
-     Bit 0:      Hardware switch supported
-     Bit 1:      Wifi locator supported
-     Bit 2:      Wifi is supported
-     Bit 3:      Bluetooth is supported
-     Bit 4:      WWAN is supported
-     Bit 5:      Wireless keyboard supported
-     Bits 6-7:   Reserved
-     Bit 8:      Wifi is installed
-     Bit 9:      Bluetooth is installed
-     Bit 10:     WWAN is installed
-     Bits 11-15: Reserved
-     Bit 16:     Hardware switch is on
-     Bit 17:     Wifi is blocked
-     Bit 18:     Bluetooth is blocked
-     Bit 19:     WWAN is blocked
-     Bits 20-31: Reserved
-   result[2]: NVRAM size in bytes
-   result[3]: NVRAM format version number
-
-   Input byte 0 = 2: Wireless switch configuration
-   result[0]: return code
-   result[1]:
-     Bit 0:      Wifi controlled by switch
-     Bit 1:      Bluetooth controlled by switch
-     Bit 2:      WWAN controlled by switch
-     Bits 3-6:   Reserved
-     Bit 7:      Wireless switch config locked
-     Bit 8:      Wifi locator enabled
-     Bits 9-14:  Reserved
-     Bit 15:     Wifi locator setting locked
-     Bits 16-31: Reserved
-*/
+/*
+ * Derived from information in smbios-wireless-ctl:
+ *
+ * cbSelect 17, Value 11
+ *
+ * Return Wireless Info
+ * cbArg1, byte0 = 0x00
+ *
+ *     cbRes1 Standard return codes (0, -1, -2)
+ *     cbRes2 Info bit flags:
+ *
+ *     0 Hardware switch supported (1)
+ *     1 WiFi locator supported (1)
+ *     2 WLAN supported (1)
+ *     3 Bluetooth (BT) supported (1)
+ *     4 WWAN supported (1)
+ *     5 Wireless KBD supported (1)
+ *     6 Uw b supported (1)
+ *     7 WiGig supported (1)
+ *     8 WLAN installed (1)
+ *     9 BT installed (1)
+ *     10 WWAN installed (1)
+ *     11 Uw b installed (1)
+ *     12 WiGig installed (1)
+ *     13-15 Reserved (0)
+ *     16 Hardware (HW) switch is On (1)
+ *     17 WLAN disabled (1)
+ *     18 BT disabled (1)
+ *     19 WWAN disabled (1)
+ *     20 Uw b disabled (1)
+ *     21 WiGig disabled (1)
+ *     20-31 Reserved (0)
+ *
+ *     cbRes3 NVRAM size in bytes
+ *     cbRes4, byte 0 NVRAM format version number
+ *
+ *
+ * Set QuickSet Radio Disable Flag
+ *     cbArg1, byte0 = 0x01
+ *     cbArg1, byte1
+ *     Radio ID     value:
+ *     0        Radio Status
+ *     1        WLAN ID
+ *     2        BT ID
+ *     3        WWAN ID
+ *     4        UWB ID
+ *     5        WIGIG ID
+ *     cbArg1, byte2    Flag bits:
+ *             0 QuickSet disables radio (1)
+ *             1-7 Reserved (0)
+ *
+ *     cbRes1    Standard return codes (0, -1, -2)
+ *     cbRes2    QuickSet (QS) radio disable bit map:
+ *     0 QS disables WLAN
+ *     1 QS disables BT
+ *     2 QS disables WWAN
+ *     3 QS disables UWB
+ *     4 QS disables WIGIG
+ *     5-31 Reserved (0)
+ *
+ * Wireless Switch Configuration
+ *     cbArg1, byte0 = 0x02
+ *
+ *     cbArg1, byte1
+ *     Subcommand:
+ *     0 Get config
+ *     1 Set config
+ *     2 Set WiFi locator enable/disable
+ *     cbArg1,byte2
+ *     Switch settings (if byte 1==1):
+ *     0 WLAN sw itch control (1)
+ *     1 BT sw itch control (1)
+ *     2 WWAN sw itch control (1)
+ *     3 UWB sw itch control (1)
+ *     4 WiGig sw itch control (1)
+ *     5-7 Reserved (0)
+ *    cbArg1, byte2 Enable bits (if byte 1==2):
+ *     0 Enable WiFi locator (1)
+ *
+ *    cbRes1     Standard return codes (0, -1, -2)
+ *    cbRes2 QuickSet radio disable bit map:
+ *     0 WLAN controlled by sw itch (1)
+ *     1 BT controlled by sw itch (1)
+ *     2 WWAN controlled by sw itch (1)
+ *     3 UWB controlled by sw itch (1)
+ *     4 WiGig controlled by sw itch (1)
+ *     5-6 Reserved (0)
+ *     7 Wireless sw itch config locked (1)
+ *     8 WiFi locator enabled (1)
+ *     9-14 Reserved (0)
+ *     15 WiFi locator setting locked (1)
+ *     16-31 Reserved (0)
+ *
+ * Read Local Config Data (LCD)
+ *     cbArg1, byte0 = 0x10
+ *     cbArg1, byte1 NVRAM index low byte
+ *     cbArg1, byte2 NVRAM index high byte
+ *     cbRes1 Standard return codes (0, -1, -2)
+ *     cbRes2 4 bytes read from LCD[index]
+ *     cbRes3 4 bytes read from LCD[index+4]
+ *     cbRes4 4 bytes read from LCD[index+8]
+ *
+ * Write Local Config Data (LCD)
+ *     cbArg1, byte0 = 0x11
+ *     cbArg1, byte1 NVRAM index low byte
+ *     cbArg1, byte2 NVRAM index high byte
+ *     cbArg2 4 bytes to w rite at LCD[index]
+ *     cbArg3 4 bytes to w rite at LCD[index+4]
+ *     cbArg4 4 bytes to w rite at LCD[index+8]
+ *     cbRes1 Standard return codes (0, -1, -2)
+ *
+ * Populate Local Config Data from NVRAM
+ *     cbArg1, byte0 = 0x12
+ *     cbRes1 Standard return codes (0, -1, -2)
+ *
+ * Commit Local Config Data to NVRAM
+ *     cbArg1, byte0 = 0x13
+ *     cbRes1 Standard return codes (0, -1, -2)
+ */
 
 static int dell_rfkill_set(void *data, bool blocked)
 {
@@ -548,12 +629,21 @@ static int dell_debugfs_show(struct seq_file *s, void *data)
 		  (status & BIT(4)) >> 4);
 	seq_printf(s, "Bit 5 : Wireless keyboard supported: %lu\n",
 		  (status & BIT(5)) >> 5);
+	seq_printf(s, "Bit 6 : UWB supported:               %lu\n",
+		  (status & BIT(6)) >> 6);
+	seq_printf(s, "Bit 7 : WiGig supported:             %lu\n",
+		  (status & BIT(7)) >> 7);
 	seq_printf(s, "Bit 8 : Wifi is installed:           %lu\n",
 		  (status & BIT(8)) >> 8);
 	seq_printf(s, "Bit 9 : Bluetooth is installed:      %lu\n",
 		  (status & BIT(9)) >> 9);
 	seq_printf(s, "Bit 10: WWAN is installed:           %lu\n",
 		  (status & BIT(10)) >> 10);
+	seq_printf(s, "Bit 11: UWB installed:               %lu\n",
+		  (status & BIT(11)) >> 11);
+	seq_printf(s, "Bit 12: WiGig installed:             %lu\n",
+		  (status & BIT(12)) >> 12);
+
 	seq_printf(s, "Bit 16: Hardware switch is on:       %lu\n",
 		  (status & BIT(16)) >> 16);
 	seq_printf(s, "Bit 17: Wifi is blocked:             %lu\n",
@@ -562,6 +652,10 @@ static int dell_debugfs_show(struct seq_file *s, void *data)
 		  (status & BIT(18)) >> 18);
 	seq_printf(s, "Bit 19: WWAN is blocked:             %lu\n",
 		  (status & BIT(19)) >> 19);
+	seq_printf(s, "Bit 20: UWB is blocked:              %lu\n",
+		  (status & BIT(20)) >> 20);
+	seq_printf(s, "Bit 21: WiGig is blocked:            %lu\n",
+		  (status & BIT(21)) >> 21);
 
 	seq_printf(s, "\nhwswitch_state:\t0x%X\n", hwswitch_state);
 	seq_printf(s, "Bit 0 : Wifi controlled by switch:      %lu\n",
@@ -570,6 +664,10 @@ static int dell_debugfs_show(struct seq_file *s, void *data)
 		   (hwswitch_state & BIT(1)) >> 1);
 	seq_printf(s, "Bit 2 : WWAN controlled by switch:      %lu\n",
 		   (hwswitch_state & BIT(2)) >> 2);
+	seq_printf(s, "Bit 3 : UWB controlled by switch:       %lu\n",
+		   (hwswitch_state & BIT(3)) >> 3);
+	seq_printf(s, "Bit 4 : WiGig controlled by switch:     %lu\n",
+		   (hwswitch_state & BIT(4)) >> 4);
 	seq_printf(s, "Bit 7 : Wireless switch config locked:  %lu\n",
 		   (hwswitch_state & BIT(7)) >> 7);
 	seq_printf(s, "Bit 8 : Wifi locator enabled:           %lu\n",
@@ -642,6 +740,20 @@ static bool dell_laptop_i8042_filter(unsigned char data, unsigned char str,
 	return false;
 }
 
+static int (*dell_rbtn_notifier_register_func)(struct notifier_block *);
+static int (*dell_rbtn_notifier_unregister_func)(struct notifier_block *);
+
+static int dell_laptop_rbtn_notifier_call(struct notifier_block *nb,
+					  unsigned long action, void *data)
+{
+	schedule_delayed_work(&dell_rfkill_work, 0);
+	return NOTIFY_OK;
+}
+
+static struct notifier_block dell_laptop_rbtn_notifier = {
+	.notifier_call = dell_laptop_rbtn_notifier_call,
+};
+
 static int __init dell_setup_rfkill(void)
 {
 	int status, ret, whitelisted;
@@ -718,10 +830,62 @@ static int __init dell_setup_rfkill(void)
 			goto err_wwan;
 	}
 
-	ret = i8042_install_filter(dell_laptop_i8042_filter);
-	if (ret) {
-		pr_warn("Unable to install key filter\n");
+	/*
+	 * Dell Airplane Mode Switch driver (dell-rbtn) supports ACPI devices
+	 * which can receive events from HW slider switch.
+	 *
+	 * Dell SMBIOS on whitelisted models supports controlling radio devices
+	 * but does not support receiving HW button switch events. We can use
+	 * i8042 filter hook function to receive keyboard data and handle
+	 * keycode for HW button.
+	 *
+	 * So if it is possible we will use Dell Airplane Mode Switch ACPI
+	 * driver for receiving HW events and Dell SMBIOS for setting rfkill
+	 * states. If ACPI driver or device is not available we will fallback to
+	 * i8042 filter hook function.
+	 *
+	 * To prevent duplicate rfkill devices which control and do same thing,
+	 * dell-rbtn driver will automatically remove its own rfkill devices
+	 * once function dell_rbtn_notifier_register() is called.
+	 */
+
+	dell_rbtn_notifier_register_func =
+		symbol_request(dell_rbtn_notifier_register);
+	if (dell_rbtn_notifier_register_func) {
+		dell_rbtn_notifier_unregister_func =
+			symbol_request(dell_rbtn_notifier_unregister);
+		if (!dell_rbtn_notifier_unregister_func) {
+			symbol_put(dell_rbtn_notifier_register);
+			dell_rbtn_notifier_register_func = NULL;
+		}
+	}
+
+	if (dell_rbtn_notifier_register_func) {
+		ret = dell_rbtn_notifier_register_func(
+			&dell_laptop_rbtn_notifier);
+		symbol_put(dell_rbtn_notifier_register);
+		dell_rbtn_notifier_register_func = NULL;
+		if (ret != 0) {
+			symbol_put(dell_rbtn_notifier_unregister);
+			dell_rbtn_notifier_unregister_func = NULL;
+		}
+	} else {
+		pr_info("Symbols from dell-rbtn acpi driver are not available\n");
+		ret = -ENODEV;
+	}
+
+	if (ret == 0) {
+		pr_info("Using dell-rbtn acpi driver for receiving events\n");
+	} else if (ret != -ENODEV) {
+		pr_warn("Unable to register dell rbtn notifier\n");
 		goto err_filter;
+	} else {
+		ret = i8042_install_filter(dell_laptop_i8042_filter);
+		if (ret) {
+			pr_warn("Unable to install key filter\n");
+			goto err_filter;
+		}
+		pr_info("Using i8042 filter function for receiving events\n");
 	}
 
 	return 0;
@@ -744,6 +908,14 @@ err_wifi:
 
 static void dell_cleanup_rfkill(void)
 {
+	if (dell_rbtn_notifier_unregister_func) {
+		dell_rbtn_notifier_unregister_func(&dell_laptop_rbtn_notifier);
+		symbol_put(dell_rbtn_notifier_unregister);
+		dell_rbtn_notifier_unregister_func = NULL;
+	} else {
+		i8042_remove_filter(dell_laptop_i8042_filter);
+	}
+	cancel_delayed_work_sync(&dell_rfkill_work);
 	if (wifi_rfkill) {
 		rfkill_unregister(wifi_rfkill);
 		rfkill_destroy(wifi_rfkill);
@@ -1896,12 +2068,11 @@ static int __init dell_init(void)
 	 * Allocate buffer below 4GB for SMI data--only 32-bit physical addr
 	 * is passed to SMI handler.
 	 */
-	bufferpage = alloc_page(GFP_KERNEL | GFP_DMA32);
-	if (!bufferpage) {
+	buffer = (void *)__get_free_page(GFP_KERNEL | GFP_DMA32);
+	if (!buffer) {
 		ret = -ENOMEM;
 		goto fail_buffer;
 	}
-	buffer = page_address(bufferpage);
 
 	ret = dell_setup_rfkill();
 
@@ -1920,13 +2091,8 @@ static int __init dell_init(void)
 		debugfs_create_file("rfkill", 0444, dell_laptop_dir, NULL,
 				    &dell_debugfs_fops);
 
-#ifdef CONFIG_ACPI
-	/* In the event of an ACPI backlight being available, don't
-	 * register the platform controller.
-	 */
-	if (acpi_video_backlight_support())
+	if (acpi_video_get_backlight_type() != acpi_backlight_vendor)
 		return 0;
-#endif
 
 	get_buffer();
 	buffer->input[0] = find_token_location(BRIGHTNESS_TOKEN);
@@ -1961,11 +2127,9 @@ static int __init dell_init(void)
 	return 0;
 
 fail_backlight:
-	i8042_remove_filter(dell_laptop_i8042_filter);
-	cancel_delayed_work_sync(&dell_rfkill_work);
 	dell_cleanup_rfkill();
 fail_rfkill:
-	free_page((unsigned long)bufferpage);
+	free_page((unsigned long)buffer);
 fail_buffer:
 	platform_device_del(platform_device);
 fail_platform_device2:
@@ -1983,8 +2147,6 @@ static void __exit dell_exit(void)
 	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);
 	dell_cleanup_rfkill();
 	if (platform_device) {
@@ -1995,7 +2157,14 @@ static void __exit dell_exit(void)
 	free_page((unsigned long)buffer);
 }
 
-module_init(dell_init);
+/* dell-rbtn.c driver export functions which will not work correctly (and could
+ * cause kernel crash) if they are called before dell-rbtn.c init code. This is
+ * not problem when dell-rbtn.c is compiled as external module. When both files
+ * (dell-rbtn.c and dell-laptop.c) are compiled statically into kernel, then we
+ * need to ensure that dell_init() will be called after initializing dell-rbtn.
+ * This can be achieved by late_initcall() instead module_init().
+ */
+late_initcall(dell_init);
 module_exit(dell_exit);
 
 MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
diff --git a/drivers/platform/x86/dell-rbtn.c b/drivers/platform/x86/dell-rbtn.c
new file mode 100644
index 000000000000..cd410e392550
--- /dev/null
+++ b/drivers/platform/x86/dell-rbtn.c
@@ -0,0 +1,423 @@
+/*
+    Dell Airplane Mode Switch driver
+    Copyright (C) 2014-2015  Pali Rohár <pali.rohar@gmail.com>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+*/
+
+#include <linux/module.h>
+#include <linux/acpi.h>
+#include <linux/rfkill.h>
+#include <linux/input.h>
+
+enum rbtn_type {
+	RBTN_UNKNOWN,
+	RBTN_TOGGLE,
+	RBTN_SLIDER,
+};
+
+struct rbtn_data {
+	enum rbtn_type type;
+	struct rfkill *rfkill;
+	struct input_dev *input_dev;
+};
+
+
+/*
+ * acpi functions
+ */
+
+static enum rbtn_type rbtn_check(struct acpi_device *device)
+{
+	unsigned long long output;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(device->handle, "CRBT", NULL, &output);
+	if (ACPI_FAILURE(status))
+		return RBTN_UNKNOWN;
+
+	switch (output) {
+	case 0:
+	case 1:
+		return RBTN_TOGGLE;
+	case 2:
+	case 3:
+		return RBTN_SLIDER;
+	default:
+		return RBTN_UNKNOWN;
+	}
+}
+
+static int rbtn_get(struct acpi_device *device)
+{
+	unsigned long long output;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(device->handle, "GRBT", NULL, &output);
+	if (ACPI_FAILURE(status))
+		return -EINVAL;
+
+	return !output;
+}
+
+static int rbtn_acquire(struct acpi_device *device, bool enable)
+{
+	struct acpi_object_list input;
+	union acpi_object param;
+	acpi_status status;
+
+	param.type = ACPI_TYPE_INTEGER;
+	param.integer.value = enable;
+	input.count = 1;
+	input.pointer = &param;
+
+	status = acpi_evaluate_object(device->handle, "ARBT", &input, NULL);
+	if (ACPI_FAILURE(status))
+		return -EINVAL;
+
+	return 0;
+}
+
+
+/*
+ * rfkill device
+ */
+
+static void rbtn_rfkill_query(struct rfkill *rfkill, void *data)
+{
+	struct acpi_device *device = data;
+	int state;
+
+	state = rbtn_get(device);
+	if (state < 0)
+		return;
+
+	rfkill_set_states(rfkill, state, state);
+}
+
+static int rbtn_rfkill_set_block(void *data, bool blocked)
+{
+	/* NOTE: setting soft rfkill state is not supported */
+	return -EINVAL;
+}
+
+static struct rfkill_ops rbtn_ops = {
+	.query = rbtn_rfkill_query,
+	.set_block = rbtn_rfkill_set_block,
+};
+
+static int rbtn_rfkill_init(struct acpi_device *device)
+{
+	struct rbtn_data *rbtn_data = device->driver_data;
+	int ret;
+
+	if (rbtn_data->rfkill)
+		return 0;
+
+	/*
+	 * NOTE: rbtn controls all radio devices, not only WLAN
+	 *       but rfkill interface does not support "ANY" type
+	 *       so "WLAN" type is used
+	 */
+	rbtn_data->rfkill = rfkill_alloc("dell-rbtn", &device->dev,
+					 RFKILL_TYPE_WLAN, &rbtn_ops, device);
+	if (!rbtn_data->rfkill)
+		return -ENOMEM;
+
+	ret = rfkill_register(rbtn_data->rfkill);
+	if (ret) {
+		rfkill_destroy(rbtn_data->rfkill);
+		rbtn_data->rfkill = NULL;
+		return ret;
+	}
+
+	return 0;
+}
+
+static void rbtn_rfkill_exit(struct acpi_device *device)
+{
+	struct rbtn_data *rbtn_data = device->driver_data;
+
+	if (!rbtn_data->rfkill)
+		return;
+
+	rfkill_unregister(rbtn_data->rfkill);
+	rfkill_destroy(rbtn_data->rfkill);
+	rbtn_data->rfkill = NULL;
+}
+
+static void rbtn_rfkill_event(struct acpi_device *device)
+{
+	struct rbtn_data *rbtn_data = device->driver_data;
+
+	if (rbtn_data->rfkill)
+		rbtn_rfkill_query(rbtn_data->rfkill, device);
+}
+
+
+/*
+ * input device
+ */
+
+static int rbtn_input_init(struct rbtn_data *rbtn_data)
+{
+	int ret;
+
+	rbtn_data->input_dev = input_allocate_device();
+	if (!rbtn_data->input_dev)
+		return -ENOMEM;
+
+	rbtn_data->input_dev->name = "DELL Wireless hotkeys";
+	rbtn_data->input_dev->phys = "dellabce/input0";
+	rbtn_data->input_dev->id.bustype = BUS_HOST;
+	rbtn_data->input_dev->evbit[0] = BIT(EV_KEY);
+	set_bit(KEY_RFKILL, rbtn_data->input_dev->keybit);
+
+	ret = input_register_device(rbtn_data->input_dev);
+	if (ret) {
+		input_free_device(rbtn_data->input_dev);
+		rbtn_data->input_dev = NULL;
+		return ret;
+	}
+
+	return 0;
+}
+
+static void rbtn_input_exit(struct rbtn_data *rbtn_data)
+{
+	input_unregister_device(rbtn_data->input_dev);
+	rbtn_data->input_dev = NULL;
+}
+
+static void rbtn_input_event(struct rbtn_data *rbtn_data)
+{
+	input_report_key(rbtn_data->input_dev, KEY_RFKILL, 1);
+	input_sync(rbtn_data->input_dev);
+	input_report_key(rbtn_data->input_dev, KEY_RFKILL, 0);
+	input_sync(rbtn_data->input_dev);
+}
+
+
+/*
+ * acpi driver
+ */
+
+static int rbtn_add(struct acpi_device *device);
+static int rbtn_remove(struct acpi_device *device);
+static void rbtn_notify(struct acpi_device *device, u32 event);
+
+static const struct acpi_device_id rbtn_ids[] = {
+	{ "DELRBTN", 0 },
+	{ "DELLABCE", 0 },
+	{ "", 0 },
+};
+
+static struct acpi_driver rbtn_driver = {
+	.name = "dell-rbtn",
+	.ids = rbtn_ids,
+	.ops = {
+		.add = rbtn_add,
+		.remove = rbtn_remove,
+		.notify = rbtn_notify,
+	},
+	.owner = THIS_MODULE,
+};
+
+
+/*
+ * notifier export functions
+ */
+
+static bool auto_remove_rfkill = true;
+
+static ATOMIC_NOTIFIER_HEAD(rbtn_chain_head);
+
+static int rbtn_inc_count(struct device *dev, void *data)
+{
+	struct acpi_device *device = to_acpi_device(dev);
+	struct rbtn_data *rbtn_data = device->driver_data;
+	int *count = data;
+
+	if (rbtn_data->type == RBTN_SLIDER)
+		(*count)++;
+
+	return 0;
+}
+
+static int rbtn_switch_dev(struct device *dev, void *data)
+{
+	struct acpi_device *device = to_acpi_device(dev);
+	struct rbtn_data *rbtn_data = device->driver_data;
+	bool enable = data;
+
+	if (rbtn_data->type != RBTN_SLIDER)
+		return 0;
+
+	if (enable)
+		rbtn_rfkill_init(device);
+	else
+		rbtn_rfkill_exit(device);
+
+	return 0;
+}
+
+int dell_rbtn_notifier_register(struct notifier_block *nb)
+{
+	bool first;
+	int count;
+	int ret;
+
+	count = 0;
+	ret = driver_for_each_device(&rbtn_driver.drv, NULL, &count,
+				     rbtn_inc_count);
+	if (ret || count == 0)
+		return -ENODEV;
+
+	first = !rbtn_chain_head.head;
+
+	ret = atomic_notifier_chain_register(&rbtn_chain_head, nb);
+	if (ret != 0)
+		return ret;
+
+	if (auto_remove_rfkill && first)
+		ret = driver_for_each_device(&rbtn_driver.drv, NULL,
+					     (void *)false, rbtn_switch_dev);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(dell_rbtn_notifier_register);
+
+int dell_rbtn_notifier_unregister(struct notifier_block *nb)
+{
+	int ret;
+
+	ret = atomic_notifier_chain_unregister(&rbtn_chain_head, nb);
+	if (ret != 0)
+		return ret;
+
+	if (auto_remove_rfkill && !rbtn_chain_head.head)
+		ret = driver_for_each_device(&rbtn_driver.drv, NULL,
+					     (void *)true, rbtn_switch_dev);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(dell_rbtn_notifier_unregister);
+
+
+/*
+ * acpi driver functions
+ */
+
+static int rbtn_add(struct acpi_device *device)
+{
+	struct rbtn_data *rbtn_data;
+	enum rbtn_type type;
+	int ret = 0;
+
+	type = rbtn_check(device);
+	if (type == RBTN_UNKNOWN) {
+		dev_info(&device->dev, "Unknown device type\n");
+		return -EINVAL;
+	}
+
+	ret = rbtn_acquire(device, true);
+	if (ret < 0) {
+		dev_err(&device->dev, "Cannot enable device\n");
+		return ret;
+	}
+
+	rbtn_data = devm_kzalloc(&device->dev, sizeof(*rbtn_data), GFP_KERNEL);
+	if (!rbtn_data)
+		return -ENOMEM;
+
+	rbtn_data->type = type;
+	device->driver_data = rbtn_data;
+
+	switch (rbtn_data->type) {
+	case RBTN_TOGGLE:
+		ret = rbtn_input_init(rbtn_data);
+		break;
+	case RBTN_SLIDER:
+		if (auto_remove_rfkill && rbtn_chain_head.head)
+			ret = 0;
+		else
+			ret = rbtn_rfkill_init(device);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+
+}
+
+static int rbtn_remove(struct acpi_device *device)
+{
+	struct rbtn_data *rbtn_data = device->driver_data;
+
+	switch (rbtn_data->type) {
+	case RBTN_TOGGLE:
+		rbtn_input_exit(rbtn_data);
+		break;
+	case RBTN_SLIDER:
+		rbtn_rfkill_exit(device);
+		break;
+	default:
+		break;
+	}
+
+	rbtn_acquire(device, false);
+	device->driver_data = NULL;
+
+	return 0;
+}
+
+static void rbtn_notify(struct acpi_device *device, u32 event)
+{
+	struct rbtn_data *rbtn_data = device->driver_data;
+
+	if (event != 0x80) {
+		dev_info(&device->dev, "Received unknown event (0x%x)\n",
+			 event);
+		return;
+	}
+
+	switch (rbtn_data->type) {
+	case RBTN_TOGGLE:
+		rbtn_input_event(rbtn_data);
+		break;
+	case RBTN_SLIDER:
+		rbtn_rfkill_event(device);
+		atomic_notifier_call_chain(&rbtn_chain_head, event, device);
+		break;
+	default:
+		break;
+	}
+}
+
+
+/*
+ * module functions
+ */
+
+module_acpi_driver(rbtn_driver);
+
+module_param(auto_remove_rfkill, bool, 0444);
+
+MODULE_PARM_DESC(auto_remove_rfkill, "Automatically remove rfkill devices when "
+				     "other modules start receiving events "
+				     "from this module and re-add them when "
+				     "the last module stops receiving events "
+				     "(default true)");
+MODULE_DEVICE_TABLE(acpi, rbtn_ids);
+MODULE_DESCRIPTION("Dell Airplane Mode Switch driver");
+MODULE_AUTHOR("Pali Rohár <pali.rohar@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/dell-rbtn.h b/drivers/platform/x86/dell-rbtn.h
new file mode 100644
index 000000000000..c59cc6b8ec2b
--- /dev/null
+++ b/drivers/platform/x86/dell-rbtn.h
@@ -0,0 +1,24 @@
+/*
+    Dell Airplane Mode Switch driver
+    Copyright (C) 2014-2015  Pali Rohár <pali.rohar@gmail.com>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+*/
+
+#ifndef _DELL_RBTN_H_
+#define _DELL_RBTN_H_
+
+struct notifier_block;
+
+int dell_rbtn_notifier_register(struct notifier_block *nb);
+int dell_rbtn_notifier_unregister(struct notifier_block *nb);
+
+#endif
diff --git a/drivers/platform/x86/dell-wmi.c b/drivers/platform/x86/dell-wmi.c
index 6512a06bc053..f2d77fe696ac 100644
--- a/drivers/platform/x86/dell-wmi.c
+++ b/drivers/platform/x86/dell-wmi.c
@@ -35,6 +35,7 @@
 #include <linux/acpi.h>
 #include <linux/string.h>
 #include <linux/dmi.h>
+#include <acpi/video.h>
 
 MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
 MODULE_DESCRIPTION("Dell laptop WMI hotkeys driver");
@@ -397,7 +398,7 @@ static int __init dell_wmi_init(void)
 	}
 
 	dmi_walk(find_hk_type, NULL);
-	acpi_video = acpi_video_backlight_support();
+	acpi_video = acpi_video_get_backlight_type() != acpi_backlight_vendor;
 
 	err = dell_wmi_input_setup();
 	if (err)
diff --git a/drivers/platform/x86/eeepc-laptop.c b/drivers/platform/x86/eeepc-laptop.c
index 844c2096bde9..8cdf315f9730 100644
--- a/drivers/platform/x86/eeepc-laptop.c
+++ b/drivers/platform/x86/eeepc-laptop.c
@@ -37,6 +37,7 @@
 #include <linux/pci_hotplug.h>
 #include <linux/leds.h>
 #include <linux/dmi.h>
+#include <acpi/video.h>
 
 #define EEEPC_LAPTOP_VERSION	"0.1"
 #define EEEPC_LAPTOP_NAME	"Eee PC Hotkey Driver"
@@ -1433,12 +1434,10 @@ static int eeepc_acpi_add(struct acpi_device *device)
 	if (result)
 		goto fail_platform;
 
-	if (!acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		result = eeepc_backlight_init(eeepc);
 		if (result)
 			goto fail_backlight;
-	} else {
-		pr_info("Backlight controlled by ACPI video driver\n");
 	}
 
 	result = eeepc_input_init(eeepc);
diff --git a/drivers/platform/x86/fujitsu-laptop.c b/drivers/platform/x86/fujitsu-laptop.c
index 2a9afa261c61..1c62caff93fd 100644
--- a/drivers/platform/x86/fujitsu-laptop.c
+++ b/drivers/platform/x86/fujitsu-laptop.c
@@ -72,6 +72,7 @@
 #if defined(CONFIG_LEDS_CLASS) || defined(CONFIG_LEDS_CLASS_MODULE)
 #include <linux/leds.h>
 #endif
+#include <acpi/video.h>
 
 #define FUJITSU_DRIVER_VERSION "0.6.0"
 
@@ -1099,7 +1100,7 @@ static int __init fujitsu_init(void)
 
 	/* Register backlight stuff */
 
-	if (!acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		struct backlight_properties props;
 
 		memset(&props, 0, sizeof(struct backlight_properties));
@@ -1137,8 +1138,7 @@ static int __init fujitsu_init(void)
 	}
 
 	/* Sync backlight power status (needs FUJ02E3 device, hence deferred) */
-
-	if (!acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		if (call_fext_func(FUNC_BACKLIGHT, 0x2, 0x4, 0x0) == 3)
 			fujitsu->bl_device->props.power = FB_BLANK_POWERDOWN;
 		else
diff --git a/drivers/platform/x86/ideapad-laptop.c b/drivers/platform/x86/ideapad-laptop.c
index b496db87bc05..76b57388d01b 100644
--- a/drivers/platform/x86/ideapad-laptop.c
+++ b/drivers/platform/x86/ideapad-laptop.c
@@ -38,6 +38,7 @@
 #include <linux/i8042.h>
 #include <linux/dmi.h>
 #include <linux/device.h>
+#include <acpi/video.h>
 
 #define IDEAPAD_RFKILL_DEV_NUM	(3)
 
@@ -464,8 +465,9 @@ static const struct ideapad_rfk_data ideapad_rfk_data[] = {
 static int ideapad_rfk_set(void *data, bool blocked)
 {
 	struct ideapad_rfk_priv *priv = data;
+	int opcode = ideapad_rfk_data[priv->dev].opcode;
 
-	return write_ec_cmd(priv->priv->adev->handle, priv->dev, !blocked);
+	return write_ec_cmd(priv->priv->adev->handle, opcode, !blocked);
 }
 
 static struct rfkill_ops ideapad_rfk_ops = {
@@ -837,6 +839,13 @@ static const struct dmi_system_id no_hw_rfkill_list[] = {
 		},
 	},
 	{
+		.ident = "Lenovo G50-30",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+			DMI_MATCH(DMI_PRODUCT_VERSION, "Lenovo G50-30"),
+		},
+	},
+	{
 		.ident = "Lenovo Yoga 2 11 / 13 / Pro",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
@@ -903,7 +912,7 @@ static int ideapad_acpi_add(struct platform_device *pdev)
 	ideapad_sync_rfk_state(priv);
 	ideapad_sync_touchpad_state(priv);
 
-	if (!acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		ret = ideapad_backlight_init(priv);
 		if (ret && ret != -ENODEV)
 			goto backlight_failed;
diff --git a/drivers/platform/x86/intel_oaktrail.c b/drivers/platform/x86/intel_oaktrail.c
index 8037c8b46241..6aa33c4a809f 100644
--- a/drivers/platform/x86/intel_oaktrail.c
+++ b/drivers/platform/x86/intel_oaktrail.c
@@ -50,6 +50,7 @@
 #include <linux/platform_device.h>
 #include <linux/dmi.h>
 #include <linux/rfkill.h>
+#include <acpi/video.h>
 
 #define DRIVER_NAME	"intel_oaktrail"
 #define DRIVER_VERSION	"0.4ac1"
@@ -343,13 +344,11 @@ static int __init oaktrail_init(void)
 		goto err_device_add;
 	}
 
-	if (!acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		ret = oaktrail_backlight_init();
 		if (ret)
 			goto err_backlight;
-
-	} else
-		pr_info("Backlight controlled by ACPI video driver\n");
+	}
 
 	ret = oaktrail_rfkill_init();
 	if (ret) {
diff --git a/drivers/platform/x86/intel_pmc_ipc.c b/drivers/platform/x86/intel_pmc_ipc.c
new file mode 100644
index 000000000000..d734763dab69
--- /dev/null
+++ b/drivers/platform/x86/intel_pmc_ipc.c
@@ -0,0 +1,767 @@
+/*
+ * intel_pmc_ipc.c: Driver for the Intel PMC IPC mechanism
+ *
+ * (C) Copyright 2014-2015 Intel Corporation
+ *
+ * This driver is based on Intel SCU IPC driver(intel_scu_opc.c) by
+ *     Sreedhara DS <sreedhara.ds@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; version 2
+ * of the License.
+ *
+ * PMC running in ARC processor communicates with other entity running in IA
+ * core through IPC mechanism which in turn messaging between IA core ad PMC.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/pm.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/pm_qos.h>
+#include <linux/kernel.h>
+#include <linux/bitops.h>
+#include <linux/sched.h>
+#include <linux/atomic.h>
+#include <linux/notifier.h>
+#include <linux/suspend.h>
+#include <linux/acpi.h>
+#include <asm/intel_pmc_ipc.h>
+#include <linux/mfd/lpc_ich.h>
+
+/*
+ * IPC registers
+ * The IA write to IPC_CMD command register triggers an interrupt to the ARC,
+ * The ARC handles the interrupt and services it, writing optional data to
+ * the IPC1 registers, updates the IPC_STS response register with the status.
+ */
+#define IPC_CMD			0x0
+#define		IPC_CMD_MSI		0x100
+#define		IPC_CMD_SIZE		16
+#define		IPC_CMD_SUBCMD		12
+#define IPC_STATUS		0x04
+#define		IPC_STATUS_IRQ		0x4
+#define		IPC_STATUS_ERR		0x2
+#define		IPC_STATUS_BUSY		0x1
+#define IPC_SPTR		0x08
+#define IPC_DPTR		0x0C
+#define IPC_WRITE_BUFFER	0x80
+#define IPC_READ_BUFFER		0x90
+
+/*
+ * 16-byte buffer for sending data associated with IPC command.
+ */
+#define IPC_DATA_BUFFER_SIZE	16
+
+#define IPC_LOOP_CNT		3000000
+#define IPC_MAX_SEC		3
+
+#define IPC_TRIGGER_MODE_IRQ		true
+
+/* exported resources from IFWI */
+#define PLAT_RESOURCE_IPC_INDEX		0
+#define PLAT_RESOURCE_IPC_SIZE		0x1000
+#define PLAT_RESOURCE_GCR_SIZE		0x1000
+#define PLAT_RESOURCE_PUNIT_DATA_INDEX	1
+#define PLAT_RESOURCE_PUNIT_INTER_INDEX	2
+#define PLAT_RESOURCE_ACPI_IO_INDEX	0
+
+/*
+ * BIOS does not create an ACPI device for each PMC function,
+ * but exports multiple resources from one ACPI device(IPC) for
+ * multiple functions. This driver is responsible to create a
+ * platform device and to export resources for those functions.
+ */
+#define TCO_DEVICE_NAME			"iTCO_wdt"
+#define SMI_EN_OFFSET			0x30
+#define SMI_EN_SIZE			4
+#define TCO_BASE_OFFSET			0x60
+#define TCO_REGS_SIZE			16
+#define PUNIT_DEVICE_NAME		"intel_punit_ipc"
+
+static const int iTCO_version = 3;
+
+static struct intel_pmc_ipc_dev {
+	struct device *dev;
+	void __iomem *ipc_base;
+	bool irq_mode;
+	int irq;
+	int cmd;
+	struct completion cmd_complete;
+
+	/* The following PMC BARs share the same ACPI device with the IPC */
+	void *acpi_io_base;
+	int acpi_io_size;
+	struct platform_device *tco_dev;
+
+	/* gcr */
+	void *gcr_base;
+	int gcr_size;
+
+	/* punit */
+	void *punit_base;
+	int punit_size;
+	void *punit_base2;
+	int punit_size2;
+	struct platform_device *punit_dev;
+} ipcdev;
+
+static char *ipc_err_sources[] = {
+	[IPC_ERR_NONE] =
+		"no error",
+	[IPC_ERR_CMD_NOT_SUPPORTED] =
+		"command not supported",
+	[IPC_ERR_CMD_NOT_SERVICED] =
+		"command not serviced",
+	[IPC_ERR_UNABLE_TO_SERVICE] =
+		"unable to service",
+	[IPC_ERR_CMD_INVALID] =
+		"command invalid",
+	[IPC_ERR_CMD_FAILED] =
+		"command failed",
+	[IPC_ERR_EMSECURITY] =
+		"Invalid Battery",
+	[IPC_ERR_UNSIGNEDKERNEL] =
+		"Unsigned kernel",
+};
+
+/* Prevent concurrent calls to the PMC */
+static DEFINE_MUTEX(ipclock);
+
+static inline void ipc_send_command(u32 cmd)
+{
+	ipcdev.cmd = cmd;
+	if (ipcdev.irq_mode) {
+		reinit_completion(&ipcdev.cmd_complete);
+		cmd |= IPC_CMD_MSI;
+	}
+	writel(cmd, ipcdev.ipc_base + IPC_CMD);
+}
+
+static inline u32 ipc_read_status(void)
+{
+	return readl(ipcdev.ipc_base + IPC_STATUS);
+}
+
+static inline void ipc_data_writel(u32 data, u32 offset)
+{
+	writel(data, ipcdev.ipc_base + IPC_WRITE_BUFFER + offset);
+}
+
+static inline u8 ipc_data_readb(u32 offset)
+{
+	return readb(ipcdev.ipc_base + IPC_READ_BUFFER + offset);
+}
+
+static inline u32 ipc_data_readl(u32 offset)
+{
+	return readl(ipcdev.ipc_base + IPC_READ_BUFFER + offset);
+}
+
+static int intel_pmc_ipc_check_status(void)
+{
+	int status;
+	int ret = 0;
+
+	if (ipcdev.irq_mode) {
+		if (0 == wait_for_completion_timeout(
+				&ipcdev.cmd_complete, IPC_MAX_SEC * HZ))
+			ret = -ETIMEDOUT;
+	} else {
+		int loop_count = IPC_LOOP_CNT;
+
+		while ((ipc_read_status() & IPC_STATUS_BUSY) && --loop_count)
+			udelay(1);
+		if (loop_count == 0)
+			ret = -ETIMEDOUT;
+	}
+
+	status = ipc_read_status();
+	if (ret == -ETIMEDOUT) {
+		dev_err(ipcdev.dev,
+			"IPC timed out, TS=0x%x, CMD=0x%x\n",
+			status, ipcdev.cmd);
+		return ret;
+	}
+
+	if (status & IPC_STATUS_ERR) {
+		int i;
+
+		ret = -EIO;
+		i = (status >> IPC_CMD_SIZE) & 0xFF;
+		if (i < ARRAY_SIZE(ipc_err_sources))
+			dev_err(ipcdev.dev,
+				"IPC failed: %s, STS=0x%x, CMD=0x%x\n",
+				ipc_err_sources[i], status, ipcdev.cmd);
+		else
+			dev_err(ipcdev.dev,
+				"IPC failed: unknown, STS=0x%x, CMD=0x%x\n",
+				status, ipcdev.cmd);
+		if ((i == IPC_ERR_UNSIGNEDKERNEL) || (i == IPC_ERR_EMSECURITY))
+			ret = -EACCES;
+	}
+
+	return ret;
+}
+
+/*
+ * intel_pmc_ipc_simple_command
+ * @cmd: command
+ * @sub: sub type
+ */
+int intel_pmc_ipc_simple_command(int cmd, int sub)
+{
+	int ret;
+
+	mutex_lock(&ipclock);
+	if (ipcdev.dev == NULL) {
+		mutex_unlock(&ipclock);
+		return -ENODEV;
+	}
+	ipc_send_command(sub << IPC_CMD_SUBCMD | cmd);
+	ret = intel_pmc_ipc_check_status();
+	mutex_unlock(&ipclock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(intel_pmc_ipc_simple_command);
+
+/*
+ * intel_pmc_ipc_raw_cmd
+ * @cmd: command
+ * @sub: sub type
+ * @in: input data
+ * @inlen: input length in bytes
+ * @out: output data
+ * @outlen: output length in dwords
+ * @sptr: data writing to SPTR register
+ * @dptr: data writing to DPTR register
+ */
+int intel_pmc_ipc_raw_cmd(u32 cmd, u32 sub, u8 *in, u32 inlen, u32 *out,
+			  u32 outlen, u32 dptr, u32 sptr)
+{
+	u32 wbuf[4] = { 0 };
+	int ret;
+	int i;
+
+	if (inlen > IPC_DATA_BUFFER_SIZE || outlen > IPC_DATA_BUFFER_SIZE / 4)
+		return -EINVAL;
+
+	mutex_lock(&ipclock);
+	if (ipcdev.dev == NULL) {
+		mutex_unlock(&ipclock);
+		return -ENODEV;
+	}
+	memcpy(wbuf, in, inlen);
+	writel(dptr, ipcdev.ipc_base + IPC_DPTR);
+	writel(sptr, ipcdev.ipc_base + IPC_SPTR);
+	/* The input data register is 32bit register and inlen is in Byte */
+	for (i = 0; i < ((inlen + 3) / 4); i++)
+		ipc_data_writel(wbuf[i], 4 * i);
+	ipc_send_command((inlen << IPC_CMD_SIZE) |
+			(sub << IPC_CMD_SUBCMD) | cmd);
+	ret = intel_pmc_ipc_check_status();
+	if (!ret) {
+		/* out is read from 32bit register and outlen is in 32bit */
+		for (i = 0; i < outlen; i++)
+			*out++ = ipc_data_readl(4 * i);
+	}
+	mutex_unlock(&ipclock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(intel_pmc_ipc_raw_cmd);
+
+/*
+ * intel_pmc_ipc_command
+ * @cmd: command
+ * @sub: sub type
+ * @in: input data
+ * @inlen: input length in bytes
+ * @out: output data
+ * @outlen: output length in dwords
+ */
+int intel_pmc_ipc_command(u32 cmd, u32 sub, u8 *in, u32 inlen,
+			  u32 *out, u32 outlen)
+{
+	return intel_pmc_ipc_raw_cmd(cmd, sub, in, inlen, out, outlen, 0, 0);
+}
+EXPORT_SYMBOL_GPL(intel_pmc_ipc_command);
+
+static irqreturn_t ioc(int irq, void *dev_id)
+{
+	int status;
+
+	if (ipcdev.irq_mode) {
+		status = ipc_read_status();
+		writel(status | IPC_STATUS_IRQ, ipcdev.ipc_base + IPC_STATUS);
+	}
+	complete(&ipcdev.cmd_complete);
+
+	return IRQ_HANDLED;
+}
+
+static int ipc_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	resource_size_t pci_resource;
+	int ret;
+	int len;
+
+	ipcdev.dev = &pci_dev_get(pdev)->dev;
+	ipcdev.irq_mode = IPC_TRIGGER_MODE_IRQ;
+
+	ret = pci_enable_device(pdev);
+	if (ret)
+		return ret;
+
+	ret = pci_request_regions(pdev, "intel_pmc_ipc");
+	if (ret)
+		return ret;
+
+	pci_resource = pci_resource_start(pdev, 0);
+	len = pci_resource_len(pdev, 0);
+	if (!pci_resource || !len) {
+		dev_err(&pdev->dev, "Failed to get resource\n");
+		return -ENOMEM;
+	}
+
+	init_completion(&ipcdev.cmd_complete);
+
+	if (request_irq(pdev->irq, ioc, 0, "intel_pmc_ipc", &ipcdev)) {
+		dev_err(&pdev->dev, "Failed to request irq\n");
+		return -EBUSY;
+	}
+
+	ipcdev.ipc_base = ioremap_nocache(pci_resource, len);
+	if (!ipcdev.ipc_base) {
+		dev_err(&pdev->dev, "Failed to ioremap ipc base\n");
+		free_irq(pdev->irq, &ipcdev);
+		ret = -ENOMEM;
+	}
+
+	return ret;
+}
+
+static void ipc_pci_remove(struct pci_dev *pdev)
+{
+	free_irq(pdev->irq, &ipcdev);
+	pci_release_regions(pdev);
+	pci_dev_put(pdev);
+	iounmap(ipcdev.ipc_base);
+	ipcdev.dev = NULL;
+}
+
+static const struct pci_device_id ipc_pci_ids[] = {
+	{PCI_VDEVICE(INTEL, 0x0a94), 0},
+	{PCI_VDEVICE(INTEL, 0x1a94), 0},
+	{ 0,}
+};
+MODULE_DEVICE_TABLE(pci, ipc_pci_ids);
+
+static struct pci_driver ipc_pci_driver = {
+	.name = "intel_pmc_ipc",
+	.id_table = ipc_pci_ids,
+	.probe = ipc_pci_probe,
+	.remove = ipc_pci_remove,
+};
+
+static ssize_t intel_pmc_ipc_simple_cmd_store(struct device *dev,
+					      struct device_attribute *attr,
+					      const char *buf, size_t count)
+{
+	int subcmd;
+	int cmd;
+	int ret;
+
+	ret = sscanf(buf, "%d %d", &cmd, &subcmd);
+	if (ret != 2) {
+		dev_err(dev, "Error args\n");
+		return -EINVAL;
+	}
+
+	ret = intel_pmc_ipc_simple_command(cmd, subcmd);
+	if (ret) {
+		dev_err(dev, "command %d error with %d\n", cmd, ret);
+		return ret;
+	}
+	return (ssize_t)count;
+}
+
+static ssize_t intel_pmc_ipc_northpeak_store(struct device *dev,
+					     struct device_attribute *attr,
+					     const char *buf, size_t count)
+{
+	unsigned long val;
+	int subcmd;
+	int ret;
+
+	if (kstrtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if (val)
+		subcmd = 1;
+	else
+		subcmd = 0;
+	ret = intel_pmc_ipc_simple_command(PMC_IPC_NORTHPEAK_CTRL, subcmd);
+	if (ret) {
+		dev_err(dev, "command north %d error with %d\n", subcmd, ret);
+		return ret;
+	}
+	return (ssize_t)count;
+}
+
+static DEVICE_ATTR(simplecmd, S_IWUSR,
+		   NULL, intel_pmc_ipc_simple_cmd_store);
+static DEVICE_ATTR(northpeak, S_IWUSR,
+		   NULL, intel_pmc_ipc_northpeak_store);
+
+static struct attribute *intel_ipc_attrs[] = {
+	&dev_attr_northpeak.attr,
+	&dev_attr_simplecmd.attr,
+	NULL
+};
+
+static const struct attribute_group intel_ipc_group = {
+	.attrs = intel_ipc_attrs,
+};
+
+#define PUNIT_RESOURCE_INTER		1
+static struct resource punit_res[] = {
+	/* Punit */
+	{
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+#define TCO_RESOURCE_ACPI_IO		0
+#define TCO_RESOURCE_SMI_EN_IO		1
+#define TCO_RESOURCE_GCR_MEM		2
+static struct resource tco_res[] = {
+	/* ACPI - TCO */
+	{
+		.flags = IORESOURCE_IO,
+	},
+	/* ACPI - SMI */
+	{
+		.flags = IORESOURCE_IO,
+	},
+	/* GCS */
+	{
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct lpc_ich_info tco_info = {
+	.name = "Apollo Lake SoC",
+	.iTCO_version = 3,
+};
+
+static int ipc_create_punit_device(void)
+{
+	struct platform_device *pdev;
+	struct resource *res;
+	int ret;
+
+	pdev = platform_device_alloc(PUNIT_DEVICE_NAME, -1);
+	if (!pdev) {
+		dev_err(ipcdev.dev, "Failed to alloc punit platform device\n");
+		return -ENOMEM;
+	}
+
+	pdev->dev.parent = ipcdev.dev;
+
+	res = punit_res;
+	res->start = (resource_size_t)ipcdev.punit_base;
+	res->end = res->start + ipcdev.punit_size - 1;
+
+	res = punit_res + PUNIT_RESOURCE_INTER;
+	res->start = (resource_size_t)ipcdev.punit_base2;
+	res->end = res->start + ipcdev.punit_size2 - 1;
+
+	ret = platform_device_add_resources(pdev, punit_res,
+					    ARRAY_SIZE(punit_res));
+	if (ret) {
+		dev_err(ipcdev.dev, "Failed to add platform punit resources\n");
+		goto err;
+	}
+
+	ret = platform_device_add(pdev);
+	if (ret) {
+		dev_err(ipcdev.dev, "Failed to add punit platform device\n");
+		goto err;
+	}
+	ipcdev.punit_dev = pdev;
+
+	return 0;
+err:
+	platform_device_put(pdev);
+	return ret;
+}
+
+static int ipc_create_tco_device(void)
+{
+	struct platform_device *pdev;
+	struct resource *res;
+	int ret;
+
+	pdev = platform_device_alloc(TCO_DEVICE_NAME, -1);
+	if (!pdev) {
+		dev_err(ipcdev.dev, "Failed to alloc tco platform device\n");
+		return -ENOMEM;
+	}
+
+	pdev->dev.parent = ipcdev.dev;
+
+	res = tco_res + TCO_RESOURCE_ACPI_IO;
+	res->start = (resource_size_t)ipcdev.acpi_io_base + TCO_BASE_OFFSET;
+	res->end = res->start + TCO_REGS_SIZE - 1;
+
+	res = tco_res + TCO_RESOURCE_SMI_EN_IO;
+	res->start = (resource_size_t)ipcdev.acpi_io_base + SMI_EN_OFFSET;
+	res->end = res->start + SMI_EN_SIZE - 1;
+
+	res = tco_res + TCO_RESOURCE_GCR_MEM;
+	res->start = (resource_size_t)ipcdev.gcr_base;
+	res->end = res->start + ipcdev.gcr_size - 1;
+
+	ret = platform_device_add_resources(pdev, tco_res, ARRAY_SIZE(tco_res));
+	if (ret) {
+		dev_err(ipcdev.dev, "Failed to add tco platform resources\n");
+		goto err;
+	}
+
+	ret = platform_device_add_data(pdev, &tco_info,
+				       sizeof(struct lpc_ich_info));
+	if (ret) {
+		dev_err(ipcdev.dev, "Failed to add tco platform data\n");
+		goto err;
+	}
+
+	ret = platform_device_add(pdev);
+	if (ret) {
+		dev_err(ipcdev.dev, "Failed to add tco platform device\n");
+		goto err;
+	}
+	ipcdev.tco_dev = pdev;
+
+	return 0;
+err:
+	platform_device_put(pdev);
+	return ret;
+}
+
+static int ipc_create_pmc_devices(void)
+{
+	int ret;
+
+	ret = ipc_create_tco_device();
+	if (ret) {
+		dev_err(ipcdev.dev, "Failed to add tco platform device\n");
+		return ret;
+	}
+	ret = ipc_create_punit_device();
+	if (ret) {
+		dev_err(ipcdev.dev, "Failed to add punit platform device\n");
+		platform_device_unregister(ipcdev.tco_dev);
+	}
+	return ret;
+}
+
+static int ipc_plat_get_res(struct platform_device *pdev)
+{
+	struct resource *res;
+	void __iomem *addr;
+	int size;
+
+	res = platform_get_resource(pdev, IORESOURCE_IO,
+				    PLAT_RESOURCE_ACPI_IO_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get io resource\n");
+		return -ENXIO;
+	}
+	size = resource_size(res);
+	ipcdev.acpi_io_base = (void *)res->start;
+	ipcdev.acpi_io_size = size;
+	dev_info(&pdev->dev, "io res: %llx %x\n",
+		 (long long)res->start, (int)resource_size(res));
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_PUNIT_DATA_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get punit resource\n");
+		return -ENXIO;
+	}
+	size = resource_size(res);
+	ipcdev.punit_base = (void *)res->start;
+	ipcdev.punit_size = size;
+	dev_info(&pdev->dev, "punit data res: %llx %x\n",
+		 (long long)res->start, (int)resource_size(res));
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_PUNIT_INTER_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get punit inter resource\n");
+		return -ENXIO;
+	}
+	size = resource_size(res);
+	ipcdev.punit_base2 = (void *)res->start;
+	ipcdev.punit_size2 = size;
+	dev_info(&pdev->dev, "punit interface res: %llx %x\n",
+		 (long long)res->start, (int)resource_size(res));
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_IPC_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get ipc resource\n");
+		return -ENXIO;
+	}
+	size = PLAT_RESOURCE_IPC_SIZE;
+	if (!request_mem_region(res->start, size, pdev->name)) {
+		dev_err(&pdev->dev, "Failed to request ipc resource\n");
+		return -EBUSY;
+	}
+	addr = ioremap_nocache(res->start, size);
+	if (!addr) {
+		dev_err(&pdev->dev, "I/O memory remapping failed\n");
+		release_mem_region(res->start, size);
+		return -ENOMEM;
+	}
+	ipcdev.ipc_base = addr;
+
+	ipcdev.gcr_base = (void *)(res->start + size);
+	ipcdev.gcr_size = PLAT_RESOURCE_GCR_SIZE;
+	dev_info(&pdev->dev, "ipc res: %llx %x\n",
+		 (long long)res->start, (int)resource_size(res));
+
+	return 0;
+}
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ipc_acpi_ids[] = {
+	{ "INT34D2", 0},
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, ipc_acpi_ids);
+#endif
+
+static int ipc_plat_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int ret;
+
+	ipcdev.dev = &pdev->dev;
+	ipcdev.irq_mode = IPC_TRIGGER_MODE_IRQ;
+	init_completion(&ipcdev.cmd_complete);
+
+	ipcdev.irq = platform_get_irq(pdev, 0);
+	if (ipcdev.irq < 0) {
+		dev_err(&pdev->dev, "Failed to get irq\n");
+		return -EINVAL;
+	}
+
+	ret = ipc_plat_get_res(pdev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request resource\n");
+		return ret;
+	}
+
+	ret = ipc_create_pmc_devices();
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to create pmc devices\n");
+		goto err_device;
+	}
+
+	if (request_irq(ipcdev.irq, ioc, 0, "intel_pmc_ipc", &ipcdev)) {
+		dev_err(&pdev->dev, "Failed to request irq\n");
+		ret = -EBUSY;
+		goto err_irq;
+	}
+
+	ret = sysfs_create_group(&pdev->dev.kobj, &intel_ipc_group);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to create sysfs group %d\n",
+			ret);
+		goto err_sys;
+	}
+
+	return 0;
+err_sys:
+	free_irq(ipcdev.irq, &ipcdev);
+err_irq:
+	platform_device_unregister(ipcdev.tco_dev);
+	platform_device_unregister(ipcdev.punit_dev);
+err_device:
+	iounmap(ipcdev.ipc_base);
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_IPC_INDEX);
+	if (res)
+		release_mem_region(res->start, PLAT_RESOURCE_IPC_SIZE);
+	return ret;
+}
+
+static int ipc_plat_remove(struct platform_device *pdev)
+{
+	struct resource *res;
+
+	sysfs_remove_group(&pdev->dev.kobj, &intel_ipc_group);
+	free_irq(ipcdev.irq, &ipcdev);
+	platform_device_unregister(ipcdev.tco_dev);
+	platform_device_unregister(ipcdev.punit_dev);
+	iounmap(ipcdev.ipc_base);
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_IPC_INDEX);
+	if (res)
+		release_mem_region(res->start, PLAT_RESOURCE_IPC_SIZE);
+	ipcdev.dev = NULL;
+	return 0;
+}
+
+static struct platform_driver ipc_plat_driver = {
+	.remove = ipc_plat_remove,
+	.probe = ipc_plat_probe,
+	.driver = {
+		.name = "pmc-ipc-plat",
+		.acpi_match_table = ACPI_PTR(ipc_acpi_ids),
+	},
+};
+
+static int __init intel_pmc_ipc_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&ipc_plat_driver);
+	if (ret) {
+		pr_err("Failed to register PMC ipc platform driver\n");
+		return ret;
+	}
+	ret = pci_register_driver(&ipc_pci_driver);
+	if (ret) {
+		pr_err("Failed to register PMC ipc pci driver\n");
+		platform_driver_unregister(&ipc_plat_driver);
+		return ret;
+	}
+	return ret;
+}
+
+static void __exit intel_pmc_ipc_exit(void)
+{
+	pci_unregister_driver(&ipc_pci_driver);
+	platform_driver_unregister(&ipc_plat_driver);
+}
+
+MODULE_AUTHOR("Zha Qipeng <qipeng.zha@intel.com>");
+MODULE_DESCRIPTION("Intel PMC IPC driver");
+MODULE_LICENSE("GPL");
+
+/* Some modules are dependent on this, so init earlier */
+fs_initcall(intel_pmc_ipc_init);
+module_exit(intel_pmc_ipc_exit);
diff --git a/drivers/platform/x86/msi-laptop.c b/drivers/platform/x86/msi-laptop.c
index 085987730aab..42317704629d 100644
--- a/drivers/platform/x86/msi-laptop.c
+++ b/drivers/platform/x86/msi-laptop.c
@@ -64,6 +64,7 @@
 #include <linux/i8042.h>
 #include <linux/input.h>
 #include <linux/input/sparse-keymap.h>
+#include <acpi/video.h>
 
 #define MSI_DRIVER_VERSION "0.5"
 
@@ -1069,9 +1070,8 @@ static int __init msi_init(void)
 
 	/* Register backlight stuff */
 
-	if (!quirks->old_ec_model || acpi_video_backlight_support()) {
-		pr_info("Brightness ignored, must be controlled by ACPI video driver\n");
-	} else {
+	if (quirks->old_ec_model ||
+	    acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		struct backlight_properties props;
 		memset(&props, 0, sizeof(struct backlight_properties));
 		props.type = BACKLIGHT_PLATFORM;
diff --git a/drivers/platform/x86/msi-wmi.c b/drivers/platform/x86/msi-wmi.c
index 6d2bac0c463c..978e6d640572 100644
--- a/drivers/platform/x86/msi-wmi.c
+++ b/drivers/platform/x86/msi-wmi.c
@@ -29,6 +29,7 @@
 #include <linux/backlight.h>
 #include <linux/slab.h>
 #include <linux/module.h>
+#include <acpi/video.h>
 
 MODULE_AUTHOR("Thomas Renninger <trenn@suse.de>");
 MODULE_DESCRIPTION("MSI laptop WMI hotkeys driver");
@@ -320,7 +321,8 @@ static int __init msi_wmi_init(void)
 		break;
 	}
 
-	if (wmi_has_guid(MSIWMI_BIOS_GUID) && !acpi_video_backlight_support()) {
+	if (wmi_has_guid(MSIWMI_BIOS_GUID) &&
+	    acpi_video_get_backlight_type() == acpi_backlight_vendor) {
 		err = msi_wmi_backlight_setup();
 		if (err) {
 			pr_err("Unable to setup backlight device\n");
diff --git a/drivers/platform/x86/pvpanic.c b/drivers/platform/x86/pvpanic.c
index 073a90a63dbc..fd86daba7ffd 100644
--- a/drivers/platform/x86/pvpanic.c
+++ b/drivers/platform/x86/pvpanic.c
@@ -92,13 +92,13 @@ pvpanic_walk_resources(struct acpi_resource *res, void *context)
 
 static int pvpanic_add(struct acpi_device *device)
 {
-	acpi_status status;
-	u64 ret;
+	int ret;
 
-	status = acpi_evaluate_integer(device->handle, "_STA", NULL,
-				       &ret);
+	ret = acpi_bus_get_status(device);
+	if (ret < 0)
+		return ret;
 
-	if (ACPI_FAILURE(status) || (ret & 0x0B) != 0x0B)
+	if (!device->status.enabled || !device->status.functional)
 		return -ENODEV;
 
 	acpi_walk_resources(device->handle, METHOD_NAME__CRS,
diff --git a/drivers/platform/x86/samsung-laptop.c b/drivers/platform/x86/samsung-laptop.c
index 9e701b2256f9..8c146e2b6727 100644
--- a/drivers/platform/x86/samsung-laptop.c
+++ b/drivers/platform/x86/samsung-laptop.c
@@ -1720,27 +1720,14 @@ static int __init samsung_init(void)
 	samsung->handle_backlight = true;
 	samsung->quirks = quirks;
 
-
 #ifdef CONFIG_ACPI
 	if (samsung->quirks->broken_acpi_video)
-		acpi_video_dmi_promote_vendor();
-
-	/* Don't handle backlight here if the acpi video already handle it */
-	if (acpi_video_backlight_support()) {
-		samsung->handle_backlight = false;
-	} else if (samsung->quirks->broken_acpi_video) {
-		pr_info("Disabling ACPI video driver\n");
-		acpi_video_unregister();
-	}
+		acpi_video_set_dmi_backlight_type(acpi_backlight_vendor);
+	if (samsung->quirks->use_native_backlight)
+		acpi_video_set_dmi_backlight_type(acpi_backlight_native);
 
-	if (samsung->quirks->use_native_backlight) {
-		pr_info("Using native backlight driver\n");
-		/* Tell acpi-video to not handle the backlight */
-		acpi_video_dmi_promote_vendor();
-		acpi_video_unregister();
-		/* And also do not handle it ourselves */
+	if (acpi_video_get_backlight_type() != acpi_backlight_vendor)
 		samsung->handle_backlight = false;
-	}
 #endif
 
 	ret = samsung_platform_init(samsung);
@@ -1751,12 +1738,6 @@ static int __init samsung_init(void)
 	if (ret)
 		goto error_sabi;
 
-#ifdef CONFIG_ACPI
-	/* Only log that if we are really on a sabi platform */
-	if (acpi_video_backlight_support())
-		pr_info("Backlight controlled by ACPI video driver\n");
-#endif
-
 	ret = samsung_sysfs_init(samsung);
 	if (ret)
 		goto error_sysfs;
diff --git a/drivers/platform/x86/sony-laptop.c b/drivers/platform/x86/sony-laptop.c
index e51c1e753607..aeb80d1c2b07 100644
--- a/drivers/platform/x86/sony-laptop.c
+++ b/drivers/platform/x86/sony-laptop.c
@@ -69,6 +69,7 @@
 #include <linux/miscdevice.h>
 #endif
 #include <asm/uaccess.h>
+#include <acpi/video.h>
 
 #define dprintk(fmt, ...)			\
 do {						\
@@ -3198,12 +3199,8 @@ static int sony_nc_add(struct acpi_device *device)
 			sony_nc_function_setup(device, sony_pf_device);
 	}
 
-	/* setup input devices and helper fifo */
-	if (acpi_video_backlight_support()) {
-		pr_info("brightness ignored, must be controlled by ACPI video driver\n");
-	} else {
+	if (acpi_video_get_backlight_type() == acpi_backlight_vendor)
 		sony_nc_backlight_setup();
-	}
 
 	/* create sony_pf sysfs attributes related to the SNC device */
 	for (item = sony_nc_values; item->name; ++item) {
diff --git a/drivers/platform/x86/tc1100-wmi.c b/drivers/platform/x86/tc1100-wmi.c
index e36542564131..89aa976f0ab2 100644
--- a/drivers/platform/x86/tc1100-wmi.c
+++ b/drivers/platform/x86/tc1100-wmi.c
@@ -82,7 +82,7 @@ static int get_state(u32 *out, u8 instance)
 		tmp = 0;
 	}
 
-	if (result.length > 0 && result.pointer)
+	if (result.length > 0)
 		kfree(result.pointer);
 
 	switch (instance) {
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index 28f328136f0d..33e488cf5569 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -83,6 +83,7 @@
 #include <sound/control.h>
 #include <sound/initval.h>
 #include <asm/uaccess.h>
+#include <acpi/video.h>
 
 /* ThinkPad CMOS commands */
 #define TP_CMOS_VOLUME_DOWN	0
@@ -3487,7 +3488,7 @@ static int __init hotkey_init(struct ibm_init_struct *iibm)
 	/* Do not issue duplicate brightness change events to
 	 * userspace. tpacpi_detect_brightness_capabilities() must have
 	 * been called before this point  */
-	if (acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() != acpi_backlight_vendor) {
 		pr_info("This ThinkPad has standard ACPI backlight "
 			"brightness control, supported by the ACPI "
 			"video driver\n");
@@ -6491,7 +6492,7 @@ static int __init brightness_init(struct ibm_init_struct *iibm)
 		return 1;
 	}
 
-	if (acpi_video_backlight_support()) {
+	if (acpi_video_get_backlight_type() != acpi_backlight_vendor) {
 		if (brightness_enable > 1) {
 			pr_info("Standard ACPI backlight interface "
 				"available, not loading native one\n");
diff --git a/drivers/platform/x86/toshiba_acpi.c b/drivers/platform/x86/toshiba_acpi.c
index 9956b9902bb4..3ad7b1fa24ce 100644
--- a/drivers/platform/x86/toshiba_acpi.c
+++ b/drivers/platform/x86/toshiba_acpi.c
@@ -31,7 +31,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#define TOSHIBA_ACPI_VERSION	"0.21"
+#define TOSHIBA_ACPI_VERSION	"0.22"
 #define PROC_INTERFACE_VERSION	1
 
 #include <linux/kernel.h>
@@ -41,7 +41,6 @@
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
 #include <linux/backlight.h>
-#include <linux/rfkill.h>
 #include <linux/input.h>
 #include <linux/input/sparse-keymap.h>
 #include <linux/leds.h>
@@ -82,7 +81,7 @@ MODULE_LICENSE("GPL");
 
 #define TCI_WORDS			6
 
-/* operations */
+/* Operations */
 #define HCI_SET				0xff00
 #define HCI_GET				0xfe00
 #define SCI_OPEN			0xf100
@@ -90,7 +89,7 @@ MODULE_LICENSE("GPL");
 #define SCI_GET				0xf300
 #define SCI_SET				0xf400
 
-/* return codes */
+/* Return codes */
 #define TOS_SUCCESS			0x0000
 #define TOS_OPEN_CLOSE_OK		0x0044
 #define TOS_FAILURE			0x1000
@@ -105,7 +104,7 @@ MODULE_LICENSE("GPL");
 #define TOS_NOT_INITIALIZED		0x8d50
 #define TOS_NOT_INSTALLED		0x8e00
 
-/* registers */
+/* Registers */
 #define HCI_FAN				0x0004
 #define HCI_TR_BACKLIGHT		0x0005
 #define HCI_SYSTEM_EVENT		0x0016
@@ -127,7 +126,7 @@ MODULE_LICENSE("GPL");
 #define SCI_TOUCHPAD			0x050e
 #define SCI_KBD_FUNCTION_KEYS		0x0522
 
-/* field definitions */
+/* Field definitions */
 #define HCI_ACCEL_MASK			0x7fff
 #define HCI_HOTKEY_DISABLE		0x0b
 #define HCI_HOTKEY_ENABLE		0x09
@@ -165,7 +164,6 @@ MODULE_LICENSE("GPL");
 struct toshiba_acpi_dev {
 	struct acpi_device *acpi_dev;
 	const char *method_hci;
-	struct rfkill *bt_rfk;
 	struct input_dev *hotkey_dev;
 	struct work_struct hotkey_work;
 	struct backlight_device *backlight_dev;
@@ -202,8 +200,6 @@ struct toshiba_acpi_dev {
 	unsigned int panel_power_on_supported:1;
 	unsigned int usb_three_supported:1;
 	unsigned int sysfs_created:1;
-
-	struct mutex mutex;
 };
 
 static struct toshiba_acpi_dev *toshiba_acpi;
@@ -330,13 +326,13 @@ static acpi_status tci_raw(struct toshiba_acpi_dev *dev,
 }
 
 /*
- * Common hci tasks (get or set one or two value)
+ * Common hci tasks
  *
  * In addition to the ACPI status, the HCI system returns a result which
  * may be useful (such as "not supported").
  */
 
-static u32 hci_write1(struct toshiba_acpi_dev *dev, u32 reg, u32 in1)
+static u32 hci_write(struct toshiba_acpi_dev *dev, u32 reg, u32 in1)
 {
 	u32 in[TCI_WORDS] = { HCI_SET, reg, in1, 0, 0, 0 };
 	u32 out[TCI_WORDS];
@@ -345,7 +341,7 @@ static u32 hci_write1(struct toshiba_acpi_dev *dev, u32 reg, u32 in1)
 	return ACPI_SUCCESS(status) ? out[0] : TOS_FAILURE;
 }
 
-static u32 hci_read1(struct toshiba_acpi_dev *dev, u32 reg, u32 *out1)
+static u32 hci_read(struct toshiba_acpi_dev *dev, u32 reg, u32 *out1)
 {
 	u32 in[TCI_WORDS] = { HCI_GET, reg, 0, 0, 0, 0 };
 	u32 out[TCI_WORDS];
@@ -359,31 +355,6 @@ static u32 hci_read1(struct toshiba_acpi_dev *dev, u32 reg, u32 *out1)
 	return out[0];
 }
 
-static u32 hci_write2(struct toshiba_acpi_dev *dev, u32 reg, u32 in1, u32 in2)
-{
-	u32 in[TCI_WORDS] = { HCI_SET, reg, in1, in2, 0, 0 };
-	u32 out[TCI_WORDS];
-	acpi_status status = tci_raw(dev, in, out);
-
-	return ACPI_SUCCESS(status) ? out[0] : TOS_FAILURE;
-}
-
-static u32 hci_read2(struct toshiba_acpi_dev *dev,
-		     u32 reg, u32 *out1, u32 *out2)
-{
-	u32 in[TCI_WORDS] = { HCI_GET, reg, *out1, *out2, 0, 0 };
-	u32 out[TCI_WORDS];
-	acpi_status status = tci_raw(dev, in, out);
-
-	if (ACPI_FAILURE(status))
-		return TOS_FAILURE;
-
-	*out1 = out[2];
-	*out2 = out[3];
-
-	return out[0];
-}
-
 /*
  * Common sci tasks
  */
@@ -395,7 +366,7 @@ static int sci_open(struct toshiba_acpi_dev *dev)
 	acpi_status status;
 
 	status = tci_raw(dev, in, out);
-	if  (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if  (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to open SCI failed\n");
 		return 0;
 	}
@@ -433,7 +404,7 @@ static void sci_close(struct toshiba_acpi_dev *dev)
 	acpi_status status;
 
 	status = tci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to close SCI failed\n");
 		return;
 	}
@@ -481,7 +452,7 @@ static int toshiba_illumination_available(struct toshiba_acpi_dev *dev)
 
 	status = tci_raw(dev, in, out);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to query Illumination support failed\n");
 		return 0;
 	} else if (out[0] == TOS_NOT_SUPPORTED) {
@@ -522,7 +493,7 @@ static enum led_brightness toshiba_illumination_get(struct led_classdev *cdev)
 			struct toshiba_acpi_dev, led_dev);
 	u32 state, result;
 
-	/* First request : initialize communication. */
+	/* First request : initialize communication. */
 	if (!sci_open(dev))
 		return LED_OFF;
 
@@ -625,7 +596,7 @@ static enum led_brightness toshiba_kbd_backlight_get(struct led_classdev *cdev)
 	u32 state, result;
 
 	/* Check the keyboard backlight state */
-	result = hci_read1(dev, HCI_KBD_ILLUMINATION, &state);
+	result = hci_read(dev, HCI_KBD_ILLUMINATION, &state);
 	if (result == TOS_FAILURE || result == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to get the keyboard backlight failed\n");
 		return LED_OFF;
@@ -646,7 +617,7 @@ static void toshiba_kbd_backlight_set(struct led_classdev *cdev,
 
 	/* Set the keyboard backlight state */
 	state = brightness ? 1 : 0;
-	result = hci_write1(dev, HCI_KBD_ILLUMINATION, state);
+	result = hci_write(dev, HCI_KBD_ILLUMINATION, state);
 	if (result == TOS_FAILURE || result == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to set KBD Illumination mode failed\n");
 		return;
@@ -703,7 +674,7 @@ static int toshiba_eco_mode_available(struct toshiba_acpi_dev *dev)
 	u32 out[TCI_WORDS];
 
 	status = tci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to get ECO led failed\n");
 	} else if (out[0] == TOS_NOT_INSTALLED) {
 		pr_info("ECO led not installed");
@@ -825,7 +796,7 @@ static void toshiba_usb_sleep_charge_available(struct toshiba_acpi_dev *dev)
 		return;
 
 	status = tci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to get USB Sleep and Charge mode failed\n");
 		sci_close(dev);
 		return;
@@ -839,7 +810,7 @@ static void toshiba_usb_sleep_charge_available(struct toshiba_acpi_dev *dev)
 
 	in[5] = SCI_USB_CHARGE_BAT_LVL;
 	status = tci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to get USB Sleep and Charge mode failed\n");
 		sci_close(dev);
 		return;
@@ -919,7 +890,7 @@ static int toshiba_sleep_functions_status_get(struct toshiba_acpi_dev *dev,
 	in[5] = SCI_USB_CHARGE_BAT_LVL;
 	status = tci_raw(dev, in, out);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to get USB S&C battery level failed\n");
 		return -EIO;
 	} else if (out[0] == TOS_NOT_SUPPORTED) {
@@ -948,7 +919,7 @@ static int toshiba_sleep_functions_status_set(struct toshiba_acpi_dev *dev,
 	in[5] = SCI_USB_CHARGE_BAT_LVL;
 	status = tci_raw(dev, in, out);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to set USB S&C battery level failed\n");
 		return -EIO;
 	} else if (out[0] == TOS_NOT_SUPPORTED) {
@@ -974,7 +945,7 @@ static int toshiba_usb_rapid_charge_get(struct toshiba_acpi_dev *dev,
 	in[5] = SCI_USB_CHARGE_RAPID_DSP;
 	status = tci_raw(dev, in, out);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to get USB Rapid Charge failed\n");
 		return -EIO;
 	} else if (out[0] == TOS_NOT_SUPPORTED ||
@@ -1002,7 +973,7 @@ static int toshiba_usb_rapid_charge_set(struct toshiba_acpi_dev *dev,
 	in[5] = SCI_USB_CHARGE_RAPID_DSP;
 	status = tci_raw(dev, in, out);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to set USB Rapid Charge failed\n");
 		return -EIO;
 	} else if (out[0] == TOS_NOT_SUPPORTED) {
@@ -1194,121 +1165,31 @@ static int toshiba_usb_three_set(struct toshiba_acpi_dev *dev, u32 state)
 static int toshiba_hotkey_event_type_get(struct toshiba_acpi_dev *dev,
 					 u32 *type)
 {
-	u32 val1 = 0x03;
-	u32 val2 = 0;
-	u32 result;
+	u32 in[TCI_WORDS] = { HCI_GET, HCI_SYSTEM_INFO, 0x03, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status;
 
-	result = hci_read2(dev, HCI_SYSTEM_INFO, &val1, &val2);
-	if (result == TOS_FAILURE) {
+	status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status)) {
 		pr_err("ACPI call to get System type failed\n");
 		return -EIO;
-	} else if (result == TOS_NOT_SUPPORTED) {
+	} else if (out[0] == TOS_NOT_SUPPORTED) {
 		pr_info("System type not supported\n");
 		return -ENODEV;
 	}
 
-	*type = val2;
+	*type = out[3];
 
 	return 0;
 }
 
-/* Bluetooth rfkill handlers */
-
-static u32 hci_get_bt_present(struct toshiba_acpi_dev *dev, bool *present)
-{
-	u32 hci_result;
-	u32 value, value2;
-
-	value = 0;
-	value2 = 0;
-	hci_result = hci_read2(dev, HCI_WIRELESS, &value, &value2);
-	if (hci_result == TOS_SUCCESS)
-		*present = (value & HCI_WIRELESS_BT_PRESENT) ? true : false;
-
-	return hci_result;
-}
-
-static u32 hci_get_radio_state(struct toshiba_acpi_dev *dev, bool *radio_state)
-{
-	u32 hci_result;
-	u32 value, value2;
-
-	value = 0;
-	value2 = 0x0001;
-	hci_result = hci_read2(dev, HCI_WIRELESS, &value, &value2);
-
-	*radio_state = value & HCI_WIRELESS_KILL_SWITCH;
-	return hci_result;
-}
-
-static int bt_rfkill_set_block(void *data, bool blocked)
-{
-	struct toshiba_acpi_dev *dev = data;
-	u32 result1, result2;
-	u32 value;
-	int err;
-	bool radio_state;
-
-	value = (blocked == false);
-
-	mutex_lock(&dev->mutex);
-	if (hci_get_radio_state(dev, &radio_state) != TOS_SUCCESS) {
-		err = -EIO;
-		goto out;
-	}
-
-	if (!radio_state) {
-		err = 0;
-		goto out;
-	}
-
-	result1 = hci_write2(dev, HCI_WIRELESS, value, HCI_WIRELESS_BT_POWER);
-	result2 = hci_write2(dev, HCI_WIRELESS, value, HCI_WIRELESS_BT_ATTACH);
-
-	if (result1 != TOS_SUCCESS || result2 != TOS_SUCCESS)
-		err = -EIO;
-	else
-		err = 0;
- out:
-	mutex_unlock(&dev->mutex);
-	return err;
-}
-
-static void bt_rfkill_poll(struct rfkill *rfkill, void *data)
-{
-	bool new_rfk_state;
-	bool value;
-	u32 hci_result;
-	struct toshiba_acpi_dev *dev = data;
-
-	mutex_lock(&dev->mutex);
-
-	hci_result = hci_get_radio_state(dev, &value);
-	if (hci_result != TOS_SUCCESS) {
-		/* Can't do anything useful */
-		mutex_unlock(&dev->mutex);
-		return;
-	}
-
-	new_rfk_state = value;
-
-	mutex_unlock(&dev->mutex);
-
-	if (rfkill_set_hw_state(rfkill, !new_rfk_state))
-		bt_rfkill_set_block(data, true);
-}
-
-static const struct rfkill_ops toshiba_rfk_ops = {
-	.set_block = bt_rfkill_set_block,
-	.poll = bt_rfkill_poll,
-};
-
+/* Transflective Backlight */
 static int get_tr_backlight_status(struct toshiba_acpi_dev *dev, bool *enabled)
 {
 	u32 hci_result;
 	u32 status;
 
-	hci_result = hci_read1(dev, HCI_TR_BACKLIGHT, &status);
+	hci_result = hci_read(dev, HCI_TR_BACKLIGHT, &status);
 	*enabled = !status;
 	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
@@ -1318,12 +1199,13 @@ static int set_tr_backlight_status(struct toshiba_acpi_dev *dev, bool enable)
 	u32 hci_result;
 	u32 value = !enable;
 
-	hci_result = hci_write1(dev, HCI_TR_BACKLIGHT, value);
+	hci_result = hci_write(dev, HCI_TR_BACKLIGHT, value);
 	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
-static struct proc_dir_entry *toshiba_proc_dir /*= 0*/;
+static struct proc_dir_entry *toshiba_proc_dir;
 
+/* LCD Brightness */
 static int __get_lcd_brightness(struct toshiba_acpi_dev *dev)
 {
 	u32 hci_result;
@@ -1341,7 +1223,7 @@ static int __get_lcd_brightness(struct toshiba_acpi_dev *dev)
 		brightness++;
 	}
 
-	hci_result = hci_read1(dev, HCI_LCD_BRIGHTNESS, &value);
+	hci_result = hci_read(dev, HCI_LCD_BRIGHTNESS, &value);
 	if (hci_result == TOS_SUCCESS)
 		return brightness + (value >> HCI_LCD_BRIGHTNESS_SHIFT);
 
@@ -1396,7 +1278,7 @@ static int set_lcd_brightness(struct toshiba_acpi_dev *dev, int value)
 	}
 
 	value = value << HCI_LCD_BRIGHTNESS_SHIFT;
-	hci_result = hci_write1(dev, HCI_LCD_BRIGHTNESS, value);
+	hci_result = hci_write(dev, HCI_LCD_BRIGHTNESS, value);
 	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
@@ -1446,7 +1328,7 @@ static int get_video_status(struct toshiba_acpi_dev *dev, u32 *status)
 {
 	u32 hci_result;
 
-	hci_result = hci_read1(dev, HCI_VIDEO_OUT, status);
+	hci_result = hci_read(dev, HCI_VIDEO_OUT, status);
 	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
@@ -1531,7 +1413,8 @@ static ssize_t video_proc_write(struct file *file, const char __user *buf,
 			_set_bit(&new_video_out, HCI_VIDEO_OUT_TV, tv_out);
 		/*
 		 * To avoid unnecessary video disruption, only write the new
-		 * video setting if something changed. */
+		 * video setting if something changed.
+		 */
 		if (new_video_out != video_out)
 			ret = write_acpi_int(METHOD_VIDEO_OUT, new_video_out);
 	}
@@ -1552,7 +1435,7 @@ static int get_fan_status(struct toshiba_acpi_dev *dev, u32 *status)
 {
 	u32 hci_result;
 
-	hci_result = hci_read1(dev, HCI_FAN, status);
+	hci_result = hci_read(dev, HCI_FAN, status);
 	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
@@ -1592,7 +1475,7 @@ static ssize_t fan_proc_write(struct file *file, const char __user *buf,
 
 	if (sscanf(cmd, " force_on : %i", &value) == 1 &&
 	    value >= 0 && value <= 1) {
-		hci_result = hci_write1(dev, HCI_FAN, value);
+		hci_result = hci_write(dev, HCI_FAN, value);
 		if (hci_result == TOS_SUCCESS)
 			dev->force_fan = value;
 		else
@@ -1620,7 +1503,7 @@ static int keys_proc_show(struct seq_file *m, void *v)
 	u32 value;
 
 	if (!dev->key_event_valid && dev->system_event_supported) {
-		hci_result = hci_read1(dev, HCI_SYSTEM_EVENT, &value);
+		hci_result = hci_read(dev, HCI_SYSTEM_EVENT, &value);
 		if (hci_result == TOS_SUCCESS) {
 			dev->key_event_valid = 1;
 			dev->last_key_event = value;
@@ -1632,7 +1515,7 @@ static int keys_proc_show(struct seq_file *m, void *v)
 			 * some machines where system events sporadically
 			 * become disabled.
 			 */
-			hci_result = hci_write1(dev, HCI_SYSTEM_EVENT, 1);
+			hci_result = hci_write(dev, HCI_SYSTEM_EVENT, 1);
 			pr_notice("Re-enabled hotkeys\n");
 		} else {
 			pr_err("Error reading hotkey status\n");
@@ -1769,7 +1652,7 @@ static ssize_t fan_store(struct device *dev,
 	if (state != 0 && state != 1)
 		return -EINVAL;
 
-	result = hci_write1(toshiba, HCI_FAN, state);
+	result = hci_write(toshiba, HCI_FAN, state);
 	if (result == TOS_FAILURE)
 		return -EIO;
 	else if (result == TOS_NOT_SUPPORTED)
@@ -2391,7 +2274,7 @@ static int toshiba_acpi_enable_hotkeys(struct toshiba_acpi_dev *dev)
 	if (ACPI_FAILURE(status))
 		return -ENODEV;
 
-	result = hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE);
+	result = hci_write(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE);
 	if (result == TOS_FAILURE)
 		return -EIO;
 	else if (result == TOS_NOT_SUPPORTED)
@@ -2408,8 +2291,8 @@ static void toshiba_acpi_enable_special_functions(struct toshiba_acpi_dev *dev)
 	 * Re-activate the hotkeys, but this time, we are using the
 	 * "Special Functions" mode.
 	 */
-	result = hci_write1(dev, HCI_HOTKEY_EVENT,
-			    HCI_HOTKEY_SPECIAL_FUNCTIONS);
+	result = hci_write(dev, HCI_HOTKEY_EVENT,
+			   HCI_HOTKEY_SPECIAL_FUNCTIONS);
 	if (result != TOS_SUCCESS)
 		pr_err("Could not enable the Special Function mode\n");
 }
@@ -2490,7 +2373,7 @@ static void toshiba_acpi_process_hotkeys(struct toshiba_acpi_dev *dev)
 			toshiba_acpi_report_hotkey(dev, scancode);
 	} else if (dev->system_event_supported) {
 		do {
-			hci_result = hci_read1(dev, HCI_SYSTEM_EVENT, &value);
+			hci_result = hci_read(dev, HCI_SYSTEM_EVENT, &value);
 			switch (hci_result) {
 			case TOS_SUCCESS:
 				toshiba_acpi_report_hotkey(dev, (int)value);
@@ -2502,7 +2385,7 @@ static void toshiba_acpi_process_hotkeys(struct toshiba_acpi_dev *dev)
 				 * sporadically become disabled.
 				 */
 				hci_result =
-					hci_write1(dev, HCI_SYSTEM_EVENT, 1);
+					hci_write(dev, HCI_SYSTEM_EVENT, 1);
 				pr_notice("Re-enabled hotkeys\n");
 				/* Fall through */
 			default:
@@ -2579,7 +2462,7 @@ static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 	if (acpi_has_method(dev->acpi_dev->handle, "INFO"))
 		dev->info_supported = 1;
 	else {
-		hci_result = hci_write1(dev, HCI_SYSTEM_EVENT, 1);
+		hci_result = hci_write(dev, HCI_SYSTEM_EVENT, 1);
 		if (hci_result == TOS_SUCCESS)
 			dev->system_event_supported = 1;
 	}
@@ -2640,14 +2523,11 @@ static int toshiba_acpi_setup_backlight(struct toshiba_acpi_dev *dev)
 	 */
 	if (dev->tr_backlight_supported ||
 	    dmi_check_system(toshiba_vendor_backlight_dmi))
-		acpi_video_dmi_promote_vendor();
+		acpi_video_set_dmi_backlight_type(acpi_backlight_vendor);
 
-	if (acpi_video_backlight_support())
+	if (acpi_video_get_backlight_type() != acpi_backlight_vendor)
 		return 0;
 
-	/* acpi-video may have loaded before we called dmi_promote_vendor() */
-	acpi_video_unregister_backlight();
-
 	memset(&props, 0, sizeof(props));
 	props.type = BACKLIGHT_PLATFORM;
 	props.max_brightness = HCI_LCD_BRIGHTNESS_LEVELS - 1;
@@ -2692,11 +2572,6 @@ static int toshiba_acpi_remove(struct acpi_device *acpi_dev)
 		sparse_keymap_free(dev->hotkey_dev);
 	}
 
-	if (dev->bt_rfk) {
-		rfkill_unregister(dev->bt_rfk);
-		rfkill_destroy(dev->bt_rfk);
-	}
-
 	backlight_device_unregister(dev->backlight_dev);
 
 	if (dev->illumination_supported)
@@ -2733,7 +2608,6 @@ static int toshiba_acpi_add(struct acpi_device *acpi_dev)
 	const char *hci_method;
 	u32 special_functions;
 	u32 dummy;
-	bool bt_present;
 	int ret = 0;
 
 	if (toshiba_acpi)
@@ -2769,33 +2643,10 @@ static int toshiba_acpi_add(struct acpi_device *acpi_dev)
 	if (toshiba_acpi_setup_keyboard(dev))
 		pr_info("Unable to activate hotkeys\n");
 
-	mutex_init(&dev->mutex);
-
 	ret = toshiba_acpi_setup_backlight(dev);
 	if (ret)
 		goto error;
 
-	/* Register rfkill switch for Bluetooth */
-	if (hci_get_bt_present(dev, &bt_present) == TOS_SUCCESS && bt_present) {
-		dev->bt_rfk = rfkill_alloc("Toshiba Bluetooth",
-					   &acpi_dev->dev,
-					   RFKILL_TYPE_BLUETOOTH,
-					   &toshiba_rfk_ops,
-					   dev);
-		if (!dev->bt_rfk) {
-			pr_err("unable to allocate rfkill device\n");
-			ret = -ENOMEM;
-			goto error;
-		}
-
-		ret = rfkill_register(dev->bt_rfk);
-		if (ret) {
-			pr_err("unable to register rfkill device\n");
-			rfkill_destroy(dev->bt_rfk);
-			goto error;
-		}
-	}
-
 	if (toshiba_illumination_available(dev)) {
 		dev->led_dev.name = "toshiba::illumination";
 		dev->led_dev.max_brightness = 1;
@@ -2933,7 +2784,7 @@ static int toshiba_acpi_suspend(struct device *device)
 	u32 result;
 
 	if (dev->hotkey_dev)
-		result = hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_DISABLE);
+		result = hci_write(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_DISABLE);
 
 	return 0;
 }
diff --git a/drivers/platform/x86/toshiba_bluetooth.c b/drivers/platform/x86/toshiba_bluetooth.c
index 249800763362..c5e45089ac51 100644
--- a/drivers/platform/x86/toshiba_bluetooth.c
+++ b/drivers/platform/x86/toshiba_bluetooth.c
@@ -10,12 +10,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * Note the Toshiba Bluetooth RFKill switch seems to be a strange
- * fish. It only provides a BT event when the switch is flipped to
- * the 'on' position. When flipping it to 'off', the USB device is
- * simply pulled away underneath us, without any BT event being
- * delivered.
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -25,6 +19,7 @@
 #include <linux/init.h>
 #include <linux/types.h>
 #include <linux/acpi.h>
+#include <linux/rfkill.h>
 
 #define BT_KILLSWITCH_MASK	0x01
 #define BT_PLUGGED_MASK		0x40
@@ -34,6 +29,15 @@ MODULE_AUTHOR("Jes Sorensen <Jes.Sorensen@gmail.com>");
 MODULE_DESCRIPTION("Toshiba Laptop ACPI Bluetooth Enable Driver");
 MODULE_LICENSE("GPL");
 
+struct toshiba_bluetooth_dev {
+	struct acpi_device *acpi_dev;
+	struct rfkill *rfk;
+
+	bool killswitch;
+	bool plugged;
+	bool powered;
+};
+
 static int toshiba_bt_rfkill_add(struct acpi_device *device);
 static int toshiba_bt_rfkill_remove(struct acpi_device *device);
 static void toshiba_bt_rfkill_notify(struct acpi_device *device, u32 event);
@@ -95,41 +99,12 @@ static int toshiba_bluetooth_status(acpi_handle handle)
 		return -ENXIO;
 	}
 
-	pr_info("Bluetooth status %llu\n", status);
-
 	return status;
 }
 
 static int toshiba_bluetooth_enable(acpi_handle handle)
 {
 	acpi_status result;
-	bool killswitch;
-	bool powered;
-	bool plugged;
-	int status;
-
-	/*
-	 * Query ACPI to verify RFKill switch is set to 'on'.
-	 * If not, we return silently, no need to report it as
-	 * an error.
-	 */
-	status = toshiba_bluetooth_status(handle);
-	if (status < 0)
-		return status;
-
-	killswitch = (status & BT_KILLSWITCH_MASK) ? true : false;
-	powered = (status & BT_POWER_MASK) ? true : false;
-	plugged = (status & BT_PLUGGED_MASK) ? true : false;
-
-	if (!killswitch)
-		return 0;
-	/*
-	 * This check ensures to only enable the device if it is powered
-	 * off or detached, as some recent devices somehow pass the killswitch
-	 * test, causing a loop enabling/disabling the device, see bug 93911.
-	 */
-	if (powered || plugged)
-		return 0;
 
 	result = acpi_evaluate_object(handle, "AUSB", NULL, NULL);
 	if (ACPI_FAILURE(result)) {
@@ -165,20 +140,102 @@ static int toshiba_bluetooth_disable(acpi_handle handle)
 	return 0;
 }
 
+/* Helper function */
+static int toshiba_bluetooth_sync_status(struct toshiba_bluetooth_dev *bt_dev)
+{
+	int status;
+
+	status = toshiba_bluetooth_status(bt_dev->acpi_dev->handle);
+	if (status < 0) {
+		pr_err("Could not sync bluetooth device status\n");
+		return status;
+	}
+
+	bt_dev->killswitch = (status & BT_KILLSWITCH_MASK) ? true : false;
+	bt_dev->plugged = (status & BT_PLUGGED_MASK) ? true : false;
+	bt_dev->powered = (status & BT_POWER_MASK) ? true : false;
+
+	pr_debug("Bluetooth status %d killswitch %d plugged %d powered %d\n",
+		 status, bt_dev->killswitch, bt_dev->plugged, bt_dev->powered);
+
+	return 0;
+}
+
+/* RFKill handlers */
+static int bt_rfkill_set_block(void *data, bool blocked)
+{
+	struct toshiba_bluetooth_dev *bt_dev = data;
+	int ret;
+
+	ret = toshiba_bluetooth_sync_status(bt_dev);
+	if (ret)
+		return ret;
+
+	if (!bt_dev->killswitch)
+		return 0;
+
+	if (blocked)
+		ret = toshiba_bluetooth_disable(bt_dev->acpi_dev->handle);
+	else
+		ret = toshiba_bluetooth_enable(bt_dev->acpi_dev->handle);
+
+	return ret;
+}
+
+static void bt_rfkill_poll(struct rfkill *rfkill, void *data)
+{
+	struct toshiba_bluetooth_dev *bt_dev = data;
+
+	if (toshiba_bluetooth_sync_status(bt_dev))
+		return;
+
+	/*
+	 * Note the Toshiba Bluetooth RFKill switch seems to be a strange
+	 * fish. It only provides a BT event when the switch is flipped to
+	 * the 'on' position. When flipping it to 'off', the USB device is
+	 * simply pulled away underneath us, without any BT event being
+	 * delivered.
+	 */
+	rfkill_set_hw_state(bt_dev->rfk, !bt_dev->killswitch);
+}
+
+static const struct rfkill_ops rfk_ops = {
+	.set_block = bt_rfkill_set_block,
+	.poll = bt_rfkill_poll,
+};
+
+/* ACPI driver functions */
 static void toshiba_bt_rfkill_notify(struct acpi_device *device, u32 event)
 {
-	toshiba_bluetooth_enable(device->handle);
+	struct toshiba_bluetooth_dev *bt_dev = acpi_driver_data(device);
+
+	if (toshiba_bluetooth_sync_status(bt_dev))
+		return;
+
+	rfkill_set_hw_state(bt_dev->rfk, !bt_dev->killswitch);
 }
 
 #ifdef CONFIG_PM_SLEEP
 static int toshiba_bt_resume(struct device *dev)
 {
-	return toshiba_bluetooth_enable(to_acpi_device(dev)->handle);
+	struct toshiba_bluetooth_dev *bt_dev;
+	int ret;
+
+	bt_dev = acpi_driver_data(to_acpi_device(dev));
+
+	ret = toshiba_bluetooth_sync_status(bt_dev);
+	if (ret)
+		return ret;
+
+	rfkill_set_hw_state(bt_dev->rfk, !bt_dev->killswitch);
+
+	return 0;
 }
 #endif
 
 static int toshiba_bt_rfkill_add(struct acpi_device *device)
 {
+	struct toshiba_bluetooth_dev *bt_dev;
 	int result;
 
 	result = toshiba_bluetooth_present(device->handle);
@@ -187,17 +244,54 @@ static int toshiba_bt_rfkill_add(struct acpi_device *device)
 
 	pr_info("Toshiba ACPI Bluetooth device driver\n");
 
-	/* Enable the BT device */
-	result = toshiba_bluetooth_enable(device->handle);
-	if (result)
+	bt_dev = kzalloc(sizeof(*bt_dev), GFP_KERNEL);
+	if (!bt_dev)
+		return -ENOMEM;
+	bt_dev->acpi_dev = device;
+	device->driver_data = bt_dev;
+	dev_set_drvdata(&device->dev, bt_dev);
+
+	result = toshiba_bluetooth_sync_status(bt_dev);
+	if (result) {
+		kfree(bt_dev);
 		return result;
+	}
+
+	bt_dev->rfk = rfkill_alloc("Toshiba Bluetooth",
+				   &device->dev,
+				   RFKILL_TYPE_BLUETOOTH,
+				   &rfk_ops,
+				   bt_dev);
+	if (!bt_dev->rfk) {
+		pr_err("Unable to allocate rfkill device\n");
+		kfree(bt_dev);
+		return -ENOMEM;
+	}
+
+	rfkill_set_hw_state(bt_dev->rfk, !bt_dev->killswitch);
+
+	result = rfkill_register(bt_dev->rfk);
+	if (result) {
+		pr_err("Unable to register rfkill device\n");
+		rfkill_destroy(bt_dev->rfk);
+		kfree(bt_dev);
+	}
 
 	return result;
 }
 
 static int toshiba_bt_rfkill_remove(struct acpi_device *device)
 {
+	struct toshiba_bluetooth_dev *bt_dev = acpi_driver_data(device);
+
 	/* clean up */
+	if (bt_dev->rfk) {
+		rfkill_unregister(bt_dev->rfk);
+		rfkill_destroy(bt_dev->rfk);
+	}
+
+	kfree(bt_dev);
+
 	return toshiba_bluetooth_disable(device->handle);
 }
 
diff --git a/drivers/platform/x86/toshiba_haps.c b/drivers/platform/x86/toshiba_haps.c
index 65300b6a84b9..7f2afc6b5eb9 100644
--- a/drivers/platform/x86/toshiba_haps.c
+++ b/drivers/platform/x86/toshiba_haps.c
@@ -78,15 +78,20 @@ static ssize_t protection_level_store(struct device *dev,
 				      const char *buf, size_t count)
 {
 	struct toshiba_haps_dev *haps = dev_get_drvdata(dev);
-	int level, ret;
-
-	if (sscanf(buf, "%d", &level) != 1 || level < 0 || level > 3)
-		return -EINVAL;
+	int level;
+	int ret;
 
-	/* Set the sensor level.
-	 * Acceptable levels are:
+	ret = kstrtoint(buf, 0, &level);
+	if (ret)
+		return ret;
+	/*
+	 * Check for supported levels, which can be:
 	 * 0 - Disabled | 1 - Low | 2 - Medium | 3 - High
 	 */
+	if (level < 0 || level > 3)
+		return -EINVAL;
+
+	/* Set the sensor level */
 	ret = toshiba_haps_protection_level(haps->acpi_dev->handle, level);
 	if (ret != 0)
 		return ret;
@@ -95,15 +100,21 @@ static ssize_t protection_level_store(struct device *dev,
 
 	return count;
 }
+static DEVICE_ATTR_RW(protection_level);
 
 static ssize_t reset_protection_store(struct device *dev,
 				      struct device_attribute *attr,
 				      const char *buf, size_t count)
 {
 	struct toshiba_haps_dev *haps = dev_get_drvdata(dev);
-	int reset, ret;
+	int reset;
+	int ret;
 
-	if (sscanf(buf, "%d", &reset) != 1 || reset != 1)
+	ret = kstrtoint(buf, 0, &reset);
+	if (ret)
+		return ret;
+	/* The only accepted value is 1 */
+	if (reset != 1)
 		return -EINVAL;
 
 	/* Reset the protection interface */
@@ -113,10 +124,7 @@ static ssize_t reset_protection_store(struct device *dev,
 
 	return count;
 }
-
-static DEVICE_ATTR(protection_level, S_IRUGO | S_IWUSR,
-		   protection_level_show, protection_level_store);
-static DEVICE_ATTR(reset_protection, S_IWUSR, NULL, reset_protection_store);
+static DEVICE_ATTR_WO(reset_protection);
 
 static struct attribute *haps_attributes[] = {
 	&dev_attr_protection_level.attr,
diff --git a/drivers/pnp/pnpacpi/rsparser.c b/drivers/pnp/pnpacpi/rsparser.c
index ff0356fb378f..05796495be0e 100644
--- a/drivers/pnp/pnpacpi/rsparser.c
+++ b/drivers/pnp/pnpacpi/rsparser.c
@@ -28,8 +28,8 @@
 #include "../base.h"
 #include "pnpacpi.h"
 
-static void decode_irq_flags(struct pnp_dev *dev, int flags, int *triggering,
-			     int *polarity, int *shareable)
+static void decode_irq_flags(struct pnp_dev *dev, int flags, u8 *triggering,
+			     u8 *polarity, u8 *shareable)
 {
 	switch (flags & (IORESOURCE_IRQ_LOWLEVEL | IORESOURCE_IRQ_HIGHLEVEL |
 			 IORESOURCE_IRQ_LOWEDGE  | IORESOURCE_IRQ_HIGHEDGE)) {
@@ -654,7 +654,7 @@ static void pnpacpi_encode_irq(struct pnp_dev *dev,
 			       struct resource *p)
 {
 	struct acpi_resource_irq *irq = &resource->data.irq;
-	int triggering, polarity, shareable;
+	u8 triggering, polarity, shareable;
 
 	if (!pnp_resource_enabled(p)) {
 		irq->interrupt_count = 0;
@@ -683,7 +683,7 @@ static void pnpacpi_encode_ext_irq(struct pnp_dev *dev,
 				   struct resource *p)
 {
 	struct acpi_resource_extended_irq *extended_irq = &resource->data.extended_irq;
-	int triggering, polarity, shareable;
+	u8 triggering, polarity, shareable;
 
 	if (!pnp_resource_enabled(p)) {
 		extended_irq->interrupt_count = 0;
@@ -873,7 +873,7 @@ int pnpacpi_encode_resources(struct pnp_dev *dev, struct acpi_buffer *buffer)
 	/* pnpacpi_build_resource_template allocates extra mem */
 	int res_cnt = (buffer->length - 1) / sizeof(struct acpi_resource) - 1;
 	struct acpi_resource *resource = buffer->pointer;
-	int port = 0, irq = 0, dma = 0, mem = 0;
+	unsigned int port = 0, irq = 0, dma = 0, mem = 0;
 
 	pnp_dbg(&dev->dev, "encode %d resources\n", res_cnt);
 	while (i < res_cnt) {
diff --git a/drivers/power/88pm860x_charger.c b/drivers/power/88pm860x_charger.c
index 0e448c68c02b..297e72dc70e6 100644
--- a/drivers/power/88pm860x_charger.c
+++ b/drivers/power/88pm860x_charger.c
@@ -742,7 +742,6 @@ static int pm860x_charger_remove(struct platform_device *pdev)
 	int i;
 
 	power_supply_unregister(info->usb);
-	free_irq(info->irq[0], info);
 	for (i = 0; i < info->irq_nums; i++)
 		free_irq(info->irq[i], info);
 	return 0;
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 4091fb092d06..08beeed5485d 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -204,6 +204,13 @@ config CHARGER_DA9150
 	  This driver can also be built as a module. If so, the module will be
 	  called da9150-charger.
 
+config AXP288_CHARGER
+	tristate "X-Powers AXP288 Charger"
+	depends on MFD_AXP20X && EXTCON_AXP288
+	help
+	  Say yes here to have support X-Power AXP288 power management IC (PMIC)
+	  integrated charger.
+
 config AXP288_FUEL_GAUGE
 	tristate "X-Powers AXP288 Fuel Gauge"
 	depends on MFD_AXP20X && IIO
@@ -388,12 +395,26 @@ config CHARGER_BQ24190
 	help
 	  Say Y to enable support for the TI BQ24190 battery charger.
 
+config CHARGER_BQ24257
+	tristate "TI BQ24257 battery charger driver"
+	depends on I2C && GPIOLIB
+	depends on REGMAP_I2C
+	help
+	  Say Y to enable support for the TI BQ24257 battery charger.
+
 config CHARGER_BQ24735
 	tristate "TI BQ24735 battery charger support"
 	depends on I2C && GPIOLIB
 	help
 	  Say Y to enable support for the TI BQ24735 battery charger.
 
+config CHARGER_BQ25890
+	tristate "TI BQ25890 battery charger driver"
+	depends on I2C && GPIOLIB
+	select REGMAP_I2C
+	help
+	  Say Y to enable support for the TI BQ25890 battery charger.
+
 config CHARGER_SMB347
 	tristate "Summit Microelectronics SMB347 Battery Charger"
 	depends on I2C
@@ -439,6 +460,13 @@ config BATTERY_RT5033
 	  The fuelgauge calculates and determines the battery state of charge
 	  according to battery open circuit voltage.
 
+config CHARGER_RT9455
+	tristate "Richtek RT9455 battery charger driver"
+	depends on I2C && GPIOLIB
+	select REGMAP_I2C
+	help
+	  Say Y to enable support for Richtek RT9455 battery charger.
+
 source "drivers/power/reset/Kconfig"
 
 endif # POWER_SUPPLY
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index b7b0181c95e5..5752ce818f51 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -37,6 +37,7 @@ obj-$(CONFIG_BATTERY_MAX17040)	+= max17040_battery.o
 obj-$(CONFIG_BATTERY_MAX17042)	+= max17042_battery.o
 obj-$(CONFIG_BATTERY_Z2)	+= z2_battery.o
 obj-$(CONFIG_BATTERY_RT5033)	+= rt5033_battery.o
+obj-$(CONFIG_CHARGER_RT9455)	+= rt9455_charger.o
 obj-$(CONFIG_BATTERY_S3C_ADC)	+= s3c_adc_battery.o
 obj-$(CONFIG_BATTERY_TWL4030_MADC)	+= twl4030_madc_battery.o
 obj-$(CONFIG_CHARGER_88PM860X)	+= 88pm860x_charger.o
@@ -58,9 +59,12 @@ obj-$(CONFIG_CHARGER_MAX8997)	+= max8997_charger.o
 obj-$(CONFIG_CHARGER_MAX8998)	+= max8998_charger.o
 obj-$(CONFIG_CHARGER_BQ2415X)	+= bq2415x_charger.o
 obj-$(CONFIG_CHARGER_BQ24190)	+= bq24190_charger.o
+obj-$(CONFIG_CHARGER_BQ24257)	+= bq24257_charger.o
 obj-$(CONFIG_CHARGER_BQ24735)	+= bq24735-charger.o
+obj-$(CONFIG_CHARGER_BQ25890)	+= bq25890_charger.o
 obj-$(CONFIG_POWER_AVS)		+= avs/
 obj-$(CONFIG_CHARGER_SMB347)	+= smb347-charger.o
 obj-$(CONFIG_CHARGER_TPS65090)	+= tps65090-charger.o
 obj-$(CONFIG_POWER_RESET)	+= reset/
 obj-$(CONFIG_AXP288_FUEL_GAUGE) += axp288_fuel_gauge.o
+obj-$(CONFIG_AXP288_CHARGER)	+= axp288_charger.o
diff --git a/drivers/power/axp288_charger.c b/drivers/power/axp288_charger.c
new file mode 100644
index 000000000000..e4d569f57acc
--- /dev/null
+++ b/drivers/power/axp288_charger.c
@@ -0,0 +1,941 @@
+/*
+ * axp288_charger.c - X-power AXP288 PMIC Charger driver
+ *
+ * Copyright (C) 2014 Intel Corporation
+ * Author: Ramakrishna Pallala <ramakrishna.pallala@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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/device.h>
+#include <linux/regmap.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/usb/otg.h>
+#include <linux/notifier.h>
+#include <linux/power_supply.h>
+#include <linux/notifier.h>
+#include <linux/property.h>
+#include <linux/mfd/axp20x.h>
+#include <linux/extcon.h>
+
+#define PS_STAT_VBUS_TRIGGER		(1 << 0)
+#define PS_STAT_BAT_CHRG_DIR		(1 << 2)
+#define PS_STAT_VBAT_ABOVE_VHOLD	(1 << 3)
+#define PS_STAT_VBUS_VALID		(1 << 4)
+#define PS_STAT_VBUS_PRESENT		(1 << 5)
+
+#define CHRG_STAT_BAT_SAFE_MODE		(1 << 3)
+#define CHRG_STAT_BAT_VALID		(1 << 4)
+#define CHRG_STAT_BAT_PRESENT		(1 << 5)
+#define CHRG_STAT_CHARGING		(1 << 6)
+#define CHRG_STAT_PMIC_OTP		(1 << 7)
+
+#define VBUS_ISPOUT_CUR_LIM_MASK	0x03
+#define VBUS_ISPOUT_CUR_LIM_BIT_POS	0
+#define VBUS_ISPOUT_CUR_LIM_900MA	0x0	/* 900mA */
+#define VBUS_ISPOUT_CUR_LIM_1500MA	0x1	/* 1500mA */
+#define VBUS_ISPOUT_CUR_LIM_2000MA	0x2	/* 2000mA */
+#define VBUS_ISPOUT_CUR_NO_LIM		0x3	/* 2500mA */
+#define VBUS_ISPOUT_VHOLD_SET_MASK	0x31
+#define VBUS_ISPOUT_VHOLD_SET_BIT_POS	0x3
+#define VBUS_ISPOUT_VHOLD_SET_OFFSET	4000	/* 4000mV */
+#define VBUS_ISPOUT_VHOLD_SET_LSB_RES	100	/* 100mV */
+#define VBUS_ISPOUT_VHOLD_SET_4300MV	0x3	/* 4300mV */
+#define VBUS_ISPOUT_VBUS_PATH_DIS	(1 << 7)
+
+#define CHRG_CCCV_CC_MASK		0xf		/* 4 bits */
+#define CHRG_CCCV_CC_BIT_POS		0
+#define CHRG_CCCV_CC_OFFSET		200		/* 200mA */
+#define CHRG_CCCV_CC_LSB_RES		200		/* 200mA */
+#define CHRG_CCCV_ITERM_20P		(1 << 4)	/* 20% of CC */
+#define CHRG_CCCV_CV_MASK		0x60		/* 2 bits */
+#define CHRG_CCCV_CV_BIT_POS		5
+#define CHRG_CCCV_CV_4100MV		0x0		/* 4.10V */
+#define CHRG_CCCV_CV_4150MV		0x1		/* 4.15V */
+#define CHRG_CCCV_CV_4200MV		0x2		/* 4.20V */
+#define CHRG_CCCV_CV_4350MV		0x3		/* 4.35V */
+#define CHRG_CCCV_CHG_EN		(1 << 7)
+
+#define CNTL2_CC_TIMEOUT_MASK		0x3	/* 2 bits */
+#define CNTL2_CC_TIMEOUT_OFFSET		6	/* 6 Hrs */
+#define CNTL2_CC_TIMEOUT_LSB_RES	2	/* 2 Hrs */
+#define CNTL2_CC_TIMEOUT_12HRS		0x3	/* 12 Hrs */
+#define CNTL2_CHGLED_TYPEB		(1 << 4)
+#define CNTL2_CHG_OUT_TURNON		(1 << 5)
+#define CNTL2_PC_TIMEOUT_MASK		0xC0
+#define CNTL2_PC_TIMEOUT_OFFSET		40	/* 40 mins */
+#define CNTL2_PC_TIMEOUT_LSB_RES	10	/* 10 mins */
+#define CNTL2_PC_TIMEOUT_70MINS		0x3
+
+#define CHRG_ILIM_TEMP_LOOP_EN		(1 << 3)
+#define CHRG_VBUS_ILIM_MASK		0xf0
+#define CHRG_VBUS_ILIM_BIT_POS		4
+#define CHRG_VBUS_ILIM_100MA		0x0	/* 100mA */
+#define CHRG_VBUS_ILIM_500MA		0x1	/* 500mA */
+#define CHRG_VBUS_ILIM_900MA		0x2	/* 900mA */
+#define CHRG_VBUS_ILIM_1500MA		0x3	/* 1500mA */
+#define CHRG_VBUS_ILIM_2000MA		0x4	/* 2000mA */
+#define CHRG_VBUS_ILIM_2500MA		0x5	/* 2500mA */
+#define CHRG_VBUS_ILIM_3000MA		0x6	/* 3000mA */
+
+#define CHRG_VLTFC_0C			0xA5	/* 0 DegC */
+#define CHRG_VHTFC_45C			0x1F	/* 45 DegC */
+
+#define BAT_IRQ_CFG_CHRG_DONE		(1 << 2)
+#define BAT_IRQ_CFG_CHRG_START		(1 << 3)
+#define BAT_IRQ_CFG_BAT_SAFE_EXIT	(1 << 4)
+#define BAT_IRQ_CFG_BAT_SAFE_ENTER	(1 << 5)
+#define BAT_IRQ_CFG_BAT_DISCON		(1 << 6)
+#define BAT_IRQ_CFG_BAT_CONN		(1 << 7)
+#define BAT_IRQ_CFG_BAT_MASK		0xFC
+
+#define TEMP_IRQ_CFG_QCBTU		(1 << 4)
+#define TEMP_IRQ_CFG_CBTU		(1 << 5)
+#define TEMP_IRQ_CFG_QCBTO		(1 << 6)
+#define TEMP_IRQ_CFG_CBTO		(1 << 7)
+#define TEMP_IRQ_CFG_MASK		0xF0
+
+#define FG_CNTL_OCV_ADJ_EN		(1 << 3)
+
+#define CV_4100MV			4100	/* 4100mV */
+#define CV_4150MV			4150	/* 4150mV */
+#define CV_4200MV			4200	/* 4200mV */
+#define CV_4350MV			4350	/* 4350mV */
+
+#define CC_200MA			200	/*  200mA */
+#define CC_600MA			600	/*  600mA */
+#define CC_800MA			800	/*  800mA */
+#define CC_1000MA			1000	/* 1000mA */
+#define CC_1600MA			1600	/* 1600mA */
+#define CC_2000MA			2000	/* 2000mA */
+
+#define ILIM_100MA			100	/* 100mA */
+#define ILIM_500MA			500	/* 500mA */
+#define ILIM_900MA			900	/* 900mA */
+#define ILIM_1500MA			1500	/* 1500mA */
+#define ILIM_2000MA			2000	/* 2000mA */
+#define ILIM_2500MA			2500	/* 2500mA */
+#define ILIM_3000MA			3000	/* 3000mA */
+
+#define AXP288_EXTCON_DEV_NAME		"axp288_extcon"
+
+#define AXP288_EXTCON_SLOW_CHARGER		"SLOW-CHARGER"
+#define AXP288_EXTCON_DOWNSTREAM_CHARGER	"CHARGE-DOWNSTREAM"
+#define AXP288_EXTCON_FAST_CHARGER		"FAST-CHARGER"
+
+enum {
+	VBUS_OV_IRQ = 0,
+	CHARGE_DONE_IRQ,
+	CHARGE_CHARGING_IRQ,
+	BAT_SAFE_QUIT_IRQ,
+	BAT_SAFE_ENTER_IRQ,
+	QCBTU_IRQ,
+	CBTU_IRQ,
+	QCBTO_IRQ,
+	CBTO_IRQ,
+	CHRG_INTR_END,
+};
+
+struct axp288_chrg_info {
+	struct platform_device *pdev;
+	struct axp20x_chrg_pdata *pdata;
+	struct regmap *regmap;
+	struct regmap_irq_chip_data *regmap_irqc;
+	int irq[CHRG_INTR_END];
+	struct power_supply *psy_usb;
+	struct mutex lock;
+
+	/* OTG/Host mode */
+	struct {
+		struct work_struct work;
+		struct extcon_specific_cable_nb cable;
+		struct notifier_block id_nb;
+		bool id_short;
+	} otg;
+
+	/* SDP/CDP/DCP USB charging cable notifications */
+	struct {
+		struct extcon_dev *edev;
+		bool connected;
+		enum power_supply_type chg_type;
+		struct notifier_block nb;
+		struct work_struct work;
+	} cable;
+
+	int health;
+	int inlmt;
+	int cc;
+	int cv;
+	int max_cc;
+	int max_cv;
+	bool online;
+	bool present;
+	bool enable_charger;
+	bool is_charger_enabled;
+};
+
+static inline int axp288_charger_set_cc(struct axp288_chrg_info *info, int cc)
+{
+	u8 reg_val;
+	int ret;
+
+	if (cc < CHRG_CCCV_CC_OFFSET)
+		cc = CHRG_CCCV_CC_OFFSET;
+	else if (cc > info->max_cc)
+		cc = info->max_cc;
+
+	reg_val = (cc - CHRG_CCCV_CC_OFFSET) / CHRG_CCCV_CC_LSB_RES;
+	cc = (reg_val * CHRG_CCCV_CC_LSB_RES) + CHRG_CCCV_CC_OFFSET;
+	reg_val = reg_val << CHRG_CCCV_CC_BIT_POS;
+
+	ret = regmap_update_bits(info->regmap,
+				AXP20X_CHRG_CTRL1,
+				CHRG_CCCV_CC_MASK, reg_val);
+	if (ret >= 0)
+		info->cc = cc;
+
+	return ret;
+}
+
+static inline int axp288_charger_set_cv(struct axp288_chrg_info *info, int cv)
+{
+	u8 reg_val;
+	int ret;
+
+	if (cv <= CV_4100MV) {
+		reg_val = CHRG_CCCV_CV_4100MV;
+		cv = CV_4100MV;
+	} else if (cv <= CV_4150MV) {
+		reg_val = CHRG_CCCV_CV_4150MV;
+		cv = CV_4150MV;
+	} else if (cv <= CV_4200MV) {
+		reg_val = CHRG_CCCV_CV_4200MV;
+		cv = CV_4200MV;
+	} else {
+		reg_val = CHRG_CCCV_CV_4350MV;
+		cv = CV_4350MV;
+	}
+
+	reg_val = reg_val << CHRG_CCCV_CV_BIT_POS;
+
+	ret = regmap_update_bits(info->regmap,
+				AXP20X_CHRG_CTRL1,
+				CHRG_CCCV_CV_MASK, reg_val);
+
+	if (ret >= 0)
+		info->cv = cv;
+
+	return ret;
+}
+
+static inline int axp288_charger_set_vbus_inlmt(struct axp288_chrg_info *info,
+					   int inlmt)
+{
+	int ret;
+	unsigned int val;
+	u8 reg_val;
+
+	/* Read in limit register */
+	ret = regmap_read(info->regmap, AXP20X_CHRG_BAK_CTRL, &val);
+	if (ret < 0)
+		goto set_inlmt_fail;
+
+	if (inlmt <= ILIM_100MA) {
+		reg_val = CHRG_VBUS_ILIM_100MA;
+		inlmt = ILIM_100MA;
+	} else if (inlmt <= ILIM_500MA) {
+		reg_val = CHRG_VBUS_ILIM_500MA;
+		inlmt = ILIM_500MA;
+	} else if (inlmt <= ILIM_900MA) {
+		reg_val = CHRG_VBUS_ILIM_900MA;
+		inlmt = ILIM_900MA;
+	} else if (inlmt <= ILIM_1500MA) {
+		reg_val = CHRG_VBUS_ILIM_1500MA;
+		inlmt = ILIM_1500MA;
+	} else if (inlmt <= ILIM_2000MA) {
+		reg_val = CHRG_VBUS_ILIM_2000MA;
+		inlmt = ILIM_2000MA;
+	} else if (inlmt <= ILIM_2500MA) {
+		reg_val = CHRG_VBUS_ILIM_2500MA;
+		inlmt = ILIM_2500MA;
+	} else {
+		reg_val = CHRG_VBUS_ILIM_3000MA;
+		inlmt = ILIM_3000MA;
+	}
+
+	reg_val = (val & ~CHRG_VBUS_ILIM_MASK)
+			| (reg_val << CHRG_VBUS_ILIM_BIT_POS);
+	ret = regmap_write(info->regmap, AXP20X_CHRG_BAK_CTRL, reg_val);
+	if (ret >= 0)
+		info->inlmt = inlmt;
+	else
+		dev_err(&info->pdev->dev, "charger BAK control %d\n", ret);
+
+
+set_inlmt_fail:
+	return ret;
+}
+
+static int axp288_charger_vbus_path_select(struct axp288_chrg_info *info,
+								bool enable)
+{
+	int ret;
+
+	if (enable)
+		ret = regmap_update_bits(info->regmap, AXP20X_VBUS_IPSOUT_MGMT,
+					VBUS_ISPOUT_VBUS_PATH_DIS, 0);
+	else
+		ret = regmap_update_bits(info->regmap, AXP20X_VBUS_IPSOUT_MGMT,
+			VBUS_ISPOUT_VBUS_PATH_DIS, VBUS_ISPOUT_VBUS_PATH_DIS);
+
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "axp288 vbus path select %d\n", ret);
+
+
+	return ret;
+}
+
+static int axp288_charger_enable_charger(struct axp288_chrg_info *info,
+								bool enable)
+{
+	int ret;
+
+	if (enable)
+		ret = regmap_update_bits(info->regmap, AXP20X_CHRG_CTRL1,
+				CHRG_CCCV_CHG_EN, CHRG_CCCV_CHG_EN);
+	else
+		ret = regmap_update_bits(info->regmap, AXP20X_CHRG_CTRL1,
+				CHRG_CCCV_CHG_EN, 0);
+	if (ret < 0)
+		dev_err(&info->pdev->dev, "axp288 enable charger %d\n", ret);
+	else
+		info->is_charger_enabled = enable;
+
+	return ret;
+}
+
+static int axp288_charger_is_present(struct axp288_chrg_info *info)
+{
+	int ret, present = 0;
+	unsigned int val;
+
+	ret = regmap_read(info->regmap, AXP20X_PWR_INPUT_STATUS, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val & PS_STAT_VBUS_PRESENT)
+		present = 1;
+	return present;
+}
+
+static int axp288_charger_is_online(struct axp288_chrg_info *info)
+{
+	int ret, online = 0;
+	unsigned int val;
+
+	ret = regmap_read(info->regmap, AXP20X_PWR_INPUT_STATUS, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val & PS_STAT_VBUS_VALID)
+		online = 1;
+	return online;
+}
+
+static int axp288_get_charger_health(struct axp288_chrg_info *info)
+{
+	int ret, pwr_stat, chrg_stat;
+	int health = POWER_SUPPLY_HEALTH_UNKNOWN;
+	unsigned int val;
+
+	ret = regmap_read(info->regmap, AXP20X_PWR_INPUT_STATUS, &val);
+	if ((ret < 0) || !(val & PS_STAT_VBUS_PRESENT))
+		goto health_read_fail;
+	else
+		pwr_stat = val;
+
+	ret = regmap_read(info->regmap, AXP20X_PWR_OP_MODE, &val);
+	if (ret < 0)
+		goto health_read_fail;
+	else
+		chrg_stat = val;
+
+	if (!(pwr_stat & PS_STAT_VBUS_VALID))
+		health = POWER_SUPPLY_HEALTH_DEAD;
+	else if (chrg_stat & CHRG_STAT_PMIC_OTP)
+		health = POWER_SUPPLY_HEALTH_OVERHEAT;
+	else if (chrg_stat & CHRG_STAT_BAT_SAFE_MODE)
+		health = POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE;
+	else
+		health = POWER_SUPPLY_HEALTH_GOOD;
+
+health_read_fail:
+	return health;
+}
+
+static int axp288_charger_usb_set_property(struct power_supply *psy,
+				    enum power_supply_property psp,
+				    const union power_supply_propval *val)
+{
+	struct axp288_chrg_info *info = power_supply_get_drvdata(psy);
+	int ret = 0;
+	int scaled_val;
+
+	mutex_lock(&info->lock);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+		scaled_val = min(val->intval, info->max_cc);
+		scaled_val = DIV_ROUND_CLOSEST(scaled_val, 1000);
+		ret = axp288_charger_set_cc(info, scaled_val);
+		if (ret < 0)
+			dev_warn(&info->pdev->dev, "set charge current failed\n");
+		break;
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		scaled_val = min(val->intval, info->max_cv);
+		scaled_val = DIV_ROUND_CLOSEST(scaled_val, 1000);
+		ret = axp288_charger_set_cv(info, scaled_val);
+		if (ret < 0)
+			dev_warn(&info->pdev->dev, "set charge voltage failed\n");
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int axp288_charger_usb_get_property(struct power_supply *psy,
+				    enum power_supply_property psp,
+				    union power_supply_propval *val)
+{
+	struct axp288_chrg_info *info = power_supply_get_drvdata(psy);
+	int ret = 0;
+
+	mutex_lock(&info->lock);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_PRESENT:
+		/* Check for OTG case first */
+		if (info->otg.id_short) {
+			val->intval = 0;
+			break;
+		}
+		ret = axp288_charger_is_present(info);
+		if (ret < 0)
+			goto psy_get_prop_fail;
+		info->present = ret;
+		val->intval = info->present;
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		/* Check for OTG case first */
+		if (info->otg.id_short) {
+			val->intval = 0;
+			break;
+		}
+		ret = axp288_charger_is_online(info);
+		if (ret < 0)
+			goto psy_get_prop_fail;
+		info->online = ret;
+		val->intval = info->online;
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		val->intval = axp288_get_charger_health(info);
+		break;
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+		val->intval = info->cc * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX:
+		val->intval = info->max_cc * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		val->intval = info->cv * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX:
+		val->intval = info->max_cv * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT:
+		val->intval = info->inlmt * 1000;
+		break;
+	default:
+		ret = -EINVAL;
+		goto psy_get_prop_fail;
+	}
+
+psy_get_prop_fail:
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int axp288_charger_property_is_writeable(struct power_supply *psy,
+		enum power_supply_property psp)
+{
+	int ret;
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		ret = 1;
+		break;
+	default:
+		ret = 0;
+	}
+
+	return ret;
+}
+
+static enum power_supply_property axp288_usb_props[] = {
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_TYPE,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX,
+	POWER_SUPPLY_PROP_CHARGE_CONTROL_LIMIT,
+};
+
+static const struct power_supply_desc axp288_charger_desc = {
+	.name			= "axp288_charger",
+	.type			= POWER_SUPPLY_TYPE_USB,
+	.properties		= axp288_usb_props,
+	.num_properties		= ARRAY_SIZE(axp288_usb_props),
+	.get_property		= axp288_charger_usb_get_property,
+	.set_property		= axp288_charger_usb_set_property,
+	.property_is_writeable	= axp288_charger_property_is_writeable,
+};
+
+static irqreturn_t axp288_charger_irq_thread_handler(int irq, void *dev)
+{
+	struct axp288_chrg_info *info = dev;
+	int i;
+
+	for (i = 0; i < CHRG_INTR_END; i++) {
+		if (info->irq[i] == irq)
+			break;
+	}
+
+	if (i >= CHRG_INTR_END) {
+		dev_warn(&info->pdev->dev, "spurious interrupt!!\n");
+		return IRQ_NONE;
+	}
+
+	switch (i) {
+	case VBUS_OV_IRQ:
+		dev_dbg(&info->pdev->dev, "VBUS Over Voltage INTR\n");
+		break;
+	case CHARGE_DONE_IRQ:
+		dev_dbg(&info->pdev->dev, "Charging Done INTR\n");
+		break;
+	case CHARGE_CHARGING_IRQ:
+		dev_dbg(&info->pdev->dev, "Start Charging IRQ\n");
+		break;
+	case BAT_SAFE_QUIT_IRQ:
+		dev_dbg(&info->pdev->dev,
+			"Quit Safe Mode(restart timer) Charging IRQ\n");
+		break;
+	case BAT_SAFE_ENTER_IRQ:
+		dev_dbg(&info->pdev->dev,
+			"Enter Safe Mode(timer expire) Charging IRQ\n");
+		break;
+	case QCBTU_IRQ:
+		dev_dbg(&info->pdev->dev,
+			"Quit Battery Under Temperature(CHRG) INTR\n");
+		break;
+	case CBTU_IRQ:
+		dev_dbg(&info->pdev->dev,
+			"Hit Battery Under Temperature(CHRG) INTR\n");
+		break;
+	case QCBTO_IRQ:
+		dev_dbg(&info->pdev->dev,
+			"Quit Battery Over Temperature(CHRG) INTR\n");
+		break;
+	case CBTO_IRQ:
+		dev_dbg(&info->pdev->dev,
+			"Hit Battery Over Temperature(CHRG) INTR\n");
+		break;
+	default:
+		dev_warn(&info->pdev->dev, "Spurious Interrupt!!!\n");
+		goto out;
+	}
+
+	power_supply_changed(info->psy_usb);
+out:
+	return IRQ_HANDLED;
+}
+
+static void axp288_charger_extcon_evt_worker(struct work_struct *work)
+{
+	struct axp288_chrg_info *info =
+	    container_of(work, struct axp288_chrg_info, cable.work);
+	int ret, current_limit;
+	bool changed = false;
+	struct extcon_dev *edev = info->cable.edev;
+	bool old_connected = info->cable.connected;
+
+	/* Determine cable/charger type */
+	if (extcon_get_cable_state(edev, AXP288_EXTCON_SLOW_CHARGER) > 0) {
+		dev_dbg(&info->pdev->dev, "USB SDP charger  is connected");
+		info->cable.connected = true;
+		info->cable.chg_type = POWER_SUPPLY_TYPE_USB;
+	} else if (extcon_get_cable_state(edev,
+				AXP288_EXTCON_DOWNSTREAM_CHARGER) > 0) {
+		dev_dbg(&info->pdev->dev, "USB CDP charger is connected");
+		info->cable.connected = true;
+		info->cable.chg_type = POWER_SUPPLY_TYPE_USB_CDP;
+	} else if (extcon_get_cable_state(edev,
+					AXP288_EXTCON_FAST_CHARGER) > 0) {
+		dev_dbg(&info->pdev->dev, "USB DCP charger is connected");
+		info->cable.connected = true;
+		info->cable.chg_type = POWER_SUPPLY_TYPE_USB_DCP;
+	} else {
+		if (old_connected)
+			dev_dbg(&info->pdev->dev, "USB charger disconnected");
+		info->cable.connected = false;
+		info->cable.chg_type = POWER_SUPPLY_TYPE_USB;
+	}
+
+	/* Cable status changed */
+	if (old_connected != info->cable.connected)
+		changed = true;
+
+	if (!changed)
+		return;
+
+	mutex_lock(&info->lock);
+
+	if (info->is_charger_enabled && !info->cable.connected) {
+		info->enable_charger = false;
+		ret = axp288_charger_enable_charger(info, info->enable_charger);
+		if (ret < 0)
+			dev_err(&info->pdev->dev,
+				"cannot disable charger (%d)", ret);
+
+	} else if (!info->is_charger_enabled && info->cable.connected) {
+		switch (info->cable.chg_type) {
+		case POWER_SUPPLY_TYPE_USB:
+			current_limit = ILIM_500MA;
+			break;
+		case POWER_SUPPLY_TYPE_USB_CDP:
+			current_limit = ILIM_1500MA;
+			break;
+		case POWER_SUPPLY_TYPE_USB_DCP:
+			current_limit = ILIM_2000MA;
+			break;
+		default:
+			/* Unknown */
+			current_limit = 0;
+			break;
+		}
+
+		/* Set vbus current limit first, then enable charger */
+		ret = axp288_charger_set_vbus_inlmt(info, current_limit);
+		if (ret < 0) {
+			dev_err(&info->pdev->dev,
+				"error setting current limit (%d)", ret);
+		} else {
+			info->enable_charger = (current_limit > 0);
+			ret = axp288_charger_enable_charger(info,
+							info->enable_charger);
+			if (ret < 0)
+				dev_err(&info->pdev->dev,
+					"cannot enable charger (%d)", ret);
+		}
+	}
+
+	if (changed)
+		info->health = axp288_get_charger_health(info);
+
+	mutex_unlock(&info->lock);
+
+	if (changed)
+		power_supply_changed(info->psy_usb);
+}
+
+static int axp288_charger_handle_cable_evt(struct notifier_block *nb,
+					  unsigned long event, void *param)
+{
+	struct axp288_chrg_info *info =
+	    container_of(nb, struct axp288_chrg_info, cable.nb);
+
+	schedule_work(&info->cable.work);
+
+	return NOTIFY_OK;
+}
+
+static void axp288_charger_otg_evt_worker(struct work_struct *work)
+{
+	struct axp288_chrg_info *info =
+	    container_of(work, struct axp288_chrg_info, otg.work);
+	int ret;
+
+	/* Disable VBUS path before enabling the 5V boost */
+	ret = axp288_charger_vbus_path_select(info, !info->otg.id_short);
+	if (ret < 0)
+		dev_warn(&info->pdev->dev, "vbus path disable failed\n");
+}
+
+static int axp288_charger_handle_otg_evt(struct notifier_block *nb,
+				   unsigned long event, void *param)
+{
+	struct axp288_chrg_info *info =
+	    container_of(nb, struct axp288_chrg_info, otg.id_nb);
+	struct extcon_dev *edev = param;
+	int usb_host = extcon_get_cable_state(edev, "USB-Host");
+
+	dev_dbg(&info->pdev->dev, "external connector USB-Host is %s\n",
+				usb_host ? "attached" : "detached");
+
+	/*
+	 * Set usb_id_short flag to avoid running charger detection logic
+	 * in case usb host.
+	 */
+	info->otg.id_short = usb_host;
+	schedule_work(&info->otg.work);
+
+	return NOTIFY_OK;
+}
+
+static void charger_init_hw_regs(struct axp288_chrg_info *info)
+{
+	int ret, cc, cv;
+	unsigned int val;
+
+	/* Program temperature thresholds */
+	ret = regmap_write(info->regmap, AXP20X_V_LTF_CHRG, CHRG_VLTFC_0C);
+	if (ret < 0)
+		dev_warn(&info->pdev->dev, "register(%x) write error(%d)\n",
+							AXP20X_V_LTF_CHRG, ret);
+
+	ret = regmap_write(info->regmap, AXP20X_V_HTF_CHRG, CHRG_VHTFC_45C);
+	if (ret < 0)
+		dev_warn(&info->pdev->dev, "register(%x) write error(%d)\n",
+							AXP20X_V_HTF_CHRG, ret);
+
+	/* Do not turn-off charger o/p after charge cycle ends */
+	ret = regmap_update_bits(info->regmap,
+				AXP20X_CHRG_CTRL2,
+				CNTL2_CHG_OUT_TURNON, 1);
+	if (ret < 0)
+		dev_warn(&info->pdev->dev, "register(%x) write error(%d)\n",
+						AXP20X_CHRG_CTRL2, ret);
+
+	/* Enable interrupts */
+	ret = regmap_update_bits(info->regmap,
+				AXP20X_IRQ2_EN,
+				BAT_IRQ_CFG_BAT_MASK, 1);
+	if (ret < 0)
+		dev_warn(&info->pdev->dev, "register(%x) write error(%d)\n",
+						AXP20X_IRQ2_EN, ret);
+
+	ret = regmap_update_bits(info->regmap, AXP20X_IRQ3_EN,
+				TEMP_IRQ_CFG_MASK, 1);
+	if (ret < 0)
+		dev_warn(&info->pdev->dev, "register(%x) write error(%d)\n",
+						AXP20X_IRQ3_EN, ret);
+
+	/* Setup ending condition for charging to be 10% of I(chrg) */
+	ret = regmap_update_bits(info->regmap,
+				AXP20X_CHRG_CTRL1,
+				CHRG_CCCV_ITERM_20P, 0);
+	if (ret < 0)
+		dev_warn(&info->pdev->dev, "register(%x) write error(%d)\n",
+						AXP20X_CHRG_CTRL1, ret);
+
+	/* Disable OCV-SOC curve calibration */
+	ret = regmap_update_bits(info->regmap,
+				AXP20X_CC_CTRL,
+				FG_CNTL_OCV_ADJ_EN, 0);
+	if (ret < 0)
+		dev_warn(&info->pdev->dev, "register(%x) write error(%d)\n",
+						AXP20X_CC_CTRL, ret);
+
+	/* Init charging current and voltage */
+	info->max_cc = info->pdata->max_cc;
+	info->max_cv = info->pdata->max_cv;
+
+	/* Read current charge voltage and current limit */
+	ret = regmap_read(info->regmap, AXP20X_CHRG_CTRL1, &val);
+	if (ret < 0) {
+		/* Assume default if cannot read */
+		info->cc = info->pdata->def_cc;
+		info->cv = info->pdata->def_cv;
+	} else {
+		/* Determine charge voltage */
+		cv = (val & CHRG_CCCV_CV_MASK) >> CHRG_CCCV_CV_BIT_POS;
+		switch (cv) {
+		case CHRG_CCCV_CV_4100MV:
+			info->cv = CV_4100MV;
+			break;
+		case CHRG_CCCV_CV_4150MV:
+			info->cv = CV_4150MV;
+			break;
+		case CHRG_CCCV_CV_4200MV:
+			info->cv = CV_4200MV;
+			break;
+		case CHRG_CCCV_CV_4350MV:
+			info->cv = CV_4350MV;
+			break;
+		default:
+			info->cv = INT_MAX;
+			break;
+		}
+
+		/* Determine charge current limit */
+		cc = (ret & CHRG_CCCV_CC_MASK) >> CHRG_CCCV_CC_BIT_POS;
+		cc = (cc * CHRG_CCCV_CC_LSB_RES) + CHRG_CCCV_CC_OFFSET;
+		info->cc = cc;
+
+		/* Program default charging voltage and current */
+		cc = min(info->pdata->def_cc, info->max_cc);
+		cv = min(info->pdata->def_cv, info->max_cv);
+
+		ret = axp288_charger_set_cc(info, cc);
+		if (ret < 0)
+			dev_warn(&info->pdev->dev,
+					"error(%d) in setting CC\n", ret);
+
+		ret = axp288_charger_set_cv(info, cv);
+		if (ret < 0)
+			dev_warn(&info->pdev->dev,
+					"error(%d) in setting CV\n", ret);
+	}
+}
+
+static int axp288_charger_probe(struct platform_device *pdev)
+{
+	int ret, i, pirq;
+	struct axp288_chrg_info *info;
+	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+	struct power_supply_config charger_cfg = {};
+
+	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->pdev = pdev;
+	info->regmap = axp20x->regmap;
+	info->regmap_irqc = axp20x->regmap_irqc;
+	info->pdata = pdev->dev.platform_data;
+
+	if (!info->pdata) {
+		/* Try ACPI provided pdata via device properties */
+		if (!device_property_present(&pdev->dev,
+						"axp288_charger_data\n"))
+			dev_err(&pdev->dev, "failed to get platform data\n");
+		return -ENODEV;
+	}
+
+	info->cable.edev = extcon_get_extcon_dev(AXP288_EXTCON_DEV_NAME);
+	if (info->cable.edev == NULL) {
+		dev_dbg(&pdev->dev, "%s is not ready, probe deferred\n",
+			AXP288_EXTCON_DEV_NAME);
+		return -EPROBE_DEFER;
+	}
+
+	/* Register for extcon notification */
+	INIT_WORK(&info->cable.work, axp288_charger_extcon_evt_worker);
+	info->cable.nb.notifier_call = axp288_charger_handle_cable_evt;
+	ret = extcon_register_notifier(info->cable.edev, EXTCON_NONE, &info->cable.nb);
+	if (ret) {
+		dev_err(&info->pdev->dev,
+			"failed to register extcon notifier %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, info);
+	mutex_init(&info->lock);
+
+	/* Register with power supply class */
+	charger_cfg.drv_data = info;
+	info->psy_usb = power_supply_register(&pdev->dev, &axp288_charger_desc,
+						&charger_cfg);
+	if (IS_ERR(info->psy_usb)) {
+		dev_err(&pdev->dev, "failed to register power supply charger\n");
+		ret = PTR_ERR(info->psy_usb);
+		goto psy_reg_failed;
+	}
+
+	/* Register for OTG notification */
+	INIT_WORK(&info->otg.work, axp288_charger_otg_evt_worker);
+	info->otg.id_nb.notifier_call = axp288_charger_handle_otg_evt;
+	ret = extcon_register_interest(&info->otg.cable, NULL, "USB-Host",
+				       &info->otg.id_nb);
+	if (ret)
+		dev_warn(&pdev->dev, "failed to register otg notifier\n");
+
+	if (info->otg.cable.edev)
+		info->otg.id_short = extcon_get_cable_state(
+					info->otg.cable.edev, "USB-Host");
+
+	/* Register charger interrupts */
+	for (i = 0; i < CHRG_INTR_END; i++) {
+		pirq = platform_get_irq(info->pdev, i);
+		info->irq[i] = regmap_irq_get_virq(info->regmap_irqc, pirq);
+		if (info->irq[i] < 0) {
+			dev_warn(&info->pdev->dev,
+				"failed to get virtual interrupt=%d\n", pirq);
+			ret = info->irq[i];
+			goto intr_reg_failed;
+		}
+		ret = devm_request_threaded_irq(&info->pdev->dev, info->irq[i],
+					NULL, axp288_charger_irq_thread_handler,
+					IRQF_ONESHOT, info->pdev->name, info);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to request interrupt=%d\n",
+								info->irq[i]);
+			goto intr_reg_failed;
+		}
+	}
+
+	charger_init_hw_regs(info);
+
+	return 0;
+
+intr_reg_failed:
+	if (info->otg.cable.edev)
+		extcon_unregister_interest(&info->otg.cable);
+	power_supply_unregister(info->psy_usb);
+psy_reg_failed:
+	extcon_unregister_notifier(info->cable.edev, EXTCON_NONE, &info->cable.nb);
+	return ret;
+}
+
+static int axp288_charger_remove(struct platform_device *pdev)
+{
+	struct axp288_chrg_info *info =  dev_get_drvdata(&pdev->dev);
+
+	if (info->otg.cable.edev)
+		extcon_unregister_interest(&info->otg.cable);
+
+	extcon_unregister_notifier(info->cable.edev, EXTCON_NONE, &info->cable.nb);
+	power_supply_unregister(info->psy_usb);
+
+	return 0;
+}
+
+static struct platform_driver axp288_charger_driver = {
+	.probe = axp288_charger_probe,
+	.remove = axp288_charger_remove,
+	.driver = {
+		.name = "axp288_charger",
+	},
+};
+
+module_platform_driver(axp288_charger_driver);
+
+MODULE_AUTHOR("Ramakrishna Pallala <ramakrishna.pallala@intel.com>");
+MODULE_DESCRIPTION("X-power AXP288 Charger Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/power/axp288_fuel_gauge.c b/drivers/power/axp288_fuel_gauge.c
index bd1dbfee2515..50c0110d6b58 100644
--- a/drivers/power/axp288_fuel_gauge.c
+++ b/drivers/power/axp288_fuel_gauge.c
@@ -1117,7 +1117,7 @@ static int axp288_fuel_gauge_probe(struct platform_device *pdev)
 	return ret;
 }
 
-static struct platform_device_id axp288_fg_id_table[] = {
+static const struct platform_device_id axp288_fg_id_table[] = {
 	{ .name = DEV_NAME },
 	{},
 };
diff --git a/drivers/power/bq2415x_charger.c b/drivers/power/bq2415x_charger.c
index 6c534dcbc19c..e98dcb661cc9 100644
--- a/drivers/power/bq2415x_charger.c
+++ b/drivers/power/bq2415x_charger.c
@@ -35,6 +35,7 @@
 #include <linux/idr.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
+#include <linux/acpi.h>
 
 #include <linux/power/bq2415x_charger.h>
 
@@ -631,7 +632,7 @@ static int bq2415x_set_charge_current(struct bq2415x_device *bq, int mA)
 	int val;
 
 	if (bq->init_data.resistor_sense <= 0)
-		return -ENOSYS;
+		return -EINVAL;
 
 	val = (mA * bq->init_data.resistor_sense - 37400) / 6800;
 	if (val < 0)
@@ -650,7 +651,7 @@ static int bq2415x_get_charge_current(struct bq2415x_device *bq)
 	int ret;
 
 	if (bq->init_data.resistor_sense <= 0)
-		return -ENOSYS;
+		return -EINVAL;
 
 	ret = bq2415x_i2c_read_mask(bq, BQ2415X_REG_CURRENT,
 			BQ2415X_MASK_VI_CHRG, BQ2415X_SHIFT_VI_CHRG);
@@ -665,7 +666,7 @@ static int bq2415x_set_termination_current(struct bq2415x_device *bq, int mA)
 	int val;
 
 	if (bq->init_data.resistor_sense <= 0)
-		return -ENOSYS;
+		return -EINVAL;
 
 	val = (mA * bq->init_data.resistor_sense - 3400) / 3400;
 	if (val < 0)
@@ -684,7 +685,7 @@ static int bq2415x_get_termination_current(struct bq2415x_device *bq)
 	int ret;
 
 	if (bq->init_data.resistor_sense <= 0)
-		return -ENOSYS;
+		return -EINVAL;
 
 	ret = bq2415x_i2c_read_mask(bq, BQ2415X_REG_CURRENT,
 			BQ2415X_MASK_VI_TERM, BQ2415X_SHIFT_VI_TERM);
@@ -1166,7 +1167,7 @@ static ssize_t bq2415x_sysfs_set_mode(struct device *dev,
 
 	if (strncmp(buf, "auto", 4) == 0) {
 		if (bq->automode < 0)
-			return -ENOSYS;
+			return -EINVAL;
 		bq->automode = 1;
 		mode = bq->reported_mode;
 	} else if (strncmp(buf, "off", 3) == 0) {
@@ -1530,13 +1531,14 @@ static int bq2415x_probe(struct i2c_client *client,
 {
 	int ret;
 	int num;
-	char *name;
+	char *name = NULL;
 	struct bq2415x_device *bq;
 	struct device_node *np = client->dev.of_node;
 	struct bq2415x_platform_data *pdata = client->dev.platform_data;
+	const struct acpi_device_id *acpi_id = NULL;
 
-	if (!np && !pdata) {
-		dev_err(&client->dev, "platform data missing\n");
+	if (!np && !pdata && !ACPI_HANDLE(&client->dev)) {
+		dev_err(&client->dev, "Neither devicetree, nor platform data, nor ACPI support\n");
 		return -ENODEV;
 	}
 
@@ -1547,7 +1549,14 @@ static int bq2415x_probe(struct i2c_client *client,
 	if (num < 0)
 		return num;
 
-	name = kasprintf(GFP_KERNEL, "%s-%d", id->name, num);
+	if (id) {
+		name = kasprintf(GFP_KERNEL, "%s-%d", id->name, num);
+	} else if (ACPI_HANDLE(&client->dev)) {
+		acpi_id =
+			acpi_match_device(client->dev.driver->acpi_match_table,
+					  &client->dev);
+		name = kasprintf(GFP_KERNEL, "%s-%d", acpi_id->id, num);
+	}
 	if (!name) {
 		dev_err(&client->dev, "failed to allocate device name\n");
 		ret = -ENOMEM;
@@ -1556,63 +1565,72 @@ static int bq2415x_probe(struct i2c_client *client,
 
 	bq = devm_kzalloc(&client->dev, sizeof(*bq), GFP_KERNEL);
 	if (!bq) {
-		dev_err(&client->dev, "failed to allocate device data\n");
 		ret = -ENOMEM;
 		goto error_2;
 	}
 
 	if (np) {
-		bq->notify_psy = power_supply_get_by_phandle(np, "ti,usb-charger-detection");
+		bq->notify_psy = power_supply_get_by_phandle(np,
+						"ti,usb-charger-detection");
 
 		if (IS_ERR(bq->notify_psy)) {
 			dev_info(&client->dev,
-				"no 'ti,usb-charger-detection' property (err=%ld)\n",
+				 "no 'ti,usb-charger-detection' property (err=%ld)\n",
 				PTR_ERR(bq->notify_psy));
 			bq->notify_psy = NULL;
 		} else if (!bq->notify_psy) {
 			ret = -EPROBE_DEFER;
 			goto error_2;
 		}
-	}
-	else if (pdata->notify_device)
+	} else if (pdata && pdata->notify_device) {
 		bq->notify_psy = power_supply_get_by_name(pdata->notify_device);
-	else
+	} else {
 		bq->notify_psy = NULL;
+	}
 
 	i2c_set_clientdata(client, bq);
 
 	bq->id = num;
 	bq->dev = &client->dev;
-	bq->chip = id->driver_data;
+	if (id)
+		bq->chip = id->driver_data;
+	else if (ACPI_HANDLE(bq->dev))
+		bq->chip = acpi_id->driver_data;
 	bq->name = name;
 	bq->mode = BQ2415X_MODE_OFF;
 	bq->reported_mode = BQ2415X_MODE_OFF;
 	bq->autotimer = 0;
 	bq->automode = 0;
 
-	if (np) {
-		ret = of_property_read_u32(np, "ti,current-limit",
-				&bq->init_data.current_limit);
+	if (np || ACPI_HANDLE(bq->dev)) {
+		ret = device_property_read_u32(bq->dev,
+					       "ti,current-limit",
+					       &bq->init_data.current_limit);
 		if (ret)
 			goto error_3;
-		ret = of_property_read_u32(np, "ti,weak-battery-voltage",
-				&bq->init_data.weak_battery_voltage);
+		ret = device_property_read_u32(bq->dev,
+					"ti,weak-battery-voltage",
+					&bq->init_data.weak_battery_voltage);
 		if (ret)
 			goto error_3;
-		ret = of_property_read_u32(np, "ti,battery-regulation-voltage",
+		ret = device_property_read_u32(bq->dev,
+				"ti,battery-regulation-voltage",
 				&bq->init_data.battery_regulation_voltage);
 		if (ret)
 			goto error_3;
-		ret = of_property_read_u32(np, "ti,charge-current",
-				&bq->init_data.charge_current);
+		ret = device_property_read_u32(bq->dev,
+					       "ti,charge-current",
+					       &bq->init_data.charge_current);
 		if (ret)
 			goto error_3;
-		ret = of_property_read_u32(np, "ti,termination-current",
+		ret = device_property_read_u32(bq->dev,
+				"ti,termination-current",
 				&bq->init_data.termination_current);
 		if (ret)
 			goto error_3;
-		ret = of_property_read_u32(np, "ti,resistor-sense",
-				&bq->init_data.resistor_sense);
+		ret = device_property_read_u32(bq->dev,
+					       "ti,resistor-sense",
+					       &bq->init_data.resistor_sense);
 		if (ret)
 			goto error_3;
 	} else {
@@ -1648,7 +1666,8 @@ static int bq2415x_probe(struct i2c_client *client,
 		}
 
 		/* Query for initial reported_mode and set it */
-		bq2415x_notifier_call(&bq->nb, PSY_EVENT_PROP_CHANGED, bq->notify_psy);
+		bq2415x_notifier_call(&bq->nb, PSY_EVENT_PROP_CHANGED,
+				      bq->notify_psy);
 		bq2415x_set_mode(bq, bq->reported_mode);
 
 		bq->automode = 1;
@@ -1727,9 +1746,28 @@ static const struct i2c_device_id bq2415x_i2c_id_table[] = {
 };
 MODULE_DEVICE_TABLE(i2c, bq2415x_i2c_id_table);
 
+static const struct acpi_device_id bq2415x_i2c_acpi_match[] = {
+	{ "BQ2415X", BQUNKNOWN },
+	{ "BQ241500", BQ24150 },
+	{ "BQA24150", BQ24150A },
+	{ "BQ241510", BQ24151 },
+	{ "BQA24151", BQ24151A },
+	{ "BQ241520", BQ24152 },
+	{ "BQ241530", BQ24153 },
+	{ "BQA24153", BQ24153A },
+	{ "BQ241550", BQ24155 },
+	{ "BQ241560", BQ24156 },
+	{ "BQA24156", BQ24156A },
+	{ "BQS24157", BQ24157S },
+	{ "BQ241580", BQ24158 },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, bq2415x_i2c_acpi_match);
+
 static struct i2c_driver bq2415x_driver = {
 	.driver = {
 		.name = "bq2415x-charger",
+		.acpi_match_table = ACPI_PTR(bq2415x_i2c_acpi_match),
 	},
 	.probe = bq2415x_probe,
 	.remove = bq2415x_remove,
diff --git a/drivers/power/bq24190_charger.c b/drivers/power/bq24190_charger.c
index 407c4af83891..052db78c3736 100644
--- a/drivers/power/bq24190_charger.c
+++ b/drivers/power/bq24190_charger.c
@@ -1258,10 +1258,13 @@ static irqreturn_t bq24190_irq_handler_thread(int irq, void *data)
 	 * register reset so we should ignore that one (the very first
 	 * interrupt received).
 	 */
-	if (alert_userspace && !bdi->first_time) {
-		power_supply_changed(bdi->charger);
-		power_supply_changed(bdi->battery);
-		bdi->first_time = false;
+	if (alert_userspace) {
+		if (!bdi->first_time) {
+			power_supply_changed(bdi->charger);
+			power_supply_changed(bdi->battery);
+		} else {
+			bdi->first_time = false;
+		}
 	}
 
 out:
diff --git a/drivers/power/bq24257_charger.c b/drivers/power/bq24257_charger.c
new file mode 100644
index 000000000000..5859bc7c1616
--- /dev/null
+++ b/drivers/power/bq24257_charger.c
@@ -0,0 +1,858 @@
+/*
+ * TI BQ24257 charger driver
+ *
+ * Copyright (C) 2015 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/power_supply.h>
+#include <linux/regmap.h>
+#include <linux/types.h>
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include <linux/acpi.h>
+#include <linux/of.h>
+
+#define BQ24257_REG_1			0x00
+#define BQ24257_REG_2			0x01
+#define BQ24257_REG_3			0x02
+#define BQ24257_REG_4			0x03
+#define BQ24257_REG_5			0x04
+#define BQ24257_REG_6			0x05
+#define BQ24257_REG_7			0x06
+
+#define BQ24257_MANUFACTURER		"Texas Instruments"
+#define BQ24257_STAT_IRQ		"stat"
+#define BQ24257_PG_GPIO			"pg"
+
+#define BQ24257_ILIM_SET_DELAY		1000	/* msec */
+
+enum bq24257_fields {
+	F_WD_FAULT, F_WD_EN, F_STAT, F_FAULT,			    /* REG 1 */
+	F_RESET, F_IILIMIT, F_EN_STAT, F_EN_TERM, F_CE, F_HZ_MODE,  /* REG 2 */
+	F_VBAT, F_USB_DET,					    /* REG 3 */
+	F_ICHG, F_ITERM,					    /* REG 4 */
+	F_LOOP_STATUS, F_LOW_CHG, F_DPDM_EN, F_CE_STATUS, F_VINDPM, /* REG 5 */
+	F_X2_TMR_EN, F_TMR, F_SYSOFF, F_TS_STAT,		    /* REG 6 */
+	F_VOVP, F_CLR_VDP, F_FORCE_BATDET, F_FORCE_PTM,		    /* REG 7 */
+
+	F_MAX_FIELDS
+};
+
+/* initial field values, converted from uV/uA */
+struct bq24257_init_data {
+	u8 ichg;	/* charge current      */
+	u8 vbat;	/* regulation voltage  */
+	u8 iterm;	/* termination current */
+};
+
+struct bq24257_state {
+	u8 status;
+	u8 fault;
+	bool power_good;
+};
+
+struct bq24257_device {
+	struct i2c_client *client;
+	struct device *dev;
+	struct power_supply *charger;
+
+	struct regmap *rmap;
+	struct regmap_field *rmap_fields[F_MAX_FIELDS];
+
+	struct gpio_desc *pg;
+
+	struct delayed_work iilimit_setup_work;
+
+	struct bq24257_init_data init_data;
+	struct bq24257_state state;
+
+	struct mutex lock; /* protect state data */
+};
+
+static bool bq24257_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case BQ24257_REG_2:
+	case BQ24257_REG_4:
+		return false;
+
+	default:
+		return true;
+	}
+}
+
+static const struct regmap_config bq24257_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = BQ24257_REG_7,
+	.cache_type = REGCACHE_RBTREE,
+
+	.volatile_reg = bq24257_is_volatile_reg,
+};
+
+static const struct reg_field bq24257_reg_fields[] = {
+	/* REG 1 */
+	[F_WD_FAULT]		= REG_FIELD(BQ24257_REG_1, 7, 7),
+	[F_WD_EN]		= REG_FIELD(BQ24257_REG_1, 6, 6),
+	[F_STAT]		= REG_FIELD(BQ24257_REG_1, 4, 5),
+	[F_FAULT]		= REG_FIELD(BQ24257_REG_1, 0, 3),
+	/* REG 2 */
+	[F_RESET]		= REG_FIELD(BQ24257_REG_2, 7, 7),
+	[F_IILIMIT]		= REG_FIELD(BQ24257_REG_2, 4, 6),
+	[F_EN_STAT]		= REG_FIELD(BQ24257_REG_2, 3, 3),
+	[F_EN_TERM]		= REG_FIELD(BQ24257_REG_2, 2, 2),
+	[F_CE]			= REG_FIELD(BQ24257_REG_2, 1, 1),
+	[F_HZ_MODE]		= REG_FIELD(BQ24257_REG_2, 0, 0),
+	/* REG 3 */
+	[F_VBAT]		= REG_FIELD(BQ24257_REG_3, 2, 7),
+	[F_USB_DET]		= REG_FIELD(BQ24257_REG_3, 0, 1),
+	/* REG 4 */
+	[F_ICHG]		= REG_FIELD(BQ24257_REG_4, 3, 7),
+	[F_ITERM]		= REG_FIELD(BQ24257_REG_4, 0, 2),
+	/* REG 5 */
+	[F_LOOP_STATUS]		= REG_FIELD(BQ24257_REG_5, 6, 7),
+	[F_LOW_CHG]		= REG_FIELD(BQ24257_REG_5, 5, 5),
+	[F_DPDM_EN]		= REG_FIELD(BQ24257_REG_5, 4, 4),
+	[F_CE_STATUS]		= REG_FIELD(BQ24257_REG_5, 3, 3),
+	[F_VINDPM]		= REG_FIELD(BQ24257_REG_5, 0, 2),
+	/* REG 6 */
+	[F_X2_TMR_EN]		= REG_FIELD(BQ24257_REG_6, 7, 7),
+	[F_TMR]			= REG_FIELD(BQ24257_REG_6, 5, 6),
+	[F_SYSOFF]		= REG_FIELD(BQ24257_REG_6, 4, 4),
+	[F_TS_STAT]		= REG_FIELD(BQ24257_REG_6, 0, 2),
+	/* REG 7 */
+	[F_VOVP]		= REG_FIELD(BQ24257_REG_7, 5, 7),
+	[F_CLR_VDP]		= REG_FIELD(BQ24257_REG_7, 4, 4),
+	[F_FORCE_BATDET]	= REG_FIELD(BQ24257_REG_7, 3, 3),
+	[F_FORCE_PTM]		= REG_FIELD(BQ24257_REG_7, 2, 2)
+};
+
+static const u32 bq24257_vbat_map[] = {
+	3500000, 3520000, 3540000, 3560000, 3580000, 3600000, 3620000, 3640000,
+	3660000, 3680000, 3700000, 3720000, 3740000, 3760000, 3780000, 3800000,
+	3820000, 3840000, 3860000, 3880000, 3900000, 3920000, 3940000, 3960000,
+	3980000, 4000000, 4020000, 4040000, 4060000, 4080000, 4100000, 4120000,
+	4140000, 4160000, 4180000, 4200000, 4220000, 4240000, 4260000, 4280000,
+	4300000, 4320000, 4340000, 4360000, 4380000, 4400000, 4420000, 4440000
+};
+
+#define BQ24257_VBAT_MAP_SIZE		ARRAY_SIZE(bq24257_vbat_map)
+
+static const u32 bq24257_ichg_map[] = {
+	500000, 550000, 600000, 650000, 700000, 750000, 800000, 850000, 900000,
+	950000, 1000000, 1050000, 1100000, 1150000, 1200000, 1250000, 1300000,
+	1350000, 1400000, 1450000, 1500000, 1550000, 1600000, 1650000, 1700000,
+	1750000, 1800000, 1850000, 1900000, 1950000, 2000000
+};
+
+#define BQ24257_ICHG_MAP_SIZE		ARRAY_SIZE(bq24257_ichg_map)
+
+static const u32 bq24257_iterm_map[] = {
+	50000, 75000, 100000, 125000, 150000, 175000, 200000, 225000
+};
+
+#define BQ24257_ITERM_MAP_SIZE		ARRAY_SIZE(bq24257_iterm_map)
+
+static int bq24257_field_read(struct bq24257_device *bq,
+			      enum bq24257_fields field_id)
+{
+	int ret;
+	int val;
+
+	ret = regmap_field_read(bq->rmap_fields[field_id], &val);
+	if (ret < 0)
+		return ret;
+
+	return val;
+}
+
+static int bq24257_field_write(struct bq24257_device *bq,
+			       enum bq24257_fields field_id, u8 val)
+{
+	return regmap_field_write(bq->rmap_fields[field_id], val);
+}
+
+static u8 bq24257_find_idx(u32 value, const u32 *map, u8 map_size)
+{
+	u8 idx;
+
+	for (idx = 1; idx < map_size; idx++)
+		if (value < map[idx])
+			break;
+
+	return idx - 1;
+}
+
+enum bq24257_status {
+	STATUS_READY,
+	STATUS_CHARGE_IN_PROGRESS,
+	STATUS_CHARGE_DONE,
+	STATUS_FAULT,
+};
+
+enum bq24257_fault {
+	FAULT_NORMAL,
+	FAULT_INPUT_OVP,
+	FAULT_INPUT_UVLO,
+	FAULT_SLEEP,
+	FAULT_BAT_TS,
+	FAULT_BAT_OVP,
+	FAULT_TS,
+	FAULT_TIMER,
+	FAULT_NO_BAT,
+	FAULT_ISET,
+	FAULT_INPUT_LDO_LOW,
+};
+
+static int bq24257_power_supply_get_property(struct power_supply *psy,
+					     enum power_supply_property psp,
+					     union power_supply_propval *val)
+{
+	struct bq24257_device *bq = power_supply_get_drvdata(psy);
+	struct bq24257_state state;
+
+	mutex_lock(&bq->lock);
+	state = bq->state;
+	mutex_unlock(&bq->lock);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		if (!state.power_good)
+			val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+		else if (state.status == STATUS_READY)
+			val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
+		else if (state.status == STATUS_CHARGE_IN_PROGRESS)
+			val->intval = POWER_SUPPLY_STATUS_CHARGING;
+		else if (state.status == STATUS_CHARGE_DONE)
+			val->intval = POWER_SUPPLY_STATUS_FULL;
+		else
+			val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+		break;
+
+	case POWER_SUPPLY_PROP_MANUFACTURER:
+		val->strval = BQ24257_MANUFACTURER;
+		break;
+
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = state.power_good;
+		break;
+
+	case POWER_SUPPLY_PROP_HEALTH:
+		switch (state.fault) {
+		case FAULT_NORMAL:
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+			break;
+
+		case FAULT_INPUT_OVP:
+		case FAULT_BAT_OVP:
+			val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+			break;
+
+		case FAULT_TS:
+		case FAULT_BAT_TS:
+			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+			break;
+
+		case FAULT_TIMER:
+			val->intval = POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE;
+			break;
+
+		default:
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+			break;
+		}
+
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+		val->intval = bq24257_ichg_map[bq->init_data.ichg];
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX:
+		val->intval = bq24257_ichg_map[BQ24257_ICHG_MAP_SIZE - 1];
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		val->intval = bq24257_vbat_map[bq->init_data.vbat];
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX:
+		val->intval = bq24257_vbat_map[BQ24257_VBAT_MAP_SIZE - 1];
+		break;
+
+	case POWER_SUPPLY_PROP_CHARGE_TERM_CURRENT:
+		val->intval = bq24257_iterm_map[bq->init_data.iterm];
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int bq24257_get_chip_state(struct bq24257_device *bq,
+				  struct bq24257_state *state)
+{
+	int ret;
+
+	ret = bq24257_field_read(bq, F_STAT);
+	if (ret < 0)
+		return ret;
+
+	state->status = ret;
+
+	ret = bq24257_field_read(bq, F_FAULT);
+	if (ret < 0)
+		return ret;
+
+	state->fault = ret;
+
+	state->power_good = !gpiod_get_value_cansleep(bq->pg);
+
+	return 0;
+}
+
+static bool bq24257_state_changed(struct bq24257_device *bq,
+				  struct bq24257_state *new_state)
+{
+	int ret;
+
+	mutex_lock(&bq->lock);
+	ret = (bq->state.status != new_state->status ||
+	       bq->state.fault != new_state->fault ||
+	       bq->state.power_good != new_state->power_good);
+	mutex_unlock(&bq->lock);
+
+	return ret;
+}
+
+enum bq24257_loop_status {
+	LOOP_STATUS_NONE,
+	LOOP_STATUS_IN_DPM,
+	LOOP_STATUS_IN_CURRENT_LIMIT,
+	LOOP_STATUS_THERMAL,
+};
+
+enum bq24257_in_ilimit {
+	IILIMIT_100,
+	IILIMIT_150,
+	IILIMIT_500,
+	IILIMIT_900,
+	IILIMIT_1500,
+	IILIMIT_2000,
+	IILIMIT_EXT,
+	IILIMIT_NONE,
+};
+
+enum bq24257_port_type {
+	PORT_TYPE_DCP,		/* Dedicated Charging Port */
+	PORT_TYPE_CDP,		/* Charging Downstream Port */
+	PORT_TYPE_SDP,		/* Standard Downstream Port */
+	PORT_TYPE_NON_STANDARD,
+};
+
+enum bq24257_safety_timer {
+	SAFETY_TIMER_45,
+	SAFETY_TIMER_360,
+	SAFETY_TIMER_540,
+	SAFETY_TIMER_NONE,
+};
+
+static int bq24257_iilimit_autoset(struct bq24257_device *bq)
+{
+	int loop_status;
+	int iilimit;
+	int port_type;
+	int ret;
+	const u8 new_iilimit[] = {
+		[PORT_TYPE_DCP] = IILIMIT_2000,
+		[PORT_TYPE_CDP] = IILIMIT_2000,
+		[PORT_TYPE_SDP] = IILIMIT_500,
+		[PORT_TYPE_NON_STANDARD] = IILIMIT_500
+	};
+
+	ret = bq24257_field_read(bq, F_LOOP_STATUS);
+	if (ret < 0)
+		goto error;
+
+	loop_status = ret;
+
+	ret = bq24257_field_read(bq, F_IILIMIT);
+	if (ret < 0)
+		goto error;
+
+	iilimit = ret;
+
+	/*
+	 * All USB ports should be able to handle 500mA. If not, DPM will lower
+	 * the charging current to accommodate the power source. No need to set
+	 * a lower IILIMIT value.
+	 */
+	if (loop_status == LOOP_STATUS_IN_DPM && iilimit == IILIMIT_500)
+		return 0;
+
+	ret = bq24257_field_read(bq, F_USB_DET);
+	if (ret < 0)
+		goto error;
+
+	port_type = ret;
+
+	ret = bq24257_field_write(bq, F_IILIMIT, new_iilimit[port_type]);
+	if (ret < 0)
+		goto error;
+
+	ret = bq24257_field_write(bq, F_TMR, SAFETY_TIMER_360);
+	if (ret < 0)
+		goto error;
+
+	ret = bq24257_field_write(bq, F_CLR_VDP, 1);
+	if (ret < 0)
+		goto error;
+
+	dev_dbg(bq->dev, "port/loop = %d/%d -> iilimit = %d\n",
+		port_type, loop_status, new_iilimit[port_type]);
+
+	return 0;
+
+error:
+	dev_err(bq->dev, "%s: Error communicating with the chip.\n", __func__);
+	return ret;
+}
+
+static void bq24257_iilimit_setup_work(struct work_struct *work)
+{
+	struct bq24257_device *bq = container_of(work, struct bq24257_device,
+						 iilimit_setup_work.work);
+
+	bq24257_iilimit_autoset(bq);
+}
+
+static void bq24257_handle_state_change(struct bq24257_device *bq,
+					struct bq24257_state *new_state)
+{
+	int ret;
+	struct bq24257_state old_state;
+	bool reset_iilimit = false;
+	bool config_iilimit = false;
+
+	mutex_lock(&bq->lock);
+	old_state = bq->state;
+	mutex_unlock(&bq->lock);
+
+	if (!new_state->power_good) {			     /* power removed */
+		cancel_delayed_work_sync(&bq->iilimit_setup_work);
+
+		/* activate D+/D- port detection algorithm */
+		ret = bq24257_field_write(bq, F_DPDM_EN, 1);
+		if (ret < 0)
+			goto error;
+
+		reset_iilimit = true;
+	} else if (!old_state.power_good) {		    /* power inserted */
+		config_iilimit = true;
+	} else if (new_state->fault == FAULT_NO_BAT) {	   /* battery removed */
+		cancel_delayed_work_sync(&bq->iilimit_setup_work);
+
+		reset_iilimit = true;
+	} else if (old_state.fault == FAULT_NO_BAT) {    /* battery connected */
+		config_iilimit = true;
+	} else if (new_state->fault == FAULT_TIMER) { /* safety timer expired */
+		dev_err(bq->dev, "Safety timer expired! Battery dead?\n");
+	}
+
+	if (reset_iilimit) {
+		ret = bq24257_field_write(bq, F_IILIMIT, IILIMIT_500);
+		if (ret < 0)
+			goto error;
+	} else if (config_iilimit) {
+		schedule_delayed_work(&bq->iilimit_setup_work,
+				      msecs_to_jiffies(BQ24257_ILIM_SET_DELAY));
+	}
+
+	return;
+
+error:
+	dev_err(bq->dev, "%s: Error communicating with the chip.\n", __func__);
+}
+
+static irqreturn_t bq24257_irq_handler_thread(int irq, void *private)
+{
+	int ret;
+	struct bq24257_device *bq = private;
+	struct bq24257_state state;
+
+	ret = bq24257_get_chip_state(bq, &state);
+	if (ret < 0)
+		return IRQ_HANDLED;
+
+	if (!bq24257_state_changed(bq, &state))
+		return IRQ_HANDLED;
+
+	dev_dbg(bq->dev, "irq(state changed): status/fault/pg = %d/%d/%d\n",
+		state.status, state.fault, state.power_good);
+
+	bq24257_handle_state_change(bq, &state);
+
+	mutex_lock(&bq->lock);
+	bq->state = state;
+	mutex_unlock(&bq->lock);
+
+	power_supply_changed(bq->charger);
+
+	return IRQ_HANDLED;
+}
+
+static int bq24257_hw_init(struct bq24257_device *bq)
+{
+	int ret;
+	int i;
+	struct bq24257_state state;
+
+	const struct {
+		int field;
+		u32 value;
+	} init_data[] = {
+		{F_ICHG, bq->init_data.ichg},
+		{F_VBAT, bq->init_data.vbat},
+		{F_ITERM, bq->init_data.iterm}
+	};
+
+	/*
+	 * Disable the watchdog timer to prevent the IC from going back to
+	 * default settings after 50 seconds of I2C inactivity.
+	 */
+	ret = bq24257_field_write(bq, F_WD_EN, 0);
+	if (ret < 0)
+		return ret;
+
+	/* configure the charge currents and voltages */
+	for (i = 0; i < ARRAY_SIZE(init_data); i++) {
+		ret = bq24257_field_write(bq, init_data[i].field,
+					  init_data[i].value);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = bq24257_get_chip_state(bq, &state);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&bq->lock);
+	bq->state = state;
+	mutex_unlock(&bq->lock);
+
+	if (!state.power_good)
+		/* activate D+/D- detection algorithm */
+		ret = bq24257_field_write(bq, F_DPDM_EN, 1);
+	else if (state.fault != FAULT_NO_BAT)
+		ret = bq24257_iilimit_autoset(bq);
+
+	return ret;
+}
+
+static enum power_supply_property bq24257_power_supply_props[] = {
+	POWER_SUPPLY_PROP_MANUFACTURER,
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX,
+	POWER_SUPPLY_PROP_CHARGE_TERM_CURRENT,
+};
+
+static char *bq24257_charger_supplied_to[] = {
+	"main-battery",
+};
+
+static const struct power_supply_desc bq24257_power_supply_desc = {
+	.name = "bq24257-charger",
+	.type = POWER_SUPPLY_TYPE_USB,
+	.properties = bq24257_power_supply_props,
+	.num_properties = ARRAY_SIZE(bq24257_power_supply_props),
+	.get_property = bq24257_power_supply_get_property,
+};
+
+static int bq24257_power_supply_init(struct bq24257_device *bq)
+{
+	struct power_supply_config psy_cfg = { .drv_data = bq, };
+
+	psy_cfg.supplied_to = bq24257_charger_supplied_to;
+	psy_cfg.num_supplicants = ARRAY_SIZE(bq24257_charger_supplied_to);
+
+	bq->charger = power_supply_register(bq->dev, &bq24257_power_supply_desc,
+					    &psy_cfg);
+	if (IS_ERR(bq->charger))
+		return PTR_ERR(bq->charger);
+
+	return 0;
+}
+
+static int bq24257_irq_probe(struct bq24257_device *bq)
+{
+	struct gpio_desc *stat_irq;
+
+	stat_irq = devm_gpiod_get_index(bq->dev, BQ24257_STAT_IRQ, 0, GPIOD_IN);
+	if (IS_ERR(stat_irq)) {
+		dev_err(bq->dev, "could not probe stat_irq pin\n");
+		return PTR_ERR(stat_irq);
+	}
+
+	return gpiod_to_irq(stat_irq);
+}
+
+static int bq24257_pg_gpio_probe(struct bq24257_device *bq)
+{
+	bq->pg = devm_gpiod_get_index(bq->dev, BQ24257_PG_GPIO, 0, GPIOD_IN);
+	if (IS_ERR(bq->pg)) {
+		dev_err(bq->dev, "could not probe PG pin\n");
+		return PTR_ERR(bq->pg);
+	}
+
+	return 0;
+}
+
+static int bq24257_fw_probe(struct bq24257_device *bq)
+{
+	int ret;
+	u32 property;
+
+	ret = device_property_read_u32(bq->dev, "ti,charge-current", &property);
+	if (ret < 0)
+		return ret;
+
+	bq->init_data.ichg = bq24257_find_idx(property, bq24257_ichg_map,
+					      BQ24257_ICHG_MAP_SIZE);
+
+	ret = device_property_read_u32(bq->dev, "ti,battery-regulation-voltage",
+				       &property);
+	if (ret < 0)
+		return ret;
+
+	bq->init_data.vbat = bq24257_find_idx(property, bq24257_vbat_map,
+					      BQ24257_VBAT_MAP_SIZE);
+
+	ret = device_property_read_u32(bq->dev, "ti,termination-current",
+				       &property);
+	if (ret < 0)
+		return ret;
+
+	bq->init_data.iterm = bq24257_find_idx(property, bq24257_iterm_map,
+					       BQ24257_ITERM_MAP_SIZE);
+
+	return 0;
+}
+
+static int bq24257_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct device *dev = &client->dev;
+	struct bq24257_device *bq;
+	int ret;
+	int i;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(dev, "No support for SMBUS_BYTE_DATA\n");
+		return -ENODEV;
+	}
+
+	bq = devm_kzalloc(dev, sizeof(*bq), GFP_KERNEL);
+	if (!bq)
+		return -ENOMEM;
+
+	bq->client = client;
+	bq->dev = dev;
+
+	mutex_init(&bq->lock);
+
+	bq->rmap = devm_regmap_init_i2c(client, &bq24257_regmap_config);
+	if (IS_ERR(bq->rmap)) {
+		dev_err(dev, "failed to allocate register map\n");
+		return PTR_ERR(bq->rmap);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bq24257_reg_fields); i++) {
+		const struct reg_field *reg_fields = bq24257_reg_fields;
+
+		bq->rmap_fields[i] = devm_regmap_field_alloc(dev, bq->rmap,
+							     reg_fields[i]);
+		if (IS_ERR(bq->rmap_fields[i])) {
+			dev_err(dev, "cannot allocate regmap field\n");
+			return PTR_ERR(bq->rmap_fields[i]);
+		}
+	}
+
+	i2c_set_clientdata(client, bq);
+
+	INIT_DELAYED_WORK(&bq->iilimit_setup_work, bq24257_iilimit_setup_work);
+
+	if (!dev->platform_data) {
+		ret = bq24257_fw_probe(bq);
+		if (ret < 0) {
+			dev_err(dev, "Cannot read device properties.\n");
+			return ret;
+		}
+	} else {
+		return -ENODEV;
+	}
+
+	/* we can only check Power Good status by probing the PG pin */
+	ret = bq24257_pg_gpio_probe(bq);
+	if (ret < 0)
+		return ret;
+
+	/* reset all registers to defaults */
+	ret = bq24257_field_write(bq, F_RESET, 1);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Put the RESET bit back to 0, in cache. For some reason the HW always
+	 * returns 1 on this bit, so this is the only way to avoid resetting the
+	 * chip every time we update another field in this register.
+	 */
+	ret = bq24257_field_write(bq, F_RESET, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = bq24257_hw_init(bq);
+	if (ret < 0) {
+		dev_err(dev, "Cannot initialize the chip.\n");
+		return ret;
+	}
+
+	if (client->irq <= 0)
+		client->irq = bq24257_irq_probe(bq);
+
+	if (client->irq < 0) {
+		dev_err(dev, "no irq resource found\n");
+		return client->irq;
+	}
+
+	ret = devm_request_threaded_irq(dev, client->irq, NULL,
+					bq24257_irq_handler_thread,
+					IRQF_TRIGGER_FALLING |
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					BQ24257_STAT_IRQ, bq);
+	if (ret)
+		return ret;
+
+	ret = bq24257_power_supply_init(bq);
+	if (ret < 0)
+		dev_err(dev, "Failed to register power supply\n");
+
+	return ret;
+}
+
+static int bq24257_remove(struct i2c_client *client)
+{
+	struct bq24257_device *bq = i2c_get_clientdata(client);
+
+	cancel_delayed_work_sync(&bq->iilimit_setup_work);
+
+	power_supply_unregister(bq->charger);
+
+	bq24257_field_write(bq, F_RESET, 1); /* reset to defaults */
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int bq24257_suspend(struct device *dev)
+{
+	struct bq24257_device *bq = dev_get_drvdata(dev);
+	int ret = 0;
+
+	cancel_delayed_work_sync(&bq->iilimit_setup_work);
+
+	/* reset all registers to default (and activate standalone mode) */
+	ret = bq24257_field_write(bq, F_RESET, 1);
+	if (ret < 0)
+		dev_err(bq->dev, "Cannot reset chip to standalone mode.\n");
+
+	return ret;
+}
+
+static int bq24257_resume(struct device *dev)
+{
+	int ret;
+	struct bq24257_device *bq = dev_get_drvdata(dev);
+
+	ret = regcache_drop_region(bq->rmap, BQ24257_REG_1, BQ24257_REG_7);
+	if (ret < 0)
+		return ret;
+
+	ret = bq24257_field_write(bq, F_RESET, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = bq24257_hw_init(bq);
+	if (ret < 0) {
+		dev_err(bq->dev, "Cannot init chip after resume.\n");
+		return ret;
+	}
+
+	/* signal userspace, maybe state changed while suspended */
+	power_supply_changed(bq->charger);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops bq24257_pm = {
+	SET_SYSTEM_SLEEP_PM_OPS(bq24257_suspend, bq24257_resume)
+};
+
+static const struct i2c_device_id bq24257_i2c_ids[] = {
+	{ "bq24257", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, bq24257_i2c_ids);
+
+static const struct of_device_id bq24257_of_match[] = {
+	{ .compatible = "ti,bq24257", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, bq24257_of_match);
+
+static const struct acpi_device_id bq24257_acpi_match[] = {
+	{"BQ242570", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, bq24257_acpi_match);
+
+static struct i2c_driver bq24257_driver = {
+	.driver = {
+		.name = "bq24257-charger",
+		.of_match_table = of_match_ptr(bq24257_of_match),
+		.acpi_match_table = ACPI_PTR(bq24257_acpi_match),
+		.pm = &bq24257_pm,
+	},
+	.probe = bq24257_probe,
+	.remove = bq24257_remove,
+	.id_table = bq24257_i2c_ids,
+};
+module_i2c_driver(bq24257_driver);
+
+MODULE_AUTHOR("Laurentiu Palcu <laurentiu.palcu@intel.com>");
+MODULE_DESCRIPTION("bq24257 charger driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/bq25890_charger.c b/drivers/power/bq25890_charger.c
new file mode 100644
index 000000000000..f993a55cde20
--- /dev/null
+++ b/drivers/power/bq25890_charger.c
@@ -0,0 +1,994 @@
+/*
+ * TI BQ25890 charger driver
+ *
+ * Copyright (C) 2015 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/power_supply.h>
+#include <linux/regmap.h>
+#include <linux/types.h>
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/usb/phy.h>
+
+#include <linux/acpi.h>
+#include <linux/of.h>
+
+#define BQ25890_MANUFACTURER		"Texas Instruments"
+#define BQ25890_IRQ_PIN			"bq25890_irq"
+
+#define BQ25890_ID			3
+
+enum bq25890_fields {
+	F_EN_HIZ, F_EN_ILIM, F_IILIM,				     /* Reg00 */
+	F_BHOT, F_BCOLD, F_VINDPM_OFS,				     /* Reg01 */
+	F_CONV_START, F_CONV_RATE, F_BOOSTF, F_ICO_EN,
+	F_HVDCP_EN, F_MAXC_EN, F_FORCE_DPM, F_AUTO_DPDM_EN,	     /* Reg02 */
+	F_BAT_LOAD_EN, F_WD_RST, F_OTG_CFG, F_CHG_CFG, F_SYSVMIN,    /* Reg03 */
+	F_PUMPX_EN, F_ICHG,					     /* Reg04 */
+	F_IPRECHG, F_ITERM,					     /* Reg05 */
+	F_VREG, F_BATLOWV, F_VRECHG,				     /* Reg06 */
+	F_TERM_EN, F_STAT_DIS, F_WD, F_TMR_EN, F_CHG_TMR,
+	F_JEITA_ISET,						     /* Reg07 */
+	F_BATCMP, F_VCLAMP, F_TREG,				     /* Reg08 */
+	F_FORCE_ICO, F_TMR2X_EN, F_BATFET_DIS, F_JEITA_VSET,
+	F_BATFET_DLY, F_BATFET_RST_EN, F_PUMPX_UP, F_PUMPX_DN,	     /* Reg09 */
+	F_BOOSTV, F_BOOSTI,					     /* Reg0A */
+	F_VBUS_STAT, F_CHG_STAT, F_PG_STAT, F_SDP_STAT, F_VSYS_STAT, /* Reg0B */
+	F_WD_FAULT, F_BOOST_FAULT, F_CHG_FAULT, F_BAT_FAULT,
+	F_NTC_FAULT,						     /* Reg0C */
+	F_FORCE_VINDPM, F_VINDPM,				     /* Reg0D */
+	F_THERM_STAT, F_BATV,					     /* Reg0E */
+	F_SYSV,							     /* Reg0F */
+	F_TSPCT,						     /* Reg10 */
+	F_VBUS_GD, F_VBUSV,					     /* Reg11 */
+	F_ICHGR,						     /* Reg12 */
+	F_VDPM_STAT, F_IDPM_STAT, F_IDPM_LIM,			     /* Reg13 */
+	F_REG_RST, F_ICO_OPTIMIZED, F_PN, F_TS_PROFILE, F_DEV_REV,   /* Reg14 */
+
+	F_MAX_FIELDS
+};
+
+/* initial field values, converted to register values */
+struct bq25890_init_data {
+	u8 ichg;	/* charge current		*/
+	u8 vreg;	/* regulation voltage		*/
+	u8 iterm;	/* termination current		*/
+	u8 iprechg;	/* precharge current		*/
+	u8 sysvmin;	/* minimum system voltage limit */
+	u8 boostv;	/* boost regulation voltage	*/
+	u8 boosti;	/* boost current limit		*/
+	u8 boostf;	/* boost frequency		*/
+	u8 ilim_en;	/* enable ILIM pin		*/
+	u8 treg;	/* thermal regulation threshold */
+};
+
+struct bq25890_state {
+	u8 online;
+	u8 chrg_status;
+	u8 chrg_fault;
+	u8 vsys_status;
+	u8 boost_fault;
+	u8 bat_fault;
+};
+
+struct bq25890_device {
+	struct i2c_client *client;
+	struct device *dev;
+	struct power_supply *charger;
+
+	struct usb_phy *usb_phy;
+	struct notifier_block usb_nb;
+	struct work_struct usb_work;
+	unsigned long usb_event;
+
+	struct regmap *rmap;
+	struct regmap_field *rmap_fields[F_MAX_FIELDS];
+
+	int chip_id;
+	struct bq25890_init_data init_data;
+	struct bq25890_state state;
+
+	struct mutex lock; /* protect state data */
+};
+
+static const struct regmap_range bq25890_readonly_reg_ranges[] = {
+	regmap_reg_range(0x0b, 0x0c),
+	regmap_reg_range(0x0e, 0x13),
+};
+
+static const struct regmap_access_table bq25890_writeable_regs = {
+	.no_ranges = bq25890_readonly_reg_ranges,
+	.n_no_ranges = ARRAY_SIZE(bq25890_readonly_reg_ranges),
+};
+
+static const struct regmap_range bq25890_volatile_reg_ranges[] = {
+	regmap_reg_range(0x00, 0x00),
+	regmap_reg_range(0x09, 0x09),
+	regmap_reg_range(0x0b, 0x0c),
+	regmap_reg_range(0x0e, 0x14),
+};
+
+static const struct regmap_access_table bq25890_volatile_regs = {
+	.yes_ranges = bq25890_volatile_reg_ranges,
+	.n_yes_ranges = ARRAY_SIZE(bq25890_volatile_reg_ranges),
+};
+
+static const struct regmap_config bq25890_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = 0x14,
+	.cache_type = REGCACHE_RBTREE,
+
+	.wr_table = &bq25890_writeable_regs,
+	.volatile_table = &bq25890_volatile_regs,
+};
+
+static const struct reg_field bq25890_reg_fields[] = {
+	/* REG00 */
+	[F_EN_HIZ]		= REG_FIELD(0x00, 7, 7),
+	[F_EN_ILIM]		= REG_FIELD(0x00, 6, 6),
+	[F_IILIM]		= REG_FIELD(0x00, 0, 5),
+	/* REG01 */
+	[F_BHOT]		= REG_FIELD(0x01, 6, 7),
+	[F_BCOLD]		= REG_FIELD(0x01, 5, 5),
+	[F_VINDPM_OFS]		= REG_FIELD(0x01, 0, 4),
+	/* REG02 */
+	[F_CONV_START]		= REG_FIELD(0x02, 7, 7),
+	[F_CONV_RATE]		= REG_FIELD(0x02, 6, 6),
+	[F_BOOSTF]		= REG_FIELD(0x02, 5, 5),
+	[F_ICO_EN]		= REG_FIELD(0x02, 4, 4),
+	[F_HVDCP_EN]		= REG_FIELD(0x02, 3, 3),
+	[F_MAXC_EN]		= REG_FIELD(0x02, 2, 2),
+	[F_FORCE_DPM]		= REG_FIELD(0x02, 1, 1),
+	[F_AUTO_DPDM_EN]	= REG_FIELD(0x02, 0, 0),
+	/* REG03 */
+	[F_BAT_LOAD_EN]		= REG_FIELD(0x03, 7, 7),
+	[F_WD_RST]		= REG_FIELD(0x03, 6, 6),
+	[F_OTG_CFG]		= REG_FIELD(0x03, 5, 5),
+	[F_CHG_CFG]		= REG_FIELD(0x03, 4, 4),
+	[F_SYSVMIN]		= REG_FIELD(0x03, 1, 3),
+	/* REG04 */
+	[F_PUMPX_EN]		= REG_FIELD(0x04, 7, 7),
+	[F_ICHG]		= REG_FIELD(0x04, 0, 6),
+	/* REG05 */
+	[F_IPRECHG]		= REG_FIELD(0x05, 4, 7),
+	[F_ITERM]		= REG_FIELD(0x05, 0, 3),
+	/* REG06 */
+	[F_VREG]		= REG_FIELD(0x06, 2, 7),
+	[F_BATLOWV]		= REG_FIELD(0x06, 1, 1),
+	[F_VRECHG]		= REG_FIELD(0x06, 0, 0),
+	/* REG07 */
+	[F_TERM_EN]		= REG_FIELD(0x07, 7, 7),
+	[F_STAT_DIS]		= REG_FIELD(0x07, 6, 6),
+	[F_WD]			= REG_FIELD(0x07, 4, 5),
+	[F_TMR_EN]		= REG_FIELD(0x07, 3, 3),
+	[F_CHG_TMR]		= REG_FIELD(0x07, 1, 2),
+	[F_JEITA_ISET]		= REG_FIELD(0x07, 0, 0),
+	/* REG08 */
+	[F_BATCMP]		= REG_FIELD(0x08, 6, 7),
+	[F_VCLAMP]		= REG_FIELD(0x08, 2, 4),
+	[F_TREG]		= REG_FIELD(0x08, 0, 1),
+	/* REG09 */
+	[F_FORCE_ICO]		= REG_FIELD(0x09, 7, 7),
+	[F_TMR2X_EN]		= REG_FIELD(0x09, 6, 6),
+	[F_BATFET_DIS]		= REG_FIELD(0x09, 5, 5),
+	[F_JEITA_VSET]		= REG_FIELD(0x09, 4, 4),
+	[F_BATFET_DLY]		= REG_FIELD(0x09, 3, 3),
+	[F_BATFET_RST_EN]	= REG_FIELD(0x09, 2, 2),
+	[F_PUMPX_UP]		= REG_FIELD(0x09, 1, 1),
+	[F_PUMPX_DN]		= REG_FIELD(0x09, 0, 0),
+	/* REG0A */
+	[F_BOOSTV]		= REG_FIELD(0x0A, 4, 7),
+	[F_BOOSTI]		= REG_FIELD(0x0A, 0, 2),
+	/* REG0B */
+	[F_VBUS_STAT]		= REG_FIELD(0x0B, 5, 7),
+	[F_CHG_STAT]		= REG_FIELD(0x0B, 3, 4),
+	[F_PG_STAT]		= REG_FIELD(0x0B, 2, 2),
+	[F_SDP_STAT]		= REG_FIELD(0x0B, 1, 1),
+	[F_VSYS_STAT]		= REG_FIELD(0x0B, 0, 0),
+	/* REG0C */
+	[F_WD_FAULT]		= REG_FIELD(0x0C, 7, 7),
+	[F_BOOST_FAULT]		= REG_FIELD(0x0C, 6, 6),
+	[F_CHG_FAULT]		= REG_FIELD(0x0C, 4, 5),
+	[F_BAT_FAULT]		= REG_FIELD(0x0C, 3, 3),
+	[F_NTC_FAULT]		= REG_FIELD(0x0C, 0, 2),
+	/* REG0D */
+	[F_FORCE_VINDPM]	= REG_FIELD(0x0D, 7, 7),
+	[F_VINDPM]		= REG_FIELD(0x0D, 0, 6),
+	/* REG0E */
+	[F_THERM_STAT]		= REG_FIELD(0x0E, 7, 7),
+	[F_BATV]		= REG_FIELD(0x0E, 0, 6),
+	/* REG0F */
+	[F_SYSV]		= REG_FIELD(0x0F, 0, 6),
+	/* REG10 */
+	[F_TSPCT]		= REG_FIELD(0x10, 0, 6),
+	/* REG11 */
+	[F_VBUS_GD]		= REG_FIELD(0x11, 7, 7),
+	[F_VBUSV]		= REG_FIELD(0x11, 0, 6),
+	/* REG12 */
+	[F_ICHGR]		= REG_FIELD(0x12, 0, 6),
+	/* REG13 */
+	[F_VDPM_STAT]		= REG_FIELD(0x13, 7, 7),
+	[F_IDPM_STAT]		= REG_FIELD(0x13, 6, 6),
+	[F_IDPM_LIM]		= REG_FIELD(0x13, 0, 5),
+	/* REG14 */
+	[F_REG_RST]		= REG_FIELD(0x14, 7, 7),
+	[F_ICO_OPTIMIZED]	= REG_FIELD(0x14, 6, 6),
+	[F_PN]			= REG_FIELD(0x14, 3, 5),
+	[F_TS_PROFILE]		= REG_FIELD(0x14, 2, 2),
+	[F_DEV_REV]		= REG_FIELD(0x14, 0, 1)
+};
+
+/*
+ * Most of the val -> idx conversions can be computed, given the minimum,
+ * maximum and the step between values. For the rest of conversions, we use
+ * lookup tables.
+ */
+enum bq25890_table_ids {
+	/* range tables */
+	TBL_ICHG,
+	TBL_ITERM,
+	TBL_IPRECHG,
+	TBL_VREG,
+	TBL_BATCMP,
+	TBL_VCLAMP,
+	TBL_BOOSTV,
+	TBL_SYSVMIN,
+
+	/* lookup tables */
+	TBL_TREG,
+	TBL_BOOSTI,
+};
+
+/* Thermal Regulation Threshold lookup table, in degrees Celsius */
+static const u32 bq25890_treg_tbl[] = { 60, 80, 100, 120 };
+
+#define BQ25890_TREG_TBL_SIZE		ARRAY_SIZE(bq25890_treg_tbl)
+
+/* Boost mode current limit lookup table, in uA */
+static const u32 bq25890_boosti_tbl[] = {
+	500000, 700000, 1100000, 1300000, 1600000, 1800000, 2100000, 2400000
+};
+
+#define BQ25890_BOOSTI_TBL_SIZE		ARRAY_SIZE(bq25890_boosti_tbl)
+
+struct bq25890_range {
+	u32 min;
+	u32 max;
+	u32 step;
+};
+
+struct bq25890_lookup {
+	const u32 *tbl;
+	u32 size;
+};
+
+static const union {
+	struct bq25890_range  rt;
+	struct bq25890_lookup lt;
+} bq25890_tables[] = {
+	/* range tables */
+	[TBL_ICHG] =	{ .rt = {0,	  5056000, 64000} },	 /* uA */
+	[TBL_ITERM] =	{ .rt = {64000,   1024000, 64000} },	 /* uA */
+	[TBL_VREG] =	{ .rt = {3840000, 4608000, 16000} },	 /* uV */
+	[TBL_BATCMP] =	{ .rt = {0,	  140,     20} },	 /* mOhm */
+	[TBL_VCLAMP] =	{ .rt = {0,	  224000,  32000} },	 /* uV */
+	[TBL_BOOSTV] =	{ .rt = {4550000, 5510000, 64000} },	 /* uV */
+	[TBL_SYSVMIN] = { .rt = {3000000, 3700000, 100000} },	 /* uV */
+
+	/* lookup tables */
+	[TBL_TREG] =	{ .lt = {bq25890_treg_tbl, BQ25890_TREG_TBL_SIZE} },
+	[TBL_BOOSTI] =	{ .lt = {bq25890_boosti_tbl, BQ25890_BOOSTI_TBL_SIZE} }
+};
+
+static int bq25890_field_read(struct bq25890_device *bq,
+			      enum bq25890_fields field_id)
+{
+	int ret;
+	int val;
+
+	ret = regmap_field_read(bq->rmap_fields[field_id], &val);
+	if (ret < 0)
+		return ret;
+
+	return val;
+}
+
+static int bq25890_field_write(struct bq25890_device *bq,
+			       enum bq25890_fields field_id, u8 val)
+{
+	return regmap_field_write(bq->rmap_fields[field_id], val);
+}
+
+static u8 bq25890_find_idx(u32 value, enum bq25890_table_ids id)
+{
+	u8 idx;
+
+	if (id >= TBL_TREG) {
+		const u32 *tbl = bq25890_tables[id].lt.tbl;
+		u32 tbl_size = bq25890_tables[id].lt.size;
+
+		for (idx = 1; idx < tbl_size && tbl[idx] <= value; idx++)
+			;
+	} else {
+		const struct bq25890_range *rtbl = &bq25890_tables[id].rt;
+		u8 rtbl_size;
+
+		rtbl_size = (rtbl->max - rtbl->min) / rtbl->step + 1;
+
+		for (idx = 1;
+		     idx < rtbl_size && (idx * rtbl->step + rtbl->min <= value);
+		     idx++)
+			;
+	}
+
+	return idx - 1;
+}
+
+static u32 bq25890_find_val(u8 idx, enum bq25890_table_ids id)
+{
+	const struct bq25890_range *rtbl;
+
+	/* lookup table? */
+	if (id >= TBL_TREG)
+		return bq25890_tables[id].lt.tbl[idx];
+
+	/* range table */
+	rtbl = &bq25890_tables[id].rt;
+
+	return (rtbl->min + idx * rtbl->step);
+}
+
+enum bq25890_status {
+	STATUS_NOT_CHARGING,
+	STATUS_PRE_CHARGING,
+	STATUS_FAST_CHARGING,
+	STATUS_TERMINATION_DONE,
+};
+
+enum bq25890_chrg_fault {
+	CHRG_FAULT_NORMAL,
+	CHRG_FAULT_INPUT,
+	CHRG_FAULT_THERMAL_SHUTDOWN,
+	CHRG_FAULT_TIMER_EXPIRED,
+};
+
+static int bq25890_power_supply_get_property(struct power_supply *psy,
+					     enum power_supply_property psp,
+					     union power_supply_propval *val)
+{
+	int ret;
+	struct bq25890_device *bq = power_supply_get_drvdata(psy);
+	struct bq25890_state state;
+
+	mutex_lock(&bq->lock);
+	state = bq->state;
+	mutex_unlock(&bq->lock);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		if (!state.online)
+			val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+		else if (state.chrg_status == STATUS_NOT_CHARGING)
+			val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
+		else if (state.chrg_status == STATUS_PRE_CHARGING ||
+			 state.chrg_status == STATUS_FAST_CHARGING)
+			val->intval = POWER_SUPPLY_STATUS_CHARGING;
+		else if (state.chrg_status == STATUS_TERMINATION_DONE)
+			val->intval = POWER_SUPPLY_STATUS_FULL;
+		else
+			val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+
+		break;
+
+	case POWER_SUPPLY_PROP_MANUFACTURER:
+		val->strval = BQ25890_MANUFACTURER;
+		break;
+
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = state.online;
+		break;
+
+	case POWER_SUPPLY_PROP_HEALTH:
+		if (!state.chrg_fault && !state.bat_fault && !state.boost_fault)
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		else if (state.bat_fault)
+			val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		else if (state.chrg_fault == CHRG_FAULT_TIMER_EXPIRED)
+			val->intval = POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE;
+		else if (state.chrg_fault == CHRG_FAULT_THERMAL_SHUTDOWN)
+			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		else
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+		ret = bq25890_field_read(bq, F_ICHGR); /* read measured value */
+		if (ret < 0)
+			return ret;
+
+		/* converted_val = ADC_val * 50mA (table 10.3.19) */
+		val->intval = ret * 50000;
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX:
+		val->intval = bq25890_tables[TBL_ICHG].rt.max;
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		if (!state.online) {
+			val->intval = 0;
+			break;
+		}
+
+		ret = bq25890_field_read(bq, F_BATV); /* read measured value */
+		if (ret < 0)
+			return ret;
+
+		/* converted_val = 2.304V + ADC_val * 20mV (table 10.3.15) */
+		val->intval = 2304000 + ret * 20000;
+		break;
+
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX:
+		val->intval = bq25890_tables[TBL_VREG].rt.max;
+		break;
+
+	case POWER_SUPPLY_PROP_CHARGE_TERM_CURRENT:
+		val->intval = bq25890_find_val(bq->init_data.iterm, TBL_ITERM);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int bq25890_get_chip_state(struct bq25890_device *bq,
+				  struct bq25890_state *state)
+{
+	int i, ret;
+
+	struct {
+		enum bq25890_fields id;
+		u8 *data;
+	} state_fields[] = {
+		{F_CHG_STAT,	&state->chrg_status},
+		{F_PG_STAT,	&state->online},
+		{F_VSYS_STAT,	&state->vsys_status},
+		{F_BOOST_FAULT, &state->boost_fault},
+		{F_BAT_FAULT,	&state->bat_fault},
+		{F_CHG_FAULT,	&state->chrg_fault}
+	};
+
+	for (i = 0; i < ARRAY_SIZE(state_fields); i++) {
+		ret = bq25890_field_read(bq, state_fields[i].id);
+		if (ret < 0)
+			return ret;
+
+		*state_fields[i].data = ret;
+	}
+
+	dev_dbg(bq->dev, "S:CHG/PG/VSYS=%d/%d/%d, F:CHG/BOOST/BAT=%d/%d/%d\n",
+		state->chrg_status, state->online, state->vsys_status,
+		state->chrg_fault, state->boost_fault, state->bat_fault);
+
+	return 0;
+}
+
+static bool bq25890_state_changed(struct bq25890_device *bq,
+				  struct bq25890_state *new_state)
+{
+	struct bq25890_state old_state;
+
+	mutex_lock(&bq->lock);
+	old_state = bq->state;
+	mutex_unlock(&bq->lock);
+
+	return (old_state.chrg_status != new_state->chrg_status ||
+		old_state.chrg_fault != new_state->chrg_fault	||
+		old_state.online != new_state->online		||
+		old_state.bat_fault != new_state->bat_fault	||
+		old_state.boost_fault != new_state->boost_fault ||
+		old_state.vsys_status != new_state->vsys_status);
+}
+
+static void bq25890_handle_state_change(struct bq25890_device *bq,
+					struct bq25890_state *new_state)
+{
+	int ret;
+	struct bq25890_state old_state;
+
+	mutex_lock(&bq->lock);
+	old_state = bq->state;
+	mutex_unlock(&bq->lock);
+
+	if (!new_state->online) {			     /* power removed */
+		/* disable ADC */
+		ret = bq25890_field_write(bq, F_CONV_START, 0);
+		if (ret < 0)
+			goto error;
+	} else if (!old_state.online) {			    /* power inserted */
+		/* enable ADC, to have control of charge current/voltage */
+		ret = bq25890_field_write(bq, F_CONV_START, 1);
+		if (ret < 0)
+			goto error;
+	}
+
+	return;
+
+error:
+	dev_err(bq->dev, "Error communicating with the chip.\n");
+}
+
+static irqreturn_t bq25890_irq_handler_thread(int irq, void *private)
+{
+	struct bq25890_device *bq = private;
+	int ret;
+	struct bq25890_state state;
+
+	ret = bq25890_get_chip_state(bq, &state);
+	if (ret < 0)
+		goto handled;
+
+	if (!bq25890_state_changed(bq, &state))
+		goto handled;
+
+	bq25890_handle_state_change(bq, &state);
+
+	mutex_lock(&bq->lock);
+	bq->state = state;
+	mutex_unlock(&bq->lock);
+
+	power_supply_changed(bq->charger);
+
+handled:
+	return IRQ_HANDLED;
+}
+
+static int bq25890_chip_reset(struct bq25890_device *bq)
+{
+	int ret;
+	int rst_check_counter = 10;
+
+	ret = bq25890_field_write(bq, F_REG_RST, 1);
+	if (ret < 0)
+		return ret;
+
+	do {
+		ret = bq25890_field_read(bq, F_REG_RST);
+		if (ret < 0)
+			return ret;
+
+		usleep_range(5, 10);
+	} while (ret == 1 && --rst_check_counter);
+
+	if (!rst_check_counter)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int bq25890_hw_init(struct bq25890_device *bq)
+{
+	int ret;
+	int i;
+	struct bq25890_state state;
+
+	const struct {
+		enum bq25890_fields id;
+		u32 value;
+	} init_data[] = {
+		{F_ICHG,	 bq->init_data.ichg},
+		{F_VREG,	 bq->init_data.vreg},
+		{F_ITERM,	 bq->init_data.iterm},
+		{F_IPRECHG,	 bq->init_data.iprechg},
+		{F_SYSVMIN,	 bq->init_data.sysvmin},
+		{F_BOOSTV,	 bq->init_data.boostv},
+		{F_BOOSTI,	 bq->init_data.boosti},
+		{F_BOOSTF,	 bq->init_data.boostf},
+		{F_EN_ILIM,	 bq->init_data.ilim_en},
+		{F_TREG,	 bq->init_data.treg}
+	};
+
+	ret = bq25890_chip_reset(bq);
+	if (ret < 0)
+		return ret;
+
+	/* disable watchdog */
+	ret = bq25890_field_write(bq, F_WD, 0);
+	if (ret < 0)
+		return ret;
+
+	/* initialize currents/voltages and other parameters */
+	for (i = 0; i < ARRAY_SIZE(init_data); i++) {
+		ret = bq25890_field_write(bq, init_data[i].id,
+					  init_data[i].value);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Configure ADC for continuous conversions. This does not enable it. */
+	ret = bq25890_field_write(bq, F_CONV_RATE, 1);
+	if (ret < 0)
+		return ret;
+
+	ret = bq25890_get_chip_state(bq, &state);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&bq->lock);
+	bq->state = state;
+	mutex_unlock(&bq->lock);
+
+	return 0;
+}
+
+static enum power_supply_property bq25890_power_supply_props[] = {
+	POWER_SUPPLY_PROP_MANUFACTURER,
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX,
+	POWER_SUPPLY_PROP_CHARGE_TERM_CURRENT,
+};
+
+static char *bq25890_charger_supplied_to[] = {
+	"main-battery",
+};
+
+static const struct power_supply_desc bq25890_power_supply_desc = {
+	.name = "bq25890-charger",
+	.type = POWER_SUPPLY_TYPE_USB,
+	.properties = bq25890_power_supply_props,
+	.num_properties = ARRAY_SIZE(bq25890_power_supply_props),
+	.get_property = bq25890_power_supply_get_property,
+};
+
+static int bq25890_power_supply_init(struct bq25890_device *bq)
+{
+	struct power_supply_config psy_cfg = { .drv_data = bq, };
+
+	psy_cfg.supplied_to = bq25890_charger_supplied_to;
+	psy_cfg.num_supplicants = ARRAY_SIZE(bq25890_charger_supplied_to);
+
+	bq->charger = power_supply_register(bq->dev, &bq25890_power_supply_desc,
+					    &psy_cfg);
+
+	return PTR_ERR_OR_ZERO(bq->charger);
+}
+
+static void bq25890_usb_work(struct work_struct *data)
+{
+	int ret;
+	struct bq25890_device *bq =
+			container_of(data, struct bq25890_device, usb_work);
+
+	switch (bq->usb_event) {
+	case USB_EVENT_ID:
+		/* Enable boost mode */
+		ret = bq25890_field_write(bq, F_OTG_CFG, 1);
+		if (ret < 0)
+			goto error;
+		break;
+
+	case USB_EVENT_NONE:
+		/* Disable boost mode */
+		ret = bq25890_field_write(bq, F_OTG_CFG, 0);
+		if (ret < 0)
+			goto error;
+
+		power_supply_changed(bq->charger);
+		break;
+	}
+
+	return;
+
+error:
+	dev_err(bq->dev, "Error switching to boost/charger mode.\n");
+}
+
+static int bq25890_usb_notifier(struct notifier_block *nb, unsigned long val,
+				void *priv)
+{
+	struct bq25890_device *bq =
+			container_of(nb, struct bq25890_device, usb_nb);
+
+	bq->usb_event = val;
+	queue_work(system_power_efficient_wq, &bq->usb_work);
+
+	return NOTIFY_OK;
+}
+
+static int bq25890_irq_probe(struct bq25890_device *bq)
+{
+	struct gpio_desc *irq;
+
+	irq = devm_gpiod_get_index(bq->dev, BQ25890_IRQ_PIN, 0, GPIOD_IN);
+	if (IS_ERR(irq)) {
+		dev_err(bq->dev, "Could not probe irq pin.\n");
+		return PTR_ERR(irq);
+	}
+
+	return gpiod_to_irq(irq);
+}
+
+static int bq25890_fw_read_u32_props(struct bq25890_device *bq)
+{
+	int ret;
+	u32 property;
+	int i;
+	struct bq25890_init_data *init = &bq->init_data;
+	struct {
+		char *name;
+		bool optional;
+		enum bq25890_table_ids tbl_id;
+		u8 *conv_data; /* holds converted value from given property */
+	} props[] = {
+		/* required properties */
+		{"ti,charge-current", false, TBL_ICHG, &init->ichg},
+		{"ti,battery-regulation-voltage", false, TBL_VREG, &init->vreg},
+		{"ti,termination-current", false, TBL_ITERM, &init->iterm},
+		{"ti,precharge-current", false, TBL_ITERM, &init->iprechg},
+		{"ti,minimum-sys-voltage", false, TBL_SYSVMIN, &init->sysvmin},
+		{"ti,boost-voltage", false, TBL_BOOSTV, &init->boostv},
+		{"ti,boost-max-current", false, TBL_BOOSTI, &init->boosti},
+
+		/* optional properties */
+		{"ti,thermal-regulation-threshold", true, TBL_TREG, &init->treg}
+	};
+
+	/* initialize data for optional properties */
+	init->treg = 3; /* 120 degrees Celsius */
+
+	for (i = 0; i < ARRAY_SIZE(props); i++) {
+		ret = device_property_read_u32(bq->dev, props[i].name,
+					       &property);
+		if (ret < 0) {
+			if (props[i].optional)
+				continue;
+
+			return ret;
+		}
+
+		*props[i].conv_data = bq25890_find_idx(property,
+						       props[i].tbl_id);
+	}
+
+	return 0;
+}
+
+static int bq25890_fw_probe(struct bq25890_device *bq)
+{
+	int ret;
+	struct bq25890_init_data *init = &bq->init_data;
+
+	ret = bq25890_fw_read_u32_props(bq);
+	if (ret < 0)
+		return ret;
+
+	init->ilim_en = device_property_read_bool(bq->dev, "ti,use-ilim-pin");
+	init->boostf = device_property_read_bool(bq->dev, "ti,boost-low-freq");
+
+	return 0;
+}
+
+static int bq25890_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct device *dev = &client->dev;
+	struct bq25890_device *bq;
+	int ret;
+	int i;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(dev, "No support for SMBUS_BYTE_DATA\n");
+		return -ENODEV;
+	}
+
+	bq = devm_kzalloc(dev, sizeof(*bq), GFP_KERNEL);
+	if (!bq)
+		return -ENOMEM;
+
+	bq->client = client;
+	bq->dev = dev;
+
+	mutex_init(&bq->lock);
+
+	bq->rmap = devm_regmap_init_i2c(client, &bq25890_regmap_config);
+	if (IS_ERR(bq->rmap)) {
+		dev_err(dev, "failed to allocate register map\n");
+		return PTR_ERR(bq->rmap);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(bq25890_reg_fields); i++) {
+		const struct reg_field *reg_fields = bq25890_reg_fields;
+
+		bq->rmap_fields[i] = devm_regmap_field_alloc(dev, bq->rmap,
+							     reg_fields[i]);
+		if (IS_ERR(bq->rmap_fields[i])) {
+			dev_err(dev, "cannot allocate regmap field\n");
+			return PTR_ERR(bq->rmap_fields[i]);
+		}
+	}
+
+	i2c_set_clientdata(client, bq);
+
+	bq->chip_id = bq25890_field_read(bq, F_PN);
+	if (bq->chip_id < 0) {
+		dev_err(dev, "Cannot read chip ID.\n");
+		return bq->chip_id;
+	}
+
+	if (bq->chip_id != BQ25890_ID) {
+		dev_err(dev, "Chip with ID=%d, not supported!\n", bq->chip_id);
+		return -ENODEV;
+	}
+
+	if (!dev->platform_data) {
+		ret = bq25890_fw_probe(bq);
+		if (ret < 0) {
+			dev_err(dev, "Cannot read device properties.\n");
+			return ret;
+		}
+	} else {
+		return -ENODEV;
+	}
+
+	ret = bq25890_hw_init(bq);
+	if (ret < 0) {
+		dev_err(dev, "Cannot initialize the chip.\n");
+		return ret;
+	}
+
+	if (client->irq <= 0)
+		client->irq = bq25890_irq_probe(bq);
+
+	if (client->irq < 0) {
+		dev_err(dev, "No irq resource found.\n");
+		return client->irq;
+	}
+
+	/* OTG reporting */
+	bq->usb_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
+	if (!IS_ERR_OR_NULL(bq->usb_phy)) {
+		INIT_WORK(&bq->usb_work, bq25890_usb_work);
+		bq->usb_nb.notifier_call = bq25890_usb_notifier;
+		usb_register_notifier(bq->usb_phy, &bq->usb_nb);
+	}
+
+	ret = devm_request_threaded_irq(dev, client->irq, NULL,
+					bq25890_irq_handler_thread,
+					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					BQ25890_IRQ_PIN, bq);
+	if (ret)
+		goto irq_fail;
+
+	ret = bq25890_power_supply_init(bq);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register power supply\n");
+		goto irq_fail;
+	}
+
+	return 0;
+
+irq_fail:
+	if (!IS_ERR_OR_NULL(bq->usb_phy))
+		usb_unregister_notifier(bq->usb_phy, &bq->usb_nb);
+
+	return ret;
+}
+
+static int bq25890_remove(struct i2c_client *client)
+{
+	struct bq25890_device *bq = i2c_get_clientdata(client);
+
+	power_supply_unregister(bq->charger);
+
+	if (!IS_ERR_OR_NULL(bq->usb_phy))
+		usb_unregister_notifier(bq->usb_phy, &bq->usb_nb);
+
+	/* reset all registers to default values */
+	bq25890_chip_reset(bq);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int bq25890_suspend(struct device *dev)
+{
+	struct bq25890_device *bq = dev_get_drvdata(dev);
+
+	/*
+	 * If charger is removed, while in suspend, make sure ADC is diabled
+	 * since it consumes slightly more power.
+	 */
+	return bq25890_field_write(bq, F_CONV_START, 0);
+}
+
+static int bq25890_resume(struct device *dev)
+{
+	int ret;
+	struct bq25890_state state;
+	struct bq25890_device *bq = dev_get_drvdata(dev);
+
+	ret = bq25890_get_chip_state(bq, &state);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&bq->lock);
+	bq->state = state;
+	mutex_unlock(&bq->lock);
+
+	/* Re-enable ADC only if charger is plugged in. */
+	if (state.online) {
+		ret = bq25890_field_write(bq, F_CONV_START, 1);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* signal userspace, maybe state changed while suspended */
+	power_supply_changed(bq->charger);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops bq25890_pm = {
+	SET_SYSTEM_SLEEP_PM_OPS(bq25890_suspend, bq25890_resume)
+};
+
+static const struct i2c_device_id bq25890_i2c_ids[] = {
+	{ "bq25890", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, bq25890_i2c_ids);
+
+static const struct of_device_id bq25890_of_match[] = {
+	{ .compatible = "ti,bq25890", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, bq25890_of_match);
+
+static const struct acpi_device_id bq25890_acpi_match[] = {
+	{"BQ258900", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, bq25890_acpi_match);
+
+static struct i2c_driver bq25890_driver = {
+	.driver = {
+		.name = "bq25890-charger",
+		.of_match_table = of_match_ptr(bq25890_of_match),
+		.acpi_match_table = ACPI_PTR(bq25890_acpi_match),
+		.pm = &bq25890_pm,
+	},
+	.probe = bq25890_probe,
+	.remove = bq25890_remove,
+	.id_table = bq25890_i2c_ids,
+};
+module_i2c_driver(bq25890_driver);
+
+MODULE_AUTHOR("Laurentiu Palcu <laurentiu.palcu@intel.com>");
+MODULE_DESCRIPTION("bq25890 charger driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/charger-manager.c b/drivers/power/charger-manager.c
index 0aed13f90891..1c202ccbd2a6 100644
--- a/drivers/power/charger-manager.c
+++ b/drivers/power/charger-manager.c
@@ -1768,7 +1768,8 @@ static int charger_manager_probe(struct platform_device *pdev)
 
 	INIT_DELAYED_WORK(&cm->fullbatt_vchk_work, fullbatt_vchk);
 
-	cm->charger_psy = power_supply_register(NULL, &cm->charger_psy_desc,
+	cm->charger_psy = power_supply_register(&pdev->dev,
+						&cm->charger_psy_desc,
 						&psy_cfg);
 	if (IS_ERR(cm->charger_psy)) {
 		dev_err(&pdev->dev, "Cannot register charger-manager with name \"%s\"\n",
diff --git a/drivers/power/max17042_battery.c b/drivers/power/max17042_battery.c
index 6cc5e87ec031..e89255764745 100644
--- a/drivers/power/max17042_battery.c
+++ b/drivers/power/max17042_battery.c
@@ -63,6 +63,8 @@
 #define dP_ACC_100	0x1900
 #define dP_ACC_200	0x3200
 
+#define MAX17042_VMAX_TOLERANCE		50 /* 50 mV */
+
 struct max17042_chip {
 	struct i2c_client *client;
 	struct regmap *regmap;
@@ -85,10 +87,94 @@ static enum power_supply_property max17042_battery_props[] = {
 	POWER_SUPPLY_PROP_CHARGE_FULL,
 	POWER_SUPPLY_PROP_CHARGE_COUNTER,
 	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_TEMP_ALERT_MIN,
+	POWER_SUPPLY_PROP_TEMP_ALERT_MAX,
+	POWER_SUPPLY_PROP_TEMP_MIN,
+	POWER_SUPPLY_PROP_TEMP_MAX,
+	POWER_SUPPLY_PROP_HEALTH,
 	POWER_SUPPLY_PROP_CURRENT_NOW,
 	POWER_SUPPLY_PROP_CURRENT_AVG,
 };
 
+static int max17042_get_temperature(struct max17042_chip *chip, int *temp)
+{
+	int ret;
+	u32 data;
+	struct regmap *map = chip->regmap;
+
+	ret = regmap_read(map, MAX17042_TEMP, &data);
+	if (ret < 0)
+		return ret;
+
+	*temp = data;
+	/* The value is signed. */
+	if (*temp & 0x8000) {
+		*temp = (0x7fff & ~*temp) + 1;
+		*temp *= -1;
+	}
+
+	/* The value is converted into deci-centigrade scale */
+	/* Units of LSB = 1 / 256 degree Celsius */
+	*temp = *temp * 10 / 256;
+	return 0;
+}
+
+static int max17042_get_battery_health(struct max17042_chip *chip, int *health)
+{
+	int temp, vavg, vbatt, ret;
+	u32 val;
+
+	ret = regmap_read(chip->regmap, MAX17042_AvgVCELL, &val);
+	if (ret < 0)
+		goto health_error;
+
+	/* bits [0-3] unused */
+	vavg = val * 625 / 8;
+	/* Convert to millivolts */
+	vavg /= 1000;
+
+	ret = regmap_read(chip->regmap, MAX17042_VCELL, &val);
+	if (ret < 0)
+		goto health_error;
+
+	/* bits [0-3] unused */
+	vbatt = val * 625 / 8;
+	/* Convert to millivolts */
+	vbatt /= 1000;
+
+	if (vavg < chip->pdata->vmin) {
+		*health = POWER_SUPPLY_HEALTH_DEAD;
+		goto out;
+	}
+
+	if (vbatt > chip->pdata->vmax + MAX17042_VMAX_TOLERANCE) {
+		*health = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		goto out;
+	}
+
+	ret = max17042_get_temperature(chip, &temp);
+	if (ret < 0)
+		goto health_error;
+
+	if (temp <= chip->pdata->temp_min) {
+		*health = POWER_SUPPLY_HEALTH_COLD;
+		goto out;
+	}
+
+	if (temp >= chip->pdata->temp_max) {
+		*health = POWER_SUPPLY_HEALTH_OVERHEAT;
+		goto out;
+	}
+
+	*health = POWER_SUPPLY_HEALTH_GOOD;
+
+out:
+	return 0;
+
+health_error:
+	return ret;
+}
+
 static int max17042_get_property(struct power_supply *psy,
 			    enum power_supply_property psp,
 			    union power_supply_propval *val)
@@ -181,19 +267,34 @@ static int max17042_get_property(struct power_supply *psy,
 		val->intval = data * 1000 / 2;
 		break;
 	case POWER_SUPPLY_PROP_TEMP:
-		ret = regmap_read(map, MAX17042_TEMP, &data);
+		ret = max17042_get_temperature(chip, &val->intval);
+		if (ret < 0)
+			return ret;
+		break;
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MIN:
+		ret = regmap_read(map, MAX17042_TALRT_Th, &data);
+		if (ret < 0)
+			return ret;
+		/* LSB is Alert Minimum. In deci-centigrade */
+		val->intval = (data & 0xff) * 10;
+		break;
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MAX:
+		ret = regmap_read(map, MAX17042_TALRT_Th, &data);
+		if (ret < 0)
+			return ret;
+		/* MSB is Alert Maximum. In deci-centigrade */
+		val->intval = (data >> 8) * 10;
+		break;
+	case POWER_SUPPLY_PROP_TEMP_MIN:
+		val->intval = chip->pdata->temp_min;
+		break;
+	case POWER_SUPPLY_PROP_TEMP_MAX:
+		val->intval = chip->pdata->temp_max;
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		ret = max17042_get_battery_health(chip, &val->intval);
 		if (ret < 0)
 			return ret;
-
-		val->intval = data;
-		/* The value is signed. */
-		if (val->intval & 0x8000) {
-			val->intval = (0x7fff & ~val->intval) + 1;
-			val->intval *= -1;
-		}
-		/* The value is converted into deci-centigrade scale */
-		/* Units of LSB = 1 / 256 degree Celsius */
-		val->intval = val->intval * 10 / 256;
 		break;
 	case POWER_SUPPLY_PROP_CURRENT_NOW:
 		if (chip->pdata->enable_current_sense) {
@@ -237,6 +338,69 @@ static int max17042_get_property(struct power_supply *psy,
 	return 0;
 }
 
+static int max17042_set_property(struct power_supply *psy,
+			    enum power_supply_property psp,
+			    const union power_supply_propval *val)
+{
+	struct max17042_chip *chip = power_supply_get_drvdata(psy);
+	struct regmap *map = chip->regmap;
+	int ret = 0;
+	u32 data;
+	int8_t temp;
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MIN:
+		ret = regmap_read(map, MAX17042_TALRT_Th, &data);
+		if (ret < 0)
+			return ret;
+
+		/* Input in deci-centigrade, convert to centigrade */
+		temp = val->intval / 10;
+		/* force min < max */
+		if (temp >= (int8_t)(data >> 8))
+			temp = (int8_t)(data >> 8) - 1;
+		/* Write both MAX and MIN ALERT */
+		data = (data & 0xff00) + temp;
+		ret = regmap_write(map, MAX17042_TALRT_Th, data);
+		break;
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MAX:
+		ret = regmap_read(map, MAX17042_TALRT_Th, &data);
+		if (ret < 0)
+			return ret;
+
+		/* Input in Deci-Centigrade, convert to centigrade */
+		temp = val->intval / 10;
+		/* force max > min */
+		if (temp <= (int8_t)(data & 0xff))
+			temp = (int8_t)(data & 0xff) + 1;
+		/* Write both MAX and MIN ALERT */
+		data = (data & 0xff) + (temp << 8);
+		ret = regmap_write(map, MAX17042_TALRT_Th, data);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int max17042_property_is_writeable(struct power_supply *psy,
+		enum power_supply_property psp)
+{
+	int ret;
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MIN:
+	case POWER_SUPPLY_PROP_TEMP_ALERT_MAX:
+		ret = 1;
+		break;
+	default:
+		ret = 0;
+	}
+
+	return ret;
+}
+
 static int max17042_write_verify_reg(struct regmap *map, u8 reg, u32 value)
 {
 	int retries = 8;
@@ -645,6 +809,15 @@ max17042_get_pdata(struct device *dev)
 		pdata->enable_current_sense = true;
 	}
 
+	if (of_property_read_s32(np, "maxim,cold-temp", &pdata->temp_min))
+		pdata->temp_min = INT_MIN;
+	if (of_property_read_s32(np, "maxim,over-heat-temp", &pdata->temp_max))
+		pdata->temp_max = INT_MAX;
+	if (of_property_read_s32(np, "maxim,dead-volt", &pdata->vmin))
+		pdata->vmin = INT_MIN;
+	if (of_property_read_s32(np, "maxim,over-volt", &pdata->vmax))
+		pdata->vmax = INT_MAX;
+
 	return pdata;
 }
 #else
@@ -665,6 +838,8 @@ static const struct power_supply_desc max17042_psy_desc = {
 	.name		= "max170xx_battery",
 	.type		= POWER_SUPPLY_TYPE_BATTERY,
 	.get_property	= max17042_get_property,
+	.set_property	= max17042_set_property,
+	.property_is_writeable	= max17042_property_is_writeable,
 	.properties	= max17042_battery_props,
 	.num_properties	= ARRAY_SIZE(max17042_battery_props),
 };
@@ -673,6 +848,8 @@ static const struct power_supply_desc max17042_no_current_sense_psy_desc = {
 	.name		= "max170xx_battery",
 	.type		= POWER_SUPPLY_TYPE_BATTERY,
 	.get_property	= max17042_get_property,
+	.set_property	= max17042_set_property,
+	.property_is_writeable	= max17042_property_is_writeable,
 	.properties	= max17042_battery_props,
 	.num_properties	= ARRAY_SIZE(max17042_battery_props) - 2,
 };
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 2ed4a4a6b3c5..869284c2e1e8 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -30,6 +30,8 @@ EXPORT_SYMBOL_GPL(power_supply_notifier);
 
 static struct device_type power_supply_dev_type;
 
+#define POWER_SUPPLY_DEFERRED_REGISTER_TIME	msecs_to_jiffies(10)
+
 static bool __power_supply_is_supplied_by(struct power_supply *supplier,
 					 struct power_supply *supply)
 {
@@ -121,6 +123,30 @@ void power_supply_changed(struct power_supply *psy)
 }
 EXPORT_SYMBOL_GPL(power_supply_changed);
 
+/*
+ * Notify that power supply was registered after parent finished the probing.
+ *
+ * Often power supply is registered from driver's probe function. However
+ * calling power_supply_changed() directly from power_supply_register()
+ * would lead to execution of get_property() function provided by the driver
+ * too early - before the probe ends.
+ *
+ * Avoid that by waiting on parent's mutex.
+ */
+static void power_supply_deferred_register_work(struct work_struct *work)
+{
+	struct power_supply *psy = container_of(work, struct power_supply,
+						deferred_register_work.work);
+
+	if (psy->dev.parent)
+		mutex_lock(&psy->dev.parent->mutex);
+
+	power_supply_changed(psy);
+
+	if (psy->dev.parent)
+		mutex_unlock(&psy->dev.parent->mutex);
+}
+
 #ifdef CONFIG_OF
 #include <linux/of.h>
 
@@ -420,6 +446,45 @@ struct power_supply *power_supply_get_by_phandle(struct device_node *np,
 	return psy;
 }
 EXPORT_SYMBOL_GPL(power_supply_get_by_phandle);
+
+static void devm_power_supply_put(struct device *dev, void *res)
+{
+	struct power_supply **psy = res;
+
+	power_supply_put(*psy);
+}
+
+/**
+ * devm_power_supply_get_by_phandle() - Resource managed version of
+ *  power_supply_get_by_phandle()
+ * @dev: Pointer to device holding phandle property
+ * @phandle_name: Name of property holding a power supply phandle
+ *
+ * Return: On success returns a reference to a power supply with
+ * matching name equals to value under @property, NULL or ERR_PTR otherwise.
+ */
+struct power_supply *devm_power_supply_get_by_phandle(struct device *dev,
+						      const char *property)
+{
+	struct power_supply **ptr, *psy;
+
+	if (!dev->of_node)
+		return ERR_PTR(-ENODEV);
+
+	ptr = devres_alloc(devm_power_supply_put, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	psy = power_supply_get_by_phandle(dev->of_node, property);
+	if (IS_ERR_OR_NULL(psy)) {
+		devres_free(ptr);
+	} else {
+		*ptr = psy;
+		devres_add(dev, ptr);
+	}
+	return psy;
+}
+EXPORT_SYMBOL_GPL(devm_power_supply_get_by_phandle);
 #endif /* CONFIG_OF */
 
 int power_supply_get_property(struct power_supply *psy,
@@ -645,6 +710,10 @@ __power_supply_register(struct device *parent,
 	struct power_supply *psy;
 	int rc;
 
+	if (!parent)
+		pr_warn("%s: Expected proper parent device for '%s'\n",
+			__func__, desc->name);
+
 	psy = kzalloc(sizeof(*psy), GFP_KERNEL);
 	if (!psy)
 		return ERR_PTR(-ENOMEM);
@@ -659,7 +728,6 @@ __power_supply_register(struct device *parent,
 	dev->release = power_supply_dev_release;
 	dev_set_drvdata(dev, psy);
 	psy->desc = desc;
-	atomic_inc(&psy->use_cnt);
 	if (cfg) {
 		psy->drv_data = cfg->drv_data;
 		psy->of_node = cfg->of_node;
@@ -672,6 +740,8 @@ __power_supply_register(struct device *parent,
 		goto dev_set_name_failed;
 
 	INIT_WORK(&psy->changed_work, power_supply_changed_work);
+	INIT_DELAYED_WORK(&psy->deferred_register_work,
+			  power_supply_deferred_register_work);
 
 	rc = power_supply_check_supplies(psy);
 	if (rc) {
@@ -700,7 +770,20 @@ __power_supply_register(struct device *parent,
 	if (rc)
 		goto create_triggers_failed;
 
-	power_supply_changed(psy);
+	/*
+	 * Update use_cnt after any uevents (most notably from device_add()).
+	 * We are here still during driver's probe but
+	 * the power_supply_uevent() calls back driver's get_property
+	 * method so:
+	 * 1. Driver did not assigned the returned struct power_supply,
+	 * 2. Driver could not finish initialization (anything in its probe
+	 *    after calling power_supply_register()).
+	 */
+	atomic_inc(&psy->use_cnt);
+
+	queue_delayed_work(system_power_efficient_wq,
+			   &psy->deferred_register_work,
+			   POWER_SUPPLY_DEFERRED_REGISTER_TIME);
 
 	return psy;
 
@@ -720,7 +803,8 @@ dev_set_name_failed:
 
 /**
  * power_supply_register() - Register new power supply
- * @parent:	Device to be a parent of power supply's device
+ * @parent:	Device to be a parent of power supply's device, usually
+ *		the device which probe function calls this
  * @desc:	Description of power supply, must be valid through whole
  *		lifetime of this power supply
  * @cfg:	Run-time specific configuration accessed during registering,
@@ -740,8 +824,9 @@ struct power_supply *__must_check power_supply_register(struct device *parent,
 EXPORT_SYMBOL_GPL(power_supply_register);
 
 /**
- * power_supply_register() - Register new non-waking-source power supply
- * @parent:	Device to be a parent of power supply's device
+ * power_supply_register_no_ws() - Register new non-waking-source power supply
+ * @parent:	Device to be a parent of power supply's device, usually
+ *		the device which probe function calls this
  * @desc:	Description of power supply, must be valid through whole
  *		lifetime of this power supply
  * @cfg:	Run-time specific configuration accessed during registering,
@@ -769,8 +854,9 @@ static void devm_power_supply_release(struct device *dev, void *res)
 }
 
 /**
- * power_supply_register() - Register managed power supply
- * @parent:	Device to be a parent of power supply's device
+ * devm_power_supply_register() - Register managed power supply
+ * @parent:	Device to be a parent of power supply's device, usually
+ *		the device which probe function calls this
  * @desc:	Description of power supply, must be valid through whole
  *		lifetime of this power supply
  * @cfg:	Run-time specific configuration accessed during registering,
@@ -804,8 +890,9 @@ devm_power_supply_register(struct device *parent,
 EXPORT_SYMBOL_GPL(devm_power_supply_register);
 
 /**
- * power_supply_register() - Register managed non-waking-source power supply
- * @parent:	Device to be a parent of power supply's device
+ * devm_power_supply_register_no_ws() - Register managed non-waking-source power supply
+ * @parent:	Device to be a parent of power supply's device, usually
+ *		the device which probe function calls this
  * @desc:	Description of power supply, must be valid through whole
  *		lifetime of this power supply
  * @cfg:	Run-time specific configuration accessed during registering,
@@ -849,6 +936,7 @@ void power_supply_unregister(struct power_supply *psy)
 {
 	WARN_ON(atomic_dec_return(&psy->use_cnt));
 	cancel_work_sync(&psy->changed_work);
+	cancel_delayed_work_sync(&psy->deferred_register_work);
 	sysfs_remove_link(&psy->dev.kobj, "powers");
 	power_supply_remove_triggers(psy);
 	psy_unregister_cooler(psy);
diff --git a/drivers/power/power_supply_leds.c b/drivers/power/power_supply_leds.c
index 2d41a43fc81a..2277ad9c2f68 100644
--- a/drivers/power/power_supply_leds.c
+++ b/drivers/power/power_supply_leds.c
@@ -25,7 +25,7 @@ static void power_supply_update_bat_leds(struct power_supply *psy)
 	unsigned long delay_on = 0;
 	unsigned long delay_off = 0;
 
-	if (psy->desc->get_property(psy, POWER_SUPPLY_PROP_STATUS, &status))
+	if (power_supply_get_property(psy, POWER_SUPPLY_PROP_STATUS, &status))
 		return;
 
 	dev_dbg(&psy->dev, "%s %d\n", __func__, status.intval);
@@ -115,7 +115,7 @@ static void power_supply_update_gen_leds(struct power_supply *psy)
 {
 	union power_supply_propval online;
 
-	if (psy->desc->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &online))
+	if (power_supply_get_property(psy, POWER_SUPPLY_PROP_ONLINE, &online))
 		return;
 
 	dev_dbg(&psy->dev, "%s %d\n", __func__, online.intval);
diff --git a/drivers/power/power_supply_sysfs.c b/drivers/power/power_supply_sysfs.c
index 9134e3d2d95e..ed2d7fd0c734 100644
--- a/drivers/power/power_supply_sysfs.c
+++ b/drivers/power/power_supply_sysfs.c
@@ -125,7 +125,7 @@ static ssize_t power_supply_store_property(struct device *dev,
 
 	value.intval = long_val;
 
-	ret = psy->desc->set_property(psy, off, &value);
+	ret = power_supply_set_property(psy, off, &value);
 	if (ret < 0)
 		return ret;
 
@@ -223,7 +223,7 @@ static umode_t power_supply_attr_is_visible(struct kobject *kobj,
 
 		if (property == attrno) {
 			if (psy->desc->property_is_writeable &&
-			    power_supply_property_is_writeable(psy, property) > 0)
+			    psy->desc->property_is_writeable(psy, property) > 0)
 				mode |= S_IWUSR;
 
 			return mode;
diff --git a/drivers/power/reset/at91-reset.c b/drivers/power/reset/at91-reset.c
index ca461ebc7ae8..36dc52fb2ec8 100644
--- a/drivers/power/reset/at91-reset.c
+++ b/drivers/power/reset/at91-reset.c
@@ -243,7 +243,7 @@ static int at91_reset_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id at91_reset_plat_match[] = {
+static const struct platform_device_id at91_reset_plat_match[] = {
 	{ "at91-sam9260-reset", (unsigned long)at91sam9260_restart },
 	{ "at91-sam9g45-reset", (unsigned long)at91sam9g45_restart },
 	{ /* sentinel */ }
diff --git a/drivers/power/reset/gpio-poweroff.c b/drivers/power/reset/gpio-poweroff.c
index e5332f1db8a7..be3d81ff51cc 100644
--- a/drivers/power/reset/gpio-poweroff.c
+++ b/drivers/power/reset/gpio-poweroff.c
@@ -48,6 +48,7 @@ static void gpio_poweroff_do_poweroff(void)
 static int gpio_poweroff_probe(struct platform_device *pdev)
 {
 	bool input = false;
+	enum gpiod_flags flags;
 
 	/* If a pm_power_off function has already been added, leave it alone */
 	if (pm_power_off != NULL) {
@@ -57,25 +58,15 @@ static int gpio_poweroff_probe(struct platform_device *pdev)
 		return -EBUSY;
 	}
 
-	reset_gpio = devm_gpiod_get(&pdev->dev, NULL);
-	if (IS_ERR(reset_gpio))
-		return PTR_ERR(reset_gpio);
-
 	input = of_property_read_bool(pdev->dev.of_node, "input");
+	if (input)
+		flags = GPIOD_IN;
+	else
+		flags = GPIOD_OUT_LOW;
 
-	if (input) {
-		if (gpiod_direction_input(reset_gpio)) {
-			dev_err(&pdev->dev,
-				"Could not set direction of reset GPIO to input\n");
-			return -ENODEV;
-		}
-	} else {
-		if (gpiod_direction_output(reset_gpio, 0)) {
-			dev_err(&pdev->dev,
-				"Could not set direction of reset GPIO\n");
-			return -ENODEV;
-		}
-	}
+	reset_gpio = devm_gpiod_get(&pdev->dev, NULL, flags);
+	if (IS_ERR(reset_gpio))
+		return PTR_ERR(reset_gpio);
 
 	pm_power_off = &gpio_poweroff_do_poweroff;
 	return 0;
diff --git a/drivers/power/reset/gpio-restart.c b/drivers/power/reset/gpio-restart.c
index edb327efee8b..829b45f42021 100644
--- a/drivers/power/reset/gpio-restart.c
+++ b/drivers/power/reset/gpio-restart.c
@@ -78,7 +78,7 @@ static int gpio_restart_probe(struct platform_device *pdev)
 	}
 
 	gpio_restart->restart_handler.notifier_call = gpio_restart_notify;
-	gpio_restart->restart_handler.priority = 128;
+	gpio_restart->restart_handler.priority = 129;
 	gpio_restart->active_delay_ms = 100;
 	gpio_restart->inactive_delay_ms = 100;
 	gpio_restart->wait_delay_ms = 3000;
diff --git a/drivers/power/reset/ltc2952-poweroff.c b/drivers/power/reset/ltc2952-poweroff.c
index 1e08195551fe..15fed9d8f871 100644
--- a/drivers/power/reset/ltc2952-poweroff.c
+++ b/drivers/power/reset/ltc2952-poweroff.c
@@ -158,7 +158,6 @@ static irqreturn_t ltc2952_poweroff_handler(int irq, void *dev_id)
 			      HRTIMER_MODE_REL);
 	} else {
 		hrtimer_cancel(&data->timer_trigger);
-		/* omitting return value check, timer should have been valid */
 	}
 	return IRQ_HANDLED;
 }
@@ -202,16 +201,15 @@ static int ltc2952_poweroff_init(struct platform_device *pdev)
 		return ret;
 	}
 
-	data->gpio_trigger = devm_gpiod_get(&pdev->dev, "trigger", GPIOD_IN);
+	data->gpio_trigger = devm_gpiod_get_optional(&pdev->dev, "trigger",
+						     GPIOD_IN);
 	if (IS_ERR(data->gpio_trigger)) {
 		/*
 		 * It's not a problem if the trigger gpio isn't available, but
 		 * it is worth a warning if its use was defined in the device
 		 * tree.
 		 */
-		if (PTR_ERR(data->gpio_trigger) != -ENOENT)
-			dev_err(&pdev->dev,
-				"unable to claim gpio \"trigger\"\n");
+		dev_err(&pdev->dev, "unable to claim gpio \"trigger\"\n");
 		data->gpio_trigger = NULL;
 	}
 
diff --git a/drivers/power/reset/syscon-reboot.c b/drivers/power/reset/syscon-reboot.c
index d3c7d245ae63..7d0d269a0837 100644
--- a/drivers/power/reset/syscon-reboot.c
+++ b/drivers/power/reset/syscon-reboot.c
@@ -88,4 +88,4 @@ static struct platform_driver syscon_reboot_driver = {
 		.of_match_table = syscon_reboot_of_match,
 	},
 };
-module_platform_driver(syscon_reboot_driver);
+builtin_platform_driver(syscon_reboot_driver);
diff --git a/drivers/power/rt9455_charger.c b/drivers/power/rt9455_charger.c
new file mode 100644
index 000000000000..08baac6e3ada
--- /dev/null
+++ b/drivers/power/rt9455_charger.c
@@ -0,0 +1,1752 @@
+/*
+ * Driver for Richtek RT9455WSC battery charger.
+ *
+ * Copyright (C) 2015 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/of_irq.h>
+#include <linux/of_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/power_supply.h>
+#include <linux/i2c.h>
+#include <linux/acpi.h>
+#include <linux/usb/phy.h>
+#include <linux/regmap.h>
+
+#define RT9455_MANUFACTURER			"Richtek"
+#define RT9455_MODEL_NAME			"RT9455"
+#define RT9455_DRIVER_NAME			"rt9455-charger"
+
+#define RT9455_IRQ_NAME				"interrupt"
+
+#define RT9455_PWR_RDY_DELAY			1 /* 1 second */
+#define RT9455_MAX_CHARGING_TIME		21600 /* 6 hrs */
+#define RT9455_BATT_PRESENCE_DELAY		60 /* 60 seconds */
+
+#define RT9455_CHARGE_MODE			0x00
+#define RT9455_BOOST_MODE			0x01
+
+#define RT9455_FAULT				0x03
+
+#define RT9455_IAICR_100MA			0x00
+#define RT9455_IAICR_500MA			0x01
+#define RT9455_IAICR_NO_LIMIT			0x03
+
+#define RT9455_CHARGE_DISABLE			0x00
+#define RT9455_CHARGE_ENABLE			0x01
+
+#define RT9455_PWR_FAULT			0x00
+#define RT9455_PWR_GOOD				0x01
+
+#define RT9455_REG_CTRL1			0x00 /* CTRL1 reg address */
+#define RT9455_REG_CTRL2			0x01 /* CTRL2 reg address */
+#define RT9455_REG_CTRL3			0x02 /* CTRL3 reg address */
+#define RT9455_REG_DEV_ID			0x03 /* DEV_ID reg address */
+#define RT9455_REG_CTRL4			0x04 /* CTRL4 reg address */
+#define RT9455_REG_CTRL5			0x05 /* CTRL5 reg address */
+#define RT9455_REG_CTRL6			0x06 /* CTRL6 reg address */
+#define RT9455_REG_CTRL7			0x07 /* CTRL7 reg address */
+#define RT9455_REG_IRQ1				0x08 /* IRQ1 reg address */
+#define RT9455_REG_IRQ2				0x09 /* IRQ2 reg address */
+#define RT9455_REG_IRQ3				0x0A /* IRQ3 reg address */
+#define RT9455_REG_MASK1			0x0B /* MASK1 reg address */
+#define RT9455_REG_MASK2			0x0C /* MASK2 reg address */
+#define RT9455_REG_MASK3			0x0D /* MASK3 reg address */
+
+enum rt9455_fields {
+	F_STAT, F_BOOST, F_PWR_RDY, F_OTG_PIN_POLARITY, /* CTRL1 reg fields */
+
+	F_IAICR, F_TE_SHDN_EN, F_HIGHER_OCP, F_TE, F_IAICR_INT, F_HIZ,
+	F_OPA_MODE, /* CTRL2 reg fields */
+
+	F_VOREG, F_OTG_PL, F_OTG_EN, /* CTRL3 reg fields */
+
+	F_VENDOR_ID, F_CHIP_REV, /* DEV_ID reg fields */
+
+	F_RST, /* CTRL4 reg fields */
+
+	F_TMR_EN, F_MIVR, F_IPREC, F_IEOC_PERCENTAGE, /* CTRL5 reg fields*/
+
+	F_IAICR_SEL, F_ICHRG, F_VPREC, /* CTRL6 reg fields */
+
+	F_BATD_EN, F_CHG_EN, F_VMREG, /* CTRL7 reg fields */
+
+	F_TSDI, F_VINOVPI, F_BATAB, /* IRQ1 reg fields */
+
+	F_CHRVPI, F_CHBATOVI, F_CHTERMI, F_CHRCHGI, F_CH32MI, F_CHTREGI,
+	F_CHMIVRI, /* IRQ2 reg fields */
+
+	F_BSTBUSOVI, F_BSTOLI, F_BSTLOWVI, F_BST32SI, /* IRQ3 reg fields */
+
+	F_TSDM, F_VINOVPIM, F_BATABM, /* MASK1 reg fields */
+
+	F_CHRVPIM, F_CHBATOVIM, F_CHTERMIM, F_CHRCHGIM, F_CH32MIM, F_CHTREGIM,
+	F_CHMIVRIM, /* MASK2 reg fields */
+
+	F_BSTVINOVIM, F_BSTOLIM, F_BSTLOWVIM, F_BST32SIM, /* MASK3 reg fields */
+
+	F_MAX_FIELDS
+};
+
+static const struct reg_field rt9455_reg_fields[] = {
+	[F_STAT]		= REG_FIELD(RT9455_REG_CTRL1, 4, 5),
+	[F_BOOST]		= REG_FIELD(RT9455_REG_CTRL1, 3, 3),
+	[F_PWR_RDY]		= REG_FIELD(RT9455_REG_CTRL1, 2, 2),
+	[F_OTG_PIN_POLARITY]	= REG_FIELD(RT9455_REG_CTRL1, 1, 1),
+
+	[F_IAICR]		= REG_FIELD(RT9455_REG_CTRL2, 6, 7),
+	[F_TE_SHDN_EN]		= REG_FIELD(RT9455_REG_CTRL2, 5, 5),
+	[F_HIGHER_OCP]		= REG_FIELD(RT9455_REG_CTRL2, 4, 4),
+	[F_TE]			= REG_FIELD(RT9455_REG_CTRL2, 3, 3),
+	[F_IAICR_INT]		= REG_FIELD(RT9455_REG_CTRL2, 2, 2),
+	[F_HIZ]			= REG_FIELD(RT9455_REG_CTRL2, 1, 1),
+	[F_OPA_MODE]		= REG_FIELD(RT9455_REG_CTRL2, 0, 0),
+
+	[F_VOREG]		= REG_FIELD(RT9455_REG_CTRL3, 2, 7),
+	[F_OTG_PL]		= REG_FIELD(RT9455_REG_CTRL3, 1, 1),
+	[F_OTG_EN]		= REG_FIELD(RT9455_REG_CTRL3, 0, 0),
+
+	[F_VENDOR_ID]		= REG_FIELD(RT9455_REG_DEV_ID, 4, 7),
+	[F_CHIP_REV]		= REG_FIELD(RT9455_REG_DEV_ID, 0, 3),
+
+	[F_RST]			= REG_FIELD(RT9455_REG_CTRL4, 7, 7),
+
+	[F_TMR_EN]		= REG_FIELD(RT9455_REG_CTRL5, 7, 7),
+	[F_MIVR]		= REG_FIELD(RT9455_REG_CTRL5, 4, 5),
+	[F_IPREC]		= REG_FIELD(RT9455_REG_CTRL5, 2, 3),
+	[F_IEOC_PERCENTAGE]	= REG_FIELD(RT9455_REG_CTRL5, 0, 1),
+
+	[F_IAICR_SEL]		= REG_FIELD(RT9455_REG_CTRL6, 7, 7),
+	[F_ICHRG]		= REG_FIELD(RT9455_REG_CTRL6, 4, 6),
+	[F_VPREC]		= REG_FIELD(RT9455_REG_CTRL6, 0, 2),
+
+	[F_BATD_EN]		= REG_FIELD(RT9455_REG_CTRL7, 6, 6),
+	[F_CHG_EN]		= REG_FIELD(RT9455_REG_CTRL7, 4, 4),
+	[F_VMREG]		= REG_FIELD(RT9455_REG_CTRL7, 0, 3),
+
+	[F_TSDI]		= REG_FIELD(RT9455_REG_IRQ1, 7, 7),
+	[F_VINOVPI]		= REG_FIELD(RT9455_REG_IRQ1, 6, 6),
+	[F_BATAB]		= REG_FIELD(RT9455_REG_IRQ1, 0, 0),
+
+	[F_CHRVPI]		= REG_FIELD(RT9455_REG_IRQ2, 7, 7),
+	[F_CHBATOVI]		= REG_FIELD(RT9455_REG_IRQ2, 5, 5),
+	[F_CHTERMI]		= REG_FIELD(RT9455_REG_IRQ2, 4, 4),
+	[F_CHRCHGI]		= REG_FIELD(RT9455_REG_IRQ2, 3, 3),
+	[F_CH32MI]		= REG_FIELD(RT9455_REG_IRQ2, 2, 2),
+	[F_CHTREGI]		= REG_FIELD(RT9455_REG_IRQ2, 1, 1),
+	[F_CHMIVRI]		= REG_FIELD(RT9455_REG_IRQ2, 0, 0),
+
+	[F_BSTBUSOVI]		= REG_FIELD(RT9455_REG_IRQ3, 7, 7),
+	[F_BSTOLI]		= REG_FIELD(RT9455_REG_IRQ3, 6, 6),
+	[F_BSTLOWVI]		= REG_FIELD(RT9455_REG_IRQ3, 5, 5),
+	[F_BST32SI]		= REG_FIELD(RT9455_REG_IRQ3, 3, 3),
+
+	[F_TSDM]		= REG_FIELD(RT9455_REG_MASK1, 7, 7),
+	[F_VINOVPIM]		= REG_FIELD(RT9455_REG_MASK1, 6, 6),
+	[F_BATABM]		= REG_FIELD(RT9455_REG_MASK1, 0, 0),
+
+	[F_CHRVPIM]		= REG_FIELD(RT9455_REG_MASK2, 7, 7),
+	[F_CHBATOVIM]		= REG_FIELD(RT9455_REG_MASK2, 5, 5),
+	[F_CHTERMIM]		= REG_FIELD(RT9455_REG_MASK2, 4, 4),
+	[F_CHRCHGIM]		= REG_FIELD(RT9455_REG_MASK2, 3, 3),
+	[F_CH32MIM]		= REG_FIELD(RT9455_REG_MASK2, 2, 2),
+	[F_CHTREGIM]		= REG_FIELD(RT9455_REG_MASK2, 1, 1),
+	[F_CHMIVRIM]		= REG_FIELD(RT9455_REG_MASK2, 0, 0),
+
+	[F_BSTVINOVIM]		= REG_FIELD(RT9455_REG_MASK3, 7, 7),
+	[F_BSTOLIM]		= REG_FIELD(RT9455_REG_MASK3, 6, 6),
+	[F_BSTLOWVIM]		= REG_FIELD(RT9455_REG_MASK3, 5, 5),
+	[F_BST32SIM]		= REG_FIELD(RT9455_REG_MASK3, 3, 3),
+};
+
+#define GET_MASK(fid)	(BIT(rt9455_reg_fields[fid].msb + 1) - \
+			 BIT(rt9455_reg_fields[fid].lsb))
+
+/*
+ * Each array initialised below shows the possible real-world values for a
+ * group of bits belonging to RT9455 registers. The arrays are sorted in
+ * ascending order. The index of each real-world value represents the value
+ * that is encoded in the group of bits belonging to RT9455 registers.
+ */
+/* REG06[6:4] (ICHRG) in uAh */
+static const int rt9455_ichrg_values[] = {
+	 500000,  650000,  800000,  950000, 1100000, 1250000, 1400000, 1550000
+};
+
+/*
+ * When the charger is in charge mode, REG02[7:2] represent battery regulation
+ * voltage.
+ */
+/* REG02[7:2] (VOREG) in uV */
+static const int rt9455_voreg_values[] = {
+	3500000, 3520000, 3540000, 3560000, 3580000, 3600000, 3620000, 3640000,
+	3660000, 3680000, 3700000, 3720000, 3740000, 3760000, 3780000, 3800000,
+	3820000, 3840000, 3860000, 3880000, 3900000, 3920000, 3940000, 3960000,
+	3980000, 4000000, 4020000, 4040000, 4060000, 4080000, 4100000, 4120000,
+	4140000, 4160000, 4180000, 4200000, 4220000, 4240000, 4260000, 4280000,
+	4300000, 4330000, 4350000, 4370000, 4390000, 4410000, 4430000, 4450000,
+	4450000, 4450000, 4450000, 4450000, 4450000, 4450000, 4450000, 4450000,
+	4450000, 4450000, 4450000, 4450000, 4450000, 4450000, 4450000, 4450000
+};
+
+/*
+ * When the charger is in boost mode, REG02[7:2] represent boost output
+ * voltage.
+ */
+/* REG02[7:2] (Boost output voltage) in uV */
+static const int rt9455_boost_voltage_values[] = {
+	4425000, 4450000, 4475000, 4500000, 4525000, 4550000, 4575000, 4600000,
+	4625000, 4650000, 4675000, 4700000, 4725000, 4750000, 4775000, 4800000,
+	4825000, 4850000, 4875000, 4900000, 4925000, 4950000, 4975000, 5000000,
+	5025000, 5050000, 5075000, 5100000, 5125000, 5150000, 5175000, 5200000,
+	5225000, 5250000, 5275000, 5300000, 5325000, 5350000, 5375000, 5400000,
+	5425000, 5450000, 5475000, 5500000, 5525000, 5550000, 5575000, 5600000,
+	5600000, 5600000, 5600000, 5600000, 5600000, 5600000, 5600000, 5600000,
+	5600000, 5600000, 5600000, 5600000, 5600000, 5600000, 5600000, 5600000,
+};
+
+/* REG07[3:0] (VMREG) in uV */
+static const int rt9455_vmreg_values[] = {
+	4200000, 4220000, 4240000, 4260000, 4280000, 4300000, 4320000, 4340000,
+	4360000, 4380000, 4400000, 4430000, 4450000, 4450000, 4450000, 4450000
+};
+
+/* REG05[5:4] (IEOC_PERCENTAGE) */
+static const int rt9455_ieoc_percentage_values[] = {
+	10, 30, 20, 30
+};
+
+/* REG05[1:0] (MIVR) in uV */
+static const int rt9455_mivr_values[] = {
+	4000000, 4250000, 4500000, 5000000
+};
+
+/* REG05[1:0] (IAICR) in uA */
+static const int rt9455_iaicr_values[] = {
+	100000, 500000, 1000000, 2000000
+};
+
+struct rt9455_info {
+	struct i2c_client		*client;
+	struct regmap			*regmap;
+	struct regmap_field		*regmap_fields[F_MAX_FIELDS];
+	struct power_supply		*charger;
+#if IS_ENABLED(CONFIG_USB_PHY)
+	struct usb_phy			*usb_phy;
+	struct notifier_block		nb;
+#endif
+	struct delayed_work		pwr_rdy_work;
+	struct delayed_work		max_charging_time_work;
+	struct delayed_work		batt_presence_work;
+	u32				voreg;
+	u32				boost_voltage;
+};
+
+/*
+ * Iterate through each element of the 'tbl' array until an element whose value
+ * is greater than v is found. Return the index of the respective element,
+ * or the index of the last element in the array, if no such element is found.
+ */
+static unsigned int rt9455_find_idx(const int tbl[], int tbl_size, int v)
+{
+	int i;
+
+	/*
+	 * No need to iterate until the last index in the table because
+	 * if no element greater than v is found in the table,
+	 * or if only the last element is greater than v,
+	 * function returns the index of the last element.
+	 */
+	for (i = 0; i < tbl_size - 1; i++)
+		if (v <= tbl[i])
+			return i;
+
+	return (tbl_size - 1);
+}
+
+static int rt9455_get_field_val(struct rt9455_info *info,
+				enum rt9455_fields field,
+				const int tbl[], int tbl_size, int *val)
+{
+	unsigned int v;
+	int ret;
+
+	ret = regmap_field_read(info->regmap_fields[field], &v);
+	if (ret)
+		return ret;
+
+	v = (v >= tbl_size) ? (tbl_size - 1) : v;
+	*val = tbl[v];
+
+	return 0;
+}
+
+static int rt9455_set_field_val(struct rt9455_info *info,
+				enum rt9455_fields field,
+				const int tbl[], int tbl_size, int val)
+{
+	unsigned int idx = rt9455_find_idx(tbl, tbl_size, val);
+
+	return regmap_field_write(info->regmap_fields[field], idx);
+}
+
+static int rt9455_register_reset(struct rt9455_info *info)
+{
+	struct device *dev = &info->client->dev;
+	unsigned int v;
+	int ret, limit = 100;
+
+	ret = regmap_field_write(info->regmap_fields[F_RST], 0x01);
+	if (ret) {
+		dev_err(dev, "Failed to set RST bit\n");
+		return ret;
+	}
+
+	/*
+	 * To make sure that reset operation has finished, loop until RST bit
+	 * is set to 0.
+	 */
+	do {
+		ret = regmap_field_read(info->regmap_fields[F_RST], &v);
+		if (ret) {
+			dev_err(dev, "Failed to read RST bit\n");
+			return ret;
+		}
+
+		if (!v)
+			break;
+
+		usleep_range(10, 100);
+	} while (--limit);
+
+	if (!limit)
+		return -EIO;
+
+	return 0;
+}
+
+/* Charger power supply property routines */
+static enum power_supply_property rt9455_charger_properties[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE,
+	POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX,
+	POWER_SUPPLY_PROP_SCOPE,
+	POWER_SUPPLY_PROP_CHARGE_TERM_CURRENT,
+	POWER_SUPPLY_PROP_MODEL_NAME,
+	POWER_SUPPLY_PROP_MANUFACTURER,
+};
+
+static char *rt9455_charger_supplied_to[] = {
+	"main-battery",
+};
+
+static int rt9455_charger_get_status(struct rt9455_info *info,
+				     union power_supply_propval *val)
+{
+	unsigned int v, pwr_rdy;
+	int ret;
+
+	ret = regmap_field_read(info->regmap_fields[F_PWR_RDY],
+				&pwr_rdy);
+	if (ret) {
+		dev_err(&info->client->dev, "Failed to read PWR_RDY bit\n");
+		return ret;
+	}
+
+	/*
+	 * If PWR_RDY bit is unset, the battery is discharging. Otherwise,
+	 * STAT bits value must be checked.
+	 */
+	if (!pwr_rdy) {
+		val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+		return 0;
+	}
+
+	ret = regmap_field_read(info->regmap_fields[F_STAT], &v);
+	if (ret) {
+		dev_err(&info->client->dev, "Failed to read STAT bits\n");
+		return ret;
+	}
+
+	switch (v) {
+	case 0:
+		/*
+		 * If PWR_RDY bit is set, but STAT bits value is 0, the charger
+		 * may be in one of the following cases:
+		 * 1. CHG_EN bit is 0.
+		 * 2. CHG_EN bit is 1 but the battery is not connected.
+		 * In any of these cases, POWER_SUPPLY_STATUS_NOT_CHARGING is
+		 * returned.
+		 */
+		val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
+		return 0;
+	case 1:
+		val->intval = POWER_SUPPLY_STATUS_CHARGING;
+		return 0;
+	case 2:
+		val->intval = POWER_SUPPLY_STATUS_FULL;
+		return 0;
+	default:
+		val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+		return 0;
+	}
+}
+
+static int rt9455_charger_get_health(struct rt9455_info *info,
+				     union power_supply_propval *val)
+{
+	struct device *dev = &info->client->dev;
+	unsigned int v;
+	int ret;
+
+	val->intval = POWER_SUPPLY_HEALTH_GOOD;
+
+	ret = regmap_read(info->regmap, RT9455_REG_IRQ1, &v);
+	if (ret) {
+		dev_err(dev, "Failed to read IRQ1 register\n");
+		return ret;
+	}
+
+	if (v & GET_MASK(F_TSDI)) {
+		val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		return 0;
+	}
+	if (v & GET_MASK(F_VINOVPI)) {
+		val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		return 0;
+	}
+	if (v & GET_MASK(F_BATAB)) {
+		val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		return 0;
+	}
+
+	ret = regmap_read(info->regmap, RT9455_REG_IRQ2, &v);
+	if (ret) {
+		dev_err(dev, "Failed to read IRQ2 register\n");
+		return ret;
+	}
+
+	if (v & GET_MASK(F_CHBATOVI)) {
+		val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		return 0;
+	}
+	if (v & GET_MASK(F_CH32MI)) {
+		val->intval = POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE;
+		return 0;
+	}
+
+	ret = regmap_read(info->regmap, RT9455_REG_IRQ3, &v);
+	if (ret) {
+		dev_err(dev, "Failed to read IRQ3 register\n");
+		return ret;
+	}
+
+	if (v & GET_MASK(F_BSTBUSOVI)) {
+		val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		return 0;
+	}
+	if (v & GET_MASK(F_BSTOLI)) {
+		val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		return 0;
+	}
+	if (v & GET_MASK(F_BSTLOWVI)) {
+		val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		return 0;
+	}
+	if (v & GET_MASK(F_BST32SI)) {
+		val->intval = POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE;
+		return 0;
+	}
+
+	ret = regmap_field_read(info->regmap_fields[F_STAT], &v);
+	if (ret) {
+		dev_err(dev, "Failed to read STAT bits\n");
+		return ret;
+	}
+
+	if (v == RT9455_FAULT) {
+		val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		return 0;
+	}
+
+	return 0;
+}
+
+static int rt9455_charger_get_battery_presence(struct rt9455_info *info,
+					       union power_supply_propval *val)
+{
+	unsigned int v;
+	int ret;
+
+	ret = regmap_field_read(info->regmap_fields[F_BATAB], &v);
+	if (ret) {
+		dev_err(&info->client->dev, "Failed to read BATAB bit\n");
+		return ret;
+	}
+
+	/*
+	 * Since BATAB is 1 when battery is NOT present and 0 otherwise,
+	 * !BATAB is returned.
+	 */
+	val->intval = !v;
+
+	return 0;
+}
+
+static int rt9455_charger_get_online(struct rt9455_info *info,
+				     union power_supply_propval *val)
+{
+	unsigned int v;
+	int ret;
+
+	ret = regmap_field_read(info->regmap_fields[F_PWR_RDY], &v);
+	if (ret) {
+		dev_err(&info->client->dev, "Failed to read PWR_RDY bit\n");
+		return ret;
+	}
+
+	val->intval = (int)v;
+
+	return 0;
+}
+
+static int rt9455_charger_get_current(struct rt9455_info *info,
+				      union power_supply_propval *val)
+{
+	int curr;
+	int ret;
+
+	ret = rt9455_get_field_val(info, F_ICHRG,
+				   rt9455_ichrg_values,
+				   ARRAY_SIZE(rt9455_ichrg_values),
+				   &curr);
+	if (ret) {
+		dev_err(&info->client->dev, "Failed to read ICHRG value\n");
+		return ret;
+	}
+
+	val->intval = curr;
+
+	return 0;
+}
+
+static int rt9455_charger_get_current_max(struct rt9455_info *info,
+					  union power_supply_propval *val)
+{
+	int idx = ARRAY_SIZE(rt9455_ichrg_values) - 1;
+
+	val->intval = rt9455_ichrg_values[idx];
+
+	return 0;
+}
+
+static int rt9455_charger_get_voltage(struct rt9455_info *info,
+				      union power_supply_propval *val)
+{
+	int voltage;
+	int ret;
+
+	ret = rt9455_get_field_val(info, F_VOREG,
+				   rt9455_voreg_values,
+				   ARRAY_SIZE(rt9455_voreg_values),
+				   &voltage);
+	if (ret) {
+		dev_err(&info->client->dev, "Failed to read VOREG value\n");
+		return ret;
+	}
+
+	val->intval = voltage;
+
+	return 0;
+}
+
+static int rt9455_charger_get_voltage_max(struct rt9455_info *info,
+					  union power_supply_propval *val)
+{
+	int idx = ARRAY_SIZE(rt9455_vmreg_values) - 1;
+
+	val->intval = rt9455_vmreg_values[idx];
+
+	return 0;
+}
+
+static int rt9455_charger_get_term_current(struct rt9455_info *info,
+					   union power_supply_propval *val)
+{
+	struct device *dev = &info->client->dev;
+	int ichrg, ieoc_percentage, ret;
+
+	ret = rt9455_get_field_val(info, F_ICHRG,
+				   rt9455_ichrg_values,
+				   ARRAY_SIZE(rt9455_ichrg_values),
+				   &ichrg);
+	if (ret) {
+		dev_err(dev, "Failed to read ICHRG value\n");
+		return ret;
+	}
+
+	ret = rt9455_get_field_val(info, F_IEOC_PERCENTAGE,
+				   rt9455_ieoc_percentage_values,
+				   ARRAY_SIZE(rt9455_ieoc_percentage_values),
+				   &ieoc_percentage);
+	if (ret) {
+		dev_err(dev, "Failed to read IEOC value\n");
+		return ret;
+	}
+
+	val->intval = ichrg * ieoc_percentage / 100;
+
+	return 0;
+}
+
+static int rt9455_charger_get_property(struct power_supply *psy,
+				       enum power_supply_property psp,
+				       union power_supply_propval *val)
+{
+	struct rt9455_info *info = power_supply_get_drvdata(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		return rt9455_charger_get_status(info, val);
+	case POWER_SUPPLY_PROP_HEALTH:
+		return rt9455_charger_get_health(info, val);
+	case POWER_SUPPLY_PROP_PRESENT:
+		return rt9455_charger_get_battery_presence(info, val);
+	case POWER_SUPPLY_PROP_ONLINE:
+		return rt9455_charger_get_online(info, val);
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT:
+		return rt9455_charger_get_current(info, val);
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_CURRENT_MAX:
+		return rt9455_charger_get_current_max(info, val);
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE:
+		return rt9455_charger_get_voltage(info, val);
+	case POWER_SUPPLY_PROP_CONSTANT_CHARGE_VOLTAGE_MAX:
+		return rt9455_charger_get_voltage_max(info, val);
+	case POWER_SUPPLY_PROP_SCOPE:
+		val->intval = POWER_SUPPLY_SCOPE_SYSTEM;
+		return 0;
+	case POWER_SUPPLY_PROP_CHARGE_TERM_CURRENT:
+		return rt9455_charger_get_term_current(info, val);
+	case POWER_SUPPLY_PROP_MODEL_NAME:
+		val->strval = RT9455_MODEL_NAME;
+		return 0;
+	case POWER_SUPPLY_PROP_MANUFACTURER:
+		val->strval = RT9455_MANUFACTURER;
+		return 0;
+	default:
+		return -ENODATA;
+	}
+}
+
+static int rt9455_hw_init(struct rt9455_info *info, u32 ichrg,
+			  u32 ieoc_percentage,
+			  u32 mivr, u32 iaicr)
+{
+	struct device *dev = &info->client->dev;
+	int idx, ret;
+
+	ret = rt9455_register_reset(info);
+	if (ret) {
+		dev_err(dev, "Power On Reset failed\n");
+		return ret;
+	}
+
+	/* Set TE bit in order to enable end of charge detection */
+	ret = regmap_field_write(info->regmap_fields[F_TE], 1);
+	if (ret) {
+		dev_err(dev, "Failed to set TE bit\n");
+		return ret;
+	}
+
+	/* Set TE_SHDN_EN bit in order to enable end of charge detection */
+	ret = regmap_field_write(info->regmap_fields[F_TE_SHDN_EN], 1);
+	if (ret) {
+		dev_err(dev, "Failed to set TE_SHDN_EN bit\n");
+		return ret;
+	}
+
+	/*
+	 * Set BATD_EN bit in order to enable battery detection
+	 * when charging is done
+	 */
+	ret = regmap_field_write(info->regmap_fields[F_BATD_EN], 1);
+	if (ret) {
+		dev_err(dev, "Failed to set BATD_EN bit\n");
+		return ret;
+	}
+
+	/*
+	 * Disable Safety Timer. In charge mode, this timer terminates charging
+	 * if no read or write via I2C is done within 32 minutes. This timer
+	 * avoids overcharging the baterry when the OS is not loaded and the
+	 * charger is connected to a power source.
+	 * In boost mode, this timer triggers BST32SI interrupt if no read or
+	 * write via I2C is done within 32 seconds.
+	 * When the OS is loaded and the charger driver is inserted, it is used
+	 * delayed_work, named max_charging_time_work, to avoid overcharging
+	 * the battery.
+	 */
+	ret = regmap_field_write(info->regmap_fields[F_TMR_EN], 0x00);
+	if (ret) {
+		dev_err(dev, "Failed to disable Safety Timer\n");
+		return ret;
+	}
+
+	/* Set ICHRG to value retrieved from device-specific data */
+	ret = rt9455_set_field_val(info, F_ICHRG,
+				   rt9455_ichrg_values,
+				   ARRAY_SIZE(rt9455_ichrg_values), ichrg);
+	if (ret) {
+		dev_err(dev, "Failed to set ICHRG value\n");
+		return ret;
+	}
+
+	/* Set IEOC Percentage to value retrieved from device-specific data */
+	ret = rt9455_set_field_val(info, F_IEOC_PERCENTAGE,
+				   rt9455_ieoc_percentage_values,
+				   ARRAY_SIZE(rt9455_ieoc_percentage_values),
+				   ieoc_percentage);
+	if (ret) {
+		dev_err(dev, "Failed to set IEOC Percentage value\n");
+		return ret;
+	}
+
+	/* Set VOREG to value retrieved from device-specific data */
+	ret = rt9455_set_field_val(info, F_VOREG,
+				   rt9455_voreg_values,
+				   ARRAY_SIZE(rt9455_voreg_values),
+				   info->voreg);
+	if (ret) {
+		dev_err(dev, "Failed to set VOREG value\n");
+		return ret;
+	}
+
+	/* Set VMREG value to maximum (4.45V). */
+	idx = ARRAY_SIZE(rt9455_vmreg_values) - 1;
+	ret = rt9455_set_field_val(info, F_VMREG,
+				   rt9455_vmreg_values,
+				   ARRAY_SIZE(rt9455_vmreg_values),
+				   rt9455_vmreg_values[idx]);
+	if (ret) {
+		dev_err(dev, "Failed to set VMREG value\n");
+		return ret;
+	}
+
+	/*
+	 * Set MIVR to value retrieved from device-specific data.
+	 * If no value is specified, default value for MIVR is 4.5V.
+	 */
+	if (mivr == -1)
+		mivr = 4500000;
+
+	ret = rt9455_set_field_val(info, F_MIVR,
+				   rt9455_mivr_values,
+				   ARRAY_SIZE(rt9455_mivr_values), mivr);
+	if (ret) {
+		dev_err(dev, "Failed to set MIVR value\n");
+		return ret;
+	}
+
+	/*
+	 * Set IAICR to value retrieved from device-specific data.
+	 * If no value is specified, default value for IAICR is 500 mA.
+	 */
+	if (iaicr == -1)
+		iaicr = 500000;
+
+	ret = rt9455_set_field_val(info, F_IAICR,
+				   rt9455_iaicr_values,
+				   ARRAY_SIZE(rt9455_iaicr_values), iaicr);
+	if (ret) {
+		dev_err(dev, "Failed to set IAICR value\n");
+		return ret;
+	}
+
+	/*
+	 * Set IAICR_INT bit so that IAICR value is determined by IAICR bits
+	 * and not by OTG pin.
+	 */
+	ret = regmap_field_write(info->regmap_fields[F_IAICR_INT], 0x01);
+	if (ret) {
+		dev_err(dev, "Failed to set IAICR_INT bit\n");
+		return ret;
+	}
+
+	/*
+	 * Disable CHMIVRI interrupt. Because the driver sets MIVR value,
+	 * CHMIVRI is triggered, but there is no action to be taken by the
+	 * driver when CHMIVRI is triggered.
+	 */
+	ret = regmap_field_write(info->regmap_fields[F_CHMIVRIM], 0x01);
+	if (ret) {
+		dev_err(dev, "Failed to mask CHMIVRI interrupt\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+#if IS_ENABLED(CONFIG_USB_PHY)
+/*
+ * Before setting the charger into boost mode, boost output voltage is
+ * set. This is needed because boost output voltage may differ from battery
+ * regulation voltage. F_VOREG bits represent either battery regulation voltage
+ * or boost output voltage, depending on the mode the charger is. Both battery
+ * regulation voltage and boost output voltage are read from DT/ACPI during
+ * probe.
+ */
+static int rt9455_set_boost_voltage_before_boost_mode(struct rt9455_info *info)
+{
+	struct device *dev = &info->client->dev;
+	int ret;
+
+	ret = rt9455_set_field_val(info, F_VOREG,
+				   rt9455_boost_voltage_values,
+				   ARRAY_SIZE(rt9455_boost_voltage_values),
+				   info->boost_voltage);
+	if (ret) {
+		dev_err(dev, "Failed to set boost output voltage value\n");
+		return ret;
+	}
+
+	return 0;
+}
+#endif
+
+/*
+ * Before setting the charger into charge mode, battery regulation voltage is
+ * set. This is needed because boost output voltage may differ from battery
+ * regulation voltage. F_VOREG bits represent either battery regulation voltage
+ * or boost output voltage, depending on the mode the charger is. Both battery
+ * regulation voltage and boost output voltage are read from DT/ACPI during
+ * probe.
+ */
+static int rt9455_set_voreg_before_charge_mode(struct rt9455_info *info)
+{
+	struct device *dev = &info->client->dev;
+	int ret;
+
+	ret = rt9455_set_field_val(info, F_VOREG,
+				   rt9455_voreg_values,
+				   ARRAY_SIZE(rt9455_voreg_values),
+				   info->voreg);
+	if (ret) {
+		dev_err(dev, "Failed to set VOREG value\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int rt9455_irq_handler_check_irq1_register(struct rt9455_info *info,
+						  bool *_is_battery_absent,
+						  bool *_alert_userspace)
+{
+	unsigned int irq1, mask1, mask2;
+	struct device *dev = &info->client->dev;
+	bool is_battery_absent = false;
+	bool alert_userspace = false;
+	int ret;
+
+	ret = regmap_read(info->regmap, RT9455_REG_IRQ1, &irq1);
+	if (ret) {
+		dev_err(dev, "Failed to read IRQ1 register\n");
+		return ret;
+	}
+
+	ret = regmap_read(info->regmap, RT9455_REG_MASK1, &mask1);
+	if (ret) {
+		dev_err(dev, "Failed to read MASK1 register\n");
+		return ret;
+	}
+
+	if (irq1 & GET_MASK(F_TSDI)) {
+		dev_err(dev, "Thermal shutdown fault occurred\n");
+		alert_userspace = true;
+	}
+
+	if (irq1 & GET_MASK(F_VINOVPI)) {
+		dev_err(dev, "Overvoltage input occurred\n");
+		alert_userspace = true;
+	}
+
+	if (irq1 & GET_MASK(F_BATAB)) {
+		dev_err(dev, "Battery absence occurred\n");
+		is_battery_absent = true;
+		alert_userspace = true;
+
+		if ((mask1 & GET_MASK(F_BATABM)) == 0) {
+			ret = regmap_field_write(info->regmap_fields[F_BATABM],
+						 0x01);
+			if (ret) {
+				dev_err(dev, "Failed to mask BATAB interrupt\n");
+				return ret;
+			}
+		}
+
+		ret = regmap_read(info->regmap, RT9455_REG_MASK2, &mask2);
+		if (ret) {
+			dev_err(dev, "Failed to read MASK2 register\n");
+			return ret;
+		}
+
+		if (mask2 & GET_MASK(F_CHTERMIM)) {
+			ret = regmap_field_write(
+				info->regmap_fields[F_CHTERMIM], 0x00);
+			if (ret) {
+				dev_err(dev, "Failed to unmask CHTERMI interrupt\n");
+				return ret;
+			}
+		}
+
+		if (mask2 & GET_MASK(F_CHRCHGIM)) {
+			ret = regmap_field_write(
+				info->regmap_fields[F_CHRCHGIM], 0x00);
+			if (ret) {
+				dev_err(dev, "Failed to unmask CHRCHGI interrupt\n");
+				return ret;
+			}
+		}
+
+		/*
+		 * When the battery is absent, max_charging_time_work is
+		 * cancelled, since no charging is done.
+		 */
+		cancel_delayed_work_sync(&info->max_charging_time_work);
+		/*
+		 * Since no interrupt is triggered when the battery is
+		 * reconnected, max_charging_time_work is not rescheduled.
+		 * Therefore, batt_presence_work is scheduled to check whether
+		 * the battery is still absent or not.
+		 */
+		queue_delayed_work(system_power_efficient_wq,
+				   &info->batt_presence_work,
+				   RT9455_BATT_PRESENCE_DELAY * HZ);
+	}
+
+	*_is_battery_absent = is_battery_absent;
+
+	if (alert_userspace)
+		*_alert_userspace = alert_userspace;
+
+	return 0;
+}
+
+static int rt9455_irq_handler_check_irq2_register(struct rt9455_info *info,
+						  bool is_battery_absent,
+						  bool *_alert_userspace)
+{
+	unsigned int irq2, mask2;
+	struct device *dev = &info->client->dev;
+	bool alert_userspace = false;
+	int ret;
+
+	ret = regmap_read(info->regmap, RT9455_REG_IRQ2, &irq2);
+	if (ret) {
+		dev_err(dev, "Failed to read IRQ2 register\n");
+		return ret;
+	}
+
+	ret = regmap_read(info->regmap, RT9455_REG_MASK2, &mask2);
+	if (ret) {
+		dev_err(dev, "Failed to read MASK2 register\n");
+		return ret;
+	}
+
+	if (irq2 & GET_MASK(F_CHRVPI)) {
+		dev_dbg(dev, "Charger fault occurred\n");
+		alert_userspace = true;
+		/*
+		 * CHRVPI bit is set in 2 cases:
+		 * 1. when the power source is connected to the charger.
+		 * 2. when the power source is disconnected from the charger.
+		 * To identify the case, PWR_RDY bit is checked. Because
+		 * PWR_RDY bit is set / cleared after CHRVPI interrupt is
+		 * triggered, it is used delayed_work to later read PWR_RDY bit.
+		 */
+		queue_delayed_work(system_power_efficient_wq,
+				   &info->pwr_rdy_work,
+				   RT9455_PWR_RDY_DELAY * HZ);
+	}
+	if (irq2 & GET_MASK(F_CHBATOVI)) {
+		dev_err(dev, "Battery OVP occurred\n");
+		alert_userspace = true;
+	}
+	if (irq2 & GET_MASK(F_CHTERMI)) {
+		dev_dbg(dev, "Charge terminated\n");
+		if (!is_battery_absent) {
+			if ((mask2 & GET_MASK(F_CHTERMIM)) == 0) {
+				ret = regmap_field_write(
+					info->regmap_fields[F_CHTERMIM], 0x01);
+				if (ret) {
+					dev_err(dev, "Failed to mask CHTERMI interrupt\n");
+					return ret;
+				}
+				/*
+				 * Update MASK2 value, since CHTERMIM bit is
+				 * set.
+				 */
+				mask2 = mask2 | GET_MASK(F_CHTERMIM);
+			}
+			cancel_delayed_work_sync(&info->max_charging_time_work);
+			alert_userspace = true;
+		}
+	}
+	if (irq2 & GET_MASK(F_CHRCHGI)) {
+		dev_dbg(dev, "Recharge request\n");
+		ret = regmap_field_write(info->regmap_fields[F_CHG_EN],
+					 RT9455_CHARGE_ENABLE);
+		if (ret) {
+			dev_err(dev, "Failed to enable charging\n");
+			return ret;
+		}
+		if (mask2 & GET_MASK(F_CHTERMIM)) {
+			ret = regmap_field_write(
+				info->regmap_fields[F_CHTERMIM], 0x00);
+			if (ret) {
+				dev_err(dev, "Failed to unmask CHTERMI interrupt\n");
+				return ret;
+			}
+			/* Update MASK2 value, since CHTERMIM bit is cleared. */
+			mask2 = mask2 & ~GET_MASK(F_CHTERMIM);
+		}
+		if (!is_battery_absent) {
+			/*
+			 * No need to check whether the charger is connected to
+			 * power source when CHRCHGI is received, since CHRCHGI
+			 * is not triggered if the charger is not connected to
+			 * the power source.
+			 */
+			queue_delayed_work(system_power_efficient_wq,
+					   &info->max_charging_time_work,
+					   RT9455_MAX_CHARGING_TIME * HZ);
+			alert_userspace = true;
+		}
+	}
+	if (irq2 & GET_MASK(F_CH32MI)) {
+		dev_err(dev, "Charger fault. 32 mins timeout occurred\n");
+		alert_userspace = true;
+	}
+	if (irq2 & GET_MASK(F_CHTREGI)) {
+		dev_warn(dev,
+			 "Charger warning. Thermal regulation loop active\n");
+		alert_userspace = true;
+	}
+	if (irq2 & GET_MASK(F_CHMIVRI)) {
+		dev_dbg(dev,
+			"Charger warning. Input voltage MIVR loop active\n");
+	}
+
+	if (alert_userspace)
+		*_alert_userspace = alert_userspace;
+
+	return 0;
+}
+
+static int rt9455_irq_handler_check_irq3_register(struct rt9455_info *info,
+						  bool *_alert_userspace)
+{
+	unsigned int irq3, mask3;
+	struct device *dev = &info->client->dev;
+	bool alert_userspace = false;
+	int ret;
+
+	ret = regmap_read(info->regmap, RT9455_REG_IRQ3, &irq3);
+	if (ret) {
+		dev_err(dev, "Failed to read IRQ3 register\n");
+		return ret;
+	}
+
+	ret = regmap_read(info->regmap, RT9455_REG_MASK3, &mask3);
+	if (ret) {
+		dev_err(dev, "Failed to read MASK3 register\n");
+		return ret;
+	}
+
+	if (irq3 & GET_MASK(F_BSTBUSOVI)) {
+		dev_err(dev, "Boost fault. Overvoltage input occurred\n");
+		alert_userspace = true;
+	}
+	if (irq3 & GET_MASK(F_BSTOLI)) {
+		dev_err(dev, "Boost fault. Overload\n");
+		alert_userspace = true;
+	}
+	if (irq3 & GET_MASK(F_BSTLOWVI)) {
+		dev_err(dev, "Boost fault. Battery voltage too low\n");
+		alert_userspace = true;
+	}
+	if (irq3 & GET_MASK(F_BST32SI)) {
+		dev_err(dev, "Boost fault. 32 seconds timeout occurred.\n");
+		alert_userspace = true;
+	}
+
+	if (alert_userspace) {
+		dev_info(dev, "Boost fault occurred, therefore the charger goes into charge mode\n");
+		ret = rt9455_set_voreg_before_charge_mode(info);
+		if (ret) {
+			dev_err(dev, "Failed to set VOREG before entering charge mode\n");
+			return ret;
+		}
+		ret = regmap_field_write(info->regmap_fields[F_OPA_MODE],
+					 RT9455_CHARGE_MODE);
+		if (ret) {
+			dev_err(dev, "Failed to set charger in charge mode\n");
+			return ret;
+		}
+		*_alert_userspace = alert_userspace;
+	}
+
+	return 0;
+}
+
+static irqreturn_t rt9455_irq_handler_thread(int irq, void *data)
+{
+	struct rt9455_info *info = data;
+	struct device *dev;
+	bool alert_userspace = false;
+	bool is_battery_absent = false;
+	unsigned int status;
+	int ret;
+
+	if (!info)
+		return IRQ_NONE;
+
+	dev = &info->client->dev;
+
+	if (irq != info->client->irq) {
+		dev_err(dev, "Interrupt is not for RT9455 charger\n");
+		return IRQ_NONE;
+	}
+
+	ret = regmap_field_read(info->regmap_fields[F_STAT], &status);
+	if (ret) {
+		dev_err(dev, "Failed to read STAT bits\n");
+		return IRQ_HANDLED;
+	}
+	dev_dbg(dev, "Charger status is %d\n", status);
+
+	/*
+	 * Each function that processes an IRQ register receives as output
+	 * parameter alert_userspace pointer. alert_userspace is set to true
+	 * in such a function only if an interrupt has occurred in the
+	 * respective interrupt register. This way, it is avoided the following
+	 * case: interrupt occurs only in IRQ1 register,
+	 * rt9455_irq_handler_check_irq1_register() function sets to true
+	 * alert_userspace, but rt9455_irq_handler_check_irq2_register()
+	 * and rt9455_irq_handler_check_irq3_register() functions set to false
+	 * alert_userspace and power_supply_changed() is never called.
+	 */
+	ret = rt9455_irq_handler_check_irq1_register(info, &is_battery_absent,
+						     &alert_userspace);
+	if (ret) {
+		dev_err(dev, "Failed to handle IRQ1 register\n");
+		return IRQ_HANDLED;
+	}
+
+	ret = rt9455_irq_handler_check_irq2_register(info, is_battery_absent,
+						     &alert_userspace);
+	if (ret) {
+		dev_err(dev, "Failed to handle IRQ2 register\n");
+		return IRQ_HANDLED;
+	}
+
+	ret = rt9455_irq_handler_check_irq3_register(info, &alert_userspace);
+	if (ret) {
+		dev_err(dev, "Failed to handle IRQ3 register\n");
+		return IRQ_HANDLED;
+	}
+
+	if (alert_userspace) {
+		/*
+		 * Sometimes, an interrupt occurs while rt9455_probe() function
+		 * is executing and power_supply_register() is not yet called.
+		 * Do not call power_supply_charged() in this case.
+		 */
+		if (info->charger)
+			power_supply_changed(info->charger);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int rt9455_discover_charger(struct rt9455_info *info, u32 *ichrg,
+				   u32 *ieoc_percentage,
+				   u32 *mivr, u32 *iaicr)
+{
+	struct device *dev = &info->client->dev;
+	int ret;
+
+	if (!dev->of_node && !ACPI_HANDLE(dev)) {
+		dev_err(dev, "No support for either device tree or ACPI\n");
+		return -EINVAL;
+	}
+	/*
+	 * ICHRG, IEOC_PERCENTAGE, VOREG and boost output voltage are mandatory
+	 * parameters.
+	 */
+	ret = device_property_read_u32(dev, "richtek,output-charge-current",
+				       ichrg);
+	if (ret) {
+		dev_err(dev, "Error: missing \"output-charge-current\" property\n");
+		return ret;
+	}
+
+	ret = device_property_read_u32(dev, "richtek,end-of-charge-percentage",
+				       ieoc_percentage);
+	if (ret) {
+		dev_err(dev, "Error: missing \"end-of-charge-percentage\" property\n");
+		return ret;
+	}
+
+	ret = device_property_read_u32(dev,
+				       "richtek,battery-regulation-voltage",
+				       &info->voreg);
+	if (ret) {
+		dev_err(dev, "Error: missing \"battery-regulation-voltage\" property\n");
+		return ret;
+	}
+
+	ret = device_property_read_u32(dev, "richtek,boost-output-voltage",
+				       &info->boost_voltage);
+	if (ret) {
+		dev_err(dev, "Error: missing \"boost-output-voltage\" property\n");
+		return ret;
+	}
+
+	/*
+	 * MIVR and IAICR are optional parameters. Do not return error if one of
+	 * them is not present in ACPI table or device tree specification.
+	 */
+	device_property_read_u32(dev, "richtek,min-input-voltage-regulation",
+				 mivr);
+	device_property_read_u32(dev, "richtek,avg-input-current-regulation",
+				 iaicr);
+
+	return 0;
+}
+
+#if IS_ENABLED(CONFIG_USB_PHY)
+static int rt9455_usb_event_none(struct rt9455_info *info,
+				 u8 opa_mode, u8 iaicr)
+{
+	struct device *dev = &info->client->dev;
+	int ret;
+
+	if (opa_mode == RT9455_BOOST_MODE) {
+		ret = rt9455_set_voreg_before_charge_mode(info);
+		if (ret) {
+			dev_err(dev, "Failed to set VOREG before entering charge mode\n");
+			return ret;
+		}
+		/*
+		 * If the charger is in boost mode, and it has received
+		 * USB_EVENT_NONE, this means the consumer device powered by the
+		 * charger is not connected anymore.
+		 * In this case, the charger goes into charge mode.
+		 */
+		dev_dbg(dev, "USB_EVENT_NONE received, therefore the charger goes into charge mode\n");
+		ret = regmap_field_write(info->regmap_fields[F_OPA_MODE],
+					 RT9455_CHARGE_MODE);
+		if (ret) {
+			dev_err(dev, "Failed to set charger in charge mode\n");
+			return NOTIFY_DONE;
+		}
+	}
+
+	dev_dbg(dev, "USB_EVENT_NONE received, therefore IAICR is set to its minimum value\n");
+	if (iaicr != RT9455_IAICR_100MA) {
+		ret = regmap_field_write(info->regmap_fields[F_IAICR],
+					 RT9455_IAICR_100MA);
+		if (ret) {
+			dev_err(dev, "Failed to set IAICR value\n");
+			return NOTIFY_DONE;
+		}
+	}
+
+	return NOTIFY_OK;
+}
+
+static int rt9455_usb_event_vbus(struct rt9455_info *info,
+				 u8 opa_mode, u8 iaicr)
+{
+	struct device *dev = &info->client->dev;
+	int ret;
+
+	if (opa_mode == RT9455_BOOST_MODE) {
+		ret = rt9455_set_voreg_before_charge_mode(info);
+		if (ret) {
+			dev_err(dev, "Failed to set VOREG before entering charge mode\n");
+			return ret;
+		}
+		/*
+		 * If the charger is in boost mode, and it has received
+		 * USB_EVENT_VBUS, this means the consumer device powered by the
+		 * charger is not connected anymore.
+		 * In this case, the charger goes into charge mode.
+		 */
+		dev_dbg(dev, "USB_EVENT_VBUS received, therefore the charger goes into charge mode\n");
+		ret = regmap_field_write(info->regmap_fields[F_OPA_MODE],
+					 RT9455_CHARGE_MODE);
+		if (ret) {
+			dev_err(dev, "Failed to set charger in charge mode\n");
+			return NOTIFY_DONE;
+		}
+	}
+
+	dev_dbg(dev, "USB_EVENT_VBUS received, therefore IAICR is set to 500 mA\n");
+	if (iaicr != RT9455_IAICR_500MA) {
+		ret = regmap_field_write(info->regmap_fields[F_IAICR],
+					 RT9455_IAICR_500MA);
+		if (ret) {
+			dev_err(dev, "Failed to set IAICR value\n");
+			return NOTIFY_DONE;
+		}
+	}
+
+	return NOTIFY_OK;
+}
+
+static int rt9455_usb_event_id(struct rt9455_info *info,
+			       u8 opa_mode, u8 iaicr)
+{
+	struct device *dev = &info->client->dev;
+	int ret;
+
+	if (opa_mode == RT9455_CHARGE_MODE) {
+		ret = rt9455_set_boost_voltage_before_boost_mode(info);
+		if (ret) {
+			dev_err(dev, "Failed to set boost output voltage before entering boost mode\n");
+			return ret;
+		}
+		/*
+		 * If the charger is in charge mode, and it has received
+		 * USB_EVENT_ID, this means a consumer device is connected and
+		 * it should be powered by the charger.
+		 * In this case, the charger goes into boost mode.
+		 */
+		dev_dbg(dev, "USB_EVENT_ID received, therefore the charger goes into boost mode\n");
+		ret = regmap_field_write(info->regmap_fields[F_OPA_MODE],
+					 RT9455_BOOST_MODE);
+		if (ret) {
+			dev_err(dev, "Failed to set charger in boost mode\n");
+			return NOTIFY_DONE;
+		}
+	}
+
+	dev_dbg(dev, "USB_EVENT_ID received, therefore IAICR is set to its minimum value\n");
+	if (iaicr != RT9455_IAICR_100MA) {
+		ret = regmap_field_write(info->regmap_fields[F_IAICR],
+					 RT9455_IAICR_100MA);
+		if (ret) {
+			dev_err(dev, "Failed to set IAICR value\n");
+			return NOTIFY_DONE;
+		}
+	}
+
+	return NOTIFY_OK;
+}
+
+static int rt9455_usb_event_charger(struct rt9455_info *info,
+				    u8 opa_mode, u8 iaicr)
+{
+	struct device *dev = &info->client->dev;
+	int ret;
+
+	if (opa_mode == RT9455_BOOST_MODE) {
+		ret = rt9455_set_voreg_before_charge_mode(info);
+		if (ret) {
+			dev_err(dev, "Failed to set VOREG before entering charge mode\n");
+			return ret;
+		}
+		/*
+		 * If the charger is in boost mode, and it has received
+		 * USB_EVENT_CHARGER, this means the consumer device powered by
+		 * the charger is not connected anymore.
+		 * In this case, the charger goes into charge mode.
+		 */
+		dev_dbg(dev, "USB_EVENT_CHARGER received, therefore the charger goes into charge mode\n");
+		ret = regmap_field_write(info->regmap_fields[F_OPA_MODE],
+					 RT9455_CHARGE_MODE);
+		if (ret) {
+			dev_err(dev, "Failed to set charger in charge mode\n");
+			return NOTIFY_DONE;
+		}
+	}
+
+	dev_dbg(dev, "USB_EVENT_CHARGER received, therefore IAICR is set to no current limit\n");
+	if (iaicr != RT9455_IAICR_NO_LIMIT) {
+		ret = regmap_field_write(info->regmap_fields[F_IAICR],
+					 RT9455_IAICR_NO_LIMIT);
+		if (ret) {
+			dev_err(dev, "Failed to set IAICR value\n");
+			return NOTIFY_DONE;
+		}
+	}
+
+	return NOTIFY_OK;
+}
+
+static int rt9455_usb_event(struct notifier_block *nb,
+			    unsigned long event, void *power)
+{
+	struct rt9455_info *info = container_of(nb, struct rt9455_info, nb);
+	struct device *dev = &info->client->dev;
+	unsigned int opa_mode, iaicr;
+	int ret;
+
+	/*
+	 * Determine whether the charger is in charge mode
+	 * or in boost mode.
+	 */
+	ret = regmap_field_read(info->regmap_fields[F_OPA_MODE],
+				&opa_mode);
+	if (ret) {
+		dev_err(dev, "Failed to read OPA_MODE value\n");
+		return NOTIFY_DONE;
+	}
+
+	ret = regmap_field_read(info->regmap_fields[F_IAICR],
+				&iaicr);
+	if (ret) {
+		dev_err(dev, "Failed to read IAICR value\n");
+		return NOTIFY_DONE;
+	}
+
+	dev_dbg(dev, "Received USB event %lu\n", event);
+	switch (event) {
+	case USB_EVENT_NONE:
+		return rt9455_usb_event_none(info, opa_mode, iaicr);
+	case USB_EVENT_VBUS:
+		return rt9455_usb_event_vbus(info, opa_mode, iaicr);
+	case USB_EVENT_ID:
+		return rt9455_usb_event_id(info, opa_mode, iaicr);
+	case USB_EVENT_CHARGER:
+		return rt9455_usb_event_charger(info, opa_mode, iaicr);
+	default:
+		dev_err(dev, "Unknown USB event\n");
+	}
+	return NOTIFY_DONE;
+}
+#endif
+
+static void rt9455_pwr_rdy_work_callback(struct work_struct *work)
+{
+	struct rt9455_info *info = container_of(work, struct rt9455_info,
+						pwr_rdy_work.work);
+	struct device *dev = &info->client->dev;
+	unsigned int pwr_rdy;
+	int ret;
+
+	ret = regmap_field_read(info->regmap_fields[F_PWR_RDY], &pwr_rdy);
+	if (ret) {
+		dev_err(dev, "Failed to read PWR_RDY bit\n");
+		return;
+	}
+	switch (pwr_rdy) {
+	case RT9455_PWR_FAULT:
+		dev_dbg(dev, "Charger disconnected from power source\n");
+		cancel_delayed_work_sync(&info->max_charging_time_work);
+		break;
+	case RT9455_PWR_GOOD:
+		dev_dbg(dev, "Charger connected to power source\n");
+		ret = regmap_field_write(info->regmap_fields[F_CHG_EN],
+					 RT9455_CHARGE_ENABLE);
+		if (ret) {
+			dev_err(dev, "Failed to enable charging\n");
+			return;
+		}
+		queue_delayed_work(system_power_efficient_wq,
+				   &info->max_charging_time_work,
+				   RT9455_MAX_CHARGING_TIME * HZ);
+		break;
+	}
+}
+
+static void rt9455_max_charging_time_work_callback(struct work_struct *work)
+{
+	struct rt9455_info *info = container_of(work, struct rt9455_info,
+						max_charging_time_work.work);
+	struct device *dev = &info->client->dev;
+	int ret;
+
+	dev_err(dev, "Battery has been charging for at least 6 hours and is not yet fully charged. Battery is dead, therefore charging is disabled.\n");
+	ret = regmap_field_write(info->regmap_fields[F_CHG_EN],
+				 RT9455_CHARGE_DISABLE);
+	if (ret)
+		dev_err(dev, "Failed to disable charging\n");
+}
+
+static void rt9455_batt_presence_work_callback(struct work_struct *work)
+{
+	struct rt9455_info *info = container_of(work, struct rt9455_info,
+						batt_presence_work.work);
+	struct device *dev = &info->client->dev;
+	unsigned int irq1, mask1;
+	int ret;
+
+	ret = regmap_read(info->regmap, RT9455_REG_IRQ1, &irq1);
+	if (ret) {
+		dev_err(dev, "Failed to read IRQ1 register\n");
+		return;
+	}
+
+	/*
+	 * If the battery is still absent, batt_presence_work is rescheduled.
+	 * Otherwise, max_charging_time is scheduled.
+	 */
+	if (irq1 & GET_MASK(F_BATAB)) {
+		queue_delayed_work(system_power_efficient_wq,
+				   &info->batt_presence_work,
+				   RT9455_BATT_PRESENCE_DELAY * HZ);
+	} else {
+		queue_delayed_work(system_power_efficient_wq,
+				   &info->max_charging_time_work,
+				   RT9455_MAX_CHARGING_TIME * HZ);
+
+		ret = regmap_read(info->regmap, RT9455_REG_MASK1, &mask1);
+		if (ret) {
+			dev_err(dev, "Failed to read MASK1 register\n");
+			return;
+		}
+
+		if (mask1 & GET_MASK(F_BATABM)) {
+			ret = regmap_field_write(info->regmap_fields[F_BATABM],
+						 0x00);
+			if (ret)
+				dev_err(dev, "Failed to unmask BATAB interrupt\n");
+		}
+	}
+}
+
+static const struct power_supply_desc rt9455_charger_desc = {
+	.name			= RT9455_DRIVER_NAME,
+	.type			= POWER_SUPPLY_TYPE_USB,
+	.properties		= rt9455_charger_properties,
+	.num_properties		= ARRAY_SIZE(rt9455_charger_properties),
+	.get_property		= rt9455_charger_get_property,
+};
+
+static bool rt9455_is_writeable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case RT9455_REG_DEV_ID:
+	case RT9455_REG_IRQ1:
+	case RT9455_REG_IRQ2:
+	case RT9455_REG_IRQ3:
+		return false;
+	default:
+		return true;
+	}
+}
+
+static bool rt9455_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case RT9455_REG_DEV_ID:
+	case RT9455_REG_CTRL5:
+	case RT9455_REG_CTRL6:
+		return false;
+	default:
+		return true;
+	}
+}
+
+static const struct regmap_config rt9455_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.writeable_reg	= rt9455_is_writeable_reg,
+	.volatile_reg	= rt9455_is_volatile_reg,
+	.max_register	= RT9455_REG_MASK3,
+	.cache_type	= REGCACHE_RBTREE,
+};
+
+static int rt9455_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct device *dev = &client->dev;
+	struct rt9455_info *info;
+	struct power_supply_config rt9455_charger_config = {};
+	/*
+	 * Mandatory device-specific data values. Also, VOREG and boost output
+	 * voltage are mandatory values, but they are stored in rt9455_info
+	 * structure.
+	 */
+	u32 ichrg, ieoc_percentage;
+	/* Optional device-specific data values. */
+	u32 mivr = -1, iaicr = -1;
+	int i, ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_err(dev, "No support for SMBUS_BYTE_DATA\n");
+		return -ENODEV;
+	}
+	info = devm_kzalloc(dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	info->client = client;
+	i2c_set_clientdata(client, info);
+
+	info->regmap = devm_regmap_init_i2c(client,
+					    &rt9455_regmap_config);
+	if (IS_ERR(info->regmap)) {
+		dev_err(dev, "Failed to initialize register map\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < F_MAX_FIELDS; i++) {
+		info->regmap_fields[i] =
+			devm_regmap_field_alloc(dev, info->regmap,
+						rt9455_reg_fields[i]);
+		if (IS_ERR(info->regmap_fields[i])) {
+			dev_err(dev,
+				"Failed to allocate regmap field = %d\n", i);
+			return PTR_ERR(info->regmap_fields[i]);
+		}
+	}
+
+	ret = rt9455_discover_charger(info, &ichrg, &ieoc_percentage,
+				      &mivr, &iaicr);
+	if (ret) {
+		dev_err(dev, "Failed to discover charger\n");
+		return ret;
+	}
+
+#if IS_ENABLED(CONFIG_USB_PHY)
+	info->usb_phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
+	if (IS_ERR(info->usb_phy)) {
+		dev_err(dev, "Failed to get USB transceiver\n");
+	} else {
+		info->nb.notifier_call = rt9455_usb_event;
+		ret = usb_register_notifier(info->usb_phy, &info->nb);
+		if (ret) {
+			dev_err(dev, "Failed to register USB notifier\n");
+			/*
+			 * If usb_register_notifier() fails, set notifier_call
+			 * to NULL, to avoid calling usb_unregister_notifier().
+			 */
+			info->nb.notifier_call = NULL;
+		}
+	}
+#endif
+
+	INIT_DEFERRABLE_WORK(&info->pwr_rdy_work, rt9455_pwr_rdy_work_callback);
+	INIT_DEFERRABLE_WORK(&info->max_charging_time_work,
+			     rt9455_max_charging_time_work_callback);
+	INIT_DEFERRABLE_WORK(&info->batt_presence_work,
+			     rt9455_batt_presence_work_callback);
+
+	rt9455_charger_config.of_node		= dev->of_node;
+	rt9455_charger_config.drv_data		= info;
+	rt9455_charger_config.supplied_to	= rt9455_charger_supplied_to;
+	rt9455_charger_config.num_supplicants	=
+					ARRAY_SIZE(rt9455_charger_supplied_to);
+	ret = devm_request_threaded_irq(dev, client->irq, NULL,
+					rt9455_irq_handler_thread,
+					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					RT9455_DRIVER_NAME, info);
+	if (ret) {
+		dev_err(dev, "Failed to register IRQ handler\n");
+		goto put_usb_notifier;
+	}
+
+	ret = rt9455_hw_init(info, ichrg, ieoc_percentage, mivr, iaicr);
+	if (ret) {
+		dev_err(dev, "Failed to set charger to its default values\n");
+		goto put_usb_notifier;
+	}
+
+	info->charger = devm_power_supply_register(dev, &rt9455_charger_desc,
+						   &rt9455_charger_config);
+	if (IS_ERR(info->charger)) {
+		dev_err(dev, "Failed to register charger\n");
+		ret = PTR_ERR(info->charger);
+		goto put_usb_notifier;
+	}
+
+	return 0;
+
+put_usb_notifier:
+#if IS_ENABLED(CONFIG_USB_PHY)
+	if (info->nb.notifier_call)  {
+		usb_unregister_notifier(info->usb_phy, &info->nb);
+		info->nb.notifier_call = NULL;
+	}
+#endif
+	return ret;
+}
+
+static int rt9455_remove(struct i2c_client *client)
+{
+	int ret;
+	struct rt9455_info *info = i2c_get_clientdata(client);
+
+	ret = rt9455_register_reset(info);
+	if (ret)
+		dev_err(&info->client->dev, "Failed to set charger to its default values\n");
+
+#if IS_ENABLED(CONFIG_USB_PHY)
+	if (info->nb.notifier_call)
+		usb_unregister_notifier(info->usb_phy, &info->nb);
+#endif
+
+	cancel_delayed_work_sync(&info->pwr_rdy_work);
+	cancel_delayed_work_sync(&info->max_charging_time_work);
+	cancel_delayed_work_sync(&info->batt_presence_work);
+
+	return ret;
+}
+
+static const struct i2c_device_id rt9455_i2c_id_table[] = {
+	{ RT9455_DRIVER_NAME, 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, rt9455_i2c_id_table);
+
+static const struct of_device_id rt9455_of_match[] = {
+	{ .compatible = "richtek,rt9455", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, rt9455_of_match);
+
+static const struct acpi_device_id rt9455_i2c_acpi_match[] = {
+	{ "RT945500", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, rt9455_i2c_acpi_match);
+
+static struct i2c_driver rt9455_driver = {
+	.probe		= rt9455_probe,
+	.remove		= rt9455_remove,
+	.id_table	= rt9455_i2c_id_table,
+	.driver = {
+		.name		= RT9455_DRIVER_NAME,
+		.of_match_table	= of_match_ptr(rt9455_of_match),
+		.acpi_match_table = ACPI_PTR(rt9455_i2c_acpi_match),
+	},
+};
+module_i2c_driver(rt9455_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Anda-Maria Nicolae <anda-maria.nicolae@intel.com>");
+MODULE_ALIAS("i2c:rt9455-charger");
+MODULE_DESCRIPTION("Richtek RT9455 Charger Driver");
diff --git a/drivers/power/sbs-battery.c b/drivers/power/sbs-battery.c
index de1178659d4b..d6226d68b574 100644
--- a/drivers/power/sbs-battery.c
+++ b/drivers/power/sbs-battery.c
@@ -28,6 +28,7 @@
 #include <linux/interrupt.h>
 #include <linux/gpio.h>
 #include <linux/of.h>
+#include <linux/stat.h>
 
 #include <linux/power/sbs-battery.h>
 
@@ -170,6 +171,7 @@ struct sbs_info {
 
 static char model_name[I2C_SMBUS_BLOCK_MAX + 1];
 static char manufacturer[I2C_SMBUS_BLOCK_MAX + 1];
+static bool force_load;
 
 static int sbs_read_word_data(struct i2c_client *client, u8 address)
 {
@@ -885,14 +887,17 @@ static int sbs_probe(struct i2c_client *client,
 
 skip_gpio:
 	/*
-	 * Before we register, we need to make sure we can actually talk
+	 * Before we register, we might need to make sure we can actually talk
 	 * to the battery.
 	 */
-	rc = sbs_read_word_data(client, sbs_data[REG_STATUS].addr);
-	if (rc < 0) {
-		dev_err(&client->dev, "%s: Failed to get device status\n",
-			__func__);
-		goto exit_psupply;
+	if (!force_load) {
+		rc = sbs_read_word_data(client, sbs_data[REG_STATUS].addr);
+
+		if (rc < 0) {
+			dev_err(&client->dev, "%s: Failed to get device status\n",
+				__func__);
+			goto exit_psupply;
+		}
 	}
 
 	chip->power_supply = power_supply_register(&client->dev, sbs_desc,
@@ -991,3 +996,7 @@ module_i2c_driver(sbs_battery_driver);
 
 MODULE_DESCRIPTION("SBS battery monitor driver");
 MODULE_LICENSE("GPL");
+
+module_param(force_load, bool, S_IRUSR | S_IRGRP | S_IROTH);
+MODULE_PARM_DESC(force_load,
+		 "Attempt to load the driver even if no battery is connected");
diff --git a/drivers/power/test_power.c b/drivers/power/test_power.c
index f986e0cca7ac..83c42ea88f2b 100644
--- a/drivers/power/test_power.c
+++ b/drivers/power/test_power.c
@@ -448,42 +448,42 @@ static int param_set_battery_voltage(const char *key,
 
 #define param_get_battery_voltage param_get_int
 
-static struct kernel_param_ops param_ops_ac_online = {
+static const struct kernel_param_ops param_ops_ac_online = {
 	.set = param_set_ac_online,
 	.get = param_get_ac_online,
 };
 
-static struct kernel_param_ops param_ops_usb_online = {
+static const struct kernel_param_ops param_ops_usb_online = {
 	.set = param_set_usb_online,
 	.get = param_get_usb_online,
 };
 
-static struct kernel_param_ops param_ops_battery_status = {
+static const struct kernel_param_ops param_ops_battery_status = {
 	.set = param_set_battery_status,
 	.get = param_get_battery_status,
 };
 
-static struct kernel_param_ops param_ops_battery_present = {
+static const struct kernel_param_ops param_ops_battery_present = {
 	.set = param_set_battery_present,
 	.get = param_get_battery_present,
 };
 
-static struct kernel_param_ops param_ops_battery_technology = {
+static const struct kernel_param_ops param_ops_battery_technology = {
 	.set = param_set_battery_technology,
 	.get = param_get_battery_technology,
 };
 
-static struct kernel_param_ops param_ops_battery_health = {
+static const struct kernel_param_ops param_ops_battery_health = {
 	.set = param_set_battery_health,
 	.get = param_get_battery_health,
 };
 
-static struct kernel_param_ops param_ops_battery_capacity = {
+static const struct kernel_param_ops param_ops_battery_capacity = {
 	.set = param_set_battery_capacity,
 	.get = param_get_battery_capacity,
 };
 
-static struct kernel_param_ops param_ops_battery_voltage = {
+static const struct kernel_param_ops param_ops_battery_voltage = {
 	.set = param_set_battery_voltage,
 	.get = param_get_battery_voltage,
 };
diff --git a/drivers/power/twl4030_charger.c b/drivers/power/twl4030_charger.c
index 02a522cb7753..022b8910e443 100644
--- a/drivers/power/twl4030_charger.c
+++ b/drivers/power/twl4030_charger.c
@@ -638,10 +638,15 @@ static int __init twl4030_bci_probe(struct platform_device *pdev)
 
 	INIT_WORK(&bci->work, twl4030_bci_usb_work);
 
-	bci->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
-	if (!IS_ERR_OR_NULL(bci->transceiver)) {
-		bci->usb_nb.notifier_call = twl4030_bci_usb_ncb;
-		usb_register_notifier(bci->transceiver, &bci->usb_nb);
+	bci->usb_nb.notifier_call = twl4030_bci_usb_ncb;
+	if (bci->dev->of_node) {
+		struct device_node *phynode;
+
+		phynode = of_find_compatible_node(bci->dev->of_node->parent,
+						  NULL, "ti,twl4030-usb");
+		if (phynode)
+			bci->transceiver = devm_usb_get_phy_by_node(
+				bci->dev, phynode, &bci->usb_nb);
 	}
 
 	/* Enable interrupts now. */
@@ -671,10 +676,6 @@ static int __init twl4030_bci_probe(struct platform_device *pdev)
 	return 0;
 
 fail_unmask_interrupts:
-	if (!IS_ERR_OR_NULL(bci->transceiver)) {
-		usb_unregister_notifier(bci->transceiver, &bci->usb_nb);
-		usb_put_phy(bci->transceiver);
-	}
 	free_irq(bci->irq_bci, bci);
 fail_bci_irq:
 	free_irq(bci->irq_chg, bci);
@@ -703,10 +704,6 @@ static int __exit twl4030_bci_remove(struct platform_device *pdev)
 	twl_i2c_write_u8(TWL4030_MODULE_INTERRUPTS, 0xff,
 			 TWL4030_INTERRUPTS_BCIIMR2A);
 
-	if (!IS_ERR_OR_NULL(bci->transceiver)) {
-		usb_unregister_notifier(bci->transceiver, &bci->usb_nb);
-		usb_put_phy(bci->transceiver);
-	}
 	free_irq(bci->irq_bci, bci);
 	free_irq(bci->irq_chg, bci);
 	power_supply_unregister(bci->usb);
diff --git a/drivers/power/wm831x_power.c b/drivers/power/wm831x_power.c
index 0161bdabd5a3..db11ae6599f3 100644
--- a/drivers/power/wm831x_power.c
+++ b/drivers/power/wm831x_power.c
@@ -609,6 +609,7 @@ static int wm831x_power_probe(struct platform_device *pdev)
 	return ret;
 
 err_bat_irq:
+	--i;
 	for (; i >= 0; i--) {
 		irq = platform_get_irq_byname(pdev, wm831x_bat_irqs[i]);
 		free_irq(irq, power);
diff --git a/drivers/powercap/intel_rapl.c b/drivers/powercap/intel_rapl.c
index fd243231620a..482b22ddc7b2 100644
--- a/drivers/powercap/intel_rapl.c
+++ b/drivers/powercap/intel_rapl.c
@@ -187,6 +187,7 @@ struct rapl_package {
 };
 
 struct rapl_defaults {
+	u8 floor_freq_reg_addr;
 	int (*check_unit)(struct rapl_package *rp, int cpu);
 	void (*set_floor_freq)(struct rapl_domain *rd, bool mode);
 	u64 (*compute_time_window)(struct rapl_package *rp, u64 val,
@@ -196,7 +197,8 @@ struct rapl_defaults {
 static struct rapl_defaults *rapl_defaults;
 
 /* Sideband MBI registers */
-#define IOSF_CPU_POWER_BUDGET_CTL (0x2)
+#define IOSF_CPU_POWER_BUDGET_CTL_BYT (0x2)
+#define IOSF_CPU_POWER_BUDGET_CTL_TNG (0xdf)
 
 #define PACKAGE_PLN_INT_SAVED   BIT(0)
 #define MAX_PRIM_NAME (32)
@@ -358,7 +360,8 @@ static int set_domain_enable(struct powercap_zone *power_zone, bool mode)
 
 	get_online_cpus();
 	rapl_write_data_raw(rd, PL1_ENABLE, mode);
-	rapl_defaults->set_floor_freq(rd, mode);
+	if (rapl_defaults->set_floor_freq)
+		rapl_defaults->set_floor_freq(rd, mode);
 	put_online_cpus();
 
 	return 0;
@@ -979,16 +982,22 @@ static void set_floor_freq_atom(struct rapl_domain *rd, bool enable)
 	static u32 power_ctrl_orig_val;
 	u32 mdata;
 
+	if (!rapl_defaults->floor_freq_reg_addr) {
+		pr_err("Invalid floor frequency config register\n");
+		return;
+	}
+
 	if (!power_ctrl_orig_val)
 		iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_PMC_READ,
-			IOSF_CPU_POWER_BUDGET_CTL, &power_ctrl_orig_val);
+			rapl_defaults->floor_freq_reg_addr,
+				&power_ctrl_orig_val);
 	mdata = power_ctrl_orig_val;
 	if (enable) {
 		mdata &= ~(0x7f << 8);
 		mdata |= 1 << 8;
 	}
 	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_PMC_WRITE,
-		IOSF_CPU_POWER_BUDGET_CTL, mdata);
+		rapl_defaults->floor_freq_reg_addr, mdata);
 }
 
 static u64 rapl_compute_time_window_core(struct rapl_package *rp, u64 value,
@@ -1029,6 +1038,7 @@ static u64 rapl_compute_time_window_atom(struct rapl_package *rp, u64 value,
 }
 
 static const struct rapl_defaults rapl_defaults_core = {
+	.floor_freq_reg_addr = 0,
 	.check_unit = rapl_check_unit_core,
 	.set_floor_freq = set_floor_freq_default,
 	.compute_time_window = rapl_compute_time_window_core,
@@ -1041,12 +1051,34 @@ static const struct rapl_defaults rapl_defaults_hsw_server = {
 	.dram_domain_energy_unit = 15300,
 };
 
-static const struct rapl_defaults rapl_defaults_atom = {
+static const struct rapl_defaults rapl_defaults_byt = {
+	.floor_freq_reg_addr = IOSF_CPU_POWER_BUDGET_CTL_BYT,
+	.check_unit = rapl_check_unit_atom,
+	.set_floor_freq = set_floor_freq_atom,
+	.compute_time_window = rapl_compute_time_window_atom,
+};
+
+static const struct rapl_defaults rapl_defaults_tng = {
+	.floor_freq_reg_addr = IOSF_CPU_POWER_BUDGET_CTL_TNG,
 	.check_unit = rapl_check_unit_atom,
 	.set_floor_freq = set_floor_freq_atom,
 	.compute_time_window = rapl_compute_time_window_atom,
 };
 
+static const struct rapl_defaults rapl_defaults_ann = {
+	.floor_freq_reg_addr = 0,
+	.check_unit = rapl_check_unit_atom,
+	.set_floor_freq = NULL,
+	.compute_time_window = rapl_compute_time_window_atom,
+};
+
+static const struct rapl_defaults rapl_defaults_cht = {
+	.floor_freq_reg_addr = 0,
+	.check_unit = rapl_check_unit_atom,
+	.set_floor_freq = NULL,
+	.compute_time_window = rapl_compute_time_window_atom,
+};
+
 #define RAPL_CPU(_model, _ops) {			\
 		.vendor = X86_VENDOR_INTEL,		\
 		.family = 6,				\
@@ -1057,7 +1089,7 @@ static const struct rapl_defaults rapl_defaults_atom = {
 static const struct x86_cpu_id rapl_ids[] __initconst = {
 	RAPL_CPU(0x2a, rapl_defaults_core),/* Sandy Bridge */
 	RAPL_CPU(0x2d, rapl_defaults_core),/* Sandy Bridge EP */
-	RAPL_CPU(0x37, rapl_defaults_atom),/* Valleyview */
+	RAPL_CPU(0x37, rapl_defaults_byt),/* Valleyview */
 	RAPL_CPU(0x3a, rapl_defaults_core),/* Ivy Bridge */
 	RAPL_CPU(0x3c, rapl_defaults_core),/* Haswell */
 	RAPL_CPU(0x3d, rapl_defaults_core),/* Broadwell */
@@ -1065,10 +1097,11 @@ static const struct x86_cpu_id rapl_ids[] __initconst = {
 	RAPL_CPU(0x4f, rapl_defaults_hsw_server),/* Broadwell servers */
 	RAPL_CPU(0x45, rapl_defaults_core),/* Haswell ULT */
 	RAPL_CPU(0x4E, rapl_defaults_core),/* Skylake */
-	RAPL_CPU(0x4C, rapl_defaults_atom),/* Braswell */
-	RAPL_CPU(0x4A, rapl_defaults_atom),/* Tangier */
+	RAPL_CPU(0x4C, rapl_defaults_cht),/* Braswell/Cherryview */
+	RAPL_CPU(0x4A, rapl_defaults_tng),/* Tangier */
 	RAPL_CPU(0x56, rapl_defaults_core),/* Future Xeon */
-	RAPL_CPU(0x5A, rapl_defaults_atom),/* Annidale */
+	RAPL_CPU(0x5A, rapl_defaults_ann),/* Annidale */
+	RAPL_CPU(0x57, rapl_defaults_hsw_server),/* Knights Landing */
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, rapl_ids);
diff --git a/drivers/pwm/core.c b/drivers/pwm/core.c
index ba34c7d89042..3a7769fe53de 100644
--- a/drivers/pwm/core.c
+++ b/drivers/pwm/core.c
@@ -223,13 +223,16 @@ void *pwm_get_chip_data(struct pwm_device *pwm)
 EXPORT_SYMBOL_GPL(pwm_get_chip_data);
 
 /**
- * pwmchip_add() - register a new PWM chip
+ * pwmchip_add_with_polarity() - register a new PWM chip
  * @chip: the PWM chip to add
+ * @polarity: initial polarity of PWM channels
  *
  * Register a new PWM chip. If chip->base < 0 then a dynamically assigned base
- * will be used.
+ * will be used. The initial polarity for all channels is specified by the
+ * @polarity parameter.
  */
-int pwmchip_add(struct pwm_chip *chip)
+int pwmchip_add_with_polarity(struct pwm_chip *chip,
+			      enum pwm_polarity polarity)
 {
 	struct pwm_device *pwm;
 	unsigned int i;
@@ -259,6 +262,7 @@ int pwmchip_add(struct pwm_chip *chip)
 		pwm->chip = chip;
 		pwm->pwm = chip->base + i;
 		pwm->hwpwm = i;
+		pwm->polarity = polarity;
 
 		radix_tree_insert(&pwm_tree, pwm->pwm, pwm);
 	}
@@ -279,6 +283,19 @@ out:
 	mutex_unlock(&pwm_lock);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(pwmchip_add_with_polarity);
+
+/**
+ * pwmchip_add() - register a new PWM chip
+ * @chip: the PWM chip to add
+ *
+ * Register a new PWM chip. If chip->base < 0 then a dynamically assigned base
+ * will be used. The initial polarity for all channels is normal.
+ */
+int pwmchip_add(struct pwm_chip *chip)
+{
+	return pwmchip_add_with_polarity(chip, PWM_POLARITY_NORMAL);
+}
 EXPORT_SYMBOL_GPL(pwmchip_add);
 
 /**
@@ -586,6 +603,23 @@ void pwm_add_table(struct pwm_lookup *table, size_t num)
 }
 
 /**
+ * pwm_remove_table() - unregister PWM device consumers
+ * @table: array of consumers to unregister
+ * @num: number of consumers in table
+ */
+void pwm_remove_table(struct pwm_lookup *table, size_t num)
+{
+	mutex_lock(&pwm_lookup_lock);
+
+	while (num--) {
+		list_del(&table->list);
+		table++;
+	}
+
+	mutex_unlock(&pwm_lookup_lock);
+}
+
+/**
  * pwm_get() - look up and request a PWM device
  * @dev: device for PWM consumer
  * @con_id: consumer name
diff --git a/drivers/pwm/pwm-atmel.c b/drivers/pwm/pwm-atmel.c
index d3c22de9ee47..a947c9095d9d 100644
--- a/drivers/pwm/pwm-atmel.c
+++ b/drivers/pwm/pwm-atmel.c
@@ -8,9 +8,11 @@
  */
 
 #include <linux/clk.h>
+#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
@@ -21,6 +23,7 @@
 #define PWM_ENA			0x04
 #define PWM_DIS			0x08
 #define PWM_SR			0x0C
+#define PWM_ISR			0x1C
 /* Bit field in SR */
 #define PWM_SR_ALL_CH_ON	0x0F
 
@@ -60,6 +63,9 @@ struct atmel_pwm_chip {
 	struct clk *clk;
 	void __iomem *base;
 
+	unsigned int updated_pwms;
+	struct mutex isr_lock; /* ISR is cleared when read, ensure only one thread does that */
+
 	void (*config)(struct pwm_chip *chip, struct pwm_device *pwm,
 		       unsigned long dty, unsigned long prd);
 };
@@ -144,6 +150,10 @@ static int atmel_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	val = (val & ~PWM_CMR_CPRE_MSK) | (pres & PWM_CMR_CPRE_MSK);
 	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWM_CMR, val);
 	atmel_pwm->config(chip, pwm, dty, prd);
+	mutex_lock(&atmel_pwm->isr_lock);
+	atmel_pwm->updated_pwms |= atmel_pwm_readl(atmel_pwm, PWM_ISR);
+	atmel_pwm->updated_pwms &= ~(1 << pwm->hwpwm);
+	mutex_unlock(&atmel_pwm->isr_lock);
 
 	clk_disable(atmel_pwm->clk);
 	return ret;
@@ -155,24 +165,25 @@ static void atmel_pwm_config_v1(struct pwm_chip *chip, struct pwm_device *pwm,
 	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
 	unsigned int val;
 
-	if (test_bit(PWMF_ENABLED, &pwm->flags)) {
-		/*
-		 * If the PWM channel is enabled, using the update register,
-		 * it needs to set bit 10 of CMR to 0
-		 */
-		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CUPD, dty);
 
-		val = atmel_pwm_ch_readl(atmel_pwm, pwm->hwpwm, PWM_CMR);
-		val &= ~PWM_CMR_UPD_CDTY;
-		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWM_CMR, val);
-	} else {
-		/*
-		 * If the PWM channel is disabled, write value to duty and
-		 * period registers directly.
-		 */
-		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CDTY, dty);
-		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CPRD, prd);
-	}
+	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CUPD, dty);
+
+	val = atmel_pwm_ch_readl(atmel_pwm, pwm->hwpwm, PWM_CMR);
+	val &= ~PWM_CMR_UPD_CDTY;
+	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWM_CMR, val);
+
+	/*
+	 * If the PWM channel is enabled, only update CDTY by using the update
+	 * register, it needs to set bit 10 of CMR to 0
+	 */
+	if (test_bit(PWMF_ENABLED, &pwm->flags))
+		return;
+	/*
+	 * If the PWM channel is disabled, write value to duty and period
+	 * registers directly.
+	 */
+	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CDTY, dty);
+	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CPRD, prd);
 }
 
 static void atmel_pwm_config_v2(struct pwm_chip *chip, struct pwm_device *pwm,
@@ -242,7 +253,22 @@ static int atmel_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 static void atmel_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
+	unsigned long timeout = jiffies + 2 * HZ;
+
+	/*
+	 * Wait for at least a complete period to have passed before disabling a
+	 * channel to be sure that CDTY has been updated
+	 */
+	mutex_lock(&atmel_pwm->isr_lock);
+	atmel_pwm->updated_pwms |= atmel_pwm_readl(atmel_pwm, PWM_ISR);
+
+	while (!(atmel_pwm->updated_pwms & (1 << pwm->hwpwm)) &&
+	       time_before(jiffies, timeout)) {
+		usleep_range(10, 100);
+		atmel_pwm->updated_pwms |= atmel_pwm_readl(atmel_pwm, PWM_ISR);
+	}
 
+	mutex_unlock(&atmel_pwm->isr_lock);
 	atmel_pwm_writel(atmel_pwm, PWM_DIS, 1 << pwm->hwpwm);
 
 	clk_disable(atmel_pwm->clk);
@@ -357,6 +383,8 @@ static int atmel_pwm_probe(struct platform_device *pdev)
 	atmel_pwm->chip.npwm = 4;
 	atmel_pwm->chip.can_sleep = true;
 	atmel_pwm->config = data->config;
+	atmel_pwm->updated_pwms = 0;
+	mutex_init(&atmel_pwm->isr_lock);
 
 	ret = pwmchip_add(&atmel_pwm->chip);
 	if (ret < 0) {
@@ -378,6 +406,7 @@ static int atmel_pwm_remove(struct platform_device *pdev)
 	struct atmel_pwm_chip *atmel_pwm = platform_get_drvdata(pdev);
 
 	clk_unprepare(atmel_pwm->clk);
+	mutex_destroy(&atmel_pwm->isr_lock);
 
 	return pwmchip_remove(&atmel_pwm->chip);
 }
diff --git a/drivers/pwm/pwm-bcm-kona.c b/drivers/pwm/pwm-bcm-kona.c
index 02bc048892a9..7af8fea2dc5b 100644
--- a/drivers/pwm/pwm-bcm-kona.c
+++ b/drivers/pwm/pwm-bcm-kona.c
@@ -266,18 +266,15 @@ static int kona_pwmc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	/* Set smooth mode, push/pull, and normal polarity for all channels */
-	for (chan = 0; chan < kp->chip.npwm; chan++) {
-		value |= (1 << PWM_CONTROL_SMOOTH_SHIFT(chan));
+	/* Set push/pull for all channels */
+	for (chan = 0; chan < kp->chip.npwm; chan++)
 		value |= (1 << PWM_CONTROL_TYPE_SHIFT(chan));
-		value |= (1 << PWM_CONTROL_POLARITY_SHIFT(chan));
-	}
 
 	writel(value, kp->base + PWM_CONTROL_OFFSET);
 
 	clk_disable_unprepare(kp->clk);
 
-	ret = pwmchip_add(&kp->chip);
+	ret = pwmchip_add_with_polarity(&kp->chip, PWM_POLARITY_INVERSED);
 	if (ret < 0)
 		dev_err(&pdev->dev, "failed to add PWM chip: %d\n", ret);
 
diff --git a/drivers/pwm/pwm-lpss-pci.c b/drivers/pwm/pwm-lpss-pci.c
index cf20d2beacdd..45042c1b2046 100644
--- a/drivers/pwm/pwm-lpss-pci.c
+++ b/drivers/pwm/pwm-lpss-pci.c
@@ -44,8 +44,10 @@ static void pwm_lpss_remove_pci(struct pci_dev *pdev)
 }
 
 static const struct pci_device_id pwm_lpss_pci_ids[] = {
+	{ PCI_VDEVICE(INTEL, 0x0ac8), (unsigned long)&pwm_lpss_bsw_info},
 	{ PCI_VDEVICE(INTEL, 0x0f08), (unsigned long)&pwm_lpss_byt_info},
 	{ PCI_VDEVICE(INTEL, 0x0f09), (unsigned long)&pwm_lpss_byt_info},
+	{ PCI_VDEVICE(INTEL, 0x1ac8), (unsigned long)&pwm_lpss_bsw_info},
 	{ PCI_VDEVICE(INTEL, 0x2288), (unsigned long)&pwm_lpss_bsw_info},
 	{ PCI_VDEVICE(INTEL, 0x2289), (unsigned long)&pwm_lpss_bsw_info},
 	{ },
diff --git a/drivers/pwm/pwm-samsung.c b/drivers/pwm/pwm-samsung.c
index ff201e1b9219..ada2d326dc3e 100644
--- a/drivers/pwm/pwm-samsung.c
+++ b/drivers/pwm/pwm-samsung.c
@@ -456,6 +456,7 @@ static const struct of_device_id samsung_pwm_matches[] = {
 	{ .compatible = "samsung,exynos4210-pwm", .data = &s5p64x0_variant },
 	{},
 };
+MODULE_DEVICE_TABLE(of, samsung_pwm_matches);
 
 static int pwm_samsung_parse_dt(struct samsung_pwm_chip *chip)
 {
diff --git a/drivers/rapidio/rio-scan.c b/drivers/rapidio/rio-scan.c
index 47a1b2ea76c4..d6a126c17c03 100644
--- a/drivers/rapidio/rio-scan.c
+++ b/drivers/rapidio/rio-scan.c
@@ -83,7 +83,7 @@ static u16 rio_destid_alloc(struct rio_net *net)
  * @destid: destID to reserve
  *
  * Tries to reserve the specified destID.
- * Returns 0 if successfull.
+ * Returns 0 if successful.
  */
 static int rio_destid_reserve(struct rio_net *net, u16 destid)
 {
diff --git a/drivers/regulator/88pm8607.c b/drivers/regulator/88pm8607.c
index c3d15427adc7..b100a63ff3b3 100644
--- a/drivers/regulator/88pm8607.c
+++ b/drivers/regulator/88pm8607.c
@@ -404,7 +404,7 @@ static int pm8607_regulator_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id pm8607_regulator_driver_ids[] = {
+static const struct platform_device_id pm8607_regulator_driver_ids[] = {
 	{
 		.name	= "88pm860x-regulator",
 		.driver_data	= 0,
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index a6f116aa5235..bef3bde6971b 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -178,6 +178,16 @@ config REGULATOR_DA9055
 	  This driver can also be built as a module. If so, the module
 	  will be called da9055-regulator.
 
+config REGULATOR_DA9062
+	tristate "Dialog Semiconductor DA9062 regulators"
+	depends on MFD_DA9062
+	help
+	  Say y here to support the BUCKs and LDOs regulators found on
+	  DA9062 PMICs.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called da9062-regulator.
+
 config REGULATOR_DA9063
 	tristate "Dialog Semiconductor DA9063 regulators"
 	depends on MFD_DA9063
@@ -233,7 +243,7 @@ config REGULATOR_FAN53555
 
 config REGULATOR_GPIO
 	tristate "GPIO regulator support"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  This driver provides support for regulators that can be
 	  controlled via gpios.
@@ -512,6 +522,17 @@ config REGULATOR_QCOM_RPM
 	  Qualcomm RPM as a module. The module will be named
 	  "qcom_rpm-regulator".
 
+config REGULATOR_QCOM_SPMI
+	tristate "Qualcomm SPMI regulator driver"
+	depends on SPMI || COMPILE_TEST
+	help
+	  If you say yes to this option, support will be included for the
+	  regulators found in Qualcomm SPMI PMICs.
+
+	  Say M here if you want to include support for the regulators on the
+	  Qualcomm SPMI PMICs as a module. The module will be named
+	  "qcom_spmi-regulator".
+
 config REGULATOR_RC5T583
 	tristate "RICOH RC5T583 Power regulators"
 	depends on MFD_RC5T583
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 2c4da15e1545..91bf76267404 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -25,6 +25,7 @@ obj-$(CONFIG_REGULATOR_BCM590XX) += bcm590xx-regulator.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 obj-$(CONFIG_REGULATOR_DA9052)	+= da9052-regulator.o
 obj-$(CONFIG_REGULATOR_DA9055)	+= da9055-regulator.o
+obj-$(CONFIG_REGULATOR_DA9062)	+= da9062-regulator.o
 obj-$(CONFIG_REGULATOR_DA9063)	+= da9063-regulator.o
 obj-$(CONFIG_REGULATOR_DA9210) += da9210-regulator.o
 obj-$(CONFIG_REGULATOR_DA9211) += da9211-regulator.o
@@ -61,6 +62,7 @@ obj-$(CONFIG_REGULATOR_MC13892) += mc13892-regulator.o
 obj-$(CONFIG_REGULATOR_MC13XXX_CORE) +=  mc13xxx-regulator-core.o
 obj-$(CONFIG_REGULATOR_MT6397)	+= mt6397-regulator.o
 obj-$(CONFIG_REGULATOR_QCOM_RPM) += qcom_rpm-regulator.o
+obj-$(CONFIG_REGULATOR_QCOM_SPMI) += qcom_spmi-regulator.o
 obj-$(CONFIG_REGULATOR_PALMAS) += palmas-regulator.o
 obj-$(CONFIG_REGULATOR_PFUZE100) += pfuze100-regulator.o
 obj-$(CONFIG_REGULATOR_PWM) += pwm-regulator.o
diff --git a/drivers/regulator/arizona-ldo1.c b/drivers/regulator/arizona-ldo1.c
index a1d07d347c20..5e947a8ddb84 100644
--- a/drivers/regulator/arizona-ldo1.c
+++ b/drivers/regulator/arizona-ldo1.c
@@ -78,11 +78,6 @@ static int arizona_ldo1_hc_set_voltage_sel(struct regulator_dev *rdev,
 	if (ret != 0)
 		return ret;
 
-	ret = regmap_update_bits(regmap, ARIZONA_DYNAMIC_FREQUENCY_SCALING_1,
-				 ARIZONA_SUBSYS_MAX_FREQ, val);
-	if (ret != 0)
-		return ret;
-
 	if (val)
 		return 0;
 
@@ -178,6 +173,16 @@ static const struct regulator_init_data arizona_ldo1_default = {
 	.num_consumer_supplies = 1,
 };
 
+static const struct regulator_init_data arizona_ldo1_wm5110 = {
+	.constraints = {
+		.min_uV = 1175000,
+		.max_uV = 1200000,
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS |
+				  REGULATOR_CHANGE_VOLTAGE,
+	},
+	.num_consumer_supplies = 1,
+};
+
 static int arizona_ldo1_of_get_pdata(struct arizona *arizona,
 				     struct regulator_config *config,
 				     const struct regulator_desc *desc)
@@ -243,6 +248,11 @@ static int arizona_ldo1_probe(struct platform_device *pdev)
 		desc = &arizona_ldo1_hc;
 		ldo1->init_data = arizona_ldo1_dvfs;
 		break;
+	case WM5110:
+	case WM8280:
+		desc = &arizona_ldo1;
+		ldo1->init_data = arizona_ldo1_wm5110;
+		break;
 	default:
 		desc = &arizona_ldo1;
 		ldo1->init_data = arizona_ldo1_default;
diff --git a/drivers/regulator/axp20x-regulator.c b/drivers/regulator/axp20x-regulator.c
index e4331f5e5d7d..646829132b59 100644
--- a/drivers/regulator/axp20x-regulator.c
+++ b/drivers/regulator/axp20x-regulator.c
@@ -27,20 +27,24 @@
 #define AXP20X_IO_ENABLED		0x03
 #define AXP20X_IO_DISABLED		0x07
 
+#define AXP22X_IO_ENABLED		0x04
+#define AXP22X_IO_DISABLED		0x03
+
 #define AXP20X_WORKMODE_DCDC2_MASK	BIT(2)
 #define AXP20X_WORKMODE_DCDC3_MASK	BIT(1)
+#define AXP22X_WORKMODE_DCDCX_MASK(x)	BIT(x)
 
 #define AXP20X_FREQ_DCDC_MASK		0x0f
 
-#define AXP20X_DESC_IO(_id, _match, _supply, _min, _max, _step, _vreg, _vmask,	\
-		       _ereg, _emask, _enable_val, _disable_val)		\
-	[AXP20X_##_id] = {							\
+#define AXP_DESC_IO(_family, _id, _match, _supply, _min, _max, _step, _vreg,	\
+		    _vmask, _ereg, _emask, _enable_val, _disable_val)		\
+	[_family##_##_id] = {							\
 		.name		= #_id,						\
 		.supply_name	= (_supply),					\
 		.of_match	= of_match_ptr(_match),				\
 		.regulators_node = of_match_ptr("regulators"),			\
 		.type		= REGULATOR_VOLTAGE,				\
-		.id		= AXP20X_##_id,					\
+		.id		= _family##_##_id,				\
 		.n_voltages	= (((_max) - (_min)) / (_step) + 1),		\
 		.owner		= THIS_MODULE,					\
 		.min_uV		= (_min) * 1000,				\
@@ -54,15 +58,15 @@
 		.ops		= &axp20x_ops,					\
 	}
 
-#define AXP20X_DESC(_id, _match, _supply, _min, _max, _step, _vreg, _vmask,	\
-		    _ereg, _emask)						\
-	[AXP20X_##_id] = {							\
+#define AXP_DESC(_family, _id, _match, _supply, _min, _max, _step, _vreg,	\
+		 _vmask, _ereg, _emask) 					\
+	[_family##_##_id] = {							\
 		.name		= #_id,						\
 		.supply_name	= (_supply),					\
 		.of_match	= of_match_ptr(_match),				\
 		.regulators_node = of_match_ptr("regulators"),			\
 		.type		= REGULATOR_VOLTAGE,				\
-		.id		= AXP20X_##_id,					\
+		.id		= _family##_##_id,				\
 		.n_voltages	= (((_max) - (_min)) / (_step) + 1),		\
 		.owner		= THIS_MODULE,					\
 		.min_uV		= (_min) * 1000,				\
@@ -74,29 +78,49 @@
 		.ops		= &axp20x_ops,					\
 	}
 
-#define AXP20X_DESC_FIXED(_id, _match, _supply, _volt)				\
-	[AXP20X_##_id] = {							\
+#define AXP_DESC_SW(_family, _id, _match, _supply, _min, _max, _step, _vreg,	\
+		    _vmask, _ereg, _emask) 					\
+	[_family##_##_id] = {							\
 		.name		= #_id,						\
 		.supply_name	= (_supply),					\
 		.of_match	= of_match_ptr(_match),				\
 		.regulators_node = of_match_ptr("regulators"),			\
 		.type		= REGULATOR_VOLTAGE,				\
-		.id		= AXP20X_##_id,					\
+		.id		= _family##_##_id,				\
+		.n_voltages	= (((_max) - (_min)) / (_step) + 1),		\
+		.owner		= THIS_MODULE,					\
+		.min_uV		= (_min) * 1000,				\
+		.uV_step	= (_step) * 1000,				\
+		.vsel_reg	= (_vreg),					\
+		.vsel_mask	= (_vmask),					\
+		.enable_reg	= (_ereg),					\
+		.enable_mask	= (_emask),					\
+		.ops		= &axp20x_ops_sw,				\
+	}
+
+#define AXP_DESC_FIXED(_family, _id, _match, _supply, _volt)			\
+	[_family##_##_id] = {							\
+		.name		= #_id,						\
+		.supply_name	= (_supply),					\
+		.of_match	= of_match_ptr(_match),				\
+		.regulators_node = of_match_ptr("regulators"),			\
+		.type		= REGULATOR_VOLTAGE,				\
+		.id		= _family##_##_id,				\
 		.n_voltages	= 1,						\
 		.owner		= THIS_MODULE,					\
 		.min_uV		= (_volt) * 1000,				\
 		.ops		= &axp20x_ops_fixed				\
 	}
 
-#define AXP20X_DESC_TABLE(_id, _match, _supply, _table, _vreg, _vmask, _ereg,	\
-			  _emask)						\
-	[AXP20X_##_id] = {							\
+#define AXP_DESC_TABLE(_family, _id, _match, _supply, _table, _vreg, _vmask,	\
+		       _ereg, _emask)						\
+	[_family##_##_id] = {							\
 		.name		= #_id,						\
 		.supply_name	= (_supply),					\
 		.of_match	= of_match_ptr(_match),				\
 		.regulators_node = of_match_ptr("regulators"),			\
 		.type		= REGULATOR_VOLTAGE,				\
-		.id		= AXP20X_##_id,					\
+		.id		= _family##_##_id,				\
 		.n_voltages	= ARRAY_SIZE(_table),				\
 		.owner		= THIS_MODULE,					\
 		.vsel_reg	= (_vreg),					\
@@ -135,38 +159,118 @@ static struct regulator_ops axp20x_ops = {
 	.is_enabled		= regulator_is_enabled_regmap,
 };
 
+static struct regulator_ops axp20x_ops_sw = {
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+};
+
 static const struct regulator_desc axp20x_regulators[] = {
-	AXP20X_DESC(DCDC2, "dcdc2", "vin2", 700, 2275, 25, AXP20X_DCDC2_V_OUT,
-		    0x3f, AXP20X_PWR_OUT_CTRL, 0x10),
-	AXP20X_DESC(DCDC3, "dcdc3", "vin3", 700, 3500, 25, AXP20X_DCDC3_V_OUT,
-		    0x7f, AXP20X_PWR_OUT_CTRL, 0x02),
-	AXP20X_DESC_FIXED(LDO1, "ldo1", "acin", 1300),
-	AXP20X_DESC(LDO2, "ldo2", "ldo24in", 1800, 3300, 100,
-		    AXP20X_LDO24_V_OUT, 0xf0, AXP20X_PWR_OUT_CTRL, 0x04),
-	AXP20X_DESC(LDO3, "ldo3", "ldo3in", 700, 3500, 25, AXP20X_LDO3_V_OUT,
-		    0x7f, AXP20X_PWR_OUT_CTRL, 0x40),
-	AXP20X_DESC_TABLE(LDO4, "ldo4", "ldo24in", axp20x_ldo4_data,
-			  AXP20X_LDO24_V_OUT, 0x0f, AXP20X_PWR_OUT_CTRL, 0x08),
-	AXP20X_DESC_IO(LDO5, "ldo5", "ldo5in", 1800, 3300, 100,
-		       AXP20X_LDO5_V_OUT, 0xf0, AXP20X_GPIO0_CTRL, 0x07,
-		       AXP20X_IO_ENABLED, AXP20X_IO_DISABLED),
+	AXP_DESC(AXP20X, DCDC2, "dcdc2", "vin2", 700, 2275, 25,
+		 AXP20X_DCDC2_V_OUT, 0x3f, AXP20X_PWR_OUT_CTRL, 0x10),
+	AXP_DESC(AXP20X, DCDC3, "dcdc3", "vin3", 700, 3500, 25,
+		 AXP20X_DCDC3_V_OUT, 0x7f, AXP20X_PWR_OUT_CTRL, 0x02),
+	AXP_DESC_FIXED(AXP20X, LDO1, "ldo1", "acin", 1300),
+	AXP_DESC(AXP20X, LDO2, "ldo2", "ldo24in", 1800, 3300, 100,
+		 AXP20X_LDO24_V_OUT, 0xf0, AXP20X_PWR_OUT_CTRL, 0x04),
+	AXP_DESC(AXP20X, LDO3, "ldo3", "ldo3in", 700, 3500, 25,
+		 AXP20X_LDO3_V_OUT, 0x7f, AXP20X_PWR_OUT_CTRL, 0x40),
+	AXP_DESC_TABLE(AXP20X, LDO4, "ldo4", "ldo24in", axp20x_ldo4_data,
+		       AXP20X_LDO24_V_OUT, 0x0f, AXP20X_PWR_OUT_CTRL, 0x08),
+	AXP_DESC_IO(AXP20X, LDO5, "ldo5", "ldo5in", 1800, 3300, 100,
+		    AXP20X_LDO5_V_OUT, 0xf0, AXP20X_GPIO0_CTRL, 0x07,
+		    AXP20X_IO_ENABLED, AXP20X_IO_DISABLED),
+};
+
+static const struct regulator_desc axp22x_regulators[] = {
+	AXP_DESC(AXP22X, DCDC1, "dcdc1", "vin1", 1600, 3400, 100,
+		 AXP22X_DCDC1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL1, BIT(1)),
+	AXP_DESC(AXP22X, DCDC2, "dcdc2", "vin2", 600, 1540, 20,
+		 AXP22X_DCDC2_V_OUT, 0x3f, AXP22X_PWR_OUT_CTRL1, BIT(2)),
+	AXP_DESC(AXP22X, DCDC3, "dcdc3", "vin3", 600, 1860, 20,
+		 AXP22X_DCDC3_V_OUT, 0x3f, AXP22X_PWR_OUT_CTRL1, BIT(3)),
+	AXP_DESC(AXP22X, DCDC4, "dcdc4", "vin4", 600, 1540, 20,
+		 AXP22X_DCDC4_V_OUT, 0x3f, AXP22X_PWR_OUT_CTRL1, BIT(3)),
+	AXP_DESC(AXP22X, DCDC5, "dcdc5", "vin5", 1000, 2550, 50,
+		 AXP22X_DCDC5_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL1, BIT(4)),
+	/* secondary switchable output of DCDC1 */
+	AXP_DESC_SW(AXP22X, DC1SW, "dc1sw", "dcdc1", 1600, 3400, 100,
+		    AXP22X_DCDC1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(7)),
+	/* LDO regulator internally chained to DCDC5 */
+	AXP_DESC(AXP22X, DC5LDO, "dc5ldo", "dcdc5", 700, 1400, 100,
+		 AXP22X_DC5LDO_V_OUT, 0x7, AXP22X_PWR_OUT_CTRL1, BIT(0)),
+	AXP_DESC(AXP22X, ALDO1, "aldo1", "aldoin", 700, 3300, 100,
+		 AXP22X_ALDO1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL1, BIT(6)),
+	AXP_DESC(AXP22X, ALDO2, "aldo2", "aldoin", 700, 3300, 100,
+		 AXP22X_ALDO2_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL1, BIT(7)),
+	AXP_DESC(AXP22X, ALDO3, "aldo3", "aldoin", 700, 3300, 100,
+		 AXP22X_ALDO3_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL3, BIT(7)),
+	AXP_DESC(AXP22X, DLDO1, "dldo1", "dldoin", 700, 3300, 100,
+		 AXP22X_DLDO1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(3)),
+	AXP_DESC(AXP22X, DLDO2, "dldo2", "dldoin", 700, 3300, 100,
+		 AXP22X_DLDO2_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(4)),
+	AXP_DESC(AXP22X, DLDO3, "dldo3", "dldoin", 700, 3300, 100,
+		 AXP22X_DLDO3_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(5)),
+	AXP_DESC(AXP22X, DLDO4, "dldo4", "dldoin", 700, 3300, 100,
+		 AXP22X_DLDO4_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(6)),
+	AXP_DESC(AXP22X, ELDO1, "eldo1", "eldoin", 700, 3300, 100,
+		 AXP22X_ELDO1_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(0)),
+	AXP_DESC(AXP22X, ELDO2, "eldo2", "eldoin", 700, 3300, 100,
+		 AXP22X_ELDO2_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(1)),
+	AXP_DESC(AXP22X, ELDO3, "eldo3", "eldoin", 700, 3300, 100,
+		 AXP22X_ELDO3_V_OUT, 0x1f, AXP22X_PWR_OUT_CTRL2, BIT(2)),
+	AXP_DESC_IO(AXP22X, LDO_IO0, "ldo_io0", "ips", 1800, 3300, 100,
+		    AXP22X_LDO_IO0_V_OUT, 0x1f, AXP20X_GPIO0_CTRL, 0x07,
+		    AXP22X_IO_ENABLED, AXP22X_IO_DISABLED),
+	AXP_DESC_IO(AXP22X, LDO_IO1, "ldo_io1", "ips", 1800, 3300, 100,
+		    AXP22X_LDO_IO1_V_OUT, 0x1f, AXP20X_GPIO1_CTRL, 0x07,
+		    AXP22X_IO_ENABLED, AXP22X_IO_DISABLED),
+	AXP_DESC_FIXED(AXP22X, RTC_LDO, "rtc_ldo", "ips", 3000),
 };
 
 static int axp20x_set_dcdc_freq(struct platform_device *pdev, u32 dcdcfreq)
 {
 	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+	u32 min, max, def, step;
+
+	switch (axp20x->variant) {
+	case AXP202_ID:
+	case AXP209_ID:
+		min = 750;
+		max = 1875;
+		def = 1500;
+		step = 75;
+		break;
+	case AXP221_ID:
+		min = 1800;
+		max = 4050;
+		def = 3000;
+		step = 150;
+		break;
+	default:
+		dev_err(&pdev->dev,
+			"Setting DCDC frequency for unsupported AXP variant\n");
+		return -EINVAL;
+	}
+
+	if (dcdcfreq == 0)
+		dcdcfreq = def;
 
-	if (dcdcfreq < 750) {
-		dcdcfreq = 750;
-		dev_warn(&pdev->dev, "DCDC frequency too low. Set to 750kHz\n");
+	if (dcdcfreq < min) {
+		dcdcfreq = min;
+		dev_warn(&pdev->dev, "DCDC frequency too low. Set to %ukHz\n",
+			 min);
 	}
 
-	if (dcdcfreq > 1875) {
-		dcdcfreq = 1875;
-		dev_warn(&pdev->dev, "DCDC frequency too high. Set to 1875kHz\n");
+	if (dcdcfreq > max) {
+		dcdcfreq = max;
+		dev_warn(&pdev->dev, "DCDC frequency too high. Set to %ukHz\n",
+			 max);
 	}
 
-	dcdcfreq = (dcdcfreq - 750) / 75;
+	dcdcfreq = (dcdcfreq - min) / step;
 
 	return regmap_update_bits(axp20x->regmap, AXP20X_DCDC_FREQ,
 				  AXP20X_FREQ_DCDC_MASK, dcdcfreq);
@@ -176,7 +280,7 @@ static int axp20x_regulator_parse_dt(struct platform_device *pdev)
 {
 	struct device_node *np, *regulators;
 	int ret;
-	u32 dcdcfreq;
+	u32 dcdcfreq = 0;
 
 	np = of_node_get(pdev->dev.parent->of_node);
 	if (!np)
@@ -186,7 +290,6 @@ static int axp20x_regulator_parse_dt(struct platform_device *pdev)
 	if (!regulators) {
 		dev_warn(&pdev->dev, "regulators node not found\n");
 	} else {
-		dcdcfreq = 1500;
 		of_property_read_u32(regulators, "x-powers,dcdc-freq", &dcdcfreq);
 		ret = axp20x_set_dcdc_freq(pdev, dcdcfreq);
 		if (ret < 0) {
@@ -202,15 +305,35 @@ static int axp20x_regulator_parse_dt(struct platform_device *pdev)
 
 static int axp20x_set_dcdc_workmode(struct regulator_dev *rdev, int id, u32 workmode)
 {
-	unsigned int mask = AXP20X_WORKMODE_DCDC2_MASK;
+	struct axp20x_dev *axp20x = rdev_get_drvdata(rdev);
+	unsigned int mask;
 
-	if ((id != AXP20X_DCDC2) && (id != AXP20X_DCDC3))
-		return -EINVAL;
+	switch (axp20x->variant) {
+	case AXP202_ID:
+	case AXP209_ID:
+		if ((id != AXP20X_DCDC2) && (id != AXP20X_DCDC3))
+			return -EINVAL;
+
+		mask = AXP20X_WORKMODE_DCDC2_MASK;
+		if (id == AXP20X_DCDC3)
+			mask = AXP20X_WORKMODE_DCDC3_MASK;
+
+		workmode <<= ffs(mask) - 1;
+		break;
 
-	if (id == AXP20X_DCDC3)
-		mask = AXP20X_WORKMODE_DCDC3_MASK;
+	case AXP221_ID:
+		if (id < AXP22X_DCDC1 || id > AXP22X_DCDC5)
+			return -EINVAL;
 
-	workmode <<= ffs(mask) - 1;
+		mask = AXP22X_WORKMODE_DCDCX_MASK(id - AXP22X_DCDC1);
+		workmode <<= id - AXP22X_DCDC1;
+		break;
+
+	default:
+		/* should not happen */
+		WARN_ON(1);
+		return -EINVAL;
+	}
 
 	return regmap_update_bits(rdev->regmap, AXP20X_DCDC_MODE, mask, workmode);
 }
@@ -219,22 +342,40 @@ static int axp20x_regulator_probe(struct platform_device *pdev)
 {
 	struct regulator_dev *rdev;
 	struct axp20x_dev *axp20x = dev_get_drvdata(pdev->dev.parent);
+	const struct regulator_desc *regulators;
 	struct regulator_config config = {
 		.dev = pdev->dev.parent,
 		.regmap = axp20x->regmap,
+		.driver_data = axp20x,
 	};
-	int ret, i;
+	int ret, i, nregulators;
 	u32 workmode;
 
+	switch (axp20x->variant) {
+	case AXP202_ID:
+	case AXP209_ID:
+		regulators = axp20x_regulators;
+		nregulators = AXP20X_REG_ID_MAX;
+		break;
+	case AXP221_ID:
+		regulators = axp22x_regulators;
+		nregulators = AXP22X_REG_ID_MAX;
+		break;
+	default:
+		dev_err(&pdev->dev, "Unsupported AXP variant: %ld\n",
+			axp20x->variant);
+		return -EINVAL;
+	}
+
 	/* This only sets the dcdc freq. Ignore any errors */
 	axp20x_regulator_parse_dt(pdev);
 
-	for (i = 0; i < AXP20X_REG_ID_MAX; i++) {
-		rdev = devm_regulator_register(&pdev->dev, &axp20x_regulators[i],
+	for (i = 0; i < nregulators; i++) {
+		rdev = devm_regulator_register(&pdev->dev, &regulators[i],
 					       &config);
 		if (IS_ERR(rdev)) {
 			dev_err(&pdev->dev, "Failed to register %s\n",
-				axp20x_regulators[i].name);
+				regulators[i].name);
 
 			return PTR_ERR(rdev);
 		}
@@ -245,7 +386,7 @@ static int axp20x_regulator_probe(struct platform_device *pdev)
 		if (!ret) {
 			if (axp20x_set_dcdc_workmode(rdev, i, workmode))
 				dev_err(&pdev->dev, "Failed to set workmode on %s\n",
-					axp20x_regulators[i].name);
+					rdev->desc->name);
 		}
 	}
 
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 443eaab933fc..c9f72019bd68 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -678,6 +678,8 @@ static int drms_uA_update(struct regulator_dev *rdev)
 	list_for_each_entry(sibling, &rdev->consumer_list, list)
 		current_uA += sibling->uA_load;
 
+	current_uA += rdev->constraints->system_load;
+
 	if (rdev->desc->ops->set_load) {
 		/* set the optimum mode for our new total regulator load */
 		err = rdev->desc->ops->set_load(rdev, current_uA);
@@ -779,59 +781,64 @@ static int suspend_prepare(struct regulator_dev *rdev, suspend_state_t state)
 static void print_constraints(struct regulator_dev *rdev)
 {
 	struct regulation_constraints *constraints = rdev->constraints;
-	char buf[80] = "";
+	char buf[160] = "";
+	size_t len = sizeof(buf) - 1;
 	int count = 0;
 	int ret;
 
 	if (constraints->min_uV && constraints->max_uV) {
 		if (constraints->min_uV == constraints->max_uV)
-			count += sprintf(buf + count, "%d mV ",
-					 constraints->min_uV / 1000);
+			count += scnprintf(buf + count, len - count, "%d mV ",
+					   constraints->min_uV / 1000);
 		else
-			count += sprintf(buf + count, "%d <--> %d mV ",
-					 constraints->min_uV / 1000,
-					 constraints->max_uV / 1000);
+			count += scnprintf(buf + count, len - count,
+					   "%d <--> %d mV ",
+					   constraints->min_uV / 1000,
+					   constraints->max_uV / 1000);
 	}
 
 	if (!constraints->min_uV ||
 	    constraints->min_uV != constraints->max_uV) {
 		ret = _regulator_get_voltage(rdev);
 		if (ret > 0)
-			count += sprintf(buf + count, "at %d mV ", ret / 1000);
+			count += scnprintf(buf + count, len - count,
+					   "at %d mV ", ret / 1000);
 	}
 
 	if (constraints->uV_offset)
-		count += sprintf(buf, "%dmV offset ",
-				 constraints->uV_offset / 1000);
+		count += scnprintf(buf + count, len - count, "%dmV offset ",
+				   constraints->uV_offset / 1000);
 
 	if (constraints->min_uA && constraints->max_uA) {
 		if (constraints->min_uA == constraints->max_uA)
-			count += sprintf(buf + count, "%d mA ",
-					 constraints->min_uA / 1000);
+			count += scnprintf(buf + count, len - count, "%d mA ",
+					   constraints->min_uA / 1000);
 		else
-			count += sprintf(buf + count, "%d <--> %d mA ",
-					 constraints->min_uA / 1000,
-					 constraints->max_uA / 1000);
+			count += scnprintf(buf + count, len - count,
+					   "%d <--> %d mA ",
+					   constraints->min_uA / 1000,
+					   constraints->max_uA / 1000);
 	}
 
 	if (!constraints->min_uA ||
 	    constraints->min_uA != constraints->max_uA) {
 		ret = _regulator_get_current_limit(rdev);
 		if (ret > 0)
-			count += sprintf(buf + count, "at %d mA ", ret / 1000);
+			count += scnprintf(buf + count, len - count,
+					   "at %d mA ", ret / 1000);
 	}
 
 	if (constraints->valid_modes_mask & REGULATOR_MODE_FAST)
-		count += sprintf(buf + count, "fast ");
+		count += scnprintf(buf + count, len - count, "fast ");
 	if (constraints->valid_modes_mask & REGULATOR_MODE_NORMAL)
-		count += sprintf(buf + count, "normal ");
+		count += scnprintf(buf + count, len - count, "normal ");
 	if (constraints->valid_modes_mask & REGULATOR_MODE_IDLE)
-		count += sprintf(buf + count, "idle ");
+		count += scnprintf(buf + count, len - count, "idle ");
 	if (constraints->valid_modes_mask & REGULATOR_MODE_STANDBY)
-		count += sprintf(buf + count, "standby");
+		count += scnprintf(buf + count, len - count, "standby");
 
 	if (!count)
-		sprintf(buf, "no parameters");
+		scnprintf(buf, len, "no parameters");
 
 	rdev_dbg(rdev, "%s\n", buf);
 
@@ -1006,6 +1013,15 @@ static int set_machine_constraints(struct regulator_dev *rdev,
 	if (ret != 0)
 		goto out;
 
+	if (rdev->constraints->ilim_uA && ops->set_input_current_limit) {
+		ret = ops->set_input_current_limit(rdev,
+						   rdev->constraints->ilim_uA);
+		if (ret < 0) {
+			rdev_err(rdev, "failed to set input limit\n");
+			goto out;
+		}
+	}
+
 	/* do we need to setup our suspend state */
 	if (rdev->constraints->initial_state) {
 		ret = suspend_prepare(rdev, rdev->constraints->initial_state);
@@ -1049,6 +1065,22 @@ static int set_machine_constraints(struct regulator_dev *rdev,
 		}
 	}
 
+	if (rdev->constraints->pull_down && ops->set_pull_down) {
+		ret = ops->set_pull_down(rdev);
+		if (ret < 0) {
+			rdev_err(rdev, "failed to set pull down\n");
+			goto out;
+		}
+	}
+
+	if (rdev->constraints->soft_start && ops->set_soft_start) {
+		ret = ops->set_soft_start(rdev);
+		if (ret < 0) {
+			rdev_err(rdev, "failed to set soft start\n");
+			goto out;
+		}
+	}
+
 	print_constraints(rdev);
 	return 0;
 out:
@@ -1192,10 +1224,10 @@ static struct regulator *create_regulator(struct regulator_dev *rdev,
 		if (regulator->supply_name == NULL)
 			goto overflow_err;
 
-		err = sysfs_create_link(&rdev->dev.kobj, &dev->kobj,
+		err = sysfs_create_link_nowarn(&rdev->dev.kobj, &dev->kobj,
 					buf);
 		if (err) {
-			rdev_warn(rdev, "could not add device link %s err %d\n",
+			rdev_dbg(rdev, "could not add device link %s err %d\n",
 				  dev->kobj.name, err);
 			/* non-fatal */
 		}
diff --git a/drivers/regulator/da9062-regulator.c b/drivers/regulator/da9062-regulator.c
new file mode 100644
index 000000000000..dd76da09b3c7
--- /dev/null
+++ b/drivers/regulator/da9062-regulator.c
@@ -0,0 +1,842 @@
+/*
+ * da9062-regulator.c - REGULATOR device driver for DA9062
+ * Copyright (C) 2015  Dialog Semiconductor Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/mfd/da9062/core.h>
+#include <linux/mfd/da9062/registers.h>
+
+/* Regulator IDs */
+enum {
+	DA9062_ID_BUCK1,
+	DA9062_ID_BUCK2,
+	DA9062_ID_BUCK3,
+	DA9062_ID_BUCK4,
+	DA9062_ID_LDO1,
+	DA9062_ID_LDO2,
+	DA9062_ID_LDO3,
+	DA9062_ID_LDO4,
+	DA9062_MAX_REGULATORS,
+};
+
+/* Regulator capabilities and registers description */
+struct da9062_regulator_info {
+	struct regulator_desc desc;
+	/* Current limiting */
+	unsigned int n_current_limits;
+	const int *current_limits;
+	/* Main register fields */
+	struct reg_field mode;
+	struct reg_field suspend;
+	struct reg_field sleep;
+	struct reg_field suspend_sleep;
+	unsigned int suspend_vsel_reg;
+	struct reg_field ilimit;
+	/* Event detection bit */
+	struct reg_field oc_event;
+};
+
+/* Single regulator settings */
+struct da9062_regulator {
+	struct regulator_desc			desc;
+	struct regulator_dev			*rdev;
+	struct da9062				*hw;
+	const struct da9062_regulator_info	*info;
+
+	struct regmap_field			*mode;
+	struct regmap_field			*suspend;
+	struct regmap_field			*sleep;
+	struct regmap_field			*suspend_sleep;
+	struct regmap_field			*ilimit;
+};
+
+/* Encapsulates all information for the regulators driver */
+struct da9062_regulators {
+	int					irq_ldo_lim;
+	unsigned				n_regulators;
+	/* Array size to be defined during init. Keep at end. */
+	struct da9062_regulator			regulator[0];
+};
+
+/* BUCK modes */
+enum {
+	BUCK_MODE_MANUAL,	/* 0 */
+	BUCK_MODE_SLEEP,	/* 1 */
+	BUCK_MODE_SYNC,		/* 2 */
+	BUCK_MODE_AUTO		/* 3 */
+};
+
+/* Regulator operations */
+
+/* Current limits array (in uA) BUCK1 and BUCK3.
+   Entry indexes corresponds to register values. */
+static const int da9062_buck_a_limits[] = {
+	 500000,  600000,  700000,  800000,  900000, 1000000, 1100000, 1200000,
+	1300000, 1400000, 1500000, 1600000, 1700000, 1800000, 1900000, 2000000
+};
+
+/* Current limits array (in uA) for BUCK2.
+   Entry indexes corresponds to register values. */
+static const int da9062_buck_b_limits[] = {
+	1500000, 1600000, 1700000, 1800000, 1900000, 2000000, 2100000, 2200000,
+	2300000, 2400000, 2500000, 2600000, 2700000, 2800000, 2900000, 3000000
+};
+
+static int da9062_set_current_limit(struct regulator_dev *rdev,
+				    int min_ua, int max_ua)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	const struct da9062_regulator_info *rinfo = regl->info;
+	int n, tval;
+
+	for (n = 0; n < rinfo->n_current_limits; n++) {
+		tval = rinfo->current_limits[n];
+		if (tval >= min_ua && tval <= max_ua)
+			return regmap_field_write(regl->ilimit, n);
+	}
+
+	return -EINVAL;
+}
+
+static int da9062_get_current_limit(struct regulator_dev *rdev)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	const struct da9062_regulator_info *rinfo = regl->info;
+	unsigned int sel;
+	int ret;
+
+	ret = regmap_field_read(regl->ilimit, &sel);
+	if (ret < 0)
+		return ret;
+
+	if (sel >= rinfo->n_current_limits)
+		sel = rinfo->n_current_limits - 1;
+
+	return rinfo->current_limits[sel];
+}
+
+static int da9062_buck_set_mode(struct regulator_dev *rdev, unsigned mode)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	unsigned val;
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		val = BUCK_MODE_SYNC;
+		break;
+	case REGULATOR_MODE_NORMAL:
+		val = BUCK_MODE_AUTO;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		val = BUCK_MODE_SLEEP;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return regmap_field_write(regl->mode, val);
+}
+
+/*
+ * Bucks use single mode register field for normal operation
+ * and suspend state.
+ * There are 3 modes to map to: FAST, NORMAL, and STANDBY.
+ */
+
+static unsigned da9062_buck_get_mode(struct regulator_dev *rdev)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	struct regmap_field *field;
+	unsigned int val, mode = 0;
+	int ret;
+
+	ret = regmap_field_read(regl->mode, &val);
+	if (ret < 0)
+		return ret;
+
+	switch (val) {
+	default:
+	case BUCK_MODE_MANUAL:
+		mode = REGULATOR_MODE_FAST | REGULATOR_MODE_STANDBY;
+		/* Sleep flag bit decides the mode */
+		break;
+	case BUCK_MODE_SLEEP:
+		return REGULATOR_MODE_STANDBY;
+	case BUCK_MODE_SYNC:
+		return REGULATOR_MODE_FAST;
+	case BUCK_MODE_AUTO:
+		return REGULATOR_MODE_NORMAL;
+	}
+
+	/* Detect current regulator state */
+	ret = regmap_field_read(regl->suspend, &val);
+	if (ret < 0)
+		return 0;
+
+	/* Read regulator mode from proper register, depending on state */
+	if (val)
+		field = regl->suspend_sleep;
+	else
+		field = regl->sleep;
+
+	ret = regmap_field_read(field, &val);
+	if (ret < 0)
+		return 0;
+
+	if (val)
+		mode &= REGULATOR_MODE_STANDBY;
+	else
+		mode &= REGULATOR_MODE_NORMAL | REGULATOR_MODE_FAST;
+
+	return mode;
+}
+
+/*
+ * LDOs use sleep flags - one for normal and one for suspend state.
+ * There are 2 modes to map to: NORMAL and STANDBY (sleep) for each state.
+ */
+
+static int da9062_ldo_set_mode(struct regulator_dev *rdev, unsigned mode)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	unsigned val;
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		val = 0;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		val = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return regmap_field_write(regl->sleep, val);
+}
+
+static unsigned da9062_ldo_get_mode(struct regulator_dev *rdev)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	struct regmap_field *field;
+	int ret, val;
+
+	/* Detect current regulator state */
+	ret = regmap_field_read(regl->suspend, &val);
+	if (ret < 0)
+		return 0;
+
+	/* Read regulator mode from proper register, depending on state */
+	if (val)
+		field = regl->suspend_sleep;
+	else
+		field = regl->sleep;
+
+	ret = regmap_field_read(field, &val);
+	if (ret < 0)
+		return 0;
+
+	if (val)
+		return REGULATOR_MODE_STANDBY;
+	else
+		return REGULATOR_MODE_NORMAL;
+}
+
+static int da9062_buck_get_status(struct regulator_dev *rdev)
+{
+	int ret = regulator_is_enabled_regmap(rdev);
+
+	if (ret == 0) {
+		ret = REGULATOR_STATUS_OFF;
+	} else if (ret > 0) {
+		ret = da9062_buck_get_mode(rdev);
+		if (ret > 0)
+			ret = regulator_mode_to_status(ret);
+		else if (ret == 0)
+			ret = -EIO;
+	}
+
+	return ret;
+}
+
+static int da9062_ldo_get_status(struct regulator_dev *rdev)
+{
+	int ret = regulator_is_enabled_regmap(rdev);
+
+	if (ret == 0) {
+		ret = REGULATOR_STATUS_OFF;
+	} else if (ret > 0) {
+		ret = da9062_ldo_get_mode(rdev);
+		if (ret > 0)
+			ret = regulator_mode_to_status(ret);
+		else if (ret == 0)
+			ret = -EIO;
+	}
+
+	return ret;
+}
+
+static int da9062_set_suspend_voltage(struct regulator_dev *rdev, int uv)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	const struct da9062_regulator_info *rinfo = regl->info;
+	int ret, sel;
+
+	sel = regulator_map_voltage_linear(rdev, uv, uv);
+	if (sel < 0)
+		return sel;
+
+	sel <<= ffs(rdev->desc->vsel_mask) - 1;
+
+	ret = regmap_update_bits(regl->hw->regmap, rinfo->suspend_vsel_reg,
+				 rdev->desc->vsel_mask, sel);
+
+	return ret;
+}
+
+static int da9062_suspend_enable(struct regulator_dev *rdev)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+
+	return regmap_field_write(regl->suspend, 1);
+}
+
+static int da9062_suspend_disable(struct regulator_dev *rdev)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+
+	return regmap_field_write(regl->suspend, 0);
+}
+
+static int da9062_buck_set_suspend_mode(struct regulator_dev *rdev,
+					unsigned mode)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	int val;
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		val = BUCK_MODE_SYNC;
+		break;
+	case REGULATOR_MODE_NORMAL:
+		val = BUCK_MODE_AUTO;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		val = BUCK_MODE_SLEEP;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return regmap_field_write(regl->mode, val);
+}
+
+static int da9062_ldo_set_suspend_mode(struct regulator_dev *rdev,
+						unsigned mode)
+{
+	struct da9062_regulator *regl = rdev_get_drvdata(rdev);
+	unsigned val;
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		val = 0;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		val = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return regmap_field_write(regl->suspend_sleep, val);
+}
+
+static struct regulator_ops da9062_buck_ops = {
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.set_current_limit	= da9062_set_current_limit,
+	.get_current_limit	= da9062_get_current_limit,
+	.set_mode		= da9062_buck_set_mode,
+	.get_mode		= da9062_buck_get_mode,
+	.get_status		= da9062_buck_get_status,
+	.set_suspend_voltage	= da9062_set_suspend_voltage,
+	.set_suspend_enable	= da9062_suspend_enable,
+	.set_suspend_disable	= da9062_suspend_disable,
+	.set_suspend_mode	= da9062_buck_set_suspend_mode,
+};
+
+static struct regulator_ops da9062_ldo_ops = {
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.set_mode		= da9062_ldo_set_mode,
+	.get_mode		= da9062_ldo_get_mode,
+	.get_status		= da9062_ldo_get_status,
+	.set_suspend_voltage	= da9062_set_suspend_voltage,
+	.set_suspend_enable	= da9062_suspend_enable,
+	.set_suspend_disable	= da9062_suspend_disable,
+	.set_suspend_mode	= da9062_ldo_set_suspend_mode,
+};
+
+/* Regulator information */
+static const struct da9062_regulator_info local_regulator_info[] = {
+	{
+		.desc.id = DA9062_ID_BUCK1,
+		.desc.name = "DA9062 BUCK1",
+		.desc.of_match = of_match_ptr("buck1"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_buck_ops,
+		.desc.min_uV = (300) * 1000,
+		.desc.uV_step = (10) * 1000,
+		.desc.n_voltages = ((1570) - (300))/(10) + 1,
+		.current_limits = da9062_buck_a_limits,
+		.n_current_limits = ARRAY_SIZE(da9062_buck_a_limits),
+		.desc.enable_reg = DA9062AA_BUCK1_CONT,
+		.desc.enable_mask = DA9062AA_BUCK1_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VBUCK1_A,
+		.desc.vsel_mask = DA9062AA_VBUCK1_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VBUCK1_A,
+			__builtin_ffs((int)DA9062AA_BUCK1_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK1_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VBUCK1_B,
+			__builtin_ffs((int)DA9062AA_BUCK1_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK1_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VBUCK1_B,
+		.mode = REG_FIELD(DA9062AA_BUCK1_CFG,
+			__builtin_ffs((int)DA9062AA_BUCK1_MODE_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK1_MODE_MASK)) - 1),
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VBUCK1_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VBUCK1_SEL_MASK)) - 1),
+		.ilimit = REG_FIELD(DA9062AA_BUCK_ILIM_C,
+			__builtin_ffs((int)DA9062AA_BUCK1_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK1_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9062_ID_BUCK2,
+		.desc.name = "DA9062 BUCK2",
+		.desc.of_match = of_match_ptr("buck2"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_buck_ops,
+		.desc.min_uV = (300) * 1000,
+		.desc.uV_step = (10) * 1000,
+		.desc.n_voltages = ((1570) - (300))/(10) + 1,
+		.current_limits = da9062_buck_a_limits,
+		.n_current_limits = ARRAY_SIZE(da9062_buck_a_limits),
+		.desc.enable_reg = DA9062AA_BUCK2_CONT,
+		.desc.enable_mask = DA9062AA_BUCK2_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VBUCK2_A,
+		.desc.vsel_mask = DA9062AA_VBUCK2_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VBUCK2_A,
+			__builtin_ffs((int)DA9062AA_BUCK2_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK2_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VBUCK2_B,
+			__builtin_ffs((int)DA9062AA_BUCK2_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK2_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VBUCK2_B,
+		.mode = REG_FIELD(DA9062AA_BUCK2_CFG,
+			__builtin_ffs((int)DA9062AA_BUCK2_MODE_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK2_MODE_MASK)) - 1),
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VBUCK2_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VBUCK2_SEL_MASK)) - 1),
+		.ilimit = REG_FIELD(DA9062AA_BUCK_ILIM_C,
+			__builtin_ffs((int)DA9062AA_BUCK2_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK2_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9062_ID_BUCK3,
+		.desc.name = "DA9062 BUCK3",
+		.desc.of_match = of_match_ptr("buck3"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_buck_ops,
+		.desc.min_uV = (800) * 1000,
+		.desc.uV_step = (20) * 1000,
+		.desc.n_voltages = ((3340) - (800))/(20) + 1,
+		.current_limits = da9062_buck_b_limits,
+		.n_current_limits = ARRAY_SIZE(da9062_buck_b_limits),
+		.desc.enable_reg = DA9062AA_BUCK3_CONT,
+		.desc.enable_mask = DA9062AA_BUCK3_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VBUCK3_A,
+		.desc.vsel_mask = DA9062AA_VBUCK3_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VBUCK3_A,
+			__builtin_ffs((int)DA9062AA_BUCK3_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK3_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VBUCK3_B,
+			__builtin_ffs((int)DA9062AA_BUCK3_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK3_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VBUCK3_B,
+		.mode = REG_FIELD(DA9062AA_BUCK3_CFG,
+			__builtin_ffs((int)DA9062AA_BUCK3_MODE_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK3_MODE_MASK)) - 1),
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VBUCK3_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VBUCK3_SEL_MASK)) - 1),
+		.ilimit = REG_FIELD(DA9062AA_BUCK_ILIM_A,
+			__builtin_ffs((int)DA9062AA_BUCK3_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK3_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9062_ID_BUCK4,
+		.desc.name = "DA9062 BUCK4",
+		.desc.of_match = of_match_ptr("buck4"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_buck_ops,
+		.desc.min_uV = (530) * 1000,
+		.desc.uV_step = (10) * 1000,
+		.desc.n_voltages = ((1800) - (530))/(10) + 1,
+		.current_limits = da9062_buck_a_limits,
+		.n_current_limits = ARRAY_SIZE(da9062_buck_a_limits),
+		.desc.enable_reg = DA9062AA_BUCK4_CONT,
+		.desc.enable_mask = DA9062AA_BUCK4_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VBUCK4_A,
+		.desc.vsel_mask = DA9062AA_VBUCK4_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VBUCK4_A,
+			__builtin_ffs((int)DA9062AA_BUCK4_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK4_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VBUCK4_B,
+			__builtin_ffs((int)DA9062AA_BUCK4_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK4_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VBUCK4_B,
+		.mode = REG_FIELD(DA9062AA_BUCK4_CFG,
+			__builtin_ffs((int)DA9062AA_BUCK4_MODE_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK4_MODE_MASK)) - 1),
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VBUCK4_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VBUCK4_SEL_MASK)) - 1),
+		.ilimit = REG_FIELD(DA9062AA_BUCK_ILIM_B,
+			__builtin_ffs((int)DA9062AA_BUCK4_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_BUCK4_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9062_ID_LDO1,
+		.desc.name = "DA9062 LDO1",
+		.desc.of_match = of_match_ptr("ldo1"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_ldo_ops,
+		.desc.min_uV = (900) * 1000,
+		.desc.uV_step = (50) * 1000,
+		.desc.n_voltages = ((3600) - (900))/(50) + 1,
+		.desc.enable_reg = DA9062AA_LDO1_CONT,
+		.desc.enable_mask = DA9062AA_LDO1_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VLDO1_A,
+		.desc.vsel_mask = DA9062AA_VLDO1_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VLDO1_A,
+			__builtin_ffs((int)DA9062AA_LDO1_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO1_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VLDO1_B,
+			__builtin_ffs((int)DA9062AA_LDO1_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO1_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VLDO1_B,
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VLDO1_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VLDO1_SEL_MASK)) - 1),
+		.oc_event = REG_FIELD(DA9062AA_STATUS_D,
+			__builtin_ffs((int)DA9062AA_LDO1_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO1_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9062_ID_LDO2,
+		.desc.name = "DA9062 LDO2",
+		.desc.of_match = of_match_ptr("ldo2"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_ldo_ops,
+		.desc.min_uV = (900) * 1000,
+		.desc.uV_step = (50) * 1000,
+		.desc.n_voltages = ((3600) - (600))/(50) + 1,
+		.desc.enable_reg = DA9062AA_LDO2_CONT,
+		.desc.enable_mask = DA9062AA_LDO2_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VLDO2_A,
+		.desc.vsel_mask = DA9062AA_VLDO2_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VLDO2_A,
+			__builtin_ffs((int)DA9062AA_LDO2_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO2_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VLDO2_B,
+			__builtin_ffs((int)DA9062AA_LDO2_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO2_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VLDO2_B,
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VLDO2_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VLDO2_SEL_MASK)) - 1),
+		.oc_event = REG_FIELD(DA9062AA_STATUS_D,
+			__builtin_ffs((int)DA9062AA_LDO2_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO2_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9062_ID_LDO3,
+		.desc.name = "DA9062 LDO3",
+		.desc.of_match = of_match_ptr("ldo3"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_ldo_ops,
+		.desc.min_uV = (900) * 1000,
+		.desc.uV_step = (50) * 1000,
+		.desc.n_voltages = ((3600) - (900))/(50) + 1,
+		.desc.enable_reg = DA9062AA_LDO3_CONT,
+		.desc.enable_mask = DA9062AA_LDO3_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VLDO3_A,
+		.desc.vsel_mask = DA9062AA_VLDO3_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VLDO3_A,
+			__builtin_ffs((int)DA9062AA_LDO3_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO3_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VLDO3_B,
+			__builtin_ffs((int)DA9062AA_LDO3_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO3_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VLDO3_B,
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VLDO3_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VLDO3_SEL_MASK)) - 1),
+		.oc_event = REG_FIELD(DA9062AA_STATUS_D,
+			__builtin_ffs((int)DA9062AA_LDO3_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO3_ILIM_MASK)) - 1),
+	},
+	{
+		.desc.id = DA9062_ID_LDO4,
+		.desc.name = "DA9062 LDO4",
+		.desc.of_match = of_match_ptr("ldo4"),
+		.desc.regulators_node = of_match_ptr("regulators"),
+		.desc.ops = &da9062_ldo_ops,
+		.desc.min_uV = (900) * 1000,
+		.desc.uV_step = (50) * 1000,
+		.desc.n_voltages = ((3600) - (900))/(50) + 1,
+		.desc.enable_reg = DA9062AA_LDO4_CONT,
+		.desc.enable_mask = DA9062AA_LDO4_EN_MASK,
+		.desc.vsel_reg = DA9062AA_VLDO4_A,
+		.desc.vsel_mask = DA9062AA_VLDO4_A_MASK,
+		.desc.linear_min_sel = 0,
+		.sleep = REG_FIELD(DA9062AA_VLDO4_A,
+			__builtin_ffs((int)DA9062AA_LDO4_SL_A_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO4_SL_A_MASK)) - 1),
+		.suspend_sleep = REG_FIELD(DA9062AA_VLDO4_B,
+			__builtin_ffs((int)DA9062AA_LDO4_SL_B_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO4_SL_B_MASK)) - 1),
+		.suspend_vsel_reg = DA9062AA_VLDO4_B,
+		.suspend = REG_FIELD(DA9062AA_DVC_1,
+			__builtin_ffs((int)DA9062AA_VLDO4_SEL_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_VLDO4_SEL_MASK)) - 1),
+		.oc_event = REG_FIELD(DA9062AA_STATUS_D,
+			__builtin_ffs((int)DA9062AA_LDO4_ILIM_MASK) - 1,
+			sizeof(unsigned int) * 8 -
+			__builtin_clz((DA9062AA_LDO4_ILIM_MASK)) - 1),
+	},
+};
+
+/* Regulator interrupt handlers */
+static irqreturn_t da9062_ldo_lim_event(int irq, void *data)
+{
+	struct da9062_regulators *regulators = data;
+	struct da9062 *hw = regulators->regulator[0].hw;
+	struct da9062_regulator *regl;
+	int handled = IRQ_NONE;
+	int bits, i, ret;
+
+	ret = regmap_read(hw->regmap, DA9062AA_STATUS_D, &bits);
+	if (ret < 0) {
+		dev_err(hw->dev,
+			"Failed to read LDO overcurrent indicator\n");
+		goto ldo_lim_error;
+	}
+
+	for (i = regulators->n_regulators - 1; i >= 0; i--) {
+		regl = &regulators->regulator[i];
+		if (regl->info->oc_event.reg != DA9062AA_STATUS_D)
+			continue;
+
+		if (BIT(regl->info->oc_event.lsb) & bits) {
+			regulator_notifier_call_chain(regl->rdev,
+					REGULATOR_EVENT_OVER_CURRENT, NULL);
+			handled = IRQ_HANDLED;
+		}
+	}
+
+ldo_lim_error:
+	return handled;
+}
+
+static int da9062_regulator_probe(struct platform_device *pdev)
+{
+	struct da9062 *chip = dev_get_drvdata(pdev->dev.parent);
+	struct da9062_regulators *regulators;
+	struct da9062_regulator *regl;
+	struct regulator_config config = { };
+	int irq, n, ret;
+	size_t size;
+
+	/* Allocate memory required by usable regulators */
+	size = sizeof(struct da9062_regulators) +
+		DA9062_MAX_REGULATORS * sizeof(struct da9062_regulator);
+	regulators = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
+	if (!regulators)
+		return -ENOMEM;
+
+	regulators->n_regulators = DA9062_MAX_REGULATORS;
+	platform_set_drvdata(pdev, regulators);
+
+	n = 0;
+	while (n < regulators->n_regulators) {
+		/* Initialise regulator structure */
+		regl = &regulators->regulator[n];
+		regl->hw = chip;
+		regl->info = &local_regulator_info[n];
+		regl->desc = regl->info->desc;
+		regl->desc.type = REGULATOR_VOLTAGE;
+		regl->desc.owner = THIS_MODULE;
+
+		if (regl->info->mode.reg)
+			regl->mode = devm_regmap_field_alloc(
+					&pdev->dev,
+					chip->regmap,
+					regl->info->mode);
+		if (regl->info->suspend.reg)
+			regl->suspend = devm_regmap_field_alloc(
+					&pdev->dev,
+					chip->regmap,
+					regl->info->suspend);
+		if (regl->info->sleep.reg)
+			regl->sleep = devm_regmap_field_alloc(
+					&pdev->dev,
+					chip->regmap,
+					regl->info->sleep);
+		if (regl->info->suspend_sleep.reg)
+			regl->suspend_sleep = devm_regmap_field_alloc(
+					&pdev->dev,
+					chip->regmap,
+					regl->info->suspend_sleep);
+		if (regl->info->ilimit.reg)
+			regl->ilimit = devm_regmap_field_alloc(
+					&pdev->dev,
+					chip->regmap,
+					regl->info->ilimit);
+
+		/* Register regulator */
+		memset(&config, 0, sizeof(config));
+		config.dev = chip->dev;
+		config.driver_data = regl;
+		config.regmap = chip->regmap;
+
+		regl->rdev = devm_regulator_register(&pdev->dev, &regl->desc,
+						     &config);
+		if (IS_ERR(regl->rdev)) {
+			dev_err(&pdev->dev,
+				"Failed to register %s regulator\n",
+				regl->desc.name);
+			return PTR_ERR(regl->rdev);
+		}
+
+		n++;
+	}
+
+	/* LDOs overcurrent event support */
+	irq = platform_get_irq_byname(pdev, "LDO_LIM");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Failed to get IRQ.\n");
+		return irq;
+	}
+	regulators->irq_ldo_lim = irq;
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq,
+					NULL, da9062_ldo_lim_event,
+					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					"LDO_LIM", regulators);
+	if (ret) {
+		dev_warn(&pdev->dev,
+			 "Failed to request LDO_LIM IRQ.\n");
+		regulators->irq_ldo_lim = -ENXIO;
+	}
+
+	return 0;
+}
+
+static struct platform_driver da9062_regulator_driver = {
+	.driver = {
+		.name = "da9062-regulators",
+		.owner = THIS_MODULE,
+	},
+	.probe = da9062_regulator_probe,
+};
+
+static int __init da9062_regulator_init(void)
+{
+	return platform_driver_register(&da9062_regulator_driver);
+}
+subsys_initcall(da9062_regulator_init);
+
+static void __exit da9062_regulator_cleanup(void)
+{
+	platform_driver_unregister(&da9062_regulator_driver);
+}
+module_exit(da9062_regulator_cleanup);
+
+/* Module information */
+MODULE_AUTHOR("S Twiss <stwiss.opensource@diasemi.com>");
+MODULE_DESCRIPTION("REGULATOR device driver for Dialog DA9062");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:da9062-regulators");
diff --git a/drivers/regulator/da9063-regulator.c b/drivers/regulator/da9063-regulator.c
index 31c2c593ae0b..aed1ad3dc964 100644
--- a/drivers/regulator/da9063-regulator.c
+++ b/drivers/regulator/da9063-regulator.c
@@ -117,9 +117,6 @@ struct da9063_regulator {
 
 /* Encapsulates all information for the regulators driver */
 struct da9063_regulators {
-	int					irq_ldo_lim;
-	int					irq_uvov;
-
 	unsigned				n_regulators;
 	/* Array size to be defined during init. Keep at end. */
 	struct da9063_regulator			regulator[0];
@@ -867,35 +864,23 @@ static int da9063_regulator_probe(struct platform_device *pdev)
 		return irq;
 	}
 
-	ret = request_threaded_irq(irq,
+	ret = devm_request_threaded_irq(&pdev->dev, irq,
 				NULL, da9063_ldo_lim_event,
 				IRQF_TRIGGER_LOW | IRQF_ONESHOT,
 				"LDO_LIM", regulators);
 	if (ret) {
-		dev_err(&pdev->dev,
-				"Failed to request LDO_LIM IRQ.\n");
-		regulators->irq_ldo_lim = -ENXIO;
+		dev_err(&pdev->dev, "Failed to request LDO_LIM IRQ.\n");
+		return ret;
 	}
 
 	return 0;
 }
 
-static int da9063_regulator_remove(struct platform_device *pdev)
-{
-	struct da9063_regulators *regulators = platform_get_drvdata(pdev);
-
-	free_irq(regulators->irq_ldo_lim, regulators);
-	free_irq(regulators->irq_uvov, regulators);
-
-	return 0;
-}
-
 static struct platform_driver da9063_regulator_driver = {
 	.driver = {
 		.name = DA9063_DRVNAME_REGULATORS,
 	},
 	.probe = da9063_regulator_probe,
-	.remove = da9063_regulator_remove,
 };
 
 static int __init da9063_regulator_init(void)
diff --git a/drivers/regulator/fan53555.c b/drivers/regulator/fan53555.c
index 3c25db89a021..42865681c00b 100644
--- a/drivers/regulator/fan53555.c
+++ b/drivers/regulator/fan53555.c
@@ -182,6 +182,7 @@ static int fan53555_set_ramp(struct regulator_dev *rdev, int ramp)
 static struct regulator_ops fan53555_regulator_ops = {
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_time_sel = regulator_set_voltage_time_sel,
 	.map_voltage = regulator_map_voltage_linear,
 	.list_voltage = regulator_list_voltage_linear,
 	.set_suspend_voltage = fan53555_set_suspend_voltage,
diff --git a/drivers/regulator/helpers.c b/drivers/regulator/helpers.c
index cbc39096c78d..3bbb32680a94 100644
--- a/drivers/regulator/helpers.c
+++ b/drivers/regulator/helpers.c
@@ -275,7 +275,7 @@ int regulator_map_voltage_linear(struct regulator_dev *rdev,
 EXPORT_SYMBOL_GPL(regulator_map_voltage_linear);
 
 /**
- * regulator_map_voltage_linear - map_voltage() for multiple linear ranges
+ * regulator_map_voltage_linear_range - map_voltage() for multiple linear ranges
  *
  * @rdev: Regulator to operate on
  * @min_uV: Lower bound for voltage
diff --git a/drivers/regulator/lp8755.c b/drivers/regulator/lp8755.c
index 4a415d4ee463..d6773da925ba 100644
--- a/drivers/regulator/lp8755.c
+++ b/drivers/regulator/lp8755.c
@@ -419,20 +419,16 @@ static int lp8755_int_config(struct lp8755_chip *pchip)
 	}
 
 	ret = lp8755_read(pchip, 0x0F, &regval);
-	if (ret < 0)
-		goto err_i2c;
-	pchip->irqmask = regval;
-	ret = request_threaded_irq(pchip->irq, NULL, lp8755_irq_handler,
-				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
-				   "lp8755-irq", pchip);
-	if (ret)
+	if (ret < 0) {
+		dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
 		return ret;
+	}
 
-	return ret;
-
-err_i2c:
-	dev_err(pchip->dev, "i2c acceess error %s\n", __func__);
-	return ret;
+	pchip->irqmask = regval;
+	return devm_request_threaded_irq(pchip->dev, pchip->irq, NULL,
+					 lp8755_irq_handler,
+					 IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					 "lp8755-irq", pchip);
 }
 
 static const struct regmap_config lp8755_regmap = {
@@ -514,9 +510,6 @@ static int lp8755_remove(struct i2c_client *client)
 	for (icnt = 0; icnt < LP8755_BUCK_MAX; icnt++)
 		lp8755_write(pchip, icnt, 0x00);
 
-	if (pchip->irq != 0)
-		free_irq(pchip->irq, pchip);
-
 	return 0;
 }
 
diff --git a/drivers/regulator/max14577.c b/drivers/regulator/max14577.c
index b3678d289619..b2daa6641417 100644
--- a/drivers/regulator/max14577.c
+++ b/drivers/regulator/max14577.c
@@ -100,31 +100,34 @@ static struct regulator_ops max14577_charger_ops = {
 	.set_current_limit	= max14577_reg_set_current_limit,
 };
 
+#define MAX14577_SAFEOUT_REG	{ \
+	.name		= "SAFEOUT", \
+	.of_match	= of_match_ptr("SAFEOUT"), \
+	.regulators_node = of_match_ptr("regulators"), \
+	.id		= MAX14577_SAFEOUT, \
+	.ops		= &max14577_safeout_ops, \
+	.type		= REGULATOR_VOLTAGE, \
+	.owner		= THIS_MODULE, \
+	.n_voltages	= 1, \
+	.min_uV		= MAX14577_REGULATOR_SAFEOUT_VOLTAGE, \
+	.enable_reg	= MAX14577_REG_CONTROL2, \
+	.enable_mask	= CTRL2_SFOUTORD_MASK, \
+}
+#define MAX14577_CHARGER_REG	{ \
+	.name		= "CHARGER", \
+	.of_match	= of_match_ptr("CHARGER"), \
+	.regulators_node = of_match_ptr("regulators"), \
+	.id		= MAX14577_CHARGER, \
+	.ops		= &max14577_charger_ops, \
+	.type		= REGULATOR_CURRENT, \
+	.owner		= THIS_MODULE, \
+	.enable_reg	= MAX14577_CHG_REG_CHG_CTRL2, \
+	.enable_mask	= CHGCTRL2_MBCHOSTEN_MASK, \
+}
+
 static const struct regulator_desc max14577_supported_regulators[] = {
-	[MAX14577_SAFEOUT] = {
-		.name		= "SAFEOUT",
-		.of_match	= of_match_ptr("SAFEOUT"),
-		.regulators_node = of_match_ptr("regulators"),
-		.id		= MAX14577_SAFEOUT,
-		.ops		= &max14577_safeout_ops,
-		.type		= REGULATOR_VOLTAGE,
-		.owner		= THIS_MODULE,
-		.n_voltages	= 1,
-		.min_uV		= MAX14577_REGULATOR_SAFEOUT_VOLTAGE,
-		.enable_reg	= MAX14577_REG_CONTROL2,
-		.enable_mask	= CTRL2_SFOUTORD_MASK,
-	},
-	[MAX14577_CHARGER] = {
-		.name		= "CHARGER",
-		.of_match	= of_match_ptr("CHARGER"),
-		.regulators_node = of_match_ptr("regulators"),
-		.id		= MAX14577_CHARGER,
-		.ops		= &max14577_charger_ops,
-		.type		= REGULATOR_CURRENT,
-		.owner		= THIS_MODULE,
-		.enable_reg	= MAX14577_CHG_REG_CHG_CTRL2,
-		.enable_mask	= CHGCTRL2_MBCHOSTEN_MASK,
-	},
+	[MAX14577_SAFEOUT] = MAX14577_SAFEOUT_REG,
+	[MAX14577_CHARGER] = MAX14577_CHARGER_REG,
 };
 
 static struct regulator_ops max77836_ldo_ops = {
@@ -138,63 +141,28 @@ static struct regulator_ops max77836_ldo_ops = {
 	/* TODO: add .set_suspend_mode */
 };
 
+#define MAX77836_LDO_REG(num)	{ \
+	.name		= "LDO" # num, \
+	.of_match	= of_match_ptr("LDO" # num), \
+	.regulators_node = of_match_ptr("regulators"), \
+	.id		= MAX77836_LDO ## num, \
+	.ops		= &max77836_ldo_ops, \
+	.type		= REGULATOR_VOLTAGE, \
+	.owner		= THIS_MODULE, \
+	.n_voltages	= MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM, \
+	.min_uV		= MAX77836_REGULATOR_LDO_VOLTAGE_MIN, \
+	.uV_step	= MAX77836_REGULATOR_LDO_VOLTAGE_STEP, \
+	.enable_reg	= MAX77836_LDO_REG_CNFG1_LDO ## num, \
+	.enable_mask	= MAX77836_CNFG1_LDO_PWRMD_MASK, \
+	.vsel_reg	= MAX77836_LDO_REG_CNFG1_LDO ## num, \
+	.vsel_mask	= MAX77836_CNFG1_LDO_TV_MASK, \
+}
+
 static const struct regulator_desc max77836_supported_regulators[] = {
-	[MAX14577_SAFEOUT] = {
-		.name		= "SAFEOUT",
-		.of_match	= of_match_ptr("SAFEOUT"),
-		.regulators_node = of_match_ptr("regulators"),
-		.id		= MAX14577_SAFEOUT,
-		.ops		= &max14577_safeout_ops,
-		.type		= REGULATOR_VOLTAGE,
-		.owner		= THIS_MODULE,
-		.n_voltages	= 1,
-		.min_uV		= MAX14577_REGULATOR_SAFEOUT_VOLTAGE,
-		.enable_reg	= MAX14577_REG_CONTROL2,
-		.enable_mask	= CTRL2_SFOUTORD_MASK,
-	},
-	[MAX14577_CHARGER] = {
-		.name		= "CHARGER",
-		.of_match	= of_match_ptr("CHARGER"),
-		.regulators_node = of_match_ptr("regulators"),
-		.id		= MAX14577_CHARGER,
-		.ops		= &max14577_charger_ops,
-		.type		= REGULATOR_CURRENT,
-		.owner		= THIS_MODULE,
-		.enable_reg	= MAX14577_CHG_REG_CHG_CTRL2,
-		.enable_mask	= CHGCTRL2_MBCHOSTEN_MASK,
-	},
-	[MAX77836_LDO1] = {
-		.name		= "LDO1",
-		.of_match	= of_match_ptr("LDO1"),
-		.regulators_node = of_match_ptr("regulators"),
-		.id		= MAX77836_LDO1,
-		.ops		= &max77836_ldo_ops,
-		.type		= REGULATOR_VOLTAGE,
-		.owner		= THIS_MODULE,
-		.n_voltages	= MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM,
-		.min_uV		= MAX77836_REGULATOR_LDO_VOLTAGE_MIN,
-		.uV_step	= MAX77836_REGULATOR_LDO_VOLTAGE_STEP,
-		.enable_reg	= MAX77836_LDO_REG_CNFG1_LDO1,
-		.enable_mask	= MAX77836_CNFG1_LDO_PWRMD_MASK,
-		.vsel_reg	= MAX77836_LDO_REG_CNFG1_LDO1,
-		.vsel_mask	= MAX77836_CNFG1_LDO_TV_MASK,
-	},
-	[MAX77836_LDO2] = {
-		.name		= "LDO2",
-		.of_match	= of_match_ptr("LDO2"),
-		.regulators_node = of_match_ptr("regulators"),
-		.id		= MAX77836_LDO2,
-		.ops		= &max77836_ldo_ops,
-		.type		= REGULATOR_VOLTAGE,
-		.owner		= THIS_MODULE,
-		.n_voltages	= MAX77836_REGULATOR_LDO_VOLTAGE_STEPS_NUM,
-		.min_uV		= MAX77836_REGULATOR_LDO_VOLTAGE_MIN,
-		.uV_step	= MAX77836_REGULATOR_LDO_VOLTAGE_STEP,
-		.enable_reg	= MAX77836_LDO_REG_CNFG1_LDO2,
-		.enable_mask	= MAX77836_CNFG1_LDO_PWRMD_MASK,
-		.vsel_reg	= MAX77836_LDO_REG_CNFG1_LDO2,
-		.vsel_mask	= MAX77836_CNFG1_LDO_TV_MASK,
-	},
+	[MAX14577_SAFEOUT] = MAX14577_SAFEOUT_REG,
+	[MAX14577_CHARGER] = MAX14577_CHARGER_REG,
+	[MAX77836_LDO1] = MAX77836_LDO_REG(1),
+	[MAX77836_LDO2] = MAX77836_LDO_REG(2),
 };
 
 #ifdef CONFIG_OF
diff --git a/drivers/regulator/max77686.c b/drivers/regulator/max77686.c
index 15fb1416bfbd..17ccf365a9c0 100644
--- a/drivers/regulator/max77686.c
+++ b/drivers/regulator/max77686.c
@@ -2,7 +2,7 @@
  * max77686.c - Regulator driver for the Maxim 77686
  *
  * Copyright (C) 2012 Samsung Electronics
- * Chiwoong Byun <woong.byun@smasung.com>
+ * Chiwoong Byun <woong.byun@samsung.com>
  * Jonghwa Lee <jonghwa3.lee@samsung.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -88,7 +88,7 @@ enum max77686_ramp_rate {
 };
 
 struct max77686_data {
-	u64 gpio_enabled:MAX77686_REGULATORS;
+	DECLARE_BITMAP(gpio_enabled, MAX77686_REGULATORS);
 
 	/* Array indexed by regulator id */
 	unsigned int opmode[MAX77686_REGULATORS];
@@ -121,7 +121,7 @@ static unsigned int max77686_map_normal_mode(struct max77686_data *max77686,
 	case MAX77686_BUCK8:
 	case MAX77686_BUCK9:
 	case MAX77686_LDO20 ... MAX77686_LDO22:
-		if (max77686->gpio_enabled & (1 << id))
+		if (test_bit(id, max77686->gpio_enabled))
 			return MAX77686_GPIO_CONTROL;
 	}
 
@@ -277,7 +277,7 @@ static int max77686_of_parse_cb(struct device_node *np,
 	}
 
 	if (gpio_is_valid(config->ena_gpio)) {
-		max77686->gpio_enabled |= (1 << desc->id);
+		set_bit(desc->id, max77686->gpio_enabled);
 
 		return regmap_update_bits(config->regmap, desc->enable_reg,
 					  desc->enable_mask,
diff --git a/drivers/regulator/max77693.c b/drivers/regulator/max77693.c
index 9665a488e2f1..38722c8311a5 100644
--- a/drivers/regulator/max77693.c
+++ b/drivers/regulator/max77693.c
@@ -35,20 +35,6 @@
 
 #define CHGIN_ILIM_STEP_20mA			20000
 
-/* CHARGER regulator ops */
-/* CHARGER regulator uses two bits for enabling */
-static int max77693_chg_is_enabled(struct regulator_dev *rdev)
-{
-	int ret;
-	unsigned int val;
-
-	ret = regmap_read(rdev->regmap, rdev->desc->enable_reg, &val);
-	if (ret)
-		return ret;
-
-	return (val & rdev->desc->enable_mask) == rdev->desc->enable_mask;
-}
-
 /*
  * CHARGER regulator - Min : 20mA, Max : 2580mA, step : 20mA
  * 0x00, 0x01, 0x2, 0x03	= 60 mA
@@ -118,7 +104,7 @@ static struct regulator_ops max77693_safeout_ops = {
 };
 
 static struct regulator_ops max77693_charger_ops = {
-	.is_enabled		= max77693_chg_is_enabled,
+	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
 	.get_current_limit	= max77693_chg_get_current_limit,
@@ -155,6 +141,7 @@ static const struct regulator_desc regulators[] = {
 		.enable_reg = MAX77693_CHG_REG_CHG_CNFG_00,
 		.enable_mask = CHG_CNFG_00_CHG_MASK |
 				CHG_CNFG_00_BUCK_MASK,
+		.enable_val = CHG_CNFG_00_CHG_MASK | CHG_CNFG_00_BUCK_MASK,
 	},
 };
 
diff --git a/drivers/regulator/max77802.c b/drivers/regulator/max77802.c
index 6af41abccacb..c07ee13bd470 100644
--- a/drivers/regulator/max77802.c
+++ b/drivers/regulator/max77802.c
@@ -27,6 +27,7 @@
 #include <linux/gpio.h>
 #include <linux/slab.h>
 #include <linux/gpio/consumer.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
diff --git a/drivers/regulator/max77843.c b/drivers/regulator/max77843.c
index c132ef527cdd..f4fd0d3cfa6e 100644
--- a/drivers/regulator/max77843.c
+++ b/drivers/regulator/max77843.c
@@ -33,21 +33,6 @@ static const unsigned int max77843_safeout_voltage_table[] = {
 	3300000,
 };
 
-static int max77843_reg_is_enabled(struct regulator_dev *rdev)
-{
-	struct regmap *regmap = rdev->regmap;
-	int ret;
-	unsigned int reg;
-
-	ret = regmap_read(regmap, rdev->desc->enable_reg, &reg);
-	if (ret) {
-		dev_err(&rdev->dev, "Fialed to read charger register\n");
-		return ret;
-	}
-
-	return (reg & rdev->desc->enable_mask) == rdev->desc->enable_mask;
-}
-
 static int max77843_reg_get_current_limit(struct regulator_dev *rdev)
 {
 	struct regmap *regmap = rdev->regmap;
@@ -96,7 +81,7 @@ static int max77843_reg_set_current_limit(struct regulator_dev *rdev,
 }
 
 static struct regulator_ops max77843_charger_ops = {
-	.is_enabled		= max77843_reg_is_enabled,
+	.is_enabled		= regulator_is_enabled_regmap,
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
 	.get_current_limit	= max77843_reg_get_current_limit,
@@ -112,37 +97,25 @@ static struct regulator_ops max77843_regulator_ops = {
 	.set_voltage_sel        = regulator_set_voltage_sel_regmap,
 };
 
+#define	MAX77843_SAFEOUT(num)	{ \
+	.name		= "SAFEOUT" # num, \
+	.id		= MAX77843_SAFEOUT ## num, \
+	.ops		= &max77843_regulator_ops, \
+	.of_match	= of_match_ptr("SAFEOUT" # num), \
+	.regulators_node = of_match_ptr("regulators"), \
+	.type		= REGULATOR_VOLTAGE, \
+	.owner		= THIS_MODULE, \
+	.n_voltages	= ARRAY_SIZE(max77843_safeout_voltage_table), \
+	.volt_table	= max77843_safeout_voltage_table, \
+	.enable_reg	= MAX77843_SYS_REG_SAFEOUTCTRL, \
+	.enable_mask	= MAX77843_REG_SAFEOUTCTRL_ENSAFEOUT ## num, \
+	.vsel_reg	= MAX77843_SYS_REG_SAFEOUTCTRL, \
+	.vsel_mask	= MAX77843_REG_SAFEOUTCTRL_SAFEOUT ## num ## _MASK, \
+}
+
 static const struct regulator_desc max77843_supported_regulators[] = {
-	[MAX77843_SAFEOUT1] = {
-		.name		= "SAFEOUT1",
-		.id		= MAX77843_SAFEOUT1,
-		.ops		= &max77843_regulator_ops,
-		.of_match	= of_match_ptr("SAFEOUT1"),
-		.regulators_node = of_match_ptr("regulators"),
-		.type		= REGULATOR_VOLTAGE,
-		.owner		= THIS_MODULE,
-		.n_voltages	= ARRAY_SIZE(max77843_safeout_voltage_table),
-		.volt_table	= max77843_safeout_voltage_table,
-		.enable_reg	= MAX77843_SYS_REG_SAFEOUTCTRL,
-		.enable_mask	= MAX77843_REG_SAFEOUTCTRL_ENSAFEOUT1,
-		.vsel_reg	= MAX77843_SYS_REG_SAFEOUTCTRL,
-		.vsel_mask	= MAX77843_REG_SAFEOUTCTRL_SAFEOUT1_MASK,
-	},
-	[MAX77843_SAFEOUT2] = {
-		.name           = "SAFEOUT2",
-		.id             = MAX77843_SAFEOUT2,
-		.ops            = &max77843_regulator_ops,
-		.of_match	= of_match_ptr("SAFEOUT2"),
-		.regulators_node = of_match_ptr("regulators"),
-		.type           = REGULATOR_VOLTAGE,
-		.owner          = THIS_MODULE,
-		.n_voltages	= ARRAY_SIZE(max77843_safeout_voltage_table),
-		.volt_table	= max77843_safeout_voltage_table,
-		.enable_reg     = MAX77843_SYS_REG_SAFEOUTCTRL,
-		.enable_mask    = MAX77843_REG_SAFEOUTCTRL_ENSAFEOUT2,
-		.vsel_reg	= MAX77843_SYS_REG_SAFEOUTCTRL,
-		.vsel_mask	= MAX77843_REG_SAFEOUTCTRL_SAFEOUT2_MASK,
-	},
+	[MAX77843_SAFEOUT1] = MAX77843_SAFEOUT(1),
+	[MAX77843_SAFEOUT2] = MAX77843_SAFEOUT(2),
 	[MAX77843_CHARGER] = {
 		.name		= "CHARGER",
 		.id		= MAX77843_CHARGER,
@@ -152,7 +125,8 @@ static const struct regulator_desc max77843_supported_regulators[] = {
 		.type		= REGULATOR_CURRENT,
 		.owner		= THIS_MODULE,
 		.enable_reg	= MAX77843_CHG_REG_CHG_CNFG_00,
-		.enable_mask	= MAX77843_CHG_MASK,
+		.enable_mask	= MAX77843_CHG_MASK | MAX77843_CHG_BUCK_MASK,
+		.enable_val	= MAX77843_CHG_MASK | MAX77843_CHG_BUCK_MASK,
 	},
 };
 
diff --git a/drivers/regulator/max8973-regulator.c b/drivers/regulator/max8973-regulator.c
index c3d55c2db593..6f2bdad8b4d8 100644
--- a/drivers/regulator/max8973-regulator.c
+++ b/drivers/regulator/max8973-regulator.c
@@ -27,12 +27,14 @@
 #include <linux/init.h>
 #include <linux/err.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/max8973-regulator.h>
 #include <linux/regulator/of_regulator.h>
 #include <linux/gpio.h>
+#include <linux/of_gpio.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/regmap.h>
@@ -66,6 +68,7 @@
 #define MAX8973_RAMP_25mV_PER_US			0x1
 #define MAX8973_RAMP_50mV_PER_US			0x2
 #define MAX8973_RAMP_200mV_PER_US			0x3
+#define MAX8973_RAMP_MASK				0x3
 
 /* MAX8973_CONTROL2 */
 #define MAX8973_WDTMR_ENABLE				BIT(6)
@@ -89,19 +92,25 @@
 #define MAX8973_VOLATGE_STEP				6250
 #define MAX8973_BUCK_N_VOLTAGE				0x80
 
+enum device_id {
+	MAX8973,
+	MAX77621
+};
+
 /* Maxim 8973 chip information */
 struct max8973_chip {
 	struct device *dev;
 	struct regulator_desc desc;
 	struct regmap *regmap;
 	bool enable_external_control;
+	int enable_gpio;
 	int dvs_gpio;
 	int lru_index[MAX8973_MAX_VOUT_REG];
 	int curr_vout_val[MAX8973_MAX_VOUT_REG];
 	int curr_vout_reg;
 	int curr_gpio_val;
-	bool valid_dvs_gpio;
 	struct regulator_ops ops;
+	enum device_id id;
 };
 
 /*
@@ -174,7 +183,7 @@ static int max8973_dcdc_set_voltage_sel(struct regulator_dev *rdev,
 	 * If gpios are available to select the VOUT register then least
 	 * recently used register for new configuration.
 	 */
-	if (max->valid_dvs_gpio)
+	if (gpio_is_valid(max->dvs_gpio))
 		found = find_voltage_set_register(max, vsel,
 					&vout_reg, &gpio_val);
 
@@ -191,7 +200,7 @@ static int max8973_dcdc_set_voltage_sel(struct regulator_dev *rdev,
 	}
 
 	/* Select proper VOUT register vio gpios */
-	if (max->valid_dvs_gpio) {
+	if (gpio_is_valid(max->dvs_gpio)) {
 		gpio_set_value_cansleep(max->dvs_gpio, gpio_val & 0x1);
 		max->curr_gpio_val = gpio_val;
 	}
@@ -242,12 +251,45 @@ static unsigned int max8973_dcdc_get_mode(struct regulator_dev *rdev)
 		REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
 }
 
+static int max8973_set_ramp_delay(struct regulator_dev *rdev,
+		int ramp_delay)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	unsigned int control;
+	int ret;
+	int ret_val;
+
+	/* Set ramp delay */
+	if (ramp_delay < 25000) {
+		control = MAX8973_RAMP_12mV_PER_US;
+		ret_val = 12000;
+	} else if (ramp_delay < 50000) {
+		control = MAX8973_RAMP_25mV_PER_US;
+		ret_val = 25000;
+	} else if (ramp_delay < 200000) {
+		control = MAX8973_RAMP_50mV_PER_US;
+		ret_val = 50000;
+	} else {
+		control = MAX8973_RAMP_200mV_PER_US;
+		ret_val = 200000;
+	}
+
+	ret = regmap_update_bits(max->regmap, MAX8973_CONTROL1,
+			MAX8973_RAMP_MASK, control);
+	if (ret < 0)
+		dev_err(max->dev, "register %d update failed, %d",
+				MAX8973_CONTROL1, ret);
+	return ret;
+}
+
 static const struct regulator_ops max8973_dcdc_ops = {
 	.get_voltage_sel	= max8973_dcdc_get_voltage_sel,
 	.set_voltage_sel	= max8973_dcdc_set_voltage_sel,
 	.list_voltage		= regulator_list_voltage_linear,
 	.set_mode		= max8973_dcdc_set_mode,
 	.get_mode		= max8973_dcdc_get_mode,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+	.set_ramp_delay		= max8973_set_ramp_delay,
 };
 
 static int max8973_init_dcdc(struct max8973_chip *max,
@@ -256,6 +298,29 @@ static int max8973_init_dcdc(struct max8973_chip *max,
 	int ret;
 	uint8_t	control1 = 0;
 	uint8_t control2 = 0;
+	unsigned int data;
+
+	ret = regmap_read(max->regmap, MAX8973_CONTROL1, &data);
+	if (ret < 0) {
+		dev_err(max->dev, "register %d read failed, err = %d",
+				MAX8973_CONTROL1, ret);
+		return ret;
+	}
+	control1 = data & MAX8973_RAMP_MASK;
+	switch (control1) {
+	case MAX8973_RAMP_12mV_PER_US:
+		max->desc.ramp_delay = 12000;
+		break;
+	case MAX8973_RAMP_25mV_PER_US:
+		max->desc.ramp_delay = 25000;
+		break;
+	case MAX8973_RAMP_50mV_PER_US:
+		max->desc.ramp_delay = 50000;
+		break;
+	case MAX8973_RAMP_200mV_PER_US:
+		max->desc.ramp_delay = 200000;
+		break;
+	}
 
 	if (pdata->control_flags & MAX8973_CONTROL_REMOTE_SENSE_ENABLE)
 		control1 |= MAX8973_SNS_ENABLE;
@@ -266,28 +331,16 @@ static int max8973_init_dcdc(struct max8973_chip *max,
 	if (pdata->control_flags & MAX8973_CONTROL_OUTPUT_ACTIVE_DISCH_ENABLE)
 		control1 |= MAX8973_AD_ENABLE;
 
-	if (pdata->control_flags & MAX8973_CONTROL_BIAS_ENABLE)
+	if (pdata->control_flags & MAX8973_CONTROL_BIAS_ENABLE) {
 		control1 |= MAX8973_BIAS_ENABLE;
+		max->desc.enable_time = 20;
+	} else {
+		max->desc.enable_time = 240;
+	}
 
 	if (pdata->control_flags & MAX8973_CONTROL_FREQ_SHIFT_9PER_ENABLE)
 		control1 |= MAX8973_FREQSHIFT_9PER;
 
-	/* Set ramp delay */
-	if (pdata->reg_init_data &&
-			pdata->reg_init_data->constraints.ramp_delay) {
-		if (pdata->reg_init_data->constraints.ramp_delay < 25000)
-			control1 |= MAX8973_RAMP_12mV_PER_US;
-		else if (pdata->reg_init_data->constraints.ramp_delay < 50000)
-			control1 |= MAX8973_RAMP_25mV_PER_US;
-		else if (pdata->reg_init_data->constraints.ramp_delay < 200000)
-			control1 |= MAX8973_RAMP_50mV_PER_US;
-		else
-			control1 |= MAX8973_RAMP_200mV_PER_US;
-	} else {
-		control1 |= MAX8973_RAMP_12mV_PER_US;
-		max->desc.ramp_delay = 12500;
-	}
-
 	if (!(pdata->control_flags & MAX8973_CONTROL_PULL_DOWN_ENABLE))
 		control2 |= MAX8973_DISCH_ENBABLE;
 
@@ -344,7 +397,7 @@ static int max8973_init_dcdc(struct max8973_chip *max,
 	}
 
 	/* If external control is enabled then disable EN bit */
-	if (max->enable_external_control) {
+	if (max->enable_external_control && (max->id == MAX8973)) {
 		ret = regmap_update_bits(max->regmap, MAX8973_VOUT,
 						MAX8973_VOUT_ENABLE, 0);
 		if (ret < 0)
@@ -361,22 +414,82 @@ static const struct regmap_config max8973_regmap_config = {
 	.cache_type		= REGCACHE_RBTREE,
 };
 
+static struct max8973_regulator_platform_data *max8973_parse_dt(
+		struct device *dev)
+{
+	struct max8973_regulator_platform_data *pdata;
+	struct device_node *np = dev->of_node;
+	int ret;
+	u32 pval;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	pdata->enable_ext_control = of_property_read_bool(np,
+						"maxim,externally-enable");
+	pdata->enable_gpio = of_get_named_gpio(np, "maxim,enable-gpio", 0);
+	pdata->dvs_gpio = of_get_named_gpio(np, "maxim,dvs-gpio", 0);
+
+	ret = of_property_read_u32(np, "maxim,dvs-default-state", &pval);
+	if (!ret)
+		pdata->dvs_def_state = pval;
+
+	if (of_property_read_bool(np, "maxim,enable-remote-sense"))
+		pdata->control_flags  |= MAX8973_CONTROL_REMOTE_SENSE_ENABLE;
+
+	if (of_property_read_bool(np, "maxim,enable-falling-slew-rate"))
+		pdata->control_flags  |=
+				MAX8973_CONTROL_FALLING_SLEW_RATE_ENABLE;
+
+	if (of_property_read_bool(np, "maxim,enable-active-discharge"))
+		pdata->control_flags  |=
+				MAX8973_CONTROL_OUTPUT_ACTIVE_DISCH_ENABLE;
+
+	if (of_property_read_bool(np, "maxim,enable-frequency-shift"))
+		pdata->control_flags  |= MAX8973_CONTROL_FREQ_SHIFT_9PER_ENABLE;
+
+	if (of_property_read_bool(np, "maxim,enable-bias-control"))
+		pdata->control_flags  |= MAX8973_BIAS_ENABLE;
+
+	return pdata;
+}
+
+static const struct of_device_id of_max8973_match_tbl[] = {
+	{ .compatible = "maxim,max8973", .data = (void *)MAX8973, },
+	{ .compatible = "maxim,max77621", .data = (void *)MAX77621, },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, of_max8973_match_tbl);
+
 static int max8973_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct max8973_regulator_platform_data *pdata;
+	struct regulator_init_data *ridata;
 	struct regulator_config config = { };
 	struct regulator_dev *rdev;
 	struct max8973_chip *max;
+	bool pdata_from_dt = false;
+	unsigned int chip_id;
 	int ret;
 
 	pdata = dev_get_platdata(&client->dev);
 
-	if (!pdata && !client->dev.of_node) {
+	if (!pdata && client->dev.of_node) {
+		pdata = max8973_parse_dt(&client->dev);
+		pdata_from_dt = true;
+	}
+
+	if (!pdata) {
 		dev_err(&client->dev, "No Platform data");
 		return -EIO;
 	}
 
+	if ((pdata->dvs_gpio == -EPROBE_DEFER) ||
+		(pdata->enable_gpio == -EPROBE_DEFER))
+		return -EPROBE_DEFER;
+
 	max = devm_kzalloc(&client->dev, sizeof(*max), GFP_KERNEL);
 	if (!max)
 		return -ENOMEM;
@@ -388,6 +501,27 @@ static int max8973_probe(struct i2c_client *client,
 		return ret;
 	}
 
+	if (client->dev.of_node) {
+		const struct of_device_id *match;
+
+		match = of_match_device(of_match_ptr(of_max8973_match_tbl),
+				&client->dev);
+		if (!match)
+			return -ENODATA;
+		max->id = (u32)((uintptr_t)match->data);
+	} else {
+		max->id = id->driver_data;
+	}
+
+	ret = regmap_read(max->regmap, MAX8973_CHIPID1, &chip_id);
+	if (ret < 0) {
+		dev_err(&client->dev, "register CHIPID1 read failed, %d", ret);
+		return ret;
+	}
+
+	dev_info(&client->dev, "CHIP-ID OTP: 0x%02x ID_M: 0x%02x\n",
+			(chip_id >> 4) & 0xF, (chip_id >> 1) & 0x7);
+
 	i2c_set_clientdata(client, max);
 	max->ops = max8973_dcdc_ops;
 	max->dev = &client->dev;
@@ -400,23 +534,14 @@ static int max8973_probe(struct i2c_client *client,
 	max->desc.uV_step = MAX8973_VOLATGE_STEP;
 	max->desc.n_voltages = MAX8973_BUCK_N_VOLTAGE;
 
-	if (!pdata || !pdata->enable_ext_control) {
-		max->desc.enable_reg = MAX8973_VOUT;
-		max->desc.enable_mask = MAX8973_VOUT_ENABLE;
-		max->ops.enable = regulator_enable_regmap;
-		max->ops.disable = regulator_disable_regmap;
-		max->ops.is_enabled = regulator_is_enabled_regmap;
-	}
+	max->dvs_gpio = (pdata->dvs_gpio) ? pdata->dvs_gpio : -EINVAL;
+	max->enable_gpio = (pdata->enable_gpio) ? pdata->enable_gpio : -EINVAL;
+	max->enable_external_control = pdata->enable_ext_control;
+	max->curr_gpio_val = pdata->dvs_def_state;
+	max->curr_vout_reg = MAX8973_VOUT + pdata->dvs_def_state;
 
-	if (pdata) {
-		max->dvs_gpio = pdata->dvs_gpio;
-		max->enable_external_control = pdata->enable_ext_control;
-		max->curr_gpio_val = pdata->dvs_def_state;
-		max->curr_vout_reg = MAX8973_VOUT + pdata->dvs_def_state;
-	} else {
-		max->dvs_gpio = -EINVAL;
-		max->curr_vout_reg = MAX8973_VOUT;
-	}
+	if (gpio_is_valid(max->enable_gpio))
+		max->enable_external_control = true;
 
 	max->lru_index[0] = max->curr_vout_reg;
 
@@ -434,7 +559,6 @@ static int max8973_probe(struct i2c_client *client,
 				max->dvs_gpio, ret);
 			return ret;
 		}
-		max->valid_dvs_gpio = true;
 
 		/*
 		 * Initialize the lru index with vout_reg id
@@ -444,22 +568,64 @@ static int max8973_probe(struct i2c_client *client,
 			max->lru_index[i] = i;
 		max->lru_index[0] = max->curr_vout_reg;
 		max->lru_index[max->curr_vout_reg] = 0;
-	} else {
-		max->valid_dvs_gpio = false;
 	}
 
-	if (pdata) {
-		ret = max8973_init_dcdc(max, pdata);
-		if (ret < 0) {
-			dev_err(max->dev, "Max8973 Init failed, err = %d\n", ret);
-			return ret;
+	if (pdata_from_dt)
+		pdata->reg_init_data = of_get_regulator_init_data(&client->dev,
+					client->dev.of_node, &max->desc);
+
+	ridata = pdata->reg_init_data;
+	switch (max->id) {
+	case MAX8973:
+		if (!pdata->enable_ext_control) {
+			max->desc.enable_reg = MAX8973_VOUT;
+			max->desc.enable_mask = MAX8973_VOUT_ENABLE;
+			max->ops.enable = regulator_enable_regmap;
+			max->ops.disable = regulator_disable_regmap;
+			max->ops.is_enabled = regulator_is_enabled_regmap;
+			break;
+		}
+
+		if (gpio_is_valid(max->enable_gpio)) {
+			config.ena_gpio_flags = GPIOF_OUT_INIT_LOW;
+			if (ridata && (ridata->constraints.always_on ||
+					ridata->constraints.boot_on))
+				config.ena_gpio_flags = GPIOF_OUT_INIT_HIGH;
+			config.ena_gpio = max->enable_gpio;
 		}
+		break;
+
+	case MAX77621:
+		if (gpio_is_valid(max->enable_gpio)) {
+			ret = devm_gpio_request_one(&client->dev,
+					max->enable_gpio, GPIOF_OUT_INIT_HIGH,
+					"max8973-en-gpio");
+			if (ret) {
+				dev_err(&client->dev,
+					"gpio_request for gpio %d failed: %d\n",
+					max->enable_gpio, ret);
+				return ret;
+			}
+		}
+
+		max->desc.enable_reg = MAX8973_VOUT;
+		max->desc.enable_mask = MAX8973_VOUT_ENABLE;
+		max->ops.enable = regulator_enable_regmap;
+		max->ops.disable = regulator_disable_regmap;
+		max->ops.is_enabled = regulator_is_enabled_regmap;
+		break;
+	default:
+		break;
+	}
+
+	ret = max8973_init_dcdc(max, pdata);
+	if (ret < 0) {
+		dev_err(max->dev, "Max8973 Init failed, err = %d\n", ret);
+		return ret;
 	}
 
 	config.dev = &client->dev;
-	config.init_data = pdata ? pdata->reg_init_data :
-		of_get_regulator_init_data(&client->dev, client->dev.of_node,
-					   &max->desc);
+	config.init_data = pdata->reg_init_data;
 	config.driver_data = max;
 	config.of_node = client->dev.of_node;
 	config.regmap = max->regmap;
@@ -476,15 +642,16 @@ static int max8973_probe(struct i2c_client *client,
 }
 
 static const struct i2c_device_id max8973_id[] = {
-	{.name = "max8973",},
+	{.name = "max8973", .driver_data = MAX8973},
+	{.name = "max77621", .driver_data = MAX77621},
 	{},
 };
-
 MODULE_DEVICE_TABLE(i2c, max8973_id);
 
 static struct i2c_driver max8973_i2c_driver = {
 	.driver = {
 		.name = "max8973",
+		.of_match_table = of_max8973_match_tbl,
 		.owner = THIS_MODULE,
 	},
 	.probe = max8973_probe,
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
index 24e812c48d93..b1c485b24ab2 100644
--- a/drivers/regulator/of_regulator.c
+++ b/drivers/regulator/of_regulator.c
@@ -58,6 +58,10 @@ static void of_get_regulation_constraints(struct device_node *np,
 	if (!of_property_read_u32(np, "regulator-max-microamp", &pval))
 		constraints->max_uA = pval;
 
+	if (!of_property_read_u32(np, "regulator-input-current-limit-microamp",
+				  &pval))
+		constraints->ilim_uA = pval;
+
 	/* Current change possible? */
 	if (constraints->min_uA != constraints->max_uA)
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_CURRENT;
@@ -67,6 +71,8 @@ static void of_get_regulation_constraints(struct device_node *np,
 	if (!constraints->always_on) /* status change should be possible. */
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_STATUS;
 
+	constraints->pull_down = of_property_read_bool(np, "regulator-pull-down");
+
 	if (of_property_read_bool(np, "regulator-allow-bypass"))
 		constraints->valid_ops_mask |= REGULATOR_CHANGE_BYPASS;
 
@@ -82,6 +88,9 @@ static void of_get_regulation_constraints(struct device_node *np,
 	if (!ret)
 		constraints->enable_time = pval;
 
+	constraints->soft_start = of_property_read_bool(np,
+					"regulator-soft-start");
+
 	if (!of_property_read_u32(np, "regulator-initial-mode", &pval)) {
 		if (desc && desc->of_map_mode) {
 			ret = desc->of_map_mode(pval);
@@ -95,6 +104,9 @@ static void of_get_regulation_constraints(struct device_node *np,
 		}
 	}
 
+	if (!of_property_read_u32(np, "regulator-system-load", &pval))
+		constraints->system_load = pval;
+
 	for (i = 0; i < ARRAY_SIZE(regulator_states); i++) {
 		switch (i) {
 		case PM_SUSPEND_MEM:
@@ -108,7 +120,7 @@ static void of_get_regulation_constraints(struct device_node *np,
 		case PM_SUSPEND_STANDBY:
 		default:
 			continue;
-		};
+		}
 
 		suspend_np = of_get_child_by_name(np, regulator_states[i]);
 		if (!suspend_np || !suspend_state)
@@ -292,7 +304,7 @@ struct regulator_init_data *regulator_of_get_init_data(struct device *dev,
 		return NULL;
 	}
 
-	for_each_child_of_node(search, child) {
+	for_each_available_child_of_node(search, child) {
 		name = of_get_property(child, "regulator-compatible", NULL);
 		if (!name)
 			name = child->name;
diff --git a/drivers/regulator/pwm-regulator.c b/drivers/regulator/pwm-regulator.c
index 253833ae35f3..ffa96124a5e7 100644
--- a/drivers/regulator/pwm-regulator.c
+++ b/drivers/regulator/pwm-regulator.c
@@ -21,10 +21,8 @@
 #include <linux/pwm.h>
 
 struct pwm_regulator_data {
-	struct regulator_desc desc;
 	struct pwm_voltages *duty_cycle_table;
 	struct pwm_device *pwm;
-	bool enabled;
 	int state;
 };
 
@@ -33,17 +31,17 @@ struct pwm_voltages {
 	unsigned int dutycycle;
 };
 
-static int pwm_regulator_get_voltage_sel(struct regulator_dev *dev)
+static int pwm_regulator_get_voltage_sel(struct regulator_dev *rdev)
 {
-	struct pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
+	struct pwm_regulator_data *drvdata = rdev_get_drvdata(rdev);
 
 	return drvdata->state;
 }
 
-static int pwm_regulator_set_voltage_sel(struct regulator_dev *dev,
+static int pwm_regulator_set_voltage_sel(struct regulator_dev *rdev,
 					 unsigned selector)
 {
-	struct pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
+	struct pwm_regulator_data *drvdata = rdev_get_drvdata(rdev);
 	unsigned int pwm_reg_period;
 	int dutycycle;
 	int ret;
@@ -55,30 +53,27 @@ static int pwm_regulator_set_voltage_sel(struct regulator_dev *dev,
 
 	ret = pwm_config(drvdata->pwm, dutycycle, pwm_reg_period);
 	if (ret) {
-		dev_err(&dev->dev, "Failed to configure PWM\n");
+		dev_err(&rdev->dev, "Failed to configure PWM\n");
 		return ret;
 	}
 
 	drvdata->state = selector;
 
-	if (!drvdata->enabled) {
-		ret = pwm_enable(drvdata->pwm);
-		if (ret) {
-			dev_err(&dev->dev, "Failed to enable PWM\n");
-			return ret;
-		}
-		drvdata->enabled = true;
+	ret = pwm_enable(drvdata->pwm);
+	if (ret) {
+		dev_err(&rdev->dev, "Failed to enable PWM\n");
+		return ret;
 	}
 
 	return 0;
 }
 
-static int pwm_regulator_list_voltage(struct regulator_dev *dev,
+static int pwm_regulator_list_voltage(struct regulator_dev *rdev,
 				      unsigned selector)
 {
-	struct pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
+	struct pwm_regulator_data *drvdata = rdev_get_drvdata(rdev);
 
-	if (selector >= drvdata->desc.n_voltages)
+	if (selector >= rdev->desc->n_voltages)
 		return -EINVAL;
 
 	return drvdata->duty_cycle_table[selector].uV;
@@ -91,7 +86,7 @@ static struct regulator_ops pwm_regulator_voltage_ops = {
 	.map_voltage     = regulator_map_voltage_iterate,
 };
 
-static const struct regulator_desc pwm_regulator_desc = {
+static struct regulator_desc pwm_regulator_desc = {
 	.name		= "pwm-regulator",
 	.ops		= &pwm_regulator_voltage_ops,
 	.type		= REGULATOR_VOLTAGE,
@@ -117,8 +112,6 @@ static int pwm_regulator_probe(struct platform_device *pdev)
 	if (!drvdata)
 		return -ENOMEM;
 
-	memcpy(&drvdata->desc, &pwm_regulator_desc, sizeof(pwm_regulator_desc));
-
 	/* determine the number of voltage-table */
 	prop = of_find_property(np, "voltage-table", &length);
 	if (!prop) {
@@ -133,7 +126,7 @@ static int pwm_regulator_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	drvdata->desc.n_voltages = length / sizeof(*drvdata->duty_cycle_table);
+	pwm_regulator_desc.n_voltages = length / sizeof(*drvdata->duty_cycle_table);
 
 	drvdata->duty_cycle_table = devm_kzalloc(&pdev->dev,
 						 length, GFP_KERNEL);
@@ -150,7 +143,7 @@ static int pwm_regulator_probe(struct platform_device *pdev)
 	}
 
 	config.init_data = of_get_regulator_init_data(&pdev->dev, np,
-						      &drvdata->desc);
+						      &pwm_regulator_desc);
 	if (!config.init_data)
 		return -ENOMEM;
 
@@ -165,10 +158,10 @@ static int pwm_regulator_probe(struct platform_device *pdev)
 	}
 
 	regulator = devm_regulator_register(&pdev->dev,
-					    &drvdata->desc, &config);
+					    &pwm_regulator_desc, &config);
 	if (IS_ERR(regulator)) {
 		dev_err(&pdev->dev, "Failed to register regulator %s\n",
-			drvdata->desc.name);
+			pwm_regulator_desc.name);
 		return PTR_ERR(regulator);
 	}
 
diff --git a/drivers/regulator/qcom_spmi-regulator.c b/drivers/regulator/qcom_spmi-regulator.c
new file mode 100644
index 000000000000..850a30a95b5b
--- /dev/null
+++ b/drivers/regulator/qcom_spmi-regulator.c
@@ -0,0 +1,1435 @@
+/*
+ * Copyright (c) 2012-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.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/ktime.h>
+#include <linux/regulator/driver.h>
+#include <linux/regmap.h>
+#include <linux/list.h>
+
+/* These types correspond to unique register layouts. */
+enum spmi_regulator_logical_type {
+	SPMI_REGULATOR_LOGICAL_TYPE_SMPS,
+	SPMI_REGULATOR_LOGICAL_TYPE_LDO,
+	SPMI_REGULATOR_LOGICAL_TYPE_VS,
+	SPMI_REGULATOR_LOGICAL_TYPE_BOOST,
+	SPMI_REGULATOR_LOGICAL_TYPE_FTSMPS,
+	SPMI_REGULATOR_LOGICAL_TYPE_BOOST_BYP,
+	SPMI_REGULATOR_LOGICAL_TYPE_LN_LDO,
+	SPMI_REGULATOR_LOGICAL_TYPE_ULT_LO_SMPS,
+	SPMI_REGULATOR_LOGICAL_TYPE_ULT_HO_SMPS,
+	SPMI_REGULATOR_LOGICAL_TYPE_ULT_LDO,
+};
+
+enum spmi_regulator_type {
+	SPMI_REGULATOR_TYPE_BUCK		= 0x03,
+	SPMI_REGULATOR_TYPE_LDO			= 0x04,
+	SPMI_REGULATOR_TYPE_VS			= 0x05,
+	SPMI_REGULATOR_TYPE_BOOST		= 0x1b,
+	SPMI_REGULATOR_TYPE_FTS			= 0x1c,
+	SPMI_REGULATOR_TYPE_BOOST_BYP		= 0x1f,
+	SPMI_REGULATOR_TYPE_ULT_LDO		= 0x21,
+	SPMI_REGULATOR_TYPE_ULT_BUCK		= 0x22,
+};
+
+enum spmi_regulator_subtype {
+	SPMI_REGULATOR_SUBTYPE_GP_CTL		= 0x08,
+	SPMI_REGULATOR_SUBTYPE_RF_CTL		= 0x09,
+	SPMI_REGULATOR_SUBTYPE_N50		= 0x01,
+	SPMI_REGULATOR_SUBTYPE_N150		= 0x02,
+	SPMI_REGULATOR_SUBTYPE_N300		= 0x03,
+	SPMI_REGULATOR_SUBTYPE_N600		= 0x04,
+	SPMI_REGULATOR_SUBTYPE_N1200		= 0x05,
+	SPMI_REGULATOR_SUBTYPE_N600_ST		= 0x06,
+	SPMI_REGULATOR_SUBTYPE_N1200_ST		= 0x07,
+	SPMI_REGULATOR_SUBTYPE_N900_ST		= 0x14,
+	SPMI_REGULATOR_SUBTYPE_N300_ST		= 0x15,
+	SPMI_REGULATOR_SUBTYPE_P50		= 0x08,
+	SPMI_REGULATOR_SUBTYPE_P150		= 0x09,
+	SPMI_REGULATOR_SUBTYPE_P300		= 0x0a,
+	SPMI_REGULATOR_SUBTYPE_P600		= 0x0b,
+	SPMI_REGULATOR_SUBTYPE_P1200		= 0x0c,
+	SPMI_REGULATOR_SUBTYPE_LN		= 0x10,
+	SPMI_REGULATOR_SUBTYPE_LV_P50		= 0x28,
+	SPMI_REGULATOR_SUBTYPE_LV_P150		= 0x29,
+	SPMI_REGULATOR_SUBTYPE_LV_P300		= 0x2a,
+	SPMI_REGULATOR_SUBTYPE_LV_P600		= 0x2b,
+	SPMI_REGULATOR_SUBTYPE_LV_P1200		= 0x2c,
+	SPMI_REGULATOR_SUBTYPE_LV_P450		= 0x2d,
+	SPMI_REGULATOR_SUBTYPE_LV100		= 0x01,
+	SPMI_REGULATOR_SUBTYPE_LV300		= 0x02,
+	SPMI_REGULATOR_SUBTYPE_MV300		= 0x08,
+	SPMI_REGULATOR_SUBTYPE_MV500		= 0x09,
+	SPMI_REGULATOR_SUBTYPE_HDMI		= 0x10,
+	SPMI_REGULATOR_SUBTYPE_OTG		= 0x11,
+	SPMI_REGULATOR_SUBTYPE_5V_BOOST		= 0x01,
+	SPMI_REGULATOR_SUBTYPE_FTS_CTL		= 0x08,
+	SPMI_REGULATOR_SUBTYPE_FTS2p5_CTL	= 0x09,
+	SPMI_REGULATOR_SUBTYPE_BB_2A		= 0x01,
+	SPMI_REGULATOR_SUBTYPE_ULT_HF_CTL1	= 0x0d,
+	SPMI_REGULATOR_SUBTYPE_ULT_HF_CTL2	= 0x0e,
+	SPMI_REGULATOR_SUBTYPE_ULT_HF_CTL3	= 0x0f,
+	SPMI_REGULATOR_SUBTYPE_ULT_HF_CTL4	= 0x10,
+};
+
+enum spmi_common_regulator_registers {
+	SPMI_COMMON_REG_DIG_MAJOR_REV		= 0x01,
+	SPMI_COMMON_REG_TYPE			= 0x04,
+	SPMI_COMMON_REG_SUBTYPE			= 0x05,
+	SPMI_COMMON_REG_VOLTAGE_RANGE		= 0x40,
+	SPMI_COMMON_REG_VOLTAGE_SET		= 0x41,
+	SPMI_COMMON_REG_MODE			= 0x45,
+	SPMI_COMMON_REG_ENABLE			= 0x46,
+	SPMI_COMMON_REG_PULL_DOWN		= 0x48,
+	SPMI_COMMON_REG_SOFT_START		= 0x4c,
+	SPMI_COMMON_REG_STEP_CTRL		= 0x61,
+};
+
+enum spmi_vs_registers {
+	SPMI_VS_REG_OCP				= 0x4a,
+	SPMI_VS_REG_SOFT_START			= 0x4c,
+};
+
+enum spmi_boost_registers {
+	SPMI_BOOST_REG_CURRENT_LIMIT		= 0x4a,
+};
+
+enum spmi_boost_byp_registers {
+	SPMI_BOOST_BYP_REG_CURRENT_LIMIT	= 0x4b,
+};
+
+/* Used for indexing into ctrl_reg.  These are offets from 0x40 */
+enum spmi_common_control_register_index {
+	SPMI_COMMON_IDX_VOLTAGE_RANGE		= 0,
+	SPMI_COMMON_IDX_VOLTAGE_SET		= 1,
+	SPMI_COMMON_IDX_MODE			= 5,
+	SPMI_COMMON_IDX_ENABLE			= 6,
+};
+
+/* Common regulator control register layout */
+#define SPMI_COMMON_ENABLE_MASK			0x80
+#define SPMI_COMMON_ENABLE			0x80
+#define SPMI_COMMON_DISABLE			0x00
+#define SPMI_COMMON_ENABLE_FOLLOW_HW_EN3_MASK	0x08
+#define SPMI_COMMON_ENABLE_FOLLOW_HW_EN2_MASK	0x04
+#define SPMI_COMMON_ENABLE_FOLLOW_HW_EN1_MASK	0x02
+#define SPMI_COMMON_ENABLE_FOLLOW_HW_EN0_MASK	0x01
+#define SPMI_COMMON_ENABLE_FOLLOW_ALL_MASK	0x0f
+
+/* Common regulator mode register layout */
+#define SPMI_COMMON_MODE_HPM_MASK		0x80
+#define SPMI_COMMON_MODE_AUTO_MASK		0x40
+#define SPMI_COMMON_MODE_BYPASS_MASK		0x20
+#define SPMI_COMMON_MODE_FOLLOW_AWAKE_MASK	0x10
+#define SPMI_COMMON_MODE_FOLLOW_HW_EN3_MASK	0x08
+#define SPMI_COMMON_MODE_FOLLOW_HW_EN2_MASK	0x04
+#define SPMI_COMMON_MODE_FOLLOW_HW_EN1_MASK	0x02
+#define SPMI_COMMON_MODE_FOLLOW_HW_EN0_MASK	0x01
+#define SPMI_COMMON_MODE_FOLLOW_ALL_MASK	0x1f
+
+/* Common regulator pull down control register layout */
+#define SPMI_COMMON_PULL_DOWN_ENABLE_MASK	0x80
+
+/* LDO regulator current limit control register layout */
+#define SPMI_LDO_CURRENT_LIMIT_ENABLE_MASK	0x80
+
+/* LDO regulator soft start control register layout */
+#define SPMI_LDO_SOFT_START_ENABLE_MASK		0x80
+
+/* VS regulator over current protection control register layout */
+#define SPMI_VS_OCP_OVERRIDE			0x01
+#define SPMI_VS_OCP_NO_OVERRIDE			0x00
+
+/* VS regulator soft start control register layout */
+#define SPMI_VS_SOFT_START_ENABLE_MASK		0x80
+#define SPMI_VS_SOFT_START_SEL_MASK		0x03
+
+/* Boost regulator current limit control register layout */
+#define SPMI_BOOST_CURRENT_LIMIT_ENABLE_MASK	0x80
+#define SPMI_BOOST_CURRENT_LIMIT_MASK		0x07
+
+#define SPMI_VS_OCP_DEFAULT_MAX_RETRIES		10
+#define SPMI_VS_OCP_DEFAULT_RETRY_DELAY_MS	30
+#define SPMI_VS_OCP_FALL_DELAY_US		90
+#define SPMI_VS_OCP_FAULT_DELAY_US		20000
+
+#define SPMI_FTSMPS_STEP_CTRL_STEP_MASK		0x18
+#define SPMI_FTSMPS_STEP_CTRL_STEP_SHIFT	3
+#define SPMI_FTSMPS_STEP_CTRL_DELAY_MASK	0x07
+#define SPMI_FTSMPS_STEP_CTRL_DELAY_SHIFT	0
+
+/* Clock rate in kHz of the FTSMPS regulator reference clock. */
+#define SPMI_FTSMPS_CLOCK_RATE		19200
+
+/* Minimum voltage stepper delay for each step. */
+#define SPMI_FTSMPS_STEP_DELAY		8
+
+/*
+ * The ratio SPMI_FTSMPS_STEP_MARGIN_NUM/SPMI_FTSMPS_STEP_MARGIN_DEN is used to
+ * adjust the step rate in order to account for oscillator variance.
+ */
+#define SPMI_FTSMPS_STEP_MARGIN_NUM	4
+#define SPMI_FTSMPS_STEP_MARGIN_DEN	5
+
+/*
+ * This voltage in uV is returned by get_voltage functions when there is no way
+ * to determine the current voltage level.  It is needed because the regulator
+ * framework treats a 0 uV voltage as an error.
+ */
+#define VOLTAGE_UNKNOWN 1
+
+/* VSET value to decide the range of ULT SMPS */
+#define ULT_SMPS_RANGE_SPLIT 0x60
+
+/**
+ * struct spmi_voltage_range - regulator set point voltage mapping description
+ * @min_uV:		Minimum programmable output voltage resulting from
+ *			set point register value 0x00
+ * @max_uV:		Maximum programmable output voltage
+ * @step_uV:		Output voltage increase resulting from the set point
+ *			register value increasing by 1
+ * @set_point_min_uV:	Minimum allowed voltage
+ * @set_point_max_uV:	Maximum allowed voltage.  This may be tweaked in order
+ *			to pick which range should be used in the case of
+ *			overlapping set points.
+ * @n_voltages:		Number of preferred voltage set points present in this
+ *			range
+ * @range_sel:		Voltage range register value corresponding to this range
+ *
+ * The following relationships must be true for the values used in this struct:
+ * (max_uV - min_uV) % step_uV == 0
+ * (set_point_min_uV - min_uV) % step_uV == 0*
+ * (set_point_max_uV - min_uV) % step_uV == 0*
+ * n_voltages = (set_point_max_uV - set_point_min_uV) / step_uV + 1
+ *
+ * *Note, set_point_min_uV == set_point_max_uV == 0 is allowed in order to
+ * specify that the voltage range has meaning, but is not preferred.
+ */
+struct spmi_voltage_range {
+	int					min_uV;
+	int					max_uV;
+	int					step_uV;
+	int					set_point_min_uV;
+	int					set_point_max_uV;
+	unsigned				n_voltages;
+	u8					range_sel;
+};
+
+/*
+ * The ranges specified in the spmi_voltage_set_points struct must be listed
+ * so that range[i].set_point_max_uV < range[i+1].set_point_min_uV.
+ */
+struct spmi_voltage_set_points {
+	struct spmi_voltage_range		*range;
+	int					count;
+	unsigned				n_voltages;
+};
+
+struct spmi_regulator {
+	struct regulator_desc			desc;
+	struct device				*dev;
+	struct delayed_work			ocp_work;
+	struct regmap				*regmap;
+	struct spmi_voltage_set_points		*set_points;
+	enum spmi_regulator_logical_type	logical_type;
+	int					ocp_irq;
+	int					ocp_count;
+	int					ocp_max_retries;
+	int					ocp_retry_delay_ms;
+	int					hpm_min_load;
+	int					slew_rate;
+	ktime_t					vs_enable_time;
+	u16					base;
+	struct list_head			node;
+};
+
+struct spmi_regulator_mapping {
+	enum spmi_regulator_type		type;
+	enum spmi_regulator_subtype		subtype;
+	enum spmi_regulator_logical_type	logical_type;
+	u32					revision_min;
+	u32					revision_max;
+	struct regulator_ops			*ops;
+	struct spmi_voltage_set_points		*set_points;
+	int					hpm_min_load;
+};
+
+struct spmi_regulator_data {
+	const char			*name;
+	u16				base;
+	const char			*supply;
+	const char			*ocp;
+	u16				force_type;
+};
+
+#define SPMI_VREG(_type, _subtype, _dig_major_min, _dig_major_max, \
+		      _logical_type, _ops_val, _set_points_val, _hpm_min_load) \
+	{ \
+		.type		= SPMI_REGULATOR_TYPE_##_type, \
+		.subtype	= SPMI_REGULATOR_SUBTYPE_##_subtype, \
+		.revision_min	= _dig_major_min, \
+		.revision_max	= _dig_major_max, \
+		.logical_type	= SPMI_REGULATOR_LOGICAL_TYPE_##_logical_type, \
+		.ops		= &spmi_##_ops_val##_ops, \
+		.set_points	= &_set_points_val##_set_points, \
+		.hpm_min_load	= _hpm_min_load, \
+	}
+
+#define SPMI_VREG_VS(_subtype, _dig_major_min, _dig_major_max) \
+	{ \
+		.type		= SPMI_REGULATOR_TYPE_VS, \
+		.subtype	= SPMI_REGULATOR_SUBTYPE_##_subtype, \
+		.revision_min	= _dig_major_min, \
+		.revision_max	= _dig_major_max, \
+		.logical_type	= SPMI_REGULATOR_LOGICAL_TYPE_VS, \
+		.ops		= &spmi_vs_ops, \
+	}
+
+#define SPMI_VOLTAGE_RANGE(_range_sel, _min_uV, _set_point_min_uV, \
+			_set_point_max_uV, _max_uV, _step_uV) \
+	{ \
+		.min_uV			= _min_uV, \
+		.max_uV			= _max_uV, \
+		.set_point_min_uV	= _set_point_min_uV, \
+		.set_point_max_uV	= _set_point_max_uV, \
+		.step_uV		= _step_uV, \
+		.range_sel		= _range_sel, \
+	}
+
+#define DEFINE_SPMI_SET_POINTS(name) \
+struct spmi_voltage_set_points name##_set_points = { \
+	.range	= name##_ranges, \
+	.count	= ARRAY_SIZE(name##_ranges), \
+}
+
+/*
+ * These tables contain the physically available PMIC regulator voltage setpoint
+ * ranges.  Where two ranges overlap in hardware, one of the ranges is trimmed
+ * to ensure that the setpoints available to software are monotonically
+ * increasing and unique.  The set_voltage callback functions expect these
+ * properties to hold.
+ */
+static struct spmi_voltage_range pldo_ranges[] = {
+	SPMI_VOLTAGE_RANGE(2,  750000,  750000, 1537500, 1537500, 12500),
+	SPMI_VOLTAGE_RANGE(3, 1500000, 1550000, 3075000, 3075000, 25000),
+	SPMI_VOLTAGE_RANGE(4, 1750000, 3100000, 4900000, 4900000, 50000),
+};
+
+static struct spmi_voltage_range nldo1_ranges[] = {
+	SPMI_VOLTAGE_RANGE(2,  750000,  750000, 1537500, 1537500, 12500),
+};
+
+static struct spmi_voltage_range nldo2_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0,  375000,       0,       0, 1537500, 12500),
+	SPMI_VOLTAGE_RANGE(1,  375000,  375000,  768750,  768750,  6250),
+	SPMI_VOLTAGE_RANGE(2,  750000,  775000, 1537500, 1537500, 12500),
+};
+
+static struct spmi_voltage_range nldo3_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0,  375000,  375000, 1537500, 1537500, 12500),
+	SPMI_VOLTAGE_RANGE(1,  375000,       0,       0, 1537500, 12500),
+	SPMI_VOLTAGE_RANGE(2,  750000,       0,       0, 1537500, 12500),
+};
+
+static struct spmi_voltage_range ln_ldo_ranges[] = {
+	SPMI_VOLTAGE_RANGE(1,  690000,  690000, 1110000, 1110000, 60000),
+	SPMI_VOLTAGE_RANGE(0, 1380000, 1380000, 2220000, 2220000, 120000),
+};
+
+static struct spmi_voltage_range smps_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0,  375000,  375000, 1562500, 1562500, 12500),
+	SPMI_VOLTAGE_RANGE(1, 1550000, 1575000, 3125000, 3125000, 25000),
+};
+
+static struct spmi_voltage_range ftsmps_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0,       0,  350000, 1275000, 1275000,  5000),
+	SPMI_VOLTAGE_RANGE(1,       0, 1280000, 2040000, 2040000, 10000),
+};
+
+static struct spmi_voltage_range ftsmps2p5_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0,   80000,  350000, 1355000, 1355000,  5000),
+	SPMI_VOLTAGE_RANGE(1,  160000, 1360000, 2200000, 2200000, 10000),
+};
+
+static struct spmi_voltage_range boost_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0, 4000000, 4000000, 5550000, 5550000, 50000),
+};
+
+static struct spmi_voltage_range boost_byp_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0, 2500000, 2500000, 5200000, 5650000, 50000),
+};
+
+static struct spmi_voltage_range ult_lo_smps_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0,  375000,  375000, 1562500, 1562500, 12500),
+	SPMI_VOLTAGE_RANGE(1,  750000,       0,       0, 1525000, 25000),
+};
+
+static struct spmi_voltage_range ult_ho_smps_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0, 1550000, 1550000, 2325000, 2325000, 25000),
+};
+
+static struct spmi_voltage_range ult_nldo_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0,  375000,  375000, 1537500, 1537500, 12500),
+};
+
+static struct spmi_voltage_range ult_pldo_ranges[] = {
+	SPMI_VOLTAGE_RANGE(0, 1750000, 1750000, 3337500, 3337500, 12500),
+};
+
+static DEFINE_SPMI_SET_POINTS(pldo);
+static DEFINE_SPMI_SET_POINTS(nldo1);
+static DEFINE_SPMI_SET_POINTS(nldo2);
+static DEFINE_SPMI_SET_POINTS(nldo3);
+static DEFINE_SPMI_SET_POINTS(ln_ldo);
+static DEFINE_SPMI_SET_POINTS(smps);
+static DEFINE_SPMI_SET_POINTS(ftsmps);
+static DEFINE_SPMI_SET_POINTS(ftsmps2p5);
+static DEFINE_SPMI_SET_POINTS(boost);
+static DEFINE_SPMI_SET_POINTS(boost_byp);
+static DEFINE_SPMI_SET_POINTS(ult_lo_smps);
+static DEFINE_SPMI_SET_POINTS(ult_ho_smps);
+static DEFINE_SPMI_SET_POINTS(ult_nldo);
+static DEFINE_SPMI_SET_POINTS(ult_pldo);
+
+static inline int spmi_vreg_read(struct spmi_regulator *vreg, u16 addr, u8 *buf,
+				 int len)
+{
+	return regmap_bulk_read(vreg->regmap, vreg->base + addr, buf, len);
+}
+
+static inline int spmi_vreg_write(struct spmi_regulator *vreg, u16 addr,
+				u8 *buf, int len)
+{
+	return regmap_bulk_write(vreg->regmap, vreg->base + addr, buf, len);
+}
+
+static int spmi_vreg_update_bits(struct spmi_regulator *vreg, u16 addr, u8 val,
+		u8 mask)
+{
+	return regmap_update_bits(vreg->regmap, vreg->base + addr, mask, val);
+}
+
+static int spmi_regulator_common_is_enabled(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	u8 reg;
+
+	spmi_vreg_read(vreg, SPMI_COMMON_REG_ENABLE, &reg, 1);
+
+	return (reg & SPMI_COMMON_ENABLE_MASK) == SPMI_COMMON_ENABLE;
+}
+
+static int spmi_regulator_common_enable(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+
+	return spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_ENABLE,
+		SPMI_COMMON_ENABLE, SPMI_COMMON_ENABLE_MASK);
+}
+
+static int spmi_regulator_vs_enable(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+
+	if (vreg->ocp_irq) {
+		vreg->ocp_count = 0;
+		vreg->vs_enable_time = ktime_get();
+	}
+
+	return spmi_regulator_common_enable(rdev);
+}
+
+static int spmi_regulator_common_disable(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+
+	return spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_ENABLE,
+		SPMI_COMMON_DISABLE, SPMI_COMMON_ENABLE_MASK);
+}
+
+static int spmi_regulator_select_voltage(struct spmi_regulator *vreg,
+		int min_uV, int max_uV, u8 *range_sel, u8 *voltage_sel,
+		unsigned *selector)
+{
+	const struct spmi_voltage_range *range;
+	int uV = min_uV;
+	int lim_min_uV, lim_max_uV, i, range_id, range_max_uV;
+
+	/* Check if request voltage is outside of physically settable range. */
+	lim_min_uV = vreg->set_points->range[0].set_point_min_uV;
+	lim_max_uV =
+	  vreg->set_points->range[vreg->set_points->count - 1].set_point_max_uV;
+
+	if (uV < lim_min_uV && max_uV >= lim_min_uV)
+		uV = lim_min_uV;
+
+	if (uV < lim_min_uV || uV > lim_max_uV) {
+		dev_err(vreg->dev,
+			"request v=[%d, %d] is outside possible v=[%d, %d]\n",
+			 min_uV, max_uV, lim_min_uV, lim_max_uV);
+		return -EINVAL;
+	}
+
+	/* Find the range which uV is inside of. */
+	for (i = vreg->set_points->count - 1; i > 0; i--) {
+		range_max_uV = vreg->set_points->range[i - 1].set_point_max_uV;
+		if (uV > range_max_uV && range_max_uV > 0)
+			break;
+	}
+
+	range_id = i;
+	range = &vreg->set_points->range[range_id];
+	*range_sel = range->range_sel;
+
+	/*
+	 * Force uV to be an allowed set point by applying a ceiling function to
+	 * the uV value.
+	 */
+	*voltage_sel = (uV - range->min_uV + range->step_uV - 1)
+			/ range->step_uV;
+	uV = *voltage_sel * range->step_uV + range->min_uV;
+
+	if (uV > max_uV) {
+		dev_err(vreg->dev,
+			"request v=[%d, %d] cannot be met by any set point; "
+			"next set point: %d\n",
+			min_uV, max_uV, uV);
+		return -EINVAL;
+	}
+
+	*selector = 0;
+	for (i = 0; i < range_id; i++)
+		*selector += vreg->set_points->range[i].n_voltages;
+	*selector += (uV - range->set_point_min_uV) / range->step_uV;
+
+	return 0;
+}
+
+static const struct spmi_voltage_range *
+spmi_regulator_find_range(struct spmi_regulator *vreg)
+{
+	u8 range_sel;
+	const struct spmi_voltage_range *range, *end;
+
+	range = vreg->set_points->range;
+	end = range + vreg->set_points->count;
+
+	spmi_vreg_read(vreg, SPMI_COMMON_REG_VOLTAGE_RANGE, &range_sel, 1);
+
+	for (; range < end; range++)
+		if (range->range_sel == range_sel)
+			return range;
+
+	return NULL;
+}
+
+static int spmi_regulator_select_voltage_same_range(struct spmi_regulator *vreg,
+		int min_uV, int max_uV, u8 *range_sel, u8 *voltage_sel,
+		unsigned *selector)
+{
+	const struct spmi_voltage_range *range;
+	int uV = min_uV;
+	int i;
+
+	range = spmi_regulator_find_range(vreg);
+	if (!range)
+		goto different_range;
+
+	if (uV < range->min_uV && max_uV >= range->min_uV)
+		uV = range->min_uV;
+
+	if (uV < range->min_uV || uV > range->max_uV) {
+		/* Current range doesn't support the requested voltage. */
+		goto different_range;
+	}
+
+	/*
+	 * Force uV to be an allowed set point by applying a ceiling function to
+	 * the uV value.
+	 */
+	*voltage_sel = DIV_ROUND_UP(uV - range->min_uV, range->step_uV);
+	uV = *voltage_sel * range->step_uV + range->min_uV;
+
+	if (uV > max_uV) {
+		/*
+		 * No set point in the current voltage range is within the
+		 * requested min_uV to max_uV range.
+		 */
+		goto different_range;
+	}
+
+	*selector = 0;
+	for (i = 0; i < vreg->set_points->count; i++) {
+		if (uV >= vreg->set_points->range[i].set_point_min_uV
+		    && uV <= vreg->set_points->range[i].set_point_max_uV) {
+			*selector +=
+			    (uV - vreg->set_points->range[i].set_point_min_uV)
+				/ vreg->set_points->range[i].step_uV;
+			break;
+		}
+
+		*selector += vreg->set_points->range[i].n_voltages;
+	}
+
+	if (*selector >= vreg->set_points->n_voltages)
+		goto different_range;
+
+	return 0;
+
+different_range:
+	return spmi_regulator_select_voltage(vreg, min_uV, max_uV,
+			range_sel, voltage_sel, selector);
+}
+
+static int spmi_regulator_common_set_voltage(struct regulator_dev *rdev,
+		int min_uV, int max_uV, unsigned *selector)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	int ret;
+	u8 buf[2];
+	u8 range_sel, voltage_sel;
+
+	/*
+	 * Favor staying in the current voltage range if possible.  This avoids
+	 * voltage spikes that occur when changing the voltage range.
+	 */
+	ret = spmi_regulator_select_voltage_same_range(vreg, min_uV, max_uV,
+		&range_sel, &voltage_sel, selector);
+	if (ret)
+		return ret;
+
+	buf[0] = range_sel;
+	buf[1] = voltage_sel;
+	return spmi_vreg_write(vreg, SPMI_COMMON_REG_VOLTAGE_RANGE, buf, 2);
+}
+
+static int spmi_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
+		unsigned int old_selector, unsigned int new_selector)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	const struct spmi_voltage_range *range;
+	int diff_uV;
+
+	range = spmi_regulator_find_range(vreg);
+	if (!range)
+		return -EINVAL;
+
+	diff_uV = abs(new_selector - old_selector) * range->step_uV;
+
+	return DIV_ROUND_UP(diff_uV, vreg->slew_rate);
+}
+
+static int spmi_regulator_common_get_voltage(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	const struct spmi_voltage_range *range;
+	u8 voltage_sel;
+
+	spmi_vreg_read(vreg, SPMI_COMMON_REG_VOLTAGE_SET, &voltage_sel, 1);
+
+	range = spmi_regulator_find_range(vreg);
+	if (!range)
+		return VOLTAGE_UNKNOWN;
+
+	return range->step_uV * voltage_sel + range->min_uV;
+}
+
+static int spmi_regulator_single_range_set_voltage(struct regulator_dev *rdev,
+		int min_uV, int max_uV, unsigned *selector)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	int ret;
+	u8 range_sel, sel;
+
+	ret = spmi_regulator_select_voltage(vreg, min_uV, max_uV, &range_sel,
+		&sel, selector);
+	if (ret) {
+		dev_err(vreg->dev, "could not set voltage, ret=%d\n", ret);
+		return ret;
+	}
+
+	/*
+	 * Certain types of regulators do not have a range select register so
+	 * only voltage set register needs to be written.
+	 */
+	return spmi_vreg_write(vreg, SPMI_COMMON_REG_VOLTAGE_SET, &sel, 1);
+}
+
+static int spmi_regulator_single_range_get_voltage(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	const struct spmi_voltage_range *range = vreg->set_points->range;
+	u8 voltage_sel;
+
+	spmi_vreg_read(vreg, SPMI_COMMON_REG_VOLTAGE_SET, &voltage_sel, 1);
+
+	return range->step_uV * voltage_sel + range->min_uV;
+}
+
+static int spmi_regulator_ult_lo_smps_set_voltage(struct regulator_dev *rdev,
+		int min_uV, int max_uV, unsigned *selector)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	int ret;
+	u8 range_sel, voltage_sel;
+
+	/*
+	 * Favor staying in the current voltage range if possible. This avoids
+	 * voltage spikes that occur when changing the voltage range.
+	 */
+	ret = spmi_regulator_select_voltage_same_range(vreg, min_uV, max_uV,
+		&range_sel, &voltage_sel, selector);
+	if (ret)
+		return ret;
+
+	/*
+	 * Calculate VSET based on range
+	 * In case of range 0: voltage_sel is a 7 bit value, can be written
+	 *			witout any modification.
+	 * In case of range 1: voltage_sel is a 5 bit value, bits[7-5] set to
+	 *			[011].
+	 */
+	if (range_sel == 1)
+		voltage_sel |= ULT_SMPS_RANGE_SPLIT;
+
+	return spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_VOLTAGE_SET,
+	       voltage_sel, 0xff);
+}
+
+static int spmi_regulator_ult_lo_smps_get_voltage(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	const struct spmi_voltage_range *range;
+	u8 voltage_sel;
+
+	spmi_vreg_read(vreg, SPMI_COMMON_REG_VOLTAGE_SET, &voltage_sel, 1);
+
+	range = spmi_regulator_find_range(vreg);
+	if (!range)
+		return VOLTAGE_UNKNOWN;
+
+	if (range->range_sel == 1)
+		voltage_sel &= ~ULT_SMPS_RANGE_SPLIT;
+
+	return range->step_uV * voltage_sel + range->min_uV;
+}
+
+static int spmi_regulator_common_list_voltage(struct regulator_dev *rdev,
+			unsigned selector)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	int uV = 0;
+	int i;
+
+	if (selector >= vreg->set_points->n_voltages)
+		return 0;
+
+	for (i = 0; i < vreg->set_points->count; i++) {
+		if (selector < vreg->set_points->range[i].n_voltages) {
+			uV = selector * vreg->set_points->range[i].step_uV
+				+ vreg->set_points->range[i].set_point_min_uV;
+			break;
+		}
+
+		selector -= vreg->set_points->range[i].n_voltages;
+	}
+
+	return uV;
+}
+
+static int
+spmi_regulator_common_set_bypass(struct regulator_dev *rdev, bool enable)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	u8 mask = SPMI_COMMON_MODE_BYPASS_MASK;
+	u8 val = 0;
+
+	if (enable)
+		val = mask;
+
+	return spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_MODE, val, mask);
+}
+
+static int
+spmi_regulator_common_get_bypass(struct regulator_dev *rdev, bool *enable)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	u8 val;
+	int ret;
+
+	ret = spmi_vreg_read(vreg, SPMI_COMMON_REG_MODE, &val, 1);
+	*enable = val & SPMI_COMMON_MODE_BYPASS_MASK;
+
+	return ret;
+}
+
+static unsigned int spmi_regulator_common_get_mode(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	u8 reg;
+
+	spmi_vreg_read(vreg, SPMI_COMMON_REG_MODE, &reg, 1);
+
+	if (reg & SPMI_COMMON_MODE_HPM_MASK)
+		return REGULATOR_MODE_NORMAL;
+
+	return REGULATOR_MODE_IDLE;
+}
+
+static int
+spmi_regulator_common_set_mode(struct regulator_dev *rdev, unsigned int mode)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	u8 mask = SPMI_COMMON_MODE_HPM_MASK;
+	u8 val = 0;
+
+	if (mode == REGULATOR_MODE_NORMAL)
+		val = mask;
+
+	return spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_MODE, val, mask);
+}
+
+static int
+spmi_regulator_common_set_load(struct regulator_dev *rdev, int load_uA)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	unsigned int mode;
+
+	if (load_uA >= vreg->hpm_min_load)
+		mode = REGULATOR_MODE_NORMAL;
+	else
+		mode = REGULATOR_MODE_IDLE;
+
+	return spmi_regulator_common_set_mode(rdev, mode);
+}
+
+static int spmi_regulator_common_set_pull_down(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	unsigned int mask = SPMI_COMMON_PULL_DOWN_ENABLE_MASK;
+
+	return spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_PULL_DOWN,
+				     mask, mask);
+}
+
+static int spmi_regulator_common_set_soft_start(struct regulator_dev *rdev)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	unsigned int mask = SPMI_LDO_SOFT_START_ENABLE_MASK;
+
+	return spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_SOFT_START,
+				     mask, mask);
+}
+
+static int spmi_regulator_set_ilim(struct regulator_dev *rdev, int ilim_uA)
+{
+	struct spmi_regulator *vreg = rdev_get_drvdata(rdev);
+	enum spmi_regulator_logical_type type = vreg->logical_type;
+	unsigned int current_reg;
+	u8 reg;
+	u8 mask = SPMI_BOOST_CURRENT_LIMIT_MASK |
+		  SPMI_BOOST_CURRENT_LIMIT_ENABLE_MASK;
+	int max = (SPMI_BOOST_CURRENT_LIMIT_MASK + 1) * 500;
+
+	if (type == SPMI_REGULATOR_LOGICAL_TYPE_BOOST)
+		current_reg = SPMI_BOOST_REG_CURRENT_LIMIT;
+	else
+		current_reg = SPMI_BOOST_BYP_REG_CURRENT_LIMIT;
+
+	if (ilim_uA > max || ilim_uA <= 0)
+		return -EINVAL;
+
+	reg = (ilim_uA - 1) / 500;
+	reg |= SPMI_BOOST_CURRENT_LIMIT_ENABLE_MASK;
+
+	return spmi_vreg_update_bits(vreg, current_reg, reg, mask);
+}
+
+static int spmi_regulator_vs_clear_ocp(struct spmi_regulator *vreg)
+{
+	int ret;
+
+	ret = spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_ENABLE,
+		SPMI_COMMON_DISABLE, SPMI_COMMON_ENABLE_MASK);
+
+	vreg->vs_enable_time = ktime_get();
+
+	ret = spmi_vreg_update_bits(vreg, SPMI_COMMON_REG_ENABLE,
+		SPMI_COMMON_ENABLE, SPMI_COMMON_ENABLE_MASK);
+
+	return ret;
+}
+
+static void spmi_regulator_vs_ocp_work(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct spmi_regulator *vreg
+		= container_of(dwork, struct spmi_regulator, ocp_work);
+
+	spmi_regulator_vs_clear_ocp(vreg);
+}
+
+static irqreturn_t spmi_regulator_vs_ocp_isr(int irq, void *data)
+{
+	struct spmi_regulator *vreg = data;
+	ktime_t ocp_irq_time;
+	s64 ocp_trigger_delay_us;
+
+	ocp_irq_time = ktime_get();
+	ocp_trigger_delay_us = ktime_us_delta(ocp_irq_time,
+						vreg->vs_enable_time);
+
+	/*
+	 * Reset the OCP count if there is a large delay between switch enable
+	 * and when OCP triggers.  This is indicative of a hotplug event as
+	 * opposed to a fault.
+	 */
+	if (ocp_trigger_delay_us > SPMI_VS_OCP_FAULT_DELAY_US)
+		vreg->ocp_count = 0;
+
+	/* Wait for switch output to settle back to 0 V after OCP triggered. */
+	udelay(SPMI_VS_OCP_FALL_DELAY_US);
+
+	vreg->ocp_count++;
+
+	if (vreg->ocp_count == 1) {
+		/* Immediately clear the over current condition. */
+		spmi_regulator_vs_clear_ocp(vreg);
+	} else if (vreg->ocp_count <= vreg->ocp_max_retries) {
+		/* Schedule the over current clear task to run later. */
+		schedule_delayed_work(&vreg->ocp_work,
+			msecs_to_jiffies(vreg->ocp_retry_delay_ms) + 1);
+	} else {
+		dev_err(vreg->dev,
+			"OCP triggered %d times; no further retries\n",
+			vreg->ocp_count);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static struct regulator_ops spmi_smps_ops = {
+	.enable			= spmi_regulator_common_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_voltage		= spmi_regulator_common_set_voltage,
+	.get_voltage		= spmi_regulator_common_get_voltage,
+	.list_voltage		= spmi_regulator_common_list_voltage,
+	.set_mode		= spmi_regulator_common_set_mode,
+	.get_mode		= spmi_regulator_common_get_mode,
+	.set_load		= spmi_regulator_common_set_load,
+	.set_pull_down		= spmi_regulator_common_set_pull_down,
+};
+
+static struct regulator_ops spmi_ldo_ops = {
+	.enable			= spmi_regulator_common_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_voltage		= spmi_regulator_common_set_voltage,
+	.get_voltage		= spmi_regulator_common_get_voltage,
+	.list_voltage		= spmi_regulator_common_list_voltage,
+	.set_mode		= spmi_regulator_common_set_mode,
+	.get_mode		= spmi_regulator_common_get_mode,
+	.set_load		= spmi_regulator_common_set_load,
+	.set_bypass		= spmi_regulator_common_set_bypass,
+	.get_bypass		= spmi_regulator_common_get_bypass,
+	.set_pull_down		= spmi_regulator_common_set_pull_down,
+	.set_soft_start		= spmi_regulator_common_set_soft_start,
+};
+
+static struct regulator_ops spmi_ln_ldo_ops = {
+	.enable			= spmi_regulator_common_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_voltage		= spmi_regulator_common_set_voltage,
+	.get_voltage		= spmi_regulator_common_get_voltage,
+	.list_voltage		= spmi_regulator_common_list_voltage,
+	.set_bypass		= spmi_regulator_common_set_bypass,
+	.get_bypass		= spmi_regulator_common_get_bypass,
+};
+
+static struct regulator_ops spmi_vs_ops = {
+	.enable			= spmi_regulator_vs_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_pull_down		= spmi_regulator_common_set_pull_down,
+	.set_soft_start		= spmi_regulator_common_set_soft_start,
+};
+
+static struct regulator_ops spmi_boost_ops = {
+	.enable			= spmi_regulator_common_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_voltage		= spmi_regulator_single_range_set_voltage,
+	.get_voltage		= spmi_regulator_single_range_get_voltage,
+	.list_voltage		= spmi_regulator_common_list_voltage,
+	.set_input_current_limit = spmi_regulator_set_ilim,
+};
+
+static struct regulator_ops spmi_ftsmps_ops = {
+	.enable			= spmi_regulator_common_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_voltage		= spmi_regulator_common_set_voltage,
+	.set_voltage_time_sel	= spmi_regulator_set_voltage_time_sel,
+	.get_voltage		= spmi_regulator_common_get_voltage,
+	.list_voltage		= spmi_regulator_common_list_voltage,
+	.set_mode		= spmi_regulator_common_set_mode,
+	.get_mode		= spmi_regulator_common_get_mode,
+	.set_load		= spmi_regulator_common_set_load,
+	.set_pull_down		= spmi_regulator_common_set_pull_down,
+};
+
+static struct regulator_ops spmi_ult_lo_smps_ops = {
+	.enable			= spmi_regulator_common_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_voltage		= spmi_regulator_ult_lo_smps_set_voltage,
+	.get_voltage		= spmi_regulator_ult_lo_smps_get_voltage,
+	.list_voltage		= spmi_regulator_common_list_voltage,
+	.set_mode		= spmi_regulator_common_set_mode,
+	.get_mode		= spmi_regulator_common_get_mode,
+	.set_load		= spmi_regulator_common_set_load,
+	.set_pull_down		= spmi_regulator_common_set_pull_down,
+};
+
+static struct regulator_ops spmi_ult_ho_smps_ops = {
+	.enable			= spmi_regulator_common_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_voltage		= spmi_regulator_single_range_set_voltage,
+	.get_voltage		= spmi_regulator_single_range_get_voltage,
+	.list_voltage		= spmi_regulator_common_list_voltage,
+	.set_mode		= spmi_regulator_common_set_mode,
+	.get_mode		= spmi_regulator_common_get_mode,
+	.set_load		= spmi_regulator_common_set_load,
+	.set_pull_down		= spmi_regulator_common_set_pull_down,
+};
+
+static struct regulator_ops spmi_ult_ldo_ops = {
+	.enable			= spmi_regulator_common_enable,
+	.disable		= spmi_regulator_common_disable,
+	.is_enabled		= spmi_regulator_common_is_enabled,
+	.set_voltage		= spmi_regulator_single_range_set_voltage,
+	.get_voltage		= spmi_regulator_single_range_get_voltage,
+	.list_voltage		= spmi_regulator_common_list_voltage,
+	.set_mode		= spmi_regulator_common_set_mode,
+	.get_mode		= spmi_regulator_common_get_mode,
+	.set_load		= spmi_regulator_common_set_load,
+	.set_bypass		= spmi_regulator_common_set_bypass,
+	.get_bypass		= spmi_regulator_common_get_bypass,
+	.set_pull_down		= spmi_regulator_common_set_pull_down,
+	.set_soft_start		= spmi_regulator_common_set_soft_start,
+};
+
+/* Maximum possible digital major revision value */
+#define INF 0xFF
+
+static const struct spmi_regulator_mapping supported_regulators[] = {
+	/*           type subtype dig_min dig_max ltype ops setpoints hpm_min */
+	SPMI_VREG(BUCK,  GP_CTL,   0, INF, SMPS,   smps,   smps,   100000),
+	SPMI_VREG(LDO,   N300,     0, INF, LDO,    ldo,    nldo1,   10000),
+	SPMI_VREG(LDO,   N600,     0,   0, LDO,    ldo,    nldo2,   10000),
+	SPMI_VREG(LDO,   N1200,    0,   0, LDO,    ldo,    nldo2,   10000),
+	SPMI_VREG(LDO,   N600,     1, INF, LDO,    ldo,    nldo3,   10000),
+	SPMI_VREG(LDO,   N1200,    1, INF, LDO,    ldo,    nldo3,   10000),
+	SPMI_VREG(LDO,   N600_ST,  0,   0, LDO,    ldo,    nldo2,   10000),
+	SPMI_VREG(LDO,   N1200_ST, 0,   0, LDO,    ldo,    nldo2,   10000),
+	SPMI_VREG(LDO,   N600_ST,  1, INF, LDO,    ldo,    nldo3,   10000),
+	SPMI_VREG(LDO,   N1200_ST, 1, INF, LDO,    ldo,    nldo3,   10000),
+	SPMI_VREG(LDO,   P50,      0, INF, LDO,    ldo,    pldo,     5000),
+	SPMI_VREG(LDO,   P150,     0, INF, LDO,    ldo,    pldo,    10000),
+	SPMI_VREG(LDO,   P300,     0, INF, LDO,    ldo,    pldo,    10000),
+	SPMI_VREG(LDO,   P600,     0, INF, LDO,    ldo,    pldo,    10000),
+	SPMI_VREG(LDO,   P1200,    0, INF, LDO,    ldo,    pldo,    10000),
+	SPMI_VREG(LDO,   LN,       0, INF, LN_LDO, ln_ldo, ln_ldo,      0),
+	SPMI_VREG(LDO,   LV_P50,   0, INF, LDO,    ldo,    pldo,     5000),
+	SPMI_VREG(LDO,   LV_P150,  0, INF, LDO,    ldo,    pldo,    10000),
+	SPMI_VREG(LDO,   LV_P300,  0, INF, LDO,    ldo,    pldo,    10000),
+	SPMI_VREG(LDO,   LV_P600,  0, INF, LDO,    ldo,    pldo,    10000),
+	SPMI_VREG(LDO,   LV_P1200, 0, INF, LDO,    ldo,    pldo,    10000),
+	SPMI_VREG_VS(LV100,        0, INF),
+	SPMI_VREG_VS(LV300,        0, INF),
+	SPMI_VREG_VS(MV300,        0, INF),
+	SPMI_VREG_VS(MV500,        0, INF),
+	SPMI_VREG_VS(HDMI,         0, INF),
+	SPMI_VREG_VS(OTG,          0, INF),
+	SPMI_VREG(BOOST, 5V_BOOST, 0, INF, BOOST,  boost,  boost,       0),
+	SPMI_VREG(FTS,   FTS_CTL,  0, INF, FTSMPS, ftsmps, ftsmps, 100000),
+	SPMI_VREG(FTS, FTS2p5_CTL, 0, INF, FTSMPS, ftsmps, ftsmps2p5, 100000),
+	SPMI_VREG(BOOST_BYP, BB_2A, 0, INF, BOOST_BYP, boost, boost_byp, 0),
+	SPMI_VREG(ULT_BUCK, ULT_HF_CTL1, 0, INF, ULT_LO_SMPS, ult_lo_smps,
+						ult_lo_smps,   100000),
+	SPMI_VREG(ULT_BUCK, ULT_HF_CTL2, 0, INF, ULT_LO_SMPS, ult_lo_smps,
+						ult_lo_smps,   100000),
+	SPMI_VREG(ULT_BUCK, ULT_HF_CTL3, 0, INF, ULT_LO_SMPS, ult_lo_smps,
+						ult_lo_smps,   100000),
+	SPMI_VREG(ULT_BUCK, ULT_HF_CTL4, 0, INF, ULT_HO_SMPS, ult_ho_smps,
+						ult_ho_smps,   100000),
+	SPMI_VREG(ULT_LDO, N300_ST, 0, INF, ULT_LDO, ult_ldo, ult_nldo, 10000),
+	SPMI_VREG(ULT_LDO, N600_ST, 0, INF, ULT_LDO, ult_ldo, ult_nldo, 10000),
+	SPMI_VREG(ULT_LDO, N900_ST, 0, INF, ULT_LDO, ult_ldo, ult_nldo, 10000),
+	SPMI_VREG(ULT_LDO, N1200_ST, 0, INF, ULT_LDO, ult_ldo, ult_nldo, 10000),
+	SPMI_VREG(ULT_LDO, LV_P150,  0, INF, ULT_LDO, ult_ldo, ult_pldo, 10000),
+	SPMI_VREG(ULT_LDO, LV_P300,  0, INF, ULT_LDO, ult_ldo, ult_pldo, 10000),
+	SPMI_VREG(ULT_LDO, LV_P450,  0, INF, ULT_LDO, ult_ldo, ult_pldo, 10000),
+	SPMI_VREG(ULT_LDO, P600,     0, INF, ULT_LDO, ult_ldo, ult_pldo, 10000),
+	SPMI_VREG(ULT_LDO, P150,     0, INF, ULT_LDO, ult_ldo, ult_pldo, 10000),
+	SPMI_VREG(ULT_LDO, P50,     0, INF, ULT_LDO, ult_ldo, ult_pldo, 5000),
+};
+
+static void spmi_calculate_num_voltages(struct spmi_voltage_set_points *points)
+{
+	unsigned int n;
+	struct spmi_voltage_range *range = points->range;
+
+	for (; range < points->range + points->count; range++) {
+		n = 0;
+		if (range->set_point_max_uV) {
+			n = range->set_point_max_uV - range->set_point_min_uV;
+			n = (n / range->step_uV) + 1;
+		}
+		range->n_voltages = n;
+		points->n_voltages += n;
+	}
+}
+
+static int spmi_regulator_match(struct spmi_regulator *vreg, u16 force_type)
+{
+	const struct spmi_regulator_mapping *mapping;
+	int ret, i;
+	u32 dig_major_rev;
+	u8 version[SPMI_COMMON_REG_SUBTYPE - SPMI_COMMON_REG_DIG_MAJOR_REV + 1];
+	u8 type, subtype;
+
+	ret = spmi_vreg_read(vreg, SPMI_COMMON_REG_DIG_MAJOR_REV, version,
+		ARRAY_SIZE(version));
+	if (ret) {
+		dev_err(vreg->dev, "could not read version registers\n");
+		return ret;
+	}
+	dig_major_rev	= version[SPMI_COMMON_REG_DIG_MAJOR_REV
+					- SPMI_COMMON_REG_DIG_MAJOR_REV];
+	if (!force_type) {
+		type		= version[SPMI_COMMON_REG_TYPE -
+					  SPMI_COMMON_REG_DIG_MAJOR_REV];
+		subtype		= version[SPMI_COMMON_REG_SUBTYPE -
+					  SPMI_COMMON_REG_DIG_MAJOR_REV];
+	} else {
+		type = force_type >> 8;
+		subtype = force_type;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(supported_regulators); i++) {
+		mapping = &supported_regulators[i];
+		if (mapping->type == type && mapping->subtype == subtype
+		    && mapping->revision_min <= dig_major_rev
+		    && mapping->revision_max >= dig_major_rev)
+			goto found;
+	}
+
+	dev_err(vreg->dev,
+		"unsupported regulator: name=%s type=0x%02X, subtype=0x%02X, dig major rev=0x%02X\n",
+		vreg->desc.name, type, subtype, dig_major_rev);
+
+	return -ENODEV;
+
+found:
+	vreg->logical_type	= mapping->logical_type;
+	vreg->set_points	= mapping->set_points;
+	vreg->hpm_min_load	= mapping->hpm_min_load;
+	vreg->desc.ops		= mapping->ops;
+
+	if (mapping->set_points) {
+		if (!mapping->set_points->n_voltages)
+			spmi_calculate_num_voltages(mapping->set_points);
+		vreg->desc.n_voltages = mapping->set_points->n_voltages;
+	}
+
+	return 0;
+}
+
+static int spmi_regulator_ftsmps_init_slew_rate(struct spmi_regulator *vreg)
+{
+	int ret;
+	u8 reg = 0;
+	int step, delay, slew_rate;
+	const struct spmi_voltage_range *range;
+
+	ret = spmi_vreg_read(vreg, SPMI_COMMON_REG_STEP_CTRL, &reg, 1);
+	if (ret) {
+		dev_err(vreg->dev, "spmi read failed, ret=%d\n", ret);
+		return ret;
+	}
+
+	range = spmi_regulator_find_range(vreg);
+	if (!range)
+		return -EINVAL;
+
+	step = reg & SPMI_FTSMPS_STEP_CTRL_STEP_MASK;
+	step >>= SPMI_FTSMPS_STEP_CTRL_STEP_SHIFT;
+
+	delay = reg & SPMI_FTSMPS_STEP_CTRL_DELAY_MASK;
+	delay >>= SPMI_FTSMPS_STEP_CTRL_DELAY_SHIFT;
+
+	/* slew_rate has units of uV/us */
+	slew_rate = SPMI_FTSMPS_CLOCK_RATE * range->step_uV * (1 << step);
+	slew_rate /= 1000 * (SPMI_FTSMPS_STEP_DELAY << delay);
+	slew_rate *= SPMI_FTSMPS_STEP_MARGIN_NUM;
+	slew_rate /= SPMI_FTSMPS_STEP_MARGIN_DEN;
+
+	/* Ensure that the slew rate is greater than 0 */
+	vreg->slew_rate = max(slew_rate, 1);
+
+	return ret;
+}
+
+static unsigned int spmi_regulator_of_map_mode(unsigned int mode)
+{
+	if (mode)
+		return REGULATOR_MODE_NORMAL;
+
+	return REGULATOR_MODE_IDLE;
+}
+
+static int spmi_regulator_of_parse(struct device_node *node,
+			    const struct regulator_desc *desc,
+			    struct regulator_config *config)
+{
+	struct spmi_regulator *vreg = config->driver_data;
+	struct device *dev = config->dev;
+	int ret;
+
+	vreg->ocp_max_retries = SPMI_VS_OCP_DEFAULT_MAX_RETRIES;
+	vreg->ocp_retry_delay_ms = SPMI_VS_OCP_DEFAULT_RETRY_DELAY_MS;
+
+	if (vreg->logical_type == SPMI_REGULATOR_LOGICAL_TYPE_FTSMPS) {
+		ret = spmi_regulator_ftsmps_init_slew_rate(vreg);
+		if (ret)
+			return ret;
+	}
+
+	if (vreg->logical_type != SPMI_REGULATOR_LOGICAL_TYPE_VS)
+		vreg->ocp_irq = 0;
+
+	if (vreg->ocp_irq) {
+		ret = devm_request_irq(dev, vreg->ocp_irq,
+			spmi_regulator_vs_ocp_isr, IRQF_TRIGGER_RISING, "ocp",
+			vreg);
+		if (ret < 0) {
+			dev_err(dev, "failed to request irq %d, ret=%d\n",
+				vreg->ocp_irq, ret);
+			return ret;
+		}
+
+		INIT_DELAYED_WORK(&vreg->ocp_work, spmi_regulator_vs_ocp_work);
+	}
+
+	return 0;
+}
+
+static const struct spmi_regulator_data pm8941_regulators[] = {
+	{ "s1", 0x1400, "vdd_s1", },
+	{ "s2", 0x1700, "vdd_s2", },
+	{ "s3", 0x1a00, "vdd_s3", },
+	{ "l1", 0x4000, "vdd_l1_l3", },
+	{ "l2", 0x4100, "vdd_l2_lvs_1_2_3", },
+	{ "l3", 0x4200, "vdd_l1_l3", },
+	{ "l4", 0x4300, "vdd_l4_l11", },
+	{ "l5", 0x4400, "vdd_l5_l7", NULL, 0x0410 },
+	{ "l6", 0x4500, "vdd_l6_l12_l14_l15", },
+	{ "l7", 0x4600, "vdd_l5_l7", NULL, 0x0410 },
+	{ "l8", 0x4700, "vdd_l8_l16_l18_19", },
+	{ "l9", 0x4800, "vdd_l9_l10_l17_l22", },
+	{ "l10", 0x4900, "vdd_l9_l10_l17_l22", },
+	{ "l11", 0x4a00, "vdd_l4_l11", },
+	{ "l12", 0x4b00, "vdd_l6_l12_l14_l15", },
+	{ "l13", 0x4c00, "vdd_l13_l20_l23_l24", },
+	{ "l14", 0x4d00, "vdd_l6_l12_l14_l15", },
+	{ "l15", 0x4e00, "vdd_l6_l12_l14_l15", },
+	{ "l16", 0x4f00, "vdd_l8_l16_l18_19", },
+	{ "l17", 0x5000, "vdd_l9_l10_l17_l22", },
+	{ "l18", 0x5100, "vdd_l8_l16_l18_19", },
+	{ "l19", 0x5200, "vdd_l8_l16_l18_19", },
+	{ "l20", 0x5300, "vdd_l13_l20_l23_l24", },
+	{ "l21", 0x5400, "vdd_l21", },
+	{ "l22", 0x5500, "vdd_l9_l10_l17_l22", },
+	{ "l23", 0x5600, "vdd_l13_l20_l23_l24", },
+	{ "l24", 0x5700, "vdd_l13_l20_l23_l24", },
+	{ "lvs1", 0x8000, "vdd_l2_lvs_1_2_3", },
+	{ "lvs2", 0x8100, "vdd_l2_lvs_1_2_3", },
+	{ "lvs3", 0x8200, "vdd_l2_lvs_1_2_3", },
+	{ "mvs1", 0x8300, "vin_5vs", },
+	{ "mvs2", 0x8400, "vin_5vs", },
+	{ }
+};
+
+static const struct spmi_regulator_data pm8841_regulators[] = {
+	{ "s1", 0x1400, "vdd_s1", },
+	{ "s2", 0x1700, "vdd_s2", NULL, 0x1c08 },
+	{ "s3", 0x1a00, "vdd_s3", },
+	{ "s4", 0x1d00, "vdd_s4", NULL, 0x1c08 },
+	{ "s5", 0x2000, "vdd_s5", NULL, 0x1c08 },
+	{ "s6", 0x2300, "vdd_s6", NULL, 0x1c08 },
+	{ "s7", 0x2600, "vdd_s7", NULL, 0x1c08 },
+	{ "s8", 0x2900, "vdd_s8", NULL, 0x1c08 },
+	{ }
+};
+
+static const struct spmi_regulator_data pm8916_regulators[] = {
+	{ "s1", 0x1400, "vdd_s1", },
+	{ "s2", 0x1700, "vdd_s2", },
+	{ "s3", 0x1a00, "vdd_s3", },
+	{ "s4", 0x1d00, "vdd_s4", },
+	{ "l1", 0x4000, "vdd_l1_l3", },
+	{ "l2", 0x4100, "vdd_l2", },
+	{ "l3", 0x4200, "vdd_l1_l3", },
+	{ "l4", 0x4300, "vdd_l4_l5_l6", },
+	{ "l5", 0x4400, "vdd_l4_l5_l6", },
+	{ "l6", 0x4500, "vdd_l4_l5_l6", },
+	{ "l7", 0x4600, "vdd_l7", },
+	{ "l8", 0x4700, "vdd_l8_l11_l14_l15_l16", },
+	{ "l9", 0x4800, "vdd_l9_l10_l12_l13_l17_l18", },
+	{ "l10", 0x4900, "vdd_l9_l10_l12_l13_l17_l18", },
+	{ "l11", 0x4a00, "vdd_l8_l11_l14_l15_l16", },
+	{ "l12", 0x4b00, "vdd_l9_l10_l12_l13_l17_l18", },
+	{ "l13", 0x4c00, "vdd_l9_l10_l12_l13_l17_l18", },
+	{ "l14", 0x4d00, "vdd_l8_l11_l14_l15_l16", },
+	{ "l15", 0x4e00, "vdd_l8_l11_l14_l15_l16", },
+	{ "l16", 0x4f00, "vdd_l8_l11_l14_l15_l16", },
+	{ "l17", 0x5000, "vdd_l9_l10_l12_l13_l17_l18", },
+	{ "l18", 0x5100, "vdd_l9_l10_l12_l13_l17_l18", },
+	{ }
+};
+
+static const struct of_device_id qcom_spmi_regulator_match[] = {
+	{ .compatible = "qcom,pm8841-regulators", .data = &pm8841_regulators },
+	{ .compatible = "qcom,pm8916-regulators", .data = &pm8916_regulators },
+	{ .compatible = "qcom,pm8941-regulators", .data = &pm8941_regulators },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, qcom_spmi_regulator_match);
+
+static int qcom_spmi_regulator_probe(struct platform_device *pdev)
+{
+	const struct spmi_regulator_data *reg;
+	const struct of_device_id *match;
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+	struct spmi_regulator *vreg;
+	struct regmap *regmap;
+	const char *name;
+	struct device *dev = &pdev->dev;
+	int ret;
+	struct list_head *vreg_list;
+
+	vreg_list = devm_kzalloc(dev, sizeof(*vreg_list), GFP_KERNEL);
+	if (!vreg_list)
+		return -ENOMEM;
+	INIT_LIST_HEAD(vreg_list);
+	platform_set_drvdata(pdev, vreg_list);
+
+	regmap = dev_get_regmap(dev->parent, NULL);
+	if (!regmap)
+		return -ENODEV;
+
+	match = of_match_device(qcom_spmi_regulator_match, &pdev->dev);
+	if (!match)
+		return -ENODEV;
+
+	for (reg = match->data; reg->name; reg++) {
+		vreg = devm_kzalloc(dev, sizeof(*vreg), GFP_KERNEL);
+		if (!vreg)
+			return -ENOMEM;
+
+		vreg->dev = dev;
+		vreg->base = reg->base;
+		vreg->regmap = regmap;
+
+		if (reg->ocp) {
+			vreg->ocp_irq = platform_get_irq_byname(pdev, reg->ocp);
+			if (vreg->ocp_irq < 0) {
+				ret = vreg->ocp_irq;
+				goto err;
+			}
+		}
+
+		vreg->desc.id = -1;
+		vreg->desc.owner = THIS_MODULE;
+		vreg->desc.type = REGULATOR_VOLTAGE;
+		vreg->desc.name = name = reg->name;
+		vreg->desc.supply_name = reg->supply;
+		vreg->desc.of_match = reg->name;
+		vreg->desc.of_parse_cb = spmi_regulator_of_parse;
+		vreg->desc.of_map_mode = spmi_regulator_of_map_mode;
+
+		ret = spmi_regulator_match(vreg, reg->force_type);
+		if (ret)
+			goto err;
+
+		config.dev = dev;
+		config.driver_data = vreg;
+		rdev = devm_regulator_register(dev, &vreg->desc, &config);
+		if (IS_ERR(rdev)) {
+			dev_err(dev, "failed to register %s\n", name);
+			ret = PTR_ERR(rdev);
+			goto err;
+		}
+
+		INIT_LIST_HEAD(&vreg->node);
+		list_add(&vreg->node, vreg_list);
+	}
+
+	return 0;
+
+err:
+	list_for_each_entry(vreg, vreg_list, node)
+		if (vreg->ocp_irq)
+			cancel_delayed_work_sync(&vreg->ocp_work);
+	return ret;
+}
+
+static int qcom_spmi_regulator_remove(struct platform_device *pdev)
+{
+	struct spmi_regulator *vreg;
+	struct list_head *vreg_list = platform_get_drvdata(pdev);
+
+	list_for_each_entry(vreg, vreg_list, node)
+		if (vreg->ocp_irq)
+			cancel_delayed_work_sync(&vreg->ocp_work);
+
+	return 0;
+}
+
+static struct platform_driver qcom_spmi_regulator_driver = {
+	.driver		= {
+		.name	= "qcom-spmi-regulator",
+		.of_match_table = qcom_spmi_regulator_match,
+	},
+	.probe		= qcom_spmi_regulator_probe,
+	.remove		= qcom_spmi_regulator_remove,
+};
+module_platform_driver(qcom_spmi_regulator_driver);
+
+MODULE_DESCRIPTION("Qualcomm SPMI PMIC regulator driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:qcom-spmi-regulator");
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
index ff828117798f..326ffb553371 100644
--- a/drivers/regulator/s2mps11.c
+++ b/drivers/regulator/s2mps11.c
@@ -515,7 +515,7 @@ static int s2mps14_regulator_enable(struct regulator_dev *rdev)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
 			rdev->desc->enable_mask, val);
@@ -538,7 +538,7 @@ static int s2mps14_regulator_set_suspend_disable(struct regulator_dev *rdev)
 		default:
 			state = S2MPS14_ENABLE_SUSPEND;
 			break;
-		};
+		}
 		break;
 	case S2MPU02:
 		switch (rdev_id) {
@@ -552,11 +552,11 @@ static int s2mps14_regulator_set_suspend_disable(struct regulator_dev *rdev)
 		default:
 			state = S2MPU02_ENABLE_SUSPEND;
 			break;
-		};
+		}
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	ret = regmap_read(rdev->regmap, rdev->desc->enable_reg, &val);
 	if (ret < 0)
@@ -977,7 +977,7 @@ static int s2mps11_pmic_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Invalid device type: %u\n",
 				    s2mps11->dev_type);
 		return -EINVAL;
-	};
+	}
 
 	s2mps11->ext_control_gpio = devm_kmalloc(&pdev->dev,
 			sizeof(*s2mps11->ext_control_gpio) * s2mps11->rdev_num,
diff --git a/drivers/regulator/wm831x-dcdc.c b/drivers/regulator/wm831x-dcdc.c
index 0d7e164a5e76..8cbb82ceec40 100644
--- a/drivers/regulator/wm831x-dcdc.c
+++ b/drivers/regulator/wm831x-dcdc.c
@@ -533,7 +533,8 @@ static int wm831x_buckv_probe(struct platform_device *pdev)
 	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 					wm831x_dcdc_uv_irq,
-					IRQF_TRIGGER_RISING, dcdc->name, dcdc);
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					dcdc->name, dcdc);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
 			irq, ret);
@@ -543,7 +544,8 @@ static int wm831x_buckv_probe(struct platform_device *pdev)
 	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "HC"));
 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 					wm831x_dcdc_oc_irq,
-					IRQF_TRIGGER_RISING, dcdc->name, dcdc);
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					dcdc->name, dcdc);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request HC IRQ %d: %d\n",
 			irq, ret);
@@ -669,7 +671,8 @@ static int wm831x_buckp_probe(struct platform_device *pdev)
 	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 					wm831x_dcdc_uv_irq,
-					IRQF_TRIGGER_RISING, dcdc->name, dcdc);
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					dcdc->name, dcdc);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
 			irq, ret);
@@ -785,7 +788,8 @@ static int wm831x_boostp_probe(struct platform_device *pdev)
 	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 					wm831x_dcdc_uv_irq,
-					IRQF_TRIGGER_RISING, dcdc->name,
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					dcdc->name,
 					dcdc);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
diff --git a/drivers/regulator/wm831x-isink.c b/drivers/regulator/wm831x-isink.c
index 1e88391a1628..1442828fcd9a 100644
--- a/drivers/regulator/wm831x-isink.c
+++ b/drivers/regulator/wm831x-isink.c
@@ -204,7 +204,8 @@ static int wm831x_isink_probe(struct platform_device *pdev)
 	irq = wm831x_irq(wm831x, platform_get_irq(pdev, 0));
 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 					wm831x_isink_irq,
-					IRQF_TRIGGER_RISING, isink->name,
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					isink->name,
 					isink);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request ISINK IRQ %d: %d\n",
diff --git a/drivers/regulator/wm831x-ldo.c b/drivers/regulator/wm831x-ldo.c
index 7ae2dc82f636..5a7b65e8a529 100644
--- a/drivers/regulator/wm831x-ldo.c
+++ b/drivers/regulator/wm831x-ldo.c
@@ -287,7 +287,8 @@ static int wm831x_gp_ldo_probe(struct platform_device *pdev)
 	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 					wm831x_ldo_uv_irq,
-					IRQF_TRIGGER_RISING, ldo->name,
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					ldo->name,
 					ldo);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
@@ -496,7 +497,8 @@ static int wm831x_aldo_probe(struct platform_device *pdev)
 	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "UV"));
 	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 					wm831x_ldo_uv_irq,
-					IRQF_TRIGGER_RISING, ldo->name, ldo);
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					ldo->name, ldo);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "Failed to request UV IRQ %d: %d\n",
 			irq, ret);
diff --git a/drivers/remoteproc/Kconfig b/drivers/remoteproc/Kconfig
index 5e343bab9458..28c711f0ac6b 100644
--- a/drivers/remoteproc/Kconfig
+++ b/drivers/remoteproc/Kconfig
@@ -41,6 +41,19 @@ config STE_MODEM_RPROC
 	  This can be either built-in or a loadable module.
 	  If unsure say N.
 
+config WKUP_M3_RPROC
+	tristate "AMx3xx Wakeup M3 remoteproc support"
+	depends on SOC_AM33XX || SOC_AM43XX
+	select REMOTEPROC
+	help
+	  Say y here to support Wakeup M3 remote processor on TI AM33xx
+	  and AM43xx family of SoCs.
+
+	  Required for Suspend-to-RAM on AM33xx and AM43xx SoCs. Also needed
+	  for deep CPUIdle states on AM33xx SoCs. Allows for loading of the
+	  firmware onto these remote processors.
+	  If unsure say N.
+
 config DA8XX_REMOTEPROC
 	tristate "DA8xx/OMAP-L13x remoteproc support"
 	depends on ARCH_DAVINCI_DA8XX
diff --git a/drivers/remoteproc/Makefile b/drivers/remoteproc/Makefile
index ac2ff75686d2..81b04d1e2e58 100644
--- a/drivers/remoteproc/Makefile
+++ b/drivers/remoteproc/Makefile
@@ -9,4 +9,5 @@ remoteproc-y				+= remoteproc_virtio.o
 remoteproc-y				+= remoteproc_elf_loader.o
 obj-$(CONFIG_OMAP_REMOTEPROC)		+= omap_remoteproc.o
 obj-$(CONFIG_STE_MODEM_RPROC)	 	+= ste_modem_rproc.o
+obj-$(CONFIG_WKUP_M3_RPROC)		+= wkup_m3_rproc.o
 obj-$(CONFIG_DA8XX_REMOTEPROC)		+= da8xx_remoteproc.o
diff --git a/drivers/remoteproc/da8xx_remoteproc.c b/drivers/remoteproc/da8xx_remoteproc.c
index f8d6a0661c14..009e56f67de2 100644
--- a/drivers/remoteproc/da8xx_remoteproc.c
+++ b/drivers/remoteproc/da8xx_remoteproc.c
@@ -26,8 +26,7 @@
 static char *da8xx_fw_name;
 module_param(da8xx_fw_name, charp, S_IRUGO);
 MODULE_PARM_DESC(da8xx_fw_name,
-		 "\n\t\tName of DSP firmware file in /lib/firmware"
-		 " (if not specified defaults to 'rproc-dsp-fw')");
+		 "Name of DSP firmware file in /lib/firmware (if not specified defaults to 'rproc-dsp-fw')");
 
 /*
  * OMAP-L138 Technical References:
diff --git a/drivers/remoteproc/remoteproc_core.c b/drivers/remoteproc/remoteproc_core.c
index 11cdb119e4f3..8b3130f22b42 100644
--- a/drivers/remoteproc/remoteproc_core.c
+++ b/drivers/remoteproc/remoteproc_core.c
@@ -44,6 +44,9 @@
 
 #include "remoteproc_internal.h"
 
+static DEFINE_MUTEX(rproc_list_mutex);
+static LIST_HEAD(rproc_list);
+
 typedef int (*rproc_handle_resources_t)(struct rproc *rproc,
 				struct resource_table *table, int len);
 typedef int (*rproc_handle_resource_t)(struct rproc *rproc,
@@ -132,32 +135,48 @@ static void rproc_disable_iommu(struct rproc *rproc)
 
 	iommu_detach_device(domain, dev);
 	iommu_domain_free(domain);
-
-	return;
 }
 
-/*
+/**
+ * rproc_da_to_va() - lookup the kernel virtual address for a remoteproc address
+ * @rproc: handle of a remote processor
+ * @da: remoteproc device address to translate
+ * @len: length of the memory region @da is pointing to
+ *
  * Some remote processors will ask us to allocate them physically contiguous
  * memory regions (which we call "carveouts"), and map them to specific
- * device addresses (which are hardcoded in the firmware).
+ * device addresses (which are hardcoded in the firmware). They may also have
+ * dedicated memory regions internal to the processors, and use them either
+ * exclusively or alongside carveouts.
  *
  * They may then ask us to copy objects into specific device addresses (e.g.
  * code/data sections) or expose us certain symbols in other device address
  * (e.g. their trace buffer).
  *
- * This function is an internal helper with which we can go over the allocated
- * carveouts and translate specific device address to kernel virtual addresses
- * so we can access the referenced memory.
+ * This function is a helper function with which we can go over the allocated
+ * carveouts and translate specific device addresses to kernel virtual addresses
+ * so we can access the referenced memory. This function also allows to perform
+ * translations on the internal remoteproc memory regions through a platform
+ * implementation specific da_to_va ops, if present.
+ *
+ * The function returns a valid kernel address on success or NULL on failure.
  *
  * Note: phys_to_virt(iommu_iova_to_phys(rproc->domain, da)) will work too,
  * but only on kernel direct mapped RAM memory. Instead, we're just using
- * here the output of the DMA API, which should be more correct.
+ * here the output of the DMA API for the carveouts, which should be more
+ * correct.
  */
 void *rproc_da_to_va(struct rproc *rproc, u64 da, int len)
 {
 	struct rproc_mem_entry *carveout;
 	void *ptr = NULL;
 
+	if (rproc->ops->da_to_va) {
+		ptr = rproc->ops->da_to_va(rproc, da, len);
+		if (ptr)
+			goto out;
+	}
+
 	list_for_each_entry(carveout, &rproc->carveouts, node) {
 		int offset = da - carveout->da;
 
@@ -174,6 +193,7 @@ void *rproc_da_to_va(struct rproc *rproc, u64 da, int len)
 		break;
 	}
 
+out:
 	return ptr;
 }
 EXPORT_SYMBOL(rproc_da_to_va);
@@ -411,10 +431,8 @@ static int rproc_handle_trace(struct rproc *rproc, struct fw_rsc_trace *rsc,
 	}
 
 	trace = kzalloc(sizeof(*trace), GFP_KERNEL);
-	if (!trace) {
-		dev_err(dev, "kzalloc trace failed\n");
+	if (!trace)
 		return -ENOMEM;
-	}
 
 	/* set the trace buffer dma properties */
 	trace->len = rsc->len;
@@ -489,10 +507,8 @@ static int rproc_handle_devmem(struct rproc *rproc, struct fw_rsc_devmem *rsc,
 	}
 
 	mapping = kzalloc(sizeof(*mapping), GFP_KERNEL);
-	if (!mapping) {
-		dev_err(dev, "kzalloc mapping failed\n");
+	if (!mapping)
 		return -ENOMEM;
-	}
 
 	ret = iommu_map(rproc->domain, rsc->da, rsc->pa, rsc->len, rsc->flags);
 	if (ret) {
@@ -565,10 +581,8 @@ static int rproc_handle_carveout(struct rproc *rproc,
 			rsc->da, rsc->pa, rsc->len, rsc->flags);
 
 	carveout = kzalloc(sizeof(*carveout), GFP_KERNEL);
-	if (!carveout) {
-		dev_err(dev, "kzalloc carveout failed\n");
+	if (!carveout)
 		return -ENOMEM;
-	}
 
 	va = dma_alloc_coherent(dev->parent, rsc->len, &dma, GFP_KERNEL);
 	if (!va) {
@@ -768,7 +782,8 @@ static void rproc_resource_cleanup(struct rproc *rproc)
 
 	/* clean up carveout allocations */
 	list_for_each_entry_safe(entry, tmp, &rproc->carveouts, node) {
-		dma_free_coherent(dev->parent, entry->len, entry->va, entry->dma);
+		dma_free_coherent(dev->parent, entry->len, entry->va,
+				  entry->dma);
 		list_del(&entry->node);
 		kfree(entry);
 	}
@@ -808,9 +823,8 @@ static int rproc_fw_boot(struct rproc *rproc, const struct firmware *fw)
 
 	/* look for the resource table */
 	table = rproc_find_rsc_table(rproc, fw, &tablesz);
-	if (!table) {
+	if (!table)
 		goto clean_up;
-	}
 
 	/* Verify that resource table in loaded fw is unchanged */
 	if (rproc->table_csum != crc32(0, table, tablesz)) {
@@ -911,7 +925,8 @@ static void rproc_fw_config_virtio(const struct firmware *fw, void *context)
 
 	/* count the number of notify-ids */
 	rproc->max_notifyid = -1;
-	ret = rproc_handle_resources(rproc, tablesz, rproc_count_vrings_handler);
+	ret = rproc_handle_resources(rproc, tablesz,
+				     rproc_count_vrings_handler);
 	if (ret)
 		goto out;
 
@@ -1152,6 +1167,50 @@ out:
 EXPORT_SYMBOL(rproc_shutdown);
 
 /**
+ * rproc_get_by_phandle() - find a remote processor by phandle
+ * @phandle: phandle to the rproc
+ *
+ * Finds an rproc handle using the remote processor's phandle, and then
+ * return a handle to the rproc.
+ *
+ * This function increments the remote processor's refcount, so always
+ * use rproc_put() to decrement it back once rproc isn't needed anymore.
+ *
+ * Returns the rproc handle on success, and NULL on failure.
+ */
+#ifdef CONFIG_OF
+struct rproc *rproc_get_by_phandle(phandle phandle)
+{
+	struct rproc *rproc = NULL, *r;
+	struct device_node *np;
+
+	np = of_find_node_by_phandle(phandle);
+	if (!np)
+		return NULL;
+
+	mutex_lock(&rproc_list_mutex);
+	list_for_each_entry(r, &rproc_list, node) {
+		if (r->dev.parent && r->dev.parent->of_node == np) {
+			rproc = r;
+			get_device(&rproc->dev);
+			break;
+		}
+	}
+	mutex_unlock(&rproc_list_mutex);
+
+	of_node_put(np);
+
+	return rproc;
+}
+#else
+struct rproc *rproc_get_by_phandle(phandle phandle)
+{
+	return NULL;
+}
+#endif
+EXPORT_SYMBOL(rproc_get_by_phandle);
+
+/**
  * rproc_add() - register a remote processor
  * @rproc: the remote processor handle to register
  *
@@ -1180,6 +1239,11 @@ int rproc_add(struct rproc *rproc)
 	if (ret < 0)
 		return ret;
 
+	/* expose to rproc_get_by_phandle users */
+	mutex_lock(&rproc_list_mutex);
+	list_add(&rproc->node, &rproc_list);
+	mutex_unlock(&rproc_list_mutex);
+
 	dev_info(dev, "%s is available\n", rproc->name);
 
 	dev_info(dev, "Note: remoteproc is still under development and considered experimental.\n");
@@ -1268,10 +1332,8 @@ struct rproc *rproc_alloc(struct device *dev, const char *name,
 		name_len = strlen(name) + strlen(template) - 2 + 1;
 
 	rproc = kzalloc(sizeof(struct rproc) + len + name_len, GFP_KERNEL);
-	if (!rproc) {
-		dev_err(dev, "%s: kzalloc failed\n", __func__);
+	if (!rproc)
 		return NULL;
-	}
 
 	if (!firmware) {
 		p = (char *)rproc + sizeof(struct rproc) + len;
@@ -1369,6 +1431,11 @@ int rproc_del(struct rproc *rproc)
 	/* Free the copy of the resource table */
 	kfree(rproc->cached_table);
 
+	/* the rproc is downref'ed as soon as it's removed from the klist */
+	mutex_lock(&rproc_list_mutex);
+	list_del(&rproc->node);
+	mutex_unlock(&rproc_list_mutex);
+
 	device_del(&rproc->dev);
 
 	return 0;
diff --git a/drivers/remoteproc/remoteproc_internal.h b/drivers/remoteproc/remoteproc_internal.h
index 70701a50ddfa..8041b95cb058 100644
--- a/drivers/remoteproc/remoteproc_internal.h
+++ b/drivers/remoteproc/remoteproc_internal.h
@@ -35,7 +35,7 @@ struct rproc;
  * @get_boot_addr:	get boot address to entry point specified in firmware
  */
 struct rproc_fw_ops {
-	struct resource_table *(*find_rsc_table) (struct rproc *rproc,
+	struct resource_table *(*find_rsc_table)(struct rproc *rproc,
 						const struct firmware *fw,
 						int *tablesz);
 	struct resource_table *(*find_loaded_rsc_table)(struct rproc *rproc,
diff --git a/drivers/remoteproc/ste_modem_rproc.c b/drivers/remoteproc/ste_modem_rproc.c
index dd193f35a1ff..53dc17bdd54e 100644
--- a/drivers/remoteproc/ste_modem_rproc.c
+++ b/drivers/remoteproc/ste_modem_rproc.c
@@ -67,8 +67,7 @@ static int sproc_load_segments(struct rproc *rproc, const struct firmware *fw)
 static const struct ste_toc_entry *sproc_find_rsc_entry(const void *data)
 {
 	int i;
-	const struct ste_toc *toc;
-	toc = data;
+	const struct ste_toc *toc = data;
 
 	/* Search the table for the resource table */
 	for (i = 0; i < SPROC_MAX_TOC_ENTRIES &&
@@ -230,6 +229,7 @@ static int sproc_start(struct rproc *rproc)
 static int sproc_stop(struct rproc *rproc)
 {
 	struct sproc *sproc = rproc->priv;
+
 	sproc_dbg(sproc, "stop ste-modem\n");
 
 	return sproc->mdev->ops.power(sproc->mdev, false);
diff --git a/drivers/remoteproc/wkup_m3_rproc.c b/drivers/remoteproc/wkup_m3_rproc.c
new file mode 100644
index 000000000000..edf81819cce1
--- /dev/null
+++ b/drivers/remoteproc/wkup_m3_rproc.c
@@ -0,0 +1,257 @@
+/*
+ * TI AMx3 Wakeup M3 Remote Processor driver
+ *
+ * Copyright (C) 2014-2015 Texas Instruments, Inc.
+ *
+ * Dave Gerlach <d-gerlach@ti.com>
+ * Suman Anna <s-anna@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/remoteproc.h>
+
+#include <linux/platform_data/wkup_m3.h>
+
+#include "remoteproc_internal.h"
+
+#define WKUPM3_MEM_MAX	2
+
+/**
+ * struct wkup_m3_mem - WkupM3 internal memory structure
+ * @cpu_addr: MPU virtual address of the memory region
+ * @bus_addr: Bus address used to access the memory region
+ * @dev_addr: Device address from Wakeup M3 view
+ * @size: Size of the memory region
+ */
+struct wkup_m3_mem {
+	void __iomem *cpu_addr;
+	phys_addr_t bus_addr;
+	u32 dev_addr;
+	size_t size;
+};
+
+/**
+ * struct wkup_m3_rproc - WkupM3 remote processor state
+ * @rproc: rproc handle
+ * @pdev: pointer to platform device
+ * @mem: WkupM3 memory information
+ */
+struct wkup_m3_rproc {
+	struct rproc *rproc;
+	struct platform_device *pdev;
+	struct wkup_m3_mem mem[WKUPM3_MEM_MAX];
+};
+
+static int wkup_m3_rproc_start(struct rproc *rproc)
+{
+	struct wkup_m3_rproc *wkupm3 = rproc->priv;
+	struct platform_device *pdev = wkupm3->pdev;
+	struct device *dev = &pdev->dev;
+	struct wkup_m3_platform_data *pdata = dev_get_platdata(dev);
+
+	if (pdata->deassert_reset(pdev, pdata->reset_name)) {
+		dev_err(dev, "Unable to reset wkup_m3!\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int wkup_m3_rproc_stop(struct rproc *rproc)
+{
+	struct wkup_m3_rproc *wkupm3 = rproc->priv;
+	struct platform_device *pdev = wkupm3->pdev;
+	struct device *dev = &pdev->dev;
+	struct wkup_m3_platform_data *pdata = dev_get_platdata(dev);
+
+	if (pdata->assert_reset(pdev, pdata->reset_name)) {
+		dev_err(dev, "Unable to assert reset of wkup_m3!\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void *wkup_m3_rproc_da_to_va(struct rproc *rproc, u64 da, int len)
+{
+	struct wkup_m3_rproc *wkupm3 = rproc->priv;
+	void *va = NULL;
+	int i;
+	u32 offset;
+
+	if (len <= 0)
+		return NULL;
+
+	for (i = 0; i < WKUPM3_MEM_MAX; i++) {
+		if (da >= wkupm3->mem[i].dev_addr && da + len <=
+		    wkupm3->mem[i].dev_addr +  wkupm3->mem[i].size) {
+			offset = da -  wkupm3->mem[i].dev_addr;
+			/* __force to make sparse happy with type conversion */
+			va = (__force void *)(wkupm3->mem[i].cpu_addr + offset);
+			break;
+		}
+	}
+
+	return va;
+}
+
+static struct rproc_ops wkup_m3_rproc_ops = {
+	.start		= wkup_m3_rproc_start,
+	.stop		= wkup_m3_rproc_stop,
+	.da_to_va	= wkup_m3_rproc_da_to_va,
+};
+
+static const struct of_device_id wkup_m3_rproc_of_match[] = {
+	{ .compatible = "ti,am3352-wkup-m3", },
+	{ .compatible = "ti,am4372-wkup-m3", },
+	{},
+};
+
+static int wkup_m3_rproc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct wkup_m3_platform_data *pdata = dev->platform_data;
+	/* umem always needs to be processed first */
+	const char *mem_names[WKUPM3_MEM_MAX] = { "umem", "dmem" };
+	struct wkup_m3_rproc *wkupm3;
+	const char *fw_name;
+	struct rproc *rproc;
+	struct resource *res;
+	const __be32 *addrp;
+	u32 l4_offset = 0;
+	u64 size;
+	int ret;
+	int i;
+
+	if (!(pdata && pdata->deassert_reset && pdata->assert_reset &&
+	      pdata->reset_name)) {
+		dev_err(dev, "Platform data missing!\n");
+		return -ENODEV;
+	}
+
+	ret = of_property_read_string(dev->of_node, "ti,pm-firmware",
+				      &fw_name);
+	if (ret) {
+		dev_err(dev, "No firmware filename given\n");
+		return -ENODEV;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "pm_runtime_get_sync() failed\n");
+		goto err;
+	}
+
+	rproc = rproc_alloc(dev, "wkup_m3", &wkup_m3_rproc_ops,
+			    fw_name, sizeof(*wkupm3));
+	if (!rproc) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	wkupm3 = rproc->priv;
+	wkupm3->rproc = rproc;
+	wkupm3->pdev = pdev;
+
+	for (i = 0; i < ARRAY_SIZE(mem_names); i++) {
+		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+						   mem_names[i]);
+		wkupm3->mem[i].cpu_addr = devm_ioremap_resource(dev, res);
+		if (IS_ERR(wkupm3->mem[i].cpu_addr)) {
+			dev_err(&pdev->dev, "devm_ioremap_resource failed for resource %d\n",
+				i);
+			ret = PTR_ERR(wkupm3->mem[i].cpu_addr);
+			goto err;
+		}
+		wkupm3->mem[i].bus_addr = res->start;
+		wkupm3->mem[i].size = resource_size(res);
+		addrp = of_get_address(dev->of_node, i, &size, NULL);
+		/*
+		 * The wkupm3 has umem at address 0 in its view, so the device
+		 * addresses for each memory region is computed as a relative
+		 * offset of the bus address for umem, and therefore needs to be
+		 * processed first.
+		 */
+		if (!strcmp(mem_names[i], "umem"))
+			l4_offset = be32_to_cpu(*addrp);
+		wkupm3->mem[i].dev_addr = be32_to_cpu(*addrp) - l4_offset;
+	}
+
+	dev_set_drvdata(dev, rproc);
+
+	ret = rproc_add(rproc);
+	if (ret) {
+		dev_err(dev, "rproc_add failed\n");
+		goto err_put_rproc;
+	}
+
+	return 0;
+
+err_put_rproc:
+	rproc_put(rproc);
+err:
+	pm_runtime_put_noidle(dev);
+	pm_runtime_disable(dev);
+	return ret;
+}
+
+static int wkup_m3_rproc_remove(struct platform_device *pdev)
+{
+	struct rproc *rproc = platform_get_drvdata(pdev);
+
+	rproc_del(rproc);
+	rproc_put(rproc);
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int wkup_m3_rpm_suspend(struct device *dev)
+{
+	return -EBUSY;
+}
+
+static int wkup_m3_rpm_resume(struct device *dev)
+{
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops wkup_m3_rproc_pm_ops = {
+	SET_RUNTIME_PM_OPS(wkup_m3_rpm_suspend, wkup_m3_rpm_resume, NULL)
+};
+
+static struct platform_driver wkup_m3_rproc_driver = {
+	.probe = wkup_m3_rproc_probe,
+	.remove = wkup_m3_rproc_remove,
+	.driver = {
+		.name = "wkup_m3_rproc",
+		.of_match_table = wkup_m3_rproc_of_match,
+		.pm = &wkup_m3_rproc_pm_ops,
+	},
+};
+
+module_platform_driver(wkup_m3_rproc_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TI Wakeup M3 remote processor control driver");
+MODULE_AUTHOR("Dave Gerlach <d-gerlach@ti.com>");
diff --git a/drivers/reset/reset-berlin.c b/drivers/reset/reset-berlin.c
index f8b48a13cf0b..3c922d37255c 100644
--- a/drivers/reset/reset-berlin.c
+++ b/drivers/reset/reset-berlin.c
@@ -11,10 +11,12 @@
 
 #include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/mfd/syscon.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
+#include <linux/regmap.h>
 #include <linux/reset-controller.h>
 #include <linux/slab.h>
 #include <linux/types.h>
@@ -25,8 +27,7 @@
 	container_of((p), struct berlin_reset_priv, rcdev)
 
 struct berlin_reset_priv {
-	void __iomem			*base;
-	unsigned int			size;
+	struct regmap			*regmap;
 	struct reset_controller_dev	rcdev;
 };
 
@@ -37,7 +38,7 @@ static int berlin_reset_reset(struct reset_controller_dev *rcdev,
 	int offset = id >> 8;
 	int mask = BIT(id & 0x1f);
 
-	writel(mask, priv->base + offset);
+	regmap_write(priv->regmap, offset, mask);
 
 	/* let the reset be effective */
 	udelay(10);
@@ -52,7 +53,6 @@ static struct reset_control_ops berlin_reset_ops = {
 static int berlin_reset_xlate(struct reset_controller_dev *rcdev,
 			      const struct of_phandle_args *reset_spec)
 {
-	struct berlin_reset_priv *priv = to_berlin_reset_priv(rcdev);
 	unsigned offset, bit;
 
 	if (WARN_ON(reset_spec->args_count != rcdev->of_reset_n_cells))
@@ -61,71 +61,53 @@ static int berlin_reset_xlate(struct reset_controller_dev *rcdev,
 	offset = reset_spec->args[0];
 	bit = reset_spec->args[1];
 
-	if (offset >= priv->size)
-		return -EINVAL;
-
 	if (bit >= BERLIN_MAX_RESETS)
 		return -EINVAL;
 
 	return (offset << 8) | bit;
 }
 
-static int __berlin_reset_init(struct device_node *np)
+static int berlin2_reset_probe(struct platform_device *pdev)
 {
+	struct device_node *parent_np = of_get_parent(pdev->dev.of_node);
 	struct berlin_reset_priv *priv;
-	struct resource res;
-	resource_size_t size;
-	int ret;
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
-	ret = of_address_to_resource(np, 0, &res);
-	if (ret)
-		goto err;
-
-	size = resource_size(&res);
-	priv->base = ioremap(res.start, size);
-	if (!priv->base) {
-		ret = -ENOMEM;
-		goto err;
-	}
-	priv->size = size;
+	priv->regmap = syscon_node_to_regmap(parent_np);
+	of_node_put(parent_np);
+	if (IS_ERR(priv->regmap))
+		return PTR_ERR(priv->regmap);
 
 	priv->rcdev.owner = THIS_MODULE;
 	priv->rcdev.ops = &berlin_reset_ops;
-	priv->rcdev.of_node = np;
+	priv->rcdev.of_node = pdev->dev.of_node;
 	priv->rcdev.of_reset_n_cells = 2;
 	priv->rcdev.of_xlate = berlin_reset_xlate;
 
 	reset_controller_register(&priv->rcdev);
 
 	return 0;
-
-err:
-	kfree(priv);
-	return ret;
 }
 
-static const struct of_device_id berlin_reset_of_match[] __initconst = {
-	{ .compatible = "marvell,berlin2-chip-ctrl" },
-	{ .compatible = "marvell,berlin2cd-chip-ctrl" },
-	{ .compatible = "marvell,berlin2q-chip-ctrl" },
+static const struct of_device_id berlin_reset_dt_match[] = {
+	{ .compatible = "marvell,berlin2-reset" },
 	{ },
 };
+MODULE_DEVICE_TABLE(of, berlin_reset_dt_match);
+
+static struct platform_driver berlin_reset_driver = {
+	.probe	= berlin2_reset_probe,
+	.driver	= {
+		.name = "berlin2-reset",
+		.of_match_table = berlin_reset_dt_match,
+	},
+};
+module_platform_driver(berlin_reset_driver);
 
-static int __init berlin_reset_init(void)
-{
-	struct device_node *np;
-	int ret;
-
-	for_each_matching_node(np, berlin_reset_of_match) {
-		ret = __berlin_reset_init(np);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-arch_initcall(berlin_reset_init);
+MODULE_AUTHOR("Antoine Tenart <antoine.tenart@free-electrons.com>");
+MODULE_AUTHOR("Sebastian Hesselbarth <sebastian.hesselbarth@gmail.com>");
+MODULE_DESCRIPTION("Marvell Berlin reset driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 0fe4ad8826b2..83b4b89b9d5a 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -12,7 +12,7 @@ menuconfig RTC_CLASS
 	select RTC_LIB
 	help
 	  Generic RTC class support. If you say yes here, you will
- 	  be allowed to plug one or more RTCs to your system. You will
+	  be allowed to plug one or more RTCs to your system. You will
 	  probably want to enable one or more of the interfaces below.
 
 if RTC_CLASS
@@ -25,17 +25,9 @@ config RTC_HCTOSYS
 	  the value read from a specified RTC device. This is useful to avoid
 	  unnecessary fsck runs at boot time, and to network better.
 
-config RTC_SYSTOHC
-	bool "Set the RTC time based on NTP synchronization"
-	default y
-	help
-	  If you say yes here, the system time (wall clock) will be stored
-	  in the RTC specified by RTC_HCTOSYS_DEVICE approximately every 11
-	  minutes if userspace reports synchronized NTP status.
-
 config RTC_HCTOSYS_DEVICE
 	string "RTC used to set the system time"
-	depends on RTC_HCTOSYS = y || RTC_SYSTOHC = y
+	depends on RTC_HCTOSYS
 	default "rtc0"
 	help
 	  The RTC device that will be used to (re)initialize the system
@@ -56,6 +48,25 @@ config RTC_HCTOSYS_DEVICE
 	  sleep states. Do not specify an RTC here unless it stays powered
 	  during all this system's supported sleep states.
 
+config RTC_SYSTOHC
+	bool "Set the RTC time based on NTP synchronization"
+	default y
+	help
+	  If you say yes here, the system time (wall clock) will be stored
+	  in the RTC specified by RTC_HCTOSYS_DEVICE approximately every 11
+	  minutes if userspace reports synchronized NTP status.
+
+config RTC_SYSTOHC_DEVICE
+	string "RTC used to synchronize NTP adjustment"
+	depends on RTC_SYSTOHC
+	default RTC_HCTOSYS_DEVICE if RTC_HCTOSYS
+	default "rtc0"
+	help
+	  The RTC device used for NTP synchronization. The main difference
+	  between RTC_HCTOSYS_DEVICE and RTC_SYSTOHC_DEVICE is that this
+	  one can sleep when setting time, because it runs in the workqueue
+	  context.
+
 config RTC_DEBUG
 	bool "RTC debug support"
 	help
@@ -135,7 +146,7 @@ if I2C
 
 config RTC_DRV_88PM860X
 	tristate "Marvell 88PM860x"
-	depends on I2C && MFD_88PM860X
+	depends on MFD_88PM860X
 	help
 	  If you say yes here you get support for RTC function in Marvell
 	  88PM860x chips.
@@ -145,7 +156,7 @@ config RTC_DRV_88PM860X
 
 config RTC_DRV_88PM80X
 	tristate "Marvell 88PM80x"
-	depends on I2C && MFD_88PM800
+	depends on MFD_88PM800
 	help
 	  If you say yes here you get support for RTC function in Marvell
 	  88PM80x chips.
@@ -154,10 +165,9 @@ config RTC_DRV_88PM80X
 	  will be called rtc-88pm80x.
 
 config RTC_DRV_ABB5ZES3
-       depends on I2C
-       select REGMAP_I2C
-       tristate "Abracon AB-RTCMC-32.768kHz-B5ZE-S3"
-       help
+	select REGMAP_I2C
+	tristate "Abracon AB-RTCMC-32.768kHz-B5ZE-S3"
+	help
 	  If you say yes here you get support for the Abracon
 	  AB-RTCMC-32.768kHz-B5ZE-S3 I2C RTC chip.
 
@@ -204,7 +214,6 @@ config RTC_DRV_DS1307
 
 config RTC_DRV_DS1374
 	tristate "Dallas/Maxim DS1374"
-	depends on I2C
 	help
 	  If you say yes here you get support for Dallas Semiconductor
 	  DS1374 real-time clock chips. If an interrupt is associated
@@ -232,7 +241,6 @@ config RTC_DRV_DS1672
 
 config RTC_DRV_DS3232
 	tristate "Dallas/Maxim DS3232"
-	depends on I2C
 	help
 	  If you say yes here you get support for Dallas Semiconductor
 	  DS3232 real-time clock chips. If an interrupt is associated
@@ -243,7 +251,7 @@ config RTC_DRV_DS3232
 
 config RTC_DRV_HYM8563
 	tristate "Haoyu Microelectronics HYM8563"
-	depends on I2C && OF
+	depends on OF
 	help
 	  Say Y to enable support for the HYM8563 I2C RTC chip. Apart
 	  from the usual rtc functions it provides a clock output of
@@ -365,10 +373,9 @@ config RTC_DRV_ISL12022
 	  will be called rtc-isl12022.
 
 config RTC_DRV_ISL12057
-       depends on I2C
-       select REGMAP_I2C
-       tristate "Intersil ISL12057"
-       help
+	select REGMAP_I2C
+	tristate "Intersil ISL12057"
+	help
 	  If you say yes here you get support for the Intersil ISL12057
 	  I2C RTC chip.
 
@@ -603,13 +610,13 @@ comment "SPI RTC drivers"
 if SPI_MASTER
 
 config RTC_DRV_M41T93
-        tristate "ST M41T93"
-        help
-          If you say yes here you will get support for the
-          ST M41T93 SPI RTC chip.
+	tristate "ST M41T93"
+	help
+	  If you say yes here you will get support for the
+	  ST M41T93 SPI RTC chip.
 
-          This driver can also be built as a module. If so, the module
-          will be called rtc-m41t93.
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-m41t93.
 
 config RTC_DRV_M41T94
 	tristate "ST M41T94"
@@ -1200,7 +1207,7 @@ config RTC_DRV_SH
 	  Say Y here to enable support for the on-chip RTC found in
 	  most SuperH processors.
 
- 	  To compile this driver as a module, choose M here: the
+	  To compile this driver as a module, choose M here: the
 	  module will be called rtc-sh.
 
 config RTC_DRV_VR41XX
@@ -1299,14 +1306,14 @@ config RTC_DRV_GENERIC
 	  just say Y.
 
 config RTC_DRV_PXA
-       tristate "PXA27x/PXA3xx"
-       depends on ARCH_PXA
-       help
-         If you say Y here you will get access to the real time clock
-         built into your PXA27x or PXA3xx CPU.
+	tristate "PXA27x/PXA3xx"
+	depends on ARCH_PXA
+	help
+	 If you say Y here you will get access to the real time clock
+	 built into your PXA27x or PXA3xx CPU.
 
-         This RTC driver uses PXA RTC registers available since pxa27x
-         series (RDxR, RYxR) instead of legacy RCNR, RTAR.
+	 This RTC driver uses PXA RTC registers available since pxa27x
+	 series (RDxR, RYxR) instead of legacy RCNR, RTAR.
 
 config RTC_DRV_VT8500
 	tristate "VIA/WonderMedia 85xx SoC RTC"
@@ -1372,6 +1379,17 @@ config RTC_DRV_ARMADA38X
 	  This driver can also be built as a module. If so, the module
 	  will be called armada38x-rtc.
 
+config RTC_DRV_GEMINI
+	tristate "Gemini SoC RTC"
+	depends on ARCH_GEMINI || COMPILE_TEST
+	depends on HAS_IOMEM
+	help
+	  If you say Y here you will get support for the
+	  RTC found on Gemini SoC's.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-gemini.
+
 config RTC_DRV_PS3
 	tristate "PS3 RTC"
 	depends on PPC_PS3
@@ -1396,6 +1414,7 @@ config RTC_DRV_COH901331
 config RTC_DRV_STMP
 	tristate "Freescale STMP3xxx/i.MX23/i.MX28 RTC"
 	depends on ARCH_MXS
+	select STMP_DEVICE
 	help
 	  If you say yes here you will get support for the onboard
 	  STMP3xxx/i.MX23/i.MX28 RTC.
@@ -1484,7 +1503,7 @@ config RTC_DRV_PUV3
 
 config RTC_DRV_LOONGSON1
 	tristate "loongson1 RTC support"
-	depends on MACH_LOONGSON1
+	depends on MACH_LOONGSON32
 	help
 	  This is a driver for the loongson1 on-chip Counter0 (Time-Of-Year
 	  counter) to be used as a RTC.
@@ -1520,6 +1539,17 @@ config RTC_DRV_SIRFSOC
 	  Say "yes" here to support the real time clock on SiRF SOC chips.
 	  This driver can also be built as a module called rtc-sirfsoc.
 
+config RTC_DRV_ST_LPC
+	tristate "STMicroelectronics LPC RTC"
+	depends on ARCH_STI
+	depends on OF
+	help
+	  Say Y here to include STMicroelectronics Low Power Controller
+	  (LPC) based RTC support.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called rtc-st-lpc.
+
 config RTC_DRV_MOXART
 	tristate "MOXA ART RTC"
 	depends on ARCH_MOXART || COMPILE_TEST
@@ -1530,9 +1560,20 @@ config RTC_DRV_MOXART
 	   This driver can also be built as a module. If so, the module
 	   will be called rtc-moxart
 
+config RTC_DRV_MT6397
+	tristate "Mediatek Real Time Clock driver"
+	depends on MFD_MT6397 || COMPILE_TEST
+	help
+	  This selects the Mediatek(R) RTC driver. RTC is part of Mediatek
+	  MT6397 PMIC. You should enable MT6397 PMIC MFD before select
+	  Mediatek(R) RTC driver.
+
+	  If you want to use Mediatek(R) RTC interface, select Y or M here.
+
 config RTC_DRV_XGENE
 	tristate "APM X-Gene RTC"
 	depends on HAS_IOMEM
+	depends on ARCH_XGENE || COMPILE_TEST
 	help
 	  If you say yes here you get support for the APM X-Gene SoC real time
 	  clock.
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 2b82e2b0311b..1b09a62fcf4b 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -14,14 +14,14 @@ ifdef CONFIG_RTC_DRV_EFI
 rtc-core-y			+= rtc-efi-platform.o
 endif
 
-rtc-core-$(CONFIG_RTC_INTF_DEV)	+= rtc-dev.o
-rtc-core-$(CONFIG_RTC_INTF_PROC) += rtc-proc.o
-rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
+rtc-core-$(CONFIG_RTC_INTF_DEV)		+= rtc-dev.o
+rtc-core-$(CONFIG_RTC_INTF_PROC)	+= rtc-proc.o
+rtc-core-$(CONFIG_RTC_INTF_SYSFS)	+= rtc-sysfs.o
 
 # Keep the list ordered.
 
-obj-$(CONFIG_RTC_DRV_88PM860X)  += rtc-88pm860x.o
 obj-$(CONFIG_RTC_DRV_88PM80X)	+= rtc-88pm80x.o
+obj-$(CONFIG_RTC_DRV_88PM860X)	+= rtc-88pm860x.o
 obj-$(CONFIG_RTC_DRV_AB3100)	+= rtc-ab3100.o
 obj-$(CONFIG_RTC_DRV_AB8500)	+= rtc-ab8500.o
 obj-$(CONFIG_RTC_DRV_ABB5ZES3)	+= rtc-ab-b5ze-s3.o
@@ -43,7 +43,6 @@ obj-$(CONFIG_RTC_DRV_DA9063)	+= rtc-da9063.o
 obj-$(CONFIG_RTC_DRV_DAVINCI)	+= rtc-davinci.o
 obj-$(CONFIG_RTC_DRV_DIGICOLOR)	+= rtc-digicolor.o
 obj-$(CONFIG_RTC_DRV_DM355EVM)	+= rtc-dm355evm.o
-obj-$(CONFIG_RTC_DRV_VRTC)	+= rtc-mrst.o
 obj-$(CONFIG_RTC_DRV_DS1216)	+= rtc-ds1216.o
 obj-$(CONFIG_RTC_DRV_DS1286)	+= rtc-ds1286.o
 obj-$(CONFIG_RTC_DRV_DS1302)	+= rtc-ds1302.o
@@ -58,20 +57,21 @@ obj-$(CONFIG_RTC_DRV_DS1553)	+= rtc-ds1553.o
 obj-$(CONFIG_RTC_DRV_DS1672)	+= rtc-ds1672.o
 obj-$(CONFIG_RTC_DRV_DS1685_FAMILY)	+= rtc-ds1685.o
 obj-$(CONFIG_RTC_DRV_DS1742)	+= rtc-ds1742.o
-obj-$(CONFIG_RTC_DRV_DS2404)    += rtc-ds2404.o
+obj-$(CONFIG_RTC_DRV_DS2404)	+= rtc-ds2404.o
 obj-$(CONFIG_RTC_DRV_DS3232)	+= rtc-ds3232.o
 obj-$(CONFIG_RTC_DRV_DS3234)	+= rtc-ds3234.o
 obj-$(CONFIG_RTC_DRV_EFI)	+= rtc-efi.o
 obj-$(CONFIG_RTC_DRV_EM3027)	+= rtc-em3027.o
 obj-$(CONFIG_RTC_DRV_EP93XX)	+= rtc-ep93xx.o
 obj-$(CONFIG_RTC_DRV_FM3130)	+= rtc-fm3130.o
+obj-$(CONFIG_RTC_DRV_GEMINI)	+= rtc-gemini.o
 obj-$(CONFIG_RTC_DRV_GENERIC)	+= rtc-generic.o
 obj-$(CONFIG_RTC_DRV_HID_SENSOR_TIME) += rtc-hid-sensor-time.o
 obj-$(CONFIG_RTC_DRV_HYM8563)	+= rtc-hym8563.o
 obj-$(CONFIG_RTC_DRV_IMXDI)	+= rtc-imxdi.o
-obj-$(CONFIG_RTC_DRV_ISL1208)	+= rtc-isl1208.o
 obj-$(CONFIG_RTC_DRV_ISL12022)	+= rtc-isl12022.o
 obj-$(CONFIG_RTC_DRV_ISL12057)	+= rtc-isl12057.o
+obj-$(CONFIG_RTC_DRV_ISL1208)	+= rtc-isl1208.o
 obj-$(CONFIG_RTC_DRV_JZ4740)	+= rtc-jz4740.o
 obj-$(CONFIG_RTC_DRV_LP8788)	+= rtc-lp8788.o
 obj-$(CONFIG_RTC_DRV_LPC32XX)	+= rtc-lpc32xx.o
@@ -82,32 +82,35 @@ obj-$(CONFIG_RTC_DRV_M41T94)	+= rtc-m41t94.o
 obj-$(CONFIG_RTC_DRV_M48T35)	+= rtc-m48t35.o
 obj-$(CONFIG_RTC_DRV_M48T59)	+= rtc-m48t59.o
 obj-$(CONFIG_RTC_DRV_M48T86)	+= rtc-m48t86.o
-obj-$(CONFIG_RTC_DRV_MXC)	+= rtc-mxc.o
 obj-$(CONFIG_RTC_DRV_MAX6900)	+= rtc-max6900.o
+obj-$(CONFIG_RTC_DRV_MAX6902)	+= rtc-max6902.o
+obj-$(CONFIG_RTC_DRV_MAX77686)	+= rtc-max77686.o
+obj-$(CONFIG_RTC_DRV_MAX77802)	+= rtc-max77802.o
 obj-$(CONFIG_RTC_DRV_MAX8907)	+= rtc-max8907.o
 obj-$(CONFIG_RTC_DRV_MAX8925)	+= rtc-max8925.o
-obj-$(CONFIG_RTC_DRV_MAX8998)	+= rtc-max8998.o
 obj-$(CONFIG_RTC_DRV_MAX8997)	+= rtc-max8997.o
-obj-$(CONFIG_RTC_DRV_MAX6902)	+= rtc-max6902.o
-obj-$(CONFIG_RTC_DRV_MAX77686)	+= rtc-max77686.o
-obj-$(CONFIG_RTC_DRV_MAX77802)  += rtc-max77802.o
+obj-$(CONFIG_RTC_DRV_MAX8998)	+= rtc-max8998.o
 obj-$(CONFIG_RTC_DRV_MC13XXX)	+= rtc-mc13xxx.o
 obj-$(CONFIG_RTC_DRV_MCP795)	+= rtc-mcp795.o
-obj-$(CONFIG_RTC_DRV_MSM6242)	+= rtc-msm6242.o
+obj-$(CONFIG_RTC_DRV_MOXART)	+= rtc-moxart.o
 obj-$(CONFIG_RTC_DRV_MPC5121)	+= rtc-mpc5121.o
+obj-$(CONFIG_RTC_DRV_VRTC)	+= rtc-mrst.o
+obj-$(CONFIG_RTC_DRV_MSM6242)	+= rtc-msm6242.o
+obj-$(CONFIG_RTC_DRV_MT6397)	+= rtc-mt6397.o
 obj-$(CONFIG_RTC_DRV_MV)	+= rtc-mv.o
+obj-$(CONFIG_RTC_DRV_MXC)	+= rtc-mxc.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_OPAL)	+= rtc-opal.o
 obj-$(CONFIG_RTC_DRV_PALMAS)	+= rtc-palmas.o
 obj-$(CONFIG_RTC_DRV_PCAP)	+= rtc-pcap.o
+obj-$(CONFIG_RTC_DRV_PCF2123)	+= rtc-pcf2123.o
 obj-$(CONFIG_RTC_DRV_PCF2127)	+= rtc-pcf2127.o
+obj-$(CONFIG_RTC_DRV_PCF50633)	+= rtc-pcf50633.o
+obj-$(CONFIG_RTC_DRV_PCF85063)	+= rtc-pcf85063.o
 obj-$(CONFIG_RTC_DRV_PCF8523)	+= rtc-pcf8523.o
 obj-$(CONFIG_RTC_DRV_PCF8563)	+= rtc-pcf8563.o
-obj-$(CONFIG_RTC_DRV_PCF85063)	+= rtc-pcf85063.o
 obj-$(CONFIG_RTC_DRV_PCF8583)	+= rtc-pcf8583.o
-obj-$(CONFIG_RTC_DRV_PCF2123)	+= rtc-pcf2123.o
-obj-$(CONFIG_RTC_DRV_PCF50633)	+= rtc-pcf50633.o
 obj-$(CONFIG_RTC_DRV_PL030)	+= rtc-pl030.o
 obj-$(CONFIG_RTC_DRV_PL031)	+= rtc-pl031.o
 obj-$(CONFIG_RTC_DRV_PM8XXX)	+= rtc-pm8xxx.o
@@ -130,21 +133,23 @@ obj-$(CONFIG_RTC_DRV_S3C)	+= rtc-s3c.o
 obj-$(CONFIG_RTC_DRV_S5M)	+= rtc-s5m.o
 obj-$(CONFIG_RTC_DRV_SA1100)	+= rtc-sa1100.o
 obj-$(CONFIG_RTC_DRV_SH)	+= rtc-sh.o
+obj-$(CONFIG_RTC_DRV_SIRFSOC)	+= rtc-sirfsoc.o
 obj-$(CONFIG_RTC_DRV_SNVS)	+= rtc-snvs.o
 obj-$(CONFIG_RTC_DRV_SPEAR)	+= rtc-spear.o
 obj-$(CONFIG_RTC_DRV_STARFIRE)	+= rtc-starfire.o
 obj-$(CONFIG_RTC_DRV_STK17TA8)	+= rtc-stk17ta8.o
 obj-$(CONFIG_RTC_DRV_STMP)	+= rtc-stmp3xxx.o
+obj-$(CONFIG_RTC_DRV_ST_LPC)	+= rtc-st-lpc.o
 obj-$(CONFIG_RTC_DRV_SUN4V)	+= rtc-sun4v.o
 obj-$(CONFIG_RTC_DRV_SUN6I)	+= rtc-sun6i.o
 obj-$(CONFIG_RTC_DRV_SUNXI)	+= rtc-sunxi.o
 obj-$(CONFIG_RTC_DRV_TEGRA)	+= rtc-tegra.o
 obj-$(CONFIG_RTC_DRV_TEST)	+= rtc-test.o
 obj-$(CONFIG_RTC_DRV_TILE)	+= rtc-tile.o
-obj-$(CONFIG_RTC_DRV_TWL4030)	+= rtc-twl.o
 obj-$(CONFIG_RTC_DRV_TPS6586X)	+= rtc-tps6586x.o
 obj-$(CONFIG_RTC_DRV_TPS65910)	+= rtc-tps65910.o
 obj-$(CONFIG_RTC_DRV_TPS80031)	+= rtc-tps80031.o
+obj-$(CONFIG_RTC_DRV_TWL4030)	+= rtc-twl.o
 obj-$(CONFIG_RTC_DRV_TX4939)	+= rtc-tx4939.o
 obj-$(CONFIG_RTC_DRV_V3020)	+= rtc-v3020.o
 obj-$(CONFIG_RTC_DRV_VR41XX)	+= rtc-vr41xx.o
@@ -153,5 +158,3 @@ obj-$(CONFIG_RTC_DRV_WM831X)	+= rtc-wm831x.o
 obj-$(CONFIG_RTC_DRV_WM8350)	+= rtc-wm8350.o
 obj-$(CONFIG_RTC_DRV_X1205)	+= rtc-x1205.o
 obj-$(CONFIG_RTC_DRV_XGENE)	+= rtc-xgene.o
-obj-$(CONFIG_RTC_DRV_SIRFSOC)	+= rtc-sirfsoc.o
-obj-$(CONFIG_RTC_DRV_MOXART)	+= rtc-moxart.o
diff --git a/drivers/rtc/interface.c b/drivers/rtc/interface.c
index 166fc60d8b55..11b639067312 100644
--- a/drivers/rtc/interface.c
+++ b/drivers/rtc/interface.c
@@ -91,51 +91,6 @@ int rtc_set_time(struct rtc_device *rtc, struct rtc_time *tm)
 }
 EXPORT_SYMBOL_GPL(rtc_set_time);
 
-int rtc_set_mmss(struct rtc_device *rtc, unsigned long secs)
-{
-	int err;
-
-	err = mutex_lock_interruptible(&rtc->ops_lock);
-	if (err)
-		return err;
-
-	if (!rtc->ops)
-		err = -ENODEV;
-	else if (rtc->ops->set_mmss64)
-		err = rtc->ops->set_mmss64(rtc->dev.parent, secs);
-	else if (rtc->ops->set_mmss)
-		err = rtc->ops->set_mmss(rtc->dev.parent, secs);
-	else if (rtc->ops->read_time && rtc->ops->set_time) {
-		struct rtc_time new, old;
-
-		err = rtc->ops->read_time(rtc->dev.parent, &old);
-		if (err == 0) {
-			rtc_time64_to_tm(secs, &new);
-
-			/*
-			 * avoid writing when we're going to change the day of
-			 * the month. We will retry in the next minute. This
-			 * basically means that if the RTC must not drift
-			 * by more than 1 minute in 11 minutes.
-			 */
-			if (!((old.tm_hour == 23 && old.tm_min == 59) ||
-				(new.tm_hour == 23 && new.tm_min == 59)))
-				err = rtc->ops->set_time(rtc->dev.parent,
-						&new);
-		}
-	} else {
-		err = -EINVAL;
-	}
-
-	pm_stay_awake(rtc->dev.parent);
-	mutex_unlock(&rtc->ops_lock);
-	/* A timer might have just expired */
-	schedule_work(&rtc->irqwork);
-
-	return err;
-}
-EXPORT_SYMBOL_GPL(rtc_set_mmss);
-
 static int rtc_read_alarm_internal(struct rtc_device *rtc, struct rtc_wkalrm *alarm)
 {
 	int err;
@@ -976,14 +931,12 @@ int rtc_timer_start(struct rtc_device *rtc, struct rtc_timer *timer,
  *
  * Kernel interface to cancel an rtc_timer
  */
-int rtc_timer_cancel(struct rtc_device *rtc, struct rtc_timer *timer)
+void rtc_timer_cancel(struct rtc_device *rtc, struct rtc_timer *timer)
 {
-	int ret = 0;
 	mutex_lock(&rtc->ops_lock);
 	if (timer->enabled)
 		rtc_timer_remove(rtc, timer);
 	mutex_unlock(&rtc->ops_lock);
-	return ret;
 }
 
 
diff --git a/drivers/rtc/rtc-ab8500.c b/drivers/rtc/rtc-ab8500.c
index 6856f0a3a3d5..133d2e2e1a25 100644
--- a/drivers/rtc/rtc-ab8500.c
+++ b/drivers/rtc/rtc-ab8500.c
@@ -442,7 +442,7 @@ static const struct rtc_class_ops ab8540_rtc_ops = {
 	.alarm_irq_enable	= ab8500_rtc_irq_enable,
 };
 
-static struct platform_device_id ab85xx_rtc_ids[] = {
+static const struct platform_device_id ab85xx_rtc_ids[] = {
 	{ "ab8500-rtc", (kernel_ulong_t)&ab8500_rtc_ops, },
 	{ "ab8540-rtc", (kernel_ulong_t)&ab8540_rtc_ops, },
 };
diff --git a/drivers/rtc/rtc-at32ap700x.c b/drivers/rtc/rtc-at32ap700x.c
index d618d6c7ef93..83ac2337c0f7 100644
--- a/drivers/rtc/rtc-at32ap700x.c
+++ b/drivers/rtc/rtc-at32ap700x.c
@@ -282,6 +282,6 @@ static struct platform_driver at32_rtc_driver = {
 
 module_platform_driver_probe(at32_rtc_driver, at32_rtc_probe);
 
-MODULE_AUTHOR("Hans-Christian Egtvedt <hcegtvedt@atmel.com>");
+MODULE_AUTHOR("Hans-Christian Egtvedt <egtvedt@samfundet.no>");
 MODULE_DESCRIPTION("Real time clock for AVR32 AT32AP700x");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-ds1216.c b/drivers/rtc/rtc-ds1216.c
index d16f550897b8..12dbd70859ae 100644
--- a/drivers/rtc/rtc-ds1216.c
+++ b/drivers/rtc/rtc-ds1216.c
@@ -144,15 +144,13 @@ static int __init ds1216_rtc_probe(struct platform_device *pdev)
 	struct ds1216_priv *priv;
 	u8 dummy[8];
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENODEV;
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
 	platform_set_drvdata(pdev, priv);
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	priv->ioaddr = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(priv->ioaddr))
 		return PTR_ERR(priv->ioaddr);
diff --git a/drivers/rtc/rtc-ds1286.c b/drivers/rtc/rtc-ds1286.c
index 2fe537f4e2bd..8247a29a4eb4 100644
--- a/drivers/rtc/rtc-ds1286.c
+++ b/drivers/rtc/rtc-ds1286.c
@@ -332,13 +332,11 @@ static int ds1286_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct ds1286_priv *priv;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -ENODEV;
 	priv = devm_kzalloc(&pdev->dev, sizeof(struct ds1286_priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	priv->rtcregs = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(priv->rtcregs))
 		return PTR_ERR(priv->rtcregs);
diff --git a/drivers/rtc/rtc-ds1307.c b/drivers/rtc/rtc-ds1307.c
index 4ffabb322a9a..6e76de1856fc 100644
--- a/drivers/rtc/rtc-ds1307.c
+++ b/drivers/rtc/rtc-ds1307.c
@@ -742,17 +742,17 @@ static int mcp794xx_set_alarm(struct device *dev, struct rtc_wkalrm *t)
 	regs[6] &= ~MCP794XX_BIT_ALMX_IF;
 	/* Set alarm match: second, minute, hour, day, date, month. */
 	regs[6] |= MCP794XX_MSK_ALMX_MATCH;
-
-	if (t->enabled)
-		regs[0] |= MCP794XX_BIT_ALM0_EN;
-	else
-		regs[0] &= ~MCP794XX_BIT_ALM0_EN;
+	/* Disable interrupt. We will not enable until completely programmed */
+	regs[0] &= ~MCP794XX_BIT_ALM0_EN;
 
 	ret = ds1307->write_block_data(client, MCP794XX_REG_CONTROL, 10, regs);
 	if (ret < 0)
 		return ret;
 
-	return 0;
+	if (!t->enabled)
+		return 0;
+	regs[0] |= MCP794XX_BIT_ALM0_EN;
+	return i2c_smbus_write_byte_data(client, MCP794XX_REG_CONTROL, regs[0]);
 }
 
 static int mcp794xx_alarm_irq_enable(struct device *dev, unsigned int enabled)
diff --git a/drivers/rtc/rtc-ds1672.c b/drivers/rtc/rtc-ds1672.c
index a4888dbca2e1..92b1cbf2c4a7 100644
--- a/drivers/rtc/rtc-ds1672.c
+++ b/drivers/rtc/rtc-ds1672.c
@@ -198,6 +198,7 @@ static struct i2c_device_id ds1672_id[] = {
 	{ "ds1672", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, ds1672_id);
 
 static struct i2c_driver ds1672_driver = {
 	.driver = {
diff --git a/drivers/rtc/rtc-efi.c b/drivers/rtc/rtc-efi.c
index cb989cd00b14..3806961b4348 100644
--- a/drivers/rtc/rtc-efi.c
+++ b/drivers/rtc/rtc-efi.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2009 Hewlett-Packard Development Company, L.P.
  *
- * Author: dann frazier <dannf@hp.com>
+ * Author: dann frazier <dannf@dannf.org>
  * Based on efirtc.c by Stephane Eranian
  *
  *  This program is free software; you can redistribute  it and/or modify it
@@ -24,10 +24,6 @@
 #include <linux/efi.h>
 
 #define EFI_ISDST (EFI_TIME_ADJUST_DAYLIGHT|EFI_TIME_IN_DAYLIGHT)
-/*
- * EFI Epoch is 1/1/1998
- */
-#define EFI_RTC_EPOCH		1998
 
 /*
  * returns day of the year [0-365]
@@ -38,31 +34,24 @@ compute_yday(efi_time_t *eft)
 	/* efi_time_t.month is in the [1-12] so, we need -1 */
 	return rtc_year_days(eft->day, eft->month - 1, eft->year);
 }
+
 /*
  * returns day of the week [0-6] 0=Sunday
- *
- * Don't try to provide a year that's before 1998, please !
  */
 static int
-compute_wday(efi_time_t *eft)
+compute_wday(efi_time_t *eft, int yday)
 {
-	int y;
-	int ndays = 0;
-
-	if (eft->year < EFI_RTC_EPOCH) {
-		pr_err("EFI year < " __stringify(EFI_RTC_EPOCH) ", invalid date\n");
-		return -1;
-	}
-
-	for (y = EFI_RTC_EPOCH; y < eft->year; y++)
-		ndays += 365 + (is_leap_year(y) ? 1 : 0);
-
-	ndays += compute_yday(eft);
+	int ndays = eft->year * (365 % 7)
+		    + (eft->year - 1) / 4
+		    - (eft->year - 1) / 100
+		    + (eft->year - 1) / 400
+		    + yday;
 
 	/*
-	 * 4=1/1/1998 was a Thursday
+	 * 1/1/0000 may or may not have been a Sunday (if it ever existed at
+	 * all) but assuming it was makes this calculation work correctly.
 	 */
-	return (ndays + 4) % 7;
+	return ndays % 7;
 }
 
 static void
@@ -103,16 +92,16 @@ convert_from_efi_time(efi_time_t *eft, struct rtc_time *wtime)
 	if (!eft->month || eft->month > 12)
 		return false;
 	wtime->tm_mon  = eft->month - 1;
-	wtime->tm_year = eft->year - 1900;
 
-	/* day of the week [0-6], Sunday=0 */
-	wtime->tm_wday = compute_wday(eft);
-	if (wtime->tm_wday < 0)
+	if (eft->year < 1900 || eft->year > 9999)
 		return false;
+	wtime->tm_year = eft->year - 1900;
 
 	/* day in the year [1-365]*/
 	wtime->tm_yday = compute_yday(eft);
 
+	/* day of the week [0-6], Sunday=0 */
+	wtime->tm_wday = compute_wday(eft, wtime->tm_yday);
 
 	switch (eft->daylight & EFI_ISDST) {
 	case EFI_ISDST:
@@ -233,7 +222,7 @@ static struct platform_driver efi_rtc_driver = {
 module_platform_driver_probe(efi_rtc_driver, efi_rtc_probe);
 
 MODULE_ALIAS("platform:rtc-efi");
-MODULE_AUTHOR("dann frazier <dannf@hp.com>");
+MODULE_AUTHOR("dann frazier <dannf@dannf.org>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("EFI RTC driver");
 MODULE_ALIAS("platform:rtc-efi");
diff --git a/drivers/rtc/rtc-ep93xx.c b/drivers/rtc/rtc-ep93xx.c
index de325d68c7e4..a1628adf9f52 100644
--- a/drivers/rtc/rtc-ep93xx.c
+++ b/drivers/rtc/rtc-ep93xx.c
@@ -45,7 +45,7 @@ static int ep93xx_rtc_get_swcomp(struct device *dev, unsigned short *preload,
 	struct ep93xx_rtc *ep93xx_rtc = dev_get_platdata(dev);
 	unsigned long comp;
 
-	comp = __raw_readl(ep93xx_rtc->mmio_base + EP93XX_RTC_SWCOMP);
+	comp = readl(ep93xx_rtc->mmio_base + EP93XX_RTC_SWCOMP);
 
 	if (preload)
 		*preload = (comp & EP93XX_RTC_SWCOMP_INT_MASK)
@@ -63,7 +63,7 @@ static int ep93xx_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	struct ep93xx_rtc *ep93xx_rtc = dev_get_platdata(dev);
 	unsigned long time;
 
-	 time = __raw_readl(ep93xx_rtc->mmio_base + EP93XX_RTC_DATA);
+	 time = readl(ep93xx_rtc->mmio_base + EP93XX_RTC_DATA);
 
 	rtc_time_to_tm(time, tm);
 	return 0;
@@ -73,7 +73,7 @@ static int ep93xx_rtc_set_mmss(struct device *dev, unsigned long secs)
 {
 	struct ep93xx_rtc *ep93xx_rtc = dev_get_platdata(dev);
 
-	__raw_writel(secs + 1, ep93xx_rtc->mmio_base + EP93XX_RTC_LOAD);
+	writel(secs + 1, ep93xx_rtc->mmio_base + EP93XX_RTC_LOAD);
 	return 0;
 }
 
diff --git a/drivers/rtc/rtc-gemini.c b/drivers/rtc/rtc-gemini.c
new file mode 100644
index 000000000000..35f4486738fc
--- /dev/null
+++ b/drivers/rtc/rtc-gemini.c
@@ -0,0 +1,175 @@
+/*
+ *  Gemini OnChip RTC
+ *
+ *  Copyright (C) 2009 Janos Laube <janos.dev@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * Original code for older kernel 2.6.15 are from Stormlinksemi
+ * first update from Janos Laube for > 2.6.29 kernels
+ *
+ * checkpatch fixes and usage of rtc-lib code
+ * Hans Ulli Kroll <ulli.kroll@googlemail.com>
+ */
+
+#include <linux/rtc.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#define DRV_NAME        "rtc-gemini"
+#define DRV_VERSION     "0.2"
+
+MODULE_AUTHOR("Hans Ulli Kroll <ulli.kroll@googlemail.com>");
+MODULE_DESCRIPTION("RTC driver for Gemini SoC");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRV_NAME);
+
+struct gemini_rtc {
+	struct rtc_device	*rtc_dev;
+	void __iomem		*rtc_base;
+	int			rtc_irq;
+};
+
+enum gemini_rtc_offsets {
+	GEMINI_RTC_SECOND	= 0x00,
+	GEMINI_RTC_MINUTE	= 0x04,
+	GEMINI_RTC_HOUR		= 0x08,
+	GEMINI_RTC_DAYS		= 0x0C,
+	GEMINI_RTC_ALARM_SECOND	= 0x10,
+	GEMINI_RTC_ALARM_MINUTE	= 0x14,
+	GEMINI_RTC_ALARM_HOUR	= 0x18,
+	GEMINI_RTC_RECORD	= 0x1C,
+	GEMINI_RTC_CR		= 0x20
+};
+
+static irqreturn_t gemini_rtc_interrupt(int irq, void *dev)
+{
+	return IRQ_HANDLED;
+}
+
+/*
+ * Looks like the RTC in the Gemini SoC is (totaly) broken
+ * We can't read/write directly the time from RTC registers.
+ * We must do some "offset" calculation to get the real time
+ *
+ * This FIX works pretty fine and Stormlinksemi aka Cortina-Networks does
+ * the same thing, without the rtc-lib.c calls.
+ */
+
+static int gemini_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct gemini_rtc *rtc = dev_get_drvdata(dev);
+
+	unsigned int  days, hour, min, sec;
+	unsigned long offset, time;
+
+	sec  = readl(rtc->rtc_base + GEMINI_RTC_SECOND);
+	min  = readl(rtc->rtc_base + GEMINI_RTC_MINUTE);
+	hour = readl(rtc->rtc_base + GEMINI_RTC_HOUR);
+	days = readl(rtc->rtc_base + GEMINI_RTC_DAYS);
+	offset = readl(rtc->rtc_base + GEMINI_RTC_RECORD);
+
+	time = offset + days * 86400 + hour * 3600 + min * 60 + sec;
+
+	rtc_time_to_tm(time, tm);
+
+	return 0;
+}
+
+static int gemini_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct gemini_rtc *rtc = dev_get_drvdata(dev);
+	unsigned int sec, min, hour, day;
+	unsigned long offset, time;
+
+	if (tm->tm_year >= 2148)	/* EPOCH Year + 179 */
+		return -EINVAL;
+
+	rtc_tm_to_time(tm, &time);
+
+	sec = readl(rtc->rtc_base + GEMINI_RTC_SECOND);
+	min = readl(rtc->rtc_base + GEMINI_RTC_MINUTE);
+	hour = readl(rtc->rtc_base + GEMINI_RTC_HOUR);
+	day = readl(rtc->rtc_base + GEMINI_RTC_DAYS);
+
+	offset = time - (day * 86400 + hour * 3600 + min * 60 + sec);
+
+	writel(offset, rtc->rtc_base + GEMINI_RTC_RECORD);
+	writel(0x01, rtc->rtc_base + GEMINI_RTC_CR);
+
+	return 0;
+}
+
+static struct rtc_class_ops gemini_rtc_ops = {
+	.read_time     = gemini_rtc_read_time,
+	.set_time      = gemini_rtc_set_time,
+};
+
+static int gemini_rtc_probe(struct platform_device *pdev)
+{
+	struct gemini_rtc *rtc;
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	int ret;
+
+	rtc = devm_kzalloc(&pdev->dev, sizeof(*rtc), GFP_KERNEL);
+	if (unlikely(!rtc))
+		return -ENOMEM;
+	platform_set_drvdata(pdev, rtc);
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res)
+		return -ENODEV;
+
+	rtc->rtc_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	rtc->rtc_base = devm_ioremap(dev, res->start,
+				     resource_size(res));
+
+	ret = devm_request_irq(dev, rtc->rtc_irq, gemini_rtc_interrupt,
+			       IRQF_SHARED, pdev->name, dev);
+	if (unlikely(ret))
+		return ret;
+
+	rtc->rtc_dev = rtc_device_register(pdev->name, dev,
+					   &gemini_rtc_ops, THIS_MODULE);
+	if (likely(IS_ERR(rtc->rtc_dev)))
+		return PTR_ERR(rtc->rtc_dev);
+
+	return 0;
+}
+
+static int gemini_rtc_remove(struct platform_device *pdev)
+{
+	struct gemini_rtc *rtc = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(rtc->rtc_dev);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver gemini_rtc_driver = {
+	.driver		= {
+		.name	= DRV_NAME,
+	},
+	.probe		= gemini_rtc_probe,
+	.remove		= gemini_rtc_remove,
+};
+
+module_platform_driver_probe(gemini_rtc_driver, gemini_rtc_probe);
diff --git a/drivers/rtc/rtc-hid-sensor-time.c b/drivers/rtc/rtc-hid-sensor-time.c
index af4f85a66b39..c398f74234c6 100644
--- a/drivers/rtc/rtc-hid-sensor-time.c
+++ b/drivers/rtc/rtc-hid-sensor-time.c
@@ -318,7 +318,7 @@ static int hid_time_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id hid_time_ids[] = {
+static const struct platform_device_id hid_time_ids[] = {
 	{
 		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
 		.name = "HID-SENSOR-2000a0",
diff --git a/drivers/rtc/rtc-hym8563.c b/drivers/rtc/rtc-hym8563.c
index 0f710e98538f..e9da7959d3fe 100644
--- a/drivers/rtc/rtc-hym8563.c
+++ b/drivers/rtc/rtc-hym8563.c
@@ -548,14 +548,16 @@ static int hym8563_probe(struct i2c_client *client,
 		return ret;
 	}
 
-	ret = devm_request_threaded_irq(&client->dev, client->irq,
-					NULL, hym8563_irq,
-					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
-					client->name, hym8563);
-	if (ret < 0) {
-		dev_err(&client->dev, "irq %d request failed, %d\n",
-			client->irq, ret);
-		return ret;
+	if (client->irq > 0) {
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+						NULL, hym8563_irq,
+						IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+						client->name, hym8563);
+		if (ret < 0) {
+			dev_err(&client->dev, "irq %d request failed, %d\n",
+				client->irq, ret);
+			return ret;
+		}
 	}
 
 	/* check state of calendar information */
diff --git a/drivers/rtc/rtc-imxdi.c b/drivers/rtc/rtc-imxdi.c
index c666eab98273..7bffd7f0e306 100644
--- a/drivers/rtc/rtc-imxdi.c
+++ b/drivers/rtc/rtc-imxdi.c
@@ -129,6 +129,324 @@ struct imxdi_dev {
 	struct work_struct work;
 };
 
+/* Some background:
+ *
+ * The DryIce unit is a complex security/tamper monitor device. To be able do
+ * its job in a useful manner it runs a bigger statemachine to bring it into
+ * security/tamper failure state and once again to bring it out of this state.
+ *
+ * This unit can be in one of three states:
+ *
+ * - "NON-VALID STATE"
+ *   always after the battery power was removed
+ * - "FAILURE STATE"
+ *   if one of the enabled security events has happened
+ * - "VALID STATE"
+ *   if the unit works as expected
+ *
+ * Everything stops when the unit enters the failure state including the RTC
+ * counter (to be able to detect the time the security event happened).
+ *
+ * The following events (when enabled) let the DryIce unit enter the failure
+ * state:
+ *
+ * - wire-mesh-tamper detect
+ * - external tamper B detect
+ * - external tamper A detect
+ * - temperature tamper detect
+ * - clock tamper detect
+ * - voltage tamper detect
+ * - RTC counter overflow
+ * - monotonic counter overflow
+ * - external boot
+ *
+ * If we find the DryIce unit in "FAILURE STATE" and the TDCHL cleared, we
+ * can only detect this state. In this case the unit is completely locked and
+ * must force a second "SYSTEM POR" to bring the DryIce into the
+ * "NON-VALID STATE" + "FAILURE STATE" where a recovery is possible.
+ * If the TDCHL is set in the "FAILURE STATE" we are out of luck. In this case
+ * a battery power cycle is required.
+ *
+ * In the "NON-VALID STATE" + "FAILURE STATE" we can clear the "FAILURE STATE"
+ * and recover the DryIce unit. By clearing the "NON-VALID STATE" as the last
+ * task, we bring back this unit into life.
+ */
+
+/*
+ * Do a write into the unit without interrupt support.
+ * We do not need to check the WEF here, because the only reason this kind of
+ * write error can happen is if we write to the unit twice within the 122 us
+ * interval. This cannot happen, since we are using this function only while
+ * setting up the unit.
+ */
+static void di_write_busy_wait(const struct imxdi_dev *imxdi, u32 val,
+			       unsigned reg)
+{
+	/* do the register write */
+	writel(val, imxdi->ioaddr + reg);
+
+	/*
+	 * now it takes four 32,768 kHz clock cycles to take
+	 * the change into effect = 122 us
+	 */
+	usleep_range(130, 200);
+}
+
+static void di_report_tamper_info(struct imxdi_dev *imxdi,  u32 dsr)
+{
+	u32 dtcr;
+
+	dtcr = readl(imxdi->ioaddr + DTCR);
+
+	dev_emerg(&imxdi->pdev->dev, "DryIce tamper event detected\n");
+	/* the following flags force a transition into the "FAILURE STATE" */
+	if (dsr & DSR_VTD)
+		dev_emerg(&imxdi->pdev->dev, "%sVoltage Tamper Event\n",
+			  dtcr & DTCR_VTE ? "" : "Spurious ");
+
+	if (dsr & DSR_CTD)
+		dev_emerg(&imxdi->pdev->dev, "%s32768 Hz Clock Tamper Event\n",
+			  dtcr & DTCR_CTE ? "" : "Spurious ");
+
+	if (dsr & DSR_TTD)
+		dev_emerg(&imxdi->pdev->dev, "%sTemperature Tamper Event\n",
+			  dtcr & DTCR_TTE ? "" : "Spurious ");
+
+	if (dsr & DSR_SAD)
+		dev_emerg(&imxdi->pdev->dev,
+			  "%sSecure Controller Alarm Event\n",
+			  dtcr & DTCR_SAIE ? "" : "Spurious ");
+
+	if (dsr & DSR_EBD)
+		dev_emerg(&imxdi->pdev->dev, "%sExternal Boot Tamper Event\n",
+			  dtcr & DTCR_EBE ? "" : "Spurious ");
+
+	if (dsr & DSR_ETAD)
+		dev_emerg(&imxdi->pdev->dev, "%sExternal Tamper A Event\n",
+			  dtcr & DTCR_ETAE ? "" : "Spurious ");
+
+	if (dsr & DSR_ETBD)
+		dev_emerg(&imxdi->pdev->dev, "%sExternal Tamper B Event\n",
+			  dtcr & DTCR_ETBE ? "" : "Spurious ");
+
+	if (dsr & DSR_WTD)
+		dev_emerg(&imxdi->pdev->dev, "%sWire-mesh Tamper Event\n",
+			  dtcr & DTCR_WTE ? "" : "Spurious ");
+
+	if (dsr & DSR_MCO)
+		dev_emerg(&imxdi->pdev->dev,
+			  "%sMonotonic-counter Overflow Event\n",
+			  dtcr & DTCR_MOE ? "" : "Spurious ");
+
+	if (dsr & DSR_TCO)
+		dev_emerg(&imxdi->pdev->dev, "%sTimer-counter Overflow Event\n",
+			  dtcr & DTCR_TOE ? "" : "Spurious ");
+}
+
+static void di_what_is_to_be_done(struct imxdi_dev *imxdi,
+				  const char *power_supply)
+{
+	dev_emerg(&imxdi->pdev->dev, "Please cycle the %s power supply in order to get the DryIce/RTC unit working again\n",
+		  power_supply);
+}
+
+static int di_handle_failure_state(struct imxdi_dev *imxdi, u32 dsr)
+{
+	u32 dcr;
+
+	dev_dbg(&imxdi->pdev->dev, "DSR register reports: %08X\n", dsr);
+
+	/* report the cause */
+	di_report_tamper_info(imxdi, dsr);
+
+	dcr = readl(imxdi->ioaddr + DCR);
+
+	if (dcr & DCR_FSHL) {
+		/* we are out of luck */
+		di_what_is_to_be_done(imxdi, "battery");
+		return -ENODEV;
+	}
+	/*
+	 * with the next SYSTEM POR we will transit from the "FAILURE STATE"
+	 * into the "NON-VALID STATE" + "FAILURE STATE"
+	 */
+	di_what_is_to_be_done(imxdi, "main");
+
+	return -ENODEV;
+}
+
+static int di_handle_valid_state(struct imxdi_dev *imxdi, u32 dsr)
+{
+	/* initialize alarm */
+	di_write_busy_wait(imxdi, DCAMR_UNSET, DCAMR);
+	di_write_busy_wait(imxdi, 0, DCALR);
+
+	/* clear alarm flag */
+	if (dsr & DSR_CAF)
+		di_write_busy_wait(imxdi, DSR_CAF, DSR);
+
+	return 0;
+}
+
+static int di_handle_invalid_state(struct imxdi_dev *imxdi, u32 dsr)
+{
+	u32 dcr, sec;
+
+	/*
+	 * lets disable all sources which can force the DryIce unit into
+	 * the "FAILURE STATE" for now
+	 */
+	di_write_busy_wait(imxdi, 0x00000000, DTCR);
+	/* and lets protect them at runtime from any change */
+	di_write_busy_wait(imxdi, DCR_TDCSL, DCR);
+
+	sec = readl(imxdi->ioaddr + DTCMR);
+	if (sec != 0)
+		dev_warn(&imxdi->pdev->dev,
+			 "The security violation has happend at %u seconds\n",
+			 sec);
+	/*
+	 * the timer cannot be set/modified if
+	 * - the TCHL or TCSL bit is set in DCR
+	 */
+	dcr = readl(imxdi->ioaddr + DCR);
+	if (!(dcr & DCR_TCE)) {
+		if (dcr & DCR_TCHL) {
+			/* we are out of luck */
+			di_what_is_to_be_done(imxdi, "battery");
+			return -ENODEV;
+		}
+		if (dcr & DCR_TCSL) {
+			di_what_is_to_be_done(imxdi, "main");
+			return -ENODEV;
+		}
+	}
+	/*
+	 * - the timer counter stops/is stopped if
+	 *   - its overflow flag is set (TCO in DSR)
+	 *      -> clear overflow bit to make it count again
+	 *   - NVF is set in DSR
+	 *      -> clear non-valid bit to make it count again
+	 *   - its TCE (DCR) is cleared
+	 *      -> set TCE to make it count
+	 *   - it was never set before
+	 *      -> write a time into it (required again if the NVF was set)
+	 */
+	/* state handled */
+	di_write_busy_wait(imxdi, DSR_NVF, DSR);
+	/* clear overflow flag */
+	di_write_busy_wait(imxdi, DSR_TCO, DSR);
+	/* enable the counter */
+	di_write_busy_wait(imxdi, dcr | DCR_TCE, DCR);
+	/* set and trigger it to make it count */
+	di_write_busy_wait(imxdi, sec, DTCMR);
+
+	/* now prepare for the valid state */
+	return di_handle_valid_state(imxdi, __raw_readl(imxdi->ioaddr + DSR));
+}
+
+static int di_handle_invalid_and_failure_state(struct imxdi_dev *imxdi, u32 dsr)
+{
+	u32 dcr;
+
+	/*
+	 * now we must first remove the tamper sources in order to get the
+	 * device out of the "FAILURE STATE"
+	 * To disable any of the following sources we need to modify the DTCR
+	 */
+	if (dsr & (DSR_WTD | DSR_ETBD | DSR_ETAD | DSR_EBD | DSR_SAD |
+			DSR_TTD | DSR_CTD | DSR_VTD | DSR_MCO | DSR_TCO)) {
+		dcr = __raw_readl(imxdi->ioaddr + DCR);
+		if (dcr & DCR_TDCHL) {
+			/*
+			 * the tamper register is locked. We cannot disable the
+			 * tamper detection. The TDCHL can only be reset by a
+			 * DRYICE POR, but we cannot force a DRYICE POR in
+			 * softwere because we are still in "FAILURE STATE".
+			 * We need a DRYICE POR via battery power cycling....
+			 */
+			/*
+			 * out of luck!
+			 * we cannot disable them without a DRYICE POR
+			 */
+			di_what_is_to_be_done(imxdi, "battery");
+			return -ENODEV;
+		}
+		if (dcr & DCR_TDCSL) {
+			/* a soft lock can be removed by a SYSTEM POR */
+			di_what_is_to_be_done(imxdi, "main");
+			return -ENODEV;
+		}
+	}
+
+	/* disable all sources */
+	di_write_busy_wait(imxdi, 0x00000000, DTCR);
+
+	/* clear the status bits now */
+	di_write_busy_wait(imxdi, dsr & (DSR_WTD | DSR_ETBD | DSR_ETAD |
+			DSR_EBD | DSR_SAD | DSR_TTD | DSR_CTD | DSR_VTD |
+			DSR_MCO | DSR_TCO), DSR);
+
+	dsr = readl(imxdi->ioaddr + DSR);
+	if ((dsr & ~(DSR_NVF | DSR_SVF | DSR_WBF | DSR_WNF |
+			DSR_WCF | DSR_WEF)) != 0)
+		dev_warn(&imxdi->pdev->dev,
+			 "There are still some sources of pain in DSR: %08x!\n",
+			 dsr & ~(DSR_NVF | DSR_SVF | DSR_WBF | DSR_WNF |
+				 DSR_WCF | DSR_WEF));
+
+	/*
+	 * now we are trying to clear the "Security-violation flag" to
+	 * get the DryIce out of this state
+	 */
+	di_write_busy_wait(imxdi, DSR_SVF, DSR);
+
+	/* success? */
+	dsr = readl(imxdi->ioaddr + DSR);
+	if (dsr & DSR_SVF) {
+		dev_crit(&imxdi->pdev->dev,
+			 "Cannot clear the security violation flag. We are ending up in an endless loop!\n");
+		/* last resort */
+		di_what_is_to_be_done(imxdi, "battery");
+		return -ENODEV;
+	}
+
+	/*
+	 * now we have left the "FAILURE STATE" and ending up in the
+	 * "NON-VALID STATE" time to recover everything
+	 */
+	return di_handle_invalid_state(imxdi, dsr);
+}
+
+static int di_handle_state(struct imxdi_dev *imxdi)
+{
+	int rc;
+	u32 dsr;
+
+	dsr = readl(imxdi->ioaddr + DSR);
+
+	switch (dsr & (DSR_NVF | DSR_SVF)) {
+	case DSR_NVF:
+		dev_warn(&imxdi->pdev->dev, "Invalid stated unit detected\n");
+		rc = di_handle_invalid_state(imxdi, dsr);
+		break;
+	case DSR_SVF:
+		dev_warn(&imxdi->pdev->dev, "Failure stated unit detected\n");
+		rc = di_handle_failure_state(imxdi, dsr);
+		break;
+	case DSR_NVF | DSR_SVF:
+		dev_warn(&imxdi->pdev->dev,
+			 "Failure+Invalid stated unit detected\n");
+		rc = di_handle_invalid_and_failure_state(imxdi, dsr);
+		break;
+	default:
+		dev_notice(&imxdi->pdev->dev, "Unlocked unit detected\n");
+		rc = di_handle_valid_state(imxdi, dsr);
+	}
+
+	return rc;
+}
+
 /*
  * enable a dryice interrupt
  */
@@ -137,8 +455,8 @@ static void di_int_enable(struct imxdi_dev *imxdi, u32 intr)
 	unsigned long flags;
 
 	spin_lock_irqsave(&imxdi->irq_lock, flags);
-	__raw_writel(__raw_readl(imxdi->ioaddr + DIER) | intr,
-			imxdi->ioaddr + DIER);
+	writel(readl(imxdi->ioaddr + DIER) | intr,
+	       imxdi->ioaddr + DIER);
 	spin_unlock_irqrestore(&imxdi->irq_lock, flags);
 }
 
@@ -150,8 +468,8 @@ static void di_int_disable(struct imxdi_dev *imxdi, u32 intr)
 	unsigned long flags;
 
 	spin_lock_irqsave(&imxdi->irq_lock, flags);
-	__raw_writel(__raw_readl(imxdi->ioaddr + DIER) & ~intr,
-			imxdi->ioaddr + DIER);
+	writel(readl(imxdi->ioaddr + DIER) & ~intr,
+	       imxdi->ioaddr + DIER);
 	spin_unlock_irqrestore(&imxdi->irq_lock, flags);
 }
 
@@ -169,11 +487,11 @@ static void clear_write_error(struct imxdi_dev *imxdi)
 	dev_warn(&imxdi->pdev->dev, "WARNING: Register write error!\n");
 
 	/* clear the write error flag */
-	__raw_writel(DSR_WEF, imxdi->ioaddr + DSR);
+	writel(DSR_WEF, imxdi->ioaddr + DSR);
 
 	/* wait for it to take effect */
 	for (cnt = 0; cnt < 1000; cnt++) {
-		if ((__raw_readl(imxdi->ioaddr + DSR) & DSR_WEF) == 0)
+		if ((readl(imxdi->ioaddr + DSR) & DSR_WEF) == 0)
 			return;
 		udelay(10);
 	}
@@ -201,7 +519,7 @@ static int di_write_wait(struct imxdi_dev *imxdi, u32 val, int reg)
 	imxdi->dsr = 0;
 
 	/* do the register write */
-	__raw_writel(val, imxdi->ioaddr + reg);
+	writel(val, imxdi->ioaddr + reg);
 
 	/* wait for the write to finish */
 	ret = wait_event_interruptible_timeout(imxdi->write_wait,
@@ -235,7 +553,7 @@ static int dryice_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	struct imxdi_dev *imxdi = dev_get_drvdata(dev);
 	unsigned long now;
 
-	now = __raw_readl(imxdi->ioaddr + DTCMR);
+	now = readl(imxdi->ioaddr + DTCMR);
 	rtc_time_to_tm(now, tm);
 
 	return 0;
@@ -248,14 +566,35 @@ static int dryice_rtc_read_time(struct device *dev, struct rtc_time *tm)
 static int dryice_rtc_set_mmss(struct device *dev, unsigned long secs)
 {
 	struct imxdi_dev *imxdi = dev_get_drvdata(dev);
+	u32 dcr, dsr;
 	int rc;
 
+	dcr = readl(imxdi->ioaddr + DCR);
+	dsr = readl(imxdi->ioaddr + DSR);
+
+	if (!(dcr & DCR_TCE) || (dsr & DSR_SVF)) {
+		if (dcr & DCR_TCHL) {
+			/* we are even more out of luck */
+			di_what_is_to_be_done(imxdi, "battery");
+			return -EPERM;
+		}
+		if ((dcr & DCR_TCSL) || (dsr & DSR_SVF)) {
+			/* we are out of luck for now */
+			di_what_is_to_be_done(imxdi, "main");
+			return -EPERM;
+		}
+	}
+
 	/* zero the fractional part first */
 	rc = di_write_wait(imxdi, 0, DTCLR);
-	if (rc == 0)
-		rc = di_write_wait(imxdi, secs, DTCMR);
+	if (rc != 0)
+		return rc;
 
-	return rc;
+	rc = di_write_wait(imxdi, secs, DTCMR);
+	if (rc != 0)
+		return rc;
+
+	return di_write_wait(imxdi, readl(imxdi->ioaddr + DCR) | DCR_TCE, DCR);
 }
 
 static int dryice_rtc_alarm_irq_enable(struct device *dev,
@@ -280,17 +619,17 @@ static int dryice_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 	struct imxdi_dev *imxdi = dev_get_drvdata(dev);
 	u32 dcamr;
 
-	dcamr = __raw_readl(imxdi->ioaddr + DCAMR);
+	dcamr = readl(imxdi->ioaddr + DCAMR);
 	rtc_time_to_tm(dcamr, &alarm->time);
 
 	/* alarm is enabled if the interrupt is enabled */
-	alarm->enabled = (__raw_readl(imxdi->ioaddr + DIER) & DIER_CAIE) != 0;
+	alarm->enabled = (readl(imxdi->ioaddr + DIER) & DIER_CAIE) != 0;
 
 	/* don't allow the DSR read to mess up DSR_WCF */
 	mutex_lock(&imxdi->write_mutex);
 
 	/* alarm is pending if the alarm flag is set */
-	alarm->pending = (__raw_readl(imxdi->ioaddr + DSR) & DSR_CAF) != 0;
+	alarm->pending = (readl(imxdi->ioaddr + DSR) & DSR_CAF) != 0;
 
 	mutex_unlock(&imxdi->write_mutex);
 
@@ -312,7 +651,7 @@ static int dryice_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 		return rc;
 
 	/* don't allow setting alarm in the past */
-	now = __raw_readl(imxdi->ioaddr + DTCMR);
+	now = readl(imxdi->ioaddr + DTCMR);
 	if (alarm_time < now)
 		return -EINVAL;
 
@@ -346,7 +685,26 @@ static irqreturn_t dryice_norm_irq(int irq, void *dev_id)
 	u32 dsr, dier;
 	irqreturn_t rc = IRQ_NONE;
 
-	dier = __raw_readl(imxdi->ioaddr + DIER);
+	dier = readl(imxdi->ioaddr + DIER);
+	dsr = readl(imxdi->ioaddr + DSR);
+
+	/* handle the security violation event */
+	if (dier & DIER_SVIE) {
+		if (dsr & DSR_SVF) {
+			/*
+			 * Disable the interrupt when this kind of event has
+			 * happened.
+			 * There cannot be more than one event of this type,
+			 * because it needs a complex state change
+			 * including a main power cycle to get again out of
+			 * this state.
+			 */
+			di_int_disable(imxdi, DIER_SVIE);
+			/* report the violation */
+			di_report_tamper_info(imxdi, dsr);
+			rc = IRQ_HANDLED;
+		}
+	}
 
 	/* handle write complete and write error cases */
 	if (dier & DIER_WCIE) {
@@ -357,7 +715,6 @@ static irqreturn_t dryice_norm_irq(int irq, void *dev_id)
 			return rc;
 
 		/* DSR_WCF clears itself on DSR read */
-		dsr = __raw_readl(imxdi->ioaddr + DSR);
 		if (dsr & (DSR_WCF | DSR_WEF)) {
 			/* mask the interrupt */
 			di_int_disable(imxdi, DIER_WCIE);
@@ -373,7 +730,6 @@ static irqreturn_t dryice_norm_irq(int irq, void *dev_id)
 	/* handle the alarm case */
 	if (dier & DIER_CAIE) {
 		/* DSR_WCF clears itself on DSR read */
-		dsr = __raw_readl(imxdi->ioaddr + DSR);
 		if (dsr & DSR_CAF) {
 			/* mask the interrupt */
 			di_int_disable(imxdi, DIER_CAIE);
@@ -446,7 +802,11 @@ static int __init dryice_rtc_probe(struct platform_device *pdev)
 	 */
 
 	/* mask all interrupts */
-	__raw_writel(0, imxdi->ioaddr + DIER);
+	writel(0, imxdi->ioaddr + DIER);
+
+	rc = di_handle_state(imxdi);
+	if (rc != 0)
+		goto err;
 
 	rc = devm_request_irq(&pdev->dev, imxdi->irq, dryice_norm_irq,
 			IRQF_SHARED, pdev->name, imxdi);
@@ -455,44 +815,6 @@ static int __init dryice_rtc_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	/* put dryice into valid state */
-	if (__raw_readl(imxdi->ioaddr + DSR) & DSR_NVF) {
-		rc = di_write_wait(imxdi, DSR_NVF | DSR_SVF, DSR);
-		if (rc)
-			goto err;
-	}
-
-	/* initialize alarm */
-	rc = di_write_wait(imxdi, DCAMR_UNSET, DCAMR);
-	if (rc)
-		goto err;
-	rc = di_write_wait(imxdi, 0, DCALR);
-	if (rc)
-		goto err;
-
-	/* clear alarm flag */
-	if (__raw_readl(imxdi->ioaddr + DSR) & DSR_CAF) {
-		rc = di_write_wait(imxdi, DSR_CAF, DSR);
-		if (rc)
-			goto err;
-	}
-
-	/* the timer won't count if it has never been written to */
-	if (__raw_readl(imxdi->ioaddr + DTCMR) == 0) {
-		rc = di_write_wait(imxdi, 0, DTCMR);
-		if (rc)
-			goto err;
-	}
-
-	/* start keeping time */
-	if (!(__raw_readl(imxdi->ioaddr + DCR) & DCR_TCE)) {
-		rc = di_write_wait(imxdi,
-				__raw_readl(imxdi->ioaddr + DCR) | DCR_TCE,
-				DCR);
-		if (rc)
-			goto err;
-	}
-
 	platform_set_drvdata(pdev, imxdi);
 	imxdi->rtc = devm_rtc_device_register(&pdev->dev, pdev->name,
 				  &dryice_rtc_ops, THIS_MODULE);
@@ -516,7 +838,7 @@ static int __exit dryice_rtc_remove(struct platform_device *pdev)
 	flush_work(&imxdi->work);
 
 	/* mask all interrupts */
-	__raw_writel(0, imxdi->ioaddr + DIER);
+	writel(0, imxdi->ioaddr + DIER);
 
 	clk_disable_unprepare(imxdi->clk);
 
diff --git a/drivers/rtc/rtc-isl1208.c b/drivers/rtc/rtc-isl1208.c
index c3c549d511b9..aa3b8f1b34d9 100644
--- a/drivers/rtc/rtc-isl1208.c
+++ b/drivers/rtc/rtc-isl1208.c
@@ -370,22 +370,15 @@ isl1208_i2c_set_alarm(struct i2c_client *client, struct rtc_wkalrm *alarm)
 	struct rtc_time *alarm_tm = &alarm->time;
 	u8 regs[ISL1208_ALARM_SECTION_LEN] = { 0, };
 	const int offs = ISL1208_REG_SCA;
-	unsigned long rtc_secs, alarm_secs;
 	struct rtc_time rtc_tm;
 	int err, enable;
 
 	err = isl1208_i2c_read_time(client, &rtc_tm);
 	if (err)
 		return err;
-	err = rtc_tm_to_time(&rtc_tm, &rtc_secs);
-	if (err)
-		return err;
-	err = rtc_tm_to_time(alarm_tm, &alarm_secs);
-	if (err)
-		return err;
 
 	/* If the alarm time is before the current time disable the alarm */
-	if (!alarm->enabled || alarm_secs <= rtc_secs)
+	if (!alarm->enabled || rtc_tm_sub(alarm_tm, &rtc_tm) <= 0)
 		enable = 0x00;
 	else
 		enable = 0x80;
diff --git a/drivers/rtc/rtc-ls1x.c b/drivers/rtc/rtc-ls1x.c
index 8445e564094a..22a9ec4f2b83 100644
--- a/drivers/rtc/rtc-ls1x.c
+++ b/drivers/rtc/rtc-ls1x.c
@@ -17,7 +17,7 @@
 #include <linux/delay.h>
 #include <linux/types.h>
 #include <linux/io.h>
-#include <asm/mach-loongson1/loongson1.h>
+#include <loongson1.h>
 
 #define LS1X_RTC_REG_OFFSET	(LS1X_RTC_BASE + 0x20)
 #define LS1X_RTC_REGS(x) \
diff --git a/drivers/rtc/rtc-max6900.c b/drivers/rtc/rtc-max6900.c
index 4804985b876e..b2a76077bbfa 100644
--- a/drivers/rtc/rtc-max6900.c
+++ b/drivers/rtc/rtc-max6900.c
@@ -234,6 +234,7 @@ static struct i2c_device_id max6900_id[] = {
 	{ "max6900", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, max6900_id);
 
 static struct i2c_driver max6900_driver = {
 	.driver = {
diff --git a/drivers/rtc/rtc-max77686.c b/drivers/rtc/rtc-max77686.c
index 7632a87784c3..7184a0eda793 100644
--- a/drivers/rtc/rtc-max77686.c
+++ b/drivers/rtc/rtc-max77686.c
@@ -511,6 +511,7 @@ static const struct platform_device_id rtc_id[] = {
 	{ "max77686-rtc", 0 },
 	{},
 };
+MODULE_DEVICE_TABLE(platform, rtc_id);
 
 static struct platform_driver max77686_rtc_driver = {
 	.driver		= {
diff --git a/drivers/rtc/rtc-max77802.c b/drivers/rtc/rtc-max77802.c
index 7f8adf8d6feb..82ffcc5a5345 100644
--- a/drivers/rtc/rtc-max77802.c
+++ b/drivers/rtc/rtc-max77802.c
@@ -484,6 +484,7 @@ static const struct platform_device_id rtc_id[] = {
 	{ "max77802-rtc", 0 },
 	{},
 };
+MODULE_DEVICE_TABLE(platform, rtc_id);
 
 static struct platform_driver max77802_rtc_driver = {
 	.driver		= {
diff --git a/drivers/rtc/rtc-max8998.c b/drivers/rtc/rtc-max8998.c
index 5726ef7bd56e..30804b00985e 100644
--- a/drivers/rtc/rtc-max8998.c
+++ b/drivers/rtc/rtc-max8998.c
@@ -309,6 +309,7 @@ static const struct platform_device_id max8998_rtc_id[] = {
 	{ "lp3974-rtc", TYPE_LP3974 },
 	{ }
 };
+MODULE_DEVICE_TABLE(platform, max8998_rtc_id);
 
 static struct platform_driver max8998_rtc_driver = {
 	.driver		= {
diff --git a/drivers/rtc/rtc-mc13xxx.c b/drivers/rtc/rtc-mc13xxx.c
index 32df1d812367..a65868065743 100644
--- a/drivers/rtc/rtc-mc13xxx.c
+++ b/drivers/rtc/rtc-mc13xxx.c
@@ -216,7 +216,7 @@ static int mc13xxx_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 
 	s1970 = rtc_tm_to_time64(&alarm->time);
 
-	dev_dbg(dev, "%s: o%2.s %lld\n", __func__, alarm->enabled ? "n" : "ff",
+	dev_dbg(dev, "%s: %s %lld\n", __func__, alarm->enabled ? "on" : "off",
 			(long long)s1970);
 
 	ret = mc13xxx_rtc_irq_enable_unlocked(dev, alarm->enabled,
diff --git a/drivers/rtc/rtc-mt6397.c b/drivers/rtc/rtc-mt6397.c
new file mode 100644
index 000000000000..c0090b698ff3
--- /dev/null
+++ b/drivers/rtc/rtc-mt6397.c
@@ -0,0 +1,395 @@
+/*
+* Copyright (c) 2014-2015 MediaTek Inc.
+* Author: Tianping.Fang <tianping.fang@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/delay.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/rtc.h>
+#include <linux/irqdomain.h>
+#include <linux/platform_device.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/io.h>
+#include <linux/mfd/mt6397/core.h>
+
+#define RTC_BBPU		0x0000
+#define RTC_BBPU_CBUSY		BIT(6)
+
+#define RTC_WRTGR		0x003c
+
+#define RTC_IRQ_STA		0x0002
+#define RTC_IRQ_STA_AL		BIT(0)
+#define RTC_IRQ_STA_LP		BIT(3)
+
+#define RTC_IRQ_EN		0x0004
+#define RTC_IRQ_EN_AL		BIT(0)
+#define RTC_IRQ_EN_ONESHOT	BIT(2)
+#define RTC_IRQ_EN_LP		BIT(3)
+#define RTC_IRQ_EN_ONESHOT_AL	(RTC_IRQ_EN_ONESHOT | RTC_IRQ_EN_AL)
+
+#define RTC_AL_MASK		0x0008
+#define RTC_AL_MASK_DOW		BIT(4)
+
+#define RTC_TC_SEC		0x000a
+/* Min, Hour, Dom... register offset to RTC_TC_SEC */
+#define RTC_OFFSET_SEC		0
+#define RTC_OFFSET_MIN		1
+#define RTC_OFFSET_HOUR		2
+#define RTC_OFFSET_DOM		3
+#define RTC_OFFSET_DOW		4
+#define RTC_OFFSET_MTH		5
+#define RTC_OFFSET_YEAR		6
+#define RTC_OFFSET_COUNT	7
+
+#define RTC_AL_SEC		0x0018
+
+#define RTC_PDN2		0x002e
+#define RTC_PDN2_PWRON_ALARM	BIT(4)
+
+#define RTC_MIN_YEAR		1968
+#define RTC_BASE_YEAR		1900
+#define RTC_NUM_YEARS		128
+#define RTC_MIN_YEAR_OFFSET	(RTC_MIN_YEAR - RTC_BASE_YEAR)
+
+struct mt6397_rtc {
+	struct device		*dev;
+	struct rtc_device	*rtc_dev;
+	struct mutex		lock;
+	struct regmap		*regmap;
+	int			irq;
+	u32			addr_base;
+};
+
+static int mtk_rtc_write_trigger(struct mt6397_rtc *rtc)
+{
+	unsigned long timeout = jiffies + HZ;
+	int ret;
+	u32 data;
+
+	ret = regmap_write(rtc->regmap, rtc->addr_base + RTC_WRTGR, 1);
+	if (ret < 0)
+		return ret;
+
+	while (1) {
+		ret = regmap_read(rtc->regmap, rtc->addr_base + RTC_BBPU,
+				  &data);
+		if (ret < 0)
+			break;
+		if (!(data & RTC_BBPU_CBUSY))
+			break;
+		if (time_after(jiffies, timeout)) {
+			ret = -ETIMEDOUT;
+			break;
+		}
+		cpu_relax();
+	}
+
+	return ret;
+}
+
+static irqreturn_t mtk_rtc_irq_handler_thread(int irq, void *data)
+{
+	struct mt6397_rtc *rtc = data;
+	u32 irqsta, irqen;
+	int ret;
+
+	ret = regmap_read(rtc->regmap, rtc->addr_base + RTC_IRQ_STA, &irqsta);
+	if ((ret >= 0) && (irqsta & RTC_IRQ_STA_AL)) {
+		rtc_update_irq(rtc->rtc_dev, 1, RTC_IRQF | RTC_AF);
+		irqen = irqsta & ~RTC_IRQ_EN_AL;
+		mutex_lock(&rtc->lock);
+		if (regmap_write(rtc->regmap, rtc->addr_base + RTC_IRQ_EN,
+				 irqen) < 0)
+			mtk_rtc_write_trigger(rtc);
+		mutex_unlock(&rtc->lock);
+
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+static int __mtk_rtc_read_time(struct mt6397_rtc *rtc,
+			       struct rtc_time *tm, int *sec)
+{
+	int ret;
+	u16 data[RTC_OFFSET_COUNT];
+
+	mutex_lock(&rtc->lock);
+	ret = regmap_bulk_read(rtc->regmap, rtc->addr_base + RTC_TC_SEC,
+			       data, RTC_OFFSET_COUNT);
+	if (ret < 0)
+		goto exit;
+
+	tm->tm_sec = data[RTC_OFFSET_SEC];
+	tm->tm_min = data[RTC_OFFSET_MIN];
+	tm->tm_hour = data[RTC_OFFSET_HOUR];
+	tm->tm_mday = data[RTC_OFFSET_DOM];
+	tm->tm_mon = data[RTC_OFFSET_MTH];
+	tm->tm_year = data[RTC_OFFSET_YEAR];
+
+	ret = regmap_read(rtc->regmap, rtc->addr_base + RTC_TC_SEC, sec);
+exit:
+	mutex_unlock(&rtc->lock);
+	return ret;
+}
+
+static int mtk_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	time64_t time;
+	struct mt6397_rtc *rtc = dev_get_drvdata(dev);
+	int days, sec, ret;
+
+	do {
+		ret = __mtk_rtc_read_time(rtc, tm, &sec);
+		if (ret < 0)
+			goto exit;
+	} while (sec < tm->tm_sec);
+
+	/* HW register use 7 bits to store year data, minus
+	 * RTC_MIN_YEAR_OFFSET before write year data to register, and plus
+	 * RTC_MIN_YEAR_OFFSET back after read year from register
+	 */
+	tm->tm_year += RTC_MIN_YEAR_OFFSET;
+
+	/* HW register start mon from one, but tm_mon start from zero. */
+	tm->tm_mon--;
+	time = rtc_tm_to_time64(tm);
+
+	/* rtc_tm_to_time64 covert Gregorian date to seconds since
+	 * 01-01-1970 00:00:00, and this date is Thursday.
+	 */
+	days = div_s64(time, 86400);
+	tm->tm_wday = (days + 4) % 7;
+
+exit:
+	return ret;
+}
+
+static int mtk_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct mt6397_rtc *rtc = dev_get_drvdata(dev);
+	int ret;
+	u16 data[RTC_OFFSET_COUNT];
+
+	tm->tm_year -= RTC_MIN_YEAR_OFFSET;
+	tm->tm_mon++;
+
+	data[RTC_OFFSET_SEC] = tm->tm_sec;
+	data[RTC_OFFSET_MIN] = tm->tm_min;
+	data[RTC_OFFSET_HOUR] = tm->tm_hour;
+	data[RTC_OFFSET_DOM] = tm->tm_mday;
+	data[RTC_OFFSET_MTH] = tm->tm_mon;
+	data[RTC_OFFSET_YEAR] = tm->tm_year;
+
+	mutex_lock(&rtc->lock);
+	ret = regmap_bulk_write(rtc->regmap, rtc->addr_base + RTC_TC_SEC,
+				data, RTC_OFFSET_COUNT);
+	if (ret < 0)
+		goto exit;
+
+	/* Time register write to hardware after call trigger function */
+	ret = mtk_rtc_write_trigger(rtc);
+
+exit:
+	mutex_unlock(&rtc->lock);
+	return ret;
+}
+
+static int mtk_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alm)
+{
+	struct rtc_time *tm = &alm->time;
+	struct mt6397_rtc *rtc = dev_get_drvdata(dev);
+	u32 irqen, pdn2;
+	int ret;
+	u16 data[RTC_OFFSET_COUNT];
+
+	mutex_lock(&rtc->lock);
+	ret = regmap_read(rtc->regmap, rtc->addr_base + RTC_IRQ_EN, &irqen);
+	if (ret < 0)
+		goto err_exit;
+	ret = regmap_read(rtc->regmap, rtc->addr_base + RTC_PDN2, &pdn2);
+	if (ret < 0)
+		goto err_exit;
+
+	ret = regmap_bulk_read(rtc->regmap, rtc->addr_base + RTC_AL_SEC,
+			       data, RTC_OFFSET_COUNT);
+	if (ret < 0)
+		goto err_exit;
+
+	alm->enabled = !!(irqen & RTC_IRQ_EN_AL);
+	alm->pending = !!(pdn2 & RTC_PDN2_PWRON_ALARM);
+	mutex_unlock(&rtc->lock);
+
+	tm->tm_sec = data[RTC_OFFSET_SEC];
+	tm->tm_min = data[RTC_OFFSET_MIN];
+	tm->tm_hour = data[RTC_OFFSET_HOUR];
+	tm->tm_mday = data[RTC_OFFSET_DOM];
+	tm->tm_mon = data[RTC_OFFSET_MTH];
+	tm->tm_year = data[RTC_OFFSET_YEAR];
+
+	tm->tm_year += RTC_MIN_YEAR_OFFSET;
+	tm->tm_mon--;
+
+	return 0;
+err_exit:
+	mutex_unlock(&rtc->lock);
+	return ret;
+}
+
+static int mtk_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alm)
+{
+	struct rtc_time *tm = &alm->time;
+	struct mt6397_rtc *rtc = dev_get_drvdata(dev);
+	int ret;
+	u16 data[RTC_OFFSET_COUNT];
+
+	tm->tm_year -= RTC_MIN_YEAR_OFFSET;
+	tm->tm_mon++;
+
+	data[RTC_OFFSET_SEC] = tm->tm_sec;
+	data[RTC_OFFSET_MIN] = tm->tm_min;
+	data[RTC_OFFSET_HOUR] = tm->tm_hour;
+	data[RTC_OFFSET_DOM] = tm->tm_mday;
+	data[RTC_OFFSET_MTH] = tm->tm_mon;
+	data[RTC_OFFSET_YEAR] = tm->tm_year;
+
+	mutex_lock(&rtc->lock);
+	if (alm->enabled) {
+		ret = regmap_bulk_write(rtc->regmap,
+					rtc->addr_base + RTC_AL_SEC,
+					data, RTC_OFFSET_COUNT);
+		if (ret < 0)
+			goto exit;
+		ret = regmap_write(rtc->regmap, rtc->addr_base + RTC_AL_MASK,
+				   RTC_AL_MASK_DOW);
+		if (ret < 0)
+			goto exit;
+		ret = regmap_update_bits(rtc->regmap,
+					 rtc->addr_base + RTC_IRQ_EN,
+					 RTC_IRQ_EN_ONESHOT_AL,
+					 RTC_IRQ_EN_ONESHOT_AL);
+		if (ret < 0)
+			goto exit;
+	} else {
+		ret = regmap_update_bits(rtc->regmap,
+					 rtc->addr_base + RTC_IRQ_EN,
+					 RTC_IRQ_EN_ONESHOT_AL, 0);
+		if (ret < 0)
+			goto exit;
+	}
+
+	/* All alarm time register write to hardware after calling
+	 * mtk_rtc_write_trigger. This can avoid race condition if alarm
+	 * occur happen during writing alarm time register.
+	 */
+	ret = mtk_rtc_write_trigger(rtc);
+exit:
+	mutex_unlock(&rtc->lock);
+	return ret;
+}
+
+static struct rtc_class_ops mtk_rtc_ops = {
+	.read_time  = mtk_rtc_read_time,
+	.set_time   = mtk_rtc_set_time,
+	.read_alarm = mtk_rtc_read_alarm,
+	.set_alarm  = mtk_rtc_set_alarm,
+};
+
+static int mtk_rtc_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct mt6397_chip *mt6397_chip = dev_get_drvdata(pdev->dev.parent);
+	struct mt6397_rtc *rtc;
+	int ret;
+
+	rtc = devm_kzalloc(&pdev->dev, sizeof(struct mt6397_rtc), GFP_KERNEL);
+	if (!rtc)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	rtc->addr_base = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	rtc->irq = irq_create_mapping(mt6397_chip->irq_domain, res->start);
+	if (rtc->irq <= 0)
+		return -EINVAL;
+
+	rtc->regmap = mt6397_chip->regmap;
+	rtc->dev = &pdev->dev;
+	mutex_init(&rtc->lock);
+
+	platform_set_drvdata(pdev, rtc);
+
+	ret = request_threaded_irq(rtc->irq, NULL,
+				   mtk_rtc_irq_handler_thread,
+				   IRQF_ONESHOT | IRQF_TRIGGER_HIGH,
+				   "mt6397-rtc", rtc);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request alarm IRQ: %d: %d\n",
+			rtc->irq, ret);
+		goto out_dispose_irq;
+	}
+
+	rtc->rtc_dev = rtc_device_register("mt6397-rtc", &pdev->dev,
+					   &mtk_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc->rtc_dev)) {
+		dev_err(&pdev->dev, "register rtc device failed\n");
+		ret = PTR_ERR(rtc->rtc_dev);
+		goto out_free_irq;
+	}
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	return 0;
+
+out_free_irq:
+	free_irq(rtc->irq, rtc->rtc_dev);
+out_dispose_irq:
+	irq_dispose_mapping(rtc->irq);
+	return ret;
+}
+
+static int mtk_rtc_remove(struct platform_device *pdev)
+{
+	struct mt6397_rtc *rtc = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(rtc->rtc_dev);
+	free_irq(rtc->irq, rtc->rtc_dev);
+	irq_dispose_mapping(rtc->irq);
+
+	return 0;
+}
+
+static const struct of_device_id mt6397_rtc_of_match[] = {
+	{ .compatible = "mediatek,mt6397-rtc", },
+	{ }
+};
+
+static struct platform_driver mtk_rtc_driver = {
+	.driver = {
+		.name = "mt6397-rtc",
+		.of_match_table = mt6397_rtc_of_match,
+	},
+	.probe	= mtk_rtc_probe,
+	.remove = mtk_rtc_remove,
+};
+
+module_platform_driver(mtk_rtc_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Tianping Fang <tianping.fang@mediatek.com>");
+MODULE_DESCRIPTION("RTC Driver for MediaTek MT6397 PMIC");
+MODULE_ALIAS("platform:mt6397-rtc");
diff --git a/drivers/rtc/rtc-mv.c b/drivers/rtc/rtc-mv.c
index 423762241042..7f50d2ef7f6e 100644
--- a/drivers/rtc/rtc-mv.c
+++ b/drivers/rtc/rtc-mv.c
@@ -10,6 +10,7 @@
 #include <linux/kernel.h>
 #include <linux/rtc.h>
 #include <linux/bcd.h>
+#include <linux/bitops.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
@@ -24,7 +25,7 @@
 #define RTC_MINUTES_OFFS	8
 #define RTC_HOURS_OFFS		16
 #define RTC_WDAY_OFFS		24
-#define RTC_HOURS_12H_MODE		(1 << 22) /* 12 hours mode */
+#define RTC_HOURS_12H_MODE	BIT(22) /* 12 hour mode */
 
 #define RTC_DATE_REG_OFFS	4
 #define RTC_MDAY_OFFS		0
@@ -33,7 +34,7 @@
 
 #define RTC_ALARM_TIME_REG_OFFS	8
 #define RTC_ALARM_DATE_REG_OFFS	0xc
-#define RTC_ALARM_VALID		(1 << 7)
+#define RTC_ALARM_VALID		BIT(7)
 
 #define RTC_ALARM_INTERRUPT_MASK_REG_OFFS	0x10
 #define RTC_ALARM_INTERRUPT_CASUE_REG_OFFS	0x14
@@ -77,7 +78,7 @@ static int mv_rtc_read_time(struct device *dev, struct rtc_time *tm)
 
 	second = rtc_time & 0x7f;
 	minute = (rtc_time >> RTC_MINUTES_OFFS) & 0x7f;
-	hour = (rtc_time >> RTC_HOURS_OFFS) & 0x3f; /* assume 24 hours mode */
+	hour = (rtc_time >> RTC_HOURS_OFFS) & 0x3f; /* assume 24 hour mode */
 	wday = (rtc_time >> RTC_WDAY_OFFS) & 0x7;
 
 	day = rtc_date & 0x3f;
@@ -108,7 +109,7 @@ static int mv_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alm)
 
 	second = rtc_time & 0x7f;
 	minute = (rtc_time >> RTC_MINUTES_OFFS) & 0x7f;
-	hour = (rtc_time >> RTC_HOURS_OFFS) & 0x3f; /* assume 24 hours mode */
+	hour = (rtc_time >> RTC_HOURS_OFFS) & 0x3f; /* assume 24 hour mode */
 	wday = (rtc_time >> RTC_WDAY_OFFS) & 0x7;
 
 	day = rtc_date & 0x3f;
@@ -239,10 +240,10 @@ static int __init mv_rtc_probe(struct platform_device *pdev)
 	if (!IS_ERR(pdata->clk))
 		clk_prepare_enable(pdata->clk);
 
-	/* make sure the 24 hours mode is enabled */
+	/* make sure the 24 hour mode is enabled */
 	rtc_time = readl(pdata->ioaddr + RTC_TIME_REG_OFFS);
 	if (rtc_time & RTC_HOURS_12H_MODE) {
-		dev_err(&pdev->dev, "24 Hours mode not supported.\n");
+		dev_err(&pdev->dev, "12 Hour mode is enabled but not supported.\n");
 		ret = -EINVAL;
 		goto out;
 	}
diff --git a/drivers/rtc/rtc-mxc.c b/drivers/rtc/rtc-mxc.c
index 09d422b9f7f7..5fc292c2dfdf 100644
--- a/drivers/rtc/rtc-mxc.c
+++ b/drivers/rtc/rtc-mxc.c
@@ -84,7 +84,7 @@ struct rtc_plat_data {
 	enum imx_rtc_type devtype;
 };
 
-static struct platform_device_id imx_rtc_devtype[] = {
+static const struct platform_device_id imx_rtc_devtype[] = {
 	{
 		.name = "imx1-rtc",
 		.driver_data = IMX1_RTC,
diff --git a/drivers/rtc/rtc-palmas.c b/drivers/rtc/rtc-palmas.c
index 3b01d567496d..7ea2c471feca 100644
--- a/drivers/rtc/rtc-palmas.c
+++ b/drivers/rtc/rtc-palmas.c
@@ -239,7 +239,7 @@ static int palmas_rtc_probe(struct platform_device *pdev)
 	struct palmas_rtc *palmas_rtc = NULL;
 	int ret;
 	bool enable_bb_charging = false;
-	bool high_bb_charging;
+	bool high_bb_charging = false;
 
 	if (pdev->dev.of_node) {
 		enable_bb_charging = of_property_read_bool(pdev->dev.of_node,
diff --git a/drivers/rtc/rtc-pcf8563.c b/drivers/rtc/rtc-pcf8563.c
index 0ba7e59929be..8bba022be946 100644
--- a/drivers/rtc/rtc-pcf8563.c
+++ b/drivers/rtc/rtc-pcf8563.c
@@ -22,7 +22,7 @@
 #include <linux/of.h>
 #include <linux/err.h>
 
-#define DRV_VERSION "0.4.3"
+#define DRV_VERSION "0.4.4"
 
 #define PCF8563_REG_ST1		0x00 /* status */
 #define PCF8563_REG_ST2		0x01
@@ -202,8 +202,9 @@ static int pcf8563_get_datetime(struct i2c_client *client, struct rtc_time *tm)
 
 	if (buf[PCF8563_REG_SC] & PCF8563_SC_LV) {
 		pcf8563->voltage_low = 1;
-		dev_info(&client->dev,
+		dev_err(&client->dev,
 			"low voltage detected, date/time is not reliable.\n");
+		return -EINVAL;
 	}
 
 	dev_dbg(&client->dev,
@@ -234,12 +235,6 @@ static int pcf8563_get_datetime(struct i2c_client *client, struct rtc_time *tm)
 		tm->tm_sec, tm->tm_min, tm->tm_hour,
 		tm->tm_mday, tm->tm_mon, tm->tm_year, tm->tm_wday);
 
-	/* the clock can give out invalid datetime, but we cannot return
-	 * -EINVAL otherwise hwclock will refuse to set the time on bootup.
-	 */
-	if (rtc_valid_tm(tm) < 0)
-		dev_err(&client->dev, "retrieved date/time is not valid.\n");
-
 	return 0;
 }
 
@@ -363,13 +358,13 @@ static int pcf8563_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *tm)
 	struct i2c_client *client = to_i2c_client(dev);
 	unsigned char buf[4];
 	int err;
-	unsigned long alarm_time;
 
 	/* The alarm has no seconds, round up to nearest minute */
 	if (tm->time.tm_sec) {
-		rtc_tm_to_time(&tm->time, &alarm_time);
-		alarm_time += 60-tm->time.tm_sec;
-		rtc_time_to_tm(alarm_time, &tm->time);
+		time64_t alarm_time = rtc_tm_to_time64(&tm->time);
+
+		alarm_time += 60 - tm->time.tm_sec;
+		rtc_time64_to_tm(alarm_time, &tm->time);
 	}
 
 	dev_dbg(dev, "%s, min=%d hour=%d wday=%d mday=%d "
@@ -437,7 +432,7 @@ static int pcf8563_probe(struct i2c_client *client,
 	}
 
 	err = pcf8563_get_alarm_mode(client, NULL, &alm_pending);
-	if (err < 0) {
+	if (err) {
 		dev_err(&client->dev, "%s: read error\n", __func__);
 		return err;
 	}
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 76cbad7a99d3..a0f832362199 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -772,18 +772,6 @@ static struct s3c_rtc_data const s3c6410_rtc_data = {
 	.disable		= s3c6410_rtc_disable,
 };
 
-static struct s3c_rtc_data const exynos3250_rtc_data = {
-	.max_user_freq		= 32768,
-	.needs_src_clk		= true,
-	.irq_handler		= s3c6410_rtc_irq,
-	.set_freq		= s3c6410_rtc_setfreq,
-	.enable_tick		= s3c6410_rtc_enable_tick,
-	.save_tick_cnt		= s3c6410_rtc_save_tick_cnt,
-	.restore_tick_cnt	= s3c6410_rtc_restore_tick_cnt,
-	.enable			= s3c24xx_rtc_enable,
-	.disable		= s3c6410_rtc_disable,
-};
-
 static const struct of_device_id s3c_rtc_dt_match[] = {
 	{
 		.compatible = "samsung,s3c2410-rtc",
@@ -799,7 +787,7 @@ static const struct of_device_id s3c_rtc_dt_match[] = {
 		.data = (void *)&s3c6410_rtc_data,
 	}, {
 		.compatible = "samsung,exynos3250-rtc",
-		.data = (void *)&exynos3250_rtc_data,
+		.data = (void *)&s3c6410_rtc_data,
 	},
 	{ /* sentinel */ },
 };
diff --git a/drivers/rtc/rtc-snvs.c b/drivers/rtc/rtc-snvs.c
index 0479e807a776..d87a85cefb66 100644
--- a/drivers/rtc/rtc-snvs.c
+++ b/drivers/rtc/rtc-snvs.c
@@ -322,6 +322,13 @@ static int snvs_rtc_suspend(struct device *dev)
 	if (device_may_wakeup(dev))
 		enable_irq_wake(data->irq);
 
+	return 0;
+}
+
+static int snvs_rtc_suspend_noirq(struct device *dev)
+{
+	struct snvs_rtc_data *data = dev_get_drvdata(dev);
+
 	if (data->clk)
 		clk_disable_unprepare(data->clk);
 
@@ -331,23 +338,28 @@ static int snvs_rtc_suspend(struct device *dev)
 static int snvs_rtc_resume(struct device *dev)
 {
 	struct snvs_rtc_data *data = dev_get_drvdata(dev);
-	int ret;
 
 	if (device_may_wakeup(dev))
-		disable_irq_wake(data->irq);
+		return disable_irq_wake(data->irq);
 
-	if (data->clk) {
-		ret = clk_prepare_enable(data->clk);
-		if (ret)
-			return ret;
-	}
+	return 0;
+}
+
+static int snvs_rtc_resume_noirq(struct device *dev)
+{
+	struct snvs_rtc_data *data = dev_get_drvdata(dev);
+
+	if (data->clk)
+		return clk_prepare_enable(data->clk);
 
 	return 0;
 }
 
 static const struct dev_pm_ops snvs_rtc_pm_ops = {
-	.suspend_noirq = snvs_rtc_suspend,
-	.resume_noirq = snvs_rtc_resume,
+	.suspend = snvs_rtc_suspend,
+	.suspend_noirq = snvs_rtc_suspend_noirq,
+	.resume = snvs_rtc_resume,
+	.resume_noirq = snvs_rtc_resume_noirq,
 };
 
 #define SNVS_RTC_PM_OPS	(&snvs_rtc_pm_ops)
diff --git a/drivers/rtc/rtc-spear.c b/drivers/rtc/rtc-spear.c
index d2cdb9823a15..f05ef8568480 100644
--- a/drivers/rtc/rtc-spear.c
+++ b/drivers/rtc/rtc-spear.c
@@ -358,12 +358,6 @@ static int spear_rtc_probe(struct platform_device *pdev)
 	int status = 0;
 	int irq;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "no resource defined\n");
-		return -EBUSY;
-	}
-
 	config = devm_kzalloc(&pdev->dev, sizeof(*config), GFP_KERNEL);
 	if (!config)
 		return -ENOMEM;
@@ -383,6 +377,7 @@ static int spear_rtc_probe(struct platform_device *pdev)
 		return status;
 	}
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	config->ioaddr = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(config->ioaddr))
 		return PTR_ERR(config->ioaddr);
diff --git a/drivers/rtc/rtc-st-lpc.c b/drivers/rtc/rtc-st-lpc.c
new file mode 100644
index 000000000000..3f9d0acb81c7
--- /dev/null
+++ b/drivers/rtc/rtc-st-lpc.c
@@ -0,0 +1,354 @@
+/*
+ * rtc-st-lpc.c - ST's LPC RTC, powered by the Low Power Timer
+ *
+ * Copyright (C) 2014 STMicroelectronics Limited
+ *
+ * Author: David Paris <david.paris@st.com> for STMicroelectronics
+ *         Lee Jones <lee.jones@linaro.org> for STMicroelectronics
+ *
+ * Based on the original driver written by Stuart Menefy.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public Licence
+ * as published by the Free Software Foundation; either version
+ * 2 of the Licence, or (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+
+#include <dt-bindings/mfd/st-lpc.h>
+
+/* Low Power Timer */
+#define LPC_LPT_LSB_OFF		0x400
+#define LPC_LPT_MSB_OFF		0x404
+#define LPC_LPT_START_OFF	0x408
+
+/* Low Power Alarm */
+#define LPC_LPA_LSB_OFF		0x410
+#define LPC_LPA_MSB_OFF		0x414
+#define LPC_LPA_START_OFF	0x418
+
+/* LPC as WDT */
+#define LPC_WDT_OFF		0x510
+#define LPC_WDT_FLAG_OFF	0x514
+
+struct st_rtc {
+	struct rtc_device *rtc_dev;
+	struct rtc_wkalrm alarm;
+	struct resource *res;
+	struct clk *clk;
+	unsigned long clkrate;
+	void __iomem *ioaddr;
+	bool irq_enabled:1;
+	spinlock_t lock;
+	short irq;
+};
+
+static void st_rtc_set_hw_alarm(struct st_rtc *rtc,
+				unsigned long msb, unsigned long  lsb)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&rtc->lock, flags);
+
+	writel_relaxed(1, rtc->ioaddr + LPC_WDT_OFF);
+
+	writel_relaxed(msb, rtc->ioaddr + LPC_LPA_MSB_OFF);
+	writel_relaxed(lsb, rtc->ioaddr + LPC_LPA_LSB_OFF);
+	writel_relaxed(1, rtc->ioaddr + LPC_LPA_START_OFF);
+
+	writel_relaxed(0, rtc->ioaddr + LPC_WDT_OFF);
+
+	spin_unlock_irqrestore(&rtc->lock, flags);
+}
+
+static irqreturn_t st_rtc_handler(int this_irq, void *data)
+{
+	struct st_rtc *rtc = (struct st_rtc *)data;
+
+	rtc_update_irq(rtc->rtc_dev, 1, RTC_AF);
+
+	return IRQ_HANDLED;
+}
+
+static int st_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct st_rtc *rtc = dev_get_drvdata(dev);
+	unsigned long lpt_lsb, lpt_msb;
+	unsigned long long lpt;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rtc->lock, flags);
+
+	do {
+		lpt_msb = readl_relaxed(rtc->ioaddr + LPC_LPT_MSB_OFF);
+		lpt_lsb = readl_relaxed(rtc->ioaddr + LPC_LPT_LSB_OFF);
+	} while (readl_relaxed(rtc->ioaddr + LPC_LPT_MSB_OFF) != lpt_msb);
+
+	spin_unlock_irqrestore(&rtc->lock, flags);
+
+	lpt = ((unsigned long long)lpt_msb << 32) | lpt_lsb;
+	do_div(lpt, rtc->clkrate);
+	rtc_time_to_tm(lpt, tm);
+
+	return 0;
+}
+
+static int st_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct st_rtc *rtc = dev_get_drvdata(dev);
+	unsigned long long lpt;
+	unsigned long secs, flags;
+	int ret;
+
+	ret = rtc_tm_to_time(tm, &secs);
+	if (ret)
+		return ret;
+
+	lpt = (unsigned long long)secs * rtc->clkrate;
+
+	spin_lock_irqsave(&rtc->lock, flags);
+
+	writel_relaxed(lpt >> 32, rtc->ioaddr + LPC_LPT_MSB_OFF);
+	writel_relaxed(lpt, rtc->ioaddr + LPC_LPT_LSB_OFF);
+	writel_relaxed(1, rtc->ioaddr + LPC_LPT_START_OFF);
+
+	spin_unlock_irqrestore(&rtc->lock, flags);
+
+	return 0;
+}
+
+static int st_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *wkalrm)
+{
+	struct st_rtc *rtc = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&rtc->lock, flags);
+
+	memcpy(wkalrm, &rtc->alarm, sizeof(struct rtc_wkalrm));
+
+	spin_unlock_irqrestore(&rtc->lock, flags);
+
+	return 0;
+}
+
+static int st_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
+{
+	struct st_rtc *rtc = dev_get_drvdata(dev);
+
+	if (enabled && !rtc->irq_enabled) {
+		enable_irq(rtc->irq);
+		rtc->irq_enabled = true;
+	} else if (!enabled && rtc->irq_enabled) {
+		disable_irq(rtc->irq);
+		rtc->irq_enabled = false;
+	}
+
+	return 0;
+}
+
+static int st_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *t)
+{
+	struct st_rtc *rtc = dev_get_drvdata(dev);
+	struct rtc_time now;
+	unsigned long now_secs;
+	unsigned long alarm_secs;
+	unsigned long long lpa;
+
+	st_rtc_read_time(dev, &now);
+	rtc_tm_to_time(&now, &now_secs);
+	rtc_tm_to_time(&t->time, &alarm_secs);
+
+	/* Invalid alarm time */
+	if (now_secs > alarm_secs)
+		return -EINVAL;
+
+	memcpy(&rtc->alarm, t, sizeof(struct rtc_wkalrm));
+
+	/* Now many secs to fire */
+	alarm_secs -= now_secs;
+	lpa = (unsigned long long)alarm_secs * rtc->clkrate;
+
+	st_rtc_set_hw_alarm(rtc, lpa >> 32, lpa);
+	st_rtc_alarm_irq_enable(dev, t->enabled);
+
+	return 0;
+}
+
+static struct rtc_class_ops st_rtc_ops = {
+	.read_time		= st_rtc_read_time,
+	.set_time		= st_rtc_set_time,
+	.read_alarm		= st_rtc_read_alarm,
+	.set_alarm		= st_rtc_set_alarm,
+	.alarm_irq_enable	= st_rtc_alarm_irq_enable,
+};
+
+static int st_rtc_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct st_rtc *rtc;
+	struct resource *res;
+	struct rtc_time tm_check;
+	uint32_t mode;
+	int ret = 0;
+
+	ret = of_property_read_u32(np, "st,lpc-mode", &mode);
+	if (ret) {
+		dev_err(&pdev->dev, "An LPC mode must be provided\n");
+		return -EINVAL;
+	}
+
+	/* LPC can either run in RTC or WDT mode */
+	if (mode != ST_LPC_MODE_RTC)
+		return -ENODEV;
+
+	rtc = devm_kzalloc(&pdev->dev, sizeof(struct st_rtc), GFP_KERNEL);
+	if (!rtc)
+		return -ENOMEM;
+
+	spin_lock_init(&rtc->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	rtc->ioaddr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(rtc->ioaddr))
+		return PTR_ERR(rtc->ioaddr);
+
+	rtc->irq = irq_of_parse_and_map(np, 0);
+	if (!rtc->irq) {
+		dev_err(&pdev->dev, "IRQ missing or invalid\n");
+		return -EINVAL;
+	}
+
+	ret = devm_request_irq(&pdev->dev, rtc->irq, st_rtc_handler, 0,
+			       pdev->name, rtc);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request irq %i\n", rtc->irq);
+		return ret;
+	}
+
+	enable_irq_wake(rtc->irq);
+	disable_irq(rtc->irq);
+
+	rtc->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(rtc->clk)) {
+		dev_err(&pdev->dev, "Unable to request clock\n");
+		return PTR_ERR(rtc->clk);
+	}
+
+	clk_prepare_enable(rtc->clk);
+
+	rtc->clkrate = clk_get_rate(rtc->clk);
+	if (!rtc->clkrate) {
+		dev_err(&pdev->dev, "Unable to fetch clock rate\n");
+		return -EINVAL;
+	}
+
+	device_set_wakeup_capable(&pdev->dev, 1);
+
+	platform_set_drvdata(pdev, rtc);
+
+	/*
+	 * The RTC-LPC is able to manage date.year > 2038
+	 * but currently the kernel can not manage this date!
+	 * If the RTC-LPC has a date.year > 2038 then
+	 * it's set to the epoch "Jan 1st 2000"
+	 */
+	st_rtc_read_time(&pdev->dev, &tm_check);
+
+	if (tm_check.tm_year >=  (2038 - 1900)) {
+		memset(&tm_check, 0, sizeof(tm_check));
+		tm_check.tm_year = 100;
+		tm_check.tm_mday = 1;
+		st_rtc_set_time(&pdev->dev, &tm_check);
+	}
+
+	rtc->rtc_dev = rtc_device_register("st-lpc-rtc", &pdev->dev,
+					   &st_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc->rtc_dev)) {
+		clk_disable_unprepare(rtc->clk);
+		return PTR_ERR(rtc->rtc_dev);
+	}
+
+	return 0;
+}
+
+static int st_rtc_remove(struct platform_device *pdev)
+{
+	struct st_rtc *rtc = platform_get_drvdata(pdev);
+
+	if (likely(rtc->rtc_dev))
+		rtc_device_unregister(rtc->rtc_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int st_rtc_suspend(struct device *dev)
+{
+	struct st_rtc *rtc = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		return 0;
+
+	writel_relaxed(1, rtc->ioaddr + LPC_WDT_OFF);
+	writel_relaxed(0, rtc->ioaddr + LPC_LPA_START_OFF);
+	writel_relaxed(0, rtc->ioaddr + LPC_WDT_OFF);
+
+	return 0;
+}
+
+static int st_rtc_resume(struct device *dev)
+{
+	struct st_rtc *rtc = dev_get_drvdata(dev);
+
+	rtc_alarm_irq_enable(rtc->rtc_dev, 0);
+
+	/*
+	 * clean 'rtc->alarm' to allow a new
+	 * .set_alarm to the upper RTC layer
+	 */
+	memset(&rtc->alarm, 0, sizeof(struct rtc_wkalrm));
+
+	writel_relaxed(0, rtc->ioaddr + LPC_LPA_MSB_OFF);
+	writel_relaxed(0, rtc->ioaddr + LPC_LPA_LSB_OFF);
+	writel_relaxed(1, rtc->ioaddr + LPC_WDT_OFF);
+	writel_relaxed(1, rtc->ioaddr + LPC_LPA_START_OFF);
+	writel_relaxed(0, rtc->ioaddr + LPC_WDT_OFF);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(st_rtc_pm_ops, st_rtc_suspend, st_rtc_resume);
+
+static const struct of_device_id st_rtc_match[] = {
+	{ .compatible = "st,stih407-lpc" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, st_rtc_match);
+
+static struct platform_driver st_rtc_platform_driver = {
+	.driver = {
+		.name = "st-lpc-rtc",
+		.pm = &st_rtc_pm_ops,
+		.of_match_table = st_rtc_match,
+	},
+	.probe = st_rtc_probe,
+	.remove = st_rtc_remove,
+};
+
+module_platform_driver(st_rtc_platform_driver);
+
+MODULE_DESCRIPTION("STMicroelectronics LPC RTC driver");
+MODULE_AUTHOR("David Paris <david.paris@st.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-sunxi.c b/drivers/rtc/rtc-sunxi.c
index 6e678fa4dfaf..52543ae37c98 100644
--- a/drivers/rtc/rtc-sunxi.c
+++ b/drivers/rtc/rtc-sunxi.c
@@ -269,14 +269,13 @@ static int sunxi_rtc_setalarm(struct device *dev, struct rtc_wkalrm *wkalrm)
 	struct sunxi_rtc_dev *chip = dev_get_drvdata(dev);
 	struct rtc_time *alrm_tm = &wkalrm->time;
 	struct rtc_time tm_now;
-	u32 alrm = 0;
-	unsigned long time_now = 0;
-	unsigned long time_set = 0;
-	unsigned long time_gap = 0;
-	unsigned long time_gap_day = 0;
-	unsigned long time_gap_hour = 0;
-	unsigned long time_gap_min = 0;
-	int ret = 0;
+	u32 alrm;
+	time64_t diff;
+	unsigned long time_gap;
+	unsigned long time_gap_day;
+	unsigned long time_gap_hour;
+	unsigned long time_gap_min;
+	int ret;
 
 	ret = sunxi_rtc_gettime(dev, &tm_now);
 	if (ret < 0) {
@@ -284,14 +283,18 @@ static int sunxi_rtc_setalarm(struct device *dev, struct rtc_wkalrm *wkalrm)
 		return -EINVAL;
 	}
 
-	rtc_tm_to_time(alrm_tm, &time_set);
-	rtc_tm_to_time(&tm_now, &time_now);
-	if (time_set <= time_now) {
+	diff = rtc_tm_sub(alrm_tm, &tm_now);
+	if (diff <= 0) {
 		dev_err(dev, "Date to set in the past\n");
 		return -EINVAL;
 	}
 
-	time_gap = time_set - time_now;
+	if (diff > 255 * SEC_IN_DAY) {
+		dev_err(dev, "Day must be in the range 0 - 255\n");
+		return -EINVAL;
+	}
+
+	time_gap = diff;
 	time_gap_day = time_gap / SEC_IN_DAY;
 	time_gap -= time_gap_day * SEC_IN_DAY;
 	time_gap_hour = time_gap / SEC_IN_HOUR;
@@ -299,11 +302,6 @@ static int sunxi_rtc_setalarm(struct device *dev, struct rtc_wkalrm *wkalrm)
 	time_gap_min = time_gap / SEC_IN_MIN;
 	time_gap -= time_gap_min * SEC_IN_MIN;
 
-	if (time_gap_day > 255) {
-		dev_err(dev, "Day must be in the range 0 - 255\n");
-		return -EINVAL;
-	}
-
 	sunxi_rtc_setaie(0, chip);
 	writel(0, chip->base + SUNXI_ALRM_DHMS);
 	usleep_range(100, 300);
diff --git a/drivers/rtc/rtc-v3020.c b/drivers/rtc/rtc-v3020.c
index bfbfa7ed7bbf..f9f97098c254 100644
--- a/drivers/rtc/rtc-v3020.c
+++ b/drivers/rtc/rtc-v3020.c
@@ -49,18 +49,13 @@ struct v3020_chip_ops {
 #define V3020_RD	2
 #define V3020_IO	3
 
-struct v3020_gpio {
-	const char *name;
-	unsigned int gpio;
-};
-
 struct v3020 {
 	/* MMIO access */
 	void __iomem *ioaddress;
 	int leftshift;
 
 	/* GPIO access */
-	struct v3020_gpio *gpio;
+	struct gpio *gpio;
 
 	struct v3020_chip_ops *ops;
 
@@ -107,48 +102,34 @@ static struct v3020_chip_ops v3020_mmio_ops = {
 	.write_bit	= v3020_mmio_write_bit,
 };
 
-static struct v3020_gpio v3020_gpio[] = {
-	{ "RTC CS", 0 },
-	{ "RTC WR", 0 },
-	{ "RTC RD", 0 },
-	{ "RTC IO", 0 },
+static struct gpio v3020_gpio[] = {
+	{ 0, GPIOF_OUT_INIT_HIGH, "RTC CS"},
+	{ 0, GPIOF_OUT_INIT_HIGH, "RTC WR"},
+	{ 0, GPIOF_OUT_INIT_HIGH, "RTC RD"},
+	{ 0, GPIOF_OUT_INIT_HIGH, "RTC IO"},
 };
 
 static int v3020_gpio_map(struct v3020 *chip, struct platform_device *pdev,
 			  struct v3020_platform_data *pdata)
 {
-	int i, err;
+	int err;
 
 	v3020_gpio[V3020_CS].gpio = pdata->gpio_cs;
 	v3020_gpio[V3020_WR].gpio = pdata->gpio_wr;
 	v3020_gpio[V3020_RD].gpio = pdata->gpio_rd;
 	v3020_gpio[V3020_IO].gpio = pdata->gpio_io;
 
-	for (i = 0; i < ARRAY_SIZE(v3020_gpio); i++) {
-		err = gpio_request(v3020_gpio[i].gpio, v3020_gpio[i].name);
-		if (err)
-			goto err_request;
+	err = gpio_request_array(v3020_gpio, ARRAY_SIZE(v3020_gpio));
 
-		gpio_direction_output(v3020_gpio[i].gpio, 1);
-	}
-
-	chip->gpio = v3020_gpio;
-
-	return 0;
-
-err_request:
-	while (--i >= 0)
-		gpio_free(v3020_gpio[i].gpio);
+	if (!err)
+		chip->gpio = v3020_gpio;
 
 	return err;
 }
 
 static void v3020_gpio_unmap(struct v3020 *chip)
 {
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(v3020_gpio); i++)
-		gpio_free(v3020_gpio[i].gpio);
+	gpio_free_array(v3020_gpio, ARRAY_SIZE(v3020_gpio));
 }
 
 static void v3020_gpio_write_bit(struct v3020 *chip, unsigned char bit)
diff --git a/drivers/rtc/systohc.c b/drivers/rtc/systohc.c
index 7728d5e32bf4..b4a68ffcd06b 100644
--- a/drivers/rtc/systohc.c
+++ b/drivers/rtc/systohc.c
@@ -31,7 +31,7 @@ int rtc_set_ntp_time(struct timespec64 now)
 	else
 		rtc_time64_to_tm(now.tv_sec + 1, &tm);
 
-	rtc = rtc_class_open(CONFIG_RTC_HCTOSYS_DEVICE);
+	rtc = rtc_class_open(CONFIG_RTC_SYSTOHC_DEVICE);
 	if (rtc) {
 		/* rtc_hctosys exclusively uses UTC, so we call set_time here,
 		 * not set_mmss. */
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index 57fd66357b95..1aec8ff0b587 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -38,6 +38,8 @@
  */
 #define DASD_CHANQ_MAX_SIZE 4
 
+#define DASD_DIAG_MOD		"dasd_diag_mod"
+
 /*
  * SECTION: exported variables of dasd.c
  */
@@ -3300,6 +3302,21 @@ int dasd_generic_set_online(struct ccw_device *cdev,
 	discipline = base_discipline;
 	if (device->features & DASD_FEATURE_USEDIAG) {
 	  	if (!dasd_diag_discipline_pointer) {
+			/* Try to load the required module. */
+			rc = request_module(DASD_DIAG_MOD);
+			if (rc) {
+				pr_warn("%s Setting the DASD online failed "
+					"because the required module %s "
+					"could not be loaded (rc=%d)\n",
+					dev_name(&cdev->dev), DASD_DIAG_MOD,
+					rc);
+				dasd_delete_device(device);
+				return -ENODEV;
+			}
+		}
+		/* Module init could have failed, so check again here after
+		 * request_module(). */
+		if (!dasd_diag_discipline_pointer) {
 			pr_warn("%s Setting the DASD online failed because of missing DIAG discipline\n",
 				dev_name(&cdev->dev));
 			dasd_delete_device(device);
diff --git a/drivers/s390/block/dasd_genhd.c b/drivers/s390/block/dasd_genhd.c
index 90f39f79f5d7..ef1d9fb06cab 100644
--- a/drivers/s390/block/dasd_genhd.c
+++ b/drivers/s390/block/dasd_genhd.c
@@ -99,9 +99,8 @@ void dasd_gendisk_free(struct dasd_block *block)
 int dasd_scan_partitions(struct dasd_block *block)
 {
 	struct block_device *bdev;
-	int retry, rc;
+	int rc;
 
-	retry = 5;
 	bdev = bdget_disk(block->gdp, 0);
 	if (!bdev) {
 		DBF_DEV_EVENT(DBF_ERR, block->base, "%s",
@@ -116,19 +115,11 @@ int dasd_scan_partitions(struct dasd_block *block)
 			      rc);
 		return -ENODEV;
 	}
-	/*
-	 * See fs/partition/check.c:register_disk,rescan_partitions
-	 * Can't call rescan_partitions directly. Use ioctl.
-	 */
-	rc = ioctl_by_bdev(bdev, BLKRRPART, 0);
-	while (rc == -EBUSY && retry > 0) {
-		schedule();
-		rc = ioctl_by_bdev(bdev, BLKRRPART, 0);
-		retry--;
+
+	rc = blkdev_reread_part(bdev);
+	if (rc)
 		DBF_DEV_EVENT(DBF_ERR, block->base,
-			      "scan partitions error, retry %d rc %d",
-			      retry, rc);
-	}
+				"scan partitions error, rc %d", rc);
 
 	/*
 	 * Since the matching blkdev_put call to the blkdev_get in
diff --git a/drivers/s390/char/keyboard.c b/drivers/s390/char/keyboard.c
index 01463b052ae7..ef04a9f7a704 100644
--- a/drivers/s390/char/keyboard.c
+++ b/drivers/s390/char/keyboard.c
@@ -433,20 +433,23 @@ do_kdgkb_ioctl(struct kbd_data *kbd, struct kbsentry __user *u_kbs,
 	case KDSKBSENT:
 		if (!perm)
 			return -EPERM;
-		len = strnlen_user(u_kbs->kb_string,
-				   sizeof(u_kbs->kb_string) - 1);
+		len = strnlen_user(u_kbs->kb_string, sizeof(u_kbs->kb_string));
 		if (!len)
 			return -EFAULT;
-		if (len > sizeof(u_kbs->kb_string) - 1)
+		if (len > sizeof(u_kbs->kb_string))
 			return -EINVAL;
-		p = kmalloc(len + 1, GFP_KERNEL);
+		p = kmalloc(len, GFP_KERNEL);
 		if (!p)
 			return -ENOMEM;
 		if (copy_from_user(p, u_kbs->kb_string, len)) {
 			kfree(p);
 			return -EFAULT;
 		}
-		p[len] = 0;
+		/*
+		 * Make sure the string is terminated by 0. User could have
+		 * modified it between us running strnlen_user() and copying it.
+		 */
+		p[len - 1] = 0;
 		kfree(kbd->func_table[kb_func]);
 		kbd->func_table[kb_func] = p;
 		break;
diff --git a/drivers/s390/char/sclp.c b/drivers/s390/char/sclp.c
index 41ba56d2e752..5e20513c0587 100644
--- a/drivers/s390/char/sclp.c
+++ b/drivers/s390/char/sclp.c
@@ -665,7 +665,7 @@ sclp_state_change_cb(struct evbuf_header *evbuf)
 		sclp_send_mask = scbuf->sclp_send_mask;
 	spin_unlock_irqrestore(&sclp_lock, flags);
 	if (scbuf->validity_sclp_active_facility_mask)
-		sclp_facilities = scbuf->sclp_active_facility_mask;
+		sclp.facilities = scbuf->sclp_active_facility_mask;
 	sclp_dispatch_state_change();
 }
 
diff --git a/drivers/s390/char/sclp.h b/drivers/s390/char/sclp.h
index a88069f8c677..026e38990952 100644
--- a/drivers/s390/char/sclp.h
+++ b/drivers/s390/char/sclp.h
@@ -100,13 +100,11 @@ struct init_sccb {
 	sccb_mask_t sclp_send_mask;
 } __attribute__((packed));
 
-extern u64 sclp_facilities;
-
-#define SCLP_HAS_CHP_INFO	(sclp_facilities & 0x8000000000000000ULL)
-#define SCLP_HAS_CHP_RECONFIG	(sclp_facilities & 0x2000000000000000ULL)
-#define SCLP_HAS_CPU_INFO	(sclp_facilities & 0x0800000000000000ULL)
-#define SCLP_HAS_CPU_RECONFIG	(sclp_facilities & 0x0400000000000000ULL)
-#define SCLP_HAS_PCI_RECONFIG	(sclp_facilities & 0x0000000040000000ULL)
+#define SCLP_HAS_CHP_INFO	(sclp.facilities & 0x8000000000000000ULL)
+#define SCLP_HAS_CHP_RECONFIG	(sclp.facilities & 0x2000000000000000ULL)
+#define SCLP_HAS_CPU_INFO	(sclp.facilities & 0x0800000000000000ULL)
+#define SCLP_HAS_CPU_RECONFIG	(sclp.facilities & 0x0400000000000000ULL)
+#define SCLP_HAS_PCI_RECONFIG	(sclp.facilities & 0x0000000040000000ULL)
 
 
 struct gds_subvector {
@@ -191,9 +189,6 @@ void sclp_sdias_exit(void);
 extern int sclp_console_pages;
 extern int sclp_console_drop;
 extern unsigned long sclp_console_full;
-extern u8 sclp_fac84;
-extern unsigned long long sclp_rzm;
-extern unsigned long long sclp_rnmax;
 
 /* useful inlines */
 
diff --git a/drivers/s390/char/sclp_cmd.c b/drivers/s390/char/sclp_cmd.c
index 7be782116dab..e9485fbbb373 100644
--- a/drivers/s390/char/sclp_cmd.c
+++ b/drivers/s390/char/sclp_cmd.c
@@ -92,8 +92,8 @@ struct read_cpu_info_sccb {
 	u8	reserved[4096 - 16];
 } __attribute__((packed, aligned(PAGE_SIZE)));
 
-static void sclp_fill_cpu_info(struct sclp_cpu_info *info,
-			       struct read_cpu_info_sccb *sccb)
+static void sclp_fill_core_info(struct sclp_core_info *info,
+				struct read_cpu_info_sccb *sccb)
 {
 	char *page = (char *) sccb;
 
@@ -101,12 +101,11 @@ static void sclp_fill_cpu_info(struct sclp_cpu_info *info,
 	info->configured = sccb->nr_configured;
 	info->standby = sccb->nr_standby;
 	info->combined = sccb->nr_configured + sccb->nr_standby;
-	info->has_cpu_type = sclp_fac84 & 0x1;
-	memcpy(&info->cpu, page + sccb->offset_configured,
-	       info->combined * sizeof(struct sclp_cpu_entry));
+	memcpy(&info->core, page + sccb->offset_configured,
+	       info->combined * sizeof(struct sclp_core_entry));
 }
 
-int sclp_get_cpu_info(struct sclp_cpu_info *info)
+int sclp_get_core_info(struct sclp_core_info *info)
 {
 	int rc;
 	struct read_cpu_info_sccb *sccb;
@@ -127,7 +126,7 @@ int sclp_get_cpu_info(struct sclp_cpu_info *info)
 		rc = -EIO;
 		goto out;
 	}
-	sclp_fill_cpu_info(info, sccb);
+	sclp_fill_core_info(info, sccb);
 out:
 	free_page((unsigned long) sccb);
 	return rc;
@@ -137,7 +136,7 @@ struct cpu_configure_sccb {
 	struct sccb_header header;
 } __attribute__((packed, aligned(8)));
 
-static int do_cpu_configure(sclp_cmdw_t cmd)
+static int do_core_configure(sclp_cmdw_t cmd)
 {
 	struct cpu_configure_sccb *sccb;
 	int rc;
@@ -171,14 +170,14 @@ out:
 	return rc;
 }
 
-int sclp_cpu_configure(u8 cpu)
+int sclp_core_configure(u8 core)
 {
-	return do_cpu_configure(SCLP_CMDW_CONFIGURE_CPU | cpu << 8);
+	return do_core_configure(SCLP_CMDW_CONFIGURE_CPU | core << 8);
 }
 
-int sclp_cpu_deconfigure(u8 cpu)
+int sclp_core_deconfigure(u8 core)
 {
-	return do_cpu_configure(SCLP_CMDW_DECONFIGURE_CPU | cpu << 8);
+	return do_core_configure(SCLP_CMDW_DECONFIGURE_CPU | core << 8);
 }
 
 #ifdef CONFIG_MEMORY_HOTPLUG
@@ -186,7 +185,7 @@ int sclp_cpu_deconfigure(u8 cpu)
 static DEFINE_MUTEX(sclp_mem_mutex);
 static LIST_HEAD(sclp_mem_list);
 static u8 sclp_max_storage_id;
-static unsigned long sclp_storage_ids[256 / BITS_PER_LONG];
+static DECLARE_BITMAP(sclp_storage_ids, 256);
 static int sclp_mem_state_changed;
 
 struct memory_increment {
@@ -202,14 +201,14 @@ struct assign_storage_sccb {
 
 int arch_get_memory_phys_device(unsigned long start_pfn)
 {
-	if (!sclp_rzm)
+	if (!sclp.rzm)
 		return 0;
-	return PFN_PHYS(start_pfn) >> ilog2(sclp_rzm);
+	return PFN_PHYS(start_pfn) >> ilog2(sclp.rzm);
 }
 
 static unsigned long long rn2addr(u16 rn)
 {
-	return (unsigned long long) (rn - 1) * sclp_rzm;
+	return (unsigned long long) (rn - 1) * sclp.rzm;
 }
 
 static int do_assign_storage(sclp_cmdw_t cmd, u16 rn)
@@ -250,7 +249,7 @@ static int sclp_assign_storage(u16 rn)
 	if (rc)
 		return rc;
 	start = rn2addr(rn);
-	storage_key_init_range(start, start + sclp_rzm);
+	storage_key_init_range(start, start + sclp.rzm);
 	return 0;
 }
 
@@ -309,7 +308,7 @@ static int sclp_mem_change_state(unsigned long start, unsigned long size,
 		istart = rn2addr(incr->rn);
 		if (start + size - 1 < istart)
 			break;
-		if (start > istart + sclp_rzm - 1)
+		if (start > istart + sclp.rzm - 1)
 			continue;
 		if (online)
 			rc |= sclp_assign_storage(incr->rn);
@@ -330,7 +329,7 @@ static bool contains_standby_increment(unsigned long start, unsigned long end)
 		istart = rn2addr(incr->rn);
 		if (end - 1 < istart)
 			continue;
-		if (start > istart + sclp_rzm - 1)
+		if (start > istart + sclp.rzm - 1)
 			continue;
 		if (incr->standby)
 			return true;
@@ -415,7 +414,7 @@ static void __init add_memory_merged(u16 rn)
 	if (!first_rn)
 		goto skip_add;
 	start = rn2addr(first_rn);
-	size = (unsigned long long) num * sclp_rzm;
+	size = (unsigned long long) num * sclp.rzm;
 	if (start >= VMEM_MAX_PHYS)
 		goto skip_add;
 	if (start + size > VMEM_MAX_PHYS)
@@ -465,7 +464,7 @@ static void __init insert_increment(u16 rn, int standby, int assigned)
 	}
 	if (!assigned)
 		new_incr->rn = last_rn + 1;
-	if (new_incr->rn > sclp_rnmax) {
+	if (new_incr->rn > sclp.rnmax) {
 		kfree(new_incr);
 		return;
 	}
@@ -508,7 +507,7 @@ static int __init sclp_detect_standby_memory(void)
 
 	if (OLDMEM_BASE) /* No standby memory in kdump mode */
 		return 0;
-	if ((sclp_facilities & 0xe00000000000ULL) != 0xe00000000000ULL)
+	if ((sclp.facilities & 0xe00000000000ULL) != 0xe00000000000ULL)
 		return 0;
 	rc = -ENOMEM;
 	sccb = (void *) __get_free_page(GFP_KERNEL | GFP_DMA);
@@ -550,7 +549,7 @@ static int __init sclp_detect_standby_memory(void)
 	}
 	if (rc || list_empty(&sclp_mem_list))
 		goto out;
-	for (i = 1; i <= sclp_rnmax - assigned; i++)
+	for (i = 1; i <= sclp.rnmax - assigned; i++)
 		insert_increment(0, 1, 0);
 	rc = register_memory_notifier(&sclp_mem_nb);
 	if (rc)
@@ -753,8 +752,3 @@ out:
 	free_page((unsigned long) sccb);
 	return rc;
 }
-
-bool sclp_has_sprp(void)
-{
-	return !!(sclp_fac84 & 0x2);
-}
diff --git a/drivers/s390/char/sclp_early.c b/drivers/s390/char/sclp_early.c
index 1efa4fdb7fe2..aeed7969fd79 100644
--- a/drivers/s390/char/sclp_early.c
+++ b/drivers/s390/char/sclp_early.c
@@ -48,23 +48,10 @@ struct read_info_sccb {
 } __packed __aligned(PAGE_SIZE);
 
 static char sccb_early[PAGE_SIZE] __aligned(PAGE_SIZE) __initdata;
-static unsigned int sclp_con_has_vt220 __initdata;
-static unsigned int sclp_con_has_linemode __initdata;
-static unsigned long sclp_hsa_size;
-static unsigned int sclp_max_cpu;
 static struct sclp_ipl_info sclp_ipl_info;
-static unsigned char sclp_siif;
-static unsigned char sclp_sigpif;
-static u32 sclp_ibc;
-static unsigned int sclp_mtid;
-static unsigned int sclp_mtid_cp;
-static unsigned int sclp_mtid_max;
-static unsigned int sclp_mtid_prev;
-
-u64 sclp_facilities;
-u8 sclp_fac84;
-unsigned long long sclp_rzm;
-unsigned long long sclp_rnmax;
+
+struct sclp_info sclp;
+EXPORT_SYMBOL(sclp);
 
 static int __init sclp_cmd_sync_early(sclp_cmdw_t cmd, void *sccb)
 {
@@ -111,28 +98,29 @@ static int __init sclp_read_info_early(struct read_info_sccb *sccb)
 
 static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 {
-	struct sclp_cpu_entry *cpue;
+	struct sclp_core_entry *cpue;
 	u16 boot_cpu_address, cpu;
 
 	if (sclp_read_info_early(sccb))
 		return;
 
-	sclp_facilities = sccb->facilities;
-	sclp_fac84 = sccb->fac84;
+	sclp.facilities = sccb->facilities;
+	sclp.has_sprp = !!(sccb->fac84 & 0x02);
+	sclp.has_core_type = !!(sccb->fac84 & 0x01);
 	if (sccb->fac85 & 0x02)
 		S390_lowcore.machine_flags |= MACHINE_FLAG_ESOP;
-	sclp_rnmax = sccb->rnmax ? sccb->rnmax : sccb->rnmax2;
-	sclp_rzm = sccb->rnsize ? sccb->rnsize : sccb->rnsize2;
-	sclp_rzm <<= 20;
-	sclp_ibc = sccb->ibc;
+	sclp.rnmax = sccb->rnmax ? sccb->rnmax : sccb->rnmax2;
+	sclp.rzm = sccb->rnsize ? sccb->rnsize : sccb->rnsize2;
+	sclp.rzm <<= 20;
+	sclp.ibc = sccb->ibc;
 
 	if (!sccb->hcpua) {
 		if (MACHINE_IS_VM)
-			sclp_max_cpu = 64;
+			sclp.max_cores = 64;
 		else
-			sclp_max_cpu = sccb->ncpurl;
+			sclp.max_cores = sccb->ncpurl;
 	} else {
-		sclp_max_cpu = sccb->hcpua + 1;
+		sclp.max_cores = sccb->hcpua + 1;
 	}
 
 	boot_cpu_address = stap();
@@ -140,8 +128,8 @@ static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 	for (cpu = 0; cpu < sccb->ncpurl; cpue++, cpu++) {
 		if (boot_cpu_address != cpue->core_id)
 			continue;
-		sclp_siif = cpue->siif;
-		sclp_sigpif = cpue->sigpif;
+		sclp.has_siif = cpue->siif;
+		sclp.has_sigpif = cpue->sigpif;
 		break;
 	}
 
@@ -151,68 +139,9 @@ static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 		sclp_ipl_info.has_dump = 1;
 	memcpy(&sclp_ipl_info.loadparm, &sccb->loadparm, LOADPARM_LEN);
 
-	sclp_mtid = (sccb->fac42 & 0x80) ? (sccb->fac42 & 31) : 0;
-	sclp_mtid_cp = (sccb->fac42 & 0x80) ? (sccb->fac43 & 31) : 0;
-	sclp_mtid_max = max(sclp_mtid, sclp_mtid_cp);
-	sclp_mtid_prev = (sccb->fac42 & 0x80) ? (sccb->fac66 & 31) : 0;
-}
-
-bool __init sclp_has_linemode(void)
-{
-	return !!sclp_con_has_linemode;
-}
-
-bool __init sclp_has_vt220(void)
-{
-	return !!sclp_con_has_vt220;
-}
-
-unsigned long long sclp_get_rnmax(void)
-{
-	return sclp_rnmax;
-}
-
-unsigned long long sclp_get_rzm(void)
-{
-	return sclp_rzm;
-}
-
-unsigned int sclp_get_max_cpu(void)
-{
-	return sclp_max_cpu;
-}
-
-int sclp_has_siif(void)
-{
-	return sclp_siif;
-}
-EXPORT_SYMBOL(sclp_has_siif);
-
-int sclp_has_sigpif(void)
-{
-	return sclp_sigpif;
-}
-EXPORT_SYMBOL(sclp_has_sigpif);
-
-unsigned int sclp_get_ibc(void)
-{
-	return sclp_ibc;
-}
-EXPORT_SYMBOL(sclp_get_ibc);
-
-unsigned int sclp_get_mtid(u8 cpu_type)
-{
-	return cpu_type ? sclp_mtid : sclp_mtid_cp;
-}
-
-unsigned int sclp_get_mtid_max(void)
-{
-	return sclp_mtid_max;
-}
-
-unsigned int sclp_get_mtid_prev(void)
-{
-	return sclp_mtid_prev;
+	sclp.mtid = (sccb->fac42 & 0x80) ? (sccb->fac42 & 31) : 0;
+	sclp.mtid_cp = (sccb->fac42 & 0x80) ? (sccb->fac43 & 31) : 0;
+	sclp.mtid_prev = (sccb->fac42 & 0x80) ? (sccb->fac66 & 31) : 0;
 }
 
 /*
@@ -286,11 +215,6 @@ static long __init sclp_hsa_copy_wait(struct sccb_header *sccb)
 	return (((struct sdias_sccb *) sccb)->evbuf.blk_cnt - 1) * PAGE_SIZE;
 }
 
-unsigned long sclp_get_hsa_size(void)
-{
-	return sclp_hsa_size;
-}
-
 static void __init sclp_hsa_size_detect(void *sccb)
 {
 	long size;
@@ -313,7 +237,7 @@ static void __init sclp_hsa_size_detect(void *sccb)
 	if (size < 0)
 		return;
 out:
-	sclp_hsa_size = size;
+	sclp.hsa_size = size;
 }
 
 static unsigned int __init sclp_con_check_linemode(struct init_sccb *sccb)
@@ -331,10 +255,10 @@ static void __init sclp_console_detect(struct init_sccb *sccb)
 		return;
 
 	if (sccb->sclp_send_mask & EVTYP_VT220MSG_MASK)
-		sclp_con_has_vt220 = 1;
+		sclp.has_vt220 = 1;
 
 	if (sclp_con_check_linemode(sccb))
-		sclp_con_has_linemode = 1;
+		sclp.has_linemode = 1;
 }
 
 void __init sclp_early_detect(void)
diff --git a/drivers/s390/char/sclp_sdias.c b/drivers/s390/char/sclp_sdias.c
index eb7cb076c001..7cdd13dd7be1 100644
--- a/drivers/s390/char/sclp_sdias.c
+++ b/drivers/s390/char/sclp_sdias.c
@@ -21,7 +21,6 @@
 #define TRACE(x...) debug_sprintf_event(sdias_dbf, 1, x)
 
 #define SDIAS_RETRIES 300
-#define SDIAS_SLEEP_TICKS 50
 
 static struct debug_info *sdias_dbf;
 
@@ -68,7 +67,7 @@ static int sdias_sclp_send(struct sclp_req *req)
 			/* not initiated, wait some time and retry */
 			set_current_state(TASK_INTERRUPTIBLE);
 			TRACE("add request failed: rc = %i\n",rc);
-			schedule_timeout(SDIAS_SLEEP_TICKS);
+			schedule_timeout(msecs_to_jiffies(500));
 			continue;
 		}
 		/* initiated, wait for completion of service call */
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index a68fcfd1d48c..823f41fc4bbd 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -154,7 +154,7 @@ static int __init init_cpu_info(enum arch_id arch)
 
 	/* get info for boot cpu from lowcore, stored in the HSA */
 
-	sa_ext = dump_save_area_create(0);
+	sa_ext = dump_save_areas.areas[0];
 	if (!sa_ext)
 		return -ENOMEM;
 	if (memcpy_hsa_kernel(&sa_ext->sa, sys_info.sa_base,
@@ -330,9 +330,9 @@ static ssize_t zcore_read(struct file *file, char __user *buf, size_t count,
 	mem_offs = 0;
 
 	/* Copy from HSA data */
-	if (*ppos < sclp_get_hsa_size() + HEADER_SIZE) {
+	if (*ppos < sclp.hsa_size + HEADER_SIZE) {
 		size = min((count - hdr_count),
-			   (size_t) (sclp_get_hsa_size() - mem_start));
+			   (size_t) (sclp.hsa_size - mem_start));
 		rc = memcpy_hsa_user(buf + hdr_count, mem_start, size);
 		if (rc)
 			goto fail;
@@ -483,7 +483,7 @@ static ssize_t zcore_hsa_read(struct file *filp, char __user *buf,
 	static char str[18];
 
 	if (hsa_available)
-		snprintf(str, sizeof(str), "%lx\n", sclp_get_hsa_size());
+		snprintf(str, sizeof(str), "%lx\n", sclp.hsa_size);
 	else
 		snprintf(str, sizeof(str), "0\n");
 	return simple_read_from_buffer(buf, count, ppos, str, strlen(str));
@@ -558,7 +558,7 @@ static int __init sys_info_init(enum arch_id arch, unsigned long mem_end)
 
 static int __init check_sdias(void)
 {
-	if (!sclp_get_hsa_size()) {
+	if (!sclp.hsa_size) {
 		TRACE("Could not determine HSA size\n");
 		return -ENODEV;
 	}
@@ -619,7 +619,7 @@ static int __init zcore_reipl_init(void)
 	ipl_block = (void *) __get_free_page(GFP_KERNEL);
 	if (!ipl_block)
 		return -ENOMEM;
-	if (ipib_info.ipib < sclp_get_hsa_size())
+	if (ipib_info.ipib < sclp.hsa_size)
 		rc = memcpy_hsa_kernel(ipl_block, ipib_info.ipib, PAGE_SIZE);
 	else
 		rc = memcpy_real(ipl_block, (void *) ipib_info.ipib, PAGE_SIZE);
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 3ba611419759..559a9dcdb15d 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -60,7 +60,7 @@ static int __ap_poll_device(struct ap_device *ap_dev, unsigned long *flags);
 static int ap_device_remove(struct device *dev);
 static int ap_device_probe(struct device *dev);
 static void ap_interrupt_handler(struct airq_struct *airq);
-static void ap_reset(struct ap_device *ap_dev);
+static void ap_reset(struct ap_device *ap_dev, unsigned long *flags);
 static void ap_config_timeout(unsigned long ptr);
 static int ap_select_domain(void);
 static void ap_query_configuration(void);
@@ -310,35 +310,26 @@ static inline int __ap_query_configuration(struct ap_config_info *config)
 static int ap_query_functions(ap_qid_t qid, unsigned int *functions)
 {
 	struct ap_queue_status status;
-	int i;
+
 	status = __ap_query_functions(qid, functions);
 
-	for (i = 0; i < AP_MAX_RESET; i++) {
-		if (ap_queue_status_invalid_test(&status))
-			return -ENODEV;
+	if (ap_queue_status_invalid_test(&status))
+		return -ENODEV;
 
-		switch (status.response_code) {
-		case AP_RESPONSE_NORMAL:
-			return 0;
-		case AP_RESPONSE_RESET_IN_PROGRESS:
-		case AP_RESPONSE_BUSY:
-			break;
-		case AP_RESPONSE_Q_NOT_AVAIL:
-		case AP_RESPONSE_DECONFIGURED:
-		case AP_RESPONSE_CHECKSTOPPED:
-		case AP_RESPONSE_INVALID_ADDRESS:
-			return -ENODEV;
-		case AP_RESPONSE_OTHERWISE_CHANGED:
-			break;
-		default:
-			break;
-		}
-		if (i < AP_MAX_RESET - 1) {
-			udelay(5);
-			status = __ap_query_functions(qid, functions);
-		}
+	switch (status.response_code) {
+	case AP_RESPONSE_NORMAL:
+		return 0;
+	case AP_RESPONSE_Q_NOT_AVAIL:
+	case AP_RESPONSE_DECONFIGURED:
+	case AP_RESPONSE_CHECKSTOPPED:
+	case AP_RESPONSE_INVALID_ADDRESS:
+		return -ENODEV;
+	case AP_RESPONSE_RESET_IN_PROGRESS:
+	case AP_RESPONSE_BUSY:
+	case AP_RESPONSE_OTHERWISE_CHANGED:
+	default:
+		return -EBUSY;
 	}
-	return -EBUSY;
 }
 
 /**
@@ -350,47 +341,25 @@ static int ap_query_functions(ap_qid_t qid, unsigned int *functions)
  * on the return value it waits a while and tests the AP queue if interrupts
  * have been switched on using ap_test_queue().
  */
-static int ap_queue_enable_interruption(ap_qid_t qid, void *ind)
+static int ap_queue_enable_interruption(struct ap_device *ap_dev, void *ind)
 {
 	struct ap_queue_status status;
-	int t_depth, t_device_type, rc, i;
 
-	rc = -EBUSY;
-	status = ap_queue_interruption_control(qid, ind);
-
-	for (i = 0; i < AP_MAX_RESET; i++) {
-		switch (status.response_code) {
-		case AP_RESPONSE_NORMAL:
-			if (status.int_enabled)
-				return 0;
-			break;
-		case AP_RESPONSE_RESET_IN_PROGRESS:
-		case AP_RESPONSE_BUSY:
-			if (i < AP_MAX_RESET - 1) {
-				udelay(5);
-				status = ap_queue_interruption_control(qid,
-								       ind);
-				continue;
-			}
-			break;
-		case AP_RESPONSE_Q_NOT_AVAIL:
-		case AP_RESPONSE_DECONFIGURED:
-		case AP_RESPONSE_CHECKSTOPPED:
-		case AP_RESPONSE_INVALID_ADDRESS:
-			return -ENODEV;
-		case AP_RESPONSE_OTHERWISE_CHANGED:
-			if (status.int_enabled)
-				return 0;
-			break;
-		default:
-			break;
-		}
-		if (i < AP_MAX_RESET - 1) {
-			udelay(5);
-			status = ap_test_queue(qid, &t_depth, &t_device_type);
-		}
+	status = ap_queue_interruption_control(ap_dev->qid, ind);
+	switch (status.response_code) {
+	case AP_RESPONSE_NORMAL:
+	case AP_RESPONSE_OTHERWISE_CHANGED:
+		return 0;
+	case AP_RESPONSE_Q_NOT_AVAIL:
+	case AP_RESPONSE_DECONFIGURED:
+	case AP_RESPONSE_CHECKSTOPPED:
+	case AP_RESPONSE_INVALID_ADDRESS:
+		return -ENODEV;
+	case AP_RESPONSE_RESET_IN_PROGRESS:
+	case AP_RESPONSE_BUSY:
+	default:
+		return -EBUSY;
 	}
-	return rc;
 }
 
 /**
@@ -511,109 +480,94 @@ int ap_recv(ap_qid_t qid, unsigned long long *psmid, void *msg, size_t length)
 EXPORT_SYMBOL(ap_recv);
 
 /**
+ * __ap_schedule_poll_timer(): Schedule poll timer.
+ *
+ * Set up the timer to run the poll tasklet
+ */
+static inline void __ap_schedule_poll_timer(void)
+{
+	ktime_t hr_time;
+
+	spin_lock_bh(&ap_poll_timer_lock);
+	if (!hrtimer_is_queued(&ap_poll_timer) && !ap_suspend_flag) {
+		hr_time = ktime_set(0, poll_timeout);
+		hrtimer_forward_now(&ap_poll_timer, hr_time);
+		hrtimer_restart(&ap_poll_timer);
+	}
+	spin_unlock_bh(&ap_poll_timer_lock);
+}
+
+/**
+ * ap_schedule_poll_timer(): Schedule poll timer.
+ *
+ * Set up the timer to run the poll tasklet
+ */
+static inline void ap_schedule_poll_timer(void)
+{
+	if (ap_using_interrupts())
+		return;
+	__ap_schedule_poll_timer();
+}
+
+
+/**
  * ap_query_queue(): Check if an AP queue is available.
  * @qid: The AP queue number
  * @queue_depth: Pointer to queue depth value
  * @device_type: Pointer to device type value
- *
- * The test is repeated for AP_MAX_RESET times.
  */
 static int ap_query_queue(ap_qid_t qid, int *queue_depth, int *device_type)
 {
 	struct ap_queue_status status;
-	int t_depth, t_device_type, rc, i;
+	int t_depth, t_device_type;
 
-	rc = -EBUSY;
-	for (i = 0; i < AP_MAX_RESET; i++) {
-		status = ap_test_queue(qid, &t_depth, &t_device_type);
-		switch (status.response_code) {
-		case AP_RESPONSE_NORMAL:
-			*queue_depth = t_depth + 1;
-			*device_type = t_device_type;
-			rc = 0;
-			break;
-		case AP_RESPONSE_Q_NOT_AVAIL:
-			rc = -ENODEV;
-			break;
-		case AP_RESPONSE_RESET_IN_PROGRESS:
-			break;
-		case AP_RESPONSE_DECONFIGURED:
-			rc = -ENODEV;
-			break;
-		case AP_RESPONSE_CHECKSTOPPED:
-			rc = -ENODEV;
-			break;
-		case AP_RESPONSE_INVALID_ADDRESS:
-			rc = -ENODEV;
-			break;
-		case AP_RESPONSE_OTHERWISE_CHANGED:
-			break;
-		case AP_RESPONSE_BUSY:
-			break;
-		default:
-			BUG();
-		}
-		if (rc != -EBUSY)
-			break;
-		if (i < AP_MAX_RESET - 1)
-			udelay(5);
+	status = ap_test_queue(qid, &t_depth, &t_device_type);
+	switch (status.response_code) {
+	case AP_RESPONSE_NORMAL:
+		*queue_depth = t_depth + 1;
+		*device_type = t_device_type;
+		return 0;
+	case AP_RESPONSE_Q_NOT_AVAIL:
+	case AP_RESPONSE_DECONFIGURED:
+	case AP_RESPONSE_CHECKSTOPPED:
+	case AP_RESPONSE_INVALID_ADDRESS:
+		return -ENODEV;
+	case AP_RESPONSE_RESET_IN_PROGRESS:
+	case AP_RESPONSE_OTHERWISE_CHANGED:
+	case AP_RESPONSE_BUSY:
+		return -EBUSY;
+	default:
+		BUG();
 	}
-	return rc;
 }
 
 /**
  * ap_init_queue(): Reset an AP queue.
  * @qid: The AP queue number
  *
- * Reset an AP queue and wait for it to become available again.
+ * Submit the Reset command to an AP queue.
+ * Since the reset is asynchron set the state to 'RESET_IN_PROGRESS'
+ * and check later via ap_poll_queue() if the reset is done.
  */
-static int ap_init_queue(ap_qid_t qid)
+static int ap_init_queue(struct ap_device *ap_dev)
 {
 	struct ap_queue_status status;
-	int rc, dummy, i;
 
-	rc = -ENODEV;
-	status = ap_reset_queue(qid);
-	for (i = 0; i < AP_MAX_RESET; i++) {
-		switch (status.response_code) {
-		case AP_RESPONSE_NORMAL:
-			if (status.queue_empty)
-				rc = 0;
-			break;
-		case AP_RESPONSE_Q_NOT_AVAIL:
-		case AP_RESPONSE_DECONFIGURED:
-		case AP_RESPONSE_CHECKSTOPPED:
-			i = AP_MAX_RESET;	/* return with -ENODEV */
-			break;
-		case AP_RESPONSE_RESET_IN_PROGRESS:
-			rc = -EBUSY;
-		case AP_RESPONSE_BUSY:
-		default:
-			break;
-		}
-		if (rc != -ENODEV && rc != -EBUSY)
-			break;
-		if (i < AP_MAX_RESET - 1) {
-			/* Time we are waiting until we give up (0.7sec * 90).
-			 * Since the actual request (in progress) will not
-			 * interrupted immediately for the reset command,
-			 * we have to be patient. In worst case we have to
-			 * wait 60sec + reset time (some msec).
-			 */
-			schedule_timeout(AP_RESET_TIMEOUT);
-			status = ap_test_queue(qid, &dummy, &dummy);
-		}
-	}
-	if (rc == 0 && ap_using_interrupts()) {
-		rc = ap_queue_enable_interruption(qid, ap_airq.lsi_ptr);
-		/* If interruption mode is supported by the machine,
-		* but an AP can not be enabled for interruption then
-		* the AP will be discarded.    */
-		if (rc)
-			pr_err("Registering adapter interrupts for "
-			       "AP %d failed\n", AP_QID_DEVICE(qid));
+	status = ap_reset_queue(ap_dev->qid);
+	switch (status.response_code) {
+	case AP_RESPONSE_NORMAL:
+		ap_dev->interrupt = AP_INTR_DISABLED;
+		ap_dev->reset = AP_RESET_IN_PROGRESS;
+		return 0;
+	case AP_RESPONSE_RESET_IN_PROGRESS:
+	case AP_RESPONSE_BUSY:
+		return -EBUSY;
+	case AP_RESPONSE_Q_NOT_AVAIL:
+	case AP_RESPONSE_DECONFIGURED:
+	case AP_RESPONSE_CHECKSTOPPED:
+	default:
+		return -ENODEV;
 	}
-	return rc;
 }
 
 /**
@@ -729,10 +683,63 @@ static ssize_t ap_pendingq_count_show(struct device *dev,
 
 static DEVICE_ATTR(pendingq_count, 0444, ap_pendingq_count_show, NULL);
 
+static ssize_t ap_reset_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct ap_device *ap_dev = to_ap_dev(dev);
+	int rc = 0;
+
+	spin_lock_bh(&ap_dev->lock);
+	switch (ap_dev->reset) {
+	case AP_RESET_IGNORE:
+		rc = snprintf(buf, PAGE_SIZE, "No Reset Timer set.\n");
+		break;
+	case AP_RESET_ARMED:
+		rc = snprintf(buf, PAGE_SIZE, "Reset Timer armed.\n");
+		break;
+	case AP_RESET_DO:
+		rc = snprintf(buf, PAGE_SIZE, "Reset Timer expired.\n");
+		break;
+	case AP_RESET_IN_PROGRESS:
+		rc = snprintf(buf, PAGE_SIZE, "Reset in progress.\n");
+		break;
+	default:
+		break;
+	}
+	spin_unlock_bh(&ap_dev->lock);
+	return rc;
+}
+
+static DEVICE_ATTR(reset, 0444, ap_reset_show, NULL);
+
+static ssize_t ap_interrupt_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct ap_device *ap_dev = to_ap_dev(dev);
+	int rc = 0;
+
+	spin_lock_bh(&ap_dev->lock);
+	switch (ap_dev->interrupt) {
+	case AP_INTR_DISABLED:
+		rc = snprintf(buf, PAGE_SIZE, "Interrupts disabled.\n");
+		break;
+	case AP_INTR_ENABLED:
+		rc = snprintf(buf, PAGE_SIZE, "Interrupts enabled.\n");
+		break;
+	case AP_INTR_IN_PROGRESS:
+		rc = snprintf(buf, PAGE_SIZE, "Enable Interrupt pending.\n");
+		break;
+	}
+	spin_unlock_bh(&ap_dev->lock);
+	return rc;
+}
+
+static DEVICE_ATTR(interrupt, 0444, ap_interrupt_show, NULL);
+
 static ssize_t ap_modalias_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
-	return sprintf(buf, "ap:t%02X", to_ap_dev(dev)->device_type);
+	return sprintf(buf, "ap:t%02X\n", to_ap_dev(dev)->device_type);
 }
 
 static DEVICE_ATTR(modalias, 0444, ap_modalias_show, NULL);
@@ -753,6 +760,8 @@ static struct attribute *ap_dev_attrs[] = {
 	&dev_attr_request_count.attr,
 	&dev_attr_requestq_count.attr,
 	&dev_attr_pendingq_count.attr,
+	&dev_attr_reset.attr,
+	&dev_attr_interrupt.attr,
 	&dev_attr_modalias.attr,
 	&dev_attr_ap_functions.attr,
 	NULL
@@ -926,6 +935,10 @@ static int ap_device_probe(struct device *dev)
 		spin_lock_bh(&ap_device_list_lock);
 		list_del_init(&ap_dev->list);
 		spin_unlock_bh(&ap_device_list_lock);
+	} else {
+		if (ap_dev->reset == AP_RESET_IN_PROGRESS ||
+			ap_dev->interrupt == AP_INTR_IN_PROGRESS)
+			__ap_schedule_poll_timer();
 	}
 	return rc;
 }
@@ -1411,7 +1424,7 @@ static void ap_scan_bus(struct work_struct *unused)
 	struct ap_device *ap_dev;
 	struct device *dev;
 	ap_qid_t qid;
-	int queue_depth, device_type;
+	int queue_depth = 0, device_type = 0;
 	unsigned int device_functions;
 	int rc, i;
 
@@ -1429,15 +1442,9 @@ static void ap_scan_bus(struct work_struct *unused)
 		else
 			rc = -ENODEV;
 		if (dev) {
-			if (rc == -EBUSY) {
-				set_current_state(TASK_UNINTERRUPTIBLE);
-				schedule_timeout(AP_RESET_TIMEOUT);
-				rc = ap_query_queue(qid, &queue_depth,
-						    &device_type);
-			}
 			ap_dev = to_ap_dev(dev);
 			spin_lock_bh(&ap_dev->lock);
-			if (rc || ap_dev->unregistered) {
+			if (rc == -ENODEV || ap_dev->unregistered) {
 				spin_unlock_bh(&ap_dev->lock);
 				if (ap_dev->unregistered)
 					i--;
@@ -1451,13 +1458,15 @@ static void ap_scan_bus(struct work_struct *unused)
 		}
 		if (rc)
 			continue;
-		rc = ap_init_queue(qid);
-		if (rc)
-			continue;
 		ap_dev = kzalloc(sizeof(*ap_dev), GFP_KERNEL);
 		if (!ap_dev)
 			break;
 		ap_dev->qid = qid;
+		rc = ap_init_queue(ap_dev);
+		if ((rc != 0) && (rc != -EBUSY)) {
+			kfree(ap_dev);
+			continue;
+		}
 		ap_dev->queue_depth = queue_depth;
 		ap_dev->unregistered = 1;
 		spin_lock_init(&ap_dev->lock);
@@ -1520,36 +1529,6 @@ ap_config_timeout(unsigned long ptr)
 }
 
 /**
- * __ap_schedule_poll_timer(): Schedule poll timer.
- *
- * Set up the timer to run the poll tasklet
- */
-static inline void __ap_schedule_poll_timer(void)
-{
-	ktime_t hr_time;
-
-	spin_lock_bh(&ap_poll_timer_lock);
-	if (!hrtimer_is_queued(&ap_poll_timer) && !ap_suspend_flag) {
-		hr_time = ktime_set(0, poll_timeout);
-		hrtimer_forward_now(&ap_poll_timer, hr_time);
-		hrtimer_restart(&ap_poll_timer);
-	}
-	spin_unlock_bh(&ap_poll_timer_lock);
-}
-
-/**
- * ap_schedule_poll_timer(): Schedule poll timer.
- *
- * Set up the timer to run the poll tasklet
- */
-static inline void ap_schedule_poll_timer(void)
-{
-	if (ap_using_interrupts())
-		return;
-	__ap_schedule_poll_timer();
-}
-
-/**
  * ap_poll_read(): Receive pending reply messages from an AP device.
  * @ap_dev: pointer to the AP device
  * @flags: pointer to control flags, bit 2^0 is set if another poll is
@@ -1568,6 +1547,7 @@ static int ap_poll_read(struct ap_device *ap_dev, unsigned long *flags)
 			   ap_dev->reply->message, ap_dev->reply->length);
 	switch (status.response_code) {
 	case AP_RESPONSE_NORMAL:
+		ap_dev->interrupt = status.int_enabled;
 		atomic_dec(&ap_poll_requests);
 		ap_decrease_queue_count(ap_dev);
 		list_for_each_entry(ap_msg, &ap_dev->pendingq, list) {
@@ -1582,6 +1562,7 @@ static int ap_poll_read(struct ap_device *ap_dev, unsigned long *flags)
 			*flags |= 1;
 		break;
 	case AP_RESPONSE_NO_PENDING_REPLY:
+		ap_dev->interrupt = status.int_enabled;
 		if (status.queue_empty) {
 			/* The card shouldn't forget requests but who knows. */
 			atomic_sub(ap_dev->queue_count, &ap_poll_requests);
@@ -1612,7 +1593,8 @@ static int ap_poll_write(struct ap_device *ap_dev, unsigned long *flags)
 	struct ap_message *ap_msg;
 
 	if (ap_dev->requestq_count <= 0 ||
-	    ap_dev->queue_count >= ap_dev->queue_depth)
+	    (ap_dev->queue_count >= ap_dev->queue_depth) ||
+	    (ap_dev->reset == AP_RESET_IN_PROGRESS))
 		return 0;
 	/* Start the next request on the queue. */
 	ap_msg = list_entry(ap_dev->requestq.next, struct ap_message, list);
@@ -1646,6 +1628,8 @@ static int ap_poll_write(struct ap_device *ap_dev, unsigned long *flags)
 
 /**
  * ap_poll_queue(): Poll AP device for pending replies and send new messages.
+ * Check if the queue has a pending reset. In case it's done re-enable
+ * interrupts, otherwise reschedule the poll_timer for another attempt.
  * @ap_dev: pointer to the bus device
  * @flags: pointer to control flags, bit 2^0 is set if another poll is
  *	   required, bit 2^1 is set if the poll timer needs to get armed
@@ -1656,7 +1640,51 @@ static int ap_poll_write(struct ap_device *ap_dev, unsigned long *flags)
  */
 static inline int ap_poll_queue(struct ap_device *ap_dev, unsigned long *flags)
 {
-	int rc;
+	int rc, depth, type;
+	struct ap_queue_status status;
+
+
+	if (ap_dev->reset == AP_RESET_IN_PROGRESS) {
+		status = ap_test_queue(ap_dev->qid, &depth, &type);
+		switch (status.response_code) {
+		case AP_RESPONSE_NORMAL:
+			ap_dev->reset = AP_RESET_IGNORE;
+			if (ap_using_interrupts()) {
+				rc = ap_queue_enable_interruption(
+					ap_dev, ap_airq.lsi_ptr);
+				if (!rc)
+					ap_dev->interrupt = AP_INTR_IN_PROGRESS;
+				else if (rc == -ENODEV) {
+					pr_err("Registering adapter interrupts for "
+					"AP %d failed\n", AP_QID_DEVICE(ap_dev->qid));
+					return rc;
+				}
+			}
+			/* fall through */
+		case AP_RESPONSE_BUSY:
+		case AP_RESPONSE_RESET_IN_PROGRESS:
+			*flags |= AP_POLL_AFTER_TIMEOUT;
+			break;
+		case AP_RESPONSE_Q_NOT_AVAIL:
+		case AP_RESPONSE_DECONFIGURED:
+		case AP_RESPONSE_CHECKSTOPPED:
+			return -ENODEV;
+		default:
+			break;
+		}
+	}
+
+	if ((ap_dev->reset != AP_RESET_IN_PROGRESS) &&
+		(ap_dev->interrupt == AP_INTR_IN_PROGRESS)) {
+		status = ap_test_queue(ap_dev->qid, &depth, &type);
+		if (ap_using_interrupts()) {
+			if (status.int_enabled == 1)
+				ap_dev->interrupt = AP_INTR_ENABLED;
+			else
+				*flags |= AP_POLL_AFTER_TIMEOUT;
+		} else
+			ap_dev->interrupt = AP_INTR_DISABLED;
+	}
 
 	rc = ap_poll_read(ap_dev, flags);
 	if (rc)
@@ -1676,7 +1704,8 @@ static int __ap_queue_message(struct ap_device *ap_dev, struct ap_message *ap_ms
 	struct ap_queue_status status;
 
 	if (list_empty(&ap_dev->requestq) &&
-	    ap_dev->queue_count < ap_dev->queue_depth) {
+	    (ap_dev->queue_count < ap_dev->queue_depth) &&
+	    (ap_dev->reset != AP_RESET_IN_PROGRESS)) {
 		status = __ap_send(ap_dev->qid, ap_msg->psmid,
 				   ap_msg->message, ap_msg->length,
 				   ap_msg->special);
@@ -1789,21 +1818,20 @@ static enum hrtimer_restart ap_poll_timeout(struct hrtimer *unused)
  * Reset a not responding AP device and move all requests from the
  * pending queue to the request queue.
  */
-static void ap_reset(struct ap_device *ap_dev)
+static void ap_reset(struct ap_device *ap_dev, unsigned long *flags)
 {
 	int rc;
 
-	ap_dev->reset = AP_RESET_IGNORE;
 	atomic_sub(ap_dev->queue_count, &ap_poll_requests);
 	ap_dev->queue_count = 0;
 	list_splice_init(&ap_dev->pendingq, &ap_dev->requestq);
 	ap_dev->requestq_count += ap_dev->pendingq_count;
 	ap_dev->pendingq_count = 0;
-	rc = ap_init_queue(ap_dev->qid);
+	rc = ap_init_queue(ap_dev);
 	if (rc == -ENODEV)
 		ap_dev->unregistered = 1;
 	else
-		__ap_schedule_poll_timer();
+		*flags |= AP_POLL_AFTER_TIMEOUT;
 }
 
 static int __ap_poll_device(struct ap_device *ap_dev, unsigned long *flags)
@@ -1812,7 +1840,7 @@ static int __ap_poll_device(struct ap_device *ap_dev, unsigned long *flags)
 		if (ap_poll_queue(ap_dev, flags))
 			ap_dev->unregistered = 1;
 		if (ap_dev->reset == AP_RESET_DO)
-			ap_reset(ap_dev);
+			ap_reset(ap_dev, flags);
 	}
 	return 0;
 }
@@ -1845,9 +1873,9 @@ static void ap_poll_all(unsigned long dummy)
 			spin_unlock(&ap_dev->lock);
 		}
 		spin_unlock(&ap_device_list_lock);
-	} while (flags & 1);
-	if (flags & 2)
-		ap_schedule_poll_timer();
+	} while (flags & AP_POLL_IMMEDIATELY);
+	if (flags & AP_POLL_AFTER_TIMEOUT)
+		__ap_schedule_poll_timer();
 }
 
 /**
diff --git a/drivers/s390/crypto/ap_bus.h b/drivers/s390/crypto/ap_bus.h
index 2737d261a324..00468c8d0781 100644
--- a/drivers/s390/crypto/ap_bus.h
+++ b/drivers/s390/crypto/ap_bus.h
@@ -32,11 +32,13 @@
 
 #define AP_DEVICES 64		/* Number of AP devices. */
 #define AP_DOMAINS 256		/* Number of AP domains. */
-#define AP_MAX_RESET 90		/* Maximum number of resets. */
 #define AP_RESET_TIMEOUT (HZ*0.7)	/* Time in ticks for reset timeouts. */
 #define AP_CONFIG_TIME 30	/* Time in seconds between AP bus rescans. */
 #define AP_POLL_TIME 1		/* Time in ticks between receive polls. */
 
+#define AP_POLL_IMMEDIATELY	1 /* continue running poll tasklet */
+#define AP_POLL_AFTER_TIMEOUT	2 /* run poll tasklet again after timout */
+
 extern int ap_domain_index;
 
 /**
@@ -135,6 +137,14 @@ static inline int ap_test_bit(unsigned int *ptr, unsigned int nr)
 #define AP_RESET_IGNORE	0	/* request timeout will be ignored */
 #define AP_RESET_ARMED	1	/* request timeout timer is active */
 #define AP_RESET_DO	2	/* AP reset required */
+#define AP_RESET_IN_PROGRESS	3	/* AP reset in progress */
+
+/*
+ * AP interrupt states
+ */
+#define AP_INTR_DISABLED	0	/* AP interrupt disabled */
+#define AP_INTR_ENABLED		1	/* AP interrupt enabled */
+#define AP_INTR_IN_PROGRESS	3	/* AP interrupt in progress */
 
 struct ap_device;
 struct ap_message;
@@ -168,6 +178,7 @@ struct ap_device {
 	struct timer_list timeout;	/* Timer for request timeouts. */
 	int reset;			/* Reset required after req. timeout. */
 
+	int interrupt;			/* indicate if interrupts are enabled */
 	int queue_count;		/* # messages currently on AP queue. */
 
 	struct list_head pendingq;	/* List of message sent to AP queue. */
diff --git a/drivers/s390/crypto/zcrypt_cex4.c b/drivers/s390/crypto/zcrypt_cex4.c
index 71e698b85772..bb3908818505 100644
--- a/drivers/s390/crypto/zcrypt_cex4.c
+++ b/drivers/s390/crypto/zcrypt_cex4.c
@@ -39,7 +39,7 @@
  * But the maximum time limit managed by the stomper code is set to 60sec.
  * Hence we have to wait at least that time period.
  */
-#define CEX4_CLEANUP_TIME	(61*HZ)
+#define CEX4_CLEANUP_TIME	(900*HZ)
 
 static struct ap_device_id zcrypt_cex4_ids[] = {
 	{ AP_DEVICE(AP_DEVICE_TYPE_CEX4)  },
diff --git a/drivers/s390/crypto/zcrypt_pcicc.c b/drivers/s390/crypto/zcrypt_pcicc.c
index 4d14c04b746e..9f18876f058b 100644
--- a/drivers/s390/crypto/zcrypt_pcicc.c
+++ b/drivers/s390/crypto/zcrypt_pcicc.c
@@ -98,11 +98,11 @@ static struct ap_driver zcrypt_pcicc_driver = {
  * - VUD block
  */
 static struct CPRB static_cprb = {
-	.cprb_len	= __constant_cpu_to_le16(0x0070),
+	.cprb_len	= cpu_to_le16(0x0070),
 	.cprb_ver_id	=  0x41,
 	.func_id	= {0x54,0x32},
 	.checkpoint_flag=  0x01,
-	.svr_namel	= __constant_cpu_to_le16(0x0008),
+	.svr_namel	= cpu_to_le16(0x0008),
 	.svr_name	= {'I','C','S','F',' ',' ',' ',' '}
 };
 
@@ -164,7 +164,7 @@ static int ICAMEX_msg_to_type6MEX_msg(struct zcrypt_device *zdev,
 	};
 	static struct function_and_rules_block static_pke_function_and_rules ={
 		.function_code	= {'P','K'},
-		.ulen		= __constant_cpu_to_le16(10),
+		.ulen		= cpu_to_le16(10),
 		.only_rule	= {'P','K','C','S','-','1','.','2'}
 	};
 	struct {
@@ -251,7 +251,7 @@ static int ICACRT_msg_to_type6CRT_msg(struct zcrypt_device *zdev,
 	};
 	static struct function_and_rules_block static_pkd_function_and_rules ={
 		.function_code	= {'P','D'},
-		.ulen		= __constant_cpu_to_le16(10),
+		.ulen		= cpu_to_le16(10),
 		.only_rule	= {'P','K','C','S','-','1','.','2'}
 	};
 	struct {
diff --git a/drivers/s390/kvm/kvm_virtio.c b/drivers/s390/kvm/kvm_virtio.c
index dd65c8b4c7fe..53fb975c404b 100644
--- a/drivers/s390/kvm/kvm_virtio.c
+++ b/drivers/s390/kvm/kvm_virtio.c
@@ -450,7 +450,7 @@ static int __init test_devices_support(unsigned long addr)
 static int __init kvm_devices_init(void)
 {
 	int rc;
-	unsigned long total_memory_size = sclp_get_rzm() * sclp_get_rnmax();
+	unsigned long total_memory_size = sclp.rzm * sclp.rnmax;
 
 	if (!MACHINE_IS_KVM)
 		return -ENODEV;
@@ -497,7 +497,7 @@ static __init int early_put_chars(u32 vtermno, const char *buf, int count)
 
 static int __init s390_virtio_console_init(void)
 {
-	if (sclp_has_vt220() || sclp_has_linemode())
+	if (sclp.has_vt220 || sclp.has_linemode)
 		return -ENODEV;
 	return virtio_cons_early_init(early_put_chars);
 }
diff --git a/drivers/s390/kvm/virtio_ccw.c b/drivers/s390/kvm/virtio_ccw.c
index 6f1fa1773e76..f8d8fdb26b72 100644
--- a/drivers/s390/kvm/virtio_ccw.c
+++ b/drivers/s390/kvm/virtio_ccw.c
@@ -65,6 +65,7 @@ struct virtio_ccw_device {
 	bool is_thinint;
 	bool going_away;
 	bool device_lost;
+	unsigned int config_ready;
 	void *airq_info;
 };
 
@@ -833,8 +834,11 @@ static void virtio_ccw_get_config(struct virtio_device *vdev,
 	if (ret)
 		goto out_free;
 
-	memcpy(vcdev->config, config_area, sizeof(vcdev->config));
-	memcpy(buf, &vcdev->config[offset], len);
+	memcpy(vcdev->config, config_area, offset + len);
+	if (buf)
+		memcpy(buf, &vcdev->config[offset], len);
+	if (vcdev->config_ready < offset + len)
+		vcdev->config_ready = offset + len;
 
 out_free:
 	kfree(config_area);
@@ -857,6 +861,9 @@ static void virtio_ccw_set_config(struct virtio_device *vdev,
 	if (!config_area)
 		goto out_free;
 
+	/* Make sure we don't overwrite fields. */
+	if (vcdev->config_ready < offset)
+		virtio_ccw_get_config(vdev, 0, NULL, offset);
 	memcpy(&vcdev->config[offset], buf, len);
 	/* Write the config area to the host. */
 	memcpy(config_area, vcdev->config, sizeof(vcdev->config));
diff --git a/drivers/s390/net/lcs.c b/drivers/s390/net/lcs.c
index 00b7d9c9fe48..2f5b518b0e78 100644
--- a/drivers/s390/net/lcs.c
+++ b/drivers/s390/net/lcs.c
@@ -2150,7 +2150,7 @@ lcs_new_device(struct ccwgroup_device *ccwgdev)
 	rc = lcs_detect(card);
 	if (rc) {
 		LCS_DBF_TEXT(2, setup, "dtctfail");
-		dev_err(&card->dev->dev,
+		dev_err(&ccwgdev->dev,
 			"Detecting a network adapter for LCS devices"
 			" failed with rc=%d (0x%x)\n", rc, rc);
 		lcs_stopcard(card);
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index 3abac028899f..ba974a2e409f 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -175,6 +175,8 @@ struct qeth_sbp_info {
 	__u32 supported_funcs;
 	enum qeth_sbp_roles role;
 	__u32 hostnotification:1;
+	__u32 reflect_promisc:1;
+	__u32 reflect_promisc_primary:1;
 };
 
 static inline int qeth_is_ipa_supported(struct qeth_ipa_info *ipa,
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 3466d3cb7647..5e20fba37bff 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -645,7 +645,8 @@ static struct qeth_ipa_cmd *qeth_check_ipa_data(struct qeth_card *card,
 					card->info.hwtrap = 2;
 				qeth_schedule_recovery(card);
 				return NULL;
-			case IPA_CMD_SETBRIDGEPORT:
+			case IPA_CMD_SETBRIDGEPORT_IQD:
+			case IPA_CMD_SETBRIDGEPORT_OSA:
 			case IPA_CMD_ADDRESS_CHANGE_NOTIF:
 				if (card->discipline->control_event_handler
 								(card, cmd))
diff --git a/drivers/s390/net/qeth_core_mpc.c b/drivers/s390/net/qeth_core_mpc.c
index 7b55768a9592..beb4bdc26de5 100644
--- a/drivers/s390/net/qeth_core_mpc.c
+++ b/drivers/s390/net/qeth_core_mpc.c
@@ -237,6 +237,7 @@ static struct ipa_cmd_names qeth_ipa_cmd_names[] = {
 	{IPA_CMD_DELGMAC,	"delgmac"},
 	{IPA_CMD_SETVLAN,	"setvlan"},
 	{IPA_CMD_DELVLAN,	"delvlan"},
+	{IPA_CMD_SETBRIDGEPORT_OSA,	"set_bridge_port(osa)"},
 	{IPA_CMD_SETCCID,	"setccid"},
 	{IPA_CMD_DELCCID,	"delccid"},
 	{IPA_CMD_MODCCID,	"modccid"},
@@ -249,7 +250,7 @@ static struct ipa_cmd_names qeth_ipa_cmd_names[] = {
 	{IPA_CMD_DELIP,		"delip"},
 	{IPA_CMD_SETADAPTERPARMS, "setadapterparms"},
 	{IPA_CMD_SET_DIAG_ASS,	"set_diag_ass"},
-	{IPA_CMD_SETBRIDGEPORT,	"set_bridge_port"},
+	{IPA_CMD_SETBRIDGEPORT_IQD,	"set_bridge_port(hs)"},
 	{IPA_CMD_CREATE_ADDR,	"create_addr"},
 	{IPA_CMD_DESTROY_ADDR,	"destroy_addr"},
 	{IPA_CMD_REGISTER_LOCAL_ADDR,	"register_local_addr"},
diff --git a/drivers/s390/net/qeth_core_mpc.h b/drivers/s390/net/qeth_core_mpc.h
index 1558be1af72d..6cccc9a49ede 100644
--- a/drivers/s390/net/qeth_core_mpc.h
+++ b/drivers/s390/net/qeth_core_mpc.h
@@ -92,6 +92,7 @@ enum qeth_ipa_cmds {
 	IPA_CMD_DELGMAC			= 0x24,
 	IPA_CMD_SETVLAN			= 0x25,
 	IPA_CMD_DELVLAN			= 0x26,
+	IPA_CMD_SETBRIDGEPORT_OSA	= 0x2b,
 	IPA_CMD_SETCCID			= 0x41,
 	IPA_CMD_DELCCID			= 0x42,
 	IPA_CMD_MODCCID			= 0x43,
@@ -104,7 +105,7 @@ enum qeth_ipa_cmds {
 	IPA_CMD_DELIP			= 0xb7,
 	IPA_CMD_SETADAPTERPARMS		= 0xb8,
 	IPA_CMD_SET_DIAG_ASS		= 0xb9,
-	IPA_CMD_SETBRIDGEPORT		= 0xbe,
+	IPA_CMD_SETBRIDGEPORT_IQD	= 0xbe,
 	IPA_CMD_CREATE_ADDR		= 0xc3,
 	IPA_CMD_DESTROY_ADDR		= 0xc4,
 	IPA_CMD_REGISTER_LOCAL_ADDR	= 0xd1,
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 0ea0869120cf..2e65b989a9ea 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -137,7 +137,7 @@ static int qeth_setdel_makerc(struct qeth_card *card, int retcode)
 		rc = 0;
 		break;
 	case IPA_RC_L2_UNSUPPORTED_CMD:
-		rc = -ENOSYS;
+		rc = -EOPNOTSUPP;
 		break;
 	case IPA_RC_L2_ADDR_TABLE_FULL:
 		rc = -ENOSPC;
@@ -683,6 +683,39 @@ static int qeth_l2_set_mac_address(struct net_device *dev, void *p)
 	return rc ? -EINVAL : 0;
 }
 
+static void qeth_promisc_to_bridge(struct qeth_card *card)
+{
+	struct net_device *dev = card->dev;
+	enum qeth_ipa_promisc_modes promisc_mode;
+	int role;
+	int rc;
+
+	QETH_CARD_TEXT(card, 3, "pmisc2br");
+
+	if (!card->options.sbp.reflect_promisc)
+		return;
+	promisc_mode = (dev->flags & IFF_PROMISC) ? SET_PROMISC_MODE_ON
+						: SET_PROMISC_MODE_OFF;
+	if (promisc_mode == card->info.promisc_mode)
+		return;
+
+	if (promisc_mode == SET_PROMISC_MODE_ON) {
+		if (card->options.sbp.reflect_promisc_primary)
+			role = QETH_SBP_ROLE_PRIMARY;
+		else
+			role = QETH_SBP_ROLE_SECONDARY;
+	} else
+		role = QETH_SBP_ROLE_NONE;
+
+	rc = qeth_bridgeport_setrole(card, role);
+	QETH_DBF_TEXT_(SETUP, 2, "bpm%c%04x",
+			(promisc_mode == SET_PROMISC_MODE_ON) ? '+' : '-', rc);
+	if (!rc) {
+		card->options.sbp.role = role;
+		card->info.promisc_mode = promisc_mode;
+	}
+}
+
 static void qeth_l2_set_multicast_list(struct net_device *dev)
 {
 	struct qeth_card *card = dev->ml_priv;
@@ -704,9 +737,10 @@ static void qeth_l2_set_multicast_list(struct net_device *dev)
 		qeth_l2_add_mc(card, ha->addr, 1);
 
 	spin_unlock_bh(&card->mclock);
-	if (!qeth_adp_supported(card, IPA_SETADP_SET_PROMISC_MODE))
-		return;
-	qeth_setadp_promisc_mode(card);
+	if (qeth_adp_supported(card, IPA_SETADP_SET_PROMISC_MODE))
+		qeth_setadp_promisc_mode(card);
+	else
+		qeth_promisc_to_bridge(card);
 }
 
 static int qeth_l2_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
@@ -994,7 +1028,7 @@ static int __qeth_l2_set_online(struct ccwgroup_device *gdev, int recovery_mode)
 	qeth_bridgeport_query_support(card);
 	if (card->options.sbp.supported_funcs)
 		dev_info(&card->gdev->dev,
-		"The device represents a HiperSockets Bridge Capable Port\n");
+		"The device represents a Bridge Capable Port\n");
 	qeth_trace_features(card);
 
 	if (!card->dev && qeth_l2_setup_netdev(card)) {
@@ -1247,7 +1281,8 @@ static int qeth_l2_control_event(struct qeth_card *card,
 					struct qeth_ipa_cmd *cmd)
 {
 	switch (cmd->hdr.command) {
-	case IPA_CMD_SETBRIDGEPORT:
+	case IPA_CMD_SETBRIDGEPORT_OSA:
+	case IPA_CMD_SETBRIDGEPORT_IQD:
 		if (cmd->data.sbp.hdr.command_code ==
 				IPA_SBP_BRIDGE_PORT_STATE_CHANGE) {
 			qeth_bridge_state_change(card, cmd);
@@ -1533,7 +1568,7 @@ static void qeth_bridge_host_event_worker(struct work_struct *work)
 
 	if (data->hostevs.lost_event_mask) {
 		dev_info(&data->card->gdev->dev,
-"Address notification from the HiperSockets Bridge Port stopped %s (%s)\n",
+"Address notification from the Bridge Port stopped %s (%s)\n",
 			data->card->dev->name,
 			(data->hostevs.lost_event_mask == 0x01)
 			? "Overflow"
@@ -1617,70 +1652,80 @@ static int qeth_bridgeport_makerc(struct qeth_card *card,
 	struct _qeth_sbp_cbctl *cbctl, enum qeth_ipa_sbp_cmd setcmd)
 {
 	int rc;
+	int is_iqd = (card->info.type == QETH_CARD_TYPE_IQD);
 
-	switch (cbctl->ipa_rc) {
-	case IPA_RC_SUCCESS:
+	if ((is_iqd && (cbctl->ipa_rc == IPA_RC_SUCCESS)) ||
+	    (!is_iqd && (cbctl->ipa_rc == cbctl->cmd_rc)))
 		switch (cbctl->cmd_rc) {
 		case 0x0000:
 			rc = 0;
 			break;
+		case 0x2B04:
 		case 0x0004:
-			rc = -ENOSYS;
+			rc = -EOPNOTSUPP;
 			break;
+		case 0x2B0C:
 		case 0x000C: /* Not configured as bridge Port */
 			rc = -ENODEV; /* maybe not the best code here? */
 			dev_err(&card->gdev->dev,
-	"The HiperSockets device is not configured as a Bridge Port\n");
+	"The device is not configured as a Bridge Port\n");
 			break;
+		case 0x2B14:
 		case 0x0014: /* Another device is Primary */
 			switch (setcmd) {
 			case IPA_SBP_SET_PRIMARY_BRIDGE_PORT:
 				rc = -EEXIST;
 				dev_err(&card->gdev->dev,
-	"The HiperSockets LAN already has a primary Bridge Port\n");
+	"The LAN already has a primary Bridge Port\n");
 				break;
 			case IPA_SBP_SET_SECONDARY_BRIDGE_PORT:
 				rc = -EBUSY;
 				dev_err(&card->gdev->dev,
-	"The HiperSockets device is already a primary Bridge Port\n");
+	"The device is already a primary Bridge Port\n");
 				break;
 			default:
 				rc = -EIO;
 			}
 			break;
+		case 0x2B18:
 		case 0x0018: /* This device is currently Secondary */
 			rc = -EBUSY;
 			dev_err(&card->gdev->dev,
-	"The HiperSockets device is already a secondary Bridge Port\n");
+	"The device is already a secondary Bridge Port\n");
 			break;
+		case 0x2B1C:
 		case 0x001C: /* Limit for Secondary devices reached */
 			rc = -EEXIST;
 			dev_err(&card->gdev->dev,
-	"The HiperSockets LAN cannot have more secondary Bridge Ports\n");
+	"The LAN cannot have more secondary Bridge Ports\n");
 			break;
+		case 0x2B24:
 		case 0x0024: /* This device is currently Primary */
 			rc = -EBUSY;
 			dev_err(&card->gdev->dev,
-	"The HiperSockets device is already a primary Bridge Port\n");
+	"The device is already a primary Bridge Port\n");
 			break;
+		case 0x2B20:
 		case 0x0020: /* Not authorized by zManager */
 			rc = -EACCES;
 			dev_err(&card->gdev->dev,
-	"The HiperSockets device is not authorized to be a Bridge Port\n");
+	"The device is not authorized to be a Bridge Port\n");
 			break;
 		default:
 			rc = -EIO;
 		}
-		break;
-	case IPA_RC_NOTSUPP:
-		rc = -ENOSYS;
-		break;
-	case IPA_RC_UNSUPPORTED_COMMAND:
-		rc = -ENOSYS;
-		break;
-	default:
-		rc = -EIO;
-	}
+	else
+		switch (cbctl->ipa_rc) {
+		case IPA_RC_NOTSUPP:
+			rc = -EOPNOTSUPP;
+			break;
+		case IPA_RC_UNSUPPORTED_COMMAND:
+			rc = -EOPNOTSUPP;
+			break;
+		default:
+			rc = -EIO;
+		}
+
 	if (rc) {
 		QETH_CARD_TEXT_(card, 2, "SBPi%04x", cbctl->ipa_rc);
 		QETH_CARD_TEXT_(card, 2, "SBPc%04x", cbctl->cmd_rc);
@@ -1688,6 +1733,13 @@ static int qeth_bridgeport_makerc(struct qeth_card *card,
 	return rc;
 }
 
+static inline int ipa_cmd_sbp(struct qeth_card *card)
+{
+	return (card->info.type == QETH_CARD_TYPE_IQD) ?
+		IPA_CMD_SETBRIDGEPORT_IQD :
+		IPA_CMD_SETBRIDGEPORT_OSA;
+}
+
 static int qeth_bridgeport_query_support_cb(struct qeth_card *card,
 	struct qeth_reply *reply, unsigned long data)
 {
@@ -1719,7 +1771,7 @@ static void qeth_bridgeport_query_support(struct qeth_card *card)
 	struct _qeth_sbp_cbctl cbctl;
 
 	QETH_CARD_TEXT(card, 2, "brqsuppo");
-	iob = qeth_get_ipacmd_buffer(card, IPA_CMD_SETBRIDGEPORT, 0);
+	iob = qeth_get_ipacmd_buffer(card, ipa_cmd_sbp(card), 0);
 	if (!iob)
 		return;
 	cmd = (struct qeth_ipa_cmd *)(iob->data+IPA_PDU_HEADER_SIZE);
@@ -1796,7 +1848,7 @@ int qeth_bridgeport_query_ports(struct qeth_card *card,
 	QETH_CARD_TEXT(card, 2, "brqports");
 	if (!(card->options.sbp.supported_funcs & IPA_SBP_QUERY_BRIDGE_PORTS))
 		return -EOPNOTSUPP;
-	iob = qeth_get_ipacmd_buffer(card, IPA_CMD_SETBRIDGEPORT, 0);
+	iob = qeth_get_ipacmd_buffer(card, ipa_cmd_sbp(card), 0);
 	if (!iob)
 		return -ENOMEM;
 	cmd = (struct qeth_ipa_cmd *)(iob->data+IPA_PDU_HEADER_SIZE);
@@ -1808,10 +1860,9 @@ int qeth_bridgeport_query_ports(struct qeth_card *card,
 	cmd->data.sbp.hdr.seq_no = 1;
 	rc = qeth_send_ipa_cmd(card, iob, qeth_bridgeport_query_ports_cb,
 				(void *)&cbctl);
-	if (rc)
+	if (rc < 0)
 		return rc;
-	rc = qeth_bridgeport_makerc(card, &cbctl, IPA_SBP_QUERY_BRIDGE_PORTS);
-	return rc;
+	return qeth_bridgeport_makerc(card, &cbctl, IPA_SBP_QUERY_BRIDGE_PORTS);
 }
 EXPORT_SYMBOL_GPL(qeth_bridgeport_query_ports);
 
@@ -1864,7 +1915,7 @@ int qeth_bridgeport_setrole(struct qeth_card *card, enum qeth_sbp_roles role)
 	}
 	if (!(card->options.sbp.supported_funcs & setcmd))
 		return -EOPNOTSUPP;
-	iob = qeth_get_ipacmd_buffer(card, IPA_CMD_SETBRIDGEPORT, 0);
+	iob = qeth_get_ipacmd_buffer(card, ipa_cmd_sbp(card), 0);
 	if (!iob)
 		return -ENOMEM;
 	cmd = (struct qeth_ipa_cmd *)(iob->data+IPA_PDU_HEADER_SIZE);
@@ -1874,10 +1925,9 @@ int qeth_bridgeport_setrole(struct qeth_card *card, enum qeth_sbp_roles role)
 	cmd->data.sbp.hdr.seq_no = 1;
 	rc = qeth_send_ipa_cmd(card, iob, qeth_bridgeport_set_cb,
 				(void *)&cbctl);
-	if (rc)
+	if (rc < 0)
 		return rc;
-	rc = qeth_bridgeport_makerc(card, &cbctl, setcmd);
-	return rc;
+	return qeth_bridgeport_makerc(card, &cbctl, setcmd);
 }
 
 /**
@@ -1898,7 +1948,7 @@ static int qeth_anset_makerc(struct qeth_card *card, int pnso_rc, u16 response)
 		case 0x0004:
 		case 0x0100:
 		case 0x0106:
-			rc = -ENOSYS;
+			rc = -EOPNOTSUPP;
 			dev_err(&card->gdev->dev,
 				"Setting address notification failed\n");
 			break;
diff --git a/drivers/s390/net/qeth_l2_sys.c b/drivers/s390/net/qeth_l2_sys.c
index 59e3aa538b4d..52673cd1db99 100644
--- a/drivers/s390/net/qeth_l2_sys.c
+++ b/drivers/s390/net/qeth_l2_sys.c
@@ -23,8 +23,6 @@ static ssize_t qeth_bridge_port_role_state_show(struct device *dev,
 	if (!card)
 		return -EINVAL;
 
-	mutex_lock(&card->conf_mutex);
-
 	if (qeth_card_hw_is_reachable(card) &&
 					card->options.sbp.supported_funcs)
 		rc = qeth_bridgeport_query_ports(card,
@@ -59,8 +57,6 @@ static ssize_t qeth_bridge_port_role_state_show(struct device *dev,
 			rc = sprintf(buf, "%s\n", word);
 	}
 
-	mutex_unlock(&card->conf_mutex);
-
 	return rc;
 }
 
@@ -90,7 +86,9 @@ static ssize_t qeth_bridge_port_role_store(struct device *dev,
 
 	mutex_lock(&card->conf_mutex);
 
-	if (qeth_card_hw_is_reachable(card)) {
+	if (card->options.sbp.reflect_promisc) /* Forbid direct manipulation */
+		rc = -EPERM;
+	else if (qeth_card_hw_is_reachable(card)) {
 		rc = qeth_bridgeport_setrole(card, role);
 		if (!rc)
 			card->options.sbp.role = role;
@@ -123,12 +121,8 @@ static ssize_t qeth_bridgeport_hostnotification_show(struct device *dev,
 	if (!card)
 		return -EINVAL;
 
-	mutex_lock(&card->conf_mutex);
-
 	enabled = card->options.sbp.hostnotification;
 
-	mutex_unlock(&card->conf_mutex);
-
 	return sprintf(buf, "%d\n", enabled);
 }
 
@@ -167,10 +161,72 @@ static DEVICE_ATTR(bridge_hostnotify, 0644,
 			qeth_bridgeport_hostnotification_show,
 			qeth_bridgeport_hostnotification_store);
 
+static ssize_t qeth_bridgeport_reflect_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct qeth_card *card = dev_get_drvdata(dev);
+	char *state;
+
+	if (!card)
+		return -EINVAL;
+
+	if (card->options.sbp.reflect_promisc) {
+		if (card->options.sbp.reflect_promisc_primary)
+			state = "primary";
+		else
+			state = "secondary";
+	} else
+		state = "none";
+
+	return sprintf(buf, "%s\n", state);
+}
+
+static ssize_t qeth_bridgeport_reflect_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct qeth_card *card = dev_get_drvdata(dev);
+	int enable, primary;
+	int rc = 0;
+
+	if (!card)
+		return -EINVAL;
+
+	if (sysfs_streq(buf, "none")) {
+		enable = 0;
+		primary = 0;
+	} else if (sysfs_streq(buf, "primary")) {
+		enable = 1;
+		primary = 1;
+	} else if (sysfs_streq(buf, "secondary")) {
+		enable = 1;
+		primary = 0;
+	} else
+		return -EINVAL;
+
+	mutex_lock(&card->conf_mutex);
+
+	if (card->options.sbp.role != QETH_SBP_ROLE_NONE)
+		rc = -EPERM;
+	else {
+		card->options.sbp.reflect_promisc = enable;
+		card->options.sbp.reflect_promisc_primary = primary;
+		rc = 0;
+	}
+
+	mutex_unlock(&card->conf_mutex);
+
+	return rc ? rc : count;
+}
+
+static DEVICE_ATTR(bridge_reflect_promisc, 0644,
+			qeth_bridgeport_reflect_show,
+			qeth_bridgeport_reflect_store);
+
 static struct attribute *qeth_l2_bridgeport_attrs[] = {
 	&dev_attr_bridge_role.attr,
 	&dev_attr_bridge_state.attr,
 	&dev_attr_bridge_hostnotify.attr,
+	&dev_attr_bridge_reflect_promisc.attr,
 	NULL,
 };
 
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index 04e42c649134..70eb2f61bb92 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -3198,8 +3198,7 @@ static int qeth_l3_set_features(struct net_device *dev,
 	netdev_features_t features)
 {
 	struct qeth_card *card = dev->ml_priv;
-	u32 changed = dev->features ^ features;
-	int err;
+	netdev_features_t changed = dev->features ^ features;
 
 	if (!(changed & NETIF_F_RXCSUM))
 		return 0;
@@ -3208,11 +3207,7 @@ static int qeth_l3_set_features(struct net_device *dev,
 	    card->state == CARD_STATE_RECOVER)
 		return 0;
 
-	err = qeth_l3_set_rx_csum(card, features & NETIF_F_RXCSUM);
-	if (err)
-		dev->features = features ^ NETIF_F_RXCSUM;
-
-	return err;
+	return qeth_l3_set_rx_csum(card, features & NETIF_F_RXCSUM ? 1 : 0);
 }
 
 static const struct ethtool_ops qeth_l3_ethtool_ops = {
diff --git a/drivers/s390/scsi/zfcp_scsi.c b/drivers/s390/scsi/zfcp_scsi.c
index 75f4bfc2b98a..b3c6ff49103b 100644
--- a/drivers/s390/scsi/zfcp_scsi.c
+++ b/drivers/s390/scsi/zfcp_scsi.c
@@ -297,7 +297,6 @@ static struct scsi_host_template zfcp_scsi_host_template = {
 				     * ZFCP_QDIO_MAX_SBALS_PER_REQ) - 2) * 8,
 				   /* GCD, adjusted later */
 	.dma_boundary		 = ZFCP_QDIO_SBALE_LEN - 1,
-	.cmd_per_lun		 = 1,
 	.use_clustering		 = 1,
 	.shost_attrs		 = zfcp_sysfs_shost_attrs,
 	.sdev_attrs		 = zfcp_sysfs_sdev_attrs,
diff --git a/drivers/scsi/Kconfig b/drivers/scsi/Kconfig
index b021bcb88537..456e1567841c 100644
--- a/drivers/scsi/Kconfig
+++ b/drivers/scsi/Kconfig
@@ -52,7 +52,7 @@ config SCSI_MQ_DEFAULT
 	  This option enables the new blk-mq based I/O path for SCSI
 	  devices by default.  With the option the scsi_mod.use_blk_mq
 	  module/boot option defaults to Y, without it to N, but it can
-	  still be overriden either way.
+	  still be overridden either way.
 
 	  If unsure say N.
 
@@ -503,7 +503,7 @@ config SCSI_DPT_I2O
 
 config SCSI_ADVANSYS
 	tristate "AdvanSys SCSI support"
-	depends on SCSI && VIRT_TO_BUS && !ARM
+	depends on SCSI
 	depends on ISA || EISA || PCI
 	help
 	  This is a driver for all SCSI host adapters manufactured by
@@ -634,6 +634,23 @@ config FCOE_FNIC
 	  <file:Documentation/scsi/scsi.txt>.
 	  The module will be called fnic.
 
+config SCSI_SNIC
+	tristate "Cisco SNIC Driver"
+	depends on PCI && SCSI
+	help
+	  This is support for the Cisco PCI-Express SCSI HBA.
+
+	  To compile this driver as a module, choose M here and read
+	  <file:Documentation/scsi/scsi.txt>.
+	  The module will be called snic.
+
+config SCSI_SNIC_DEBUG_FS
+	bool "Cisco SNIC Driver Debugfs Support"
+	depends on SCSI_SNIC && DEBUG_FS
+	help
+	  This enables to list debugging information from SNIC Driver
+	  available via debugfs file system
+
 config SCSI_DMX3191D
 	tristate "DMX3191D SCSI support"
 	depends on PCI && SCSI
@@ -1743,7 +1760,6 @@ config SCSI_BFA_FC
 config SCSI_VIRTIO
 	tristate "virtio-scsi support"
 	depends on VIRTIO
-	select BLK_DEV_INTEGRITY
 	help
           This is the virtual HBA driver for virtio.  If the kernel will
           be used in a virtual machine, say Y or M.
diff --git a/drivers/scsi/Makefile b/drivers/scsi/Makefile
index dee160a4f163..91209e3d27e3 100644
--- a/drivers/scsi/Makefile
+++ b/drivers/scsi/Makefile
@@ -39,6 +39,7 @@ obj-$(CONFIG_LIBFC)		+= libfc/
 obj-$(CONFIG_LIBFCOE)		+= fcoe/
 obj-$(CONFIG_FCOE)		+= fcoe/
 obj-$(CONFIG_FCOE_FNIC)		+= fnic/
+obj-$(CONFIG_SCSI_SNIC)		+= snic/
 obj-$(CONFIG_SCSI_BNX2X_FCOE)	+= libfc/ fcoe/ bnx2fc/
 obj-$(CONFIG_ISCSI_TCP) 	+= libiscsi.o	libiscsi_tcp.o iscsi_tcp.o
 obj-$(CONFIG_INFINIBAND_ISER) 	+= libiscsi.o
@@ -161,6 +162,7 @@ obj-$(CONFIG_SCSI_OSD_INITIATOR) += osd/
 obj-$(CONFIG_SCSI_DEBUG)	+= scsi_debug.o
 scsi_mod-y			+= scsi.o hosts.o scsi_ioctl.o \
 				   scsicam.o scsi_error.o scsi_lib.o
+scsi_mod-y			+= scsi_common.o
 scsi_mod-$(CONFIG_SCSI_CONSTANTS) += constants.o
 scsi_mod-$(CONFIG_SCSI_DMA)	+= scsi_lib_dma.o
 scsi_mod-y			+= scsi_scan.o scsi_sysfs.o scsi_devinfo.o
diff --git a/drivers/scsi/NCR53c406a.c b/drivers/scsi/NCR53c406a.c
index 42c7161474f7..6e110c630d2c 100644
--- a/drivers/scsi/NCR53c406a.c
+++ b/drivers/scsi/NCR53c406a.c
@@ -1064,7 +1064,6 @@ static struct scsi_host_template driver_template =
      .can_queue         	= 1			/* can_queue */,        
      .this_id           	= 7			/* SCSI ID of the chip */,
      .sg_tablesize      	= 32			/*SG_ALL*/ /*SG_NONE*/, 
-     .cmd_per_lun       	= 1			/* commands per lun */, 
      .unchecked_isa_dma 	= 1			/* unchecked_isa_dma */,
      .use_clustering    	= ENABLE_CLUSTERING,
 };
diff --git a/drivers/scsi/a100u2w.c b/drivers/scsi/a100u2w.c
index 7e33a61c1ba4..cac6b37d7b1b 100644
--- a/drivers/scsi/a100u2w.c
+++ b/drivers/scsi/a100u2w.c
@@ -1078,7 +1078,6 @@ static struct scsi_host_template inia100_template = {
 	.can_queue		= 1,
 	.this_id		= 1,
 	.sg_tablesize		= SG_ALL,
-	.cmd_per_lun 		= 1,
 	.use_clustering		= ENABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/aacraid/src.c b/drivers/scsi/aacraid/src.c
index 4596e9dd757c..e63cf9f22f36 100644
--- a/drivers/scsi/aacraid/src.c
+++ b/drivers/scsi/aacraid/src.c
@@ -46,7 +46,7 @@
 
 static int aac_src_get_sync_status(struct aac_dev *dev);
 
-irqreturn_t aac_src_intr_message(int irq, void *dev_id)
+static irqreturn_t aac_src_intr_message(int irq, void *dev_id)
 {
 	struct aac_msix_ctx *ctx;
 	struct aac_dev *dev;
diff --git a/drivers/scsi/advansys.c b/drivers/scsi/advansys.c
index ae95e347f37d..4305178e4e01 100644
--- a/drivers/scsi/advansys.c
+++ b/drivers/scsi/advansys.c
@@ -1,12 +1,10 @@
-#define DRV_NAME "advansys"
-#define ASC_VERSION "3.4"	/* AdvanSys Driver Version */
-
 /*
  * advansys.c - Linux Host Driver for AdvanSys SCSI Adapters
  *
  * Copyright (c) 1995-2000 Advanced System Products, Inc.
  * Copyright (c) 2000-2001 ConnectCom Solutions, Inc.
  * Copyright (c) 2007 Matthew Wilcox <matthew@wil.cx>
+ * Copyright (c) 2014 Hannes Reinecke <hare@suse.de>
  * All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -39,6 +37,7 @@
 #include <linux/spinlock.h>
 #include <linux/dma-mapping.h>
 #include <linux/firmware.h>
+#include <linux/dmapool.h>
 
 #include <asm/io.h>
 #include <asm/dma.h>
@@ -49,26 +48,15 @@
 #include <scsi/scsi.h>
 #include <scsi/scsi_host.h>
 
+#define DRV_NAME "advansys"
+#define ASC_VERSION "3.5"	/* AdvanSys Driver Version */
+
 /* FIXME:
  *
- *  1. Although all of the necessary command mapping places have the
- *     appropriate dma_map.. APIs, the driver still processes its internal
- *     queue using bus_to_virt() and virt_to_bus() which are illegal under
- *     the API.  The entire queue processing structure will need to be
- *     altered to fix this.
- *  2. Need to add memory mapping workaround. Test the memory mapping.
- *     If it doesn't work revert to I/O port access. Can a test be done
- *     safely?
- *  3. Handle an interrupt not working. Keep an interrupt counter in
- *     the interrupt handler. In the timeout function if the interrupt
- *     has not occurred then print a message and run in polled mode.
- *  4. Need to add support for target mode commands, cf. CAM XPT.
- *  5. check DMA mapping functions for failure
- *  6. Use scsi_transport_spi
- *  7. advansys_info is not safe against multiple simultaneous callers
- *  8. Add module_param to override ISA/VLB ioport array
+ *  1. Use scsi_transport_spi
+ *  2. advansys_info is not safe against multiple simultaneous callers
+ *  3. Add module_param to override ISA/VLB ioport array
  */
-#warning this driver is still not properly converted to the DMA API
 
 /* Enable driver /proc statistics. */
 #define ADVANSYS_STATS
@@ -76,31 +64,8 @@
 /* Enable driver tracing. */
 #undef ADVANSYS_DEBUG
 
-/*
- * Portable Data Types
- *
- * Any instance where a 32-bit long or pointer type is assumed
- * for precision or HW defined structures, the following define
- * types must be used. In Linux the char, short, and int types
- * are all consistent at 8, 16, and 32 bits respectively. Pointers
- * and long types are 64 bits on Alpha and UltraSPARC.
- */
-#define ASC_PADDR __u32		/* Physical/Bus address data type. */
-#define ASC_VADDR __u32		/* Virtual address data type. */
-#define ASC_DCNT  __u32		/* Unsigned Data count type. */
-#define ASC_SDCNT __s32		/* Signed Data count type. */
-
 typedef unsigned char uchar;
 
-#ifndef TRUE
-#define TRUE     (1)
-#endif
-#ifndef FALSE
-#define FALSE    (0)
-#endif
-
-#define ERR      (-1)
-#define UW_ERR   (uint)(0xFFFF)
 #define isodd_word(val)   ((((uint)val) & (uint)0x0001) != 0)
 
 #define PCI_VENDOR_ID_ASP		0x10cd
@@ -111,15 +76,6 @@ typedef unsigned char uchar;
 #define PCI_DEVICE_ID_38C0800_REV1	0x2500
 #define PCI_DEVICE_ID_38C1600_REV1	0x2700
 
-/*
- * Enable CC_VERY_LONG_SG_LIST to support up to 64K element SG lists.
- * The SRB structure will have to be changed and the ASC_SRB2SCSIQ()
- * macro re-defined to be able to obtain a ASC_SCSI_Q pointer from the
- * SRB structure.
- */
-#define CC_VERY_LONG_SG_LIST 0
-#define ASC_SRB2SCSIQ(srb_ptr)  (srb_ptr)
-
 #define PortAddr                 unsigned int	/* port address size  */
 #define inp(port)                inb(port)
 #define outp(port, byte)         outb((byte), (port))
@@ -307,15 +263,15 @@ typedef struct asc_scsiq_1 {
 	uchar sg_queue_cnt;
 	uchar target_id;
 	uchar target_lun;
-	ASC_PADDR data_addr;
-	ASC_DCNT data_cnt;
-	ASC_PADDR sense_addr;
+	__le32 data_addr;
+	__le32 data_cnt;
+	__le32 sense_addr;
 	uchar sense_len;
 	uchar extra_bytes;
 } ASC_SCSIQ_1;
 
 typedef struct asc_scsiq_2 {
-	ASC_VADDR srb_ptr;
+	u32 srb_tag;
 	uchar target_ix;
 	uchar flag;
 	uchar cdb_len;
@@ -338,8 +294,8 @@ typedef struct asc_scsiq_4 {
 	uchar y_res;
 	ushort x_req_count;
 	ushort x_reconnect_rtn;
-	ASC_PADDR x_saved_data_addr;
-	ASC_DCNT x_saved_data_cnt;
+	__le32 x_saved_data_addr;
+	__le32 x_saved_data_cnt;
 } ASC_SCSIQ_4;
 
 typedef struct asc_q_done_info {
@@ -351,12 +307,12 @@ typedef struct asc_q_done_info {
 	uchar sense_len;
 	uchar extra_bytes;
 	uchar res;
-	ASC_DCNT remain_bytes;
+	u32 remain_bytes;
 } ASC_QDONE_INFO;
 
 typedef struct asc_sg_list {
-	ASC_PADDR addr;
-	ASC_DCNT bytes;
+	__le32 addr;
+	__le32 bytes;
 } ASC_SG_LIST;
 
 typedef struct asc_sg_head {
@@ -376,17 +332,6 @@ typedef struct asc_scsi_q {
 	ushort next_sg_index;
 } ASC_SCSI_Q;
 
-typedef struct asc_scsi_req_q {
-	ASC_SCSIQ_1 r1;
-	ASC_SCSIQ_2 r2;
-	uchar *cdbptr;
-	ASC_SG_HEAD *sg_head;
-	uchar *sense_ptr;
-	ASC_SCSIQ_3 r3;
-	uchar cdb[ASC_MAX_CDB_LEN];
-	uchar sense[ASC_MIN_SENSE_LEN];
-} ASC_SCSI_REQ_Q;
-
 typedef struct asc_scsi_bios_req_q {
 	ASC_SCSIQ_1 r1;
 	ASC_SCSIQ_2 r2;
@@ -570,7 +515,7 @@ typedef struct asc_dvc_var {
 	dma_addr_t overrun_dma;
 	uchar scsi_reset_wait;
 	uchar chip_no;
-	char is_in_int;
+	bool is_in_int;
 	uchar max_total_qng;
 	uchar cur_total_qng;
 	uchar in_critical_cnt;
@@ -586,15 +531,13 @@ typedef struct asc_dvc_var {
 	char redo_scam;
 	ushort res2;
 	uchar dos_int13_table[ASC_MAX_TID + 1];
-	ASC_DCNT max_dma_count;
+	unsigned int max_dma_count;
 	ASC_SCSI_BIT_ID_TYPE no_scam;
 	ASC_SCSI_BIT_ID_TYPE pci_fix_asyn_xfer;
 	uchar min_sdtr_index;
 	uchar max_sdtr_index;
 	struct asc_board *drv_ptr;
-	int ptr_map_count;
-	void **ptr_map;
-	ASC_DCNT uc_break;
+	unsigned int uc_break;
 } ASC_DVC_VAR;
 
 typedef struct asc_dvc_inq_info {
@@ -602,8 +545,8 @@ typedef struct asc_dvc_inq_info {
 } ASC_DVC_INQ_INFO;
 
 typedef struct asc_cap_info {
-	ASC_DCNT lba;
-	ASC_DCNT blk_size;
+	u32 lba;
+	u32 blk_size;
 } ASC_CAP_INFO;
 
 typedef struct asc_cap_info_array {
@@ -929,31 +872,6 @@ typedef struct asc_mc_saved {
 #define AscReadChipDvcID(port)            (uchar)inp((port)+IOP_REG_ID)
 #define AscWriteChipDvcID(port, data)     outp((port)+IOP_REG_ID, data)
 
-/*
- * Portable Data Types
- *
- * Any instance where a 32-bit long or pointer type is assumed
- * for precision or HW defined structures, the following define
- * types must be used. In Linux the char, short, and int types
- * are all consistent at 8, 16, and 32 bits respectively. Pointers
- * and long types are 64 bits on Alpha and UltraSPARC.
- */
-#define ADV_PADDR __u32		/* Physical address data type. */
-#define ADV_VADDR __u32		/* Virtual address data type. */
-#define ADV_DCNT  __u32		/* Unsigned Data count type. */
-#define ADV_SDCNT __s32		/* Signed Data count type. */
-
-/*
- * These macros are used to convert a virtual address to a
- * 32-bit value. This currently can be used on Linux Alpha
- * which uses 64-bit virtual address but a 32-bit bus address.
- * This is likely to break in the future, but doing this now
- * will give us time to change the HW and FW to handle 64-bit
- * addresses.
- */
-#define ADV_VADDR_TO_U32   virt_to_bus
-#define ADV_U32_TO_VADDR   bus_to_virt
-
 #define AdvPortAddr  void __iomem *	/* Virtual memory address size */
 
 /*
@@ -965,8 +883,6 @@ typedef struct asc_mc_saved {
 #define ADV_MEM_WRITEW(addr, word) writew(word, addr)
 #define ADV_MEM_WRITEDW(addr, dword) writel(dword, addr)
 
-#define ADV_CARRIER_COUNT (ASC_DEF_MAX_HOST_QNG + 15)
-
 /*
  * Define total number of simultaneous maximum element scatter-gather
  * request blocks per wide adapter. ASC_DEF_MAX_HOST_QNG (253) is the
@@ -1747,44 +1663,37 @@ typedef struct adveep_38C1600_config {
  * little-endian.
  */
 typedef struct adv_carr_t {
-	ADV_VADDR carr_va;	/* Carrier Virtual Address */
-	ADV_PADDR carr_pa;	/* Carrier Physical Address */
-	ADV_VADDR areq_vpa;	/* ASC_SCSI_REQ_Q Virtual or Physical Address */
+	__le32 carr_va;	/* Carrier Virtual Address */
+	__le32 carr_pa;	/* Carrier Physical Address */
+	__le32 areq_vpa;	/* ADV_SCSI_REQ_Q Virtual or Physical Address */
 	/*
 	 * next_vpa [31:4]            Carrier Virtual or Physical Next Pointer
 	 *
 	 * next_vpa [3:1]             Reserved Bits
 	 * next_vpa [0]               Done Flag set in Response Queue.
 	 */
-	ADV_VADDR next_vpa;
+	__le32 next_vpa;
 } ADV_CARR_T;
 
 /*
  * Mask used to eliminate low 4 bits of carrier 'next_vpa' field.
  */
-#define ASC_NEXT_VPA_MASK       0xFFFFFFF0
-
-#define ASC_RQ_DONE             0x00000001
-#define ASC_RQ_GOOD             0x00000002
-#define ASC_CQ_STOPPER          0x00000000
+#define ADV_NEXT_VPA_MASK       0xFFFFFFF0
 
-#define ASC_GET_CARRP(carrp) ((carrp) & ASC_NEXT_VPA_MASK)
+#define ADV_RQ_DONE             0x00000001
+#define ADV_RQ_GOOD             0x00000002
+#define ADV_CQ_STOPPER          0x00000000
 
-#define ADV_CARRIER_NUM_PAGE_CROSSING \
-    (((ADV_CARRIER_COUNT * sizeof(ADV_CARR_T)) + (PAGE_SIZE - 1))/PAGE_SIZE)
-
-#define ADV_CARRIER_BUFSIZE \
-    ((ADV_CARRIER_COUNT + ADV_CARRIER_NUM_PAGE_CROSSING) * sizeof(ADV_CARR_T))
+#define ADV_GET_CARRP(carrp) ((carrp) & ADV_NEXT_VPA_MASK)
 
 /*
- * ASC_SCSI_REQ_Q 'a_flag' definitions
- *
- * The Adv Library should limit use to the lower nibble (4 bits) of
- * a_flag. Drivers are free to use the upper nibble (4 bits) of a_flag.
+ * Each carrier is 64 bytes, and we need three additional
+ * carrier for icq, irq, and the termination carrier.
  */
-#define ADV_POLL_REQUEST                0x01	/* poll for request completion */
-#define ADV_SCSIQ_DONE                  0x02	/* request done */
-#define ADV_DONT_RETRY                  0x08	/* don't do retry */
+#define ADV_CARRIER_COUNT (ASC_DEF_MAX_HOST_QNG + 3)
+
+#define ADV_CARRIER_BUFSIZE \
+	(ADV_CARRIER_COUNT * sizeof(ADV_CARR_T))
 
 #define ADV_CHIP_ASC3550          0x01	/* Ultra-Wide IC */
 #define ADV_CHIP_ASC38C0800       0x02	/* Ultra2-Wide/LVD IC */
@@ -1816,15 +1725,15 @@ typedef struct adv_dvc_cfg {
 struct adv_dvc_var;
 struct adv_scsi_req_q;
 
-typedef struct asc_sg_block {
+typedef struct adv_sg_block {
 	uchar reserved1;
 	uchar reserved2;
 	uchar reserved3;
 	uchar sg_cnt;		/* Valid entries in block. */
-	ADV_PADDR sg_ptr;	/* Pointer to next sg block. */
+	__le32 sg_ptr;	/* Pointer to next sg block. */
 	struct {
-		ADV_PADDR sg_addr;	/* SG element address. */
-		ADV_DCNT sg_count;	/* SG element count. */
+		__le32 sg_addr;	/* SG element address. */
+		__le32 sg_count;	/* SG element count. */
 	} sg_list[NO_OF_SG_PER_BLOCK];
 } ADV_SG_BLOCK;
 
@@ -1844,10 +1753,10 @@ typedef struct adv_scsi_req_q {
 	uchar target_cmd;
 	uchar target_id;	/* Device target identifier. */
 	uchar target_lun;	/* Device target logical unit number. */
-	ADV_PADDR data_addr;	/* Data buffer physical address. */
-	ADV_DCNT data_cnt;	/* Data count. Ucode sets to residual. */
-	ADV_PADDR sense_addr;
-	ADV_PADDR carr_pa;
+	__le32 data_addr;	/* Data buffer physical address. */
+	__le32 data_cnt;	/* Data count. Ucode sets to residual. */
+	__le32 sense_addr;
+	__le32 carr_pa;
 	uchar mflag;
 	uchar sense_len;
 	uchar cdb_len;		/* SCSI CDB length. Must <= 16 bytes. */
@@ -1857,29 +1766,26 @@ typedef struct adv_scsi_req_q {
 	uchar host_status;	/* Ucode host status. */
 	uchar sg_working_ix;
 	uchar cdb[12];		/* SCSI CDB bytes 0-11. */
-	ADV_PADDR sg_real_addr;	/* SG list physical address. */
-	ADV_PADDR scsiq_rptr;
+	__le32 sg_real_addr;	/* SG list physical address. */
+	__le32 scsiq_rptr;
 	uchar cdb16[4];		/* SCSI CDB bytes 12-15. */
-	ADV_VADDR scsiq_ptr;
-	ADV_VADDR carr_va;
+	__le32 scsiq_ptr;
+	__le32 carr_va;
 	/*
 	 * End of microcode structure - 60 bytes. The rest of the structure
 	 * is used by the Adv Library and ignored by the microcode.
 	 */
-	ADV_VADDR srb_ptr;
+	u32 srb_tag;
 	ADV_SG_BLOCK *sg_list_ptr;	/* SG list virtual address. */
-	char *vdata_addr;	/* Data buffer virtual address. */
-	uchar a_flag;
-	uchar pad[2];		/* Pad out to a word boundary. */
 } ADV_SCSI_REQ_Q;
 
 /*
  * The following two structures are used to process Wide Board requests.
  *
  * The ADV_SCSI_REQ_Q structure in adv_req_t is passed to the Adv Library
- * and microcode with the ADV_SCSI_REQ_Q field 'srb_ptr' pointing to the
- * adv_req_t. The adv_req_t structure 'cmndp' field in turn points to the
- * Mid-Level SCSI request structure.
+ * and microcode with the ADV_SCSI_REQ_Q field 'srb_tag' set to the
+ * SCSI request tag. The adv_req_t structure 'cmndp' field in turn points
+ * to the Mid-Level SCSI request structure.
  *
  * Zero or more ADV_SG_BLOCK are used with each ADV_SCSI_REQ_Q. Each
  * ADV_SG_BLOCK structure holds 15 scatter-gather elements. Under Linux
@@ -1890,17 +1796,17 @@ typedef struct adv_scsi_req_q {
  */
 typedef struct adv_sgblk {
 	ADV_SG_BLOCK sg_block;	/* Sgblock structure. */
-	uchar align[32];	/* Sgblock structure padding. */
+	dma_addr_t sg_addr;	/* Physical address */
 	struct adv_sgblk *next_sgblkp;	/* Next scatter-gather structure. */
 } adv_sgblk_t;
 
 typedef struct adv_req {
 	ADV_SCSI_REQ_Q scsi_req_q;	/* Adv Library request structure. */
-	uchar align[32];	/* Request structure padding. */
+	uchar align[24];	/* Request structure padding. */
 	struct scsi_cmnd *cmndp;	/* Mid-Level SCSI command pointer. */
+	dma_addr_t req_addr;
 	adv_sgblk_t *sgblkp;	/* Adv Library scatter-gather pointer. */
-	struct adv_req *next_reqp;	/* Next Request Structure. */
-} adv_req_t;
+} adv_req_t __aligned(32);
 
 /*
  * Adapter operation variable structure.
@@ -1937,12 +1843,12 @@ typedef struct adv_dvc_var {
 	uchar chip_scsi_id;	/* chip SCSI target ID */
 	uchar chip_type;
 	uchar bist_err_code;
-	ADV_CARR_T *carrier_buf;
+	ADV_CARR_T *carrier;
 	ADV_CARR_T *carr_freelist;	/* Carrier free list. */
+	dma_addr_t carrier_addr;
 	ADV_CARR_T *icq_sp;	/* Initiator command queue stopper pointer. */
 	ADV_CARR_T *irq_sp;	/* Initiator response queue stopper pointer. */
 	ushort carr_pending_cnt;	/* Count of pending carriers. */
-	struct adv_req *orig_reqp;	/* adv_req_t memory block. */
 	/*
 	 * Note: The following fields will not be used after initialization. The
 	 * driver may discard the buffer after initialization is done.
@@ -2068,8 +1974,8 @@ do { \
     AdvReadByteRegister((iop_base), IOPB_CHIP_TYPE_REV)
 
 /*
- * Abort an SRB in the chip's RISC Memory. The 'srb_ptr' argument must
- * match the ASC_SCSI_REQ_Q 'srb_ptr' field.
+ * Abort an SRB in the chip's RISC Memory. The 'srb_tag' argument must
+ * match the ADV_SCSI_REQ_Q 'srb_tag' field.
  *
  * If the request has not yet been sent to the device it will simply be
  * aborted from RISC memory. If the request is disconnected it will be
@@ -2079,9 +1985,9 @@ do { \
  *      ADV_TRUE(1) - Queue was successfully aborted.
  *      ADV_FALSE(0) - Queue was not found on the active queue list.
  */
-#define AdvAbortQueue(asc_dvc, scsiq) \
-        AdvSendIdleCmd((asc_dvc), (ushort) IDLE_CMD_ABORT, \
-                       (ADV_DCNT) (scsiq))
+#define AdvAbortQueue(asc_dvc, srb_tag) \
+     AdvSendIdleCmd((asc_dvc), (ushort) IDLE_CMD_ABORT, \
+		    (ADV_DCNT) (srb_tag))
 
 /*
  * Send a Bus Device Reset Message to the specified target ID.
@@ -2095,8 +2001,8 @@ do { \
  *                     are not purged.
  */
 #define AdvResetDevice(asc_dvc, target_id) \
-        AdvSendIdleCmd((asc_dvc), (ushort) IDLE_CMD_DEVICE_RESET, \
-                    (ADV_DCNT) (target_id))
+     AdvSendIdleCmd((asc_dvc), (ushort) IDLE_CMD_DEVICE_RESET,	\
+		    (ADV_DCNT) (target_id))
 
 /*
  * SCSI Wide Type definition.
@@ -2115,7 +2021,7 @@ do { \
 #define ADV_TID_TO_TIDMASK(tid)   (0x01 << ((tid) & ADV_MAX_TID))
 
 /*
- * ASC_SCSI_REQ_Q 'done_status' and 'host_status' return values.
+ * ADV_SCSI_REQ_Q 'done_status' and 'host_status' return values.
  */
 
 #define QD_NO_STATUS         0x00	/* Request not completed yet. */
@@ -2153,8 +2059,6 @@ do { \
 #define QHSTA_M_SGBACKUP_ERROR      0x47	/* Scatter-Gather backup error */
 
 /* Return the address that is aligned at the next doubleword >= to 'addr'. */
-#define ADV_8BALIGN(addr)      (((ulong) (addr) + 0x7) & ~0x7)
-#define ADV_16BALIGN(addr)     (((ulong) (addr) + 0xF) & ~0xF)
 #define ADV_32BALIGN(addr)     (((ulong) (addr) + 0x1F) & ~0x1F)
 
 /*
@@ -2315,24 +2219,24 @@ do { \
 /* Per board statistics structure */
 struct asc_stats {
 	/* Driver Entrypoint Statistics */
-	ADV_DCNT queuecommand;	/* # calls to advansys_queuecommand() */
-	ADV_DCNT reset;		/* # calls to advansys_eh_bus_reset() */
-	ADV_DCNT biosparam;	/* # calls to advansys_biosparam() */
-	ADV_DCNT interrupt;	/* # advansys_interrupt() calls */
-	ADV_DCNT callback;	/* # calls to asc/adv_isr_callback() */
-	ADV_DCNT done;		/* # calls to request's scsi_done function */
-	ADV_DCNT build_error;	/* # asc/adv_build_req() ASC_ERROR returns. */
-	ADV_DCNT adv_build_noreq;	/* # adv_build_req() adv_req_t alloc. fail. */
-	ADV_DCNT adv_build_nosg;	/* # adv_build_req() adv_sgblk_t alloc. fail. */
+	unsigned int queuecommand;	/* # calls to advansys_queuecommand() */
+	unsigned int reset;		/* # calls to advansys_eh_bus_reset() */
+	unsigned int biosparam;	/* # calls to advansys_biosparam() */
+	unsigned int interrupt;	/* # advansys_interrupt() calls */
+	unsigned int callback;	/* # calls to asc/adv_isr_callback() */
+	unsigned int done;		/* # calls to request's scsi_done function */
+	unsigned int build_error;	/* # asc/adv_build_req() ASC_ERROR returns. */
+	unsigned int adv_build_noreq;	/* # adv_build_req() adv_req_t alloc. fail. */
+	unsigned int adv_build_nosg;	/* # adv_build_req() adv_sgblk_t alloc. fail. */
 	/* AscExeScsiQueue()/AdvExeScsiQueue() Statistics */
-	ADV_DCNT exe_noerror;	/* # ASC_NOERROR returns. */
-	ADV_DCNT exe_busy;	/* # ASC_BUSY returns. */
-	ADV_DCNT exe_error;	/* # ASC_ERROR returns. */
-	ADV_DCNT exe_unknown;	/* # unknown returns. */
+	unsigned int exe_noerror;	/* # ASC_NOERROR returns. */
+	unsigned int exe_busy;	/* # ASC_BUSY returns. */
+	unsigned int exe_error;	/* # ASC_ERROR returns. */
+	unsigned int exe_unknown;	/* # unknown returns. */
 	/* Data Transfer Statistics */
-	ADV_DCNT xfer_cnt;	/* # I/O requests received */
-	ADV_DCNT xfer_elem;	/* # scatter-gather elements */
-	ADV_DCNT xfer_sect;	/* # 512-byte blocks */
+	unsigned int xfer_cnt;	/* # I/O requests received */
+	unsigned int xfer_elem;	/* # scatter-gather elements */
+	unsigned int xfer_sect;	/* # 512-byte blocks */
 };
 #endif /* ADVANSYS_STATS */
 
@@ -2345,6 +2249,7 @@ struct asc_stats {
  */
 struct asc_board {
 	struct device *dev;
+	struct Scsi_Host *shost;
 	uint flags;		/* Board flags */
 	unsigned int irq;
 	union {
@@ -2366,7 +2271,6 @@ struct asc_board {
 		ADVEEP_38C0800_CONFIG adv_38C0800_eep;	/* 38C0800 EEPROM config. */
 		ADVEEP_38C1600_CONFIG adv_38C1600_eep;	/* 38C1600 EEPROM config. */
 	} eep_config;
-	ulong last_reset;	/* Saved last reset time */
 	/* /proc/scsi/advansys/[0...] */
 #ifdef ADVANSYS_STATS
 	struct asc_stats asc_stats;	/* Board statistics */
@@ -2381,7 +2285,9 @@ struct asc_board {
 	void __iomem *ioremap_addr;	/* I/O Memory remap address. */
 	ushort ioport;		/* I/O Port address. */
 	adv_req_t *adv_reqp;	/* Request structures. */
-	adv_sgblk_t *adv_sgblkp;	/* Scatter-gather structures. */
+	dma_addr_t adv_reqp_addr;
+	size_t adv_reqp_size;
+	struct dma_pool *adv_sgblk_pool;	/* Scatter-gather structures. */
 	ushort bios_signature;	/* BIOS Signature. */
 	ushort bios_version;	/* BIOS Version. */
 	ushort bios_codeseg;	/* BIOS Code Segment. */
@@ -2470,12 +2376,11 @@ static void asc_prt_adv_dvc_var(ADV_DVC_VAR *h)
 	printk("  start_motor 0x%x, scsi_reset_wait 0x%x\n",
 	       (unsigned)h->start_motor, (unsigned)h->scsi_reset_wait);
 
-	printk("  max_host_qng %u, max_dvc_qng %u, carr_freelist 0x%lxn\n",
+	printk("  max_host_qng %u, max_dvc_qng %u, carr_freelist 0x%p\n",
 	       (unsigned)h->max_host_qng, (unsigned)h->max_dvc_qng,
-	       (ulong)h->carr_freelist);
+	       h->carr_freelist);
 
-	printk("  icq_sp 0x%lx, irq_sp 0x%lx\n",
-	       (ulong)h->icq_sp, (ulong)h->irq_sp);
+	printk("  icq_sp 0x%p, irq_sp 0x%p\n", h->icq_sp, h->irq_sp);
 
 	printk("  no_scam 0x%x, tagqng_able 0x%x\n",
 	       (unsigned)h->no_scam, (unsigned)h->tagqng_able);
@@ -2600,8 +2505,8 @@ static void asc_prt_asc_scsi_q(ASC_SCSI_Q *q)
 	printk("ASC_SCSI_Q at addr 0x%lx\n", (ulong)q);
 
 	printk
-	    (" target_ix 0x%x, target_lun %u, srb_ptr 0x%lx, tag_code 0x%x,\n",
-	     q->q2.target_ix, q->q1.target_lun, (ulong)q->q2.srb_ptr,
+	    (" target_ix 0x%x, target_lun %u, srb_tag 0x%x, tag_code 0x%x,\n",
+	     q->q2.target_ix, q->q1.target_lun, q->q2.srb_tag,
 	     q->q2.tag_code);
 
 	printk
@@ -2634,8 +2539,8 @@ static void asc_prt_asc_scsi_q(ASC_SCSI_Q *q)
 static void asc_prt_asc_qdone_info(ASC_QDONE_INFO *q)
 {
 	printk("ASC_QDONE_INFO at addr 0x%lx\n", (ulong)q);
-	printk(" srb_ptr 0x%lx, target_ix %u, cdb_len %u, tag_code %u,\n",
-	       (ulong)q->d2.srb_ptr, q->d2.target_ix, q->d2.cdb_len,
+	printk(" srb_tag 0x%x, target_ix %u, cdb_len %u, tag_code %u,\n",
+	       q->d2.srb_tag, q->d2.target_ix, q->d2.cdb_len,
 	       q->d2.tag_code);
 	printk
 	    (" done_stat 0x%x, host_stat 0x%x, scsi_stat 0x%x, scsi_msg 0x%x\n",
@@ -2651,17 +2556,17 @@ static void asc_prt_adv_sgblock(int sgblockno, ADV_SG_BLOCK *b)
 {
 	int i;
 
-	printk(" ASC_SG_BLOCK at addr 0x%lx (sgblockno %d)\n",
+	printk(" ADV_SG_BLOCK at addr 0x%lx (sgblockno %d)\n",
 	       (ulong)b, sgblockno);
-	printk("  sg_cnt %u, sg_ptr 0x%lx\n",
-	       b->sg_cnt, (ulong)le32_to_cpu(b->sg_ptr));
+	printk("  sg_cnt %u, sg_ptr 0x%x\n",
+	       b->sg_cnt, (u32)le32_to_cpu(b->sg_ptr));
 	BUG_ON(b->sg_cnt > NO_OF_SG_PER_BLOCK);
 	if (b->sg_ptr != 0)
 		BUG_ON(b->sg_cnt != NO_OF_SG_PER_BLOCK);
 	for (i = 0; i < b->sg_cnt; i++) {
-		printk("  [%u]: sg_addr 0x%lx, sg_count 0x%lx\n",
-		       i, (ulong)b->sg_list[i].sg_addr,
-		       (ulong)b->sg_list[i].sg_count);
+		printk("  [%u]: sg_addr 0x%x, sg_count 0x%x\n",
+		       i, (u32)le32_to_cpu(b->sg_list[i].sg_addr),
+		       (u32)le32_to_cpu(b->sg_list[i].sg_count));
 	}
 }
 
@@ -2673,15 +2578,16 @@ static void asc_prt_adv_sgblock(int sgblockno, ADV_SG_BLOCK *b)
 static void asc_prt_adv_scsi_req_q(ADV_SCSI_REQ_Q *q)
 {
 	int sg_blk_cnt;
-	struct asc_sg_block *sg_ptr;
+	struct adv_sg_block *sg_ptr;
+	adv_sgblk_t *sgblkp;
 
 	printk("ADV_SCSI_REQ_Q at addr 0x%lx\n", (ulong)q);
 
-	printk("  target_id %u, target_lun %u, srb_ptr 0x%lx, a_flag 0x%x\n",
-	       q->target_id, q->target_lun, (ulong)q->srb_ptr, q->a_flag);
+	printk("  target_id %u, target_lun %u, srb_tag 0x%x\n",
+	       q->target_id, q->target_lun, q->srb_tag);
 
-	printk("  cntl 0x%x, data_addr 0x%lx, vdata_addr 0x%lx\n",
-	       q->cntl, (ulong)le32_to_cpu(q->data_addr), (ulong)q->vdata_addr);
+	printk("  cntl 0x%x, data_addr 0x%lx\n",
+	       q->cntl, (ulong)le32_to_cpu(q->data_addr));
 
 	printk("  data_cnt %lu, sense_addr 0x%lx, sense_len %u,\n",
 	       (ulong)le32_to_cpu(q->data_cnt),
@@ -2700,21 +2606,15 @@ static void asc_prt_adv_scsi_req_q(ADV_SCSI_REQ_Q *q)
 
 	/* Display the request's ADV_SG_BLOCK structures. */
 	if (q->sg_list_ptr != NULL) {
+		sgblkp = container_of(q->sg_list_ptr, adv_sgblk_t, sg_block);
 		sg_blk_cnt = 0;
-		while (1) {
-			/*
-			 * 'sg_ptr' is a physical address. Convert it to a virtual
-			 * address by indexing 'sg_blk_cnt' into the virtual address
-			 * array 'sg_list_ptr'.
-			 *
-			 * XXX - Assumes all SG physical blocks are virtually contiguous.
-			 */
-			sg_ptr =
-			    &(((ADV_SG_BLOCK *)(q->sg_list_ptr))[sg_blk_cnt]);
+		while (sgblkp) {
+			sg_ptr = &sgblkp->sg_block;
 			asc_prt_adv_sgblock(sg_blk_cnt, sg_ptr);
 			if (sg_ptr->sg_ptr == 0) {
 				break;
 			}
+			sgblkp = sgblkp->next_sgblkp;
 			sg_blk_cnt++;
 		}
 	}
@@ -2722,59 +2622,6 @@ static void asc_prt_adv_scsi_req_q(ADV_SCSI_REQ_Q *q)
 #endif /* ADVANSYS_DEBUG */
 
 /*
- * The advansys chip/microcode contains a 32-bit identifier for each command
- * known as the 'srb'.  I don't know what it stands for.  The driver used
- * to encode the scsi_cmnd pointer by calling virt_to_bus and retrieve it
- * with bus_to_virt.  Now the driver keeps a per-host map of integers to
- * pointers.  It auto-expands when full, unless it can't allocate memory.
- * Note that an srb of 0 is treated specially by the chip/firmware, hence
- * the return of i+1 in this routine, and the corresponding subtraction in
- * the inverse routine.
- */
-#define BAD_SRB 0
-static u32 advansys_ptr_to_srb(struct asc_dvc_var *asc_dvc, void *ptr)
-{
-	int i;
-	void **new_ptr;
-
-	for (i = 0; i < asc_dvc->ptr_map_count; i++) {
-		if (!asc_dvc->ptr_map[i])
-			goto out;
-	}
-
-	if (asc_dvc->ptr_map_count == 0)
-		asc_dvc->ptr_map_count = 1;
-	else
-		asc_dvc->ptr_map_count *= 2;
-
-	new_ptr = krealloc(asc_dvc->ptr_map,
-			asc_dvc->ptr_map_count * sizeof(void *), GFP_ATOMIC);
-	if (!new_ptr)
-		return BAD_SRB;
-	asc_dvc->ptr_map = new_ptr;
- out:
-	ASC_DBG(3, "Putting ptr %p into array offset %d\n", ptr, i);
-	asc_dvc->ptr_map[i] = ptr;
-	return i + 1;
-}
-
-static void * advansys_srb_to_ptr(struct asc_dvc_var *asc_dvc, u32 srb)
-{
-	void *ptr;
-
-	srb--;
-	if (srb >= asc_dvc->ptr_map_count) {
-		printk("advansys: bad SRB %u, max %u\n", srb,
-							asc_dvc->ptr_map_count);
-		return NULL;
-	}
-	ptr = asc_dvc->ptr_map[srb];
-	asc_dvc->ptr_map[srb] = NULL;
-	ASC_DBG(3, "Returning ptr %p from array offset %d\n", ptr, srb);
-	return ptr;
-}
-
-/*
  * advansys_info()
  *
  * Return suitable for printing on the console with the argument
@@ -3350,7 +3197,7 @@ static void asc_prt_driver_conf(struct seq_file *m, struct Scsi_Host *shost)
 
 	seq_printf(m,
 		   " flags 0x%x, last_reset 0x%lx, jiffies 0x%lx, asc_n_io_port 0x%x\n",
-		   boardp->flags, boardp->last_reset, jiffies,
+		   boardp->flags, shost->last_reset, jiffies,
 		   boardp->asc_n_io_port);
 
 	seq_printf(m, " io_port 0x%lx\n", shost->io_port);
@@ -3844,7 +3691,7 @@ static int AscStartChip(PortAddr iop_base)
 	return (1);
 }
 
-static int AscStopChip(PortAddr iop_base)
+static bool AscStopChip(PortAddr iop_base)
 {
 	uchar cc_val;
 
@@ -3855,22 +3702,22 @@ static int AscStopChip(PortAddr iop_base)
 	AscSetChipIH(iop_base, INS_HALT);
 	AscSetChipIH(iop_base, INS_RFLAG_WTM);
 	if ((AscGetChipStatus(iop_base) & CSW_HALTED) == 0) {
-		return (0);
+		return false;
 	}
-	return (1);
+	return true;
 }
 
-static int AscIsChipHalted(PortAddr iop_base)
+static bool AscIsChipHalted(PortAddr iop_base)
 {
 	if ((AscGetChipStatus(iop_base) & CSW_HALTED) != 0) {
 		if ((AscGetChipControl(iop_base) & CC_HALT) != 0) {
-			return (1);
+			return true;
 		}
 	}
-	return (0);
+	return false;
 }
 
-static int AscResetChipAndScsiBus(ASC_DVC_VAR *asc_dvc)
+static bool AscResetChipAndScsiBus(ASC_DVC_VAR *asc_dvc)
 {
 	PortAddr iop_base;
 	int i = 10;
@@ -3953,20 +3800,6 @@ static ushort AscReadLramWord(PortAddr iop_base, ushort addr)
 	return (word_data);
 }
 
-#if CC_VERY_LONG_SG_LIST
-static ASC_DCNT AscReadLramDWord(PortAddr iop_base, ushort addr)
-{
-	ushort val_low, val_high;
-	ASC_DCNT dword_data;
-
-	AscSetChipLramAddr(iop_base, addr);
-	val_low = AscGetChipLramData(iop_base);
-	val_high = AscGetChipLramData(iop_base);
-	dword_data = ((ASC_DCNT) val_high << 16) | (ASC_DCNT) val_low;
-	return (dword_data);
-}
-#endif /* CC_VERY_LONG_SG_LIST */
-
 static void
 AscMemWordSetLram(PortAddr iop_base, ushort s_addr, ushort set_wval, int words)
 {
@@ -4068,27 +3901,24 @@ AscMemWordCopyPtrFromLram(PortAddr iop_base,
 	}
 }
 
-static ASC_DCNT AscMemSumLramWord(PortAddr iop_base, ushort s_addr, int words)
+static u32 AscMemSumLramWord(PortAddr iop_base, ushort s_addr, int words)
 {
-	ASC_DCNT sum;
+	u32 sum = 0;
 	int i;
 
-	sum = 0L;
 	for (i = 0; i < words; i++, s_addr += 2) {
 		sum += AscReadLramWord(iop_base, s_addr);
 	}
 	return (sum);
 }
 
-static ushort AscInitLram(ASC_DVC_VAR *asc_dvc)
+static void AscInitLram(ASC_DVC_VAR *asc_dvc)
 {
 	uchar i;
 	ushort s_addr;
 	PortAddr iop_base;
-	ushort warn_code;
 
 	iop_base = asc_dvc->iop_base;
-	warn_code = 0;
 	AscMemWordSetLram(iop_base, ASC_QADR_BEG, 0,
 			  (ushort)(((int)(asc_dvc->max_total_qng + 2 + 1) *
 				    64) >> 1));
@@ -4127,14 +3957,13 @@ static ushort AscInitLram(ASC_DVC_VAR *asc_dvc)
 		AscWriteLramByte(iop_base,
 				 (ushort)(s_addr + (ushort)ASC_SCSIQ_B_QNO), i);
 	}
-	return warn_code;
 }
 
-static ASC_DCNT
+static u32
 AscLoadMicroCode(PortAddr iop_base, ushort s_addr,
 		 const uchar *mcode_buf, ushort mcode_size)
 {
-	ASC_DCNT chksum;
+	u32 chksum;
 	ushort mcode_word_size;
 	ushort mcode_chksum;
 
@@ -4186,13 +4015,13 @@ static void AscInitQLinkVar(ASC_DVC_VAR *asc_dvc)
 	}
 }
 
-static ushort AscInitMicroCodeVar(ASC_DVC_VAR *asc_dvc)
+static int AscInitMicroCodeVar(ASC_DVC_VAR *asc_dvc)
 {
 	int i;
-	ushort warn_code;
+	int warn_code;
 	PortAddr iop_base;
-	ASC_PADDR phy_addr;
-	ASC_DCNT phy_size;
+	__le32 phy_addr;
+	__le32 phy_size;
 	struct asc_board *board = asc_dvc_to_board(asc_dvc);
 
 	iop_base = asc_dvc->iop_base;
@@ -4231,12 +4060,12 @@ static ushort AscInitMicroCodeVar(ASC_DVC_VAR *asc_dvc)
 	AscSetPCAddr(iop_base, ASC_MCODE_START_ADDR);
 	if (AscGetPCAddr(iop_base) != ASC_MCODE_START_ADDR) {
 		asc_dvc->err_code |= ASC_IERR_SET_PC_ADDR;
-		warn_code = UW_ERR;
+		warn_code = -EINVAL;
 		goto err_mcode_start;
 	}
 	if (AscStartChip(iop_base) != 1) {
 		asc_dvc->err_code |= ASC_IERR_START_STOP_CHIP;
-		warn_code = UW_ERR;
+		warn_code = -EIO;
 		goto err_mcode_start;
 	}
 
@@ -4250,13 +4079,13 @@ err_dma_map:
 	return warn_code;
 }
 
-static ushort AscInitAsc1000Driver(ASC_DVC_VAR *asc_dvc)
+static int AscInitAsc1000Driver(ASC_DVC_VAR *asc_dvc)
 {
 	const struct firmware *fw;
 	const char fwname[] = "advansys/mcode.bin";
 	int err;
 	unsigned long chksum;
-	ushort warn_code;
+	int warn_code;
 	PortAddr iop_base;
 
 	iop_base = asc_dvc->iop_base;
@@ -4268,15 +4097,13 @@ static ushort AscInitAsc1000Driver(ASC_DVC_VAR *asc_dvc)
 	}
 	asc_dvc->init_state |= ASC_INIT_STATE_BEG_LOAD_MC;
 	if (asc_dvc->err_code != 0)
-		return UW_ERR;
+		return ASC_ERROR;
 	if (!AscFindSignature(asc_dvc->iop_base)) {
 		asc_dvc->err_code = ASC_IERR_BAD_SIGNATURE;
 		return warn_code;
 	}
 	AscDisableInterrupt(iop_base);
-	warn_code |= AscInitLram(asc_dvc);
-	if (asc_dvc->err_code != 0)
-		return UW_ERR;
+	AscInitLram(asc_dvc);
 
 	err = request_firmware(&fw, fwname, asc_dvc->drv_ptr->dev);
 	if (err) {
@@ -4336,7 +4163,7 @@ static int AdvLoadMicrocode(AdvPortAddr iop_base, const unsigned char *buf,
 			    int size, int memsize, int chksum)
 {
 	int i, j, end, len = 0;
-	ADV_DCNT sum;
+	u32 sum;
 
 	AdvWriteWordRegister(iop_base, IOPW_RAM_ADDR, 0);
 
@@ -4382,38 +4209,72 @@ static int AdvLoadMicrocode(AdvPortAddr iop_base, const unsigned char *buf,
 	return 0;
 }
 
-static void AdvBuildCarrierFreelist(struct adv_dvc_var *asc_dvc)
+static void AdvBuildCarrierFreelist(struct adv_dvc_var *adv_dvc)
 {
-	ADV_CARR_T *carrp;
-	ADV_SDCNT buf_size;
-	ADV_PADDR carr_paddr;
+	off_t carr_offset = 0, next_offset;
+	dma_addr_t carr_paddr;
+	int carr_num = ADV_CARRIER_BUFSIZE / sizeof(ADV_CARR_T), i;
 
-	carrp = (ADV_CARR_T *) ADV_16BALIGN(asc_dvc->carrier_buf);
-	asc_dvc->carr_freelist = NULL;
-	if (carrp == asc_dvc->carrier_buf) {
-		buf_size = ADV_CARRIER_BUFSIZE;
-	} else {
-		buf_size = ADV_CARRIER_BUFSIZE - sizeof(ADV_CARR_T);
+	for (i = 0; i < carr_num; i++) {
+		carr_offset = i * sizeof(ADV_CARR_T);
+		/* Get physical address of the carrier 'carrp'. */
+		carr_paddr = adv_dvc->carrier_addr + carr_offset;
+
+		adv_dvc->carrier[i].carr_pa = cpu_to_le32(carr_paddr);
+		adv_dvc->carrier[i].carr_va = cpu_to_le32(carr_offset);
+		adv_dvc->carrier[i].areq_vpa = 0;
+		next_offset = carr_offset + sizeof(ADV_CARR_T);
+		if (i == carr_num)
+			next_offset = ~0;
+		adv_dvc->carrier[i].next_vpa = cpu_to_le32(next_offset);
 	}
+	/*
+	 * We cannot have a carrier with 'carr_va' of '0', as
+	 * a reference to this carrier would be interpreted as
+	 * list termination.
+	 * So start at carrier 1 with the freelist.
+	 */
+	adv_dvc->carr_freelist = &adv_dvc->carrier[1];
+}
 
-	do {
-		/* Get physical address of the carrier 'carrp'. */
-		carr_paddr = cpu_to_le32(virt_to_bus(carrp));
+static ADV_CARR_T *adv_get_carrier(struct adv_dvc_var *adv_dvc, u32 offset)
+{
+	int index;
 
-		buf_size -= sizeof(ADV_CARR_T);
+	BUG_ON(offset > ADV_CARRIER_BUFSIZE);
 
-		carrp->carr_pa = carr_paddr;
-		carrp->carr_va = cpu_to_le32(ADV_VADDR_TO_U32(carrp));
+	index = offset / sizeof(ADV_CARR_T);
+	return &adv_dvc->carrier[index];
+}
 
-		/*
-		 * Insert the carrier at the beginning of the freelist.
-		 */
-		carrp->next_vpa =
-			cpu_to_le32(ADV_VADDR_TO_U32(asc_dvc->carr_freelist));
-		asc_dvc->carr_freelist = carrp;
+static ADV_CARR_T *adv_get_next_carrier(struct adv_dvc_var *adv_dvc)
+{
+	ADV_CARR_T *carrp = adv_dvc->carr_freelist;
+	u32 next_vpa = le32_to_cpu(carrp->next_vpa);
+
+	if (next_vpa == 0 || next_vpa == ~0) {
+		ASC_DBG(1, "invalid vpa offset 0x%x\n", next_vpa);
+		return NULL;
+	}
+
+	adv_dvc->carr_freelist = adv_get_carrier(adv_dvc, next_vpa);
+	/*
+	 * insert stopper carrier to terminate list
+	 */
+	carrp->next_vpa = cpu_to_le32(ADV_CQ_STOPPER);
+
+	return carrp;
+}
+
+/*
+ * 'offset' is the index in the request pointer array
+ */
+static adv_req_t * adv_get_reqp(struct adv_dvc_var *adv_dvc, u32 offset)
+{
+	struct asc_board *boardp = adv_dvc->drv_ptr;
 
-		carrp++;
-	} while (buf_size > 0);
+	BUG_ON(offset > adv_dvc->max_host_qng);
+	return &boardp->adv_reqp[offset];
 }
 
 /*
@@ -4432,10 +4293,9 @@ static void AdvBuildCarrierFreelist(struct adv_dvc_var *asc_dvc)
  */
 static int
 AdvSendIdleCmd(ADV_DVC_VAR *asc_dvc,
-	       ushort idle_cmd, ADV_DCNT idle_cmd_parameter)
+	       ushort idle_cmd, u32 idle_cmd_parameter)
 {
-	int result;
-	ADV_DCNT i, j;
+	int result, i, j;
 	AdvPortAddr iop_base;
 
 	iop_base = asc_dvc->iop_base;
@@ -4902,17 +4762,11 @@ static int AdvInitAsc3550Driver(ADV_DVC_VAR *asc_dvc)
 	 * Set-up the Host->RISC Initiator Command Queue (ICQ).
 	 */
 
-	if ((asc_dvc->icq_sp = asc_dvc->carr_freelist) == NULL) {
+	asc_dvc->icq_sp = adv_get_next_carrier(asc_dvc);
+	if (!asc_dvc->icq_sp) {
 		asc_dvc->err_code |= ASC_IERR_NO_CARRIER;
 		return ADV_ERROR;
 	}
-	asc_dvc->carr_freelist = (ADV_CARR_T *)
-	    ADV_U32_TO_VADDR(le32_to_cpu(asc_dvc->icq_sp->next_vpa));
-
-	/*
-	 * The first command issued will be placed in the stopper carrier.
-	 */
-	asc_dvc->icq_sp->next_vpa = cpu_to_le32(ASC_CQ_STOPPER);
 
 	/*
 	 * Set RISC ICQ physical address start value.
@@ -4922,21 +4776,11 @@ static int AdvInitAsc3550Driver(ADV_DVC_VAR *asc_dvc)
 	/*
 	 * Set-up the RISC->Host Initiator Response Queue (IRQ).
 	 */
-	if ((asc_dvc->irq_sp = asc_dvc->carr_freelist) == NULL) {
+	asc_dvc->irq_sp = adv_get_next_carrier(asc_dvc);
+	if (!asc_dvc->irq_sp) {
 		asc_dvc->err_code |= ASC_IERR_NO_CARRIER;
 		return ADV_ERROR;
 	}
-	asc_dvc->carr_freelist = (ADV_CARR_T *)
-	    ADV_U32_TO_VADDR(le32_to_cpu(asc_dvc->irq_sp->next_vpa));
-
-	/*
-	 * The first command completed by the RISC will be placed in
-	 * the stopper.
-	 *
-	 * Note: Set 'next_vpa' to ASC_CQ_STOPPER. When the request is
-	 * completed the RISC will set the ASC_RQ_STOPPER bit.
-	 */
-	asc_dvc->irq_sp->next_vpa = cpu_to_le32(ASC_CQ_STOPPER);
 
 	/*
 	 * Set RISC IRQ physical address start value.
@@ -5399,17 +5243,12 @@ static int AdvInitAsc38C0800Driver(ADV_DVC_VAR *asc_dvc)
 	 * Set-up the Host->RISC Initiator Command Queue (ICQ).
 	 */
 
-	if ((asc_dvc->icq_sp = asc_dvc->carr_freelist) == NULL) {
+	asc_dvc->icq_sp = adv_get_next_carrier(asc_dvc);
+	if (!asc_dvc->icq_sp) {
+		ASC_DBG(0, "Failed to get ICQ carrier\n");
 		asc_dvc->err_code |= ASC_IERR_NO_CARRIER;
 		return ADV_ERROR;
 	}
-	asc_dvc->carr_freelist = (ADV_CARR_T *)
-	    ADV_U32_TO_VADDR(le32_to_cpu(asc_dvc->icq_sp->next_vpa));
-
-	/*
-	 * The first command issued will be placed in the stopper carrier.
-	 */
-	asc_dvc->icq_sp->next_vpa = cpu_to_le32(ASC_CQ_STOPPER);
 
 	/*
 	 * Set RISC ICQ physical address start value.
@@ -5420,21 +5259,12 @@ static int AdvInitAsc38C0800Driver(ADV_DVC_VAR *asc_dvc)
 	/*
 	 * Set-up the RISC->Host Initiator Response Queue (IRQ).
 	 */
-	if ((asc_dvc->irq_sp = asc_dvc->carr_freelist) == NULL) {
+	asc_dvc->irq_sp = adv_get_next_carrier(asc_dvc);
+	if (!asc_dvc->irq_sp) {
+		ASC_DBG(0, "Failed to get IRQ carrier\n");
 		asc_dvc->err_code |= ASC_IERR_NO_CARRIER;
 		return ADV_ERROR;
 	}
-	asc_dvc->carr_freelist = (ADV_CARR_T *)
-	    ADV_U32_TO_VADDR(le32_to_cpu(asc_dvc->irq_sp->next_vpa));
-
-	/*
-	 * The first command completed by the RISC will be placed in
-	 * the stopper.
-	 *
-	 * Note: Set 'next_vpa' to ASC_CQ_STOPPER. When the request is
-	 * completed the RISC will set the ASC_RQ_STOPPER bit.
-	 */
-	asc_dvc->irq_sp->next_vpa = cpu_to_le32(ASC_CQ_STOPPER);
 
 	/*
 	 * Set RISC IRQ physical address start value.
@@ -5909,17 +5739,11 @@ static int AdvInitAsc38C1600Driver(ADV_DVC_VAR *asc_dvc)
 	/*
 	 * Set-up the Host->RISC Initiator Command Queue (ICQ).
 	 */
-	if ((asc_dvc->icq_sp = asc_dvc->carr_freelist) == NULL) {
+	asc_dvc->icq_sp = adv_get_next_carrier(asc_dvc);
+	if (!asc_dvc->icq_sp) {
 		asc_dvc->err_code |= ASC_IERR_NO_CARRIER;
 		return ADV_ERROR;
 	}
-	asc_dvc->carr_freelist = (ADV_CARR_T *)
-	    ADV_U32_TO_VADDR(le32_to_cpu(asc_dvc->icq_sp->next_vpa));
-
-	/*
-	 * The first command issued will be placed in the stopper carrier.
-	 */
-	asc_dvc->icq_sp->next_vpa = cpu_to_le32(ASC_CQ_STOPPER);
 
 	/*
 	 * Set RISC ICQ physical address start value. Initialize the
@@ -5933,21 +5757,11 @@ static int AdvInitAsc38C1600Driver(ADV_DVC_VAR *asc_dvc)
 	/*
 	 * Set-up the RISC->Host Initiator Response Queue (IRQ).
 	 */
-	if ((asc_dvc->irq_sp = asc_dvc->carr_freelist) == NULL) {
+	asc_dvc->irq_sp = adv_get_next_carrier(asc_dvc);
+	if (!asc_dvc->irq_sp) {
 		asc_dvc->err_code |= ASC_IERR_NO_CARRIER;
 		return ADV_ERROR;
 	}
-	asc_dvc->carr_freelist = (ADV_CARR_T *)
-	    ADV_U32_TO_VADDR(le32_to_cpu(asc_dvc->irq_sp->next_vpa));
-
-	/*
-	 * The first command completed by the RISC will be placed in
-	 * the stopper.
-	 *
-	 * Note: Set 'next_vpa' to ASC_CQ_STOPPER. When the request is
-	 * completed the RISC will set the ASC_RQ_STOPPER bit.
-	 */
-	asc_dvc->irq_sp->next_vpa = cpu_to_le32(ASC_CQ_STOPPER);
 
 	/*
 	 * Set RISC IRQ physical address start value.
@@ -6134,15 +5948,16 @@ static void adv_async_callback(ADV_DVC_VAR *adv_dvc_varp, uchar code)
  */
 static void adv_isr_callback(ADV_DVC_VAR *adv_dvc_varp, ADV_SCSI_REQ_Q *scsiqp)
 {
-	struct asc_board *boardp;
+	struct asc_board *boardp = adv_dvc_varp->drv_ptr;
+	u32 srb_tag;
 	adv_req_t *reqp;
 	adv_sgblk_t *sgblkp;
 	struct scsi_cmnd *scp;
-	struct Scsi_Host *shost;
-	ADV_DCNT resid_cnt;
+	u32 resid_cnt;
+	dma_addr_t sense_addr;
 
-	ASC_DBG(1, "adv_dvc_varp 0x%lx, scsiqp 0x%lx\n",
-		 (ulong)adv_dvc_varp, (ulong)scsiqp);
+	ASC_DBG(1, "adv_dvc_varp 0x%p, scsiqp 0x%p\n",
+		adv_dvc_varp, scsiqp);
 	ASC_DBG_PRT_ADV_SCSI_REQ_Q(2, scsiqp);
 
 	/*
@@ -6150,22 +5965,9 @@ static void adv_isr_callback(ADV_DVC_VAR *adv_dvc_varp, ADV_SCSI_REQ_Q *scsiqp)
 	 * completed. The adv_req_t structure actually contains the
 	 * completed ADV_SCSI_REQ_Q structure.
 	 */
-	reqp = (adv_req_t *)ADV_U32_TO_VADDR(scsiqp->srb_ptr);
-	ASC_DBG(1, "reqp 0x%lx\n", (ulong)reqp);
-	if (reqp == NULL) {
-		ASC_PRINT("adv_isr_callback: reqp is NULL\n");
-		return;
-	}
+	srb_tag = le32_to_cpu(scsiqp->srb_tag);
+	scp = scsi_host_find_tag(boardp->shost, scsiqp->srb_tag);
 
-	/*
-	 * Get the struct scsi_cmnd structure and Scsi_Host structure for the
-	 * command that has been completed.
-	 *
-	 * Note: The adv_req_t request structure and adv_sgblk_t structure,
-	 * if any, are dropped, because a board structure pointer can not be
-	 * determined.
-	 */
-	scp = reqp->cmndp;
 	ASC_DBG(1, "scp 0x%p\n", scp);
 	if (scp == NULL) {
 		ASC_PRINT
@@ -6174,12 +5976,25 @@ static void adv_isr_callback(ADV_DVC_VAR *adv_dvc_varp, ADV_SCSI_REQ_Q *scsiqp)
 	}
 	ASC_DBG_PRT_CDB(2, scp->cmnd, scp->cmd_len);
 
-	shost = scp->device->host;
-	ASC_STATS(shost, callback);
-	ASC_DBG(1, "shost 0x%p\n", shost);
+	reqp = (adv_req_t *)scp->host_scribble;
+	ASC_DBG(1, "reqp 0x%lx\n", (ulong)reqp);
+	if (reqp == NULL) {
+		ASC_PRINT("adv_isr_callback: reqp is NULL\n");
+		return;
+	}
+	/*
+	 * Remove backreferences to avoid duplicate
+	 * command completions.
+	 */
+	scp->host_scribble = NULL;
+	reqp->cmndp = NULL;
+
+	ASC_STATS(boardp->shost, callback);
+	ASC_DBG(1, "shost 0x%p\n", boardp->shost);
 
-	boardp = shost_priv(shost);
-	BUG_ON(adv_dvc_varp != &boardp->dvc_var.adv_dvc_var);
+	sense_addr = le32_to_cpu(scsiqp->sense_addr);
+	dma_unmap_single(boardp->dev, sense_addr,
+			 SCSI_SENSE_BUFFERSIZE, DMA_FROM_DEVICE);
 
 	/*
 	 * 'done_status' contains the command's ending status.
@@ -6272,18 +6087,10 @@ static void adv_isr_callback(ADV_DVC_VAR *adv_dvc_varp, ADV_SCSI_REQ_Q *scsiqp)
 		/* Remove 'sgblkp' from the request list. */
 		reqp->sgblkp = sgblkp->next_sgblkp;
 
-		/* Add 'sgblkp' to the board free list. */
-		sgblkp->next_sgblkp = boardp->adv_sgblkp;
-		boardp->adv_sgblkp = sgblkp;
+		dma_pool_free(boardp->adv_sgblk_pool, sgblkp,
+			      sgblkp->sg_addr);
 	}
 
-	/*
-	 * Free the adv_req_t structure used with the command by adding
-	 * it back to the board free list.
-	 */
-	reqp->next_reqp = boardp->adv_reqp;
-	boardp->adv_reqp = reqp;
-
 	ASC_DBG(1, "done\n");
 }
 
@@ -6312,8 +6119,9 @@ static int AdvISR(ADV_DVC_VAR *asc_dvc)
 	uchar int_stat;
 	ushort target_bit;
 	ADV_CARR_T *free_carrp;
-	ADV_VADDR irq_next_vpa;
+	__le32 irq_next_vpa;
 	ADV_SCSI_REQ_Q *scsiq;
+	adv_req_t *reqp;
 
 	iop_base = asc_dvc->iop_base;
 
@@ -6356,25 +6164,28 @@ static int AdvISR(ADV_DVC_VAR *asc_dvc)
 	 * Check if the IRQ stopper carrier contains a completed request.
 	 */
 	while (((irq_next_vpa =
-		 le32_to_cpu(asc_dvc->irq_sp->next_vpa)) & ASC_RQ_DONE) != 0) {
+		 le32_to_cpu(asc_dvc->irq_sp->next_vpa)) & ADV_RQ_DONE) != 0) {
 		/*
 		 * Get a pointer to the newly completed ADV_SCSI_REQ_Q structure.
 		 * The RISC will have set 'areq_vpa' to a virtual address.
 		 *
-		 * The firmware will have copied the ASC_SCSI_REQ_Q.scsiq_ptr
+		 * The firmware will have copied the ADV_SCSI_REQ_Q.scsiq_ptr
 		 * field to the carrier ADV_CARR_T.areq_vpa field. The conversion
-		 * below complements the conversion of ASC_SCSI_REQ_Q.scsiq_ptr'
+		 * below complements the conversion of ADV_SCSI_REQ_Q.scsiq_ptr'
 		 * in AdvExeScsiQueue().
 		 */
-		scsiq = (ADV_SCSI_REQ_Q *)
-		    ADV_U32_TO_VADDR(le32_to_cpu(asc_dvc->irq_sp->areq_vpa));
+		u32 pa_offset = le32_to_cpu(asc_dvc->irq_sp->areq_vpa);
+		ASC_DBG(1, "irq_sp %p areq_vpa %u\n",
+			asc_dvc->irq_sp, pa_offset);
+		reqp = adv_get_reqp(asc_dvc, pa_offset);
+		scsiq = &reqp->scsi_req_q;
 
 		/*
 		 * Request finished with good status and the queue was not
 		 * DMAed to host memory by the firmware. Set all status fields
 		 * to indicate good status.
 		 */
-		if ((irq_next_vpa & ASC_RQ_GOOD) != 0) {
+		if ((irq_next_vpa & ADV_RQ_GOOD) != 0) {
 			scsiq->done_status = QD_NO_ERROR;
 			scsiq->host_status = scsiq->scsi_status = 0;
 			scsiq->data_cnt = 0L;
@@ -6386,11 +6197,10 @@ static int AdvISR(ADV_DVC_VAR *asc_dvc)
 		 * stopper carrier.
 		 */
 		free_carrp = asc_dvc->irq_sp;
-		asc_dvc->irq_sp = (ADV_CARR_T *)
-		    ADV_U32_TO_VADDR(ASC_GET_CARRP(irq_next_vpa));
+		asc_dvc->irq_sp = adv_get_carrier(asc_dvc,
+						  ADV_GET_CARRP(irq_next_vpa));
 
-		free_carrp->next_vpa =
-		    cpu_to_le32(ADV_VADDR_TO_U32(asc_dvc->carr_freelist));
+		free_carrp->next_vpa = asc_dvc->carr_freelist->carr_va;
 		asc_dvc->carr_freelist = free_carrp;
 		asc_dvc->carr_pending_cnt--;
 
@@ -6405,7 +6215,6 @@ static int AdvISR(ADV_DVC_VAR *asc_dvc)
 		 * Notify the driver of the completed request by passing
 		 * the ADV_SCSI_REQ_Q pointer to its callback function.
 		 */
-		scsiq->a_flag |= ADV_SCSIQ_DONE;
 		adv_isr_callback(asc_dvc, scsiq);
 		/*
 		 * Note: After the driver callback function is called, 'scsiq'
@@ -6521,11 +6330,11 @@ AscCalSDTRData(ASC_DVC_VAR *asc_dvc, uchar sdtr_period, uchar syn_offset)
 	return byte;
 }
 
-static int AscSetChipSynRegAtID(PortAddr iop_base, uchar id, uchar sdtr_data)
+static bool AscSetChipSynRegAtID(PortAddr iop_base, uchar id, uchar sdtr_data)
 {
 	ASC_SCSI_BIT_ID_TYPE org_id;
 	int i;
-	int sta = TRUE;
+	bool sta = true;
 
 	AscSetBank(iop_base, 1);
 	org_id = AscReadChipDvcID(iop_base);
@@ -6539,10 +6348,10 @@ static int AscSetChipSynRegAtID(PortAddr iop_base, uchar id, uchar sdtr_data)
 		AscSetBank(iop_base, 0);
 		AscSetChipSyn(iop_base, sdtr_data);
 		if (AscGetChipSyn(iop_base) != sdtr_data) {
-			sta = FALSE;
+			sta = false;
 		}
 	} else {
-		sta = FALSE;
+		sta = false;
 	}
 	AscSetBank(iop_base, 1);
 	AscWriteChipDvcID(iop_base, org_id);
@@ -6556,12 +6365,12 @@ static void AscSetChipSDTR(PortAddr iop_base, uchar sdtr_data, uchar tid_no)
 	AscPutMCodeSDTRDoneAtID(iop_base, tid_no, sdtr_data);
 }
 
-static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
+static void AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 {
 	EXT_MSG ext_msg;
 	EXT_MSG out_msg;
 	ushort halt_q_addr;
-	int sdtr_accept;
+	bool sdtr_accept;
 	ushort int_halt_code;
 	ASC_SCSI_BIT_ID_TYPE scsi_busy;
 	ASC_SCSI_BIT_ID_TYPE target_id;
@@ -6603,14 +6412,14 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 			boardp->sdtr_data[tid_no] = 0;
 		}
 		AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-		return (0);
+		return;
 	} else if (int_halt_code == ASC_HALT_ENABLE_ASYN_USE_SYN_FIX) {
 		if (asc_dvc->pci_fix_asyn_xfer & target_id) {
 			AscSetChipSDTR(iop_base, asyn_sdtr, tid_no);
 			boardp->sdtr_data[tid_no] = asyn_sdtr;
 		}
 		AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-		return (0);
+		return;
 	} else if (int_halt_code == ASC_HALT_EXTMSG_IN) {
 		AscMemWordCopyPtrFromLram(iop_base,
 					  ASCV_MSGIN_BEG,
@@ -6620,10 +6429,10 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 		if (ext_msg.msg_type == EXTENDED_MESSAGE &&
 		    ext_msg.msg_req == EXTENDED_SDTR &&
 		    ext_msg.msg_len == MS_SDTR_LEN) {
-			sdtr_accept = TRUE;
+			sdtr_accept = true;
 			if ((ext_msg.req_ack_offset > ASC_SYN_MAX_OFFSET)) {
 
-				sdtr_accept = FALSE;
+				sdtr_accept = false;
 				ext_msg.req_ack_offset = ASC_SYN_MAX_OFFSET;
 			}
 			if ((ext_msg.xfer_period <
@@ -6631,7 +6440,7 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 			    || (ext_msg.xfer_period >
 				asc_dvc->sdtr_period_tbl[asc_dvc->
 							 max_sdtr_index])) {
-				sdtr_accept = FALSE;
+				sdtr_accept = false;
 				ext_msg.xfer_period =
 				    asc_dvc->sdtr_period_tbl[asc_dvc->
 							     min_sdtr_index];
@@ -6696,7 +6505,7 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 						  (ushort)ASC_SCSIQ_B_CNTL),
 					 q_cntl);
 			AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-			return (0);
+			return;
 		} else if (ext_msg.msg_type == EXTENDED_MESSAGE &&
 			   ext_msg.msg_req == EXTENDED_WDTR &&
 			   ext_msg.msg_len == MS_WDTR_LEN) {
@@ -6712,7 +6521,7 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 						  (ushort)ASC_SCSIQ_B_CNTL),
 					 q_cntl);
 			AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-			return (0);
+			return;
 		} else {
 
 			ext_msg.msg_type = MESSAGE_REJECT;
@@ -6726,7 +6535,7 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 						  (ushort)ASC_SCSIQ_B_CNTL),
 					 q_cntl);
 			AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-			return (0);
+			return;
 		}
 	} else if (int_halt_code == ASC_HALT_CHK_CONDITION) {
 
@@ -6783,7 +6592,7 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 		AscWriteLramByte(iop_base, (ushort)ASCV_SCSIBUSY_B, scsi_busy);
 
 		AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-		return (0);
+		return;
 	} else if (int_halt_code == ASC_HALT_SDTR_REJECTED) {
 
 		AscMemWordCopyPtrFromLram(iop_base,
@@ -6805,7 +6614,7 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 				 (ushort)(halt_q_addr +
 					  (ushort)ASC_SCSIQ_B_CNTL), q_cntl);
 		AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-		return (0);
+		return;
 	} else if (int_halt_code == ASC_HALT_SS_QUEUE_FULL) {
 
 		scsi_status = AscReadLramByte(iop_base,
@@ -6850,166 +6659,9 @@ static int AscIsrChipHalted(ASC_DVC_VAR *asc_dvc)
 			}
 		}
 		AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-		return (0);
-	}
-#if CC_VERY_LONG_SG_LIST
-	else if (int_halt_code == ASC_HALT_HOST_COPY_SG_LIST_TO_RISC) {
-		uchar q_no;
-		ushort q_addr;
-		uchar sg_wk_q_no;
-		uchar first_sg_wk_q_no;
-		ASC_SCSI_Q *scsiq;	/* Ptr to driver request. */
-		ASC_SG_HEAD *sg_head;	/* Ptr to driver SG request. */
-		ASC_SG_LIST_Q scsi_sg_q;	/* Structure written to queue. */
-		ushort sg_list_dwords;
-		ushort sg_entry_cnt;
-		uchar next_qp;
-		int i;
-
-		q_no = AscReadLramByte(iop_base, (ushort)ASCV_REQ_SG_LIST_QP);
-		if (q_no == ASC_QLINK_END)
-			return 0;
-
-		q_addr = ASC_QNO_TO_QADDR(q_no);
-
-		/*
-		 * Convert the request's SRB pointer to a host ASC_SCSI_REQ
-		 * structure pointer using a macro provided by the driver.
-		 * The ASC_SCSI_REQ pointer provides a pointer to the
-		 * host ASC_SG_HEAD structure.
-		 */
-		/* Read request's SRB pointer. */
-		scsiq = (ASC_SCSI_Q *)
-		    ASC_SRB2SCSIQ(ASC_U32_TO_VADDR(AscReadLramDWord(iop_base,
-								    (ushort)
-								    (q_addr +
-								     ASC_SCSIQ_D_SRBPTR))));
-
-		/*
-		 * Get request's first and working SG queue.
-		 */
-		sg_wk_q_no = AscReadLramByte(iop_base,
-					     (ushort)(q_addr +
-						      ASC_SCSIQ_B_SG_WK_QP));
-
-		first_sg_wk_q_no = AscReadLramByte(iop_base,
-						   (ushort)(q_addr +
-							    ASC_SCSIQ_B_FIRST_SG_WK_QP));
-
-		/*
-		 * Reset request's working SG queue back to the
-		 * first SG queue.
-		 */
-		AscWriteLramByte(iop_base,
-				 (ushort)(q_addr +
-					  (ushort)ASC_SCSIQ_B_SG_WK_QP),
-				 first_sg_wk_q_no);
-
-		sg_head = scsiq->sg_head;
-
-		/*
-		 * Set sg_entry_cnt to the number of SG elements
-		 * that will be completed on this interrupt.
-		 *
-		 * Note: The allocated SG queues contain ASC_MAX_SG_LIST - 1
-		 * SG elements. The data_cnt and data_addr fields which
-		 * add 1 to the SG element capacity are not used when
-		 * restarting SG handling after a halt.
-		 */
-		if (scsiq->remain_sg_entry_cnt > (ASC_MAX_SG_LIST - 1)) {
-			sg_entry_cnt = ASC_MAX_SG_LIST - 1;
-
-			/*
-			 * Keep track of remaining number of SG elements that
-			 * will need to be handled on the next interrupt.
-			 */
-			scsiq->remain_sg_entry_cnt -= (ASC_MAX_SG_LIST - 1);
-		} else {
-			sg_entry_cnt = scsiq->remain_sg_entry_cnt;
-			scsiq->remain_sg_entry_cnt = 0;
-		}
-
-		/*
-		 * Copy SG elements into the list of allocated SG queues.
-		 *
-		 * Last index completed is saved in scsiq->next_sg_index.
-		 */
-		next_qp = first_sg_wk_q_no;
-		q_addr = ASC_QNO_TO_QADDR(next_qp);
-		scsi_sg_q.sg_head_qp = q_no;
-		scsi_sg_q.cntl = QCSG_SG_XFER_LIST;
-		for (i = 0; i < sg_head->queue_cnt; i++) {
-			scsi_sg_q.seq_no = i + 1;
-			if (sg_entry_cnt > ASC_SG_LIST_PER_Q) {
-				sg_list_dwords = (uchar)(ASC_SG_LIST_PER_Q * 2);
-				sg_entry_cnt -= ASC_SG_LIST_PER_Q;
-				/*
-				 * After very first SG queue RISC FW uses next
-				 * SG queue first element then checks sg_list_cnt
-				 * against zero and then decrements, so set
-				 * sg_list_cnt 1 less than number of SG elements
-				 * in each SG queue.
-				 */
-				scsi_sg_q.sg_list_cnt = ASC_SG_LIST_PER_Q - 1;
-				scsi_sg_q.sg_cur_list_cnt =
-				    ASC_SG_LIST_PER_Q - 1;
-			} else {
-				/*
-				 * This is the last SG queue in the list of
-				 * allocated SG queues. If there are more
-				 * SG elements than will fit in the allocated
-				 * queues, then set the QCSG_SG_XFER_MORE flag.
-				 */
-				if (scsiq->remain_sg_entry_cnt != 0) {
-					scsi_sg_q.cntl |= QCSG_SG_XFER_MORE;
-				} else {
-					scsi_sg_q.cntl |= QCSG_SG_XFER_END;
-				}
-				/* equals sg_entry_cnt * 2 */
-				sg_list_dwords = sg_entry_cnt << 1;
-				scsi_sg_q.sg_list_cnt = sg_entry_cnt - 1;
-				scsi_sg_q.sg_cur_list_cnt = sg_entry_cnt - 1;
-				sg_entry_cnt = 0;
-			}
-
-			scsi_sg_q.q_no = next_qp;
-			AscMemWordCopyPtrToLram(iop_base,
-						q_addr + ASC_SCSIQ_SGHD_CPY_BEG,
-						(uchar *)&scsi_sg_q,
-						sizeof(ASC_SG_LIST_Q) >> 1);
-
-			AscMemDWordCopyPtrToLram(iop_base,
-						 q_addr + ASC_SGQ_LIST_BEG,
-						 (uchar *)&sg_head->
-						 sg_list[scsiq->next_sg_index],
-						 sg_list_dwords);
-
-			scsiq->next_sg_index += ASC_SG_LIST_PER_Q;
-
-			/*
-			 * If the just completed SG queue contained the
-			 * last SG element, then no more SG queues need
-			 * to be written.
-			 */
-			if (scsi_sg_q.cntl & QCSG_SG_XFER_END) {
-				break;
-			}
-
-			next_qp = AscReadLramByte(iop_base,
-						  (ushort)(q_addr +
-							   ASC_SCSIQ_B_FWD));
-			q_addr = ASC_QNO_TO_QADDR(next_qp);
-		}
-
-		/*
-		 * Clear the halt condition so the RISC will be restarted
-		 * after the return.
-		 */
-		AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0);
-		return (0);
+		return;
 	}
-#endif /* CC_VERY_LONG_SG_LIST */
-	return (0);
+	return;
 }
 
 /*
@@ -7043,7 +6695,7 @@ DvcGetQinfo(PortAddr iop_base, ushort s_addr, uchar *inbuf, int words)
 static uchar
 _AscCopyLramScsiDoneQ(PortAddr iop_base,
 		      ushort q_addr,
-		      ASC_QDONE_INFO *scsiq, ASC_DCNT max_dma_count)
+		      ASC_QDONE_INFO *scsiq, unsigned int max_dma_count)
 {
 	ushort _val;
 	uchar sg_queue_cnt;
@@ -7070,10 +6722,10 @@ _AscCopyLramScsiDoneQ(PortAddr iop_base,
 	/*
 	 * Read high word of remain bytes from alternate location.
 	 */
-	scsiq->remain_bytes = (((ADV_DCNT)AscReadLramWord(iop_base,
-							  (ushort)(q_addr +
-								   (ushort)
-								   ASC_SCSIQ_W_ALT_DC1)))
+	scsiq->remain_bytes = (((u32)AscReadLramWord(iop_base,
+						     (ushort)(q_addr +
+							      (ushort)
+							      ASC_SCSIQ_W_ALT_DC1)))
 			       << 16);
 	/*
 	 * Read low word of remain bytes from original location.
@@ -7093,25 +6745,24 @@ _AscCopyLramScsiDoneQ(PortAddr iop_base,
  */
 static void asc_isr_callback(ASC_DVC_VAR *asc_dvc_varp, ASC_QDONE_INFO *qdonep)
 {
-	struct asc_board *boardp;
+	struct asc_board *boardp = asc_dvc_varp->drv_ptr;
+	u32 srb_tag;
 	struct scsi_cmnd *scp;
-	struct Scsi_Host *shost;
 
 	ASC_DBG(1, "asc_dvc_varp 0x%p, qdonep 0x%p\n", asc_dvc_varp, qdonep);
 	ASC_DBG_PRT_ASC_QDONE_INFO(2, qdonep);
 
-	scp = advansys_srb_to_ptr(asc_dvc_varp, qdonep->d2.srb_ptr);
+	/*
+	 * Decrease the srb_tag by 1 to find the SCSI command
+	 */
+	srb_tag = qdonep->d2.srb_tag - 1;
+	scp = scsi_host_find_tag(boardp->shost, srb_tag);
 	if (!scp)
 		return;
 
 	ASC_DBG_PRT_CDB(2, scp->cmnd, scp->cmd_len);
 
-	shost = scp->device->host;
-	ASC_STATS(shost, callback);
-	ASC_DBG(1, "shost 0x%p\n", shost);
-
-	boardp = shost_priv(shost);
-	BUG_ON(asc_dvc_varp != &boardp->dvc_var.asc_dvc_var);
+	ASC_STATS(boardp->shost, callback);
 
 	dma_unmap_single(boardp->dev, scp->SCp.dma_handle,
 			 SCSI_SENSE_BUFFERSIZE, DMA_FROM_DEVICE);
@@ -7220,7 +6871,7 @@ static int AscIsrQDone(ASC_DVC_VAR *asc_dvc)
 	uchar cur_target_qng;
 	ASC_QDONE_INFO scsiq_buf;
 	ASC_QDONE_INFO *scsiq;
-	int false_overrun;
+	bool false_overrun;
 
 	iop_base = asc_dvc->iop_base;
 	n_q_used = 1;
@@ -7294,14 +6945,17 @@ static int AscIsrQDone(ASC_DVC_VAR *asc_dvc)
 			scsiq->d3.done_stat = QD_WITH_ERROR;
 			goto FATAL_ERR_QDONE;
 		}
-		if ((scsiq->d2.srb_ptr == 0UL) ||
+		if ((scsiq->d2.srb_tag == 0UL) ||
 		    ((scsiq->q_status & QS_ABORTED) != 0)) {
 			return (0x11);
 		} else if (scsiq->q_status == QS_DONE) {
-			false_overrun = FALSE;
+			/*
+			 * This is also curious.
+			 * false_overrun will _always_ be set to 'false'
+			 */
+			false_overrun = false;
 			if (scsiq->extra_bytes != 0) {
-				scsiq->remain_bytes +=
-				    (ADV_DCNT)scsiq->extra_bytes;
+				scsiq->remain_bytes += scsiq->extra_bytes;
 			}
 			if (scsiq->d3.done_stat == QD_WITH_ERROR) {
 				if (scsiq->d3.host_stat ==
@@ -7372,23 +7026,23 @@ static int AscISR(ASC_DVC_VAR *asc_dvc)
 	uchar host_flag;
 
 	iop_base = asc_dvc->iop_base;
-	int_pending = FALSE;
+	int_pending = ASC_FALSE;
 
 	if (AscIsIntPending(iop_base) == 0)
 		return int_pending;
 
 	if ((asc_dvc->init_state & ASC_INIT_STATE_END_LOAD_MC) == 0) {
-		return ERR;
+		return ASC_ERROR;
 	}
 	if (asc_dvc->in_critical_cnt != 0) {
 		AscSetLibErrorCode(asc_dvc, ASCQ_ERR_ISR_ON_CRITICAL);
-		return ERR;
+		return ASC_ERROR;
 	}
 	if (asc_dvc->is_in_int) {
 		AscSetLibErrorCode(asc_dvc, ASCQ_ERR_ISR_RE_ENTRY);
-		return ERR;
+		return ASC_ERROR;
 	}
-	asc_dvc->is_in_int = TRUE;
+	asc_dvc->is_in_int = true;
 	ctrl_reg = AscGetChipControl(iop_base);
 	saved_ctrl_reg = ctrl_reg & (~(CC_SCSI_RESET | CC_CHIP_RESET |
 				       CC_SINGLE_STEP | CC_DIAG | CC_TEST));
@@ -7396,7 +7050,7 @@ static int AscISR(ASC_DVC_VAR *asc_dvc)
 	if (chipstat & CSW_SCSI_RESET_LATCH) {
 		if (!(asc_dvc->bus_type & (ASC_IS_VL | ASC_IS_EISA))) {
 			int i = 10;
-			int_pending = TRUE;
+			int_pending = ASC_TRUE;
 			asc_dvc->sdtr_done = 0;
 			saved_ctrl_reg &= (uchar)(~CC_HALT);
 			while ((AscGetChipStatus(iop_base) &
@@ -7418,15 +7072,11 @@ static int AscISR(ASC_DVC_VAR *asc_dvc)
 			 (uchar)(host_flag | (uchar)ASC_HOST_FLAG_IN_ISR));
 	if ((chipstat & CSW_INT_PENDING) || (int_pending)) {
 		AscAckInterrupt(iop_base);
-		int_pending = TRUE;
+		int_pending = ASC_TRUE;
 		if ((chipstat & CSW_HALTED) && (ctrl_reg & CC_SINGLE_STEP)) {
-			if (AscIsrChipHalted(asc_dvc) == ERR) {
-				goto ISR_REPORT_QDONE_FATAL_ERROR;
-			} else {
-				saved_ctrl_reg &= (uchar)(~CC_HALT);
-			}
+			AscIsrChipHalted(asc_dvc);
+			saved_ctrl_reg &= (uchar)(~CC_HALT);
 		} else {
- ISR_REPORT_QDONE_FATAL_ERROR:
 			if ((asc_dvc->dvc_cntl & ASC_CNTL_INT_MULTI_Q) != 0) {
 				while (((status =
 					 AscIsrQDone(asc_dvc)) & 0x01) != 0) {
@@ -7440,20 +7090,20 @@ static int AscISR(ASC_DVC_VAR *asc_dvc)
 				} while (status == 0x11);
 			}
 			if ((status & 0x80) != 0)
-				int_pending = ERR;
+				int_pending = ASC_ERROR;
 		}
 	}
 	AscWriteLramByte(iop_base, ASCV_HOST_FLAG_B, host_flag);
 	AscSetChipLramAddr(iop_base, saved_ram_addr);
 	AscSetChipControl(iop_base, saved_ctrl_reg);
-	asc_dvc->is_in_int = FALSE;
+	asc_dvc->is_in_int = false;
 	return int_pending;
 }
 
 /*
  * advansys_reset()
  *
- * Reset the bus associated with the command 'scp'.
+ * Reset the host associated with the command 'scp'.
  *
  * This function runs its own thread. Interrupts must be blocked but
  * sleeping is allowed and no locking other than for host structures is
@@ -7471,7 +7121,7 @@ static int advansys_reset(struct scsi_cmnd *scp)
 
 	ASC_STATS(shost, reset);
 
-	scmd_printk(KERN_INFO, scp, "SCSI bus reset started...\n");
+	scmd_printk(KERN_INFO, scp, "SCSI host reset started...\n");
 
 	if (ASC_NARROW_BOARD(boardp)) {
 		ASC_DVC_VAR *asc_dvc = &boardp->dvc_var.asc_dvc_var;
@@ -7482,20 +7132,19 @@ static int advansys_reset(struct scsi_cmnd *scp)
 
 		/* Refer to ASC_IERR_* definitions for meaning of 'err_code'. */
 		if (asc_dvc->err_code || !asc_dvc->overrun_dma) {
-			scmd_printk(KERN_INFO, scp, "SCSI bus reset error: "
+			scmd_printk(KERN_INFO, scp, "SCSI host reset error: "
 				    "0x%x, status: 0x%x\n", asc_dvc->err_code,
 				    status);
 			ret = FAILED;
 		} else if (status) {
-			scmd_printk(KERN_INFO, scp, "SCSI bus reset warning: "
+			scmd_printk(KERN_INFO, scp, "SCSI host reset warning: "
 				    "0x%x\n", status);
 		} else {
-			scmd_printk(KERN_INFO, scp, "SCSI bus reset "
+			scmd_printk(KERN_INFO, scp, "SCSI host reset "
 				    "successful\n");
 		}
 
 		ASC_DBG(1, "after AscInitAsc1000Driver()\n");
-		spin_lock_irqsave(shost->host_lock, flags);
 	} else {
 		/*
 		 * If the suggest reset bus flags are set, then reset the bus.
@@ -7504,28 +7153,25 @@ static int advansys_reset(struct scsi_cmnd *scp)
 		ADV_DVC_VAR *adv_dvc = &boardp->dvc_var.adv_dvc_var;
 
 		/*
-		 * Reset the target's SCSI bus.
+		 * Reset the chip and SCSI bus.
 		 */
 		ASC_DBG(1, "before AdvResetChipAndSB()\n");
 		switch (AdvResetChipAndSB(adv_dvc)) {
 		case ASC_TRUE:
-			scmd_printk(KERN_INFO, scp, "SCSI bus reset "
+			scmd_printk(KERN_INFO, scp, "SCSI host reset "
 				    "successful\n");
 			break;
 		case ASC_FALSE:
 		default:
-			scmd_printk(KERN_INFO, scp, "SCSI bus reset error\n");
+			scmd_printk(KERN_INFO, scp, "SCSI host reset error\n");
 			ret = FAILED;
 			break;
 		}
 		spin_lock_irqsave(shost->host_lock, flags);
 		AdvISR(adv_dvc);
+		spin_unlock_irqrestore(shost->host_lock, flags);
 	}
 
-	/* Save the time of the most recently completed reset. */
-	boardp->last_reset = jiffies;
-	spin_unlock_irqrestore(shost->host_lock, flags);
-
 	ASC_DBG(1, "ret %d\n", ret);
 
 	return ret;
@@ -7584,9 +7230,10 @@ static irqreturn_t advansys_interrupt(int irq, void *dev_id)
 	struct Scsi_Host *shost = dev_id;
 	struct asc_board *boardp = shost_priv(shost);
 	irqreturn_t result = IRQ_NONE;
+	unsigned long flags;
 
 	ASC_DBG(2, "boardp 0x%p\n", boardp);
-	spin_lock(shost->host_lock);
+	spin_lock_irqsave(shost->host_lock, flags);
 	if (ASC_NARROW_BOARD(boardp)) {
 		if (AscIsIntPending(shost->io_port)) {
 			result = IRQ_HANDLED;
@@ -7601,38 +7248,38 @@ static irqreturn_t advansys_interrupt(int irq, void *dev_id)
 			ASC_STATS(shost, interrupt);
 		}
 	}
-	spin_unlock(shost->host_lock);
+	spin_unlock_irqrestore(shost->host_lock, flags);
 
 	ASC_DBG(1, "end\n");
 	return result;
 }
 
-static int AscHostReqRiscHalt(PortAddr iop_base)
+static bool AscHostReqRiscHalt(PortAddr iop_base)
 {
 	int count = 0;
-	int sta = 0;
+	bool sta = false;
 	uchar saved_stop_code;
 
 	if (AscIsChipHalted(iop_base))
-		return (1);
+		return true;
 	saved_stop_code = AscReadLramByte(iop_base, ASCV_STOP_CODE_B);
 	AscWriteLramByte(iop_base, ASCV_STOP_CODE_B,
 			 ASC_STOP_HOST_REQ_RISC_HALT | ASC_STOP_REQ_RISC_STOP);
 	do {
 		if (AscIsChipHalted(iop_base)) {
-			sta = 1;
+			sta = true;
 			break;
 		}
 		mdelay(100);
 	} while (count++ < 20);
 	AscWriteLramByte(iop_base, ASCV_STOP_CODE_B, saved_stop_code);
-	return (sta);
+	return sta;
 }
 
-static int
+static bool
 AscSetRunChipSynRegAtID(PortAddr iop_base, uchar tid_no, uchar sdtr_data)
 {
-	int sta = FALSE;
+	bool sta = false;
 
 	if (AscHostReqRiscHalt(iop_base)) {
 		sta = AscSetChipSynRegAtID(iop_base, tid_no, sdtr_data);
@@ -7851,13 +7498,17 @@ static int advansys_slave_configure(struct scsi_device *sdev)
 	return 0;
 }
 
-static __le32 advansys_get_sense_buffer_dma(struct scsi_cmnd *scp)
+static __le32 asc_get_sense_buffer_dma(struct scsi_cmnd *scp)
 {
 	struct asc_board *board = shost_priv(scp->device->host);
+
 	scp->SCp.dma_handle = dma_map_single(board->dev, scp->sense_buffer,
-					     SCSI_SENSE_BUFFERSIZE, DMA_FROM_DEVICE);
-	dma_cache_sync(board->dev, scp->sense_buffer,
-		       SCSI_SENSE_BUFFERSIZE, DMA_FROM_DEVICE);
+					     SCSI_SENSE_BUFFERSIZE,
+					     DMA_FROM_DEVICE);
+	if (dma_mapping_error(board->dev, scp->SCp.dma_handle)) {
+		ASC_DBG(1, "failed to map sense buffer\n");
+		return 0;
+	}
 	return cpu_to_le32(scp->SCp.dma_handle);
 }
 
@@ -7866,17 +7517,16 @@ static int asc_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
 {
 	struct asc_dvc_var *asc_dvc = &boardp->dvc_var.asc_dvc_var;
 	int use_sg;
+	u32 srb_tag;
 
 	memset(asc_scsi_q, 0, sizeof(*asc_scsi_q));
 
 	/*
-	 * Point the ASC_SCSI_Q to the 'struct scsi_cmnd'.
+	 * Set the srb_tag to the command tag + 1, as
+	 * srb_tag '0' is used internally by the chip.
 	 */
-	asc_scsi_q->q2.srb_ptr = advansys_ptr_to_srb(asc_dvc, scp);
-	if (asc_scsi_q->q2.srb_ptr == BAD_SRB) {
-		scp->result = HOST_BYTE(DID_SOFT_ERROR);
-		return ASC_ERROR;
-	}
+	srb_tag = scp->request->tag + 1;
+	asc_scsi_q->q2.srb_tag = srb_tag;
 
 	/*
 	 * Build the ASC_SCSI_Q request.
@@ -7887,8 +7537,10 @@ static int asc_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
 	asc_scsi_q->q1.target_lun = scp->device->lun;
 	asc_scsi_q->q2.target_ix =
 	    ASC_TIDLUN_TO_IX(scp->device->id, scp->device->lun);
-	asc_scsi_q->q1.sense_addr = advansys_get_sense_buffer_dma(scp);
+	asc_scsi_q->q1.sense_addr = asc_get_sense_buffer_dma(scp);
 	asc_scsi_q->q1.sense_len = SCSI_SENSE_BUFFERSIZE;
+	if (!asc_scsi_q->q1.sense_addr)
+		return ASC_BUSY;
 
 	/*
 	 * If there are any outstanding requests for the current target,
@@ -7910,7 +7562,10 @@ static int asc_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
 
 	/* Build ASC_SCSI_Q */
 	use_sg = scsi_dma_map(scp);
-	if (use_sg != 0) {
+	if (use_sg < 0) {
+		ASC_DBG(1, "failed to map sglist\n");
+		return ASC_BUSY;
+	} else if (use_sg > 0) {
 		int sgcnt;
 		struct scatterlist *slp;
 		struct asc_sg_head *asc_sg_head;
@@ -7975,20 +7630,19 @@ static int asc_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
  *      ADV_ERROR(-1) - SG List creation failed
  */
 static int
-adv_get_sglist(struct asc_board *boardp, adv_req_t *reqp, struct scsi_cmnd *scp,
-	       int use_sg)
+adv_get_sglist(struct asc_board *boardp, adv_req_t *reqp,
+	       ADV_SCSI_REQ_Q *scsiqp, struct scsi_cmnd *scp, int use_sg)
 {
-	adv_sgblk_t *sgblkp;
-	ADV_SCSI_REQ_Q *scsiqp;
+	adv_sgblk_t *sgblkp, *prev_sgblkp;
 	struct scatterlist *slp;
 	int sg_elem_cnt;
 	ADV_SG_BLOCK *sg_block, *prev_sg_block;
-	ADV_PADDR sg_block_paddr;
+	dma_addr_t sgblk_paddr;
 	int i;
 
-	scsiqp = (ADV_SCSI_REQ_Q *)ADV_32BALIGN(&reqp->scsi_req_q);
 	slp = scsi_sglist(scp);
 	sg_elem_cnt = use_sg;
+	prev_sgblkp = NULL;
 	prev_sg_block = NULL;
 	reqp->sgblkp = NULL;
 
@@ -7998,7 +7652,9 @@ adv_get_sglist(struct asc_board *boardp, adv_req_t *reqp, struct scsi_cmnd *scp,
 		 * list. One 'adv_sgblk_t' structure holds NO_OF_SG_PER_BLOCK
 		 * (15) scatter-gather elements.
 		 */
-		if ((sgblkp = boardp->adv_sgblkp) == NULL) {
+		sgblkp = dma_pool_alloc(boardp->adv_sgblk_pool, GFP_ATOMIC,
+					&sgblk_paddr);
+		if (!sgblkp) {
 			ASC_DBG(1, "no free adv_sgblk_t\n");
 			ASC_STATS(scp->device->host, adv_build_nosg);
 
@@ -8009,24 +7665,16 @@ adv_get_sglist(struct asc_board *boardp, adv_req_t *reqp, struct scsi_cmnd *scp,
 			while ((sgblkp = reqp->sgblkp) != NULL) {
 				/* Remove 'sgblkp' from the request list. */
 				reqp->sgblkp = sgblkp->next_sgblkp;
-
-				/* Add 'sgblkp' to the board free list. */
-				sgblkp->next_sgblkp = boardp->adv_sgblkp;
-				boardp->adv_sgblkp = sgblkp;
+				sgblkp->next_sgblkp = NULL;
+				dma_pool_free(boardp->adv_sgblk_pool, sgblkp,
+					      sgblkp->sg_addr);
 			}
 			return ASC_BUSY;
 		}
-
 		/* Complete 'adv_sgblk_t' board allocation. */
-		boardp->adv_sgblkp = sgblkp->next_sgblkp;
+		sgblkp->sg_addr = sgblk_paddr;
 		sgblkp->next_sgblkp = NULL;
-
-		/*
-		 * Get 8 byte aligned virtual and physical addresses
-		 * for the allocated ADV_SG_BLOCK structure.
-		 */
-		sg_block = (ADV_SG_BLOCK *)ADV_8BALIGN(&sgblkp->sg_block);
-		sg_block_paddr = virt_to_bus(sg_block);
+		sg_block = &sgblkp->sg_block;
 
 		/*
 		 * Check if this is the first 'adv_sgblk_t' for the
@@ -8041,17 +7689,16 @@ adv_get_sglist(struct asc_board *boardp, adv_req_t *reqp, struct scsi_cmnd *scp,
 			 * address pointers.
 			 */
 			scsiqp->sg_list_ptr = sg_block;
-			scsiqp->sg_real_addr = cpu_to_le32(sg_block_paddr);
+			scsiqp->sg_real_addr = cpu_to_le32(sgblk_paddr);
 		} else {
 			/* Request's second or later scatter-gather block. */
-			sgblkp->next_sgblkp = reqp->sgblkp;
-			reqp->sgblkp = sgblkp;
+			prev_sgblkp->next_sgblkp = sgblkp;
 
 			/*
 			 * Point the previous ADV_SG_BLOCK structure to
 			 * the newly allocated ADV_SG_BLOCK structure.
 			 */
-			prev_sg_block->sg_ptr = cpu_to_le32(sg_block_paddr);
+			prev_sg_block->sg_ptr = cpu_to_le32(sgblk_paddr);
 		}
 
 		for (i = 0; i < NO_OF_SG_PER_BLOCK; i++) {
@@ -8062,15 +7709,19 @@ adv_get_sglist(struct asc_board *boardp, adv_req_t *reqp, struct scsi_cmnd *scp,
 			ASC_STATS_ADD(scp->device->host, xfer_sect,
 				      DIV_ROUND_UP(sg_dma_len(slp), 512));
 
-			if (--sg_elem_cnt == 0) {	/* Last ADV_SG_BLOCK and scatter-gather entry. */
+			if (--sg_elem_cnt == 0) {
+				/*
+				 * Last ADV_SG_BLOCK and scatter-gather entry.
+				 */
 				sg_block->sg_cnt = i + 1;
-				sg_block->sg_ptr = 0L;	/* Last ADV_SG_BLOCK in list. */
+				sg_block->sg_ptr = 0L; /* Last ADV_SG_BLOCK in list. */
 				return ADV_SUCCESS;
 			}
 			slp++;
 		}
 		sg_block->sg_cnt = NO_OF_SG_PER_BLOCK;
 		prev_sg_block = sg_block;
+		prev_sgblkp = sgblkp;
 	}
 }
 
@@ -8080,38 +7731,35 @@ adv_get_sglist(struct asc_board *boardp, adv_req_t *reqp, struct scsi_cmnd *scp,
  * If an adv_req_t can not be allocated to issue the request,
  * then return ASC_BUSY. If an error occurs, then return ASC_ERROR.
  *
- * Multi-byte fields in the ASC_SCSI_REQ_Q that are used by the
+ * Multi-byte fields in the ADV_SCSI_REQ_Q that are used by the
  * microcode for DMA addresses or math operations are byte swapped
  * to little-endian order.
  */
 static int
 adv_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
-	      ADV_SCSI_REQ_Q **adv_scsiqpp)
+	      adv_req_t **adv_reqpp)
 {
+	u32 srb_tag = scp->request->tag;
 	adv_req_t *reqp;
 	ADV_SCSI_REQ_Q *scsiqp;
-	int i;
 	int ret;
 	int use_sg;
+	dma_addr_t sense_addr;
 
 	/*
 	 * Allocate an adv_req_t structure from the board to execute
 	 * the command.
 	 */
-	if (boardp->adv_reqp == NULL) {
+	reqp = &boardp->adv_reqp[srb_tag];
+	if (reqp->cmndp && reqp->cmndp != scp ) {
 		ASC_DBG(1, "no free adv_req_t\n");
 		ASC_STATS(scp->device->host, adv_build_noreq);
 		return ASC_BUSY;
-	} else {
-		reqp = boardp->adv_reqp;
-		boardp->adv_reqp = reqp->next_reqp;
-		reqp->next_reqp = NULL;
 	}
 
-	/*
-	 * Get 32-byte aligned ADV_SCSI_REQ_Q and ADV_SG_BLOCK pointers.
-	 */
-	scsiqp = (ADV_SCSI_REQ_Q *)ADV_32BALIGN(&reqp->scsi_req_q);
+	reqp->req_addr = boardp->adv_reqp_addr + (srb_tag * sizeof(adv_req_t));
+
+	scsiqp = &reqp->scsi_req_q;
 
 	/*
 	 * Initialize the structure.
@@ -8119,14 +7767,15 @@ adv_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
 	scsiqp->cntl = scsiqp->scsi_cntl = scsiqp->done_status = 0;
 
 	/*
-	 * Set the ADV_SCSI_REQ_Q 'srb_ptr' to point to the adv_req_t structure.
+	 * Set the srb_tag to the command tag.
 	 */
-	scsiqp->srb_ptr = ADV_VADDR_TO_U32(reqp);
+	scsiqp->srb_tag = srb_tag;
 
 	/*
-	 * Set the adv_req_t 'cmndp' to point to the struct scsi_cmnd structure.
+	 * Set 'host_scribble' to point to the adv_req_t structure.
 	 */
 	reqp->cmndp = scp;
+	scp->host_scribble = (void *)reqp;
 
 	/*
 	 * Build the ADV_SCSI_REQ_Q request.
@@ -8135,28 +7784,38 @@ adv_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
 	/* Set CDB length and copy it to the request structure.  */
 	scsiqp->cdb_len = scp->cmd_len;
 	/* Copy first 12 CDB bytes to cdb[]. */
-	for (i = 0; i < scp->cmd_len && i < 12; i++) {
-		scsiqp->cdb[i] = scp->cmnd[i];
-	}
+	memcpy(scsiqp->cdb, scp->cmnd, scp->cmd_len < 12 ? scp->cmd_len : 12);
 	/* Copy last 4 CDB bytes, if present, to cdb16[]. */
-	for (; i < scp->cmd_len; i++) {
-		scsiqp->cdb16[i - 12] = scp->cmnd[i];
+	if (scp->cmd_len > 12) {
+		int cdb16_len = scp->cmd_len - 12;
+
+		memcpy(scsiqp->cdb16, &scp->cmnd[12], cdb16_len);
 	}
 
 	scsiqp->target_id = scp->device->id;
 	scsiqp->target_lun = scp->device->lun;
 
-	scsiqp->sense_addr = cpu_to_le32(virt_to_bus(&scp->sense_buffer[0]));
-	scsiqp->sense_len = SCSI_SENSE_BUFFERSIZE;
+	sense_addr = dma_map_single(boardp->dev, scp->sense_buffer,
+				    SCSI_SENSE_BUFFERSIZE, DMA_FROM_DEVICE);
+	if (dma_mapping_error(boardp->dev, sense_addr)) {
+		ASC_DBG(1, "failed to map sense buffer\n");
+		ASC_STATS(scp->device->host, adv_build_noreq);
+		return ASC_BUSY;
+	}
+	scsiqp->sense_addr = cpu_to_le32(sense_addr);
+	scsiqp->sense_len = cpu_to_le32(SCSI_SENSE_BUFFERSIZE);
 
 	/* Build ADV_SCSI_REQ_Q */
 
 	use_sg = scsi_dma_map(scp);
-	if (use_sg == 0) {
+	if (use_sg < 0) {
+		ASC_DBG(1, "failed to map SG list\n");
+		ASC_STATS(scp->device->host, adv_build_noreq);
+		return ASC_BUSY;
+	} else if (use_sg == 0) {
 		/* Zero-length transfer */
 		reqp->sgblkp = NULL;
 		scsiqp->data_cnt = 0;
-		scsiqp->vdata_addr = NULL;
 
 		scsiqp->data_addr = 0;
 		scsiqp->sg_list_ptr = NULL;
@@ -8168,27 +7827,20 @@ adv_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
 				   scp->device->host->sg_tablesize);
 			scsi_dma_unmap(scp);
 			scp->result = HOST_BYTE(DID_ERROR);
-
-			/*
-			 * Free the 'adv_req_t' structure by adding it back
-			 * to the board free list.
-			 */
-			reqp->next_reqp = boardp->adv_reqp;
-			boardp->adv_reqp = reqp;
+			reqp->cmndp = NULL;
+			scp->host_scribble = NULL;
 
 			return ASC_ERROR;
 		}
 
 		scsiqp->data_cnt = cpu_to_le32(scsi_bufflen(scp));
 
-		ret = adv_get_sglist(boardp, reqp, scp, use_sg);
+		ret = adv_get_sglist(boardp, reqp, scsiqp, scp, use_sg);
 		if (ret != ADV_SUCCESS) {
-			/*
-			 * Free the adv_req_t structure by adding it back to
-			 * the board free list.
-			 */
-			reqp->next_reqp = boardp->adv_reqp;
-			boardp->adv_reqp = reqp;
+			scsi_dma_unmap(scp);
+			scp->result = HOST_BYTE(DID_ERROR);
+			reqp->cmndp = NULL;
+			scp->host_scribble = NULL;
 
 			return ret;
 		}
@@ -8201,7 +7853,7 @@ adv_build_req(struct asc_board *boardp, struct scsi_cmnd *scp,
 	ASC_DBG_PRT_ADV_SCSI_REQ_Q(2, scsiqp);
 	ASC_DBG_PRT_CDB(1, scp->cmnd, scp->cmd_len);
 
-	*adv_scsiqpp = scsiqp;
+	*adv_reqpp = reqp;
 
 	return ASC_NOERROR;
 }
@@ -8358,8 +8010,8 @@ AscPutReadySgListQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq, uchar q_no)
 	int i;
 	ASC_SG_HEAD *sg_head;
 	ASC_SG_LIST_Q scsi_sg_q;
-	ASC_DCNT saved_data_addr;
-	ASC_DCNT saved_data_cnt;
+	__le32 saved_data_addr;
+	__le32 saved_data_cnt;
 	PortAddr iop_base;
 	ushort sg_list_dwords;
 	ushort sg_index;
@@ -8371,42 +8023,15 @@ AscPutReadySgListQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq, uchar q_no)
 	sg_head = scsiq->sg_head;
 	saved_data_addr = scsiq->q1.data_addr;
 	saved_data_cnt = scsiq->q1.data_cnt;
-	scsiq->q1.data_addr = (ASC_PADDR) sg_head->sg_list[0].addr;
-	scsiq->q1.data_cnt = (ASC_DCNT) sg_head->sg_list[0].bytes;
-#if CC_VERY_LONG_SG_LIST
+	scsiq->q1.data_addr = cpu_to_le32(sg_head->sg_list[0].addr);
+	scsiq->q1.data_cnt = cpu_to_le32(sg_head->sg_list[0].bytes);
 	/*
-	 * If sg_head->entry_cnt is greater than ASC_MAX_SG_LIST
-	 * then not all SG elements will fit in the allocated queues.
-	 * The rest of the SG elements will be copied when the RISC
-	 * completes the SG elements that fit and halts.
+	 * Set sg_entry_cnt to be the number of SG elements that
+	 * will fit in the allocated SG queues. It is minus 1, because
+	 * the first SG element is handled above.
 	 */
-	if (sg_head->entry_cnt > ASC_MAX_SG_LIST) {
-		/*
-		 * Set sg_entry_cnt to be the number of SG elements that
-		 * will fit in the allocated SG queues. It is minus 1, because
-		 * the first SG element is handled above. ASC_MAX_SG_LIST is
-		 * already inflated by 1 to account for this. For example it
-		 * may be 50 which is 1 + 7 queues * 7 SG elements.
-		 */
-		sg_entry_cnt = ASC_MAX_SG_LIST - 1;
+	sg_entry_cnt = sg_head->entry_cnt - 1;
 
-		/*
-		 * Keep track of remaining number of SG elements that will
-		 * need to be handled from a_isr.c.
-		 */
-		scsiq->remain_sg_entry_cnt =
-		    sg_head->entry_cnt - ASC_MAX_SG_LIST;
-	} else {
-#endif /* CC_VERY_LONG_SG_LIST */
-		/*
-		 * Set sg_entry_cnt to be the number of SG elements that
-		 * will fit in the allocated SG queues. It is minus 1, because
-		 * the first SG element is handled above.
-		 */
-		sg_entry_cnt = sg_head->entry_cnt - 1;
-#if CC_VERY_LONG_SG_LIST
-	}
-#endif /* CC_VERY_LONG_SG_LIST */
 	if (sg_entry_cnt != 0) {
 		scsiq->q1.cntl |= QC_SG_HEAD;
 		q_addr = ASC_QNO_TO_QADDR(q_no);
@@ -8431,21 +8056,7 @@ AscPutReadySgListQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq, uchar q_no)
 					    ASC_SG_LIST_PER_Q - 1;
 				}
 			} else {
-#if CC_VERY_LONG_SG_LIST
-				/*
-				 * This is the last SG queue in the list of
-				 * allocated SG queues. If there are more
-				 * SG elements than will fit in the allocated
-				 * queues, then set the QCSG_SG_XFER_MORE flag.
-				 */
-				if (sg_head->entry_cnt > ASC_MAX_SG_LIST) {
-					scsi_sg_q.cntl |= QCSG_SG_XFER_MORE;
-				} else {
-#endif /* CC_VERY_LONG_SG_LIST */
-					scsi_sg_q.cntl |= QCSG_SG_XFER_END;
-#if CC_VERY_LONG_SG_LIST
-				}
-#endif /* CC_VERY_LONG_SG_LIST */
+				scsi_sg_q.cntl |= QCSG_SG_XFER_END;
 				sg_list_dwords = sg_entry_cnt << 1;
 				if (i == 0) {
 					scsi_sg_q.sg_list_cnt = sg_entry_cnt;
@@ -8550,9 +8161,9 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 	PortAddr iop_base;
 	int sta;
 	int n_q_required;
-	int disable_syn_offset_one_fix;
+	bool disable_syn_offset_one_fix;
 	int i;
-	ASC_PADDR addr;
+	u32 addr;
 	ushort sg_entry_cnt = 0;
 	ushort sg_entry_cnt_minus_one = 0;
 	uchar target_ix;
@@ -8562,12 +8173,12 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 	uchar scsi_cmd;
 	uchar disable_cmd;
 	ASC_SG_HEAD *sg_head;
-	ASC_DCNT data_cnt;
+	unsigned long data_cnt;
 
 	iop_base = asc_dvc->iop_base;
 	sg_head = scsiq->sg_head;
 	if (asc_dvc->err_code != 0)
-		return (ERR);
+		return ASC_ERROR;
 	scsiq->q1.q_no = 0;
 	if ((scsiq->q2.tag_code & ASC_TAG_FLAG_EXTRA_BYTES) == 0) {
 		scsiq->q1.extra_bytes = 0;
@@ -8593,46 +8204,41 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 	}
 	if (asc_dvc->in_critical_cnt != 0) {
 		AscSetLibErrorCode(asc_dvc, ASCQ_ERR_CRITICAL_RE_ENTRY);
-		return (ERR);
+		return ASC_ERROR;
 	}
 	asc_dvc->in_critical_cnt++;
 	if ((scsiq->q1.cntl & QC_SG_HEAD) != 0) {
 		if ((sg_entry_cnt = sg_head->entry_cnt) == 0) {
 			asc_dvc->in_critical_cnt--;
-			return (ERR);
+			return ASC_ERROR;
 		}
-#if !CC_VERY_LONG_SG_LIST
 		if (sg_entry_cnt > ASC_MAX_SG_LIST) {
 			asc_dvc->in_critical_cnt--;
-			return (ERR);
+			return ASC_ERROR;
 		}
-#endif /* !CC_VERY_LONG_SG_LIST */
 		if (sg_entry_cnt == 1) {
-			scsiq->q1.data_addr =
-			    (ADV_PADDR)sg_head->sg_list[0].addr;
-			scsiq->q1.data_cnt =
-			    (ADV_DCNT)sg_head->sg_list[0].bytes;
+			scsiq->q1.data_addr = cpu_to_le32(sg_head->sg_list[0].addr);
+			scsiq->q1.data_cnt = cpu_to_le32(sg_head->sg_list[0].bytes);
 			scsiq->q1.cntl &= ~(QC_SG_HEAD | QC_SG_SWAP_QUEUE);
 		}
 		sg_entry_cnt_minus_one = sg_entry_cnt - 1;
 	}
 	scsi_cmd = scsiq->cdbptr[0];
-	disable_syn_offset_one_fix = FALSE;
+	disable_syn_offset_one_fix = false;
 	if ((asc_dvc->pci_fix_asyn_xfer & scsiq->q1.target_id) &&
 	    !(asc_dvc->pci_fix_asyn_xfer_always & scsiq->q1.target_id)) {
 		if (scsiq->q1.cntl & QC_SG_HEAD) {
 			data_cnt = 0;
 			for (i = 0; i < sg_entry_cnt; i++) {
-				data_cnt +=
-				    (ADV_DCNT)le32_to_cpu(sg_head->sg_list[i].
-							  bytes);
+				data_cnt += le32_to_cpu(sg_head->sg_list[i].
+							bytes);
 			}
 		} else {
 			data_cnt = le32_to_cpu(scsiq->q1.data_cnt);
 		}
 		if (data_cnt != 0UL) {
 			if (data_cnt < 512UL) {
-				disable_syn_offset_one_fix = TRUE;
+				disable_syn_offset_one_fix = true;
 			} else {
 				for (i = 0; i < ASC_SYN_OFFSET_ONE_DISABLE_LIST;
 				     i++) {
@@ -8643,7 +8249,7 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 					}
 					if (scsi_cmd == disable_cmd) {
 						disable_syn_offset_one_fix =
-						    TRUE;
+						    true;
 						break;
 					}
 				}
@@ -8662,12 +8268,11 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 			if (asc_dvc->bug_fix_cntl & ASC_BUG_FIX_IF_NOT_DWB) {
 				if ((scsi_cmd == READ_6) ||
 				    (scsi_cmd == READ_10)) {
-					addr =
-					    (ADV_PADDR)le32_to_cpu(sg_head->
+					addr = le32_to_cpu(sg_head->
 								   sg_list
 								   [sg_entry_cnt_minus_one].
 								   addr) +
-					    (ADV_DCNT)le32_to_cpu(sg_head->
+						le32_to_cpu(sg_head->
 								  sg_list
 								  [sg_entry_cnt_minus_one].
 								  bytes);
@@ -8688,8 +8293,7 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 								sg_list
 								[sg_entry_cnt_minus_one].
 								bytes);
-						data_cnt -=
-						    (ASC_DCNT) extra_bytes;
+						data_cnt -= extra_bytes;
 						sg_head->
 						    sg_list
 						    [sg_entry_cnt_minus_one].
@@ -8700,16 +8304,6 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 			}
 		}
 		sg_head->entry_to_copy = sg_head->entry_cnt;
-#if CC_VERY_LONG_SG_LIST
-		/*
-		 * Set the sg_entry_cnt to the maximum possible. The rest of
-		 * the SG elements will be copied when the RISC completes the
-		 * SG elements that fit and halts.
-		 */
-		if (sg_entry_cnt > ASC_MAX_SG_LIST) {
-			sg_entry_cnt = ASC_MAX_SG_LIST;
-		}
-#endif /* CC_VERY_LONG_SG_LIST */
 		n_q_required = AscSgListToQueue(sg_entry_cnt);
 		if ((AscGetNumOfFreeQueue(asc_dvc, target_ix, n_q_required) >=
 		     (uint) n_q_required)
@@ -8744,8 +8338,7 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
 						    == 0) {
 							scsiq->q2.tag_code |=
 							    ASC_TAG_FLAG_EXTRA_BYTES;
-							data_cnt -= (ASC_DCNT)
-							    extra_bytes;
+							data_cnt -= extra_bytes;
 							scsiq->q1.data_cnt =
 							    cpu_to_le32
 							    (data_cnt);
@@ -8780,7 +8373,7 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
  * If 'done_status' is not set to QD_DO_RETRY, then 'error_retry' will be
  * set to SCSI_MAX_RETRY.
  *
- * Multi-byte fields in the ASC_SCSI_REQ_Q that are used by the microcode
+ * Multi-byte fields in the ADV_SCSI_REQ_Q that are used by the microcode
  * for DMA addresses or math operations are byte swapped to little-endian
  * order.
  *
@@ -8791,11 +8384,11 @@ static int AscExeScsiQueue(ASC_DVC_VAR *asc_dvc, ASC_SCSI_Q *scsiq)
  *      ADV_ERROR(-1) -  Invalid ADV_SCSI_REQ_Q request structure
  *                       host IC error.
  */
-static int AdvExeScsiQueue(ADV_DVC_VAR *asc_dvc, ADV_SCSI_REQ_Q *scsiq)
+static int AdvExeScsiQueue(ADV_DVC_VAR *asc_dvc, adv_req_t *reqp)
 {
 	AdvPortAddr iop_base;
-	ADV_PADDR req_paddr;
 	ADV_CARR_T *new_carrp;
+	ADV_SCSI_REQ_Q *scsiq = &reqp->scsi_req_q;
 
 	/*
 	 * The ADV_SCSI_REQ_Q 'target_id' field should never exceed ADV_MAX_TID.
@@ -8812,39 +8405,19 @@ static int AdvExeScsiQueue(ADV_DVC_VAR *asc_dvc, ADV_SCSI_REQ_Q *scsiq)
 	 * Allocate a carrier ensuring at least one carrier always
 	 * remains on the freelist and initialize fields.
 	 */
-	if ((new_carrp = asc_dvc->carr_freelist) == NULL) {
+	new_carrp = adv_get_next_carrier(asc_dvc);
+	if (!new_carrp) {
+		ASC_DBG(1, "No free carriers\n");
 		return ADV_BUSY;
 	}
-	asc_dvc->carr_freelist = (ADV_CARR_T *)
-	    ADV_U32_TO_VADDR(le32_to_cpu(new_carrp->next_vpa));
-	asc_dvc->carr_pending_cnt++;
-
-	/*
-	 * Set the carrier to be a stopper by setting 'next_vpa'
-	 * to the stopper value. The current stopper will be changed
-	 * below to point to the new stopper.
-	 */
-	new_carrp->next_vpa = cpu_to_le32(ASC_CQ_STOPPER);
 
-	/*
-	 * Clear the ADV_SCSI_REQ_Q done flag.
-	 */
-	scsiq->a_flag &= ~ADV_SCSIQ_DONE;
-
-	req_paddr = virt_to_bus(scsiq);
-	BUG_ON(req_paddr & 31);
-	/* Wait for assertion before making little-endian */
-	req_paddr = cpu_to_le32(req_paddr);
+	asc_dvc->carr_pending_cnt++;
 
 	/* Save virtual and physical address of ADV_SCSI_REQ_Q and carrier. */
-	scsiq->scsiq_ptr = cpu_to_le32(ADV_VADDR_TO_U32(scsiq));
-	scsiq->scsiq_rptr = req_paddr;
+	scsiq->scsiq_ptr = cpu_to_le32(scsiq->srb_tag);
+	scsiq->scsiq_rptr = cpu_to_le32(reqp->req_addr);
 
-	scsiq->carr_va = cpu_to_le32(ADV_VADDR_TO_U32(asc_dvc->icq_sp));
-	/*
-	 * Every ADV_CARR_T.carr_pa is byte swapped to little-endian
-	 * order during initialization.
-	 */
+	scsiq->carr_va = asc_dvc->icq_sp->carr_va;
 	scsiq->carr_pa = asc_dvc->icq_sp->carr_pa;
 
 	/*
@@ -8852,7 +8425,7 @@ static int AdvExeScsiQueue(ADV_DVC_VAR *asc_dvc, ADV_SCSI_REQ_Q *scsiq)
 	 * the microcode. The newly allocated stopper will become the new
 	 * stopper.
 	 */
-	asc_dvc->icq_sp->areq_vpa = req_paddr;
+	asc_dvc->icq_sp->areq_vpa = scsiq->scsiq_rptr;
 
 	/*
 	 * Set the 'next_vpa' pointer for the old stopper to be the
@@ -8907,11 +8480,10 @@ static int asc_execute_scsi_cmnd(struct scsi_cmnd *scp)
 		ASC_DVC_VAR *asc_dvc = &boardp->dvc_var.asc_dvc_var;
 		struct asc_scsi_q asc_scsi_q;
 
-		/* asc_build_req() can not return ASC_BUSY. */
 		ret = asc_build_req(boardp, scp, &asc_scsi_q);
-		if (ret == ASC_ERROR) {
+		if (ret != ASC_NOERROR) {
 			ASC_STATS(scp->device->host, build_error);
-			return ASC_ERROR;
+			return ret;
 		}
 
 		ret = AscExeScsiQueue(asc_dvc, &asc_scsi_q);
@@ -8919,9 +8491,9 @@ static int asc_execute_scsi_cmnd(struct scsi_cmnd *scp)
 		err_code = asc_dvc->err_code;
 	} else {
 		ADV_DVC_VAR *adv_dvc = &boardp->dvc_var.adv_dvc_var;
-		ADV_SCSI_REQ_Q *adv_scsiqp;
+		adv_req_t *adv_reqp;
 
-		switch (adv_build_req(boardp, scp, &adv_scsiqp)) {
+		switch (adv_build_req(boardp, scp, &adv_reqp)) {
 		case ASC_NOERROR:
 			ASC_DBG(3, "adv_build_req ASC_NOERROR\n");
 			break;
@@ -8941,7 +8513,7 @@ static int asc_execute_scsi_cmnd(struct scsi_cmnd *scp)
 			return ASC_ERROR;
 		}
 
-		ret = AdvExeScsiQueue(adv_dvc, adv_scsiqp);
+		ret = AdvExeScsiQueue(adv_dvc, adv_reqp);
 		err_code = adv_dvc->err_code;
 	}
 
@@ -8956,6 +8528,7 @@ static int asc_execute_scsi_cmnd(struct scsi_cmnd *scp)
 		ASC_DBG(1, "ExeScsiQueue() ASC_NOERROR\n");
 		break;
 	case ASC_BUSY:
+		ASC_DBG(1, "ExeScsiQueue() ASC_BUSY\n");
 		ASC_STATS(scp->device->host, exe_busy);
 		break;
 	case ASC_ERROR:
@@ -9122,7 +8695,7 @@ static int AscStopQueueExe(PortAddr iop_base)
 	return (0);
 }
 
-static ASC_DCNT AscGetMaxDmaCount(ushort bus_type)
+static unsigned int AscGetMaxDmaCount(ushort bus_type)
 {
 	if (bus_type & ASC_IS_ISA)
 		return ASC_MAX_ISA_DMA_COUNT;
@@ -9183,15 +8756,13 @@ static uchar AscSetIsaDmaSpeed(PortAddr iop_base, uchar speed_value)
 }
 #endif /* CONFIG_ISA */
 
-static ushort AscInitAscDvcVar(ASC_DVC_VAR *asc_dvc)
+static void AscInitAscDvcVar(ASC_DVC_VAR *asc_dvc)
 {
 	int i;
 	PortAddr iop_base;
-	ushort warn_code;
 	uchar chip_version;
 
 	iop_base = asc_dvc->iop_base;
-	warn_code = 0;
 	asc_dvc->err_code = 0;
 	if ((asc_dvc->bus_type &
 	     (ASC_IS_ISA | ASC_IS_PCI | ASC_IS_EISA | ASC_IS_VL)) == 0) {
@@ -9205,7 +8776,7 @@ static ushort AscInitAscDvcVar(ASC_DVC_VAR *asc_dvc)
 	/* asc_dvc->init_state initialized in AscInitGetConfig(). */
 	asc_dvc->sdtr_done = 0;
 	asc_dvc->cur_total_qng = 0;
-	asc_dvc->is_in_int = 0;
+	asc_dvc->is_in_int = false;
 	asc_dvc->in_critical_cnt = 0;
 	asc_dvc->last_q_shortage = 0;
 	asc_dvc->use_tagged_qng = 0;
@@ -9267,7 +8838,6 @@ static ushort AscInitAscDvcVar(ASC_DVC_VAR *asc_dvc)
 		asc_dvc->scsiq_busy_tail[i] = (ASC_SCSI_Q *)0L;
 		asc_dvc->cfg->max_tag_qng[i] = ASC_MAX_INRAM_TAG_QNG;
 	}
-	return warn_code;
 }
 
 static int AscWriteEEPCmdReg(PortAddr iop_base, uchar cmd_reg)
@@ -9385,7 +8955,7 @@ static int AscWriteEEPDataReg(PortAddr iop_base, ushort data_reg)
 	int retry;
 
 	retry = 0;
-	while (TRUE) {
+	while (true) {
 		AscSetChipEEPData(iop_base, data_reg);
 		mdelay(1);
 		read_back = AscGetChipEEPData(iop_base);
@@ -9521,7 +9091,7 @@ static int AscSetEEPConfig(PortAddr iop_base, ASCEEP_CONFIG *cfg_buf,
 	int n_error;
 
 	retry = 0;
-	while (TRUE) {
+	while (true) {
 		if ((n_error = AscSetEEPConfigOnce(iop_base, cfg_buf,
 						   bus_type)) == 0) {
 			break;
@@ -9533,7 +9103,7 @@ static int AscSetEEPConfig(PortAddr iop_base, ASCEEP_CONFIG *cfg_buf,
 	return n_error;
 }
 
-static ushort AscInitFromEEP(ASC_DVC_VAR *asc_dvc)
+static int AscInitFromEEP(ASC_DVC_VAR *asc_dvc)
 {
 	ASCEEP_CONFIG eep_config_buf;
 	ASCEEP_CONFIG *eep_config;
@@ -9548,13 +9118,13 @@ static ushort AscInitFromEEP(ASC_DVC_VAR *asc_dvc)
 	warn_code = 0;
 	AscWriteLramWord(iop_base, ASCV_HALTCODE_W, 0x00FE);
 	AscStopQueueExe(iop_base);
-	if ((AscStopChip(iop_base) == FALSE) ||
+	if ((AscStopChip(iop_base)) ||
 	    (AscGetChipScsiCtrl(iop_base) != 0)) {
 		asc_dvc->init_state |= ASC_INIT_RESET_SCSI_DONE;
 		AscResetChipAndScsiBus(asc_dvc);
 		mdelay(asc_dvc->scsi_reset_wait * 1000); /* XXX: msleep? */
 	}
-	if (AscIsChipHalted(iop_base) == FALSE) {
+	if (!AscIsChipHalted(iop_base)) {
 		asc_dvc->err_code |= ASC_IERR_START_STOP_CHIP;
 		return (warn_code);
 	}
@@ -9709,8 +9279,8 @@ static int AscInitGetConfig(struct Scsi_Host *shost)
 		return asc_dvc->err_code;
 
 	if (AscFindSignature(asc_dvc->iop_base)) {
-		warn_code |= AscInitAscDvcVar(asc_dvc);
-		warn_code |= AscInitFromEEP(asc_dvc);
+		AscInitAscDvcVar(asc_dvc);
+		warn_code = AscInitFromEEP(asc_dvc);
 		asc_dvc->init_state |= ASC_INIT_STATE_END_GET_CFG;
 		if (asc_dvc->scsi_reset_wait > ASC_MAX_SCSI_RESET_WAIT)
 			asc_dvc->scsi_reset_wait = ASC_MAX_SCSI_RESET_WAIT;
@@ -9866,6 +9436,7 @@ static int AscInitSetConfig(struct pci_dev *pdev, struct Scsi_Host *shost)
  * on big-endian platforms so char fields read as words are actually being
  * unswapped on big-endian platforms.
  */
+#ifdef CONFIG_PCI
 static ADVEEP_3550_CONFIG Default_3550_EEPROM_Config = {
 	ADV_EEPROM_BIOS_ENABLE,	/* cfg_lsw */
 	0x0000,			/* cfg_msw */
@@ -10202,7 +9773,6 @@ static ADVEEP_38C1600_CONFIG ADVEEP_38C1600_Config_Field_IsChar = {
 	0			/* 63 reserved */
 };
 
-#ifdef CONFIG_PCI
 /*
  * Wait for EEPROM command to complete
  */
@@ -11232,7 +10802,7 @@ static struct scsi_host_template advansys_template = {
 	.name = DRV_NAME,
 	.info = advansys_info,
 	.queuecommand = advansys_queuecommand,
-	.eh_bus_reset_handler = advansys_reset,
+	.eh_host_reset_handler = advansys_reset,
 	.bios_param = advansys_biosparam,
 	.slave_configure = advansys_slave_configure,
 	/*
@@ -11240,7 +10810,7 @@ static struct scsi_host_template advansys_template = {
 	 * must be set. The flag will be cleared in advansys_board_found
 	 * for non-ISA adapters.
 	 */
-	.unchecked_isa_dma = 1,
+	.unchecked_isa_dma = true,
 	/*
 	 * All adapters controlled by this driver are capable of large
 	 * scatter-gather lists. According to the mid-level SCSI documentation
@@ -11249,26 +10819,25 @@ static struct scsi_host_template advansys_template = {
 	 * by enabling clustering, I/O throughput increases as well.
 	 */
 	.use_clustering = ENABLE_CLUSTERING,
+	.use_blk_tags = 1,
 };
 
 static int advansys_wide_init_chip(struct Scsi_Host *shost)
 {
 	struct asc_board *board = shost_priv(shost);
 	struct adv_dvc_var *adv_dvc = &board->dvc_var.adv_dvc_var;
-	int req_cnt = 0;
-	adv_req_t *reqp = NULL;
-	int sg_cnt = 0;
-	adv_sgblk_t *sgp;
+	size_t sgblk_pool_size;
 	int warn_code, err_code;
 
 	/*
 	 * Allocate buffer carrier structures. The total size
-	 * is about 4 KB, so allocate all at once.
+	 * is about 8 KB, so allocate all at once.
 	 */
-	adv_dvc->carrier_buf = kmalloc(ADV_CARRIER_BUFSIZE, GFP_KERNEL);
-	ASC_DBG(1, "carrier_buf 0x%p\n", adv_dvc->carrier_buf);
+	adv_dvc->carrier = dma_alloc_coherent(board->dev,
+		ADV_CARRIER_BUFSIZE, &adv_dvc->carrier_addr, GFP_KERNEL);
+	ASC_DBG(1, "carrier 0x%p\n", adv_dvc->carrier);
 
-	if (!adv_dvc->carrier_buf)
+	if (!adv_dvc->carrier)
 		goto kmalloc_failed;
 
 	/*
@@ -11276,54 +10845,34 @@ static int advansys_wide_init_chip(struct Scsi_Host *shost)
 	 * board. The total size is about 16 KB, so allocate all at once.
 	 * If the allocation fails decrement and try again.
 	 */
-	for (req_cnt = adv_dvc->max_host_qng; req_cnt > 0; req_cnt--) {
-		reqp = kmalloc(sizeof(adv_req_t) * req_cnt, GFP_KERNEL);
-
-		ASC_DBG(1, "reqp 0x%p, req_cnt %d, bytes %lu\n", reqp, req_cnt,
-			 (ulong)sizeof(adv_req_t) * req_cnt);
-
-		if (reqp)
-			break;
+	board->adv_reqp_size = adv_dvc->max_host_qng * sizeof(adv_req_t);
+	if (board->adv_reqp_size & 0x1f) {
+		ASC_DBG(1, "unaligned reqp %lu bytes\n", sizeof(adv_req_t));
+		board->adv_reqp_size = ADV_32BALIGN(board->adv_reqp_size);
 	}
+	board->adv_reqp = dma_alloc_coherent(board->dev, board->adv_reqp_size,
+		&board->adv_reqp_addr, GFP_KERNEL);
 
-	if (!reqp)
+	if (!board->adv_reqp)
 		goto kmalloc_failed;
 
-	adv_dvc->orig_reqp = reqp;
+	ASC_DBG(1, "reqp 0x%p, req_cnt %d, bytes %lu\n", board->adv_reqp,
+		adv_dvc->max_host_qng, board->adv_reqp_size);
 
 	/*
 	 * Allocate up to ADV_TOT_SG_BLOCK request structures for
 	 * the Wide board. Each structure is about 136 bytes.
 	 */
-	board->adv_sgblkp = NULL;
-	for (sg_cnt = 0; sg_cnt < ADV_TOT_SG_BLOCK; sg_cnt++) {
-		sgp = kmalloc(sizeof(adv_sgblk_t), GFP_KERNEL);
+	sgblk_pool_size = sizeof(adv_sgblk_t) * ADV_TOT_SG_BLOCK;
+	board->adv_sgblk_pool = dma_pool_create("adv_sgblk", board->dev,
+						sgblk_pool_size, 32, 0);
 
-		if (!sgp)
-			break;
-
-		sgp->next_sgblkp = board->adv_sgblkp;
-		board->adv_sgblkp = sgp;
-
-	}
-
-	ASC_DBG(1, "sg_cnt %d * %lu = %lu bytes\n", sg_cnt, sizeof(adv_sgblk_t),
-		 sizeof(adv_sgblk_t) * sg_cnt);
+	ASC_DBG(1, "sg_cnt %d * %lu = %lu bytes\n", ADV_TOT_SG_BLOCK,
+		sizeof(adv_sgblk_t), sgblk_pool_size);
 
-	if (!board->adv_sgblkp)
+	if (!board->adv_sgblk_pool)
 		goto kmalloc_failed;
 
-	/*
-	 * Point 'adv_reqp' to the request structures and
-	 * link them together.
-	 */
-	req_cnt--;
-	reqp[req_cnt].next_reqp = NULL;
-	for (; req_cnt > 0; req_cnt--) {
-		reqp[req_cnt - 1].next_reqp = &reqp[req_cnt];
-	}
-	board->adv_reqp = &reqp[0];
-
 	if (adv_dvc->chip_type == ADV_CHIP_ASC3550) {
 		ASC_DBG(2, "AdvInitAsc3550Driver()\n");
 		warn_code = AdvInitAsc3550Driver(adv_dvc);
@@ -11353,14 +10902,20 @@ static int advansys_wide_init_chip(struct Scsi_Host *shost)
 static void advansys_wide_free_mem(struct asc_board *board)
 {
 	struct adv_dvc_var *adv_dvc = &board->dvc_var.adv_dvc_var;
-	kfree(adv_dvc->carrier_buf);
-	adv_dvc->carrier_buf = NULL;
-	kfree(adv_dvc->orig_reqp);
-	adv_dvc->orig_reqp = board->adv_reqp = NULL;
-	while (board->adv_sgblkp) {
-		adv_sgblk_t *sgp = board->adv_sgblkp;
-		board->adv_sgblkp = sgp->next_sgblkp;
-		kfree(sgp);
+
+	if (adv_dvc->carrier) {
+		dma_free_coherent(board->dev, ADV_CARRIER_BUFSIZE,
+				  adv_dvc->carrier, adv_dvc->carrier_addr);
+		adv_dvc->carrier = NULL;
+	}
+	if (board->adv_reqp) {
+		dma_free_coherent(board->dev, board->adv_reqp_size,
+				  board->adv_reqp, board->adv_reqp_addr);
+		board->adv_reqp = NULL;
+	}
+	if (board->adv_sgblk_pool) {
+		dma_pool_destroy(board->adv_sgblk_pool);
+		board->adv_sgblk_pool = NULL;
 	}
 }
 
@@ -11431,28 +10986,28 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
 		switch (asc_dvc_varp->bus_type) {
 #ifdef CONFIG_ISA
 		case ASC_IS_ISA:
-			shost->unchecked_isa_dma = TRUE;
+			shost->unchecked_isa_dma = true;
 			share_irq = 0;
 			break;
 		case ASC_IS_VL:
-			shost->unchecked_isa_dma = FALSE;
+			shost->unchecked_isa_dma = false;
 			share_irq = 0;
 			break;
 		case ASC_IS_EISA:
-			shost->unchecked_isa_dma = FALSE;
+			shost->unchecked_isa_dma = false;
 			share_irq = IRQF_SHARED;
 			break;
 #endif /* CONFIG_ISA */
 #ifdef CONFIG_PCI
 		case ASC_IS_PCI:
-			shost->unchecked_isa_dma = FALSE;
+			shost->unchecked_isa_dma = false;
 			share_irq = IRQF_SHARED;
 			break;
 #endif /* CONFIG_PCI */
 		default:
 			shost_printk(KERN_ERR, shost, "unknown adapter type: "
 					"%d\n", asc_dvc_varp->bus_type);
-			shost->unchecked_isa_dma = TRUE;
+			shost->unchecked_isa_dma = false;
 			share_irq = 0;
 			break;
 		}
@@ -11471,7 +11026,7 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
 		 * For Wide boards set PCI information before calling
 		 * AdvInitGetConfig().
 		 */
-		shost->unchecked_isa_dma = FALSE;
+		shost->unchecked_isa_dma = false;
 		share_irq = IRQF_SHARED;
 		ASC_DBG(2, "AdvInitGetConfig()\n");
 
@@ -11656,24 +11211,11 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
 		/* Set maximum number of queues the adapter can handle. */
 		shost->can_queue = adv_dvc_varp->max_host_qng;
 	}
-
-	/*
-	 * Following v1.3.89, 'cmd_per_lun' is no longer needed
-	 * and should be set to zero.
-	 *
-	 * But because of a bug introduced in v1.3.89 if the driver is
-	 * compiled as a module and 'cmd_per_lun' is zero, the Mid-Level
-	 * SCSI function 'allocate_device' will panic. To allow the driver
-	 * to work as a module in these kernels set 'cmd_per_lun' to 1.
-	 *
-	 * Note: This is wrong.  cmd_per_lun should be set to the depth
-	 * you want on untagged devices always.
-	 #ifdef MODULE
-	 */
-	shost->cmd_per_lun = 1;
-/* #else
-            shost->cmd_per_lun = 0;
-#endif */
+	ret = scsi_init_shared_tag_map(shost, shost->can_queue);
+	if (ret) {
+		shost_printk(KERN_ERR, shost, "init tag map failed\n");
+		goto err_free_dma;
+	}
 
 	/*
 	 * Set the maximum number of scatter-gather elements the
@@ -11844,7 +11386,9 @@ static int advansys_board_found(struct Scsi_Host *shost, unsigned int iop,
  err_unmap:
 	if (boardp->ioremap_addr)
 		iounmap(boardp->ioremap_addr);
+#ifdef CONFIG_PCI
  err_shost:
+#endif
 	return ret;
 }
 
@@ -11927,6 +11471,7 @@ static int advansys_isa_probe(struct device *dev, unsigned int id)
 	board = shost_priv(shost);
 	board->irq = advansys_isa_irq_no(iop_base);
 	board->dev = dev;
+	board->shost = shost;
 
 	err = advansys_board_found(shost, iop_base, ASC_IS_ISA);
 	if (err)
@@ -12009,6 +11554,7 @@ static int advansys_vlb_probe(struct device *dev, unsigned int id)
 	board = shost_priv(shost);
 	board->irq = advansys_vlb_irq_no(iop_base);
 	board->dev = dev;
+	board->shost = shost;
 
 	err = advansys_board_found(shost, iop_base, ASC_IS_VL);
 	if (err)
@@ -12116,6 +11662,7 @@ static int advansys_eisa_probe(struct device *dev)
 		board = shost_priv(shost);
 		board->irq = irq;
 		board->dev = dev;
+		board->shost = shost;
 
 		err = advansys_board_found(shost, ioport, ASC_IS_EISA);
 		if (!err) {
@@ -12232,6 +11779,7 @@ static int advansys_pci_probe(struct pci_dev *pdev,
 	board = shost_priv(shost);
 	board->irq = pdev->irq;
 	board->dev = &pdev->dev;
+	board->shost = shost;
 
 	if (pdev->device == PCI_DEVICE_ID_ASP_ABP940UW ||
 	    pdev->device == PCI_DEVICE_ID_38C0800_REV1 ||
diff --git a/drivers/scsi/aha152x.c b/drivers/scsi/aha152x.c
index e31c460a1335..f44d0487236e 100644
--- a/drivers/scsi/aha152x.c
+++ b/drivers/scsi/aha152x.c
@@ -2922,7 +2922,6 @@ static struct scsi_host_template aha152x_driver_template = {
 	.can_queue			= 1,
 	.this_id			= 7,
 	.sg_tablesize			= SG_ALL,
-	.cmd_per_lun			= 1,
 	.use_clustering			= DISABLE_CLUSTERING,
 	.slave_alloc			= aha152x_adjust_queue,
 };
diff --git a/drivers/scsi/aha1542.c b/drivers/scsi/aha1542.c
index b95d2779f467..5b8b2937a3fe 100644
--- a/drivers/scsi/aha1542.c
+++ b/drivers/scsi/aha1542.c
@@ -950,7 +950,6 @@ static struct scsi_host_template driver_template = {
 	.can_queue		= AHA1542_MAILBOXES, 
 	.this_id		= 7,
 	.sg_tablesize		= 16,
-	.cmd_per_lun		= 1,
 	.unchecked_isa_dma	= 1, 
 	.use_clustering		= ENABLE_CLUSTERING,
 };
diff --git a/drivers/scsi/aha1740.c b/drivers/scsi/aha1740.c
index 31ace4bef8fe..bad35ffc015d 100644
--- a/drivers/scsi/aha1740.c
+++ b/drivers/scsi/aha1740.c
@@ -544,7 +544,6 @@ static struct scsi_host_template aha1740_template = {
 	.can_queue        = AHA1740_ECBS,
 	.this_id          = 7,
 	.sg_tablesize     = AHA1740_SCATTER,
-	.cmd_per_lun      = AHA1740_CMDLUN,
 	.use_clustering   = ENABLE_CLUSTERING,
 	.eh_abort_handler = aha1740_eh_abort_handler,
 };
diff --git a/drivers/scsi/aha1740.h b/drivers/scsi/aha1740.h
index af23fd6bd795..b0c5603461ca 100644
--- a/drivers/scsi/aha1740.h
+++ b/drivers/scsi/aha1740.h
@@ -149,6 +149,5 @@ struct ecb {			/* Enhanced Control Block 6.1 */
 
 #define AHA1740_ECBS 32
 #define AHA1740_SCATTER 16
-#define AHA1740_CMDLUN 1
 
 #endif
diff --git a/drivers/scsi/aic94xx/aic94xx_init.c b/drivers/scsi/aic94xx/aic94xx_init.c
index 02a2512b76a8..4b135cca42a1 100644
--- a/drivers/scsi/aic94xx/aic94xx_init.c
+++ b/drivers/scsi/aic94xx/aic94xx_init.c
@@ -65,7 +65,6 @@ static struct scsi_host_template aic94xx_sht = {
 	.change_queue_depth	= sas_change_queue_depth,
 	.bios_param		= sas_bios_param,
 	.can_queue		= 1,
-	.cmd_per_lun		= 1,
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
 	.max_sectors		= SCSI_DEFAULT_MAX_SECTORS,
diff --git a/drivers/scsi/arm/arxescsi.c b/drivers/scsi/arm/arxescsi.c
index 32d23212de48..3110736fd337 100644
--- a/drivers/scsi/arm/arxescsi.c
+++ b/drivers/scsi/arm/arxescsi.c
@@ -245,7 +245,6 @@ static struct scsi_host_template arxescsi_template = {
 	.can_queue			= 0,
 	.this_id			= 7,
 	.sg_tablesize			= SG_ALL,
-	.cmd_per_lun			= 1,
 	.use_clustering			= DISABLE_CLUSTERING,
 	.proc_name			= "arxescsi",
 };
diff --git a/drivers/scsi/arm/cumana_2.c b/drivers/scsi/arm/cumana_2.c
index abc66f5263ec..faa1bee07c8a 100644
--- a/drivers/scsi/arm/cumana_2.c
+++ b/drivers/scsi/arm/cumana_2.c
@@ -367,7 +367,6 @@ static struct scsi_host_template cumanascsi2_template = {
 	.this_id			= 7,
 	.sg_tablesize			= SCSI_MAX_SG_CHAIN_SEGMENTS,
 	.dma_boundary			= IOMD_DMA_BOUNDARY,
-	.cmd_per_lun			= 1,
 	.use_clustering			= DISABLE_CLUSTERING,
 	.proc_name			= "cumanascsi2",
 };
diff --git a/drivers/scsi/arm/eesox.c b/drivers/scsi/arm/eesox.c
index 5bf3c0d134b4..a8ad6880dd91 100644
--- a/drivers/scsi/arm/eesox.c
+++ b/drivers/scsi/arm/eesox.c
@@ -486,7 +486,6 @@ static struct scsi_host_template eesox_template = {
 	.this_id			= 7,
 	.sg_tablesize			= SCSI_MAX_SG_CHAIN_SEGMENTS,
 	.dma_boundary			= IOMD_DMA_BOUNDARY,
-	.cmd_per_lun			= 1,
 	.use_clustering			= DISABLE_CLUSTERING,
 	.proc_name			= "eesox",
 };
diff --git a/drivers/scsi/atp870u.c b/drivers/scsi/atp870u.c
index 0836433e3a2d..05301bc752ee 100644
--- a/drivers/scsi/atp870u.c
+++ b/drivers/scsi/atp870u.c
@@ -3158,7 +3158,6 @@ static struct scsi_host_template atp870u_template = {
      .can_queue         	= qcnt			/* can_queue */,
      .this_id           	= 7			/* SCSI ID */,
      .sg_tablesize      	= ATP870U_SCATTER	/*SG_ALL*/ /*SG_NONE*/,
-     .cmd_per_lun       	= ATP870U_CMDLUN		/* commands per lun */,
      .use_clustering    	= ENABLE_CLUSTERING,
      .max_sectors		= ATP870U_MAX_SECTORS,
 };
diff --git a/drivers/scsi/atp870u.h b/drivers/scsi/atp870u.h
index 62bae64a01c1..5cf62566ad42 100644
--- a/drivers/scsi/atp870u.h
+++ b/drivers/scsi/atp870u.h
@@ -10,7 +10,6 @@
 #define MAX_SENSE 	14
 #define qcnt	       	32
 #define ATP870U_SCATTER 	128
-#define ATP870U_CMDLUN  	1
 
 #define MAX_ADAPTER	8
 #define MAX_SCSI_ID	16
diff --git a/drivers/scsi/be2iscsi/be_cmds.c b/drivers/scsi/be2iscsi/be_cmds.c
index 447cf7ce606e..185391a64d4b 100644
--- a/drivers/scsi/be2iscsi/be_cmds.c
+++ b/drivers/scsi/be2iscsi/be_cmds.c
@@ -452,6 +452,7 @@ void beiscsi_async_link_state_process(struct beiscsi_hba *phba,
 		    ((evt->port_link_status & ASYNC_EVENT_LOGICAL) &&
 		     (evt->port_fault == BEISCSI_PHY_LINK_FAULT_NONE))) {
 		phba->state = BE_ADAPTER_LINK_UP | BE_ADAPTER_CHECK_BOOT;
+		phba->get_boot = BE_GET_BOOT_RETRIES;
 
 		beiscsi_log(phba, KERN_ERR,
 			    BEISCSI_LOG_CONFIG | BEISCSI_LOG_INIT,
@@ -480,6 +481,7 @@ int beiscsi_process_mcc(struct beiscsi_hba *phba)
 				case ASYNC_EVENT_NEW_ISCSI_CONN:
 				case ASYNC_EVENT_NEW_TCP_CONN:
 					phba->state |= BE_ADAPTER_CHECK_BOOT;
+					phba->get_boot = BE_GET_BOOT_RETRIES;
 					beiscsi_log(phba, KERN_ERR,
 						    BEISCSI_LOG_CONFIG |
 						    BEISCSI_LOG_MBOX,
@@ -488,6 +490,8 @@ int beiscsi_process_mcc(struct beiscsi_hba *phba)
 						    compl->flags);
 					break;
 				default:
+					phba->state |= BE_ADAPTER_CHECK_BOOT;
+					phba->get_boot = BE_GET_BOOT_RETRIES;
 					beiscsi_log(phba, KERN_ERR,
 						    BEISCSI_LOG_CONFIG |
 						    BEISCSI_LOG_MBOX,
diff --git a/drivers/scsi/be2iscsi/be_cmds.h b/drivers/scsi/be2iscsi/be_cmds.h
index f11d325fe696..cdfbc5c19cf4 100644
--- a/drivers/scsi/be2iscsi/be_cmds.h
+++ b/drivers/scsi/be2iscsi/be_cmds.h
@@ -304,6 +304,17 @@ struct mgmt_auth_method_format {
 	struct	mgmt_chap_format chap;
 } __packed;
 
+struct be_cmd_req_logout_fw_sess {
+	struct be_cmd_req_hdr hdr;	/* dw[4] */
+	uint32_t session_handle;
+} __packed;
+
+struct be_cmd_resp_logout_fw_sess {
+	struct be_cmd_resp_hdr hdr;	/* dw[4] */
+#define BEISCSI_MGMT_SESSION_CLOSE 0x20
+	uint32_t session_status;
+} __packed;
+
 struct mgmt_conn_login_options {
 	u8 flags;
 	u8 header_digest;
@@ -1136,6 +1147,7 @@ struct be_cmd_get_all_if_id_req {
 #define OPCODE_ISCSI_INI_CFG_GET_HBA_NAME	6
 #define OPCODE_ISCSI_INI_CFG_SET_HBA_NAME	7
 #define OPCODE_ISCSI_INI_SESSION_GET_A_SESSION  14
+#define OPCODE_ISCSI_INI_SESSION_LOGOUT_TARGET	 24
 #define OPCODE_ISCSI_INI_DRIVER_REOPEN_ALL_SESSIONS 36
 #define OPCODE_ISCSI_INI_DRIVER_OFFLOAD_SESSION 41
 #define OPCODE_ISCSI_INI_DRIVER_INVALIDATE_CONNECTION 42
diff --git a/drivers/scsi/be2iscsi/be_main.c b/drivers/scsi/be2iscsi/be_main.c
index 1f74760ce86c..7a6dbfbccec9 100644
--- a/drivers/scsi/be2iscsi/be_main.c
+++ b/drivers/scsi/be2iscsi/be_main.c
@@ -668,14 +668,20 @@ static int beiscsi_enable_pci(struct pci_dev *pcidev)
 		return ret;
 	}
 
+	ret = pci_request_regions(pcidev, DRV_NAME);
+	if (ret) {
+		dev_err(&pcidev->dev,
+				"beiscsi_enable_pci - request region failed\n");
+		goto pci_dev_disable;
+	}
+
 	pci_set_master(pcidev);
 	ret = pci_set_dma_mask(pcidev, DMA_BIT_MASK(64));
 	if (ret) {
 		ret = pci_set_dma_mask(pcidev, DMA_BIT_MASK(32));
 		if (ret) {
 			dev_err(&pcidev->dev, "Could not set PCI DMA Mask\n");
-			pci_disable_device(pcidev);
-			return ret;
+			goto pci_region_release;
 		} else {
 			ret = pci_set_consistent_dma_mask(pcidev,
 							  DMA_BIT_MASK(32));
@@ -684,11 +690,17 @@ static int beiscsi_enable_pci(struct pci_dev *pcidev)
 		ret = pci_set_consistent_dma_mask(pcidev, DMA_BIT_MASK(64));
 		if (ret) {
 			dev_err(&pcidev->dev, "Could not set PCI DMA Mask\n");
-			pci_disable_device(pcidev);
-			return ret;
+			goto pci_region_release;
 		}
 	}
 	return 0;
+
+pci_region_release:
+	pci_release_regions(pcidev);
+pci_dev_disable:
+	pci_disable_device(pcidev);
+
+	return ret;
 }
 
 static int be_ctrl_init(struct beiscsi_hba *phba, struct pci_dev *pdev)
@@ -1356,8 +1368,10 @@ be_complete_io(struct beiscsi_conn *beiscsi_conn,
 	if (io_task->cmd_bhs->iscsi_hdr.flags & ISCSI_FLAG_CMD_READ)
 		conn->rxdata_octets += resid;
 unmap:
-	scsi_dma_unmap(io_task->scsi_cmnd);
-	io_task->scsi_cmnd = NULL;
+	if (io_task->scsi_cmnd) {
+		scsi_dma_unmap(io_task->scsi_cmnd);
+		io_task->scsi_cmnd = NULL;
+	}
 	iscsi_complete_scsi_task(task, exp_cmdsn, max_cmdsn);
 }
 
@@ -2037,11 +2051,16 @@ static void  beiscsi_process_mcc_isr(struct beiscsi_hba *phba)
 				/* Interpret compl as a async link evt */
 				beiscsi_async_link_state_process(phba,
 				(struct be_async_event_link_state *) mcc_compl);
-			else
+			else {
 				beiscsi_log(phba, KERN_ERR, BEISCSI_LOG_MBOX,
 					    "BM_%d :  Unsupported Async Event, flags"
 					    " = 0x%08x\n",
 					    mcc_compl->flags);
+				if (phba->state & BE_ADAPTER_LINK_UP) {
+					phba->state |= BE_ADAPTER_CHECK_BOOT;
+					phba->get_boot = BE_GET_BOOT_RETRIES;
+				}
+			}
 		} else if (mcc_compl->flags & CQE_FLAGS_COMPLETED_MASK) {
 			be_mcc_compl_process_isr(&phba->ctrl, mcc_compl);
 			atomic_dec(&phba->ctrl.mcc_obj.q.used);
@@ -3678,14 +3697,16 @@ static void be_mcc_queues_destroy(struct beiscsi_hba *phba)
 	struct be_ctrl_info *ctrl = &phba->ctrl;
 
 	q = &phba->ctrl.mcc_obj.q;
-	if (q->created)
+	if (q->created) {
 		beiscsi_cmd_q_destroy(ctrl, q, QTYPE_MCCQ);
-	be_queue_free(phba, q);
+		be_queue_free(phba, q);
+	}
 
 	q = &phba->ctrl.mcc_obj.cq;
-	if (q->created)
+	if (q->created) {
 		beiscsi_cmd_q_destroy(ctrl, q, QTYPE_CQ);
-	be_queue_free(phba, q);
+		be_queue_free(phba, q);
+	}
 }
 
 static void hwi_cleanup(struct beiscsi_hba *phba)
@@ -3729,8 +3750,10 @@ static void hwi_cleanup(struct beiscsi_hba *phba)
 
 	for (i = 0; i < (phba->num_cpus); i++) {
 		q = &phwi_context->be_cq[i];
-		if (q->created)
+		if (q->created) {
+			be_queue_free(phba, q);
 			beiscsi_cmd_q_destroy(ctrl, q, QTYPE_CQ);
+		}
 	}
 
 	be_mcc_queues_destroy(phba);
@@ -3740,8 +3763,10 @@ static void hwi_cleanup(struct beiscsi_hba *phba)
 		eq_for_mcc = 0;
 	for (i = 0; i < (phba->num_cpus + eq_for_mcc); i++) {
 		q = &phwi_context->be_eq[i].q;
-		if (q->created)
+		if (q->created) {
+			be_queue_free(phba, q);
 			beiscsi_cmd_q_destroy(ctrl, q, QTYPE_EQ);
+		}
 	}
 	be_cmd_fw_uninit(ctrl);
 }
@@ -4328,8 +4353,14 @@ static int beiscsi_get_boot_info(struct beiscsi_hba *phba)
 		beiscsi_log(phba, KERN_ERR,
 			    BEISCSI_LOG_INIT | BEISCSI_LOG_CONFIG,
 			    "BM_%d : No boot session\n");
+
+		if (ret == -ENXIO)
+			phba->get_boot = 0;
+
+
 		return ret;
 	}
+	phba->get_boot = 0;
 	nonemb_cmd.va = pci_zalloc_consistent(phba->ctrl.pdev,
 					      sizeof(*session_resp),
 					      &nonemb_cmd.dma);
@@ -4369,6 +4400,9 @@ static int beiscsi_get_boot_info(struct beiscsi_hba *phba)
 
 	memcpy(&phba->boot_sess, &session_resp->session_info,
 	       sizeof(struct mgmt_session_info));
+
+	 beiscsi_logout_fw_sess(phba,
+				phba->boot_sess.session_handle);
 	ret = 0;
 
 boot_freemem:
@@ -4580,11 +4614,13 @@ beiscsi_free_mgmt_task_handles(struct beiscsi_conn *beiscsi_conn,
 		spin_unlock_bh(&phba->mgmt_sgl_lock);
 	}
 
-	if (io_task->mtask_addr)
+	if (io_task->mtask_addr) {
 		pci_unmap_single(phba->pcidev,
 				 io_task->mtask_addr,
 				 io_task->mtask_data_count,
 				 PCI_DMA_TODEVICE);
+		io_task->mtask_addr = 0;
+	}
 }
 
 /**
@@ -5264,6 +5300,7 @@ static void beiscsi_remove(struct pci_dev *pcidev)
 	iscsi_host_free(phba->shost);
 	pci_disable_pcie_error_reporting(pcidev);
 	pci_set_drvdata(pcidev, NULL);
+	pci_release_regions(pcidev);
 	pci_disable_device(pcidev);
 }
 
@@ -5374,8 +5411,14 @@ beiscsi_hw_health_check(struct work_struct *work)
 	be_eqd_update(phba);
 
 	if (phba->state & BE_ADAPTER_CHECK_BOOT) {
-		phba->state &= ~BE_ADAPTER_CHECK_BOOT;
-		be_check_boot_session(phba);
+		if ((phba->get_boot > 0) && (!phba->boot_kset)) {
+			phba->get_boot--;
+			if (!(phba->get_boot % BE_GET_BOOT_TO))
+				be_check_boot_session(phba);
+		} else {
+			phba->state &= ~BE_ADAPTER_CHECK_BOOT;
+			phba->get_boot = 0;
+		}
 	}
 
 	beiscsi_ue_detect(phba);
@@ -5738,6 +5781,7 @@ hba_free:
 	iscsi_host_free(phba->shost);
 	pci_set_drvdata(pcidev, NULL);
 disable_pci:
+	pci_release_regions(pcidev);
 	pci_disable_device(pcidev);
 	return ret;
 }
diff --git a/drivers/scsi/be2iscsi/be_main.h b/drivers/scsi/be2iscsi/be_main.h
index e70ea26bbc2b..b8c0c7819cb1 100644
--- a/drivers/scsi/be2iscsi/be_main.h
+++ b/drivers/scsi/be2iscsi/be_main.h
@@ -36,7 +36,7 @@
 #include <scsi/scsi_transport_iscsi.h>
 
 #define DRV_NAME		"be2iscsi"
-#define BUILD_STR		"10.4.114.0"
+#define BUILD_STR		"10.6.0.0"
 #define BE_NAME			"Avago Technologies OneConnect" \
 				"Open-iSCSI Driver version" BUILD_STR
 #define DRV_DESC		BE_NAME " " "Driver"
@@ -109,6 +109,9 @@
 
 #define BEISCSI_CLEAN_UNLOAD	0x01
 #define BEISCSI_EEH_UNLOAD	0x02
+
+#define BE_GET_BOOT_RETRIES	45
+#define BE_GET_BOOT_TO		20
 /**
  * hardware needs the async PDU buffers to be posted in multiples of 8
  * So have atleast 8 of them by default
@@ -413,6 +416,7 @@ struct beiscsi_hba {
 	} fw_config;
 
 	unsigned int state;
+	int get_boot;
 	bool fw_timeout;
 	bool ue_detected;
 	struct delayed_work beiscsi_hw_check_task;
diff --git a/drivers/scsi/be2iscsi/be_mgmt.c b/drivers/scsi/be2iscsi/be_mgmt.c
index c2c4d6975fb7..ca4016f20e76 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.c
+++ b/drivers/scsi/be2iscsi/be_mgmt.c
@@ -1707,3 +1707,72 @@ void beiscsi_offload_cxn_v2(struct beiscsi_offload_params *params,
 		     (params->dw[offsetof(struct amap_beiscsi_offload_params,
 		      exp_statsn) / 32] + 1));
 }
+
+/**
+ * beiscsi_logout_fw_sess()- Firmware Session Logout
+ * @phba: Device priv structure instance
+ * @fw_sess_handle: FW session handle
+ *
+ * Logout from the FW established sessions.
+ * returns
+ *  Success: 0
+ *  Failure: Non-Zero Value
+ *
+ */
+int beiscsi_logout_fw_sess(struct beiscsi_hba *phba,
+		uint32_t fw_sess_handle)
+{
+	struct be_ctrl_info *ctrl = &phba->ctrl;
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_logout_fw_sess *req;
+	struct be_cmd_resp_logout_fw_sess *resp;
+	unsigned int tag;
+	int rc;
+
+	beiscsi_log(phba, KERN_INFO,
+		    BEISCSI_LOG_CONFIG | BEISCSI_LOG_MBOX,
+		    "BG_%d : In bescsi_logout_fwboot_sess\n");
+
+	spin_lock(&ctrl->mbox_lock);
+	tag = alloc_mcc_tag(phba);
+	if (!tag) {
+		spin_unlock(&ctrl->mbox_lock);
+		beiscsi_log(phba, KERN_INFO,
+			    BEISCSI_LOG_CONFIG | BEISCSI_LOG_MBOX,
+			    "BG_%d : MBX Tag Failure\n");
+		return -EINVAL;
+	}
+
+	wrb = wrb_from_mccq(phba);
+	req = embedded_payload(wrb);
+	wrb->tag0 |= tag;
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ISCSI_INI,
+			   OPCODE_ISCSI_INI_SESSION_LOGOUT_TARGET,
+			   sizeof(struct be_cmd_req_logout_fw_sess));
+
+	/* Set the session handle */
+	req->session_handle = fw_sess_handle;
+	be_mcc_notify(phba);
+	spin_unlock(&ctrl->mbox_lock);
+
+	rc = beiscsi_mccq_compl(phba, tag, &wrb, NULL);
+	if (rc) {
+		beiscsi_log(phba, KERN_ERR,
+			    BEISCSI_LOG_INIT | BEISCSI_LOG_CONFIG,
+			    "BG_%d : MBX CMD FW_SESSION_LOGOUT_TARGET Failed\n");
+		return -EBUSY;
+	}
+
+	resp = embedded_payload(wrb);
+	if (resp->session_status !=
+		BEISCSI_MGMT_SESSION_CLOSE) {
+		beiscsi_log(phba, KERN_ERR,
+			    BEISCSI_LOG_INIT | BEISCSI_LOG_CONFIG,
+			    "BG_%d : FW_SESSION_LOGOUT_TARGET resp : 0x%x\n",
+			    resp->session_status);
+		rc = -EINVAL;
+	}
+
+	return rc;
+}
diff --git a/drivers/scsi/be2iscsi/be_mgmt.h b/drivers/scsi/be2iscsi/be_mgmt.h
index 9356b9a86b66..b58a7decbd94 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.h
+++ b/drivers/scsi/be2iscsi/be_mgmt.h
@@ -338,4 +338,7 @@ void beiscsi_ue_detect(struct beiscsi_hba *phba);
 int be_cmd_modify_eq_delay(struct beiscsi_hba *phba,
 			 struct be_set_eqd *, int num);
 
+int beiscsi_logout_fw_sess(struct beiscsi_hba *phba,
+			    uint32_t fw_sess_handle);
+
 #endif
diff --git a/drivers/scsi/bnx2i/bnx2i_iscsi.c b/drivers/scsi/bnx2i/bnx2i_iscsi.c
index e53078d03309..72894378ffcf 100644
--- a/drivers/scsi/bnx2i/bnx2i_iscsi.c
+++ b/drivers/scsi/bnx2i/bnx2i_iscsi.c
@@ -1173,8 +1173,10 @@ static void bnx2i_cleanup_task(struct iscsi_task *task)
 		bnx2i_send_cmd_cleanup_req(hba, task->dd_data);
 
 		spin_unlock_bh(&conn->session->back_lock);
+		spin_unlock_bh(&conn->session->frwd_lock);
 		wait_for_completion_timeout(&bnx2i_conn->cmd_cleanup_cmpl,
 				msecs_to_jiffies(ISCSI_CMD_CLEANUP_TIMEOUT));
+		spin_lock_bh(&conn->session->frwd_lock);
 		spin_lock_bh(&conn->session->back_lock);
 	}
 	bnx2i_iscsi_unmap_sg_list(task->dd_data);
@@ -2093,7 +2095,8 @@ int bnx2i_hw_ep_disconnect(struct bnx2i_endpoint *bnx2i_ep)
 	else
 		/* wait for option-2 conn teardown */
 		wait_event_interruptible(bnx2i_ep->ofld_wait,
-				 bnx2i_ep->state != EP_STATE_DISCONN_START);
+				((bnx2i_ep->state != EP_STATE_DISCONN_START)
+				&& (bnx2i_ep->state != EP_STATE_TCP_FIN_RCVD)));
 
 	if (signal_pending(current))
 		flush_signals(current);
diff --git a/drivers/scsi/csiostor/csio_hw.c b/drivers/scsi/csiostor/csio_hw.c
index 2e66f34ebb79..622bdabc8894 100644
--- a/drivers/scsi/csiostor/csio_hw.c
+++ b/drivers/scsi/csiostor/csio_hw.c
@@ -3928,6 +3928,7 @@ csio_hw_init(struct csio_hw *hw)
 
 		evt_entry = kzalloc(sizeof(struct csio_evt_msg), GFP_KERNEL);
 		if (!evt_entry) {
+			rv = -ENOMEM;
 			csio_err(hw, "Failed to initialize eventq");
 			goto err_evtq_cleanup;
 		}
diff --git a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
index 3db4c63978c5..0e2bee937fe8 100644
--- a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
+++ b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
@@ -1,7 +1,7 @@
 /*
  * cxgb3i_offload.c: Chelsio S3xx iscsi offloaded tcp connection management
  *
- * Copyright (C) 2003-2008 Chelsio Communications.  All rights reserved.
+ * Copyright (C) 2003-2015 Chelsio Communications.  All rights reserved.
  *
  * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
@@ -32,8 +32,8 @@ static unsigned int dbg_level;
 
 #define DRV_MODULE_NAME         "cxgb3i"
 #define DRV_MODULE_DESC         "Chelsio T3 iSCSI Driver"
-#define DRV_MODULE_VERSION	"2.0.0"
-#define DRV_MODULE_RELDATE	"Jun. 2010"
+#define DRV_MODULE_VERSION	"2.0.1-ko"
+#define DRV_MODULE_RELDATE	"Apr. 2015"
 
 static char version[] =
 	DRV_MODULE_DESC " " DRV_MODULE_NAME
@@ -156,7 +156,7 @@ static int push_tx_frames(struct cxgbi_sock *csk, int req_completion);
 static void send_act_open_req(struct cxgbi_sock *csk, struct sk_buff *skb,
 			      const struct l2t_entry *e)
 {
-	unsigned int wscale = cxgbi_sock_compute_wscale(cxgb3i_rcv_win);
+	unsigned int wscale = cxgbi_sock_compute_wscale(csk->rcv_win);
 	struct cpl_act_open_req *req = (struct cpl_act_open_req *)skb->head;
 
 	skb->priority = CPL_PRIORITY_SETUP;
@@ -172,7 +172,7 @@ static void send_act_open_req(struct cxgbi_sock *csk, struct sk_buff *skb,
 			V_WND_SCALE(wscale) | V_MSS_IDX(csk->mss_idx) |
 			V_L2T_IDX(e->idx) | V_TX_CHANNEL(e->smt_idx));
 	req->opt0l = htonl(V_ULP_MODE(ULP2_MODE_ISCSI) |
-			V_RCV_BUFSIZ(cxgb3i_rcv_win>>10));
+			V_RCV_BUFSIZ(csk->rcv_win >> 10));
 
 	log_debug(1 << CXGBI_DBG_TOE | 1 << CXGBI_DBG_SOCK,
 		"csk 0x%p,%u,0x%lx,%u, %pI4:%u-%pI4:%u, %u,%u,%u.\n",
@@ -369,7 +369,7 @@ static inline void make_tx_data_wr(struct cxgbi_sock *csk, struct sk_buff *skb,
 		req->flags |= htonl(V_TX_ACK_PAGES(2) | F_TX_INIT |
 				    V_TX_CPU_IDX(csk->rss_qid));
 		/* sendbuffer is in units of 32KB. */
-		req->param |= htonl(V_TX_SNDBUF(cxgb3i_snd_win >> 15));
+		req->param |= htonl(V_TX_SNDBUF(csk->snd_win >> 15));
 		cxgbi_sock_set_flag(csk, CTPF_TX_DATA_SENT);
 	}
 }
@@ -503,8 +503,8 @@ static int do_act_establish(struct t3cdev *tdev, struct sk_buff *skb, void *ctx)
 			csk, csk->state, csk->flags, csk->tid);
 
 	csk->copied_seq = csk->rcv_wup = csk->rcv_nxt = rcv_isn;
-	if (cxgb3i_rcv_win > (M_RCV_BUFSIZ << 10))
-		csk->rcv_wup -= cxgb3i_rcv_win - (M_RCV_BUFSIZ << 10);
+	if (csk->rcv_win > (M_RCV_BUFSIZ << 10))
+		csk->rcv_wup -= csk->rcv_win - (M_RCV_BUFSIZ << 10);
 
 	cxgbi_sock_established(csk, ntohl(req->snd_isn), ntohs(req->tcp_opt));
 
@@ -988,6 +988,8 @@ static int init_act_open(struct cxgbi_sock *csk)
 		goto rel_resource;
 	skb->sk = (struct sock *)csk;
 	set_arp_failure_handler(skb, act_open_arp_failure);
+	csk->snd_win = cxgb3i_snd_win;
+	csk->rcv_win = cxgb3i_rcv_win;
 
 	csk->wr_max_cred = csk->wr_cred = T3C_DATA(t3dev)->max_wrs - 1;
 	csk->wr_una_cred = 0;
@@ -1320,8 +1322,6 @@ static void cxgb3i_dev_open(struct t3cdev *t3dev)
 	cdev->nports = adapter->params.nports;
 	cdev->mtus = adapter->params.mtus;
 	cdev->nmtus = NMTUS;
-	cdev->snd_win = cxgb3i_snd_win;
-	cdev->rcv_win = cxgb3i_rcv_win;
 	cdev->rx_credit_thres = cxgb3i_rx_credit_thres;
 	cdev->skb_tx_rsvd = CXGB3I_TX_HEADER_LEN;
 	cdev->skb_rx_extra = sizeof(struct cpl_iscsi_hdr_norss);
diff --git a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.h b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.h
index 20593fd69d8f..b0430c9359e7 100644
--- a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.h
+++ b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.h
@@ -1,7 +1,7 @@
 /*
  * cxgb3i.h: Chelsio S3xx iSCSI driver.
  *
- * Copyright (c) 2008 Chelsio Communications, Inc.
+ * Copyright (c) 2008-2015 Chelsio Communications, 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
diff --git a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
index dd00e5fe4a5e..de6feb8964c9 100644
--- a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
+++ b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
@@ -1,7 +1,7 @@
 /*
  * cxgb4i.c: Chelsio T4 iSCSI driver.
  *
- * Copyright (c) 2010 Chelsio Communications, Inc.
+ * Copyright (c) 2010-2015 Chelsio Communications, 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
@@ -36,11 +36,12 @@ static unsigned int dbg_level;
 
 #define	DRV_MODULE_NAME		"cxgb4i"
 #define DRV_MODULE_DESC		"Chelsio T4/T5 iSCSI Driver"
-#define	DRV_MODULE_VERSION	"0.9.4"
+#define	DRV_MODULE_VERSION	"0.9.5-ko"
+#define DRV_MODULE_RELDATE	"Apr. 2015"
 
 static char version[] =
 	DRV_MODULE_DESC " " DRV_MODULE_NAME
-	" v" DRV_MODULE_VERSION "\n";
+	" v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
 
 MODULE_AUTHOR("Chelsio Communications, Inc.");
 MODULE_DESCRIPTION(DRV_MODULE_DESC);
@@ -50,11 +51,13 @@ MODULE_LICENSE("GPL");
 module_param(dbg_level, uint, 0644);
 MODULE_PARM_DESC(dbg_level, "Debug flag (default=0)");
 
-static int cxgb4i_rcv_win = 256 * 1024;
+#define CXGB4I_DEFAULT_10G_RCV_WIN (256 * 1024)
+static int cxgb4i_rcv_win = -1;
 module_param(cxgb4i_rcv_win, int, 0644);
 MODULE_PARM_DESC(cxgb4i_rcv_win, "TCP reveive window in bytes");
 
-static int cxgb4i_snd_win = 128 * 1024;
+#define CXGB4I_DEFAULT_10G_SND_WIN (128 * 1024)
+static int cxgb4i_snd_win = -1;
 module_param(cxgb4i_snd_win, int, 0644);
 MODULE_PARM_DESC(cxgb4i_snd_win, "TCP send window in bytes");
 
@@ -196,10 +199,10 @@ static void send_act_open_req(struct cxgbi_sock *csk, struct sk_buff *skb,
 		TX_CHAN_V(csk->tx_chan) |
 		SMAC_SEL_V(csk->smac_idx) |
 		ULP_MODE_V(ULP_MODE_ISCSI) |
-		RCV_BUFSIZ_V(cxgb4i_rcv_win >> 10);
+		RCV_BUFSIZ_V(csk->rcv_win >> 10);
+
 	opt2 = RX_CHANNEL_V(0) |
 		RSS_QUEUE_VALID_F |
-		(RX_FC_DISABLE_F) |
 		RSS_QUEUE_V(csk->rss_qid);
 
 	if (is_t4(lldi->adapter_type)) {
@@ -228,6 +231,7 @@ static void send_act_open_req(struct cxgbi_sock *csk, struct sk_buff *skb,
 	} else {
 		struct cpl_t5_act_open_req *req =
 				(struct cpl_t5_act_open_req *)skb->head;
+		u32 isn = (prandom_u32() & ~7UL) - 1;
 
 		INIT_TP_WR(req, 0);
 		OPCODE_TID(req) = cpu_to_be32(MK_OPCODE_TID(CPL_ACT_OPEN_REQ,
@@ -241,7 +245,10 @@ static void send_act_open_req(struct cxgbi_sock *csk, struct sk_buff *skb,
 				cxgb4_select_ntuple(
 					csk->cdev->ports[csk->port_id],
 					csk->l2t)));
-		opt2 |= 1 << 31;
+		req->rsvd = cpu_to_be32(isn);
+		opt2 |= T5_ISS_VALID;
+		opt2 |= T5_OPT_2_VALID_F;
+
 		req->opt2 = cpu_to_be32(opt2);
 
 		log_debug(1 << CXGBI_DBG_TOE | 1 << CXGBI_DBG_SOCK,
@@ -279,7 +286,7 @@ static void send_act_open_req6(struct cxgbi_sock *csk, struct sk_buff *skb,
 		TX_CHAN_V(csk->tx_chan) |
 		SMAC_SEL_V(csk->smac_idx) |
 		ULP_MODE_V(ULP_MODE_ISCSI) |
-		RCV_BUFSIZ_V(cxgb4i_rcv_win >> 10);
+		RCV_BUFSIZ_V(csk->rcv_win >> 10);
 
 	opt2 = RX_CHANNEL_V(0) |
 		RSS_QUEUE_VALID_F |
@@ -544,7 +551,7 @@ static inline int send_tx_flowc_wr(struct cxgbi_sock *csk)
 	flowc->mnemval[5].mnemonic = FW_FLOWC_MNEM_RCVNXT;
 	flowc->mnemval[5].val = htonl(csk->rcv_nxt);
 	flowc->mnemval[6].mnemonic = FW_FLOWC_MNEM_SNDBUF;
-	flowc->mnemval[6].val = htonl(cxgb4i_snd_win);
+	flowc->mnemval[6].val = htonl(csk->snd_win);
 	flowc->mnemval[7].mnemonic = FW_FLOWC_MNEM_MSS;
 	flowc->mnemval[7].val = htonl(csk->advmss);
 	flowc->mnemval[8].mnemonic = 0;
@@ -557,7 +564,7 @@ static inline int send_tx_flowc_wr(struct cxgbi_sock *csk)
 	log_debug(1 << CXGBI_DBG_TOE | 1 << CXGBI_DBG_SOCK,
 		"csk 0x%p, tid 0x%x, %u,%u,%u,%u,%u,%u,%u.\n",
 		csk, csk->tid, 0, csk->tx_chan, csk->rss_qid,
-		csk->snd_nxt, csk->rcv_nxt, cxgb4i_snd_win,
+		csk->snd_nxt, csk->rcv_nxt, csk->snd_win,
 		csk->advmss);
 
 	cxgb4_ofld_send(csk->cdev->ports[csk->port_id], skb);
@@ -750,8 +757,8 @@ static void do_act_establish(struct cxgbi_device *cdev, struct sk_buff *skb)
 	 * Causes the first RX_DATA_ACK to supply any Rx credits we couldn't
 	 * pass through opt0.
 	 */
-	if (cxgb4i_rcv_win > (RCV_BUFSIZ_MASK << 10))
-		csk->rcv_wup -= cxgb4i_rcv_win - (RCV_BUFSIZ_MASK << 10);
+	if (csk->rcv_win > (RCV_BUFSIZ_MASK << 10))
+		csk->rcv_wup -= csk->rcv_win - (RCV_BUFSIZ_MASK << 10);
 
 	csk->advmss = lldi->mtus[TCPOPT_MSS_G(tcp_opt)] - 40;
 	if (TCPOPT_TSTAMP_G(tcp_opt))
@@ -1367,6 +1374,8 @@ static int init_act_open(struct cxgbi_sock *csk)
 	unsigned int step;
 	unsigned int size, size6;
 	int t4 = is_t4(lldi->adapter_type);
+	unsigned int linkspeed;
+	unsigned int rcv_winf, snd_winf;
 
 	log_debug(1 << CXGBI_DBG_TOE | 1 << CXGBI_DBG_SOCK,
 		"csk 0x%p,%u,0x%lx,%u.\n",
@@ -1440,6 +1449,21 @@ static int init_act_open(struct cxgbi_sock *csk)
 	csk->txq_idx = cxgb4_port_idx(ndev) * step;
 	step = lldi->nrxq / lldi->nchan;
 	csk->rss_qid = lldi->rxq_ids[cxgb4_port_idx(ndev) * step];
+	linkspeed = ((struct port_info *)netdev_priv(ndev))->link_cfg.speed;
+	csk->snd_win = cxgb4i_snd_win;
+	csk->rcv_win = cxgb4i_rcv_win;
+	if (cxgb4i_rcv_win <= 0) {
+		csk->rcv_win = CXGB4I_DEFAULT_10G_RCV_WIN;
+		rcv_winf = linkspeed / SPEED_10000;
+		if (rcv_winf)
+			csk->rcv_win *= rcv_winf;
+	}
+	if (cxgb4i_snd_win <= 0) {
+		csk->snd_win = CXGB4I_DEFAULT_10G_SND_WIN;
+		snd_winf = linkspeed / SPEED_10000;
+		if (snd_winf)
+			csk->snd_win *= snd_winf;
+	}
 	csk->wr_cred = lldi->wr_cred -
 		       DIV_ROUND_UP(sizeof(struct cpl_abort_req), 16);
 	csk->wr_max_cred = csk->wr_cred;
@@ -1758,8 +1782,6 @@ static void *t4_uld_add(const struct cxgb4_lld_info *lldi)
 	cdev->nports = lldi->nports;
 	cdev->mtus = lldi->mtus;
 	cdev->nmtus = NMTUS;
-	cdev->snd_win = cxgb4i_snd_win;
-	cdev->rcv_win = cxgb4i_rcv_win;
 	cdev->rx_credit_thres = cxgb4i_rx_credit_thres;
 	cdev->skb_tx_rsvd = CXGB4I_TX_HEADER_LEN;
 	cdev->skb_rx_extra = sizeof(struct cpl_iscsi_hdr);
diff --git a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.h b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.h
index 1096026ba241..22dd8d670e4a 100644
--- a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.h
+++ b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.h
@@ -1,7 +1,7 @@
 /*
  * cxgb4i.h: Chelsio T4 iSCSI driver.
  *
- * Copyright (c) 2010 Chelsio Communications, Inc.
+ * Copyright (c) 2010-2015 Chelsio Communications, 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
@@ -23,6 +23,8 @@
 #define CXGB4I_TX_HEADER_LEN \
 	(sizeof(struct fw_ofld_tx_data_wr) + sizeof(struct sge_opaque_hdr))
 
+#define T5_ISS_VALID		(1 << 18)
+
 struct ulptx_idata {
 	__be32 cmd_more;
 	__be32 len;
diff --git a/drivers/scsi/cxgbi/libcxgbi.c b/drivers/scsi/cxgbi/libcxgbi.c
index eb58afcfb73b..f3bb7af4e984 100644
--- a/drivers/scsi/cxgbi/libcxgbi.c
+++ b/drivers/scsi/cxgbi/libcxgbi.c
@@ -1,7 +1,7 @@
 /*
  * libcxgbi.c: Chelsio common library for T3/T4 iSCSI driver.
  *
- * Copyright (c) 2010 Chelsio Communications, Inc.
+ * Copyright (c) 2010-2015 Chelsio Communications, 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
@@ -38,8 +38,12 @@ static unsigned int dbg_level;
 
 #define DRV_MODULE_NAME		"libcxgbi"
 #define DRV_MODULE_DESC		"Chelsio iSCSI driver library"
-#define DRV_MODULE_VERSION	"0.9.0"
-#define DRV_MODULE_RELDATE	"Jun. 2010"
+#define DRV_MODULE_VERSION	"0.9.1-ko"
+#define DRV_MODULE_RELDATE	"Apr. 2015"
+
+static char version[] =
+	DRV_MODULE_DESC " " DRV_MODULE_NAME
+	" v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
 
 MODULE_AUTHOR("Chelsio Communications, Inc.");
 MODULE_DESCRIPTION(DRV_MODULE_DESC);
@@ -728,7 +732,7 @@ static struct cxgbi_sock *cxgbi_check_route6(struct sockaddr *dst_addr)
 	}
 	ndev = n->dev;
 
-	if (ipv6_addr_is_multicast(&rt->rt6i_dst.addr)) {
+	if (ipv6_addr_is_multicast(&daddr6->sin6_addr)) {
 		pr_info("multi-cast route %pI6 port %u, dev %s.\n",
 			daddr6->sin6_addr.s6_addr,
 			ntohs(daddr6->sin6_port), ndev->name);
@@ -1126,11 +1130,11 @@ static int cxgbi_sock_send_pdus(struct cxgbi_sock *csk, struct sk_buff *skb)
 		goto out_err;
 	}
 
-	if (csk->write_seq - csk->snd_una >= cdev->snd_win) {
+	if (csk->write_seq - csk->snd_una >= csk->snd_win) {
 		log_debug(1 << CXGBI_DBG_PDU_TX,
 			"csk 0x%p,%u,0x%lx,%u, FULL %u-%u >= %u.\n",
 			csk, csk->state, csk->flags, csk->tid, csk->write_seq,
-			csk->snd_una, cdev->snd_win);
+			csk->snd_una, csk->snd_win);
 		err = -ENOBUFS;
 		goto out_err;
 	}
@@ -1885,7 +1889,7 @@ static void csk_return_rx_credits(struct cxgbi_sock *csk, int copied)
 		"csk 0x%p,%u,0x%lx,%u, seq %u, wup %u, thre %u, %u.\n",
 		csk, csk->state, csk->flags, csk->tid, csk->copied_seq,
 		csk->rcv_wup, cdev->rx_credit_thres,
-		cdev->rcv_win);
+		csk->rcv_win);
 
 	if (csk->state != CTP_ESTABLISHED)
 		return;
@@ -1896,7 +1900,7 @@ static void csk_return_rx_credits(struct cxgbi_sock *csk, int copied)
 	if (unlikely(cdev->rx_credit_thres == 0))
 		return;
 
-	must_send = credits + 16384 >= cdev->rcv_win;
+	must_send = credits + 16384 >= csk->rcv_win;
 	if (must_send || credits >= cdev->rx_credit_thres)
 		csk->rcv_wup += cdev->csk_send_rx_credits(csk, credits);
 }
@@ -2913,6 +2917,8 @@ static int __init libcxgbi_init_module(void)
 	sw_tag_idx_bits = (__ilog2_u32(ISCSI_ITT_MASK)) + 1;
 	sw_tag_age_bits = (__ilog2_u32(ISCSI_AGE_MASK)) + 1;
 
+	pr_info("%s", version);
+
 	pr_info("tag itt 0x%x, %u bits, age 0x%x, %u bits.\n",
 		ISCSI_ITT_MASK, sw_tag_idx_bits,
 		ISCSI_AGE_MASK, sw_tag_age_bits);
diff --git a/drivers/scsi/cxgbi/libcxgbi.h b/drivers/scsi/cxgbi/libcxgbi.h
index aba1af720df6..9842301f7980 100644
--- a/drivers/scsi/cxgbi/libcxgbi.h
+++ b/drivers/scsi/cxgbi/libcxgbi.h
@@ -1,7 +1,7 @@
 /*
  * libcxgbi.h: Chelsio common library for T3/T4 iSCSI driver.
  *
- * Copyright (c) 2010 Chelsio Communications, Inc.
+ * Copyright (c) 2010-2015 Chelsio Communications, 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
@@ -234,6 +234,8 @@ struct cxgbi_sock {
 	u32 snd_nxt;
 	u32 snd_una;
 	u32 write_seq;
+	u32 snd_win;
+	u32 rcv_win;
 };
 
 /*
@@ -540,8 +542,6 @@ struct cxgbi_device {
 	struct iscsi_transport *itp;
 
 	unsigned int pfvf;
-	unsigned int snd_win;
-	unsigned int rcv_win;
 	unsigned int rx_credit_thres;
 	unsigned int skb_tx_rsvd;
 	unsigned int skb_rx_extra;	/* for msg coalesced mode */
@@ -685,10 +685,7 @@ static inline void *cxgbi_alloc_big_mem(unsigned int size,
 
 static inline void cxgbi_free_big_mem(void *addr)
 {
-	if (is_vmalloc_addr(addr))
-		vfree(addr);
-	else
-		kfree(addr);
+	kvfree(addr);
 }
 
 static inline void cxgbi_set_iscsi_ipv4(struct cxgbi_hba *chba, __be32 ipaddr)
diff --git a/drivers/scsi/dpt_i2o.c b/drivers/scsi/dpt_i2o.c
index 2806cfbec2b9..f35ed53adaac 100644
--- a/drivers/scsi/dpt_i2o.c
+++ b/drivers/scsi/dpt_i2o.c
@@ -3562,7 +3562,6 @@ static struct scsi_host_template driver_template = {
 	.slave_configure	= adpt_slave_configure,
 	.can_queue		= MAX_TO_IOP_MESSAGES,
 	.this_id		= 7,
-	.cmd_per_lun		= 1,
 	.use_clustering		= ENABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/fdomain.c b/drivers/scsi/fdomain.c
index fff682976c56..eefe14d453db 100644
--- a/drivers/scsi/fdomain.c
+++ b/drivers/scsi/fdomain.c
@@ -1764,7 +1764,6 @@ struct scsi_host_template fdomain_driver_template = {
 	.can_queue		= 1,
 	.this_id		= 6,
 	.sg_tablesize		= 64,
-	.cmd_per_lun		= 1,
 	.use_clustering		= DISABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/fnic/fnic_debugfs.c b/drivers/scsi/fnic/fnic_debugfs.c
index 5980c10c734d..d6498fabe628 100644
--- a/drivers/scsi/fnic/fnic_debugfs.c
+++ b/drivers/scsi/fnic/fnic_debugfs.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #include <linux/errno.h>
 #include <linux/debugfs.h>
+#include <linux/vmalloc.h>
 #include "fnic.h"
 
 static struct dentry *fnic_trace_debugfs_root;
diff --git a/drivers/scsi/fnic/fnic_trace.c b/drivers/scsi/fnic/fnic_trace.c
index 65a9bde26974..4e15c4bf0795 100644
--- a/drivers/scsi/fnic/fnic_trace.c
+++ b/drivers/scsi/fnic/fnic_trace.c
@@ -21,6 +21,7 @@
 #include <linux/spinlock.h>
 #include <linux/kallsyms.h>
 #include <linux/time.h>
+#include <linux/vmalloc.h>
 #include "fnic_io.h"
 #include "fnic.h"
 
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index 8eab107b53fb..1dafeb43333b 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -43,6 +43,8 @@
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_tcq.h>
+#include <scsi/scsi_eh.h>
+#include <scsi/scsi_dbg.h>
 #include <linux/cciss_ioctl.h>
 #include <linux/string.h>
 #include <linux/bitmap.h>
@@ -56,7 +58,7 @@
 #include "hpsa.h"
 
 /* HPSA_DRIVER_VERSION must be 3 byte values (0-255) separated by '.' */
-#define HPSA_DRIVER_VERSION "3.4.4-1"
+#define HPSA_DRIVER_VERSION "3.4.10-0"
 #define DRIVER_NAME "HP HPSA Driver (v " HPSA_DRIVER_VERSION ")"
 #define HPSA "hpsa"
 
@@ -129,6 +131,7 @@ static const struct pci_device_id hpsa_pci_device_id[] = {
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21CC},
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21CD},
 	{PCI_VENDOR_ID_HP,     PCI_DEVICE_ID_HP_CISSI,     0x103C, 0x21CE},
+	{PCI_VENDOR_ID_ADAPTEC2, 0x0290, 0x9005, 0x0580},
 	{PCI_VENDOR_ID_HP_3PAR, 0x0075, 0x1590, 0x0076},
 	{PCI_VENDOR_ID_HP_3PAR, 0x0075, 0x1590, 0x0087},
 	{PCI_VENDOR_ID_HP_3PAR, 0x0075, 0x1590, 0x007D},
@@ -186,6 +189,7 @@ static struct board_type products[] = {
 	{0x21CC103C, "Smart Array", &SA5_access},
 	{0x21CD103C, "Smart Array", &SA5_access},
 	{0x21CE103C, "Smart HBA", &SA5_access},
+	{0x05809005, "SmartHBA-SA", &SA5_access},
 	{0x00761590, "HP Storage P1224 Array Controller", &SA5_access},
 	{0x00871590, "HP Storage P1224e Array Controller", &SA5_access},
 	{0x007D1590, "HP Storage P1228 Array Controller", &SA5_access},
@@ -194,6 +198,10 @@ static struct board_type products[] = {
 	{0xFFFF103C, "Unknown Smart Array", &SA5_access},
 };
 
+#define SCSI_CMD_BUSY ((struct scsi_cmnd *)&hpsa_cmd_busy)
+static const struct scsi_cmnd hpsa_cmd_busy;
+#define SCSI_CMD_IDLE ((struct scsi_cmnd *)&hpsa_cmd_idle)
+static const struct scsi_cmnd hpsa_cmd_idle;
 static int number_of_controllers;
 
 static irqreturn_t do_hpsa_intr_intx(int irq, void *dev_id);
@@ -207,6 +215,9 @@ static int hpsa_compat_ioctl(struct scsi_device *dev, int cmd,
 
 static void cmd_free(struct ctlr_info *h, struct CommandList *c);
 static struct CommandList *cmd_alloc(struct ctlr_info *h);
+static void cmd_tagged_free(struct ctlr_info *h, struct CommandList *c);
+static struct CommandList *cmd_tagged_alloc(struct ctlr_info *h,
+					    struct scsi_cmnd *scmd);
 static int fill_cmd(struct CommandList *c, u8 cmd, struct ctlr_info *h,
 	void *buff, size_t size, u16 page_code, unsigned char *scsi3addr,
 	int cmd_type);
@@ -222,6 +233,7 @@ static int hpsa_change_queue_depth(struct scsi_device *sdev, int qdepth);
 static int hpsa_eh_device_reset_handler(struct scsi_cmnd *scsicmd);
 static int hpsa_eh_abort_handler(struct scsi_cmnd *scsicmd);
 static int hpsa_slave_alloc(struct scsi_device *sdev);
+static int hpsa_slave_configure(struct scsi_device *sdev);
 static void hpsa_slave_destroy(struct scsi_device *sdev);
 
 static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno);
@@ -232,7 +244,8 @@ static void check_ioctl_unit_attention(struct ctlr_info *h,
 /* performant mode helper functions */
 static void calc_bucket_map(int *bucket, int num_buckets,
 	int nsgs, int min_blocks, u32 *bucket_map);
-static void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h);
+static void hpsa_free_performant_mode(struct ctlr_info *h);
+static int hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h);
 static inline u32 next_command(struct ctlr_info *h, u8 q);
 static int hpsa_find_cfg_addrs(struct pci_dev *pdev, void __iomem *vaddr,
 			       u32 *cfg_base_addr, u64 *cfg_base_addr_index,
@@ -252,6 +265,8 @@ static int hpsa_scsi_ioaccel_queue_command(struct ctlr_info *h,
 	struct CommandList *c, u32 ioaccel_handle, u8 *cdb, int cdb_len,
 	u8 *scsi3addr, struct hpsa_scsi_dev_t *phys_disk);
 static void hpsa_command_resubmit_worker(struct work_struct *work);
+static u32 lockup_detected(struct ctlr_info *h);
+static int detect_controller_lockup(struct ctlr_info *h);
 
 static inline struct ctlr_info *sdev_to_hba(struct scsi_device *sdev)
 {
@@ -265,40 +280,86 @@ static inline struct ctlr_info *shost_to_hba(struct Scsi_Host *sh)
 	return (struct ctlr_info *) *priv;
 }
 
+static inline bool hpsa_is_cmd_idle(struct CommandList *c)
+{
+	return c->scsi_cmd == SCSI_CMD_IDLE;
+}
+
+static inline bool hpsa_is_pending_event(struct CommandList *c)
+{
+	return c->abort_pending || c->reset_pending;
+}
+
+/* extract sense key, asc, and ascq from sense data.  -1 means invalid. */
+static void decode_sense_data(const u8 *sense_data, int sense_data_len,
+			u8 *sense_key, u8 *asc, u8 *ascq)
+{
+	struct scsi_sense_hdr sshdr;
+	bool rc;
+
+	*sense_key = -1;
+	*asc = -1;
+	*ascq = -1;
+
+	if (sense_data_len < 1)
+		return;
+
+	rc = scsi_normalize_sense(sense_data, sense_data_len, &sshdr);
+	if (rc) {
+		*sense_key = sshdr.sense_key;
+		*asc = sshdr.asc;
+		*ascq = sshdr.ascq;
+	}
+}
+
 static int check_for_unit_attention(struct ctlr_info *h,
 	struct CommandList *c)
 {
-	if (c->err_info->SenseInfo[2] != UNIT_ATTENTION)
+	u8 sense_key, asc, ascq;
+	int sense_len;
+
+	if (c->err_info->SenseLen > sizeof(c->err_info->SenseInfo))
+		sense_len = sizeof(c->err_info->SenseInfo);
+	else
+		sense_len = c->err_info->SenseLen;
+
+	decode_sense_data(c->err_info->SenseInfo, sense_len,
+				&sense_key, &asc, &ascq);
+	if (sense_key != UNIT_ATTENTION || asc == -1)
 		return 0;
 
-	switch (c->err_info->SenseInfo[12]) {
+	switch (asc) {
 	case STATE_CHANGED:
-		dev_warn(&h->pdev->dev, HPSA "%d: a state change "
-			"detected, command retried\n", h->ctlr);
+		dev_warn(&h->pdev->dev,
+			"%s: a state change detected, command retried\n",
+			h->devname);
 		break;
 	case LUN_FAILED:
 		dev_warn(&h->pdev->dev,
-			HPSA "%d: LUN failure detected\n", h->ctlr);
+			"%s: LUN failure detected\n", h->devname);
 		break;
 	case REPORT_LUNS_CHANGED:
 		dev_warn(&h->pdev->dev,
-			HPSA "%d: report LUN data changed\n", h->ctlr);
+			"%s: report LUN data changed\n", h->devname);
 	/*
 	 * Note: this REPORT_LUNS_CHANGED condition only occurs on the external
 	 * target (array) devices.
 	 */
 		break;
 	case POWER_OR_RESET:
-		dev_warn(&h->pdev->dev, HPSA "%d: a power on "
-			"or device reset detected\n", h->ctlr);
+		dev_warn(&h->pdev->dev,
+			"%s: a power on or device reset detected\n",
+			h->devname);
 		break;
 	case UNIT_ATTENTION_CLEARED:
-		dev_warn(&h->pdev->dev, HPSA "%d: unit attention "
-		    "cleared by another initiator\n", h->ctlr);
+		dev_warn(&h->pdev->dev,
+			"%s: unit attention cleared by another initiator\n",
+			h->devname);
 		break;
 	default:
-		dev_warn(&h->pdev->dev, HPSA "%d: unknown "
-			"unit attention detected\n", h->ctlr);
+		dev_warn(&h->pdev->dev,
+			"%s: unknown unit attention detected\n",
+			h->devname);
 		break;
 	}
 	return 1;
@@ -314,6 +375,20 @@ static int check_for_busy(struct ctlr_info *h, struct CommandList *c)
 	return 1;
 }
 
+static u32 lockup_detected(struct ctlr_info *h);
+static ssize_t host_show_lockup_detected(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int ld;
+	struct ctlr_info *h;
+	struct Scsi_Host *shost = class_to_shost(dev);
+
+	h = shost_to_hba(shost);
+	ld = lockup_detected(h);
+
+	return sprintf(buf, "ld=%d\n", ld);
+}
+
 static ssize_t host_store_hp_ssd_smart_path_status(struct device *dev,
 					 struct device_attribute *attr,
 					 const char *buf, size_t count)
@@ -425,7 +500,7 @@ static ssize_t host_show_hp_ssd_smart_path_status(struct device *dev,
 /* List of controllers which cannot be hard reset on kexec with reset_devices */
 static u32 unresettable_controller[] = {
 	0x324a103C, /* Smart Array P712m */
-	0x324b103C, /* SmartArray P711m */
+	0x324b103C, /* Smart Array P711m */
 	0x3223103C, /* Smart Array P800 */
 	0x3234103C, /* Smart Array P400 */
 	0x3235103C, /* Smart Array P400i */
@@ -467,24 +542,32 @@ static u32 soft_unresettable_controller[] = {
 	0x409D0E11, /* Smart Array 6400 EM */
 };
 
-static int ctlr_is_hard_resettable(u32 board_id)
+static u32 needs_abort_tags_swizzled[] = {
+	0x323D103C, /* Smart Array P700m */
+	0x324a103C, /* Smart Array P712m */
+	0x324b103C, /* SmartArray P711m */
+};
+
+static int board_id_in_array(u32 a[], int nelems, u32 board_id)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(unresettable_controller); i++)
-		if (unresettable_controller[i] == board_id)
-			return 0;
-	return 1;
+	for (i = 0; i < nelems; i++)
+		if (a[i] == board_id)
+			return 1;
+	return 0;
 }
 
-static int ctlr_is_soft_resettable(u32 board_id)
+static int ctlr_is_hard_resettable(u32 board_id)
 {
-	int i;
+	return !board_id_in_array(unresettable_controller,
+			ARRAY_SIZE(unresettable_controller), board_id);
+}
 
-	for (i = 0; i < ARRAY_SIZE(soft_unresettable_controller); i++)
-		if (soft_unresettable_controller[i] == board_id)
-			return 0;
-	return 1;
+static int ctlr_is_soft_resettable(u32 board_id)
+{
+	return !board_id_in_array(soft_unresettable_controller,
+			ARRAY_SIZE(soft_unresettable_controller), board_id);
 }
 
 static int ctlr_is_resettable(u32 board_id)
@@ -493,6 +576,12 @@ static int ctlr_is_resettable(u32 board_id)
 		ctlr_is_soft_resettable(board_id);
 }
 
+static int ctlr_needs_abort_tags_swizzled(u32 board_id)
+{
+	return board_id_in_array(needs_abort_tags_swizzled,
+			ARRAY_SIZE(needs_abort_tags_swizzled), board_id);
+}
+
 static ssize_t host_show_resettable(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
@@ -647,12 +736,15 @@ static DEVICE_ATTR(transport_mode, S_IRUGO,
 	host_show_transport_mode, NULL);
 static DEVICE_ATTR(resettable, S_IRUGO,
 	host_show_resettable, NULL);
+static DEVICE_ATTR(lockup_detected, S_IRUGO,
+	host_show_lockup_detected, NULL);
 
 static struct device_attribute *hpsa_sdev_attrs[] = {
 	&dev_attr_raid_level,
 	&dev_attr_lunid,
 	&dev_attr_unique_id,
 	&dev_attr_hp_ssd_smart_path_enabled,
+	&dev_attr_lockup_detected,
 	NULL,
 };
 
@@ -667,6 +759,9 @@ static struct device_attribute *hpsa_shost_attrs[] = {
 	NULL,
 };
 
+#define HPSA_NRESERVED_CMDS	(HPSA_CMDS_RESERVED_FOR_ABORTS + \
+		HPSA_CMDS_RESERVED_FOR_DRIVER + HPSA_MAX_CONCURRENT_PASSTHRUS)
+
 static struct scsi_host_template hpsa_driver_template = {
 	.module			= THIS_MODULE,
 	.name			= HPSA,
@@ -681,6 +776,7 @@ static struct scsi_host_template hpsa_driver_template = {
 	.eh_device_reset_handler = hpsa_eh_device_reset_handler,
 	.ioctl			= hpsa_ioctl,
 	.slave_alloc		= hpsa_slave_alloc,
+	.slave_configure	= hpsa_slave_configure,
 	.slave_destroy		= hpsa_slave_destroy,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl		= hpsa_compat_ioctl,
@@ -743,30 +839,43 @@ static inline u32 next_command(struct ctlr_info *h, u8 q)
  * a separate special register for submitting commands.
  */
 
-/* set_performant_mode: Modify the tag for cciss performant
+/*
+ * set_performant_mode: Modify the tag for cciss performant
  * set bit 0 for pull model, bits 3-1 for block fetch
  * register number
  */
-static void set_performant_mode(struct ctlr_info *h, struct CommandList *c)
+#define DEFAULT_REPLY_QUEUE (-1)
+static void set_performant_mode(struct ctlr_info *h, struct CommandList *c,
+					int reply_queue)
 {
 	if (likely(h->transMethod & CFGTBL_Trans_Performant)) {
 		c->busaddr |= 1 | (h->blockFetchTable[c->Header.SGList] << 1);
-		if (likely(h->msix_vector > 0))
+		if (unlikely(!h->msix_vector))
+			return;
+		if (likely(reply_queue == DEFAULT_REPLY_QUEUE))
 			c->Header.ReplyQueue =
 				raw_smp_processor_id() % h->nreply_queues;
+		else
+			c->Header.ReplyQueue = reply_queue % h->nreply_queues;
 	}
 }
 
 static void set_ioaccel1_performant_mode(struct ctlr_info *h,
-						struct CommandList *c)
+						struct CommandList *c,
+						int reply_queue)
 {
 	struct io_accel1_cmd *cp = &h->ioaccel_cmd_pool[c->cmdindex];
 
-	/* Tell the controller to post the reply to the queue for this
+	/*
+	 * Tell the controller to post the reply to the queue for this
 	 * processor.  This seems to give the best I/O throughput.
 	 */
-	cp->ReplyQueue = smp_processor_id() % h->nreply_queues;
-	/* Set the bits in the address sent down to include:
+	if (likely(reply_queue == DEFAULT_REPLY_QUEUE))
+		cp->ReplyQueue = smp_processor_id() % h->nreply_queues;
+	else
+		cp->ReplyQueue = reply_queue % h->nreply_queues;
+	/*
+	 * Set the bits in the address sent down to include:
 	 *  - performant mode bit (bit 0)
 	 *  - pull count (bits 1-3)
 	 *  - command type (bits 4-6)
@@ -775,20 +884,48 @@ static void set_ioaccel1_performant_mode(struct ctlr_info *h,
 					IOACCEL1_BUSADDR_CMDTYPE;
 }
 
-static void set_ioaccel2_performant_mode(struct ctlr_info *h,
-						struct CommandList *c)
+static void set_ioaccel2_tmf_performant_mode(struct ctlr_info *h,
+						struct CommandList *c,
+						int reply_queue)
 {
-	struct io_accel2_cmd *cp = &h->ioaccel2_cmd_pool[c->cmdindex];
+	struct hpsa_tmf_struct *cp = (struct hpsa_tmf_struct *)
+		&h->ioaccel2_cmd_pool[c->cmdindex];
 
 	/* Tell the controller to post the reply to the queue for this
 	 * processor.  This seems to give the best I/O throughput.
 	 */
-	cp->reply_queue = smp_processor_id() % h->nreply_queues;
+	if (likely(reply_queue == DEFAULT_REPLY_QUEUE))
+		cp->reply_queue = smp_processor_id() % h->nreply_queues;
+	else
+		cp->reply_queue = reply_queue % h->nreply_queues;
 	/* Set the bits in the address sent down to include:
 	 *  - performant mode bit not used in ioaccel mode 2
 	 *  - pull count (bits 0-3)
 	 *  - command type isn't needed for ioaccel2
 	 */
+	c->busaddr |= h->ioaccel2_blockFetchTable[0];
+}
+
+static void set_ioaccel2_performant_mode(struct ctlr_info *h,
+						struct CommandList *c,
+						int reply_queue)
+{
+	struct io_accel2_cmd *cp = &h->ioaccel2_cmd_pool[c->cmdindex];
+
+	/*
+	 * Tell the controller to post the reply to the queue for this
+	 * processor.  This seems to give the best I/O throughput.
+	 */
+	if (likely(reply_queue == DEFAULT_REPLY_QUEUE))
+		cp->reply_queue = smp_processor_id() % h->nreply_queues;
+	else
+		cp->reply_queue = reply_queue % h->nreply_queues;
+	/*
+	 * Set the bits in the address sent down to include:
+	 *  - performant mode bit not used in ioaccel mode 2
+	 *  - pull count (bits 0-3)
+	 *  - command type isn't needed for ioaccel2
+	 */
 	c->busaddr |= (h->ioaccel2_blockFetchTable[cp->sg_count]);
 }
 
@@ -821,26 +958,38 @@ static void dial_up_lockup_detection_on_fw_flash_complete(struct ctlr_info *h,
 		h->heartbeat_sample_interval = HEARTBEAT_SAMPLE_INTERVAL;
 }
 
-static void enqueue_cmd_and_start_io(struct ctlr_info *h,
-	struct CommandList *c)
+static void __enqueue_cmd_and_start_io(struct ctlr_info *h,
+	struct CommandList *c, int reply_queue)
 {
 	dial_down_lockup_detection_during_fw_flash(h, c);
 	atomic_inc(&h->commands_outstanding);
 	switch (c->cmd_type) {
 	case CMD_IOACCEL1:
-		set_ioaccel1_performant_mode(h, c);
+		set_ioaccel1_performant_mode(h, c, reply_queue);
 		writel(c->busaddr, h->vaddr + SA5_REQUEST_PORT_OFFSET);
 		break;
 	case CMD_IOACCEL2:
-		set_ioaccel2_performant_mode(h, c);
+		set_ioaccel2_performant_mode(h, c, reply_queue);
+		writel(c->busaddr, h->vaddr + IOACCEL2_INBOUND_POSTQ_32);
+		break;
+	case IOACCEL2_TMF:
+		set_ioaccel2_tmf_performant_mode(h, c, reply_queue);
 		writel(c->busaddr, h->vaddr + IOACCEL2_INBOUND_POSTQ_32);
 		break;
 	default:
-		set_performant_mode(h, c);
+		set_performant_mode(h, c, reply_queue);
 		h->access.submit_command(h, c);
 	}
 }
 
+static void enqueue_cmd_and_start_io(struct ctlr_info *h, struct CommandList *c)
+{
+	if (unlikely(hpsa_is_pending_event(c)))
+		return finish_cmd(c);
+
+	__enqueue_cmd_and_start_io(h, c, DEFAULT_REPLY_QUEUE);
+}
+
 static inline int is_hba_lunid(unsigned char scsi3addr[])
 {
 	return memcmp(scsi3addr, RAID_CTLR_LUNID, 8) == 0;
@@ -881,6 +1030,23 @@ static int hpsa_find_target_lun(struct ctlr_info *h,
 	return !found;
 }
 
+static inline void hpsa_show_dev_msg(const char *level, struct ctlr_info *h,
+	struct hpsa_scsi_dev_t *dev, char *description)
+{
+	dev_printk(level, &h->pdev->dev,
+			"scsi %d:%d:%d:%d: %s %s %.8s %.16s RAID-%s SSDSmartPathCap%c En%c Exp=%d\n",
+			h->scsi_host->host_no, dev->bus, dev->target, dev->lun,
+			description,
+			scsi_device_type(dev->devtype),
+			dev->vendor,
+			dev->model,
+			dev->raid_level > RAID_UNKNOWN ?
+				"RAID-?" : raid_label[dev->raid_level],
+			dev->offload_config ? '+' : '-',
+			dev->offload_enabled ? '+' : '-',
+			dev->expose_state);
+}
+
 /* Add an entry into h->dev[] array. */
 static int hpsa_scsi_add_entry(struct ctlr_info *h, int hostno,
 		struct hpsa_scsi_dev_t *device,
@@ -948,15 +1114,10 @@ lun_assigned:
 	h->ndevices++;
 	added[*nadded] = device;
 	(*nadded)++;
-
-	/* initially, (before registering with scsi layer) we don't
-	 * know our hostno and we don't want to print anything first
-	 * time anyway (the scsi layer's inquiries will show that info)
-	 */
-	/* if (hostno != -1) */
-		dev_info(&h->pdev->dev, "%s device c%db%dt%dl%d added.\n",
-			scsi_device_type(device->devtype), hostno,
-			device->bus, device->target, device->lun);
+	hpsa_show_dev_msg(KERN_INFO, h, device,
+		device->expose_state & HPSA_SCSI_ADD ? "added" : "masked");
+	device->offload_to_be_enabled = device->offload_enabled;
+	device->offload_enabled = 0;
 	return 0;
 }
 
@@ -964,6 +1125,7 @@ lun_assigned:
 static void hpsa_scsi_update_entry(struct ctlr_info *h, int hostno,
 	int entry, struct hpsa_scsi_dev_t *new_entry)
 {
+	int offload_enabled;
 	/* assumes h->devlock is held */
 	BUG_ON(entry < 0 || entry >= HPSA_MAX_DEVICES);
 
@@ -982,16 +1144,29 @@ static void hpsa_scsi_update_entry(struct ctlr_info *h, int hostno,
 		 */
 		h->dev[entry]->raid_map = new_entry->raid_map;
 		h->dev[entry]->ioaccel_handle = new_entry->ioaccel_handle;
-		wmb(); /* ensure raid map updated prior to ->offload_enabled */
 	}
+	if (new_entry->hba_ioaccel_enabled) {
+		h->dev[entry]->ioaccel_handle = new_entry->ioaccel_handle;
+		wmb(); /* set ioaccel_handle *before* hba_ioaccel_enabled */
+	}
+	h->dev[entry]->hba_ioaccel_enabled = new_entry->hba_ioaccel_enabled;
 	h->dev[entry]->offload_config = new_entry->offload_config;
 	h->dev[entry]->offload_to_mirror = new_entry->offload_to_mirror;
-	h->dev[entry]->offload_enabled = new_entry->offload_enabled;
 	h->dev[entry]->queue_depth = new_entry->queue_depth;
 
-	dev_info(&h->pdev->dev, "%s device c%db%dt%dl%d updated.\n",
-		scsi_device_type(new_entry->devtype), hostno, new_entry->bus,
-		new_entry->target, new_entry->lun);
+	/*
+	 * We can turn off ioaccel offload now, but need to delay turning
+	 * it on until we can update h->dev[entry]->phys_disk[], but we
+	 * can't do that until all the devices are updated.
+	 */
+	h->dev[entry]->offload_to_be_enabled = new_entry->offload_enabled;
+	if (!new_entry->offload_enabled)
+		h->dev[entry]->offload_enabled = 0;
+
+	offload_enabled = h->dev[entry]->offload_enabled;
+	h->dev[entry]->offload_enabled = h->dev[entry]->offload_to_be_enabled;
+	hpsa_show_dev_msg(KERN_INFO, h, h->dev[entry], "updated");
+	h->dev[entry]->offload_enabled = offload_enabled;
 }
 
 /* Replace an entry from h->dev[] array. */
@@ -1017,9 +1192,9 @@ static void hpsa_scsi_replace_entry(struct ctlr_info *h, int hostno,
 	h->dev[entry] = new_entry;
 	added[*nadded] = new_entry;
 	(*nadded)++;
-	dev_info(&h->pdev->dev, "%s device c%db%dt%dl%d changed.\n",
-		scsi_device_type(new_entry->devtype), hostno, new_entry->bus,
-			new_entry->target, new_entry->lun);
+	hpsa_show_dev_msg(KERN_INFO, h, new_entry, "replaced");
+	new_entry->offload_to_be_enabled = new_entry->offload_enabled;
+	new_entry->offload_enabled = 0;
 }
 
 /* Remove an entry from h->dev[] array. */
@@ -1039,9 +1214,7 @@ static void hpsa_scsi_remove_entry(struct ctlr_info *h, int hostno, int entry,
 	for (i = entry; i < h->ndevices-1; i++)
 		h->dev[i] = h->dev[i+1];
 	h->ndevices--;
-	dev_info(&h->pdev->dev, "%s device c%db%dt%dl%d removed.\n",
-		scsi_device_type(sd->devtype), hostno, sd->bus, sd->target,
-		sd->lun);
+	hpsa_show_dev_msg(KERN_INFO, h, sd, "removed");
 }
 
 #define SCSI3ADDR_EQ(a, b) ( \
@@ -1283,6 +1456,8 @@ static void hpsa_figure_phys_disk_ptrs(struct ctlr_info *h,
 	if (nraid_map_entries > RAID_MAP_MAX_ENTRIES)
 		nraid_map_entries = RAID_MAP_MAX_ENTRIES;
 
+	logical_drive->nphysical_disks = nraid_map_entries;
+
 	qdepth = 0;
 	for (i = 0; i < nraid_map_entries; i++) {
 		logical_drive->phys_disk[i] = NULL;
@@ -1312,7 +1487,8 @@ static void hpsa_figure_phys_disk_ptrs(struct ctlr_info *h,
 		 */
 		if (!logical_drive->phys_disk[i]) {
 			logical_drive->offload_enabled = 0;
-			logical_drive->queue_depth = h->nr_cmds;
+			logical_drive->offload_to_be_enabled = 0;
+			logical_drive->queue_depth = 8;
 		}
 	}
 	if (nraid_map_entries)
@@ -1335,6 +1511,16 @@ static void hpsa_update_log_drive_phys_drive_ptrs(struct ctlr_info *h,
 			continue;
 		if (!is_logical_dev_addr_mode(dev[i]->scsi3addr))
 			continue;
+
+		/*
+		 * If offload is currently enabled, the RAID map and
+		 * phys_disk[] assignment *better* not be changing
+		 * and since it isn't changing, we do not need to
+		 * update it.
+		 */
+		if (dev[i]->offload_enabled)
+			continue;
+
 		hpsa_figure_phys_disk_ptrs(h, dev, ndevices, dev[i]);
 	}
 }
@@ -1411,9 +1597,7 @@ static void adjust_hpsa_scsi_table(struct ctlr_info *h, int hostno,
 		 */
 		if (sd[i]->volume_offline) {
 			hpsa_show_volume_status(h, sd[i]);
-			dev_info(&h->pdev->dev, "c%db%dt%dl%d: temporarily offline\n",
-				h->scsi_host->host_no,
-				sd[i]->bus, sd[i]->target, sd[i]->lun);
+			hpsa_show_dev_msg(KERN_INFO, h, sd[i], "offline");
 			continue;
 		}
 
@@ -1433,6 +1617,14 @@ static void adjust_hpsa_scsi_table(struct ctlr_info *h, int hostno,
 			/* but if it does happen, we just ignore that device */
 		}
 	}
+	hpsa_update_log_drive_phys_drive_ptrs(h, h->dev, h->ndevices);
+
+	/* Now that h->dev[]->phys_disk[] is coherent, we can enable
+	 * any logical drives that need it enabled.
+	 */
+	for (i = 0; i < h->ndevices; i++)
+		h->dev[i]->offload_enabled = h->dev[i]->offload_to_be_enabled;
+
 	spin_unlock_irqrestore(&h->devlock, flags);
 
 	/* Monitor devices which are in one of several NOT READY states to be
@@ -1456,20 +1648,22 @@ static void adjust_hpsa_scsi_table(struct ctlr_info *h, int hostno,
 	sh = h->scsi_host;
 	/* Notify scsi mid layer of any removed devices */
 	for (i = 0; i < nremoved; i++) {
-		struct scsi_device *sdev =
-			scsi_device_lookup(sh, removed[i]->bus,
-				removed[i]->target, removed[i]->lun);
-		if (sdev != NULL) {
-			scsi_remove_device(sdev);
-			scsi_device_put(sdev);
-		} else {
-			/* We don't expect to get here.
-			 * future cmds to this device will get selection
-			 * timeout as if the device was gone.
-			 */
-			dev_warn(&h->pdev->dev, "didn't find c%db%dt%dl%d "
-				" for removal.", hostno, removed[i]->bus,
-				removed[i]->target, removed[i]->lun);
+		if (removed[i]->expose_state & HPSA_SCSI_ADD) {
+			struct scsi_device *sdev =
+				scsi_device_lookup(sh, removed[i]->bus,
+					removed[i]->target, removed[i]->lun);
+			if (sdev != NULL) {
+				scsi_remove_device(sdev);
+				scsi_device_put(sdev);
+			} else {
+				/*
+				 * We don't expect to get here.
+				 * future cmds to this device will get selection
+				 * timeout as if the device was gone.
+				 */
+				hpsa_show_dev_msg(KERN_WARNING, h, removed[i],
+					"didn't find device for removal.");
+			}
 		}
 		kfree(removed[i]);
 		removed[i] = NULL;
@@ -1477,16 +1671,18 @@ static void adjust_hpsa_scsi_table(struct ctlr_info *h, int hostno,
 
 	/* Notify scsi mid layer of any added devices */
 	for (i = 0; i < nadded; i++) {
+		if (!(added[i]->expose_state & HPSA_SCSI_ADD))
+			continue;
 		if (scsi_add_device(sh, added[i]->bus,
 			added[i]->target, added[i]->lun) == 0)
 			continue;
-		dev_warn(&h->pdev->dev, "scsi_add_device c%db%dt%dl%d failed, "
-			"device not added.\n", hostno, added[i]->bus,
-			added[i]->target, added[i]->lun);
+		hpsa_show_dev_msg(KERN_WARNING, h, added[i],
+					"addition failed, device not added.");
 		/* now we have to remove it from h->dev,
 		 * since it didn't get added to scsi mid layer
 		 */
 		fixup_botched_add(h, added[i]);
+		added[i] = NULL;
 	}
 
 free_and_out:
@@ -1512,7 +1708,6 @@ static struct hpsa_scsi_dev_t *lookup_hpsa_scsi_dev(struct ctlr_info *h,
 	return NULL;
 }
 
-/* link sdev->hostdata to our per-device structure. */
 static int hpsa_slave_alloc(struct scsi_device *sdev)
 {
 	struct hpsa_scsi_dev_t *sd;
@@ -1523,21 +1718,80 @@ static int hpsa_slave_alloc(struct scsi_device *sdev)
 	spin_lock_irqsave(&h->devlock, flags);
 	sd = lookup_hpsa_scsi_dev(h, sdev_channel(sdev),
 		sdev_id(sdev), sdev->lun);
-	if (sd != NULL) {
-		sdev->hostdata = sd;
-		if (sd->queue_depth)
-			scsi_change_queue_depth(sdev, sd->queue_depth);
+	if (likely(sd)) {
 		atomic_set(&sd->ioaccel_cmds_out, 0);
-	}
+		sdev->hostdata = (sd->expose_state & HPSA_SCSI_ADD) ? sd : NULL;
+	} else
+		sdev->hostdata = NULL;
 	spin_unlock_irqrestore(&h->devlock, flags);
 	return 0;
 }
 
+/* configure scsi device based on internal per-device structure */
+static int hpsa_slave_configure(struct scsi_device *sdev)
+{
+	struct hpsa_scsi_dev_t *sd;
+	int queue_depth;
+
+	sd = sdev->hostdata;
+	sdev->no_uld_attach = !sd || !(sd->expose_state & HPSA_ULD_ATTACH);
+
+	if (sd)
+		queue_depth = sd->queue_depth != 0 ?
+			sd->queue_depth : sdev->host->can_queue;
+	else
+		queue_depth = sdev->host->can_queue;
+
+	scsi_change_queue_depth(sdev, queue_depth);
+
+	return 0;
+}
+
 static void hpsa_slave_destroy(struct scsi_device *sdev)
 {
 	/* nothing to do. */
 }
 
+static void hpsa_free_ioaccel2_sg_chain_blocks(struct ctlr_info *h)
+{
+	int i;
+
+	if (!h->ioaccel2_cmd_sg_list)
+		return;
+	for (i = 0; i < h->nr_cmds; i++) {
+		kfree(h->ioaccel2_cmd_sg_list[i]);
+		h->ioaccel2_cmd_sg_list[i] = NULL;
+	}
+	kfree(h->ioaccel2_cmd_sg_list);
+	h->ioaccel2_cmd_sg_list = NULL;
+}
+
+static int hpsa_allocate_ioaccel2_sg_chain_blocks(struct ctlr_info *h)
+{
+	int i;
+
+	if (h->chainsize <= 0)
+		return 0;
+
+	h->ioaccel2_cmd_sg_list =
+		kzalloc(sizeof(*h->ioaccel2_cmd_sg_list) * h->nr_cmds,
+					GFP_KERNEL);
+	if (!h->ioaccel2_cmd_sg_list)
+		return -ENOMEM;
+	for (i = 0; i < h->nr_cmds; i++) {
+		h->ioaccel2_cmd_sg_list[i] =
+			kmalloc(sizeof(*h->ioaccel2_cmd_sg_list[i]) *
+					h->maxsgentries, GFP_KERNEL);
+		if (!h->ioaccel2_cmd_sg_list[i])
+			goto clean;
+	}
+	return 0;
+
+clean:
+	hpsa_free_ioaccel2_sg_chain_blocks(h);
+	return -ENOMEM;
+}
+
 static void hpsa_free_sg_chain_blocks(struct ctlr_info *h)
 {
 	int i;
@@ -1552,7 +1806,7 @@ static void hpsa_free_sg_chain_blocks(struct ctlr_info *h)
 	h->cmd_sg_list = NULL;
 }
 
-static int hpsa_allocate_sg_chain_blocks(struct ctlr_info *h)
+static int hpsa_alloc_sg_chain_blocks(struct ctlr_info *h)
 {
 	int i;
 
@@ -1580,6 +1834,39 @@ clean:
 	return -ENOMEM;
 }
 
+static int hpsa_map_ioaccel2_sg_chain_block(struct ctlr_info *h,
+	struct io_accel2_cmd *cp, struct CommandList *c)
+{
+	struct ioaccel2_sg_element *chain_block;
+	u64 temp64;
+	u32 chain_size;
+
+	chain_block = h->ioaccel2_cmd_sg_list[c->cmdindex];
+	chain_size = le32_to_cpu(cp->data_len);
+	temp64 = pci_map_single(h->pdev, chain_block, chain_size,
+				PCI_DMA_TODEVICE);
+	if (dma_mapping_error(&h->pdev->dev, temp64)) {
+		/* prevent subsequent unmapping */
+		cp->sg->address = 0;
+		return -1;
+	}
+	cp->sg->address = cpu_to_le64(temp64);
+	return 0;
+}
+
+static void hpsa_unmap_ioaccel2_sg_chain_block(struct ctlr_info *h,
+	struct io_accel2_cmd *cp)
+{
+	struct ioaccel2_sg_element *chain_sg;
+	u64 temp64;
+	u32 chain_size;
+
+	chain_sg = cp->sg;
+	temp64 = le64_to_cpu(chain_sg->address);
+	chain_size = le32_to_cpu(cp->data_len);
+	pci_unmap_single(h->pdev, temp64, chain_size, PCI_DMA_TODEVICE);
+}
+
 static int hpsa_map_sg_chain_block(struct ctlr_info *h,
 	struct CommandList *c)
 {
@@ -1629,6 +1916,7 @@ static int handle_ioaccel_mode2_error(struct ctlr_info *h,
 {
 	int data_len;
 	int retry = 0;
+	u32 ioaccel2_resid = 0;
 
 	switch (c2->error_data.serv_response) {
 	case IOACCEL2_SERV_RESPONSE_COMPLETE:
@@ -1636,9 +1924,6 @@ static int handle_ioaccel_mode2_error(struct ctlr_info *h,
 		case IOACCEL2_STATUS_SR_TASK_COMP_GOOD:
 			break;
 		case IOACCEL2_STATUS_SR_TASK_COMP_CHK_COND:
-			dev_warn(&h->pdev->dev,
-				"%s: task complete with check condition.\n",
-				"HP SSD Smart Path");
 			cmd->result |= SAM_STAT_CHECK_CONDITION;
 			if (c2->error_data.data_present !=
 					IOACCEL2_SENSE_DATA_PRESENT) {
@@ -1658,58 +1943,56 @@ static int handle_ioaccel_mode2_error(struct ctlr_info *h,
 			retry = 1;
 			break;
 		case IOACCEL2_STATUS_SR_TASK_COMP_BUSY:
-			dev_warn(&h->pdev->dev,
-				"%s: task complete with BUSY status.\n",
-				"HP SSD Smart Path");
 			retry = 1;
 			break;
 		case IOACCEL2_STATUS_SR_TASK_COMP_RES_CON:
-			dev_warn(&h->pdev->dev,
-				"%s: task complete with reservation conflict.\n",
-				"HP SSD Smart Path");
 			retry = 1;
 			break;
 		case IOACCEL2_STATUS_SR_TASK_COMP_SET_FULL:
-			/* Make scsi midlayer do unlimited retries */
-			cmd->result = DID_IMM_RETRY << 16;
+			retry = 1;
 			break;
 		case IOACCEL2_STATUS_SR_TASK_COMP_ABORTED:
-			dev_warn(&h->pdev->dev,
-				"%s: task complete with aborted status.\n",
-				"HP SSD Smart Path");
 			retry = 1;
 			break;
 		default:
-			dev_warn(&h->pdev->dev,
-				"%s: task complete with unrecognized status: 0x%02x\n",
-				"HP SSD Smart Path", c2->error_data.status);
 			retry = 1;
 			break;
 		}
 		break;
 	case IOACCEL2_SERV_RESPONSE_FAILURE:
-		/* don't expect to get here. */
-		dev_warn(&h->pdev->dev,
-			"unexpected delivery or target failure, status = 0x%02x\n",
-			c2->error_data.status);
-		retry = 1;
+		switch (c2->error_data.status) {
+		case IOACCEL2_STATUS_SR_IO_ERROR:
+		case IOACCEL2_STATUS_SR_IO_ABORTED:
+		case IOACCEL2_STATUS_SR_OVERRUN:
+			retry = 1;
+			break;
+		case IOACCEL2_STATUS_SR_UNDERRUN:
+			cmd->result = (DID_OK << 16);		/* host byte */
+			cmd->result |= (COMMAND_COMPLETE << 8);	/* msg byte */
+			ioaccel2_resid = get_unaligned_le32(
+						&c2->error_data.resid_cnt[0]);
+			scsi_set_resid(cmd, ioaccel2_resid);
+			break;
+		case IOACCEL2_STATUS_SR_NO_PATH_TO_DEVICE:
+		case IOACCEL2_STATUS_SR_INVALID_DEVICE:
+		case IOACCEL2_STATUS_SR_IOACCEL_DISABLED:
+			/* We will get an event from ctlr to trigger rescan */
+			retry = 1;
+			break;
+		default:
+			retry = 1;
+		}
 		break;
 	case IOACCEL2_SERV_RESPONSE_TMF_COMPLETE:
 		break;
 	case IOACCEL2_SERV_RESPONSE_TMF_SUCCESS:
 		break;
 	case IOACCEL2_SERV_RESPONSE_TMF_REJECTED:
-		dev_warn(&h->pdev->dev, "task management function rejected.\n");
 		retry = 1;
 		break;
 	case IOACCEL2_SERV_RESPONSE_TMF_WRONG_LUN:
-		dev_warn(&h->pdev->dev, "task management function invalid LUN\n");
 		break;
 	default:
-		dev_warn(&h->pdev->dev,
-			"%s: Unrecognized server response: 0x%02x\n",
-			"HP SSD Smart Path",
-			c2->error_data.serv_response);
 		retry = 1;
 		break;
 	}
@@ -1717,6 +2000,87 @@ static int handle_ioaccel_mode2_error(struct ctlr_info *h,
 	return retry;	/* retry on raid path? */
 }
 
+static void hpsa_cmd_resolve_events(struct ctlr_info *h,
+		struct CommandList *c)
+{
+	bool do_wake = false;
+
+	/*
+	 * Prevent the following race in the abort handler:
+	 *
+	 * 1. LLD is requested to abort a SCSI command
+	 * 2. The SCSI command completes
+	 * 3. The struct CommandList associated with step 2 is made available
+	 * 4. New I/O request to LLD to another LUN re-uses struct CommandList
+	 * 5. Abort handler follows scsi_cmnd->host_scribble and
+	 *    finds struct CommandList and tries to aborts it
+	 * Now we have aborted the wrong command.
+	 *
+	 * Reset c->scsi_cmd here so that the abort or reset handler will know
+	 * this command has completed.  Then, check to see if the handler is
+	 * waiting for this command, and, if so, wake it.
+	 */
+	c->scsi_cmd = SCSI_CMD_IDLE;
+	mb();	/* Declare command idle before checking for pending events. */
+	if (c->abort_pending) {
+		do_wake = true;
+		c->abort_pending = false;
+	}
+	if (c->reset_pending) {
+		unsigned long flags;
+		struct hpsa_scsi_dev_t *dev;
+
+		/*
+		 * There appears to be a reset pending; lock the lock and
+		 * reconfirm.  If so, then decrement the count of outstanding
+		 * commands and wake the reset command if this is the last one.
+		 */
+		spin_lock_irqsave(&h->lock, flags);
+		dev = c->reset_pending;		/* Re-fetch under the lock. */
+		if (dev && atomic_dec_and_test(&dev->reset_cmds_out))
+			do_wake = true;
+		c->reset_pending = NULL;
+		spin_unlock_irqrestore(&h->lock, flags);
+	}
+
+	if (do_wake)
+		wake_up_all(&h->event_sync_wait_queue);
+}
+
+static void hpsa_cmd_resolve_and_free(struct ctlr_info *h,
+				      struct CommandList *c)
+{
+	hpsa_cmd_resolve_events(h, c);
+	cmd_tagged_free(h, c);
+}
+
+static void hpsa_cmd_free_and_done(struct ctlr_info *h,
+		struct CommandList *c, struct scsi_cmnd *cmd)
+{
+	hpsa_cmd_resolve_and_free(h, c);
+	cmd->scsi_done(cmd);
+}
+
+static void hpsa_retry_cmd(struct ctlr_info *h, struct CommandList *c)
+{
+	INIT_WORK(&c->work, hpsa_command_resubmit_worker);
+	queue_work_on(raw_smp_processor_id(), h->resubmit_wq, &c->work);
+}
+
+static void hpsa_set_scsi_cmd_aborted(struct scsi_cmnd *cmd)
+{
+	cmd->result = DID_ABORT << 16;
+}
+
+static void hpsa_cmd_abort_and_free(struct ctlr_info *h, struct CommandList *c,
+				    struct scsi_cmnd *cmd)
+{
+	hpsa_set_scsi_cmd_aborted(cmd);
+	dev_warn(&h->pdev->dev, "CDB %16phN was aborted with status 0x%x\n",
+			 c->Request.CDB, c->err_info->ScsiStatus);
+	hpsa_cmd_resolve_and_free(h, c);
+}
+
 static void process_ioaccel2_completion(struct ctlr_info *h,
 		struct CommandList *c, struct scsi_cmnd *cmd,
 		struct hpsa_scsi_dev_t *dev)
@@ -1725,13 +2089,11 @@ static void process_ioaccel2_completion(struct ctlr_info *h,
 
 	/* check for good status */
 	if (likely(c2->error_data.serv_response == 0 &&
-			c2->error_data.status == 0)) {
-		cmd_free(h, c);
-		cmd->scsi_done(cmd);
-		return;
-	}
+			c2->error_data.status == 0))
+		return hpsa_cmd_free_and_done(h, c, cmd);
 
-	/* Any RAID offload error results in retry which will use
+	/*
+	 * Any RAID offload error results in retry which will use
 	 * the normal I/O path so the controller can handle whatever's
 	 * wrong.
 	 */
@@ -1741,19 +2103,42 @@ static void process_ioaccel2_completion(struct ctlr_info *h,
 		if (c2->error_data.status ==
 			IOACCEL2_STATUS_SR_IOACCEL_DISABLED)
 			dev->offload_enabled = 0;
-		goto retry_cmd;
+
+		return hpsa_retry_cmd(h, c);
 	}
 
 	if (handle_ioaccel_mode2_error(h, c, cmd, c2))
-		goto retry_cmd;
+		return hpsa_retry_cmd(h, c);
 
-	cmd_free(h, c);
-	cmd->scsi_done(cmd);
-	return;
+	return hpsa_cmd_free_and_done(h, c, cmd);
+}
 
-retry_cmd:
-	INIT_WORK(&c->work, hpsa_command_resubmit_worker);
-	queue_work_on(raw_smp_processor_id(), h->resubmit_wq, &c->work);
+/* Returns 0 on success, < 0 otherwise. */
+static int hpsa_evaluate_tmf_status(struct ctlr_info *h,
+					struct CommandList *cp)
+{
+	u8 tmf_status = cp->err_info->ScsiStatus;
+
+	switch (tmf_status) {
+	case CISS_TMF_COMPLETE:
+		/*
+		 * CISS_TMF_COMPLETE never happens, instead,
+		 * ei->CommandStatus == 0 for this case.
+		 */
+	case CISS_TMF_SUCCESS:
+		return 0;
+	case CISS_TMF_INVALID_FRAME:
+	case CISS_TMF_NOT_SUPPORTED:
+	case CISS_TMF_FAILED:
+	case CISS_TMF_WRONG_LUN:
+	case CISS_TMF_OVERLAPPED_TAG:
+		break;
+	default:
+		dev_warn(&h->pdev->dev, "Unknown TMF status: 0x%02x\n",
+				tmf_status);
+		break;
+	}
+	return -tmf_status;
 }
 
 static void complete_scsi_command(struct CommandList *cp)
@@ -1762,51 +2147,58 @@ static void complete_scsi_command(struct CommandList *cp)
 	struct ctlr_info *h;
 	struct ErrorInfo *ei;
 	struct hpsa_scsi_dev_t *dev;
+	struct io_accel2_cmd *c2;
 
-	unsigned char sense_key;
-	unsigned char asc;      /* additional sense code */
-	unsigned char ascq;     /* additional sense code qualifier */
+	u8 sense_key;
+	u8 asc;      /* additional sense code */
+	u8 ascq;     /* additional sense code qualifier */
 	unsigned long sense_data_size;
 
 	ei = cp->err_info;
 	cmd = cp->scsi_cmd;
 	h = cp->h;
 	dev = cmd->device->hostdata;
+	c2 = &h->ioaccel2_cmd_pool[cp->cmdindex];
 
 	scsi_dma_unmap(cmd); /* undo the DMA mappings */
 	if ((cp->cmd_type == CMD_SCSI) &&
 		(le16_to_cpu(cp->Header.SGTotal) > h->max_cmd_sg_entries))
 		hpsa_unmap_sg_chain_block(h, cp);
 
+	if ((cp->cmd_type == CMD_IOACCEL2) &&
+		(c2->sg[0].chain_indicator == IOACCEL2_CHAIN))
+		hpsa_unmap_ioaccel2_sg_chain_block(h, c2);
+
 	cmd->result = (DID_OK << 16); 		/* host byte */
 	cmd->result |= (COMMAND_COMPLETE << 8);	/* msg byte */
 
 	if (cp->cmd_type == CMD_IOACCEL2 || cp->cmd_type == CMD_IOACCEL1)
 		atomic_dec(&cp->phys_disk->ioaccel_cmds_out);
 
-	if (cp->cmd_type == CMD_IOACCEL2)
-		return process_ioaccel2_completion(h, cp, cmd, dev);
-
-	cmd->result |= ei->ScsiStatus;
+	/*
+	 * We check for lockup status here as it may be set for
+	 * CMD_SCSI, CMD_IOACCEL1 and CMD_IOACCEL2 commands by
+	 * fail_all_oustanding_cmds()
+	 */
+	if (unlikely(ei->CommandStatus == CMD_CTLR_LOCKUP)) {
+		/* DID_NO_CONNECT will prevent a retry */
+		cmd->result = DID_NO_CONNECT << 16;
+		return hpsa_cmd_free_and_done(h, cp, cmd);
+	}
 
-	scsi_set_resid(cmd, ei->ResidualCnt);
-	if (ei->CommandStatus == 0) {
-		if (cp->cmd_type == CMD_IOACCEL1)
-			atomic_dec(&cp->phys_disk->ioaccel_cmds_out);
-		cmd_free(h, cp);
-		cmd->scsi_done(cmd);
-		return;
+	if ((unlikely(hpsa_is_pending_event(cp)))) {
+		if (cp->reset_pending)
+			return hpsa_cmd_resolve_and_free(h, cp);
+		if (cp->abort_pending)
+			return hpsa_cmd_abort_and_free(h, cp, cmd);
 	}
 
-	/* copy the sense data */
-	if (SCSI_SENSE_BUFFERSIZE < sizeof(ei->SenseInfo))
-		sense_data_size = SCSI_SENSE_BUFFERSIZE;
-	else
-		sense_data_size = sizeof(ei->SenseInfo);
-	if (ei->SenseLen < sense_data_size)
-		sense_data_size = ei->SenseLen;
+	if (cp->cmd_type == CMD_IOACCEL2)
+		return process_ioaccel2_completion(h, cp, cmd, dev);
 
-	memcpy(cmd->sense_buffer, ei->SenseInfo, sense_data_size);
+	scsi_set_resid(cmd, ei->ResidualCnt);
+	if (ei->CommandStatus == 0)
+		return hpsa_cmd_free_and_done(h, cp, cmd);
 
 	/* For I/O accelerator commands, copy over some fields to the normal
 	 * CISS header used below for error handling.
@@ -1828,10 +2220,7 @@ static void complete_scsi_command(struct CommandList *cp)
 		if (is_logical_dev_addr_mode(dev->scsi3addr)) {
 			if (ei->CommandStatus == CMD_IOACCEL_DISABLED)
 				dev->offload_enabled = 0;
-			INIT_WORK(&cp->work, hpsa_command_resubmit_worker);
-			queue_work_on(raw_smp_processor_id(),
-					h->resubmit_wq, &cp->work);
-			return;
+			return hpsa_retry_cmd(h, cp);
 		}
 	}
 
@@ -1839,14 +2228,18 @@ static void complete_scsi_command(struct CommandList *cp)
 	switch (ei->CommandStatus) {
 
 	case CMD_TARGET_STATUS:
-		if (ei->ScsiStatus) {
-			/* Get sense key */
-			sense_key = 0xf & ei->SenseInfo[2];
-			/* Get additional sense code */
-			asc = ei->SenseInfo[12];
-			/* Get addition sense code qualifier */
-			ascq = ei->SenseInfo[13];
-		}
+		cmd->result |= ei->ScsiStatus;
+		/* copy the sense data */
+		if (SCSI_SENSE_BUFFERSIZE < sizeof(ei->SenseInfo))
+			sense_data_size = SCSI_SENSE_BUFFERSIZE;
+		else
+			sense_data_size = sizeof(ei->SenseInfo);
+		if (ei->SenseLen < sense_data_size)
+			sense_data_size = ei->SenseLen;
+		memcpy(cmd->sense_buffer, ei->SenseInfo, sense_data_size);
+		if (ei->ScsiStatus)
+			decode_sense_data(ei->SenseInfo, sense_data_size,
+				&sense_key, &asc, &ascq);
 		if (ei->ScsiStatus == SAM_STAT_CHECK_CONDITION) {
 			if (sense_key == ABORTED_COMMAND) {
 				cmd->result |= DID_SOFT_ERROR << 16;
@@ -1918,10 +2311,8 @@ static void complete_scsi_command(struct CommandList *cp)
 			cp->Request.CDB);
 		break;
 	case CMD_ABORTED:
-		cmd->result = DID_ABORT << 16;
-		dev_warn(&h->pdev->dev, "CDB %16phN was aborted with status 0x%x\n",
-				cp->Request.CDB, ei->ScsiStatus);
-		break;
+		/* Return now to avoid calling scsi_done(). */
+		return hpsa_cmd_abort_and_free(h, cp, cmd);
 	case CMD_ABORT_FAILED:
 		cmd->result = DID_ERROR << 16;
 		dev_warn(&h->pdev->dev, "CDB %16phN : abort failed\n",
@@ -1941,6 +2332,10 @@ static void complete_scsi_command(struct CommandList *cp)
 		cmd->result = DID_ERROR << 16;
 		dev_warn(&h->pdev->dev, "Command unabortable\n");
 		break;
+	case CMD_TMF_STATUS:
+		if (hpsa_evaluate_tmf_status(h, cp)) /* TMF failed? */
+			cmd->result = DID_ERROR << 16;
+		break;
 	case CMD_IOACCEL_DISABLED:
 		/* This only handles the direct pass-through case since RAID
 		 * offload is handled above.  Just attempt a retry.
@@ -1954,8 +2349,8 @@ static void complete_scsi_command(struct CommandList *cp)
 		dev_warn(&h->pdev->dev, "cp %p returned unknown status %x\n",
 				cp, ei->CommandStatus);
 	}
-	cmd_free(h, cp);
-	cmd->scsi_done(cmd);
+
+	return hpsa_cmd_free_and_done(h, cp, cmd);
 }
 
 static void hpsa_pci_unmap(struct pci_dev *pdev,
@@ -1998,14 +2393,36 @@ static int hpsa_map_one(struct pci_dev *pdev,
 	return 0;
 }
 
-static inline void hpsa_scsi_do_simple_cmd_core(struct ctlr_info *h,
-	struct CommandList *c)
+#define NO_TIMEOUT ((unsigned long) -1)
+#define DEFAULT_TIMEOUT 30000 /* milliseconds */
+static int hpsa_scsi_do_simple_cmd_core(struct ctlr_info *h,
+	struct CommandList *c, int reply_queue, unsigned long timeout_msecs)
 {
 	DECLARE_COMPLETION_ONSTACK(wait);
 
 	c->waiting = &wait;
-	enqueue_cmd_and_start_io(h, c);
-	wait_for_completion(&wait);
+	__enqueue_cmd_and_start_io(h, c, reply_queue);
+	if (timeout_msecs == NO_TIMEOUT) {
+		/* TODO: get rid of this no-timeout thing */
+		wait_for_completion_io(&wait);
+		return IO_OK;
+	}
+	if (!wait_for_completion_io_timeout(&wait,
+					msecs_to_jiffies(timeout_msecs))) {
+		dev_warn(&h->pdev->dev, "Command timed out.\n");
+		return -ETIMEDOUT;
+	}
+	return IO_OK;
+}
+
+static int hpsa_scsi_do_simple_cmd(struct ctlr_info *h, struct CommandList *c,
+				   int reply_queue, unsigned long timeout_msecs)
+{
+	if (unlikely(lockup_detected(h))) {
+		c->err_info->CommandStatus = CMD_CTLR_LOCKUP;
+		return IO_OK;
+	}
+	return hpsa_scsi_do_simple_cmd_core(h, c, reply_queue, timeout_msecs);
 }
 
 static u32 lockup_detected(struct ctlr_info *h)
@@ -2020,25 +2437,19 @@ static u32 lockup_detected(struct ctlr_info *h)
 	return rc;
 }
 
-static void hpsa_scsi_do_simple_cmd_core_if_no_lockup(struct ctlr_info *h,
-	struct CommandList *c)
-{
-	/* If controller lockup detected, fake a hardware error. */
-	if (unlikely(lockup_detected(h)))
-		c->err_info->CommandStatus = CMD_HARDWARE_ERR;
-	else
-		hpsa_scsi_do_simple_cmd_core(h, c);
-}
-
 #define MAX_DRIVER_CMD_RETRIES 25
-static void hpsa_scsi_do_simple_cmd_with_retry(struct ctlr_info *h,
-	struct CommandList *c, int data_direction)
+static int hpsa_scsi_do_simple_cmd_with_retry(struct ctlr_info *h,
+	struct CommandList *c, int data_direction, unsigned long timeout_msecs)
 {
 	int backoff_time = 10, retry_count = 0;
+	int rc;
 
 	do {
 		memset(c->err_info, 0, sizeof(*c->err_info));
-		hpsa_scsi_do_simple_cmd_core(h, c);
+		rc = hpsa_scsi_do_simple_cmd(h, c, DEFAULT_REPLY_QUEUE,
+						  timeout_msecs);
+		if (rc)
+			break;
 		retry_count++;
 		if (retry_count > 3) {
 			msleep(backoff_time);
@@ -2049,6 +2460,9 @@ static void hpsa_scsi_do_simple_cmd_with_retry(struct ctlr_info *h,
 			check_for_busy(h, c)) &&
 			retry_count <= MAX_DRIVER_CMD_RETRIES);
 	hpsa_pci_unmap(h->pdev, c, 1, data_direction);
+	if (retry_count > MAX_DRIVER_CMD_RETRIES)
+		rc = -EIO;
+	return rc;
 }
 
 static void hpsa_print_cmd(struct ctlr_info *h, char *txt,
@@ -2072,16 +2486,23 @@ static void hpsa_scsi_interpret_error(struct ctlr_info *h,
 {
 	const struct ErrorInfo *ei = cp->err_info;
 	struct device *d = &cp->h->pdev->dev;
-	const u8 *sd = ei->SenseInfo;
+	u8 sense_key, asc, ascq;
+	int sense_len;
 
 	switch (ei->CommandStatus) {
 	case CMD_TARGET_STATUS:
+		if (ei->SenseLen > sizeof(ei->SenseInfo))
+			sense_len = sizeof(ei->SenseInfo);
+		else
+			sense_len = ei->SenseLen;
+		decode_sense_data(ei->SenseInfo, sense_len,
+					&sense_key, &asc, &ascq);
 		hpsa_print_cmd(h, "SCSI status", cp);
 		if (ei->ScsiStatus == SAM_STAT_CHECK_CONDITION)
-			dev_warn(d, "SCSI Status = 02, Sense key = %02x, ASC = %02x, ASCQ = %02x\n",
-				sd[2] & 0x0f, sd[12], sd[13]);
+			dev_warn(d, "SCSI Status = 02, Sense key = 0x%02x, ASC = 0x%02x, ASCQ = 0x%02x\n",
+				sense_key, asc, ascq);
 		else
-			dev_warn(d, "SCSI Status = %02x\n", ei->ScsiStatus);
+			dev_warn(d, "SCSI Status = 0x%02x\n", ei->ScsiStatus);
 		if (ei->ScsiStatus == 0)
 			dev_warn(d, "SCSI status is abnormally zero.  "
 			"(probably indicates selection timeout "
@@ -2125,6 +2546,9 @@ static void hpsa_scsi_interpret_error(struct ctlr_info *h,
 	case CMD_UNABORTABLE:
 		hpsa_print_cmd(h, "unabortable", cp);
 		break;
+	case CMD_CTLR_LOCKUP:
+		hpsa_print_cmd(h, "controller lockup detected", cp);
+		break;
 	default:
 		hpsa_print_cmd(h, "unknown status", cp);
 		dev_warn(d, "Unknown command status %x\n",
@@ -2142,17 +2566,15 @@ static int hpsa_scsi_do_inquiry(struct ctlr_info *h, unsigned char *scsi3addr,
 
 	c = cmd_alloc(h);
 
-	if (c == NULL) {
-		dev_warn(&h->pdev->dev, "cmd_alloc returned NULL!\n");
-		return -ENOMEM;
-	}
-
 	if (fill_cmd(c, HPSA_INQUIRY, h, buf, bufsize,
 			page, scsi3addr, TYPE_CMD)) {
 		rc = -1;
 		goto out;
 	}
-	hpsa_scsi_do_simple_cmd_with_retry(h, c, PCI_DMA_FROMDEVICE);
+	rc = hpsa_scsi_do_simple_cmd_with_retry(h, c,
+					PCI_DMA_FROMDEVICE, NO_TIMEOUT);
+	if (rc)
+		goto out;
 	ei = c->err_info;
 	if (ei->CommandStatus != 0 && ei->CommandStatus != CMD_DATA_UNDERRUN) {
 		hpsa_scsi_interpret_error(h, c);
@@ -2172,17 +2594,15 @@ static int hpsa_bmic_ctrl_mode_sense(struct ctlr_info *h,
 	struct ErrorInfo *ei;
 
 	c = cmd_alloc(h);
-	if (c == NULL) {			/* trouble... */
-		dev_warn(&h->pdev->dev, "cmd_alloc returned NULL!\n");
-		return -ENOMEM;
-	}
-
 	if (fill_cmd(c, BMIC_SENSE_CONTROLLER_PARAMETERS, h, buf, bufsize,
 			page, scsi3addr, TYPE_CMD)) {
 		rc = -1;
 		goto out;
 	}
-	hpsa_scsi_do_simple_cmd_with_retry(h, c, PCI_DMA_FROMDEVICE);
+	rc = hpsa_scsi_do_simple_cmd_with_retry(h, c,
+			PCI_DMA_FROMDEVICE, NO_TIMEOUT);
+	if (rc)
+		goto out;
 	ei = c->err_info;
 	if (ei->CommandStatus != 0 && ei->CommandStatus != CMD_DATA_UNDERRUN) {
 		hpsa_scsi_interpret_error(h, c);
@@ -2191,10 +2611,10 @@ static int hpsa_bmic_ctrl_mode_sense(struct ctlr_info *h,
 out:
 	cmd_free(h, c);
 	return rc;
-	}
+}
 
 static int hpsa_send_reset(struct ctlr_info *h, unsigned char *scsi3addr,
-	u8 reset_type)
+	u8 reset_type, int reply_queue)
 {
 	int rc = IO_OK;
 	struct CommandList *c;
@@ -2202,16 +2622,16 @@ static int hpsa_send_reset(struct ctlr_info *h, unsigned char *scsi3addr,
 
 	c = cmd_alloc(h);
 
-	if (c == NULL) {			/* trouble... */
-		dev_warn(&h->pdev->dev, "cmd_alloc returned NULL!\n");
-		return -ENOMEM;
-	}
 
 	/* fill_cmd can't fail here, no data buffer to map. */
 	(void) fill_cmd(c, HPSA_DEVICE_RESET_MSG, h, NULL, 0, 0,
 			scsi3addr, TYPE_MSG);
 	c->Request.CDB[1] = reset_type; /* fill_cmd defaults to LUN reset */
-	hpsa_scsi_do_simple_cmd_core(h, c);
+	rc = hpsa_scsi_do_simple_cmd(h, c, reply_queue, NO_TIMEOUT);
+	if (rc) {
+		dev_warn(&h->pdev->dev, "Failed to send reset command\n");
+		goto out;
+	}
 	/* no unmap needed here because no data xfer. */
 
 	ei = c->err_info;
@@ -2219,10 +2639,129 @@ static int hpsa_send_reset(struct ctlr_info *h, unsigned char *scsi3addr,
 		hpsa_scsi_interpret_error(h, c);
 		rc = -1;
 	}
+out:
 	cmd_free(h, c);
 	return rc;
 }
 
+static bool hpsa_cmd_dev_match(struct ctlr_info *h, struct CommandList *c,
+			       struct hpsa_scsi_dev_t *dev,
+			       unsigned char *scsi3addr)
+{
+	int i;
+	bool match = false;
+	struct io_accel2_cmd *c2 = &h->ioaccel2_cmd_pool[c->cmdindex];
+	struct hpsa_tmf_struct *ac = (struct hpsa_tmf_struct *) c2;
+
+	if (hpsa_is_cmd_idle(c))
+		return false;
+
+	switch (c->cmd_type) {
+	case CMD_SCSI:
+	case CMD_IOCTL_PEND:
+		match = !memcmp(scsi3addr, &c->Header.LUN.LunAddrBytes,
+				sizeof(c->Header.LUN.LunAddrBytes));
+		break;
+
+	case CMD_IOACCEL1:
+	case CMD_IOACCEL2:
+		if (c->phys_disk == dev) {
+			/* HBA mode match */
+			match = true;
+		} else {
+			/* Possible RAID mode -- check each phys dev. */
+			/* FIXME:  Do we need to take out a lock here?  If
+			 * so, we could just call hpsa_get_pdisk_of_ioaccel2()
+			 * instead. */
+			for (i = 0; i < dev->nphysical_disks && !match; i++) {
+				/* FIXME: an alternate test might be
+				 *
+				 * match = dev->phys_disk[i]->ioaccel_handle
+				 *              == c2->scsi_nexus;      */
+				match = dev->phys_disk[i] == c->phys_disk;
+			}
+		}
+		break;
+
+	case IOACCEL2_TMF:
+		for (i = 0; i < dev->nphysical_disks && !match; i++) {
+			match = dev->phys_disk[i]->ioaccel_handle ==
+					le32_to_cpu(ac->it_nexus);
+		}
+		break;
+
+	case 0:		/* The command is in the middle of being initialized. */
+		match = false;
+		break;
+
+	default:
+		dev_err(&h->pdev->dev, "unexpected cmd_type: %d\n",
+			c->cmd_type);
+		BUG();
+	}
+
+	return match;
+}
+
+static int hpsa_do_reset(struct ctlr_info *h, struct hpsa_scsi_dev_t *dev,
+	unsigned char *scsi3addr, u8 reset_type, int reply_queue)
+{
+	int i;
+	int rc = 0;
+
+	/* We can really only handle one reset at a time */
+	if (mutex_lock_interruptible(&h->reset_mutex) == -EINTR) {
+		dev_warn(&h->pdev->dev, "concurrent reset wait interrupted.\n");
+		return -EINTR;
+	}
+
+	BUG_ON(atomic_read(&dev->reset_cmds_out) != 0);
+
+	for (i = 0; i < h->nr_cmds; i++) {
+		struct CommandList *c = h->cmd_pool + i;
+		int refcount = atomic_inc_return(&c->refcount);
+
+		if (refcount > 1 && hpsa_cmd_dev_match(h, c, dev, scsi3addr)) {
+			unsigned long flags;
+
+			/*
+			 * Mark the target command as having a reset pending,
+			 * then lock a lock so that the command cannot complete
+			 * while we're considering it.  If the command is not
+			 * idle then count it; otherwise revoke the event.
+			 */
+			c->reset_pending = dev;
+			spin_lock_irqsave(&h->lock, flags);	/* Implied MB */
+			if (!hpsa_is_cmd_idle(c))
+				atomic_inc(&dev->reset_cmds_out);
+			else
+				c->reset_pending = NULL;
+			spin_unlock_irqrestore(&h->lock, flags);
+		}
+
+		cmd_free(h, c);
+	}
+
+	rc = hpsa_send_reset(h, scsi3addr, reset_type, reply_queue);
+	if (!rc)
+		wait_event(h->event_sync_wait_queue,
+			atomic_read(&dev->reset_cmds_out) == 0 ||
+			lockup_detected(h));
+
+	if (unlikely(lockup_detected(h))) {
+			dev_warn(&h->pdev->dev,
+				 "Controller lockup detected during reset wait\n");
+			mutex_unlock(&h->reset_mutex);
+			rc = -ENODEV;
+		}
+
+	if (unlikely(rc))
+		atomic_set(&dev->reset_cmds_out, 0);
+
+	mutex_unlock(&h->reset_mutex);
+	return rc;
+}
+
 static void hpsa_get_raid_level(struct ctlr_info *h,
 	unsigned char *scsi3addr, unsigned char *raid_level)
 {
@@ -2328,23 +2867,23 @@ static int hpsa_get_raid_map(struct ctlr_info *h,
 	struct ErrorInfo *ei;
 
 	c = cmd_alloc(h);
-	if (c == NULL) {
-		dev_warn(&h->pdev->dev, "cmd_alloc returned NULL!\n");
-		return -ENOMEM;
-	}
+
 	if (fill_cmd(c, HPSA_GET_RAID_MAP, h, &this_device->raid_map,
 			sizeof(this_device->raid_map), 0,
 			scsi3addr, TYPE_CMD)) {
-		dev_warn(&h->pdev->dev, "Out of memory in hpsa_get_raid_map()\n");
+		dev_warn(&h->pdev->dev, "hpsa_get_raid_map fill_cmd failed\n");
 		cmd_free(h, c);
-		return -ENOMEM;
+		return -1;
 	}
-	hpsa_scsi_do_simple_cmd_with_retry(h, c, PCI_DMA_FROMDEVICE);
+	rc = hpsa_scsi_do_simple_cmd_with_retry(h, c,
+					PCI_DMA_FROMDEVICE, NO_TIMEOUT);
+	if (rc)
+		goto out;
 	ei = c->err_info;
 	if (ei->CommandStatus != 0 && ei->CommandStatus != CMD_DATA_UNDERRUN) {
 		hpsa_scsi_interpret_error(h, c);
-		cmd_free(h, c);
-		return -1;
+		rc = -1;
+		goto out;
 	}
 	cmd_free(h, c);
 
@@ -2356,6 +2895,9 @@ static int hpsa_get_raid_map(struct ctlr_info *h,
 	}
 	hpsa_debug_map_buff(h, rc, &this_device->raid_map);
 	return rc;
+out:
+	cmd_free(h, c);
+	return rc;
 }
 
 static int hpsa_bmic_id_physical_device(struct ctlr_info *h,
@@ -2375,7 +2917,8 @@ static int hpsa_bmic_id_physical_device(struct ctlr_info *h,
 	c->Request.CDB[2] = bmic_device_index & 0xff;
 	c->Request.CDB[9] = (bmic_device_index >> 8) & 0xff;
 
-	hpsa_scsi_do_simple_cmd_with_retry(h, c, PCI_DMA_FROMDEVICE);
+	hpsa_scsi_do_simple_cmd_with_retry(h, c, PCI_DMA_FROMDEVICE,
+						NO_TIMEOUT);
 	ei = c->err_info;
 	if (ei->CommandStatus != 0 && ei->CommandStatus != CMD_DATA_UNDERRUN) {
 		hpsa_scsi_interpret_error(h, c);
@@ -2438,6 +2981,7 @@ static void hpsa_get_ioaccel_status(struct ctlr_info *h,
 
 	this_device->offload_config = 0;
 	this_device->offload_enabled = 0;
+	this_device->offload_to_be_enabled = 0;
 
 	buf = kzalloc(64, GFP_KERNEL);
 	if (!buf)
@@ -2461,6 +3005,7 @@ static void hpsa_get_ioaccel_status(struct ctlr_info *h,
 		if (hpsa_get_raid_map(h, scsi3addr, this_device))
 			this_device->offload_enabled = 0;
 	}
+	this_device->offload_to_be_enabled = this_device->offload_enabled;
 out:
 	kfree(buf);
 	return;
@@ -2495,10 +3040,7 @@ static int hpsa_scsi_do_report_luns(struct ctlr_info *h, int logical,
 	struct ErrorInfo *ei;
 
 	c = cmd_alloc(h);
-	if (c == NULL) {			/* trouble... */
-		dev_err(&h->pdev->dev, "cmd_alloc returned NULL!\n");
-		return -1;
-	}
+
 	/* address the controller */
 	memset(scsi3addr, 0, sizeof(scsi3addr));
 	if (fill_cmd(c, logical ? HPSA_REPORT_LOG : HPSA_REPORT_PHYS, h,
@@ -2508,7 +3050,10 @@ static int hpsa_scsi_do_report_luns(struct ctlr_info *h, int logical,
 	}
 	if (extended_response)
 		c->Request.CDB[1] = extended_response;
-	hpsa_scsi_do_simple_cmd_with_retry(h, c, PCI_DMA_FROMDEVICE);
+	rc = hpsa_scsi_do_simple_cmd_with_retry(h, c,
+					PCI_DMA_FROMDEVICE, NO_TIMEOUT);
+	if (rc)
+		goto out;
 	ei = c->err_info;
 	if (ei->CommandStatus != 0 &&
 	    ei->CommandStatus != CMD_DATA_UNDERRUN) {
@@ -2600,8 +3145,10 @@ static int hpsa_volume_offline(struct ctlr_info *h,
 					unsigned char scsi3addr[])
 {
 	struct CommandList *c;
-	unsigned char *sense, sense_key, asc, ascq;
-	int ldstat = 0;
+	unsigned char *sense;
+	u8 sense_key, asc, ascq;
+	int sense_len;
+	int rc, ldstat = 0;
 	u16 cmd_status;
 	u8 scsi_status;
 #define ASC_LUN_NOT_READY 0x04
@@ -2609,14 +3156,19 @@ static int hpsa_volume_offline(struct ctlr_info *h,
 #define ASCQ_LUN_NOT_READY_INITIALIZING_CMD_REQ 0x02
 
 	c = cmd_alloc(h);
-	if (!c)
-		return 0;
+
 	(void) fill_cmd(c, TEST_UNIT_READY, h, NULL, 0, 0, scsi3addr, TYPE_CMD);
-	hpsa_scsi_do_simple_cmd_core(h, c);
+	rc = hpsa_scsi_do_simple_cmd(h, c, DEFAULT_REPLY_QUEUE, NO_TIMEOUT);
+	if (rc) {
+		cmd_free(h, c);
+		return 0;
+	}
 	sense = c->err_info->SenseInfo;
-	sense_key = sense[2];
-	asc = sense[12];
-	ascq = sense[13];
+	if (c->err_info->SenseLen > sizeof(c->err_info->SenseInfo))
+		sense_len = sizeof(c->err_info->SenseInfo);
+	else
+		sense_len = c->err_info->SenseLen;
+	decode_sense_data(sense, sense_len, &sense_key, &asc, &ascq);
 	cmd_status = c->err_info->CommandStatus;
 	scsi_status = c->err_info->ScsiStatus;
 	cmd_free(h, c);
@@ -2656,6 +3208,52 @@ static int hpsa_volume_offline(struct ctlr_info *h,
 	return 0;
 }
 
+/*
+ * Find out if a logical device supports aborts by simply trying one.
+ * Smart Array may claim not to support aborts on logical drives, but
+ * if a MSA2000 * is connected, the drives on that will be presented
+ * by the Smart Array as logical drives, and aborts may be sent to
+ * those devices successfully.  So the simplest way to find out is
+ * to simply try an abort and see how the device responds.
+ */
+static int hpsa_device_supports_aborts(struct ctlr_info *h,
+					unsigned char *scsi3addr)
+{
+	struct CommandList *c;
+	struct ErrorInfo *ei;
+	int rc = 0;
+
+	u64 tag = (u64) -1; /* bogus tag */
+
+	/* Assume that physical devices support aborts */
+	if (!is_logical_dev_addr_mode(scsi3addr))
+		return 1;
+
+	c = cmd_alloc(h);
+
+	(void) fill_cmd(c, HPSA_ABORT_MSG, h, &tag, 0, 0, scsi3addr, TYPE_MSG);
+	(void) hpsa_scsi_do_simple_cmd(h, c, DEFAULT_REPLY_QUEUE, NO_TIMEOUT);
+	/* no unmap needed here because no data xfer. */
+	ei = c->err_info;
+	switch (ei->CommandStatus) {
+	case CMD_INVALID:
+		rc = 0;
+		break;
+	case CMD_UNABORTABLE:
+	case CMD_ABORT_FAILED:
+		rc = 1;
+		break;
+	case CMD_TMF_STATUS:
+		rc = hpsa_evaluate_tmf_status(h, c);
+		break;
+	default:
+		rc = 0;
+		break;
+	}
+	cmd_free(h, c);
+	return rc;
+}
+
 static int hpsa_update_device_info(struct ctlr_info *h,
 	unsigned char scsi3addr[], struct hpsa_scsi_dev_t *this_device,
 	unsigned char *is_OBDR_device)
@@ -2708,6 +3306,8 @@ static int hpsa_update_device_info(struct ctlr_info *h,
 		this_device->raid_level = RAID_UNKNOWN;
 		this_device->offload_config = 0;
 		this_device->offload_enabled = 0;
+		this_device->offload_to_be_enabled = 0;
+		this_device->hba_ioaccel_enabled = 0;
 		this_device->volume_offline = 0;
 		this_device->queue_depth = h->nr_cmds;
 	}
@@ -2721,7 +3321,6 @@ static int hpsa_update_device_info(struct ctlr_info *h,
 					strncmp(obdr_sig, OBDR_TAPE_SIG,
 						OBDR_SIG_LEN) == 0);
 	}
-
 	kfree(inq_buff);
 	return 0;
 
@@ -2730,6 +3329,31 @@ bail_out:
 	return 1;
 }
 
+static void hpsa_update_device_supports_aborts(struct ctlr_info *h,
+			struct hpsa_scsi_dev_t *dev, u8 *scsi3addr)
+{
+	unsigned long flags;
+	int rc, entry;
+	/*
+	 * See if this device supports aborts.  If we already know
+	 * the device, we already know if it supports aborts, otherwise
+	 * we have to find out if it supports aborts by trying one.
+	 */
+	spin_lock_irqsave(&h->devlock, flags);
+	rc = hpsa_scsi_find_entry(dev, h->dev, h->ndevices, &entry);
+	if ((rc == DEVICE_SAME || rc == DEVICE_UPDATED) &&
+		entry >= 0 && entry < h->ndevices) {
+		dev->supports_aborts = h->dev[entry]->supports_aborts;
+		spin_unlock_irqrestore(&h->devlock, flags);
+	} else {
+		spin_unlock_irqrestore(&h->devlock, flags);
+		dev->supports_aborts =
+				hpsa_device_supports_aborts(h, scsi3addr);
+		if (dev->supports_aborts < 0)
+			dev->supports_aborts = 0;
+	}
+}
+
 static unsigned char *ext_target_model[] = {
 	"MSA2012",
 	"MSA2024",
@@ -2835,6 +3459,7 @@ static int add_ext_target_dev(struct ctlr_info *h,
 	(*n_ext_target_devs)++;
 	hpsa_set_bus_target_lun(this_device,
 				tmpdevice->bus, tmpdevice->target, 0);
+	hpsa_update_device_supports_aborts(h, this_device, scsi3addr);
 	set_bit(tmpdevice->target, lunzerobits);
 	return 1;
 }
@@ -2850,88 +3475,23 @@ static int add_ext_target_dev(struct ctlr_info *h,
 static int hpsa_get_pdisk_of_ioaccel2(struct ctlr_info *h,
 	struct CommandList *ioaccel2_cmd_to_abort, unsigned char *scsi3addr)
 {
-	struct ReportExtendedLUNdata *physicals = NULL;
-	int responsesize = 24;	/* size of physical extended response */
-	int reportsize = sizeof(*physicals) + HPSA_MAX_PHYS_LUN * responsesize;
-	u32 nphysicals = 0;	/* number of reported physical devs */
-	int found = 0;		/* found match (1) or not (0) */
-	u32 find;		/* handle we need to match */
+	struct io_accel2_cmd *c2 =
+			&h->ioaccel2_cmd_pool[ioaccel2_cmd_to_abort->cmdindex];
+	unsigned long flags;
 	int i;
-	struct scsi_cmnd *scmd;	/* scsi command within request being aborted */
-	struct hpsa_scsi_dev_t *d; /* device of request being aborted */
-	struct io_accel2_cmd *c2a; /* ioaccel2 command to abort */
-	__le32 it_nexus;	/* 4 byte device handle for the ioaccel2 cmd */
-	__le32 scsi_nexus;	/* 4 byte device handle for the ioaccel2 cmd */
-
-	if (ioaccel2_cmd_to_abort->cmd_type != CMD_IOACCEL2)
-		return 0; /* no match */
-
-	/* point to the ioaccel2 device handle */
-	c2a = &h->ioaccel2_cmd_pool[ioaccel2_cmd_to_abort->cmdindex];
-	if (c2a == NULL)
-		return 0; /* no match */
-
-	scmd = (struct scsi_cmnd *) ioaccel2_cmd_to_abort->scsi_cmd;
-	if (scmd == NULL)
-		return 0; /* no match */
-
-	d = scmd->device->hostdata;
-	if (d == NULL)
-		return 0; /* no match */
-
-	it_nexus = cpu_to_le32(d->ioaccel_handle);
-	scsi_nexus = c2a->scsi_nexus;
-	find = le32_to_cpu(c2a->scsi_nexus);
-
-	if (h->raid_offload_debug > 0)
-		dev_info(&h->pdev->dev,
-			"%s: scsi_nexus:0x%08x device id: 0x%02x%02x%02x%02x %02x%02x%02x%02x %02x%02x%02x%02x %02x%02x%02x%02x\n",
-			__func__, scsi_nexus,
-			d->device_id[0], d->device_id[1], d->device_id[2],
-			d->device_id[3], d->device_id[4], d->device_id[5],
-			d->device_id[6], d->device_id[7], d->device_id[8],
-			d->device_id[9], d->device_id[10], d->device_id[11],
-			d->device_id[12], d->device_id[13], d->device_id[14],
-			d->device_id[15]);
-
-	/* Get the list of physical devices */
-	physicals = kzalloc(reportsize, GFP_KERNEL);
-	if (physicals == NULL)
-		return 0;
-	if (hpsa_scsi_do_report_phys_luns(h, physicals, reportsize)) {
-		dev_err(&h->pdev->dev,
-			"Can't lookup %s device handle: report physical LUNs failed.\n",
-			"HP SSD Smart Path");
-		kfree(physicals);
-		return 0;
-	}
-	nphysicals = be32_to_cpu(*((__be32 *)physicals->LUNListLength)) /
-							responsesize;
-
-	/* find ioaccel2 handle in list of physicals: */
-	for (i = 0; i < nphysicals; i++) {
-		struct ext_report_lun_entry *entry = &physicals->LUN[i];
-
-		/* handle is in bytes 28-31 of each lun */
-		if (entry->ioaccel_handle != find)
-			continue; /* didn't match */
-		found = 1;
-		memcpy(scsi3addr, entry->lunid, 8);
-		if (h->raid_offload_debug > 0)
-			dev_info(&h->pdev->dev,
-				"%s: Searched h=0x%08x, Found h=0x%08x, scsiaddr 0x%8phN\n",
-				__func__, find,
-				entry->ioaccel_handle, scsi3addr);
-		break; /* found it */
-	}
-
-	kfree(physicals);
-	if (found)
-		return 1;
-	else
-		return 0;
 
+	spin_lock_irqsave(&h->devlock, flags);
+	for (i = 0; i < h->ndevices; i++)
+		if (h->dev[i]->ioaccel_handle == le32_to_cpu(c2->scsi_nexus)) {
+			memcpy(scsi3addr, h->dev[i]->scsi3addr,
+				sizeof(h->dev[i]->scsi3addr));
+			spin_unlock_irqrestore(&h->devlock, flags);
+			return 1;
+		}
+	spin_unlock_irqrestore(&h->devlock, flags);
+	return 0;
 }
+
 /*
  * Do CISS_REPORT_PHYS and CISS_REPORT_LOG.  Data is returned in physdev,
  * logdev.  The number of luns in physdev and logdev are returned in
@@ -3036,6 +3596,8 @@ static void hpsa_get_ioaccel_drive_info(struct ctlr_info *h,
 		(struct ext_report_lun_entry *) lunaddrbytes;
 
 	dev->ioaccel_handle = rle->ioaccel_handle;
+	if (PHYS_IOACCEL(lunaddrbytes) && dev->ioaccel_handle)
+		dev->hba_ioaccel_enabled = 1;
 	memset(id_phys, 0, sizeof(*id_phys));
 	rc = hpsa_bmic_id_physical_device(h, lunaddrbytes,
 			GET_BMIC_DRIVE_NUMBER(lunaddrbytes), id_phys,
@@ -3050,6 +3612,7 @@ static void hpsa_get_ioaccel_drive_info(struct ctlr_info *h,
 	else
 		dev->queue_depth = DRIVE_QUEUE_DEPTH; /* conservative */
 	atomic_set(&dev->ioaccel_cmds_out, 0);
+	atomic_set(&dev->reset_cmds_out, 0);
 }
 
 static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno)
@@ -3142,16 +3705,19 @@ static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno)
 		/* Figure out where the LUN ID info is coming from */
 		lunaddrbytes = figure_lunaddrbytes(h, raid_ctlr_position,
 			i, nphysicals, nlogicals, physdev_list, logdev_list);
-		/* skip masked physical devices. */
-		if (lunaddrbytes[3] & 0xC0 &&
-			i < nphysicals + (raid_ctlr_position == 0))
-			continue;
+
+		/* skip masked non-disk devices */
+		if (MASKED_DEVICE(lunaddrbytes))
+			if (i < nphysicals + (raid_ctlr_position == 0) &&
+				NON_DISK_PHYS_DEV(lunaddrbytes))
+				continue;
 
 		/* Get device type, vendor, model, device id */
 		if (hpsa_update_device_info(h, lunaddrbytes, tmpdevice,
 							&is_OBDR))
 			continue; /* skip it if we can't talk to it. */
 		figure_bus_target_lun(h, lunaddrbytes, tmpdevice);
+		hpsa_update_device_supports_aborts(h, tmpdevice, lunaddrbytes);
 		this_device = currentsd[ncurrent];
 
 		/*
@@ -3170,6 +3736,18 @@ static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno)
 
 		*this_device = *tmpdevice;
 
+		/* do not expose masked devices */
+		if (MASKED_DEVICE(lunaddrbytes) &&
+			i < nphysicals + (raid_ctlr_position == 0)) {
+			if (h->hba_mode_enabled)
+				dev_warn(&h->pdev->dev,
+					"Masked physical device detected\n");
+			this_device->expose_state = HPSA_DO_NOT_EXPOSE;
+		} else {
+			this_device->expose_state =
+					HPSA_SG_ATTACH | HPSA_ULD_ATTACH;
+		}
+
 		switch (this_device->devtype) {
 		case TYPE_ROM:
 			/* We don't *really* support actual CD-ROM devices,
@@ -3183,34 +3761,31 @@ static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno)
 				ncurrent++;
 			break;
 		case TYPE_DISK:
-			if (h->hba_mode_enabled) {
-				/* never use raid mapper in HBA mode */
-				this_device->offload_enabled = 0;
-				ncurrent++;
-				break;
-			} else if (h->acciopath_status) {
-				if (i >= nphysicals) {
-					ncurrent++;
-					break;
-				}
-			} else {
-				if (i < nphysicals)
-					break;
+			if (i >= nphysicals) {
 				ncurrent++;
 				break;
 			}
-			if (h->transMethod & CFGTBL_Trans_io_accel1 ||
-				h->transMethod & CFGTBL_Trans_io_accel2) {
-				hpsa_get_ioaccel_drive_info(h, this_device,
-							lunaddrbytes, id_phys);
-				atomic_set(&this_device->ioaccel_cmds_out, 0);
-				ncurrent++;
-			}
+
+			if (h->hba_mode_enabled)
+				/* never use raid mapper in HBA mode */
+				this_device->offload_enabled = 0;
+			else if (!(h->transMethod & CFGTBL_Trans_io_accel1 ||
+				h->transMethod & CFGTBL_Trans_io_accel2))
+				break;
+
+			hpsa_get_ioaccel_drive_info(h, this_device,
+						lunaddrbytes, id_phys);
+			atomic_set(&this_device->ioaccel_cmds_out, 0);
+			ncurrent++;
 			break;
 		case TYPE_TAPE:
 		case TYPE_MEDIUM_CHANGER:
 			ncurrent++;
 			break;
+		case TYPE_ENCLOSURE:
+			if (h->hba_mode_enabled)
+				ncurrent++;
+			break;
 		case TYPE_RAID:
 			/* Only present the Smartarray HBA as a RAID controller.
 			 * If it's a RAID controller other than the HBA itself
@@ -3227,7 +3802,6 @@ static void hpsa_update_scsi_devices(struct ctlr_info *h, int hostno)
 		if (ncurrent >= HPSA_MAX_DEVICES)
 			break;
 	}
-	hpsa_update_log_drive_phys_drive_ptrs(h, currentsd, ncurrent);
 	adjust_hpsa_scsi_table(h, hostno, currentsd, ncurrent);
 out:
 	kfree(tmpdevice);
@@ -3260,7 +3834,7 @@ static int hpsa_scatter_gather(struct ctlr_info *h,
 		struct scsi_cmnd *cmd)
 {
 	struct scatterlist *sg;
-	int use_sg, i, sg_index, chained;
+	int use_sg, i, sg_limit, chained, last_sg;
 	struct SGDescriptor *curr_sg;
 
 	BUG_ON(scsi_sg_count(cmd) > h->maxsgentries);
@@ -3272,22 +3846,39 @@ static int hpsa_scatter_gather(struct ctlr_info *h,
 	if (!use_sg)
 		goto sglist_finished;
 
+	/*
+	 * If the number of entries is greater than the max for a single list,
+	 * then we have a chained list; we will set up all but one entry in the
+	 * first list (the last entry is saved for link information);
+	 * otherwise, we don't have a chained list and we'll set up at each of
+	 * the entries in the one list.
+	 */
 	curr_sg = cp->SG;
-	chained = 0;
-	sg_index = 0;
-	scsi_for_each_sg(cmd, sg, use_sg, i) {
-		if (i == h->max_cmd_sg_entries - 1 &&
-			use_sg > h->max_cmd_sg_entries) {
-			chained = 1;
-			curr_sg = h->cmd_sg_list[cp->cmdindex];
-			sg_index = 0;
-		}
+	chained = use_sg > h->max_cmd_sg_entries;
+	sg_limit = chained ? h->max_cmd_sg_entries - 1 : use_sg;
+	last_sg = scsi_sg_count(cmd) - 1;
+	scsi_for_each_sg(cmd, sg, sg_limit, i) {
 		hpsa_set_sg_descriptor(curr_sg, sg);
 		curr_sg++;
 	}
 
+	if (chained) {
+		/*
+		 * Continue with the chained list.  Set curr_sg to the chained
+		 * list.  Modify the limit to the total count less the entries
+		 * we've already set up.  Resume the scan at the list entry
+		 * where the previous loop left off.
+		 */
+		curr_sg = h->cmd_sg_list[cp->cmdindex];
+		sg_limit = use_sg - sg_limit;
+		for_each_sg(sg, sg, sg_limit, i) {
+			hpsa_set_sg_descriptor(curr_sg, sg);
+			curr_sg++;
+		}
+	}
+
 	/* Back the pointer up to the last entry and mark it as "last". */
-	(--curr_sg)->Ext = cpu_to_le32(HPSA_SG_LAST);
+	(curr_sg - 1)->Ext = cpu_to_le32(HPSA_SG_LAST);
 
 	if (use_sg + chained > h->maxSG)
 		h->maxSG = use_sg + chained;
@@ -3530,10 +4121,7 @@ static int hpsa_scsi_ioaccel2_queue_command(struct ctlr_info *h,
 	u32 len;
 	u32 total_len = 0;
 
-	if (scsi_sg_count(cmd) > h->ioaccel_maxsg) {
-		atomic_dec(&phys_disk->ioaccel_cmds_out);
-		return IO_ACCEL_INELIGIBLE;
-	}
+	BUG_ON(scsi_sg_count(cmd) > h->maxsgentries);
 
 	if (fixup_ioaccel_cdb(cdb, &cdb_len)) {
 		atomic_dec(&phys_disk->ioaccel_cmds_out);
@@ -3556,8 +4144,19 @@ static int hpsa_scsi_ioaccel2_queue_command(struct ctlr_info *h,
 	}
 
 	if (use_sg) {
-		BUG_ON(use_sg > IOACCEL2_MAXSGENTRIES);
 		curr_sg = cp->sg;
+		if (use_sg > h->ioaccel_maxsg) {
+			addr64 = le64_to_cpu(
+				h->ioaccel2_cmd_sg_list[c->cmdindex]->address);
+			curr_sg->address = cpu_to_le64(addr64);
+			curr_sg->length = 0;
+			curr_sg->reserved[0] = 0;
+			curr_sg->reserved[1] = 0;
+			curr_sg->reserved[2] = 0;
+			curr_sg->chain_indicator = 0x80;
+
+			curr_sg = h->ioaccel2_cmd_sg_list[c->cmdindex];
+		}
 		scsi_for_each_sg(cmd, sg, use_sg, i) {
 			addr64 = (u64) sg_dma_address(sg);
 			len  = sg_dma_len(sg);
@@ -3602,14 +4201,22 @@ static int hpsa_scsi_ioaccel2_queue_command(struct ctlr_info *h,
 	cp->Tag = cpu_to_le32(c->cmdindex << DIRECT_LOOKUP_SHIFT);
 	memcpy(cp->cdb, cdb, sizeof(cp->cdb));
 
-	/* fill in sg elements */
-	cp->sg_count = (u8) use_sg;
-
 	cp->data_len = cpu_to_le32(total_len);
 	cp->err_ptr = cpu_to_le64(c->busaddr +
 			offsetof(struct io_accel2_cmd, error_data));
 	cp->err_len = cpu_to_le32(sizeof(cp->error_data));
 
+	/* fill in sg elements */
+	if (use_sg > h->ioaccel_maxsg) {
+		cp->sg_count = 1;
+		if (hpsa_map_ioaccel2_sg_chain_block(h, cp, c)) {
+			atomic_dec(&phys_disk->ioaccel_cmds_out);
+			scsi_dma_unmap(cmd);
+			return -1;
+		}
+	} else
+		cp->sg_count = (u8) use_sg;
+
 	enqueue_cmd_and_start_io(h, c);
 	return 0;
 }
@@ -3992,7 +4599,11 @@ static int hpsa_scsi_ioaccel_raid_map(struct ctlr_info *h,
 						dev->phys_disk[map_index]);
 }
 
-/* Submit commands down the "normal" RAID stack path */
+/*
+ * Submit commands down the "normal" RAID stack path
+ * All callers to hpsa_ciss_submit must check lockup_detected
+ * beforehand, before (opt.) and after calling cmd_alloc
+ */
 static int hpsa_ciss_submit(struct ctlr_info *h,
 	struct CommandList *c, struct scsi_cmnd *cmd,
 	unsigned char scsi3addr[])
@@ -4007,7 +4618,6 @@ static int hpsa_ciss_submit(struct ctlr_info *h,
 	/* Fill in the request block... */
 
 	c->Request.Timeout = 0;
-	memset(c->Request.CDB, 0, sizeof(c->Request.CDB));
 	BUG_ON(cmd->cmd_len > sizeof(c->Request.CDB));
 	c->Request.CDBLen = cmd->cmd_len;
 	memcpy(c->Request.CDB, cmd->cmnd, cmd->cmd_len);
@@ -4050,7 +4660,7 @@ static int hpsa_ciss_submit(struct ctlr_info *h,
 	}
 
 	if (hpsa_scatter_gather(h, c, cmd) < 0) { /* Fill SG list */
-		cmd_free(h, c);
+		hpsa_cmd_resolve_and_free(h, c);
 		return SCSI_MLQUEUE_HOST_BUSY;
 	}
 	enqueue_cmd_and_start_io(h, c);
@@ -4058,25 +4668,125 @@ static int hpsa_ciss_submit(struct ctlr_info *h,
 	return 0;
 }
 
+static void hpsa_cmd_init(struct ctlr_info *h, int index,
+				struct CommandList *c)
+{
+	dma_addr_t cmd_dma_handle, err_dma_handle;
+
+	/* Zero out all of commandlist except the last field, refcount */
+	memset(c, 0, offsetof(struct CommandList, refcount));
+	c->Header.tag = cpu_to_le64((u64) (index << DIRECT_LOOKUP_SHIFT));
+	cmd_dma_handle = h->cmd_pool_dhandle + index * sizeof(*c);
+	c->err_info = h->errinfo_pool + index;
+	memset(c->err_info, 0, sizeof(*c->err_info));
+	err_dma_handle = h->errinfo_pool_dhandle
+	    + index * sizeof(*c->err_info);
+	c->cmdindex = index;
+	c->busaddr = (u32) cmd_dma_handle;
+	c->ErrDesc.Addr = cpu_to_le64((u64) err_dma_handle);
+	c->ErrDesc.Len = cpu_to_le32((u32) sizeof(*c->err_info));
+	c->h = h;
+	c->scsi_cmd = SCSI_CMD_IDLE;
+}
+
+static void hpsa_preinitialize_commands(struct ctlr_info *h)
+{
+	int i;
+
+	for (i = 0; i < h->nr_cmds; i++) {
+		struct CommandList *c = h->cmd_pool + i;
+
+		hpsa_cmd_init(h, i, c);
+		atomic_set(&c->refcount, 0);
+	}
+}
+
+static inline void hpsa_cmd_partial_init(struct ctlr_info *h, int index,
+				struct CommandList *c)
+{
+	dma_addr_t cmd_dma_handle = h->cmd_pool_dhandle + index * sizeof(*c);
+
+	BUG_ON(c->cmdindex != index);
+
+	memset(c->Request.CDB, 0, sizeof(c->Request.CDB));
+	memset(c->err_info, 0, sizeof(*c->err_info));
+	c->busaddr = (u32) cmd_dma_handle;
+}
+
+static int hpsa_ioaccel_submit(struct ctlr_info *h,
+		struct CommandList *c, struct scsi_cmnd *cmd,
+		unsigned char *scsi3addr)
+{
+	struct hpsa_scsi_dev_t *dev = cmd->device->hostdata;
+	int rc = IO_ACCEL_INELIGIBLE;
+
+	cmd->host_scribble = (unsigned char *) c;
+
+	if (dev->offload_enabled) {
+		hpsa_cmd_init(h, c->cmdindex, c);
+		c->cmd_type = CMD_SCSI;
+		c->scsi_cmd = cmd;
+		rc = hpsa_scsi_ioaccel_raid_map(h, c);
+		if (rc < 0)     /* scsi_dma_map failed. */
+			rc = SCSI_MLQUEUE_HOST_BUSY;
+	} else if (dev->hba_ioaccel_enabled) {
+		hpsa_cmd_init(h, c->cmdindex, c);
+		c->cmd_type = CMD_SCSI;
+		c->scsi_cmd = cmd;
+		rc = hpsa_scsi_ioaccel_direct_map(h, c);
+		if (rc < 0)     /* scsi_dma_map failed. */
+			rc = SCSI_MLQUEUE_HOST_BUSY;
+	}
+	return rc;
+}
+
 static void hpsa_command_resubmit_worker(struct work_struct *work)
 {
 	struct scsi_cmnd *cmd;
 	struct hpsa_scsi_dev_t *dev;
-	struct CommandList *c =
-			container_of(work, struct CommandList, work);
+	struct CommandList *c = container_of(work, struct CommandList, work);
 
 	cmd = c->scsi_cmd;
 	dev = cmd->device->hostdata;
 	if (!dev) {
 		cmd->result = DID_NO_CONNECT << 16;
-		cmd->scsi_done(cmd);
-		return;
+		return hpsa_cmd_free_and_done(c->h, c, cmd);
+	}
+	if (c->reset_pending)
+		return hpsa_cmd_resolve_and_free(c->h, c);
+	if (c->abort_pending)
+		return hpsa_cmd_abort_and_free(c->h, c, cmd);
+	if (c->cmd_type == CMD_IOACCEL2) {
+		struct ctlr_info *h = c->h;
+		struct io_accel2_cmd *c2 = &h->ioaccel2_cmd_pool[c->cmdindex];
+		int rc;
+
+		if (c2->error_data.serv_response ==
+				IOACCEL2_STATUS_SR_TASK_COMP_SET_FULL) {
+			rc = hpsa_ioaccel_submit(h, c, cmd, dev->scsi3addr);
+			if (rc == 0)
+				return;
+			if (rc == SCSI_MLQUEUE_HOST_BUSY) {
+				/*
+				 * If we get here, it means dma mapping failed.
+				 * Try again via scsi mid layer, which will
+				 * then get SCSI_MLQUEUE_HOST_BUSY.
+				 */
+				cmd->result = DID_IMM_RETRY << 16;
+				return hpsa_cmd_free_and_done(h, c, cmd);
+			}
+			/* else, fall thru and resubmit down CISS path */
+		}
 	}
+	hpsa_cmd_partial_init(c->h, c->cmdindex, c);
 	if (hpsa_ciss_submit(c->h, c, cmd, dev->scsi3addr)) {
 		/*
 		 * If we get here, it means dma mapping failed. Try
 		 * again via scsi mid layer, which will then get
 		 * SCSI_MLQUEUE_HOST_BUSY.
+		 *
+		 * hpsa_ciss_submit will have already freed c
+		 * if it encountered a dma mapping failure.
 		 */
 		cmd->result = DID_IMM_RETRY << 16;
 		cmd->scsi_done(cmd);
@@ -4094,30 +4804,24 @@ static int hpsa_scsi_queue_command(struct Scsi_Host *sh, struct scsi_cmnd *cmd)
 
 	/* Get the ptr to our adapter structure out of cmd->host. */
 	h = sdev_to_hba(cmd->device);
+
+	BUG_ON(cmd->request->tag < 0);
+
 	dev = cmd->device->hostdata;
 	if (!dev) {
 		cmd->result = DID_NO_CONNECT << 16;
 		cmd->scsi_done(cmd);
 		return 0;
 	}
+
 	memcpy(scsi3addr, dev->scsi3addr, sizeof(scsi3addr));
 
 	if (unlikely(lockup_detected(h))) {
-		cmd->result = DID_ERROR << 16;
-		cmd->scsi_done(cmd);
-		return 0;
-	}
-	c = cmd_alloc(h);
-	if (c == NULL) {			/* trouble... */
-		dev_err(&h->pdev->dev, "cmd_alloc returned NULL!\n");
-		return SCSI_MLQUEUE_HOST_BUSY;
-	}
-	if (unlikely(lockup_detected(h))) {
-		cmd->result = DID_ERROR << 16;
-		cmd_free(h, c);
+		cmd->result = DID_NO_CONNECT << 16;
 		cmd->scsi_done(cmd);
 		return 0;
 	}
+	c = cmd_tagged_alloc(h, cmd);
 
 	/*
 	 * Call alternate submit routine for I/O accelerated commands.
@@ -4126,27 +4830,12 @@ static int hpsa_scsi_queue_command(struct Scsi_Host *sh, struct scsi_cmnd *cmd)
 	if (likely(cmd->retries == 0 &&
 		cmd->request->cmd_type == REQ_TYPE_FS &&
 		h->acciopath_status)) {
-
-		cmd->host_scribble = (unsigned char *) c;
-		c->cmd_type = CMD_SCSI;
-		c->scsi_cmd = cmd;
-
-		if (dev->offload_enabled) {
-			rc = hpsa_scsi_ioaccel_raid_map(h, c);
-			if (rc == 0)
-				return 0; /* Sent on ioaccel path */
-			if (rc < 0) {   /* scsi_dma_map failed. */
-				cmd_free(h, c);
-				return SCSI_MLQUEUE_HOST_BUSY;
-			}
-		} else if (dev->ioaccel_handle) {
-			rc = hpsa_scsi_ioaccel_direct_map(h, c);
-			if (rc == 0)
-				return 0; /* Sent on direct map path */
-			if (rc < 0) {   /* scsi_dma_map failed. */
-				cmd_free(h, c);
-				return SCSI_MLQUEUE_HOST_BUSY;
-			}
+		rc = hpsa_ioaccel_submit(h, c, cmd, scsi3addr);
+		if (rc == 0)
+			return 0;
+		if (rc == SCSI_MLQUEUE_HOST_BUSY) {
+			hpsa_cmd_resolve_and_free(h, c);
+			return SCSI_MLQUEUE_HOST_BUSY;
 		}
 	}
 	return hpsa_ciss_submit(h, c, cmd, scsi3addr);
@@ -4228,22 +4917,16 @@ static int hpsa_scan_finished(struct Scsi_Host *sh,
 	return finished;
 }
 
-static void hpsa_unregister_scsi(struct ctlr_info *h)
-{
-	/* we are being forcibly unloaded, and may not refuse. */
-	scsi_remove_host(h->scsi_host);
-	scsi_host_put(h->scsi_host);
-	h->scsi_host = NULL;
-}
-
-static int hpsa_register_scsi(struct ctlr_info *h)
+static int hpsa_scsi_host_alloc(struct ctlr_info *h)
 {
 	struct Scsi_Host *sh;
 	int error;
 
 	sh = scsi_host_alloc(&hpsa_driver_template, sizeof(h));
-	if (sh == NULL)
-		goto fail;
+	if (sh == NULL) {
+		dev_err(&h->pdev->dev, "scsi_host_alloc failed\n");
+		return -ENOMEM;
+	}
 
 	sh->io_port = 0;
 	sh->n_io_port = 0;
@@ -4252,80 +4935,156 @@ static int hpsa_register_scsi(struct ctlr_info *h)
 	sh->max_cmd_len = MAX_COMMAND_SIZE;
 	sh->max_lun = HPSA_MAX_LUN;
 	sh->max_id = HPSA_MAX_LUN;
-	sh->can_queue = h->nr_cmds -
-			HPSA_CMDS_RESERVED_FOR_ABORTS -
-			HPSA_CMDS_RESERVED_FOR_DRIVER -
-			HPSA_MAX_CONCURRENT_PASSTHRUS;
+	sh->can_queue = h->nr_cmds - HPSA_NRESERVED_CMDS;
 	sh->cmd_per_lun = sh->can_queue;
 	sh->sg_tablesize = h->maxsgentries;
-	h->scsi_host = sh;
 	sh->hostdata[0] = (unsigned long) h;
 	sh->irq = h->intr[h->intr_mode];
 	sh->unique_id = sh->irq;
-	error = scsi_add_host(sh, &h->pdev->dev);
-	if (error)
-		goto fail_host_put;
-	scsi_scan_host(sh);
+	error = scsi_init_shared_tag_map(sh, sh->can_queue);
+	if (error) {
+		dev_err(&h->pdev->dev,
+			"%s: scsi_init_shared_tag_map failed for controller %d\n",
+			__func__, h->ctlr);
+			scsi_host_put(sh);
+			return error;
+	}
+	h->scsi_host = sh;
 	return 0;
+}
 
- fail_host_put:
-	dev_err(&h->pdev->dev, "%s: scsi_add_host"
-		" failed for controller %d\n", __func__, h->ctlr);
-	scsi_host_put(sh);
-	return error;
- fail:
-	dev_err(&h->pdev->dev, "%s: scsi_host_alloc"
-		" failed for controller %d\n", __func__, h->ctlr);
-	return -ENOMEM;
+static int hpsa_scsi_add_host(struct ctlr_info *h)
+{
+	int rv;
+
+	rv = scsi_add_host(h->scsi_host, &h->pdev->dev);
+	if (rv) {
+		dev_err(&h->pdev->dev, "scsi_add_host failed\n");
+		return rv;
+	}
+	scsi_scan_host(h->scsi_host);
+	return 0;
 }
 
-static int wait_for_device_to_become_ready(struct ctlr_info *h,
-	unsigned char lunaddr[])
+/*
+ * The block layer has already gone to the trouble of picking out a unique,
+ * small-integer tag for this request.  We use an offset from that value as
+ * an index to select our command block.  (The offset allows us to reserve the
+ * low-numbered entries for our own uses.)
+ */
+static int hpsa_get_cmd_index(struct scsi_cmnd *scmd)
+{
+	int idx = scmd->request->tag;
+
+	if (idx < 0)
+		return idx;
+
+	/* Offset to leave space for internal cmds. */
+	return idx += HPSA_NRESERVED_CMDS;
+}
+
+/*
+ * Send a TEST_UNIT_READY command to the specified LUN using the specified
+ * reply queue; returns zero if the unit is ready, and non-zero otherwise.
+ */
+static int hpsa_send_test_unit_ready(struct ctlr_info *h,
+				struct CommandList *c, unsigned char lunaddr[],
+				int reply_queue)
+{
+	int rc;
+
+	/* Send the Test Unit Ready, fill_cmd can't fail, no mapping */
+	(void) fill_cmd(c, TEST_UNIT_READY, h,
+			NULL, 0, 0, lunaddr, TYPE_CMD);
+	rc = hpsa_scsi_do_simple_cmd(h, c, reply_queue, NO_TIMEOUT);
+	if (rc)
+		return rc;
+	/* no unmap needed here because no data xfer. */
+
+	/* Check if the unit is already ready. */
+	if (c->err_info->CommandStatus == CMD_SUCCESS)
+		return 0;
+
+	/*
+	 * The first command sent after reset will receive "unit attention" to
+	 * indicate that the LUN has been reset...this is actually what we're
+	 * looking for (but, success is good too).
+	 */
+	if (c->err_info->CommandStatus == CMD_TARGET_STATUS &&
+		c->err_info->ScsiStatus == SAM_STAT_CHECK_CONDITION &&
+			(c->err_info->SenseInfo[2] == NO_SENSE ||
+			 c->err_info->SenseInfo[2] == UNIT_ATTENTION))
+		return 0;
+
+	return 1;
+}
+
+/*
+ * Wait for a TEST_UNIT_READY command to complete, retrying as necessary;
+ * returns zero when the unit is ready, and non-zero when giving up.
+ */
+static int hpsa_wait_for_test_unit_ready(struct ctlr_info *h,
+				struct CommandList *c,
+				unsigned char lunaddr[], int reply_queue)
 {
 	int rc;
 	int count = 0;
 	int waittime = 1; /* seconds */
-	struct CommandList *c;
-
-	c = cmd_alloc(h);
-	if (!c) {
-		dev_warn(&h->pdev->dev, "out of memory in "
-			"wait_for_device_to_become_ready.\n");
-		return IO_ERROR;
-	}
 
 	/* Send test unit ready until device ready, or give up. */
-	while (count < HPSA_TUR_RETRY_LIMIT) {
+	for (count = 0; count < HPSA_TUR_RETRY_LIMIT; count++) {
 
-		/* Wait for a bit.  do this first, because if we send
+		/*
+		 * Wait for a bit.  do this first, because if we send
 		 * the TUR right away, the reset will just abort it.
 		 */
 		msleep(1000 * waittime);
-		count++;
-		rc = 0; /* Device ready. */
+
+		rc = hpsa_send_test_unit_ready(h, c, lunaddr, reply_queue);
+		if (!rc)
+			break;
 
 		/* Increase wait time with each try, up to a point. */
 		if (waittime < HPSA_MAX_WAIT_INTERVAL_SECS)
-			waittime = waittime * 2;
+			waittime *= 2;
 
-		/* Send the Test Unit Ready, fill_cmd can't fail, no mapping */
-		(void) fill_cmd(c, TEST_UNIT_READY, h,
-				NULL, 0, 0, lunaddr, TYPE_CMD);
-		hpsa_scsi_do_simple_cmd_core(h, c);
-		/* no unmap needed here because no data xfer. */
+		dev_warn(&h->pdev->dev,
+			 "waiting %d secs for device to become ready.\n",
+			 waittime);
+	}
 
-		if (c->err_info->CommandStatus == CMD_SUCCESS)
-			break;
+	return rc;
+}
 
-		if (c->err_info->CommandStatus == CMD_TARGET_STATUS &&
-			c->err_info->ScsiStatus == SAM_STAT_CHECK_CONDITION &&
-			(c->err_info->SenseInfo[2] == NO_SENSE ||
-			c->err_info->SenseInfo[2] == UNIT_ATTENTION))
-			break;
+static int wait_for_device_to_become_ready(struct ctlr_info *h,
+					   unsigned char lunaddr[],
+					   int reply_queue)
+{
+	int first_queue;
+	int last_queue;
+	int rq;
+	int rc = 0;
+	struct CommandList *c;
 
-		dev_warn(&h->pdev->dev, "waiting %d secs "
-			"for device to become ready.\n", waittime);
-		rc = 1; /* device not ready. */
+	c = cmd_alloc(h);
+
+	/*
+	 * If no specific reply queue was requested, then send the TUR
+	 * repeatedly, requesting a reply on each reply queue; otherwise execute
+	 * the loop exactly once using only the specified queue.
+	 */
+	if (reply_queue == DEFAULT_REPLY_QUEUE) {
+		first_queue = 0;
+		last_queue = h->nreply_queues - 1;
+	} else {
+		first_queue = reply_queue;
+		last_queue = reply_queue;
+	}
+
+	for (rq = first_queue; rq <= last_queue; rq++) {
+		rc = hpsa_wait_for_test_unit_ready(h, c, lunaddr, rq);
+		if (rc)
+			break;
 	}
 
 	if (rc)
@@ -4345,6 +5104,7 @@ static int hpsa_eh_device_reset_handler(struct scsi_cmnd *scsicmd)
 	int rc;
 	struct ctlr_info *h;
 	struct hpsa_scsi_dev_t *dev;
+	char msg[40];
 
 	/* find the controller to which the command to be aborted was sent */
 	h = sdev_to_hba(scsicmd->device);
@@ -4356,19 +5116,38 @@ static int hpsa_eh_device_reset_handler(struct scsi_cmnd *scsicmd)
 
 	dev = scsicmd->device->hostdata;
 	if (!dev) {
-		dev_err(&h->pdev->dev, "hpsa_eh_device_reset_handler: "
-			"device lookup failed.\n");
+		dev_err(&h->pdev->dev, "%s: device lookup failed\n", __func__);
 		return FAILED;
 	}
-	dev_warn(&h->pdev->dev, "resetting device %d:%d:%d:%d\n",
-		h->scsi_host->host_no, dev->bus, dev->target, dev->lun);
-	/* send a reset to the SCSI LUN which the command was sent to */
-	rc = hpsa_send_reset(h, dev->scsi3addr, HPSA_RESET_TYPE_LUN);
-	if (rc == 0 && wait_for_device_to_become_ready(h, dev->scsi3addr) == 0)
+
+	/* if controller locked up, we can guarantee command won't complete */
+	if (lockup_detected(h)) {
+		sprintf(msg, "cmd %d RESET FAILED, lockup detected",
+				hpsa_get_cmd_index(scsicmd));
+		hpsa_show_dev_msg(KERN_WARNING, h, dev, msg);
+		return FAILED;
+	}
+
+	/* this reset request might be the result of a lockup; check */
+	if (detect_controller_lockup(h)) {
+		sprintf(msg, "cmd %d RESET FAILED, new lockup detected",
+				hpsa_get_cmd_index(scsicmd));
+		hpsa_show_dev_msg(KERN_WARNING, h, dev, msg);
+		return FAILED;
+	}
+
+	/* Do not attempt on controller */
+	if (is_hba_lunid(dev->scsi3addr))
 		return SUCCESS;
 
-	dev_warn(&h->pdev->dev, "resetting device failed.\n");
-	return FAILED;
+	hpsa_show_dev_msg(KERN_WARNING, h, dev, "resetting");
+
+	/* send a reset to the SCSI LUN which the command was sent to */
+	rc = hpsa_do_reset(h, dev, dev->scsi3addr, HPSA_RESET_TYPE_LUN,
+			   DEFAULT_REPLY_QUEUE);
+	sprintf(msg, "reset %s", rc == 0 ? "completed successfully" : "failed");
+	hpsa_show_dev_msg(KERN_WARNING, h, dev, msg);
+	return rc == 0 ? SUCCESS : FAILED;
 }
 
 static void swizzle_abort_tag(u8 *tag)
@@ -4412,7 +5191,7 @@ static void hpsa_get_tag(struct ctlr_info *h,
 }
 
 static int hpsa_send_abort(struct ctlr_info *h, unsigned char *scsi3addr,
-	struct CommandList *abort, int swizzle)
+	struct CommandList *abort, int reply_queue)
 {
 	int rc = IO_OK;
 	struct CommandList *c;
@@ -4420,19 +5199,15 @@ static int hpsa_send_abort(struct ctlr_info *h, unsigned char *scsi3addr,
 	__le32 tagupper, taglower;
 
 	c = cmd_alloc(h);
-	if (c == NULL) {	/* trouble... */
-		dev_warn(&h->pdev->dev, "cmd_alloc returned NULL!\n");
-		return -ENOMEM;
-	}
 
 	/* fill_cmd can't fail here, no buffer to map */
-	(void) fill_cmd(c, HPSA_ABORT_MSG, h, abort,
+	(void) fill_cmd(c, HPSA_ABORT_MSG, h, &abort->Header.tag,
 		0, 0, scsi3addr, TYPE_MSG);
-	if (swizzle)
+	if (h->needs_abort_tags_swizzled)
 		swizzle_abort_tag(&c->Request.CDB[4]);
-	hpsa_scsi_do_simple_cmd_core(h, c);
+	(void) hpsa_scsi_do_simple_cmd(h, c, reply_queue, NO_TIMEOUT);
 	hpsa_get_tag(h, abort, &taglower, &tagupper);
-	dev_dbg(&h->pdev->dev, "%s: Tag:0x%08x:%08x: do_simple_cmd_core completed.\n",
+	dev_dbg(&h->pdev->dev, "%s: Tag:0x%08x:%08x: do_simple_cmd(abort) completed.\n",
 		__func__, tagupper, taglower);
 	/* no unmap needed here because no data xfer. */
 
@@ -4440,6 +5215,9 @@ static int hpsa_send_abort(struct ctlr_info *h, unsigned char *scsi3addr,
 	switch (ei->CommandStatus) {
 	case CMD_SUCCESS:
 		break;
+	case CMD_TMF_STATUS:
+		rc = hpsa_evaluate_tmf_status(h, c);
+		break;
 	case CMD_UNABORTABLE: /* Very common, don't make noise. */
 		rc = -1;
 		break;
@@ -4456,6 +5234,48 @@ static int hpsa_send_abort(struct ctlr_info *h, unsigned char *scsi3addr,
 	return rc;
 }
 
+static void setup_ioaccel2_abort_cmd(struct CommandList *c, struct ctlr_info *h,
+	struct CommandList *command_to_abort, int reply_queue)
+{
+	struct io_accel2_cmd *c2 = &h->ioaccel2_cmd_pool[c->cmdindex];
+	struct hpsa_tmf_struct *ac = (struct hpsa_tmf_struct *) c2;
+	struct io_accel2_cmd *c2a =
+		&h->ioaccel2_cmd_pool[command_to_abort->cmdindex];
+	struct scsi_cmnd *scmd = command_to_abort->scsi_cmd;
+	struct hpsa_scsi_dev_t *dev = scmd->device->hostdata;
+
+	/*
+	 * We're overlaying struct hpsa_tmf_struct on top of something which
+	 * was allocated as a struct io_accel2_cmd, so we better be sure it
+	 * actually fits, and doesn't overrun the error info space.
+	 */
+	BUILD_BUG_ON(sizeof(struct hpsa_tmf_struct) >
+			sizeof(struct io_accel2_cmd));
+	BUG_ON(offsetof(struct io_accel2_cmd, error_data) <
+			offsetof(struct hpsa_tmf_struct, error_len) +
+				sizeof(ac->error_len));
+
+	c->cmd_type = IOACCEL2_TMF;
+	c->scsi_cmd = SCSI_CMD_BUSY;
+
+	/* Adjust the DMA address to point to the accelerated command buffer */
+	c->busaddr = (u32) h->ioaccel2_cmd_pool_dhandle +
+				(c->cmdindex * sizeof(struct io_accel2_cmd));
+	BUG_ON(c->busaddr & 0x0000007F);
+
+	memset(ac, 0, sizeof(*c2)); /* yes this is correct */
+	ac->iu_type = IOACCEL2_IU_TMF_TYPE;
+	ac->reply_queue = reply_queue;
+	ac->tmf = IOACCEL2_TMF_ABORT;
+	ac->it_nexus = cpu_to_le32(dev->ioaccel_handle);
+	memset(ac->lun_id, 0, sizeof(ac->lun_id));
+	ac->tag = cpu_to_le64(c->cmdindex << DIRECT_LOOKUP_SHIFT);
+	ac->abort_tag = cpu_to_le64(le32_to_cpu(c2a->Tag));
+	ac->error_ptr = cpu_to_le64(c->busaddr +
+			offsetof(struct io_accel2_cmd, error_data));
+	ac->error_len = cpu_to_le32(sizeof(c2->error_data));
+}
+
 /* ioaccel2 path firmware cannot handle abort task requests.
  * Change abort requests to physical target reset, and send to the
  * address of the physical disk used for the ioaccel 2 command.
@@ -4464,7 +5284,7 @@ static int hpsa_send_abort(struct ctlr_info *h, unsigned char *scsi3addr,
  */
 
 static int hpsa_send_reset_as_abort_ioaccel2(struct ctlr_info *h,
-	unsigned char *scsi3addr, struct CommandList *abort)
+	unsigned char *scsi3addr, struct CommandList *abort, int reply_queue)
 {
 	int rc = IO_OK;
 	struct scsi_cmnd *scmd; /* scsi command within request being aborted */
@@ -4483,8 +5303,9 @@ static int hpsa_send_reset_as_abort_ioaccel2(struct ctlr_info *h,
 
 	if (h->raid_offload_debug > 0)
 		dev_info(&h->pdev->dev,
-			"Reset as abort: Abort requested on C%d:B%d:T%d:L%d scsi3addr 0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
+			"scsi %d:%d:%d:%d %s scsi3addr 0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
 			h->scsi_host->host_no, dev->bus, dev->target, dev->lun,
+			"Reset as abort",
 			scsi3addr[0], scsi3addr[1], scsi3addr[2], scsi3addr[3],
 			scsi3addr[4], scsi3addr[5], scsi3addr[6], scsi3addr[7]);
 
@@ -4506,7 +5327,7 @@ static int hpsa_send_reset_as_abort_ioaccel2(struct ctlr_info *h,
 			"Reset as abort: Resetting physical device at scsi3addr 0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
 			psa[0], psa[1], psa[2], psa[3],
 			psa[4], psa[5], psa[6], psa[7]);
-	rc = hpsa_send_reset(h, psa, HPSA_RESET_TYPE_TARGET);
+	rc = hpsa_do_reset(h, dev, psa, HPSA_RESET_TYPE_TARGET, reply_queue);
 	if (rc != 0) {
 		dev_warn(&h->pdev->dev,
 			"Reset as abort: Failed on physical device at scsi3addr 0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
@@ -4516,7 +5337,7 @@ static int hpsa_send_reset_as_abort_ioaccel2(struct ctlr_info *h,
 	}
 
 	/* wait for device to recover */
-	if (wait_for_device_to_become_ready(h, psa) != 0) {
+	if (wait_for_device_to_become_ready(h, psa, reply_queue) != 0) {
 		dev_warn(&h->pdev->dev,
 			"Reset as abort: Failed: Device never recovered from reset: 0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
 			psa[0], psa[1], psa[2], psa[3],
@@ -4533,25 +5354,94 @@ static int hpsa_send_reset_as_abort_ioaccel2(struct ctlr_info *h,
 	return rc; /* success */
 }
 
-/* Some Smart Arrays need the abort tag swizzled, and some don't.  It's hard to
- * tell which kind we're dealing with, so we send the abort both ways.  There
- * shouldn't be any collisions between swizzled and unswizzled tags due to the
- * way we construct our tags but we check anyway in case the assumptions which
- * make this true someday become false.
- */
+static int hpsa_send_abort_ioaccel2(struct ctlr_info *h,
+	struct CommandList *abort, int reply_queue)
+{
+	int rc = IO_OK;
+	struct CommandList *c;
+	__le32 taglower, tagupper;
+	struct hpsa_scsi_dev_t *dev;
+	struct io_accel2_cmd *c2;
+
+	dev = abort->scsi_cmd->device->hostdata;
+	if (!dev->offload_enabled && !dev->hba_ioaccel_enabled)
+		return -1;
+
+	c = cmd_alloc(h);
+	setup_ioaccel2_abort_cmd(c, h, abort, reply_queue);
+	c2 = &h->ioaccel2_cmd_pool[c->cmdindex];
+	(void) hpsa_scsi_do_simple_cmd(h, c, reply_queue, NO_TIMEOUT);
+	hpsa_get_tag(h, abort, &taglower, &tagupper);
+	dev_dbg(&h->pdev->dev,
+		"%s: Tag:0x%08x:%08x: do_simple_cmd(ioaccel2 abort) completed.\n",
+		__func__, tagupper, taglower);
+	/* no unmap needed here because no data xfer. */
+
+	dev_dbg(&h->pdev->dev,
+		"%s: Tag:0x%08x:%08x: abort service response = 0x%02x.\n",
+		__func__, tagupper, taglower, c2->error_data.serv_response);
+	switch (c2->error_data.serv_response) {
+	case IOACCEL2_SERV_RESPONSE_TMF_COMPLETE:
+	case IOACCEL2_SERV_RESPONSE_TMF_SUCCESS:
+		rc = 0;
+		break;
+	case IOACCEL2_SERV_RESPONSE_TMF_REJECTED:
+	case IOACCEL2_SERV_RESPONSE_FAILURE:
+	case IOACCEL2_SERV_RESPONSE_TMF_WRONG_LUN:
+		rc = -1;
+		break;
+	default:
+		dev_warn(&h->pdev->dev,
+			"%s: Tag:0x%08x:%08x: unknown abort service response 0x%02x\n",
+			__func__, tagupper, taglower,
+			c2->error_data.serv_response);
+		rc = -1;
+	}
+	cmd_free(h, c);
+	dev_dbg(&h->pdev->dev, "%s: Tag:0x%08x:%08x: Finished.\n", __func__,
+		tagupper, taglower);
+	return rc;
+}
+
 static int hpsa_send_abort_both_ways(struct ctlr_info *h,
-	unsigned char *scsi3addr, struct CommandList *abort)
+	unsigned char *scsi3addr, struct CommandList *abort, int reply_queue)
 {
-	/* ioccelerator mode 2 commands should be aborted via the
+	/*
+	 * ioccelerator mode 2 commands should be aborted via the
 	 * accelerated path, since RAID path is unaware of these commands,
-	 * but underlying firmware can't handle abort TMF.
-	 * Change abort to physical device reset.
+	 * but not all underlying firmware can handle abort TMF.
+	 * Change abort to physical device reset when abort TMF is unsupported.
 	 */
-	if (abort->cmd_type == CMD_IOACCEL2)
-		return hpsa_send_reset_as_abort_ioaccel2(h, scsi3addr, abort);
+	if (abort->cmd_type == CMD_IOACCEL2) {
+		if (HPSATMF_IOACCEL_ENABLED & h->TMFSupportFlags)
+			return hpsa_send_abort_ioaccel2(h, abort,
+						reply_queue);
+		else
+			return hpsa_send_reset_as_abort_ioaccel2(h, scsi3addr,
+							abort, reply_queue);
+	}
+	return hpsa_send_abort(h, scsi3addr, abort, reply_queue);
+}
 
-	return hpsa_send_abort(h, scsi3addr, abort, 0) &&
-			hpsa_send_abort(h, scsi3addr, abort, 1);
+/* Find out which reply queue a command was meant to return on */
+static int hpsa_extract_reply_queue(struct ctlr_info *h,
+					struct CommandList *c)
+{
+	if (c->cmd_type == CMD_IOACCEL2)
+		return h->ioaccel2_cmd_pool[c->cmdindex].reply_queue;
+	return c->Header.ReplyQueue;
+}
+
+/*
+ * Limit concurrency of abort commands to prevent
+ * over-subscription of commands
+ */
+static inline int wait_for_available_abort_cmd(struct ctlr_info *h)
+{
+#define ABORT_CMD_WAIT_MSECS 5000
+	return !wait_event_timeout(h->abort_cmd_wait_queue,
+			atomic_dec_if_positive(&h->abort_cmds_available) >= 0,
+			msecs_to_jiffies(ABORT_CMD_WAIT_MSECS));
 }
 
 /* Send an abort for the specified command.
@@ -4561,7 +5451,7 @@ static int hpsa_send_abort_both_ways(struct ctlr_info *h,
 static int hpsa_eh_abort_handler(struct scsi_cmnd *sc)
 {
 
-	int i, rc;
+	int rc;
 	struct ctlr_info *h;
 	struct hpsa_scsi_dev_t *dev;
 	struct CommandList *abort; /* pointer to command to be aborted */
@@ -4569,27 +5459,19 @@ static int hpsa_eh_abort_handler(struct scsi_cmnd *sc)
 	char msg[256];		/* For debug messaging. */
 	int ml = 0;
 	__le32 tagupper, taglower;
-	int refcount;
+	int refcount, reply_queue;
 
-	/* Find the controller of the command to be aborted */
-	h = sdev_to_hba(sc->device);
-	if (WARN(h == NULL,
-			"ABORT REQUEST FAILED, Controller lookup failed.\n"))
+	if (sc == NULL)
 		return FAILED;
 
-	if (lockup_detected(h))
+	if (sc->device == NULL)
 		return FAILED;
 
-	/* Check that controller supports some kind of task abort */
-	if (!(HPSATMF_PHYS_TASK_ABORT & h->TMFSupportFlags) &&
-		!(HPSATMF_LOG_TASK_ABORT & h->TMFSupportFlags))
+	/* Find the controller of the command to be aborted */
+	h = sdev_to_hba(sc->device);
+	if (h == NULL)
 		return FAILED;
 
-	memset(msg, 0, sizeof(msg));
-	ml += sprintf(msg+ml, "ABORT REQUEST on C%d:B%d:T%d:L%llu ",
-		h->scsi_host->host_no, sc->device->channel,
-		sc->device->id, sc->device->lun);
-
 	/* Find the device of the command to be aborted */
 	dev = sc->device->hostdata;
 	if (!dev) {
@@ -4598,6 +5480,31 @@ static int hpsa_eh_abort_handler(struct scsi_cmnd *sc)
 		return FAILED;
 	}
 
+	/* If controller locked up, we can guarantee command won't complete */
+	if (lockup_detected(h)) {
+		hpsa_show_dev_msg(KERN_WARNING, h, dev,
+					"ABORT FAILED, lockup detected");
+		return FAILED;
+	}
+
+	/* This is a good time to check if controller lockup has occurred */
+	if (detect_controller_lockup(h)) {
+		hpsa_show_dev_msg(KERN_WARNING, h, dev,
+					"ABORT FAILED, new lockup detected");
+		return FAILED;
+	}
+
+	/* Check that controller supports some kind of task abort */
+	if (!(HPSATMF_PHYS_TASK_ABORT & h->TMFSupportFlags) &&
+		!(HPSATMF_LOG_TASK_ABORT & h->TMFSupportFlags))
+		return FAILED;
+
+	memset(msg, 0, sizeof(msg));
+	ml += sprintf(msg+ml, "scsi %d:%d:%d:%llu %s %p",
+		h->scsi_host->host_no, sc->device->channel,
+		sc->device->id, sc->device->lun,
+		"Aborting command", sc);
+
 	/* Get SCSI command to be aborted */
 	abort = (struct CommandList *) sc->host_scribble;
 	if (abort == NULL) {
@@ -4609,50 +5516,115 @@ static int hpsa_eh_abort_handler(struct scsi_cmnd *sc)
 		cmd_free(h, abort);
 		return SUCCESS;
 	}
+
+	/* Don't bother trying the abort if we know it won't work. */
+	if (abort->cmd_type != CMD_IOACCEL2 &&
+		abort->cmd_type != CMD_IOACCEL1 && !dev->supports_aborts) {
+		cmd_free(h, abort);
+		return FAILED;
+	}
+
+	/*
+	 * Check that we're aborting the right command.
+	 * It's possible the CommandList already completed and got re-used.
+	 */
+	if (abort->scsi_cmd != sc) {
+		cmd_free(h, abort);
+		return SUCCESS;
+	}
+
+	abort->abort_pending = true;
 	hpsa_get_tag(h, abort, &taglower, &tagupper);
+	reply_queue = hpsa_extract_reply_queue(h, abort);
 	ml += sprintf(msg+ml, "Tag:0x%08x:%08x ", tagupper, taglower);
 	as  = abort->scsi_cmd;
 	if (as != NULL)
-		ml += sprintf(msg+ml, "Command:0x%x SN:0x%lx ",
-			as->cmnd[0], as->serial_number);
-	dev_dbg(&h->pdev->dev, "%s\n", msg);
-	dev_warn(&h->pdev->dev, "Abort request on C%d:B%d:T%d:L%d\n",
-		h->scsi_host->host_no, dev->bus, dev->target, dev->lun);
+		ml += sprintf(msg+ml,
+			"CDBLen: %d CDB: 0x%02x%02x... SN: 0x%lx ",
+			as->cmd_len, as->cmnd[0], as->cmnd[1],
+			as->serial_number);
+	dev_warn(&h->pdev->dev, "%s BEING SENT\n", msg);
+	hpsa_show_dev_msg(KERN_WARNING, h, dev, "Aborting command");
+
 	/*
 	 * Command is in flight, or possibly already completed
 	 * by the firmware (but not to the scsi mid layer) but we can't
 	 * distinguish which.  Send the abort down.
 	 */
-	rc = hpsa_send_abort_both_ways(h, dev->scsi3addr, abort);
+	if (wait_for_available_abort_cmd(h)) {
+		dev_warn(&h->pdev->dev,
+			"%s FAILED, timeout waiting for an abort command to become available.\n",
+			msg);
+		cmd_free(h, abort);
+		return FAILED;
+	}
+	rc = hpsa_send_abort_both_ways(h, dev->scsi3addr, abort, reply_queue);
+	atomic_inc(&h->abort_cmds_available);
+	wake_up_all(&h->abort_cmd_wait_queue);
 	if (rc != 0) {
-		dev_dbg(&h->pdev->dev, "%s Request FAILED.\n", msg);
-		dev_warn(&h->pdev->dev, "FAILED abort on device C%d:B%d:T%d:L%d\n",
-			h->scsi_host->host_no,
-			dev->bus, dev->target, dev->lun);
+		dev_warn(&h->pdev->dev, "%s SENT, FAILED\n", msg);
+		hpsa_show_dev_msg(KERN_WARNING, h, dev,
+				"FAILED to abort command");
 		cmd_free(h, abort);
 		return FAILED;
 	}
-	dev_info(&h->pdev->dev, "%s REQUEST SUCCEEDED.\n", msg);
+	dev_info(&h->pdev->dev, "%s SENT, SUCCESS\n", msg);
+	wait_event(h->event_sync_wait_queue,
+		   abort->scsi_cmd != sc || lockup_detected(h));
+	cmd_free(h, abort);
+	return !lockup_detected(h) ? SUCCESS : FAILED;
+}
 
-	/* If the abort(s) above completed and actually aborted the
-	 * command, then the command to be aborted should already be
-	 * completed.  If not, wait around a bit more to see if they
-	 * manage to complete normally.
-	 */
-#define ABORT_COMPLETE_WAIT_SECS 30
-	for (i = 0; i < ABORT_COMPLETE_WAIT_SECS * 10; i++) {
-		refcount = atomic_read(&abort->refcount);
-		if (refcount < 2) {
-			cmd_free(h, abort);
-			return SUCCESS;
-		} else {
-			msleep(100);
-		}
+/*
+ * For operations with an associated SCSI command, a command block is allocated
+ * at init, and managed by cmd_tagged_alloc() and cmd_tagged_free() using the
+ * block request tag as an index into a table of entries.  cmd_tagged_free() is
+ * the complement, although cmd_free() may be called instead.
+ */
+static struct CommandList *cmd_tagged_alloc(struct ctlr_info *h,
+					    struct scsi_cmnd *scmd)
+{
+	int idx = hpsa_get_cmd_index(scmd);
+	struct CommandList *c = h->cmd_pool + idx;
+
+	if (idx < HPSA_NRESERVED_CMDS || idx >= h->nr_cmds) {
+		dev_err(&h->pdev->dev, "Bad block tag: %d not in [%d..%d]\n",
+			idx, HPSA_NRESERVED_CMDS, h->nr_cmds - 1);
+		/* The index value comes from the block layer, so if it's out of
+		 * bounds, it's probably not our bug.
+		 */
+		BUG();
 	}
-	dev_warn(&h->pdev->dev, "%s FAILED. Aborted command has not completed after %d seconds.\n",
-		msg, ABORT_COMPLETE_WAIT_SECS);
-	cmd_free(h, abort);
-	return FAILED;
+
+	atomic_inc(&c->refcount);
+	if (unlikely(!hpsa_is_cmd_idle(c))) {
+		/*
+		 * We expect that the SCSI layer will hand us a unique tag
+		 * value.  Thus, there should never be a collision here between
+		 * two requests...because if the selected command isn't idle
+		 * then someone is going to be very disappointed.
+		 */
+		dev_err(&h->pdev->dev,
+			"tag collision (tag=%d) in cmd_tagged_alloc().\n",
+			idx);
+		if (c->scsi_cmd != NULL)
+			scsi_print_command(c->scsi_cmd);
+		scsi_print_command(scmd);
+	}
+
+	hpsa_cmd_partial_init(h, idx, c);
+	return c;
+}
+
+static void cmd_tagged_free(struct ctlr_info *h, struct CommandList *c)
+{
+	/*
+	 * Release our reference to the block.  We don't need to do anything
+	 * else to free it, because it is accessed by index.  (There's no point
+	 * in checking the result of the decrement, since we cannot guarantee
+	 * that there isn't a concurrent abort which is also accessing it.)
+	 */
+	(void)atomic_dec(&c->refcount);
 }
 
 /*
@@ -4660,16 +5632,15 @@ static int hpsa_eh_abort_handler(struct scsi_cmnd *sc)
  * and managed by cmd_alloc() and cmd_free() using a simple bitmap to track
  * which ones are free or in use.  Lock must be held when calling this.
  * cmd_free() is the complement.
+ * This function never gives up and returns NULL.  If it hangs,
+ * another thread must call cmd_free() to free some tags.
  */
 
 static struct CommandList *cmd_alloc(struct ctlr_info *h)
 {
 	struct CommandList *c;
-	int i;
-	union u64bit temp64;
-	dma_addr_t cmd_dma_handle, err_dma_handle;
-	int refcount;
-	unsigned long offset;
+	int refcount, i;
+	int offset = 0;
 
 	/*
 	 * There is some *extremely* small but non-zero chance that that
@@ -4681,12 +5652,20 @@ static struct CommandList *cmd_alloc(struct ctlr_info *h)
 	 * very unlucky thread might be starved anyway, never able to
 	 * beat the other threads.  In reality, this happens so
 	 * infrequently as to be indistinguishable from never.
+	 *
+	 * Note that we start allocating commands before the SCSI host structure
+	 * is initialized.  Since the search starts at bit zero, this
+	 * all works, since we have at least one command structure available;
+	 * however, it means that the structures with the low indexes have to be
+	 * reserved for driver-initiated requests, while requests from the block
+	 * layer will use the higher indexes.
 	 */
 
-	offset = h->last_allocation; /* benignly racy */
 	for (;;) {
-		i = find_next_zero_bit(h->cmd_pool_bits, h->nr_cmds, offset);
-		if (unlikely(i == h->nr_cmds)) {
+		i = find_next_zero_bit(h->cmd_pool_bits,
+					HPSA_NRESERVED_CMDS,
+					offset);
+		if (unlikely(i >= HPSA_NRESERVED_CMDS)) {
 			offset = 0;
 			continue;
 		}
@@ -4694,35 +5673,23 @@ static struct CommandList *cmd_alloc(struct ctlr_info *h)
 		refcount = atomic_inc_return(&c->refcount);
 		if (unlikely(refcount > 1)) {
 			cmd_free(h, c); /* already in use */
-			offset = (i + 1) % h->nr_cmds;
+			offset = (i + 1) % HPSA_NRESERVED_CMDS;
 			continue;
 		}
 		set_bit(i & (BITS_PER_LONG - 1),
 			h->cmd_pool_bits + (i / BITS_PER_LONG));
 		break; /* it's ours now. */
 	}
-	h->last_allocation = i; /* benignly racy */
-
-	/* Zero out all of commandlist except the last field, refcount */
-	memset(c, 0, offsetof(struct CommandList, refcount));
-	c->Header.tag = cpu_to_le64((u64) (i << DIRECT_LOOKUP_SHIFT));
-	cmd_dma_handle = h->cmd_pool_dhandle + i * sizeof(*c);
-	c->err_info = h->errinfo_pool + i;
-	memset(c->err_info, 0, sizeof(*c->err_info));
-	err_dma_handle = h->errinfo_pool_dhandle
-	    + i * sizeof(*c->err_info);
-
-	c->cmdindex = i;
-
-	c->busaddr = (u32) cmd_dma_handle;
-	temp64.val = (u64) err_dma_handle;
-	c->ErrDesc.Addr = cpu_to_le64((u64) err_dma_handle);
-	c->ErrDesc.Len = cpu_to_le32((u32) sizeof(*c->err_info));
-
-	c->h = h;
+	hpsa_cmd_partial_init(h, i, c);
 	return c;
 }
 
+/*
+ * This is the complementary operation to cmd_alloc().  Note, however, in some
+ * corner cases it may also be used to free blocks allocated by
+ * cmd_tagged_alloc() in which case the ref-count decrement does the trick and
+ * the clear-bit is harmless.
+ */
 static void cmd_free(struct ctlr_info *h, struct CommandList *c)
 {
 	if (atomic_dec_and_test(&c->refcount)) {
@@ -4900,7 +5867,7 @@ static int hpsa_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 	if (iocommand.buf_size > 0) {
 		buff = kmalloc(iocommand.buf_size, GFP_KERNEL);
 		if (buff == NULL)
-			return -EFAULT;
+			return -ENOMEM;
 		if (iocommand.Request.Type.Direction & XFER_WRITE) {
 			/* Copy the data into the buffer we created */
 			if (copy_from_user(buff, iocommand.buf,
@@ -4913,12 +5880,10 @@ static int hpsa_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 		}
 	}
 	c = cmd_alloc(h);
-	if (c == NULL) {
-		rc = -ENOMEM;
-		goto out_kfree;
-	}
+
 	/* Fill in the command type */
 	c->cmd_type = CMD_IOCTL_PEND;
+	c->scsi_cmd = SCSI_CMD_BUSY;
 	/* Fill in Command Header */
 	c->Header.ReplyQueue = 0; /* unused in simple mode */
 	if (iocommand.buf_size > 0) {	/* buffer to fill */
@@ -4948,10 +5913,14 @@ static int hpsa_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 		c->SG[0].Len = cpu_to_le32(iocommand.buf_size);
 		c->SG[0].Ext = cpu_to_le32(HPSA_SG_LAST); /* not chaining */
 	}
-	hpsa_scsi_do_simple_cmd_core_if_no_lockup(h, c);
+	rc = hpsa_scsi_do_simple_cmd(h, c, DEFAULT_REPLY_QUEUE, NO_TIMEOUT);
 	if (iocommand.buf_size > 0)
 		hpsa_pci_unmap(h->pdev, c, 1, PCI_DMA_BIDIRECTIONAL);
 	check_ioctl_unit_attention(h, c);
+	if (rc) {
+		rc = -EIO;
+		goto out;
+	}
 
 	/* Copy the error information out */
 	memcpy(&iocommand.error_info, c->err_info,
@@ -5048,11 +6017,9 @@ static int hpsa_big_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 		sg_used++;
 	}
 	c = cmd_alloc(h);
-	if (c == NULL) {
-		status = -ENOMEM;
-		goto cleanup1;
-	}
+
 	c->cmd_type = CMD_IOCTL_PEND;
+	c->scsi_cmd = SCSI_CMD_BUSY;
 	c->Header.ReplyQueue = 0;
 	c->Header.SGList = (u8) sg_used;
 	c->Header.SGTotal = cpu_to_le16(sg_used);
@@ -5078,10 +6045,15 @@ static int hpsa_big_passthru_ioctl(struct ctlr_info *h, void __user *argp)
 		}
 		c->SG[--i].Ext = cpu_to_le32(HPSA_SG_LAST);
 	}
-	hpsa_scsi_do_simple_cmd_core_if_no_lockup(h, c);
+	status = hpsa_scsi_do_simple_cmd(h, c, DEFAULT_REPLY_QUEUE, NO_TIMEOUT);
 	if (sg_used)
 		hpsa_pci_unmap(h->pdev, c, sg_used, PCI_DMA_BIDIRECTIONAL);
 	check_ioctl_unit_attention(h, c);
+	if (status) {
+		status = -EIO;
+		goto cleanup0;
+	}
+
 	/* Copy the error information out */
 	memcpy(&ioc->error_info, c->err_info, sizeof(ioc->error_info));
 	if (copy_to_user(argp, ioc, sizeof(*ioc))) {
@@ -5163,14 +6135,13 @@ static int hpsa_ioctl(struct scsi_device *dev, int cmd, void __user *arg)
 	}
 }
 
-static int hpsa_send_host_reset(struct ctlr_info *h, unsigned char *scsi3addr,
+static void hpsa_send_host_reset(struct ctlr_info *h, unsigned char *scsi3addr,
 				u8 reset_type)
 {
 	struct CommandList *c;
 
 	c = cmd_alloc(h);
-	if (!c)
-		return -ENOMEM;
+
 	/* fill_cmd can't fail here, no data buffer to map */
 	(void) fill_cmd(c, HPSA_DEVICE_RESET_MSG, h, NULL, 0, 0,
 		RAID_CTLR_LUNID, TYPE_MSG);
@@ -5181,7 +6152,7 @@ static int hpsa_send_host_reset(struct ctlr_info *h, unsigned char *scsi3addr,
 	 * the command either.  This is the last command we will send before
 	 * re-initializing everything, so it doesn't matter and won't leak.
 	 */
-	return 0;
+	return;
 }
 
 static int fill_cmd(struct CommandList *c, u8 cmd, struct ctlr_info *h,
@@ -5189,9 +6160,10 @@ static int fill_cmd(struct CommandList *c, u8 cmd, struct ctlr_info *h,
 	int cmd_type)
 {
 	int pci_dir = XFER_NONE;
-	struct CommandList *a; /* for commands to be aborted */
+	u64 tag; /* for commands to be aborted */
 
 	c->cmd_type = CMD_IOCTL_PEND;
+	c->scsi_cmd = SCSI_CMD_BUSY;
 	c->Header.ReplyQueue = 0;
 	if (buff != NULL && size > 0) {
 		c->Header.SGList = 1;
@@ -5305,10 +6277,10 @@ static int fill_cmd(struct CommandList *c, u8 cmd, struct ctlr_info *h,
 			c->Request.CDB[7] = 0x00;
 			break;
 		case  HPSA_ABORT_MSG:
-			a = buff;       /* point to command to be aborted */
+			memcpy(&tag, buff, sizeof(tag));
 			dev_dbg(&h->pdev->dev,
-				"Abort Tag:0x%016llx request Tag:0x%016llx",
-				a->Header.tag, c->Header.tag);
+				"Abort Tag:0x%016llx using rqst Tag:0x%016llx",
+				tag, c->Header.tag);
 			c->Request.CDBLen = 16;
 			c->Request.type_attr_dir =
 					TYPE_ATTR_DIR(cmd_type,
@@ -5319,8 +6291,7 @@ static int fill_cmd(struct CommandList *c, u8 cmd, struct ctlr_info *h,
 			c->Request.CDB[2] = 0x00; /* reserved */
 			c->Request.CDB[3] = 0x00; /* reserved */
 			/* Tag to abort goes in CDB[4]-CDB[11] */
-			memcpy(&c->Request.CDB[4], &a->Header.tag,
-				sizeof(a->Header.tag));
+			memcpy(&c->Request.CDB[4], &tag, sizeof(tag));
 			c->Request.CDB[12] = 0x00; /* reserved */
 			c->Request.CDB[13] = 0x00; /* reserved */
 			c->Request.CDB[14] = 0x00; /* reserved */
@@ -5399,7 +6370,7 @@ static inline void finish_cmd(struct CommandList *c)
 	if (likely(c->cmd_type == CMD_IOACCEL1 || c->cmd_type == CMD_SCSI
 			|| c->cmd_type == CMD_IOACCEL2))
 		complete_scsi_command(c);
-	else if (c->cmd_type == CMD_IOCTL_PEND)
+	else if (c->cmd_type == CMD_IOCTL_PEND || c->cmd_type == IOACCEL2_TMF)
 		complete(c->waiting);
 }
 
@@ -5733,7 +6704,7 @@ static int controller_reset_failed(struct CfgTable __iomem *cfgtable)
 /* This does a hard reset of the controller using PCI power management
  * states or the using the doorbell register.
  */
-static int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
+static int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev, u32 board_id)
 {
 	u64 cfg_offset;
 	u32 cfg_base_addr;
@@ -5744,7 +6715,6 @@ static int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
 	int rc;
 	struct CfgTable __iomem *cfgtable;
 	u32 use_doorbell;
-	u32 board_id;
 	u16 command_register;
 
 	/* For controllers as old as the P600, this is very nearly
@@ -5760,11 +6730,6 @@ static int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
 	 * using the doorbell register.
 	 */
 
-	rc = hpsa_lookup_board_id(pdev, &board_id);
-	if (rc < 0) {
-		dev_warn(&pdev->dev, "Board ID not found\n");
-		return rc;
-	}
 	if (!ctlr_is_resettable(board_id)) {
 		dev_warn(&pdev->dev, "Controller not resettable\n");
 		return -ENODEV;
@@ -5930,10 +6895,22 @@ static int find_PCI_BAR_index(struct pci_dev *pdev, unsigned long pci_bar_addr)
 	return -1;
 }
 
+static void hpsa_disable_interrupt_mode(struct ctlr_info *h)
+{
+	if (h->msix_vector) {
+		if (h->pdev->msix_enabled)
+			pci_disable_msix(h->pdev);
+		h->msix_vector = 0;
+	} else if (h->msi_vector) {
+		if (h->pdev->msi_enabled)
+			pci_disable_msi(h->pdev);
+		h->msi_vector = 0;
+	}
+}
+
 /* If MSI/MSI-X is supported by the kernel we will try to enable it on
  * controllers that are capable. If not, we use legacy INTx mode.
  */
-
 static void hpsa_interrupt_mode(struct ctlr_info *h)
 {
 #ifdef CONFIG_PCI_MSI
@@ -6064,6 +7041,21 @@ static int hpsa_find_cfg_addrs(struct pci_dev *pdev, void __iomem *vaddr,
 	return 0;
 }
 
+static void hpsa_free_cfgtables(struct ctlr_info *h)
+{
+	if (h->transtable) {
+		iounmap(h->transtable);
+		h->transtable = NULL;
+	}
+	if (h->cfgtable) {
+		iounmap(h->cfgtable);
+		h->cfgtable = NULL;
+	}
+}
+
+/* Find and map CISS config table and transfer table
++ * several items must be unmapped (freed) later
++ * */
 static int hpsa_find_cfgtables(struct ctlr_info *h)
 {
 	u64 cfg_offset;
@@ -6090,25 +7082,31 @@ static int hpsa_find_cfgtables(struct ctlr_info *h)
 	h->transtable = remap_pci_mem(pci_resource_start(h->pdev,
 				cfg_base_addr_index)+cfg_offset+trans_offset,
 				sizeof(*h->transtable));
-	if (!h->transtable)
+	if (!h->transtable) {
+		dev_err(&h->pdev->dev, "Failed mapping transfer table\n");
+		hpsa_free_cfgtables(h);
 		return -ENOMEM;
+	}
 	return 0;
 }
 
 static void hpsa_get_max_perf_mode_cmds(struct ctlr_info *h)
 {
-	h->max_commands = readl(&(h->cfgtable->MaxPerformantModeCommands));
+#define MIN_MAX_COMMANDS 16
+	BUILD_BUG_ON(MIN_MAX_COMMANDS <= HPSA_NRESERVED_CMDS);
+
+	h->max_commands = readl(&h->cfgtable->MaxPerformantModeCommands);
 
 	/* Limit commands in memory limited kdump scenario. */
 	if (reset_devices && h->max_commands > 32)
 		h->max_commands = 32;
 
-	if (h->max_commands < 16) {
-		dev_warn(&h->pdev->dev, "Controller reports "
-			"max supported commands of %d, an obvious lie. "
-			"Using 16.  Ensure that firmware is up to date.\n",
-			h->max_commands);
-		h->max_commands = 16;
+	if (h->max_commands < MIN_MAX_COMMANDS) {
+		dev_warn(&h->pdev->dev,
+			"Controller reports max supported commands of %d Using %d instead. Ensure that firmware is up to date.\n",
+			h->max_commands,
+			MIN_MAX_COMMANDS);
+		h->max_commands = MIN_MAX_COMMANDS;
 	}
 }
 
@@ -6153,6 +7151,8 @@ static void hpsa_find_board_params(struct ctlr_info *h)
 		dev_warn(&h->pdev->dev, "Physical aborts not supported\n");
 	if (!(HPSATMF_LOG_TASK_ABORT & h->TMFSupportFlags))
 		dev_warn(&h->pdev->dev, "Logical aborts not supported\n");
+	if (!(HPSATMF_IOACCEL_ENABLED & h->TMFSupportFlags))
+		dev_warn(&h->pdev->dev, "HP SSD Smart Path aborts not supported\n");
 }
 
 static inline bool hpsa_CISS_signature_present(struct ctlr_info *h)
@@ -6222,6 +7222,8 @@ static int hpsa_wait_for_mode_change_ack(struct ctlr_info *h)
 	 * as we enter this code.)
 	 */
 	for (i = 0; i < MAX_MODE_CHANGE_WAIT; i++) {
+		if (h->remove_in_progress)
+			goto done;
 		spin_lock_irqsave(&h->lock, flags);
 		doorbell_value = readl(h->vaddr + SA5_DOORBELL);
 		spin_unlock_irqrestore(&h->lock, flags);
@@ -6262,6 +7264,22 @@ error:
 	return -ENODEV;
 }
 
+/* free items allocated or mapped by hpsa_pci_init */
+static void hpsa_free_pci_init(struct ctlr_info *h)
+{
+	hpsa_free_cfgtables(h);			/* pci_init 4 */
+	iounmap(h->vaddr);			/* pci_init 3 */
+	h->vaddr = NULL;
+	hpsa_disable_interrupt_mode(h);		/* pci_init 2 */
+	/*
+	 * call pci_disable_device before pci_release_regions per
+	 * Documentation/PCI/pci.txt
+	 */
+	pci_disable_device(h->pdev);		/* pci_init 1 */
+	pci_release_regions(h->pdev);		/* pci_init 2 */
+}
+
+/* several items must be freed later */
 static int hpsa_pci_init(struct ctlr_info *h)
 {
 	int prod_index, err;
@@ -6272,19 +7290,24 @@ static int hpsa_pci_init(struct ctlr_info *h)
 	h->product_name = products[prod_index].product_name;
 	h->access = *(products[prod_index].access);
 
+	h->needs_abort_tags_swizzled =
+		ctlr_needs_abort_tags_swizzled(h->board_id);
+
 	pci_disable_link_state(h->pdev, PCIE_LINK_STATE_L0S |
 			       PCIE_LINK_STATE_L1 | PCIE_LINK_STATE_CLKPM);
 
 	err = pci_enable_device(h->pdev);
 	if (err) {
-		dev_warn(&h->pdev->dev, "unable to enable PCI device\n");
+		dev_err(&h->pdev->dev, "failed to enable PCI device\n");
+		pci_disable_device(h->pdev);
 		return err;
 	}
 
 	err = pci_request_regions(h->pdev, HPSA);
 	if (err) {
 		dev_err(&h->pdev->dev,
-			"cannot obtain PCI resources, aborting\n");
+			"failed to obtain PCI resources\n");
+		pci_disable_device(h->pdev);
 		return err;
 	}
 
@@ -6293,38 +7316,43 @@ static int hpsa_pci_init(struct ctlr_info *h)
 	hpsa_interrupt_mode(h);
 	err = hpsa_pci_find_memory_BAR(h->pdev, &h->paddr);
 	if (err)
-		goto err_out_free_res;
+		goto clean2;	/* intmode+region, pci */
 	h->vaddr = remap_pci_mem(h->paddr, 0x250);
 	if (!h->vaddr) {
+		dev_err(&h->pdev->dev, "failed to remap PCI mem\n");
 		err = -ENOMEM;
-		goto err_out_free_res;
+		goto clean2;	/* intmode+region, pci */
 	}
 	err = hpsa_wait_for_board_state(h->pdev, h->vaddr, BOARD_READY);
 	if (err)
-		goto err_out_free_res;
+		goto clean3;	/* vaddr, intmode+region, pci */
 	err = hpsa_find_cfgtables(h);
 	if (err)
-		goto err_out_free_res;
+		goto clean3;	/* vaddr, intmode+region, pci */
 	hpsa_find_board_params(h);
 
 	if (!hpsa_CISS_signature_present(h)) {
 		err = -ENODEV;
-		goto err_out_free_res;
+		goto clean4;	/* cfgtables, vaddr, intmode+region, pci */
 	}
 	hpsa_set_driver_support_bits(h);
 	hpsa_p600_dma_prefetch_quirk(h);
 	err = hpsa_enter_simple_mode(h);
 	if (err)
-		goto err_out_free_res;
+		goto clean4;	/* cfgtables, vaddr, intmode+region, pci */
 	return 0;
 
-err_out_free_res:
-	if (h->transtable)
-		iounmap(h->transtable);
-	if (h->cfgtable)
-		iounmap(h->cfgtable);
-	if (h->vaddr)
-		iounmap(h->vaddr);
+clean4:	/* cfgtables, vaddr, intmode+region, pci */
+	hpsa_free_cfgtables(h);
+clean3:	/* vaddr, intmode+region, pci */
+	iounmap(h->vaddr);
+	h->vaddr = NULL;
+clean2:	/* intmode+region, pci */
+	hpsa_disable_interrupt_mode(h);
+	/*
+	 * call pci_disable_device before pci_release_regions per
+	 * Documentation/PCI/pci.txt
+	 */
 	pci_disable_device(h->pdev);
 	pci_release_regions(h->pdev);
 	return err;
@@ -6346,7 +7374,7 @@ static void hpsa_hba_inquiry(struct ctlr_info *h)
 	}
 }
 
-static int hpsa_init_reset_devices(struct pci_dev *pdev)
+static int hpsa_init_reset_devices(struct pci_dev *pdev, u32 board_id)
 {
 	int rc, i;
 	void __iomem *vaddr;
@@ -6382,7 +7410,7 @@ static int hpsa_init_reset_devices(struct pci_dev *pdev)
 	iounmap(vaddr);
 
 	/* Reset the controller with a PCI power-cycle or via doorbell */
-	rc = hpsa_kdump_hard_reset_controller(pdev);
+	rc = hpsa_kdump_hard_reset_controller(pdev, board_id);
 
 	/* -ENOTSUPP here means we cannot reset the controller
 	 * but it's already (and still) up and running in
@@ -6408,7 +7436,29 @@ out_disable:
 	return rc;
 }
 
-static int hpsa_allocate_cmd_pool(struct ctlr_info *h)
+static void hpsa_free_cmd_pool(struct ctlr_info *h)
+{
+	kfree(h->cmd_pool_bits);
+	h->cmd_pool_bits = NULL;
+	if (h->cmd_pool) {
+		pci_free_consistent(h->pdev,
+				h->nr_cmds * sizeof(struct CommandList),
+				h->cmd_pool,
+				h->cmd_pool_dhandle);
+		h->cmd_pool = NULL;
+		h->cmd_pool_dhandle = 0;
+	}
+	if (h->errinfo_pool) {
+		pci_free_consistent(h->pdev,
+				h->nr_cmds * sizeof(struct ErrorInfo),
+				h->errinfo_pool,
+				h->errinfo_pool_dhandle);
+		h->errinfo_pool = NULL;
+		h->errinfo_pool_dhandle = 0;
+	}
+}
+
+static int hpsa_alloc_cmd_pool(struct ctlr_info *h)
 {
 	h->cmd_pool_bits = kzalloc(
 		DIV_ROUND_UP(h->nr_cmds, BITS_PER_LONG) *
@@ -6425,34 +7475,13 @@ static int hpsa_allocate_cmd_pool(struct ctlr_info *h)
 		dev_err(&h->pdev->dev, "out of memory in %s", __func__);
 		goto clean_up;
 	}
+	hpsa_preinitialize_commands(h);
 	return 0;
 clean_up:
 	hpsa_free_cmd_pool(h);
 	return -ENOMEM;
 }
 
-static void hpsa_free_cmd_pool(struct ctlr_info *h)
-{
-	kfree(h->cmd_pool_bits);
-	if (h->cmd_pool)
-		pci_free_consistent(h->pdev,
-			    h->nr_cmds * sizeof(struct CommandList),
-			    h->cmd_pool, h->cmd_pool_dhandle);
-	if (h->ioaccel2_cmd_pool)
-		pci_free_consistent(h->pdev,
-			h->nr_cmds * sizeof(*h->ioaccel2_cmd_pool),
-			h->ioaccel2_cmd_pool, h->ioaccel2_cmd_pool_dhandle);
-	if (h->errinfo_pool)
-		pci_free_consistent(h->pdev,
-			    h->nr_cmds * sizeof(struct ErrorInfo),
-			    h->errinfo_pool,
-			    h->errinfo_pool_dhandle);
-	if (h->ioaccel_cmd_pool)
-		pci_free_consistent(h->pdev,
-			h->nr_cmds * sizeof(struct io_accel1_cmd),
-			h->ioaccel_cmd_pool, h->ioaccel_cmd_pool_dhandle);
-}
-
 static void hpsa_irq_affinity_hints(struct ctlr_info *h)
 {
 	int i, cpu;
@@ -6474,12 +7503,14 @@ static void hpsa_free_irqs(struct ctlr_info *h)
 		i = h->intr_mode;
 		irq_set_affinity_hint(h->intr[i], NULL);
 		free_irq(h->intr[i], &h->q[i]);
+		h->q[i] = 0;
 		return;
 	}
 
 	for (i = 0; i < h->msix_vector; i++) {
 		irq_set_affinity_hint(h->intr[i], NULL);
 		free_irq(h->intr[i], &h->q[i]);
+		h->q[i] = 0;
 	}
 	for (; i < MAX_REPLY_QUEUES; i++)
 		h->q[i] = 0;
@@ -6502,8 +7533,9 @@ static int hpsa_request_irqs(struct ctlr_info *h,
 	if (h->intr_mode == PERF_MODE_INT && h->msix_vector > 0) {
 		/* If performant mode and MSI-X, use multiple reply queues */
 		for (i = 0; i < h->msix_vector; i++) {
+			sprintf(h->intrname[i], "%s-msix%d", h->devname, i);
 			rc = request_irq(h->intr[i], msixhandler,
-					0, h->devname,
+					0, h->intrname[i],
 					&h->q[i]);
 			if (rc) {
 				int j;
@@ -6524,18 +7556,30 @@ static int hpsa_request_irqs(struct ctlr_info *h,
 	} else {
 		/* Use single reply pool */
 		if (h->msix_vector > 0 || h->msi_vector) {
+			if (h->msix_vector)
+				sprintf(h->intrname[h->intr_mode],
+					"%s-msix", h->devname);
+			else
+				sprintf(h->intrname[h->intr_mode],
+					"%s-msi", h->devname);
 			rc = request_irq(h->intr[h->intr_mode],
-				msixhandler, 0, h->devname,
+				msixhandler, 0,
+				h->intrname[h->intr_mode],
 				&h->q[h->intr_mode]);
 		} else {
+			sprintf(h->intrname[h->intr_mode],
+				"%s-intx", h->devname);
 			rc = request_irq(h->intr[h->intr_mode],
-				intxhandler, IRQF_SHARED, h->devname,
+				intxhandler, IRQF_SHARED,
+				h->intrname[h->intr_mode],
 				&h->q[h->intr_mode]);
 		}
+		irq_set_affinity_hint(h->intr[h->intr_mode], NULL);
 	}
 	if (rc) {
-		dev_err(&h->pdev->dev, "unable to get irq %d for %s\n",
+		dev_err(&h->pdev->dev, "failed to get irq %d for %s\n",
 		       h->intr[h->intr_mode], h->devname);
+		hpsa_free_irqs(h);
 		return -ENODEV;
 	}
 	return 0;
@@ -6543,42 +7587,27 @@ static int hpsa_request_irqs(struct ctlr_info *h,
 
 static int hpsa_kdump_soft_reset(struct ctlr_info *h)
 {
-	if (hpsa_send_host_reset(h, RAID_CTLR_LUNID,
-		HPSA_RESET_TYPE_CONTROLLER)) {
-		dev_warn(&h->pdev->dev, "Resetting array controller failed.\n");
-		return -EIO;
-	}
+	int rc;
+	hpsa_send_host_reset(h, RAID_CTLR_LUNID, HPSA_RESET_TYPE_CONTROLLER);
 
 	dev_info(&h->pdev->dev, "Waiting for board to soft reset.\n");
-	if (hpsa_wait_for_board_state(h->pdev, h->vaddr, BOARD_NOT_READY)) {
+	rc = hpsa_wait_for_board_state(h->pdev, h->vaddr, BOARD_NOT_READY);
+	if (rc) {
 		dev_warn(&h->pdev->dev, "Soft reset had no effect.\n");
-		return -1;
+		return rc;
 	}
 
 	dev_info(&h->pdev->dev, "Board reset, awaiting READY status.\n");
-	if (hpsa_wait_for_board_state(h->pdev, h->vaddr, BOARD_READY)) {
+	rc = hpsa_wait_for_board_state(h->pdev, h->vaddr, BOARD_READY);
+	if (rc) {
 		dev_warn(&h->pdev->dev, "Board failed to become ready "
 			"after soft reset.\n");
-		return -1;
+		return rc;
 	}
 
 	return 0;
 }
 
-static void hpsa_free_irqs_and_disable_msix(struct ctlr_info *h)
-{
-	hpsa_free_irqs(h);
-#ifdef CONFIG_PCI_MSI
-	if (h->msix_vector) {
-		if (h->pdev->msix_enabled)
-			pci_disable_msix(h->pdev);
-	} else if (h->msi_vector) {
-		if (h->pdev->msi_enabled)
-			pci_disable_msi(h->pdev);
-	}
-#endif /* CONFIG_PCI_MSI */
-}
-
 static void hpsa_free_reply_queues(struct ctlr_info *h)
 {
 	int i;
@@ -6586,30 +7615,36 @@ static void hpsa_free_reply_queues(struct ctlr_info *h)
 	for (i = 0; i < h->nreply_queues; i++) {
 		if (!h->reply_queue[i].head)
 			continue;
-		pci_free_consistent(h->pdev, h->reply_queue_size,
-			h->reply_queue[i].head, h->reply_queue[i].busaddr);
+		pci_free_consistent(h->pdev,
+					h->reply_queue_size,
+					h->reply_queue[i].head,
+					h->reply_queue[i].busaddr);
 		h->reply_queue[i].head = NULL;
 		h->reply_queue[i].busaddr = 0;
 	}
+	h->reply_queue_size = 0;
 }
 
 static void hpsa_undo_allocations_after_kdump_soft_reset(struct ctlr_info *h)
 {
-	hpsa_free_irqs_and_disable_msix(h);
-	hpsa_free_sg_chain_blocks(h);
-	hpsa_free_cmd_pool(h);
-	kfree(h->ioaccel1_blockFetchTable);
-	kfree(h->blockFetchTable);
-	hpsa_free_reply_queues(h);
-	if (h->vaddr)
-		iounmap(h->vaddr);
-	if (h->transtable)
-		iounmap(h->transtable);
-	if (h->cfgtable)
-		iounmap(h->cfgtable);
-	pci_disable_device(h->pdev);
-	pci_release_regions(h->pdev);
-	kfree(h);
+	hpsa_free_performant_mode(h);		/* init_one 7 */
+	hpsa_free_sg_chain_blocks(h);		/* init_one 6 */
+	hpsa_free_cmd_pool(h);			/* init_one 5 */
+	hpsa_free_irqs(h);			/* init_one 4 */
+	scsi_host_put(h->scsi_host);		/* init_one 3 */
+	h->scsi_host = NULL;			/* init_one 3 */
+	hpsa_free_pci_init(h);			/* init_one 2_5 */
+	free_percpu(h->lockup_detected);	/* init_one 2 */
+	h->lockup_detected = NULL;		/* init_one 2 */
+	if (h->resubmit_wq) {
+		destroy_workqueue(h->resubmit_wq);	/* init_one 1 */
+		h->resubmit_wq = NULL;
+	}
+	if (h->rescan_ctlr_wq) {
+		destroy_workqueue(h->rescan_ctlr_wq);
+		h->rescan_ctlr_wq = NULL;
+	}
+	kfree(h);				/* init_one 1 */
 }
 
 /* Called when controller lockup detected. */
@@ -6617,17 +7652,22 @@ static void fail_all_outstanding_cmds(struct ctlr_info *h)
 {
 	int i, refcount;
 	struct CommandList *c;
+	int failcount = 0;
 
 	flush_workqueue(h->resubmit_wq); /* ensure all cmds are fully built */
 	for (i = 0; i < h->nr_cmds; i++) {
 		c = h->cmd_pool + i;
 		refcount = atomic_inc_return(&c->refcount);
 		if (refcount > 1) {
-			c->err_info->CommandStatus = CMD_HARDWARE_ERR;
+			c->err_info->CommandStatus = CMD_CTLR_LOCKUP;
 			finish_cmd(c);
+			atomic_dec(&h->commands_outstanding);
+			failcount++;
 		}
 		cmd_free(h, c);
 	}
+	dev_warn(&h->pdev->dev,
+		"failed %d commands in fail_all\n", failcount);
 }
 
 static void set_lockup_detected_for_all_cpus(struct ctlr_info *h, u32 value)
@@ -6653,18 +7693,19 @@ static void controller_lockup_detected(struct ctlr_info *h)
 	if (!lockup_detected) {
 		/* no heartbeat, but controller gave us a zero. */
 		dev_warn(&h->pdev->dev,
-			"lockup detected but scratchpad register is zero\n");
+			"lockup detected after %d but scratchpad register is zero\n",
+			h->heartbeat_sample_interval / HZ);
 		lockup_detected = 0xffffffff;
 	}
 	set_lockup_detected_for_all_cpus(h, lockup_detected);
 	spin_unlock_irqrestore(&h->lock, flags);
-	dev_warn(&h->pdev->dev, "Controller lockup detected: 0x%08x\n",
-			lockup_detected);
+	dev_warn(&h->pdev->dev, "Controller lockup detected: 0x%08x after %d\n",
+			lockup_detected, h->heartbeat_sample_interval / HZ);
 	pci_disable_device(h->pdev);
 	fail_all_outstanding_cmds(h);
 }
 
-static void detect_controller_lockup(struct ctlr_info *h)
+static int detect_controller_lockup(struct ctlr_info *h)
 {
 	u64 now;
 	u32 heartbeat;
@@ -6674,7 +7715,7 @@ static void detect_controller_lockup(struct ctlr_info *h)
 	/* If we've received an interrupt recently, we're ok. */
 	if (time_after64(h->last_intr_timestamp +
 				(h->heartbeat_sample_interval), now))
-		return;
+		return false;
 
 	/*
 	 * If we've already checked the heartbeat recently, we're ok.
@@ -6683,7 +7724,7 @@ static void detect_controller_lockup(struct ctlr_info *h)
 	 */
 	if (time_after64(h->last_heartbeat_timestamp +
 				(h->heartbeat_sample_interval), now))
-		return;
+		return false;
 
 	/* If heartbeat has not changed since we last looked, we're not ok. */
 	spin_lock_irqsave(&h->lock, flags);
@@ -6691,12 +7732,13 @@ static void detect_controller_lockup(struct ctlr_info *h)
 	spin_unlock_irqrestore(&h->lock, flags);
 	if (h->last_heartbeat == heartbeat) {
 		controller_lockup_detected(h);
-		return;
+		return true;
 	}
 
 	/* We're ok. */
 	h->last_heartbeat = heartbeat;
 	h->last_heartbeat_timestamp = now;
+	return false;
 }
 
 static void hpsa_ack_ctlr_events(struct ctlr_info *h)
@@ -6843,11 +7885,18 @@ static int hpsa_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct ctlr_info *h;
 	int try_soft_reset = 0;
 	unsigned long flags;
+	u32 board_id;
 
 	if (number_of_controllers == 0)
 		printk(KERN_INFO DRIVER_NAME "\n");
 
-	rc = hpsa_init_reset_devices(pdev);
+	rc = hpsa_lookup_board_id(pdev, &board_id);
+	if (rc < 0) {
+		dev_warn(&pdev->dev, "Board ID not found\n");
+		return rc;
+	}
+
+	rc = hpsa_init_reset_devices(pdev, board_id);
 	if (rc) {
 		if (rc != -ENOTSUPP)
 			return rc;
@@ -6868,42 +7917,41 @@ reinit_after_soft_reset:
 	 */
 	BUILD_BUG_ON(sizeof(struct CommandList) % COMMANDLIST_ALIGNMENT);
 	h = kzalloc(sizeof(*h), GFP_KERNEL);
-	if (!h)
+	if (!h) {
+		dev_err(&pdev->dev, "Failed to allocate controller head\n");
 		return -ENOMEM;
+	}
 
 	h->pdev = pdev;
+
 	h->intr_mode = hpsa_simple_mode ? SIMPLE_MODE_INT : PERF_MODE_INT;
 	INIT_LIST_HEAD(&h->offline_device_list);
 	spin_lock_init(&h->lock);
 	spin_lock_init(&h->offline_device_lock);
 	spin_lock_init(&h->scan_lock);
 	atomic_set(&h->passthru_cmds_avail, HPSA_MAX_CONCURRENT_PASSTHRUS);
-
-	h->rescan_ctlr_wq = hpsa_create_controller_wq(h, "rescan");
-	if (!h->rescan_ctlr_wq) {
-		rc = -ENOMEM;
-		goto clean1;
-	}
-
-	h->resubmit_wq = hpsa_create_controller_wq(h, "resubmit");
-	if (!h->resubmit_wq) {
-		rc = -ENOMEM;
-		goto clean1;
-	}
+	atomic_set(&h->abort_cmds_available, HPSA_CMDS_RESERVED_FOR_ABORTS);
 
 	/* Allocate and clear per-cpu variable lockup_detected */
 	h->lockup_detected = alloc_percpu(u32);
 	if (!h->lockup_detected) {
+		dev_err(&h->pdev->dev, "Failed to allocate lockup detector\n");
 		rc = -ENOMEM;
-		goto clean1;
+		goto clean1;	/* aer/h */
 	}
 	set_lockup_detected_for_all_cpus(h, 0);
 
 	rc = hpsa_pci_init(h);
-	if (rc != 0)
-		goto clean1;
+	if (rc)
+		goto clean2;	/* lu, aer/h */
+
+	/* relies on h-> settings made by hpsa_pci_init, including
+	 * interrupt_mode h->intr */
+	rc = hpsa_scsi_host_alloc(h);
+	if (rc)
+		goto clean2_5;	/* pci, lu, aer/h */
 
-	sprintf(h->devname, HPSA "%d", number_of_controllers);
+	sprintf(h->devname, HPSA "%d", h->scsi_host->host_no);
 	h->ctlr = number_of_controllers;
 	number_of_controllers++;
 
@@ -6917,34 +7965,57 @@ reinit_after_soft_reset:
 			dac = 0;
 		} else {
 			dev_err(&pdev->dev, "no suitable DMA available\n");
-			goto clean1;
+			goto clean3;	/* shost, pci, lu, aer/h */
 		}
 	}
 
 	/* make sure the board interrupts are off */
 	h->access.set_intr_mask(h, HPSA_INTR_OFF);
 
-	if (hpsa_request_irqs(h, do_hpsa_intr_msi, do_hpsa_intr_intx))
-		goto clean2;
-	dev_info(&pdev->dev, "%s: <0x%x> at IRQ %d%s using DAC\n",
-	       h->devname, pdev->device,
-	       h->intr[h->intr_mode], dac ? "" : " not");
-	rc = hpsa_allocate_cmd_pool(h);
+	rc = hpsa_request_irqs(h, do_hpsa_intr_msi, do_hpsa_intr_intx);
+	if (rc)
+		goto clean3;	/* shost, pci, lu, aer/h */
+	rc = hpsa_alloc_cmd_pool(h);
 	if (rc)
-		goto clean2_and_free_irqs;
-	if (hpsa_allocate_sg_chain_blocks(h))
-		goto clean4;
+		goto clean4;	/* irq, shost, pci, lu, aer/h */
+	rc = hpsa_alloc_sg_chain_blocks(h);
+	if (rc)
+		goto clean5;	/* cmd, irq, shost, pci, lu, aer/h */
 	init_waitqueue_head(&h->scan_wait_queue);
+	init_waitqueue_head(&h->abort_cmd_wait_queue);
+	init_waitqueue_head(&h->event_sync_wait_queue);
+	mutex_init(&h->reset_mutex);
 	h->scan_finished = 1; /* no scan currently in progress */
 
 	pci_set_drvdata(pdev, h);
 	h->ndevices = 0;
 	h->hba_mode_enabled = 0;
-	h->scsi_host = NULL;
+
 	spin_lock_init(&h->devlock);
-	hpsa_put_ctlr_into_performant_mode(h);
+	rc = hpsa_put_ctlr_into_performant_mode(h);
+	if (rc)
+		goto clean6; /* sg, cmd, irq, shost, pci, lu, aer/h */
+
+	/* hook into SCSI subsystem */
+	rc = hpsa_scsi_add_host(h);
+	if (rc)
+		goto clean7; /* perf, sg, cmd, irq, shost, pci, lu, aer/h */
+
+	/* create the resubmit workqueue */
+	h->rescan_ctlr_wq = hpsa_create_controller_wq(h, "rescan");
+	if (!h->rescan_ctlr_wq) {
+		rc = -ENOMEM;
+		goto clean7;
+	}
 
-	/* At this point, the controller is ready to take commands.
+	h->resubmit_wq = hpsa_create_controller_wq(h, "resubmit");
+	if (!h->resubmit_wq) {
+		rc = -ENOMEM;
+		goto clean7;	/* aer/h */
+	}
+
+	/*
+	 * At this point, the controller is ready to take commands.
 	 * Now, if reset_devices and the hard reset didn't work, try
 	 * the soft reset and see if that works.
 	 */
@@ -6966,13 +8037,24 @@ reinit_after_soft_reset:
 		if (rc) {
 			dev_warn(&h->pdev->dev,
 				"Failed to request_irq after soft reset.\n");
-			goto clean4;
+			/*
+			 * cannot goto clean7 or free_irqs will be called
+			 * again. Instead, do its work
+			 */
+			hpsa_free_performant_mode(h);	/* clean7 */
+			hpsa_free_sg_chain_blocks(h);	/* clean6 */
+			hpsa_free_cmd_pool(h);		/* clean5 */
+			/*
+			 * skip hpsa_free_irqs(h) clean4 since that
+			 * was just called before request_irqs failed
+			 */
+			goto clean3;
 		}
 
 		rc = hpsa_kdump_soft_reset(h);
 		if (rc)
 			/* Neither hard nor soft reset worked, we're hosed. */
-			goto clean4;
+			goto clean9;
 
 		dev_info(&h->pdev->dev, "Board READY.\n");
 		dev_info(&h->pdev->dev,
@@ -6993,21 +8075,20 @@ reinit_after_soft_reset:
 		hpsa_undo_allocations_after_kdump_soft_reset(h);
 		try_soft_reset = 0;
 		if (rc)
-			/* don't go to clean4, we already unallocated */
+			/* don't goto clean, we already unallocated */
 			return -ENODEV;
 
 		goto reinit_after_soft_reset;
 	}
 
-		/* Enable Accelerated IO path at driver layer */
-		h->acciopath_status = 1;
+	/* Enable Accelerated IO path at driver layer */
+	h->acciopath_status = 1;
 
 
 	/* Turn the interrupts on so we can service requests */
 	h->access.set_intr_mask(h, HPSA_INTR_ON);
 
 	hpsa_hba_inquiry(h);
-	hpsa_register_scsi(h);	/* hook ourselves into SCSI subsystem */
 
 	/* Monitor the controller for firmware lockups */
 	h->heartbeat_sample_interval = HEARTBEAT_SAMPLE_INTERVAL;
@@ -7019,19 +8100,36 @@ reinit_after_soft_reset:
 				h->heartbeat_sample_interval);
 	return 0;
 
-clean4:
+clean9: /* wq, sh, perf, sg, cmd, irq, shost, pci, lu, aer/h */
+	kfree(h->hba_inquiry_data);
+clean7: /* perf, sg, cmd, irq, shost, pci, lu, aer/h */
+	hpsa_free_performant_mode(h);
+	h->access.set_intr_mask(h, HPSA_INTR_OFF);
+clean6: /* sg, cmd, irq, pci, lockup, wq/aer/h */
 	hpsa_free_sg_chain_blocks(h);
+clean5: /* cmd, irq, shost, pci, lu, aer/h */
 	hpsa_free_cmd_pool(h);
-clean2_and_free_irqs:
+clean4: /* irq, shost, pci, lu, aer/h */
 	hpsa_free_irqs(h);
-clean2:
-clean1:
-	if (h->resubmit_wq)
+clean3: /* shost, pci, lu, aer/h */
+	scsi_host_put(h->scsi_host);
+	h->scsi_host = NULL;
+clean2_5: /* pci, lu, aer/h */
+	hpsa_free_pci_init(h);
+clean2: /* lu, aer/h */
+	if (h->lockup_detected) {
+		free_percpu(h->lockup_detected);
+		h->lockup_detected = NULL;
+	}
+clean1:	/* wq/aer/h */
+	if (h->resubmit_wq) {
 		destroy_workqueue(h->resubmit_wq);
-	if (h->rescan_ctlr_wq)
+		h->resubmit_wq = NULL;
+	}
+	if (h->rescan_ctlr_wq) {
 		destroy_workqueue(h->rescan_ctlr_wq);
-	if (h->lockup_detected)
-		free_percpu(h->lockup_detected);
+		h->rescan_ctlr_wq = NULL;
+	}
 	kfree(h);
 	return rc;
 }
@@ -7040,8 +8138,8 @@ static void hpsa_flush_cache(struct ctlr_info *h)
 {
 	char *flush_buf;
 	struct CommandList *c;
+	int rc;
 
-	/* Don't bother trying to flush the cache if locked up */
 	if (unlikely(lockup_detected(h)))
 		return;
 	flush_buf = kzalloc(4, GFP_KERNEL);
@@ -7049,21 +8147,20 @@ static void hpsa_flush_cache(struct ctlr_info *h)
 		return;
 
 	c = cmd_alloc(h);
-	if (!c) {
-		dev_warn(&h->pdev->dev, "cmd_alloc returned NULL!\n");
-		goto out_of_memory;
-	}
+
 	if (fill_cmd(c, HPSA_CACHE_FLUSH, h, flush_buf, 4, 0,
 		RAID_CTLR_LUNID, TYPE_CMD)) {
 		goto out;
 	}
-	hpsa_scsi_do_simple_cmd_with_retry(h, c, PCI_DMA_TODEVICE);
+	rc = hpsa_scsi_do_simple_cmd_with_retry(h, c,
+					PCI_DMA_TODEVICE, NO_TIMEOUT);
+	if (rc)
+		goto out;
 	if (c->err_info->CommandStatus != 0)
 out:
 		dev_warn(&h->pdev->dev,
 			"error flushing cache on controller\n");
 	cmd_free(h, c);
-out_of_memory:
 	kfree(flush_buf);
 }
 
@@ -7078,15 +8175,18 @@ static void hpsa_shutdown(struct pci_dev *pdev)
 	 */
 	hpsa_flush_cache(h);
 	h->access.set_intr_mask(h, HPSA_INTR_OFF);
-	hpsa_free_irqs_and_disable_msix(h);
+	hpsa_free_irqs(h);			/* init_one 4 */
+	hpsa_disable_interrupt_mode(h);		/* pci_init 2 */
 }
 
 static void hpsa_free_device_info(struct ctlr_info *h)
 {
 	int i;
 
-	for (i = 0; i < h->ndevices; i++)
+	for (i = 0; i < h->ndevices; i++) {
 		kfree(h->dev[i]);
+		h->dev[i] = NULL;
+	}
 }
 
 static void hpsa_remove_one(struct pci_dev *pdev)
@@ -7108,29 +8208,34 @@ static void hpsa_remove_one(struct pci_dev *pdev)
 	cancel_delayed_work_sync(&h->rescan_ctlr_work);
 	destroy_workqueue(h->rescan_ctlr_wq);
 	destroy_workqueue(h->resubmit_wq);
-	hpsa_unregister_scsi(h);	/* unhook from SCSI subsystem */
+
+	/* includes hpsa_free_irqs - init_one 4 */
+	/* includes hpsa_disable_interrupt_mode - pci_init 2 */
 	hpsa_shutdown(pdev);
-	iounmap(h->vaddr);
-	iounmap(h->transtable);
-	iounmap(h->cfgtable);
-	hpsa_free_device_info(h);
-	hpsa_free_sg_chain_blocks(h);
-	pci_free_consistent(h->pdev,
-		h->nr_cmds * sizeof(struct CommandList),
-		h->cmd_pool, h->cmd_pool_dhandle);
-	pci_free_consistent(h->pdev,
-		h->nr_cmds * sizeof(struct ErrorInfo),
-		h->errinfo_pool, h->errinfo_pool_dhandle);
-	hpsa_free_reply_queues(h);
-	kfree(h->cmd_pool_bits);
-	kfree(h->blockFetchTable);
-	kfree(h->ioaccel1_blockFetchTable);
-	kfree(h->ioaccel2_blockFetchTable);
-	kfree(h->hba_inquiry_data);
-	pci_disable_device(pdev);
-	pci_release_regions(pdev);
-	free_percpu(h->lockup_detected);
-	kfree(h);
+
+	hpsa_free_device_info(h);		/* scan */
+
+	kfree(h->hba_inquiry_data);			/* init_one 10 */
+	h->hba_inquiry_data = NULL;			/* init_one 10 */
+	if (h->scsi_host)
+		scsi_remove_host(h->scsi_host);		/* init_one 8 */
+	hpsa_free_ioaccel2_sg_chain_blocks(h);
+	hpsa_free_performant_mode(h);			/* init_one 7 */
+	hpsa_free_sg_chain_blocks(h);			/* init_one 6 */
+	hpsa_free_cmd_pool(h);				/* init_one 5 */
+
+	/* hpsa_free_irqs already called via hpsa_shutdown init_one 4 */
+
+	scsi_host_put(h->scsi_host);			/* init_one 3 */
+	h->scsi_host = NULL;				/* init_one 3 */
+
+	/* includes hpsa_disable_interrupt_mode - pci_init 2 */
+	hpsa_free_pci_init(h);				/* init_one 2.5 */
+
+	free_percpu(h->lockup_detected);		/* init_one 2 */
+	h->lockup_detected = NULL;			/* init_one 2 */
+	/* (void) pci_disable_pcie_error_reporting(pdev); */	/* init_one 1 */
+	kfree(h);					/* init_one 1 */
 }
 
 static int hpsa_suspend(__attribute__((unused)) struct pci_dev *pdev,
@@ -7188,7 +8293,10 @@ static void  calc_bucket_map(int bucket[], int num_buckets,
 	}
 }
 
-/* return -ENODEV or other reason on error, 0 on success */
+/*
+ * return -ENODEV on err, 0 on success (or no action)
+ * allocates numerous items that must be freed later
+ */
 static int hpsa_enter_performant_mode(struct ctlr_info *h, u32 trans_support)
 {
 	int i;
@@ -7370,7 +8478,23 @@ static int hpsa_enter_performant_mode(struct ctlr_info *h, u32 trans_support)
 	return 0;
 }
 
-static int hpsa_alloc_ioaccel_cmd_and_bft(struct ctlr_info *h)
+/* Free ioaccel1 mode command blocks and block fetch table */
+static void hpsa_free_ioaccel1_cmd_and_bft(struct ctlr_info *h)
+{
+	if (h->ioaccel_cmd_pool) {
+		pci_free_consistent(h->pdev,
+			h->nr_cmds * sizeof(*h->ioaccel_cmd_pool),
+			h->ioaccel_cmd_pool,
+			h->ioaccel_cmd_pool_dhandle);
+		h->ioaccel_cmd_pool = NULL;
+		h->ioaccel_cmd_pool_dhandle = 0;
+	}
+	kfree(h->ioaccel1_blockFetchTable);
+	h->ioaccel1_blockFetchTable = NULL;
+}
+
+/* Allocate ioaccel1 mode command blocks and block fetch table */
+static int hpsa_alloc_ioaccel1_cmd_and_bft(struct ctlr_info *h)
 {
 	h->ioaccel_maxsg =
 		readl(&(h->cfgtable->io_accel_max_embedded_sg_count));
@@ -7401,16 +8525,32 @@ static int hpsa_alloc_ioaccel_cmd_and_bft(struct ctlr_info *h)
 	return 0;
 
 clean_up:
-	if (h->ioaccel_cmd_pool)
+	hpsa_free_ioaccel1_cmd_and_bft(h);
+	return -ENOMEM;
+}
+
+/* Free ioaccel2 mode command blocks and block fetch table */
+static void hpsa_free_ioaccel2_cmd_and_bft(struct ctlr_info *h)
+{
+	hpsa_free_ioaccel2_sg_chain_blocks(h);
+
+	if (h->ioaccel2_cmd_pool) {
 		pci_free_consistent(h->pdev,
-			h->nr_cmds * sizeof(*h->ioaccel_cmd_pool),
-			h->ioaccel_cmd_pool, h->ioaccel_cmd_pool_dhandle);
-	kfree(h->ioaccel1_blockFetchTable);
-	return 1;
+			h->nr_cmds * sizeof(*h->ioaccel2_cmd_pool),
+			h->ioaccel2_cmd_pool,
+			h->ioaccel2_cmd_pool_dhandle);
+		h->ioaccel2_cmd_pool = NULL;
+		h->ioaccel2_cmd_pool_dhandle = 0;
+	}
+	kfree(h->ioaccel2_blockFetchTable);
+	h->ioaccel2_blockFetchTable = NULL;
 }
 
-static int ioaccel2_alloc_cmds_and_bft(struct ctlr_info *h)
+/* Allocate ioaccel2 mode command blocks and block fetch table */
+static int hpsa_alloc_ioaccel2_cmd_and_bft(struct ctlr_info *h)
 {
+	int rc;
+
 	/* Allocate ioaccel2 mode command blocks and block fetch table */
 
 	h->ioaccel_maxsg =
@@ -7430,7 +8570,13 @@ static int ioaccel2_alloc_cmds_and_bft(struct ctlr_info *h)
 				sizeof(u32)), GFP_KERNEL);
 
 	if ((h->ioaccel2_cmd_pool == NULL) ||
-		(h->ioaccel2_blockFetchTable == NULL))
+		(h->ioaccel2_blockFetchTable == NULL)) {
+		rc = -ENOMEM;
+		goto clean_up;
+	}
+
+	rc = hpsa_allocate_ioaccel2_sg_chain_blocks(h);
+	if (rc)
 		goto clean_up;
 
 	memset(h->ioaccel2_cmd_pool, 0,
@@ -7438,41 +8584,50 @@ static int ioaccel2_alloc_cmds_and_bft(struct ctlr_info *h)
 	return 0;
 
 clean_up:
-	if (h->ioaccel2_cmd_pool)
-		pci_free_consistent(h->pdev,
-			h->nr_cmds * sizeof(*h->ioaccel2_cmd_pool),
-			h->ioaccel2_cmd_pool, h->ioaccel2_cmd_pool_dhandle);
-	kfree(h->ioaccel2_blockFetchTable);
-	return 1;
+	hpsa_free_ioaccel2_cmd_and_bft(h);
+	return rc;
 }
 
-static void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
+/* Free items allocated by hpsa_put_ctlr_into_performant_mode */
+static void hpsa_free_performant_mode(struct ctlr_info *h)
+{
+	kfree(h->blockFetchTable);
+	h->blockFetchTable = NULL;
+	hpsa_free_reply_queues(h);
+	hpsa_free_ioaccel1_cmd_and_bft(h);
+	hpsa_free_ioaccel2_cmd_and_bft(h);
+}
+
+/* return -ENODEV on error, 0 on success (or no action)
+ * allocates numerous items that must be freed later
+ */
+static int hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 {
 	u32 trans_support;
 	unsigned long transMethod = CFGTBL_Trans_Performant |
 					CFGTBL_Trans_use_short_tags;
-	int i;
+	int i, rc;
 
 	if (hpsa_simple_mode)
-		return;
+		return 0;
 
 	trans_support = readl(&(h->cfgtable->TransportSupport));
 	if (!(trans_support & PERFORMANT_MODE))
-		return;
+		return 0;
 
 	/* Check for I/O accelerator mode support */
 	if (trans_support & CFGTBL_Trans_io_accel1) {
 		transMethod |= CFGTBL_Trans_io_accel1 |
 				CFGTBL_Trans_enable_directed_msix;
-		if (hpsa_alloc_ioaccel_cmd_and_bft(h))
-			goto clean_up;
-	} else {
-		if (trans_support & CFGTBL_Trans_io_accel2) {
-				transMethod |= CFGTBL_Trans_io_accel2 |
+		rc = hpsa_alloc_ioaccel1_cmd_and_bft(h);
+		if (rc)
+			return rc;
+	} else if (trans_support & CFGTBL_Trans_io_accel2) {
+		transMethod |= CFGTBL_Trans_io_accel2 |
 				CFGTBL_Trans_enable_directed_msix;
-		if (ioaccel2_alloc_cmds_and_bft(h))
-			goto clean_up;
-		}
+		rc = hpsa_alloc_ioaccel2_cmd_and_bft(h);
+		if (rc)
+			return rc;
 	}
 
 	h->nreply_queues = h->msix_vector > 0 ? h->msix_vector : 1;
@@ -7484,8 +8639,10 @@ static void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 		h->reply_queue[i].head = pci_alloc_consistent(h->pdev,
 						h->reply_queue_size,
 						&(h->reply_queue[i].busaddr));
-		if (!h->reply_queue[i].head)
-			goto clean_up;
+		if (!h->reply_queue[i].head) {
+			rc = -ENOMEM;
+			goto clean1;	/* rq, ioaccel */
+		}
 		h->reply_queue[i].size = h->max_commands;
 		h->reply_queue[i].wraparound = 1;  /* spec: init to 1 */
 		h->reply_queue[i].current_entry = 0;
@@ -7494,15 +8651,24 @@ static void hpsa_put_ctlr_into_performant_mode(struct ctlr_info *h)
 	/* Need a block fetch table for performant mode */
 	h->blockFetchTable = kmalloc(((SG_ENTRIES_IN_CMD + 1) *
 				sizeof(u32)), GFP_KERNEL);
-	if (!h->blockFetchTable)
-		goto clean_up;
+	if (!h->blockFetchTable) {
+		rc = -ENOMEM;
+		goto clean1;	/* rq, ioaccel */
+	}
 
-	hpsa_enter_performant_mode(h, trans_support);
-	return;
+	rc = hpsa_enter_performant_mode(h, trans_support);
+	if (rc)
+		goto clean2;	/* bft, rq, ioaccel */
+	return 0;
 
-clean_up:
-	hpsa_free_reply_queues(h);
+clean2:	/* bft, rq, ioaccel */
 	kfree(h->blockFetchTable);
+	h->blockFetchTable = NULL;
+clean1:	/* rq, ioaccel */
+	hpsa_free_reply_queues(h);
+	hpsa_free_ioaccel1_cmd_and_bft(h);
+	hpsa_free_ioaccel2_cmd_and_bft(h);
+	return rc;
 }
 
 static int is_accelerated_cmd(struct CommandList *c)
diff --git a/drivers/scsi/hpsa.h b/drivers/scsi/hpsa.h
index 657713050349..6ee4da6b1153 100644
--- a/drivers/scsi/hpsa.h
+++ b/drivers/scsi/hpsa.h
@@ -47,6 +47,7 @@ struct hpsa_scsi_dev_t {
 	unsigned char raid_level;	/* from inquiry page 0xC1 */
 	unsigned char volume_offline;	/* discovered via TUR or VPD */
 	u16 queue_depth;		/* max queue_depth for this device */
+	atomic_t reset_cmds_out;	/* Count of commands to-be affected */
 	atomic_t ioaccel_cmds_out;	/* Only used for physical devices
 					 * counts commands sent to physical
 					 * device via "ioaccel" path.
@@ -54,6 +55,8 @@ struct hpsa_scsi_dev_t {
 	u32 ioaccel_handle;
 	int offload_config;		/* I/O accel RAID offload configured */
 	int offload_enabled;		/* I/O accel RAID offload enabled */
+	int offload_to_be_enabled;
+	int hba_ioaccel_enabled;
 	int offload_to_mirror;		/* Send next I/O accelerator RAID
 					 * offload request to mirror drive
 					 */
@@ -68,6 +71,13 @@ struct hpsa_scsi_dev_t {
 	 * devices in order to honor physical device queue depth limits.
 	 */
 	struct hpsa_scsi_dev_t *phys_disk[RAID_MAP_MAX_ENTRIES];
+	int nphysical_disks;
+	int supports_aborts;
+#define HPSA_DO_NOT_EXPOSE	0x0
+#define HPSA_SG_ATTACH		0x1
+#define HPSA_ULD_ATTACH		0x2
+#define HPSA_SCSI_ADD		(HPSA_SG_ATTACH | HPSA_ULD_ATTACH)
+	u8 expose_state;
 };
 
 struct reply_queue_buffer {
@@ -133,7 +143,6 @@ struct ctlr_info {
 	struct CfgTable __iomem *cfgtable;
 	int	interrupts_enabled;
 	int 	max_commands;
-	int last_allocation;
 	atomic_t commands_outstanding;
 #	define PERF_MODE_INT	0
 #	define DOORBELL_INT	1
@@ -154,6 +163,7 @@ struct ctlr_info {
 	u8 max_cmd_sg_entries;
 	int chainsize;
 	struct SGDescriptor **cmd_sg_list;
+	struct ioaccel2_sg_element **ioaccel2_cmd_sg_list;
 
 	/* pointers to command and error info pool */
 	struct CommandList 	*cmd_pool;
@@ -211,6 +221,7 @@ struct ctlr_info {
 	int remove_in_progress;
 	/* Address of h->q[x] is passed to intr handler to know which queue */
 	u8 q[MAX_REPLY_QUEUES];
+	char intrname[MAX_REPLY_QUEUES][16];	/* "hpsa0-msix00" names */
 	u32 TMFSupportFlags; /* cache what task mgmt funcs are supported. */
 #define HPSATMF_BITS_SUPPORTED  (1 << 0)
 #define HPSATMF_PHYS_LUN_RESET  (1 << 1)
@@ -222,6 +233,7 @@ struct ctlr_info {
 #define HPSATMF_PHYS_QRY_TASK   (1 << 7)
 #define HPSATMF_PHYS_QRY_TSET   (1 << 8)
 #define HPSATMF_PHYS_QRY_ASYNC  (1 << 9)
+#define HPSATMF_IOACCEL_ENABLED (1 << 15)
 #define HPSATMF_MASK_SUPPORTED  (1 << 16)
 #define HPSATMF_LOG_LUN_RESET   (1 << 17)
 #define HPSATMF_LOG_NEX_RESET   (1 << 18)
@@ -251,8 +263,13 @@ struct ctlr_info {
 	struct list_head offline_device_list;
 	int	acciopath_status;
 	int	raid_offload_debug;
+	int	needs_abort_tags_swizzled;
 	struct workqueue_struct *resubmit_wq;
 	struct workqueue_struct *rescan_ctlr_wq;
+	atomic_t abort_cmds_available;
+	wait_queue_head_t abort_cmd_wait_queue;
+	wait_queue_head_t event_sync_wait_queue;
+	struct mutex reset_mutex;
 };
 
 struct offline_device_entry {
diff --git a/drivers/scsi/hpsa_cmd.h b/drivers/scsi/hpsa_cmd.h
index 3a621c74b76f..c601622cc98e 100644
--- a/drivers/scsi/hpsa_cmd.h
+++ b/drivers/scsi/hpsa_cmd.h
@@ -42,8 +42,22 @@
 #define CMD_UNSOLICITED_ABORT   0x000A
 #define CMD_TIMEOUT             0x000B
 #define CMD_UNABORTABLE		0x000C
+#define CMD_TMF_STATUS		0x000D
 #define CMD_IOACCEL_DISABLED	0x000E
+#define CMD_CTLR_LOCKUP		0xffff
+/* Note: CMD_CTLR_LOCKUP is not a value defined by the CISS spec
+ * it is a value defined by the driver that commands can be marked
+ * with when a controller lockup has been detected by the driver
+ */
 
+/* TMF function status values */
+#define CISS_TMF_COMPLETE	0x00
+#define CISS_TMF_INVALID_FRAME	0x02
+#define CISS_TMF_NOT_SUPPORTED	0x04
+#define CISS_TMF_FAILED		0x05
+#define CISS_TMF_SUCCESS	0x08
+#define CISS_TMF_WRONG_LUN	0x09
+#define CISS_TMF_OVERLAPPED_TAG 0x0a
 
 /* Unit Attentions ASC's as defined for the MSA2012sa */
 #define POWER_OR_RESET			0x29
@@ -240,6 +254,7 @@ struct ReportLUNdata {
 
 struct ext_report_lun_entry {
 	u8 lunid[8];
+#define MASKED_DEVICE(x) ((x)[3] & 0xC0)
 #define GET_BMIC_BUS(lunid) ((lunid)[7] & 0x3F)
 #define GET_BMIC_LEVEL_TWO_TARGET(lunid) ((lunid)[6])
 #define GET_BMIC_DRIVE_NUMBER(lunid) (((GET_BMIC_BUS((lunid)) - 1) << 8) + \
@@ -247,6 +262,8 @@ struct ext_report_lun_entry {
 	u8 wwid[8];
 	u8 device_type;
 	u8 device_flags;
+#define NON_DISK_PHYS_DEV(x) ((x)[17] & 0x01)
+#define PHYS_IOACCEL(x) ((x)[17] & 0x08)
 	u8 lun_count; /* multi-lun device, how many luns */
 	u8 redundant_paths;
 	u32 ioaccel_handle; /* ioaccel1 only uses lower 16 bits */
@@ -379,6 +396,7 @@ struct ErrorInfo {
 #define CMD_SCSI	0x03
 #define CMD_IOACCEL1	0x04
 #define CMD_IOACCEL2	0x05
+#define IOACCEL2_TMF	0x06
 
 #define DIRECT_LOOKUP_SHIFT 4
 #define DIRECT_LOOKUP_MASK (~((1 << DIRECT_LOOKUP_SHIFT) - 1))
@@ -421,7 +439,10 @@ struct CommandList {
 	 * not used.
 	 */
 	struct hpsa_scsi_dev_t *phys_disk;
-	atomic_t refcount; /* Must be last to avoid memset in cmd_alloc */
+
+	int abort_pending;
+	struct hpsa_scsi_dev_t *reset_pending;
+	atomic_t refcount; /* Must be last to avoid memset in hpsa_cmd_init() */
 } __aligned(COMMANDLIST_ALIGNMENT);
 
 /* Max S/G elements in I/O accelerator command */
@@ -515,6 +536,12 @@ struct io_accel2_scsi_response {
 #define IOACCEL2_STATUS_SR_TASK_COMP_SET_FULL	0x28
 #define IOACCEL2_STATUS_SR_TASK_COMP_ABORTED	0x40
 #define IOACCEL2_STATUS_SR_IOACCEL_DISABLED	0x0E
+#define IOACCEL2_STATUS_SR_IO_ERROR		0x01
+#define IOACCEL2_STATUS_SR_IO_ABORTED		0x02
+#define IOACCEL2_STATUS_SR_NO_PATH_TO_DEVICE	0x03
+#define IOACCEL2_STATUS_SR_INVALID_DEVICE	0x04
+#define IOACCEL2_STATUS_SR_UNDERRUN		0x51
+#define IOACCEL2_STATUS_SR_OVERRUN		0x75
 	u8 data_present;		/* low 2 bits */
 #define IOACCEL2_NO_DATAPRESENT		0x000
 #define IOACCEL2_RESPONSE_DATAPRESENT	0x001
@@ -567,6 +594,7 @@ struct io_accel2_cmd {
 #define IOACCEL2_DIR_NO_DATA	0x00
 #define IOACCEL2_DIR_DATA_IN	0x01
 #define IOACCEL2_DIR_DATA_OUT	0x02
+#define IOACCEL2_TMF_ABORT	0x01
 /*
  * SCSI Task Management Request format for Accelerator Mode 2
  */
@@ -575,13 +603,13 @@ struct hpsa_tmf_struct {
 	u8 reply_queue;		/* Reply Queue ID */
 	u8 tmf;			/* Task Management Function */
 	u8 reserved1;		/* byte 3 Reserved */
-	u32 it_nexus;		/* SCSI I-T Nexus */
+	__le32 it_nexus;	/* SCSI I-T Nexus */
 	u8 lun_id[8];		/* LUN ID for TMF request */
 	__le64 tag;		/* cciss tag associated w/ request */
 	__le64 abort_tag;	/* cciss tag of SCSI cmd or TMF to abort */
 	__le64 error_ptr;		/* Error Pointer */
 	__le32 error_len;		/* Error Length */
-};
+} __aligned(IOACCEL2_COMMANDLIST_ALIGNMENT);
 
 /* Configuration Table Structure */
 struct HostWrite {
diff --git a/drivers/scsi/ibmvscsi/ibmvscsi.c b/drivers/scsi/ibmvscsi/ibmvscsi.c
index acea5d6eebd0..6a41c36b16b0 100644
--- a/drivers/scsi/ibmvscsi/ibmvscsi.c
+++ b/drivers/scsi/ibmvscsi/ibmvscsi.c
@@ -1053,7 +1053,7 @@ static int ibmvscsi_queuecommand_lck(struct scsi_cmnd *cmnd,
 	memset(srp_cmd, 0x00, SRP_MAX_IU_LEN);
 	srp_cmd->opcode = SRP_CMD;
 	memcpy(srp_cmd->cdb, cmnd->cmnd, sizeof(srp_cmd->cdb));
-	srp_cmd->lun = cpu_to_be64(((u64)lun) << 48);
+	int_to_scsilun(lun, &srp_cmd->lun);
 
 	if (!map_data_for_srp_cmd(cmnd, evt_struct, srp_cmd, hostdata->dev)) {
 		if (!firmware_has_feature(FW_FEATURE_CMO))
@@ -1529,7 +1529,7 @@ static int ibmvscsi_eh_abort_handler(struct scsi_cmnd *cmd)
 		/* Set up an abort SRP command */
 		memset(tsk_mgmt, 0x00, sizeof(*tsk_mgmt));
 		tsk_mgmt->opcode = SRP_TSK_MGMT;
-		tsk_mgmt->lun = cpu_to_be64(((u64) lun) << 48);
+		int_to_scsilun(lun, &tsk_mgmt->lun);
 		tsk_mgmt->tsk_mgmt_func = SRP_TSK_ABORT_TASK;
 		tsk_mgmt->task_tag = (u64) found_evt;
 
@@ -1652,7 +1652,7 @@ static int ibmvscsi_eh_device_reset_handler(struct scsi_cmnd *cmd)
 		/* Set up a lun reset SRP command */
 		memset(tsk_mgmt, 0x00, sizeof(*tsk_mgmt));
 		tsk_mgmt->opcode = SRP_TSK_MGMT;
-		tsk_mgmt->lun = cpu_to_be64(((u64) lun) << 48);
+		int_to_scsilun(lun, &tsk_mgmt->lun);
 		tsk_mgmt->tsk_mgmt_func = SRP_TSK_LUN_RESET;
 
 		evt->sync_srp = &srp_rsp;
diff --git a/drivers/scsi/imm.c b/drivers/scsi/imm.c
index 89a8266560d0..4e1a632ccf16 100644
--- a/drivers/scsi/imm.c
+++ b/drivers/scsi/imm.c
@@ -1109,7 +1109,6 @@ static struct scsi_host_template imm_template = {
 	.bios_param		= imm_biosparam,
 	.this_id		= 7,
 	.sg_tablesize		= SG_ALL,
-	.cmd_per_lun		= 1,
 	.use_clustering		= ENABLE_CLUSTERING,
 	.can_queue		= 1,
 	.slave_alloc		= imm_adjust_queue,
diff --git a/drivers/scsi/initio.c b/drivers/scsi/initio.c
index e5dae7b54d9a..6a926bae76b2 100644
--- a/drivers/scsi/initio.c
+++ b/drivers/scsi/initio.c
@@ -2833,7 +2833,6 @@ static struct scsi_host_template initio_template = {
 	.can_queue		= MAX_TARGETS * i91u_MAXQUEUE,
 	.this_id		= 1,
 	.sg_tablesize		= SG_ALL,
-	.cmd_per_lun		= 1,
 	.use_clustering		= ENABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/ipr.h b/drivers/scsi/ipr.h
index 47412cf4eaac..73790a1d0969 100644
--- a/drivers/scsi/ipr.h
+++ b/drivers/scsi/ipr.h
@@ -272,7 +272,7 @@
 #define IPR_RUNTIME_RESET				0x40000000
 
 #define IPR_IPL_INIT_MIN_STAGE_TIME			5
-#define IPR_IPL_INIT_DEFAULT_STAGE_TIME                 15
+#define IPR_IPL_INIT_DEFAULT_STAGE_TIME                 30
 #define IPR_IPL_INIT_STAGE_UNKNOWN			0x0
 #define IPR_IPL_INIT_STAGE_TRANSOP			0xB0000000
 #define IPR_IPL_INIT_STAGE_MASK				0xff000000
diff --git a/drivers/scsi/ips.c b/drivers/scsi/ips.c
index 7542f11d3fcd..02cb76fd4420 100644
--- a/drivers/scsi/ips.c
+++ b/drivers/scsi/ips.c
@@ -206,10 +206,6 @@ module_param(ips, charp, 0);
 #define IPS_VERSION_HIGH        IPS_VER_MAJOR_STRING "." IPS_VER_MINOR_STRING
 #define IPS_VERSION_LOW         "." IPS_VER_BUILD_STRING " "
 
-#if !defined(__i386__) && !defined(__ia64__) && !defined(__x86_64__)
-#warning "This driver has only been tested on the x86/ia64/x86_64 platforms"
-#endif
-
 #define IPS_DMA_DIR(scb) ((!scb->scsi_cmd || ips_is_passthru(scb->scsi_cmd) || \
                          DMA_NONE == scb->scsi_cmd->sc_data_direction) ? \
                          PCI_DMA_BIDIRECTIONAL : \
@@ -6788,6 +6784,11 @@ ips_remove_device(struct pci_dev *pci_dev)
 static int __init
 ips_module_init(void)
 {
+#if !defined(__i386__) && !defined(__ia64__) && !defined(__x86_64__)
+	printk(KERN_ERR "ips: This driver has only been tested on the x86/ia64/x86_64 platforms\n");
+	add_taint(TAINT_CPU_OUT_OF_SPEC, LOCKDEP_STILL_OK);
+#endif
+
 	if (pci_register_driver(&ips_pci_driver) < 0)
 		return -ENODEV;
 	ips_driver_template.module = THIS_MODULE;
diff --git a/drivers/scsi/isci/init.c b/drivers/scsi/isci/init.c
index cd41b63a2f10..0dfcabe3ca7c 100644
--- a/drivers/scsi/isci/init.c
+++ b/drivers/scsi/isci/init.c
@@ -160,7 +160,6 @@ static struct scsi_host_template isci_sht = {
 	.change_queue_depth		= sas_change_queue_depth,
 	.bios_param			= sas_bios_param,
 	.can_queue			= ISCI_CAN_QUEUE_VAL,
-	.cmd_per_lun			= 1,
 	.this_id			= -1,
 	.sg_tablesize			= SG_ALL,
 	.max_sectors			= SCSI_DEFAULT_MAX_SECTORS,
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index 9b81a34d7449..a5a56fa31e70 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -230,6 +230,8 @@ struct lpfc_stats {
 	uint32_t elsRcvRRQ;
 	uint32_t elsRcvRTV;
 	uint32_t elsRcvECHO;
+	uint32_t elsRcvLCB;
+	uint32_t elsRcvRDP;
 	uint32_t elsXmitFLOGI;
 	uint32_t elsXmitFDISC;
 	uint32_t elsXmitPLOGI;
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index 587e3e962f2b..b0e6fe46448d 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -498,3 +498,5 @@ bool lpfc_disable_oas_lun(struct lpfc_hba *, struct lpfc_name *,
 bool lpfc_find_next_oas_lun(struct lpfc_hba *, struct lpfc_name *,
 			    struct lpfc_name *, uint64_t *, struct lpfc_name *,
 			    struct lpfc_name *, uint64_t *, uint32_t *);
+int lpfc_sli4_dump_page_a0(struct lpfc_hba *phba, struct lpfcMboxq *mbox);
+void lpfc_mbx_cmpl_rdp_page_a0(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb);
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.c b/drivers/scsi/lpfc/lpfc_debugfs.c
index 513edcb0c2da..25aa9b98d53a 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.c
+++ b/drivers/scsi/lpfc/lpfc_debugfs.c
@@ -710,7 +710,7 @@ lpfc_debugfs_slow_ring_trc(struct lpfc_hba *phba, char *fmt,
  * returns a pointer to that log in the private_data field in @file.
  *
  * Returns:
- * This function returns zero if successful. On error it will return an negative
+ * This function returns zero if successful. On error it will return a negative
  * error value.
  **/
 static int
@@ -760,7 +760,7 @@ out:
  * returns a pointer to that log in the private_data field in @file.
  *
  * Returns:
- * This function returns zero if successful. On error it will return an negative
+ * This function returns zero if successful. On error it will return a negative
  * error value.
  **/
 static int
@@ -810,7 +810,7 @@ out:
  * returns a pointer to that log in the private_data field in @file.
  *
  * Returns:
- * This function returns zero if successful. On error it will return an negative
+ * This function returns zero if successful. On error it will return a negative
  * error value.
  **/
 static int
@@ -852,7 +852,7 @@ out:
  * returns a pointer to that log in the private_data field in @file.
  *
  * Returns:
- * This function returns zero if successful. On error it will return an negative
+ * This function returns zero if successful. On error it will return a negative
  * error value.
  **/
 static int
@@ -894,7 +894,7 @@ out:
  * returns a pointer to that log in the private_data field in @file.
  *
  * Returns:
- * This function returns zero if successful. On error it will return an negative
+ * This function returns zero if successful. On error it will return a negative
  * error value.
  **/
 static int
@@ -1115,7 +1115,7 @@ lpfc_debugfs_dif_err_release(struct inode *inode, struct file *file)
  * returns a pointer to that log in the private_data field in @file.
  *
  * Returns:
- * This function returns zero if successful. On error it will return an negative
+ * This function returns zero if successful. On error it will return a negative
  * error value.
  **/
 static int
diff --git a/drivers/scsi/lpfc/lpfc_disc.h b/drivers/scsi/lpfc/lpfc_disc.h
index 6977027979be..361f5b3d9d93 100644
--- a/drivers/scsi/lpfc/lpfc_disc.h
+++ b/drivers/scsi/lpfc/lpfc_disc.h
@@ -79,7 +79,6 @@ struct lpfc_nodelist {
 	struct lpfc_name nlp_portname;
 	struct lpfc_name nlp_nodename;
 	uint32_t         nlp_flag;		/* entry flags */
-	uint32_t         nlp_add_flag;		/* additional flags */
 	uint32_t         nlp_DID;		/* FC D_ID of entry */
 	uint32_t         nlp_last_elscmd;	/* Last ELS cmd sent */
 	uint16_t         nlp_type;
@@ -147,6 +146,7 @@ struct lpfc_node_rrq {
 #define NLP_LOGO_ACC       0x00100000	/* Process LOGO after ACC completes */
 #define NLP_TGT_NO_SCSIID  0x00200000	/* good PRLI but no binding for scsid */
 #define NLP_ISSUE_LOGO     0x00400000	/* waiting to issue a LOGO */
+#define NLP_IN_DEV_LOSS    0x00800000	/* devloss in progress */
 #define NLP_ACC_REGLOGIN   0x01000000	/* Issue Reg Login after successful
 					   ACC */
 #define NLP_NPR_ADISC      0x02000000	/* Issue ADISC when dq'ed from
@@ -158,8 +158,6 @@ struct lpfc_node_rrq {
 #define NLP_FIRSTBURST     0x40000000	/* Target supports FirstBurst */
 #define NLP_RPI_REGISTERED 0x80000000	/* nlp_rpi is valid */
 
-/* Defines for nlp_add_flag (uint32) */
-#define NLP_IN_DEV_LOSS  0x00000001	/* Dev Loss processing in progress */
 
 /* ndlp usage management macros */
 #define NLP_CHK_NODE_ACT(ndlp)		(((ndlp)->nlp_usg_map \
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index 851e8efe364e..36bf58ba750a 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -1509,12 +1509,14 @@ lpfc_plogi_confirm_nport(struct lpfc_hba *phba, uint32_t *prsp,
 			 struct lpfc_nodelist *ndlp)
 {
 	struct lpfc_vport    *vport = ndlp->vport;
+	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
 	struct lpfc_nodelist *new_ndlp;
 	struct lpfc_rport_data *rdata;
 	struct fc_rport *rport;
 	struct serv_parm *sp;
 	uint8_t  name[sizeof(struct lpfc_name)];
-	uint32_t rc, keepDID = 0;
+	uint32_t rc, keepDID = 0, keep_nlp_flag = 0;
+	uint16_t keep_nlp_state;
 	int  put_node;
 	int  put_rport;
 	unsigned long *active_rrqs_xri_bitmap = NULL;
@@ -1603,11 +1605,14 @@ lpfc_plogi_confirm_nport(struct lpfc_hba *phba, uint32_t *prsp,
 		       ndlp->active_rrqs_xri_bitmap,
 		       phba->cfg_rrq_xri_bitmap_sz);
 
-	if (ndlp->nlp_flag & NLP_NPR_2B_DISC)
-		new_ndlp->nlp_flag |= NLP_NPR_2B_DISC;
-	ndlp->nlp_flag &= ~NLP_NPR_2B_DISC;
+	spin_lock_irq(shost->host_lock);
+	keep_nlp_flag = new_ndlp->nlp_flag;
+	new_ndlp->nlp_flag = ndlp->nlp_flag;
+	ndlp->nlp_flag = keep_nlp_flag;
+	spin_unlock_irq(shost->host_lock);
 
-	/* Set state will put new_ndlp on to node list if not already done */
+	/* Set nlp_states accordingly */
+	keep_nlp_state = new_ndlp->nlp_state;
 	lpfc_nlp_set_state(vport, new_ndlp, ndlp->nlp_state);
 
 	/* Move this back to NPR state */
@@ -1624,8 +1629,9 @@ lpfc_plogi_confirm_nport(struct lpfc_hba *phba, uint32_t *prsp,
 		if (rport) {
 			rdata = rport->dd_data;
 			if (rdata->pnode == ndlp) {
-				lpfc_nlp_put(ndlp);
+				/* break the link before dropping the ref */
 				ndlp->rport = NULL;
+				lpfc_nlp_put(ndlp);
 				rdata->pnode = lpfc_nlp_get(new_ndlp);
 				new_ndlp->rport = rport;
 			}
@@ -1648,7 +1654,9 @@ lpfc_plogi_confirm_nport(struct lpfc_hba *phba, uint32_t *prsp,
 			memcpy(ndlp->active_rrqs_xri_bitmap,
 			       active_rrqs_xri_bitmap,
 			       phba->cfg_rrq_xri_bitmap_sz);
-		lpfc_drop_node(vport, ndlp);
+
+		if (!NLP_CHK_NODE_ACT(ndlp))
+			lpfc_drop_node(vport, ndlp);
 	}
 	else {
 		lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
@@ -1665,20 +1673,13 @@ lpfc_plogi_confirm_nport(struct lpfc_hba *phba, uint32_t *prsp,
 			       active_rrqs_xri_bitmap,
 			       phba->cfg_rrq_xri_bitmap_sz);
 
-		/* Since we are swapping the ndlp passed in with the new one
-		 * and the did has already been swapped, copy over state.
-		 * The new WWNs are already in new_ndlp since thats what
-		 * we looked it up by in the begining of this routine.
-		 */
-		new_ndlp->nlp_state = ndlp->nlp_state;
-
-		/* Since we are switching over to the new_ndlp, the old
-		 * ndlp should be put in the NPR state, unless we have
-		 * already started re-discovery on it.
+		/* Since we are switching over to the new_ndlp,
+		 * reset the old ndlp state
 		 */
 		if ((ndlp->nlp_state == NLP_STE_UNMAPPED_NODE) ||
 		    (ndlp->nlp_state == NLP_STE_MAPPED_NODE))
-			lpfc_nlp_set_state(vport, ndlp, NLP_STE_NPR_NODE);
+			keep_nlp_state = NLP_STE_NPR_NODE;
+		lpfc_nlp_set_state(vport, ndlp, keep_nlp_state);
 
 		/* Fix up the rport accordingly */
 		rport = ndlp->rport;
@@ -3667,15 +3668,6 @@ lpfc_cmpl_els_logo_acc(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 	 * At this point, the driver is done so release the IOCB
 	 */
 	lpfc_els_free_iocb(phba, cmdiocb);
-
-	/*
-	 * Remove the ndlp reference if it's a fabric node that has
-	 * sent us an unsolicted LOGO.
-	 */
-	if (ndlp->nlp_type & NLP_FABRIC)
-		lpfc_nlp_put(ndlp);
-
-	return;
 }
 
 /**
@@ -4020,7 +4012,9 @@ lpfc_els_rsp_acc(struct lpfc_vport *vport, uint32_t flag,
 			 ndlp->nlp_rpi, vport->fc_flag);
 	if (ndlp->nlp_flag & NLP_LOGO_ACC) {
 		spin_lock_irq(shost->host_lock);
-		ndlp->nlp_flag &= ~NLP_LOGO_ACC;
+		if (!(ndlp->nlp_flag & NLP_RPI_REGISTERED ||
+			ndlp->nlp_flag & NLP_REG_LOGIN_SEND))
+			ndlp->nlp_flag &= ~NLP_LOGO_ACC;
 		spin_unlock_irq(shost->host_lock);
 		elsiocb->iocb_cmpl = lpfc_cmpl_els_logo_acc;
 	} else {
@@ -4587,16 +4581,16 @@ lpfc_els_disc_plogi(struct lpfc_vport *vport)
 		if (!NLP_CHK_NODE_ACT(ndlp))
 			continue;
 		if (ndlp->nlp_state == NLP_STE_NPR_NODE &&
-		    (ndlp->nlp_flag & NLP_NPR_2B_DISC) != 0 &&
-		    (ndlp->nlp_flag & NLP_DELAY_TMO) == 0 &&
-		    (ndlp->nlp_flag & NLP_NPR_ADISC) == 0) {
+				(ndlp->nlp_flag & NLP_NPR_2B_DISC) != 0 &&
+				(ndlp->nlp_flag & NLP_DELAY_TMO) == 0 &&
+				(ndlp->nlp_flag & NLP_NPR_ADISC) == 0) {
 			ndlp->nlp_prev_state = ndlp->nlp_state;
 			lpfc_nlp_set_state(vport, ndlp, NLP_STE_PLOGI_ISSUE);
 			lpfc_issue_els_plogi(vport, ndlp->nlp_DID, 0);
 			sentplogi++;
 			vport->num_disc_nodes++;
 			if (vport->num_disc_nodes >=
-			    vport->cfg_discovery_threads) {
+					vport->cfg_discovery_threads) {
 				spin_lock_irq(shost->host_lock);
 				vport->fc_flag |= FC_NLP_MORE;
 				spin_unlock_irq(shost->host_lock);
@@ -4615,6 +4609,660 @@ lpfc_els_disc_plogi(struct lpfc_vport *vport)
 	return sentplogi;
 }
 
+void
+lpfc_rdp_res_link_service(struct fc_rdp_link_service_desc *desc,
+		uint32_t word0)
+{
+
+	desc->tag = cpu_to_be32(RDP_LINK_SERVICE_DESC_TAG);
+	desc->payload.els_req = word0;
+	desc->length = cpu_to_be32(sizeof(desc->payload));
+}
+
+void
+lpfc_rdp_res_sfp_desc(struct fc_rdp_sfp_desc *desc,
+		uint8_t *page_a0, uint8_t *page_a2)
+{
+	uint16_t wavelength;
+	uint16_t temperature;
+	uint16_t rx_power;
+	uint16_t tx_bias;
+	uint16_t tx_power;
+	uint16_t vcc;
+	uint16_t flag = 0;
+	struct sff_trasnceiver_codes_byte4 *trasn_code_byte4;
+	struct sff_trasnceiver_codes_byte5 *trasn_code_byte5;
+
+	desc->tag = cpu_to_be32(RDP_SFP_DESC_TAG);
+
+	trasn_code_byte4 = (struct sff_trasnceiver_codes_byte4 *)
+			&page_a0[SSF_TRANSCEIVER_CODE_B4];
+	trasn_code_byte5 = (struct sff_trasnceiver_codes_byte5 *)
+			&page_a0[SSF_TRANSCEIVER_CODE_B5];
+
+	if ((trasn_code_byte4->fc_sw_laser) ||
+	    (trasn_code_byte5->fc_sw_laser_sl) ||
+	    (trasn_code_byte5->fc_sw_laser_sn)) {  /* check if its short WL */
+		flag |= (SFP_FLAG_PT_SWLASER << SFP_FLAG_PT_SHIFT);
+	} else if (trasn_code_byte4->fc_lw_laser) {
+		wavelength = (page_a0[SSF_WAVELENGTH_B1] << 8) |
+			page_a0[SSF_WAVELENGTH_B0];
+		if (wavelength == SFP_WAVELENGTH_LC1310)
+			flag |= SFP_FLAG_PT_LWLASER_LC1310 << SFP_FLAG_PT_SHIFT;
+		if (wavelength == SFP_WAVELENGTH_LL1550)
+			flag |= SFP_FLAG_PT_LWLASER_LL1550 << SFP_FLAG_PT_SHIFT;
+	}
+	/* check if its SFP+ */
+	flag |= ((page_a0[SSF_IDENTIFIER] == SFF_PG0_IDENT_SFP) ?
+			SFP_FLAG_CT_SFP_PLUS : SFP_FLAG_CT_UNKNOWN)
+					<< SFP_FLAG_CT_SHIFT;
+
+	/* check if its OPTICAL */
+	flag |= ((page_a0[SSF_CONNECTOR] == SFF_PG0_CONNECTOR_LC) ?
+			SFP_FLAG_IS_OPTICAL_PORT : 0)
+					<< SFP_FLAG_IS_OPTICAL_SHIFT;
+
+	temperature = (page_a2[SFF_TEMPERATURE_B1] << 8 |
+		page_a2[SFF_TEMPERATURE_B0]);
+	vcc = (page_a2[SFF_VCC_B1] << 8 |
+		page_a2[SFF_VCC_B0]);
+	tx_power = (page_a2[SFF_TXPOWER_B1] << 8 |
+		page_a2[SFF_TXPOWER_B0]);
+	tx_bias = (page_a2[SFF_TX_BIAS_CURRENT_B1] << 8 |
+		page_a2[SFF_TX_BIAS_CURRENT_B0]);
+	rx_power = (page_a2[SFF_RXPOWER_B1] << 8 |
+		page_a2[SFF_RXPOWER_B0]);
+	desc->sfp_info.temperature = cpu_to_be16(temperature);
+	desc->sfp_info.rx_power = cpu_to_be16(rx_power);
+	desc->sfp_info.tx_bias = cpu_to_be16(tx_bias);
+	desc->sfp_info.tx_power = cpu_to_be16(tx_power);
+	desc->sfp_info.vcc = cpu_to_be16(vcc);
+
+	desc->sfp_info.flags = cpu_to_be16(flag);
+	desc->length = cpu_to_be32(sizeof(desc->sfp_info));
+}
+
+void
+lpfc_rdp_res_link_error(struct fc_rdp_link_error_status_desc *desc,
+		READ_LNK_VAR *stat)
+{
+	uint32_t type;
+
+	desc->tag = cpu_to_be32(RDP_LINK_ERROR_STATUS_DESC_TAG);
+
+	type = VN_PT_PHY_PF_PORT << VN_PT_PHY_SHIFT;
+
+	desc->info.port_type = cpu_to_be32(type);
+
+	desc->info.link_status.link_failure_cnt =
+		cpu_to_be32(stat->linkFailureCnt);
+	desc->info.link_status.loss_of_synch_cnt =
+		cpu_to_be32(stat->lossSyncCnt);
+	desc->info.link_status.loss_of_signal_cnt =
+		cpu_to_be32(stat->lossSignalCnt);
+	desc->info.link_status.primitive_seq_proto_err =
+		cpu_to_be32(stat->primSeqErrCnt);
+	desc->info.link_status.invalid_trans_word =
+		cpu_to_be32(stat->invalidXmitWord);
+	desc->info.link_status.invalid_crc_cnt = cpu_to_be32(stat->crcCnt);
+
+	desc->length = cpu_to_be32(sizeof(desc->info));
+}
+
+void
+lpfc_rdp_res_speed(struct fc_rdp_port_speed_desc *desc, struct lpfc_hba *phba)
+{
+	uint16_t rdp_cap = 0;
+	uint16_t rdp_speed;
+
+	desc->tag = cpu_to_be32(RDP_PORT_SPEED_DESC_TAG);
+
+	switch (phba->sli4_hba.link_state.speed) {
+	case LPFC_FC_LA_SPEED_1G:
+		rdp_speed = RDP_PS_1GB;
+		break;
+	case LPFC_FC_LA_SPEED_2G:
+		rdp_speed = RDP_PS_2GB;
+		break;
+	case LPFC_FC_LA_SPEED_4G:
+		rdp_speed = RDP_PS_4GB;
+		break;
+	case LPFC_FC_LA_SPEED_8G:
+		rdp_speed = RDP_PS_8GB;
+		break;
+	case LPFC_FC_LA_SPEED_10G:
+		rdp_speed = RDP_PS_10GB;
+		break;
+	case LPFC_FC_LA_SPEED_16G:
+		rdp_speed = RDP_PS_16GB;
+		break;
+	case LPFC_FC_LA_SPEED_32G:
+		rdp_speed = RDP_PS_32GB;
+		break;
+	default:
+		rdp_speed = RDP_PS_UNKNOWN;
+		break;
+	}
+
+	desc->info.port_speed.speed = cpu_to_be16(rdp_speed);
+
+	if (phba->lmt & LMT_16Gb)
+		rdp_cap |= RDP_PS_16GB;
+	if (phba->lmt & LMT_10Gb)
+		rdp_cap |= RDP_PS_10GB;
+	if (phba->lmt & LMT_8Gb)
+		rdp_cap |= RDP_PS_8GB;
+	if (phba->lmt & LMT_4Gb)
+		rdp_cap |= RDP_PS_4GB;
+	if (phba->lmt & LMT_2Gb)
+		rdp_cap |= RDP_PS_2GB;
+	if (phba->lmt & LMT_1Gb)
+		rdp_cap |= RDP_PS_1GB;
+
+	if (rdp_cap == 0)
+		rdp_cap = RDP_CAP_UNKNOWN;
+
+	desc->info.port_speed.capabilities = cpu_to_be16(rdp_cap);
+	desc->length = cpu_to_be32(sizeof(desc->info));
+}
+
+void
+lpfc_rdp_res_diag_port_names(struct fc_rdp_port_name_desc *desc,
+		struct lpfc_hba *phba)
+{
+
+	desc->tag = cpu_to_be32(RDP_PORT_NAMES_DESC_TAG);
+
+	memcpy(desc->port_names.wwnn, phba->wwnn,
+			sizeof(desc->port_names.wwnn));
+
+	memcpy(desc->port_names.wwpn, &phba->wwpn,
+			sizeof(desc->port_names.wwpn));
+
+	desc->length = cpu_to_be32(sizeof(desc->port_names));
+}
+
+void
+lpfc_rdp_res_attach_port_names(struct fc_rdp_port_name_desc *desc,
+		struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
+{
+
+	desc->tag = cpu_to_be32(RDP_PORT_NAMES_DESC_TAG);
+	if (vport->fc_flag & FC_FABRIC) {
+		memcpy(desc->port_names.wwnn, &vport->fabric_nodename,
+				sizeof(desc->port_names.wwnn));
+
+		memcpy(desc->port_names.wwpn, &vport->fabric_portname,
+				sizeof(desc->port_names.wwpn));
+	} else {  /* Point to Point */
+		memcpy(desc->port_names.wwnn, &ndlp->nlp_nodename,
+				sizeof(desc->port_names.wwnn));
+
+		memcpy(desc->port_names.wwnn, &ndlp->nlp_portname,
+				sizeof(desc->port_names.wwpn));
+	}
+
+	desc->length = cpu_to_be32(sizeof(desc->port_names));
+}
+
+void
+lpfc_els_rdp_cmpl(struct lpfc_hba *phba, struct lpfc_rdp_context *rdp_context,
+		int status)
+{
+	struct lpfc_nodelist *ndlp = rdp_context->ndlp;
+	struct lpfc_vport *vport = ndlp->vport;
+	struct lpfc_iocbq *elsiocb;
+	IOCB_t *icmd;
+	uint8_t *pcmd;
+	struct ls_rjt *stat;
+	struct fc_rdp_res_frame *rdp_res;
+	uint32_t cmdsize;
+	int rc;
+
+	if (status != SUCCESS)
+		goto error;
+	cmdsize = sizeof(struct fc_rdp_res_frame);
+
+	elsiocb = lpfc_prep_els_iocb(vport, 0, cmdsize,
+			lpfc_max_els_tries, rdp_context->ndlp,
+			rdp_context->ndlp->nlp_DID, ELS_CMD_ACC);
+	lpfc_nlp_put(ndlp);
+	if (!elsiocb)
+		goto free_rdp_context;
+
+	icmd = &elsiocb->iocb;
+	icmd->ulpContext = rdp_context->rx_id;
+	icmd->unsli3.rcvsli3.ox_id = rdp_context->ox_id;
+
+	lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
+			"2171 Xmit RDP response tag x%x xri x%x, "
+			"did x%x, nlp_flag x%x, nlp_state x%x, rpi x%x",
+			elsiocb->iotag, elsiocb->iocb.ulpContext,
+			ndlp->nlp_DID, ndlp->nlp_flag, ndlp->nlp_state,
+			ndlp->nlp_rpi);
+	rdp_res = (struct fc_rdp_res_frame *)
+		(((struct lpfc_dmabuf *) elsiocb->context2)->virt);
+	pcmd = (uint8_t *) (((struct lpfc_dmabuf *) elsiocb->context2)->virt);
+	memset(pcmd, 0, sizeof(struct fc_rdp_res_frame));
+	*((uint32_t *) (pcmd)) = ELS_CMD_ACC;
+
+	/* For RDP payload */
+	lpfc_rdp_res_link_service(&rdp_res->link_service_desc, ELS_CMD_RDP);
+
+	lpfc_rdp_res_sfp_desc(&rdp_res->sfp_desc,
+			rdp_context->page_a0, rdp_context->page_a2);
+	lpfc_rdp_res_speed(&rdp_res->portspeed_desc, phba);
+	lpfc_rdp_res_link_error(&rdp_res->link_error_desc,
+			&rdp_context->link_stat);
+	lpfc_rdp_res_diag_port_names(&rdp_res->diag_port_names_desc, phba);
+	lpfc_rdp_res_attach_port_names(&rdp_res->attached_port_names_desc,
+			vport, ndlp);
+	rdp_res->length = cpu_to_be32(RDP_DESC_PAYLOAD_SIZE);
+
+	elsiocb->iocb_cmpl = lpfc_cmpl_els_rsp;
+
+	phba->fc_stat.elsXmitACC++;
+	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, elsiocb, 0);
+	if (rc == IOCB_ERROR)
+		lpfc_els_free_iocb(phba, elsiocb);
+
+	kfree(rdp_context);
+
+	return;
+error:
+	cmdsize = 2 * sizeof(uint32_t);
+	elsiocb = lpfc_prep_els_iocb(vport, 0, cmdsize, lpfc_max_els_tries,
+			ndlp, ndlp->nlp_DID, ELS_CMD_LS_RJT);
+	lpfc_nlp_put(ndlp);
+	if (!elsiocb)
+		goto free_rdp_context;
+
+	icmd = &elsiocb->iocb;
+	icmd->ulpContext = rdp_context->rx_id;
+	icmd->unsli3.rcvsli3.ox_id = rdp_context->ox_id;
+	pcmd = (uint8_t *) (((struct lpfc_dmabuf *) elsiocb->context2)->virt);
+
+	*((uint32_t *) (pcmd)) = ELS_CMD_LS_RJT;
+	stat = (struct ls_rjt *)(pcmd + sizeof(uint32_t));
+	stat->un.b.lsRjtRsnCode = LSRJT_UNABLE_TPC;
+
+	phba->fc_stat.elsXmitLSRJT++;
+	elsiocb->iocb_cmpl = lpfc_cmpl_els_rsp;
+	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, elsiocb, 0);
+
+	if (rc == IOCB_ERROR)
+		lpfc_els_free_iocb(phba, elsiocb);
+free_rdp_context:
+	kfree(rdp_context);
+}
+
+int
+lpfc_get_rdp_info(struct lpfc_hba *phba, struct lpfc_rdp_context *rdp_context)
+{
+	LPFC_MBOXQ_t *mbox = NULL;
+	int rc;
+
+	mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!mbox) {
+		lpfc_printf_log(phba, KERN_WARNING, LOG_MBOX | LOG_ELS,
+				"7105 failed to allocate mailbox memory");
+		return 1;
+	}
+
+	if (lpfc_sli4_dump_page_a0(phba, mbox))
+		goto prep_mbox_fail;
+	mbox->vport = rdp_context->ndlp->vport;
+	mbox->mbox_cmpl = lpfc_mbx_cmpl_rdp_page_a0;
+	mbox->context2 = (struct lpfc_rdp_context *) rdp_context;
+	rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
+	if (rc == MBX_NOT_FINISHED)
+		goto issue_mbox_fail;
+
+	return 0;
+
+prep_mbox_fail:
+issue_mbox_fail:
+	mempool_free(mbox, phba->mbox_mem_pool);
+	return 1;
+}
+
+/*
+ * lpfc_els_rcv_rdp - Process an unsolicited RDP ELS.
+ * @vport: pointer to a host virtual N_Port data structure.
+ * @cmdiocb: pointer to lpfc command iocb data structure.
+ * @ndlp: pointer to a node-list data structure.
+ *
+ * This routine processes an unsolicited RDP(Read Diagnostic Parameters)
+ * IOCB. First, the payload of the unsolicited RDP is checked.
+ * Then it will (1) send MBX_DUMP_MEMORY, Embedded DMP_LMSD sub command TYPE-3
+ * for Page A0, (2) send MBX_DUMP_MEMORY, DMP_LMSD for Page A2,
+ * (3) send MBX_READ_LNK_STAT to get link stat, (4) Call lpfc_els_rdp_cmpl
+ * gather all data and send RDP response.
+ *
+ * Return code
+ *   0 - Sent the acc response
+ *   1 - Sent the reject response.
+ */
+static int
+lpfc_els_rcv_rdp(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
+		struct lpfc_nodelist *ndlp)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_dmabuf *pcmd;
+	uint8_t rjt_err, rjt_expl = LSEXP_NOTHING_MORE;
+	struct fc_rdp_req_frame *rdp_req;
+	struct lpfc_rdp_context *rdp_context;
+	IOCB_t *cmd = NULL;
+	struct ls_rjt stat;
+
+	if (phba->sli_rev < LPFC_SLI_REV4 ||
+			(bf_get(lpfc_sli_intf_if_type,
+				&phba->sli4_hba.sli_intf) !=
+						LPFC_SLI_INTF_IF_TYPE_2)) {
+		rjt_err = LSRJT_UNABLE_TPC;
+		rjt_expl = LSEXP_REQ_UNSUPPORTED;
+		goto error;
+	}
+
+	if (phba->sli_rev < LPFC_SLI_REV4 || (phba->hba_flag & HBA_FCOE_MODE)) {
+		rjt_err = LSRJT_UNABLE_TPC;
+		rjt_expl = LSEXP_REQ_UNSUPPORTED;
+		goto error;
+	}
+
+	pcmd = (struct lpfc_dmabuf *) cmdiocb->context2;
+	rdp_req = (struct fc_rdp_req_frame *) pcmd->virt;
+
+
+	lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
+			 "2422 ELS RDP Request "
+			 "dec len %d tag x%x port_id %d len %d\n",
+			 be32_to_cpu(rdp_req->rdp_des_length),
+			 be32_to_cpu(rdp_req->nport_id_desc.tag),
+			 be32_to_cpu(rdp_req->nport_id_desc.nport_id),
+			 be32_to_cpu(rdp_req->nport_id_desc.length));
+
+	if (sizeof(struct fc_rdp_nport_desc) !=
+			be32_to_cpu(rdp_req->rdp_des_length))
+		goto rjt_logerr;
+	if (RDP_N_PORT_DESC_TAG != be32_to_cpu(rdp_req->nport_id_desc.tag))
+		goto rjt_logerr;
+	if (RDP_NPORT_ID_SIZE !=
+			be32_to_cpu(rdp_req->nport_id_desc.length))
+		goto rjt_logerr;
+	rdp_context = kmalloc(sizeof(struct lpfc_rdp_context), GFP_KERNEL);
+	if (!rdp_context) {
+		rjt_err = LSRJT_UNABLE_TPC;
+		goto error;
+	}
+
+	memset(rdp_context, 0, sizeof(struct lpfc_rdp_context));
+	cmd = &cmdiocb->iocb;
+	rdp_context->ndlp = lpfc_nlp_get(ndlp);
+	rdp_context->ox_id = cmd->unsli3.rcvsli3.ox_id;
+	rdp_context->rx_id = cmd->ulpContext;
+	rdp_context->cmpl = lpfc_els_rdp_cmpl;
+	if (lpfc_get_rdp_info(phba, rdp_context)) {
+		lpfc_printf_vlog(ndlp->vport, KERN_WARNING, LOG_ELS,
+				 "2423 Unable to send mailbox");
+		kfree(rdp_context);
+		rjt_err = LSRJT_UNABLE_TPC;
+		lpfc_nlp_put(ndlp);
+		goto error;
+	}
+
+	return 0;
+
+rjt_logerr:
+	rjt_err = LSRJT_LOGICAL_ERR;
+
+error:
+	memset(&stat, 0, sizeof(stat));
+	stat.un.b.lsRjtRsnCode = rjt_err;
+	stat.un.b.lsRjtRsnCodeExp = rjt_expl;
+	lpfc_els_rsp_reject(vport, stat.un.lsRjtError, cmdiocb, ndlp, NULL);
+	return 1;
+}
+
+
+static void
+lpfc_els_lcb_rsp(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
+{
+	MAILBOX_t *mb;
+	IOCB_t *icmd;
+	uint8_t *pcmd;
+	struct lpfc_iocbq *elsiocb;
+	struct lpfc_nodelist *ndlp;
+	struct ls_rjt *stat;
+	union lpfc_sli4_cfg_shdr *shdr;
+	struct lpfc_lcb_context *lcb_context;
+	struct fc_lcb_res_frame *lcb_res;
+	uint32_t cmdsize, shdr_status, shdr_add_status;
+	int rc;
+
+	mb = &pmb->u.mb;
+	lcb_context = (struct lpfc_lcb_context *)pmb->context1;
+	ndlp = lcb_context->ndlp;
+	pmb->context1 = NULL;
+	pmb->context2 = NULL;
+
+	shdr = (union lpfc_sli4_cfg_shdr *)
+			&pmb->u.mqe.un.beacon_config.header.cfg_shdr;
+	shdr_status = bf_get(lpfc_mbox_hdr_status, &shdr->response);
+	shdr_add_status = bf_get(lpfc_mbox_hdr_add_status, &shdr->response);
+
+	lpfc_printf_log(phba, KERN_INFO, LOG_MBOX,
+				"0194 SET_BEACON_CONFIG mailbox "
+				"completed with status x%x add_status x%x,"
+				" mbx status x%x\n",
+				shdr_status, shdr_add_status, mb->mbxStatus);
+
+	if (mb->mbxStatus && !(shdr_status &&
+		shdr_add_status == ADD_STATUS_OPERATION_ALREADY_ACTIVE)) {
+		mempool_free(pmb, phba->mbox_mem_pool);
+		goto error;
+	}
+
+	mempool_free(pmb, phba->mbox_mem_pool);
+	cmdsize = sizeof(struct fc_lcb_res_frame);
+	elsiocb = lpfc_prep_els_iocb(phba->pport, 0, cmdsize,
+			lpfc_max_els_tries, ndlp,
+			ndlp->nlp_DID, ELS_CMD_ACC);
+
+	/* Decrement the ndlp reference count from previous mbox command */
+	lpfc_nlp_put(ndlp);
+
+	if (!elsiocb)
+		goto free_lcb_context;
+
+	lcb_res = (struct fc_lcb_res_frame *)
+		(((struct lpfc_dmabuf *)elsiocb->context2)->virt);
+
+	icmd = &elsiocb->iocb;
+	icmd->ulpContext = lcb_context->rx_id;
+	icmd->unsli3.rcvsli3.ox_id = lcb_context->ox_id;
+
+	pcmd = (uint8_t *)(((struct lpfc_dmabuf *)elsiocb->context2)->virt);
+	*((uint32_t *)(pcmd)) = ELS_CMD_ACC;
+	lcb_res->lcb_sub_command = lcb_context->sub_command;
+	lcb_res->lcb_type = lcb_context->type;
+	lcb_res->lcb_frequency = lcb_context->frequency;
+	elsiocb->iocb_cmpl = lpfc_cmpl_els_rsp;
+	phba->fc_stat.elsXmitACC++;
+	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, elsiocb, 0);
+	if (rc == IOCB_ERROR)
+		lpfc_els_free_iocb(phba, elsiocb);
+
+	kfree(lcb_context);
+	return;
+
+error:
+	cmdsize = sizeof(struct fc_lcb_res_frame);
+	elsiocb = lpfc_prep_els_iocb(phba->pport, 0, cmdsize,
+			lpfc_max_els_tries, ndlp,
+			ndlp->nlp_DID, ELS_CMD_LS_RJT);
+	lpfc_nlp_put(ndlp);
+	if (!elsiocb)
+		goto free_lcb_context;
+
+	icmd = &elsiocb->iocb;
+	icmd->ulpContext = lcb_context->rx_id;
+	icmd->unsli3.rcvsli3.ox_id = lcb_context->ox_id;
+	pcmd = (uint8_t *)(((struct lpfc_dmabuf *)elsiocb->context2)->virt);
+
+	*((uint32_t *)(pcmd)) = ELS_CMD_LS_RJT;
+	stat = (struct ls_rjt *)(pcmd + sizeof(uint32_t));
+	stat->un.b.lsRjtRsnCode = LSRJT_UNABLE_TPC;
+
+	elsiocb->iocb_cmpl = lpfc_cmpl_els_rsp;
+	phba->fc_stat.elsXmitLSRJT++;
+	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, elsiocb, 0);
+	if (rc == IOCB_ERROR)
+		lpfc_els_free_iocb(phba, elsiocb);
+free_lcb_context:
+	kfree(lcb_context);
+}
+
+static int
+lpfc_sli4_set_beacon(struct lpfc_vport *vport,
+		     struct lpfc_lcb_context *lcb_context,
+		     uint32_t beacon_state)
+{
+	struct lpfc_hba *phba = vport->phba;
+	LPFC_MBOXQ_t *mbox = NULL;
+	uint32_t len;
+	int rc;
+
+	mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!mbox)
+		return 1;
+
+	len = sizeof(struct lpfc_mbx_set_beacon_config) -
+		sizeof(struct lpfc_sli4_cfg_mhdr);
+	lpfc_sli4_config(phba, mbox, LPFC_MBOX_SUBSYSTEM_COMMON,
+			 LPFC_MBOX_OPCODE_SET_BEACON_CONFIG, len,
+			 LPFC_SLI4_MBX_EMBED);
+	mbox->context1 = (void *)lcb_context;
+	mbox->vport = phba->pport;
+	mbox->mbox_cmpl = lpfc_els_lcb_rsp;
+	bf_set(lpfc_mbx_set_beacon_port_num, &mbox->u.mqe.un.beacon_config,
+	       phba->sli4_hba.physical_port);
+	bf_set(lpfc_mbx_set_beacon_state, &mbox->u.mqe.un.beacon_config,
+	       beacon_state);
+	bf_set(lpfc_mbx_set_beacon_port_type, &mbox->u.mqe.un.beacon_config, 1);
+	bf_set(lpfc_mbx_set_beacon_duration, &mbox->u.mqe.un.beacon_config, 0);
+	rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
+	if (rc == MBX_NOT_FINISHED) {
+		mempool_free(mbox, phba->mbox_mem_pool);
+		return 1;
+	}
+
+	return 0;
+}
+
+
+/**
+ * lpfc_els_rcv_lcb - Process an unsolicited LCB
+ * @vport: pointer to a host virtual N_Port data structure.
+ * @cmdiocb: pointer to lpfc command iocb data structure.
+ * @ndlp: pointer to a node-list data structure.
+ *
+ * This routine processes an unsolicited LCB(LINK CABLE BEACON) IOCB.
+ * First, the payload of the unsolicited LCB is checked.
+ * Then based on Subcommand beacon will either turn on or off.
+ *
+ * Return code
+ * 0 - Sent the acc response
+ * 1 - Sent the reject response.
+ **/
+static int
+lpfc_els_rcv_lcb(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
+		 struct lpfc_nodelist *ndlp)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_dmabuf *pcmd;
+	IOCB_t *icmd;
+	uint8_t *lp;
+	struct fc_lcb_request_frame *beacon;
+	struct lpfc_lcb_context *lcb_context;
+	uint8_t state, rjt_err;
+	struct ls_rjt stat;
+
+	icmd = &cmdiocb->iocb;
+	pcmd = (struct lpfc_dmabuf *)cmdiocb->context2;
+	lp = (uint8_t *)pcmd->virt;
+	beacon = (struct fc_lcb_request_frame *)pcmd->virt;
+
+	lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
+			"0192 ELS LCB Data x%x x%x x%x x%x sub x%x "
+			"type x%x frequency %x duration x%x\n",
+			lp[0], lp[1], lp[2],
+			beacon->lcb_command,
+			beacon->lcb_sub_command,
+			beacon->lcb_type,
+			beacon->lcb_frequency,
+			be16_to_cpu(beacon->lcb_duration));
+
+	if (phba->sli_rev < LPFC_SLI_REV4 ||
+	    (bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf) !=
+	    LPFC_SLI_INTF_IF_TYPE_2)) {
+		rjt_err = LSRJT_CMD_UNSUPPORTED;
+		goto rjt;
+	}
+	lcb_context = kmalloc(sizeof(struct lpfc_lcb_context), GFP_KERNEL);
+
+	if (phba->hba_flag & HBA_FCOE_MODE) {
+		rjt_err = LSRJT_CMD_UNSUPPORTED;
+		goto rjt;
+	}
+	if (beacon->lcb_frequency == 0) {
+		rjt_err = LSRJT_CMD_UNSUPPORTED;
+		goto rjt;
+	}
+	if ((beacon->lcb_type != LPFC_LCB_GREEN) &&
+	    (beacon->lcb_type != LPFC_LCB_AMBER)) {
+		rjt_err = LSRJT_CMD_UNSUPPORTED;
+		goto rjt;
+	}
+	if ((beacon->lcb_sub_command != LPFC_LCB_ON) &&
+	    (beacon->lcb_sub_command != LPFC_LCB_OFF)) {
+		rjt_err = LSRJT_CMD_UNSUPPORTED;
+		goto rjt;
+	}
+	if ((beacon->lcb_sub_command == LPFC_LCB_ON) &&
+	    (beacon->lcb_type != LPFC_LCB_GREEN) &&
+	    (beacon->lcb_type != LPFC_LCB_AMBER)) {
+		rjt_err = LSRJT_CMD_UNSUPPORTED;
+		goto rjt;
+	}
+	if (be16_to_cpu(beacon->lcb_duration) != 0) {
+		rjt_err = LSRJT_CMD_UNSUPPORTED;
+		goto rjt;
+	}
+
+	state = (beacon->lcb_sub_command == LPFC_LCB_ON) ? 1 : 0;
+	lcb_context->sub_command = beacon->lcb_sub_command;
+	lcb_context->type = beacon->lcb_type;
+	lcb_context->frequency = beacon->lcb_frequency;
+	lcb_context->ox_id = cmdiocb->iocb.unsli3.rcvsli3.ox_id;
+	lcb_context->rx_id = cmdiocb->iocb.ulpContext;
+	lcb_context->ndlp = lpfc_nlp_get(ndlp);
+	if (lpfc_sli4_set_beacon(vport, lcb_context, state)) {
+		lpfc_printf_vlog(ndlp->vport, KERN_ERR,
+				 LOG_ELS, "0193 failed to send mail box");
+		lpfc_nlp_put(ndlp);
+		rjt_err = LSRJT_UNABLE_TPC;
+		goto rjt;
+	}
+	return 0;
+rjt:
+	memset(&stat, 0, sizeof(stat));
+	stat.un.b.lsRjtRsnCode = rjt_err;
+	lpfc_els_rsp_reject(vport, stat.un.lsRjtError, cmdiocb, ndlp, NULL);
+	return 1;
+}
+
+
 /**
  * lpfc_els_flush_rscn - Clean up any rscn activities with a vport
  * @vport: pointer to a host virtual N_Port data structure.
@@ -6706,8 +7354,13 @@ lpfc_els_unsol_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 	 * Do not process any unsolicited ELS commands
 	 * if the ndlp is in DEV_LOSS
 	 */
-	if (ndlp->nlp_add_flag & NLP_IN_DEV_LOSS)
+	shost = lpfc_shost_from_vport(vport);
+	spin_lock_irq(shost->host_lock);
+	if (ndlp->nlp_flag & NLP_IN_DEV_LOSS) {
+		spin_unlock_irq(shost->host_lock);
 		goto dropit;
+	}
+	spin_unlock_irq(shost->host_lock);
 
 	elsiocb->context1 = lpfc_nlp_get(ndlp);
 	elsiocb->vport = vport;
@@ -6751,7 +7404,6 @@ lpfc_els_unsol_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 			rjt_exp = LSEXP_NOTHING_MORE;
 			break;
 		}
-		shost = lpfc_shost_from_vport(vport);
 		if (vport->port_state < LPFC_DISC_AUTH) {
 			if (!(phba->pport->fc_flag & FC_PT2PT) ||
 				(phba->pport->fc_flag & FC_PT2PT_PLOGI)) {
@@ -6821,6 +7473,14 @@ lpfc_els_unsol_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 		}
 		lpfc_disc_state_machine(vport, ndlp, elsiocb, NLP_EVT_RCV_PRLO);
 		break;
+	case ELS_CMD_LCB:
+		phba->fc_stat.elsRcvLCB++;
+		lpfc_els_rcv_lcb(vport, elsiocb, ndlp);
+		break;
+	case ELS_CMD_RDP:
+		phba->fc_stat.elsRcvRDP++;
+		lpfc_els_rcv_rdp(vport, elsiocb, ndlp);
+		break;
 	case ELS_CMD_RSCN:
 		phba->fc_stat.elsRcvRSCN++;
 		lpfc_els_rcv_rscn(vport, elsiocb, ndlp);
@@ -7586,7 +8246,8 @@ lpfc_cmpl_els_fdisc(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 		lpfc_do_scr_ns_plogi(phba, vport);
 	goto out;
 fdisc_failed:
-	lpfc_vport_set_state(vport, FC_VPORT_FAILED);
+	if (vport->fc_vport->vport_state != FC_VPORT_NO_FABRIC_RSCS)
+		lpfc_vport_set_state(vport, FC_VPORT_FAILED);
 	/* Cancel discovery timer */
 	lpfc_can_disctmo(vport);
 	lpfc_nlp_put(ndlp);
@@ -7739,8 +8400,10 @@ lpfc_cmpl_els_npiv_logo(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 
 	if (irsp->ulpStatus == IOSTAT_SUCCESS) {
 		spin_lock_irq(shost->host_lock);
+		vport->fc_flag &= ~FC_NDISC_ACTIVE;
 		vport->fc_flag &= ~FC_FABRIC;
 		spin_unlock_irq(shost->host_lock);
+		lpfc_can_disctmo(vport);
 	}
 }
 
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index 2500f15d437f..ce96d5bf8ae7 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -106,6 +106,7 @@ lpfc_dev_loss_tmo_callbk(struct fc_rport *rport)
 	struct lpfc_rport_data *rdata;
 	struct lpfc_nodelist * ndlp;
 	struct lpfc_vport *vport;
+	struct Scsi_Host *shost;
 	struct lpfc_hba   *phba;
 	struct lpfc_work_evt *evtp;
 	int  put_node;
@@ -146,48 +147,32 @@ lpfc_dev_loss_tmo_callbk(struct fc_rport *rport)
 	if (ndlp->nlp_state == NLP_STE_MAPPED_NODE)
 		return;
 
-	if (ndlp->nlp_type & NLP_FABRIC) {
-
-		/* If the WWPN of the rport and ndlp don't match, ignore it */
-		if (rport->port_name != wwn_to_u64(ndlp->nlp_portname.u.wwn)) {
-			lpfc_printf_vlog(vport, KERN_ERR, LOG_NODE,
-				"6789 rport name %lx != node port name %lx",
-				(unsigned long)rport->port_name,
-				(unsigned long)wwn_to_u64(
-						ndlp->nlp_portname.u.wwn));
-			put_node = rdata->pnode != NULL;
-			put_rport = ndlp->rport != NULL;
-			rdata->pnode = NULL;
-			ndlp->rport = NULL;
-			if (put_node)
-				lpfc_nlp_put(ndlp);
-			put_device(&rport->dev);
-			return;
-		}
-
-		put_node = rdata->pnode != NULL;
-		put_rport = ndlp->rport != NULL;
-		rdata->pnode = NULL;
-		ndlp->rport = NULL;
-		if (put_node)
-			lpfc_nlp_put(ndlp);
-		if (put_rport)
-			put_device(&rport->dev);
-		return;
-	}
+	if (rport->port_name != wwn_to_u64(ndlp->nlp_portname.u.wwn))
+		lpfc_printf_vlog(vport, KERN_ERR, LOG_NODE,
+				"6789 rport name %llx != node port name %llx",
+				rport->port_name,
+				wwn_to_u64(ndlp->nlp_portname.u.wwn));
 
 	evtp = &ndlp->dev_loss_evt;
 
-	if (!list_empty(&evtp->evt_listp))
+	if (!list_empty(&evtp->evt_listp)) {
+		lpfc_printf_vlog(vport, KERN_ERR, LOG_NODE,
+				"6790 rport name %llx dev_loss_evt pending",
+				rport->port_name);
 		return;
+	}
 
-	evtp->evt_arg1  = lpfc_nlp_get(ndlp);
-	ndlp->nlp_add_flag |= NLP_IN_DEV_LOSS;
+	shost = lpfc_shost_from_vport(vport);
+	spin_lock_irq(shost->host_lock);
+	ndlp->nlp_flag |= NLP_IN_DEV_LOSS;
+	spin_unlock_irq(shost->host_lock);
 
-	spin_lock_irq(&phba->hbalock);
 	/* We need to hold the node by incrementing the reference
 	 * count until this queued work is done
 	 */
+	evtp->evt_arg1  = lpfc_nlp_get(ndlp);
+
+	spin_lock_irq(&phba->hbalock);
 	if (evtp->evt_arg1) {
 		evtp->evt = LPFC_EVT_DEV_LOSS;
 		list_add_tail(&evtp->evt_listp, &phba->work_list);
@@ -215,22 +200,24 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 	struct fc_rport   *rport;
 	struct lpfc_vport *vport;
 	struct lpfc_hba   *phba;
+	struct Scsi_Host  *shost;
 	uint8_t *name;
 	int  put_node;
-	int  put_rport;
 	int warn_on = 0;
 	int fcf_inuse = 0;
 
 	rport = ndlp->rport;
+	vport = ndlp->vport;
+	shost = lpfc_shost_from_vport(vport);
+
+	spin_lock_irq(shost->host_lock);
+	ndlp->nlp_flag &= ~NLP_IN_DEV_LOSS;
+	spin_unlock_irq(shost->host_lock);
 
-	if (!rport) {
-		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
+	if (!rport)
 		return fcf_inuse;
-	}
 
-	rdata = rport->dd_data;
 	name = (uint8_t *) &ndlp->nlp_portname;
-	vport = ndlp->vport;
 	phba  = vport->phba;
 
 	if (phba->sli_rev == LPFC_SLI_REV4)
@@ -244,6 +231,13 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 			 "3182 dev_loss_tmo_handler x%06x, rport %p flg x%x\n",
 			 ndlp->nlp_DID, ndlp->rport, ndlp->nlp_flag);
 
+	/*
+	 * lpfc_nlp_remove if reached with dangling rport drops the
+	 * reference. To make sure that does not happen clear rport
+	 * pointer in ndlp before lpfc_nlp_put.
+	 */
+	rdata = rport->dd_data;
+
 	/* Don't defer this if we are in the process of deleting the vport
 	 * or unloading the driver. The unload will cleanup the node
 	 * appropriately we just need to cleanup the ndlp rport info here.
@@ -256,14 +250,12 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 					ndlp->nlp_sid, 0, LPFC_CTX_TGT);
 		}
 		put_node = rdata->pnode != NULL;
-		put_rport = ndlp->rport != NULL;
 		rdata->pnode = NULL;
 		ndlp->rport = NULL;
-		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 		if (put_node)
 			lpfc_nlp_put(ndlp);
-		if (put_rport)
-			put_device(&rport->dev);
+		put_device(&rport->dev);
+
 		return fcf_inuse;
 	}
 
@@ -275,28 +267,21 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 				 *name, *(name+1), *(name+2), *(name+3),
 				 *(name+4), *(name+5), *(name+6), *(name+7),
 				 ndlp->nlp_DID);
-		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 		return fcf_inuse;
 	}
 
-	if (ndlp->nlp_type & NLP_FABRIC) {
-		/* We will clean up these Nodes in linkup */
-		put_node = rdata->pnode != NULL;
-		put_rport = ndlp->rport != NULL;
-		rdata->pnode = NULL;
-		ndlp->rport = NULL;
-		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
-		if (put_node)
-			lpfc_nlp_put(ndlp);
-		if (put_rport)
-			put_device(&rport->dev);
+	put_node = rdata->pnode != NULL;
+	rdata->pnode = NULL;
+	ndlp->rport = NULL;
+	if (put_node)
+		lpfc_nlp_put(ndlp);
+	put_device(&rport->dev);
+
+	if (ndlp->nlp_type & NLP_FABRIC)
 		return fcf_inuse;
-	}
 
 	if (ndlp->nlp_sid != NLP_NO_SID) {
 		warn_on = 1;
-		/* flush the target */
-		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 		lpfc_sli_abort_iocb(vport, &phba->sli.ring[phba->sli.fcp_ring],
 				    ndlp->nlp_sid, 0, LPFC_CTX_TGT);
 	}
@@ -321,16 +306,6 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 				 ndlp->nlp_state, ndlp->nlp_rpi);
 	}
 
-	put_node = rdata->pnode != NULL;
-	put_rport = ndlp->rport != NULL;
-	rdata->pnode = NULL;
-	ndlp->rport = NULL;
-	ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
-	if (put_node)
-		lpfc_nlp_put(ndlp);
-	if (put_rport)
-		put_device(&rport->dev);
-
 	if (!(vport->load_flag & FC_UNLOADING) &&
 	    !(ndlp->nlp_flag & NLP_DELAY_TMO) &&
 	    !(ndlp->nlp_flag & NLP_NPR_2B_DISC) &&
@@ -1802,7 +1777,7 @@ lpfc_sli4_fcf_rec_mbox_parse(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq,
 	dma_addr_t phys_addr;
 	struct lpfc_mbx_sge sge;
 	struct lpfc_mbx_read_fcf_tbl *read_fcf;
-	uint32_t shdr_status, shdr_add_status;
+	uint32_t shdr_status, shdr_add_status, if_type;
 	union lpfc_sli4_cfg_shdr *shdr;
 	struct fcf_record *new_fcf_record;
 
@@ -1823,9 +1798,11 @@ lpfc_sli4_fcf_rec_mbox_parse(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq,
 	lpfc_sli_pcimem_bcopy(shdr, shdr,
 			      sizeof(union lpfc_sli4_cfg_shdr));
 	shdr_status = bf_get(lpfc_mbox_hdr_status, &shdr->response);
+	if_type = bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf);
 	shdr_add_status = bf_get(lpfc_mbox_hdr_add_status, &shdr->response);
 	if (shdr_status || shdr_add_status) {
-		if (shdr_status == STATUS_FCF_TABLE_EMPTY)
+		if (shdr_status == STATUS_FCF_TABLE_EMPTY ||
+					if_type == LPFC_SLI_INTF_IF_TYPE_2)
 			lpfc_printf_log(phba, KERN_ERR, LOG_FIP,
 					"2726 READ_FCF_RECORD Indicates empty "
 					"FCF table.\n");
@@ -3868,11 +3845,11 @@ out:
 
 	if (vport->port_state < LPFC_VPORT_READY) {
 		/* Link up discovery requires Fabric registration. */
-		lpfc_ns_cmd(vport, SLI_CTNS_RFF_ID, 0, 0); /* Do this first! */
 		lpfc_ns_cmd(vport, SLI_CTNS_RNN_ID, 0, 0);
 		lpfc_ns_cmd(vport, SLI_CTNS_RSNN_NN, 0, 0);
 		lpfc_ns_cmd(vport, SLI_CTNS_RSPN_ID, 0, 0);
 		lpfc_ns_cmd(vport, SLI_CTNS_RFT_ID, 0, 0);
+		lpfc_ns_cmd(vport, SLI_CTNS_RFF_ID, 0, 0);
 
 		/* Issue SCR just before NameServer GID_FT Query */
 		lpfc_issue_els_scr(vport, SCR_DID, 0);
@@ -3918,9 +3895,17 @@ lpfc_register_remote_port(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 	 * registered port, drop the reference that we took the last time we
 	 * registered the port.
 	 */
-	if (ndlp->rport && ndlp->rport->dd_data &&
-	    ((struct lpfc_rport_data *) ndlp->rport->dd_data)->pnode == ndlp)
-		lpfc_nlp_put(ndlp);
+	rport = ndlp->rport;
+	if (rport) {
+		rdata = rport->dd_data;
+		/* break the link before dropping the ref */
+		ndlp->rport = NULL;
+		if (rdata && rdata->pnode == ndlp)
+			lpfc_nlp_put(ndlp);
+		rdata->pnode = NULL;
+		/* drop reference for earlier registeration */
+		put_device(&rport->dev);
+	}
 
 	lpfc_debugfs_disc_trc(vport, LPFC_DISC_TRC_RPORT,
 		"rport add:       did:x%x flg:x%x type x%x",
@@ -4296,9 +4281,9 @@ lpfc_drop_node(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 	if (vport->phba->sli_rev == LPFC_SLI_REV4) {
 		lpfc_cleanup_vports_rrqs(vport, ndlp);
 		lpfc_unreg_rpi(vport, ndlp);
-	} else {
-		lpfc_nlp_put(ndlp);
 	}
+
+	lpfc_nlp_put(ndlp);
 	return;
 }
 
@@ -4510,7 +4495,7 @@ lpfc_unreg_rpi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 {
 	struct lpfc_hba *phba = vport->phba;
 	LPFC_MBOXQ_t    *mbox;
-	int rc;
+	int rc, acc_plogi = 1;
 	uint16_t rpi;
 
 	if (ndlp->nlp_flag & NLP_RPI_REGISTERED ||
@@ -4543,14 +4528,20 @@ lpfc_unreg_rpi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 					mbox->context1 = lpfc_nlp_get(ndlp);
 					mbox->mbox_cmpl =
 						lpfc_sli4_unreg_rpi_cmpl_clr;
+					/*
+					 * accept PLOGIs after unreg_rpi_cmpl
+					 */
+					acc_plogi = 0;
 				} else
 					mbox->mbox_cmpl =
 						lpfc_sli_def_mbox_cmpl;
 			}
 
 			rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
-			if (rc == MBX_NOT_FINISHED)
+			if (rc == MBX_NOT_FINISHED) {
 				mempool_free(mbox, phba->mbox_mem_pool);
+				acc_plogi = 1;
+			}
 		}
 		lpfc_no_rpi(phba, ndlp);
 
@@ -4558,8 +4549,11 @@ lpfc_unreg_rpi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 			ndlp->nlp_rpi = 0;
 		ndlp->nlp_flag &= ~NLP_RPI_REGISTERED;
 		ndlp->nlp_flag &= ~NLP_NPR_ADISC;
+		if (acc_plogi)
+			ndlp->nlp_flag &= ~NLP_LOGO_ACC;
 		return 1;
 	}
+	ndlp->nlp_flag &= ~NLP_LOGO_ACC;
 	return 0;
 }
 
@@ -4761,6 +4755,7 @@ lpfc_nlp_remove(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 {
 	struct lpfc_hba  *phba = vport->phba;
 	struct lpfc_rport_data *rdata;
+	struct fc_rport *rport;
 	LPFC_MBOXQ_t *mbox;
 	int rc;
 
@@ -4798,14 +4793,24 @@ lpfc_nlp_remove(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 	lpfc_cleanup_node(vport, ndlp);
 
 	/*
-	 * We can get here with a non-NULL ndlp->rport because when we
-	 * unregister a rport we don't break the rport/node linkage.  So if we
-	 * do, make sure we don't leaving any dangling pointers behind.
+	 * ndlp->rport must be set to NULL before it reaches here
+	 * i.e. break rport/node link before doing lpfc_nlp_put for
+	 * registered rport and then drop the reference of rport.
 	 */
 	if (ndlp->rport) {
-		rdata = ndlp->rport->dd_data;
+		/*
+		 * extra lpfc_nlp_put dropped the reference of ndlp
+		 * for registered rport so need to cleanup rport
+		 */
+		lpfc_printf_vlog(vport, KERN_WARNING, LOG_NODE,
+				"0940 removed node x%p DID x%x "
+				" rport not null %p\n",
+				ndlp, ndlp->nlp_DID, ndlp->rport);
+		rport = ndlp->rport;
+		rdata = rport->dd_data;
 		rdata->pnode = NULL;
 		ndlp->rport = NULL;
+		put_device(&rport->dev);
 	}
 }
 
@@ -4833,9 +4838,19 @@ lpfc_matchdid(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	if (matchdid.un.b.id == ndlpdid.un.b.id) {
 		if ((mydid.un.b.domain == matchdid.un.b.domain) &&
 		    (mydid.un.b.area == matchdid.un.b.area)) {
+			/* This code is supposed to match the ID
+			 * for a private loop device that is
+			 * connect to fl_port. But we need to
+			 * check that the port did not just go
+			 * from pt2pt to fabric or we could end
+			 * up matching ndlp->nlp_DID 000001 to
+			 * fabric DID 0x20101
+			 */
 			if ((ndlpdid.un.b.domain == 0) &&
 			    (ndlpdid.un.b.area == 0)) {
-				if (ndlpdid.un.b.id)
+				if (ndlpdid.un.b.id &&
+				    vport->phba->fc_topology ==
+				    LPFC_TOPOLOGY_LOOP)
 					return 1;
 			}
 			return 0;
diff --git a/drivers/scsi/lpfc/lpfc_hw.h b/drivers/scsi/lpfc/lpfc_hw.h
index 37beb9dc1311..892c5257d87c 100644
--- a/drivers/scsi/lpfc/lpfc_hw.h
+++ b/drivers/scsi/lpfc/lpfc_hw.h
@@ -543,6 +543,7 @@ struct fc_vft_header {
 #define ELS_CMD_TEST      0x11000000
 #define ELS_CMD_RRQ       0x12000000
 #define ELS_CMD_REC       0x13000000
+#define ELS_CMD_RDP       0x18000000
 #define ELS_CMD_PRLI      0x20100014
 #define ELS_CMD_PRLO      0x21100014
 #define ELS_CMD_PRLO_ACC  0x02100014
@@ -558,6 +559,7 @@ struct fc_vft_header {
 #define ELS_CMD_SCR       0x62000000
 #define ELS_CMD_RNID      0x78000000
 #define ELS_CMD_LIRR      0x7A000000
+#define ELS_CMD_LCB	  0x81000000
 #else	/*  __LITTLE_ENDIAN_BITFIELD */
 #define ELS_CMD_MASK      0xffff
 #define ELS_RSP_MASK      0xff
@@ -580,6 +582,7 @@ struct fc_vft_header {
 #define ELS_CMD_TEST      0x11
 #define ELS_CMD_RRQ       0x12
 #define ELS_CMD_REC       0x13
+#define ELS_CMD_RDP	  0x18
 #define ELS_CMD_PRLI      0x14001020
 #define ELS_CMD_PRLO      0x14001021
 #define ELS_CMD_PRLO_ACC  0x14001002
@@ -595,6 +598,7 @@ struct fc_vft_header {
 #define ELS_CMD_SCR       0x62
 #define ELS_CMD_RNID      0x78
 #define ELS_CMD_LIRR      0x7A
+#define ELS_CMD_LCB	  0x81
 #endif
 
 /*
@@ -1010,6 +1014,198 @@ typedef struct _ELS_PKT {	/* Structure is in Big Endian format */
 	} un;
 } ELS_PKT;
 
+/*
+ * Link Cable Beacon (LCB) ELS Frame
+ */
+
+struct fc_lcb_request_frame {
+	uint32_t      lcb_command;      /* ELS command opcode (0x81)     */
+	uint8_t       lcb_sub_command;/* LCB Payload Word 1, bit 24:31 */
+#define LPFC_LCB_ON    0x1
+#define LPFC_LCB_OFF   0x2
+	uint8_t       reserved[3];
+
+	uint8_t       lcb_type; /* LCB Payload Word 2, bit 24:31 */
+#define LPFC_LCB_GREEN 0x1
+#define LPFC_LCB_AMBER 0x2
+	uint8_t       lcb_frequency;    /* LCB Payload Word 2, bit 16:23 */
+	uint16_t      lcb_duration;     /* LCB Payload Word 2, bit 15:0  */
+};
+
+/*
+ * Link Cable Beacon (LCB) ELS Response Frame
+ */
+struct fc_lcb_res_frame {
+	uint32_t      lcb_ls_acc;       /* Acceptance of LCB request (0x02) */
+	uint8_t       lcb_sub_command;/* LCB Payload Word 1, bit 24:31 */
+	uint8_t       reserved[3];
+	uint8_t       lcb_type; /* LCB Payload Word 2, bit 24:31 */
+	uint8_t       lcb_frequency;    /* LCB Payload Word 2, bit 16:23 */
+	uint16_t      lcb_duration;     /* LCB Payload Word 2, bit 15:0  */
+};
+
+/*
+ * Read Diagnostic Parameters (RDP) ELS frame.
+ */
+#define SFF_PG0_IDENT_SFP              0x3
+
+#define SFP_FLAG_PT_OPTICAL            0x0
+#define SFP_FLAG_PT_SWLASER            0x01
+#define SFP_FLAG_PT_LWLASER_LC1310     0x02
+#define SFP_FLAG_PT_LWLASER_LL1550     0x03
+#define SFP_FLAG_PT_MASK               0x0F
+#define SFP_FLAG_PT_SHIFT              0
+
+#define SFP_FLAG_IS_OPTICAL_PORT       0x01
+#define SFP_FLAG_IS_OPTICAL_MASK       0x010
+#define SFP_FLAG_IS_OPTICAL_SHIFT      4
+
+#define SFP_FLAG_IS_DESC_VALID         0x01
+#define SFP_FLAG_IS_DESC_VALID_MASK    0x020
+#define SFP_FLAG_IS_DESC_VALID_SHIFT   5
+
+#define SFP_FLAG_CT_UNKNOWN            0x0
+#define SFP_FLAG_CT_SFP_PLUS           0x01
+#define SFP_FLAG_CT_MASK               0x3C
+#define SFP_FLAG_CT_SHIFT              6
+
+struct fc_rdp_port_name_info {
+	uint8_t wwnn[8];
+	uint8_t wwpn[8];
+};
+
+
+/*
+ * Link Error Status Block Structure (FC-FS-3) for RDP
+ * This similar to RPS ELS
+ */
+struct fc_link_status {
+	uint32_t      link_failure_cnt;
+	uint32_t      loss_of_synch_cnt;
+	uint32_t      loss_of_signal_cnt;
+	uint32_t      primitive_seq_proto_err;
+	uint32_t      invalid_trans_word;
+	uint32_t      invalid_crc_cnt;
+
+};
+
+#define RDP_PORT_NAMES_DESC_TAG  0x00010003
+struct fc_rdp_port_name_desc {
+	uint32_t	tag;     /* 0001 0003h */
+	uint32_t	length;  /* set to size of payload struct */
+	struct fc_rdp_port_name_info  port_names;
+};
+
+
+struct fc_rdp_link_error_status_payload_info {
+	struct fc_link_status link_status; /* 24 bytes */
+	uint32_t  port_type;             /* bits 31-30 only */
+};
+
+#define RDP_LINK_ERROR_STATUS_DESC_TAG  0x00010002
+struct fc_rdp_link_error_status_desc {
+	uint32_t         tag;     /* 0001 0002h */
+	uint32_t         length;  /* set to size of payload struct */
+	struct fc_rdp_link_error_status_payload_info info;
+};
+
+#define VN_PT_PHY_UNKNOWN      0x00
+#define VN_PT_PHY_PF_PORT      0x01
+#define VN_PT_PHY_ETH_MAC      0x10
+#define VN_PT_PHY_SHIFT                30
+
+#define RDP_PS_1GB             0x8000
+#define RDP_PS_2GB             0x4000
+#define RDP_PS_4GB             0x2000
+#define RDP_PS_10GB            0x1000
+#define RDP_PS_8GB             0x0800
+#define RDP_PS_16GB            0x0400
+#define RDP_PS_32GB            0x0200
+
+#define RDP_CAP_UNKNOWN        0x0001
+#define RDP_PS_UNKNOWN         0x0002
+#define RDP_PS_NOT_ESTABLISHED 0x0001
+
+struct fc_rdp_port_speed {
+	uint16_t   capabilities;
+	uint16_t   speed;
+};
+
+struct fc_rdp_port_speed_info {
+	struct fc_rdp_port_speed   port_speed;
+};
+
+#define RDP_PORT_SPEED_DESC_TAG  0x00010001
+struct fc_rdp_port_speed_desc {
+	uint32_t         tag;            /* 00010001h */
+	uint32_t         length;         /* set to size of payload struct */
+	struct fc_rdp_port_speed_info info;
+};
+
+#define RDP_NPORT_ID_SIZE      4
+#define RDP_N_PORT_DESC_TAG    0x00000003
+struct fc_rdp_nport_desc {
+	uint32_t         tag;          /* 0000 0003h, big endian */
+	uint32_t         length;       /* size of RDP_N_PORT_ID struct */
+	uint32_t         nport_id : 12;
+	uint32_t         reserved : 8;
+};
+
+
+struct fc_rdp_link_service_info {
+	uint32_t         els_req;    /* Request payload word 0 value.*/
+};
+
+#define RDP_LINK_SERVICE_DESC_TAG  0x00000001
+struct fc_rdp_link_service_desc {
+	uint32_t         tag;     /* Descriptor tag  1 */
+	uint32_t         length;  /* set to size of payload struct. */
+	struct fc_rdp_link_service_info  payload;
+				  /* must be ELS req Word 0(0x18) */
+};
+
+struct fc_rdp_sfp_info {
+	uint16_t	temperature;
+	uint16_t	vcc;
+	uint16_t	tx_bias;
+	uint16_t	tx_power;
+	uint16_t	rx_power;
+	uint16_t	flags;
+};
+
+#define RDP_SFP_DESC_TAG  0x00010000
+struct fc_rdp_sfp_desc {
+	uint32_t         tag;
+	uint32_t         length;  /* set to size of sfp_info struct */
+	struct fc_rdp_sfp_info sfp_info;
+};
+
+struct fc_rdp_req_frame {
+	uint32_t         rdp_command;           /* ELS command opcode (0x18)*/
+	uint32_t         rdp_des_length;        /* RDP Payload Word 1 */
+	struct fc_rdp_nport_desc nport_id_desc; /* RDP Payload Word 2 - 4 */
+};
+
+
+struct fc_rdp_res_frame {
+	uint32_t	reply_sequence;		/* FC word0 LS_ACC or LS_RJT */
+	uint32_t	length;			/* FC Word 1      */
+	struct fc_rdp_link_service_desc link_service_desc;    /* Word 2 -4  */
+	struct fc_rdp_sfp_desc sfp_desc;                      /* Word 5 -9  */
+	struct fc_rdp_port_speed_desc portspeed_desc;         /* Word 10-12 */
+	struct fc_rdp_link_error_status_desc link_error_desc; /* Word 13-21 */
+	struct fc_rdp_port_name_desc diag_port_names_desc;    /* Word 22-27 */
+	struct fc_rdp_port_name_desc attached_port_names_desc;/* Word 28-33 */
+};
+
+
+#define RDP_DESC_PAYLOAD_SIZE (sizeof(struct fc_rdp_link_service_desc) \
+			+ sizeof(struct fc_rdp_sfp_desc) \
+			+ sizeof(struct fc_rdp_port_speed_desc) \
+			+ sizeof(struct fc_rdp_link_error_status_desc) \
+			+ (sizeof(struct fc_rdp_port_name_desc) * 2))
+
+
 /******** FDMI ********/
 
 /* lpfc_sli_ct_request defines the CT_IU preamble for FDMI commands */
@@ -1587,6 +1783,11 @@ typedef struct {		/* FireFly BIU registers */
 #define TEMPERATURE_OFFSET 0xB0	/* Slim offset for critical temperature event */
 
 /*
+ * return code Fail
+ */
+#define FAILURE 1
+
+/*
  *    Begin Structure Definitions for Mailbox Commands
  */
 
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 1813c45946f4..33ec4fa39ccb 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -291,7 +291,7 @@ struct sli4_bls_rsp {
 struct lpfc_eqe {
 	uint32_t word0;
 #define lpfc_eqe_resource_id_SHIFT	16
-#define lpfc_eqe_resource_id_MASK	0x000000FF
+#define lpfc_eqe_resource_id_MASK	0x0000FFFF
 #define lpfc_eqe_resource_id_WORD	word0
 #define lpfc_eqe_minor_code_SHIFT	4
 #define lpfc_eqe_minor_code_MASK	0x00000FFF
@@ -914,6 +914,8 @@ struct mbox_header {
 #define LPFC_MBOX_OPCODE_FUNCTION_RESET			0x3D
 #define LPFC_MBOX_OPCODE_SET_PHYSICAL_LINK_CONFIG	0x3E
 #define LPFC_MBOX_OPCODE_SET_BOOT_CONFIG		0x43
+#define LPFC_MBOX_OPCODE_SET_BEACON_CONFIG              0x45
+#define LPFC_MBOX_OPCODE_GET_BEACON_CONFIG              0x46
 #define LPFC_MBOX_OPCODE_GET_PORT_NAME			0x4D
 #define LPFC_MBOX_OPCODE_MQ_CREATE_EXT			0x5A
 #define LPFC_MBOX_OPCODE_GET_VPD_DATA			0x5B
@@ -1479,6 +1481,26 @@ struct lpfc_mbx_query_fw_config {
 	} rsp;
 };
 
+struct lpfc_mbx_set_beacon_config {
+	struct mbox_header header;
+	uint32_t word4;
+#define lpfc_mbx_set_beacon_port_num_SHIFT		0
+#define lpfc_mbx_set_beacon_port_num_MASK		0x0000003F
+#define lpfc_mbx_set_beacon_port_num_WORD		word4
+#define lpfc_mbx_set_beacon_port_type_SHIFT		6
+#define lpfc_mbx_set_beacon_port_type_MASK		0x00000003
+#define lpfc_mbx_set_beacon_port_type_WORD		word4
+#define lpfc_mbx_set_beacon_state_SHIFT			8
+#define lpfc_mbx_set_beacon_state_MASK			0x000000FF
+#define lpfc_mbx_set_beacon_state_WORD			word4
+#define lpfc_mbx_set_beacon_duration_SHIFT		16
+#define lpfc_mbx_set_beacon_duration_MASK		0x000000FF
+#define lpfc_mbx_set_beacon_duration_WORD		word4
+#define lpfc_mbx_set_beacon_status_duration_SHIFT	24
+#define lpfc_mbx_set_beacon_status_duration_MASK	0x000000FF
+#define lpfc_mbx_set_beacon_status_duration_WORD	word4
+};
+
 struct lpfc_id_range {
 	uint32_t word5;
 #define lpfc_mbx_rsrc_id_word4_0_SHIFT	0
@@ -1921,6 +1943,12 @@ struct lpfc_mbx_redisc_fcf_tbl {
 #define STATUS_FCF_IN_USE				0x3a
 #define STATUS_FCF_TABLE_EMPTY				0x43
 
+/*
+ * Additional status field for embedded SLI_CONFIG mailbox
+ * command.
+ */
+#define ADD_STATUS_OPERATION_ALREADY_ACTIVE		0x67
+
 struct lpfc_mbx_sli4_config {
 	struct mbox_header header;
 };
@@ -2433,6 +2461,205 @@ struct lpfc_mbx_supp_pages {
 #define LPFC_SLI4_PARAMETERS		2
 };
 
+struct lpfc_mbx_memory_dump_type3 {
+	uint32_t word1;
+#define lpfc_mbx_memory_dump_type3_type_SHIFT    0
+#define lpfc_mbx_memory_dump_type3_type_MASK     0x0000000f
+#define lpfc_mbx_memory_dump_type3_type_WORD     word1
+#define lpfc_mbx_memory_dump_type3_link_SHIFT    24
+#define lpfc_mbx_memory_dump_type3_link_MASK     0x000000ff
+#define lpfc_mbx_memory_dump_type3_link_WORD     word1
+	uint32_t word2;
+#define lpfc_mbx_memory_dump_type3_page_no_SHIFT  0
+#define lpfc_mbx_memory_dump_type3_page_no_MASK   0x0000ffff
+#define lpfc_mbx_memory_dump_type3_page_no_WORD   word2
+#define lpfc_mbx_memory_dump_type3_offset_SHIFT   16
+#define lpfc_mbx_memory_dump_type3_offset_MASK    0x0000ffff
+#define lpfc_mbx_memory_dump_type3_offset_WORD    word2
+	uint32_t word3;
+#define lpfc_mbx_memory_dump_type3_length_SHIFT  0
+#define lpfc_mbx_memory_dump_type3_length_MASK   0x00ffffff
+#define lpfc_mbx_memory_dump_type3_length_WORD   word3
+	uint32_t addr_lo;
+	uint32_t addr_hi;
+	uint32_t return_len;
+};
+
+#define DMP_PAGE_A0             0xa0
+#define DMP_PAGE_A2             0xa2
+#define DMP_SFF_PAGE_A0_SIZE	256
+#define DMP_SFF_PAGE_A2_SIZE	256
+
+#define SFP_WAVELENGTH_LC1310	1310
+#define SFP_WAVELENGTH_LL1550	1550
+
+
+/*
+ *  * SFF-8472 TABLE 3.4
+ *   */
+#define  SFF_PG0_CONNECTOR_UNKNOWN    0x00   /* Unknown  */
+#define  SFF_PG0_CONNECTOR_SC         0x01   /* SC       */
+#define  SFF_PG0_CONNECTOR_FC_COPPER1 0x02   /* FC style 1 copper connector */
+#define  SFF_PG0_CONNECTOR_FC_COPPER2 0x03   /* FC style 2 copper connector */
+#define  SFF_PG0_CONNECTOR_BNC        0x04   /* BNC / TNC */
+#define  SFF_PG0_CONNECTOR__FC_COAX   0x05   /* FC coaxial headers */
+#define  SFF_PG0_CONNECTOR_FIBERJACK  0x06   /* FiberJack */
+#define  SFF_PG0_CONNECTOR_LC         0x07   /* LC        */
+#define  SFF_PG0_CONNECTOR_MT         0x08   /* MT - RJ   */
+#define  SFF_PG0_CONNECTOR_MU         0x09   /* MU        */
+#define  SFF_PG0_CONNECTOR_SF         0x0A   /* SG        */
+#define  SFF_PG0_CONNECTOR_OPTICAL_PIGTAIL 0x0B /* Optical pigtail */
+#define  SFF_PG0_CONNECTOR_OPTICAL_PARALLEL 0x0C /* MPO Parallel Optic */
+#define  SFF_PG0_CONNECTOR_HSSDC_II   0x20   /* HSSDC II */
+#define  SFF_PG0_CONNECTOR_COPPER_PIGTAIL 0x21 /* Copper pigtail */
+#define  SFF_PG0_CONNECTOR_RJ45       0x22  /* RJ45 */
+
+/* SFF-8472 Table 3.1 Diagnostics: Data Fields Address/Page A0 */
+
+#define SSF_IDENTIFIER			0
+#define SSF_EXT_IDENTIFIER		1
+#define SSF_CONNECTOR			2
+#define SSF_TRANSCEIVER_CODE_B0		3
+#define SSF_TRANSCEIVER_CODE_B1		4
+#define SSF_TRANSCEIVER_CODE_B2		5
+#define SSF_TRANSCEIVER_CODE_B3		6
+#define SSF_TRANSCEIVER_CODE_B4		7
+#define SSF_TRANSCEIVER_CODE_B5		8
+#define SSF_TRANSCEIVER_CODE_B6		9
+#define SSF_TRANSCEIVER_CODE_B7		10
+#define SSF_ENCODING			11
+#define SSF_BR_NOMINAL			12
+#define SSF_RATE_IDENTIFIER		13
+#define SSF_LENGTH_9UM_KM		14
+#define SSF_LENGTH_9UM			15
+#define SSF_LENGTH_50UM_OM2		16
+#define SSF_LENGTH_62UM_OM1		17
+#define SFF_LENGTH_COPPER		18
+#define SSF_LENGTH_50UM_OM3		19
+#define SSF_VENDOR_NAME			20
+#define SSF_VENDOR_OUI			36
+#define SSF_VENDOR_PN			40
+#define SSF_VENDOR_REV			56
+#define SSF_WAVELENGTH_B1		60
+#define SSF_WAVELENGTH_B0		61
+#define SSF_CC_BASE			63
+#define SSF_OPTIONS_B1			64
+#define SSF_OPTIONS_B0			65
+#define SSF_BR_MAX			66
+#define SSF_BR_MIN			67
+#define SSF_VENDOR_SN			68
+#define SSF_DATE_CODE			84
+#define SSF_MONITORING_TYPEDIAGNOSTIC	92
+#define SSF_ENHANCED_OPTIONS		93
+#define SFF_8472_COMPLIANCE		94
+#define SSF_CC_EXT			95
+#define SSF_A0_VENDOR_SPECIFIC		96
+
+/* SFF-8472 Table 3.1a Diagnostics: Data Fields Address/Page A2 */
+
+#define SSF_AW_THRESHOLDS		0
+#define SSF_EXT_CAL_CONSTANTS		56
+#define SSF_CC_DMI			95
+#define SFF_TEMPERATURE_B1		96
+#define SFF_TEMPERATURE_B0		97
+#define SFF_VCC_B1			98
+#define SFF_VCC_B0			99
+#define SFF_TX_BIAS_CURRENT_B1		100
+#define SFF_TX_BIAS_CURRENT_B0		101
+#define SFF_TXPOWER_B1			102
+#define SFF_TXPOWER_B0			103
+#define SFF_RXPOWER_B1			104
+#define SFF_RXPOWER_B0			105
+#define SSF_STATUS_CONTROL		110
+#define SSF_ALARM_FLAGS_B1		112
+#define SSF_ALARM_FLAGS_B0		113
+#define SSF_WARNING_FLAGS_B1		116
+#define SSF_WARNING_FLAGS_B0		117
+#define SSF_EXT_TATUS_CONTROL_B1	118
+#define SSF_EXT_TATUS_CONTROL_B0	119
+#define SSF_A2_VENDOR_SPECIFIC		120
+#define SSF_USER_EEPROM			128
+#define SSF_VENDOR_CONTROL		148
+
+
+/*
+ * Tranceiver codes Fibre Channel SFF-8472
+ * Table 3.5.
+ */
+
+struct sff_trasnceiver_codes_byte0 {
+	uint8_t inifiband:4;
+	uint8_t teng_ethernet:4;
+};
+
+struct sff_trasnceiver_codes_byte1 {
+	uint8_t  sonet:6;
+	uint8_t  escon:2;
+};
+
+struct sff_trasnceiver_codes_byte2 {
+	uint8_t  soNet:8;
+};
+
+struct sff_trasnceiver_codes_byte3 {
+	uint8_t ethernet:8;
+};
+
+struct sff_trasnceiver_codes_byte4 {
+	uint8_t fc_el_lo:1;
+	uint8_t fc_lw_laser:1;
+	uint8_t fc_sw_laser:1;
+	uint8_t fc_md_distance:1;
+	uint8_t fc_lg_distance:1;
+	uint8_t fc_int_distance:1;
+	uint8_t fc_short_distance:1;
+	uint8_t fc_vld_distance:1;
+};
+
+struct sff_trasnceiver_codes_byte5 {
+	uint8_t reserved1:1;
+	uint8_t reserved2:1;
+	uint8_t fc_sfp_active:1;  /* Active cable   */
+	uint8_t fc_sfp_passive:1; /* Passive cable  */
+	uint8_t fc_lw_laser:1;     /* Longwave laser */
+	uint8_t fc_sw_laser_sl:1;
+	uint8_t fc_sw_laser_sn:1;
+	uint8_t fc_el_hi:1;        /* Electrical enclosure high bit */
+};
+
+struct sff_trasnceiver_codes_byte6 {
+	uint8_t fc_tm_sm:1;      /* Single Mode */
+	uint8_t reserved:1;
+	uint8_t fc_tm_m6:1;       /* Multimode, 62.5um (M6) */
+	uint8_t fc_tm_tv:1;      /* Video Coax (TV) */
+	uint8_t fc_tm_mi:1;      /* Miniature Coax (MI) */
+	uint8_t fc_tm_tp:1;      /* Twisted Pair (TP) */
+	uint8_t fc_tm_tw:1;      /* Twin Axial Pair  */
+};
+
+struct sff_trasnceiver_codes_byte7 {
+	uint8_t fc_sp_100MB:1;   /*  100 MB/sec */
+	uint8_t reserve:1;
+	uint8_t fc_sp_200mb:1;   /*  200 MB/sec */
+	uint8_t fc_sp_3200MB:1;  /* 3200 MB/sec */
+	uint8_t fc_sp_400MB:1;   /*  400 MB/sec */
+	uint8_t fc_sp_1600MB:1;  /* 1600 MB/sec */
+	uint8_t fc_sp_800MB:1;   /*  800 MB/sec */
+	uint8_t fc_sp_1200MB:1;  /* 1200 MB/sec */
+};
+
+/* User writable non-volatile memory, SFF-8472 Table 3.20 */
+struct user_eeprom {
+	uint8_t vendor_name[16];
+	uint8_t vendor_oui[3];
+	uint8_t vendor_pn[816];
+	uint8_t vendor_rev[4];
+	uint8_t vendor_sn[16];
+	uint8_t datecode[6];
+	uint8_t lot_code[2];
+	uint8_t reserved191[57];
+};
+
 struct lpfc_mbx_pc_sli4_params {
 	uint32_t word1;
 #define qs_SHIFT				0
@@ -3021,6 +3248,7 @@ struct lpfc_mqe {
 		struct lpfc_mbx_request_features req_ftrs;
 		struct lpfc_mbx_post_hdr_tmpl hdr_tmpl;
 		struct lpfc_mbx_query_fw_config query_fw_cfg;
+		struct lpfc_mbx_set_beacon_config beacon_config;
 		struct lpfc_mbx_supp_pages supp_pages;
 		struct lpfc_mbx_pc_sli4_params sli4_params;
 		struct lpfc_mbx_get_sli4_parameters get_sli4_parameters;
@@ -3031,6 +3259,7 @@ struct lpfc_mqe {
 		struct lpfc_mbx_get_prof_cfg get_prof_cfg;
 		struct lpfc_mbx_wr_object wr_object;
 		struct lpfc_mbx_get_port_name get_port_name;
+		struct lpfc_mbx_memory_dump_type3 mem_dump_type3;
 		struct lpfc_mbx_nop nop;
 	} un;
 };
@@ -3041,8 +3270,8 @@ struct lpfc_mcqe {
 #define lpfc_mcqe_status_MASK		0x0000FFFF
 #define lpfc_mcqe_status_WORD		word0
 #define lpfc_mcqe_ext_status_SHIFT	16
-#define lpfc_mcqe_ext_status_MASK  	0x0000FFFF
-#define lpfc_mcqe_ext_status_WORD 	word0
+#define lpfc_mcqe_ext_status_MASK	0x0000FFFF
+#define lpfc_mcqe_ext_status_WORD	word0
 	uint32_t mcqe_tag0;
 	uint32_t mcqe_tag1;
 	uint32_t trailer;
@@ -3176,6 +3405,7 @@ struct lpfc_acqe_fc_la {
 #define LPFC_FC_LA_SPEED_8G		0x8
 #define LPFC_FC_LA_SPEED_10G		0xA
 #define LPFC_FC_LA_SPEED_16G		0x10
+#define LPFC_FC_LA_SPEED_32G            0x20
 #define lpfc_acqe_fc_la_topology_SHIFT		16
 #define lpfc_acqe_fc_la_topology_MASK		0x000000FF
 #define lpfc_acqe_fc_la_topology_WORD		word0
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index e8c8c1ecc1f5..f962118da8ed 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -3303,6 +3303,7 @@ lpfc_create_port(struct lpfc_hba *phba, int instance, struct device *dev)
 	shost->max_lun = vport->cfg_max_luns;
 	shost->this_id = -1;
 	shost->max_cmd_len = 16;
+	shost->nr_hw_queues = phba->cfg_fcp_io_channel;
 	if (phba->sli_rev == LPFC_SLI_REV4) {
 		shost->dma_boundary =
 			phba->sli4_hba.pc_sli4_params.sge_supp_len-1;
@@ -4483,7 +4484,13 @@ lpfc_sli4_async_fip_evt(struct lpfc_hba *phba,
 			lpfc_destroy_vport_work_array(phba, vports);
 		}
 
-		if (active_vlink_present) {
+		/*
+		 * Don't re-instantiate if vport is marked for deletion.
+		 * If we are here first then vport_delete is going to wait
+		 * for discovery to complete.
+		 */
+		if (!(vport->load_flag & FC_UNLOADING) &&
+					active_vlink_present) {
 			/*
 			 * If there are other active VLinks present,
 			 * re-instantiate the Vlink using FDISC.
@@ -7500,6 +7507,8 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 			mboxq->u.mqe.un.query_fw_cfg.rsp.function_mode;
 	phba->sli4_hba.ulp0_mode = mboxq->u.mqe.un.query_fw_cfg.rsp.ulp0_mode;
 	phba->sli4_hba.ulp1_mode = mboxq->u.mqe.un.query_fw_cfg.rsp.ulp1_mode;
+	phba->sli4_hba.physical_port =
+			mboxq->u.mqe.un.query_fw_cfg.rsp.physical_port;
 	lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
 			"3251 QUERY_FW_CFG: func_mode:x%x, ulp0_mode:x%x, "
 			"ulp1_mode:x%x\n", phba->sli4_hba.fw_func_mode,
@@ -8367,7 +8376,7 @@ lpfc_sli_enable_msix(struct lpfc_hba *phba)
 
 	/* vector-0 is associated to slow-path handler */
 	rc = request_irq(phba->msix_entries[0].vector,
-			 &lpfc_sli_sp_intr_handler, IRQF_SHARED,
+			 &lpfc_sli_sp_intr_handler, 0,
 			 LPFC_SP_DRIVER_HANDLER_NAME, phba);
 	if (rc) {
 		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
@@ -8378,7 +8387,7 @@ lpfc_sli_enable_msix(struct lpfc_hba *phba)
 
 	/* vector-1 is associated to fast-path handler */
 	rc = request_irq(phba->msix_entries[1].vector,
-			 &lpfc_sli_fp_intr_handler, IRQF_SHARED,
+			 &lpfc_sli_fp_intr_handler, 0,
 			 LPFC_FP_DRIVER_HANDLER_NAME, phba);
 
 	if (rc) {
@@ -8487,7 +8496,7 @@ lpfc_sli_enable_msi(struct lpfc_hba *phba)
 	}
 
 	rc = request_irq(phba->pcidev->irq, lpfc_sli_intr_handler,
-			 IRQF_SHARED, LPFC_DRIVER_NAME, phba);
+			 0, LPFC_DRIVER_NAME, phba);
 	if (rc) {
 		pci_disable_msi(phba->pcidev);
 		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
@@ -8944,13 +8953,13 @@ lpfc_sli4_enable_msix(struct lpfc_hba *phba)
 		if (phba->cfg_fof && (index == (vectors - 1)))
 			rc = request_irq(
 				phba->sli4_hba.msix_entries[index].vector,
-				 &lpfc_sli4_fof_intr_handler, IRQF_SHARED,
+				 &lpfc_sli4_fof_intr_handler, 0,
 				 (char *)&phba->sli4_hba.handler_name[index],
 				 &phba->sli4_hba.fcp_eq_hdl[index]);
 		else
 			rc = request_irq(
 				phba->sli4_hba.msix_entries[index].vector,
-				 &lpfc_sli4_hba_intr_handler, IRQF_SHARED,
+				 &lpfc_sli4_hba_intr_handler, 0,
 				 (char *)&phba->sli4_hba.handler_name[index],
 				 &phba->sli4_hba.fcp_eq_hdl[index]);
 		if (rc) {
@@ -8972,7 +8981,8 @@ lpfc_sli4_enable_msix(struct lpfc_hba *phba)
 		phba->cfg_fcp_io_channel = vectors;
 	}
 
-	lpfc_sli4_set_affinity(phba, vectors);
+	if (!shost_use_blk_mq(lpfc_shost_from_vport(phba->pport)))
+		lpfc_sli4_set_affinity(phba, vectors);
 	return rc;
 
 cfg_fail_out:
@@ -9050,7 +9060,7 @@ lpfc_sli4_enable_msi(struct lpfc_hba *phba)
 	}
 
 	rc = request_irq(phba->pcidev->irq, lpfc_sli4_intr_handler,
-			 IRQF_SHARED, LPFC_DRIVER_NAME, phba);
+			 0, LPFC_DRIVER_NAME, phba);
 	if (rc) {
 		pci_disable_msi(phba->pcidev);
 		lpfc_printf_log(phba, KERN_WARNING, LOG_INIT,
diff --git a/drivers/scsi/lpfc/lpfc_mbox.c b/drivers/scsi/lpfc/lpfc_mbox.c
index 816f596cda60..eb627724417e 100644
--- a/drivers/scsi/lpfc/lpfc_mbox.c
+++ b/drivers/scsi/lpfc/lpfc_mbox.c
@@ -2255,6 +2255,158 @@ lpfc_sli4_dump_cfg_rg23(struct lpfc_hba *phba, struct lpfcMboxq *mbox)
 	return 0;
 }
 
+void
+lpfc_mbx_cmpl_rdp_link_stat(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
+{
+	MAILBOX_t *mb;
+	int rc = FAILURE;
+	struct lpfc_rdp_context *rdp_context =
+			(struct lpfc_rdp_context *)(mboxq->context2);
+
+	mb = &mboxq->u.mb;
+	if (mb->mbxStatus)
+		goto mbx_failed;
+
+	memcpy(&rdp_context->link_stat, &mb->un.varRdLnk, sizeof(READ_LNK_VAR));
+
+	rc = SUCCESS;
+
+mbx_failed:
+	lpfc_sli4_mbox_cmd_free(phba, mboxq);
+	rdp_context->cmpl(phba, rdp_context, rc);
+}
+
+void
+lpfc_mbx_cmpl_rdp_page_a2(struct lpfc_hba *phba, LPFC_MBOXQ_t *mbox)
+{
+	struct lpfc_dmabuf *mp = (struct lpfc_dmabuf *) mbox->context1;
+	struct lpfc_rdp_context *rdp_context =
+			(struct lpfc_rdp_context *)(mbox->context2);
+
+	if (bf_get(lpfc_mqe_status, &mbox->u.mqe))
+		goto error;
+
+	lpfc_sli_bemem_bcopy(mp->virt, &rdp_context->page_a2,
+				DMP_SFF_PAGE_A2_SIZE);
+
+	/* We don't need dma buffer for link stat. */
+	lpfc_mbuf_free(phba, mp->virt, mp->phys);
+	kfree(mp);
+
+	memset(mbox, 0, sizeof(*mbox));
+	lpfc_read_lnk_stat(phba, mbox);
+	mbox->vport = rdp_context->ndlp->vport;
+	mbox->mbox_cmpl = lpfc_mbx_cmpl_rdp_link_stat;
+	mbox->context2 = (struct lpfc_rdp_context *) rdp_context;
+	if (lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT) == MBX_NOT_FINISHED)
+		goto error;
+
+	return;
+
+error:
+	lpfc_mbuf_free(phba, mp->virt, mp->phys);
+	kfree(mp);
+	lpfc_sli4_mbox_cmd_free(phba, mbox);
+	rdp_context->cmpl(phba, rdp_context, FAILURE);
+}
+
+void
+lpfc_mbx_cmpl_rdp_page_a0(struct lpfc_hba *phba, LPFC_MBOXQ_t *mbox)
+{
+	int rc;
+	struct lpfc_dmabuf *mp = (struct lpfc_dmabuf *) (mbox->context1);
+	struct lpfc_rdp_context *rdp_context =
+			(struct lpfc_rdp_context *)(mbox->context2);
+
+	if (bf_get(lpfc_mqe_status, &mbox->u.mqe))
+		goto error;
+
+	lpfc_sli_bemem_bcopy(mp->virt, &rdp_context->page_a0,
+				DMP_SFF_PAGE_A0_SIZE);
+
+	memset(mbox, 0, sizeof(*mbox));
+
+	memset(mp->virt, 0, DMP_SFF_PAGE_A2_SIZE);
+	INIT_LIST_HEAD(&mp->list);
+
+	/* save address for completion */
+	mbox->context1 = mp;
+	mbox->vport = rdp_context->ndlp->vport;
+
+	bf_set(lpfc_mqe_command, &mbox->u.mqe, MBX_DUMP_MEMORY);
+	bf_set(lpfc_mbx_memory_dump_type3_type,
+		&mbox->u.mqe.un.mem_dump_type3, DMP_LMSD);
+	bf_set(lpfc_mbx_memory_dump_type3_link,
+		&mbox->u.mqe.un.mem_dump_type3, phba->sli4_hba.physical_port);
+	bf_set(lpfc_mbx_memory_dump_type3_page_no,
+		&mbox->u.mqe.un.mem_dump_type3, DMP_PAGE_A2);
+	bf_set(lpfc_mbx_memory_dump_type3_length,
+		&mbox->u.mqe.un.mem_dump_type3, DMP_SFF_PAGE_A2_SIZE);
+	mbox->u.mqe.un.mem_dump_type3.addr_lo = putPaddrLow(mp->phys);
+	mbox->u.mqe.un.mem_dump_type3.addr_hi = putPaddrHigh(mp->phys);
+
+	mbox->mbox_cmpl = lpfc_mbx_cmpl_rdp_page_a2;
+	mbox->context2 = (struct lpfc_rdp_context *) rdp_context;
+	rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
+	if (rc == MBX_NOT_FINISHED)
+		goto error;
+
+	return;
+
+error:
+	lpfc_mbuf_free(phba, mp->virt, mp->phys);
+	kfree(mp);
+	lpfc_sli4_mbox_cmd_free(phba, mbox);
+	rdp_context->cmpl(phba, rdp_context, FAILURE);
+}
+
+
+/*
+ * lpfc_sli4_dump_sfp_pagea0 - Dump sli4 read SFP Diagnostic.
+ * @phba: pointer to the hba structure containing.
+ * @mbox: pointer to lpfc mbox command to initialize.
+ *
+ * This function create a SLI4 dump mailbox command to dump configure
+ * type 3 page 0xA0.
+ */
+int
+lpfc_sli4_dump_page_a0(struct lpfc_hba *phba, struct lpfcMboxq *mbox)
+{
+	struct lpfc_dmabuf *mp = NULL;
+
+	memset(mbox, 0, sizeof(*mbox));
+
+	mp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
+	if (mp)
+		mp->virt = lpfc_mbuf_alloc(phba, 0, &mp->phys);
+	if (!mp || !mp->virt) {
+		kfree(mp);
+		lpfc_printf_log(phba, KERN_WARNING, LOG_MBOX,
+			"3569 dump type 3 page 0xA0 allocation failed\n");
+		return 1;
+	}
+
+	memset(mp->virt, 0, LPFC_BPL_SIZE);
+	INIT_LIST_HEAD(&mp->list);
+
+	bf_set(lpfc_mqe_command, &mbox->u.mqe, MBX_DUMP_MEMORY);
+	/* save address for completion */
+	mbox->context1 = mp;
+
+	bf_set(lpfc_mbx_memory_dump_type3_type,
+		&mbox->u.mqe.un.mem_dump_type3, DMP_LMSD);
+	bf_set(lpfc_mbx_memory_dump_type3_link,
+		&mbox->u.mqe.un.mem_dump_type3, phba->sli4_hba.physical_port);
+	bf_set(lpfc_mbx_memory_dump_type3_page_no,
+		&mbox->u.mqe.un.mem_dump_type3, DMP_PAGE_A0);
+	bf_set(lpfc_mbx_memory_dump_type3_length,
+		&mbox->u.mqe.un.mem_dump_type3, DMP_SFF_PAGE_A0_SIZE);
+	mbox->u.mqe.un.mem_dump_type3.addr_lo = putPaddrLow(mp->phys);
+	mbox->u.mqe.un.mem_dump_type3.addr_hi = putPaddrHigh(mp->phys);
+
+	return 0;
+}
+
 /**
  * lpfc_reg_fcfi - Initialize the REG_FCFI mailbox command
  * @phba: pointer to the hba structure containing the FCF index and RQ ID.
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index 4cb9882af157..af3b38aba65e 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -661,7 +661,13 @@ lpfc_rcv_logo(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 			lpfc_destroy_vport_work_array(phba, vports);
 		}
 
-		if (active_vlink_present) {
+		/*
+		 * Don't re-instantiate if vport is marked for deletion.
+		 * If we are here first then vport_delete is going to wait
+		 * for discovery to complete.
+		 */
+		if (!(vport->load_flag & FC_UNLOADING) &&
+					active_vlink_present) {
 			/*
 			 * If there are other active VLinks present,
 			 * re-instantiate the Vlink using FDISC.
@@ -1868,7 +1874,7 @@ lpfc_rcv_logo_logo_issue(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	struct lpfc_iocbq *cmdiocb = (struct lpfc_iocbq *)arg;
 
 	spin_lock_irq(shost->host_lock);
-	ndlp->nlp_flag &= NLP_LOGO_ACC;
+	ndlp->nlp_flag |= NLP_LOGO_ACC;
 	spin_unlock_irq(shost->host_lock);
 	lpfc_els_rsp_acc(vport, ELS_CMD_ACC, cmdiocb, ndlp, NULL);
 	return ndlp->nlp_state;
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index c140f99772ca..e5eb40d2c512 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -3257,7 +3257,7 @@ lpfc_scsi_prep_dma_buf_s4(struct lpfc_hba *phba, struct lpfc_scsi_buf *lpfc_cmd)
 		 */
 
 		nseg = scsi_dma_map(scsi_cmnd);
-		if (unlikely(!nseg))
+		if (unlikely(nseg <= 0))
 			return 1;
 		sgl += 1;
 		/* clear the last flag in the fcp_rsp map entry */
@@ -3846,6 +3846,49 @@ lpfc_handle_fcp_err(struct lpfc_vport *vport, struct lpfc_scsi_buf *lpfc_cmd,
 }
 
 /**
+ * lpfc_sli4_scmd_to_wqidx_distr - scsi command to SLI4 WQ index distribution
+ * @phba: Pointer to HBA context object.
+ *
+ * This routine performs a roundrobin SCSI command to SLI4 FCP WQ index
+ * distribution.  This is called by __lpfc_sli_issue_iocb_s4() with the hbalock
+ * held.
+ * If scsi-mq is enabled, get the default block layer mapping of software queues
+ * to hardware queues. This information is saved in request tag.
+ *
+ * Return: index into SLI4 fast-path FCP queue index.
+ **/
+int lpfc_sli4_scmd_to_wqidx_distr(struct lpfc_hba *phba,
+				  struct lpfc_scsi_buf *lpfc_cmd)
+{
+	struct scsi_cmnd *cmnd = lpfc_cmd->pCmd;
+	struct lpfc_vector_map_info *cpup;
+	int chann, cpu;
+	uint32_t tag;
+	uint16_t hwq;
+
+	if (shost_use_blk_mq(cmnd->device->host)) {
+		tag = blk_mq_unique_tag(cmnd->request);
+		hwq = blk_mq_unique_tag_to_hwq(tag);
+
+		return hwq;
+	}
+
+	if (phba->cfg_fcp_io_sched == LPFC_FCP_SCHED_BY_CPU
+	    && phba->cfg_fcp_io_channel > 1) {
+		cpu = smp_processor_id();
+		if (cpu < phba->sli4_hba.num_present_cpu) {
+			cpup = phba->sli4_hba.cpu_map;
+			cpup += cpu;
+			return cpup->channel_id;
+		}
+	}
+	chann = atomic_add_return(1, &phba->fcp_qidx);
+	chann = (chann % phba->cfg_fcp_io_channel);
+	return chann;
+}
+
+
+/**
  * lpfc_scsi_cmd_iocb_cmpl - Scsi cmnd IOCB completion routine
  * @phba: The Hba for which this call is being executed.
  * @pIocbIn: The command IOCBQ for the scsi cmnd.
@@ -4537,7 +4580,7 @@ lpfc_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *cmnd)
 	if (lpfc_cmd == NULL) {
 		lpfc_rampdown_queue_depth(phba);
 
-		lpfc_printf_vlog(vport, KERN_INFO, LOG_FCP,
+		lpfc_printf_vlog(vport, KERN_INFO, LOG_MISC,
 				 "0707 driver's buffer pool is empty, "
 				 "IO busied\n");
 		goto out_host_busy;
@@ -4968,13 +5011,16 @@ lpfc_send_taskmgmt(struct lpfc_vport *vport, struct lpfc_rport_data *rdata,
 					  iocbq, iocbqrsp, lpfc_cmd->timeout);
 	if ((status != IOCB_SUCCESS) ||
 	    (iocbqrsp->iocb.ulpStatus != IOSTAT_SUCCESS)) {
-		lpfc_printf_vlog(vport, KERN_ERR, LOG_FCP,
-			 "0727 TMF %s to TGT %d LUN %llu failed (%d, %d) "
-			 "iocb_flag x%x\n",
-			 lpfc_taskmgmt_name(task_mgmt_cmd),
-			 tgt_id, lun_id, iocbqrsp->iocb.ulpStatus,
-			 iocbqrsp->iocb.un.ulpWord[4],
-			 iocbq->iocb_flag);
+		if (status != IOCB_SUCCESS ||
+		    iocbqrsp->iocb.ulpStatus != IOSTAT_FCP_RSP_ERROR)
+			lpfc_printf_vlog(vport, KERN_ERR, LOG_FCP,
+					 "0727 TMF %s to TGT %d LUN %llu "
+					 "failed (%d, %d) iocb_flag x%x\n",
+					 lpfc_taskmgmt_name(task_mgmt_cmd),
+					 tgt_id, lun_id,
+					 iocbqrsp->iocb.ulpStatus,
+					 iocbqrsp->iocb.un.ulpWord[4],
+					 iocbq->iocb_flag);
 		/* if ulpStatus != IOCB_SUCCESS, then status == IOCB_SUCCESS */
 		if (status == IOCB_SUCCESS) {
 			if (iocbqrsp->iocb.ulpStatus == IOSTAT_FCP_RSP_ERROR)
@@ -4988,7 +5034,6 @@ lpfc_send_taskmgmt(struct lpfc_vport *vport, struct lpfc_rport_data *rdata,
 		} else {
 			ret = FAILED;
 		}
-		lpfc_cmd->status = IOSTAT_DRIVER_REJECT;
 	} else
 		ret = SUCCESS;
 
diff --git a/drivers/scsi/lpfc/lpfc_scsi.h b/drivers/scsi/lpfc/lpfc_scsi.h
index 474e30cdee6e..18b9260ccfac 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.h
+++ b/drivers/scsi/lpfc/lpfc_scsi.h
@@ -184,3 +184,6 @@ struct lpfc_scsi_buf {
 #define FIND_FIRST_OAS_LUN		 0
 #define NO_MORE_OAS_LUN			-1
 #define NOT_OAS_ENABLED_LUN		NO_MORE_OAS_LUN
+
+int lpfc_sli4_scmd_to_wqidx_distr(struct lpfc_hba *phba,
+				  struct lpfc_scsi_buf *lpfc_cmd);
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 56f73682d4bd..4feb9312a447 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -2249,7 +2249,7 @@ lpfc_sli4_unreg_rpi_cmpl_clr(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 						 vport->vpi, ndlp->nlp_rpi,
 						 ndlp->nlp_DID,
 						 ndlp->nlp_usg_map, ndlp);
-
+				ndlp->nlp_flag &= ~NLP_LOGO_ACC;
 				lpfc_nlp_put(ndlp);
 			}
 		}
@@ -8138,36 +8138,6 @@ lpfc_sli4_bpl2sgl(struct lpfc_hba *phba, struct lpfc_iocbq *piocbq,
 }
 
 /**
- * lpfc_sli4_scmd_to_wqidx_distr - scsi command to SLI4 WQ index distribution
- * @phba: Pointer to HBA context object.
- *
- * This routine performs a roundrobin SCSI command to SLI4 FCP WQ index
- * distribution.  This is called by __lpfc_sli_issue_iocb_s4() with the hbalock
- * held.
- *
- * Return: index into SLI4 fast-path FCP queue index.
- **/
-static inline int
-lpfc_sli4_scmd_to_wqidx_distr(struct lpfc_hba *phba)
-{
-	struct lpfc_vector_map_info *cpup;
-	int chann, cpu;
-
-	if (phba->cfg_fcp_io_sched == LPFC_FCP_SCHED_BY_CPU
-	    && phba->cfg_fcp_io_channel > 1) {
-		cpu = smp_processor_id();
-		if (cpu < phba->sli4_hba.num_present_cpu) {
-			cpup = phba->sli4_hba.cpu_map;
-			cpup += cpu;
-			return cpup->channel_id;
-		}
-	}
-	chann = atomic_add_return(1, &phba->fcp_qidx);
-	chann = (chann % phba->cfg_fcp_io_channel);
-	return chann;
-}
-
-/**
  * lpfc_sli_iocb2wqe - Convert the IOCB to a work queue entry.
  * @phba: Pointer to HBA context object.
  * @piocb: Pointer to command iocb.
@@ -8792,32 +8762,44 @@ lpfc_sli_api_table_setup(struct lpfc_hba *phba, uint8_t dev_grp)
 	return 0;
 }
 
+/**
+ * lpfc_sli_calc_ring - Calculates which ring to use
+ * @phba: Pointer to HBA context object.
+ * @ring_number: Initial ring
+ * @piocb: Pointer to command iocb.
+ *
+ * For SLI4, FCP IO can deferred to one fo many WQs, based on
+ * fcp_wqidx, thus we need to calculate the corresponding ring.
+ * Since ABORTS must go on the same WQ of the command they are
+ * aborting, we use command's fcp_wqidx.
+ */
 int
 lpfc_sli_calc_ring(struct lpfc_hba *phba, uint32_t ring_number,
 		    struct lpfc_iocbq *piocb)
 {
-	uint32_t idx;
+	if (phba->sli_rev < LPFC_SLI_REV4)
+		return ring_number;
 
-	if (phba->sli_rev == LPFC_SLI_REV4) {
-		if (piocb->iocb_flag &  (LPFC_IO_FCP | LPFC_USE_FCPWQIDX)) {
+	if (piocb->iocb_flag &  (LPFC_IO_FCP | LPFC_USE_FCPWQIDX)) {
+		if (!(phba->cfg_fof) ||
+				(!(piocb->iocb_flag & LPFC_IO_FOF))) {
+			if (unlikely(!phba->sli4_hba.fcp_wq))
+				return LPFC_HBA_ERROR;
 			/*
-			 * fcp_wqidx should already be setup based on what
-			 * completion queue we want to use.
+			 * for abort iocb fcp_wqidx should already
+			 * be setup based on what work queue we used.
 			 */
-			if (!(phba->cfg_fof) ||
-			    (!(piocb->iocb_flag & LPFC_IO_FOF))) {
-				if (unlikely(!phba->sli4_hba.fcp_wq))
-					return LPFC_HBA_ERROR;
-				idx = lpfc_sli4_scmd_to_wqidx_distr(phba);
-				piocb->fcp_wqidx = idx;
-				ring_number = MAX_SLI3_CONFIGURED_RINGS + idx;
-			} else {
-				if (unlikely(!phba->sli4_hba.oas_wq))
-					return LPFC_HBA_ERROR;
-				idx = 0;
-				piocb->fcp_wqidx = idx;
-				ring_number =  LPFC_FCP_OAS_RING;
-			}
+			if (!(piocb->iocb_flag & LPFC_USE_FCPWQIDX))
+				piocb->fcp_wqidx =
+					lpfc_sli4_scmd_to_wqidx_distr(phba,
+							      piocb->context1);
+			ring_number = MAX_SLI3_CONFIGURED_RINGS +
+				piocb->fcp_wqidx;
+		} else {
+			if (unlikely(!phba->sli4_hba.oas_wq))
+				return LPFC_HBA_ERROR;
+			piocb->fcp_wqidx = 0;
+			ring_number =  LPFC_FCP_OAS_RING;
 		}
 	}
 	return ring_number;
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index 6eca3b8124d3..d1a5b057c6f3 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -602,6 +602,7 @@ struct lpfc_sli4_hba {
 	struct lpfc_iov iov;
 	spinlock_t abts_scsi_buf_list_lock; /* list of aborted SCSI IOs */
 	spinlock_t abts_sgl_list_lock; /* list of aborted els IOs */
+	uint32_t physical_port;
 
 	/* CPU to vector mapping information */
 	struct lpfc_vector_map_info *cpu_map;
@@ -651,6 +652,26 @@ struct lpfc_rsrc_blks {
 	uint16_t rsrc_used;
 };
 
+struct lpfc_rdp_context {
+	struct lpfc_nodelist *ndlp;
+	uint16_t ox_id;
+	uint16_t rx_id;
+	READ_LNK_VAR link_stat;
+	uint8_t page_a0[DMP_SFF_PAGE_A0_SIZE];
+	uint8_t page_a2[DMP_SFF_PAGE_A2_SIZE];
+	void (*cmpl)(struct lpfc_hba *, struct lpfc_rdp_context*, int);
+};
+
+struct lpfc_lcb_context {
+	uint8_t  sub_command;
+	uint8_t  type;
+	uint8_t  frequency;
+	uint16_t ox_id;
+	uint16_t rx_id;
+	struct lpfc_nodelist *ndlp;
+};
+
+
 /*
  * SLI4 specific function prototypes
  */
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index c37bb9f91c3b..6258d3d7722a 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "10.5.0.0."
+#define LPFC_DRIVER_VERSION "10.7.0.0."
 #define LPFC_DRIVER_NAME		"lpfc"
 
 /* Used for SLI 2/3 */
diff --git a/drivers/scsi/lpfc/lpfc_vport.c b/drivers/scsi/lpfc/lpfc_vport.c
index a87ee33f4f2a..769012663a8f 100644
--- a/drivers/scsi/lpfc/lpfc_vport.c
+++ b/drivers/scsi/lpfc/lpfc_vport.c
@@ -567,8 +567,8 @@ int
 lpfc_vport_delete(struct fc_vport *fc_vport)
 {
 	struct lpfc_nodelist *ndlp = NULL;
-	struct Scsi_Host *shost = (struct Scsi_Host *) fc_vport->shost;
 	struct lpfc_vport *vport = *(struct lpfc_vport **)fc_vport->dd_data;
+	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
 	struct lpfc_hba   *phba = vport->phba;
 	long timeout;
 	bool ns_ndlp_referenced = false;
@@ -645,8 +645,8 @@ lpfc_vport_delete(struct fc_vport *fc_vport)
 	}
 
 	/* Remove FC host and then SCSI host with the vport */
-	fc_remove_host(lpfc_shost_from_vport(vport));
-	scsi_remove_host(lpfc_shost_from_vport(vport));
+	fc_remove_host(shost);
+	scsi_remove_host(shost);
 
 	ndlp = lpfc_findnode_did(phba->pport, Fabric_DID);
 
@@ -772,7 +772,8 @@ skip_logo:
 		 * Completion of unreg_vpi (lpfc_mbx_cmpl_unreg_vpi)
 		 * does the scsi_host_put() to release the vport.
 		 */
-		if (lpfc_mbx_unreg_vpi(vport))
+		if (!(vport->vpi_state & LPFC_VPI_REGISTERED) ||
+				lpfc_mbx_unreg_vpi(vport))
 			scsi_host_put(shost);
 	} else
 		scsi_host_put(shost);
diff --git a/drivers/scsi/mac53c94.c b/drivers/scsi/mac53c94.c
index 0adb2e015597..141226631429 100644
--- a/drivers/scsi/mac53c94.c
+++ b/drivers/scsi/mac53c94.c
@@ -403,7 +403,6 @@ static struct scsi_host_template mac53c94_template = {
 	.can_queue	= 1,
 	.this_id	= 7,
 	.sg_tablesize	= SG_ALL,
-	.cmd_per_lun	= 1,
 	.use_clustering	= DISABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/megaraid/megaraid_sas.h b/drivers/scsi/megaraid/megaraid_sas.h
index 14e5c7cea929..20c37541963f 100644
--- a/drivers/scsi/megaraid/megaraid_sas.h
+++ b/drivers/scsi/megaraid/megaraid_sas.h
@@ -35,7 +35,8 @@
 /*
  * MegaRAID SAS Driver meta data
  */
-#define MEGASAS_VERSION				"06.806.08.00-rc1"
+#define MEGASAS_VERSION				"06.807.10.00-rc1"
+#define MEGASAS_RELDATE				"March 6, 2015"
 
 /*
  * Device IDs
@@ -153,6 +154,9 @@
 #define MFI_FRAME_DIR_BOTH			0x0018
 #define MFI_FRAME_IEEE                          0x0020
 
+/* Driver internal */
+#define DRV_DCMD_POLLED_MODE		0x1
+
 /*
  * Definition for cmd_status
  */
@@ -408,7 +412,7 @@ enum MR_PD_STATE {
  * defines the physical drive address structure
  */
 struct MR_PD_ADDRESS {
-	u16     deviceId;
+	__le16	deviceId;
 	u16     enclDeviceId;
 
 	union {
@@ -433,8 +437,8 @@ struct MR_PD_ADDRESS {
  * defines the physical drive list structure
  */
 struct MR_PD_LIST {
-	u32             size;
-	u32             count;
+	__le32		size;
+	__le32		count;
 	struct MR_PD_ADDRESS   addr[1];
 } __packed;
 
@@ -451,28 +455,28 @@ union  MR_LD_REF {
 	struct {
 		u8      targetId;
 		u8      reserved;
-		u16     seqNum;
+		__le16     seqNum;
 	};
-	u32     ref;
+	__le32     ref;
 } __packed;
 
 /*
  * defines the logical drive list structure
  */
 struct MR_LD_LIST {
-	u32     ldCount;
-	u32     reserved;
+	__le32     ldCount;
+	__le32     reserved;
 	struct {
 		union MR_LD_REF   ref;
 		u8          state;
 		u8          reserved[3];
-		u64         size;
+		__le64		size;
 	} ldList[MAX_LOGICAL_DRIVES_EXT];
 } __packed;
 
 struct MR_LD_TARGETID_LIST {
-	u32	size;
-	u32	count;
+	__le32	size;
+	__le32	count;
 	u8	pad[3];
 	u8	targetId[MAX_LOGICAL_DRIVES_EXT];
 };
@@ -553,7 +557,7 @@ struct megasas_ctrl_prop {
 	} OnOffProperties;
 	u8 autoSnapVDSpace;
 	u8 viewSpace;
-	u16 spinDownTime;
+	__le16 spinDownTime;
 	u8  reserved[24];
 } __packed;
 
@@ -567,10 +571,10 @@ struct megasas_ctrl_info {
 	 */
 	struct {
 
-		u16 vendor_id;
-		u16 device_id;
-		u16 sub_vendor_id;
-		u16 sub_device_id;
+		__le16 vendor_id;
+		__le16 device_id;
+		__le16 sub_vendor_id;
+		__le16 sub_device_id;
 		u8 reserved[24];
 
 	} __attribute__ ((packed)) pci;
@@ -611,8 +615,8 @@ struct megasas_ctrl_info {
 	/*
 	 * List of components residing in flash. All str are null terminated
 	 */
-	u32 image_check_word;
-	u32 image_component_count;
+	__le32 image_check_word;
+	__le32 image_component_count;
 
 	struct {
 
@@ -629,7 +633,7 @@ struct megasas_ctrl_info {
 	 * empty if a flash operation has not occurred. All stings are null
 	 * terminated
 	 */
-	u32 pending_image_component_count;
+	__le32 pending_image_component_count;
 
 	struct {
 
@@ -662,39 +666,39 @@ struct megasas_ctrl_info {
 
 	} __attribute__ ((packed)) hw_present;
 
-	u32 current_fw_time;
+	__le32 current_fw_time;
 
 	/*
 	 * Maximum data transfer sizes
 	 */
-	u16 max_concurrent_cmds;
-	u16 max_sge_count;
-	u32 max_request_size;
+	__le16 max_concurrent_cmds;
+	__le16 max_sge_count;
+	__le32 max_request_size;
 
 	/*
 	 * Logical and physical device counts
 	 */
-	u16 ld_present_count;
-	u16 ld_degraded_count;
-	u16 ld_offline_count;
+	__le16 ld_present_count;
+	__le16 ld_degraded_count;
+	__le16 ld_offline_count;
 
-	u16 pd_present_count;
-	u16 pd_disk_present_count;
-	u16 pd_disk_pred_failure_count;
-	u16 pd_disk_failed_count;
+	__le16 pd_present_count;
+	__le16 pd_disk_present_count;
+	__le16 pd_disk_pred_failure_count;
+	__le16 pd_disk_failed_count;
 
 	/*
 	 * Memory size information
 	 */
-	u16 nvram_size;
-	u16 memory_size;
-	u16 flash_size;
+	__le16 nvram_size;
+	__le16 memory_size;
+	__le16 flash_size;
 
 	/*
 	 * Error counters
 	 */
-	u16 mem_correctable_error_count;
-	u16 mem_uncorrectable_error_count;
+	__le16 mem_correctable_error_count;
+	__le16 mem_uncorrectable_error_count;
 
 	/*
 	 * Cluster information
@@ -705,7 +709,7 @@ struct megasas_ctrl_info {
 	/*
 	 * Additional max data transfer sizes
 	 */
-	u16 max_strips_per_io;
+	__le16 max_strips_per_io;
 
 	/*
 	 * Controller capabilities structures
@@ -805,7 +809,7 @@ struct megasas_ctrl_info {
 	* deviceInterface.portAddr, and the rest shall be
 	* populated in deviceInterfacePortAddr2.
 	*/
-	u64         deviceInterfacePortAddr2[8]; /*6a0h */
+	__le64	    deviceInterfacePortAddr2[8]; /*6a0h */
 	u8          reserved3[128];              /*6e0h */
 
 	struct {                                /*760h */
@@ -842,26 +846,26 @@ struct megasas_ctrl_info {
 		u16 reserved[6];
 	} pdsForRaidLevels;
 
-	u16 maxPds;                             /*780h */
-	u16 maxDedHSPs;                         /*782h */
-	u16 maxGlobalHSPs;                      /*784h */
-	u16 ddfSize;                            /*786h */
+	__le16 maxPds;                          /*780h */
+	__le16 maxDedHSPs;                      /*782h */
+	__le16 maxGlobalHSP;                    /*784h */
+	__le16 ddfSize;                         /*786h */
 	u8  maxLdsPerArray;                     /*788h */
 	u8  partitionsInDDF;                    /*789h */
 	u8  lockKeyBinding;                     /*78ah */
 	u8  maxPITsPerLd;                       /*78bh */
 	u8  maxViewsPerLd;                      /*78ch */
 	u8  maxTargetId;                        /*78dh */
-	u16 maxBvlVdSize;                       /*78eh */
+	__le16 maxBvlVdSize;                    /*78eh */
 
-	u16 maxConfigurableSSCSize;             /*790h */
-	u16 currentSSCsize;                     /*792h */
+	__le16 maxConfigurableSSCSize;          /*790h */
+	__le16 currentSSCsize;                  /*792h */
 
 	char    expanderFwVersion[12];          /*794h */
 
-	u16 PFKTrialTimeRemaining;              /*7A0h */
+	__le16 PFKTrialTimeRemaining;           /*7A0h */
 
-	u16 cacheMemorySize;                    /*7A2h */
+	__le16 cacheMemorySize;                 /*7A2h */
 
 	struct {                                /*7A4h */
 #if   defined(__BIG_ENDIAN_BITFIELD)
@@ -931,7 +935,7 @@ struct megasas_ctrl_info {
 	u8  temperatureROC;                     /*7C9h */
 	u8  temperatureCtrl;                    /*7CAh */
 	u8  reserved4;                          /*7CBh */
-	u16 maxConfigurablePds;                 /*7CCh */
+	__le16 maxConfigurablePds;              /*7CCh */
 
 
 	u8  reserved5[2];                       /*0x7CDh */
@@ -1042,11 +1046,6 @@ struct megasas_ctrl_info {
 
 #define VD_EXT_DEBUG 0
 
-enum MR_MFI_MPT_PTHR_FLAGS {
-	MFI_MPT_DETACHED = 0,
-	MFI_LIST_ADDED = 1,
-	MFI_MPT_ATTACHED = 2,
-};
 
 enum MR_SCSI_CMD_TYPE {
 	READ_WRITE_LDIO = 0,
@@ -1084,6 +1083,7 @@ enum MR_SCSI_CMD_TYPE {
 #define MEGASAS_SKINNY_INT_CMDS			5
 #define MEGASAS_FUSION_INTERNAL_CMDS		5
 #define MEGASAS_FUSION_IOCTL_CMDS		3
+#define MEGASAS_MFI_IOCTL_CMDS			27
 
 #define MEGASAS_MAX_MSIX_QUEUES			128
 /*
@@ -1172,22 +1172,22 @@ struct megasas_register_set {
 
 struct megasas_sge32 {
 
-	u32 phys_addr;
-	u32 length;
+	__le32 phys_addr;
+	__le32 length;
 
 } __attribute__ ((packed));
 
 struct megasas_sge64 {
 
-	u64 phys_addr;
-	u32 length;
+	__le64 phys_addr;
+	__le32 length;
 
 } __attribute__ ((packed));
 
 struct megasas_sge_skinny {
-	u64 phys_addr;
-	u32 length;
-	u32 flag;
+	__le64 phys_addr;
+	__le32 length;
+	__le32 flag;
 } __packed;
 
 union megasas_sgl {
@@ -1210,12 +1210,12 @@ struct megasas_header {
 	u8 cdb_len;		/*06h */
 	u8 sge_count;		/*07h */
 
-	u32 context;		/*08h */
-	u32 pad_0;		/*0Ch */
+	__le32 context;		/*08h */
+	__le32 pad_0;		/*0Ch */
 
-	u16 flags;		/*10h */
-	u16 timeout;		/*12h */
-	u32 data_xferlen;	/*14h */
+	__le16 flags;		/*10h */
+	__le16 timeout;		/*12h */
+	__le32 data_xferlen;	/*14h */
 
 } __attribute__ ((packed));
 
@@ -1248,7 +1248,7 @@ typedef union _MFI_CAPABILITIES {
 		u32     reserved:25;
 #endif
 	} mfi_capabilities;
-	u32     reg;
+	__le32		reg;
 } MFI_CAPABILITIES;
 
 struct megasas_init_frame {
@@ -1260,33 +1260,35 @@ struct megasas_init_frame {
 	u8 reserved_1;		/*03h */
 	MFI_CAPABILITIES driver_operations; /*04h*/
 
-	u32 context;		/*08h */
-	u32 pad_0;		/*0Ch */
-
-	u16 flags;		/*10h */
-	u16 reserved_3;		/*12h */
-	u32 data_xfer_len;	/*14h */
+	__le32 context;		/*08h */
+	__le32 pad_0;		/*0Ch */
 
-	u32 queue_info_new_phys_addr_lo;	/*18h */
-	u32 queue_info_new_phys_addr_hi;	/*1Ch */
-	u32 queue_info_old_phys_addr_lo;	/*20h */
-	u32 queue_info_old_phys_addr_hi;	/*24h */
+	__le16 flags;		/*10h */
+	__le16 reserved_3;		/*12h */
+	__le32 data_xfer_len;	/*14h */
 
-	u32 reserved_4[6];	/*28h */
+	__le32 queue_info_new_phys_addr_lo;	/*18h */
+	__le32 queue_info_new_phys_addr_hi;	/*1Ch */
+	__le32 queue_info_old_phys_addr_lo;	/*20h */
+	__le32 queue_info_old_phys_addr_hi;	/*24h */
+	__le32 reserved_4[2];	/*28h */
+	__le32 system_info_lo;      /*30h */
+	__le32 system_info_hi;      /*34h */
+	__le32 reserved_5[2];	/*38h */
 
 } __attribute__ ((packed));
 
 struct megasas_init_queue_info {
 
-	u32 init_flags;		/*00h */
-	u32 reply_queue_entries;	/*04h */
+	__le32 init_flags;		/*00h */
+	__le32 reply_queue_entries;	/*04h */
 
-	u32 reply_queue_start_phys_addr_lo;	/*08h */
-	u32 reply_queue_start_phys_addr_hi;	/*0Ch */
-	u32 producer_index_phys_addr_lo;	/*10h */
-	u32 producer_index_phys_addr_hi;	/*14h */
-	u32 consumer_index_phys_addr_lo;	/*18h */
-	u32 consumer_index_phys_addr_hi;	/*1Ch */
+	__le32 reply_queue_start_phys_addr_lo;	/*08h */
+	__le32 reply_queue_start_phys_addr_hi;	/*0Ch */
+	__le32 producer_index_phys_addr_lo;	/*10h */
+	__le32 producer_index_phys_addr_hi;	/*14h */
+	__le32 consumer_index_phys_addr_lo;	/*18h */
+	__le32 consumer_index_phys_addr_hi;	/*1Ch */
 
 } __attribute__ ((packed));
 
@@ -1302,18 +1304,18 @@ struct megasas_io_frame {
 	u8 reserved_0;		/*06h */
 	u8 sge_count;		/*07h */
 
-	u32 context;		/*08h */
-	u32 pad_0;		/*0Ch */
+	__le32 context;		/*08h */
+	__le32 pad_0;		/*0Ch */
 
-	u16 flags;		/*10h */
-	u16 timeout;		/*12h */
-	u32 lba_count;		/*14h */
+	__le16 flags;		/*10h */
+	__le16 timeout;		/*12h */
+	__le32 lba_count;	/*14h */
 
-	u32 sense_buf_phys_addr_lo;	/*18h */
-	u32 sense_buf_phys_addr_hi;	/*1Ch */
+	__le32 sense_buf_phys_addr_lo;	/*18h */
+	__le32 sense_buf_phys_addr_hi;	/*1Ch */
 
-	u32 start_lba_lo;	/*20h */
-	u32 start_lba_hi;	/*24h */
+	__le32 start_lba_lo;	/*20h */
+	__le32 start_lba_hi;	/*24h */
 
 	union megasas_sgl sgl;	/*28h */
 
@@ -1331,15 +1333,15 @@ struct megasas_pthru_frame {
 	u8 cdb_len;		/*06h */
 	u8 sge_count;		/*07h */
 
-	u32 context;		/*08h */
-	u32 pad_0;		/*0Ch */
+	__le32 context;		/*08h */
+	__le32 pad_0;		/*0Ch */
 
-	u16 flags;		/*10h */
-	u16 timeout;		/*12h */
-	u32 data_xfer_len;	/*14h */
+	__le16 flags;		/*10h */
+	__le16 timeout;		/*12h */
+	__le32 data_xfer_len;	/*14h */
 
-	u32 sense_buf_phys_addr_lo;	/*18h */
-	u32 sense_buf_phys_addr_hi;	/*1Ch */
+	__le32 sense_buf_phys_addr_lo;	/*18h */
+	__le32 sense_buf_phys_addr_hi;	/*1Ch */
 
 	u8 cdb[16];		/*20h */
 	union megasas_sgl sgl;	/*30h */
@@ -1354,19 +1356,19 @@ struct megasas_dcmd_frame {
 	u8 reserved_1[4];	/*03h */
 	u8 sge_count;		/*07h */
 
-	u32 context;		/*08h */
-	u32 pad_0;		/*0Ch */
+	__le32 context;		/*08h */
+	__le32 pad_0;		/*0Ch */
 
-	u16 flags;		/*10h */
-	u16 timeout;		/*12h */
+	__le16 flags;		/*10h */
+	__le16 timeout;		/*12h */
 
-	u32 data_xfer_len;	/*14h */
-	u32 opcode;		/*18h */
+	__le32 data_xfer_len;	/*14h */
+	__le32 opcode;		/*18h */
 
 	union {			/*1Ch */
 		u8 b[12];
-		u16 s[6];
-		u32 w[3];
+		__le16 s[6];
+		__le32 w[3];
 	} mbox;
 
 	union megasas_sgl sgl;	/*28h */
@@ -1380,22 +1382,22 @@ struct megasas_abort_frame {
 	u8 cmd_status;		/*02h */
 
 	u8 reserved_1;		/*03h */
-	u32 reserved_2;		/*04h */
+	__le32 reserved_2;	/*04h */
 
-	u32 context;		/*08h */
-	u32 pad_0;		/*0Ch */
+	__le32 context;		/*08h */
+	__le32 pad_0;		/*0Ch */
 
-	u16 flags;		/*10h */
-	u16 reserved_3;		/*12h */
-	u32 reserved_4;		/*14h */
+	__le16 flags;		/*10h */
+	__le16 reserved_3;	/*12h */
+	__le32 reserved_4;	/*14h */
 
-	u32 abort_context;	/*18h */
-	u32 pad_1;		/*1Ch */
+	__le32 abort_context;	/*18h */
+	__le32 pad_1;		/*1Ch */
 
-	u32 abort_mfi_phys_addr_lo;	/*20h */
-	u32 abort_mfi_phys_addr_hi;	/*24h */
+	__le32 abort_mfi_phys_addr_lo;	/*20h */
+	__le32 abort_mfi_phys_addr_hi;	/*24h */
 
-	u32 reserved_5[6];	/*28h */
+	__le32 reserved_5[6];	/*28h */
 
 } __attribute__ ((packed));
 
@@ -1409,14 +1411,14 @@ struct megasas_smp_frame {
 	u8 reserved_2[3];	/*04h */
 	u8 sge_count;		/*07h */
 
-	u32 context;		/*08h */
-	u32 pad_0;		/*0Ch */
+	__le32 context;		/*08h */
+	__le32 pad_0;		/*0Ch */
 
-	u16 flags;		/*10h */
-	u16 timeout;		/*12h */
+	__le16 flags;		/*10h */
+	__le16 timeout;		/*12h */
 
-	u32 data_xfer_len;	/*14h */
-	u64 sas_addr;		/*18h */
+	__le32 data_xfer_len;	/*14h */
+	__le64 sas_addr;	/*18h */
 
 	union {
 		struct megasas_sge32 sge32[2];	/* [0]: resp [1]: req */
@@ -1436,16 +1438,16 @@ struct megasas_stp_frame {
 	u8 reserved_3[2];	/*05h */
 	u8 sge_count;		/*07h */
 
-	u32 context;		/*08h */
-	u32 pad_0;		/*0Ch */
+	__le32 context;		/*08h */
+	__le32 pad_0;		/*0Ch */
 
-	u16 flags;		/*10h */
-	u16 timeout;		/*12h */
+	__le16 flags;		/*10h */
+	__le16 timeout;		/*12h */
 
-	u32 data_xfer_len;	/*14h */
+	__le32 data_xfer_len;	/*14h */
 
-	u16 fis[10];		/*18h */
-	u32 stp_flags;
+	__le16 fis[10];		/*18h */
+	__le32 stp_flags;
 
 	union {
 		struct megasas_sge32 sge32[2];	/* [0]: resp [1]: data */
@@ -1489,18 +1491,18 @@ union megasas_evt_class_locale {
 } __attribute__ ((packed));
 
 struct megasas_evt_log_info {
-	u32 newest_seq_num;
-	u32 oldest_seq_num;
-	u32 clear_seq_num;
-	u32 shutdown_seq_num;
-	u32 boot_seq_num;
+	__le32 newest_seq_num;
+	__le32 oldest_seq_num;
+	__le32 clear_seq_num;
+	__le32 shutdown_seq_num;
+	__le32 boot_seq_num;
 
 } __attribute__ ((packed));
 
 struct megasas_progress {
 
-	u16 progress;
-	u16 elapsed_seconds;
+	__le16 progress;
+	__le16 elapsed_seconds;
 
 } __attribute__ ((packed));
 
@@ -1521,9 +1523,9 @@ struct megasas_evtarg_pd {
 
 struct megasas_evt_detail {
 
-	u32 seq_num;
-	u32 time_stamp;
-	u32 code;
+	__le32 seq_num;
+	__le32 time_stamp;
+	__le32 code;
 	union megasas_evt_class_locale cl;
 	u8 arg_type;
 	u8 reserved1[15];
@@ -1542,18 +1544,18 @@ struct megasas_evt_detail {
 
 		struct {
 			struct megasas_evtarg_ld ld;
-			u64 count;
+			__le64 count;
 		} __attribute__ ((packed)) ld_count;
 
 		struct {
-			u64 lba;
+			__le64 lba;
 			struct megasas_evtarg_ld ld;
 		} __attribute__ ((packed)) ld_lba;
 
 		struct {
 			struct megasas_evtarg_ld ld;
-			u32 prevOwner;
-			u32 newOwner;
+			__le32 prevOwner;
+			__le32 newOwner;
 		} __attribute__ ((packed)) ld_owner;
 
 		struct {
@@ -1610,7 +1612,7 @@ struct megasas_evt_detail {
 
 		struct {
 			u16 vendorId;
-			u16 deviceId;
+			__le16 deviceId;
 			u16 subVendorId;
 			u16 subDeviceId;
 		} __attribute__ ((packed)) pci;
@@ -1630,9 +1632,9 @@ struct megasas_evt_detail {
 		} __attribute__ ((packed)) ecc;
 
 		u8 b[96];
-		u16 s[48];
-		u32 w[24];
-		u64 d[12];
+		__le16 s[48];
+		__le32 w[24];
+		__le64 d[12];
 	} args;
 
 	char description[128];
@@ -1649,12 +1651,22 @@ struct megasas_irq_context {
 	u32 MSIxIndex;
 };
 
+struct MR_DRV_SYSTEM_INFO {
+	u8	infoVersion;
+	u8	systemIdLength;
+	u16	reserved0;
+	u8	systemId[64];
+	u8	reserved[1980];
+};
+
 struct megasas_instance {
 
-	u32 *producer;
+	__le32 *producer;
 	dma_addr_t producer_h;
-	u32 *consumer;
+	__le32 *consumer;
 	dma_addr_t consumer_h;
+	struct MR_DRV_SYSTEM_INFO *system_info_buf;
+	dma_addr_t system_info_h;
 	struct MR_LD_VF_AFFILIATION *vf_affiliation;
 	dma_addr_t vf_affiliation_h;
 	struct MR_LD_VF_AFFILIATION_111 *vf_affiliation_111;
@@ -1662,7 +1674,7 @@ struct megasas_instance {
 	struct MR_CTRL_HB_HOST_MEM *hb_host_mem;
 	dma_addr_t hb_host_mem_h;
 
-	u32 *reply_queue;
+	__le32 *reply_queue;
 	dma_addr_t reply_queue_h;
 
 	u32 *crash_dump_buf;
@@ -1681,7 +1693,7 @@ struct megasas_instance {
 	spinlock_t crashdump_lock;
 
 	struct megasas_register_set __iomem *reg_set;
-	u32 *reply_post_host_index_addr[MR_MAX_MSIX_REG_ARRAY];
+	u32 __iomem *reply_post_host_index_addr[MR_MAX_MSIX_REG_ARRAY];
 	struct megasas_pd_list          pd_list[MEGASAS_MAX_PD];
 	struct megasas_pd_list          local_pd_list[MEGASAS_MAX_PD];
 	u8 ld_ids[MEGASAS_MAX_LD_IDS];
@@ -1769,6 +1781,7 @@ struct megasas_instance {
 	u16 throttlequeuedepth;
 	u8 mask_interrupts;
 	u8 is_imr;
+	bool dev_handle;
 };
 struct MR_LD_VF_MAP {
 	u32 size;
@@ -1864,9 +1877,13 @@ struct megasas_instance_template {
 #define MEGASAS_IS_LOGICAL(scp)						\
 	(scp->device->channel < MEGASAS_MAX_PD_CHANNELS) ? 0 : 1
 
-#define MEGASAS_DEV_INDEX(inst, scp)					\
-	((scp->device->channel % 2) * MEGASAS_MAX_DEV_PER_CHANNEL) + 	\
-	scp->device->id
+#define MEGASAS_DEV_INDEX(scp)						\
+	(((scp->device->channel % 2) * MEGASAS_MAX_DEV_PER_CHANNEL) +	\
+	scp->device->id)
+
+#define MEGASAS_PD_INDEX(scp)						\
+	((scp->device->channel * MEGASAS_MAX_DEV_PER_CHANNEL) +		\
+	scp->device->id)
 
 struct megasas_cmd {
 
@@ -1877,17 +1894,14 @@ struct megasas_cmd {
 
 	u32 index;
 	u8 sync_cmd;
-	u8 cmd_status;
+	u8 cmd_status_drv;
 	u8 abort_aen;
 	u8 retry_for_fw_reset;
 
 
 	struct list_head list;
 	struct scsi_cmnd *scmd;
-
-	void *mpt_pthr_cmd_blocked;
-	atomic_t mfi_mpt_pthr;
-	u8 is_wait_event;
+	u8 flags;
 
 	struct megasas_instance *instance;
 	union {
@@ -1963,10 +1977,10 @@ u8 MR_TargetIdToLdGet(u32 ldTgtId, struct MR_DRV_RAID_MAP_ALL *map);
 struct MR_LD_RAID *MR_LdRaidGet(u32 ld, struct MR_DRV_RAID_MAP_ALL *map);
 u16 MR_ArPdGet(u32 ar, u32 arm, struct MR_DRV_RAID_MAP_ALL *map);
 u16 MR_LdSpanArrayGet(u32 ld, u32 span, struct MR_DRV_RAID_MAP_ALL *map);
-u16 MR_PdDevHandleGet(u32 pd, struct MR_DRV_RAID_MAP_ALL *map);
+__le16 MR_PdDevHandleGet(u32 pd, struct MR_DRV_RAID_MAP_ALL *map);
 u16 MR_GetLDTgtId(u32 ld, struct MR_DRV_RAID_MAP_ALL *map);
 
-u16 get_updated_dev_handle(struct megasas_instance *instance,
+__le16 get_updated_dev_handle(struct megasas_instance *instance,
 	struct LD_LOAD_BALANCE_INFO *lbInfo, struct IO_REQUEST_INFO *in_info);
 void mr_update_load_balance_params(struct MR_DRV_RAID_MAP_ALL *map,
 	struct LD_LOAD_BALANCE_INFO *lbInfo);
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index 890637fdd61e..71b884dae27c 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -94,8 +94,8 @@ MODULE_PARM_DESC(smp_affinity_enable, "SMP affinity feature enable/disbale Defau
 
 MODULE_LICENSE("GPL");
 MODULE_VERSION(MEGASAS_VERSION);
-MODULE_AUTHOR("megaraidlinux@lsi.com");
-MODULE_DESCRIPTION("LSI MegaRAID SAS Driver");
+MODULE_AUTHOR("megaraidlinux.pdl@avagotech.com");
+MODULE_DESCRIPTION("Avago MegaRAID SAS Driver");
 
 int megasas_transition_to_ready(struct megasas_instance *instance, int ocr);
 static int megasas_get_pd_list(struct megasas_instance *instance);
@@ -215,7 +215,6 @@ struct megasas_cmd *megasas_get_cmd(struct megasas_instance
 		cmd = list_entry((&instance->cmd_pool)->next,
 				 struct megasas_cmd, list);
 		list_del_init(&cmd->list);
-		atomic_set(&cmd->mfi_mpt_pthr, MFI_MPT_DETACHED);
 	} else {
 		printk(KERN_ERR "megasas: Command pool empty!\n");
 	}
@@ -225,52 +224,41 @@ struct megasas_cmd *megasas_get_cmd(struct megasas_instance
 }
 
 /**
- * __megasas_return_cmd -	Return a cmd to free command pool
+ * megasas_return_cmd -	Return a cmd to free command pool
  * @instance:		Adapter soft state
  * @cmd:		Command packet to be returned to free command pool
  */
 inline void
-__megasas_return_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd)
+megasas_return_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd)
 {
-	/*
-	 * Don't go ahead and free the MFI frame, if corresponding
-	 * MPT frame is not freed(valid for only fusion adapters).
-	 * In case of MFI adapters, anyways for any allocated MFI
-	 * frame will have cmd->mfi_mpt_mpthr set to MFI_MPT_DETACHED
+	unsigned long flags;
+	u32 blk_tags;
+	struct megasas_cmd_fusion *cmd_fusion;
+	struct fusion_context *fusion = instance->ctrl_context;
+
+	/* This flag is used only for fusion adapter.
+	 * Wait for Interrupt for Polled mode DCMD
 	 */
-	if (atomic_read(&cmd->mfi_mpt_pthr) != MFI_MPT_DETACHED)
+	if (cmd->flags & DRV_DCMD_POLLED_MODE)
 		return;
 
+	spin_lock_irqsave(&instance->mfi_pool_lock, flags);
+
+	if (fusion) {
+		blk_tags = instance->max_scsi_cmds + cmd->index;
+		cmd_fusion = fusion->cmd_list[blk_tags];
+		megasas_return_cmd_fusion(instance, cmd_fusion);
+	}
 	cmd->scmd = NULL;
 	cmd->frame_count = 0;
-	cmd->is_wait_event = 0;
-	cmd->mpt_pthr_cmd_blocked = NULL;
-
-	if ((instance->pdev->device != PCI_DEVICE_ID_LSI_FUSION) &&
-	    (instance->pdev->device != PCI_DEVICE_ID_LSI_INVADER) &&
-	    (instance->pdev->device != PCI_DEVICE_ID_LSI_FURY) &&
-	    (reset_devices))
+	cmd->flags = 0;
+	if (!fusion && reset_devices)
 		cmd->frame->hdr.cmd = MFI_CMD_INVALID;
-
-	atomic_set(&cmd->mfi_mpt_pthr, MFI_LIST_ADDED);
 	list_add(&cmd->list, (&instance->cmd_pool)->next);
-}
 
-/**
- * megasas_return_cmd -	Return a cmd to free command pool
- * @instance:		Adapter soft state
- * @cmd:		Command packet to be returned to free command pool
- */
-inline void
-megasas_return_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&instance->mfi_pool_lock, flags);
-	__megasas_return_cmd(instance, cmd);
 	spin_unlock_irqrestore(&instance->mfi_pool_lock, flags);
-}
 
+}
 
 /**
 *	The following functions are defined for xscale
@@ -814,8 +802,8 @@ megasas_adp_reset_gen2(struct megasas_instance *instance,
 {
 	u32			retry = 0 ;
 	u32			HostDiag;
-	u32			*seq_offset = &reg_set->seq_offset;
-	u32			*hostdiag_offset = &reg_set->host_diag;
+	u32 __iomem		*seq_offset = &reg_set->seq_offset;
+	u32 __iomem		*hostdiag_offset = &reg_set->host_diag;
 
 	if (instance->instancet == &megasas_instance_template_skinny) {
 		seq_offset = &reg_set->fusion_seq_offset;
@@ -910,7 +898,7 @@ extern struct megasas_instance_template megasas_instance_template_fusion;
  * @instance:			Adapter soft state
  * @cmd:			Command packet to be issued
  *
- * For polling, MFI requires the cmd_status to be set to 0xFF before posting.
+ * For polling, MFI requires the cmd_status to be set to MFI_STAT_INVALID_STATUS before posting.
  */
 int
 megasas_issue_polled(struct megasas_instance *instance, struct megasas_cmd *cmd)
@@ -952,20 +940,20 @@ megasas_issue_blocked_cmd(struct megasas_instance *instance,
 			  struct megasas_cmd *cmd, int timeout)
 {
 	int ret = 0;
-	cmd->cmd_status = ENODATA;
+	cmd->cmd_status_drv = MFI_STAT_INVALID_STATUS;
 
-	cmd->is_wait_event = 1;
 	instance->instancet->issue_dcmd(instance, cmd);
 	if (timeout) {
 		ret = wait_event_timeout(instance->int_cmd_wait_q,
-				cmd->cmd_status != ENODATA, timeout * HZ);
+				cmd->cmd_status_drv != MFI_STAT_INVALID_STATUS, timeout * HZ);
 		if (!ret)
 			return 1;
 	} else
 		wait_event(instance->int_cmd_wait_q,
-				cmd->cmd_status != ENODATA);
+				cmd->cmd_status_drv != MFI_STAT_INVALID_STATUS);
 
-	return 0;
+	return (cmd->cmd_status_drv == MFI_STAT_OK) ?
+		0 : 1;
 }
 
 /**
@@ -998,7 +986,7 @@ megasas_issue_blocked_abort_cmd(struct megasas_instance *instance,
 	 * Prepare and issue the abort frame
 	 */
 	abort_fr->cmd = MFI_CMD_ABORT;
-	abort_fr->cmd_status = 0xFF;
+	abort_fr->cmd_status = MFI_STAT_INVALID_STATUS;
 	abort_fr->flags = cpu_to_le16(0);
 	abort_fr->abort_context = cpu_to_le32(cmd_to_abort->index);
 	abort_fr->abort_mfi_phys_addr_lo =
@@ -1007,13 +995,13 @@ megasas_issue_blocked_abort_cmd(struct megasas_instance *instance,
 		cpu_to_le32(upper_32_bits(cmd_to_abort->frame_phys_addr));
 
 	cmd->sync_cmd = 1;
-	cmd->cmd_status = ENODATA;
+	cmd->cmd_status_drv = MFI_STAT_INVALID_STATUS;
 
 	instance->instancet->issue_dcmd(instance, cmd);
 
 	if (timeout) {
 		ret = wait_event_timeout(instance->abort_cmd_wait_q,
-				cmd->cmd_status != ENODATA, timeout * HZ);
+				cmd->cmd_status_drv != MFI_STAT_INVALID_STATUS, timeout * HZ);
 		if (!ret) {
 			dev_err(&instance->pdev->dev, "Command timedout"
 				"from %s\n", __func__);
@@ -1021,7 +1009,7 @@ megasas_issue_blocked_abort_cmd(struct megasas_instance *instance,
 		}
 	} else
 		wait_event(instance->abort_cmd_wait_q,
-				cmd->cmd_status != ENODATA);
+				cmd->cmd_status_drv != MFI_STAT_INVALID_STATUS);
 
 	cmd->sync_cmd = 0;
 
@@ -1196,7 +1184,7 @@ megasas_build_dcdb(struct megasas_instance *instance, struct scsi_cmnd *scp,
 	struct megasas_pthru_frame *pthru;
 
 	is_logical = MEGASAS_IS_LOGICAL(scp);
-	device_id = MEGASAS_DEV_INDEX(instance, scp);
+	device_id = MEGASAS_DEV_INDEX(scp);
 	pthru = (struct megasas_pthru_frame *)cmd->frame;
 
 	if (scp->sc_data_direction == PCI_DMA_TODEVICE)
@@ -1232,7 +1220,7 @@ megasas_build_dcdb(struct megasas_instance *instance, struct scsi_cmnd *scp,
 	*/
 	if (scp->device->type == TYPE_TAPE) {
 		if ((scp->request->timeout / HZ) > 0xFFFF)
-			pthru->timeout = 0xFFFF;
+			pthru->timeout = cpu_to_le16(0xFFFF);
 		else
 			pthru->timeout = cpu_to_le16(scp->request->timeout / HZ);
 	}
@@ -1294,7 +1282,7 @@ megasas_build_ldio(struct megasas_instance *instance, struct scsi_cmnd *scp,
 	u16 flags = 0;
 	struct megasas_io_frame *ldio;
 
-	device_id = MEGASAS_DEV_INDEX(instance, scp);
+	device_id = MEGASAS_DEV_INDEX(scp);
 	ldio = (struct megasas_io_frame *)cmd->frame;
 
 	if (scp->sc_data_direction == PCI_DMA_TODEVICE)
@@ -1698,7 +1686,7 @@ static int megasas_slave_alloc(struct scsi_device *sdev)
 * @instance:				Adapter soft state
 *
 */
-void megasas_complete_outstanding_ioctls(struct megasas_instance *instance)
+static void megasas_complete_outstanding_ioctls(struct megasas_instance *instance)
 {
 	int i;
 	struct megasas_cmd *cmd_mfi;
@@ -1922,22 +1910,24 @@ static int megasas_get_ld_vf_affiliation_111(struct megasas_instance *instance,
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
 
 	dcmd->cmd = MFI_CMD_DCMD;
-	dcmd->cmd_status = 0xFF;
+	dcmd->cmd_status = MFI_STAT_INVALID_STATUS;
 	dcmd->sge_count = 1;
-	dcmd->flags = MFI_FRAME_DIR_BOTH;
+	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_BOTH);
 	dcmd->timeout = 0;
 	dcmd->pad_0 = 0;
-	dcmd->data_xfer_len = sizeof(struct MR_LD_VF_AFFILIATION_111);
-	dcmd->opcode = MR_DCMD_LD_VF_MAP_GET_ALL_LDS_111;
+	dcmd->data_xfer_len =
+		cpu_to_le32(sizeof(struct MR_LD_VF_AFFILIATION_111));
+	dcmd->opcode = cpu_to_le32(MR_DCMD_LD_VF_MAP_GET_ALL_LDS_111);
 
 	if (initial)
 		dcmd->sgl.sge32[0].phys_addr =
-			instance->vf_affiliation_111_h;
+			cpu_to_le32(instance->vf_affiliation_111_h);
 	else
-		dcmd->sgl.sge32[0].phys_addr = new_affiliation_111_h;
+		dcmd->sgl.sge32[0].phys_addr =
+			cpu_to_le32(new_affiliation_111_h);
 
-	dcmd->sgl.sge32[0].length =
-		sizeof(struct MR_LD_VF_AFFILIATION_111);
+	dcmd->sgl.sge32[0].length = cpu_to_le32(
+		sizeof(struct MR_LD_VF_AFFILIATION_111));
 
 	printk(KERN_WARNING "megasas: SR-IOV: Getting LD/VF affiliation for "
 	       "scsi%d\n", instance->host->host_no);
@@ -1976,11 +1966,7 @@ out:
 				    new_affiliation_111_h);
 	}
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	return retval;
 }
@@ -2037,22 +2023,24 @@ static int megasas_get_ld_vf_affiliation_12(struct megasas_instance *instance,
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
 
 	dcmd->cmd = MFI_CMD_DCMD;
-	dcmd->cmd_status = 0xFF;
+	dcmd->cmd_status = MFI_STAT_INVALID_STATUS;
 	dcmd->sge_count = 1;
-	dcmd->flags = MFI_FRAME_DIR_BOTH;
+	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_BOTH);
 	dcmd->timeout = 0;
 	dcmd->pad_0 = 0;
-	dcmd->data_xfer_len = (MAX_LOGICAL_DRIVES + 1) *
-		sizeof(struct MR_LD_VF_AFFILIATION);
-	dcmd->opcode = MR_DCMD_LD_VF_MAP_GET_ALL_LDS;
+	dcmd->data_xfer_len = cpu_to_le32((MAX_LOGICAL_DRIVES + 1) *
+		sizeof(struct MR_LD_VF_AFFILIATION));
+	dcmd->opcode = cpu_to_le32(MR_DCMD_LD_VF_MAP_GET_ALL_LDS);
 
 	if (initial)
-		dcmd->sgl.sge32[0].phys_addr = instance->vf_affiliation_h;
+		dcmd->sgl.sge32[0].phys_addr =
+			cpu_to_le32(instance->vf_affiliation_h);
 	else
-		dcmd->sgl.sge32[0].phys_addr = new_affiliation_h;
+		dcmd->sgl.sge32[0].phys_addr =
+			cpu_to_le32(new_affiliation_h);
 
-	dcmd->sgl.sge32[0].length = (MAX_LOGICAL_DRIVES + 1) *
-		sizeof(struct MR_LD_VF_AFFILIATION);
+	dcmd->sgl.sge32[0].length = cpu_to_le32((MAX_LOGICAL_DRIVES + 1) *
+		sizeof(struct MR_LD_VF_AFFILIATION));
 
 	printk(KERN_WARNING "megasas: SR-IOV: Getting LD/VF affiliation for "
 	       "scsi%d\n", instance->host->host_no);
@@ -2147,11 +2135,7 @@ out:
 				    (MAX_LOGICAL_DRIVES + 1) *
 				    sizeof(struct MR_LD_VF_AFFILIATION),
 				    new_affiliation, new_affiliation_h);
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	return retval;
 }
@@ -2204,39 +2188,33 @@ int megasas_sriov_start_heartbeat(struct megasas_instance *instance,
 
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
 
-	dcmd->mbox.s[0] = sizeof(struct MR_CTRL_HB_HOST_MEM);
+	dcmd->mbox.s[0] = cpu_to_le16(sizeof(struct MR_CTRL_HB_HOST_MEM));
 	dcmd->cmd = MFI_CMD_DCMD;
-	dcmd->cmd_status = 0xFF;
+	dcmd->cmd_status = MFI_STAT_INVALID_STATUS;
 	dcmd->sge_count = 1;
-	dcmd->flags = MFI_FRAME_DIR_BOTH;
+	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_BOTH);
 	dcmd->timeout = 0;
 	dcmd->pad_0 = 0;
-	dcmd->data_xfer_len = sizeof(struct MR_CTRL_HB_HOST_MEM);
-	dcmd->opcode = MR_DCMD_CTRL_SHARED_HOST_MEM_ALLOC;
-	dcmd->sgl.sge32[0].phys_addr = instance->hb_host_mem_h;
-	dcmd->sgl.sge32[0].length = sizeof(struct MR_CTRL_HB_HOST_MEM);
+	dcmd->data_xfer_len = cpu_to_le32(sizeof(struct MR_CTRL_HB_HOST_MEM));
+	dcmd->opcode = cpu_to_le32(MR_DCMD_CTRL_SHARED_HOST_MEM_ALLOC);
+	dcmd->sgl.sge32[0].phys_addr = cpu_to_le32(instance->hb_host_mem_h);
+	dcmd->sgl.sge32[0].length = cpu_to_le32(sizeof(struct MR_CTRL_HB_HOST_MEM));
 
 	printk(KERN_WARNING "megasas: SR-IOV: Starting heartbeat for scsi%d\n",
 	       instance->host->host_no);
 
-	if (!megasas_issue_polled(instance, cmd)) {
-		retval = 0;
-	} else {
-		printk(KERN_WARNING "megasas: SR-IOV: MR_DCMD_CTRL_SHARED_HOST"
-		       "_MEM_ALLOC DCMD timed out for scsi%d\n",
-		       instance->host->host_no);
-		retval = 1;
-		goto out;
-	}
-
+	if (instance->ctrl_context && !instance->mask_interrupts)
+		retval = megasas_issue_blocked_cmd(instance, cmd,
+			MEGASAS_ROUTINE_WAIT_TIME_VF);
+	else
+		retval = megasas_issue_polled(instance, cmd);
 
-	if (dcmd->cmd_status) {
-		printk(KERN_WARNING "megasas: SR-IOV: MR_DCMD_CTRL_SHARED_HOST"
-		       "_MEM_ALLOC DCMD failed with status 0x%x for scsi%d\n",
-		       dcmd->cmd_status,
-		       instance->host->host_no);
+	if (retval) {
+		dev_warn(&instance->pdev->dev, "SR-IOV: MR_DCMD_CTRL_SHARED_HOST"
+			"_MEM_ALLOC DCMD %s for scsi%d\n",
+			(dcmd->cmd_status == MFI_STAT_INVALID_STATUS) ?
+			"timed out" : "failed", instance->host->host_no);
 		retval = 1;
-		goto out;
 	}
 
 out:
@@ -2332,7 +2310,7 @@ static int megasas_wait_for_outstanding(struct megasas_instance *instance)
 						"reset queue\n",
 						reset_cmd);
 
-				reset_cmd->cmd_status = ENODATA;
+				reset_cmd->cmd_status_drv = MFI_STAT_INVALID_STATUS;
 				instance->instancet->fire_cmd(instance,
 						reset_cmd->frame_phys_addr,
 						0, instance->reg_set);
@@ -2612,11 +2590,7 @@ megasas_service_aen(struct megasas_instance *instance, struct megasas_cmd *cmd)
 
 	instance->aen_cmd = NULL;
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	if ((instance->unload == 0) &&
 		((instance->issuepend_done == 1))) {
@@ -2786,7 +2760,7 @@ struct device_attribute *megaraid_host_attrs[] = {
 static struct scsi_host_template megasas_template = {
 
 	.module = THIS_MODULE,
-	.name = "LSI SAS based MegaRAID driver",
+	.name = "Avago SAS based MegaRAID driver",
 	.proc_name = "megaraid_sas",
 	.slave_configure = megasas_slave_configure,
 	.slave_alloc = megasas_slave_alloc,
@@ -2815,11 +2789,7 @@ static void
 megasas_complete_int_cmd(struct megasas_instance *instance,
 			 struct megasas_cmd *cmd)
 {
-	cmd->cmd_status = cmd->frame->io.cmd_status;
-
-	if (cmd->cmd_status == ENODATA) {
-		cmd->cmd_status = 0;
-	}
+	cmd->cmd_status_drv = cmd->frame->io.cmd_status;
 	wake_up(&instance->int_cmd_wait_q);
 }
 
@@ -2838,7 +2808,7 @@ megasas_complete_abort(struct megasas_instance *instance,
 {
 	if (cmd->sync_cmd) {
 		cmd->sync_cmd = 0;
-		cmd->cmd_status = 0;
+		cmd->cmd_status_drv = 0;
 		wake_up(&instance->abort_cmd_wait_q);
 	}
 
@@ -2978,8 +2948,7 @@ megasas_complete_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd,
 					       "failed, status = 0x%x.\n",
 					       cmd->frame->hdr.cmd_status);
 				else {
-					megasas_return_mfi_mpt_pthr(instance,
-						cmd, cmd->mpt_pthr_cmd_blocked);
+					megasas_return_cmd(instance, cmd);
 					spin_unlock_irqrestore(
 						instance->host->host_lock,
 						flags);
@@ -2987,8 +2956,7 @@ megasas_complete_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd,
 				}
 			} else
 				instance->map_id++;
-			megasas_return_mfi_mpt_pthr(instance, cmd,
-				cmd->mpt_pthr_cmd_blocked);
+			megasas_return_cmd(instance, cmd);
 
 			/*
 			 * Set fast path IO to ZERO.
@@ -3086,7 +3054,7 @@ megasas_issue_pending_cmds_again(struct megasas_instance *instance)
 			printk(KERN_NOTICE "megasas: %p synchronous cmd"
 						"on the internal reset queue,"
 						"issue it again.\n", cmd);
-			cmd->cmd_status = ENODATA;
+			cmd->cmd_status_drv = MFI_STAT_INVALID_STATUS;
 			instance->instancet->fire_cmd(instance,
 							cmd->frame_phys_addr ,
 							0, instance->reg_set);
@@ -3766,7 +3734,6 @@ int megasas_alloc_cmds(struct megasas_instance *instance)
 		cmd = instance->cmd_list[i];
 		memset(cmd, 0, sizeof(struct megasas_cmd));
 		cmd->index = i;
-		atomic_set(&cmd->mfi_mpt_pthr, MFI_LIST_ADDED);
 		cmd->scmd = NULL;
 		cmd->instance = instance;
 
@@ -3827,7 +3794,7 @@ megasas_get_pd_list(struct megasas_instance *instance)
 	dcmd->mbox.b[0] = MR_PD_QUERY_TYPE_EXPOSED_TO_HOST;
 	dcmd->mbox.b[1] = 0;
 	dcmd->cmd = MFI_CMD_DCMD;
-	dcmd->cmd_status = 0xFF;
+	dcmd->cmd_status = MFI_STAT_INVALID_STATUS;
 	dcmd->sge_count = 1;
 	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_READ);
 	dcmd->timeout = 0;
@@ -3874,11 +3841,7 @@ megasas_get_pd_list(struct megasas_instance *instance)
 				MEGASAS_MAX_PD * sizeof(struct MR_PD_LIST),
 				ci, ci_h);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	return ret;
 }
@@ -3927,7 +3890,7 @@ megasas_get_ld_list(struct megasas_instance *instance)
 	if (instance->supportmax256vd)
 		dcmd->mbox.b[0] = 1;
 	dcmd->cmd = MFI_CMD_DCMD;
-	dcmd->cmd_status = 0xFF;
+	dcmd->cmd_status = MFI_STAT_INVALID_STATUS;
 	dcmd->sge_count = 1;
 	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_READ);
 	dcmd->timeout = 0;
@@ -3965,11 +3928,7 @@ megasas_get_ld_list(struct megasas_instance *instance)
 				ci,
 				ci_h);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 	return ret;
 }
 
@@ -4020,7 +3979,7 @@ megasas_ld_list_query(struct megasas_instance *instance, u8 query_type)
 		dcmd->mbox.b[2] = 1;
 
 	dcmd->cmd = MFI_CMD_DCMD;
-	dcmd->cmd_status = 0xFF;
+	dcmd->cmd_status = MFI_STAT_INVALID_STATUS;
 	dcmd->sge_count = 1;
 	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_READ);
 	dcmd->timeout = 0;
@@ -4050,11 +4009,7 @@ megasas_ld_list_query(struct megasas_instance *instance, u8 query_type)
 	pci_free_consistent(instance->pdev, sizeof(struct MR_LD_TARGETID_LIST),
 			    ci, ci_h);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	return ret;
 }
@@ -4091,12 +4046,11 @@ static void megasas_update_ext_vd_details(struct megasas_instance *instance)
 		instance->fw_supported_vd_count = MAX_LOGICAL_DRIVES;
 		instance->fw_supported_pd_count = MAX_PHYSICAL_DEVICES;
 	}
-	dev_info(&instance->pdev->dev, "Firmware supports %d VD %d PD\n",
-		instance->fw_supported_vd_count,
-		instance->fw_supported_pd_count);
-	dev_info(&instance->pdev->dev, "Driver supports %d VD  %d PD\n",
-		instance->drv_supported_vd_count,
-		instance->drv_supported_pd_count);
+
+	dev_info(&instance->pdev->dev,
+		"firmware type\t: %s\n",
+		instance->supportmax256vd ? "Extended VD(240 VD)firmware" :
+		"Legacy(64 VD) firmware");
 
 	old_map_sz =  sizeof(struct MR_FW_RAID_MAP) +
 				(sizeof(struct MR_LD_SPAN_MAP) *
@@ -4158,7 +4112,7 @@ megasas_get_ctrl_info(struct megasas_instance *instance)
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
 
 	dcmd->cmd = MFI_CMD_DCMD;
-	dcmd->cmd_status = 0xFF;
+	dcmd->cmd_status = MFI_STAT_INVALID_STATUS;
 	dcmd->sge_count = 1;
 	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_READ);
 	dcmd->timeout = 0;
@@ -4181,16 +4135,17 @@ megasas_get_ctrl_info(struct megasas_instance *instance)
 		le32_to_cpus((u32 *)&ctrl_info->adapterOperations2);
 		le32_to_cpus((u32 *)&ctrl_info->adapterOperations3);
 		megasas_update_ext_vd_details(instance);
+		instance->is_imr = (ctrl_info->memory_size ? 0 : 1);
+		dev_info(&instance->pdev->dev,
+				"controller type\t: %s(%dMB)\n",
+				instance->is_imr ? "iMR" : "MR",
+				le16_to_cpu(ctrl_info->memory_size));
 	}
 
 	pci_free_consistent(instance->pdev, sizeof(struct megasas_ctrl_info),
 			    ci, ci_h);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 	return ret;
 }
 
@@ -4229,7 +4184,7 @@ int megasas_set_crash_dump_params(struct megasas_instance *instance,
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
 	dcmd->mbox.b[0] = crash_buf_state;
 	dcmd->cmd = MFI_CMD_DCMD;
-	dcmd->cmd_status = 0xFF;
+	dcmd->cmd_status = MFI_STAT_INVALID_STATUS;
 	dcmd->sge_count = 1;
 	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_NONE);
 	dcmd->timeout = 0;
@@ -4245,11 +4200,7 @@ int megasas_set_crash_dump_params(struct megasas_instance *instance,
 	else
 		ret = megasas_issue_polled(instance, cmd);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 	return ret;
 }
 
@@ -4262,7 +4213,7 @@ int megasas_set_crash_dump_params(struct megasas_instance *instance,
 static int
 megasas_issue_init_mfi(struct megasas_instance *instance)
 {
-	u32 context;
+	__le32 context;
 
 	struct megasas_cmd *cmd;
 
@@ -4300,7 +4251,7 @@ megasas_issue_init_mfi(struct megasas_instance *instance)
 	initq_info->consumer_index_phys_addr_lo = cpu_to_le32(instance->consumer_h);
 
 	init_frame->cmd = MFI_CMD_INIT;
-	init_frame->cmd_status = 0xFF;
+	init_frame->cmd_status = MFI_STAT_INVALID_STATUS;
 	init_frame->queue_info_new_phys_addr_lo =
 		cpu_to_le32(lower_32_bits(initq_info_h));
 	init_frame->queue_info_new_phys_addr_hi =
@@ -4354,6 +4305,21 @@ megasas_init_adapter_mfi(struct megasas_instance *instance)
 	instance->max_num_sge = (instance->instancet->read_fw_status_reg(reg_set) & 0xFF0000) >>
 					0x10;
 	/*
+	 * For MFI skinny adapters, MEGASAS_SKINNY_INT_CMDS commands
+	 * are reserved for IOCTL + driver's internal DCMDs.
+	 */
+	if ((instance->pdev->device == PCI_DEVICE_ID_LSI_SAS0073SKINNY) ||
+		(instance->pdev->device == PCI_DEVICE_ID_LSI_SAS0071SKINNY)) {
+		instance->max_scsi_cmds = (instance->max_fw_cmds -
+			MEGASAS_SKINNY_INT_CMDS);
+		sema_init(&instance->ioctl_sem, MEGASAS_SKINNY_INT_CMDS);
+	} else {
+		instance->max_scsi_cmds = (instance->max_fw_cmds -
+			MEGASAS_INT_CMDS);
+		sema_init(&instance->ioctl_sem, (MEGASAS_MFI_IOCTL_CMDS));
+	}
+
+	/*
 	 * Create a pool of commands
 	 */
 	if (megasas_alloc_cmds(instance))
@@ -4414,6 +4380,107 @@ fail_alloc_cmds:
 	return 1;
 }
 
+/*
+ * megasas_setup_irqs_msix -		register legacy interrupts.
+ * @instance:				Adapter soft state
+ *
+ * Do not enable interrupt, only setup ISRs.
+ *
+ * Return 0 on success.
+ */
+static int
+megasas_setup_irqs_ioapic(struct megasas_instance *instance)
+{
+	struct pci_dev *pdev;
+
+	pdev = instance->pdev;
+	instance->irq_context[0].instance = instance;
+	instance->irq_context[0].MSIxIndex = 0;
+	if (request_irq(pdev->irq, instance->instancet->service_isr,
+		IRQF_SHARED, "megasas", &instance->irq_context[0])) {
+		dev_err(&instance->pdev->dev,
+				"Failed to register IRQ from %s %d\n",
+				__func__, __LINE__);
+		return -1;
+	}
+	return 0;
+}
+
+/**
+ * megasas_setup_irqs_msix -		register MSI-x interrupts.
+ * @instance:				Adapter soft state
+ * @is_probe:				Driver probe check
+ *
+ * Do not enable interrupt, only setup ISRs.
+ *
+ * Return 0 on success.
+ */
+static int
+megasas_setup_irqs_msix(struct megasas_instance *instance, u8 is_probe)
+{
+	int i, j, cpu;
+	struct pci_dev *pdev;
+
+	pdev = instance->pdev;
+
+	/* Try MSI-x */
+	cpu = cpumask_first(cpu_online_mask);
+	for (i = 0; i < instance->msix_vectors; i++) {
+		instance->irq_context[i].instance = instance;
+		instance->irq_context[i].MSIxIndex = i;
+		if (request_irq(instance->msixentry[i].vector,
+			instance->instancet->service_isr, 0, "megasas",
+			&instance->irq_context[i])) {
+			dev_err(&instance->pdev->dev,
+				"Failed to register IRQ for vector %d.\n", i);
+			for (j = 0; j < i; j++) {
+				if (smp_affinity_enable)
+					irq_set_affinity_hint(
+						instance->msixentry[j].vector, NULL);
+				free_irq(instance->msixentry[j].vector,
+					&instance->irq_context[j]);
+			}
+			/* Retry irq register for IO_APIC*/
+			instance->msix_vectors = 0;
+			if (is_probe)
+				return megasas_setup_irqs_ioapic(instance);
+			else
+				return -1;
+		}
+		if (smp_affinity_enable) {
+			if (irq_set_affinity_hint(instance->msixentry[i].vector,
+				get_cpu_mask(cpu)))
+				dev_err(&instance->pdev->dev,
+					"Failed to set affinity hint"
+					" for cpu %d\n", cpu);
+			cpu = cpumask_next(cpu, cpu_online_mask);
+		}
+	}
+	return 0;
+}
+
+/*
+ * megasas_destroy_irqs-		unregister interrupts.
+ * @instance:				Adapter soft state
+ * return:				void
+ */
+static void
+megasas_destroy_irqs(struct megasas_instance *instance) {
+
+	int i;
+
+	if (instance->msix_vectors)
+		for (i = 0; i < instance->msix_vectors; i++) {
+			if (smp_affinity_enable)
+				irq_set_affinity_hint(
+					instance->msixentry[i].vector, NULL);
+			free_irq(instance->msixentry[i].vector,
+				 &instance->irq_context[i]);
+		}
+	else
+		free_irq(instance->pdev->irq, &instance->irq_context[0]);
+}
+
 /**
  * megasas_init_fw -	Initializes the FW
  * @instance:		Adapter soft state
@@ -4499,7 +4566,7 @@ static int megasas_init_fw(struct megasas_instance *instance)
 	 * It is used for all MPT based Adapters.
 	 */
 	instance->reply_post_host_index_addr[0] =
-		(u32 *)((u8 *)instance->reg_set +
+		(u32 __iomem *)((u8 __iomem *)instance->reg_set +
 		MPI2_REPLY_POST_HOST_INDEX_OFFSET);
 
 	/* Check if MSI-X is supported while in ready state */
@@ -4531,7 +4598,8 @@ static int megasas_init_fw(struct megasas_instance *instance)
 			 */
 			for (loop = 1; loop < MR_MAX_MSIX_REG_ARRAY; loop++) {
 				instance->reply_post_host_index_addr[loop] =
-					(u32 *)((u8 *)instance->reg_set +
+					(u32 __iomem *)
+					((u8 __iomem *)instance->reg_set +
 					MPI2_SUP_REPLY_POST_HOST_INDEX_OFFSET
 					+ (loop * 0x10));
 			}
@@ -4551,14 +4619,19 @@ static int megasas_init_fw(struct megasas_instance *instance)
 			instance->msix_vectors = i;
 		else
 			instance->msix_vectors = 0;
-
-		dev_info(&instance->pdev->dev, "[scsi%d]: FW supports"
-			"<%d> MSIX vector,Online CPUs: <%d>,"
-			"Current MSIX <%d>\n", instance->host->host_no,
-			fw_msix_count, (unsigned int)num_online_cpus(),
-			instance->msix_vectors);
 	}
 
+	dev_info(&instance->pdev->dev,
+		"firmware supports msix\t: (%d)", fw_msix_count);
+	dev_info(&instance->pdev->dev,
+		"current msix/online cpus\t: (%d/%d)\n",
+		instance->msix_vectors, (unsigned int)num_online_cpus());
+
+	if (instance->msix_vectors ?
+		megasas_setup_irqs_msix(instance, 1) :
+		megasas_setup_irqs_ioapic(instance))
+		goto fail_setup_irqs;
+
 	instance->ctrl_info = kzalloc(sizeof(struct megasas_ctrl_info),
 				GFP_KERNEL);
 	if (instance->ctrl_info == NULL)
@@ -4574,6 +4647,11 @@ static int megasas_init_fw(struct megasas_instance *instance)
 	if (instance->instancet->init_adapter(instance))
 		goto fail_init_adapter;
 
+	tasklet_init(&instance->isr_tasklet, instance->instancet->tasklet,
+		(unsigned long)instance);
+
+	instance->instancet->enable_intr(instance);
+
 	printk(KERN_ERR "megasas: INIT adapter done\n");
 
 	/** for passthrough
@@ -4584,7 +4662,7 @@ static int megasas_init_fw(struct megasas_instance *instance)
 		(MEGASAS_MAX_PD * sizeof(struct megasas_pd_list)));
 	if (megasas_get_pd_list(instance) < 0) {
 		printk(KERN_ERR "megasas: failed to get PD list\n");
-		goto fail_init_adapter;
+		goto fail_get_pd_list;
 	}
 
 	memset(instance->ld_ids, 0xff, MEGASAS_MAX_LD_IDS);
@@ -4610,17 +4688,6 @@ static int megasas_init_fw(struct megasas_instance *instance)
 
 	tmp_sectors = min_t(u32, max_sectors_1 , max_sectors_2);
 
-	/*Check whether controller is iMR or MR */
-	if (ctrl_info->memory_size) {
-		instance->is_imr = 0;
-		dev_info(&instance->pdev->dev, "Controller type: MR,"
-			"Memory size is: %dMB\n",
-			le16_to_cpu(ctrl_info->memory_size));
-	} else {
-		instance->is_imr = 1;
-		dev_info(&instance->pdev->dev,
-			"Controller type: iMR\n");
-	}
 	instance->disableOnlineCtrlReset =
 	ctrl_info->properties.OnOffProperties.disableOnlineCtrlReset;
 	instance->mpio = ctrl_info->adapterOperations2.mpio;
@@ -4628,9 +4695,6 @@ static int megasas_init_fw(struct megasas_instance *instance)
 		ctrl_info->adapterOperations2.supportUnevenSpans;
 	if (instance->UnevenSpanSupport) {
 		struct fusion_context *fusion = instance->ctrl_context;
-
-		dev_info(&instance->pdev->dev, "FW supports: "
-		"UnevenSpanSupport=%x\n", instance->UnevenSpanSupport);
 		if (MR_ValidateMapInfo(instance))
 			fusion->fast_path_io = 1;
 		else
@@ -4657,13 +4721,11 @@ static int megasas_init_fw(struct megasas_instance *instance)
 	instance->crash_dump_drv_support =
 		(instance->crash_dump_fw_support &&
 		instance->crash_dump_buf);
-	if (instance->crash_dump_drv_support) {
-		dev_info(&instance->pdev->dev, "Firmware Crash dump "
-			"feature is supported\n");
+	if (instance->crash_dump_drv_support)
 		megasas_set_crash_dump_params(instance,
 			MR_CRASH_BUF_TURN_OFF);
 
-	} else {
+	else {
 		if (instance->crash_dump_buf)
 			pci_free_consistent(instance->pdev,
 				CRASH_DMA_BUF_SIZE,
@@ -4674,37 +4736,28 @@ static int megasas_init_fw(struct megasas_instance *instance)
 
 	instance->secure_jbod_support =
 		ctrl_info->adapterOperations3.supportSecurityonJBOD;
-	if (instance->secure_jbod_support)
-		dev_info(&instance->pdev->dev, "Firmware supports Secure JBOD\n");
+
+	dev_info(&instance->pdev->dev,
+		"pci id\t\t: (0x%04x)/(0x%04x)/(0x%04x)/(0x%04x)\n",
+		le16_to_cpu(ctrl_info->pci.vendor_id),
+		le16_to_cpu(ctrl_info->pci.device_id),
+		le16_to_cpu(ctrl_info->pci.sub_vendor_id),
+		le16_to_cpu(ctrl_info->pci.sub_device_id));
+	dev_info(&instance->pdev->dev, "unevenspan support	: %s\n",
+		instance->UnevenSpanSupport ? "yes" : "no");
+	dev_info(&instance->pdev->dev, "disable ocr		: %s\n",
+		instance->disableOnlineCtrlReset ? "yes" : "no");
+	dev_info(&instance->pdev->dev, "firmware crash dump	: %s\n",
+		instance->crash_dump_drv_support ? "yes" : "no");
+	dev_info(&instance->pdev->dev, "secure jbod		: %s\n",
+		instance->secure_jbod_support ? "yes" : "no");
+
+
 	instance->max_sectors_per_req = instance->max_num_sge *
 						PAGE_SIZE / 512;
 	if (tmp_sectors && (instance->max_sectors_per_req > tmp_sectors))
 		instance->max_sectors_per_req = tmp_sectors;
 
-	/*
-	 * 1. For fusion adapters, 3 commands for IOCTL and 5 commands
-	 *    for driver's internal DCMDs.
-	 * 2. For MFI skinny adapters, 5 commands for IOCTL + driver's
-	 *    internal DCMDs.
-	 * 3. For rest of MFI adapters, 27 commands reserved for IOCTLs
-	 *    and 5 commands for drivers's internal DCMD.
-	 */
-	if (instance->ctrl_context) {
-		instance->max_scsi_cmds = instance->max_fw_cmds -
-					(MEGASAS_FUSION_INTERNAL_CMDS +
-					MEGASAS_FUSION_IOCTL_CMDS);
-		sema_init(&instance->ioctl_sem, MEGASAS_FUSION_IOCTL_CMDS);
-	} else if ((instance->pdev->device == PCI_DEVICE_ID_LSI_SAS0073SKINNY) ||
-		(instance->pdev->device == PCI_DEVICE_ID_LSI_SAS0071SKINNY)) {
-		instance->max_scsi_cmds = instance->max_fw_cmds -
-						MEGASAS_SKINNY_INT_CMDS;
-		sema_init(&instance->ioctl_sem, MEGASAS_SKINNY_INT_CMDS);
-	} else {
-		instance->max_scsi_cmds = instance->max_fw_cmds -
-						MEGASAS_INT_CMDS;
-		sema_init(&instance->ioctl_sem, (MEGASAS_INT_CMDS - 5));
-	}
-
 	/* Check for valid throttlequeuedepth module parameter */
 	if (throttlequeuedepth &&
 			throttlequeuedepth <= instance->max_scsi_cmds)
@@ -4713,12 +4766,6 @@ static int megasas_init_fw(struct megasas_instance *instance)
 		instance->throttlequeuedepth =
 				MEGASAS_THROTTLE_QUEUE_DEPTH;
 
-        /*
-	* Setup tasklet for cmd completion
-	*/
-
-	tasklet_init(&instance->isr_tasklet, instance->instancet->tasklet,
-		(unsigned long)instance);
 
 	/* Launch SR-IOV heartbeat timer */
 	if (instance->requestorId) {
@@ -4733,7 +4780,14 @@ static int megasas_init_fw(struct megasas_instance *instance)
 
 	return 0;
 
+fail_get_pd_list:
+	instance->instancet->disable_intr(instance);
 fail_init_adapter:
+	megasas_destroy_irqs(instance);
+fail_setup_irqs:
+	if (instance->msix_vectors)
+		pci_disable_msix(instance->pdev);
+	instance->msix_vectors = 0;
 fail_ready_state:
 	kfree(instance->ctrl_info);
 	instance->ctrl_info = NULL;
@@ -4747,7 +4801,7 @@ fail_ready_state:
 
 /**
  * megasas_release_mfi -	Reverses the FW initialization
- * @intance:			Adapter soft state
+ * @instance:			Adapter soft state
  */
 static void megasas_release_mfi(struct megasas_instance *instance)
 {
@@ -4822,21 +4876,17 @@ megasas_get_seq_num(struct megasas_instance *instance,
 		/*
 		 * Copy the data back into callers buffer
 		 */
-		eli->newest_seq_num = le32_to_cpu(el_info->newest_seq_num);
-		eli->oldest_seq_num = le32_to_cpu(el_info->oldest_seq_num);
-		eli->clear_seq_num = le32_to_cpu(el_info->clear_seq_num);
-		eli->shutdown_seq_num = le32_to_cpu(el_info->shutdown_seq_num);
-		eli->boot_seq_num = le32_to_cpu(el_info->boot_seq_num);
+		eli->newest_seq_num = el_info->newest_seq_num;
+		eli->oldest_seq_num = el_info->oldest_seq_num;
+		eli->clear_seq_num = el_info->clear_seq_num;
+		eli->shutdown_seq_num = el_info->shutdown_seq_num;
+		eli->boot_seq_num = el_info->boot_seq_num;
 	}
 
 	pci_free_consistent(instance->pdev, sizeof(struct megasas_evt_log_info),
 			    el_info, el_info_h);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	return 0;
 }
@@ -4877,8 +4927,8 @@ megasas_register_aen(struct megasas_instance *instance, u32 seq_num,
 
 	if (instance->aen_cmd) {
 
-		prev_aen.word = instance->aen_cmd->frame->dcmd.mbox.w[1];
-		prev_aen.members.locale = le16_to_cpu(prev_aen.members.locale);
+		prev_aen.word =
+			le32_to_cpu(instance->aen_cmd->frame->dcmd.mbox.w[1]);
 
 		/*
 		 * A class whose enum value is smaller is inclusive of all
@@ -4990,7 +5040,7 @@ static int megasas_start_aen(struct megasas_instance *instance)
 	class_locale.members.class = MR_EVT_CLASS_DEBUG;
 
 	return megasas_register_aen(instance,
-			eli.newest_seq_num + 1,
+			le32_to_cpu(eli.newest_seq_num) + 1,
 			class_locale.word);
 }
 
@@ -5001,6 +5051,7 @@ static int megasas_start_aen(struct megasas_instance *instance)
 static int megasas_io_attach(struct megasas_instance *instance)
 {
 	struct Scsi_Host *host = instance->host;
+	u32		error;
 
 	/*
 	 * Export parameters required by SCSI mid-layer
@@ -5050,12 +5101,21 @@ static int megasas_io_attach(struct megasas_instance *instance)
 		host->hostt->eh_device_reset_handler = NULL;
 		host->hostt->eh_bus_reset_handler = NULL;
 	}
+	error = scsi_init_shared_tag_map(host, host->can_queue);
+	if (error) {
+		dev_err(&instance->pdev->dev,
+			"Failed to shared tag from %s %d\n",
+			__func__, __LINE__);
+		return -ENODEV;
+	}
 
 	/*
 	 * Notify the mid-layer about the new controller
 	 */
 	if (scsi_add_host(host, &instance->pdev->dev)) {
-		printk(KERN_DEBUG "megasas: scsi_add_host failed\n");
+		dev_err(&instance->pdev->dev,
+			"Failed to add host from %s %d\n",
+			__func__, __LINE__);
 		return -ENODEV;
 	}
 
@@ -5106,7 +5166,7 @@ fail_set_dma_mask:
 static int megasas_probe_one(struct pci_dev *pdev,
 			     const struct pci_device_id *id)
 {
-	int rval, pos, i, j, cpu;
+	int rval, pos;
 	struct Scsi_Host *host;
 	struct megasas_instance *instance;
 	u16 control = 0;
@@ -5129,16 +5189,6 @@ static int megasas_probe_one(struct pci_dev *pdev,
 	}
 
 	/*
-	 * Announce PCI information
-	 */
-	printk(KERN_INFO "megasas: %#4.04x:%#4.04x:%#4.04x:%#4.04x: ",
-	       pdev->vendor, pdev->device, pdev->subsystem_vendor,
-	       pdev->subsystem_device);
-
-	printk("bus %d:slot %d:func %d\n",
-	       pdev->bus->number, PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
-
-	/*
 	 * PCI prepping: enable device set bus mastering and dma mask
 	 */
 	rval = pci_enable_device_mem(pdev);
@@ -5183,8 +5233,6 @@ static int megasas_probe_one(struct pci_dev *pdev,
 		fusion = instance->ctrl_context;
 		memset(fusion, 0,
 			((1 << PAGE_SHIFT) << instance->ctrl_context_pages));
-		INIT_LIST_HEAD(&fusion->cmd_pool);
-		spin_lock_init(&fusion->mpt_pool_lock);
 	}
 	break;
 	default: /* For all other supported controllers */
@@ -5207,6 +5255,13 @@ static int megasas_probe_one(struct pci_dev *pdev,
 		break;
 	}
 
+	instance->system_info_buf = pci_zalloc_consistent(pdev,
+					sizeof(struct MR_DRV_SYSTEM_INFO),
+					&instance->system_info_h);
+
+	if (!instance->system_info_buf)
+		dev_info(&instance->pdev->dev, "Can't allocate system info buffer\n");
+
 	/* Crash dump feature related initialisation*/
 	instance->drv_buf_index = 0;
 	instance->drv_buf_alloc = 0;
@@ -5315,55 +5370,6 @@ static int megasas_probe_one(struct pci_dev *pdev,
 		}
 	}
 
-retry_irq_register:
-	/*
-	 * Register IRQ
-	 */
-	if (instance->msix_vectors) {
-		cpu = cpumask_first(cpu_online_mask);
-		for (i = 0; i < instance->msix_vectors; i++) {
-			instance->irq_context[i].instance = instance;
-			instance->irq_context[i].MSIxIndex = i;
-			if (request_irq(instance->msixentry[i].vector,
-					instance->instancet->service_isr, 0,
-					"megasas",
-					&instance->irq_context[i])) {
-				printk(KERN_DEBUG "megasas: Failed to "
-				       "register IRQ for vector %d.\n", i);
-				for (j = 0; j < i; j++) {
-					if (smp_affinity_enable)
-						irq_set_affinity_hint(
-							instance->msixentry[j].vector, NULL);
-					free_irq(
-						instance->msixentry[j].vector,
-						&instance->irq_context[j]);
-				}
-				/* Retry irq register for IO_APIC */
-				instance->msix_vectors = 0;
-				goto retry_irq_register;
-			}
-			if (smp_affinity_enable) {
-				if (irq_set_affinity_hint(instance->msixentry[i].vector,
-					get_cpu_mask(cpu)))
-					dev_err(&instance->pdev->dev,
-						"Error setting affinity hint "
-						"for cpu %d\n", cpu);
-				cpu = cpumask_next(cpu, cpu_online_mask);
-			}
-		}
-	} else {
-		instance->irq_context[0].instance = instance;
-		instance->irq_context[0].MSIxIndex = 0;
-		if (request_irq(pdev->irq, instance->instancet->service_isr,
-				IRQF_SHARED, "megasas",
-				&instance->irq_context[0])) {
-			printk(KERN_DEBUG "megasas: Failed to register IRQ\n");
-			goto fail_irq;
-		}
-	}
-
-	instance->instancet->enable_intr(instance);
-
 	/*
 	 * Store instance in PCI softstate
 	 */
@@ -5410,17 +5416,8 @@ retry_irq_register:
 	megasas_mgmt_info.max_index--;
 
 	instance->instancet->disable_intr(instance);
-	if (instance->msix_vectors)
-		for (i = 0; i < instance->msix_vectors; i++) {
-			if (smp_affinity_enable)
-				irq_set_affinity_hint(
-					instance->msixentry[i].vector, NULL);
-			free_irq(instance->msixentry[i].vector,
-				 &instance->irq_context[i]);
-		}
-	else
-		free_irq(instance->pdev->irq, &instance->irq_context[0]);
-fail_irq:
+	megasas_destroy_irqs(instance);
+
 	if ((instance->pdev->device == PCI_DEVICE_ID_LSI_FUSION) ||
 	    (instance->pdev->device == PCI_DEVICE_ID_LSI_PLASMA) ||
 	    (instance->pdev->device == PCI_DEVICE_ID_LSI_INVADER) ||
@@ -5428,9 +5425,9 @@ fail_irq:
 		megasas_release_fusion(instance);
 	else
 		megasas_release_mfi(instance);
-      fail_init_mfi:
 	if (instance->msix_vectors)
 		pci_disable_msix(instance->pdev);
+fail_init_mfi:
       fail_alloc_dma_buf:
 	if (instance->evt_detail)
 		pci_free_consistent(pdev, sizeof(struct megasas_evt_detail),
@@ -5487,11 +5484,7 @@ static void megasas_flush_cache(struct megasas_instance *instance)
 		dev_err(&instance->pdev->dev, "Command timedout"
 			" from %s\n", __func__);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	return;
 }
@@ -5538,11 +5531,7 @@ static void megasas_shutdown_controller(struct megasas_instance *instance,
 		dev_err(&instance->pdev->dev, "Command timedout"
 			"from %s\n", __func__);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	return;
 }
@@ -5558,7 +5547,6 @@ megasas_suspend(struct pci_dev *pdev, pm_message_t state)
 {
 	struct Scsi_Host *host;
 	struct megasas_instance *instance;
-	int i;
 
 	instance = pci_get_drvdata(pdev);
 	host = instance->host;
@@ -5583,16 +5571,8 @@ megasas_suspend(struct pci_dev *pdev, pm_message_t state)
 	pci_set_drvdata(instance->pdev, instance);
 	instance->instancet->disable_intr(instance);
 
-	if (instance->msix_vectors)
-		for (i = 0; i < instance->msix_vectors; i++) {
-			if (smp_affinity_enable)
-				irq_set_affinity_hint(
-					instance->msixentry[i].vector, NULL);
-			free_irq(instance->msixentry[i].vector,
-				 &instance->irq_context[i]);
-		}
-	else
-		free_irq(instance->pdev->irq, &instance->irq_context[0]);
+	megasas_destroy_irqs(instance);
+
 	if (instance->msix_vectors)
 		pci_disable_msix(instance->pdev);
 
@@ -5611,7 +5591,7 @@ megasas_suspend(struct pci_dev *pdev, pm_message_t state)
 static int
 megasas_resume(struct pci_dev *pdev)
 {
-	int rval, i, j, cpu;
+	int rval;
 	struct Scsi_Host *host;
 	struct megasas_instance *instance;
 
@@ -5681,50 +5661,10 @@ megasas_resume(struct pci_dev *pdev)
 	tasklet_init(&instance->isr_tasklet, instance->instancet->tasklet,
 		     (unsigned long)instance);
 
-	/*
-	 * Register IRQ
-	 */
-	if (instance->msix_vectors) {
-		cpu = cpumask_first(cpu_online_mask);
-		for (i = 0 ; i < instance->msix_vectors; i++) {
-			instance->irq_context[i].instance = instance;
-			instance->irq_context[i].MSIxIndex = i;
-			if (request_irq(instance->msixentry[i].vector,
-					instance->instancet->service_isr, 0,
-					"megasas",
-					&instance->irq_context[i])) {
-				printk(KERN_DEBUG "megasas: Failed to "
-				       "register IRQ for vector %d.\n", i);
-				for (j = 0; j < i; j++) {
-					if (smp_affinity_enable)
-						irq_set_affinity_hint(
-							instance->msixentry[j].vector, NULL);
-					free_irq(
-						instance->msixentry[j].vector,
-						&instance->irq_context[j]);
-				}
-				goto fail_irq;
-			}
-
-			if (smp_affinity_enable) {
-				if (irq_set_affinity_hint(instance->msixentry[i].vector,
-					get_cpu_mask(cpu)))
-					dev_err(&instance->pdev->dev, "Error "
-						"setting affinity hint for cpu "
-						"%d\n", cpu);
-				cpu = cpumask_next(cpu, cpu_online_mask);
-			}
-		}
-	} else {
-		instance->irq_context[0].instance = instance;
-		instance->irq_context[0].MSIxIndex = 0;
-		if (request_irq(pdev->irq, instance->instancet->service_isr,
-				IRQF_SHARED, "megasas",
-				&instance->irq_context[0])) {
-			printk(KERN_DEBUG "megasas: Failed to register IRQ\n");
-			goto fail_irq;
-		}
-	}
+	if (instance->msix_vectors ?
+			megasas_setup_irqs_msix(instance, 0) :
+			megasas_setup_irqs_ioapic(instance))
+		goto fail_init_mfi;
 
 	/* Re-launch SR-IOV heartbeat timer */
 	if (instance->requestorId) {
@@ -5733,8 +5673,10 @@ megasas_resume(struct pci_dev *pdev)
 					    &instance->sriov_heartbeat_timer,
 					    megasas_sriov_heartbeat_handler,
 					    MEGASAS_SRIOV_HEARTBEAT_INTERVAL_VF);
-		else
+		else {
 			instance->skip_heartbeat_timer_del = 1;
+			goto fail_init_mfi;
+		}
 	}
 
 	instance->instancet->enable_intr(instance);
@@ -5748,7 +5690,6 @@ megasas_resume(struct pci_dev *pdev)
 
 	return 0;
 
-fail_irq:
 fail_init_mfi:
 	if (instance->evt_detail)
 		pci_free_consistent(pdev, sizeof(struct megasas_evt_detail),
@@ -5829,16 +5770,8 @@ static void megasas_detach_one(struct pci_dev *pdev)
 
 	instance->instancet->disable_intr(instance);
 
-	if (instance->msix_vectors)
-		for (i = 0; i < instance->msix_vectors; i++) {
-			if (smp_affinity_enable)
-				irq_set_affinity_hint(
-					instance->msixentry[i].vector, NULL);
-			free_irq(instance->msixentry[i].vector,
-				 &instance->irq_context[i]);
-		}
-	else
-		free_irq(instance->pdev->irq, &instance->irq_context[0]);
+	megasas_destroy_irqs(instance);
+
 	if (instance->msix_vectors)
 		pci_disable_msix(instance->pdev);
 
@@ -5899,6 +5832,10 @@ static void megasas_detach_one(struct pci_dev *pdev)
 		pci_free_consistent(pdev, CRASH_DMA_BUF_SIZE,
 			    instance->crash_dump_buf, instance->crash_dump_h);
 
+	if (instance->system_info_buf)
+		pci_free_consistent(pdev, sizeof(struct MR_DRV_SYSTEM_INFO),
+				    instance->system_info_buf, instance->system_info_h);
+
 	scsi_host_put(host);
 
 	pci_disable_device(pdev);
@@ -5912,23 +5849,14 @@ static void megasas_detach_one(struct pci_dev *pdev)
  */
 static void megasas_shutdown(struct pci_dev *pdev)
 {
-	int i;
 	struct megasas_instance *instance = pci_get_drvdata(pdev);
 
 	instance->unload = 1;
 	megasas_flush_cache(instance);
 	megasas_shutdown_controller(instance, MR_DCMD_CTRL_SHUTDOWN);
 	instance->instancet->disable_intr(instance);
-	if (instance->msix_vectors)
-		for (i = 0; i < instance->msix_vectors; i++) {
-			if (smp_affinity_enable)
-				irq_set_affinity_hint(
-					instance->msixentry[i].vector, NULL);
-			free_irq(instance->msixentry[i].vector,
-				 &instance->irq_context[i]);
-		}
-	else
-		free_irq(instance->pdev->irq, &instance->irq_context[0]);
+	megasas_destroy_irqs(instance);
+
 	if (instance->msix_vectors)
 		pci_disable_msix(instance->pdev);
 }
@@ -6211,11 +6139,7 @@ megasas_mgmt_fw_ioctl(struct megasas_instance *instance,
 			kbuff_arr[i] = NULL;
 	}
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 	return error;
 }
 
@@ -6502,6 +6426,15 @@ static ssize_t megasas_sysfs_show_version(struct device_driver *dd, char *buf)
 static DRIVER_ATTR(version, S_IRUGO, megasas_sysfs_show_version, NULL);
 
 static ssize_t
+megasas_sysfs_show_release_date(struct device_driver *dd, char *buf)
+{
+	return snprintf(buf, strlen(MEGASAS_RELDATE) + 2, "%s\n",
+		MEGASAS_RELDATE);
+}
+
+static DRIVER_ATTR(release_date, S_IRUGO, megasas_sysfs_show_release_date, NULL);
+
+static ssize_t
 megasas_sysfs_show_support_poll_for_event(struct device_driver *dd, char *buf)
 {
 	return sprintf(buf, "%u\n", support_poll_for_event);
@@ -6841,6 +6774,11 @@ static int __init megasas_init(void)
 		goto err_dcf_attr_ver;
 
 	rval = driver_create_file(&megasas_pci_driver.driver,
+				  &driver_attr_release_date);
+	if (rval)
+		goto err_dcf_rel_date;
+
+	rval = driver_create_file(&megasas_pci_driver.driver,
 				&driver_attr_support_poll_for_event);
 	if (rval)
 		goto err_dcf_support_poll_for_event;
@@ -6863,6 +6801,9 @@ err_dcf_dbg_lvl:
 	driver_remove_file(&megasas_pci_driver.driver,
 			&driver_attr_support_poll_for_event);
 err_dcf_support_poll_for_event:
+	driver_remove_file(&megasas_pci_driver.driver,
+			   &driver_attr_release_date);
+err_dcf_rel_date:
 	driver_remove_file(&megasas_pci_driver.driver, &driver_attr_version);
 err_dcf_attr_ver:
 	pci_unregister_driver(&megasas_pci_driver);
@@ -6882,6 +6823,8 @@ static void __exit megasas_exit(void)
 			&driver_attr_support_poll_for_event);
 	driver_remove_file(&megasas_pci_driver.driver,
 			&driver_attr_support_device_change);
+	driver_remove_file(&megasas_pci_driver.driver,
+			   &driver_attr_release_date);
 	driver_remove_file(&megasas_pci_driver.driver, &driver_attr_version);
 
 	pci_unregister_driver(&megasas_pci_driver);
diff --git a/drivers/scsi/megaraid/megaraid_sas_fp.c b/drivers/scsi/megaraid/megaraid_sas_fp.c
index 4f72287860ee..be57b18675a4 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fp.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fp.c
@@ -66,7 +66,15 @@ MODULE_PARM_DESC(lb_pending_cmds, "Change raid-1 load balancing outstanding "
 
 #define ABS_DIFF(a, b)   (((a) > (b)) ? ((a) - (b)) : ((b) - (a)))
 #define MR_LD_STATE_OPTIMAL 3
+
+#ifdef FALSE
+#undef FALSE
+#endif
 #define FALSE 0
+
+#ifdef TRUE
+#undef TRUE
+#endif
 #define TRUE 1
 
 #define SPAN_DEBUG 0
@@ -142,7 +150,7 @@ u16 MR_LdSpanArrayGet(u32 ld, u32 span, struct MR_DRV_RAID_MAP_ALL *map)
 	return le16_to_cpu(map->raidMap.ldSpanMap[ld].spanBlock[span].span.arrayRef);
 }
 
-u16 MR_PdDevHandleGet(u32 pd, struct MR_DRV_RAID_MAP_ALL *map)
+__le16 MR_PdDevHandleGet(u32 pd, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return map->raidMap.devHndlInfo[pd].curDevHdl;
 }
@@ -735,7 +743,7 @@ static u8 mr_spanset_get_phy_params(struct megasas_instance *instance, u32 ld,
 	u8	retval = TRUE;
 	u8	do_invader = 0;
 	u64	*pdBlock = &io_info->pdBlock;
-	u16	*pDevHandle = &io_info->devHandle;
+	__le16	*pDevHandle = &io_info->devHandle;
 	u32	logArm, rowMod, armQ, arm;
 
 	if ((instance->pdev->device == PCI_DEVICE_ID_LSI_INVADER ||
@@ -769,7 +777,7 @@ static u8 mr_spanset_get_phy_params(struct megasas_instance *instance, u32 ld,
 	if (pd != MR_PD_INVALID)
 		*pDevHandle = MR_PdDevHandleGet(pd, map);
 	else {
-		*pDevHandle = MR_PD_INVALID;
+		*pDevHandle = cpu_to_le16(MR_PD_INVALID);
 		if ((raid->level >= 5) &&
 			(!do_invader  || (do_invader &&
 			(raid->regTypeReqOnRead != REGION_TYPE_UNUSED))))
@@ -817,7 +825,7 @@ u8 MR_GetPhyParams(struct megasas_instance *instance, u32 ld, u64 stripRow,
 	u8	    retval = TRUE;
 	u8          do_invader = 0;
 	u64	    *pdBlock = &io_info->pdBlock;
-	u16	    *pDevHandle = &io_info->devHandle;
+	__le16	    *pDevHandle = &io_info->devHandle;
 
 	if ((instance->pdev->device == PCI_DEVICE_ID_LSI_INVADER ||
 		instance->pdev->device == PCI_DEVICE_ID_LSI_FURY))
@@ -864,7 +872,8 @@ u8 MR_GetPhyParams(struct megasas_instance *instance, u32 ld, u64 stripRow,
 		/* Get dev handle from Pd. */
 		*pDevHandle = MR_PdDevHandleGet(pd, map);
 	else {
-		*pDevHandle = MR_PD_INVALID; /* set dev handle as invalid. */
+		/* set dev handle as invalid. */
+		*pDevHandle = cpu_to_le16(MR_PD_INVALID);
 		if ((raid->level >= 5) &&
 			(!do_invader  || (do_invader &&
 			(raid->regTypeReqOnRead != REGION_TYPE_UNUSED))))
@@ -1109,7 +1118,7 @@ MR_BuildRaidContext(struct megasas_instance *instance,
 					ref_in_start_stripe, io_info,
 					pRAID_Context, map);
 		/* If IO on an invalid Pd, then FP is not possible.*/
-		if (io_info->devHandle == MR_PD_INVALID)
+		if (io_info->devHandle == cpu_to_le16(MR_PD_INVALID))
 			io_info->fpOkForIo = FALSE;
 		return retval;
 	} else if (isRead) {
@@ -1341,11 +1350,11 @@ u8 megasas_get_best_arm_pd(struct megasas_instance *instance,
 	return io_info->pd_after_lb;
 }
 
-u16 get_updated_dev_handle(struct megasas_instance *instance,
+__le16 get_updated_dev_handle(struct megasas_instance *instance,
 	struct LD_LOAD_BALANCE_INFO *lbInfo, struct IO_REQUEST_INFO *io_info)
 {
 	u8 arm_pd;
-	u16 devHandle;
+	__le16 devHandle;
 	struct fusion_context *fusion;
 	struct MR_DRV_RAID_MAP_ALL *drv_map;
 
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.c b/drivers/scsi/megaraid/megaraid_sas_fusion.c
index 5a0800d19970..46a0f8f4f677 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.c
@@ -53,10 +53,12 @@
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_dbg.h>
+#include <linux/dmi.h>
 
 #include "megaraid_sas_fusion.h"
 #include "megaraid_sas.h"
 
+
 extern void megasas_free_cmds(struct megasas_instance *instance);
 extern struct megasas_cmd *megasas_get_cmd(struct megasas_instance
 					   *instance);
@@ -156,28 +158,15 @@ megasas_clear_intr_fusion(struct megasas_register_set __iomem *regs)
  * megasas_get_cmd_fusion -	Get a command from the free pool
  * @instance:		Adapter soft state
  *
- * Returns a free command from the pool
+ * Returns a blk_tag indexed mpt frame
  */
-struct megasas_cmd_fusion *megasas_get_cmd_fusion(struct megasas_instance
-						  *instance)
+inline struct megasas_cmd_fusion *megasas_get_cmd_fusion(struct megasas_instance
+						  *instance, u32 blk_tag)
 {
-	unsigned long flags;
-	struct fusion_context *fusion =
-		(struct fusion_context *)instance->ctrl_context;
-	struct megasas_cmd_fusion *cmd = NULL;
-
-	spin_lock_irqsave(&fusion->mpt_pool_lock, flags);
-
-	if (!list_empty(&fusion->cmd_pool)) {
-		cmd = list_entry((&fusion->cmd_pool)->next,
-				 struct megasas_cmd_fusion, list);
-		list_del_init(&cmd->list);
-	} else {
-		printk(KERN_ERR "megasas: Command pool (fusion) empty!\n");
-	}
+	struct fusion_context *fusion;
 
-	spin_unlock_irqrestore(&fusion->mpt_pool_lock, flags);
-	return cmd;
+	fusion = instance->ctrl_context;
+	return fusion->cmd_list[blk_tag];
 }
 
 /**
@@ -188,47 +177,35 @@ struct megasas_cmd_fusion *megasas_get_cmd_fusion(struct megasas_instance
 inline void megasas_return_cmd_fusion(struct megasas_instance *instance,
 	struct megasas_cmd_fusion *cmd)
 {
-	unsigned long flags;
-	struct fusion_context *fusion =
-		(struct fusion_context *)instance->ctrl_context;
-
-	spin_lock_irqsave(&fusion->mpt_pool_lock, flags);
-
 	cmd->scmd = NULL;
-	cmd->sync_cmd_idx = (u32)ULONG_MAX;
 	memset(cmd->io_request, 0, sizeof(struct MPI2_RAID_SCSI_IO_REQUEST));
-	list_add(&cmd->list, (&fusion->cmd_pool)->next);
-
-	spin_unlock_irqrestore(&fusion->mpt_pool_lock, flags);
 }
 
 /**
- * megasas_return_mfi_mpt_pthr - Return a mfi and mpt to free command pool
- * @instance:		Adapter soft state
- * @cmd_mfi:		MFI Command packet to be returned to free command pool
- * @cmd_mpt:		MPT Command packet to be returned to free command pool
+ * megasas_fire_cmd_fusion -	Sends command to the FW
  */
-inline void megasas_return_mfi_mpt_pthr(struct megasas_instance *instance,
-		struct megasas_cmd *cmd_mfi,
-		struct megasas_cmd_fusion *cmd_fusion)
+static void
+megasas_fire_cmd_fusion(struct megasas_instance *instance,
+		union MEGASAS_REQUEST_DESCRIPTOR_UNION *req_desc)
 {
+#if defined(writeq) && defined(CONFIG_64BIT)
+	u64 req_data = (((u64)le32_to_cpu(req_desc->u.high) << 32) |
+			le32_to_cpu(req_desc->u.low));
+
+	writeq(req_data, &instance->reg_set->inbound_low_queue_port);
+#else
 	unsigned long flags;
 
-	/*
-	 * TO DO: optimize this code and use only one lock instead of two
-	 * locks being used currently- mpt_pool_lock is acquired
-	 * inside mfi_pool_lock
-	 */
-	spin_lock_irqsave(&instance->mfi_pool_lock, flags);
-	megasas_return_cmd_fusion(instance, cmd_fusion);
-	if (atomic_read(&cmd_mfi->mfi_mpt_pthr) != MFI_MPT_ATTACHED)
-		dev_err(&instance->pdev->dev, "Possible bug from %s %d\n",
-			__func__, __LINE__);
-	atomic_set(&cmd_mfi->mfi_mpt_pthr, MFI_MPT_DETACHED);
-	__megasas_return_cmd(instance, cmd_mfi);
-	spin_unlock_irqrestore(&instance->mfi_pool_lock, flags);
+	spin_lock_irqsave(&instance->hba_lock, flags);
+	writel(le32_to_cpu(req_desc->u.low),
+		&instance->reg_set->inbound_low_queue_port);
+	writel(le32_to_cpu(req_desc->u.high),
+		&instance->reg_set->inbound_high_queue_port);
+	spin_unlock_irqrestore(&instance->hba_lock, flags);
+#endif
 }
 
+
 /**
  * megasas_teardown_frame_pool_fusion -	Destroy the cmd frame DMA pool
  * @instance:				Adapter soft state
@@ -326,7 +303,6 @@ megasas_free_cmds_fusion(struct megasas_instance *instance)
 	kfree(fusion->cmd_list);
 	fusion->cmd_list = NULL;
 
-	INIT_LIST_HEAD(&fusion->cmd_pool);
 }
 
 /**
@@ -464,7 +440,7 @@ megasas_alloc_cmds_fusion(struct megasas_instance *instance)
 
 	reply_desc = fusion->reply_frames_desc;
 	for (i = 0; i < fusion->reply_q_depth * count; i++, reply_desc++)
-		reply_desc->Words = ULLONG_MAX;
+		reply_desc->Words = cpu_to_le64(ULLONG_MAX);
 
 	io_frames_sz = fusion->io_frames_alloc_sz;
 
@@ -535,7 +511,9 @@ megasas_alloc_cmds_fusion(struct megasas_instance *instance)
 		memset(cmd, 0, sizeof(struct megasas_cmd_fusion));
 		cmd->index = i + 1;
 		cmd->scmd = NULL;
-		cmd->sync_cmd_idx = (u32)ULONG_MAX; /* Set to Invalid */
+		cmd->sync_cmd_idx = (i >= instance->max_scsi_cmds) ?
+				(i - instance->max_scsi_cmds) :
+				(u32)ULONG_MAX; /* Set to Invalid */
 		cmd->instance = instance;
 		cmd->io_request =
 			(struct MPI2_RAID_SCSI_IO_REQUEST *)
@@ -543,8 +521,6 @@ megasas_alloc_cmds_fusion(struct megasas_instance *instance)
 		memset(cmd->io_request, 0,
 		       sizeof(struct MPI2_RAID_SCSI_IO_REQUEST));
 		cmd->io_request_phys_addr = io_req_base_phys + offset;
-
-		list_add_tail(&cmd->list, &fusion->cmd_pool);
 	}
 
 	/*
@@ -605,14 +581,11 @@ wait_and_poll(struct megasas_instance *instance, struct megasas_cmd *cmd,
 		msleep(20);
 	}
 
-	if (frame_hdr->cmd_status == 0xff) {
-		if (fusion)
-			megasas_return_mfi_mpt_pthr(instance, cmd,
-				cmd->mpt_pthr_cmd_blocked);
+	if (frame_hdr->cmd_status == 0xff)
 		return -ETIME;
-	}
 
-	return 0;
+	return (frame_hdr->cmd_status == MFI_STAT_OK) ?
+		0 : 1;
 }
 
 /**
@@ -633,6 +606,7 @@ megasas_ioc_init_fusion(struct megasas_instance *instance)
 	union MEGASAS_REQUEST_DESCRIPTOR_UNION req_desc;
 	int i;
 	struct megasas_header *frame_hdr;
+	const char *sys_info;
 
 	fusion = instance->ctrl_context;
 
@@ -673,7 +647,9 @@ megasas_ioc_init_fusion(struct megasas_instance *instance)
 
 	frame_hdr = &cmd->frame->hdr;
 	frame_hdr->cmd_status = 0xFF;
-	frame_hdr->flags |= cpu_to_le16(MFI_FRAME_DONT_POST_IN_REPLY_QUEUE);
+	frame_hdr->flags = cpu_to_le16(
+		le16_to_cpu(frame_hdr->flags) |
+		MFI_FRAME_DONT_POST_IN_REPLY_QUEUE);
 
 	init_frame->cmd	= MFI_CMD_INIT;
 	init_frame->cmd_status = 0xFF;
@@ -695,6 +671,16 @@ megasas_ioc_init_fusion(struct megasas_instance *instance)
 	/* Convert capability to LE32 */
 	cpu_to_le32s((u32 *)&init_frame->driver_operations.mfi_capabilities);
 
+	sys_info = dmi_get_system_info(DMI_PRODUCT_UUID);
+	if (instance->system_info_buf && sys_info) {
+		memcpy(instance->system_info_buf->systemId, sys_info,
+			strlen(sys_info) > 64 ? 64 : strlen(sys_info));
+		instance->system_info_buf->systemIdLength =
+			strlen(sys_info) > 64 ? 64 : strlen(sys_info);
+		init_frame->system_info_lo = instance->system_info_h;
+		init_frame->system_info_hi = 0;
+	}
+
 	init_frame->queue_info_new_phys_addr_hi =
 		cpu_to_le32(upper_32_bits(ioc_init_handle));
 	init_frame->queue_info_new_phys_addr_lo =
@@ -719,8 +705,7 @@ megasas_ioc_init_fusion(struct megasas_instance *instance)
 			break;
 	}
 
-	instance->instancet->fire_cmd(instance, req_desc.u.low,
-				      req_desc.u.high, instance->reg_set);
+	megasas_fire_cmd_fusion(instance, &req_desc);
 
 	wait_and_poll(instance, cmd, MFI_POLL_TIMEOUT_SECS);
 
@@ -820,11 +805,7 @@ megasas_get_ld_map_info(struct megasas_instance *instance)
 	else
 		ret = megasas_issue_polled(instance, cmd);
 
-	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
-		megasas_return_mfi_mpt_pthr(instance, cmd,
-			cmd->mpt_pthr_cmd_blocked);
-	else
-		megasas_return_cmd(instance, cmd);
+	megasas_return_cmd(instance, cmd);
 
 	return ret;
 }
@@ -1061,6 +1042,15 @@ megasas_init_adapter_fusion(struct megasas_instance *instance)
 		fusion->last_reply_idx[i] = 0;
 
 	/*
+	 * For fusion adapters, 3 commands for IOCTL and 5 commands
+	 * for driver's internal DCMDs.
+	 */
+	instance->max_scsi_cmds = instance->max_fw_cmds -
+				(MEGASAS_FUSION_INTERNAL_CMDS +
+				MEGASAS_FUSION_IOCTL_CMDS);
+	sema_init(&instance->ioctl_sem, MEGASAS_FUSION_IOCTL_CMDS);
+
+	/*
 	 * Allocate memory for descriptors
 	 * Create a pool of commands
 	 */
@@ -1131,34 +1121,6 @@ fail_alloc_mfi_cmds:
 }
 
 /**
- * megasas_fire_cmd_fusion -	Sends command to the FW
- * @frame_phys_addr :		Physical address of cmd
- * @frame_count :		Number of frames for the command
- * @regs :			MFI register set
- */
-void
-megasas_fire_cmd_fusion(struct megasas_instance *instance,
-			dma_addr_t req_desc_lo,
-			u32 req_desc_hi,
-			struct megasas_register_set __iomem *regs)
-{
-#if defined(writeq) && defined(CONFIG_64BIT)
-	u64 req_data = (((u64)le32_to_cpu(req_desc_hi) << 32) |
-			le32_to_cpu(req_desc_lo));
-
-	writeq(req_data, &(regs)->inbound_low_queue_port);
-#else
-	unsigned long flags;
-
-	spin_lock_irqsave(&instance->hba_lock, flags);
-
-	writel(le32_to_cpu(req_desc_lo), &(regs)->inbound_low_queue_port);
-	writel(le32_to_cpu(req_desc_hi), &(regs)->inbound_high_queue_port);
-	spin_unlock_irqrestore(&instance->hba_lock, flags);
-#endif
-}
-
-/**
  * map_cmd_status -	Maps FW cmd status to OS cmd status
  * @cmd :		Pointer to cmd
  * @status :		status of cmd returned by FW
@@ -1497,7 +1459,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 	struct MR_DRV_RAID_MAP_ALL *local_map_ptr;
 	u8 *raidLUN;
 
-	device_id = MEGASAS_DEV_INDEX(instance, scp);
+	device_id = MEGASAS_DEV_INDEX(scp);
 
 	fusion = instance->ctrl_context;
 
@@ -1621,6 +1583,14 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 			cmd->pd_r1_lb = io_info.pd_after_lb;
 		} else
 			scp->SCp.Status &= ~MEGASAS_LOAD_BALANCE_FLAG;
+
+		if ((raidLUN[0] == 1) &&
+			(local_map_ptr->raidMap.devHndlInfo[io_info.pd_after_lb].validHandles > 2)) {
+			instance->dev_handle = !(instance->dev_handle);
+			io_info.devHandle =
+				local_map_ptr->raidMap.devHndlInfo[io_info.pd_after_lb].devHandle[instance->dev_handle];
+		}
+
 		cmd->request_desc->SCSIIO.DevHandle = io_info.devHandle;
 		io_request->DevHandle = io_info.devHandle;
 		/* populate the LUN field */
@@ -1650,121 +1620,68 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 }
 
 /**
- * megasas_build_dcdb_fusion -	Prepares IOs to devices
+ * megasas_build_ld_nonrw_fusion - prepares non rw ios for virtual disk
  * @instance:		Adapter soft state
  * @scp:		SCSI command
  * @cmd:		Command to be prepared
  *
- * Prepares the io_request frame for non-io cmds
+ * Prepares the io_request frame for non-rw io cmds for vd.
  */
-static void
-megasas_build_dcdb_fusion(struct megasas_instance *instance,
-			  struct scsi_cmnd *scmd,
-			  struct megasas_cmd_fusion *cmd)
+static void megasas_build_ld_nonrw_fusion(struct megasas_instance *instance,
+			  struct scsi_cmnd *scmd, struct megasas_cmd_fusion *cmd)
 {
 	u32 device_id;
 	struct MPI2_RAID_SCSI_IO_REQUEST *io_request;
 	u16 pd_index = 0;
-	u16 os_timeout_value;
-	u16 timeout_limit;
 	struct MR_DRV_RAID_MAP_ALL *local_map_ptr;
 	struct fusion_context *fusion = instance->ctrl_context;
 	u8                          span, physArm;
-	u16                         devHandle;
+	__le16                      devHandle;
 	u32                         ld, arRef, pd;
 	struct MR_LD_RAID                  *raid;
 	struct RAID_CONTEXT                *pRAID_Context;
+	u8 fp_possible = 1;
 
 	io_request = cmd->io_request;
-	device_id = MEGASAS_DEV_INDEX(instance, scmd);
-	pd_index = (scmd->device->channel * MEGASAS_MAX_DEV_PER_CHANNEL)
-		+scmd->device->id;
+	device_id = MEGASAS_DEV_INDEX(scmd);
+	pd_index = MEGASAS_PD_INDEX(scmd);
 	local_map_ptr = fusion->ld_drv_map[(instance->map_id & 1)];
-
 	io_request->DataLength = cpu_to_le32(scsi_bufflen(scmd));
+	/* get RAID_Context pointer */
+	pRAID_Context = &io_request->RaidContext;
+	/* Check with FW team */
+	pRAID_Context->VirtualDiskTgtId = cpu_to_le16(device_id);
+	pRAID_Context->regLockRowLBA    = 0;
+	pRAID_Context->regLockLength    = 0;
 
-	if (scmd->device->channel < MEGASAS_MAX_PD_CHANNELS &&
-	    instance->pd_list[pd_index].driveState == MR_PD_STATE_SYSTEM) {
-		if (fusion->fast_path_io)
-			io_request->DevHandle =
-			local_map_ptr->raidMap.devHndlInfo[device_id].curDevHdl;
-		io_request->RaidContext.RAIDFlags =
-			MR_RAID_FLAGS_IO_SUB_TYPE_SYSTEM_PD
-			<< MR_RAID_CTX_RAID_FLAGS_IO_SUB_TYPE_SHIFT;
-		cmd->request_desc->SCSIIO.DevHandle = io_request->DevHandle;
-		cmd->request_desc->SCSIIO.MSIxIndex =
-			instance->msix_vectors ?
-				raw_smp_processor_id() %
-					instance->msix_vectors :
-				0;
-		os_timeout_value = scmd->request->timeout / HZ;
-
-		if (instance->secure_jbod_support &&
-			(megasas_cmd_type(scmd) == NON_READ_WRITE_SYSPDIO)) {
-			/* system pd firmware path */
-			io_request->Function  =
-				MEGASAS_MPI2_FUNCTION_LD_IO_REQUEST;
-			cmd->request_desc->SCSIIO.RequestFlags =
-				(MPI2_REQ_DESCRIPT_FLAGS_SCSI_IO <<
-				MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
-			io_request->RaidContext.timeoutValue =
-				cpu_to_le16(os_timeout_value);
-		} else {
-			/* system pd Fast Path */
-			io_request->Function = MPI2_FUNCTION_SCSI_IO_REQUEST;
-			io_request->RaidContext.regLockFlags = 0;
-			io_request->RaidContext.regLockRowLBA = 0;
-			io_request->RaidContext.regLockLength = 0;
-			timeout_limit = (scmd->device->type == TYPE_DISK) ?
-					255 : 0xFFFF;
-			io_request->RaidContext.timeoutValue =
-				cpu_to_le16((os_timeout_value > timeout_limit) ?
-				timeout_limit : os_timeout_value);
-		if ((instance->pdev->device == PCI_DEVICE_ID_LSI_INVADER) ||
-			(instance->pdev->device == PCI_DEVICE_ID_LSI_FURY))
-			io_request->IoFlags |=
-			cpu_to_le16(MPI25_SAS_DEVICE0_FLAGS_ENABLED_FAST_PATH);
-
-			cmd->request_desc->SCSIIO.RequestFlags =
-				(MPI2_REQ_DESCRIPT_FLAGS_HIGH_PRIORITY <<
-				MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
-		}
-	} else {
-		if (scmd->device->channel < MEGASAS_MAX_PD_CHANNELS)
-			goto NonFastPath;
-
-		/*
-		 * For older firmware, Driver should not access ldTgtIdToLd
-		 * beyond index 127 and for Extended VD firmware, ldTgtIdToLd
-		 * should not go beyond 255.
-		 */
-
-		if ((!fusion->fast_path_io) ||
-			(device_id >= instance->fw_supported_vd_count))
-			goto NonFastPath;
+	if (fusion->fast_path_io && (
+		device_id < instance->fw_supported_vd_count)) {
 
 		ld = MR_TargetIdToLdGet(device_id, local_map_ptr);
-
 		if (ld >= instance->fw_supported_vd_count)
-			goto NonFastPath;
+			fp_possible = 0;
 
 		raid = MR_LdRaidGet(ld, local_map_ptr);
-
-		/* check if this LD is FP capable */
 		if (!(raid->capability.fpNonRWCapable))
-			/* not FP capable, send as non-FP */
-			goto NonFastPath;
+			fp_possible = 0;
+	} else
+		fp_possible = 0;
 
-		/* get RAID_Context pointer */
-		pRAID_Context = &io_request->RaidContext;
+	if (!fp_possible) {
+		io_request->Function  = MEGASAS_MPI2_FUNCTION_LD_IO_REQUEST;
+		io_request->DevHandle = cpu_to_le16(device_id);
+		io_request->LUN[1] = scmd->device->lun;
+		pRAID_Context->timeoutValue =
+			cpu_to_le16 (scmd->request->timeout / HZ);
+		cmd->request_desc->SCSIIO.RequestFlags =
+			(MPI2_REQ_DESCRIPT_FLAGS_SCSI_IO <<
+			MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
+	} else {
 
 		/* set RAID context values */
-		pRAID_Context->regLockFlags     = REGION_TYPE_SHARED_READ;
-		pRAID_Context->timeoutValue     = cpu_to_le16(raid->fpIoTimeoutForLd);
-		pRAID_Context->VirtualDiskTgtId = cpu_to_le16(device_id);
-		pRAID_Context->regLockRowLBA    = 0;
-		pRAID_Context->regLockLength    = 0;
-		pRAID_Context->configSeqNum     = raid->seqNum;
+		pRAID_Context->configSeqNum = raid->seqNum;
+		pRAID_Context->regLockFlags = REGION_TYPE_SHARED_READ;
+		pRAID_Context->timeoutValue = cpu_to_le16(raid->fpIoTimeoutForLd);
 
 		/* get the DevHandle for the PD (since this is
 		   fpNonRWCapable, this is a single disk RAID0) */
@@ -1776,7 +1693,7 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 		/* build request descriptor */
 		cmd->request_desc->SCSIIO.RequestFlags =
 			(MPI2_REQ_DESCRIPT_FLAGS_HIGH_PRIORITY <<
-			 MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
+			MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
 		cmd->request_desc->SCSIIO.DevHandle = devHandle;
 
 		/* populate the LUN field */
@@ -1785,18 +1702,87 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 		/* build the raidScsiIO structure */
 		io_request->Function = MPI2_FUNCTION_SCSI_IO_REQUEST;
 		io_request->DevHandle = devHandle;
+	}
+}
 
-		return;
+/**
+ * megasas_build_syspd_fusion - prepares rw/non-rw ios for syspd
+ * @instance:		Adapter soft state
+ * @scp:		SCSI command
+ * @cmd:		Command to be prepared
+ * @fp_possible:	parameter to detect fast path or firmware path io.
+ *
+ * Prepares the io_request frame for rw/non-rw io cmds for syspds
+ */
+static void
+megasas_build_syspd_fusion(struct megasas_instance *instance,
+	struct scsi_cmnd *scmd, struct megasas_cmd_fusion *cmd, u8 fp_possible)
+{
+	u32 device_id;
+	struct MPI2_RAID_SCSI_IO_REQUEST *io_request;
+	u16 pd_index = 0;
+	u16 os_timeout_value;
+	u16 timeout_limit;
+	struct MR_DRV_RAID_MAP_ALL *local_map_ptr;
+	struct RAID_CONTEXT	*pRAID_Context;
+	struct fusion_context *fusion = instance->ctrl_context;
+
+	device_id = MEGASAS_DEV_INDEX(scmd);
+	pd_index = MEGASAS_PD_INDEX(scmd);
+	os_timeout_value = scmd->request->timeout / HZ;
 
-NonFastPath:
+	io_request = cmd->io_request;
+	/* get RAID_Context pointer */
+	pRAID_Context = &io_request->RaidContext;
+	io_request->DataLength = cpu_to_le32(scsi_bufflen(scmd));
+	io_request->LUN[1] = scmd->device->lun;
+	pRAID_Context->RAIDFlags = MR_RAID_FLAGS_IO_SUB_TYPE_SYSTEM_PD
+		<< MR_RAID_CTX_RAID_FLAGS_IO_SUB_TYPE_SHIFT;
+
+	pRAID_Context->VirtualDiskTgtId = cpu_to_le16(device_id);
+	pRAID_Context->configSeqNum = 0;
+	local_map_ptr = fusion->ld_drv_map[(instance->map_id & 1)];
+	io_request->DevHandle =
+		local_map_ptr->raidMap.devHndlInfo[device_id].curDevHdl;
+
+	cmd->request_desc->SCSIIO.DevHandle = io_request->DevHandle;
+	cmd->request_desc->SCSIIO.MSIxIndex =
+		instance->msix_vectors ?
+		(raw_smp_processor_id() % instance->msix_vectors) : 0;
+
+
+	if (!fp_possible) {
+		/* system pd firmware path */
 		io_request->Function  = MEGASAS_MPI2_FUNCTION_LD_IO_REQUEST;
-		io_request->DevHandle = cpu_to_le16(device_id);
 		cmd->request_desc->SCSIIO.RequestFlags =
 			(MPI2_REQ_DESCRIPT_FLAGS_SCSI_IO <<
-			 MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
+				MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
+		pRAID_Context->timeoutValue = cpu_to_le16(os_timeout_value);
+	} else {
+		/* system pd Fast Path */
+		io_request->Function = MPI2_FUNCTION_SCSI_IO_REQUEST;
+		pRAID_Context->regLockFlags = 0;
+		pRAID_Context->regLockRowLBA = 0;
+		pRAID_Context->regLockLength = 0;
+		timeout_limit = (scmd->device->type == TYPE_DISK) ?
+				255 : 0xFFFF;
+		pRAID_Context->timeoutValue =
+			cpu_to_le16((os_timeout_value > timeout_limit) ?
+			timeout_limit : os_timeout_value);
+		if ((instance->pdev->device == PCI_DEVICE_ID_LSI_INVADER) ||
+			(instance->pdev->device == PCI_DEVICE_ID_LSI_FURY)) {
+			cmd->request_desc->SCSIIO.RequestFlags |=
+				(MEGASAS_REQ_DESCRIPT_FLAGS_NO_LOCK <<
+				MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
+			pRAID_Context->Type = MPI2_TYPE_CUDA;
+			pRAID_Context->nseg = 0x1;
+			io_request->IoFlags |=
+				cpu_to_le16(MPI25_SAS_DEVICE0_FLAGS_ENABLED_FAST_PATH);
+		}
+		cmd->request_desc->SCSIIO.RequestFlags =
+			(MPI2_REQ_DESCRIPT_FLAGS_HIGH_PRIORITY <<
+				MEGASAS_REQ_DESCRIPT_FLAGS_TYPE_SHIFT);
 	}
-	io_request->RaidContext.VirtualDiskTgtId = cpu_to_le16(device_id);
-	int_to_scsilun(scmd->device->lun, (struct scsi_lun *)io_request->LUN);
 }
 
 /**
@@ -1813,11 +1799,10 @@ megasas_build_io_fusion(struct megasas_instance *instance,
 			struct scsi_cmnd *scp,
 			struct megasas_cmd_fusion *cmd)
 {
-	u32 device_id, sge_count;
+	u32 sge_count;
+	u8  cmd_type;
 	struct MPI2_RAID_SCSI_IO_REQUEST *io_request = cmd->io_request;
 
-	device_id = MEGASAS_DEV_INDEX(instance, scp);
-
 	/* Zero out some fields so they don't get reused */
 	memset(io_request->LUN, 0x0, 8);
 	io_request->CDB.EEDP32.PrimaryReferenceTag = 0;
@@ -1837,10 +1822,24 @@ megasas_build_io_fusion(struct megasas_instance *instance,
 	 */
 	io_request->IoFlags = cpu_to_le16(scp->cmd_len);
 
-	if (megasas_cmd_type(scp) == READ_WRITE_LDIO)
+	switch (cmd_type = megasas_cmd_type(scp)) {
+	case READ_WRITE_LDIO:
 		megasas_build_ldio_fusion(instance, scp, cmd);
-	else
-		megasas_build_dcdb_fusion(instance, scp, cmd);
+		break;
+	case NON_READ_WRITE_LDIO:
+		megasas_build_ld_nonrw_fusion(instance, scp, cmd);
+		break;
+	case READ_WRITE_SYSPDIO:
+	case NON_READ_WRITE_SYSPDIO:
+		if (instance->secure_jbod_support &&
+			(cmd_type == NON_READ_WRITE_SYSPDIO))
+			megasas_build_syspd_fusion(instance, scp, cmd, 0);
+		else
+			megasas_build_syspd_fusion(instance, scp, cmd, 1);
+		break;
+	default:
+		break;
+	}
 
 	/*
 	 * Construct SGL
@@ -1915,9 +1914,7 @@ megasas_build_and_issue_cmd_fusion(struct megasas_instance *instance,
 
 	fusion = instance->ctrl_context;
 
-	cmd = megasas_get_cmd_fusion(instance);
-	if (!cmd)
-		return SCSI_MLQUEUE_HOST_BUSY;
+	cmd = megasas_get_cmd_fusion(instance, scmd->request->tag);
 
 	index = cmd->index;
 
@@ -1948,9 +1945,7 @@ megasas_build_and_issue_cmd_fusion(struct megasas_instance *instance,
 	 */
 	atomic_inc(&instance->fw_outstanding);
 
-	instance->instancet->fire_cmd(instance,
-				      req_desc->u.low, req_desc->u.high,
-				      instance->reg_set);
+	megasas_fire_cmd_fusion(instance, req_desc);
 
 	return 0;
 }
@@ -1975,6 +1970,7 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 	union desc_value d_val;
 	struct LD_LOAD_BALANCE_INFO *lbinfo;
 	int threshold_reply_count = 0;
+	struct scsi_cmnd *scmd_local = NULL;
 
 	fusion = instance->ctrl_context;
 
@@ -1998,7 +1994,8 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 
 	num_completed = 0;
 
-	while ((d_val.u.low != UINT_MAX) && (d_val.u.high != UINT_MAX)) {
+	while (d_val.u.low != cpu_to_le32(UINT_MAX) &&
+	       d_val.u.high != cpu_to_le32(UINT_MAX)) {
 		smid = le16_to_cpu(reply_desc->SMID);
 
 		cmd_fusion = fusion->cmd_list[smid - 1];
@@ -2010,14 +2007,14 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 		if (cmd_fusion->scmd)
 			cmd_fusion->scmd->SCp.ptr = NULL;
 
+		scmd_local = cmd_fusion->scmd;
 		status = scsi_io_req->RaidContext.status;
 		extStatus = scsi_io_req->RaidContext.exStatus;
 
 		switch (scsi_io_req->Function) {
 		case MPI2_FUNCTION_SCSI_IO_REQUEST:  /*Fast Path IO.*/
 			/* Update load balancing info */
-			device_id = MEGASAS_DEV_INDEX(instance,
-						      cmd_fusion->scmd);
+			device_id = MEGASAS_DEV_INDEX(scmd_local);
 			lbinfo = &fusion->load_balance_info[device_id];
 			if (cmd_fusion->scmd->SCp.Status &
 			    MEGASAS_LOAD_BALANCE_FLAG) {
@@ -2035,29 +2032,25 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 		case MEGASAS_MPI2_FUNCTION_LD_IO_REQUEST: /* LD-IO Path */
 			/* Map the FW Cmd Status */
 			map_cmd_status(cmd_fusion, status, extStatus);
-			scsi_dma_unmap(cmd_fusion->scmd);
-			cmd_fusion->scmd->scsi_done(cmd_fusion->scmd);
 			scsi_io_req->RaidContext.status = 0;
 			scsi_io_req->RaidContext.exStatus = 0;
 			megasas_return_cmd_fusion(instance, cmd_fusion);
+			scsi_dma_unmap(scmd_local);
+			scmd_local->scsi_done(scmd_local);
 			atomic_dec(&instance->fw_outstanding);
 
 			break;
 		case MEGASAS_MPI2_FUNCTION_PASSTHRU_IO_REQUEST: /*MFI command */
 			cmd_mfi = instance->cmd_list[cmd_fusion->sync_cmd_idx];
 
-			if (!cmd_mfi->mpt_pthr_cmd_blocked) {
-				if (megasas_dbg_lvl == 5)
-					dev_info(&instance->pdev->dev,
-						"freeing mfi/mpt pass-through "
-						"from %s %d\n",
-						 __func__, __LINE__);
-				megasas_return_mfi_mpt_pthr(instance, cmd_mfi,
-					cmd_fusion);
-			}
-
-			megasas_complete_cmd(instance, cmd_mfi, DID_OK);
-			cmd_fusion->flags = 0;
+			/* Poll mode. Dummy free.
+			 * In case of Interrupt mode, caller has reverse check.
+			 */
+			if (cmd_mfi->flags & DRV_DCMD_POLLED_MODE) {
+				cmd_mfi->flags &= ~DRV_DCMD_POLLED_MODE;
+				megasas_return_cmd(instance, cmd_mfi);
+			} else
+				megasas_complete_cmd(instance, cmd_mfi, DID_OK);
 			break;
 		}
 
@@ -2066,7 +2059,7 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 		    fusion->reply_q_depth)
 			fusion->last_reply_idx[MSIxIndex] = 0;
 
-		desc->Words = ULLONG_MAX;
+		desc->Words = cpu_to_le64(ULLONG_MAX);
 		num_completed++;
 		threshold_reply_count++;
 
@@ -2217,27 +2210,14 @@ build_mpt_mfi_pass_thru(struct megasas_instance *instance,
 	struct megasas_cmd_fusion *cmd;
 	struct fusion_context *fusion;
 	struct megasas_header *frame_hdr = &mfi_cmd->frame->hdr;
-	u32 opcode;
 
-	cmd = megasas_get_cmd_fusion(instance);
-	if (!cmd)
-		return 1;
+	fusion = instance->ctrl_context;
+
+	cmd = megasas_get_cmd_fusion(instance,
+			instance->max_scsi_cmds + mfi_cmd->index);
 
 	/*  Save the smid. To be used for returning the cmd */
 	mfi_cmd->context.smid = cmd->index;
-	cmd->sync_cmd_idx = mfi_cmd->index;
-
-	/* Set this only for Blocked commands */
-	opcode = le32_to_cpu(mfi_cmd->frame->dcmd.opcode);
-	if ((opcode == MR_DCMD_LD_MAP_GET_INFO)
-		&& (mfi_cmd->frame->dcmd.mbox.b[1] == 1))
-		mfi_cmd->is_wait_event = 1;
-
-	if (opcode == MR_DCMD_CTRL_EVENT_WAIT)
-		mfi_cmd->is_wait_event = 1;
-
-	if (mfi_cmd->is_wait_event)
-		mfi_cmd->mpt_pthr_cmd_blocked = cmd;
 
 	/*
 	 * For cmds where the flag is set, store the flag and check
@@ -2246,9 +2226,8 @@ build_mpt_mfi_pass_thru(struct megasas_instance *instance,
 	 */
 
 	if (frame_hdr->flags & cpu_to_le16(MFI_FRAME_DONT_POST_IN_REPLY_QUEUE))
-		cmd->flags = MFI_FRAME_DONT_POST_IN_REPLY_QUEUE;
+		mfi_cmd->flags |= DRV_DCMD_POLLED_MODE;
 
-	fusion = instance->ctrl_context;
 	io_req = cmd->io_request;
 
 	if ((instance->pdev->device == PCI_DEVICE_ID_LSI_INVADER) ||
@@ -2327,14 +2306,12 @@ megasas_issue_dcmd_fusion(struct megasas_instance *instance,
 		printk(KERN_ERR "Couldn't issue MFI pass thru cmd\n");
 		return;
 	}
-	atomic_set(&cmd->mfi_mpt_pthr, MFI_MPT_ATTACHED);
-	instance->instancet->fire_cmd(instance, req_desc->u.low,
-				      req_desc->u.high, instance->reg_set);
+	megasas_fire_cmd_fusion(instance, req_desc);
 }
 
 /**
  * megasas_release_fusion -	Reverses the FW initialization
- * @intance:			Adapter soft state
+ * @instance:			Adapter soft state
  */
 void
 megasas_release_fusion(struct megasas_instance *instance)
@@ -2508,7 +2485,42 @@ void  megasas_reset_reply_desc(struct megasas_instance *instance)
 		fusion->last_reply_idx[i] = 0;
 	reply_desc = fusion->reply_frames_desc;
 	for (i = 0 ; i < fusion->reply_q_depth * count; i++, reply_desc++)
-		reply_desc->Words = ULLONG_MAX;
+		reply_desc->Words = cpu_to_le64(ULLONG_MAX);
+}
+
+/*
+ * megasas_refire_mgmt_cmd :	Re-fire management commands
+ * @instance:				Controller's soft instance
+*/
+void megasas_refire_mgmt_cmd(struct megasas_instance *instance)
+{
+	int j;
+	struct megasas_cmd_fusion *cmd_fusion;
+	struct fusion_context *fusion;
+	struct megasas_cmd *cmd_mfi;
+	union MEGASAS_REQUEST_DESCRIPTOR_UNION *req_desc;
+	u16 smid;
+
+	fusion = instance->ctrl_context;
+
+	/* Re-fire management commands.
+	 * Do not traverse complet MPT frame pool. Start from max_scsi_cmds.
+	 */
+	for (j = instance->max_scsi_cmds ; j < instance->max_fw_cmds; j++) {
+		cmd_fusion = fusion->cmd_list[j];
+		cmd_mfi = instance->cmd_list[cmd_fusion->sync_cmd_idx];
+		smid = le16_to_cpu(cmd_mfi->context.smid);
+
+		if (!smid)
+			continue;
+		req_desc = megasas_get_request_descriptor
+					(instance, smid - 1);
+		if (req_desc && (cmd_mfi->frame->dcmd.opcode !=
+				cpu_to_le32(MR_DCMD_LD_MAP_GET_INFO)))
+			megasas_fire_cmd_fusion(instance, req_desc);
+		else
+			megasas_return_cmd(instance, cmd_mfi);
+	}
 }
 
 /* Check for a second path that is currently UP */
@@ -2538,14 +2550,13 @@ out:
 /* Core fusion reset function */
 int megasas_reset_fusion(struct Scsi_Host *shost, int iotimeout)
 {
-	int retval = SUCCESS, i, j, retry = 0, convert = 0;
+	int retval = SUCCESS, i, retry = 0, convert = 0;
 	struct megasas_instance *instance;
 	struct megasas_cmd_fusion *cmd_fusion;
 	struct fusion_context *fusion;
-	struct megasas_cmd *cmd_mfi;
-	union MEGASAS_REQUEST_DESCRIPTOR_UNION *req_desc;
 	u32 host_diag, abs_state, status_reg, reset_adapter;
 	u32 io_timeout_in_crash_mode = 0;
+	struct scsi_cmnd *scmd_local = NULL;
 
 	instance = (struct megasas_instance *)shost->hostdata;
 	fusion = instance->ctrl_context;
@@ -2613,15 +2624,16 @@ int megasas_reset_fusion(struct Scsi_Host *shost, int iotimeout)
 			iotimeout = 0;
 
 		/* Now return commands back to the OS */
-		for (i = 0 ; i < instance->max_fw_cmds; i++) {
+		for (i = 0 ; i < instance->max_scsi_cmds; i++) {
 			cmd_fusion = fusion->cmd_list[i];
+			scmd_local = cmd_fusion->scmd;
 			if (cmd_fusion->scmd) {
-				scsi_dma_unmap(cmd_fusion->scmd);
-				cmd_fusion->scmd->result =
+				scmd_local->result =
 					megasas_check_mpio_paths(instance,
-								 cmd_fusion->scmd);
-				cmd_fusion->scmd->scsi_done(cmd_fusion->scmd);
+							scmd_local);
 				megasas_return_cmd_fusion(instance, cmd_fusion);
+				scsi_dma_unmap(scmd_local);
+				scmd_local->scsi_done(scmd_local);
 				atomic_dec(&instance->fw_outstanding);
 			}
 		}
@@ -2790,44 +2802,7 @@ int megasas_reset_fusion(struct Scsi_Host *shost, int iotimeout)
 				continue;
 			}
 
-			/* Re-fire management commands */
-			for (j = 0 ; j < instance->max_fw_cmds; j++) {
-				cmd_fusion = fusion->cmd_list[j];
-				if (cmd_fusion->sync_cmd_idx !=
-				    (u32)ULONG_MAX) {
-					cmd_mfi =
-					instance->
-					cmd_list[cmd_fusion->sync_cmd_idx];
-					if (cmd_mfi->frame->dcmd.opcode ==
-					    cpu_to_le32(MR_DCMD_LD_MAP_GET_INFO)) {
-						megasas_return_mfi_mpt_pthr(instance, cmd_mfi, cmd_fusion);
-					} else  {
-						req_desc =
-						megasas_get_request_descriptor(
-							instance,
-							cmd_mfi->context.smid
-							-1);
-						if (!req_desc) {
-							printk(KERN_WARNING
-							       "req_desc NULL"
-							       " for scsi%d\n",
-								instance->host->host_no);
-							/* Return leaked MPT
-							   frame */
-							megasas_return_cmd_fusion(instance, cmd_fusion);
-						} else {
-							instance->instancet->
-							fire_cmd(instance,
-								 req_desc->
-								 u.low,
-								 req_desc->
-								 u.high,
-								 instance->
-								 reg_set);
-						}
-					}
-				}
-			}
+			megasas_refire_mgmt_cmd(instance);
 
 			if (megasas_get_ctrl_info(instance)) {
 				dev_info(&instance->pdev->dev,
@@ -2978,7 +2953,6 @@ void megasas_fusion_ocr_wq(struct work_struct *work)
 }
 
 struct megasas_instance_template megasas_instance_template_fusion = {
-	.fire_cmd = megasas_fire_cmd_fusion,
 	.enable_intr = megasas_enable_intr_fusion,
 	.disable_intr = megasas_disable_intr_fusion,
 	.clear_intr = megasas_clear_intr_fusion,
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.h b/drivers/scsi/megaraid/megaraid_sas_fusion.h
index 56e6db2d5874..ced6dc0cf8e8 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.h
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.h
@@ -104,18 +104,18 @@ struct RAID_CONTEXT {
 	u8	nseg:4;
 #endif
 	u8	resvd0;
-	u16     timeoutValue;
+	__le16	timeoutValue;
 	u8      regLockFlags;
 	u8      resvd1;
-	u16     VirtualDiskTgtId;
-	u64     regLockRowLBA;
-	u32     regLockLength;
-	u16     nextLMId;
+	__le16	VirtualDiskTgtId;
+	__le64	regLockRowLBA;
+	__le32	regLockLength;
+	__le16	nextLMId;
 	u8      exStatus;
 	u8      status;
 	u8      RAIDFlags;
 	u8      numSGE;
-	u16	configSeqNum;
+	__le16	configSeqNum;
 	u8      spanArm;
 	u8      resvd2[3];
 };
@@ -182,61 +182,61 @@ enum REGION_TYPE {
 #define MPI2_WRSEQ_6TH_KEY_VALUE                (0xD)
 
 struct MPI25_IEEE_SGE_CHAIN64 {
-	u64                     Address;
-	u32                     Length;
-	u16                     Reserved1;
+	__le64			Address;
+	__le32			Length;
+	__le16			Reserved1;
 	u8                      NextChainOffset;
 	u8                      Flags;
 };
 
 struct MPI2_SGE_SIMPLE_UNION {
-	u32                     FlagsLength;
+	__le32                     FlagsLength;
 	union {
-		u32                 Address32;
-		u64                 Address64;
+		__le32                 Address32;
+		__le64                 Address64;
 	} u;
 };
 
 struct MPI2_SCSI_IO_CDB_EEDP32 {
 	u8                      CDB[20];                    /* 0x00 */
-	u32                     PrimaryReferenceTag;        /* 0x14 */
-	u16                     PrimaryApplicationTag;      /* 0x18 */
-	u16                     PrimaryApplicationTagMask;  /* 0x1A */
-	u32                     TransferLength;             /* 0x1C */
+	__be32			PrimaryReferenceTag;        /* 0x14 */
+	__be16			PrimaryApplicationTag;      /* 0x18 */
+	__be16			PrimaryApplicationTagMask;  /* 0x1A */
+	__le32			TransferLength;             /* 0x1C */
 };
 
 struct MPI2_SGE_CHAIN_UNION {
-	u16                     Length;
+	__le16			Length;
 	u8                      NextChainOffset;
 	u8                      Flags;
 	union {
-		u32                 Address32;
-		u64                 Address64;
+		__le32		Address32;
+		__le64		Address64;
 	} u;
 };
 
 struct MPI2_IEEE_SGE_SIMPLE32 {
-	u32                     Address;
-	u32                     FlagsLength;
+	__le32			Address;
+	__le32			FlagsLength;
 };
 
 struct MPI2_IEEE_SGE_CHAIN32 {
-	u32                     Address;
-	u32                     FlagsLength;
+	__le32			Address;
+	__le32			FlagsLength;
 };
 
 struct MPI2_IEEE_SGE_SIMPLE64 {
-	u64                     Address;
-	u32                     Length;
-	u16                     Reserved1;
+	__le64			Address;
+	__le32			Length;
+	__le16			Reserved1;
 	u8                      Reserved2;
 	u8                      Flags;
 };
 
 struct MPI2_IEEE_SGE_CHAIN64 {
-	u64                     Address;
-	u32                     Length;
-	u16                     Reserved1;
+	__le64			Address;
+	__le32			Length;
+	__le16			Reserved1;
 	u8                      Reserved2;
 	u8                      Flags;
 };
@@ -269,34 +269,34 @@ union MPI2_SCSI_IO_CDB_UNION {
  * Total SGE count will be one less than  _MPI2_SCSI_IO_REQUEST
  */
 struct MPI2_RAID_SCSI_IO_REQUEST {
-	u16                     DevHandle;                      /* 0x00 */
+	__le16			DevHandle;                      /* 0x00 */
 	u8                      ChainOffset;                    /* 0x02 */
 	u8                      Function;                       /* 0x03 */
-	u16                     Reserved1;                      /* 0x04 */
+	__le16			Reserved1;                      /* 0x04 */
 	u8                      Reserved2;                      /* 0x06 */
 	u8                      MsgFlags;                       /* 0x07 */
 	u8                      VP_ID;                          /* 0x08 */
 	u8                      VF_ID;                          /* 0x09 */
-	u16                     Reserved3;                      /* 0x0A */
-	u32                     SenseBufferLowAddress;          /* 0x0C */
-	u16                     SGLFlags;                       /* 0x10 */
+	__le16			Reserved3;                      /* 0x0A */
+	__le32			SenseBufferLowAddress;          /* 0x0C */
+	__le16			SGLFlags;                       /* 0x10 */
 	u8                      SenseBufferLength;              /* 0x12 */
 	u8                      Reserved4;                      /* 0x13 */
 	u8                      SGLOffset0;                     /* 0x14 */
 	u8                      SGLOffset1;                     /* 0x15 */
 	u8                      SGLOffset2;                     /* 0x16 */
 	u8                      SGLOffset3;                     /* 0x17 */
-	u32                     SkipCount;                      /* 0x18 */
-	u32                     DataLength;                     /* 0x1C */
-	u32                     BidirectionalDataLength;        /* 0x20 */
-	u16                     IoFlags;                        /* 0x24 */
-	u16                     EEDPFlags;                      /* 0x26 */
-	u32                     EEDPBlockSize;                  /* 0x28 */
-	u32                     SecondaryReferenceTag;          /* 0x2C */
-	u16                     SecondaryApplicationTag;        /* 0x30 */
-	u16                     ApplicationTagTranslationMask;  /* 0x32 */
+	__le32			SkipCount;                      /* 0x18 */
+	__le32			DataLength;                     /* 0x1C */
+	__le32			BidirectionalDataLength;        /* 0x20 */
+	__le16			IoFlags;                        /* 0x24 */
+	__le16			EEDPFlags;                      /* 0x26 */
+	__le32			EEDPBlockSize;                  /* 0x28 */
+	__le32			SecondaryReferenceTag;          /* 0x2C */
+	__le16			SecondaryApplicationTag;        /* 0x30 */
+	__le16			ApplicationTagTranslationMask;  /* 0x32 */
 	u8                      LUN[8];                         /* 0x34 */
-	u32                     Control;                        /* 0x3C */
+	__le32			Control;                        /* 0x3C */
 	union MPI2_SCSI_IO_CDB_UNION  CDB;			/* 0x40 */
 	struct RAID_CONTEXT	RaidContext;                    /* 0x60 */
 	union MPI2_SGE_IO_UNION       SGL;			/* 0x80 */
@@ -315,45 +315,45 @@ struct MEGASAS_RAID_MFA_IO_REQUEST_DESCRIPTOR {
 struct MPI2_DEFAULT_REQUEST_DESCRIPTOR {
 	u8              RequestFlags;               /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
-	u16             LMID;                       /* 0x04 */
-	u16             DescriptorTypeDependent;    /* 0x06 */
+	__le16		SMID;                       /* 0x02 */
+	__le16		LMID;                       /* 0x04 */
+	__le16		DescriptorTypeDependent;    /* 0x06 */
 };
 
 /* High Priority Request Descriptor */
 struct MPI2_HIGH_PRIORITY_REQUEST_DESCRIPTOR {
 	u8              RequestFlags;               /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
-	u16             LMID;                       /* 0x04 */
-	u16             Reserved1;                  /* 0x06 */
+	__le16		SMID;                       /* 0x02 */
+	__le16		LMID;                       /* 0x04 */
+	__le16		Reserved1;                  /* 0x06 */
 };
 
 /* SCSI IO Request Descriptor */
 struct MPI2_SCSI_IO_REQUEST_DESCRIPTOR {
 	u8              RequestFlags;               /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
-	u16             LMID;                       /* 0x04 */
-	u16             DevHandle;                  /* 0x06 */
+	__le16		SMID;                       /* 0x02 */
+	__le16		LMID;                       /* 0x04 */
+	__le16		DevHandle;                  /* 0x06 */
 };
 
 /* SCSI Target Request Descriptor */
 struct MPI2_SCSI_TARGET_REQUEST_DESCRIPTOR {
 	u8              RequestFlags;               /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
-	u16             LMID;                       /* 0x04 */
-	u16             IoIndex;                    /* 0x06 */
+	__le16		SMID;                       /* 0x02 */
+	__le16		LMID;                       /* 0x04 */
+	__le16		IoIndex;                    /* 0x06 */
 };
 
 /* RAID Accelerator Request Descriptor */
 struct MPI2_RAID_ACCEL_REQUEST_DESCRIPTOR {
 	u8              RequestFlags;               /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
-	u16             LMID;                       /* 0x04 */
-	u16             Reserved;                   /* 0x06 */
+	__le16		SMID;                       /* 0x02 */
+	__le16		LMID;                       /* 0x04 */
+	__le16		Reserved;                   /* 0x06 */
 };
 
 /* union of Request Descriptors */
@@ -366,10 +366,10 @@ union MEGASAS_REQUEST_DESCRIPTOR_UNION {
 	struct MEGASAS_RAID_MFA_IO_REQUEST_DESCRIPTOR      MFAIo;
 	union {
 		struct {
-			u32 low;
-			u32 high;
+			__le32 low;
+			__le32 high;
 		} u;
-		u64 Words;
+		__le64 Words;
 	};
 };
 
@@ -377,35 +377,35 @@ union MEGASAS_REQUEST_DESCRIPTOR_UNION {
 struct MPI2_DEFAULT_REPLY_DESCRIPTOR {
 	u8              ReplyFlags;                 /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             DescriptorTypeDependent1;   /* 0x02 */
-	u32             DescriptorTypeDependent2;   /* 0x04 */
+	__le16		DescriptorTypeDependent1;   /* 0x02 */
+	__le32		DescriptorTypeDependent2;   /* 0x04 */
 };
 
 /* Address Reply Descriptor */
 struct MPI2_ADDRESS_REPLY_DESCRIPTOR {
 	u8              ReplyFlags;                 /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
-	u32             ReplyFrameAddress;          /* 0x04 */
+	__le16		SMID;                       /* 0x02 */
+	__le32		ReplyFrameAddress;          /* 0x04 */
 };
 
 /* SCSI IO Success Reply Descriptor */
 struct MPI2_SCSI_IO_SUCCESS_REPLY_DESCRIPTOR {
 	u8              ReplyFlags;                 /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
-	u16             TaskTag;                    /* 0x04 */
-	u16             Reserved1;                  /* 0x06 */
+	__le16		SMID;                       /* 0x02 */
+	__le16		TaskTag;                    /* 0x04 */
+	__le16		Reserved1;                  /* 0x06 */
 };
 
 /* TargetAssist Success Reply Descriptor */
 struct MPI2_TARGETASSIST_SUCCESS_REPLY_DESCRIPTOR {
 	u8              ReplyFlags;                 /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
+	__le16		SMID;                       /* 0x02 */
 	u8              SequenceNumber;             /* 0x04 */
 	u8              Reserved1;                  /* 0x05 */
-	u16             IoIndex;                    /* 0x06 */
+	__le16		IoIndex;                    /* 0x06 */
 };
 
 /* Target Command Buffer Reply Descriptor */
@@ -414,16 +414,16 @@ struct MPI2_TARGET_COMMAND_BUFFER_REPLY_DESCRIPTOR {
 	u8              MSIxIndex;                  /* 0x01 */
 	u8              VP_ID;                      /* 0x02 */
 	u8              Flags;                      /* 0x03 */
-	u16             InitiatorDevHandle;         /* 0x04 */
-	u16             IoIndex;                    /* 0x06 */
+	__le16		InitiatorDevHandle;         /* 0x04 */
+	__le16		IoIndex;                    /* 0x06 */
 };
 
 /* RAID Accelerator Success Reply Descriptor */
 struct MPI2_RAID_ACCELERATOR_SUCCESS_REPLY_DESCRIPTOR {
 	u8              ReplyFlags;                 /* 0x00 */
 	u8              MSIxIndex;                  /* 0x01 */
-	u16             SMID;                       /* 0x02 */
-	u32             Reserved;                   /* 0x04 */
+	__le16		SMID;                       /* 0x02 */
+	__le32		Reserved;                   /* 0x04 */
 };
 
 /* union of Reply Descriptors */
@@ -435,7 +435,7 @@ union MPI2_REPLY_DESCRIPTORS_UNION {
 	struct MPI2_TARGET_COMMAND_BUFFER_REPLY_DESCRIPTOR TargetCommandBuffer;
 	struct MPI2_RAID_ACCELERATOR_SUCCESS_REPLY_DESCRIPTOR
 	RAIDAcceleratorSuccess;
-	u64                                             Words;
+	__le64                                             Words;
 };
 
 /* IOCInit Request message */
@@ -444,28 +444,28 @@ struct MPI2_IOC_INIT_REQUEST {
 	u8                      Reserved1;                      /* 0x01 */
 	u8                      ChainOffset;                    /* 0x02 */
 	u8                      Function;                       /* 0x03 */
-	u16                     Reserved2;                      /* 0x04 */
+	__le16			Reserved2;                      /* 0x04 */
 	u8                      Reserved3;                      /* 0x06 */
 	u8                      MsgFlags;                       /* 0x07 */
 	u8                      VP_ID;                          /* 0x08 */
 	u8                      VF_ID;                          /* 0x09 */
-	u16                     Reserved4;                      /* 0x0A */
-	u16                     MsgVersion;                     /* 0x0C */
-	u16                     HeaderVersion;                  /* 0x0E */
+	__le16			Reserved4;                      /* 0x0A */
+	__le16			MsgVersion;                     /* 0x0C */
+	__le16			HeaderVersion;                  /* 0x0E */
 	u32                     Reserved5;                      /* 0x10 */
-	u16                     Reserved6;                      /* 0x14 */
+	__le16			Reserved6;                      /* 0x14 */
 	u8                      Reserved7;                      /* 0x16 */
 	u8                      HostMSIxVectors;                /* 0x17 */
-	u16                     Reserved8;                      /* 0x18 */
-	u16                     SystemRequestFrameSize;         /* 0x1A */
-	u16                     ReplyDescriptorPostQueueDepth;  /* 0x1C */
-	u16                     ReplyFreeQueueDepth;            /* 0x1E */
-	u32                     SenseBufferAddressHigh;         /* 0x20 */
-	u32                     SystemReplyAddressHigh;         /* 0x24 */
-	u64                     SystemRequestFrameBaseAddress;  /* 0x28 */
-	u64                     ReplyDescriptorPostQueueAddress;/* 0x30 */
-	u64                     ReplyFreeQueueAddress;          /* 0x38 */
-	u64                     TimeStamp;                      /* 0x40 */
+	__le16			Reserved8;                      /* 0x18 */
+	__le16			SystemRequestFrameSize;         /* 0x1A */
+	__le16			ReplyDescriptorPostQueueDepth;  /* 0x1C */
+	__le16			ReplyFreeQueueDepth;            /* 0x1E */
+	__le32			SenseBufferAddressHigh;         /* 0x20 */
+	__le32			SystemReplyAddressHigh;         /* 0x24 */
+	__le64			SystemRequestFrameBaseAddress;  /* 0x28 */
+	__le64			ReplyDescriptorPostQueueAddress;/* 0x30 */
+	__le64			ReplyFreeQueueAddress;          /* 0x38 */
+	__le64			TimeStamp;                      /* 0x40 */
 };
 
 /* mrpriv defines */
@@ -491,41 +491,41 @@ struct MPI2_IOC_INIT_REQUEST {
 #define MR_DCMD_LD_VF_MAP_GET_ALL_LDS       0x03150200
 
 struct MR_DEV_HANDLE_INFO {
-	u16     curDevHdl;
+	__le16	curDevHdl;
 	u8      validHandles;
 	u8      reserved;
-	u16     devHandle[2];
+	__le16	devHandle[2];
 };
 
 struct MR_ARRAY_INFO {
-	u16      pd[MAX_RAIDMAP_ROW_SIZE];
+	__le16	pd[MAX_RAIDMAP_ROW_SIZE];
 };
 
 struct MR_QUAD_ELEMENT {
-	u64     logStart;
-	u64     logEnd;
-	u64     offsetInSpan;
-	u32     diff;
-	u32     reserved1;
+	__le64     logStart;
+	__le64     logEnd;
+	__le64     offsetInSpan;
+	__le32     diff;
+	__le32     reserved1;
 };
 
 struct MR_SPAN_INFO {
-	u32             noElements;
-	u32             reserved1;
+	__le32             noElements;
+	__le32             reserved1;
 	struct MR_QUAD_ELEMENT quad[MAX_RAIDMAP_SPAN_DEPTH];
 };
 
 struct MR_LD_SPAN {
-	u64      startBlk;
-	u64      numBlks;
-	u16      arrayRef;
+	__le64	 startBlk;
+	__le64	 numBlks;
+	__le16	 arrayRef;
 	u8       spanRowSize;
 	u8       spanRowDataSize;
 	u8       reserved[4];
 };
 
 struct MR_SPAN_BLOCK_INFO {
-	u64          num_rows;
+	__le64          num_rows;
 	struct MR_LD_SPAN   span;
 	struct MR_SPAN_INFO block_span_info;
 };
@@ -558,8 +558,8 @@ struct MR_LD_RAID {
 		u32     reserved4:7;
 #endif
 	} capability;
-	u32     reserved6;
-	u64     size;
+	__le32     reserved6;
+	__le64     size;
 	u8      spanDepth;
 	u8      level;
 	u8      stripeShift;
@@ -568,12 +568,12 @@ struct MR_LD_RAID {
 	u8      writeMode;
 	u8      PRL;
 	u8      SRL;
-	u16     targetId;
+	__le16     targetId;
 	u8      ldState;
 	u8      regTypeReqOnWrite;
 	u8      modFactor;
 	u8	regTypeReqOnRead;
-	u16     seqNum;
+	__le16     seqNum;
 
 	struct {
 		u32 ldSyncRequired:1;
@@ -592,20 +592,20 @@ struct MR_LD_SPAN_MAP {
 };
 
 struct MR_FW_RAID_MAP {
-	u32                 totalSize;
+	__le32                 totalSize;
 	union {
 		struct {
-			u32         maxLd;
-			u32         maxSpanDepth;
-			u32         maxRowSize;
-			u32         maxPdCount;
-			u32         maxArrays;
+			__le32         maxLd;
+			__le32         maxSpanDepth;
+			__le32         maxRowSize;
+			__le32         maxPdCount;
+			__le32         maxArrays;
 		} validationInfo;
-		u32             version[5];
+		__le32             version[5];
 	};
 
-	u32                 ldCount;
-	u32                 Reserved1;
+	__le32                 ldCount;
+	__le32                 Reserved1;
 	u8                  ldTgtIdToLd[MAX_RAIDMAP_LOGICAL_DRIVES+
 					MAX_RAIDMAP_VIEWS];
 	u8                  fpPdIoTimeoutSec;
@@ -620,7 +620,7 @@ struct IO_REQUEST_INFO {
 	u32 numBlocks;
 	u16 ldTgtId;
 	u8 isRead;
-	u16 devHandle;
+	__le16 devHandle;
 	u64 pdBlock;
 	u8 fpOkForIo;
 	u8 IoforUnevenSpan;
@@ -634,7 +634,7 @@ struct IO_REQUEST_INFO {
 struct MR_LD_TARGET_SYNC {
 	u8  targetId;
 	u8  reserved;
-	u16 seqNum;
+	__le16 seqNum;
 };
 
 #define IEEE_SGE_FLAGS_ADDR_MASK            (0x03)
@@ -679,7 +679,6 @@ struct megasas_cmd_fusion {
 	 */
 	u32 sync_cmd_idx;
 	u32 index;
-	u8 flags;
 	u8 pd_r1_lb;
 };
 
@@ -720,27 +719,27 @@ struct MR_DRV_RAID_MAP {
 	 * This feild will be manupulated by driver for ext raid map,
 	 * else pick the value from firmware raid map.
 	 */
-	u32                 totalSize;
+	__le32                 totalSize;
 
 	union {
 	struct {
-		u32         maxLd;
-		u32         maxSpanDepth;
-		u32         maxRowSize;
-		u32         maxPdCount;
-		u32         maxArrays;
+		__le32         maxLd;
+		__le32         maxSpanDepth;
+		__le32         maxRowSize;
+		__le32         maxPdCount;
+		__le32         maxArrays;
 	} validationInfo;
-	u32             version[5];
+	__le32             version[5];
 	};
 
 	/* timeout value used by driver in FP IOs*/
 	u8                  fpPdIoTimeoutSec;
 	u8                  reserved2[7];
 
-	u16                 ldCount;
-	u16                 arCount;
-	u16                 spanCount;
-	u16                 reserve3;
+	__le16                 ldCount;
+	__le16                 arCount;
+	__le16                 spanCount;
+	__le16                 reserve3;
 
 	struct MR_DEV_HANDLE_INFO  devHndlInfo[MAX_RAIDMAP_PHYSICAL_DEVICES];
 	u8                  ldTgtIdToLd[MAX_LOGICAL_DRIVES_EXT];
@@ -779,10 +778,10 @@ struct MR_FW_RAID_MAP_EXT {
 	u8                  fpPdIoTimeoutSec;
 	u8                  reserved2[7];
 
-	u16                 ldCount;
-	u16                 arCount;
-	u16                 spanCount;
-	u16                 reserve3;
+	__le16                 ldCount;
+	__le16                 arCount;
+	__le16                 spanCount;
+	__le16                 reserve3;
 
 	struct MR_DEV_HANDLE_INFO  devHndlInfo[MAX_RAIDMAP_PHYSICAL_DEVICES];
 	u8                  ldTgtIdToLd[MAX_LOGICAL_DRIVES_EXT];
@@ -792,10 +791,6 @@ struct MR_FW_RAID_MAP_EXT {
 
 struct fusion_context {
 	struct megasas_cmd_fusion **cmd_list;
-	struct list_head cmd_pool;
-
-	spinlock_t mpt_pool_lock;
-
 	dma_addr_t req_frames_desc_phys;
 	u8 *req_frames_desc;
 
@@ -839,10 +834,10 @@ struct fusion_context {
 };
 
 union desc_value {
-	u64 word;
+	__le64 word;
 	struct {
-		u32 low;
-		u32 high;
+		__le32 low;
+		__le32 high;
 	} u;
 };
 
diff --git a/drivers/scsi/mvsas/mv_init.c b/drivers/scsi/mvsas/mv_init.c
index 53030b0e8015..d40d734aa53a 100644
--- a/drivers/scsi/mvsas/mv_init.c
+++ b/drivers/scsi/mvsas/mv_init.c
@@ -56,7 +56,6 @@ static struct scsi_host_template mvs_sht = {
 	.change_queue_depth	= sas_change_queue_depth,
 	.bios_param		= sas_bios_param,
 	.can_queue		= 1,
-	.cmd_per_lun		= 1,
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
 	.max_sectors		= SCSI_DEFAULT_MAX_SECTORS,
diff --git a/drivers/scsi/nsp32.c b/drivers/scsi/nsp32.c
index c6077cefbeca..53c84771f0e8 100644
--- a/drivers/scsi/nsp32.c
+++ b/drivers/scsi/nsp32.c
@@ -274,7 +274,6 @@ static struct scsi_host_template nsp32_template = {
 	.can_queue			= 1,
 	.sg_tablesize			= NSP32_SG_SIZE,
 	.max_sectors			= 128,
-	.cmd_per_lun			= 1,
 	.this_id			= NSP32_HOST_SCSIID,
 	.use_clustering			= DISABLE_CLUSTERING,
 	.eh_abort_handler       	= nsp32_eh_abort,
diff --git a/drivers/scsi/pcmcia/nsp_cs.c b/drivers/scsi/pcmcia/nsp_cs.c
index 1b6c8833a304..5fb6eefc6541 100644
--- a/drivers/scsi/pcmcia/nsp_cs.c
+++ b/drivers/scsi/pcmcia/nsp_cs.c
@@ -86,7 +86,6 @@ static struct scsi_host_template nsp_driver_template = {
 	.can_queue		 = 1,
 	.this_id		 = NSP_INITIATOR_ID,
 	.sg_tablesize		 = SG_ALL,
-	.cmd_per_lun		 = 1,
 	.use_clustering		 = DISABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/pcmcia/qlogic_stub.c b/drivers/scsi/pcmcia/qlogic_stub.c
index bcaf89fe0c9e..c670dc704c74 100644
--- a/drivers/scsi/pcmcia/qlogic_stub.c
+++ b/drivers/scsi/pcmcia/qlogic_stub.c
@@ -72,7 +72,6 @@ static struct scsi_host_template qlogicfas_driver_template = {
 	.can_queue		= 1,
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
-	.cmd_per_lun		= 1,
 	.use_clustering		= DISABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/pcmcia/sym53c500_cs.c b/drivers/scsi/pcmcia/sym53c500_cs.c
index 155f9573021f..20011c8afbb5 100644
--- a/drivers/scsi/pcmcia/sym53c500_cs.c
+++ b/drivers/scsi/pcmcia/sym53c500_cs.c
@@ -680,7 +680,6 @@ static struct scsi_host_template sym53c500_driver_template = {
      .can_queue			= 1,
      .this_id			= 7,
      .sg_tablesize		= 32,
-     .cmd_per_lun		= 1,
      .use_clustering		= ENABLE_CLUSTERING,
      .shost_attrs		= SYM53C500_shost_attrs
 };
diff --git a/drivers/scsi/pm8001/pm8001_init.c b/drivers/scsi/pm8001/pm8001_init.c
index 65555916d3b8..a132f2664d2f 100644
--- a/drivers/scsi/pm8001/pm8001_init.c
+++ b/drivers/scsi/pm8001/pm8001_init.c
@@ -78,7 +78,6 @@ static struct scsi_host_template pm8001_sht = {
 	.change_queue_depth	= sas_change_queue_depth,
 	.bios_param		= sas_bios_param,
 	.can_queue		= 1,
-	.cmd_per_lun		= 1,
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
 	.max_sectors		= SCSI_DEFAULT_MAX_SECTORS,
diff --git a/drivers/scsi/ppa.c b/drivers/scsi/ppa.c
index 1db8b26063b4..ee00e27ba396 100644
--- a/drivers/scsi/ppa.c
+++ b/drivers/scsi/ppa.c
@@ -974,7 +974,6 @@ static struct scsi_host_template ppa_template = {
 	.bios_param		= ppa_biosparam,
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
-	.cmd_per_lun		= 1,
 	.use_clustering		= ENABLE_CLUSTERING,
 	.can_queue		= 1,
 	.slave_alloc		= ppa_adjust_queue,
diff --git a/drivers/scsi/ps3rom.c b/drivers/scsi/ps3rom.c
index 5298def33733..4924424d20fe 100644
--- a/drivers/scsi/ps3rom.c
+++ b/drivers/scsi/ps3rom.c
@@ -347,7 +347,6 @@ static struct scsi_host_template ps3rom_host_template = {
 	.can_queue =		1,
 	.this_id =		7,
 	.sg_tablesize =		SG_ALL,
-	.cmd_per_lun =		1,
 	.emulated =             1,		/* only sg driver uses this */
 	.max_sectors =		PS3ROM_MAX_SECTORS,
 	.use_clustering =	ENABLE_CLUSTERING,
diff --git a/drivers/scsi/qla1280.c b/drivers/scsi/qla1280.c
index c68a66e8cfc1..5d0ec42a9317 100644
--- a/drivers/scsi/qla1280.c
+++ b/drivers/scsi/qla1280.c
@@ -4217,7 +4217,6 @@ static struct scsi_host_template qla1280_driver_template = {
 	.can_queue		= 0xfffff,
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
-	.cmd_per_lun		= 1,
 	.use_clustering		= ENABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index 285cb204f300..664013115c9d 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -708,7 +708,7 @@ qla2x00_initialize_adapter(scsi_qla_host_t *vha)
 		if (rval != QLA_SUCCESS) {
 			ql_log(ql_log_warn, vha, 0x00d4,
 			    "Unable to initialize ISP84XX.\n");
-		qla84xx_put_chip(vha);
+			qla84xx_put_chip(vha);
 		}
 	}
 
diff --git a/drivers/scsi/qla2xxx/qla_iocb.c b/drivers/scsi/qla2xxx/qla_iocb.c
index a1ab25fca874..36fbd4c7af8f 100644
--- a/drivers/scsi/qla2xxx/qla_iocb.c
+++ b/drivers/scsi/qla2xxx/qla_iocb.c
@@ -2797,10 +2797,10 @@ qla2x00_start_bidir(srb_t *sp, struct scsi_qla_host *vha, uint32_t tot_dsds)
 	handle = req->current_outstanding_cmd;
 	for (index = 1; index < req->num_outstanding_cmds; index++) {
 		handle++;
-	if (handle == req->num_outstanding_cmds)
-		handle = 1;
-	if (!req->outstanding_cmds[handle])
-		break;
+		if (handle == req->num_outstanding_cmds)
+			handle = 1;
+		if (!req->outstanding_cmds[handle])
+			break;
 	}
 
 	if (index == req->num_outstanding_cmds) {
diff --git a/drivers/scsi/qla2xxx/qla_isr.c b/drivers/scsi/qla2xxx/qla_isr.c
index 6dc14cd782b2..5559d5e75bbf 100644
--- a/drivers/scsi/qla2xxx/qla_isr.c
+++ b/drivers/scsi/qla2xxx/qla_isr.c
@@ -1580,7 +1580,7 @@ qla24xx_tm_iocb_entry(scsi_qla_host_t *vha, struct req_que *req, void *tsk)
 			ql_log(ql_log_warn, fcport->vha, 0x503c,
 			    "Async-%s error - hdl=%x response(%x).\n",
 			    type, sp->handle, sts->data[3]);
-		iocb->u.tmf.data = QLA_FUNCTION_FAILED;
+			iocb->u.tmf.data = QLA_FUNCTION_FAILED;
 		}
 	}
 
@@ -1979,7 +1979,7 @@ qla25xx_process_bidir_status_iocb(scsi_qla_host_t *vha, void *pkt,
 		rval = EXT_STATUS_ERR;
 		break;
 	}
-		bsg_job->reply->reply_payload_rcv_len = 0;
+	bsg_job->reply->reply_payload_rcv_len = 0;
 
 done:
 	/* Return the vendor specific reply to API */
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index 7d2b18f2675c..1620b0ec977b 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -1843,7 +1843,7 @@ qla82xx_set_product_offset(struct qla_hw_data *ha)
 
 	ptab_desc = qla82xx_get_table_desc(unirom,
 		 QLA82XX_URI_DIR_SECT_PRODUCT_TBL);
-       if (!ptab_desc)
+	if (!ptab_desc)
 		return -1;
 
 	entries = cpu_to_le32(ptab_desc->num_entries);
diff --git a/drivers/scsi/qla2xxx/qla_nx2.c b/drivers/scsi/qla2xxx/qla_nx2.c
index ed4d6b6b53e3..000c57e4d033 100644
--- a/drivers/scsi/qla2xxx/qla_nx2.c
+++ b/drivers/scsi/qla2xxx/qla_nx2.c
@@ -397,11 +397,11 @@ qla8044_idc_lock(struct qla_hw_data *ha)
 				 * has the lock, wait for 2secs
 				 * and retry
 				 */
-				 ql_dbg(ql_dbg_p3p, vha, 0xb08a,
-				     "%s: IDC lock Recovery by %d "
-				     "failed, Retrying timeout\n", __func__,
-				     ha->portnum);
-				 timeout = 0;
+				ql_dbg(ql_dbg_p3p, vha, 0xb08a,
+				       "%s: IDC lock Recovery by %d "
+				       "failed, Retrying timeout\n", __func__,
+				       ha->portnum);
+				timeout = 0;
 			}
 		}
 		msleep(QLA8044_DRV_LOCK_MSLEEP);
@@ -3141,8 +3141,7 @@ qla8044_minidump_process_rdmdio(struct scsi_qla_host *vha,
 			goto error;
 
 		addr7 = addr2 - (4 * stride1);
-			data = qla8044_ipmdio_rd_reg(vha, addr1, addr3,
-			    mask, addr7);
+		data = qla8044_ipmdio_rd_reg(vha, addr1, addr3, mask, addr7);
 		if (data == -1)
 			goto error;
 
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index 7462dd70b150..a28815b8276f 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -4418,7 +4418,10 @@ retry_lock2:
 void
 qla83xx_idc_unlock(scsi_qla_host_t *base_vha, uint16_t requester_id)
 {
-	uint16_t options = (requester_id << 15) | BIT_7, retry;
+#if 0
+	uint16_t options = (requester_id << 15) | BIT_7;
+#endif
+	uint16_t retry;
 	uint32_t data;
 	struct qla_hw_data *ha = base_vha->hw;
 
@@ -4454,6 +4457,7 @@ retry_unlock:
 
 	return;
 
+#if 0
 	/* XXX: IDC-unlock implementation using access-control mbx */
 	retry = 0;
 retry_unlock2:
@@ -4469,6 +4473,7 @@ retry_unlock2:
 	}
 
 	return;
+#endif
 }
 
 int
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index fe8a8d157e22..b749026aa592 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -1191,7 +1191,7 @@ static int __qlt_24xx_handle_abts(struct scsi_qla_host *vha,
 	list_for_each_entry(se_cmd, &se_sess->sess_cmd_list, se_cmd_list) {
 		struct qla_tgt_cmd *cmd =
 			container_of(se_cmd, struct qla_tgt_cmd, se_cmd);
-		if (cmd->tag == abts->exchange_addr_to_abort) {
+		if (se_cmd->tag == abts->exchange_addr_to_abort) {
 			lun = cmd->unpacked_lun;
 			found_lun = true;
 			break;
@@ -1728,9 +1728,8 @@ static int qlt_pre_xmit_response(struct qla_tgt_cmd *cmd,
 
 	if (unlikely(cmd->aborted)) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf014,
-		    "qla_target(%d): terminating exchange "
-		    "for aborted cmd=%p (se_cmd=%p, tag=%d)", vha->vp_idx, cmd,
-		    se_cmd, cmd->tag);
+		       "qla_target(%d): terminating exchange for aborted cmd=%p (se_cmd=%p, tag=%lld)",
+		       vha->vp_idx, cmd, se_cmd, se_cmd->tag);
 
 		cmd->state = QLA_TGT_STATE_ABORTED;
 		cmd->cmd_flags |= BIT_6;
@@ -1765,18 +1764,17 @@ static int qlt_pre_xmit_response(struct qla_tgt_cmd *cmd,
 	if (se_cmd->se_cmd_flags & SCF_UNDERFLOW_BIT) {
 		prm->residual = se_cmd->residual_count;
 		ql_dbg(ql_dbg_io + ql_dbg_verbose, vha, 0x305c,
-		    "Residual underflow: %d (tag %d, "
-		    "op %x, bufflen %d, rq_result %x)\n", prm->residual,
-		    cmd->tag, se_cmd->t_task_cdb ? se_cmd->t_task_cdb[0] : 0,
-		    cmd->bufflen, prm->rq_result);
+		    "Residual underflow: %d (tag %lld, op %x, bufflen %d, rq_result %x)\n",
+		       prm->residual, se_cmd->tag,
+		       se_cmd->t_task_cdb ? se_cmd->t_task_cdb[0] : 0,
+		       cmd->bufflen, prm->rq_result);
 		prm->rq_result |= SS_RESIDUAL_UNDER;
 	} else if (se_cmd->se_cmd_flags & SCF_OVERFLOW_BIT) {
 		prm->residual = se_cmd->residual_count;
 		ql_dbg(ql_dbg_io, vha, 0x305d,
-		    "Residual overflow: %d (tag %d, "
-		    "op %x, bufflen %d, rq_result %x)\n", prm->residual,
-		    cmd->tag, se_cmd->t_task_cdb ? se_cmd->t_task_cdb[0] : 0,
-		    cmd->bufflen, prm->rq_result);
+		    "Residual overflow: %d (tag %lld, op %x, bufflen %d, rq_result %x)\n",
+		       prm->residual, se_cmd->tag, se_cmd->t_task_cdb ?
+		       se_cmd->t_task_cdb[0] : 0, cmd->bufflen, prm->rq_result);
 		prm->rq_result |= SS_RESIDUAL_OVER;
 	}
 
@@ -1849,7 +1847,7 @@ static void qlt_check_srr_debug(struct qla_tgt_cmd *cmd, int *xmit_type)
 	    == 50) {
 		*xmit_type &= ~QLA_TGT_XMIT_STATUS;
 		ql_dbg(ql_dbg_tgt_mgt, cmd->vha, 0xf015,
-		    "Dropping cmd %p (tag %d) status", cmd, cmd->tag);
+		    "Dropping cmd %p (tag %d) status", cmd, se_cmd->tag);
 	}
 #endif
 	/*
@@ -1873,7 +1871,7 @@ static void qlt_check_srr_debug(struct qla_tgt_cmd *cmd, int *xmit_type)
 		ql_dbg(ql_dbg_tgt_mgt, cmd->vha, 0xf016,
 		    "Cutting cmd %p (tag %d) buffer"
 		    " tail to len %d, sg_cnt %d (cmd->bufflen %d,"
-		    " cmd->sg_cnt %d)", cmd, cmd->tag, tot_len, leave,
+		    " cmd->sg_cnt %d)", cmd, se_cmd->tag, tot_len, leave,
 		    cmd->bufflen, cmd->sg_cnt);
 
 		cmd->bufflen = tot_len;
@@ -1885,13 +1883,13 @@ static void qlt_check_srr_debug(struct qla_tgt_cmd *cmd, int *xmit_type)
 
 		ql_dbg(ql_dbg_tgt_mgt, cmd->vha, 0xf017,
 		    "Cutting cmd %p (tag %d) buffer head "
-		    "to offset %d (cmd->bufflen %d)", cmd, cmd->tag, offset,
+		    "to offset %d (cmd->bufflen %d)", cmd, se_cmd->tag, offset,
 		    cmd->bufflen);
 		if (offset == 0)
 			*xmit_type &= ~QLA_TGT_XMIT_DATA;
 		else if (qlt_set_data_offset(cmd, offset)) {
 			ql_dbg(ql_dbg_tgt_mgt, cmd->vha, 0xf018,
-			    "qlt_set_data_offset() failed (tag %d)", cmd->tag);
+			    "qlt_set_data_offset() failed (tag %d)", se_cmd->tag);
 		}
 	}
 }
@@ -3194,7 +3192,7 @@ skip_term:
 		return;
 	} else if (cmd->state == QLA_TGT_STATE_ABORTED) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf01e,
-		    "Aborted command %p (tag %d) finished\n", cmd, cmd->tag);
+		  "Aborted command %p (tag %lld) finished\n", cmd, se_cmd->tag);
 	} else {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf05c,
 		    "qla_target(%d): A command in state (%d) should "
@@ -3266,7 +3264,7 @@ static void __qlt_do_work(struct qla_tgt_cmd *cmd)
 		goto out_term;
 
 	cdb = &atio->u.isp24.fcp_cmnd.cdb[0];
-	cmd->tag = atio->u.isp24.exchange_addr;
+	cmd->se_cmd.tag = atio->u.isp24.exchange_addr;
 	cmd->unpacked_lun = scsilun_to_int(
 	    (struct scsi_lun *)&atio->u.isp24.fcp_cmnd.lun);
 
@@ -3712,6 +3710,14 @@ static int qlt_24xx_handle_els(struct scsi_qla_host *vha,
 
 static int qlt_set_data_offset(struct qla_tgt_cmd *cmd, uint32_t offset)
 {
+#if 1
+	/*
+	 * FIXME: Reject non zero SRR relative offset until we can test
+	 * this code properly.
+	 */
+	pr_debug("Rejecting non zero SRR rel_offs: %u\n", offset);
+	return -1;
+#else
 	struct scatterlist *sg, *sgp, *sg_srr, *sg_srr_start = NULL;
 	size_t first_offset = 0, rem_offset = offset, tmp = 0;
 	int i, sg_srr_cnt, bufflen = 0;
@@ -3721,13 +3727,6 @@ static int qlt_set_data_offset(struct qla_tgt_cmd *cmd, uint32_t offset)
 	    "cmd->sg_cnt: %u, direction: %d\n",
 	    cmd, cmd->sg, cmd->sg_cnt, cmd->dma_data_direction);
 
-	/*
-	 * FIXME: Reject non zero SRR relative offset until we can test
-	 * this code properly.
-	 */
-	pr_debug("Rejecting non zero SRR rel_offs: %u\n", offset);
-	return -1;
-
 	if (!cmd->sg || !cmd->sg_cnt) {
 		ql_dbg(ql_dbg_tgt, cmd->vha, 0xe055,
 		    "Missing cmd->sg or zero cmd->sg_cnt in"
@@ -3810,6 +3809,7 @@ static int qlt_set_data_offset(struct qla_tgt_cmd *cmd, uint32_t offset)
 		BUG();
 
 	return 0;
+#endif
 }
 
 static inline int qlt_srr_adjust_data(struct qla_tgt_cmd *cmd,
@@ -3891,9 +3891,8 @@ static void qlt_handle_srr(struct scsi_qla_host *vha,
 			resp = 1;
 		} else {
 			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf064,
-			    "qla_target(%d): SRR for in data for cmd "
-			    "without them (tag %d, SCSI status %d), "
-			    "reject", vha->vp_idx, cmd->tag,
+			       "qla_target(%d): SRR for in data for cmd without them (tag %lld, SCSI status %d), reject",
+			       vha->vp_idx, se_cmd->tag,
 			    cmd->se_cmd.scsi_status);
 			goto out_reject;
 		}
@@ -3927,10 +3926,8 @@ static void qlt_handle_srr(struct scsi_qla_host *vha,
 			}
 		} else {
 			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf066,
-			    "qla_target(%d): SRR for out data for cmd "
-			    "without them (tag %d, SCSI status %d), "
-			    "reject", vha->vp_idx, cmd->tag,
-			    cmd->se_cmd.scsi_status);
+			    "qla_target(%d): SRR for out data for cmd without them (tag %lld, SCSI status %d), reject",
+			       vha->vp_idx, se_cmd->tag, cmd->se_cmd.scsi_status);
 			goto out_reject;
 		}
 		break;
@@ -4051,10 +4048,9 @@ restart:
 		cmd->sg = se_cmd->t_data_sg;
 
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf02c,
-		    "SRR cmd %p (se_cmd %p, tag %d, op %x), "
-		    "sg_cnt=%d, offset=%d", cmd, &cmd->se_cmd, cmd->tag,
-		    se_cmd->t_task_cdb ? se_cmd->t_task_cdb[0] : 0,
-		    cmd->sg_cnt, cmd->offset);
+		       "SRR cmd %p (se_cmd %p, tag %lld, op %x), sg_cnt=%d, offset=%d",
+		       cmd, &cmd->se_cmd, se_cmd->tag, se_cmd->t_task_cdb ?
+		       se_cmd->t_task_cdb[0] : 0, cmd->sg_cnt, cmd->offset);
 
 		qlt_handle_srr(vha, sctio, imm);
 
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index 332086776dfe..985d76dd706b 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -924,7 +924,6 @@ struct qla_tgt_cmd {
 	int sg_cnt;		/* SG segments count */
 	int bufflen;		/* cmd buffer length */
 	int offset;
-	uint32_t tag;
 	uint32_t unpacked_lun;
 	enum dma_data_direction dma_data_direction;
 	uint32_t reset_count;
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.c b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
index 5c9e680aa375..d9a8c6084346 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -27,6 +27,7 @@
 #include <linux/moduleparam.h>
 #include <generated/utsrelease.h>
 #include <linux/utsname.h>
+#include <linux/vmalloc.h>
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/slab.h>
@@ -43,7 +44,6 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 
 #include "qla_def.h"
@@ -53,9 +53,6 @@
 static struct workqueue_struct *tcm_qla2xxx_free_wq;
 static struct workqueue_struct *tcm_qla2xxx_cmd_wq;
 
-static const struct target_core_fabric_ops tcm_qla2xxx_ops;
-static const struct target_core_fabric_ops tcm_qla2xxx_npiv_ops;
-
 /*
  * Parse WWN.
  * If strict, we require lower-case hex and colon separators to be sure
@@ -190,23 +187,6 @@ static char *tcm_qla2xxx_npiv_get_fabric_name(void)
 	return "qla2xxx_npiv";
 }
 
-static u8 tcm_qla2xxx_get_fabric_proto_ident(struct se_portal_group *se_tpg)
-{
-	struct tcm_qla2xxx_tpg *tpg = container_of(se_tpg,
-				struct tcm_qla2xxx_tpg, se_tpg);
-	struct tcm_qla2xxx_lport *lport = tpg->lport;
-	u8 proto_id;
-
-	switch (lport->lport_proto_id) {
-	case SCSI_PROTOCOL_FCP:
-	default:
-		proto_id = fc_get_fabric_proto_ident(se_tpg);
-		break;
-	}
-
-	return proto_id;
-}
-
 static char *tcm_qla2xxx_get_fabric_wwn(struct se_portal_group *se_tpg)
 {
 	struct tcm_qla2xxx_tpg *tpg = container_of(se_tpg,
@@ -223,78 +203,6 @@ static u16 tcm_qla2xxx_get_tag(struct se_portal_group *se_tpg)
 	return tpg->lport_tpgt;
 }
 
-static u32 tcm_qla2xxx_get_default_depth(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static u32 tcm_qla2xxx_get_pr_transport_id(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code,
-	unsigned char *buf)
-{
-	struct tcm_qla2xxx_tpg *tpg = container_of(se_tpg,
-				struct tcm_qla2xxx_tpg, se_tpg);
-	struct tcm_qla2xxx_lport *lport = tpg->lport;
-	int ret = 0;
-
-	switch (lport->lport_proto_id) {
-	case SCSI_PROTOCOL_FCP:
-	default:
-		ret = fc_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-		break;
-	}
-
-	return ret;
-}
-
-static u32 tcm_qla2xxx_get_pr_transport_id_len(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code)
-{
-	struct tcm_qla2xxx_tpg *tpg = container_of(se_tpg,
-				struct tcm_qla2xxx_tpg, se_tpg);
-	struct tcm_qla2xxx_lport *lport = tpg->lport;
-	int ret = 0;
-
-	switch (lport->lport_proto_id) {
-	case SCSI_PROTOCOL_FCP:
-	default:
-		ret = fc_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-		break;
-	}
-
-	return ret;
-}
-
-static char *tcm_qla2xxx_parse_pr_out_transport_id(
-	struct se_portal_group *se_tpg,
-	const char *buf,
-	u32 *out_tid_len,
-	char **port_nexus_ptr)
-{
-	struct tcm_qla2xxx_tpg *tpg = container_of(se_tpg,
-				struct tcm_qla2xxx_tpg, se_tpg);
-	struct tcm_qla2xxx_lport *lport = tpg->lport;
-	char *tid = NULL;
-
-	switch (lport->lport_proto_id) {
-	case SCSI_PROTOCOL_FCP:
-	default:
-		tid = fc_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-		break;
-	}
-
-	return tid;
-}
-
 static int tcm_qla2xxx_check_demo_mode(struct se_portal_group *se_tpg)
 {
 	struct tcm_qla2xxx_tpg *tpg = container_of(se_tpg,
@@ -343,29 +251,6 @@ static int tcm_qla2xxx_check_prot_fabric_only(struct se_portal_group *se_tpg)
 	return tpg->tpg_attrib.fabric_prot_type;
 }
 
-static struct se_node_acl *tcm_qla2xxx_alloc_fabric_acl(
-	struct se_portal_group *se_tpg)
-{
-	struct tcm_qla2xxx_nacl *nacl;
-
-	nacl = kzalloc(sizeof(struct tcm_qla2xxx_nacl), GFP_KERNEL);
-	if (!nacl) {
-		pr_err("Unable to allocate struct tcm_qla2xxx_nacl\n");
-		return NULL;
-	}
-
-	return &nacl->se_node_acl;
-}
-
-static void tcm_qla2xxx_release_fabric_acl(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl)
-{
-	struct tcm_qla2xxx_nacl *nacl = container_of(se_nacl,
-			struct tcm_qla2xxx_nacl, se_node_acl);
-	kfree(nacl);
-}
-
 static u32 tcm_qla2xxx_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
 	struct tcm_qla2xxx_tpg *tpg = container_of(se_tpg,
@@ -429,7 +314,7 @@ static int tcm_qla2xxx_check_stop_free(struct se_cmd *se_cmd)
 		cmd->cmd_flags |= BIT_14;
 	}
 
-	return target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+	return target_put_sess_cmd(se_cmd);
 }
 
 /* tcm_qla2xxx_release_cmd - Callback from TCM Core to release underlying
@@ -533,19 +418,6 @@ static void tcm_qla2xxx_set_default_node_attrs(struct se_node_acl *nacl)
 	return;
 }
 
-static u32 tcm_qla2xxx_get_task_tag(struct se_cmd *se_cmd)
-{
-	struct qla_tgt_cmd *cmd;
-
-	/* check for task mgmt cmd */
-	if (se_cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)
-		return 0xffffffff;
-
-	cmd = container_of(se_cmd, struct qla_tgt_cmd, se_cmd);
-
-	return cmd->tag;
-}
-
 static int tcm_qla2xxx_get_cmd_state(struct se_cmd *se_cmd)
 {
 	return 0;
@@ -826,17 +698,6 @@ static void tcm_qla2xxx_release_session(struct kref *kref)
 	qlt_unreg_sess(se_sess->fabric_sess_ptr);
 }
 
-static void tcm_qla2xxx_put_session(struct se_session *se_sess)
-{
-	struct qla_tgt_sess *sess = se_sess->fabric_sess_ptr;
-	struct qla_hw_data *ha = sess->vha->hw;
-	unsigned long flags;
-
-	spin_lock_irqsave(&ha->hardware_lock, flags);
-	kref_put(&se_sess->sess_kref, tcm_qla2xxx_release_session);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
-}
-
 static void tcm_qla2xxx_put_sess(struct qla_tgt_sess *sess)
 {
 	if (!sess)
@@ -852,53 +713,20 @@ static void tcm_qla2xxx_shutdown_sess(struct qla_tgt_sess *sess)
 	target_sess_cmd_list_set_waiting(sess->se_sess);
 }
 
-static struct se_node_acl *tcm_qla2xxx_make_nodeacl(
-	struct se_portal_group *se_tpg,
-	struct config_group *group,
-	const char *name)
+static int tcm_qla2xxx_init_nodeacl(struct se_node_acl *se_nacl,
+		const char *name)
 {
-	struct se_node_acl *se_nacl, *se_nacl_new;
-	struct tcm_qla2xxx_nacl *nacl;
+	struct tcm_qla2xxx_nacl *nacl =
+		container_of(se_nacl, struct tcm_qla2xxx_nacl, se_node_acl);
 	u64 wwnn;
-	u32 qla2xxx_nexus_depth;
 
 	if (tcm_qla2xxx_parse_wwn(name, &wwnn, 1) < 0)
-		return ERR_PTR(-EINVAL);
-
-	se_nacl_new = tcm_qla2xxx_alloc_fabric_acl(se_tpg);
-	if (!se_nacl_new)
-		return ERR_PTR(-ENOMEM);
-/* #warning FIXME: Hardcoded qla2xxx_nexus depth in tcm_qla2xxx_make_nodeacl */
-	qla2xxx_nexus_depth = 1;
+		return -EINVAL;
 
-	/*
-	 * se_nacl_new may be released by core_tpg_add_initiator_node_acl()
-	 * when converting a NodeACL from demo mode -> explict
-	 */
-	se_nacl = core_tpg_add_initiator_node_acl(se_tpg, se_nacl_new,
-				name, qla2xxx_nexus_depth);
-	if (IS_ERR(se_nacl)) {
-		tcm_qla2xxx_release_fabric_acl(se_tpg, se_nacl_new);
-		return se_nacl;
-	}
-	/*
-	 * Locate our struct tcm_qla2xxx_nacl and set the FC Nport WWPN
-	 */
-	nacl = container_of(se_nacl, struct tcm_qla2xxx_nacl, se_node_acl);
 	nacl->nport_wwnn = wwnn;
 	tcm_qla2xxx_format_wwn(&nacl->nport_name[0], TCM_QLA2XXX_NAMELEN, wwnn);
 
-	return se_nacl;
-}
-
-static void tcm_qla2xxx_drop_nodeacl(struct se_node_acl *se_acl)
-{
-	struct se_portal_group *se_tpg = se_acl->se_tpg;
-	struct tcm_qla2xxx_nacl *nacl = container_of(se_acl,
-				struct tcm_qla2xxx_nacl, se_node_acl);
-
-	core_tpg_del_initiator_node_acl(se_tpg, se_acl, 1);
-	kfree(nacl);
+	return 0;
 }
 
 /* Start items for tcm_qla2xxx_tpg_attrib_cit */
@@ -1174,8 +1002,7 @@ static struct se_portal_group *tcm_qla2xxx_make_tpg(
 	tpg->tpg_attrib.cache_dynamic_acls = 1;
 	tpg->tpg_attrib.demo_mode_login_only = 1;
 
-	ret = core_tpg_register(&tcm_qla2xxx_ops, wwn,
-				&tpg->se_tpg, tpg, TRANSPORT_TPG_TYPE_NORMAL);
+	ret = core_tpg_register(wwn, &tpg->se_tpg, SCSI_PROTOCOL_FCP);
 	if (ret < 0) {
 		kfree(tpg);
 		return NULL;
@@ -1294,8 +1121,7 @@ static struct se_portal_group *tcm_qla2xxx_npiv_make_tpg(
 	tpg->tpg_attrib.cache_dynamic_acls = 1;
 	tpg->tpg_attrib.demo_mode_login_only = 1;
 
-	ret = core_tpg_register(&tcm_qla2xxx_npiv_ops, wwn,
-				&tpg->se_tpg, tpg, TRANSPORT_TPG_TYPE_NORMAL);
+	ret = core_tpg_register(wwn, &tpg->se_tpg, SCSI_PROTOCOL_FCP);
 	if (ret < 0) {
 		kfree(tpg);
 		return NULL;
@@ -1987,14 +1813,10 @@ static struct configfs_attribute *tcm_qla2xxx_wwn_attrs[] = {
 static const struct target_core_fabric_ops tcm_qla2xxx_ops = {
 	.module				= THIS_MODULE,
 	.name				= "qla2xxx",
+	.node_acl_size			= sizeof(struct tcm_qla2xxx_nacl),
 	.get_fabric_name		= tcm_qla2xxx_get_fabric_name,
-	.get_fabric_proto_ident		= tcm_qla2xxx_get_fabric_proto_ident,
 	.tpg_get_wwn			= tcm_qla2xxx_get_fabric_wwn,
 	.tpg_get_tag			= tcm_qla2xxx_get_tag,
-	.tpg_get_default_depth		= tcm_qla2xxx_get_default_depth,
-	.tpg_get_pr_transport_id	= tcm_qla2xxx_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= tcm_qla2xxx_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= tcm_qla2xxx_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= tcm_qla2xxx_check_demo_mode,
 	.tpg_check_demo_mode_cache	= tcm_qla2xxx_check_demo_mode_cache,
 	.tpg_check_demo_mode_write_protect =
@@ -2003,12 +1825,9 @@ static const struct target_core_fabric_ops tcm_qla2xxx_ops = {
 					tcm_qla2xxx_check_prod_write_protect,
 	.tpg_check_prot_fabric_only	= tcm_qla2xxx_check_prot_fabric_only,
 	.tpg_check_demo_mode_login_only = tcm_qla2xxx_check_demo_mode_login_only,
-	.tpg_alloc_fabric_acl		= tcm_qla2xxx_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= tcm_qla2xxx_release_fabric_acl,
 	.tpg_get_inst_index		= tcm_qla2xxx_tpg_get_inst_index,
 	.check_stop_free		= tcm_qla2xxx_check_stop_free,
 	.release_cmd			= tcm_qla2xxx_release_cmd,
-	.put_session			= tcm_qla2xxx_put_session,
 	.shutdown_session		= tcm_qla2xxx_shutdown_session,
 	.close_session			= tcm_qla2xxx_close_session,
 	.sess_get_index			= tcm_qla2xxx_sess_get_index,
@@ -2016,7 +1835,6 @@ static const struct target_core_fabric_ops tcm_qla2xxx_ops = {
 	.write_pending			= tcm_qla2xxx_write_pending,
 	.write_pending_status		= tcm_qla2xxx_write_pending_status,
 	.set_default_node_attributes	= tcm_qla2xxx_set_default_node_attrs,
-	.get_task_tag			= tcm_qla2xxx_get_task_tag,
 	.get_cmd_state			= tcm_qla2xxx_get_cmd_state,
 	.queue_data_in			= tcm_qla2xxx_queue_data_in,
 	.queue_status			= tcm_qla2xxx_queue_status,
@@ -2030,12 +1848,7 @@ static const struct target_core_fabric_ops tcm_qla2xxx_ops = {
 	.fabric_drop_wwn		= tcm_qla2xxx_drop_lport,
 	.fabric_make_tpg		= tcm_qla2xxx_make_tpg,
 	.fabric_drop_tpg		= tcm_qla2xxx_drop_tpg,
-	.fabric_post_link		= NULL,
-	.fabric_pre_unlink		= NULL,
-	.fabric_make_np			= NULL,
-	.fabric_drop_np			= NULL,
-	.fabric_make_nodeacl		= tcm_qla2xxx_make_nodeacl,
-	.fabric_drop_nodeacl		= tcm_qla2xxx_drop_nodeacl,
+	.fabric_init_nodeacl		= tcm_qla2xxx_init_nodeacl,
 
 	.tfc_wwn_attrs			= tcm_qla2xxx_wwn_attrs,
 	.tfc_tpg_base_attrs		= tcm_qla2xxx_tpg_attrs,
@@ -2045,26 +1858,19 @@ static const struct target_core_fabric_ops tcm_qla2xxx_ops = {
 static const struct target_core_fabric_ops tcm_qla2xxx_npiv_ops = {
 	.module				= THIS_MODULE,
 	.name				= "qla2xxx_npiv",
+	.node_acl_size			= sizeof(struct tcm_qla2xxx_nacl),
 	.get_fabric_name		= tcm_qla2xxx_npiv_get_fabric_name,
-	.get_fabric_proto_ident		= tcm_qla2xxx_get_fabric_proto_ident,
 	.tpg_get_wwn			= tcm_qla2xxx_get_fabric_wwn,
 	.tpg_get_tag			= tcm_qla2xxx_get_tag,
-	.tpg_get_default_depth		= tcm_qla2xxx_get_default_depth,
-	.tpg_get_pr_transport_id	= tcm_qla2xxx_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= tcm_qla2xxx_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= tcm_qla2xxx_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= tcm_qla2xxx_check_demo_mode,
 	.tpg_check_demo_mode_cache	= tcm_qla2xxx_check_demo_mode_cache,
 	.tpg_check_demo_mode_write_protect = tcm_qla2xxx_check_demo_mode,
 	.tpg_check_prod_mode_write_protect =
 	    tcm_qla2xxx_check_prod_write_protect,
 	.tpg_check_demo_mode_login_only	= tcm_qla2xxx_check_demo_mode_login_only,
-	.tpg_alloc_fabric_acl		= tcm_qla2xxx_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= tcm_qla2xxx_release_fabric_acl,
 	.tpg_get_inst_index		= tcm_qla2xxx_tpg_get_inst_index,
 	.check_stop_free                = tcm_qla2xxx_check_stop_free,
 	.release_cmd			= tcm_qla2xxx_release_cmd,
-	.put_session			= tcm_qla2xxx_put_session,
 	.shutdown_session		= tcm_qla2xxx_shutdown_session,
 	.close_session			= tcm_qla2xxx_close_session,
 	.sess_get_index			= tcm_qla2xxx_sess_get_index,
@@ -2072,7 +1878,6 @@ static const struct target_core_fabric_ops tcm_qla2xxx_npiv_ops = {
 	.write_pending			= tcm_qla2xxx_write_pending,
 	.write_pending_status		= tcm_qla2xxx_write_pending_status,
 	.set_default_node_attributes	= tcm_qla2xxx_set_default_node_attrs,
-	.get_task_tag			= tcm_qla2xxx_get_task_tag,
 	.get_cmd_state			= tcm_qla2xxx_get_cmd_state,
 	.queue_data_in			= tcm_qla2xxx_queue_data_in,
 	.queue_status			= tcm_qla2xxx_queue_status,
@@ -2086,12 +1891,7 @@ static const struct target_core_fabric_ops tcm_qla2xxx_npiv_ops = {
 	.fabric_drop_wwn		= tcm_qla2xxx_npiv_drop_lport,
 	.fabric_make_tpg		= tcm_qla2xxx_npiv_make_tpg,
 	.fabric_drop_tpg		= tcm_qla2xxx_drop_tpg,
-	.fabric_post_link		= NULL,
-	.fabric_pre_unlink		= NULL,
-	.fabric_make_np			= NULL,
-	.fabric_drop_np			= NULL,
-	.fabric_make_nodeacl		= tcm_qla2xxx_make_nodeacl,
-	.fabric_drop_nodeacl		= tcm_qla2xxx_drop_nodeacl,
+	.fabric_init_nodeacl		= tcm_qla2xxx_init_nodeacl,
 
 	.tfc_wwn_attrs			= tcm_qla2xxx_wwn_attrs,
 	.tfc_tpg_base_attrs		= tcm_qla2xxx_npiv_tpg_attrs,
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.h b/drivers/scsi/qla2xxx/tcm_qla2xxx.h
index 23295115c9fc..3bbf4cb6fd97 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.h
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.h
@@ -13,6 +13,8 @@
 #include "qla_target.h"
 
 struct tcm_qla2xxx_nacl {
+	struct se_node_acl se_node_acl;
+
 	/* From libfc struct fc_rport->port_id */
 	u32 nport_id;
 	/* Binary World Wide unique Node Name for remote FC Initiator Nport */
@@ -23,8 +25,6 @@ struct tcm_qla2xxx_nacl {
 	struct qla_tgt_sess *qla_tgt_sess;
 	/* Pointer to TCM FC nexus */
 	struct se_session *nport_nexus;
-	/* Returned by tcm_qla2xxx_make_nodeacl() */
-	struct se_node_acl se_node_acl;
 };
 
 struct tcm_qla2xxx_tpg_attrib {
@@ -57,8 +57,6 @@ struct tcm_qla2xxx_fc_loopid {
 };
 
 struct tcm_qla2xxx_lport {
-	/* SCSI protocol the lport is providing */
-	u8 lport_proto_id;
 	/* Binary World Wide unique Port Name for FC Target Lport */
 	u64 lport_wwpn;
 	/* Binary World Wide unique Port Name for FC NPIV Target Lport */
diff --git a/drivers/scsi/qla4xxx/ql4_83xx.c b/drivers/scsi/qla4xxx/ql4_83xx.c
index 556c1525f881..5d4f8e67fb25 100644
--- a/drivers/scsi/qla4xxx/ql4_83xx.c
+++ b/drivers/scsi/qla4xxx/ql4_83xx.c
@@ -828,7 +828,7 @@ void qla4_83xx_read_reset_template(struct scsi_qla_host *ha)
 	ret_val = qla4_83xx_flash_read_u32(ha, addr, p_buff,
 					   tmplt_hdr_def_size);
 	if (ret_val != QLA_SUCCESS) {
-		ql4_printk(KERN_ERR, ha, "%s: Failed to read reset tempelate\n",
+		ql4_printk(KERN_ERR, ha, "%s: Failed to read reset template\n",
 			   __func__);
 		goto exit_read_template_error;
 	}
diff --git a/drivers/scsi/qla4xxx/ql4_bsg.c b/drivers/scsi/qla4xxx/ql4_bsg.c
index 9f92cbf96477..415ee5eb3fc7 100644
--- a/drivers/scsi/qla4xxx/ql4_bsg.c
+++ b/drivers/scsi/qla4xxx/ql4_bsg.c
@@ -571,7 +571,7 @@ static int qla4_83xx_pre_loopback_config(struct scsi_qla_host *ha,
 
 	if ((config & ENABLE_INTERNAL_LOOPBACK) ||
 	    (config & ENABLE_EXTERNAL_LOOPBACK)) {
-		ql4_printk(KERN_INFO, ha, "%s: Loopback diagnostics already in progress. Invalid requiest\n",
+		ql4_printk(KERN_INFO, ha, "%s: Loopback diagnostics already in progress. Invalid request\n",
 			   __func__);
 		goto exit_pre_loopback_config;
 	}
diff --git a/drivers/scsi/qla4xxx/ql4_def.h b/drivers/scsi/qla4xxx/ql4_def.h
index 8f6d0fb2cd80..a7cfc270bd08 100644
--- a/drivers/scsi/qla4xxx/ql4_def.h
+++ b/drivers/scsi/qla4xxx/ql4_def.h
@@ -26,6 +26,7 @@
 #include <linux/mutex.h>
 #include <linux/aer.h>
 #include <linux/bsg-lib.h>
+#include <linux/vmalloc.h>
 
 #include <net/tcp.h>
 #include <scsi/scsi.h>
diff --git a/drivers/scsi/qlogicfas.c b/drivers/scsi/qlogicfas.c
index a22bb1b40ce2..61cac87fb86f 100644
--- a/drivers/scsi/qlogicfas.c
+++ b/drivers/scsi/qlogicfas.c
@@ -193,7 +193,6 @@ static struct scsi_host_template qlogicfas_driver_template = {
 	.can_queue		= 1,
 	.this_id		= -1,
 	.sg_tablesize		= SG_ALL,
-	.cmd_per_lun		= 1,
 	.use_clustering		= DISABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/qlogicpti.c b/drivers/scsi/qlogicpti.c
index fe122700cad8..676385ff28ef 100644
--- a/drivers/scsi/qlogicpti.c
+++ b/drivers/scsi/qlogicpti.c
@@ -1287,7 +1287,6 @@ static struct scsi_host_template qpti_template = {
 	.can_queue		= QLOGICPTI_REQ_QUEUE_LEN,
 	.this_id		= 7,
 	.sg_tablesize		= QLOGICPTI_MAX_SG(QLOGICPTI_REQ_QUEUE_LEN),
-	.cmd_per_lun		= 1,
 	.use_clustering		= ENABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/scsi.c b/drivers/scsi/scsi.c
index 3833bf59fb66..207d6a7a1bd0 100644
--- a/drivers/scsi/scsi.c
+++ b/drivers/scsi/scsi.c
@@ -98,52 +98,6 @@ EXPORT_SYMBOL(scsi_sd_probe_domain);
 ASYNC_DOMAIN_EXCLUSIVE(scsi_sd_pm_domain);
 EXPORT_SYMBOL(scsi_sd_pm_domain);
 
-/* NB: These are exposed through /proc/scsi/scsi and form part of the ABI.
- * You may not alter any existing entry (although adding new ones is
- * encouraged once assigned by ANSI/INCITS T10
- */
-static const char *const scsi_device_types[] = {
-	"Direct-Access    ",
-	"Sequential-Access",
-	"Printer          ",
-	"Processor        ",
-	"WORM             ",
-	"CD-ROM           ",
-	"Scanner          ",
-	"Optical Device   ",
-	"Medium Changer   ",
-	"Communications   ",
-	"ASC IT8          ",
-	"ASC IT8          ",
-	"RAID             ",
-	"Enclosure        ",
-	"Direct-Access-RBC",
-	"Optical card     ",
-	"Bridge controller",
-	"Object storage   ",
-	"Automation/Drive ",
-	"Security Manager ",
-	"Direct-Access-ZBC",
-};
-
-/**
- * scsi_device_type - Return 17 char string indicating device type.
- * @type: type number to look up
- */
-
-const char * scsi_device_type(unsigned type)
-{
-	if (type == 0x1e)
-		return "Well-known LUN   ";
-	if (type == 0x1f)
-		return "No Device        ";
-	if (type >= ARRAY_SIZE(scsi_device_types))
-		return "Unknown          ";
-	return scsi_device_types[type];
-}
-
-EXPORT_SYMBOL(scsi_device_type);
-
 struct scsi_host_cmd_pool {
 	struct kmem_cache	*cmd_slab;
 	struct kmem_cache	*sense_slab;
diff --git a/drivers/scsi/scsi_common.c b/drivers/scsi/scsi_common.c
new file mode 100644
index 000000000000..2ff092252b76
--- /dev/null
+++ b/drivers/scsi/scsi_common.c
@@ -0,0 +1,178 @@
+/*
+ * SCSI functions used by both the initiator and the target code.
+ */
+
+#include <linux/bug.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <scsi/scsi_common.h>
+
+/* NB: These are exposed through /proc/scsi/scsi and form part of the ABI.
+ * You may not alter any existing entry (although adding new ones is
+ * encouraged once assigned by ANSI/INCITS T10
+ */
+static const char *const scsi_device_types[] = {
+	"Direct-Access    ",
+	"Sequential-Access",
+	"Printer          ",
+	"Processor        ",
+	"WORM             ",
+	"CD-ROM           ",
+	"Scanner          ",
+	"Optical Device   ",
+	"Medium Changer   ",
+	"Communications   ",
+	"ASC IT8          ",
+	"ASC IT8          ",
+	"RAID             ",
+	"Enclosure        ",
+	"Direct-Access-RBC",
+	"Optical card     ",
+	"Bridge controller",
+	"Object storage   ",
+	"Automation/Drive ",
+	"Security Manager ",
+	"Direct-Access-ZBC",
+};
+
+/**
+ * scsi_device_type - Return 17 char string indicating device type.
+ * @type: type number to look up
+ */
+const char *scsi_device_type(unsigned type)
+{
+	if (type == 0x1e)
+		return "Well-known LUN   ";
+	if (type == 0x1f)
+		return "No Device        ";
+	if (type >= ARRAY_SIZE(scsi_device_types))
+		return "Unknown          ";
+	return scsi_device_types[type];
+}
+EXPORT_SYMBOL(scsi_device_type);
+
+/**
+ * scsilun_to_int - convert a scsi_lun to an int
+ * @scsilun:	struct scsi_lun to be converted.
+ *
+ * Description:
+ *     Convert @scsilun from a struct scsi_lun to a four byte host byte-ordered
+ *     integer, and return the result. The caller must check for
+ *     truncation before using this function.
+ *
+ * Notes:
+ *     For a description of the LUN format, post SCSI-3 see the SCSI
+ *     Architecture Model, for SCSI-3 see the SCSI Controller Commands.
+ *
+ *     Given a struct scsi_lun of: d2 04 0b 03 00 00 00 00, this function
+ *     returns the integer: 0x0b03d204
+ *
+ *     This encoding will return a standard integer LUN for LUNs smaller
+ *     than 256, which typically use a single level LUN structure with
+ *     addressing method 0.
+ */
+u64 scsilun_to_int(struct scsi_lun *scsilun)
+{
+	int i;
+	u64 lun;
+
+	lun = 0;
+	for (i = 0; i < sizeof(lun); i += 2)
+		lun = lun | (((u64)scsilun->scsi_lun[i] << ((i + 1) * 8)) |
+			     ((u64)scsilun->scsi_lun[i + 1] << (i * 8)));
+	return lun;
+}
+EXPORT_SYMBOL(scsilun_to_int);
+
+/**
+ * int_to_scsilun - reverts an int into a scsi_lun
+ * @lun:        integer to be reverted
+ * @scsilun:	struct scsi_lun to be set.
+ *
+ * Description:
+ *     Reverts the functionality of the scsilun_to_int, which packed
+ *     an 8-byte lun value into an int. This routine unpacks the int
+ *     back into the lun value.
+ *
+ * Notes:
+ *     Given an integer : 0x0b03d204,  this function returns a
+ *     struct scsi_lun of: d2 04 0b 03 00 00 00 00
+ *
+ */
+void int_to_scsilun(u64 lun, struct scsi_lun *scsilun)
+{
+	int i;
+
+	memset(scsilun->scsi_lun, 0, sizeof(scsilun->scsi_lun));
+
+	for (i = 0; i < sizeof(lun); i += 2) {
+		scsilun->scsi_lun[i] = (lun >> 8) & 0xFF;
+		scsilun->scsi_lun[i+1] = lun & 0xFF;
+		lun = lun >> 16;
+	}
+}
+EXPORT_SYMBOL(int_to_scsilun);
+
+/**
+ * scsi_normalize_sense - normalize main elements from either fixed or
+ *			descriptor sense data format into a common format.
+ *
+ * @sense_buffer:	byte array containing sense data returned by device
+ * @sb_len:		number of valid bytes in sense_buffer
+ * @sshdr:		pointer to instance of structure that common
+ *			elements are written to.
+ *
+ * Notes:
+ *	The "main elements" from sense data are: response_code, sense_key,
+ *	asc, ascq and additional_length (only for descriptor format).
+ *
+ *	Typically this function can be called after a device has
+ *	responded to a SCSI command with the CHECK_CONDITION status.
+ *
+ * Return value:
+ *	true if valid sense data information found, else false;
+ */
+bool scsi_normalize_sense(const u8 *sense_buffer, int sb_len,
+			  struct scsi_sense_hdr *sshdr)
+{
+	if (!sense_buffer || !sb_len)
+		return false;
+
+	memset(sshdr, 0, sizeof(struct scsi_sense_hdr));
+
+	sshdr->response_code = (sense_buffer[0] & 0x7f);
+
+	if (!scsi_sense_valid(sshdr))
+		return false;
+
+	if (sshdr->response_code >= 0x72) {
+		/*
+		 * descriptor format
+		 */
+		if (sb_len > 1)
+			sshdr->sense_key = (sense_buffer[1] & 0xf);
+		if (sb_len > 2)
+			sshdr->asc = sense_buffer[2];
+		if (sb_len > 3)
+			sshdr->ascq = sense_buffer[3];
+		if (sb_len > 7)
+			sshdr->additional_length = sense_buffer[7];
+	} else {
+		/*
+		 * fixed format
+		 */
+		if (sb_len > 2)
+			sshdr->sense_key = (sense_buffer[2] & 0xf);
+		if (sb_len > 7) {
+			sb_len = (sb_len < (sense_buffer[7] + 8)) ?
+					 sb_len : (sense_buffer[7] + 8);
+			if (sb_len > 12)
+				sshdr->asc = sense_buffer[12];
+			if (sb_len > 13)
+				sshdr->ascq = sense_buffer[13];
+		}
+	}
+
+	return true;
+}
+EXPORT_SYMBOL(scsi_normalize_sense);
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index 1f8e2dc9c616..30268bb2ddb6 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -2363,17 +2363,13 @@ do_device_access(struct scsi_cmnd *scmd, u64 lba, u32 num, bool do_write)
 	u64 block, rest = 0;
 	struct scsi_data_buffer *sdb;
 	enum dma_data_direction dir;
-	size_t (*func)(struct scatterlist *, unsigned int, void *, size_t,
-		       off_t);
 
 	if (do_write) {
 		sdb = scsi_out(scmd);
 		dir = DMA_TO_DEVICE;
-		func = sg_pcopy_to_buffer;
 	} else {
 		sdb = scsi_in(scmd);
 		dir = DMA_FROM_DEVICE;
-		func = sg_pcopy_from_buffer;
 	}
 
 	if (!sdb->length)
@@ -2385,16 +2381,16 @@ do_device_access(struct scsi_cmnd *scmd, u64 lba, u32 num, bool do_write)
 	if (block + num > sdebug_store_sectors)
 		rest = block + num - sdebug_store_sectors;
 
-	ret = func(sdb->table.sgl, sdb->table.nents,
+	ret = sg_copy_buffer(sdb->table.sgl, sdb->table.nents,
 		   fake_storep + (block * scsi_debug_sector_size),
-		   (num - rest) * scsi_debug_sector_size, 0);
+		   (num - rest) * scsi_debug_sector_size, 0, do_write);
 	if (ret != (num - rest) * scsi_debug_sector_size)
 		return ret;
 
 	if (rest) {
-		ret += func(sdb->table.sgl, sdb->table.nents,
+		ret += sg_copy_buffer(sdb->table.sgl, sdb->table.nents,
 			    fake_storep, rest * scsi_debug_sector_size,
-			    (num - rest) * scsi_debug_sector_size);
+			    (num - rest) * scsi_debug_sector_size, do_write);
 	}
 
 	return ret;
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index c95a4e943fc6..106884a5444e 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -2399,70 +2399,6 @@ out_put_autopm_host:
 }
 EXPORT_SYMBOL(scsi_ioctl_reset);
 
-/**
- * scsi_normalize_sense - normalize main elements from either fixed or
- *			descriptor sense data format into a common format.
- *
- * @sense_buffer:	byte array containing sense data returned by device
- * @sb_len:		number of valid bytes in sense_buffer
- * @sshdr:		pointer to instance of structure that common
- *			elements are written to.
- *
- * Notes:
- *	The "main elements" from sense data are: response_code, sense_key,
- *	asc, ascq and additional_length (only for descriptor format).
- *
- *	Typically this function can be called after a device has
- *	responded to a SCSI command with the CHECK_CONDITION status.
- *
- * Return value:
- *	true if valid sense data information found, else false;
- */
-bool scsi_normalize_sense(const u8 *sense_buffer, int sb_len,
-			  struct scsi_sense_hdr *sshdr)
-{
-	if (!sense_buffer || !sb_len)
-		return false;
-
-	memset(sshdr, 0, sizeof(struct scsi_sense_hdr));
-
-	sshdr->response_code = (sense_buffer[0] & 0x7f);
-
-	if (!scsi_sense_valid(sshdr))
-		return false;
-
-	if (sshdr->response_code >= 0x72) {
-		/*
-		 * descriptor format
-		 */
-		if (sb_len > 1)
-			sshdr->sense_key = (sense_buffer[1] & 0xf);
-		if (sb_len > 2)
-			sshdr->asc = sense_buffer[2];
-		if (sb_len > 3)
-			sshdr->ascq = sense_buffer[3];
-		if (sb_len > 7)
-			sshdr->additional_length = sense_buffer[7];
-	} else {
-		/*
-		 * fixed format
-		 */
-		if (sb_len > 2)
-			sshdr->sense_key = (sense_buffer[2] & 0xf);
-		if (sb_len > 7) {
-			sb_len = (sb_len < (sense_buffer[7] + 8)) ?
-					 sb_len : (sense_buffer[7] + 8);
-			if (sb_len > 12)
-				sshdr->asc = sense_buffer[12];
-			if (sb_len > 13)
-				sshdr->ascq = sense_buffer[13];
-		}
-	}
-
-	return true;
-}
-EXPORT_SYMBOL(scsi_normalize_sense);
-
 bool scsi_command_normalize_sense(const struct scsi_cmnd *cmd,
 				  struct scsi_sense_hdr *sshdr)
 {
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 6efab1c455e1..f9f3f8203d42 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -280,7 +280,8 @@ static struct scsi_device *scsi_alloc_sdev(struct scsi_target *starget,
 				    sdev->host->cmd_per_lun, shost->bqt,
 				    shost->hostt->tag_alloc_policy);
 	}
-	scsi_change_queue_depth(sdev, sdev->host->cmd_per_lun);
+	scsi_change_queue_depth(sdev, sdev->host->cmd_per_lun ?
+					sdev->host->cmd_per_lun : 1);
 
 	scsi_sysfs_device_initialize(sdev);
 
@@ -1269,68 +1270,6 @@ static void scsi_sequential_lun_scan(struct scsi_target *starget,
 }
 
 /**
- * scsilun_to_int - convert a scsi_lun to an int
- * @scsilun:	struct scsi_lun to be converted.
- *
- * Description:
- *     Convert @scsilun from a struct scsi_lun to a four byte host byte-ordered
- *     integer, and return the result. The caller must check for
- *     truncation before using this function.
- *
- * Notes:
- *     For a description of the LUN format, post SCSI-3 see the SCSI
- *     Architecture Model, for SCSI-3 see the SCSI Controller Commands.
- *
- *     Given a struct scsi_lun of: d2 04 0b 03 00 00 00 00, this function
- *     returns the integer: 0x0b03d204
- *
- *     This encoding will return a standard integer LUN for LUNs smaller
- *     than 256, which typically use a single level LUN structure with
- *     addressing method 0.
- **/
-u64 scsilun_to_int(struct scsi_lun *scsilun)
-{
-	int i;
-	u64 lun;
-
-	lun = 0;
-	for (i = 0; i < sizeof(lun); i += 2)
-		lun = lun | (((u64)scsilun->scsi_lun[i] << ((i + 1) * 8)) |
-			     ((u64)scsilun->scsi_lun[i + 1] << (i * 8)));
-	return lun;
-}
-EXPORT_SYMBOL(scsilun_to_int);
-
-/**
- * int_to_scsilun - reverts an int into a scsi_lun
- * @lun:        integer to be reverted
- * @scsilun:	struct scsi_lun to be set.
- *
- * Description:
- *     Reverts the functionality of the scsilun_to_int, which packed
- *     an 8-byte lun value into an int. This routine unpacks the int
- *     back into the lun value.
- *
- * Notes:
- *     Given an integer : 0x0b03d204,  this function returns a
- *     struct scsi_lun of: d2 04 0b 03 00 00 00 00
- *
- **/
-void int_to_scsilun(u64 lun, struct scsi_lun *scsilun)
-{
-	int i;
-
-	memset(scsilun->scsi_lun, 0, sizeof(scsilun->scsi_lun));
-
-	for (i = 0; i < sizeof(lun); i += 2) {
-		scsilun->scsi_lun[i] = (lun >> 8) & 0xFF;
-		scsilun->scsi_lun[i+1] = lun & 0xFF;
-		lun = lun >> 16;
-	}
-}
-EXPORT_SYMBOL(int_to_scsilun);
-
-/**
  * scsi_report_lun_scan - Scan using SCSI REPORT LUN results
  * @starget: which target
  * @bflags: Zero or a mix of BLIST_NOLUN, BLIST_REPORTLUN2, or BLIST_NOREPORTLUN
diff --git a/drivers/scsi/scsi_transport_iscsi.c b/drivers/scsi/scsi_transport_iscsi.c
index 67d43e35693d..55647aae065c 100644
--- a/drivers/scsi/scsi_transport_iscsi.c
+++ b/drivers/scsi/scsi_transport_iscsi.c
@@ -204,6 +204,8 @@ iscsi_create_endpoint(int dd_size)
 					iscsi_match_epid);
 		if (!dev)
 			break;
+		else
+			put_device(dev);
 	}
 	if (id == ISCSI_MAX_EPID) {
 		printk(KERN_ERR "Too many connections. Max supported %u\n",
diff --git a/drivers/scsi/scsi_transport_srp.c b/drivers/scsi/scsi_transport_srp.c
index ae45bd99baed..a85292b1d09d 100644
--- a/drivers/scsi/scsi_transport_srp.c
+++ b/drivers/scsi/scsi_transport_srp.c
@@ -61,6 +61,11 @@ static inline struct Scsi_Host *rport_to_shost(struct srp_rport *r)
 	return dev_to_shost(r->dev.parent);
 }
 
+static inline struct srp_rport *shost_to_rport(struct Scsi_Host *shost)
+{
+	return transport_class_to_srp_rport(&shost->shost_gendev);
+}
+
 /**
  * srp_tmo_valid() - check timeout combination validity
  * @reconnect_delay: Reconnect delay in seconds.
@@ -396,6 +401,36 @@ static void srp_reconnect_work(struct work_struct *work)
 	}
 }
 
+/**
+ * scsi_request_fn_active() - number of kernel threads inside scsi_request_fn()
+ * @shost: SCSI host for which to count the number of scsi_request_fn() callers.
+ *
+ * To do: add support for scsi-mq in this function.
+ */
+static int scsi_request_fn_active(struct Scsi_Host *shost)
+{
+	struct scsi_device *sdev;
+	struct request_queue *q;
+	int request_fn_active = 0;
+
+	shost_for_each_device(sdev, shost) {
+		q = sdev->request_queue;
+
+		spin_lock_irq(q->queue_lock);
+		request_fn_active += q->request_fn_active;
+		spin_unlock_irq(q->queue_lock);
+	}
+
+	return request_fn_active;
+}
+
+/* Wait until ongoing shost->hostt->queuecommand() calls have finished. */
+static void srp_wait_for_queuecommand(struct Scsi_Host *shost)
+{
+	while (scsi_request_fn_active(shost))
+		msleep(20);
+}
+
 static void __rport_fail_io_fast(struct srp_rport *rport)
 {
 	struct Scsi_Host *shost = rport_to_shost(rport);
@@ -409,8 +444,10 @@ static void __rport_fail_io_fast(struct srp_rport *rport)
 
 	/* Involve the LLD if possible to terminate all I/O on the rport. */
 	i = to_srp_internal(shost->transportt);
-	if (i->f->terminate_rport_io)
+	if (i->f->terminate_rport_io) {
+		srp_wait_for_queuecommand(shost);
 		i->f->terminate_rport_io(rport);
+	}
 }
 
 /**
@@ -504,27 +541,6 @@ void srp_start_tl_fail_timers(struct srp_rport *rport)
 EXPORT_SYMBOL(srp_start_tl_fail_timers);
 
 /**
- * scsi_request_fn_active() - number of kernel threads inside scsi_request_fn()
- * @shost: SCSI host for which to count the number of scsi_request_fn() callers.
- */
-static int scsi_request_fn_active(struct Scsi_Host *shost)
-{
-	struct scsi_device *sdev;
-	struct request_queue *q;
-	int request_fn_active = 0;
-
-	shost_for_each_device(sdev, shost) {
-		q = sdev->request_queue;
-
-		spin_lock_irq(q->queue_lock);
-		request_fn_active += q->request_fn_active;
-		spin_unlock_irq(q->queue_lock);
-	}
-
-	return request_fn_active;
-}
-
-/**
  * srp_reconnect_rport() - reconnect to an SRP target port
  * @rport: SRP target port.
  *
@@ -559,8 +575,7 @@ int srp_reconnect_rport(struct srp_rport *rport)
 	if (res)
 		goto out;
 	scsi_target_block(&shost->shost_gendev);
-	while (scsi_request_fn_active(shost))
-		msleep(20);
+	srp_wait_for_queuecommand(shost);
 	res = rport->state != SRP_RPORT_LOST ? i->f->reconnect(rport) : -ENODEV;
 	pr_debug("%s (state %d): transport.reconnect() returned %d\n",
 		 dev_name(&shost->shost_gendev), rport->state, res);
@@ -618,9 +633,11 @@ static enum blk_eh_timer_return srp_timed_out(struct scsi_cmnd *scmd)
 	struct scsi_device *sdev = scmd->device;
 	struct Scsi_Host *shost = sdev->host;
 	struct srp_internal *i = to_srp_internal(shost->transportt);
+	struct srp_rport *rport = shost_to_rport(shost);
 
 	pr_debug("timeout for sdev %s\n", dev_name(&sdev->sdev_gendev));
-	return i->f->reset_timer_if_blocked && scsi_device_blocked(sdev) ?
+	return rport->fast_io_fail_tmo < 0 && rport->dev_loss_tmo < 0 &&
+		i->f->reset_timer_if_blocked && scsi_device_blocked(sdev) ?
 		BLK_EH_RESET_TIMER : BLK_EH_NOT_HANDLED;
 }
 
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 7f9d65fe4fd9..3b2fcb4fada0 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -2988,7 +2988,8 @@ static int sd_probe(struct device *dev)
 	sdkp->dev.class = &sd_disk_class;
 	dev_set_name(&sdkp->dev, "%s", dev_name(dev));
 
-	if (device_add(&sdkp->dev))
+	error = device_add(&sdkp->dev);
+	if (error)
 		goto out_free_index;
 
 	get_device(dev);
diff --git a/drivers/scsi/snic/Makefile b/drivers/scsi/snic/Makefile
new file mode 100644
index 000000000000..ef7c0dd47f40
--- /dev/null
+++ b/drivers/scsi/snic/Makefile
@@ -0,0 +1,17 @@
+obj-$(CONFIG_SCSI_SNIC) += snic.o
+
+snic-y := \
+	snic_attrs.o \
+	snic_main.o \
+	snic_res.o \
+	snic_isr.o \
+	snic_ctl.o \
+	snic_io.o \
+	snic_scsi.o \
+	snic_disc.o \
+	vnic_cq.o \
+	vnic_intr.o \
+	vnic_dev.o \
+	vnic_wq.o
+
+snic-$(CONFIG_SCSI_SNIC_DEBUG_FS) += snic_debugfs.o snic_trc.o
diff --git a/drivers/scsi/snic/cq_desc.h b/drivers/scsi/snic/cq_desc.h
new file mode 100644
index 000000000000..a5290562c1fa
--- /dev/null
+++ b/drivers/scsi/snic/cq_desc.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _CQ_DESC_H_
+#define _CQ_DESC_H_
+
+/*
+ * Completion queue descriptor types
+ */
+enum cq_desc_types {
+	CQ_DESC_TYPE_WQ_ENET = 0,
+	CQ_DESC_TYPE_DESC_COPY = 1,
+	CQ_DESC_TYPE_WQ_EXCH = 2,
+	CQ_DESC_TYPE_RQ_ENET = 3,
+	CQ_DESC_TYPE_RQ_FCP = 4,
+};
+
+/* Completion queue descriptor: 16B
+ *
+ * All completion queues have this basic layout.  The
+ * type_specific area is unique for each completion
+ * queue type.
+ */
+struct cq_desc {
+	__le16 completed_index;
+	__le16 q_number;
+	u8 type_specific[11];
+	u8 type_color;
+};
+
+#define CQ_DESC_TYPE_BITS        4
+#define CQ_DESC_TYPE_MASK        ((1 << CQ_DESC_TYPE_BITS) - 1)
+#define CQ_DESC_COLOR_MASK       1
+#define CQ_DESC_COLOR_SHIFT      7
+#define CQ_DESC_Q_NUM_BITS       10
+#define CQ_DESC_Q_NUM_MASK       ((1 << CQ_DESC_Q_NUM_BITS) - 1)
+#define CQ_DESC_COMP_NDX_BITS    12
+#define CQ_DESC_COMP_NDX_MASK    ((1 << CQ_DESC_COMP_NDX_BITS) - 1)
+
+static inline void cq_desc_dec(const struct cq_desc *desc_arg,
+	u8 *type, u8 *color, u16 *q_number, u16 *completed_index)
+{
+	const struct cq_desc *desc = desc_arg;
+	const u8 type_color = desc->type_color;
+
+	*color = (type_color >> CQ_DESC_COLOR_SHIFT) & CQ_DESC_COLOR_MASK;
+
+	/*
+	 * Make sure color bit is read from desc *before* other fields
+	 * are read from desc.  Hardware guarantees color bit is last
+	 * bit (byte) written.  Adding the rmb() prevents the compiler
+	 * and/or CPU from reordering the reads which would potentially
+	 * result in reading stale values.
+	 */
+	rmb();
+
+	*type = type_color & CQ_DESC_TYPE_MASK;
+	*q_number = le16_to_cpu(desc->q_number) & CQ_DESC_Q_NUM_MASK;
+	*completed_index = le16_to_cpu(desc->completed_index) &
+		CQ_DESC_COMP_NDX_MASK;
+}
+
+#endif /* _CQ_DESC_H_ */
diff --git a/drivers/scsi/snic/cq_enet_desc.h b/drivers/scsi/snic/cq_enet_desc.h
new file mode 100644
index 000000000000..0a1be2ed0288
--- /dev/null
+++ b/drivers/scsi/snic/cq_enet_desc.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _CQ_ENET_DESC_H_
+#define _CQ_ENET_DESC_H_
+
+#include "cq_desc.h"
+
+/* Ethernet completion queue descriptor: 16B */
+struct cq_enet_wq_desc {
+	__le16 completed_index;
+	__le16 q_number;
+	u8 reserved[11];
+	u8 type_color;
+};
+
+static inline void cq_enet_wq_desc_dec(struct cq_enet_wq_desc *desc,
+	u8 *type, u8 *color, u16 *q_number, u16 *completed_index)
+{
+	cq_desc_dec((struct cq_desc *)desc, type,
+		color, q_number, completed_index);
+}
+
+#endif /* _CQ_ENET_DESC_H_ */
diff --git a/drivers/scsi/snic/snic.h b/drivers/scsi/snic/snic.h
new file mode 100644
index 000000000000..d7f5ba6ba84c
--- /dev/null
+++ b/drivers/scsi/snic/snic.h
@@ -0,0 +1,414 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _SNIC_H_
+#define _SNIC_H_
+
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/workqueue.h>
+#include <linux/bitops.h>
+#include <linux/mempool.h>
+#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi.h>
+#include <scsi/scsi_host.h>
+
+#include "snic_disc.h"
+#include "snic_io.h"
+#include "snic_res.h"
+#include "snic_trc.h"
+#include "snic_stats.h"
+#include "vnic_dev.h"
+#include "vnic_wq.h"
+#include "vnic_cq.h"
+#include "vnic_intr.h"
+#include "vnic_stats.h"
+#include "vnic_snic.h"
+
+#define SNIC_DRV_NAME		"snic"
+#define SNIC_DRV_DESCRIPTION	"Cisco SCSI NIC Driver"
+#define SNIC_DRV_VERSION	"0.0.1.18"
+#define PFX			SNIC_DRV_NAME ":"
+#define DFX			SNIC_DRV_NAME "%d: "
+
+#define DESC_CLEAN_LOW_WATERMARK	8
+#define SNIC_UCSM_DFLT_THROTTLE_CNT_BLD 16 /* UCSM default throttle count */
+#define SNIC_MAX_IO_REQ			50 /* scsi_cmnd tag map entries */
+#define SNIC_MIN_IO_REQ			8  /* Min IO throttle count */
+#define SNIC_IO_LOCKS			64 /* IO locks: power of 2 */
+#define SNIC_DFLT_QUEUE_DEPTH		32 /* Default Queue Depth */
+#define SNIC_MAX_QUEUE_DEPTH		64 /* Max Queue Depth */
+#define SNIC_DFLT_CMD_TIMEOUT		90 /* Extended tmo for FW */
+
+/*
+ * Tag bits used for special requests.
+ */
+#define SNIC_TAG_ABORT		BIT(30)		/* Tag indicating abort */
+#define SNIC_TAG_DEV_RST	BIT(29)		/* Tag for device reset */
+#define SNIC_TAG_IOCTL_DEV_RST	BIT(28)		/* Tag for User Device Reset */
+#define SNIC_TAG_MASK		(BIT(24) - 1)	/* Mask for lookup */
+#define SNIC_NO_TAG		-1
+
+/*
+ * Command flags to identify the type of command and for other future use
+ */
+#define SNIC_NO_FLAGS			0
+#define SNIC_IO_INITIALIZED		BIT(0)
+#define SNIC_IO_ISSUED			BIT(1)
+#define SNIC_IO_DONE			BIT(2)
+#define SNIC_IO_REQ_NULL		BIT(3)
+#define SNIC_IO_ABTS_PENDING		BIT(4)
+#define SNIC_IO_ABORTED			BIT(5)
+#define SNIC_IO_ABTS_ISSUED		BIT(6)
+#define SNIC_IO_TERM_ISSUED		BIT(7)
+#define SNIC_IO_ABTS_TIMEDOUT		BIT(8)
+#define SNIC_IO_ABTS_TERM_DONE		BIT(9)
+#define SNIC_IO_ABTS_TERM_REQ_NULL	BIT(10)
+#define SNIC_IO_ABTS_TERM_TIMEDOUT	BIT(11)
+#define SNIC_IO_INTERNAL_TERM_PENDING	BIT(12)
+#define SNIC_IO_INTERNAL_TERM_ISSUED	BIT(13)
+#define SNIC_DEVICE_RESET		BIT(14)
+#define SNIC_DEV_RST_ISSUED		BIT(15)
+#define SNIC_DEV_RST_TIMEDOUT		BIT(16)
+#define SNIC_DEV_RST_ABTS_ISSUED	BIT(17)
+#define SNIC_DEV_RST_TERM_ISSUED	BIT(18)
+#define SNIC_DEV_RST_DONE		BIT(19)
+#define SNIC_DEV_RST_REQ_NULL		BIT(20)
+#define SNIC_DEV_RST_ABTS_DONE		BIT(21)
+#define SNIC_DEV_RST_TERM_DONE		BIT(22)
+#define SNIC_DEV_RST_ABTS_PENDING	BIT(23)
+#define SNIC_DEV_RST_PENDING		BIT(24)
+#define SNIC_DEV_RST_NOTSUP		BIT(25)
+#define SNIC_SCSI_CLEANUP		BIT(26)
+#define SNIC_HOST_RESET_ISSUED		BIT(27)
+
+#define SNIC_ABTS_TIMEOUT		30000		/* msec */
+#define SNIC_LUN_RESET_TIMEOUT		30000		/* msec */
+#define SNIC_HOST_RESET_TIMEOUT		30000		/* msec */
+
+
+/*
+ * These are protected by the hashed req_lock.
+ */
+#define CMD_SP(Cmnd)		\
+	(((struct snic_internal_io_state *)scsi_cmd_priv(Cmnd))->rqi)
+#define CMD_STATE(Cmnd)		\
+	(((struct snic_internal_io_state *)scsi_cmd_priv(Cmnd))->state)
+#define CMD_ABTS_STATUS(Cmnd)	\
+	(((struct snic_internal_io_state *)scsi_cmd_priv(Cmnd))->abts_status)
+#define CMD_LR_STATUS(Cmnd)	\
+	(((struct snic_internal_io_state *)scsi_cmd_priv(Cmnd))->lr_status)
+#define CMD_FLAGS(Cmnd)	\
+	(((struct snic_internal_io_state *)scsi_cmd_priv(Cmnd))->flags)
+
+#define SNIC_INVALID_CODE 0x100	/* Hdr Status val unused by firmware */
+
+#define SNIC_MAX_TARGET			256
+#define SNIC_FLAGS_NONE			(0)
+
+/* snic module params */
+extern unsigned int snic_max_qdepth;
+
+/* snic debugging */
+extern unsigned int snic_log_level;
+
+#define SNIC_MAIN_LOGGING	0x1
+#define SNIC_SCSI_LOGGING	0x2
+#define SNIC_ISR_LOGGING	0x8
+#define SNIC_DESC_LOGGING	0x10
+
+#define SNIC_CHECK_LOGGING(LEVEL, CMD)		\
+do {						\
+	if (unlikely(snic_log_level & LEVEL))	\
+		do {				\
+			CMD;			\
+		} while (0);			\
+} while (0)
+
+#define SNIC_MAIN_DBG(host, fmt, args...)	\
+	SNIC_CHECK_LOGGING(SNIC_MAIN_LOGGING,		\
+		shost_printk(KERN_INFO, host, fmt, ## args);)
+
+#define SNIC_SCSI_DBG(host, fmt, args...)	\
+	SNIC_CHECK_LOGGING(SNIC_SCSI_LOGGING,		\
+		shost_printk(KERN_INFO, host, fmt, ##args);)
+
+#define SNIC_DISC_DBG(host, fmt, args...)	\
+	SNIC_CHECK_LOGGING(SNIC_SCSI_LOGGING,		\
+		shost_printk(KERN_INFO, host, fmt, ##args);)
+
+#define SNIC_ISR_DBG(host, fmt, args...)	\
+	SNIC_CHECK_LOGGING(SNIC_ISR_LOGGING,		\
+		shost_printk(KERN_INFO, host, fmt, ##args);)
+
+#define SNIC_HOST_ERR(host, fmt, args...)		\
+	shost_printk(KERN_ERR, host, fmt, ##args)
+
+#define SNIC_HOST_INFO(host, fmt, args...)		\
+	shost_printk(KERN_INFO, host, fmt, ##args)
+
+#define SNIC_INFO(fmt, args...)				\
+	pr_info(PFX fmt, ## args)
+
+#define SNIC_DBG(fmt, args...)				\
+	pr_info(PFX fmt, ## args)
+
+#define SNIC_ERR(fmt, args...)				\
+	pr_err(PFX fmt, ## args)
+
+#ifdef DEBUG
+#define SNIC_BUG_ON(EXPR) \
+	({ \
+		if (EXPR) { \
+			SNIC_ERR("SNIC BUG(%s)\n", #EXPR); \
+			BUG_ON(EXPR); \
+		} \
+	})
+#else
+#define SNIC_BUG_ON(EXPR) \
+	({ \
+		if (EXPR) { \
+			SNIC_ERR("SNIC BUG(%s) at %s : %d\n", \
+				 #EXPR, __func__, __LINE__); \
+			WARN_ON_ONCE(EXPR); \
+		} \
+	})
+#endif
+
+/* Soft assert */
+#define SNIC_ASSERT_NOT_IMPL(EXPR) \
+	({ \
+		if (EXPR) {\
+			SNIC_INFO("Functionality not impl'ed at %s:%d\n", \
+				  __func__, __LINE__); \
+			WARN_ON_ONCE(EXPR); \
+		} \
+	 })
+
+
+extern const char *snic_state_str[];
+
+enum snic_intx_intr_index {
+	SNIC_INTX_WQ_RQ_COPYWQ,
+	SNIC_INTX_ERR,
+	SNIC_INTX_NOTIFY,
+	SNIC_INTX_INTR_MAX,
+};
+
+enum snic_msix_intr_index {
+	SNIC_MSIX_WQ,
+	SNIC_MSIX_IO_CMPL,
+	SNIC_MSIX_ERR_NOTIFY,
+	SNIC_MSIX_INTR_MAX,
+};
+
+struct snic_msix_entry {
+	int requested;
+	char devname[IFNAMSIZ];
+	irqreturn_t (*isr)(int, void *);
+	void *devid;
+};
+
+enum snic_state {
+	SNIC_INIT = 0,
+	SNIC_ERROR,
+	SNIC_ONLINE,
+	SNIC_OFFLINE,
+	SNIC_FWRESET,
+};
+
+#define SNIC_WQ_MAX		1
+#define SNIC_CQ_IO_CMPL_MAX	1
+#define SNIC_CQ_MAX		(SNIC_WQ_MAX + SNIC_CQ_IO_CMPL_MAX)
+
+/* firmware version information */
+struct snic_fw_info {
+	u32	fw_ver;
+	u32	hid;			/* u16 hid | u16 vnic id */
+	u32	max_concur_ios;		/* max concurrent ios */
+	u32	max_sgs_per_cmd;	/* max sgls per IO */
+	u32	max_io_sz;		/* max io size supported */
+	u32	hba_cap;		/* hba capabilities */
+	u32	max_tgts;		/* max tgts supported */
+	u16	io_tmo;			/* FW Extended timeout */
+	struct completion *wait;	/* protected by snic lock*/
+};
+
+/*
+ * snic_work item : defined to process asynchronous events
+ */
+struct snic_work {
+	struct work_struct work;
+	u16	ev_id;
+	u64	*ev_data;
+};
+
+/*
+ * snic structure to represent SCSI vNIC
+ */
+struct snic {
+	/* snic specific members */
+	struct list_head list;
+	char name[IFNAMSIZ];
+	atomic_t state;
+	spinlock_t snic_lock;
+	struct completion *remove_wait;
+	bool in_remove;
+	bool stop_link_events;		/* stop processing link events */
+
+	/* discovery related */
+	struct snic_disc disc;
+
+	/* Scsi Host info */
+	struct Scsi_Host *shost;
+
+	/* vnic related structures */
+	struct vnic_dev_bar bar0;
+
+	struct vnic_stats *stats;
+	unsigned long stats_time;
+	unsigned long stats_reset_time;
+
+	struct vnic_dev *vdev;
+
+	/* hw resource info */
+	unsigned int wq_count;
+	unsigned int cq_count;
+	unsigned int intr_count;
+	unsigned int err_intr_offset;
+
+	int link_status; /* retrieved from svnic_dev_link_status() */
+	u32 link_down_cnt;
+
+	/* pci related */
+	struct pci_dev *pdev;
+	struct msix_entry msix_entry[SNIC_MSIX_INTR_MAX];
+	struct snic_msix_entry msix[SNIC_MSIX_INTR_MAX];
+
+	/* io related info */
+	mempool_t *req_pool[SNIC_REQ_MAX_CACHES]; /* (??) */
+	____cacheline_aligned spinlock_t io_req_lock[SNIC_IO_LOCKS];
+
+	/* Maintain snic specific commands, cmds with no tag in spl_cmd_list */
+	____cacheline_aligned spinlock_t spl_cmd_lock;
+	struct list_head spl_cmd_list;
+
+	unsigned int max_tag_id;
+	atomic_t ios_inflight;		/* io in flight counter */
+
+	struct vnic_snic_config config;
+
+	struct work_struct link_work;
+
+	/* firmware information */
+	struct snic_fw_info fwinfo;
+
+	/* Work for processing Target related work */
+	struct work_struct tgt_work;
+
+	/* Work for processing Discovery */
+	struct work_struct disc_work;
+
+	/* stats related */
+	unsigned int reset_stats;
+	atomic64_t io_cmpl_skip;
+	struct snic_stats s_stats;	/* Per SNIC driver stats */
+
+	/* platform specific */
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+	struct dentry *stats_host;	/* Per snic debugfs root */
+	struct dentry *stats_file;	/* Per snic debugfs file */
+	struct dentry *reset_stats_file;/* Per snic reset stats file */
+#endif
+
+	/* completion queue cache line section */
+	____cacheline_aligned struct vnic_cq cq[SNIC_CQ_MAX];
+
+	/* work queue cache line section */
+	____cacheline_aligned struct vnic_wq wq[SNIC_WQ_MAX];
+	spinlock_t wq_lock[SNIC_WQ_MAX];
+
+	/* interrupt resource cache line section */
+	____cacheline_aligned struct vnic_intr intr[SNIC_MSIX_INTR_MAX];
+}; /* end of snic structure */
+
+/*
+ * SNIC Driver's Global Data
+ */
+struct snic_global {
+	struct list_head snic_list;
+	spinlock_t snic_list_lock;
+
+	struct kmem_cache *req_cache[SNIC_REQ_MAX_CACHES];
+
+	struct workqueue_struct *event_q;
+
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+	/* debugfs related global data */
+	struct dentry *trc_root;
+	struct dentry *stats_root;
+
+	struct snic_trc trc ____cacheline_aligned;
+#endif
+};
+
+extern struct snic_global *snic_glob;
+
+int snic_glob_init(void);
+void snic_glob_cleanup(void);
+
+extern struct workqueue_struct *snic_event_queue;
+extern struct device_attribute *snic_attrs[];
+
+int snic_queuecommand(struct Scsi_Host *, struct scsi_cmnd *);
+int snic_abort_cmd(struct scsi_cmnd *);
+int snic_device_reset(struct scsi_cmnd *);
+int snic_host_reset(struct scsi_cmnd *);
+int snic_reset(struct Scsi_Host *, struct scsi_cmnd *);
+void snic_shutdown_scsi_cleanup(struct snic *);
+
+
+int snic_request_intr(struct snic *);
+void snic_free_intr(struct snic *);
+int snic_set_intr_mode(struct snic *);
+void snic_clear_intr_mode(struct snic *);
+
+int snic_fwcq_cmpl_handler(struct snic *, int);
+int snic_wq_cmpl_handler(struct snic *, int);
+void snic_free_wq_buf(struct vnic_wq *, struct vnic_wq_buf *);
+
+
+void snic_log_q_error(struct snic *);
+void snic_handle_link_event(struct snic *);
+void snic_handle_link(struct work_struct *);
+
+int snic_queue_exch_ver_req(struct snic *);
+int snic_io_exch_ver_cmpl_handler(struct snic *, struct snic_fw_req *);
+
+int snic_queue_wq_desc(struct snic *, void *os_buf, u16 len);
+
+void snic_handle_untagged_req(struct snic *, struct snic_req_info *);
+void snic_release_untagged_req(struct snic *, struct snic_req_info *);
+void snic_free_all_untagged_reqs(struct snic *);
+int snic_get_conf(struct snic *);
+void snic_set_state(struct snic *, enum snic_state);
+int snic_get_state(struct snic *);
+const char *snic_state_to_str(unsigned int);
+void snic_hex_dump(char *, char *, int);
+void snic_print_desc(const char *fn, char *os_buf, int len);
+const char *show_opcode_name(int val);
+#endif /* _SNIC_H */
diff --git a/drivers/scsi/snic/snic_attrs.c b/drivers/scsi/snic/snic_attrs.c
new file mode 100644
index 000000000000..32d5d556b6f8
--- /dev/null
+++ b/drivers/scsi/snic/snic_attrs.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/string.h>
+#include <linux/device.h>
+
+#include "snic.h"
+
+static ssize_t
+snic_show_sym_name(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct snic *snic = shost_priv(class_to_shost(dev));
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", snic->name);
+}
+
+static ssize_t
+snic_show_state(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct snic *snic = shost_priv(class_to_shost(dev));
+
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			snic_state_str[snic_get_state(snic)]);
+}
+
+static ssize_t
+snic_show_drv_version(struct device *dev,
+		      struct device_attribute *attr,
+		      char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", SNIC_DRV_VERSION);
+}
+
+static ssize_t
+snic_show_link_state(struct device *dev,
+		     struct device_attribute *attr,
+		     char *buf)
+{
+	struct snic *snic = shost_priv(class_to_shost(dev));
+
+	if (snic->config.xpt_type == SNIC_DAS)
+		snic->link_status = svnic_dev_link_status(snic->vdev);
+
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			(snic->link_status) ? "Link Up" : "Link Down");
+}
+
+static DEVICE_ATTR(snic_sym_name, S_IRUGO, snic_show_sym_name, NULL);
+static DEVICE_ATTR(snic_state, S_IRUGO, snic_show_state, NULL);
+static DEVICE_ATTR(drv_version, S_IRUGO, snic_show_drv_version, NULL);
+static DEVICE_ATTR(link_state, S_IRUGO, snic_show_link_state, NULL);
+
+struct device_attribute *snic_attrs[] = {
+	&dev_attr_snic_sym_name,
+	&dev_attr_snic_state,
+	&dev_attr_drv_version,
+	&dev_attr_link_state,
+	NULL,
+};
diff --git a/drivers/scsi/snic/snic_ctl.c b/drivers/scsi/snic/snic_ctl.c
new file mode 100644
index 000000000000..aebe75320ed3
--- /dev/null
+++ b/drivers/scsi/snic/snic_ctl.c
@@ -0,0 +1,279 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/errno.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/mempool.h>
+#include <scsi/scsi_tcq.h>
+#include <linux/ctype.h>
+
+#include "snic_io.h"
+#include "snic.h"
+#include "cq_enet_desc.h"
+#include "snic_fwint.h"
+
+/*
+ * snic_handle_link : Handles link flaps.
+ */
+void
+snic_handle_link(struct work_struct *work)
+{
+	struct snic *snic = container_of(work, struct snic, link_work);
+
+	if (snic->config.xpt_type != SNIC_DAS) {
+		SNIC_HOST_INFO(snic->shost, "Link Event Received.\n");
+		SNIC_ASSERT_NOT_IMPL(1);
+
+		return;
+	}
+
+	snic->link_status = svnic_dev_link_status(snic->vdev);
+	snic->link_down_cnt = svnic_dev_link_down_cnt(snic->vdev);
+	SNIC_HOST_INFO(snic->shost, "Link Event: Link %s.\n",
+		       ((snic->link_status) ? "Up" : "Down"));
+}
+
+
+/*
+ * snic_ver_enc : Encodes version str to int
+ * version string is similar to netmask string
+ */
+static int
+snic_ver_enc(const char *s)
+{
+	int v[4] = {0};
+	int  i = 0, x = 0;
+	char c;
+	const char *p = s;
+
+	/* validate version string */
+	if ((strlen(s) > 15) || (strlen(s) < 7))
+		goto end;
+
+	while ((c = *p++)) {
+		if (c == '.') {
+			i++;
+			continue;
+		}
+
+		if (i > 4 || !isdigit(c))
+			goto end;
+
+		v[i] = v[i] * 10 + (c - '0');
+	}
+
+	/* validate sub version numbers */
+	for (i = 3; i >= 0; i--)
+		if (v[i] > 0xff)
+			goto end;
+
+	x |= (v[0] << 24) | v[1] << 16 | v[2] << 8 | v[3];
+
+end:
+	if (x == 0) {
+		SNIC_ERR("Invalid version string [%s].\n", s);
+
+		return -1;
+	}
+
+	return x;
+} /* end of snic_ver_enc */
+
+/*
+ * snic_qeueue_exch_ver_req :
+ *
+ * Queues Exchange Version Request, to communicate host information
+ * in return, it gets firmware version details
+ */
+int
+snic_queue_exch_ver_req(struct snic *snic)
+{
+	struct snic_req_info *rqi = NULL;
+	struct snic_host_req *req = NULL;
+	u32 ver = 0;
+	int ret = 0;
+
+	SNIC_HOST_INFO(snic->shost, "Exch Ver Req Preparing...\n");
+
+	rqi = snic_req_init(snic, 0);
+	if (!rqi) {
+		SNIC_HOST_ERR(snic->shost,
+			      "Queuing Exch Ver Req failed, err = %d\n",
+			      ret);
+
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	req = rqi_to_req(rqi);
+
+	/* Initialize snic_host_req */
+	snic_io_hdr_enc(&req->hdr, SNIC_REQ_EXCH_VER, 0, SCSI_NO_TAG,
+			snic->config.hid, 0, (ulong)rqi);
+	ver = snic_ver_enc(SNIC_DRV_VERSION);
+	req->u.exch_ver.drvr_ver = cpu_to_le32(ver);
+	req->u.exch_ver.os_type = cpu_to_le32(SNIC_OS_LINUX);
+
+	snic_handle_untagged_req(snic, rqi);
+
+	ret = snic_queue_wq_desc(snic, req, sizeof(*req));
+	if (ret) {
+		snic_release_untagged_req(snic, rqi);
+		SNIC_HOST_ERR(snic->shost,
+			      "Queuing Exch Ver Req failed, err = %d\n",
+			      ret);
+		goto error;
+	}
+
+	SNIC_HOST_INFO(snic->shost, "Exch Ver Req is issued. ret = %d\n", ret);
+
+error:
+	return ret;
+} /* end of snic_queue_exch_ver_req */
+
+/*
+ * snic_io_exch_ver_cmpl_handler
+ */
+int
+snic_io_exch_ver_cmpl_handler(struct snic *snic, struct snic_fw_req *fwreq)
+{
+	struct snic_req_info *rqi = NULL;
+	struct snic_exch_ver_rsp *exv_cmpl = &fwreq->u.exch_ver_cmpl;
+	u8 typ, hdr_stat;
+	u32 cmnd_id, hid, max_sgs;
+	ulong ctx = 0;
+	unsigned long flags;
+	int ret = 0;
+
+	SNIC_HOST_INFO(snic->shost, "Exch Ver Compl Received.\n");
+	snic_io_hdr_dec(&fwreq->hdr, &typ, &hdr_stat, &cmnd_id, &hid, &ctx);
+	SNIC_BUG_ON(snic->config.hid != hid);
+	rqi = (struct snic_req_info *) ctx;
+
+	if (hdr_stat) {
+		SNIC_HOST_ERR(snic->shost,
+			      "Exch Ver Completed w/ err status %d\n",
+			      hdr_stat);
+
+		goto exch_cmpl_end;
+	}
+
+	spin_lock_irqsave(&snic->snic_lock, flags);
+	snic->fwinfo.fw_ver = le32_to_cpu(exv_cmpl->version);
+	snic->fwinfo.hid = le32_to_cpu(exv_cmpl->hid);
+	snic->fwinfo.max_concur_ios = le32_to_cpu(exv_cmpl->max_concur_ios);
+	snic->fwinfo.max_sgs_per_cmd = le32_to_cpu(exv_cmpl->max_sgs_per_cmd);
+	snic->fwinfo.max_io_sz = le32_to_cpu(exv_cmpl->max_io_sz);
+	snic->fwinfo.max_tgts = le32_to_cpu(exv_cmpl->max_tgts);
+	snic->fwinfo.io_tmo = le16_to_cpu(exv_cmpl->io_timeout);
+
+	SNIC_HOST_INFO(snic->shost,
+		       "vers %u hid %u max_concur_ios %u max_sgs_per_cmd %u max_io_sz %u max_tgts %u fw tmo %u\n",
+		       snic->fwinfo.fw_ver,
+		       snic->fwinfo.hid,
+		       snic->fwinfo.max_concur_ios,
+		       snic->fwinfo.max_sgs_per_cmd,
+		       snic->fwinfo.max_io_sz,
+		       snic->fwinfo.max_tgts,
+		       snic->fwinfo.io_tmo);
+
+	SNIC_HOST_INFO(snic->shost,
+		       "HBA Capabilities = 0x%x\n",
+		       le32_to_cpu(exv_cmpl->hba_cap));
+
+	/* Updating SGList size */
+	max_sgs = snic->fwinfo.max_sgs_per_cmd;
+	if (max_sgs && max_sgs < SNIC_MAX_SG_DESC_CNT) {
+		snic->shost->sg_tablesize = max_sgs;
+		SNIC_HOST_INFO(snic->shost, "Max SGs set to %d\n",
+			       snic->shost->sg_tablesize);
+	} else if (max_sgs > snic->shost->sg_tablesize) {
+		SNIC_HOST_INFO(snic->shost,
+			       "Target type %d Supports Larger Max SGList %d than driver's Max SG List %d.\n",
+			       snic->config.xpt_type, max_sgs,
+			       snic->shost->sg_tablesize);
+	}
+
+	if (snic->shost->can_queue > snic->fwinfo.max_concur_ios)
+		snic->shost->can_queue = snic->fwinfo.max_concur_ios;
+
+	snic->shost->max_sectors = snic->fwinfo.max_io_sz >> 9;
+	if (snic->fwinfo.wait)
+		complete(snic->fwinfo.wait);
+
+	spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+exch_cmpl_end:
+	snic_release_untagged_req(snic, rqi);
+
+	SNIC_HOST_INFO(snic->shost, "Exch_cmpl Done, hdr_stat %d.\n", hdr_stat);
+
+	return ret;
+} /* end of snic_io_exch_ver_cmpl_handler */
+
+/*
+ * snic_get_conf
+ *
+ * Synchronous call, and Retrieves snic params.
+ */
+int
+snic_get_conf(struct snic *snic)
+{
+	DECLARE_COMPLETION_ONSTACK(wait);
+	unsigned long flags;
+	int ret;
+	int nr_retries = 3;
+
+	SNIC_HOST_INFO(snic->shost, "Retrieving snic params.\n");
+	spin_lock_irqsave(&snic->snic_lock, flags);
+	memset(&snic->fwinfo, 0, sizeof(snic->fwinfo));
+	snic->fwinfo.wait = &wait;
+	spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+	/* Additional delay to handle HW Resource initialization. */
+	msleep(50);
+
+	/*
+	 * Exch ver req can be ignored by FW, if HW Resource initialization
+	 * is in progress, Hence retry.
+	 */
+	do {
+		ret = snic_queue_exch_ver_req(snic);
+		if (ret)
+			return ret;
+
+		wait_for_completion_timeout(&wait, msecs_to_jiffies(2000));
+		spin_lock_irqsave(&snic->snic_lock, flags);
+		ret = (snic->fwinfo.fw_ver != 0) ? 0 : -ETIMEDOUT;
+		if (ret)
+			SNIC_HOST_ERR(snic->shost,
+				      "Failed to retrieve snic params,\n");
+
+		/* Unset fwinfo.wait, on success or on last retry */
+		if (ret == 0 || nr_retries == 1)
+			snic->fwinfo.wait = NULL;
+
+		spin_unlock_irqrestore(&snic->snic_lock, flags);
+	} while (ret && --nr_retries);
+
+	return ret;
+} /* end of snic_get_info */
diff --git a/drivers/scsi/snic/snic_debugfs.c b/drivers/scsi/snic/snic_debugfs.c
new file mode 100644
index 000000000000..1686f0196251
--- /dev/null
+++ b/drivers/scsi/snic/snic_debugfs.c
@@ -0,0 +1,560 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/module.h>
+#include <linux/errno.h>
+#include <linux/debugfs.h>
+
+#include "snic.h"
+
+/*
+ * snic_debugfs_init - Initialize debugfs for snic debug logging
+ *
+ * Description:
+ * When Debugfs is configured this routine sets up fnic debugfs
+ * filesystem. If not already created. this routine will crate the
+ * fnic directory and statistics directory for trace buffer and
+ * stats logging
+ */
+
+int
+snic_debugfs_init(void)
+{
+	int rc = -1;
+	struct dentry *de = NULL;
+
+	de = debugfs_create_dir("snic", NULL);
+	if (!de) {
+		SNIC_DBG("Cannot create debugfs root\n");
+
+		return rc;
+	}
+	snic_glob->trc_root = de;
+
+	de = debugfs_create_dir("statistics", snic_glob->trc_root);
+	if (!de) {
+		SNIC_DBG("Cannot create Statistics directory\n");
+
+		return rc;
+	}
+	snic_glob->stats_root = de;
+
+	rc = 0;
+
+	return rc;
+} /* end of snic_debugfs_init */
+
+/*
+ * snic_debugfs_term - Tear down debugfs intrastructure
+ *
+ * Description:
+ * When Debufs is configured this routine removes debugfs file system
+ * elements that are specific to snic
+ */
+void
+snic_debugfs_term(void)
+{
+	debugfs_remove(snic_glob->stats_root);
+	snic_glob->stats_root = NULL;
+
+	debugfs_remove(snic_glob->trc_root);
+	snic_glob->trc_root = NULL;
+}
+
+/*
+ * snic_reset_stats_open - Open the reset_stats file
+ */
+static int
+snic_reset_stats_open(struct inode *inode, struct file *filp)
+{
+	SNIC_BUG_ON(!inode->i_private);
+	filp->private_data = inode->i_private;
+
+	return 0;
+}
+
+/*
+ * snic_reset_stats_read - Read a reset_stats debugfs file
+ * @filp: The file pointer to read from.
+ * @ubuf: The buffer tocopy the data to.
+ * @cnt: The number of bytes to read.
+ * @ppos: The position in the file to start reading frm.
+ *
+ * Description:
+ * This routine reads value of variable reset_stats
+ * and stores into local @buf. It will start reading file @ppos and
+ * copy up to @cnt of data to @ubuf from @buf.
+ *
+ * Returns:
+ * This function returns the amount of data that was read.
+ */
+static ssize_t
+snic_reset_stats_read(struct file *filp,
+		      char __user *ubuf,
+		      size_t cnt,
+		      loff_t *ppos)
+{
+	struct snic *snic = (struct snic *) filp->private_data;
+	char buf[64];
+	int len;
+
+	len = sprintf(buf, "%u\n", snic->reset_stats);
+
+	return simple_read_from_buffer(ubuf, cnt, ppos, buf, len);
+}
+
+/*
+ * snic_reset_stats_write - Write to reset_stats debugfs file
+ * @filp: The file pointer to write from
+ * @ubuf: The buffer to copy the data from.
+ * @cnt: The number of bytes to write.
+ * @ppos: The position in the file to start writing to.
+ *
+ * Description:
+ * This routine writes data from user buffer @ubuf to buffer @buf and
+ * resets cumulative stats of snic.
+ *
+ * Returns:
+ * This function returns the amount of data that was written.
+ */
+static ssize_t
+snic_reset_stats_write(struct file *filp,
+		       const char __user *ubuf,
+		       size_t cnt,
+		       loff_t *ppos)
+{
+	struct snic *snic = (struct snic *) filp->private_data;
+	struct snic_stats *stats = &snic->s_stats;
+	u64 *io_stats_p = (u64 *) &stats->io;
+	u64 *fw_stats_p = (u64 *) &stats->fw;
+	char buf[64];
+	unsigned long val;
+	int ret;
+
+	if (cnt >= sizeof(buf))
+		return -EINVAL;
+
+	if (copy_from_user(&buf, ubuf, cnt))
+		return -EFAULT;
+
+	buf[cnt] = '\0';
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret < 0)
+		return ret;
+
+	snic->reset_stats = val;
+
+	if (snic->reset_stats) {
+		/* Skip variable is used to avoid descrepancies to Num IOs
+		 * and IO Completions stats. Skip incrementing No IO Compls
+		 * for pending active IOs after reset_stats
+		 */
+		atomic64_set(&snic->io_cmpl_skip,
+			     atomic64_read(&stats->io.active));
+		memset(&stats->abts, 0, sizeof(struct snic_abort_stats));
+		memset(&stats->reset, 0, sizeof(struct snic_reset_stats));
+		memset(&stats->misc, 0, sizeof(struct snic_misc_stats));
+		memset(io_stats_p+1,
+			0,
+			sizeof(struct snic_io_stats) - sizeof(u64));
+		memset(fw_stats_p+1,
+			0,
+			sizeof(struct snic_fw_stats) - sizeof(u64));
+	}
+
+	(*ppos)++;
+
+	SNIC_HOST_INFO(snic->shost, "Reset Op: Driver statistics.\n");
+
+	return cnt;
+}
+
+static int
+snic_reset_stats_release(struct inode *inode, struct file *filp)
+{
+	filp->private_data = NULL;
+
+	return 0;
+}
+
+/*
+ * snic_stats_show - Formats and prints per host specific driver stats.
+ */
+static int
+snic_stats_show(struct seq_file *sfp, void *data)
+{
+	struct snic *snic = (struct snic *) sfp->private;
+	struct snic_stats *stats = &snic->s_stats;
+	struct timespec last_isr_tms, last_ack_tms;
+	u64 maxio_tm;
+	int i;
+
+	/* Dump IO Stats */
+	seq_printf(sfp,
+		   "------------------------------------------\n"
+		   "\t\t IO Statistics\n"
+		   "------------------------------------------\n");
+
+	maxio_tm = (u64) atomic64_read(&stats->io.max_time);
+	seq_printf(sfp,
+		   "Active IOs                  : %lld\n"
+		   "Max Active IOs              : %lld\n"
+		   "Total IOs                   : %lld\n"
+		   "IOs Completed               : %lld\n"
+		   "IOs Failed                  : %lld\n"
+		   "IOs Not Found               : %lld\n"
+		   "Memory Alloc Failures       : %lld\n"
+		   "REQs Null                   : %lld\n"
+		   "SCSI Cmd Pointers Null      : %lld\n"
+		   "Max SGL for any IO          : %lld\n"
+		   "Max IO Size                 : %lld Sectors\n"
+		   "Max Queuing Time            : %lld\n"
+		   "Max Completion Time         : %lld\n"
+		   "Max IO Process Time(FW)     : %lld (%u msec)\n",
+		   (u64) atomic64_read(&stats->io.active),
+		   (u64) atomic64_read(&stats->io.max_active),
+		   (u64) atomic64_read(&stats->io.num_ios),
+		   (u64) atomic64_read(&stats->io.compl),
+		   (u64) atomic64_read(&stats->io.fail),
+		   (u64) atomic64_read(&stats->io.io_not_found),
+		   (u64) atomic64_read(&stats->io.alloc_fail),
+		   (u64) atomic64_read(&stats->io.req_null),
+		   (u64) atomic64_read(&stats->io.sc_null),
+		   (u64) atomic64_read(&stats->io.max_sgl),
+		   (u64) atomic64_read(&stats->io.max_io_sz),
+		   (u64) atomic64_read(&stats->io.max_qtime),
+		   (u64) atomic64_read(&stats->io.max_cmpl_time),
+		   maxio_tm,
+		   jiffies_to_msecs(maxio_tm));
+
+	seq_puts(sfp, "\nSGL Counters\n");
+
+	for (i = 0; i < SNIC_MAX_SG_DESC_CNT; i++) {
+		seq_printf(sfp,
+			   "%10lld ",
+			   (u64) atomic64_read(&stats->io.sgl_cnt[i]));
+
+		if ((i + 1) % 8 == 0)
+			seq_puts(sfp, "\n");
+	}
+
+	/* Dump Abort Stats */
+	seq_printf(sfp,
+		   "\n-------------------------------------------\n"
+		   "\t\t Abort Statistics\n"
+		   "---------------------------------------------\n");
+
+	seq_printf(sfp,
+		   "Aborts                      : %lld\n"
+		   "Aborts Fail                 : %lld\n"
+		   "Aborts Driver Timeout       : %lld\n"
+		   "Abort FW Timeout            : %lld\n"
+		   "Abort IO NOT Found          : %lld\n",
+		   (u64) atomic64_read(&stats->abts.num),
+		   (u64) atomic64_read(&stats->abts.fail),
+		   (u64) atomic64_read(&stats->abts.drv_tmo),
+		   (u64) atomic64_read(&stats->abts.fw_tmo),
+		   (u64) atomic64_read(&stats->abts.io_not_found));
+
+	/* Dump Reset Stats */
+	seq_printf(sfp,
+		   "\n-------------------------------------------\n"
+		   "\t\t Reset Statistics\n"
+		   "---------------------------------------------\n");
+
+	seq_printf(sfp,
+		   "HBA Resets                  : %lld\n"
+		   "HBA Reset Cmpls             : %lld\n"
+		   "HBA Reset Fail              : %lld\n",
+		   (u64) atomic64_read(&stats->reset.hba_resets),
+		   (u64) atomic64_read(&stats->reset.hba_reset_cmpl),
+		   (u64) atomic64_read(&stats->reset.hba_reset_fail));
+
+	/* Dump Firmware Stats */
+	seq_printf(sfp,
+		   "\n-------------------------------------------\n"
+		   "\t\t Firmware Statistics\n"
+		   "---------------------------------------------\n");
+
+	seq_printf(sfp,
+		"Active FW Requests             : %lld\n"
+		"Max FW Requests                : %lld\n"
+		"FW Out Of Resource Errs        : %lld\n"
+		"FW IO Errors                   : %lld\n"
+		"FW SCSI Errors                 : %lld\n",
+		(u64) atomic64_read(&stats->fw.actv_reqs),
+		(u64) atomic64_read(&stats->fw.max_actv_reqs),
+		(u64) atomic64_read(&stats->fw.out_of_res),
+		(u64) atomic64_read(&stats->fw.io_errs),
+		(u64) atomic64_read(&stats->fw.scsi_errs));
+
+
+	/* Dump Miscellenous Stats */
+	seq_printf(sfp,
+		   "\n---------------------------------------------\n"
+		   "\t\t Other Statistics\n"
+		   "\n---------------------------------------------\n");
+
+	jiffies_to_timespec(stats->misc.last_isr_time, &last_isr_tms);
+	jiffies_to_timespec(stats->misc.last_ack_time, &last_ack_tms);
+
+	seq_printf(sfp,
+		   "Last ISR Time               : %llu (%8lu.%8lu)\n"
+		   "Last Ack Time               : %llu (%8lu.%8lu)\n"
+		   "ISRs                        : %llu\n"
+		   "Max CQ Entries              : %lld\n"
+		   "Data Count Mismatch         : %lld\n"
+		   "IOs w/ Timeout Status       : %lld\n"
+		   "IOs w/ Aborted Status       : %lld\n"
+		   "IOs w/ SGL Invalid Stat     : %lld\n"
+		   "WQ Desc Alloc Fail          : %lld\n"
+		   "Queue Full                  : %lld\n"
+		   "Target Not Ready            : %lld\n",
+		   (u64) stats->misc.last_isr_time,
+		   last_isr_tms.tv_sec, last_isr_tms.tv_nsec,
+		   (u64)stats->misc.last_ack_time,
+		   last_ack_tms.tv_sec, last_ack_tms.tv_nsec,
+		   (u64) atomic64_read(&stats->misc.isr_cnt),
+		   (u64) atomic64_read(&stats->misc.max_cq_ents),
+		   (u64) atomic64_read(&stats->misc.data_cnt_mismat),
+		   (u64) atomic64_read(&stats->misc.io_tmo),
+		   (u64) atomic64_read(&stats->misc.io_aborted),
+		   (u64) atomic64_read(&stats->misc.sgl_inval),
+		   (u64) atomic64_read(&stats->misc.wq_alloc_fail),
+		   (u64) atomic64_read(&stats->misc.qfull),
+		   (u64) atomic64_read(&stats->misc.tgt_not_rdy));
+
+	return 0;
+}
+
+/*
+ * snic_stats_open - Open the stats file for specific host
+ *
+ * Description:
+ * This routine opens a debugfs file stats of specific host
+ */
+static int
+snic_stats_open(struct inode *inode, struct file *filp)
+{
+	return single_open(filp, snic_stats_show, inode->i_private);
+}
+
+static const struct file_operations snic_stats_fops = {
+	.owner	= THIS_MODULE,
+	.open	= snic_stats_open,
+	.read	= seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static const struct file_operations snic_reset_stats_fops = {
+	.owner = THIS_MODULE,
+	.open = snic_reset_stats_open,
+	.read = snic_reset_stats_read,
+	.write = snic_reset_stats_write,
+	.release = snic_reset_stats_release,
+};
+
+/*
+ * snic_stats_init - Initialize stats struct and create stats file
+ * per snic
+ *
+ * Description:
+ * When debugfs is cofigured this routine sets up the stats file per snic
+ * It will create file stats and reset_stats under statistics/host# directory
+ * to log per snic stats
+ */
+int
+snic_stats_debugfs_init(struct snic *snic)
+{
+	int rc = -1;
+	char name[16];
+	struct dentry *de = NULL;
+
+	snprintf(name, sizeof(name), "host%d", snic->shost->host_no);
+	if (!snic_glob->stats_root) {
+		SNIC_DBG("snic_stats root doesn't exist\n");
+
+		return rc;
+	}
+
+	de = debugfs_create_dir(name, snic_glob->stats_root);
+	if (!de) {
+		SNIC_DBG("Cannot create host directory\n");
+
+		return rc;
+	}
+	snic->stats_host = de;
+
+	de = debugfs_create_file("stats",
+				S_IFREG|S_IRUGO,
+				snic->stats_host,
+				snic,
+				&snic_stats_fops);
+	if (!de) {
+		SNIC_DBG("Cannot create host's stats file\n");
+
+		return rc;
+	}
+	snic->stats_file = de;
+
+	de = debugfs_create_file("reset_stats",
+				S_IFREG|S_IRUGO|S_IWUSR,
+				snic->stats_host,
+				snic,
+				&snic_reset_stats_fops);
+
+	if (!de) {
+		SNIC_DBG("Cannot create host's reset_stats file\n");
+
+		return rc;
+	}
+	snic->reset_stats_file = de;
+	rc = 0;
+
+	return rc;
+} /* end of snic_stats_debugfs_init */
+
+/*
+ * snic_stats_debugfs_remove - Tear down debugfs infrastructure of stats
+ *
+ * Description:
+ * When Debufs is configured this routine removes debugfs file system
+ * elements that are specific to to snic stats
+ */
+void
+snic_stats_debugfs_remove(struct snic *snic)
+{
+	debugfs_remove(snic->stats_file);
+	snic->stats_file = NULL;
+
+	debugfs_remove(snic->reset_stats_file);
+	snic->reset_stats_file = NULL;
+
+	debugfs_remove(snic->stats_host);
+	snic->stats_host = NULL;
+}
+
+/* Trace Facility related API */
+static void *
+snic_trc_seq_start(struct seq_file *sfp, loff_t *pos)
+{
+	return &snic_glob->trc;
+}
+
+static void *
+snic_trc_seq_next(struct seq_file *sfp, void *data, loff_t *pos)
+{
+	return NULL;
+}
+
+static void
+snic_trc_seq_stop(struct seq_file *sfp, void *data)
+{
+}
+
+#define SNIC_TRC_PBLEN	256
+static int
+snic_trc_seq_show(struct seq_file *sfp, void *data)
+{
+	char buf[SNIC_TRC_PBLEN];
+
+	if (snic_get_trc_data(buf, SNIC_TRC_PBLEN) > 0)
+		seq_printf(sfp, "%s\n", buf);
+
+	return 0;
+}
+
+static const struct seq_operations snic_trc_seq_ops = {
+	.start	= snic_trc_seq_start,
+	.next	= snic_trc_seq_next,
+	.stop	= snic_trc_seq_stop,
+	.show	= snic_trc_seq_show,
+};
+
+static int
+snic_trc_open(struct inode *inode, struct file *filp)
+{
+	return seq_open(filp, &snic_trc_seq_ops);
+}
+
+static const struct file_operations snic_trc_fops = {
+	.owner	= THIS_MODULE,
+	.open	= snic_trc_open,
+	.read	= seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+/*
+ * snic_trc_debugfs_init : creates trace/tracing_enable files for trace
+ * under debugfs
+ */
+int
+snic_trc_debugfs_init(void)
+{
+	struct dentry *de = NULL;
+	int ret = -1;
+
+	if (!snic_glob->trc_root) {
+		SNIC_ERR("Debugfs root directory for snic doesn't exist.\n");
+
+		return ret;
+	}
+
+	de = debugfs_create_bool("tracing_enable",
+				 S_IFREG | S_IRUGO | S_IWUSR,
+				 snic_glob->trc_root,
+				 &snic_glob->trc.enable);
+
+	if (!de) {
+		SNIC_ERR("Can't create trace_enable file.\n");
+
+		return ret;
+	}
+	snic_glob->trc.trc_enable = de;
+
+	de = debugfs_create_file("trace",
+				 S_IFREG | S_IRUGO | S_IWUSR,
+				 snic_glob->trc_root,
+				 NULL,
+				 &snic_trc_fops);
+
+	if (!de) {
+		SNIC_ERR("Cann't create trace file.\n");
+
+		return ret;
+	}
+	snic_glob->trc.trc_file = de;
+	ret = 0;
+
+	return ret;
+} /* end of snic_trc_debugfs_init */
+
+/*
+ * snic_trc_debugfs_term : cleans up the files created for trace under debugfs
+ */
+void
+snic_trc_debugfs_term(void)
+{
+	debugfs_remove(snic_glob->trc.trc_file);
+	snic_glob->trc.trc_file = NULL;
+
+	debugfs_remove(snic_glob->trc.trc_enable);
+	snic_glob->trc.trc_enable = NULL;
+}
diff --git a/drivers/scsi/snic/snic_disc.c b/drivers/scsi/snic/snic_disc.c
new file mode 100644
index 000000000000..5f6321759ad9
--- /dev/null
+++ b/drivers/scsi/snic/snic_disc.c
@@ -0,0 +1,551 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/errno.h>
+#include <linux/mempool.h>
+
+#include <scsi/scsi_tcq.h>
+
+#include "snic_disc.h"
+#include "snic.h"
+#include "snic_io.h"
+
+
+/* snic target types */
+static const char * const snic_tgt_type_str[] = {
+	[SNIC_TGT_DAS] = "DAS",
+	[SNIC_TGT_SAN] = "SAN",
+};
+
+static inline const char *
+snic_tgt_type_to_str(int typ)
+{
+	return ((typ > SNIC_TGT_NONE && typ <= SNIC_TGT_SAN) ?
+		 snic_tgt_type_str[typ] : "Unknown");
+}
+
+static const char * const snic_tgt_state_str[] = {
+	[SNIC_TGT_STAT_INIT]	= "INIT",
+	[SNIC_TGT_STAT_ONLINE]	= "ONLINE",
+	[SNIC_TGT_STAT_OFFLINE]	= "OFFLINE",
+	[SNIC_TGT_STAT_DEL]	= "DELETION IN PROGRESS",
+};
+
+const char *
+snic_tgt_state_to_str(int state)
+{
+	return ((state >= SNIC_TGT_STAT_INIT && state <= SNIC_TGT_STAT_DEL) ?
+		snic_tgt_state_str[state] : "UNKNOWN");
+}
+
+/*
+ * Initiate report_tgt req desc
+ */
+static void
+snic_report_tgt_init(struct snic_host_req *req, u32 hid, u8 *buf, u32 len,
+		     dma_addr_t rsp_buf_pa, ulong ctx)
+{
+	struct snic_sg_desc *sgd = NULL;
+
+
+	snic_io_hdr_enc(&req->hdr, SNIC_REQ_REPORT_TGTS, 0, SCSI_NO_TAG, hid,
+			1, ctx);
+
+	req->u.rpt_tgts.sg_cnt = cpu_to_le16(1);
+	sgd = req_to_sgl(req);
+	sgd[0].addr = cpu_to_le64(rsp_buf_pa);
+	sgd[0].len = cpu_to_le32(len);
+	sgd[0]._resvd = 0;
+	req->u.rpt_tgts.sg_addr = cpu_to_le64((ulong)sgd);
+}
+
+/*
+ * snic_queue_report_tgt_req: Queues report target request.
+ */
+static int
+snic_queue_report_tgt_req(struct snic *snic)
+{
+	struct snic_req_info *rqi = NULL;
+	u32 ntgts, buf_len = 0;
+	u8 *buf = NULL;
+	dma_addr_t pa = 0;
+	int ret = 0;
+
+	rqi = snic_req_init(snic, 1);
+	if (!rqi) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	if (snic->fwinfo.max_tgts)
+		ntgts = min_t(u32, snic->fwinfo.max_tgts, snic->shost->max_id);
+	else
+		ntgts = snic->shost->max_id;
+
+	/* Allocate Response Buffer */
+	SNIC_BUG_ON(ntgts == 0);
+	buf_len = ntgts * sizeof(struct snic_tgt_id) + SNIC_SG_DESC_ALIGN;
+
+	buf = kzalloc(buf_len, GFP_KERNEL|GFP_DMA);
+	if (!buf) {
+		snic_req_free(snic, rqi);
+		SNIC_HOST_ERR(snic->shost, "Resp Buf Alloc Failed.\n");
+
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	SNIC_BUG_ON((((unsigned long)buf) % SNIC_SG_DESC_ALIGN) != 0);
+
+	pa = pci_map_single(snic->pdev, buf, buf_len, PCI_DMA_FROMDEVICE);
+	if (pci_dma_mapping_error(snic->pdev, pa)) {
+		kfree(buf);
+		snic_req_free(snic, rqi);
+		SNIC_HOST_ERR(snic->shost,
+			      "Rpt-tgt rspbuf %p: PCI DMA Mapping Failed\n",
+			      buf);
+		ret = -EINVAL;
+
+		goto error;
+	}
+
+
+	SNIC_BUG_ON(pa == 0);
+	rqi->sge_va = (ulong) buf;
+
+	snic_report_tgt_init(rqi->req,
+			     snic->config.hid,
+			     buf,
+			     buf_len,
+			     pa,
+			     (ulong)rqi);
+
+	snic_handle_untagged_req(snic, rqi);
+
+	ret = snic_queue_wq_desc(snic, rqi->req, rqi->req_len);
+	if (ret) {
+		pci_unmap_single(snic->pdev, pa, buf_len, PCI_DMA_FROMDEVICE);
+		kfree(buf);
+		rqi->sge_va = 0;
+		snic_release_untagged_req(snic, rqi);
+		SNIC_HOST_ERR(snic->shost, "Queuing Report Tgts Failed.\n");
+
+		goto error;
+	}
+
+	SNIC_DISC_DBG(snic->shost, "Report Targets Issued.\n");
+
+	return ret;
+
+error:
+	SNIC_HOST_ERR(snic->shost,
+		      "Queuing Report Targets Failed, err = %d\n",
+		      ret);
+	return ret;
+} /* end of snic_queue_report_tgt_req */
+
+/* call into SML */
+static void
+snic_scsi_scan_tgt(struct work_struct *work)
+{
+	struct snic_tgt *tgt = container_of(work, struct snic_tgt, scan_work);
+	struct Scsi_Host *shost = dev_to_shost(&tgt->dev);
+	unsigned long flags;
+
+	SNIC_HOST_INFO(shost, "Scanning Target id 0x%x\n", tgt->id);
+	scsi_scan_target(&tgt->dev,
+			 tgt->channel,
+			 tgt->scsi_tgt_id,
+			 SCAN_WILD_CARD,
+			 1);
+
+	spin_lock_irqsave(shost->host_lock, flags);
+	tgt->flags &= ~SNIC_TGT_SCAN_PENDING;
+	spin_unlock_irqrestore(shost->host_lock, flags);
+} /* end of snic_scsi_scan_tgt */
+
+/*
+ * snic_tgt_lookup :
+ */
+static struct snic_tgt *
+snic_tgt_lookup(struct snic *snic, struct snic_tgt_id *tgtid)
+{
+	struct list_head *cur, *nxt;
+	struct snic_tgt *tgt = NULL;
+
+	list_for_each_safe(cur, nxt, &snic->disc.tgt_list) {
+		tgt = list_entry(cur, struct snic_tgt, list);
+		if (tgt->id == le32_to_cpu(tgtid->tgt_id))
+			return tgt;
+		tgt = NULL;
+	}
+
+	return tgt;
+} /* end of snic_tgt_lookup */
+
+/*
+ * snic_tgt_dev_release : Called on dropping last ref for snic_tgt object
+ */
+void
+snic_tgt_dev_release(struct device *dev)
+{
+	struct snic_tgt *tgt = dev_to_tgt(dev);
+
+	SNIC_HOST_INFO(snic_tgt_to_shost(tgt),
+		       "Target Device ID %d (%s) Permanently Deleted.\n",
+		       tgt->id,
+		       dev_name(dev));
+
+	SNIC_BUG_ON(!list_empty(&tgt->list));
+	kfree(tgt);
+}
+
+/*
+ * snic_tgt_del : work function to delete snic_tgt
+ */
+static void
+snic_tgt_del(struct work_struct *work)
+{
+	struct snic_tgt *tgt = container_of(work, struct snic_tgt, del_work);
+	struct Scsi_Host *shost = snic_tgt_to_shost(tgt);
+
+	if (tgt->flags & SNIC_TGT_SCAN_PENDING)
+		scsi_flush_work(shost);
+
+	/* Block IOs on child devices, stops new IOs */
+	scsi_target_block(&tgt->dev);
+
+	/* Cleanup IOs */
+	snic_tgt_scsi_abort_io(tgt);
+
+	/* Unblock IOs now, to flush if there are any. */
+	scsi_target_unblock(&tgt->dev, SDEV_TRANSPORT_OFFLINE);
+
+	/* Delete SCSI Target and sdevs */
+	scsi_remove_target(&tgt->dev);  /* ?? */
+	device_del(&tgt->dev);
+	put_device(&tgt->dev);
+} /* end of snic_tgt_del */
+
+/* snic_tgt_create: checks for existence of snic_tgt, if it doesn't
+ * it creates one.
+ */
+static struct snic_tgt *
+snic_tgt_create(struct snic *snic, struct snic_tgt_id *tgtid)
+{
+	struct snic_tgt *tgt = NULL;
+	unsigned long flags;
+	int ret;
+
+	tgt = snic_tgt_lookup(snic, tgtid);
+	if (tgt) {
+		/* update the information if required */
+		return tgt;
+	}
+
+	tgt = kzalloc(sizeof(*tgt), GFP_KERNEL);
+	if (!tgt) {
+		SNIC_HOST_ERR(snic->shost, "Failure to allocate snic_tgt.\n");
+		ret = -ENOMEM;
+
+		return tgt;
+	}
+
+	INIT_LIST_HEAD(&tgt->list);
+	tgt->id = le32_to_cpu(tgtid->tgt_id);
+	tgt->channel = 0;
+
+	SNIC_BUG_ON(le16_to_cpu(tgtid->tgt_type) > SNIC_TGT_SAN);
+	tgt->tdata.typ = le16_to_cpu(tgtid->tgt_type);
+
+	/*
+	 * Plugging into SML Device Tree
+	 */
+	tgt->tdata.disc_id = 0;
+	tgt->state = SNIC_TGT_STAT_INIT;
+	device_initialize(&tgt->dev);
+	tgt->dev.parent = get_device(&snic->shost->shost_gendev);
+	tgt->dev.release = snic_tgt_dev_release;
+	INIT_WORK(&tgt->scan_work, snic_scsi_scan_tgt);
+	INIT_WORK(&tgt->del_work, snic_tgt_del);
+	switch (tgt->tdata.typ) {
+	case SNIC_TGT_DAS:
+		dev_set_name(&tgt->dev, "snic_das_tgt:%d:%d-%d",
+			     snic->shost->host_no, tgt->channel, tgt->id);
+		break;
+
+	case SNIC_TGT_SAN:
+		dev_set_name(&tgt->dev, "snic_san_tgt:%d:%d-%d",
+			     snic->shost->host_no, tgt->channel, tgt->id);
+		break;
+
+	default:
+		SNIC_HOST_INFO(snic->shost, "Target type Unknown Detected.\n");
+		dev_set_name(&tgt->dev, "snic_das_tgt:%d:%d-%d",
+			     snic->shost->host_no, tgt->channel, tgt->id);
+		break;
+	}
+
+	spin_lock_irqsave(snic->shost->host_lock, flags);
+	list_add_tail(&tgt->list, &snic->disc.tgt_list);
+	tgt->scsi_tgt_id = snic->disc.nxt_tgt_id++;
+	tgt->state = SNIC_TGT_STAT_ONLINE;
+	spin_unlock_irqrestore(snic->shost->host_lock, flags);
+
+	SNIC_HOST_INFO(snic->shost,
+		       "Tgt %d, type = %s detected. Adding..\n",
+		       tgt->id, snic_tgt_type_to_str(tgt->tdata.typ));
+
+	ret = device_add(&tgt->dev);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "Snic Tgt: device_add, with err = %d\n",
+			      ret);
+
+		put_device(&snic->shost->shost_gendev);
+		kfree(tgt);
+		tgt = NULL;
+
+		return tgt;
+	}
+
+	SNIC_HOST_INFO(snic->shost, "Scanning %s.\n", dev_name(&tgt->dev));
+
+	scsi_queue_work(snic->shost, &tgt->scan_work);
+
+	return tgt;
+} /* end of snic_tgt_create */
+
+/* Handler for discovery */
+void
+snic_handle_tgt_disc(struct work_struct *work)
+{
+	struct snic *snic = container_of(work, struct snic, tgt_work);
+	struct snic_tgt_id *tgtid = NULL;
+	struct snic_tgt *tgt = NULL;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&snic->snic_lock, flags);
+	if (snic->in_remove) {
+		spin_unlock_irqrestore(&snic->snic_lock, flags);
+		kfree(snic->disc.rtgt_info);
+
+		return;
+	}
+	spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+	mutex_lock(&snic->disc.mutex);
+	/* Discover triggered during disc in progress */
+	if (snic->disc.req_cnt) {
+		snic->disc.state = SNIC_DISC_DONE;
+		snic->disc.req_cnt = 0;
+		mutex_unlock(&snic->disc.mutex);
+		kfree(snic->disc.rtgt_info);
+		snic->disc.rtgt_info = NULL;
+
+		SNIC_HOST_INFO(snic->shost, "tgt_disc: Discovery restart.\n");
+		/* Start Discovery Again */
+		snic_disc_start(snic);
+
+		return;
+	}
+
+	tgtid = (struct snic_tgt_id *)snic->disc.rtgt_info;
+
+	SNIC_BUG_ON(snic->disc.rtgt_cnt == 0 || tgtid == NULL);
+
+	for (i = 0; i < snic->disc.rtgt_cnt; i++) {
+		tgt = snic_tgt_create(snic, &tgtid[i]);
+		if (!tgt) {
+			int buf_sz = snic->disc.rtgt_cnt * sizeof(*tgtid);
+
+			SNIC_HOST_ERR(snic->shost, "Failed to create tgt.\n");
+			snic_hex_dump("rpt_tgt_rsp", (char *)tgtid, buf_sz);
+			break;
+		}
+	}
+
+	snic->disc.rtgt_info = NULL;
+	snic->disc.state = SNIC_DISC_DONE;
+	mutex_unlock(&snic->disc.mutex);
+
+	SNIC_HOST_INFO(snic->shost, "Discovery Completed.\n");
+
+	kfree(tgtid);
+} /* end of snic_handle_tgt_disc */
+
+
+int
+snic_report_tgt_cmpl_handler(struct snic *snic, struct snic_fw_req *fwreq)
+{
+
+	u8 typ, cmpl_stat;
+	u32 cmnd_id, hid, tgt_cnt = 0;
+	ulong ctx;
+	struct snic_req_info *rqi = NULL;
+	struct snic_tgt_id *tgtid;
+	int i, ret = 0;
+
+	snic_io_hdr_dec(&fwreq->hdr, &typ, &cmpl_stat, &cmnd_id, &hid, &ctx);
+	rqi = (struct snic_req_info *) ctx;
+	tgtid = (struct snic_tgt_id *) rqi->sge_va;
+
+	tgt_cnt = le32_to_cpu(fwreq->u.rpt_tgts_cmpl.tgt_cnt);
+	if (tgt_cnt == 0) {
+		SNIC_HOST_ERR(snic->shost, "No Targets Found on this host.\n");
+		ret = 1;
+
+		goto end;
+	}
+
+	/* printing list of targets here */
+	SNIC_HOST_INFO(snic->shost, "Target Count = %d\n", tgt_cnt);
+
+	SNIC_BUG_ON(tgt_cnt > snic->fwinfo.max_tgts);
+
+	for (i = 0; i < tgt_cnt; i++)
+		SNIC_HOST_INFO(snic->shost,
+			       "Tgt id = 0x%x\n",
+			       le32_to_cpu(tgtid[i].tgt_id));
+
+	/*
+	 * Queue work for further processing,
+	 * Response Buffer Memory is freed after creating targets
+	 */
+	snic->disc.rtgt_cnt = tgt_cnt;
+	snic->disc.rtgt_info = (u8 *) tgtid;
+	queue_work(snic_glob->event_q, &snic->tgt_work);
+	ret = 0;
+
+end:
+	/* Unmap Response Buffer */
+	snic_pci_unmap_rsp_buf(snic, rqi);
+	if (ret)
+		kfree(tgtid);
+
+	rqi->sge_va = 0;
+	snic_release_untagged_req(snic, rqi);
+
+	return ret;
+} /* end of snic_report_tgt_cmpl_handler */
+
+/* Discovery init fn */
+void
+snic_disc_init(struct snic_disc *disc)
+{
+	INIT_LIST_HEAD(&disc->tgt_list);
+	mutex_init(&disc->mutex);
+	disc->disc_id = 0;
+	disc->nxt_tgt_id = 0;
+	disc->state = SNIC_DISC_INIT;
+	disc->req_cnt = 0;
+	disc->rtgt_cnt = 0;
+	disc->rtgt_info = NULL;
+	disc->cb = NULL;
+} /* end of snic_disc_init */
+
+/* Discovery, uninit fn */
+void
+snic_disc_term(struct snic *snic)
+{
+	struct snic_disc *disc = &snic->disc;
+
+	mutex_lock(&disc->mutex);
+	if (disc->req_cnt) {
+		disc->req_cnt = 0;
+		SNIC_SCSI_DBG(snic->shost, "Terminating Discovery.\n");
+	}
+	mutex_unlock(&disc->mutex);
+}
+
+/*
+ * snic_disc_start: Discovery Start ...
+ */
+int
+snic_disc_start(struct snic *snic)
+{
+	struct snic_disc *disc = &snic->disc;
+	int ret = 0;
+
+	SNIC_SCSI_DBG(snic->shost, "Discovery Start.\n");
+
+	mutex_lock(&disc->mutex);
+	if (disc->state == SNIC_DISC_PENDING) {
+		disc->req_cnt++;
+		mutex_unlock(&disc->mutex);
+
+		return ret;
+	}
+	disc->state = SNIC_DISC_PENDING;
+	mutex_unlock(&disc->mutex);
+
+	ret = snic_queue_report_tgt_req(snic);
+	if (ret)
+		SNIC_HOST_INFO(snic->shost, "Discovery Failed, err=%d.\n", ret);
+
+	return ret;
+} /* end of snic_disc_start */
+
+/*
+ * snic_disc_work :
+ */
+void
+snic_handle_disc(struct work_struct *work)
+{
+	struct snic *snic = container_of(work, struct snic, disc_work);
+	int ret = 0;
+
+	SNIC_HOST_INFO(snic->shost, "disc_work: Discovery\n");
+
+	ret = snic_disc_start(snic);
+	if (ret)
+		goto disc_err;
+
+disc_err:
+	SNIC_HOST_ERR(snic->shost,
+		      "disc_work: Discovery Failed w/ err = %d\n",
+		      ret);
+} /* end of snic_disc_work */
+
+/*
+ * snic_tgt_del_all : cleanup all snic targets
+ * Called on unbinding the interface
+ */
+void
+snic_tgt_del_all(struct snic *snic)
+{
+	struct snic_tgt *tgt = NULL;
+	struct list_head *cur, *nxt;
+	unsigned long flags;
+
+	mutex_lock(&snic->disc.mutex);
+	spin_lock_irqsave(snic->shost->host_lock, flags);
+
+	list_for_each_safe(cur, nxt, &snic->disc.tgt_list) {
+		tgt = list_entry(cur, struct snic_tgt, list);
+		tgt->state = SNIC_TGT_STAT_DEL;
+		list_del_init(&tgt->list);
+		SNIC_HOST_INFO(snic->shost, "Tgt %d q'ing for del\n", tgt->id);
+		queue_work(snic_glob->event_q, &tgt->del_work);
+		tgt = NULL;
+	}
+	spin_unlock_irqrestore(snic->shost->host_lock, flags);
+
+	scsi_flush_work(snic->shost);
+	mutex_unlock(&snic->disc.mutex);
+} /* end of snic_tgt_del_all */
diff --git a/drivers/scsi/snic/snic_disc.h b/drivers/scsi/snic/snic_disc.h
new file mode 100644
index 000000000000..97fa3f5c5bb4
--- /dev/null
+++ b/drivers/scsi/snic/snic_disc.h
@@ -0,0 +1,124 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __SNIC_DISC_H
+#define __SNIC_DISC_H
+
+#include "snic_fwint.h"
+
+enum snic_disc_state {
+	SNIC_DISC_NONE,
+	SNIC_DISC_INIT,
+	SNIC_DISC_PENDING,
+	SNIC_DISC_DONE
+};
+
+struct snic;
+struct snic_disc {
+	struct list_head tgt_list;
+	enum snic_disc_state state;
+	struct mutex mutex;
+	u16	disc_id;
+	u8	req_cnt;
+	u32	nxt_tgt_id;
+	u32	rtgt_cnt;
+	u8	*rtgt_info;
+	struct delayed_work disc_timeout;
+	void (*cb)(struct snic *);
+};
+
+#define SNIC_TGT_NAM_LEN	16
+
+enum snic_tgt_state {
+	SNIC_TGT_STAT_NONE,
+	SNIC_TGT_STAT_INIT,
+	SNIC_TGT_STAT_ONLINE,	/* Target is Online */
+	SNIC_TGT_STAT_OFFLINE,	/* Target is Offline */
+	SNIC_TGT_STAT_DEL,
+};
+
+struct snic_tgt_priv {
+	struct list_head list;
+	enum snic_tgt_type typ;
+	u16 disc_id;
+	char *name[SNIC_TGT_NAM_LEN];
+
+	union {
+		/*DAS Target specific info */
+		/*SAN Target specific info */
+		u8 dummmy;
+	} u;
+};
+
+/* snic tgt flags */
+#define SNIC_TGT_SCAN_PENDING	0x01
+
+struct snic_tgt {
+	struct list_head list;
+	u16	id;
+	u16	channel;
+	u32	flags;
+	u32	scsi_tgt_id;
+	enum snic_tgt_state state;
+	struct device dev;
+	struct work_struct scan_work;
+	struct work_struct del_work;
+	struct snic_tgt_priv tdata;
+};
+
+
+struct snic_fw_req;
+
+void snic_disc_init(struct snic_disc *);
+int snic_disc_start(struct snic *);
+void snic_disc_term(struct snic *);
+int snic_report_tgt_cmpl_handler(struct snic *, struct snic_fw_req *);
+int snic_tgtinfo_cmpl_handler(struct snic *snic, struct snic_fw_req *fwreq);
+void snic_process_report_tgts_rsp(struct work_struct *);
+void snic_handle_tgt_disc(struct work_struct *);
+void snic_handle_disc(struct work_struct *);
+void snic_tgt_dev_release(struct device *);
+void snic_tgt_del_all(struct snic *);
+
+#define dev_to_tgt(d) \
+	container_of(d, struct snic_tgt, dev)
+
+static inline int
+is_snic_target(struct device *dev)
+{
+	return dev->release == snic_tgt_dev_release;
+}
+
+#define starget_to_tgt(st)	\
+	(is_snic_target(((struct scsi_target *) st)->dev.parent) ? \
+		dev_to_tgt(st->dev.parent) : NULL)
+
+#define snic_tgt_to_shost(t)	\
+	dev_to_shost(t->dev.parent)
+
+static inline int
+snic_tgt_chkready(struct snic_tgt *tgt)
+{
+	if (tgt->state == SNIC_TGT_STAT_ONLINE)
+		return 0;
+	else
+		return DID_NO_CONNECT << 16;
+}
+
+const char *snic_tgt_state_to_str(int);
+int snic_tgt_scsi_abort_io(struct snic_tgt *);
+#endif /* end of  __SNIC_DISC_H */
diff --git a/drivers/scsi/snic/snic_fwint.h b/drivers/scsi/snic/snic_fwint.h
new file mode 100644
index 000000000000..2cfaf2dc915f
--- /dev/null
+++ b/drivers/scsi/snic/snic_fwint.h
@@ -0,0 +1,525 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __SNIC_FWINT_H
+#define __SNIC_FWINT_H
+
+#define SNIC_CDB_LEN	32	/* SCSI CDB size 32, can be used for 16 bytes */
+#define LUN_ADDR_LEN	8
+
+/*
+ * Command entry type
+ */
+enum snic_io_type {
+	/*
+	 * Initiator request types
+	 */
+	SNIC_REQ_REPORT_TGTS = 0x2,	/* Report Targets */
+	SNIC_REQ_ICMND,			/* Initiator command for SCSI IO */
+	SNIC_REQ_ITMF,			/* Initiator command for Task Mgmt */
+	SNIC_REQ_HBA_RESET,		/* SNIC Reset */
+	SNIC_REQ_EXCH_VER,		/* Exchange Version Information */
+	SNIC_REQ_TGT_INFO,		/* Backend/Target Information */
+	SNIC_REQ_BOOT_LUNS,
+
+	/*
+	 * Response type
+	 */
+	SNIC_RSP_REPORT_TGTS_CMPL = 0x12,/* Report Targets Completion */
+	SNIC_RSP_ICMND_CMPL,		/* SCSI IO Completion */
+	SNIC_RSP_ITMF_CMPL,		/* Task Management Completion */
+	SNIC_RSP_HBA_RESET_CMPL,	/* SNIC Reset Completion */
+	SNIC_RSP_EXCH_VER_CMPL,		/* Exchange Version Completion*/
+	SNIC_RSP_BOOT_LUNS_CMPL,
+
+	/*
+	 * Misc Request types
+	 */
+	SNIC_MSG_ACK = 0x80,		/* Ack: snic_notify_msg */
+	SNIC_MSG_ASYNC_EVNOTIFY,	/* Asynchronous Event Notification */
+}; /* end of enum snic_io_type */
+
+
+/*
+ * Header status codes from firmware
+ */
+enum snic_io_status {
+	SNIC_STAT_IO_SUCCESS = 0,	/* request was successful */
+
+	/*
+	 * If a request to the fw is rejected, the original request header
+	 * will be returned with the status set to one of the following:
+	 */
+	SNIC_STAT_INVALID_HDR,	/* header contains invalid data */
+	SNIC_STAT_OUT_OF_RES,	/* out of resources to complete request */
+	SNIC_STAT_INVALID_PARM,	/* some parameter in request is not valid */
+	SNIC_STAT_REQ_NOT_SUP,	/* req type is not supported */
+	SNIC_STAT_IO_NOT_FOUND,	/* requested IO was not found */
+
+	/*
+	 * Once a request is processed, the fw will usually return
+	 * a cmpl message type. In cases where errors occurred,
+	 * the header status would be filled in with one of the following:
+	 */
+	SNIC_STAT_ABORTED,		/* req was aborted */
+	SNIC_STAT_TIMEOUT,		/* req was timed out */
+	SNIC_STAT_SGL_INVALID,		/* req was aborted due to sgl error */
+	SNIC_STAT_DATA_CNT_MISMATCH,	/*recv/sent more/less data than expec */
+	SNIC_STAT_FW_ERR,		/* req was terminated due to fw error */
+	SNIC_STAT_ITMF_REJECT,		/* itmf req was rejected by target */
+	SNIC_STAT_ITMF_FAIL,		/* itmf req was failed */
+	SNIC_STAT_ITMF_INCORRECT_LUN,	/* itmf req has incorrect LUN id*/
+	SNIC_STAT_CMND_REJECT,		/* req was invalid and rejected */
+	SNIC_STAT_DEV_OFFLINE,		/* req sent to offline device */
+	SNIC_STAT_NO_BOOTLUN,
+	SNIC_STAT_SCSI_ERR,		/* SCSI error returned by Target. */
+	SNIC_STAT_NOT_READY,		/* sNIC Subsystem is not ready */
+	SNIC_STAT_FATAL_ERROR,		/* sNIC is in unrecoverable state */
+}; /* end of enum snic_io_status */
+
+/*
+ * snic_io_hdr : host <--> firmare
+ *
+ * for any other message that will be queued to firmware should
+ *  have the following request header
+ */
+struct snic_io_hdr {
+	__le32	hid;
+	__le32	cmnd_id;	/* tag here */
+	ulong	init_ctx;	/* initiator context */
+	u8	type;		/* request/response type */
+	u8	status;		/* header status entry */
+	u8	protocol;	/* Protocol specific, may needed for RoCE*/
+	u8	flags;
+	__le16	sg_cnt;
+	u16	resvd;
+};
+
+/* auxillary funciton for encoding the snic_io_hdr */
+static inline void
+snic_io_hdr_enc(struct snic_io_hdr *hdr, u8 typ, u8 status, u32 id, u32 hid,
+		u16 sg_cnt, ulong ctx)
+{
+	hdr->type = typ;
+	hdr->status = status;
+	hdr->protocol = 0;
+	hdr->hid = cpu_to_le32(hid);
+	hdr->cmnd_id = cpu_to_le32(id);
+	hdr->sg_cnt = cpu_to_le16(sg_cnt);
+	hdr->init_ctx = ctx;
+	hdr->flags = 0;
+}
+
+/* auxillary funciton for decoding the snic_io_hdr */
+static inline void
+snic_io_hdr_dec(struct snic_io_hdr *hdr, u8 *typ, u8 *stat, u32 *cmnd_id,
+		u32 *hid, ulong *ctx)
+{
+	*typ = hdr->type;
+	*stat = hdr->status;
+	*hid = le32_to_cpu(hdr->hid);
+	*cmnd_id = le32_to_cpu(hdr->cmnd_id);
+	*ctx = hdr->init_ctx;
+}
+
+/*
+ * snic_host_info: host -> firmware
+ *
+ * Used for sending host information to firmware, and request fw version
+ */
+struct snic_exch_ver_req {
+	__le32	drvr_ver;	/* for debugging, when fw dump captured */
+	__le32	os_type;	/* for OS specific features */
+};
+
+/*
+ * os_type flags
+ * Bit 0-7 : OS information
+ * Bit 8-31: Feature/Capability Information
+ */
+#define SNIC_OS_LINUX	0x1
+#define SNIC_OS_WIN	0x2
+#define SNIC_OS_ESX	0x3
+
+/*
+ * HBA Capabilities
+ * Bit 1: Reserved.
+ * Bit 2: Dynamic Discovery of LUNs.
+ * Bit 3: Async event notifications on on tgt online/offline events.
+ * Bit 4: IO timeout support in FW.
+ * Bit 5-31: Reserved.
+ */
+#define SNIC_HBA_CAP_DDL	0x02	/* Supports Dynamic Discovery of LUNs */
+#define SNIC_HBA_CAP_AEN	0x04	/* Supports Async Event Noitifcation */
+#define SNIC_HBA_CAP_TMO	0x08	/* Supports IO timeout in FW */
+
+/*
+ * snic_exch_ver_rsp : firmware -> host
+ *
+ * Used by firmware to send response to version request
+ */
+struct snic_exch_ver_rsp {
+	__le32	version;
+	__le32	hid;
+	__le32	max_concur_ios;		/* max concurrent ios */
+	__le32	max_sgs_per_cmd;	/* max sgls per IO */
+	__le32	max_io_sz;		/* max io size supported */
+	__le32	hba_cap;		/* hba capabilities */
+	__le32	max_tgts;		/* max tgts supported */
+	__le16	io_timeout;		/* FW extended timeout */
+	u16	rsvd;
+};
+
+
+/*
+ * snic_report_tgts : host -> firmware request
+ *
+ * Used by the host to request list of targets
+ */
+struct snic_report_tgts {
+	__le16	sg_cnt;
+	__le16	flags;		/* specific flags from fw */
+	u8	_resvd[4];
+	__le64	sg_addr;	/* Points to SGL */
+	__le64	sense_addr;
+};
+
+enum snic_type {
+	SNIC_NONE = 0x0,
+	SNIC_DAS,
+	SNIC_SAN,
+};
+
+
+/* Report Target Response */
+enum snic_tgt_type {
+	SNIC_TGT_NONE = 0x0,
+	SNIC_TGT_DAS,	/* DAS Target */
+	SNIC_TGT_SAN,	/* SAN Target */
+};
+
+/* target id format */
+struct snic_tgt_id {
+	__le32	tgt_id;		/* target id */
+	__le16	tgt_type;	/* tgt type */
+	__le16	vnic_id;	/* corresponding vnic id */
+};
+
+/*
+ * snic_report_tgts_cmpl : firmware -> host response
+ *
+ * Used by firmware to send response to Report Targets request
+ */
+struct snic_report_tgts_cmpl {
+	__le32	tgt_cnt;	/* Number of Targets accessible */
+	u32	_resvd;
+};
+
+/*
+ * Command flags
+ *
+ * Bit 0: Read flags
+ * Bit 1: Write flag
+ * Bit 2: ESGL - sg/esg array contains extended sg
+ *	  ESGE - is a host buffer contains sg elements
+ * Bit 3-4: Task Attributes
+ *		00b - simple
+ *		01b - head of queue
+ *		10b - ordered
+ * Bit 5-7: Priority - future use
+ * Bit 8-15: Reserved
+ */
+
+#define SNIC_ICMND_WR		0x01	/* write command */
+#define SNIC_ICMND_RD		0x02	/* read command */
+#define SNIC_ICMND_ESGL		0x04	/* SGE/ESGE array contains valid data*/
+
+/*
+ * Priority/Task Attribute settings
+ */
+#define SNIC_ICMND_TSK_SHIFT		2	/* task attr starts at bit 2 */
+#define SNIC_ICMND_TSK_MASK(x)		((x>>SNIC_ICMND_TSK_SHIFT) & ~(0xffff))
+#define SNIC_ICMND_TSK_SIMPLE		0	/* simple task attr */
+#define SNIC_ICMND_TSK_HEAD_OF_QUEUE	1	/* head of qeuue task attr */
+#define SNIC_ICMND_TSK_ORDERED		2	/* ordered task attr */
+
+#define SNIC_ICMND_PRI_SHIFT		5	/* prio val starts at bit 5 */
+
+/*
+ * snic_icmnd : host-> firmware request
+ *
+ * used for sending out an initiator SCSI 16/32-byte command
+ */
+struct snic_icmnd {
+	__le16	sg_cnt;		/* Number of SG Elements */
+	__le16	flags;		/* flags */
+	__le32	sense_len;	/* Sense buffer length */
+	__le64	tgt_id;		/* Destination Target ID */
+	__le64	lun_id;		/* Destination LUN ID */
+	u8	cdb_len;
+	u8	_resvd;
+	__le16	time_out;	/* ms time for Res allocations fw to handle io*/
+	__le32	data_len;	/* Total number of bytes to be transferred */
+	u8	cdb[SNIC_CDB_LEN];
+	__le64	sg_addr;	/* Points to SG List */
+	__le64	sense_addr;	/* Sense buffer address */
+};
+
+
+/* Response flags */
+/* Bit 0: Under run
+ * Bit 1: Over Run
+ * Bit 2-7: Reserved
+ */
+#define SNIC_ICMND_CMPL_UNDR_RUN	0x01	/* resid under and valid */
+#define SNIC_ICMND_CMPL_OVER_RUN	0x02	/* resid over and valid */
+
+/*
+ * snic_icmnd_cmpl: firmware -> host response
+ *
+ * Used for sending the host a response to an icmnd (initiator command)
+ */
+struct snic_icmnd_cmpl {
+	u8	scsi_status;	/* value as per SAM */
+	u8	flags;
+	__le16	sense_len;	/* Sense Length */
+	__le32	resid;		/* Residue : # bytes under or over run */
+};
+
+/*
+ * snic_itmf: host->firmware request
+ *
+ * used for requesting the firmware to abort a request and/or send out
+ * a task management function
+ *
+ * the req_id field is valid in case of abort task and clear task
+ */
+struct snic_itmf {
+	u8	tm_type;	/* SCSI Task Management request */
+	u8	resvd;
+	__le16	flags;		/* flags */
+	__le32	req_id;		/* Command id of snic req to be aborted */
+	__le64	tgt_id;		/* Target ID */
+	__le64	lun_id;		/* Destination LUN ID */
+	__le16	timeout;	/* in sec */
+};
+
+/*
+ * Task Management Request
+ */
+enum snic_itmf_tm_type {
+	SNIC_ITMF_ABTS_TASK = 0x01,	/* Abort Task */
+	SNIC_ITMF_ABTS_TASK_SET,	/* Abort Task Set */
+	SNIC_ITMF_CLR_TASK,		/* Clear Task */
+	SNIC_ITMF_CLR_TASKSET,		/* Clear Task Set */
+	SNIC_ITMF_LUN_RESET,		/* Lun Reset */
+	SNIC_ITMF_ABTS_TASK_TERM,	/* Supported for SAN Targets */
+};
+
+/*
+ * snic_itmf_cmpl: firmware -> host resposne
+ *
+ * used for sending the host a response for a itmf request
+ */
+struct snic_itmf_cmpl {
+	__le32	nterminated;	/* # IOs terminated as a result of tmf */
+	u8	flags;		/* flags */
+	u8	_resvd[3];
+};
+
+/*
+ * itmfl_cmpl flags
+ * Bit 0 : 1 - Num terminated field valid
+ * Bit 1 - 7 : Reserved
+ */
+#define SNIC_NUM_TERM_VALID	0x01	/* Number of IOs terminated */
+
+/*
+ * snic_hba_reset: host -> firmware request
+ *
+ * used for requesting firmware to reset snic
+ */
+struct snic_hba_reset {
+	__le16	flags;		/* flags */
+	u8	_resvd[6];
+};
+
+/*
+ * snic_hba_reset_cmpl: firmware -> host response
+ *
+ * Used by firmware to respond to the host's hba reset request
+ */
+struct snic_hba_reset_cmpl {
+	u8	flags;		/* flags : more info needs to be added*/
+	u8	_resvd[7];
+};
+
+/*
+ * snic_notify_msg: firmware -> host response
+ *
+ * Used by firmware to notify host of the last work queue entry received
+ */
+struct snic_notify_msg {
+	__le32	wqe_num;	/* wq entry number */
+	u8	flags;		/* flags, macros */
+	u8	_resvd[4];
+};
+
+
+#define SNIC_EVDATA_LEN		24	/* in bytes */
+/* snic_async_evnotify: firmware -> host notification
+ *
+ * Used by firmware to notify the host about configuration/state changes
+ */
+struct snic_async_evnotify {
+	u8	FLS_EVENT_DESC;
+	u8	vnic;			/* vnic id */
+	u8	_resvd[2];
+	__le32	ev_id;			/* Event ID */
+	u8	ev_data[SNIC_EVDATA_LEN]; /* Event Data */
+	u8	_resvd2[4];
+};
+
+/* async event flags */
+enum snic_ev_type {
+	SNIC_EV_TGT_OFFLINE = 0x01, /* Target Offline, PL contains TGT ID */
+	SNIC_EV_TGT_ONLINE,	/* Target Online, PL contains TGT ID */
+	SNIC_EV_LUN_OFFLINE,	/* LUN Offline, PL contains LUN ID */
+	SNIC_EV_LUN_ONLINE,	/* LUN Online, PL contains LUN ID */
+	SNIC_EV_CONF_CHG,	/* Dev Config/Attr Change Event */
+	SNIC_EV_TGT_ADDED,	/* Target Added */
+	SNIC_EV_TGT_DELTD,	/* Target Del'd, PL contains TGT ID */
+	SNIC_EV_LUN_ADDED,	/* LUN Added */
+	SNIC_EV_LUN_DELTD,	/* LUN Del'd, PL cont. TGT & LUN ID */
+
+	SNIC_EV_DISC_CMPL = 0x10, /* Discovery Completed Event */
+};
+
+
+#define SNIC_HOST_REQ_LEN	128	/*Exp length of host req, wq desc sz*/
+/* Payload 88 bytes = 128 - 24 - 16 */
+#define SNIC_HOST_REQ_PAYLOAD	((int)(SNIC_HOST_REQ_LEN -		\
+					sizeof(struct snic_io_hdr) -	\
+					(2 * sizeof(u64))))
+
+/*
+ * snic_host_req: host -> firmware request
+ *
+ * Basic structure for all snic requests that are sent from the host to
+ * firmware. They are 128 bytes in size.
+ */
+struct snic_host_req {
+	u64	ctrl_data[2];	/*16 bytes - Control Data */
+	struct snic_io_hdr hdr;
+	union {
+		/*
+		 * Entry specific space, last byte contains color
+		 */
+		u8	buf[SNIC_HOST_REQ_PAYLOAD];
+
+		/*
+		 * Exchange firmware version
+		 */
+		struct snic_exch_ver_req	exch_ver;
+
+		/* report targets */
+		struct snic_report_tgts		rpt_tgts;
+
+		/* io request */
+		struct snic_icmnd		icmnd;
+
+		/* task management request */
+		struct snic_itmf		itmf;
+
+		/* hba reset */
+		struct snic_hba_reset		reset;
+	} u;
+}; /* end of snic_host_req structure */
+
+
+#define SNIC_FW_REQ_LEN		64 /* Expected length of fw req */
+struct snic_fw_req {
+	struct snic_io_hdr hdr;
+	union {
+		/*
+		 * Entry specific space, last byte contains color
+		 */
+		u8	buf[SNIC_FW_REQ_LEN - sizeof(struct snic_io_hdr)];
+
+		/* Exchange Version Response */
+		struct snic_exch_ver_rsp	exch_ver_cmpl;
+
+		/* Report Targets Response */
+		struct snic_report_tgts_cmpl	rpt_tgts_cmpl;
+
+		/* scsi response */
+		struct snic_icmnd_cmpl		icmnd_cmpl;
+
+		/* task management response */
+		struct snic_itmf_cmpl		itmf_cmpl;
+
+		/* hba reset response */
+		struct snic_hba_reset_cmpl	reset_cmpl;
+
+		/* notify message */
+		struct snic_notify_msg		ack;
+
+		/* async notification event */
+		struct snic_async_evnotify	async_ev;
+
+	} u;
+}; /* end of snic_fw_req structure */
+
+/*
+ * Auxillary macro to verify specific snic req/cmpl structures
+ * to ensure that it will be aligned to 64 bit, and not using
+ * color bit field
+ */
+#define VERIFY_REQ_SZ(x)
+#define VERIFY_CMPL_SZ(x)
+
+/*
+ * Access routines to encode and decode the color bit, which is the most
+ * significant bit of the structure.
+ */
+static inline void
+snic_color_enc(struct snic_fw_req *req, u8 color)
+{
+	u8 *c = ((u8 *) req) + sizeof(struct snic_fw_req) - 1;
+
+	if (color)
+		*c |= 0x80;
+	else
+		*c &= ~0x80;
+}
+
+static inline void
+snic_color_dec(struct snic_fw_req *req, u8 *color)
+{
+	u8 *c = ((u8 *) req) + sizeof(struct snic_fw_req) - 1;
+
+	*color = *c >> 7;
+
+	/* Make sure color bit is read from desc *before* other fields
+	 * are read from desc. Hardware guarantees color bit is last
+	 * bit (byte) written. Adding the rmb() prevents the compiler
+	 * and/or CPU from reordering the reads which would potentially
+	 * result in reading stale values.
+	 */
+	rmb();
+}
+#endif /* end of __SNIC_FWINT_H */
diff --git a/drivers/scsi/snic/snic_io.c b/drivers/scsi/snic/snic_io.c
new file mode 100644
index 000000000000..993db7de4e4b
--- /dev/null
+++ b/drivers/scsi/snic/snic_io.c
@@ -0,0 +1,518 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/errno.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/mempool.h>
+#include <scsi/scsi_tcq.h>
+
+#include "snic_io.h"
+#include "snic.h"
+#include "cq_enet_desc.h"
+#include "snic_fwint.h"
+
+static void
+snic_wq_cmpl_frame_send(struct vnic_wq *wq,
+			    struct cq_desc *cq_desc,
+			    struct vnic_wq_buf *buf,
+			    void *opaque)
+{
+	struct snic *snic = svnic_dev_priv(wq->vdev);
+
+	SNIC_BUG_ON(buf->os_buf == NULL);
+
+	if (snic_log_level & SNIC_DESC_LOGGING)
+		SNIC_HOST_INFO(snic->shost,
+			       "Ack received for snic_host_req %p.\n",
+			       buf->os_buf);
+
+	SNIC_TRC(snic->shost->host_no, 0, 0,
+		 ((ulong)(buf->os_buf) - sizeof(struct snic_req_info)), 0, 0,
+		 0);
+	pci_unmap_single(snic->pdev, buf->dma_addr, buf->len, PCI_DMA_TODEVICE);
+	buf->os_buf = NULL;
+}
+
+static int
+snic_wq_cmpl_handler_cont(struct vnic_dev *vdev,
+			  struct cq_desc *cq_desc,
+			  u8 type,
+			  u16 q_num,
+			  u16 cmpl_idx,
+			  void *opaque)
+{
+	struct snic *snic = svnic_dev_priv(vdev);
+	unsigned long flags;
+
+	SNIC_BUG_ON(q_num != 0);
+
+	spin_lock_irqsave(&snic->wq_lock[q_num], flags);
+	svnic_wq_service(&snic->wq[q_num],
+			 cq_desc,
+			 cmpl_idx,
+			 snic_wq_cmpl_frame_send,
+			 NULL);
+	spin_unlock_irqrestore(&snic->wq_lock[q_num], flags);
+
+	return 0;
+} /* end of snic_cmpl_handler_cont */
+
+int
+snic_wq_cmpl_handler(struct snic *snic, int work_to_do)
+{
+	unsigned int work_done = 0;
+	unsigned int i;
+
+	snic->s_stats.misc.last_ack_time = jiffies;
+	for (i = 0; i < snic->wq_count; i++) {
+		work_done += svnic_cq_service(&snic->cq[i],
+					      work_to_do,
+					      snic_wq_cmpl_handler_cont,
+					      NULL);
+	}
+
+	return work_done;
+} /* end of snic_wq_cmpl_handler */
+
+void
+snic_free_wq_buf(struct vnic_wq *wq, struct vnic_wq_buf *buf)
+{
+
+	struct snic_host_req *req = buf->os_buf;
+	struct snic *snic = svnic_dev_priv(wq->vdev);
+	struct snic_req_info *rqi = NULL;
+	unsigned long flags;
+
+	pci_unmap_single(snic->pdev, buf->dma_addr, buf->len, PCI_DMA_TODEVICE);
+
+	rqi = req_to_rqi(req);
+	spin_lock_irqsave(&snic->spl_cmd_lock, flags);
+	if (list_empty(&rqi->list)) {
+		spin_unlock_irqrestore(&snic->spl_cmd_lock, flags);
+		goto end;
+	}
+
+	SNIC_BUG_ON(rqi->list.next == NULL); /* if not added to spl_cmd_list */
+	list_del_init(&rqi->list);
+	spin_unlock_irqrestore(&snic->spl_cmd_lock, flags);
+
+	if (rqi->sge_va) {
+		snic_pci_unmap_rsp_buf(snic, rqi);
+		kfree((void *)rqi->sge_va);
+		rqi->sge_va = 0;
+	}
+	snic_req_free(snic, rqi);
+	SNIC_HOST_INFO(snic->shost, "snic_free_wq_buf .. freed.\n");
+
+end:
+	return;
+}
+
+/* Criteria to select work queue in multi queue mode */
+static int
+snic_select_wq(struct snic *snic)
+{
+	/* No multi queue support for now */
+	BUILD_BUG_ON(SNIC_WQ_MAX > 1);
+
+	return 0;
+}
+
+int
+snic_queue_wq_desc(struct snic *snic, void *os_buf, u16 len)
+{
+	dma_addr_t pa = 0;
+	unsigned long flags;
+	struct snic_fw_stats *fwstats = &snic->s_stats.fw;
+	long act_reqs;
+	int q_num = 0;
+
+	snic_print_desc(__func__, os_buf, len);
+
+	/* Map request buffer */
+	pa = pci_map_single(snic->pdev, os_buf, len, PCI_DMA_TODEVICE);
+	if (pci_dma_mapping_error(snic->pdev, pa)) {
+		SNIC_HOST_ERR(snic->shost, "qdesc: PCI DMA Mapping Fail.\n");
+
+		return -ENOMEM;
+	}
+
+	q_num = snic_select_wq(snic);
+
+	spin_lock_irqsave(&snic->wq_lock[q_num], flags);
+	if (!svnic_wq_desc_avail(snic->wq)) {
+		pci_unmap_single(snic->pdev, pa, len, PCI_DMA_TODEVICE);
+		spin_unlock_irqrestore(&snic->wq_lock[q_num], flags);
+		atomic64_inc(&snic->s_stats.misc.wq_alloc_fail);
+		SNIC_DBG("host = %d, WQ is Full\n", snic->shost->host_no);
+
+		return -ENOMEM;
+	}
+
+	snic_queue_wq_eth_desc(&snic->wq[q_num], os_buf, pa, len, 0, 0, 1);
+	spin_unlock_irqrestore(&snic->wq_lock[q_num], flags);
+
+	/* Update stats */
+	act_reqs = atomic64_inc_return(&fwstats->actv_reqs);
+	if (act_reqs > atomic64_read(&fwstats->max_actv_reqs))
+		atomic64_set(&fwstats->max_actv_reqs, act_reqs);
+
+	return 0;
+} /* end of snic_queue_wq_desc() */
+
+/*
+ * snic_handle_untagged_req: Adds snic specific requests to spl_cmd_list.
+ * Purpose : Used during driver unload to clean up the requests.
+ */
+void
+snic_handle_untagged_req(struct snic *snic, struct snic_req_info *rqi)
+{
+	unsigned long flags;
+
+	INIT_LIST_HEAD(&rqi->list);
+
+	spin_lock_irqsave(&snic->spl_cmd_lock, flags);
+	list_add_tail(&rqi->list, &snic->spl_cmd_list);
+	spin_unlock_irqrestore(&snic->spl_cmd_lock, flags);
+}
+
+/*
+ * snic_req_init:
+ * Allocates snic_req_info + snic_host_req + sgl data, and initializes.
+ */
+struct snic_req_info *
+snic_req_init(struct snic *snic, int sg_cnt)
+{
+	u8 typ;
+	struct snic_req_info *rqi = NULL;
+
+	typ = (sg_cnt <= SNIC_REQ_CACHE_DFLT_SGL) ?
+		SNIC_REQ_CACHE_DFLT_SGL : SNIC_REQ_CACHE_MAX_SGL;
+
+	rqi = mempool_alloc(snic->req_pool[typ], GFP_ATOMIC);
+	if (!rqi) {
+		atomic64_inc(&snic->s_stats.io.alloc_fail);
+		SNIC_HOST_ERR(snic->shost,
+			      "Failed to allocate memory from snic req pool id = %d\n",
+			      typ);
+		return rqi;
+	}
+
+	memset(rqi, 0, sizeof(*rqi));
+	rqi->rq_pool_type = typ;
+	rqi->start_time = jiffies;
+	rqi->req = (struct snic_host_req *) (rqi + 1);
+	rqi->req_len = sizeof(struct snic_host_req);
+	rqi->snic = snic;
+
+	rqi->req = (struct snic_host_req *)(rqi + 1);
+
+	if (sg_cnt == 0)
+		goto end;
+
+	rqi->req_len += (sg_cnt * sizeof(struct snic_sg_desc));
+
+	if (sg_cnt > atomic64_read(&snic->s_stats.io.max_sgl))
+		atomic64_set(&snic->s_stats.io.max_sgl, sg_cnt);
+
+	SNIC_BUG_ON(sg_cnt > SNIC_MAX_SG_DESC_CNT);
+	atomic64_inc(&snic->s_stats.io.sgl_cnt[sg_cnt - 1]);
+
+end:
+	memset(rqi->req, 0, rqi->req_len);
+
+	/* pre initialization of init_ctx to support req_to_rqi */
+	rqi->req->hdr.init_ctx = (ulong) rqi;
+
+	SNIC_SCSI_DBG(snic->shost, "Req_alloc:rqi = %p allocatd.\n", rqi);
+
+	return rqi;
+} /* end of snic_req_init */
+
+/*
+ * snic_abort_req_init : Inits abort request.
+ */
+struct snic_host_req *
+snic_abort_req_init(struct snic *snic, struct snic_req_info *rqi)
+{
+	struct snic_host_req *req = NULL;
+
+	SNIC_BUG_ON(!rqi);
+
+	/* If abort to be issued second time, then reuse */
+	if (rqi->abort_req)
+		return rqi->abort_req;
+
+
+	req = mempool_alloc(snic->req_pool[SNIC_REQ_TM_CACHE], GFP_ATOMIC);
+	if (!req) {
+		SNIC_HOST_ERR(snic->shost, "abts:Failed to alloc tm req.\n");
+		WARN_ON_ONCE(1);
+
+		return NULL;
+	}
+
+	rqi->abort_req = req;
+	memset(req, 0, sizeof(struct snic_host_req));
+	/* pre initialization of init_ctx to support req_to_rqi */
+	req->hdr.init_ctx = (ulong) rqi;
+
+	return req;
+} /* end of snic_abort_req_init */
+
+/*
+ * snic_dr_req_init : Inits device reset req
+ */
+struct snic_host_req *
+snic_dr_req_init(struct snic *snic, struct snic_req_info *rqi)
+{
+	struct snic_host_req *req = NULL;
+
+	SNIC_BUG_ON(!rqi);
+
+	req = mempool_alloc(snic->req_pool[SNIC_REQ_TM_CACHE], GFP_ATOMIC);
+	if (!req) {
+		SNIC_HOST_ERR(snic->shost, "dr:Failed to alloc tm req.\n");
+		WARN_ON_ONCE(1);
+
+		return NULL;
+	}
+
+	SNIC_BUG_ON(rqi->dr_req != NULL);
+	rqi->dr_req = req;
+	memset(req, 0, sizeof(struct snic_host_req));
+	/* pre initialization of init_ctx to support req_to_rqi */
+	req->hdr.init_ctx = (ulong) rqi;
+
+	return req;
+} /* end of snic_dr_req_init */
+
+/* frees snic_req_info and snic_host_req */
+void
+snic_req_free(struct snic *snic, struct snic_req_info *rqi)
+{
+	SNIC_BUG_ON(rqi->req == rqi->abort_req);
+	SNIC_BUG_ON(rqi->req == rqi->dr_req);
+	SNIC_BUG_ON(rqi->sge_va != 0);
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "Req_free:rqi %p:ioreq %p:abt %p:dr %p\n",
+		      rqi, rqi->req, rqi->abort_req, rqi->dr_req);
+
+	if (rqi->abort_req)
+		mempool_free(rqi->abort_req, snic->req_pool[SNIC_REQ_TM_CACHE]);
+
+	if (rqi->dr_req)
+		mempool_free(rqi->dr_req, snic->req_pool[SNIC_REQ_TM_CACHE]);
+
+	mempool_free(rqi, snic->req_pool[rqi->rq_pool_type]);
+}
+
+void
+snic_pci_unmap_rsp_buf(struct snic *snic, struct snic_req_info *rqi)
+{
+	struct snic_sg_desc *sgd;
+
+	sgd = req_to_sgl(rqi_to_req(rqi));
+	SNIC_BUG_ON(sgd[0].addr == 0);
+	pci_unmap_single(snic->pdev,
+			 le64_to_cpu(sgd[0].addr),
+			 le32_to_cpu(sgd[0].len),
+			 PCI_DMA_FROMDEVICE);
+}
+
+/*
+ * snic_free_all_untagged_reqs: Walks through untagged reqs and frees them.
+ */
+void
+snic_free_all_untagged_reqs(struct snic *snic)
+{
+	struct snic_req_info *rqi;
+	struct list_head *cur, *nxt;
+	unsigned long flags;
+
+	spin_lock_irqsave(&snic->spl_cmd_lock, flags);
+	list_for_each_safe(cur, nxt, &snic->spl_cmd_list) {
+		rqi = list_entry(cur, struct snic_req_info, list);
+		list_del_init(&rqi->list);
+		if (rqi->sge_va) {
+			snic_pci_unmap_rsp_buf(snic, rqi);
+			kfree((void *)rqi->sge_va);
+			rqi->sge_va = 0;
+		}
+
+		snic_req_free(snic, rqi);
+	}
+	spin_unlock_irqrestore(&snic->spl_cmd_lock, flags);
+}
+
+/*
+ * snic_release_untagged_req : Unlinks the untagged req and frees it.
+ */
+void
+snic_release_untagged_req(struct snic *snic, struct snic_req_info *rqi)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&snic->snic_lock, flags);
+	if (snic->in_remove) {
+		spin_unlock_irqrestore(&snic->snic_lock, flags);
+		goto end;
+	}
+	spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+	spin_lock_irqsave(&snic->spl_cmd_lock, flags);
+	if (list_empty(&rqi->list)) {
+		spin_unlock_irqrestore(&snic->spl_cmd_lock, flags);
+		goto end;
+	}
+	list_del_init(&rqi->list);
+	spin_unlock_irqrestore(&snic->spl_cmd_lock, flags);
+	snic_req_free(snic, rqi);
+
+end:
+	return;
+}
+
+/* dump buf in hex fmt */
+void
+snic_hex_dump(char *pfx, char *data, int len)
+{
+	SNIC_INFO("%s Dumping Data of Len = %d\n", pfx, len);
+	print_hex_dump_bytes(pfx, DUMP_PREFIX_NONE, data, len);
+}
+
+#define	LINE_BUFSZ	128	/* for snic_print_desc fn */
+static void
+snic_dump_desc(const char *fn, char *os_buf, int len)
+{
+	struct snic_host_req *req = (struct snic_host_req *) os_buf;
+	struct snic_fw_req *fwreq = (struct snic_fw_req *) os_buf;
+	struct snic_req_info *rqi = NULL;
+	char line[LINE_BUFSZ] = { '\0' };
+	char *cmd_str = NULL;
+
+	if (req->hdr.type >= SNIC_RSP_REPORT_TGTS_CMPL)
+		rqi = (struct snic_req_info *) fwreq->hdr.init_ctx;
+	else
+		rqi = (struct snic_req_info *) req->hdr.init_ctx;
+
+	SNIC_BUG_ON(rqi == NULL || rqi->req == NULL);
+	switch (req->hdr.type) {
+	case SNIC_REQ_REPORT_TGTS:
+		cmd_str = "report-tgt : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_REQ_REPORT_TGTS :");
+		break;
+
+	case SNIC_REQ_ICMND:
+		cmd_str = "icmnd : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_REQ_ICMND : 0x%x :",
+			 req->u.icmnd.cdb[0]);
+		break;
+
+	case SNIC_REQ_ITMF:
+		cmd_str = "itmf : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_REQ_ITMF :");
+		break;
+
+	case SNIC_REQ_HBA_RESET:
+		cmd_str = "hba reset :";
+		snprintf(line, LINE_BUFSZ, "SNIC_REQ_HBA_RESET :");
+		break;
+
+	case SNIC_REQ_EXCH_VER:
+		cmd_str = "exch ver : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_REQ_EXCH_VER :");
+		break;
+
+	case SNIC_REQ_TGT_INFO:
+		cmd_str = "tgt info : ";
+		break;
+
+	case SNIC_RSP_REPORT_TGTS_CMPL:
+		cmd_str = "report tgt cmpl : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_RSP_REPORT_TGTS_CMPL :");
+		break;
+
+	case SNIC_RSP_ICMND_CMPL:
+		cmd_str = "icmnd_cmpl : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_RSP_ICMND_CMPL : 0x%x :",
+			 rqi->req->u.icmnd.cdb[0]);
+		break;
+
+	case SNIC_RSP_ITMF_CMPL:
+		cmd_str = "itmf_cmpl : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_RSP_ITMF_CMPL :");
+		break;
+
+	case SNIC_RSP_HBA_RESET_CMPL:
+		cmd_str = "hba_reset_cmpl : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_RSP_HBA_RESET_CMPL :");
+		break;
+
+	case SNIC_RSP_EXCH_VER_CMPL:
+		cmd_str = "exch_ver_cmpl : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_RSP_EXCH_VER_CMPL :");
+		break;
+
+	case SNIC_MSG_ACK:
+		cmd_str = "msg ack : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_MSG_ACK :");
+		break;
+
+	case SNIC_MSG_ASYNC_EVNOTIFY:
+		cmd_str = "async notify : ";
+		snprintf(line, LINE_BUFSZ, "SNIC_MSG_ASYNC_EVNOTIFY :");
+		break;
+
+	default:
+		cmd_str = "unknown : ";
+		SNIC_BUG_ON(1);
+		break;
+	}
+
+	SNIC_INFO("%s:%s >>cmndid=%x:sg_cnt = %x:status = %x:ctx = %lx.\n",
+		  fn, line, req->hdr.cmnd_id, req->hdr.sg_cnt, req->hdr.status,
+		  req->hdr.init_ctx);
+
+	/* Enable it, to dump byte stream */
+	if (snic_log_level & 0x20)
+		snic_hex_dump(cmd_str, os_buf, len);
+} /* end of __snic_print_desc */
+
+void
+snic_print_desc(const char *fn, char *os_buf, int len)
+{
+	if (snic_log_level & SNIC_DESC_LOGGING)
+		snic_dump_desc(fn, os_buf, len);
+}
+
+void
+snic_calc_io_process_time(struct snic *snic, struct snic_req_info *rqi)
+{
+	u64 duration;
+
+	duration = jiffies - rqi->start_time;
+
+	if (duration > atomic64_read(&snic->s_stats.io.max_time))
+		atomic64_set(&snic->s_stats.io.max_time, duration);
+}
diff --git a/drivers/scsi/snic/snic_io.h b/drivers/scsi/snic/snic_io.h
new file mode 100644
index 000000000000..093d6524cd42
--- /dev/null
+++ b/drivers/scsi/snic/snic_io.h
@@ -0,0 +1,118 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _SNIC_IO_H
+#define _SNIC_IO_H
+
+#define SNIC_DFLT_SG_DESC_CNT	32	/* Default descriptors for sgl */
+#define SNIC_MAX_SG_DESC_CNT	60	/* Max descriptor for sgl */
+#define SNIC_SG_DESC_ALIGN	16	/* Descriptor address alignment */
+
+/* SG descriptor for snic */
+struct snic_sg_desc {
+	__le64 addr;
+	__le32 len;
+	u32 _resvd;
+};
+
+struct snic_dflt_sgl {
+	struct snic_sg_desc sg_desc[SNIC_DFLT_SG_DESC_CNT];
+};
+
+struct snic_max_sgl {
+	struct snic_sg_desc sg_desc[SNIC_MAX_SG_DESC_CNT];
+};
+
+enum snic_req_cache_type {
+	SNIC_REQ_CACHE_DFLT_SGL = 0,	/* cache with default size sgl */
+	SNIC_REQ_CACHE_MAX_SGL,		/* cache with max size sgl */
+	SNIC_REQ_TM_CACHE,		/* cache for task mgmt reqs contains
+					   snic_host_req objects only*/
+	SNIC_REQ_MAX_CACHES		/* number of sgl caches */
+};
+
+/* Per IO internal state */
+struct snic_internal_io_state {
+	char	*rqi;
+	u64	flags;
+	u32	state;
+	u32	abts_status;	/* Abort completion status */
+	u32	lr_status;	/* device reset completion status */
+};
+
+/* IO state machine */
+enum snic_ioreq_state {
+	SNIC_IOREQ_NOT_INITED = 0,
+	SNIC_IOREQ_PENDING,
+	SNIC_IOREQ_ABTS_PENDING,
+	SNIC_IOREQ_ABTS_COMPLETE,
+	SNIC_IOREQ_LR_PENDING,
+	SNIC_IOREQ_LR_COMPLETE,
+	SNIC_IOREQ_COMPLETE,
+};
+
+struct snic;
+struct snic_host_req;
+
+/*
+ * snic_req_info : Contains info about IO, one per scsi command.
+ * Notes: Make sure that the structure is aligned to 16 B
+ * this helps in easy access to snic_req_info from snic_host_req
+ */
+struct snic_req_info {
+	struct list_head list;
+	struct snic_host_req *req;
+	u64	start_time;		/* start time in jiffies */
+	u16	rq_pool_type;		/* noticion of request pool type */
+	u16	req_len;		/* buf len passing to fw (req + sgl)*/
+	u32	tgt_id;
+
+	u32	tm_tag;
+	u8	io_cmpl:1;		/* sets to 1 when fw completes IO */
+	u8	resvd[3];
+	struct scsi_cmnd *sc;		/* Associated scsi cmd */
+	struct snic	*snic;		/* Associated snic */
+	ulong	sge_va;			/* Pointer to Resp Buffer */
+	u64	snsbuf_va;
+
+	struct snic_host_req *abort_req;
+	struct completion *abts_done;
+
+	struct snic_host_req *dr_req;
+	struct completion *dr_done;
+};
+
+
+#define rqi_to_req(rqi)	\
+	((struct snic_host_req *) (((struct snic_req_info *)rqi)->req))
+
+#define req_to_rqi(req)	\
+	((struct snic_req_info *) (((struct snic_host_req *)req)->hdr.init_ctx))
+
+#define req_to_sgl(req)	\
+	((struct snic_sg_desc *) (((struct snic_host_req *)req)+1))
+
+struct snic_req_info *
+snic_req_init(struct snic *, int sg_cnt);
+void snic_req_free(struct snic *, struct snic_req_info *);
+void snic_calc_io_process_time(struct snic *, struct snic_req_info *);
+void snic_pci_unmap_rsp_buf(struct snic *, struct snic_req_info *);
+struct snic_host_req *
+snic_abort_req_init(struct snic *, struct snic_req_info *);
+struct snic_host_req *
+snic_dr_req_init(struct snic *, struct snic_req_info *);
+#endif /* _SNIC_IO_H */
diff --git a/drivers/scsi/snic/snic_isr.c b/drivers/scsi/snic/snic_isr.c
new file mode 100644
index 000000000000..a85fae25ec8c
--- /dev/null
+++ b/drivers/scsi/snic/snic_isr.c
@@ -0,0 +1,204 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/string.h>
+#include <linux/errno.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+
+#include "vnic_dev.h"
+#include "vnic_intr.h"
+#include "vnic_stats.h"
+#include "snic_io.h"
+#include "snic.h"
+
+
+/*
+ * snic_isr_msix_wq : MSIx ISR for work queue.
+ */
+
+static irqreturn_t
+snic_isr_msix_wq(int irq, void *data)
+{
+	struct snic *snic = data;
+	unsigned long wq_work_done = 0;
+
+	snic->s_stats.misc.last_isr_time = jiffies;
+	atomic64_inc(&snic->s_stats.misc.isr_cnt);
+
+	wq_work_done = snic_wq_cmpl_handler(snic, -1);
+	svnic_intr_return_credits(&snic->intr[SNIC_MSIX_WQ],
+				  wq_work_done,
+				  1 /* unmask intr */,
+				  1 /* reset intr timer */);
+
+	return IRQ_HANDLED;
+} /* end of snic_isr_msix_wq */
+
+static irqreturn_t
+snic_isr_msix_io_cmpl(int irq, void *data)
+{
+	struct snic *snic = data;
+	unsigned long iocmpl_work_done = 0;
+
+	snic->s_stats.misc.last_isr_time = jiffies;
+	atomic64_inc(&snic->s_stats.misc.isr_cnt);
+
+	iocmpl_work_done = snic_fwcq_cmpl_handler(snic, -1);
+	svnic_intr_return_credits(&snic->intr[SNIC_MSIX_IO_CMPL],
+				  iocmpl_work_done,
+				  1 /* unmask intr */,
+				  1 /* reset intr timer */);
+
+	return IRQ_HANDLED;
+} /* end of snic_isr_msix_io_cmpl */
+
+static irqreturn_t
+snic_isr_msix_err_notify(int irq, void *data)
+{
+	struct snic *snic = data;
+
+	snic->s_stats.misc.last_isr_time = jiffies;
+	atomic64_inc(&snic->s_stats.misc.isr_cnt);
+
+	svnic_intr_return_all_credits(&snic->intr[SNIC_MSIX_ERR_NOTIFY]);
+	snic_log_q_error(snic);
+
+	/*Handling link events */
+	snic_handle_link_event(snic);
+
+	return IRQ_HANDLED;
+} /* end of snic_isr_msix_err_notify */
+
+
+void
+snic_free_intr(struct snic *snic)
+{
+	int i;
+
+	/* ONLY interrupt mode MSIX is supported */
+	for (i = 0; i < ARRAY_SIZE(snic->msix); i++) {
+		if (snic->msix[i].requested) {
+			free_irq(snic->msix_entry[i].vector,
+				 snic->msix[i].devid);
+		}
+	}
+} /* end of snic_free_intr */
+
+int
+snic_request_intr(struct snic *snic)
+{
+	int ret = 0, i;
+	enum vnic_dev_intr_mode intr_mode;
+
+	intr_mode = svnic_dev_get_intr_mode(snic->vdev);
+	SNIC_BUG_ON(intr_mode != VNIC_DEV_INTR_MODE_MSIX);
+
+	/*
+	 * Currently HW supports single WQ and CQ. So passing devid as snic.
+	 * When hardware supports multiple WQs and CQs, one idea is
+	 * to pass devid as corresponding WQ or CQ ptr and retrieve snic
+	 * from queue ptr.
+	 * Except for err_notify, which is always one.
+	 */
+	sprintf(snic->msix[SNIC_MSIX_WQ].devname,
+		"%.11s-scsi-wq",
+		snic->name);
+	snic->msix[SNIC_MSIX_WQ].isr = snic_isr_msix_wq;
+	snic->msix[SNIC_MSIX_WQ].devid = snic;
+
+	sprintf(snic->msix[SNIC_MSIX_IO_CMPL].devname,
+		"%.11s-io-cmpl",
+		snic->name);
+	snic->msix[SNIC_MSIX_IO_CMPL].isr = snic_isr_msix_io_cmpl;
+	snic->msix[SNIC_MSIX_IO_CMPL].devid = snic;
+
+	sprintf(snic->msix[SNIC_MSIX_ERR_NOTIFY].devname,
+		"%.11s-err-notify",
+		snic->name);
+	snic->msix[SNIC_MSIX_ERR_NOTIFY].isr = snic_isr_msix_err_notify;
+	snic->msix[SNIC_MSIX_ERR_NOTIFY].devid = snic;
+
+	for (i = 0; i < ARRAY_SIZE(snic->msix); i++) {
+		ret = request_irq(snic->msix_entry[i].vector,
+				  snic->msix[i].isr,
+				  0,
+				  snic->msix[i].devname,
+				  snic->msix[i].devid);
+		if (ret) {
+			SNIC_HOST_ERR(snic->shost,
+				      "MSI-X: requrest_irq(%d) failed %d\n",
+				      i,
+				      ret);
+			snic_free_intr(snic);
+			break;
+		}
+		snic->msix[i].requested = 1;
+	}
+
+	return ret;
+} /* end of snic_requrest_intr */
+
+int
+snic_set_intr_mode(struct snic *snic)
+{
+	unsigned int n = ARRAY_SIZE(snic->wq);
+	unsigned int m = SNIC_CQ_IO_CMPL_MAX;
+	unsigned int i;
+
+	/*
+	 * We need n WQs, m CQs, and n+m+1 INTRs
+	 * (last INTR is used for WQ/CQ errors and notification area
+	 */
+
+	BUILD_BUG_ON((ARRAY_SIZE(snic->wq) + SNIC_CQ_IO_CMPL_MAX) >
+			ARRAY_SIZE(snic->intr));
+	SNIC_BUG_ON(ARRAY_SIZE(snic->msix_entry) < (n + m + 1));
+
+	for (i = 0; i < (n + m + 1); i++)
+		snic->msix_entry[i].entry = i;
+
+	if (snic->wq_count >= n && snic->cq_count >= (n + m)) {
+		if (!pci_enable_msix(snic->pdev,
+				     snic->msix_entry,
+				     (n + m + 1))) {
+			snic->wq_count = n;
+			snic->cq_count = n + m;
+			snic->intr_count = n + m + 1;
+			snic->err_intr_offset = SNIC_MSIX_ERR_NOTIFY;
+
+			SNIC_ISR_DBG(snic->shost,
+				     "Using MSI-X Interrupts\n");
+			svnic_dev_set_intr_mode(snic->vdev,
+						VNIC_DEV_INTR_MODE_MSIX);
+
+			return 0;
+		}
+	}
+
+	svnic_dev_set_intr_mode(snic->vdev, VNIC_DEV_INTR_MODE_UNKNOWN);
+
+	return -EINVAL;
+} /* end of snic_set_intr_mode */
+
+void
+snic_clear_intr_mode(struct snic *snic)
+{
+	pci_disable_msix(snic->pdev);
+
+	svnic_dev_set_intr_mode(snic->vdev, VNIC_DEV_INTR_MODE_INTX);
+}
diff --git a/drivers/scsi/snic/snic_main.c b/drivers/scsi/snic/snic_main.c
new file mode 100644
index 000000000000..b2b87cef00fc
--- /dev/null
+++ b/drivers/scsi/snic/snic_main.c
@@ -0,0 +1,1044 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/module.h>
+#include <linux/mempool.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/skbuff.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <scsi/scsi_host.h>
+#include <scsi/scsi_tcq.h>
+
+#include "snic.h"
+#include "snic_fwint.h"
+
+#define PCI_DEVICE_ID_CISCO_SNIC	0x0046
+
+/* Supported devices by snic module */
+static struct pci_device_id snic_id_table[] = {
+	{PCI_DEVICE(0x1137, PCI_DEVICE_ID_CISCO_SNIC) },
+	{ 0, }	/* end of table */
+};
+
+unsigned int snic_log_level = 0x0;
+module_param(snic_log_level, int, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(snic_log_level, "bitmask for snic logging levels");
+
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+unsigned int snic_trace_max_pages = 16;
+module_param(snic_trace_max_pages, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(snic_trace_max_pages,
+		"Total allocated memory pages for snic trace buffer");
+
+#endif
+unsigned int snic_max_qdepth = SNIC_DFLT_QUEUE_DEPTH;
+module_param(snic_max_qdepth, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(snic_max_qdepth, "Queue depth to report for each LUN");
+
+/*
+ * snic_slave_alloc : callback function to SCSI Mid Layer, called on
+ * scsi device initialization.
+ */
+static int
+snic_slave_alloc(struct scsi_device *sdev)
+{
+	struct snic_tgt *tgt = starget_to_tgt(scsi_target(sdev));
+
+	if (!tgt || snic_tgt_chkready(tgt))
+		return -ENXIO;
+
+	return 0;
+}
+
+/*
+ * snic_slave_configure : callback function to SCSI Mid Layer, called on
+ * scsi device initialization.
+ */
+static int
+snic_slave_configure(struct scsi_device *sdev)
+{
+	struct snic *snic = shost_priv(sdev->host);
+	u32 qdepth = 0, max_ios = 0;
+	int tmo = SNIC_DFLT_CMD_TIMEOUT * HZ;
+
+	/* Set Queue Depth */
+	max_ios = snic_max_qdepth;
+	qdepth = min_t(u32, max_ios, SNIC_MAX_QUEUE_DEPTH);
+	scsi_change_queue_depth(sdev, qdepth);
+
+	if (snic->fwinfo.io_tmo > 1)
+		tmo = snic->fwinfo.io_tmo * HZ;
+
+	/* FW requires extended timeouts */
+	blk_queue_rq_timeout(sdev->request_queue, tmo);
+
+	return 0;
+}
+
+static int
+snic_change_queue_depth(struct scsi_device *sdev, int qdepth)
+{
+	int qsz = 0;
+
+	qsz = min_t(u32, qdepth, SNIC_MAX_QUEUE_DEPTH);
+	scsi_change_queue_depth(sdev, qsz);
+	SNIC_INFO("QDepth Changed to %d\n", sdev->queue_depth);
+
+	return sdev->queue_depth;
+}
+
+static struct scsi_host_template snic_host_template = {
+	.module = THIS_MODULE,
+	.name = SNIC_DRV_NAME,
+	.queuecommand = snic_queuecommand,
+	.eh_abort_handler = snic_abort_cmd,
+	.eh_device_reset_handler = snic_device_reset,
+	.eh_host_reset_handler = snic_host_reset,
+	.slave_alloc = snic_slave_alloc,
+	.slave_configure = snic_slave_configure,
+	.change_queue_depth = snic_change_queue_depth,
+	.this_id = -1,
+	.cmd_per_lun = SNIC_DFLT_QUEUE_DEPTH,
+	.can_queue = SNIC_MAX_IO_REQ,
+	.use_clustering = ENABLE_CLUSTERING,
+	.sg_tablesize = SNIC_MAX_SG_DESC_CNT,
+	.max_sectors = 0x800,
+	.shost_attrs = snic_attrs,
+	.use_blk_tags = 1,
+	.track_queue_depth = 1,
+	.cmd_size = sizeof(struct snic_internal_io_state),
+	.proc_name = "snic_scsi",
+};
+
+/*
+ * snic_handle_link_event : Handles link events such as link up/down/error
+ */
+void
+snic_handle_link_event(struct snic *snic)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&snic->snic_lock, flags);
+	if (snic->stop_link_events) {
+		spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+		return;
+	}
+	spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+	queue_work(snic_glob->event_q, &snic->link_work);
+} /* end of snic_handle_link_event */
+
+/*
+ * snic_notify_set : sets notification area
+ * This notification area is to receive events from fw
+ * Note: snic supports only MSIX interrupts, in which we can just call
+ *  svnic_dev_notify_set directly
+ */
+static int
+snic_notify_set(struct snic *snic)
+{
+	int ret = 0;
+	enum vnic_dev_intr_mode intr_mode;
+
+	intr_mode = svnic_dev_get_intr_mode(snic->vdev);
+
+	if (intr_mode == VNIC_DEV_INTR_MODE_MSIX) {
+		ret = svnic_dev_notify_set(snic->vdev, SNIC_MSIX_ERR_NOTIFY);
+	} else {
+		SNIC_HOST_ERR(snic->shost,
+			      "Interrupt mode should be setup before devcmd notify set %d\n",
+			      intr_mode);
+		ret = -1;
+	}
+
+	return ret;
+} /* end of snic_notify_set */
+
+/*
+ * snic_dev_wait : polls vnic open status.
+ */
+static int
+snic_dev_wait(struct vnic_dev *vdev,
+		int (*start)(struct vnic_dev *, int),
+		int (*finished)(struct vnic_dev *, int *),
+		int arg)
+{
+	unsigned long time;
+	int ret, done;
+	int retry_cnt = 0;
+
+	ret = start(vdev, arg);
+	if (ret)
+		return ret;
+
+	/*
+	 * Wait for func to complete...2 seconds max.
+	 *
+	 * Sometimes schedule_timeout_uninterruptible take long	time
+	 * to wakeup, which results skipping retry. The retry counter
+	 * ensures to retry at least two times.
+	 */
+	time = jiffies + (HZ * 2);
+	do {
+		ret = finished(vdev, &done);
+		if (ret)
+			return ret;
+
+		if (done)
+			return 0;
+		schedule_timeout_uninterruptible(HZ/10);
+		++retry_cnt;
+	} while (time_after(time, jiffies) || (retry_cnt < 3));
+
+	return -ETIMEDOUT;
+} /* end of snic_dev_wait */
+
+/*
+ * snic_cleanup: called by snic_remove
+ * Stops the snic device, masks all interrupts, Completed CQ entries are
+ * drained. Posted WQ/RQ/Copy-WQ entries are cleanup
+ */
+static int
+snic_cleanup(struct snic *snic)
+{
+	unsigned int i;
+	int ret;
+
+	svnic_dev_disable(snic->vdev);
+	for (i = 0; i < snic->intr_count; i++)
+		svnic_intr_mask(&snic->intr[i]);
+
+	for (i = 0; i < snic->wq_count; i++) {
+		ret = svnic_wq_disable(&snic->wq[i]);
+		if (ret)
+			return ret;
+	}
+
+	/* Clean up completed IOs */
+	snic_fwcq_cmpl_handler(snic, -1);
+
+	snic_wq_cmpl_handler(snic, -1);
+
+	/* Clean up the IOs that have not completed */
+	for (i = 0; i < snic->wq_count; i++)
+		svnic_wq_clean(&snic->wq[i], snic_free_wq_buf);
+
+	for (i = 0; i < snic->cq_count; i++)
+		svnic_cq_clean(&snic->cq[i]);
+
+	for (i = 0; i < snic->intr_count; i++)
+		svnic_intr_clean(&snic->intr[i]);
+
+	/* Cleanup snic specific requests */
+	snic_free_all_untagged_reqs(snic);
+
+	/* Cleanup Pending SCSI commands */
+	snic_shutdown_scsi_cleanup(snic);
+
+	for (i = 0; i < SNIC_REQ_MAX_CACHES; i++)
+		mempool_destroy(snic->req_pool[i]);
+
+	return 0;
+} /* end of snic_cleanup */
+
+
+static void
+snic_iounmap(struct snic *snic)
+{
+	if (snic->bar0.vaddr)
+		iounmap(snic->bar0.vaddr);
+}
+
+/*
+ * snic_vdev_open_done : polls for svnic_dev_open cmd completion.
+ */
+static int
+snic_vdev_open_done(struct vnic_dev *vdev, int *done)
+{
+	struct snic *snic = svnic_dev_priv(vdev);
+	int ret;
+	int nretries = 5;
+
+	do {
+		ret = svnic_dev_open_done(vdev, done);
+		if (ret == 0)
+			break;
+
+		SNIC_HOST_INFO(snic->shost, "VNIC_DEV_OPEN Timedout.\n");
+	} while (nretries--);
+
+	return ret;
+} /* end of snic_vdev_open_done */
+
+/*
+ * snic_add_host : registers scsi host with ML
+ */
+static int
+snic_add_host(struct Scsi_Host *shost, struct pci_dev *pdev)
+{
+	int ret = 0;
+
+	ret = scsi_add_host(shost, &pdev->dev);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "snic: scsi_add_host failed. %d\n",
+			      ret);
+
+		return ret;
+	}
+
+	SNIC_BUG_ON(shost->work_q != NULL);
+	snprintf(shost->work_q_name, sizeof(shost->work_q_name), "scsi_wq_%d",
+		 shost->host_no);
+	shost->work_q = create_singlethread_workqueue(shost->work_q_name);
+	if (!shost->work_q) {
+		SNIC_HOST_ERR(shost, "Failed to Create ScsiHost wq.\n");
+
+		ret = -ENOMEM;
+	}
+
+	return ret;
+} /* end of snic_add_host */
+
+static void
+snic_del_host(struct Scsi_Host *shost)
+{
+	if (!shost->work_q)
+		return;
+
+	destroy_workqueue(shost->work_q);
+	shost->work_q = NULL;
+	scsi_remove_host(shost);
+}
+
+int
+snic_get_state(struct snic *snic)
+{
+	return atomic_read(&snic->state);
+}
+
+void
+snic_set_state(struct snic *snic, enum snic_state state)
+{
+	SNIC_HOST_INFO(snic->shost, "snic state change from %s to %s\n",
+		       snic_state_to_str(snic_get_state(snic)),
+		       snic_state_to_str(state));
+
+	atomic_set(&snic->state, state);
+}
+
+/*
+ * snic_probe : Initialize the snic interface.
+ */
+static int
+snic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct Scsi_Host *shost;
+	struct snic *snic;
+	mempool_t *pool;
+	unsigned long flags;
+	u32 max_ios = 0;
+	int ret, i;
+
+	/* Device Information */
+	SNIC_INFO("snic device %4x:%4x:%4x:%4x: ",
+		  pdev->vendor, pdev->device, pdev->subsystem_vendor,
+		  pdev->subsystem_device);
+
+	SNIC_INFO("snic device bus %x: slot %x: fn %x\n",
+		  pdev->bus->number, PCI_SLOT(pdev->devfn),
+		  PCI_FUNC(pdev->devfn));
+
+	/*
+	 * Allocate SCSI Host and setup association between host, and snic
+	 */
+	shost = scsi_host_alloc(&snic_host_template, sizeof(struct snic));
+	if (!shost) {
+		SNIC_ERR("Unable to alloc scsi_host\n");
+		ret = -ENOMEM;
+
+		goto prob_end;
+	}
+	snic = shost_priv(shost);
+	snic->shost = shost;
+
+	snprintf(snic->name, sizeof(snic->name) - 1, "%s%d", SNIC_DRV_NAME,
+		 shost->host_no);
+
+	SNIC_HOST_INFO(shost,
+		       "snic%d = %p shost = %p device bus %x: slot %x: fn %x\n",
+		       shost->host_no, snic, shost, pdev->bus->number,
+		       PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+	/* Per snic debugfs init */
+	ret = snic_stats_debugfs_init(snic);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "Failed to initialize debugfs stats\n");
+		snic_stats_debugfs_remove(snic);
+	}
+#endif
+
+	/* Setup PCI Resources */
+	pci_set_drvdata(pdev, snic);
+	snic->pdev = pdev;
+
+	ret = pci_enable_device(pdev);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Cannot enable PCI Resources, aborting : %d\n",
+			      ret);
+
+		goto err_free_snic;
+	}
+
+	ret = pci_request_regions(pdev, SNIC_DRV_NAME);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Cannot obtain PCI Resources, aborting : %d\n",
+			      ret);
+
+		goto err_pci_disable;
+	}
+
+	pci_set_master(pdev);
+
+	/*
+	 * Query PCI Controller on system for DMA addressing
+	 * limitation for the device. Try 43-bit first, and
+	 * fail to 32-bit.
+	 */
+	ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(43));
+	if (ret) {
+		ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (ret) {
+			SNIC_HOST_ERR(shost,
+				      "No Usable DMA Configuration, aborting %d\n",
+				      ret);
+
+			goto err_rel_regions;
+		}
+
+		ret = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (ret) {
+			SNIC_HOST_ERR(shost,
+				      "Unable to obtain 32-bit DMA for consistent allocations, aborting: %d\n",
+				      ret);
+
+			goto err_rel_regions;
+		}
+	} else {
+		ret = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(43));
+		if (ret) {
+			SNIC_HOST_ERR(shost,
+				      "Unable to obtain 43-bit DMA for consistent allocations. aborting: %d\n",
+				      ret);
+
+			goto err_rel_regions;
+		}
+	}
+
+
+	/* Map vNIC resources from BAR0 */
+	if (!(pci_resource_flags(pdev, 0) & IORESOURCE_MEM)) {
+		SNIC_HOST_ERR(shost, "BAR0 not memory mappable aborting.\n");
+
+		ret = -ENODEV;
+		goto err_rel_regions;
+	}
+
+	snic->bar0.vaddr = pci_iomap(pdev, 0, 0);
+	if (!snic->bar0.vaddr) {
+		SNIC_HOST_ERR(shost,
+			      "Cannot memory map BAR0 res hdr aborting.\n");
+
+		ret = -ENODEV;
+		goto err_rel_regions;
+	}
+
+	snic->bar0.bus_addr = pci_resource_start(pdev, 0);
+	snic->bar0.len = pci_resource_len(pdev, 0);
+	SNIC_BUG_ON(snic->bar0.bus_addr == 0);
+
+	/* Devcmd2 Resource Allocation and Initialization */
+	snic->vdev = svnic_dev_alloc_discover(NULL, snic, pdev, &snic->bar0, 1);
+	if (!snic->vdev) {
+		SNIC_HOST_ERR(shost, "vNIC Resource Discovery Failed.\n");
+
+		ret = -ENODEV;
+		goto err_iounmap;
+	}
+
+	ret = svnic_dev_cmd_init(snic->vdev, 0);
+	if (ret) {
+		SNIC_HOST_INFO(shost, "Devcmd2 Init Failed. err = %d\n", ret);
+
+		goto err_vnic_unreg;
+	}
+
+	ret = snic_dev_wait(snic->vdev, svnic_dev_open, snic_vdev_open_done, 0);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "vNIC dev open failed, aborting. %d\n",
+			      ret);
+
+		goto err_vnic_unreg;
+	}
+
+	ret = svnic_dev_init(snic->vdev, 0);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "vNIC dev init failed. aborting. %d\n",
+			      ret);
+
+		goto err_dev_close;
+	}
+
+	/* Get vNIC information */
+	ret = snic_get_vnic_config(snic);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Get vNIC configuration failed, aborting. %d\n",
+			      ret);
+
+		goto err_dev_close;
+	}
+
+	/* Configure Maximum Outstanding IO reqs */
+	max_ios = snic->config.io_throttle_count;
+	if (max_ios != SNIC_UCSM_DFLT_THROTTLE_CNT_BLD)
+		shost->can_queue = min_t(u32, SNIC_MAX_IO_REQ,
+					 max_t(u32, SNIC_MIN_IO_REQ, max_ios));
+
+	snic->max_tag_id = shost->can_queue;
+
+	ret = scsi_init_shared_tag_map(shost, snic->max_tag_id);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Unable to alloc shared tag map. %d\n",
+			      ret);
+
+		goto err_dev_close;
+	}
+
+	shost->max_lun = snic->config.luns_per_tgt;
+	shost->max_id = SNIC_MAX_TARGET;
+
+	shost->max_cmd_len = MAX_COMMAND_SIZE; /*defined in scsi_cmnd.h*/
+
+	snic_get_res_counts(snic);
+
+	/*
+	 * Assumption: Only MSIx is supported
+	 */
+	ret = snic_set_intr_mode(snic);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Failed to set intr mode aborting. %d\n",
+			      ret);
+
+		goto err_dev_close;
+	}
+
+	ret = snic_alloc_vnic_res(snic);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Failed to alloc vNIC resources aborting. %d\n",
+			      ret);
+
+		goto err_clear_intr;
+	}
+
+	/* Initialize specific lists */
+	INIT_LIST_HEAD(&snic->list);
+
+	/*
+	 * spl_cmd_list for maintaining snic specific cmds
+	 * such as EXCH_VER_REQ, REPORT_TARGETS etc
+	 */
+	INIT_LIST_HEAD(&snic->spl_cmd_list);
+	spin_lock_init(&snic->spl_cmd_lock);
+
+	/* initialize all snic locks */
+	spin_lock_init(&snic->snic_lock);
+
+	for (i = 0; i < SNIC_WQ_MAX; i++)
+		spin_lock_init(&snic->wq_lock[i]);
+
+	for (i = 0; i < SNIC_IO_LOCKS; i++)
+		spin_lock_init(&snic->io_req_lock[i]);
+
+	pool = mempool_create_slab_pool(2,
+				snic_glob->req_cache[SNIC_REQ_CACHE_DFLT_SGL]);
+	if (!pool) {
+		SNIC_HOST_ERR(shost, "dflt sgl pool creation failed\n");
+
+		goto err_free_res;
+	}
+
+	snic->req_pool[SNIC_REQ_CACHE_DFLT_SGL] = pool;
+
+	pool = mempool_create_slab_pool(2,
+				snic_glob->req_cache[SNIC_REQ_CACHE_MAX_SGL]);
+	if (!pool) {
+		SNIC_HOST_ERR(shost, "max sgl pool creation failed\n");
+
+		goto err_free_dflt_sgl_pool;
+	}
+
+	snic->req_pool[SNIC_REQ_CACHE_MAX_SGL] = pool;
+
+	pool = mempool_create_slab_pool(2,
+				snic_glob->req_cache[SNIC_REQ_TM_CACHE]);
+	if (!pool) {
+		SNIC_HOST_ERR(shost, "snic tmreq info pool creation failed.\n");
+
+		goto err_free_max_sgl_pool;
+	}
+
+	snic->req_pool[SNIC_REQ_TM_CACHE] = pool;
+
+	/* Initialize snic state */
+	atomic_set(&snic->state, SNIC_INIT);
+
+	atomic_set(&snic->ios_inflight, 0);
+
+	/* Setup notification buffer area */
+	ret = snic_notify_set(snic);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Failed to alloc notify buffer aborting. %d\n",
+			      ret);
+
+		goto err_free_tmreq_pool;
+	}
+
+	/*
+	 * Initialization done with PCI system, hardware, firmware.
+	 * Add shost to SCSI
+	 */
+	ret = snic_add_host(shost, pdev);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Adding scsi host Failed ... exiting. %d\n",
+			      ret);
+
+		goto err_notify_unset;
+	}
+
+	spin_lock_irqsave(&snic_glob->snic_list_lock, flags);
+	list_add_tail(&snic->list, &snic_glob->snic_list);
+	spin_unlock_irqrestore(&snic_glob->snic_list_lock, flags);
+
+	snic_disc_init(&snic->disc);
+	INIT_WORK(&snic->tgt_work, snic_handle_tgt_disc);
+	INIT_WORK(&snic->disc_work, snic_handle_disc);
+	INIT_WORK(&snic->link_work, snic_handle_link);
+
+	/* Enable all queues */
+	for (i = 0; i < snic->wq_count; i++)
+		svnic_wq_enable(&snic->wq[i]);
+
+	ret = svnic_dev_enable_wait(snic->vdev);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "vNIC dev enable failed w/ error %d\n",
+			      ret);
+
+		goto err_vdev_enable;
+	}
+
+	ret = snic_request_intr(snic);
+	if (ret) {
+		SNIC_HOST_ERR(shost, "Unable to request irq. %d\n", ret);
+
+		goto err_req_intr;
+	}
+
+	for (i = 0; i < snic->intr_count; i++)
+		svnic_intr_unmask(&snic->intr[i]);
+
+	snic_set_state(snic, SNIC_ONLINE);
+
+	/* Get snic params */
+	ret = snic_get_conf(snic);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "Failed to get snic io config from FW w err %d\n",
+			      ret);
+
+		goto err_get_conf;
+	}
+
+	ret = snic_disc_start(snic);
+	if (ret) {
+		SNIC_HOST_ERR(shost, "snic_probe:Discovery Failed w err = %d\n",
+			      ret);
+
+		goto err_get_conf;
+	}
+
+	SNIC_HOST_INFO(shost, "SNIC Device Probe Successful.\n");
+
+	return 0;
+
+err_get_conf:
+	snic_free_all_untagged_reqs(snic);
+
+	for (i = 0; i < snic->intr_count; i++)
+		svnic_intr_mask(&snic->intr[i]);
+
+	snic_free_intr(snic);
+
+err_req_intr:
+	svnic_dev_disable(snic->vdev);
+
+err_vdev_enable:
+	for (i = 0; i < snic->wq_count; i++) {
+		int rc = 0;
+
+		rc = svnic_wq_disable(&snic->wq[i]);
+		if (rc) {
+			SNIC_HOST_ERR(shost,
+				      "WQ Disable Failed w/ err = %d\n", rc);
+
+			 break;
+		}
+	}
+	snic_del_host(snic->shost);
+
+err_notify_unset:
+	svnic_dev_notify_unset(snic->vdev);
+
+err_free_tmreq_pool:
+	mempool_destroy(snic->req_pool[SNIC_REQ_TM_CACHE]);
+
+err_free_max_sgl_pool:
+	mempool_destroy(snic->req_pool[SNIC_REQ_CACHE_MAX_SGL]);
+
+err_free_dflt_sgl_pool:
+	mempool_destroy(snic->req_pool[SNIC_REQ_CACHE_DFLT_SGL]);
+
+err_free_res:
+	snic_free_vnic_res(snic);
+
+err_clear_intr:
+	snic_clear_intr_mode(snic);
+
+err_dev_close:
+	svnic_dev_close(snic->vdev);
+
+err_vnic_unreg:
+	svnic_dev_unregister(snic->vdev);
+
+err_iounmap:
+	snic_iounmap(snic);
+
+err_rel_regions:
+	pci_release_regions(pdev);
+
+err_pci_disable:
+	pci_disable_device(pdev);
+
+err_free_snic:
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+	snic_stats_debugfs_remove(snic);
+#endif
+	scsi_host_put(shost);
+	pci_set_drvdata(pdev, NULL);
+
+prob_end:
+	SNIC_INFO("sNIC device : bus %d: slot %d: fn %d Registration Failed.\n",
+		  pdev->bus->number, PCI_SLOT(pdev->devfn),
+		  PCI_FUNC(pdev->devfn));
+
+	return ret;
+} /* end of snic_probe */
+
+
+/*
+ * snic_remove : invoked on unbinding the interface to cleanup the
+ * resources allocated in snic_probe on initialization.
+ */
+static void
+snic_remove(struct pci_dev *pdev)
+{
+	struct snic *snic = pci_get_drvdata(pdev);
+	unsigned long flags;
+
+	if (!snic) {
+		SNIC_INFO("sNIC dev: bus %d slot %d fn %d snic inst is null.\n",
+			  pdev->bus->number, PCI_SLOT(pdev->devfn),
+			  PCI_FUNC(pdev->devfn));
+
+		return;
+	}
+
+	/*
+	 * Mark state so that the workqueue thread stops forwarding
+	 * received frames and link events. ISR and other threads
+	 * that can queue work items will also stop creating work
+	 * items on the snic workqueue
+	 */
+	snic_set_state(snic, SNIC_OFFLINE);
+	spin_lock_irqsave(&snic->snic_lock, flags);
+	snic->stop_link_events = 1;
+	spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+	flush_workqueue(snic_glob->event_q);
+	snic_disc_term(snic);
+
+	spin_lock_irqsave(&snic->snic_lock, flags);
+	snic->in_remove = 1;
+	spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+	/*
+	 * This stops the snic device, masks all interrupts, Completed
+	 * CQ entries are drained. Posted WQ/RQ/Copy-WQ entries are
+	 * cleanup
+	 */
+	snic_cleanup(snic);
+
+	spin_lock_irqsave(&snic_glob->snic_list_lock, flags);
+	list_del(&snic->list);
+	spin_unlock_irqrestore(&snic_glob->snic_list_lock, flags);
+
+	snic_tgt_del_all(snic);
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+	snic_stats_debugfs_remove(snic);
+#endif
+	snic_del_host(snic->shost);
+
+	svnic_dev_notify_unset(snic->vdev);
+	snic_free_intr(snic);
+	snic_free_vnic_res(snic);
+	snic_clear_intr_mode(snic);
+	svnic_dev_close(snic->vdev);
+	svnic_dev_unregister(snic->vdev);
+	snic_iounmap(snic);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+
+	/* this frees Scsi_Host and snic memory (continuous chunk) */
+	scsi_host_put(snic->shost);
+} /* end of snic_remove */
+
+
+struct snic_global *snic_glob;
+
+/*
+ * snic_global_data_init: Initialize SNIC Global Data
+ * Notes: All the global lists, variables should be part of global data
+ * this helps in debugging.
+ */
+static int
+snic_global_data_init(void)
+{
+	int ret = 0;
+	struct kmem_cache *cachep;
+	ssize_t len = 0;
+
+	snic_glob = kzalloc(sizeof(*snic_glob), GFP_KERNEL);
+
+	if (!snic_glob) {
+		SNIC_ERR("Failed to allocate Global Context.\n");
+
+		ret = -ENOMEM;
+		goto gdi_end;
+	}
+
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+	/* Debugfs related Initialization */
+	/* Create debugfs entries for snic */
+	ret = snic_debugfs_init();
+	if (ret < 0) {
+		SNIC_ERR("Failed to create sysfs dir for tracing and stats.\n");
+		snic_debugfs_term();
+		/* continue even if it fails */
+	}
+
+	/* Trace related Initialization */
+	/* Allocate memory for trace buffer */
+	ret = snic_trc_init();
+	if (ret < 0) {
+		SNIC_ERR("Trace buffer init failed, SNIC tracing disabled\n");
+		snic_trc_free();
+		/* continue even if it fails */
+	}
+
+#endif
+	INIT_LIST_HEAD(&snic_glob->snic_list);
+	spin_lock_init(&snic_glob->snic_list_lock);
+
+	/* Create a cache for allocation of snic_host_req+default size ESGLs */
+	len = sizeof(struct snic_req_info);
+	len += sizeof(struct snic_host_req) + sizeof(struct snic_dflt_sgl);
+	cachep = kmem_cache_create("snic_req_dfltsgl", len, SNIC_SG_DESC_ALIGN,
+				   SLAB_HWCACHE_ALIGN, NULL);
+	if (!cachep) {
+		SNIC_ERR("Failed to create snic default sgl slab\n");
+		ret = -ENOMEM;
+
+		goto err_dflt_req_slab;
+	}
+	snic_glob->req_cache[SNIC_REQ_CACHE_DFLT_SGL] = cachep;
+
+	/* Create a cache for allocation of max size Extended SGLs */
+	len = sizeof(struct snic_req_info);
+	len += sizeof(struct snic_host_req) + sizeof(struct snic_max_sgl);
+	cachep = kmem_cache_create("snic_req_maxsgl", len, SNIC_SG_DESC_ALIGN,
+				   SLAB_HWCACHE_ALIGN, NULL);
+	if (!cachep) {
+		SNIC_ERR("Failed to create snic max sgl slab\n");
+		ret = -ENOMEM;
+
+		goto err_max_req_slab;
+	}
+	snic_glob->req_cache[SNIC_REQ_CACHE_MAX_SGL] = cachep;
+
+	len = sizeof(struct snic_host_req);
+	cachep = kmem_cache_create("snic_req_maxsgl", len, SNIC_SG_DESC_ALIGN,
+				   SLAB_HWCACHE_ALIGN, NULL);
+	if (!cachep) {
+		SNIC_ERR("Failed to create snic tm req slab\n");
+		ret = -ENOMEM;
+
+		goto err_tmreq_slab;
+	}
+	snic_glob->req_cache[SNIC_REQ_TM_CACHE] = cachep;
+
+	/* snic_event queue */
+	snic_glob->event_q = create_singlethread_workqueue("snic_event_wq");
+	if (!snic_glob->event_q) {
+		SNIC_ERR("snic event queue create failed\n");
+		ret = -ENOMEM;
+
+		goto err_eventq;
+	}
+
+	return ret;
+
+err_eventq:
+	kmem_cache_destroy(snic_glob->req_cache[SNIC_REQ_TM_CACHE]);
+
+err_tmreq_slab:
+	kmem_cache_destroy(snic_glob->req_cache[SNIC_REQ_CACHE_MAX_SGL]);
+
+err_max_req_slab:
+	kmem_cache_destroy(snic_glob->req_cache[SNIC_REQ_CACHE_DFLT_SGL]);
+
+err_dflt_req_slab:
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+	snic_trc_free();
+	snic_debugfs_term();
+#endif
+	kfree(snic_glob);
+	snic_glob = NULL;
+
+gdi_end:
+	return ret;
+} /* end of snic_glob_init */
+
+/*
+ * snic_global_data_cleanup : Frees SNIC Global Data
+ */
+static void
+snic_global_data_cleanup(void)
+{
+	SNIC_BUG_ON(snic_glob == NULL);
+
+	destroy_workqueue(snic_glob->event_q);
+	kmem_cache_destroy(snic_glob->req_cache[SNIC_REQ_TM_CACHE]);
+	kmem_cache_destroy(snic_glob->req_cache[SNIC_REQ_CACHE_MAX_SGL]);
+	kmem_cache_destroy(snic_glob->req_cache[SNIC_REQ_CACHE_DFLT_SGL]);
+
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+	/* Freeing Trace Resources */
+	snic_trc_free();
+
+	/* Freeing Debugfs Resources */
+	snic_debugfs_term();
+#endif
+	kfree(snic_glob);
+	snic_glob = NULL;
+} /* end of snic_glob_cleanup */
+
+static struct pci_driver snic_driver = {
+	.name = SNIC_DRV_NAME,
+	.id_table = snic_id_table,
+	.probe = snic_probe,
+	.remove = snic_remove,
+};
+
+static int __init
+snic_init_module(void)
+{
+	int ret = 0;
+
+#ifndef __x86_64__
+	SNIC_INFO("SNIC Driver is supported only for x86_64 platforms!\n");
+	add_taint(TAINT_CPU_OUT_OF_SPEC, LOCKDEP_STILL_OK);
+#endif
+
+	SNIC_INFO("%s, ver %s\n", SNIC_DRV_DESCRIPTION, SNIC_DRV_VERSION);
+
+	ret = snic_global_data_init();
+	if (ret) {
+		SNIC_ERR("Failed to Initialize Global Data.\n");
+
+		return ret;
+	}
+
+	ret = pci_register_driver(&snic_driver);
+	if (ret < 0) {
+		SNIC_ERR("PCI driver register error\n");
+
+		goto err_pci_reg;
+	}
+
+	return ret;
+
+err_pci_reg:
+	snic_global_data_cleanup();
+
+	return ret;
+}
+
+static void __exit
+snic_cleanup_module(void)
+{
+	pci_unregister_driver(&snic_driver);
+	snic_global_data_cleanup();
+}
+
+module_init(snic_init_module);
+module_exit(snic_cleanup_module);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION(SNIC_DRV_DESCRIPTION);
+MODULE_VERSION(SNIC_DRV_VERSION);
+MODULE_DEVICE_TABLE(pci, snic_id_table);
+MODULE_AUTHOR("Narsimhulu Musini <nmusini@cisco.com>, "
+	      "Sesidhar Baddela <sebaddel@cisco.com>");
diff --git a/drivers/scsi/snic/snic_res.c b/drivers/scsi/snic/snic_res.c
new file mode 100644
index 000000000000..b54912c8ca0c
--- /dev/null
+++ b/drivers/scsi/snic/snic_res.c
@@ -0,0 +1,295 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/errno.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+
+#include "wq_enet_desc.h"
+#include "cq_enet_desc.h"
+#include "vnic_resource.h"
+#include "vnic_dev.h"
+#include "vnic_wq.h"
+#include "vnic_cq.h"
+#include "vnic_intr.h"
+#include "vnic_stats.h"
+#include "snic.h"
+
+int
+snic_get_vnic_config(struct snic *snic)
+{
+	struct vnic_snic_config *c = &snic->config;
+	int ret;
+
+#define GET_CONFIG(m) \
+	do { \
+		ret = svnic_dev_spec(snic->vdev, \
+				     offsetof(struct vnic_snic_config, m), \
+				     sizeof(c->m), \
+				     &c->m); \
+		if (ret) { \
+			SNIC_HOST_ERR(snic->shost, \
+				      "Error getting %s, %d\n", #m, ret); \
+			return ret; \
+		} \
+	} while (0)
+
+	GET_CONFIG(wq_enet_desc_count);
+	GET_CONFIG(maxdatafieldsize);
+	GET_CONFIG(intr_timer);
+	GET_CONFIG(intr_timer_type);
+	GET_CONFIG(flags);
+	GET_CONFIG(io_throttle_count);
+	GET_CONFIG(port_down_timeout);
+	GET_CONFIG(port_down_io_retries);
+	GET_CONFIG(luns_per_tgt);
+	GET_CONFIG(xpt_type);
+	GET_CONFIG(hid);
+
+	c->wq_enet_desc_count = min_t(u32,
+				      VNIC_SNIC_WQ_DESCS_MAX,
+				      max_t(u32,
+					    VNIC_SNIC_WQ_DESCS_MIN,
+					    c->wq_enet_desc_count));
+
+	c->wq_enet_desc_count = ALIGN(c->wq_enet_desc_count, 16);
+
+	c->maxdatafieldsize = min_t(u32,
+				    VNIC_SNIC_MAXDATAFIELDSIZE_MAX,
+				    max_t(u32,
+					  VNIC_SNIC_MAXDATAFIELDSIZE_MIN,
+					  c->maxdatafieldsize));
+
+	c->io_throttle_count = min_t(u32,
+				     VNIC_SNIC_IO_THROTTLE_COUNT_MAX,
+				     max_t(u32,
+					   VNIC_SNIC_IO_THROTTLE_COUNT_MIN,
+					   c->io_throttle_count));
+
+	c->port_down_timeout = min_t(u32,
+				     VNIC_SNIC_PORT_DOWN_TIMEOUT_MAX,
+				     c->port_down_timeout);
+
+	c->port_down_io_retries = min_t(u32,
+				     VNIC_SNIC_PORT_DOWN_IO_RETRIES_MAX,
+				     c->port_down_io_retries);
+
+	c->luns_per_tgt = min_t(u32,
+				VNIC_SNIC_LUNS_PER_TARGET_MAX,
+				max_t(u32,
+				      VNIC_SNIC_LUNS_PER_TARGET_MIN,
+				      c->luns_per_tgt));
+
+	c->intr_timer = min_t(u32, VNIC_INTR_TIMER_MAX, c->intr_timer);
+
+	SNIC_INFO("vNIC resources wq %d\n", c->wq_enet_desc_count);
+	SNIC_INFO("vNIC mtu %d intr timer %d\n",
+		  c->maxdatafieldsize,
+		  c->intr_timer);
+
+	SNIC_INFO("vNIC flags 0x%x luns per tgt %d\n",
+		  c->flags,
+		  c->luns_per_tgt);
+
+	SNIC_INFO("vNIC io throttle count %d\n", c->io_throttle_count);
+	SNIC_INFO("vNIC port down timeout %d port down io retries %d\n",
+		  c->port_down_timeout,
+		  c->port_down_io_retries);
+
+	SNIC_INFO("vNIC back end type = %d\n", c->xpt_type);
+	SNIC_INFO("vNIC hid = %d\n", c->hid);
+
+	return 0;
+}
+
+void
+snic_get_res_counts(struct snic *snic)
+{
+	snic->wq_count = svnic_dev_get_res_count(snic->vdev, RES_TYPE_WQ);
+	SNIC_BUG_ON(snic->wq_count == 0);
+	snic->cq_count = svnic_dev_get_res_count(snic->vdev, RES_TYPE_CQ);
+	SNIC_BUG_ON(snic->cq_count == 0);
+	snic->intr_count = svnic_dev_get_res_count(snic->vdev,
+						  RES_TYPE_INTR_CTRL);
+	SNIC_BUG_ON(snic->intr_count == 0);
+}
+
+void
+snic_free_vnic_res(struct snic *snic)
+{
+	unsigned int i;
+
+	for (i = 0; i < snic->wq_count; i++)
+		svnic_wq_free(&snic->wq[i]);
+
+	for (i = 0; i < snic->cq_count; i++)
+		svnic_cq_free(&snic->cq[i]);
+
+	for (i = 0; i < snic->intr_count; i++)
+		svnic_intr_free(&snic->intr[i]);
+}
+
+int
+snic_alloc_vnic_res(struct snic *snic)
+{
+	enum vnic_dev_intr_mode intr_mode;
+	unsigned int mask_on_assertion;
+	unsigned int intr_offset;
+	unsigned int err_intr_enable;
+	unsigned int err_intr_offset;
+	unsigned int i;
+	int ret;
+
+	intr_mode = svnic_dev_get_intr_mode(snic->vdev);
+
+	SNIC_INFO("vNIC interrupt mode: %s\n",
+		  ((intr_mode == VNIC_DEV_INTR_MODE_INTX) ?
+		   "Legacy PCI INTx" :
+		   ((intr_mode == VNIC_DEV_INTR_MODE_MSI) ?
+		    "MSI" :
+		    ((intr_mode == VNIC_DEV_INTR_MODE_MSIX) ?
+		     "MSI-X" : "Unknown"))));
+
+	/* only MSI-X is supported */
+	SNIC_BUG_ON(intr_mode != VNIC_DEV_INTR_MODE_MSIX);
+
+	SNIC_INFO("wq %d cq %d intr %d\n", snic->wq_count,
+		  snic->cq_count,
+		  snic->intr_count);
+
+
+	/* Allocate WQs used for SCSI IOs */
+	for (i = 0; i < snic->wq_count; i++) {
+		ret = svnic_wq_alloc(snic->vdev,
+				     &snic->wq[i],
+				     i,
+				     snic->config.wq_enet_desc_count,
+				     sizeof(struct wq_enet_desc));
+		if (ret)
+			goto error_cleanup;
+	}
+
+	/* CQ for each WQ */
+	for (i = 0; i < snic->wq_count; i++) {
+		ret = svnic_cq_alloc(snic->vdev,
+				     &snic->cq[i],
+				     i,
+				     snic->config.wq_enet_desc_count,
+				     sizeof(struct cq_enet_wq_desc));
+		if (ret)
+			goto error_cleanup;
+	}
+
+	SNIC_BUG_ON(snic->cq_count != 2 * snic->wq_count);
+	/* CQ for FW TO host */
+	for (i = snic->wq_count; i < snic->cq_count; i++) {
+		ret = svnic_cq_alloc(snic->vdev,
+				     &snic->cq[i],
+				     i,
+				     (snic->config.wq_enet_desc_count * 3),
+				     sizeof(struct snic_fw_req));
+		if (ret)
+			goto error_cleanup;
+	}
+
+	for (i = 0; i < snic->intr_count; i++) {
+		ret = svnic_intr_alloc(snic->vdev, &snic->intr[i], i);
+		if (ret)
+			goto error_cleanup;
+	}
+
+	/*
+	 * Init WQ Resources.
+	 * WQ[0 to n] points to CQ[0 to n-1]
+	 * firmware to host comm points to CQ[n to m+1]
+	 */
+	err_intr_enable = 1;
+	err_intr_offset = snic->err_intr_offset;
+
+	for (i = 0; i < snic->wq_count; i++) {
+		svnic_wq_init(&snic->wq[i],
+			      i,
+			      err_intr_enable,
+			      err_intr_offset);
+	}
+
+	for (i = 0; i < snic->cq_count; i++) {
+		intr_offset = i;
+
+		svnic_cq_init(&snic->cq[i],
+			      0 /* flow_control_enable */,
+			      1 /* color_enable */,
+			      0 /* cq_head */,
+			      0 /* cq_tail */,
+			      1 /* cq_tail_color */,
+			      1 /* interrupt_enable */,
+			      1 /* cq_entry_enable */,
+			      0 /* cq_message_enable */,
+			      intr_offset,
+			      0 /* cq_message_addr */);
+	}
+
+	/*
+	 * Init INTR resources
+	 * Assumption : snic is always in MSI-X mode
+	 */
+	SNIC_BUG_ON(intr_mode != VNIC_DEV_INTR_MODE_MSIX);
+	mask_on_assertion = 1;
+
+	for (i = 0; i < snic->intr_count; i++) {
+		svnic_intr_init(&snic->intr[i],
+				snic->config.intr_timer,
+				snic->config.intr_timer_type,
+				mask_on_assertion);
+	}
+
+	/* init the stats memory by making the first call here */
+	ret = svnic_dev_stats_dump(snic->vdev, &snic->stats);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "svnic_dev_stats_dump failed - x%x\n",
+			      ret);
+		goto error_cleanup;
+	}
+
+	/* Clear LIF stats */
+	svnic_dev_stats_clear(snic->vdev);
+	ret = 0;
+
+	return ret;
+
+error_cleanup:
+	snic_free_vnic_res(snic);
+
+	return ret;
+}
+
+void
+snic_log_q_error(struct snic *snic)
+{
+	unsigned int i;
+	u32 err_status;
+
+	for (i = 0; i < snic->wq_count; i++) {
+		err_status = ioread32(&snic->wq[i].ctrl->error_status);
+		if (err_status)
+			SNIC_HOST_ERR(snic->shost,
+				      "WQ[%d] error status %d\n",
+				      i,
+				      err_status);
+	}
+} /* end of snic_log_q_error */
diff --git a/drivers/scsi/snic/snic_res.h b/drivers/scsi/snic/snic_res.h
new file mode 100644
index 000000000000..273f72f2a023
--- /dev/null
+++ b/drivers/scsi/snic/snic_res.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __SNIC_RES_H
+#define __SNIC_RES_H
+
+#include "snic_io.h"
+#include "wq_enet_desc.h"
+#include "vnic_wq.h"
+#include "snic_fwint.h"
+#include "vnic_cq_fw.h"
+
+static inline void
+snic_icmnd_init(struct snic_host_req *req, u32 cmnd_id, u32 host_id, u64 ctx,
+		u16 flags, u64 tgt_id, u8 *lun, u8 *scsi_cdb, u8 cdb_len,
+		u32 data_len, u16 sg_cnt, ulong sgl_addr,
+		dma_addr_t sns_addr_pa, u32 sense_len)
+{
+	snic_io_hdr_enc(&req->hdr, SNIC_REQ_ICMND, 0, cmnd_id, host_id, sg_cnt,
+			ctx);
+
+	req->u.icmnd.flags = cpu_to_le16(flags);
+	req->u.icmnd.tgt_id = cpu_to_le64(tgt_id);
+	memcpy(&req->u.icmnd.lun_id, lun, LUN_ADDR_LEN);
+	req->u.icmnd.cdb_len = cdb_len;
+	memset(req->u.icmnd.cdb, 0, SNIC_CDB_LEN);
+	memcpy(req->u.icmnd.cdb, scsi_cdb, cdb_len);
+	req->u.icmnd.data_len = cpu_to_le32(data_len);
+	req->u.icmnd.sg_addr = cpu_to_le64(sgl_addr);
+	req->u.icmnd.sense_len = cpu_to_le32(sense_len);
+	req->u.icmnd.sense_addr = cpu_to_le64(sns_addr_pa);
+}
+
+static inline void
+snic_itmf_init(struct snic_host_req *req, u32 cmnd_id, u32 host_id, ulong ctx,
+	       u16 flags, u32 req_id, u64 tgt_id, u8 *lun, u8 tm_type)
+{
+	snic_io_hdr_enc(&req->hdr, SNIC_REQ_ITMF, 0, cmnd_id, host_id, 0, ctx);
+
+	req->u.itmf.tm_type = tm_type;
+	req->u.itmf.flags = cpu_to_le16(flags);
+	/* req_id valid only in abort, clear task */
+	req->u.itmf.req_id = cpu_to_le32(req_id);
+	req->u.itmf.tgt_id = cpu_to_le64(tgt_id);
+	memcpy(&req->u.itmf.lun_id, lun, LUN_ADDR_LEN);
+}
+
+static inline void
+snic_queue_wq_eth_desc(struct vnic_wq *wq,
+		       void *os_buf,
+		       dma_addr_t dma_addr,
+		       unsigned int len,
+		       int vlan_tag_insert,
+		       unsigned int vlan_tag,
+		       int cq_entry)
+{
+	struct wq_enet_desc *desc = svnic_wq_next_desc(wq);
+
+	wq_enet_desc_enc(desc,
+			(u64)dma_addr | VNIC_PADDR_TARGET,
+			(u16)len,
+			0, /* mss_or_csum_offset */
+			0, /* fc_eof */
+			0, /* offload mode */
+			1, /* eop */
+			(u8)cq_entry,
+			0, /* fcoe_encap */
+			(u8)vlan_tag_insert,
+			(u16)vlan_tag,
+			0 /* loopback */);
+
+	svnic_wq_post(wq, os_buf, dma_addr, len, 1, 1);
+}
+
+struct snic;
+
+int snic_get_vnic_config(struct snic *);
+int snic_alloc_vnic_res(struct snic *);
+void snic_free_vnic_res(struct snic *);
+void snic_get_res_counts(struct snic *);
+void snic_log_q_error(struct snic *);
+int snic_get_vnic_resources_size(struct snic *);
+#endif /* __SNIC_RES_H */
diff --git a/drivers/scsi/snic/snic_scsi.c b/drivers/scsi/snic/snic_scsi.c
new file mode 100644
index 000000000000..2c7b4c321cbe
--- /dev/null
+++ b/drivers/scsi/snic/snic_scsi.c
@@ -0,0 +1,2632 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/mempool.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/workqueue.h>
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/gfp.h>
+#include <scsi/scsi.h>
+#include <scsi/scsi_host.h>
+#include <scsi/scsi_device.h>
+#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_tcq.h>
+#include <scsi/scsi_dbg.h>
+
+#include "snic_io.h"
+#include "snic.h"
+
+#define snic_cmd_tag(sc)	(((struct scsi_cmnd *) sc)->request->tag)
+
+const char *snic_state_str[] = {
+	[SNIC_INIT]	= "SNIC_INIT",
+	[SNIC_ERROR]	= "SNIC_ERROR",
+	[SNIC_ONLINE]	= "SNIC_ONLINE",
+	[SNIC_OFFLINE]	= "SNIC_OFFLINE",
+	[SNIC_FWRESET]	= "SNIC_FWRESET",
+};
+
+static const char * const snic_req_state_str[] = {
+	[SNIC_IOREQ_NOT_INITED]	= "SNIC_IOREQ_NOT_INITED",
+	[SNIC_IOREQ_PENDING]	= "SNIC_IOREQ_PENDING",
+	[SNIC_IOREQ_ABTS_PENDING] = "SNIC_IOREQ_ABTS_PENDING",
+	[SNIC_IOREQ_ABTS_COMPLETE] = "SNIC_IOREQ_ABTS_COMPELTE",
+	[SNIC_IOREQ_LR_PENDING]	= "SNIC_IOREQ_LR_PENDING",
+	[SNIC_IOREQ_LR_COMPLETE] = "SNIC_IOREQ_LR_COMPELTE",
+	[SNIC_IOREQ_COMPLETE]	= "SNIC_IOREQ_CMD_COMPELTE",
+};
+
+/* snic cmd status strings */
+static const char * const snic_io_status_str[] = {
+	[SNIC_STAT_IO_SUCCESS]	= "SNIC_STAT_IO_SUCCESS", /* 0x0 */
+	[SNIC_STAT_INVALID_HDR] = "SNIC_STAT_INVALID_HDR",
+	[SNIC_STAT_OUT_OF_RES]	= "SNIC_STAT_OUT_OF_RES",
+	[SNIC_STAT_INVALID_PARM] = "SNIC_STAT_INVALID_PARM",
+	[SNIC_STAT_REQ_NOT_SUP]	= "SNIC_STAT_REQ_NOT_SUP",
+	[SNIC_STAT_IO_NOT_FOUND] = "SNIC_STAT_IO_NOT_FOUND",
+	[SNIC_STAT_ABORTED]	= "SNIC_STAT_ABORTED",
+	[SNIC_STAT_TIMEOUT]	= "SNIC_STAT_TIMEOUT",
+	[SNIC_STAT_SGL_INVALID] = "SNIC_STAT_SGL_INVALID",
+	[SNIC_STAT_DATA_CNT_MISMATCH] = "SNIC_STAT_DATA_CNT_MISMATCH",
+	[SNIC_STAT_FW_ERR]	= "SNIC_STAT_FW_ERR",
+	[SNIC_STAT_ITMF_REJECT] = "SNIC_STAT_ITMF_REJECT",
+	[SNIC_STAT_ITMF_FAIL]	= "SNIC_STAT_ITMF_FAIL",
+	[SNIC_STAT_ITMF_INCORRECT_LUN] = "SNIC_STAT_ITMF_INCORRECT_LUN",
+	[SNIC_STAT_CMND_REJECT] = "SNIC_STAT_CMND_REJECT",
+	[SNIC_STAT_DEV_OFFLINE] = "SNIC_STAT_DEV_OFFLINE",
+	[SNIC_STAT_NO_BOOTLUN]	= "SNIC_STAT_NO_BOOTLUN",
+	[SNIC_STAT_SCSI_ERR]	= "SNIC_STAT_SCSI_ERR",
+	[SNIC_STAT_NOT_READY]	= "SNIC_STAT_NOT_READY",
+	[SNIC_STAT_FATAL_ERROR]	= "SNIC_STAT_FATAL_ERROR",
+};
+
+static void snic_scsi_cleanup(struct snic *, int);
+
+const char *
+snic_state_to_str(unsigned int state)
+{
+	if (state >= ARRAY_SIZE(snic_state_str) || !snic_state_str[state])
+		return "Unknown";
+
+	return snic_state_str[state];
+}
+
+static const char *
+snic_io_status_to_str(unsigned int state)
+{
+	if ((state >= ARRAY_SIZE(snic_io_status_str)) ||
+	     (!snic_io_status_str[state]))
+		return "Unknown";
+
+	return snic_io_status_str[state];
+}
+
+static const char *
+snic_ioreq_state_to_str(unsigned int state)
+{
+	if (state >= ARRAY_SIZE(snic_req_state_str) ||
+			!snic_req_state_str[state])
+		return "Unknown";
+
+	return snic_req_state_str[state];
+}
+
+static inline spinlock_t *
+snic_io_lock_hash(struct snic *snic, struct scsi_cmnd *sc)
+{
+	u32 hash = snic_cmd_tag(sc) & (SNIC_IO_LOCKS - 1);
+
+	return &snic->io_req_lock[hash];
+}
+
+static inline spinlock_t *
+snic_io_lock_tag(struct snic *snic, int tag)
+{
+	return &snic->io_req_lock[tag & (SNIC_IO_LOCKS - 1)];
+}
+
+/* snic_release_req_buf : Releases snic_req_info */
+static void
+snic_release_req_buf(struct snic *snic,
+		   struct snic_req_info *rqi,
+		   struct scsi_cmnd *sc)
+{
+	struct snic_host_req *req = rqi_to_req(rqi);
+
+	/* Freeing cmd without marking completion, not okay */
+	SNIC_BUG_ON(!((CMD_STATE(sc) == SNIC_IOREQ_COMPLETE) ||
+		      (CMD_STATE(sc) == SNIC_IOREQ_ABTS_COMPLETE) ||
+		      (CMD_FLAGS(sc) & SNIC_DEV_RST_NOTSUP) ||
+		      (CMD_FLAGS(sc) & SNIC_IO_INTERNAL_TERM_ISSUED) ||
+		      (CMD_FLAGS(sc) & SNIC_DEV_RST_TERM_ISSUED) ||
+		      (CMD_FLAGS(sc) & SNIC_SCSI_CLEANUP) ||
+		      (CMD_STATE(sc) == SNIC_IOREQ_LR_COMPLETE)));
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "Rel_req:sc %p:tag %x:rqi %p:ioreq %p:abt %p:dr %p: state %s:flags 0x%llx\n",
+		      sc, snic_cmd_tag(sc), rqi, rqi->req, rqi->abort_req,
+		      rqi->dr_req, snic_ioreq_state_to_str(CMD_STATE(sc)),
+		      CMD_FLAGS(sc));
+
+	if (req->u.icmnd.sense_addr)
+		pci_unmap_single(snic->pdev,
+				 le64_to_cpu(req->u.icmnd.sense_addr),
+				 SCSI_SENSE_BUFFERSIZE,
+				 PCI_DMA_FROMDEVICE);
+
+	scsi_dma_unmap(sc);
+
+	snic_req_free(snic, rqi);
+} /* end of snic_release_req_buf */
+
+/*
+ * snic_queue_icmnd_req : Queues snic_icmnd request
+ */
+static int
+snic_queue_icmnd_req(struct snic *snic,
+		     struct snic_req_info *rqi,
+		     struct scsi_cmnd *sc,
+		     int sg_cnt)
+{
+	struct scatterlist *sg;
+	struct snic_sg_desc *sgd;
+	dma_addr_t pa = 0;
+	struct scsi_lun lun;
+	u16 flags = 0;
+	int ret = 0;
+	unsigned int i;
+
+	if (sg_cnt) {
+		flags = SNIC_ICMND_ESGL;
+		sgd = (struct snic_sg_desc *) req_to_sgl(rqi->req);
+
+		for_each_sg(scsi_sglist(sc), sg, sg_cnt, i) {
+			sgd->addr = cpu_to_le64(sg_dma_address(sg));
+			sgd->len = cpu_to_le32(sg_dma_len(sg));
+			sgd->_resvd = 0;
+			sgd++;
+		}
+	}
+
+	pa = pci_map_single(snic->pdev,
+			    sc->sense_buffer,
+			    SCSI_SENSE_BUFFERSIZE,
+			    PCI_DMA_FROMDEVICE);
+
+	if (pci_dma_mapping_error(snic->pdev, pa)) {
+		SNIC_HOST_ERR(snic->shost,
+			      "QIcmnd:PCI Map Failed for sns buf %p tag %x\n",
+			      sc->sense_buffer, snic_cmd_tag(sc));
+		ret = -ENOMEM;
+
+		return ret;
+	}
+
+	int_to_scsilun(sc->device->lun, &lun);
+	if (sc->sc_data_direction == DMA_FROM_DEVICE)
+		flags |= SNIC_ICMND_RD;
+	if (sc->sc_data_direction == DMA_TO_DEVICE)
+		flags |= SNIC_ICMND_WR;
+
+	/* Initialize icmnd */
+	snic_icmnd_init(rqi->req,
+			snic_cmd_tag(sc),
+			snic->config.hid, /* hid */
+			(ulong) rqi,
+			flags, /* command flags */
+			rqi->tgt_id,
+			lun.scsi_lun,
+			sc->cmnd,
+			sc->cmd_len,
+			scsi_bufflen(sc),
+			sg_cnt,
+			(ulong) req_to_sgl(rqi->req),
+			pa, /* sense buffer pa */
+			SCSI_SENSE_BUFFERSIZE);
+
+	ret = snic_queue_wq_desc(snic, rqi->req, rqi->req_len);
+	if (ret)
+		SNIC_HOST_ERR(snic->shost,
+			      "QIcmnd: Queuing Icmnd Failed. ret = %d\n",
+			      ret);
+
+	return ret;
+} /* end of snic_queue_icmnd_req */
+
+/*
+ * snic_issue_scsi_req : Prepares IO request and Issues to FW.
+ */
+static int
+snic_issue_scsi_req(struct snic *snic,
+		      struct snic_tgt *tgt,
+		      struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = NULL;
+	int sg_cnt = 0;
+	int ret = 0;
+	u32 tag = snic_cmd_tag(sc);
+	u64 cmd_trc = 0, cmd_st_flags = 0;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+
+	CMD_STATE(sc) = SNIC_IOREQ_NOT_INITED;
+	CMD_FLAGS(sc) = SNIC_NO_FLAGS;
+	sg_cnt = scsi_dma_map(sc);
+	if (sg_cnt < 0) {
+		SNIC_TRC((u16)snic->shost->host_no, tag, (ulong) sc, 0,
+			 sc->cmnd[0], sg_cnt, CMD_STATE(sc));
+
+		SNIC_HOST_ERR(snic->shost, "issue_sc:Failed to map SG List.\n");
+		ret = -ENOMEM;
+
+		goto issue_sc_end;
+	}
+
+	rqi = snic_req_init(snic, sg_cnt);
+	if (!rqi) {
+		scsi_dma_unmap(sc);
+		ret = -ENOMEM;
+
+		goto issue_sc_end;
+	}
+
+	rqi->tgt_id = tgt->id;
+	rqi->sc = sc;
+
+	CMD_STATE(sc) = SNIC_IOREQ_PENDING;
+	CMD_SP(sc) = (char *) rqi;
+	cmd_trc = SNIC_TRC_CMD(sc);
+	CMD_FLAGS(sc) |= (SNIC_IO_INITIALIZED | SNIC_IO_ISSUED);
+	cmd_st_flags = SNIC_TRC_CMD_STATE_FLAGS(sc);
+	io_lock = snic_io_lock_hash(snic, sc);
+
+	/* create wq desc and enqueue it */
+	ret = snic_queue_icmnd_req(snic, rqi, sc, sg_cnt);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "issue_sc: icmnd qing Failed for sc %p, err %d\n",
+			      sc, ret);
+
+		spin_lock_irqsave(io_lock, flags);
+		rqi = (struct snic_req_info *) CMD_SP(sc);
+		CMD_SP(sc) = NULL;
+		CMD_STATE(sc) = SNIC_IOREQ_COMPLETE;
+		CMD_FLAGS(sc) &= ~SNIC_IO_ISSUED; /* turn off the flag */
+		spin_unlock_irqrestore(io_lock, flags);
+
+		if (rqi)
+			snic_release_req_buf(snic, rqi, sc);
+
+		SNIC_TRC(snic->shost->host_no, tag, (ulong) sc, 0, 0, 0,
+			 SNIC_TRC_CMD_STATE_FLAGS(sc));
+	} else {
+		u32 io_sz = scsi_bufflen(sc) >> 9;
+		u32 qtime = jiffies - rqi->start_time;
+		struct snic_io_stats *iostats = &snic->s_stats.io;
+
+		if (io_sz > atomic64_read(&iostats->max_io_sz))
+			atomic64_set(&iostats->max_io_sz, io_sz);
+
+		if (qtime > atomic64_read(&iostats->max_qtime))
+			atomic64_set(&iostats->max_qtime, qtime);
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "issue_sc:sc %p, tag %d queued to WQ.\n",
+			      sc, tag);
+
+		SNIC_TRC(snic->shost->host_no, tag, (ulong) sc, (ulong) rqi,
+			 sg_cnt, cmd_trc, cmd_st_flags);
+	}
+
+issue_sc_end:
+
+	return ret;
+} /* end of snic_issue_scsi_req */
+
+
+/*
+ * snic_queuecommand
+ * Routine to send a scsi cdb to LLD
+ * Called with host_lock held and interrupts disabled
+ */
+int
+snic_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *sc)
+{
+	struct snic_tgt *tgt = NULL;
+	struct snic *snic = shost_priv(shost);
+	int ret;
+
+	tgt = starget_to_tgt(scsi_target(sc->device));
+	ret = snic_tgt_chkready(tgt);
+	if (ret) {
+		SNIC_HOST_ERR(shost, "Tgt %p id %d Not Ready.\n", tgt, tgt->id);
+		atomic64_inc(&snic->s_stats.misc.tgt_not_rdy);
+		sc->result = ret;
+		sc->scsi_done(sc);
+
+		return 0;
+	}
+
+	if (snic_get_state(snic) != SNIC_ONLINE) {
+		SNIC_HOST_ERR(shost, "snic state is %s\n",
+			      snic_state_str[snic_get_state(snic)]);
+
+		return SCSI_MLQUEUE_HOST_BUSY;
+	}
+	atomic_inc(&snic->ios_inflight);
+
+	SNIC_SCSI_DBG(shost, "sc %p Tag %d (sc %0x) lun %lld in snic_qcmd\n",
+		      sc, snic_cmd_tag(sc), sc->cmnd[0], sc->device->lun);
+
+	memset(scsi_cmd_priv(sc), 0, sizeof(struct snic_internal_io_state));
+
+	ret = snic_issue_scsi_req(snic, tgt, sc);
+	if (ret) {
+		SNIC_HOST_ERR(shost, "Failed to Q, Scsi Req w/ err %d.\n", ret);
+		ret = SCSI_MLQUEUE_HOST_BUSY;
+	} else
+		snic_stats_update_active_ios(&snic->s_stats);
+
+	atomic_dec(&snic->ios_inflight);
+
+	return ret;
+} /* end of snic_queuecommand */
+
+/*
+ * snic_process_abts_pending_state:
+ * caller should hold IO lock
+ */
+static void
+snic_proc_tmreq_pending_state(struct snic *snic,
+			      struct scsi_cmnd *sc,
+			      u8 cmpl_status)
+{
+	int state = CMD_STATE(sc);
+
+	if (state == SNIC_IOREQ_ABTS_PENDING)
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_PENDING;
+	else if (state == SNIC_IOREQ_LR_PENDING)
+		CMD_FLAGS(sc) |= SNIC_DEV_RST_PENDING;
+	else
+		SNIC_BUG_ON(1);
+
+	switch (cmpl_status) {
+	case SNIC_STAT_IO_SUCCESS:
+		CMD_FLAGS(sc) |= SNIC_IO_DONE;
+		break;
+
+	case SNIC_STAT_ABORTED:
+		CMD_FLAGS(sc) |= SNIC_IO_ABORTED;
+		break;
+
+	default:
+		SNIC_BUG_ON(1);
+	}
+}
+
+/*
+ * snic_process_io_failed_state:
+ * Processes IO's error states
+ */
+static void
+snic_process_io_failed_state(struct snic *snic,
+			     struct snic_icmnd_cmpl *icmnd_cmpl,
+			     struct scsi_cmnd *sc,
+			     u8 cmpl_stat)
+{
+	int res = 0;
+
+	switch (cmpl_stat) {
+	case SNIC_STAT_TIMEOUT:		/* Req was timedout */
+		atomic64_inc(&snic->s_stats.misc.io_tmo);
+		res = DID_TIME_OUT;
+		break;
+
+	case SNIC_STAT_ABORTED:		/* Req was aborted */
+		atomic64_inc(&snic->s_stats.misc.io_aborted);
+		res = DID_ABORT;
+		break;
+
+	case SNIC_STAT_DATA_CNT_MISMATCH:/* Recv/Sent more/less data than exp */
+		atomic64_inc(&snic->s_stats.misc.data_cnt_mismat);
+		scsi_set_resid(sc, le32_to_cpu(icmnd_cmpl->resid));
+		res = DID_ERROR;
+		break;
+
+	case SNIC_STAT_OUT_OF_RES: /* Out of resources to complete request */
+		atomic64_inc(&snic->s_stats.fw.out_of_res);
+		res = DID_REQUEUE;
+		break;
+
+	case SNIC_STAT_IO_NOT_FOUND:	/* Requested I/O was not found */
+		atomic64_inc(&snic->s_stats.io.io_not_found);
+		res = DID_ERROR;
+		break;
+
+	case SNIC_STAT_SGL_INVALID:	/* Req was aborted to due to sgl error*/
+		atomic64_inc(&snic->s_stats.misc.sgl_inval);
+		res = DID_ERROR;
+		break;
+
+	case SNIC_STAT_FW_ERR:		/* Req terminated due to FW Error */
+		atomic64_inc(&snic->s_stats.fw.io_errs);
+		res = DID_ERROR;
+		break;
+
+	case SNIC_STAT_SCSI_ERR:	/* FW hits SCSI Error */
+		atomic64_inc(&snic->s_stats.fw.scsi_errs);
+		break;
+
+	case SNIC_STAT_NOT_READY:	/* XPT yet to initialize */
+	case SNIC_STAT_DEV_OFFLINE:	/* Device offline */
+		res = DID_NO_CONNECT;
+		break;
+
+	case SNIC_STAT_INVALID_HDR:	/* Hdr contains invalid data */
+	case SNIC_STAT_INVALID_PARM:	/* Some param in req is invalid */
+	case SNIC_STAT_REQ_NOT_SUP:	/* Req type is not supported */
+	case SNIC_STAT_CMND_REJECT:	/* Req rejected */
+	case SNIC_STAT_FATAL_ERROR:	/* XPT Error */
+	default:
+		SNIC_SCSI_DBG(snic->shost,
+			      "Invalid Hdr/Param or Req Not Supported or Cmnd Rejected or Device Offline. or Unknown\n");
+		res = DID_ERROR;
+		break;
+	}
+
+	SNIC_HOST_ERR(snic->shost, "fw returns failed status %s flags 0x%llx\n",
+		      snic_io_status_to_str(cmpl_stat), CMD_FLAGS(sc));
+
+	/* Set sc->result */
+	sc->result = (res << 16) | icmnd_cmpl->scsi_status;
+} /* end of snic_process_io_failed_state */
+
+/*
+ * snic_tmreq_pending : is task management in progress.
+ */
+static int
+snic_tmreq_pending(struct scsi_cmnd *sc)
+{
+	int state = CMD_STATE(sc);
+
+	return ((state == SNIC_IOREQ_ABTS_PENDING) ||
+			(state == SNIC_IOREQ_LR_PENDING));
+}
+
+/*
+ * snic_process_icmnd_cmpl_status:
+ * Caller should hold io_lock
+ */
+static int
+snic_process_icmnd_cmpl_status(struct snic *snic,
+			       struct snic_icmnd_cmpl *icmnd_cmpl,
+			       u8 cmpl_stat,
+			       struct scsi_cmnd *sc)
+{
+	u8 scsi_stat = icmnd_cmpl->scsi_status;
+	u64 xfer_len = 0;
+	int ret = 0;
+
+	/* Mark the IO as complete */
+	CMD_STATE(sc) = SNIC_IOREQ_COMPLETE;
+
+	if (likely(cmpl_stat == SNIC_STAT_IO_SUCCESS)) {
+		sc->result = (DID_OK << 16) | scsi_stat;
+
+		xfer_len = scsi_bufflen(sc);
+
+		/* Update SCSI Cmd with resid value */
+		scsi_set_resid(sc, le32_to_cpu(icmnd_cmpl->resid));
+
+		if (icmnd_cmpl->flags & SNIC_ICMND_CMPL_UNDR_RUN) {
+			xfer_len -= le32_to_cpu(icmnd_cmpl->resid);
+			atomic64_inc(&snic->s_stats.misc.io_under_run);
+		}
+
+		if (icmnd_cmpl->scsi_status == SAM_STAT_TASK_SET_FULL)
+			atomic64_inc(&snic->s_stats.misc.qfull);
+
+		ret = 0;
+	} else {
+		snic_process_io_failed_state(snic, icmnd_cmpl, sc, cmpl_stat);
+		atomic64_inc(&snic->s_stats.io.fail);
+		SNIC_HOST_ERR(snic->shost,
+			      "icmnd_cmpl: IO Failed : Hdr Status %s flags 0x%llx\n",
+			      snic_io_status_to_str(cmpl_stat), CMD_FLAGS(sc));
+		ret = 1;
+	}
+
+	return ret;
+} /* end of snic_process_icmnd_cmpl_status */
+
+
+/*
+ * snic_icmnd_cmpl_handler
+ * Routine to handle icmnd completions
+ */
+static void
+snic_icmnd_cmpl_handler(struct snic *snic, struct snic_fw_req *fwreq)
+{
+	u8 typ, hdr_stat;
+	u32 cmnd_id, hid;
+	ulong ctx;
+	struct scsi_cmnd *sc = NULL;
+	struct snic_icmnd_cmpl *icmnd_cmpl = NULL;
+	struct snic_host_req *req = NULL;
+	struct snic_req_info *rqi = NULL;
+	unsigned long flags, start_time;
+	spinlock_t *io_lock;
+	u8 sc_stat = 0;
+
+	snic_io_hdr_dec(&fwreq->hdr, &typ, &hdr_stat, &cmnd_id, &hid, &ctx);
+	icmnd_cmpl = &fwreq->u.icmnd_cmpl;
+	sc_stat = icmnd_cmpl->scsi_status;
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "Icmnd_cmpl: type = %x, hdr_stat = %x, cmnd_id = %x, hid = %x,i ctx = %lx\n",
+		      typ, hdr_stat, cmnd_id, hid, ctx);
+
+	if (cmnd_id >= snic->max_tag_id) {
+		SNIC_HOST_ERR(snic->shost,
+			      "Icmnd_cmpl:Tag Error:Out of Range Tag %d, hdr status = %s\n",
+			      cmnd_id, snic_io_status_to_str(hdr_stat));
+		return;
+	}
+
+	sc = scsi_host_find_tag(snic->shost, cmnd_id);
+	WARN_ON_ONCE(!sc);
+
+	if (!sc) {
+		atomic64_inc(&snic->s_stats.io.sc_null);
+		SNIC_HOST_ERR(snic->shost,
+			      "Icmnd_cmpl: Scsi Cmnd Not found, sc = NULL Hdr Status = %s tag = 0x%x fwreq = 0x%p\n",
+			      snic_io_status_to_str(hdr_stat),
+			      cmnd_id,
+			      fwreq);
+
+		SNIC_TRC(snic->shost->host_no, cmnd_id, 0,
+			 ((u64)hdr_stat << 16 |
+			  (u64)sc_stat << 8 | (u64)icmnd_cmpl->flags),
+			 (ulong) fwreq, le32_to_cpu(icmnd_cmpl->resid), ctx);
+
+		return;
+	}
+
+	io_lock = snic_io_lock_hash(snic, sc);
+
+	spin_lock_irqsave(io_lock, flags);
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	SNIC_SCSI_DBG(snic->shost,
+		      "Icmnd_cmpl:lun %lld sc %p cmd %xtag %d flags 0x%llx rqi %p\n",
+		      sc->device->lun, sc, sc->cmnd[0], snic_cmd_tag(sc),
+		      CMD_FLAGS(sc), rqi);
+
+	SNIC_BUG_ON(rqi != (struct snic_req_info *)ctx);
+	WARN_ON_ONCE(req);
+	if (!rqi) {
+		atomic64_inc(&snic->s_stats.io.req_null);
+		CMD_FLAGS(sc) |= SNIC_IO_REQ_NULL;
+		spin_unlock_irqrestore(io_lock, flags);
+
+		SNIC_HOST_ERR(snic->shost,
+			      "Icmnd_cmpl:Host Req Not Found(null), Hdr Status %s, Tag 0x%x, sc 0x%p flags 0x%llx\n",
+			      snic_io_status_to_str(hdr_stat),
+			      cmnd_id, sc, CMD_FLAGS(sc));
+		return;
+	}
+
+	rqi = (struct snic_req_info *) ctx;
+	start_time = rqi->start_time;
+
+	/* firmware completed the io */
+	rqi->io_cmpl = 1;
+
+	/*
+	 * if SCSI-ML has already issued abort on this command,
+	 * ignore completion of the IO. The abts path will clean it up
+	 */
+	if (unlikely(snic_tmreq_pending(sc))) {
+		snic_proc_tmreq_pending_state(snic, sc, hdr_stat);
+		spin_unlock_irqrestore(io_lock, flags);
+
+		snic_stats_update_io_cmpl(&snic->s_stats);
+
+		/* Expected value is SNIC_STAT_ABORTED */
+		if (likely(hdr_stat == SNIC_STAT_ABORTED))
+			return;
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "icmnd_cmpl:TM Req Pending(%s), Hdr Status %s sc 0x%p scsi status %x resid %d flags 0x%llx\n",
+			      snic_ioreq_state_to_str(CMD_STATE(sc)),
+			      snic_io_status_to_str(hdr_stat),
+			      sc, sc_stat, le32_to_cpu(icmnd_cmpl->resid),
+			      CMD_FLAGS(sc));
+
+		SNIC_TRC(snic->shost->host_no, cmnd_id, (ulong) sc,
+			 jiffies_to_msecs(jiffies - start_time), (ulong) fwreq,
+			 SNIC_TRC_CMD(sc), SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+		return;
+	}
+
+	if (snic_process_icmnd_cmpl_status(snic, icmnd_cmpl, hdr_stat, sc)) {
+		scsi_print_command(sc);
+		SNIC_HOST_ERR(snic->shost,
+			      "icmnd_cmpl:IO Failed, sc 0x%p Tag %d Cmd %x Hdr Status %s flags 0x%llx\n",
+			      sc, sc->cmnd[0], cmnd_id,
+			      snic_io_status_to_str(hdr_stat), CMD_FLAGS(sc));
+	}
+
+	/* Break link with the SCSI Command */
+	CMD_SP(sc) = NULL;
+	CMD_FLAGS(sc) |= SNIC_IO_DONE;
+
+	spin_unlock_irqrestore(io_lock, flags);
+
+	/* For now, consider only successful IO. */
+	snic_calc_io_process_time(snic, rqi);
+
+	snic_release_req_buf(snic, rqi, sc);
+
+	SNIC_TRC(snic->shost->host_no, cmnd_id, (ulong) sc,
+		 jiffies_to_msecs(jiffies - start_time), (ulong) fwreq,
+		 SNIC_TRC_CMD(sc), SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+
+	if (sc->scsi_done)
+		sc->scsi_done(sc);
+
+	snic_stats_update_io_cmpl(&snic->s_stats);
+} /* end of snic_icmnd_cmpl_handler */
+
+static void
+snic_proc_dr_cmpl_locked(struct snic *snic,
+			 struct snic_fw_req *fwreq,
+			 u8 cmpl_stat,
+			 u32 cmnd_id,
+			 struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = (struct snic_req_info *) CMD_SP(sc);
+	u32 start_time = rqi->start_time;
+
+	CMD_LR_STATUS(sc) = cmpl_stat;
+
+	SNIC_SCSI_DBG(snic->shost, "itmf_cmpl: Cmd State = %s\n",
+		      snic_ioreq_state_to_str(CMD_STATE(sc)));
+
+	if (CMD_STATE(sc) == SNIC_IOREQ_ABTS_PENDING) {
+		CMD_FLAGS(sc) |= SNIC_DEV_RST_ABTS_PENDING;
+
+		SNIC_TRC(snic->shost->host_no, cmnd_id, (ulong) sc,
+			 jiffies_to_msecs(jiffies - start_time),
+			 (ulong) fwreq, 0, SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "itmf_cmpl: Terminate Pending Dev Reset Cmpl Recvd.id %x, status %s flags 0x%llx\n",
+			      (int)(cmnd_id & SNIC_TAG_MASK),
+			      snic_io_status_to_str(cmpl_stat),
+			      CMD_FLAGS(sc));
+
+		return;
+	}
+
+
+	if (CMD_FLAGS(sc) & SNIC_DEV_RST_TIMEDOUT) {
+		SNIC_TRC(snic->shost->host_no, cmnd_id, (ulong) sc,
+			 jiffies_to_msecs(jiffies - start_time),
+			 (ulong) fwreq, 0, SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "itmf_cmpl:Dev Reset Completion Received after timeout. id %d cmpl status %s flags 0x%llx\n",
+			      (int)(cmnd_id & SNIC_TAG_MASK),
+			      snic_io_status_to_str(cmpl_stat),
+			      CMD_FLAGS(sc));
+
+		return;
+	}
+
+	CMD_STATE(sc) = SNIC_IOREQ_LR_COMPLETE;
+	CMD_FLAGS(sc) |= SNIC_DEV_RST_DONE;
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "itmf_cmpl:Dev Reset Cmpl Recvd id %d cmpl status %s flags 0x%llx\n",
+		      (int)(cmnd_id & SNIC_TAG_MASK),
+		      snic_io_status_to_str(cmpl_stat),
+		      CMD_FLAGS(sc));
+
+	if (rqi->dr_done)
+		complete(rqi->dr_done);
+} /* end of snic_proc_dr_cmpl_locked */
+
+/*
+ * snic_update_abort_stats : Updates abort stats based on completion status.
+ */
+static void
+snic_update_abort_stats(struct snic *snic, u8 cmpl_stat)
+{
+	struct snic_abort_stats *abt_stats = &snic->s_stats.abts;
+
+	SNIC_SCSI_DBG(snic->shost, "Updating Abort stats.\n");
+
+	switch (cmpl_stat) {
+	case  SNIC_STAT_IO_SUCCESS:
+		break;
+
+	case SNIC_STAT_TIMEOUT:
+		atomic64_inc(&abt_stats->fw_tmo);
+		break;
+
+	case SNIC_STAT_IO_NOT_FOUND:
+		atomic64_inc(&abt_stats->io_not_found);
+		break;
+
+	default:
+		atomic64_inc(&abt_stats->fail);
+		break;
+	}
+}
+
+static int
+snic_process_itmf_cmpl(struct snic *snic,
+		       struct snic_fw_req *fwreq,
+		       u32 cmnd_id,
+		       u8 cmpl_stat,
+		       struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = NULL;
+	u32 tm_tags = 0;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+	u32 start_time = 0;
+	int ret = 0;
+
+	io_lock = snic_io_lock_hash(snic, sc);
+	spin_lock_irqsave(io_lock, flags);
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	WARN_ON_ONCE(!rqi);
+
+	if (!rqi) {
+		atomic64_inc(&snic->s_stats.io.req_null);
+		spin_unlock_irqrestore(io_lock, flags);
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_TERM_REQ_NULL;
+		SNIC_HOST_ERR(snic->shost,
+			      "itmf_cmpl: rqi is null,Hdr stat = %s Tag = 0x%x sc = 0x%p flags 0x%llx\n",
+			      snic_io_status_to_str(cmpl_stat), cmnd_id, sc,
+			      CMD_FLAGS(sc));
+
+		return ret;
+	}
+
+	/* Extract task management flags */
+	tm_tags = cmnd_id & ~(SNIC_TAG_MASK);
+
+	start_time = rqi->start_time;
+	cmnd_id &= (SNIC_TAG_MASK);
+
+	switch (tm_tags) {
+	case SNIC_TAG_ABORT:
+		/* Abort only issued on cmd */
+		snic_update_abort_stats(snic, cmpl_stat);
+
+		if (CMD_STATE(sc) != SNIC_IOREQ_ABTS_PENDING) {
+			/* This is a late completion. Ignore it. */
+			ret = -1;
+			spin_unlock_irqrestore(io_lock, flags);
+			break;
+		}
+
+		CMD_STATE(sc) = SNIC_IOREQ_ABTS_COMPLETE;
+		CMD_ABTS_STATUS(sc) = cmpl_stat;
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_TERM_DONE;
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "itmf_cmpl:Abort Cmpl Recvd.Tag 0x%x Status %s flags 0x%llx\n",
+			      cmnd_id,
+			      snic_io_status_to_str(cmpl_stat),
+			      CMD_FLAGS(sc));
+
+		/*
+		 * If scsi_eh thread is blocked waiting for abts complete,
+		 * signal completion to it. IO will be cleaned in the thread,
+		 * else clean it in this context.
+		 */
+		if (rqi->abts_done) {
+			complete(rqi->abts_done);
+			spin_unlock_irqrestore(io_lock, flags);
+
+			break; /* jump out */
+		}
+
+		CMD_SP(sc) = NULL;
+		sc->result = (DID_ERROR << 16);
+		SNIC_SCSI_DBG(snic->shost,
+			      "itmf_cmpl: Completing IO. sc %p flags 0x%llx\n",
+			      sc, CMD_FLAGS(sc));
+
+		spin_unlock_irqrestore(io_lock, flags);
+
+		snic_release_req_buf(snic, rqi, sc);
+
+		if (sc->scsi_done) {
+			SNIC_TRC(snic->shost->host_no, cmnd_id, (ulong) sc,
+				 jiffies_to_msecs(jiffies - start_time),
+				 (ulong) fwreq, SNIC_TRC_CMD(sc),
+				 SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+			sc->scsi_done(sc);
+		}
+
+		break;
+
+	case SNIC_TAG_DEV_RST:
+	case SNIC_TAG_DEV_RST | SNIC_TAG_IOCTL_DEV_RST:
+		snic_proc_dr_cmpl_locked(snic, fwreq, cmpl_stat, cmnd_id, sc);
+		spin_unlock_irqrestore(io_lock, flags);
+		ret = 0;
+
+		break;
+
+	case SNIC_TAG_ABORT | SNIC_TAG_DEV_RST:
+		/* Abort and terminate completion of device reset req */
+
+		CMD_STATE(sc) = SNIC_IOREQ_ABTS_COMPLETE;
+		CMD_ABTS_STATUS(sc) = cmpl_stat;
+		CMD_FLAGS(sc) |= SNIC_DEV_RST_DONE;
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "itmf_cmpl:dev reset abts cmpl recvd. id %d status %s flags 0x%llx\n",
+			      cmnd_id, snic_io_status_to_str(cmpl_stat),
+			      CMD_FLAGS(sc));
+
+		if (rqi->abts_done)
+			complete(rqi->abts_done);
+
+		spin_unlock_irqrestore(io_lock, flags);
+
+		break;
+
+	default:
+		spin_unlock_irqrestore(io_lock, flags);
+		SNIC_HOST_ERR(snic->shost,
+			      "itmf_cmpl: Unknown TM tag bit 0x%x\n", tm_tags);
+
+		SNIC_HOST_ERR(snic->shost,
+			      "itmf_cmpl:Unexpected itmf io stat %s Tag = 0x%x flags 0x%llx\n",
+			      snic_ioreq_state_to_str(CMD_STATE(sc)),
+			      cmnd_id,
+			      CMD_FLAGS(sc));
+		ret = -1;
+		SNIC_BUG_ON(1);
+
+		break;
+	}
+
+	return ret;
+} /* end of snic_process_itmf_cmpl_status */
+
+/*
+ * snic_itmf_cmpl_handler.
+ * Routine to handle itmf completions.
+ */
+static void
+snic_itmf_cmpl_handler(struct snic *snic, struct snic_fw_req *fwreq)
+{
+	struct scsi_cmnd  *sc = NULL;
+	struct snic_req_info *rqi = NULL;
+	struct snic_itmf_cmpl *itmf_cmpl = NULL;
+	ulong ctx;
+	u32 cmnd_id;
+	u32 hid;
+	u8 typ;
+	u8 hdr_stat;
+
+	snic_io_hdr_dec(&fwreq->hdr, &typ, &hdr_stat, &cmnd_id, &hid, &ctx);
+	SNIC_SCSI_DBG(snic->shost,
+		      "Itmf_cmpl: %s: type = %x, hdr_stat = %x, cmnd_id = %x, hid = %x,ctx = %lx\n",
+		      __func__, typ, hdr_stat, cmnd_id, hid, ctx);
+
+	itmf_cmpl = &fwreq->u.itmf_cmpl;
+	SNIC_SCSI_DBG(snic->shost,
+		      "Itmf_cmpl: nterm %u , flags 0x%x\n",
+		      le32_to_cpu(itmf_cmpl->nterminated), itmf_cmpl->flags);
+
+	/* spl case, dev reset issued through ioctl */
+	if (cmnd_id & SNIC_TAG_IOCTL_DEV_RST) {
+		rqi = (struct snic_req_info *) ctx;
+		sc = rqi->sc;
+
+		goto ioctl_dev_rst;
+	}
+
+	if ((cmnd_id & SNIC_TAG_MASK) >= snic->max_tag_id) {
+		SNIC_HOST_ERR(snic->shost,
+			      "Itmf_cmpl: Tag 0x%x out of Range,HdrStat %s\n",
+			      cmnd_id, snic_io_status_to_str(hdr_stat));
+		SNIC_BUG_ON(1);
+
+		return;
+	}
+
+	sc = scsi_host_find_tag(snic->shost, cmnd_id & SNIC_TAG_MASK);
+	WARN_ON_ONCE(!sc);
+
+ioctl_dev_rst:
+	if (!sc) {
+		atomic64_inc(&snic->s_stats.io.sc_null);
+		SNIC_HOST_ERR(snic->shost,
+			      "Itmf_cmpl: sc is NULL - Hdr Stat %s Tag 0x%x\n",
+			      snic_io_status_to_str(hdr_stat), cmnd_id);
+
+		return;
+	}
+
+	snic_process_itmf_cmpl(snic, fwreq, cmnd_id, hdr_stat, sc);
+} /* end of snic_itmf_cmpl_handler */
+
+
+
+static void
+snic_hba_reset_scsi_cleanup(struct snic *snic, struct scsi_cmnd *sc)
+{
+	struct snic_stats *st = &snic->s_stats;
+	long act_ios = 0, act_fwreqs = 0;
+
+	SNIC_SCSI_DBG(snic->shost, "HBA Reset scsi cleanup.\n");
+	snic_scsi_cleanup(snic, snic_cmd_tag(sc));
+
+	/* Update stats on pending IOs */
+	act_ios = atomic64_read(&st->io.active);
+	atomic64_add(act_ios, &st->io.compl);
+	atomic64_sub(act_ios, &st->io.active);
+
+	act_fwreqs = atomic64_read(&st->fw.actv_reqs);
+	atomic64_sub(act_fwreqs, &st->fw.actv_reqs);
+}
+
+/*
+ * snic_hba_reset_cmpl_handler :
+ *
+ * Notes :
+ * 1. Cleanup all the scsi cmds, release all snic specific cmds
+ * 2. Issue Report Targets in case of SAN targets
+ */
+static int
+snic_hba_reset_cmpl_handler(struct snic *snic, struct snic_fw_req *fwreq)
+{
+	ulong ctx;
+	u32 cmnd_id;
+	u32 hid;
+	u8 typ;
+	u8 hdr_stat;
+	struct scsi_cmnd *sc = NULL;
+	struct snic_req_info *rqi = NULL;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags, gflags;
+	int ret = 0;
+
+	SNIC_HOST_INFO(snic->shost,
+		       "reset_cmpl:HBA Reset Completion received.\n");
+
+	snic_io_hdr_dec(&fwreq->hdr, &typ, &hdr_stat, &cmnd_id, &hid, &ctx);
+	SNIC_SCSI_DBG(snic->shost,
+		      "reset_cmpl: type = %x, hdr_stat = %x, cmnd_id = %x, hid = %x, ctx = %lx\n",
+		      typ, hdr_stat, cmnd_id, hid, ctx);
+
+	/* spl case, host reset issued through ioctl */
+	if (cmnd_id == SCSI_NO_TAG) {
+		rqi = (struct snic_req_info *) ctx;
+		sc = rqi->sc;
+
+		goto ioctl_hba_rst;
+	}
+
+	if (cmnd_id >= snic->max_tag_id) {
+		SNIC_HOST_ERR(snic->shost,
+			      "reset_cmpl: Tag 0x%x out of Range,HdrStat %s\n",
+			      cmnd_id, snic_io_status_to_str(hdr_stat));
+		SNIC_BUG_ON(1);
+
+		return 1;
+	}
+
+	sc = scsi_host_find_tag(snic->shost, cmnd_id);
+ioctl_hba_rst:
+	if (!sc) {
+		atomic64_inc(&snic->s_stats.io.sc_null);
+		SNIC_HOST_ERR(snic->shost,
+			      "reset_cmpl: sc is NULL - Hdr Stat %s Tag 0x%x\n",
+			      snic_io_status_to_str(hdr_stat), cmnd_id);
+		ret = 1;
+
+		return ret;
+	}
+
+	io_lock = snic_io_lock_hash(snic, sc);
+	spin_lock_irqsave(io_lock, flags);
+
+	if (!snic->remove_wait) {
+		spin_unlock_irqrestore(io_lock, flags);
+		SNIC_HOST_ERR(snic->shost,
+			      "reset_cmpl:host reset completed after timout\n");
+		ret = 1;
+
+		return ret;
+	}
+
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	WARN_ON_ONCE(!rqi);
+
+	if (!rqi) {
+		atomic64_inc(&snic->s_stats.io.req_null);
+		spin_unlock_irqrestore(io_lock, flags);
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_TERM_REQ_NULL;
+		SNIC_HOST_ERR(snic->shost,
+			      "reset_cmpl: rqi is null,Hdr stat %s Tag 0x%x sc 0x%p flags 0x%llx\n",
+			      snic_io_status_to_str(hdr_stat), cmnd_id, sc,
+			      CMD_FLAGS(sc));
+
+		ret = 1;
+
+		return ret;
+	}
+	/* stats */
+	spin_unlock_irqrestore(io_lock, flags);
+
+	/* scsi cleanup */
+	snic_hba_reset_scsi_cleanup(snic, sc);
+
+	SNIC_BUG_ON(snic_get_state(snic) != SNIC_OFFLINE &&
+		    snic_get_state(snic) != SNIC_FWRESET);
+
+	/* Careful locking between snic_lock and io lock */
+	spin_lock_irqsave(io_lock, flags);
+	spin_lock_irqsave(&snic->snic_lock, gflags);
+	if (snic_get_state(snic) == SNIC_FWRESET)
+		snic_set_state(snic, SNIC_ONLINE);
+	spin_unlock_irqrestore(&snic->snic_lock, gflags);
+
+	if (snic->remove_wait)
+		complete(snic->remove_wait);
+
+	spin_unlock_irqrestore(io_lock, flags);
+	atomic64_inc(&snic->s_stats.reset.hba_reset_cmpl);
+
+	ret = 0;
+	/* Rediscovery is for SAN */
+	if (snic->config.xpt_type == SNIC_DAS)
+			return ret;
+
+	SNIC_SCSI_DBG(snic->shost, "reset_cmpl: Queuing discovery work.\n");
+	queue_work(snic_glob->event_q, &snic->disc_work);
+
+	return ret;
+}
+
+static void
+snic_msg_ack_handler(struct snic *snic, struct snic_fw_req *fwreq)
+{
+	SNIC_HOST_INFO(snic->shost, "Message Ack Received.\n");
+
+	SNIC_ASSERT_NOT_IMPL(1);
+}
+
+static void
+snic_aen_handler(struct snic *snic, struct snic_fw_req *fwreq)
+{
+	u8 typ, hdr_stat;
+	u32 cmnd_id, hid;
+	ulong ctx;
+	struct snic_async_evnotify *aen = &fwreq->u.async_ev;
+	u32 event_id = 0;
+
+	snic_io_hdr_dec(&fwreq->hdr, &typ, &hdr_stat, &cmnd_id, &hid, &ctx);
+	SNIC_SCSI_DBG(snic->shost,
+		      "aen: type = %x, hdr_stat = %x, cmnd_id = %x, hid = %x, ctx = %lx\n",
+		      typ, hdr_stat, cmnd_id, hid, ctx);
+
+	event_id = le32_to_cpu(aen->ev_id);
+
+	switch (event_id) {
+	case SNIC_EV_TGT_OFFLINE:
+		SNIC_HOST_INFO(snic->shost, "aen:TGT_OFFLINE Event Recvd.\n");
+		break;
+
+	case SNIC_EV_TGT_ONLINE:
+		SNIC_HOST_INFO(snic->shost, "aen:TGT_ONLINE Event Recvd.\n");
+		break;
+
+	case SNIC_EV_LUN_OFFLINE:
+		SNIC_HOST_INFO(snic->shost, "aen:LUN_OFFLINE Event Recvd.\n");
+		break;
+
+	case SNIC_EV_LUN_ONLINE:
+		SNIC_HOST_INFO(snic->shost, "aen:LUN_ONLINE Event Recvd.\n");
+		break;
+
+	case SNIC_EV_CONF_CHG:
+		SNIC_HOST_INFO(snic->shost, "aen:Config Change Event Recvd.\n");
+		break;
+
+	case SNIC_EV_TGT_ADDED:
+		SNIC_HOST_INFO(snic->shost, "aen:TGT_ADD Event Recvd.\n");
+		break;
+
+	case SNIC_EV_TGT_DELTD:
+		SNIC_HOST_INFO(snic->shost, "aen:TGT_DEL Event Recvd.\n");
+		break;
+
+	case SNIC_EV_LUN_ADDED:
+		SNIC_HOST_INFO(snic->shost, "aen:LUN_ADD Event Recvd.\n");
+		break;
+
+	case SNIC_EV_LUN_DELTD:
+		SNIC_HOST_INFO(snic->shost, "aen:LUN_DEL Event Recvd.\n");
+		break;
+
+	case SNIC_EV_DISC_CMPL:
+		SNIC_HOST_INFO(snic->shost, "aen:DISC_CMPL Event Recvd.\n");
+		break;
+
+	default:
+		SNIC_HOST_INFO(snic->shost, "aen:Unknown Event Recvd.\n");
+		SNIC_BUG_ON(1);
+		break;
+	}
+
+	SNIC_ASSERT_NOT_IMPL(1);
+} /* end of snic_aen_handler */
+
+/*
+ * snic_io_cmpl_handler
+ * Routine to process CQ entries(IO Completions) posted by fw.
+ */
+static int
+snic_io_cmpl_handler(struct vnic_dev *vdev,
+		     unsigned int cq_idx,
+		     struct snic_fw_req *fwreq)
+{
+	struct snic *snic = svnic_dev_priv(vdev);
+	u64 start = jiffies, cmpl_time;
+
+	snic_print_desc(__func__, (char *)fwreq, sizeof(*fwreq));
+
+	/* Update FW Stats */
+	if ((fwreq->hdr.type >= SNIC_RSP_REPORT_TGTS_CMPL) &&
+		(fwreq->hdr.type <= SNIC_RSP_BOOT_LUNS_CMPL))
+		atomic64_dec(&snic->s_stats.fw.actv_reqs);
+
+	SNIC_BUG_ON((fwreq->hdr.type > SNIC_RSP_BOOT_LUNS_CMPL) &&
+		    (fwreq->hdr.type < SNIC_MSG_ASYNC_EVNOTIFY));
+
+	/* Check for snic subsys errors */
+	switch (fwreq->hdr.status) {
+	case SNIC_STAT_NOT_READY:	/* XPT yet to initialize */
+		SNIC_HOST_ERR(snic->shost,
+			      "sNIC SubSystem is NOT Ready.\n");
+		break;
+
+	case SNIC_STAT_FATAL_ERROR:	/* XPT Error */
+		SNIC_HOST_ERR(snic->shost,
+			      "sNIC SubSystem in Unrecoverable State.\n");
+		break;
+	}
+
+	switch (fwreq->hdr.type) {
+	case SNIC_RSP_EXCH_VER_CMPL:
+		snic_io_exch_ver_cmpl_handler(snic, fwreq);
+		break;
+
+	case SNIC_RSP_REPORT_TGTS_CMPL:
+		snic_report_tgt_cmpl_handler(snic, fwreq);
+		break;
+
+	case SNIC_RSP_ICMND_CMPL:
+		snic_icmnd_cmpl_handler(snic, fwreq);
+		break;
+
+	case SNIC_RSP_ITMF_CMPL:
+		snic_itmf_cmpl_handler(snic, fwreq);
+		break;
+
+	case SNIC_RSP_HBA_RESET_CMPL:
+		snic_hba_reset_cmpl_handler(snic, fwreq);
+		break;
+
+	case SNIC_MSG_ACK:
+		snic_msg_ack_handler(snic, fwreq);
+		break;
+
+	case SNIC_MSG_ASYNC_EVNOTIFY:
+		snic_aen_handler(snic, fwreq);
+		break;
+
+	default:
+		SNIC_BUG_ON(1);
+		SNIC_SCSI_DBG(snic->shost,
+			      "Unknown Firmwqre completion request type %d\n",
+			      fwreq->hdr.type);
+		break;
+	}
+
+	/* Update Stats */
+	cmpl_time = jiffies - start;
+	if (cmpl_time > atomic64_read(&snic->s_stats.io.max_cmpl_time))
+		atomic64_set(&snic->s_stats.io.max_cmpl_time, cmpl_time);
+
+	return 0;
+} /* end of snic_io_cmpl_handler */
+
+/*
+ * snic_fwcq_cmpl_handler
+ * Routine to process fwCQ
+ * This CQ is independent, and not associated with wq/rq/wq_copy queues
+ */
+int
+snic_fwcq_cmpl_handler(struct snic *snic, int io_cmpl_work)
+{
+	unsigned int num_ent = 0;	/* number cq entries processed */
+	unsigned int cq_idx;
+	unsigned int nent_per_cq;
+	struct snic_misc_stats *misc_stats = &snic->s_stats.misc;
+
+	for (cq_idx = snic->wq_count; cq_idx < snic->cq_count; cq_idx++) {
+		nent_per_cq = vnic_cq_fw_service(&snic->cq[cq_idx],
+						 snic_io_cmpl_handler,
+						 io_cmpl_work);
+		num_ent += nent_per_cq;
+
+		if (nent_per_cq > atomic64_read(&misc_stats->max_cq_ents))
+			atomic64_set(&misc_stats->max_cq_ents, nent_per_cq);
+	}
+
+	return num_ent;
+} /* end of snic_fwcq_cmpl_handler */
+
+/*
+ * snic_queue_itmf_req: Common API to queue Task Management requests.
+ * Use rqi->tm_tag for passing special tags.
+ * @req_id : aborted request's tag, -1 for lun reset.
+ */
+static int
+snic_queue_itmf_req(struct snic *snic,
+		    struct snic_host_req *tmreq,
+		    struct scsi_cmnd *sc,
+		    u32 tmf,
+		    u32 req_id)
+{
+	struct snic_req_info *rqi = req_to_rqi(tmreq);
+	struct scsi_lun lun;
+	int tm_tag = snic_cmd_tag(sc) | rqi->tm_tag;
+	int ret = 0;
+
+	SNIC_BUG_ON(!rqi);
+	SNIC_BUG_ON(!rqi->tm_tag);
+
+	/* fill in lun info */
+	int_to_scsilun(sc->device->lun, &lun);
+
+	/* Initialize snic_host_req: itmf */
+	snic_itmf_init(tmreq,
+		       tm_tag,
+		       snic->config.hid,
+		       (ulong) rqi,
+		       0 /* flags */,
+		       req_id, /* Command to be aborted. */
+		       rqi->tgt_id,
+		       lun.scsi_lun,
+		       tmf);
+
+	/*
+	 * In case of multiple aborts on same cmd,
+	 * use try_wait_for_completion and completion_done() to check
+	 * whether it queues aborts even after completion of abort issued
+	 * prior.SNIC_BUG_ON(completion_done(&rqi->done));
+	 */
+
+	ret = snic_queue_wq_desc(snic, tmreq, sizeof(*tmreq));
+	if (ret)
+		SNIC_HOST_ERR(snic->shost,
+			      "qitmf:Queuing ITMF(%d) Req sc %p, rqi %p, req_id %d tag %d Failed, ret = %d\n",
+			      tmf, sc, rqi, req_id, snic_cmd_tag(sc), ret);
+	else
+		SNIC_SCSI_DBG(snic->shost,
+			      "qitmf:Queuing ITMF(%d) Req sc %p, rqi %p, req_id %d, tag %d (req_id)- Success.",
+			      tmf, sc, rqi, req_id, snic_cmd_tag(sc));
+
+	return ret;
+} /* end of snic_queue_itmf_req */
+
+static int
+snic_issue_tm_req(struct snic *snic,
+		    struct snic_req_info *rqi,
+		    struct scsi_cmnd *sc,
+		    int tmf)
+{
+	struct snic_host_req *tmreq = NULL;
+	int req_id = 0, tag = snic_cmd_tag(sc);
+	int ret = 0;
+
+	if (snic_get_state(snic) == SNIC_FWRESET)
+		return -EBUSY;
+
+	atomic_inc(&snic->ios_inflight);
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "issu_tmreq: Task mgmt req %d. rqi %p w/ tag %x\n",
+		      tmf, rqi, tag);
+
+
+	if (tmf == SNIC_ITMF_LUN_RESET) {
+		tmreq = snic_dr_req_init(snic, rqi);
+		req_id = SCSI_NO_TAG;
+	} else {
+		tmreq = snic_abort_req_init(snic, rqi);
+		req_id = tag;
+	}
+
+	if (!tmreq) {
+		ret = -ENOMEM;
+
+		goto tmreq_err;
+	}
+
+	ret = snic_queue_itmf_req(snic, tmreq, sc, tmf, req_id);
+	if (ret)
+		goto tmreq_err;
+
+	ret = 0;
+
+tmreq_err:
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "issu_tmreq: Queing ITMF(%d) Req, sc %p rqi %p req_id %d tag %x fails err = %d\n",
+			      tmf, sc, rqi, req_id, tag, ret);
+	} else {
+		SNIC_SCSI_DBG(snic->shost,
+			      "issu_tmreq: Queuing ITMF(%d) Req, sc %p, rqi %p, req_id %d tag %x - Success.\n",
+			      tmf, sc, rqi, req_id, tag);
+	}
+
+	atomic_dec(&snic->ios_inflight);
+
+	return ret;
+}
+
+/*
+ * snic_queue_abort_req : Queues abort req to WQ
+ */
+static int
+snic_queue_abort_req(struct snic *snic,
+		     struct snic_req_info *rqi,
+		     struct scsi_cmnd *sc,
+		     int tmf)
+{
+	SNIC_SCSI_DBG(snic->shost, "q_abtreq: sc %p, rqi %p, tag %x, tmf %d\n",
+		      sc, rqi, snic_cmd_tag(sc), tmf);
+
+	/* Add special tag for abort */
+	rqi->tm_tag |= SNIC_TAG_ABORT;
+
+	return snic_issue_tm_req(snic, rqi, sc, tmf);
+}
+
+/*
+ * snic_abort_finish : called by snic_abort_cmd on queuing abort successfully.
+ */
+static int
+snic_abort_finish(struct snic *snic, struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = NULL;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+	int ret = 0, tag = snic_cmd_tag(sc);
+
+	io_lock = snic_io_lock_hash(snic, sc);
+	spin_lock_irqsave(io_lock, flags);
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	if (!rqi) {
+		atomic64_inc(&snic->s_stats.io.req_null);
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_TERM_REQ_NULL;
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "abt_fini:req info is null tag 0x%x, sc 0x%p flags 0x%llx\n",
+			      tag, sc, CMD_FLAGS(sc));
+		ret = FAILED;
+
+		goto abort_fail;
+	}
+
+	rqi->abts_done = NULL;
+
+	ret = FAILED;
+
+	/* Check the abort status. */
+	switch (CMD_ABTS_STATUS(sc)) {
+	case SNIC_INVALID_CODE:
+		/* Firmware didn't complete abort req, timedout */
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_TIMEDOUT;
+		atomic64_inc(&snic->s_stats.abts.drv_tmo);
+		SNIC_SCSI_DBG(snic->shost,
+			      "abt_fini:sc %p Tag %x Driver Timeout.flags 0x%llx\n",
+			      sc, snic_cmd_tag(sc), CMD_FLAGS(sc));
+		/* do not release snic request in timedout case */
+		rqi = NULL;
+
+		goto abort_fail;
+
+	case SNIC_STAT_IO_SUCCESS:
+	case SNIC_STAT_IO_NOT_FOUND:
+		ret = SUCCESS;
+		break;
+
+	default:
+		/* Firmware completed abort with error */
+		ret = FAILED;
+		break;
+	}
+
+	CMD_SP(sc) = NULL;
+	SNIC_HOST_INFO(snic->shost,
+		       "abt_fini: Tag %x, Cmpl Status %s flags 0x%llx\n",
+		       tag, snic_io_status_to_str(CMD_ABTS_STATUS(sc)),
+		       CMD_FLAGS(sc));
+
+abort_fail:
+	spin_unlock_irqrestore(io_lock, flags);
+	if (rqi)
+		snic_release_req_buf(snic, rqi, sc);
+
+	return ret;
+} /* end of snic_abort_finish */
+
+/*
+ * snic_send_abort_and_wait : Issues Abort, and Waits
+ */
+static int
+snic_send_abort_and_wait(struct snic *snic, struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = NULL;
+	enum snic_ioreq_state sv_state;
+	struct snic_tgt *tgt = NULL;
+	spinlock_t *io_lock = NULL;
+	DECLARE_COMPLETION_ONSTACK(tm_done);
+	unsigned long flags;
+	int ret = 0, tmf = 0, tag = snic_cmd_tag(sc);
+
+	tgt = starget_to_tgt(scsi_target(sc->device));
+	if ((snic_tgt_chkready(tgt) != 0) && (tgt->tdata.typ == SNIC_TGT_SAN))
+		tmf = SNIC_ITMF_ABTS_TASK_TERM;
+	else
+		tmf = SNIC_ITMF_ABTS_TASK;
+
+	/* stats */
+
+	io_lock = snic_io_lock_hash(snic, sc);
+
+	/*
+	 * Avoid a race between SCSI issuing the abort and the device
+	 * completing the command.
+	 *
+	 * If the command is already completed by fw_cmpl code,
+	 * we just return SUCCESS from here. This means that the abort
+	 * succeeded. In the SCSI ML, since the timeout for command has
+	 * happend, the completion wont actually complete the command
+	 * and it will be considered as an aborted command
+	 *
+	 * The CMD_SP will not be cleared except while holding io_lock
+	 */
+	spin_lock_irqsave(io_lock, flags);
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	if (!rqi) {
+		spin_unlock_irqrestore(io_lock, flags);
+
+		SNIC_HOST_ERR(snic->shost,
+			      "abt_cmd: rqi is null. Tag %d flags 0x%llx\n",
+			      tag, CMD_FLAGS(sc));
+
+		ret = SUCCESS;
+
+		goto send_abts_end;
+	}
+
+	rqi->abts_done = &tm_done;
+	if (CMD_STATE(sc) == SNIC_IOREQ_ABTS_PENDING) {
+		spin_unlock_irqrestore(io_lock, flags);
+
+		ret = 0;
+		goto abts_pending;
+	}
+	SNIC_BUG_ON(!rqi->abts_done);
+
+	/* Save Command State, should be restored on failed to Queue. */
+	sv_state = CMD_STATE(sc);
+
+	/*
+	 * Command is still pending, need to abort it
+	 * If the fw completes the command after this point,
+	 * the completion won't be done till mid-layer, since abot
+	 * has already started.
+	 */
+	CMD_STATE(sc) = SNIC_IOREQ_ABTS_PENDING;
+	CMD_ABTS_STATUS(sc) = SNIC_INVALID_CODE;
+
+	SNIC_SCSI_DBG(snic->shost, "send_abt_cmd: TAG 0x%x\n", tag);
+
+	spin_unlock_irqrestore(io_lock, flags);
+
+	/* Now Queue the abort command to firmware */
+	ret = snic_queue_abort_req(snic, rqi, sc, tmf);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "send_abt_cmd: IO w/ Tag 0x%x fail w/ err %d flags 0x%llx\n",
+			      tag, ret, CMD_FLAGS(sc));
+
+		spin_lock_irqsave(io_lock, flags);
+		/* Restore Command's previous state */
+		CMD_STATE(sc) = sv_state;
+		rqi = (struct snic_req_info *) CMD_SP(sc);
+		if (rqi)
+			rqi->abts_done = NULL;
+		spin_unlock_irqrestore(io_lock, flags);
+		ret = FAILED;
+
+		goto send_abts_end;
+	}
+
+	spin_lock_irqsave(io_lock, flags);
+	if (tmf == SNIC_ITMF_ABTS_TASK) {
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_ISSUED;
+		atomic64_inc(&snic->s_stats.abts.num);
+	} else {
+		/* term stats */
+		CMD_FLAGS(sc) |= SNIC_IO_TERM_ISSUED;
+	}
+	spin_unlock_irqrestore(io_lock, flags);
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "send_abt_cmd: sc %p Tag %x flags 0x%llx\n",
+		      sc, tag, CMD_FLAGS(sc));
+
+
+	ret = 0;
+
+abts_pending:
+	/*
+	 * Queued an abort IO, wait for its completion.
+	 * Once the fw completes the abort command, it will
+	 * wakeup this thread.
+	 */
+	wait_for_completion_timeout(&tm_done, SNIC_ABTS_TIMEOUT);
+
+send_abts_end:
+	return ret;
+} /* end of snic_send_abort_and_wait */
+
+/*
+ * This function is exported to SCSI for sending abort cmnds.
+ * A SCSI IO is represent by snic_ioreq in the driver.
+ * The snic_ioreq is linked to the SCSI Cmd, thus a link with the ULP'S IO
+ */
+int
+snic_abort_cmd(struct scsi_cmnd *sc)
+{
+	struct snic *snic = shost_priv(sc->device->host);
+	int ret = SUCCESS, tag = snic_cmd_tag(sc);
+	u32 start_time = jiffies;
+
+	SNIC_SCSI_DBG(snic->shost, "abt_cmd:sc %p :0x%x :req = %p :tag = %d\n",
+		       sc, sc->cmnd[0], sc->request, tag);
+
+	if (unlikely(snic_get_state(snic) != SNIC_ONLINE)) {
+		SNIC_HOST_ERR(snic->shost,
+			      "abt_cmd: tag %x Parent Devs are not rdy\n",
+			      tag);
+		ret = FAST_IO_FAIL;
+
+		goto abort_end;
+	}
+
+
+	ret = snic_send_abort_and_wait(snic, sc);
+	if (ret)
+		goto abort_end;
+
+	ret = snic_abort_finish(snic, sc);
+
+abort_end:
+	SNIC_TRC(snic->shost->host_no, tag, (ulong) sc,
+		 jiffies_to_msecs(jiffies - start_time), 0,
+		 SNIC_TRC_CMD(sc), SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "abts: Abort Req Status = %s\n",
+		      (ret == SUCCESS) ? "SUCCESS" :
+		       ((ret == FAST_IO_FAIL) ? "FAST_IO_FAIL" : "FAILED"));
+
+	return ret;
+}
+
+
+
+static int
+snic_is_abts_pending(struct snic *snic, struct scsi_cmnd *lr_sc)
+{
+	struct snic_req_info *rqi = NULL;
+	struct scsi_cmnd *sc = NULL;
+	struct scsi_device *lr_sdev = NULL;
+	spinlock_t *io_lock = NULL;
+	u32 tag;
+	unsigned long flags;
+
+	if (lr_sc)
+		lr_sdev = lr_sc->device;
+
+	/* walk through the tag map, an dcheck if IOs are still pending in fw*/
+	for (tag = 0; tag < snic->max_tag_id; tag++) {
+		io_lock = snic_io_lock_tag(snic, tag);
+
+		spin_lock_irqsave(io_lock, flags);
+		sc = scsi_host_find_tag(snic->shost, tag);
+
+		if (!sc || (lr_sc && (sc->device != lr_sdev || sc == lr_sc))) {
+			spin_unlock_irqrestore(io_lock, flags);
+
+			continue;
+		}
+
+		rqi = (struct snic_req_info *) CMD_SP(sc);
+		if (!rqi) {
+			spin_unlock_irqrestore(io_lock, flags);
+
+			continue;
+		}
+
+		/*
+		 * Found IO that is still pending w/ firmware and belongs to
+		 * the LUN that is under reset, if lr_sc != NULL
+		 */
+		SNIC_SCSI_DBG(snic->shost, "Found IO in %s on LUN\n",
+			      snic_ioreq_state_to_str(CMD_STATE(sc)));
+
+		if (CMD_STATE(sc) == SNIC_IOREQ_ABTS_PENDING) {
+			spin_unlock_irqrestore(io_lock, flags);
+
+			return 1;
+		}
+
+		spin_unlock_irqrestore(io_lock, flags);
+	}
+
+	return 0;
+} /* end of snic_is_abts_pending */
+
+static int
+snic_dr_clean_single_req(struct snic *snic,
+			 u32 tag,
+			 struct scsi_device *lr_sdev)
+{
+	struct snic_req_info *rqi = NULL;
+	struct snic_tgt *tgt = NULL;
+	struct scsi_cmnd *sc = NULL;
+	spinlock_t *io_lock = NULL;
+	u32 sv_state = 0, tmf = 0;
+	DECLARE_COMPLETION_ONSTACK(tm_done);
+	unsigned long flags;
+	int ret = 0;
+
+	io_lock = snic_io_lock_tag(snic, tag);
+	spin_lock_irqsave(io_lock, flags);
+	sc = scsi_host_find_tag(snic->shost, tag);
+
+	/* Ignore Cmd that don't belong to Lun Reset device */
+	if (!sc || sc->device != lr_sdev)
+		goto skip_clean;
+
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+
+	if (!rqi)
+		goto skip_clean;
+
+
+	if (CMD_STATE(sc) == SNIC_IOREQ_ABTS_PENDING)
+		goto skip_clean;
+
+
+	if ((CMD_FLAGS(sc) & SNIC_DEVICE_RESET) &&
+			(!(CMD_FLAGS(sc) & SNIC_DEV_RST_ISSUED))) {
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "clean_single_req: devrst is not pending sc 0x%p\n",
+			      sc);
+
+		goto skip_clean;
+	}
+
+	SNIC_SCSI_DBG(snic->shost,
+		"clean_single_req: Found IO in %s on lun\n",
+		snic_ioreq_state_to_str(CMD_STATE(sc)));
+
+	/* Save Command State */
+	sv_state = CMD_STATE(sc);
+
+	/*
+	 * Any pending IO issued prior to reset is expected to be
+	 * in abts pending state, if not we need to set SNIC_IOREQ_ABTS_PENDING
+	 * to indicate the IO is abort pending.
+	 * When IO is completed, the IO will be handed over and handled
+	 * in this function.
+	 */
+
+	CMD_STATE(sc) = SNIC_IOREQ_ABTS_PENDING;
+	SNIC_BUG_ON(rqi->abts_done);
+
+	if (CMD_FLAGS(sc) & SNIC_DEVICE_RESET) {
+		rqi->tm_tag = SNIC_TAG_DEV_RST;
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "clean_single_req:devrst sc 0x%p\n", sc);
+	}
+
+	CMD_ABTS_STATUS(sc) = SNIC_INVALID_CODE;
+	rqi->abts_done = &tm_done;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	tgt = starget_to_tgt(scsi_target(sc->device));
+	if ((snic_tgt_chkready(tgt) != 0) && (tgt->tdata.typ == SNIC_TGT_SAN))
+		tmf = SNIC_ITMF_ABTS_TASK_TERM;
+	else
+		tmf = SNIC_ITMF_ABTS_TASK;
+
+	/* Now queue the abort command to firmware */
+	ret = snic_queue_abort_req(snic, rqi, sc, tmf);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "clean_single_req_err:sc %p, tag %d abt failed. tm_tag %d flags 0x%llx\n",
+			      sc, tag, rqi->tm_tag, CMD_FLAGS(sc));
+
+		spin_lock_irqsave(io_lock, flags);
+		rqi = (struct snic_req_info *) CMD_SP(sc);
+		if (rqi)
+			rqi->abts_done = NULL;
+
+		/* Restore Command State */
+		if (CMD_STATE(sc) == SNIC_IOREQ_ABTS_PENDING)
+			CMD_STATE(sc) = sv_state;
+
+		ret = 1;
+		goto skip_clean;
+	}
+
+	spin_lock_irqsave(io_lock, flags);
+	if (CMD_FLAGS(sc) & SNIC_DEVICE_RESET)
+		CMD_FLAGS(sc) |= SNIC_DEV_RST_TERM_ISSUED;
+
+	CMD_FLAGS(sc) |= SNIC_IO_INTERNAL_TERM_ISSUED;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	wait_for_completion_timeout(&tm_done, SNIC_ABTS_TIMEOUT);
+
+	/* Recheck cmd state to check if it now aborted. */
+	spin_lock_irqsave(io_lock, flags);
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	if (!rqi) {
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_TERM_REQ_NULL;
+		goto skip_clean;
+	}
+	rqi->abts_done = NULL;
+
+	/* if abort is still pending w/ fw, fail */
+	if (CMD_ABTS_STATUS(sc) == SNIC_INVALID_CODE) {
+		SNIC_HOST_ERR(snic->shost,
+			      "clean_single_req_err:sc %p tag %d abt still pending w/ fw, tm_tag %d flags 0x%llx\n",
+			      sc, tag, rqi->tm_tag, CMD_FLAGS(sc));
+
+		CMD_FLAGS(sc) |= SNIC_IO_ABTS_TERM_DONE;
+		ret = 1;
+
+		goto skip_clean;
+	}
+
+	CMD_STATE(sc) = SNIC_IOREQ_ABTS_COMPLETE;
+	CMD_SP(sc) = NULL;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	snic_release_req_buf(snic, rqi, sc);
+
+	ret = 0;
+
+	return ret;
+
+skip_clean:
+	spin_unlock_irqrestore(io_lock, flags);
+
+	return ret;
+} /* end of snic_dr_clean_single_req */
+
+static int
+snic_dr_clean_pending_req(struct snic *snic, struct scsi_cmnd *lr_sc)
+{
+	struct scsi_device *lr_sdev = lr_sc->device;
+	u32 tag = 0;
+	int ret = FAILED;
+
+	for (tag = 0; tag < snic->max_tag_id; tag++) {
+		if (tag == snic_cmd_tag(lr_sc))
+			continue;
+
+		ret = snic_dr_clean_single_req(snic, tag, lr_sdev);
+		if (ret) {
+			SNIC_HOST_ERR(snic->shost, "clean_err:tag = %d\n", tag);
+
+			goto clean_err;
+		}
+	}
+
+	schedule_timeout(msecs_to_jiffies(100));
+
+	/* Walk through all the cmds and check abts status. */
+	if (snic_is_abts_pending(snic, lr_sc)) {
+		ret = FAILED;
+
+		goto clean_err;
+	}
+
+	ret = 0;
+	SNIC_SCSI_DBG(snic->shost, "clean_pending_req: Success.\n");
+
+	return ret;
+
+clean_err:
+	ret = FAILED;
+	SNIC_HOST_ERR(snic->shost,
+		      "Failed to Clean Pending IOs on %s device.\n",
+		      dev_name(&lr_sdev->sdev_gendev));
+
+	return ret;
+
+} /* end of snic_dr_clean_pending_req */
+
+/*
+ * snic_dr_finish : Called by snic_device_reset
+ */
+static int
+snic_dr_finish(struct snic *snic, struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = NULL;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+	int lr_res = 0;
+	int ret = FAILED;
+
+	io_lock = snic_io_lock_hash(snic, sc);
+	spin_lock_irqsave(io_lock, flags);
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	if (!rqi) {
+		spin_unlock_irqrestore(io_lock, flags);
+		SNIC_SCSI_DBG(snic->shost,
+			      "dr_fini: rqi is null tag 0x%x sc 0x%p flags 0x%llx\n",
+			      snic_cmd_tag(sc), sc, CMD_FLAGS(sc));
+
+		ret = FAILED;
+		goto dr_fini_end;
+	}
+
+	rqi->dr_done = NULL;
+
+	lr_res = CMD_LR_STATUS(sc);
+
+	switch (lr_res) {
+	case SNIC_INVALID_CODE:
+		/* stats */
+		SNIC_SCSI_DBG(snic->shost,
+			      "dr_fini: Tag %x Dev Reset Timedout. flags 0x%llx\n",
+			      snic_cmd_tag(sc), CMD_FLAGS(sc));
+
+		CMD_FLAGS(sc) |= SNIC_DEV_RST_TIMEDOUT;
+		ret = FAILED;
+
+		goto dr_failed;
+
+	case SNIC_STAT_IO_SUCCESS:
+		SNIC_SCSI_DBG(snic->shost,
+			      "dr_fini: Tag %x Dev Reset cmpl\n",
+			      snic_cmd_tag(sc));
+		ret = 0;
+		break;
+
+	default:
+		SNIC_HOST_ERR(snic->shost,
+			      "dr_fini:Device Reset completed& failed.Tag = %x lr_status %s flags 0x%llx\n",
+			      snic_cmd_tag(sc),
+			      snic_io_status_to_str(lr_res), CMD_FLAGS(sc));
+		ret = FAILED;
+		goto dr_failed;
+	}
+	spin_unlock_irqrestore(io_lock, flags);
+
+	/*
+	 * Cleanup any IOs on this LUN that have still not completed.
+	 * If any of these fail, then LUN Reset fails.
+	 * Cleanup cleans all commands on this LUN except
+	 * the lun reset command. If all cmds get cleaned, the LUN Reset
+	 * succeeds.
+	 */
+
+	ret = snic_dr_clean_pending_req(snic, sc);
+	if (ret) {
+		spin_lock_irqsave(io_lock, flags);
+		SNIC_SCSI_DBG(snic->shost,
+			      "dr_fini: Device Reset Failed since could not abort all IOs. Tag = %x.\n",
+			      snic_cmd_tag(sc));
+		rqi = (struct snic_req_info *) CMD_SP(sc);
+
+		goto dr_failed;
+	} else {
+		/* Cleanup LUN Reset Command */
+		spin_lock_irqsave(io_lock, flags);
+		rqi = (struct snic_req_info *) CMD_SP(sc);
+		if (rqi)
+			ret = SUCCESS; /* Completed Successfully */
+		else
+			ret = FAILED;
+	}
+
+dr_failed:
+	SNIC_BUG_ON(!spin_is_locked(io_lock));
+	if (rqi)
+		CMD_SP(sc) = NULL;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	if (rqi)
+		snic_release_req_buf(snic, rqi, sc);
+
+dr_fini_end:
+	return ret;
+} /* end of snic_dr_finish */
+
+static int
+snic_queue_dr_req(struct snic *snic,
+		  struct snic_req_info *rqi,
+		  struct scsi_cmnd *sc)
+{
+	/* Add special tag for device reset */
+	rqi->tm_tag |= SNIC_TAG_DEV_RST;
+
+	return snic_issue_tm_req(snic, rqi, sc, SNIC_ITMF_LUN_RESET);
+}
+
+static int
+snic_send_dr_and_wait(struct snic *snic, struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = NULL;
+	enum snic_ioreq_state sv_state;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+	DECLARE_COMPLETION_ONSTACK(tm_done);
+	int ret = FAILED, tag = snic_cmd_tag(sc);
+
+	io_lock = snic_io_lock_hash(snic, sc);
+	spin_lock_irqsave(io_lock, flags);
+	CMD_FLAGS(sc) |= SNIC_DEVICE_RESET;
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	if (!rqi) {
+		SNIC_HOST_ERR(snic->shost,
+			      "send_dr: rqi is null, Tag 0x%x flags 0x%llx\n",
+			      tag, CMD_FLAGS(sc));
+		spin_unlock_irqrestore(io_lock, flags);
+
+		ret = FAILED;
+		goto send_dr_end;
+	}
+
+	/* Save Command state to restore in case Queuing failed. */
+	sv_state = CMD_STATE(sc);
+
+	CMD_STATE(sc) = SNIC_IOREQ_LR_PENDING;
+	CMD_LR_STATUS(sc) = SNIC_INVALID_CODE;
+
+	SNIC_SCSI_DBG(snic->shost, "dr: TAG = %x\n", tag);
+
+	rqi->dr_done = &tm_done;
+	SNIC_BUG_ON(!rqi->dr_done);
+
+	spin_unlock_irqrestore(io_lock, flags);
+	/*
+	 * The Command state is changed to IOREQ_PENDING,
+	 * in this case, if the command is completed, the icmnd_cmpl will
+	 * mark the cmd as completed.
+	 * This logic still makes LUN Reset is inevitable.
+	 */
+
+	ret = snic_queue_dr_req(snic, rqi, sc);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "send_dr: IO w/ Tag 0x%x Failed err = %d. flags 0x%llx\n",
+			      tag, ret, CMD_FLAGS(sc));
+
+		spin_lock_irqsave(io_lock, flags);
+		/* Restore State */
+		CMD_STATE(sc) = sv_state;
+		rqi = (struct snic_req_info *) CMD_SP(sc);
+		if (rqi)
+			rqi->dr_done = NULL;
+		/* rqi is freed in caller. */
+		spin_unlock_irqrestore(io_lock, flags);
+		ret = FAILED;
+
+		goto send_dr_end;
+	}
+
+	spin_lock_irqsave(io_lock, flags);
+	CMD_FLAGS(sc) |= SNIC_DEV_RST_ISSUED;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	ret = 0;
+
+	wait_for_completion_timeout(&tm_done, SNIC_LUN_RESET_TIMEOUT);
+
+send_dr_end:
+	return ret;
+}
+
+/*
+ * auxillary funciton to check lun reset op is supported or not
+ * Not supported if returns 0
+ */
+static int
+snic_dev_reset_supported(struct scsi_device *sdev)
+{
+	struct snic_tgt *tgt = starget_to_tgt(scsi_target(sdev));
+
+	if (tgt->tdata.typ == SNIC_TGT_DAS)
+		return 0;
+
+	return 1;
+}
+
+static void
+snic_unlink_and_release_req(struct snic *snic, struct scsi_cmnd *sc, int flag)
+{
+	struct snic_req_info *rqi = NULL;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+	u32 start_time = jiffies;
+
+	io_lock = snic_io_lock_hash(snic, sc);
+	spin_lock_irqsave(io_lock, flags);
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	if (rqi) {
+		start_time = rqi->start_time;
+		CMD_SP(sc) = NULL;
+	}
+
+	CMD_FLAGS(sc) |= flag;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	if (rqi)
+		snic_release_req_buf(snic, rqi, sc);
+
+	SNIC_TRC(snic->shost->host_no, snic_cmd_tag(sc), (ulong) sc,
+		 jiffies_to_msecs(jiffies - start_time), (ulong) rqi,
+		 SNIC_TRC_CMD(sc), SNIC_TRC_CMD_STATE_FLAGS(sc));
+}
+
+/*
+ * SCSI Eh thread issues a LUN Reset when one or more commands on a LUN
+ * fail to get aborted. It calls driver's eh_device_reset with a SCSI
+ * command on the LUN.
+ */
+int
+snic_device_reset(struct scsi_cmnd *sc)
+{
+	struct Scsi_Host *shost = sc->device->host;
+	struct snic *snic = shost_priv(shost);
+	struct snic_req_info *rqi = NULL;
+	int tag = snic_cmd_tag(sc);
+	int start_time = jiffies;
+	int ret = FAILED;
+	int dr_supp = 0;
+
+	SNIC_SCSI_DBG(shost, "dev_reset:sc %p :0x%x :req = %p :tag = %d\n",
+		      sc, sc->cmnd[0], sc->request,
+		      snic_cmd_tag(sc));
+	dr_supp = snic_dev_reset_supported(sc->device);
+	if (!dr_supp) {
+		/* device reset op is not supported */
+		SNIC_HOST_INFO(shost, "LUN Reset Op not supported.\n");
+		snic_unlink_and_release_req(snic, sc, SNIC_DEV_RST_NOTSUP);
+
+		goto dev_rst_end;
+	}
+
+	if (unlikely(snic_get_state(snic) != SNIC_ONLINE)) {
+		snic_unlink_and_release_req(snic, sc, 0);
+		SNIC_HOST_ERR(shost, "Devrst: Parent Devs are not online.\n");
+
+		goto dev_rst_end;
+	}
+
+	/* There is no tag when lun reset is issue through ioctl. */
+	if (unlikely(tag <= SNIC_NO_TAG)) {
+		SNIC_HOST_INFO(snic->shost,
+			       "Devrst: LUN Reset Recvd thru IOCTL.\n");
+
+		rqi = snic_req_init(snic, 0);
+		if (!rqi)
+			goto dev_rst_end;
+
+		memset(scsi_cmd_priv(sc), 0,
+			sizeof(struct snic_internal_io_state));
+		CMD_SP(sc) = (char *)rqi;
+		CMD_FLAGS(sc) = SNIC_NO_FLAGS;
+
+		/* Add special tag for dr coming from user spc */
+		rqi->tm_tag = SNIC_TAG_IOCTL_DEV_RST;
+		rqi->sc = sc;
+	}
+
+	ret = snic_send_dr_and_wait(snic, sc);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "Devrst: IO w/ Tag %x Failed w/ err = %d\n",
+			      tag, ret);
+
+		snic_unlink_and_release_req(snic, sc, 0);
+
+		goto dev_rst_end;
+	}
+
+	ret = snic_dr_finish(snic, sc);
+
+dev_rst_end:
+	SNIC_TRC(snic->shost->host_no, tag, (ulong) sc,
+		 jiffies_to_msecs(jiffies - start_time),
+		 0, SNIC_TRC_CMD(sc), SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "Devrst: Returning from Device Reset : %s\n",
+		      (ret == SUCCESS) ? "SUCCESS" : "FAILED");
+
+	return ret;
+} /* end of snic_device_reset */
+
+/*
+ * SCSI Error handling calls driver's eh_host_reset if all prior
+ * error handling levels return FAILED.
+ *
+ * Host Reset is the highest level of error recovery. If this fails, then
+ * host is offlined by SCSI.
+ */
+/*
+ * snic_issue_hba_reset : Queues FW Reset Request.
+ */
+static int
+snic_issue_hba_reset(struct snic *snic, struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = NULL;
+	struct snic_host_req *req = NULL;
+	spinlock_t *io_lock = NULL;
+	DECLARE_COMPLETION_ONSTACK(wait);
+	unsigned long flags;
+	int ret = -ENOMEM;
+
+	rqi = snic_req_init(snic, 0);
+	if (!rqi) {
+		ret = -ENOMEM;
+
+		goto hba_rst_end;
+	}
+
+	if (snic_cmd_tag(sc) == SCSI_NO_TAG) {
+		memset(scsi_cmd_priv(sc), 0,
+			sizeof(struct snic_internal_io_state));
+		SNIC_HOST_INFO(snic->shost, "issu_hr:Host reset thru ioctl.\n");
+		rqi->sc = sc;
+	}
+
+	req = rqi_to_req(rqi);
+
+	io_lock = snic_io_lock_hash(snic, sc);
+	spin_lock_irqsave(io_lock, flags);
+	SNIC_BUG_ON(CMD_SP(sc) != NULL);
+	CMD_STATE(sc) = SNIC_IOREQ_PENDING;
+	CMD_SP(sc) = (char *) rqi;
+	CMD_FLAGS(sc) |= SNIC_IO_INITIALIZED;
+	snic->remove_wait = &wait;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	/* Initialize Request */
+	snic_io_hdr_enc(&req->hdr, SNIC_REQ_HBA_RESET, 0, snic_cmd_tag(sc),
+			snic->config.hid, 0, (ulong) rqi);
+
+	req->u.reset.flags = 0;
+
+	ret = snic_queue_wq_desc(snic, req, sizeof(*req));
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "issu_hr:Queuing HBA Reset Failed. w err %d\n",
+			      ret);
+
+		goto hba_rst_err;
+	}
+
+	spin_lock_irqsave(io_lock, flags);
+	CMD_FLAGS(sc) |= SNIC_HOST_RESET_ISSUED;
+	spin_unlock_irqrestore(io_lock, flags);
+	atomic64_inc(&snic->s_stats.reset.hba_resets);
+	SNIC_HOST_INFO(snic->shost, "Queued HBA Reset Successfully.\n");
+
+	wait_for_completion_timeout(snic->remove_wait,
+				    SNIC_HOST_RESET_TIMEOUT);
+
+	if (snic_get_state(snic) == SNIC_FWRESET) {
+		SNIC_HOST_ERR(snic->shost, "reset_cmpl: Reset Timedout.\n");
+		ret = -ETIMEDOUT;
+
+		goto hba_rst_err;
+	}
+
+	spin_lock_irqsave(io_lock, flags);
+	snic->remove_wait = NULL;
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	CMD_SP(sc) = NULL;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	if (rqi)
+		snic_req_free(snic, rqi);
+
+	ret = 0;
+
+	return ret;
+
+hba_rst_err:
+	spin_lock_irqsave(io_lock, flags);
+	snic->remove_wait = NULL;
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	CMD_SP(sc) = NULL;
+	spin_unlock_irqrestore(io_lock, flags);
+
+	if (rqi)
+		snic_req_free(snic, rqi);
+
+hba_rst_end:
+	SNIC_HOST_ERR(snic->shost,
+		      "reset:HBA Reset Failed w/ err = %d.\n",
+		      ret);
+
+	return ret;
+} /* end of snic_issue_hba_reset */
+
+int
+snic_reset(struct Scsi_Host *shost, struct scsi_cmnd *sc)
+{
+	struct snic *snic = shost_priv(shost);
+	enum snic_state sv_state;
+	unsigned long flags;
+	int ret = FAILED;
+
+	/* Set snic state as SNIC_FWRESET*/
+	sv_state = snic_get_state(snic);
+
+	spin_lock_irqsave(&snic->snic_lock, flags);
+	if (snic_get_state(snic) == SNIC_FWRESET) {
+		spin_unlock_irqrestore(&snic->snic_lock, flags);
+		SNIC_HOST_INFO(shost, "reset:prev reset is in progres\n");
+
+		msleep(SNIC_HOST_RESET_TIMEOUT);
+		ret = SUCCESS;
+
+		goto reset_end;
+	}
+
+	snic_set_state(snic, SNIC_FWRESET);
+	spin_unlock_irqrestore(&snic->snic_lock, flags);
+
+
+	/* Wait for all the IOs that are entered in Qcmd */
+	while (atomic_read(&snic->ios_inflight))
+		schedule_timeout(msecs_to_jiffies(1));
+
+	ret = snic_issue_hba_reset(snic, sc);
+	if (ret) {
+		SNIC_HOST_ERR(shost,
+			      "reset:Host Reset Failed w/ err %d.\n",
+			      ret);
+		spin_lock_irqsave(&snic->snic_lock, flags);
+		snic_set_state(snic, sv_state);
+		spin_unlock_irqrestore(&snic->snic_lock, flags);
+		atomic64_inc(&snic->s_stats.reset.hba_reset_fail);
+		ret = FAILED;
+
+		goto reset_end;
+	}
+
+	ret = SUCCESS;
+
+reset_end:
+	return ret;
+} /* end of snic_reset */
+
+/*
+ * SCSI Error handling calls driver's eh_host_reset if all prior
+ * error handling levels return FAILED.
+ *
+ * Host Reset is the highest level of error recovery. If this fails, then
+ * host is offlined by SCSI.
+ */
+int
+snic_host_reset(struct scsi_cmnd *sc)
+{
+	struct Scsi_Host *shost = sc->device->host;
+	u32 start_time  = jiffies;
+	int ret = FAILED;
+
+	SNIC_SCSI_DBG(shost,
+		      "host reset:sc %p sc_cmd 0x%x req %p tag %d flags 0x%llx\n",
+		      sc, sc->cmnd[0], sc->request,
+		      snic_cmd_tag(sc), CMD_FLAGS(sc));
+
+	ret = snic_reset(shost, sc);
+
+	SNIC_TRC(shost->host_no, snic_cmd_tag(sc), (ulong) sc,
+		 jiffies_to_msecs(jiffies - start_time),
+		 0, SNIC_TRC_CMD(sc), SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+	return ret;
+} /* end of snic_host_reset */
+
+/*
+ * snic_cmpl_pending_tmreq : Caller should hold io_lock
+ */
+static void
+snic_cmpl_pending_tmreq(struct snic *snic, struct scsi_cmnd *sc)
+{
+	struct snic_req_info *rqi = NULL;
+
+	SNIC_SCSI_DBG(snic->shost,
+		      "Completing Pending TM Req sc %p, state %s flags 0x%llx\n",
+		      sc, snic_io_status_to_str(CMD_STATE(sc)), CMD_FLAGS(sc));
+
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	if (!rqi)
+		return;
+
+	if (rqi->dr_done)
+		complete(rqi->dr_done);
+	else if (rqi->abts_done)
+		complete(rqi->abts_done);
+}
+
+/*
+ * snic_scsi_cleanup: Walks through tag map and releases the reqs
+ */
+static void
+snic_scsi_cleanup(struct snic *snic, int ex_tag)
+{
+	struct snic_req_info *rqi = NULL;
+	struct scsi_cmnd *sc = NULL;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+	int tag;
+	u64 st_time = 0;
+
+	SNIC_SCSI_DBG(snic->shost, "sc_clean: scsi cleanup.\n");
+
+	for (tag = 0; tag < snic->max_tag_id; tag++) {
+		/* Skip ex_tag */
+		if (tag == ex_tag)
+			continue;
+
+		io_lock = snic_io_lock_tag(snic, tag);
+		spin_lock_irqsave(io_lock, flags);
+		sc = scsi_host_find_tag(snic->shost, tag);
+		if (!sc) {
+			spin_unlock_irqrestore(io_lock, flags);
+
+			continue;
+		}
+
+		if (unlikely(snic_tmreq_pending(sc))) {
+			/*
+			 * When FW Completes reset w/o sending completions
+			 * for outstanding ios.
+			 */
+			snic_cmpl_pending_tmreq(snic, sc);
+			spin_unlock_irqrestore(io_lock, flags);
+
+			continue;
+		}
+
+		rqi = (struct snic_req_info *) CMD_SP(sc);
+		if (!rqi) {
+			spin_unlock_irqrestore(io_lock, flags);
+
+			goto cleanup;
+		}
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "sc_clean: sc %p, rqi %p, tag %d flags 0x%llx\n",
+			      sc, rqi, tag, CMD_FLAGS(sc));
+
+		CMD_SP(sc) = NULL;
+		CMD_FLAGS(sc) |= SNIC_SCSI_CLEANUP;
+		spin_unlock_irqrestore(io_lock, flags);
+		st_time = rqi->start_time;
+
+		SNIC_HOST_INFO(snic->shost,
+			       "sc_clean: Releasing rqi %p : flags 0x%llx\n",
+			       rqi, CMD_FLAGS(sc));
+
+		snic_release_req_buf(snic, rqi, sc);
+
+cleanup:
+		sc->result = DID_TRANSPORT_DISRUPTED << 16;
+		SNIC_HOST_INFO(snic->shost,
+			       "sc_clean: DID_TRANSPORT_DISRUPTED for sc %p. rqi %p duration %llu msecs\n",
+			       sc, rqi, (jiffies - st_time));
+
+		/* Update IO stats */
+		snic_stats_update_io_cmpl(&snic->s_stats);
+
+		if (sc->scsi_done) {
+			SNIC_TRC(snic->shost->host_no, tag, (ulong) sc,
+				 jiffies_to_msecs(jiffies - st_time), 0,
+				 SNIC_TRC_CMD(sc),
+				 SNIC_TRC_CMD_STATE_FLAGS(sc));
+
+			sc->scsi_done(sc);
+		}
+	}
+} /* end of snic_scsi_cleanup */
+
+void
+snic_shutdown_scsi_cleanup(struct snic *snic)
+{
+	SNIC_HOST_INFO(snic->shost, "Shutdown time SCSI Cleanup.\n");
+
+	snic_scsi_cleanup(snic, SCSI_NO_TAG);
+} /* end of snic_shutdown_scsi_cleanup */
+
+/*
+ * snic_internal_abort_io
+ * called by : snic_tgt_scsi_abort_io
+ */
+static int
+snic_internal_abort_io(struct snic *snic, struct scsi_cmnd *sc, int tmf)
+{
+	struct snic_req_info *rqi = NULL;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+	u32 sv_state = 0;
+	int ret = 0;
+
+	io_lock = snic_io_lock_hash(snic, sc);
+	spin_lock_irqsave(io_lock, flags);
+	rqi = (struct snic_req_info *) CMD_SP(sc);
+	if (!rqi)
+		goto skip_internal_abts;
+
+	if (CMD_STATE(sc) == SNIC_IOREQ_ABTS_PENDING)
+		goto skip_internal_abts;
+
+	if ((CMD_FLAGS(sc) & SNIC_DEVICE_RESET) &&
+		(!(CMD_FLAGS(sc) & SNIC_DEV_RST_ISSUED))) {
+
+		SNIC_SCSI_DBG(snic->shost,
+			      "internal_abts: dev rst not pending sc 0x%p\n",
+			      sc);
+
+		goto skip_internal_abts;
+	}
+
+
+	if (!(CMD_FLAGS(sc) & SNIC_IO_ISSUED)) {
+		SNIC_SCSI_DBG(snic->shost,
+			"internal_abts: IO not yet issued sc 0x%p tag 0x%x flags 0x%llx state %d\n",
+			sc, snic_cmd_tag(sc), CMD_FLAGS(sc), CMD_STATE(sc));
+
+		goto skip_internal_abts;
+	}
+
+	sv_state = CMD_STATE(sc);
+	CMD_STATE(sc) = SNIC_IOREQ_ABTS_PENDING;
+	CMD_ABTS_STATUS(sc) = SNIC_INVALID_CODE;
+	CMD_FLAGS(sc) |= SNIC_IO_INTERNAL_TERM_PENDING;
+
+	if (CMD_FLAGS(sc) & SNIC_DEVICE_RESET) {
+		/* stats */
+		rqi->tm_tag = SNIC_TAG_DEV_RST;
+		SNIC_SCSI_DBG(snic->shost, "internal_abts:dev rst sc %p\n", sc);
+	}
+
+	SNIC_SCSI_DBG(snic->shost, "internal_abts: Issuing abts tag %x\n",
+		      snic_cmd_tag(sc));
+	SNIC_BUG_ON(rqi->abts_done);
+	spin_unlock_irqrestore(io_lock, flags);
+
+	ret = snic_queue_abort_req(snic, rqi, sc, tmf);
+	if (ret) {
+		SNIC_HOST_ERR(snic->shost,
+			      "internal_abts: Tag = %x , Failed w/ err = %d\n",
+			      snic_cmd_tag(sc), ret);
+
+		spin_lock_irqsave(io_lock, flags);
+
+		if (CMD_STATE(sc) == SNIC_IOREQ_ABTS_PENDING)
+			CMD_STATE(sc) = sv_state;
+
+		goto skip_internal_abts;
+	}
+
+	spin_lock_irqsave(io_lock, flags);
+	if (CMD_FLAGS(sc) & SNIC_DEVICE_RESET)
+		CMD_FLAGS(sc) |= SNIC_DEV_RST_TERM_ISSUED;
+	else
+		CMD_FLAGS(sc) |= SNIC_IO_INTERNAL_TERM_ISSUED;
+
+	ret = SUCCESS;
+
+skip_internal_abts:
+	SNIC_BUG_ON(!spin_is_locked(io_lock));
+	spin_unlock_irqrestore(io_lock, flags);
+
+	return ret;
+} /* end of snic_internal_abort_io */
+
+/*
+ * snic_tgt_scsi_abort_io : called by snic_tgt_del
+ */
+int
+snic_tgt_scsi_abort_io(struct snic_tgt *tgt)
+{
+	struct snic *snic = NULL;
+	struct scsi_cmnd *sc = NULL;
+	struct snic_tgt *sc_tgt = NULL;
+	spinlock_t *io_lock = NULL;
+	unsigned long flags;
+	int ret = 0, tag, abt_cnt = 0, tmf = 0;
+
+	if (!tgt)
+		return -1;
+
+	snic = shost_priv(snic_tgt_to_shost(tgt));
+	SNIC_SCSI_DBG(snic->shost, "tgt_abt_io: Cleaning Pending IOs.\n");
+
+	if (tgt->tdata.typ == SNIC_TGT_DAS)
+		tmf = SNIC_ITMF_ABTS_TASK;
+	else
+		tmf = SNIC_ITMF_ABTS_TASK_TERM;
+
+	for (tag = 0; tag < snic->max_tag_id; tag++) {
+		io_lock = snic_io_lock_tag(snic, tag);
+
+		spin_lock_irqsave(io_lock, flags);
+		sc = scsi_host_find_tag(snic->shost, tag);
+		if (!sc) {
+			spin_unlock_irqrestore(io_lock, flags);
+
+			continue;
+		}
+
+		sc_tgt = starget_to_tgt(scsi_target(sc->device));
+		if (sc_tgt != tgt) {
+			spin_unlock_irqrestore(io_lock, flags);
+
+			continue;
+		}
+		spin_unlock_irqrestore(io_lock, flags);
+
+		ret = snic_internal_abort_io(snic, sc, tmf);
+		if (ret < 0) {
+			SNIC_HOST_ERR(snic->shost,
+				      "tgt_abt_io: Tag %x, Failed w err = %d\n",
+				      tag, ret);
+
+			continue;
+		}
+
+		if (ret == SUCCESS)
+			abt_cnt++;
+	}
+
+	SNIC_SCSI_DBG(snic->shost, "tgt_abt_io: abt_cnt = %d\n", abt_cnt);
+
+	return 0;
+} /* end of snic_tgt_scsi_abort_io */
diff --git a/drivers/scsi/snic/snic_stats.h b/drivers/scsi/snic/snic_stats.h
new file mode 100644
index 000000000000..11e614849a82
--- /dev/null
+++ b/drivers/scsi/snic/snic_stats.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __SNIC_STATS_H
+#define __SNIC_STATS_H
+
+struct snic_io_stats {
+	atomic64_t active;		/* Active IOs */
+	atomic64_t max_active;		/* Max # active IOs */
+	atomic64_t max_sgl;		/* Max # SGLs for any IO */
+	atomic64_t max_time;		/* Max time to process IO */
+	atomic64_t max_qtime;		/* Max time to Queue the IO */
+	atomic64_t max_cmpl_time;	/* Max time to complete the IO */
+	atomic64_t sgl_cnt[SNIC_MAX_SG_DESC_CNT]; /* SGL Counters */
+	atomic64_t max_io_sz;		/* Max IO Size */
+	atomic64_t compl;		/* IO Completions */
+	atomic64_t fail;		/* IO Failures */
+	atomic64_t req_null;		/* req or req info is NULL */
+	atomic64_t alloc_fail;		/* Alloc Failures */
+	atomic64_t sc_null;
+	atomic64_t io_not_found;	/* IO Not Found */
+	atomic64_t num_ios;		/* Number of IOs */
+};
+
+struct snic_abort_stats {
+	atomic64_t num;		/* Abort counter */
+	atomic64_t fail;	/* Abort Failure Counter */
+	atomic64_t drv_tmo;	/* Abort Driver Timeouts */
+	atomic64_t fw_tmo;	/* Abort Firmware Timeouts */
+	atomic64_t io_not_found;/* Abort IO Not Found */
+};
+
+struct snic_reset_stats {
+	atomic64_t dev_resets;		/* Device Reset Counter */
+	atomic64_t dev_reset_fail;	/* Device Reset Failures */
+	atomic64_t dev_reset_aborts;	/* Device Reset Aborts */
+	atomic64_t dev_reset_tmo;	/* Device Reset Timeout */
+	atomic64_t dev_reset_terms;	/* Device Reset terminate */
+	atomic64_t hba_resets;		/* hba/firmware resets */
+	atomic64_t hba_reset_cmpl;	/* hba/firmware reset completions */
+	atomic64_t hba_reset_fail;	/* hba/firmware failures */
+	atomic64_t snic_resets;		/* snic resets */
+	atomic64_t snic_reset_compl;	/* snic reset completions */
+	atomic64_t snic_reset_fail;	/* snic reset failures */
+};
+
+struct snic_fw_stats {
+	atomic64_t actv_reqs;		/* Active Requests */
+	atomic64_t max_actv_reqs;	/* Max Active Requests */
+	atomic64_t out_of_res;		/* Firmware Out Of Resources */
+	atomic64_t io_errs;		/* Firmware IO Firmware Errors */
+	atomic64_t scsi_errs;		/* Target hits check condition */
+};
+
+struct snic_misc_stats {
+	u64	last_isr_time;
+	u64	last_ack_time;
+	atomic64_t isr_cnt;
+	atomic64_t max_cq_ents;		/* Max CQ Entries */
+	atomic64_t data_cnt_mismat;	/* Data Count Mismatch */
+	atomic64_t io_tmo;
+	atomic64_t io_aborted;
+	atomic64_t sgl_inval;		/* SGL Invalid */
+	atomic64_t abts_wq_alloc_fail;	/* Abort Path WQ desc alloc failure */
+	atomic64_t devrst_wq_alloc_fail;/* Device Reset - WQ desc alloc fail */
+	atomic64_t wq_alloc_fail;	/* IO WQ desc alloc failure */
+	atomic64_t no_icmnd_itmf_cmpls;
+	atomic64_t io_under_run;
+	atomic64_t qfull;
+	atomic64_t tgt_not_rdy;
+};
+
+struct snic_stats {
+	struct snic_io_stats io;
+	struct snic_abort_stats abts;
+	struct snic_reset_stats reset;
+	struct snic_fw_stats fw;
+	struct snic_misc_stats misc;
+	atomic64_t io_cmpl_skip;
+};
+
+int snic_stats_debugfs_init(struct snic *);
+void snic_stats_debugfs_remove(struct snic *);
+
+/* Auxillary function to update active IO counter */
+static inline void
+snic_stats_update_active_ios(struct snic_stats *s_stats)
+{
+	struct snic_io_stats *io = &s_stats->io;
+	u32 nr_active_ios;
+
+	nr_active_ios = atomic64_inc_return(&io->active);
+	if (atomic64_read(&io->max_active) < nr_active_ios)
+		atomic64_set(&io->max_active, nr_active_ios);
+
+	atomic64_inc(&io->num_ios);
+}
+
+/* Auxillary function to update IO completion counter */
+static inline void
+snic_stats_update_io_cmpl(struct snic_stats *s_stats)
+{
+	atomic64_dec(&s_stats->io.active);
+	if (unlikely(atomic64_read(&s_stats->io_cmpl_skip)))
+		atomic64_dec(&s_stats->io_cmpl_skip);
+	else
+		atomic64_inc(&s_stats->io.compl);
+}
+#endif /* __SNIC_STATS_H */
diff --git a/drivers/scsi/snic/snic_trc.c b/drivers/scsi/snic/snic_trc.c
new file mode 100644
index 000000000000..28a40a7ade38
--- /dev/null
+++ b/drivers/scsi/snic/snic_trc.c
@@ -0,0 +1,181 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/module.h>
+#include <linux/mempool.h>
+#include <linux/errno.h>
+#include <linux/vmalloc.h>
+
+#include "snic_io.h"
+#include "snic.h"
+
+/*
+ * snic_get_trc_buf : Allocates a trace record and returns.
+ */
+struct snic_trc_data *
+snic_get_trc_buf(void)
+{
+	struct snic_trc *trc = &snic_glob->trc;
+	struct snic_trc_data *td = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&trc->lock, flags);
+	td = &trc->buf[trc->wr_idx];
+	trc->wr_idx++;
+
+	if (trc->wr_idx == trc->max_idx)
+		trc->wr_idx = 0;
+
+	if (trc->wr_idx != trc->rd_idx) {
+		spin_unlock_irqrestore(&trc->lock, flags);
+
+		goto end;
+	}
+
+	trc->rd_idx++;
+	if (trc->rd_idx == trc->max_idx)
+		trc->rd_idx = 0;
+
+	td->ts = 0;	/* Marker for checking the record, for complete data*/
+	spin_unlock_irqrestore(&trc->lock, flags);
+
+end:
+
+	return td;
+} /* end of snic_get_trc_buf */
+
+/*
+ * snic_fmt_trc_data : Formats trace data for printing.
+ */
+static int
+snic_fmt_trc_data(struct snic_trc_data *td, char *buf, int buf_sz)
+{
+	int len = 0;
+	struct timespec tmspec;
+
+	jiffies_to_timespec(td->ts, &tmspec);
+
+	len += snprintf(buf, buf_sz,
+			"%lu.%10lu %-25s %3d %4x %16llx %16llx %16llx %16llx %16llx\n",
+			tmspec.tv_sec,
+			tmspec.tv_nsec,
+			td->fn,
+			td->hno,
+			td->tag,
+			td->data[0], td->data[1], td->data[2], td->data[3],
+			td->data[4]);
+
+	return len;
+} /* end of snic_fmt_trc_data */
+
+/*
+ * snic_get_trc_data : Returns a formatted trace buffer.
+ */
+int
+snic_get_trc_data(char *buf, int buf_sz)
+{
+	struct snic_trc_data *td = NULL;
+	struct snic_trc *trc = &snic_glob->trc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&trc->lock, flags);
+	if (trc->rd_idx == trc->wr_idx) {
+		spin_unlock_irqrestore(&trc->lock, flags);
+
+		return -1;
+	}
+	td = &trc->buf[trc->rd_idx];
+
+	if (td->ts == 0) {
+		/* write in progress. */
+		spin_unlock_irqrestore(&trc->lock, flags);
+
+		return -1;
+	}
+
+	trc->rd_idx++;
+	if (trc->rd_idx == trc->max_idx)
+		trc->rd_idx = 0;
+	spin_unlock_irqrestore(&trc->lock, flags);
+
+	return snic_fmt_trc_data(td, buf, buf_sz);
+} /* end of snic_get_trc_data */
+
+/*
+ * snic_trc_init() : Configures Trace Functionality for snic.
+ */
+int
+snic_trc_init(void)
+{
+	struct snic_trc *trc = &snic_glob->trc;
+	void *tbuf = NULL;
+	int tbuf_sz = 0, ret;
+
+	tbuf_sz = (snic_trace_max_pages * PAGE_SIZE);
+	tbuf = vmalloc(tbuf_sz);
+	if (!tbuf) {
+		SNIC_ERR("Failed to Allocate Trace Buffer Size. %d\n", tbuf_sz);
+		SNIC_ERR("Trace Facility not enabled.\n");
+		ret = -ENOMEM;
+
+		return ret;
+	}
+
+	memset(tbuf, 0, tbuf_sz);
+	trc->buf = (struct snic_trc_data *) tbuf;
+	spin_lock_init(&trc->lock);
+
+	ret = snic_trc_debugfs_init();
+	if (ret) {
+		SNIC_ERR("Failed to create Debugfs Files.\n");
+
+		goto error;
+	}
+
+	trc->max_idx = (tbuf_sz / SNIC_TRC_ENTRY_SZ);
+	trc->rd_idx = trc->wr_idx = 0;
+	trc->enable = 1;
+	SNIC_INFO("Trace Facility Enabled.\n Trace Buffer SZ %lu Pages.\n",
+		  tbuf_sz / PAGE_SIZE);
+	ret = 0;
+
+	return ret;
+
+error:
+	snic_trc_free();
+
+	return ret;
+} /* end of snic_trc_init */
+
+/*
+ * snic_trc_free : Releases the trace buffer and disables the tracing.
+ */
+void
+snic_trc_free(void)
+{
+	struct snic_trc *trc = &snic_glob->trc;
+
+	trc->enable = 0;
+	snic_trc_debugfs_term();
+
+	if (trc->buf) {
+		vfree(trc->buf);
+		trc->buf = NULL;
+	}
+
+	SNIC_INFO("Trace Facility Disabled.\n");
+} /* end of snic_trc_free */
diff --git a/drivers/scsi/snic/snic_trc.h b/drivers/scsi/snic/snic_trc.h
new file mode 100644
index 000000000000..427faee5f97e
--- /dev/null
+++ b/drivers/scsi/snic/snic_trc.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __SNIC_TRC_H
+#define __SNIC_TRC_H
+
+#ifdef CONFIG_SCSI_SNIC_DEBUG_FS
+
+extern ssize_t simple_read_from_buffer(void __user *to,
+					size_t count,
+					loff_t *ppos,
+					const void *from,
+					size_t available);
+
+extern unsigned int snic_trace_max_pages;
+
+/* Global Data structure for trace to manage trace functionality */
+struct snic_trc_data {
+	u64	ts;		/* Time Stamp */
+	char	*fn;		/* Ptr to Function Name */
+	u32	hno;		/* SCSI Host ID */
+	u32	tag;		/* Command Tag */
+	u64 data[5];
+} __attribute__((__packed__));
+
+#define SNIC_TRC_ENTRY_SZ  64	/* in Bytes */
+
+struct snic_trc {
+	spinlock_t lock;
+	struct snic_trc_data *buf;	/* Trace Buffer */
+	u32	max_idx;		/* Max Index into trace buffer */
+	u32	rd_idx;
+	u32	wr_idx;
+	u32	enable;			/* Control Variable for Tracing */
+
+	struct dentry *trc_enable;	/* debugfs file object */
+	struct dentry *trc_file;
+};
+
+int snic_trc_init(void);
+void snic_trc_free(void);
+int snic_trc_debugfs_init(void);
+void snic_trc_debugfs_term(void);
+struct snic_trc_data *snic_get_trc_buf(void);
+int snic_get_trc_data(char *buf, int buf_sz);
+
+int snic_debugfs_init(void);
+void snic_debugfs_term(void);
+
+static inline void
+snic_trace(char *fn, u16 hno, u32 tag, u64 d1, u64 d2, u64 d3, u64 d4, u64 d5)
+{
+	struct snic_trc_data *tr_rec = snic_get_trc_buf();
+
+	if (!tr_rec)
+		return;
+
+	tr_rec->fn = (char *)fn;
+	tr_rec->hno = hno;
+	tr_rec->tag = tag;
+	tr_rec->data[0] = d1;
+	tr_rec->data[1] = d2;
+	tr_rec->data[2] = d3;
+	tr_rec->data[3] = d4;
+	tr_rec->data[4] = d5;
+	tr_rec->ts = jiffies; /* Update time stamp at last */
+}
+
+#define SNIC_TRC(_hno, _tag, d1, d2, d3, d4, d5)			\
+	do {								\
+		if (unlikely(snic_glob->trc.enable))			\
+			snic_trace((char *)__func__,			\
+				   (u16)(_hno),				\
+				   (u32)(_tag),				\
+				   (u64)(d1),				\
+				   (u64)(d2),				\
+				   (u64)(d3),				\
+				   (u64)(d4),				\
+				   (u64)(d5));				\
+	} while (0)
+#else
+
+#define SNIC_TRC(_hno, _tag, d1, d2, d3, d4, d5)	\
+	do {						\
+		if (unlikely(snic_log_level & 0x2))	\
+			SNIC_DBG("SnicTrace: %s %2u %2u %llx %llx %llx %llx %llx", \
+				 (char *)__func__,	\
+				 (u16)(_hno),		\
+				 (u32)(_tag),		\
+				 (u64)(d1),		\
+				 (u64)(d2),		\
+				 (u64)(d3),		\
+				 (u64)(d4),		\
+				 (u64)(d5));		\
+	} while (0)
+#endif /* end of CONFIG_SCSI_SNIC_DEBUG_FS */
+
+#define SNIC_TRC_CMD(sc)	\
+	((u64)sc->cmnd[0] << 56 | (u64)sc->cmnd[7] << 40 |	\
+	 (u64)sc->cmnd[8] << 32 | (u64)sc->cmnd[2] << 24 |	\
+	 (u64)sc->cmnd[3] << 16 | (u64)sc->cmnd[4] << 8 |	\
+	 (u64)sc->cmnd[5])
+
+#define SNIC_TRC_CMD_STATE_FLAGS(sc)	\
+	((u64) CMD_FLAGS(sc) << 32 | CMD_STATE(sc))
+
+#endif /* end of __SNIC_TRC_H */
diff --git a/drivers/scsi/snic/vnic_cq.c b/drivers/scsi/snic/vnic_cq.c
new file mode 100644
index 000000000000..4c8e64e4fba6
--- /dev/null
+++ b/drivers/scsi/snic/vnic_cq.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/errno.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+#include "vnic_dev.h"
+#include "vnic_cq.h"
+
+void svnic_cq_free(struct vnic_cq *cq)
+{
+	svnic_dev_free_desc_ring(cq->vdev, &cq->ring);
+
+	cq->ctrl = NULL;
+}
+
+int svnic_cq_alloc(struct vnic_dev *vdev, struct vnic_cq *cq,
+	unsigned int index, unsigned int desc_count, unsigned int desc_size)
+{
+	int err;
+
+	cq->index = index;
+	cq->vdev = vdev;
+
+	cq->ctrl = svnic_dev_get_res(vdev, RES_TYPE_CQ, index);
+	if (!cq->ctrl) {
+		pr_err("Failed to hook CQ[%d] resource\n", index);
+
+		return -EINVAL;
+	}
+
+	err = svnic_dev_alloc_desc_ring(vdev, &cq->ring, desc_count, desc_size);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+void svnic_cq_init(struct vnic_cq *cq, unsigned int flow_control_enable,
+	unsigned int color_enable, unsigned int cq_head, unsigned int cq_tail,
+	unsigned int cq_tail_color, unsigned int interrupt_enable,
+	unsigned int cq_entry_enable, unsigned int cq_message_enable,
+	unsigned int interrupt_offset, u64 cq_message_addr)
+{
+	u64 paddr;
+
+	paddr = (u64)cq->ring.base_addr | VNIC_PADDR_TARGET;
+	writeq(paddr, &cq->ctrl->ring_base);
+	iowrite32(cq->ring.desc_count, &cq->ctrl->ring_size);
+	iowrite32(flow_control_enable, &cq->ctrl->flow_control_enable);
+	iowrite32(color_enable, &cq->ctrl->color_enable);
+	iowrite32(cq_head, &cq->ctrl->cq_head);
+	iowrite32(cq_tail, &cq->ctrl->cq_tail);
+	iowrite32(cq_tail_color, &cq->ctrl->cq_tail_color);
+	iowrite32(interrupt_enable, &cq->ctrl->interrupt_enable);
+	iowrite32(cq_entry_enable, &cq->ctrl->cq_entry_enable);
+	iowrite32(cq_message_enable, &cq->ctrl->cq_message_enable);
+	iowrite32(interrupt_offset, &cq->ctrl->interrupt_offset);
+	writeq(cq_message_addr, &cq->ctrl->cq_message_addr);
+}
+
+void svnic_cq_clean(struct vnic_cq *cq)
+{
+	cq->to_clean = 0;
+	cq->last_color = 0;
+
+	iowrite32(0, &cq->ctrl->cq_head);
+	iowrite32(0, &cq->ctrl->cq_tail);
+	iowrite32(1, &cq->ctrl->cq_tail_color);
+
+	svnic_dev_clear_desc_ring(&cq->ring);
+}
diff --git a/drivers/scsi/snic/vnic_cq.h b/drivers/scsi/snic/vnic_cq.h
new file mode 100644
index 000000000000..6e651c3e16f7
--- /dev/null
+++ b/drivers/scsi/snic/vnic_cq.h
@@ -0,0 +1,110 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_CQ_H_
+#define _VNIC_CQ_H_
+
+#include "cq_desc.h"
+#include "vnic_dev.h"
+
+/* Completion queue control */
+struct vnic_cq_ctrl {
+	u64 ring_base;			/* 0x00 */
+	u32 ring_size;			/* 0x08 */
+	u32 pad0;
+	u32 flow_control_enable;	/* 0x10 */
+	u32 pad1;
+	u32 color_enable;		/* 0x18 */
+	u32 pad2;
+	u32 cq_head;			/* 0x20 */
+	u32 pad3;
+	u32 cq_tail;			/* 0x28 */
+	u32 pad4;
+	u32 cq_tail_color;		/* 0x30 */
+	u32 pad5;
+	u32 interrupt_enable;		/* 0x38 */
+	u32 pad6;
+	u32 cq_entry_enable;		/* 0x40 */
+	u32 pad7;
+	u32 cq_message_enable;		/* 0x48 */
+	u32 pad8;
+	u32 interrupt_offset;		/* 0x50 */
+	u32 pad9;
+	u64 cq_message_addr;		/* 0x58 */
+	u32 pad10;
+};
+
+struct vnic_cq {
+	unsigned int index;
+	struct vnic_dev *vdev;
+	struct vnic_cq_ctrl __iomem *ctrl;	/* memory-mapped */
+	struct vnic_dev_ring ring;
+	unsigned int to_clean;
+	unsigned int last_color;
+};
+
+static inline unsigned int svnic_cq_service(struct vnic_cq *cq,
+	unsigned int work_to_do,
+	int (*q_service)(struct vnic_dev *vdev, struct cq_desc *cq_desc,
+	u8 type, u16 q_number, u16 completed_index, void *opaque),
+	void *opaque)
+{
+	struct cq_desc *cq_desc;
+	unsigned int work_done = 0;
+	u16 q_number, completed_index;
+	u8 type, color;
+
+	cq_desc = (struct cq_desc *)((u8 *)cq->ring.descs +
+		cq->ring.desc_size * cq->to_clean);
+	cq_desc_dec(cq_desc, &type, &color,
+		&q_number, &completed_index);
+
+	while (color != cq->last_color) {
+
+		if ((*q_service)(cq->vdev, cq_desc, type,
+			q_number, completed_index, opaque))
+			break;
+
+		cq->to_clean++;
+		if (cq->to_clean == cq->ring.desc_count) {
+			cq->to_clean = 0;
+			cq->last_color = cq->last_color ? 0 : 1;
+		}
+
+		cq_desc = (struct cq_desc *)((u8 *)cq->ring.descs +
+			cq->ring.desc_size * cq->to_clean);
+		cq_desc_dec(cq_desc, &type, &color,
+			&q_number, &completed_index);
+
+		work_done++;
+		if (work_done >= work_to_do)
+			break;
+	}
+
+	return work_done;
+}
+
+void svnic_cq_free(struct vnic_cq *cq);
+int svnic_cq_alloc(struct vnic_dev *vdev, struct vnic_cq *cq,
+	unsigned int index, unsigned int desc_count, unsigned int desc_size);
+void svnic_cq_init(struct vnic_cq *cq, unsigned int flow_control_enable,
+	unsigned int color_enable, unsigned int cq_head, unsigned int cq_tail,
+	unsigned int cq_tail_color, unsigned int interrupt_enable,
+	unsigned int cq_entry_enable, unsigned int message_enable,
+	unsigned int interrupt_offset, u64 message_addr);
+void svnic_cq_clean(struct vnic_cq *cq);
+#endif /* _VNIC_CQ_H_ */
diff --git a/drivers/scsi/snic/vnic_cq_fw.h b/drivers/scsi/snic/vnic_cq_fw.h
new file mode 100644
index 000000000000..c2d1bbd44bd1
--- /dev/null
+++ b/drivers/scsi/snic/vnic_cq_fw.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_CQ_FW_H_
+#define _VNIC_CQ_FW_H_
+
+#include "snic_fwint.h"
+
+static inline unsigned int
+vnic_cq_fw_service(struct vnic_cq *cq,
+		   int (*q_service)(struct vnic_dev *vdev,
+				    unsigned int index,
+				    struct snic_fw_req *desc),
+		   unsigned int work_to_do)
+
+{
+	struct snic_fw_req *desc;
+	unsigned int work_done = 0;
+	u8 color;
+
+	desc = (struct snic_fw_req *)((u8 *)cq->ring.descs +
+		cq->ring.desc_size * cq->to_clean);
+	snic_color_dec(desc, &color);
+
+	while (color != cq->last_color) {
+
+		if ((*q_service)(cq->vdev, cq->index, desc))
+			break;
+
+		cq->to_clean++;
+		if (cq->to_clean == cq->ring.desc_count) {
+			cq->to_clean = 0;
+			cq->last_color = cq->last_color ? 0 : 1;
+		}
+
+		desc = (struct snic_fw_req *)((u8 *)cq->ring.descs +
+			cq->ring.desc_size * cq->to_clean);
+		snic_color_dec(desc, &color);
+
+		work_done++;
+		if (work_done >= work_to_do)
+			break;
+	}
+
+	return work_done;
+}
+
+#endif /* _VNIC_CQ_FW_H_ */
diff --git a/drivers/scsi/snic/vnic_dev.c b/drivers/scsi/snic/vnic_dev.c
new file mode 100644
index 000000000000..e0b5549bc9fb
--- /dev/null
+++ b/drivers/scsi/snic/vnic_dev.c
@@ -0,0 +1,748 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/errno.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/if_ether.h>
+#include <linux/slab.h>
+#include "vnic_resource.h"
+#include "vnic_devcmd.h"
+#include "vnic_dev.h"
+#include "vnic_stats.h"
+#include "vnic_wq.h"
+
+#define VNIC_DVCMD_TMO	10000	/* Devcmd Timeout value */
+#define VNIC_NOTIFY_INTR_MASK 0x0000ffff00000000ULL
+
+struct devcmd2_controller {
+	struct vnic_wq_ctrl __iomem *wq_ctrl;
+	struct vnic_dev_ring results_ring;
+	struct vnic_wq wq;
+	struct vnic_devcmd2 *cmd_ring;
+	struct devcmd2_result *result;
+	u16 next_result;
+	u16 result_size;
+	int color;
+};
+
+struct vnic_res {
+	void __iomem *vaddr;
+	unsigned int count;
+};
+
+struct vnic_dev {
+	void *priv;
+	struct pci_dev *pdev;
+	struct vnic_res res[RES_TYPE_MAX];
+	enum vnic_dev_intr_mode intr_mode;
+	struct vnic_devcmd __iomem *devcmd;
+	struct vnic_devcmd_notify *notify;
+	struct vnic_devcmd_notify notify_copy;
+	dma_addr_t notify_pa;
+	u32 *linkstatus;
+	dma_addr_t linkstatus_pa;
+	struct vnic_stats *stats;
+	dma_addr_t stats_pa;
+	struct vnic_devcmd_fw_info *fw_info;
+	dma_addr_t fw_info_pa;
+	u64 args[VNIC_DEVCMD_NARGS];
+	struct devcmd2_controller *devcmd2;
+
+	int (*devcmd_rtn)(struct vnic_dev *vdev, enum vnic_devcmd_cmd cmd,
+			  int wait);
+};
+
+#define VNIC_MAX_RES_HDR_SIZE \
+	(sizeof(struct vnic_resource_header) + \
+	sizeof(struct vnic_resource) * RES_TYPE_MAX)
+#define VNIC_RES_STRIDE	128
+
+void *svnic_dev_priv(struct vnic_dev *vdev)
+{
+	return vdev->priv;
+}
+
+static int vnic_dev_discover_res(struct vnic_dev *vdev,
+	struct vnic_dev_bar *bar, unsigned int num_bars)
+{
+	struct vnic_resource_header __iomem *rh;
+	struct vnic_resource __iomem *r;
+	u8 type;
+
+	if (num_bars == 0)
+		return -EINVAL;
+
+	if (bar->len < VNIC_MAX_RES_HDR_SIZE) {
+		pr_err("vNIC BAR0 res hdr length error\n");
+
+		return -EINVAL;
+	}
+
+	rh = bar->vaddr;
+	if (!rh) {
+		pr_err("vNIC BAR0 res hdr not mem-mapped\n");
+
+		return -EINVAL;
+	}
+
+	if (ioread32(&rh->magic) != VNIC_RES_MAGIC ||
+	    ioread32(&rh->version) != VNIC_RES_VERSION) {
+		pr_err("vNIC BAR0 res magic/version error exp (%lx/%lx) curr (%x/%x)\n",
+			VNIC_RES_MAGIC, VNIC_RES_VERSION,
+			ioread32(&rh->magic), ioread32(&rh->version));
+
+		return -EINVAL;
+	}
+
+	r = (struct vnic_resource __iomem *)(rh + 1);
+
+	while ((type = ioread8(&r->type)) != RES_TYPE_EOL) {
+
+		u8 bar_num = ioread8(&r->bar);
+		u32 bar_offset = ioread32(&r->bar_offset);
+		u32 count = ioread32(&r->count);
+		u32 len;
+
+		r++;
+
+		if (bar_num >= num_bars)
+			continue;
+
+		if (!bar[bar_num].len || !bar[bar_num].vaddr)
+			continue;
+
+		switch (type) {
+		case RES_TYPE_WQ:
+		case RES_TYPE_RQ:
+		case RES_TYPE_CQ:
+		case RES_TYPE_INTR_CTRL:
+			/* each count is stride bytes long */
+			len = count * VNIC_RES_STRIDE;
+			if (len + bar_offset > bar->len) {
+				pr_err("vNIC BAR0 resource %d out-of-bounds, offset 0x%x + size 0x%x > bar len 0x%lx\n",
+					type, bar_offset,
+					len,
+					bar->len);
+
+				return -EINVAL;
+			}
+			break;
+
+		case RES_TYPE_INTR_PBA_LEGACY:
+		case RES_TYPE_DEVCMD:
+		case RES_TYPE_DEVCMD2:
+			len = count;
+			break;
+
+		default:
+			continue;
+		}
+
+		vdev->res[type].count = count;
+		vdev->res[type].vaddr = (char __iomem *)bar->vaddr + bar_offset;
+	}
+
+	return 0;
+}
+
+unsigned int svnic_dev_get_res_count(struct vnic_dev *vdev,
+	enum vnic_res_type type)
+{
+	return vdev->res[type].count;
+}
+
+void __iomem *svnic_dev_get_res(struct vnic_dev *vdev, enum vnic_res_type type,
+	unsigned int index)
+{
+	if (!vdev->res[type].vaddr)
+		return NULL;
+
+	switch (type) {
+	case RES_TYPE_WQ:
+	case RES_TYPE_RQ:
+	case RES_TYPE_CQ:
+	case RES_TYPE_INTR_CTRL:
+		return (char __iomem *)vdev->res[type].vaddr +
+					index * VNIC_RES_STRIDE;
+
+	default:
+		return (char __iomem *)vdev->res[type].vaddr;
+	}
+}
+
+unsigned int svnic_dev_desc_ring_size(struct vnic_dev_ring *ring,
+				      unsigned int desc_count,
+				      unsigned int desc_size)
+{
+	/* The base address of the desc rings must be 512 byte aligned.
+	 * Descriptor count is aligned to groups of 32 descriptors.  A
+	 * count of 0 means the maximum 4096 descriptors.  Descriptor
+	 * size is aligned to 16 bytes.
+	 */
+
+	unsigned int count_align = 32;
+	unsigned int desc_align = 16;
+
+	ring->base_align = 512;
+
+	if (desc_count == 0)
+		desc_count = 4096;
+
+	ring->desc_count = ALIGN(desc_count, count_align);
+
+	ring->desc_size = ALIGN(desc_size, desc_align);
+
+	ring->size = ring->desc_count * ring->desc_size;
+	ring->size_unaligned = ring->size + ring->base_align;
+
+	return ring->size_unaligned;
+}
+
+void svnic_dev_clear_desc_ring(struct vnic_dev_ring *ring)
+{
+	memset(ring->descs, 0, ring->size);
+}
+
+int svnic_dev_alloc_desc_ring(struct vnic_dev *vdev, struct vnic_dev_ring *ring,
+	unsigned int desc_count, unsigned int desc_size)
+{
+	svnic_dev_desc_ring_size(ring, desc_count, desc_size);
+
+	ring->descs_unaligned = pci_alloc_consistent(vdev->pdev,
+		ring->size_unaligned,
+		&ring->base_addr_unaligned);
+
+	if (!ring->descs_unaligned) {
+		pr_err("Failed to allocate ring (size=%d), aborting\n",
+			(int)ring->size);
+
+		return -ENOMEM;
+	}
+
+	ring->base_addr = ALIGN(ring->base_addr_unaligned,
+		ring->base_align);
+	ring->descs = (u8 *)ring->descs_unaligned +
+		(ring->base_addr - ring->base_addr_unaligned);
+
+	svnic_dev_clear_desc_ring(ring);
+
+	ring->desc_avail = ring->desc_count - 1;
+
+	return 0;
+}
+
+void svnic_dev_free_desc_ring(struct vnic_dev *vdev, struct vnic_dev_ring *ring)
+{
+	if (ring->descs) {
+		pci_free_consistent(vdev->pdev,
+			ring->size_unaligned,
+			ring->descs_unaligned,
+			ring->base_addr_unaligned);
+		ring->descs = NULL;
+	}
+}
+
+static int _svnic_dev_cmd2(struct vnic_dev *vdev, enum vnic_devcmd_cmd cmd,
+	int wait)
+{
+	struct devcmd2_controller *dc2c = vdev->devcmd2;
+	struct devcmd2_result *result = dc2c->result + dc2c->next_result;
+	unsigned int i;
+	int delay;
+	int err;
+	u32 posted;
+	u32 new_posted;
+
+	posted = ioread32(&dc2c->wq_ctrl->posted_index);
+
+	if (posted == 0xFFFFFFFF) { /* check for hardware gone  */
+		/* Hardware surprise removal: return error */
+		return -ENODEV;
+	}
+
+	new_posted = (posted + 1) % DEVCMD2_RING_SIZE;
+	dc2c->cmd_ring[posted].cmd = cmd;
+	dc2c->cmd_ring[posted].flags = 0;
+
+	if ((_CMD_FLAGS(cmd) & _CMD_FLAGS_NOWAIT))
+		dc2c->cmd_ring[posted].flags |= DEVCMD2_FNORESULT;
+
+	if (_CMD_DIR(cmd) & _CMD_DIR_WRITE) {
+		for (i = 0; i < VNIC_DEVCMD_NARGS; i++)
+			dc2c->cmd_ring[posted].args[i] = vdev->args[i];
+	}
+	/* Adding write memory barrier prevents compiler and/or CPU
+	 * reordering, thus avoiding descriptor posting before
+	 * descriptor is initialized. Otherwise, hardware can read
+	 * stale descriptor fields.
+	 */
+	wmb();
+	iowrite32(new_posted, &dc2c->wq_ctrl->posted_index);
+
+	if (dc2c->cmd_ring[posted].flags & DEVCMD2_FNORESULT)
+		return 0;
+
+	for (delay = 0; delay < wait; delay++) {
+		udelay(100);
+		if (result->color == dc2c->color) {
+			dc2c->next_result++;
+			if (dc2c->next_result == dc2c->result_size) {
+				dc2c->next_result = 0;
+				dc2c->color = dc2c->color ? 0 : 1;
+			}
+			if (result->error) {
+				err = (int) result->error;
+				if (err != ERR_ECMDUNKNOWN ||
+				    cmd != CMD_CAPABILITY)
+					pr_err("Error %d devcmd %d\n",
+						err, _CMD_N(cmd));
+
+				return err;
+			}
+			if (_CMD_DIR(cmd) & _CMD_DIR_READ) {
+				/*
+				 * Adding the rmb() prevents the compiler
+				 * and/or CPU from reordering the reads which
+				 * would potentially result in reading stale
+				 * values.
+				 */
+				rmb();
+				for (i = 0; i < VNIC_DEVCMD_NARGS; i++)
+					vdev->args[i] = result->results[i];
+			}
+
+			return 0;
+		}
+	}
+
+	pr_err("Timed out devcmd %d\n", _CMD_N(cmd));
+
+	return -ETIMEDOUT;
+}
+
+static int svnic_dev_init_devcmd2(struct vnic_dev *vdev)
+{
+	struct devcmd2_controller *dc2c = NULL;
+	unsigned int fetch_idx;
+	int ret;
+	void __iomem *p;
+
+	if (vdev->devcmd2)
+		return 0;
+
+	p = svnic_dev_get_res(vdev, RES_TYPE_DEVCMD2, 0);
+	if (!p)
+		return -ENODEV;
+
+	dc2c = kzalloc(sizeof(*dc2c), GFP_ATOMIC);
+	if (!dc2c)
+		return -ENOMEM;
+
+	vdev->devcmd2 = dc2c;
+
+	dc2c->color = 1;
+	dc2c->result_size = DEVCMD2_RING_SIZE;
+
+	ret  = vnic_wq_devcmd2_alloc(vdev,
+				     &dc2c->wq,
+				     DEVCMD2_RING_SIZE,
+				     DEVCMD2_DESC_SIZE);
+	if (ret)
+		goto err_free_devcmd2;
+
+	fetch_idx = ioread32(&dc2c->wq.ctrl->fetch_index);
+	if (fetch_idx == 0xFFFFFFFF) { /* check for hardware gone  */
+		/* Hardware surprise removal: reset fetch_index */
+		fetch_idx = 0;
+	}
+
+	/*
+	 * Don't change fetch_index ever and
+	 * set posted_index same as fetch_index
+	 * when setting up the WQ for devcmd2.
+	 */
+	vnic_wq_init_start(&dc2c->wq, 0, fetch_idx, fetch_idx, 0, 0);
+	svnic_wq_enable(&dc2c->wq);
+	ret = svnic_dev_alloc_desc_ring(vdev,
+					&dc2c->results_ring,
+					DEVCMD2_RING_SIZE,
+					DEVCMD2_DESC_SIZE);
+	if (ret)
+		goto err_free_wq;
+
+	dc2c->result = (struct devcmd2_result *) dc2c->results_ring.descs;
+	dc2c->cmd_ring = (struct vnic_devcmd2 *) dc2c->wq.ring.descs;
+	dc2c->wq_ctrl = dc2c->wq.ctrl;
+	vdev->args[0] = (u64) dc2c->results_ring.base_addr | VNIC_PADDR_TARGET;
+	vdev->args[1] = DEVCMD2_RING_SIZE;
+
+	ret = _svnic_dev_cmd2(vdev, CMD_INITIALIZE_DEVCMD2, VNIC_DVCMD_TMO);
+	if (ret < 0)
+		goto err_free_desc_ring;
+
+	vdev->devcmd_rtn = &_svnic_dev_cmd2;
+	pr_info("DEVCMD2 Initialized.\n");
+
+	return ret;
+
+err_free_desc_ring:
+	svnic_dev_free_desc_ring(vdev, &dc2c->results_ring);
+
+err_free_wq:
+	svnic_wq_disable(&dc2c->wq);
+	svnic_wq_free(&dc2c->wq);
+
+err_free_devcmd2:
+	kfree(dc2c);
+	vdev->devcmd2 = NULL;
+
+	return ret;
+} /* end of svnic_dev_init_devcmd2 */
+
+static void vnic_dev_deinit_devcmd2(struct vnic_dev *vdev)
+{
+	struct devcmd2_controller *dc2c = vdev->devcmd2;
+
+	vdev->devcmd2 = NULL;
+	vdev->devcmd_rtn = NULL;
+
+	svnic_dev_free_desc_ring(vdev, &dc2c->results_ring);
+	svnic_wq_disable(&dc2c->wq);
+	svnic_wq_free(&dc2c->wq);
+	kfree(dc2c);
+}
+
+int svnic_dev_cmd(struct vnic_dev *vdev, enum vnic_devcmd_cmd cmd,
+	u64 *a0, u64 *a1, int wait)
+{
+	int err;
+
+	memset(vdev->args, 0, sizeof(vdev->args));
+	vdev->args[0] = *a0;
+	vdev->args[1] = *a1;
+
+	err = (*vdev->devcmd_rtn)(vdev, cmd, wait);
+
+	*a0 = vdev->args[0];
+	*a1 = vdev->args[1];
+
+	return  err;
+}
+
+int svnic_dev_fw_info(struct vnic_dev *vdev,
+	struct vnic_devcmd_fw_info **fw_info)
+{
+	u64 a0, a1 = 0;
+	int wait = VNIC_DVCMD_TMO;
+	int err = 0;
+
+	if (!vdev->fw_info) {
+		vdev->fw_info = pci_alloc_consistent(vdev->pdev,
+			sizeof(struct vnic_devcmd_fw_info),
+			&vdev->fw_info_pa);
+		if (!vdev->fw_info)
+			return -ENOMEM;
+
+		a0 = vdev->fw_info_pa;
+
+		/* only get fw_info once and cache it */
+		err = svnic_dev_cmd(vdev, CMD_MCPU_FW_INFO, &a0, &a1, wait);
+	}
+
+	*fw_info = vdev->fw_info;
+
+	return err;
+}
+
+int svnic_dev_spec(struct vnic_dev *vdev, unsigned int offset,
+	unsigned int size, void *value)
+{
+	u64 a0, a1;
+	int wait = VNIC_DVCMD_TMO;
+	int err;
+
+	a0 = offset;
+	a1 = size;
+
+	err = svnic_dev_cmd(vdev, CMD_DEV_SPEC, &a0, &a1, wait);
+
+	switch (size) {
+	case 1:
+		*(u8 *)value = (u8)a0;
+		break;
+	case 2:
+		*(u16 *)value = (u16)a0;
+		break;
+	case 4:
+		*(u32 *)value = (u32)a0;
+		break;
+	case 8:
+		*(u64 *)value = a0;
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	return err;
+}
+
+int svnic_dev_stats_clear(struct vnic_dev *vdev)
+{
+	u64 a0 = 0, a1 = 0;
+	int wait = VNIC_DVCMD_TMO;
+
+	return svnic_dev_cmd(vdev, CMD_STATS_CLEAR, &a0, &a1, wait);
+}
+
+int svnic_dev_stats_dump(struct vnic_dev *vdev, struct vnic_stats **stats)
+{
+	u64 a0, a1;
+	int wait = VNIC_DVCMD_TMO;
+
+	if (!vdev->stats) {
+		vdev->stats = pci_alloc_consistent(vdev->pdev,
+			sizeof(struct vnic_stats), &vdev->stats_pa);
+		if (!vdev->stats)
+			return -ENOMEM;
+	}
+
+	*stats = vdev->stats;
+	a0 = vdev->stats_pa;
+	a1 = sizeof(struct vnic_stats);
+
+	return svnic_dev_cmd(vdev, CMD_STATS_DUMP, &a0, &a1, wait);
+}
+
+int svnic_dev_close(struct vnic_dev *vdev)
+{
+	u64 a0 = 0, a1 = 0;
+	int wait = VNIC_DVCMD_TMO;
+
+	return svnic_dev_cmd(vdev, CMD_CLOSE, &a0, &a1, wait);
+}
+
+int svnic_dev_enable_wait(struct vnic_dev *vdev)
+{
+	u64 a0 = 0, a1 = 0;
+	int wait = VNIC_DVCMD_TMO;
+	int err = 0;
+
+	err = svnic_dev_cmd(vdev, CMD_ENABLE_WAIT, &a0, &a1, wait);
+	if (err == ERR_ECMDUNKNOWN)
+		return svnic_dev_cmd(vdev, CMD_ENABLE, &a0, &a1, wait);
+
+	return err;
+}
+
+int svnic_dev_disable(struct vnic_dev *vdev)
+{
+	u64 a0 = 0, a1 = 0;
+	int wait = VNIC_DVCMD_TMO;
+
+	return svnic_dev_cmd(vdev, CMD_DISABLE, &a0, &a1, wait);
+}
+
+int svnic_dev_open(struct vnic_dev *vdev, int arg)
+{
+	u64 a0 = (u32)arg, a1 = 0;
+	int wait = VNIC_DVCMD_TMO;
+
+	return svnic_dev_cmd(vdev, CMD_OPEN, &a0, &a1, wait);
+}
+
+int svnic_dev_open_done(struct vnic_dev *vdev, int *done)
+{
+	u64 a0 = 0, a1 = 0;
+	int wait = VNIC_DVCMD_TMO;
+	int err;
+
+	*done = 0;
+
+	err = svnic_dev_cmd(vdev, CMD_OPEN_STATUS, &a0, &a1, wait);
+	if (err)
+		return err;
+
+	*done = (a0 == 0);
+
+	return 0;
+}
+
+int svnic_dev_notify_set(struct vnic_dev *vdev, u16 intr)
+{
+	u64 a0, a1;
+	int wait = VNIC_DVCMD_TMO;
+
+	if (!vdev->notify) {
+		vdev->notify = pci_alloc_consistent(vdev->pdev,
+			sizeof(struct vnic_devcmd_notify),
+			&vdev->notify_pa);
+		if (!vdev->notify)
+			return -ENOMEM;
+	}
+
+	a0 = vdev->notify_pa;
+	a1 = ((u64)intr << 32) & VNIC_NOTIFY_INTR_MASK;
+	a1 += sizeof(struct vnic_devcmd_notify);
+
+	return svnic_dev_cmd(vdev, CMD_NOTIFY, &a0, &a1, wait);
+}
+
+void svnic_dev_notify_unset(struct vnic_dev *vdev)
+{
+	u64 a0, a1;
+	int wait = VNIC_DVCMD_TMO;
+
+	a0 = 0;  /* paddr = 0 to unset notify buffer */
+	a1 = VNIC_NOTIFY_INTR_MASK; /* intr num = -1 to unreg for intr */
+	a1 += sizeof(struct vnic_devcmd_notify);
+
+	svnic_dev_cmd(vdev, CMD_NOTIFY, &a0, &a1, wait);
+}
+
+static int vnic_dev_notify_ready(struct vnic_dev *vdev)
+{
+	u32 *words;
+	unsigned int nwords = sizeof(struct vnic_devcmd_notify) / 4;
+	unsigned int i;
+	u32 csum;
+
+	if (!vdev->notify)
+		return 0;
+
+	do {
+		csum = 0;
+		memcpy(&vdev->notify_copy, vdev->notify,
+			sizeof(struct vnic_devcmd_notify));
+		words = (u32 *)&vdev->notify_copy;
+		for (i = 1; i < nwords; i++)
+			csum += words[i];
+	} while (csum != words[0]);
+
+	return 1;
+}
+
+int svnic_dev_init(struct vnic_dev *vdev, int arg)
+{
+	u64 a0 = (u32)arg, a1 = 0;
+	int wait = VNIC_DVCMD_TMO;
+
+	return svnic_dev_cmd(vdev, CMD_INIT, &a0, &a1, wait);
+}
+
+int svnic_dev_link_status(struct vnic_dev *vdev)
+{
+	if (vdev->linkstatus)
+		return *vdev->linkstatus;
+
+	if (!vnic_dev_notify_ready(vdev))
+		return 0;
+
+	return vdev->notify_copy.link_state;
+}
+
+u32 svnic_dev_link_down_cnt(struct vnic_dev *vdev)
+{
+	if (!vnic_dev_notify_ready(vdev))
+		return 0;
+
+	return vdev->notify_copy.link_down_cnt;
+}
+
+void svnic_dev_set_intr_mode(struct vnic_dev *vdev,
+	enum vnic_dev_intr_mode intr_mode)
+{
+	vdev->intr_mode = intr_mode;
+}
+
+enum vnic_dev_intr_mode svnic_dev_get_intr_mode(struct vnic_dev *vdev)
+{
+	return vdev->intr_mode;
+}
+
+void svnic_dev_unregister(struct vnic_dev *vdev)
+{
+	if (vdev) {
+		if (vdev->notify)
+			pci_free_consistent(vdev->pdev,
+				sizeof(struct vnic_devcmd_notify),
+				vdev->notify,
+				vdev->notify_pa);
+		if (vdev->linkstatus)
+			pci_free_consistent(vdev->pdev,
+				sizeof(u32),
+				vdev->linkstatus,
+				vdev->linkstatus_pa);
+		if (vdev->stats)
+			pci_free_consistent(vdev->pdev,
+				sizeof(struct vnic_stats),
+				vdev->stats, vdev->stats_pa);
+		if (vdev->fw_info)
+			pci_free_consistent(vdev->pdev,
+				sizeof(struct vnic_devcmd_fw_info),
+				vdev->fw_info, vdev->fw_info_pa);
+		if (vdev->devcmd2)
+			vnic_dev_deinit_devcmd2(vdev);
+		kfree(vdev);
+	}
+}
+
+struct vnic_dev *svnic_dev_alloc_discover(struct vnic_dev *vdev,
+					  void *priv,
+					  struct pci_dev *pdev,
+					  struct vnic_dev_bar *bar,
+					  unsigned int num_bars)
+{
+	if (!vdev) {
+		vdev = kzalloc(sizeof(struct vnic_dev), GFP_ATOMIC);
+		if (!vdev)
+			return NULL;
+	}
+
+	vdev->priv = priv;
+	vdev->pdev = pdev;
+
+	if (vnic_dev_discover_res(vdev, bar, num_bars))
+		goto err_out;
+
+	return vdev;
+
+err_out:
+	svnic_dev_unregister(vdev);
+
+	return NULL;
+} /* end of svnic_dev_alloc_discover */
+
+/*
+ * fallback option is left to keep the interface common for other vnics.
+ */
+int svnic_dev_cmd_init(struct vnic_dev *vdev, int fallback)
+{
+	int err = -ENODEV;
+	void __iomem *p;
+
+	p = svnic_dev_get_res(vdev, RES_TYPE_DEVCMD2, 0);
+	if (p)
+		err = svnic_dev_init_devcmd2(vdev);
+	else
+		pr_err("DEVCMD2 resource not found.\n");
+
+	return err;
+} /* end of svnic_dev_cmd_init */
diff --git a/drivers/scsi/snic/vnic_dev.h b/drivers/scsi/snic/vnic_dev.h
new file mode 100644
index 000000000000..e65726da6504
--- /dev/null
+++ b/drivers/scsi/snic/vnic_dev.h
@@ -0,0 +1,110 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_DEV_H_
+#define _VNIC_DEV_H_
+
+#include "vnic_resource.h"
+#include "vnic_devcmd.h"
+
+#ifndef VNIC_PADDR_TARGET
+#define VNIC_PADDR_TARGET	0x0000000000000000ULL
+#endif
+
+#ifndef readq
+static inline u64 readq(void __iomem *reg)
+{
+	return ((u64)readl(reg + 0x4UL) << 32) | (u64)readl(reg);
+}
+
+static inline void writeq(u64 val, void __iomem *reg)
+{
+	writel(lower_32_bits(val), reg);
+	writel(upper_32_bits(val), reg + 0x4UL);
+}
+#endif
+
+enum vnic_dev_intr_mode {
+	VNIC_DEV_INTR_MODE_UNKNOWN,
+	VNIC_DEV_INTR_MODE_INTX,
+	VNIC_DEV_INTR_MODE_MSI,
+	VNIC_DEV_INTR_MODE_MSIX,
+};
+
+struct vnic_dev_bar {
+	void __iomem *vaddr;
+	dma_addr_t bus_addr;
+	unsigned long len;
+};
+
+struct vnic_dev_ring {
+	void *descs;
+	size_t size;
+	dma_addr_t base_addr;
+	size_t base_align;
+	void *descs_unaligned;
+	size_t size_unaligned;
+	dma_addr_t base_addr_unaligned;
+	unsigned int desc_size;
+	unsigned int desc_count;
+	unsigned int desc_avail;
+};
+
+struct vnic_dev;
+struct vnic_stats;
+
+void *svnic_dev_priv(struct vnic_dev *vdev);
+unsigned int svnic_dev_get_res_count(struct vnic_dev *vdev,
+				    enum vnic_res_type type);
+void __iomem *svnic_dev_get_res(struct vnic_dev *vdev, enum vnic_res_type type,
+			       unsigned int index);
+unsigned int svnic_dev_desc_ring_size(struct vnic_dev_ring *ring,
+				     unsigned int desc_count,
+				     unsigned int desc_size);
+void svnic_dev_clear_desc_ring(struct vnic_dev_ring *ring);
+int svnic_dev_alloc_desc_ring(struct vnic_dev *vdev, struct vnic_dev_ring *ring,
+			     unsigned int desc_count, unsigned int desc_size);
+void svnic_dev_free_desc_ring(struct vnic_dev *vdev,
+			     struct vnic_dev_ring *ring);
+int svnic_dev_cmd(struct vnic_dev *vdev, enum vnic_devcmd_cmd cmd,
+		 u64 *a0, u64 *a1, int wait);
+int svnic_dev_fw_info(struct vnic_dev *vdev,
+		     struct vnic_devcmd_fw_info **fw_info);
+int svnic_dev_spec(struct vnic_dev *vdev, unsigned int offset,
+		  unsigned int size, void *value);
+int svnic_dev_stats_clear(struct vnic_dev *vdev);
+int svnic_dev_stats_dump(struct vnic_dev *vdev, struct vnic_stats **stats);
+int svnic_dev_notify_set(struct vnic_dev *vdev, u16 intr);
+void svnic_dev_notify_unset(struct vnic_dev *vdev);
+int svnic_dev_link_status(struct vnic_dev *vdev);
+u32 svnic_dev_link_down_cnt(struct vnic_dev *vdev);
+int svnic_dev_close(struct vnic_dev *vdev);
+int svnic_dev_enable_wait(struct vnic_dev *vdev);
+int svnic_dev_disable(struct vnic_dev *vdev);
+int svnic_dev_open(struct vnic_dev *vdev, int arg);
+int svnic_dev_open_done(struct vnic_dev *vdev, int *done);
+int svnic_dev_init(struct vnic_dev *vdev, int arg);
+struct vnic_dev *svnic_dev_alloc_discover(struct vnic_dev *vdev,
+					 void *priv, struct pci_dev *pdev,
+					 struct vnic_dev_bar *bar,
+					 unsigned int num_bars);
+void svnic_dev_set_intr_mode(struct vnic_dev *vdev,
+			    enum vnic_dev_intr_mode intr_mode);
+enum vnic_dev_intr_mode svnic_dev_get_intr_mode(struct vnic_dev *vdev);
+void svnic_dev_unregister(struct vnic_dev *vdev);
+int svnic_dev_cmd_init(struct vnic_dev *vdev, int fallback);
+#endif /* _VNIC_DEV_H_ */
diff --git a/drivers/scsi/snic/vnic_devcmd.h b/drivers/scsi/snic/vnic_devcmd.h
new file mode 100644
index 000000000000..d81b4f0ceaaa
--- /dev/null
+++ b/drivers/scsi/snic/vnic_devcmd.h
@@ -0,0 +1,270 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_DEVCMD_H_
+#define _VNIC_DEVCMD_H_
+
+#define _CMD_NBITS      14
+#define _CMD_VTYPEBITS	10
+#define _CMD_FLAGSBITS  6
+#define _CMD_DIRBITS	2
+
+#define _CMD_NMASK      ((1 << _CMD_NBITS)-1)
+#define _CMD_VTYPEMASK  ((1 << _CMD_VTYPEBITS)-1)
+#define _CMD_FLAGSMASK  ((1 << _CMD_FLAGSBITS)-1)
+#define _CMD_DIRMASK    ((1 << _CMD_DIRBITS)-1)
+
+#define _CMD_NSHIFT     0
+#define _CMD_VTYPESHIFT (_CMD_NSHIFT+_CMD_NBITS)
+#define _CMD_FLAGSSHIFT (_CMD_VTYPESHIFT+_CMD_VTYPEBITS)
+#define _CMD_DIRSHIFT   (_CMD_FLAGSSHIFT+_CMD_FLAGSBITS)
+
+/*
+ * Direction bits (from host perspective).
+ */
+#define _CMD_DIR_NONE   0U
+#define _CMD_DIR_WRITE  1U
+#define _CMD_DIR_READ   2U
+#define _CMD_DIR_RW     (_CMD_DIR_WRITE | _CMD_DIR_READ)
+
+/*
+ * Flag bits.
+ */
+#define _CMD_FLAGS_NONE 0U
+#define _CMD_FLAGS_NOWAIT 1U
+
+/*
+ * vNIC type bits.
+ */
+#define _CMD_VTYPE_NONE  0U
+#define _CMD_VTYPE_ENET  1U
+#define _CMD_VTYPE_FC    2U
+#define _CMD_VTYPE_SCSI  4U
+#define _CMD_VTYPE_ALL   (_CMD_VTYPE_ENET | _CMD_VTYPE_FC | _CMD_VTYPE_SCSI)
+
+/*
+ * Used to create cmds..
+*/
+#define _CMDCF(dir, flags, vtype, nr)  \
+	(((dir)   << _CMD_DIRSHIFT) | \
+	((flags) << _CMD_FLAGSSHIFT) | \
+	((vtype) << _CMD_VTYPESHIFT) | \
+	((nr)    << _CMD_NSHIFT))
+#define _CMDC(dir, vtype, nr)    _CMDCF(dir, 0, vtype, nr)
+#define _CMDCNW(dir, vtype, nr)  _CMDCF(dir, _CMD_FLAGS_NOWAIT, vtype, nr)
+
+/*
+ * Used to decode cmds..
+*/
+#define _CMD_DIR(cmd)            (((cmd) >> _CMD_DIRSHIFT) & _CMD_DIRMASK)
+#define _CMD_FLAGS(cmd)          (((cmd) >> _CMD_FLAGSSHIFT) & _CMD_FLAGSMASK)
+#define _CMD_VTYPE(cmd)          (((cmd) >> _CMD_VTYPESHIFT) & _CMD_VTYPEMASK)
+#define _CMD_N(cmd)              (((cmd) >> _CMD_NSHIFT) & _CMD_NMASK)
+
+enum vnic_devcmd_cmd {
+	CMD_NONE                = _CMDC(_CMD_DIR_NONE, _CMD_VTYPE_NONE, 0),
+
+	/* mcpu fw info in mem: (u64)a0=paddr to struct vnic_devcmd_fw_info */
+	CMD_MCPU_FW_INFO        = _CMDC(_CMD_DIR_WRITE, _CMD_VTYPE_ALL, 1),
+
+	/* dev-specific block member:
+	 *    in: (u16)a0=offset,(u8)a1=size
+	 *    out: a0=value */
+	CMD_DEV_SPEC            = _CMDC(_CMD_DIR_RW, _CMD_VTYPE_ALL, 2),
+
+	/* stats clear */
+	CMD_STATS_CLEAR         = _CMDCNW(_CMD_DIR_NONE, _CMD_VTYPE_ALL, 3),
+
+	/* stats dump in mem: (u64)a0=paddr to stats area,
+	 *                    (u16)a1=sizeof stats area */
+	CMD_STATS_DUMP          = _CMDC(_CMD_DIR_WRITE, _CMD_VTYPE_ALL, 4),
+
+	/* nic_cfg in (u32)a0 */
+	CMD_NIC_CFG             = _CMDCNW(_CMD_DIR_WRITE, _CMD_VTYPE_ALL, 16),
+
+	/* set struct vnic_devcmd_notify buffer in mem:
+	 * in:
+	 *   (u64)a0=paddr to notify (set paddr=0 to unset)
+	 *   (u32)a1 & 0x00000000ffffffff=sizeof(struct vnic_devcmd_notify)
+	 *   (u16)a1 & 0x0000ffff00000000=intr num (-1 for no intr)
+	 * out:
+	 *   (u32)a1 = effective size
+	 */
+	CMD_NOTIFY              = _CMDC(_CMD_DIR_RW, _CMD_VTYPE_ALL, 21),
+
+	/* initiate open sequence (u32)a0=flags (see CMD_OPENF_*) */
+	CMD_OPEN		= _CMDCNW(_CMD_DIR_WRITE, _CMD_VTYPE_ALL, 23),
+
+	/* open status:
+	 *    out: a0=0 open complete, a0=1 open in progress */
+	CMD_OPEN_STATUS		= _CMDC(_CMD_DIR_READ, _CMD_VTYPE_ALL, 24),
+
+	/* close vnic */
+	CMD_CLOSE		= _CMDC(_CMD_DIR_NONE, _CMD_VTYPE_ALL, 25),
+
+	/* initialize virtual link: (u32)a0=flags (see CMD_INITF_*) */
+	CMD_INIT		= _CMDCNW(_CMD_DIR_READ, _CMD_VTYPE_ALL, 26),
+
+	/* enable virtual link */
+	CMD_ENABLE		= _CMDCNW(_CMD_DIR_WRITE, _CMD_VTYPE_ALL, 28),
+
+	/* enable virtual link, waiting variant. */
+	CMD_ENABLE_WAIT		= _CMDC(_CMD_DIR_WRITE, _CMD_VTYPE_ALL, 28),
+
+	/* disable virtual link */
+	CMD_DISABLE		= _CMDC(_CMD_DIR_NONE, _CMD_VTYPE_ALL, 29),
+
+	/* stats dump all vnics on uplink in mem: (u64)a0=paddr (u32)a1=uif */
+	CMD_STATS_DUMP_ALL	= _CMDC(_CMD_DIR_WRITE, _CMD_VTYPE_ALL, 30),
+
+	/* init status:
+	 *    out: a0=0 init complete, a0=1 init in progress
+	 *         if a0=0, a1=errno */
+	CMD_INIT_STATUS		= _CMDC(_CMD_DIR_READ, _CMD_VTYPE_ALL, 31),
+
+	/* undo initialize of virtual link */
+	CMD_DEINIT		= _CMDCNW(_CMD_DIR_NONE, _CMD_VTYPE_ALL, 34),
+
+	/* check fw capability of a cmd:
+	 * in:  (u32)a0=cmd
+	 * out: (u32)a0=errno, 0:valid cmd, a1=supported VNIC_STF_* bits */
+	CMD_CAPABILITY      = _CMDC(_CMD_DIR_RW, _CMD_VTYPE_ALL, 36),
+
+	/*
+	 * Initialization for the devcmd2 interface.
+	 * in: (u64) a0=host result buffer physical address
+	 * in: (u16) a1=number of entries in result buffer
+	 */
+	CMD_INITIALIZE_DEVCMD2 = _CMDC(_CMD_DIR_WRITE, _CMD_VTYPE_ALL, 57)
+};
+
+/* flags for CMD_OPEN */
+#define CMD_OPENF_OPROM		0x1	/* open coming from option rom */
+
+/* flags for CMD_INIT */
+#define CMD_INITF_DEFAULT_MAC	0x1	/* init with default mac addr */
+
+/* flags for CMD_PACKET_FILTER */
+#define CMD_PFILTER_DIRECTED		0x01
+#define CMD_PFILTER_MULTICAST		0x02
+#define CMD_PFILTER_BROADCAST		0x04
+#define CMD_PFILTER_PROMISCUOUS		0x08
+#define CMD_PFILTER_ALL_MULTICAST	0x10
+
+enum vnic_devcmd_status {
+	STAT_NONE = 0,
+	STAT_BUSY = 1 << 0,	/* cmd in progress */
+	STAT_ERROR = 1 << 1,	/* last cmd caused error (code in a0) */
+};
+
+enum vnic_devcmd_error {
+	ERR_SUCCESS = 0,
+	ERR_EINVAL = 1,
+	ERR_EFAULT = 2,
+	ERR_EPERM = 3,
+	ERR_EBUSY = 4,
+	ERR_ECMDUNKNOWN = 5,
+	ERR_EBADSTATE = 6,
+	ERR_ENOMEM = 7,
+	ERR_ETIMEDOUT = 8,
+	ERR_ELINKDOWN = 9,
+};
+
+struct vnic_devcmd_fw_info {
+	char fw_version[32];
+	char fw_build[32];
+	char hw_version[32];
+	char hw_serial_number[32];
+};
+
+struct vnic_devcmd_notify {
+	u32 csum;		/* checksum over following words */
+
+	u32 link_state;		/* link up == 1 */
+	u32 port_speed;		/* effective port speed (rate limit) */
+	u32 mtu;		/* MTU */
+	u32 msglvl;		/* requested driver msg lvl */
+	u32 uif;		/* uplink interface */
+	u32 status;		/* status bits (see VNIC_STF_*) */
+	u32 error;		/* error code (see ERR_*) for first ERR */
+	u32 link_down_cnt;	/* running count of link down transitions */
+};
+#define VNIC_STF_FATAL_ERR	0x0001	/* fatal fw error */
+
+struct vnic_devcmd_provinfo {
+	u8 oui[3];
+	u8 type;
+	u8 data[0];
+};
+
+/*
+ * Writing cmd register causes STAT_BUSY to get set in status register.
+ * When cmd completes, STAT_BUSY will be cleared.
+ *
+ * If cmd completed successfully STAT_ERROR will be clear
+ * and args registers contain cmd-specific results.
+ *
+ * If cmd error, STAT_ERROR will be set and args[0] contains error code.
+ *
+ * status register is read-only.  While STAT_BUSY is set,
+ * all other register contents are read-only.
+ */
+
+/* Make sizeof(vnic_devcmd) a power-of-2 for I/O BAR. */
+#define VNIC_DEVCMD_NARGS 15
+struct vnic_devcmd {
+	u32 status;			/* RO */
+	u32 cmd;			/* RW */
+	u64 args[VNIC_DEVCMD_NARGS];	/* RW cmd args (little-endian) */
+};
+
+
+/*
+ * Version 2 of the interface.
+ *
+ * Some things are carried over, notably the vnic_devcmd_cmd enum.
+ */
+
+/*
+ * Flags for vnic_devcmd2.flags
+ */
+
+#define DEVCMD2_FNORESULT       0x1     /* Don't copy result to host */
+
+#define VNIC_DEVCMD2_NARGS      VNIC_DEVCMD_NARGS
+struct vnic_devcmd2 {
+	u16 pad;
+	u16 flags;
+	u32 cmd;                /* same command #defines as original */
+	u64 args[VNIC_DEVCMD2_NARGS];
+};
+
+#define VNIC_DEVCMD2_NRESULTS   VNIC_DEVCMD_NARGS
+struct devcmd2_result {
+	u64 results[VNIC_DEVCMD2_NRESULTS];
+	u32 pad;
+	u16 completed_index;    /* into copy WQ */
+	u8  error;              /* same error codes as original */
+	u8  color;              /* 0 or 1 as with completion queues */
+};
+
+#define DEVCMD2_RING_SIZE   32
+#define DEVCMD2_DESC_SIZE   128
+
+#define DEVCMD2_RESULTS_SIZE_MAX   ((1 << 16) - 1)
+
+#endif /* _VNIC_DEVCMD_H_ */
diff --git a/drivers/scsi/snic/vnic_intr.c b/drivers/scsi/snic/vnic_intr.c
new file mode 100644
index 000000000000..a7d54806787d
--- /dev/null
+++ b/drivers/scsi/snic/vnic_intr.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/errno.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include "vnic_dev.h"
+#include "vnic_intr.h"
+
+void svnic_intr_free(struct vnic_intr *intr)
+{
+	intr->ctrl = NULL;
+}
+
+int svnic_intr_alloc(struct vnic_dev *vdev, struct vnic_intr *intr,
+	unsigned int index)
+{
+	intr->index = index;
+	intr->vdev = vdev;
+
+	intr->ctrl = svnic_dev_get_res(vdev, RES_TYPE_INTR_CTRL, index);
+	if (!intr->ctrl) {
+		pr_err("Failed to hook INTR[%d].ctrl resource\n",
+			index);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void svnic_intr_init(struct vnic_intr *intr, unsigned int coalescing_timer,
+	unsigned int coalescing_type, unsigned int mask_on_assertion)
+{
+	iowrite32(coalescing_timer, &intr->ctrl->coalescing_timer);
+	iowrite32(coalescing_type, &intr->ctrl->coalescing_type);
+	iowrite32(mask_on_assertion, &intr->ctrl->mask_on_assertion);
+	iowrite32(0, &intr->ctrl->int_credits);
+}
+
+void svnic_intr_clean(struct vnic_intr *intr)
+{
+	iowrite32(0, &intr->ctrl->int_credits);
+}
diff --git a/drivers/scsi/snic/vnic_intr.h b/drivers/scsi/snic/vnic_intr.h
new file mode 100644
index 000000000000..4547f603fe5e
--- /dev/null
+++ b/drivers/scsi/snic/vnic_intr.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_INTR_H_
+#define _VNIC_INTR_H_
+
+#include <linux/pci.h>
+#include "vnic_dev.h"
+
+#define VNIC_INTR_TIMER_MAX		0xffff
+
+#define VNIC_INTR_TIMER_TYPE_ABS	0
+#define VNIC_INTR_TIMER_TYPE_QUIET	1
+
+/* Interrupt control */
+struct vnic_intr_ctrl {
+	u32 coalescing_timer;		/* 0x00 */
+	u32 pad0;
+	u32 coalescing_value;		/* 0x08 */
+	u32 pad1;
+	u32 coalescing_type;		/* 0x10 */
+	u32 pad2;
+	u32 mask_on_assertion;		/* 0x18 */
+	u32 pad3;
+	u32 mask;			/* 0x20 */
+	u32 pad4;
+	u32 int_credits;		/* 0x28 */
+	u32 pad5;
+	u32 int_credit_return;		/* 0x30 */
+	u32 pad6;
+};
+
+struct vnic_intr {
+	unsigned int index;
+	struct vnic_dev *vdev;
+	struct vnic_intr_ctrl __iomem *ctrl;	/* memory-mapped */
+};
+
+static inline void
+svnic_intr_unmask(struct vnic_intr *intr)
+{
+	iowrite32(0, &intr->ctrl->mask);
+}
+
+static inline void
+svnic_intr_mask(struct vnic_intr *intr)
+{
+	iowrite32(1, &intr->ctrl->mask);
+}
+
+static inline void
+svnic_intr_return_credits(struct vnic_intr *intr,
+			  unsigned int credits,
+			  int unmask,
+			  int reset_timer)
+{
+#define VNIC_INTR_UNMASK_SHIFT		16
+#define VNIC_INTR_RESET_TIMER_SHIFT	17
+
+	u32 int_credit_return = (credits & 0xffff) |
+		(unmask ? (1 << VNIC_INTR_UNMASK_SHIFT) : 0) |
+		(reset_timer ? (1 << VNIC_INTR_RESET_TIMER_SHIFT) : 0);
+
+	iowrite32(int_credit_return, &intr->ctrl->int_credit_return);
+}
+
+static inline unsigned int
+svnic_intr_credits(struct vnic_intr *intr)
+{
+	return ioread32(&intr->ctrl->int_credits);
+}
+
+static inline void
+svnic_intr_return_all_credits(struct vnic_intr *intr)
+{
+	unsigned int credits = svnic_intr_credits(intr);
+	int unmask = 1;
+	int reset_timer = 1;
+
+	svnic_intr_return_credits(intr, credits, unmask, reset_timer);
+}
+
+void svnic_intr_free(struct vnic_intr *);
+int svnic_intr_alloc(struct vnic_dev *, struct vnic_intr *, unsigned int);
+void svnic_intr_init(struct vnic_intr *intr,
+		     unsigned int coalescing_timer,
+		     unsigned int coalescing_type,
+		     unsigned int mask_on_assertion);
+void svnic_intr_clean(struct vnic_intr *);
+
+#endif /* _VNIC_INTR_H_ */
diff --git a/drivers/scsi/snic/vnic_resource.h b/drivers/scsi/snic/vnic_resource.h
new file mode 100644
index 000000000000..9713d6835db3
--- /dev/null
+++ b/drivers/scsi/snic/vnic_resource.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_RESOURCE_H_
+#define _VNIC_RESOURCE_H_
+
+#define VNIC_RES_MAGIC		0x766E6963L	/* 'vnic' */
+#define VNIC_RES_VERSION	0x00000000L
+
+/* vNIC resource types */
+enum vnic_res_type {
+	RES_TYPE_EOL,			/* End-of-list */
+	RES_TYPE_WQ,			/* Work queues */
+	RES_TYPE_RQ,			/* Receive queues */
+	RES_TYPE_CQ,			/* Completion queues */
+	RES_TYPE_RSVD1,
+	RES_TYPE_NIC_CFG,		/* Enet NIC config registers */
+	RES_TYPE_RSVD2,
+	RES_TYPE_RSVD3,
+	RES_TYPE_RSVD4,
+	RES_TYPE_RSVD5,
+	RES_TYPE_INTR_CTRL,		/* Interrupt ctrl table */
+	RES_TYPE_INTR_TABLE,		/* MSI/MSI-X Interrupt table */
+	RES_TYPE_INTR_PBA,		/* MSI/MSI-X PBA table */
+	RES_TYPE_INTR_PBA_LEGACY,	/* Legacy intr status */
+	RES_TYPE_RSVD6,
+	RES_TYPE_RSVD7,
+	RES_TYPE_DEVCMD,		/* Device command region */
+	RES_TYPE_PASS_THRU_PAGE,	/* Pass-thru page */
+	RES_TYPE_SUBVNIC,		/* subvnic resource type */
+	RES_TYPE_MQ_WQ,			/* MQ Work queues */
+	RES_TYPE_MQ_RQ,			/* MQ Receive queues */
+	RES_TYPE_MQ_CQ,			/* MQ Completion queues */
+	RES_TYPE_DEPRECATED1,		/* Old version of devcmd 2 */
+	RES_TYPE_DEPRECATED2,		/* Old version of devcmd 2 */
+	RES_TYPE_DEVCMD2,		/* Device control region */
+
+	RES_TYPE_MAX,			/* Count of resource types */
+};
+
+struct vnic_resource_header {
+	u32 magic;
+	u32 version;
+};
+
+struct vnic_resource {
+	u8 type;
+	u8 bar;
+	u8 pad[2];
+	u32 bar_offset;
+	u32 count;
+};
+
+#endif /* _VNIC_RESOURCE_H_ */
diff --git a/drivers/scsi/snic/vnic_snic.h b/drivers/scsi/snic/vnic_snic.h
new file mode 100644
index 000000000000..514d39f5cf00
--- /dev/null
+++ b/drivers/scsi/snic/vnic_snic.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_SNIC_H_
+#define _VNIC_SNIC_H_
+
+#define VNIC_SNIC_WQ_DESCS_MIN              64
+#define VNIC_SNIC_WQ_DESCS_MAX              1024
+
+#define VNIC_SNIC_MAXDATAFIELDSIZE_MIN      256
+#define VNIC_SNIC_MAXDATAFIELDSIZE_MAX      2112
+
+#define VNIC_SNIC_IO_THROTTLE_COUNT_MIN     1
+#define VNIC_SNIC_IO_THROTTLE_COUNT_MAX     1024
+
+#define VNIC_SNIC_PORT_DOWN_TIMEOUT_MIN     0
+#define VNIC_SNIC_PORT_DOWN_TIMEOUT_MAX     240000
+
+#define VNIC_SNIC_PORT_DOWN_IO_RETRIES_MIN  0
+#define VNIC_SNIC_PORT_DOWN_IO_RETRIES_MAX  255
+
+#define VNIC_SNIC_LUNS_PER_TARGET_MIN       1
+#define VNIC_SNIC_LUNS_PER_TARGET_MAX       1024
+
+/* Device-specific region: scsi configuration */
+struct vnic_snic_config {
+	u32 flags;
+	u32 wq_enet_desc_count;
+	u32 io_throttle_count;
+	u32 port_down_timeout;
+	u32 port_down_io_retries;
+	u32 luns_per_tgt;
+	u16 maxdatafieldsize;
+	u16 intr_timer;
+	u8 intr_timer_type;
+	u8 _resvd2;
+	u8 xpt_type;
+	u8 hid;
+};
+#endif /* _VNIC_SNIC_H_ */
diff --git a/drivers/scsi/snic/vnic_stats.h b/drivers/scsi/snic/vnic_stats.h
new file mode 100644
index 000000000000..370a37c97748
--- /dev/null
+++ b/drivers/scsi/snic/vnic_stats.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_STATS_H_
+#define _VNIC_STATS_H_
+
+/* Tx statistics */
+struct vnic_tx_stats {
+	u64 tx_frames_ok;
+	u64 tx_unicast_frames_ok;
+	u64 tx_multicast_frames_ok;
+	u64 tx_broadcast_frames_ok;
+	u64 tx_bytes_ok;
+	u64 tx_unicast_bytes_ok;
+	u64 tx_multicast_bytes_ok;
+	u64 tx_broadcast_bytes_ok;
+	u64 tx_drops;
+	u64 tx_errors;
+	u64 tx_tso;
+	u64 rsvd[16];
+};
+
+/* Rx statistics */
+struct vnic_rx_stats {
+	u64 rx_frames_ok;
+	u64 rx_frames_total;
+	u64 rx_unicast_frames_ok;
+	u64 rx_multicast_frames_ok;
+	u64 rx_broadcast_frames_ok;
+	u64 rx_bytes_ok;
+	u64 rx_unicast_bytes_ok;
+	u64 rx_multicast_bytes_ok;
+	u64 rx_broadcast_bytes_ok;
+	u64 rx_drop;
+	u64 rx_no_bufs;
+	u64 rx_errors;
+	u64 rx_rss;
+	u64 rx_crc_errors;
+	u64 rx_frames_64;
+	u64 rx_frames_127;
+	u64 rx_frames_255;
+	u64 rx_frames_511;
+	u64 rx_frames_1023;
+	u64 rx_frames_1518;
+	u64 rx_frames_to_max;
+	u64 rsvd[16];
+};
+
+struct vnic_stats {
+	struct vnic_tx_stats tx;
+	struct vnic_rx_stats rx;
+};
+
+#endif /* _VNIC_STATS_H_ */
diff --git a/drivers/scsi/snic/vnic_wq.c b/drivers/scsi/snic/vnic_wq.c
new file mode 100644
index 000000000000..1e91d432089e
--- /dev/null
+++ b/drivers/scsi/snic/vnic_wq.c
@@ -0,0 +1,237 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * 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/errno.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include "vnic_dev.h"
+#include "vnic_wq.h"
+
+static inline int vnic_wq_get_ctrl(struct vnic_dev *vdev, struct vnic_wq *wq,
+	unsigned int index, enum vnic_res_type res_type)
+{
+	wq->ctrl = svnic_dev_get_res(vdev, res_type, index);
+	if (!wq->ctrl)
+		return -EINVAL;
+
+	return 0;
+}
+
+static inline int vnic_wq_alloc_ring(struct vnic_dev *vdev, struct vnic_wq *wq,
+	unsigned int index, unsigned int desc_count, unsigned int desc_size)
+{
+	return svnic_dev_alloc_desc_ring(vdev, &wq->ring, desc_count,
+					 desc_size);
+}
+
+static int vnic_wq_alloc_bufs(struct vnic_wq *wq)
+{
+	struct vnic_wq_buf *buf;
+	unsigned int i, j, count = wq->ring.desc_count;
+	unsigned int blks = VNIC_WQ_BUF_BLKS_NEEDED(count);
+
+	for (i = 0; i < blks; i++) {
+		wq->bufs[i] = kzalloc(VNIC_WQ_BUF_BLK_SZ, GFP_ATOMIC);
+		if (!wq->bufs[i]) {
+			pr_err("Failed to alloc wq_bufs\n");
+
+			return -ENOMEM;
+		}
+	}
+
+	for (i = 0; i < blks; i++) {
+		buf = wq->bufs[i];
+		for (j = 0; j < VNIC_WQ_BUF_DFLT_BLK_ENTRIES; j++) {
+			buf->index = i * VNIC_WQ_BUF_DFLT_BLK_ENTRIES + j;
+			buf->desc = (u8 *)wq->ring.descs +
+				wq->ring.desc_size * buf->index;
+			if (buf->index + 1 == count) {
+				buf->next = wq->bufs[0];
+				break;
+			} else if (j + 1 == VNIC_WQ_BUF_DFLT_BLK_ENTRIES) {
+				buf->next = wq->bufs[i + 1];
+			} else {
+				buf->next = buf + 1;
+				buf++;
+			}
+		}
+	}
+
+	wq->to_use = wq->to_clean = wq->bufs[0];
+
+	return 0;
+}
+
+void svnic_wq_free(struct vnic_wq *wq)
+{
+	struct vnic_dev *vdev;
+	unsigned int i;
+
+	vdev = wq->vdev;
+
+	svnic_dev_free_desc_ring(vdev, &wq->ring);
+
+	for (i = 0; i < VNIC_WQ_BUF_BLKS_MAX; i++) {
+		kfree(wq->bufs[i]);
+		wq->bufs[i] = NULL;
+	}
+
+	wq->ctrl = NULL;
+
+}
+
+int vnic_wq_devcmd2_alloc(struct vnic_dev *vdev, struct vnic_wq *wq,
+	unsigned int desc_count, unsigned int desc_size)
+{
+	int err;
+
+	wq->index = 0;
+	wq->vdev = vdev;
+
+	err = vnic_wq_get_ctrl(vdev, wq, 0, RES_TYPE_DEVCMD2);
+	if (err) {
+		pr_err("Failed to get devcmd2 resource\n");
+
+		return err;
+	}
+
+	svnic_wq_disable(wq);
+
+	err = vnic_wq_alloc_ring(vdev, wq, 0, desc_count, desc_size);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+int svnic_wq_alloc(struct vnic_dev *vdev, struct vnic_wq *wq,
+	unsigned int index, unsigned int desc_count, unsigned int desc_size)
+{
+	int err;
+
+	wq->index = index;
+	wq->vdev = vdev;
+
+	err = vnic_wq_get_ctrl(vdev, wq, index, RES_TYPE_WQ);
+	if (err) {
+		pr_err("Failed to hook WQ[%d] resource\n", index);
+
+		return err;
+	}
+
+	svnic_wq_disable(wq);
+
+	err = vnic_wq_alloc_ring(vdev, wq, index, desc_count, desc_size);
+	if (err)
+		return err;
+
+	err = vnic_wq_alloc_bufs(wq);
+	if (err) {
+		svnic_wq_free(wq);
+
+		return err;
+	}
+
+	return 0;
+}
+
+void vnic_wq_init_start(struct vnic_wq *wq, unsigned int cq_index,
+	unsigned int fetch_index, unsigned int posted_index,
+	unsigned int error_interrupt_enable,
+	unsigned int error_interrupt_offset)
+{
+	u64 paddr;
+	unsigned int count = wq->ring.desc_count;
+
+	paddr = (u64)wq->ring.base_addr | VNIC_PADDR_TARGET;
+	writeq(paddr, &wq->ctrl->ring_base);
+	iowrite32(count, &wq->ctrl->ring_size);
+	iowrite32(fetch_index, &wq->ctrl->fetch_index);
+	iowrite32(posted_index, &wq->ctrl->posted_index);
+	iowrite32(cq_index, &wq->ctrl->cq_index);
+	iowrite32(error_interrupt_enable, &wq->ctrl->error_interrupt_enable);
+	iowrite32(error_interrupt_offset, &wq->ctrl->error_interrupt_offset);
+	iowrite32(0, &wq->ctrl->error_status);
+
+	wq->to_use = wq->to_clean =
+		&wq->bufs[fetch_index / VNIC_WQ_BUF_BLK_ENTRIES(count)]
+			[fetch_index % VNIC_WQ_BUF_BLK_ENTRIES(count)];
+}
+
+void svnic_wq_init(struct vnic_wq *wq, unsigned int cq_index,
+	unsigned int error_interrupt_enable,
+	unsigned int error_interrupt_offset)
+{
+	vnic_wq_init_start(wq, cq_index, 0, 0, error_interrupt_enable,
+			   error_interrupt_offset);
+}
+
+unsigned int svnic_wq_error_status(struct vnic_wq *wq)
+{
+	return ioread32(&wq->ctrl->error_status);
+}
+
+void svnic_wq_enable(struct vnic_wq *wq)
+{
+	iowrite32(1, &wq->ctrl->enable);
+}
+
+int svnic_wq_disable(struct vnic_wq *wq)
+{
+	unsigned int wait;
+
+	iowrite32(0, &wq->ctrl->enable);
+
+	/* Wait for HW to ACK disable request */
+	for (wait = 0; wait < 100; wait++) {
+		if (!(ioread32(&wq->ctrl->running)))
+			return 0;
+		udelay(1);
+	}
+
+	pr_err("Failed to disable WQ[%d]\n", wq->index);
+
+	return -ETIMEDOUT;
+}
+
+void svnic_wq_clean(struct vnic_wq *wq,
+	void (*buf_clean)(struct vnic_wq *wq, struct vnic_wq_buf *buf))
+{
+	struct vnic_wq_buf *buf;
+
+	BUG_ON(ioread32(&wq->ctrl->enable));
+
+	buf = wq->to_clean;
+
+	while (svnic_wq_desc_used(wq) > 0) {
+
+		(*buf_clean)(wq, buf);
+
+		buf = wq->to_clean = buf->next;
+		wq->ring.desc_avail++;
+	}
+
+	wq->to_use = wq->to_clean = wq->bufs[0];
+
+	iowrite32(0, &wq->ctrl->fetch_index);
+	iowrite32(0, &wq->ctrl->posted_index);
+	iowrite32(0, &wq->ctrl->error_status);
+
+	svnic_dev_clear_desc_ring(&wq->ring);
+}
diff --git a/drivers/scsi/snic/vnic_wq.h b/drivers/scsi/snic/vnic_wq.h
new file mode 100644
index 000000000000..7cc031c7ceba
--- /dev/null
+++ b/drivers/scsi/snic/vnic_wq.h
@@ -0,0 +1,170 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VNIC_WQ_H_
+#define _VNIC_WQ_H_
+
+#include <linux/pci.h>
+#include "vnic_dev.h"
+#include "vnic_cq.h"
+
+/* Work queue control */
+struct vnic_wq_ctrl {
+	u64 ring_base;			/* 0x00 */
+	u32 ring_size;			/* 0x08 */
+	u32 pad0;
+	u32 posted_index;		/* 0x10 */
+	u32 pad1;
+	u32 cq_index;			/* 0x18 */
+	u32 pad2;
+	u32 enable;			/* 0x20 */
+	u32 pad3;
+	u32 running;			/* 0x28 */
+	u32 pad4;
+	u32 fetch_index;		/* 0x30 */
+	u32 pad5;
+	u32 dca_value;			/* 0x38 */
+	u32 pad6;
+	u32 error_interrupt_enable;	/* 0x40 */
+	u32 pad7;
+	u32 error_interrupt_offset;	/* 0x48 */
+	u32 pad8;
+	u32 error_status;		/* 0x50 */
+	u32 pad9;
+};
+
+struct vnic_wq_buf {
+	struct vnic_wq_buf *next;
+	dma_addr_t dma_addr;
+	void *os_buf;
+	unsigned int len;
+	unsigned int index;
+	int sop;
+	void *desc;
+};
+
+/* Break the vnic_wq_buf allocations into blocks of 64 entries */
+#define VNIC_WQ_BUF_MIN_BLK_ENTRIES 32
+#define VNIC_WQ_BUF_DFLT_BLK_ENTRIES 64
+#define VNIC_WQ_BUF_BLK_ENTRIES(entries) \
+	((unsigned int)(entries < VNIC_WQ_BUF_DFLT_BLK_ENTRIES) ? \
+		VNIC_WQ_BUF_MIN_BLK_ENTRIES : VNIC_WQ_BUF_DFLT_BLK_ENTRIES)
+#define VNIC_WQ_BUF_BLK_SZ \
+	(VNIC_WQ_BUF_DFLT_BLK_ENTRIES * sizeof(struct vnic_wq_buf))
+#define VNIC_WQ_BUF_BLKS_NEEDED(entries) \
+	DIV_ROUND_UP(entries, VNIC_WQ_BUF_DFLT_BLK_ENTRIES)
+#define VNIC_WQ_BUF_BLKS_NEEDED(entries) \
+	DIV_ROUND_UP(entries, VNIC_WQ_BUF_DFLT_BLK_ENTRIES)
+#define VNIC_WQ_BUF_BLKS_MAX VNIC_WQ_BUF_BLKS_NEEDED(4096)
+
+struct vnic_wq {
+	unsigned int index;
+	struct vnic_dev *vdev;
+	struct vnic_wq_ctrl __iomem *ctrl;	/* memory-mapped */
+	struct vnic_dev_ring ring;
+	struct vnic_wq_buf *bufs[VNIC_WQ_BUF_BLKS_MAX];
+	struct vnic_wq_buf *to_use;
+	struct vnic_wq_buf *to_clean;
+	unsigned int pkts_outstanding;
+};
+
+static inline unsigned int svnic_wq_desc_avail(struct vnic_wq *wq)
+{
+	/* how many does SW own? */
+	return wq->ring.desc_avail;
+}
+
+static inline unsigned int svnic_wq_desc_used(struct vnic_wq *wq)
+{
+	/* how many does HW own? */
+	return wq->ring.desc_count - wq->ring.desc_avail - 1;
+}
+
+static inline void *svnic_wq_next_desc(struct vnic_wq *wq)
+{
+	return wq->to_use->desc;
+}
+
+static inline void svnic_wq_post(struct vnic_wq *wq,
+	void *os_buf, dma_addr_t dma_addr,
+	unsigned int len, int sop, int eop)
+{
+	struct vnic_wq_buf *buf = wq->to_use;
+
+	buf->sop = sop;
+	buf->os_buf = eop ? os_buf : NULL;
+	buf->dma_addr = dma_addr;
+	buf->len = len;
+
+	buf = buf->next;
+	if (eop) {
+		/* Adding write memory barrier prevents compiler and/or CPU
+		 * reordering, thus avoiding descriptor posting before
+		 * descriptor is initialized. Otherwise, hardware can read
+		 * stale descriptor fields.
+		 */
+		wmb();
+		iowrite32(buf->index, &wq->ctrl->posted_index);
+	}
+	wq->to_use = buf;
+
+	wq->ring.desc_avail--;
+}
+
+static inline void svnic_wq_service(struct vnic_wq *wq,
+	struct cq_desc *cq_desc, u16 completed_index,
+	void (*buf_service)(struct vnic_wq *wq,
+	struct cq_desc *cq_desc, struct vnic_wq_buf *buf, void *opaque),
+	void *opaque)
+{
+	struct vnic_wq_buf *buf;
+
+	buf = wq->to_clean;
+	while (1) {
+
+		(*buf_service)(wq, cq_desc, buf, opaque);
+
+		wq->ring.desc_avail++;
+
+		wq->to_clean = buf->next;
+
+		if (buf->index == completed_index)
+			break;
+
+		buf = wq->to_clean;
+	}
+}
+
+void svnic_wq_free(struct vnic_wq *wq);
+int svnic_wq_alloc(struct vnic_dev *vdev, struct vnic_wq *wq,
+	unsigned int index, unsigned int desc_count, unsigned int desc_size);
+int vnic_wq_devcmd2_alloc(struct vnic_dev *vdev, struct vnic_wq *wq,
+		unsigned int desc_count, unsigned int desc_size);
+void vnic_wq_init_start(struct vnic_wq *wq, unsigned int cq_index,
+		unsigned int fetch_index, unsigned int post_index,
+		unsigned int error_interrupt_enable,
+		unsigned int error_interrupt_offset);
+
+void svnic_wq_init(struct vnic_wq *wq, unsigned int cq_index,
+	unsigned int error_interrupt_enable,
+	unsigned int error_interrupt_offset);
+unsigned int svnic_wq_error_status(struct vnic_wq *wq);
+void svnic_wq_enable(struct vnic_wq *wq);
+int svnic_wq_disable(struct vnic_wq *wq);
+void svnic_wq_clean(struct vnic_wq *wq,
+	void (*buf_clean)(struct vnic_wq *wq, struct vnic_wq_buf *buf));
+#endif /* _VNIC_WQ_H_ */
diff --git a/drivers/scsi/snic/wq_enet_desc.h b/drivers/scsi/snic/wq_enet_desc.h
new file mode 100644
index 000000000000..68f62b6d105b
--- /dev/null
+++ b/drivers/scsi/snic/wq_enet_desc.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright 2014 Cisco Systems, Inc.  All rights reserved.
+ *
+ * This program is free software; you may 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.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _WQ_ENET_DESC_H_
+#define _WQ_ENET_DESC_H_
+
+/* Ethernet work queue descriptor: 16B */
+struct wq_enet_desc {
+	__le64 address;
+	__le16 length;
+	__le16 mss_loopback;
+	__le16 header_length_flags;
+	__le16 vlan_tag;
+};
+
+#define WQ_ENET_ADDR_BITS		64
+#define WQ_ENET_LEN_BITS		14
+#define WQ_ENET_LEN_MASK		((1 << WQ_ENET_LEN_BITS) - 1)
+#define WQ_ENET_MSS_BITS		14
+#define WQ_ENET_MSS_MASK		((1 << WQ_ENET_MSS_BITS) - 1)
+#define WQ_ENET_MSS_SHIFT		2
+#define WQ_ENET_LOOPBACK_SHIFT		1
+#define WQ_ENET_HDRLEN_BITS		10
+#define WQ_ENET_HDRLEN_MASK		((1 << WQ_ENET_HDRLEN_BITS) - 1)
+#define WQ_ENET_FLAGS_OM_BITS		2
+#define WQ_ENET_FLAGS_OM_MASK		((1 << WQ_ENET_FLAGS_OM_BITS) - 1)
+#define WQ_ENET_FLAGS_EOP_SHIFT		12
+#define WQ_ENET_FLAGS_CQ_ENTRY_SHIFT	13
+#define WQ_ENET_FLAGS_FCOE_ENCAP_SHIFT	14
+#define WQ_ENET_FLAGS_VLAN_TAG_INSERT_SHIFT	15
+
+#define WQ_ENET_OFFLOAD_MODE_CSUM	0
+#define WQ_ENET_OFFLOAD_MODE_RESERVED	1
+#define WQ_ENET_OFFLOAD_MODE_CSUM_L4	2
+#define WQ_ENET_OFFLOAD_MODE_TSO	3
+
+static inline void wq_enet_desc_enc(struct wq_enet_desc *desc,
+	u64 address, u16 length, u16 mss, u16 header_length,
+	u8 offload_mode, u8 eop, u8 cq_entry, u8 fcoe_encap,
+	u8 vlan_tag_insert, u16 vlan_tag, u8 loopback)
+{
+	desc->address = cpu_to_le64(address);
+	desc->length = cpu_to_le16(length & WQ_ENET_LEN_MASK);
+	desc->mss_loopback = cpu_to_le16((mss & WQ_ENET_MSS_MASK) <<
+		WQ_ENET_MSS_SHIFT | (loopback & 1) << WQ_ENET_LOOPBACK_SHIFT);
+	desc->header_length_flags = cpu_to_le16(
+		(header_length & WQ_ENET_HDRLEN_MASK) |
+		(offload_mode & WQ_ENET_FLAGS_OM_MASK) << WQ_ENET_HDRLEN_BITS |
+		(eop & 1) << WQ_ENET_FLAGS_EOP_SHIFT |
+		(cq_entry & 1) << WQ_ENET_FLAGS_CQ_ENTRY_SHIFT |
+		(fcoe_encap & 1) << WQ_ENET_FLAGS_FCOE_ENCAP_SHIFT |
+		(vlan_tag_insert & 1) << WQ_ENET_FLAGS_VLAN_TAG_INSERT_SHIFT);
+	desc->vlan_tag = cpu_to_le16(vlan_tag);
+}
+
+static inline void wq_enet_desc_dec(struct wq_enet_desc *desc,
+	u64 *address, u16 *length, u16 *mss, u16 *header_length,
+	u8 *offload_mode, u8 *eop, u8 *cq_entry, u8 *fcoe_encap,
+	u8 *vlan_tag_insert, u16 *vlan_tag, u8 *loopback)
+{
+	*address = le64_to_cpu(desc->address);
+	*length = le16_to_cpu(desc->length) & WQ_ENET_LEN_MASK;
+	*mss = (le16_to_cpu(desc->mss_loopback) >> WQ_ENET_MSS_SHIFT) &
+		WQ_ENET_MSS_MASK;
+	*loopback = (u8)((le16_to_cpu(desc->mss_loopback) >>
+		WQ_ENET_LOOPBACK_SHIFT) & 1);
+	*header_length = le16_to_cpu(desc->header_length_flags) &
+		WQ_ENET_HDRLEN_MASK;
+	*offload_mode = (u8)((le16_to_cpu(desc->header_length_flags) >>
+		WQ_ENET_HDRLEN_BITS) & WQ_ENET_FLAGS_OM_MASK);
+	*eop = (u8)((le16_to_cpu(desc->header_length_flags) >>
+		WQ_ENET_FLAGS_EOP_SHIFT) & 1);
+	*cq_entry = (u8)((le16_to_cpu(desc->header_length_flags) >>
+		WQ_ENET_FLAGS_CQ_ENTRY_SHIFT) & 1);
+	*fcoe_encap = (u8)((le16_to_cpu(desc->header_length_flags) >>
+		WQ_ENET_FLAGS_FCOE_ENCAP_SHIFT) & 1);
+	*vlan_tag_insert = (u8)((le16_to_cpu(desc->header_length_flags) >>
+		WQ_ENET_FLAGS_VLAN_TAG_INSERT_SHIFT) & 1);
+	*vlan_tag = le16_to_cpu(desc->vlan_tag);
+}
+
+#endif /* _WQ_ENET_DESC_H_ */
diff --git a/drivers/scsi/st.c b/drivers/scsi/st.c
index 9a1c34205254..3f25b8fa921d 100644
--- a/drivers/scsi/st.c
+++ b/drivers/scsi/st.c
@@ -471,6 +471,47 @@ static void st_release_request(struct st_request *streq)
 	kfree(streq);
 }
 
+static void st_do_stats(struct scsi_tape *STp, struct request *req)
+{
+	ktime_t now;
+
+	now = ktime_get();
+	if (req->cmd[0] == WRITE_6) {
+		now = ktime_sub(now, STp->stats->write_time);
+		atomic64_add(ktime_to_ns(now), &STp->stats->tot_write_time);
+		atomic64_add(ktime_to_ns(now), &STp->stats->tot_io_time);
+		atomic64_inc(&STp->stats->write_cnt);
+		if (req->errors) {
+			atomic64_add(atomic_read(&STp->stats->last_write_size)
+				- STp->buffer->cmdstat.residual,
+				&STp->stats->write_byte_cnt);
+			if (STp->buffer->cmdstat.residual > 0)
+				atomic64_inc(&STp->stats->resid_cnt);
+		} else
+			atomic64_add(atomic_read(&STp->stats->last_write_size),
+				&STp->stats->write_byte_cnt);
+	} else if (req->cmd[0] == READ_6) {
+		now = ktime_sub(now, STp->stats->read_time);
+		atomic64_add(ktime_to_ns(now), &STp->stats->tot_read_time);
+		atomic64_add(ktime_to_ns(now), &STp->stats->tot_io_time);
+		atomic64_inc(&STp->stats->read_cnt);
+		if (req->errors) {
+			atomic64_add(atomic_read(&STp->stats->last_read_size)
+				- STp->buffer->cmdstat.residual,
+				&STp->stats->read_byte_cnt);
+			if (STp->buffer->cmdstat.residual > 0)
+				atomic64_inc(&STp->stats->resid_cnt);
+		} else
+			atomic64_add(atomic_read(&STp->stats->last_read_size),
+				&STp->stats->read_byte_cnt);
+	} else {
+		now = ktime_sub(now, STp->stats->other_time);
+		atomic64_add(ktime_to_ns(now), &STp->stats->tot_io_time);
+		atomic64_inc(&STp->stats->other_cnt);
+	}
+	atomic64_dec(&STp->stats->in_flight);
+}
+
 static void st_scsi_execute_end(struct request *req, int uptodate)
 {
 	struct st_request *SRpnt = req->end_io_data;
@@ -480,6 +521,8 @@ static void st_scsi_execute_end(struct request *req, int uptodate)
 	STp->buffer->cmdstat.midlevel_result = SRpnt->result = req->errors;
 	STp->buffer->cmdstat.residual = req->resid_len;
 
+	st_do_stats(STp, req);
+
 	tmp = SRpnt->bio;
 	if (SRpnt->waiting)
 		complete(SRpnt->waiting);
@@ -496,6 +539,7 @@ static int st_scsi_execute(struct st_request *SRpnt, const unsigned char *cmd,
 	struct rq_map_data *mdata = &SRpnt->stp->buffer->map_data;
 	int err = 0;
 	int write = (data_direction == DMA_TO_DEVICE);
+	struct scsi_tape *STp = SRpnt->stp;
 
 	req = blk_get_request(SRpnt->stp->device->request_queue, write,
 			      GFP_KERNEL);
@@ -516,6 +560,17 @@ static int st_scsi_execute(struct st_request *SRpnt, const unsigned char *cmd,
 		}
 	}
 
+	atomic64_inc(&STp->stats->in_flight);
+	if (cmd[0] == WRITE_6) {
+		atomic_set(&STp->stats->last_write_size, bufflen);
+		STp->stats->write_time = ktime_get();
+	} else if (cmd[0] == READ_6) {
+		atomic_set(&STp->stats->last_read_size, bufflen);
+		STp->stats->read_time = ktime_get();
+	} else {
+		STp->stats->other_time = ktime_get();
+	}
+
 	SRpnt->bio = req->bio;
 	req->cmd_len = COMMAND_SIZE(cmd[0]);
 	memset(req->cmd, 0, BLK_MAX_CDB);
@@ -4222,6 +4277,12 @@ static int st_probe(struct device *dev)
 	}
 	tpnt->index = error;
 	sprintf(disk->disk_name, "st%d", tpnt->index);
+	tpnt->stats = kzalloc(sizeof(struct scsi_tape_stats), GFP_KERNEL);
+	if (tpnt->stats == NULL) {
+		sdev_printk(KERN_ERR, SDp,
+			    "st: Can't allocate statistics.\n");
+		goto out_idr_remove;
+	}
 
 	dev_set_drvdata(dev, tpnt);
 
@@ -4241,6 +4302,8 @@ static int st_probe(struct device *dev)
 
 out_remove_devs:
 	remove_cdevs(tpnt);
+	kfree(tpnt->stats);
+out_idr_remove:
 	spin_lock(&st_index_lock);
 	idr_remove(&st_index_idr, tpnt->index);
 	spin_unlock(&st_index_lock);
@@ -4298,6 +4361,7 @@ static void scsi_tape_release(struct kref *kref)
 
 	disk->private_data = NULL;
 	put_disk(disk);
+	kfree(tpnt->stats);
 	kfree(tpnt);
 	return;
 }
@@ -4513,6 +4577,184 @@ options_show(struct device *dev, struct device_attribute *attr, char *buf)
 }
 static DEVICE_ATTR_RO(options);
 
+/* Support for tape stats */
+
+/**
+ * read_cnt_show - return read count - count of reads made from tape drive
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t read_cnt_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->read_cnt));
+}
+static DEVICE_ATTR_RO(read_cnt);
+
+/**
+ * read_byte_cnt_show - return read byte count - tape drives
+ * may use blocks less than 512 bytes this gives the raw byte count of
+ * of data read from the tape drive.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t read_byte_cnt_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->read_byte_cnt));
+}
+static DEVICE_ATTR_RO(read_byte_cnt);
+
+/**
+ * read_us_show - return read us - overall time spent waiting on reads in ns.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t read_ns_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->tot_read_time));
+}
+static DEVICE_ATTR_RO(read_ns);
+
+/**
+ * write_cnt_show - write count - number of user calls
+ * to write(2) that have written data to tape.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t write_cnt_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->write_cnt));
+}
+static DEVICE_ATTR_RO(write_cnt);
+
+/**
+ * write_byte_cnt_show - write byte count - raw count of
+ * bytes written to tape.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t write_byte_cnt_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->write_byte_cnt));
+}
+static DEVICE_ATTR_RO(write_byte_cnt);
+
+/**
+ * write_ns_show - write ns - number of nanoseconds waiting on write
+ * requests to complete.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t write_ns_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->tot_write_time));
+}
+static DEVICE_ATTR_RO(write_ns);
+
+/**
+ * in_flight_show - number of I/Os currently in flight -
+ * in most cases this will be either 0 or 1. It may be higher if someone
+ * has also issued other SCSI commands such as via an ioctl.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t in_flight_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->in_flight));
+}
+static DEVICE_ATTR_RO(in_flight);
+
+/**
+ * io_ns_show - io wait ns - this is the number of ns spent
+ * waiting on all I/O to complete. This includes tape movement commands
+ * such as rewinding, seeking to end of file or tape, it also includes
+ * read and write. To determine the time spent on tape movement
+ * subtract the read and write ns from this value.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t io_ns_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->tot_io_time));
+}
+static DEVICE_ATTR_RO(io_ns);
+
+/**
+ * other_cnt_show - other io count - this is the number of
+ * I/O requests other than read and write requests.
+ * Typically these are tape movement requests but will include driver
+ * tape movement. This includes only requests issued by the st driver.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t other_cnt_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->other_cnt));
+}
+static DEVICE_ATTR_RO(other_cnt);
+
+/**
+ * resid_cnt_show - A count of the number of times we get a residual
+ * count - this should indicate someone issuing reads larger than the
+ * block size on tape.
+ * @dev: struct device
+ * @attr: attribute structure
+ * @buf: buffer to return formatted data in
+ */
+static ssize_t resid_cnt_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct st_modedef *STm = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%lld",
+		       (long long)atomic64_read(&STm->tape->stats->resid_cnt));
+}
+static DEVICE_ATTR_RO(resid_cnt);
+
 static struct attribute *st_dev_attrs[] = {
 	&dev_attr_defined.attr,
 	&dev_attr_default_blksize.attr,
@@ -4521,7 +4763,35 @@ static struct attribute *st_dev_attrs[] = {
 	&dev_attr_options.attr,
 	NULL,
 };
-ATTRIBUTE_GROUPS(st_dev);
+
+static struct attribute *st_stats_attrs[] = {
+	&dev_attr_read_cnt.attr,
+	&dev_attr_read_byte_cnt.attr,
+	&dev_attr_read_ns.attr,
+	&dev_attr_write_cnt.attr,
+	&dev_attr_write_byte_cnt.attr,
+	&dev_attr_write_ns.attr,
+	&dev_attr_in_flight.attr,
+	&dev_attr_io_ns.attr,
+	&dev_attr_other_cnt.attr,
+	&dev_attr_resid_cnt.attr,
+	NULL,
+};
+
+static struct attribute_group stats_group = {
+	.name = "stats",
+	.attrs = st_stats_attrs,
+};
+
+static struct attribute_group st_group = {
+	.attrs = st_dev_attrs,
+};
+
+static const struct attribute_group *st_dev_groups[] = {
+	&st_group,
+	&stats_group,
+	NULL,
+};
 
 /* The following functions may be useful for a larger audience. */
 static int sgl_map_user_pages(struct st_buffer *STbp,
diff --git a/drivers/scsi/st.h b/drivers/scsi/st.h
index f3eee0f9f40c..b6486b5d8681 100644
--- a/drivers/scsi/st.h
+++ b/drivers/scsi/st.h
@@ -92,6 +92,27 @@ struct st_partstat {
 	int drv_file;
 };
 
+/* Tape statistics */
+struct scsi_tape_stats {
+	atomic64_t read_byte_cnt;  /* bytes read */
+	atomic64_t write_byte_cnt; /* bytes written */
+	atomic64_t in_flight;      /* Number of I/Os in flight */
+	atomic64_t read_cnt;       /* Count of read requests */
+	atomic64_t write_cnt;      /* Count of write requests */
+	atomic64_t other_cnt;      /* Count of other requests either
+				    * implicit or from user space
+				    * ioctl. */
+	atomic64_t resid_cnt;      /* Count of resid_len > 0 */
+	atomic64_t tot_read_time;  /* ktime spent completing reads */
+	atomic64_t tot_write_time; /* ktime spent completing writes */
+	atomic64_t tot_io_time;    /* ktime spent doing any I/O */
+	ktime_t read_time;         /* holds ktime request was queued */
+	ktime_t write_time;        /* holds ktime request was queued */
+	ktime_t other_time;        /* holds ktime request was queued */
+	atomic_t last_read_size;   /* Number of bytes issued for last read */
+	atomic_t last_write_size;  /* Number of bytes issued for last write */
+};
+
 #define ST_NBR_PARTITIONS 4
 
 /* The tape drive descriptor */
@@ -171,6 +192,7 @@ struct scsi_tape {
 #endif
 	struct gendisk *disk;
 	struct kref     kref;
+	struct scsi_tape_stats *stats;
 };
 
 /* Bit masks for use_pf */
diff --git a/drivers/scsi/sym53c416.c b/drivers/scsi/sym53c416.c
index 0b7819f3e09b..5bdcbe8fa958 100644
--- a/drivers/scsi/sym53c416.c
+++ b/drivers/scsi/sym53c416.c
@@ -838,7 +838,6 @@ static struct scsi_host_template driver_template = {
 	.can_queue =		1,
 	.this_id =		SYM53C416_SCSI_ID,
 	.sg_tablesize =		32,
-	.cmd_per_lun =		1,
 	.unchecked_isa_dma =	1,
 	.use_clustering =	ENABLE_CLUSTERING,
 };
diff --git a/drivers/scsi/ufs/Kconfig b/drivers/scsi/ufs/Kconfig
index 8a1f4b355416..e94538362536 100644
--- a/drivers/scsi/ufs/Kconfig
+++ b/drivers/scsi/ufs/Kconfig
@@ -73,7 +73,7 @@ config SCSI_UFSHCD_PLATFORM
 
 config SCSI_UFS_QCOM
 	bool "QCOM specific hooks to UFS controller platform driver"
-	depends on SCSI_UFSHCD_PLATFORM && ARCH_MSM
+	depends on SCSI_UFSHCD_PLATFORM && ARCH_QCOM
 	select PHY_QCOM_UFS
 	help
 	  This selects the QCOM specific additions to UFSHCD platform driver.
diff --git a/drivers/scsi/ufs/ufs-qcom.c b/drivers/scsi/ufs/ufs-qcom.c
index 6652a8171de6..4cdffa46d401 100644
--- a/drivers/scsi/ufs/ufs-qcom.c
+++ b/drivers/scsi/ufs/ufs-qcom.c
@@ -307,6 +307,7 @@ static int ufs_qcom_hce_enable_notify(struct ufs_hba *hba, bool status)
 static unsigned long
 ufs_qcom_cfg_timers(struct ufs_hba *hba, u32 gear, u32 hs, u32 rate)
 {
+	struct ufs_qcom_host *host = hba->priv;
 	struct ufs_clk_info *clki;
 	u32 core_clk_period_in_ns;
 	u32 tx_clk_cycles_per_us = 0;
@@ -330,6 +331,16 @@ ufs_qcom_cfg_timers(struct ufs_hba *hba, u32 gear, u32 hs, u32 rate)
 		{UFS_HS_G2, 0x49},
 	};
 
+	/*
+	 * The Qunipro controller does not use following registers:
+	 * SYS1CLK_1US_REG, TX_SYMBOL_CLK_1US_REG, CLK_NS_REG &
+	 * UFS_REG_PA_LINK_STARTUP_TIMER
+	 * But UTP controller uses SYS1CLK_1US_REG register for Interrupt
+	 * Aggregation logic.
+	*/
+	if (ufs_qcom_cap_qunipro(host) && !ufshcd_is_intr_aggr_allowed(hba))
+		goto out;
+
 	if (gear == 0) {
 		dev_err(hba->dev, "%s: invalid gear = %d\n", __func__, gear);
 		goto out_error;
@@ -683,6 +694,16 @@ out:
 	return ret;
 }
 
+static u32 ufs_qcom_get_ufs_hci_version(struct ufs_hba *hba)
+{
+	struct ufs_qcom_host *host = hba->priv;
+
+	if (host->hw_ver.major == 0x1)
+		return UFSHCI_VERSION_11;
+	else
+		return UFSHCI_VERSION_20;
+}
+
 /**
  * ufs_qcom_advertise_quirks - advertise the known QCOM UFS controller quirks
  * @hba: host controller instance
@@ -696,13 +717,24 @@ static void ufs_qcom_advertise_quirks(struct ufs_hba *hba)
 {
 	struct ufs_qcom_host *host = hba->priv;
 
-	if (host->hw_ver.major == 0x1)
-		hba->quirks |= UFSHCD_QUIRK_DELAY_BEFORE_DME_CMDS;
+	if (host->hw_ver.major == 0x01) {
+		hba->quirks |= UFSHCD_QUIRK_DELAY_BEFORE_DME_CMDS
+			    | UFSHCD_QUIRK_BROKEN_PA_RXHSUNTERMCAP
+			    | UFSHCD_QUIRK_DME_PEER_ACCESS_AUTO_MODE;
+
+		if (host->hw_ver.minor == 0x0001 && host->hw_ver.step == 0x0001)
+			hba->quirks |= UFSHCD_QUIRK_BROKEN_INTR_AGGR;
+	}
 
 	if (host->hw_ver.major >= 0x2) {
+		hba->quirks |= UFSHCD_QUIRK_BROKEN_LCC;
+		hba->quirks |= UFSHCD_QUIRK_BROKEN_UFS_HCI_VERSION;
+
 		if (!ufs_qcom_cap_qunipro(host))
 			/* Legacy UniPro mode still need following quirks */
-			hba->quirks |= UFSHCD_QUIRK_DELAY_BEFORE_DME_CMDS;
+			hba->quirks |= (UFSHCD_QUIRK_DELAY_BEFORE_DME_CMDS
+				| UFSHCD_QUIRK_DME_PEER_ACCESS_AUTO_MODE
+				| UFSHCD_QUIRK_BROKEN_PA_RXHSUNTERMCAP);
 	}
 }
 
@@ -1005,6 +1037,7 @@ static const struct ufs_hba_variant_ops ufs_hba_qcom_vops = {
 	.name                   = "qcom",
 	.init                   = ufs_qcom_init,
 	.exit                   = ufs_qcom_exit,
+	.get_ufs_hci_version	= ufs_qcom_get_ufs_hci_version,
 	.clk_scale_notify	= ufs_qcom_clk_scale_notify,
 	.setup_clocks           = ufs_qcom_setup_clocks,
 	.hce_enable_notify      = ufs_qcom_hce_enable_notify,
diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index 648a44675880..b0ade73f8c6a 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -188,6 +188,8 @@ static int ufshcd_host_reset_and_restore(struct ufs_hba *hba);
 static irqreturn_t ufshcd_intr(int irq, void *__hba);
 static int ufshcd_config_pwr_mode(struct ufs_hba *hba,
 		struct ufs_pa_layer_attr *desired_pwr_mode);
+static int ufshcd_change_power_mode(struct ufs_hba *hba,
+			     struct ufs_pa_layer_attr *pwr_mode);
 
 static inline int ufshcd_enable_irq(struct ufs_hba *hba)
 {
@@ -269,6 +271,11 @@ static inline u32 ufshcd_get_intr_mask(struct ufs_hba *hba)
  */
 static inline u32 ufshcd_get_ufs_version(struct ufs_hba *hba)
 {
+	if (hba->quirks & UFSHCD_QUIRK_BROKEN_UFS_HCI_VERSION) {
+		if (hba->vops && hba->vops->get_ufs_hci_version)
+			return hba->vops->get_ufs_hci_version(hba);
+	}
+
 	return ufshcd_readl(hba, REG_UFS_VERSION);
 }
 
@@ -481,6 +488,15 @@ ufshcd_config_intr_aggr(struct ufs_hba *hba, u8 cnt, u8 tmout)
 }
 
 /**
+ * ufshcd_disable_intr_aggr - Disables interrupt aggregation.
+ * @hba: per adapter instance
+ */
+static inline void ufshcd_disable_intr_aggr(struct ufs_hba *hba)
+{
+	ufshcd_writel(hba, 0, REG_UTP_TRANSFER_REQ_INT_AGG_CONTROL);
+}
+
+/**
  * ufshcd_enable_run_stop_reg - Enable run-stop registers,
  *			When run-stop registers are set to 1, it indicates the
  *			host controller that it can process the requests
@@ -1326,7 +1342,7 @@ static int ufshcd_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 	lrbp->sense_buffer = cmd->sense_buffer;
 	lrbp->task_tag = tag;
 	lrbp->lun = ufshcd_scsi_to_upiu_lun(cmd->device->lun);
-	lrbp->intr_cmd = false;
+	lrbp->intr_cmd = !ufshcd_is_intr_aggr_allowed(hba) ? true : false;
 	lrbp->command_type = UTP_CMD_TYPE_SCSI;
 
 	/* form UPIU before issuing the command */
@@ -2147,6 +2163,31 @@ int ufshcd_dme_get_attr(struct ufs_hba *hba, u32 attr_sel,
 	};
 	const char *get = action[!!peer];
 	int ret;
+	struct ufs_pa_layer_attr orig_pwr_info;
+	struct ufs_pa_layer_attr temp_pwr_info;
+	bool pwr_mode_change = false;
+
+	if (peer && (hba->quirks & UFSHCD_QUIRK_DME_PEER_ACCESS_AUTO_MODE)) {
+		orig_pwr_info = hba->pwr_info;
+		temp_pwr_info = orig_pwr_info;
+
+		if (orig_pwr_info.pwr_tx == FAST_MODE ||
+		    orig_pwr_info.pwr_rx == FAST_MODE) {
+			temp_pwr_info.pwr_tx = FASTAUTO_MODE;
+			temp_pwr_info.pwr_rx = FASTAUTO_MODE;
+			pwr_mode_change = true;
+		} else if (orig_pwr_info.pwr_tx == SLOW_MODE ||
+		    orig_pwr_info.pwr_rx == SLOW_MODE) {
+			temp_pwr_info.pwr_tx = SLOWAUTO_MODE;
+			temp_pwr_info.pwr_rx = SLOWAUTO_MODE;
+			pwr_mode_change = true;
+		}
+		if (pwr_mode_change) {
+			ret = ufshcd_change_power_mode(hba, &temp_pwr_info);
+			if (ret)
+				goto out;
+		}
+	}
 
 	uic_cmd.command = peer ?
 		UIC_CMD_DME_PEER_GET : UIC_CMD_DME_GET;
@@ -2161,6 +2202,10 @@ int ufshcd_dme_get_attr(struct ufs_hba *hba, u32 attr_sel,
 
 	if (mib_val)
 		*mib_val = uic_cmd.argument3;
+
+	if (peer && (hba->quirks & UFSHCD_QUIRK_DME_PEER_ACCESS_AUTO_MODE)
+	    && pwr_mode_change)
+		ufshcd_change_power_mode(hba, &orig_pwr_info);
 out:
 	return ret;
 }
@@ -2249,6 +2294,16 @@ static int ufshcd_uic_change_pwr_mode(struct ufs_hba *hba, u8 mode)
 	struct uic_command uic_cmd = {0};
 	int ret;
 
+	if (hba->quirks & UFSHCD_QUIRK_BROKEN_PA_RXHSUNTERMCAP) {
+		ret = ufshcd_dme_set(hba,
+				UIC_ARG_MIB_SEL(PA_RXHSUNTERMCAP, 0), 1);
+		if (ret) {
+			dev_err(hba->dev, "%s: failed to enable PA_RXHSUNTERMCAP ret %d\n",
+						__func__, ret);
+			goto out;
+		}
+	}
+
 	uic_cmd.command = UIC_CMD_DME_SET;
 	uic_cmd.argument1 = UIC_ARG_MIB(PA_PWRMODE);
 	uic_cmd.argument3 = mode;
@@ -2256,6 +2311,7 @@ static int ufshcd_uic_change_pwr_mode(struct ufs_hba *hba, u8 mode)
 	ret = ufshcd_uic_pwr_ctrl(hba, &uic_cmd);
 	ufshcd_release(hba);
 
+out:
 	return ret;
 }
 
@@ -2522,7 +2578,10 @@ static int ufshcd_make_hba_operational(struct ufs_hba *hba)
 	ufshcd_enable_intr(hba, UFSHCD_ENABLE_INTRS);
 
 	/* Configure interrupt aggregation */
-	ufshcd_config_intr_aggr(hba, hba->nutrs - 1, INT_AGGR_DEF_TO);
+	if (ufshcd_is_intr_aggr_allowed(hba))
+		ufshcd_config_intr_aggr(hba, hba->nutrs - 1, INT_AGGR_DEF_TO);
+	else
+		ufshcd_disable_intr_aggr(hba);
 
 	/* Configure UTRL and UTMRL base address registers */
 	ufshcd_writel(hba, lower_32_bits(hba->utrdl_dma_addr),
@@ -2628,6 +2687,42 @@ static int ufshcd_hba_enable(struct ufs_hba *hba)
 	return 0;
 }
 
+static int ufshcd_disable_tx_lcc(struct ufs_hba *hba, bool peer)
+{
+	int tx_lanes, i, err = 0;
+
+	if (!peer)
+		ufshcd_dme_get(hba, UIC_ARG_MIB(PA_CONNECTEDTXDATALANES),
+			       &tx_lanes);
+	else
+		ufshcd_dme_peer_get(hba, UIC_ARG_MIB(PA_CONNECTEDTXDATALANES),
+				    &tx_lanes);
+	for (i = 0; i < tx_lanes; i++) {
+		if (!peer)
+			err = ufshcd_dme_set(hba,
+				UIC_ARG_MIB_SEL(TX_LCC_ENABLE,
+					UIC_ARG_MPHY_TX_GEN_SEL_INDEX(i)),
+					0);
+		else
+			err = ufshcd_dme_peer_set(hba,
+				UIC_ARG_MIB_SEL(TX_LCC_ENABLE,
+					UIC_ARG_MPHY_TX_GEN_SEL_INDEX(i)),
+					0);
+		if (err) {
+			dev_err(hba->dev, "%s: TX LCC Disable failed, peer = %d, lane = %d, err = %d",
+				__func__, peer, i, err);
+			break;
+		}
+	}
+
+	return err;
+}
+
+static inline int ufshcd_disable_device_tx_lcc(struct ufs_hba *hba)
+{
+	return ufshcd_disable_tx_lcc(hba, true);
+}
+
 /**
  * ufshcd_link_startup - Initialize unipro link startup
  * @hba: per adapter instance
@@ -2665,6 +2760,12 @@ static int ufshcd_link_startup(struct ufs_hba *hba)
 		/* failed to get the link up... retire */
 		goto out;
 
+	if (hba->quirks & UFSHCD_QUIRK_BROKEN_LCC) {
+		ret = ufshcd_disable_device_tx_lcc(hba);
+		if (ret)
+			goto out;
+	}
+
 	/* Include any host controller configuration via UIC commands */
 	if (hba->vops && hba->vops->link_startup_notify) {
 		ret = hba->vops->link_startup_notify(hba, POST_CHANGE);
@@ -3073,7 +3174,8 @@ static void ufshcd_transfer_req_compl(struct ufs_hba *hba)
 	 * false interrupt if device completes another request after resetting
 	 * aggregation and before reading the DB.
 	 */
-	ufshcd_reset_intr_aggr(hba);
+	if (ufshcd_is_intr_aggr_allowed(hba))
+		ufshcd_reset_intr_aggr(hba);
 
 	tr_doorbell = ufshcd_readl(hba, REG_UTP_TRANSFER_REQ_DOOR_BELL);
 	completed_reqs = tr_doorbell ^ hba->outstanding_reqs;
diff --git a/drivers/scsi/ufs/ufshcd.h b/drivers/scsi/ufs/ufshcd.h
index b47ff07698e8..c40a0e78a6c4 100644
--- a/drivers/scsi/ufs/ufshcd.h
+++ b/drivers/scsi/ufs/ufshcd.h
@@ -246,6 +246,7 @@ struct ufs_pwr_mode_info {
  * @name: variant name
  * @init: called when the driver is initialized
  * @exit: called to cleanup everything done in init
+ * @get_ufs_hci_version: called to get UFS HCI version
  * @clk_scale_notify: notifies that clks are scaled up/down
  * @setup_clocks: called before touching any of the controller registers
  * @setup_regulators: called before accessing the host controller
@@ -263,6 +264,7 @@ struct ufs_hba_variant_ops {
 	const char *name;
 	int	(*init)(struct ufs_hba *);
 	void    (*exit)(struct ufs_hba *);
+	u32	(*get_ufs_hci_version)(struct ufs_hba *);
 	void    (*clk_scale_notify)(struct ufs_hba *);
 	int     (*setup_clocks)(struct ufs_hba *, bool);
 	int     (*setup_regulators)(struct ufs_hba *, bool);
@@ -417,11 +419,45 @@ struct ufs_hba {
 	unsigned int irq;
 	bool is_irq_enabled;
 
+	/* Interrupt aggregation support is broken */
+	#define UFSHCD_QUIRK_BROKEN_INTR_AGGR			UFS_BIT(0)
+
 	/*
 	 * delay before each dme command is required as the unipro
 	 * layer has shown instabilities
 	 */
-	#define UFSHCD_QUIRK_DELAY_BEFORE_DME_CMDS		UFS_BIT(0)
+	#define UFSHCD_QUIRK_DELAY_BEFORE_DME_CMDS		UFS_BIT(1)
+
+	/*
+	 * If UFS host controller is having issue in processing LCC (Line
+	 * Control Command) coming from device then enable this quirk.
+	 * When this quirk is enabled, host controller driver should disable
+	 * the LCC transmission on UFS device (by clearing TX_LCC_ENABLE
+	 * attribute of device to 0).
+	 */
+	#define UFSHCD_QUIRK_BROKEN_LCC				UFS_BIT(2)
+
+	/*
+	 * The attribute PA_RXHSUNTERMCAP specifies whether or not the
+	 * inbound Link supports unterminated line in HS mode. Setting this
+	 * attribute to 1 fixes moving to HS gear.
+	 */
+	#define UFSHCD_QUIRK_BROKEN_PA_RXHSUNTERMCAP		UFS_BIT(3)
+
+	/*
+	 * This quirk needs to be enabled if the host contoller only allows
+	 * accessing the peer dme attributes in AUTO mode (FAST AUTO or
+	 * SLOW AUTO).
+	 */
+	#define UFSHCD_QUIRK_DME_PEER_ACCESS_AUTO_MODE		UFS_BIT(4)
+
+	/*
+	 * This quirk needs to be enabled if the host contoller doesn't
+	 * advertise the correct version in UFS_VER register. If this quirk
+	 * is enabled, standard UFS host driver will call the vendor specific
+	 * ops (get_ufs_hci_version) to get the correct version.
+	 */
+	#define UFSHCD_QUIRK_BROKEN_UFS_HCI_VERSION		UFS_BIT(5)
 
 	unsigned int quirks;	/* Deviations from standard UFSHCI spec. */
 
@@ -478,6 +514,12 @@ struct ufs_hba {
 #define UFSHCD_CAP_CLK_SCALING	(1 << 2)
 	/* Allow auto bkops to enabled during runtime suspend */
 #define UFSHCD_CAP_AUTO_BKOPS_SUSPEND (1 << 3)
+	/*
+	 * This capability allows host controller driver to use the UFS HCI's
+	 * interrupt aggregation capability.
+	 * CAUTION: Enabling this might reduce overall UFS throughput.
+	 */
+#define UFSHCD_CAP_INTR_AGGR (1 << 4)
 
 	struct devfreq *devfreq;
 	struct ufs_clk_scaling clk_scaling;
@@ -502,6 +544,15 @@ static inline bool ufshcd_can_autobkops_during_suspend(struct ufs_hba *hba)
 	return hba->caps & UFSHCD_CAP_AUTO_BKOPS_SUSPEND;
 }
 
+static inline bool ufshcd_is_intr_aggr_allowed(struct ufs_hba *hba)
+{
+	if ((hba->caps & UFSHCD_CAP_INTR_AGGR) &&
+	    !(hba->quirks & UFSHCD_QUIRK_BROKEN_INTR_AGGR))
+		return true;
+	else
+		return false;
+}
+
 #define ufshcd_writel(hba, val, reg)	\
 	writel((val), (hba)->mmio_base + (reg))
 #define ufshcd_readl(hba, reg)	\
diff --git a/drivers/scsi/ufs/ufshci.h b/drivers/scsi/ufs/ufshci.h
index d5721199e9cc..0ae0967aaed8 100644
--- a/drivers/scsi/ufs/ufshci.h
+++ b/drivers/scsi/ufs/ufshci.h
@@ -89,8 +89,9 @@ enum {
 
 /* Controller UFSHCI version */
 enum {
-	UFSHCI_VERSION_10 = 0x00010000,
-	UFSHCI_VERSION_11 = 0x00010100,
+	UFSHCI_VERSION_10 = 0x00010000, /* 1.0 */
+	UFSHCI_VERSION_11 = 0x00010100, /* 1.1 */
+	UFSHCI_VERSION_20 = 0x00000200, /* 2.0 */
 };
 
 /*
@@ -206,6 +207,9 @@ enum {
 #define CONFIG_RESULT_CODE_MASK		0xFF
 #define GENERIC_ERROR_CODE_MASK		0xFF
 
+/* GenSelectorIndex calculation macros for M-PHY attributes */
+#define UIC_ARG_MPHY_TX_GEN_SEL_INDEX(lane) (lane)
+
 #define UIC_ARG_MIB_SEL(attr, sel)	((((attr) & 0xFFFF) << 16) |\
 					 ((sel) & 0xFFFF))
 #define UIC_ARG_MIB(attr)		UIC_ARG_MIB_SEL(attr, 0)
diff --git a/drivers/scsi/ufs/unipro.h b/drivers/scsi/ufs/unipro.h
index 3fc3e21b746b..816a8a46efb8 100644
--- a/drivers/scsi/ufs/unipro.h
+++ b/drivers/scsi/ufs/unipro.h
@@ -198,6 +198,14 @@ enum ufs_hs_gear_tag {
 #define T_TC0TXMAXSDUSIZE	0x4060
 #define T_TC1TXMAXSDUSIZE	0x4061
 
+#ifdef FALSE
+#undef FALSE
+#endif
+
+#ifdef TRUE
+#undef TRUE
+#endif
+
 /* Boolean attribute values */
 enum {
 	FALSE = 0,
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index f164f24a4a55..285f77544c36 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -501,6 +501,7 @@ static void virtio_scsi_init_hdr(struct virtio_device *vdev,
 	cmd->crn = 0;
 }
 
+#ifdef CONFIG_BLK_DEV_INTEGRITY
 static void virtio_scsi_init_hdr_pi(struct virtio_device *vdev,
 				    struct virtio_scsi_cmd_req_pi *cmd_pi,
 				    struct scsi_cmnd *sc)
@@ -524,6 +525,7 @@ static void virtio_scsi_init_hdr_pi(struct virtio_device *vdev,
 						       blk_rq_sectors(rq) *
 						       bi->tuple_size);
 }
+#endif
 
 static int virtscsi_queuecommand(struct virtio_scsi *vscsi,
 				 struct virtio_scsi_vq *req_vq,
@@ -546,11 +548,14 @@ static int virtscsi_queuecommand(struct virtio_scsi *vscsi,
 
 	BUG_ON(sc->cmd_len > VIRTIO_SCSI_CDB_SIZE);
 
+#ifdef CONFIG_BLK_DEV_INTEGRITY
 	if (virtio_has_feature(vscsi->vdev, VIRTIO_SCSI_F_T10_PI)) {
 		virtio_scsi_init_hdr_pi(vscsi->vdev, &cmd->req.cmd_pi, sc);
 		memcpy(cmd->req.cmd_pi.cdb, sc->cmnd, sc->cmd_len);
 		req_size = sizeof(cmd->req.cmd_pi);
-	} else {
+	} else
+#endif
+	{
 		virtio_scsi_init_hdr(vscsi->vdev, &cmd->req.cmd, sc);
 		memcpy(cmd->req.cmd.cdb, sc->cmnd, sc->cmd_len);
 		req_size = sizeof(cmd->req.cmd);
@@ -1002,6 +1007,7 @@ static int virtscsi_probe(struct virtio_device *vdev)
 	shost->max_cmd_len = VIRTIO_SCSI_CDB_SIZE;
 	shost->nr_hw_queues = num_queues;
 
+#ifdef CONFIG_BLK_DEV_INTEGRITY
 	if (virtio_has_feature(vdev, VIRTIO_SCSI_F_T10_PI)) {
 		host_prot = SHOST_DIF_TYPE1_PROTECTION | SHOST_DIF_TYPE2_PROTECTION |
 			    SHOST_DIF_TYPE3_PROTECTION | SHOST_DIX_TYPE1_PROTECTION |
@@ -1010,6 +1016,7 @@ static int virtscsi_probe(struct virtio_device *vdev)
 		scsi_host_set_prot(shost, host_prot);
 		scsi_host_set_guard(shost, SHOST_DIX_GUARD_CRC);
 	}
+#endif
 
 	err = scsi_add_host(shost, &vdev->dev);
 	if (err)
@@ -1090,7 +1097,9 @@ static struct virtio_device_id id_table[] = {
 static unsigned int features[] = {
 	VIRTIO_SCSI_F_HOTPLUG,
 	VIRTIO_SCSI_F_CHANGE,
+#ifdef CONFIG_BLK_DEV_INTEGRITY
 	VIRTIO_SCSI_F_T10_PI,
+#endif
 };
 
 static struct virtio_driver virtio_scsi_driver = {
diff --git a/drivers/scsi/wd719x.c b/drivers/scsi/wd719x.c
index 289ad016d925..61346aa73178 100644
--- a/drivers/scsi/wd719x.c
+++ b/drivers/scsi/wd719x.c
@@ -882,7 +882,6 @@ static struct scsi_host_template wd719x_template = {
 	.can_queue			= 255,
 	.this_id			= 7,
 	.sg_tablesize			= WD719X_SG,
-	.cmd_per_lun			= WD719X_CMD_PER_LUN,
 	.use_clustering			= ENABLE_CLUSTERING,
 };
 
diff --git a/drivers/scsi/wd719x.h b/drivers/scsi/wd719x.h
index 185e30e4eb93..9c6dd45f95f5 100644
--- a/drivers/scsi/wd719x.h
+++ b/drivers/scsi/wd719x.h
@@ -2,8 +2,6 @@
 #define _WD719X_H_
 
 #define WD719X_SG 255		/* Scatter/gather size */
-#define WD719X_CMD_PER_LUN 1	/* We should be able to do linked commands, but
-				 * this is 1 for now to be safe. */
 
 struct wd719x_sglist {
 	__le32 ptr;
diff --git a/drivers/sh/intc/core.c b/drivers/sh/intc/core.c
index 81f22980b2de..156b790072b4 100644
--- a/drivers/sh/intc/core.c
+++ b/drivers/sh/intc/core.c
@@ -366,8 +366,9 @@ int __init register_intc_controller(struct intc_desc *desc)
 
 			/* redirect this interrupts to the first one */
 			irq_set_chip(irq2, &dummy_irq_chip);
-			irq_set_chained_handler(irq2, intc_redirect_irq);
-			irq_set_handler_data(irq2, (void *)irq);
+			irq_set_chained_handler_and_data(irq2,
+							 intc_redirect_irq,
+							 (void *)irq);
 		}
 	}
 
diff --git a/drivers/sh/intc/virq.c b/drivers/sh/intc/virq.c
index f30ac9354ff2..f5f1b821241a 100644
--- a/drivers/sh/intc/virq.c
+++ b/drivers/sh/intc/virq.c
@@ -243,8 +243,9 @@ restart:
 		 */
 		irq_set_nothread(irq);
 
-		irq_set_chained_handler(entry->pirq, intc_virq_handler);
+		/* Set handler data before installing the handler */
 		add_virq_to_pirq(entry->pirq, irq);
+		irq_set_chained_handler(entry->pirq, intc_virq_handler);
 
 		radix_tree_tag_clear(&d->tree, entry->enum_id,
 				     INTC_TAG_VIRQ_NEEDS_ALLOC);
diff --git a/drivers/sh/pm_runtime.c b/drivers/sh/pm_runtime.c
index fe8875f0d7be..d3d1891cda3c 100644
--- a/drivers/sh/pm_runtime.c
+++ b/drivers/sh/pm_runtime.c
@@ -20,58 +20,15 @@
 #include <linux/bitmap.h>
 #include <linux/slab.h>
 
-#ifdef CONFIG_PM
-static int sh_pm_runtime_suspend(struct device *dev)
-{
-	int ret;
-
-	ret = pm_generic_runtime_suspend(dev);
-	if (ret) {
-		dev_err(dev, "failed to suspend device\n");
-		return ret;
-	}
-
-	ret = pm_clk_suspend(dev);
-	if (ret) {
-		dev_err(dev, "failed to suspend clock\n");
-		pm_generic_runtime_resume(dev);
-		return ret;
-	}
-
-	return 0;
-}
-
-static int sh_pm_runtime_resume(struct device *dev)
-{
-	int ret;
-
-	ret = pm_clk_resume(dev);
-	if (ret) {
-		dev_err(dev, "failed to resume clock\n");
-		return ret;
-	}
-
-	return pm_generic_runtime_resume(dev);
-}
-
 static struct dev_pm_domain default_pm_domain = {
 	.ops = {
-		.runtime_suspend = sh_pm_runtime_suspend,
-		.runtime_resume = sh_pm_runtime_resume,
+		USE_PM_CLK_RUNTIME_OPS
 		USE_PLATFORM_PM_SLEEP_OPS
 	},
 };
 
-#define DEFAULT_PM_DOMAIN_PTR	(&default_pm_domain)
-
-#else
-
-#define DEFAULT_PM_DOMAIN_PTR	NULL
-
-#endif /* CONFIG_PM */
-
 static struct pm_clk_notifier_block platform_bus_notifier = {
-	.pm_domain = DEFAULT_PM_DOMAIN_PTR,
+	.pm_domain = &default_pm_domain,
 	.con_ids = { NULL, },
 };
 
diff --git a/drivers/soc/Kconfig b/drivers/soc/Kconfig
index d8bde82f0370..96ddecb92254 100644
--- a/drivers/soc/Kconfig
+++ b/drivers/soc/Kconfig
@@ -2,6 +2,7 @@ menu "SOC (System On Chip) specific Drivers"
 
 source "drivers/soc/mediatek/Kconfig"
 source "drivers/soc/qcom/Kconfig"
+source "drivers/soc/sunxi/Kconfig"
 source "drivers/soc/ti/Kconfig"
 source "drivers/soc/versatile/Kconfig"
 
diff --git a/drivers/soc/Makefile b/drivers/soc/Makefile
index 70042b259744..7dc7c0d8a2c1 100644
--- a/drivers/soc/Makefile
+++ b/drivers/soc/Makefile
@@ -4,6 +4,7 @@
 
 obj-$(CONFIG_ARCH_MEDIATEK)	+= mediatek/
 obj-$(CONFIG_ARCH_QCOM)		+= qcom/
+obj-$(CONFIG_ARCH_SUNXI)	+= sunxi/
 obj-$(CONFIG_ARCH_TEGRA)	+= tegra/
 obj-$(CONFIG_SOC_TI)		+= ti/
 obj-$(CONFIG_PLAT_VERSATILE)	+= versatile/
diff --git a/drivers/soc/mediatek/Kconfig b/drivers/soc/mediatek/Kconfig
index bcdb22d5e215..3c1850332a90 100644
--- a/drivers/soc/mediatek/Kconfig
+++ b/drivers/soc/mediatek/Kconfig
@@ -4,6 +4,7 @@
 config MTK_PMIC_WRAP
 	tristate "MediaTek PMIC Wrapper Support"
 	depends on ARCH_MEDIATEK
+	depends on RESET_CONTROLLER
 	select REGMAP
 	help
 	  Say yes here to add support for MediaTek PMIC Wrapper found
diff --git a/drivers/soc/mediatek/mtk-pmic-wrap.c b/drivers/soc/mediatek/mtk-pmic-wrap.c
index db5be1eec54c..f432291feee9 100644
--- a/drivers/soc/mediatek/mtk-pmic-wrap.c
+++ b/drivers/soc/mediatek/mtk-pmic-wrap.c
@@ -443,11 +443,6 @@ static int pwrap_wait_for_state(struct pmic_wrapper *wrp,
 static int pwrap_write(struct pmic_wrapper *wrp, u32 adr, u32 wdata)
 {
 	int ret;
-	u32 val;
-
-	val = pwrap_readl(wrp, PWRAP_WACS2_RDATA);
-	if (PWRAP_GET_WACS_FSM(val) == PWRAP_WACS_FSM_WFVLDCLR)
-		pwrap_writel(wrp, 1, PWRAP_WACS2_VLDCLR);
 
 	ret = pwrap_wait_for_state(wrp, pwrap_is_fsm_idle);
 	if (ret)
@@ -462,11 +457,6 @@ static int pwrap_write(struct pmic_wrapper *wrp, u32 adr, u32 wdata)
 static int pwrap_read(struct pmic_wrapper *wrp, u32 adr, u32 *rdata)
 {
 	int ret;
-	u32 val;
-
-	val = pwrap_readl(wrp, PWRAP_WACS2_RDATA);
-	if (PWRAP_GET_WACS_FSM(val) == PWRAP_WACS_FSM_WFVLDCLR)
-		pwrap_writel(wrp, 1, PWRAP_WACS2_VLDCLR);
 
 	ret = pwrap_wait_for_state(wrp, pwrap_is_fsm_idle);
 	if (ret)
@@ -480,6 +470,8 @@ static int pwrap_read(struct pmic_wrapper *wrp, u32 adr, u32 *rdata)
 
 	*rdata = PWRAP_GET_WACS_RDATA(pwrap_readl(wrp, PWRAP_WACS2_RDATA));
 
+	pwrap_writel(wrp, 1, PWRAP_WACS2_VLDCLR);
+
 	return 0;
 }
 
@@ -563,45 +555,17 @@ static int pwrap_init_sidly(struct pmic_wrapper *wrp)
 
 static int pwrap_init_reg_clock(struct pmic_wrapper *wrp)
 {
-	unsigned long rate_spi;
-	int ck_mhz;
-
-	rate_spi = clk_get_rate(wrp->clk_spi);
-
-	if (rate_spi > 26000000)
-		ck_mhz = 26;
-	else if (rate_spi > 18000000)
-		ck_mhz = 18;
-	else
-		ck_mhz = 0;
-
-	switch (ck_mhz) {
-	case 18:
-		if (pwrap_is_mt8135(wrp))
-			pwrap_writel(wrp, 0xc, PWRAP_CSHEXT);
-		pwrap_writel(wrp, 0x4, PWRAP_CSHEXT_WRITE);
-		pwrap_writel(wrp, 0xc, PWRAP_CSHEXT_READ);
-		pwrap_writel(wrp, 0x0, PWRAP_CSLEXT_START);
-		pwrap_writel(wrp, 0x0, PWRAP_CSLEXT_END);
-		break;
-	case 26:
-		if (pwrap_is_mt8135(wrp))
-			pwrap_writel(wrp, 0x4, PWRAP_CSHEXT);
+	if (pwrap_is_mt8135(wrp)) {
+		pwrap_writel(wrp, 0x4, PWRAP_CSHEXT);
 		pwrap_writel(wrp, 0x0, PWRAP_CSHEXT_WRITE);
 		pwrap_writel(wrp, 0x4, PWRAP_CSHEXT_READ);
 		pwrap_writel(wrp, 0x0, PWRAP_CSLEXT_START);
 		pwrap_writel(wrp, 0x0, PWRAP_CSLEXT_END);
-		break;
-	case 0:
-		if (pwrap_is_mt8135(wrp))
-			pwrap_writel(wrp, 0xf, PWRAP_CSHEXT);
-		pwrap_writel(wrp, 0xf, PWRAP_CSHEXT_WRITE);
-		pwrap_writel(wrp, 0xf, PWRAP_CSHEXT_READ);
-		pwrap_writel(wrp, 0xf, PWRAP_CSLEXT_START);
-		pwrap_writel(wrp, 0xf, PWRAP_CSLEXT_END);
-		break;
-	default:
-		return -EINVAL;
+	} else {
+		pwrap_writel(wrp, 0x0, PWRAP_CSHEXT_WRITE);
+		pwrap_writel(wrp, 0x4, PWRAP_CSHEXT_READ);
+		pwrap_writel(wrp, 0x2, PWRAP_CSLEXT_START);
+		pwrap_writel(wrp, 0x2, PWRAP_CSLEXT_END);
 	}
 
 	return 0;
diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
index 460b2dba109c..5eea374c8fa6 100644
--- a/drivers/soc/qcom/Kconfig
+++ b/drivers/soc/qcom/Kconfig
@@ -10,3 +10,10 @@ config QCOM_GSBI
           functions for connecting the underlying serial UART, SPI, and I2C
           devices to the output pins.
 
+config QCOM_PM
+	bool "Qualcomm Power Management"
+	depends on ARCH_QCOM && !ARM64
+	help
+	  QCOM Platform specific power driver to manage cores and L2 low power
+	  modes. It interface with various system drivers to put the cores in
+	  low power modes.
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
index 438901257ac1..931d385386c5 100644
--- a/drivers/soc/qcom/Makefile
+++ b/drivers/soc/qcom/Makefile
@@ -1 +1,2 @@
 obj-$(CONFIG_QCOM_GSBI)	+=	qcom_gsbi.o
+obj-$(CONFIG_QCOM_PM)	+=	spm.o
diff --git a/drivers/soc/qcom/spm.c b/drivers/soc/qcom/spm.c
new file mode 100644
index 000000000000..b04b05a0904e
--- /dev/null
+++ b/drivers/soc/qcom/spm.c
@@ -0,0 +1,385 @@
+/*
+ * Copyright (c) 2011-2014, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2014,2015, Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 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/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/cpuidle.h>
+#include <linux/cpu_pm.h>
+#include <linux/qcom_scm.h>
+
+#include <asm/cpuidle.h>
+#include <asm/proc-fns.h>
+#include <asm/suspend.h>
+
+#define MAX_PMIC_DATA		2
+#define MAX_SEQ_DATA		64
+#define SPM_CTL_INDEX		0x7f
+#define SPM_CTL_INDEX_SHIFT	4
+#define SPM_CTL_EN		BIT(0)
+
+enum pm_sleep_mode {
+	PM_SLEEP_MODE_STBY,
+	PM_SLEEP_MODE_RET,
+	PM_SLEEP_MODE_SPC,
+	PM_SLEEP_MODE_PC,
+	PM_SLEEP_MODE_NR,
+};
+
+enum spm_reg {
+	SPM_REG_CFG,
+	SPM_REG_SPM_CTL,
+	SPM_REG_DLY,
+	SPM_REG_PMIC_DLY,
+	SPM_REG_PMIC_DATA_0,
+	SPM_REG_PMIC_DATA_1,
+	SPM_REG_VCTL,
+	SPM_REG_SEQ_ENTRY,
+	SPM_REG_SPM_STS,
+	SPM_REG_PMIC_STS,
+	SPM_REG_NR,
+};
+
+struct spm_reg_data {
+	const u8 *reg_offset;
+	u32 spm_cfg;
+	u32 spm_dly;
+	u32 pmic_dly;
+	u32 pmic_data[MAX_PMIC_DATA];
+	u8 seq[MAX_SEQ_DATA];
+	u8 start_index[PM_SLEEP_MODE_NR];
+};
+
+struct spm_driver_data {
+	void __iomem *reg_base;
+	const struct spm_reg_data *reg_data;
+};
+
+static const u8 spm_reg_offset_v2_1[SPM_REG_NR] = {
+	[SPM_REG_CFG]		= 0x08,
+	[SPM_REG_SPM_CTL]	= 0x30,
+	[SPM_REG_DLY]		= 0x34,
+	[SPM_REG_SEQ_ENTRY]	= 0x80,
+};
+
+/* SPM register data for 8974, 8084 */
+static const struct spm_reg_data spm_reg_8974_8084_cpu  = {
+	.reg_offset = spm_reg_offset_v2_1,
+	.spm_cfg = 0x1,
+	.spm_dly = 0x3C102800,
+	.seq = { 0x03, 0x0B, 0x0F, 0x00, 0x20, 0x80, 0x10, 0xE8, 0x5B, 0x03,
+		0x3B, 0xE8, 0x5B, 0x82, 0x10, 0x0B, 0x30, 0x06, 0x26, 0x30,
+		0x0F },
+	.start_index[PM_SLEEP_MODE_STBY] = 0,
+	.start_index[PM_SLEEP_MODE_SPC] = 3,
+};
+
+static const u8 spm_reg_offset_v1_1[SPM_REG_NR] = {
+	[SPM_REG_CFG]		= 0x08,
+	[SPM_REG_SPM_CTL]	= 0x20,
+	[SPM_REG_PMIC_DLY]	= 0x24,
+	[SPM_REG_PMIC_DATA_0]	= 0x28,
+	[SPM_REG_PMIC_DATA_1]	= 0x2C,
+	[SPM_REG_SEQ_ENTRY]	= 0x80,
+};
+
+/* SPM register data for 8064 */
+static const struct spm_reg_data spm_reg_8064_cpu = {
+	.reg_offset = spm_reg_offset_v1_1,
+	.spm_cfg = 0x1F,
+	.pmic_dly = 0x02020004,
+	.pmic_data[0] = 0x0084009C,
+	.pmic_data[1] = 0x00A4001C,
+	.seq = { 0x03, 0x0F, 0x00, 0x24, 0x54, 0x10, 0x09, 0x03, 0x01,
+		0x10, 0x54, 0x30, 0x0C, 0x24, 0x30, 0x0F },
+	.start_index[PM_SLEEP_MODE_STBY] = 0,
+	.start_index[PM_SLEEP_MODE_SPC] = 2,
+};
+
+static DEFINE_PER_CPU(struct spm_driver_data *, cpu_spm_drv);
+
+typedef int (*idle_fn)(int);
+static DEFINE_PER_CPU(idle_fn*, qcom_idle_ops);
+
+static inline void spm_register_write(struct spm_driver_data *drv,
+					enum spm_reg reg, u32 val)
+{
+	if (drv->reg_data->reg_offset[reg])
+		writel_relaxed(val, drv->reg_base +
+				drv->reg_data->reg_offset[reg]);
+}
+
+/* Ensure a guaranteed write, before return */
+static inline void spm_register_write_sync(struct spm_driver_data *drv,
+					enum spm_reg reg, u32 val)
+{
+	u32 ret;
+
+	if (!drv->reg_data->reg_offset[reg])
+		return;
+
+	do {
+		writel_relaxed(val, drv->reg_base +
+				drv->reg_data->reg_offset[reg]);
+		ret = readl_relaxed(drv->reg_base +
+				drv->reg_data->reg_offset[reg]);
+		if (ret == val)
+			break;
+		cpu_relax();
+	} while (1);
+}
+
+static inline u32 spm_register_read(struct spm_driver_data *drv,
+					enum spm_reg reg)
+{
+	return readl_relaxed(drv->reg_base + drv->reg_data->reg_offset[reg]);
+}
+
+static void spm_set_low_power_mode(struct spm_driver_data *drv,
+					enum pm_sleep_mode mode)
+{
+	u32 start_index;
+	u32 ctl_val;
+
+	start_index = drv->reg_data->start_index[mode];
+
+	ctl_val = spm_register_read(drv, SPM_REG_SPM_CTL);
+	ctl_val &= ~(SPM_CTL_INDEX << SPM_CTL_INDEX_SHIFT);
+	ctl_val |= start_index << SPM_CTL_INDEX_SHIFT;
+	ctl_val |= SPM_CTL_EN;
+	spm_register_write_sync(drv, SPM_REG_SPM_CTL, ctl_val);
+}
+
+static int qcom_pm_collapse(unsigned long int unused)
+{
+	qcom_scm_cpu_power_down(QCOM_SCM_CPU_PWR_DOWN_L2_ON);
+
+	/*
+	 * Returns here only if there was a pending interrupt and we did not
+	 * power down as a result.
+	 */
+	return -1;
+}
+
+static int qcom_cpu_spc(int cpu)
+{
+	int ret;
+	struct spm_driver_data *drv = per_cpu(cpu_spm_drv, cpu);
+
+	spm_set_low_power_mode(drv, PM_SLEEP_MODE_SPC);
+	ret = cpu_suspend(0, qcom_pm_collapse);
+	/*
+	 * ARM common code executes WFI without calling into our driver and
+	 * if the SPM mode is not reset, then we may accidently power down the
+	 * cpu when we intended only to gate the cpu clock.
+	 * Ensure the state is set to standby before returning.
+	 */
+	spm_set_low_power_mode(drv, PM_SLEEP_MODE_STBY);
+
+	return ret;
+}
+
+static int qcom_idle_enter(int cpu, unsigned long index)
+{
+	return per_cpu(qcom_idle_ops, cpu)[index](cpu);
+}
+
+static const struct of_device_id qcom_idle_state_match[] __initconst = {
+	{ .compatible = "qcom,idle-state-spc", .data = qcom_cpu_spc },
+	{ },
+};
+
+static int __init qcom_cpuidle_init(struct device_node *cpu_node, int cpu)
+{
+	const struct of_device_id *match_id;
+	struct device_node *state_node;
+	int i;
+	int state_count = 1;
+	idle_fn idle_fns[CPUIDLE_STATE_MAX];
+	idle_fn *fns;
+	cpumask_t mask;
+	bool use_scm_power_down = false;
+
+	for (i = 0; ; i++) {
+		state_node = of_parse_phandle(cpu_node, "cpu-idle-states", i);
+		if (!state_node)
+			break;
+
+		if (!of_device_is_available(state_node))
+			continue;
+
+		if (i == CPUIDLE_STATE_MAX) {
+			pr_warn("%s: cpuidle states reached max possible\n",
+					__func__);
+			break;
+		}
+
+		match_id = of_match_node(qcom_idle_state_match, state_node);
+		if (!match_id)
+			return -ENODEV;
+
+		idle_fns[state_count] = match_id->data;
+
+		/* Check if any of the states allow power down */
+		if (match_id->data == qcom_cpu_spc)
+			use_scm_power_down = true;
+
+		state_count++;
+	}
+
+	if (state_count == 1)
+		goto check_spm;
+
+	fns = devm_kcalloc(get_cpu_device(cpu), state_count, sizeof(*fns),
+			GFP_KERNEL);
+	if (!fns)
+		return -ENOMEM;
+
+	for (i = 1; i < state_count; i++)
+		fns[i] = idle_fns[i];
+
+	if (use_scm_power_down) {
+		/* We have atleast one power down mode */
+		cpumask_clear(&mask);
+		cpumask_set_cpu(cpu, &mask);
+		qcom_scm_set_warm_boot_addr(cpu_resume_arm, &mask);
+	}
+
+	per_cpu(qcom_idle_ops, cpu) = fns;
+
+	/*
+	 * SPM probe for the cpu should have happened by now, if the
+	 * SPM device does not exist, return -ENXIO to indicate that the
+	 * cpu does not support idle states.
+	 */
+check_spm:
+	return per_cpu(cpu_spm_drv, cpu) ? 0 : -ENXIO;
+}
+
+static struct cpuidle_ops qcom_cpuidle_ops __initdata = {
+	.suspend = qcom_idle_enter,
+	.init = qcom_cpuidle_init,
+};
+
+CPUIDLE_METHOD_OF_DECLARE(qcom_idle_v1, "qcom,kpss-acc-v1", &qcom_cpuidle_ops);
+CPUIDLE_METHOD_OF_DECLARE(qcom_idle_v2, "qcom,kpss-acc-v2", &qcom_cpuidle_ops);
+
+static struct spm_driver_data *spm_get_drv(struct platform_device *pdev,
+		int *spm_cpu)
+{
+	struct spm_driver_data *drv = NULL;
+	struct device_node *cpu_node, *saw_node;
+	int cpu;
+	bool found;
+
+	for_each_possible_cpu(cpu) {
+		cpu_node = of_cpu_device_node_get(cpu);
+		if (!cpu_node)
+			continue;
+		saw_node = of_parse_phandle(cpu_node, "qcom,saw", 0);
+		found = (saw_node == pdev->dev.of_node);
+		of_node_put(saw_node);
+		of_node_put(cpu_node);
+		if (found)
+			break;
+	}
+
+	if (found) {
+		drv = devm_kzalloc(&pdev->dev, sizeof(*drv), GFP_KERNEL);
+		if (drv)
+			*spm_cpu = cpu;
+	}
+
+	return drv;
+}
+
+static const struct of_device_id spm_match_table[] = {
+	{ .compatible = "qcom,msm8974-saw2-v2.1-cpu",
+	  .data = &spm_reg_8974_8084_cpu },
+	{ .compatible = "qcom,apq8084-saw2-v2.1-cpu",
+	  .data = &spm_reg_8974_8084_cpu },
+	{ .compatible = "qcom,apq8064-saw2-v1.1-cpu",
+	  .data = &spm_reg_8064_cpu },
+	{ },
+};
+
+static int spm_dev_probe(struct platform_device *pdev)
+{
+	struct spm_driver_data *drv;
+	struct resource *res;
+	const struct of_device_id *match_id;
+	void __iomem *addr;
+	int cpu;
+
+	drv = spm_get_drv(pdev, &cpu);
+	if (!drv)
+		return -EINVAL;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	drv->reg_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(drv->reg_base))
+		return PTR_ERR(drv->reg_base);
+
+	match_id = of_match_node(spm_match_table, pdev->dev.of_node);
+	if (!match_id)
+		return -ENODEV;
+
+	drv->reg_data = match_id->data;
+
+	/* Write the SPM sequences first.. */
+	addr = drv->reg_base + drv->reg_data->reg_offset[SPM_REG_SEQ_ENTRY];
+	__iowrite32_copy(addr, drv->reg_data->seq,
+			ARRAY_SIZE(drv->reg_data->seq) / 4);
+
+	/*
+	 * ..and then the control registers.
+	 * On some SoC if the control registers are written first and if the
+	 * CPU was held in reset, the reset signal could trigger the SPM state
+	 * machine, before the sequences are completely written.
+	 */
+	spm_register_write(drv, SPM_REG_CFG, drv->reg_data->spm_cfg);
+	spm_register_write(drv, SPM_REG_DLY, drv->reg_data->spm_dly);
+	spm_register_write(drv, SPM_REG_PMIC_DLY, drv->reg_data->pmic_dly);
+	spm_register_write(drv, SPM_REG_PMIC_DATA_0,
+				drv->reg_data->pmic_data[0]);
+	spm_register_write(drv, SPM_REG_PMIC_DATA_1,
+				drv->reg_data->pmic_data[1]);
+
+	/* Set up Standby as the default low power mode */
+	spm_set_low_power_mode(drv, PM_SLEEP_MODE_STBY);
+
+	per_cpu(cpu_spm_drv, cpu) = drv;
+
+	return 0;
+}
+
+static struct platform_driver spm_driver = {
+	.probe = spm_dev_probe,
+	.driver = {
+		.name = "saw",
+		.of_match_table = spm_match_table,
+	},
+};
+module_platform_driver(spm_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("SAW power controller driver");
+MODULE_ALIAS("platform:saw");
diff --git a/drivers/soc/sunxi/Kconfig b/drivers/soc/sunxi/Kconfig
new file mode 100644
index 000000000000..353b07e40176
--- /dev/null
+++ b/drivers/soc/sunxi/Kconfig
@@ -0,0 +1,10 @@
+#
+# Allwinner sunXi SoC drivers
+#
+config SUNXI_SRAM
+	bool
+	default ARCH_SUNXI
+	help
+	  Say y here to enable the SRAM controller support. This
+	  device is responsible on mapping the SRAM in the sunXi SoCs
+	  whether to the CPU/DMA, or to the devices.
diff --git a/drivers/soc/sunxi/Makefile b/drivers/soc/sunxi/Makefile
new file mode 100644
index 000000000000..4cf9dbdf346e
--- /dev/null
+++ b/drivers/soc/sunxi/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_SUNXI_SRAM) +=	sunxi_sram.o
diff --git a/drivers/soc/sunxi/sunxi_sram.c b/drivers/soc/sunxi/sunxi_sram.c
new file mode 100644
index 000000000000..bc52670c8f4b
--- /dev/null
+++ b/drivers/soc/sunxi/sunxi_sram.c
@@ -0,0 +1,284 @@
+/*
+ * Allwinner SoCs SRAM Controller Driver
+ *
+ * Copyright (C) 2015 Maxime Ripard
+ *
+ * Author: Maxime Ripard <maxime.ripard@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/debugfs.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+
+#include <linux/soc/sunxi/sunxi_sram.h>
+
+struct sunxi_sram_func {
+	char	*func;
+	u8	val;
+};
+
+struct sunxi_sram_data {
+	char			*name;
+	u8			reg;
+	u8			offset;
+	u8			width;
+	struct sunxi_sram_func	*func;
+	struct list_head	list;
+};
+
+struct sunxi_sram_desc {
+	struct sunxi_sram_data	data;
+	bool			claimed;
+};
+
+#define SUNXI_SRAM_MAP(_val, _func)				\
+	{							\
+		.func = _func,					\
+		.val = _val,					\
+	}
+
+#define SUNXI_SRAM_DATA(_name, _reg, _off, _width, ...)		\
+	{							\
+		.name = _name,					\
+		.reg = _reg,					\
+		.offset = _off,					\
+		.width = _width,				\
+		.func = (struct sunxi_sram_func[]){		\
+			__VA_ARGS__, { } },			\
+	}
+
+static struct sunxi_sram_desc sun4i_a10_sram_a3_a4 = {
+	.data	= SUNXI_SRAM_DATA("A3-A4", 0x4, 0x4, 2,
+				  SUNXI_SRAM_MAP(0, "cpu"),
+				  SUNXI_SRAM_MAP(1, "emac")),
+};
+
+static struct sunxi_sram_desc sun4i_a10_sram_d = {
+	.data	= SUNXI_SRAM_DATA("D", 0x4, 0x0, 1,
+				  SUNXI_SRAM_MAP(0, "cpu"),
+				  SUNXI_SRAM_MAP(1, "usb-otg")),
+};
+
+static const struct of_device_id sunxi_sram_dt_ids[] = {
+	{
+		.compatible	= "allwinner,sun4i-a10-sram-a3-a4",
+		.data		= &sun4i_a10_sram_a3_a4.data,
+	},
+	{
+		.compatible	= "allwinner,sun4i-a10-sram-d",
+		.data		= &sun4i_a10_sram_d.data,
+	},
+	{}
+};
+
+static struct device *sram_dev;
+static LIST_HEAD(claimed_sram);
+static DEFINE_SPINLOCK(sram_lock);
+static void __iomem *base;
+
+static int sunxi_sram_show(struct seq_file *s, void *data)
+{
+	struct device_node *sram_node, *section_node;
+	const struct sunxi_sram_data *sram_data;
+	const struct of_device_id *match;
+	struct sunxi_sram_func *func;
+	const __be32 *sram_addr_p, *section_addr_p;
+	u32 val;
+
+	seq_puts(s, "Allwinner sunXi SRAM\n");
+	seq_puts(s, "--------------------\n\n");
+
+	for_each_child_of_node(sram_dev->of_node, sram_node) {
+		sram_addr_p = of_get_address(sram_node, 0, NULL, NULL);
+
+		seq_printf(s, "sram@%08x\n",
+			   be32_to_cpu(*sram_addr_p));
+
+		for_each_child_of_node(sram_node, section_node) {
+			match = of_match_node(sunxi_sram_dt_ids, section_node);
+			if (!match)
+				continue;
+			sram_data = match->data;
+
+			section_addr_p = of_get_address(section_node, 0,
+							NULL, NULL);
+
+			seq_printf(s, "\tsection@%04x\t(%s)\n",
+				   be32_to_cpu(*section_addr_p),
+				   sram_data->name);
+
+			val = readl(base + sram_data->reg);
+			val >>= sram_data->offset;
+			val &= sram_data->width;
+
+			for (func = sram_data->func; func->func; func++) {
+				seq_printf(s, "\t\t%s%c\n", func->func,
+					   func->val == val ? '*' : ' ');
+			}
+		}
+
+		seq_puts(s, "\n");
+	}
+
+	return 0;
+}
+
+static int sunxi_sram_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, sunxi_sram_show, inode->i_private);
+}
+
+static const struct file_operations sunxi_sram_fops = {
+	.open = sunxi_sram_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static inline struct sunxi_sram_desc *to_sram_desc(const struct sunxi_sram_data *data)
+{
+	return container_of(data, struct sunxi_sram_desc, data);
+}
+
+static const struct sunxi_sram_data *sunxi_sram_of_parse(struct device_node *node,
+							 unsigned int *value)
+{
+	const struct of_device_id *match;
+	struct of_phandle_args args;
+	int ret;
+
+	ret = of_parse_phandle_with_fixed_args(node, "allwinner,sram", 1, 0,
+					       &args);
+	if (ret)
+		return ERR_PTR(ret);
+
+	if (!of_device_is_available(args.np)) {
+		ret = -EBUSY;
+		goto err;
+	}
+
+	if (value)
+		*value = args.args[0];
+
+	match = of_match_node(sunxi_sram_dt_ids, args.np);
+	if (!match) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	of_node_put(args.np);
+	return match->data;
+
+err:
+	of_node_put(args.np);
+	return ERR_PTR(ret);
+}
+
+int sunxi_sram_claim(struct device *dev)
+{
+	const struct sunxi_sram_data *sram_data;
+	struct sunxi_sram_desc *sram_desc;
+	unsigned int device;
+	u32 val, mask;
+
+	if (IS_ERR(base))
+		return -EPROBE_DEFER;
+
+	if (!dev || !dev->of_node)
+		return -EINVAL;
+
+	sram_data = sunxi_sram_of_parse(dev->of_node, &device);
+	if (IS_ERR(sram_data))
+		return PTR_ERR(sram_data);
+
+	sram_desc = to_sram_desc(sram_data);
+
+	spin_lock(&sram_lock);
+
+	if (sram_desc->claimed) {
+		spin_unlock(&sram_lock);
+		return -EBUSY;
+	}
+
+	mask = GENMASK(sram_data->offset + sram_data->width, sram_data->offset);
+	val = readl(base + sram_data->reg);
+	val &= ~mask;
+	writel(val | ((device << sram_data->offset) & mask),
+	       base + sram_data->reg);
+
+	spin_unlock(&sram_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(sunxi_sram_claim);
+
+int sunxi_sram_release(struct device *dev)
+{
+	const struct sunxi_sram_data *sram_data;
+	struct sunxi_sram_desc *sram_desc;
+
+	if (!dev || !dev->of_node)
+		return -EINVAL;
+
+	sram_data = sunxi_sram_of_parse(dev->of_node, NULL);
+	if (IS_ERR(sram_data))
+		return -EINVAL;
+
+	sram_desc = to_sram_desc(sram_data);
+
+	spin_lock(&sram_lock);
+	sram_desc->claimed = false;
+	spin_unlock(&sram_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(sunxi_sram_release);
+
+static int sunxi_sram_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct dentry *d;
+
+	sram_dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	of_platform_populate(pdev->dev.of_node, NULL, NULL, &pdev->dev);
+
+	d = debugfs_create_file("sram", S_IRUGO, NULL, NULL,
+				&sunxi_sram_fops);
+	if (!d)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static const struct of_device_id sunxi_sram_dt_match[] = {
+	{ .compatible = "allwinner,sun4i-a10-sram-controller" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, sunxi_sram_dt_match);
+
+static struct platform_driver sunxi_sram_driver = {
+	.driver = {
+		.name		= "sunxi-sram",
+		.of_match_table	= sunxi_sram_dt_match,
+	},
+	.probe	= sunxi_sram_probe,
+};
+module_platform_driver(sunxi_sram_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com>");
+MODULE_DESCRIPTION("Allwinner sunXi SRAM Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/soc/tegra/fuse/fuse-tegra20.c b/drivers/soc/tegra/fuse/fuse-tegra20.c
index 5eff6f097f98..6acc2c44ee2c 100644
--- a/drivers/soc/tegra/fuse/fuse-tegra20.c
+++ b/drivers/soc/tegra/fuse/fuse-tegra20.c
@@ -59,6 +59,7 @@ static u32 tegra20_fuse_readl(const unsigned int offset)
 	int ret;
 	u32 val = 0;
 	struct dma_async_tx_descriptor *dma_desc;
+	unsigned long time_left;
 
 	mutex_lock(&apb_dma_lock);
 
@@ -82,9 +83,10 @@ static u32 tegra20_fuse_readl(const unsigned int offset)
 
 	dmaengine_submit(dma_desc);
 	dma_async_issue_pending(apb_dma_chan);
-	ret = wait_for_completion_timeout(&apb_dma_wait, msecs_to_jiffies(50));
+	time_left = wait_for_completion_timeout(&apb_dma_wait,
+						msecs_to_jiffies(50));
 
-	if (WARN(ret == 0, "apb read dma timed out"))
+	if (WARN(time_left == 0, "apb read dma timed out"))
 		dmaengine_terminate_all(apb_dma_chan);
 	else
 		val = *apb_buffer;
diff --git a/drivers/soc/tegra/fuse/tegra-apbmisc.c b/drivers/soc/tegra/fuse/tegra-apbmisc.c
index 3bf5aba4caaa..73fad05d8f2c 100644
--- a/drivers/soc/tegra/fuse/tegra-apbmisc.c
+++ b/drivers/soc/tegra/fuse/tegra-apbmisc.c
@@ -28,8 +28,15 @@
 #define APBMISC_SIZE	0x64
 #define FUSE_SKU_INFO	0x10
 
+#define PMC_STRAPPING_OPT_A_RAM_CODE_SHIFT	4
+#define PMC_STRAPPING_OPT_A_RAM_CODE_MASK_LONG	\
+	(0xf << PMC_STRAPPING_OPT_A_RAM_CODE_SHIFT)
+#define PMC_STRAPPING_OPT_A_RAM_CODE_MASK_SHORT	\
+	(0x3 << PMC_STRAPPING_OPT_A_RAM_CODE_SHIFT)
+
 static void __iomem *apbmisc_base;
 static void __iomem *strapping_base;
+static bool long_ram_code;
 
 u32 tegra_read_chipid(void)
 {
@@ -54,6 +61,18 @@ u32 tegra_read_straps(void)
 		return 0;
 }
 
+u32 tegra_read_ram_code(void)
+{
+	u32 straps = tegra_read_straps();
+
+	if (long_ram_code)
+		straps &= PMC_STRAPPING_OPT_A_RAM_CODE_MASK_LONG;
+	else
+		straps &= PMC_STRAPPING_OPT_A_RAM_CODE_MASK_SHORT;
+
+	return straps >> PMC_STRAPPING_OPT_A_RAM_CODE_SHIFT;
+}
+
 static const struct of_device_id apbmisc_match[] __initconst = {
 	{ .compatible = "nvidia,tegra20-apbmisc", },
 	{},
@@ -112,4 +131,6 @@ void __init tegra_init_apbmisc(void)
 	strapping_base = of_iomap(np, 1);
 	if (!strapping_base)
 		pr_err("ioremap tegra strapping_base failed\n");
+
+	long_ram_code = of_property_read_bool(np, "nvidia,long-ram-code");
 }
diff --git a/drivers/soc/tegra/pmc.c b/drivers/soc/tegra/pmc.c
index c956395cf46f..75d0457a77b7 100644
--- a/drivers/soc/tegra/pmc.c
+++ b/drivers/soc/tegra/pmc.c
@@ -377,13 +377,10 @@ int tegra_pmc_cpu_remove_clamping(int cpuid)
 }
 #endif /* CONFIG_SMP */
 
-/**
- * tegra_pmc_restart() - reboot the system
- * @mode: which mode to reboot in
- * @cmd: reboot command
- */
-void tegra_pmc_restart(enum reboot_mode mode, const char *cmd)
+static int tegra_pmc_restart_notify(struct notifier_block *this,
+				    unsigned long action, void *data)
 {
+	const char *cmd = data;
 	u32 value;
 
 	value = tegra_pmc_readl(PMC_SCRATCH0);
@@ -405,8 +402,15 @@ void tegra_pmc_restart(enum reboot_mode mode, const char *cmd)
 	value = tegra_pmc_readl(0);
 	value |= 0x10;
 	tegra_pmc_writel(value, 0);
+
+	return NOTIFY_DONE;
 }
 
+static struct notifier_block tegra_pmc_restart_handler = {
+	.notifier_call = tegra_pmc_restart_notify,
+	.priority = 128,
+};
+
 static int powergate_show(struct seq_file *s, void *data)
 {
 	unsigned int i;
@@ -837,6 +841,13 @@ static int tegra_pmc_probe(struct platform_device *pdev)
 			return err;
 	}
 
+	err = register_restart_handler(&tegra_pmc_restart_handler);
+	if (err) {
+		dev_err(&pdev->dev, "unable to register restart handler, %d\n",
+			err);
+		return err;
+	}
+
 	return 0;
 }
 
@@ -1010,7 +1021,7 @@ static struct platform_driver tegra_pmc_driver = {
 	},
 	.probe = tegra_pmc_probe,
 };
-module_platform_driver(tegra_pmc_driver);
+builtin_platform_driver(tegra_pmc_driver);
 
 /*
  * Early initialization to allow access to registers in the very early boot
diff --git a/drivers/soc/versatile/soc-realview.c b/drivers/soc/versatile/soc-realview.c
index 1a07bf540fec..e642c4540dda 100644
--- a/drivers/soc/versatile/soc-realview.c
+++ b/drivers/soc/versatile/soc-realview.c
@@ -142,4 +142,4 @@ static struct platform_driver realview_soc_driver = {
 		.of_match_table = realview_soc_of_match,
 	},
 };
-module_platform_driver(realview_soc_driver);
+builtin_platform_driver(realview_soc_driver);
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 72b059081559..0cae1694014d 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -77,6 +77,7 @@ config SPI_ATMEL
 
 config SPI_BCM2835
 	tristate "BCM2835 SPI controller"
+	depends on GPIOLIB
 	depends on ARCH_BCM2835 || COMPILE_TEST
 	depends on GPIOLIB
 	help
@@ -221,7 +222,7 @@ config SPI_FALCON
 
 config SPI_GPIO
 	tristate "GPIO-based bitbanging SPI Master"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	select SPI_BITBANG
 	help
 	  This simple GPIO bitbanging SPI master uses the arch-neutral GPIO
@@ -327,7 +328,7 @@ config SPI_MESON_SPIFC
 
 config SPI_OC_TINY
 	tristate "OpenCores tiny SPI"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	select SPI_BITBANG
 	help
 	  This is the driver for OpenCores tiny SPI master controller.
@@ -394,16 +395,9 @@ config SPI_PPC4xx
 	help
 	  This selects a driver for the PPC4xx SPI Controller.
 
-config SPI_PXA2XX_PXADMA
-	bool "PXA2xx SSP legacy PXA DMA API support"
-	depends on SPI_PXA2XX && ARCH_PXA
-	help
-	  Enable PXA private legacy DMA API support. Note that this is
-	  deprecated in favor of generic DMA engine API.
-
 config SPI_PXA2XX_DMA
 	def_bool y
-	depends on SPI_PXA2XX && !SPI_PXA2XX_PXADMA
+	depends on SPI_PXA2XX
 
 config SPI_PXA2XX
 	tristate "PXA2xx SSP SPI master"
@@ -429,6 +423,12 @@ config SPI_ROCKCHIP
 	  The main usecase of this controller is to use spi flash as boot
 	  device.
 
+config SPI_RB4XX
+	tristate "Mikrotik RB4XX SPI master"
+	depends on SPI_MASTER && ATH79
+	help
+	  SPI controller driver for the Mikrotik RB4xx series boards.
+
 config SPI_RSPI
 	tristate "Renesas RSPI/QSPI controller"
 	depends on SUPERH || ARCH_SHMOBILE || COMPILE_TEST
@@ -610,6 +610,12 @@ config SPI_XTENSA_XTFPGA
 	  16 bit words in SPI mode 0, automatically asserting CS on transfer
 	  start and deasserting on end.
 
+config SPI_ZYNQMP_GQSPI
+	tristate "Xilinx ZynqMP GQSPI controller"
+	depends on SPI_MASTER
+	help
+	  Enables Xilinx GQSPI controller driver for Zynq UltraScale+ MPSoC.
+
 config SPI_NUC900
 	tristate "Nuvoton NUC900 series SPI"
 	depends on ARCH_W90X900
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index d8cbf654976b..1154dbac8f2c 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -60,12 +60,12 @@ obj-$(CONFIG_SPI_ORION)			+= spi-orion.o
 obj-$(CONFIG_SPI_PL022)			+= spi-pl022.o
 obj-$(CONFIG_SPI_PPC4xx)		+= spi-ppc4xx.o
 spi-pxa2xx-platform-objs		:= spi-pxa2xx.o
-spi-pxa2xx-platform-$(CONFIG_SPI_PXA2XX_PXADMA)	+= spi-pxa2xx-pxadma.o
 spi-pxa2xx-platform-$(CONFIG_SPI_PXA2XX_DMA)	+= spi-pxa2xx-dma.o
 obj-$(CONFIG_SPI_PXA2XX)		+= spi-pxa2xx-platform.o
 obj-$(CONFIG_SPI_PXA2XX_PCI)		+= spi-pxa2xx-pci.o
 obj-$(CONFIG_SPI_QUP)			+= spi-qup.o
 obj-$(CONFIG_SPI_ROCKCHIP)		+= spi-rockchip.o
+obj-$(CONFIG_SPI_RB4XX)			+= spi-rb4xx.o
 obj-$(CONFIG_SPI_RSPI)			+= spi-rspi.o
 obj-$(CONFIG_SPI_S3C24XX)		+= spi-s3c24xx-hw.o
 spi-s3c24xx-hw-y			:= spi-s3c24xx.o
@@ -89,3 +89,4 @@ obj-$(CONFIG_SPI_TXX9)			+= spi-txx9.o
 obj-$(CONFIG_SPI_XCOMM)		+= spi-xcomm.o
 obj-$(CONFIG_SPI_XILINX)		+= spi-xilinx.o
 obj-$(CONFIG_SPI_XTENSA_XTFPGA)		+= spi-xtensa-xtfpga.o
+obj-$(CONFIG_SPI_ZYNQMP_GQSPI)		+= spi-zynqmp-gqspi.o
diff --git a/drivers/spi/spi-ath79.c b/drivers/spi/spi-ath79.c
index b02eb4ac0218..bf1f9b32c597 100644
--- a/drivers/spi/spi-ath79.c
+++ b/drivers/spi/spi-ath79.c
@@ -79,10 +79,8 @@ static void ath79_spi_chipselect(struct spi_device *spi, int is_active)
 	}
 
 	if (spi->chip_select) {
-		struct ath79_spi_controller_data *cdata = spi->controller_data;
-
 		/* SPI is normally active-low */
-		gpio_set_value(cdata->gpio, cs_high);
+		gpio_set_value(spi->cs_gpio, cs_high);
 	} else {
 		if (cs_high)
 			sp->ioc_base |= AR71XX_SPI_IOC_CS0;
@@ -117,11 +115,10 @@ static void ath79_spi_disable(struct ath79_spi *sp)
 
 static int ath79_spi_setup_cs(struct spi_device *spi)
 {
-	struct ath79_spi_controller_data *cdata;
+	struct ath79_spi *sp = ath79_spidev_to_sp(spi);
 	int status;
 
-	cdata = spi->controller_data;
-	if (spi->chip_select && !cdata)
+	if (spi->chip_select && !gpio_is_valid(spi->cs_gpio))
 		return -EINVAL;
 
 	status = 0;
@@ -134,8 +131,15 @@ static int ath79_spi_setup_cs(struct spi_device *spi)
 		else
 			flags |= GPIOF_INIT_HIGH;
 
-		status = gpio_request_one(cdata->gpio, flags,
+		status = gpio_request_one(spi->cs_gpio, flags,
 					  dev_name(&spi->dev));
+	} else {
+		if (spi->mode & SPI_CS_HIGH)
+			sp->ioc_base &= ~AR71XX_SPI_IOC_CS0;
+		else
+			sp->ioc_base |= AR71XX_SPI_IOC_CS0;
+
+		ath79_spi_wr(sp, AR71XX_SPI_REG_IOC, sp->ioc_base);
 	}
 
 	return status;
@@ -144,8 +148,7 @@ static int ath79_spi_setup_cs(struct spi_device *spi)
 static void ath79_spi_cleanup_cs(struct spi_device *spi)
 {
 	if (spi->chip_select) {
-		struct ath79_spi_controller_data *cdata = spi->controller_data;
-		gpio_free(cdata->gpio);
+		gpio_free(spi->cs_gpio);
 	}
 }
 
@@ -217,6 +220,7 @@ static int ath79_spi_probe(struct platform_device *pdev)
 	}
 
 	sp = spi_master_get_devdata(master);
+	master->dev.of_node = pdev->dev.of_node;
 	platform_set_drvdata(pdev, sp);
 
 	pdata = dev_get_platdata(&pdev->dev);
@@ -253,7 +257,7 @@ static int ath79_spi_probe(struct platform_device *pdev)
 		goto err_put_master;
 	}
 
-	ret = clk_enable(sp->clk);
+	ret = clk_prepare_enable(sp->clk);
 	if (ret)
 		goto err_put_master;
 
@@ -277,7 +281,7 @@ static int ath79_spi_probe(struct platform_device *pdev)
 err_disable:
 	ath79_spi_disable(sp);
 err_clk_disable:
-	clk_disable(sp->clk);
+	clk_disable_unprepare(sp->clk);
 err_put_master:
 	spi_master_put(sp->bitbang.master);
 
@@ -290,7 +294,7 @@ static int ath79_spi_remove(struct platform_device *pdev)
 
 	spi_bitbang_stop(&sp->bitbang);
 	ath79_spi_disable(sp);
-	clk_disable(sp->clk);
+	clk_disable_unprepare(sp->clk);
 	spi_master_put(sp->bitbang.master);
 
 	return 0;
@@ -301,12 +305,18 @@ static void ath79_spi_shutdown(struct platform_device *pdev)
 	ath79_spi_remove(pdev);
 }
 
+static const struct of_device_id ath79_spi_of_match[] = {
+	{ .compatible = "qca,ar7100-spi", },
+	{ },
+};
+
 static struct platform_driver ath79_spi_driver = {
 	.probe		= ath79_spi_probe,
 	.remove		= ath79_spi_remove,
 	.shutdown	= ath79_spi_shutdown,
 	.driver		= {
 		.name	= DRV_NAME,
+		.of_match_table = ath79_spi_of_match,
 	},
 };
 module_platform_driver(ath79_spi_driver);
diff --git a/drivers/spi/spi-atmel.c b/drivers/spi/spi-atmel.c
index a2f40b1b2225..c9eca347787d 100644
--- a/drivers/spi/spi-atmel.c
+++ b/drivers/spi/spi-atmel.c
@@ -41,6 +41,8 @@
 #define SPI_CSR1				0x0034
 #define SPI_CSR2				0x0038
 #define SPI_CSR3				0x003c
+#define SPI_FMR					0x0040
+#define SPI_FLR					0x0044
 #define SPI_VERSION				0x00fc
 #define SPI_RPR					0x0100
 #define SPI_RCR					0x0104
@@ -62,6 +64,14 @@
 #define SPI_SWRST_SIZE				1
 #define SPI_LASTXFER_OFFSET			24
 #define SPI_LASTXFER_SIZE			1
+#define SPI_TXFCLR_OFFSET			16
+#define SPI_TXFCLR_SIZE				1
+#define SPI_RXFCLR_OFFSET			17
+#define SPI_RXFCLR_SIZE				1
+#define SPI_FIFOEN_OFFSET			30
+#define SPI_FIFOEN_SIZE				1
+#define SPI_FIFODIS_OFFSET			31
+#define SPI_FIFODIS_SIZE			1
 
 /* Bitfields in MR */
 #define SPI_MSTR_OFFSET				0
@@ -114,6 +124,22 @@
 #define SPI_TXEMPTY_SIZE			1
 #define SPI_SPIENS_OFFSET			16
 #define SPI_SPIENS_SIZE				1
+#define SPI_TXFEF_OFFSET			24
+#define SPI_TXFEF_SIZE				1
+#define SPI_TXFFF_OFFSET			25
+#define SPI_TXFFF_SIZE				1
+#define SPI_TXFTHF_OFFSET			26
+#define SPI_TXFTHF_SIZE				1
+#define SPI_RXFEF_OFFSET			27
+#define SPI_RXFEF_SIZE				1
+#define SPI_RXFFF_OFFSET			28
+#define SPI_RXFFF_SIZE				1
+#define SPI_RXFTHF_OFFSET			29
+#define SPI_RXFTHF_SIZE				1
+#define SPI_TXFPTEF_OFFSET			30
+#define SPI_TXFPTEF_SIZE			1
+#define SPI_RXFPTEF_OFFSET			31
+#define SPI_RXFPTEF_SIZE			1
 
 /* Bitfields in CSR0 */
 #define SPI_CPOL_OFFSET				0
@@ -157,6 +183,22 @@
 #define SPI_TXTDIS_OFFSET			9
 #define SPI_TXTDIS_SIZE				1
 
+/* Bitfields in FMR */
+#define SPI_TXRDYM_OFFSET			0
+#define SPI_TXRDYM_SIZE				2
+#define SPI_RXRDYM_OFFSET			4
+#define SPI_RXRDYM_SIZE				2
+#define SPI_TXFTHRES_OFFSET			16
+#define SPI_TXFTHRES_SIZE			6
+#define SPI_RXFTHRES_OFFSET			24
+#define SPI_RXFTHRES_SIZE			6
+
+/* Bitfields in FLR */
+#define SPI_TXFL_OFFSET				0
+#define SPI_TXFL_SIZE				6
+#define SPI_RXFL_OFFSET				16
+#define SPI_RXFL_SIZE				6
+
 /* Constants for BITS */
 #define SPI_BITS_8_BPT				0
 #define SPI_BITS_9_BPT				1
@@ -167,6 +209,9 @@
 #define SPI_BITS_14_BPT				6
 #define SPI_BITS_15_BPT				7
 #define SPI_BITS_16_BPT				8
+#define SPI_ONE_DATA				0
+#define SPI_TWO_DATA				1
+#define SPI_FOUR_DATA				2
 
 /* Bit manipulation macros */
 #define SPI_BIT(name) \
@@ -185,11 +230,31 @@
 	__raw_readl((port)->regs + SPI_##reg)
 #define spi_writel(port, reg, value) \
 	__raw_writel((value), (port)->regs + SPI_##reg)
+
+#define spi_readw(port, reg) \
+	__raw_readw((port)->regs + SPI_##reg)
+#define spi_writew(port, reg, value) \
+	__raw_writew((value), (port)->regs + SPI_##reg)
+
+#define spi_readb(port, reg) \
+	__raw_readb((port)->regs + SPI_##reg)
+#define spi_writeb(port, reg, value) \
+	__raw_writeb((value), (port)->regs + SPI_##reg)
 #else
 #define spi_readl(port, reg) \
 	readl_relaxed((port)->regs + SPI_##reg)
 #define spi_writel(port, reg, value) \
 	writel_relaxed((value), (port)->regs + SPI_##reg)
+
+#define spi_readw(port, reg) \
+	readw_relaxed((port)->regs + SPI_##reg)
+#define spi_writew(port, reg, value) \
+	writew_relaxed((value), (port)->regs + SPI_##reg)
+
+#define spi_readb(port, reg) \
+	readb_relaxed((port)->regs + SPI_##reg)
+#define spi_writeb(port, reg, value) \
+	writeb_relaxed((value), (port)->regs + SPI_##reg)
 #endif
 /* use PIO for small transfers, avoiding DMA setup/teardown overhead and
  * cache operations; better heuristics consider wordsize and bitrate.
@@ -246,11 +311,14 @@ struct atmel_spi {
 
 	bool			use_dma;
 	bool			use_pdc;
+	bool			use_cs_gpios;
 	/* dmaengine data */
 	struct atmel_spi_dma	dma;
 
 	bool			keep_cs;
 	bool			cs_active;
+
+	u32			fifo_size;
 };
 
 /* Controller-specific per-slave state */
@@ -321,7 +389,8 @@ static void cs_activate(struct atmel_spi *as, struct spi_device *spi)
 		}
 
 		mr = spi_readl(as, MR);
-		gpio_set_value(asd->npcs_pin, active);
+		if (as->use_cs_gpios)
+			gpio_set_value(asd->npcs_pin, active);
 	} else {
 		u32 cpol = (spi->mode & SPI_CPOL) ? SPI_BIT(CPOL) : 0;
 		int i;
@@ -337,7 +406,7 @@ static void cs_activate(struct atmel_spi *as, struct spi_device *spi)
 
 		mr = spi_readl(as, MR);
 		mr = SPI_BFINS(PCS, ~(1 << spi->chip_select), mr);
-		if (spi->chip_select != 0)
+		if (as->use_cs_gpios && spi->chip_select != 0)
 			gpio_set_value(asd->npcs_pin, active);
 		spi_writel(as, MR, mr);
 	}
@@ -366,7 +435,9 @@ static void cs_deactivate(struct atmel_spi *as, struct spi_device *spi)
 			asd->npcs_pin, active ? " (low)" : "",
 			mr);
 
-	if (atmel_spi_is_v2(as) || spi->chip_select != 0)
+	if (!as->use_cs_gpios)
+		spi_writel(as, CR, SPI_BIT(LASTXFER));
+	else if (atmel_spi_is_v2(as) || spi->chip_select != 0)
 		gpio_set_value(asd->npcs_pin, !active);
 }
 
@@ -406,6 +477,20 @@ static int atmel_spi_dma_slave_config(struct atmel_spi *as,
 	slave_config->dst_maxburst = 1;
 	slave_config->device_fc = false;
 
+	/*
+	 * This driver uses fixed peripheral select mode (PS bit set to '0' in
+	 * the Mode Register).
+	 * So according to the datasheet, when FIFOs are available (and
+	 * enabled), the Transmit FIFO operates in Multiple Data Mode.
+	 * In this mode, up to 2 data, not 4, can be written into the Transmit
+	 * Data Register in a single access.
+	 * However, the first data has to be written into the lowest 16 bits and
+	 * the second data into the highest 16 bits of the Transmit
+	 * Data Register. For 8bit data (the most frequent case), it would
+	 * require to rework tx_buf so each data would actualy fit 16 bits.
+	 * So we'd rather write only one data at the time. Hence the transmit
+	 * path works the same whether FIFOs are available (and enabled) or not.
+	 */
 	slave_config->direction = DMA_MEM_TO_DEV;
 	if (dmaengine_slave_config(as->dma.chan_tx, slave_config)) {
 		dev_err(&as->pdev->dev,
@@ -413,6 +498,14 @@ static int atmel_spi_dma_slave_config(struct atmel_spi *as,
 		err = -EINVAL;
 	}
 
+	/*
+	 * This driver configures the spi controller for master mode (MSTR bit
+	 * set to '1' in the Mode Register).
+	 * So according to the datasheet, when FIFOs are available (and
+	 * enabled), the Receive FIFO operates in Single Data Mode.
+	 * So the receive path works the same whether FIFOs are available (and
+	 * enabled) or not.
+	 */
 	slave_config->direction = DMA_DEV_TO_MEM;
 	if (dmaengine_slave_config(as->dma.chan_rx, slave_config)) {
 		dev_err(&as->pdev->dev,
@@ -502,10 +595,10 @@ static void dma_callback(void *data)
 }
 
 /*
- * Next transfer using PIO.
+ * Next transfer using PIO without FIFO.
  */
-static void atmel_spi_next_xfer_pio(struct spi_master *master,
-				struct spi_transfer *xfer)
+static void atmel_spi_next_xfer_single(struct spi_master *master,
+				       struct spi_transfer *xfer)
 {
 	struct atmel_spi	*as = spi_master_get_devdata(master);
 	unsigned long xfer_pos = xfer->len - as->current_remaining_bytes;
@@ -538,6 +631,99 @@ static void atmel_spi_next_xfer_pio(struct spi_master *master,
 }
 
 /*
+ * Next transfer using PIO with FIFO.
+ */
+static void atmel_spi_next_xfer_fifo(struct spi_master *master,
+				     struct spi_transfer *xfer)
+{
+	struct atmel_spi *as = spi_master_get_devdata(master);
+	u32 current_remaining_data, num_data;
+	u32 offset = xfer->len - as->current_remaining_bytes;
+	const u16 *words = (const u16 *)((u8 *)xfer->tx_buf + offset);
+	const u8  *bytes = (const u8  *)((u8 *)xfer->tx_buf + offset);
+	u16 td0, td1;
+	u32 fifomr;
+
+	dev_vdbg(master->dev.parent, "atmel_spi_next_xfer_fifo\n");
+
+	/* Compute the number of data to transfer in the current iteration */
+	current_remaining_data = ((xfer->bits_per_word > 8) ?
+				  ((u32)as->current_remaining_bytes >> 1) :
+				  (u32)as->current_remaining_bytes);
+	num_data = min(current_remaining_data, as->fifo_size);
+
+	/* Flush RX and TX FIFOs */
+	spi_writel(as, CR, SPI_BIT(RXFCLR) | SPI_BIT(TXFCLR));
+	while (spi_readl(as, FLR))
+		cpu_relax();
+
+	/* Set RX FIFO Threshold to the number of data to transfer */
+	fifomr = spi_readl(as, FMR);
+	spi_writel(as, FMR, SPI_BFINS(RXFTHRES, num_data, fifomr));
+
+	/* Clear FIFO flags in the Status Register, especially RXFTHF */
+	(void)spi_readl(as, SR);
+
+	/* Fill TX FIFO */
+	while (num_data >= 2) {
+		if (xfer->tx_buf) {
+			if (xfer->bits_per_word > 8) {
+				td0 = *words++;
+				td1 = *words++;
+			} else {
+				td0 = *bytes++;
+				td1 = *bytes++;
+			}
+		} else {
+			td0 = 0;
+			td1 = 0;
+		}
+
+		spi_writel(as, TDR, (td1 << 16) | td0);
+		num_data -= 2;
+	}
+
+	if (num_data) {
+		if (xfer->tx_buf) {
+			if (xfer->bits_per_word > 8)
+				td0 = *words++;
+			else
+				td0 = *bytes++;
+		} else {
+			td0 = 0;
+		}
+
+		spi_writew(as, TDR, td0);
+		num_data--;
+	}
+
+	dev_dbg(master->dev.parent,
+		"  start fifo xfer %p: len %u tx %p rx %p bitpw %d\n",
+		xfer, xfer->len, xfer->tx_buf, xfer->rx_buf,
+		xfer->bits_per_word);
+
+	/*
+	 * Enable RX FIFO Threshold Flag interrupt to be notified about
+	 * transfer completion.
+	 */
+	spi_writel(as, IER, SPI_BIT(RXFTHF) | SPI_BIT(OVRES));
+}
+
+/*
+ * Next transfer using PIO.
+ */
+static void atmel_spi_next_xfer_pio(struct spi_master *master,
+				    struct spi_transfer *xfer)
+{
+	struct atmel_spi *as = spi_master_get_devdata(master);
+
+	if (as->fifo_size)
+		atmel_spi_next_xfer_fifo(master, xfer);
+	else
+		atmel_spi_next_xfer_single(master, xfer);
+}
+
+/*
  * Submit next transfer for DMA.
  */
 static int atmel_spi_next_xfer_dma_submit(struct spi_master *master,
@@ -839,13 +1025,8 @@ static void atmel_spi_disable_pdc_transfer(struct atmel_spi *as)
 	spi_writel(as, PTCR, SPI_BIT(RXTDIS) | SPI_BIT(TXTDIS));
 }
 
-/* Called from IRQ
- *
- * Must update "current_remaining_bytes" to keep track of data
- * to transfer.
- */
 static void
-atmel_spi_pump_pio_data(struct atmel_spi *as, struct spi_transfer *xfer)
+atmel_spi_pump_single_data(struct atmel_spi *as, struct spi_transfer *xfer)
 {
 	u8		*rxp;
 	u16		*rxp16;
@@ -872,6 +1053,57 @@ atmel_spi_pump_pio_data(struct atmel_spi *as, struct spi_transfer *xfer)
 	}
 }
 
+static void
+atmel_spi_pump_fifo_data(struct atmel_spi *as, struct spi_transfer *xfer)
+{
+	u32 fifolr = spi_readl(as, FLR);
+	u32 num_bytes, num_data = SPI_BFEXT(RXFL, fifolr);
+	u32 offset = xfer->len - as->current_remaining_bytes;
+	u16 *words = (u16 *)((u8 *)xfer->rx_buf + offset);
+	u8  *bytes = (u8  *)((u8 *)xfer->rx_buf + offset);
+	u16 rd; /* RD field is the lowest 16 bits of RDR */
+
+	/* Update the number of remaining bytes to transfer */
+	num_bytes = ((xfer->bits_per_word > 8) ?
+		     (num_data << 1) :
+		     num_data);
+
+	if (as->current_remaining_bytes > num_bytes)
+		as->current_remaining_bytes -= num_bytes;
+	else
+		as->current_remaining_bytes = 0;
+
+	/* Handle odd number of bytes when data are more than 8bit width */
+	if (xfer->bits_per_word > 8)
+		as->current_remaining_bytes &= ~0x1;
+
+	/* Read data */
+	while (num_data) {
+		rd = spi_readl(as, RDR);
+		if (xfer->rx_buf) {
+			if (xfer->bits_per_word > 8)
+				*words++ = rd;
+			else
+				*bytes++ = rd;
+		}
+		num_data--;
+	}
+}
+
+/* Called from IRQ
+ *
+ * Must update "current_remaining_bytes" to keep track of data
+ * to transfer.
+ */
+static void
+atmel_spi_pump_pio_data(struct atmel_spi *as, struct spi_transfer *xfer)
+{
+	if (as->fifo_size)
+		atmel_spi_pump_fifo_data(as, xfer);
+	else
+		atmel_spi_pump_single_data(as, xfer);
+}
+
 /* Interrupt
  *
  * No need for locking in this Interrupt handler: done_status is the
@@ -912,7 +1144,7 @@ atmel_spi_pio_interrupt(int irq, void *dev_id)
 
 		complete(&as->xfer_completion);
 
-	} else if (pending & SPI_BIT(RDRF)) {
+	} else if (pending & (SPI_BIT(RDRF) | SPI_BIT(RXFTHF))) {
 		atmel_spi_lock(as);
 
 		if (as->current_remaining_bytes) {
@@ -996,6 +1228,8 @@ static int atmel_spi_setup(struct spi_device *spi)
 		csr |= SPI_BIT(CPOL);
 	if (!(spi->mode & SPI_CPHA))
 		csr |= SPI_BIT(NCPHA);
+	if (!as->use_cs_gpios)
+		csr |= SPI_BIT(CSAAT);
 
 	/* DLYBS is mostly irrelevant since we manage chipselect using GPIOs.
 	 *
@@ -1009,7 +1243,9 @@ static int atmel_spi_setup(struct spi_device *spi)
 	/* chipselect must have been muxed as GPIO (e.g. in board setup) */
 	npcs_pin = (unsigned long)spi->controller_data;
 
-	if (gpio_is_valid(spi->cs_gpio))
+	if (!as->use_cs_gpios)
+		npcs_pin = spi->chip_select;
+	else if (gpio_is_valid(spi->cs_gpio))
 		npcs_pin = spi->cs_gpio;
 
 	asd = spi->controller_state;
@@ -1018,15 +1254,19 @@ static int atmel_spi_setup(struct spi_device *spi)
 		if (!asd)
 			return -ENOMEM;
 
-		ret = gpio_request(npcs_pin, dev_name(&spi->dev));
-		if (ret) {
-			kfree(asd);
-			return ret;
+		if (as->use_cs_gpios) {
+			ret = gpio_request(npcs_pin, dev_name(&spi->dev));
+			if (ret) {
+				kfree(asd);
+				return ret;
+			}
+
+			gpio_direction_output(npcs_pin,
+					      !(spi->mode & SPI_CS_HIGH));
 		}
 
 		asd->npcs_pin = npcs_pin;
 		spi->controller_state = asd;
-		gpio_direction_output(npcs_pin, !(spi->mode & SPI_CS_HIGH));
 	}
 
 	asd->csr = csr;
@@ -1338,6 +1578,13 @@ static int atmel_spi_probe(struct platform_device *pdev)
 
 	atmel_get_caps(as);
 
+	as->use_cs_gpios = true;
+	if (atmel_spi_is_v2(as) &&
+	    !of_get_property(pdev->dev.of_node, "cs-gpios", NULL)) {
+		as->use_cs_gpios = false;
+		master->num_chipselect = 4;
+	}
+
 	as->use_dma = false;
 	as->use_pdc = false;
 	if (as->caps.has_dma_support) {
@@ -1380,6 +1627,13 @@ static int atmel_spi_probe(struct platform_device *pdev)
 		spi_writel(as, PTCR, SPI_BIT(RXTDIS) | SPI_BIT(TXTDIS));
 	spi_writel(as, CR, SPI_BIT(SPIEN));
 
+	as->fifo_size = 0;
+	if (!of_property_read_u32(pdev->dev.of_node, "atmel,fifo-size",
+				  &as->fifo_size)) {
+		dev_info(&pdev->dev, "Using FIFO (%u data)\n", as->fifo_size);
+		spi_writel(as, CR, SPI_BIT(FIFOEN));
+	}
+
 	/* go! */
 	dev_info(&pdev->dev, "Atmel SPI Controller at 0x%08lx (irq %d)\n",
 			(unsigned long)regs->start, irq);
diff --git a/drivers/spi/spi-bcm2835.c b/drivers/spi/spi-bcm2835.c
index 37875cf942f7..59705ab23577 100644
--- a/drivers/spi/spi-bcm2835.c
+++ b/drivers/spi/spi-bcm2835.c
@@ -20,18 +20,22 @@
  * GNU General Public License for more details.
  */
 
+#include <asm/page.h>
 #include <linux/clk.h>
 #include <linux/completion.h>
 #include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
 #include <linux/err.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/of.h>
-#include <linux/of_irq.h>
-#include <linux/of_gpio.h>
+#include <linux/of_address.h>
 #include <linux/of_device.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
 #include <linux/spi/spi.h>
 
 /* SPI register offsets */
@@ -69,7 +73,8 @@
 #define BCM2835_SPI_CS_CS_01		0x00000001
 
 #define BCM2835_SPI_POLLING_LIMIT_US	30
-#define BCM2835_SPI_TIMEOUT_MS		30000
+#define BCM2835_SPI_POLLING_JIFFIES	2
+#define BCM2835_SPI_DMA_MIN_LENGTH	96
 #define BCM2835_SPI_MODE_BITS	(SPI_CPOL | SPI_CPHA | SPI_CS_HIGH \
 				| SPI_NO_CS | SPI_3WIRE)
 
@@ -83,6 +88,7 @@ struct bcm2835_spi {
 	u8 *rx_buf;
 	int tx_len;
 	int rx_len;
+	bool dma_pending;
 };
 
 static inline u32 bcm2835_rd(struct bcm2835_spi *bs, unsigned reg)
@@ -128,12 +134,15 @@ static void bcm2835_spi_reset_hw(struct spi_master *master)
 	/* Disable SPI interrupts and transfer */
 	cs &= ~(BCM2835_SPI_CS_INTR |
 		BCM2835_SPI_CS_INTD |
+		BCM2835_SPI_CS_DMAEN |
 		BCM2835_SPI_CS_TA);
 	/* and reset RX/TX FIFOS */
 	cs |= BCM2835_SPI_CS_CLEAR_RX | BCM2835_SPI_CS_CLEAR_TX;
 
 	/* and reset the SPI_HW */
 	bcm2835_wr(bs, BCM2835_SPI_CS, cs);
+	/* as well as DLEN */
+	bcm2835_wr(bs, BCM2835_SPI_DLEN, 0);
 }
 
 static irqreturn_t bcm2835_spi_interrupt(int irq, void *dev_id)
@@ -157,42 +166,6 @@ static irqreturn_t bcm2835_spi_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int bcm2835_spi_transfer_one_poll(struct spi_master *master,
-					 struct spi_device *spi,
-					 struct spi_transfer *tfr,
-					 u32 cs,
-					 unsigned long xfer_time_us)
-{
-	struct bcm2835_spi *bs = spi_master_get_devdata(master);
-	/* set timeout to 1 second of maximum polling */
-	unsigned long timeout = jiffies + HZ;
-
-	/* enable HW block without interrupts */
-	bcm2835_wr(bs, BCM2835_SPI_CS, cs | BCM2835_SPI_CS_TA);
-
-	/* loop until finished the transfer */
-	while (bs->rx_len) {
-		/* read from fifo as much as possible */
-		bcm2835_rd_fifo(bs);
-		/* fill in tx fifo as much as possible */
-		bcm2835_wr_fifo(bs);
-		/* if we still expect some data after the read,
-		 * check for a possible timeout
-		 */
-		if (bs->rx_len && time_after(jiffies, timeout)) {
-			/* Transfer complete - reset SPI HW */
-			bcm2835_spi_reset_hw(master);
-			/* and return timeout */
-			return -ETIMEDOUT;
-		}
-	}
-
-	/* Transfer complete - reset SPI HW */
-	bcm2835_spi_reset_hw(master);
-	/* and return without waiting for completion */
-	return 0;
-}
-
 static int bcm2835_spi_transfer_one_irq(struct spi_master *master,
 					struct spi_device *spi,
 					struct spi_transfer *tfr,
@@ -229,6 +202,329 @@ static int bcm2835_spi_transfer_one_irq(struct spi_master *master,
 	return 1;
 }
 
+/*
+ * DMA support
+ *
+ * this implementation has currently a few issues in so far as it does
+ * not work arrount limitations of the HW.
+ *
+ * the main one being that DMA transfers are limited to 16 bit
+ * (so 0 to 65535 bytes) by the SPI HW due to BCM2835_SPI_DLEN
+ *
+ * also we currently assume that the scatter-gather fragments are
+ * all multiple of 4 (except the last) - otherwise we would need
+ * to reset the FIFO before subsequent transfers...
+ * this also means that tx/rx transfers sg's need to be of equal size!
+ *
+ * there may be a few more border-cases we may need to address as well
+ * but unfortunately this would mean splitting up the scatter-gather
+ * list making it slightly unpractical...
+ */
+static void bcm2835_spi_dma_done(void *data)
+{
+	struct spi_master *master = data;
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+
+	/* reset fifo and HW */
+	bcm2835_spi_reset_hw(master);
+
+	/* and terminate tx-dma as we do not have an irq for it
+	 * because when the rx dma will terminate and this callback
+	 * is called the tx-dma must have finished - can't get to this
+	 * situation otherwise...
+	 */
+	dmaengine_terminate_all(master->dma_tx);
+
+	/* mark as no longer pending */
+	bs->dma_pending = 0;
+
+	/* and mark as completed */;
+	complete(&master->xfer_completion);
+}
+
+static int bcm2835_spi_prepare_sg(struct spi_master *master,
+				  struct spi_transfer *tfr,
+				  bool is_tx)
+{
+	struct dma_chan *chan;
+	struct scatterlist *sgl;
+	unsigned int nents;
+	enum dma_transfer_direction dir;
+	unsigned long flags;
+
+	struct dma_async_tx_descriptor *desc;
+	dma_cookie_t cookie;
+
+	if (is_tx) {
+		dir   = DMA_MEM_TO_DEV;
+		chan  = master->dma_tx;
+		nents = tfr->tx_sg.nents;
+		sgl   = tfr->tx_sg.sgl;
+		flags = 0 /* no  tx interrupt */;
+
+	} else {
+		dir   = DMA_DEV_TO_MEM;
+		chan  = master->dma_rx;
+		nents = tfr->rx_sg.nents;
+		sgl   = tfr->rx_sg.sgl;
+		flags = DMA_PREP_INTERRUPT;
+	}
+	/* prepare the channel */
+	desc = dmaengine_prep_slave_sg(chan, sgl, nents, dir, flags);
+	if (!desc)
+		return -EINVAL;
+
+	/* set callback for rx */
+	if (!is_tx) {
+		desc->callback = bcm2835_spi_dma_done;
+		desc->callback_param = master;
+	}
+
+	/* submit it to DMA-engine */
+	cookie = dmaengine_submit(desc);
+
+	return dma_submit_error(cookie);
+}
+
+static inline int bcm2835_check_sg_length(struct sg_table *sgt)
+{
+	int i;
+	struct scatterlist *sgl;
+
+	/* check that the sg entries are word-sized (except for last) */
+	for_each_sg(sgt->sgl, sgl, (int)sgt->nents - 1, i) {
+		if (sg_dma_len(sgl) % 4)
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int bcm2835_spi_transfer_one_dma(struct spi_master *master,
+					struct spi_device *spi,
+					struct spi_transfer *tfr,
+					u32 cs)
+{
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+	int ret;
+
+	/* check that the scatter gather segments are all a multiple of 4 */
+	if (bcm2835_check_sg_length(&tfr->tx_sg) ||
+	    bcm2835_check_sg_length(&tfr->rx_sg)) {
+		dev_warn_once(&spi->dev,
+			      "scatter gather segment length is not a multiple of 4 - falling back to interrupt mode\n");
+		return bcm2835_spi_transfer_one_irq(master, spi, tfr, cs);
+	}
+
+	/* setup tx-DMA */
+	ret = bcm2835_spi_prepare_sg(master, tfr, true);
+	if (ret)
+		return ret;
+
+	/* start TX early */
+	dma_async_issue_pending(master->dma_tx);
+
+	/* mark as dma pending */
+	bs->dma_pending = 1;
+
+	/* set the DMA length */
+	bcm2835_wr(bs, BCM2835_SPI_DLEN, tfr->len);
+
+	/* start the HW */
+	bcm2835_wr(bs, BCM2835_SPI_CS,
+		   cs | BCM2835_SPI_CS_TA | BCM2835_SPI_CS_DMAEN);
+
+	/* setup rx-DMA late - to run transfers while
+	 * mapping of the rx buffers still takes place
+	 * this saves 10us or more.
+	 */
+	ret = bcm2835_spi_prepare_sg(master, tfr, false);
+	if (ret) {
+		/* need to reset on errors */
+		dmaengine_terminate_all(master->dma_tx);
+		bcm2835_spi_reset_hw(master);
+		return ret;
+	}
+
+	/* start rx dma late */
+	dma_async_issue_pending(master->dma_rx);
+
+	/* wait for wakeup in framework */
+	return 1;
+}
+
+static bool bcm2835_spi_can_dma(struct spi_master *master,
+				struct spi_device *spi,
+				struct spi_transfer *tfr)
+{
+	/* only run for gpio_cs */
+	if (!gpio_is_valid(spi->cs_gpio))
+		return false;
+
+	/* we start DMA efforts only on bigger transfers */
+	if (tfr->len < BCM2835_SPI_DMA_MIN_LENGTH)
+		return false;
+
+	/* BCM2835_SPI_DLEN has defined a max transfer size as
+	 * 16 bit, so max is 65535
+	 * we can revisit this by using an alternative transfer
+	 * method - ideally this would get done without any more
+	 * interaction...
+	 */
+	if (tfr->len > 65535) {
+		dev_warn_once(&spi->dev,
+			      "transfer size of %d too big for dma-transfer\n",
+			      tfr->len);
+		return false;
+	}
+
+	/* if we run rx/tx_buf with word aligned addresses then we are OK */
+	if ((((size_t)tfr->rx_buf & 3) == 0) &&
+	    (((size_t)tfr->tx_buf & 3) == 0))
+		return true;
+
+	/* otherwise we only allow transfers within the same page
+	 * to avoid wasting time on dma_mapping when it is not practical
+	 */
+	if (((size_t)tfr->tx_buf & PAGE_MASK) + tfr->len > PAGE_SIZE) {
+		dev_warn_once(&spi->dev,
+			      "Unaligned spi tx-transfer bridging page\n");
+		return false;
+	}
+	if (((size_t)tfr->rx_buf & PAGE_MASK) + tfr->len > PAGE_SIZE) {
+		dev_warn_once(&spi->dev,
+			      "Unaligned spi tx-transfer bridging page\n");
+		return false;
+	}
+
+	/* return OK */
+	return true;
+}
+
+static void bcm2835_dma_release(struct spi_master *master)
+{
+	if (master->dma_tx) {
+		dmaengine_terminate_all(master->dma_tx);
+		dma_release_channel(master->dma_tx);
+		master->dma_tx = NULL;
+	}
+	if (master->dma_rx) {
+		dmaengine_terminate_all(master->dma_rx);
+		dma_release_channel(master->dma_rx);
+		master->dma_rx = NULL;
+	}
+}
+
+static void bcm2835_dma_init(struct spi_master *master, struct device *dev)
+{
+	struct dma_slave_config slave_config;
+	const __be32 *addr;
+	dma_addr_t dma_reg_base;
+	int ret;
+
+	/* base address in dma-space */
+	addr = of_get_address(master->dev.of_node, 0, NULL, NULL);
+	if (!addr) {
+		dev_err(dev, "could not get DMA-register address - not using dma mode\n");
+		goto err;
+	}
+	dma_reg_base = be32_to_cpup(addr);
+
+	/* get tx/rx dma */
+	master->dma_tx = dma_request_slave_channel(dev, "tx");
+	if (!master->dma_tx) {
+		dev_err(dev, "no tx-dma configuration found - not using dma mode\n");
+		goto err;
+	}
+	master->dma_rx = dma_request_slave_channel(dev, "rx");
+	if (!master->dma_rx) {
+		dev_err(dev, "no rx-dma configuration found - not using dma mode\n");
+		goto err_release;
+	}
+
+	/* configure DMAs */
+	slave_config.direction = DMA_MEM_TO_DEV;
+	slave_config.dst_addr = (u32)(dma_reg_base + BCM2835_SPI_FIFO);
+	slave_config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+
+	ret = dmaengine_slave_config(master->dma_tx, &slave_config);
+	if (ret)
+		goto err_config;
+
+	slave_config.direction = DMA_DEV_TO_MEM;
+	slave_config.src_addr = (u32)(dma_reg_base + BCM2835_SPI_FIFO);
+	slave_config.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+
+	ret = dmaengine_slave_config(master->dma_rx, &slave_config);
+	if (ret)
+		goto err_config;
+
+	/* all went well, so set can_dma */
+	master->can_dma = bcm2835_spi_can_dma;
+	master->max_dma_len = 65535; /* limitation by BCM2835_SPI_DLEN */
+	/* need to do TX AND RX DMA, so we need dummy buffers */
+	master->flags = SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX;
+
+	return;
+
+err_config:
+	dev_err(dev, "issue configuring dma: %d - not using DMA mode\n",
+		ret);
+err_release:
+	bcm2835_dma_release(master);
+err:
+	return;
+}
+
+static int bcm2835_spi_transfer_one_poll(struct spi_master *master,
+					 struct spi_device *spi,
+					 struct spi_transfer *tfr,
+					 u32 cs,
+					 unsigned long xfer_time_us)
+{
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+	unsigned long timeout;
+
+	/* enable HW block without interrupts */
+	bcm2835_wr(bs, BCM2835_SPI_CS, cs | BCM2835_SPI_CS_TA);
+
+	/* fill in the fifo before timeout calculations
+	 * if we are interrupted here, then the data is
+	 * getting transferred by the HW while we are interrupted
+	 */
+	bcm2835_wr_fifo(bs);
+
+	/* set the timeout */
+	timeout = jiffies + BCM2835_SPI_POLLING_JIFFIES;
+
+	/* loop until finished the transfer */
+	while (bs->rx_len) {
+		/* fill in tx fifo with remaining data */
+		bcm2835_wr_fifo(bs);
+
+		/* read from fifo as much as possible */
+		bcm2835_rd_fifo(bs);
+
+		/* if there is still data pending to read
+		 * then check the timeout
+		 */
+		if (bs->rx_len && time_after(jiffies, timeout)) {
+			dev_dbg_ratelimited(&spi->dev,
+					    "timeout period reached: jiffies: %lu remaining tx/rx: %d/%d - falling back to interrupt mode\n",
+					    jiffies - timeout,
+					    bs->tx_len, bs->rx_len);
+			/* fall back to interrupt mode */
+			return bcm2835_spi_transfer_one_irq(master, spi,
+							    tfr, cs);
+		}
+	}
+
+	/* Transfer complete - reset SPI HW */
+	bcm2835_spi_reset_hw(master);
+	/* and return without waiting for completion */
+	return 0;
+}
+
 static int bcm2835_spi_transfer_one(struct spi_master *master,
 				    struct spi_device *spi,
 				    struct spi_transfer *tfr)
@@ -288,12 +584,26 @@ static int bcm2835_spi_transfer_one(struct spi_master *master,
 		return bcm2835_spi_transfer_one_poll(master, spi, tfr,
 						     cs, xfer_time_us);
 
+	/* run in dma mode if conditions are right */
+	if (master->can_dma && bcm2835_spi_can_dma(master, spi, tfr))
+		return bcm2835_spi_transfer_one_dma(master, spi, tfr, cs);
+
+	/* run in interrupt-mode */
 	return bcm2835_spi_transfer_one_irq(master, spi, tfr, cs);
 }
 
 static void bcm2835_spi_handle_err(struct spi_master *master,
 				   struct spi_message *msg)
 {
+	struct bcm2835_spi *bs = spi_master_get_devdata(master);
+
+	/* if an error occurred and we have an active dma, then terminate */
+	if (bs->dma_pending) {
+		dmaengine_terminate_all(master->dma_tx);
+		dmaengine_terminate_all(master->dma_rx);
+		bs->dma_pending = 0;
+	}
+	/* and reset */
 	bcm2835_spi_reset_hw(master);
 }
 
@@ -463,6 +773,8 @@ static int bcm2835_spi_probe(struct platform_device *pdev)
 		goto out_clk_disable;
 	}
 
+	bcm2835_dma_init(master, &pdev->dev);
+
 	/* initialise the hardware with the default polarities */
 	bcm2835_wr(bs, BCM2835_SPI_CS,
 		   BCM2835_SPI_CS_CLEAR_RX | BCM2835_SPI_CS_CLEAR_TX);
@@ -493,6 +805,8 @@ static int bcm2835_spi_remove(struct platform_device *pdev)
 
 	clk_disable_unprepare(bs->clk);
 
+	bcm2835_dma_release(master);
+
 	return 0;
 }
 
diff --git a/drivers/spi/spi-davinci.c b/drivers/spi/spi-davinci.c
index 5e991065f5b0..987afebea093 100644
--- a/drivers/spi/spi-davinci.c
+++ b/drivers/spi/spi-davinci.c
@@ -265,7 +265,7 @@ static inline int davinci_spi_get_prescale(struct davinci_spi *dspi,
 
 	ret = DIV_ROUND_UP(clk_get_rate(dspi->clk), max_speed_hz);
 
-	if (ret < 3 || ret > 256)
+	if (ret < 1 || ret > 256)
 		return -EINVAL;
 
 	return ret - 1;
diff --git a/drivers/spi/spi-fsl-dspi.c b/drivers/spi/spi-fsl-dspi.c
index 5fe54cda309f..86bcdd68c1fe 100644
--- a/drivers/spi/spi-fsl-dspi.c
+++ b/drivers/spi/spi-fsl-dspi.c
@@ -24,6 +24,7 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/pinctrl/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regmap.h>
@@ -47,6 +48,7 @@
 #define SPI_MCR_CLR_RXF	(1 << 10)
 
 #define SPI_TCR			0x08
+#define SPI_TCR_GET_TCNT(x)	(((x) & 0xffff0000) >> 16)
 
 #define SPI_CTAR(x)		(0x0c + (((x) & 0x3) * 4))
 #define SPI_CTAR_FMSZ(x)	(((x) & 0x0000000f) << 27)
@@ -67,9 +69,11 @@
 
 #define SPI_SR			0x2c
 #define SPI_SR_EOQF		0x10000000
+#define SPI_SR_TCFQF		0x80000000
 
 #define SPI_RSER		0x30
 #define SPI_RSER_EOQFE		0x10000000
+#define SPI_RSER_TCFQE		0x80000000
 
 #define SPI_PUSHR		0x34
 #define SPI_PUSHR_CONT		(1 << 31)
@@ -102,12 +106,35 @@
 #define SPI_CS_ASSERT		0x02
 #define SPI_CS_DROP		0x04
 
+#define SPI_TCR_TCNT_MAX	0x10000
+
 struct chip_data {
 	u32 mcr_val;
 	u32 ctar_val;
 	u16 void_write_data;
 };
 
+enum dspi_trans_mode {
+	DSPI_EOQ_MODE = 0,
+	DSPI_TCFQ_MODE,
+};
+
+struct fsl_dspi_devtype_data {
+	enum dspi_trans_mode trans_mode;
+};
+
+static const struct fsl_dspi_devtype_data vf610_data = {
+	.trans_mode = DSPI_EOQ_MODE,
+};
+
+static const struct fsl_dspi_devtype_data ls1021a_v1_data = {
+	.trans_mode = DSPI_TCFQ_MODE,
+};
+
+static const struct fsl_dspi_devtype_data ls2085a_data = {
+	.trans_mode = DSPI_TCFQ_MODE,
+};
+
 struct fsl_dspi {
 	struct spi_master	*master;
 	struct platform_device	*pdev;
@@ -128,9 +155,12 @@ struct fsl_dspi {
 	u8			cs;
 	u16			void_write_data;
 	u32			cs_change;
+	struct fsl_dspi_devtype_data *devtype_data;
 
 	wait_queue_head_t	waitq;
 	u32			waitflags;
+
+	u32			spi_tcnt;
 };
 
 static inline int is_double_byte_mode(struct fsl_dspi *dspi)
@@ -213,63 +243,60 @@ static void ns_delay_scale(char *psc, char *sc, int delay_ns,
 	}
 }
 
-static int dspi_transfer_write(struct fsl_dspi *dspi)
+static u32 dspi_data_to_pushr(struct fsl_dspi *dspi, int tx_word)
 {
-	int tx_count = 0;
-	int tx_word;
 	u16 d16;
-	u8  d8;
-	u32 dspi_pushr = 0;
-	int first = 1;
 
-	tx_word = is_double_byte_mode(dspi);
+	if (!(dspi->dataflags & TRAN_STATE_TX_VOID))
+		d16 = tx_word ? *(u16 *)dspi->tx : *(u8 *)dspi->tx;
+	else
+		d16 = dspi->void_write_data;
 
-	/* If we are in word mode, but only have a single byte to transfer
-	 * then switch to byte mode temporarily.  Will switch back at the
-	 * end of the transfer.
-	 */
-	if (tx_word && (dspi->len == 1)) {
-		dspi->dataflags |= TRAN_STATE_WORD_ODD_NUM;
-		regmap_update_bits(dspi->regmap, SPI_CTAR(dspi->cs),
-				SPI_FRAME_BITS_MASK, SPI_FRAME_BITS(8));
-		tx_word = 0;
-	}
+	dspi->tx += tx_word + 1;
+	dspi->len -= tx_word + 1;
 
-	while (dspi->len && (tx_count < DSPI_FIFO_SIZE)) {
-		if (tx_word) {
-			if (dspi->len == 1)
-				break;
+	return	SPI_PUSHR_TXDATA(d16) |
+		SPI_PUSHR_PCS(dspi->cs) |
+		SPI_PUSHR_CTAS(dspi->cs) |
+		SPI_PUSHR_CONT;
+}
 
-			if (!(dspi->dataflags & TRAN_STATE_TX_VOID)) {
-				d16 = *(u16 *)dspi->tx;
-				dspi->tx += 2;
-			} else {
-				d16 = dspi->void_write_data;
-			}
+static void dspi_data_from_popr(struct fsl_dspi *dspi, int rx_word)
+{
+	u16 d;
+	unsigned int val;
 
-			dspi_pushr = SPI_PUSHR_TXDATA(d16) |
-				SPI_PUSHR_PCS(dspi->cs) |
-				SPI_PUSHR_CTAS(dspi->cs) |
-				SPI_PUSHR_CONT;
+	regmap_read(dspi->regmap, SPI_POPR, &val);
+	d = SPI_POPR_RXDATA(val);
 
-			dspi->len -= 2;
-		} else {
-			if (!(dspi->dataflags & TRAN_STATE_TX_VOID)) {
+	if (!(dspi->dataflags & TRAN_STATE_RX_VOID))
+		rx_word ? (*(u16 *)dspi->rx = d) : (*(u8 *)dspi->rx = d);
 
-				d8 = *(u8 *)dspi->tx;
-				dspi->tx++;
-			} else {
-				d8 = (u8)dspi->void_write_data;
-			}
+	dspi->rx += rx_word + 1;
+}
 
-			dspi_pushr = SPI_PUSHR_TXDATA(d8) |
-				SPI_PUSHR_PCS(dspi->cs) |
-				SPI_PUSHR_CTAS(dspi->cs) |
-				SPI_PUSHR_CONT;
+static int dspi_eoq_write(struct fsl_dspi *dspi)
+{
+	int tx_count = 0;
+	int tx_word;
+	u32 dspi_pushr = 0;
+
+	tx_word = is_double_byte_mode(dspi);
 
-			dspi->len--;
+	while (dspi->len && (tx_count < DSPI_FIFO_SIZE)) {
+		/* If we are in word mode, only have a single byte to transfer
+		 * switch to byte mode temporarily.  Will switch back at the
+		 * end of the transfer.
+		 */
+		if (tx_word && (dspi->len == 1)) {
+			dspi->dataflags |= TRAN_STATE_WORD_ODD_NUM;
+			regmap_update_bits(dspi->regmap, SPI_CTAR(dspi->cs),
+					SPI_FRAME_BITS_MASK, SPI_FRAME_BITS(8));
+			tx_word = 0;
 		}
 
+		dspi_pushr = dspi_data_to_pushr(dspi, tx_word);
+
 		if (dspi->len == 0 || tx_count == DSPI_FIFO_SIZE - 1) {
 			/* last transfer in the transfer */
 			dspi_pushr |= SPI_PUSHR_EOQ;
@@ -278,11 +305,6 @@ static int dspi_transfer_write(struct fsl_dspi *dspi)
 		} else if (tx_word && (dspi->len == 1))
 			dspi_pushr |= SPI_PUSHR_EOQ;
 
-		if (first) {
-			first = 0;
-			dspi_pushr |= SPI_PUSHR_CTCNT; /* clear counter */
-		}
-
 		regmap_write(dspi->regmap, SPI_PUSHR, dspi_pushr);
 
 		tx_count++;
@@ -291,40 +313,55 @@ static int dspi_transfer_write(struct fsl_dspi *dspi)
 	return tx_count * (tx_word + 1);
 }
 
-static int dspi_transfer_read(struct fsl_dspi *dspi)
+static int dspi_eoq_read(struct fsl_dspi *dspi)
 {
 	int rx_count = 0;
 	int rx_word = is_double_byte_mode(dspi);
-	u16 d;
 
 	while ((dspi->rx < dspi->rx_end)
 			&& (rx_count < DSPI_FIFO_SIZE)) {
-		if (rx_word) {
-			unsigned int val;
+		if (rx_word && (dspi->rx_end - dspi->rx) == 1)
+			rx_word = 0;
 
-			if ((dspi->rx_end - dspi->rx) == 1)
-				break;
+		dspi_data_from_popr(dspi, rx_word);
+		rx_count++;
+	}
 
-			regmap_read(dspi->regmap, SPI_POPR, &val);
-			d = SPI_POPR_RXDATA(val);
+	return rx_count;
+}
 
-			if (!(dspi->dataflags & TRAN_STATE_RX_VOID))
-				*(u16 *)dspi->rx = d;
-			dspi->rx += 2;
+static int dspi_tcfq_write(struct fsl_dspi *dspi)
+{
+	int tx_word;
+	u32 dspi_pushr = 0;
 
-		} else {
-			unsigned int val;
+	tx_word = is_double_byte_mode(dspi);
 
-			regmap_read(dspi->regmap, SPI_POPR, &val);
-			d = SPI_POPR_RXDATA(val);
-			if (!(dspi->dataflags & TRAN_STATE_RX_VOID))
-				*(u8 *)dspi->rx = d;
-			dspi->rx++;
-		}
-		rx_count++;
+	if (tx_word && (dspi->len == 1)) {
+		dspi->dataflags |= TRAN_STATE_WORD_ODD_NUM;
+		regmap_update_bits(dspi->regmap, SPI_CTAR(dspi->cs),
+				SPI_FRAME_BITS_MASK, SPI_FRAME_BITS(8));
+		tx_word = 0;
 	}
 
-	return rx_count;
+	dspi_pushr = dspi_data_to_pushr(dspi, tx_word);
+
+	if ((dspi->cs_change) && (!dspi->len))
+		dspi_pushr &= ~SPI_PUSHR_CONT;
+
+	regmap_write(dspi->regmap, SPI_PUSHR, dspi_pushr);
+
+	return tx_word + 1;
+}
+
+static void dspi_tcfq_read(struct fsl_dspi *dspi)
+{
+	int rx_word = is_double_byte_mode(dspi);
+
+	if (rx_word && (dspi->rx_end - dspi->rx) == 1)
+		rx_word = 0;
+
+	dspi_data_from_popr(dspi, rx_word);
 }
 
 static int dspi_transfer_one_message(struct spi_master *master,
@@ -334,6 +371,12 @@ static int dspi_transfer_one_message(struct spi_master *master,
 	struct spi_device *spi = message->spi;
 	struct spi_transfer *transfer;
 	int status = 0;
+	enum dspi_trans_mode trans_mode;
+	u32 spi_tcr;
+
+	regmap_read(dspi->regmap, SPI_TCR, &spi_tcr);
+	dspi->spi_tcnt = SPI_TCR_GET_TCNT(spi_tcr);
+
 	message->actual_length = 0;
 
 	list_for_each_entry(transfer, &message->transfers, transfer_list) {
@@ -341,10 +384,10 @@ static int dspi_transfer_one_message(struct spi_master *master,
 		dspi->cur_msg = message;
 		dspi->cur_chip = spi_get_ctldata(spi);
 		dspi->cs = spi->chip_select;
+		dspi->cs_change = 0;
 		if (dspi->cur_transfer->transfer_list.next
 				== &dspi->cur_msg->transfers)
-			transfer->cs_change = 1;
-		dspi->cs_change = transfer->cs_change;
+			dspi->cs_change = 1;
 		dspi->void_write_data = dspi->cur_chip->void_write_data;
 
 		dspi->dataflags = 0;
@@ -370,8 +413,22 @@ static int dspi_transfer_one_message(struct spi_master *master,
 			regmap_write(dspi->regmap, SPI_CTAR(dspi->cs),
 					dspi->cur_chip->ctar_val);
 
-		regmap_write(dspi->regmap, SPI_RSER, SPI_RSER_EOQFE);
-		message->actual_length += dspi_transfer_write(dspi);
+		trans_mode = dspi->devtype_data->trans_mode;
+		switch (trans_mode) {
+		case DSPI_EOQ_MODE:
+			regmap_write(dspi->regmap, SPI_RSER, SPI_RSER_EOQFE);
+			dspi_eoq_write(dspi);
+			break;
+		case DSPI_TCFQ_MODE:
+			regmap_write(dspi->regmap, SPI_RSER, SPI_RSER_TCFQE);
+			dspi_tcfq_write(dspi);
+			break;
+		default:
+			dev_err(&dspi->pdev->dev, "unsupported trans_mode %u\n",
+				trans_mode);
+			status = -EINVAL;
+			goto out;
+		}
 
 		if (wait_event_interruptible(dspi->waitq, dspi->waitflags))
 			dev_err(&dspi->pdev->dev, "wait transfer complete fail!\n");
@@ -381,6 +438,7 @@ static int dspi_transfer_one_message(struct spi_master *master,
 			udelay(transfer->delay_usecs);
 	}
 
+out:
 	message->status = status;
 	spi_finalize_current_message(master);
 
@@ -460,27 +518,89 @@ static void dspi_cleanup(struct spi_device *spi)
 static irqreturn_t dspi_interrupt(int irq, void *dev_id)
 {
 	struct fsl_dspi *dspi = (struct fsl_dspi *)dev_id;
-
 	struct spi_message *msg = dspi->cur_msg;
+	enum dspi_trans_mode trans_mode;
+	u32 spi_sr, spi_tcr;
+	u32 spi_tcnt, tcnt_diff;
+	int tx_word;
 
-	regmap_write(dspi->regmap, SPI_SR, SPI_SR_EOQF);
-	dspi_transfer_read(dspi);
-
-	if (!dspi->len) {
+	regmap_read(dspi->regmap, SPI_SR, &spi_sr);
+	regmap_write(dspi->regmap, SPI_SR, spi_sr);
+
+
+	if (spi_sr & (SPI_SR_EOQF | SPI_SR_TCFQF)) {
+		tx_word = is_double_byte_mode(dspi);
+
+		regmap_read(dspi->regmap, SPI_TCR, &spi_tcr);
+		spi_tcnt = SPI_TCR_GET_TCNT(spi_tcr);
+		/*
+		 * The width of SPI Transfer Counter in SPI_TCR is 16bits,
+		 * so the max couner is 65535. When the counter reach 65535,
+		 * it will wrap around, counter reset to zero.
+		 * spi_tcnt my be less than dspi->spi_tcnt, it means the
+		 * counter already wrapped around.
+		 * SPI Transfer Counter is a counter of transmitted frames.
+		 * The size of frame maybe two bytes.
+		 */
+		tcnt_diff = ((spi_tcnt + SPI_TCR_TCNT_MAX) - dspi->spi_tcnt)
+			% SPI_TCR_TCNT_MAX;
+		tcnt_diff *= (tx_word + 1);
 		if (dspi->dataflags & TRAN_STATE_WORD_ODD_NUM)
-			regmap_update_bits(dspi->regmap, SPI_CTAR(dspi->cs),
-			SPI_FRAME_BITS_MASK, SPI_FRAME_BITS(16));
+			tcnt_diff--;
+
+		msg->actual_length += tcnt_diff;
+
+		dspi->spi_tcnt = spi_tcnt;
+
+		trans_mode = dspi->devtype_data->trans_mode;
+		switch (trans_mode) {
+		case DSPI_EOQ_MODE:
+			dspi_eoq_read(dspi);
+			break;
+		case DSPI_TCFQ_MODE:
+			dspi_tcfq_read(dspi);
+			break;
+		default:
+			dev_err(&dspi->pdev->dev, "unsupported trans_mode %u\n",
+				trans_mode);
+				return IRQ_HANDLED;
+		}
 
-		dspi->waitflags = 1;
-		wake_up_interruptible(&dspi->waitq);
-	} else
-		msg->actual_length += dspi_transfer_write(dspi);
+		if (!dspi->len) {
+			if (dspi->dataflags & TRAN_STATE_WORD_ODD_NUM) {
+				regmap_update_bits(dspi->regmap,
+						   SPI_CTAR(dspi->cs),
+						   SPI_FRAME_BITS_MASK,
+						   SPI_FRAME_BITS(16));
+				dspi->dataflags &= ~TRAN_STATE_WORD_ODD_NUM;
+			}
+
+			dspi->waitflags = 1;
+			wake_up_interruptible(&dspi->waitq);
+		} else {
+			switch (trans_mode) {
+			case DSPI_EOQ_MODE:
+				dspi_eoq_write(dspi);
+				break;
+			case DSPI_TCFQ_MODE:
+				dspi_tcfq_write(dspi);
+				break;
+			default:
+				dev_err(&dspi->pdev->dev,
+					"unsupported trans_mode %u\n",
+					trans_mode);
+			}
+		}
+	}
 
 	return IRQ_HANDLED;
 }
 
 static const struct of_device_id fsl_dspi_dt_ids[] = {
-	{ .compatible = "fsl,vf610-dspi", .data = NULL, },
+	{ .compatible = "fsl,vf610-dspi", .data = (void *)&vf610_data, },
+	{ .compatible = "fsl,ls1021a-v1.0-dspi",
+		.data = (void *)&ls1021a_v1_data, },
+	{ .compatible = "fsl,ls2085a-dspi", .data = (void *)&ls2085a_data, },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, fsl_dspi_dt_ids);
@@ -494,6 +614,8 @@ static int dspi_suspend(struct device *dev)
 	spi_master_suspend(master);
 	clk_disable_unprepare(dspi->clk);
 
+	pinctrl_pm_select_sleep_state(dev);
+
 	return 0;
 }
 
@@ -502,6 +624,8 @@ static int dspi_resume(struct device *dev)
 	struct spi_master *master = dev_get_drvdata(dev);
 	struct fsl_dspi *dspi = spi_master_get_devdata(master);
 
+	pinctrl_pm_select_default_state(dev);
+
 	clk_prepare_enable(dspi->clk);
 	spi_master_resume(master);
 
@@ -526,6 +650,8 @@ static int dspi_probe(struct platform_device *pdev)
 	struct resource *res;
 	void __iomem *base;
 	int ret = 0, cs_num, bus_num;
+	const struct of_device_id *of_id =
+			of_match_device(fsl_dspi_dt_ids, &pdev->dev);
 
 	master = spi_alloc_master(&pdev->dev, sizeof(struct fsl_dspi));
 	if (!master)
@@ -559,6 +685,13 @@ static int dspi_probe(struct platform_device *pdev)
 	}
 	master->bus_num = bus_num;
 
+	dspi->devtype_data = (struct fsl_dspi_devtype_data *)of_id->data;
+	if (!dspi->devtype_data) {
+		dev_err(&pdev->dev, "can't get devtype_data\n");
+		ret = -EFAULT;
+		goto out_master_put;
+	}
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(base)) {
@@ -566,7 +699,7 @@ static int dspi_probe(struct platform_device *pdev)
 		goto out_master_put;
 	}
 
-	dspi->regmap = devm_regmap_init_mmio_clk(&pdev->dev, "dspi", base,
+	dspi->regmap = devm_regmap_init_mmio_clk(&pdev->dev, NULL, base,
 						&dspi_regmap_config);
 	if (IS_ERR(dspi->regmap)) {
 		dev_err(&pdev->dev, "failed to init regmap: %ld\n",
diff --git a/drivers/spi/spi-fsl-espi.c b/drivers/spi/spi-fsl-espi.c
index 80d245ac846f..d3f05a0525a4 100644
--- a/drivers/spi/spi-fsl-espi.c
+++ b/drivers/spi/spi-fsl-espi.c
@@ -561,9 +561,13 @@ void fsl_espi_cpu_irq(struct mpc8xxx_spi *mspi, u32 events)
 
 		/* spin until TX is done */
 		ret = spin_event_timeout(((events = mpc8xxx_spi_read_reg(
-				&reg_base->event)) & SPIE_NF) == 0, 1000, 0);
+				&reg_base->event)) & SPIE_NF), 1000, 0);
 		if (!ret) {
 			dev_err(mspi->dev, "tired waiting for SPIE_NF\n");
+
+			/* Clear the SPIE bits */
+			mpc8xxx_spi_write_reg(&reg_base->event, events);
+			complete(&mspi->done);
 			return;
 		}
 	}
diff --git a/drivers/spi/spi-imx.c b/drivers/spi/spi-imx.c
index f08e812b2984..eb7d3a6fb14c 100644
--- a/drivers/spi/spi-imx.c
+++ b/drivers/spi/spi-imx.c
@@ -674,7 +674,7 @@ static struct spi_imx_devtype_data imx51_ecspi_devtype_data = {
 	.devtype = IMX51_ECSPI,
 };
 
-static struct platform_device_id spi_imx_devtype[] = {
+static const struct platform_device_id spi_imx_devtype[] = {
 	{
 		.name = "imx1-cspi",
 		.driver_data = (kernel_ulong_t) &imx1_cspi_devtype_data,
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index d1a5b9fc3eba..58673841286c 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -35,6 +35,7 @@
 #include <linux/gcd.h>
 
 #include <linux/spi/spi.h>
+#include <linux/gpio.h>
 
 #include <linux/platform_data/spi-omap2-mcspi.h>
 
@@ -242,17 +243,27 @@ static void omap2_mcspi_set_enable(const struct spi_device *spi, int enable)
 	mcspi_read_cs_reg(spi, OMAP2_MCSPI_CHCTRL0);
 }
 
-static void omap2_mcspi_force_cs(struct spi_device *spi, int cs_active)
+static void omap2_mcspi_set_cs(struct spi_device *spi, bool enable)
 {
 	u32 l;
 
-	l = mcspi_cached_chconf0(spi);
-	if (cs_active)
-		l |= OMAP2_MCSPI_CHCONF_FORCE;
-	else
-		l &= ~OMAP2_MCSPI_CHCONF_FORCE;
+	/* The controller handles the inverted chip selects
+	 * using the OMAP2_MCSPI_CHCONF_EPOL bit so revert
+	 * the inversion from the core spi_set_cs function.
+	 */
+	if (spi->mode & SPI_CS_HIGH)
+		enable = !enable;
 
-	mcspi_write_chconf0(spi, l);
+	if (spi->controller_state) {
+		l = mcspi_cached_chconf0(spi);
+
+		if (enable)
+			l &= ~OMAP2_MCSPI_CHCONF_FORCE;
+		else
+			l |= OMAP2_MCSPI_CHCONF_FORCE;
+
+		mcspi_write_chconf0(spi, l);
+	}
 }
 
 static void omap2_mcspi_set_master_mode(struct spi_master *master)
@@ -1011,6 +1022,15 @@ static int omap2_mcspi_setup(struct spi_device *spi)
 			return ret;
 	}
 
+	if (gpio_is_valid(spi->cs_gpio)) {
+		ret = gpio_request(spi->cs_gpio, dev_name(&spi->dev));
+		if (ret) {
+			dev_err(&spi->dev, "failed to request gpio\n");
+			return ret;
+		}
+		gpio_direction_output(spi->cs_gpio, !(spi->mode & SPI_CS_HIGH));
+	}
+
 	ret = pm_runtime_get_sync(mcspi->dev);
 	if (ret < 0)
 		return ret;
@@ -1050,9 +1070,13 @@ static void omap2_mcspi_cleanup(struct spi_device *spi)
 			mcspi_dma->dma_tx = NULL;
 		}
 	}
+
+	if (gpio_is_valid(spi->cs_gpio))
+		gpio_free(spi->cs_gpio);
 }
 
-static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
+static int omap2_mcspi_work_one(struct omap2_mcspi *mcspi,
+		struct spi_device *spi, struct spi_transfer *t)
 {
 
 	/* We only enable one channel at a time -- the one whose message is
@@ -1062,18 +1086,14 @@ static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
 	 * chipselect with the FORCE bit ... CS != channel enable.
 	 */
 
-	struct spi_device		*spi;
-	struct spi_transfer		*t = NULL;
 	struct spi_master		*master;
 	struct omap2_mcspi_dma		*mcspi_dma;
-	int				cs_active = 0;
 	struct omap2_mcspi_cs		*cs;
 	struct omap2_mcspi_device_config *cd;
 	int				par_override = 0;
 	int				status = 0;
 	u32				chconf;
 
-	spi = m->spi;
 	master = spi->master;
 	mcspi_dma = mcspi->dma_channels + spi->chip_select;
 	cs = spi->controller_state;
@@ -1090,103 +1110,84 @@ static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
 		par_override = 1;
 
 	omap2_mcspi_set_enable(spi, 0);
-	list_for_each_entry(t, &m->transfers, transfer_list) {
-		if (t->tx_buf == NULL && t->rx_buf == NULL && t->len) {
-			status = -EINVAL;
-			break;
-		}
-		if (par_override ||
-		    (t->speed_hz != spi->max_speed_hz) ||
-		    (t->bits_per_word != spi->bits_per_word)) {
-			par_override = 1;
-			status = omap2_mcspi_setup_transfer(spi, t);
-			if (status < 0)
-				break;
-			if (t->speed_hz == spi->max_speed_hz &&
-			    t->bits_per_word == spi->bits_per_word)
-				par_override = 0;
-		}
-		if (cd && cd->cs_per_word) {
-			chconf = mcspi->ctx.modulctrl;
-			chconf &= ~OMAP2_MCSPI_MODULCTRL_SINGLE;
-			mcspi_write_reg(master, OMAP2_MCSPI_MODULCTRL, chconf);
-			mcspi->ctx.modulctrl =
-				mcspi_read_cs_reg(spi, OMAP2_MCSPI_MODULCTRL);
-		}
 
+	if (gpio_is_valid(spi->cs_gpio))
+		omap2_mcspi_set_cs(spi, spi->mode & SPI_CS_HIGH);
 
-		if (!cs_active) {
-			omap2_mcspi_force_cs(spi, 1);
-			cs_active = 1;
-		}
-
-		chconf = mcspi_cached_chconf0(spi);
-		chconf &= ~OMAP2_MCSPI_CHCONF_TRM_MASK;
-		chconf &= ~OMAP2_MCSPI_CHCONF_TURBO;
+	if (par_override ||
+	    (t->speed_hz != spi->max_speed_hz) ||
+	    (t->bits_per_word != spi->bits_per_word)) {
+		par_override = 1;
+		status = omap2_mcspi_setup_transfer(spi, t);
+		if (status < 0)
+			goto out;
+		if (t->speed_hz == spi->max_speed_hz &&
+		    t->bits_per_word == spi->bits_per_word)
+			par_override = 0;
+	}
+	if (cd && cd->cs_per_word) {
+		chconf = mcspi->ctx.modulctrl;
+		chconf &= ~OMAP2_MCSPI_MODULCTRL_SINGLE;
+		mcspi_write_reg(master, OMAP2_MCSPI_MODULCTRL, chconf);
+		mcspi->ctx.modulctrl =
+			mcspi_read_cs_reg(spi, OMAP2_MCSPI_MODULCTRL);
+	}
 
-		if (t->tx_buf == NULL)
-			chconf |= OMAP2_MCSPI_CHCONF_TRM_RX_ONLY;
-		else if (t->rx_buf == NULL)
-			chconf |= OMAP2_MCSPI_CHCONF_TRM_TX_ONLY;
-
-		if (cd && cd->turbo_mode && t->tx_buf == NULL) {
-			/* Turbo mode is for more than one word */
-			if (t->len > ((cs->word_len + 7) >> 3))
-				chconf |= OMAP2_MCSPI_CHCONF_TURBO;
-		}
+	chconf = mcspi_cached_chconf0(spi);
+	chconf &= ~OMAP2_MCSPI_CHCONF_TRM_MASK;
+	chconf &= ~OMAP2_MCSPI_CHCONF_TURBO;
+
+	if (t->tx_buf == NULL)
+		chconf |= OMAP2_MCSPI_CHCONF_TRM_RX_ONLY;
+	else if (t->rx_buf == NULL)
+		chconf |= OMAP2_MCSPI_CHCONF_TRM_TX_ONLY;
+
+	if (cd && cd->turbo_mode && t->tx_buf == NULL) {
+		/* Turbo mode is for more than one word */
+		if (t->len > ((cs->word_len + 7) >> 3))
+			chconf |= OMAP2_MCSPI_CHCONF_TURBO;
+	}
 
-		mcspi_write_chconf0(spi, chconf);
+	mcspi_write_chconf0(spi, chconf);
 
-		if (t->len) {
-			unsigned	count;
+	if (t->len) {
+		unsigned	count;
 
-			if ((mcspi_dma->dma_rx && mcspi_dma->dma_tx) &&
-			    (m->is_dma_mapped || t->len >= DMA_MIN_BYTES))
-				omap2_mcspi_set_fifo(spi, t, 1);
+		if ((mcspi_dma->dma_rx && mcspi_dma->dma_tx) &&
+		    (t->len >= DMA_MIN_BYTES))
+			omap2_mcspi_set_fifo(spi, t, 1);
 
-			omap2_mcspi_set_enable(spi, 1);
+		omap2_mcspi_set_enable(spi, 1);
 
-			/* RX_ONLY mode needs dummy data in TX reg */
-			if (t->tx_buf == NULL)
-				writel_relaxed(0, cs->base
-						+ OMAP2_MCSPI_TX0);
+		/* RX_ONLY mode needs dummy data in TX reg */
+		if (t->tx_buf == NULL)
+			writel_relaxed(0, cs->base
+					+ OMAP2_MCSPI_TX0);
 
-			if ((mcspi_dma->dma_rx && mcspi_dma->dma_tx) &&
-			    (m->is_dma_mapped || t->len >= DMA_MIN_BYTES))
-				count = omap2_mcspi_txrx_dma(spi, t);
-			else
-				count = omap2_mcspi_txrx_pio(spi, t);
-			m->actual_length += count;
+		if ((mcspi_dma->dma_rx && mcspi_dma->dma_tx) &&
+		    (t->len >= DMA_MIN_BYTES))
+			count = omap2_mcspi_txrx_dma(spi, t);
+		else
+			count = omap2_mcspi_txrx_pio(spi, t);
 
-			if (count != t->len) {
-				status = -EIO;
-				break;
-			}
+		if (count != t->len) {
+			status = -EIO;
+			goto out;
 		}
+	}
 
-		if (t->delay_usecs)
-			udelay(t->delay_usecs);
-
-		/* ignore the "leave it on after last xfer" hint */
-		if (t->cs_change) {
-			omap2_mcspi_force_cs(spi, 0);
-			cs_active = 0;
-		}
+	omap2_mcspi_set_enable(spi, 0);
 
-		omap2_mcspi_set_enable(spi, 0);
+	if (mcspi->fifo_depth > 0)
+		omap2_mcspi_set_fifo(spi, t, 0);
 
-		if (mcspi->fifo_depth > 0)
-			omap2_mcspi_set_fifo(spi, t, 0);
-	}
+out:
 	/* Restore defaults if they were overriden */
 	if (par_override) {
 		par_override = 0;
 		status = omap2_mcspi_setup_transfer(spi, NULL);
 	}
 
-	if (cs_active)
-		omap2_mcspi_force_cs(spi, 0);
-
 	if (cd && cd->cs_per_word) {
 		chconf = mcspi->ctx.modulctrl;
 		chconf |= OMAP2_MCSPI_MODULCTRL_SINGLE;
@@ -1197,78 +1198,64 @@ static void omap2_mcspi_work(struct omap2_mcspi *mcspi, struct spi_message *m)
 
 	omap2_mcspi_set_enable(spi, 0);
 
+	if (gpio_is_valid(spi->cs_gpio))
+		omap2_mcspi_set_cs(spi, !(spi->mode & SPI_CS_HIGH));
+
 	if (mcspi->fifo_depth > 0 && t)
 		omap2_mcspi_set_fifo(spi, t, 0);
 
-	m->status = status;
+	return status;
 }
 
-static int omap2_mcspi_transfer_one_message(struct spi_master *master,
-		struct spi_message *m)
+static int omap2_mcspi_transfer_one(struct spi_master *master,
+		struct spi_device *spi, struct spi_transfer *t)
 {
-	struct spi_device	*spi;
 	struct omap2_mcspi	*mcspi;
 	struct omap2_mcspi_dma	*mcspi_dma;
-	struct spi_transfer	*t;
-	int status;
+	const void	*tx_buf = t->tx_buf;
+	void		*rx_buf = t->rx_buf;
+	unsigned	len = t->len;
 
-	spi = m->spi;
 	mcspi = spi_master_get_devdata(master);
 	mcspi_dma = mcspi->dma_channels + spi->chip_select;
-	m->actual_length = 0;
-	m->status = 0;
-
-	list_for_each_entry(t, &m->transfers, transfer_list) {
-		const void	*tx_buf = t->tx_buf;
-		void		*rx_buf = t->rx_buf;
-		unsigned	len = t->len;
-
-		if ((len && !(rx_buf || tx_buf))) {
-			dev_dbg(mcspi->dev, "transfer: %d Hz, %d %s%s, %d bpw\n",
-					t->speed_hz,
-					len,
-					tx_buf ? "tx" : "",
-					rx_buf ? "rx" : "",
-					t->bits_per_word);
-			status = -EINVAL;
-			goto out;
-		}
 
-		if (m->is_dma_mapped || len < DMA_MIN_BYTES)
-			continue;
-
-		if (mcspi_dma->dma_tx && tx_buf != NULL) {
-			t->tx_dma = dma_map_single(mcspi->dev, (void *) tx_buf,
-					len, DMA_TO_DEVICE);
-			if (dma_mapping_error(mcspi->dev, t->tx_dma)) {
-				dev_dbg(mcspi->dev, "dma %cX %d bytes error\n",
-						'T', len);
-				status = -EINVAL;
-				goto out;
-			}
+	if ((len && !(rx_buf || tx_buf))) {
+		dev_dbg(mcspi->dev, "transfer: %d Hz, %d %s%s, %d bpw\n",
+				t->speed_hz,
+				len,
+				tx_buf ? "tx" : "",
+				rx_buf ? "rx" : "",
+				t->bits_per_word);
+		return -EINVAL;
+	}
+
+	if (len < DMA_MIN_BYTES)
+		goto skip_dma_map;
+
+	if (mcspi_dma->dma_tx && tx_buf != NULL) {
+		t->tx_dma = dma_map_single(mcspi->dev, (void *) tx_buf,
+				len, DMA_TO_DEVICE);
+		if (dma_mapping_error(mcspi->dev, t->tx_dma)) {
+			dev_dbg(mcspi->dev, "dma %cX %d bytes error\n",
+					'T', len);
+			return -EINVAL;
 		}
-		if (mcspi_dma->dma_rx && rx_buf != NULL) {
-			t->rx_dma = dma_map_single(mcspi->dev, rx_buf, t->len,
-					DMA_FROM_DEVICE);
-			if (dma_mapping_error(mcspi->dev, t->rx_dma)) {
-				dev_dbg(mcspi->dev, "dma %cX %d bytes error\n",
-						'R', len);
-				if (tx_buf != NULL)
-					dma_unmap_single(mcspi->dev, t->tx_dma,
-							len, DMA_TO_DEVICE);
-				status = -EINVAL;
-				goto out;
-			}
+	}
+	if (mcspi_dma->dma_rx && rx_buf != NULL) {
+		t->rx_dma = dma_map_single(mcspi->dev, rx_buf, t->len,
+				DMA_FROM_DEVICE);
+		if (dma_mapping_error(mcspi->dev, t->rx_dma)) {
+			dev_dbg(mcspi->dev, "dma %cX %d bytes error\n",
+					'R', len);
+			if (tx_buf != NULL)
+				dma_unmap_single(mcspi->dev, t->tx_dma,
+						len, DMA_TO_DEVICE);
+			return -EINVAL;
 		}
 	}
 
-	omap2_mcspi_work(mcspi, m);
-	/* spi_finalize_current_message() changes the status inside the
-	 * spi_message, save the status here. */
-	status = m->status;
-out:
-	spi_finalize_current_message(master);
-	return status;
+skip_dma_map:
+	return omap2_mcspi_work_one(mcspi, spi, t);
 }
 
 static int omap2_mcspi_master_setup(struct omap2_mcspi *mcspi)
@@ -1347,7 +1334,8 @@ static int omap2_mcspi_probe(struct platform_device *pdev)
 	master->bits_per_word_mask = SPI_BPW_RANGE_MASK(4, 32);
 	master->setup = omap2_mcspi_setup;
 	master->auto_runtime_pm = true;
-	master->transfer_one_message = omap2_mcspi_transfer_one_message;
+	master->transfer_one = omap2_mcspi_transfer_one;
+	master->set_cs = omap2_mcspi_set_cs;
 	master->cleanup = omap2_mcspi_cleanup;
 	master->dev.of_node = node;
 	master->max_speed_hz = OMAP2_MCSPI_MAX_FREQ;
diff --git a/drivers/spi/spi-orion.c b/drivers/spi/spi-orion.c
index 861664776672..8cad107a5b3f 100644
--- a/drivers/spi/spi-orion.c
+++ b/drivers/spi/spi-orion.c
@@ -61,6 +61,12 @@ enum orion_spi_type {
 
 struct orion_spi_dev {
 	enum orion_spi_type	typ;
+	/*
+	 * min_divisor and max_hz should be exclusive, the only we can
+	 * have both is for managing the armada-370-spi case with old
+	 * device tree
+	 */
+	unsigned long		max_hz;
 	unsigned int		min_divisor;
 	unsigned int		max_divisor;
 	u32			prescale_mask;
@@ -385,16 +391,54 @@ static const struct orion_spi_dev orion_spi_dev_data = {
 	.prescale_mask = ORION_SPI_CLK_PRESCALE_MASK,
 };
 
-static const struct orion_spi_dev armada_spi_dev_data = {
+static const struct orion_spi_dev armada_370_spi_dev_data = {
 	.typ = ARMADA_SPI,
-	.min_divisor = 1,
+	.min_divisor = 4,
+	.max_divisor = 1920,
+	.max_hz = 50000000,
+	.prescale_mask = ARMADA_SPI_CLK_PRESCALE_MASK,
+};
+
+static const struct orion_spi_dev armada_xp_spi_dev_data = {
+	.typ = ARMADA_SPI,
+	.max_hz = 50000000,
+	.max_divisor = 1920,
+	.prescale_mask = ARMADA_SPI_CLK_PRESCALE_MASK,
+};
+
+static const struct orion_spi_dev armada_375_spi_dev_data = {
+	.typ = ARMADA_SPI,
+	.min_divisor = 15,
 	.max_divisor = 1920,
 	.prescale_mask = ARMADA_SPI_CLK_PRESCALE_MASK,
 };
 
 static const struct of_device_id orion_spi_of_match_table[] = {
-	{ .compatible = "marvell,orion-spi", .data = &orion_spi_dev_data, },
-	{ .compatible = "marvell,armada-370-spi", .data = &armada_spi_dev_data, },
+	{
+		.compatible = "marvell,orion-spi",
+		.data = &orion_spi_dev_data,
+	},
+	{
+		.compatible = "marvell,armada-370-spi",
+		.data = &armada_370_spi_dev_data,
+	},
+	{
+		.compatible = "marvell,armada-375-spi",
+		.data = &armada_375_spi_dev_data,
+	},
+	{
+		.compatible = "marvell,armada-380-spi",
+		.data = &armada_xp_spi_dev_data,
+	},
+	{
+		.compatible = "marvell,armada-390-spi",
+		.data = &armada_xp_spi_dev_data,
+	},
+	{
+		.compatible = "marvell,armada-xp-spi",
+		.data = &armada_xp_spi_dev_data,
+	},
+
 	{}
 };
 MODULE_DEVICE_TABLE(of, orion_spi_of_match_table);
@@ -454,7 +498,23 @@ static int orion_spi_probe(struct platform_device *pdev)
 		goto out;
 
 	tclk_hz = clk_get_rate(spi->clk);
-	master->max_speed_hz = DIV_ROUND_UP(tclk_hz, devdata->min_divisor);
+
+	/*
+	 * With old device tree, armada-370-spi could be used with
+	 * Armada XP, however for this SoC the maximum frequency is
+	 * 50MHz instead of tclk/4. On Armada 370, tclk cannot be
+	 * higher than 200MHz. So, in order to be able to handle both
+	 * SoCs, we can take the minimum of 50MHz and tclk/4.
+	 */
+	if (of_device_is_compatible(pdev->dev.of_node,
+					"marvell,armada-370-spi"))
+		master->max_speed_hz = min(devdata->max_hz,
+				DIV_ROUND_UP(tclk_hz, devdata->min_divisor));
+	else if (devdata->min_divisor)
+		master->max_speed_hz =
+			DIV_ROUND_UP(tclk_hz, devdata->min_divisor);
+	else
+		master->max_speed_hz = devdata->max_hz;
 	master->min_speed_hz = DIV_ROUND_UP(tclk_hz, devdata->max_divisor);
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/spi/spi-pxa2xx-pci.c b/drivers/spi/spi-pxa2xx-pci.c
index fa7399e84bbb..3cfd4357489a 100644
--- a/drivers/spi/spi-pxa2xx-pci.c
+++ b/drivers/spi/spi-pxa2xx-pci.c
@@ -62,7 +62,7 @@ static struct pxa_spi_info spi_info_configs[] = {
 		.max_clk_rate = 3686400,
 	},
 	[PORT_BYT] = {
-		.type = LPSS_SSP,
+		.type = LPSS_BYT_SSP,
 		.port_id = 0,
 		.num_chipselect = 1,
 		.max_clk_rate = 50000000,
@@ -70,7 +70,7 @@ static struct pxa_spi_info spi_info_configs[] = {
 		.rx_param = &byt_rx_param,
 	},
 	[PORT_BSW0] = {
-		.type = LPSS_SSP,
+		.type = LPSS_BYT_SSP,
 		.port_id = 0,
 		.num_chipselect = 1,
 		.max_clk_rate = 50000000,
@@ -78,7 +78,7 @@ static struct pxa_spi_info spi_info_configs[] = {
 		.rx_param = &bsw0_rx_param,
 	},
 	[PORT_BSW1] = {
-		.type = LPSS_SSP,
+		.type = LPSS_BYT_SSP,
 		.port_id = 1,
 		.num_chipselect = 1,
 		.max_clk_rate = 50000000,
@@ -86,7 +86,7 @@ static struct pxa_spi_info spi_info_configs[] = {
 		.rx_param = &bsw1_rx_param,
 	},
 	[PORT_BSW2] = {
-		.type = LPSS_SSP,
+		.type = LPSS_BYT_SSP,
 		.port_id = 2,
 		.num_chipselect = 1,
 		.max_clk_rate = 50000000,
diff --git a/drivers/spi/spi-pxa2xx-pxadma.c b/drivers/spi/spi-pxa2xx-pxadma.c
deleted file mode 100644
index 2e0796a0003f..000000000000
--- a/drivers/spi/spi-pxa2xx-pxadma.c
+++ /dev/null
@@ -1,487 +0,0 @@
-/*
- * PXA2xx SPI private DMA support.
- *
- * Copyright (C) 2005 Stephen Street / StreetFire Sound Labs
- *
- * This program is free software; you can redistribute it and/or modify
- * it under 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>
-#include <linux/device.h>
-#include <linux/dma-mapping.h>
-#include <linux/pxa2xx_ssp.h>
-#include <linux/spi/spi.h>
-#include <linux/spi/pxa2xx_spi.h>
-
-#include <mach/dma.h>
-#include "spi-pxa2xx.h"
-
-#define DMA_INT_MASK		(DCSR_ENDINTR | DCSR_STARTINTR | DCSR_BUSERR)
-#define RESET_DMA_CHANNEL	(DCSR_NODESC | DMA_INT_MASK)
-
-bool pxa2xx_spi_dma_is_possible(size_t len)
-{
-	/* Try to map dma buffer and do a dma transfer if successful, but
-	 * only if the length is non-zero and less than MAX_DMA_LEN.
-	 *
-	 * Zero-length non-descriptor DMA is illegal on PXA2xx; force use
-	 * of PIO instead.  Care is needed above because the transfer may
-	 * have have been passed with buffers that are already dma mapped.
-	 * A zero-length transfer in PIO mode will not try to write/read
-	 * to/from the buffers
-	 *
-	 * REVISIT large transfers are exactly where we most want to be
-	 * using DMA.  If this happens much, split those transfers into
-	 * multiple DMA segments rather than forcing PIO.
-	 */
-	return len > 0 && len <= MAX_DMA_LEN;
-}
-
-int pxa2xx_spi_map_dma_buffers(struct driver_data *drv_data)
-{
-	struct spi_message *msg = drv_data->cur_msg;
-	struct device *dev = &msg->spi->dev;
-
-	if (!drv_data->cur_chip->enable_dma)
-		return 0;
-
-	if (msg->is_dma_mapped)
-		return  drv_data->rx_dma && drv_data->tx_dma;
-
-	if (!IS_DMA_ALIGNED(drv_data->rx) || !IS_DMA_ALIGNED(drv_data->tx))
-		return 0;
-
-	/* Modify setup if rx buffer is null */
-	if (drv_data->rx == NULL) {
-		*drv_data->null_dma_buf = 0;
-		drv_data->rx = drv_data->null_dma_buf;
-		drv_data->rx_map_len = 4;
-	} else
-		drv_data->rx_map_len = drv_data->len;
-
-
-	/* Modify setup if tx buffer is null */
-	if (drv_data->tx == NULL) {
-		*drv_data->null_dma_buf = 0;
-		drv_data->tx = drv_data->null_dma_buf;
-		drv_data->tx_map_len = 4;
-	} else
-		drv_data->tx_map_len = drv_data->len;
-
-	/* Stream map the tx buffer. Always do DMA_TO_DEVICE first
-	 * so we flush the cache *before* invalidating it, in case
-	 * the tx and rx buffers overlap.
-	 */
-	drv_data->tx_dma = dma_map_single(dev, drv_data->tx,
-					drv_data->tx_map_len, DMA_TO_DEVICE);
-	if (dma_mapping_error(dev, drv_data->tx_dma))
-		return 0;
-
-	/* Stream map the rx buffer */
-	drv_data->rx_dma = dma_map_single(dev, drv_data->rx,
-					drv_data->rx_map_len, DMA_FROM_DEVICE);
-	if (dma_mapping_error(dev, drv_data->rx_dma)) {
-		dma_unmap_single(dev, drv_data->tx_dma,
-					drv_data->tx_map_len, DMA_TO_DEVICE);
-		return 0;
-	}
-
-	return 1;
-}
-
-static void pxa2xx_spi_unmap_dma_buffers(struct driver_data *drv_data)
-{
-	struct device *dev;
-
-	if (!drv_data->dma_mapped)
-		return;
-
-	if (!drv_data->cur_msg->is_dma_mapped) {
-		dev = &drv_data->cur_msg->spi->dev;
-		dma_unmap_single(dev, drv_data->rx_dma,
-					drv_data->rx_map_len, DMA_FROM_DEVICE);
-		dma_unmap_single(dev, drv_data->tx_dma,
-					drv_data->tx_map_len, DMA_TO_DEVICE);
-	}
-
-	drv_data->dma_mapped = 0;
-}
-
-static int wait_ssp_rx_stall(struct driver_data *drv_data)
-{
-	unsigned long limit = loops_per_jiffy << 1;
-
-	while ((pxa2xx_spi_read(drv_data, SSSR) & SSSR_BSY) && --limit)
-		cpu_relax();
-
-	return limit;
-}
-
-static int wait_dma_channel_stop(int channel)
-{
-	unsigned long limit = loops_per_jiffy << 1;
-
-	while (!(DCSR(channel) & DCSR_STOPSTATE) && --limit)
-		cpu_relax();
-
-	return limit;
-}
-
-static void pxa2xx_spi_dma_error_stop(struct driver_data *drv_data,
-				      const char *msg)
-{
-	/* Stop and reset */
-	DCSR(drv_data->rx_channel) = RESET_DMA_CHANNEL;
-	DCSR(drv_data->tx_channel) = RESET_DMA_CHANNEL;
-	write_SSSR_CS(drv_data, drv_data->clear_sr);
-	pxa2xx_spi_write(drv_data, SSCR1,
-			 pxa2xx_spi_read(drv_data, SSCR1)
-			 & ~drv_data->dma_cr1);
-	if (!pxa25x_ssp_comp(drv_data))
-		pxa2xx_spi_write(drv_data, SSTO, 0);
-	pxa2xx_spi_flush(drv_data);
-	pxa2xx_spi_write(drv_data, SSCR0,
-			 pxa2xx_spi_read(drv_data, SSCR0) & ~SSCR0_SSE);
-
-	pxa2xx_spi_unmap_dma_buffers(drv_data);
-
-	dev_err(&drv_data->pdev->dev, "%s\n", msg);
-
-	drv_data->cur_msg->state = ERROR_STATE;
-	tasklet_schedule(&drv_data->pump_transfers);
-}
-
-static void pxa2xx_spi_dma_transfer_complete(struct driver_data *drv_data)
-{
-	struct spi_message *msg = drv_data->cur_msg;
-
-	/* Clear and disable interrupts on SSP and DMA channels*/
-	pxa2xx_spi_write(drv_data, SSCR1,
-			 pxa2xx_spi_read(drv_data, SSCR1)
-			 & ~drv_data->dma_cr1);
-	write_SSSR_CS(drv_data, drv_data->clear_sr);
-	DCSR(drv_data->tx_channel) = RESET_DMA_CHANNEL;
-	DCSR(drv_data->rx_channel) = RESET_DMA_CHANNEL;
-
-	if (wait_dma_channel_stop(drv_data->rx_channel) == 0)
-		dev_err(&drv_data->pdev->dev,
-			"dma_handler: dma rx channel stop failed\n");
-
-	if (wait_ssp_rx_stall(drv_data->ioaddr) == 0)
-		dev_err(&drv_data->pdev->dev,
-			"dma_transfer: ssp rx stall failed\n");
-
-	pxa2xx_spi_unmap_dma_buffers(drv_data);
-
-	/* update the buffer pointer for the amount completed in dma */
-	drv_data->rx += drv_data->len -
-			(DCMD(drv_data->rx_channel) & DCMD_LENGTH);
-
-	/* read trailing data from fifo, it does not matter how many
-	 * bytes are in the fifo just read until buffer is full
-	 * or fifo is empty, which ever occurs first */
-	drv_data->read(drv_data);
-
-	/* return count of what was actually read */
-	msg->actual_length += drv_data->len -
-				(drv_data->rx_end - drv_data->rx);
-
-	/* Transfer delays and chip select release are
-	 * handled in pump_transfers or giveback
-	 */
-
-	/* Move to next transfer */
-	msg->state = pxa2xx_spi_next_transfer(drv_data);
-
-	/* Schedule transfer tasklet */
-	tasklet_schedule(&drv_data->pump_transfers);
-}
-
-void pxa2xx_spi_dma_handler(int channel, void *data)
-{
-	struct driver_data *drv_data = data;
-	u32 irq_status = DCSR(channel) & DMA_INT_MASK;
-
-	if (irq_status & DCSR_BUSERR) {
-
-		if (channel == drv_data->tx_channel)
-			pxa2xx_spi_dma_error_stop(drv_data,
-				"dma_handler: bad bus address on tx channel");
-		else
-			pxa2xx_spi_dma_error_stop(drv_data,
-				"dma_handler: bad bus address on rx channel");
-		return;
-	}
-
-	/* PXA255x_SSP has no timeout interrupt, wait for tailing bytes */
-	if ((channel == drv_data->tx_channel)
-		&& (irq_status & DCSR_ENDINTR)
-		&& (drv_data->ssp_type == PXA25x_SSP)) {
-
-		/* Wait for rx to stall */
-		if (wait_ssp_rx_stall(drv_data) == 0)
-			dev_err(&drv_data->pdev->dev,
-				"dma_handler: ssp rx stall failed\n");
-
-		/* finish this transfer, start the next */
-		pxa2xx_spi_dma_transfer_complete(drv_data);
-	}
-}
-
-irqreturn_t pxa2xx_spi_dma_transfer(struct driver_data *drv_data)
-{
-	u32 irq_status;
-
-	irq_status = pxa2xx_spi_read(drv_data, SSSR) & drv_data->mask_sr;
-	if (irq_status & SSSR_ROR) {
-		pxa2xx_spi_dma_error_stop(drv_data,
-					  "dma_transfer: fifo overrun");
-		return IRQ_HANDLED;
-	}
-
-	/* Check for false positive timeout */
-	if ((irq_status & SSSR_TINT)
-		&& (DCSR(drv_data->tx_channel) & DCSR_RUN)) {
-		pxa2xx_spi_write(drv_data, SSSR, SSSR_TINT);
-		return IRQ_HANDLED;
-	}
-
-	if (irq_status & SSSR_TINT || drv_data->rx == drv_data->rx_end) {
-
-		/* Clear and disable timeout interrupt, do the rest in
-		 * dma_transfer_complete */
-		if (!pxa25x_ssp_comp(drv_data))
-			pxa2xx_spi_write(drv_data, SSTO, 0);
-
-		/* finish this transfer, start the next */
-		pxa2xx_spi_dma_transfer_complete(drv_data);
-
-		return IRQ_HANDLED;
-	}
-
-	/* Opps problem detected */
-	return IRQ_NONE;
-}
-
-int pxa2xx_spi_dma_prepare(struct driver_data *drv_data, u32 dma_burst)
-{
-	u32 dma_width;
-
-	switch (drv_data->n_bytes) {
-	case 1:
-		dma_width = DCMD_WIDTH1;
-		break;
-	case 2:
-		dma_width = DCMD_WIDTH2;
-		break;
-	default:
-		dma_width = DCMD_WIDTH4;
-		break;
-	}
-
-	/* Setup rx DMA Channel */
-	DCSR(drv_data->rx_channel) = RESET_DMA_CHANNEL;
-	DSADR(drv_data->rx_channel) = drv_data->ssdr_physical;
-	DTADR(drv_data->rx_channel) = drv_data->rx_dma;
-	if (drv_data->rx == drv_data->null_dma_buf)
-		/* No target address increment */
-		DCMD(drv_data->rx_channel) = DCMD_FLOWSRC
-						| dma_width
-						| dma_burst
-						| drv_data->len;
-	else
-		DCMD(drv_data->rx_channel) = DCMD_INCTRGADDR
-						| DCMD_FLOWSRC
-						| dma_width
-						| dma_burst
-						| drv_data->len;
-
-	/* Setup tx DMA Channel */
-	DCSR(drv_data->tx_channel) = RESET_DMA_CHANNEL;
-	DSADR(drv_data->tx_channel) = drv_data->tx_dma;
-	DTADR(drv_data->tx_channel) = drv_data->ssdr_physical;
-	if (drv_data->tx == drv_data->null_dma_buf)
-		/* No source address increment */
-		DCMD(drv_data->tx_channel) = DCMD_FLOWTRG
-						| dma_width
-						| dma_burst
-						| drv_data->len;
-	else
-		DCMD(drv_data->tx_channel) = DCMD_INCSRCADDR
-						| DCMD_FLOWTRG
-						| dma_width
-						| dma_burst
-						| drv_data->len;
-
-	/* Enable dma end irqs on SSP to detect end of transfer */
-	if (drv_data->ssp_type == PXA25x_SSP)
-		DCMD(drv_data->tx_channel) |= DCMD_ENDIRQEN;
-
-	return 0;
-}
-
-void pxa2xx_spi_dma_start(struct driver_data *drv_data)
-{
-	DCSR(drv_data->rx_channel) |= DCSR_RUN;
-	DCSR(drv_data->tx_channel) |= DCSR_RUN;
-}
-
-int pxa2xx_spi_dma_setup(struct driver_data *drv_data)
-{
-	struct device *dev = &drv_data->pdev->dev;
-	struct ssp_device *ssp = drv_data->ssp;
-
-	/* Get two DMA channels	(rx and tx) */
-	drv_data->rx_channel = pxa_request_dma("pxa2xx_spi_ssp_rx",
-						DMA_PRIO_HIGH,
-						pxa2xx_spi_dma_handler,
-						drv_data);
-	if (drv_data->rx_channel < 0) {
-		dev_err(dev, "problem (%d) requesting rx channel\n",
-			drv_data->rx_channel);
-		return -ENODEV;
-	}
-	drv_data->tx_channel = pxa_request_dma("pxa2xx_spi_ssp_tx",
-						DMA_PRIO_MEDIUM,
-						pxa2xx_spi_dma_handler,
-						drv_data);
-	if (drv_data->tx_channel < 0) {
-		dev_err(dev, "problem (%d) requesting tx channel\n",
-			drv_data->tx_channel);
-		pxa_free_dma(drv_data->rx_channel);
-		return -ENODEV;
-	}
-
-	DRCMR(ssp->drcmr_rx) = DRCMR_MAPVLD | drv_data->rx_channel;
-	DRCMR(ssp->drcmr_tx) = DRCMR_MAPVLD | drv_data->tx_channel;
-
-	return 0;
-}
-
-void pxa2xx_spi_dma_release(struct driver_data *drv_data)
-{
-	struct ssp_device *ssp = drv_data->ssp;
-
-	DRCMR(ssp->drcmr_rx) = 0;
-	DRCMR(ssp->drcmr_tx) = 0;
-
-	if (drv_data->tx_channel != 0)
-		pxa_free_dma(drv_data->tx_channel);
-	if (drv_data->rx_channel != 0)
-		pxa_free_dma(drv_data->rx_channel);
-}
-
-void pxa2xx_spi_dma_resume(struct driver_data *drv_data)
-{
-	if (drv_data->rx_channel != -1)
-		DRCMR(drv_data->ssp->drcmr_rx) =
-			DRCMR_MAPVLD | drv_data->rx_channel;
-	if (drv_data->tx_channel != -1)
-		DRCMR(drv_data->ssp->drcmr_tx) =
-			DRCMR_MAPVLD | drv_data->tx_channel;
-}
-
-int pxa2xx_spi_set_dma_burst_and_threshold(struct chip_data *chip,
-					   struct spi_device *spi,
-					   u8 bits_per_word, u32 *burst_code,
-					   u32 *threshold)
-{
-	struct pxa2xx_spi_chip *chip_info =
-			(struct pxa2xx_spi_chip *)spi->controller_data;
-	int bytes_per_word;
-	int burst_bytes;
-	int thresh_words;
-	int req_burst_size;
-	int retval = 0;
-
-	/* Set the threshold (in registers) to equal the same amount of data
-	 * as represented by burst size (in bytes).  The computation below
-	 * is (burst_size rounded up to nearest 8 byte, word or long word)
-	 * divided by (bytes/register); the tx threshold is the inverse of
-	 * the rx, so that there will always be enough data in the rx fifo
-	 * to satisfy a burst, and there will always be enough space in the
-	 * tx fifo to accept a burst (a tx burst will overwrite the fifo if
-	 * there is not enough space), there must always remain enough empty
-	 * space in the rx fifo for any data loaded to the tx fifo.
-	 * Whenever burst_size (in bytes) equals bits/word, the fifo threshold
-	 * will be 8, or half the fifo;
-	 * The threshold can only be set to 2, 4 or 8, but not 16, because
-	 * to burst 16 to the tx fifo, the fifo would have to be empty;
-	 * however, the minimum fifo trigger level is 1, and the tx will
-	 * request service when the fifo is at this level, with only 15 spaces.
-	 */
-
-	/* find bytes/word */
-	if (bits_per_word <= 8)
-		bytes_per_word = 1;
-	else if (bits_per_word <= 16)
-		bytes_per_word = 2;
-	else
-		bytes_per_word = 4;
-
-	/* use struct pxa2xx_spi_chip->dma_burst_size if available */
-	if (chip_info)
-		req_burst_size = chip_info->dma_burst_size;
-	else {
-		switch (chip->dma_burst_size) {
-		default:
-			/* if the default burst size is not set,
-			 * do it now */
-			chip->dma_burst_size = DCMD_BURST8;
-		case DCMD_BURST8:
-			req_burst_size = 8;
-			break;
-		case DCMD_BURST16:
-			req_burst_size = 16;
-			break;
-		case DCMD_BURST32:
-			req_burst_size = 32;
-			break;
-		}
-	}
-	if (req_burst_size <= 8) {
-		*burst_code = DCMD_BURST8;
-		burst_bytes = 8;
-	} else if (req_burst_size <= 16) {
-		if (bytes_per_word == 1) {
-			/* don't burst more than 1/2 the fifo */
-			*burst_code = DCMD_BURST8;
-			burst_bytes = 8;
-			retval = 1;
-		} else {
-			*burst_code = DCMD_BURST16;
-			burst_bytes = 16;
-		}
-	} else {
-		if (bytes_per_word == 1) {
-			/* don't burst more than 1/2 the fifo */
-			*burst_code = DCMD_BURST8;
-			burst_bytes = 8;
-			retval = 1;
-		} else if (bytes_per_word == 2) {
-			/* don't burst more than 1/2 the fifo */
-			*burst_code = DCMD_BURST16;
-			burst_bytes = 16;
-			retval = 1;
-		} else {
-			*burst_code = DCMD_BURST32;
-			burst_bytes = 32;
-		}
-	}
-
-	thresh_words = burst_bytes / bytes_per_word;
-
-	/* thresh_words will be between 2 and 8 */
-	*threshold = (SSCR1_RxTresh(thresh_words) & SSCR1_RFT)
-			| (SSCR1_TxTresh(16-thresh_words) & SSCR1_TFT);
-
-	return retval;
-}
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index e3223ac75a7c..7293d6d875c5 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -60,21 +60,60 @@ MODULE_ALIAS("platform:pxa2xx-spi");
 				| QUARK_X1000_SSCR1_TFT		\
 				| SSCR1_SPH | SSCR1_SPO | SSCR1_LBM)
 
-#define LPSS_RX_THRESH_DFLT	64
-#define LPSS_TX_LOTHRESH_DFLT	160
-#define LPSS_TX_HITHRESH_DFLT	224
-
-/* Offset from drv_data->lpss_base */
-#define GENERAL_REG		0x08
 #define GENERAL_REG_RXTO_HOLDOFF_DISABLE BIT(24)
-#define SSP_REG			0x0c
-#define SPI_CS_CONTROL		0x18
 #define SPI_CS_CONTROL_SW_MODE	BIT(0)
 #define SPI_CS_CONTROL_CS_HIGH	BIT(1)
 
+struct lpss_config {
+	/* LPSS offset from drv_data->ioaddr */
+	unsigned offset;
+	/* Register offsets from drv_data->lpss_base or -1 */
+	int reg_general;
+	int reg_ssp;
+	int reg_cs_ctrl;
+	/* FIFO thresholds */
+	u32 rx_threshold;
+	u32 tx_threshold_lo;
+	u32 tx_threshold_hi;
+};
+
+/* Keep these sorted with enum pxa_ssp_type */
+static const struct lpss_config lpss_platforms[] = {
+	{	/* LPSS_LPT_SSP */
+		.offset = 0x800,
+		.reg_general = 0x08,
+		.reg_ssp = 0x0c,
+		.reg_cs_ctrl = 0x18,
+		.rx_threshold = 64,
+		.tx_threshold_lo = 160,
+		.tx_threshold_hi = 224,
+	},
+	{	/* LPSS_BYT_SSP */
+		.offset = 0x400,
+		.reg_general = 0x08,
+		.reg_ssp = 0x0c,
+		.reg_cs_ctrl = 0x18,
+		.rx_threshold = 64,
+		.tx_threshold_lo = 160,
+		.tx_threshold_hi = 224,
+	},
+};
+
+static inline const struct lpss_config
+*lpss_get_config(const struct driver_data *drv_data)
+{
+	return &lpss_platforms[drv_data->ssp_type - LPSS_LPT_SSP];
+}
+
 static bool is_lpss_ssp(const struct driver_data *drv_data)
 {
-	return drv_data->ssp_type == LPSS_SSP;
+	switch (drv_data->ssp_type) {
+	case LPSS_LPT_SSP:
+	case LPSS_BYT_SSP:
+		return true;
+	default:
+		return false;
+	}
 }
 
 static bool is_quark_x1000_ssp(const struct driver_data *drv_data)
@@ -192,63 +231,43 @@ static void __lpss_ssp_write_priv(struct driver_data *drv_data,
  */
 static void lpss_ssp_setup(struct driver_data *drv_data)
 {
-	unsigned offset = 0x400;
-	u32 value, orig;
-
-	/*
-	 * Perform auto-detection of the LPSS SSP private registers. They
-	 * can be either at 1k or 2k offset from the base address.
-	 */
-	orig = readl(drv_data->ioaddr + offset + SPI_CS_CONTROL);
-
-	/* Test SPI_CS_CONTROL_SW_MODE bit enabling */
-	value = orig | SPI_CS_CONTROL_SW_MODE;
-	writel(value, drv_data->ioaddr + offset + SPI_CS_CONTROL);
-	value = readl(drv_data->ioaddr + offset + SPI_CS_CONTROL);
-	if (value != (orig | SPI_CS_CONTROL_SW_MODE)) {
-		offset = 0x800;
-		goto detection_done;
-	}
-
-	orig = readl(drv_data->ioaddr + offset + SPI_CS_CONTROL);
-
-	/* Test SPI_CS_CONTROL_SW_MODE bit disabling */
-	value = orig & ~SPI_CS_CONTROL_SW_MODE;
-	writel(value, drv_data->ioaddr + offset + SPI_CS_CONTROL);
-	value = readl(drv_data->ioaddr + offset + SPI_CS_CONTROL);
-	if (value != (orig & ~SPI_CS_CONTROL_SW_MODE)) {
-		offset = 0x800;
-		goto detection_done;
-	}
+	const struct lpss_config *config;
+	u32 value;
 
-detection_done:
-	/* Now set the LPSS base */
-	drv_data->lpss_base = drv_data->ioaddr + offset;
+	config = lpss_get_config(drv_data);
+	drv_data->lpss_base = drv_data->ioaddr + config->offset;
 
 	/* Enable software chip select control */
 	value = SPI_CS_CONTROL_SW_MODE | SPI_CS_CONTROL_CS_HIGH;
-	__lpss_ssp_write_priv(drv_data, SPI_CS_CONTROL, value);
+	__lpss_ssp_write_priv(drv_data, config->reg_cs_ctrl, value);
 
 	/* Enable multiblock DMA transfers */
 	if (drv_data->master_info->enable_dma) {
-		__lpss_ssp_write_priv(drv_data, SSP_REG, 1);
-
-		value = __lpss_ssp_read_priv(drv_data, GENERAL_REG);
-		value |= GENERAL_REG_RXTO_HOLDOFF_DISABLE;
-		__lpss_ssp_write_priv(drv_data, GENERAL_REG, value);
+		__lpss_ssp_write_priv(drv_data, config->reg_ssp, 1);
+
+		if (config->reg_general >= 0) {
+			value = __lpss_ssp_read_priv(drv_data,
+						     config->reg_general);
+			value |= GENERAL_REG_RXTO_HOLDOFF_DISABLE;
+			__lpss_ssp_write_priv(drv_data,
+					      config->reg_general, value);
+		}
 	}
 }
 
 static void lpss_ssp_cs_control(struct driver_data *drv_data, bool enable)
 {
+	const struct lpss_config *config;
 	u32 value;
 
-	value = __lpss_ssp_read_priv(drv_data, SPI_CS_CONTROL);
+	config = lpss_get_config(drv_data);
+
+	value = __lpss_ssp_read_priv(drv_data, config->reg_cs_ctrl);
 	if (enable)
 		value &= ~SPI_CS_CONTROL_CS_HIGH;
 	else
 		value |= SPI_CS_CONTROL_CS_HIGH;
-	__lpss_ssp_write_priv(drv_data, SPI_CS_CONTROL, value);
+	__lpss_ssp_write_priv(drv_data, config->reg_cs_ctrl, value);
 }
 
 static void cs_assert(struct driver_data *drv_data)
@@ -1075,6 +1094,7 @@ static int setup(struct spi_device *spi)
 {
 	struct pxa2xx_spi_chip *chip_info = NULL;
 	struct chip_data *chip;
+	const struct lpss_config *config;
 	struct driver_data *drv_data = spi_master_get_devdata(spi->master);
 	unsigned int clk_div;
 	uint tx_thres, tx_hi_thres, rx_thres;
@@ -1085,10 +1105,12 @@ static int setup(struct spi_device *spi)
 		tx_hi_thres = 0;
 		rx_thres = RX_THRESH_QUARK_X1000_DFLT;
 		break;
-	case LPSS_SSP:
-		tx_thres = LPSS_TX_LOTHRESH_DFLT;
-		tx_hi_thres = LPSS_TX_HITHRESH_DFLT;
-		rx_thres = LPSS_RX_THRESH_DFLT;
+	case LPSS_LPT_SSP:
+	case LPSS_BYT_SSP:
+		config = lpss_get_config(drv_data);
+		tx_thres = config->tx_threshold_lo;
+		tx_hi_thres = config->tx_threshold_hi;
+		rx_thres = config->rx_threshold;
 		break;
 	default:
 		tx_thres = TX_THRESH_DFLT;
@@ -1242,6 +1264,18 @@ static void cleanup(struct spi_device *spi)
 }
 
 #ifdef CONFIG_ACPI
+
+static const struct acpi_device_id pxa2xx_spi_acpi_match[] = {
+	{ "INT33C0", LPSS_LPT_SSP },
+	{ "INT33C1", LPSS_LPT_SSP },
+	{ "INT3430", LPSS_LPT_SSP },
+	{ "INT3431", LPSS_LPT_SSP },
+	{ "80860F0E", LPSS_BYT_SSP },
+	{ "8086228E", LPSS_BYT_SSP },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, pxa2xx_spi_acpi_match);
+
 static struct pxa2xx_spi_master *
 pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
 {
@@ -1249,12 +1283,19 @@ pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
 	struct acpi_device *adev;
 	struct ssp_device *ssp;
 	struct resource *res;
-	int devid;
+	const struct acpi_device_id *id;
+	int devid, type;
 
 	if (!ACPI_HANDLE(&pdev->dev) ||
 	    acpi_bus_get_device(ACPI_HANDLE(&pdev->dev), &adev))
 		return NULL;
 
+	id = acpi_match_device(pdev->dev.driver->acpi_match_table, &pdev->dev);
+	if (id)
+		type = (int)id->driver_data;
+	else
+		return NULL;
+
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return NULL;
@@ -1272,7 +1313,7 @@ pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
 
 	ssp->clk = devm_clk_get(&pdev->dev, NULL);
 	ssp->irq = platform_get_irq(pdev, 0);
-	ssp->type = LPSS_SSP;
+	ssp->type = type;
 	ssp->pdev = pdev;
 
 	ssp->port_id = -1;
@@ -1285,16 +1326,6 @@ pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
 	return pdata;
 }
 
-static struct acpi_device_id pxa2xx_spi_acpi_match[] = {
-	{ "INT33C0", 0 },
-	{ "INT33C1", 0 },
-	{ "INT3430", 0 },
-	{ "INT3431", 0 },
-	{ "80860F0E", 0 },
-	{ "8086228E", 0 },
-	{ },
-};
-MODULE_DEVICE_TABLE(acpi, pxa2xx_spi_acpi_match);
 #else
 static inline struct pxa2xx_spi_master *
 pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
diff --git a/drivers/spi/spi-pxa2xx.h b/drivers/spi/spi-pxa2xx.h
index 85a58c906869..9f01e9c9aa75 100644
--- a/drivers/spi/spi-pxa2xx.h
+++ b/drivers/spi/spi-pxa2xx.h
@@ -162,11 +162,7 @@ extern void *pxa2xx_spi_next_transfer(struct driver_data *drv_data);
 /*
  * Select the right DMA implementation.
  */
-#if defined(CONFIG_SPI_PXA2XX_PXADMA)
-#define SPI_PXA2XX_USE_DMA	1
-#define MAX_DMA_LEN		8191
-#define DEFAULT_DMA_CR1		(SSCR1_TSRE | SSCR1_RSRE | SSCR1_TINTE)
-#elif defined(CONFIG_SPI_PXA2XX_DMA)
+#if defined(CONFIG_SPI_PXA2XX_DMA)
 #define SPI_PXA2XX_USE_DMA	1
 #define MAX_DMA_LEN		SZ_64K
 #define DEFAULT_DMA_CR1		(SSCR1_TSRE | SSCR1_RSRE | SSCR1_TRAIL)
diff --git a/drivers/spi/spi-rb4xx.c b/drivers/spi/spi-rb4xx.c
new file mode 100644
index 000000000000..3641d0e20135
--- /dev/null
+++ b/drivers/spi/spi-rb4xx.c
@@ -0,0 +1,210 @@
+/*
+ * SPI controller driver for the Mikrotik RB4xx boards
+ *
+ * Copyright (C) 2010 Gabor Juhos <juhosg@openwrt.org>
+ * Copyright (C) 2015 Bert Vermeulen <bert@biot.com>
+ *
+ * This file was based on the patches for Linux 2.6.27.39 published by
+ * MikroTik for their RouterBoard 4xx series devices.
+ *
+ * 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/platform_device.h>
+#include <linux/clk.h>
+#include <linux/spi/spi.h>
+
+#include <asm/mach-ath79/ar71xx_regs.h>
+
+struct rb4xx_spi {
+	void __iomem *base;
+	struct clk *clk;
+};
+
+static inline u32 rb4xx_read(struct rb4xx_spi *rbspi, u32 reg)
+{
+	return __raw_readl(rbspi->base + reg);
+}
+
+static inline void rb4xx_write(struct rb4xx_spi *rbspi, u32 reg, u32 value)
+{
+	__raw_writel(value, rbspi->base + reg);
+}
+
+static inline void do_spi_clk(struct rb4xx_spi *rbspi, u32 spi_ioc, int value)
+{
+	u32 regval;
+
+	regval = spi_ioc;
+	if (value & BIT(0))
+		regval |= AR71XX_SPI_IOC_DO;
+
+	rb4xx_write(rbspi, AR71XX_SPI_REG_IOC, regval);
+	rb4xx_write(rbspi, AR71XX_SPI_REG_IOC, regval | AR71XX_SPI_IOC_CLK);
+}
+
+static void do_spi_byte(struct rb4xx_spi *rbspi, u32 spi_ioc, u8 byte)
+{
+	int i;
+
+	for (i = 7; i >= 0; i--)
+		do_spi_clk(rbspi, spi_ioc, byte >> i);
+}
+
+/* The CS2 pin is used to clock in a second bit per clock cycle. */
+static inline void do_spi_clk_two(struct rb4xx_spi *rbspi, u32 spi_ioc,
+				   u8 value)
+{
+	u32 regval;
+
+	regval = spi_ioc;
+	if (value & BIT(1))
+		regval |= AR71XX_SPI_IOC_DO;
+	if (value & BIT(0))
+		regval |= AR71XX_SPI_IOC_CS2;
+
+	rb4xx_write(rbspi, AR71XX_SPI_REG_IOC, regval);
+	rb4xx_write(rbspi, AR71XX_SPI_REG_IOC, regval | AR71XX_SPI_IOC_CLK);
+}
+
+/* Two bits at a time, msb first */
+static void do_spi_byte_two(struct rb4xx_spi *rbspi, u32 spi_ioc, u8 byte)
+{
+	do_spi_clk_two(rbspi, spi_ioc, byte >> 6);
+	do_spi_clk_two(rbspi, spi_ioc, byte >> 4);
+	do_spi_clk_two(rbspi, spi_ioc, byte >> 2);
+	do_spi_clk_two(rbspi, spi_ioc, byte >> 0);
+}
+
+static void rb4xx_set_cs(struct spi_device *spi, bool enable)
+{
+	struct rb4xx_spi *rbspi = spi_master_get_devdata(spi->master);
+
+	/*
+	 * Setting CS is done along with bitbanging the actual values,
+	 * since it's all on the same hardware register. However the
+	 * CPLD needs CS deselected after every command.
+	 */
+	if (enable)
+		rb4xx_write(rbspi, AR71XX_SPI_REG_IOC,
+			    AR71XX_SPI_IOC_CS0 | AR71XX_SPI_IOC_CS1);
+}
+
+static int rb4xx_transfer_one(struct spi_master *master,
+			      struct spi_device *spi, struct spi_transfer *t)
+{
+	struct rb4xx_spi *rbspi = spi_master_get_devdata(master);
+	int i;
+	u32 spi_ioc;
+	u8 *rx_buf;
+	const u8 *tx_buf;
+
+	/*
+	 * Prime the SPI register with the SPI device selected. The m25p80 boot
+	 * flash and CPLD share the CS0 pin. This works because the CPLD's
+	 * command set was designed to almost not clash with that of the
+	 * boot flash.
+	 */
+	if (spi->chip_select == 2)
+		/* MMC */
+		spi_ioc = AR71XX_SPI_IOC_CS0;
+	else
+		/* Boot flash and CPLD */
+		spi_ioc = AR71XX_SPI_IOC_CS1;
+
+	tx_buf = t->tx_buf;
+	rx_buf = t->rx_buf;
+	for (i = 0; i < t->len; ++i) {
+		if (t->tx_nbits == SPI_NBITS_DUAL)
+			/* CPLD can use two-wire transfers */
+			do_spi_byte_two(rbspi, spi_ioc, tx_buf[i]);
+		else
+			do_spi_byte(rbspi, spi_ioc, tx_buf[i]);
+		if (!rx_buf)
+			continue;
+		rx_buf[i] = rb4xx_read(rbspi, AR71XX_SPI_REG_RDS);
+	}
+	spi_finalize_current_transfer(master);
+
+	return 0;
+}
+
+static int rb4xx_spi_probe(struct platform_device *pdev)
+{
+	struct spi_master *master;
+	struct clk *ahb_clk;
+	struct rb4xx_spi *rbspi;
+	struct resource *r;
+	int err;
+	void __iomem *spi_base;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	spi_base = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(spi_base))
+		return PTR_ERR(spi_base);
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*rbspi));
+	if (!master)
+		return -ENOMEM;
+
+	ahb_clk = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(ahb_clk))
+		return PTR_ERR(ahb_clk);
+
+	master->bus_num = 0;
+	master->num_chipselect = 3;
+	master->mode_bits = SPI_TX_DUAL;
+	master->bits_per_word_mask = BIT(7);
+	master->flags = SPI_MASTER_MUST_TX;
+	master->transfer_one = rb4xx_transfer_one;
+	master->set_cs = rb4xx_set_cs;
+
+	err = devm_spi_register_master(&pdev->dev, master);
+	if (err) {
+		dev_err(&pdev->dev, "failed to register SPI master\n");
+		return err;
+	}
+
+	err = clk_prepare_enable(ahb_clk);
+	if (err)
+		return err;
+
+	rbspi = spi_master_get_devdata(master);
+	rbspi->base = spi_base;
+	rbspi->clk = ahb_clk;
+	platform_set_drvdata(pdev, rbspi);
+
+	/* Enable SPI */
+	rb4xx_write(rbspi, AR71XX_SPI_REG_FS, AR71XX_SPI_FS_GPIO);
+
+	return 0;
+}
+
+static int rb4xx_spi_remove(struct platform_device *pdev)
+{
+	struct rb4xx_spi *rbspi = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(rbspi->clk);
+
+	return 0;
+}
+
+static struct platform_driver rb4xx_spi_drv = {
+	.probe = rb4xx_spi_probe,
+	.remove = rb4xx_spi_remove,
+	.driver = {
+		.name = "rb4xx-spi",
+	},
+};
+
+module_platform_driver(rb4xx_spi_drv);
+
+MODULE_DESCRIPTION("Mikrotik RB4xx SPI controller driver");
+MODULE_AUTHOR("Gabor Juhos <juhosg@openwrt.org>");
+MODULE_AUTHOR("Bert Vermeulen <bert@biot.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-rspi.c b/drivers/spi/spi-rspi.c
index f6bac9e77d06..f9189a0c8cec 100644
--- a/drivers/spi/spi-rspi.c
+++ b/drivers/spi/spi-rspi.c
@@ -665,15 +665,12 @@ static bool rspi_can_dma(struct spi_master *master, struct spi_device *spi,
 static int rspi_dma_check_then_transfer(struct rspi_data *rspi,
 					 struct spi_transfer *xfer)
 {
-	if (rspi->master->can_dma && __rspi_can_dma(rspi, xfer)) {
-		/* rx_buf can be NULL on RSPI on SH in TX-only Mode */
-		int ret = rspi_dma_transfer(rspi, &xfer->tx_sg,
-					xfer->rx_buf ? &xfer->rx_sg : NULL);
-		if (ret != -EAGAIN)
-			return 0;
-	}
+	if (!rspi->master->can_dma || !__rspi_can_dma(rspi, xfer))
+		return -EAGAIN;
 
-	return -EAGAIN;
+	/* rx_buf can be NULL on RSPI on SH in TX-only Mode */
+	return rspi_dma_transfer(rspi, &xfer->tx_sg,
+				xfer->rx_buf ? &xfer->rx_sg : NULL);
 }
 
 static int rspi_common_transfer(struct rspi_data *rspi,
@@ -724,7 +721,7 @@ static int rspi_rz_transfer_one(struct spi_master *master,
 	return rspi_common_transfer(rspi, xfer);
 }
 
-static int qspi_trigger_transfer_out_int(struct rspi_data *rspi, const u8 *tx,
+static int qspi_trigger_transfer_out_in(struct rspi_data *rspi, const u8 *tx,
 					u8 *rx, unsigned int len)
 {
 	int i, n, ret;
@@ -771,12 +768,8 @@ static int qspi_transfer_out_in(struct rspi_data *rspi,
 	if (ret != -EAGAIN)
 		return ret;
 
-	ret = qspi_trigger_transfer_out_int(rspi, xfer->tx_buf,
+	return qspi_trigger_transfer_out_in(rspi, xfer->tx_buf,
 					    xfer->rx_buf, xfer->len);
-	if (ret < 0)
-		return ret;
-
-	return 0;
 }
 
 static int qspi_transfer_out(struct rspi_data *rspi, struct spi_transfer *xfer)
@@ -1300,7 +1293,7 @@ error1:
 	return ret;
 }
 
-static struct platform_device_id spi_driver_ids[] = {
+static const struct platform_device_id spi_driver_ids[] = {
 	{ "rspi",	(kernel_ulong_t)&rspi_ops },
 	{ "rspi-rz",	(kernel_ulong_t)&rspi_rz_ops },
 	{ "qspi",	(kernel_ulong_t)&qspi_ops },
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index b1c6731fbf27..2a8c513c4d07 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -1347,7 +1347,7 @@ static struct s3c64xx_spi_port_config exynos7_spi_port_config = {
 	.quirks		= S3C64XX_SPI_QUIRK_CS_AUTO,
 };
 
-static struct platform_device_id s3c64xx_spi_driver_ids[] = {
+static const struct platform_device_id s3c64xx_spi_driver_ids[] = {
 	{
 		.name		= "s3c2443-spi",
 		.driver_data	= (kernel_ulong_t)&s3c2443_spi_port_config,
diff --git a/drivers/spi/spi-sh-msiof.c b/drivers/spi/spi-sh-msiof.c
index bcc7c635d8e7..d3370a612d84 100644
--- a/drivers/spi/spi-sh-msiof.c
+++ b/drivers/spi/spi-sh-msiof.c
@@ -1263,7 +1263,7 @@ static int sh_msiof_spi_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id spi_driver_ids[] = {
+static const struct platform_device_id spi_driver_ids[] = {
 	{ "spi_sh_msiof",	(kernel_ulong_t)&sh_data },
 	{ "spi_r8a7790_msiof",	(kernel_ulong_t)&r8a779x_data },
 	{ "spi_r8a7791_msiof",	(kernel_ulong_t)&r8a779x_data },
diff --git a/drivers/spi/spi-sirf.c b/drivers/spi/spi-sirf.c
index f5715c9f68b0..7072276ad354 100644
--- a/drivers/spi/spi-sirf.c
+++ b/drivers/spi/spi-sirf.c
@@ -26,28 +26,6 @@
 #include <linux/reset.h>
 
 #define DRIVER_NAME "sirfsoc_spi"
-
-#define SIRFSOC_SPI_CTRL		0x0000
-#define SIRFSOC_SPI_CMD			0x0004
-#define SIRFSOC_SPI_TX_RX_EN		0x0008
-#define SIRFSOC_SPI_INT_EN		0x000C
-#define SIRFSOC_SPI_INT_STATUS		0x0010
-#define SIRFSOC_SPI_TX_DMA_IO_CTRL	0x0100
-#define SIRFSOC_SPI_TX_DMA_IO_LEN	0x0104
-#define SIRFSOC_SPI_TXFIFO_CTRL		0x0108
-#define SIRFSOC_SPI_TXFIFO_LEVEL_CHK	0x010C
-#define SIRFSOC_SPI_TXFIFO_OP		0x0110
-#define SIRFSOC_SPI_TXFIFO_STATUS	0x0114
-#define SIRFSOC_SPI_TXFIFO_DATA		0x0118
-#define SIRFSOC_SPI_RX_DMA_IO_CTRL	0x0120
-#define SIRFSOC_SPI_RX_DMA_IO_LEN	0x0124
-#define SIRFSOC_SPI_RXFIFO_CTRL		0x0128
-#define SIRFSOC_SPI_RXFIFO_LEVEL_CHK	0x012C
-#define SIRFSOC_SPI_RXFIFO_OP		0x0130
-#define SIRFSOC_SPI_RXFIFO_STATUS	0x0134
-#define SIRFSOC_SPI_RXFIFO_DATA		0x0138
-#define SIRFSOC_SPI_DUMMY_DELAY_CTL	0x0144
-
 /* SPI CTRL register defines */
 #define SIRFSOC_SPI_SLV_MODE		BIT(16)
 #define SIRFSOC_SPI_CMD_MODE		BIT(17)
@@ -80,8 +58,6 @@
 #define SIRFSOC_SPI_TXFIFO_THD_INT_EN	BIT(9)
 #define SIRFSOC_SPI_FRM_END_INT_EN	BIT(10)
 
-#define SIRFSOC_SPI_INT_MASK_ALL	0x1FFF
-
 /* Interrupt status */
 #define SIRFSOC_SPI_RX_DONE		BIT(0)
 #define SIRFSOC_SPI_TX_DONE		BIT(1)
@@ -110,20 +86,66 @@
 #define SIRFSOC_SPI_FIFO_WIDTH_BYTE	(0 << 0)
 #define SIRFSOC_SPI_FIFO_WIDTH_WORD	(1 << 0)
 #define SIRFSOC_SPI_FIFO_WIDTH_DWORD	(2 << 0)
-
-/* FIFO Status */
-#define	SIRFSOC_SPI_FIFO_LEVEL_MASK	0xFF
-#define SIRFSOC_SPI_FIFO_FULL		BIT(8)
-#define SIRFSOC_SPI_FIFO_EMPTY		BIT(9)
-
-/* 256 bytes rx/tx FIFO */
-#define SIRFSOC_SPI_FIFO_SIZE		256
-#define SIRFSOC_SPI_DAT_FRM_LEN_MAX	(64 * 1024)
-
-#define SIRFSOC_SPI_FIFO_SC(x)		((x) & 0x3F)
-#define SIRFSOC_SPI_FIFO_LC(x)		(((x) & 0x3F) << 10)
-#define SIRFSOC_SPI_FIFO_HC(x)		(((x) & 0x3F) << 20)
-#define SIRFSOC_SPI_FIFO_THD(x)		(((x) & 0xFF) << 2)
+/* USP related */
+#define SIRFSOC_USP_SYNC_MODE		BIT(0)
+#define SIRFSOC_USP_SLV_MODE		BIT(1)
+#define SIRFSOC_USP_LSB			BIT(4)
+#define SIRFSOC_USP_EN			BIT(5)
+#define SIRFSOC_USP_RXD_FALLING_EDGE	BIT(6)
+#define SIRFSOC_USP_TXD_FALLING_EDGE	BIT(7)
+#define SIRFSOC_USP_CS_HIGH_VALID	BIT(9)
+#define SIRFSOC_USP_SCLK_IDLE_STAT	BIT(11)
+#define SIRFSOC_USP_TFS_IO_MODE		BIT(14)
+#define SIRFSOC_USP_TFS_IO_INPUT	BIT(19)
+
+#define SIRFSOC_USP_RXD_DELAY_LEN_MASK	0xFF
+#define SIRFSOC_USP_TXD_DELAY_LEN_MASK	0xFF
+#define SIRFSOC_USP_RXD_DELAY_OFFSET	0
+#define SIRFSOC_USP_TXD_DELAY_OFFSET	8
+#define SIRFSOC_USP_RXD_DELAY_LEN	1
+#define SIRFSOC_USP_TXD_DELAY_LEN	1
+#define SIRFSOC_USP_CLK_DIVISOR_OFFSET	21
+#define SIRFSOC_USP_CLK_DIVISOR_MASK	0x3FF
+#define SIRFSOC_USP_CLK_10_11_MASK	0x3
+#define SIRFSOC_USP_CLK_10_11_OFFSET	30
+#define SIRFSOC_USP_CLK_12_15_MASK	0xF
+#define SIRFSOC_USP_CLK_12_15_OFFSET	24
+
+#define SIRFSOC_USP_TX_DATA_OFFSET	0
+#define SIRFSOC_USP_TX_SYNC_OFFSET	8
+#define SIRFSOC_USP_TX_FRAME_OFFSET	16
+#define SIRFSOC_USP_TX_SHIFTER_OFFSET	24
+
+#define SIRFSOC_USP_TX_DATA_MASK	0xFF
+#define SIRFSOC_USP_TX_SYNC_MASK	0xFF
+#define SIRFSOC_USP_TX_FRAME_MASK	0xFF
+#define SIRFSOC_USP_TX_SHIFTER_MASK	0x1F
+
+#define SIRFSOC_USP_RX_DATA_OFFSET	0
+#define SIRFSOC_USP_RX_FRAME_OFFSET	8
+#define SIRFSOC_USP_RX_SHIFTER_OFFSET	16
+
+#define SIRFSOC_USP_RX_DATA_MASK	0xFF
+#define SIRFSOC_USP_RX_FRAME_MASK	0xFF
+#define SIRFSOC_USP_RX_SHIFTER_MASK	0x1F
+#define SIRFSOC_USP_CS_HIGH_VALUE	BIT(1)
+
+#define SIRFSOC_SPI_FIFO_SC_OFFSET	0
+#define SIRFSOC_SPI_FIFO_LC_OFFSET	10
+#define SIRFSOC_SPI_FIFO_HC_OFFSET	20
+
+#define SIRFSOC_SPI_FIFO_FULL_MASK(s)	(1 << ((s)->fifo_full_offset))
+#define SIRFSOC_SPI_FIFO_EMPTY_MASK(s)	(1 << ((s)->fifo_full_offset + 1))
+#define SIRFSOC_SPI_FIFO_THD_MASK(s)	((s)->fifo_size - 1)
+#define SIRFSOC_SPI_FIFO_THD_OFFSET	2
+#define SIRFSOC_SPI_FIFO_LEVEL_CHK_MASK(s, val)	\
+	((val) & (s)->fifo_level_chk_mask)
+
+enum sirf_spi_type {
+	SIRF_REAL_SPI,
+	SIRF_USP_SPI_P2,
+	SIRF_USP_SPI_A7,
+};
 
 /*
  * only if the rx/tx buffer and transfer size are 4-bytes aligned, we use dma
@@ -137,6 +159,95 @@
 #define SIRFSOC_MAX_CMD_BYTES	4
 #define SIRFSOC_SPI_DEFAULT_FRQ 1000000
 
+struct sirf_spi_register {
+	/*SPI and USP-SPI common*/
+	u32 tx_rx_en;
+	u32 int_en;
+	u32 int_st;
+	u32 tx_dma_io_ctrl;
+	u32 tx_dma_io_len;
+	u32 txfifo_ctrl;
+	u32 txfifo_level_chk;
+	u32 txfifo_op;
+	u32 txfifo_st;
+	u32 txfifo_data;
+	u32 rx_dma_io_ctrl;
+	u32 rx_dma_io_len;
+	u32 rxfifo_ctrl;
+	u32 rxfifo_level_chk;
+	u32 rxfifo_op;
+	u32 rxfifo_st;
+	u32 rxfifo_data;
+	/*SPI self*/
+	u32 spi_ctrl;
+	u32 spi_cmd;
+	u32 spi_dummy_delay_ctrl;
+	/*USP-SPI self*/
+	u32 usp_mode1;
+	u32 usp_mode2;
+	u32 usp_tx_frame_ctrl;
+	u32 usp_rx_frame_ctrl;
+	u32 usp_pin_io_data;
+	u32 usp_risc_dsp_mode;
+	u32 usp_async_param_reg;
+	u32 usp_irda_x_mode_div;
+	u32 usp_sm_cfg;
+	u32 usp_int_en_clr;
+};
+
+static const struct sirf_spi_register real_spi_register = {
+	.tx_rx_en		= 0x8,
+	.int_en		= 0xc,
+	.int_st		= 0x10,
+	.tx_dma_io_ctrl	= 0x100,
+	.tx_dma_io_len	= 0x104,
+	.txfifo_ctrl	= 0x108,
+	.txfifo_level_chk	= 0x10c,
+	.txfifo_op		= 0x110,
+	.txfifo_st		= 0x114,
+	.txfifo_data	= 0x118,
+	.rx_dma_io_ctrl	= 0x120,
+	.rx_dma_io_len	= 0x124,
+	.rxfifo_ctrl	= 0x128,
+	.rxfifo_level_chk	= 0x12c,
+	.rxfifo_op		= 0x130,
+	.rxfifo_st		= 0x134,
+	.rxfifo_data	= 0x138,
+	.spi_ctrl		= 0x0,
+	.spi_cmd		= 0x4,
+	.spi_dummy_delay_ctrl	= 0x144,
+};
+
+static const struct sirf_spi_register usp_spi_register = {
+	.tx_rx_en		= 0x10,
+	.int_en		= 0x14,
+	.int_st		= 0x18,
+	.tx_dma_io_ctrl	= 0x100,
+	.tx_dma_io_len	= 0x104,
+	.txfifo_ctrl	= 0x108,
+	.txfifo_level_chk	= 0x10c,
+	.txfifo_op		= 0x110,
+	.txfifo_st		= 0x114,
+	.txfifo_data	= 0x118,
+	.rx_dma_io_ctrl	= 0x120,
+	.rx_dma_io_len	= 0x124,
+	.rxfifo_ctrl	= 0x128,
+	.rxfifo_level_chk	= 0x12c,
+	.rxfifo_op		= 0x130,
+	.rxfifo_st		= 0x134,
+	.rxfifo_data	= 0x138,
+	.usp_mode1		= 0x0,
+	.usp_mode2		= 0x4,
+	.usp_tx_frame_ctrl	= 0x8,
+	.usp_rx_frame_ctrl	= 0xc,
+	.usp_pin_io_data	= 0x1c,
+	.usp_risc_dsp_mode	= 0x20,
+	.usp_async_param_reg	= 0x24,
+	.usp_irda_x_mode_div	= 0x28,
+	.usp_sm_cfg		= 0x2c,
+	.usp_int_en_clr		= 0x140,
+};
+
 struct sirfsoc_spi {
 	struct spi_bitbang bitbang;
 	struct completion rx_done;
@@ -164,7 +275,6 @@ struct sirfsoc_spi {
 	struct dma_chan *tx_chan;
 	dma_addr_t src_start;
 	dma_addr_t dst_start;
-	void *dummypage;
 	int word_width; /* in bytes */
 
 	/*
@@ -173,14 +283,39 @@ struct sirfsoc_spi {
 	 */
 	bool	tx_by_cmd;
 	bool	hw_cs;
+	enum sirf_spi_type type;
+	const struct sirf_spi_register *regs;
+	unsigned int fifo_size;
+	/* fifo empty offset is (fifo full offset + 1)*/
+	unsigned int fifo_full_offset;
+	/* fifo_level_chk_mask is (fifo_size/4 - 1) */
+	unsigned int fifo_level_chk_mask;
+	unsigned int dat_max_frm_len;
+};
+
+struct sirf_spi_comp_data {
+	const struct sirf_spi_register *regs;
+	enum sirf_spi_type type;
+	unsigned int dat_max_frm_len;
+	unsigned int fifo_size;
+	void (*hwinit)(struct sirfsoc_spi *sspi);
 };
 
+static void sirfsoc_usp_hwinit(struct sirfsoc_spi *sspi)
+{
+	/* reset USP and let USP can operate */
+	writel(readl(sspi->base + sspi->regs->usp_mode1) &
+		~SIRFSOC_USP_EN, sspi->base + sspi->regs->usp_mode1);
+	writel(readl(sspi->base + sspi->regs->usp_mode1) |
+		SIRFSOC_USP_EN, sspi->base + sspi->regs->usp_mode1);
+}
+
 static void spi_sirfsoc_rx_word_u8(struct sirfsoc_spi *sspi)
 {
 	u32 data;
 	u8 *rx = sspi->rx;
 
-	data = readl(sspi->base + SIRFSOC_SPI_RXFIFO_DATA);
+	data = readl(sspi->base + sspi->regs->rxfifo_data);
 
 	if (rx) {
 		*rx++ = (u8) data;
@@ -199,8 +334,7 @@ static void spi_sirfsoc_tx_word_u8(struct sirfsoc_spi *sspi)
 		data = *tx++;
 		sspi->tx = tx;
 	}
-
-	writel(data, sspi->base + SIRFSOC_SPI_TXFIFO_DATA);
+	writel(data, sspi->base + sspi->regs->txfifo_data);
 	sspi->left_tx_word--;
 }
 
@@ -209,7 +343,7 @@ static void spi_sirfsoc_rx_word_u16(struct sirfsoc_spi *sspi)
 	u32 data;
 	u16 *rx = sspi->rx;
 
-	data = readl(sspi->base + SIRFSOC_SPI_RXFIFO_DATA);
+	data = readl(sspi->base + sspi->regs->rxfifo_data);
 
 	if (rx) {
 		*rx++ = (u16) data;
@@ -229,7 +363,7 @@ static void spi_sirfsoc_tx_word_u16(struct sirfsoc_spi *sspi)
 		sspi->tx = tx;
 	}
 
-	writel(data, sspi->base + SIRFSOC_SPI_TXFIFO_DATA);
+	writel(data, sspi->base + sspi->regs->txfifo_data);
 	sspi->left_tx_word--;
 }
 
@@ -238,7 +372,7 @@ static void spi_sirfsoc_rx_word_u32(struct sirfsoc_spi *sspi)
 	u32 data;
 	u32 *rx = sspi->rx;
 
-	data = readl(sspi->base + SIRFSOC_SPI_RXFIFO_DATA);
+	data = readl(sspi->base + sspi->regs->rxfifo_data);
 
 	if (rx) {
 		*rx++ = (u32) data;
@@ -259,41 +393,59 @@ static void spi_sirfsoc_tx_word_u32(struct sirfsoc_spi *sspi)
 		sspi->tx = tx;
 	}
 
-	writel(data, sspi->base + SIRFSOC_SPI_TXFIFO_DATA);
+	writel(data, sspi->base + sspi->regs->txfifo_data);
 	sspi->left_tx_word--;
 }
 
 static irqreturn_t spi_sirfsoc_irq(int irq, void *dev_id)
 {
 	struct sirfsoc_spi *sspi = dev_id;
-	u32 spi_stat = readl(sspi->base + SIRFSOC_SPI_INT_STATUS);
-	if (sspi->tx_by_cmd && (spi_stat & SIRFSOC_SPI_FRM_END)) {
+	u32 spi_stat;
+
+	spi_stat = readl(sspi->base + sspi->regs->int_st);
+	if (sspi->tx_by_cmd && sspi->type == SIRF_REAL_SPI
+		&& (spi_stat & SIRFSOC_SPI_FRM_END)) {
 		complete(&sspi->tx_done);
-		writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
-		writel(SIRFSOC_SPI_INT_MASK_ALL,
-				sspi->base + SIRFSOC_SPI_INT_STATUS);
+		writel(0x0, sspi->base + sspi->regs->int_en);
+		writel(readl(sspi->base + sspi->regs->int_st),
+				sspi->base + sspi->regs->int_st);
 		return IRQ_HANDLED;
 	}
-
 	/* Error Conditions */
 	if (spi_stat & SIRFSOC_SPI_RX_OFLOW ||
 			spi_stat & SIRFSOC_SPI_TX_UFLOW) {
 		complete(&sspi->tx_done);
 		complete(&sspi->rx_done);
-		writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
-		writel(SIRFSOC_SPI_INT_MASK_ALL,
-				sspi->base + SIRFSOC_SPI_INT_STATUS);
+		switch (sspi->type) {
+		case SIRF_REAL_SPI:
+		case SIRF_USP_SPI_P2:
+			writel(0x0, sspi->base + sspi->regs->int_en);
+			break;
+		case SIRF_USP_SPI_A7:
+			writel(~0UL, sspi->base + sspi->regs->usp_int_en_clr);
+			break;
+		}
+		writel(readl(sspi->base + sspi->regs->int_st),
+				sspi->base + sspi->regs->int_st);
 		return IRQ_HANDLED;
 	}
 	if (spi_stat & SIRFSOC_SPI_TXFIFO_EMPTY)
 		complete(&sspi->tx_done);
-	while (!(readl(sspi->base + SIRFSOC_SPI_INT_STATUS) &
+	while (!(readl(sspi->base + sspi->regs->int_st) &
 		SIRFSOC_SPI_RX_IO_DMA))
 		cpu_relax();
 	complete(&sspi->rx_done);
-	writel(0x0, sspi->base + SIRFSOC_SPI_INT_EN);
-	writel(SIRFSOC_SPI_INT_MASK_ALL,
-			sspi->base + SIRFSOC_SPI_INT_STATUS);
+	switch (sspi->type) {
+	case SIRF_REAL_SPI:
+	case SIRF_USP_SPI_P2:
+		writel(0x0, sspi->base + sspi->regs->int_en);
+		break;
+	case SIRF_USP_SPI_A7:
+		writel(~0UL, sspi->base + sspi->regs->usp_int_en_clr);
+		break;
+	}
+	writel(readl(sspi->base + sspi->regs->int_st),
+			sspi->base + sspi->regs->int_st);
 
 	return IRQ_HANDLED;
 }
@@ -313,8 +465,8 @@ static void spi_sirfsoc_cmd_transfer(struct spi_device *spi,
 	u32 cmd;
 
 	sspi = spi_master_get_devdata(spi->master);
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + sspi->regs->txfifo_op);
+	writel(SIRFSOC_SPI_FIFO_START, sspi->base + sspi->regs->txfifo_op);
 	memcpy(&cmd, sspi->tx, t->len);
 	if (sspi->word_width == 1 && !(spi->mode & SPI_LSB_FIRST))
 		cmd = cpu_to_be32(cmd) >>
@@ -322,11 +474,11 @@ static void spi_sirfsoc_cmd_transfer(struct spi_device *spi,
 	if (sspi->word_width == 2 && t->len == 4 &&
 			(!(spi->mode & SPI_LSB_FIRST)))
 		cmd = ((cmd & 0xffff) << 16) | (cmd >> 16);
-	writel(cmd, sspi->base + SIRFSOC_SPI_CMD);
+	writel(cmd, sspi->base + sspi->regs->spi_cmd);
 	writel(SIRFSOC_SPI_FRM_END_INT_EN,
-		sspi->base + SIRFSOC_SPI_INT_EN);
+		sspi->base + sspi->regs->int_en);
 	writel(SIRFSOC_SPI_CMD_TX_EN,
-		sspi->base + SIRFSOC_SPI_TX_RX_EN);
+		sspi->base + sspi->regs->tx_rx_en);
 	if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
 		dev_err(&spi->dev, "cmd transfer timeout\n");
 		return;
@@ -342,25 +494,56 @@ static void spi_sirfsoc_dma_transfer(struct spi_device *spi,
 	int timeout = t->len * 10;
 
 	sspi = spi_master_get_devdata(spi->master);
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(0, sspi->base + SIRFSOC_SPI_INT_EN);
-	writel(SIRFSOC_SPI_INT_MASK_ALL, sspi->base + SIRFSOC_SPI_INT_STATUS);
-	if (sspi->left_tx_word < SIRFSOC_SPI_DAT_FRM_LEN_MAX) {
-		writel(readl(sspi->base + SIRFSOC_SPI_CTRL) |
-			SIRFSOC_SPI_ENA_AUTO_CLR | SIRFSOC_SPI_MUL_DAT_MODE,
-			sspi->base + SIRFSOC_SPI_CTRL);
-		writel(sspi->left_tx_word - 1,
-				sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
-		writel(sspi->left_tx_word - 1,
-				sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + sspi->regs->rxfifo_op);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + sspi->regs->txfifo_op);
+	switch (sspi->type) {
+	case SIRF_REAL_SPI:
+		writel(SIRFSOC_SPI_FIFO_START,
+			sspi->base + sspi->regs->rxfifo_op);
+		writel(SIRFSOC_SPI_FIFO_START,
+			sspi->base + sspi->regs->txfifo_op);
+		writel(0, sspi->base + sspi->regs->int_en);
+		break;
+	case SIRF_USP_SPI_P2:
+		writel(0x0, sspi->base + sspi->regs->rxfifo_op);
+		writel(0x0, sspi->base + sspi->regs->txfifo_op);
+		writel(0, sspi->base + sspi->regs->int_en);
+		break;
+	case SIRF_USP_SPI_A7:
+		writel(0x0, sspi->base + sspi->regs->rxfifo_op);
+		writel(0x0, sspi->base + sspi->regs->txfifo_op);
+		writel(~0UL, sspi->base + sspi->regs->usp_int_en_clr);
+		break;
+	}
+	writel(readl(sspi->base + sspi->regs->int_st),
+		sspi->base + sspi->regs->int_st);
+	if (sspi->left_tx_word < sspi->dat_max_frm_len) {
+		switch (sspi->type) {
+		case SIRF_REAL_SPI:
+			writel(readl(sspi->base + sspi->regs->spi_ctrl) |
+				SIRFSOC_SPI_ENA_AUTO_CLR |
+				SIRFSOC_SPI_MUL_DAT_MODE,
+				sspi->base + sspi->regs->spi_ctrl);
+			writel(sspi->left_tx_word - 1,
+				sspi->base + sspi->regs->tx_dma_io_len);
+			writel(sspi->left_tx_word - 1,
+				sspi->base + sspi->regs->rx_dma_io_len);
+			break;
+		case SIRF_USP_SPI_P2:
+		case SIRF_USP_SPI_A7:
+			/*USP simulate SPI, tx/rx_dma_io_len indicates bytes*/
+			writel(sspi->left_tx_word * sspi->word_width,
+				sspi->base + sspi->regs->tx_dma_io_len);
+			writel(sspi->left_tx_word * sspi->word_width,
+				sspi->base + sspi->regs->rx_dma_io_len);
+			break;
+		}
 	} else {
-		writel(readl(sspi->base + SIRFSOC_SPI_CTRL),
-			sspi->base + SIRFSOC_SPI_CTRL);
-		writel(0, sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
-		writel(0, sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
+		if (sspi->type == SIRF_REAL_SPI)
+			writel(readl(sspi->base + sspi->regs->spi_ctrl),
+				sspi->base + sspi->regs->spi_ctrl);
+		writel(0, sspi->base + sspi->regs->tx_dma_io_len);
+		writel(0, sspi->base + sspi->regs->rx_dma_io_len);
 	}
 	sspi->dst_start = dma_map_single(&spi->dev, sspi->rx, t->len,
 					(t->tx_buf != t->rx_buf) ?
@@ -385,7 +568,14 @@ static void spi_sirfsoc_dma_transfer(struct spi_device *spi,
 	dma_async_issue_pending(sspi->tx_chan);
 	dma_async_issue_pending(sspi->rx_chan);
 	writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN,
-			sspi->base + SIRFSOC_SPI_TX_RX_EN);
+			sspi->base + sspi->regs->tx_rx_en);
+	if (sspi->type == SIRF_USP_SPI_P2 ||
+		sspi->type == SIRF_USP_SPI_A7) {
+		writel(SIRFSOC_SPI_FIFO_START,
+			sspi->base + sspi->regs->rxfifo_op);
+		writel(SIRFSOC_SPI_FIFO_START,
+			sspi->base + sspi->regs->txfifo_op);
+	}
 	if (wait_for_completion_timeout(&sspi->rx_done, timeout) == 0) {
 		dev_err(&spi->dev, "transfer timeout\n");
 		dmaengine_terminate_all(sspi->rx_chan);
@@ -398,15 +588,21 @@ static void spi_sirfsoc_dma_transfer(struct spi_device *spi,
 	 */
 	if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
 		dev_err(&spi->dev, "transfer timeout\n");
+		if (sspi->type == SIRF_USP_SPI_P2 ||
+			sspi->type == SIRF_USP_SPI_A7)
+			writel(0, sspi->base + sspi->regs->tx_rx_en);
 		dmaengine_terminate_all(sspi->tx_chan);
 	}
 	dma_unmap_single(&spi->dev, sspi->src_start, t->len, DMA_TO_DEVICE);
 	dma_unmap_single(&spi->dev, sspi->dst_start, t->len, DMA_FROM_DEVICE);
 	/* TX, RX FIFO stop */
-	writel(0, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(0, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	if (sspi->left_tx_word >= SIRFSOC_SPI_DAT_FRM_LEN_MAX)
-		writel(0, sspi->base + SIRFSOC_SPI_TX_RX_EN);
+	writel(0, sspi->base + sspi->regs->rxfifo_op);
+	writel(0, sspi->base + sspi->regs->txfifo_op);
+	if (sspi->left_tx_word >= sspi->dat_max_frm_len)
+		writel(0, sspi->base + sspi->regs->tx_rx_en);
+	if (sspi->type == SIRF_USP_SPI_P2 ||
+		sspi->type == SIRF_USP_SPI_A7)
+		writel(0, sspi->base + sspi->regs->tx_rx_en);
 }
 
 static void spi_sirfsoc_pio_transfer(struct spi_device *spi,
@@ -414,57 +610,105 @@ static void spi_sirfsoc_pio_transfer(struct spi_device *spi,
 {
 	struct sirfsoc_spi *sspi;
 	int timeout = t->len * 10;
+	unsigned int data_units;
 
 	sspi = spi_master_get_devdata(spi->master);
 	do {
 		writel(SIRFSOC_SPI_FIFO_RESET,
-			sspi->base + SIRFSOC_SPI_RXFIFO_OP);
+			sspi->base + sspi->regs->rxfifo_op);
 		writel(SIRFSOC_SPI_FIFO_RESET,
-			sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-		writel(SIRFSOC_SPI_FIFO_START,
-			sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-		writel(SIRFSOC_SPI_FIFO_START,
-			sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-		writel(0, sspi->base + SIRFSOC_SPI_INT_EN);
-		writel(SIRFSOC_SPI_INT_MASK_ALL,
-			sspi->base + SIRFSOC_SPI_INT_STATUS);
-		writel(readl(sspi->base + SIRFSOC_SPI_CTRL) |
-			SIRFSOC_SPI_MUL_DAT_MODE | SIRFSOC_SPI_ENA_AUTO_CLR,
-			sspi->base + SIRFSOC_SPI_CTRL);
-		writel(min(sspi->left_tx_word, (u32)(256 / sspi->word_width))
-				- 1, sspi->base + SIRFSOC_SPI_TX_DMA_IO_LEN);
-		writel(min(sspi->left_rx_word, (u32)(256 / sspi->word_width))
-				- 1, sspi->base + SIRFSOC_SPI_RX_DMA_IO_LEN);
-		while (!((readl(sspi->base + SIRFSOC_SPI_TXFIFO_STATUS)
-			& SIRFSOC_SPI_FIFO_FULL)) && sspi->left_tx_word)
+			sspi->base + sspi->regs->txfifo_op);
+		switch (sspi->type) {
+		case SIRF_USP_SPI_P2:
+			writel(0x0, sspi->base + sspi->regs->rxfifo_op);
+			writel(0x0, sspi->base + sspi->regs->txfifo_op);
+			writel(0, sspi->base + sspi->regs->int_en);
+			writel(readl(sspi->base + sspi->regs->int_st),
+				sspi->base + sspi->regs->int_st);
+			writel(min((sspi->left_tx_word * sspi->word_width),
+				sspi->fifo_size),
+				sspi->base + sspi->regs->tx_dma_io_len);
+			writel(min((sspi->left_rx_word * sspi->word_width),
+				sspi->fifo_size),
+				sspi->base + sspi->regs->rx_dma_io_len);
+			break;
+		case SIRF_USP_SPI_A7:
+			writel(0x0, sspi->base + sspi->regs->rxfifo_op);
+			writel(0x0, sspi->base + sspi->regs->txfifo_op);
+			writel(~0UL, sspi->base + sspi->regs->usp_int_en_clr);
+			writel(readl(sspi->base + sspi->regs->int_st),
+				sspi->base + sspi->regs->int_st);
+			writel(min((sspi->left_tx_word * sspi->word_width),
+				sspi->fifo_size),
+				sspi->base + sspi->regs->tx_dma_io_len);
+			writel(min((sspi->left_rx_word * sspi->word_width),
+				sspi->fifo_size),
+				sspi->base + sspi->regs->rx_dma_io_len);
+			break;
+		case SIRF_REAL_SPI:
+			writel(SIRFSOC_SPI_FIFO_START,
+				sspi->base + sspi->regs->rxfifo_op);
+			writel(SIRFSOC_SPI_FIFO_START,
+				sspi->base + sspi->regs->txfifo_op);
+			writel(0, sspi->base + sspi->regs->int_en);
+			writel(readl(sspi->base + sspi->regs->int_st),
+				sspi->base + sspi->regs->int_st);
+			writel(readl(sspi->base + sspi->regs->spi_ctrl) |
+				SIRFSOC_SPI_MUL_DAT_MODE |
+				SIRFSOC_SPI_ENA_AUTO_CLR,
+				sspi->base + sspi->regs->spi_ctrl);
+			data_units = sspi->fifo_size / sspi->word_width;
+			writel(min(sspi->left_tx_word, data_units) - 1,
+				sspi->base + sspi->regs->tx_dma_io_len);
+			writel(min(sspi->left_rx_word, data_units) - 1,
+				sspi->base + sspi->regs->rx_dma_io_len);
+			break;
+		}
+		while (!((readl(sspi->base + sspi->regs->txfifo_st)
+			& SIRFSOC_SPI_FIFO_FULL_MASK(sspi))) &&
+			sspi->left_tx_word)
 			sspi->tx_word(sspi);
 		writel(SIRFSOC_SPI_TXFIFO_EMPTY_INT_EN |
 			SIRFSOC_SPI_TX_UFLOW_INT_EN |
 			SIRFSOC_SPI_RX_OFLOW_INT_EN |
 			SIRFSOC_SPI_RX_IO_DMA_INT_EN,
-			sspi->base + SIRFSOC_SPI_INT_EN);
+			sspi->base + sspi->regs->int_en);
 		writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN,
-			sspi->base + SIRFSOC_SPI_TX_RX_EN);
+			sspi->base + sspi->regs->tx_rx_en);
+		if (sspi->type == SIRF_USP_SPI_P2 ||
+			sspi->type == SIRF_USP_SPI_A7) {
+			writel(SIRFSOC_SPI_FIFO_START,
+				sspi->base + sspi->regs->rxfifo_op);
+			writel(SIRFSOC_SPI_FIFO_START,
+				sspi->base + sspi->regs->txfifo_op);
+		}
 		if (!wait_for_completion_timeout(&sspi->tx_done, timeout) ||
 			!wait_for_completion_timeout(&sspi->rx_done, timeout)) {
 			dev_err(&spi->dev, "transfer timeout\n");
+			if (sspi->type == SIRF_USP_SPI_P2 ||
+				sspi->type == SIRF_USP_SPI_A7)
+				writel(0, sspi->base + sspi->regs->tx_rx_en);
 			break;
 		}
-		while (!((readl(sspi->base + SIRFSOC_SPI_RXFIFO_STATUS)
-			& SIRFSOC_SPI_FIFO_EMPTY)) && sspi->left_rx_word)
+		while (!((readl(sspi->base + sspi->regs->rxfifo_st)
+			& SIRFSOC_SPI_FIFO_EMPTY_MASK(sspi))) &&
+			sspi->left_rx_word)
 			sspi->rx_word(sspi);
-		writel(0, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-		writel(0, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+		if (sspi->type == SIRF_USP_SPI_P2 ||
+			sspi->type == SIRF_USP_SPI_A7)
+			writel(0, sspi->base + sspi->regs->tx_rx_en);
+		writel(0, sspi->base + sspi->regs->rxfifo_op);
+		writel(0, sspi->base + sspi->regs->txfifo_op);
 	} while (sspi->left_tx_word != 0 || sspi->left_rx_word != 0);
 }
 
 static int spi_sirfsoc_transfer(struct spi_device *spi, struct spi_transfer *t)
 {
 	struct sirfsoc_spi *sspi;
-	sspi = spi_master_get_devdata(spi->master);
 
-	sspi->tx = t->tx_buf ? t->tx_buf : sspi->dummypage;
-	sspi->rx = t->rx_buf ? t->rx_buf : sspi->dummypage;
+	sspi = spi_master_get_devdata(spi->master);
+	sspi->tx = t->tx_buf;
+	sspi->rx = t->rx_buf;
 	sspi->left_tx_word = sspi->left_rx_word = t->len / sspi->word_width;
 	reinit_completion(&sspi->rx_done);
 	reinit_completion(&sspi->tx_done);
@@ -473,7 +717,7 @@ static int spi_sirfsoc_transfer(struct spi_device *spi, struct spi_transfer *t)
 	 * null, just fill command data into command register and wait for its
 	 * completion.
 	 */
-	if (sspi->tx_by_cmd)
+	if (sspi->type == SIRF_REAL_SPI && sspi->tx_by_cmd)
 		spi_sirfsoc_cmd_transfer(spi, t);
 	else if (IS_DMA_VALID(t))
 		spi_sirfsoc_dma_transfer(spi, t);
@@ -488,22 +732,49 @@ static void spi_sirfsoc_chipselect(struct spi_device *spi, int value)
 	struct sirfsoc_spi *sspi = spi_master_get_devdata(spi->master);
 
 	if (sspi->hw_cs) {
-		u32 regval = readl(sspi->base + SIRFSOC_SPI_CTRL);
-		switch (value) {
-		case BITBANG_CS_ACTIVE:
-			if (spi->mode & SPI_CS_HIGH)
-				regval |= SIRFSOC_SPI_CS_IO_OUT;
-			else
-				regval &= ~SIRFSOC_SPI_CS_IO_OUT;
+		u32 regval;
+
+		switch (sspi->type) {
+		case SIRF_REAL_SPI:
+			regval = readl(sspi->base + sspi->regs->spi_ctrl);
+			switch (value) {
+			case BITBANG_CS_ACTIVE:
+				if (spi->mode & SPI_CS_HIGH)
+					regval |= SIRFSOC_SPI_CS_IO_OUT;
+				else
+					regval &= ~SIRFSOC_SPI_CS_IO_OUT;
+				break;
+			case BITBANG_CS_INACTIVE:
+				if (spi->mode & SPI_CS_HIGH)
+					regval &= ~SIRFSOC_SPI_CS_IO_OUT;
+				else
+					regval |= SIRFSOC_SPI_CS_IO_OUT;
+				break;
+			}
+			writel(regval, sspi->base + sspi->regs->spi_ctrl);
 			break;
-		case BITBANG_CS_INACTIVE:
-			if (spi->mode & SPI_CS_HIGH)
-				regval &= ~SIRFSOC_SPI_CS_IO_OUT;
-			else
-				regval |= SIRFSOC_SPI_CS_IO_OUT;
+		case SIRF_USP_SPI_P2:
+		case SIRF_USP_SPI_A7:
+			regval = readl(sspi->base +
+					sspi->regs->usp_pin_io_data);
+			switch (value) {
+			case BITBANG_CS_ACTIVE:
+				if (spi->mode & SPI_CS_HIGH)
+					regval |= SIRFSOC_USP_CS_HIGH_VALUE;
+				else
+					regval &= ~(SIRFSOC_USP_CS_HIGH_VALUE);
+				break;
+			case BITBANG_CS_INACTIVE:
+				if (spi->mode & SPI_CS_HIGH)
+					regval &= ~(SIRFSOC_USP_CS_HIGH_VALUE);
+				else
+					regval |= SIRFSOC_USP_CS_HIGH_VALUE;
+				break;
+			}
+			writel(regval,
+				sspi->base + sspi->regs->usp_pin_io_data);
 			break;
 		}
-		writel(regval, sspi->base + SIRFSOC_SPI_CTRL);
 	} else {
 		switch (value) {
 		case BITBANG_CS_ACTIVE:
@@ -518,27 +789,102 @@ static void spi_sirfsoc_chipselect(struct spi_device *spi, int value)
 	}
 }
 
+static int spi_sirfsoc_config_mode(struct spi_device *spi)
+{
+	struct sirfsoc_spi *sspi;
+	u32 regval, usp_mode1;
+
+	sspi = spi_master_get_devdata(spi->master);
+	regval = readl(sspi->base + sspi->regs->spi_ctrl);
+	usp_mode1 = readl(sspi->base + sspi->regs->usp_mode1);
+	if (!(spi->mode & SPI_CS_HIGH)) {
+		regval |= SIRFSOC_SPI_CS_IDLE_STAT;
+		usp_mode1 &= ~SIRFSOC_USP_CS_HIGH_VALID;
+	} else {
+		regval &= ~SIRFSOC_SPI_CS_IDLE_STAT;
+		usp_mode1 |= SIRFSOC_USP_CS_HIGH_VALID;
+	}
+	if (!(spi->mode & SPI_LSB_FIRST)) {
+		regval |= SIRFSOC_SPI_TRAN_MSB;
+		usp_mode1 &= ~SIRFSOC_USP_LSB;
+	} else {
+		regval &= ~SIRFSOC_SPI_TRAN_MSB;
+		usp_mode1 |= SIRFSOC_USP_LSB;
+	}
+	if (spi->mode & SPI_CPOL) {
+		regval |= SIRFSOC_SPI_CLK_IDLE_STAT;
+		usp_mode1 |= SIRFSOC_USP_SCLK_IDLE_STAT;
+	} else {
+		regval &= ~SIRFSOC_SPI_CLK_IDLE_STAT;
+		usp_mode1 &= ~SIRFSOC_USP_SCLK_IDLE_STAT;
+	}
+	/*
+	 * Data should be driven at least 1/2 cycle before the fetch edge
+	 * to make sure that data gets stable at the fetch edge.
+	 */
+	if (((spi->mode & SPI_CPOL) && (spi->mode & SPI_CPHA)) ||
+	    (!(spi->mode & SPI_CPOL) && !(spi->mode & SPI_CPHA))) {
+		regval &= ~SIRFSOC_SPI_DRV_POS_EDGE;
+		usp_mode1 |= (SIRFSOC_USP_TXD_FALLING_EDGE |
+				SIRFSOC_USP_RXD_FALLING_EDGE);
+	} else {
+		regval |= SIRFSOC_SPI_DRV_POS_EDGE;
+		usp_mode1 &= ~(SIRFSOC_USP_RXD_FALLING_EDGE |
+				SIRFSOC_USP_TXD_FALLING_EDGE);
+	}
+	writel((SIRFSOC_SPI_FIFO_LEVEL_CHK_MASK(sspi, sspi->fifo_size - 2) <<
+		SIRFSOC_SPI_FIFO_SC_OFFSET) |
+		(SIRFSOC_SPI_FIFO_LEVEL_CHK_MASK(sspi, sspi->fifo_size / 2) <<
+		SIRFSOC_SPI_FIFO_LC_OFFSET) |
+		(SIRFSOC_SPI_FIFO_LEVEL_CHK_MASK(sspi, 2) <<
+		SIRFSOC_SPI_FIFO_HC_OFFSET),
+		sspi->base + sspi->regs->txfifo_level_chk);
+	writel((SIRFSOC_SPI_FIFO_LEVEL_CHK_MASK(sspi, 2) <<
+		SIRFSOC_SPI_FIFO_SC_OFFSET) |
+		(SIRFSOC_SPI_FIFO_LEVEL_CHK_MASK(sspi, sspi->fifo_size / 2) <<
+		SIRFSOC_SPI_FIFO_LC_OFFSET) |
+		(SIRFSOC_SPI_FIFO_LEVEL_CHK_MASK(sspi, sspi->fifo_size - 2) <<
+		SIRFSOC_SPI_FIFO_HC_OFFSET),
+		sspi->base + sspi->regs->rxfifo_level_chk);
+	/*
+	 * it should never set to hardware cs mode because in hardware cs mode,
+	 * cs signal can't controlled by driver.
+	 */
+	switch (sspi->type) {
+	case SIRF_REAL_SPI:
+		regval |= SIRFSOC_SPI_CS_IO_MODE;
+		writel(regval, sspi->base + sspi->regs->spi_ctrl);
+		break;
+	case SIRF_USP_SPI_P2:
+	case SIRF_USP_SPI_A7:
+		usp_mode1 |= SIRFSOC_USP_SYNC_MODE;
+		usp_mode1 |= SIRFSOC_USP_TFS_IO_MODE;
+		usp_mode1 &= ~SIRFSOC_USP_TFS_IO_INPUT;
+		writel(usp_mode1, sspi->base + sspi->regs->usp_mode1);
+		break;
+	}
+
+	return 0;
+}
+
 static int
 spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 {
 	struct sirfsoc_spi *sspi;
 	u8 bits_per_word = 0;
 	int hz = 0;
-	u32 regval;
-	u32 txfifo_ctrl, rxfifo_ctrl;
-	u32 fifo_size = SIRFSOC_SPI_FIFO_SIZE / 4;
+	u32 regval, txfifo_ctrl, rxfifo_ctrl, tx_frm_ctl, rx_frm_ctl, usp_mode2;
 
 	sspi = spi_master_get_devdata(spi->master);
 
 	bits_per_word = (t) ? t->bits_per_word : spi->bits_per_word;
 	hz = t && t->speed_hz ? t->speed_hz : spi->max_speed_hz;
 
-	regval = (sspi->ctrl_freq / (2 * hz)) - 1;
+	usp_mode2 = regval = (sspi->ctrl_freq / (2 * hz)) - 1;
 	if (regval > 0xFFFF || regval < 0) {
 		dev_err(&spi->dev, "Speed %d not supported\n", hz);
 		return -EINVAL;
 	}
-
 	switch (bits_per_word) {
 	case 8:
 		regval |= SIRFSOC_SPI_TRAN_DAT_FORMAT_8;
@@ -559,94 +905,177 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 		sspi->tx_word = spi_sirfsoc_tx_word_u32;
 		break;
 	default:
-		BUG();
+		dev_err(&spi->dev, "bpw %d not supported\n", bits_per_word);
+		return -EINVAL;
 	}
-
 	sspi->word_width = DIV_ROUND_UP(bits_per_word, 8);
-	txfifo_ctrl = SIRFSOC_SPI_FIFO_THD(SIRFSOC_SPI_FIFO_SIZE / 2) |
-					   (sspi->word_width >> 1);
-	rxfifo_ctrl = SIRFSOC_SPI_FIFO_THD(SIRFSOC_SPI_FIFO_SIZE / 2) |
-					   (sspi->word_width >> 1);
-
-	if (!(spi->mode & SPI_CS_HIGH))
-		regval |= SIRFSOC_SPI_CS_IDLE_STAT;
-	if (!(spi->mode & SPI_LSB_FIRST))
-		regval |= SIRFSOC_SPI_TRAN_MSB;
-	if (spi->mode & SPI_CPOL)
-		regval |= SIRFSOC_SPI_CLK_IDLE_STAT;
-
-	/*
-	 * Data should be driven at least 1/2 cycle before the fetch edge
-	 * to make sure that data gets stable at the fetch edge.
-	 */
-	if (((spi->mode & SPI_CPOL) && (spi->mode & SPI_CPHA)) ||
-	    (!(spi->mode & SPI_CPOL) && !(spi->mode & SPI_CPHA)))
-		regval &= ~SIRFSOC_SPI_DRV_POS_EDGE;
-	else
-		regval |= SIRFSOC_SPI_DRV_POS_EDGE;
-
-	writel(SIRFSOC_SPI_FIFO_SC(fifo_size - 2) |
-			SIRFSOC_SPI_FIFO_LC(fifo_size / 2) |
-			SIRFSOC_SPI_FIFO_HC(2),
-		sspi->base + SIRFSOC_SPI_TXFIFO_LEVEL_CHK);
-	writel(SIRFSOC_SPI_FIFO_SC(2) |
-			SIRFSOC_SPI_FIFO_LC(fifo_size / 2) |
-			SIRFSOC_SPI_FIFO_HC(fifo_size - 2),
-		sspi->base + SIRFSOC_SPI_RXFIFO_LEVEL_CHK);
-	writel(txfifo_ctrl, sspi->base + SIRFSOC_SPI_TXFIFO_CTRL);
-	writel(rxfifo_ctrl, sspi->base + SIRFSOC_SPI_RXFIFO_CTRL);
-
-	if (t && t->tx_buf && !t->rx_buf && (t->len <= SIRFSOC_MAX_CMD_BYTES)) {
-		regval |= (SIRFSOC_SPI_CMD_BYTE_NUM((t->len - 1)) |
-				SIRFSOC_SPI_CMD_MODE);
-		sspi->tx_by_cmd = true;
-	} else {
-		regval &= ~SIRFSOC_SPI_CMD_MODE;
-		sspi->tx_by_cmd = false;
+	txfifo_ctrl = (((sspi->fifo_size / 2) &
+			SIRFSOC_SPI_FIFO_THD_MASK(sspi))
+			<< SIRFSOC_SPI_FIFO_THD_OFFSET) |
+			(sspi->word_width >> 1);
+	rxfifo_ctrl = (((sspi->fifo_size / 2) &
+			SIRFSOC_SPI_FIFO_THD_MASK(sspi))
+			<< SIRFSOC_SPI_FIFO_THD_OFFSET) |
+			(sspi->word_width >> 1);
+	writel(txfifo_ctrl, sspi->base + sspi->regs->txfifo_ctrl);
+	writel(rxfifo_ctrl, sspi->base + sspi->regs->rxfifo_ctrl);
+	if (sspi->type == SIRF_USP_SPI_P2 ||
+		sspi->type == SIRF_USP_SPI_A7) {
+		tx_frm_ctl = 0;
+		tx_frm_ctl |= ((bits_per_word - 1) & SIRFSOC_USP_TX_DATA_MASK)
+				<< SIRFSOC_USP_TX_DATA_OFFSET;
+		tx_frm_ctl |= ((bits_per_word + 1 + SIRFSOC_USP_TXD_DELAY_LEN
+				- 1) & SIRFSOC_USP_TX_SYNC_MASK) <<
+				SIRFSOC_USP_TX_SYNC_OFFSET;
+		tx_frm_ctl |= ((bits_per_word + 1 + SIRFSOC_USP_TXD_DELAY_LEN
+				+ 2 - 1) & SIRFSOC_USP_TX_FRAME_MASK) <<
+				SIRFSOC_USP_TX_FRAME_OFFSET;
+		tx_frm_ctl |= ((bits_per_word - 1) &
+				SIRFSOC_USP_TX_SHIFTER_MASK) <<
+				SIRFSOC_USP_TX_SHIFTER_OFFSET;
+		rx_frm_ctl = 0;
+		rx_frm_ctl |= ((bits_per_word - 1) & SIRFSOC_USP_RX_DATA_MASK)
+				<< SIRFSOC_USP_RX_DATA_OFFSET;
+		rx_frm_ctl |= ((bits_per_word + 1 + SIRFSOC_USP_RXD_DELAY_LEN
+				+ 2 - 1) & SIRFSOC_USP_RX_FRAME_MASK) <<
+				SIRFSOC_USP_RX_FRAME_OFFSET;
+		rx_frm_ctl |= ((bits_per_word - 1)
+				& SIRFSOC_USP_RX_SHIFTER_MASK) <<
+				SIRFSOC_USP_RX_SHIFTER_OFFSET;
+		writel(tx_frm_ctl | (((usp_mode2 >> 10) &
+			SIRFSOC_USP_CLK_10_11_MASK) <<
+			SIRFSOC_USP_CLK_10_11_OFFSET),
+			sspi->base + sspi->regs->usp_tx_frame_ctrl);
+		writel(rx_frm_ctl | (((usp_mode2 >> 12) &
+			SIRFSOC_USP_CLK_12_15_MASK) <<
+			SIRFSOC_USP_CLK_12_15_OFFSET),
+			sspi->base + sspi->regs->usp_rx_frame_ctrl);
+		writel(readl(sspi->base + sspi->regs->usp_mode2) |
+			((usp_mode2 & SIRFSOC_USP_CLK_DIVISOR_MASK) <<
+			SIRFSOC_USP_CLK_DIVISOR_OFFSET) |
+			(SIRFSOC_USP_RXD_DELAY_LEN <<
+			 SIRFSOC_USP_RXD_DELAY_OFFSET) |
+			(SIRFSOC_USP_TXD_DELAY_LEN <<
+			 SIRFSOC_USP_TXD_DELAY_OFFSET),
+			sspi->base + sspi->regs->usp_mode2);
+	}
+	if (sspi->type == SIRF_REAL_SPI)
+		writel(regval, sspi->base + sspi->regs->spi_ctrl);
+	spi_sirfsoc_config_mode(spi);
+	if (sspi->type == SIRF_REAL_SPI) {
+		if (t && t->tx_buf && !t->rx_buf &&
+			(t->len <= SIRFSOC_MAX_CMD_BYTES)) {
+			sspi->tx_by_cmd = true;
+			writel(readl(sspi->base + sspi->regs->spi_ctrl) |
+				(SIRFSOC_SPI_CMD_BYTE_NUM((t->len - 1)) |
+				SIRFSOC_SPI_CMD_MODE),
+				sspi->base + sspi->regs->spi_ctrl);
+		} else {
+			sspi->tx_by_cmd = false;
+			writel(readl(sspi->base + sspi->regs->spi_ctrl) &
+				~SIRFSOC_SPI_CMD_MODE,
+				sspi->base + sspi->regs->spi_ctrl);
+		}
 	}
-	/*
-	 * it should never set to hardware cs mode because in hardware cs mode,
-	 * cs signal can't controlled by driver.
-	 */
-	regval |= SIRFSOC_SPI_CS_IO_MODE;
-	writel(regval, sspi->base + SIRFSOC_SPI_CTRL);
-
 	if (IS_DMA_VALID(t)) {
 		/* Enable DMA mode for RX, TX */
-		writel(0, sspi->base + SIRFSOC_SPI_TX_DMA_IO_CTRL);
+		writel(0, sspi->base + sspi->regs->tx_dma_io_ctrl);
 		writel(SIRFSOC_SPI_RX_DMA_FLUSH,
-			sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
+			sspi->base + sspi->regs->rx_dma_io_ctrl);
 	} else {
 		/* Enable IO mode for RX, TX */
 		writel(SIRFSOC_SPI_IO_MODE_SEL,
-			sspi->base + SIRFSOC_SPI_TX_DMA_IO_CTRL);
+			sspi->base + sspi->regs->tx_dma_io_ctrl);
 		writel(SIRFSOC_SPI_IO_MODE_SEL,
-			sspi->base + SIRFSOC_SPI_RX_DMA_IO_CTRL);
+			sspi->base + sspi->regs->rx_dma_io_ctrl);
 	}
-
 	return 0;
 }
 
 static int spi_sirfsoc_setup(struct spi_device *spi)
 {
 	struct sirfsoc_spi *sspi;
+	int ret = 0;
 
 	sspi = spi_master_get_devdata(spi->master);
-
 	if (spi->cs_gpio == -ENOENT)
 		sspi->hw_cs = true;
-	else
+	else {
 		sspi->hw_cs = false;
-	return spi_sirfsoc_setup_transfer(spi, NULL);
+		if (!spi_get_ctldata(spi)) {
+			void *cs = kmalloc(sizeof(int), GFP_KERNEL);
+			if (!cs) {
+				ret = -ENOMEM;
+				goto exit;
+			}
+			ret = gpio_is_valid(spi->cs_gpio);
+			if (!ret) {
+				dev_err(&spi->dev, "no valid gpio\n");
+				ret = -ENOENT;
+				goto exit;
+			}
+			ret = gpio_request(spi->cs_gpio, DRIVER_NAME);
+			if (ret) {
+				dev_err(&spi->dev, "failed to request gpio\n");
+				goto exit;
+			}
+			spi_set_ctldata(spi, cs);
+		}
+	}
+	spi_sirfsoc_config_mode(spi);
+	spi_sirfsoc_chipselect(spi, BITBANG_CS_INACTIVE);
+exit:
+	return ret;
+}
+
+static void spi_sirfsoc_cleanup(struct spi_device *spi)
+{
+	if (spi_get_ctldata(spi)) {
+		gpio_free(spi->cs_gpio);
+		kfree(spi_get_ctldata(spi));
+	}
 }
 
+static const struct sirf_spi_comp_data sirf_real_spi = {
+	.regs = &real_spi_register,
+	.type = SIRF_REAL_SPI,
+	.dat_max_frm_len = 64 * 1024,
+	.fifo_size = 256,
+};
+
+static const struct sirf_spi_comp_data sirf_usp_spi_p2 = {
+	.regs = &usp_spi_register,
+	.type = SIRF_USP_SPI_P2,
+	.dat_max_frm_len = 1024 * 1024,
+	.fifo_size = 128,
+	.hwinit = sirfsoc_usp_hwinit,
+};
+
+static const struct sirf_spi_comp_data sirf_usp_spi_a7 = {
+	.regs = &usp_spi_register,
+	.type = SIRF_USP_SPI_A7,
+	.dat_max_frm_len = 1024 * 1024,
+	.fifo_size = 512,
+	.hwinit = sirfsoc_usp_hwinit,
+};
+
+static const struct of_device_id spi_sirfsoc_of_match[] = {
+	{ .compatible = "sirf,prima2-spi", .data = &sirf_real_spi},
+	{ .compatible = "sirf,prima2-usp-spi", .data = &sirf_usp_spi_p2},
+	{ .compatible = "sirf,atlas7-usp-spi", .data = &sirf_usp_spi_a7},
+	{}
+};
+MODULE_DEVICE_TABLE(of, spi_sirfsoc_of_match);
+
 static int spi_sirfsoc_probe(struct platform_device *pdev)
 {
 	struct sirfsoc_spi *sspi;
 	struct spi_master *master;
 	struct resource *mem_res;
+	struct sirf_spi_comp_data *spi_comp_data;
 	int irq;
-	int i, ret;
+	int ret;
+	const struct of_device_id *match;
 
 	ret = device_reset(&pdev->dev);
 	if (ret) {
@@ -659,16 +1088,22 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Unable to allocate SPI master\n");
 		return -ENOMEM;
 	}
+	match = of_match_node(spi_sirfsoc_of_match, pdev->dev.of_node);
 	platform_set_drvdata(pdev, master);
 	sspi = spi_master_get_devdata(master);
-
+	sspi->fifo_full_offset = ilog2(sspi->fifo_size);
+	spi_comp_data = (struct sirf_spi_comp_data *)match->data;
+	sspi->regs = spi_comp_data->regs;
+	sspi->type = spi_comp_data->type;
+	sspi->fifo_level_chk_mask = (sspi->fifo_size / 4) - 1;
+	sspi->dat_max_frm_len = spi_comp_data->dat_max_frm_len;
+	sspi->fifo_size = spi_comp_data->fifo_size;
 	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	sspi->base = devm_ioremap_resource(&pdev->dev, mem_res);
 	if (IS_ERR(sspi->base)) {
 		ret = PTR_ERR(sspi->base);
 		goto free_master;
 	}
-
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		ret = -ENXIO;
@@ -684,11 +1119,13 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 	sspi->bitbang.setup_transfer = spi_sirfsoc_setup_transfer;
 	sspi->bitbang.txrx_bufs = spi_sirfsoc_transfer;
 	sspi->bitbang.master->setup = spi_sirfsoc_setup;
+	sspi->bitbang.master->cleanup = spi_sirfsoc_cleanup;
 	master->bus_num = pdev->id;
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_LSB_FIRST | SPI_CS_HIGH;
 	master->bits_per_word_mask = SPI_BPW_MASK(8) | SPI_BPW_MASK(12) |
 					SPI_BPW_MASK(16) | SPI_BPW_MASK(32);
 	master->max_speed_hz = SIRFSOC_SPI_DEFAULT_FRQ;
+	master->flags = SPI_MASTER_MUST_RX | SPI_MASTER_MUST_TX;
 	sspi->bitbang.master->dev.of_node = pdev->dev.of_node;
 
 	/* request DMA channels */
@@ -711,47 +1148,19 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 		goto free_tx_dma;
 	}
 	clk_prepare_enable(sspi->clk);
+	if (spi_comp_data->hwinit)
+		spi_comp_data->hwinit(sspi);
 	sspi->ctrl_freq = clk_get_rate(sspi->clk);
 
 	init_completion(&sspi->rx_done);
 	init_completion(&sspi->tx_done);
 
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	/* We are not using dummy delay between command and data */
-	writel(0, sspi->base + SIRFSOC_SPI_DUMMY_DELAY_CTL);
-
-	sspi->dummypage = kmalloc(2 * PAGE_SIZE, GFP_KERNEL);
-	if (!sspi->dummypage) {
-		ret = -ENOMEM;
-		goto free_clk;
-	}
-
 	ret = spi_bitbang_start(&sspi->bitbang);
 	if (ret)
-		goto free_dummypage;
-	for (i = 0; master->cs_gpios && i < master->num_chipselect; i++) {
-		if (master->cs_gpios[i] == -ENOENT)
-			continue;
-		if (!gpio_is_valid(master->cs_gpios[i])) {
-			dev_err(&pdev->dev, "no valid gpio\n");
-			ret = -EINVAL;
-			goto free_dummypage;
-		}
-		ret = devm_gpio_request(&pdev->dev,
-				master->cs_gpios[i], DRIVER_NAME);
-		if (ret) {
-			dev_err(&pdev->dev, "failed to request gpio\n");
-			goto free_dummypage;
-		}
-	}
+		goto free_clk;
 	dev_info(&pdev->dev, "registerred, bus number = %d\n", master->bus_num);
 
 	return 0;
-free_dummypage:
-	kfree(sspi->dummypage);
 free_clk:
 	clk_disable_unprepare(sspi->clk);
 	clk_put(sspi->clk);
@@ -772,9 +1181,7 @@ static int  spi_sirfsoc_remove(struct platform_device *pdev)
 
 	master = platform_get_drvdata(pdev);
 	sspi = spi_master_get_devdata(master);
-
 	spi_bitbang_stop(&sspi->bitbang);
-	kfree(sspi->dummypage);
 	clk_disable_unprepare(sspi->clk);
 	clk_put(sspi->clk);
 	dma_release_channel(sspi->rx_chan);
@@ -804,24 +1211,17 @@ static int spi_sirfsoc_resume(struct device *dev)
 	struct sirfsoc_spi *sspi = spi_master_get_devdata(master);
 
 	clk_enable(sspi->clk);
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_RXFIFO_OP);
-	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
-
-	return spi_master_resume(master);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + sspi->regs->txfifo_op);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + sspi->regs->rxfifo_op);
+	writel(SIRFSOC_SPI_FIFO_START, sspi->base + sspi->regs->txfifo_op);
+	writel(SIRFSOC_SPI_FIFO_START, sspi->base + sspi->regs->rxfifo_op);
+	return 0;
 }
 #endif
 
 static SIMPLE_DEV_PM_OPS(spi_sirfsoc_pm_ops, spi_sirfsoc_suspend,
 			 spi_sirfsoc_resume);
 
-static const struct of_device_id spi_sirfsoc_of_match[] = {
-	{ .compatible = "sirf,prima2-spi", },
-	{}
-};
-MODULE_DEVICE_TABLE(of, spi_sirfsoc_of_match);
-
 static struct platform_driver spi_sirfsoc_driver = {
 	.driver = {
 		.name = DRIVER_NAME,
@@ -835,4 +1235,5 @@ module_platform_driver(spi_sirfsoc_driver);
 MODULE_DESCRIPTION("SiRF SoC SPI master driver");
 MODULE_AUTHOR("Zhiwu Song <Zhiwu.Song@csr.com>");
 MODULE_AUTHOR("Barry Song <Baohua.Song@csr.com>");
+MODULE_AUTHOR("Qipan Li <Qipan.Li@csr.com>");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/spi/spi-zynqmp-gqspi.c b/drivers/spi/spi-zynqmp-gqspi.c
new file mode 100644
index 000000000000..87b20a511a6b
--- /dev/null
+++ b/drivers/spi/spi-zynqmp-gqspi.c
@@ -0,0 +1,1122 @@
+/*
+ * Xilinx Zynq UltraScale+ MPSoC Quad-SPI (QSPI) controller driver
+ * (master mode only)
+ *
+ * Copyright (C) 2009 - 2015 Xilinx, 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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+
+/* Generic QSPI register offsets */
+#define GQSPI_CONFIG_OFST		0x00000100
+#define GQSPI_ISR_OFST			0x00000104
+#define GQSPI_IDR_OFST			0x0000010C
+#define GQSPI_IER_OFST			0x00000108
+#define GQSPI_IMASK_OFST		0x00000110
+#define GQSPI_EN_OFST			0x00000114
+#define GQSPI_TXD_OFST			0x0000011C
+#define GQSPI_RXD_OFST			0x00000120
+#define GQSPI_TX_THRESHOLD_OFST		0x00000128
+#define GQSPI_RX_THRESHOLD_OFST		0x0000012C
+#define GQSPI_LPBK_DLY_ADJ_OFST		0x00000138
+#define GQSPI_GEN_FIFO_OFST		0x00000140
+#define GQSPI_SEL_OFST			0x00000144
+#define GQSPI_GF_THRESHOLD_OFST		0x00000150
+#define GQSPI_FIFO_CTRL_OFST		0x0000014C
+#define GQSPI_QSPIDMA_DST_CTRL_OFST	0x0000080C
+#define GQSPI_QSPIDMA_DST_SIZE_OFST	0x00000804
+#define GQSPI_QSPIDMA_DST_STS_OFST	0x00000808
+#define GQSPI_QSPIDMA_DST_I_STS_OFST	0x00000814
+#define GQSPI_QSPIDMA_DST_I_EN_OFST	0x00000818
+#define GQSPI_QSPIDMA_DST_I_DIS_OFST	0x0000081C
+#define GQSPI_QSPIDMA_DST_I_MASK_OFST	0x00000820
+#define GQSPI_QSPIDMA_DST_ADDR_OFST	0x00000800
+#define GQSPI_QSPIDMA_DST_ADDR_MSB_OFST 0x00000828
+
+/* GQSPI register bit masks */
+#define GQSPI_SEL_MASK				0x00000001
+#define GQSPI_EN_MASK				0x00000001
+#define GQSPI_LPBK_DLY_ADJ_USE_LPBK_MASK	0x00000020
+#define GQSPI_ISR_WR_TO_CLR_MASK		0x00000002
+#define GQSPI_IDR_ALL_MASK			0x00000FBE
+#define GQSPI_CFG_MODE_EN_MASK			0xC0000000
+#define GQSPI_CFG_GEN_FIFO_START_MODE_MASK	0x20000000
+#define GQSPI_CFG_ENDIAN_MASK			0x04000000
+#define GQSPI_CFG_EN_POLL_TO_MASK		0x00100000
+#define GQSPI_CFG_WP_HOLD_MASK			0x00080000
+#define GQSPI_CFG_BAUD_RATE_DIV_MASK		0x00000038
+#define GQSPI_CFG_CLK_PHA_MASK			0x00000004
+#define GQSPI_CFG_CLK_POL_MASK			0x00000002
+#define GQSPI_CFG_START_GEN_FIFO_MASK		0x10000000
+#define GQSPI_GENFIFO_IMM_DATA_MASK		0x000000FF
+#define GQSPI_GENFIFO_DATA_XFER			0x00000100
+#define GQSPI_GENFIFO_EXP			0x00000200
+#define GQSPI_GENFIFO_MODE_SPI			0x00000400
+#define GQSPI_GENFIFO_MODE_DUALSPI		0x00000800
+#define GQSPI_GENFIFO_MODE_QUADSPI		0x00000C00
+#define GQSPI_GENFIFO_MODE_MASK			0x00000C00
+#define GQSPI_GENFIFO_CS_LOWER			0x00001000
+#define GQSPI_GENFIFO_CS_UPPER			0x00002000
+#define GQSPI_GENFIFO_BUS_LOWER			0x00004000
+#define GQSPI_GENFIFO_BUS_UPPER			0x00008000
+#define GQSPI_GENFIFO_BUS_BOTH			0x0000C000
+#define GQSPI_GENFIFO_BUS_MASK			0x0000C000
+#define GQSPI_GENFIFO_TX			0x00010000
+#define GQSPI_GENFIFO_RX			0x00020000
+#define GQSPI_GENFIFO_STRIPE			0x00040000
+#define GQSPI_GENFIFO_POLL			0x00080000
+#define GQSPI_GENFIFO_EXP_START			0x00000100
+#define GQSPI_FIFO_CTRL_RST_RX_FIFO_MASK	0x00000004
+#define GQSPI_FIFO_CTRL_RST_TX_FIFO_MASK	0x00000002
+#define GQSPI_FIFO_CTRL_RST_GEN_FIFO_MASK	0x00000001
+#define GQSPI_ISR_RXEMPTY_MASK			0x00000800
+#define GQSPI_ISR_GENFIFOFULL_MASK		0x00000400
+#define GQSPI_ISR_GENFIFONOT_FULL_MASK		0x00000200
+#define GQSPI_ISR_TXEMPTY_MASK			0x00000100
+#define GQSPI_ISR_GENFIFOEMPTY_MASK		0x00000080
+#define GQSPI_ISR_RXFULL_MASK			0x00000020
+#define GQSPI_ISR_RXNEMPTY_MASK			0x00000010
+#define GQSPI_ISR_TXFULL_MASK			0x00000008
+#define GQSPI_ISR_TXNOT_FULL_MASK		0x00000004
+#define GQSPI_ISR_POLL_TIME_EXPIRE_MASK		0x00000002
+#define GQSPI_IER_TXNOT_FULL_MASK		0x00000004
+#define GQSPI_IER_RXEMPTY_MASK			0x00000800
+#define GQSPI_IER_POLL_TIME_EXPIRE_MASK		0x00000002
+#define GQSPI_IER_RXNEMPTY_MASK			0x00000010
+#define GQSPI_IER_GENFIFOEMPTY_MASK		0x00000080
+#define GQSPI_IER_TXEMPTY_MASK			0x00000100
+#define GQSPI_QSPIDMA_DST_INTR_ALL_MASK		0x000000FE
+#define GQSPI_QSPIDMA_DST_STS_WTC		0x0000E000
+#define GQSPI_CFG_MODE_EN_DMA_MASK		0x80000000
+#define GQSPI_ISR_IDR_MASK			0x00000994
+#define GQSPI_QSPIDMA_DST_I_EN_DONE_MASK	0x00000002
+#define GQSPI_QSPIDMA_DST_I_STS_DONE_MASK	0x00000002
+#define GQSPI_IRQ_MASK				0x00000980
+
+#define GQSPI_CFG_BAUD_RATE_DIV_SHIFT		3
+#define GQSPI_GENFIFO_CS_SETUP			0x4
+#define GQSPI_GENFIFO_CS_HOLD			0x3
+#define GQSPI_TXD_DEPTH				64
+#define GQSPI_RX_FIFO_THRESHOLD			32
+#define GQSPI_RX_FIFO_FILL	(GQSPI_RX_FIFO_THRESHOLD * 4)
+#define GQSPI_TX_FIFO_THRESHOLD_RESET_VAL	32
+#define GQSPI_TX_FIFO_FILL	(GQSPI_TXD_DEPTH -\
+				GQSPI_TX_FIFO_THRESHOLD_RESET_VAL)
+#define GQSPI_GEN_FIFO_THRESHOLD_RESET_VAL	0X10
+#define GQSPI_QSPIDMA_DST_CTRL_RESET_VAL	0x803FFA00
+#define GQSPI_SELECT_FLASH_CS_LOWER		0x1
+#define GQSPI_SELECT_FLASH_CS_UPPER		0x2
+#define GQSPI_SELECT_FLASH_CS_BOTH		0x3
+#define GQSPI_SELECT_FLASH_BUS_LOWER		0x1
+#define GQSPI_SELECT_FLASH_BUS_UPPER		0x2
+#define GQSPI_SELECT_FLASH_BUS_BOTH		0x3
+#define GQSPI_BAUD_DIV_MAX	7	/* Baud rate divisor maximum */
+#define GQSPI_BAUD_DIV_SHIFT	2	/* Baud rate divisor shift */
+#define GQSPI_SELECT_MODE_SPI		0x1
+#define GQSPI_SELECT_MODE_DUALSPI	0x2
+#define GQSPI_SELECT_MODE_QUADSPI	0x4
+#define GQSPI_DMA_UNALIGN		0x3
+#define GQSPI_DEFAULT_NUM_CS	1	/* Default number of chip selects */
+
+enum mode_type {GQSPI_MODE_IO, GQSPI_MODE_DMA};
+
+/**
+ * struct zynqmp_qspi - Defines qspi driver instance
+ * @regs:		Virtual address of the QSPI controller registers
+ * @refclk:		Pointer to the peripheral clock
+ * @pclk:		Pointer to the APB clock
+ * @irq:		IRQ number
+ * @dev:		Pointer to struct device
+ * @txbuf:		Pointer to the TX buffer
+ * @rxbuf:		Pointer to the RX buffer
+ * @bytes_to_transfer:	Number of bytes left to transfer
+ * @bytes_to_receive:	Number of bytes left to receive
+ * @genfifocs:		Used for chip select
+ * @genfifobus:		Used to select the upper or lower bus
+ * @dma_rx_bytes:	Remaining bytes to receive by DMA mode
+ * @dma_addr:		DMA address after mapping the kernel buffer
+ * @genfifoentry:	Used for storing the genfifoentry instruction.
+ * @mode:		Defines the mode in which QSPI is operating
+ */
+struct zynqmp_qspi {
+	void __iomem *regs;
+	struct clk *refclk;
+	struct clk *pclk;
+	int irq;
+	struct device *dev;
+	const void *txbuf;
+	void *rxbuf;
+	int bytes_to_transfer;
+	int bytes_to_receive;
+	u32 genfifocs;
+	u32 genfifobus;
+	u32 dma_rx_bytes;
+	dma_addr_t dma_addr;
+	u32 genfifoentry;
+	enum mode_type mode;
+};
+
+/**
+ * zynqmp_gqspi_read:	For GQSPI controller read operation
+ * @xqspi:	Pointer to the zynqmp_qspi structure
+ * @offset:	Offset from where to read
+ */
+static u32 zynqmp_gqspi_read(struct zynqmp_qspi *xqspi, u32 offset)
+{
+	return readl_relaxed(xqspi->regs + offset);
+}
+
+/**
+ * zynqmp_gqspi_write:	For GQSPI controller write operation
+ * @xqspi:	Pointer to the zynqmp_qspi structure
+ * @offset:	Offset where to write
+ * @val:	Value to be written
+ */
+static inline void zynqmp_gqspi_write(struct zynqmp_qspi *xqspi, u32 offset,
+				      u32 val)
+{
+	writel_relaxed(val, (xqspi->regs + offset));
+}
+
+/**
+ * zynqmp_gqspi_selectslave:	For selection of slave device
+ * @instanceptr:	Pointer to the zynqmp_qspi structure
+ * @flashcs:	For chip select
+ * @flashbus:	To check which bus is selected- upper or lower
+ */
+static void zynqmp_gqspi_selectslave(struct zynqmp_qspi *instanceptr,
+				     u8 slavecs, u8 slavebus)
+{
+	/*
+	 * Bus and CS lines selected here will be updated in the instance and
+	 * used for subsequent GENFIFO entries during transfer.
+	 */
+
+	/* Choose slave select line */
+	switch (slavecs) {
+	case GQSPI_SELECT_FLASH_CS_BOTH:
+		instanceptr->genfifocs = GQSPI_GENFIFO_CS_LOWER |
+			GQSPI_GENFIFO_CS_UPPER;
+	case GQSPI_SELECT_FLASH_CS_UPPER:
+		instanceptr->genfifocs = GQSPI_GENFIFO_CS_UPPER;
+		break;
+	case GQSPI_SELECT_FLASH_CS_LOWER:
+		instanceptr->genfifocs = GQSPI_GENFIFO_CS_LOWER;
+		break;
+	default:
+		dev_warn(instanceptr->dev, "Invalid slave select\n");
+	}
+
+	/* Choose the bus */
+	switch (slavebus) {
+	case GQSPI_SELECT_FLASH_BUS_BOTH:
+		instanceptr->genfifobus = GQSPI_GENFIFO_BUS_LOWER |
+			GQSPI_GENFIFO_BUS_UPPER;
+		break;
+	case GQSPI_SELECT_FLASH_BUS_UPPER:
+		instanceptr->genfifobus = GQSPI_GENFIFO_BUS_UPPER;
+		break;
+	case GQSPI_SELECT_FLASH_BUS_LOWER:
+		instanceptr->genfifobus = GQSPI_GENFIFO_BUS_LOWER;
+		break;
+	default:
+		dev_warn(instanceptr->dev, "Invalid slave bus\n");
+	}
+}
+
+/**
+ * zynqmp_qspi_init_hw:	Initialize the hardware
+ * @xqspi:	Pointer to the zynqmp_qspi structure
+ *
+ * The default settings of the QSPI controller's configurable parameters on
+ * reset are
+ *	- Master mode
+ *	- TX threshold set to 1
+ *	- RX threshold set to 1
+ *	- Flash memory interface mode enabled
+ * This function performs the following actions
+ *	- Disable and clear all the interrupts
+ *	- Enable manual slave select
+ *	- Enable manual start
+ *	- Deselect all the chip select lines
+ *	- Set the little endian mode of TX FIFO and
+ *	- Enable the QSPI controller
+ */
+static void zynqmp_qspi_init_hw(struct zynqmp_qspi *xqspi)
+{
+	u32 config_reg;
+
+	/* Select the GQSPI mode */
+	zynqmp_gqspi_write(xqspi, GQSPI_SEL_OFST, GQSPI_SEL_MASK);
+	/* Clear and disable interrupts */
+	zynqmp_gqspi_write(xqspi, GQSPI_ISR_OFST,
+			   zynqmp_gqspi_read(xqspi, GQSPI_ISR_OFST) |
+			   GQSPI_ISR_WR_TO_CLR_MASK);
+	/* Clear the DMA STS */
+	zynqmp_gqspi_write(xqspi, GQSPI_QSPIDMA_DST_I_STS_OFST,
+			   zynqmp_gqspi_read(xqspi,
+					     GQSPI_QSPIDMA_DST_I_STS_OFST));
+	zynqmp_gqspi_write(xqspi, GQSPI_QSPIDMA_DST_STS_OFST,
+			   zynqmp_gqspi_read(xqspi,
+					     GQSPI_QSPIDMA_DST_STS_OFST) |
+					     GQSPI_QSPIDMA_DST_STS_WTC);
+	zynqmp_gqspi_write(xqspi, GQSPI_IDR_OFST, GQSPI_IDR_ALL_MASK);
+	zynqmp_gqspi_write(xqspi,
+			   GQSPI_QSPIDMA_DST_I_DIS_OFST,
+			   GQSPI_QSPIDMA_DST_INTR_ALL_MASK);
+	/* Disable the GQSPI */
+	zynqmp_gqspi_write(xqspi, GQSPI_EN_OFST, 0x0);
+	config_reg = zynqmp_gqspi_read(xqspi, GQSPI_CONFIG_OFST);
+	config_reg &= ~GQSPI_CFG_MODE_EN_MASK;
+	/* Manual start */
+	config_reg |= GQSPI_CFG_GEN_FIFO_START_MODE_MASK;
+	/* Little endian by default */
+	config_reg &= ~GQSPI_CFG_ENDIAN_MASK;
+	/* Disable poll time out */
+	config_reg &= ~GQSPI_CFG_EN_POLL_TO_MASK;
+	/* Set hold bit */
+	config_reg |= GQSPI_CFG_WP_HOLD_MASK;
+	/* Clear pre-scalar by default */
+	config_reg &= ~GQSPI_CFG_BAUD_RATE_DIV_MASK;
+	/* CPHA 0 */
+	config_reg &= ~GQSPI_CFG_CLK_PHA_MASK;
+	/* CPOL 0 */
+	config_reg &= ~GQSPI_CFG_CLK_POL_MASK;
+	zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST, config_reg);
+
+	/* Clear the TX and RX FIFO */
+	zynqmp_gqspi_write(xqspi, GQSPI_FIFO_CTRL_OFST,
+			   GQSPI_FIFO_CTRL_RST_RX_FIFO_MASK |
+			   GQSPI_FIFO_CTRL_RST_TX_FIFO_MASK |
+			   GQSPI_FIFO_CTRL_RST_GEN_FIFO_MASK);
+	/* Set by default to allow for high frequencies */
+	zynqmp_gqspi_write(xqspi, GQSPI_LPBK_DLY_ADJ_OFST,
+			   zynqmp_gqspi_read(xqspi, GQSPI_LPBK_DLY_ADJ_OFST) |
+			   GQSPI_LPBK_DLY_ADJ_USE_LPBK_MASK);
+	/* Reset thresholds */
+	zynqmp_gqspi_write(xqspi, GQSPI_TX_THRESHOLD_OFST,
+			   GQSPI_TX_FIFO_THRESHOLD_RESET_VAL);
+	zynqmp_gqspi_write(xqspi, GQSPI_RX_THRESHOLD_OFST,
+			   GQSPI_RX_FIFO_THRESHOLD);
+	zynqmp_gqspi_write(xqspi, GQSPI_GF_THRESHOLD_OFST,
+			   GQSPI_GEN_FIFO_THRESHOLD_RESET_VAL);
+	zynqmp_gqspi_selectslave(xqspi,
+				 GQSPI_SELECT_FLASH_CS_LOWER,
+				 GQSPI_SELECT_FLASH_BUS_LOWER);
+	/* Initialize DMA */
+	zynqmp_gqspi_write(xqspi,
+			GQSPI_QSPIDMA_DST_CTRL_OFST,
+			GQSPI_QSPIDMA_DST_CTRL_RESET_VAL);
+
+	/* Enable the GQSPI */
+	zynqmp_gqspi_write(xqspi, GQSPI_EN_OFST, GQSPI_EN_MASK);
+}
+
+/**
+ * zynqmp_qspi_copy_read_data:	Copy data to RX buffer
+ * @xqspi:	Pointer to the zynqmp_qspi structure
+ * @data:	The variable where data is stored
+ * @size:	Number of bytes to be copied from data to RX buffer
+ */
+static void zynqmp_qspi_copy_read_data(struct zynqmp_qspi *xqspi,
+				       ulong data, u8 size)
+{
+	memcpy(xqspi->rxbuf, &data, size);
+	xqspi->rxbuf += size;
+	xqspi->bytes_to_receive -= size;
+}
+
+/**
+ * zynqmp_prepare_transfer_hardware:	Prepares hardware for transfer.
+ * @master:	Pointer to the spi_master structure which provides
+ *		information about the controller.
+ *
+ * This function enables SPI master controller.
+ *
+ * Return:	0 on success; error value otherwise
+ */
+static int zynqmp_prepare_transfer_hardware(struct spi_master *master)
+{
+	struct zynqmp_qspi *xqspi = spi_master_get_devdata(master);
+	int ret;
+
+	ret = clk_enable(xqspi->refclk);
+	if (ret)
+		goto clk_err;
+
+	ret = clk_enable(xqspi->pclk);
+	if (ret)
+		goto clk_err;
+
+	zynqmp_gqspi_write(xqspi, GQSPI_EN_OFST, GQSPI_EN_MASK);
+	return 0;
+clk_err:
+	return ret;
+}
+
+/**
+ * zynqmp_unprepare_transfer_hardware:	Relaxes hardware after transfer
+ * @master:	Pointer to the spi_master structure which provides
+ *		information about the controller.
+ *
+ * This function disables the SPI master controller.
+ *
+ * Return:	Always 0
+ */
+static int zynqmp_unprepare_transfer_hardware(struct spi_master *master)
+{
+	struct zynqmp_qspi *xqspi = spi_master_get_devdata(master);
+
+	zynqmp_gqspi_write(xqspi, GQSPI_EN_OFST, 0x0);
+	clk_disable(xqspi->refclk);
+	clk_disable(xqspi->pclk);
+	return 0;
+}
+
+/**
+ * zynqmp_qspi_chipselect:	Select or deselect the chip select line
+ * @qspi:	Pointer to the spi_device structure
+ * @is_high:	Select(0) or deselect (1) the chip select line
+ */
+static void zynqmp_qspi_chipselect(struct spi_device *qspi, bool is_high)
+{
+	struct zynqmp_qspi *xqspi = spi_master_get_devdata(qspi->master);
+	ulong timeout;
+	u32 genfifoentry = 0x0, statusreg;
+
+	genfifoentry |= GQSPI_GENFIFO_MODE_SPI;
+	genfifoentry |= xqspi->genfifobus;
+
+	if (!is_high) {
+		genfifoentry |= xqspi->genfifocs;
+		genfifoentry |= GQSPI_GENFIFO_CS_SETUP;
+	} else {
+		genfifoentry |= GQSPI_GENFIFO_CS_HOLD;
+	}
+
+	zynqmp_gqspi_write(xqspi, GQSPI_GEN_FIFO_OFST, genfifoentry);
+
+	/* Dummy generic FIFO entry */
+	zynqmp_gqspi_write(xqspi, GQSPI_GEN_FIFO_OFST, 0x0);
+
+	/* Manually start the generic FIFO command */
+	zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST,
+			zynqmp_gqspi_read(xqspi, GQSPI_CONFIG_OFST) |
+			GQSPI_CFG_START_GEN_FIFO_MASK);
+
+	timeout = jiffies + msecs_to_jiffies(1000);
+
+	/* Wait until the generic FIFO command is empty */
+	do {
+		statusreg = zynqmp_gqspi_read(xqspi, GQSPI_ISR_OFST);
+
+		if ((statusreg & GQSPI_ISR_GENFIFOEMPTY_MASK) &&
+			(statusreg & GQSPI_ISR_TXEMPTY_MASK))
+			break;
+		else
+			cpu_relax();
+	} while (!time_after_eq(jiffies, timeout));
+
+	if (time_after_eq(jiffies, timeout))
+		dev_err(xqspi->dev, "Chip select timed out\n");
+}
+
+/**
+ * zynqmp_qspi_setup_transfer:	Configure QSPI controller for specified
+ *				transfer
+ * @qspi:	Pointer to the spi_device structure
+ * @transfer:	Pointer to the spi_transfer structure which provides
+ *		information about next transfer setup parameters
+ *
+ * Sets the operational mode of QSPI controller for the next QSPI transfer and
+ * sets the requested clock frequency.
+ *
+ * Return:	Always 0
+ *
+ * Note:
+ *	If the requested frequency is not an exact match with what can be
+ *	obtained using the pre-scalar value, the driver sets the clock
+ *	frequency which is lower than the requested frequency (maximum lower)
+ *	for the transfer.
+ *
+ *	If the requested frequency is higher or lower than that is supported
+ *	by the QSPI controller the driver will set the highest or lowest
+ *	frequency supported by controller.
+ */
+static int zynqmp_qspi_setup_transfer(struct spi_device *qspi,
+				      struct spi_transfer *transfer)
+{
+	struct zynqmp_qspi *xqspi = spi_master_get_devdata(qspi->master);
+	ulong clk_rate;
+	u32 config_reg, req_hz, baud_rate_val = 0;
+
+	if (transfer)
+		req_hz = transfer->speed_hz;
+	else
+		req_hz = qspi->max_speed_hz;
+
+	/* Set the clock frequency */
+	/* If req_hz == 0, default to lowest speed */
+	clk_rate = clk_get_rate(xqspi->refclk);
+
+	while ((baud_rate_val < GQSPI_BAUD_DIV_MAX) &&
+	       (clk_rate /
+		(GQSPI_BAUD_DIV_SHIFT << baud_rate_val)) > req_hz)
+		baud_rate_val++;
+
+	config_reg = zynqmp_gqspi_read(xqspi, GQSPI_CONFIG_OFST);
+
+	/* Set the QSPI clock phase and clock polarity */
+	config_reg &= (~GQSPI_CFG_CLK_PHA_MASK) & (~GQSPI_CFG_CLK_POL_MASK);
+
+	if (qspi->mode & SPI_CPHA)
+		config_reg |= GQSPI_CFG_CLK_PHA_MASK;
+	if (qspi->mode & SPI_CPOL)
+		config_reg |= GQSPI_CFG_CLK_POL_MASK;
+
+	config_reg &= ~GQSPI_CFG_BAUD_RATE_DIV_MASK;
+	config_reg |= (baud_rate_val << GQSPI_CFG_BAUD_RATE_DIV_SHIFT);
+	zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST, config_reg);
+	return 0;
+}
+
+/**
+ * zynqmp_qspi_setup:	Configure the QSPI controller
+ * @qspi:	Pointer to the spi_device structure
+ *
+ * Sets the operational mode of QSPI controller for the next QSPI transfer,
+ * baud rate and divisor value to setup the requested qspi clock.
+ *
+ * Return:	0 on success; error value otherwise.
+ */
+static int zynqmp_qspi_setup(struct spi_device *qspi)
+{
+	if (qspi->master->busy)
+		return -EBUSY;
+	return 0;
+}
+
+/**
+ * zynqmp_qspi_filltxfifo:	Fills the TX FIFO as long as there is room in
+ *				the FIFO or the bytes required to be
+ *				transmitted.
+ * @xqspi:	Pointer to the zynqmp_qspi structure
+ * @size:	Number of bytes to be copied from TX buffer to TX FIFO
+ */
+static void zynqmp_qspi_filltxfifo(struct zynqmp_qspi *xqspi, int size)
+{
+	u32 count = 0, intermediate;
+
+	while ((xqspi->bytes_to_transfer > 0) && (count < size)) {
+		memcpy(&intermediate, xqspi->txbuf, 4);
+		zynqmp_gqspi_write(xqspi, GQSPI_TXD_OFST, intermediate);
+
+		if (xqspi->bytes_to_transfer >= 4) {
+			xqspi->txbuf += 4;
+			xqspi->bytes_to_transfer -= 4;
+		} else {
+			xqspi->txbuf += xqspi->bytes_to_transfer;
+			xqspi->bytes_to_transfer = 0;
+		}
+		count++;
+	}
+}
+
+/**
+ * zynqmp_qspi_readrxfifo:	Fills the RX FIFO as long as there is room in
+ *				the FIFO.
+ * @xqspi:	Pointer to the zynqmp_qspi structure
+ * @size:	Number of bytes to be copied from RX buffer to RX FIFO
+ */
+static void zynqmp_qspi_readrxfifo(struct zynqmp_qspi *xqspi, u32 size)
+{
+	ulong data;
+	int count = 0;
+
+	while ((count < size) && (xqspi->bytes_to_receive > 0)) {
+		if (xqspi->bytes_to_receive >= 4) {
+			(*(u32 *) xqspi->rxbuf) =
+			zynqmp_gqspi_read(xqspi, GQSPI_RXD_OFST);
+			xqspi->rxbuf += 4;
+			xqspi->bytes_to_receive -= 4;
+			count += 4;
+		} else {
+			data = zynqmp_gqspi_read(xqspi, GQSPI_RXD_OFST);
+			count += xqspi->bytes_to_receive;
+			zynqmp_qspi_copy_read_data(xqspi, data,
+						   xqspi->bytes_to_receive);
+			xqspi->bytes_to_receive = 0;
+		}
+	}
+}
+
+/**
+ * zynqmp_process_dma_irq:	Handler for DMA done interrupt of QSPI
+ *				controller
+ * @xqspi:	zynqmp_qspi instance pointer
+ *
+ * This function handles DMA interrupt only.
+ */
+static void zynqmp_process_dma_irq(struct zynqmp_qspi *xqspi)
+{
+	u32 config_reg, genfifoentry;
+
+	dma_unmap_single(xqspi->dev, xqspi->dma_addr,
+				xqspi->dma_rx_bytes, DMA_FROM_DEVICE);
+	xqspi->rxbuf += xqspi->dma_rx_bytes;
+	xqspi->bytes_to_receive -= xqspi->dma_rx_bytes;
+	xqspi->dma_rx_bytes = 0;
+
+	/* Disabling the DMA interrupts */
+	zynqmp_gqspi_write(xqspi, GQSPI_QSPIDMA_DST_I_DIS_OFST,
+					GQSPI_QSPIDMA_DST_I_EN_DONE_MASK);
+
+	if (xqspi->bytes_to_receive > 0) {
+		/* Switch to IO mode,for remaining bytes to receive */
+		config_reg = zynqmp_gqspi_read(xqspi, GQSPI_CONFIG_OFST);
+		config_reg &= ~GQSPI_CFG_MODE_EN_MASK;
+		zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST, config_reg);
+
+		/* Initiate the transfer of remaining bytes */
+		genfifoentry = xqspi->genfifoentry;
+		genfifoentry |= xqspi->bytes_to_receive;
+		zynqmp_gqspi_write(xqspi, GQSPI_GEN_FIFO_OFST, genfifoentry);
+
+		/* Dummy generic FIFO entry */
+		zynqmp_gqspi_write(xqspi, GQSPI_GEN_FIFO_OFST, 0x0);
+
+		/* Manual start */
+		zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST,
+			(zynqmp_gqspi_read(xqspi, GQSPI_CONFIG_OFST) |
+			GQSPI_CFG_START_GEN_FIFO_MASK));
+
+		/* Enable the RX interrupts for IO mode */
+		zynqmp_gqspi_write(xqspi, GQSPI_IER_OFST,
+				GQSPI_IER_GENFIFOEMPTY_MASK |
+				GQSPI_IER_RXNEMPTY_MASK |
+				GQSPI_IER_RXEMPTY_MASK);
+	}
+}
+
+/**
+ * zynqmp_qspi_irq:	Interrupt service routine of the QSPI controller
+ * @irq:	IRQ number
+ * @dev_id:	Pointer to the xqspi structure
+ *
+ * This function handles TX empty only.
+ * On TX empty interrupt this function reads the received data from RX FIFO
+ * and fills the TX FIFO if there is any data remaining to be transferred.
+ *
+ * Return:	IRQ_HANDLED when interrupt is handled
+ *		IRQ_NONE otherwise.
+ */
+static irqreturn_t zynqmp_qspi_irq(int irq, void *dev_id)
+{
+	struct spi_master *master = dev_id;
+	struct zynqmp_qspi *xqspi = spi_master_get_devdata(master);
+	int ret = IRQ_NONE;
+	u32 status, mask, dma_status = 0;
+
+	status = zynqmp_gqspi_read(xqspi, GQSPI_ISR_OFST);
+	zynqmp_gqspi_write(xqspi, GQSPI_ISR_OFST, status);
+	mask = (status & ~(zynqmp_gqspi_read(xqspi, GQSPI_IMASK_OFST)));
+
+	/* Read and clear DMA status */
+	if (xqspi->mode == GQSPI_MODE_DMA) {
+		dma_status =
+			zynqmp_gqspi_read(xqspi, GQSPI_QSPIDMA_DST_I_STS_OFST);
+		zynqmp_gqspi_write(xqspi, GQSPI_QSPIDMA_DST_I_STS_OFST,
+								dma_status);
+	}
+
+	if (mask & GQSPI_ISR_TXNOT_FULL_MASK) {
+		zynqmp_qspi_filltxfifo(xqspi, GQSPI_TX_FIFO_FILL);
+		ret = IRQ_HANDLED;
+	}
+
+	if (dma_status & GQSPI_QSPIDMA_DST_I_STS_DONE_MASK) {
+		zynqmp_process_dma_irq(xqspi);
+		ret = IRQ_HANDLED;
+	} else if (!(mask & GQSPI_IER_RXEMPTY_MASK) &&
+			(mask & GQSPI_IER_GENFIFOEMPTY_MASK)) {
+		zynqmp_qspi_readrxfifo(xqspi, GQSPI_RX_FIFO_FILL);
+		ret = IRQ_HANDLED;
+	}
+
+	if ((xqspi->bytes_to_receive == 0) && (xqspi->bytes_to_transfer == 0)
+			&& ((status & GQSPI_IRQ_MASK) == GQSPI_IRQ_MASK)) {
+		zynqmp_gqspi_write(xqspi, GQSPI_IDR_OFST, GQSPI_ISR_IDR_MASK);
+		spi_finalize_current_transfer(master);
+		ret = IRQ_HANDLED;
+	}
+	return ret;
+}
+
+/**
+ * zynqmp_qspi_selectspimode:	Selects SPI mode - x1 or x2 or x4.
+ * @xqspi:	xqspi is a pointer to the GQSPI instance
+ * @spimode:	spimode - SPI or DUAL or QUAD.
+ * Return:	Mask to set desired SPI mode in GENFIFO entry.
+ */
+static inline u32 zynqmp_qspi_selectspimode(struct zynqmp_qspi *xqspi,
+						u8 spimode)
+{
+	u32 mask = 0;
+
+	switch (spimode) {
+	case GQSPI_SELECT_MODE_DUALSPI:
+		mask = GQSPI_GENFIFO_MODE_DUALSPI;
+		break;
+	case GQSPI_SELECT_MODE_QUADSPI:
+		mask = GQSPI_GENFIFO_MODE_QUADSPI;
+		break;
+	case GQSPI_SELECT_MODE_SPI:
+		mask = GQSPI_GENFIFO_MODE_SPI;
+		break;
+	default:
+		dev_warn(xqspi->dev, "Invalid SPI mode\n");
+	}
+
+	return mask;
+}
+
+/**
+ * zynq_qspi_setuprxdma:	This function sets up the RX DMA operation
+ * @xqspi:	xqspi is a pointer to the GQSPI instance.
+ */
+static void zynq_qspi_setuprxdma(struct zynqmp_qspi *xqspi)
+{
+	u32 rx_bytes, rx_rem, config_reg;
+	dma_addr_t addr;
+	u64 dma_align =  (u64)(uintptr_t)xqspi->rxbuf;
+
+	if ((xqspi->bytes_to_receive < 8) ||
+		((dma_align & GQSPI_DMA_UNALIGN) != 0x0)) {
+		/* Setting to IO mode */
+		config_reg = zynqmp_gqspi_read(xqspi, GQSPI_CONFIG_OFST);
+		config_reg &= ~GQSPI_CFG_MODE_EN_MASK;
+		zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST, config_reg);
+		xqspi->mode = GQSPI_MODE_IO;
+		xqspi->dma_rx_bytes = 0;
+		return;
+	}
+
+	rx_rem = xqspi->bytes_to_receive % 4;
+	rx_bytes = (xqspi->bytes_to_receive - rx_rem);
+
+	addr = dma_map_single(xqspi->dev, (void *)xqspi->rxbuf,
+						rx_bytes, DMA_FROM_DEVICE);
+	if (dma_mapping_error(xqspi->dev, addr))
+		dev_err(xqspi->dev, "ERR:rxdma:memory not mapped\n");
+
+	xqspi->dma_rx_bytes = rx_bytes;
+	xqspi->dma_addr = addr;
+	zynqmp_gqspi_write(xqspi, GQSPI_QSPIDMA_DST_ADDR_OFST,
+				(u32)(addr & 0xffffffff));
+	addr = ((addr >> 16) >> 16);
+	zynqmp_gqspi_write(xqspi, GQSPI_QSPIDMA_DST_ADDR_MSB_OFST,
+				((u32)addr) & 0xfff);
+
+	/* Enabling the DMA mode */
+	config_reg = zynqmp_gqspi_read(xqspi, GQSPI_CONFIG_OFST);
+	config_reg &= ~GQSPI_CFG_MODE_EN_MASK;
+	config_reg |= GQSPI_CFG_MODE_EN_DMA_MASK;
+	zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST, config_reg);
+
+	/* Switch to DMA mode */
+	xqspi->mode = GQSPI_MODE_DMA;
+
+	/* Write the number of bytes to transfer */
+	zynqmp_gqspi_write(xqspi, GQSPI_QSPIDMA_DST_SIZE_OFST, rx_bytes);
+}
+
+/**
+ * zynqmp_qspi_txrxsetup:	This function checks the TX/RX buffers in
+ *				the transfer and sets up the GENFIFO entries,
+ *				TX FIFO as required.
+ * @xqspi:	xqspi is a pointer to the GQSPI instance.
+ * @transfer:	It is a pointer to the structure containing transfer data.
+ * @genfifoentry:	genfifoentry is pointer to the variable in which
+ *			GENFIFO	mask is returned to calling function
+ */
+static void zynqmp_qspi_txrxsetup(struct zynqmp_qspi *xqspi,
+				  struct spi_transfer *transfer,
+				  u32 *genfifoentry)
+{
+	u32 config_reg;
+
+	/* Transmit */
+	if ((xqspi->txbuf != NULL) && (xqspi->rxbuf == NULL)) {
+		/* Setup data to be TXed */
+		*genfifoentry &= ~GQSPI_GENFIFO_RX;
+		*genfifoentry |= GQSPI_GENFIFO_DATA_XFER;
+		*genfifoentry |= GQSPI_GENFIFO_TX;
+		*genfifoentry |=
+			zynqmp_qspi_selectspimode(xqspi, transfer->tx_nbits);
+		xqspi->bytes_to_transfer = transfer->len;
+		if (xqspi->mode == GQSPI_MODE_DMA) {
+			config_reg = zynqmp_gqspi_read(xqspi,
+							GQSPI_CONFIG_OFST);
+			config_reg &= ~GQSPI_CFG_MODE_EN_MASK;
+			zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST,
+								config_reg);
+			xqspi->mode = GQSPI_MODE_IO;
+		}
+		zynqmp_qspi_filltxfifo(xqspi, GQSPI_TXD_DEPTH);
+		/* Discard RX data */
+		xqspi->bytes_to_receive = 0;
+	} else if ((xqspi->txbuf == NULL) && (xqspi->rxbuf != NULL)) {
+		/* Receive */
+
+		/* TX auto fill */
+		*genfifoentry &= ~GQSPI_GENFIFO_TX;
+		/* Setup RX */
+		*genfifoentry |= GQSPI_GENFIFO_DATA_XFER;
+		*genfifoentry |= GQSPI_GENFIFO_RX;
+		*genfifoentry |=
+			zynqmp_qspi_selectspimode(xqspi, transfer->rx_nbits);
+		xqspi->bytes_to_transfer = 0;
+		xqspi->bytes_to_receive = transfer->len;
+		zynq_qspi_setuprxdma(xqspi);
+	}
+}
+
+/**
+ * zynqmp_qspi_start_transfer:	Initiates the QSPI transfer
+ * @master:	Pointer to the spi_master structure which provides
+ *		information about the controller.
+ * @qspi:	Pointer to the spi_device structure
+ * @transfer:	Pointer to the spi_transfer structure which provide information
+ *		about next transfer parameters
+ *
+ * This function fills the TX FIFO, starts the QSPI transfer, and waits for the
+ * transfer to be completed.
+ *
+ * Return:	Number of bytes transferred in the last transfer
+ */
+static int zynqmp_qspi_start_transfer(struct spi_master *master,
+				      struct spi_device *qspi,
+				      struct spi_transfer *transfer)
+{
+	struct zynqmp_qspi *xqspi = spi_master_get_devdata(master);
+	u32 genfifoentry = 0x0, transfer_len;
+
+	xqspi->txbuf = transfer->tx_buf;
+	xqspi->rxbuf = transfer->rx_buf;
+
+	zynqmp_qspi_setup_transfer(qspi, transfer);
+
+	genfifoentry |= xqspi->genfifocs;
+	genfifoentry |= xqspi->genfifobus;
+
+	zynqmp_qspi_txrxsetup(xqspi, transfer, &genfifoentry);
+
+	if (xqspi->mode == GQSPI_MODE_DMA)
+		transfer_len = xqspi->dma_rx_bytes;
+	else
+		transfer_len = transfer->len;
+
+	xqspi->genfifoentry = genfifoentry;
+	if ((transfer_len) < GQSPI_GENFIFO_IMM_DATA_MASK) {
+		genfifoentry &= ~GQSPI_GENFIFO_IMM_DATA_MASK;
+		genfifoentry |= transfer_len;
+		zynqmp_gqspi_write(xqspi, GQSPI_GEN_FIFO_OFST, genfifoentry);
+	} else {
+		int tempcount = transfer_len;
+		u32 exponent = 8;	/* 2^8 = 256 */
+		u8 imm_data = tempcount & 0xFF;
+
+		tempcount &= ~(tempcount & 0xFF);
+		/* Immediate entry */
+		if (tempcount != 0) {
+			/* Exponent entries */
+			genfifoentry |= GQSPI_GENFIFO_EXP;
+			while (tempcount != 0) {
+				if (tempcount & GQSPI_GENFIFO_EXP_START) {
+					genfifoentry &=
+					    ~GQSPI_GENFIFO_IMM_DATA_MASK;
+					genfifoentry |= exponent;
+					zynqmp_gqspi_write(xqspi,
+							   GQSPI_GEN_FIFO_OFST,
+							   genfifoentry);
+				}
+				tempcount = tempcount >> 1;
+				exponent++;
+			}
+		}
+		if (imm_data != 0) {
+			genfifoentry &= ~GQSPI_GENFIFO_EXP;
+			genfifoentry &= ~GQSPI_GENFIFO_IMM_DATA_MASK;
+			genfifoentry |= (u8) (imm_data & 0xFF);
+			zynqmp_gqspi_write(xqspi,
+					   GQSPI_GEN_FIFO_OFST, genfifoentry);
+		}
+	}
+
+	if ((xqspi->mode == GQSPI_MODE_IO) &&
+			(xqspi->rxbuf != NULL)) {
+		/* Dummy generic FIFO entry */
+		zynqmp_gqspi_write(xqspi, GQSPI_GEN_FIFO_OFST, 0x0);
+	}
+
+	/* Since we are using manual mode */
+	zynqmp_gqspi_write(xqspi, GQSPI_CONFIG_OFST,
+			   zynqmp_gqspi_read(xqspi, GQSPI_CONFIG_OFST) |
+			   GQSPI_CFG_START_GEN_FIFO_MASK);
+
+	if (xqspi->txbuf != NULL)
+		/* Enable interrupts for TX */
+		zynqmp_gqspi_write(xqspi, GQSPI_IER_OFST,
+				   GQSPI_IER_TXEMPTY_MASK |
+					GQSPI_IER_GENFIFOEMPTY_MASK |
+					GQSPI_IER_TXNOT_FULL_MASK);
+
+	if (xqspi->rxbuf != NULL) {
+		/* Enable interrupts for RX */
+		if (xqspi->mode == GQSPI_MODE_DMA) {
+			/* Enable DMA interrupts */
+			zynqmp_gqspi_write(xqspi,
+					GQSPI_QSPIDMA_DST_I_EN_OFST,
+					GQSPI_QSPIDMA_DST_I_EN_DONE_MASK);
+		} else {
+			zynqmp_gqspi_write(xqspi, GQSPI_IER_OFST,
+					GQSPI_IER_GENFIFOEMPTY_MASK |
+					GQSPI_IER_RXNEMPTY_MASK |
+					GQSPI_IER_RXEMPTY_MASK);
+		}
+	}
+
+	return transfer->len;
+}
+
+/**
+ * zynqmp_qspi_suspend:	Suspend method for the QSPI driver
+ * @_dev:	Address of the platform_device structure
+ *
+ * This function stops the QSPI driver queue and disables the QSPI controller
+ *
+ * Return:	Always 0
+ */
+static int __maybe_unused zynqmp_qspi_suspend(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+						    struct platform_device,
+						    dev);
+	struct spi_master *master = platform_get_drvdata(pdev);
+
+	spi_master_suspend(master);
+
+	zynqmp_unprepare_transfer_hardware(master);
+
+	return 0;
+}
+
+/**
+ * zynqmp_qspi_resume:	Resume method for the QSPI driver
+ * @dev:	Address of the platform_device structure
+ *
+ * The function starts the QSPI driver queue and initializes the QSPI
+ * controller
+ *
+ * Return:	0 on success; error value otherwise
+ */
+static int __maybe_unused zynqmp_qspi_resume(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+						    struct platform_device,
+						    dev);
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct zynqmp_qspi *xqspi = spi_master_get_devdata(master);
+	int ret = 0;
+
+	ret = clk_enable(xqspi->pclk);
+	if (ret) {
+		dev_err(dev, "Cannot enable APB clock.\n");
+		return ret;
+	}
+
+	ret = clk_enable(xqspi->refclk);
+	if (ret) {
+		dev_err(dev, "Cannot enable device clock.\n");
+		clk_disable(xqspi->pclk);
+		return ret;
+	}
+
+	spi_master_resume(master);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(zynqmp_qspi_dev_pm_ops, zynqmp_qspi_suspend,
+			 zynqmp_qspi_resume);
+
+/**
+ * zynqmp_qspi_probe:	Probe method for the QSPI driver
+ * @pdev:	Pointer to the platform_device structure
+ *
+ * This function initializes the driver data structures and the hardware.
+ *
+ * Return:	0 on success; error value otherwise
+ */
+static int zynqmp_qspi_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct spi_master *master;
+	struct zynqmp_qspi *xqspi;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+
+	master = spi_alloc_master(&pdev->dev, sizeof(*xqspi));
+	if (!master)
+		return -ENOMEM;
+
+	xqspi = spi_master_get_devdata(master);
+	master->dev.of_node = pdev->dev.of_node;
+	platform_set_drvdata(pdev, master);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	xqspi->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(xqspi->regs)) {
+		ret = PTR_ERR(xqspi->regs);
+		goto remove_master;
+	}
+
+	xqspi->dev = dev;
+	xqspi->pclk = devm_clk_get(&pdev->dev, "pclk");
+	if (IS_ERR(xqspi->pclk)) {
+		dev_err(dev, "pclk clock not found.\n");
+		ret = PTR_ERR(xqspi->pclk);
+		goto remove_master;
+	}
+
+	ret = clk_prepare_enable(xqspi->pclk);
+	if (ret) {
+		dev_err(dev, "Unable to enable APB clock.\n");
+		goto remove_master;
+	}
+
+	xqspi->refclk = devm_clk_get(&pdev->dev, "ref_clk");
+	if (IS_ERR(xqspi->refclk)) {
+		dev_err(dev, "ref_clk clock not found.\n");
+		ret = PTR_ERR(xqspi->refclk);
+		goto clk_dis_pclk;
+	}
+
+	ret = clk_prepare_enable(xqspi->refclk);
+	if (ret) {
+		dev_err(dev, "Unable to enable device clock.\n");
+		goto clk_dis_pclk;
+	}
+
+	/* QSPI controller initializations */
+	zynqmp_qspi_init_hw(xqspi);
+
+	xqspi->irq = platform_get_irq(pdev, 0);
+	if (xqspi->irq <= 0) {
+		ret = -ENXIO;
+		dev_err(dev, "irq resource not found\n");
+		goto clk_dis_all;
+	}
+	ret = devm_request_irq(&pdev->dev, xqspi->irq, zynqmp_qspi_irq,
+			       0, pdev->name, master);
+	if (ret != 0) {
+		ret = -ENXIO;
+		dev_err(dev, "request_irq failed\n");
+		goto clk_dis_all;
+	}
+
+	master->num_chipselect = GQSPI_DEFAULT_NUM_CS;
+
+	master->setup = zynqmp_qspi_setup;
+	master->set_cs = zynqmp_qspi_chipselect;
+	master->transfer_one = zynqmp_qspi_start_transfer;
+	master->prepare_transfer_hardware = zynqmp_prepare_transfer_hardware;
+	master->unprepare_transfer_hardware =
+					zynqmp_unprepare_transfer_hardware;
+	master->max_speed_hz = clk_get_rate(xqspi->refclk) / 2;
+	master->bits_per_word_mask = SPI_BPW_MASK(8);
+	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_RX_DUAL | SPI_RX_QUAD |
+			    SPI_TX_DUAL | SPI_TX_QUAD;
+
+	if (master->dev.parent == NULL)
+		master->dev.parent = &master->dev;
+
+	ret = spi_register_master(master);
+	if (ret)
+		goto clk_dis_all;
+
+	return 0;
+
+clk_dis_all:
+	clk_disable_unprepare(xqspi->refclk);
+clk_dis_pclk:
+	clk_disable_unprepare(xqspi->pclk);
+remove_master:
+	spi_master_put(master);
+
+	return ret;
+}
+
+/**
+ * zynqmp_qspi_remove:	Remove method for the QSPI driver
+ * @pdev:	Pointer to the platform_device structure
+ *
+ * This function is called if a device is physically removed from the system or
+ * if the driver module is being unloaded. It frees all resources allocated to
+ * the device.
+ *
+ * Return:	0 Always
+ */
+static int zynqmp_qspi_remove(struct platform_device *pdev)
+{
+	struct spi_master *master = platform_get_drvdata(pdev);
+	struct zynqmp_qspi *xqspi = spi_master_get_devdata(master);
+
+	zynqmp_gqspi_write(xqspi, GQSPI_EN_OFST, 0x0);
+	clk_disable_unprepare(xqspi->refclk);
+	clk_disable_unprepare(xqspi->pclk);
+
+	spi_unregister_master(master);
+
+	return 0;
+}
+
+static const struct of_device_id zynqmp_qspi_of_match[] = {
+	{ .compatible = "xlnx,zynqmp-qspi-1.0", },
+	{ /* End of table */ }
+};
+
+MODULE_DEVICE_TABLE(of, zynqmp_qspi_of_match);
+
+static struct platform_driver zynqmp_qspi_driver = {
+	.probe = zynqmp_qspi_probe,
+	.remove = zynqmp_qspi_remove,
+	.driver = {
+		.name = "zynqmp-qspi",
+		.of_match_table = zynqmp_qspi_of_match,
+		.pm = &zynqmp_qspi_dev_pm_ops,
+	},
+};
+
+module_platform_driver(zynqmp_qspi_driver);
+
+MODULE_AUTHOR("Xilinx, Inc.");
+MODULE_DESCRIPTION("Xilinx Zynqmp QSPI driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 50910d85df5a..cf8b91b23a76 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -571,7 +571,7 @@ static int __spi_map_msg(struct spi_master *master, struct spi_message *msg)
 	return 0;
 }
 
-static int spi_unmap_msg(struct spi_master *master, struct spi_message *msg)
+static int __spi_unmap_msg(struct spi_master *master, struct spi_message *msg)
 {
 	struct spi_transfer *xfer;
 	struct device *tx_dev, *rx_dev;
@@ -583,15 +583,6 @@ static int spi_unmap_msg(struct spi_master *master, struct spi_message *msg)
 	rx_dev = master->dma_rx->device->dev;
 
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
-		/*
-		 * Restore the original value of tx_buf or rx_buf if they are
-		 * NULL.
-		 */
-		if (xfer->tx_buf == master->dummy_tx)
-			xfer->tx_buf = NULL;
-		if (xfer->rx_buf == master->dummy_rx)
-			xfer->rx_buf = NULL;
-
 		if (!master->can_dma(master, msg->spi, xfer))
 			continue;
 
@@ -608,13 +599,32 @@ static inline int __spi_map_msg(struct spi_master *master,
 	return 0;
 }
 
-static inline int spi_unmap_msg(struct spi_master *master,
-				struct spi_message *msg)
+static inline int __spi_unmap_msg(struct spi_master *master,
+				  struct spi_message *msg)
 {
 	return 0;
 }
 #endif /* !CONFIG_HAS_DMA */
 
+static inline int spi_unmap_msg(struct spi_master *master,
+				struct spi_message *msg)
+{
+	struct spi_transfer *xfer;
+
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		/*
+		 * Restore the original value of tx_buf or rx_buf if they are
+		 * NULL.
+		 */
+		if (xfer->tx_buf == master->dummy_tx)
+			xfer->tx_buf = NULL;
+		if (xfer->rx_buf == master->dummy_rx)
+			xfer->rx_buf = NULL;
+	}
+
+	return __spi_unmap_msg(master, msg);
+}
+
 static int spi_map_msg(struct spi_master *master, struct spi_message *msg)
 {
 	struct spi_transfer *xfer;
@@ -988,9 +998,6 @@ void spi_finalize_current_message(struct spi_master *master)
 
 	spin_lock_irqsave(&master->queue_lock, flags);
 	mesg = master->cur_msg;
-	master->cur_msg = NULL;
-
-	queue_kthread_work(&master->kworker, &master->pump_messages);
 	spin_unlock_irqrestore(&master->queue_lock, flags);
 
 	spi_unmap_msg(master, mesg);
@@ -1003,9 +1010,13 @@ void spi_finalize_current_message(struct spi_master *master)
 		}
 	}
 
-	trace_spi_message_done(mesg);
-
+	spin_lock_irqsave(&master->queue_lock, flags);
+	master->cur_msg = NULL;
 	master->cur_msg_prepared = false;
+	queue_kthread_work(&master->kworker, &master->pump_messages);
+	spin_unlock_irqrestore(&master->queue_lock, flags);
+
+	trace_spi_message_done(mesg);
 
 	mesg->state = NULL;
 	if (mesg->complete)
diff --git a/drivers/spi/spidev.c b/drivers/spi/spidev.c
index 92c909eed6b5..dd616ff0ffc5 100644
--- a/drivers/spi/spidev.c
+++ b/drivers/spi/spidev.c
@@ -95,37 +95,25 @@ MODULE_PARM_DESC(bufsiz, "data bytes in biggest supported SPI message");
 
 /*-------------------------------------------------------------------------*/
 
-/*
- * We can't use the standard synchronous wrappers for file I/O; we
- * need to protect against async removal of the underlying spi_device.
- */
-static void spidev_complete(void *arg)
-{
-	complete(arg);
-}
-
 static ssize_t
 spidev_sync(struct spidev_data *spidev, struct spi_message *message)
 {
 	DECLARE_COMPLETION_ONSTACK(done);
 	int status;
-
-	message->complete = spidev_complete;
-	message->context = &done;
+	struct spi_device *spi;
 
 	spin_lock_irq(&spidev->spi_lock);
-	if (spidev->spi == NULL)
+	spi = spidev->spi;
+	spin_unlock_irq(&spidev->spi_lock);
+
+	if (spi == NULL)
 		status = -ESHUTDOWN;
 	else
-		status = spi_async(spidev->spi, message);
-	spin_unlock_irq(&spidev->spi_lock);
+		status = spi_sync(spi, message);
+
+	if (status == 0)
+		status = message->actual_length;
 
-	if (status == 0) {
-		wait_for_completion(&done);
-		status = message->status;
-		if (status == 0)
-			status = message->actual_length;
-	}
 	return status;
 }
 
@@ -647,7 +635,6 @@ err_find_dev:
 static int spidev_release(struct inode *inode, struct file *filp)
 {
 	struct spidev_data	*spidev;
-	int			status = 0;
 
 	mutex_lock(&device_list_lock);
 	spidev = filp->private_data;
@@ -676,7 +663,7 @@ static int spidev_release(struct inode *inode, struct file *filp)
 	}
 	mutex_unlock(&device_list_lock);
 
-	return status;
+	return 0;
 }
 
 static const struct file_operations spidev_fops = {
diff --git a/drivers/spmi/Kconfig b/drivers/spmi/Kconfig
index c8d99563d245..982580af1d16 100644
--- a/drivers/spmi/Kconfig
+++ b/drivers/spmi/Kconfig
@@ -14,6 +14,7 @@ config SPMI_MSM_PMIC_ARB
 	tristate "Qualcomm MSM SPMI Controller (PMIC Arbiter)"
 	depends on IRQ_DOMAIN
 	depends on ARCH_QCOM || COMPILE_TEST
+	depends on HAS_IOMEM
 	default ARCH_QCOM
 	help
 	  If you say yes to this option, support will be included for the
diff --git a/drivers/ssb/driver_chipcommon_pmu.c b/drivers/ssb/driver_chipcommon_pmu.c
index 09428412139e..c5352ea4821e 100644
--- a/drivers/ssb/driver_chipcommon_pmu.c
+++ b/drivers/ssb/driver_chipcommon_pmu.c
@@ -621,8 +621,8 @@ static u32 ssb_pmu_get_alp_clock_clk0(struct ssb_chipcommon *cc)
 	u32 crystalfreq;
 	const struct pmu0_plltab_entry *e = NULL;
 
-	crystalfreq = chipco_read32(cc, SSB_CHIPCO_PMU_CTL) &
-		      SSB_CHIPCO_PMU_CTL_XTALFREQ >> SSB_CHIPCO_PMU_CTL_XTALFREQ_SHIFT;
+	crystalfreq = (chipco_read32(cc, SSB_CHIPCO_PMU_CTL) &
+		       SSB_CHIPCO_PMU_CTL_XTALFREQ)  >> SSB_CHIPCO_PMU_CTL_XTALFREQ_SHIFT;
 	e = pmu0_plltab_find_entry(crystalfreq);
 	BUG_ON(!e);
 	return e->freq * 1000;
@@ -634,7 +634,7 @@ u32 ssb_pmu_get_alp_clock(struct ssb_chipcommon *cc)
 
 	switch (bus->chip_id) {
 	case 0x5354:
-		ssb_pmu_get_alp_clock_clk0(cc);
+		return ssb_pmu_get_alp_clock_clk0(cc);
 	default:
 		ssb_err("ERROR: PMU alp clock unknown for device %04X\n",
 			bus->chip_id);
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index bfacf69f68f4..7f6cae5beb90 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -108,8 +108,8 @@ source "drivers/staging/clocking-wizard/Kconfig"
 
 source "drivers/staging/fbtft/Kconfig"
 
-source "drivers/staging/i2o/Kconfig"
-
 source "drivers/staging/fsl-mc/Kconfig"
 
+source "drivers/staging/wilc1000/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 2bbd1bf04c55..347f6477aa3e 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -23,7 +23,7 @@ obj-$(CONFIG_VT6656)		+= vt6656/
 obj-$(CONFIG_VME_BUS)		+= vme/
 obj-$(CONFIG_IIO)		+= iio/
 obj-$(CONFIG_FB_SM7XX)		+= sm7xxfb/
-obj-$(CONFIG_FB_SM7XX)		+= sm750fb/
+obj-$(CONFIG_FB_SM750)		+= sm750fb/
 obj-$(CONFIG_FB_XGI)		+= xgifb/
 obj-$(CONFIG_USB_EMXX)		+= emxx_udc/
 obj-$(CONFIG_FT1000)		+= ft1000/
@@ -46,5 +46,5 @@ obj-$(CONFIG_CRYPTO_SKEIN)	+= skein/
 obj-$(CONFIG_UNISYSSPAR)	+= unisys/
 obj-$(CONFIG_COMMON_CLK_XLNX_CLKWZRD)	+= clocking-wizard/
 obj-$(CONFIG_FB_TFT)		+= fbtft/
-obj-$(CONFIG_I2O)		+= i2o/
 obj-$(CONFIG_FSL_MC_BUS)	+= fsl-mc/
+obj-$(CONFIG_WILC1000)		+= wilc1000/
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index 8feb9048e62f..24d657b3ab99 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -22,11 +22,20 @@ config ANDROID_TIMED_GPIO
 	tristate "Android timed gpio driver"
 	depends on GPIOLIB && ANDROID_TIMED_OUTPUT
 	default n
+        ---help---
+	  Unlike generic gpio is to allow programs to access and manipulate gpio
+	  registers from user space, timed output/gpio is a system to allow changing
+	  a gpio pin and restore it automatically after a specified timeout.
 
 config ANDROID_LOW_MEMORY_KILLER
 	bool "Android Low Memory Killer"
 	---help---
-	  Registers processes to be killed when memory is low
+	  Registers processes to be killed when low memory conditions, this is useful
+	  as there is no particular swap space on android.
+
+	  The registered process will kills according to the priorities in android init
+	  scripts (/init.rc), and it defines priority values with minimum free memory size
+	  for each priority.
 
 config SYNC
 	bool "Synchronization framework"
diff --git a/drivers/staging/android/ion/ion.c b/drivers/staging/android/ion/ion.c
index b0b96ab31954..6f4811263557 100644
--- a/drivers/staging/android/ion/ion.c
+++ b/drivers/staging/android/ion/ion.c
@@ -1579,6 +1579,7 @@ struct ion_device *ion_device_create(long (*custom_ioctl)
 	ret = misc_register(&idev->dev);
 	if (ret) {
 		pr_err("ion: failed to register misc device.\n");
+		kfree(idev);
 		return ERR_PTR(ret);
 	}
 
diff --git a/drivers/staging/android/ion/ion_chunk_heap.c b/drivers/staging/android/ion/ion_chunk_heap.c
index 3e6ec2ee6802..54746157d799 100644
--- a/drivers/staging/android/ion/ion_chunk_heap.c
+++ b/drivers/staging/android/ion/ion_chunk_heap.c
@@ -173,7 +173,7 @@ struct ion_heap *ion_chunk_heap_create(struct ion_platform_heap *heap_data)
 	chunk_heap->heap.ops = &chunk_heap_ops;
 	chunk_heap->heap.type = ION_HEAP_TYPE_CHUNK;
 	chunk_heap->heap.flags = ION_HEAP_FLAG_DEFER_FREE;
-	pr_info("%s: base %lu size %zu align %ld\n", __func__, chunk_heap->base,
+	pr_debug("%s: base %lu size %zu align %ld\n", __func__, chunk_heap->base,
 		heap_data->size, heap_data->align);
 
 	return &chunk_heap->heap;
diff --git a/drivers/staging/android/ion/ion_priv.h b/drivers/staging/android/ion/ion_priv.h
index 18a5f93e13b7..52f1cd1a67ed 100644
--- a/drivers/staging/android/ion/ion_priv.h
+++ b/drivers/staging/android/ion/ion_priv.h
@@ -33,7 +33,7 @@ struct ion_buffer *ion_handle_buffer(struct ion_handle *handle);
 
 /**
  * struct ion_buffer - metadata for a particular buffer
- * @ref:		refernce count
+ * @ref:		reference count
  * @node:		node in the ion_device buffers tree
  * @dev:		back pointer to the ion_device
  * @heap:		back pointer to the heap the buffer came from
@@ -46,7 +46,7 @@ struct ion_buffer *ion_handle_buffer(struct ion_handle *handle);
  *			an ion_phys_addr_t (and someday a phys_addr_t)
  * @lock:		protects the buffers cnt fields
  * @kmap_cnt:		number of times the buffer is mapped to the kernel
- * @vaddr:		the kenrel mapping if kmap_cnt is not zero
+ * @vaddr:		the kernel mapping if kmap_cnt is not zero
  * @dmap_cnt:		number of times the buffer is mapped for dma
  * @sg_table:		the sg table for the buffer if dmap_cnt is not zero
  * @pages:		flat array of pages in the buffer -- used by fault
@@ -266,7 +266,7 @@ void ion_heap_freelist_add(struct ion_heap *heap, struct ion_buffer *buffer);
 /**
  * ion_heap_freelist_drain - drain the deferred free list
  * @heap:		the heap
- * @size:		ammount of memory to drain in bytes
+ * @size:		amount of memory to drain in bytes
  *
  * Drains the indicated amount of memory from the deferred freelist immediately.
  * Returns the total amount freed.  The total freed may be higher depending
diff --git a/drivers/staging/android/ion/ion_test.c b/drivers/staging/android/ion/ion_test.c
index 3bc461cbbfa3..7d6e6b6bc894 100644
--- a/drivers/staging/android/ion/ion_test.c
+++ b/drivers/staging/android/ion/ion_test.c
@@ -261,7 +261,20 @@ static int __init ion_test_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static int ion_test_remove(struct platform_device *pdev)
+{
+	struct ion_test_device *testdev;
+
+	testdev = platform_get_drvdata(pdev);
+	if (!testdev)
+		return -ENODATA;
+
+	return misc_deregister(&testdev->misc);
+}
+
+static struct platform_device *ion_test_pdev;
 static struct platform_driver ion_test_platform_driver = {
+	.remove = ion_test_remove,
 	.driver = {
 		.name = "ion-test",
 	},
@@ -269,13 +282,18 @@ static struct platform_driver ion_test_platform_driver = {
 
 static int __init ion_test_init(void)
 {
-	platform_device_register_simple("ion-test", -1, NULL, 0);
+	ion_test_pdev = platform_device_register_simple("ion-test",
+							-1, NULL, 0);
+	if (!ion_test_pdev)
+		return -ENODEV;
+
 	return platform_driver_probe(&ion_test_platform_driver, ion_test_probe);
 }
 
 static void __exit ion_test_exit(void)
 {
 	platform_driver_unregister(&ion_test_platform_driver);
+	platform_device_unregister(ion_test_pdev);
 }
 
 module_init(ion_test_init);
diff --git a/drivers/staging/android/ion/tegra/tegra_ion.c b/drivers/staging/android/ion/tegra/tegra_ion.c
index 5b8ef0e66010..4d3c516cc15e 100644
--- a/drivers/staging/android/ion/tegra/tegra_ion.c
+++ b/drivers/staging/android/ion/tegra/tegra_ion.c
@@ -15,6 +15,7 @@
  */
 
 #include <linux/err.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include "../ion.h"
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
index feafa172b155..872bd603fd0d 100644
--- a/drivers/staging/android/lowmemorykiller.c
+++ b/drivers/staging/android/lowmemorykiller.c
@@ -156,20 +156,27 @@ static unsigned long lowmem_scan(struct shrinker *s, struct shrink_control *sc)
 			     p->pid, p->comm, oom_score_adj, tasksize);
 	}
 	if (selected) {
-		lowmem_print(1, "send sigkill to %d (%s), adj %hd, size %d\n",
-			     selected->pid, selected->comm,
-			     selected_oom_score_adj, selected_tasksize);
-		lowmem_deathpending_timeout = jiffies + HZ;
+		task_lock(selected);
+		if (!selected->mm) {
+			/* Already exited, cannot do mark_tsk_oom_victim() */
+			task_unlock(selected);
+			goto out;
+		}
 		/*
 		 * FIXME: lowmemorykiller shouldn't abuse global OOM killer
 		 * infrastructure. There is no real reason why the selected
 		 * task should have access to the memory reserves.
 		 */
-		mark_tsk_oom_victim(selected);
+		mark_oom_victim(selected);
+		task_unlock(selected);
+		lowmem_print(1, "send sigkill to %d (%s), adj %hd, size %d\n",
+			     selected->pid, selected->comm,
+			     selected_oom_score_adj, selected_tasksize);
+		lowmem_deathpending_timeout = jiffies + HZ;
 		send_sig(SIGKILL, selected, 0);
 		rem += selected_tasksize;
 	}
-
+out:
 	lowmem_print(4, "lowmem_scan %lu, %x, return %lu\n",
 		     sc->nr_to_scan, sc->gfp_mask, rem);
 	rcu_read_unlock();
diff --git a/drivers/staging/android/uapi/ion.h b/drivers/staging/android/uapi/ion.h
index 6aa495673370..68a14b4e21cb 100644
--- a/drivers/staging/android/uapi/ion.h
+++ b/drivers/staging/android/uapi/ion.h
@@ -179,7 +179,7 @@ struct ion_custom_data {
  * DOC: ION_IOC_SYNC - syncs a shared file descriptors to memory
  *
  * Deprecated in favor of using the dma_buf api's correctly (syncing
- * will happend automatically when the buffer is mapped to a device).
+ * will happen automatically when the buffer is mapped to a device).
  * If necessary should be used after touching a cached buffer from the cpu,
  * this will make the buffer in memory coherent.
  */
diff --git a/drivers/staging/board/Kconfig b/drivers/staging/board/Kconfig
index 0a89ad16371f..b8ee81840666 100644
--- a/drivers/staging/board/Kconfig
+++ b/drivers/staging/board/Kconfig
@@ -1,7 +1,6 @@
 config STAGING_BOARD
 	bool "Staging Board Support"
 	depends on OF_ADDRESS
-	depends on BROKEN
 	help
 	  Select to enable per-board staging support code.
 
diff --git a/drivers/staging/board/Makefile b/drivers/staging/board/Makefile
index 65d39ecfad63..6842745feb94 100644
--- a/drivers/staging/board/Makefile
+++ b/drivers/staging/board/Makefile
@@ -1,2 +1,3 @@
 obj-y	:= board.o
-obj-$(CONFIG_ARCH_EMEV2)      += kzm9d.o
+obj-$(CONFIG_ARCH_EMEV2)	+= kzm9d.o
+obj-$(CONFIG_ARCH_R8A7740)	+= armadillo800eva.o
diff --git a/drivers/staging/board/armadillo800eva.c b/drivers/staging/board/armadillo800eva.c
new file mode 100644
index 000000000000..81df77bd55cc
--- /dev/null
+++ b/drivers/staging/board/armadillo800eva.c
@@ -0,0 +1,105 @@
+/*
+ * Staging board support for Armadillo 800 eva.
+ * Enable not-yet-DT-capable devices here.
+ *
+ * Based on board-armadillo800eva.c
+ *
+ * Copyright (C) 2012 Renesas Solutions Corp.
+ * Copyright (C) 2012 Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/fb.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+
+#include <video/sh_mobile_lcdc.h>
+
+#include "board.h"
+
+
+static struct fb_videomode lcdc0_mode = {
+	.name		= "AMPIER/AM-800480",
+	.xres		= 800,
+	.yres		= 480,
+	.left_margin	= 88,
+	.right_margin	= 40,
+	.hsync_len	= 128,
+	.upper_margin	= 20,
+	.lower_margin	= 5,
+	.vsync_len	= 5,
+	.sync		= 0,
+};
+
+static struct sh_mobile_lcdc_info lcdc0_info = {
+	.clock_source	= LCDC_CLK_BUS,
+	.ch[0] = {
+		.chan		= LCDC_CHAN_MAINLCD,
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.interface_type	= RGB24,
+		.clock_divider	= 5,
+		.flags		= 0,
+		.lcd_modes	= &lcdc0_mode,
+		.num_modes	= 1,
+		.panel_cfg = {
+			.width	= 111,
+			.height = 68,
+		},
+	},
+};
+
+static struct resource lcdc0_resources[] = {
+	[0] = {
+		.name	= "LCD0",
+		.start	= 0xfe940000,
+		.end	= 0xfe943fff,
+		.flags	= IORESOURCE_MEM,
+	},
+	[1] = {
+		.start	= 177 + 32,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+static struct platform_device lcdc0_device = {
+	.name		= "sh_mobile_lcdc_fb",
+	.num_resources	= ARRAY_SIZE(lcdc0_resources),
+	.resource	= lcdc0_resources,
+	.id		= 0,
+	.dev	= {
+		.platform_data	= &lcdc0_info,
+		.coherent_dma_mask = DMA_BIT_MASK(32),
+	},
+};
+
+static const struct board_staging_clk lcdc0_clocks[] __initconst = {
+	{ "lcdc0", NULL, "sh_mobile_lcdc_fb.0" },
+};
+
+static const struct board_staging_dev armadillo800eva_devices[] __initconst = {
+	{
+		.pdev		= &lcdc0_device,
+		.clocks		= lcdc0_clocks,
+		.nclocks	= ARRAY_SIZE(lcdc0_clocks),
+		.domain		= "a4lc",
+	},
+};
+
+static void __init armadillo800eva_init(void)
+{
+	board_staging_gic_setup_xlate("arm,cortex-a9-gic", 32);
+	board_staging_register_devices(armadillo800eva_devices,
+				       ARRAY_SIZE(armadillo800eva_devices));
+}
+
+board_staging("renesas,armadillo800eva", armadillo800eva_init);
diff --git a/drivers/staging/board/board.c b/drivers/staging/board/board.c
index d5a6abc84519..29d456e29f38 100644
--- a/drivers/staging/board/board.c
+++ b/drivers/staging/board/board.c
@@ -1,10 +1,30 @@
+/*
+ * Copyright (C) 2014 Magnus Damm
+ * Copyright (C) 2015 Glider bvba
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#define pr_fmt(fmt)	"board_staging: "  fmt
+
+#include <linux/clkdev.h>
 #include <linux/init.h>
+#include <linux/irq.h>
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/pm_domain.h>
+
 #include "board.h"
 
+static struct device_node *irqc_node __initdata;
+static unsigned int irqc_base __initdata;
+
 static bool find_by_address(u64 base_address)
 {
 	struct device_node *dn = of_find_all_nodes(NULL);
@@ -38,3 +58,119 @@ bool __init board_staging_dt_node_available(const struct resource *resource,
 
 	return false; /* Nothing found */
 }
+
+int __init board_staging_gic_setup_xlate(const char *gic_match,
+					 unsigned int base)
+{
+	WARN_ON(irqc_node);
+
+	irqc_node = of_find_compatible_node(NULL, NULL, gic_match);
+
+	WARN_ON(!irqc_node);
+	if (!irqc_node)
+		return -ENOENT;
+
+	irqc_base = base;
+	return 0;
+}
+
+static void __init gic_fixup_resource(struct resource *res)
+{
+	struct of_phandle_args irq_data;
+	unsigned int hwirq = res->start;
+	unsigned int virq;
+
+	if (resource_type(res) != IORESOURCE_IRQ || !irqc_node)
+		return;
+
+	irq_data.np = irqc_node;
+	irq_data.args_count = 3;
+	irq_data.args[0] = 0;
+	irq_data.args[1] = hwirq - irqc_base;
+	switch (res->flags &
+		(IORESOURCE_IRQ_LOWEDGE | IORESOURCE_IRQ_HIGHEDGE |
+		 IORESOURCE_IRQ_LOWLEVEL | IORESOURCE_IRQ_HIGHLEVEL)) {
+	case IORESOURCE_IRQ_LOWEDGE:
+		irq_data.args[2] = IRQ_TYPE_EDGE_FALLING;
+		break;
+	case IORESOURCE_IRQ_HIGHEDGE:
+		irq_data.args[2] = IRQ_TYPE_EDGE_RISING;
+		break;
+	case IORESOURCE_IRQ_LOWLEVEL:
+		irq_data.args[2] = IRQ_TYPE_LEVEL_LOW;
+		break;
+	case IORESOURCE_IRQ_HIGHLEVEL:
+	default:
+		irq_data.args[2] = IRQ_TYPE_LEVEL_HIGH;
+		break;
+	}
+
+	virq = irq_create_of_mapping(&irq_data);
+	if (WARN_ON(!virq))
+		return;
+
+	pr_debug("hwirq %u -> virq %u\n", hwirq, virq);
+	res->start = virq;
+}
+
+void __init board_staging_gic_fixup_resources(struct resource *res,
+					      unsigned int nres)
+{
+	unsigned int i;
+
+	for (i = 0; i < nres; i++)
+		gic_fixup_resource(&res[i]);
+}
+
+int __init board_staging_register_clock(const struct board_staging_clk *bsc)
+{
+	int error;
+
+	pr_debug("Aliasing clock %s for con_id %s dev_id %s\n", bsc->clk,
+		 bsc->con_id, bsc->dev_id);
+	error = clk_add_alias(bsc->con_id, bsc->dev_id, bsc->clk, NULL);
+	if (error)
+		pr_err("Failed to alias clock %s (%d)\n", bsc->clk, error);
+
+	return error;
+}
+
+int __init board_staging_register_device(const struct board_staging_dev *dev)
+{
+	struct platform_device *pdev = dev->pdev;
+	unsigned int i;
+	int error;
+
+	pr_debug("Trying to register device %s\n", pdev->name);
+	if (board_staging_dt_node_available(pdev->resource,
+					    pdev->num_resources)) {
+		pr_warn("Skipping %s, already in DT\n", pdev->name);
+		return -EEXIST;
+	}
+
+	board_staging_gic_fixup_resources(pdev->resource, pdev->num_resources);
+
+	for (i = 0; i < dev->nclocks; i++)
+		board_staging_register_clock(&dev->clocks[i]);
+
+	error = platform_device_register(pdev);
+	if (error) {
+		pr_err("Failed to register device %s (%d)\n", pdev->name,
+		       error);
+		return error;
+	}
+
+	if (dev->domain)
+		__pm_genpd_name_add_device(dev->domain, &pdev->dev, NULL);
+
+	return error;
+}
+
+void __init board_staging_register_devices(const struct board_staging_dev *devs,
+					   unsigned int ndevs)
+{
+	unsigned int i;
+
+	for (i = 0; i < ndevs; i++)
+		board_staging_register_device(&devs[i]);
+}
diff --git a/drivers/staging/board/board.h b/drivers/staging/board/board.h
index 2390ed6c31a4..42ed12513220 100644
--- a/drivers/staging/board/board.h
+++ b/drivers/staging/board/board.h
@@ -1,10 +1,35 @@
 #ifndef __BOARD_H__
 #define __BOARD_H__
+
 #include <linux/init.h>
 #include <linux/of.h>
 
+struct board_staging_clk {
+	const char *clk;
+	const char *con_id;
+	const char *dev_id;
+};
+
+struct board_staging_dev {
+	/* Platform Device */
+	struct platform_device *pdev;
+	/* Clocks (optional) */
+	const struct board_staging_clk *clocks;
+	unsigned int nclocks;
+	/* Generic PM Domain (optional) */
+	const char *domain;
+};
+
+struct resource;
+
 bool board_staging_dt_node_available(const struct resource *resource,
 				     unsigned int num_resources);
+int board_staging_gic_setup_xlate(const char *gic_match, unsigned int base);
+void board_staging_gic_fixup_resources(struct resource *res, unsigned int nres);
+int board_staging_register_clock(const struct board_staging_clk *bsc);
+int board_staging_register_device(const struct board_staging_dev *dev);
+void board_staging_register_devices(const struct board_staging_dev *devs,
+				    unsigned int ndevs);
 
 #define board_staging(str, fn)			\
 static int __init runtime_board_check(void)	\
@@ -15,6 +40,6 @@ static int __init runtime_board_check(void)	\
 	return 0;				\
 }						\
 						\
-late_initcall(runtime_board_check)
+device_initcall(runtime_board_check)
 
 #endif /* __BOARD_H__ */
diff --git a/drivers/staging/board/kzm9d.c b/drivers/staging/board/kzm9d.c
index 533f3026e17a..8d1eb09bc66e 100644
--- a/drivers/staging/board/kzm9d.c
+++ b/drivers/staging/board/kzm9d.c
@@ -4,16 +4,22 @@
 #include <linux/platform_device.h>
 #include "board.h"
 
-static const struct resource usbs1_res[] __initconst = {
+static struct resource usbs1_res[] __initdata = {
 	DEFINE_RES_MEM(0xe2800000, 0x2000),
 	DEFINE_RES_IRQ(159),
 };
 
 static void __init kzm9d_init(void)
 {
-	if (!board_staging_dt_node_available(usbs1_res, ARRAY_SIZE(usbs1_res)))
+	board_staging_gic_setup_xlate("arm,cortex-a9-gic", 32);
+
+	if (!board_staging_dt_node_available(usbs1_res,
+					     ARRAY_SIZE(usbs1_res))) {
+		board_staging_gic_fixup_resources(usbs1_res,
+						  ARRAY_SIZE(usbs1_res));
 		platform_device_register_simple("emxx_udc", -1, usbs1_res,
 						ARRAY_SIZE(usbs1_res));
+	}
 }
 
 board_staging("renesas,kzm9d", kzm9d_init);
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 61c6351f55ac..7dee73dfbf88 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -1247,16 +1247,22 @@ config COMEDI_8254
 	tristate
 
 config COMEDI_8255
-	tristate "Generic 8255 support"
+	tristate
+
+config COMEDI_8255_SA
+	tristate "Standalone 8255 support"
+	select COMEDI_8255
 	---help---
-	  Enable generic 8255 support.
+	  Enable support for 8255 digital I/O as a standalone driver.
 
 	  You should enable compilation this driver if you plan to use a board
-	  that has an 8255 chip. For multifunction boards, the main driver will
-	  configure the 8255 subdevice automatically.
+	  that has an 8255 chip at a known I/O base address and there are no
+	  other Comedi drivers for the board.
 
-	  Note that most PCI based 8255 boards use the 8255_pci driver as a
-	  wrapper around this driver.
+	  Note that Comedi drivers for most multi-function boards incorporating
+	  an 8255 chip use the 'comedi_8255' module.  Most PCI-based 8255
+	  boards use the 8255_pci driver as a wrapper around the 'comedi_8255'
+	  module.
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called 8255.
diff --git a/drivers/staging/comedi/comedi.h b/drivers/staging/comedi/comedi.h
index 745574077352..66edda190b75 100644
--- a/drivers/staging/comedi/comedi.h
+++ b/drivers/staging/comedi/comedi.h
@@ -217,7 +217,7 @@
 #define SDF_RUNNING	0x08000000	/* subdevice is acquiring data */
 #define SDF_LSAMPL	0x10000000	/* subdevice uses 32-bit samples */
 #define SDF_PACKED	0x20000000	/* subdevice can do packed DIO */
-/* re recyle these flags for PWM */
+/* re recycle these flags for PWM */
 #define SDF_PWM_COUNTER SDF_MODE0	/* PWM can automatically switch off */
 #define SDF_PWM_HBRIDGE SDF_MODE1	/* PWM is signed (H-bridge) */
 
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index e78ddbe5a954..985d94b6cbfd 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -44,6 +44,23 @@
 #include "comedi_internal.h"
 
 /**
+ * comedi_subdevice "runflags"
+ * @COMEDI_SRF_RT:		DEPRECATED: command is running real-time
+ * @COMEDI_SRF_ERROR:		indicates an COMEDI_CB_ERROR event has occurred
+ *				since the last command was started
+ * @COMEDI_SRF_RUNNING:		command is running
+ * @COMEDI_SRF_FREE_SPRIV:	free s->private on detach
+ *
+ * @COMEDI_SRF_BUSY_MASK:	runflags that indicate the subdevice is "busy"
+ */
+#define COMEDI_SRF_RT		BIT(1)
+#define COMEDI_SRF_ERROR	BIT(2)
+#define COMEDI_SRF_RUNNING	BIT(27)
+#define COMEDI_SRF_FREE_SPRIV	BIT(31)
+
+#define COMEDI_SRF_BUSY_MASK	(COMEDI_SRF_ERROR | COMEDI_SRF_RUNNING)
+
+/**
  * struct comedi_file - per-file private data for comedi device
  * @dev: comedi_device struct
  * @read_subdev: current "read" subdevice
@@ -679,8 +696,28 @@ static bool comedi_is_subdevice_idle(struct comedi_subdevice *s)
 	return !(runflags & COMEDI_SRF_BUSY_MASK);
 }
 
+bool comedi_can_auto_free_spriv(struct comedi_subdevice *s)
+{
+	unsigned runflags = __comedi_get_subdevice_runflags(s);
+
+	return runflags & COMEDI_SRF_FREE_SPRIV;
+}
+
 /**
- * comedi_alloc_spriv() - Allocate memory for the subdevice private data.
+ * comedi_set_spriv_auto_free - mark subdevice private data as freeable
+ * @s: comedi_subdevice struct
+ *
+ * Mark the subdevice as having a pointer to private data that can be
+ * automatically freed by the comedi core during the detach.
+ */
+void comedi_set_spriv_auto_free(struct comedi_subdevice *s)
+{
+	__comedi_set_subdevice_runflags(s, COMEDI_SRF_FREE_SPRIV);
+}
+EXPORT_SYMBOL_GPL(comedi_set_spriv_auto_free);
+
+/**
+ * comedi_alloc_spriv - Allocate memory for the subdevice private data.
  * @s: comedi_subdevice struct
  * @size: size of the memory to allocate
  *
@@ -691,7 +728,7 @@ void *comedi_alloc_spriv(struct comedi_subdevice *s, size_t size)
 {
 	s->private = kzalloc(size, GFP_KERNEL);
 	if (s->private)
-		s->runflags |= COMEDI_SRF_FREE_SPRIV;
+		comedi_set_spriv_auto_free(s);
 	return s->private;
 }
 EXPORT_SYMBOL_GPL(comedi_alloc_spriv);
@@ -1048,11 +1085,6 @@ static int do_chaninfo_ioctl(struct comedi_device *dev,
 			if (put_user(x, it.rangelist + i))
 				return -EFAULT;
 		}
-#if 0
-		if (copy_to_user(it.rangelist, s->range_type_list,
-				 s->n_chan * sizeof(unsigned int)))
-			return -EFAULT;
-#endif
 	}
 
 	return 0;
@@ -1725,7 +1757,7 @@ cleanup:
 
 /*
  * COMEDI_CMDTEST ioctl
- * asynchronous aquisition command testing
+ * asynchronous acquisition command testing
  *
  * arg:
  *	pointer to comedi_cmd structure
diff --git a/drivers/staging/comedi/comedi_internal.h b/drivers/staging/comedi/comedi_internal.h
index 3b918538847e..cd9437f72c35 100644
--- a/drivers/staging/comedi/comedi_internal.h
+++ b/drivers/staging/comedi/comedi_internal.h
@@ -33,6 +33,7 @@ struct comedi_buf_map *comedi_buf_map_from_subdev_get(
 		struct comedi_subdevice *s);
 unsigned int comedi_buf_write_n_allocated(struct comedi_subdevice *s);
 void comedi_device_cancel_all(struct comedi_device *dev);
+bool comedi_can_auto_free_spriv(struct comedi_subdevice *s);
 
 extern unsigned int comedi_default_buf_size_kb;
 extern unsigned int comedi_default_buf_maxsize_kb;
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index dfab5a84b011..28a5d3a037a1 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -227,12 +227,12 @@ struct comedi_async {
  * @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_EOS		BIT(0)
+#define COMEDI_CB_EOA		BIT(1)
+#define COMEDI_CB_BLOCK		BIT(2)
+#define COMEDI_CB_EOBUF		BIT(3)
+#define COMEDI_CB_ERROR		BIT(4)
+#define COMEDI_CB_OVERFLOW	BIT(5)
 
 #define COMEDI_CB_ERROR_MASK	(COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)
 #define COMEDI_CB_CANCEL_MASK	(COMEDI_CB_EOA | COMEDI_CB_ERROR_MASK)
@@ -303,26 +303,10 @@ void comedi_event(struct comedi_device *dev, struct comedi_subdevice *s);
 struct comedi_device *comedi_dev_get_from_minor(unsigned minor);
 int comedi_dev_put(struct comedi_device *dev);
 
-/**
- * comedi_subdevice "runflags"
- * @COMEDI_SRF_RT:		DEPRECATED: command is running real-time
- * @COMEDI_SRF_ERROR:		indicates an COMEDI_CB_ERROR event has occurred
- *				since the last command was started
- * @COMEDI_SRF_RUNNING:		command is running
- * @COMEDI_SRF_FREE_SPRIV:	free s->private on detach
- *
- * @COMEDI_SRF_BUSY_MASK:	runflags that indicate the subdevice is "busy"
- */
-#define COMEDI_SRF_RT		BIT(1)
-#define COMEDI_SRF_ERROR	BIT(2)
-#define COMEDI_SRF_RUNNING	BIT(27)
-#define COMEDI_SRF_FREE_SPRIV	BIT(31)
-
-#define COMEDI_SRF_BUSY_MASK	(COMEDI_SRF_ERROR | COMEDI_SRF_RUNNING)
-
 bool comedi_is_subdevice_running(struct comedi_subdevice *s);
 
 void *comedi_alloc_spriv(struct comedi_subdevice *s, size_t size);
+void comedi_set_spriv_auto_free(struct comedi_subdevice *s);
 
 int comedi_check_chanlist(struct comedi_subdevice *s,
 			  int n,
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index 57dcffe00204..ed0b60c925de 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -125,7 +125,7 @@ static void comedi_device_detach_cleanup(struct comedi_device *dev)
 	if (dev->subdevices) {
 		for (i = 0; i < dev->n_subdevices; i++) {
 			s = &dev->subdevices[i];
-			if (s->runflags & COMEDI_SRF_FREE_SPRIV)
+			if (comedi_can_auto_free_spriv(s))
 				kfree(s->private);
 			comedi_free_subdevice_minor(s);
 			if (s->async) {
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index ba89321df65d..b79d3764a8a0 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -53,221 +53,6 @@
 
 #include "8255.h"
 
-struct subdev_8255_private {
-	unsigned long regbase;
-	int (*io)(struct comedi_device *, int, int, int, unsigned long);
-};
-
-static int subdev_8255_io(struct comedi_device *dev,
-			  int dir, int port, int data, unsigned long regbase)
-{
-	if (dir) {
-		outb(data, dev->iobase + regbase + port);
-		return 0;
-	}
-	return inb(dev->iobase + regbase + port);
-}
-
-static int subdev_8255_mmio(struct comedi_device *dev,
-			    int dir, int port, int data, unsigned long regbase)
-{
-	if (dir) {
-		writeb(data, dev->mmio + regbase + port);
-		return 0;
-	}
-	return readb(dev->mmio + regbase + port);
-}
-
-static int subdev_8255_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn,
-			    unsigned int *data)
-{
-	struct subdev_8255_private *spriv = s->private;
-	unsigned long regbase = spriv->regbase;
-	unsigned int mask;
-	unsigned int v;
-
-	mask = comedi_dio_update_state(s, data);
-	if (mask) {
-		if (mask & 0xff)
-			spriv->io(dev, 1, I8255_DATA_A_REG,
-				  s->state & 0xff, regbase);
-		if (mask & 0xff00)
-			spriv->io(dev, 1, I8255_DATA_B_REG,
-				  (s->state >> 8) & 0xff, regbase);
-		if (mask & 0xff0000)
-			spriv->io(dev, 1, I8255_DATA_C_REG,
-				  (s->state >> 16) & 0xff, regbase);
-	}
-
-	v = spriv->io(dev, 0, I8255_DATA_A_REG, 0, regbase);
-	v |= (spriv->io(dev, 0, I8255_DATA_B_REG, 0, regbase) << 8);
-	v |= (spriv->io(dev, 0, I8255_DATA_C_REG, 0, regbase) << 16);
-
-	data[1] = v;
-
-	return insn->n;
-}
-
-static void subdev_8255_do_config(struct comedi_device *dev,
-				  struct comedi_subdevice *s)
-{
-	struct subdev_8255_private *spriv = s->private;
-	unsigned long regbase = spriv->regbase;
-	int config;
-
-	config = I8255_CTRL_CW;
-	/* 1 in io_bits indicates output, 1 in config indicates input */
-	if (!(s->io_bits & 0x0000ff))
-		config |= I8255_CTRL_A_IO;
-	if (!(s->io_bits & 0x00ff00))
-		config |= I8255_CTRL_B_IO;
-	if (!(s->io_bits & 0x0f0000))
-		config |= I8255_CTRL_C_LO_IO;
-	if (!(s->io_bits & 0xf00000))
-		config |= I8255_CTRL_C_HI_IO;
-
-	spriv->io(dev, 1, I8255_CTRL_REG, config, regbase);
-}
-
-static int subdev_8255_insn_config(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data)
-{
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int mask;
-	int ret;
-
-	if (chan < 8)
-		mask = 0x0000ff;
-	else if (chan < 16)
-		mask = 0x00ff00;
-	else if (chan < 20)
-		mask = 0x0f0000;
-	else
-		mask = 0xf00000;
-
-	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
-	if (ret)
-		return ret;
-
-	subdev_8255_do_config(dev, s);
-
-	return insn->n;
-}
-
-static int __subdev_8255_init(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      int (*io)(struct comedi_device *,
-					int, int, int, unsigned long),
-			      unsigned long regbase,
-			      bool is_mmio)
-{
-	struct subdev_8255_private *spriv;
-
-	spriv = comedi_alloc_spriv(s, sizeof(*spriv));
-	if (!spriv)
-		return -ENOMEM;
-
-	if (io)
-		spriv->io = io;
-	else if (is_mmio)
-		spriv->io = subdev_8255_mmio;
-	else
-		spriv->io = subdev_8255_io;
-	spriv->regbase	= regbase;
-
-	s->type		= COMEDI_SUBD_DIO;
-	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
-	s->n_chan	= 24;
-	s->range_table	= &range_digital;
-	s->maxdata	= 1;
-	s->insn_bits	= subdev_8255_insn;
-	s->insn_config	= subdev_8255_insn_config;
-
-	subdev_8255_do_config(dev, s);
-
-	return 0;
-}
-
-/**
- * subdev_8255_init - initialize DIO subdevice for driving I/O mapped 8255
- * @dev: comedi device owning subdevice
- * @s: comedi subdevice to initialize
- * @io: (optional) register I/O call-back function
- * @regbase: offset of 8255 registers from dev->iobase, or call-back context
- *
- * Initializes a comedi subdevice as a DIO subdevice driving an 8255 chip.
- *
- * If the optional I/O call-back function is provided, its prototype is of
- * the following form:
- *
- *   int my_8255_callback(struct comedi_device *dev,
- *                        struct comedi_subdevice *s, int dir, int port,
- *                        int data, unsigned long regbase);
- *
- * where 'dev', 's', and 'regbase' match the values passed to this function,
- * 'port' is the 8255 port number 0 to 3 (including the control port), 'dir'
- * is the direction (0 for read, 1 for write) and 'data' is the value to be
- * written.  It should return 0 if writing or the value read if reading.
- *
- * If the optional I/O call-back function is not provided, an internal
- * call-back function is used which uses consecutive I/O port addresses
- * starting at dev->iobase + regbase.
- *
- * Return: -ENOMEM if failed to allocate memory, zero on success.
- */
-int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
-		     int (*io)(struct comedi_device *,
-			       int, int, int, unsigned long),
-		     unsigned long regbase)
-{
-	return __subdev_8255_init(dev, s, io, regbase, false);
-}
-EXPORT_SYMBOL_GPL(subdev_8255_init);
-
-/**
- * subdev_8255_mm_init - initialize DIO subdevice for driving mmio-mapped 8255
- * @dev: comedi device owning subdevice
- * @s: comedi subdevice to initialize
- * @io: (optional) register I/O call-back function
- * @regbase: offset of 8255 registers from dev->mmio, or call-back context
- *
- * Initializes a comedi subdevice as a DIO subdevice driving an 8255 chip.
- *
- * If the optional I/O call-back function is provided, its prototype is of
- * the following form:
- *
- *   int my_8255_callback(struct comedi_device *dev,
- *                        struct comedi_subdevice *s, int dir, int port,
- *                        int data, unsigned long regbase);
- *
- * where 'dev', 's', and 'regbase' match the values passed to this function,
- * 'port' is the 8255 port number 0 to 3 (including the control port), 'dir'
- * is the direction (0 for read, 1 for write) and 'data' is the value to be
- * written.  It should return 0 if writing or the value read if reading.
- *
- * If the optional I/O call-back function is not provided, an internal
- * call-back function is used which uses consecutive MMIO virtual addresses
- * starting at dev->mmio + regbase.
- *
- * Return: -ENOMEM if failed to allocate memory, zero on success.
- */
-int subdev_8255_mm_init(struct comedi_device *dev, struct comedi_subdevice *s,
-			int (*io)(struct comedi_device *,
-				  int, int, int, unsigned long),
-			unsigned long regbase)
-{
-	return __subdev_8255_init(dev, s, io, regbase, true);
-}
-EXPORT_SYMBOL_GPL(subdev_8255_mm_init);
-
-/*
- * Start of the 8255 standalone device
- */
-
 static int dev_8255_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
@@ -306,8 +91,15 @@ static int dev_8255_attach(struct comedi_device *dev,
 			s->type = COMEDI_SUBD_UNUSED;
 		} else {
 			ret = subdev_8255_init(dev, s, NULL, iobase);
-			if (ret)
+			if (ret) {
+				/*
+				 * Release the I/O port region here, as the
+				 * "detach" handler cannot find it.
+				 */
+				release_region(iobase, I8255_SIZE);
+				s->type = COMEDI_SUBD_UNUSED;
 				return ret;
+			}
 		}
 	}
 
@@ -317,14 +109,14 @@ static int dev_8255_attach(struct comedi_device *dev,
 static void dev_8255_detach(struct comedi_device *dev)
 {
 	struct comedi_subdevice *s;
-	struct subdev_8255_private *spriv;
 	int i;
 
 	for (i = 0; i < dev->n_subdevices; i++) {
 		s = &dev->subdevices[i];
 		if (s->type != COMEDI_SUBD_UNUSED) {
-			spriv = s->private;
-			release_region(spriv->regbase, I8255_SIZE);
+			unsigned long regbase = subdev_8255_regbase(s);
+
+			release_region(regbase, I8255_SIZE);
 		}
 	}
 }
@@ -338,5 +130,5 @@ static struct comedi_driver dev_8255_driver = {
 module_comedi_driver(dev_8255_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for standalone 8255 devices");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/8255.h b/drivers/staging/comedi/drivers/8255.h
index 934b940ebd3c..41823de69b77 100644
--- a/drivers/staging/comedi/drivers/8255.h
+++ b/drivers/staging/comedi/drivers/8255.h
@@ -19,8 +19,6 @@
 #ifndef _8255_H
 #define _8255_H
 
-#include "../comedidev.h"
-
 #define I8255_SIZE		0x04
 
 #define I8255_DATA_A_REG	0x00
@@ -35,14 +33,19 @@
 #define I8255_CTRL_A_MODE(x)	((x) << 5)
 #define I8255_CTRL_CW		(1 << 7)
 
-int subdev_8255_init(struct comedi_device *, struct comedi_subdevice *,
-		     int (*io)(struct comedi_device *,
-			       int, int, int, unsigned long),
+struct comedi_device;
+struct comedi_subdevice;
+
+int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
+		     int (*io)(struct comedi_device *dev, int dir, int port,
+			       int data, unsigned long regbase),
 		     unsigned long regbase);
 
-int subdev_8255_mm_init(struct comedi_device *, struct comedi_subdevice *,
-			int (*io)(struct comedi_device *,
-				  int, int, int, unsigned long),
+int subdev_8255_mm_init(struct comedi_device *dev, struct comedi_subdevice *s,
+			int (*io)(struct comedi_device *dev, int dir, int port,
+				  int data, unsigned long regbase),
 			unsigned long regbase);
 
+unsigned long subdev_8255_regbase(struct comedi_subdevice *s);
+
 #endif
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index d6d834006015..5764dc9a6893 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -139,7 +139,8 @@ obj-$(CONFIG_COMEDI_NI_TIOCMD)		+= ni_tiocmd.o
 obj-$(CONFIG_COMEDI_NI_LABPC)		+= ni_labpc_common.o
 obj-$(CONFIG_COMEDI_NI_LABPC_ISADMA)	+= ni_labpc_isadma.o
 
-obj-$(CONFIG_COMEDI_8255)		+= 8255.o
+obj-$(CONFIG_COMEDI_8255)		+= comedi_8255.o
+obj-$(CONFIG_COMEDI_8255_SA)		+= 8255.o
 obj-$(CONFIG_COMEDI_AMPLC_DIO200)	+= amplc_dio200_common.o
 obj-$(CONFIG_COMEDI_AMPLC_PC236)	+= amplc_pc236_common.o
 obj-$(CONFIG_COMEDI_DAS08)		+= das08.o
diff --git a/drivers/staging/comedi/drivers/addi_apci_1516.c b/drivers/staging/comedi/drivers/addi_apci_1516.c
index 9c516d1fe896..f1f8b1c422a7 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1516.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1516.c
@@ -100,10 +100,10 @@ static int apci1516_do_insn_bits(struct comedi_device *dev,
 
 static int apci1516_reset(struct comedi_device *dev)
 {
-	const struct apci1516_boardinfo *this_board = dev->board_ptr;
+	const struct apci1516_boardinfo *board = dev->board_ptr;
 	struct apci1516_private *devpriv = dev->private;
 
-	if (!this_board->has_wdog)
+	if (!board->has_wdog)
 		return 0;
 
 	outw(0x0, dev->iobase + APCI1516_DO_REG);
@@ -117,17 +117,17 @@ static int apci1516_auto_attach(struct comedi_device *dev,
 				unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct apci1516_boardinfo *this_board = NULL;
+	const struct apci1516_boardinfo *board = NULL;
 	struct apci1516_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
 	if (context < ARRAY_SIZE(apci1516_boardtypes))
-		this_board = &apci1516_boardtypes[context];
-	if (!this_board)
+		board = &apci1516_boardtypes[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = this_board;
-	dev->board_name = this_board->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -146,10 +146,10 @@ static int apci1516_auto_attach(struct comedi_device *dev,
 
 	/* Initialize the digital input subdevice */
 	s = &dev->subdevices[0];
-	if (this_board->di_nchan) {
+	if (board->di_nchan) {
 		s->type		= COMEDI_SUBD_DI;
 		s->subdev_flags	= SDF_READABLE;
-		s->n_chan	= this_board->di_nchan;
+		s->n_chan	= board->di_nchan;
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
 		s->insn_bits	= apci1516_di_insn_bits;
@@ -159,10 +159,10 @@ static int apci1516_auto_attach(struct comedi_device *dev,
 
 	/* Initialize the digital output subdevice */
 	s = &dev->subdevices[1];
-	if (this_board->do_nchan) {
+	if (board->do_nchan) {
 		s->type		= COMEDI_SUBD_DO;
 		s->subdev_flags	= SDF_WRITABLE;
-		s->n_chan	= this_board->do_nchan;
+		s->n_chan	= board->do_nchan;
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
 		s->insn_bits	= apci1516_do_insn_bits;
@@ -172,7 +172,7 @@ static int apci1516_auto_attach(struct comedi_device *dev,
 
 	/* Initialize the watchdog subdevice */
 	s = &dev->subdevices[2];
-	if (this_board->has_wdog) {
+	if (board->has_wdog) {
 		ret = addi_watchdog_init(s, devpriv->wdog_iobase);
 		if (ret)
 			return ret;
diff --git a/drivers/staging/comedi/drivers/addi_apci_3120.c b/drivers/staging/comedi/drivers/addi_apci_3120.c
index 95dc64bfec73..5bfd43d5c889 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3120.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3120.c
@@ -977,18 +977,18 @@ static int apci3120_auto_attach(struct comedi_device *dev,
 				unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct apci3120_board *this_board = NULL;
+	const struct apci3120_board *board = NULL;
 	struct apci3120_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int status;
 	int ret;
 
 	if (context < ARRAY_SIZE(apci3120_boardtypes))
-		this_board = &apci3120_boardtypes[context];
-	if (!this_board)
+		board = &apci3120_boardtypes[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = this_board;
-	dev->board_name = this_board->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -1031,7 +1031,7 @@ static int apci3120_auto_attach(struct comedi_device *dev,
 	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->maxdata	= board->ai_is_16bit ? 0xffff : 0x0fff;
 	s->range_table	= &apci3120_ai_range;
 	s->insn_read	= apci3120_ai_insn_read;
 	if (dev->irq) {
@@ -1045,7 +1045,7 @@ static int apci3120_auto_attach(struct comedi_device *dev,
 
 	/* Analog Output subdevice */
 	s = &dev->subdevices[1];
-	if (this_board->has_ao) {
+	if (board->has_ao) {
 		s->type		= COMEDI_SUBD_AO;
 		s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
 		s->n_chan	= 8;
diff --git a/drivers/staging/comedi/drivers/addi_watchdog.h b/drivers/staging/comedi/drivers/addi_watchdog.h
index 83b47befa4d1..3f8e7388bbca 100644
--- a/drivers/staging/comedi/drivers/addi_watchdog.h
+++ b/drivers/staging/comedi/drivers/addi_watchdog.h
@@ -1,7 +1,7 @@
 #ifndef _ADDI_WATCHDOG_H
 #define _ADDI_WATCHDOG_H
 
-#include "../comedidev.h"
+struct comedi_subdevice;
 
 void addi_watchdog_reset(unsigned long iobase);
 int addi_watchdog_init(struct comedi_subdevice *, unsigned long iobase);
diff --git a/drivers/staging/comedi/drivers/adv_pci1724.c b/drivers/staging/comedi/drivers/adv_pci1724.c
index f7a7dab013db..9677111f9ab2 100644
--- a/drivers/staging/comedi/drivers/adv_pci1724.c
+++ b/drivers/staging/comedi/drivers/adv_pci1724.c
@@ -180,11 +180,7 @@ static int adv_pci1724_auto_attach(struct comedi_device *dev,
 	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;
-
-	return 0;
+	return comedi_alloc_subdev_readback(s);
 }
 
 static struct comedi_driver adv_pci1724_driver = {
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index 456e87013063..f1b3c5aa8d79 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -732,9 +732,9 @@ static int pci1760_reset(struct comedi_device *dev)
 */
 static int pci_dio_reset(struct comedi_device *dev)
 {
-	const struct dio_boardtype *this_board = dev->board_ptr;
+	const struct dio_boardtype *board = dev->board_ptr;
 
-	switch (this_board->cardtype) {
+	switch (board->cardtype) {
 	case TYPE_PCI1730:
 		outb(0, dev->iobase + PCI1730_DO);	/*  clear outputs */
 		outb(0, dev->iobase + PCI1730_DO + 1);
@@ -887,7 +887,7 @@ static int pci_dio_add_di(struct comedi_device *dev,
 			  struct comedi_subdevice *s,
 			  const struct diosubd_data *d)
 {
-	const struct dio_boardtype *this_board = dev->board_ptr;
+	const struct dio_boardtype *board = dev->board_ptr;
 
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | d->specflags;
@@ -897,7 +897,7 @@ static int pci_dio_add_di(struct comedi_device *dev,
 	s->maxdata = 1;
 	s->len_chanlist = d->chans;
 	s->range_table = &range_digital;
-	switch (this_board->io_access) {
+	switch (board->io_access) {
 	case IO_8b:
 		s->insn_bits = pci_dio_insn_bits_di_b;
 		break;
@@ -917,7 +917,7 @@ static int pci_dio_add_do(struct comedi_device *dev,
 			  struct comedi_subdevice *s,
 			  const struct diosubd_data *d)
 {
-	const struct dio_boardtype *this_board = dev->board_ptr;
+	const struct dio_boardtype *board = dev->board_ptr;
 
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
@@ -928,7 +928,7 @@ static int pci_dio_add_do(struct comedi_device *dev,
 	s->len_chanlist = d->chans;
 	s->range_table = &range_digital;
 	s->state = 0;
-	switch (this_board->io_access) {
+	switch (board->io_access) {
 	case IO_8b:
 		s->insn_bits = pci_dio_insn_bits_do_b;
 		break;
@@ -978,17 +978,17 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct dio_boardtype *this_board = NULL;
+	const struct dio_boardtype *board = NULL;
 	struct pci_dio_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret, subdev, i, j;
 
 	if (context < ARRAY_SIZE(boardtypes))
-		this_board = &boardtypes[context];
-	if (!this_board)
+		board = &boardtypes[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = this_board;
-	dev->board_name = this_board->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -997,50 +997,50 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	dev->iobase = pci_resource_start(pcidev, this_board->main_pci_region);
+	dev->iobase = pci_resource_start(pcidev, board->main_pci_region);
 
-	ret = comedi_alloc_subdevices(dev, this_board->nsubdevs);
+	ret = comedi_alloc_subdevices(dev, board->nsubdevs);
 	if (ret)
 		return ret;
 
 	subdev = 0;
 	for (i = 0; i < MAX_DI_SUBDEVS; i++)
-		if (this_board->sdi[i].chans) {
+		if (board->sdi[i].chans) {
 			s = &dev->subdevices[subdev];
-			pci_dio_add_di(dev, s, &this_board->sdi[i]);
+			pci_dio_add_di(dev, s, &board->sdi[i]);
 			subdev++;
 		}
 
 	for (i = 0; i < MAX_DO_SUBDEVS; i++)
-		if (this_board->sdo[i].chans) {
+		if (board->sdo[i].chans) {
 			s = &dev->subdevices[subdev];
-			pci_dio_add_do(dev, s, &this_board->sdo[i]);
+			pci_dio_add_do(dev, s, &board->sdo[i]);
 			subdev++;
 		}
 
 	for (i = 0; i < MAX_DIO_SUBDEVG; i++)
-		for (j = 0; j < this_board->sdio[i].regs; j++) {
+		for (j = 0; j < board->sdio[i].regs; j++) {
 			s = &dev->subdevices[subdev];
 			ret = subdev_8255_init(dev, s, NULL,
-					       this_board->sdio[i].addr +
+					       board->sdio[i].addr +
 					       j * I8255_SIZE);
 			if (ret)
 				return ret;
 			subdev++;
 		}
 
-	if (this_board->boardid.chans) {
+	if (board->boardid.chans) {
 		s = &dev->subdevices[subdev];
 		s->type = COMEDI_SUBD_DI;
-		pci_dio_add_di(dev, s, &this_board->boardid);
+		pci_dio_add_di(dev, s, &board->boardid);
 		subdev++;
 	}
 
-	if (this_board->timer_regbase) {
+	if (board->timer_regbase) {
 		s = &dev->subdevices[subdev];
 
 		dev->pacer = comedi_8254_init(dev->iobase +
-					      this_board->timer_regbase,
+					      board->timer_regbase,
 					      0, I8254_IO8, 0);
 		if (!dev->pacer)
 			return -ENOMEM;
@@ -1050,7 +1050,7 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 		subdev++;
 	}
 
-	if (this_board->cardtype == TYPE_PCI1760)
+	if (board->cardtype == TYPE_PCI1760)
 		pci1760_attach(dev);
 
 	pci_dio_reset(dev);
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index 4fe118380218..f5cfa71a90c6 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -1,23 +1,24 @@
 /*
-    comedi/drivers/amplc_dio200.c
-
-    Driver for Amplicon PC212E, PC214E, PC215E, PC218E, PC272E.
-
-    Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998,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.
+ * comedi/drivers/amplc_dio200.c
+ *
+ * Driver for Amplicon PC212E, PC214E, PC215E, PC218E, PC272E.
+ *
+ * Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998,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: amplc_dio200
  * Description: Amplicon 200 Series ISA Digital I/O
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.h b/drivers/staging/comedi/drivers/amplc_dio200.h
index d6d6a265c461..53fb86d59fc3 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.h
+++ b/drivers/staging/comedi/drivers/amplc_dio200.h
@@ -1,28 +1,32 @@
 /*
-    comedi/drivers/amplc_dio.h
-
-    Header for amplc_dio200.c, amplc_dio200_common.c and
-    amplc_dio200_pci.c.
-
-    Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998,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/amplc_dio.h
+ *
+ * Header for amplc_dio200.c, amplc_dio200_common.c and
+ * amplc_dio200_pci.c.
+ *
+ * Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998,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.
+ */
 
 #ifndef AMPLC_DIO200_H_INCLUDED
 #define AMPLC_DIO200_H_INCLUDED
 
+#include <linux/types.h>
+
+struct comedi_device;
+
 /*
  * Subdevice types.
  */
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index d15a3dc1216a..d1539e798ffd 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -1,23 +1,23 @@
 /*
-    comedi/drivers/amplc_dio200_common.c
-
-    Common support code for "amplc_dio200" and "amplc_dio200_pci".
-
-    Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998,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/amplc_dio200_common.c
+ *
+ * Common support code for "amplc_dio200" and "amplc_dio200_pci".
+ *
+ * Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998,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.
+ */
 
 #include <linux/module.h>
 #include <linux/interrupt.h>
@@ -337,9 +337,10 @@ static int dio200_handle_read_intr(struct comedi_device *dev,
 			 * interested in (just in case there's a race
 			 * condition).
 			 */
-			if (triggered & subpriv->enabled_isns)
+			if (triggered & subpriv->enabled_isns) {
 				/* Collect scan data. */
 				dio200_read_scan_intr(dev, s, triggered);
+			}
 		}
 	}
 	spin_unlock_irqrestore(&subpriv->spinlock, flags);
@@ -576,12 +577,13 @@ static int dio200_subdev_8254_init(struct comedi_device *dev,
 		regshift = 0;
 	}
 
-	if (dev->mmio)
+	if (dev->mmio) {
 		i8254 = comedi_8254_mm_init(dev->mmio + offset,
 					    0, I8254_IO8, regshift);
-	else
+	} else {
 		i8254 = comedi_8254_init(dev->iobase + offset,
 					 0, I8254_IO8, regshift);
+	}
 	if (!i8254)
 		return -ENOMEM;
 
@@ -593,10 +595,10 @@ static int dio200_subdev_8254_init(struct comedi_device *dev,
 	 * There could be multiple timers so this driver does not
 	 * use dev->pacer to save the i8254 pointer. Instead,
 	 * comedi_8254_subdevice_init() saved the i8254 pointer in
-	 * s->private. Set the runflag bit so that the core will
-	 * automatically free it when the driver is detached.
+	 * s->private.  Mark the subdevice as having private data
+	 * to be automatically freed when the device is detached.
 	 */
-	s->runflags |= COMEDI_SRF_FREE_SPRIV;
+	comedi_set_spriv_auto_free(s);
 
 	/* Initialize channels. */
 	if (board->has_clk_gat_sce) {
@@ -641,15 +643,18 @@ static int dio200_subdev_8255_bits(struct comedi_device *dev,
 
 	mask = comedi_dio_update_state(s, data);
 	if (mask) {
-		if (mask & 0xff)
+		if (mask & 0xff) {
 			dio200_write8(dev, subpriv->ofs + I8255_DATA_A_REG,
 				      s->state & 0xff);
-		if (mask & 0xff00)
+		}
+		if (mask & 0xff00) {
 			dio200_write8(dev, subpriv->ofs + I8255_DATA_B_REG,
 				      (s->state >> 8) & 0xff);
-		if (mask & 0xff0000)
+		}
+		if (mask & 0xff0000) {
 			dio200_write8(dev, subpriv->ofs + I8255_DATA_C_REG,
 				      (s->state >> 16) & 0xff);
+		}
 	}
 
 	val = dio200_read8(dev, subpriv->ofs + I8255_DATA_A_REG);
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_pci.c b/drivers/staging/comedi/drivers/amplc_dio200_pci.c
index d9850c917163..2598e6e7d47d 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_pci.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_pci.c
@@ -1,22 +1,23 @@
 /* comedi/drivers/amplc_dio200_pci.c
+ *
+ * Driver for Amplicon PCI215, PCI272, PCIe215, PCIe236, PCIe296.
+ *
+ * Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998,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 for Amplicon PCI215, PCI272, PCIe215, PCIe236, PCIe296.
-
-    Copyright (C) 2005-2013 MEV Ltd. <http://www.mev.co.uk/>
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998,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: amplc_dio200_pci
  * Description: Amplicon 200 Series PCI Digital I/O
diff --git a/drivers/staging/comedi/drivers/amplc_pc236_common.c b/drivers/staging/comedi/drivers/amplc_pc236_common.c
index 245f932a75a8..0c02d3245679 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236_common.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236_common.c
@@ -28,14 +28,14 @@
 
 static void pc236_intr_update(struct comedi_device *dev, bool enable)
 {
-	const struct pc236_board *thisboard = dev->board_ptr;
+	const struct pc236_board *board = dev->board_ptr;
 	struct pc236_private *devpriv = dev->private;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->enable_irq = enable;
-	if (thisboard->intr_update_cb)
-		thisboard->intr_update_cb(dev, enable);
+	if (board->intr_update_cb)
+		board->intr_update_cb(dev, enable);
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 }
 
@@ -48,15 +48,15 @@ static void pc236_intr_update(struct comedi_device *dev, bool enable)
  */
 static bool pc236_intr_check(struct comedi_device *dev)
 {
-	const struct pc236_board *thisboard = dev->board_ptr;
+	const struct pc236_board *board = dev->board_ptr;
 	struct pc236_private *devpriv = dev->private;
 	bool retval = false;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 	if (devpriv->enable_irq) {
-		if (thisboard->intr_chk_clr_cb)
-			retval = thisboard->intr_chk_clr_cb(dev);
+		if (board->intr_chk_clr_cb)
+			retval = board->intr_chk_clr_cb(dev);
 		else
 			retval = true;
 	}
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 08a91854837a..b2f7679a0116 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -385,14 +385,14 @@ static void
 pci224_ao_set_data(struct comedi_device *dev, int chan, int range,
 		   unsigned int data)
 {
-	const struct pci224_board *thisboard = dev->board_ptr;
+	const struct pci224_board *board = dev->board_ptr;
 	struct pci224_private *devpriv = dev->private;
 	unsigned short mangled;
 
 	/* Enable the channel. */
 	outw(1 << chan, dev->iobase + PCI224_DACCEN);
 	/* Set range and reset FIFO. */
-	devpriv->daccon = COMBINE(devpriv->daccon, thisboard->ao_hwrange[range],
+	devpriv->daccon = COMBINE(devpriv->daccon, board->ao_hwrange[range],
 				  PCI224_DACCON_POLAR_MASK |
 				  PCI224_DACCON_VREF_MASK);
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
@@ -402,7 +402,7 @@ pci224_ao_set_data(struct comedi_device *dev, int chan, int range,
 	 * - bipolar: 16-bit 2's complement
 	 * - unipolar: 16-bit unsigned
 	 */
-	mangled = (unsigned short)data << (16 - thisboard->ao_bits);
+	mangled = (unsigned short)data << (16 - board->ao_bits);
 	if ((devpriv->daccon & PCI224_DACCON_POLAR_MASK) ==
 	    PCI224_DACCON_POLAR_BI) {
 		mangled ^= 0x8000;
@@ -620,12 +620,12 @@ static int pci224_ao_check_chanlist(struct comedi_device *dev,
 				    struct comedi_subdevice *s,
 				    struct comedi_cmd *cmd)
 {
-	const struct pci224_board *thisboard = dev->board_ptr;
+	const struct pci224_board *board = dev->board_ptr;
 	unsigned int range_check_0;
 	unsigned int chan_mask = 0;
 	int i;
 
-	range_check_0 = thisboard->ao_range_check[CR_RANGE(cmd->chanlist[0])];
+	range_check_0 = board->ao_range_check[CR_RANGE(cmd->chanlist[0])];
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 
@@ -637,7 +637,7 @@ static int pci224_ao_check_chanlist(struct comedi_device *dev,
 		}
 		chan_mask |= 1 << chan;
 
-		if (thisboard->ao_range_check[CR_RANGE(cmd->chanlist[i])] !=
+		if (board->ao_range_check[CR_RANGE(cmd->chanlist[i])] !=
 		    range_check_0) {
 			dev_dbg(dev->class_dev,
 				"%s: entries in chanlist have incompatible ranges\n",
@@ -826,7 +826,7 @@ static void pci224_ao_start_pacer(struct comedi_device *dev,
 
 static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct pci224_board *thisboard = dev->board_ptr;
+	const struct pci224_board *board = dev->board_ptr;
 	struct pci224_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int range;
@@ -869,7 +869,7 @@ static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 */
 	devpriv->daccon =
 	    COMBINE(devpriv->daccon,
-		    thisboard->ao_hwrange[range] | PCI224_DACCON_TRIG_NONE |
+		    board->ao_hwrange[range] | PCI224_DACCON_TRIG_NONE |
 		    PCI224_DACCON_FIFOINTR_NHALF,
 		    PCI224_DACCON_POLAR_MASK | PCI224_DACCON_VREF_MASK |
 		    PCI224_DACCON_TRIG_MASK | PCI224_DACCON_FIFOINTR_MASK);
@@ -911,7 +911,7 @@ static void
 pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 		void *data, unsigned int num_bytes, unsigned int chan_index)
 {
-	const struct pci224_board *thisboard = dev->board_ptr;
+	const struct pci224_board *board = dev->board_ptr;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short *array = data;
 	unsigned int length = num_bytes / sizeof(*array);
@@ -920,9 +920,9 @@ pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 	unsigned int i;
 
 	/* The hardware expects 16-bit numbers. */
-	shift = 16 - thisboard->ao_bits;
+	shift = 16 - board->ao_bits;
 	/* Channels will be all bipolar or all unipolar. */
-	if ((thisboard->ao_hwrange[CR_RANGE(cmd->chanlist[0])] &
+	if ((board->ao_hwrange[CR_RANGE(cmd->chanlist[0])] &
 	     PCI224_DACCON_POLAR_MASK) == PCI224_DACCON_POLAR_UNI) {
 		/* Unipolar */
 		offset = 0;
@@ -988,21 +988,21 @@ static int
 pci224_auto_attach(struct comedi_device *dev, unsigned long context_model)
 {
 	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
-	const struct pci224_board *thisboard = NULL;
+	const struct pci224_board *board = NULL;
 	struct pci224_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq;
 	int ret;
 
 	if (context_model < ARRAY_SIZE(pci224_boards))
-		thisboard = &pci224_boards[context_model];
-	if (!thisboard || !thisboard->name) {
+		board = &pci224_boards[context_model];
+	if (!board || !board->name) {
 		dev_err(dev->class_dev,
 			"amplc_pci224: BUG! cannot determine board type!\n");
 		return -EINVAL;
 	}
-	dev->board_ptr = thisboard;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	dev_info(dev->class_dev, "amplc_pci224: attach pci %s - %s\n",
 		 pci_name(pci_dev), dev->board_name);
@@ -1023,13 +1023,13 @@ pci224_auto_attach(struct comedi_device *dev, unsigned long context_model)
 
 	/* Allocate buffer to hold values for AO channel scan. */
 	devpriv->ao_scan_vals = kmalloc(sizeof(devpriv->ao_scan_vals[0]) *
-					thisboard->ao_chans, GFP_KERNEL);
+					board->ao_chans, GFP_KERNEL);
 	if (!devpriv->ao_scan_vals)
 		return -ENOMEM;
 
 	/* Allocate buffer to hold AO channel scan order. */
 	devpriv->ao_scan_order = kmalloc(sizeof(devpriv->ao_scan_order[0]) *
-					 thisboard->ao_chans, GFP_KERNEL);
+					 board->ao_chans, GFP_KERNEL);
 	if (!devpriv->ao_scan_order)
 		return -ENOMEM;
 
@@ -1059,9 +1059,9 @@ pci224_auto_attach(struct comedi_device *dev, unsigned long context_model)
 	/* Analog output subdevice. */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = (1 << thisboard->ao_bits) - 1;
-	s->range_table = thisboard->ao_range;
+	s->n_chan = board->ao_chans;
+	s->maxdata = (1 << board->ao_bits) - 1;
+	s->range_table = board->ao_range;
 	s->insn_write = pci224_ao_insn_write;
 	s->len_chanlist = s->n_chan;
 	dev->write_subdev = s;
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 20d592002557..5c5c4e2ec3d5 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -528,7 +528,7 @@ static const struct comedi_lrange pci230_ao_range = {
 
 static unsigned short pci230_ai_read(struct comedi_device *dev)
 {
-	const struct pci230_board *thisboard = dev->board_ptr;
+	const struct pci230_board *board = dev->board_ptr;
 	struct pci230_private *devpriv = dev->private;
 	unsigned short data;
 
@@ -543,21 +543,21 @@ static unsigned short pci230_ai_read(struct comedi_device *dev)
 	 */
 	if (devpriv->ai_bipolar)
 		data ^= 0x8000;
-	data >>= (16 - thisboard->ai_bits);
+	data >>= (16 - board->ai_bits);
 	return data;
 }
 
 static unsigned short pci230_ao_mangle_datum(struct comedi_device *dev,
 					     unsigned short datum)
 {
-	const struct pci230_board *thisboard = dev->board_ptr;
+	const struct pci230_board *board = dev->board_ptr;
 	struct pci230_private *devpriv = dev->private;
 
 	/*
 	 * PCI230 is 12 bit - stored in upper bits of 16 bit register (lower
 	 * four bits reserved for expansion).  PCI230+ is also 12 bit AO.
 	 */
-	datum <<= (16 - thisboard->ao_bits);
+	datum <<= (16 - board->ao_bits);
 	/*
 	 * If a bipolar range was specified, mangle it
 	 * (straight binary->twos complement).
@@ -877,7 +877,7 @@ static int pci230_ao_check_chanlist(struct comedi_device *dev,
 static int pci230_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct pci230_board *thisboard = dev->board_ptr;
+	const struct pci230_board *board = dev->board_ptr;
 	struct pci230_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
@@ -887,14 +887,14 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_INT);
 
 	tmp = TRIG_TIMER | TRIG_INT;
-	if (thisboard->min_hwver > 0 && devpriv->hwver >= 2) {
+	if (board->min_hwver > 0 && devpriv->hwver >= 2) {
 		/*
 		 * For PCI230+ hardware version 2 onwards, allow external
 		 * trigger from EXTTRIG/EXTCONVCLK input (PCI230+ pin 25).
 		 *
 		 * FIXME: The permitted scan_begin_src values shouldn't depend
 		 * on devpriv->hwver (the detected card's actual hardware
-		 * version).  They should only depend on thisboard->min_hwver
+		 * version).  They should only depend on board->min_hwver
 		 * (the static capabilities of the configured card).  To fix
 		 * it, a new card model, e.g. "pci230+2" would have to be
 		 * defined with min_hwver set to 2.  It doesn't seem worth it
@@ -1500,7 +1500,7 @@ static int pci230_ai_check_chanlist(struct comedi_device *dev,
 static int pci230_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct pci230_board *thisboard = dev->board_ptr;
+	const struct pci230_board *board = dev->board_ptr;
 	struct pci230_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
@@ -1510,7 +1510,7 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
 
 	tmp = TRIG_FOLLOW | TRIG_TIMER | TRIG_INT;
-	if (thisboard->have_dio || thisboard->min_hwver > 0) {
+	if (board->have_dio || board->min_hwver > 0) {
 		/*
 		 * Unfortunately, we cannot trigger a scan off an external
 		 * source on the PCI260 board, since it uses the PPIC0 (DIO)
@@ -2011,17 +2011,17 @@ static void pci230_handle_ai(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int status_fifo;
 	unsigned int i;
-	unsigned int todo;
+	unsigned int nsamples;
 	unsigned int fifoamount;
 	unsigned short val;
 
 	/* Determine number of samples to read. */
-	todo = comedi_nsamples_left(s, PCI230_ADC_FIFOLEVEL_HALFFULL);
-	if (todo == 0)
+	nsamples = comedi_nsamples_left(s, PCI230_ADC_FIFOLEVEL_HALFFULL);
+	if (nsamples == 0)
 		return;
 
 	fifoamount = 0;
-	for (i = 0; i < todo; i++) {
+	for (i = 0; i < nsamples; i++) {
 		if (fifoamount == 0) {
 			/* Read FIFO state. */
 			status_fifo = inw(devpriv->daqio + PCI230_ADCCON);
@@ -2364,7 +2364,7 @@ static int pci230_auto_attach(struct comedi_device *dev,
 			      unsigned long context_unused)
 {
 	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
-	const struct pci230_board *thisboard;
+	const struct pci230_board *board;
 	struct pci230_private *devpriv;
 	struct comedi_subdevice *s;
 	int rc;
@@ -2381,14 +2381,14 @@ static int pci230_auto_attach(struct comedi_device *dev,
 	spin_lock_init(&devpriv->ai_stop_spinlock);
 	spin_lock_init(&devpriv->ao_stop_spinlock);
 
-	dev->board_ptr = pci230_find_pci_board(pci_dev);
-	if (!dev->board_ptr) {
+	board = pci230_find_pci_board(pci_dev);
+	if (!board) {
 		dev_err(dev->class_dev,
 			"amplc_pci230: BUG! cannot determine board type!\n");
 		return -EINVAL;
 	}
-	thisboard = dev->board_ptr;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	rc = comedi_pci_enable(dev);
 	if (rc)
@@ -2414,15 +2414,15 @@ static int pci230_auto_attach(struct comedi_device *dev,
 		unsigned short extfunc = 0;
 
 		devpriv->hwver = inw(devpriv->daqio + PCI230P_HWVER);
-		if (devpriv->hwver < thisboard->min_hwver) {
+		if (devpriv->hwver < board->min_hwver) {
 			dev_err(dev->class_dev,
 				"%s - bad hardware version - got %u, need %u\n",
 				dev->board_name, devpriv->hwver,
-				thisboard->min_hwver);
+				board->min_hwver);
 			return -EIO;
 		}
 		if (devpriv->hwver > 0) {
-			if (!thisboard->have_dio) {
+			if (!board->have_dio) {
 				/*
 				 * No DIO ports.  Route counters' external gates
 				 * to the EXTTRIG signal (PCI260+ pin 17).
@@ -2432,7 +2432,7 @@ static int pci230_auto_attach(struct comedi_device *dev,
 				 */
 				extfunc |= PCI230P_EXTFUNC_GAT_EXTTRIG;
 			}
-			if (thisboard->ao_bits && devpriv->hwver >= 2) {
+			if (board->ao_bits && devpriv->hwver >= 2) {
 				/* Enable DAC FIFO functionality. */
 				extfunc |= PCI230P2_EXTFUNC_DACFIFO;
 			}
@@ -2484,7 +2484,7 @@ static int pci230_auto_attach(struct comedi_device *dev,
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND;
 	s->n_chan = 16;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->maxdata = (1 << board->ai_bits) - 1;
 	s->range_table = &pci230_ai_range;
 	s->insn_read = pci230_ai_insn_read;
 	s->len_chanlist = 256;	/* but there are restrictions. */
@@ -2498,11 +2498,11 @@ static int pci230_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[1];
 	/* analog output subdevice */
-	if (thisboard->ao_bits) {
+	if (board->ao_bits) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
 		s->n_chan = 2;
-		s->maxdata = (1 << thisboard->ao_bits) - 1;
+		s->maxdata = (1 << board->ao_bits) - 1;
 		s->range_table = &pci230_ao_range;
 		s->insn_write = pci230_ao_insn_write;
 		s->len_chanlist = 2;
@@ -2523,7 +2523,7 @@ static int pci230_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
-	if (thisboard->have_dio) {
+	if (board->have_dio) {
 		rc = subdev_8255_init(dev, s, NULL, PCI230_PPI_X_BASE);
 		if (rc)
 			return rc;
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index e3591a5824fe..b43e836575fd 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -705,9 +705,9 @@ static int trimpot_8402_write(struct comedi_device *dev, unsigned int channel,
 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;
+	const struct cb_pcidas_board *board = dev->board_ptr;
 
-	switch (thisboard->trimpot) {
+	switch (board->trimpot) {
 	case AD7376:
 		trimpot_7376_write(dev, val);
 		break;
@@ -770,7 +770,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
-	const struct cb_pcidas_board *thisboard = dev->board_ptr;
+	const struct cb_pcidas_board *board = dev->board_ptr;
 	int err = 0;
 	unsigned int arg;
 
@@ -821,7 +821,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 						~(CR_EDGE | CR_INVERT));
 			err |= -EINVAL;
 		}
-		if (!thisboard->is_1602 && (cmd->start_arg & CR_INVERT)) {
+		if (!board->is_1602 && (cmd->start_arg & CR_INVERT)) {
 			cmd->start_arg &= (CR_FLAGS_MASK & ~CR_INVERT);
 			err |= -EINVAL;
 		}
@@ -830,13 +830,13 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
-						    thisboard->ai_speed *
+						    board->ai_speed *
 						    cmd->chanlist_len);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
-						    thisboard->ai_speed);
+						    board->ai_speed);
 	}
 
 	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
@@ -879,7 +879,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
-	const struct cb_pcidas_board *thisboard = dev->board_ptr;
+	const struct cb_pcidas_board *board = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
@@ -945,7 +945,7 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 		bits |= SW_TRIGGER;
 	} else {	/* TRIG_EXT */
 		bits |= EXT_TRIGGER | TGEN | XTRCL;
-		if (thisboard->is_1602) {
+		if (board->is_1602) {
 			if (cmd->start_arg & CR_INVERT)
 				bits |= TGPOL;
 			if (cmd->start_arg & CR_EDGE)
@@ -982,7 +982,7 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
-	const struct cb_pcidas_board *thisboard = dev->board_ptr;
+	const struct cb_pcidas_board *board = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
 
@@ -1014,7 +1014,7 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
-						    thisboard->ao_scan_speed);
+						    board->ao_scan_speed);
 	}
 
 	err |= comedi_check_trigger_arg_is(&cmd->scan_end_arg,
@@ -1090,7 +1090,7 @@ 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;
+	const struct cb_pcidas_board *board = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
@@ -1099,7 +1099,7 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
-	cb_pcidas_ao_load_fifo(dev, s, thisboard->fifo_size);
+	cb_pcidas_ao_load_fifo(dev, s, board->fifo_size);
 
 	/*  enable dac half-full and empty interrupts */
 	spin_lock_irqsave(&dev->spinlock, flags);
@@ -1197,7 +1197,7 @@ static int cb_pcidas_ao_cancel(struct comedi_device *dev,
 
 static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 {
-	const struct cb_pcidas_board *thisboard = dev->board_ptr;
+	const struct cb_pcidas_board *board = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->write_subdev;
 	struct comedi_async *async = s->async;
@@ -1220,7 +1220,7 @@ static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 			}
 		}
 	} else if (status & DAHFI) {
-		cb_pcidas_ao_load_fifo(dev, s, thisboard->fifo_size / 2);
+		cb_pcidas_ao_load_fifo(dev, s, board->fifo_size / 2);
 
 		/*  clear half-full interrupt latch */
 		spin_lock_irqsave(&dev->spinlock, flags);
@@ -1235,13 +1235,13 @@ static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = (struct comedi_device *)d;
-	const struct cb_pcidas_board *thisboard = dev->board_ptr;
+	const struct cb_pcidas_board *board = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
 	struct comedi_cmd *cmd;
 	int status, s5933_status;
-	int half_fifo = thisboard->fifo_size / 2;
+	int half_fifo = board->fifo_size / 2;
 	unsigned int num_samples, i;
 	static const int timeout = 10000;
 	unsigned long flags;
@@ -1338,18 +1338,18 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 				 unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct cb_pcidas_board *thisboard = NULL;
+	const struct cb_pcidas_board *board = NULL;
 	struct cb_pcidas_private *devpriv;
 	struct comedi_subdevice *s;
 	int i;
 	int ret;
 
 	if (context < ARRAY_SIZE(cb_pcidas_boards))
-		thisboard = &cb_pcidas_boards[context];
-	if (!thisboard)
+		board = &cb_pcidas_boards[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr  = thisboard;
-	dev->board_name = thisboard->name;
+	dev->board_ptr  = board;
+	dev->board_name = board->name;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -1363,7 +1363,7 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 	devpriv->control_status = pci_resource_start(pcidev, 1);
 	devpriv->adc_fifo = pci_resource_start(pcidev, 2);
 	dev->iobase = pci_resource_start(pcidev, 3);
-	if (thisboard->ao_nchan)
+	if (board->ao_nchan)
 		devpriv->ao_registers = pci_resource_start(pcidev, 4);
 
 	/*  disable and clear interrupts on amcc s5933 */
@@ -1400,10 +1400,10 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF | SDF_CMD_READ;
 	/* WARNING: Number of inputs in differential mode is ignored */
-	s->n_chan = thisboard->ai_nchan;
-	s->len_chanlist = thisboard->ai_nchan;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = thisboard->ranges;
+	s->n_chan = board->ai_nchan;
+	s->len_chanlist = board->ai_nchan;
+	s->maxdata = (1 << board->ai_bits) - 1;
+	s->range_table = board->ranges;
 	s->insn_read = cb_pcidas_ai_rinsn;
 	s->insn_config = ai_config_insn;
 	s->do_cmd = cb_pcidas_ai_cmd;
@@ -1412,15 +1412,15 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 
 	/* analog output subdevice */
 	s = &dev->subdevices[1];
-	if (thisboard->ao_nchan) {
+	if (board->ao_nchan) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
-		s->n_chan = thisboard->ao_nchan;
+		s->n_chan = board->ao_nchan;
 		/*
 		 * analog out resolution is the same as
 		 * analog input resolution, so use ai_bits
 		 */
-		s->maxdata = (1 << thisboard->ai_bits) - 1;
+		s->maxdata = (1 << board->ai_bits) - 1;
 		s->range_table = &cb_pcidas_ao_ranges;
 		/* default to no fifo (*insn_write) */
 		s->insn_write = cb_pcidas_ao_nofifo_winsn;
@@ -1429,7 +1429,7 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 		if (ret)
 			return ret;
 
-		if (thisboard->has_ao_fifo) {
+		if (board->has_ao_fifo) {
 			dev->write_subdev = s;
 			s->subdev_flags |= SDF_CMD_WRITE;
 			/* use fifo (*insn_write) instead */
@@ -1477,7 +1477,7 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 	s = &dev->subdevices[5];
 	s->type = COMEDI_SUBD_CALIB;
 	s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-	if (thisboard->trimpot == AD7376) {
+	if (board->trimpot == AD7376) {
 		s->n_chan = NUM_CHANNELS_7376;
 		s->maxdata = 0x7f;
 	} else {
@@ -1497,7 +1497,7 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 
 	/*  dac08 caldac */
 	s = &dev->subdevices[6];
-	if (thisboard->has_dac08) {
+	if (board->has_dac08) {
 		s->type = COMEDI_SUBD_CALIB;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 		s->n_chan = NUM_CHANNELS_DAC08;
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index a94c33c3d962..d33b8fe872a7 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -155,8 +155,10 @@ static inline unsigned int dac_msb_4020_reg(unsigned int channel)
 }
 
 enum read_only_registers {
-	/*  hardware status register,
-	 *  reading this apparently clears pending interrupts as well */
+	/*
+	 * hardware status register,
+	 * reading this apparently clears pending interrupts as well
+	 */
 	HW_STATUS_REG = 0x0,
 	PIPE1_READ_REG = 0x4,
 	ADC_READ_PNTR_REG = 0x8,
@@ -243,7 +245,8 @@ enum adc_control0_contents {
 	ADC_SOFT_GATE_BITS = 0x1,	/*  software gate */
 	ADC_EXT_GATE_BITS = 0x2,	/*  external digital gate */
 	ADC_ANALOG_GATE_BITS = 0x3,	/*  analog level gate */
-	ADC_GATE_LEVEL_BIT = 0x4,	/*  level-sensitive gate (for digital) */
+	/*  level-sensitive gate (for digital) */
+	ADC_GATE_LEVEL_BIT = 0x4,
 	ADC_GATE_POLARITY_BIT = 0x8,	/*  gate active low */
 	ADC_START_TRIG_SOFT_BITS = 0x10,
 	ADC_START_TRIG_EXT_BITS = 0x20,
@@ -300,7 +303,8 @@ enum calibration_contents {
 	CAL_GAIN_BIT = 0x800,
 };
 
-/* calibration sources for 6025 are:
+/*
+ * calibration sources for 6025 are:
  *  0 : ground
  *  1 : 10V
  *  2 : 5V
@@ -660,8 +664,10 @@ static const struct hw_fifo_info ai_fifo_60xx = {
 	.fifo_size_reg_mask = 0x7f,
 };
 
-/* maximum number of dma transfers we will chain together into a ring
- * (and the maximum number of dma buffers we maintain) */
+/*
+ * maximum number of dma transfers we will chain together into a ring
+ * (and the maximum number of dma buffers we maintain)
+ */
 #define MAX_AI_DMA_RING_COUNT (0x80000 / DMA_BUFFER_SIZE)
 #define MIN_AI_DMA_RING_COUNT (0x10000 / DMA_BUFFER_SIZE)
 #define AO_DMA_RING_COUNT (0x10000 / DMA_BUFFER_SIZE)
@@ -1100,10 +1106,10 @@ static const struct pcidas64_board pcidas64_boards[] = {
 static inline unsigned short se_diff_bit_6xxx(struct comedi_device *dev,
 					      int use_differential)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 
-	if ((thisboard->layout == LAYOUT_64XX && !use_differential) ||
-	    (thisboard->layout == LAYOUT_60XX && use_differential))
+	if ((board->layout == LAYOUT_64XX && !use_differential) ||
+	    (board->layout == LAYOUT_60XX && use_differential))
 		return ADC_SE_DIFF_BIT;
 
 	return 0;
@@ -1176,17 +1182,17 @@ struct pcidas64_private {
 static unsigned int ai_range_bits_6xxx(const struct comedi_device *dev,
 				       unsigned int range_index)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 
-	return thisboard->ai_range_code[range_index] << 8;
+	return board->ai_range_code[range_index] << 8;
 }
 
 static unsigned int hw_revision(const struct comedi_device *dev,
 				uint16_t hw_status_bits)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 
-	if (thisboard->layout == LAYOUT_4020)
+	if (board->layout == LAYOUT_4020)
 		return (hw_status_bits >> 13) & 0x7;
 
 	return (hw_status_bits >> 12) & 0xf;
@@ -1196,8 +1202,8 @@ static void set_dac_range_bits(struct comedi_device *dev,
 			       uint16_t *bits, unsigned int channel,
 			       unsigned int range)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
-	unsigned int code = thisboard->ao_range_code[range];
+	const struct pcidas64_board *board = dev->board_ptr;
+	unsigned int code = board->ao_range_code[range];
 
 	if (channel > 1)
 		dev_err(dev->class_dev, "bug! bad channel?\n");
@@ -1253,18 +1259,20 @@ static void disable_ai_interrupts(struct comedi_device *dev)
 static void enable_ai_interrupts(struct comedi_device *dev,
 				 const struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	uint32_t bits;
 	unsigned long flags;
 
 	bits = EN_ADC_OVERRUN_BIT | EN_ADC_DONE_INTR_BIT |
 	       EN_ADC_ACTIVE_INTR_BIT | EN_ADC_STOP_INTR_BIT;
-	/*  Use pio transfer and interrupt on end of conversion
-	 *  if CMDF_WAKE_EOS flag is set. */
+	/*
+	 * Use pio transfer and interrupt on end of conversion
+	 * if CMDF_WAKE_EOS flag is set.
+	 */
 	if (cmd->flags & CMDF_WAKE_EOS) {
 		/*  4020 doesn't support pio transfers except for fifo dregs */
-		if (thisboard->layout != LAYOUT_4020)
+		if (board->layout != LAYOUT_4020)
 			bits |= ADC_INTR_EOSCAN_BITS | EN_ADC_INTR_SRC_BIT;
 	}
 	spin_lock_irqsave(&dev->spinlock, flags);
@@ -1277,7 +1285,7 @@ static void enable_ai_interrupts(struct comedi_device *dev,
 /* initialize plx9080 chip */
 static void init_plx9080(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	uint32_t bits;
 	void __iomem *plx_iobase = devpriv->plx9080_iobase;
@@ -1318,12 +1326,12 @@ static void init_plx9080(struct comedi_device *dev)
 	/*  enable local burst mode */
 	bits |= PLX_DMA_LOCAL_BURST_EN_BIT;
 	/*  4020 uses 32 bit dma */
-	if (thisboard->layout == LAYOUT_4020)
+	if (board->layout == LAYOUT_4020)
 		bits |= PLX_LOCAL_BUS_32_WIDE_BITS;
 	else		/*  localspace0 bus is 16 bits wide */
 		bits |= PLX_LOCAL_BUS_16_WIDE_BITS;
 	writel(bits, plx_iobase + PLX_DMA1_MODE_REG);
-	if (ao_cmd_is_supported(thisboard))
+	if (ao_cmd_is_supported(board))
 		writel(bits, plx_iobase + PLX_DMA0_MODE_REG);
 
 	/*  enable interrupts on plx 9080 */
@@ -1355,10 +1363,10 @@ static void disable_ai_pacing(struct comedi_device *dev)
 static int set_ai_fifo_segment_length(struct comedi_device *dev,
 				      unsigned int num_entries)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	static const int increment_size = 0x100;
-	const struct hw_fifo_info *const fifo = thisboard->ai_fifo;
+	const struct hw_fifo_info *const fifo = board->ai_fifo;
 	unsigned int num_increments;
 	uint16_t bits;
 
@@ -1381,13 +1389,15 @@ static int set_ai_fifo_segment_length(struct comedi_device *dev,
 	return devpriv->ai_fifo_segment_length;
 }
 
-/* adjusts the size of hardware fifo (which determines block size for dma xfers) */
+/*
+ * adjusts the size of hardware fifo (which determines block size for dma xfers)
+ */
 static int set_ai_fifo_size(struct comedi_device *dev, unsigned int num_samples)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	unsigned int num_fifo_entries;
 	int retval;
-	const struct hw_fifo_info *const fifo = thisboard->ai_fifo;
+	const struct hw_fifo_info *const fifo = board->ai_fifo;
 
 	num_fifo_entries = num_samples / fifo->sample_packing_ratio;
 
@@ -1405,25 +1415,27 @@ static int set_ai_fifo_size(struct comedi_device *dev, unsigned int num_samples)
 /* query length of fifo */
 static unsigned int ai_fifo_size(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 
 	return devpriv->ai_fifo_segment_length *
-	       thisboard->ai_fifo->num_segments *
-	       thisboard->ai_fifo->sample_packing_ratio;
+	       board->ai_fifo->num_segments *
+	       board->ai_fifo->sample_packing_ratio;
 }
 
 static void init_stc_registers(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	uint16_t bits;
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->spinlock, flags);
 
-	/*  bit should be set for 6025,
-	 *  although docs say boards with <= 16 chans should be cleared XXX */
+	/*
+	 * bit should be set for 6025,
+	 * although docs say boards with <= 16 chans should be cleared XXX
+	 */
 	if (1)
 		devpriv->adc_control1_bits |= ADC_QUEUE_CONFIG_BIT;
 	writew(devpriv->adc_control1_bits,
@@ -1433,7 +1445,7 @@ static void init_stc_registers(struct comedi_device *dev)
 	writew(0xff, devpriv->main_iobase + ADC_SAMPLE_INTERVAL_UPPER_REG);
 
 	bits = SLOW_DAC_BIT | DMA_CH_SELECT_BIT;
-	if (thisboard->layout == LAYOUT_4020)
+	if (board->layout == LAYOUT_4020)
 		bits |= INTERNAL_CLOCK_4020_BITS;
 	devpriv->hw_config_bits |= bits;
 	writew(devpriv->hw_config_bits,
@@ -1446,8 +1458,7 @@ static void init_stc_registers(struct comedi_device *dev)
 
 	/*  set fifos to maximum size */
 	devpriv->fifo_size_bits |= DAC_FIFO_BITS;
-	set_ai_fifo_segment_length(dev,
-				   thisboard->ai_fifo->max_segment_length);
+	set_ai_fifo_segment_length(dev, board->ai_fifo->max_segment_length);
 
 	devpriv->dac_control1_bits = DAC_OUTPUT_ENABLE_BIT;
 	devpriv->intr_enable_bits =
@@ -1461,13 +1472,13 @@ static void init_stc_registers(struct comedi_device *dev)
 
 static int alloc_and_init_dma_members(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct pcidas64_private *devpriv = dev->private;
 	int i;
 
 	/*  allocate pci dma buffers */
-	for (i = 0; i < ai_dma_ring_count(thisboard); i++) {
+	for (i = 0; i < ai_dma_ring_count(board); i++) {
 		devpriv->ai_buffer[i] =
 			pci_alloc_consistent(pcidev, DMA_BUFFER_SIZE,
 					     &devpriv->ai_buffer_bus_addr[i]);
@@ -1475,7 +1486,7 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 			return -ENOMEM;
 	}
 	for (i = 0; i < AO_DMA_RING_COUNT; i++) {
-		if (ao_cmd_is_supported(thisboard)) {
+		if (ao_cmd_is_supported(board)) {
 			devpriv->ao_buffer[i] =
 				pci_alloc_consistent(pcidev, DMA_BUFFER_SIZE,
 						     &devpriv->
@@ -1487,12 +1498,12 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 	/*  allocate dma descriptors */
 	devpriv->ai_dma_desc =
 		pci_alloc_consistent(pcidev, sizeof(struct plx_dma_desc) *
-				     ai_dma_ring_count(thisboard),
+				     ai_dma_ring_count(board),
 				     &devpriv->ai_dma_desc_bus_addr);
 	if (!devpriv->ai_dma_desc)
 		return -ENOMEM;
 
-	if (ao_cmd_is_supported(thisboard)) {
+	if (ao_cmd_is_supported(board)) {
 		devpriv->ao_dma_desc =
 			pci_alloc_consistent(pcidev,
 					     sizeof(struct plx_dma_desc) *
@@ -1502,10 +1513,10 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 			return -ENOMEM;
 	}
 	/*  initialize dma descriptors */
-	for (i = 0; i < ai_dma_ring_count(thisboard); i++) {
+	for (i = 0; i < ai_dma_ring_count(board); i++) {
 		devpriv->ai_dma_desc[i].pci_start_addr =
 			cpu_to_le32(devpriv->ai_buffer_bus_addr[i]);
-		if (thisboard->layout == LAYOUT_4020)
+		if (board->layout == LAYOUT_4020)
 			devpriv->ai_dma_desc[i].local_start_addr =
 				cpu_to_le32(devpriv->local1_iobase +
 					    ADC_FIFO_REG);
@@ -1516,12 +1527,12 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 		devpriv->ai_dma_desc[i].transfer_size = cpu_to_le32(0);
 		devpriv->ai_dma_desc[i].next =
 			cpu_to_le32((devpriv->ai_dma_desc_bus_addr +
-				     ((i + 1) % ai_dma_ring_count(thisboard)) *
+				     ((i + 1) % ai_dma_ring_count(board)) *
 				     sizeof(devpriv->ai_dma_desc[0])) |
 				    PLX_DESC_IN_PCI_BIT | PLX_INTR_TERM_COUNT |
 				    PLX_XFER_LOCAL_TO_PCI);
 	}
-	if (ao_cmd_is_supported(thisboard)) {
+	if (ao_cmd_is_supported(board)) {
 		for (i = 0; i < AO_DMA_RING_COUNT; i++) {
 			devpriv->ao_dma_desc[i].pci_start_addr =
 				cpu_to_le32(devpriv->ao_buffer_bus_addr[i]);
@@ -1542,7 +1553,7 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 
 static void cb_pcidas64_free_dma(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct pcidas64_private *devpriv = dev->private;
 	int i;
@@ -1551,7 +1562,7 @@ static void cb_pcidas64_free_dma(struct comedi_device *dev)
 		return;
 
 	/* free pci dma buffers */
-	for (i = 0; i < ai_dma_ring_count(thisboard); i++) {
+	for (i = 0; i < ai_dma_ring_count(board); i++) {
 		if (devpriv->ai_buffer[i])
 			pci_free_consistent(pcidev,
 					    DMA_BUFFER_SIZE,
@@ -1569,7 +1580,7 @@ static void cb_pcidas64_free_dma(struct comedi_device *dev)
 	if (devpriv->ai_dma_desc)
 		pci_free_consistent(pcidev,
 				    sizeof(struct plx_dma_desc) *
-				    ai_dma_ring_count(thisboard),
+				    ai_dma_ring_count(board),
 				    devpriv->ai_dma_desc,
 				    devpriv->ai_dma_desc_bus_addr);
 	if (devpriv->ao_dma_desc)
@@ -1588,7 +1599,9 @@ static inline void warn_external_queue(struct comedi_device *dev)
 		"Use internal AI channel queue (channels must be consecutive and use same range/aref)\n");
 }
 
-/* Their i2c requires a huge delay on setting clock or data high for some reason */
+/*
+ * their i2c requires a huge delay on setting clock or data high for some reason
+ */
 static const int i2c_high_udelay = 1000;
 static const int i2c_low_udelay = 10;
 
@@ -1684,8 +1697,10 @@ static void i2c_write(struct comedi_device *dev, unsigned int address,
 	uint8_t bitstream;
 	static const int read_bit = 0x1;
 
-	/* XXX need mutex to prevent simultaneous attempts to access
-	 * eeprom and i2c bus */
+	/*
+	 * XXX need mutex to prevent simultaneous attempts to access
+	 * eeprom and i2c bus
+	 */
 
 	/*  make sure we dont send anything to eeprom */
 	devpriv->plx_control_bits &= ~CTL_EE_CS;
@@ -1720,12 +1735,12 @@ static int cb_pcidas64_ai_eoc(struct comedi_device *dev,
 			      struct comedi_insn *insn,
 			      unsigned long context)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned int status;
 
 	status = readw(devpriv->main_iobase + HW_STATUS_REG);
-	if (thisboard->layout == LAYOUT_4020) {
+	if (board->layout == LAYOUT_4020) {
 		status = readw(devpriv->main_iobase + ADC_WRITE_PNTR_REG);
 		if (status)
 			return 0;
@@ -1739,7 +1754,7 @@ static int cb_pcidas64_ai_eoc(struct comedi_device *dev,
 static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		    struct comedi_insn *insn, unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned int bits = 0, n;
 	unsigned int channel, range, aref;
@@ -1763,7 +1778,7 @@ static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	       devpriv->main_iobase + ADC_CONTROL1_REG);
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	if (thisboard->layout != LAYOUT_4020) {
+	if (board->layout != LAYOUT_4020) {
 		/*  use internal queue */
 		devpriv->hw_config_bits &= ~EXT_QUEUE_BIT;
 		writew(devpriv->hw_config_bits,
@@ -1773,18 +1788,22 @@ static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		if (insn->chanspec & CR_ALT_SOURCE) {
 			unsigned int cal_en_bit;
 
-			if (thisboard->layout == LAYOUT_60XX)
+			if (board->layout == LAYOUT_60XX)
 				cal_en_bit = CAL_EN_60XX_BIT;
 			else
 				cal_en_bit = CAL_EN_64XX_BIT;
-			/*  select internal reference source to connect
-			 *  to channel 0 */
+			/*
+			 * select internal reference source to connect
+			 * to channel 0
+			 */
 			writew(cal_en_bit |
 			       adc_src_bits(devpriv->calibration_source),
 			       devpriv->main_iobase + CALIBRATION_REG);
 		} else {
-			/*  make sure internal calibration source
-			 *  is turned off */
+			/*
+			 * make sure internal calibration source
+			 * is turned off
+			 */
 			writew(0, devpriv->main_iobase + CALIBRATION_REG);
 		}
 		/*  load internal queue */
@@ -1816,8 +1835,10 @@ static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			devpriv->i2c_cal_range_bits |= attenuate_bit(channel);
 		else
 			devpriv->i2c_cal_range_bits &= ~attenuate_bit(channel);
-		/*  update calibration/range i2c register only if necessary,
-		 *  as it is very slow */
+		/*
+		 * update calibration/range i2c register only if necessary,
+		 * as it is very slow
+		 */
 		if (old_cal_range_bits != devpriv->i2c_cal_range_bits) {
 			uint8_t i2c_data = devpriv->i2c_cal_range_bits;
 
@@ -1825,10 +1846,12 @@ static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 				  sizeof(i2c_data));
 		}
 
-		/* 4020 manual asks that sample interval register to be set
+		/*
+		 * 4020 manual asks that sample interval register to be set
 		 * before writing to convert register.
 		 * Using somewhat arbitrary setting of 4 master clock ticks
-		 * = 0.1 usec */
+		 * = 0.1 usec
+		 */
 		writew(0, devpriv->main_iobase + ADC_SAMPLE_INTERVAL_UPPER_REG);
 		writew(2, devpriv->main_iobase + ADC_SAMPLE_INTERVAL_LOWER_REG);
 	}
@@ -1846,7 +1869,7 @@ static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		if (ret)
 			return ret;
 
-		if (thisboard->layout == LAYOUT_4020)
+		if (board->layout == LAYOUT_4020)
 			data[n] = readl(dev->mmio + ADC_FIFO_REG) & 0xffff;
 		else
 			data[n] = readw(devpriv->main_iobase + PIPE1_READ_REG);
@@ -1858,12 +1881,12 @@ static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 static int ai_config_calibration_source(struct comedi_device *dev,
 					unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned int source = data[1];
 	int num_calibration_sources;
 
-	if (thisboard->layout == LAYOUT_60XX)
+	if (board->layout == LAYOUT_60XX)
 		num_calibration_sources = 16;
 	else
 		num_calibration_sources = 8;
@@ -1880,9 +1903,9 @@ static int ai_config_calibration_source(struct comedi_device *dev,
 
 static int ai_config_block_size(struct comedi_device *dev, unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	int fifo_size;
-	const struct hw_fifo_info *const fifo = thisboard->ai_fifo;
+	const struct hw_fifo_info *const fifo = board->ai_fifo;
 	unsigned int block_size, requested_block_size;
 	int retval;
 
@@ -1933,9 +1956,9 @@ static int ai_config_master_clock_4020(struct comedi_device *dev,
 /* XXX could add support for 60xx series */
 static int ai_config_master_clock(struct comedi_device *dev, unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 
-	switch (thisboard->layout) {
+	switch (board->layout) {
 	case LAYOUT_4020:
 		return ai_config_master_clock_4020(dev, data);
 	default:
@@ -1963,9 +1986,11 @@ static int ai_config_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return -EINVAL;
 }
 
-/* Gets nearest achievable timing given master clock speed, does not
+/*
+ * Gets nearest achievable timing given master clock speed, does not
  * take into account possible minimum/maximum divisor values.  Used
- * by other timing checking functions. */
+ * by other timing checking functions.
+ */
 static unsigned int get_divisor(unsigned int ns, unsigned int flags)
 {
 	unsigned int divisor;
@@ -1985,13 +2010,15 @@ static unsigned int get_divisor(unsigned int ns, unsigned int flags)
 	return divisor;
 }
 
-/* utility function that rounds desired timing to an achievable time, and
+/*
+ * utility function that rounds desired timing to an achievable time, and
  * sets cmd members appropriately.
- * adc paces conversions from master clock by dividing by (x + 3) where x is 24 bit number
+ * adc paces conversions from master clock by dividing by (x + 3) where x is
+ * 24 bit number
  */
 static void check_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	unsigned long long convert_divisor = 0;
 	unsigned int scan_divisor;
 	static const int min_convert_divisor = 3;
@@ -2001,7 +2028,7 @@ static void check_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 	unsigned long long max_scan_divisor, min_scan_divisor;
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (thisboard->layout == LAYOUT_4020) {
+		if (board->layout == LAYOUT_4020) {
 			cmd->convert_arg = 0;
 		} else {
 			convert_divisor = get_divisor(cmd->convert_arg,
@@ -2078,7 +2105,7 @@ static int cb_pcidas64_ai_check_chanlist(struct comedi_device *dev,
 static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	int err = 0;
 	unsigned int tmp_arg, tmp_arg2;
 	unsigned int triggers;
@@ -2088,14 +2115,14 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	err |= comedi_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
 
 	triggers = TRIG_TIMER;
-	if (thisboard->layout == LAYOUT_4020)
+	if (board->layout == LAYOUT_4020)
 		triggers |= TRIG_OTHER;
 	else
 		triggers |= TRIG_FOLLOW;
 	err |= comedi_check_trigger_src(&cmd->scan_begin_src, triggers);
 
 	triggers = TRIG_TIMER;
-	if (thisboard->layout == LAYOUT_4020)
+	if (board->layout == LAYOUT_4020)
 		triggers |= TRIG_NOW;
 	else
 		triggers |= TRIG_EXT;
@@ -2137,13 +2164,12 @@ static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
-		if (thisboard->layout == LAYOUT_4020) {
+		if (board->layout == LAYOUT_4020) {
 			err |= comedi_check_trigger_arg_is(&cmd->convert_arg,
 							   0);
 		} else {
 			err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
-							    thisboard->
-							    ai_speed);
+							    board->ai_speed);
 			/*
 			 * if scans are timed faster than conversion rate
 			 * allows
@@ -2231,12 +2257,12 @@ static void setup_sample_counters(struct comedi_device *dev,
 
 static inline unsigned int dma_transfer_size(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned int num_samples;
 
 	num_samples = devpriv->ai_fifo_segment_length *
-		      thisboard->ai_fifo->sample_packing_ratio;
+		      board->ai_fifo->sample_packing_ratio;
 	if (num_samples > DMA_BUFFER_SIZE / sizeof(uint16_t))
 		num_samples = DMA_BUFFER_SIZE / sizeof(uint16_t);
 
@@ -2318,9 +2344,9 @@ static void select_master_clock_4020(struct comedi_device *dev,
 static void select_master_clock(struct comedi_device *dev,
 				const struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 
-	switch (thisboard->layout) {
+	switch (board->layout) {
 	case LAYOUT_4020:
 		select_master_clock_4020(dev, cmd);
 		break;
@@ -2350,7 +2376,7 @@ static inline void dma_start_sync(struct comedi_device *dev,
 
 static void set_ai_pacing(struct comedi_device *dev, struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	uint32_t convert_counter = 0, scan_counter = 0;
 
@@ -2358,7 +2384,7 @@ static void set_ai_pacing(struct comedi_device *dev, struct comedi_cmd *cmd)
 
 	select_master_clock(dev, cmd);
 
-	if (thisboard->layout == LAYOUT_4020) {
+	if (board->layout == LAYOUT_4020) {
 		convert_counter = ai_convert_counter_4020(dev, cmd);
 	} else {
 		convert_counter = ai_convert_counter_6xxx(dev, cmd);
@@ -2399,12 +2425,12 @@ static int use_internal_queue_6xxx(const struct comedi_cmd *cmd)
 static int setup_channel_queue(struct comedi_device *dev,
 			       const struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned short bits;
 	int i;
 
-	if (thisboard->layout != LAYOUT_4020) {
+	if (board->layout != LAYOUT_4020) {
 		if (use_internal_queue_6xxx(cmd)) {
 			devpriv->hw_config_bits &= ~EXT_QUEUE_BIT;
 			writew(devpriv->hw_config_bits,
@@ -2468,8 +2494,10 @@ static int setup_channel_queue(struct comedi_device *dev,
 				       devpriv->main_iobase +
 				       ADC_QUEUE_FIFO_REG);
 			}
-			/* doing a queue clear is not specified in board docs,
-			 * but required for reliable operation */
+			/*
+			 * doing a queue clear is not specified in board docs,
+			 * but required for reliable operation
+			 */
 			writew(0, devpriv->main_iobase + ADC_QUEUE_CLEAR_REG);
 			/*  prime queue holding register */
 			writew(0, devpriv->main_iobase + ADC_QUEUE_LOAD_REG);
@@ -2492,8 +2520,10 @@ static int setup_channel_queue(struct comedi_device *dev,
 				devpriv->i2c_cal_range_bits &=
 					~attenuate_bit(channel);
 		}
-		/*  update calibration/range i2c register only if necessary,
-		 *  as it is very slow */
+		/*
+		 * update calibration/range i2c register only if necessary,
+		 * as it is very slow
+		 */
 		if (old_cal_range_bits != devpriv->i2c_cal_range_bits) {
 			uint8_t i2c_data = devpriv->i2c_cal_range_bits;
 
@@ -2510,11 +2540,13 @@ static inline void load_first_dma_descriptor(struct comedi_device *dev,
 {
 	struct pcidas64_private *devpriv = dev->private;
 
-	/* The transfer size, pci address, and local address registers
+	/*
+	 * The transfer size, pci address, and local address registers
 	 * are supposedly unused during chained dma,
 	 * but I have found that left over values from last operation
 	 * occasionally cause problems with transfer of first dma
-	 * block.  Initializing them to zero seems to fix the problem. */
+	 * block.  Initializing them to zero seems to fix the problem.
+	 */
 	if (dma_channel) {
 		writel(0,
 		       devpriv->plx9080_iobase + PLX_DMA1_TRANSFER_SIZE_REG);
@@ -2536,7 +2568,7 @@ static inline void load_first_dma_descriptor(struct comedi_device *dev,
 
 static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
@@ -2565,7 +2597,7 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/* set mode, allow conversions through software gate */
 	devpriv->adc_control1_bits |= ADC_SW_GATE_BIT;
 	devpriv->adc_control1_bits &= ~ADC_DITHER_BIT;
-	if (thisboard->layout != LAYOUT_4020) {
+	if (board->layout != LAYOUT_4020) {
 		devpriv->adc_control1_bits &= ~ADC_MODE_MASK;
 		if (cmd->convert_src == TRIG_EXT)
 			/*  good old mode 13 */
@@ -2595,11 +2627,11 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	writew(0, devpriv->main_iobase + ADC_BUFFER_CLEAR_REG);
 
 	if ((cmd->flags & CMDF_WAKE_EOS) == 0 ||
-	    thisboard->layout == LAYOUT_4020) {
+	    board->layout == LAYOUT_4020) {
 		devpriv->ai_dma_index = 0;
 
 		/*  set dma transfer size */
-		for (i = 0; i < ai_dma_ring_count(thisboard); i++)
+		for (i = 0; i < ai_dma_ring_count(board); i++)
 			devpriv->ai_dma_desc[i].transfer_size =
 				cpu_to_le32(dma_transfer_size(dev) *
 					    sizeof(uint16_t));
@@ -2614,7 +2646,7 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		dma_start_sync(dev, 1);
 	}
 
-	if (thisboard->layout == LAYOUT_4020) {
+	if (board->layout == LAYOUT_4020) {
 		/* set source for external triggers */
 		bits = 0;
 		if (cmd->start_src == TRIG_EXT && CR_CHAN(cmd->start_arg))
@@ -2669,15 +2701,19 @@ static void pio_drain_ai_fifo_16(struct comedi_device *dev)
 			     0x7fff;
 		write_index = readw(devpriv->main_iobase + ADC_WRITE_PNTR_REG) &
 			      0x7fff;
-		/* Get most significant bits (grey code).
+		/*
+		 * Get most significant bits (grey code).
 		 * Different boards use different code so use a scheme
 		 * that doesn't depend on encoding.  This read must
 		 * occur after reading least significant 15 bits to avoid race
-		 * with fifo switching to next segment. */
+		 * with fifo switching to next segment.
+		 */
 		prepost_bits = readw(devpriv->main_iobase + PREPOST_REG);
 
-		/* if read and write pointers are not on the same fifo segment,
-		 * read to the end of the read segment */
+		/*
+		 * if read and write pointers are not on the same fifo segment,
+		 * read to the end of the read segment
+		 */
 		read_segment = adc_upper_read_ptr_code(prepost_bits);
 		write_segment = adc_upper_write_ptr_code(prepost_bits);
 
@@ -2706,7 +2742,8 @@ static void pio_drain_ai_fifo_16(struct comedi_device *dev)
 	} while (read_segment != write_segment);
 }
 
-/* Read from 32 bit wide ai fifo of 4020 - deal with insane grey coding of
+/*
+ * Read from 32 bit wide ai fifo of 4020 - deal with insane grey coding of
  * pointers.  The pci-4020 hardware only supports dma transfers (it only
  * supports the use of pio for draining the last remaining points from the
  * fifo when a data acquisition operation has completed).
@@ -2744,9 +2781,9 @@ static void pio_drain_ai_fifo_32(struct comedi_device *dev)
 /* empty fifo */
 static void pio_drain_ai_fifo(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 
-	if (thisboard->layout == LAYOUT_4020)
+	if (board->layout == LAYOUT_4020)
 		pio_drain_ai_fifo_32(dev);
 	else
 		pio_drain_ai_fifo_16(dev);
@@ -2754,7 +2791,7 @@ static void pio_drain_ai_fifo(struct comedi_device *dev)
 
 static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	uint32_t next_transfer_addr;
@@ -2775,24 +2812,26 @@ static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 	      devpriv->ai_buffer_bus_addr[devpriv->ai_dma_index] ||
 	      next_transfer_addr >=
 	      devpriv->ai_buffer_bus_addr[devpriv->ai_dma_index] +
-	      DMA_BUFFER_SIZE) && j < ai_dma_ring_count(thisboard); j++) {
+	      DMA_BUFFER_SIZE) && j < ai_dma_ring_count(board); j++) {
 		/*  transfer data from dma buffer to comedi buffer */
 		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);
+					ai_dma_ring_count(board);
 	}
-	/* XXX check for dma ring buffer overrun
-	 * (use end-of-chain bit to mark last unused buffer) */
+	/*
+	 * XXX check for dma ring buffer overrun
+	 * (use end-of-chain bit to mark last unused buffer)
+	 */
 }
 
 static void handle_ai_interrupt(struct comedi_device *dev,
 				unsigned short status,
 				unsigned int plx_status)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async = s->async;
@@ -2821,7 +2860,7 @@ static void handle_ai_interrupt(struct comedi_device *dev,
 	if ((status & ADC_DONE_BIT) ||
 	    ((cmd->flags & CMDF_WAKE_EOS) &&
 	     (status & ADC_INTR_PENDING_BIT) &&
-	     (thisboard->layout != LAYOUT_4020))) {
+	     (board->layout != LAYOUT_4020))) {
 		spin_lock_irqsave(&dev->spinlock, flags);
 		if (devpriv->ai_cmd_running) {
 			spin_unlock_irqrestore(&dev->spinlock, flags);
@@ -2933,8 +2972,10 @@ static unsigned int load_ao_dma_buffer(struct comedi_device *dev,
 	next_bits = le32_to_cpu(devpriv->ao_dma_desc[buffer_index].next);
 	next_bits |= PLX_END_OF_CHAIN_BIT;
 	devpriv->ao_dma_desc[buffer_index].next = cpu_to_le32(next_bits);
-	/* clear end of chain bit on previous buffer now that we have set it
-	 * for the last buffer */
+	/*
+	 * clear end of chain bit on previous buffer now that we have set it
+	 * for the last buffer
+	 */
 	next_bits = le32_to_cpu(devpriv->ao_dma_desc[prev_buffer_index].next);
 	next_bits &= ~PLX_END_OF_CHAIN_BIT;
 	devpriv->ao_dma_desc[prev_buffer_index].next = cpu_to_le32(next_bits);
@@ -3027,9 +3068,11 @@ static irqreturn_t handle_interrupt(int irq, void *d)
 	plx_status = readl(devpriv->plx9080_iobase + PLX_INTRCS_REG);
 	status = readw(devpriv->main_iobase + HW_STATUS_REG);
 
-	/* an interrupt before all the postconfig stuff gets done could
+	/*
+	 * an interrupt before all the postconfig stuff gets done could
 	 * cause a NULL dereference if we continue through the
-	 * interrupt handler */
+	 * interrupt handler
+	 */
 	if (!dev->attached)
 		return IRQ_HANDLED;
 
@@ -3068,7 +3111,7 @@ static int ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		    struct comedi_insn *insn, unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 	int range = CR_RANGE(insn->chanspec);
@@ -3082,7 +3125,7 @@ static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	       devpriv->main_iobase + DAC_CONTROL1_REG);
 
 	/*  write to channel */
-	if (thisboard->layout == LAYOUT_4020) {
+	if (board->layout == LAYOUT_4020) {
 		writew(data[0] & 0xff,
 		       devpriv->main_iobase + dac_lsb_4020_reg(chan));
 		writew((data[0] >> 8) & 0xf,
@@ -3189,8 +3232,10 @@ static int prep_ao_dma(struct comedi_device *dev, const struct comedi_cmd *cmd)
 	unsigned int nbytes;
 	int i;
 
-	/* clear queue pointer too, since external queue has
-	 * weird interactions with ao fifo */
+	/*
+	 * clear queue pointer too, since external queue has
+	 * weird interactions with ao fifo
+	 */
 	writew(0, devpriv->main_iobase + ADC_QUEUE_CLEAR_REG);
 	writew(0, devpriv->main_iobase + DAC_BUFFER_CLEAR_REG);
 
@@ -3223,11 +3268,11 @@ static inline int external_ai_queue_in_use(struct comedi_device *dev,
 					   struct comedi_subdevice *s,
 					   struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 
 	if (s->busy)
 		return 0;
-	if (thisboard->layout == LAYOUT_4020)
+	if (board->layout == LAYOUT_4020)
 		return 0;
 	else if (use_internal_queue_6xxx(cmd))
 		return 0;
@@ -3306,7 +3351,7 @@ static int cb_pcidas64_ao_check_chanlist(struct comedi_device *dev,
 static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	int err = 0;
 	unsigned int tmp_arg;
 
@@ -3344,7 +3389,7 @@ static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
-						    thisboard->ao_scan_speed);
+						    board->ao_scan_speed);
 		if (get_ao_divisor(cmd->scan_begin_arg, cmd->flags) >
 		    max_counter_value) {
 			cmd->scan_begin_arg = (max_counter_value + 2) *
@@ -3459,7 +3504,8 @@ static int dio_60xx_wbits(struct comedi_device *dev,
 	return insn->n;
 }
 
-/* pci-6025 8800 caldac:
+/*
+ * pci-6025 8800 caldac:
  * address 0 == dac channel 0 offset
  * address 1 == dac channel 0 gain
  * address 2 == dac channel 1 offset
@@ -3469,7 +3515,8 @@ static int dio_60xx_wbits(struct comedi_device *dev,
  * address 6 == coarse adc gain
  * address 7 == fine adc gain
  */
-/* pci-6402/16 uses all 8 channels for dac:
+/*
+ * pci-6402/16 uses all 8 channels for dac:
  * address 0 == dac channel 0 fine gain
  * address 1 == dac channel 0 coarse gain
  * address 2 == dac channel 0 coarse offset
@@ -3478,7 +3525,7 @@ static int dio_60xx_wbits(struct comedi_device *dev,
  * address 5 == dac channel 1 coarse gain
  * address 6 == dac channel 0 fine offset
  * address 7 == dac channel 1 fine offset
-*/
+ */
 
 static int caldac_8800_write(struct comedi_device *dev, unsigned int address,
 			     uint8_t value)
@@ -3575,9 +3622,9 @@ static int caldac_i2c_write(struct comedi_device *dev,
 static void caldac_write(struct comedi_device *dev, unsigned int channel,
 			 unsigned int value)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 
-	switch (thisboard->layout) {
+	switch (board->layout) {
 	case LAYOUT_60XX:
 	case LAYOUT_64XX:
 		caldac_8800_write(dev, channel, value);
@@ -3738,11 +3785,12 @@ static int eeprom_read_insn(struct comedi_device *dev,
 	return 1;
 }
 
-/* Allocate and initialize the subdevice structures.
+/*
+ * Allocate and initialize the subdevice structures.
  */
 static int setup_subdevices(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = dev->board_ptr;
+	const struct pcidas64_board *board = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	int i;
@@ -3757,24 +3805,26 @@ static int setup_subdevices(struct comedi_device *dev)
 	dev->read_subdev = s;
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DITHER | SDF_CMD_READ;
-	if (thisboard->layout == LAYOUT_60XX)
+	if (board->layout == LAYOUT_60XX)
 		s->subdev_flags |= SDF_COMMON | SDF_DIFF;
-	else if (thisboard->layout == LAYOUT_64XX)
+	else if (board->layout == LAYOUT_64XX)
 		s->subdev_flags |= SDF_DIFF;
 	/* XXX Number of inputs in differential mode is ignored */
-	s->n_chan = thisboard->ai_se_chans;
+	s->n_chan = board->ai_se_chans;
 	s->len_chanlist = 0x2000;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = thisboard->ai_range_table;
+	s->maxdata = (1 << board->ai_bits) - 1;
+	s->range_table = board->ai_range_table;
 	s->insn_read = ai_rinsn;
 	s->insn_config = ai_config_insn;
 	s->do_cmd = ai_cmd;
 	s->do_cmdtest = ai_cmdtest;
 	s->cancel = ai_cancel;
-	if (thisboard->layout == LAYOUT_4020) {
+	if (board->layout == LAYOUT_4020) {
 		uint8_t data;
-		/*  set adc to read from inputs
-		 *  (not internal calibration sources) */
+		/*
+		 * set adc to read from inputs
+		 * (not internal calibration sources)
+		 */
 		devpriv->i2c_cal_range_bits = adc_src_4020_bits(4);
 		/*  set channels to +-5 volt input ranges */
 		for (i = 0; i < s->n_chan; i++)
@@ -3785,24 +3835,24 @@ static int setup_subdevices(struct comedi_device *dev)
 
 	/* analog output subdevice */
 	s = &dev->subdevices[1];
-	if (thisboard->ao_nchan) {
+	if (board->ao_nchan) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE |
 				  SDF_GROUND | SDF_CMD_WRITE;
-		s->n_chan = thisboard->ao_nchan;
-		s->maxdata = (1 << thisboard->ao_bits) - 1;
-		s->range_table = thisboard->ao_range_table;
+		s->n_chan = board->ao_nchan;
+		s->maxdata = (1 << board->ao_bits) - 1;
+		s->range_table = board->ao_range_table;
 		s->insn_write = ao_winsn;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
 			return ret;
 
-		if (ao_cmd_is_supported(thisboard)) {
+		if (ao_cmd_is_supported(board)) {
 			dev->write_subdev = s;
 			s->do_cmdtest = ao_cmdtest;
 			s->do_cmd = ao_cmd;
-			s->len_chanlist = thisboard->ao_nchan;
+			s->len_chanlist = board->ao_nchan;
 			s->cancel = ao_cancel;
 		}
 	} else {
@@ -3811,7 +3861,7 @@ static int setup_subdevices(struct comedi_device *dev)
 
 	/*  digital input */
 	s = &dev->subdevices[2];
-	if (thisboard->layout == LAYOUT_64XX) {
+	if (board->layout == LAYOUT_64XX) {
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
 		s->n_chan = 4;
@@ -3823,7 +3873,7 @@ static int setup_subdevices(struct comedi_device *dev)
 	}
 
 	/*  digital output */
-	if (thisboard->layout == LAYOUT_64XX) {
+	if (board->layout == LAYOUT_64XX) {
 		s = &dev->subdevices[3];
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE;
@@ -3837,8 +3887,8 @@ static int setup_subdevices(struct comedi_device *dev)
 
 	/* 8255 */
 	s = &dev->subdevices[4];
-	if (thisboard->has_8255) {
-		if (thisboard->layout == LAYOUT_4020) {
+	if (board->has_8255) {
+		if (board->layout == LAYOUT_4020) {
 			ret = subdev_8255_init(dev, s, dio_callback_4020,
 					       I8255_4020_REG);
 		} else {
@@ -3853,7 +3903,7 @@ static int setup_subdevices(struct comedi_device *dev)
 
 	/*  8 channel dio for 60xx */
 	s = &dev->subdevices[5];
-	if (thisboard->layout == LAYOUT_60XX) {
+	if (board->layout == LAYOUT_60XX) {
 		s->type = COMEDI_SUBD_DIO;
 		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
 		s->n_chan = 8;
@@ -3870,7 +3920,7 @@ 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 = 8;
-	if (thisboard->layout == LAYOUT_4020)
+	if (board->layout == LAYOUT_4020)
 		s->maxdata = 0xfff;
 	else
 		s->maxdata = 0xff;
@@ -3887,7 +3937,7 @@ static int setup_subdevices(struct comedi_device *dev)
 
 	/*  2 channel ad8402 potentiometer */
 	s = &dev->subdevices[7];
-	if (thisboard->layout == LAYOUT_64XX) {
+	if (board->layout == LAYOUT_64XX) {
 		s->type = COMEDI_SUBD_CALIB;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
 		s->n_chan = 2;
@@ -3929,16 +3979,16 @@ static int auto_attach(struct comedi_device *dev,
 		       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct pcidas64_board *thisboard = NULL;
+	const struct pcidas64_board *board = NULL;
 	struct pcidas64_private *devpriv;
 	uint32_t local_range, local_decode;
 	int retval;
 
 	if (context < ARRAY_SIZE(pcidas64_boards))
-		thisboard = &pcidas64_boards[context];
-	if (!thisboard)
+		board = &pcidas64_boards[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = thisboard;
+	dev->board_ptr = board;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -3950,7 +4000,7 @@ static int auto_attach(struct comedi_device *dev,
 	pci_set_master(pcidev);
 
 	/* Initialize dev->board_name */
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 	devpriv->main_phys_iobase = pci_resource_start(pcidev, 2);
 	devpriv->dio_counter_phys_iobase = pci_resource_start(pcidev, 3);
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index 30c9e27d1ed9..b00a36a5cb36 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -335,18 +335,18 @@ static int cb_pcidda_auto_attach(struct comedi_device *dev,
 				 unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct cb_pcidda_board *thisboard = NULL;
+	const struct cb_pcidda_board *board = NULL;
 	struct cb_pcidda_private *devpriv;
 	struct comedi_subdevice *s;
 	int i;
 	int ret;
 
 	if (context < ARRAY_SIZE(cb_pcidda_boards))
-		thisboard = &cb_pcidda_boards[context];
-	if (!thisboard)
+		board = &cb_pcidda_boards[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = thisboard;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -366,8 +366,8 @@ static int cb_pcidda_auto_attach(struct comedi_device *dev,
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->ao_chans;
-	s->maxdata = (1 << thisboard->ao_bits) - 1;
+	s->n_chan = board->ao_chans;
+	s->maxdata = (1 << board->ao_bits) - 1;
 	s->range_table = &cb_pcidda_ranges;
 	s->insn_write = cb_pcidda_ao_insn_write;
 
@@ -384,7 +384,7 @@ static int cb_pcidda_auto_attach(struct comedi_device *dev,
 		devpriv->eeprom_data[i] = cb_pcidda_read_eeprom(dev, i);
 
 	/*  set calibrations dacs */
-	for (i = 0; i < thisboard->ao_chans; i++)
+	for (i = 0; i < board->ao_chans; i++)
 		cb_pcidda_calibrate(dev, i, devpriv->ao_range[i]);
 
 	return 0;
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index c458e5010a74..4ebf5aae5019 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -243,7 +243,7 @@ static int cb_pcimdas_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int cb_pcimdas_di_insn_read(struct comedi_device *dev,
+static int cb_pcimdas_di_insn_bits(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn,
 				   unsigned int *data)
@@ -258,7 +258,7 @@ static int cb_pcimdas_di_insn_read(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int cb_pcimdas_do_insn_write(struct comedi_device *dev,
+static int cb_pcimdas_do_insn_bits(struct comedi_device *dev,
 				    struct comedi_subdevice *s,
 				    struct comedi_insn *insn,
 				    unsigned int *data)
@@ -424,7 +424,7 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 	s->n_chan	= 4;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
-	s->insn_read	= cb_pcimdas_di_insn_read;
+	s->insn_bits	= cb_pcimdas_di_insn_bits;
 
 	/* Digital Output subdevice (main connector) */
 	s = &dev->subdevices[4];
@@ -433,7 +433,7 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 	s->n_chan	= 4;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
-	s->insn_write	= cb_pcimdas_do_insn_write;
+	s->insn_bits	= cb_pcimdas_do_insn_bits;
 
 	/* Counter subdevice (8254) */
 	s = &dev->subdevices[5];
diff --git a/drivers/staging/comedi/drivers/cb_pcimdda.c b/drivers/staging/comedi/drivers/cb_pcimdda.c
index a4781dbbdd82..19210d89f2b2 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdda.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdda.c
@@ -164,11 +164,7 @@ static int cb_pcimdda_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[1];
 	/* digital i/o subdevice */
-	ret = subdev_8255_init(dev, s, NULL, PCIMDDA_8255_BASE_REG);
-	if (ret)
-		return ret;
-
-	return 0;
+	return subdev_8255_init(dev, s, NULL, PCIMDDA_8255_BASE_REG);
 }
 
 static struct comedi_driver cb_pcimdda_driver = {
diff --git a/drivers/staging/comedi/drivers/comedi_8254.h b/drivers/staging/comedi/drivers/comedi_8254.h
index d89f6d94f8aa..f4610ead6172 100644
--- a/drivers/staging/comedi/drivers/comedi_8254.h
+++ b/drivers/staging/comedi/drivers/comedi_8254.h
@@ -20,6 +20,12 @@
 #ifndef _COMEDI_8254_H
 #define _COMEDI_8254_H
 
+#include <linux/types.h>
+
+struct comedi_device;
+struct comedi_insn;
+struct comedi_subdevice;
+
 /*
  * Common oscillator base values in nanoseconds
  */
diff --git a/drivers/staging/comedi/drivers/comedi_8255.c b/drivers/staging/comedi/drivers/comedi_8255.c
new file mode 100644
index 000000000000..b2441efc61cc
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_8255.c
@@ -0,0 +1,285 @@
+/*
+ * comedi_8255.c
+ * Generic 8255 digital I/O support
+ *
+ * Split from the Comedi "8255" driver module.
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998 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.
+ */
+
+/*
+ * Module: comedi_8255
+ * Description: Generic 8255 support
+ * Author: ds
+ * Updated: Fri, 22 May 2015 12:14:17 +0000
+ * Status: works
+ *
+ * This module is not used directly by end-users.  Rather, it is used by
+ * other drivers to provide support for an 8255 "Programmable Peripheral
+ * Interface" (PPI) chip.
+ *
+ * The classic in digital I/O.  The 8255 appears in Comedi as a single
+ * digital I/O subdevice with 24 channels.  The channel 0 corresponds to
+ * the 8255's port A, bit 0; channel 23 corresponds to port C, bit 7.
+ * Direction configuration is done in blocks, with channels 0-7, 8-15,
+ * 16-19, and 20-23 making up the 4 blocks.  The only 8255 mode
+ * supported is mode 0.
+ */
+
+#include <linux/module.h>
+#include "../comedidev.h"
+
+#include "8255.h"
+
+struct subdev_8255_private {
+	unsigned long regbase;
+	int (*io)(struct comedi_device *dev, int dir, int port, int data,
+		  unsigned long regbase);
+};
+
+static int subdev_8255_io(struct comedi_device *dev,
+			  int dir, int port, int data, unsigned long regbase)
+{
+	if (dir) {
+		outb(data, dev->iobase + regbase + port);
+		return 0;
+	}
+	return inb(dev->iobase + regbase + port);
+}
+
+static int subdev_8255_mmio(struct comedi_device *dev,
+			    int dir, int port, int data, unsigned long regbase)
+{
+	if (dir) {
+		writeb(data, dev->mmio + regbase + port);
+		return 0;
+	}
+	return readb(dev->mmio + regbase + port);
+}
+
+static int subdev_8255_insn(struct comedi_device *dev,
+			    struct comedi_subdevice *s,
+			    struct comedi_insn *insn,
+			    unsigned int *data)
+{
+	struct subdev_8255_private *spriv = s->private;
+	unsigned long regbase = spriv->regbase;
+	unsigned int mask;
+	unsigned int v;
+
+	mask = comedi_dio_update_state(s, data);
+	if (mask) {
+		if (mask & 0xff)
+			spriv->io(dev, 1, I8255_DATA_A_REG,
+				  s->state & 0xff, regbase);
+		if (mask & 0xff00)
+			spriv->io(dev, 1, I8255_DATA_B_REG,
+				  (s->state >> 8) & 0xff, regbase);
+		if (mask & 0xff0000)
+			spriv->io(dev, 1, I8255_DATA_C_REG,
+				  (s->state >> 16) & 0xff, regbase);
+	}
+
+	v = spriv->io(dev, 0, I8255_DATA_A_REG, 0, regbase);
+	v |= (spriv->io(dev, 0, I8255_DATA_B_REG, 0, regbase) << 8);
+	v |= (spriv->io(dev, 0, I8255_DATA_C_REG, 0, regbase) << 16);
+
+	data[1] = v;
+
+	return insn->n;
+}
+
+static void subdev_8255_do_config(struct comedi_device *dev,
+				  struct comedi_subdevice *s)
+{
+	struct subdev_8255_private *spriv = s->private;
+	unsigned long regbase = spriv->regbase;
+	int config;
+
+	config = I8255_CTRL_CW;
+	/* 1 in io_bits indicates output, 1 in config indicates input */
+	if (!(s->io_bits & 0x0000ff))
+		config |= I8255_CTRL_A_IO;
+	if (!(s->io_bits & 0x00ff00))
+		config |= I8255_CTRL_B_IO;
+	if (!(s->io_bits & 0x0f0000))
+		config |= I8255_CTRL_C_LO_IO;
+	if (!(s->io_bits & 0xf00000))
+		config |= I8255_CTRL_C_HI_IO;
+
+	spriv->io(dev, 1, I8255_CTRL_REG, config, regbase);
+}
+
+static int subdev_8255_insn_config(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int mask;
+	int ret;
+
+	if (chan < 8)
+		mask = 0x0000ff;
+	else if (chan < 16)
+		mask = 0x00ff00;
+	else if (chan < 20)
+		mask = 0x0f0000;
+	else
+		mask = 0xf00000;
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, mask);
+	if (ret)
+		return ret;
+
+	subdev_8255_do_config(dev, s);
+
+	return insn->n;
+}
+
+static int __subdev_8255_init(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      int (*io)(struct comedi_device *dev,
+					int dir, int port, int data,
+					unsigned long regbase),
+			      unsigned long regbase,
+			      bool is_mmio)
+{
+	struct subdev_8255_private *spriv;
+
+	spriv = comedi_alloc_spriv(s, sizeof(*spriv));
+	if (!spriv)
+		return -ENOMEM;
+
+	if (io)
+		spriv->io = io;
+	else if (is_mmio)
+		spriv->io = subdev_8255_mmio;
+	else
+		spriv->io = subdev_8255_io;
+	spriv->regbase	= regbase;
+
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 24;
+	s->range_table	= &range_digital;
+	s->maxdata	= 1;
+	s->insn_bits	= subdev_8255_insn;
+	s->insn_config	= subdev_8255_insn_config;
+
+	subdev_8255_do_config(dev, s);
+
+	return 0;
+}
+
+/**
+ * subdev_8255_init - initialize DIO subdevice for driving I/O mapped 8255
+ * @dev: comedi device owning subdevice
+ * @s: comedi subdevice to initialize
+ * @io: (optional) register I/O call-back function
+ * @regbase: offset of 8255 registers from dev->iobase, or call-back context
+ *
+ * Initializes a comedi subdevice as a DIO subdevice driving an 8255 chip.
+ *
+ * If the optional I/O call-back function is provided, its prototype is of
+ * the following form:
+ *
+ *   int my_8255_callback(struct comedi_device *dev, int dir, int port,
+ *                        int data, unsigned long regbase);
+ *
+ * where 'dev', and 'regbase' match the values passed to this function,
+ * 'port' is the 8255 port number 0 to 3 (including the control port), 'dir'
+ * is the direction (0 for read, 1 for write) and 'data' is the value to be
+ * written.  It should return 0 if writing or the value read if reading.
+ *
+ * If the optional I/O call-back function is not provided, an internal
+ * call-back function is used which uses consecutive I/O port addresses
+ * starting at dev->iobase + regbase.
+ *
+ * Return: -ENOMEM if failed to allocate memory, zero on success.
+ */
+int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
+		     int (*io)(struct comedi_device *dev, int dir, int port,
+			       int data, unsigned long regbase),
+		     unsigned long regbase)
+{
+	return __subdev_8255_init(dev, s, io, regbase, false);
+}
+EXPORT_SYMBOL_GPL(subdev_8255_init);
+
+/**
+ * subdev_8255_mm_init - initialize DIO subdevice for driving mmio-mapped 8255
+ * @dev: comedi device owning subdevice
+ * @s: comedi subdevice to initialize
+ * @io: (optional) register I/O call-back function
+ * @regbase: offset of 8255 registers from dev->mmio, or call-back context
+ *
+ * Initializes a comedi subdevice as a DIO subdevice driving an 8255 chip.
+ *
+ * If the optional I/O call-back function is provided, its prototype is of
+ * the following form:
+ *
+ *   int my_8255_callback(struct comedi_device *dev, int dir, int port,
+ *                        int data, unsigned long regbase);
+ *
+ * where 'dev', and 'regbase' match the values passed to this function,
+ * 'port' is the 8255 port number 0 to 3 (including the control port), 'dir'
+ * is the direction (0 for read, 1 for write) and 'data' is the value to be
+ * written.  It should return 0 if writing or the value read if reading.
+ *
+ * If the optional I/O call-back function is not provided, an internal
+ * call-back function is used which uses consecutive MMIO virtual addresses
+ * starting at dev->mmio + regbase.
+ *
+ * Return: -ENOMEM if failed to allocate memory, zero on success.
+ */
+int subdev_8255_mm_init(struct comedi_device *dev, struct comedi_subdevice *s,
+			int (*io)(struct comedi_device *dev, int dir, int port,
+				  int data, unsigned long regbase),
+			unsigned long regbase)
+{
+	return __subdev_8255_init(dev, s, io, regbase, true);
+}
+EXPORT_SYMBOL_GPL(subdev_8255_mm_init);
+
+/**
+ * subdev_8255_regbase - get offset of 8255 registers or call-back context
+ * @s: comedi subdevice
+ *
+ * Returns the 'regbase' parameter that was previously passed to to
+ * subdev_8255_init() or subdev_8255_mm_init() to set up the subdevice.
+ * Only valid if the subdevice was set up successfully.
+ */
+unsigned long subdev_8255_regbase(struct comedi_subdevice *s)
+{
+	struct subdev_8255_private *spriv = s->private;
+
+	return spriv->regbase;
+}
+EXPORT_SYMBOL_GPL(subdev_8255_regbase);
+
+static int __init comedi_8255_module_init(void)
+{
+	return 0;
+}
+module_init(comedi_8255_module_init);
+
+static void __exit comedi_8255_module_exit(void)
+{
+}
+module_exit(comedi_8255_module_exit);
+
+MODULE_AUTHOR("Comedi http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi: Generic 8255 digital I/O support");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c
index 96db0c2686a1..50b76eccb7d7 100644
--- a/drivers/staging/comedi/drivers/comedi_bond.c
+++ b/drivers/staging/comedi/drivers/comedi_bond.c
@@ -101,7 +101,8 @@ static int bonding_dio_insn_bits(struct comedi_device *dev,
 			b_chans = bdev->nchans - base_chan;
 			if (b_chans > n_left)
 				b_chans = n_left;
-			b_mask = (1U << b_chans) - 1;
+			b_mask = (b_chans < 32) ? ((1 << b_chans) - 1)
+						: 0xffffffff;
 			b_write_mask = (write_mask >> n_done) & b_mask;
 			b_data_bits = (data_bits >> n_done) & b_mask;
 			/* Read/Write the new digital lines. */
diff --git a/drivers/staging/comedi/drivers/comedi_isadma.h b/drivers/staging/comedi/drivers/comedi_isadma.h
index c7c524faf595..2fb6573ba9e4 100644
--- a/drivers/staging/comedi/drivers/comedi_isadma.h
+++ b/drivers/staging/comedi/drivers/comedi_isadma.h
@@ -16,6 +16,10 @@
 #ifndef _COMEDI_ISADMA_H
 #define _COMEDI_ISADMA_H
 
+#include <linux/types.h>
+
+struct comedi_device;
+
 /*
  * These are used to avoid issues when <asm/dma.h> and the DMA_MODE_
  * defines are not available.
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index f97d18d92255..611b0a3ef5d7 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -1,105 +1,105 @@
 /*
-   comedi/drivers/daqboard2000.c
-   hardware driver for IOtech DAQboard/2000
-
-   COMEDI - Linux Control and Measurement Device Interface
-   Copyright (C) 1999 Anders Blomdell <anders.blomdell@control.lth.se>
-
-   This program is free software; you can redistribute it and/or modify
-   it under 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/daqboard2000.c
+ * hardware driver for IOtech DAQboard/2000
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1999 Anders Blomdell <anders.blomdell@control.lth.se>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under 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: daqboard2000
-Description: IOTech DAQBoard/2000
-Author: Anders Blomdell <anders.blomdell@control.lth.se>
-Status: works
-Updated: Mon, 14 Apr 2008 15:28:52 +0100
-Devices: [IOTech] DAQBoard/2000 (daqboard2000)
-
-Much of the functionality of this driver was determined from reading
-the source code for the Windows driver.
-
-The FPGA on the board requires fimware, which is available from
-http://www.comedi.org in the comedi_nonfree_firmware tarball.
-
-Configuration options: not applicable, uses PCI auto config
-*/
+ * Driver: daqboard2000
+ * Description: IOTech DAQBoard/2000
+ * Author: Anders Blomdell <anders.blomdell@control.lth.se>
+ * Status: works
+ * Updated: Mon, 14 Apr 2008 15:28:52 +0100
+ * Devices: [IOTech] DAQBoard/2000 (daqboard2000)
+ *
+ * Much of the functionality of this driver was determined from reading
+ * the source code for the Windows driver.
+ *
+ * The FPGA on the board requires fimware, which is available from
+ * http://www.comedi.org in the comedi_nonfree_firmware tarball.
+ *
+ * Configuration options: not applicable, uses PCI auto config
+ */
 /*
-   This card was obviously never intended to leave the Windows world,
-   since it lacked all kind of hardware documentation (except for cable
-   pinouts, plug and pray has something to catch up with yet).
-
-   With some help from our swedish distributor, we got the Windows sourcecode
-   for the card, and here are the findings so far.
-
-   1. A good document that describes the PCI interface chip is 9080db-106.pdf
-      available from http://www.plxtech.com/products/io/pci9080 
-
-   2. The initialization done so far is:
-        a. program the FPGA (windows code sans a lot of error messages)
-	b.
-
-   3. Analog out seems to work OK with DAC's disabled, if DAC's are enabled,
-      you have to output values to all enabled DAC's until result appears, I
-      guess that it has something to do with pacer clocks, but the source
-      gives me no clues. I'll keep it simple so far.
-
-   4. Analog in.
-        Each channel in the scanlist seems to be controlled by four
-	control words:
-
-        Word0:
-          +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-          ! | | | ! | | | ! | | | ! | | | !
-          +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-
-        Word1:
-          +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-          ! | | | ! | | | ! | | | ! | | | !
-          +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-	   |             |       | | | | |
-           +------+------+       | | | | +-- Digital input (??)
-		  |		 | | | +---- 10 us settling time
-		  |		 | | +------ Suspend acquisition (last to scan)
-		  |		 | +-------- Simultaneous sample and hold
-		  |		 +---------- Signed data format
-		  +------------------------- Correction offset low
-
-        Word2:
-          +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-          ! | | | ! | | | ! | | | ! | | | !
-          +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-           |     | |     | | | | | |     |
-           +-----+ +--+--+ +++ +++ +--+--+
-              |       |     |   |     +----- Expansion channel
-	      |       |     |   +----------- Expansion gain
-              |       |     +--------------- Channel (low)
-	      |       +--------------------- Correction offset high
-	      +----------------------------- Correction gain low
-        Word3:
-          +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-          ! | | | ! | | | ! | | | ! | | | !
-          +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
-           |             | | | |   | | | |
-           +------+------+ | | +-+-+ | | +-- Low bank enable
-                  |        | |   |   | +---- High bank enable
-                  |        | |   |   +------ Hi/low select
-		  |    	   | |   +---------- Gain (1,?,2,4,8,16,32,64)
-		  |    	   | +-------------- differential/single ended
-		  |    	   +---------------- Unipolar
-		  +------------------------- Correction gain high
-
-   999. The card seems to have an incredible amount of capabilities, but
-        trying to reverse engineer them from the Windows source is beyond my
-	patience.
-
+ * This card was obviously never intended to leave the Windows world,
+ * since it lacked all kind of hardware documentation (except for cable
+ * pinouts, plug and pray has something to catch up with yet).
+ *
+ * With some help from our swedish distributor, we got the Windows sourcecode
+ * for the card, and here are the findings so far.
+ *
+ * 1. A good document that describes the PCI interface chip is 9080db-106.pdf
+ *    available from http://www.plxtech.com/products/io/pci9080
+ *
+ * 2. The initialization done so far is:
+ *      a. program the FPGA (windows code sans a lot of error messages)
+ *      b.
+ *
+ * 3. Analog out seems to work OK with DAC's disabled, if DAC's are enabled,
+ *    you have to output values to all enabled DAC's until result appears, I
+ *    guess that it has something to do with pacer clocks, but the source
+ *    gives me no clues. I'll keep it simple so far.
+ *
+ * 4. Analog in.
+ *    Each channel in the scanlist seems to be controlled by four
+ *    control words:
+ *
+ *	Word0:
+ *	  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *	  ! | | | ! | | | ! | | | ! | | | !
+ *	  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ *	Word1:
+ *	  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *	  ! | | | ! | | | ! | | | ! | | | !
+ *	  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *	   |             |       | | | | |
+ *	   +------+------+       | | | | +-- Digital input (??)
+ *		  |		 | | | +---- 10 us settling time
+ *		  |		 | | +------ Suspend acquisition (last to scan)
+ *		  |		 | +-------- Simultaneous sample and hold
+ *		  |		 +---------- Signed data format
+ *		  +------------------------- Correction offset low
+ *
+ *	Word2:
+ *	  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *	  ! | | | ! | | | ! | | | ! | | | !
+ *	  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *	   |     | |     | | | | | |     |
+ *	   +-----+ +--+--+ +++ +++ +--+--+
+ *	      |       |     |   |     +----- Expansion channel
+ *	      |       |     |   +----------- Expansion gain
+ *	      |       |     +--------------- Channel (low)
+ *	      |       +--------------------- Correction offset high
+ *	      +----------------------------- Correction gain low
+ *	Word3:
+ *	  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *	  ! | | | ! | | | ! | | | ! | | | !
+ *	  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *	   |             | | | |   | | | |
+ *	   +------+------+ | | +-+-+ | | +-- Low bank enable
+ *		  |	   | |   |   | +---- High bank enable
+ *		  |	   | |   |   +------ Hi/low select
+ *		  |	   | |   +---------- Gain (1,?,2,4,8,16,32,64)
+ *		  |	   | +-------------- differential/single ended
+ *		  |	   +---------------- Unipolar
+ *		  +------------------------- Correction gain high
+ *
+ * 999. The card seems to have an incredible amount of capabilities, but
+ *      trying to reverse engineer them from the Windows source is beyond my
+ *      patience.
+ *
  */
 
 #include <linux/module.h>
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index 73f4c8dbbde3..3d8fc6ad44df 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -1,21 +1,21 @@
 /*
- *  comedi/drivers/das08.c
- *  comedi module for common DAS08 support (used by ISA/PCI/PCMCIA drivers)
+ * comedi/drivers/das08.c
+ * comedi module for common DAS08 support (used by ISA/PCI/PCMCIA drivers)
  *
- *  COMEDI - Linux Control and Measurement Device Interface
- *  Copyright (C) 2000 David A. Schleef <ds@schleef.org>
- *  Copyright (C) 2001,2002,2003 Frank Mori Hess <fmhess@users.sourceforge.net>
- *  Copyright (C) 2004 Salvador E. Tropea <set@users.sf.net> <set@ieee.org>
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+ * Copyright (C) 2001,2002,2003 Frank Mori Hess <fmhess@users.sourceforge.net>
+ * Copyright (C) 2004 Salvador E. Tropea <set@users.sf.net> <set@ieee.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 free software; you can redistribute it and/or modify
+ * it under 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.
  */
 
 #include <linux/module.h>
@@ -27,85 +27,74 @@
 #include "das08.h"
 
 /*
-    cio-das08.pdf
-
-  "isa-das08"
-
-  0	a/d bits 0-3		start 8 bit
-  1	a/d bits 4-11		start 12 bit
-  2	eoc, ip1-3, irq, mux	op1-4, inte, mux
-  3	unused			unused
-  4567	8254
-  89ab	8255
-
-  requires hard-wiring for async ai
-
-*/
-
-#define DAS08_LSB		0
-#define DAS08_MSB		1
-#define DAS08_TRIG_12BIT	1
-#define DAS08_STATUS		2
-#define   DAS08_EOC			(1<<7)
-#define   DAS08_IRQ			(1<<3)
-#define   DAS08_IP(x)			(((x)>>4)&0x7)
-#define DAS08_CONTROL		2
-#define   DAS08_MUX_MASK	0x7
-#define   DAS08_MUX(x)		((x) & DAS08_MUX_MASK)
-#define   DAS08_INTE			(1<<3)
-#define   DAS08_DO_MASK		0xf0
-#define   DAS08_OP(x)		(((x) << 4) & DAS08_DO_MASK)
-
+ * Data format of DAS08_AI_LSB_REG and DAS08_AI_MSB_REG depends on
+ * 'ai_encoding' member of board structure:
+ *
+ * das08_encode12     : DATA[11..4] = MSB[7..0], DATA[3..0] = LSB[7..4].
+ * das08_pcm_encode12 : DATA[11..8] = MSB[3..0], DATA[7..9] = LSB[7..0].
+ * das08_encode16     : SIGN = MSB[7], MAGNITUDE[14..8] = MSB[6..0],
+ *                      MAGNITUDE[7..0] = LSB[7..0].
+ *                      SIGN==0 for negative input, SIGN==1 for positive input.
+ *                      Note: when read a second time after conversion
+ *                            complete, MSB[7] is an "over-range" bit.
+ */
+#define DAS08_AI_LSB_REG	0x00	/* (R) AI least significant bits */
+#define DAS08_AI_MSB_REG	0x01	/* (R) AI most significant bits */
+#define DAS08_AI_TRIG_REG	0x01	/* (W) AI software trigger */
+#define DAS08_STATUS_REG	0x02	/* (R) status */
+#define DAS08_STATUS_AI_BUSY	BIT(7)	/* AI conversion in progress */
 /*
-    cio-das08jr.pdf
-
-  "das08/jr-ao"
-
-  0	a/d bits 0-3		unused
-  1	a/d bits 4-11		start 12 bit
-  2	eoc, mux		mux
-  3	di			do
-  4	unused			ao0_lsb
-  5	unused			ao0_msb
-  6	unused			ao1_lsb
-  7	unused			ao1_msb
-
-*/
-
-#define DAS08JR_DIO		3
-#define DAS08JR_AO_LSB(x)	((x) ? 6 : 4)
-#define DAS08JR_AO_MSB(x)	((x) ? 7 : 5)
+ * The IRQ status bit is set to 1 by a rising edge on the external interrupt
+ * input (which may be jumpered to the pacer output).  It is cleared by
+ * setting the INTE control bit to 0.  Not present on "JR" boards.
+ */
+#define DAS08_STATUS_IRQ	BIT(3)	/* latched interrupt input */
+/* digital inputs (not "JR" boards) */
+#define DAS08_STATUS_DI(x)	(((x) & 0x70) >> 4)
+#define DAS08_CONTROL_REG	0x02	/* (W) control */
+/*
+ * Note: The AI multiplexor channel can also be read from status register using
+ * the same mask.
+ */
+#define DAS08_CONTROL_MUX_MASK	0x7	/* multiplexor channel mask */
+#define DAS08_CONTROL_MUX(x)	((x) & DAS08_CONTROL_MUX_MASK) /* mux channel */
+#define DAS08_CONTROL_INTE	BIT(3)	/* interrupt enable (not "JR" boards) */
+#define DAS08_CONTROL_DO_MASK	0xf0	/* digital outputs mask (not "JR") */
+/* digital outputs (not "JR" boards) */
+#define DAS08_CONTROL_DO(x)	(((x) << 4) & DAS08_CONTROL_DO_MASK)
+/*
+ * (R/W) programmable AI gain ("PGx" and "AOx" boards):
+ * + bits 3..0 (R/W) show/set the gain for the current AI mux channel
+ * + bits 6..4 (R) show the current AI mux channel
+ * + bit 7 (R) not unused
+ */
+#define DAS08_GAIN_REG		0x03
+
+#define DAS08JR_DI_REG		0x03	/* (R) digital inputs ("JR" boards) */
+#define DAS08JR_DO_REG		0x03	/* (W) digital outputs ("JR" boards) */
+/* (W) analog output l.s.b. registers for 2 channels ("JR" boards) */
+#define DAS08JR_AO_LSB_REG(x)	((x) ? 0x06 : 0x04)
+/* (W) analog output m.s.b. registers for 2 channels ("JR" boards) */
+#define DAS08JR_AO_MSB_REG(x)	((x) ? 0x07 : 0x05)
+/*
+ * (R) update analog outputs ("JR" boards set for simultaneous output)
+ *     (same register as digital inputs)
+ */
+#define DAS08JR_AO_UPDATE_REG	0x03
 
+/* (W) analog output l.s.b. registers for 2 channels ("AOx" boards) */
+#define DAS08AOX_AO_LSB_REG(x)	((x) ? 0x0a : 0x08)
+/* (W) analog output m.s.b. registers for 2 channels ("AOx" boards) */
+#define DAS08AOX_AO_MSB_REG(x)	((x) ? 0x0b : 0x09)
 /*
-    cio-das08_aox.pdf
-
-  "das08-aoh"
-  "das08-aol"
-  "das08-aom"
-
-  0	a/d bits 0-3		start 8 bit
-  1	a/d bits 4-11		start 12 bit
-  2	eoc, ip1-3, irq, mux	op1-4, inte, mux
-  3	mux, gain status	gain control
-  4567	8254
-  8	unused			ao0_lsb
-  9	unused			ao0_msb
-  a	unused			ao1_lsb
-  b	unused			ao1_msb
-  89ab
-  cdef	8255
-*/
-
-#define DAS08AO_GAIN_CONTROL	3
-#define DAS08AO_GAIN_STATUS	3
-
-#define DAS08AO_AO_LSB(x)	((x) ? 0xa : 8)
-#define DAS08AO_AO_MSB(x)	((x) ? 0xb : 9)
-#define DAS08AO_AO_UPDATE	8
+ * (R) update analog outputs ("AOx" boards set for simultaneous output)
+ *     (any of the analog output registers could be used for this)
+ */
+#define DAS08AOX_AO_UPDATE_REG	0x08
 
 /* gainlist same as _pgx_ below */
 
-static const struct comedi_lrange range_das08_pgl = {
+static const struct comedi_lrange das08_pgl_ai_range = {
 	9, {
 		BIP_RANGE(10),
 		BIP_RANGE(5),
@@ -119,7 +108,7 @@ static const struct comedi_lrange range_das08_pgl = {
 	}
 };
 
-static const struct comedi_lrange range_das08_pgh = {
+static const struct comedi_lrange das08_pgh_ai_range = {
 	12, {
 		BIP_RANGE(10),
 		BIP_RANGE(5),
@@ -136,7 +125,7 @@ static const struct comedi_lrange range_das08_pgh = {
 	}
 };
 
-static const struct comedi_lrange range_das08_pgm = {
+static const struct comedi_lrange das08_pgm_ai_range = {
 	9, {
 		BIP_RANGE(10),
 		BIP_RANGE(5),
@@ -148,42 +137,28 @@ static const struct comedi_lrange range_das08_pgm = {
 		UNI_RANGE(0.1),
 		UNI_RANGE(0.01)
 	}
-};				/*
-				   cio-das08jr.pdf
-
-				   "das08/jr-ao"
-
-				   0 a/d bits 0-3            unused
-				   1 a/d bits 4-11           start 12 bit
-				   2 eoc, mux                mux
-				   3 di                      do
-				   4 unused                  ao0_lsb
-				   5 unused                  ao0_msb
-				   6 unused                  ao1_lsb
-				   7 unused                  ao1_msb
-
-				 */
+};
 
 static const struct comedi_lrange *const das08_ai_lranges[] = {
-	&range_unknown,
-	&range_bipolar5,
-	&range_das08_pgh,
-	&range_das08_pgl,
-	&range_das08_pgm,
+	[das08_pg_none]		= &range_unknown,
+	[das08_bipolar5]	= &range_bipolar5,
+	[das08_pgh]		= &das08_pgh_ai_range,
+	[das08_pgl]		= &das08_pgl_ai_range,
+	[das08_pgm]		= &das08_pgm_ai_range,
 };
 
-static const int das08_pgh_gainlist[] = {
+static const int das08_pgh_ai_gainlist[] = {
 	8, 0, 10, 2, 12, 4, 14, 6, 1, 3, 5, 7
 };
-static const int das08_pgl_gainlist[] = { 8, 0, 2, 4, 6, 1, 3, 5, 7 };
-static const int das08_pgm_gainlist[] = { 8, 0, 10, 12, 14, 9, 11, 13, 15 };
-
-static const int *const das08_gainlists[] = {
-	NULL,
-	NULL,
-	das08_pgh_gainlist,
-	das08_pgl_gainlist,
-	das08_pgm_gainlist,
+static const int das08_pgl_ai_gainlist[] = { 8, 0, 2, 4, 6, 1, 3, 5, 7 };
+static const int das08_pgm_ai_gainlist[] = { 8, 0, 10, 12, 14, 9, 11, 13, 15 };
+
+static const int *const das08_ai_gainlists[] = {
+	[das08_pg_none]		= NULL,
+	[das08_bipolar5]	= NULL,
+	[das08_pgh]		= das08_pgh_ai_gainlist,
+	[das08_pgl]		= das08_pgl_ai_gainlist,
+	[das08_pgm]		= das08_pgm_ai_gainlist,
 };
 
 static int das08_ai_eoc(struct comedi_device *dev,
@@ -193,16 +168,17 @@ static int das08_ai_eoc(struct comedi_device *dev,
 {
 	unsigned int status;
 
-	status = inb(dev->iobase + DAS08_STATUS);
-	if ((status & DAS08_EOC) == 0)
+	status = inb(dev->iobase + DAS08_STATUS_REG);
+	if ((status & DAS08_STATUS_AI_BUSY) == 0)
 		return 0;
 	return -EBUSY;
 }
 
-static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
+static int das08_ai_insn_read(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das08_board_struct *thisboard = dev->board_ptr;
+	const struct das08_board_struct *board = dev->board_ptr;
 	struct das08_private_struct *devpriv = dev->private;
 	int n;
 	int chan;
@@ -214,49 +190,64 @@ static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	range = CR_RANGE(insn->chanspec);
 
 	/* clear crap */
-	inb(dev->iobase + DAS08_LSB);
-	inb(dev->iobase + DAS08_MSB);
+	inb(dev->iobase + DAS08_AI_LSB_REG);
+	inb(dev->iobase + DAS08_AI_MSB_REG);
 
 	/* set multiplexer */
-	/*  lock to prevent race with digital output */
+	/* lock to prevent race with digital output */
 	spin_lock(&dev->spinlock);
-	devpriv->do_mux_bits &= ~DAS08_MUX_MASK;
-	devpriv->do_mux_bits |= DAS08_MUX(chan);
-	outb(devpriv->do_mux_bits, dev->iobase + DAS08_CONTROL);
+	devpriv->do_mux_bits &= ~DAS08_CONTROL_MUX_MASK;
+	devpriv->do_mux_bits |= DAS08_CONTROL_MUX(chan);
+	outb(devpriv->do_mux_bits, dev->iobase + DAS08_CONTROL_REG);
 	spin_unlock(&dev->spinlock);
 
-	if (s->range_table->length > 1) {
+	if (devpriv->pg_gainlist) {
 		/* set gain/range */
 		range = CR_RANGE(insn->chanspec);
 		outb(devpriv->pg_gainlist[range],
-		     dev->iobase + DAS08AO_GAIN_CONTROL);
+		     dev->iobase + DAS08_GAIN_REG);
 	}
 
 	for (n = 0; n < insn->n; n++) {
 		/* clear over-range bits for 16-bit boards */
-		if (thisboard->ai_nbits == 16)
-			if (inb(dev->iobase + DAS08_MSB) & 0x80)
+		if (board->ai_nbits == 16)
+			if (inb(dev->iobase + DAS08_AI_MSB_REG) & 0x80)
 				dev_info(dev->class_dev, "over-range\n");
 
 		/* trigger conversion */
-		outb_p(0, dev->iobase + DAS08_TRIG_12BIT);
+		outb_p(0, dev->iobase + DAS08_AI_TRIG_REG);
 
 		ret = comedi_timeout(dev, s, insn, das08_ai_eoc, 0);
 		if (ret)
 			return ret;
 
-		msb = inb(dev->iobase + DAS08_MSB);
-		lsb = inb(dev->iobase + DAS08_LSB);
-		if (thisboard->ai_encoding == das08_encode12) {
+		msb = inb(dev->iobase + DAS08_AI_MSB_REG);
+		lsb = inb(dev->iobase + DAS08_AI_LSB_REG);
+		if (board->ai_encoding == das08_encode12) {
 			data[n] = (lsb >> 4) | (msb << 4);
-		} else if (thisboard->ai_encoding == das08_pcm_encode12) {
+		} else if (board->ai_encoding == das08_pcm_encode12) {
 			data[n] = (msb << 8) + lsb;
-		} else if (thisboard->ai_encoding == das08_encode16) {
-			/* FPOS 16-bit boards are sign-magnitude */
+		} else if (board->ai_encoding == das08_encode16) {
+			/*
+			 * "JR" 16-bit boards are sign-magnitude.
+			 *
+			 * XXX The manual seems to imply that 0 is full-scale
+			 * negative and 65535 is full-scale positive, but the
+			 * original COMEDI patch to add support for the
+			 * DAS08/JR/16 and DAS08/JR/16-AO boards have it
+			 * encoded as sign-magnitude.  Assume the original
+			 * COMEDI code is correct for now.
+			 */
+			unsigned int magnitude = lsb | ((msb & 0x7f) << 8);
+
+			/*
+			 * MSB bit 7 is 0 for negative, 1 for positive voltage.
+			 * COMEDI 16-bit bipolar data value for 0V is 0x8000.
+			 */
 			if (msb & 0x80)
-				data[n] = (1 << 15) | lsb | ((msb & 0x7f) << 8);
+				data[n] = (1 << 15) + magnitude;
 			else
-				data[n] = (1 << 15) - (lsb | (msb & 0x7f) << 8);
+				data[n] = (1 << 15) - magnitude;
 		} else {
 			dev_err(dev->class_dev, "bug! unknown ai encoding\n");
 			return -1;
@@ -266,28 +257,28 @@ static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return n;
 }
 
-static int das08_di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
+static int das08_di_insn_bits(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn, unsigned int *data)
 {
 	data[0] = 0;
-	data[1] = DAS08_IP(inb(dev->iobase + DAS08_STATUS));
+	data[1] = DAS08_STATUS_DI(inb(dev->iobase + DAS08_STATUS_REG));
 
 	return insn->n;
 }
 
-static int das08_do_wbits(struct comedi_device *dev,
-			  struct comedi_subdevice *s,
-			  struct comedi_insn *insn,
-			  unsigned int *data)
+static int das08_do_insn_bits(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn, unsigned int *data)
 {
 	struct das08_private_struct *devpriv = dev->private;
 
 	if (comedi_dio_update_state(s, data)) {
 		/* prevent race with setting of analog input mux */
 		spin_lock(&dev->spinlock);
-		devpriv->do_mux_bits &= ~DAS08_DO_MASK;
-		devpriv->do_mux_bits |= DAS08_OP(s->state);
-		outb(devpriv->do_mux_bits, dev->iobase + DAS08_CONTROL);
+		devpriv->do_mux_bits &= ~DAS08_CONTROL_DO_MASK;
+		devpriv->do_mux_bits |= DAS08_CONTROL_DO(s->state);
+		outb(devpriv->do_mux_bits, dev->iobase + DAS08_CONTROL_REG);
 		spin_unlock(&dev->spinlock);
 	}
 
@@ -296,23 +287,22 @@ static int das08_do_wbits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int das08jr_di_rbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int das08jr_di_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn, unsigned int *data)
 {
 	data[0] = 0;
-	data[1] = inb(dev->iobase + DAS08JR_DIO);
+	data[1] = inb(dev->iobase + DAS08JR_DI_REG);
 
 	return insn->n;
 }
 
-static int das08jr_do_wbits(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn,
-			    unsigned int *data)
+static int das08jr_do_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn, unsigned int *data)
 {
 	if (comedi_dio_update_state(s, data))
-		outb(s->state, dev->iobase + DAS08JR_DIO);
+		outb(s->state, dev->iobase + DAS08JR_DO_REG);
 
 	data[1] = s->state;
 
@@ -322,22 +312,22 @@ static int das08jr_do_wbits(struct comedi_device *dev,
 static void das08_ao_set_data(struct comedi_device *dev,
 			      unsigned int chan, unsigned int data)
 {
-	const struct das08_board_struct *thisboard = dev->board_ptr;
+	const struct das08_board_struct *board = dev->board_ptr;
 	unsigned char lsb;
 	unsigned char msb;
 
 	lsb = data & 0xff;
 	msb = (data >> 8) & 0xff;
-	if (thisboard->is_jr) {
-		outb(lsb, dev->iobase + DAS08JR_AO_LSB(chan));
-		outb(msb, dev->iobase + DAS08JR_AO_MSB(chan));
+	if (board->is_jr) {
+		outb(lsb, dev->iobase + DAS08JR_AO_LSB_REG(chan));
+		outb(msb, dev->iobase + DAS08JR_AO_MSB_REG(chan));
 		/* load DACs */
-		inb(dev->iobase + DAS08JR_DIO);
+		inb(dev->iobase + DAS08JR_AO_UPDATE_REG);
 	} else {
-		outb(lsb, dev->iobase + DAS08AO_AO_LSB(chan));
-		outb(msb, dev->iobase + DAS08AO_AO_MSB(chan));
+		outb(lsb, dev->iobase + DAS08AOX_AO_LSB_REG(chan));
+		outb(msb, dev->iobase + DAS08AOX_AO_MSB_REG(chan));
 		/* load DACs */
-		inb(dev->iobase + DAS08AO_AO_UPDATE);
+		inb(dev->iobase + DAS08AOX_AO_UPDATE_REG);
 	}
 }
 
@@ -361,7 +351,7 @@ static int das08_ao_insn_write(struct comedi_device *dev,
 
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 {
-	const struct das08_board_struct *thisboard = dev->board_ptr;
+	const struct das08_board_struct *board = dev->board_ptr;
 	struct das08_private_struct *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	int ret;
@@ -369,7 +359,7 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 
 	dev->iobase = iobase;
 
-	dev->board_name = thisboard->name;
+	dev->board_name = board->name;
 
 	ret = comedi_alloc_subdevices(dev, 6);
 	if (ret)
@@ -377,30 +367,31 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 
 	s = &dev->subdevices[0];
 	/* ai */
-	if (thisboard->ai_nbits) {
+	if (board->ai_nbits) {
 		s->type = COMEDI_SUBD_AI;
-		/* XXX some boards actually have differential
+		/*
+		 * XXX some boards actually have differential
 		 * inputs instead of single ended.
 		 * The driver does nothing with arefs though,
 		 * so it's no big deal.
 		 */
 		s->subdev_flags = SDF_READABLE | SDF_GROUND;
 		s->n_chan = 8;
-		s->maxdata = (1 << thisboard->ai_nbits) - 1;
-		s->range_table = das08_ai_lranges[thisboard->ai_pg];
-		s->insn_read = das08_ai_rinsn;
-		devpriv->pg_gainlist = das08_gainlists[thisboard->ai_pg];
+		s->maxdata = (1 << board->ai_nbits) - 1;
+		s->range_table = das08_ai_lranges[board->ai_pg];
+		s->insn_read = das08_ai_insn_read;
+		devpriv->pg_gainlist = das08_ai_gainlists[board->ai_pg];
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
 	s = &dev->subdevices[1];
 	/* ao */
-	if (thisboard->ao_nbits) {
+	if (board->ao_nbits) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE;
 		s->n_chan = 2;
-		s->maxdata = (1 << thisboard->ao_nbits) - 1;
+		s->maxdata = (1 << board->ao_nbits) - 1;
 		s->range_table = &range_bipolar5;
 		s->insn_write = das08_ao_insn_write;
 
@@ -419,36 +410,36 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 
 	s = &dev->subdevices[2];
 	/* di */
-	if (thisboard->di_nchan) {
+	if (board->di_nchan) {
 		s->type = COMEDI_SUBD_DI;
 		s->subdev_flags = SDF_READABLE;
-		s->n_chan = thisboard->di_nchan;
+		s->n_chan = board->di_nchan;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
-		s->insn_bits =
-			thisboard->is_jr ? das08jr_di_rbits : das08_di_rbits;
+		s->insn_bits = board->is_jr ? das08jr_di_insn_bits :
+			       das08_di_insn_bits;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
 	s = &dev->subdevices[3];
 	/* do */
-	if (thisboard->do_nchan) {
+	if (board->do_nchan) {
 		s->type = COMEDI_SUBD_DO;
 		s->subdev_flags = SDF_WRITABLE;
-		s->n_chan = thisboard->do_nchan;
+		s->n_chan = board->do_nchan;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
-		s->insn_bits =
-			thisboard->is_jr ? das08jr_do_wbits : das08_do_wbits;
+		s->insn_bits = board->is_jr ? das08jr_do_insn_bits :
+			       das08_do_insn_bits;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
 	s = &dev->subdevices[4];
 	/* 8255 */
-	if (thisboard->i8255_offset != 0) {
-		ret = subdev_8255_init(dev, s, NULL, thisboard->i8255_offset);
+	if (board->i8255_offset != 0) {
+		ret = subdev_8255_init(dev, s, NULL, board->i8255_offset);
 		if (ret)
 			return ret;
 	} else {
@@ -457,9 +448,8 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 
 	/* Counter subdevice (8254) */
 	s = &dev->subdevices[5];
-	if (thisboard->i8254_offset) {
-		dev->pacer = comedi_8254_init(dev->iobase +
-					      thisboard->i8254_offset,
+	if (board->i8254_offset) {
+		dev->pacer = comedi_8254_init(dev->iobase + board->i8254_offset,
 					      0, I8254_IO8, 0);
 		if (!dev->pacer)
 			return -ENOMEM;
@@ -485,5 +475,5 @@ static void __exit das08_exit(void)
 module_exit(das08_exit);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi common DAS08 support module");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/das08.h b/drivers/staging/comedi/drivers/das08.h
index f86167da5895..d27044cb7158 100644
--- a/drivers/staging/comedi/drivers/das08.h
+++ b/drivers/staging/comedi/drivers/das08.h
@@ -1,28 +1,33 @@
 /*
-    das08.h
-
-    Header for das08.c and das08_cs.c
-
-    Copyright (C) 2003 Frank Mori Hess <fmhess@users.sourceforge.net>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-*/
+ * das08.h
+ *
+ * Header for common DAS08 support (used by ISA/PCI/PCMCIA drivers)
+ *
+ * Copyright (C) 2003 Frank Mori Hess <fmhess@users.sourceforge.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
 
 #ifndef _DAS08_H
 #define _DAS08_H
 
+#include <linux/types.h>
+
+struct comedi_device;
+
 /* different ways ai data is encoded in first two registers */
 enum das08_ai_encoding { das08_encode12, das08_encode16, das08_pcm_encode12 };
-enum das08_lrange { das08_pg_none, das08_bipolar5, das08_pgh, das08_pgl,
-	das08_pgm
+/* types of ai range table used by different boards */
+enum das08_lrange {
+	das08_pg_none, das08_bipolar5, das08_pgh, das08_pgl, das08_pgm
 };
 
 struct das08_board_struct {
@@ -36,13 +41,12 @@ struct das08_board_struct {
 	unsigned int do_nchan;
 	unsigned int i8255_offset;
 	unsigned int i8254_offset;
-	unsigned int iosize;	/*  number of ioports used */
+	unsigned int iosize;	/* number of ioports used */
 };
 
 struct das08_private_struct {
-	unsigned int do_mux_bits;	/*  bits for do/mux register on boards
-					 *  without separate do register
-					 */
+	/* bits for do/mux register on boards without separate do register */
+	unsigned int do_mux_bits;
 	const unsigned int *pg_gainlist;
 };
 
diff --git a/drivers/staging/comedi/drivers/das08_isa.c b/drivers/staging/comedi/drivers/das08_isa.c
index 2d9a31dab552..cdefc99b6db3 100644
--- a/drivers/staging/comedi/drivers/das08_isa.c
+++ b/drivers/staging/comedi/drivers/das08_isa.c
@@ -168,7 +168,7 @@ static const struct das08_board_struct das08_isa_boards[] = {
 static int das08_isa_attach(struct comedi_device *dev,
 			    struct comedi_devconfig *it)
 {
-	const struct das08_board_struct *thisboard = dev->board_ptr;
+	const struct das08_board_struct *board = dev->board_ptr;
 	struct das08_private_struct *devpriv;
 	int ret;
 
@@ -176,7 +176,7 @@ static int das08_isa_attach(struct comedi_device *dev,
 	if (!devpriv)
 		return -ENOMEM;
 
-	ret = comedi_request_region(dev, it->options[0], thisboard->iosize);
+	ret = comedi_request_region(dev, it->options[0], board->iosize);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index 1adf6a71a9f3..a18a8878bdb8 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -46,8 +46,8 @@ list has 2 or more channels in it, then two conditions must be satisfied:
 (2) - the list must have an even number of entries.
 
 Options:
-        [0] - base io address
-        [1] - irq (optional, but you probably want it)
+	[0] - base io address
+	[1] - irq (optional, but you probably want it)
 
 irq can be omitted, although the cmd interface will not work without it.
 */
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 53baf37cd21a..bfa42620a3f6 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -444,9 +444,9 @@ static const struct comedi_lrange range_ao_2 = {
 static inline uint16_t munge_bipolar_sample(const struct comedi_device *dev,
 					    uint16_t sample)
 {
-	const struct das1800_board *thisboard = dev->board_ptr;
+	const struct das1800_board *board = dev->board_ptr;
 
-	sample += 1 << (thisboard->resolution - 1);
+	sample += 1 << (board->resolution - 1);
 	return sample;
 }
 
@@ -725,7 +725,7 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_cmd *cmd)
 {
-	const struct das1800_board *thisboard = dev->board_ptr;
+	const struct das1800_board *board = dev->board_ptr;
 	int err = 0;
 	unsigned int arg;
 
@@ -765,7 +765,7 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
-						    thisboard->ai_speed);
+						    board->ai_speed);
 	}
 
 	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
@@ -1048,7 +1048,7 @@ static int das1800_ai_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das1800_board *thisboard = dev->board_ptr;
+	const struct das1800_board *board = dev->board_ptr;
 	int i, n;
 	int chan, range, aref, chan_range;
 	int timeout = 1000;
@@ -1098,7 +1098,7 @@ static int das1800_ai_rinsn(struct comedi_device *dev,
 		dpnt = inw(dev->iobase + DAS1800_FIFO);
 		/* shift data to offset binary for bipolar ranges */
 		if ((conv_flags & UB) == 0)
-			dpnt += 1 << (thisboard->resolution - 1);
+			dpnt += 1 << (board->resolution - 1);
 		data[n] = dpnt;
 	}
 exit:
@@ -1112,16 +1112,16 @@ static int das1800_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das1800_board *thisboard = dev->board_ptr;
+	const struct das1800_board *board = dev->board_ptr;
 	struct das1800_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 /* int range = CR_RANGE(insn->chanspec); */
-	int update_chan = thisboard->ao_n_chan - 1;
+	int update_chan = board->ao_n_chan - 1;
 	unsigned short output;
 	unsigned long irq_flags;
 
 	/*   card expects two's complement data */
-	output = data[0] - (1 << (thisboard->resolution - 1));
+	output = data[0] - (1 << (board->resolution - 1));
 	/*  if the write is to the 'update' channel, we need to remember its value */
 	if (chan == update_chan)
 		devpriv->ao_update_bits = output;
@@ -1216,72 +1216,76 @@ static void das1800_free_dma(struct comedi_device *dev)
 		comedi_isadma_free(devpriv->dma);
 }
 
-static int das1800_probe(struct comedi_device *dev)
+static const struct das1800_board *das1800_probe(struct comedi_device *dev)
 {
 	const struct das1800_board *board = dev->board_ptr;
-	int index;
+	int index = board ? board - das1800_boards : -EINVAL;
 	int id;
 
-	/* calc the offset to the boardinfo that was found by the core */
-	index = board - das1800_boards;
-
-	/* verify that the board id matches the boardinfo */
+	/*
+	 * The dev->board_ptr will be set by comedi_device_attach() if the
+	 * board name provided by the user matches a board->name in this
+	 * driver. If so, this function sanity checks the id to verify that
+	 * the board is correct.
+	 *
+	 * If the dev->board_ptr is not set, the user is trying to attach
+	 * an unspecified board to this driver. In this case the id is used
+	 * to 'probe' for the correct dev->board_ptr.
+	 */
 	id = (inb(dev->iobase + DAS1800_DIGITAL) >> 4) & 0xf;
 	switch (id) {
 	case 0x3:
 		if (index == das1801st_da || index == das1802st_da ||
 		    index == das1701st_da || index == das1702st_da)
-			return index;
+			return board;
 		index = das1801st;
 		break;
 	case 0x4:
 		if (index == das1802hr_da || index == das1702hr_da)
-			return index;
+			return board;
 		index = das1802hr;
 		break;
 	case 0x5:
 		if (index == das1801ao || index == das1802ao ||
 		    index == das1701ao || index == das1702ao)
-			return index;
+			return board;
 		index = das1801ao;
 		break;
 	case 0x6:
 		if (index == das1802hr || index == das1702hr)
-			return index;
+			return board;
 		index = das1802hr;
 		break;
 	case 0x7:
 		if (index == das1801st || index == das1802st ||
 		    index == das1701st || index == das1702st)
-			return index;
+			return board;
 		index = das1801st;
 		break;
 	case 0x8:
 		if (index == das1801hc || index == das1802hc)
-			return index;
+			return board;
 		index = das1801hc;
-		break;
 	default:
 		dev_err(dev->class_dev,
 			"Board model: probe returned 0x%x (unknown, please report)\n",
 			id);
-		break;
+		return NULL;
 	}
 	dev_err(dev->class_dev,
 		"Board model (probed, not recommended): %s series\n",
 		das1800_boards[index].name);
 
-	return index;
+	return &das1800_boards[index];
 }
 
 static int das1800_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
-	const struct das1800_board *thisboard;
+	const struct das1800_board *board;
 	struct das1800_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq = it->options[1];
-	int board;
 	int ret;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
@@ -1293,17 +1297,15 @@ static int das1800_attach(struct comedi_device *dev,
 		return ret;
 
 	board = das1800_probe(dev);
-	if (board < 0) {
+	if (!board) {
 		dev_err(dev->class_dev, "unable to determine board type\n");
 		return -ENODEV;
 	}
-
-	dev->board_ptr = das1800_boards + board;
-	thisboard = dev->board_ptr;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	/*  if it is an 'ao' board with fancy analog out then we need extra io ports */
-	if (thisboard->ao_ability == 2) {
+	if (board->ao_ability == 2) {
 		unsigned long iobase2 = dev->iobase + IOBASE2;
 
 		ret = __comedi_request_region(dev, iobase2, DAS1800_SIZE);
@@ -1363,11 +1365,11 @@ static int das1800_attach(struct comedi_device *dev,
 	s = &dev->subdevices[0];
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND;
-	if (thisboard->common)
+	if (board->common)
 		s->subdev_flags |= SDF_COMMON;
-	s->n_chan = thisboard->qram_len;
-	s->maxdata = (1 << thisboard->resolution) - 1;
-	s->range_table = thisboard->range_ai;
+	s->n_chan = board->qram_len;
+	s->maxdata = (1 << board->resolution) - 1;
+	s->range_table = board->range_ai;
 	s->insn_read = das1800_ai_rinsn;
 	if (dev->irq) {
 		dev->read_subdev = s;
@@ -1381,11 +1383,11 @@ static int das1800_attach(struct comedi_device *dev,
 
 	/* analog out */
 	s = &dev->subdevices[1];
-	if (thisboard->ao_ability == 1) {
+	if (board->ao_ability == 1) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE;
-		s->n_chan = thisboard->ao_n_chan;
-		s->maxdata = (1 << thisboard->resolution) - 1;
+		s->n_chan = board->ao_n_chan;
+		s->maxdata = (1 << board->resolution) - 1;
 		s->range_table = &range_bipolar10;
 		s->insn_write = das1800_ao_winsn;
 	} else {
@@ -1405,7 +1407,7 @@ static int das1800_attach(struct comedi_device *dev,
 	s = &dev->subdevices[3];
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->do_n_chan;
+	s->n_chan = board->do_n_chan;
 	s->maxdata = 1;
 	s->range_table = &range_digital;
 	s->insn_bits = das1800_do_wbits;
@@ -1416,9 +1418,9 @@ static int das1800_attach(struct comedi_device *dev,
 	outb(0, dev->iobase + DAS1800_DIGITAL);
 
 	/*  initialize analog out channels */
-	if (thisboard->ao_ability == 1) {
+	if (board->ao_ability == 1) {
 		/*  select 'update' dac channel for baseAddress + 0x0 */
-		outb(DAC(thisboard->ao_n_chan - 1),
+		outb(DAC(board->ao_n_chan - 1),
 		     dev->iobase + DAS1800_SELECT);
 		outw(devpriv->ao_update_bits, dev->iobase + DAS1800_DAC);
 	}
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index 39d304a1259f..b02f12201cf7 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -240,13 +240,13 @@ static unsigned das800_ind_read(struct comedi_device *dev, unsigned reg)
 
 static void das800_enable(struct comedi_device *dev)
 {
-	const struct das800_board *thisboard = dev->board_ptr;
+	const struct das800_board *board = dev->board_ptr;
 	struct das800_private *devpriv = dev->private;
 	unsigned long irq_flags;
 
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 	/*  enable fifo-half full interrupts for cio-das802/16 */
-	if (thisboard->resolution == 16)
+	if (board->resolution == 16)
 		outb(CIO_ENHF, dev->iobase + DAS800_GAIN);
 	/* enable hardware triggering */
 	das800_ind_write(dev, CONV_HCEN, CONV_CONTROL);
@@ -303,7 +303,7 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
-	const struct das800_board *thisboard = dev->board_ptr;
+	const struct das800_board *board = dev->board_ptr;
 	int err = 0;
 
 	/* Step 1 : check if triggers are trivially valid */
@@ -335,7 +335,7 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
-						    thisboard->ai_speed);
+						    board->ai_speed);
 	}
 
 	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
@@ -375,7 +375,7 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 static int das800_ai_do_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
-	const struct das800_board *thisboard = dev->board_ptr;
+	const struct das800_board *board = dev->board_ptr;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned int gain = CR_RANGE(cmd->chanlist[0]);
@@ -393,7 +393,7 @@ static int das800_ai_do_cmd(struct comedi_device *dev,
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 
 	/* set gain */
-	if (thisboard->resolution == 12 && gain > 0)
+	if (board->resolution == 12 && gain > 0)
 		gain += 0x7;
 	gain &= 0xf;
 	outb(gain, dev->iobase + DAS800_GAIN);
@@ -604,54 +604,62 @@ static int das800_do_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int das800_probe(struct comedi_device *dev)
+static const struct das800_board *das800_probe(struct comedi_device *dev)
 {
-	const struct das800_board *thisboard = dev->board_ptr;
-	int board = thisboard ? thisboard - das800_boards : -EINVAL;
+	const struct das800_board *board = dev->board_ptr;
+	int index = board ? board - das800_boards : -EINVAL;
 	int id_bits;
 	unsigned long irq_flags;
 
+	/*
+	 * The dev->board_ptr will be set by comedi_device_attach() if the
+	 * board name provided by the user matches a board->name in this
+	 * driver. If so, this function sanity checks the id_bits to verify
+	 * that the board is correct.
+	 *
+	 * If the dev->board_ptr is not set, the user is trying to attach
+	 * an unspecified board to this driver. In this case the id_bits
+	 * are used to 'probe' for the correct dev->board_ptr.
+	 */
 	spin_lock_irqsave(&dev->spinlock, irq_flags);
 	id_bits = das800_ind_read(dev, ID) & 0x3;
 	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
 
 	switch (id_bits) {
 	case 0x0:
-		if (board == BOARD_DAS800 || board == BOARD_CIODAS800)
-			break;
-		dev_dbg(dev->class_dev, "Board model (probed): DAS-800\n");
-		board = BOARD_DAS800;
+		if (index == BOARD_DAS800 || index == BOARD_CIODAS800)
+			return board;
+		index = BOARD_DAS800;
 		break;
 	case 0x2:
-		if (board == BOARD_DAS801 || board == BOARD_CIODAS801)
-			break;
-		dev_dbg(dev->class_dev, "Board model (probed): DAS-801\n");
-		board = BOARD_DAS801;
+		if (index == BOARD_DAS801 || index == BOARD_CIODAS801)
+			return board;
+		index = BOARD_DAS801;
 		break;
 	case 0x3:
-		if (board == BOARD_DAS802 || board == BOARD_CIODAS802 ||
-		    board == BOARD_CIODAS80216)
-			break;
-		dev_dbg(dev->class_dev, "Board model (probed): DAS-802\n");
-		board = BOARD_DAS802;
+		if (index == BOARD_DAS802 || index == BOARD_CIODAS802 ||
+		    index == BOARD_CIODAS80216)
+			return board;
+		index = BOARD_DAS802;
 		break;
 	default:
 		dev_dbg(dev->class_dev, "Board model: 0x%x (unknown)\n",
 			id_bits);
-		board = -EINVAL;
-		break;
+		return NULL;
 	}
-	return board;
+	dev_dbg(dev->class_dev, "Board model (probed): %s series\n",
+		das800_boards[index].name);
+
+	return &das800_boards[index];
 }
 
 static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct das800_board *thisboard;
+	const struct das800_board *board;
 	struct das800_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq = it->options[1];
 	unsigned long irq_flags;
-	int board;
 	int ret;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
@@ -663,13 +671,10 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return ret;
 
 	board = das800_probe(dev);
-	if (board < 0) {
-		dev_dbg(dev->class_dev, "unable to determine board type\n");
+	if (!board)
 		return -ENODEV;
-	}
-	dev->board_ptr = das800_boards + board;
-	thisboard = dev->board_ptr;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	if (irq > 1 && irq <= 7) {
 		ret = request_irq(irq, das800_interrupt, 0, dev->board_name,
@@ -693,8 +698,8 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->type		= COMEDI_SUBD_AI;
 	s->subdev_flags	= SDF_READABLE | SDF_GROUND;
 	s->n_chan	= 8;
-	s->maxdata	= (1 << thisboard->resolution) - 1;
-	s->range_table	= thisboard->ai_range;
+	s->maxdata	= (1 << board->resolution) - 1;
+	s->range_table	= board->ai_range;
 	s->insn_read	= das800_ai_insn_read;
 	if (dev->irq) {
 		s->subdev_flags	|= SDF_CMD_READ;
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 031282c82f43..8c4f284d1919 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -400,7 +400,7 @@ static int dt3k_ns_to_timer(unsigned int timer_base, unsigned int *nanosec,
 static int dt3k_ai_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct dt3k_boardtype *this_board = dev->board_ptr;
+	const struct dt3k_boardtype *board = dev->board_ptr;
 	int err = 0;
 	unsigned int arg;
 
@@ -424,14 +424,14 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->scan_begin_arg,
-						    this_board->ai_speed);
+						    board->ai_speed);
 		err |= comedi_check_trigger_arg_max(&cmd->scan_begin_arg,
 						    100 * 16 * 65535);
 	}
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
-						    this_board->ai_speed);
+						    board->ai_speed);
 		err |= comedi_check_trigger_arg_max(&cmd->convert_arg,
 						    50 * 16 * 65535);
 	}
@@ -635,17 +635,17 @@ static int dt3000_auto_attach(struct comedi_device *dev,
 			      unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct dt3k_boardtype *this_board = NULL;
+	const struct dt3k_boardtype *board = NULL;
 	struct dt3k_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret = 0;
 
 	if (context < ARRAY_SIZE(dt3k_boardtypes))
-		this_board = &dt3k_boardtypes[context];
-	if (!this_board)
+		board = &dt3k_boardtypes[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = this_board;
-	dev->board_name = this_board->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -674,9 +674,9 @@ static int dt3000_auto_attach(struct comedi_device *dev,
 	/* ai subdevice */
 	s->type		= COMEDI_SUBD_AI;
 	s->subdev_flags	= SDF_READABLE | SDF_GROUND | SDF_DIFF;
-	s->n_chan	= this_board->adchan;
+	s->n_chan	= board->adchan;
 	s->insn_read	= dt3k_ai_insn;
-	s->maxdata	= (1 << this_board->adbits) - 1;
+	s->maxdata	= (1 << board->adbits) - 1;
 	s->range_table	= &range_dt3000_ai;	/* XXX */
 	if (dev->irq) {
 		dev->read_subdev = s;
@@ -692,7 +692,7 @@ static int dt3000_auto_attach(struct comedi_device *dev,
 	s->type		= COMEDI_SUBD_AO;
 	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 2;
-	s->maxdata	= (1 << this_board->dabits) - 1;
+	s->maxdata	= (1 << board->dabits) - 1;
 	s->len_chanlist	= 1;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= dt3k_ao_insn_write;
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index 3cb6409c4f01..e9296182236e 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -52,45 +52,45 @@
  * PCI BAR2 Register map (dev->mmio)
  */
 #define FIRMWARE_REV_REG			0x00
-#define FEATURES_REG_PRESENT_BIT		(1 << 15)
+#define FEATURES_REG_PRESENT_BIT		BIT(15)
 #define BOARD_CONTROL_REG			0x04
-#define BOARD_RESET_BIT				(1 << 0)
-#define TX_FIFO_RESET_BIT			(1 << 1)
-#define RX_FIFO_RESET_BIT			(1 << 2)
-#define TX_ENABLE_BIT				(1 << 4)
-#define RX_ENABLE_BIT				(1 << 5)
-#define DEMAND_DMA_DIRECTION_TX_BIT		(1 << 6)  /* ch 0 only */
-#define LINE_VALID_ON_STATUS_VALID_BIT		(1 << 7)
-#define START_TX_BIT				(1 << 8)
-#define CABLE_THROTTLE_ENABLE_BIT		(1 << 9)
-#define TEST_MODE_ENABLE_BIT			(1 << 31)
+#define BOARD_RESET_BIT				BIT(0)
+#define TX_FIFO_RESET_BIT			BIT(1)
+#define RX_FIFO_RESET_BIT			BIT(2)
+#define TX_ENABLE_BIT				BIT(4)
+#define RX_ENABLE_BIT				BIT(5)
+#define DEMAND_DMA_DIRECTION_TX_BIT		BIT(6)  /* ch 0 only */
+#define LINE_VALID_ON_STATUS_VALID_BIT		BIT(7)
+#define START_TX_BIT				BIT(8)
+#define CABLE_THROTTLE_ENABLE_BIT		BIT(9)
+#define TEST_MODE_ENABLE_BIT			BIT(31)
 #define BOARD_STATUS_REG			0x08
 #define COMMAND_LINE_STATUS_MASK		(0x7f << 0)
-#define TX_IN_PROGRESS_BIT			(1 << 7)
-#define TX_NOT_EMPTY_BIT			(1 << 8)
-#define TX_NOT_ALMOST_EMPTY_BIT			(1 << 9)
-#define TX_NOT_ALMOST_FULL_BIT			(1 << 10)
-#define TX_NOT_FULL_BIT				(1 << 11)
-#define RX_NOT_EMPTY_BIT			(1 << 12)
-#define RX_NOT_ALMOST_EMPTY_BIT			(1 << 13)
-#define RX_NOT_ALMOST_FULL_BIT			(1 << 14)
-#define RX_NOT_FULL_BIT				(1 << 15)
-#define BOARD_JUMPER0_INSTALLED_BIT		(1 << 16)
-#define BOARD_JUMPER1_INSTALLED_BIT		(1 << 17)
-#define TX_OVERRUN_BIT				(1 << 21)
-#define RX_UNDERRUN_BIT				(1 << 22)
-#define RX_OVERRUN_BIT				(1 << 23)
+#define TX_IN_PROGRESS_BIT			BIT(7)
+#define TX_NOT_EMPTY_BIT			BIT(8)
+#define TX_NOT_ALMOST_EMPTY_BIT			BIT(9)
+#define TX_NOT_ALMOST_FULL_BIT			BIT(10)
+#define TX_NOT_FULL_BIT				BIT(11)
+#define RX_NOT_EMPTY_BIT			BIT(12)
+#define RX_NOT_ALMOST_EMPTY_BIT			BIT(13)
+#define RX_NOT_ALMOST_FULL_BIT			BIT(14)
+#define RX_NOT_FULL_BIT				BIT(15)
+#define BOARD_JUMPER0_INSTALLED_BIT		BIT(16)
+#define BOARD_JUMPER1_INSTALLED_BIT		BIT(17)
+#define TX_OVERRUN_BIT				BIT(21)
+#define RX_UNDERRUN_BIT				BIT(22)
+#define RX_OVERRUN_BIT				BIT(23)
 #define TX_PROG_ALMOST_REG			0x0c
 #define RX_PROG_ALMOST_REG			0x10
 #define ALMOST_EMPTY_BITS(x)			(((x) & 0xffff) << 0)
 #define ALMOST_FULL_BITS(x)			(((x) & 0xff) << 16)
 #define FEATURES_REG				0x14
-#define FIFO_SIZE_PRESENT_BIT			(1 << 0)
-#define FIFO_WORDS_PRESENT_BIT			(1 << 1)
-#define LEVEL_EDGE_INTERRUPTS_PRESENT_BIT	(1 << 2)
-#define GPIO_SUPPORTED_BIT			(1 << 3)
-#define PLX_DMA_CH1_SUPPORTED_BIT		(1 << 4)
-#define OVERRUN_UNDERRUN_SUPPORTED_BIT		(1 << 5)
+#define FIFO_SIZE_PRESENT_BIT			BIT(0)
+#define FIFO_WORDS_PRESENT_BIT			BIT(1)
+#define LEVEL_EDGE_INTERRUPTS_PRESENT_BIT	BIT(2)
+#define GPIO_SUPPORTED_BIT			BIT(3)
+#define PLX_DMA_CH1_SUPPORTED_BIT		BIT(4)
+#define OVERRUN_UNDERRUN_SUPPORTED_BIT		BIT(5)
 #define FIFO_REG				0x18
 #define TX_STATUS_COUNT_REG			0x1c
 #define TX_LINE_VALID_COUNT_REG			0x20,
@@ -98,16 +98,16 @@
 #define RX_STATUS_COUNT_REG			0x28
 #define RX_LINE_COUNT_REG			0x2c
 #define INTERRUPT_CONTROL_REG			0x30
-#define FRAME_VALID_START_INTR			(1 << 0)
-#define FRAME_VALID_END_INTR			(1 << 1)
-#define TX_FIFO_EMPTY_INTR			(1 << 8)
-#define TX_FIFO_ALMOST_EMPTY_INTR		(1 << 9)
-#define TX_FIFO_ALMOST_FULL_INTR		(1 << 10)
-#define TX_FIFO_FULL_INTR			(1 << 11)
-#define RX_EMPTY_INTR				(1 << 12)
-#define RX_ALMOST_EMPTY_INTR			(1 << 13)
-#define RX_ALMOST_FULL_INTR			(1 << 14)
-#define RX_FULL_INTR				(1 << 15)
+#define FRAME_VALID_START_INTR			BIT(0)
+#define FRAME_VALID_END_INTR			BIT(1)
+#define TX_FIFO_EMPTY_INTR			BIT(8)
+#define TX_FIFO_ALMOST_EMPTY_INTR		BIT(9)
+#define TX_FIFO_ALMOST_FULL_INTR		BIT(10)
+#define TX_FIFO_FULL_INTR			BIT(11)
+#define RX_EMPTY_INTR				BIT(12)
+#define RX_ALMOST_EMPTY_INTR			BIT(13)
+#define RX_ALMOST_FULL_INTR			BIT(14)
+#define RX_FULL_INTR				BIT(15)
 #define INTERRUPT_STATUS_REG			0x34
 #define TX_CLOCK_DIVIDER_REG			0x38
 #define TX_FIFO_SIZE_REG			0x40
@@ -123,34 +123,15 @@
 #define NUM_DMA_BUFFERS				4
 #define NUM_DMA_DESCRIPTORS			256
 
-struct hpdi_board {
-	const char *name;
-	int device_id;
-	int subdevice_id;
-};
-
-static const struct hpdi_board hpdi_boards[] = {
-	{
-		.name		= "pci-hpdi32",
-		.device_id	= PCI_DEVICE_ID_PLX_9080,
-		.subdevice_id	= 0x2400,
-	 },
-#if 0
-	{
-		.name		= "pxi-hpdi32",
-		.device_id	= 0x9656,
-		.subdevice_id	= 0x2705,
-	 },
-#endif
-};
-
 struct hpdi_private {
 	void __iomem *plx9080_mmio;
-	uint32_t *dio_buffer[NUM_DMA_BUFFERS];	/*  dma buffers */
+	uint32_t *dio_buffer[NUM_DMA_BUFFERS];	/* dma buffers */
 	/* physical addresses of dma buffers */
 	dma_addr_t dio_buffer_phys_addr[NUM_DMA_BUFFERS];
-	/* array of dma descriptors read by plx9080, allocated to get proper
-	 * alignment */
+	/*
+	 * array of dma descriptors read by plx9080, allocated to get proper
+	 * alignment
+	 */
 	struct plx_dma_desc *dma_desc;
 	/* physical address of dma descriptor array */
 	dma_addr_t dma_desc_phys_addr;
@@ -202,7 +183,7 @@ static void gsc_hpdi_drain_dma(struct comedi_device *dev, unsigned int channel)
 
 		devpriv->dma_desc_index = idx;
 	}
-	/*  XXX check for buffer overrun somehow */
+	/* XXX check for buffer overrun somehow */
 }
 
 static irqreturn_t gsc_hpdi_interrupt(int irq, void *d)
@@ -230,10 +211,11 @@ static irqreturn_t gsc_hpdi_interrupt(int irq, void *d)
 	if (hpdi_intr_status)
 		writel(hpdi_intr_status, dev->mmio + INTERRUPT_STATUS_REG);
 
-	/*  spin lock makes sure no one else changes plx dma control reg */
+	/* spin lock makes sure no one else changes plx dma control reg */
 	spin_lock_irqsave(&dev->spinlock, flags);
 	dma0_status = readb(devpriv->plx9080_mmio + PLX_DMA0_CS_REG);
-	if (plx_status & ICS_DMA0_A) {	/*  dma chan 0 interrupt */
+	if (plx_status & ICS_DMA0_A) {
+		/* dma chan 0 interrupt */
 		writeb((dma0_status & PLX_DMA_EN_BIT) | PLX_CLEAR_DMA_INTR_BIT,
 		       devpriv->plx9080_mmio + PLX_DMA0_CS_REG);
 
@@ -242,17 +224,19 @@ static irqreturn_t gsc_hpdi_interrupt(int irq, void *d)
 	}
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	/*  spin lock makes sure no one else changes plx dma control reg */
+	/* spin lock makes sure no one else changes plx dma control reg */
 	spin_lock_irqsave(&dev->spinlock, flags);
 	dma1_status = readb(devpriv->plx9080_mmio + PLX_DMA1_CS_REG);
-	if (plx_status & ICS_DMA1_A) {	/*  XXX *//*  dma chan 1 interrupt */
+	if (plx_status & ICS_DMA1_A) {
+		/* XXX */ /* dma chan 1 interrupt */
 		writeb((dma1_status & PLX_DMA_EN_BIT) | PLX_CLEAR_DMA_INTR_BIT,
 		       devpriv->plx9080_mmio + PLX_DMA1_CS_REG);
 	}
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
-	/*  clear possible plx9080 interrupt sources */
-	if (plx_status & ICS_LDIA) {	/*  clear local doorbell interrupt */
+	/* clear possible plx9080 interrupt sources */
+	if (plx_status & ICS_LDIA) {
+		/* clear local doorbell interrupt */
 		plx_bits = readl(devpriv->plx9080_mmio + PLX_DBR_OUT_REG);
 		writel(plx_bits, devpriv->plx9080_mmio + PLX_DBR_OUT_REG);
 	}
@@ -280,7 +264,7 @@ static void gsc_hpdi_abort_dma(struct comedi_device *dev, unsigned int channel)
 	struct hpdi_private *devpriv = dev->private;
 	unsigned long flags;
 
-	/*  spinlock for plx dma control/status reg */
+	/* spinlock for plx dma control/status reg */
 	spin_lock_irqsave(&dev->spinlock, flags);
 
 	plx9080_abort_dma(devpriv->plx9080_mmio, channel);
@@ -536,7 +520,7 @@ static int gsc_hpdi_init(struct comedi_device *dev)
 
 	/* wait 10usec after reset before accessing fifos */
 	writel(BOARD_RESET_BIT, dev->mmio + BOARD_CONTROL_REG);
-	udelay(10);
+	usleep_range(10, 1000);
 
 	writel(ALMOST_EMPTY_BITS(32) | ALMOST_FULL_BITS(32),
 	       dev->mmio + RX_PROG_ALMOST_REG);
@@ -550,7 +534,7 @@ static int gsc_hpdi_init(struct comedi_device *dev)
 
 	writel(0, dev->mmio + INTERRUPT_CONTROL_REG);
 
-	/*  enable interrupts */
+	/* enable interrupts */
 	plx_intcsr_bits =
 	    ICS_AERR | ICS_PERR | ICS_PIE | ICS_PLIE | ICS_PAIE | ICS_LIE |
 	    ICS_DMA0_E;
@@ -577,57 +561,42 @@ static void gsc_hpdi_init_plx9080(struct comedi_device *dev)
 	gsc_hpdi_abort_dma(dev, 0);
 	gsc_hpdi_abort_dma(dev, 1);
 
-	/*  configure dma0 mode */
+	/* configure dma0 mode */
 	bits = 0;
-	/*  enable ready input */
+	/* enable ready input */
 	bits |= PLX_DMA_EN_READYIN_BIT;
-	/*  enable dma chaining */
+	/* enable dma chaining */
 	bits |= PLX_EN_CHAIN_BIT;
-	/*  enable interrupt on dma done
-	 *  (probably don't need this, since chain never finishes) */
+	/*
+	 * enable interrupt on dma done
+	 * (probably don't need this, since chain never finishes)
+	 */
 	bits |= PLX_EN_DMA_DONE_INTR_BIT;
-	/*  don't increment local address during transfers
-	 *  (we are transferring from a fixed fifo register) */
+	/*
+	 * don't increment local address during transfers
+	 * (we are transferring from a fixed fifo register)
+	 */
 	bits |= PLX_LOCAL_ADDR_CONST_BIT;
-	/*  route dma interrupt to pci bus */
+	/* route dma interrupt to pci bus */
 	bits |= PLX_DMA_INTR_PCI_BIT;
-	/*  enable demand mode */
+	/* enable demand mode */
 	bits |= PLX_DEMAND_MODE_BIT;
-	/*  enable local burst mode */
+	/* enable local burst mode */
 	bits |= PLX_DMA_LOCAL_BURST_EN_BIT;
 	bits |= PLX_LOCAL_BUS_32_WIDE_BITS;
 	writel(bits, plx_iobase + PLX_DMA0_MODE_REG);
 }
 
-static const struct hpdi_board *gsc_hpdi_find_board(struct pci_dev *pcidev)
-{
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(hpdi_boards); i++)
-		if (pcidev->device == hpdi_boards[i].device_id &&
-		    pcidev->subsystem_device == hpdi_boards[i].subdevice_id)
-			return &hpdi_boards[i];
-	return NULL;
-}
-
 static int gsc_hpdi_auto_attach(struct comedi_device *dev,
 				unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct hpdi_board *thisboard;
 	struct hpdi_private *devpriv;
 	struct comedi_subdevice *s;
 	int i;
 	int retval;
 
-	thisboard = gsc_hpdi_find_board(pcidev);
-	if (!thisboard) {
-		dev_err(dev->class_dev, "gsc_hpdi: pci %s not supported\n",
-			pci_name(pcidev));
-		return -EINVAL;
-	}
-	dev->board_ptr = thisboard;
-	dev->board_name = thisboard->name;
+	dev->board_name = "pci-hpdi32";
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -647,7 +616,7 @@ static int gsc_hpdi_auto_attach(struct comedi_device *dev,
 
 	gsc_hpdi_init_plx9080(dev);
 
-	/*  get irq */
+	/* get irq */
 	if (request_irq(pcidev->irq, gsc_hpdi_interrupt, IRQF_SHARED,
 			dev->board_name, dev)) {
 		dev_warn(dev->class_dev,
@@ -658,13 +627,13 @@ static int gsc_hpdi_auto_attach(struct comedi_device *dev,
 
 	dev_dbg(dev->class_dev, " irq %u\n", dev->irq);
 
-	/*  allocate pci dma buffers */
+	/* allocate pci dma buffers */
 	for (i = 0; i < NUM_DMA_BUFFERS; i++) {
 		devpriv->dio_buffer[i] =
 		    pci_alloc_consistent(pcidev, DMA_BUFFER_SIZE,
 					 &devpriv->dio_buffer_phys_addr[i]);
 	}
-	/*  allocate dma descriptors */
+	/* allocate dma descriptors */
 	devpriv->dma_desc = pci_alloc_consistent(pcidev,
 						 sizeof(struct plx_dma_desc) *
 						 NUM_DMA_DESCRIPTORS,
@@ -733,8 +702,8 @@ static int gsc_hpdi_pci_probe(struct pci_dev *dev,
 }
 
 static const struct pci_device_id gsc_hpdi_pci_table[] = {
-	{ PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9080, PCI_VENDOR_ID_PLX,
-		    0x2400, 0, 0, 0},
+	{ PCI_DEVICE_SUB(PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9080,
+			 PCI_VENDOR_ID_PLX, 0x2400) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, gsc_hpdi_pci_table);
@@ -748,5 +717,5 @@ static struct pci_driver gsc_hpdi_pci_driver = {
 module_comedi_pci_driver(gsc_hpdi_driver, gsc_hpdi_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for General Standards PCI-HPDI32/PMC-HPDI32");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index 7a8c27dd7154..a8f3ca48784b 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -453,7 +453,7 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 			       struct comedi_subdevice *subdevice,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	const struct me4000_board *thisboard = dev->board_ptr;
+	const struct me4000_board *board = dev->board_ptr;
 	int chan = CR_CHAN(insn->chanspec);
 	int rang = CR_RANGE(insn->chanspec);
 	int aref = CR_AREF(insn->chanspec);
@@ -491,7 +491,7 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 	switch (aref) {
 	case AREF_GROUND:
 	case AREF_COMMON:
-		if (chan >= thisboard->ai_nchan) {
+		if (chan >= board->ai_nchan) {
 			dev_err(dev->class_dev,
 				"Analog input is not available\n");
 			return -EINVAL;
@@ -506,7 +506,7 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 			return -EINVAL;
 		}
 
-		if (chan >= thisboard->ai_diff_nchan) {
+		if (chan >= board->ai_diff_nchan) {
 			dev_err(dev->class_dev,
 				"Analog input is not available\n");
 			return -EINVAL;
@@ -1252,17 +1252,17 @@ static int me4000_auto_attach(struct comedi_device *dev,
 			      unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct me4000_board *thisboard = NULL;
+	const struct me4000_board *board = NULL;
 	struct me4000_info *info;
 	struct comedi_subdevice *s;
 	int result;
 
 	if (context < ARRAY_SIZE(me4000_boards))
-		thisboard = &me4000_boards[context];
-	if (!thisboard)
+		board = &me4000_boards[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = thisboard;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	info = comedi_alloc_devpriv(dev, sizeof(*info));
 	if (!info)
@@ -1301,11 +1301,11 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[0];
 
-	if (thisboard->ai_nchan) {
+	if (board->ai_nchan) {
 		s->type = COMEDI_SUBD_AI;
 		s->subdev_flags =
 		    SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
-		s->n_chan = thisboard->ai_nchan;
+		s->n_chan = board->ai_nchan;
 		s->maxdata = 0xFFFF;	/*  16 bit ADC */
 		s->len_chanlist = ME4000_AI_CHANNEL_LIST_COUNT;
 		s->range_table = &me4000_ai_range;
@@ -1328,10 +1328,10 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[1];
 
-	if (thisboard->ao_nchan) {
+	if (board->ao_nchan) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_COMMON | SDF_GROUND;
-		s->n_chan = thisboard->ao_nchan;
+		s->n_chan = board->ao_nchan;
 		s->maxdata = 0xFFFF;	/*  16 bit DAC */
 		s->range_table = &range_bipolar10;
 		s->insn_write = me4000_ao_insn_write;
@@ -1349,10 +1349,10 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[2];
 
-	if (thisboard->dio_nchan) {
+	if (board->dio_nchan) {
 		s->type = COMEDI_SUBD_DIO;
 		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = thisboard->dio_nchan;
+		s->n_chan = board->dio_nchan;
 		s->maxdata = 1;
 		s->range_table = &range_digital;
 		s->insn_bits = me4000_dio_insn_bits;
@@ -1373,7 +1373,7 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 	/* Counter subdevice (8254) */
 	s = &dev->subdevices[3];
-	if (thisboard->has_counter) {
+	if (board->has_counter) {
 		unsigned long timer_base = pci_resource_start(pcidev, 3);
 
 		if (!timer_base)
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index d78e9195fbce..9ea1ba4b1b6f 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -173,8 +173,7 @@ struct me_private_data {
 
 static inline void sleep(unsigned sec)
 {
-	__set_current_state(TASK_INTERRUPTIBLE);
-	schedule_timeout(sec * HZ);
+	schedule_timeout_interruptible(sec * HZ);
 }
 
 static int me_dio_insn_config(struct comedi_device *dev,
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
index e43a0c8323c1..fa7ae2c04556 100644
--- a/drivers/staging/comedi/drivers/mite.c
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -1,52 +1,51 @@
 /*
-    comedi/drivers/mite.c
-    Hardware driver for NI Mite PCI interface chip
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1997-2002 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/mite.c
+ * Hardware driver for NI Mite PCI interface chip
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1997-2002 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.
+ */
 
 /*
-	The PCI-MIO E series driver was originally written by
-	Tomasz Motylewski <...>, and ported to comedi by ds.
-
-	References for specifications:
-
-	   321747b.pdf  Register Level Programmer Manual (obsolete)
-	   321747c.pdf  Register Level Programmer Manual (new)
-	   DAQ-STC reference manual
-
-	Other possibly relevant info:
-
-	   320517c.pdf  User manual (obsolete)
-	   320517f.pdf  User manual (new)
-	   320889a.pdf  delete
-	   320906c.pdf  maximum signal ratings
-	   321066a.pdf  about 16x
-	   321791a.pdf  discontinuation of at-mio-16e-10 rev. c
-	   321808a.pdf  about at-mio-16e-10 rev P
-	   321837a.pdf  discontinuation of at-mio-16de-10 rev d
-	   321838a.pdf  about at-mio-16de-10 rev N
-
-	ISSUES:
-
-*/
-
-/* #define USE_KMALLOC */
+ * The PCI-MIO E series driver was originally written by
+ * Tomasz Motylewski <...>, and ported to comedi by ds.
+ *
+ * References for specifications:
+ *
+ *    321747b.pdf  Register Level Programmer Manual (obsolete)
+ *    321747c.pdf  Register Level Programmer Manual (new)
+ *    DAQ-STC reference manual
+ *
+ * Other possibly relevant info:
+ *
+ *    320517c.pdf  User manual (obsolete)
+ *    320517f.pdf  User manual (new)
+ *    320889a.pdf  delete
+ *    320906c.pdf  maximum signal ratings
+ *    321066a.pdf  about 16x
+ *    321791a.pdf  discontinuation of at-mio-16e-10 rev. c
+ *    321808a.pdf  about at-mio-16e-10 rev P
+ *    321837a.pdf  discontinuation of at-mio-16de-10 rev d
+ *    321838a.pdf  about at-mio-16de-10 rev N
+ *
+ * ISSUES:
+ *
+ */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
+#include <linux/slab.h>
 
 #include "../comedi_pci.h"
 
@@ -132,7 +131,7 @@ int mite_setup2(struct comedi_device *dev,
 		       mite->mite_io_addr + MITE_IODWBSR);
 	}
 	/*
-	 * make sure dma bursts work. I got this from running a bus analyzer
+	 * Make sure dma bursts work. I got this from running a bus analyzer
 	 * on a pxi-6281 and a pxi-6713. 6713 powered up with register value
 	 * of 0x61f and bursts worked. 6281 powered up with register value of
 	 * 0x1f and bursts didn't work. The NI windows driver reads the
@@ -224,7 +223,8 @@ struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
 	unsigned long flags;
 	struct mite_channel *channel = NULL;
 
-	/* spin lock so mite_release_channel can be called safely
+	/*
+	 * spin lock so mite_release_channel can be called safely
 	 * from interrupts
 	 */
 	spin_lock_irqsave(&mite->lock, flags);
@@ -246,15 +246,15 @@ void mite_release_channel(struct mite_channel *mite_chan)
 	struct mite_struct *mite = mite_chan->mite;
 	unsigned long flags;
 
-	/*  spin lock to prevent races with mite_request_channel */
+	/* spin lock to prevent races with mite_request_channel */
 	spin_lock_irqsave(&mite->lock, flags);
 	if (mite->channel_allocated[mite_chan->channel]) {
 		mite_dma_disarm(mite_chan);
 		mite_dma_reset(mite_chan);
-	/*
-	 * disable all channel's interrupts (do it after disarm/reset so
-	 * MITE_CHCR reg isn't changed while dma is still active!)
-	 */
+		/*
+		 * disable all channel's interrupts (do it after disarm/reset so
+		 * MITE_CHCR reg isn't changed while dma is still active!)
+		 */
 		writel(CHCR_CLR_DMA_IE | CHCR_CLR_LINKP_IE |
 		       CHCR_CLR_SAR_IE | CHCR_CLR_DONE_IE |
 		       CHCR_CLR_MRDY_IE | CHCR_CLR_DRDY_IE |
@@ -286,7 +286,7 @@ void mite_dma_arm(struct mite_channel *mite_chan)
 	writel(chor, mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
 	mmiowb();
 	spin_unlock_irqrestore(&mite->lock, flags);
-/*       mite_dma_tcr(mite, channel); */
+	/* mite_dma_tcr(mite, channel); */
 }
 EXPORT_SYMBOL_GPL(mite_dma_arm);
 
@@ -529,8 +529,10 @@ int mite_sync_input_dma(struct mite_channel *mite_chan,
 	}
 
 	count = nbytes - async->buf_write_count;
-	/* it's possible count will be negative due to
-	 * conservative value returned by mite_bytes_written_to_memory_lb */
+	/*
+	 * it's possible count will be negative due to conservative value
+	 * returned by mite_bytes_written_to_memory_lb
+	 */
 	if (count <= 0)
 		return 0;
 
@@ -551,7 +553,7 @@ int mite_sync_output_dma(struct mite_channel *mite_chan,
 	u32 nbytes_ub, nbytes_lb;
 	int count;
 
-	/*  read alloc as much as we can */
+	/* read alloc as much as we can */
 	comedi_buf_read_alloc(s, async->prealloc_bufsz);
 	nbytes_lb = mite_bytes_read_from_memory_lb(mite_chan);
 	if (cmd->stop_src == TRIG_COUNT && (int)(nbytes_lb - stop_count) > 0)
@@ -622,5 +624,5 @@ module_init(mite_module_init);
 module_exit(mite_module_exit);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi helper for NI Mite PCI interface chip");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/mite.h b/drivers/staging/comedi/drivers/mite.h
index b3ca7fc3a31e..c32d4e4ddccc 100644
--- a/drivers/staging/comedi/drivers/mite.h
+++ b/drivers/staging/comedi/drivers/mite.h
@@ -1,32 +1,35 @@
 /*
-    module/mite.h
-    Hardware driver for NI Mite PCI interface chip
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1999 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.
-*/
+ * module/mite.h
+ * Hardware driver for NI Mite PCI interface chip
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1999 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.
+ */
 
 #ifndef _MITE_H_
 #define _MITE_H_
 
+#include <linux/io.h>
 #include <linux/log2.h>
-#include <linux/slab.h>
-#include "../comedi_pci.h"
-
-#define PCIMIO_COMPAT
+#include <linux/spinlock.h>
 
 #define MAX_MITE_DMA_CHANNELS 8
 
+struct comedi_device;
+struct comedi_subdevice;
+struct device;
+struct pci_dev;
+
 struct mite_dma_descriptor {
 	__le32 count;
 	__le32 addr;
@@ -74,16 +77,13 @@ static inline int mite_setup(struct comedi_device *dev,
 void mite_detach(struct mite_struct *mite);
 struct mite_dma_descriptor_ring *mite_alloc_ring(struct mite_struct *mite);
 void mite_free_ring(struct mite_dma_descriptor_ring *ring);
-struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
-						   struct
-						   mite_dma_descriptor_ring
-						   *ring, unsigned min_channel,
-						   unsigned max_channel);
-static inline struct mite_channel *mite_request_channel(struct mite_struct
-							*mite,
-							struct
-							mite_dma_descriptor_ring
-							*ring)
+struct mite_channel *
+mite_request_channel_in_range(struct mite_struct *mite,
+			      struct mite_dma_descriptor_ring *ring,
+			      unsigned min_channel, unsigned max_channel);
+static inline struct mite_channel *
+mite_request_channel(struct mite_struct *mite,
+		     struct mite_dma_descriptor_ring *ring)
 {
 	return mite_request_channel_in_range(mite, ring, 0,
 					     mite->num_channels - 1);
@@ -112,12 +112,14 @@ int mite_buf_change(struct mite_dma_descriptor_ring *ring,
 		    struct comedi_subdevice *s);
 
 enum mite_registers {
-	/* The bits 0x90180700 in MITE_UNKNOWN_DMA_BURST_REG can be
-	   written and read back.  The bits 0x1f always read as 1.
-	   The rest always read as zero. */
+	/*
+	 * The bits 0x90180700 in MITE_UNKNOWN_DMA_BURST_REG can be
+	 * written and read back.  The bits 0x1f always read as 1.
+	 * The rest always read as zero.
+	 */
 	MITE_UNKNOWN_DMA_BURST_REG = 0x28,
 	MITE_IODWBSR = 0xc0,	/* IO Device Window Base Size Register */
-	MITE_IODWBSR_1 = 0xc4,	/*  IO Device Window Base Size Register 1 */
+	MITE_IODWBSR_1 = 0xc4,	/* IO Device Window Base Size Register 1 */
 	MITE_IODWCR_1 = 0xf4,
 	MITE_PCI_CONFIG_OFFSET = 0x300,
 	MITE_CSIGR = 0x460	/* chip signature */
@@ -143,7 +145,7 @@ enum mite_registers {
 #define MITE_FCR(x)	(0x40 + MITE_CHAN(x))	/* fifo count */
 
 enum MITE_IODWBSR_bits {
-	WENAB = 0x80,		/*  window enable */
+	WENAB = 0x80,		/* window enable */
 };
 
 static inline unsigned MITE_IODWBSR_1_WSIZE_bits(unsigned size)
@@ -166,27 +168,27 @@ static inline int mite_csigr_version(u32 csigr_bits)
 };
 
 static inline int mite_csigr_type(u32 csigr_bits)
-{				/*  original mite = 0, minimite = 1 */
+{				/* original mite = 0, minimite = 1 */
 	return (csigr_bits >> 4) & 0xf;
 };
 
 static inline int mite_csigr_mmode(u32 csigr_bits)
-{				/*  mite mode, minimite = 1 */
+{				/* mite mode, minimite = 1 */
 	return (csigr_bits >> 8) & 0x3;
 };
 
 static inline int mite_csigr_imode(u32 csigr_bits)
-{				/*  cpu port interface mode, pci = 0x3 */
+{				/* cpu port interface mode, pci = 0x3 */
 	return (csigr_bits >> 12) & 0x3;
 };
 
 static inline int mite_csigr_dmac(u32 csigr_bits)
-{				/*  number of dma channels */
+{				/* number of dma channels */
 	return (csigr_bits >> 16) & 0xf;
 };
 
 static inline int mite_csigr_wpdep(u32 csigr_bits)
-{				/*  write post fifo depth */
+{				/* write post fifo depth */
 	unsigned int wpdep_bits = (csigr_bits >> 20) & 0x7;
 
 	return (wpdep_bits) ? (1 << (wpdep_bits - 1)) : 0;
@@ -198,7 +200,7 @@ static inline int mite_csigr_wins(u32 csigr_bits)
 };
 
 static inline int mite_csigr_iowins(u32 csigr_bits)
-{				/*  number of io windows */
+{				/* number of io windows */
 	return (csigr_bits >> 29) & 0x7;
 };
 
@@ -280,6 +282,7 @@ enum ConfigRegister_bits {
 	CR_PORTMXI = (3 << 6),
 	CR_AMDEVICE = (1 << 0),
 };
+
 static inline int CR_REQS(int source)
 {
 	return (source & 0x7) << 16;
@@ -287,8 +290,7 @@ static inline int CR_REQS(int source)
 
 static inline int CR_REQSDRQ(unsigned drq_line)
 {
-	/* This also works on m-series when
-	   using channels (drq_line) 4 or 5. */
+	/* This also works on m-series when using channels (drq_line) 4 or 5. */
 	return CR_REQS((drq_line & 0x3) | 0x4);
 }
 
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index 13c6ccb1fdea..f4c580f65a89 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -172,18 +172,18 @@ static int ni_670x_auto_attach(struct comedi_device *dev,
 			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct ni_670x_board *thisboard = NULL;
+	const struct ni_670x_board *board = NULL;
 	struct ni_670x_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 	int i;
 
 	if (context < ARRAY_SIZE(ni_670x_boards))
-		thisboard = &ni_670x_boards[context];
-	if (!thisboard)
+		board = &ni_670x_boards[context];
+	if (!board)
 		return -ENODEV;
-	dev->board_ptr = thisboard;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	ret = comedi_pci_enable(dev);
 	if (ret)
@@ -209,7 +209,7 @@ static int ni_670x_auto_attach(struct comedi_device *dev,
 	/* analog output subdevice */
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = thisboard->ao_chans;
+	s->n_chan = board->ao_chans;
 	s->maxdata = 0xffff;
 	if (s->n_chan == 32) {
 		const struct comedi_lrange **range_table_list;
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index 3a972d1538ab..9b444f8c4e33 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -106,7 +106,7 @@ TRIG_WAKE_EOS
 #define   IRQ_LVL_BITS(x)		(((x) & 0xf) << 4)	/*  sets irq level */
 #define   FIFO_INTR_EN_BIT		0x100	/*  enable fifo interrupts */
 #define   FIFO_INTR_FHF_BIT		0x200	/*  interrupt fifo half full */
-#define   DMA_INTR_EN_BIT 		0x800	/*  enable interrupt on dma terminal count */
+#define   DMA_INTR_EN_BIT		0x800	/*  enable interrupt on dma terminal count */
 #define   DMA_DEM_EN_BIT	0x1000	/*  enables demand mode dma */
 #define I8253_BASE_REG		0x14
 
@@ -264,7 +264,7 @@ static int a2150_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 			    unsigned int flags)
 {
-	const struct a2150_board *thisboard = dev->board_ptr;
+	const struct a2150_board *board = dev->board_ptr;
 	struct a2150_private *devpriv = dev->private;
 	int lub, glb, temp;
 	int lub_divisor_shift, lub_index, glb_divisor_shift, glb_index;
@@ -273,10 +273,10 @@ static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 	/*  initialize greatest lower and least upper bounds */
 	lub_divisor_shift = 3;
 	lub_index = 0;
-	lub = thisboard->clock[lub_index] * (1 << lub_divisor_shift);
+	lub = board->clock[lub_index] * (1 << lub_divisor_shift);
 	glb_divisor_shift = 0;
-	glb_index = thisboard->num_clocks - 1;
-	glb = thisboard->clock[glb_index] * (1 << glb_divisor_shift);
+	glb_index = board->num_clocks - 1;
+	glb = board->clock[glb_index] * (1 << glb_divisor_shift);
 
 	/*  make sure period is in available range */
 	if (*period < glb)
@@ -287,9 +287,9 @@ static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 	/*  we can multiply period by 1, 2, 4, or 8, using (1 << i) */
 	for (i = 0; i < 4; i++) {
 		/*  there are a maximum of 4 master clocks */
-		for (j = 0; j < thisboard->num_clocks; j++) {
+		for (j = 0; j < board->num_clocks; j++) {
 			/*  temp is the period in nanosec we are evaluating */
-			temp = thisboard->clock[j] * (1 << i);
+			temp = board->clock[j] * (1 << i);
 			/*  if it is the best match yet */
 			if (temp < lub && temp >= *period) {
 				lub_divisor_shift = i;
@@ -413,7 +413,7 @@ static int a2150_ai_check_chanlist(struct comedi_device *dev,
 static int a2150_ai_cmdtest(struct comedi_device *dev,
 			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct a2150_board *thisboard = dev->board_ptr;
+	const struct a2150_board *board = dev->board_ptr;
 	int err = 0;
 	unsigned int arg;
 
@@ -444,7 +444,7 @@ static int a2150_ai_cmdtest(struct comedi_device *dev,
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		err |= comedi_check_trigger_arg_min(&cmd->convert_arg,
-						    thisboard->ai_speed);
+						    board->ai_speed);
 	}
 
 	err |= comedi_check_trigger_arg_min(&cmd->chanlist_len, 1);
@@ -687,17 +687,19 @@ static void a2150_free_dma(struct comedi_device *dev)
 		comedi_isadma_free(devpriv->dma);
 }
 
-/* probes board type, returns offset */
-static int a2150_probe(struct comedi_device *dev)
+static const struct a2150_board *a2150_probe(struct comedi_device *dev)
 {
-	int status = inw(dev->iobase + STATUS_REG);
+	int id = ID_BITS(inw(dev->iobase + STATUS_REG));
 
-	return ID_BITS(status);
+	if (id >= ARRAY_SIZE(a2150_boards))
+		return NULL;
+
+	return &a2150_boards[id];
 }
 
 static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct a2150_board *thisboard;
+	const struct a2150_board *board;
 	struct a2150_private *devpriv;
 	struct comedi_subdevice *s;
 	static const int timeout = 2000;
@@ -712,13 +714,11 @@ static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	i = a2150_probe(dev);
-	if (i >= ARRAY_SIZE(a2150_boards))
+	board = a2150_probe(dev);
+	if (!board)
 		return -ENODEV;
-
-	dev->board_ptr = a2150_boards + i;
-	thisboard = dev->board_ptr;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	/* an IRQ and DMA are required to support async commands */
 	a2150_alloc_irq_and_dma(dev, it);
diff --git a/drivers/staging/comedi/drivers/ni_atmio.c b/drivers/staging/comedi/drivers/ni_atmio.c
index 1304b06980a6..95435b81aa55 100644
--- a/drivers/staging/comedi/drivers/ni_atmio.c
+++ b/drivers/staging/comedi/drivers/ni_atmio.c
@@ -274,14 +274,16 @@ static int ni_isapnp_find_board(struct pnp_dev **dev)
 	return 0;
 }
 
-static int ni_getboardtype(struct comedi_device *dev)
+static const struct ni_board_struct *ni_atmio_probe(struct comedi_device *dev)
 {
 	int device_id = ni_read_eeprom(dev, 511);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(ni_boards); i++) {
-		if (ni_boards[i].device_id == device_id)
-			return i;
+		const struct ni_board_struct *board = &ni_boards[i];
+
+		if (board->device_id == device_id)
+			return board;
 	}
 	if (device_id == 255)
 		dev_err(dev->class_dev, "can't find board\n");
@@ -292,17 +294,16 @@ static int ni_getboardtype(struct comedi_device *dev)
 		dev_err(dev->class_dev,
 			"unknown device ID %d -- contact author\n", device_id);
 
-	return -1;
+	return NULL;
 }
 
 static int ni_atmio_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
-	const struct ni_board_struct *boardtype;
+	const struct ni_board_struct *board;
 	struct pnp_dev *isapnp_dev;
 	int ret;
 	unsigned long iobase;
-	int board;
 	unsigned int irq;
 
 	ret = ni_alloc_private(dev);
@@ -326,15 +327,11 @@ static int ni_atmio_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	/* get board type */
-
-	board = ni_getboardtype(dev);
-	if (board < 0)
-		return -EIO;
-
-	dev->board_ptr = ni_boards + board;
-	boardtype = dev->board_ptr;
-	dev->board_name = boardtype->name;
+	board = ni_atmio_probe(dev);
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	/* irq stuff */
 
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index c66affd993aa..6cc304a4c59b 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -60,7 +60,6 @@
 
 /* A timeout count */
 #define NI_TIMEOUT 1000
-static const unsigned old_RTSI_clock_channel = 7;
 
 /* Note: this table must match the ai_gain_* definitions */
 static const short ni_gainlkup[][16] = {
@@ -308,262 +307,154 @@ static uint8_t ni_readb(struct comedi_device *dev, int reg)
  * windowed STC registers to the m series register offsets.
  */
 
-static void m_series_stc_writel(struct comedi_device *dev,
-				uint32_t data, int reg)
+struct mio_regmap {
+	unsigned int mio_reg;
+	int size;
+};
+
+static const struct mio_regmap m_series_stc_write_regmap[] = {
+	[NISTC_INTA_ACK_REG]		= { 0x104, 2 },
+	[NISTC_INTB_ACK_REG]		= { 0x106, 2 },
+	[NISTC_AI_CMD2_REG]		= { 0x108, 2 },
+	[NISTC_AO_CMD2_REG]		= { 0x10a, 2 },
+	[NISTC_G0_CMD_REG]		= { 0x10c, 2 },
+	[NISTC_G1_CMD_REG]		= { 0x10e, 2 },
+	[NISTC_AI_CMD1_REG]		= { 0x110, 2 },
+	[NISTC_AO_CMD1_REG]		= { 0x112, 2 },
+	/*
+	 * NISTC_DIO_OUT_REG maps to:
+	 * { NI_M_DIO_REG, 4 } and { NI_M_SCXI_SER_DO_REG, 1 }
+	 */
+	[NISTC_DIO_OUT_REG]		= { 0, 0 }, /* DOES NOT MAP CLEANLY */
+	[NISTC_DIO_CTRL_REG]		= { 0, 0 }, /* DOES NOT MAP CLEANLY */
+	[NISTC_AI_MODE1_REG]		= { 0x118, 2 },
+	[NISTC_AI_MODE2_REG]		= { 0x11a, 2 },
+	[NISTC_AI_SI_LOADA_REG]		= { 0x11c, 4 },
+	[NISTC_AI_SI_LOADB_REG]		= { 0x120, 4 },
+	[NISTC_AI_SC_LOADA_REG]		= { 0x124, 4 },
+	[NISTC_AI_SC_LOADB_REG]		= { 0x128, 4 },
+	[NISTC_AI_SI2_LOADA_REG]	= { 0x12c, 4 },
+	[NISTC_AI_SI2_LOADB_REG]	= { 0x130, 4 },
+	[NISTC_G0_MODE_REG]		= { 0x134, 2 },
+	[NISTC_G1_MODE_REG]		= { 0x136, 2 },
+	[NISTC_G0_LOADA_REG]		= { 0x138, 4 },
+	[NISTC_G0_LOADB_REG]		= { 0x13c, 4 },
+	[NISTC_G1_LOADA_REG]		= { 0x140, 4 },
+	[NISTC_G1_LOADB_REG]		= { 0x144, 4 },
+	[NISTC_G0_INPUT_SEL_REG]	= { 0x148, 2 },
+	[NISTC_G1_INPUT_SEL_REG]	= { 0x14a, 2 },
+	[NISTC_AO_MODE1_REG]		= { 0x14c, 2 },
+	[NISTC_AO_MODE2_REG]		= { 0x14e, 2 },
+	[NISTC_AO_UI_LOADA_REG]		= { 0x150, 4 },
+	[NISTC_AO_UI_LOADB_REG]		= { 0x154, 4 },
+	[NISTC_AO_BC_LOADA_REG]		= { 0x158, 4 },
+	[NISTC_AO_BC_LOADB_REG]		= { 0x15c, 4 },
+	[NISTC_AO_UC_LOADA_REG]		= { 0x160, 4 },
+	[NISTC_AO_UC_LOADB_REG]		= { 0x164, 4 },
+	[NISTC_CLK_FOUT_REG]		= { 0x170, 2 },
+	[NISTC_IO_BIDIR_PIN_REG]	= { 0x172, 2 },
+	[NISTC_RTSI_TRIG_DIR_REG]	= { 0x174, 2 },
+	[NISTC_INT_CTRL_REG]		= { 0x176, 2 },
+	[NISTC_AI_OUT_CTRL_REG]		= { 0x178, 2 },
+	[NISTC_ATRIG_ETC_REG]		= { 0x17a, 2 },
+	[NISTC_AI_START_STOP_REG]	= { 0x17c, 2 },
+	[NISTC_AI_TRIG_SEL_REG]		= { 0x17e, 2 },
+	[NISTC_AI_DIV_LOADA_REG]	= { 0x180, 4 },
+	[NISTC_AO_START_SEL_REG]	= { 0x184, 2 },
+	[NISTC_AO_TRIG_SEL_REG]		= { 0x186, 2 },
+	[NISTC_G0_AUTOINC_REG]		= { 0x188, 2 },
+	[NISTC_G1_AUTOINC_REG]		= { 0x18a, 2 },
+	[NISTC_AO_MODE3_REG]		= { 0x18c, 2 },
+	[NISTC_RESET_REG]		= { 0x190, 2 },
+	[NISTC_INTA_ENA_REG]		= { 0x192, 2 },
+	[NISTC_INTA2_ENA_REG]		= { 0, 0 }, /* E-Series only */
+	[NISTC_INTB_ENA_REG]		= { 0x196, 2 },
+	[NISTC_INTB2_ENA_REG]		= { 0, 0 }, /* E-Series only */
+	[NISTC_AI_PERSONAL_REG]		= { 0x19a, 2 },
+	[NISTC_AO_PERSONAL_REG]		= { 0x19c, 2 },
+	[NISTC_RTSI_TRIGA_OUT_REG]	= { 0x19e, 2 },
+	[NISTC_RTSI_TRIGB_OUT_REG]	= { 0x1a0, 2 },
+	[NISTC_RTSI_BOARD_REG]		= { 0, 0 }, /* Unknown */
+	[NISTC_CFG_MEM_CLR_REG]		= { 0x1a4, 2 },
+	[NISTC_ADC_FIFO_CLR_REG]	= { 0x1a6, 2 },
+	[NISTC_DAC_FIFO_CLR_REG]	= { 0x1a8, 2 },
+	[NISTC_AO_OUT_CTRL_REG]		= { 0x1ac, 2 },
+	[NISTC_AI_MODE3_REG]		= { 0x1ae, 2 },
+};
+
+static void m_series_stc_write(struct comedi_device *dev,
+			       unsigned int data, unsigned int reg)
 {
-	unsigned offset;
+	const struct mio_regmap *regmap;
 
-	switch (reg) {
-	case AI_SC_Load_A_Registers:
-		offset = M_Offset_AI_SC_Load_A;
-		break;
-	case AI_SI_Load_A_Registers:
-		offset = M_Offset_AI_SI_Load_A;
-		break;
-	case AO_BC_Load_A_Register:
-		offset = M_Offset_AO_BC_Load_A;
-		break;
-	case AO_UC_Load_A_Register:
-		offset = M_Offset_AO_UC_Load_A;
-		break;
-	case AO_UI_Load_A_Register:
-		offset = M_Offset_AO_UI_Load_A;
-		break;
-	case G_Load_A_Register(0):
-		offset = M_Offset_G0_Load_A;
-		break;
-	case G_Load_A_Register(1):
-		offset = M_Offset_G1_Load_A;
-		break;
-	case G_Load_B_Register(0):
-		offset = M_Offset_G0_Load_B;
-		break;
-	case G_Load_B_Register(1):
-		offset = M_Offset_G1_Load_B;
-		break;
-	default:
-		dev_warn(dev->class_dev,
-			 "%s: bug! unhandled register=0x%x in switch\n",
+	if (reg < ARRAY_SIZE(m_series_stc_write_regmap)) {
+		regmap = &m_series_stc_write_regmap[reg];
+	} else {
+		dev_warn(dev->class_dev, "%s: unhandled register=0x%x\n",
 			 __func__, reg);
 		return;
 	}
-	ni_writel(dev, data, offset);
-}
 
-static void m_series_stc_writew(struct comedi_device *dev,
-				uint16_t data, int reg)
-{
-	unsigned offset;
-
-	switch (reg) {
-	case ADC_FIFO_Clear:
-		offset = M_Offset_AI_FIFO_Clear;
-		break;
-	case AI_Command_1_Register:
-		offset = M_Offset_AI_Command_1;
-		break;
-	case AI_Command_2_Register:
-		offset = M_Offset_AI_Command_2;
-		break;
-	case AI_Mode_1_Register:
-		offset = M_Offset_AI_Mode_1;
-		break;
-	case AI_Mode_2_Register:
-		offset = M_Offset_AI_Mode_2;
-		break;
-	case AI_Mode_3_Register:
-		offset = M_Offset_AI_Mode_3;
-		break;
-	case AI_Output_Control_Register:
-		offset = M_Offset_AI_Output_Control;
-		break;
-	case AI_Personal_Register:
-		offset = M_Offset_AI_Personal;
-		break;
-	case AI_SI2_Load_A_Register:
-		/* this is a 32 bit register on m series boards */
-		ni_writel(dev, data, M_Offset_AI_SI2_Load_A);
-		return;
-	case AI_SI2_Load_B_Register:
-		/* this is a 32 bit register on m series boards */
-		ni_writel(dev, data, M_Offset_AI_SI2_Load_B);
-		return;
-	case AI_START_STOP_Select_Register:
-		offset = M_Offset_AI_START_STOP_Select;
-		break;
-	case AI_Trigger_Select_Register:
-		offset = M_Offset_AI_Trigger_Select;
-		break;
-	case Analog_Trigger_Etc_Register:
-		offset = M_Offset_Analog_Trigger_Etc;
-		break;
-	case AO_Command_1_Register:
-		offset = M_Offset_AO_Command_1;
-		break;
-	case AO_Command_2_Register:
-		offset = M_Offset_AO_Command_2;
-		break;
-	case AO_Mode_1_Register:
-		offset = M_Offset_AO_Mode_1;
-		break;
-	case AO_Mode_2_Register:
-		offset = M_Offset_AO_Mode_2;
-		break;
-	case AO_Mode_3_Register:
-		offset = M_Offset_AO_Mode_3;
-		break;
-	case AO_Output_Control_Register:
-		offset = M_Offset_AO_Output_Control;
-		break;
-	case AO_Personal_Register:
-		offset = M_Offset_AO_Personal;
-		break;
-	case AO_Start_Select_Register:
-		offset = M_Offset_AO_Start_Select;
-		break;
-	case AO_Trigger_Select_Register:
-		offset = M_Offset_AO_Trigger_Select;
-		break;
-	case Clock_and_FOUT_Register:
-		offset = M_Offset_Clock_and_FOUT;
-		break;
-	case Configuration_Memory_Clear:
-		offset = M_Offset_Configuration_Memory_Clear;
-		break;
-	case DAC_FIFO_Clear:
-		offset = M_Offset_AO_FIFO_Clear;
-		break;
-	case DIO_Control_Register:
-		dev_dbg(dev->class_dev,
-			"%s: FIXME: register 0x%x does not map cleanly on to m-series boards\n",
-			__func__, reg);
-		return;
-	case G_Autoincrement_Register(0):
-		offset = M_Offset_G0_Autoincrement;
-		break;
-	case G_Autoincrement_Register(1):
-		offset = M_Offset_G1_Autoincrement;
-		break;
-	case G_Command_Register(0):
-		offset = M_Offset_G0_Command;
-		break;
-	case G_Command_Register(1):
-		offset = M_Offset_G1_Command;
-		break;
-	case G_Input_Select_Register(0):
-		offset = M_Offset_G0_Input_Select;
-		break;
-	case G_Input_Select_Register(1):
-		offset = M_Offset_G1_Input_Select;
-		break;
-	case G_Mode_Register(0):
-		offset = M_Offset_G0_Mode;
-		break;
-	case G_Mode_Register(1):
-		offset = M_Offset_G1_Mode;
-		break;
-	case Interrupt_A_Ack_Register:
-		offset = M_Offset_Interrupt_A_Ack;
-		break;
-	case Interrupt_A_Enable_Register:
-		offset = M_Offset_Interrupt_A_Enable;
-		break;
-	case Interrupt_B_Ack_Register:
-		offset = M_Offset_Interrupt_B_Ack;
-		break;
-	case Interrupt_B_Enable_Register:
-		offset = M_Offset_Interrupt_B_Enable;
-		break;
-	case Interrupt_Control_Register:
-		offset = M_Offset_Interrupt_Control;
-		break;
-	case IO_Bidirection_Pin_Register:
-		offset = M_Offset_IO_Bidirection_Pin;
-		break;
-	case Joint_Reset_Register:
-		offset = M_Offset_Joint_Reset;
-		break;
-	case RTSI_Trig_A_Output_Register:
-		offset = M_Offset_RTSI_Trig_A_Output;
-		break;
-	case RTSI_Trig_B_Output_Register:
-		offset = M_Offset_RTSI_Trig_B_Output;
+	switch (regmap->size) {
+	case 4:
+		ni_writel(dev, data, regmap->mio_reg);
 		break;
-	case RTSI_Trig_Direction_Register:
-		offset = M_Offset_RTSI_Trig_Direction;
+	case 2:
+		ni_writew(dev, data, regmap->mio_reg);
 		break;
-	/*
-	 * FIXME: DIO_Output_Register (16 bit reg) is replaced by
-	 * M_Offset_Static_Digital_Output (32 bit) and
-	 * M_Offset_SCXI_Serial_Data_Out (8 bit)
-	 */
 	default:
-		dev_warn(dev->class_dev,
-			 "%s: bug! unhandled register=0x%x in switch\n",
+		dev_warn(dev->class_dev, "%s: unmapped register=0x%x\n",
 			 __func__, reg);
-		return;
+		break;
 	}
-	ni_writew(dev, data, offset);
 }
 
-static uint32_t m_series_stc_readl(struct comedi_device *dev, int reg)
+static const struct mio_regmap m_series_stc_read_regmap[] = {
+	[NISTC_AI_STATUS1_REG]		= { 0x104, 2 },
+	[NISTC_AO_STATUS1_REG]		= { 0x106, 2 },
+	[NISTC_G01_STATUS_REG]		= { 0x108, 2 },
+	[NISTC_AI_STATUS2_REG]		= { 0, 0 }, /* Unknown */
+	[NISTC_AO_STATUS2_REG]		= { 0x10c, 2 },
+	[NISTC_DIO_IN_REG]		= { 0, 0 }, /* Unknown */
+	[NISTC_G0_HW_SAVE_REG]		= { 0x110, 4 },
+	[NISTC_G1_HW_SAVE_REG]		= { 0x114, 4 },
+	[NISTC_G0_SAVE_REG]		= { 0x118, 4 },
+	[NISTC_G1_SAVE_REG]		= { 0x11c, 4 },
+	[NISTC_AO_UI_SAVE_REG]		= { 0x120, 4 },
+	[NISTC_AO_BC_SAVE_REG]		= { 0x124, 4 },
+	[NISTC_AO_UC_SAVE_REG]		= { 0x128, 4 },
+	[NISTC_STATUS1_REG]		= { 0x136, 2 },
+	[NISTC_DIO_SERIAL_IN_REG]	= { 0x009, 1 },
+	[NISTC_STATUS2_REG]		= { 0x13a, 2 },
+	[NISTC_AI_SI_SAVE_REG]		= { 0x180, 4 },
+	[NISTC_AI_SC_SAVE_REG]		= { 0x184, 4 },
+};
+
+static unsigned int m_series_stc_read(struct comedi_device *dev,
+				      unsigned int reg)
 {
-	unsigned offset;
+	const struct mio_regmap *regmap;
 
-	switch (reg) {
-	case G_HW_Save_Register(0):
-		offset = M_Offset_G0_HW_Save;
-		break;
-	case G_HW_Save_Register(1):
-		offset = M_Offset_G1_HW_Save;
-		break;
-	case G_Save_Register(0):
-		offset = M_Offset_G0_Save;
-		break;
-	case G_Save_Register(1):
-		offset = M_Offset_G1_Save;
-		break;
-	default:
-		dev_warn(dev->class_dev,
-			 "%s: bug! unhandled register=0x%x in switch\n",
+	if (reg < ARRAY_SIZE(m_series_stc_read_regmap)) {
+		regmap = &m_series_stc_read_regmap[reg];
+	} else {
+		dev_warn(dev->class_dev, "%s: unhandled register=0x%x\n",
 			 __func__, reg);
 		return 0;
 	}
-	return ni_readl(dev, offset);
-}
 
-static uint16_t m_series_stc_readw(struct comedi_device *dev, int reg)
-{
-	unsigned offset;
-
-	switch (reg) {
-	case AI_Status_1_Register:
-		offset = M_Offset_AI_Status_1;
-		break;
-	case AO_Status_1_Register:
-		offset = M_Offset_AO_Status_1;
-		break;
-	case AO_Status_2_Register:
-		offset = M_Offset_AO_Status_2;
-		break;
-	case DIO_Serial_Input_Register:
-		return ni_readb(dev, M_Offset_SCXI_Serial_Data_In);
-	case Joint_Status_1_Register:
-		offset = M_Offset_Joint_Status_1;
-		break;
-	case Joint_Status_2_Register:
-		offset = M_Offset_Joint_Status_2;
-		break;
-	case G_Status_Register:
-		offset = M_Offset_G01_Status;
-		break;
+	switch (regmap->size) {
+	case 4:
+		return ni_readl(dev, regmap->mio_reg);
+	case 2:
+		return ni_readw(dev, regmap->mio_reg);
+	case 1:
+		return ni_readb(dev, regmap->mio_reg);
 	default:
-		dev_warn(dev->class_dev,
-			 "%s: bug! unhandled register=0x%x in switch\n",
+		dev_warn(dev->class_dev, "%s: unmapped register=0x%x\n",
 			 __func__, reg);
 		return 0;
 	}
-	return ni_readw(dev, offset);
 }
 
 static void ni_stc_writew(struct comedi_device *dev, uint16_t data, int reg)
@@ -572,14 +463,14 @@ static void ni_stc_writew(struct comedi_device *dev, uint16_t data, int reg)
 	unsigned long flags;
 
 	if (devpriv->is_m_series) {
-		m_series_stc_writew(dev, data, reg);
+		m_series_stc_write(dev, data, reg);
 	} else {
 		spin_lock_irqsave(&devpriv->window_lock, flags);
 		if (!devpriv->mite && reg < 8) {
 			ni_writew(dev, data, reg * 2);
 		} else {
-			ni_writew(dev, reg, Window_Address);
-			ni_writew(dev, data, Window_Data);
+			ni_writew(dev, reg, NI_E_STC_WINDOW_ADDR_REG);
+			ni_writew(dev, data, NI_E_STC_WINDOW_DATA_REG);
 		}
 		spin_unlock_irqrestore(&devpriv->window_lock, flags);
 	}
@@ -590,7 +481,7 @@ static void ni_stc_writel(struct comedi_device *dev, uint32_t data, int reg)
 	struct ni_private *devpriv = dev->private;
 
 	if (devpriv->is_m_series) {
-		m_series_stc_writel(dev, data, reg);
+		m_series_stc_write(dev, data, reg);
 	} else {
 		ni_stc_writew(dev, data >> 16, reg);
 		ni_stc_writew(dev, data & 0xffff, reg + 1);
@@ -604,14 +495,14 @@ static uint16_t ni_stc_readw(struct comedi_device *dev, int reg)
 	uint16_t val;
 
 	if (devpriv->is_m_series) {
-		val = m_series_stc_readw(dev, reg);
+		val = m_series_stc_read(dev, reg);
 	} else {
 		spin_lock_irqsave(&devpriv->window_lock, flags);
 		if (!devpriv->mite && reg < 8) {
 			val = ni_readw(dev, reg * 2);
 		} else {
-			ni_writew(dev, reg, Window_Address);
-			val = ni_readw(dev, Window_Data);
+			ni_writew(dev, reg, NI_E_STC_WINDOW_ADDR_REG);
+			val = ni_readw(dev, NI_E_STC_WINDOW_DATA_REG);
 		}
 		spin_unlock_irqrestore(&devpriv->window_lock, flags);
 	}
@@ -624,7 +515,7 @@ static uint32_t ni_stc_readl(struct comedi_device *dev, int reg)
 	uint32_t val;
 
 	if (devpriv->is_m_series) {
-		val = m_series_stc_readl(dev, reg);
+		val = m_series_stc_read(dev, reg);
 	} else {
 		val = ni_stc_readw(dev, reg) << 16;
 		val |= ni_stc_readw(dev, reg + 1);
@@ -640,33 +531,30 @@ static inline void ni_set_bitfield(struct comedi_device *dev, int reg,
 
 	spin_lock_irqsave(&devpriv->soft_reg_copy_lock, flags);
 	switch (reg) {
-	case Interrupt_A_Enable_Register:
+	case NISTC_INTA_ENA_REG:
 		devpriv->int_a_enable_reg &= ~bit_mask;
 		devpriv->int_a_enable_reg |= bit_values & bit_mask;
-		ni_stc_writew(dev, devpriv->int_a_enable_reg,
-			      Interrupt_A_Enable_Register);
+		ni_stc_writew(dev, devpriv->int_a_enable_reg, reg);
 		break;
-	case Interrupt_B_Enable_Register:
+	case NISTC_INTB_ENA_REG:
 		devpriv->int_b_enable_reg &= ~bit_mask;
 		devpriv->int_b_enable_reg |= bit_values & bit_mask;
-		ni_stc_writew(dev, devpriv->int_b_enable_reg,
-			      Interrupt_B_Enable_Register);
+		ni_stc_writew(dev, devpriv->int_b_enable_reg, reg);
 		break;
-	case IO_Bidirection_Pin_Register:
+	case NISTC_IO_BIDIR_PIN_REG:
 		devpriv->io_bidirection_pin_reg &= ~bit_mask;
 		devpriv->io_bidirection_pin_reg |= bit_values & bit_mask;
-		ni_stc_writew(dev, devpriv->io_bidirection_pin_reg,
-			      IO_Bidirection_Pin_Register);
+		ni_stc_writew(dev, devpriv->io_bidirection_pin_reg, reg);
 		break;
-	case AI_AO_Select:
+	case NI_E_DMA_AI_AO_SEL_REG:
 		devpriv->ai_ao_select_reg &= ~bit_mask;
 		devpriv->ai_ao_select_reg |= bit_values & bit_mask;
-		ni_writeb(dev, devpriv->ai_ao_select_reg, AI_AO_Select);
+		ni_writeb(dev, devpriv->ai_ao_select_reg, reg);
 		break;
-	case G0_G1_Select:
+	case NI_E_DMA_G0_G1_SEL_REG:
 		devpriv->g0_g1_select_reg &= ~bit_mask;
 		devpriv->g0_g1_select_reg |= bit_values & bit_mask;
-		ni_writeb(dev, devpriv->g0_g1_select_reg, G0_G1_Select);
+		ni_writeb(dev, devpriv->g0_g1_select_reg, reg);
 		break;
 	default:
 		dev_err(dev->class_dev, "called with invalid register %d\n",
@@ -679,48 +567,55 @@ static inline void ni_set_bitfield(struct comedi_device *dev, int reg,
 
 #ifdef PCIDMA
 /* DMA channel setup */
+static inline unsigned ni_stc_dma_channel_select_bitfield(unsigned channel)
+{
+	if (channel < 4)
+		return 1 << channel;
+	if (channel == 4)
+		return 0x3;
+	if (channel == 5)
+		return 0x5;
+	BUG();
+	return 0;
+}
 
 /* negative channel means no channel */
 static inline void ni_set_ai_dma_channel(struct comedi_device *dev, int channel)
 {
-	unsigned bitfield;
+	unsigned bits = 0;
 
 	if (channel >= 0)
-		bitfield =
-		    (ni_stc_dma_channel_select_bitfield(channel) <<
-		     AI_DMA_Select_Shift) & AI_DMA_Select_Mask;
-	else
-		bitfield = 0;
-	ni_set_bitfield(dev, AI_AO_Select, AI_DMA_Select_Mask, bitfield);
+		bits = ni_stc_dma_channel_select_bitfield(channel);
+
+	ni_set_bitfield(dev, NI_E_DMA_AI_AO_SEL_REG,
+			NI_E_DMA_AI_SEL_MASK, NI_E_DMA_AI_SEL(bits));
 }
 
 /* negative channel means no channel */
 static inline void ni_set_ao_dma_channel(struct comedi_device *dev, int channel)
 {
-	unsigned bitfield;
+	unsigned bits = 0;
 
 	if (channel >= 0)
-		bitfield =
-		    (ni_stc_dma_channel_select_bitfield(channel) <<
-		     AO_DMA_Select_Shift) & AO_DMA_Select_Mask;
-	else
-		bitfield = 0;
-	ni_set_bitfield(dev, AI_AO_Select, AO_DMA_Select_Mask, bitfield);
+		bits = ni_stc_dma_channel_select_bitfield(channel);
+
+	ni_set_bitfield(dev, NI_E_DMA_AI_AO_SEL_REG,
+			NI_E_DMA_AO_SEL_MASK, NI_E_DMA_AO_SEL(bits));
 }
 
-/* negative mite_channel means no channel */
+/* negative channel means no channel */
 static inline void ni_set_gpct_dma_channel(struct comedi_device *dev,
 					   unsigned gpct_index,
-					   int mite_channel)
+					   int channel)
 {
-	unsigned bitfield;
+	unsigned bits = 0;
 
-	if (mite_channel >= 0)
-		bitfield = GPCT_DMA_Select_Bits(gpct_index, mite_channel);
-	else
-		bitfield = 0;
-	ni_set_bitfield(dev, G0_G1_Select, GPCT_DMA_Select_Mask(gpct_index),
-			bitfield);
+	if (channel >= 0)
+		bits = ni_stc_dma_channel_select_bitfield(channel);
+
+	ni_set_bitfield(dev, NI_E_DMA_G0_G1_SEL_REG,
+			NI_E_DMA_G0_G1_SEL_MASK(gpct_index),
+			NI_E_DMA_G0_G1_SEL(gpct_index, bits));
 }
 
 /* negative mite_channel means no channel */
@@ -729,18 +624,21 @@ static inline void ni_set_cdo_dma_channel(struct comedi_device *dev,
 {
 	struct ni_private *devpriv = dev->private;
 	unsigned long flags;
+	unsigned bits;
 
 	spin_lock_irqsave(&devpriv->soft_reg_copy_lock, flags);
-	devpriv->cdio_dma_select_reg &= ~CDO_DMA_Select_Mask;
+	devpriv->cdio_dma_select_reg &= ~NI_M_CDIO_DMA_SEL_CDO_MASK;
 	if (mite_channel >= 0) {
-		/*XXX just guessing ni_stc_dma_channel_select_bitfield() returns the right bits,
-		   under the assumption the cdio dma selection works just like ai/ao/gpct.
-		   Definitely works for dma channels 0 and 1. */
-		devpriv->cdio_dma_select_reg |=
-		    (ni_stc_dma_channel_select_bitfield(mite_channel) <<
-		     CDO_DMA_Select_Shift) & CDO_DMA_Select_Mask;
-	}
-	ni_writeb(dev, devpriv->cdio_dma_select_reg, M_Offset_CDIO_DMA_Select);
+		/*
+		 * XXX just guessing ni_stc_dma_channel_select_bitfield()
+		 * returns the right bits, under the assumption the cdio dma
+		 * selection works just like ai/ao/gpct.
+		 * Definitely works for dma channels 0 and 1.
+		 */
+		bits = ni_stc_dma_channel_select_bitfield(mite_channel);
+		devpriv->cdio_dma_select_reg |= NI_M_CDIO_DMA_SEL_CDO(bits);
+	}
+	ni_writeb(dev, devpriv->cdio_dma_select_reg, NI_M_CDIO_DMA_SEL_REG);
 	mmiowb();
 	spin_unlock_irqrestore(&devpriv->soft_reg_copy_lock, flags);
 }
@@ -795,7 +693,6 @@ static int ni_request_gpct_mite_channel(struct comedi_device *dev,
 	unsigned long flags;
 	struct mite_channel *mite_chan;
 
-	BUG_ON(gpct_index >= NUM_GPCT);
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	BUG_ON(devpriv->counter_dev->counters[gpct_index].mite_chan);
 	mite_chan =
@@ -879,7 +776,6 @@ static void ni_release_gpct_mite_channel(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	unsigned long flags;
 
-	BUG_ON(gpct_index >= NUM_GPCT);
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->counter_dev->counters[gpct_index].mite_chan) {
 		struct mite_channel *mite_chan =
@@ -927,13 +823,13 @@ static void ni_e_series_enable_second_irq(struct comedi_device *dev,
 	 * dma requests for their counters
 	 */
 	if (gpct_index == 0) {
-		reg = Second_IRQ_A_Enable_Register;
+		reg = NISTC_INTA2_ENA_REG;
 		if (enable)
-			val = G0_Gate_Second_Irq_Enable;
+			val = NISTC_INTA_ENA_G0_GATE;
 	} else {
-		reg = Second_IRQ_B_Enable_Register;
+		reg = NISTC_INTB2_ENA_REG;
 		if (enable)
-			val = G1_Gate_Second_Irq_Enable;
+			val = NISTC_INTB_ENA_G1_GATE;
 	}
 	ni_stc_writew(dev, val, reg);
 }
@@ -947,30 +843,30 @@ static void ni_clear_ai_fifo(struct comedi_device *dev)
 
 	if (devpriv->is_6143) {
 		/*  Flush the 6143 data FIFO */
-		ni_writel(dev, 0x10, AIFIFO_Control_6143);
-		ni_writel(dev, 0x00, AIFIFO_Control_6143);
+		ni_writel(dev, 0x10, NI6143_AI_FIFO_CTRL_REG);
+		ni_writel(dev, 0x00, NI6143_AI_FIFO_CTRL_REG);
 		/*  Wait for complete */
 		for (i = 0; i < timeout; i++) {
-			if (!(ni_readl(dev, AIFIFO_Status_6143) & 0x10))
+			if (!(ni_readl(dev, NI6143_AI_FIFO_STATUS_REG) & 0x10))
 				break;
 			udelay(1);
 		}
 		if (i == timeout)
 			dev_err(dev->class_dev, "FIFO flush timeout\n");
 	} else {
-		ni_stc_writew(dev, 1, ADC_FIFO_Clear);
+		ni_stc_writew(dev, 1, NISTC_ADC_FIFO_CLR_REG);
 		if (devpriv->is_625x) {
-			ni_writeb(dev, 0, M_Offset_Static_AI_Control(0));
-			ni_writeb(dev, 1, M_Offset_Static_AI_Control(0));
+			ni_writeb(dev, 0, NI_M_STATIC_AI_CTRL_REG(0));
+			ni_writeb(dev, 1, NI_M_STATIC_AI_CTRL_REG(0));
 #if 0
 			/* the NI example code does 3 convert pulses for 625x boards,
 			   but that appears to be wrong in practice. */
-			ni_stc_writew(dev, AI_CONVERT_Pulse,
-				      AI_Command_1_Register);
-			ni_stc_writew(dev, AI_CONVERT_Pulse,
-				      AI_Command_1_Register);
-			ni_stc_writew(dev, AI_CONVERT_Pulse,
-				      AI_Command_1_Register);
+			ni_stc_writew(dev, NISTC_AI_CMD1_CONVERT_PULSE,
+				      NISTC_AI_CMD1_REG);
+			ni_stc_writew(dev, NISTC_AI_CMD1_CONVERT_PULSE,
+				      NISTC_AI_CMD1_REG);
+			ni_stc_writew(dev, NISTC_AI_CMD1_CONVERT_PULSE,
+				      NISTC_AI_CMD1_REG);
 #endif
 		}
 	}
@@ -983,8 +879,8 @@ static inline void ni_ao_win_outw(struct comedi_device *dev, uint16_t data,
 	unsigned long flags;
 
 	spin_lock_irqsave(&devpriv->window_lock, flags);
-	ni_writew(dev, addr, AO_Window_Address_611x);
-	ni_writew(dev, data, AO_Window_Data_611x);
+	ni_writew(dev, addr, NI611X_AO_WINDOW_ADDR_REG);
+	ni_writew(dev, data, NI611X_AO_WINDOW_DATA_REG);
 	spin_unlock_irqrestore(&devpriv->window_lock, flags);
 }
 
@@ -995,8 +891,8 @@ static inline void ni_ao_win_outl(struct comedi_device *dev, uint32_t data,
 	unsigned long flags;
 
 	spin_lock_irqsave(&devpriv->window_lock, flags);
-	ni_writew(dev, addr, AO_Window_Address_611x);
-	ni_writel(dev, data, AO_Window_Data_611x);
+	ni_writew(dev, addr, NI611X_AO_WINDOW_ADDR_REG);
+	ni_writel(dev, data, NI611X_AO_WINDOW_DATA_REG);
 	spin_unlock_irqrestore(&devpriv->window_lock, flags);
 }
 
@@ -1007,8 +903,8 @@ static inline unsigned short ni_ao_win_inw(struct comedi_device *dev, int addr)
 	unsigned short data;
 
 	spin_lock_irqsave(&devpriv->window_lock, flags);
-	ni_writew(dev, addr, AO_Window_Address_611x);
-	data = ni_readw(dev, AO_Window_Data_611x);
+	ni_writew(dev, addr, NI611X_AO_WINDOW_ADDR_REG);
+	data = ni_readw(dev, NI611X_AO_WINDOW_DATA_REG);
 	spin_unlock_irqrestore(&devpriv->window_lock, flags);
 	return data;
 }
@@ -1059,8 +955,8 @@ static int ni_ai_drain_dma(struct comedi_device *dev)
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ai_mite_chan) {
 		for (i = 0; i < timeout; i++) {
-			if ((ni_stc_readw(dev, AI_Status_1_Register) &
-			     AI_FIFO_Empty_St)
+			if ((ni_stc_readw(dev, NISTC_AI_STATUS1_REG) &
+			     NISTC_AI_STATUS1_FIFO_E)
 			    && mite_bytes_in_transit(devpriv->ai_mite_chan) ==
 			    0)
 				break;
@@ -1071,7 +967,7 @@ static int ni_ai_drain_dma(struct comedi_device *dev)
 			dev_err(dev->class_dev,
 				"mite_bytes_in_transit=%i, AI_Status1_Register=0x%x\n",
 				mite_bytes_in_transit(devpriv->ai_mite_chan),
-				ni_stc_readw(dev, AI_Status_1_Register));
+				ni_stc_readw(dev, NISTC_AI_STATUS1_REG));
 			retval = -1;
 		}
 	}
@@ -1103,8 +999,8 @@ static int ni_ao_wait_for_dma_load(struct comedi_device *dev)
 	for (i = 0; i < timeout; i++) {
 		unsigned short b_status;
 
-		b_status = ni_stc_readw(dev, AO_Status_1_Register);
-		if (b_status & AO_FIFO_Half_Full_St)
+		b_status = ni_stc_readw(dev, NISTC_AO_STATUS1_REG);
+		if (b_status & NISTC_AO_STATUS1_FIFO_HF)
 			break;
 		/* if we poll too often, the pci bus activity seems
 		   to slow the dma transfer down */
@@ -1139,9 +1035,9 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 				i++;
 				packed_data |= (d << 16) & 0xffff0000;
 			}
-			ni_writel(dev, packed_data, DAC_FIFO_Data_611x);
+			ni_writel(dev, packed_data, NI611X_AO_FIFO_DATA_REG);
 		} else {
-			ni_writew(dev, d, DAC_FIFO_Data);
+			ni_writew(dev, d, NI_E_AO_FIFO_DATA_REG);
 		}
 	}
 }
@@ -1193,9 +1089,9 @@ static int ni_ao_prep_fifo(struct comedi_device *dev,
 	unsigned int nsamples;
 
 	/* reset fifo */
-	ni_stc_writew(dev, 1, DAC_FIFO_Clear);
+	ni_stc_writew(dev, 1, NISTC_DAC_FIFO_CLR_REG);
 	if (devpriv->is_6xxx)
-		ni_ao_win_outl(dev, 0x6, AO_FIFO_Offset_Load_611x);
+		ni_ao_win_outl(dev, 0x6, NI611X_AO_FIFO_OFFSET_LOAD_REG);
 
 	/* load some data */
 	nbytes = comedi_buf_read_n_available(s);
@@ -1222,7 +1118,7 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 
 	if (devpriv->is_611x) {
 		for (i = 0; i < n / 2; i++) {
-			dl = ni_readl(dev, ADC_FIFO_Data_611x);
+			dl = ni_readl(dev, NI611X_AI_FIFO_DATA_REG);
 			/* This may get the hi/lo data in the wrong order */
 			data = (dl >> 16) & 0xffff;
 			comedi_buf_write_samples(s, &data, 1);
@@ -1231,14 +1127,14 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 		}
 		/* Check if there's a single sample stuck in the FIFO */
 		if (n % 2) {
-			dl = ni_readl(dev, ADC_FIFO_Data_611x);
+			dl = ni_readl(dev, NI611X_AI_FIFO_DATA_REG);
 			data = dl & 0xffff;
 			comedi_buf_write_samples(s, &data, 1);
 		}
 	} else if (devpriv->is_6143) {
 		/*  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);
+			dl = ni_readl(dev, NI6143_AI_FIFO_DATA_REG);
 
 			data = (dl >> 16) & 0xffff;
 			comedi_buf_write_samples(s, &data, 1);
@@ -1248,8 +1144,8 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 		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);
+			ni_writel(dev, 0x01, NI6143_AI_FIFO_CTRL_REG);
+			dl = ni_readl(dev, NI6143_AI_FIFO_DATA_REG);
 			data = (dl >> 16) & 0xffff;
 			comedi_buf_write_samples(s, &data, 1);
 		}
@@ -1263,7 +1159,7 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 		}
 		for (i = 0; i < n; i++) {
 			devpriv->ai_fifo_buffer[i] =
-			    ni_readw(dev, ADC_FIFO_Data_Register);
+			    ni_readw(dev, NI_E_AI_FIFO_DATA_REG);
 		}
 		comedi_buf_write_samples(s, devpriv->ai_fifo_buffer, n);
 	}
@@ -1294,9 +1190,9 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 	int i;
 
 	if (devpriv->is_611x) {
-		while ((ni_stc_readw(dev, AI_Status_1_Register) &
-			AI_FIFO_Empty_St) == 0) {
-			dl = ni_readl(dev, ADC_FIFO_Data_611x);
+		while ((ni_stc_readw(dev, NISTC_AI_STATUS1_REG) &
+			NISTC_AI_STATUS1_FIFO_E) == 0) {
+			dl = ni_readl(dev, NI611X_AI_FIFO_DATA_REG);
 
 			/* This may get the hi/lo data in the wrong order */
 			data = dl >> 16;
@@ -1306,8 +1202,8 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 		}
 	} else if (devpriv->is_6143) {
 		i = 0;
-		while (ni_readl(dev, AIFIFO_Status_6143) & 0x04) {
-			dl = ni_readl(dev, AIFIFO_Data_6143);
+		while (ni_readl(dev, NI6143_AI_FIFO_STATUS_REG) & 0x04) {
+			dl = ni_readl(dev, NI6143_AI_FIFO_DATA_REG);
 
 			/* This may get the hi/lo data in the wrong order */
 			data = dl >> 16;
@@ -1317,29 +1213,29 @@ static void ni_handle_fifo_dregs(struct comedi_device *dev)
 			i += 2;
 		}
 		/*  Check if stranded sample is present */
-		if (ni_readl(dev, AIFIFO_Status_6143) & 0x01) {
+		if (ni_readl(dev, NI6143_AI_FIFO_STATUS_REG) & 0x01) {
 			/* Get stranded sample into FIFO */
-			ni_writel(dev, 0x01, AIFIFO_Control_6143);
-			dl = ni_readl(dev, AIFIFO_Data_6143);
+			ni_writel(dev, 0x01, NI6143_AI_FIFO_CTRL_REG);
+			dl = ni_readl(dev, NI6143_AI_FIFO_DATA_REG);
 			data = (dl >> 16) & 0xffff;
 			comedi_buf_write_samples(s, &data, 1);
 		}
 
 	} else {
-		fifo_empty = ni_stc_readw(dev, AI_Status_1_Register) &
-			     AI_FIFO_Empty_St;
+		fifo_empty = ni_stc_readw(dev, NISTC_AI_STATUS1_REG) &
+			     NISTC_AI_STATUS1_FIFO_E;
 		while (fifo_empty == 0) {
 			for (i = 0;
 			     i <
 			     sizeof(devpriv->ai_fifo_buffer) /
 			     sizeof(devpriv->ai_fifo_buffer[0]); i++) {
 				fifo_empty = ni_stc_readw(dev,
-							  AI_Status_1_Register) &
-						AI_FIFO_Empty_St;
+							  NISTC_AI_STATUS1_REG) &
+						NISTC_AI_STATUS1_FIFO_E;
 				if (fifo_empty)
 					break;
 				devpriv->ai_fifo_buffer[i] =
-				    ni_readw(dev, ADC_FIFO_Data_Register);
+				    ni_readw(dev, NI_E_AI_FIFO_DATA_REG);
 			}
 			comedi_buf_write_samples(s, devpriv->ai_fifo_buffer, i);
 		}
@@ -1357,8 +1253,8 @@ static void get_last_sample_611x(struct comedi_device *dev)
 		return;
 
 	/* Check if there's a single sample stuck in the FIFO */
-	if (ni_readb(dev, XXX_Status) & 0x80) {
-		dl = ni_readl(dev, ADC_FIFO_Data_611x);
+	if (ni_readb(dev, NI_E_STATUS_REG) & 0x80) {
+		dl = ni_readl(dev, NI611X_AI_FIFO_DATA_REG);
 		data = dl & 0xffff;
 		comedi_buf_write_samples(s, &data, 1);
 	}
@@ -1375,10 +1271,10 @@ static void get_last_sample_6143(struct comedi_device *dev)
 		return;
 
 	/* Check if there's a single sample stuck in the FIFO */
-	if (ni_readl(dev, AIFIFO_Status_6143) & 0x01) {
+	if (ni_readl(dev, NI6143_AI_FIFO_STATUS_REG) & 0x01) {
 		/* Get stranded sample into FIFO */
-		ni_writel(dev, 0x01, AIFIFO_Control_6143);
-		dl = ni_readl(dev, AIFIFO_Data_6143);
+		ni_writel(dev, 0x01, NI6143_AI_FIFO_CTRL_REG);
+		dl = ni_readl(dev, NI6143_AI_FIFO_DATA_REG);
 
 		/* This may get the hi/lo data in the wrong order */
 		data = (dl >> 16) & 0xffff;
@@ -1420,8 +1316,8 @@ static void ni_handle_eos(struct comedi_device *dev, struct comedi_subdevice *s)
 		s->async->events |= COMEDI_CB_EOS;
 #endif
 	}
-	/* handle special case of single scan using AI_End_On_End_Of_Scan */
-	if ((devpriv->ai_cmd2 & AI_End_On_End_Of_Scan))
+	/* handle special case of single scan */
+	if (devpriv->ai_cmd2 & NISTC_AI_CMD2_END_ON_EOS)
 		shutdown_ai_command(dev);
 }
 
@@ -1444,17 +1340,16 @@ static void ack_a_interrupt(struct comedi_device *dev, unsigned short a_status)
 {
 	unsigned short ack = 0;
 
-	if (a_status & AI_SC_TC_St)
-		ack |= AI_SC_TC_Interrupt_Ack;
-	if (a_status & AI_START1_St)
-		ack |= AI_START1_Interrupt_Ack;
-	if (a_status & AI_START_St)
-		ack |= AI_START_Interrupt_Ack;
-	if (a_status & AI_STOP_St)
-		/* not sure why we used to ack the START here also, instead of doing it independently. Frank Hess 2007-07-06 */
-		ack |= AI_STOP_Interrupt_Ack /*| AI_START_Interrupt_Ack */;
+	if (a_status & NISTC_AI_STATUS1_SC_TC)
+		ack |= NISTC_INTA_ACK_AI_SC_TC;
+	if (a_status & NISTC_AI_STATUS1_START1)
+		ack |= NISTC_INTA_ACK_AI_START1;
+	if (a_status & NISTC_AI_STATUS1_START)
+		ack |= NISTC_INTA_ACK_AI_START;
+	if (a_status & NISTC_AI_STATUS1_STOP)
+		ack |= NISTC_INTA_ACK_AI_STOP;
 	if (ack)
-		ni_stc_writew(dev, ack, Interrupt_A_Ack_Register);
+		ni_stc_writew(dev, ack, NISTC_INTA_ACK_REG);
 }
 
 static void handle_a_interrupt(struct comedi_device *dev, unsigned short status,
@@ -1483,8 +1378,8 @@ static void handle_a_interrupt(struct comedi_device *dev, unsigned short status,
 #endif
 
 	/* test for all uncommon interrupt events at the same time */
-	if (status & (AI_Overrun_St | AI_Overflow_St | AI_SC_TC_Error_St |
-		      AI_SC_TC_St | AI_START1_St)) {
+	if (status & (NISTC_AI_STATUS1_ERR |
+		      NISTC_AI_STATUS1_SC_TC | NISTC_AI_STATUS1_START1)) {
 		if (status == 0xffff) {
 			dev_err(dev->class_dev, "Card removed?\n");
 			/* we probably aren't even running a command now,
@@ -1495,41 +1390,40 @@ static void handle_a_interrupt(struct comedi_device *dev, unsigned short status,
 			}
 			return;
 		}
-		if (status & (AI_Overrun_St | AI_Overflow_St |
-			      AI_SC_TC_Error_St)) {
+		if (status & NISTC_AI_STATUS1_ERR) {
 			dev_err(dev->class_dev, "ai error a_status=%04x\n",
 				status);
 
 			shutdown_ai_command(dev);
 
 			s->async->events |= COMEDI_CB_ERROR;
-			if (status & (AI_Overrun_St | AI_Overflow_St))
+			if (status & NISTC_AI_STATUS1_OVER)
 				s->async->events |= COMEDI_CB_OVERFLOW;
 
 			comedi_handle_events(dev, s);
 			return;
 		}
-		if (status & AI_SC_TC_St) {
+		if (status & NISTC_AI_STATUS1_SC_TC) {
 			if (cmd->stop_src == TRIG_COUNT)
 				shutdown_ai_command(dev);
 		}
 	}
 #ifndef PCIDMA
-	if (status & AI_FIFO_Half_Full_St) {
+	if (status & NISTC_AI_STATUS1_FIFO_HF) {
 		int i;
 		static const int timeout = 10;
 		/* pcmcia cards (at least 6036) seem to stop producing interrupts if we
 		 *fail to get the fifo less than half full, so loop to be sure.*/
 		for (i = 0; i < timeout; ++i) {
 			ni_handle_fifo_half_full(dev);
-			if ((ni_stc_readw(dev, AI_Status_1_Register) &
-			     AI_FIFO_Half_Full_St) == 0)
+			if ((ni_stc_readw(dev, NISTC_AI_STATUS1_REG) &
+			     NISTC_AI_STATUS1_FIFO_HF) == 0)
 				break;
 		}
 	}
 #endif /*  !PCIDMA */
 
-	if ((status & AI_STOP_St))
+	if (status & NISTC_AI_STATUS1_STOP)
 		ni_handle_eos(dev, s);
 
 	comedi_handle_events(dev, s);
@@ -1539,22 +1433,22 @@ static void ack_b_interrupt(struct comedi_device *dev, unsigned short b_status)
 {
 	unsigned short ack = 0;
 
-	if (b_status & AO_BC_TC_St)
-		ack |= AO_BC_TC_Interrupt_Ack;
-	if (b_status & AO_Overrun_St)
-		ack |= AO_Error_Interrupt_Ack;
-	if (b_status & AO_START_St)
-		ack |= AO_START_Interrupt_Ack;
-	if (b_status & AO_START1_St)
-		ack |= AO_START1_Interrupt_Ack;
-	if (b_status & AO_UC_TC_St)
-		ack |= AO_UC_TC_Interrupt_Ack;
-	if (b_status & AO_UI2_TC_St)
-		ack |= AO_UI2_TC_Interrupt_Ack;
-	if (b_status & AO_UPDATE_St)
-		ack |= AO_UPDATE_Interrupt_Ack;
+	if (b_status & NISTC_AO_STATUS1_BC_TC)
+		ack |= NISTC_INTB_ACK_AO_BC_TC;
+	if (b_status & NISTC_AO_STATUS1_OVERRUN)
+		ack |= NISTC_INTB_ACK_AO_ERR;
+	if (b_status & NISTC_AO_STATUS1_START)
+		ack |= NISTC_INTB_ACK_AO_START;
+	if (b_status & NISTC_AO_STATUS1_START1)
+		ack |= NISTC_INTB_ACK_AO_START1;
+	if (b_status & NISTC_AO_STATUS1_UC_TC)
+		ack |= NISTC_INTB_ACK_AO_UC_TC;
+	if (b_status & NISTC_AO_STATUS1_UI2_TC)
+		ack |= NISTC_INTB_ACK_AO_UI2_TC;
+	if (b_status & NISTC_AO_STATUS1_UPDATE)
+		ack |= NISTC_INTB_ACK_AO_UPDATE;
 	if (ack)
-		ni_stc_writew(dev, ack, Interrupt_B_Ack_Register);
+		ni_stc_writew(dev, ack, NISTC_INTB_ACK_REG);
 }
 
 static void handle_b_interrupt(struct comedi_device *dev,
@@ -1583,26 +1477,26 @@ static void handle_b_interrupt(struct comedi_device *dev,
 
 	if (b_status == 0xffff)
 		return;
-	if (b_status & AO_Overrun_St) {
+	if (b_status & NISTC_AO_STATUS1_OVERRUN) {
 		dev_err(dev->class_dev,
 			"AO FIFO underrun status=0x%04x status2=0x%04x\n",
-			b_status, ni_stc_readw(dev, AO_Status_2_Register));
+			b_status, ni_stc_readw(dev, NISTC_AO_STATUS2_REG));
 		s->async->events |= COMEDI_CB_OVERFLOW;
 	}
 
-	if (b_status & AO_BC_TC_St)
+	if (b_status & NISTC_AO_STATUS1_BC_TC)
 		s->async->events |= COMEDI_CB_EOA;
 
 #ifndef PCIDMA
-	if (b_status & AO_FIFO_Request_St) {
+	if (b_status & NISTC_AO_STATUS1_FIFO_REQ) {
 		int ret;
 
 		ret = ni_ao_fifo_half_empty(dev, s);
 		if (!ret) {
 			dev_err(dev->class_dev, "AO buffer underrun\n");
-			ni_set_bits(dev, Interrupt_B_Enable_Register,
-				    AO_FIFO_Interrupt_Enable |
-				    AO_Error_Interrupt_Enable, 0);
+			ni_set_bits(dev, NISTC_INTB_ENA_REG,
+				    NISTC_INTB_ENA_AO_FIFO |
+				    NISTC_INTB_ENA_AO_ERR, 0);
 			s->async->events |= COMEDI_CB_OVERFLOW;
 		}
 	}
@@ -1718,101 +1612,65 @@ static int ni_ao_setup_MITE_dma(struct comedi_device *dev)
 static int ni_ai_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct ni_private *devpriv = dev->private;
+	unsigned ai_personal;
+	unsigned ai_out_ctrl;
 
 	ni_release_ai_mite_channel(dev);
 	/* ai configuration */
-	ni_stc_writew(dev, AI_Configuration_Start | AI_Reset,
-		      Joint_Reset_Register);
+	ni_stc_writew(dev, NISTC_RESET_AI_CFG_START | NISTC_RESET_AI,
+		      NISTC_RESET_REG);
 
-	ni_set_bits(dev, Interrupt_A_Enable_Register,
-		    AI_SC_TC_Interrupt_Enable | AI_START1_Interrupt_Enable |
-		    AI_START2_Interrupt_Enable | AI_START_Interrupt_Enable |
-		    AI_STOP_Interrupt_Enable | AI_Error_Interrupt_Enable |
-		    AI_FIFO_Interrupt_Enable, 0);
+	ni_set_bits(dev, NISTC_INTA_ENA_REG, NISTC_INTA_ENA_AI_MASK, 0);
 
 	ni_clear_ai_fifo(dev);
 
 	if (!devpriv->is_6143)
-		ni_writeb(dev, 0, Misc_Command);
-
-	ni_stc_writew(dev, AI_Disarm, AI_Command_1_Register); /* reset pulses */
-	ni_stc_writew(dev, AI_Start_Stop | AI_Mode_1_Reserved
-			    /*| AI_Trigger_Once */,
-		      AI_Mode_1_Register);
-	ni_stc_writew(dev, 0x0000, AI_Mode_2_Register);
+		ni_writeb(dev, NI_E_MISC_CMD_EXT_ATRIG, NI_E_MISC_CMD_REG);
+
+	ni_stc_writew(dev, NISTC_AI_CMD1_DISARM, NISTC_AI_CMD1_REG);
+	ni_stc_writew(dev, NISTC_AI_MODE1_START_STOP |
+			   NISTC_AI_MODE1_RSVD
+			    /*| NISTC_AI_MODE1_TRIGGER_ONCE */,
+		      NISTC_AI_MODE1_REG);
+	ni_stc_writew(dev, 0, NISTC_AI_MODE2_REG);
 	/* generate FIFO interrupts on non-empty */
-	ni_stc_writew(dev, (0 << 6) | 0x0000, AI_Mode_3_Register);
+	ni_stc_writew(dev, NISTC_AI_MODE3_FIFO_MODE_NE,
+		      NISTC_AI_MODE3_REG);
+
+	ai_personal = NISTC_AI_PERSONAL_SHIFTIN_PW |
+		      NISTC_AI_PERSONAL_SOC_POLARITY |
+		      NISTC_AI_PERSONAL_LOCALMUX_CLK_PW;
+	ai_out_ctrl = NISTC_AI_OUT_CTRL_SCAN_IN_PROG_SEL(3) |
+		      NISTC_AI_OUT_CTRL_EXTMUX_CLK_SEL(0) |
+		      NISTC_AI_OUT_CTRL_LOCALMUX_CLK_SEL(2) |
+		      NISTC_AI_OUT_CTRL_SC_TC_SEL(3);
 	if (devpriv->is_611x) {
-		ni_stc_writew(dev,
-			      AI_SHIFTIN_Pulse_Width |
-			      AI_SOC_Polarity |
-			      AI_LOCALMUX_CLK_Pulse_Width,
-			      AI_Personal_Register);
-		ni_stc_writew(dev,
-			      AI_SCAN_IN_PROG_Output_Select(3) |
-			      AI_EXTMUX_CLK_Output_Select(0) |
-			      AI_LOCALMUX_CLK_Output_Select(2) |
-			      AI_SC_TC_Output_Select(3) |
-			      AI_CONVERT_Output_Select
-			      (AI_CONVERT_Output_Enable_High),
-			      AI_Output_Control_Register);
+		ai_out_ctrl |= NISTC_AI_OUT_CTRL_CONVERT_HIGH;
 	} else if (devpriv->is_6143) {
-		ni_stc_writew(dev, AI_SHIFTIN_Pulse_Width |
-				   AI_SOC_Polarity |
-				   AI_LOCALMUX_CLK_Pulse_Width,
-			      AI_Personal_Register);
-		ni_stc_writew(dev,
-			      AI_SCAN_IN_PROG_Output_Select(3) |
-			      AI_EXTMUX_CLK_Output_Select(0) |
-			      AI_LOCALMUX_CLK_Output_Select(2) |
-			      AI_SC_TC_Output_Select(3) |
-			      AI_CONVERT_Output_Select
-			      (AI_CONVERT_Output_Enable_Low),
-			      AI_Output_Control_Register);
+		ai_out_ctrl |= NISTC_AI_OUT_CTRL_CONVERT_LOW;
 	} else {
-		unsigned ai_output_control_bits;
-
-		ni_stc_writew(dev,
-			      AI_SHIFTIN_Pulse_Width |
-			      AI_SOC_Polarity |
-			      AI_CONVERT_Pulse_Width |
-			      AI_LOCALMUX_CLK_Pulse_Width,
-			      AI_Personal_Register);
-		ai_output_control_bits =
-		    AI_SCAN_IN_PROG_Output_Select(3) |
-		    AI_EXTMUX_CLK_Output_Select(0) |
-		    AI_LOCALMUX_CLK_Output_Select(2) |
-		    AI_SC_TC_Output_Select(3);
+		ai_personal |= NISTC_AI_PERSONAL_CONVERT_PW;
 		if (devpriv->is_622x)
-			ai_output_control_bits |=
-			    AI_CONVERT_Output_Select
-			    (AI_CONVERT_Output_Enable_High);
+			ai_out_ctrl |= NISTC_AI_OUT_CTRL_CONVERT_HIGH;
 		else
-			ai_output_control_bits |=
-			    AI_CONVERT_Output_Select
-			    (AI_CONVERT_Output_Enable_Low);
-		ni_stc_writew(dev, ai_output_control_bits,
-			      AI_Output_Control_Register);
+			ai_out_ctrl |= NISTC_AI_OUT_CTRL_CONVERT_LOW;
 	}
+	ni_stc_writew(dev, ai_personal, NISTC_AI_PERSONAL_REG);
+	ni_stc_writew(dev, ai_out_ctrl, NISTC_AI_OUT_CTRL_REG);
+
 	/* the following registers should not be changed, because there
 	 * are no backup registers in devpriv.  If you want to change
 	 * any of these, add a backup register and other appropriate code:
-	 *      AI_Mode_1_Register
-	 *      AI_Mode_3_Register
-	 *      AI_Personal_Register
-	 *      AI_Output_Control_Register
+	 *      NISTC_AI_MODE1_REG
+	 *      NISTC_AI_MODE3_REG
+	 *      NISTC_AI_PERSONAL_REG
+	 *      NISTC_AI_OUT_CTRL_REG
 	 */
-	ni_stc_writew(dev,
-		      AI_SC_TC_Error_Confirm |
-		      AI_START_Interrupt_Ack |
-		      AI_START2_Interrupt_Ack |
-		      AI_START1_Interrupt_Ack |
-		      AI_SC_TC_Interrupt_Ack |
-		      AI_Error_Interrupt_Ack |
-		      AI_STOP_Interrupt_Ack,
-		      Interrupt_A_Ack_Register);	/* clear interrupts */
-
-	ni_stc_writew(dev, AI_Configuration_End, Joint_Reset_Register);
+
+	/* clear interrupts */
+	ni_stc_writew(dev, NISTC_INTA_ACK_AI_ALL, NISTC_INTA_ACK_REG);
+
+	ni_stc_writew(dev, NISTC_RESET_AI_CFG_END, NISTC_RESET_REG);
 
 	return 0;
 }
@@ -1839,11 +1697,11 @@ static void ni_prime_channelgain_list(struct comedi_device *dev)
 {
 	int i;
 
-	ni_stc_writew(dev, AI_CONVERT_Pulse, AI_Command_1_Register);
+	ni_stc_writew(dev, NISTC_AI_CMD1_CONVERT_PULSE, NISTC_AI_CMD1_REG);
 	for (i = 0; i < NI_TIMEOUT; ++i) {
-		if (!(ni_stc_readw(dev, AI_Status_1_Register) &
-		      AI_FIFO_Empty_St)) {
-			ni_stc_writew(dev, 1, ADC_FIFO_Clear);
+		if (!(ni_stc_readw(dev, NISTC_AI_STATUS1_REG) &
+		      NISTC_AI_STATUS1_FIFO_E)) {
+			ni_stc_writew(dev, 1, NISTC_ADC_FIFO_CLR_REG);
 			return;
 		}
 		udelay(1);
@@ -1862,7 +1720,7 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 	unsigned int dither;
 	unsigned range_code;
 
-	ni_stc_writew(dev, 1, Configuration_Memory_Clear);
+	ni_stc_writew(dev, 1, NISTC_CFG_MEM_CLR_REG);
 
 	if ((list[0] & CR_ALT_SOURCE)) {
 		unsigned bypass_bits;
@@ -1871,22 +1729,17 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 		range = CR_RANGE(list[0]);
 		range_code = ni_gainlkup[board->gainlkup][range];
 		dither = (list[0] & CR_ALT_FILTER) != 0;
-		bypass_bits = MSeries_AI_Bypass_Config_FIFO_Bit;
-		bypass_bits |= chan;
-		bypass_bits |=
-		    (devpriv->ai_calib_source) &
-		    (MSeries_AI_Bypass_Cal_Sel_Pos_Mask |
-		     MSeries_AI_Bypass_Cal_Sel_Neg_Mask |
-		     MSeries_AI_Bypass_Mode_Mux_Mask |
-		     MSeries_AO_Bypass_AO_Cal_Sel_Mask);
-		bypass_bits |= MSeries_AI_Bypass_Gain_Bits(range_code);
+		bypass_bits = NI_M_CFG_BYPASS_FIFO |
+			      NI_M_CFG_BYPASS_AI_CHAN(chan) |
+			      NI_M_CFG_BYPASS_AI_GAIN(range_code) |
+			      devpriv->ai_calib_source;
 		if (dither)
-			bypass_bits |= MSeries_AI_Bypass_Dither_Bit;
+			bypass_bits |= NI_M_CFG_BYPASS_AI_DITHER;
 		/*  don't use 2's complement encoding */
-		bypass_bits |= MSeries_AI_Bypass_Polarity_Bit;
-		ni_writel(dev, bypass_bits, M_Offset_AI_Config_FIFO_Bypass);
+		bypass_bits |= NI_M_CFG_BYPASS_AI_POLARITY;
+		ni_writel(dev, bypass_bits, NI_M_CFG_BYPASS_FIFO_REG);
 	} else {
-		ni_writel(dev, 0, M_Offset_AI_Config_FIFO_Bypass);
+		ni_writel(dev, 0, NI_M_CFG_BYPASS_FIFO_REG);
 	}
 	for (i = 0; i < n_chan; i++) {
 		unsigned config_bits = 0;
@@ -1900,31 +1753,27 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 		devpriv->ai_offset[i] = 0;
 		switch (aref) {
 		case AREF_DIFF:
-			config_bits |=
-			    MSeries_AI_Config_Channel_Type_Differential_Bits;
+			config_bits |= NI_M_AI_CFG_CHAN_TYPE_DIFF;
 			break;
 		case AREF_COMMON:
-			config_bits |=
-			    MSeries_AI_Config_Channel_Type_Common_Ref_Bits;
+			config_bits |= NI_M_AI_CFG_CHAN_TYPE_COMMON;
 			break;
 		case AREF_GROUND:
-			config_bits |=
-			    MSeries_AI_Config_Channel_Type_Ground_Ref_Bits;
+			config_bits |= NI_M_AI_CFG_CHAN_TYPE_GROUND;
 			break;
 		case AREF_OTHER:
 			break;
 		}
-		config_bits |= MSeries_AI_Config_Channel_Bits(chan);
-		config_bits |=
-		    MSeries_AI_Config_Bank_Bits(board->reg_type, chan);
-		config_bits |= MSeries_AI_Config_Gain_Bits(range_code);
+		config_bits |= NI_M_AI_CFG_CHAN_SEL(chan);
+		config_bits |= NI_M_AI_CFG_BANK_SEL(chan);
+		config_bits |= NI_M_AI_CFG_GAIN(range_code);
 		if (i == n_chan - 1)
-			config_bits |= MSeries_AI_Config_Last_Channel_Bit;
+			config_bits |= NI_M_AI_CFG_LAST_CHAN;
 		if (dither)
-			config_bits |= MSeries_AI_Config_Dither_Bit;
+			config_bits |= NI_M_AI_CFG_DITHER;
 		/*  don't use 2's complement encoding */
-		config_bits |= MSeries_AI_Config_Polarity_Bit;
-		ni_writew(dev, config_bits, M_Offset_AI_Config_FIFO_Data);
+		config_bits |= NI_M_AI_CFG_POLARITY;
+		ni_writew(dev, config_bits, NI_M_AI_CFG_FIFO_DATA_REG);
 	}
 	ni_prime_channelgain_list(dev);
 }
@@ -1986,7 +1835,7 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 		devpriv->changain_state = 0;
 	}
 
-	ni_stc_writew(dev, 1, Configuration_Memory_Clear);
+	ni_stc_writew(dev, 1, NISTC_CFG_MEM_CLR_REG);
 
 	/*  Set up Calibration mode if required */
 	if (devpriv->is_6143) {
@@ -1994,20 +1843,20 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 		    && !devpriv->ai_calib_source_enabled) {
 			/*  Strobe Relay enable bit */
 			ni_writew(dev, devpriv->ai_calib_source |
-				       Calibration_Channel_6143_RelayOn,
-				  Calibration_Channel_6143);
+				       NI6143_CALIB_CHAN_RELAY_ON,
+				  NI6143_CALIB_CHAN_REG);
 			ni_writew(dev, devpriv->ai_calib_source,
-				  Calibration_Channel_6143);
+				  NI6143_CALIB_CHAN_REG);
 			devpriv->ai_calib_source_enabled = 1;
 			msleep_interruptible(100);	/*  Allow relays to change */
 		} else if (!(list[0] & CR_ALT_SOURCE)
 			   && devpriv->ai_calib_source_enabled) {
 			/*  Strobe Relay disable bit */
 			ni_writew(dev, devpriv->ai_calib_source |
-				       Calibration_Channel_6143_RelayOff,
-				  Calibration_Channel_6143);
+				       NI6143_CALIB_CHAN_RELAY_OFF,
+				  NI6143_CALIB_CHAN_REG);
 			ni_writew(dev, devpriv->ai_calib_source,
-				  Calibration_Channel_6143);
+				  NI6143_CALIB_CHAN_REG);
 			devpriv->ai_calib_source_enabled = 0;
 			msleep_interruptible(100);	/*  Allow relays to change */
 		}
@@ -2033,7 +1882,7 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 		if ((list[i] & CR_ALT_SOURCE)) {
 			if (devpriv->is_611x)
 				ni_writew(dev, CR_CHAN(list[i]) & 0x0003,
-					  Calibration_Channel_Select_611x);
+					  NI611X_CALIB_CHAN_SEL_REG);
 		} else {
 			if (devpriv->is_611x)
 				aref = AREF_DIFF;
@@ -2041,30 +1890,31 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 				aref = AREF_OTHER;
 			switch (aref) {
 			case AREF_DIFF:
-				hi |= AI_DIFFERENTIAL;
+				hi |= NI_E_AI_CFG_HI_TYPE_DIFF;
 				break;
 			case AREF_COMMON:
-				hi |= AI_COMMON;
+				hi |= NI_E_AI_CFG_HI_TYPE_COMMON;
 				break;
 			case AREF_GROUND:
-				hi |= AI_GROUND;
+				hi |= NI_E_AI_CFG_HI_TYPE_GROUND;
 				break;
 			case AREF_OTHER:
 				break;
 			}
 		}
-		hi |= AI_CONFIG_CHANNEL(chan);
+		hi |= NI_E_AI_CFG_HI_CHAN(chan);
 
-		ni_writew(dev, hi, Configuration_Memory_High);
+		ni_writew(dev, hi, NI_E_AI_CFG_HI_REG);
 
 		if (!devpriv->is_6143) {
-			lo = range;
+			lo = NI_E_AI_CFG_LO_GAIN(range);
+
 			if (i == n_chan - 1)
-				lo |= AI_LAST_CHANNEL;
+				lo |= NI_E_AI_CFG_LO_LAST_CHAN;
 			if (dither)
-				lo |= AI_DITHER;
+				lo |= NI_E_AI_CFG_LO_DITHER;
 
-			ni_writew(dev, lo, Configuration_Memory_Low);
+			ni_writew(dev, lo, NI_E_AI_CFG_LO_REG);
 		}
 	}
 
@@ -2092,25 +1942,27 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 	signbits = devpriv->ai_offset[0];
 	if (devpriv->is_611x) {
 		for (n = 0; n < num_adc_stages_611x; n++) {
-			ni_stc_writew(dev, AI_CONVERT_Pulse,
-				      AI_Command_1_Register);
+			ni_stc_writew(dev, NISTC_AI_CMD1_CONVERT_PULSE,
+				      NISTC_AI_CMD1_REG);
 			udelay(1);
 		}
 		for (n = 0; n < insn->n; n++) {
-			ni_stc_writew(dev, AI_CONVERT_Pulse,
-				      AI_Command_1_Register);
+			ni_stc_writew(dev, NISTC_AI_CMD1_CONVERT_PULSE,
+				      NISTC_AI_CMD1_REG);
 			/* The 611x has screwy 32-bit FIFOs. */
 			d = 0;
 			for (i = 0; i < NI_TIMEOUT; i++) {
-				if (ni_readb(dev, XXX_Status) & 0x80) {
-					d = ni_readl(dev, ADC_FIFO_Data_611x);
+				if (ni_readb(dev, NI_E_STATUS_REG) & 0x80) {
+					d = ni_readl(dev,
+						     NI611X_AI_FIFO_DATA_REG);
 					d >>= 16;
 					d &= 0xffff;
 					break;
 				}
-				if (!(ni_stc_readw(dev, AI_Status_1_Register) &
-				      AI_FIFO_Empty_St)) {
-					d = ni_readl(dev, ADC_FIFO_Data_611x);
+				if (!(ni_stc_readw(dev, NISTC_AI_STATUS1_REG) &
+				      NISTC_AI_STATUS1_FIFO_E)) {
+					d = ni_readl(dev,
+						     NI611X_AI_FIFO_DATA_REG);
 					d &= 0xffff;
 					break;
 				}
@@ -2124,17 +1976,19 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 		}
 	} else if (devpriv->is_6143) {
 		for (n = 0; n < insn->n; n++) {
-			ni_stc_writew(dev, AI_CONVERT_Pulse,
-				      AI_Command_1_Register);
+			ni_stc_writew(dev, NISTC_AI_CMD1_CONVERT_PULSE,
+				      NISTC_AI_CMD1_REG);
 
 			/* The 6143 has 32-bit FIFOs. You need to strobe a bit to move a single 16bit stranded sample into the FIFO */
 			dl = 0;
 			for (i = 0; i < NI_TIMEOUT; i++) {
-				if (ni_readl(dev, AIFIFO_Status_6143) & 0x01) {
+				if (ni_readl(dev, NI6143_AI_FIFO_STATUS_REG) &
+				    0x01) {
 					/* Get stranded sample into FIFO */
 					ni_writel(dev, 0x01,
-						  AIFIFO_Control_6143);
-					dl = ni_readl(dev, AIFIFO_Data_6143);
+						  NI6143_AI_FIFO_CTRL_REG);
+					dl = ni_readl(dev,
+						      NI6143_AI_FIFO_DATA_REG);
 					break;
 				}
 			}
@@ -2146,11 +2000,11 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 		}
 	} else {
 		for (n = 0; n < insn->n; n++) {
-			ni_stc_writew(dev, AI_CONVERT_Pulse,
-				      AI_Command_1_Register);
+			ni_stc_writew(dev, NISTC_AI_CMD1_CONVERT_PULSE,
+				      NISTC_AI_CMD1_REG);
 			for (i = 0; i < NI_TIMEOUT; i++) {
-				if (!(ni_stc_readw(dev, AI_Status_1_Register) &
-				      AI_FIFO_Empty_St))
+				if (!(ni_stc_readw(dev, NISTC_AI_STATUS1_REG) &
+				      NISTC_AI_STATUS1_FIFO_E))
 					break;
 			}
 			if (i == NI_TIMEOUT) {
@@ -2158,11 +2012,11 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 				return -ETIME;
 			}
 			if (devpriv->is_m_series) {
-				dl = ni_readl(dev, M_Offset_AI_FIFO_Data);
+				dl = ni_readl(dev, NI_M_AI_FIFO_DATA_REG);
 				dl &= mask;
 				data[n] = dl;
 			} else {
-				d = ni_readw(dev, ADC_FIFO_Data_Register);
+				d = ni_readw(dev, NI_E_AI_FIFO_DATA_REG);
 				d += signbits;	/* subtle: needs to be short addition */
 				data[n] = d;
 			}
@@ -2374,8 +2228,8 @@ static int ni_ai_inttrig(struct comedi_device *dev,
 	if (trig_num != cmd->start_arg)
 		return -EINVAL;
 
-	ni_stc_writew(dev, AI_START1_Pulse | devpriv->ai_cmd2,
-		      AI_Command_2_Register);
+	ni_stc_writew(dev, NISTC_AI_CMD2_START1_PULSE | devpriv->ai_cmd2,
+		      NISTC_AI_CMD2_REG);
 	s->async->inttrig = NULL;
 
 	return 1;
@@ -2391,6 +2245,7 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	int start_stop_select = 0;
 	unsigned int stop_count;
 	int interrupt_a_enable = 0;
+	unsigned ai_trig;
 
 	if (dev->irq == 0) {
 		dev_err(dev->class_dev, "cannot run command without an irq\n");
@@ -2401,51 +2256,47 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	ni_load_channelgain_list(dev, s, cmd->chanlist_len, cmd->chanlist);
 
 	/* start configuration */
-	ni_stc_writew(dev, AI_Configuration_Start, Joint_Reset_Register);
+	ni_stc_writew(dev, NISTC_RESET_AI_CFG_START, NISTC_RESET_REG);
 
 	/* disable analog triggering for now, since it
 	 * interferes with the use of pfi0 */
-	devpriv->an_trig_etc_reg &= ~Analog_Trigger_Enable;
-	ni_stc_writew(dev, devpriv->an_trig_etc_reg,
-		      Analog_Trigger_Etc_Register);
+	devpriv->an_trig_etc_reg &= ~NISTC_ATRIG_ETC_ENA;
+	ni_stc_writew(dev, devpriv->an_trig_etc_reg, NISTC_ATRIG_ETC_REG);
 
+	ai_trig = NISTC_AI_TRIG_START2_SEL(0) | NISTC_AI_TRIG_START1_SYNC;
 	switch (cmd->start_src) {
 	case TRIG_INT:
 	case TRIG_NOW:
-		ni_stc_writew(dev,
-			      AI_START2_Select(0) |
-			      AI_START1_Sync | AI_START1_Edge |
-			      AI_START1_Select(0),
-			      AI_Trigger_Select_Register);
+		ai_trig |= NISTC_AI_TRIG_START1_EDGE |
+			   NISTC_AI_TRIG_START1_SEL(0);
 		break;
 	case TRIG_EXT:
-		{
-			int chan = CR_CHAN(cmd->start_arg);
-			unsigned int bits = AI_START2_Select(0) |
-			    AI_START1_Sync | AI_START1_Select(chan + 1);
-
-			if (cmd->start_arg & CR_INVERT)
-				bits |= AI_START1_Polarity;
-			if (cmd->start_arg & CR_EDGE)
-				bits |= AI_START1_Edge;
-			ni_stc_writew(dev, bits, AI_Trigger_Select_Register);
-			break;
-		}
+		ai_trig |= NISTC_AI_TRIG_START1_SEL(CR_CHAN(cmd->start_arg) +
+						    1);
+
+		if (cmd->start_arg & CR_INVERT)
+			ai_trig |= NISTC_AI_TRIG_START1_POLARITY;
+		if (cmd->start_arg & CR_EDGE)
+			ai_trig |= NISTC_AI_TRIG_START1_EDGE;
+		break;
 	}
+	ni_stc_writew(dev, ai_trig, NISTC_AI_TRIG_SEL_REG);
 
-	mode2 &= ~AI_Pre_Trigger;
-	mode2 &= ~AI_SC_Initial_Load_Source;
-	mode2 &= ~AI_SC_Reload_Mode;
-	ni_stc_writew(dev, mode2, AI_Mode_2_Register);
+	mode2 &= ~NISTC_AI_MODE2_PRE_TRIGGER;
+	mode2 &= ~NISTC_AI_MODE2_SC_INIT_LOAD_SRC;
+	mode2 &= ~NISTC_AI_MODE2_SC_RELOAD_MODE;
+	ni_stc_writew(dev, mode2, NISTC_AI_MODE2_REG);
 
 	if (cmd->chanlist_len == 1 || devpriv->is_611x || devpriv->is_6143) {
-		start_stop_select |= AI_STOP_Polarity;
-		start_stop_select |= AI_STOP_Select(31);	/*  logic low */
-		start_stop_select |= AI_STOP_Sync;
+		/* logic low */
+		start_stop_select |= NISTC_AI_STOP_POLARITY |
+				     NISTC_AI_STOP_SEL(31) |
+				     NISTC_AI_STOP_SYNC;
 	} else {
-		start_stop_select |= AI_STOP_Select(19);	/*  ai configuration memory */
+		/*  ai configuration memory */
+		start_stop_select |= NISTC_AI_STOP_SEL(19);
 	}
-	ni_stc_writew(dev, start_stop_select, AI_START_STOP_Select_Register);
+	ni_stc_writew(dev, start_stop_select, NISTC_AI_START_STOP_REG);
 
 	devpriv->ai_cmd2 = 0;
 	switch (cmd->stop_src) {
@@ -2457,80 +2308,80 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			stop_count += num_adc_stages_611x;
 		}
 		/* stage number of scans */
-		ni_stc_writel(dev, stop_count, AI_SC_Load_A_Registers);
+		ni_stc_writel(dev, stop_count, NISTC_AI_SC_LOADA_REG);
 
-		mode1 |= AI_Start_Stop | AI_Mode_1_Reserved | AI_Trigger_Once;
-		ni_stc_writew(dev, mode1, AI_Mode_1_Register);
+		mode1 |= NISTC_AI_MODE1_START_STOP |
+			 NISTC_AI_MODE1_RSVD |
+			 NISTC_AI_MODE1_TRIGGER_ONCE;
+		ni_stc_writew(dev, mode1, NISTC_AI_MODE1_REG);
 		/* load SC (Scan Count) */
-		ni_stc_writew(dev, AI_SC_Load, AI_Command_1_Register);
+		ni_stc_writew(dev, NISTC_AI_CMD1_SC_LOAD, NISTC_AI_CMD1_REG);
 
 		if (stop_count == 0) {
-			devpriv->ai_cmd2 |= AI_End_On_End_Of_Scan;
-			interrupt_a_enable |= AI_STOP_Interrupt_Enable;
+			devpriv->ai_cmd2 |= NISTC_AI_CMD2_END_ON_EOS;
+			interrupt_a_enable |= NISTC_INTA_ENA_AI_STOP;
 			/*  this is required to get the last sample for chanlist_len > 1, not sure why */
 			if (cmd->chanlist_len > 1)
-				start_stop_select |=
-				    AI_STOP_Polarity | AI_STOP_Edge;
+				start_stop_select |= NISTC_AI_STOP_POLARITY |
+						     NISTC_AI_STOP_EDGE;
 		}
 		break;
 	case TRIG_NONE:
 		/* stage number of scans */
-		ni_stc_writel(dev, 0, AI_SC_Load_A_Registers);
+		ni_stc_writel(dev, 0, NISTC_AI_SC_LOADA_REG);
 
-		mode1 |= AI_Start_Stop | AI_Mode_1_Reserved | AI_Continuous;
-		ni_stc_writew(dev, mode1, AI_Mode_1_Register);
+		mode1 |= NISTC_AI_MODE1_START_STOP |
+			 NISTC_AI_MODE1_RSVD |
+			 NISTC_AI_MODE1_CONTINUOUS;
+		ni_stc_writew(dev, mode1, NISTC_AI_MODE1_REG);
 
 		/* load SC (Scan Count) */
-		ni_stc_writew(dev, AI_SC_Load, AI_Command_1_Register);
+		ni_stc_writew(dev, NISTC_AI_CMD1_SC_LOAD, NISTC_AI_CMD1_REG);
 		break;
 	}
 
 	switch (cmd->scan_begin_src) {
 	case TRIG_TIMER:
 		/*
-		   stop bits for non 611x boards
-		   AI_SI_Special_Trigger_Delay=0
-		   AI_Pre_Trigger=0
-		   AI_START_STOP_Select_Register:
-		   AI_START_Polarity=0 (?)      rising edge
-		   AI_START_Edge=1              edge triggered
-		   AI_START_Sync=1 (?)
-		   AI_START_Select=0            SI_TC
-		   AI_STOP_Polarity=0           rising edge
-		   AI_STOP_Edge=0               level
-		   AI_STOP_Sync=1
-		   AI_STOP_Select=19            external pin (configuration mem)
+		 * stop bits for non 611x boards
+		 * NISTC_AI_MODE3_SI_TRIG_DELAY=0
+		 * NISTC_AI_MODE2_PRE_TRIGGER=0
+		 * NISTC_AI_START_STOP_REG:
+		 * NISTC_AI_START_POLARITY=0	(?) rising edge
+		 * NISTC_AI_START_EDGE=1	edge triggered
+		 * NISTC_AI_START_SYNC=1	(?)
+		 * NISTC_AI_START_SEL=0		SI_TC
+		 * NISTC_AI_STOP_POLARITY=0	rising edge
+		 * NISTC_AI_STOP_EDGE=0		level
+		 * NISTC_AI_STOP_SYNC=1
+		 * NISTC_AI_STOP_SEL=19		external pin (configuration mem)
 		 */
-		start_stop_select |= AI_START_Edge | AI_START_Sync;
-		ni_stc_writew(dev, start_stop_select,
-			      AI_START_STOP_Select_Register);
+		start_stop_select |= NISTC_AI_START_EDGE | NISTC_AI_START_SYNC;
+		ni_stc_writew(dev, start_stop_select, NISTC_AI_START_STOP_REG);
 
-		mode2 |= AI_SI_Reload_Mode(0);
-		/* AI_SI_Initial_Load_Source=A */
-		mode2 &= ~AI_SI_Initial_Load_Source;
-		/* mode2 |= AI_SC_Reload_Mode; */
-		ni_stc_writew(dev, mode2, AI_Mode_2_Register);
+		mode2 &= ~NISTC_AI_MODE2_SI_INIT_LOAD_SRC;	/* A */
+		mode2 |= NISTC_AI_MODE2_SI_RELOAD_MODE(0);
+		/* mode2 |= NISTC_AI_MODE2_SC_RELOAD_MODE; */
+		ni_stc_writew(dev, mode2, NISTC_AI_MODE2_REG);
 
 		/* load SI */
 		timer = ni_ns_to_timer(dev, cmd->scan_begin_arg,
 				       CMDF_ROUND_NEAREST);
-		ni_stc_writel(dev, timer, AI_SI_Load_A_Registers);
-		ni_stc_writew(dev, AI_SI_Load, AI_Command_1_Register);
+		ni_stc_writel(dev, timer, NISTC_AI_SI_LOADA_REG);
+		ni_stc_writew(dev, NISTC_AI_CMD1_SI_LOAD, NISTC_AI_CMD1_REG);
 		break;
 	case TRIG_EXT:
 		if (cmd->scan_begin_arg & CR_EDGE)
-			start_stop_select |= AI_START_Edge;
-		/* AI_START_Polarity==1 is falling edge */
-		if (cmd->scan_begin_arg & CR_INVERT)
-			start_stop_select |= AI_START_Polarity;
+			start_stop_select |= NISTC_AI_START_EDGE;
+		if (cmd->scan_begin_arg & CR_INVERT)	/* falling edge */
+			start_stop_select |= NISTC_AI_START_POLARITY;
 		if (cmd->scan_begin_src != cmd->convert_src ||
 		    (cmd->scan_begin_arg & ~CR_EDGE) !=
 		    (cmd->convert_arg & ~CR_EDGE))
-			start_stop_select |= AI_START_Sync;
+			start_stop_select |= NISTC_AI_START_SYNC;
 		start_stop_select |=
-		    AI_START_Select(1 + CR_CHAN(cmd->scan_begin_arg));
-		ni_stc_writew(dev, start_stop_select,
-			      AI_START_STOP_Select_Register);
+		    NISTC_AI_START_SEL(1 + CR_CHAN(cmd->scan_begin_arg));
+		ni_stc_writew(dev, start_stop_select, NISTC_AI_START_STOP_REG);
 		break;
 	}
 
@@ -2543,46 +2394,43 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			timer = ni_ns_to_timer(dev, cmd->convert_arg,
 					       CMDF_ROUND_NEAREST);
 		/* 0,0 does not work */
-		ni_stc_writew(dev, 1, AI_SI2_Load_A_Register);
-		ni_stc_writew(dev, timer, AI_SI2_Load_B_Register);
+		ni_stc_writew(dev, 1, NISTC_AI_SI2_LOADA_REG);
+		ni_stc_writew(dev, timer, NISTC_AI_SI2_LOADB_REG);
 
-		/* AI_SI2_Reload_Mode = alternate */
-		/* AI_SI2_Initial_Load_Source = A */
-		mode2 &= ~AI_SI2_Initial_Load_Source;
-		mode2 |= AI_SI2_Reload_Mode;
-		ni_stc_writew(dev, mode2, AI_Mode_2_Register);
+		mode2 &= ~NISTC_AI_MODE2_SI2_INIT_LOAD_SRC;	/* A */
+		mode2 |= NISTC_AI_MODE2_SI2_RELOAD_MODE;	/* alternate */
+		ni_stc_writew(dev, mode2, NISTC_AI_MODE2_REG);
 
-		/* AI_SI2_Load */
-		ni_stc_writew(dev, AI_SI2_Load, AI_Command_1_Register);
+		ni_stc_writew(dev, NISTC_AI_CMD1_SI2_LOAD, NISTC_AI_CMD1_REG);
 
-		mode2 |= AI_SI2_Reload_Mode;	/*  alternate */
-		mode2 |= AI_SI2_Initial_Load_Source;	/*  B */
-
-		ni_stc_writew(dev, mode2, AI_Mode_2_Register);
+		mode2 |= NISTC_AI_MODE2_SI2_INIT_LOAD_SRC;	/* B */
+		mode2 |= NISTC_AI_MODE2_SI2_RELOAD_MODE;	/* alternate */
+		ni_stc_writew(dev, mode2, NISTC_AI_MODE2_REG);
 		break;
 	case TRIG_EXT:
-		mode1 |= AI_CONVERT_Source_Select(1 + cmd->convert_arg);
+		mode1 |= NISTC_AI_MODE1_CONVERT_SRC(1 + cmd->convert_arg);
 		if ((cmd->convert_arg & CR_INVERT) == 0)
-			mode1 |= AI_CONVERT_Source_Polarity;
-		ni_stc_writew(dev, mode1, AI_Mode_1_Register);
+			mode1 |= NISTC_AI_MODE1_CONVERT_POLARITY;
+		ni_stc_writew(dev, mode1, NISTC_AI_MODE1_REG);
 
-		mode2 |= AI_Start_Stop_Gate_Enable | AI_SC_Gate_Enable;
-		ni_stc_writew(dev, mode2, AI_Mode_2_Register);
+		mode2 |= NISTC_AI_MODE2_SC_GATE_ENA |
+			 NISTC_AI_MODE2_START_STOP_GATE_ENA;
+		ni_stc_writew(dev, mode2, NISTC_AI_MODE2_REG);
 
 		break;
 	}
 
 	if (dev->irq) {
 		/* interrupt on FIFO, errors, SC_TC */
-		interrupt_a_enable |= AI_Error_Interrupt_Enable |
-		    AI_SC_TC_Interrupt_Enable;
+		interrupt_a_enable |= NISTC_INTA_ENA_AI_ERR |
+				      NISTC_INTA_ENA_AI_SC_TC;
 
 #ifndef PCIDMA
-		interrupt_a_enable |= AI_FIFO_Interrupt_Enable;
+		interrupt_a_enable |= NISTC_INTA_ENA_AI_FIFO;
 #endif
 
-		if (cmd->flags & CMDF_WAKE_EOS
-		    || (devpriv->ai_cmd2 & AI_End_On_End_Of_Scan)) {
+		if ((cmd->flags & CMDF_WAKE_EOS) ||
+		    (devpriv->ai_cmd2 & NISTC_AI_CMD2_END_ON_EOS)) {
 			/* wake on end-of-scan */
 			devpriv->aimode = AIMODE_SCAN;
 		} else {
@@ -2593,66 +2441,60 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		case AIMODE_HALF_FULL:
 			/*generate FIFO interrupts and DMA requests on half-full */
 #ifdef PCIDMA
-			ni_stc_writew(dev, AI_FIFO_Mode_HF_to_E,
-				      AI_Mode_3_Register);
+			ni_stc_writew(dev, NISTC_AI_MODE3_FIFO_MODE_HF_E,
+				      NISTC_AI_MODE3_REG);
 #else
-			ni_stc_writew(dev, AI_FIFO_Mode_HF,
-				      AI_Mode_3_Register);
+			ni_stc_writew(dev, NISTC_AI_MODE3_FIFO_MODE_HF,
+				      NISTC_AI_MODE3_REG);
 #endif
 			break;
 		case AIMODE_SAMPLE:
 			/*generate FIFO interrupts on non-empty */
-			ni_stc_writew(dev, AI_FIFO_Mode_NE,
-				      AI_Mode_3_Register);
+			ni_stc_writew(dev, NISTC_AI_MODE3_FIFO_MODE_NE,
+				      NISTC_AI_MODE3_REG);
 			break;
 		case AIMODE_SCAN:
 #ifdef PCIDMA
-			ni_stc_writew(dev, AI_FIFO_Mode_NE,
-				      AI_Mode_3_Register);
+			ni_stc_writew(dev, NISTC_AI_MODE3_FIFO_MODE_NE,
+				      NISTC_AI_MODE3_REG);
 #else
-			ni_stc_writew(dev, AI_FIFO_Mode_HF,
-				      AI_Mode_3_Register);
+			ni_stc_writew(dev, NISTC_AI_MODE3_FIFO_MODE_HF,
+				      NISTC_AI_MODE3_REG);
 #endif
-			interrupt_a_enable |= AI_STOP_Interrupt_Enable;
+			interrupt_a_enable |= NISTC_INTA_ENA_AI_STOP;
 			break;
 		default:
 			break;
 		}
 
 		/* clear interrupts */
-		ni_stc_writew(dev,
-			      AI_Error_Interrupt_Ack |
-			      AI_STOP_Interrupt_Ack |
-			      AI_START_Interrupt_Ack |
-			      AI_START2_Interrupt_Ack |
-			      AI_START1_Interrupt_Ack |
-			      AI_SC_TC_Interrupt_Ack |
-			      AI_SC_TC_Error_Confirm,
-			      Interrupt_A_Ack_Register);
-
-		ni_set_bits(dev, Interrupt_A_Enable_Register,
-			    interrupt_a_enable, 1);
+		ni_stc_writew(dev, NISTC_INTA_ACK_AI_ALL, NISTC_INTA_ACK_REG);
+
+		ni_set_bits(dev, NISTC_INTA_ENA_REG, interrupt_a_enable, 1);
 	} else {
 		/* interrupt on nothing */
-		ni_set_bits(dev, Interrupt_A_Enable_Register, ~0, 0);
+		ni_set_bits(dev, NISTC_INTA_ENA_REG, ~0, 0);
 
 		/* XXX start polling if necessary */
 	}
 
 	/* end configuration */
-	ni_stc_writew(dev, AI_Configuration_End, Joint_Reset_Register);
+	ni_stc_writew(dev, NISTC_RESET_AI_CFG_END, NISTC_RESET_REG);
 
 	switch (cmd->scan_begin_src) {
 	case TRIG_TIMER:
-		ni_stc_writew(dev,
-			      AI_SI2_Arm | AI_SI_Arm | AI_DIV_Arm | AI_SC_Arm,
-			      AI_Command_1_Register);
+		ni_stc_writew(dev, NISTC_AI_CMD1_SI2_ARM |
+				   NISTC_AI_CMD1_SI_ARM |
+				   NISTC_AI_CMD1_DIV_ARM |
+				   NISTC_AI_CMD1_SC_ARM,
+			      NISTC_AI_CMD1_REG);
 		break;
 	case TRIG_EXT:
-		/* XXX AI_SI_Arm? */
-		ni_stc_writew(dev,
-			      AI_SI2_Arm | AI_SI_Arm | AI_DIV_Arm | AI_SC_Arm,
-			      AI_Command_1_Register);
+		ni_stc_writew(dev, NISTC_AI_CMD1_SI2_ARM |
+				   NISTC_AI_CMD1_SI_ARM |	/* XXX ? */
+				   NISTC_AI_CMD1_DIV_ARM |
+				   NISTC_AI_CMD1_SC_ARM,
+			      NISTC_AI_CMD1_REG);
 		break;
 	}
 
@@ -2666,9 +2508,9 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 #endif
 
 	if (cmd->start_src == TRIG_NOW) {
-		/* AI_START1_Pulse */
-		ni_stc_writew(dev, AI_START1_Pulse | devpriv->ai_cmd2,
-			      AI_Command_2_Register);
+		ni_stc_writew(dev, NISTC_AI_CMD2_START1_PULSE |
+				   devpriv->ai_cmd2,
+			      NISTC_AI_CMD2_REG);
 		s->async->inttrig = NULL;
 	} else if (cmd->start_src == TRIG_EXT) {
 		s->async->inttrig = NULL;
@@ -2691,12 +2533,8 @@ static int ni_ai_insn_config(struct comedi_device *dev,
 	switch (data[0]) {
 	case INSN_CONFIG_ALT_SOURCE:
 		if (devpriv->is_m_series) {
-			if (data[1] & ~(MSeries_AI_Bypass_Cal_Sel_Pos_Mask |
-					MSeries_AI_Bypass_Cal_Sel_Neg_Mask |
-					MSeries_AI_Bypass_Mode_Mux_Mask |
-					MSeries_AO_Bypass_AO_Cal_Sel_Mask)) {
+			if (data[1] & ~NI_M_CFG_BYPASS_AI_CAL_MASK)
 				return -EINVAL;
-			}
 			devpriv->ai_calib_source = data[1];
 		} else if (devpriv->is_6143) {
 			unsigned int calib_source;
@@ -2704,7 +2542,7 @@ static int ni_ai_insn_config(struct comedi_device *dev,
 			calib_source = data[1] & 0xf;
 
 			devpriv->ai_calib_source = calib_source;
-			ni_writew(dev, calib_source, Calibration_Channel_6143);
+			ni_writew(dev, calib_source, NI6143_CALIB_CHAN_REG);
 		} else {
 			unsigned int calib_source;
 			unsigned int calib_source_adjust;
@@ -2717,7 +2555,7 @@ static int ni_ai_insn_config(struct comedi_device *dev,
 			devpriv->ai_calib_source = calib_source;
 			if (devpriv->is_611x) {
 				ni_writeb(dev, calib_source_adjust,
-					  Cal_Gain_Select_611x);
+					  NI611X_CAL_GAIN_SEL_REG);
 			}
 		}
 		return 2;
@@ -2771,10 +2609,10 @@ static int ni_m_series_ao_config_chanlist(struct comedi_device *dev,
 
 	if (timed) {
 		for (i = 0; i < s->n_chan; ++i) {
-			devpriv->ao_conf[i] &= ~MSeries_AO_Update_Timed_Bit;
+			devpriv->ao_conf[i] &= ~NI_M_AO_CFG_BANK_UPDATE_TIMED;
 			ni_writeb(dev, devpriv->ao_conf[i],
-				  M_Offset_AO_Config_Bank(i));
-			ni_writeb(dev, 0xf, M_Offset_AO_Waveform_Order(i));
+				  NI_M_AO_CFG_BANK_REG(i));
+			ni_writeb(dev, 0xf, NI_M_AO_WAVEFORM_ORDER_REG(i));
 		}
 	}
 	for (i = 0; i < n_chans; i++) {
@@ -2787,24 +2625,22 @@ static int ni_m_series_ao_config_chanlist(struct comedi_device *dev,
 		conf = 0;
 		switch (krange->max - krange->min) {
 		case 20000000:
-			conf |= MSeries_AO_DAC_Reference_10V_Internal_Bits;
-			ni_writeb(dev, 0,
-				  M_Offset_AO_Reference_Attenuation(chan));
+			conf |= NI_M_AO_CFG_BANK_REF_INT_10V;
+			ni_writeb(dev, 0, NI_M_AO_REF_ATTENUATION_REG(chan));
 			break;
 		case 10000000:
-			conf |= MSeries_AO_DAC_Reference_5V_Internal_Bits;
-			ni_writeb(dev, 0,
-				  M_Offset_AO_Reference_Attenuation(chan));
+			conf |= NI_M_AO_CFG_BANK_REF_INT_5V;
+			ni_writeb(dev, 0, NI_M_AO_REF_ATTENUATION_REG(chan));
 			break;
 		case 4000000:
-			conf |= MSeries_AO_DAC_Reference_10V_Internal_Bits;
-			ni_writeb(dev, MSeries_Attenuate_x5_Bit,
-				  M_Offset_AO_Reference_Attenuation(chan));
+			conf |= NI_M_AO_CFG_BANK_REF_INT_10V;
+			ni_writeb(dev, NI_M_AO_REF_ATTENUATION_X5,
+				  NI_M_AO_REF_ATTENUATION_REG(chan));
 			break;
 		case 2000000:
-			conf |= MSeries_AO_DAC_Reference_5V_Internal_Bits;
-			ni_writeb(dev, MSeries_Attenuate_x5_Bit,
-				  M_Offset_AO_Reference_Attenuation(chan));
+			conf |= NI_M_AO_CFG_BANK_REF_INT_5V;
+			ni_writeb(dev, NI_M_AO_REF_ATTENUATION_X5,
+				  NI_M_AO_REF_ATTENUATION_REG(chan));
 			break;
 		default:
 			dev_err(dev->class_dev,
@@ -2813,10 +2649,10 @@ static int ni_m_series_ao_config_chanlist(struct comedi_device *dev,
 		}
 		switch (krange->max + krange->min) {
 		case 0:
-			conf |= MSeries_AO_DAC_Offset_0V_Bits;
+			conf |= NI_M_AO_CFG_BANK_OFFSET_0V;
 			break;
 		case 10000000:
-			conf |= MSeries_AO_DAC_Offset_5V_Bits;
+			conf |= NI_M_AO_CFG_BANK_OFFSET_5V;
 			break;
 		default:
 			dev_err(dev->class_dev,
@@ -2824,10 +2660,10 @@ static int ni_m_series_ao_config_chanlist(struct comedi_device *dev,
 			break;
 		}
 		if (timed)
-			conf |= MSeries_AO_Update_Timed_Bit;
-		ni_writeb(dev, conf, M_Offset_AO_Config_Bank(chan));
+			conf |= NI_M_AO_CFG_BANK_UPDATE_TIMED;
+		ni_writeb(dev, conf, NI_M_AO_CFG_BANK_REG(chan));
 		devpriv->ao_conf[chan] = conf;
-		ni_writeb(dev, i, M_Offset_AO_Waveform_Order(chan));
+		ni_writeb(dev, i, NI_M_AO_WAVEFORM_ORDER_REG(chan));
 	}
 	return invert;
 }
@@ -2847,27 +2683,27 @@ static int ni_old_ao_config_chanlist(struct comedi_device *dev,
 	for (i = 0; i < n_chans; i++) {
 		chan = CR_CHAN(chanspec[i]);
 		range = CR_RANGE(chanspec[i]);
-		conf = AO_Channel(chan);
+		conf = NI_E_AO_DACSEL(chan);
 
 		if (comedi_range_is_bipolar(s, range)) {
-			conf |= AO_Bipolar;
+			conf |= NI_E_AO_CFG_BIP;
 			invert = (s->maxdata + 1) >> 1;
 		} else {
 			invert = 0;
 		}
 		if (comedi_range_is_external(s, range))
-			conf |= AO_Ext_Ref;
+			conf |= NI_E_AO_EXT_REF;
 
 		/* not all boards can deglitch, but this shouldn't hurt */
 		if (chanspec[i] & CR_DEGLITCH)
-			conf |= AO_Deglitch;
+			conf |= NI_E_AO_DEGLITCH;
 
 		/* analog reference */
 		/* AREF_OTHER connects AO ground to AI ground, i think */
-		conf |= (CR_AREF(chanspec[i]) ==
-			 AREF_OTHER) ? AO_Ground_Ref : 0;
+		if (CR_AREF(chanspec[i]) == AREF_OTHER)
+			conf |= NI_E_AO_GROUND_REF;
 
-		ni_writew(dev, conf, AO_Configuration);
+		ni_writew(dev, conf, NI_E_AO_CFG_REG);
 		devpriv->ao_conf[chan] = conf;
 	}
 	return invert;
@@ -2899,13 +2735,13 @@ static int ni_ao_insn_write(struct comedi_device *dev,
 	int i;
 
 	if (devpriv->is_6xxx) {
-		ni_ao_win_outw(dev, 1 << chan, AO_Immediate_671x);
+		ni_ao_win_outw(dev, 1 << chan, NI671X_AO_IMMEDIATE_REG);
 
-		reg = DACx_Direct_Data_671x(chan);
+		reg = NI671X_DAC_DIRECT_DATA_REG(chan);
 	} else if (devpriv->is_m_series) {
-		reg = M_Offset_DAC_Direct_Data(chan);
+		reg = NI_M_DAC_DIRECT_DATA_REG(chan);
 	} else {
-		reg = (chan) ? DAC1_Direct_Data : DAC0_Direct_Data;
+		reg = NI_E_DAC_DIRECT_DATA_REG(chan);
 	}
 
 	ni_ao_config_chanlist(dev, s, &insn->chanspec, 1, 0);
@@ -2995,13 +2831,13 @@ static int ni_ao_inttrig(struct comedi_device *dev,
 	   multiple times) */
 	s->async->inttrig = NULL;
 
-	ni_set_bits(dev, Interrupt_B_Enable_Register,
-		    AO_FIFO_Interrupt_Enable | AO_Error_Interrupt_Enable, 0);
-	interrupt_b_bits = AO_Error_Interrupt_Enable;
+	ni_set_bits(dev, NISTC_INTB_ENA_REG,
+		    NISTC_INTB_ENA_AO_FIFO | NISTC_INTB_ENA_AO_ERR, 0);
+	interrupt_b_bits = NISTC_INTB_ENA_AO_ERR;
 #ifdef PCIDMA
-	ni_stc_writew(dev, 1, DAC_FIFO_Clear);
+	ni_stc_writew(dev, 1, NISTC_DAC_FIFO_CLR_REG);
 	if (devpriv->is_6xxx)
-		ni_ao_win_outl(dev, 0x6, AO_FIFO_Offset_Load_611x);
+		ni_ao_win_outl(dev, 0x6, NI611X_AO_FIFO_OFFSET_LOAD_REG);
 	ret = ni_ao_setup_MITE_dma(dev);
 	if (ret)
 		return ret;
@@ -3013,17 +2849,17 @@ static int ni_ao_inttrig(struct comedi_device *dev,
 	if (ret == 0)
 		return -EPIPE;
 
-	interrupt_b_bits |= AO_FIFO_Interrupt_Enable;
+	interrupt_b_bits |= NISTC_INTB_ENA_AO_FIFO;
 #endif
 
-	ni_stc_writew(dev, devpriv->ao_mode3 | AO_Not_An_UPDATE,
-		      AO_Mode_3_Register);
-	ni_stc_writew(dev, devpriv->ao_mode3, AO_Mode_3_Register);
+	ni_stc_writew(dev, devpriv->ao_mode3 | NISTC_AO_MODE3_NOT_AN_UPDATE,
+		      NISTC_AO_MODE3_REG);
+	ni_stc_writew(dev, devpriv->ao_mode3, NISTC_AO_MODE3_REG);
 	/* wait for DACs to be loaded */
 	for (i = 0; i < timeout; i++) {
 		udelay(1);
-		if ((ni_stc_readw(dev, Joint_Status_2_Register) &
-		     AO_TMRDACWRs_In_Progress_St) == 0)
+		if ((ni_stc_readw(dev, NISTC_STATUS2_REG) &
+		     NISTC_STATUS2_AO_TMRDACWRS_IN_PROGRESS) == 0)
 			break;
 	}
 	if (i == timeout) {
@@ -3035,17 +2871,20 @@ static int ni_ao_inttrig(struct comedi_device *dev,
 	 * stc manual says we are need to clear error interrupt after
 	 * AO_TMRDACWRs_In_Progress_St clears
 	 */
-	ni_stc_writew(dev, AO_Error_Interrupt_Ack, Interrupt_B_Ack_Register);
+	ni_stc_writew(dev, NISTC_INTB_ACK_AO_ERR, NISTC_INTB_ACK_REG);
 
-	ni_set_bits(dev, Interrupt_B_Enable_Register, interrupt_b_bits, 1);
+	ni_set_bits(dev, NISTC_INTB_ENA_REG, interrupt_b_bits, 1);
 
-	ni_stc_writew(dev, devpriv->ao_cmd1 |
-		      AO_UI_Arm | AO_UC_Arm | AO_BC_Arm |
-		      AO_DAC1_Update_Mode | AO_DAC0_Update_Mode,
-		      AO_Command_1_Register);
+	ni_stc_writew(dev, NISTC_AO_CMD1_UI_ARM |
+			   NISTC_AO_CMD1_UC_ARM |
+			   NISTC_AO_CMD1_BC_ARM |
+			   NISTC_AO_CMD1_DAC1_UPDATE_MODE |
+			   NISTC_AO_CMD1_DAC0_UPDATE_MODE |
+			   devpriv->ao_cmd1,
+		      NISTC_AO_CMD1_REG);
 
-	ni_stc_writew(dev, devpriv->ao_cmd2 | AO_START1_Pulse,
-		      AO_Command_2_Register);
+	ni_stc_writew(dev, NISTC_AO_CMD2_START1_PULSE | devpriv->ao_cmd2,
+		      NISTC_AO_CMD2_REG);
 
 	return 0;
 }
@@ -3058,18 +2897,20 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	int bits;
 	int i;
 	unsigned trigvar;
+	unsigned val;
 
 	if (dev->irq == 0) {
 		dev_err(dev->class_dev, "cannot run command without an irq\n");
 		return -EIO;
 	}
 
-	ni_stc_writew(dev, AO_Configuration_Start, Joint_Reset_Register);
+	ni_stc_writew(dev, NISTC_RESET_AO_CFG_START, NISTC_RESET_REG);
 
-	ni_stc_writew(dev, AO_Disarm, AO_Command_1_Register);
+	ni_stc_writew(dev, NISTC_AO_CMD1_DISARM, NISTC_AO_CMD1_REG);
 
 	if (devpriv->is_6xxx) {
-		ni_ao_win_outw(dev, CLEAR_WG, AO_Misc_611x);
+		ni_ao_win_outw(dev, NI611X_AO_MISC_CLEAR_WG,
+			       NI611X_AO_MISC_REG);
 
 		bits = 0;
 		for (i = 0; i < cmd->chanlist_len; i++) {
@@ -3077,172 +2918,186 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 			chan = CR_CHAN(cmd->chanlist[i]);
 			bits |= 1 << chan;
-			ni_ao_win_outw(dev, chan, AO_Waveform_Generation_611x);
+			ni_ao_win_outw(dev, chan, NI611X_AO_WAVEFORM_GEN_REG);
 		}
-		ni_ao_win_outw(dev, bits, AO_Timed_611x);
+		ni_ao_win_outw(dev, bits, NI611X_AO_TIMED_REG);
 	}
 
 	ni_ao_config_chanlist(dev, s, cmd->chanlist, cmd->chanlist_len, 1);
 
 	if (cmd->stop_src == TRIG_NONE) {
-		devpriv->ao_mode1 |= AO_Continuous;
-		devpriv->ao_mode1 &= ~AO_Trigger_Once;
+		devpriv->ao_mode1 |= NISTC_AO_MODE1_CONTINUOUS;
+		devpriv->ao_mode1 &= ~NISTC_AO_MODE1_TRIGGER_ONCE;
 	} else {
-		devpriv->ao_mode1 &= ~AO_Continuous;
-		devpriv->ao_mode1 |= AO_Trigger_Once;
+		devpriv->ao_mode1 &= ~NISTC_AO_MODE1_CONTINUOUS;
+		devpriv->ao_mode1 |= NISTC_AO_MODE1_TRIGGER_ONCE;
 	}
-	ni_stc_writew(dev, devpriv->ao_mode1, AO_Mode_1_Register);
+	ni_stc_writew(dev, devpriv->ao_mode1, NISTC_AO_MODE1_REG);
+
+	val = devpriv->ao_trigger_select;
 	switch (cmd->start_src) {
 	case TRIG_INT:
 	case TRIG_NOW:
-		devpriv->ao_trigger_select &=
-		    ~(AO_START1_Polarity | AO_START1_Select(-1));
-		devpriv->ao_trigger_select |= AO_START1_Edge | AO_START1_Sync;
-		ni_stc_writew(dev, devpriv->ao_trigger_select,
-			      AO_Trigger_Select_Register);
+		val &= ~(NISTC_AO_TRIG_START1_POLARITY |
+			 NISTC_AO_TRIG_START1_SEL_MASK);
+		val |= NISTC_AO_TRIG_START1_EDGE |
+		       NISTC_AO_TRIG_START1_SYNC;
 		break;
 	case TRIG_EXT:
-		devpriv->ao_trigger_select =
-		    AO_START1_Select(CR_CHAN(cmd->start_arg) + 1);
-		if (cmd->start_arg & CR_INVERT)
-			devpriv->ao_trigger_select |= AO_START1_Polarity;	/*  0=active high, 1=active low. see daq-stc 3-24 (p186) */
-		if (cmd->start_arg & CR_EDGE)
-			devpriv->ao_trigger_select |= AO_START1_Edge;	/*  0=edge detection disabled, 1=enabled */
+		val = NISTC_AO_TRIG_START1_SEL(CR_CHAN(cmd->start_arg) + 1);
+		if (cmd->start_arg & CR_INVERT) {
+			/* 0=active high, 1=active low. see daq-stc 3-24 (p186) */
+			val |= NISTC_AO_TRIG_START1_POLARITY;
+		}
+		if (cmd->start_arg & CR_EDGE) {
+			/* 0=edge detection disabled, 1=enabled */
+			val |= NISTC_AO_TRIG_START1_EDGE;
+		}
 		ni_stc_writew(dev, devpriv->ao_trigger_select,
-			      AO_Trigger_Select_Register);
+			      NISTC_AO_TRIG_SEL_REG);
 		break;
 	default:
 		BUG();
 		break;
 	}
-	devpriv->ao_mode3 &= ~AO_Trigger_Length;
-	ni_stc_writew(dev, devpriv->ao_mode3, AO_Mode_3_Register);
+	devpriv->ao_trigger_select = val;
+	ni_stc_writew(dev, devpriv->ao_trigger_select, NISTC_AO_TRIG_SEL_REG);
+
+	devpriv->ao_mode3 &= ~NISTC_AO_MODE3_TRIG_LEN;
+	ni_stc_writew(dev, devpriv->ao_mode3, NISTC_AO_MODE3_REG);
 
-	ni_stc_writew(dev, devpriv->ao_mode1, AO_Mode_1_Register);
-	devpriv->ao_mode2 &= ~AO_BC_Initial_Load_Source;
-	ni_stc_writew(dev, devpriv->ao_mode2, AO_Mode_2_Register);
+	ni_stc_writew(dev, devpriv->ao_mode1, NISTC_AO_MODE1_REG);
+	devpriv->ao_mode2 &= ~NISTC_AO_MODE2_BC_INIT_LOAD_SRC;
+	ni_stc_writew(dev, devpriv->ao_mode2, NISTC_AO_MODE2_REG);
 	if (cmd->stop_src == TRIG_NONE)
-		ni_stc_writel(dev, 0xffffff, AO_BC_Load_A_Register);
+		ni_stc_writel(dev, 0xffffff, NISTC_AO_BC_LOADA_REG);
 	else
-		ni_stc_writel(dev, 0, AO_BC_Load_A_Register);
-	ni_stc_writew(dev, AO_BC_Load, AO_Command_1_Register);
-	devpriv->ao_mode2 &= ~AO_UC_Initial_Load_Source;
-	ni_stc_writew(dev, devpriv->ao_mode2, AO_Mode_2_Register);
+		ni_stc_writel(dev, 0, NISTC_AO_BC_LOADA_REG);
+	ni_stc_writew(dev, NISTC_AO_CMD1_BC_LOAD, NISTC_AO_CMD1_REG);
+	devpriv->ao_mode2 &= ~NISTC_AO_MODE2_UC_INIT_LOAD_SRC;
+	ni_stc_writew(dev, devpriv->ao_mode2, NISTC_AO_MODE2_REG);
 	switch (cmd->stop_src) {
 	case TRIG_COUNT:
 		if (devpriv->is_m_series) {
 			/*  this is how the NI example code does it for m-series boards, verified correct with 6259 */
 			ni_stc_writel(dev, cmd->stop_arg - 1,
-				      AO_UC_Load_A_Register);
-			ni_stc_writew(dev, AO_UC_Load, AO_Command_1_Register);
+				      NISTC_AO_UC_LOADA_REG);
+			ni_stc_writew(dev, NISTC_AO_CMD1_UC_LOAD,
+				      NISTC_AO_CMD1_REG);
 		} else {
 			ni_stc_writel(dev, cmd->stop_arg,
-				      AO_UC_Load_A_Register);
-			ni_stc_writew(dev, AO_UC_Load, AO_Command_1_Register);
+				      NISTC_AO_UC_LOADA_REG);
+			ni_stc_writew(dev, NISTC_AO_CMD1_UC_LOAD,
+				      NISTC_AO_CMD1_REG);
 			ni_stc_writel(dev, cmd->stop_arg - 1,
-				      AO_UC_Load_A_Register);
+				      NISTC_AO_UC_LOADA_REG);
 		}
 		break;
 	case TRIG_NONE:
-		ni_stc_writel(dev, 0xffffff, AO_UC_Load_A_Register);
-		ni_stc_writew(dev, AO_UC_Load, AO_Command_1_Register);
-		ni_stc_writel(dev, 0xffffff, AO_UC_Load_A_Register);
+		ni_stc_writel(dev, 0xffffff, NISTC_AO_UC_LOADA_REG);
+		ni_stc_writew(dev, NISTC_AO_CMD1_UC_LOAD, NISTC_AO_CMD1_REG);
+		ni_stc_writel(dev, 0xffffff, NISTC_AO_UC_LOADA_REG);
 		break;
 	default:
-		ni_stc_writel(dev, 0, AO_UC_Load_A_Register);
-		ni_stc_writew(dev, AO_UC_Load, AO_Command_1_Register);
-		ni_stc_writel(dev, cmd->stop_arg, AO_UC_Load_A_Register);
+		ni_stc_writel(dev, 0, NISTC_AO_UC_LOADA_REG);
+		ni_stc_writew(dev, NISTC_AO_CMD1_UC_LOAD, NISTC_AO_CMD1_REG);
+		ni_stc_writel(dev, cmd->stop_arg, NISTC_AO_UC_LOADA_REG);
 	}
 
-	devpriv->ao_mode1 &=
-	    ~(AO_UI_Source_Select(0x1f) | AO_UI_Source_Polarity |
-	      AO_UPDATE_Source_Select(0x1f) | AO_UPDATE_Source_Polarity);
+	devpriv->ao_mode1 &= ~(NISTC_AO_MODE1_UPDATE_SRC_MASK |
+			       NISTC_AO_MODE1_UI_SRC_MASK |
+			       NISTC_AO_MODE1_UPDATE_SRC_POLARITY |
+			       NISTC_AO_MODE1_UI_SRC_POLARITY);
 	switch (cmd->scan_begin_src) {
 	case TRIG_TIMER:
-		devpriv->ao_cmd2 &= ~AO_BC_Gate_Enable;
+		devpriv->ao_cmd2 &= ~NISTC_AO_CMD2_BC_GATE_ENA;
 		trigvar =
 		    ni_ns_to_timer(dev, cmd->scan_begin_arg,
 				   CMDF_ROUND_NEAREST);
-		ni_stc_writel(dev, 1, AO_UI_Load_A_Register);
-		ni_stc_writew(dev, AO_UI_Load, AO_Command_1_Register);
-		ni_stc_writel(dev, trigvar, AO_UI_Load_A_Register);
+		ni_stc_writel(dev, 1, NISTC_AO_UI_LOADA_REG);
+		ni_stc_writew(dev, NISTC_AO_CMD1_UI_LOAD, NISTC_AO_CMD1_REG);
+		ni_stc_writel(dev, trigvar, NISTC_AO_UI_LOADA_REG);
 		break;
 	case TRIG_EXT:
 		devpriv->ao_mode1 |=
-		    AO_UPDATE_Source_Select(cmd->scan_begin_arg);
+		    NISTC_AO_MODE1_UPDATE_SRC(cmd->scan_begin_arg);
 		if (cmd->scan_begin_arg & CR_INVERT)
-			devpriv->ao_mode1 |= AO_UPDATE_Source_Polarity;
-		devpriv->ao_cmd2 |= AO_BC_Gate_Enable;
+			devpriv->ao_mode1 |= NISTC_AO_MODE1_UPDATE_SRC_POLARITY;
+		devpriv->ao_cmd2 |= NISTC_AO_CMD2_BC_GATE_ENA;
 		break;
 	default:
 		BUG();
 		break;
 	}
-	ni_stc_writew(dev, devpriv->ao_cmd2, AO_Command_2_Register);
-	ni_stc_writew(dev, devpriv->ao_mode1, AO_Mode_1_Register);
-	devpriv->ao_mode2 &=
-	    ~(AO_UI_Reload_Mode(3) | AO_UI_Initial_Load_Source);
-	ni_stc_writew(dev, devpriv->ao_mode2, AO_Mode_2_Register);
+	ni_stc_writew(dev, devpriv->ao_cmd2, NISTC_AO_CMD2_REG);
+	ni_stc_writew(dev, devpriv->ao_mode1, NISTC_AO_MODE1_REG);
+	devpriv->ao_mode2 &= ~(NISTC_AO_MODE2_UI_RELOAD_MODE(3) |
+			       NISTC_AO_MODE2_UI_INIT_LOAD_SRC);
+	ni_stc_writew(dev, devpriv->ao_mode2, NISTC_AO_MODE2_REG);
 
 	if (cmd->scan_end_arg > 1) {
-		devpriv->ao_mode1 |= AO_Multiple_Channels;
+		devpriv->ao_mode1 |= NISTC_AO_MODE1_MULTI_CHAN;
 		ni_stc_writew(dev,
-			      AO_Number_Of_Channels(cmd->scan_end_arg - 1) |
-			      AO_UPDATE_Output_Select(AO_Update_Output_High_Z),
-			      AO_Output_Control_Register);
+			      NISTC_AO_OUT_CTRL_CHANS(cmd->scan_end_arg - 1) |
+			      NISTC_AO_OUT_CTRL_UPDATE_SEL_HIGHZ,
+			      NISTC_AO_OUT_CTRL_REG);
 	} else {
 		unsigned bits;
 
-		devpriv->ao_mode1 &= ~AO_Multiple_Channels;
-		bits = AO_UPDATE_Output_Select(AO_Update_Output_High_Z);
+		devpriv->ao_mode1 &= ~NISTC_AO_MODE1_MULTI_CHAN;
+		bits = NISTC_AO_OUT_CTRL_UPDATE_SEL_HIGHZ;
 		if (devpriv->is_m_series || devpriv->is_6xxx) {
-			bits |= AO_Number_Of_Channels(0);
+			bits |= NISTC_AO_OUT_CTRL_CHANS(0);
 		} else {
 			bits |=
-			    AO_Number_Of_Channels(CR_CHAN(cmd->chanlist[0]));
+			    NISTC_AO_OUT_CTRL_CHANS(CR_CHAN(cmd->chanlist[0]));
 		}
-		ni_stc_writew(dev, bits, AO_Output_Control_Register);
+		ni_stc_writew(dev, bits, NISTC_AO_OUT_CTRL_REG);
 	}
-	ni_stc_writew(dev, devpriv->ao_mode1, AO_Mode_1_Register);
+	ni_stc_writew(dev, devpriv->ao_mode1, NISTC_AO_MODE1_REG);
 
-	ni_stc_writew(dev, AO_DAC0_Update_Mode | AO_DAC1_Update_Mode,
-		      AO_Command_1_Register);
+	ni_stc_writew(dev, NISTC_AO_CMD1_DAC1_UPDATE_MODE |
+			   NISTC_AO_CMD1_DAC0_UPDATE_MODE,
+		      NISTC_AO_CMD1_REG);
 
-	devpriv->ao_mode3 |= AO_Stop_On_Overrun_Error;
-	ni_stc_writew(dev, devpriv->ao_mode3, AO_Mode_3_Register);
+	devpriv->ao_mode3 |= NISTC_AO_MODE3_STOP_ON_OVERRUN_ERR;
+	ni_stc_writew(dev, devpriv->ao_mode3, NISTC_AO_MODE3_REG);
 
-	devpriv->ao_mode2 &= ~AO_FIFO_Mode_Mask;
+	devpriv->ao_mode2 &= ~NISTC_AO_MODE2_FIFO_MODE_MASK;
 #ifdef PCIDMA
-	devpriv->ao_mode2 |= AO_FIFO_Mode_HF_to_F;
+	devpriv->ao_mode2 |= NISTC_AO_MODE2_FIFO_MODE_HF_F;
 #else
-	devpriv->ao_mode2 |= AO_FIFO_Mode_HF;
+	devpriv->ao_mode2 |= NISTC_AO_MODE2_FIFO_MODE_HF;
 #endif
-	devpriv->ao_mode2 &= ~AO_FIFO_Retransmit_Enable;
-	ni_stc_writew(dev, devpriv->ao_mode2, AO_Mode_2_Register);
+	devpriv->ao_mode2 &= ~NISTC_AO_MODE2_FIFO_REXMIT_ENA;
+	ni_stc_writew(dev, devpriv->ao_mode2, NISTC_AO_MODE2_REG);
 
-	bits = AO_BC_Source_Select | AO_UPDATE_Pulse_Width |
-	    AO_TMRDACWR_Pulse_Width;
+	bits = NISTC_AO_PERSONAL_BC_SRC_SEL |
+	       NISTC_AO_PERSONAL_UPDATE_PW |
+	       NISTC_AO_PERSONAL_TMRDACWR_PW;
 	if (board->ao_fifo_depth)
-		bits |= AO_FIFO_Enable;
+		bits |= NISTC_AO_PERSONAL_FIFO_ENA;
 	else
-		bits |= AO_DMA_PIO_Control;
+		bits |= NISTC_AO_PERSONAL_DMA_PIO_CTRL;
 #if 0
-	/* F Hess: windows driver does not set AO_Number_Of_DAC_Packages bit for 6281,
-	   verified with bus analyzer. */
+	/*
+	 * F Hess: windows driver does not set NISTC_AO_PERSONAL_NUM_DAC bit
+	 * for 6281, verified with bus analyzer.
+	 */
 	if (devpriv->is_m_series)
-		bits |= AO_Number_Of_DAC_Packages;
+		bits |= NISTC_AO_PERSONAL_NUM_DAC;
 #endif
-	ni_stc_writew(dev, bits, AO_Personal_Register);
+	ni_stc_writew(dev, bits, NISTC_AO_PERSONAL_REG);
 	/*  enable sending of ao dma requests */
-	ni_stc_writew(dev, AO_AOFREQ_Enable, AO_Start_Select_Register);
+	ni_stc_writew(dev, NISTC_AO_START_AOFREQ_ENA, NISTC_AO_START_SEL_REG);
 
-	ni_stc_writew(dev, AO_Configuration_End, Joint_Reset_Register);
+	ni_stc_writew(dev, NISTC_RESET_AO_CFG_END, NISTC_RESET_REG);
 
 	if (cmd->stop_src == TRIG_COUNT) {
-		ni_stc_writew(dev, AO_BC_TC_Interrupt_Ack,
-			      Interrupt_B_Ack_Register);
-		ni_set_bits(dev, Interrupt_B_Enable_Register,
-			    AO_BC_TC_Interrupt_Enable, 1);
+		ni_stc_writew(dev, NISTC_INTB_ACK_AO_BC_TC,
+			      NISTC_INTB_ACK_REG);
+		ni_set_bits(dev, NISTC_INTB_ENA_REG,
+			    NISTC_INTB_ENA_AO_BC_TC, 1);
 	}
 
 	s->async->inttrig = ni_ao_inttrig;
@@ -3339,41 +3194,44 @@ static int ni_ao_reset(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	ni_release_ao_mite_channel(dev);
 
-	ni_stc_writew(dev, AO_Configuration_Start, Joint_Reset_Register);
-	ni_stc_writew(dev, AO_Disarm, AO_Command_1_Register);
-	ni_set_bits(dev, Interrupt_B_Enable_Register, ~0, 0);
-	ni_stc_writew(dev, AO_BC_Source_Select, AO_Personal_Register);
-	ni_stc_writew(dev, 0x3f98, Interrupt_B_Ack_Register);
-	ni_stc_writew(dev, AO_BC_Source_Select | AO_UPDATE_Pulse_Width |
-		      AO_TMRDACWR_Pulse_Width, AO_Personal_Register);
-	ni_stc_writew(dev, 0, AO_Output_Control_Register);
-	ni_stc_writew(dev, 0, AO_Start_Select_Register);
+	ni_stc_writew(dev, NISTC_RESET_AO_CFG_START, NISTC_RESET_REG);
+	ni_stc_writew(dev, NISTC_AO_CMD1_DISARM, NISTC_AO_CMD1_REG);
+	ni_set_bits(dev, NISTC_INTB_ENA_REG, ~0, 0);
+	ni_stc_writew(dev, NISTC_AO_PERSONAL_BC_SRC_SEL, NISTC_AO_PERSONAL_REG);
+	ni_stc_writew(dev, NISTC_INTB_ACK_AO_ALL, NISTC_INTB_ACK_REG);
+	ni_stc_writew(dev, NISTC_AO_PERSONAL_BC_SRC_SEL |
+			   NISTC_AO_PERSONAL_UPDATE_PW |
+			   NISTC_AO_PERSONAL_TMRDACWR_PW,
+		      NISTC_AO_PERSONAL_REG);
+	ni_stc_writew(dev, 0, NISTC_AO_OUT_CTRL_REG);
+	ni_stc_writew(dev, 0, NISTC_AO_START_SEL_REG);
 	devpriv->ao_cmd1 = 0;
-	ni_stc_writew(dev, devpriv->ao_cmd1, AO_Command_1_Register);
+	ni_stc_writew(dev, devpriv->ao_cmd1, NISTC_AO_CMD1_REG);
 	devpriv->ao_cmd2 = 0;
-	ni_stc_writew(dev, devpriv->ao_cmd2, AO_Command_2_Register);
+	ni_stc_writew(dev, devpriv->ao_cmd2, NISTC_AO_CMD2_REG);
 	devpriv->ao_mode1 = 0;
-	ni_stc_writew(dev, devpriv->ao_mode1, AO_Mode_1_Register);
+	ni_stc_writew(dev, devpriv->ao_mode1, NISTC_AO_MODE1_REG);
 	devpriv->ao_mode2 = 0;
-	ni_stc_writew(dev, devpriv->ao_mode2, AO_Mode_2_Register);
+	ni_stc_writew(dev, devpriv->ao_mode2, NISTC_AO_MODE2_REG);
 	if (devpriv->is_m_series)
-		devpriv->ao_mode3 = AO_Last_Gate_Disable;
+		devpriv->ao_mode3 = NISTC_AO_MODE3_LAST_GATE_DISABLE;
 	else
 		devpriv->ao_mode3 = 0;
-	ni_stc_writew(dev, devpriv->ao_mode3, AO_Mode_3_Register);
+	ni_stc_writew(dev, devpriv->ao_mode3, NISTC_AO_MODE3_REG);
 	devpriv->ao_trigger_select = 0;
 	ni_stc_writew(dev, devpriv->ao_trigger_select,
-		      AO_Trigger_Select_Register);
+		      NISTC_AO_TRIG_SEL_REG);
 	if (devpriv->is_6xxx) {
 		unsigned immediate_bits = 0;
 		unsigned i;
 
 		for (i = 0; i < s->n_chan; ++i)
 			immediate_bits |= 1 << i;
-		ni_ao_win_outw(dev, immediate_bits, AO_Immediate_671x);
-		ni_ao_win_outw(dev, CLEAR_WG, AO_Misc_611x);
+		ni_ao_win_outw(dev, immediate_bits, NI671X_AO_IMMEDIATE_REG);
+		ni_ao_win_outw(dev, NI611X_AO_MISC_CLEAR_WG,
+			       NI611X_AO_MISC_REG);
 	}
-	ni_stc_writew(dev, AO_Configuration_End, Joint_Reset_Register);
+	ni_stc_writew(dev, NISTC_RESET_AO_CFG_END, NISTC_RESET_REG);
 
 	return 0;
 }
@@ -3392,9 +3250,9 @@ static int ni_dio_insn_config(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	devpriv->dio_control &= ~DIO_Pins_Dir_Mask;
-	devpriv->dio_control |= DIO_Pins_Dir(s->io_bits);
-	ni_stc_writew(dev, devpriv->dio_control, DIO_Control_Register);
+	devpriv->dio_control &= ~NISTC_DIO_CTRL_DIR_MASK;
+	devpriv->dio_control |= NISTC_DIO_CTRL_DIR(s->io_bits);
+	ni_stc_writew(dev, devpriv->dio_control, NISTC_DIO_CTRL_REG);
 
 	return insn->n;
 }
@@ -3407,16 +3265,17 @@ static int ni_dio_insn_bits(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 
 	/* Make sure we're not using the serial part of the dio */
-	if ((data[0] & (DIO_SDIN | DIO_SDOUT)) && devpriv->serial_interval_ns)
+	if ((data[0] & (NISTC_DIO_SDIN | NISTC_DIO_SDOUT)) &&
+	    devpriv->serial_interval_ns)
 		return -EBUSY;
 
 	if (comedi_dio_update_state(s, data)) {
-		devpriv->dio_output &= ~DIO_Parallel_Data_Mask;
-		devpriv->dio_output |= DIO_Parallel_Data_Out(s->state);
-		ni_stc_writew(dev, devpriv->dio_output, DIO_Output_Register);
+		devpriv->dio_output &= ~NISTC_DIO_OUT_PARALLEL_MASK;
+		devpriv->dio_output |= NISTC_DIO_OUT_PARALLEL(s->state);
+		ni_stc_writew(dev, devpriv->dio_output, NISTC_DIO_OUT_REG);
 	}
 
-	data[1] = ni_stc_readw(dev, DIO_Parallel_Input_Register);
+	data[1] = ni_stc_readw(dev, NISTC_DIO_IN_REG);
 
 	return insn->n;
 }
@@ -3432,7 +3291,7 @@ static int ni_m_series_dio_insn_config(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	ni_writel(dev, s->io_bits, M_Offset_DIO_Direction);
+	ni_writel(dev, s->io_bits, NI_M_DIO_DIR_REG);
 
 	return insn->n;
 }
@@ -3443,9 +3302,9 @@ static int ni_m_series_dio_insn_bits(struct comedi_device *dev,
 				     unsigned int *data)
 {
 	if (comedi_dio_update_state(s, data))
-		ni_writel(dev, s->state, M_Offset_Static_Digital_Output);
+		ni_writel(dev, s->state, NI_M_DIO_REG);
 
-	data[1] = ni_readl(dev, M_Offset_Static_Digital_Input);
+	data[1] = ni_readl(dev, NI_M_DIO_REG);
 
 	return insn->n;
 }
@@ -3491,7 +3350,7 @@ static int ni_cdio_cmdtest(struct comedi_device *dev,
 	err |= comedi_check_trigger_arg_is(&cmd->start_arg, 0);
 
 	tmp = cmd->scan_begin_arg;
-	tmp &= CR_PACK_FLAGS(CDO_Sample_Source_Select_Mask, 0, 0, CR_INVERT);
+	tmp &= CR_PACK_FLAGS(NI_M_CDO_MODE_SAMPLE_SRC_MASK, 0, 0, CR_INVERT);
 	if (tmp != cmd->scan_begin_arg)
 		err |= -EINVAL;
 
@@ -3550,13 +3409,14 @@ static int ni_cdo_inttrig(struct comedi_device *dev,
 	if (retval < 0)
 		return retval;
 #endif
-/*
-* XXX not sure what interrupt C group does
-* ni_writeb(dev, Interrupt_Group_C_Enable_Bit,
-* M_Offset_Interrupt_C_Enable); wait for dma to fill output fifo
-*/
+	/*
+	 * XXX not sure what interrupt C group does
+	 * wait for dma to fill output fifo
+	 * ni_writeb(dev, NI_M_INTC_ENA, NI_M_INTC_ENA_REG);
+	 */
 	for (i = 0; i < timeout; ++i) {
-		if (ni_readl(dev, M_Offset_CDIO_Status) & CDO_FIFO_Full_Bit)
+		if (ni_readl(dev, NI_M_CDIO_STATUS_REG) &
+		    NI_M_CDIO_STATUS_CDO_FIFO_FULL)
 			break;
 		udelay(10);
 	}
@@ -3565,36 +3425,30 @@ static int ni_cdo_inttrig(struct comedi_device *dev,
 		s->cancel(dev, s);
 		return -EIO;
 	}
-	ni_writel(dev, CDO_Arm_Bit | CDO_Error_Interrupt_Enable_Set_Bit |
-		       CDO_Empty_FIFO_Interrupt_Enable_Set_Bit,
-		  M_Offset_CDIO_Command);
+	ni_writel(dev, NI_M_CDO_CMD_ARM |
+		       NI_M_CDO_CMD_ERR_INT_ENA_SET |
+		       NI_M_CDO_CMD_F_E_INT_ENA_SET,
+		  NI_M_CDIO_CMD_REG);
 	return retval;
 }
 
 static int ni_cdio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	const struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned cdo_mode_bits = CDO_FIFO_Mode_Bit | CDO_Halt_On_Error_Bit;
+	unsigned cdo_mode_bits;
 	int retval;
 
-	ni_writel(dev, CDO_Reset_Bit, M_Offset_CDIO_Command);
-	switch (cmd->scan_begin_src) {
-	case TRIG_EXT:
-		cdo_mode_bits |=
-		    CR_CHAN(cmd->scan_begin_arg) &
-		    CDO_Sample_Source_Select_Mask;
-		break;
-	default:
-		BUG();
-		break;
-	}
+	ni_writel(dev, NI_M_CDO_CMD_RESET, NI_M_CDIO_CMD_REG);
+	cdo_mode_bits = NI_M_CDO_MODE_FIFO_MODE |
+			NI_M_CDO_MODE_HALT_ON_ERROR |
+			NI_M_CDO_MODE_SAMPLE_SRC(CR_CHAN(cmd->scan_begin_arg));
 	if (cmd->scan_begin_arg & CR_INVERT)
-		cdo_mode_bits |= CDO_Polarity_Bit;
-	ni_writel(dev, cdo_mode_bits, M_Offset_CDO_Mode);
+		cdo_mode_bits |= NI_M_CDO_MODE_POLARITY;
+	ni_writel(dev, cdo_mode_bits, NI_M_CDO_MODE_REG);
 	if (s->io_bits) {
-		ni_writel(dev, s->state, M_Offset_CDO_FIFO_Data);
-		ni_writel(dev, CDO_SW_Update_Bit, M_Offset_CDIO_Command);
-		ni_writel(dev, s->io_bits, M_Offset_CDO_Mask_Enable);
+		ni_writel(dev, s->state, NI_M_CDO_FIFO_DATA_REG);
+		ni_writel(dev, NI_M_CDO_CMD_SW_UPDATE, NI_M_CDIO_CMD_REG);
+		ni_writel(dev, s->io_bits, NI_M_CDO_MASK_ENA_REG);
 	} else {
 		dev_err(dev->class_dev,
 			"attempted to run digital output command with no lines configured as outputs\n");
@@ -3611,15 +3465,16 @@ static int ni_cdio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static int ni_cdio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	ni_writel(dev, CDO_Disarm_Bit | CDO_Error_Interrupt_Enable_Clear_Bit |
-		       CDO_Empty_FIFO_Interrupt_Enable_Clear_Bit |
-		       CDO_FIFO_Request_Interrupt_Enable_Clear_Bit,
-		  M_Offset_CDIO_Command);
-/*
-* XXX not sure what interrupt C group does ni_writeb(dev, 0,
-* M_Offset_Interrupt_C_Enable);
-*/
-	ni_writel(dev, 0, M_Offset_CDO_Mask_Enable);
+	ni_writel(dev, NI_M_CDO_CMD_DISARM |
+		       NI_M_CDO_CMD_ERR_INT_ENA_CLR |
+		       NI_M_CDO_CMD_F_E_INT_ENA_CLR |
+		       NI_M_CDO_CMD_F_REQ_INT_ENA_CLR,
+		  NI_M_CDIO_CMD_REG);
+	/*
+	 * XXX not sure what interrupt C group does
+	 * ni_writeb(dev, 0, NI_M_INTC_ENA_REG);
+	 */
+	ni_writel(dev, 0, NI_M_CDO_MASK_ENA_REG);
 	ni_release_cdo_mite_channel(dev);
 	return 0;
 }
@@ -3650,16 +3505,16 @@ static void handle_cdio_interrupt(struct comedi_device *dev)
 	spin_unlock_irqrestore(&devpriv->mite_channel_lock, flags);
 #endif
 
-	cdio_status = ni_readl(dev, M_Offset_CDIO_Status);
-	if (cdio_status & (CDO_Overrun_Bit | CDO_Underflow_Bit)) {
+	cdio_status = ni_readl(dev, NI_M_CDIO_STATUS_REG);
+	if (cdio_status & NI_M_CDIO_STATUS_CDO_ERROR) {
 		/* XXX just guessing this is needed and does something useful */
-		ni_writel(dev, CDO_Error_Interrupt_Confirm_Bit,
-			  M_Offset_CDIO_Command);
+		ni_writel(dev, NI_M_CDO_CMD_ERR_INT_CONFIRM,
+			  NI_M_CDIO_CMD_REG);
 		s->async->events |= COMEDI_CB_OVERFLOW;
 	}
-	if (cdio_status & CDO_FIFO_Empty_Bit) {
-		ni_writel(dev, CDO_Empty_FIFO_Interrupt_Enable_Clear_Bit,
-			  M_Offset_CDIO_Command);
+	if (cdio_status & NI_M_CDIO_STATUS_CDO_FIFO_EMPTY) {
+		ni_writel(dev, NI_M_CDO_CMD_F_E_INT_ENA_CLR,
+			  NI_M_CDIO_CMD_REG);
 		/* s->async->events |= COMEDI_CB_EOA; */
 	}
 	comedi_handle_events(dev, s);
@@ -3674,23 +3529,23 @@ static int ni_serial_hw_readwrite8(struct comedi_device *dev,
 	unsigned int status1;
 	int err = 0, count = 20;
 
-	devpriv->dio_output &= ~DIO_Serial_Data_Mask;
-	devpriv->dio_output |= DIO_Serial_Data_Out(data_out);
-	ni_stc_writew(dev, devpriv->dio_output, DIO_Output_Register);
+	devpriv->dio_output &= ~NISTC_DIO_OUT_SERIAL_MASK;
+	devpriv->dio_output |= NISTC_DIO_OUT_SERIAL(data_out);
+	ni_stc_writew(dev, devpriv->dio_output, NISTC_DIO_OUT_REG);
 
-	status1 = ni_stc_readw(dev, Joint_Status_1_Register);
-	if (status1 & DIO_Serial_IO_In_Progress_St) {
+	status1 = ni_stc_readw(dev, NISTC_STATUS1_REG);
+	if (status1 & NISTC_STATUS1_SERIO_IN_PROG) {
 		err = -EBUSY;
 		goto Error;
 	}
 
-	devpriv->dio_control |= DIO_HW_Serial_Start;
-	ni_stc_writew(dev, devpriv->dio_control, DIO_Control_Register);
-	devpriv->dio_control &= ~DIO_HW_Serial_Start;
+	devpriv->dio_control |= NISTC_DIO_CTRL_HW_SER_START;
+	ni_stc_writew(dev, devpriv->dio_control, NISTC_DIO_CTRL_REG);
+	devpriv->dio_control &= ~NISTC_DIO_CTRL_HW_SER_START;
 
 	/* Wait until STC says we're done, but don't loop infinitely. */
-	while ((status1 = ni_stc_readw(dev, Joint_Status_1_Register)) &
-	       DIO_Serial_IO_In_Progress_St) {
+	while ((status1 = ni_stc_readw(dev, NISTC_STATUS1_REG)) &
+	       NISTC_STATUS1_SERIO_IN_PROG) {
 		/* Delay one bit per loop */
 		udelay((devpriv->serial_interval_ns + 999) / 1000);
 		if (--count < 0) {
@@ -3701,15 +3556,17 @@ static int ni_serial_hw_readwrite8(struct comedi_device *dev,
 		}
 	}
 
-	/* Delay for last bit. This delay is absolutely necessary, because
-	   DIO_Serial_IO_In_Progress_St goes high one bit too early. */
+	/*
+	 * Delay for last bit. This delay is absolutely necessary, because
+	 * NISTC_STATUS1_SERIO_IN_PROG goes high one bit too early.
+	 */
 	udelay((devpriv->serial_interval_ns + 999) / 1000);
 
 	if (data_in)
-		*data_in = ni_stc_readw(dev, DIO_Serial_Input_Register);
+		*data_in = ni_stc_readw(dev, NISTC_DIO_SERIAL_IN_REG);
 
 Error:
-	ni_stc_writew(dev, devpriv->dio_control, DIO_Control_Register);
+	ni_stc_writew(dev, devpriv->dio_control, NISTC_DIO_CTRL_REG);
 
 	return err;
 }
@@ -3729,25 +3586,25 @@ static int ni_serial_sw_readwrite8(struct comedi_device *dev,
 		/* Output current bit; note that we cannot touch s->state
 		   because it is a per-subdevice field, and serial is
 		   a separate subdevice from DIO. */
-		devpriv->dio_output &= ~DIO_SDOUT;
+		devpriv->dio_output &= ~NISTC_DIO_SDOUT;
 		if (data_out & mask)
-			devpriv->dio_output |= DIO_SDOUT;
-		ni_stc_writew(dev, devpriv->dio_output, DIO_Output_Register);
+			devpriv->dio_output |= NISTC_DIO_SDOUT;
+		ni_stc_writew(dev, devpriv->dio_output, NISTC_DIO_OUT_REG);
 
 		/* Assert SDCLK (active low, inverted), wait for half of
 		   the delay, deassert SDCLK, and wait for the other half. */
-		devpriv->dio_control |= DIO_Software_Serial_Control;
-		ni_stc_writew(dev, devpriv->dio_control, DIO_Control_Register);
+		devpriv->dio_control |= NISTC_DIO_SDCLK;
+		ni_stc_writew(dev, devpriv->dio_control, NISTC_DIO_CTRL_REG);
 
 		udelay((devpriv->serial_interval_ns + 999) / 2000);
 
-		devpriv->dio_control &= ~DIO_Software_Serial_Control;
-		ni_stc_writew(dev, devpriv->dio_control, DIO_Control_Register);
+		devpriv->dio_control &= ~NISTC_DIO_SDCLK;
+		ni_stc_writew(dev, devpriv->dio_control, NISTC_DIO_CTRL_REG);
 
 		udelay((devpriv->serial_interval_ns + 999) / 2000);
 
 		/* Input current bit */
-		if (ni_stc_readw(dev, DIO_Parallel_Input_Register) & DIO_SDIN)
+		if (ni_stc_readw(dev, NISTC_DIO_IN_REG) & NISTC_DIO_SDIN)
 			input |= mask;
 	}
 
@@ -3763,6 +3620,7 @@ static int ni_serial_insn_config(struct comedi_device *dev,
 				 unsigned int *data)
 {
 	struct ni_private *devpriv = dev->private;
+	unsigned clk_fout = devpriv->clock_and_fout;
 	int err = insn->n;
 	unsigned char byte_out, byte_in = 0;
 
@@ -3772,49 +3630,49 @@ static int ni_serial_insn_config(struct comedi_device *dev,
 	switch (data[0]) {
 	case INSN_CONFIG_SERIAL_CLOCK:
 		devpriv->serial_hw_mode = 1;
-		devpriv->dio_control |= DIO_HW_Serial_Enable;
+		devpriv->dio_control |= NISTC_DIO_CTRL_HW_SER_ENA;
 
 		if (data[1] == SERIAL_DISABLED) {
 			devpriv->serial_hw_mode = 0;
-			devpriv->dio_control &= ~(DIO_HW_Serial_Enable |
-						  DIO_Software_Serial_Control);
+			devpriv->dio_control &= ~(NISTC_DIO_CTRL_HW_SER_ENA |
+						  NISTC_DIO_SDCLK);
 			data[1] = SERIAL_DISABLED;
 			devpriv->serial_interval_ns = data[1];
 		} else if (data[1] <= SERIAL_600NS) {
 			/* Warning: this clock speed is too fast to reliably
 			   control SCXI. */
-			devpriv->dio_control &= ~DIO_HW_Serial_Timebase;
-			devpriv->clock_and_fout |= Slow_Internal_Timebase;
-			devpriv->clock_and_fout &= ~DIO_Serial_Out_Divide_By_2;
+			devpriv->dio_control &= ~NISTC_DIO_CTRL_HW_SER_TIMEBASE;
+			clk_fout |= NISTC_CLK_FOUT_SLOW_TIMEBASE;
+			clk_fout &= ~NISTC_CLK_FOUT_DIO_SER_OUT_DIV2;
 			data[1] = SERIAL_600NS;
 			devpriv->serial_interval_ns = data[1];
 		} else if (data[1] <= SERIAL_1_2US) {
-			devpriv->dio_control &= ~DIO_HW_Serial_Timebase;
-			devpriv->clock_and_fout |= Slow_Internal_Timebase |
-			    DIO_Serial_Out_Divide_By_2;
+			devpriv->dio_control &= ~NISTC_DIO_CTRL_HW_SER_TIMEBASE;
+			clk_fout |= NISTC_CLK_FOUT_SLOW_TIMEBASE |
+				    NISTC_CLK_FOUT_DIO_SER_OUT_DIV2;
 			data[1] = SERIAL_1_2US;
 			devpriv->serial_interval_ns = data[1];
 		} else if (data[1] <= SERIAL_10US) {
-			devpriv->dio_control |= DIO_HW_Serial_Timebase;
-			devpriv->clock_and_fout |= Slow_Internal_Timebase |
-			    DIO_Serial_Out_Divide_By_2;
-			/* Note: DIO_Serial_Out_Divide_By_2 only affects
+			devpriv->dio_control |= NISTC_DIO_CTRL_HW_SER_TIMEBASE;
+			clk_fout |= NISTC_CLK_FOUT_SLOW_TIMEBASE |
+				    NISTC_CLK_FOUT_DIO_SER_OUT_DIV2;
+			/* Note: NISTC_CLK_FOUT_DIO_SER_OUT_DIV2 only affects
 			   600ns/1.2us. If you turn divide_by_2 off with the
 			   slow clock, you will still get 10us, except then
 			   all your delays are wrong. */
 			data[1] = SERIAL_10US;
 			devpriv->serial_interval_ns = data[1];
 		} else {
-			devpriv->dio_control &= ~(DIO_HW_Serial_Enable |
-						  DIO_Software_Serial_Control);
+			devpriv->dio_control &= ~(NISTC_DIO_CTRL_HW_SER_ENA |
+						  NISTC_DIO_SDCLK);
 			devpriv->serial_hw_mode = 0;
 			data[1] = (data[1] / 1000) * 1000;
 			devpriv->serial_interval_ns = data[1];
 		}
+		devpriv->clock_and_fout = clk_fout;
 
-		ni_stc_writew(dev, devpriv->dio_control, DIO_Control_Register);
-		ni_stc_writew(dev, devpriv->clock_and_fout,
-			      Clock_and_FOUT_Register);
+		ni_stc_writew(dev, devpriv->dio_control, NISTC_DIO_CTRL_REG);
+		ni_stc_writew(dev, devpriv->clock_and_fout, NISTC_CLK_FOUT_REG);
 		return 1;
 
 	case INSN_CONFIG_BIDIRECTIONAL_DATA:
@@ -3850,141 +3708,91 @@ static void init_ao_67xx(struct comedi_device *dev, struct comedi_subdevice *s)
 	int i;
 
 	for (i = 0; i < s->n_chan; i++) {
-		ni_ao_win_outw(dev, AO_Channel(i) | 0x0,
-			       AO_Configuration_2_67xx);
-	}
-	ni_ao_win_outw(dev, 0x0, AO_Later_Single_Point_Updates);
-}
+		ni_ao_win_outw(dev, NI_E_AO_DACSEL(i) | 0x0,
+			       NI67XX_AO_CFG2_REG);
+	}
+	ni_ao_win_outw(dev, 0x0, NI67XX_AO_SP_UPDATES_REG);
+}
+
+static const struct mio_regmap ni_gpct_to_stc_regmap[] = {
+	[NITIO_G0_AUTO_INC]	= { NISTC_G0_AUTOINC_REG, 2 },
+	[NITIO_G1_AUTO_INC]	= { NISTC_G1_AUTOINC_REG, 2 },
+	[NITIO_G0_CMD]		= { NISTC_G0_CMD_REG, 2 },
+	[NITIO_G1_CMD]		= { NISTC_G1_CMD_REG, 2 },
+	[NITIO_G0_HW_SAVE]	= { NISTC_G0_HW_SAVE_REG, 4 },
+	[NITIO_G1_HW_SAVE]	= { NISTC_G1_HW_SAVE_REG, 4 },
+	[NITIO_G0_SW_SAVE]	= { NISTC_G0_SAVE_REG, 4 },
+	[NITIO_G1_SW_SAVE]	= { NISTC_G1_SAVE_REG, 4 },
+	[NITIO_G0_MODE]		= { NISTC_G0_MODE_REG, 2 },
+	[NITIO_G1_MODE]		= { NISTC_G1_MODE_REG, 2 },
+	[NITIO_G0_LOADA]	= { NISTC_G0_LOADA_REG, 4 },
+	[NITIO_G1_LOADA]	= { NISTC_G1_LOADA_REG, 4 },
+	[NITIO_G0_LOADB]	= { NISTC_G0_LOADB_REG, 4 },
+	[NITIO_G1_LOADB]	= { NISTC_G1_LOADB_REG, 4 },
+	[NITIO_G0_INPUT_SEL]	= { NISTC_G0_INPUT_SEL_REG, 2 },
+	[NITIO_G1_INPUT_SEL]	= { NISTC_G1_INPUT_SEL_REG, 2 },
+	[NITIO_G0_CNT_MODE]	= { 0x1b0, 2 },	/* M-Series only */
+	[NITIO_G1_CNT_MODE]	= { 0x1b2, 2 },	/* M-Series only */
+	[NITIO_G0_GATE2]	= { 0x1b4, 2 },	/* M-Series only */
+	[NITIO_G1_GATE2]	= { 0x1b6, 2 },	/* M-Series only */
+	[NITIO_G01_STATUS]	= { NISTC_G01_STATUS_REG, 2 },
+	[NITIO_G01_RESET]	= { NISTC_RESET_REG, 2 },
+	[NITIO_G01_STATUS1]	= { NISTC_STATUS1_REG, 2 },
+	[NITIO_G01_STATUS2]	= { NISTC_STATUS2_REG, 2 },
+	[NITIO_G0_DMA_CFG]	= { 0x1b8, 2 },	/* M-Series only */
+	[NITIO_G1_DMA_CFG]	= { 0x1ba, 2 },	/* M-Series only */
+	[NITIO_G0_DMA_STATUS]	= { 0x1b8, 2 },	/* M-Series only */
+	[NITIO_G1_DMA_STATUS]	= { 0x1ba, 2 },	/* M-Series only */
+	[NITIO_G0_ABZ]		= { 0x1c0, 2 },	/* M-Series only */
+	[NITIO_G1_ABZ]		= { 0x1c2, 2 },	/* M-Series only */
+	[NITIO_G0_INT_ACK]	= { NISTC_INTA_ACK_REG, 2 },
+	[NITIO_G1_INT_ACK]	= { NISTC_INTB_ACK_REG, 2 },
+	[NITIO_G0_STATUS]	= { NISTC_AI_STATUS1_REG, 2 },
+	[NITIO_G1_STATUS]	= { NISTC_AO_STATUS1_REG, 2 },
+	[NITIO_G0_INT_ENA]	= { NISTC_INTA_ENA_REG, 2 },
+	[NITIO_G1_INT_ENA]	= { NISTC_INTB_ENA_REG, 2 },
+};
 
-static unsigned ni_gpct_to_stc_register(enum ni_gpct_register reg)
+static unsigned int ni_gpct_to_stc_register(struct comedi_device *dev,
+					    enum ni_gpct_register reg)
 {
-	unsigned stc_register;
+	const struct mio_regmap *regmap;
 
-	switch (reg) {
-	case NITIO_G0_AUTO_INC:
-		stc_register = G_Autoincrement_Register(0);
-		break;
-	case NITIO_G1_AUTO_INC:
-		stc_register = G_Autoincrement_Register(1);
-		break;
-	case NITIO_G0_CMD:
-		stc_register = G_Command_Register(0);
-		break;
-	case NITIO_G1_CMD:
-		stc_register = G_Command_Register(1);
-		break;
-	case NITIO_G0_HW_SAVE:
-		stc_register = G_HW_Save_Register(0);
-		break;
-	case NITIO_G1_HW_SAVE:
-		stc_register = G_HW_Save_Register(1);
-		break;
-	case NITIO_G0_SW_SAVE:
-		stc_register = G_Save_Register(0);
-		break;
-	case NITIO_G1_SW_SAVE:
-		stc_register = G_Save_Register(1);
-		break;
-	case NITIO_G0_MODE:
-		stc_register = G_Mode_Register(0);
-		break;
-	case NITIO_G1_MODE:
-		stc_register = G_Mode_Register(1);
-		break;
-	case NITIO_G0_LOADA:
-		stc_register = G_Load_A_Register(0);
-		break;
-	case NITIO_G1_LOADA:
-		stc_register = G_Load_A_Register(1);
-		break;
-	case NITIO_G0_LOADB:
-		stc_register = G_Load_B_Register(0);
-		break;
-	case NITIO_G1_LOADB:
-		stc_register = G_Load_B_Register(1);
-		break;
-	case NITIO_G0_INPUT_SEL:
-		stc_register = G_Input_Select_Register(0);
-		break;
-	case NITIO_G1_INPUT_SEL:
-		stc_register = G_Input_Select_Register(1);
-		break;
-	case NITIO_G01_STATUS:
-		stc_register = G_Status_Register;
-		break;
-	case NITIO_G01_RESET:
-		stc_register = Joint_Reset_Register;
-		break;
-	case NITIO_G01_STATUS1:
-		stc_register = Joint_Status_1_Register;
-		break;
-	case NITIO_G01_STATUS2:
-		stc_register = Joint_Status_2_Register;
-		break;
-	case NITIO_G0_INT_ACK:
-		stc_register = Interrupt_A_Ack_Register;
-		break;
-	case NITIO_G1_INT_ACK:
-		stc_register = Interrupt_B_Ack_Register;
-		break;
-	case NITIO_G0_STATUS:
-		stc_register = AI_Status_1_Register;
-		break;
-	case NITIO_G1_STATUS:
-		stc_register = AO_Status_1_Register;
-		break;
-	case NITIO_G0_INT_ENA:
-		stc_register = Interrupt_A_Enable_Register;
-		break;
-	case NITIO_G1_INT_ENA:
-		stc_register = Interrupt_B_Enable_Register;
-		break;
-	default:
-		pr_err("%s: unhandled register 0x%x in switch.\n",
-		       __func__, reg);
-		BUG();
+	if (reg < ARRAY_SIZE(ni_gpct_to_stc_regmap)) {
+		regmap = &ni_gpct_to_stc_regmap[reg];
+	} else {
+		dev_warn(dev->class_dev, "%s: unhandled register=0x%x\n",
+			 __func__, reg);
 		return 0;
 	}
-	return stc_register;
+
+	return regmap->mio_reg;
 }
 
 static void ni_gpct_write_register(struct ni_gpct *counter, unsigned bits,
 				   enum ni_gpct_register reg)
 {
 	struct comedi_device *dev = counter->counter_dev->dev;
-	unsigned stc_register;
-	/* bits in the join reset register which are relevant to counters */
-	static const unsigned gpct_joint_reset_mask = G0_Reset | G1_Reset;
+	unsigned int stc_register = ni_gpct_to_stc_register(dev, reg);
 	static const unsigned gpct_interrupt_a_enable_mask =
-	    G0_Gate_Interrupt_Enable | G0_TC_Interrupt_Enable;
+	    NISTC_INTA_ENA_G0_GATE | NISTC_INTA_ENA_G0_TC;
 	static const unsigned gpct_interrupt_b_enable_mask =
-	    G1_Gate_Interrupt_Enable | G1_TC_Interrupt_Enable;
+	    NISTC_INTB_ENA_G1_GATE | NISTC_INTB_ENA_G1_TC;
+
+	if (stc_register == 0)
+		return;
 
 	switch (reg) {
-		/* m-series-only registers */
+		/* m-series only registers */
 	case NITIO_G0_CNT_MODE:
-		ni_writew(dev, bits, M_Offset_G0_Counting_Mode);
-		break;
 	case NITIO_G1_CNT_MODE:
-		ni_writew(dev, bits, M_Offset_G1_Counting_Mode);
-		break;
 	case NITIO_G0_GATE2:
-		ni_writew(dev, bits, M_Offset_G0_Second_Gate);
-		break;
 	case NITIO_G1_GATE2:
-		ni_writew(dev, bits, M_Offset_G1_Second_Gate);
-		break;
 	case NITIO_G0_DMA_CFG:
-		ni_writew(dev, bits, M_Offset_G0_DMA_Config);
-		break;
 	case NITIO_G1_DMA_CFG:
-		ni_writew(dev, bits, M_Offset_G1_DMA_Config);
-		break;
 	case NITIO_G0_ABZ:
-		ni_writew(dev, bits, M_Offset_G0_MSeries_ABZ);
-		break;
 	case NITIO_G1_ABZ:
-		ni_writew(dev, bits, M_Offset_G1_MSeries_ABZ);
+		ni_writew(dev, bits, stc_register);
 		break;
 
 		/* 32 bit registers */
@@ -3992,26 +3800,24 @@ static void ni_gpct_write_register(struct ni_gpct *counter, unsigned bits,
 	case NITIO_G1_LOADA:
 	case NITIO_G0_LOADB:
 	case NITIO_G1_LOADB:
-		stc_register = ni_gpct_to_stc_register(reg);
 		ni_stc_writel(dev, bits, stc_register);
 		break;
 
 		/* 16 bit registers */
 	case NITIO_G0_INT_ENA:
 		BUG_ON(bits & ~gpct_interrupt_a_enable_mask);
-		ni_set_bitfield(dev, Interrupt_A_Enable_Register,
+		ni_set_bitfield(dev, stc_register,
 				gpct_interrupt_a_enable_mask, bits);
 		break;
 	case NITIO_G1_INT_ENA:
 		BUG_ON(bits & ~gpct_interrupt_b_enable_mask);
-		ni_set_bitfield(dev, Interrupt_B_Enable_Register,
+		ni_set_bitfield(dev, stc_register,
 				gpct_interrupt_b_enable_mask, bits);
 		break;
 	case NITIO_G01_RESET:
-		BUG_ON(bits & ~gpct_joint_reset_mask);
+		BUG_ON(bits & ~(NISTC_RESET_G0 | NISTC_RESET_G1));
 		/* fall-through */
 	default:
-		stc_register = ni_gpct_to_stc_register(reg);
 		ni_stc_writew(dev, bits, stc_register);
 	}
 }
@@ -4020,29 +3826,28 @@ static unsigned ni_gpct_read_register(struct ni_gpct *counter,
 				      enum ni_gpct_register reg)
 {
 	struct comedi_device *dev = counter->counter_dev->dev;
-	unsigned stc_register;
+	unsigned int stc_register = ni_gpct_to_stc_register(dev, reg);
+
+	if (stc_register == 0)
+		return 0;
 
 	switch (reg) {
 		/* m-series only registers */
 	case NITIO_G0_DMA_STATUS:
-		return ni_readw(dev, M_Offset_G0_DMA_Status);
 	case NITIO_G1_DMA_STATUS:
-		return ni_readw(dev, M_Offset_G1_DMA_Status);
+		return ni_readw(dev, stc_register);
 
 		/* 32 bit registers */
 	case NITIO_G0_HW_SAVE:
 	case NITIO_G1_HW_SAVE:
 	case NITIO_G0_SW_SAVE:
 	case NITIO_G1_SW_SAVE:
-		stc_register = ni_gpct_to_stc_register(reg);
 		return ni_stc_readl(dev, stc_register);
 
 		/* 16 bit registers */
 	default:
-		stc_register = ni_gpct_to_stc_register(reg);
 		return ni_stc_readw(dev, stc_register);
 	}
-	return 0;
 }
 
 static int ni_freq_out_insn_read(struct comedi_device *dev,
@@ -4051,7 +3856,7 @@ static int ni_freq_out_insn_read(struct comedi_device *dev,
 				 unsigned int *data)
 {
 	struct ni_private *devpriv = dev->private;
-	unsigned int val = devpriv->clock_and_fout & FOUT_Divider_mask;
+	unsigned int val = NISTC_CLK_FOUT_TO_DIVIDER(devpriv->clock_and_fout);
 	int i;
 
 	for (i = 0; i < insn->n; i++)
@@ -4068,17 +3873,17 @@ static int ni_freq_out_insn_write(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 
 	if (insn->n) {
-		devpriv->clock_and_fout &= ~FOUT_Enable;
-		ni_stc_writew(dev, devpriv->clock_and_fout,
-			      Clock_and_FOUT_Register);
-		devpriv->clock_and_fout &= ~FOUT_Divider_mask;
+		unsigned int val = data[insn->n - 1];
+
+		devpriv->clock_and_fout &= ~NISTC_CLK_FOUT_ENA;
+		ni_stc_writew(dev, devpriv->clock_and_fout, NISTC_CLK_FOUT_REG);
+		devpriv->clock_and_fout &= ~NISTC_CLK_FOUT_DIVIDER_MASK;
 
 		/* use the last data value to set the fout divider */
-		devpriv->clock_and_fout |= FOUT_Divider(data[insn->n - 1]);
+		devpriv->clock_and_fout |= NISTC_CLK_FOUT_DIVIDER(val);
 
-		devpriv->clock_and_fout |= FOUT_Enable;
-		ni_stc_writew(dev, devpriv->clock_and_fout,
-			      Clock_and_FOUT_Register);
+		devpriv->clock_and_fout |= NISTC_CLK_FOUT_ENA;
+		ni_stc_writew(dev, devpriv->clock_and_fout, NISTC_CLK_FOUT_REG);
 	}
 	return insn->n;
 }
@@ -4094,19 +3899,18 @@ static int ni_freq_out_insn_config(struct comedi_device *dev,
 	case INSN_CONFIG_SET_CLOCK_SRC:
 		switch (data[1]) {
 		case NI_FREQ_OUT_TIMEBASE_1_DIV_2_CLOCK_SRC:
-			devpriv->clock_and_fout &= ~FOUT_Timebase_Select;
+			devpriv->clock_and_fout &= ~NISTC_CLK_FOUT_TIMEBASE_SEL;
 			break;
 		case NI_FREQ_OUT_TIMEBASE_2_CLOCK_SRC:
-			devpriv->clock_and_fout |= FOUT_Timebase_Select;
+			devpriv->clock_and_fout |= NISTC_CLK_FOUT_TIMEBASE_SEL;
 			break;
 		default:
 			return -EINVAL;
 		}
-		ni_stc_writew(dev, devpriv->clock_and_fout,
-			      Clock_and_FOUT_Register);
+		ni_stc_writew(dev, devpriv->clock_and_fout, NISTC_CLK_FOUT_REG);
 		break;
 	case INSN_CONFIG_GET_CLOCK_SRC:
-		if (devpriv->clock_and_fout & FOUT_Timebase_Select) {
+		if (devpriv->clock_and_fout & NISTC_CLK_FOUT_TIMEBASE_SEL) {
 			data[1] = NI_FREQ_OUT_TIMEBASE_2_CLOCK_SRC;
 			data[2] = TIMEBASE_2_NS;
 		} else {
@@ -4190,9 +3994,9 @@ static int ni_m_series_pwm_config(struct comedi_device *dev,
 			data[4] = down_count * devpriv->clock_ns;
 			return -EAGAIN;
 		}
-		ni_writel(dev, MSeries_Cal_PWM_High_Time_Bits(up_count) |
-			       MSeries_Cal_PWM_Low_Time_Bits(down_count),
-			  M_Offset_Cal_PWM);
+		ni_writel(dev, NI_M_CAL_PWM_HIGH_TIME(up_count) |
+			       NI_M_CAL_PWM_LOW_TIME(down_count),
+			  NI_M_CAL_PWM_REG);
 		devpriv->pwm_up_count = up_count;
 		devpriv->pwm_down_count = down_count;
 		return 5;
@@ -4254,9 +4058,9 @@ static int ni_6143_pwm_config(struct comedi_device *dev,
 			data[4] = down_count * devpriv->clock_ns;
 			return -EAGAIN;
 		}
-		ni_writel(dev, up_count, Calibration_HighTime_6143);
+		ni_writel(dev, up_count, NI6143_CALIB_HI_TIME_REG);
 		devpriv->pwm_up_count = up_count;
-		ni_writel(dev, down_count, Calibration_LowTime_6143);
+		ni_writel(dev, down_count, NI6143_CALIB_LO_TIME_REG);
 		devpriv->pwm_down_count = down_count;
 		return 5;
 	case INSN_CONFIG_GET_PWM_OUTPUT:
@@ -4336,6 +4140,7 @@ static void ni_write_caldac(struct comedi_device *dev, int addr, int val)
 	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	unsigned int loadbit = 0, bits = 0, bit, bitstring = 0;
+	unsigned int cmd;
 	int i;
 	int type;
 
@@ -4349,22 +4154,26 @@ static void ni_write_caldac(struct comedi_device *dev, int addr, int val)
 			break;
 		if (addr < caldacs[type].n_chans) {
 			bits = caldacs[type].packbits(addr, val, &bitstring);
-			loadbit = SerDacLd(i);
+			loadbit = NI_E_SERIAL_CMD_DAC_LD(i);
 			break;
 		}
 		addr -= caldacs[type].n_chans;
 	}
 
+	/* bits will be 0 if there is no caldac for the given addr */
+	if (bits == 0)
+		return;
+
 	for (bit = 1 << (bits - 1); bit; bit >>= 1) {
-		ni_writeb(dev, ((bit & bitstring) ? 0x02 : 0), Serial_Command);
+		cmd = (bit & bitstring) ? NI_E_SERIAL_CMD_SDATA : 0;
+		ni_writeb(dev, cmd, NI_E_SERIAL_CMD_REG);
 		udelay(1);
-		ni_writeb(dev, 1 | ((bit & bitstring) ? 0x02 : 0),
-			  Serial_Command);
+		ni_writeb(dev, NI_E_SERIAL_CMD_SCLK | cmd, NI_E_SERIAL_CMD_REG);
 		udelay(1);
 	}
-	ni_writeb(dev, loadbit, Serial_Command);
+	ni_writeb(dev, loadbit, NI_E_SERIAL_CMD_REG);
 	udelay(1);
-	ni_writeb(dev, 0, Serial_Command);
+	ni_writeb(dev, 0, NI_E_SERIAL_CMD_REG);
 }
 
 static int ni_calib_insn_write(struct comedi_device *dev,
@@ -4446,24 +4255,30 @@ static void caldac_setup(struct comedi_device *dev, struct comedi_subdevice *s)
 
 static int ni_read_eeprom(struct comedi_device *dev, int addr)
 {
+	unsigned int cmd = NI_E_SERIAL_CMD_EEPROM_CS;
 	int bit;
 	int bitstring;
 
 	bitstring = 0x0300 | ((addr & 0x100) << 3) | (addr & 0xff);
-	ni_writeb(dev, 0x04, Serial_Command);
+	ni_writeb(dev, cmd, NI_E_SERIAL_CMD_REG);
 	for (bit = 0x8000; bit; bit >>= 1) {
-		ni_writeb(dev, 0x04 | ((bit & bitstring) ? 0x02 : 0),
-			  Serial_Command);
-		ni_writeb(dev, 0x05 | ((bit & bitstring) ? 0x02 : 0),
-			  Serial_Command);
+		if (bit & bitstring)
+			cmd |= NI_E_SERIAL_CMD_SDATA;
+		else
+			cmd &= ~NI_E_SERIAL_CMD_SDATA;
+
+		ni_writeb(dev, cmd, NI_E_SERIAL_CMD_REG);
+		ni_writeb(dev, NI_E_SERIAL_CMD_SCLK | cmd, NI_E_SERIAL_CMD_REG);
 	}
+	cmd = NI_E_SERIAL_CMD_EEPROM_CS;
 	bitstring = 0;
 	for (bit = 0x80; bit; bit >>= 1) {
-		ni_writeb(dev, 0x04, Serial_Command);
-		ni_writeb(dev, 0x05, Serial_Command);
-		bitstring |= ((ni_readb(dev, XXX_Status) & PROMOUT) ? bit : 0);
+		ni_writeb(dev, cmd, NI_E_SERIAL_CMD_REG);
+		ni_writeb(dev, NI_E_SERIAL_CMD_SCLK | cmd, NI_E_SERIAL_CMD_REG);
+		if (ni_readb(dev, NI_E_STATUS_REG) & NI_E_STATUS_PROMOUT)
+			bitstring |= bit;
 	}
-	ni_writeb(dev, 0x00, Serial_Command);
+	ni_writeb(dev, 0, NI_E_SERIAL_CMD_REG);
 
 	return bitstring;
 }
@@ -4537,7 +4352,7 @@ static unsigned ni_m_series_get_pfi_routing(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 	const unsigned array_offset = chan / 3;
 
-	return MSeries_PFI_Output_Select_Source(chan,
+	return NI_M_PFI_OUT_SEL_TO_SRC(chan,
 				devpriv->pfi_output_select_reg[array_offset]);
 }
 
@@ -4545,19 +4360,17 @@ static int ni_m_series_set_pfi_routing(struct comedi_device *dev,
 				       unsigned chan, unsigned source)
 {
 	struct ni_private *devpriv = dev->private;
-	unsigned pfi_reg_index;
-	unsigned array_offset;
+	unsigned index = chan / 3;
+	unsigned short val = devpriv->pfi_output_select_reg[index];
 
 	if ((source & 0x1f) != source)
 		return -EINVAL;
-	pfi_reg_index = 1 + chan / 3;
-	array_offset = pfi_reg_index - 1;
-	devpriv->pfi_output_select_reg[array_offset] &=
-	    ~MSeries_PFI_Output_Select_Mask(chan);
-	devpriv->pfi_output_select_reg[array_offset] |=
-	    MSeries_PFI_Output_Select_Bits(chan, source);
-	ni_writew(dev, devpriv->pfi_output_select_reg[array_offset],
-		  M_Offset_PFI_Output_Select(pfi_reg_index));
+
+	val &= ~NI_M_PFI_OUT_SEL_MASK(chan);
+	val |= NI_M_PFI_OUT_SEL(chan, source);
+	ni_writew(dev, val, NI_M_PFI_OUT_SEL_REG(index));
+	devpriv->pfi_output_select_reg[index] = val;
+
 	return 2;
 }
 
@@ -4590,10 +4403,10 @@ static int ni_config_filter(struct comedi_device *dev,
 	if (!devpriv->is_m_series)
 		return -ENOTSUPP;
 
-	bits = ni_readl(dev, M_Offset_PFI_Filter);
-	bits &= ~MSeries_PFI_Filter_Select_Mask(pfi_channel);
-	bits |= MSeries_PFI_Filter_Select_Bits(pfi_channel, filter);
-	ni_writel(dev, bits, M_Offset_PFI_Filter);
+	bits = ni_readl(dev, NI_M_PFI_FILTER_REG);
+	bits &= ~NI_M_PFI_FILTER_SEL_MASK(pfi_channel);
+	bits |= NI_M_PFI_FILTER_SEL(pfi_channel, filter);
+	ni_writel(dev, bits, NI_M_PFI_FILTER_REG);
 	return 0;
 }
 
@@ -4612,10 +4425,10 @@ static int ni_pfi_insn_config(struct comedi_device *dev,
 
 	switch (data[0]) {
 	case COMEDI_OUTPUT:
-		ni_set_bits(dev, IO_Bidirection_Pin_Register, 1 << chan, 1);
+		ni_set_bits(dev, NISTC_IO_BIDIR_PIN_REG, 1 << chan, 1);
 		break;
 	case COMEDI_INPUT:
-		ni_set_bits(dev, IO_Bidirection_Pin_Register, 1 << chan, 0);
+		ni_set_bits(dev, NISTC_IO_BIDIR_PIN_REG, 1 << chan, 0);
 		break;
 	case INSN_CONFIG_DIO_QUERY:
 		data[1] =
@@ -4646,9 +4459,9 @@ static int ni_pfi_insn_bits(struct comedi_device *dev,
 		return -ENOTSUPP;
 
 	if (comedi_dio_update_state(s, data))
-		ni_writew(dev, s->state, M_Offset_PFI_DO);
+		ni_writew(dev, s->state, NI_M_PFI_DO_REG);
 
-	data[1] = ni_readw(dev, M_Offset_PFI_DI);
+	data[1] = ni_readw(dev, NI_M_PFI_DI_REG);
 
 	return insn->n;
 }
@@ -4660,8 +4473,8 @@ static int cs5529_wait_for_idle(struct comedi_device *dev)
 	int i;
 
 	for (i = 0; i < timeout; i++) {
-		status = ni_ao_win_inw(dev, CAL_ADC_Status_67xx);
-		if ((status & CSS_ADC_BUSY) == 0)
+		status = ni_ao_win_inw(dev, NI67XX_CAL_STATUS_REG);
+		if ((status & NI67XX_CAL_STATUS_BUSY) == 0)
 			break;
 		set_current_state(TASK_INTERRUPTIBLE);
 		if (schedule_timeout(1))
@@ -4679,13 +4492,14 @@ static void cs5529_command(struct comedi_device *dev, unsigned short value)
 	static const int timeout = 100;
 	int i;
 
-	ni_ao_win_outw(dev, value, CAL_ADC_Command_67xx);
+	ni_ao_win_outw(dev, value, NI67XX_CAL_CMD_REG);
 	/* give time for command to start being serially clocked into cs5529.
-	 * this insures that the CSS_ADC_BUSY bit will get properly
+	 * this insures that the NI67XX_CAL_STATUS_BUSY bit will get properly
 	 * set before we exit this function.
 	 */
 	for (i = 0; i < timeout; i++) {
-		if ((ni_ao_win_inw(dev, CAL_ADC_Status_67xx) & CSS_ADC_BUSY))
+		if (ni_ao_win_inw(dev, NI67XX_CAL_STATUS_REG) &
+		    NI67XX_CAL_STATUS_BUSY)
 			break;
 		udelay(1);
 	}
@@ -4700,25 +4514,25 @@ static int cs5529_do_conversion(struct comedi_device *dev,
 	int retval;
 	unsigned short status;
 
-	cs5529_command(dev, CSCMD_COMMAND | CSCMD_SINGLE_CONVERSION);
+	cs5529_command(dev, CS5529_CMD_CB | CS5529_CMD_SINGLE_CONV);
 	retval = cs5529_wait_for_idle(dev);
 	if (retval) {
 		dev_err(dev->class_dev,
 			"timeout or signal in cs5529_do_conversion()\n");
 		return -ETIME;
 	}
-	status = ni_ao_win_inw(dev, CAL_ADC_Status_67xx);
-	if (status & CSS_OSC_DETECT) {
+	status = ni_ao_win_inw(dev, NI67XX_CAL_STATUS_REG);
+	if (status & NI67XX_CAL_STATUS_OSC_DETECT) {
 		dev_err(dev->class_dev,
 			"cs5529 conversion error, status CSS_OSC_DETECT\n");
 		return -EIO;
 	}
-	if (status & CSS_OVERRANGE) {
+	if (status & NI67XX_CAL_STATUS_OVERRANGE) {
 		dev_err(dev->class_dev,
 			"cs5529 conversion error, overrange (ignoring)\n");
 	}
 	if (data) {
-		*data = ni_ao_win_inw(dev, CAL_ADC_Data_67xx);
+		*data = ni_ao_win_inw(dev, NI67XX_CAL_DATA_REG);
 		/* cs5529 returns 16 bit signed data in bipolar mode */
 		*data ^= (1 << 15);
 	}
@@ -4742,7 +4556,7 @@ static int cs5529_ai_insn_read(struct comedi_device *dev,
 		channel_select = INTERNAL_REF;
 	else
 		channel_select = CR_CHAN(insn->chanspec);
-	ni_ao_win_outw(dev, channel_select, AO_Calibration_Channel_Select_67xx);
+	ni_ao_win_outw(dev, channel_select, NI67XX_AO_CAL_CHAN_SEL_REG);
 
 	for (n = 0; n < insn->n; n++) {
 		retval = cs5529_do_conversion(dev, &sample);
@@ -4756,12 +4570,10 @@ static int cs5529_ai_insn_read(struct comedi_device *dev,
 static void cs5529_config_write(struct comedi_device *dev, unsigned int value,
 				unsigned int reg_select_bits)
 {
-	ni_ao_win_outw(dev, ((value >> 16) & 0xff),
-		       CAL_ADC_Config_Data_High_Word_67xx);
-	ni_ao_win_outw(dev, (value & 0xffff),
-		       CAL_ADC_Config_Data_Low_Word_67xx);
-	reg_select_bits &= CSCMD_REGISTER_SELECT_MASK;
-	cs5529_command(dev, CSCMD_COMMAND | reg_select_bits);
+	ni_ao_win_outw(dev, (value >> 16) & 0xff, NI67XX_CAL_CFG_HI_REG);
+	ni_ao_win_outw(dev, value & 0xffff, NI67XX_CAL_CFG_LO_REG);
+	reg_select_bits &= CS5529_CMD_REG_MASK;
+	cs5529_command(dev, CS5529_CMD_CB | reg_select_bits);
 	if (cs5529_wait_for_idle(dev))
 		dev_err(dev->class_dev,
 			"timeout or signal in %s\n", __func__);
@@ -4769,20 +4581,20 @@ static void cs5529_config_write(struct comedi_device *dev, unsigned int value,
 
 static int init_cs5529(struct comedi_device *dev)
 {
-	unsigned int config_bits =
-	    CSCFG_PORT_MODE | CSCFG_WORD_RATE_2180_CYCLES;
+	unsigned int config_bits = CS5529_CFG_PORT_FLAG |
+				   CS5529_CFG_WORD_RATE_2180;
 
 #if 1
 	/* do self-calibration */
-	cs5529_config_write(dev, config_bits | CSCFG_SELF_CAL_OFFSET_GAIN,
-			    CSCMD_CONFIG_REGISTER);
+	cs5529_config_write(dev, config_bits | CS5529_CFG_CALIB_BOTH_SELF,
+			    CS5529_CFG_REG);
 	/* need to force a conversion for calibration to run */
 	cs5529_do_conversion(dev, NULL);
 #else
 	/* force gain calibration to 1 */
-	cs5529_config_write(dev, 0x400000, CSCMD_GAIN_REGISTER);
-	cs5529_config_write(dev, config_bits | CSCFG_SELF_CAL_OFFSET,
-			    CSCMD_CONFIG_REGISTER);
+	cs5529_config_write(dev, 0x400000, CS5529_GAIN_REG);
+	cs5529_config_write(dev, config_bits | CS5529_CFG_CALIB_OFFSET_SELF,
+			    CS5529_CFG_REG);
 	if (cs5529_wait_for_idle(dev))
 		dev_err(dev->class_dev,
 			"timeout or signal in %s\n", __func__);
@@ -4801,10 +4613,8 @@ static int ni_mseries_get_pll_parameters(unsigned reference_period_ns,
 {
 	unsigned div;
 	unsigned best_div = 1;
-	static const unsigned max_div = 0x10;
 	unsigned mult;
 	unsigned best_mult = 1;
-	static const unsigned max_mult = 0x100;
 	static const unsigned pico_per_nano = 1000;
 
 	const unsigned reference_picosec = reference_period_ns * pico_per_nano;
@@ -4814,8 +4624,8 @@ static int ni_mseries_get_pll_parameters(unsigned reference_period_ns,
 	static const unsigned fudge_factor_80_to_20Mhz = 4;
 	int best_period_picosec = 0;
 
-	for (div = 1; div <= max_div; ++div) {
-		for (mult = 1; mult <= max_mult; ++mult) {
+	for (div = 1; div <= NI_M_PLL_MAX_DIVISOR; ++div) {
+		for (mult = 1; mult <= NI_M_PLL_MAX_MULTIPLIER; ++mult) {
 			unsigned new_period_ps =
 			    (reference_picosec * div) / mult;
 			if (abs(new_period_ps - target_picosec) <
@@ -4847,6 +4657,7 @@ static int ni_mseries_set_pll_master_clock(struct comedi_device *dev,
 	unsigned pll_control_bits;
 	unsigned freq_divider;
 	unsigned freq_multiplier;
+	unsigned rtsi;
 	unsigned i;
 	int retval;
 
@@ -4859,42 +4670,31 @@ static int ni_mseries_set_pll_master_clock(struct comedi_device *dev,
 			__func__, min_period_ns, max_period_ns);
 		return -EINVAL;
 	}
-	devpriv->rtsi_trig_direction_reg &= ~Use_RTSI_Clock_Bit;
+	devpriv->rtsi_trig_direction_reg &= ~NISTC_RTSI_TRIG_USE_CLK;
 	ni_stc_writew(dev, devpriv->rtsi_trig_direction_reg,
-		      RTSI_Trig_Direction_Register);
-	pll_control_bits =
-	    MSeries_PLL_Enable_Bit | MSeries_PLL_VCO_Mode_75_150MHz_Bits;
-	devpriv->clock_and_fout2 |=
-	    MSeries_Timebase1_Select_Bit | MSeries_Timebase3_Select_Bit;
-	devpriv->clock_and_fout2 &= ~MSeries_PLL_In_Source_Select_Mask;
+		      NISTC_RTSI_TRIG_DIR_REG);
+	pll_control_bits = NI_M_PLL_CTRL_ENA | NI_M_PLL_CTRL_VCO_MODE_75_150MHZ;
+	devpriv->clock_and_fout2 |= NI_M_CLK_FOUT2_TIMEBASE1_PLL |
+				    NI_M_CLK_FOUT2_TIMEBASE3_PLL;
+	devpriv->clock_and_fout2 &= ~NI_M_CLK_FOUT2_PLL_SRC_MASK;
 	switch (source) {
 	case NI_MIO_PLL_PXI_STAR_TRIGGER_CLOCK:
-		devpriv->clock_and_fout2 |=
-		    MSeries_PLL_In_Source_Select_Star_Trigger_Bits;
+		devpriv->clock_and_fout2 |= NI_M_CLK_FOUT2_PLL_SRC_STAR;
 		break;
 	case NI_MIO_PLL_PXI10_CLOCK:
 		/* pxi clock is 10MHz */
-		devpriv->clock_and_fout2 |=
-		    MSeries_PLL_In_Source_Select_PXI_Clock10;
+		devpriv->clock_and_fout2 |= NI_M_CLK_FOUT2_PLL_SRC_PXI10;
 		break;
 	default:
-		{
-			unsigned rtsi_channel;
-			static const unsigned max_rtsi_channel = 7;
-
-			for (rtsi_channel = 0; rtsi_channel <= max_rtsi_channel;
-			     ++rtsi_channel) {
-				if (source ==
-				    NI_MIO_PLL_RTSI_CLOCK(rtsi_channel)) {
-					devpriv->clock_and_fout2 |=
-					    MSeries_PLL_In_Source_Select_RTSI_Bits
-					    (rtsi_channel);
-					break;
-				}
+		for (rtsi = 0; rtsi <= NI_M_MAX_RTSI_CHAN; ++rtsi) {
+			if (source == NI_MIO_PLL_RTSI_CLOCK(rtsi)) {
+				devpriv->clock_and_fout2 |=
+					NI_M_CLK_FOUT2_PLL_SRC_RTSI(rtsi);
+				break;
 			}
-			if (rtsi_channel > max_rtsi_channel)
-				return -EINVAL;
 		}
+		if (rtsi > NI_M_MAX_RTSI_CHAN)
+			return -EINVAL;
 		break;
 	}
 	retval = ni_mseries_get_pll_parameters(period_ns,
@@ -4907,16 +4707,15 @@ static int ni_mseries_set_pll_master_clock(struct comedi_device *dev,
 		return retval;
 	}
 
-	ni_writew(dev, devpriv->clock_and_fout2, M_Offset_Clock_and_Fout2);
-	pll_control_bits |=
-	    MSeries_PLL_Divisor_Bits(freq_divider) |
-	    MSeries_PLL_Multiplier_Bits(freq_multiplier);
+	ni_writew(dev, devpriv->clock_and_fout2, NI_M_CLK_FOUT2_REG);
+	pll_control_bits |= NI_M_PLL_CTRL_DIVISOR(freq_divider) |
+			    NI_M_PLL_CTRL_MULTIPLIER(freq_multiplier);
 
-	ni_writew(dev, pll_control_bits, M_Offset_PLL_Control);
+	ni_writew(dev, pll_control_bits, NI_M_PLL_CTRL_REG);
 	devpriv->clock_source = source;
 	/* it seems to typically take a few hundred microseconds for PLL to lock */
 	for (i = 0; i < timeout; ++i) {
-		if (ni_readw(dev, M_Offset_PLL_Status) & MSeries_PLL_Locked_Bit)
+		if (ni_readw(dev, NI_M_PLL_STATUS_REG) & NI_M_PLL_STATUS_LOCKED)
 			break;
 		udelay(1);
 	}
@@ -4935,17 +4734,17 @@ static int ni_set_master_clock(struct comedi_device *dev,
 	struct ni_private *devpriv = dev->private;
 
 	if (source == NI_MIO_INTERNAL_CLOCK) {
-		devpriv->rtsi_trig_direction_reg &= ~Use_RTSI_Clock_Bit;
+		devpriv->rtsi_trig_direction_reg &= ~NISTC_RTSI_TRIG_USE_CLK;
 		ni_stc_writew(dev, devpriv->rtsi_trig_direction_reg,
-			      RTSI_Trig_Direction_Register);
+			      NISTC_RTSI_TRIG_DIR_REG);
 		devpriv->clock_ns = TIMEBASE_1_NS;
 		if (devpriv->is_m_series) {
 			devpriv->clock_and_fout2 &=
-			    ~(MSeries_Timebase1_Select_Bit |
-			      MSeries_Timebase3_Select_Bit);
+			    ~(NI_M_CLK_FOUT2_TIMEBASE1_PLL |
+			      NI_M_CLK_FOUT2_TIMEBASE3_PLL);
 			ni_writew(dev, devpriv->clock_and_fout2,
-				  M_Offset_Clock_and_Fout2);
-			ni_writew(dev, 0, M_Offset_PLL_Control);
+				  NI_M_CLK_FOUT2_REG);
+			ni_writew(dev, 0, NI_M_PLL_CTRL_REG);
 		}
 		devpriv->clock_source = source;
 	} else {
@@ -4955,10 +4754,10 @@ static int ni_set_master_clock(struct comedi_device *dev,
 		} else {
 			if (source == NI_MIO_RTSI_CLOCK) {
 				devpriv->rtsi_trig_direction_reg |=
-				    Use_RTSI_Clock_Bit;
+				    NISTC_RTSI_TRIG_USE_CLK;
 				ni_stc_writew(dev,
 					      devpriv->rtsi_trig_direction_reg,
-					      RTSI_Trig_Direction_Register);
+					      NISTC_RTSI_TRIG_DIR_REG);
 				if (period_ns == 0) {
 					dev_err(dev->class_dev,
 						"we don't handle an unspecified clock period correctly yet, returning error\n");
@@ -4974,26 +4773,19 @@ static int ni_set_master_clock(struct comedi_device *dev,
 	return 3;
 }
 
-static unsigned num_configurable_rtsi_channels(struct comedi_device *dev)
-{
-	struct ni_private *devpriv = dev->private;
-
-	return (devpriv->is_m_series) ? 8 : 7;
-}
-
 static int ni_valid_rtsi_output_source(struct comedi_device *dev,
 				       unsigned chan, unsigned source)
 {
 	struct ni_private *devpriv = dev->private;
 
-	if (chan >= num_configurable_rtsi_channels(dev)) {
-		if (chan == old_RTSI_clock_channel) {
+	if (chan >= NISTC_RTSI_TRIG_NUM_CHAN(devpriv->is_m_series)) {
+		if (chan == NISTC_RTSI_TRIG_OLD_CLK_CHAN) {
 			if (source == NI_RTSI_OUTPUT_RTSI_OSC)
 				return 1;
 
 			dev_err(dev->class_dev,
 				"%s: invalid source for channel=%i, channel %i is always the RTSI clock for pre-m-series boards\n",
-				__func__, chan, old_RTSI_clock_channel);
+				__func__, chan, NISTC_RTSI_TRIG_OLD_CLK_CHAN);
 			return 0;
 		}
 		return 0;
@@ -5017,24 +4809,22 @@ static int ni_valid_rtsi_output_source(struct comedi_device *dev,
 }
 
 static int ni_set_rtsi_routing(struct comedi_device *dev,
-			       unsigned chan, unsigned source)
+			       unsigned chan, unsigned src)
 {
 	struct ni_private *devpriv = dev->private;
 
-	if (ni_valid_rtsi_output_source(dev, chan, source) == 0)
+	if (ni_valid_rtsi_output_source(dev, chan, src) == 0)
 		return -EINVAL;
 	if (chan < 4) {
-		devpriv->rtsi_trig_a_output_reg &= ~RTSI_Trig_Output_Mask(chan);
-		devpriv->rtsi_trig_a_output_reg |=
-		    RTSI_Trig_Output_Bits(chan, source);
+		devpriv->rtsi_trig_a_output_reg &= ~NISTC_RTSI_TRIG_MASK(chan);
+		devpriv->rtsi_trig_a_output_reg |= NISTC_RTSI_TRIG(chan, src);
 		ni_stc_writew(dev, devpriv->rtsi_trig_a_output_reg,
-			      RTSI_Trig_A_Output_Register);
+			      NISTC_RTSI_TRIGA_OUT_REG);
 	} else if (chan < 8) {
-		devpriv->rtsi_trig_b_output_reg &= ~RTSI_Trig_Output_Mask(chan);
-		devpriv->rtsi_trig_b_output_reg |=
-		    RTSI_Trig_Output_Bits(chan, source);
+		devpriv->rtsi_trig_b_output_reg &= ~NISTC_RTSI_TRIG_MASK(chan);
+		devpriv->rtsi_trig_b_output_reg |= NISTC_RTSI_TRIG(chan, src);
 		ni_stc_writew(dev, devpriv->rtsi_trig_b_output_reg,
-			      RTSI_Trig_B_Output_Register);
+			      NISTC_RTSI_TRIGB_OUT_REG);
 	}
 	return 2;
 }
@@ -5044,13 +4834,13 @@ static unsigned ni_get_rtsi_routing(struct comedi_device *dev, unsigned chan)
 	struct ni_private *devpriv = dev->private;
 
 	if (chan < 4) {
-		return RTSI_Trig_Output_Source(chan,
-					       devpriv->rtsi_trig_a_output_reg);
-	} else if (chan < num_configurable_rtsi_channels(dev)) {
-		return RTSI_Trig_Output_Source(chan,
-					       devpriv->rtsi_trig_b_output_reg);
+		return NISTC_RTSI_TRIG_TO_SRC(chan,
+					      devpriv->rtsi_trig_a_output_reg);
+	} else if (chan < NISTC_RTSI_TRIG_NUM_CHAN(devpriv->is_m_series)) {
+		return NISTC_RTSI_TRIG_TO_SRC(chan,
+					      devpriv->rtsi_trig_b_output_reg);
 	} else {
-		if (chan == old_RTSI_clock_channel)
+		if (chan == NISTC_RTSI_TRIG_OLD_CLK_CHAN)
 			return NI_RTSI_OUTPUT_RTSI_OSC;
 		dev_err(dev->class_dev, "bug! should never get here?\n");
 		return 0;
@@ -5064,42 +4854,43 @@ static int ni_rtsi_insn_config(struct comedi_device *dev,
 {
 	struct ni_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int max_chan = NISTC_RTSI_TRIG_NUM_CHAN(devpriv->is_m_series);
 
 	switch (data[0]) {
 	case INSN_CONFIG_DIO_OUTPUT:
-		if (chan < num_configurable_rtsi_channels(dev)) {
+		if (chan < max_chan) {
 			devpriv->rtsi_trig_direction_reg |=
-			    RTSI_Output_Bit(chan, devpriv->is_m_series);
-		} else if (chan == old_RTSI_clock_channel) {
+			    NISTC_RTSI_TRIG_DIR(chan, devpriv->is_m_series);
+		} else if (chan == NISTC_RTSI_TRIG_OLD_CLK_CHAN) {
 			devpriv->rtsi_trig_direction_reg |=
-			    Drive_RTSI_Clock_Bit;
+			    NISTC_RTSI_TRIG_DRV_CLK;
 		}
 		ni_stc_writew(dev, devpriv->rtsi_trig_direction_reg,
-			      RTSI_Trig_Direction_Register);
+			      NISTC_RTSI_TRIG_DIR_REG);
 		break;
 	case INSN_CONFIG_DIO_INPUT:
-		if (chan < num_configurable_rtsi_channels(dev)) {
+		if (chan < max_chan) {
 			devpriv->rtsi_trig_direction_reg &=
-			    ~RTSI_Output_Bit(chan, devpriv->is_m_series);
-		} else if (chan == old_RTSI_clock_channel) {
+			    ~NISTC_RTSI_TRIG_DIR(chan, devpriv->is_m_series);
+		} else if (chan == NISTC_RTSI_TRIG_OLD_CLK_CHAN) {
 			devpriv->rtsi_trig_direction_reg &=
-			    ~Drive_RTSI_Clock_Bit;
+			    ~NISTC_RTSI_TRIG_DRV_CLK;
 		}
 		ni_stc_writew(dev, devpriv->rtsi_trig_direction_reg,
-			      RTSI_Trig_Direction_Register);
+			      NISTC_RTSI_TRIG_DIR_REG);
 		break;
 	case INSN_CONFIG_DIO_QUERY:
-		if (chan < num_configurable_rtsi_channels(dev)) {
+		if (chan < max_chan) {
 			data[1] =
 			    (devpriv->rtsi_trig_direction_reg &
-			     RTSI_Output_Bit(chan, devpriv->is_m_series))
+			     NISTC_RTSI_TRIG_DIR(chan, devpriv->is_m_series))
 				? INSN_CONFIG_DIO_OUTPUT
 				: INSN_CONFIG_DIO_INPUT;
-		} else if (chan == old_RTSI_clock_channel) {
-			data[1] =
-			    (devpriv->rtsi_trig_direction_reg &
-			     Drive_RTSI_Clock_Bit)
-			    ? INSN_CONFIG_DIO_OUTPUT : INSN_CONFIG_DIO_INPUT;
+		} else if (chan == NISTC_RTSI_TRIG_OLD_CLK_CHAN) {
+			data[1] = (devpriv->rtsi_trig_direction_reg &
+				   NISTC_RTSI_TRIG_DRV_CLK)
+				  ? INSN_CONFIG_DIO_OUTPUT
+				  : INSN_CONFIG_DIO_INPUT;
 		}
 		return 2;
 	case INSN_CONFIG_SET_CLOCK_SRC:
@@ -5135,37 +4926,37 @@ static void ni_rtsi_init(struct comedi_device *dev)
 
 	/*  Initialises the RTSI bus signal switch to a default state */
 
+	/*
+	 * Use 10MHz instead of 20MHz for RTSI clock frequency. Appears
+	 * to have no effect, at least on pxi-6281, which always uses
+	 * 20MHz rtsi clock frequency
+	 */
+	devpriv->clock_and_fout2 = NI_M_CLK_FOUT2_RTSI_10MHZ;
 	/*  Set clock mode to internal */
-	devpriv->clock_and_fout2 = MSeries_RTSI_10MHz_Bit;
 	if (ni_set_master_clock(dev, NI_MIO_INTERNAL_CLOCK, 0) < 0)
 		dev_err(dev->class_dev, "ni_set_master_clock failed, bug?\n");
 	/*  default internal lines routing to RTSI bus lines */
 	devpriv->rtsi_trig_a_output_reg =
-	    RTSI_Trig_Output_Bits(0,
-				  NI_RTSI_OUTPUT_ADR_START1) |
-	    RTSI_Trig_Output_Bits(1,
-				  NI_RTSI_OUTPUT_ADR_START2) |
-	    RTSI_Trig_Output_Bits(2,
-				  NI_RTSI_OUTPUT_SCLKG) |
-	    RTSI_Trig_Output_Bits(3, NI_RTSI_OUTPUT_DACUPDN);
+	    NISTC_RTSI_TRIG(0, NI_RTSI_OUTPUT_ADR_START1) |
+	    NISTC_RTSI_TRIG(1, NI_RTSI_OUTPUT_ADR_START2) |
+	    NISTC_RTSI_TRIG(2, NI_RTSI_OUTPUT_SCLKG) |
+	    NISTC_RTSI_TRIG(3, NI_RTSI_OUTPUT_DACUPDN);
 	ni_stc_writew(dev, devpriv->rtsi_trig_a_output_reg,
-		      RTSI_Trig_A_Output_Register);
+		      NISTC_RTSI_TRIGA_OUT_REG);
 	devpriv->rtsi_trig_b_output_reg =
-	    RTSI_Trig_Output_Bits(4,
-				  NI_RTSI_OUTPUT_DA_START1) |
-	    RTSI_Trig_Output_Bits(5,
-				  NI_RTSI_OUTPUT_G_SRC0) |
-	    RTSI_Trig_Output_Bits(6, NI_RTSI_OUTPUT_G_GATE0);
+	    NISTC_RTSI_TRIG(4, NI_RTSI_OUTPUT_DA_START1) |
+	    NISTC_RTSI_TRIG(5, NI_RTSI_OUTPUT_G_SRC0) |
+	    NISTC_RTSI_TRIG(6, NI_RTSI_OUTPUT_G_GATE0);
 	if (devpriv->is_m_series)
 		devpriv->rtsi_trig_b_output_reg |=
-		    RTSI_Trig_Output_Bits(7, NI_RTSI_OUTPUT_RTSI_OSC);
+		    NISTC_RTSI_TRIG(7, NI_RTSI_OUTPUT_RTSI_OSC);
 	ni_stc_writew(dev, devpriv->rtsi_trig_b_output_reg,
-		      RTSI_Trig_B_Output_Register);
+		      NISTC_RTSI_TRIGB_OUT_REG);
 
-/*
-* Sets the source and direction of the 4 on board lines
-* ni_stc_writew(dev, 0x0000, RTSI_Board_Register);
-*/
+	/*
+	 * Sets the source and direction of the 4 on board lines
+	 * ni_stc_writew(dev, 0, NISTC_RTSI_BOARD_REG);
+	 */
 }
 
 #ifdef PCIDMA
@@ -5199,88 +4990,6 @@ static int ni_gpct_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 }
 #endif
 
-#if 0
-/*
- *	Read the GPCTs current value.
- */
-static int GPCT_G_Watch(struct comedi_device *dev, int chan)
-{
-	unsigned int hi1, hi2, lo;
-
-	devpriv->gpct_command[chan] &= ~G_Save_Trace;
-	ni_stc_writew(dev, devpriv->gpct_command[chan],
-		      G_Command_Register(chan));
-
-	devpriv->gpct_command[chan] |= G_Save_Trace;
-	ni_stc_writew(dev, devpriv->gpct_command[chan],
-		      G_Command_Register(chan));
-
-	/* This procedure is used because the two registers cannot
-	 * be read atomically. */
-	do {
-		hi1 = ni_stc_readw(dev, G_Save_Register_High(chan));
-		lo = ni_stc_readw(dev, G_Save_Register_Low(chan));
-		hi2 = ni_stc_readw(dev, G_Save_Register_High(chan));
-	} while (hi1 != hi2);
-
-	return (hi1 << 16) | lo;
-}
-
-static void GPCT_Reset(struct comedi_device *dev, int chan)
-{
-	int temp_ack_reg = 0;
-
-	devpriv->gpct_cur_operation[chan] = GPCT_RESET;
-
-	switch (chan) {
-	case 0:
-		ni_stc_writew(dev, G0_Reset, Joint_Reset_Register);
-		ni_set_bits(dev, Interrupt_A_Enable_Register,
-			    G0_TC_Interrupt_Enable, 0);
-		ni_set_bits(dev, Interrupt_A_Enable_Register,
-			    G0_Gate_Interrupt_Enable, 0);
-		temp_ack_reg |= G0_Gate_Error_Confirm;
-		temp_ack_reg |= G0_TC_Error_Confirm;
-		temp_ack_reg |= G0_TC_Interrupt_Ack;
-		temp_ack_reg |= G0_Gate_Interrupt_Ack;
-		ni_stc_writew(dev, temp_ack_reg, Interrupt_A_Ack_Register);
-
-		/* problem...this interferes with the other ctr... */
-		devpriv->an_trig_etc_reg |= GPFO_0_Output_Enable;
-		ni_stc_writew(dev, devpriv->an_trig_etc_reg,
-			      Analog_Trigger_Etc_Register);
-		break;
-	case 1:
-		ni_stc_writew(dev, G1_Reset, Joint_Reset_Register);
-		ni_set_bits(dev, Interrupt_B_Enable_Register,
-			    G1_TC_Interrupt_Enable, 0);
-		ni_set_bits(dev, Interrupt_B_Enable_Register,
-			    G0_Gate_Interrupt_Enable, 0);
-		temp_ack_reg |= G1_Gate_Error_Confirm;
-		temp_ack_reg |= G1_TC_Error_Confirm;
-		temp_ack_reg |= G1_TC_Interrupt_Ack;
-		temp_ack_reg |= G1_Gate_Interrupt_Ack;
-		ni_stc_writew(dev, temp_ack_reg, Interrupt_B_Ack_Register);
-
-		devpriv->an_trig_etc_reg |= GPFO_1_Output_Enable;
-		ni_stc_writew(dev, devpriv->an_trig_etc_reg,
-			      Analog_Trigger_Etc_Register);
-		break;
-	}
-
-	devpriv->gpct_mode[chan] = 0;
-	devpriv->gpct_input_select[chan] = 0;
-	devpriv->gpct_command[chan] = 0;
-
-	devpriv->gpct_command[chan] |= G_Synchronized_Gate;
-
-	ni_stc_writew(dev, devpriv->gpct_mode[chan], G_Mode_Register(chan));
-	ni_stc_writew(dev, devpriv->gpct_input_select[chan],
-		      G_Input_Select_Register(chan));
-	ni_stc_writew(dev, 0, G_Autoincrement_Register(chan));
-}
-#endif
-
 static irqreturn_t ni_E_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
@@ -5300,8 +5009,8 @@ static irqreturn_t ni_E_interrupt(int irq, void *d)
 
 	/*  lock to avoid race with comedi_poll */
 	spin_lock_irqsave(&dev->spinlock, flags);
-	a_status = ni_stc_readw(dev, AI_Status_1_Register);
-	b_status = ni_stc_readw(dev, AO_Status_1_Register);
+	a_status = ni_stc_readw(dev, NISTC_AI_STATUS1_REG);
+	b_status = ni_stc_readw(dev, NISTC_AO_STATUS1_REG);
 #ifdef PCIDMA
 	if (mite) {
 		struct ni_private *devpriv = dev->private;
@@ -5329,9 +5038,9 @@ static irqreturn_t ni_E_interrupt(int irq, void *d)
 #endif
 	ack_a_interrupt(dev, a_status);
 	ack_b_interrupt(dev, b_status);
-	if ((a_status & Interrupt_A_St) || (ai_mite_status & CHSR_INT))
+	if ((a_status & NISTC_AI_STATUS1_INTA) || (ai_mite_status & CHSR_INT))
 		handle_a_interrupt(dev, a_status, ai_mite_status);
-	if ((b_status & Interrupt_B_St) || (ao_mite_status & CHSR_INT))
+	if ((b_status & NISTC_AO_STATUS1_INTB) || (ao_mite_status & CHSR_INT))
 		handle_b_interrupt(dev, b_status, ao_mite_status);
 	handle_gpct_interrupt(dev, 0);
 	handle_gpct_interrupt(dev, 1);
@@ -5371,16 +5080,16 @@ static int ni_E_init(struct comedi_device *dev,
 	}
 
 	/* initialize clock dividers */
-	devpriv->clock_and_fout = Slow_Internal_Time_Divide_By_2 |
-				  Slow_Internal_Timebase |
-				  Clock_To_Board_Divide_By_2 |
-				  Clock_To_Board;
+	devpriv->clock_and_fout = NISTC_CLK_FOUT_SLOW_DIV2 |
+				  NISTC_CLK_FOUT_SLOW_TIMEBASE |
+				  NISTC_CLK_FOUT_TO_BOARD_DIV2 |
+				  NISTC_CLK_FOUT_TO_BOARD;
 	if (!devpriv->is_6xxx) {
 		/* BEAM is this needed for PCI-6143 ?? */
-		devpriv->clock_and_fout |= (AI_Output_Divide_By_2 |
-					    AO_Output_Divide_By_2);
+		devpriv->clock_and_fout |= (NISTC_CLK_FOUT_AI_OUT_DIV2 |
+					    NISTC_CLK_FOUT_AO_OUT_DIV2);
 	}
-	ni_stc_writew(dev, devpriv->clock_and_fout, Clock_and_FOUT_Register);
+	ni_stc_writew(dev, devpriv->clock_and_fout, NISTC_CLK_FOUT_REG);
 
 	ret = comedi_alloc_subdevices(dev, NI_NUM_SUBDEVICES);
 	if (ret)
@@ -5489,22 +5198,24 @@ static int ni_E_init(struct comedi_device *dev,
 		}
 
 		/* reset DIO and set all channels to inputs */
-		ni_writel(dev, CDO_Reset_Bit | CDI_Reset_Bit,
-			  M_Offset_CDIO_Command);
-		ni_writel(dev, s->io_bits, M_Offset_DIO_Direction);
+		ni_writel(dev, NI_M_CDO_CMD_RESET |
+			       NI_M_CDI_CMD_RESET,
+			  NI_M_CDIO_CMD_REG);
+		ni_writel(dev, s->io_bits, NI_M_DIO_DIR_REG);
 	} else {
 		s->insn_bits	= ni_dio_insn_bits;
 		s->insn_config	= ni_dio_insn_config;
 
 		/* set all channels to inputs */
-		devpriv->dio_control = DIO_Pins_Dir(s->io_bits);
-		ni_writew(dev, devpriv->dio_control, DIO_Control_Register);
+		devpriv->dio_control = NISTC_DIO_CTRL_DIR(s->io_bits);
+		ni_writew(dev, devpriv->dio_control, NISTC_DIO_CTRL_REG);
 	}
 
 	/* 8255 device */
 	s = &dev->subdevices[NI_8255_DIO_SUBDEV];
 	if (board->has_8255) {
-		ret = subdev_8255_init(dev, s, ni_8255_callback, Port_A);
+		ret = subdev_8255_init(dev, s, ni_8255_callback,
+				       NI_E_8255_BASE);
 		if (ret)
 			return ret;
 	} else {
@@ -5525,7 +5236,7 @@ static int ni_E_init(struct comedi_device *dev,
 		/* internal PWM output used for AI nonlinearity calibration */
 		s->insn_config	= ni_m_series_pwm_config;
 
-		ni_writel(dev, 0x0, M_Offset_Cal_PWM);
+		ni_writel(dev, 0x0, NI_M_CAL_PWM_REG);
 	} else if (devpriv->is_6143) {
 		/* internal PWM output used for AI nonlinearity calibration */
 		s->insn_config	= ni_6143_pwm_config;
@@ -5560,17 +5271,17 @@ static int ni_E_init(struct comedi_device *dev,
 		s->n_chan	= 16;
 		s->insn_bits	= ni_pfi_insn_bits;
 
-		ni_writew(dev, s->state, M_Offset_PFI_DO);
+		ni_writew(dev, s->state, NI_M_PFI_DO_REG);
 		for (i = 0; i < NUM_PFI_OUTPUT_SELECT_REGS; ++i) {
 			ni_writew(dev, devpriv->pfi_output_select_reg[i],
-				  M_Offset_PFI_Output_Select(i + 1));
+				  NI_M_PFI_OUT_SEL_REG(i));
 		}
 	} else {
 		s->n_chan	= 10;
 	}
 	s->insn_config	= ni_pfi_insn_config;
 
-	ni_set_bits(dev, IO_Bidirection_Pin_Register, ~0, 0);
+	ni_set_bits(dev, NISTC_IO_BIDIR_PIN_REG, ~0, 0);
 
 	/* cs5529 calibration adc */
 	s = &dev->subdevices[NI_CS5529_CALIBRATION_SUBDEV];
@@ -5663,30 +5374,31 @@ static int ni_E_init(struct comedi_device *dev,
 
 	if (dev->irq) {
 		ni_stc_writew(dev,
-			      (irq_polarity ? Interrupt_Output_Polarity : 0) |
-			      (Interrupt_Output_On_3_Pins & 0) |
-			      Interrupt_A_Enable | Interrupt_B_Enable |
-			      Interrupt_A_Output_Select(interrupt_pin) |
-			      Interrupt_B_Output_Select(interrupt_pin),
-			      Interrupt_Control_Register);
+			      (irq_polarity ? NISTC_INT_CTRL_INT_POL : 0) |
+			      (NISTC_INT_CTRL_3PIN_INT & 0) |
+			      NISTC_INT_CTRL_INTA_ENA |
+			      NISTC_INT_CTRL_INTB_ENA |
+			      NISTC_INT_CTRL_INTA_SEL(interrupt_pin) |
+			      NISTC_INT_CTRL_INTB_SEL(interrupt_pin),
+			      NISTC_INT_CTRL_REG);
 	}
 
 	/* DMA setup */
-	ni_writeb(dev, devpriv->ai_ao_select_reg, AI_AO_Select);
-	ni_writeb(dev, devpriv->g0_g1_select_reg, G0_G1_Select);
+	ni_writeb(dev, devpriv->ai_ao_select_reg, NI_E_DMA_AI_AO_SEL_REG);
+	ni_writeb(dev, devpriv->g0_g1_select_reg, NI_E_DMA_G0_G1_SEL_REG);
 
 	if (devpriv->is_6xxx) {
-		ni_writeb(dev, 0, Magic_611x);
+		ni_writeb(dev, 0, NI611X_MAGIC_REG);
 	} else if (devpriv->is_m_series) {
 		int channel;
 
 		for (channel = 0; channel < board->n_aochan; ++channel) {
 			ni_writeb(dev, 0xf,
-				  M_Offset_AO_Waveform_Order(channel));
+				  NI_M_AO_WAVEFORM_ORDER_REG(channel));
 			ni_writeb(dev, 0x0,
-				  M_Offset_AO_Reference_Attenuation(channel));
+				  NI_M_AO_REF_ATTENUATION_REG(channel));
 		}
-		ni_writeb(dev, 0x0, M_Offset_AO_Calibration);
+		ni_writeb(dev, 0x0, NI_M_AO_CALIB_REG);
 	}
 
 	return 0;
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index 1481f71a31b1..30a5a75d1fe7 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -1085,26 +1085,25 @@ static void init_6143(struct comedi_device *dev)
 	struct ni_private *devpriv = dev->private;
 
 	/*  Disable interrupts */
-	ni_stc_writew(dev, 0, Interrupt_Control_Register);
+	ni_stc_writew(dev, 0, NISTC_INT_CTRL_REG);
 
 	/*  Initialise 6143 AI specific bits */
 
 	/* Set G0,G1 DMA mode to E series version */
-	ni_writeb(dev, 0x00, Magic_6143);
+	ni_writeb(dev, 0x00, NI6143_MAGIC_REG);
 	/* Set EOCMode, ADCMode and pipelinedelay */
-	ni_writeb(dev, 0x80, PipelineDelay_6143);
+	ni_writeb(dev, 0x80, NI6143_PIPELINE_DELAY_REG);
 	/* Set EOC Delay */
-	ni_writeb(dev, 0x00, EOC_Set_6143);
+	ni_writeb(dev, 0x00, NI6143_EOC_SET_REG);
 
 	/* Set the FIFO half full level */
-	ni_writel(dev, board->ai_fifo_depth / 2, AIFIFO_Flag_6143);
+	ni_writel(dev, board->ai_fifo_depth / 2, NI6143_AI_FIFO_FLAG_REG);
 
 	/*  Strobe Relay disable bit */
 	devpriv->ai_calib_source_enabled = 0;
-	ni_writew(dev, devpriv->ai_calib_source |
-		       Calibration_Channel_6143_RelayOff,
-		  Calibration_Channel_6143);
-	ni_writew(dev, devpriv->ai_calib_source, Calibration_Channel_6143);
+	ni_writew(dev, devpriv->ai_calib_source | NI6143_CALIB_CHAN_RELAY_OFF,
+		  NI6143_CALIB_CHAN_REG);
+	ni_writew(dev, devpriv->ai_calib_source, NI6143_CALIB_CHAN_REG);
 }
 
 static void pcimio_detach(struct comedi_device *dev)
diff --git a/drivers/staging/comedi/drivers/ni_stc.h b/drivers/staging/comedi/drivers/ni_stc.h
index bd69c3f0acdc..1d5af25b92a8 100644
--- a/drivers/staging/comedi/drivers/ni_stc.h
+++ b/drivers/staging/comedi/drivers/ni_stc.h
@@ -26,884 +26,907 @@
 
 #include "ni_tio.h"
 
-#define _bit15		0x8000
-#define _bit14		0x4000
-#define _bit13		0x2000
-#define _bit12		0x1000
-#define _bit11		0x0800
-#define _bit10		0x0400
-#define _bit9		0x0200
-#define _bit8		0x0100
-#define _bit7		0x0080
-#define _bit6		0x0040
-#define _bit5		0x0020
-#define _bit4		0x0010
-#define _bit3		0x0008
-#define _bit2		0x0004
-#define _bit1		0x0002
-#define _bit0		0x0001
-
-#define NUM_PFI_OUTPUT_SELECT_REGS 6
-
-/* Registers in the National Instruments DAQ-STC chip */
-
-#define Interrupt_A_Ack_Register	2
-#define G0_Gate_Interrupt_Ack			_bit15
-#define G0_TC_Interrupt_Ack			_bit14
-#define AI_Error_Interrupt_Ack			_bit13
-#define AI_STOP_Interrupt_Ack			_bit12
-#define AI_START_Interrupt_Ack			_bit11
-#define AI_START2_Interrupt_Ack			_bit10
-#define AI_START1_Interrupt_Ack			_bit9
-#define AI_SC_TC_Interrupt_Ack			_bit8
-#define AI_SC_TC_Error_Confirm			_bit7
-#define G0_TC_Error_Confirm			_bit6
-#define G0_Gate_Error_Confirm			_bit5
-
-#define AI_Status_1_Register		2
-#define Interrupt_A_St				0x8000
-#define AI_FIFO_Full_St				0x4000
-#define AI_FIFO_Half_Full_St			0x2000
-#define AI_FIFO_Empty_St			0x1000
-#define AI_Overrun_St				0x0800
-#define AI_Overflow_St				0x0400
-#define AI_SC_TC_Error_St			0x0200
-#define AI_START2_St				0x0100
-#define AI_START1_St				0x0080
-#define AI_SC_TC_St				0x0040
-#define AI_START_St				0x0020
-#define AI_STOP_St				0x0010
-#define G0_TC_St				0x0008
-#define G0_Gate_Interrupt_St			0x0004
-#define AI_FIFO_Request_St			0x0002
-#define Pass_Thru_0_Interrupt_St		0x0001
-
-#define AI_Status_2_Register		5
-
-#define Interrupt_B_Ack_Register	3
-enum Interrupt_B_Ack_Bits {
-	G1_Gate_Error_Confirm = _bit1,
-	G1_TC_Error_Confirm = _bit2,
-	AO_BC_TC_Trigger_Error_Confirm = _bit3,
-	AO_BC_TC_Error_Confirm = _bit4,
-	AO_UI2_TC_Error_Confrim = _bit5,
-	AO_UI2_TC_Interrupt_Ack = _bit6,
-	AO_UC_TC_Interrupt_Ack = _bit7,
-	AO_BC_TC_Interrupt_Ack = _bit8,
-	AO_START1_Interrupt_Ack = _bit9,
-	AO_UPDATE_Interrupt_Ack = _bit10,
-	AO_START_Interrupt_Ack = _bit11,
-	AO_STOP_Interrupt_Ack = _bit12,
-	AO_Error_Interrupt_Ack = _bit13,
-	G1_TC_Interrupt_Ack = _bit14,
-	G1_Gate_Interrupt_Ack = _bit15
-};
-
-#define AO_Status_1_Register		3
-#define Interrupt_B_St				_bit15
-#define AO_FIFO_Full_St				_bit14
-#define AO_FIFO_Half_Full_St			_bit13
-#define AO_FIFO_Empty_St			_bit12
-#define AO_BC_TC_Error_St			_bit11
-#define AO_START_St				_bit10
-#define AO_Overrun_St				_bit9
-#define AO_START1_St				_bit8
-#define AO_BC_TC_St				_bit7
-#define AO_UC_TC_St				_bit6
-#define AO_UPDATE_St				_bit5
-#define AO_UI2_TC_St				_bit4
-#define G1_TC_St				_bit3
-#define G1_Gate_Interrupt_St			_bit2
-#define AO_FIFO_Request_St			_bit1
-#define Pass_Thru_1_Interrupt_St		_bit0
-
-#define AI_Command_2_Register		4
-#define AI_End_On_SC_TC				_bit15
-#define AI_End_On_End_Of_Scan			_bit14
-#define AI_START1_Disable			_bit11
-#define AI_SC_Save_Trace			_bit10
-#define AI_SI_Switch_Load_On_SC_TC		_bit9
-#define AI_SI_Switch_Load_On_STOP		_bit8
-#define AI_SI_Switch_Load_On_TC			_bit7
-#define AI_SC_Switch_Load_On_TC			_bit4
-#define AI_STOP_Pulse				_bit3
-#define AI_START_Pulse				_bit2
-#define AI_START2_Pulse				_bit1
-#define AI_START1_Pulse				_bit0
-
-#define AO_Command_2_Register		5
-#define AO_End_On_BC_TC(x)			(((x) & 0x3) << 14)
-#define AO_Start_Stop_Gate_Enable		_bit13
-#define AO_UC_Save_Trace			_bit12
-#define AO_BC_Gate_Enable			_bit11
-#define AO_BC_Save_Trace			_bit10
-#define AO_UI_Switch_Load_On_BC_TC		_bit9
-#define AO_UI_Switch_Load_On_Stop		_bit8
-#define AO_UI_Switch_Load_On_TC			_bit7
-#define AO_UC_Switch_Load_On_BC_TC		_bit6
-#define AO_UC_Switch_Load_On_TC			_bit5
-#define AO_BC_Switch_Load_On_TC			_bit4
-#define AO_Mute_B				_bit3
-#define AO_Mute_A				_bit2
-#define AO_UPDATE2_Pulse			_bit1
-#define AO_START1_Pulse				_bit0
-
-#define AO_Status_2_Register		6
-
-#define DIO_Parallel_Input_Register	7
-
-#define AI_Command_1_Register		8
-#define AI_Analog_Trigger_Reset			_bit14
-#define AI_Disarm				_bit13
-#define AI_SI2_Arm				_bit12
-#define AI_SI2_Load				_bit11
-#define AI_SI_Arm				_bit10
-#define AI_SI_Load				_bit9
-#define AI_DIV_Arm				_bit8
-#define AI_DIV_Load				_bit7
-#define AI_SC_Arm				_bit6
-#define AI_SC_Load				_bit5
-#define AI_SCAN_IN_PROG_Pulse			_bit4
-#define AI_EXTMUX_CLK_Pulse			_bit3
-#define AI_LOCALMUX_CLK_Pulse			_bit2
-#define AI_SC_TC_Pulse			 	_bit1
-#define AI_CONVERT_Pulse			_bit0
-
-#define AO_Command_1_Register		9
-#define AO_Analog_Trigger_Reset			_bit15
-#define AO_START_Pulse				_bit14
-#define AO_Disarm				_bit13
-#define AO_UI2_Arm_Disarm			_bit12
-#define AO_UI2_Load				_bit11
-#define AO_UI_Arm				_bit10
-#define AO_UI_Load				_bit9
-#define AO_UC_Arm				_bit8
-#define AO_UC_Load				_bit7
-#define AO_BC_Arm				_bit6
-#define AO_BC_Load				_bit5
-#define AO_DAC1_Update_Mode			_bit4
-#define AO_LDAC1_Source_Select			_bit3
-#define AO_DAC0_Update_Mode			_bit2
-#define AO_LDAC0_Source_Select			_bit1
-#define AO_UPDATE_Pulse				_bit0
-
-#define DIO_Output_Register		10
-#define DIO_Parallel_Data_Out(a)                ((a)&0xff)
-#define DIO_Parallel_Data_Mask                  0xff
-#define DIO_SDOUT                               _bit0
-#define DIO_SDIN                                _bit4
-#define DIO_Serial_Data_Out(a)                  (((a)&0xff)<<8)
-#define DIO_Serial_Data_Mask                    0xff00
-
-#define DIO_Control_Register		11
-#define DIO_Software_Serial_Control             _bit11
-#define DIO_HW_Serial_Timebase                  _bit10
-#define DIO_HW_Serial_Enable                    _bit9
-#define DIO_HW_Serial_Start                     _bit8
-#define DIO_Pins_Dir(a)                         ((a)&0xff)
-#define DIO_Pins_Dir_Mask                       0xff
-
-#define AI_Mode_1_Register		12
-#define AI_CONVERT_Source_Select(a)		(((a) & 0x1f) << 11)
-#define AI_SI_Source_select(a)			(((a) & 0x1f) << 6)
-#define AI_CONVERT_Source_Polarity		_bit5
-#define AI_SI_Source_Polarity		_bit4
-#define AI_Start_Stop				_bit3
-#define AI_Mode_1_Reserved			_bit2
-#define AI_Continuous				_bit1
-#define AI_Trigger_Once				_bit0
-
-#define AI_Mode_2_Register		13
-#define AI_SC_Gate_Enable			_bit15
-#define AI_Start_Stop_Gate_Enable		_bit14
-#define AI_Pre_Trigger				_bit13
-#define AI_External_MUX_Present			_bit12
-#define AI_SI2_Initial_Load_Source		_bit9
-#define AI_SI2_Reload_Mode			_bit8
-#define AI_SI_Initial_Load_Source		_bit7
-#define AI_SI_Reload_Mode(a)			(((a) & 0x7)<<4)
-#define AI_SI_Write_Switch			_bit3
-#define AI_SC_Initial_Load_Source		_bit2
-#define AI_SC_Reload_Mode			_bit1
-#define AI_SC_Write_Switch			_bit0
-
-#define AI_SI_Load_A_Registers		14
-#define AI_SI_Load_B_Registers		16
-#define AI_SC_Load_A_Registers		18
-#define AI_SC_Load_B_Registers		20
-#define AI_SI_Save_Registers		64
-#define AI_SC_Save_Registers		66
-
-#define AI_SI2_Load_A_Register		23
-#define AI_SI2_Load_B_Register		25
-
-#define Joint_Status_1_Register         27
-#define DIO_Serial_IO_In_Progress_St            _bit12
-
-#define DIO_Serial_Input_Register       28
-#define Joint_Status_2_Register         29
-enum Joint_Status_2_Bits {
-	AO_TMRDACWRs_In_Progress_St = 0x20,
-};
-
-#define AO_Mode_1_Register		38
-#define AO_UPDATE_Source_Select(x)		(((x)&0x1f)<<11)
-#define AO_UI_Source_Select(x)			(((x)&0x1f)<<6)
-#define AO_Multiple_Channels			_bit5
-#define AO_UPDATE_Source_Polarity		_bit4
-#define AO_UI_Source_Polarity			_bit3
-#define AO_UC_Switch_Load_Every_TC		_bit2
-#define AO_Continuous				_bit1
-#define AO_Trigger_Once				_bit0
-
-#define AO_Mode_2_Register		39
-#define AO_FIFO_Mode_Mask (0x3 << 14)
-enum AO_FIFO_Mode_Bits {
-	AO_FIFO_Mode_HF_to_F = (3 << 14),
-	AO_FIFO_Mode_F = (2 << 14),
-	AO_FIFO_Mode_HF = (1 << 14),
-	AO_FIFO_Mode_E = (0 << 14),
-};
-#define AO_FIFO_Retransmit_Enable		_bit13
-#define AO_START1_Disable			_bit12
-#define AO_UC_Initial_Load_Source		_bit11
-#define AO_UC_Write_Switch			_bit10
-#define AO_UI2_Initial_Load_Source		_bit9
-#define AO_UI2_Reload_Mode			_bit8
-#define AO_UI_Initial_Load_Source		_bit7
-#define AO_UI_Reload_Mode(x)			(((x) & 0x7) << 4)
-#define AO_UI_Write_Switch			_bit3
-#define AO_BC_Initial_Load_Source		_bit2
-#define AO_BC_Reload_Mode			_bit1
-#define AO_BC_Write_Switch			_bit0
-
-#define AO_UI_Load_A_Register		40
-#define AO_UI_Load_A_Register_High	40
-#define AO_UI_Load_A_Register_Low	41
-#define AO_UI_Load_B_Register		42
-#define AO_UI_Save_Registers		16
-#define AO_BC_Load_A_Register		44
-#define AO_BC_Load_A_Register_High	44
-#define AO_BC_Load_A_Register_Low	45
-#define AO_BC_Load_B_Register		46
-#define AO_BC_Load_B_Register_High	46
-#define AO_BC_Load_B_Register_Low	47
-#define AO_BC_Save_Registers		18
-#define AO_UC_Load_A_Register		48
-#define AO_UC_Load_A_Register_High	48
-#define AO_UC_Load_A_Register_Low	49
-#define AO_UC_Load_B_Register		50
-#define AO_UC_Save_Registers		20
-
-#define Clock_and_FOUT_Register		56
-enum Clock_and_FOUT_bits {
-	FOUT_Enable = _bit15,
-	FOUT_Timebase_Select = _bit14,
-	DIO_Serial_Out_Divide_By_2 = _bit13,
-	Slow_Internal_Time_Divide_By_2 = _bit12,
-	Slow_Internal_Timebase = _bit11,
-	G_Source_Divide_By_2 = _bit10,
-	Clock_To_Board_Divide_By_2 = _bit9,
-	Clock_To_Board = _bit8,
-	AI_Output_Divide_By_2 = _bit7,
-	AI_Source_Divide_By_2 = _bit6,
-	AO_Output_Divide_By_2 = _bit5,
-	AO_Source_Divide_By_2 = _bit4,
-	FOUT_Divider_mask = 0xf
-};
-static inline unsigned FOUT_Divider(unsigned divider)
-{
-	return divider & FOUT_Divider_mask;
-}
-
-#define IO_Bidirection_Pin_Register	57
-#define	RTSI_Trig_Direction_Register	58
-enum RTSI_Trig_Direction_Bits {
-	Drive_RTSI_Clock_Bit = 0x1,
-	Use_RTSI_Clock_Bit = 0x2,
-};
-static inline unsigned RTSI_Output_Bit(unsigned channel, int is_mseries)
-{
-	unsigned max_channel;
-	unsigned base_bit_shift;
-	if (is_mseries) {
-		base_bit_shift = 8;
-		max_channel = 7;
-	} else {
-		base_bit_shift = 9;
-		max_channel = 6;
-	}
-	if (channel > max_channel) {
-		pr_err("%s: bug, invalid RTSI_channel=%i\n", __func__, channel);
-		return 0;
-	}
-	return 1 << (base_bit_shift + channel);
-}
-
-#define Interrupt_Control_Register	59
-#define Interrupt_B_Enable			_bit15
-#define Interrupt_B_Output_Select(x)		((x)<<12)
-#define Interrupt_A_Enable			_bit11
-#define Interrupt_A_Output_Select(x)		((x)<<8)
-#define Pass_Thru_0_Interrupt_Polarity		_bit3
-#define Pass_Thru_1_Interrupt_Polarity		_bit2
-#define Interrupt_Output_On_3_Pins		_bit1
-#define Interrupt_Output_Polarity		_bit0
-
-#define AI_Output_Control_Register	60
-#define AI_START_Output_Select			_bit10
-#define AI_SCAN_IN_PROG_Output_Select(x)	(((x) & 0x3) << 8)
-#define AI_EXTMUX_CLK_Output_Select(x)		(((x) & 0x3) << 6)
-#define AI_LOCALMUX_CLK_Output_Select(x)	((x)<<4)
-#define AI_SC_TC_Output_Select(x)		((x)<<2)
-enum ai_convert_output_selection {
-	AI_CONVERT_Output_High_Z = 0,
-	AI_CONVERT_Output_Ground = 1,
-	AI_CONVERT_Output_Enable_Low = 2,
-	AI_CONVERT_Output_Enable_High = 3
-};
-static unsigned AI_CONVERT_Output_Select(enum ai_convert_output_selection
-					 selection)
-{
-	return selection & 0x3;
-}
-
-#define AI_START_STOP_Select_Register	62
-#define AI_START_Polarity			_bit15
-#define AI_STOP_Polarity			_bit14
-#define AI_STOP_Sync				_bit13
-#define AI_STOP_Edge				_bit12
-#define AI_STOP_Select(a)			(((a) & 0x1f)<<7)
-#define AI_START_Sync				_bit6
-#define AI_START_Edge				_bit5
-#define AI_START_Select(a)			((a) & 0x1f)
-
-#define AI_Trigger_Select_Register	63
-#define AI_START1_Polarity			_bit15
-#define AI_START2_Polarity			_bit14
-#define AI_START2_Sync				_bit13
-#define AI_START2_Edge				_bit12
-#define AI_START2_Select(a)			(((a) & 0x1f) << 7)
-#define AI_START1_Sync				_bit6
-#define AI_START1_Edge				_bit5
-#define AI_START1_Select(a)			((a) & 0x1f)
-
-#define AI_DIV_Load_A_Register	64
-
-#define AO_Start_Select_Register	66
-#define AO_UI2_Software_Gate			_bit15
-#define AO_UI2_External_Gate_Polarity		_bit14
-#define AO_START_Polarity			_bit13
-#define AO_AOFREQ_Enable			_bit12
-#define AO_UI2_External_Gate_Select(a)		(((a) & 0x1f) << 7)
-#define AO_START_Sync				_bit6
-#define AO_START_Edge				_bit5
-#define AO_START_Select(a)			((a) & 0x1f)
-
-#define AO_Trigger_Select_Register	67
-#define AO_UI2_External_Gate_Enable		_bit15
-#define AO_Delayed_START1			_bit14
-#define AO_START1_Polarity			_bit13
-#define AO_UI2_Source_Polarity			_bit12
-#define AO_UI2_Source_Select(x)			(((x)&0x1f)<<7)
-#define AO_START1_Sync				_bit6
-#define AO_START1_Edge				_bit5
-#define AO_START1_Select(x)			(((x)&0x1f)<<0)
-
-#define AO_Mode_3_Register		70
-#define AO_UI2_Switch_Load_Next_TC		_bit13
-#define AO_UC_Switch_Load_Every_BC_TC		_bit12
-#define AO_Trigger_Length			_bit11
-#define AO_Stop_On_Overrun_Error		_bit5
-#define AO_Stop_On_BC_TC_Trigger_Error		_bit4
-#define AO_Stop_On_BC_TC_Error			_bit3
-#define AO_Not_An_UPDATE			_bit2
-#define AO_Software_Gate			_bit1
-#define AO_Last_Gate_Disable		_bit0	/* M Series only */
-
-#define Joint_Reset_Register		72
-#define Software_Reset			_bit11
-#define AO_Configuration_End			_bit9
-#define AI_Configuration_End			_bit8
-#define AO_Configuration_Start			_bit5
-#define AI_Configuration_Start			_bit4
-#define G1_Reset				_bit3
-#define G0_Reset				_bit2
-#define AO_Reset				_bit1
-#define AI_Reset				_bit0
-
-#define Interrupt_A_Enable_Register	73
-#define Pass_Thru_0_Interrupt_Enable		_bit9
-#define G0_Gate_Interrupt_Enable		_bit8
-#define AI_FIFO_Interrupt_Enable		_bit7
-#define G0_TC_Interrupt_Enable			_bit6
-#define AI_Error_Interrupt_Enable		_bit5
-#define AI_STOP_Interrupt_Enable		_bit4
-#define AI_START_Interrupt_Enable		_bit3
-#define AI_START2_Interrupt_Enable		_bit2
-#define AI_START1_Interrupt_Enable		_bit1
-#define AI_SC_TC_Interrupt_Enable		_bit0
-
-#define Interrupt_B_Enable_Register	75
-#define Pass_Thru_1_Interrupt_Enable		_bit11
-#define G1_Gate_Interrupt_Enable		_bit10
-#define G1_TC_Interrupt_Enable			_bit9
-#define AO_FIFO_Interrupt_Enable		_bit8
-#define AO_UI2_TC_Interrupt_Enable		_bit7
-#define AO_UC_TC_Interrupt_Enable		_bit6
-#define AO_Error_Interrupt_Enable		_bit5
-#define AO_STOP_Interrupt_Enable		_bit4
-#define AO_START_Interrupt_Enable		_bit3
-#define AO_UPDATE_Interrupt_Enable		_bit2
-#define AO_START1_Interrupt_Enable		_bit1
-#define AO_BC_TC_Interrupt_Enable		_bit0
-
-#define Second_IRQ_A_Enable_Register	74
-enum Second_IRQ_A_Enable_Bits {
-	AI_SC_TC_Second_Irq_Enable = _bit0,
-	AI_START1_Second_Irq_Enable = _bit1,
-	AI_START2_Second_Irq_Enable = _bit2,
-	AI_START_Second_Irq_Enable = _bit3,
-	AI_STOP_Second_Irq_Enable = _bit4,
-	AI_Error_Second_Irq_Enable = _bit5,
-	G0_TC_Second_Irq_Enable = _bit6,
-	AI_FIFO_Second_Irq_Enable = _bit7,
-	G0_Gate_Second_Irq_Enable = _bit8,
-	Pass_Thru_0_Second_Irq_Enable = _bit9
-};
+/*
+ * Registers in the National Instruments DAQ-STC chip
+ */
+
+#define NISTC_INTA_ACK_REG		2
+#define NISTC_INTA_ACK_G0_GATE		BIT(15)
+#define NISTC_INTA_ACK_G0_TC		BIT(14)
+#define NISTC_INTA_ACK_AI_ERR		BIT(13)
+#define NISTC_INTA_ACK_AI_STOP		BIT(12)
+#define NISTC_INTA_ACK_AI_START		BIT(11)
+#define NISTC_INTA_ACK_AI_START2	BIT(10)
+#define NISTC_INTA_ACK_AI_START1	BIT(9)
+#define NISTC_INTA_ACK_AI_SC_TC		BIT(8)
+#define NISTC_INTA_ACK_AI_SC_TC_ERR	BIT(7)
+#define NISTC_INTA_ACK_G0_TC_ERR	BIT(6)
+#define NISTC_INTA_ACK_G0_GATE_ERR	BIT(5)
+#define NISTC_INTA_ACK_AI_ALL		(NISTC_INTA_ACK_AI_ERR |	\
+					 NISTC_INTA_ACK_AI_STOP |	\
+					 NISTC_INTA_ACK_AI_START |	\
+					 NISTC_INTA_ACK_AI_START2 |	\
+					 NISTC_INTA_ACK_AI_START1 |	\
+					 NISTC_INTA_ACK_AI_SC_TC |	\
+					 NISTC_INTA_ACK_AI_SC_TC_ERR)
+
+#define NISTC_INTB_ACK_REG		3
+#define NISTC_INTB_ACK_G1_GATE		BIT(15)
+#define NISTC_INTB_ACK_G1_TC		BIT(14)
+#define NISTC_INTB_ACK_AO_ERR		BIT(13)
+#define NISTC_INTB_ACK_AO_STOP		BIT(12)
+#define NISTC_INTB_ACK_AO_START		BIT(11)
+#define NISTC_INTB_ACK_AO_UPDATE	BIT(10)
+#define NISTC_INTB_ACK_AO_START1	BIT(9)
+#define NISTC_INTB_ACK_AO_BC_TC		BIT(8)
+#define NISTC_INTB_ACK_AO_UC_TC		BIT(7)
+#define NISTC_INTB_ACK_AO_UI2_TC	BIT(6)
+#define NISTC_INTB_ACK_AO_UI2_TC_ERR	BIT(5)
+#define NISTC_INTB_ACK_AO_BC_TC_ERR	BIT(4)
+#define NISTC_INTB_ACK_AO_BC_TC_TRIG_ERR BIT(3)
+#define NISTC_INTB_ACK_G1_TC_ERR	BIT(2)
+#define NISTC_INTB_ACK_G1_GATE_ERR	BIT(1)
+#define NISTC_INTB_ACK_AO_ALL		(NISTC_INTB_ACK_AO_ERR |	\
+					 NISTC_INTB_ACK_AO_STOP |	\
+					 NISTC_INTB_ACK_AO_START |	\
+					 NISTC_INTB_ACK_AO_UPDATE |	\
+					 NISTC_INTB_ACK_AO_START1 |	\
+					 NISTC_INTB_ACK_AO_BC_TC |	\
+					 NISTC_INTB_ACK_AO_UC_TC |	\
+					 NISTC_INTB_ACK_AO_BC_TC_ERR |	\
+					 NISTC_INTB_ACK_AO_BC_TC_TRIG_ERR)
+
+#define NISTC_AI_CMD2_REG		4
+#define NISTC_AI_CMD2_END_ON_SC_TC	BIT(15)
+#define NISTC_AI_CMD2_END_ON_EOS	BIT(14)
+#define NISTC_AI_CMD2_START1_DISABLE	BIT(11)
+#define NISTC_AI_CMD2_SC_SAVE_TRACE	BIT(10)
+#define NISTC_AI_CMD2_SI_SW_ON_SC_TC	BIT(9)
+#define NISTC_AI_CMD2_SI_SW_ON_STOP	BIT(8)
+#define NISTC_AI_CMD2_SI_SW_ON_TC	BIT(7)
+#define NISTC_AI_CMD2_SC_SW_ON_TC	BIT(4)
+#define NISTC_AI_CMD2_STOP_PULSE	BIT(3)
+#define NISTC_AI_CMD2_START_PULSE	BIT(2)
+#define NISTC_AI_CMD2_START2_PULSE	BIT(1)
+#define NISTC_AI_CMD2_START1_PULSE	BIT(0)
+
+#define NISTC_AO_CMD2_REG		5
+#define NISTC_AO_CMD2_END_ON_BC_TC(x)	(((x) & 0x3) << 14)
+#define NISTC_AO_CMD2_START_STOP_GATE_ENA BIT(13)
+#define NISTC_AO_CMD2_UC_SAVE_TRACE	BIT(12)
+#define NISTC_AO_CMD2_BC_GATE_ENA	BIT(11)
+#define NISTC_AO_CMD2_BC_SAVE_TRACE	BIT(10)
+#define NISTC_AO_CMD2_UI_SW_ON_BC_TC	BIT(9)
+#define NISTC_AO_CMD2_UI_SW_ON_STOP	BIT(8)
+#define NISTC_AO_CMD2_UI_SW_ON_TC	BIT(7)
+#define NISTC_AO_CMD2_UC_SW_ON_BC_TC	BIT(6)
+#define NISTC_AO_CMD2_UC_SW_ON_TC	BIT(5)
+#define NISTC_AO_CMD2_BC_SW_ON_TC	BIT(4)
+#define NISTC_AO_CMD2_MUTE_B		BIT(3)
+#define NISTC_AO_CMD2_MUTE_A		BIT(2)
+#define NISTC_AO_CMD2_UPDATE2_PULSE	BIT(1)
+#define NISTC_AO_CMD2_START1_PULSE	BIT(0)
+
+#define NISTC_G0_CMD_REG		6
+#define NISTC_G1_CMD_REG		7
+
+#define NISTC_AI_CMD1_REG		8
+#define NISTC_AI_CMD1_ATRIG_RESET	BIT(14)
+#define NISTC_AI_CMD1_DISARM		BIT(13)
+#define NISTC_AI_CMD1_SI2_ARM		BIT(12)
+#define NISTC_AI_CMD1_SI2_LOAD		BIT(11)
+#define NISTC_AI_CMD1_SI_ARM		BIT(10)
+#define NISTC_AI_CMD1_SI_LOAD		BIT(9)
+#define NISTC_AI_CMD1_DIV_ARM		BIT(8)
+#define NISTC_AI_CMD1_DIV_LOAD		BIT(7)
+#define NISTC_AI_CMD1_SC_ARM		BIT(6)
+#define NISTC_AI_CMD1_SC_LOAD		BIT(5)
+#define NISTC_AI_CMD1_SCAN_IN_PROG_PULSE BIT(4)
+#define NISTC_AI_CMD1_EXTMUX_CLK_PULSE	BIT(3)
+#define NISTC_AI_CMD1_LOCALMUX_CLK_PULSE BIT(2)
+#define NISTC_AI_CMD1_SC_TC_PULSE	BIT(1)
+#define NISTC_AI_CMD1_CONVERT_PULSE	BIT(0)
+
+#define NISTC_AO_CMD1_REG		9
+#define NISTC_AO_CMD1_ATRIG_RESET	BIT(15)
+#define NISTC_AO_CMD1_START_PULSE	BIT(14)
+#define NISTC_AO_CMD1_DISARM		BIT(13)
+#define NISTC_AO_CMD1_UI2_ARM_DISARM	BIT(12)
+#define NISTC_AO_CMD1_UI2_LOAD		BIT(11)
+#define NISTC_AO_CMD1_UI_ARM		BIT(10)
+#define NISTC_AO_CMD1_UI_LOAD		BIT(9)
+#define NISTC_AO_CMD1_UC_ARM		BIT(8)
+#define NISTC_AO_CMD1_UC_LOAD		BIT(7)
+#define NISTC_AO_CMD1_BC_ARM		BIT(6)
+#define NISTC_AO_CMD1_BC_LOAD		BIT(5)
+#define NISTC_AO_CMD1_DAC1_UPDATE_MODE	BIT(4)
+#define NISTC_AO_CMD1_LDAC1_SRC_SEL	BIT(3)
+#define NISTC_AO_CMD1_DAC0_UPDATE_MODE	BIT(2)
+#define NISTC_AO_CMD1_LDAC0_SRC_SEL	BIT(1)
+#define NISTC_AO_CMD1_UPDATE_PULSE	BIT(0)
+
+#define NISTC_DIO_OUT_REG		10
+#define NISTC_DIO_OUT_SERIAL(x)	(((x) & 0xff) << 8)
+#define NISTC_DIO_OUT_SERIAL_MASK	NISTC_DIO_OUT_SERIAL(0xff)
+#define NISTC_DIO_OUT_PARALLEL(x)	((x) & 0xff)
+#define NISTC_DIO_OUT_PARALLEL_MASK	NISTC_DIO_OUT_PARALLEL(0xff)
+#define NISTC_DIO_SDIN			BIT(4)
+#define NISTC_DIO_SDOUT			BIT(0)
+
+#define NISTC_DIO_CTRL_REG		11
+#define NISTC_DIO_SDCLK			BIT(11)
+#define NISTC_DIO_CTRL_HW_SER_TIMEBASE	BIT(10)
+#define NISTC_DIO_CTRL_HW_SER_ENA	BIT(9)
+#define NISTC_DIO_CTRL_HW_SER_START	BIT(8)
+#define NISTC_DIO_CTRL_DIR(x)		((x) & 0xff)
+#define NISTC_DIO_CTRL_DIR_MASK		NISTC_DIO_CTRL_DIR(0xff)
+
+#define NISTC_AI_MODE1_REG		12
+#define NISTC_AI_MODE1_CONVERT_SRC(x)	(((x) & 0x1f) << 11)
+#define NISTC_AI_MODE1_SI_SRC(x)	(((x) & 0x1f) << 6)
+#define NISTC_AI_MODE1_CONVERT_POLARITY	BIT(5)
+#define NISTC_AI_MODE1_SI_POLARITY	BIT(4)
+#define NISTC_AI_MODE1_START_STOP	BIT(3)
+#define NISTC_AI_MODE1_RSVD		BIT(2)
+#define NISTC_AI_MODE1_CONTINUOUS	BIT(1)
+#define NISTC_AI_MODE1_TRIGGER_ONCE	BIT(0)
+
+#define NISTC_AI_MODE2_REG		13
+#define NISTC_AI_MODE2_SC_GATE_ENA	BIT(15)
+#define NISTC_AI_MODE2_START_STOP_GATE_ENA BIT(14)
+#define NISTC_AI_MODE2_PRE_TRIGGER	BIT(13)
+#define NISTC_AI_MODE2_EXTMUX_PRESENT	BIT(12)
+#define NISTC_AI_MODE2_SI2_INIT_LOAD_SRC BIT(9)
+#define NISTC_AI_MODE2_SI2_RELOAD_MODE	BIT(8)
+#define NISTC_AI_MODE2_SI_INIT_LOAD_SRC	BIT(7)
+#define NISTC_AI_MODE2_SI_RELOAD_MODE(x) (((x) & 0x7) << 4)
+#define NISTC_AI_MODE2_SI_WR_SWITCH	BIT(3)
+#define NISTC_AI_MODE2_SC_INIT_LOAD_SRC	BIT(2)
+#define NISTC_AI_MODE2_SC_RELOAD_MODE	BIT(1)
+#define NISTC_AI_MODE2_SC_WR_SWITCH	BIT(0)
+
+#define NISTC_AI_SI_LOADA_REG		14
+#define NISTC_AI_SI_LOADB_REG		16
+#define NISTC_AI_SC_LOADA_REG		18
+#define NISTC_AI_SC_LOADB_REG		20
+#define NISTC_AI_SI2_LOADA_REG		23
+#define NISTC_AI_SI2_LOADB_REG		25
+
+#define NISTC_G0_MODE_REG		26
+#define NISTC_G1_MODE_REG		27
+#define NISTC_G0_LOADA_REG		28
+#define NISTC_G0_LOADB_REG		30
+#define NISTC_G1_LOADA_REG		32
+#define NISTC_G1_LOADB_REG		34
+#define NISTC_G0_INPUT_SEL_REG		36
+#define NISTC_G1_INPUT_SEL_REG		37
+
+#define NISTC_AO_MODE1_REG		38
+#define NISTC_AO_MODE1_UPDATE_SRC(x)	(((x) & 0x1f) << 11)
+#define NISTC_AO_MODE1_UPDATE_SRC_MASK	NISTC_AO_MODE1_UPDATE_SRC(0x1f)
+#define NISTC_AO_MODE1_UI_SRC(x)	(((x) & 0x1f) << 6)
+#define NISTC_AO_MODE1_UI_SRC_MASK	NISTC_AO_MODE1_UI_SRC(0x1f)
+#define NISTC_AO_MODE1_MULTI_CHAN	BIT(5)
+#define NISTC_AO_MODE1_UPDATE_SRC_POLARITY BIT(4)
+#define NISTC_AO_MODE1_UI_SRC_POLARITY	BIT(3)
+#define NISTC_AO_MODE1_UC_SW_EVERY_TC	BIT(2)
+#define NISTC_AO_MODE1_CONTINUOUS	BIT(1)
+#define NISTC_AO_MODE1_TRIGGER_ONCE	BIT(0)
+
+#define NISTC_AO_MODE2_REG		39
+#define NISTC_AO_MODE2_FIFO_MODE(x)	(((x) & 0x3) << 14)
+#define NISTC_AO_MODE2_FIFO_MODE_MASK	NISTC_AO_MODE2_FIFO_MODE(3)
+#define NISTC_AO_MODE2_FIFO_MODE_E	NISTC_AO_MODE2_FIFO_MODE(0)
+#define NISTC_AO_MODE2_FIFO_MODE_HF	NISTC_AO_MODE2_FIFO_MODE(1)
+#define NISTC_AO_MODE2_FIFO_MODE_F	NISTC_AO_MODE2_FIFO_MODE(2)
+#define NISTC_AO_MODE2_FIFO_MODE_HF_F	NISTC_AO_MODE2_FIFO_MODE(3)
+#define NISTC_AO_MODE2_FIFO_REXMIT_ENA	BIT(13)
+#define NISTC_AO_MODE2_START1_DISABLE	BIT(12)
+#define NISTC_AO_MODE2_UC_INIT_LOAD_SRC	BIT(11)
+#define NISTC_AO_MODE2_UC_WR_SWITCH	BIT(10)
+#define NISTC_AO_MODE2_UI2_INIT_LOAD_SRC BIT(9)
+#define NISTC_AO_MODE2_UI2_RELOAD_MODE	BIT(8)
+#define NISTC_AO_MODE2_UI_INIT_LOAD_SRC	BIT(7)
+#define NISTC_AO_MODE2_UI_RELOAD_MODE(x) (((x) & 0x7) << 4)
+#define NISTC_AO_MODE2_UI_WR_SWITCH	BIT(3)
+#define NISTC_AO_MODE2_BC_INIT_LOAD_SRC	BIT(2)
+#define NISTC_AO_MODE2_BC_RELOAD_MODE	BIT(1)
+#define NISTC_AO_MODE2_BC_WR_SWITCH	BIT(0)
+
+#define NISTC_AO_UI_LOADA_REG		40
+#define NISTC_AO_UI_LOADB_REG		42
+#define NISTC_AO_BC_LOADA_REG		44
+#define NISTC_AO_BC_LOADB_REG		46
+#define NISTC_AO_UC_LOADA_REG		48
+#define NISTC_AO_UC_LOADB_REG		50
+
+#define NISTC_CLK_FOUT_REG		56
+#define NISTC_CLK_FOUT_ENA		BIT(15)
+#define NISTC_CLK_FOUT_TIMEBASE_SEL	BIT(14)
+#define NISTC_CLK_FOUT_DIO_SER_OUT_DIV2	BIT(13)
+#define NISTC_CLK_FOUT_SLOW_DIV2	BIT(12)
+#define NISTC_CLK_FOUT_SLOW_TIMEBASE	BIT(11)
+#define NISTC_CLK_FOUT_G_SRC_DIV2	BIT(10)
+#define NISTC_CLK_FOUT_TO_BOARD_DIV2	BIT(9)
+#define NISTC_CLK_FOUT_TO_BOARD		BIT(8)
+#define NISTC_CLK_FOUT_AI_OUT_DIV2	BIT(7)
+#define NISTC_CLK_FOUT_AI_SRC_DIV2	BIT(6)
+#define NISTC_CLK_FOUT_AO_OUT_DIV2	BIT(5)
+#define NISTC_CLK_FOUT_AO_SRC_DIV2	BIT(4)
+#define NISTC_CLK_FOUT_DIVIDER(x)	(((x) & 0xf) << 0)
+#define NISTC_CLK_FOUT_TO_DIVIDER(x)	(((x) >> 0) & 0xf)
+#define NISTC_CLK_FOUT_DIVIDER_MASK	NISTC_CLK_FOUT_DIVIDER(0xf)
+
+#define NISTC_IO_BIDIR_PIN_REG		57
+
+#define NISTC_RTSI_TRIG_DIR_REG		58
+#define NISTC_RTSI_TRIG_OLD_CLK_CHAN	7
+#define NISTC_RTSI_TRIG_NUM_CHAN(_m)	((_m) ? 8 : 7)
+#define NISTC_RTSI_TRIG_DIR(_c, _m)	((_m) ? BIT(8 + (_c)) : BIT(7 + (_c)))
+#define NISTC_RTSI_TRIG_USE_CLK		BIT(1)
+#define NISTC_RTSI_TRIG_DRV_CLK		BIT(0)
+
+#define NISTC_INT_CTRL_REG		59
+#define NISTC_INT_CTRL_INTB_ENA		BIT(15)
+#define NISTC_INT_CTRL_INTB_SEL(x)	(((x) & 0x7) << 12)
+#define NISTC_INT_CTRL_INTA_ENA		BIT(11)
+#define NISTC_INT_CTRL_INTA_SEL(x)	(((x) & 0x7) << 8)
+#define NISTC_INT_CTRL_PASSTHRU0_POL	BIT(3)
+#define NISTC_INT_CTRL_PASSTHRU1_POL	BIT(2)
+#define NISTC_INT_CTRL_3PIN_INT		BIT(1)
+#define NISTC_INT_CTRL_INT_POL		BIT(0)
+
+#define NISTC_AI_OUT_CTRL_REG		60
+#define NISTC_AI_OUT_CTRL_START_SEL	BIT(10)
+#define NISTC_AI_OUT_CTRL_SCAN_IN_PROG_SEL(x)	(((x) & 0x3) << 8)
+#define NISTC_AI_OUT_CTRL_EXTMUX_CLK_SEL(x)	(((x) & 0x3) << 6)
+#define NISTC_AI_OUT_CTRL_LOCALMUX_CLK_SEL(x)	(((x) & 0x3) << 4)
+#define NISTC_AI_OUT_CTRL_SC_TC_SEL(x)		(((x) & 0x3) << 2)
+#define NISTC_AI_OUT_CTRL_CONVERT_SEL(x)	(((x) & 0x3) << 0)
+#define NISTC_AI_OUT_CTRL_CONVERT_HIGH_Z	NISTC_AI_OUT_CTRL_CONVERT_SEL(0)
+#define NISTC_AI_OUT_CTRL_CONVERT_GND		NISTC_AI_OUT_CTRL_CONVERT_SEL(1)
+#define NISTC_AI_OUT_CTRL_CONVERT_LOW		NISTC_AI_OUT_CTRL_CONVERT_SEL(2)
+#define NISTC_AI_OUT_CTRL_CONVERT_HIGH		NISTC_AI_OUT_CTRL_CONVERT_SEL(3)
+
+#define NISTC_ATRIG_ETC_REG		61
+#define NISTC_ATRIG_ETC_GPFO_1_ENA	BIT(15)
+#define NISTC_ATRIG_ETC_GPFO_0_ENA	BIT(14)
+#define NISTC_ATRIG_ETC_GPFO_0_SEL(x)	(((x) & 0x3) << 11)
+#define NISTC_ATRIG_ETC_GPFO_1_SEL	BIT(7)
+#define NISTC_ATRIG_ETC_DRV		BIT(4)
+#define NISTC_ATRIG_ETC_ENA		BIT(3)
+#define NISTC_ATRIG_ETC_MODE(x)		(((x) & 0x7) << 0)
+
+#define NISTC_AI_START_STOP_REG		62
+#define NISTC_AI_START_POLARITY		BIT(15)
+#define NISTC_AI_STOP_POLARITY		BIT(14)
+#define NISTC_AI_STOP_SYNC		BIT(13)
+#define NISTC_AI_STOP_EDGE		BIT(12)
+#define NISTC_AI_STOP_SEL(x)		(((x) & 0x1f) << 7)
+#define NISTC_AI_START_SYNC		BIT(6)
+#define NISTC_AI_START_EDGE		BIT(5)
+#define NISTC_AI_START_SEL(x)		(((x) & 0x1f) << 0)
+
+#define NISTC_AI_TRIG_SEL_REG		63
+#define NISTC_AI_TRIG_START1_POLARITY	BIT(15)
+#define NISTC_AI_TRIG_START2_POLARITY	BIT(14)
+#define NISTC_AI_TRIG_START2_SYNC	BIT(13)
+#define NISTC_AI_TRIG_START2_EDGE	BIT(12)
+#define NISTC_AI_TRIG_START2_SEL(x)	(((x) & 0x1f) << 7)
+#define NISTC_AI_TRIG_START1_SYNC	BIT(6)
+#define NISTC_AI_TRIG_START1_EDGE	BIT(5)
+#define NISTC_AI_TRIG_START1_SEL(x)	(((x) & 0x1f) << 0)
+
+#define NISTC_AI_DIV_LOADA_REG		64
+
+#define NISTC_AO_START_SEL_REG		66
+#define NISTC_AO_START_UI2_SW_GATE	BIT(15)
+#define NISTC_AO_START_UI2_EXT_GATE_POL	BIT(14)
+#define NISTC_AO_START_POLARITY		BIT(13)
+#define NISTC_AO_START_AOFREQ_ENA	BIT(12)
+#define NISTC_AO_START_UI2_EXT_GATE_SEL(x) (((x) & 0x1f) << 7)
+#define NISTC_AO_START_SYNC		BIT(6)
+#define NISTC_AO_START_EDGE		BIT(5)
+#define NISTC_AO_START_SEL(x)		(((x) & 0x1f) << 0)
+
+#define NISTC_AO_TRIG_SEL_REG		67
+#define NISTC_AO_TRIG_UI2_EXT_GATE_ENA	BIT(15)
+#define NISTC_AO_TRIG_DELAYED_START1	BIT(14)
+#define NISTC_AO_TRIG_START1_POLARITY	BIT(13)
+#define NISTC_AO_TRIG_UI2_SRC_POLARITY	BIT(12)
+#define NISTC_AO_TRIG_UI2_SRC_SEL(x)	(((x) & 0x1f) << 7)
+#define NISTC_AO_TRIG_START1_SYNC	BIT(6)
+#define NISTC_AO_TRIG_START1_EDGE	BIT(5)
+#define NISTC_AO_TRIG_START1_SEL(x)	(((x) & 0x1f) << 0)
+#define NISTC_AO_TRIG_START1_SEL_MASK	NISTC_AO_TRIG_START1_SEL(0x1f)
+
+#define NISTC_G0_AUTOINC_REG		68
+#define NISTC_G1_AUTOINC_REG		69
+
+#define NISTC_AO_MODE3_REG		70
+#define NISTC_AO_MODE3_UI2_SW_NEXT_TC		BIT(13)
+#define NISTC_AO_MODE3_UC_SW_EVERY_BC_TC	BIT(12)
+#define NISTC_AO_MODE3_TRIG_LEN			BIT(11)
+#define NISTC_AO_MODE3_STOP_ON_OVERRUN_ERR	BIT(5)
+#define NISTC_AO_MODE3_STOP_ON_BC_TC_TRIG_ERR	BIT(4)
+#define NISTC_AO_MODE3_STOP_ON_BC_TC_ERR	BIT(3)
+#define NISTC_AO_MODE3_NOT_AN_UPDATE		BIT(2)
+#define NISTC_AO_MODE3_SW_GATE			BIT(1)
+#define NISTC_AO_MODE3_LAST_GATE_DISABLE	BIT(0)	/* M-Series only */
+
+#define NISTC_RESET_REG			72
+#define NISTC_RESET_SOFTWARE		BIT(11)
+#define NISTC_RESET_AO_CFG_END		BIT(9)
+#define NISTC_RESET_AI_CFG_END		BIT(8)
+#define NISTC_RESET_AO_CFG_START	BIT(5)
+#define NISTC_RESET_AI_CFG_START	BIT(4)
+#define NISTC_RESET_G1			BIT(3)
+#define NISTC_RESET_G0			BIT(2)
+#define NISTC_RESET_AO			BIT(1)
+#define NISTC_RESET_AI			BIT(0)
+
+#define NISTC_INTA_ENA_REG		73
+#define NISTC_INTA2_ENA_REG		74
+#define NISTC_INTA_ENA_PASSTHRU0	BIT(9)
+#define NISTC_INTA_ENA_G0_GATE		BIT(8)
+#define NISTC_INTA_ENA_AI_FIFO		BIT(7)
+#define NISTC_INTA_ENA_G0_TC		BIT(6)
+#define NISTC_INTA_ENA_AI_ERR		BIT(5)
+#define NISTC_INTA_ENA_AI_STOP		BIT(4)
+#define NISTC_INTA_ENA_AI_START		BIT(3)
+#define NISTC_INTA_ENA_AI_START2	BIT(2)
+#define NISTC_INTA_ENA_AI_START1	BIT(1)
+#define NISTC_INTA_ENA_AI_SC_TC		BIT(0)
+#define NISTC_INTA_ENA_AI_MASK		(NISTC_INTA_ENA_AI_FIFO |	\
+					 NISTC_INTA_ENA_AI_ERR |	\
+					 NISTC_INTA_ENA_AI_STOP |	\
+					 NISTC_INTA_ENA_AI_START |	\
+					 NISTC_INTA_ENA_AI_START2 |	\
+					 NISTC_INTA_ENA_AI_START1 |	\
+					 NISTC_INTA_ENA_AI_SC_TC)
+
+#define NISTC_INTB_ENA_REG		75
+#define NISTC_INTB2_ENA_REG		76
+#define NISTC_INTB_ENA_PASSTHRU1	BIT(11)
+#define NISTC_INTB_ENA_G1_GATE		BIT(10)
+#define NISTC_INTB_ENA_G1_TC		BIT(9)
+#define NISTC_INTB_ENA_AO_FIFO		BIT(8)
+#define NISTC_INTB_ENA_AO_UI2_TC	BIT(7)
+#define NISTC_INTB_ENA_AO_UC_TC		BIT(6)
+#define NISTC_INTB_ENA_AO_ERR		BIT(5)
+#define NISTC_INTB_ENA_AO_STOP		BIT(4)
+#define NISTC_INTB_ENA_AO_START		BIT(3)
+#define NISTC_INTB_ENA_AO_UPDATE	BIT(2)
+#define NISTC_INTB_ENA_AO_START1	BIT(1)
+#define NISTC_INTB_ENA_AO_BC_TC		BIT(0)
+
+#define NISTC_AI_PERSONAL_REG		77
+#define NISTC_AI_PERSONAL_SHIFTIN_PW		BIT(15)
+#define NISTC_AI_PERSONAL_EOC_POLARITY		BIT(14)
+#define NISTC_AI_PERSONAL_SOC_POLARITY		BIT(13)
+#define NISTC_AI_PERSONAL_SHIFTIN_POL		BIT(12)
+#define NISTC_AI_PERSONAL_CONVERT_TIMEBASE	BIT(11)
+#define NISTC_AI_PERSONAL_CONVERT_PW		BIT(10)
+#define NISTC_AI_PERSONAL_CONVERT_ORIG_PULSE	BIT(9)
+#define NISTC_AI_PERSONAL_FIFO_FLAGS_POL	BIT(8)
+#define NISTC_AI_PERSONAL_OVERRUN_MODE		BIT(7)
+#define NISTC_AI_PERSONAL_EXTMUX_CLK_PW		BIT(6)
+#define NISTC_AI_PERSONAL_LOCALMUX_CLK_PW	BIT(5)
+#define NISTC_AI_PERSONAL_AIFREQ_POL		BIT(4)
+
+#define NISTC_AO_PERSONAL_REG		78
+#define NISTC_AO_PERSONAL_MULTI_DACS		BIT(15)	/* M-Series only */
+#define NISTC_AO_PERSONAL_NUM_DAC		BIT(14)	/* 1:single; 0:dual */
+#define NISTC_AO_PERSONAL_FAST_CPU		BIT(13)	/* M-Series reserved */
+#define NISTC_AO_PERSONAL_TMRDACWR_PW		BIT(12)
+#define NISTC_AO_PERSONAL_FIFO_FLAGS_POL	BIT(11)	/* M-Series reserved */
+#define NISTC_AO_PERSONAL_FIFO_ENA		BIT(10)
+#define NISTC_AO_PERSONAL_AOFREQ_POL		BIT(9)	/* M-Series reserved */
+#define NISTC_AO_PERSONAL_DMA_PIO_CTRL		BIT(8)	/* M-Series reserved */
+#define NISTC_AO_PERSONAL_UPDATE_ORIG_PULSE	BIT(7)
+#define NISTC_AO_PERSONAL_UPDATE_TIMEBASE	BIT(6)
+#define NISTC_AO_PERSONAL_UPDATE_PW		BIT(5)
+#define NISTC_AO_PERSONAL_BC_SRC_SEL		BIT(4)
+#define NISTC_AO_PERSONAL_INTERVAL_BUFFER_MODE	BIT(3)
+
+#define NISTC_RTSI_TRIGA_OUT_REG	79
+#define NISTC_RTSI_TRIGB_OUT_REG	80
+#define NISTC_RTSI_TRIGB_SUB_SEL1	BIT(15)	/* not for M-Series */
+#define NISTC_RTSI_TRIG(_c, _s)		(((_s) & 0xf) << (((_c) % 4) * 4))
+#define NISTC_RTSI_TRIG_MASK(_c)	NISTC_RTSI_TRIG((_c), 0xf)
+#define NISTC_RTSI_TRIG_TO_SRC(_c, _b)	(((_b) >> (((_c) % 4) * 4)) & 0xf)
+
+#define NISTC_RTSI_BOARD_REG		81
+
+#define NISTC_CFG_MEM_CLR_REG		82
+#define NISTC_ADC_FIFO_CLR_REG		83
+#define NISTC_DAC_FIFO_CLR_REG		84
+#define NISTC_WR_STROBE3_REG		85
+
+#define NISTC_AO_OUT_CTRL_REG		86
+#define NISTC_AO_OUT_CTRL_EXT_GATE_ENA		BIT(15)
+#define NISTC_AO_OUT_CTRL_EXT_GATE_SEL(x)	(((x) & 0x1f) << 10)
+#define NISTC_AO_OUT_CTRL_CHANS(x)		(((x) & 0xf) << 6)
+#define NISTC_AO_OUT_CTRL_UPDATE2_SEL(x)	(((x) & 0x3) << 4)
+#define NISTC_AO_OUT_CTRL_EXT_GATE_POL		BIT(3)
+#define NISTC_AO_OUT_CTRL_UPDATE2_TOGGLE	BIT(2)
+#define NISTC_AO_OUT_CTRL_UPDATE_SEL(x)		(((x) & 0x3) << 0)
+#define NISTC_AO_OUT_CTRL_UPDATE_SEL_HIGHZ	NISTC_AO_OUT_CTRL_UPDATE_SEL(0)
+#define NISTC_AO_OUT_CTRL_UPDATE_SEL_GND	NISTC_AO_OUT_CTRL_UPDATE_SEL(1)
+#define NISTC_AO_OUT_CTRL_UPDATE_SEL_LOW	NISTC_AO_OUT_CTRL_UPDATE_SEL(2)
+#define NISTC_AO_OUT_CTRL_UPDATE_SEL_HIGH	NISTC_AO_OUT_CTRL_UPDATE_SEL(3)
+
+#define NISTC_AI_MODE3_REG		87
+#define NISTC_AI_MODE3_TRIG_LEN		BIT(15)
+#define NISTC_AI_MODE3_DELAY_START	BIT(14)
+#define NISTC_AI_MODE3_SOFTWARE_GATE	BIT(13)
+#define NISTC_AI_MODE3_SI_TRIG_DELAY	BIT(12)
+#define NISTC_AI_MODE3_SI2_SRC_SEL	BIT(11)
+#define NISTC_AI_MODE3_DELAYED_START2	BIT(10)
+#define NISTC_AI_MODE3_DELAYED_START1	BIT(9)
+#define NISTC_AI_MODE3_EXT_GATE_MODE	BIT(8)
+#define NISTC_AI_MODE3_FIFO_MODE(x)	(((x) & 0x3) << 6)
+#define NISTC_AI_MODE3_FIFO_MODE_NE	NISTC_AI_MODE3_FIFO_MODE(0)
+#define NISTC_AI_MODE3_FIFO_MODE_HF	NISTC_AI_MODE3_FIFO_MODE(1)
+#define NISTC_AI_MODE3_FIFO_MODE_F	NISTC_AI_MODE3_FIFO_MODE(2)
+#define NISTC_AI_MODE3_FIFO_MODE_HF_E	NISTC_AI_MODE3_FIFO_MODE(3)
+#define NISTC_AI_MODE3_EXT_GATE_POL	BIT(5)
+#define NISTC_AI_MODE3_EXT_GATE_SEL(x)	(((x) & 0x1f) << 0)
+
+#define NISTC_AI_STATUS1_REG		2
+#define NISTC_AI_STATUS1_INTA		BIT(15)
+#define NISTC_AI_STATUS1_FIFO_F		BIT(14)
+#define NISTC_AI_STATUS1_FIFO_HF	BIT(13)
+#define NISTC_AI_STATUS1_FIFO_E		BIT(12)
+#define NISTC_AI_STATUS1_OVERRUN	BIT(11)
+#define NISTC_AI_STATUS1_OVERFLOW	BIT(10)
+#define NISTC_AI_STATUS1_SC_TC_ERR	BIT(9)
+#define NISTC_AI_STATUS1_OVER		(NISTC_AI_STATUS1_OVERRUN |	\
+					 NISTC_AI_STATUS1_OVERFLOW)
+#define NISTC_AI_STATUS1_ERR		(NISTC_AI_STATUS1_OVER |	\
+					 NISTC_AI_STATUS1_SC_TC_ERR)
+#define NISTC_AI_STATUS1_START2		BIT(8)
+#define NISTC_AI_STATUS1_START1		BIT(7)
+#define NISTC_AI_STATUS1_SC_TC		BIT(6)
+#define NISTC_AI_STATUS1_START		BIT(5)
+#define NISTC_AI_STATUS1_STOP		BIT(4)
+#define NISTC_AI_STATUS1_G0_TC		BIT(3)
+#define NISTC_AI_STATUS1_G0_GATE	BIT(2)
+#define NISTC_AI_STATUS1_FIFO_REQ	BIT(1)
+#define NISTC_AI_STATUS1_PASSTHRU0	BIT(0)
+
+#define NISTC_AO_STATUS1_REG		3
+#define NISTC_AO_STATUS1_INTB		BIT(15)
+#define NISTC_AO_STATUS1_FIFO_F		BIT(14)
+#define NISTC_AO_STATUS1_FIFO_HF	BIT(13)
+#define NISTC_AO_STATUS1_FIFO_E		BIT(12)
+#define NISTC_AO_STATUS1_BC_TC_ERR	BIT(11)
+#define NISTC_AO_STATUS1_START		BIT(10)
+#define NISTC_AO_STATUS1_OVERRUN	BIT(9)
+#define NISTC_AO_STATUS1_START1		BIT(8)
+#define NISTC_AO_STATUS1_BC_TC		BIT(7)
+#define NISTC_AO_STATUS1_UC_TC		BIT(6)
+#define NISTC_AO_STATUS1_UPDATE		BIT(5)
+#define NISTC_AO_STATUS1_UI2_TC		BIT(4)
+#define NISTC_AO_STATUS1_G1_TC		BIT(3)
+#define NISTC_AO_STATUS1_G1_GATE	BIT(2)
+#define NISTC_AO_STATUS1_FIFO_REQ	BIT(1)
+#define NISTC_AO_STATUS1_PASSTHRU1	BIT(0)
+
+#define NISTC_G01_STATUS_REG		4
+
+#define NISTC_AI_STATUS2_REG		5
+
+#define NISTC_AO_STATUS2_REG		6
+
+#define NISTC_DIO_IN_REG		7
+
+#define NISTC_G0_HW_SAVE_REG		8
+#define NISTC_G1_HW_SAVE_REG		10
+
+#define NISTC_G0_SAVE_REG		12
+#define NISTC_G1_SAVE_REG		14
+
+#define NISTC_AO_UI_SAVE_REG		16
+#define NISTC_AO_BC_SAVE_REG		18
+#define NISTC_AO_UC_SAVE_REG		20
+
+#define NISTC_STATUS1_REG		27
+#define NISTC_STATUS1_SERIO_IN_PROG	BIT(12)
+
+#define NISTC_DIO_SERIAL_IN_REG		28
+
+#define NISTC_STATUS2_REG		29
+#define NISTC_STATUS2_AO_TMRDACWRS_IN_PROGRESS	BIT(5)
+
+#define NISTC_AI_SI_SAVE_REG		64
+#define NISTC_AI_SC_SAVE_REG		66
 
-#define Second_IRQ_B_Enable_Register	76
-enum Second_IRQ_B_Enable_Bits {
-	AO_BC_TC_Second_Irq_Enable = _bit0,
-	AO_START1_Second_Irq_Enable = _bit1,
-	AO_UPDATE_Second_Irq_Enable = _bit2,
-	AO_START_Second_Irq_Enable = _bit3,
-	AO_STOP_Second_Irq_Enable = _bit4,
-	AO_Error_Second_Irq_Enable = _bit5,
-	AO_UC_TC_Second_Irq_Enable = _bit6,
-	AO_UI2_TC_Second_Irq_Enable = _bit7,
-	AO_FIFO_Second_Irq_Enable = _bit8,
-	G1_TC_Second_Irq_Enable = _bit9,
-	G1_Gate_Second_Irq_Enable = _bit10,
-	Pass_Thru_1_Second_Irq_Enable = _bit11
-};
+/*
+ * PCI E Series Registers
+ */
+#define NI_E_STC_WINDOW_ADDR_REG	0x00	/* rw16 */
+#define NI_E_STC_WINDOW_DATA_REG	0x02	/* rw16 */
+
+#define NI_E_STATUS_REG			0x01	/* r8 */
+#define NI_E_STATUS_AI_FIFO_LOWER_NE	BIT(3)
+#define NI_E_STATUS_PROMOUT		BIT(0)
+
+#define NI_E_DMA_AI_AO_SEL_REG		0x09	/* w8 */
+#define NI_E_DMA_AI_SEL(x)		(((x) & 0xf) << 0)
+#define NI_E_DMA_AI_SEL_MASK		NI_E_DMA_AI_SEL(0xf)
+#define NI_E_DMA_AO_SEL(x)		(((x) & 0xf) << 4)
+#define NI_E_DMA_AO_SEL_MASK		NI_E_DMA_AO_SEL(0xf)
+
+#define NI_E_DMA_G0_G1_SEL_REG		0x0b	/* w8 */
+#define NI_E_DMA_G0_G1_SEL(_g, _c)	(((_c) & 0xf) << ((_g) * 4))
+#define NI_E_DMA_G0_G1_SEL_MASK(_g)	NI_E_DMA_G0_G1_SEL((_g), 0xf)
+
+#define NI_E_SERIAL_CMD_REG		0x0d	/* w8 */
+#define NI_E_SERIAL_CMD_DAC_LD(x)	BIT(3 + (x))
+#define NI_E_SERIAL_CMD_EEPROM_CS	BIT(2)
+#define NI_E_SERIAL_CMD_SDATA		BIT(1)
+#define NI_E_SERIAL_CMD_SCLK		BIT(0)
+
+#define NI_E_MISC_CMD_REG		0x0f	/* w8 */
+#define NI_E_MISC_CMD_INTEXT_ATRIG(x)	(((x) & 0x1) << 7)
+#define NI_E_MISC_CMD_EXT_ATRIG		NI_E_MISC_CMD_INTEXT_ATRIG(0)
+#define NI_E_MISC_CMD_INT_ATRIG		NI_E_MISC_CMD_INTEXT_ATRIG(1)
+
+#define NI_E_AI_CFG_LO_REG		0x10	/* w16 */
+#define NI_E_AI_CFG_LO_LAST_CHAN	BIT(15)
+#define NI_E_AI_CFG_LO_GEN_TRIG		BIT(12)
+#define NI_E_AI_CFG_LO_DITHER		BIT(9)
+#define NI_E_AI_CFG_LO_UNI		BIT(8)
+#define NI_E_AI_CFG_LO_GAIN(x)		((x) << 0)
+
+#define NI_E_AI_CFG_HI_REG		0x12	/* w16 */
+#define NI_E_AI_CFG_HI_TYPE(x)		(((x) & 0x7) << 12)
+#define NI_E_AI_CFG_HI_TYPE_DIFF	NI_E_AI_CFG_HI_TYPE(1)
+#define NI_E_AI_CFG_HI_TYPE_COMMON	NI_E_AI_CFG_HI_TYPE(2)
+#define NI_E_AI_CFG_HI_TYPE_GROUND	NI_E_AI_CFG_HI_TYPE(3)
+#define NI_E_AI_CFG_HI_AC_COUPLE	BIT(11)
+#define NI_E_AI_CFG_HI_CHAN(x)		(((x) & 0x3f) << 0)
+
+#define NI_E_AO_CFG_REG			0x16	/* w16 */
+#define NI_E_AO_DACSEL(x)		((x) << 8)
+#define NI_E_AO_GROUND_REF		BIT(3)
+#define NI_E_AO_EXT_REF			BIT(2)
+#define NI_E_AO_DEGLITCH		BIT(1)
+#define NI_E_AO_CFG_BIP			BIT(0)
+
+#define NI_E_DAC_DIRECT_DATA_REG(x)	(0x18 + ((x) * 2)) /* w16 */
+
+#define NI_E_8255_BASE			0x19	/* rw8 */
+
+#define NI_E_AI_FIFO_DATA_REG		0x1c	/* r16 */
+
+#define NI_E_AO_FIFO_DATA_REG		0x1e	/* w16 */
 
-#define AI_Personal_Register		77
-#define AI_SHIFTIN_Pulse_Width			_bit15
-#define AI_EOC_Polarity				_bit14
-#define AI_SOC_Polarity				_bit13
-#define AI_SHIFTIN_Polarity			_bit12
-#define AI_CONVERT_Pulse_Timebase		_bit11
-#define AI_CONVERT_Pulse_Width			_bit10
-#define AI_CONVERT_Original_Pulse		_bit9
-#define AI_FIFO_Flags_Polarity			_bit8
-#define AI_Overrun_Mode				_bit7
-#define AI_EXTMUX_CLK_Pulse_Width		_bit6
-#define AI_LOCALMUX_CLK_Pulse_Width		_bit5
-#define AI_AIFREQ_Polarity			_bit4
-
-#define AO_Personal_Register		78
-enum AO_Personal_Bits {
-	AO_Interval_Buffer_Mode = 1 << 3,
-	AO_BC_Source_Select = 1 << 4,
-	AO_UPDATE_Pulse_Width = 1 << 5,
-	AO_UPDATE_Pulse_Timebase = 1 << 6,
-	AO_UPDATE_Original_Pulse = 1 << 7,
-	AO_DMA_PIO_Control = 1 << 8,	/* M Series: reserved */
-	AO_AOFREQ_Polarity = 1 << 9,	/* M Series: reserved */
-	AO_FIFO_Enable = 1 << 10,
-	AO_FIFO_Flags_Polarity = 1 << 11,	/* M Series: reserved */
-	AO_TMRDACWR_Pulse_Width = 1 << 12,
-	AO_Fast_CPU = 1 << 13,	/* M Series: reserved */
-	AO_Number_Of_DAC_Packages = 1 << 14,	/*  1 for "single" mode, 0 for "dual" */
-	AO_Multiple_DACS_Per_Package = 1 << 15	/*  m-series only */
-};
-#define	RTSI_Trig_A_Output_Register	79
-#define	RTSI_Trig_B_Output_Register	80
-enum RTSI_Trig_B_Output_Bits {
-	RTSI_Sub_Selection_1_Bit = 0x8000	/*  not for m-series */
-};
-static inline unsigned RTSI_Trig_Output_Bits(unsigned rtsi_channel,
-					     unsigned source)
-{
-	return (source & 0xf) << ((rtsi_channel % 4) * 4);
-};
+/*
+ * 611x registers (these boards differ from the e-series)
+ */
+#define NI611X_MAGIC_REG		0x19	/* w8 (new) */
+#define NI611X_CALIB_CHAN_SEL_REG	0x1a	/* w16 (new) */
+#define NI611X_AI_FIFO_DATA_REG		0x1c	/* r32 (incompatible) */
+#define NI611X_AI_FIFO_OFFSET_LOAD_REG	0x05	/* r8 (new) */
+#define NI611X_AO_FIFO_DATA_REG		0x14	/* w32 (incompatible) */
+#define NI611X_CAL_GAIN_SEL_REG		0x05	/* w8 (new) */
+
+#define NI611X_AO_WINDOW_ADDR_REG	0x18
+#define NI611X_AO_WINDOW_DATA_REG	0x1e
 
-static inline unsigned RTSI_Trig_Output_Mask(unsigned rtsi_channel)
-{
-	return 0xf << ((rtsi_channel % 4) * 4);
-};
+/*
+ * 6143 registers
+ */
+#define NI6143_MAGIC_REG		0x19	/* w8 */
+#define NI6143_DMA_G0_G1_SEL_REG	0x0b	/* w8 */
+#define NI6143_PIPELINE_DELAY_REG	0x1f	/* w8 */
+#define NI6143_EOC_SET_REG		0x1d	/* w8 */
+#define NI6143_DMA_AI_SEL_REG		0x09	/* w8 */
+#define NI6143_AI_FIFO_DATA_REG		0x8c	/* r32 */
+#define NI6143_AI_FIFO_FLAG_REG		0x84	/* w32 */
+#define NI6143_AI_FIFO_CTRL_REG		0x88	/* w32 */
+#define NI6143_AI_FIFO_STATUS_REG	0x88	/* r32 */
+#define NI6143_AI_FIFO_DMA_THRESH_REG	0x90	/* w32 */
+#define NI6143_AI_FIFO_WORDS_AVAIL_REG	0x94	/* w32 */
+
+#define NI6143_CALIB_CHAN_REG		0x42	/* w16 */
+#define NI6143_CALIB_CHAN_RELAY_ON	BIT(15)
+#define NI6143_CALIB_CHAN_RELAY_OFF	BIT(14)
+#define NI6143_CALIB_CHAN(x)		(((x) & 0xf) << 0)
+#define NI6143_CALIB_CHAN_GND_GND	NI6143_CALIB_CHAN(0) /* Offset Cal */
+#define NI6143_CALIB_CHAN_2V5_GND	NI6143_CALIB_CHAN(2) /* 2.5V ref */
+#define NI6143_CALIB_CHAN_PWM_GND	NI6143_CALIB_CHAN(5) /* +-5V Self Cal */
+#define NI6143_CALIB_CHAN_2V5_PWM	NI6143_CALIB_CHAN(10) /* PWM Cal */
+#define NI6143_CALIB_CHAN_PWM_PWM	NI6143_CALIB_CHAN(13) /* CMRR */
+#define NI6143_CALIB_CHAN_GND_PWM	NI6143_CALIB_CHAN(14) /* PWM Cal */
+#define NI6143_CALIB_LO_TIME_REG	0x20	/* w16 */
+#define NI6143_CALIB_HI_TIME_REG	0x22	/* w16 */
+#define NI6143_RELAY_COUNTER_LOAD_REG	0x4c	/* w32 */
+#define NI6143_SIGNATURE_REG		0x50	/* w32 */
+#define NI6143_RELEASE_DATE_REG		0x54	/* w32 */
+#define NI6143_RELEASE_OLDEST_DATE_REG	0x58	/* w32 */
 
-/* inverse to RTSI_Trig_Output_Bits() */
-static inline unsigned RTSI_Trig_Output_Source(unsigned rtsi_channel,
-					       unsigned bits)
-{
-	return (bits >> ((rtsi_channel % 4) * 4)) & 0xf;
-};
+/*
+ * 671x, 611x windowed ao registers
+ */
+#define NI671X_DAC_DIRECT_DATA_REG(x)	(0x00 + (x))	/* w16 */
+#define NI611X_AO_TIMED_REG		0x10	/* w16 */
+#define NI671X_AO_IMMEDIATE_REG		0x11	/* w16 */
+#define NI611X_AO_FIFO_OFFSET_LOAD_REG	0x13	/* w32 */
+#define NI67XX_AO_SP_UPDATES_REG	0x14	/* w16 */
+#define NI611X_AO_WAVEFORM_GEN_REG	0x15	/* w16 */
+#define NI611X_AO_MISC_REG		0x16	/* w16 */
+#define NI611X_AO_MISC_CLEAR_WG		BIT(0)
+#define NI67XX_AO_CAL_CHAN_SEL_REG	0x17	/* w16 */
+#define NI67XX_AO_CFG2_REG		0x18	/* w16 */
+#define NI67XX_CAL_CMD_REG		0x19	/* w16 */
+#define NI67XX_CAL_STATUS_REG		0x1a	/* r8 */
+#define NI67XX_CAL_STATUS_BUSY		BIT(0)
+#define NI67XX_CAL_STATUS_OSC_DETECT	BIT(1)
+#define NI67XX_CAL_STATUS_OVERRANGE	BIT(2)
+#define NI67XX_CAL_DATA_REG		0x1b	/* r16 */
+#define NI67XX_CAL_CFG_HI_REG		0x1c	/* rw16 */
+#define NI67XX_CAL_CFG_LO_REG		0x1d	/* rw16 */
+
+#define CS5529_CMD_CB			BIT(7)
+#define CS5529_CMD_SINGLE_CONV		BIT(6)
+#define CS5529_CMD_CONT_CONV		BIT(5)
+#define CS5529_CMD_READ			BIT(4)
+#define CS5529_CMD_REG(x)		(((x) & 0x7) << 1)
+#define CS5529_CMD_REG_MASK		CS5529_CMD_REG(7)
+#define CS5529_CMD_PWR_SAVE		BIT(0)
+
+#define CS5529_OFFSET_REG		CS5529_CMD_REG(0)
+#define CS5529_GAIN_REG			CS5529_CMD_REG(1)
+#define CS5529_CONV_DATA_REG		CS5529_CMD_REG(3)
+#define CS5529_SETUP_REG		CS5529_CMD_REG(4)
+
+#define CS5529_CFG_REG			CS5529_CMD_REG(2)
+#define CS5529_CFG_AOUT(x)		BIT(22 + (x))
+#define CS5529_CFG_DOUT(x)		BIT(18 + (x))
+#define CS5529_CFG_LOW_PWR_MODE		BIT(16)
+#define CS5529_CFG_WORD_RATE(x)		(((x) & 0x7) << 13)
+#define CS5529_CFG_WORD_RATE_MASK	CS5529_CFG_WORD_RATE(0x7)
+#define CS5529_CFG_WORD_RATE_2180	CS5529_CFG_WORD_RATE(0)
+#define CS5529_CFG_WORD_RATE_1092	CS5529_CFG_WORD_RATE(1)
+#define CS5529_CFG_WORD_RATE_532	CS5529_CFG_WORD_RATE(2)
+#define CS5529_CFG_WORD_RATE_388	CS5529_CFG_WORD_RATE(3)
+#define CS5529_CFG_WORD_RATE_324	CS5529_CFG_WORD_RATE(4)
+#define CS5529_CFG_WORD_RATE_17444	CS5529_CFG_WORD_RATE(5)
+#define CS5529_CFG_WORD_RATE_8724	CS5529_CFG_WORD_RATE(6)
+#define CS5529_CFG_WORD_RATE_4364	CS5529_CFG_WORD_RATE(7)
+#define CS5529_CFG_UNIPOLAR		BIT(12)
+#define CS5529_CFG_RESET		BIT(7)
+#define CS5529_CFG_RESET_VALID		BIT(6)
+#define CS5529_CFG_PORT_FLAG		BIT(5)
+#define CS5529_CFG_PWR_SAVE_SEL		BIT(4)
+#define CS5529_CFG_DONE_FLAG		BIT(3)
+#define CS5529_CFG_CALIB(x)		(((x) & 0x7) << 0)
+#define CS5529_CFG_CALIB_NONE		CS5529_CFG_CALIB(0)
+#define CS5529_CFG_CALIB_OFFSET_SELF	CS5529_CFG_CALIB(1)
+#define CS5529_CFG_CALIB_GAIN_SELF	CS5529_CFG_CALIB(2)
+#define CS5529_CFG_CALIB_BOTH_SELF	CS5529_CFG_CALIB(3)
+#define CS5529_CFG_CALIB_OFFSET_SYS	CS5529_CFG_CALIB(5)
+#define CS5529_CFG_CALIB_GAIN_SYS	CS5529_CFG_CALIB(6)
 
-#define	RTSI_Board_Register		81
-#define Write_Strobe_0_Register		82
-#define Write_Strobe_1_Register		83
-#define Write_Strobe_2_Register		84
-#define Write_Strobe_3_Register		85
-
-#define AO_Output_Control_Register	86
-#define AO_External_Gate_Enable			_bit15
-#define AO_External_Gate_Select(x)		(((x)&0x1f)<<10)
-#define AO_Number_Of_Channels(x)		(((x)&0xf)<<6)
-#define AO_UPDATE2_Output_Select(x)		(((x)&0x3)<<4)
-#define AO_External_Gate_Polarity		_bit3
-#define AO_UPDATE2_Output_Toggle		_bit2
-enum ao_update_output_selection {
-	AO_Update_Output_High_Z = 0,
-	AO_Update_Output_Ground = 1,
-	AO_Update_Output_Enable_Low = 2,
-	AO_Update_Output_Enable_High = 3
-};
-static unsigned AO_UPDATE_Output_Select(enum ao_update_output_selection
-					selection)
-{
-	return selection & 0x3;
-}
-
-#define AI_Mode_3_Register		87
-#define AI_Trigger_Length			_bit15
-#define AI_Delay_START				_bit14
-#define AI_Software_Gate			_bit13
-#define AI_SI_Special_Trigger_Delay		_bit12
-#define AI_SI2_Source_Select			_bit11
-#define AI_Delayed_START2			_bit10
-#define AI_Delayed_START1			_bit9
-#define AI_External_Gate_Mode			_bit8
-#define AI_FIFO_Mode_HF_to_E			(3<<6)
-#define AI_FIFO_Mode_F				(2<<6)
-#define AI_FIFO_Mode_HF				(1<<6)
-#define AI_FIFO_Mode_NE				(0<<6)
-#define AI_External_Gate_Polarity		_bit5
-#define AI_External_Gate_Select(a)		((a) & 0x1f)
-
-#define G_Autoincrement_Register(a)	(68+(a))
-#define G_Command_Register(a)		(6+(a))
-#define G_HW_Save_Register(a)		(8+(a)*2)
-#define G_HW_Save_Register_High(a)	(8+(a)*2)
-#define G_HW_Save_Register_Low(a)	(9+(a)*2)
-#define G_Input_Select_Register(a)	(36+(a))
-#define G_Load_A_Register(a)		(28+(a)*4)
-#define G_Load_A_Register_High(a)	(28+(a)*4)
-#define G_Load_A_Register_Low(a)	(29+(a)*4)
-#define G_Load_B_Register(a)		(30+(a)*4)
-#define G_Load_B_Register_High(a)	(30+(a)*4)
-#define G_Load_B_Register_Low(a)	(31+(a)*4)
-#define G_Mode_Register(a)		(26+(a))
-#define G_Save_Register(a)		(12+(a)*2)
-#define G_Save_Register_High(a)		(12+(a)*2)
-#define G_Save_Register_Low(a)		(13+(a)*2)
-#define G_Status_Register		4
-#define Analog_Trigger_Etc_Register	61
-
-/* command register */
-#define G_Disarm_Copy			_bit15	/* strobe */
-#define G_Save_Trace_Copy		_bit14
-#define G_Arm_Copy			_bit13	/* strobe */
-#define G_Bank_Switch_Start		_bit10	/* strobe */
-#define G_Little_Big_Endian		_bit9
-#define G_Synchronized_Gate		_bit8
-#define G_Write_Switch			_bit7
-#define G_Up_Down(a)			(((a)&0x03)<<5)
-#define G_Disarm			_bit4	/* strobe */
-#define G_Analog_Trigger_Reset		_bit3	/* strobe */
-#define G_Save_Trace			_bit1
-#define G_Arm				_bit0	/* strobe */
-
-/*channel agnostic names for the command register #defines */
-#define G_Bank_Switch_Enable		_bit12
-#define G_Bank_Switch_Mode		_bit11
-#define G_Load				_bit2	/* strobe */
-
-/* input select register */
-#define G_Gate_Select(a)		(((a)&0x1f)<<7)
-#define G_Source_Select(a)		(((a)&0x1f)<<2)
-#define G_Write_Acknowledges_Irq	_bit1
-#define G_Read_Acknowledges_Irq		_bit0
-
-/* same input select register, but with channel agnostic names */
-#define G_Source_Polarity		_bit15
-#define G_Output_Polarity		_bit14
-#define G_OR_Gate			_bit13
-#define G_Gate_Select_Load_Source	_bit12
-
-/* mode register */
-#define G_Loading_On_TC			_bit12
-#define G_Output_Mode(a)		(((a)&0x03)<<8)
-#define G_Trigger_Mode_For_Edge_Gate(a)	(((a)&0x03)<<3)
-#define G_Gating_Mode(a)		(((a)&0x03)<<0)
-
-/* same input mode register, but with channel agnostic names */
-#define G_Load_Source_Select		_bit7
-#define G_Reload_Source_Switching	_bit15
-#define G_Loading_On_Gate		_bit14
-#define G_Gate_Polarity 		_bit13
-
-#define G_Counting_Once(a)		(((a)&0x03)<<10)
-#define G_Stop_Mode(a)			(((a)&0x03)<<5)
-#define G_Gate_On_Both_Edges		_bit2
-
-/* G_Status_Register */
-#define G1_Gate_Error_St		_bit15
-#define G0_Gate_Error_St		_bit14
-#define G1_TC_Error_St			_bit13
-#define G0_TC_Error_St			_bit12
-#define G1_No_Load_Between_Gates_St	_bit11
-#define G0_No_Load_Between_Gates_St	_bit10
-#define G1_Armed_St			_bit9
-#define G0_Armed_St			_bit8
-#define G1_Stale_Data_St		_bit7
-#define G0_Stale_Data_St		_bit6
-#define G1_Next_Load_Source_St		_bit5
-#define G0_Next_Load_Source_St		_bit4
-#define G1_Counting_St			_bit3
-#define G0_Counting_St			_bit2
-#define G1_Save_St			_bit1
-#define G0_Save_St			_bit0
-
-/* general purpose counter timer */
-#define G_Autoincrement(a)              ((a)<<0)
-
-/*Analog_Trigger_Etc_Register*/
-#define Analog_Trigger_Mode(x) ((x) & 0x7)
-#define Analog_Trigger_Enable _bit3
-#define Analog_Trigger_Drive _bit4
-#define GPFO_1_Output_Select		_bit7
-#define GPFO_0_Output_Select(a)		((a)<<11)
-#define GPFO_0_Output_Enable		_bit14
-#define GPFO_1_Output_Enable		_bit15
-
-/* Additional windowed registers unique to E series */
-
-/* 16 bit registers shadowed from DAQ-STC */
-#define Window_Address			0x00
-#define Window_Data			0x02
-
-#define Configuration_Memory_Clear	82
-#define ADC_FIFO_Clear			83
-#define DAC_FIFO_Clear			84
-
-/* i/o port offsets */
-
-/* 8 bit registers */
-#define XXX_Status			0x01
-enum XXX_Status_Bits {
-	PROMOUT = 0x1,
-	AI_FIFO_LOWER_NOT_EMPTY = 0x8,
-};
-#define Serial_Command			0x0d
-#define Misc_Command			0x0f
-#define Port_A				0x19
-#define Port_B				0x1b
-#define Port_C				0x1d
-#define Configuration			0x1f
-#define Strobes				0x01
-#define Channel_A_Mode			0x03
-#define Channel_B_Mode			0x05
-#define Channel_C_Mode			0x07
-#define AI_AO_Select			0x09
-enum AI_AO_Select_Bits {
-	AI_DMA_Select_Shift = 0,
-	AI_DMA_Select_Mask = 0xf,
-	AO_DMA_Select_Shift = 4,
-	AO_DMA_Select_Mask = 0xf << AO_DMA_Select_Shift
-};
-#define G0_G1_Select			0x0b
-static inline unsigned ni_stc_dma_channel_select_bitfield(unsigned channel)
-{
-	if (channel < 4)
-		return 1 << channel;
-	if (channel == 4)
-		return 0x3;
-	if (channel == 5)
-		return 0x5;
-	BUG();
-	return 0;
-}
-
-static inline unsigned GPCT_DMA_Select_Bits(unsigned gpct_index,
-					    unsigned mite_channel)
-{
-	BUG_ON(gpct_index > 1);
-	return ni_stc_dma_channel_select_bitfield(mite_channel) << (4 *
-								    gpct_index);
-}
-
-static inline unsigned GPCT_DMA_Select_Mask(unsigned gpct_index)
-{
-	BUG_ON(gpct_index > 1);
-	return 0xf << (4 * gpct_index);
-}
-
-/* 16 bit registers */
-
-#define Configuration_Memory_Low	0x10
-enum Configuration_Memory_Low_Bits {
-	AI_DITHER = 0x200,
-	AI_LAST_CHANNEL = 0x8000,
-};
-#define Configuration_Memory_High	0x12
-enum Configuration_Memory_High_Bits {
-	AI_AC_COUPLE = 0x800,
-	AI_DIFFERENTIAL = 0x1000,
-	AI_COMMON = 0x2000,
-	AI_GROUND = 0x3000,
-};
-static inline unsigned int AI_CONFIG_CHANNEL(unsigned int channel)
-{
-	return channel & 0x3f;
-}
-
-#define ADC_FIFO_Data_Register		0x1c
-
-#define AO_Configuration		0x16
-#define AO_Bipolar		_bit0
-#define AO_Deglitch		_bit1
-#define AO_Ext_Ref		_bit2
-#define AO_Ground_Ref		_bit3
-#define AO_Channel(x)		((x) << 8)
-
-#define DAC_FIFO_Data			0x1e
-#define DAC0_Direct_Data		0x18
-#define DAC1_Direct_Data		0x1a
-
-/* 611x registers (these boards differ from the e-series) */
-
-#define Magic_611x			0x19	/* w8 (new) */
-#define Calibration_Channel_Select_611x	0x1a	/* w16 (new) */
-#define ADC_FIFO_Data_611x		0x1c	/* r32 (incompatible) */
-#define AI_FIFO_Offset_Load_611x	0x05	/* r8 (new) */
-#define DAC_FIFO_Data_611x		0x14	/* w32 (incompatible) */
-#define Cal_Gain_Select_611x		0x05	/* w8 (new) */
-
-#define AO_Window_Address_611x		0x18
-#define AO_Window_Data_611x		0x1e
-
-/* 6143 registers */
-#define Magic_6143			0x19	/* w8 */
-#define G0G1_DMA_Select_6143		0x0B	/* w8 */
-#define PipelineDelay_6143		0x1f	/* w8 */
-#define EOC_Set_6143			0x1D	/* w8 */
-#define AIDMA_Select_6143		0x09	/* w8 */
-#define AIFIFO_Data_6143		0x8C	/* w32 */
-#define AIFIFO_Flag_6143		0x84	/* w32 */
-#define AIFIFO_Control_6143		0x88	/* w32 */
-#define AIFIFO_Status_6143		0x88	/* w32 */
-#define AIFIFO_DMAThreshold_6143	0x90	/* w32 */
-#define AIFIFO_Words_Available_6143	0x94	/* w32 */
-
-#define Calibration_Channel_6143	0x42	/* w16 */
-#define Calibration_LowTime_6143	0x20	/* w16 */
-#define Calibration_HighTime_6143	0x22	/* w16 */
-#define Relay_Counter_Load_Val__6143	0x4C	/* w32 */
-#define Signature_6143			0x50	/* w32 */
-#define Release_Date_6143		0x54	/* w32 */
-#define Release_Oldest_Date_6143	0x58	/* w32 */
-
-#define Calibration_Channel_6143_RelayOn	0x8000	/* Calibration relay switch On */
-#define Calibration_Channel_6143_RelayOff	0x4000	/* Calibration relay switch Off */
-#define Calibration_Channel_Gnd_Gnd	0x00	/* Offset Calibration */
-#define Calibration_Channel_2v5_Gnd	0x02	/* 2.5V Reference */
-#define Calibration_Channel_Pwm_Gnd	0x05	/* +/- 5V Self Cal */
-#define Calibration_Channel_2v5_Pwm	0x0a	/* PWM Calibration */
-#define Calibration_Channel_Pwm_Pwm	0x0d	/* CMRR */
-#define Calibration_Channel_Gnd_Pwm	0x0e	/* PWM Calibration */
-
-/* 671x, 611x registers */
-
-/* 671xi, 611x windowed ao registers */
-enum windowed_regs_67xx_61xx {
-	AO_Immediate_671x = 0x11,	/* W 16 */
-	AO_Timed_611x = 0x10,	/* W 16 */
-	AO_FIFO_Offset_Load_611x = 0x13,	/* W32 */
-	AO_Later_Single_Point_Updates = 0x14,	/* W 16 */
-	AO_Waveform_Generation_611x = 0x15,	/* W 16 */
-	AO_Misc_611x = 0x16,	/* W 16 */
-	AO_Calibration_Channel_Select_67xx = 0x17,	/* W 16 */
-	AO_Configuration_2_67xx = 0x18,	/* W 16 */
-	CAL_ADC_Command_67xx = 0x19,	/* W 8 */
-	CAL_ADC_Status_67xx = 0x1a,	/* R 8 */
-	CAL_ADC_Data_67xx = 0x1b,	/* R 16 */
-	CAL_ADC_Config_Data_High_Word_67xx = 0x1c,	/* RW 16 */
-	CAL_ADC_Config_Data_Low_Word_67xx = 0x1d,	/* RW 16 */
+/*
+ * M-Series specific registers not handled by the DAQ-STC and GPCT register
+ * remapping.
+ */
+#define NI_M_CDIO_DMA_SEL_REG		0x007
+#define NI_M_CDIO_DMA_SEL_CDO(x)	(((x) & 0xf) << 4)
+#define NI_M_CDIO_DMA_SEL_CDO_MASK	NI_M_CDIO_DMA_SEL_CDO(0xf)
+#define NI_M_CDIO_DMA_SEL_CDI(x)	(((x) & 0xf) << 0)
+#define NI_M_CDIO_DMA_SEL_CDI_MASK	NI_M_CDIO_DMA_SEL_CDI(0xf)
+#define NI_M_SCXI_STATUS_REG		0x007
+#define NI_M_AI_AO_SEL_REG		0x009
+#define NI_M_G0_G1_SEL_REG		0x00b
+#define NI_M_MISC_CMD_REG		0x00f
+#define NI_M_SCXI_SER_DO_REG		0x011
+#define NI_M_SCXI_CTRL_REG		0x013
+#define NI_M_SCXI_OUT_ENA_REG		0x015
+#define NI_M_AI_FIFO_DATA_REG		0x01c
+#define NI_M_DIO_REG			0x024
+#define NI_M_DIO_DIR_REG		0x028
+#define NI_M_CAL_PWM_REG		0x040
+#define NI_M_CAL_PWM_HIGH_TIME(x)	(((x) & 0xffff) << 16)
+#define NI_M_CAL_PWM_LOW_TIME(x)	(((x) & 0xffff) << 0)
+#define NI_M_GEN_PWM_REG(x)		(0x044 + ((x) * 2))
+#define NI_M_AI_CFG_FIFO_DATA_REG	0x05e
+#define NI_M_AI_CFG_LAST_CHAN		BIT(14)
+#define NI_M_AI_CFG_DITHER		BIT(13)
+#define NI_M_AI_CFG_POLARITY		BIT(12)
+#define NI_M_AI_CFG_GAIN(x)		(((x) & 0x7) << 9)
+#define NI_M_AI_CFG_CHAN_TYPE(x)	(((x) & 0x7) << 6)
+#define NI_M_AI_CFG_CHAN_TYPE_MASK	NI_M_AI_CFG_CHAN_TYPE(7)
+#define NI_M_AI_CFG_CHAN_TYPE_CALIB	NI_M_AI_CFG_CHAN_TYPE(0)
+#define NI_M_AI_CFG_CHAN_TYPE_DIFF	NI_M_AI_CFG_CHAN_TYPE(1)
+#define NI_M_AI_CFG_CHAN_TYPE_COMMON	NI_M_AI_CFG_CHAN_TYPE(2)
+#define NI_M_AI_CFG_CHAN_TYPE_GROUND	NI_M_AI_CFG_CHAN_TYPE(3)
+#define NI_M_AI_CFG_CHAN_TYPE_AUX	NI_M_AI_CFG_CHAN_TYPE(5)
+#define NI_M_AI_CFG_CHAN_TYPE_GHOST	NI_M_AI_CFG_CHAN_TYPE(7)
+#define NI_M_AI_CFG_BANK_SEL(x)		((((x) & 0x40) << 4) | ((x) & 0x30))
+#define NI_M_AI_CFG_CHAN_SEL(x)		(((x) & 0xf) << 0)
+#define NI_M_INTC_ENA_REG		0x088
+#define NI_M_INTC_ENA			BIT(0)
+#define NI_M_INTC_STATUS_REG		0x088
+#define NI_M_INTC_STATUS		BIT(0)
+#define NI_M_ATRIG_CTRL_REG		0x08c
+#define NI_M_AO_SER_INT_ENA_REG		0x0a0
+#define NI_M_AO_SER_INT_ACK_REG		0x0a1
+#define NI_M_AO_SER_INT_STATUS_REG	0x0a1
+#define NI_M_AO_CALIB_REG		0x0a3
+#define NI_M_AO_FIFO_DATA_REG		0x0a4
+#define NI_M_PFI_FILTER_REG		0x0b0
+#define NI_M_PFI_FILTER_SEL(_c, _f)	(((_f) & 0x3) << ((_c) * 2))
+#define NI_M_PFI_FILTER_SEL_MASK(_c)	NI_M_PFI_FILTER_SEL((_c), 0x3)
+#define NI_M_RTSI_FILTER_REG		0x0b4
+#define NI_M_SCXI_LEGACY_COMPAT_REG	0x0bc
+#define NI_M_DAC_DIRECT_DATA_REG(x)	(0x0c0 + ((x) * 4))
+#define NI_M_AO_WAVEFORM_ORDER_REG(x)	(0x0c2 + ((x) * 4))
+#define NI_M_AO_CFG_BANK_REG(x)		(0x0c3 + ((x) * 4))
+#define NI_M_AO_CFG_BANK_BIPOLAR	BIT(7)
+#define NI_M_AO_CFG_BANK_UPDATE_TIMED	BIT(6)
+#define NI_M_AO_CFG_BANK_REF(x)		(((x) & 0x7) << 3)
+#define NI_M_AO_CFG_BANK_REF_MASK	NI_M_AO_CFG_BANK_REF(7)
+#define NI_M_AO_CFG_BANK_REF_INT_10V	NI_M_AO_CFG_BANK_REF(0)
+#define NI_M_AO_CFG_BANK_REF_INT_5V	NI_M_AO_CFG_BANK_REF(1)
+#define NI_M_AO_CFG_BANK_OFFSET(x)	(((x) & 0x7) << 0)
+#define NI_M_AO_CFG_BANK_OFFSET_MASK	NI_M_AO_CFG_BANK_OFFSET(7)
+#define NI_M_AO_CFG_BANK_OFFSET_0V	NI_M_AO_CFG_BANK_OFFSET(0)
+#define NI_M_AO_CFG_BANK_OFFSET_5V	NI_M_AO_CFG_BANK_OFFSET(1)
+#define NI_M_RTSI_SHARED_MUX_REG	0x1a2
+#define NI_M_CLK_FOUT2_REG		0x1c4
+#define NI_M_CLK_FOUT2_RTSI_10MHZ	BIT(7)
+#define NI_M_CLK_FOUT2_TIMEBASE3_PLL	BIT(6)
+#define NI_M_CLK_FOUT2_TIMEBASE1_PLL	BIT(5)
+#define NI_M_CLK_FOUT2_PLL_SRC(x)	(((x) & 0x1f) << 0)
+#define NI_M_CLK_FOUT2_PLL_SRC_MASK	NI_M_CLK_FOUT2_PLL_SRC(0x1f)
+#define NI_M_MAX_RTSI_CHAN		7
+#define NI_M_CLK_FOUT2_PLL_SRC_RTSI(x)	(((x) == NI_M_MAX_RTSI_CHAN)	\
+					 ? NI_M_CLK_FOUT2_PLL_SRC(0x1b)	\
+					 : NI_M_CLK_FOUT2_PLL_SRC(0xb + (x)))
+#define NI_M_CLK_FOUT2_PLL_SRC_STAR	NI_M_CLK_FOUT2_PLL_SRC(0x14)
+#define NI_M_CLK_FOUT2_PLL_SRC_PXI10	NI_M_CLK_FOUT2_PLL_SRC(0x1d)
+#define NI_M_PLL_CTRL_REG		0x1c6
+#define NI_M_PLL_CTRL_VCO_MODE(x)	(((x) & 0x3) << 13)
+#define NI_M_PLL_CTRL_VCO_MODE_200_325MHZ NI_M_PLL_CTRL_VCO_MODE(0)
+#define NI_M_PLL_CTRL_VCO_MODE_175_225MHZ NI_M_PLL_CTRL_VCO_MODE(1)
+#define NI_M_PLL_CTRL_VCO_MODE_100_225MHZ NI_M_PLL_CTRL_VCO_MODE(2)
+#define NI_M_PLL_CTRL_VCO_MODE_75_150MHZ  NI_M_PLL_CTRL_VCO_MODE(3)
+#define NI_M_PLL_CTRL_ENA		BIT(12)
+#define NI_M_PLL_MAX_DIVISOR		0x10
+#define NI_M_PLL_CTRL_DIVISOR(x)	(((x) & 0xf) << 8)
+#define NI_M_PLL_MAX_MULTIPLIER		0x100
+#define NI_M_PLL_CTRL_MULTIPLIER(x)	(((x) & 0xff) << 0)
+#define NI_M_PLL_STATUS_REG		0x1c8
+#define NI_M_PLL_STATUS_LOCKED		BIT(0)
+#define NI_M_PFI_OUT_SEL_REG(x)		(0x1d0 + ((x) * 2))
+#define NI_M_PFI_CHAN(_c)		(((_c) % 3) * 5)
+#define NI_M_PFI_OUT_SEL(_c, _s)	(((_s) & 0x1f) << NI_M_PFI_CHAN(_c))
+#define NI_M_PFI_OUT_SEL_MASK(_c)	(0x1f << NI_M_PFI_CHAN(_c))
+#define NI_M_PFI_OUT_SEL_TO_SRC(_c, _b)	(((_b) >> NI_M_PFI_CHAN(_c)) & 0x1f)
+#define NI_M_PFI_DI_REG			0x1dc
+#define NI_M_PFI_DO_REG			0x1de
+#define NI_M_CFG_BYPASS_FIFO_REG	0x218
+#define NI_M_CFG_BYPASS_FIFO		BIT(31)
+#define NI_M_CFG_BYPASS_AI_POLARITY	BIT(22)
+#define NI_M_CFG_BYPASS_AI_DITHER	BIT(21)
+#define NI_M_CFG_BYPASS_AI_GAIN(x)	(((x) & 0x7) << 18)
+#define NI_M_CFG_BYPASS_AO_CAL(x)	(((x) & 0xf) << 15)
+#define NI_M_CFG_BYPASS_AO_CAL_MASK	NI_M_CFG_BYPASS_AO_CAL(0xf)
+#define NI_M_CFG_BYPASS_AI_MODE_MUX(x)	(((x) & 0x3) << 13)
+#define NI_M_CFG_BYPASS_AI_MODE_MUX_MASK NI_M_CFG_BYPASS_AI_MODE_MUX(3)
+#define NI_M_CFG_BYPASS_AI_CAL_NEG(x)	(((x) & 0x7) << 10)
+#define NI_M_CFG_BYPASS_AI_CAL_NEG_MASK	NI_M_CFG_BYPASS_AI_CAL_NEG(7)
+#define NI_M_CFG_BYPASS_AI_CAL_POS(x)	(((x) & 0x7) << 7)
+#define NI_M_CFG_BYPASS_AI_CAL_POS_MASK	NI_M_CFG_BYPASS_AI_CAL_POS(7)
+#define NI_M_CFG_BYPASS_AI_CAL_MASK	(NI_M_CFG_BYPASS_AI_CAL_POS_MASK | \
+					 NI_M_CFG_BYPASS_AI_CAL_NEG_MASK | \
+					 NI_M_CFG_BYPASS_AI_MODE_MUX_MASK | \
+					 NI_M_CFG_BYPASS_AO_CAL_MASK)
+#define NI_M_CFG_BYPASS_AI_BANK(x)	(((x) & 0xf) << 3)
+#define NI_M_CFG_BYPASS_AI_BANK_MASK	NI_M_CFG_BYPASS_AI_BANK(0xf)
+#define NI_M_CFG_BYPASS_AI_CHAN(x)	(((x) & 0x7) << 0)
+#define NI_M_CFG_BYPASS_AI_CHAN_MASK	NI_M_CFG_BYPASS_AI_CHAN(7)
+#define NI_M_SCXI_DIO_ENA_REG		0x21c
+#define NI_M_CDI_FIFO_DATA_REG		0x220
+#define NI_M_CDO_FIFO_DATA_REG		0x220
+#define NI_M_CDIO_STATUS_REG		0x224
+#define NI_M_CDIO_STATUS_CDI_OVERFLOW	BIT(20)
+#define NI_M_CDIO_STATUS_CDI_OVERRUN	BIT(19)
+#define NI_M_CDIO_STATUS_CDI_ERROR	(NI_M_CDIO_STATUS_CDI_OVERFLOW | \
+					 NI_M_CDIO_STATUS_CDI_OVERRUN)
+#define NI_M_CDIO_STATUS_CDI_FIFO_REQ	BIT(18)
+#define NI_M_CDIO_STATUS_CDI_FIFO_FULL	BIT(17)
+#define NI_M_CDIO_STATUS_CDI_FIFO_EMPTY	BIT(16)
+#define NI_M_CDIO_STATUS_CDO_UNDERFLOW	BIT(4)
+#define NI_M_CDIO_STATUS_CDO_OVERRUN	BIT(3)
+#define NI_M_CDIO_STATUS_CDO_ERROR	(NI_M_CDIO_STATUS_CDO_UNDERFLOW | \
+					 NI_M_CDIO_STATUS_CDO_OVERRUN)
+#define NI_M_CDIO_STATUS_CDO_FIFO_REQ	BIT(2)
+#define NI_M_CDIO_STATUS_CDO_FIFO_FULL	BIT(1)
+#define NI_M_CDIO_STATUS_CDO_FIFO_EMPTY	BIT(0)
+#define NI_M_CDIO_CMD_REG		0x224
+#define NI_M_CDI_CMD_SW_UPDATE		BIT(20)
+#define NI_M_CDO_CMD_SW_UPDATE		BIT(19)
+#define NI_M_CDO_CMD_F_E_INT_ENA_CLR	BIT(17)
+#define NI_M_CDO_CMD_F_E_INT_ENA_SET	BIT(16)
+#define NI_M_CDI_CMD_ERR_INT_CONFIRM	BIT(15)
+#define NI_M_CDO_CMD_ERR_INT_CONFIRM	BIT(14)
+#define NI_M_CDI_CMD_F_REQ_INT_ENA_CLR	BIT(13)
+#define NI_M_CDI_CMD_F_REQ_INT_ENA_SET	BIT(12)
+#define NI_M_CDO_CMD_F_REQ_INT_ENA_CLR	BIT(11)
+#define NI_M_CDO_CMD_F_REQ_INT_ENA_SET	BIT(10)
+#define NI_M_CDI_CMD_ERR_INT_ENA_CLR	BIT(9)
+#define NI_M_CDI_CMD_ERR_INT_ENA_SET	BIT(8)
+#define NI_M_CDO_CMD_ERR_INT_ENA_CLR	BIT(7)
+#define NI_M_CDO_CMD_ERR_INT_ENA_SET	BIT(6)
+#define NI_M_CDI_CMD_RESET		BIT(5)
+#define NI_M_CDO_CMD_RESET		BIT(4)
+#define NI_M_CDI_CMD_ARM		BIT(3)
+#define NI_M_CDI_CMD_DISARM		BIT(2)
+#define NI_M_CDO_CMD_ARM		BIT(1)
+#define NI_M_CDO_CMD_DISARM		BIT(0)
+#define NI_M_CDI_MODE_REG		0x228
+#define NI_M_CDI_MODE_DATA_LANE(x)	(((x) & 0x3) << 12)
+#define NI_M_CDI_MODE_DATA_LANE_MASK	NI_M_CDI_MODE_DATA_LANE(3)
+#define NI_M_CDI_MODE_DATA_LANE_0_15	NI_M_CDI_MODE_DATA_LANE(0)
+#define NI_M_CDI_MODE_DATA_LANE_16_31	NI_M_CDI_MODE_DATA_LANE(1)
+#define NI_M_CDI_MODE_DATA_LANE_0_7	NI_M_CDI_MODE_DATA_LANE(0)
+#define NI_M_CDI_MODE_DATA_LANE_8_15	NI_M_CDI_MODE_DATA_LANE(1)
+#define NI_M_CDI_MODE_DATA_LANE_16_23	NI_M_CDI_MODE_DATA_LANE(2)
+#define NI_M_CDI_MODE_DATA_LANE_24_31	NI_M_CDI_MODE_DATA_LANE(3)
+#define NI_M_CDI_MODE_FIFO_MODE		BIT(11)
+#define NI_M_CDI_MODE_POLARITY		BIT(10)
+#define NI_M_CDI_MODE_HALT_ON_ERROR	BIT(9)
+#define NI_M_CDI_MODE_SAMPLE_SRC(x)	(((x) & 0x3f) << 0)
+#define NI_M_CDI_MODE_SAMPLE_SRC_MASK	NI_M_CDI_MODE_SAMPLE_SRC(0x3f)
+#define NI_M_CDO_MODE_REG		0x22c
+#define NI_M_CDO_MODE_DATA_LANE(x)	(((x) & 0x3) << 12)
+#define NI_M_CDO_MODE_DATA_LANE_MASK	NI_M_CDO_MODE_DATA_LANE(3)
+#define NI_M_CDO_MODE_DATA_LANE_0_15	NI_M_CDO_MODE_DATA_LANE(0)
+#define NI_M_CDO_MODE_DATA_LANE_16_31	NI_M_CDO_MODE_DATA_LANE(1)
+#define NI_M_CDO_MODE_DATA_LANE_0_7	NI_M_CDO_MODE_DATA_LANE(0)
+#define NI_M_CDO_MODE_DATA_LANE_8_15	NI_M_CDO_MODE_DATA_LANE(1)
+#define NI_M_CDO_MODE_DATA_LANE_16_23	NI_M_CDO_MODE_DATA_LANE(2)
+#define NI_M_CDO_MODE_DATA_LANE_24_31	NI_M_CDO_MODE_DATA_LANE(3)
+#define NI_M_CDO_MODE_FIFO_MODE		BIT(11)
+#define NI_M_CDO_MODE_POLARITY		BIT(10)
+#define NI_M_CDO_MODE_HALT_ON_ERROR	BIT(9)
+#define NI_M_CDO_MODE_RETRANSMIT	BIT(8)
+#define NI_M_CDO_MODE_SAMPLE_SRC(x)	(((x) & 0x3f) << 0)
+#define NI_M_CDO_MODE_SAMPLE_SRC_MASK	NI_M_CDO_MODE_SAMPLE_SRC(0x3f)
+#define NI_M_CDI_MASK_ENA_REG		0x230
+#define NI_M_CDO_MASK_ENA_REG		0x234
+#define NI_M_STATIC_AI_CTRL_REG(x)	((x) ? (0x260 + (x)) : 0x064)
+#define NI_M_AO_REF_ATTENUATION_REG(x)	(0x264 + (x))
+#define NI_M_AO_REF_ATTENUATION_X5	BIT(0)
+
+enum {
+	ai_gain_16 = 0,
+	ai_gain_8,
+	ai_gain_14,
+	ai_gain_4,
+	ai_gain_611x,
+	ai_gain_622x,
+	ai_gain_628x,
+	ai_gain_6143
 };
-static inline unsigned int DACx_Direct_Data_671x(int channel)
-{
-	return channel;
-}
 
-enum AO_Misc_611x_Bits {
-	CLEAR_WG = 1,
+enum caldac_enum {
+	caldac_none = 0,
+	mb88341,
+	dac8800,
+	dac8043,
+	ad8522,
+	ad8804,
+	ad8842,
+	ad8804_debug
 };
-enum cs5529_configuration_bits {
-	CSCFG_CAL_CONTROL_MASK = 0x7,
-	CSCFG_SELF_CAL_OFFSET = 0x1,
-	CSCFG_SELF_CAL_GAIN = 0x2,
-	CSCFG_SELF_CAL_OFFSET_GAIN = 0x3,
-	CSCFG_SYSTEM_CAL_OFFSET = 0x5,
-	CSCFG_SYSTEM_CAL_GAIN = 0x6,
-	CSCFG_DONE = 1 << 3,
-	CSCFG_POWER_SAVE_SELECT = 1 << 4,
-	CSCFG_PORT_MODE = 1 << 5,
-	CSCFG_RESET_VALID = 1 << 6,
-	CSCFG_RESET = 1 << 7,
-	CSCFG_UNIPOLAR = 1 << 12,
-	CSCFG_WORD_RATE_2180_CYCLES = 0x0 << 13,
-	CSCFG_WORD_RATE_1092_CYCLES = 0x1 << 13,
-	CSCFG_WORD_RATE_532_CYCLES = 0x2 << 13,
-	CSCFG_WORD_RATE_388_CYCLES = 0x3 << 13,
-	CSCFG_WORD_RATE_324_CYCLES = 0x4 << 13,
-	CSCFG_WORD_RATE_17444_CYCLES = 0x5 << 13,
-	CSCFG_WORD_RATE_8724_CYCLES = 0x6 << 13,
-	CSCFG_WORD_RATE_4364_CYCLES = 0x7 << 13,
-	CSCFG_WORD_RATE_MASK = 0x7 << 13,
-	CSCFG_LOW_POWER = 1 << 16,
-};
-static inline unsigned int CS5529_CONFIG_DOUT(int output)
-{
-	return 1 << (18 + output);
-}
-
-static inline unsigned int CS5529_CONFIG_AOUT(int output)
-{
-	return 1 << (22 + output);
-}
-
-enum cs5529_command_bits {
-	CSCMD_POWER_SAVE = 0x1,
-	CSCMD_REGISTER_SELECT_MASK = 0xe,
-	CSCMD_OFFSET_REGISTER = 0x0,
-	CSCMD_GAIN_REGISTER = 0x2,
-	CSCMD_CONFIG_REGISTER = 0x4,
-	CSCMD_READ = 0x10,
-	CSCMD_CONTINUOUS_CONVERSIONS = 0x20,
-	CSCMD_SINGLE_CONVERSION = 0x40,
-	CSCMD_COMMAND = 0x80,
-};
-enum cs5529_status_bits {
-	CSS_ADC_BUSY = 0x1,
-	CSS_OSC_DETECT = 0x2,	/* indicates adc error */
-	CSS_OVERRANGE = 0x4,
-};
-#define SerDacLd(x)			(0x08<<(x))
-
-/*
-	This is stuff unique to the NI E series drivers,
-	but I thought I'd put it here anyway.
-*/
 
-enum { ai_gain_16 =
-	    0, ai_gain_8, ai_gain_14, ai_gain_4, ai_gain_611x, ai_gain_622x,
-	ai_gain_628x, ai_gain_6143
-};
-enum caldac_enum { caldac_none = 0, mb88341, dac8800, dac8043, ad8522,
-	ad8804, ad8842, ad8804_debug
-};
 enum ni_reg_type {
 	ni_reg_normal = 0x0,
 	ni_reg_611x = 0x1,
@@ -918,467 +941,6 @@ enum ni_reg_type {
 	ni_reg_6143 = 0x20
 };
 
-static const struct comedi_lrange range_ni_E_ao_ext;
-
-enum m_series_register_offsets {
-	M_Offset_CDIO_DMA_Select = 0x7,	/*  write */
-	M_Offset_SCXI_Status = 0x7,	/*  read */
-	M_Offset_AI_AO_Select = 0x9,	/*  write, same offset as e-series */
-	M_Offset_SCXI_Serial_Data_In = 0x9,	/*  read */
-	M_Offset_G0_G1_Select = 0xb,	/*  write, same offset as e-series */
-	M_Offset_Misc_Command = 0xf,
-	M_Offset_SCXI_Serial_Data_Out = 0x11,
-	M_Offset_SCXI_Control = 0x13,
-	M_Offset_SCXI_Output_Enable = 0x15,
-	M_Offset_AI_FIFO_Data = 0x1c,
-	M_Offset_Static_Digital_Output = 0x24,	/*  write */
-	M_Offset_Static_Digital_Input = 0x24,	/*  read */
-	M_Offset_DIO_Direction = 0x28,
-	M_Offset_Cal_PWM = 0x40,
-	M_Offset_AI_Config_FIFO_Data = 0x5e,
-	M_Offset_Interrupt_C_Enable = 0x88,	/*  write */
-	M_Offset_Interrupt_C_Status = 0x88,	/*  read */
-	M_Offset_Analog_Trigger_Control = 0x8c,
-	M_Offset_AO_Serial_Interrupt_Enable = 0xa0,
-	M_Offset_AO_Serial_Interrupt_Ack = 0xa1,	/*  write */
-	M_Offset_AO_Serial_Interrupt_Status = 0xa1,	/*  read */
-	M_Offset_AO_Calibration = 0xa3,
-	M_Offset_AO_FIFO_Data = 0xa4,
-	M_Offset_PFI_Filter = 0xb0,
-	M_Offset_RTSI_Filter = 0xb4,
-	M_Offset_SCXI_Legacy_Compatibility = 0xbc,
-	M_Offset_Interrupt_A_Ack = 0x104,	/*  write */
-	M_Offset_AI_Status_1 = 0x104,	/*  read */
-	M_Offset_Interrupt_B_Ack = 0x106,	/*  write */
-	M_Offset_AO_Status_1 = 0x106,	/*  read */
-	M_Offset_AI_Command_2 = 0x108,	/*  write */
-	M_Offset_G01_Status = 0x108,	/*  read */
-	M_Offset_AO_Command_2 = 0x10a,
-	M_Offset_AO_Status_2 = 0x10c,	/*  read */
-	M_Offset_G0_Command = 0x10c,	/*  write */
-	M_Offset_G1_Command = 0x10e,	/*  write */
-	M_Offset_G0_HW_Save = 0x110,
-	M_Offset_G0_HW_Save_High = 0x110,
-	M_Offset_AI_Command_1 = 0x110,
-	M_Offset_G0_HW_Save_Low = 0x112,
-	M_Offset_AO_Command_1 = 0x112,
-	M_Offset_G1_HW_Save = 0x114,
-	M_Offset_G1_HW_Save_High = 0x114,
-	M_Offset_G1_HW_Save_Low = 0x116,
-	M_Offset_AI_Mode_1 = 0x118,
-	M_Offset_G0_Save = 0x118,
-	M_Offset_G0_Save_High = 0x118,
-	M_Offset_AI_Mode_2 = 0x11a,
-	M_Offset_G0_Save_Low = 0x11a,
-	M_Offset_AI_SI_Load_A = 0x11c,
-	M_Offset_G1_Save = 0x11c,
-	M_Offset_G1_Save_High = 0x11c,
-	M_Offset_G1_Save_Low = 0x11e,
-	M_Offset_AI_SI_Load_B = 0x120,	/*  write */
-	M_Offset_AO_UI_Save = 0x120,	/*  read */
-	M_Offset_AI_SC_Load_A = 0x124,	/*  write */
-	M_Offset_AO_BC_Save = 0x124,	/*  read */
-	M_Offset_AI_SC_Load_B = 0x128,	/*  write */
-	M_Offset_AO_UC_Save = 0x128,	/* read */
-	M_Offset_AI_SI2_Load_A = 0x12c,
-	M_Offset_AI_SI2_Load_B = 0x130,
-	M_Offset_G0_Mode = 0x134,
-	M_Offset_G1_Mode = 0x136,	/*  write */
-	M_Offset_Joint_Status_1 = 0x136,	/*  read */
-	M_Offset_G0_Load_A = 0x138,
-	M_Offset_Joint_Status_2 = 0x13a,
-	M_Offset_G0_Load_B = 0x13c,
-	M_Offset_G1_Load_A = 0x140,
-	M_Offset_G1_Load_B = 0x144,
-	M_Offset_G0_Input_Select = 0x148,
-	M_Offset_G1_Input_Select = 0x14a,
-	M_Offset_AO_Mode_1 = 0x14c,
-	M_Offset_AO_Mode_2 = 0x14e,
-	M_Offset_AO_UI_Load_A = 0x150,
-	M_Offset_AO_UI_Load_B = 0x154,
-	M_Offset_AO_BC_Load_A = 0x158,
-	M_Offset_AO_BC_Load_B = 0x15c,
-	M_Offset_AO_UC_Load_A = 0x160,
-	M_Offset_AO_UC_Load_B = 0x164,
-	M_Offset_Clock_and_FOUT = 0x170,
-	M_Offset_IO_Bidirection_Pin = 0x172,
-	M_Offset_RTSI_Trig_Direction = 0x174,
-	M_Offset_Interrupt_Control = 0x176,
-	M_Offset_AI_Output_Control = 0x178,
-	M_Offset_Analog_Trigger_Etc = 0x17a,
-	M_Offset_AI_START_STOP_Select = 0x17c,
-	M_Offset_AI_Trigger_Select = 0x17e,
-	M_Offset_AI_SI_Save = 0x180,	/*  read */
-	M_Offset_AI_DIV_Load_A = 0x180,	/*  write */
-	M_Offset_AI_SC_Save = 0x184,	/*  read */
-	M_Offset_AO_Start_Select = 0x184,	/*  write */
-	M_Offset_AO_Trigger_Select = 0x186,
-	M_Offset_AO_Mode_3 = 0x18c,
-	M_Offset_G0_Autoincrement = 0x188,
-	M_Offset_G1_Autoincrement = 0x18a,
-	M_Offset_Joint_Reset = 0x190,
-	M_Offset_Interrupt_A_Enable = 0x192,
-	M_Offset_Interrupt_B_Enable = 0x196,
-	M_Offset_AI_Personal = 0x19a,
-	M_Offset_AO_Personal = 0x19c,
-	M_Offset_RTSI_Trig_A_Output = 0x19e,
-	M_Offset_RTSI_Trig_B_Output = 0x1a0,
-	M_Offset_RTSI_Shared_MUX = 0x1a2,
-	M_Offset_AO_Output_Control = 0x1ac,
-	M_Offset_AI_Mode_3 = 0x1ae,
-	M_Offset_Configuration_Memory_Clear = 0x1a4,
-	M_Offset_AI_FIFO_Clear = 0x1a6,
-	M_Offset_AO_FIFO_Clear = 0x1a8,
-	M_Offset_G0_Counting_Mode = 0x1b0,
-	M_Offset_G1_Counting_Mode = 0x1b2,
-	M_Offset_G0_Second_Gate = 0x1b4,
-	M_Offset_G1_Second_Gate = 0x1b6,
-	M_Offset_G0_DMA_Config = 0x1b8,	/*  write */
-	M_Offset_G0_DMA_Status = 0x1b8,	/*  read */
-	M_Offset_G1_DMA_Config = 0x1ba,	/*  write */
-	M_Offset_G1_DMA_Status = 0x1ba,	/*  read */
-	M_Offset_G0_MSeries_ABZ = 0x1c0,
-	M_Offset_G1_MSeries_ABZ = 0x1c2,
-	M_Offset_Clock_and_Fout2 = 0x1c4,
-	M_Offset_PLL_Control = 0x1c6,
-	M_Offset_PLL_Status = 0x1c8,
-	M_Offset_PFI_Output_Select_1 = 0x1d0,
-	M_Offset_PFI_Output_Select_2 = 0x1d2,
-	M_Offset_PFI_Output_Select_3 = 0x1d4,
-	M_Offset_PFI_Output_Select_4 = 0x1d6,
-	M_Offset_PFI_Output_Select_5 = 0x1d8,
-	M_Offset_PFI_Output_Select_6 = 0x1da,
-	M_Offset_PFI_DI = 0x1dc,
-	M_Offset_PFI_DO = 0x1de,
-	M_Offset_AI_Config_FIFO_Bypass = 0x218,
-	M_Offset_SCXI_DIO_Enable = 0x21c,
-	M_Offset_CDI_FIFO_Data = 0x220,	/*  read */
-	M_Offset_CDO_FIFO_Data = 0x220,	/*  write */
-	M_Offset_CDIO_Status = 0x224,	/*  read */
-	M_Offset_CDIO_Command = 0x224,	/*  write */
-	M_Offset_CDI_Mode = 0x228,
-	M_Offset_CDO_Mode = 0x22c,
-	M_Offset_CDI_Mask_Enable = 0x230,
-	M_Offset_CDO_Mask_Enable = 0x234,
-};
-static inline int M_Offset_AO_Waveform_Order(int channel)
-{
-	return 0xc2 + 0x4 * channel;
-};
-
-static inline int M_Offset_AO_Config_Bank(int channel)
-{
-	return 0xc3 + 0x4 * channel;
-};
-
-static inline int M_Offset_DAC_Direct_Data(int channel)
-{
-	return 0xc0 + 0x4 * channel;
-}
-
-static inline int M_Offset_Gen_PWM(int channel)
-{
-	return 0x44 + 0x2 * channel;
-}
-
-static inline int M_Offset_Static_AI_Control(int i)
-{
-	int offset[] = {
-		0x64,
-		0x261,
-		0x262,
-		0x263,
-	};
-	if (((unsigned)i) >= ARRAY_SIZE(offset)) {
-		pr_err("%s: invalid channel=%i\n", __func__, i);
-		return offset[0];
-	}
-	return offset[i];
-};
-
-static inline int M_Offset_AO_Reference_Attenuation(int channel)
-{
-	int offset[] = {
-		0x264,
-		0x265,
-		0x266,
-		0x267
-	};
-	if (((unsigned)channel) >= ARRAY_SIZE(offset)) {
-		pr_err("%s: invalid channel=%i\n", __func__, channel);
-		return offset[0];
-	}
-	return offset[channel];
-};
-
-static inline unsigned M_Offset_PFI_Output_Select(unsigned n)
-{
-	if (n < 1 || n > NUM_PFI_OUTPUT_SELECT_REGS) {
-		pr_err("%s: invalid pfi output select register=%i\n",
-		       __func__, n);
-		return M_Offset_PFI_Output_Select_1;
-	}
-	return M_Offset_PFI_Output_Select_1 + (n - 1) * 2;
-}
-
-enum MSeries_AI_Config_FIFO_Data_Bits {
-	MSeries_AI_Config_Channel_Type_Mask = 0x7 << 6,
-	MSeries_AI_Config_Channel_Type_Calibration_Bits = 0x0,
-	MSeries_AI_Config_Channel_Type_Differential_Bits = 0x1 << 6,
-	MSeries_AI_Config_Channel_Type_Common_Ref_Bits = 0x2 << 6,
-	MSeries_AI_Config_Channel_Type_Ground_Ref_Bits = 0x3 << 6,
-	MSeries_AI_Config_Channel_Type_Aux_Bits = 0x5 << 6,
-	MSeries_AI_Config_Channel_Type_Ghost_Bits = 0x7 << 6,
-	MSeries_AI_Config_Polarity_Bit = 0x1000,	/*  0 for 2's complement encoding */
-	MSeries_AI_Config_Dither_Bit = 0x2000,
-	MSeries_AI_Config_Last_Channel_Bit = 0x4000,
-};
-static inline unsigned MSeries_AI_Config_Channel_Bits(unsigned channel)
-{
-	return channel & 0xf;
-}
-
-static inline unsigned MSeries_AI_Config_Bank_Bits(enum ni_reg_type reg_type,
-						   unsigned channel)
-{
-	unsigned bits = channel & 0x30;
-	if (reg_type == ni_reg_622x) {
-		if (channel & 0x40)
-			bits |= 0x400;
-	}
-	return bits;
-}
-
-static inline unsigned MSeries_AI_Config_Gain_Bits(unsigned range)
-{
-	return (range & 0x7) << 9;
-}
-
-enum MSeries_Clock_and_Fout2_Bits {
-	MSeries_PLL_In_Source_Select_RTSI0_Bits = 0xb,
-	MSeries_PLL_In_Source_Select_Star_Trigger_Bits = 0x14,
-	MSeries_PLL_In_Source_Select_RTSI7_Bits = 0x1b,
-	MSeries_PLL_In_Source_Select_PXI_Clock10 = 0x1d,
-	MSeries_PLL_In_Source_Select_Mask = 0x1f,
-	MSeries_Timebase1_Select_Bit = 0x20,	/*  use PLL for timebase 1 */
-	MSeries_Timebase3_Select_Bit = 0x40,	/*  use PLL for timebase 3 */
-	/* use 10MHz instead of 20MHz for RTSI clock frequency.  Appears
-	   to have no effect, at least on pxi-6281, which always uses
-	   20MHz rtsi clock frequency */
-	MSeries_RTSI_10MHz_Bit = 0x80
-};
-static inline unsigned MSeries_PLL_In_Source_Select_RTSI_Bits(unsigned
-							      RTSI_channel)
-{
-	if (RTSI_channel > 7) {
-		pr_err("%s: bug, invalid RTSI_channel=%i\n", __func__,
-		       RTSI_channel);
-		return 0;
-	}
-	if (RTSI_channel == 7)
-		return MSeries_PLL_In_Source_Select_RTSI7_Bits;
-	else
-		return MSeries_PLL_In_Source_Select_RTSI0_Bits + RTSI_channel;
-}
-
-enum MSeries_PLL_Control_Bits {
-	MSeries_PLL_Enable_Bit = 0x1000,
-	MSeries_PLL_VCO_Mode_200_325MHz_Bits = 0x0,
-	MSeries_PLL_VCO_Mode_175_225MHz_Bits = 0x2000,
-	MSeries_PLL_VCO_Mode_100_225MHz_Bits = 0x4000,
-	MSeries_PLL_VCO_Mode_75_150MHz_Bits = 0x6000,
-};
-static inline unsigned MSeries_PLL_Divisor_Bits(unsigned divisor)
-{
-	static const unsigned max_divisor = 0x10;
-	if (divisor < 1 || divisor > max_divisor) {
-		pr_err("%s: bug, invalid divisor=%i\n", __func__, divisor);
-		return 0;
-	}
-	return (divisor & 0xf) << 8;
-}
-
-static inline unsigned MSeries_PLL_Multiplier_Bits(unsigned multiplier)
-{
-	static const unsigned max_multiplier = 0x100;
-	if (multiplier < 1 || multiplier > max_multiplier) {
-		pr_err("%s: bug, invalid multiplier=%i\n", __func__,
-		       multiplier);
-		return 0;
-	}
-	return multiplier & 0xff;
-}
-
-enum MSeries_PLL_Status {
-	MSeries_PLL_Locked_Bit = 0x1
-};
-
-enum MSeries_AI_Config_FIFO_Bypass_Bits {
-	MSeries_AI_Bypass_Channel_Mask = 0x7,
-	MSeries_AI_Bypass_Bank_Mask = 0x78,
-	MSeries_AI_Bypass_Cal_Sel_Pos_Mask = 0x380,
-	MSeries_AI_Bypass_Cal_Sel_Neg_Mask = 0x1c00,
-	MSeries_AI_Bypass_Mode_Mux_Mask = 0x6000,
-	MSeries_AO_Bypass_AO_Cal_Sel_Mask = 0x38000,
-	MSeries_AI_Bypass_Gain_Mask = 0x1c0000,
-	MSeries_AI_Bypass_Dither_Bit = 0x200000,
-	MSeries_AI_Bypass_Polarity_Bit = 0x400000,	/*  0 for 2's complement encoding */
-	MSeries_AI_Bypass_Config_FIFO_Bit = 0x80000000
-};
-static inline unsigned MSeries_AI_Bypass_Cal_Sel_Pos_Bits(int
-							  calibration_source)
-{
-	return (calibration_source << 7) & MSeries_AI_Bypass_Cal_Sel_Pos_Mask;
-}
-
-static inline unsigned MSeries_AI_Bypass_Cal_Sel_Neg_Bits(int
-							  calibration_source)
-{
-	return (calibration_source << 10) & MSeries_AI_Bypass_Cal_Sel_Pos_Mask;
-}
-
-static inline unsigned MSeries_AI_Bypass_Gain_Bits(int gain)
-{
-	return (gain << 18) & MSeries_AI_Bypass_Gain_Mask;
-}
-
-enum MSeries_AO_Config_Bank_Bits {
-	MSeries_AO_DAC_Offset_Select_Mask = 0x7,
-	MSeries_AO_DAC_Offset_0V_Bits = 0x0,
-	MSeries_AO_DAC_Offset_5V_Bits = 0x1,
-	MSeries_AO_DAC_Reference_Mask = 0x38,
-	MSeries_AO_DAC_Reference_10V_Internal_Bits = 0x0,
-	MSeries_AO_DAC_Reference_5V_Internal_Bits = 0x8,
-	MSeries_AO_Update_Timed_Bit = 0x40,
-	MSeries_AO_Bipolar_Bit = 0x80	/*  turns on 2's complement encoding */
-};
-
-enum MSeries_AO_Reference_Attenuation_Bits {
-	MSeries_Attenuate_x5_Bit = 0x1
-};
-
-static inline unsigned MSeries_Cal_PWM_High_Time_Bits(unsigned count)
-{
-	return (count << 16) & 0xffff0000;
-}
-
-static inline unsigned MSeries_Cal_PWM_Low_Time_Bits(unsigned count)
-{
-	return count & 0xffff;
-}
-
-static inline unsigned MSeries_PFI_Output_Select_Mask(unsigned channel)
-{
-	return 0x1f << (channel % 3) * 5;
-};
-
-static inline unsigned MSeries_PFI_Output_Select_Bits(unsigned channel,
-						      unsigned source)
-{
-	return (source & 0x1f) << ((channel % 3) * 5);
-};
-
-/* inverse to MSeries_PFI_Output_Select_Bits */
-static inline unsigned MSeries_PFI_Output_Select_Source(unsigned channel,
-							unsigned bits)
-{
-	return (bits >> ((channel % 3) * 5)) & 0x1f;
-};
-
-static inline unsigned MSeries_PFI_Filter_Select_Mask(unsigned channel)
-{
-	return 0x3 << (channel * 2);
-}
-
-static inline unsigned MSeries_PFI_Filter_Select_Bits(unsigned channel,
-						      unsigned filter)
-{
-	return (filter << (channel *
-			   2)) & MSeries_PFI_Filter_Select_Mask(channel);
-}
-
-enum CDIO_DMA_Select_Bits {
-	CDI_DMA_Select_Shift = 0,
-	CDI_DMA_Select_Mask = 0xf,
-	CDO_DMA_Select_Shift = 4,
-	CDO_DMA_Select_Mask = 0xf << CDO_DMA_Select_Shift
-};
-
-enum CDIO_Status_Bits {
-	CDO_FIFO_Empty_Bit = 0x1,
-	CDO_FIFO_Full_Bit = 0x2,
-	CDO_FIFO_Request_Bit = 0x4,
-	CDO_Overrun_Bit = 0x8,
-	CDO_Underflow_Bit = 0x10,
-	CDI_FIFO_Empty_Bit = 0x10000,
-	CDI_FIFO_Full_Bit = 0x20000,
-	CDI_FIFO_Request_Bit = 0x40000,
-	CDI_Overrun_Bit = 0x80000,
-	CDI_Overflow_Bit = 0x100000
-};
-
-enum CDIO_Command_Bits {
-	CDO_Disarm_Bit = 0x1,
-	CDO_Arm_Bit = 0x2,
-	CDI_Disarm_Bit = 0x4,
-	CDI_Arm_Bit = 0x8,
-	CDO_Reset_Bit = 0x10,
-	CDI_Reset_Bit = 0x20,
-	CDO_Error_Interrupt_Enable_Set_Bit = 0x40,
-	CDO_Error_Interrupt_Enable_Clear_Bit = 0x80,
-	CDI_Error_Interrupt_Enable_Set_Bit = 0x100,
-	CDI_Error_Interrupt_Enable_Clear_Bit = 0x200,
-	CDO_FIFO_Request_Interrupt_Enable_Set_Bit = 0x400,
-	CDO_FIFO_Request_Interrupt_Enable_Clear_Bit = 0x800,
-	CDI_FIFO_Request_Interrupt_Enable_Set_Bit = 0x1000,
-	CDI_FIFO_Request_Interrupt_Enable_Clear_Bit = 0x2000,
-	CDO_Error_Interrupt_Confirm_Bit = 0x4000,
-	CDI_Error_Interrupt_Confirm_Bit = 0x8000,
-	CDO_Empty_FIFO_Interrupt_Enable_Set_Bit = 0x10000,
-	CDO_Empty_FIFO_Interrupt_Enable_Clear_Bit = 0x20000,
-	CDO_SW_Update_Bit = 0x80000,
-	CDI_SW_Update_Bit = 0x100000
-};
-
-enum CDI_Mode_Bits {
-	CDI_Sample_Source_Select_Mask = 0x3f,
-	CDI_Halt_On_Error_Bit = 0x200,
-	CDI_Polarity_Bit = 0x400,	/*  sample clock on falling edge */
-	CDI_FIFO_Mode_Bit = 0x800,	/*  set for half full mode, clear for not empty mode */
-	CDI_Data_Lane_Mask = 0x3000,	/*  data lanes specify which dio channels map to byte or word accesses to the dio fifos */
-	CDI_Data_Lane_0_15_Bits = 0x0,
-	CDI_Data_Lane_16_31_Bits = 0x1000,
-	CDI_Data_Lane_0_7_Bits = 0x0,
-	CDI_Data_Lane_8_15_Bits = 0x1000,
-	CDI_Data_Lane_16_23_Bits = 0x2000,
-	CDI_Data_Lane_24_31_Bits = 0x3000
-};
-
-enum CDO_Mode_Bits {
-	CDO_Sample_Source_Select_Mask = 0x3f,
-	CDO_Retransmit_Bit = 0x100,
-	CDO_Halt_On_Error_Bit = 0x200,
-	CDO_Polarity_Bit = 0x400,	/*  sample clock on falling edge */
-	CDO_FIFO_Mode_Bit = 0x800,	/*  set for half full mode, clear for not full mode */
-	CDO_Data_Lane_Mask = 0x3000,	/*  data lanes specify which dio channels map to byte or word accesses to the dio fifos */
-	CDO_Data_Lane_0_15_Bits = 0x0,
-	CDO_Data_Lane_16_31_Bits = 0x1000,
-	CDO_Data_Lane_0_7_Bits = 0x0,
-	CDO_Data_Lane_8_15_Bits = 0x1000,
-	CDO_Data_Lane_16_23_Bits = 0x2000,
-	CDO_Data_Lane_24_31_Bits = 0x3000
-};
-
-enum Interrupt_C_Enable_Bits {
-	Interrupt_Group_C_Enable_Bit = 0x1
-};
-
-enum Interrupt_C_Status_Bits {
-	Interrupt_Group_C_Status_Bit = 0x1
-};
-
-#define M_SERIES_EEPROM_SIZE 1024
-
 struct ni_board_struct {
 	const char *name;
 	int device_id;
@@ -1405,9 +967,13 @@ struct ni_board_struct {
 	enum caldac_enum caldac[3];
 };
 
-#define MAX_N_CALDACS	34
-#define MAX_N_AO_CHAN	8
-#define NUM_GPCT	2
+#define MAX_N_CALDACS			34
+#define MAX_N_AO_CHAN			8
+#define NUM_GPCT			2
+
+#define NUM_PFI_OUTPUT_SELECT_REGS	6
+
+#define M_SERIES_EEPROM_SIZE		1024
 
 struct ni_private {
 	unsigned short dio_output;
@@ -1415,8 +981,11 @@ struct ni_private {
 	int aimode;
 	unsigned int ai_calib_source;
 	unsigned int ai_calib_source_enabled;
+	/* protects access to windowed registers */
 	spinlock_t window_lock;
+	/* protects interrupt/dma register access */
 	spinlock_t soft_reg_copy_lock;
+	/* protects mite DMA channel request/release */
 	spinlock_t mite_channel_lock;
 
 	int changain_state;
@@ -1488,4 +1057,6 @@ struct ni_private {
 	unsigned int is_6713:1;
 };
 
+static const struct comedi_lrange range_ni_E_ao_ext;
+
 #endif /* _COMEDI_NI_STC_H */
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 03a3fd6cd918..48f6cdf440b9 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -507,11 +507,11 @@ static const struct pcl812_board boardtypes[] = {
 
 struct pcl812_private {
 	struct comedi_isadma *dma;
-	unsigned char range_correction;	/*  =1 we must add 1 to range number */
+	unsigned char range_correction;	/* =1 we must add 1 to range number */
 	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 max_812_ai_mode0_rangewait;	/*  setling time for gain */
+	unsigned char mode_reg_int; /* stored INT number for some cards */
+	unsigned int ai_poll_ptr; /* how many samples transfer poll */
+	unsigned int max_812_ai_mode0_rangewait; /* settling time for gain */
 	unsigned int use_diff:1;
 	unsigned int use_mpc508:1;
 	unsigned int use_ext_trg:1;
@@ -1155,7 +1155,7 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 	/* we need an IRQ to do DMA on channel 3 or 1 */
 	if (dev->irq && board->has_dma)
-		 pcl812_alloc_dma(dev, it->options[2]);
+		pcl812_alloc_dma(dev, it->options[2]);
 
 	/* differential analog inputs? */
 	switch (board->board_type) {
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index 1ccb2f19f4be..781b321587dc 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -323,7 +323,7 @@ static int check_channel_list(struct comedi_device *dev,
 
 		/*  check whole chanlist */
 		for (i = 0, segpos = 0; i < chanlen; i++) {
-			    if (chanlist[i] != chansegment[i % seglen]) {
+			if (chanlist[i] != chansegment[i % seglen]) {
 				dev_dbg(dev->class_dev,
 					"bad channel or range number! chanlist[%i]=%d,%d,%d and not %d,%d,%d!\n",
 					i, CR_CHAN(chansegment[i]),
diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c
index 304ebff119ee..83da162deb52 100644
--- a/drivers/staging/comedi/drivers/serial2002.c
+++ b/drivers/staging/comedi/drivers/serial2002.c
@@ -373,7 +373,7 @@ static int serial2002_setup_subdevice(struct comedi_subdevice *s,
 		if (cfg[j].kind == kind) {
 			if (mapping)
 				mapping[chan] = j;
-			if (range) {
+			if (range && range_table_list) {
 				range[j].length = 1;
 				range[j].range.min = cfg[j].min;
 				range[j].range.max = cfg[j].max;
diff --git a/drivers/staging/comedi/drivers/vmk80xx.c b/drivers/staging/comedi/drivers/vmk80xx.c
index a0906685e27f..3af075aa3946 100644
--- a/drivers/staging/comedi/drivers/vmk80xx.c
+++ b/drivers/staging/comedi/drivers/vmk80xx.c
@@ -700,7 +700,7 @@ static int vmk80xx_alloc_usb_buffers(struct comedi_device *dev)
 
 static int vmk80xx_init_subdevices(struct comedi_device *dev)
 {
-	const struct vmk80xx_board *boardinfo = dev->board_ptr;
+	const struct vmk80xx_board *board = dev->board_ptr;
 	struct vmk80xx_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	int n_subd;
@@ -722,18 +722,18 @@ static int vmk80xx_init_subdevices(struct comedi_device *dev)
 	s = &dev->subdevices[0];
 	s->type		= COMEDI_SUBD_AI;
 	s->subdev_flags	= SDF_READABLE | SDF_GROUND;
-	s->n_chan	= boardinfo->ai_nchans;
-	s->maxdata	= boardinfo->ai_maxdata;
-	s->range_table	= boardinfo->range;
+	s->n_chan	= board->ai_nchans;
+	s->maxdata	= board->ai_maxdata;
+	s->range_table	= board->range;
 	s->insn_read	= vmk80xx_ai_insn_read;
 
 	/* Analog output subdevice */
 	s = &dev->subdevices[1];
 	s->type		= COMEDI_SUBD_AO;
 	s->subdev_flags	= SDF_WRITABLE | SDF_GROUND;
-	s->n_chan	= boardinfo->ao_nchans;
+	s->n_chan	= board->ao_nchans;
 	s->maxdata	= 0x00ff;
-	s->range_table	= boardinfo->range;
+	s->range_table	= board->range;
 	s->insn_write	= vmk80xx_ao_insn_write;
 	if (devpriv->model == VMK8061_MODEL) {
 		s->subdev_flags	|= SDF_READABLE;
@@ -744,7 +744,7 @@ static int vmk80xx_init_subdevices(struct comedi_device *dev)
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DI;
 	s->subdev_flags	= SDF_READABLE;
-	s->n_chan	= boardinfo->di_nchans;
+	s->n_chan	= board->di_nchans;
 	s->maxdata	= 1;
 	s->range_table	= &range_digital;
 	s->insn_bits	= vmk80xx_di_insn_bits;
@@ -763,7 +763,7 @@ static int vmk80xx_init_subdevices(struct comedi_device *dev)
 	s->type		= COMEDI_SUBD_COUNTER;
 	s->subdev_flags	= SDF_READABLE;
 	s->n_chan	= 2;
-	s->maxdata	= boardinfo->cnt_maxdata;
+	s->maxdata	= board->cnt_maxdata;
 	s->insn_read	= vmk80xx_cnt_insn_read;
 	s->insn_config	= vmk80xx_cnt_insn_config;
 	if (devpriv->model == VMK8055_MODEL) {
@@ -776,8 +776,8 @@ static int vmk80xx_init_subdevices(struct comedi_device *dev)
 		s = &dev->subdevices[5];
 		s->type		= COMEDI_SUBD_PWM;
 		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
-		s->n_chan	= boardinfo->pwm_nchans;
-		s->maxdata	= boardinfo->pwm_maxdata;
+		s->n_chan	= board->pwm_nchans;
+		s->maxdata	= board->pwm_maxdata;
 		s->insn_read	= vmk80xx_pwm_insn_read;
 		s->insn_write	= vmk80xx_pwm_insn_write;
 	}
@@ -791,19 +791,22 @@ static int vmk80xx_auto_attach(struct comedi_device *dev,
 			       unsigned long context)
 {
 	struct usb_interface *intf = comedi_to_usb_interface(dev);
-	const struct vmk80xx_board *boardinfo;
+	const struct vmk80xx_board *board = NULL;
 	struct vmk80xx_private *devpriv;
 	int ret;
 
-	boardinfo = &vmk80xx_boardinfo[context];
-	dev->board_ptr = boardinfo;
-	dev->board_name = boardinfo->name;
+	if (context < ARRAY_SIZE(vmk80xx_boardinfo))
+		board = &vmk80xx_boardinfo[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
 		return -ENOMEM;
 
-	devpriv->model = boardinfo->model;
+	devpriv->model = board->model;
 
 	ret = vmk80xx_find_usb_endpoints(dev);
 	if (ret)
diff --git a/drivers/staging/dgap/dgap.c b/drivers/staging/dgap/dgap.c
index 6766d5a91a90..26b0446d943a 100644
--- a/drivers/staging/dgap/dgap.c
+++ b/drivers/staging/dgap/dgap.c
@@ -290,8 +290,7 @@ static struct toklist dgap_tlist[] = {
 
 /*
  * 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.
+ * the group, and returns that position.
  */
 static char *dgap_sindex(char *string, char *group)
 {
@@ -300,23 +299,11 @@ static char *dgap_sindex(char *string, char *group)
 	if (!string || !group)
 		return NULL;
 
-	if (*group == '^') {
-		group++;
-		for (; *string; string++) {
-			for (ptr = group; *ptr; ptr++) {
-				if (*ptr == *string)
-					break;
-			}
-			if (*ptr == '\0')
+	for (; *string; string++) {
+		for (ptr = group; *ptr; ptr++) {
+			if (*ptr == *string)
 				return string;
 		}
-	} else {
-		for (; *string; string++) {
-			for (ptr = group; *ptr; ptr++) {
-				if (*ptr == *string)
-					return string;
-			}
-		}
 	}
 
 	return NULL;
@@ -2658,7 +2645,7 @@ static void dgap_cmdw_ext(struct channel_t *ch, u16 cmd, u16 word, uint ncmds)
  *      dgap_wmove - Write data to FEP buffer.
  *
  *              ch      - Pointer to channel structure.
- *              buf     - Poiter to characters to be moved.
+ *              buf     - Pointer to characters to be moved.
  *              cnt     - Number of characters to move.
  *
  *=======================================================================*/
@@ -6987,9 +6974,62 @@ cleanup_brd:
 	return rc;
 }
 
+/*
+ * dgap_cleanup_board()
+ *
+ * Free all the memory associated with a board
+ */
+static void dgap_cleanup_board(struct board_t *brd)
+{
+	unsigned int i;
+
+	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
+		return;
+
+	dgap_free_irq(brd);
+
+	tasklet_kill(&brd->helper_tasklet);
+
+	dgap_unmap(brd);
+
+	/* Free all allocated channels structs */
+	for (i = 0; i < MAXPORTS ; i++)
+		kfree(brd->channels[i]);
+
+	kfree(brd->flipbuf);
+	kfree(brd->flipflagbuf);
+
+	dgap_board[brd->boardnum] = NULL;
+
+	kfree(brd);
+}
+
 static void dgap_remove_one(struct pci_dev *dev)
 {
-	/* Do Nothing */
+	unsigned int i;
+	ulong lock_flags;
+	struct pci_driver *drv = to_pci_driver(dev->dev.driver);
+
+	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);
+
+	dgap_remove_driver_sysfiles(drv);
+
+	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]);
+	}
+
+	dgap_cleanup_nodes();
 }
 
 static struct pci_driver dgap_driver = {
@@ -7071,37 +7111,6 @@ static void dgap_stop(void)
 	unregister_chrdev(DIGI_DGAP_MAJOR, "dgap");
 }
 
-/*
- * dgap_cleanup_board()
- *
- * Free all the memory associated with a board
- */
-static void dgap_cleanup_board(struct board_t *brd)
-{
-	unsigned int i;
-
-	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
-		return;
-
-	dgap_free_irq(brd);
-
-	tasklet_kill(&brd->helper_tasklet);
-
-	dgap_unmap(brd);
-
-	/* Free all allocated channels structs */
-	for (i = 0; i < MAXPORTS ; i++)
-		kfree(brd->channels[i]);
-
-	kfree(brd->flipbuf);
-	kfree(brd->flipflagbuf);
-
-	dgap_board[brd->boardnum] = NULL;
-
-	kfree(brd);
-}
-
-
 /************************************************************************
  *
  * Driver load/unload functions
@@ -7150,30 +7159,6 @@ err_stop:
  */
 static void dgap_cleanup_module(void)
 {
-	unsigned 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);
-
-	dgap_remove_driver_sysfiles(&dgap_driver);
-
-	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]);
-	}
-
-	dgap_cleanup_nodes();
-
 	if (dgap_numboards)
 		pci_unregister_driver(&dgap_driver);
 }
diff --git a/drivers/staging/dgap/dgap.h b/drivers/staging/dgap/dgap.h
index a2e5b26c673a..e707ed5fe949 100644
--- a/drivers/staging/dgap/dgap.h
+++ b/drivers/staging/dgap/dgap.h
@@ -172,7 +172,7 @@
 /*
  * Define a local default termios struct. All ports will be created
  * with this termios initially.  This is the same structure that is defined
- * as the default in tty_io.c with the same settings overriden as in serial.c
+ * as the default in tty_io.c with the same settings overridden as in serial.c
  *
  * In short, this should match the internal serial ports' defaults.
  */
diff --git a/drivers/staging/dgnc/TODO b/drivers/staging/dgnc/TODO
index 2b2c6ea03c61..0e0825bd70ae 100644
--- a/drivers/staging/dgnc/TODO
+++ b/drivers/staging/dgnc/TODO
@@ -1,9 +1,9 @@
 * checkpatch fixes
-* remove unecessary comments
-* remove unecessary error messages. Example kzalloc() has its
+* remove unnecessary comments
+* remove unnecessary error messages. Example kzalloc() has its
   own error message. Adding an extra one is useless.
 * use goto statements for error handling when appropriate
-* there is a lot of unecessary code in the driver. It was
+* there is a lot of unnecessary code in the driver. It was
   originally a standalone driver. Remove uneeded code.
 
 Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
diff --git a/drivers/staging/dgnc/dgnc_cls.c b/drivers/staging/dgnc/dgnc_cls.c
index e3564d278d91..a629a78964ce 100644
--- a/drivers/staging/dgnc/dgnc_cls.c
+++ b/drivers/staging/dgnc/dgnc_cls.c
@@ -379,7 +379,7 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 		return;
 
 	ch = brd->channels[port];
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
 	/* Here we try to figure out what caused the interrupt to happen */
@@ -714,8 +714,6 @@ static void cls_tasklet(unsigned long data)
 		/* Loop on each port */
 		for (i = 0; i < ports; i++) {
 			ch = bd->channels[i];
-			if (!ch)
-				continue;
 
 			/*
 			 * NOTE: Remember you CANNOT hold any channel
diff --git a/drivers/staging/dgnc/dgnc_driver.c b/drivers/staging/dgnc/dgnc_driver.c
index 805dc617e3a7..7546aff65002 100644
--- a/drivers/staging/dgnc/dgnc_driver.c
+++ b/drivers/staging/dgnc/dgnc_driver.c
@@ -355,7 +355,6 @@ static void dgnc_cleanup_board(struct dgnc_board *brd)
 		}
 	}
 
-	kfree(brd->flipbuf);
 
 	dgnc_Board[brd->boardnum] = NULL;
 
@@ -581,14 +580,6 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 	brd->msgbuf_head = NULL;
 	spin_unlock_irqrestore(&dgnc_global_lock, flags);
 
-	/*
-	 * allocate flip buffer for board.
-	 *
-	 * Okay to malloc with GFP_KERNEL, we are not at interrupt
-	 * context, and there are no locks held.
-	 */
-	brd->flipbuf = kzalloc(MYFLIPLEN, GFP_KERNEL);
-
 	wake_up_interruptible(&brd->state_wait);
 
 	return 0;
diff --git a/drivers/staging/dgnc/dgnc_driver.h b/drivers/staging/dgnc/dgnc_driver.h
index f77fed57b873..d04671fa4b75 100644
--- a/drivers/staging/dgnc/dgnc_driver.h
+++ b/drivers/staging/dgnc/dgnc_driver.h
@@ -66,14 +66,13 @@
 
 /* 4 extra for alignment play space */
 #define WRITEBUFLEN		((4096) + 4)
-#define MYFLIPLEN		N_TTY_BUF_SIZE
 
 #define dgnc_jiffies_from_ms(a) (((a) * HZ) / 1000)
 
 /*
  * Define a local default termios struct. All ports will be created
  * with this termios initially.  This is the same structure that is defined
- * as the default in tty_io.c with the same settings overriden as in serial.c
+ * as the default in tty_io.c with the same settings overridden as in serial.c
  *
  * In short, this should match the internal serial ports' defaults.
  */
@@ -212,8 +211,6 @@ struct dgnc_board {
 
 	uint		TtyRefCnt;
 
-	char		*flipbuf;	/* Our flip buffer, alloced if board is found */
-
 	u16		dpatype;	/* The board "type", as defined by DPA */
 	u16		dpastatus;	/* The board "status", as defined by DPA */
 
@@ -288,7 +285,6 @@ struct un_t {
 #define CH_TX_FIFO_LWM  0x0800		/* TX Fifo is below Low Water	*/
 #define CH_BREAK_SENDING 0x1000		/* Break is being sent		*/
 #define CH_LOOPBACK 0x2000		/* Channel is in lookback mode	*/
-#define CH_FLIPBUF_IN_USE 0x4000	/* Channel's flipbuf is in use	*/
 #define CH_BAUD0	0x08000		/* Used for checking B0 transitions */
 #define CH_FORCED_STOP  0x20000		/* Output is forcibly stopped	*/
 #define CH_FORCED_STOPI 0x40000		/* Input is forcibly stopped	*/
diff --git a/drivers/staging/dgnc/dgnc_neo.c b/drivers/staging/dgnc/dgnc_neo.c
index f5a4d365115f..900e3ae55a38 100644
--- a/drivers/staging/dgnc/dgnc_neo.c
+++ b/drivers/staging/dgnc/dgnc_neo.c
@@ -395,7 +395,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 		return;
 
 	ch = brd->channels[port];
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
 	/* Here we try to figure out what caused the interrupt to happen */
@@ -1203,7 +1203,7 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		memcpy_fromio(ch->ch_rqueue + head, &ch->ch_neo_uart->txrxburst, n);
 
 		/*
-		 * Since RX_FIFO_DATA_ERROR was 0, we are guarenteed
+		 * Since RX_FIFO_DATA_ERROR was 0, we are guaranteed
 		 * that all the data currently in the FIFO is free of
 		 * breaks and parity/frame/orun errors.
 		 */
diff --git a/drivers/staging/dgnc/dgnc_sysfs.c b/drivers/staging/dgnc/dgnc_sysfs.c
index 65551d190515..44db8703eba4 100644
--- a/drivers/staging/dgnc/dgnc_sysfs.c
+++ b/drivers/staging/dgnc/dgnc_sysfs.c
@@ -53,7 +53,8 @@ static ssize_t dgnc_driver_pollrate_show(struct device_driver *ddp, char *buf)
 	return snprintf(buf, PAGE_SIZE, "%dms\n", dgnc_poll_tick);
 }
 
-static ssize_t dgnc_driver_pollrate_store(struct device_driver *ddp, const char *buf, size_t count)
+static ssize_t dgnc_driver_pollrate_store(struct device_driver *ddp,
+					  const char *buf, size_t count)
 {
 	int ret;
 
@@ -62,7 +63,8 @@ static ssize_t dgnc_driver_pollrate_store(struct device_driver *ddp, const char
 		return -EINVAL;
 	return count;
 }
-static DRIVER_ATTR(pollrate, (S_IRUSR | S_IWUSR), dgnc_driver_pollrate_show, dgnc_driver_pollrate_store);
+static DRIVER_ATTR(pollrate, (S_IRUSR | S_IWUSR), dgnc_driver_pollrate_show,
+		   dgnc_driver_pollrate_store);
 
 
 void dgnc_create_driver_sysfiles(struct pci_driver *dgnc_driver)
@@ -104,7 +106,8 @@ void dgnc_remove_driver_sysfiles(struct pci_driver *dgnc_driver)
 
 
 
-static ssize_t dgnc_vpd_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_vpd_show(struct device *p, struct device_attribute *attr,
+			     char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -112,7 +115,8 @@ static ssize_t dgnc_vpd_show(struct device *p, struct device_attribute *attr, ch
 
 	DGNC_VERIFY_BOARD(p, bd);
 
-	count += sprintf(buf + count, "\n      0  1  2  3  4  5  6  7  8  9  A  B  C  D  E  F");
+	count += sprintf(buf + count,
+		"\n      0  1  2  3  4  5  6  7  8  9  A  B  C  D  E  F");
 	for (i = 0; i < 0x40 * 2; i++) {
 		if (!(i % 16))
 			count += sprintf(buf + count, "\n%04X ", i * 2);
@@ -124,7 +128,8 @@ static ssize_t dgnc_vpd_show(struct device *p, struct device_attribute *attr, ch
 }
 static DEVICE_ATTR(vpd, S_IRUSR, dgnc_vpd_show, NULL);
 
-static ssize_t dgnc_serial_number_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_serial_number_show(struct device *p,
+				       struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -141,7 +146,8 @@ static ssize_t dgnc_serial_number_show(struct device *p, struct device_attribute
 static DEVICE_ATTR(serial_number, S_IRUSR, dgnc_serial_number_show, NULL);
 
 
-static ssize_t dgnc_ports_state_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_state_show(struct device *p,
+				     struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -159,7 +165,8 @@ static ssize_t dgnc_ports_state_show(struct device *p, struct device_attribute *
 static DEVICE_ATTR(ports_state, S_IRUSR, dgnc_ports_state_show, NULL);
 
 
-static ssize_t dgnc_ports_baud_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_baud_show(struct device *p,
+				    struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -169,14 +176,17 @@ static ssize_t dgnc_ports_baud_show(struct device *p, struct device_attribute *a
 
 	for (i = 0; i < bd->nasync; i++) {
 		count +=  snprintf(buf + count, PAGE_SIZE - count,
-			"%d %d\n", bd->channels[i]->ch_portnum, bd->channels[i]->ch_old_baud);
+			"%d %d\n", bd->channels[i]->ch_portnum,
+			bd->channels[i]->ch_old_baud);
 	}
 	return count;
 }
 static DEVICE_ATTR(ports_baud, S_IRUSR, dgnc_ports_baud_show, NULL);
 
 
-static ssize_t dgnc_ports_msignals_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_msignals_show(struct device *p,
+					struct device_attribute *attr,
+					char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -187,7 +197,8 @@ static ssize_t dgnc_ports_msignals_show(struct device *p, struct device_attribut
 	for (i = 0; i < bd->nasync; i++) {
 		if (bd->channels[i]->ch_open_count) {
 			count += snprintf(buf + count, PAGE_SIZE - count,
-				"%d %s %s %s %s %s %s\n", bd->channels[i]->ch_portnum,
+				"%d %s %s %s %s %s %s\n",
+				bd->channels[i]->ch_portnum,
 				(bd->channels[i]->ch_mostat & UART_MCR_RTS) ? "RTS" : "",
 				(bd->channels[i]->ch_mistat & UART_MSR_CTS) ? "CTS" : "",
 				(bd->channels[i]->ch_mostat & UART_MCR_DTR) ? "DTR" : "",
@@ -204,7 +215,8 @@ static ssize_t dgnc_ports_msignals_show(struct device *p, struct device_attribut
 static DEVICE_ATTR(ports_msignals, S_IRUSR, dgnc_ports_msignals_show, NULL);
 
 
-static ssize_t dgnc_ports_iflag_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_iflag_show(struct device *p,
+				     struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -214,14 +226,16 @@ static ssize_t dgnc_ports_iflag_show(struct device *p, struct device_attribute *
 
 	for (i = 0; i < bd->nasync; i++) {
 		count += snprintf(buf + count, PAGE_SIZE - count, "%d %x\n",
-			bd->channels[i]->ch_portnum, bd->channels[i]->ch_c_iflag);
+			bd->channels[i]->ch_portnum,
+			bd->channels[i]->ch_c_iflag);
 	}
 	return count;
 }
 static DEVICE_ATTR(ports_iflag, S_IRUSR, dgnc_ports_iflag_show, NULL);
 
 
-static ssize_t dgnc_ports_cflag_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_cflag_show(struct device *p,
+				     struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -231,14 +245,16 @@ static ssize_t dgnc_ports_cflag_show(struct device *p, struct device_attribute *
 
 	for (i = 0; i < bd->nasync; i++) {
 		count += snprintf(buf + count, PAGE_SIZE - count, "%d %x\n",
-			bd->channels[i]->ch_portnum, bd->channels[i]->ch_c_cflag);
+			bd->channels[i]->ch_portnum,
+			bd->channels[i]->ch_c_cflag);
 	}
 	return count;
 }
 static DEVICE_ATTR(ports_cflag, S_IRUSR, dgnc_ports_cflag_show, NULL);
 
 
-static ssize_t dgnc_ports_oflag_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_oflag_show(struct device *p,
+				     struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -248,14 +264,16 @@ static ssize_t dgnc_ports_oflag_show(struct device *p, struct device_attribute *
 
 	for (i = 0; i < bd->nasync; i++) {
 		count += snprintf(buf + count, PAGE_SIZE - count, "%d %x\n",
-			bd->channels[i]->ch_portnum, bd->channels[i]->ch_c_oflag);
+			bd->channels[i]->ch_portnum,
+			bd->channels[i]->ch_c_oflag);
 	}
 	return count;
 }
 static DEVICE_ATTR(ports_oflag, S_IRUSR, dgnc_ports_oflag_show, NULL);
 
 
-static ssize_t dgnc_ports_lflag_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_lflag_show(struct device *p,
+				     struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -265,14 +283,17 @@ static ssize_t dgnc_ports_lflag_show(struct device *p, struct device_attribute *
 
 	for (i = 0; i < bd->nasync; i++) {
 		count += snprintf(buf + count, PAGE_SIZE - count, "%d %x\n",
-			bd->channels[i]->ch_portnum, bd->channels[i]->ch_c_lflag);
+			bd->channels[i]->ch_portnum,
+			bd->channels[i]->ch_c_lflag);
 	}
 	return count;
 }
 static DEVICE_ATTR(ports_lflag, S_IRUSR, dgnc_ports_lflag_show, NULL);
 
 
-static ssize_t dgnc_ports_digi_flag_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_digi_flag_show(struct device *p,
+					 struct device_attribute *attr,
+					 char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -282,14 +303,16 @@ static ssize_t dgnc_ports_digi_flag_show(struct device *p, struct device_attribu
 
 	for (i = 0; i < bd->nasync; i++) {
 		count += snprintf(buf + count, PAGE_SIZE - count, "%d %x\n",
-			bd->channels[i]->ch_portnum, bd->channels[i]->ch_digi.digi_flags);
+			bd->channels[i]->ch_portnum,
+			bd->channels[i]->ch_digi.digi_flags);
 	}
 	return count;
 }
 static DEVICE_ATTR(ports_digi_flag, S_IRUSR, dgnc_ports_digi_flag_show, NULL);
 
 
-static ssize_t dgnc_ports_rxcount_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_rxcount_show(struct device *p,
+				       struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -299,14 +322,16 @@ static ssize_t dgnc_ports_rxcount_show(struct device *p, struct device_attribute
 
 	for (i = 0; i < bd->nasync; i++) {
 		count += snprintf(buf + count, PAGE_SIZE - count, "%d %ld\n",
-			bd->channels[i]->ch_portnum, bd->channels[i]->ch_rxcount);
+			bd->channels[i]->ch_portnum,
+			bd->channels[i]->ch_rxcount);
 	}
 	return count;
 }
 static DEVICE_ATTR(ports_rxcount, S_IRUSR, dgnc_ports_rxcount_show, NULL);
 
 
-static ssize_t dgnc_ports_txcount_show(struct device *p, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_ports_txcount_show(struct device *p,
+				       struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	int count = 0;
@@ -316,7 +341,8 @@ static ssize_t dgnc_ports_txcount_show(struct device *p, struct device_attribute
 
 	for (i = 0; i < bd->nasync; i++) {
 		count += snprintf(buf + count, PAGE_SIZE - count, "%d %ld\n",
-			bd->channels[i]->ch_portnum, bd->channels[i]->ch_txcount);
+			bd->channels[i]->ch_portnum,
+			bd->channels[i]->ch_txcount);
 	}
 	return count;
 }
@@ -366,7 +392,8 @@ void dgnc_remove_ports_sysfiles(struct dgnc_board *bd)
 }
 
 
-static ssize_t dgnc_tty_state_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_state_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -386,12 +413,14 @@ static ssize_t dgnc_tty_state_show(struct device *d, struct device_attribute *at
 	if (bd->state != BOARD_READY)
 		return 0;
 
-	return snprintf(buf, PAGE_SIZE, "%s", un->un_open_count ? "Open" : "Closed");
+	return snprintf(buf, PAGE_SIZE, "%s",
+			un->un_open_count ? "Open" : "Closed");
 }
 static DEVICE_ATTR(state, S_IRUSR, dgnc_tty_state_show, NULL);
 
 
-static ssize_t dgnc_tty_baud_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_baud_show(struct device *d,
+				  struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -416,7 +445,8 @@ static ssize_t dgnc_tty_baud_show(struct device *d, struct device_attribute *att
 static DEVICE_ATTR(baud, S_IRUSR, dgnc_tty_baud_show, NULL);
 
 
-static ssize_t dgnc_tty_msignals_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_msignals_show(struct device *d,
+				      struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -450,7 +480,8 @@ static ssize_t dgnc_tty_msignals_show(struct device *d, struct device_attribute
 static DEVICE_ATTR(msignals, S_IRUSR, dgnc_tty_msignals_show, NULL);
 
 
-static ssize_t dgnc_tty_iflag_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_iflag_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -475,7 +506,8 @@ static ssize_t dgnc_tty_iflag_show(struct device *d, struct device_attribute *at
 static DEVICE_ATTR(iflag, S_IRUSR, dgnc_tty_iflag_show, NULL);
 
 
-static ssize_t dgnc_tty_cflag_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_cflag_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -500,7 +532,8 @@ static ssize_t dgnc_tty_cflag_show(struct device *d, struct device_attribute *at
 static DEVICE_ATTR(cflag, S_IRUSR, dgnc_tty_cflag_show, NULL);
 
 
-static ssize_t dgnc_tty_oflag_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_oflag_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -525,7 +558,8 @@ static ssize_t dgnc_tty_oflag_show(struct device *d, struct device_attribute *at
 static DEVICE_ATTR(oflag, S_IRUSR, dgnc_tty_oflag_show, NULL);
 
 
-static ssize_t dgnc_tty_lflag_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_lflag_show(struct device *d,
+				   struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -550,7 +584,8 @@ static ssize_t dgnc_tty_lflag_show(struct device *d, struct device_attribute *at
 static DEVICE_ATTR(lflag, S_IRUSR, dgnc_tty_lflag_show, NULL);
 
 
-static ssize_t dgnc_tty_digi_flag_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_digi_flag_show(struct device *d,
+				       struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -575,7 +610,8 @@ static ssize_t dgnc_tty_digi_flag_show(struct device *d, struct device_attribute
 static DEVICE_ATTR(digi_flag, S_IRUSR, dgnc_tty_digi_flag_show, NULL);
 
 
-static ssize_t dgnc_tty_rxcount_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_rxcount_show(struct device *d,
+				     struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -600,7 +636,8 @@ static ssize_t dgnc_tty_rxcount_show(struct device *d, struct device_attribute *
 static DEVICE_ATTR(rxcount, S_IRUSR, dgnc_tty_rxcount_show, NULL);
 
 
-static ssize_t dgnc_tty_txcount_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_txcount_show(struct device *d,
+				     struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
@@ -625,7 +662,8 @@ static ssize_t dgnc_tty_txcount_show(struct device *d, struct device_attribute *
 static DEVICE_ATTR(txcount, S_IRUSR, dgnc_tty_txcount_show, NULL);
 
 
-static ssize_t dgnc_tty_name_show(struct device *d, struct device_attribute *attr, char *buf)
+static ssize_t dgnc_tty_name_show(struct device *d,
+				  struct device_attribute *attr, char *buf)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index ce4187f60cb4..fbfe79a70263 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -42,16 +42,11 @@
 #include "dgnc_sysfs.h"
 #include "dgnc_utils.h"
 
-#define init_MUTEX(sem)	 sema_init(sem, 1)
-#define DECLARE_MUTEX(name)     \
-	struct semaphore name = __SEMAPHORE_INITIALIZER(name, 1)
-
 /*
  * internal variables
  */
 static struct dgnc_board	*dgnc_BoardsByMajor[256];
 static unsigned char		*dgnc_TmpWriteBuf;
-static DECLARE_MUTEX(dgnc_TmpWriteSem);
 
 /*
  * Default transparent print information.
@@ -304,19 +299,15 @@ int dgnc_tty_init(struct dgnc_board *brd)
 
 	brd->nasync = brd->maxports;
 
-	/*
-	 * Allocate channel memory that might not have been allocated
-	 * when the driver was first loaded.
-	 */
 	for (i = 0; i < brd->nasync; i++) {
-		if (!brd->channels[i]) {
-
-			/*
-			 * Okay to malloc with GFP_KERNEL, we are not at
-			 * interrupt context, and there are no locks held.
-			 */
-			brd->channels[i] = kzalloc(sizeof(*brd->channels[i]), GFP_KERNEL);
-		}
+		/*
+		 * Okay to malloc with GFP_KERNEL, we are not at
+		 * interrupt context, and there are no locks held.
+		 */
+		brd->channels[i] = kzalloc(sizeof(*brd->channels[i]),
+					   GFP_KERNEL);
+		if (!brd->channels[i])
+			goto err_free_channels;
 	}
 
 	ch = brd->channels[0];
@@ -324,10 +315,6 @@ int dgnc_tty_init(struct dgnc_board *brd)
 
 	/* Set up channel variables */
 	for (i = 0; i < brd->nasync; i++, ch = brd->channels[i]) {
-
-		if (!brd->channels[i])
-			continue;
-
 		spin_lock_init(&ch->ch_lock);
 
 		/* Store all our magic numbers */
@@ -375,6 +362,13 @@ int dgnc_tty_init(struct dgnc_board *brd)
 	}
 
 	return 0;
+
+err_free_channels:
+	for (i = i - 1; i >= 0; --i) {
+		kfree(brd->channels[i]);
+		brd->channels[i] = NULL;
+	}
+	return -ENOMEM;
 }
 
 
@@ -404,7 +398,9 @@ void dgnc_tty_uninit(struct dgnc_board *brd)
 		dgnc_BoardsByMajor[brd->SerialDriver.major] = NULL;
 		brd->dgnc_Serial_Major = 0;
 		for (i = 0; i < brd->nasync; i++) {
-			dgnc_remove_tty_sysfs(brd->channels[i]->ch_tun.un_sysfs);
+			if (brd->channels[i])
+				dgnc_remove_tty_sysfs(brd->channels[i]->
+						      ch_tun.un_sysfs);
 			tty_unregister_device(&brd->SerialDriver, i);
 		}
 		tty_unregister_driver(&brd->SerialDriver);
@@ -415,7 +411,9 @@ void dgnc_tty_uninit(struct dgnc_board *brd)
 		dgnc_BoardsByMajor[brd->PrintDriver.major] = NULL;
 		brd->dgnc_TransparentPrint_Major = 0;
 		for (i = 0; i < brd->nasync; i++) {
-			dgnc_remove_tty_sysfs(brd->channels[i]->ch_pun.un_sysfs);
+			if (brd->channels[i])
+				dgnc_remove_tty_sysfs(brd->channels[i]->
+						      ch_pun.un_sysfs);
 			tty_unregister_device(&brd->PrintDriver, i);
 		}
 		tty_unregister_driver(&brd->PrintDriver);
@@ -424,13 +422,14 @@ void dgnc_tty_uninit(struct dgnc_board *brd)
 
 	kfree(brd->SerialDriver.ttys);
 	brd->SerialDriver.ttys = NULL;
+	kfree(brd->SerialDriver.termios);
+	brd->SerialDriver.termios = NULL;
 	kfree(brd->PrintDriver.ttys);
 	brd->PrintDriver.ttys = NULL;
+	kfree(brd->PrintDriver.termios);
+	brd->PrintDriver.termios = NULL;
 }
 
-
-#define TMPBUFLEN (1024)
-
 /*=======================================================================
  *
  *	dgnc_wmove - Write data to transmit queue.
@@ -555,15 +554,6 @@ void dgnc_input(struct channel_t *ch)
 
 	ld = tty_ldisc_ref(tp);
 
-#ifdef TTY_DONT_FLIP
-	/*
-	 * If the DONT_FLIP flag is on, don't flush our buffer, and act
-	 * like the ld doesn't have any space to put the data right now.
-	 */
-	if (test_bit(TTY_DONT_FLIP, &tp->flags))
-		len = 0;
-#endif
-
 	/*
 	 * If we were unable to get a reference to the ld,
 	 * don't flush our buffer, and act like the ld doesn't
@@ -897,10 +887,6 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 			ch->ch_stops_sent = 0;
 			ch->ch_bd->bd_ops->send_start_character(ch);
 		}
-		/* No FLOW */
-		else {
-			/* Nothing needed. */
-		}
 	}
 }
 
@@ -1705,7 +1691,6 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	ushort tail;
 	ushort tmask;
 	uint remain;
-	int from_user = 0;
 
 	if (tty == NULL || dgnc_TmpWriteBuf == NULL)
 		return 0;
@@ -1779,44 +1764,6 @@ static int dgnc_tty_write(struct tty_struct *tty,
 		ch->ch_flags &= ~CH_PRON;
 	}
 
-	/*
-	 * If there is nothing left to copy, or I can't handle any more data, leave.
-	 */
-	if (count <= 0)
-		goto exit_retry;
-
-	if (from_user) {
-
-		count = min(count, WRITEBUFLEN);
-
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-
-		/*
-		 * If data is coming from user space, copy it into a temporary
-		 * buffer so we don't get swapped out while doing the copy to
-		 * the board.
-		 */
-		/* we're allowed to block if it's from_user */
-		if (down_interruptible(&dgnc_TmpWriteSem))
-			return -EINTR;
-
-		/*
-		 * copy_from_user() returns the number
-		 * of bytes that could *NOT* be copied.
-		 */
-		count -= copy_from_user(dgnc_TmpWriteBuf, (const unsigned char __user *) buf, count);
-
-		if (!count) {
-			up(&dgnc_TmpWriteSem);
-			return -EFAULT;
-		}
-
-		spin_lock_irqsave(&ch->ch_lock, flags);
-
-		buf = dgnc_TmpWriteBuf;
-
-	}
-
 	n = count;
 
 	/*
@@ -1853,12 +1800,7 @@ static int dgnc_tty_write(struct tty_struct *tty,
 		ch->ch_cpstime += (HZ * count) / ch->ch_digi.digi_maxcps;
 	}
 
-	if (from_user) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		up(&dgnc_TmpWriteSem);
-	} else {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-	}
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	if (count) {
 		/*
diff --git a/drivers/staging/dgnc/digi.h b/drivers/staging/dgnc/digi.h
index d637a7802168..cf9dcae7cc3f 100644
--- a/drivers/staging/dgnc/digi.h
+++ b/drivers/staging/dgnc/digi.h
@@ -130,19 +130,19 @@ struct digi_getcounter {
 #define	BD_RUNNING	0x0
 #define	BD_NOFEP	0x5
 
-#define DIGI_SETCUSTOMBAUD	_IOW('e', 106, int)	/* Set integer baud rate */
-#define DIGI_GETCUSTOMBAUD	_IOR('e', 107, int)	/* Get integer baud rate */
+#define DIGI_SETCUSTOMBAUD _IOW('e', 106, int)	/* Set integer baud rate */
+#define DIGI_GETCUSTOMBAUD _IOR('e', 107, int)	/* Get integer baud rate */
 
 #define DIGI_REALPORT_GETBUFFERS (('e'<<8) | 108)
 #define DIGI_REALPORT_SENDIMMEDIATE (('e'<<8) | 109)
 #define DIGI_REALPORT_GETCOUNTERS (('e'<<8) | 110)
 #define DIGI_REALPORT_GETEVENTS (('e'<<8) | 111)
 
-#define EV_OPU		0x0001		/* !<Output paused by client */
-#define EV_OPS		0x0002		/* !<Output paused by reqular sw flowctrl */
-#define EV_IPU		0x0010		/* !<Input paused unconditionally by user */
-#define EV_IPS		0x0020		/* !<Input paused by high/low water marks */
-#define EV_TXB		0x0040		/* !<Transmit break pending */
+#define EV_OPU 0x0001 /* !<Output paused by client */
+#define EV_OPS 0x0002 /* !<Output paused by reqular sw flowctrl */
+#define EV_IPU 0x0010 /* !<Input paused unconditionally by user */
+#define EV_IPS 0x0020 /* !<Input paused by high/low water marks */
+#define EV_TXB 0x0040 /* !<Transmit break pending */
 
 /*
  * This structure holds data needed for the intelligent <--> nonintelligent
diff --git a/drivers/staging/emxx_udc/emxx_udc.c b/drivers/staging/emxx_udc/emxx_udc.c
index fbf82bc735cf..4178d96f94cf 100644
--- a/drivers/staging/emxx_udc/emxx_udc.c
+++ b/drivers/staging/emxx_udc/emxx_udc.c
@@ -1223,7 +1223,7 @@ static int _nbu2ss_epn_in_transfer(
 	}
 
 	/*-------------------------------------------------------------*/
-	/* Start tranfer */
+	/* Start transfer */
 	iBufSize = req->req.length - req->req.actual;
 	if (iBufSize > 0)
 		result = _nbu2ss_epn_in_data(udc, ep, req, iBufSize);
@@ -2199,18 +2199,6 @@ static void _nbu2ss_ep0_enable(struct nbu2ss_udc *udc)
 	_nbu2ss_writel(&udc->p_regs->EP0_INT_ENA, EP0_INT_EN_BIT);
 }
 
-#if 0
-/*-------------------------------------------------------------------------*/
-static void _nbu2ss_ep0_disable(struct nbu2ss_udc *udc)
-{
-	_nbu2ss_bitclr(&udc->p_regs->EP0_INT_ENA, EP0_INT_EN_BIT);
-
-	_nbu2ss_bitset(&udc->p_regs->EP0_CONTROL
-			, (EP0_BCLR | EP0_INAK | EP0_ONAK | EP0_BCLR));
-
-	_nbu2ss_bitclr(&udc->p_regs->EP0_CONTROL, EP0_AUTO);
-}
-#endif
 
 /*-------------------------------------------------------------------------*/
 static int _nbu2ss_nuke(struct nbu2ss_udc *udc,
@@ -2261,8 +2249,6 @@ static int _nbu2ss_pullup(struct nbu2ss_udc *udc, int is_on)
 
 	if (is_on) {
 		/* D+ Pullup */
-/*		INFO(" --- D+ Pullup\n"); */
-
 		if (udc->driver) {
 			reg_dt = (_nbu2ss_readl(&udc->p_regs->USB_CONTROL)
 				| PUE2) & ~(u32)CONNECTB;
@@ -2272,8 +2258,6 @@ static int _nbu2ss_pullup(struct nbu2ss_udc *udc, int is_on)
 
 	} else {
 		/* D+ Pulldown */
-/*		INFO(" --- D+ Pulldown\n"); */
-
 		reg_dt = (_nbu2ss_readl(&udc->p_regs->USB_CONTROL) | CONNECTB)
 			& ~(u32)PUE2;
 
@@ -2311,12 +2295,6 @@ static int _nbu2ss_enable_controller(struct nbu2ss_udc *udc)
 	if (udc->udc_enabled)
 		return 0;
 
-#if 0
-	emxx_open_clockgate(EMXX_CLK_USB1);
-	/* emxx_clkctrl_off(EMXX_CLKCTRL_USB1); */
-	/* emxx_clkctrl_on(EMXX_CLKCTRL_USB1); */
-	emxx_unreset_device(EMXX_RST_USB1);
-#endif
 	/*
 		Reset
 	*/
@@ -2330,13 +2308,6 @@ static int _nbu2ss_enable_controller(struct nbu2ss_udc *udc)
 
 	_nbu2ss_writel(&udc->p_regs->AHBSCTR, WAIT_MODE);
 
-#if 0
-	/* DMA Mode Setting */
-	if ((system_rev & EMXX_REV_MASK) == EMXX_REV_ES1) {
-		_nbu2ss_bitset(&udc->p_regs->AHBMCTR, BURST_TYPE);
-		_nbu2ss_bitclr(&udc->p_regs->AHBMCTR, HTRANS_MODE);
-	} else
-#endif
 		_nbu2ss_writel(&udc->p_regs->AHBMCTR,
 			HBUSREQ_MODE | HTRANS_MODE | WBURST_TYPE);
 
@@ -2347,11 +2318,8 @@ static int _nbu2ss_enable_controller(struct nbu2ss_udc *udc)
 			dev_err(udc->dev, "*** Reset Cancel failed\n");
 			return -EINVAL;
 		}
-	};
+	}
 
-#if 0
-	if ((system_rev & EMXX_REV_MASK) < EMXX_REV_ES3)
-#endif
 		_nbu2ss_bitset(&udc->p_regs->UTMI_CHARACTER_1, USB_SQUSET);
 
 	_nbu2ss_bitset(&udc->p_regs->USB_CONTROL, (INT_SEL | SOF_RCV));
@@ -2383,11 +2351,6 @@ static void _nbu2ss_disable_controller(struct nbu2ss_udc *udc)
 		_nbu2ss_reset_controller(udc);
 		_nbu2ss_bitset(&udc->p_regs->EPCTR, (DIRPD | EPC_RST));
 	}
-#if 0
-	emxx_reset_device(EMXX_RST_USB1);
-	/* emxx_clkctrl_on(EMXX_CLKCTRL_USB1); */
-	emxx_close_clockgate(EMXX_CLK_USB1);
-#endif
 }
 
 /*-------------------------------------------------------------------------*/
@@ -2764,8 +2727,6 @@ static int nbu2ss_ep_queue(
 	ep = container_of(_ep, struct nbu2ss_ep, ep);
 	udc = ep->udc;
 
-/*	INFO("=== %s(ep%d), zero=%d\n", __func__, ep->epnum, _req->zero); */
-
 	if (udc->vbus_active == 0) {
 		dev_info(udc->dev, "Can't ep_queue (VBUS OFF)\n");
 		return -ESHUTDOWN;
@@ -2841,8 +2802,6 @@ static int nbu2ss_ep_dequeue(
 	struct nbu2ss_udc	*udc;
 	unsigned long flags;
 
-	/*INFO("=== %s()\n", __func__);*/
-
 	/* catch various bogus parameters */
 	if ((_ep == NULL) || (_req == NULL)) {
 		/* pr_err("%s, bad param(1)\n", __func__); */
@@ -2888,8 +2847,6 @@ static int nbu2ss_ep_set_halt(struct usb_ep *_ep, int value)
 	struct nbu2ss_ep	*ep;
 	struct nbu2ss_udc	*udc;
 
-/*	INFO("=== %s()\n", __func__); */
-
 	if (!_ep) {
 		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
@@ -2942,8 +2899,6 @@ static int nbu2ss_ep_fifo_status(struct usb_ep *_ep)
 	unsigned long		flags;
 	struct fc_regs		*preg;
 
-/*	INFO("=== %s()\n", __func__); */
-
 	if (!_ep) {
 		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
@@ -2990,15 +2945,13 @@ static void  nbu2ss_ep_fifo_flush(struct usb_ep *_ep)
 	struct nbu2ss_udc	*udc;
 	unsigned long		flags;
 
-/*	INFO("=== %s()\n", __func__); */
-
 	if (!_ep) {
 		pr_err("udc: %s, bad param\n", __func__);
 		return;
 	}
 
 	ep = container_of(_ep, struct nbu2ss_ep, ep);
-	if (!_ep) {
+	if (!ep) {
 		pr_err("udc: %s, bad ep\n", __func__);
 		return;
 	}
@@ -3046,8 +2999,6 @@ static int nbu2ss_gad_get_frame(struct usb_gadget *pgadget)
 	u32			data;
 	struct nbu2ss_udc	*udc;
 
-/*	INFO("=== %s()\n", __func__); */
-
 	if (pgadget == NULL) {
 		pr_err("udc: %s, bad param\n", __func__);
 		return -EINVAL;
@@ -3076,8 +3027,6 @@ static int nbu2ss_gad_wakeup(struct usb_gadget *pgadget)
 
 	struct nbu2ss_udc	*udc;
 
-/*	INFO("=== %s()\n", __func__); */
-
 	if (pgadget == NULL) {
 		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
@@ -3116,8 +3065,6 @@ static int nbu2ss_gad_set_selfpowered(struct usb_gadget *pgadget,
 	struct nbu2ss_udc       *udc;
 	unsigned long		flags;
 
-/*	INFO("=== %s()\n", __func__); */
-
 	if (pgadget == NULL) {
 		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
@@ -3135,7 +3082,6 @@ static int nbu2ss_gad_set_selfpowered(struct usb_gadget *pgadget,
 /*-------------------------------------------------------------------------*/
 static int nbu2ss_gad_vbus_session(struct usb_gadget *pgadget, int is_active)
 {
-/*	INFO("=== %s()\n", __func__); */
 	return 0;
 }
 
@@ -3145,8 +3091,6 @@ static int nbu2ss_gad_vbus_draw(struct usb_gadget *pgadget, unsigned mA)
 	struct nbu2ss_udc	*udc;
 	unsigned long		flags;
 
-/*	INFO("=== %s()\n", __func__); */
-
 	if (pgadget == NULL) {
 		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
@@ -3167,8 +3111,6 @@ static int nbu2ss_gad_pullup(struct usb_gadget *pgadget, int is_on)
 	struct nbu2ss_udc	*udc;
 	unsigned long		flags;
 
-/*	INFO("=== %s()\n", __func__); */
-
 	if (pgadget == NULL) {
 		pr_err("%s, bad param\n", __func__);
 		return -EINVAL;
@@ -3197,7 +3139,6 @@ static int nbu2ss_gad_ioctl(
 	unsigned code,
 	unsigned long param)
 {
-/*	INFO("=== %s()\n", __func__); */
 	return 0;
 }
 
diff --git a/drivers/staging/emxx_udc/emxx_udc.h b/drivers/staging/emxx_udc/emxx_udc.h
index c19168f78354..0db6b41333ea 100644
--- a/drivers/staging/emxx_udc/emxx_udc.h
+++ b/drivers/staging/emxx_udc/emxx_udc.h
@@ -32,8 +32,6 @@
 #define UDC_DEBUG_DUMP
 #endif
 
-/* #define USE_INT_COUNT_OVER */
-
 /*----------------- Default define */
 #define	USE_DMA	1
 #define USE_SUSPEND_WAIT	1
@@ -117,14 +115,6 @@
 #define BIT30		0x40000000
 #define BIT31		0x80000000
 
-#if 0
-/*------- (0x0000) USB Control Register */
-#define USBTESTMODE			(BIT18+BIT17+BIT16)
-#define TEST_J				BIT16
-#define TEST_K				BIT17
-#define TEST_SE0_NAK			(BIT17+BIT16)
-#define TEST_PACKET			BIT18
-#endif
 #define TEST_FORCE_ENABLE		(BIT18+BIT16)
 
 #define INT_SEL				BIT10
diff --git a/drivers/staging/fbtft/Kconfig b/drivers/staging/fbtft/Kconfig
index 6cf0c58f538b..d4018780ce58 100644
--- a/drivers/staging/fbtft/Kconfig
+++ b/drivers/staging/fbtft/Kconfig
@@ -12,7 +12,7 @@ config FB_TFT_AGM1264K_FL
 	tristate "FB driver for the AGM1264K-FL LCD display"
 	depends on FB_TFT
 	help
-	  Framebuffer support for the AGM1264K-FL LCD display (two Samsung KS0108 compatable chips)
+	  Framebuffer support for the AGM1264K-FL LCD display (two Samsung KS0108 compatible chips)
 
 config FB_TFT_BD663474
 	tristate "FB driver for the BD663474 LCD Controller"
@@ -38,6 +38,12 @@ config FB_TFT_HX8353D
 	help
 	  Generic Framebuffer support for HX8353D
 
+config FB_TFT_HX8357D
+	tristate "FB driver for the HX8357D LCD Controller"
+	depends on FB_TFT
+	help
+	  Generic Framebuffer support for HX8357D
+
 config FB_TFT_ILI9163
 	tristate "FB driver for the ILI9163 LCD Controller"
 	depends on FB_TFT
diff --git a/drivers/staging/fbtft/Makefile b/drivers/staging/fbtft/Makefile
index 9e73beee23f4..554b5260b0ee 100644
--- a/drivers/staging/fbtft/Makefile
+++ b/drivers/staging/fbtft/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_FB_TFT_BD663474)    += fb_bd663474.o
 obj-$(CONFIG_FB_TFT_HX8340BN)    += fb_hx8340bn.o
 obj-$(CONFIG_FB_TFT_HX8347D)     += fb_hx8347d.o
 obj-$(CONFIG_FB_TFT_HX8353D)     += fb_hx8353d.o
+obj-$(CONFIG_FB_TFT_HX8357D)     += fb_hx8357d.o
 obj-$(CONFIG_FB_TFT_ILI9163)     += fb_ili9163.o
 obj-$(CONFIG_FB_TFT_ILI9320)     += fb_ili9320.o
 obj-$(CONFIG_FB_TFT_ILI9325)     += fb_ili9325.o
diff --git a/drivers/staging/fbtft/fb_agm1264k-fl.c b/drivers/staging/fbtft/fb_agm1264k-fl.c
index 8f5af1db852c..94dd49ce18de 100644
--- a/drivers/staging/fbtft/fb_agm1264k-fl.c
+++ b/drivers/staging/fbtft/fb_agm1264k-fl.c
@@ -94,7 +94,7 @@ static void reset(struct fbtft_par *par)
 	if (par->gpio.reset == -1)
 		return;
 
-	fbtft_dev_dbg(DEBUG_RESET, par, par->info->device, "%s()\n", __func__);
+	dev_dbg(par->info->device, "%s()\n", __func__);
 
 	gpio_set_value(par->gpio.reset, 0);
 	udelay(20);
@@ -107,7 +107,7 @@ static int verify_gpios(struct fbtft_par *par)
 {
 	int i;
 
-	fbtft_dev_dbg(DEBUG_VERIFY_GPIOS, par, par->info->device,
+	dev_dbg(par->info->device,
 		"%s()\n", __func__);
 
 	if (par->EPIN < 0) {
@@ -145,7 +145,7 @@ static int verify_gpios(struct fbtft_par *par)
 static unsigned long
 request_gpios_match(struct fbtft_par *par, const struct fbtft_gpio *gpio)
 {
-	fbtft_dev_dbg(DEBUG_REQUEST_GPIOS_MATCH, par, par->info->device,
+	dev_dbg(par->info->device,
 		"%s('%s')\n", __func__, gpio->name);
 
 	if (strcasecmp(gpio->name, "wr") == 0) {
diff --git a/drivers/staging/fbtft/fb_hx8357d.c b/drivers/staging/fbtft/fb_hx8357d.c
new file mode 100644
index 000000000000..8c7bb3ac8030
--- /dev/null
+++ b/drivers/staging/fbtft/fb_hx8357d.c
@@ -0,0 +1,222 @@
+/*
+ * FB driver for the HX8357D LCD Controller
+ * Copyright (C) 2015 Adafruit Industries
+ *
+ * Based on the HX8347D FB driver
+ * Copyright (C) 2013 Christian Vogelgsang
+ *
+ * Based on driver code found here: https://github.com/watterott/r61505u-Adapter
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+
+#include "fbtft.h"
+#include "fb_hx8357d.h"
+
+#define DRVNAME		"fb_hx8357d"
+#define WIDTH		320
+#define HEIGHT		480
+
+
+static int init_display(struct fbtft_par *par)
+{
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
+
+	par->fbtftops.reset(par);
+
+	/* Reset things like Gamma */
+	write_reg(par, HX8357B_SWRESET);
+	usleep_range(5000, 7000);
+
+	/* setextc */
+	write_reg(par, HX8357D_SETC, 0xFF, 0x83, 0x57);
+	msleep(150);
+
+	/* setRGB which also enables SDO */
+	write_reg(par, HX8357_SETRGB, 0x00, 0x00, 0x06, 0x06);
+
+	/* -1.52V */
+	write_reg(par, HX8357D_SETCOM, 0x25);
+
+	/* Normal mode 70Hz, Idle mode 55 Hz */
+	write_reg(par, HX8357_SETOSC, 0x68);
+
+	/* Set Panel - BGR, Gate direction swapped */
+	write_reg(par, HX8357_SETPANEL, 0x05);
+
+	write_reg(par, HX8357_SETPWR1,
+		0x00,  /* Not deep standby */
+		0x15,  /* BT */
+		0x1C,  /* VSPR */
+		0x1C,  /* VSNR */
+		0x83,  /* AP */
+		0xAA);  /* FS */
+
+	write_reg(par, HX8357D_SETSTBA,
+		0x50,  /* OPON normal */
+		0x50,  /* OPON idle */
+		0x01,  /* STBA */
+		0x3C,  /* STBA */
+		0x1E,  /* STBA */
+		0x08);  /* GEN */
+
+	write_reg(par, HX8357D_SETCYC,
+		0x02,  /* NW 0x02 */
+		0x40,  /* RTN */
+		0x00,  /* DIV */
+		0x2A,  /* DUM */
+		0x2A,  /* DUM */
+		0x0D,  /* GDON */
+		0x78);  /* GDOFF */
+
+	write_reg(par, HX8357D_SETGAMMA,
+		0x02,
+		0x0A,
+		0x11,
+		0x1d,
+		0x23,
+		0x35,
+		0x41,
+		0x4b,
+		0x4b,
+		0x42,
+		0x3A,
+		0x27,
+		0x1B,
+		0x08,
+		0x09,
+		0x03,
+		0x02,
+		0x0A,
+		0x11,
+		0x1d,
+		0x23,
+		0x35,
+		0x41,
+		0x4b,
+		0x4b,
+		0x42,
+		0x3A,
+		0x27,
+		0x1B,
+		0x08,
+		0x09,
+		0x03,
+		0x00,
+		0x01);
+
+	/* 16 bit */
+	write_reg(par, HX8357_COLMOD, 0x55);
+
+	write_reg(par, HX8357_MADCTL, 0xC0);
+
+	/* TE off */
+	write_reg(par, HX8357_TEON, 0x00);
+
+	/* tear line */
+	write_reg(par, HX8357_TEARLINE, 0x00, 0x02);
+
+	/* Exit Sleep */
+	write_reg(par, HX8357_SLPOUT);
+	msleep(150);
+
+	/* display on */
+	write_reg(par, HX8357_DISPON);
+	usleep_range(5000, 7000);
+
+	return 0;
+}
+
+static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+{
+	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par,
+		"%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
+
+	/* Column addr set */
+	write_reg(par, HX8357_CASET,
+		xs >> 8, xs & 0xff,  /* XSTART */
+		xe >> 8, xe & 0xff); /* XEND */
+
+	/* Row addr set */
+	write_reg(par, HX8357_PASET,
+		ys >> 8, ys & 0xff,  /* YSTART */
+		ye >> 8, ye & 0xff); /* YEND */
+
+	/* write to RAM */
+	write_reg(par, HX8357_RAMWR);
+}
+
+#define HX8357D_MADCTL_MY  0x80
+#define HX8357D_MADCTL_MX  0x40
+#define HX8357D_MADCTL_MV  0x20
+#define HX8357D_MADCTL_ML  0x10
+#define HX8357D_MADCTL_RGB 0x00
+#define HX8357D_MADCTL_BGR 0x08
+#define HX8357D_MADCTL_MH  0x04
+static int set_var(struct fbtft_par *par)
+{
+	u8 val;
+
+	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
+
+	switch (par->info->var.rotate) {
+	case 270:
+		val = HX8357D_MADCTL_MV | HX8357D_MADCTL_MX;
+		break;
+	case 180:
+		val = 0;
+		break;
+	case 90:
+		val = HX8357D_MADCTL_MV | HX8357D_MADCTL_MY;
+		break;
+	default:
+		val = HX8357D_MADCTL_MX | HX8357D_MADCTL_MY;
+		break;
+	}
+
+	val |= (par->bgr ? HX8357D_MADCTL_RGB : HX8357D_MADCTL_BGR);
+
+	/* Memory Access Control */
+	write_reg(par, HX8357_MADCTL, val);
+
+	return 0;
+}
+
+static struct fbtft_display display = {
+	.regwidth = 8,
+	.width = WIDTH,
+	.height = HEIGHT,
+	.gamma_num = 2,
+	.gamma_len = 14,
+	.fbtftops = {
+		.init_display = init_display,
+		.set_addr_win = set_addr_win,
+		.set_var = set_var,
+	},
+};
+FBTFT_REGISTER_DRIVER(DRVNAME, "himax,hx8357d", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:hx8357d");
+MODULE_ALIAS("platform:hx8357d");
+
+MODULE_DESCRIPTION("FB driver for the HX8357D LCD Controller");
+MODULE_AUTHOR("Sean Cross <xobs@kosagi.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/fbtft/fb_hx8357d.h b/drivers/staging/fbtft/fb_hx8357d.h
new file mode 100644
index 000000000000..de05e8cdf04c
--- /dev/null
+++ b/drivers/staging/fbtft/fb_hx8357d.h
@@ -0,0 +1,102 @@
+/***************************************************
+  This is our library for the Adafruit  ILI9341 Breakout and Shield
+  ----> http://www.adafruit.com/products/1651
+
+  Check out the links above for our tutorials and wiring diagrams
+  These displays use SPI to communicate, 4 or 5 pins are required to
+  interface (RST is optional)
+  Adafruit invests time and resources providing this open source code,
+  please support Adafruit and open-source hardware by purchasing
+  products from Adafruit!
+
+  Written by Limor Fried/Ladyada for Adafruit Industries.
+  MIT license, all text above must be included in any redistribution
+ ****************************************************/
+
+#ifndef __HX8357_H__
+#define __HX8357_H__
+
+#define HX8357D 0xD
+#define HX8357B 0xB
+
+#define HX8357_TFTWIDTH  320
+#define HX8357_TFTHEIGHT 480
+
+#define HX8357B_NOP     0x00
+#define HX8357B_SWRESET 0x01
+#define HX8357B_RDDID   0x04
+#define HX8357B_RDDST   0x09
+
+#define HX8357B_RDPOWMODE  0x0A
+#define HX8357B_RDMADCTL  0x0B
+#define HX8357B_RDCOLMOD  0x0C
+#define HX8357B_RDDIM  0x0D
+#define HX8357B_RDDSDR  0x0F
+
+#define HX8357_SLPIN   0x10
+#define HX8357_SLPOUT  0x11
+#define HX8357B_PTLON   0x12
+#define HX8357B_NORON   0x13
+
+#define HX8357_INVOFF  0x20
+#define HX8357_INVON   0x21
+#define HX8357_DISPOFF 0x28
+#define HX8357_DISPON  0x29
+
+#define HX8357_CASET   0x2A
+#define HX8357_PASET   0x2B
+#define HX8357_RAMWR   0x2C
+#define HX8357_RAMRD   0x2E
+
+#define HX8357B_PTLAR   0x30
+#define HX8357_TEON  0x35
+#define HX8357_TEARLINE  0x44
+#define HX8357_MADCTL  0x36
+#define HX8357_COLMOD  0x3A
+
+#define HX8357_SETOSC 0xB0
+#define HX8357_SETPWR1 0xB1
+#define HX8357B_SETDISPLAY 0xB2
+#define HX8357_SETRGB 0xB3
+#define HX8357D_SETCOM  0xB6
+
+#define HX8357B_SETDISPMODE  0xB4
+#define HX8357D_SETCYC  0xB4
+#define HX8357B_SETOTP 0xB7
+#define HX8357D_SETC 0xB9
+
+#define HX8357B_SET_PANEL_DRIVING 0xC0
+#define HX8357D_SETSTBA 0xC0
+#define HX8357B_SETDGC  0xC1
+#define HX8357B_SETID  0xC3
+#define HX8357B_SETDDB  0xC4
+#define HX8357B_SETDISPLAYFRAME 0xC5
+#define HX8357B_GAMMASET 0xC8
+#define HX8357B_SETCABC  0xC9
+#define HX8357_SETPANEL  0xCC
+
+#define HX8357B_SETPOWER 0xD0
+#define HX8357B_SETVCOM 0xD1
+#define HX8357B_SETPWRNORMAL 0xD2
+
+#define HX8357B_RDID1   0xDA
+#define HX8357B_RDID2   0xDB
+#define HX8357B_RDID3   0xDC
+#define HX8357B_RDID4   0xDD
+
+#define HX8357D_SETGAMMA 0xE0
+
+#define HX8357B_SETGAMMA 0xC8
+#define HX8357B_SETPANELRELATED  0xE9
+
+/* Color definitions */
+#define	HX8357_BLACK   0x0000
+#define	HX8357_BLUE    0x001F
+#define	HX8357_RED     0xF800
+#define	HX8357_GREEN   0x07E0
+#define HX8357_CYAN    0x07FF
+#define HX8357_MAGENTA 0xF81F
+#define HX8357_YELLOW  0xFFE0
+#define HX8357_WHITE   0xFFFF
+
+#endif /* __HX8357_H__ */
diff --git a/drivers/staging/fbtft/fb_ili9320.c b/drivers/staging/fbtft/fb_ili9320.c
index 3a02edd447d4..ef4fa6b72c79 100644
--- a/drivers/staging/fbtft/fb_ili9320.c
+++ b/drivers/staging/fbtft/fb_ili9320.c
@@ -63,43 +63,101 @@ static int init_display(struct fbtft_par *par)
 	/* Initialization sequence from ILI9320 Application Notes */
 
 	/* *********** Start Initial Sequence ********* */
-	write_reg(par, 0x00E5, 0x8000); /* Set the Vcore voltage and this setting is must. */
-	write_reg(par, 0x0000, 0x0001); /* Start internal OSC. */
-	write_reg(par, 0x0001, 0x0100); /* set SS and SM bit */
-	write_reg(par, 0x0002, 0x0700); /* set 1 line inversion */
-	write_reg(par, 0x0004, 0x0000); /* Resize register */
-	write_reg(par, 0x0008, 0x0202); /* set the back and front porch */
-	write_reg(par, 0x0009, 0x0000); /* set non-display area refresh cycle */
-	write_reg(par, 0x000A, 0x0000); /* FMARK function */
-	write_reg(par, 0x000C, 0x0000); /* RGB interface setting */
-	write_reg(par, 0x000D, 0x0000); /* Frame marker Position */
-	write_reg(par, 0x000F, 0x0000); /* RGB interface polarity */
+	/* Set the Vcore voltage and this setting is must. */
+	write_reg(par, 0x00E5, 0x8000);
+
+	/* Start internal OSC. */
+	write_reg(par, 0x0000, 0x0001);
+
+	/* set SS and SM bit */
+	write_reg(par, 0x0001, 0x0100);
+
+	/* set 1 line inversion */
+	write_reg(par, 0x0002, 0x0700);
+
+	/* Resize register */
+	write_reg(par, 0x0004, 0x0000);
+
+	/* set the back and front porch */
+	write_reg(par, 0x0008, 0x0202);
+
+	/* set non-display area refresh cycle */
+	write_reg(par, 0x0009, 0x0000);
+
+	/* FMARK function */
+	write_reg(par, 0x000A, 0x0000);
+
+	/* RGB interface setting */
+	write_reg(par, 0x000C, 0x0000);
+
+	/* Frame marker Position */
+	write_reg(par, 0x000D, 0x0000);
+
+	/* RGB interface polarity */
+	write_reg(par, 0x000F, 0x0000);
+
 
 	/* ***********Power On sequence *************** */
-	write_reg(par, 0x0010, 0x0000); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
-	write_reg(par, 0x0011, 0x0007); /* DC1[2:0], DC0[2:0], VC[2:0] */
-	write_reg(par, 0x0012, 0x0000); /* VREG1OUT voltage */
-	write_reg(par, 0x0013, 0x0000); /* VDV[4:0] for VCOM amplitude */
-	mdelay(200); /* Dis-charge capacitor power voltage */
-	write_reg(par, 0x0010, 0x17B0); /* SAP, BT[3:0], AP, DSTB, SLP, STB */
-	write_reg(par, 0x0011, 0x0031); /* R11h=0x0031 at VCI=3.3V DC1[2:0], DC0[2:0], VC[2:0] */
+	/* SAP, BT[3:0], AP, DSTB, SLP, STB */
+	write_reg(par, 0x0010, 0x0000);
+
+	/* DC1[2:0], DC0[2:0], VC[2:0] */
+	write_reg(par, 0x0011, 0x0007);
+
+	/* VREG1OUT voltage */
+	write_reg(par, 0x0012, 0x0000);
+
+	/* VDV[4:0] for VCOM amplitude */
+	write_reg(par, 0x0013, 0x0000);
+
+	/* Dis-charge capacitor power voltage */
+	mdelay(200);
+
+	/* SAP, BT[3:0], AP, DSTB, SLP, STB */
+	write_reg(par, 0x0010, 0x17B0);
+
+	/* R11h=0x0031 at VCI=3.3V DC1[2:0], DC0[2:0], VC[2:0] */
+	write_reg(par, 0x0011, 0x0031);
 	mdelay(50);
-	write_reg(par, 0x0012, 0x0138); /* R12h=0x0138 at VCI=3.3V VREG1OUT voltage */
+
+	/* R12h=0x0138 at VCI=3.3V VREG1OUT voltage */
+	write_reg(par, 0x0012, 0x0138);
 	mdelay(50);
-	write_reg(par, 0x0013, 0x1800); /* R13h=0x1800 at VCI=3.3V VDV[4:0] for VCOM amplitude */
-	write_reg(par, 0x0029, 0x0008); /* R29h=0x0008 at VCI=3.3V VCM[4:0] for VCOMH */
+
+	/* R13h=0x1800 at VCI=3.3V VDV[4:0] for VCOM amplitude */
+	write_reg(par, 0x0013, 0x1800);
+
+	/* R29h=0x0008 at VCI=3.3V VCM[4:0] for VCOMH */
+	write_reg(par, 0x0029, 0x0008);
 	mdelay(50);
-	write_reg(par, 0x0020, 0x0000); /* GRAM horizontal Address */
-	write_reg(par, 0x0021, 0x0000); /* GRAM Vertical Address */
+
+	/* GRAM horizontal Address */
+	write_reg(par, 0x0020, 0x0000);
+
+	/* GRAM Vertical Address */
+	write_reg(par, 0x0021, 0x0000);
 
 	/* ------------------ Set GRAM area --------------- */
-	write_reg(par, 0x0050, 0x0000); /* Horizontal GRAM Start Address */
-	write_reg(par, 0x0051, 0x00EF); /* Horizontal GRAM End Address */
-	write_reg(par, 0x0052, 0x0000); /* Vertical GRAM Start Address */
-	write_reg(par, 0x0053, 0x013F); /* Vertical GRAM Start Address */
-	write_reg(par, 0x0060, 0x2700); /* Gate Scan Line */
-	write_reg(par, 0x0061, 0x0001); /* NDL,VLE, REV */
-	write_reg(par, 0x006A, 0x0000); /* set scrolling line */
+	/* Horizontal GRAM Start Address */
+	write_reg(par, 0x0050, 0x0000);
+
+	/* Horizontal GRAM End Address */
+	write_reg(par, 0x0051, 0x00EF);
+
+	/* Vertical GRAM Start Address */
+	write_reg(par, 0x0052, 0x0000);
+
+	/* Vertical GRAM End Address */
+	write_reg(par, 0x0053, 0x013F);
+
+	/* Gate Scan Line */
+	write_reg(par, 0x0060, 0x2700);
+
+	/* NDL,VLE, REV */
+	write_reg(par, 0x0061, 0x0001);
+
+	/* set scrolling line */
+	write_reg(par, 0x006A, 0x0000);
 
 	/* -------------- Partial Display Control --------- */
 	write_reg(par, 0x0080, 0x0000);
diff --git a/drivers/staging/fbtft/fb_ra8875.c b/drivers/staging/fbtft/fb_ra8875.c
index 2c4d4dc70c51..54bc566b09fd 100644
--- a/drivers/staging/fbtft/fb_ra8875.c
+++ b/drivers/staging/fbtft/fb_ra8875.c
@@ -73,7 +73,9 @@ static int init_display(struct fbtft_par *par)
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
 		"%s()\n", __func__);
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
-		"display size %dx%d\n", par->info->var.xres, par->info->var.yres);
+		"display size %dx%d\n",
+		par->info->var.xres,
+		par->info->var.yres);
 
 	par->fbtftops.reset(par);
 
@@ -101,7 +103,8 @@ static int init_display(struct fbtft_par *par)
 		write_reg(par, 0x1D, 0x0E);
 		write_reg(par, 0x1E, 0x00);
 		write_reg(par, 0x1F, 0x02);
-	} else if ((par->info->var.xres == 480) && (par->info->var.yres == 272)) {
+	} else if ((par->info->var.xres == 480) &&
+		   (par->info->var.yres == 272)) {
 		/* PLL clock frequency  */
 		write_reg(par, 0x88, 0x0A);
 		write_reg(par, 0x89, 0x02);
@@ -125,7 +128,8 @@ static int init_display(struct fbtft_par *par)
 		write_reg(par, 0x1D, 0x07);
 		write_reg(par, 0x1E, 0x00);
 		write_reg(par, 0x1F, 0x09);
-	} else if ((par->info->var.xres == 640) && (par->info->var.yres == 480)) {
+	} else if ((par->info->var.xres == 640) &&
+		   (par->info->var.yres == 480)) {
 		/* PLL clock frequency */
 		write_reg(par, 0x88, 0x0B);
 		write_reg(par, 0x89, 0x02);
@@ -149,7 +153,8 @@ static int init_display(struct fbtft_par *par)
 		write_reg(par, 0x1D, 0x0E);
 		write_reg(par, 0x1E, 0x00);
 		write_reg(par, 0x1F, 0x01);
-	} else if ((par->info->var.xres == 800) && (par->info->var.yres == 480)) {
+	} else if ((par->info->var.xres == 800) &&
+		   (par->info->var.yres == 480)) {
 		/* PLL clock frequency */
 		write_reg(par, 0x88, 0x0B);
 		write_reg(par, 0x89, 0x02);
diff --git a/drivers/staging/fbtft/fb_st7735r.c b/drivers/staging/fbtft/fb_st7735r.c
index 9d874308447e..f65224318610 100644
--- a/drivers/staging/fbtft/fb_st7735r.c
+++ b/drivers/staging/fbtft/fb_st7735r.c
@@ -25,8 +25,8 @@
 #include "fbtft.h"
 
 #define DRVNAME "fb_st7735r"
-#define DEFAULT_GAMMA "0F 1A 0F 18 2F 28 20 22 1F 1B 23 37 00 07 02 10\n" \
-                      "0F 1B 0F 17 33 2C 29 2E 30 30 39 3F 00 07 03 10"
+#define DEFAULT_GAMMA   "0F 1A 0F 18 2F 28 20 22 1F 1B 23 37 00 07 02 10\n" \
+			"0F 1B 0F 17 33 2C 29 2E 30 30 39 3F 00 07 03 10"
 
 
 static int default_init_sequence[] = {
@@ -119,9 +119,9 @@ static int set_var(struct fbtft_par *par)
 	/* MADCTL - Memory data access control
 	     RGB/BGR:
 	     1. Mode selection pin SRGB
-	        RGB H/W pin for color filter setting: 0=RGB, 1=BGR
+		RGB H/W pin for color filter setting: 0=RGB, 1=BGR
 	     2. MADCTL RGB bit
-	        RGB-BGR ORDER color filter panel: 0=RGB, 1=BGR */
+		RGB-BGR ORDER color filter panel: 0=RGB, 1=BGR */
 	switch (par->info->var.rotate) {
 	case 0:
 		write_reg(par, 0x36, MX | MY | (par->bgr << 3));
diff --git a/drivers/staging/fbtft/fb_tinylcd.c b/drivers/staging/fbtft/fb_tinylcd.c
index c0b1a337fafd..4c740b7e9f53 100644
--- a/drivers/staging/fbtft/fb_tinylcd.c
+++ b/drivers/staging/fbtft/fb_tinylcd.c
@@ -53,7 +53,7 @@ static int init_display(struct fbtft_par *par)
 	write_reg(par, 0xE5, 0x00);
 	write_reg(par, 0xF0, 0x36, 0xA5, 0x53);
 	write_reg(par, 0xE0, 0x00, 0x35, 0x33, 0x00, 0x00, 0x00,
-	                     0x00, 0x35, 0x33, 0x00, 0x00, 0x00);
+			     0x00, 0x35, 0x33, 0x00, 0x00, 0x00);
 	write_reg(par, 0x3A, 0x55);
 	write_reg(par, 0x11);
 	udelay(250);
diff --git a/drivers/staging/fbtft/fb_tls8204.c b/drivers/staging/fbtft/fb_tls8204.c
index fcd38bf2ed79..3253a25e9184 100644
--- a/drivers/staging/fbtft/fb_tls8204.c
+++ b/drivers/staging/fbtft/fb_tls8204.c
@@ -35,7 +35,9 @@
 #define WIDTH		84
 #define HEIGHT		48
 #define TXBUFLEN	WIDTH
-#define DEFAULT_GAMMA	"40" /* gamma is used to control contrast in this driver */
+
+/* gamma is used to control contrast in this driver */
+#define DEFAULT_GAMMA	"40"
 
 static unsigned bs = 4;
 module_param(bs, uint, 0);
@@ -51,7 +53,8 @@ static int init_display(struct fbtft_par *par)
 	write_reg(par, 0x21); /* 5:1  1
 				 2:0  PD - Powerdown control: chip is active
 				 1:0  V  - Entry mode: horizontal addressing
-				 0:1  H  - Extended instruction set control: extended
+				 0:1  H  - Extended instruction set control:
+						extended
 			      */
 
 	/* H=1 Bias system */
@@ -83,7 +86,9 @@ static int init_display(struct fbtft_par *par)
 
 static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
 {
-	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par, "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
+	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par,
+		      "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n",
+		      __func__, xs, ys, xe, ye);
 
 	/* H=0 Set X address of RAM */
 	write_reg(par, 0x80); /* 7:1  1
@@ -115,6 +120,7 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 
 		for (x = 0; x < WIDTH; x++) {
 			u8 ch = 0;
+
 			for (i = 0; i < 8*WIDTH; i += WIDTH) {
 				ch >>= 1;
 				if (vmem16[(y*8*WIDTH)+i+x])
diff --git a/drivers/staging/fbtft/fbtft-bus.c b/drivers/staging/fbtft/fbtft-bus.c
index 52af9cbbc2a6..912c6328fb87 100644
--- a/drivers/staging/fbtft/fbtft-bus.c
+++ b/drivers/staging/fbtft/fbtft-bus.c
@@ -184,7 +184,7 @@ EXPORT_SYMBOL(fbtft_write_vmem16_bus8);
 /* 16 bit pixel over 9-bit SPI bus: dc + high byte, dc + low byte */
 int fbtft_write_vmem16_bus9(struct fbtft_par *par, size_t offset, size_t len)
 {
-	u8 *vmem8;
+	u8 __iomem *vmem8;
 	u16 *txbuf16 = par->txbuf.buf;
 	size_t remain;
 	size_t to_copy;
@@ -212,12 +212,12 @@ int fbtft_write_vmem16_bus9(struct fbtft_par *par, size_t offset, size_t len)
 
 #ifdef __LITTLE_ENDIAN
 		for (i = 0; i < to_copy; i += 2) {
-			txbuf16[i]   = 0x0100 | vmem8[i+1];
-			txbuf16[i+1] = 0x0100 | vmem8[i];
+			txbuf16[i]     = 0x0100 | ioread8(vmem8 + i + 1);
+			txbuf16[i + 1] = 0x0100 | ioread8(vmem8 + i);
 		}
 #else
 		for (i = 0; i < to_copy; i++)
-			txbuf16[i]   = 0x0100 | vmem8[i];
+			txbuf16[i]   = 0x0100 | ioread8(vmem8 + i);
 #endif
 		vmem8 = vmem8 + to_copy;
 		ret = par->fbtftops.write(par, par->txbuf.buf, to_copy*2);
diff --git a/drivers/staging/fbtft/fbtft-core.c b/drivers/staging/fbtft/fbtft-core.c
index 53b748be2712..9cc81412be69 100644
--- a/drivers/staging/fbtft/fbtft-core.c
+++ b/drivers/staging/fbtft/fbtft-core.c
@@ -47,9 +47,11 @@ static unsigned long debug;
 module_param(debug, ulong, 0);
 MODULE_PARM_DESC(debug, "override device debug level");
 
+#ifdef CONFIG_HAS_DMA
 static bool dma = true;
 module_param(dma, bool, 0);
 MODULE_PARM_DESC(dma, "Use DMA buffer");
+#endif
 
 
 void fbtft_dbg_hex(const struct device *dev, int groupsize,
@@ -484,7 +486,7 @@ static void fbtft_deferred_io(struct fb_info *info, struct list_head *pagelist)
 		index = page->index << PAGE_SHIFT;
 		y_low = index / info->fix.line_length;
 		y_high = (index + PAGE_SIZE - 1) / info->fix.line_length;
-		fbtft_dev_dbg(DEBUG_DEFERRED_IO, par, info->device,
+		dev_dbg(info->device,
 			"page->index=%lu y_low=%d y_high=%d\n",
 			page->index, y_low, y_high);
 		if (y_high > info->var.yres - 1)
@@ -505,7 +507,7 @@ static void fbtft_fb_fillrect(struct fb_info *info,
 {
 	struct fbtft_par *par = info->par;
 
-	fbtft_dev_dbg(DEBUG_FB_FILLRECT, par, info->dev,
+	dev_dbg(info->dev,
 		"%s: dx=%d, dy=%d, width=%d, height=%d\n",
 		__func__, rect->dx, rect->dy, rect->width, rect->height);
 	sys_fillrect(info, rect);
@@ -518,7 +520,7 @@ static void fbtft_fb_copyarea(struct fb_info *info,
 {
 	struct fbtft_par *par = info->par;
 
-	fbtft_dev_dbg(DEBUG_FB_COPYAREA, par, info->dev,
+	dev_dbg(info->dev,
 		"%s: dx=%d, dy=%d, width=%d, height=%d\n",
 		__func__,  area->dx, area->dy, area->width, area->height);
 	sys_copyarea(info, area);
@@ -531,7 +533,7 @@ static void fbtft_fb_imageblit(struct fb_info *info,
 {
 	struct fbtft_par *par = info->par;
 
-	fbtft_dev_dbg(DEBUG_FB_IMAGEBLIT, par, info->dev,
+	dev_dbg(info->dev,
 		"%s: dx=%d, dy=%d, width=%d, height=%d\n",
 		__func__,  image->dx, image->dy, image->width, image->height);
 	sys_imageblit(info, image);
@@ -545,7 +547,7 @@ static ssize_t fbtft_fb_write(struct fb_info *info, const char __user *buf,
 	struct fbtft_par *par = info->par;
 	ssize_t res;
 
-	fbtft_dev_dbg(DEBUG_FB_WRITE, par, info->dev,
+	dev_dbg(info->dev,
 		"%s: count=%zd, ppos=%llu\n", __func__,  count, *ppos);
 	res = fb_sys_write(info, buf, count, ppos);
 
@@ -568,11 +570,10 @@ static int fbtft_fb_setcolreg(unsigned regno, unsigned red, unsigned green,
 			      unsigned blue, unsigned transp,
 			      struct fb_info *info)
 {
-	struct fbtft_par *par = info->par;
 	unsigned val;
 	int ret = 1;
 
-	fbtft_dev_dbg(DEBUG_FB_SETCOLREG, par, info->dev,
+	dev_dbg(info->dev,
 		"%s(regno=%u, red=0x%X, green=0x%X, blue=0x%X, trans=0x%X)\n",
 		__func__, regno, red, green, blue, transp);
 
@@ -599,7 +600,7 @@ static int fbtft_fb_blank(int blank, struct fb_info *info)
 	struct fbtft_par *par = info->par;
 	int ret = -EINVAL;
 
-	fbtft_dev_dbg(DEBUG_FB_BLANK, par, info->dev, "%s(blank=%d)\n",
+	dev_dbg(info->dev, "%s(blank=%d)\n",
 		__func__, blank);
 
 	if (!par->fbtftops.blank)
@@ -856,10 +857,13 @@ struct fb_info *fbtft_framebuffer_alloc(struct fbtft_display *display,
 #endif
 
 	if (txbuflen > 0) {
+#ifdef CONFIG_HAS_DMA
 		if (dma) {
 			dev->coherent_dma_mask = ~0;
 			txbuf = dmam_alloc_coherent(dev, txbuflen, &par->txbuf.dma, GFP_DMA);
-		} else {
+		} else
+#endif
+		{
 			txbuf = devm_kzalloc(par->info->device, txbuflen, GFP_KERNEL);
 		}
 		if (!txbuf)
@@ -1062,8 +1066,6 @@ static int fbtft_init_display_dt(struct fbtft_par *par)
 	const __be32 *p;
 	u32 val;
 	int buf[64], i, j;
-	char msg[128];
-	char str[16];
 
 	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);
 
@@ -1089,13 +1091,11 @@ static int fbtft_init_display_dt(struct fbtft_par *par)
 				p = of_prop_next_u32(prop, p, &val);
 			}
 			/* make debug message */
-			msg[0] = '\0';
-			for (j = 0; j < i; j++) {
-				snprintf(str, 128, " %02X", buf[j]);
-				strcat(msg, str);
-			}
 			fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
-				"init: write_register:%s\n", msg);
+				"init: write_register:\n");
+			for (j = 0; j < i; j++)
+				fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
+					      "buf[%d] = %02X\n", j, buf[j]);
 
 			par->fbtftops.write_register(par, i,
 				buf[0], buf[1], buf[2], buf[3],
diff --git a/drivers/staging/fbtft/fbtft.h b/drivers/staging/fbtft/fbtft.h
index 9fd98cb53418..7d817eb26eab 100644
--- a/drivers/staging/fbtft/fbtft.h
+++ b/drivers/staging/fbtft/fbtft.h
@@ -430,11 +430,6 @@ do {                                                         \
 		dev_info(par->info->device, format, ##arg);  \
 } while (0)
 
-#define fbtft_dev_dbg(level, par, dev, format, arg...)       \
-do {                                                         \
-	if (unlikely(par->debug & level))                    \
-		dev_info(dev, format, ##arg);                \
-} while (0)
 
 #define fbtft_par_dbg_hex(level, par, dev, type, buf, num, format, arg...) \
 do {                                                                       \
diff --git a/drivers/staging/fbtft/fbtft_device.c b/drivers/staging/fbtft/fbtft_device.c
index df6cd775ac1e..211d504901f2 100644
--- a/drivers/staging/fbtft/fbtft_device.c
+++ b/drivers/staging/fbtft/fbtft_device.c
@@ -34,8 +34,7 @@ static struct platform_device *p_device;
 
 static char *name;
 module_param(name, charp, 0);
-MODULE_PARM_DESC(name, "Devicename (required). " \
-"name=list => list all supported devices.");
+MODULE_PARM_DESC(name, "Devicename (required). name=list => list all supported devices.");
 
 static unsigned rotate;
 module_param(rotate, uint, 0);
@@ -61,8 +60,7 @@ MODULE_PARM_DESC(mode, "SPI mode (override device default)");
 static char *gpios;
 module_param(gpios, charp, 0);
 MODULE_PARM_DESC(gpios,
-"List of gpios. Comma separated with the form: reset:23,dc:24 " \
-"(when overriding the default, all gpios must be specified)");
+"List of gpios. Comma separated with the form: reset:23,dc:24 (when overriding the default, all gpios must be specified)");
 
 static unsigned fps;
 module_param(fps, uint, 0);
@@ -88,8 +86,7 @@ MODULE_PARM_DESC(startbyte, "Sets the Start byte used by some SPI displays.");
 
 static bool custom;
 module_param(custom, bool, 0);
-MODULE_PARM_DESC(custom, "Add a custom display device. " \
-"Use speed= argument to make it a SPI device, else platform_device");
+MODULE_PARM_DESC(custom, "Add a custom display device. Use speed= argument to make it a SPI device, else platform_device");
 
 static unsigned width;
 module_param(width, uint, 0);
@@ -232,7 +229,7 @@ static struct fbtft_device_display displays[] = {
 				.display = {
 					.buswidth = 8,
 					.backlight = 1,
-					.fbtftops.set_addr_win = \
+					.fbtftops.set_addr_win =
 					    adafruit18_green_tab_set_addr_win,
 				},
 				.bgr = true,
@@ -558,8 +555,8 @@ static struct fbtft_device_display displays[] = {
 				.gpios = (const struct fbtft_gpio []) {
 					/* Wiring for LCD adapter kit */
 					{ "reset", 7 },
-					{ "dc", 0 }, 	/* rev 2: 2 */
-					{ "wr", 1 }, 	/* rev 2: 3 */
+					{ "dc", 0 },	/* rev 2: 2 */
+					{ "wr", 1 },	/* rev 2: 3 */
 					{ "cs", 8 },
 					{ "db00", 17 },
 					{ "db01", 18 },
@@ -776,13 +773,13 @@ static struct fbtft_device_display displays[] = {
 					{ "dc", 25 },
 					{},
 				},
-				.gamma =	"0 2 2 2 2 2 2 2 " \
-						"2 2 2 2 2 2 2 2 " \
-						"2 2 2 2 2 2 2 2 " \
-						"2 2 2 2 2 2 2 3 " \
-						"3 3 3 3 3 3 3 3 " \
-						"3 3 3 3 3 3 3 3 " \
-						"3 3 3 4 4 4 4 4 " \
+				.gamma =	"0 2 2 2 2 2 2 2 "
+						"2 2 2 2 2 2 2 2 "
+						"2 2 2 2 2 2 2 2 "
+						"2 2 2 2 2 2 2 3 "
+						"3 3 3 3 3 3 3 3 "
+						"3 3 3 3 3 3 3 3 "
+						"3 3 3 4 4 4 4 4 "
 						"4 4 4 4 4 4 4"
 			}
 		}
@@ -896,7 +893,7 @@ static struct fbtft_device_display displays[] = {
 					.buswidth = 16,
 					.txbuflen = -2, /* disable buffer */
 					.backlight = 1,
-					.fbtftops.write = \
+					.fbtftops.write =
 						fbtft_write_gpio16_wr_latched,
 				},
 				.bgr = true,
@@ -1063,7 +1060,8 @@ static struct fbtft_device_display displays[] = {
 				.display = {
 					.buswidth = 8,
 					.backlight = 1,
-					.init_sequence = waveshare32b_init_sequence,
+					.init_sequence =
+						waveshare32b_init_sequence,
 				},
 				.bgr = true,
 				.gpios = (const struct fbtft_gpio []) {
@@ -1291,7 +1289,7 @@ static int __init fbtft_device_init(void)
 	}
 
 	if (init_num > FBTFT_MAX_INIT_SEQUENCE) {
-		pr_err(DRVNAME \
+		pr_err(DRVNAME
 			":  init parameter: exceeded max array size: %d\n",
 			FBTFT_MAX_INIT_SEQUENCE);
 		return -EINVAL;
@@ -1300,7 +1298,7 @@ static int __init fbtft_device_init(void)
 	/* parse module parameter: gpios */
 	while ((p_gpio = strsep(&gpios, ","))) {
 		if (strchr(p_gpio, ':') == NULL) {
-			pr_err(DRVNAME \
+			pr_err(DRVNAME
 				":  error: missing ':' in gpios parameter: %s\n",
 				p_gpio);
 			return -EINVAL;
@@ -1308,14 +1306,14 @@ static int __init fbtft_device_init(void)
 		p_num = p_gpio;
 		p_name = strsep(&p_num, ":");
 		if (p_name == NULL || p_num == NULL) {
-			pr_err(DRVNAME \
+			pr_err(DRVNAME
 				":  something bad happened parsing gpios parameter: %s\n",
 				p_gpio);
 			return -EINVAL;
 		}
 		ret = kstrtol(p_num, 10, &val);
 		if (ret) {
-			pr_err(DRVNAME \
+			pr_err(DRVNAME
 				":  could not parse number in gpios parameter: %s:%s\n",
 				p_name, p_num);
 			return -EINVAL;
@@ -1323,7 +1321,7 @@ static int __init fbtft_device_init(void)
 		strcpy(fbtft_device_param_gpios[i].name, p_name);
 		fbtft_device_param_gpios[i++].gpio = (int) val;
 		if (i == MAX_GPIOS) {
-			pr_err(DRVNAME \
+			pr_err(DRVNAME
 				":  gpios parameter: exceeded max array size: %d\n",
 				MAX_GPIOS);
 			return -EINVAL;
@@ -1419,23 +1417,21 @@ static int __init fbtft_device_init(void)
 			if (displays[i].spi) {
 				ret = fbtft_device_spi_device_register(spi);
 				if (ret) {
-					pr_err(DRVNAME \
+					pr_err(DRVNAME
 						": failed to register SPI device\n");
 					return ret;
 				}
-				found = true;
-				break;
 			} else {
 				ret = platform_device_register(p_device);
 				if (ret < 0) {
-					pr_err(DRVNAME \
+					pr_err(DRVNAME
 						":    platform_device_register() returned %d\n",
 						ret);
 					return ret;
 				}
-				found = true;
-				break;
 			}
+			found = true;
+			break;
 		}
 	}
 
diff --git a/drivers/staging/fbtft/flexfb.c b/drivers/staging/fbtft/flexfb.c
index ca39fe90d1b8..2c4ce07f5106 100644
--- a/drivers/staging/fbtft/flexfb.c
+++ b/drivers/staging/fbtft/flexfb.c
@@ -73,70 +73,73 @@ static int *initp;
 static int initp_num;
 
 /* default init sequences */
-static int st7735r_init[] = { \
--1,0x01,-2,150,-1,0x11,-2,500,-1,0xB1,0x01,0x2C,0x2D,-1,0xB2,0x01,0x2C,0x2D,-1,0xB3,0x01,0x2C,0x2D,0x01,0x2C,0x2D, \
--1,0xB4,0x07,-1,0xC0,0xA2,0x02,0x84,-1,0xC1,0xC5,-1,0xC2,0x0A,0x00,-1,0xC3,0x8A,0x2A,-1,0xC4,0x8A,0xEE,-1,0xC5,0x0E, \
--1,0x20,-1,0x36,0xC0,-1,0x3A,0x05,-1,0xE0,0x0f,0x1a,0x0f,0x18,0x2f,0x28,0x20,0x22,0x1f,0x1b,0x23,0x37,0x00,0x07,0x02,0x10, \
--1,0xE1,0x0f,0x1b,0x0f,0x17,0x33,0x2c,0x29,0x2e,0x30,0x30,0x39,0x3f,0x00,0x07,0x03,0x10,-1,0x29,-2,100,-1,0x13,-2,10,-3 };
-
-static int ssd1289_init[] = { \
--1,0x00,0x0001,-1,0x03,0xA8A4,-1,0x0C,0x0000,-1,0x0D,0x080C,-1,0x0E,0x2B00,-1,0x1E,0x00B7,-1,0x01,0x2B3F,-1,0x02,0x0600, \
--1,0x10,0x0000,-1,0x11,0x6070,-1,0x05,0x0000,-1,0x06,0x0000,-1,0x16,0xEF1C,-1,0x17,0x0003,-1,0x07,0x0233,-1,0x0B,0x0000, \
--1,0x0F,0x0000,-1,0x41,0x0000,-1,0x42,0x0000,-1,0x48,0x0000,-1,0x49,0x013F,-1,0x4A,0x0000,-1,0x4B,0x0000,-1,0x44,0xEF00, \
--1,0x45,0x0000,-1,0x46,0x013F,-1,0x30,0x0707,-1,0x31,0x0204,-1,0x32,0x0204,-1,0x33,0x0502,-1,0x34,0x0507,-1,0x35,0x0204, \
--1,0x36,0x0204,-1,0x37,0x0502,-1,0x3A,0x0302,-1,0x3B,0x0302,-1,0x23,0x0000,-1,0x24,0x0000,-1,0x25,0x8000,-1,0x4f,0x0000, \
--1,0x4e,0x0000,-1,0x22,-3 };
-
-static int hx8340bn_init[] = { \
--1,0xC1,0xFF,0x83,0x40,-1,0x11,-2,150,-1,0xCA,0x70,0x00,0xD9,-1,0xB0,0x01,0x11, \
--1,0xC9,0x90,0x49,0x10,0x28,0x28,0x10,0x00,0x06,-2,20,-1,0xC2,0x60,0x71,0x01,0x0E,0x05,0x02,0x09,0x31,0x0A, \
--1,0xC3,0x67,0x30,0x61,0x17,0x48,0x07,0x05,0x33,-2,10,-1,0xB5,0x35,0x20,0x45,-1,0xB4,0x33,0x25,0x4C,-2,10, \
--1,0x3A,0x05,-1,0x29,-2,10,-3 };
-
-static int ili9225_init[] = { \
--1,0x0001,0x011C,-1,0x0002,0x0100,-1,0x0003,0x1030,-1,0x0008,0x0808,-1,0x000C,0x0000,-1,0x000F,0x0A01,-1,0x0020,0x0000, \
--1,0x0021,0x0000,-2,50,-1,0x0010,0x0A00,-1,0x0011,0x1038,-2,50,-1,0x0012,0x1121,-1,0x0013,0x004E,-1,0x0014,0x676F, \
--1,0x0030,0x0000,-1,0x0031,0x00DB,-1,0x0032,0x0000,-1,0x0033,0x0000,-1,0x0034,0x00DB,-1,0x0035,0x0000,-1,0x0036,0x00AF, \
--1,0x0037,0x0000,-1,0x0038,0x00DB,-1,0x0039,0x0000,-1,0x0050,0x0000,-1,0x0051,0x060A,-1,0x0052,0x0D0A,-1,0x0053,0x0303, \
--1,0x0054,0x0A0D,-1,0x0055,0x0A06,-1,0x0056,0x0000,-1,0x0057,0x0303,-1,0x0058,0x0000,-1,0x0059,0x0000,-2,50, \
--1,0x0007,0x1017,-2,50,-3 };
-
-static int ili9320_init[] = { \
--1,0x00E5,0x8000,-1,0x0000,0x0001,-1,0x0001,0x0100,-1,0x0002,0x0700,-1,0x0003,0x1030,-1,0x0004,0x0000,-1,0x0008,0x0202, \
--1,0x0009,0x0000,-1,0x000A,0x0000,-1,0x000C,0x0000,-1,0x000D,0x0000,-1,0x000F,0x0000,-1,0x0010,0x0000,-1,0x0011,0x0007, \
--1,0x0012,0x0000,-1,0x0013,0x0000,-2,200,-1,0x0010,0x17B0,-1,0x0011,0x0031,-2,50,-1,0x0012,0x0138,-2,50,-1,0x0013,0x1800, \
--1,0x0029,0x0008,-2,50,-1,0x0020,0x0000,-1,0x0021,0x0000,-1,0x0030,0x0000,-1,0x0031,0x0505,-1,0x0032,0x0004, \
--1,0x0035,0x0006,-1,0x0036,0x0707,-1,0x0037,0x0105,-1,0x0038,0x0002,-1,0x0039,0x0707,-1,0x003C,0x0704,-1,0x003D,0x0807, \
--1,0x0050,0x0000,-1,0x0051,0x00EF,-1,0x0052,0x0000,-1,0x0053,0x013F,-1,0x0060,0x2700,-1,0x0061,0x0001,-1,0x006A,0x0000, \
--1,0x0080,0x0000,-1,0x0081,0x0000,-1,0x0082,0x0000,-1,0x0083,0x0000,-1,0x0084,0x0000,-1,0x0085,0x0000,-1,0x0090,0x0010, \
--1,0x0092,0x0000,-1,0x0093,0x0003,-1,0x0095,0x0110,-1,0x0097,0x0000,-1,0x0098,0x0000,-1,0x0007,0x0173,-3 };
-
-static int ili9325_init[] = { \
--1,0x00E3,0x3008,-1,0x00E7,0x0012,-1,0x00EF,0x1231,-1,0x0001,0x0100,-1,0x0002,0x0700,-1,0x0003,0x1030,-1,0x0004,0x0000, \
--1,0x0008,0x0207,-1,0x0009,0x0000,-1,0x000A,0x0000,-1,0x000C,0x0000,-1,0x000D,0x0000,-1,0x000F,0x0000,-1,0x0010,0x0000, \
--1,0x0011,0x0007,-1,0x0012,0x0000,-1,0x0013,0x0000,-2,200,-1,0x0010,0x1690,-1,0x0011,0x0223,-2,50,-1,0x0012,0x000D,-2,50, \
--1,0x0013,0x1200,-1,0x0029,0x000A,-1,0x002B,0x000C,-2,50,-1,0x0020,0x0000,-1,0x0021,0x0000,-1,0x0030,0x0000, \
--1,0x0031,0x0506,-1,0x0032,0x0104,-1,0x0035,0x0207,-1,0x0036,0x000F,-1,0x0037,0x0306,-1,0x0038,0x0102,-1,0x0039,0x0707, \
--1,0x003C,0x0702,-1,0x003D,0x1604,-1,0x0050,0x0000,-1,0x0051,0x00EF,-1,0x0052,0x0000,-1,0x0053,0x013F,-1,0x0060,0xA700, \
--1,0x0061,0x0001,-1,0x006A,0x0000,-1,0x0080,0x0000,-1,0x0081,0x0000,-1,0x0082,0x0000,-1,0x0083,0x0000,-1,0x0084,0x0000, \
--1,0x0085,0x0000,-1,0x0090,0x0010,-1,0x0092,0x0600,-1,0x0007,0x0133,-3 };
-
-static int ili9341_init[] = { \
--1,0x28,-2,20,-1,0xCF,0x00,0x83,0x30,-1,0xED,0x64,0x03,0x12,0x81,-1,0xE8,0x85,0x01,0x79, \
--1,0xCB,0x39,0x2c,0x00,0x34,0x02,-1,0xF7,0x20,-1,0xEA,0x00,0x00,-1,0xC0,0x26,-1,0xC1,0x11, \
--1,0xC5,0x35,0x3E,-1,0xC7,0xBE,-1,0xB1,0x00,0x1B,-1,0xB6,0x0a,0x82,0x27,0x00,-1,0xB7,0x07, \
--1,0x3A,0x55,-1,0x36,0x48,-1,0x11,-2,120,-1,0x29,-2,20,-3 };
-
-static int ssd1351_init[] = { -1,0xfd,0x12,-1,0xfd,0xb1,-1,0xae,-1,0xb3,0xf1,-1,0xca,0x7f,-1,0xa0,0x74, \
-                              -1,0x15,0x00,0x7f,-1,0x75,0x00,0x7f,-1,0xa1,0x00,-1,0xa2,0x00,-1,0xb5,0x00, \
-                              -1,0xab,0x01,-1,0xb1,0x32,-1,0xb4,0xa0,0xb5,0x55,-1,0xbb,0x17,-1,0xbe,0x05, \
-                              -1,0xc1,0xc8,0x80,0xc8,-1,0xc7,0x0f,-1,0xb6,0x01,-1,0xa6,-1,0xaf,-3 };
+static int st7735r_init[] = {
+-1, 0x01, -2, 150, -1, 0x11, -2, 500, -1, 0xB1, 0x01, 0x2C, 0x2D, -1, 0xB2, 0x01, 0x2C, 0x2D, -1, 0xB3, 0x01, 0x2C, 0x2D, 0x01, 0x2C, 0x2D,
+-1, 0xB4, 0x07, -1, 0xC0, 0xA2, 0x02, 0x84, -1, 0xC1, 0xC5, -1, 0xC2, 0x0A, 0x00, -1, 0xC3, 0x8A, 0x2A, -1, 0xC4, 0x8A, 0xEE, -1, 0xC5, 0x0E,
+-1, 0x20, -1, 0x36, 0xC0, -1, 0x3A, 0x05, -1, 0xE0, 0x0f, 0x1a, 0x0f, 0x18, 0x2f, 0x28, 0x20, 0x22, 0x1f, 0x1b, 0x23, 0x37, 0x00, 0x07, 0x02, 0x10,
+-1, 0xE1, 0x0f, 0x1b, 0x0f, 0x17, 0x33, 0x2c, 0x29, 0x2e, 0x30, 0x30, 0x39, 0x3f, 0x00, 0x07, 0x03, 0x10, -1, 0x29, -2, 100, -1, 0x13, -2, 10, -3 };
+
+static int ssd1289_init[] = {
+-1, 0x00, 0x0001, -1, 0x03, 0xA8A4, -1, 0x0C, 0x0000, -1, 0x0D, 0x080C, -1, 0x0E, 0x2B00, -1, 0x1E, 0x00B7, -1, 0x01, 0x2B3F, -1, 0x02, 0x0600,
+-1, 0x10, 0x0000, -1, 0x11, 0x6070, -1, 0x05, 0x0000, -1, 0x06, 0x0000, -1, 0x16, 0xEF1C, -1, 0x17, 0x0003, -1, 0x07, 0x0233, -1, 0x0B, 0x0000,
+-1, 0x0F, 0x0000, -1, 0x41, 0x0000, -1, 0x42, 0x0000, -1, 0x48, 0x0000, -1, 0x49, 0x013F, -1, 0x4A, 0x0000, -1, 0x4B, 0x0000, -1, 0x44, 0xEF00,
+-1, 0x45, 0x0000, -1, 0x46, 0x013F, -1, 0x30, 0x0707, -1, 0x31, 0x0204, -1, 0x32, 0x0204, -1, 0x33, 0x0502, -1, 0x34, 0x0507, -1, 0x35, 0x0204,
+-1, 0x36, 0x0204, -1, 0x37, 0x0502, -1, 0x3A, 0x0302, -1, 0x3B, 0x0302, -1, 0x23, 0x0000, -1, 0x24, 0x0000, -1, 0x25, 0x8000, -1, 0x4f, 0x0000,
+-1, 0x4e, 0x0000, -1, 0x22, -3 };
+
+static int hx8340bn_init[] = {
+-1, 0xC1, 0xFF, 0x83, 0x40, -1, 0x11, -2, 150, -1, 0xCA, 0x70, 0x00, 0xD9, -1, 0xB0, 0x01, 0x11,
+-1, 0xC9, 0x90, 0x49, 0x10, 0x28, 0x28, 0x10, 0x00, 0x06, -2, 20, -1, 0xC2, 0x60, 0x71, 0x01, 0x0E, 0x05, 0x02, 0x09, 0x31, 0x0A,
+-1, 0xC3, 0x67, 0x30, 0x61, 0x17, 0x48, 0x07, 0x05, 0x33, -2, 10, -1, 0xB5, 0x35, 0x20, 0x45, -1, 0xB4, 0x33, 0x25, 0x4C, -2, 10,
+-1, 0x3A, 0x05, -1, 0x29, -2, 10, -3 };
+
+static int ili9225_init[] = {
+-1, 0x0001, 0x011C, -1, 0x0002, 0x0100, -1, 0x0003, 0x1030, -1, 0x0008, 0x0808, -1, 0x000C, 0x0000, -1, 0x000F, 0x0A01, -1, 0x0020, 0x0000,
+-1, 0x0021, 0x0000, -2, 50, -1, 0x0010, 0x0A00, -1, 0x0011, 0x1038, -2, 50, -1, 0x0012, 0x1121, -1, 0x0013, 0x004E, -1, 0x0014, 0x676F,
+-1, 0x0030, 0x0000, -1, 0x0031, 0x00DB, -1, 0x0032, 0x0000, -1, 0x0033, 0x0000, -1, 0x0034, 0x00DB, -1, 0x0035, 0x0000, -1, 0x0036, 0x00AF,
+-1, 0x0037, 0x0000, -1, 0x0038, 0x00DB, -1, 0x0039, 0x0000, -1, 0x0050, 0x0000, -1, 0x0051, 0x060A, -1, 0x0052, 0x0D0A, -1, 0x0053, 0x0303,
+-1, 0x0054, 0x0A0D, -1, 0x0055, 0x0A06, -1, 0x0056, 0x0000, -1, 0x0057, 0x0303, -1, 0x0058, 0x0000, -1, 0x0059, 0x0000, -2, 50,
+-1, 0x0007, 0x1017, -2, 50, -3 };
+
+static int ili9320_init[] = {
+-1, 0x00E5, 0x8000, -1, 0x0000, 0x0001, -1, 0x0001, 0x0100, -1, 0x0002, 0x0700, -1, 0x0003, 0x1030, -1, 0x0004, 0x0000, -1, 0x0008, 0x0202,
+-1, 0x0009, 0x0000, -1, 0x000A, 0x0000, -1, 0x000C, 0x0000, -1, 0x000D, 0x0000, -1, 0x000F, 0x0000, -1, 0x0010, 0x0000, -1, 0x0011, 0x0007,
+-1, 0x0012, 0x0000, -1, 0x0013, 0x0000, -2, 200, -1, 0x0010, 0x17B0, -1, 0x0011, 0x0031, -2, 50, -1, 0x0012, 0x0138, -2, 50, -1, 0x0013, 0x1800,
+-1, 0x0029, 0x0008, -2, 50, -1, 0x0020, 0x0000, -1, 0x0021, 0x0000, -1, 0x0030, 0x0000, -1, 0x0031, 0x0505, -1, 0x0032, 0x0004,
+-1, 0x0035, 0x0006, -1, 0x0036, 0x0707, -1, 0x0037, 0x0105, -1, 0x0038, 0x0002, -1, 0x0039, 0x0707, -1, 0x003C, 0x0704, -1, 0x003D, 0x0807,
+-1, 0x0050, 0x0000, -1, 0x0051, 0x00EF, -1, 0x0052, 0x0000, -1, 0x0053, 0x013F, -1, 0x0060, 0x2700, -1, 0x0061, 0x0001, -1, 0x006A, 0x0000,
+-1, 0x0080, 0x0000, -1, 0x0081, 0x0000, -1, 0x0082, 0x0000, -1, 0x0083, 0x0000, -1, 0x0084, 0x0000, -1, 0x0085, 0x0000, -1, 0x0090, 0x0010,
+-1, 0x0092, 0x0000, -1, 0x0093, 0x0003, -1, 0x0095, 0x0110, -1, 0x0097, 0x0000, -1, 0x0098, 0x0000, -1, 0x0007, 0x0173, -3 };
+
+static int ili9325_init[] = {
+-1, 0x00E3, 0x3008, -1, 0x00E7, 0x0012, -1, 0x00EF, 0x1231, -1, 0x0001, 0x0100, -1, 0x0002, 0x0700, -1, 0x0003, 0x1030, -1, 0x0004, 0x0000,
+-1, 0x0008, 0x0207, -1, 0x0009, 0x0000, -1, 0x000A, 0x0000, -1, 0x000C, 0x0000, -1, 0x000D, 0x0000, -1, 0x000F, 0x0000, -1, 0x0010, 0x0000,
+-1, 0x0011, 0x0007, -1, 0x0012, 0x0000, -1, 0x0013, 0x0000, -2, 200, -1, 0x0010, 0x1690, -1, 0x0011, 0x0223, -2, 50, -1, 0x0012, 0x000D, -2, 50,
+-1, 0x0013, 0x1200, -1, 0x0029, 0x000A, -1, 0x002B, 0x000C, -2, 50, -1, 0x0020, 0x0000, -1, 0x0021, 0x0000, -1, 0x0030, 0x0000,
+-1, 0x0031, 0x0506, -1, 0x0032, 0x0104, -1, 0x0035, 0x0207, -1, 0x0036, 0x000F, -1, 0x0037, 0x0306, -1, 0x0038, 0x0102, -1, 0x0039, 0x0707,
+-1, 0x003C, 0x0702, -1, 0x003D, 0x1604, -1, 0x0050, 0x0000, -1, 0x0051, 0x00EF, -1, 0x0052, 0x0000, -1, 0x0053, 0x013F, -1, 0x0060, 0xA700,
+-1, 0x0061, 0x0001, -1, 0x006A, 0x0000, -1, 0x0080, 0x0000, -1, 0x0081, 0x0000, -1, 0x0082, 0x0000, -1, 0x0083, 0x0000, -1, 0x0084, 0x0000,
+-1, 0x0085, 0x0000, -1, 0x0090, 0x0010, -1, 0x0092, 0x0600, -1, 0x0007, 0x0133, -3 };
+
+static int ili9341_init[] = {
+-1, 0x28, -2, 20, -1, 0xCF, 0x00, 0x83, 0x30, -1, 0xED, 0x64, 0x03, 0x12, 0x81, -1, 0xE8, 0x85, 0x01, 0x79,
+-1, 0xCB, 0x39, 0x2c, 0x00, 0x34, 0x02, -1, 0xF7, 0x20, -1, 0xEA, 0x00, 0x00, -1, 0xC0, 0x26, -1, 0xC1, 0x11,
+-1, 0xC5, 0x35, 0x3E, -1, 0xC7, 0xBE, -1, 0xB1, 0x00, 0x1B, -1, 0xB6, 0x0a, 0x82, 0x27, 0x00, -1, 0xB7, 0x07,
+-1, 0x3A, 0x55, -1, 0x36, 0x48, -1, 0x11, -2, 120, -1, 0x29, -2, 20, -3 };
+
+static int ssd1351_init[] = { -1, 0xfd, 0x12, -1, 0xfd, 0xb1, -1, 0xae, -1, 0xb3, 0xf1, -1, 0xca, 0x7f, -1, 0xa0, 0x74,
+			      -1, 0x15, 0x00, 0x7f, -1, 0x75, 0x00, 0x7f, -1, 0xa1, 0x00, -1, 0xa2, 0x00, -1, 0xb5, 0x00,
+			      -1, 0xab, 0x01, -1, 0xb1, 0x32, -1, 0xb4, 0xa0, 0xb5, 0x55, -1, 0xbb, 0x17, -1, 0xbe, 0x05,
+			      -1, 0xc1, 0xc8, 0x80, 0xc8, -1, 0xc7, 0x0f, -1, 0xb6, 0x01, -1, 0xa6, -1, 0xaf, -3 };
 
 
 /* ili9320, ili9325 */
-static void flexfb_set_addr_win_1(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+static void flexfb_set_addr_win_1(struct fbtft_par *par,
+				  int xs, int ys, int xe, int ye)
 {
-	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par, "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
+	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par,
+		     "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n",
+		     __func__, xs, ys, xe, ye);
 	switch (par->info->var.rotate) {
 	/* R20h = Horizontal GRAM Start Address */
 	/* R21h = Vertical GRAM Start Address */
@@ -161,9 +164,12 @@ static void flexfb_set_addr_win_1(struct fbtft_par *par, int xs, int ys, int xe,
 }
 
 /* ssd1289 */
-static void flexfb_set_addr_win_2(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+static void flexfb_set_addr_win_2(struct fbtft_par *par,
+				  int xs, int ys, int xe, int ye)
 {
-	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par, "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
+	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par,
+		      "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n",
+		      __func__, xs, ys, xe, ye);
 
 	switch (par->info->var.rotate) {
 	/* R4Eh - Set GDDRAM X address counter */
@@ -191,9 +197,12 @@ static void flexfb_set_addr_win_2(struct fbtft_par *par, int xs, int ys, int xe,
 }
 
 /* ssd1351 */
-static void set_addr_win_3(struct fbtft_par *par, int xs, int ys, int xe, int ye)
+static void set_addr_win_3(struct fbtft_par *par,
+			   int xs, int ys, int xe, int ye)
 {
-	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par, "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__, xs, ys, xe, ye);
+	fbtft_par_dbg(DEBUG_SET_ADDR_WIN, par,
+		      "%s(xs=%d, ys=%d, xe=%d, ye=%d)\n", __func__,
+		      xs, ys, xe, ye);
 
 	write_reg(par, 0x15, xs, xe);
 	write_reg(par, 0x75, ys, ye);
@@ -205,7 +214,8 @@ static int flexfb_verify_gpios_dc(struct fbtft_par *par)
 	fbtft_par_dbg(DEBUG_VERIFY_GPIOS, par, "%s()\n", __func__);
 
 	if (par->gpio.dc < 0) {
-		dev_err(par->info->device, "Missing info about 'dc' gpio. Aborting.\n");
+		dev_err(par->info->device,
+			"Missing info about 'dc' gpio. Aborting.\n");
 		return -EINVAL;
 	}
 
@@ -232,10 +242,12 @@ static int flexfb_verify_gpios_db(struct fbtft_par *par)
 		return -EINVAL;
 	}
 	if (latched)
-		num_db=buswidth/2;
-	for (i=0;i < num_db;i++) {
+		num_db = buswidth/2;
+	for (i = 0; i < num_db; i++) {
 		if (par->gpio.db[i] < 0) {
-			dev_err(par->info->device, "Missing info about 'db%02d' gpio. Aborting.\n", i);
+			dev_err(par->info->device,
+				"Missing info about 'db%02d' gpio. Aborting.\n",
+				i);
 			return -EINVAL;
 		}
 	}
@@ -245,7 +257,8 @@ static int flexfb_verify_gpios_db(struct fbtft_par *par)
 
 static struct fbtft_display flex_display = { };
 
-static int flexfb_probe_common(struct spi_device *sdev, struct platform_device *pdev)
+static int flexfb_probe_common(struct spi_device *sdev,
+			       struct platform_device *pdev)
 {
 	struct device *dev;
 	struct fb_info *info;
@@ -260,7 +273,8 @@ static int flexfb_probe_common(struct spi_device *sdev, struct platform_device *
 	else
 		dev = &pdev->dev;
 
-	fbtft_init_dbg(dev, "%s(%s)\n", __func__, sdev ? "'SPI device'" : "'Platform device'");
+	fbtft_init_dbg(dev, "%s(%s)\n", __func__,
+		       sdev ? "'SPI device'" : "'Platform device'");
 
 	if (chip) {
 
@@ -403,7 +417,9 @@ static int flexfb_probe_common(struct spi_device *sdev, struct platform_device *
 		par->fbtftops.write_register = fbtft_write_reg16_bus8;
 		break;
 	default:
-		dev_err(dev, "argument 'regwidth': %d is not supported.\n", regwidth);
+		dev_err(dev,
+			"argument 'regwidth': %d is not supported.\n",
+			regwidth);
 		return -EINVAL;
 	}
 
@@ -483,7 +499,8 @@ static int flexfb_probe_common(struct spi_device *sdev, struct platform_device *
 		par->fbtftops.set_addr_win = set_addr_win_3;
 		break;
 	default:
-		dev_err(dev, "argument 'setaddrwin': unknown value %d.\n", setaddrwin);
+		dev_err(dev, "argument 'setaddrwin': unknown value %d.\n",
+			setaddrwin);
 		return -EINVAL;
 	}
 
diff --git a/drivers/staging/fbtft/internal.h b/drivers/staging/fbtft/internal.h
index f69db8289151..eea0ec5ff4d3 100644
--- a/drivers/staging/fbtft/internal.h
+++ b/drivers/staging/fbtft/internal.h
@@ -13,7 +13,7 @@
  *
  */
 
-#ifndef __LINUX_FBTFT__INTERNAL_H
+#ifndef __LINUX_FBTFT_INTERNAL_H
 #define __LINUX_FBTFT_INTERNAL_H
 
 void fbtft_sysfs_init(struct fbtft_par *par);
diff --git a/drivers/staging/fsl-mc/bus/mc-bus.c b/drivers/staging/fsl-mc/bus/mc-bus.c
index 23512d096427..766a65959b01 100644
--- a/drivers/staging/fsl-mc/bus/mc-bus.c
+++ b/drivers/staging/fsl-mc/bus/mc-bus.c
@@ -713,7 +713,6 @@ MODULE_DEVICE_TABLE(of, fsl_mc_bus_match_table);
 static struct platform_driver fsl_mc_bus_driver = {
 	.driver = {
 		   .name = "fsl_mc_bus",
-		   .owner = THIS_MODULE,
 		   .pm = NULL,
 		   .of_match_table = fsl_mc_bus_match_table,
 		   },
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
index 2d758fb26eac..409266b1a886 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
@@ -55,7 +55,7 @@ struct list_head freercvpool;
 /* lock to arbitrate free buffer list for receive command data */
 spinlock_t free_buff_lock;
 
-int numofmsgbuf = 0;
+int numofmsgbuf;
 
 /*
  * Table of entry-point routines for char device
@@ -317,7 +317,7 @@ static int ft1000_open(struct inode *inode, struct file *file)
 
 	/* Search for available application info block */
 	for (i = 0; i < MAX_NUM_APP; i++) {
-		if ((dev->app_info[i].fileobject == NULL))
+		if (dev->app_info[i].fileobject == NULL)
 			break;
 	}
 
@@ -588,8 +588,7 @@ static long ft1000_ioctl(struct file *file, unsigned int command,
 				/* 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 {
+				if (!qtype) {
 					/* 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);
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
index e6fb066e0dd2..e6b5976a09e3 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
@@ -399,8 +399,8 @@ int dsp_reload(struct ft1000_usb *ft1000dev)
 	card_reset_dsp(ft1000dev, 0);
 	msleep(1000);
 
-	status =
-		ft1000_write_register(ft1000dev, HOST_INTF_BE, FT1000_REG_SUP_CTRL);
+	status = ft1000_write_register(ft1000dev, HOST_INTF_BE,
+				       FT1000_REG_SUP_CTRL);
 
 	/* Let's check for FEFE */
 	status =
@@ -587,8 +587,8 @@ static int ft1000_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		goto err;
 	}
 
-	pipe =
-		usb_sndbulkpipe(pFt1000Dev->dev, pFt1000Dev->bulk_out_endpointAddr);
+	pipe = usb_sndbulkpipe(pFt1000Dev->dev,
+			       pFt1000Dev->bulk_out_endpointAddr);
 	maxlen = usb_maxpacket(pFt1000Dev->dev, pipe, usb_pipeout(pipe));
 
 	pdata = (u8 *)skb->data;
@@ -1049,8 +1049,8 @@ static int ft1000_dsp_prov(void *arg)
 
 		/* Check if doorbell is available */
 		pr_debug("check if doorbell is cleared\n");
-		status =
-			ft1000_read_register(dev, &tempword, FT1000_REG_DOORBELL);
+		status = ft1000_read_register(dev, &tempword,
+					      FT1000_REG_DOORBELL);
 		if (status) {
 			pr_debug("ft1000_read_register error\n");
 			break;
@@ -1071,9 +1071,8 @@ static int ft1000_dsp_prov(void *arg)
 			pr_debug("*** Provision Data Sent to DSP\n");
 
 			/* Send provisioning data */
-			ptr =
-				list_entry(info->prov_list.next, struct prov_record,
-					   list);
+			ptr = list_entry(info->prov_list.next,
+					 struct prov_record, list);
 			len = *(u16 *)ptr->pprov_data;
 			len = htons(len);
 			len += PSEUDOSZ;
@@ -1261,7 +1260,7 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 			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;
 			}
@@ -1295,8 +1294,8 @@ 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);
 		dev->DrvMsgPend = 0;
@@ -1355,8 +1354,9 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 			*pmsg++ = convert.wrd;
 			*pmsg++ = htons(info->DrvErrNum);
 
-			status = card_send_command(dev, (unsigned char *)&tempbuffer[0],
-						   (u16)(0x0012 + PSEUDOSZ));
+			status = card_send_command(dev,
+					(unsigned char *)&tempbuffer[0],
+					(u16)(0x0012 + PSEUDOSZ));
 			if (status)
 				goto out;
 			info->DrvErrNum = 0;
@@ -1526,7 +1526,7 @@ int ft1000_poll(void *dev_id)
 			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,
@@ -1542,9 +1542,9 @@ int ft1000_poll(void *dev_id)
 						       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
 			 * */
@@ -1556,21 +1556,21 @@ int ft1000_poll(void *dev_id)
 			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;
 				pr_debug("DSP conditional reset requested\n");
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
index 3b303b6b1b27..fd255c62083f 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
@@ -3,8 +3,6 @@
  *
  *
  * This file is part of Express Card USB Driver
- *
- * $Id:
  *====================================================
  */
 
diff --git a/drivers/staging/fwserial/dma_fifo.c b/drivers/staging/fwserial/dma_fifo.c
index 027906249598..7a3347c3d02b 100644
--- a/drivers/staging/fwserial/dma_fifo.c
+++ b/drivers/staging/fwserial/dma_fifo.c
@@ -56,7 +56,7 @@ void dma_fifo_init(struct dma_fifo *fifo)
  * @size: 'apparent' size, in bytes, of fifo
  * @align: dma alignment to maintain (should be at least cpu cache alignment),
  *         must be power of 2
- * @tx_limit: maximum # of bytes transmissable per dma (rounded down to
+ * @tx_limit: maximum # of bytes transmissible per dma (rounded down to
  *            multiple of alignment, but at least align size)
  * @open_limit: maximum # of outstanding dma transactions allowed
  * @gfp_mask: get_free_pages mask, passed to kmalloc()
diff --git a/drivers/staging/fwserial/fwserial.c b/drivers/staging/fwserial/fwserial.c
index fdb2418c5f88..b3ea4bb54e2c 100644
--- a/drivers/staging/fwserial/fwserial.c
+++ b/drivers/staging/fwserial/fwserial.c
@@ -2815,13 +2815,14 @@ static int __init fwserial_init(void)
 	/* num_ttys/num_ports must not be set above the static alloc avail */
 	if (num_ttys + num_loops > MAX_CARD_PORTS)
 		num_ttys = MAX_CARD_PORTS - num_loops;
+
 	num_ports = num_ttys + num_loops;
 
 	fwtty_driver = tty_alloc_driver(MAX_TOTAL_PORTS, TTY_DRIVER_REAL_RAW
 					| TTY_DRIVER_DYNAMIC_DEV);
 	if (IS_ERR(fwtty_driver)) {
 		err = PTR_ERR(fwtty_driver);
-		return err;
+		goto remove_debugfs;
 	}
 
 	fwtty_driver->driver_name	= KBUILD_MODNAME;
@@ -2923,7 +2924,9 @@ unregister_driver:
 	tty_unregister_driver(fwtty_driver);
 put_tty:
 	put_tty_driver(fwtty_driver);
+remove_debugfs:
 	debugfs_remove_recursive(fwserial_debugfs);
+
 	return err;
 }
 
diff --git a/drivers/staging/fwserial/fwserial.h b/drivers/staging/fwserial/fwserial.h
index 98b853d4acbc..787aa4f3a41b 100644
--- a/drivers/staging/fwserial/fwserial.h
+++ b/drivers/staging/fwserial/fwserial.h
@@ -218,7 +218,7 @@ struct fwconsole_ops {
  *         prevented with the IN_TX flag. Scheduled under lock to
  *         limit scheduling when fifo has just been drained.
  * @tx_fifo: fifo used to store & block-up writes for dma to remote
- * @max_payload: max bytes transmissable per dma (based on peer's max_payload)
+ * @max_payload: max bytes transmissible per dma (based on peer's max_payload)
  * @status_mask: UART_LSR_* bitmask significant to rx (based on termios)
  * @ignore_mask: UART_LSR_* bitmask of states to ignore (also based on termios)
  * @break_ctl: if set, port is 'sending break' to remote
diff --git a/drivers/staging/gdm724x/gdm_endian.c b/drivers/staging/gdm724x/gdm_endian.c
index f6cc90ae9ba6..d7144e7afa32 100644
--- a/drivers/staging/gdm724x/gdm_endian.c
+++ b/drivers/staging/gdm724x/gdm_endian.c
@@ -11,57 +11,45 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/slab.h>
+#include <linux/kernel.h>
 #include "gdm_endian.h"
 
 void gdm_set_endian(struct gdm_endian *ed, u8 dev_endian)
 {
-	u8 a[2] = {0x12, 0x34};
-	u8 b[2] = {0, };
-	u16 c = 0x1234;
-
 	if (dev_endian == ENDIANNESS_BIG)
 		ed->dev_ed = ENDIANNESS_BIG;
 	else
 		ed->dev_ed = ENDIANNESS_LITTLE;
-
-	memcpy(b, &c, 2);
-
-	if (a[0] != b[0])
-		ed->host_ed = ENDIANNESS_LITTLE;
-	else
-		ed->host_ed = ENDIANNESS_BIG;
-
 }
 
 u16 gdm_cpu_to_dev16(struct gdm_endian *ed, u16 x)
 {
-	if (ed->dev_ed == ed->host_ed)
-		return x;
-
-	return Endian16_Swap(x);
+	if (ed->dev_ed == ENDIANNESS_LITTLE)
+		return cpu_to_le16(x);
+	else
+		return cpu_to_be16(x);
 }
 
 u16 gdm_dev16_to_cpu(struct gdm_endian *ed, u16 x)
 {
-	if (ed->dev_ed == ed->host_ed)
-		return x;
-
-	return Endian16_Swap(x);
+	if (ed->dev_ed == ENDIANNESS_LITTLE)
+		return le16_to_cpu(x);
+	else
+		return be16_to_cpu(x);
 }
 
 u32 gdm_cpu_to_dev32(struct gdm_endian *ed, u32 x)
 {
-	if (ed->dev_ed == ed->host_ed)
-		return x;
-
-	return Endian32_Swap(x);
+	if (ed->dev_ed == ENDIANNESS_LITTLE)
+		return cpu_to_le32(x);
+	else
+		return cpu_to_be32(x);
 }
 
 u32 gdm_dev32_to_cpu(struct gdm_endian *ed, u32 x)
 {
-	if (ed->dev_ed == ed->host_ed)
-		return x;
-
-	return Endian32_Swap(x);
+	if (ed->dev_ed == ENDIANNESS_LITTLE)
+		return le32_to_cpu(x);
+	else
+		return be32_to_cpu(x);
 }
diff --git a/drivers/staging/gdm724x/gdm_endian.h b/drivers/staging/gdm724x/gdm_endian.h
index 9b2531ff908e..6177870830e5 100644
--- a/drivers/staging/gdm724x/gdm_endian.h
+++ b/drivers/staging/gdm724x/gdm_endian.h
@@ -16,16 +16,6 @@
 
 #include <linux/types.h>
 
-#define Endian16_Swap(value) \
-	((((u16)((value) & 0x00FF)) << 8) | \
-	 (((u16)((value) & 0xFF00)) >> 8))
-
-#define Endian32_Swap(value) \
-	((((u32)((value) & 0x000000FF)) << 24) | \
-	 (((u32)((value) & 0x0000FF00)) << 8) | \
-	 (((u32)((value) & 0x00FF0000)) >> 8) | \
-	 (((u32)((value) & 0xFF000000)) >> 24))
-
 enum {
 	ENDIANNESS_MIN = 0,
 	ENDIANNESS_UNKNOWN,
@@ -37,7 +27,6 @@ enum {
 
 struct gdm_endian {
 	u8 dev_ed;
-	u8 host_ed;
 };
 
 void gdm_set_endian(struct gdm_endian *ed, u8 dev_endian);
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
index 61d168e82011..08290d901b0c 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.c
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -584,7 +584,7 @@ static int gdm_wimax_get_prepared_info(struct net_device *dev, char *buf,
 		if (T == TLV_T(T_MAC_ADDRESS)) {
 			if (L != dev->addr_len) {
 				netdev_err(dev,
-					   "%s Invalid inofrmation result T/L [%x/%d]\n",
+					   "%s Invalid information result T/L [%x/%d]\n",
 					   __func__, T, L);
 				return -1;
 			}
diff --git a/drivers/staging/gdm72xx/netlink_k.c b/drivers/staging/gdm72xx/netlink_k.c
index 9d78bfcdb2c3..f3cdaa6c468c 100644
--- a/drivers/staging/gdm72xx/netlink_k.c
+++ b/drivers/staging/gdm72xx/netlink_k.c
@@ -121,7 +121,7 @@ int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
 	int ret = 0;
 
 	if (group > ND_MAX_GROUP) {
-		pr_err("Group %d is invalied.\n", group);
+		pr_err("Group %d is invalid.\n", group);
 		pr_err("Valid group is 0 ~ %d.\n", ND_MAX_GROUP);
 		return -EINVAL;
 	}
diff --git a/drivers/staging/goldfish/goldfish_audio.c b/drivers/staging/goldfish/goldfish_audio.c
index 702ae04df912..b0927e49d0a8 100644
--- a/drivers/staging/goldfish/goldfish_audio.c
+++ b/drivers/staging/goldfish/goldfish_audio.c
@@ -63,7 +63,7 @@ struct goldfish_audio {
 #define AUDIO_READ(data, addr)		(readl(data->reg_base + addr))
 #define AUDIO_WRITE(data, addr, x)	(writel(x, data->reg_base + addr))
 #define AUDIO_WRITE64(data, addr, addr2, x)	\
-	(gf_write64((u64)(x), data->reg_base + addr, data->reg_base+addr2))
+	(gf_write_dma_addr((x), data->reg_base + addr, data->reg_base+addr2))
 
 /*
  *  temporary variable used between goldfish_audio_probe() and
diff --git a/drivers/staging/goldfish/goldfish_nand.c b/drivers/staging/goldfish/goldfish_nand.c
index 213877a2c430..66ae48fcc2b2 100644
--- a/drivers/staging/goldfish/goldfish_nand.c
+++ b/drivers/staging/goldfish/goldfish_nand.c
@@ -87,7 +87,7 @@ static u32 goldfish_nand_cmd(struct mtd_info *mtd, enum nand_cmd cmd,
 		writel((u32)(addr >> 32), base + NAND_ADDR_HIGH);
 		writel((u32)addr, base + NAND_ADDR_LOW);
 		writel(len, base + NAND_TRANSFER_SIZE);
-		gf_write64((u64)ptr, base + NAND_DATA, base + NAND_DATA_HIGH);
+		gf_write_ptr(ptr, base + NAND_DATA, base + NAND_DATA_HIGH);
 		writel(cmd, base + NAND_COMMAND);
 		rv = readl(base + NAND_RESULT);
 	}
diff --git a/drivers/staging/i2o/Kconfig b/drivers/staging/i2o/Kconfig
deleted file mode 100644
index 286c53f4b13d..000000000000
--- a/drivers/staging/i2o/Kconfig
+++ /dev/null
@@ -1,120 +0,0 @@
-menuconfig I2O
-	tristate "I2O device support"
-	depends on PCI
-	---help---
-	  The Intelligent Input/Output (I2O) architecture allows hardware
-	  drivers to be split into two parts: an operating system specific
-	  module called the OSM and an hardware specific module called the
-	  HDM. The OSM can talk to a whole range of HDM's, and ideally the
-	  HDM's are not OS dependent. This allows for the same HDM driver to
-	  be used under different operating systems if the relevant OSM is in
-	  place. In order for this to work, you need to have an I2O interface
-	  adapter card in your computer. This card contains a special I/O
-	  processor (IOP), thus allowing high speeds since the CPU does not
-	  have to deal with I/O.
-
-	  If you say Y here, you will get a choice of interface adapter
-	  drivers and OSM's with the following questions.
-
-	  To compile this support as a module, choose M here: the
-	  modules will be called i2o_core.
-
-	  If unsure, say N.
-
-if I2O
-
-config I2O_LCT_NOTIFY_ON_CHANGES
-	bool "Enable LCT notification"
-	default y
-	---help---
-	  Only say N here if you have a I2O controller from SUN. The SUN
-	  firmware doesn't support LCT notification on changes. If this option
-	  is enabled on such a controller the driver will hang up in a endless
-	  loop. On all other controllers say Y.
-
-	  If unsure, say Y.
-
-config I2O_EXT_ADAPTEC
-	bool "Enable Adaptec extensions"
-	default y
-	---help---
-	  Say Y for support of raidutils for Adaptec I2O controllers. You also
-	  have to say Y to "I2O Configuration support", "I2O SCSI OSM" below
-	  and to "SCSI generic support" under "SCSI device configuration".
-
-config I2O_EXT_ADAPTEC_DMA64
-	bool "Enable 64-bit DMA"
-	depends on I2O_EXT_ADAPTEC && ( 64BIT || HIGHMEM64G )
-	default y
-	---help---
-	  Say Y for support of 64-bit DMA transfer mode on Adaptec I2O
-	  controllers.
-	  Note: You need at least firmware version 3709.
-
-config I2O_CONFIG
-	tristate "I2O Configuration support"
-	depends on VIRT_TO_BUS
-	---help---
-	  Say Y for support of the configuration interface for the I2O adapters.
-	  If you have a RAID controller from Adaptec and you want to use the
-	  raidutils to manage your RAID array, you have to say Y here.
-
-	  To compile this support as a module, choose M here: the
-	  module will be called i2o_config.
-
-	  Note: If you want to use the new API you have to download the
-	  i2o_config patch from http://i2o.shadowconnect.com/
-
-config I2O_CONFIG_OLD_IOCTL
-	bool "Enable ioctls (OBSOLETE)"
-	depends on I2O_CONFIG
-	default y
-	---help---
-	  Enables old ioctls.
-
-config I2O_BUS
-	tristate "I2O Bus Adapter OSM"
-	---help---
-	  Include support for the I2O Bus Adapter OSM. The Bus Adapter OSM
-	  provides access to the busses on the I2O controller. The main purpose
-	  is to rescan the bus to find new devices.
-
-	  To compile this support as a module, choose M here: the
-	  module will be called i2o_bus.
-
-config I2O_BLOCK
-	tristate "I2O Block OSM"
-	depends on BLOCK
-	---help---
-	  Include support for the I2O Block OSM. The Block OSM presents disk
-	  and other structured block devices to the operating system. If you
-	  are using an RAID controller, you could access the array only by
-	  the Block OSM driver. But it is possible to access the single disks
-	  by the SCSI OSM driver, for example to monitor the disks.
-
-	  To compile this support as a module, choose M here: the
-	  module will be called i2o_block.
-
-config I2O_SCSI
-	tristate "I2O SCSI OSM"
-	depends on SCSI
-	---help---
-	  Allows direct SCSI access to SCSI devices on a SCSI or FibreChannel
-	  I2O controller. You can use both the SCSI and Block OSM together if
-	  you wish. To access a RAID array, you must use the Block OSM driver.
-	  But you could use the SCSI OSM driver to monitor the single disks.
-
-	  To compile this support as a module, choose M here: the
-	  module will be called i2o_scsi.
-
-config I2O_PROC
-	tristate "I2O /proc support"
-	---help---
-	  If you say Y here and to "/proc file system support", you will be
-	  able to read I2O related information from the virtual directory
-	  /proc/i2o.
-
-	  To compile this support as a module, choose M here: the
-	  module will be called i2o_proc.
-
-endif # I2O
diff --git a/drivers/staging/i2o/Makefile b/drivers/staging/i2o/Makefile
deleted file mode 100644
index b0982dacfd0a..000000000000
--- a/drivers/staging/i2o/Makefile
+++ /dev/null
@@ -1,16 +0,0 @@
-#
-# Makefile for the kernel I2O OSM.
-#
-# Note : at this point, these files are compiled on all systems.
-# In the future, some of these should be built conditionally.
-#
-
-i2o_core-y		+= iop.o driver.o device.o debug.o pci.o exec-osm.o memory.o
-i2o_bus-y		+= bus-osm.o
-i2o_config-y		+= config-osm.o
-obj-$(CONFIG_I2O)	+= i2o_core.o
-obj-$(CONFIG_I2O_CONFIG)+= i2o_config.o
-obj-$(CONFIG_I2O_BUS)	+= i2o_bus.o
-obj-$(CONFIG_I2O_BLOCK)	+= i2o_block.o
-obj-$(CONFIG_I2O_SCSI)	+= i2o_scsi.o
-obj-$(CONFIG_I2O_PROC)	+= i2o_proc.o
diff --git a/drivers/staging/i2o/README b/drivers/staging/i2o/README
deleted file mode 100644
index f072a8eb3041..000000000000
--- a/drivers/staging/i2o/README
+++ /dev/null
@@ -1,98 +0,0 @@
-
-	Linux I2O Support	(c) Copyright 1999 Red Hat Software
-					and others.
-
-	This program is free software; you can redistribute it and/or
-	modify it under 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.
-
-AUTHORS (so far)
-
-Alan Cox, Building Number Three Ltd.
-	Core code, SCSI and Block OSMs
-
-Steve Ralston, LSI Logic Corp.
-	Debugging SCSI and Block OSM
-
-Deepak Saxena, Intel Corp.
-	Various core/block extensions
-	/proc interface, bug fixes
-	Ioctl interfaces for control
-	Debugging LAN OSM
-
-Philip Rumpf
-	Fixed assorted dumb SMP locking bugs
-
-Juha Sievanen, University of Helsinki Finland
-	LAN OSM code
-	/proc interface to LAN class
-	Bug fixes
-	Core code extensions
-
-Auvo Häkkinen, University of Helsinki Finland
-	LAN OSM code
-	/Proc interface to LAN class
-	Bug fixes
-	Core code extensions
-
-Taneli Vähäkangas, University of Helsinki Finland
-	Fixes to i2o_config
-
-CREDITS
-
-	This work was made possible by 
-
-Red Hat Software
-	Funding for the Building #3 part of the project
-
-Symbios Logic (Now LSI)
-	Host adapters, hints, known to work platforms when I hit
-	compatibility problems
-
-BoxHill Corporation
-	Loan of initial FibreChannel disk array used for development work.
-
-European Commission
-	Funding the work done by the University of Helsinki
-
-SysKonnect
-        Loan of FDDI and Gigabit Ethernet cards
-
-ASUSTeK
-        Loan of I2O motherboard 
-
-STATUS:
-
-o	The core setup works within limits.
-o	The scsi layer seems to almost work. 
-           I'm still chasing down the hang bug.
-o	The block OSM is mostly functional
-o	LAN OSM works with FDDI and Ethernet cards.
-
-TO DO:
-
-General:
-o	Provide hidden address space if asked
-o	Long term message flow control
-o	PCI IOP's without interrupts are not supported yet
-o	Push FAIL handling into the core
-o	DDM control interfaces for module load etc
-o       Add I2O 2.0 support (Deffered to 2.5 kernel)
-
-Block:
-o	Multiple major numbers
-o	Read ahead and cache handling stuff. Talk to Ingo and people
-o	Power management
-o	Finish Media changers
-
-SCSI:
-o	Find the right way to associate drives/luns/busses
-
-Lan:	
-o	Performance tuning
-o	Test Fibre Channel code
-
-Tape:
-o	Anyone seen anything implementing this ?
-           (D.S: Will attempt to do so if spare cycles permit)
diff --git a/drivers/staging/i2o/README.ioctl b/drivers/staging/i2o/README.ioctl
deleted file mode 100644
index 4a7d2ebdfc97..000000000000
--- a/drivers/staging/i2o/README.ioctl
+++ /dev/null
@@ -1,394 +0,0 @@
-
-Linux I2O User Space Interface
-rev 0.3 - 04/20/99
-
-=============================================================================
-Originally written by Deepak Saxena(deepak@plexity.net)
-Currently maintained by Deepak Saxena(deepak@plexity.net)
-=============================================================================
-
-I. Introduction
-
-The Linux I2O subsystem provides a set of ioctl() commands that can be
-utilized by user space applications to communicate with IOPs and devices
-on individual IOPs. This document defines the specific ioctl() commands
-that are available to the user and provides examples of their uses.
-
-This document assumes the reader is familiar with or has access to the 
-I2O specification as no I2O message parameters are outlined.  For information 
-on the specification, see http://www.i2osig.org
-
-This document and the I2O user space interface are currently maintained
-by Deepak Saxena.  Please send all comments, errata, and bug fixes to
-deepak@csociety.purdue.edu
-
-II. IOP Access
-
-Access to the I2O subsystem is provided through the device file named 
-/dev/i2o/ctl.  This file is a character file with major number 10 and minor
-number 166.  It can be created through the following command:
-
-   mknod /dev/i2o/ctl c 10 166
-
-III. Determining the IOP Count
-
-   SYNOPSIS 
-
-   ioctl(fd, I2OGETIOPS,  int *count);
-
-   u8 count[MAX_I2O_CONTROLLERS];
-
-   DESCRIPTION
-
-   This function returns the system's active IOP table.  count should
-   point to a buffer containing MAX_I2O_CONTROLLERS entries.  Upon 
-   returning, each entry will contain a non-zero value if the given
-   IOP unit is active, and NULL if it is inactive or non-existent.
-
-   RETURN VALUE.
-
-   Returns 0 if no errors occur, and -1 otherwise.  If an error occurs,
-   errno is set appropriately:
-
-     EFAULT   Invalid user space pointer was passed
-
-IV. Getting Hardware Resource Table
-
-   SYNOPSIS 
- 
-   ioctl(fd, I2OHRTGET, struct i2o_cmd_hrt *hrt);
-
-      struct i2o_cmd_hrtlct
-      {
-         u32   iop;      /* IOP unit number */
-         void  *resbuf;  /* Buffer for result */
-         u32   *reslen;  /* Buffer length in bytes */
-      };
-
-   DESCRIPTION
-
-   This function returns the Hardware Resource Table of the IOP specified 
-   by hrt->iop in the buffer pointed to by hrt->resbuf. The actual size of 
-   the data is written into *(hrt->reslen).
-
-   RETURNS
-
-   This function returns 0 if no errors occur. If an error occurs, -1 
-   is returned and errno is set appropriately:
-
-      EFAULT      Invalid user space pointer was passed
-      ENXIO       Invalid IOP number
-      ENOBUFS     Buffer not large enough.  If this occurs, the required
-                  buffer length is written into *(hrt->reslen)
-  
-V. Getting Logical Configuration Table
-   
-   SYNOPSIS 
- 
-   ioctl(fd, I2OLCTGET, struct i2o_cmd_lct *lct);
-
-      struct i2o_cmd_hrtlct
-      {
-         u32   iop;      /* IOP unit number */
-         void  *resbuf;  /* Buffer for result */
-         u32   *reslen;  /* Buffer length in bytes */
-      };
-
-   DESCRIPTION
-
-   This function returns the Logical Configuration Table of the IOP specified
-   by lct->iop in the buffer pointed to by lct->resbuf. The actual size of 
-   the data is written into *(lct->reslen).
-
-   RETURNS
-
-   This function returns 0 if no errors occur. If an error occurs, -1 
-   is returned and errno is set appropriately:
-
-      EFAULT      Invalid user space pointer was passed
-      ENXIO       Invalid IOP number
-      ENOBUFS     Buffer not large enough.  If this occurs, the required
-                  buffer length is written into *(lct->reslen)
-
-VI. Setting Parameters
-   
-   SYNOPSIS 
- 
-   ioctl(fd, I2OPARMSET, struct i2o_parm_setget *ops);
-
-      struct i2o_cmd_psetget
-      {
-         u32   iop;      /* IOP unit number */
-         u32   tid;      /* Target device TID */
-         void  *opbuf;   /* Operation List buffer */
-         u32   oplen;    /* Operation List buffer length in bytes */
-         void  *resbuf;  /* Result List buffer */
-         u32   *reslen;  /* Result List buffer length in bytes */
-      };
-
-   DESCRIPTION
-
-   This function posts a UtilParamsSet message to the device identified
-   by ops->iop and ops->tid.  The operation list for the message is 
-   sent through the ops->opbuf buffer, and the result list is written
-   into the buffer pointed to by ops->resbuf.  The number of bytes 
-   written is placed into *(ops->reslen). 
-
-   RETURNS
-
-   The return value is the size in bytes of the data written into
-   ops->resbuf if no errors occur.  If an error occurs, -1 is returned 
-   and errno is set appropriately:
-
-      EFAULT      Invalid user space pointer was passed
-      ENXIO       Invalid IOP number
-      ENOBUFS     Buffer not large enough.  If this occurs, the required
-                  buffer length is written into *(ops->reslen)
-      ETIMEDOUT   Timeout waiting for reply message
-      ENOMEM      Kernel memory allocation error
-
-   A return value of 0 does not mean that the value was actually
-   changed properly on the IOP.  The user should check the result
-   list to determine the specific status of the transaction.
-
-VII. Getting Parameters
-   
-   SYNOPSIS 
- 
-   ioctl(fd, I2OPARMGET, struct i2o_parm_setget *ops);
-
-      struct i2o_parm_setget
-      {
-         u32   iop;      /* IOP unit number */
-         u32   tid;      /* Target device TID */
-         void  *opbuf;   /* Operation List buffer */
-         u32   oplen;    /* Operation List buffer length in bytes */
-         void  *resbuf;  /* Result List buffer */
-         u32   *reslen;  /* Result List buffer length in bytes */
-      };
-
-   DESCRIPTION
-
-   This function posts a UtilParamsGet message to the device identified
-   by ops->iop and ops->tid.  The operation list for the message is 
-   sent through the ops->opbuf buffer, and the result list is written
-   into the buffer pointed to by ops->resbuf.  The actual size of data
-   written is placed into *(ops->reslen).
-
-   RETURNS
-
-      EFAULT      Invalid user space pointer was passed
-      ENXIO       Invalid IOP number
-      ENOBUFS     Buffer not large enough.  If this occurs, the required
-                  buffer length is written into *(ops->reslen)
-      ETIMEDOUT   Timeout waiting for reply message
-      ENOMEM      Kernel memory allocation error
-
-   A return value of 0 does not mean that the value was actually
-   properly retrieved.  The user should check the result list 
-   to determine the specific status of the transaction.
-
-VIII. Downloading Software
-   
-   SYNOPSIS 
- 
-   ioctl(fd, I2OSWDL, struct i2o_sw_xfer *sw);
-
-      struct i2o_sw_xfer
-      {
-         u32   iop;       /* IOP unit number */
-         u8    flags;     /* DownloadFlags field */
-         u8    sw_type;   /* Software type */
-         u32   sw_id;     /* Software ID */
-         void  *buf;      /* Pointer to software buffer */
-         u32   *swlen;    /* Length of software buffer */        
-         u32   *maxfrag;  /* Number of fragments */
-         u32   *curfrag;  /* Current fragment number */
-      };
-
-   DESCRIPTION
-
-   This function downloads a software fragment pointed by sw->buf
-   to the iop identified by sw->iop. The DownloadFlags, SwID, SwType
-   and SwSize fields of the ExecSwDownload message are filled in with
-   the values of sw->flags, sw->sw_id, sw->sw_type and *(sw->swlen).
-
-   The fragments _must_ be sent in order and be 8K in size. The last
-   fragment _may_ be shorter, however. The kernel will compute its
-   size based on information in the sw->swlen field.
-
-   Please note that SW transfers can take a long time.
-
-   RETURNS
-
-   This function returns 0 no errors occur. If an error occurs, -1 
-   is returned and errno is set appropriately:
-
-      EFAULT      Invalid user space pointer was passed
-      ENXIO       Invalid IOP number
-      ETIMEDOUT   Timeout waiting for reply message
-      ENOMEM      Kernel memory allocation error
-
-IX. Uploading Software
-   
-   SYNOPSIS 
-
-   ioctl(fd, I2OSWUL, struct i2o_sw_xfer *sw);
-
-      struct i2o_sw_xfer
-      {
-         u32   iop;      /* IOP unit number */
-         u8    flags; 	 /* UploadFlags */
-         u8    sw_type;  /* Software type */
-         u32   sw_id;    /* Software ID */
-         void  *buf;     /* Pointer to software buffer */
-         u32   *swlen;   /* Length of software buffer */        
-         u32   *maxfrag; /* Number of fragments */
-         u32   *curfrag; /* Current fragment number */
-      };
-
-   DESCRIPTION
-
-   This function uploads a software fragment from the IOP identified
-   by sw->iop, sw->sw_type, sw->sw_id and optionally sw->swlen fields.
-   The UploadFlags, SwID, SwType and SwSize fields of the ExecSwUpload
-   message are filled in with the values of sw->flags, sw->sw_id,
-   sw->sw_type and *(sw->swlen).
-
-   The fragments _must_ be requested in order and be 8K in size. The
-   user is responsible for allocating memory pointed by sw->buf. The
-   last fragment _may_ be shorter.
-
-   Please note that SW transfers can take a long time.
-
-   RETURNS
-
-   This function returns 0 if no errors occur.  If an error occurs, -1
-   is returned and errno is set appropriately:
-
-      EFAULT      Invalid user space pointer was passed
-      ENXIO       Invalid IOP number
-      ETIMEDOUT   Timeout waiting for reply message
-      ENOMEM      Kernel memory allocation error
-         
-X. Removing Software
-   
-   SYNOPSIS 
- 
-   ioctl(fd, I2OSWDEL, struct i2o_sw_xfer *sw);
-
-      struct i2o_sw_xfer
-      {
-         u32   iop;      /* IOP unit number */
-         u8    flags; 	 /* RemoveFlags */
-         u8    sw_type;  /* Software type */
-         u32   sw_id;    /* Software ID */
-         void  *buf;     /* Unused */
-         u32   *swlen;   /* Length of the software data */        
-         u32   *maxfrag; /* Unused */
-         u32   *curfrag; /* Unused */
-      };
-
-   DESCRIPTION
-
-   This function removes software from the IOP identified by sw->iop.
-   The RemoveFlags, SwID, SwType and SwSize fields of the ExecSwRemove message 
-   are filled in with the values of sw->flags, sw->sw_id, sw->sw_type and 
-   *(sw->swlen). Give zero in *(sw->len) if the value is unknown. IOP uses 
-   *(sw->swlen) value to verify correct identication of the module to remove. 
-   The actual size of the module is written into *(sw->swlen).
-
-   RETURNS
-
-   This function returns 0 if no errors occur.  If an error occurs, -1
-   is returned and errno is set appropriately:
-
-      EFAULT      Invalid user space pointer was passed
-      ENXIO       Invalid IOP number
-      ETIMEDOUT   Timeout waiting for reply message
-      ENOMEM      Kernel memory allocation error
-
-X. Validating Configuration
-
-   SYNOPSIS
-
-   ioctl(fd, I2OVALIDATE, int *iop);
-	u32 iop;
-
-   DESCRIPTION
-
-   This function posts an ExecConfigValidate message to the controller
-   identified by iop. This message indicates that the current
-   configuration is accepted. The iop changes the status of suspect drivers 
-   to valid and may delete old drivers from its store.
-
-   RETURNS
-
-   This function returns 0 if no erro occur.  If an error occurs, -1 is
-   returned and errno is set appropriately:
-
-      ETIMEDOUT   Timeout waiting for reply message
-      ENXIO       Invalid IOP number
-
-XI. Configuration Dialog
-   
-   SYNOPSIS 
- 
-   ioctl(fd, I2OHTML, struct i2o_html *htquery);
-      struct i2o_html
-      {
-         u32   iop;      /* IOP unit number */
-         u32   tid;      /* Target device ID */
-         u32   page;     /* HTML page */
-         void  *resbuf;  /* Buffer for reply HTML page */
-         u32   *reslen;  /* Length in bytes of reply buffer */
-         void  *qbuf;    /* Pointer to HTTP query string */
-         u32   qlen;     /* Length in bytes of query string buffer */        
-      };
-
-   DESCRIPTION
-
-   This function posts an UtilConfigDialog message to the device identified
-   by htquery->iop and htquery->tid.  The requested HTML page number is 
-   provided by the htquery->page field, and the resultant data is stored 
-   in the buffer pointed to by htquery->resbuf.  If there is an HTTP query 
-   string that is to be sent to the device, it should be sent in the buffer
-   pointed to by htquery->qbuf.  If there is no query string, this field
-   should be set to NULL. The actual size of the reply received is written
-   into *(htquery->reslen).
-  
-   RETURNS
-
-   This function returns 0 if no error occur. If an error occurs, -1
-   is returned and errno is set appropriately:
-
-      EFAULT      Invalid user space pointer was passed
-      ENXIO       Invalid IOP number
-      ENOBUFS     Buffer not large enough.  If this occurs, the required
-                  buffer length is written into *(ops->reslen)
-      ETIMEDOUT   Timeout waiting for reply message
-      ENOMEM      Kernel memory allocation error
-
-XII. Events
-
-    In the process of determining this.  Current idea is to have use
-    the select() interface to allow user apps to periodically poll
-    the /dev/i2o/ctl device for events.  When select() notifies the user
-    that an event is available, the user would call read() to retrieve
-    a list of all the events that are pending for the specific device.
-
-=============================================================================
-Revision History
-=============================================================================
-
-Rev 0.1 - 04/01/99
-- Initial revision
-
-Rev 0.2 - 04/06/99
-- Changed return values to match UNIX ioctl() standard.  Only return values
-  are 0 and -1.  All errors are reported through errno.
-- Added summary of proposed possible event interfaces
-
-Rev 0.3 - 04/20/99
-- Changed all ioctls() to use pointers to user data instead of actual data
-- Updated error values to match the code
diff --git a/drivers/staging/i2o/bus-osm.c b/drivers/staging/i2o/bus-osm.c
deleted file mode 100644
index 43e357eeeb67..000000000000
--- a/drivers/staging/i2o/bus-osm.c
+++ /dev/null
@@ -1,177 +0,0 @@
-/*
- *	Bus Adapter OSM
- *
- *	Copyright (C) 2005	Markus Lidel <Markus.Lidel@shadowconnect.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.
- *
- *	Fixes/additions:
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>
- *			initial version.
- */
-
-#include <linux/module.h>
-#include "i2o.h"
-
-#define OSM_NAME	"bus-osm"
-#define OSM_VERSION	"1.317"
-#define OSM_DESCRIPTION	"I2O Bus Adapter OSM"
-
-static struct i2o_driver i2o_bus_driver;
-
-/* Bus OSM class handling definition */
-static struct i2o_class_id i2o_bus_class_id[] = {
-	{I2O_CLASS_BUS_ADAPTER},
-	{I2O_CLASS_END}
-};
-
-/**
- *	i2o_bus_scan - Scan the bus for new devices
- *	@dev: I2O device of the bus, which should be scanned
- *
- *	Scans the bus dev for new / removed devices. After the scan a new LCT
- *	will be fetched automatically.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_bus_scan(struct i2o_device *dev)
-{
-	struct i2o_message *msg;
-
-	msg = i2o_msg_get_wait(dev->iop, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return -ETIMEDOUT;
-
-	msg->u.head[0] = cpu_to_le32(FIVE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_BUS_SCAN << 24 | HOST_TID << 12 | dev->lct_data.
-			tid);
-
-	return i2o_msg_post_wait(dev->iop, msg, 60);
-};
-
-/**
- *	i2o_bus_store_scan - Scan the I2O Bus Adapter
- *	@d: device which should be scanned
- *	@attr: device_attribute
- *	@buf: output buffer
- *	@count: buffer size
- *
- *	Returns count.
- */
-static ssize_t i2o_bus_store_scan(struct device *d,
-				  struct device_attribute *attr,
-				  const char *buf, size_t count)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(d);
-	int rc;
-
-	rc = i2o_bus_scan(i2o_dev);
-	if (rc)
-		osm_warn("bus scan failed %d\n", rc);
-
-	return count;
-}
-
-/* Bus Adapter OSM device attributes */
-static DEVICE_ATTR(scan, S_IWUSR, NULL, i2o_bus_store_scan);
-
-/**
- *	i2o_bus_probe - verify if dev is a I2O Bus Adapter device and install it
- *	@dev: device to verify if it is a I2O Bus Adapter device
- *
- *	Because we want all Bus Adapters always return 0.
- *	Except when we fail.  Then we are sad.
- *
- *	Returns 0, except when we fail to excel.
- */
-static int i2o_bus_probe(struct device *dev)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(get_device(dev));
-	int rc;
-
-	rc = device_create_file(dev, &dev_attr_scan);
-	if (rc)
-		goto err_out;
-
-	osm_info("device added (TID: %03x)\n", i2o_dev->lct_data.tid);
-
-	return 0;
-
-err_out:
-	put_device(dev);
-	return rc;
-};
-
-/**
- *	i2o_bus_remove - remove the I2O Bus Adapter device from the system again
- *	@dev: I2O Bus Adapter device which should be removed
- *
- *	Always returns 0.
- */
-static int i2o_bus_remove(struct device *dev)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-
-	device_remove_file(dev, &dev_attr_scan);
-
-	put_device(dev);
-
-	osm_info("device removed (TID: %03x)\n", i2o_dev->lct_data.tid);
-
-	return 0;
-};
-
-/* Bus Adapter OSM driver struct */
-static struct i2o_driver i2o_bus_driver = {
-	.name = OSM_NAME,
-	.classes = i2o_bus_class_id,
-	.driver = {
-		   .probe = i2o_bus_probe,
-		   .remove = i2o_bus_remove,
-		   },
-};
-
-/**
- *	i2o_bus_init - Bus Adapter OSM initialization function
- *
- *	Only register the Bus Adapter OSM in the I2O core.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int __init i2o_bus_init(void)
-{
-	int rc;
-
-	printk(KERN_INFO OSM_DESCRIPTION " v" OSM_VERSION "\n");
-
-	/* Register Bus Adapter OSM into I2O core */
-	rc = i2o_driver_register(&i2o_bus_driver);
-	if (rc) {
-		osm_err("Could not register Bus Adapter OSM\n");
-		return rc;
-	}
-
-	return 0;
-};
-
-/**
- *	i2o_bus_exit - Bus Adapter OSM exit function
- *
- *	Unregisters Bus Adapter OSM from I2O core.
- */
-static void __exit i2o_bus_exit(void)
-{
-	i2o_driver_unregister(&i2o_bus_driver);
-};
-
-MODULE_AUTHOR("Markus Lidel <Markus.Lidel@shadowconnect.com>");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(OSM_DESCRIPTION);
-MODULE_VERSION(OSM_VERSION);
-
-module_init(i2o_bus_init);
-module_exit(i2o_bus_exit);
diff --git a/drivers/staging/i2o/config-osm.c b/drivers/staging/i2o/config-osm.c
deleted file mode 100644
index 45091ac66154..000000000000
--- a/drivers/staging/i2o/config-osm.c
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- *	Configuration OSM
- *
- *	Copyright (C) 2005	Markus Lidel <Markus.Lidel@shadowconnect.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.
- *
- *	Fixes/additions:
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>
- *			initial version.
- */
-
-#include <linux/module.h>
-#include "i2o.h"
-#include <linux/dcache.h>
-#include <linux/namei.h>
-#include <linux/fs.h>
-
-#include <linux/uaccess.h>
-
-#define OSM_NAME	"config-osm"
-#define OSM_VERSION	"1.323"
-#define OSM_DESCRIPTION	"I2O Configuration OSM"
-
-/* access mode user rw */
-#define S_IWRSR (S_IRUSR | S_IWUSR)
-
-static struct i2o_driver i2o_config_driver;
-
-/* Config OSM driver struct */
-static struct i2o_driver i2o_config_driver = {
-	.name = OSM_NAME,
-};
-
-#ifdef CONFIG_I2O_CONFIG_OLD_IOCTL
-#include "i2o_config.c"
-#endif
-
-/**
- *	i2o_config_init - Configuration OSM initialization function
- *
- *	Registers Configuration OSM in the I2O core and if old ioctl's are
- *	compiled in initialize them.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int __init i2o_config_init(void)
-{
-	printk(KERN_INFO OSM_DESCRIPTION " v" OSM_VERSION "\n");
-
-	if (i2o_driver_register(&i2o_config_driver)) {
-		osm_err("handler register failed.\n");
-		return -EBUSY;
-	}
-#ifdef CONFIG_I2O_CONFIG_OLD_IOCTL
-	if (i2o_config_old_init()) {
-		osm_err("old config handler initialization failed\n");
-		i2o_driver_unregister(&i2o_config_driver);
-		return -EBUSY;
-	}
-#endif
-
-	return 0;
-}
-
-/**
- *	i2o_config_exit - Configuration OSM exit function
- *
- *	If old ioctl's are compiled in exit remove them and unregisters
- *	Configuration OSM from I2O core.
- */
-static void i2o_config_exit(void)
-{
-#ifdef CONFIG_I2O_CONFIG_OLD_IOCTL
-	i2o_config_old_exit();
-#endif
-
-	i2o_driver_unregister(&i2o_config_driver);
-}
-
-MODULE_AUTHOR("Markus Lidel <Markus.Lidel@shadowconnect.com>");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(OSM_DESCRIPTION);
-MODULE_VERSION(OSM_VERSION);
-
-module_init(i2o_config_init);
-module_exit(i2o_config_exit);
diff --git a/drivers/staging/i2o/core.h b/drivers/staging/i2o/core.h
deleted file mode 100644
index 91614f11f89a..000000000000
--- a/drivers/staging/i2o/core.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- *	I2O core internal declarations
- *
- *	Copyright (C) 2005	Markus Lidel <Markus.Lidel@shadowconnect.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.
- *
- *	Fixes/additions:
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>
- *			initial version.
- */
-
-/* Exec-OSM */
-extern struct i2o_driver i2o_exec_driver;
-extern int i2o_exec_lct_get(struct i2o_controller *);
-
-extern int __init i2o_exec_init(void);
-extern void i2o_exec_exit(void);
-
-/* driver */
-extern struct bus_type i2o_bus_type;
-
-extern int i2o_driver_dispatch(struct i2o_controller *, u32);
-
-extern int __init i2o_driver_init(void);
-extern void i2o_driver_exit(void);
-
-/* PCI */
-extern int __init i2o_pci_init(void);
-extern void __exit i2o_pci_exit(void);
-
-/* device */
-extern const struct attribute_group *i2o_device_groups[];
-
-extern void i2o_device_remove(struct i2o_device *);
-extern int i2o_device_parse_lct(struct i2o_controller *);
-
-int i2o_parm_issue(struct i2o_device *i2o_dev, int cmd, void *oplist,
-		   int oplen, void *reslist, int reslen);
-
-/* IOP */
-extern struct i2o_controller *i2o_iop_alloc(void);
-
-/**
- *	i2o_iop_free - Free the i2o_controller struct
- *	@c: I2O controller to free
- */
-static inline void i2o_iop_free(struct i2o_controller *c)
-{
-	i2o_pool_free(&c->in_msg);
-	kfree(c);
-}
-
-extern int i2o_iop_add(struct i2o_controller *);
-extern void i2o_iop_remove(struct i2o_controller *);
-
-/* control registers relative to c->base */
-#define I2O_IRQ_STATUS	0x30
-#define I2O_IRQ_MASK	0x34
-#define I2O_IN_PORT	0x40
-#define I2O_OUT_PORT	0x44
-
-/* Motorola/Freescale specific register offset */
-#define I2O_MOTOROLA_PORT_OFFSET	0x10400
-
-#define I2O_IRQ_OUTBOUND_POST	0x00000008
diff --git a/drivers/staging/i2o/debug.c b/drivers/staging/i2o/debug.c
deleted file mode 100644
index 12b783b2a86c..000000000000
--- a/drivers/staging/i2o/debug.c
+++ /dev/null
@@ -1,473 +0,0 @@
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/pci.h>
-#include "i2o.h"
-
-static void i2o_report_util_cmd(u8 cmd);
-static void i2o_report_exec_cmd(u8 cmd);
-static void i2o_report_fail_status(u8 req_status, u32 *msg);
-static void i2o_report_common_status(u8 req_status);
-static void i2o_report_common_dsc(u16 detailed_status);
-
-/*
- * Used for error reporting/debugging purposes.
- * Report Cmd name, Request status, Detailed Status.
- */
-void i2o_report_status(const char *severity, const char *str,
-		       struct i2o_message *m)
-{
-	u32 *msg = (u32 *) m;
-	u8 cmd = (msg[1] >> 24) & 0xFF;
-	u8 req_status = (msg[4] >> 24) & 0xFF;
-	u16 detailed_status = msg[4] & 0xFFFF;
-
-	if (cmd == I2O_CMD_UTIL_EVT_REGISTER)
-		return;		/* No status in this reply */
-
-	printk("%s%s: ", severity, str);
-
-	if (cmd < 0x1F)		// Utility cmd
-		i2o_report_util_cmd(cmd);
-
-	else if (cmd >= 0xA0 && cmd <= 0xEF)	// Executive cmd
-		i2o_report_exec_cmd(cmd);
-	else
-		printk("Cmd = %0#2x, ", cmd);	// Other cmds
-
-	if (msg[0] & MSG_FAIL) {
-		i2o_report_fail_status(req_status, msg);
-		return;
-	}
-
-	i2o_report_common_status(req_status);
-
-	if (cmd < 0x1F || (cmd >= 0xA0 && cmd <= 0xEF))
-		i2o_report_common_dsc(detailed_status);
-	else
-		printk(" / DetailedStatus = %0#4x.\n",
-		       detailed_status);
-}
-
-/* Used to dump a message to syslog during debugging */
-void i2o_dump_message(struct i2o_message *m)
-{
-#ifdef DEBUG
-	u32 *msg = (u32 *) m;
-	int i;
-
-	printk(KERN_INFO "Dumping I2O message size %d @ %p\n",
-	       msg[0] >> 16 & 0xffff, msg);
-	for (i = 0; i < ((msg[0] >> 16) & 0xffff); i++)
-		printk(KERN_INFO "  msg[%d] = %0#10x\n", i, msg[i]);
-#endif
-}
-
-/*
- * Used for error reporting/debugging purposes.
- * Following fail status are common to all classes.
- * The preserved message must be handled in the reply handler.
- */
-static void i2o_report_fail_status(u8 req_status, u32 *msg)
-{
-	static char *FAIL_STATUS[] = {
-		"0x80",		/* not used */
-		"SERVICE_SUSPENDED",	/* 0x81 */
-		"SERVICE_TERMINATED",	/* 0x82 */
-		"CONGESTION",
-		"FAILURE",
-		"STATE_ERROR",
-		"TIME_OUT",
-		"ROUTING_FAILURE",
-		"INVALID_VERSION",
-		"INVALID_OFFSET",
-		"INVALID_MSG_FLAGS",
-		"FRAME_TOO_SMALL",
-		"FRAME_TOO_LARGE",
-		"INVALID_TARGET_ID",
-		"INVALID_INITIATOR_ID",
-		"INVALID_INITIATOR_CONTEX",	/* 0x8F */
-		"UNKNOWN_FAILURE"	/* 0xFF */
-	};
-
-	if (req_status == I2O_FSC_TRANSPORT_UNKNOWN_FAILURE)
-		printk("TRANSPORT_UNKNOWN_FAILURE (%0#2x).\n",
-		       req_status);
-	else
-		printk("TRANSPORT_%s.\n",
-		       FAIL_STATUS[req_status & 0x0F]);
-
-	/* Dump some details */
-
-	printk(KERN_ERR "  InitiatorId = %d, TargetId = %d\n",
-	       (msg[1] >> 12) & 0xFFF, msg[1] & 0xFFF);
-	printk(KERN_ERR "  LowestVersion = 0x%02X, HighestVersion = 0x%02X\n",
-	       (msg[4] >> 8) & 0xFF, msg[4] & 0xFF);
-	printk(KERN_ERR "  FailingHostUnit = 0x%04X,  FailingIOP = 0x%03X\n",
-	       msg[5] >> 16, msg[5] & 0xFFF);
-
-	printk(KERN_ERR "  Severity:  0x%02X\n", (msg[4] >> 16) & 0xFF);
-	if (msg[4] & (1 << 16))
-		printk(KERN_DEBUG "(FormatError), "
-		       "this msg can never be delivered/processed.\n");
-	if (msg[4] & (1 << 17))
-		printk(KERN_DEBUG "(PathError), "
-		       "this msg can no longer be delivered/processed.\n");
-	if (msg[4] & (1 << 18))
-		printk(KERN_DEBUG "(PathState), "
-		       "the system state does not allow delivery.\n");
-	if (msg[4] & (1 << 19))
-		printk(KERN_DEBUG
-		       "(Congestion), resources temporarily not available;"
-		       "do not retry immediately.\n");
-}
-
-/*
- * Used for error reporting/debugging purposes.
- * Following reply status are common to all classes.
- */
-static void i2o_report_common_status(u8 req_status)
-{
-	static char *REPLY_STATUS[] = {
-		"SUCCESS",
-		"ABORT_DIRTY",
-		"ABORT_NO_DATA_TRANSFER",
-		"ABORT_PARTIAL_TRANSFER",
-		"ERROR_DIRTY",
-		"ERROR_NO_DATA_TRANSFER",
-		"ERROR_PARTIAL_TRANSFER",
-		"PROCESS_ABORT_DIRTY",
-		"PROCESS_ABORT_NO_DATA_TRANSFER",
-		"PROCESS_ABORT_PARTIAL_TRANSFER",
-		"TRANSACTION_ERROR",
-		"PROGRESS_REPORT"
-	};
-
-	if (req_status >= ARRAY_SIZE(REPLY_STATUS))
-		printk("RequestStatus = %0#2x", req_status);
-	else
-		printk("%s", REPLY_STATUS[req_status]);
-}
-
-/*
- * Used for error reporting/debugging purposes.
- * Following detailed status are valid  for executive class,
- * utility class, DDM class and for transaction error replies.
- */
-static void i2o_report_common_dsc(u16 detailed_status)
-{
-	static char *COMMON_DSC[] = {
-		"SUCCESS",
-		"0x01",		// not used
-		"BAD_KEY",
-		"TCL_ERROR",
-		"REPLY_BUFFER_FULL",
-		"NO_SUCH_PAGE",
-		"INSUFFICIENT_RESOURCE_SOFT",
-		"INSUFFICIENT_RESOURCE_HARD",
-		"0x08",		// not used
-		"CHAIN_BUFFER_TOO_LARGE",
-		"UNSUPPORTED_FUNCTION",
-		"DEVICE_LOCKED",
-		"DEVICE_RESET",
-		"INAPPROPRIATE_FUNCTION",
-		"INVALID_INITIATOR_ADDRESS",
-		"INVALID_MESSAGE_FLAGS",
-		"INVALID_OFFSET",
-		"INVALID_PARAMETER",
-		"INVALID_REQUEST",
-		"INVALID_TARGET_ADDRESS",
-		"MESSAGE_TOO_LARGE",
-		"MESSAGE_TOO_SMALL",
-		"MISSING_PARAMETER",
-		"TIMEOUT",
-		"UNKNOWN_ERROR",
-		"UNKNOWN_FUNCTION",
-		"UNSUPPORTED_VERSION",
-		"DEVICE_BUSY",
-		"DEVICE_NOT_AVAILABLE"
-	};
-
-	if (detailed_status > I2O_DSC_DEVICE_NOT_AVAILABLE)
-		printk(" / DetailedStatus = %0#4x.\n",
-		       detailed_status);
-	else
-		printk(" / %s.\n", COMMON_DSC[detailed_status]);
-}
-
-/*
- * Used for error reporting/debugging purposes
- */
-static void i2o_report_util_cmd(u8 cmd)
-{
-	switch (cmd) {
-	case I2O_CMD_UTIL_NOP:
-		printk("UTIL_NOP, ");
-		break;
-	case I2O_CMD_UTIL_ABORT:
-		printk("UTIL_ABORT, ");
-		break;
-	case I2O_CMD_UTIL_CLAIM:
-		printk("UTIL_CLAIM, ");
-		break;
-	case I2O_CMD_UTIL_RELEASE:
-		printk("UTIL_CLAIM_RELEASE, ");
-		break;
-	case I2O_CMD_UTIL_CONFIG_DIALOG:
-		printk("UTIL_CONFIG_DIALOG, ");
-		break;
-	case I2O_CMD_UTIL_DEVICE_RESERVE:
-		printk("UTIL_DEVICE_RESERVE, ");
-		break;
-	case I2O_CMD_UTIL_DEVICE_RELEASE:
-		printk("UTIL_DEVICE_RELEASE, ");
-		break;
-	case I2O_CMD_UTIL_EVT_ACK:
-		printk("UTIL_EVENT_ACKNOWLEDGE, ");
-		break;
-	case I2O_CMD_UTIL_EVT_REGISTER:
-		printk("UTIL_EVENT_REGISTER, ");
-		break;
-	case I2O_CMD_UTIL_LOCK:
-		printk("UTIL_LOCK, ");
-		break;
-	case I2O_CMD_UTIL_LOCK_RELEASE:
-		printk("UTIL_LOCK_RELEASE, ");
-		break;
-	case I2O_CMD_UTIL_PARAMS_GET:
-		printk("UTIL_PARAMS_GET, ");
-		break;
-	case I2O_CMD_UTIL_PARAMS_SET:
-		printk("UTIL_PARAMS_SET, ");
-		break;
-	case I2O_CMD_UTIL_REPLY_FAULT_NOTIFY:
-		printk("UTIL_REPLY_FAULT_NOTIFY, ");
-		break;
-	default:
-		printk("Cmd = %0#2x, ", cmd);
-	}
-}
-
-/*
- * Used for error reporting/debugging purposes
- */
-static void i2o_report_exec_cmd(u8 cmd)
-{
-	switch (cmd) {
-	case I2O_CMD_ADAPTER_ASSIGN:
-		printk("EXEC_ADAPTER_ASSIGN, ");
-		break;
-	case I2O_CMD_ADAPTER_READ:
-		printk("EXEC_ADAPTER_READ, ");
-		break;
-	case I2O_CMD_ADAPTER_RELEASE:
-		printk("EXEC_ADAPTER_RELEASE, ");
-		break;
-	case I2O_CMD_BIOS_INFO_SET:
-		printk("EXEC_BIOS_INFO_SET, ");
-		break;
-	case I2O_CMD_BOOT_DEVICE_SET:
-		printk("EXEC_BOOT_DEVICE_SET, ");
-		break;
-	case I2O_CMD_CONFIG_VALIDATE:
-		printk("EXEC_CONFIG_VALIDATE, ");
-		break;
-	case I2O_CMD_CONN_SETUP:
-		printk("EXEC_CONN_SETUP, ");
-		break;
-	case I2O_CMD_DDM_DESTROY:
-		printk("EXEC_DDM_DESTROY, ");
-		break;
-	case I2O_CMD_DDM_ENABLE:
-		printk("EXEC_DDM_ENABLE, ");
-		break;
-	case I2O_CMD_DDM_QUIESCE:
-		printk("EXEC_DDM_QUIESCE, ");
-		break;
-	case I2O_CMD_DDM_RESET:
-		printk("EXEC_DDM_RESET, ");
-		break;
-	case I2O_CMD_DDM_SUSPEND:
-		printk("EXEC_DDM_SUSPEND, ");
-		break;
-	case I2O_CMD_DEVICE_ASSIGN:
-		printk("EXEC_DEVICE_ASSIGN, ");
-		break;
-	case I2O_CMD_DEVICE_RELEASE:
-		printk("EXEC_DEVICE_RELEASE, ");
-		break;
-	case I2O_CMD_HRT_GET:
-		printk("EXEC_HRT_GET, ");
-		break;
-	case I2O_CMD_ADAPTER_CLEAR:
-		printk("EXEC_IOP_CLEAR, ");
-		break;
-	case I2O_CMD_ADAPTER_CONNECT:
-		printk("EXEC_IOP_CONNECT, ");
-		break;
-	case I2O_CMD_ADAPTER_RESET:
-		printk("EXEC_IOP_RESET, ");
-		break;
-	case I2O_CMD_LCT_NOTIFY:
-		printk("EXEC_LCT_NOTIFY, ");
-		break;
-	case I2O_CMD_OUTBOUND_INIT:
-		printk("EXEC_OUTBOUND_INIT, ");
-		break;
-	case I2O_CMD_PATH_ENABLE:
-		printk("EXEC_PATH_ENABLE, ");
-		break;
-	case I2O_CMD_PATH_QUIESCE:
-		printk("EXEC_PATH_QUIESCE, ");
-		break;
-	case I2O_CMD_PATH_RESET:
-		printk("EXEC_PATH_RESET, ");
-		break;
-	case I2O_CMD_STATIC_MF_CREATE:
-		printk("EXEC_STATIC_MF_CREATE, ");
-		break;
-	case I2O_CMD_STATIC_MF_RELEASE:
-		printk("EXEC_STATIC_MF_RELEASE, ");
-		break;
-	case I2O_CMD_STATUS_GET:
-		printk("EXEC_STATUS_GET, ");
-		break;
-	case I2O_CMD_SW_DOWNLOAD:
-		printk("EXEC_SW_DOWNLOAD, ");
-		break;
-	case I2O_CMD_SW_UPLOAD:
-		printk("EXEC_SW_UPLOAD, ");
-		break;
-	case I2O_CMD_SW_REMOVE:
-		printk("EXEC_SW_REMOVE, ");
-		break;
-	case I2O_CMD_SYS_ENABLE:
-		printk("EXEC_SYS_ENABLE, ");
-		break;
-	case I2O_CMD_SYS_MODIFY:
-		printk("EXEC_SYS_MODIFY, ");
-		break;
-	case I2O_CMD_SYS_QUIESCE:
-		printk("EXEC_SYS_QUIESCE, ");
-		break;
-	case I2O_CMD_SYS_TAB_SET:
-		printk("EXEC_SYS_TAB_SET, ");
-		break;
-	default:
-		printk("Cmd = %#02x, ", cmd);
-	}
-}
-
-void i2o_debug_state(struct i2o_controller *c)
-{
-	printk(KERN_INFO "%s: State = ", c->name);
-	switch (((i2o_status_block *) c->status_block.virt)->iop_state) {
-	case 0x01:
-		printk("INIT\n");
-		break;
-	case 0x02:
-		printk("RESET\n");
-		break;
-	case 0x04:
-		printk("HOLD\n");
-		break;
-	case 0x05:
-		printk("READY\n");
-		break;
-	case 0x08:
-		printk("OPERATIONAL\n");
-		break;
-	case 0x10:
-		printk("FAILED\n");
-		break;
-	case 0x11:
-		printk("FAULTED\n");
-		break;
-	default:
-		printk("%x (unknown !!)\n",
-		       ((i2o_status_block *) c->status_block.virt)->iop_state);
-	}
-};
-
-void i2o_dump_hrt(struct i2o_controller *c)
-{
-	u32 *rows = (u32 *) c->hrt.virt;
-	u8 *p = (u8 *) c->hrt.virt;
-	u8 *d;
-	int count;
-	int length;
-	int i;
-	int state;
-
-	if (p[3] != 0) {
-		printk(KERN_ERR
-		       "%s: HRT table for controller is too new a version.\n",
-		       c->name);
-		return;
-	}
-
-	count = p[0] | (p[1] << 8);
-	length = p[2];
-
-	printk(KERN_INFO "%s: HRT has %d entries of %d bytes each.\n",
-	       c->name, count, length << 2);
-
-	rows += 2;
-
-	for (i = 0; i < count; i++) {
-		printk(KERN_INFO "Adapter %08X: ", rows[0]);
-		p = (u8 *) (rows + 1);
-		d = (u8 *) (rows + 2);
-		state = p[1] << 8 | p[0];
-
-		printk("TID %04X:[", state & 0xFFF);
-		state >>= 12;
-		if (state & (1 << 0))
-			printk("H");	/* Hidden */
-		if (state & (1 << 2)) {
-			printk("P");	/* Present */
-			if (state & (1 << 1))
-				printk("C");	/* Controlled */
-		}
-		if (state > 9)
-			printk("*");	/* Hard */
-
-		printk("]:");
-
-		switch (p[3] & 0xFFFF) {
-		case 0:
-			/* Adapter private bus - easy */
-			printk("Local bus %d: I/O at 0x%04X Mem 0x%08X", p[2],
-			       d[1] << 8 | d[0], *(u32 *) (d + 4));
-			break;
-		case 1:
-			/* ISA bus */
-			printk("ISA %d: CSN %d I/O at 0x%04X Mem 0x%08X", p[2],
-			       d[2], d[1] << 8 | d[0], *(u32 *) (d + 4));
-			break;
-
-		case 2:	/* EISA bus */
-			printk("EISA %d: Slot %d I/O at 0x%04X Mem 0x%08X",
-			       p[2], d[3], d[1] << 8 | d[0], *(u32 *) (d + 4));
-			break;
-
-		case 3:	/* MCA bus */
-			printk("MCA %d: Slot %d I/O at 0x%04X Mem 0x%08X", p[2],
-			       d[3], d[1] << 8 | d[0], *(u32 *) (d + 4));
-			break;
-
-		case 4:	/* PCI bus */
-			printk("PCI %d: Bus %d Device %d Function %d", p[2],
-			       d[2], d[1], d[0]);
-			break;
-
-		case 0x80:	/* Other */
-		default:
-			printk("Unsupported bus type.");
-			break;
-		}
-		printk("\n");
-		rows += length;
-	}
-}
-
-EXPORT_SYMBOL(i2o_dump_message);
diff --git a/drivers/staging/i2o/device.c b/drivers/staging/i2o/device.c
deleted file mode 100644
index e47496cb0ac2..000000000000
--- a/drivers/staging/i2o/device.c
+++ /dev/null
@@ -1,592 +0,0 @@
-/*
- *	Functions to handle I2O devices
- *
- *	Copyright (C) 2004	Markus Lidel <Markus.Lidel@shadowconnect.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.
- *
- *	Fixes/additions:
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>
- *			initial version.
- */
-
-#include <linux/module.h>
-#include "i2o.h"
-#include <linux/delay.h>
-#include <linux/string.h>
-#include <linux/slab.h>
-#include "core.h"
-
-/**
- *	i2o_device_issue_claim - claim or release a device
- *	@dev: I2O device to claim or release
- *	@cmd: claim or release command
- *	@type: type of claim
- *
- *	Issue I2O UTIL_CLAIM or UTIL_RELEASE messages. The message to be sent
- *	is set by cmd. dev is the I2O device which should be claim or
- *	released and the type is the claim type (see the I2O spec).
- *
- *	Returs 0 on success or negative error code on failure.
- */
-static inline int i2o_device_issue_claim(struct i2o_device *dev, u32 cmd,
-					 u32 type)
-{
-	struct i2o_message *msg;
-
-	msg = i2o_msg_get_wait(dev->iop, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FIVE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(cmd << 24 | HOST_TID << 12 | dev->lct_data.tid);
-	msg->body[0] = cpu_to_le32(type);
-
-	return i2o_msg_post_wait(dev->iop, msg, 60);
-}
-
-/**
- *	i2o_device_claim - claim a device for use by an OSM
- *	@dev: I2O device to claim
- *
- *	Do the leg work to assign a device to a given OSM. If the claim succeeds,
- *	the owner is the primary. If the attempt fails a negative errno code
- *	is returned. On success zero is returned.
- */
-int i2o_device_claim(struct i2o_device *dev)
-{
-	int rc = 0;
-
-	mutex_lock(&dev->lock);
-
-	rc = i2o_device_issue_claim(dev, I2O_CMD_UTIL_CLAIM, I2O_CLAIM_PRIMARY);
-	if (!rc)
-		pr_debug("i2o: claim of device %d succeeded\n",
-			 dev->lct_data.tid);
-	else
-		pr_debug("i2o: claim of device %d failed %d\n",
-			 dev->lct_data.tid, rc);
-
-	mutex_unlock(&dev->lock);
-
-	return rc;
-}
-
-/**
- *	i2o_device_claim_release - release a device that the OSM is using
- *	@dev: device to release
- *
- *	Drop a claim by an OSM on a given I2O device.
- *
- *	AC - some devices seem to want to refuse an unclaim until they have
- *	finished internal processing. It makes sense since you don't want a
- *	new device to go reconfiguring the entire system until you are done.
- *	Thus we are prepared to wait briefly.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int i2o_device_claim_release(struct i2o_device *dev)
-{
-	int tries;
-	int rc = 0;
-
-	mutex_lock(&dev->lock);
-
-	/*
-	 *      If the controller takes a nonblocking approach to
-	 *      releases we have to sleep/poll for a few times.
-	 */
-	for (tries = 0; tries < 10; tries++) {
-		rc = i2o_device_issue_claim(dev, I2O_CMD_UTIL_RELEASE,
-					    I2O_CLAIM_PRIMARY);
-		if (!rc)
-			break;
-
-		ssleep(1);
-	}
-
-	if (!rc)
-		pr_debug("i2o: claim release of device %d succeeded\n",
-			 dev->lct_data.tid);
-	else
-		pr_debug("i2o: claim release of device %d failed %d\n",
-			 dev->lct_data.tid, rc);
-
-	mutex_unlock(&dev->lock);
-
-	return rc;
-}
-
-/**
- *	i2o_device_release - release the memory for a I2O device
- *	@dev: I2O device which should be released
- *
- *	Release the allocated memory. This function is called if refcount of
- *	device reaches 0 automatically.
- */
-static void i2o_device_release(struct device *dev)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-
-	pr_debug("i2o: device %s released\n", dev_name(dev));
-
-	kfree(i2o_dev);
-}
-
-/**
- *	class_id_show - Displays class id of I2O device
- *	@dev: device of which the class id should be displayed
- *	@attr: pointer to device attribute
- *	@buf: buffer into which the class id should be printed
- *
- *	Returns the number of bytes which are printed into the buffer.
- */
-static ssize_t class_id_show(struct device *dev, struct device_attribute *attr,
-			     char *buf)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-
-	sprintf(buf, "0x%03x\n", i2o_dev->lct_data.class_id);
-	return strlen(buf) + 1;
-}
-static DEVICE_ATTR_RO(class_id);
-
-/**
- *	tid_show - Displays TID of I2O device
- *	@dev: device of which the TID should be displayed
- *	@attr: pointer to device attribute
- *	@buf: buffer into which the TID should be printed
- *
- *	Returns the number of bytes which are printed into the buffer.
- */
-static ssize_t tid_show(struct device *dev, struct device_attribute *attr,
-			char *buf)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-
-	sprintf(buf, "0x%03x\n", i2o_dev->lct_data.tid);
-	return strlen(buf) + 1;
-}
-static DEVICE_ATTR_RO(tid);
-
-/* I2O device attributes */
-static struct attribute *i2o_device_attrs[] = {
-	&dev_attr_class_id.attr,
-	&dev_attr_tid.attr,
-	NULL,
-};
-
-static const struct attribute_group i2o_device_group = {
-	.attrs = i2o_device_attrs,
-};
-
-const struct attribute_group *i2o_device_groups[] = {
-	&i2o_device_group,
-	NULL,
-};
-
-/**
- *	i2o_device_alloc - Allocate a I2O device and initialize it
- *
- *	Allocate the memory for a I2O device and initialize locks and lists
- *
- *	Returns the allocated I2O device or a negative error code if the device
- *	could not be allocated.
- */
-static struct i2o_device *i2o_device_alloc(void)
-{
-	struct i2o_device *dev;
-
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (!dev)
-		return ERR_PTR(-ENOMEM);
-
-	INIT_LIST_HEAD(&dev->list);
-	mutex_init(&dev->lock);
-
-	dev->device.bus = &i2o_bus_type;
-	dev->device.release = &i2o_device_release;
-
-	return dev;
-}
-
-/**
- *	i2o_device_add - allocate a new I2O device and add it to the IOP
- *	@c: I2O controller that the device is on
- *	@entry: LCT entry of the I2O device
- *
- *	Allocate a new I2O device and initialize it with the LCT entry. The
- *	device is appended to the device list of the controller.
- *
- *	Returns zero on success, or a -ve errno.
- */
-static int i2o_device_add(struct i2o_controller *c, i2o_lct_entry *entry)
-{
-	struct i2o_device *i2o_dev, *tmp;
-	int rc;
-
-	i2o_dev = i2o_device_alloc();
-	if (IS_ERR(i2o_dev)) {
-		printk(KERN_ERR "i2o: unable to allocate i2o device\n");
-		return PTR_ERR(i2o_dev);
-	}
-
-	i2o_dev->lct_data = *entry;
-
-	dev_set_name(&i2o_dev->device, "%d:%03x", c->unit,
-		     i2o_dev->lct_data.tid);
-
-	i2o_dev->iop = c;
-	i2o_dev->device.parent = &c->device;
-
-	rc = device_register(&i2o_dev->device);
-	if (rc)
-		goto err;
-
-	list_add_tail(&i2o_dev->list, &c->devices);
-
-	/* create user entries for this device */
-	tmp = i2o_iop_find_device(i2o_dev->iop, i2o_dev->lct_data.user_tid);
-	if (tmp && (tmp != i2o_dev)) {
-		rc = sysfs_create_link(&i2o_dev->device.kobj,
-				       &tmp->device.kobj, "user");
-		if (rc)
-			goto unreg_dev;
-	}
-
-	/* create user entries referring to this device */
-	list_for_each_entry(tmp, &c->devices, list)
-	    if ((tmp->lct_data.user_tid == i2o_dev->lct_data.tid)
-		&& (tmp != i2o_dev)) {
-		rc = sysfs_create_link(&tmp->device.kobj,
-				       &i2o_dev->device.kobj, "user");
-		if (rc)
-			goto rmlink1;
-	}
-
-	/* create parent entries for this device */
-	tmp = i2o_iop_find_device(i2o_dev->iop, i2o_dev->lct_data.parent_tid);
-	if (tmp && (tmp != i2o_dev)) {
-		rc = sysfs_create_link(&i2o_dev->device.kobj,
-				       &tmp->device.kobj, "parent");
-		if (rc)
-			goto rmlink1;
-	}
-
-	/* create parent entries referring to this device */
-	list_for_each_entry(tmp, &c->devices, list)
-	    if ((tmp->lct_data.parent_tid == i2o_dev->lct_data.tid)
-		&& (tmp != i2o_dev)) {
-		rc = sysfs_create_link(&tmp->device.kobj,
-				       &i2o_dev->device.kobj, "parent");
-		if (rc)
-			goto rmlink2;
-	}
-
-	i2o_driver_notify_device_add_all(i2o_dev);
-
-	pr_debug("i2o: device %s added\n", dev_name(&i2o_dev->device));
-
-	return 0;
-
-rmlink2:
-	/* If link creating failed halfway, we loop whole list to cleanup.
-	 * And we don't care wrong removing of link, because sysfs_remove_link
-	 * will take care of it.
-	 */
-	list_for_each_entry(tmp, &c->devices, list) {
-		if (tmp->lct_data.parent_tid == i2o_dev->lct_data.tid)
-			sysfs_remove_link(&tmp->device.kobj, "parent");
-	}
-	sysfs_remove_link(&i2o_dev->device.kobj, "parent");
-rmlink1:
-	list_for_each_entry(tmp, &c->devices, list)
-		if (tmp->lct_data.user_tid == i2o_dev->lct_data.tid)
-			sysfs_remove_link(&tmp->device.kobj, "user");
-	sysfs_remove_link(&i2o_dev->device.kobj, "user");
-unreg_dev:
-	list_del(&i2o_dev->list);
-	device_unregister(&i2o_dev->device);
-err:
-	kfree(i2o_dev);
-	return rc;
-}
-
-/**
- *	i2o_device_remove - remove an I2O device from the I2O core
- *	@i2o_dev: I2O device which should be released
- *
- *	Is used on I2O controller removal or LCT modification, when the device
- *	is removed from the system. Note that the device could still hang
- *	around until the refcount reaches 0.
- */
-void i2o_device_remove(struct i2o_device *i2o_dev)
-{
-	struct i2o_device *tmp;
-	struct i2o_controller *c = i2o_dev->iop;
-
-	i2o_driver_notify_device_remove_all(i2o_dev);
-
-	sysfs_remove_link(&i2o_dev->device.kobj, "parent");
-	sysfs_remove_link(&i2o_dev->device.kobj, "user");
-
-	list_for_each_entry(tmp, &c->devices, list) {
-		if (tmp->lct_data.parent_tid == i2o_dev->lct_data.tid)
-			sysfs_remove_link(&tmp->device.kobj, "parent");
-		if (tmp->lct_data.user_tid == i2o_dev->lct_data.tid)
-			sysfs_remove_link(&tmp->device.kobj, "user");
-	}
-	list_del(&i2o_dev->list);
-
-	device_unregister(&i2o_dev->device);
-}
-
-/**
- *	i2o_device_parse_lct - Parse a previously fetched LCT and create devices
- *	@c: I2O controller from which the LCT should be parsed.
- *
- *	The Logical Configuration Table tells us what we can talk to on the
- *	board. For every entry we create an I2O device, which is registered in
- *	the I2O core.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int i2o_device_parse_lct(struct i2o_controller *c)
-{
-	struct i2o_device *dev, *tmp;
-	i2o_lct *lct;
-	u32 *dlct = c->dlct.virt;
-	int max = 0, i = 0;
-	u16 table_size;
-	u32 buf;
-
-	mutex_lock(&c->lct_lock);
-
-	kfree(c->lct);
-
-	buf = le32_to_cpu(*dlct++);
-	table_size = buf & 0xffff;
-
-	lct = c->lct = kmalloc(table_size * 4, GFP_KERNEL);
-	if (!lct) {
-		mutex_unlock(&c->lct_lock);
-		return -ENOMEM;
-	}
-
-	lct->lct_ver = buf >> 28;
-	lct->boot_tid = buf >> 16 & 0xfff;
-	lct->table_size = table_size;
-	lct->change_ind = le32_to_cpu(*dlct++);
-	lct->iop_flags = le32_to_cpu(*dlct++);
-
-	table_size -= 3;
-
-	pr_debug("%s: LCT has %d entries (LCT size: %d)\n", c->name, max,
-		 lct->table_size);
-
-	while (table_size > 0) {
-		i2o_lct_entry *entry = &lct->lct_entry[max];
-		int found = 0;
-
-		buf = le32_to_cpu(*dlct++);
-		entry->entry_size = buf & 0xffff;
-		entry->tid = buf >> 16 & 0xfff;
-
-		entry->change_ind = le32_to_cpu(*dlct++);
-		entry->device_flags = le32_to_cpu(*dlct++);
-
-		buf = le32_to_cpu(*dlct++);
-		entry->class_id = buf & 0xfff;
-		entry->version = buf >> 12 & 0xf;
-		entry->vendor_id = buf >> 16;
-
-		entry->sub_class = le32_to_cpu(*dlct++);
-
-		buf = le32_to_cpu(*dlct++);
-		entry->user_tid = buf & 0xfff;
-		entry->parent_tid = buf >> 12 & 0xfff;
-		entry->bios_info = buf >> 24;
-
-		memcpy(&entry->identity_tag, dlct, 8);
-		dlct += 2;
-
-		entry->event_capabilities = le32_to_cpu(*dlct++);
-
-		/* add new devices, which are new in the LCT */
-		list_for_each_entry_safe(dev, tmp, &c->devices, list) {
-			if (entry->tid == dev->lct_data.tid) {
-				found = 1;
-				break;
-			}
-		}
-
-		if (!found)
-			i2o_device_add(c, entry);
-
-		table_size -= 9;
-		max++;
-	}
-
-	/* remove devices, which are not in the LCT anymore */
-	list_for_each_entry_safe(dev, tmp, &c->devices, list) {
-		int found = 0;
-
-		for (i = 0; i < max; i++) {
-			if (lct->lct_entry[i].tid == dev->lct_data.tid) {
-				found = 1;
-				break;
-			}
-		}
-
-		if (!found)
-			i2o_device_remove(dev);
-	}
-
-	mutex_unlock(&c->lct_lock);
-
-	return 0;
-}
-
-/*
- *	Run time support routines
- */
-
-/*	Issue UTIL_PARAMS_GET or UTIL_PARAMS_SET
- *
- *	This function can be used for all UtilParamsGet/Set operations.
- *	The OperationList is given in oplist-buffer,
- *	and results are returned in reslist-buffer.
- *	Note that the minimum sized reslist is 8 bytes and contains
- *	ResultCount, ErrorInfoSize, BlockStatus and BlockSize.
- */
-int i2o_parm_issue(struct i2o_device *i2o_dev, int cmd, void *oplist,
-		   int oplen, void *reslist, int reslen)
-{
-	struct i2o_message *msg;
-	int i = 0;
-	int rc;
-	struct i2o_dma res;
-	struct i2o_controller *c = i2o_dev->iop;
-	struct device *dev = &c->pdev->dev;
-
-	res.virt = NULL;
-
-	if (i2o_dma_alloc(dev, &res, reslen))
-		return -ENOMEM;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg)) {
-		i2o_dma_free(dev, &res);
-		return PTR_ERR(msg);
-	}
-
-	i = 0;
-	msg->u.head[1] =
-	    cpu_to_le32(cmd << 24 | HOST_TID << 12 | i2o_dev->lct_data.tid);
-	msg->body[i++] = cpu_to_le32(0x00000000);
-	msg->body[i++] = cpu_to_le32(0x4C000000 | oplen);	/* OperationList */
-	memcpy(&msg->body[i], oplist, oplen);
-	i += (oplen / 4 + (oplen % 4 ? 1 : 0));
-	msg->body[i++] = cpu_to_le32(0xD0000000 | res.len);	/* ResultList */
-	msg->body[i++] = cpu_to_le32(res.phys);
-
-	msg->u.head[0] =
-	    cpu_to_le32(I2O_MESSAGE_SIZE(i + sizeof(struct i2o_message) / 4) |
-			SGL_OFFSET_5);
-
-	rc = i2o_msg_post_wait_mem(c, msg, 10, &res);
-
-	/* This only looks like a memory leak - don't "fix" it. */
-	if (rc == -ETIMEDOUT)
-		return rc;
-
-	memcpy(reslist, res.virt, res.len);
-	i2o_dma_free(dev, &res);
-
-	return rc;
-}
-
-/*
- *	 Query one field group value or a whole scalar group.
- */
-int i2o_parm_field_get(struct i2o_device *i2o_dev, int group, int field,
-		       void *buf, int buflen)
-{
-	u32 opblk[] = { cpu_to_le32(0x00000001),
-		cpu_to_le32((u16) group << 16 | I2O_PARAMS_FIELD_GET),
-		cpu_to_le32((s16) field << 16 | 0x00000001)
-	};
-	u8 *resblk;		/* 8 bytes for header */
-	int rc;
-
-	resblk = kmalloc(buflen + 8, GFP_KERNEL);
-	if (!resblk)
-		return -ENOMEM;
-
-	rc = i2o_parm_issue(i2o_dev, I2O_CMD_UTIL_PARAMS_GET, opblk,
-			    sizeof(opblk), resblk, buflen + 8);
-
-	memcpy(buf, resblk + 8, buflen);	/* cut off header */
-
-	kfree(resblk);
-
-	return rc;
-}
-
-/*
- *	if oper == I2O_PARAMS_TABLE_GET, get from all rows
- *		if fieldcount == -1 return all fields
- *			ibuf and ibuflen are unused (use NULL, 0)
- *		else return specific fields
- *			ibuf contains fieldindexes
- *
- *	if oper == I2O_PARAMS_LIST_GET, get from specific rows
- *		if fieldcount == -1 return all fields
- *			ibuf contains rowcount, keyvalues
- *		else return specific fields
- *			fieldcount is # of fieldindexes
- *			ibuf contains fieldindexes, rowcount, keyvalues
- *
- *	You could also use directly function i2o_issue_params().
- */
-int i2o_parm_table_get(struct i2o_device *dev, int oper, int group,
-		       int fieldcount, void *ibuf, int ibuflen, void *resblk,
-		       int reslen)
-{
-	u16 *opblk;
-	int size;
-
-	size = 10 + ibuflen;
-	if (size % 4)
-		size += 4 - size % 4;
-
-	opblk = kmalloc(size, GFP_KERNEL);
-	if (opblk == NULL)
-		return -ENOMEM;
-
-	opblk[0] = 1;		/* operation count */
-	opblk[1] = 0;		/* pad */
-	opblk[2] = oper;
-	opblk[3] = group;
-	opblk[4] = fieldcount;
-	memcpy(opblk + 5, ibuf, ibuflen);	/* other params */
-
-	size = i2o_parm_issue(dev, I2O_CMD_UTIL_PARAMS_GET, opblk,
-			      size, resblk, reslen);
-
-	kfree(opblk);
-	if (size > reslen)
-		return reslen;
-
-	return size;
-}
-
-EXPORT_SYMBOL(i2o_device_claim);
-EXPORT_SYMBOL(i2o_device_claim_release);
-EXPORT_SYMBOL(i2o_parm_field_get);
-EXPORT_SYMBOL(i2o_parm_table_get);
-EXPORT_SYMBOL(i2o_parm_issue);
diff --git a/drivers/staging/i2o/driver.c b/drivers/staging/i2o/driver.c
deleted file mode 100644
index 06119bb3eb5f..000000000000
--- a/drivers/staging/i2o/driver.c
+++ /dev/null
@@ -1,381 +0,0 @@
-/*
- *	Functions to handle I2O drivers (OSMs) and I2O bus type for sysfs
- *
- *	Copyright (C) 2004	Markus Lidel <Markus.Lidel@shadowconnect.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.
- *
- *	Fixes/additions:
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>
- *			initial version.
- */
-
-#include <linux/device.h>
-#include <linux/module.h>
-#include <linux/rwsem.h>
-#include "i2o.h"
-#include <linux/workqueue.h>
-#include <linux/string.h>
-#include <linux/slab.h>
-#include "core.h"
-
-#define OSM_NAME	"i2o"
-
-/* max_drivers - Maximum I2O drivers (OSMs) which could be registered */
-static unsigned int i2o_max_drivers = I2O_MAX_DRIVERS;
-module_param_named(max_drivers, i2o_max_drivers, uint, 0);
-MODULE_PARM_DESC(max_drivers, "maximum number of OSM's to support");
-
-/* I2O drivers lock and array */
-static spinlock_t i2o_drivers_lock;
-static struct i2o_driver **i2o_drivers;
-
-/**
- *	i2o_bus_match - Tell if I2O device class id matches the class ids of the I2O driver (OSM)
- *	@dev: device which should be verified
- *	@drv: the driver to match against
- *
- *	Used by the bus to check if the driver wants to handle the device.
- *
- *	Returns 1 if the class ids of the driver match the class id of the
- *	device, otherwise 0.
- */
-static int i2o_bus_match(struct device *dev, struct device_driver *drv)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-	struct i2o_driver *i2o_drv = to_i2o_driver(drv);
-	struct i2o_class_id *ids = i2o_drv->classes;
-
-	if (ids)
-		while (ids->class_id != I2O_CLASS_END) {
-			if (ids->class_id == i2o_dev->lct_data.class_id)
-				return 1;
-			ids++;
-		}
-	return 0;
-};
-
-/* I2O bus type */
-struct bus_type i2o_bus_type = {
-	.name = "i2o",
-	.match = i2o_bus_match,
-	.dev_groups = i2o_device_groups,
-};
-
-/**
- *	i2o_driver_register - Register a I2O driver (OSM) in the I2O core
- *	@drv: I2O driver which should be registered
- *
- *	Registers the OSM drv in the I2O core and creates an event queues if
- *	necessary.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int i2o_driver_register(struct i2o_driver *drv)
-{
-	struct i2o_controller *c;
-	int i;
-	int rc = 0;
-	unsigned long flags;
-
-	osm_debug("Register driver %s\n", drv->name);
-
-	if (drv->event) {
-		drv->event_queue = alloc_workqueue("%s", WQ_MEM_RECLAIM, 1,
-						   drv->name);
-		if (!drv->event_queue) {
-			osm_err("Could not initialize event queue for driver "
-				"%s\n", drv->name);
-			return -EFAULT;
-		}
-		osm_debug("Event queue initialized for driver %s\n", drv->name);
-	} else
-		drv->event_queue = NULL;
-
-	drv->driver.name = drv->name;
-	drv->driver.bus = &i2o_bus_type;
-
-	spin_lock_irqsave(&i2o_drivers_lock, flags);
-
-	for (i = 0; i2o_drivers[i]; i++)
-		if (i >= i2o_max_drivers) {
-			osm_err("too many drivers registered, increase max_drivers\n");
-			spin_unlock_irqrestore(&i2o_drivers_lock, flags);
-			rc = -EFAULT;
-			goto out;
-		}
-
-	drv->context = i;
-	i2o_drivers[i] = drv;
-
-	spin_unlock_irqrestore(&i2o_drivers_lock, flags);
-
-	osm_debug("driver %s gets context id %d\n", drv->name, drv->context);
-
-	list_for_each_entry(c, &i2o_controllers, list) {
-		struct i2o_device *i2o_dev;
-
-		i2o_driver_notify_controller_add(drv, c);
-		list_for_each_entry(i2o_dev, &c->devices, list)
-		    i2o_driver_notify_device_add(drv, i2o_dev);
-	}
-
-	rc = driver_register(&drv->driver);
-	if (rc)
-		goto out;
-
-	return 0;
-out:
-	if (drv->event_queue) {
-		destroy_workqueue(drv->event_queue);
-		drv->event_queue = NULL;
-	}
-
-	return rc;
-};
-
-/**
- *	i2o_driver_unregister - Unregister a I2O driver (OSM) from the I2O core
- *	@drv: I2O driver which should be unregistered
- *
- *	Unregisters the OSM drv from the I2O core and cleanup event queues if
- *	necessary.
- */
-void i2o_driver_unregister(struct i2o_driver *drv)
-{
-	struct i2o_controller *c;
-	unsigned long flags;
-
-	osm_debug("unregister driver %s\n", drv->name);
-
-	driver_unregister(&drv->driver);
-
-	list_for_each_entry(c, &i2o_controllers, list) {
-		struct i2o_device *i2o_dev;
-
-		list_for_each_entry(i2o_dev, &c->devices, list)
-		    i2o_driver_notify_device_remove(drv, i2o_dev);
-
-		i2o_driver_notify_controller_remove(drv, c);
-	}
-
-	spin_lock_irqsave(&i2o_drivers_lock, flags);
-	i2o_drivers[drv->context] = NULL;
-	spin_unlock_irqrestore(&i2o_drivers_lock, flags);
-
-	if (drv->event_queue) {
-		destroy_workqueue(drv->event_queue);
-		drv->event_queue = NULL;
-		osm_debug("event queue removed for %s\n", drv->name);
-	}
-};
-
-/**
- *	i2o_driver_dispatch - dispatch an I2O reply message
- *	@c: I2O controller of the message
- *	@m: I2O message number
- *
- *	The reply is delivered to the driver from which the original message
- *	was. This function is only called from interrupt context.
- *
- *	Returns 0 on success and the message should not be flushed. Returns > 0
- *	on success and if the message should be flushed afterwords. Returns
- *	negative error code on failure (the message will be flushed too).
- */
-int i2o_driver_dispatch(struct i2o_controller *c, u32 m)
-{
-	struct i2o_driver *drv;
-	struct i2o_message *msg = i2o_msg_out_to_virt(c, m);
-	u32 context = le32_to_cpu(msg->u.s.icntxt);
-	unsigned long flags;
-
-	if (unlikely(context >= i2o_max_drivers)) {
-		osm_warn("%s: Spurious reply to unknown driver %d\n", c->name,
-			 context);
-		return -EIO;
-	}
-
-	spin_lock_irqsave(&i2o_drivers_lock, flags);
-	drv = i2o_drivers[context];
-	spin_unlock_irqrestore(&i2o_drivers_lock, flags);
-
-	if (unlikely(!drv)) {
-		osm_warn("%s: Spurious reply to unknown driver %d\n", c->name,
-			 context);
-		return -EIO;
-	}
-
-	if ((le32_to_cpu(msg->u.head[1]) >> 24) == I2O_CMD_UTIL_EVT_REGISTER) {
-		struct i2o_device *dev, *tmp;
-		struct i2o_event *evt;
-		u16 size;
-		u16 tid = le32_to_cpu(msg->u.head[1]) & 0xfff;
-
-		osm_debug("event received from device %d\n", tid);
-
-		if (!drv->event)
-			return -EIO;
-
-		/* cut of header from message size (in 32-bit words) */
-		size = (le32_to_cpu(msg->u.head[0]) >> 16) - 5;
-
-		evt = kzalloc(size * 4 + sizeof(*evt), GFP_ATOMIC);
-		if (!evt)
-			return -ENOMEM;
-
-		evt->size = size;
-		evt->tcntxt = le32_to_cpu(msg->u.s.tcntxt);
-		evt->event_indicator = le32_to_cpu(msg->body[0]);
-		memcpy(&evt->data, &msg->body[1], size * 4);
-
-		list_for_each_entry_safe(dev, tmp, &c->devices, list)
-		    if (dev->lct_data.tid == tid) {
-			evt->i2o_dev = dev;
-			break;
-		}
-
-		INIT_WORK(&evt->work, drv->event);
-		queue_work(drv->event_queue, &evt->work);
-		return 1;
-	}
-
-	if (unlikely(!drv->reply)) {
-		osm_debug("%s: Reply to driver %s, but no reply function defined!\n",
-			c->name, drv->name);
-		return -EIO;
-	}
-
-	return drv->reply(c, m, msg);
-}
-
-/**
- *	i2o_driver_notify_controller_add_all - Send notify of added controller
- *	@c: newly added controller
- *
- *	Send notifications to all registered drivers that a new controller was
- *	added.
- */
-void i2o_driver_notify_controller_add_all(struct i2o_controller *c)
-{
-	int i;
-	struct i2o_driver *drv;
-
-	for (i = 0; i < i2o_max_drivers; i++) {
-		drv = i2o_drivers[i];
-
-		if (drv)
-			i2o_driver_notify_controller_add(drv, c);
-	}
-}
-
-/**
- *	i2o_driver_notify_controller_remove_all - Send notify of removed controller
- *	@c: controller that is being removed
- *
- *	Send notifications to all registered drivers that a controller was
- *	removed.
- */
-void i2o_driver_notify_controller_remove_all(struct i2o_controller *c)
-{
-	int i;
-	struct i2o_driver *drv;
-
-	for (i = 0; i < i2o_max_drivers; i++) {
-		drv = i2o_drivers[i];
-
-		if (drv)
-			i2o_driver_notify_controller_remove(drv, c);
-	}
-}
-
-/**
- *	i2o_driver_notify_device_add_all - Send notify of added device
- *	@i2o_dev: newly added I2O device
- *
- *	Send notifications to all registered drivers that a device was added.
- */
-void i2o_driver_notify_device_add_all(struct i2o_device *i2o_dev)
-{
-	int i;
-	struct i2o_driver *drv;
-
-	for (i = 0; i < i2o_max_drivers; i++) {
-		drv = i2o_drivers[i];
-
-		if (drv)
-			i2o_driver_notify_device_add(drv, i2o_dev);
-	}
-}
-
-/**
- *	i2o_driver_notify_device_remove_all - Send notify of removed device
- *	@i2o_dev: device that is being removed
- *
- *	Send notifications to all registered drivers that a device was removed.
- */
-void i2o_driver_notify_device_remove_all(struct i2o_device *i2o_dev)
-{
-	int i;
-	struct i2o_driver *drv;
-
-	for (i = 0; i < i2o_max_drivers; i++) {
-		drv = i2o_drivers[i];
-
-		if (drv)
-			i2o_driver_notify_device_remove(drv, i2o_dev);
-	}
-}
-
-/**
- *	i2o_driver_init - initialize I2O drivers (OSMs)
- *
- *	Registers the I2O bus and allocate memory for the array of OSMs.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int __init i2o_driver_init(void)
-{
-	int rc = 0;
-
-	spin_lock_init(&i2o_drivers_lock);
-
-	if ((i2o_max_drivers < 2) || (i2o_max_drivers > 64)) {
-		osm_warn("max_drivers set to %d, but must be >=2 and <= 64\n",
-			 i2o_max_drivers);
-		i2o_max_drivers = I2O_MAX_DRIVERS;
-	}
-	osm_info("max drivers = %d\n", i2o_max_drivers);
-
-	i2o_drivers =
-	    kcalloc(i2o_max_drivers, sizeof(*i2o_drivers), GFP_KERNEL);
-	if (!i2o_drivers)
-		return -ENOMEM;
-
-	rc = bus_register(&i2o_bus_type);
-
-	if (rc < 0)
-		kfree(i2o_drivers);
-
-	return rc;
-};
-
-/**
- *	i2o_driver_exit - clean up I2O drivers (OSMs)
- *
- *	Unregisters the I2O bus and frees driver array.
- */
-void i2o_driver_exit(void)
-{
-	bus_unregister(&i2o_bus_type);
-	kfree(i2o_drivers);
-};
-
-EXPORT_SYMBOL(i2o_driver_register);
-EXPORT_SYMBOL(i2o_driver_unregister);
-EXPORT_SYMBOL(i2o_driver_notify_controller_add_all);
-EXPORT_SYMBOL(i2o_driver_notify_controller_remove_all);
-EXPORT_SYMBOL(i2o_driver_notify_device_add_all);
-EXPORT_SYMBOL(i2o_driver_notify_device_remove_all);
diff --git a/drivers/staging/i2o/exec-osm.c b/drivers/staging/i2o/exec-osm.c
deleted file mode 100644
index dce16e425a6e..000000000000
--- a/drivers/staging/i2o/exec-osm.c
+++ /dev/null
@@ -1,612 +0,0 @@
-/*
- *	Executive OSM
- *
- * 	Copyright (C) 1999-2002	Red Hat Software
- *
- *	Written by Alan Cox, Building Number Three Ltd
- *
- *	This program is free software; you can redistribute it and/or modify it
- *	under the terms of the GNU General Public License as published by the
- *	Free Software Foundation; either version 2 of the License, or (at your
- *	option) any later version.
- *
- *	A lot of the I2O message side code from this is taken from the Red
- *	Creek RCPCI45 adapter driver by Red Creek Communications
- *
- *	Fixes/additions:
- *		Philipp Rumpf
- *		Juha Sievänen <Juha.Sievanen@cs.Helsinki.FI>
- *		Auvo Häkkinen <Auvo.Hakkinen@cs.Helsinki.FI>
- *		Deepak Saxena <deepak@plexity.net>
- *		Boji T Kannanthanam <boji.t.kannanthanam@intel.com>
- *		Alan Cox <alan@lxorguk.ukuu.org.uk>:
- *			Ported to Linux 2.5.
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *			Minor fixes for 2.6.
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *			Support for sysfs included.
- */
-
-#include <linux/module.h>
-#include "i2o.h"
-#include <linux/delay.h>
-#include <linux/workqueue.h>
-#include <linux/string.h>
-#include <linux/slab.h>
-#include <linux/sched.h>	/* wait_event_interruptible_timeout() needs this */
-#include <asm/param.h>		/* HZ */
-#include "core.h"
-
-#define OSM_NAME "exec-osm"
-
-struct i2o_driver i2o_exec_driver;
-
-/* global wait list for POST WAIT */
-static LIST_HEAD(i2o_exec_wait_list);
-
-/* Wait struct needed for POST WAIT */
-struct i2o_exec_wait {
-	wait_queue_head_t *wq;	/* Pointer to Wait queue */
-	struct i2o_dma dma;	/* DMA buffers to free on failure */
-	u32 tcntxt;		/* transaction context from reply */
-	int complete;		/* 1 if reply received otherwise 0 */
-	u32 m;			/* message id */
-	struct i2o_message *msg;	/* pointer to the reply message */
-	struct list_head list;	/* node in global wait list */
-	spinlock_t lock;	/* lock before modifying */
-};
-
-/* Work struct needed to handle LCT NOTIFY replies */
-struct i2o_exec_lct_notify_work {
-	struct work_struct work;	/* work struct */
-	struct i2o_controller *c;	/* controller on which the LCT NOTIFY
-					   was received */
-};
-
-/* Exec OSM class handling definition */
-static struct i2o_class_id i2o_exec_class_id[] = {
-	{I2O_CLASS_EXECUTIVE},
-	{I2O_CLASS_END}
-};
-
-/**
- *	i2o_exec_wait_alloc - Allocate a i2o_exec_wait struct an initialize it
- *
- *	Allocate the i2o_exec_wait struct and initialize the wait.
- *
- *	Returns i2o_exec_wait pointer on success or negative error code on
- *	failure.
- */
-static struct i2o_exec_wait *i2o_exec_wait_alloc(void)
-{
-	struct i2o_exec_wait *wait;
-
-	wait = kzalloc(sizeof(*wait), GFP_KERNEL);
-	if (!wait)
-		return NULL;
-
-	INIT_LIST_HEAD(&wait->list);
-	spin_lock_init(&wait->lock);
-
-	return wait;
-};
-
-/**
- *	i2o_exec_wait_free - Free an i2o_exec_wait struct
- *	@wait: I2O wait data which should be cleaned up
- */
-static void i2o_exec_wait_free(struct i2o_exec_wait *wait)
-{
-	kfree(wait);
-};
-
-/**
- * 	i2o_msg_post_wait_mem - Post and wait a message with DMA buffers
- *	@c: controller
- *	@msg: message to post
- *	@timeout: time in seconds to wait
- *	@dma: i2o_dma struct of the DMA buffer to free on failure
- *
- * 	This API allows an OSM to post a message and then be told whether or
- *	not the system received a successful reply. If the message times out
- *	then the value '-ETIMEDOUT' is returned. This is a special case. In
- *	this situation the message may (should) complete at an indefinite time
- *	in the future. When it completes it will use the memory buffer
- *	attached to the request. If -ETIMEDOUT is returned then the memory
- *	buffer must not be freed. Instead the event completion will free them
- *	for you. In all other cases the buffer are your problem.
- *
- *	Returns 0 on success, negative error code on timeout or positive error
- *	code from reply.
- */
-int i2o_msg_post_wait_mem(struct i2o_controller *c, struct i2o_message *msg,
-			  unsigned long timeout, struct i2o_dma *dma)
-{
-	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wq);
-	struct i2o_exec_wait *wait;
-	static u32 tcntxt = 0x80000000;
-	unsigned long flags;
-	int rc = 0;
-
-	wait = i2o_exec_wait_alloc();
-	if (!wait) {
-		i2o_msg_nop(c, msg);
-		return -ENOMEM;
-	}
-
-	if (tcntxt == 0xffffffff)
-		tcntxt = 0x80000000;
-
-	if (dma)
-		wait->dma = *dma;
-
-	/*
-	 * Fill in the message initiator context and transaction context.
-	 * We will only use transaction contexts >= 0x80000000 for POST WAIT,
-	 * so we could find a POST WAIT reply easier in the reply handler.
-	 */
-	msg->u.s.icntxt = cpu_to_le32(i2o_exec_driver.context);
-	wait->tcntxt = tcntxt++;
-	msg->u.s.tcntxt = cpu_to_le32(wait->tcntxt);
-
-	wait->wq = &wq;
-	/*
-	 * we add elements to the head, because if a entry in the list will
-	 * never be removed, we have to iterate over it every time
-	 */
-	list_add(&wait->list, &i2o_exec_wait_list);
-
-	/*
-	 * Post the message to the controller. At some point later it will
-	 * return. If we time out before it returns then complete will be zero.
-	 */
-	i2o_msg_post(c, msg);
-
-	wait_event_interruptible_timeout(wq, wait->complete, timeout * HZ);
-
-	spin_lock_irqsave(&wait->lock, flags);
-
-	wait->wq = NULL;
-
-	if (wait->complete)
-		rc = le32_to_cpu(wait->msg->body[0]) >> 24;
-	else {
-		/*
-		 * We cannot remove it now. This is important. When it does
-		 * terminate (which it must do if the controller has not
-		 * died...) then it will otherwise scribble on stuff.
-		 *
-		 * FIXME: try abort message
-		 */
-		if (dma)
-			dma->virt = NULL;
-
-		rc = -ETIMEDOUT;
-	}
-
-	spin_unlock_irqrestore(&wait->lock, flags);
-
-	if (rc != -ETIMEDOUT) {
-		i2o_flush_reply(c, wait->m);
-		i2o_exec_wait_free(wait);
-	}
-
-	return rc;
-};
-
-/**
- *	i2o_msg_post_wait_complete - Reply to a i2o_msg_post request from IOP
- *	@c: I2O controller which answers
- *	@m: message id
- *	@msg: pointer to the I2O reply message
- *	@context: transaction context of request
- *
- *	This function is called in interrupt context only. If the reply reached
- *	before the timeout, the i2o_exec_wait struct is filled with the message
- *	and the task will be waked up. The task is now responsible for returning
- *	the message m back to the controller! If the message reaches us after
- *	the timeout clean up the i2o_exec_wait struct (including allocated
- *	DMA buffer).
- *
- *	Return 0 on success and if the message m should not be given back to the
- *	I2O controller, or >0 on success and if the message should be given back
- *	afterwords. Returns negative error code on failure. In this case the
- *	message must also be given back to the controller.
- */
-static int i2o_msg_post_wait_complete(struct i2o_controller *c, u32 m,
-				      struct i2o_message *msg, u32 context)
-{
-	struct i2o_exec_wait *wait, *tmp;
-	unsigned long flags;
-	int rc = 1;
-
-	/*
-	 * We need to search through the i2o_exec_wait_list to see if the given
-	 * message is still outstanding. If not, it means that the IOP took
-	 * longer to respond to the message than we had allowed and timer has
-	 * already expired. Not much we can do about that except log it for
-	 * debug purposes, increase timeout, and recompile.
-	 */
-	list_for_each_entry_safe(wait, tmp, &i2o_exec_wait_list, list) {
-		if (wait->tcntxt == context) {
-			spin_lock_irqsave(&wait->lock, flags);
-
-			list_del(&wait->list);
-
-			wait->m = m;
-			wait->msg = msg;
-			wait->complete = 1;
-
-			if (wait->wq)
-				rc = 0;
-			else
-				rc = -1;
-
-			spin_unlock_irqrestore(&wait->lock, flags);
-
-			if (rc) {
-				struct device *dev;
-
-				dev = &c->pdev->dev;
-
-				pr_debug("%s: timedout reply received!\n",
-					 c->name);
-				i2o_dma_free(dev, &wait->dma);
-				i2o_exec_wait_free(wait);
-			} else
-				wake_up_interruptible(wait->wq);
-
-			return rc;
-		}
-	}
-
-	osm_warn("%s: Bogus reply in POST WAIT (tr-context: %08x)!\n", c->name,
-		 context);
-
-	return -1;
-};
-
-/**
- *	i2o_exec_show_vendor_id - Displays Vendor ID of controller
- *	@d: device of which the Vendor ID should be displayed
- *	@attr: device_attribute to display
- *	@buf: buffer into which the Vendor ID should be printed
- *
- *	Returns number of bytes printed into buffer.
- */
-static ssize_t i2o_exec_show_vendor_id(struct device *d,
-				       struct device_attribute *attr, char *buf)
-{
-	struct i2o_device *dev = to_i2o_device(d);
-	u16 id;
-
-	if (!i2o_parm_field_get(dev, 0x0000, 0, &id, 2)) {
-		sprintf(buf, "0x%04x", le16_to_cpu(id));
-		return strlen(buf) + 1;
-	}
-
-	return 0;
-};
-
-/**
- *	i2o_exec_show_product_id - Displays Product ID of controller
- *	@d: device of which the Product ID should be displayed
- *	@attr: device_attribute to display
- *	@buf: buffer into which the Product ID should be printed
- *
- *	Returns number of bytes printed into buffer.
- */
-static ssize_t i2o_exec_show_product_id(struct device *d,
-					struct device_attribute *attr,
-					char *buf)
-{
-	struct i2o_device *dev = to_i2o_device(d);
-	u16 id;
-
-	if (!i2o_parm_field_get(dev, 0x0000, 1, &id, 2)) {
-		sprintf(buf, "0x%04x", le16_to_cpu(id));
-		return strlen(buf) + 1;
-	}
-
-	return 0;
-};
-
-/* Exec-OSM device attributes */
-static DEVICE_ATTR(vendor_id, S_IRUGO, i2o_exec_show_vendor_id, NULL);
-static DEVICE_ATTR(product_id, S_IRUGO, i2o_exec_show_product_id, NULL);
-
-/**
- *	i2o_exec_probe - Called if a new I2O device (executive class) appears
- *	@dev: I2O device which should be probed
- *
- *	Registers event notification for every event from Executive device. The
- *	return is always 0, because we want all devices of class Executive.
- *
- *	Returns 0 on success.
- */
-static int i2o_exec_probe(struct device *dev)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-	int rc;
-
-	rc = i2o_event_register(i2o_dev, &i2o_exec_driver, 0, 0xffffffff);
-	if (rc) goto err_out;
-
-	rc = device_create_file(dev, &dev_attr_vendor_id);
-	if (rc) goto err_evtreg;
-	rc = device_create_file(dev, &dev_attr_product_id);
-	if (rc) goto err_vid;
-
-	i2o_dev->iop->exec = i2o_dev;
-
-	return 0;
-
-err_vid:
-	device_remove_file(dev, &dev_attr_vendor_id);
-err_evtreg:
-	i2o_event_register(to_i2o_device(dev), &i2o_exec_driver, 0, 0);
-err_out:
-	return rc;
-};
-
-/**
- *	i2o_exec_remove - Called on I2O device removal
- *	@dev: I2O device which was removed
- *
- *	Unregisters event notification from Executive I2O device.
- *
- *	Returns 0 on success.
- */
-static int i2o_exec_remove(struct device *dev)
-{
-	device_remove_file(dev, &dev_attr_product_id);
-	device_remove_file(dev, &dev_attr_vendor_id);
-
-	i2o_event_register(to_i2o_device(dev), &i2o_exec_driver, 0, 0);
-
-	return 0;
-};
-
-#ifdef CONFIG_I2O_LCT_NOTIFY_ON_CHANGES
-/**
- *	i2o_exec_lct_notify - Send a asynchronus LCT NOTIFY request
- *	@c: I2O controller to which the request should be send
- *	@change_ind: change indicator
- *
- *	This function sends a LCT NOTIFY request to the I2O controller with
- *	the change indicator change_ind. If the change_ind == 0 the controller
- *	replies immediately after the request. If change_ind > 0 the reply is
- *	send after change indicator of the LCT is > change_ind.
- */
-static int i2o_exec_lct_notify(struct i2o_controller *c, u32 change_ind)
-{
-	i2o_status_block *sb = c->status_block.virt;
-	struct device *dev;
-	struct i2o_message *msg;
-
-	mutex_lock(&c->lct_lock);
-
-	dev = &c->pdev->dev;
-
-	if (i2o_dma_realloc(dev, &c->dlct,
-					le32_to_cpu(sb->expected_lct_size))) {
-		mutex_unlock(&c->lct_lock);
-		return -ENOMEM;
-	}
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg)) {
-		mutex_unlock(&c->lct_lock);
-		return PTR_ERR(msg);
-	}
-
-	msg->u.head[0] = cpu_to_le32(EIGHT_WORD_MSG_SIZE | SGL_OFFSET_6);
-	msg->u.head[1] = cpu_to_le32(I2O_CMD_LCT_NOTIFY << 24 | HOST_TID << 12 |
-				     ADAPTER_TID);
-	msg->u.s.icntxt = cpu_to_le32(i2o_exec_driver.context);
-	msg->u.s.tcntxt = cpu_to_le32(0x00000000);
-	msg->body[0] = cpu_to_le32(0xffffffff);
-	msg->body[1] = cpu_to_le32(change_ind);
-	msg->body[2] = cpu_to_le32(0xd0000000 | c->dlct.len);
-	msg->body[3] = cpu_to_le32(c->dlct.phys);
-
-	i2o_msg_post(c, msg);
-
-	mutex_unlock(&c->lct_lock);
-
-	return 0;
-}
-#endif
-
-/**
- *	i2o_exec_lct_modified - Called on LCT NOTIFY reply
- *	@_work: work struct for a specific controller
- *
- *	This function handles asynchronus LCT NOTIFY replies. It parses the
- *	new LCT and if the buffer for the LCT was to small sends a LCT NOTIFY
- *	again, otherwise send LCT NOTIFY to get informed on next LCT change.
- */
-static void i2o_exec_lct_modified(struct work_struct *_work)
-{
-	struct i2o_exec_lct_notify_work *work =
-		container_of(_work, struct i2o_exec_lct_notify_work, work);
-	u32 change_ind = 0;
-	struct i2o_controller *c = work->c;
-
-	kfree(work);
-
-	if (i2o_device_parse_lct(c) != -EAGAIN)
-		change_ind = c->lct->change_ind + 1;
-
-#ifdef CONFIG_I2O_LCT_NOTIFY_ON_CHANGES
-	i2o_exec_lct_notify(c, change_ind);
-#endif
-};
-
-/**
- *	i2o_exec_reply -  I2O Executive reply handler
- *	@c: I2O controller from which the reply comes
- *	@m: message id
- *	@msg: pointer to the I2O reply message
- *
- *	This function is always called from interrupt context. If a POST WAIT
- *	reply was received, pass it to the complete function. If a LCT NOTIFY
- *	reply was received, a new event is created to handle the update.
- *
- *	Returns 0 on success and if the reply should not be flushed or > 0
- *	on success and if the reply should be flushed. Returns negative error
- *	code on failure and if the reply should be flushed.
- */
-static int i2o_exec_reply(struct i2o_controller *c, u32 m,
-			  struct i2o_message *msg)
-{
-	u32 context;
-
-	if (le32_to_cpu(msg->u.head[0]) & MSG_FAIL) {
-		struct i2o_message __iomem *pmsg;
-		u32 pm;
-
-		/*
-		 * If Fail bit is set we must take the transaction context of
-		 * the preserved message to find the right request again.
-		 */
-
-		pm = le32_to_cpu(msg->body[3]);
-		pmsg = i2o_msg_in_to_virt(c, pm);
-		context = readl(&pmsg->u.s.tcntxt);
-
-		i2o_report_status(KERN_INFO, "i2o_core", msg);
-
-		/* Release the preserved msg */
-		i2o_msg_nop_mfa(c, pm);
-	} else
-		context = le32_to_cpu(msg->u.s.tcntxt);
-
-	if (context & 0x80000000)
-		return i2o_msg_post_wait_complete(c, m, msg, context);
-
-	if ((le32_to_cpu(msg->u.head[1]) >> 24) == I2O_CMD_LCT_NOTIFY) {
-		struct i2o_exec_lct_notify_work *work;
-
-		pr_debug("%s: LCT notify received\n", c->name);
-
-		work = kmalloc(sizeof(*work), GFP_ATOMIC);
-		if (!work)
-			return -ENOMEM;
-
-		work->c = c;
-
-		INIT_WORK(&work->work, i2o_exec_lct_modified);
-		queue_work(i2o_exec_driver.event_queue, &work->work);
-		return 1;
-	}
-
-	/*
-	 * If this happens, we want to dump the message to the syslog so
-	 * it can be sent back to the card manufacturer by the end user
-	 * to aid in debugging.
-	 *
-	 */
-	printk(KERN_WARNING "%s: Unsolicited message reply sent to core! Message dumped to syslog\n",
-			c->name);
-	i2o_dump_message(msg);
-
-	return -EFAULT;
-}
-
-/**
- *	i2o_exec_event - Event handling function
- *	@work: Work item in occurring event
- *
- *	Handles events send by the Executive device. At the moment does not do
- *	anything useful.
- */
-static void i2o_exec_event(struct work_struct *work)
-{
-	struct i2o_event *evt = container_of(work, struct i2o_event, work);
-
-	if (likely(evt->i2o_dev))
-		osm_debug("Event received from device: %d\n",
-			  evt->i2o_dev->lct_data.tid);
-	kfree(evt);
-};
-
-/**
- *	i2o_exec_lct_get - Get the IOP's Logical Configuration Table
- *	@c: I2O controller from which the LCT should be fetched
- *
- *	Send a LCT NOTIFY request to the controller, and wait
- *	I2O_TIMEOUT_LCT_GET seconds until arrival of response. If the LCT is
- *	to large, retry it.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int i2o_exec_lct_get(struct i2o_controller *c)
-{
-	struct i2o_message *msg;
-	int i = 0;
-	int rc = -EAGAIN;
-
-	for (i = 1; i <= I2O_LCT_GET_TRIES; i++) {
-		msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-		if (IS_ERR(msg))
-			return PTR_ERR(msg);
-
-		msg->u.head[0] =
-		    cpu_to_le32(EIGHT_WORD_MSG_SIZE | SGL_OFFSET_6);
-		msg->u.head[1] =
-		    cpu_to_le32(I2O_CMD_LCT_NOTIFY << 24 | HOST_TID << 12 |
-				ADAPTER_TID);
-		msg->body[0] = cpu_to_le32(0xffffffff);
-		msg->body[1] = cpu_to_le32(0x00000000);
-		msg->body[2] = cpu_to_le32(0xd0000000 | c->dlct.len);
-		msg->body[3] = cpu_to_le32(c->dlct.phys);
-
-		rc = i2o_msg_post_wait(c, msg, I2O_TIMEOUT_LCT_GET);
-		if (rc < 0)
-			break;
-
-		rc = i2o_device_parse_lct(c);
-		if (rc != -EAGAIN)
-			break;
-	}
-
-	return rc;
-}
-
-/* Exec OSM driver struct */
-struct i2o_driver i2o_exec_driver = {
-	.name = OSM_NAME,
-	.reply = i2o_exec_reply,
-	.event = i2o_exec_event,
-	.classes = i2o_exec_class_id,
-	.driver = {
-		   .probe = i2o_exec_probe,
-		   .remove = i2o_exec_remove,
-		   },
-};
-
-/**
- *	i2o_exec_init - Registers the Exec OSM
- *
- *	Registers the Exec OSM in the I2O core.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int __init i2o_exec_init(void)
-{
-	return i2o_driver_register(&i2o_exec_driver);
-};
-
-/**
- *	i2o_exec_exit - Removes the Exec OSM
- *
- *	Unregisters the Exec OSM from the I2O core.
- */
-void i2o_exec_exit(void)
-{
-	i2o_driver_unregister(&i2o_exec_driver);
-};
-
-EXPORT_SYMBOL(i2o_msg_post_wait_mem);
-EXPORT_SYMBOL(i2o_exec_lct_get);
diff --git a/drivers/staging/i2o/i2o.h b/drivers/staging/i2o/i2o.h
deleted file mode 100644
index d23c3c20b201..000000000000
--- a/drivers/staging/i2o/i2o.h
+++ /dev/null
@@ -1,988 +0,0 @@
-/*
- * I2O kernel space accessible structures/APIs
- *
- * (c) Copyright 1999, 2000 Red Hat 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 header file defined the I2O APIs/structures for use by
- * the I2O kernel modules.
- *
- */
-
-#ifndef _I2O_H
-#define _I2O_H
-
-#include <linux/i2o-dev.h>
-
-/* How many different OSM's are we allowing */
-#define I2O_MAX_DRIVERS		8
-
-#include <linux/pci.h>
-#include <linux/bug.h>
-#include <linux/dma-mapping.h>
-#include <linux/string.h>
-#include <linux/slab.h>
-#include <linux/workqueue.h>	/* work_struct */
-#include <linux/mempool.h>
-#include <linux/mutex.h>
-#include <linux/scatterlist.h>
-#include <linux/semaphore.h>	/* Needed for MUTEX init macros */
-
-#include <asm/io.h>
-
-/* message queue empty */
-#define I2O_QUEUE_EMPTY		0xffffffff
-
-/*
- *	Cache strategies
- */
-
-/*	The NULL strategy leaves everything up to the controller. This tends to be a
- *	pessimal but functional choice.
- */
-#define CACHE_NULL		0
-/*	Prefetch data when reading. We continually attempt to load the next 32 sectors
- *	into the controller cache.
- */
-#define CACHE_PREFETCH		1
-/*	Prefetch data when reading. We sometimes attempt to load the next 32 sectors
- *	into the controller cache. When an I/O is less <= 8K we assume its probably
- *	not sequential and don't prefetch (default)
- */
-#define CACHE_SMARTFETCH	2
-/*	Data is written to the cache and then out on to the disk. The I/O must be
- *	physically on the medium before the write is acknowledged (default without
- *	NVRAM)
- */
-#define CACHE_WRITETHROUGH	17
-/*	Data is written to the cache and then out on to the disk. The controller
- *	is permitted to write back the cache any way it wants. (default if battery
- *	backed NVRAM is present). It can be useful to set this for swap regardless of
- *	battery state.
- */
-#define CACHE_WRITEBACK		18
-/*	Optimise for under powered controllers, especially on RAID1 and RAID0. We
- *	write large I/O's directly to disk bypassing the cache to avoid the extra
- *	memory copy hits. Small writes are writeback cached
- */
-#define CACHE_SMARTBACK		19
-/*	Optimise for under powered controllers, especially on RAID1 and RAID0. We
- *	write large I/O's directly to disk bypassing the cache to avoid the extra
- *	memory copy hits. Small writes are writethrough cached. Suitable for devices
- *	lacking battery backup
- */
-#define CACHE_SMARTTHROUGH	20
-
-/*
- *	Ioctl structures
- */
-
-#define 	BLKI2OGRSTRAT	_IOR('2', 1, int)
-#define 	BLKI2OGWSTRAT	_IOR('2', 2, int)
-#define 	BLKI2OSRSTRAT	_IOW('2', 3, int)
-#define 	BLKI2OSWSTRAT	_IOW('2', 4, int)
-
-/*
- *	I2O Function codes
- */
-
-/*
- *	Executive Class
- */
-#define	I2O_CMD_ADAPTER_ASSIGN		0xB3
-#define	I2O_CMD_ADAPTER_READ		0xB2
-#define	I2O_CMD_ADAPTER_RELEASE		0xB5
-#define	I2O_CMD_BIOS_INFO_SET		0xA5
-#define	I2O_CMD_BOOT_DEVICE_SET		0xA7
-#define	I2O_CMD_CONFIG_VALIDATE		0xBB
-#define	I2O_CMD_CONN_SETUP		0xCA
-#define	I2O_CMD_DDM_DESTROY		0xB1
-#define	I2O_CMD_DDM_ENABLE		0xD5
-#define	I2O_CMD_DDM_QUIESCE		0xC7
-#define	I2O_CMD_DDM_RESET		0xD9
-#define	I2O_CMD_DDM_SUSPEND		0xAF
-#define	I2O_CMD_DEVICE_ASSIGN		0xB7
-#define	I2O_CMD_DEVICE_RELEASE		0xB9
-#define	I2O_CMD_HRT_GET			0xA8
-#define	I2O_CMD_ADAPTER_CLEAR		0xBE
-#define	I2O_CMD_ADAPTER_CONNECT		0xC9
-#define	I2O_CMD_ADAPTER_RESET		0xBD
-#define	I2O_CMD_LCT_NOTIFY		0xA2
-#define	I2O_CMD_OUTBOUND_INIT		0xA1
-#define	I2O_CMD_PATH_ENABLE		0xD3
-#define	I2O_CMD_PATH_QUIESCE		0xC5
-#define	I2O_CMD_PATH_RESET		0xD7
-#define	I2O_CMD_STATIC_MF_CREATE	0xDD
-#define	I2O_CMD_STATIC_MF_RELEASE	0xDF
-#define	I2O_CMD_STATUS_GET		0xA0
-#define	I2O_CMD_SW_DOWNLOAD		0xA9
-#define	I2O_CMD_SW_UPLOAD		0xAB
-#define	I2O_CMD_SW_REMOVE		0xAD
-#define	I2O_CMD_SYS_ENABLE		0xD1
-#define	I2O_CMD_SYS_MODIFY		0xC1
-#define	I2O_CMD_SYS_QUIESCE		0xC3
-#define	I2O_CMD_SYS_TAB_SET		0xA3
-
-/*
- * Utility Class
- */
-#define I2O_CMD_UTIL_NOP		0x00
-#define I2O_CMD_UTIL_ABORT		0x01
-#define I2O_CMD_UTIL_CLAIM		0x09
-#define I2O_CMD_UTIL_RELEASE		0x0B
-#define I2O_CMD_UTIL_PARAMS_GET		0x06
-#define I2O_CMD_UTIL_PARAMS_SET		0x05
-#define I2O_CMD_UTIL_EVT_REGISTER	0x13
-#define I2O_CMD_UTIL_EVT_ACK		0x14
-#define I2O_CMD_UTIL_CONFIG_DIALOG	0x10
-#define I2O_CMD_UTIL_DEVICE_RESERVE	0x0D
-#define I2O_CMD_UTIL_DEVICE_RELEASE	0x0F
-#define I2O_CMD_UTIL_LOCK		0x17
-#define I2O_CMD_UTIL_LOCK_RELEASE	0x19
-#define I2O_CMD_UTIL_REPLY_FAULT_NOTIFY	0x15
-
-/*
- * SCSI Host Bus Adapter Class
- */
-#define I2O_CMD_SCSI_EXEC		0x81
-#define I2O_CMD_SCSI_ABORT		0x83
-#define I2O_CMD_SCSI_BUSRESET		0x27
-
-/*
- * Bus Adapter Class
- */
-#define I2O_CMD_BUS_ADAPTER_RESET	0x85
-#define I2O_CMD_BUS_RESET		0x87
-#define I2O_CMD_BUS_SCAN		0x89
-#define I2O_CMD_BUS_QUIESCE		0x8b
-
-/*
- * Random Block Storage Class
- */
-#define I2O_CMD_BLOCK_READ		0x30
-#define I2O_CMD_BLOCK_WRITE		0x31
-#define I2O_CMD_BLOCK_CFLUSH		0x37
-#define I2O_CMD_BLOCK_MLOCK		0x49
-#define I2O_CMD_BLOCK_MUNLOCK		0x4B
-#define I2O_CMD_BLOCK_MMOUNT		0x41
-#define I2O_CMD_BLOCK_MEJECT		0x43
-#define I2O_CMD_BLOCK_POWER		0x70
-
-#define I2O_CMD_PRIVATE			0xFF
-
-/* Command status values  */
-
-#define I2O_CMD_IN_PROGRESS	0x01
-#define I2O_CMD_REJECTED	0x02
-#define I2O_CMD_FAILED		0x03
-#define I2O_CMD_COMPLETED	0x04
-
-/* I2O API function return values */
-
-#define I2O_RTN_NO_ERROR			0
-#define I2O_RTN_NOT_INIT			1
-#define I2O_RTN_FREE_Q_EMPTY			2
-#define I2O_RTN_TCB_ERROR			3
-#define I2O_RTN_TRANSACTION_ERROR		4
-#define I2O_RTN_ADAPTER_ALREADY_INIT		5
-#define I2O_RTN_MALLOC_ERROR			6
-#define I2O_RTN_ADPTR_NOT_REGISTERED		7
-#define I2O_RTN_MSG_REPLY_TIMEOUT		8
-#define I2O_RTN_NO_STATUS			9
-#define I2O_RTN_NO_FIRM_VER			10
-#define	I2O_RTN_NO_LINK_SPEED			11
-
-/* Reply message status defines for all messages */
-
-#define I2O_REPLY_STATUS_SUCCESS                    	0x00
-#define I2O_REPLY_STATUS_ABORT_DIRTY                	0x01
-#define I2O_REPLY_STATUS_ABORT_NO_DATA_TRANSFER     	0x02
-#define	I2O_REPLY_STATUS_ABORT_PARTIAL_TRANSFER		0x03
-#define	I2O_REPLY_STATUS_ERROR_DIRTY			0x04
-#define	I2O_REPLY_STATUS_ERROR_NO_DATA_TRANSFER		0x05
-#define	I2O_REPLY_STATUS_ERROR_PARTIAL_TRANSFER		0x06
-#define	I2O_REPLY_STATUS_PROCESS_ABORT_DIRTY		0x08
-#define	I2O_REPLY_STATUS_PROCESS_ABORT_NO_DATA_TRANSFER	0x09
-#define	I2O_REPLY_STATUS_PROCESS_ABORT_PARTIAL_TRANSFER	0x0A
-#define	I2O_REPLY_STATUS_TRANSACTION_ERROR		0x0B
-#define	I2O_REPLY_STATUS_PROGRESS_REPORT		0x80
-
-/* Status codes and Error Information for Parameter functions */
-
-#define I2O_PARAMS_STATUS_SUCCESS		0x00
-#define I2O_PARAMS_STATUS_BAD_KEY_ABORT		0x01
-#define I2O_PARAMS_STATUS_BAD_KEY_CONTINUE   	0x02
-#define I2O_PARAMS_STATUS_BUFFER_FULL		0x03
-#define I2O_PARAMS_STATUS_BUFFER_TOO_SMALL	0x04
-#define I2O_PARAMS_STATUS_FIELD_UNREADABLE	0x05
-#define I2O_PARAMS_STATUS_FIELD_UNWRITEABLE	0x06
-#define I2O_PARAMS_STATUS_INSUFFICIENT_FIELDS	0x07
-#define I2O_PARAMS_STATUS_INVALID_GROUP_ID	0x08
-#define I2O_PARAMS_STATUS_INVALID_OPERATION	0x09
-#define I2O_PARAMS_STATUS_NO_KEY_FIELD		0x0A
-#define I2O_PARAMS_STATUS_NO_SUCH_FIELD		0x0B
-#define I2O_PARAMS_STATUS_NON_DYNAMIC_GROUP	0x0C
-#define I2O_PARAMS_STATUS_OPERATION_ERROR	0x0D
-#define I2O_PARAMS_STATUS_SCALAR_ERROR		0x0E
-#define I2O_PARAMS_STATUS_TABLE_ERROR		0x0F
-#define I2O_PARAMS_STATUS_WRONG_GROUP_TYPE	0x10
-
-/* DetailedStatusCode defines for Executive, DDM, Util and Transaction error
- * messages: Table 3-2 Detailed Status Codes.*/
-
-#define I2O_DSC_SUCCESS                        0x0000
-#define I2O_DSC_BAD_KEY                        0x0002
-#define I2O_DSC_TCL_ERROR                      0x0003
-#define I2O_DSC_REPLY_BUFFER_FULL              0x0004
-#define I2O_DSC_NO_SUCH_PAGE                   0x0005
-#define I2O_DSC_INSUFFICIENT_RESOURCE_SOFT     0x0006
-#define I2O_DSC_INSUFFICIENT_RESOURCE_HARD     0x0007
-#define I2O_DSC_CHAIN_BUFFER_TOO_LARGE         0x0009
-#define I2O_DSC_UNSUPPORTED_FUNCTION           0x000A
-#define I2O_DSC_DEVICE_LOCKED                  0x000B
-#define I2O_DSC_DEVICE_RESET                   0x000C
-#define I2O_DSC_INAPPROPRIATE_FUNCTION         0x000D
-#define I2O_DSC_INVALID_INITIATOR_ADDRESS      0x000E
-#define I2O_DSC_INVALID_MESSAGE_FLAGS          0x000F
-#define I2O_DSC_INVALID_OFFSET                 0x0010
-#define I2O_DSC_INVALID_PARAMETER              0x0011
-#define I2O_DSC_INVALID_REQUEST                0x0012
-#define I2O_DSC_INVALID_TARGET_ADDRESS         0x0013
-#define I2O_DSC_MESSAGE_TOO_LARGE              0x0014
-#define I2O_DSC_MESSAGE_TOO_SMALL              0x0015
-#define I2O_DSC_MISSING_PARAMETER              0x0016
-#define I2O_DSC_TIMEOUT                        0x0017
-#define I2O_DSC_UNKNOWN_ERROR                  0x0018
-#define I2O_DSC_UNKNOWN_FUNCTION               0x0019
-#define I2O_DSC_UNSUPPORTED_VERSION            0x001A
-#define I2O_DSC_DEVICE_BUSY                    0x001B
-#define I2O_DSC_DEVICE_NOT_AVAILABLE           0x001C
-
-/* DetailedStatusCode defines for Block Storage Operation: Table 6-7 Detailed
-   Status Codes.*/
-
-#define I2O_BSA_DSC_SUCCESS               0x0000
-#define I2O_BSA_DSC_MEDIA_ERROR           0x0001
-#define I2O_BSA_DSC_ACCESS_ERROR          0x0002
-#define I2O_BSA_DSC_DEVICE_FAILURE        0x0003
-#define I2O_BSA_DSC_DEVICE_NOT_READY      0x0004
-#define I2O_BSA_DSC_MEDIA_NOT_PRESENT     0x0005
-#define I2O_BSA_DSC_MEDIA_LOCKED          0x0006
-#define I2O_BSA_DSC_MEDIA_FAILURE         0x0007
-#define I2O_BSA_DSC_PROTOCOL_FAILURE      0x0008
-#define I2O_BSA_DSC_BUS_FAILURE           0x0009
-#define I2O_BSA_DSC_ACCESS_VIOLATION      0x000A
-#define I2O_BSA_DSC_WRITE_PROTECTED       0x000B
-#define I2O_BSA_DSC_DEVICE_RESET          0x000C
-#define I2O_BSA_DSC_VOLUME_CHANGED        0x000D
-#define I2O_BSA_DSC_TIMEOUT               0x000E
-
-/* FailureStatusCodes, Table 3-3 Message Failure Codes */
-
-#define I2O_FSC_TRANSPORT_SERVICE_SUSPENDED             0x81
-#define I2O_FSC_TRANSPORT_SERVICE_TERMINATED            0x82
-#define I2O_FSC_TRANSPORT_CONGESTION                    0x83
-#define I2O_FSC_TRANSPORT_FAILURE                       0x84
-#define I2O_FSC_TRANSPORT_STATE_ERROR                   0x85
-#define I2O_FSC_TRANSPORT_TIME_OUT                      0x86
-#define I2O_FSC_TRANSPORT_ROUTING_FAILURE               0x87
-#define I2O_FSC_TRANSPORT_INVALID_VERSION               0x88
-#define I2O_FSC_TRANSPORT_INVALID_OFFSET                0x89
-#define I2O_FSC_TRANSPORT_INVALID_MSG_FLAGS             0x8A
-#define I2O_FSC_TRANSPORT_FRAME_TOO_SMALL               0x8B
-#define I2O_FSC_TRANSPORT_FRAME_TOO_LARGE               0x8C
-#define I2O_FSC_TRANSPORT_INVALID_TARGET_ID             0x8D
-#define I2O_FSC_TRANSPORT_INVALID_INITIATOR_ID          0x8E
-#define I2O_FSC_TRANSPORT_INVALID_INITIATOR_CONTEXT     0x8F
-#define I2O_FSC_TRANSPORT_UNKNOWN_FAILURE               0xFF
-
-/* Device Claim Types */
-#define	I2O_CLAIM_PRIMARY					0x01000000
-#define	I2O_CLAIM_MANAGEMENT					0x02000000
-#define	I2O_CLAIM_AUTHORIZED					0x03000000
-#define	I2O_CLAIM_SECONDARY					0x04000000
-
-/* Message header defines for VersionOffset */
-#define I2OVER15	0x0001
-#define I2OVER20	0x0002
-
-/* Default is 1.5 */
-#define I2OVERSION	I2OVER15
-
-#define SGL_OFFSET_0    I2OVERSION
-#define SGL_OFFSET_4    (0x0040 | I2OVERSION)
-#define SGL_OFFSET_5    (0x0050 | I2OVERSION)
-#define SGL_OFFSET_6    (0x0060 | I2OVERSION)
-#define SGL_OFFSET_7    (0x0070 | I2OVERSION)
-#define SGL_OFFSET_8    (0x0080 | I2OVERSION)
-#define SGL_OFFSET_9    (0x0090 | I2OVERSION)
-#define SGL_OFFSET_10   (0x00A0 | I2OVERSION)
-#define SGL_OFFSET_11   (0x00B0 | I2OVERSION)
-#define SGL_OFFSET_12   (0x00C0 | I2OVERSION)
-#define SGL_OFFSET(x)   (((x)<<4) | I2OVERSION)
-
-/* Transaction Reply Lists (TRL) Control Word structure */
-#define TRL_SINGLE_FIXED_LENGTH		0x00
-#define TRL_SINGLE_VARIABLE_LENGTH	0x40
-#define TRL_MULTIPLE_FIXED_LENGTH	0x80
-
- /* msg header defines for MsgFlags */
-#define MSG_STATIC	0x0100
-#define MSG_64BIT_CNTXT	0x0200
-#define MSG_MULTI_TRANS	0x1000
-#define MSG_FAIL	0x2000
-#define MSG_FINAL	0x4000
-#define MSG_REPLY	0x8000
-
- /* minimum size msg */
-#define THREE_WORD_MSG_SIZE	0x00030000
-#define FOUR_WORD_MSG_SIZE	0x00040000
-#define FIVE_WORD_MSG_SIZE	0x00050000
-#define SIX_WORD_MSG_SIZE	0x00060000
-#define SEVEN_WORD_MSG_SIZE	0x00070000
-#define EIGHT_WORD_MSG_SIZE	0x00080000
-#define NINE_WORD_MSG_SIZE	0x00090000
-#define TEN_WORD_MSG_SIZE	0x000A0000
-#define ELEVEN_WORD_MSG_SIZE	0x000B0000
-#define I2O_MESSAGE_SIZE(x)	((x)<<16)
-
-/* special TID assignments */
-#define ADAPTER_TID		0
-#define HOST_TID		1
-
-/* outbound queue defines */
-#define I2O_MAX_OUTBOUND_MSG_FRAMES	128
-#define I2O_OUTBOUND_MSG_FRAME_SIZE	128	/* in 32-bit words */
-
-/* inbound queue definitions */
-#define I2O_MSG_INPOOL_MIN		32
-#define I2O_INBOUND_MSG_FRAME_SIZE	128	/* in 32-bit words */
-
-#define I2O_POST_WAIT_OK	0
-#define I2O_POST_WAIT_TIMEOUT	-ETIMEDOUT
-
-#define I2O_CONTEXT_LIST_MIN_LENGTH	15
-#define I2O_CONTEXT_LIST_USED		0x01
-#define I2O_CONTEXT_LIST_DELETED	0x02
-
-/* timeouts */
-#define I2O_TIMEOUT_INIT_OUTBOUND_QUEUE	15
-#define I2O_TIMEOUT_MESSAGE_GET		5
-#define I2O_TIMEOUT_RESET		30
-#define I2O_TIMEOUT_STATUS_GET		5
-#define I2O_TIMEOUT_LCT_GET		360
-#define I2O_TIMEOUT_SCSI_SCB_ABORT	240
-
-/* retries */
-#define I2O_HRT_GET_TRIES		3
-#define I2O_LCT_GET_TRIES		3
-
-/* defines for max_sectors and max_phys_segments */
-#define I2O_MAX_SECTORS			1024
-#define I2O_MAX_SECTORS_LIMITED		128
-#define I2O_MAX_PHYS_SEGMENTS		BLK_MAX_SEGMENTS
-
-/*
- *	Message structures
- */
-struct i2o_message {
-	union {
-		struct {
-			u8 version_offset;
-			u8 flags;
-			u16 size;
-			u32 target_tid:12;
-			u32 init_tid:12;
-			u32 function:8;
-			u32 icntxt;	/* initiator context */
-			u32 tcntxt;	/* transaction context */
-		} s;
-		u32 head[4];
-	} u;
-	/* List follows */
-	u32 body[0];
-};
-
-/* MFA and I2O message used by mempool */
-struct i2o_msg_mfa {
-	u32 mfa;		/* MFA returned by the controller */
-	struct i2o_message msg;	/* I2O message */
-};
-
-/*
- *	Each I2O device entity has one of these. There is one per device.
- */
-struct i2o_device {
-	i2o_lct_entry lct_data;	/* Device LCT information */
-
-	struct i2o_controller *iop;	/* Controlling IOP */
-	struct list_head list;	/* node in IOP devices list */
-
-	struct device device;
-
-	struct mutex lock;	/* device lock */
-};
-
-/*
- *	Event structure provided to the event handling function
- */
-struct i2o_event {
-	struct work_struct work;
-	struct i2o_device *i2o_dev;	/* I2O device pointer from which the
-					   event reply was initiated */
-	u16 size;		/* Size of data in 32-bit words */
-	u32 tcntxt;		/* Transaction context used at
-				   registration */
-	u32 event_indicator;	/* Event indicator from reply */
-	u32 data[0];		/* Event data from reply */
-};
-
-/*
- *	I2O classes which could be handled by the OSM
- */
-struct i2o_class_id {
-	u16 class_id:12;
-};
-
-/*
- *	I2O driver structure for OSMs
- */
-struct i2o_driver {
-	char *name;		/* OSM name */
-	int context;		/* Low 8 bits of the transaction info */
-	struct i2o_class_id *classes;	/* I2O classes that this OSM handles */
-
-	/* Message reply handler */
-	int (*reply) (struct i2o_controller *, u32, struct i2o_message *);
-
-	/* Event handler */
-	work_func_t event;
-
-	struct workqueue_struct *event_queue;	/* Event queue */
-
-	struct device_driver driver;
-
-	/* notification of changes */
-	void (*notify_controller_add) (struct i2o_controller *);
-	void (*notify_controller_remove) (struct i2o_controller *);
-	void (*notify_device_add) (struct i2o_device *);
-	void (*notify_device_remove) (struct i2o_device *);
-
-	struct semaphore lock;
-};
-
-/*
- *	Contains DMA mapped address information
- */
-struct i2o_dma {
-	void *virt;
-	dma_addr_t phys;
-	size_t len;
-};
-
-/*
- *	Contains slab cache and mempool information
- */
-struct i2o_pool {
-	char *name;
-	struct kmem_cache *slab;
-	mempool_t *mempool;
-};
-
-/*
- *	Contains IO mapped address information
- */
-struct i2o_io {
-	void __iomem *virt;
-	unsigned long phys;
-	unsigned long len;
-};
-
-/*
- *	Context queue entry, used for 32-bit context on 64-bit systems
- */
-struct i2o_context_list_element {
-	struct list_head list;
-	u32 context;
-	void *ptr;
-	unsigned long timestamp;
-};
-
-/*
- * Each I2O controller has one of these objects
- */
-struct i2o_controller {
-	char name[16];
-	int unit;
-	int type;
-
-	struct pci_dev *pdev;	/* PCI device */
-
-	unsigned int promise:1;	/* Promise controller */
-	unsigned int adaptec:1;	/* DPT / Adaptec controller */
-	unsigned int raptor:1;	/* split bar */
-	unsigned int no_quiesce:1;	/* dont quiesce before reset */
-	unsigned int short_req:1;	/* use small block sizes */
-	unsigned int limit_sectors:1;	/* limit number of sectors / request */
-	unsigned int pae_support:1;	/* controller has 64-bit SGL support */
-
-	struct list_head devices;	/* list of I2O devices */
-	struct list_head list;	/* Controller list */
-
-	void __iomem *in_port;	/* Inbout port address */
-	void __iomem *out_port;	/* Outbound port address */
-	void __iomem *irq_status;	/* Interrupt status register address */
-	void __iomem *irq_mask;	/* Interrupt mask register address */
-
-	struct i2o_dma status;	/* IOP status block */
-
-	struct i2o_dma hrt;	/* HW Resource Table */
-	i2o_lct *lct;		/* Logical Config Table */
-	struct i2o_dma dlct;	/* Temp LCT */
-	struct mutex lct_lock;	/* Lock for LCT updates */
-	struct i2o_dma status_block;	/* IOP status block */
-
-	struct i2o_io base;	/* controller messaging unit */
-	struct i2o_io in_queue;	/* inbound message queue Host->IOP */
-	struct i2o_dma out_queue;	/* outbound message queue IOP->Host */
-
-	struct i2o_pool in_msg;	/* mempool for inbound messages */
-
-	unsigned int battery:1;	/* Has a battery backup */
-	unsigned int io_alloc:1;	/* An I/O resource was allocated */
-	unsigned int mem_alloc:1;	/* A memory resource was allocated */
-
-	struct resource io_resource;	/* I/O resource allocated to the IOP */
-	struct resource mem_resource;	/* Mem resource allocated to the IOP */
-
-	struct device device;
-	struct i2o_device *exec;	/* Executive */
-#if BITS_PER_LONG == 64
-	spinlock_t context_list_lock;	/* lock for context_list */
-	atomic_t context_list_counter;	/* needed for unique contexts */
-	struct list_head context_list;	/* list of context id's
-					   and pointers */
-#endif
-	spinlock_t lock;	/* lock for controller
-				   configuration */
-	void *driver_data[I2O_MAX_DRIVERS];	/* storage for drivers */
-};
-
-/*
- * I2O System table entry
- *
- * The system table contains information about all the IOPs in the
- * system.  It is sent to all IOPs so that they can create peer2peer
- * connections between them.
- */
-struct i2o_sys_tbl_entry {
-	u16 org_id;
-	u16 reserved1;
-	u32 iop_id:12;
-	u32 reserved2:20;
-	u16 seg_num:12;
-	u16 i2o_version:4;
-	u8 iop_state;
-	u8 msg_type;
-	u16 frame_size;
-	u16 reserved3;
-	u32 last_changed;
-	u32 iop_capabilities;
-	u32 inbound_low;
-	u32 inbound_high;
-};
-
-struct i2o_sys_tbl {
-	u8 num_entries;
-	u8 version;
-	u16 reserved1;
-	u32 change_ind;
-	u32 reserved2;
-	u32 reserved3;
-	struct i2o_sys_tbl_entry iops[0];
-};
-
-extern struct list_head i2o_controllers;
-
-/* Message functions */
-extern struct i2o_message *i2o_msg_get_wait(struct i2o_controller *, int);
-extern int i2o_msg_post_wait_mem(struct i2o_controller *, struct i2o_message *,
-				 unsigned long, struct i2o_dma *);
-
-/* IOP functions */
-extern int i2o_status_get(struct i2o_controller *);
-
-extern int i2o_event_register(struct i2o_device *, struct i2o_driver *, int,
-			      u32);
-extern struct i2o_device *i2o_iop_find_device(struct i2o_controller *, u16);
-extern struct i2o_controller *i2o_find_iop(int);
-
-/* Functions needed for handling 64-bit pointers in 32-bit context */
-#if BITS_PER_LONG == 64
-extern u32 i2o_cntxt_list_add(struct i2o_controller *, void *);
-extern void *i2o_cntxt_list_get(struct i2o_controller *, u32);
-extern u32 i2o_cntxt_list_remove(struct i2o_controller *, void *);
-extern u32 i2o_cntxt_list_get_ptr(struct i2o_controller *, void *);
-
-static inline u32 i2o_ptr_low(void *ptr)
-{
-	return (u32) (u64) ptr;
-};
-
-static inline u32 i2o_ptr_high(void *ptr)
-{
-	return (u32) ((u64) ptr >> 32);
-};
-
-static inline u32 i2o_dma_low(dma_addr_t dma_addr)
-{
-	return (u32) (u64) dma_addr;
-};
-
-static inline u32 i2o_dma_high(dma_addr_t dma_addr)
-{
-	return (u32) ((u64) dma_addr >> 32);
-};
-#else
-static inline u32 i2o_cntxt_list_add(struct i2o_controller *c, void *ptr)
-{
-	return (u32) ptr;
-};
-
-static inline void *i2o_cntxt_list_get(struct i2o_controller *c, u32 context)
-{
-	return (void *)context;
-};
-
-static inline u32 i2o_cntxt_list_remove(struct i2o_controller *c, void *ptr)
-{
-	return (u32) ptr;
-};
-
-static inline u32 i2o_cntxt_list_get_ptr(struct i2o_controller *c, void *ptr)
-{
-	return (u32) ptr;
-};
-
-static inline u32 i2o_ptr_low(void *ptr)
-{
-	return (u32) ptr;
-};
-
-static inline u32 i2o_ptr_high(void *ptr)
-{
-	return 0;
-};
-
-static inline u32 i2o_dma_low(dma_addr_t dma_addr)
-{
-	return (u32) dma_addr;
-};
-
-static inline u32 i2o_dma_high(dma_addr_t dma_addr)
-{
-	return 0;
-};
-#endif
-
-extern u16 i2o_sg_tablesize(struct i2o_controller *c, u16 body_size);
-extern dma_addr_t i2o_dma_map_single(struct i2o_controller *c, void *ptr,
-					    size_t size,
-					    enum dma_data_direction direction,
-					    u32 ** sg_ptr);
-extern int i2o_dma_map_sg(struct i2o_controller *c,
-				 struct scatterlist *sg, int sg_count,
-				 enum dma_data_direction direction,
-				 u32 ** sg_ptr);
-extern int i2o_dma_alloc(struct device *dev, struct i2o_dma *addr, size_t len);
-extern void i2o_dma_free(struct device *dev, struct i2o_dma *addr);
-extern int i2o_dma_realloc(struct device *dev, struct i2o_dma *addr,
-								size_t len);
-extern int i2o_pool_alloc(struct i2o_pool *pool, const char *name,
-				 size_t size, int min_nr);
-extern void i2o_pool_free(struct i2o_pool *pool);
-/* I2O driver (OSM) functions */
-extern int i2o_driver_register(struct i2o_driver *);
-extern void i2o_driver_unregister(struct i2o_driver *);
-
-/**
- *	i2o_driver_notify_controller_add - Send notification of added controller
- *	@drv: I2O driver
- *	@c: I2O controller
- *
- *	Send notification of added controller to a single registered driver.
- */
-static inline void i2o_driver_notify_controller_add(struct i2o_driver *drv,
-						    struct i2o_controller *c)
-{
-	if (drv->notify_controller_add)
-		drv->notify_controller_add(c);
-};
-
-/**
- *	i2o_driver_notify_controller_remove - Send notification of removed controller
- *	@drv: I2O driver
- *	@c: I2O controller
- *
- *	Send notification of removed controller to a single registered driver.
- */
-static inline void i2o_driver_notify_controller_remove(struct i2o_driver *drv,
-						       struct i2o_controller *c)
-{
-	if (drv->notify_controller_remove)
-		drv->notify_controller_remove(c);
-};
-
-/**
- *	i2o_driver_notify_device_add - Send notification of added device
- *	@drv: I2O driver
- *	@i2o_dev: the added i2o_device
- *
- *	Send notification of added device to a single registered driver.
- */
-static inline void i2o_driver_notify_device_add(struct i2o_driver *drv,
-						struct i2o_device *i2o_dev)
-{
-	if (drv->notify_device_add)
-		drv->notify_device_add(i2o_dev);
-};
-
-/**
- *	i2o_driver_notify_device_remove - Send notification of removed device
- *	@drv: I2O driver
- *	@i2o_dev: the added i2o_device
- *
- *	Send notification of removed device to a single registered driver.
- */
-static inline void i2o_driver_notify_device_remove(struct i2o_driver *drv,
-						   struct i2o_device *i2o_dev)
-{
-	if (drv->notify_device_remove)
-		drv->notify_device_remove(i2o_dev);
-};
-
-extern void i2o_driver_notify_controller_add_all(struct i2o_controller *);
-extern void i2o_driver_notify_controller_remove_all(struct i2o_controller *);
-extern void i2o_driver_notify_device_add_all(struct i2o_device *);
-extern void i2o_driver_notify_device_remove_all(struct i2o_device *);
-
-/* I2O device functions */
-extern int i2o_device_claim(struct i2o_device *);
-extern int i2o_device_claim_release(struct i2o_device *);
-
-/* Exec OSM functions */
-extern int i2o_exec_lct_get(struct i2o_controller *);
-
-/* device / driver / kobject conversion functions */
-#define to_i2o_driver(drv) container_of(drv,struct i2o_driver, driver)
-#define to_i2o_device(dev) container_of(dev, struct i2o_device, device)
-#define to_i2o_controller(dev) container_of(dev, struct i2o_controller, device)
-
-/**
- *	i2o_out_to_virt - Turn an I2O message to a virtual address
- *	@c: controller
- *	@m: message engine value
- *
- *	Turn a receive message from an I2O controller bus address into
- *	a Linux virtual address. The shared page frame is a linear block
- *	so we simply have to shift the offset. This function does not
- *	work for sender side messages as they are ioremap objects
- *	provided by the I2O controller.
- */
-static inline struct i2o_message *i2o_msg_out_to_virt(struct i2o_controller *c,
-						      u32 m)
-{
-	BUG_ON(m < c->out_queue.phys
-	       || m >= c->out_queue.phys + c->out_queue.len);
-
-	return c->out_queue.virt + (m - c->out_queue.phys);
-};
-
-/**
- *	i2o_msg_in_to_virt - Turn an I2O message to a virtual address
- *	@c: controller
- *	@m: message engine value
- *
- *	Turn a send message from an I2O controller bus address into
- *	a Linux virtual address. The shared page frame is a linear block
- *	so we simply have to shift the offset. This function does not
- *	work for receive side messages as they are kmalloc objects
- *	in a different pool.
- */
-static inline struct i2o_message __iomem *i2o_msg_in_to_virt(struct
-							     i2o_controller *c,
-							     u32 m)
-{
-	return c->in_queue.virt + m;
-};
-
-/**
- *	i2o_msg_get - obtain an I2O message from the IOP
- *	@c: I2O controller
- *
- *	This function tries to get a message frame. If no message frame is
- *	available do not wait until one is available (see also i2o_msg_get_wait).
- *	The returned pointer to the message frame is not in I/O memory, it is
- *	allocated from a mempool. But because a MFA is allocated from the
- *	controller too it is guaranteed that i2o_msg_post() will never fail.
- *
- *	On a success a pointer to the message frame is returned. If the message
- *	queue is empty -EBUSY is returned and if no memory is available -ENOMEM
- *	is returned.
- */
-static inline struct i2o_message *i2o_msg_get(struct i2o_controller *c)
-{
-	struct i2o_msg_mfa *mmsg = mempool_alloc(c->in_msg.mempool, GFP_ATOMIC);
-	if (!mmsg)
-		return ERR_PTR(-ENOMEM);
-
-	mmsg->mfa = readl(c->in_port);
-	if (unlikely(mmsg->mfa >= c->in_queue.len)) {
-		u32 mfa = mmsg->mfa;
-
-		mempool_free(mmsg, c->in_msg.mempool);
-
-		if (mfa == I2O_QUEUE_EMPTY)
-			return ERR_PTR(-EBUSY);
-		return ERR_PTR(-EFAULT);
-	}
-
-	return &mmsg->msg;
-};
-
-/**
- *	i2o_msg_post - Post I2O message to I2O controller
- *	@c: I2O controller to which the message should be send
- *	@msg: message returned by i2o_msg_get()
- *
- *	Post the message to the I2O controller and return immediately.
- */
-static inline void i2o_msg_post(struct i2o_controller *c,
-				struct i2o_message *msg)
-{
-	struct i2o_msg_mfa *mmsg;
-
-	mmsg = container_of(msg, struct i2o_msg_mfa, msg);
-	memcpy_toio(i2o_msg_in_to_virt(c, mmsg->mfa), msg,
-		    (le32_to_cpu(msg->u.head[0]) >> 16) << 2);
-	writel(mmsg->mfa, c->in_port);
-	mempool_free(mmsg, c->in_msg.mempool);
-};
-
-/**
- * 	i2o_msg_post_wait - Post and wait a message and wait until return
- *	@c: controller
- *	@msg: message to post
- *	@timeout: time in seconds to wait
- *
- * 	This API allows an OSM to post a message and then be told whether or
- *	not the system received a successful reply. If the message times out
- *	then the value '-ETIMEDOUT' is returned.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static inline int i2o_msg_post_wait(struct i2o_controller *c,
-				    struct i2o_message *msg,
-				    unsigned long timeout)
-{
-	return i2o_msg_post_wait_mem(c, msg, timeout, NULL);
-};
-
-/**
- *	i2o_msg_nop_mfa - Returns a fetched MFA back to the controller
- *	@c: I2O controller from which the MFA was fetched
- *	@mfa: MFA which should be returned
- *
- *	This function must be used for preserved messages, because i2o_msg_nop()
- *	also returns the allocated memory back to the msg_pool mempool.
- */
-static inline void i2o_msg_nop_mfa(struct i2o_controller *c, u32 mfa)
-{
-	struct i2o_message __iomem *msg;
-	u32 nop[3] = {
-		THREE_WORD_MSG_SIZE | SGL_OFFSET_0,
-		I2O_CMD_UTIL_NOP << 24 | HOST_TID << 12 | ADAPTER_TID,
-		0x00000000
-	};
-
-	msg = i2o_msg_in_to_virt(c, mfa);
-	memcpy_toio(msg, nop, sizeof(nop));
-	writel(mfa, c->in_port);
-};
-
-/**
- *	i2o_msg_nop - Returns a message which is not used
- *	@c: I2O controller from which the message was created
- *	@msg: message which should be returned
- *
- *	If you fetch a message via i2o_msg_get, and can't use it, you must
- *	return the message with this function. Otherwise the MFA is lost as well
- *	as the allocated memory from the mempool.
- */
-static inline void i2o_msg_nop(struct i2o_controller *c,
-			       struct i2o_message *msg)
-{
-	struct i2o_msg_mfa *mmsg;
-	mmsg = container_of(msg, struct i2o_msg_mfa, msg);
-
-	i2o_msg_nop_mfa(c, mmsg->mfa);
-	mempool_free(mmsg, c->in_msg.mempool);
-};
-
-/**
- *	i2o_flush_reply - Flush reply from I2O controller
- *	@c: I2O controller
- *	@m: the message identifier
- *
- *	The I2O controller must be informed that the reply message is not needed
- *	anymore. If you forget to flush the reply, the message frame can't be
- *	used by the controller anymore and is therefore lost.
- */
-static inline void i2o_flush_reply(struct i2o_controller *c, u32 m)
-{
-	writel(m, c->out_port);
-};
-
-/*
- *	Endian handling wrapped into the macro - keeps the core code
- *	cleaner.
- */
-
-#define i2o_raw_writel(val, mem)	__raw_writel(cpu_to_le32(val), mem)
-
-extern int i2o_parm_field_get(struct i2o_device *, int, int, void *, int);
-extern int i2o_parm_table_get(struct i2o_device *, int, int, int, void *, int,
-			      void *, int);
-
-/* debugging and troubleshooting/diagnostic helpers. */
-#define osm_printk(level, format, arg...)  \
-	printk(level "%s: " format, OSM_NAME , ## arg)
-
-#ifdef DEBUG
-#define osm_debug(format, arg...) \
-	osm_printk(KERN_DEBUG, format , ## arg)
-#else
-#define osm_debug(format, arg...) \
-        do { } while (0)
-#endif
-
-#define osm_err(format, arg...)		\
-	osm_printk(KERN_ERR, format , ## arg)
-#define osm_info(format, arg...)		\
-	osm_printk(KERN_INFO, format , ## arg)
-#define osm_warn(format, arg...)		\
-	osm_printk(KERN_WARNING, format , ## arg)
-
-/* debugging functions */
-extern void i2o_report_status(const char *, const char *, struct i2o_message *);
-extern void i2o_dump_message(struct i2o_message *);
-extern void i2o_dump_hrt(struct i2o_controller *c);
-extern void i2o_debug_state(struct i2o_controller *c);
-
-#endif				/* _I2O_H */
diff --git a/drivers/staging/i2o/i2o_block.c b/drivers/staging/i2o/i2o_block.c
deleted file mode 100644
index 406758f755ee..000000000000
--- a/drivers/staging/i2o/i2o_block.c
+++ /dev/null
@@ -1,1228 +0,0 @@
-/*
- *	Block OSM
- *
- * 	Copyright (C) 1999-2002	Red Hat Software
- *
- *	Written by Alan Cox, Building Number Three Ltd
- *
- *	This program is free software; you can redistribute it and/or modify it
- *	under the terms of the GNU General Public License as published by the
- *	Free Software Foundation; either version 2 of the License, or (at your
- *	option) any later version.
- *
- *	This program is distributed in the hope that it will be useful, but
- *	WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	General Public License for more details.
- *
- *	For the purpose of avoiding doubt the preferred form of the work
- *	for making modifications shall be a standards compliant form such
- *	gzipped tar and not one requiring a proprietary or patent encumbered
- *	tool to unpack.
- *
- *	Fixes/additions:
- *		Steve Ralston:
- *			Multiple device handling error fixes,
- *			Added a queue depth.
- *		Alan Cox:
- *			FC920 has an rmw bug. Dont or in the end marker.
- *			Removed queue walk, fixed for 64bitness.
- *			Rewrote much of the code over time
- *			Added indirect block lists
- *			Handle 64K limits on many controllers
- *			Don't use indirects on the Promise (breaks)
- *			Heavily chop down the queue depths
- *		Deepak Saxena:
- *			Independent queues per IOP
- *			Support for dynamic device creation/deletion
- *			Code cleanup
- *	    		Support for larger I/Os through merge* functions
- *			(taken from DAC960 driver)
- *		Boji T Kannanthanam:
- *			Set the I2O Block devices to be detected in increasing
- *			order of TIDs during boot.
- *			Search and set the I2O block device that we boot off
- *			from as the first device to be claimed (as /dev/i2o/hda)
- *			Properly attach/detach I2O gendisk structure from the
- *			system gendisk list. The I2O block devices now appear in
- *			/proc/partitions.
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *			Minor bugfixes for 2.6.
- */
-
-#include <linux/module.h>
-#include <linux/slab.h>
-#include "i2o.h"
-#include <linux/mutex.h>
-
-#include <linux/mempool.h>
-
-#include <linux/genhd.h>
-#include <linux/blkdev.h>
-#include <linux/hdreg.h>
-
-#include <scsi/scsi.h>
-
-#include "i2o_block.h"
-
-#define OSM_NAME	"block-osm"
-#define OSM_VERSION	"1.325"
-#define OSM_DESCRIPTION	"I2O Block Device OSM"
-
-static DEFINE_MUTEX(i2o_block_mutex);
-static struct i2o_driver i2o_block_driver;
-
-/* global Block OSM request mempool */
-static struct i2o_block_mempool i2o_blk_req_pool;
-
-/* Block OSM class handling definition */
-static struct i2o_class_id i2o_block_class_id[] = {
-	{I2O_CLASS_RANDOM_BLOCK_STORAGE},
-	{I2O_CLASS_END}
-};
-
-/**
- *	i2o_block_device_free - free the memory of the I2O Block device
- *	@dev: I2O Block device, which should be cleaned up
- *
- *	Frees the request queue, gendisk and the i2o_block_device structure.
- */
-static void i2o_block_device_free(struct i2o_block_device *dev)
-{
-	blk_cleanup_queue(dev->gd->queue);
-
-	put_disk(dev->gd);
-
-	kfree(dev);
-};
-
-/**
- *	i2o_block_remove - remove the I2O Block device from the system again
- *	@dev: I2O Block device which should be removed
- *
- *	Remove gendisk from system and free all allocated memory.
- *
- *	Always returns 0.
- */
-static int i2o_block_remove(struct device *dev)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-	struct i2o_block_device *i2o_blk_dev = dev_get_drvdata(dev);
-
-	osm_info("device removed (TID: %03x): %s\n", i2o_dev->lct_data.tid,
-		 i2o_blk_dev->gd->disk_name);
-
-	i2o_event_register(i2o_dev, &i2o_block_driver, 0, 0);
-
-	del_gendisk(i2o_blk_dev->gd);
-
-	dev_set_drvdata(dev, NULL);
-
-	i2o_device_claim_release(i2o_dev);
-
-	i2o_block_device_free(i2o_blk_dev);
-
-	return 0;
-};
-
-/**
- *	i2o_block_device flush - Flush all dirty data of I2O device dev
- *	@dev: I2O device which should be flushed
- *
- *	Flushes all dirty data on device dev.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_block_device_flush(struct i2o_device *dev)
-{
-	struct i2o_message *msg;
-
-	msg = i2o_msg_get_wait(dev->iop, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FIVE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_BLOCK_CFLUSH << 24 | HOST_TID << 12 | dev->
-			lct_data.tid);
-	msg->body[0] = cpu_to_le32(60 << 16);
-	osm_debug("Flushing...\n");
-
-	return i2o_msg_post_wait(dev->iop, msg, 60);
-};
-
-/**
- *	i2o_block_device_mount - Mount (load) the media of device dev
- *	@dev: I2O device which should receive the mount request
- *	@media_id: Media Identifier
- *
- *	Load a media into drive. Identifier should be set to -1, because the
- *	spec does not support any other value.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_block_device_mount(struct i2o_device *dev, u32 media_id)
-{
-	struct i2o_message *msg;
-
-	msg = i2o_msg_get_wait(dev->iop, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FIVE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_BLOCK_MMOUNT << 24 | HOST_TID << 12 | dev->
-			lct_data.tid);
-	msg->body[0] = cpu_to_le32(-1);
-	msg->body[1] = cpu_to_le32(0x00000000);
-	osm_debug("Mounting...\n");
-
-	return i2o_msg_post_wait(dev->iop, msg, 2);
-};
-
-/**
- *	i2o_block_device_lock - Locks the media of device dev
- *	@dev: I2O device which should receive the lock request
- *	@media_id: Media Identifier
- *
- *	Lock media of device dev to prevent removal. The media identifier
- *	should be set to -1, because the spec does not support any other value.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_block_device_lock(struct i2o_device *dev, u32 media_id)
-{
-	struct i2o_message *msg;
-
-	msg = i2o_msg_get_wait(dev->iop, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FIVE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_BLOCK_MLOCK << 24 | HOST_TID << 12 | dev->
-			lct_data.tid);
-	msg->body[0] = cpu_to_le32(-1);
-	osm_debug("Locking...\n");
-
-	return i2o_msg_post_wait(dev->iop, msg, 2);
-};
-
-/**
- *	i2o_block_device_unlock - Unlocks the media of device dev
- *	@dev: I2O device which should receive the unlocked request
- *	@media_id: Media Identifier
- *
- *	Unlocks the media in device dev. The media identifier should be set to
- *	-1, because the spec does not support any other value.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_block_device_unlock(struct i2o_device *dev, u32 media_id)
-{
-	struct i2o_message *msg;
-
-	msg = i2o_msg_get_wait(dev->iop, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FIVE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_BLOCK_MUNLOCK << 24 | HOST_TID << 12 | dev->
-			lct_data.tid);
-	msg->body[0] = cpu_to_le32(media_id);
-	osm_debug("Unlocking...\n");
-
-	return i2o_msg_post_wait(dev->iop, msg, 2);
-};
-
-/**
- *	i2o_block_device_power - Power management for device dev
- *	@dev: I2O device which should receive the power management request
- *	@op: Operation to send
- *
- *	Send a power management request to the device dev.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_block_device_power(struct i2o_block_device *dev, u8 op)
-{
-	struct i2o_device *i2o_dev = dev->i2o_dev;
-	struct i2o_controller *c = i2o_dev->iop;
-	struct i2o_message *msg;
-	int rc;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FOUR_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_BLOCK_POWER << 24 | HOST_TID << 12 | i2o_dev->
-			lct_data.tid);
-	msg->body[0] = cpu_to_le32(op << 24);
-	osm_debug("Power...\n");
-
-	rc = i2o_msg_post_wait(c, msg, 60);
-	if (!rc)
-		dev->power = op;
-
-	return rc;
-};
-
-/**
- *	i2o_block_request_alloc - Allocate an I2O block request struct
- *
- *	Allocates an I2O block request struct and initialize the list.
- *
- *	Returns a i2o_block_request pointer on success or negative error code
- *	on failure.
- */
-static inline struct i2o_block_request *i2o_block_request_alloc(void)
-{
-	struct i2o_block_request *ireq;
-
-	ireq = mempool_alloc(i2o_blk_req_pool.pool, GFP_ATOMIC);
-	if (!ireq)
-		return ERR_PTR(-ENOMEM);
-
-	INIT_LIST_HEAD(&ireq->queue);
-	sg_init_table(ireq->sg_table, I2O_MAX_PHYS_SEGMENTS);
-
-	return ireq;
-};
-
-/**
- *	i2o_block_request_free - Frees a I2O block request
- *	@ireq: I2O block request which should be freed
- *
- *	Frees the allocated memory (give it back to the request mempool).
- */
-static inline void i2o_block_request_free(struct i2o_block_request *ireq)
-{
-	mempool_free(ireq, i2o_blk_req_pool.pool);
-};
-
-/**
- *	i2o_block_sglist_alloc - Allocate the SG list and map it
- *	@c: I2O controller to which the request belongs
- *	@ireq: I2O block request
- *	@mptr: message body pointer
- *
- *	Builds the SG list and map it to be accessible by the controller.
- *
- *	Returns 0 on failure or 1 on success.
- */
-static inline int i2o_block_sglist_alloc(struct i2o_controller *c,
-					 struct i2o_block_request *ireq,
-					 u32 ** mptr)
-{
-	int nents;
-	enum dma_data_direction direction;
-
-	ireq->dev = &c->pdev->dev;
-	nents = blk_rq_map_sg(ireq->req->q, ireq->req, ireq->sg_table);
-
-	if (rq_data_dir(ireq->req) == READ)
-		direction = PCI_DMA_FROMDEVICE;
-	else
-		direction = PCI_DMA_TODEVICE;
-
-	ireq->sg_nents = nents;
-
-	return i2o_dma_map_sg(c, ireq->sg_table, nents, direction, mptr);
-};
-
-/**
- *	i2o_block_sglist_free - Frees the SG list
- *	@ireq: I2O block request from which the SG should be freed
- *
- *	Frees the SG list from the I2O block request.
- */
-static inline void i2o_block_sglist_free(struct i2o_block_request *ireq)
-{
-	enum dma_data_direction direction;
-
-	if (rq_data_dir(ireq->req) == READ)
-		direction = PCI_DMA_FROMDEVICE;
-	else
-		direction = PCI_DMA_TODEVICE;
-
-	dma_unmap_sg(ireq->dev, ireq->sg_table, ireq->sg_nents, direction);
-};
-
-/**
- *	i2o_block_prep_req_fn - Allocates I2O block device specific struct
- *	@q: request queue for the request
- *	@req: the request to prepare
- *
- *	Allocate the necessary i2o_block_request struct and connect it to
- *	the request. This is needed that we not lose the SG list later on.
- *
- *	Returns BLKPREP_OK on success or BLKPREP_DEFER on failure.
- */
-static int i2o_block_prep_req_fn(struct request_queue *q, struct request *req)
-{
-	struct i2o_block_device *i2o_blk_dev = q->queuedata;
-	struct i2o_block_request *ireq;
-
-	if (unlikely(!i2o_blk_dev)) {
-		osm_err("block device already removed\n");
-		return BLKPREP_KILL;
-	}
-
-	/* connect the i2o_block_request to the request */
-	if (!req->special) {
-		ireq = i2o_block_request_alloc();
-		if (IS_ERR(ireq)) {
-			osm_debug("unable to allocate i2o_block_request!\n");
-			return BLKPREP_DEFER;
-		}
-
-		ireq->i2o_blk_dev = i2o_blk_dev;
-		req->special = ireq;
-		ireq->req = req;
-	}
-	/* do not come back here */
-	req->cmd_flags |= REQ_DONTPREP;
-
-	return BLKPREP_OK;
-};
-
-/**
- *	i2o_block_delayed_request_fn - delayed request queue function
- *	@work: the delayed request with the queue to start
- *
- *	If the request queue is stopped for a disk, and there is no open
- *	request, a new event is created, which calls this function to start
- *	the queue after I2O_BLOCK_REQUEST_TIME. Otherwise the queue will never
- *	be started again.
- */
-static void i2o_block_delayed_request_fn(struct work_struct *work)
-{
-	struct i2o_block_delayed_request *dreq =
-		container_of(work, struct i2o_block_delayed_request,
-			     work.work);
-	struct request_queue *q = dreq->queue;
-	unsigned long flags;
-
-	spin_lock_irqsave(q->queue_lock, flags);
-	blk_start_queue(q);
-	spin_unlock_irqrestore(q->queue_lock, flags);
-	kfree(dreq);
-};
-
-/**
- *	i2o_block_end_request - Post-processing of completed commands
- *	@req: request which should be completed
- *	@error: 0 for success, < 0 for error
- *	@nr_bytes: number of bytes to complete
- *
- *	Mark the request as complete. The lock must not be held when entering.
- *
- */
-static void i2o_block_end_request(struct request *req, int error,
-				  int nr_bytes)
-{
-	struct i2o_block_request *ireq = req->special;
-	struct i2o_block_device *dev = ireq->i2o_blk_dev;
-	struct request_queue *q = req->q;
-	unsigned long flags;
-
-	if (blk_end_request(req, error, nr_bytes))
-		if (error)
-			blk_end_request_all(req, -EIO);
-
-	spin_lock_irqsave(q->queue_lock, flags);
-
-	if (likely(dev)) {
-		dev->open_queue_depth--;
-		list_del(&ireq->queue);
-	}
-
-	blk_start_queue(q);
-
-	spin_unlock_irqrestore(q->queue_lock, flags);
-
-	i2o_block_sglist_free(ireq);
-	i2o_block_request_free(ireq);
-};
-
-/**
- *	i2o_block_reply - Block OSM reply handler.
- *	@c: I2O controller from which the message arrives
- *	@m: message id of reply
- *	@msg: the actual I2O message reply
- *
- *	This function gets all the message replies.
- *
- */
-static int i2o_block_reply(struct i2o_controller *c, u32 m,
-			   struct i2o_message *msg)
-{
-	struct request *req;
-	int error = 0;
-
-	req = i2o_cntxt_list_get(c, le32_to_cpu(msg->u.s.tcntxt));
-	if (unlikely(!req)) {
-		osm_err("NULL reply received!\n");
-		return -1;
-	}
-
-	/*
-	 *      Lets see what is cooking. We stuffed the
-	 *      request in the context.
-	 */
-
-	if ((le32_to_cpu(msg->body[0]) >> 24) != 0) {
-		u32 status = le32_to_cpu(msg->body[0]);
-		/*
-		 *      Device not ready means two things. One is that the
-		 *      the thing went offline (but not a removal media)
-		 *
-		 *      The second is that you have a SuperTrak 100 and the
-		 *      firmware got constipated. Unlike standard i2o card
-		 *      setups the supertrak returns an error rather than
-		 *      blocking for the timeout in these cases.
-		 *
-		 *      Don't stick a supertrak100 into cache aggressive modes
-		 */
-
-		osm_err("TID %03x error status: 0x%02x, detailed status: "
-			"0x%04x\n", (le32_to_cpu(msg->u.head[1]) >> 12 & 0xfff),
-			status >> 24, status & 0xffff);
-
-		req->errors++;
-
-		error = -EIO;
-	}
-
-	i2o_block_end_request(req, error, le32_to_cpu(msg->body[1]));
-
-	return 1;
-};
-
-static void i2o_block_event(struct work_struct *work)
-{
-	struct i2o_event *evt = container_of(work, struct i2o_event, work);
-	osm_debug("event received\n");
-	kfree(evt);
-};
-
-/*
- *	SCSI-CAM for ioctl geometry mapping
- *	Duplicated with SCSI - this should be moved into somewhere common
- *	perhaps genhd ?
- *
- * LBA -> CHS mapping table taken from:
- *
- * "Incorporating the I2O Architecture into BIOS for Intel Architecture
- *  Platforms"
- *
- * This is an I2O document that is only available to I2O members,
- * not developers.
- *
- * From my understanding, this is how all the I2O cards do this
- *
- * Disk Size      | Sectors | Heads | Cylinders
- * ---------------+---------+-------+-------------------
- * 1 < X <= 528M  | 63      | 16    | X/(63 * 16 * 512)
- * 528M < X <= 1G | 63      | 32    | X/(63 * 32 * 512)
- * 1 < X <528M    | 63      | 16    | X/(63 * 16 * 512)
- * 1 < X <528M    | 63      | 16    | X/(63 * 16 * 512)
- *
- */
-#define	BLOCK_SIZE_528M		1081344
-#define	BLOCK_SIZE_1G		2097152
-#define	BLOCK_SIZE_21G		4403200
-#define	BLOCK_SIZE_42G		8806400
-#define	BLOCK_SIZE_84G		17612800
-
-static void i2o_block_biosparam(unsigned long capacity, unsigned short *cyls,
-				unsigned char *hds, unsigned char *secs)
-{
-	unsigned long heads, sectors, cylinders;
-
-	sectors = 63L;		/* Maximize sectors per track */
-	if (capacity <= BLOCK_SIZE_528M)
-		heads = 16;
-	else if (capacity <= BLOCK_SIZE_1G)
-		heads = 32;
-	else if (capacity <= BLOCK_SIZE_21G)
-		heads = 64;
-	else if (capacity <= BLOCK_SIZE_42G)
-		heads = 128;
-	else
-		heads = 255;
-
-	cylinders = (unsigned long)capacity / (heads * sectors);
-
-	*cyls = (unsigned short)cylinders;	/* Stuff return values */
-	*secs = (unsigned char)sectors;
-	*hds = (unsigned char)heads;
-}
-
-/**
- *	i2o_block_open - Open the block device
- *	@bdev: block device being opened
- *	@mode: file open mode
- *
- *	Power up the device, mount and lock the media. This function is called,
- *	if the block device is opened for access.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_block_open(struct block_device *bdev, fmode_t mode)
-{
-	struct i2o_block_device *dev = bdev->bd_disk->private_data;
-
-	if (!dev->i2o_dev)
-		return -ENODEV;
-
-	mutex_lock(&i2o_block_mutex);
-	if (dev->power > 0x1f)
-		i2o_block_device_power(dev, 0x02);
-
-	i2o_block_device_mount(dev->i2o_dev, -1);
-
-	i2o_block_device_lock(dev->i2o_dev, -1);
-
-	osm_debug("Ready.\n");
-	mutex_unlock(&i2o_block_mutex);
-
-	return 0;
-};
-
-/**
- *	i2o_block_release - Release the I2O block device
- *	@disk: gendisk device being released
- *	@mode: file open mode
- *
- *	Unlock and unmount the media, and power down the device. Gets called if
- *	the block device is closed.
- */
-static void i2o_block_release(struct gendisk *disk, fmode_t mode)
-{
-	struct i2o_block_device *dev = disk->private_data;
-	u8 operation;
-
-	/*
-	 * This is to deal with the case of an application
-	 * opening a device and then the device disappears while
-	 * it's in use, and then the application tries to release
-	 * it.  ex: Unmounting a deleted RAID volume at reboot.
-	 * If we send messages, it will just cause FAILs since
-	 * the TID no longer exists.
-	 */
-	if (!dev->i2o_dev)
-		return;
-
-	mutex_lock(&i2o_block_mutex);
-	i2o_block_device_flush(dev->i2o_dev);
-
-	i2o_block_device_unlock(dev->i2o_dev, -1);
-
-	if (dev->flags & (1 << 3 | 1 << 4))	/* Removable */
-		operation = 0x21;
-	else
-		operation = 0x24;
-
-	i2o_block_device_power(dev, operation);
-	mutex_unlock(&i2o_block_mutex);
-}
-
-static int i2o_block_getgeo(struct block_device *bdev, struct hd_geometry *geo)
-{
-	i2o_block_biosparam(get_capacity(bdev->bd_disk),
-			    &geo->cylinders, &geo->heads, &geo->sectors);
-	return 0;
-}
-
-/**
- *	i2o_block_ioctl - Issue device specific ioctl calls.
- *	@bdev: block device being opened
- *	@mode: file open mode
- *	@cmd: ioctl command
- *	@arg: arg
- *
- *	Handles ioctl request for the block device.
- *
- *	Return 0 on success or negative error on failure.
- */
-static int i2o_block_ioctl(struct block_device *bdev, fmode_t mode,
-			   unsigned int cmd, unsigned long arg)
-{
-	struct gendisk *disk = bdev->bd_disk;
-	struct i2o_block_device *dev = disk->private_data;
-	int ret = -ENOTTY;
-
-	/* Anyone capable of this syscall can do *real bad* things */
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EPERM;
-
-	mutex_lock(&i2o_block_mutex);
-	switch (cmd) {
-	case BLKI2OGRSTRAT:
-		ret = put_user(dev->rcache, (int __user *)arg);
-		break;
-	case BLKI2OGWSTRAT:
-		ret = put_user(dev->wcache, (int __user *)arg);
-		break;
-	case BLKI2OSRSTRAT:
-		ret = -EINVAL;
-		if (arg < 0 || arg > CACHE_SMARTFETCH)
-			break;
-		dev->rcache = arg;
-		ret = 0;
-		break;
-	case BLKI2OSWSTRAT:
-		ret = -EINVAL;
-		if (arg != 0
-		    && (arg < CACHE_WRITETHROUGH || arg > CACHE_SMARTBACK))
-			break;
-		dev->wcache = arg;
-		ret = 0;
-		break;
-	}
-	mutex_unlock(&i2o_block_mutex);
-
-	return ret;
-};
-
-/**
- *	i2o_block_check_events - Have we seen a media change?
- *	@disk: gendisk which should be verified
- *	@clearing: events being cleared
- *
- *	Verifies if the media has changed.
- *
- *	Returns 1 if the media was changed or 0 otherwise.
- */
-static unsigned int i2o_block_check_events(struct gendisk *disk,
-					   unsigned int clearing)
-{
-	struct i2o_block_device *p = disk->private_data;
-
-	if (p->media_change_flag) {
-		p->media_change_flag = 0;
-		return DISK_EVENT_MEDIA_CHANGE;
-	}
-	return 0;
-}
-
-/**
- *	i2o_block_transfer - Transfer a request to/from the I2O controller
- *	@req: the request which should be transferred
- *
- *	This function converts the request into a I2O message. The necessary
- *	DMA buffers are allocated and after everything is setup post the message
- *	to the I2O controller. No cleanup is done by this function. It is done
- *	on the interrupt side when the reply arrives.
- *
- *	Return 0 on success or negative error code on failure.
- */
-static int i2o_block_transfer(struct request *req)
-{
-	struct i2o_block_device *dev = req->rq_disk->private_data;
-	struct i2o_controller *c;
-	u32 tid;
-	struct i2o_message *msg;
-	u32 *mptr;
-	struct i2o_block_request *ireq = req->special;
-	u32 tcntxt;
-	u32 sgl_offset = SGL_OFFSET_8;
-	u32 ctl_flags = 0x00000000;
-	int rc;
-	u32 cmd;
-
-	if (unlikely(!dev->i2o_dev)) {
-		osm_err("transfer to removed drive\n");
-		rc = -ENODEV;
-		goto exit;
-	}
-
-	tid = dev->i2o_dev->lct_data.tid;
-	c = dev->i2o_dev->iop;
-
-	msg = i2o_msg_get(c);
-	if (IS_ERR(msg)) {
-		rc = PTR_ERR(msg);
-		goto exit;
-	}
-
-	tcntxt = i2o_cntxt_list_add(c, req);
-	if (!tcntxt) {
-		rc = -ENOMEM;
-		goto nop_msg;
-	}
-
-	msg->u.s.icntxt = cpu_to_le32(i2o_block_driver.context);
-	msg->u.s.tcntxt = cpu_to_le32(tcntxt);
-
-	mptr = &msg->body[0];
-
-	if (rq_data_dir(req) == READ) {
-		cmd = I2O_CMD_BLOCK_READ << 24;
-
-		switch (dev->rcache) {
-		case CACHE_PREFETCH:
-			ctl_flags = 0x201F0008;
-			break;
-
-		case CACHE_SMARTFETCH:
-			if (blk_rq_sectors(req) > 16)
-				ctl_flags = 0x201F0008;
-			else
-				ctl_flags = 0x001F0000;
-			break;
-
-		default:
-			break;
-		}
-	} else {
-		cmd = I2O_CMD_BLOCK_WRITE << 24;
-
-		switch (dev->wcache) {
-		case CACHE_WRITETHROUGH:
-			ctl_flags = 0x001F0008;
-			break;
-		case CACHE_WRITEBACK:
-			ctl_flags = 0x001F0010;
-			break;
-		case CACHE_SMARTBACK:
-			if (blk_rq_sectors(req) > 16)
-				ctl_flags = 0x001F0004;
-			else
-				ctl_flags = 0x001F0010;
-			break;
-		case CACHE_SMARTTHROUGH:
-			if (blk_rq_sectors(req) > 16)
-				ctl_flags = 0x001F0004;
-			else
-				ctl_flags = 0x001F0010;
-		default:
-			break;
-		}
-	}
-
-#ifdef CONFIG_I2O_EXT_ADAPTEC
-	if (c->adaptec) {
-		u8 cmd[10];
-		u32 scsi_flags;
-		u16 hwsec;
-
-		hwsec = queue_logical_block_size(req->q) >> KERNEL_SECTOR_SHIFT;
-		memset(cmd, 0, 10);
-
-		sgl_offset = SGL_OFFSET_12;
-
-		msg->u.head[1] =
-		    cpu_to_le32(I2O_CMD_PRIVATE << 24 | HOST_TID << 12 | tid);
-
-		*mptr++ = cpu_to_le32(I2O_VENDOR_DPT << 16 | I2O_CMD_SCSI_EXEC);
-		*mptr++ = cpu_to_le32(tid);
-
-		/*
-		 * ENABLE_DISCONNECT
-		 * SIMPLE_TAG
-		 * RETURN_SENSE_DATA_IN_REPLY_MESSAGE_FRAME
-		 */
-		if (rq_data_dir(req) == READ) {
-			cmd[0] = READ_10;
-			scsi_flags = 0x60a0000a;
-		} else {
-			cmd[0] = WRITE_10;
-			scsi_flags = 0xa0a0000a;
-		}
-
-		*mptr++ = cpu_to_le32(scsi_flags);
-
-		*((u32 *) & cmd[2]) = cpu_to_be32(blk_rq_pos(req) * hwsec);
-		*((u16 *) & cmd[7]) = cpu_to_be16(blk_rq_sectors(req) * hwsec);
-
-		memcpy(mptr, cmd, 10);
-		mptr += 4;
-		*mptr++ = cpu_to_le32(blk_rq_bytes(req));
-	} else
-#endif
-	{
-		msg->u.head[1] = cpu_to_le32(cmd | HOST_TID << 12 | tid);
-		*mptr++ = cpu_to_le32(ctl_flags);
-		*mptr++ = cpu_to_le32(blk_rq_bytes(req));
-		*mptr++ =
-		    cpu_to_le32((u32) (blk_rq_pos(req) << KERNEL_SECTOR_SHIFT));
-		*mptr++ =
-		    cpu_to_le32(blk_rq_pos(req) >> (32 - KERNEL_SECTOR_SHIFT));
-	}
-
-	if (!i2o_block_sglist_alloc(c, ireq, &mptr)) {
-		rc = -ENOMEM;
-		goto context_remove;
-	}
-
-	msg->u.head[0] =
-	    cpu_to_le32(I2O_MESSAGE_SIZE(mptr - &msg->u.head[0]) | sgl_offset);
-
-	list_add_tail(&ireq->queue, &dev->open_queue);
-	dev->open_queue_depth++;
-
-	i2o_msg_post(c, msg);
-
-	return 0;
-
-context_remove:
-	i2o_cntxt_list_remove(c, req);
-
-nop_msg:
-	i2o_msg_nop(c, msg);
-
-exit:
-	return rc;
-};
-
-/**
- *	i2o_block_request_fn - request queue handling function
- *	@q: request queue from which the request could be fetched
- *
- *	Takes the next request from the queue, transfers it and if no error
- *	occurs dequeue it from the queue. On arrival of the reply the message
- *	will be processed further. If an error occurs requeue the request.
- */
-static void i2o_block_request_fn(struct request_queue *q)
-{
-	struct request *req;
-
-	while ((req = blk_peek_request(q)) != NULL) {
-		if (req->cmd_type == REQ_TYPE_FS) {
-			struct i2o_block_delayed_request *dreq;
-			struct i2o_block_request *ireq = req->special;
-			unsigned int queue_depth;
-
-			queue_depth = ireq->i2o_blk_dev->open_queue_depth;
-
-			if (queue_depth < I2O_BLOCK_MAX_OPEN_REQUESTS) {
-				if (!i2o_block_transfer(req)) {
-					blk_start_request(req);
-					continue;
-				} else
-					osm_info("transfer error\n");
-			}
-
-			if (queue_depth)
-				break;
-
-			/* stop the queue and retry later */
-			dreq = kmalloc(sizeof(*dreq), GFP_ATOMIC);
-			if (!dreq)
-				continue;
-
-			dreq->queue = q;
-			INIT_DELAYED_WORK(&dreq->work,
-					  i2o_block_delayed_request_fn);
-
-			if (!queue_delayed_work(i2o_block_driver.event_queue,
-						&dreq->work,
-						I2O_BLOCK_RETRY_TIME))
-				kfree(dreq);
-			else {
-				blk_stop_queue(q);
-				break;
-			}
-		} else {
-			blk_start_request(req);
-			__blk_end_request_all(req, -EIO);
-		}
-	}
-};
-
-/* I2O Block device operations definition */
-static const struct block_device_operations i2o_block_fops = {
-	.owner = THIS_MODULE,
-	.open = i2o_block_open,
-	.release = i2o_block_release,
-	.ioctl = i2o_block_ioctl,
-	.compat_ioctl = i2o_block_ioctl,
-	.getgeo = i2o_block_getgeo,
-	.check_events = i2o_block_check_events,
-};
-
-/**
- *	i2o_block_device_alloc - Allocate memory for a I2O Block device
- *
- *	Allocate memory for the i2o_block_device struct, gendisk and request
- *	queue and initialize them as far as no additional information is needed.
- *
- *	Returns a pointer to the allocated I2O Block device on success or a
- *	negative error code on failure.
- */
-static struct i2o_block_device *i2o_block_device_alloc(void)
-{
-	struct i2o_block_device *dev;
-	struct gendisk *gd;
-	struct request_queue *queue;
-	int rc;
-
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (!dev) {
-		osm_err("Insufficient memory to allocate I2O Block disk.\n");
-		rc = -ENOMEM;
-		goto exit;
-	}
-
-	INIT_LIST_HEAD(&dev->open_queue);
-	spin_lock_init(&dev->lock);
-	dev->rcache = CACHE_PREFETCH;
-	dev->wcache = CACHE_WRITEBACK;
-
-	/* allocate a gendisk with 16 partitions */
-	gd = alloc_disk(16);
-	if (!gd) {
-		osm_err("Insufficient memory to allocate gendisk.\n");
-		rc = -ENOMEM;
-		goto cleanup_dev;
-	}
-
-	/* initialize the request queue */
-	queue = blk_init_queue(i2o_block_request_fn, &dev->lock);
-	if (!queue) {
-		osm_err("Insufficient memory to allocate request queue.\n");
-		rc = -ENOMEM;
-		goto cleanup_queue;
-	}
-
-	blk_queue_prep_rq(queue, i2o_block_prep_req_fn);
-
-	gd->major = I2O_MAJOR;
-	gd->queue = queue;
-	gd->fops = &i2o_block_fops;
-	gd->private_data = dev;
-
-	dev->gd = gd;
-
-	return dev;
-
-cleanup_queue:
-	put_disk(gd);
-
-cleanup_dev:
-	kfree(dev);
-
-exit:
-	return ERR_PTR(rc);
-};
-
-/**
- *	i2o_block_probe - verify if dev is a I2O Block device and install it
- *	@dev: device to verify if it is a I2O Block device
- *
- *	We only verify if the user_tid of the device is 0xfff and then install
- *	the device. Otherwise it is used by some other device (e. g. RAID).
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_block_probe(struct device *dev)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-	struct i2o_controller *c = i2o_dev->iop;
-	struct i2o_block_device *i2o_blk_dev;
-	struct gendisk *gd;
-	struct request_queue *queue;
-	static int unit;
-	int rc;
-	u64 size;
-	u32 blocksize;
-	u16 body_size = 4;
-	u16 power;
-	unsigned short max_sectors;
-
-#ifdef CONFIG_I2O_EXT_ADAPTEC
-	if (c->adaptec)
-		body_size = 8;
-#endif
-
-	if (c->limit_sectors)
-		max_sectors = I2O_MAX_SECTORS_LIMITED;
-	else
-		max_sectors = I2O_MAX_SECTORS;
-
-	/* skip devices which are used by IOP */
-	if (i2o_dev->lct_data.user_tid != 0xfff) {
-		osm_debug("skipping used device %03x\n", i2o_dev->lct_data.tid);
-		return -ENODEV;
-	}
-
-	if (i2o_device_claim(i2o_dev)) {
-		osm_warn("Unable to claim device. Installation aborted\n");
-		rc = -EFAULT;
-		goto exit;
-	}
-
-	i2o_blk_dev = i2o_block_device_alloc();
-	if (IS_ERR(i2o_blk_dev)) {
-		osm_err("could not alloc a new I2O block device");
-		rc = PTR_ERR(i2o_blk_dev);
-		goto claim_release;
-	}
-
-	i2o_blk_dev->i2o_dev = i2o_dev;
-	dev_set_drvdata(dev, i2o_blk_dev);
-
-	/* setup gendisk */
-	gd = i2o_blk_dev->gd;
-	gd->first_minor = unit << 4;
-	sprintf(gd->disk_name, "i2o/hd%c", 'a' + unit);
-	gd->driverfs_dev = &i2o_dev->device;
-
-	/* setup request queue */
-	queue = gd->queue;
-	queue->queuedata = i2o_blk_dev;
-
-	blk_queue_max_hw_sectors(queue, max_sectors);
-	blk_queue_max_segments(queue, i2o_sg_tablesize(c, body_size));
-
-	osm_debug("max sectors = %d\n", queue->max_sectors);
-	osm_debug("phys segments = %d\n", queue->max_phys_segments);
-	osm_debug("max hw segments = %d\n", queue->max_hw_segments);
-
-	/*
-	 *      Ask for the current media data. If that isn't supported
-	 *      then we ask for the device capacity data
-	 */
-	if (!i2o_parm_field_get(i2o_dev, 0x0004, 1, &blocksize, 4) ||
-	    !i2o_parm_field_get(i2o_dev, 0x0000, 3, &blocksize, 4)) {
-		blk_queue_logical_block_size(queue, le32_to_cpu(blocksize));
-	} else
-		osm_warn("unable to get blocksize of %s\n", gd->disk_name);
-
-	if (!i2o_parm_field_get(i2o_dev, 0x0004, 0, &size, 8) ||
-	    !i2o_parm_field_get(i2o_dev, 0x0000, 4, &size, 8)) {
-		set_capacity(gd, le64_to_cpu(size) >> KERNEL_SECTOR_SHIFT);
-	} else
-		osm_warn("could not get size of %s\n", gd->disk_name);
-
-	if (!i2o_parm_field_get(i2o_dev, 0x0000, 2, &power, 2))
-		i2o_blk_dev->power = power;
-
-	i2o_event_register(i2o_dev, &i2o_block_driver, 0, 0xffffffff);
-
-	add_disk(gd);
-
-	unit++;
-
-	osm_info("device added (TID: %03x): %s\n", i2o_dev->lct_data.tid,
-		 i2o_blk_dev->gd->disk_name);
-
-	return 0;
-
-claim_release:
-	i2o_device_claim_release(i2o_dev);
-
-exit:
-	return rc;
-};
-
-/* Block OSM driver struct */
-static struct i2o_driver i2o_block_driver = {
-	.name = OSM_NAME,
-	.event = i2o_block_event,
-	.reply = i2o_block_reply,
-	.classes = i2o_block_class_id,
-	.driver = {
-		   .probe = i2o_block_probe,
-		   .remove = i2o_block_remove,
-		   },
-};
-
-/**
- *	i2o_block_init - Block OSM initialization function
- *
- *	Allocate the slab and mempool for request structs, registers i2o_block
- *	block device and finally register the Block OSM in the I2O core.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int __init i2o_block_init(void)
-{
-	int rc;
-	int size;
-
-	printk(KERN_INFO OSM_DESCRIPTION " v" OSM_VERSION "\n");
-
-	/* Allocate request mempool and slab */
-	size = sizeof(struct i2o_block_request);
-	i2o_blk_req_pool.slab = kmem_cache_create("i2o_block_req", size, 0,
-						  SLAB_HWCACHE_ALIGN, NULL);
-	if (!i2o_blk_req_pool.slab) {
-		osm_err("can't init request slab\n");
-		rc = -ENOMEM;
-		goto exit;
-	}
-
-	i2o_blk_req_pool.pool =
-		mempool_create_slab_pool(I2O_BLOCK_REQ_MEMPOOL_SIZE,
-					 i2o_blk_req_pool.slab);
-	if (!i2o_blk_req_pool.pool) {
-		osm_err("can't init request mempool\n");
-		rc = -ENOMEM;
-		goto free_slab;
-	}
-
-	/* Register the block device interfaces */
-	rc = register_blkdev(I2O_MAJOR, "i2o_block");
-	if (rc) {
-		osm_err("unable to register block device\n");
-		goto free_mempool;
-	}
-#ifdef MODULE
-	osm_info("registered device at major %d\n", I2O_MAJOR);
-#endif
-
-	/* Register Block OSM into I2O core */
-	rc = i2o_driver_register(&i2o_block_driver);
-	if (rc) {
-		osm_err("Could not register Block driver\n");
-		goto unregister_blkdev;
-	}
-
-	return 0;
-
-unregister_blkdev:
-	unregister_blkdev(I2O_MAJOR, "i2o_block");
-
-free_mempool:
-	mempool_destroy(i2o_blk_req_pool.pool);
-
-free_slab:
-	kmem_cache_destroy(i2o_blk_req_pool.slab);
-
-exit:
-	return rc;
-};
-
-/**
- *	i2o_block_exit - Block OSM exit function
- *
- *	Unregisters Block OSM from I2O core, unregisters i2o_block block device
- *	and frees the mempool and slab.
- */
-static void __exit i2o_block_exit(void)
-{
-	/* Unregister I2O Block OSM from I2O core */
-	i2o_driver_unregister(&i2o_block_driver);
-
-	/* Unregister block device */
-	unregister_blkdev(I2O_MAJOR, "i2o_block");
-
-	/* Free request mempool and slab */
-	mempool_destroy(i2o_blk_req_pool.pool);
-	kmem_cache_destroy(i2o_blk_req_pool.slab);
-};
-
-MODULE_AUTHOR("Red Hat");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(OSM_DESCRIPTION);
-MODULE_VERSION(OSM_VERSION);
-
-module_init(i2o_block_init);
-module_exit(i2o_block_exit);
diff --git a/drivers/staging/i2o/i2o_block.h b/drivers/staging/i2o/i2o_block.h
deleted file mode 100644
index cf8873cbca3f..000000000000
--- a/drivers/staging/i2o/i2o_block.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
- *	Block OSM structures/API
- *
- * 	Copyright (C) 1999-2002	Red Hat Software
- *
- *	Written by Alan Cox, Building Number Three Ltd
- *
- *	This program is free software; you can redistribute it and/or modify it
- *	under the terms of the GNU General Public License as published by the
- *	Free Software Foundation; either version 2 of the License, or (at your
- *	option) any later version.
- *
- *	This program is distributed in the hope that it will be useful, but
- *	WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	General Public License for more details.
- *
- *	For the purpose of avoiding doubt the preferred form of the work
- *	for making modifications shall be a standards compliant form such
- *	gzipped tar and not one requiring a proprietary or patent encumbered
- *	tool to unpack.
- *
- *	Fixes/additions:
- *		Steve Ralston:
- *			Multiple device handling error fixes,
- *			Added a queue depth.
- *		Alan Cox:
- *			FC920 has an rmw bug. Dont or in the end marker.
- *			Removed queue walk, fixed for 64bitness.
- *			Rewrote much of the code over time
- *			Added indirect block lists
- *			Handle 64K limits on many controllers
- *			Don't use indirects on the Promise (breaks)
- *			Heavily chop down the queue depths
- *		Deepak Saxena:
- *			Independent queues per IOP
- *			Support for dynamic device creation/deletion
- *			Code cleanup
- *	    		Support for larger I/Os through merge* functions
- *			(taken from DAC960 driver)
- *		Boji T Kannanthanam:
- *			Set the I2O Block devices to be detected in increasing
- *			order of TIDs during boot.
- *			Search and set the I2O block device that we boot off
- *			from as the first device to be claimed (as /dev/i2o/hda)
- *			Properly attach/detach I2O gendisk structure from the
- *			system gendisk list. The I2O block devices now appear in
- *			/proc/partitions.
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *			Minor bugfixes for 2.6.
- */
-
-#ifndef I2O_BLOCK_OSM_H
-#define I2O_BLOCK_OSM_H
-
-#define I2O_BLOCK_RETRY_TIME HZ/4
-#define I2O_BLOCK_MAX_OPEN_REQUESTS 50
-
-/* request queue sizes */
-#define I2O_BLOCK_REQ_MEMPOOL_SIZE		32
-
-#define KERNEL_SECTOR_SHIFT 9
-#define KERNEL_SECTOR_SIZE (1 << KERNEL_SECTOR_SHIFT)
-
-/* I2O Block OSM mempool struct */
-struct i2o_block_mempool {
-	struct kmem_cache *slab;
-	mempool_t *pool;
-};
-
-/* I2O Block device descriptor */
-struct i2o_block_device {
-	struct i2o_device *i2o_dev;	/* pointer to I2O device */
-	struct gendisk *gd;
-	spinlock_t lock;	/* queue lock */
-	struct list_head open_queue;	/* list of transferred, but unfinished
-					   requests */
-	unsigned int open_queue_depth;	/* number of requests in the queue */
-
-	int rcache;		/* read cache flags */
-	int wcache;		/* write cache flags */
-	int flags;
-	u16 power;		/* power state */
-	int media_change_flag;	/* media changed flag */
-};
-
-/* I2O Block device request */
-struct i2o_block_request {
-	struct list_head queue;
-	struct request *req;	/* corresponding request */
-	struct i2o_block_device *i2o_blk_dev;	/* I2O block device */
-	struct device *dev;	/* device used for DMA */
-	int sg_nents;		/* number of SG elements */
-	struct scatterlist sg_table[I2O_MAX_PHYS_SEGMENTS];	/* SG table */
-};
-
-/* I2O Block device delayed request */
-struct i2o_block_delayed_request {
-	struct delayed_work work;
-	struct request_queue *queue;
-};
-
-#endif
diff --git a/drivers/staging/i2o/i2o_config.c b/drivers/staging/i2o/i2o_config.c
deleted file mode 100644
index cd7ca5eb18ff..000000000000
--- a/drivers/staging/i2o/i2o_config.c
+++ /dev/null
@@ -1,1162 +0,0 @@
-/*
- * I2O Configuration Interface Driver
- *
- * (C) Copyright 1999-2002  Red Hat
- *
- * Written by Alan Cox, Building Number Three Ltd
- *
- * Fixes/additions:
- *	Deepak Saxena (04/20/1999):
- *		Added basic ioctl() support
- *	Deepak Saxena (06/07/1999):
- *		Added software download ioctl (still testing)
- *	Auvo Häkkinen (09/10/1999):
- *		Changes to i2o_cfg_reply(), ioctl_parms()
- *		Added ioct_validate()
- *	Taneli Vähäkangas (09/30/1999):
- *		Fixed ioctl_swdl()
- *	Taneli Vähäkangas (10/04/1999):
- *		Changed ioctl_swdl(), implemented ioctl_swul() and ioctl_swdel()
- *	Deepak Saxena (11/18/1999):
- *		Added event managmenet support
- *	Alan Cox <alan@lxorguk.ukuu.org.uk>:
- *		2.4 rewrite ported to 2.5
- *	Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *		Added pass-thru support for Adaptec's raidutils
- *
- * This program is free software; you can redistribute it and/or
- * modify it under 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/miscdevice.h>
-#include <linux/mutex.h>
-#include <linux/compat.h>
-#include <linux/slab.h>
-#include <linux/uaccess.h>
-
-#include "core.h"
-
-#define SG_TABLESIZE		30
-
-static DEFINE_MUTEX(i2o_cfg_mutex);
-static long i2o_cfg_ioctl(struct file *, unsigned int, unsigned long);
-
-static spinlock_t i2o_config_lock;
-
-#define MODINC(x,y) ((x) = ((x) + 1) % (y))
-
-struct sg_simple_element {
-	u32 flag_count;
-	u32 addr_bus;
-};
-
-struct i2o_cfg_info {
-	struct file *fp;
-	struct fasync_struct *fasync;
-	struct i2o_evt_info event_q[I2O_EVT_Q_LEN];
-	u16 q_in;		// Queue head index
-	u16 q_out;		// Queue tail index
-	u16 q_len;		// Queue length
-	u16 q_lost;		// Number of lost events
-	ulong q_id;		// Event queue ID...used as tx_context
-	struct i2o_cfg_info *next;
-};
-static struct i2o_cfg_info *open_files = NULL;
-static ulong i2o_cfg_info_id;
-
-static int i2o_cfg_getiops(unsigned long arg)
-{
-	struct i2o_controller *c;
-	u8 __user *user_iop_table = (void __user *)arg;
-	u8 tmp[MAX_I2O_CONTROLLERS];
-	int ret = 0;
-
-	memset(tmp, 0, MAX_I2O_CONTROLLERS);
-
-	list_for_each_entry(c, &i2o_controllers, list)
-	    tmp[c->unit] = 1;
-
-	if (copy_to_user(user_iop_table, tmp, MAX_I2O_CONTROLLERS))
-		ret = -EFAULT;
-
-	return ret;
-};
-
-static int i2o_cfg_gethrt(unsigned long arg)
-{
-	struct i2o_controller *c;
-	struct i2o_cmd_hrtlct __user *cmd = (struct i2o_cmd_hrtlct __user *)arg;
-	struct i2o_cmd_hrtlct kcmd;
-	i2o_hrt *hrt;
-	int len;
-	u32 reslen;
-	int ret = 0;
-
-	if (copy_from_user(&kcmd, cmd, sizeof(struct i2o_cmd_hrtlct)))
-		return -EFAULT;
-
-	if (get_user(reslen, kcmd.reslen) < 0)
-		return -EFAULT;
-
-	if (kcmd.resbuf == NULL)
-		return -EFAULT;
-
-	c = i2o_find_iop(kcmd.iop);
-	if (!c)
-		return -ENXIO;
-
-	hrt = (i2o_hrt *) c->hrt.virt;
-
-	len = 8 + ((hrt->entry_len * hrt->num_entries) << 2);
-
-	if (put_user(len, kcmd.reslen))
-		ret = -EFAULT;
-	else if (len > reslen)
-		ret = -ENOBUFS;
-	else if (copy_to_user(kcmd.resbuf, (void *)hrt, len))
-		ret = -EFAULT;
-
-	return ret;
-};
-
-static int i2o_cfg_getlct(unsigned long arg)
-{
-	struct i2o_controller *c;
-	struct i2o_cmd_hrtlct __user *cmd = (struct i2o_cmd_hrtlct __user *)arg;
-	struct i2o_cmd_hrtlct kcmd;
-	i2o_lct *lct;
-	int len;
-	int ret = 0;
-	u32 reslen;
-
-	if (copy_from_user(&kcmd, cmd, sizeof(struct i2o_cmd_hrtlct)))
-		return -EFAULT;
-
-	if (get_user(reslen, kcmd.reslen) < 0)
-		return -EFAULT;
-
-	if (kcmd.resbuf == NULL)
-		return -EFAULT;
-
-	c = i2o_find_iop(kcmd.iop);
-	if (!c)
-		return -ENXIO;
-
-	lct = (i2o_lct *) c->lct;
-
-	len = (unsigned int)lct->table_size << 2;
-	if (put_user(len, kcmd.reslen))
-		ret = -EFAULT;
-	else if (len > reslen)
-		ret = -ENOBUFS;
-	else if (copy_to_user(kcmd.resbuf, lct, len))
-		ret = -EFAULT;
-
-	return ret;
-};
-
-static int i2o_cfg_parms(unsigned long arg, unsigned int type)
-{
-	int ret = 0;
-	struct i2o_controller *c;
-	struct i2o_device *dev;
-	struct i2o_cmd_psetget __user *cmd =
-	    (struct i2o_cmd_psetget __user *)arg;
-	struct i2o_cmd_psetget kcmd;
-	u32 reslen;
-	u8 *ops;
-	u8 *res;
-	int len = 0;
-
-	u32 i2o_cmd = (type == I2OPARMGET ?
-		       I2O_CMD_UTIL_PARAMS_GET : I2O_CMD_UTIL_PARAMS_SET);
-
-	if (copy_from_user(&kcmd, cmd, sizeof(struct i2o_cmd_psetget)))
-		return -EFAULT;
-
-	if (get_user(reslen, kcmd.reslen))
-		return -EFAULT;
-
-	c = i2o_find_iop(kcmd.iop);
-	if (!c)
-		return -ENXIO;
-
-	dev = i2o_iop_find_device(c, kcmd.tid);
-	if (!dev)
-		return -ENXIO;
-
-	/*
-	 * Stop users being able to try and allocate arbitrary amounts
-	 * of DMA space. 64K is way more than sufficient for this.
-	 */
-	if (kcmd.oplen > 65536)
-		return -EMSGSIZE;
-
-	ops = memdup_user(kcmd.opbuf, kcmd.oplen);
-	if (IS_ERR(ops))
-		return PTR_ERR(ops);
-
-	/*
-	 * It's possible to have a _very_ large table
-	 * and that the user asks for all of it at once...
-	 */
-	res = kmalloc(65536, GFP_KERNEL);
-	if (!res) {
-		kfree(ops);
-		return -ENOMEM;
-	}
-
-	len = i2o_parm_issue(dev, i2o_cmd, ops, kcmd.oplen, res, 65536);
-	kfree(ops);
-
-	if (len < 0) {
-		kfree(res);
-		return -EAGAIN;
-	}
-
-	if (put_user(len, kcmd.reslen))
-		ret = -EFAULT;
-	else if (len > reslen)
-		ret = -ENOBUFS;
-	else if (copy_to_user(kcmd.resbuf, res, len))
-		ret = -EFAULT;
-
-	kfree(res);
-
-	return ret;
-};
-
-static int i2o_cfg_swdl(unsigned long arg)
-{
-	struct i2o_sw_xfer kxfer;
-	struct i2o_sw_xfer __user *pxfer = (struct i2o_sw_xfer __user *)arg;
-	unsigned char maxfrag = 0, curfrag = 1;
-	struct i2o_dma buffer;
-	struct i2o_message *msg;
-	unsigned int status = 0, swlen = 0, fragsize = 8192;
-	struct i2o_controller *c;
-
-	if (copy_from_user(&kxfer, pxfer, sizeof(struct i2o_sw_xfer)))
-		return -EFAULT;
-
-	if (get_user(swlen, kxfer.swlen) < 0)
-		return -EFAULT;
-
-	if (get_user(maxfrag, kxfer.maxfrag) < 0)
-		return -EFAULT;
-
-	if (get_user(curfrag, kxfer.curfrag) < 0)
-		return -EFAULT;
-
-	if (curfrag == maxfrag)
-		fragsize = swlen - (maxfrag - 1) * 8192;
-
-	if (!kxfer.buf || !access_ok(VERIFY_READ, kxfer.buf, fragsize))
-		return -EFAULT;
-
-	c = i2o_find_iop(kxfer.iop);
-	if (!c)
-		return -ENXIO;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	if (i2o_dma_alloc(&c->pdev->dev, &buffer, fragsize)) {
-		i2o_msg_nop(c, msg);
-		return -ENOMEM;
-	}
-
-	if (__copy_from_user(buffer.virt, kxfer.buf, fragsize)) {
-		i2o_msg_nop(c, msg);
-		i2o_dma_free(&c->pdev->dev, &buffer);
-		return -EFAULT;
-	}
-
-	msg->u.head[0] = cpu_to_le32(NINE_WORD_MSG_SIZE | SGL_OFFSET_7);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_SW_DOWNLOAD << 24 | HOST_TID << 12 |
-			ADAPTER_TID);
-	msg->u.head[2] = cpu_to_le32(i2o_config_driver.context);
-	msg->u.head[3] = cpu_to_le32(0);
-	msg->body[0] =
-	    cpu_to_le32((((u32) kxfer.flags) << 24) | (((u32) kxfer.
-							sw_type) << 16) |
-			(((u32) maxfrag) << 8) | (((u32) curfrag)));
-	msg->body[1] = cpu_to_le32(swlen);
-	msg->body[2] = cpu_to_le32(kxfer.sw_id);
-	msg->body[3] = cpu_to_le32(0xD0000000 | fragsize);
-	msg->body[4] = cpu_to_le32(buffer.phys);
-
-	osm_debug("swdl frag %d/%d (size %d)\n", curfrag, maxfrag, fragsize);
-	status = i2o_msg_post_wait_mem(c, msg, 60, &buffer);
-
-	if (status != -ETIMEDOUT)
-		i2o_dma_free(&c->pdev->dev, &buffer);
-
-	if (status != I2O_POST_WAIT_OK) {
-		// it fails if you try and send frags out of order
-		// and for some yet unknown reasons too
-		osm_info("swdl failed, DetailedStatus = %d\n", status);
-		return status;
-	}
-
-	return 0;
-};
-
-static int i2o_cfg_swul(unsigned long arg)
-{
-	struct i2o_sw_xfer kxfer;
-	struct i2o_sw_xfer __user *pxfer = (struct i2o_sw_xfer __user *)arg;
-	unsigned char maxfrag = 0, curfrag = 1;
-	struct i2o_dma buffer;
-	struct i2o_message *msg;
-	unsigned int status = 0, swlen = 0, fragsize = 8192;
-	struct i2o_controller *c;
-	int ret = 0;
-
-	if (copy_from_user(&kxfer, pxfer, sizeof(struct i2o_sw_xfer)))
-		return -EFAULT;
-
-	if (get_user(swlen, kxfer.swlen) < 0)
-		return -EFAULT;
-
-	if (get_user(maxfrag, kxfer.maxfrag) < 0)
-		return -EFAULT;
-
-	if (get_user(curfrag, kxfer.curfrag) < 0)
-		return -EFAULT;
-
-	if (curfrag == maxfrag)
-		fragsize = swlen - (maxfrag - 1) * 8192;
-
-	if (!kxfer.buf)
-		return -EFAULT;
-
-	c = i2o_find_iop(kxfer.iop);
-	if (!c)
-		return -ENXIO;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	if (i2o_dma_alloc(&c->pdev->dev, &buffer, fragsize)) {
-		i2o_msg_nop(c, msg);
-		return -ENOMEM;
-	}
-
-	msg->u.head[0] = cpu_to_le32(NINE_WORD_MSG_SIZE | SGL_OFFSET_7);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_SW_UPLOAD << 24 | HOST_TID << 12 | ADAPTER_TID);
-	msg->u.head[2] = cpu_to_le32(i2o_config_driver.context);
-	msg->u.head[3] = cpu_to_le32(0);
-	msg->body[0] =
-	    cpu_to_le32((u32) kxfer.flags << 24 | (u32) kxfer.
-			sw_type << 16 | (u32) maxfrag << 8 | (u32) curfrag);
-	msg->body[1] = cpu_to_le32(swlen);
-	msg->body[2] = cpu_to_le32(kxfer.sw_id);
-	msg->body[3] = cpu_to_le32(0xD0000000 | fragsize);
-	msg->body[4] = cpu_to_le32(buffer.phys);
-
-	osm_debug("swul frag %d/%d (size %d)\n", curfrag, maxfrag, fragsize);
-	status = i2o_msg_post_wait_mem(c, msg, 60, &buffer);
-
-	if (status != I2O_POST_WAIT_OK) {
-		if (status != -ETIMEDOUT)
-			i2o_dma_free(&c->pdev->dev, &buffer);
-
-		osm_info("swul failed, DetailedStatus = %d\n", status);
-		return status;
-	}
-
-	if (copy_to_user(kxfer.buf, buffer.virt, fragsize))
-		ret = -EFAULT;
-
-	i2o_dma_free(&c->pdev->dev, &buffer);
-
-	return ret;
-}
-
-static int i2o_cfg_swdel(unsigned long arg)
-{
-	struct i2o_controller *c;
-	struct i2o_sw_xfer kxfer;
-	struct i2o_sw_xfer __user *pxfer = (struct i2o_sw_xfer __user *)arg;
-	struct i2o_message *msg;
-	unsigned int swlen;
-	int token;
-
-	if (copy_from_user(&kxfer, pxfer, sizeof(struct i2o_sw_xfer)))
-		return -EFAULT;
-
-	if (get_user(swlen, kxfer.swlen) < 0)
-		return -EFAULT;
-
-	c = i2o_find_iop(kxfer.iop);
-	if (!c)
-		return -ENXIO;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(SEVEN_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_SW_REMOVE << 24 | HOST_TID << 12 | ADAPTER_TID);
-	msg->u.head[2] = cpu_to_le32(i2o_config_driver.context);
-	msg->u.head[3] = cpu_to_le32(0);
-	msg->body[0] =
-	    cpu_to_le32((u32) kxfer.flags << 24 | (u32) kxfer.sw_type << 16);
-	msg->body[1] = cpu_to_le32(swlen);
-	msg->body[2] = cpu_to_le32(kxfer.sw_id);
-
-	token = i2o_msg_post_wait(c, msg, 10);
-
-	if (token != I2O_POST_WAIT_OK) {
-		osm_info("swdel failed, DetailedStatus = %d\n", token);
-		return -ETIMEDOUT;
-	}
-
-	return 0;
-};
-
-static int i2o_cfg_validate(unsigned long arg)
-{
-	int token;
-	int iop = (int)arg;
-	struct i2o_message *msg;
-	struct i2o_controller *c;
-
-	c = i2o_find_iop(iop);
-	if (!c)
-		return -ENXIO;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FOUR_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_CONFIG_VALIDATE << 24 | HOST_TID << 12 | iop);
-	msg->u.head[2] = cpu_to_le32(i2o_config_driver.context);
-	msg->u.head[3] = cpu_to_le32(0);
-
-	token = i2o_msg_post_wait(c, msg, 10);
-
-	if (token != I2O_POST_WAIT_OK) {
-		osm_info("Can't validate configuration, ErrorStatus = %d\n",
-			 token);
-		return -ETIMEDOUT;
-	}
-
-	return 0;
-};
-
-static int i2o_cfg_evt_reg(unsigned long arg, struct file *fp)
-{
-	struct i2o_message *msg;
-	struct i2o_evt_id __user *pdesc = (struct i2o_evt_id __user *)arg;
-	struct i2o_evt_id kdesc;
-	struct i2o_controller *c;
-	struct i2o_device *d;
-
-	if (copy_from_user(&kdesc, pdesc, sizeof(struct i2o_evt_id)))
-		return -EFAULT;
-
-	/* IOP exists? */
-	c = i2o_find_iop(kdesc.iop);
-	if (!c)
-		return -ENXIO;
-
-	/* Device exists? */
-	d = i2o_iop_find_device(c, kdesc.tid);
-	if (!d)
-		return -ENODEV;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FOUR_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_UTIL_EVT_REGISTER << 24 | HOST_TID << 12 |
-			kdesc.tid);
-	msg->u.head[2] = cpu_to_le32(i2o_config_driver.context);
-	msg->u.head[3] = cpu_to_le32(i2o_cntxt_list_add(c, fp->private_data));
-	msg->body[0] = cpu_to_le32(kdesc.evt_mask);
-
-	i2o_msg_post(c, msg);
-
-	return 0;
-}
-
-static int i2o_cfg_evt_get(unsigned long arg, struct file *fp)
-{
-	struct i2o_cfg_info *p = NULL;
-	struct i2o_evt_get __user *uget = (struct i2o_evt_get __user *)arg;
-	struct i2o_evt_get kget;
-	unsigned long flags;
-
-	for (p = open_files; p; p = p->next)
-		if (p->q_id == (ulong) fp->private_data)
-			break;
-
-	if (!p->q_len)
-		return -ENOENT;
-
-	memcpy(&kget.info, &p->event_q[p->q_out], sizeof(struct i2o_evt_info));
-	MODINC(p->q_out, I2O_EVT_Q_LEN);
-	spin_lock_irqsave(&i2o_config_lock, flags);
-	p->q_len--;
-	kget.pending = p->q_len;
-	kget.lost = p->q_lost;
-	spin_unlock_irqrestore(&i2o_config_lock, flags);
-
-	if (copy_to_user(uget, &kget, sizeof(struct i2o_evt_get)))
-		return -EFAULT;
-	return 0;
-}
-
-#ifdef CONFIG_COMPAT
-static int i2o_cfg_passthru32(struct file *file, unsigned cmnd,
-			      unsigned long arg)
-{
-	struct i2o_cmd_passthru32 __user *cmd;
-	struct i2o_controller *c;
-	u32 __user *user_msg;
-	u32 *reply = NULL;
-	u32 __user *user_reply = NULL;
-	u32 size = 0;
-	u32 reply_size = 0;
-	u32 rcode = 0;
-	struct i2o_dma sg_list[SG_TABLESIZE];
-	u32 sg_offset = 0;
-	u32 sg_count = 0;
-	u32 i = 0;
-	u32 sg_index = 0;
-	i2o_status_block *sb;
-	struct i2o_message *msg;
-	unsigned int iop;
-
-	cmd = (struct i2o_cmd_passthru32 __user *)arg;
-
-	if (get_user(iop, &cmd->iop) || get_user(i, &cmd->msg))
-		return -EFAULT;
-
-	user_msg = compat_ptr(i);
-
-	c = i2o_find_iop(iop);
-	if (!c) {
-		osm_debug("controller %d not found\n", iop);
-		return -ENXIO;
-	}
-
-	sb = c->status_block.virt;
-
-	if (get_user(size, &user_msg[0])) {
-		osm_warn("unable to get size!\n");
-		return -EFAULT;
-	}
-	size = size >> 16;
-
-	if (size > sb->inbound_frame_size) {
-		osm_warn("size of message > inbound_frame_size");
-		return -EFAULT;
-	}
-
-	user_reply = &user_msg[size];
-
-	size <<= 2;		// Convert to bytes
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	rcode = -EFAULT;
-	/* Copy in the user's I2O command */
-	if (copy_from_user(msg, user_msg, size)) {
-		osm_warn("unable to copy user message\n");
-		goto out;
-	}
-	i2o_dump_message(msg);
-
-	if (get_user(reply_size, &user_reply[0]) < 0)
-		goto out;
-
-	reply_size >>= 16;
-	reply_size <<= 2;
-
-	rcode = -ENOMEM;
-	reply = kzalloc(reply_size, GFP_KERNEL);
-	if (!reply) {
-		printk(KERN_WARNING "%s: Could not allocate reply buffer\n",
-		       c->name);
-		goto out;
-	}
-
-	sg_offset = (msg->u.head[0] >> 4) & 0x0f;
-
-	memset(sg_list, 0, sizeof(sg_list[0]) * SG_TABLESIZE);
-	if (sg_offset) {
-		struct sg_simple_element *sg;
-
-		if (sg_offset * 4 >= size) {
-			rcode = -EFAULT;
-			goto cleanup;
-		}
-		// TODO 64bit fix
-		sg = (struct sg_simple_element *)((&msg->u.head[0]) +
-						  sg_offset);
-		sg_count =
-		    (size - sg_offset * 4) / sizeof(struct sg_simple_element);
-		if (sg_count > SG_TABLESIZE) {
-			printk(KERN_DEBUG "%s:IOCTL SG List too large (%u)\n",
-			       c->name, sg_count);
-			rcode = -EINVAL;
-			goto cleanup;
-		}
-
-		for (i = 0; i < sg_count; i++) {
-			int sg_size;
-			struct i2o_dma *p;
-
-			if (!(sg[i].flag_count & 0x10000000
-			      /*I2O_SGL_FLAGS_SIMPLE_ADDRESS_ELEMENT */ )) {
-				printk(KERN_DEBUG
-				       "%s:Bad SG element %d - not simple (%x)\n",
-				       c->name, i, sg[i].flag_count);
-				rcode = -EINVAL;
-				goto cleanup;
-			}
-			sg_size = sg[i].flag_count & 0xffffff;
-			p = &(sg_list[sg_index]);
-			/* Allocate memory for the transfer */
-			if (i2o_dma_alloc(&c->pdev->dev, p, sg_size)) {
-				printk(KERN_DEBUG
-				       "%s: Could not allocate SG buffer - size = %d buffer number %d of %d\n",
-				       c->name, sg_size, i, sg_count);
-				rcode = -ENOMEM;
-				goto sg_list_cleanup;
-			}
-			sg_index++;
-			/* Copy in the user's SG buffer if necessary */
-			if (sg[i].
-			    flag_count & 0x04000000 /*I2O_SGL_FLAGS_DIR */ ) {
-				// TODO 64bit fix
-				if (copy_from_user
-				    (p->virt,
-				     (void __user *)(unsigned long)sg[i].
-				     addr_bus, sg_size)) {
-					printk(KERN_DEBUG
-					       "%s: Could not copy SG buf %d FROM user\n",
-					       c->name, i);
-					rcode = -EFAULT;
-					goto sg_list_cleanup;
-				}
-			}
-			//TODO 64bit fix
-			sg[i].addr_bus = (u32) p->phys;
-		}
-	}
-
-	rcode = i2o_msg_post_wait(c, msg, 60);
-	msg = NULL;
-	if (rcode) {
-		reply[4] = ((u32) rcode) << 24;
-		goto sg_list_cleanup;
-	}
-
-	if (sg_offset) {
-		u32 rmsg[I2O_OUTBOUND_MSG_FRAME_SIZE];
-		/* Copy back the Scatter Gather buffers back to user space */
-		u32 j;
-		// TODO 64bit fix
-		struct sg_simple_element *sg;
-		int sg_size;
-
-		// re-acquire the original message to handle correctly the sg copy operation
-		memset(&rmsg, 0, I2O_OUTBOUND_MSG_FRAME_SIZE * 4);
-		// get user msg size in u32s
-		if (get_user(size, &user_msg[0])) {
-			rcode = -EFAULT;
-			goto sg_list_cleanup;
-		}
-		size = size >> 16;
-		size *= 4;
-		if (size > sizeof(rmsg)) {
-			rcode = -EINVAL;
-			goto sg_list_cleanup;
-		}
-
-		/* Copy in the user's I2O command */
-		if (copy_from_user(rmsg, user_msg, size)) {
-			rcode = -EFAULT;
-			goto sg_list_cleanup;
-		}
-		sg_count =
-		    (size - sg_offset * 4) / sizeof(struct sg_simple_element);
-
-		// TODO 64bit fix
-		sg = (struct sg_simple_element *)(rmsg + sg_offset);
-		for (j = 0; j < sg_count; j++) {
-			/* Copy out the SG list to user's buffer if necessary */
-			if (!
-			    (sg[j].
-			     flag_count & 0x4000000 /*I2O_SGL_FLAGS_DIR */ )) {
-				sg_size = sg[j].flag_count & 0xffffff;
-				// TODO 64bit fix
-				if (copy_to_user
-				    ((void __user *)(u64) sg[j].addr_bus,
-				     sg_list[j].virt, sg_size)) {
-					printk(KERN_WARNING
-					       "%s: Could not copy %p TO user %x\n",
-					       c->name, sg_list[j].virt,
-					       sg[j].addr_bus);
-					rcode = -EFAULT;
-					goto sg_list_cleanup;
-				}
-			}
-		}
-	}
-
-sg_list_cleanup:
-	/* Copy back the reply to user space */
-	if (reply_size) {
-		// we wrote our own values for context - now restore the user supplied ones
-		if (copy_from_user(reply + 2, user_msg + 2, sizeof(u32) * 2)) {
-			printk(KERN_WARNING
-			       "%s: Could not copy message context FROM user\n",
-			       c->name);
-			rcode = -EFAULT;
-		}
-		if (copy_to_user(user_reply, reply, reply_size)) {
-			printk(KERN_WARNING
-			       "%s: Could not copy reply TO user\n", c->name);
-			rcode = -EFAULT;
-		}
-	}
-	for (i = 0; i < sg_index; i++)
-		i2o_dma_free(&c->pdev->dev, &sg_list[i]);
-
-cleanup:
-	kfree(reply);
-out:
-	if (msg)
-		i2o_msg_nop(c, msg);
-	return rcode;
-}
-
-static long i2o_cfg_compat_ioctl(struct file *file, unsigned cmd,
-				 unsigned long arg)
-{
-	int ret;
-	switch (cmd) {
-	case I2OGETIOPS:
-		ret = i2o_cfg_ioctl(file, cmd, arg);
-		break;
-	case I2OPASSTHRU32:
-		mutex_lock(&i2o_cfg_mutex);
-		ret = i2o_cfg_passthru32(file, cmd, arg);
-		mutex_unlock(&i2o_cfg_mutex);
-		break;
-	default:
-		ret = -ENOIOCTLCMD;
-		break;
-	}
-	return ret;
-}
-
-#endif
-
-#ifdef CONFIG_I2O_EXT_ADAPTEC
-static int i2o_cfg_passthru(unsigned long arg)
-{
-	struct i2o_cmd_passthru __user *cmd =
-	    (struct i2o_cmd_passthru __user *)arg;
-	struct i2o_controller *c;
-	u32 __user *user_msg;
-	u32 *reply = NULL;
-	u32 __user *user_reply = NULL;
-	u32 size = 0;
-	u32 reply_size = 0;
-	u32 rcode = 0;
-	struct i2o_dma sg_list[SG_TABLESIZE];
-	u32 sg_offset = 0;
-	u32 sg_count = 0;
-	int sg_index = 0;
-	u32 i = 0;
-	i2o_status_block *sb;
-	struct i2o_message *msg;
-	unsigned int iop;
-
-	if (get_user(iop, &cmd->iop) || get_user(user_msg, &cmd->msg))
-		return -EFAULT;
-
-	c = i2o_find_iop(iop);
-	if (!c) {
-		osm_warn("controller %d not found\n", iop);
-		return -ENXIO;
-	}
-
-	sb = c->status_block.virt;
-
-	if (get_user(size, &user_msg[0]))
-		return -EFAULT;
-	size = size >> 16;
-
-	if (size > sb->inbound_frame_size) {
-		osm_warn("size of message > inbound_frame_size");
-		return -EFAULT;
-	}
-
-	user_reply = &user_msg[size];
-
-	size <<= 2;		// Convert to bytes
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	rcode = -EFAULT;
-	/* Copy in the user's I2O command */
-	if (copy_from_user(msg, user_msg, size))
-		goto out;
-
-	if (get_user(reply_size, &user_reply[0]) < 0)
-		goto out;
-
-	reply_size >>= 16;
-	reply_size <<= 2;
-
-	reply = kzalloc(reply_size, GFP_KERNEL);
-	if (!reply) {
-		printk(KERN_WARNING "%s: Could not allocate reply buffer\n",
-		       c->name);
-		rcode = -ENOMEM;
-		goto out;
-	}
-
-	sg_offset = (msg->u.head[0] >> 4) & 0x0f;
-
-	memset(sg_list, 0, sizeof(sg_list[0]) * SG_TABLESIZE);
-	if (sg_offset) {
-		struct sg_simple_element *sg;
-		struct i2o_dma *p;
-
-		if (sg_offset * 4 >= size) {
-			rcode = -EFAULT;
-			goto cleanup;
-		}
-		// TODO 64bit fix
-		sg = (struct sg_simple_element *)((&msg->u.head[0]) +
-						  sg_offset);
-		sg_count =
-		    (size - sg_offset * 4) / sizeof(struct sg_simple_element);
-		if (sg_count > SG_TABLESIZE) {
-			printk(KERN_DEBUG "%s:IOCTL SG List too large (%u)\n",
-			       c->name, sg_count);
-			rcode = -EINVAL;
-			goto cleanup;
-		}
-
-		for (i = 0; i < sg_count; i++) {
-			int sg_size;
-
-			if (!(sg[i].flag_count & 0x10000000
-			      /*I2O_SGL_FLAGS_SIMPLE_ADDRESS_ELEMENT */ )) {
-				printk(KERN_DEBUG
-				       "%s:Bad SG element %d - not simple (%x)\n",
-				       c->name, i, sg[i].flag_count);
-				rcode = -EINVAL;
-				goto sg_list_cleanup;
-			}
-			sg_size = sg[i].flag_count & 0xffffff;
-			p = &(sg_list[sg_index]);
-			if (i2o_dma_alloc(&c->pdev->dev, p, sg_size)) {
-			/* Allocate memory for the transfer */
-				printk(KERN_DEBUG
-				       "%s: Could not allocate SG buffer - size = %d buffer number %d of %d\n",
-				       c->name, sg_size, i, sg_count);
-				rcode = -ENOMEM;
-				goto sg_list_cleanup;
-			}
-			sg_index++;
-			/* Copy in the user's SG buffer if necessary */
-			if (sg[i].
-			    flag_count & 0x04000000 /*I2O_SGL_FLAGS_DIR */ ) {
-				// TODO 64bit fix
-				if (copy_from_user
-				    (p->virt, (void __user *)sg[i].addr_bus,
-				     sg_size)) {
-					printk(KERN_DEBUG
-					       "%s: Could not copy SG buf %d FROM user\n",
-					       c->name, i);
-					rcode = -EFAULT;
-					goto sg_list_cleanup;
-				}
-			}
-			sg[i].addr_bus = p->phys;
-		}
-	}
-
-	rcode = i2o_msg_post_wait(c, msg, 60);
-	msg = NULL;
-	if (rcode) {
-		reply[4] = ((u32) rcode) << 24;
-		goto sg_list_cleanup;
-	}
-
-	if (sg_offset) {
-		u32 rmsg[I2O_OUTBOUND_MSG_FRAME_SIZE];
-		/* Copy back the Scatter Gather buffers back to user space */
-		u32 j;
-		// TODO 64bit fix
-		struct sg_simple_element *sg;
-		int sg_size;
-
-		// re-acquire the original message to handle correctly the sg copy operation
-		memset(&rmsg, 0, I2O_OUTBOUND_MSG_FRAME_SIZE * 4);
-		// get user msg size in u32s
-		if (get_user(size, &user_msg[0])) {
-			rcode = -EFAULT;
-			goto sg_list_cleanup;
-		}
-		size = size >> 16;
-		size *= 4;
-		if (size > sizeof(rmsg)) {
-			rcode = -EFAULT;
-			goto sg_list_cleanup;
-		}
-
-		/* Copy in the user's I2O command */
-		if (copy_from_user(rmsg, user_msg, size)) {
-			rcode = -EFAULT;
-			goto sg_list_cleanup;
-		}
-		sg_count =
-		    (size - sg_offset * 4) / sizeof(struct sg_simple_element);
-
-		// TODO 64bit fix
-		sg = (struct sg_simple_element *)(rmsg + sg_offset);
-		for (j = 0; j < sg_count; j++) {
-			/* Copy out the SG list to user's buffer if necessary */
-			if (!
-			    (sg[j].
-			     flag_count & 0x4000000 /*I2O_SGL_FLAGS_DIR */ )) {
-				sg_size = sg[j].flag_count & 0xffffff;
-				// TODO 64bit fix
-				if (copy_to_user
-				    ((void __user *)sg[j].addr_bus, sg_list[j].virt,
-				     sg_size)) {
-					printk(KERN_WARNING
-					       "%s: Could not copy %p TO user %x\n",
-					       c->name, sg_list[j].virt,
-					       sg[j].addr_bus);
-					rcode = -EFAULT;
-					goto sg_list_cleanup;
-				}
-			}
-		}
-	}
-
-sg_list_cleanup:
-	/* Copy back the reply to user space */
-	if (reply_size) {
-		// we wrote our own values for context - now restore the user supplied ones
-		if (copy_from_user(reply + 2, user_msg + 2, sizeof(u32) * 2)) {
-			printk(KERN_WARNING
-			       "%s: Could not copy message context FROM user\n",
-			       c->name);
-			rcode = -EFAULT;
-		}
-		if (copy_to_user(user_reply, reply, reply_size)) {
-			printk(KERN_WARNING
-			       "%s: Could not copy reply TO user\n", c->name);
-			rcode = -EFAULT;
-		}
-	}
-
-	for (i = 0; i < sg_index; i++)
-		i2o_dma_free(&c->pdev->dev, &sg_list[i]);
-
-cleanup:
-	kfree(reply);
-out:
-	if (msg)
-		i2o_msg_nop(c, msg);
-	return rcode;
-}
-#endif
-
-/*
- * IOCTL Handler
- */
-static long i2o_cfg_ioctl(struct file *fp, unsigned int cmd, unsigned long arg)
-{
-	int ret;
-
-	mutex_lock(&i2o_cfg_mutex);
-	switch (cmd) {
-	case I2OGETIOPS:
-		ret = i2o_cfg_getiops(arg);
-		break;
-
-	case I2OHRTGET:
-		ret = i2o_cfg_gethrt(arg);
-		break;
-
-	case I2OLCTGET:
-		ret = i2o_cfg_getlct(arg);
-		break;
-
-	case I2OPARMSET:
-		ret = i2o_cfg_parms(arg, I2OPARMSET);
-		break;
-
-	case I2OPARMGET:
-		ret = i2o_cfg_parms(arg, I2OPARMGET);
-		break;
-
-	case I2OSWDL:
-		ret = i2o_cfg_swdl(arg);
-		break;
-
-	case I2OSWUL:
-		ret = i2o_cfg_swul(arg);
-		break;
-
-	case I2OSWDEL:
-		ret = i2o_cfg_swdel(arg);
-		break;
-
-	case I2OVALIDATE:
-		ret = i2o_cfg_validate(arg);
-		break;
-
-	case I2OEVTREG:
-		ret = i2o_cfg_evt_reg(arg, fp);
-		break;
-
-	case I2OEVTGET:
-		ret = i2o_cfg_evt_get(arg, fp);
-		break;
-
-#ifdef CONFIG_I2O_EXT_ADAPTEC
-	case I2OPASSTHRU:
-		ret = i2o_cfg_passthru(arg);
-		break;
-#endif
-
-	default:
-		osm_debug("unknown ioctl called!\n");
-		ret = -EINVAL;
-	}
-	mutex_unlock(&i2o_cfg_mutex);
-	return ret;
-}
-
-static int cfg_open(struct inode *inode, struct file *file)
-{
-	struct i2o_cfg_info *tmp = kmalloc(sizeof(struct i2o_cfg_info),
-					   GFP_KERNEL);
-	unsigned long flags;
-
-	if (!tmp)
-		return -ENOMEM;
-
-	mutex_lock(&i2o_cfg_mutex);
-	file->private_data = (void *)(i2o_cfg_info_id++);
-	tmp->fp = file;
-	tmp->fasync = NULL;
-	tmp->q_id = (ulong) file->private_data;
-	tmp->q_len = 0;
-	tmp->q_in = 0;
-	tmp->q_out = 0;
-	tmp->q_lost = 0;
-	tmp->next = open_files;
-
-	spin_lock_irqsave(&i2o_config_lock, flags);
-	open_files = tmp;
-	spin_unlock_irqrestore(&i2o_config_lock, flags);
-	mutex_unlock(&i2o_cfg_mutex);
-
-	return 0;
-}
-
-static int cfg_fasync(int fd, struct file *fp, int on)
-{
-	ulong id = (ulong) fp->private_data;
-	struct i2o_cfg_info *p;
-	int ret = -EBADF;
-
-	mutex_lock(&i2o_cfg_mutex);
-	for (p = open_files; p; p = p->next)
-		if (p->q_id == id)
-			break;
-
-	if (p)
-		ret = fasync_helper(fd, fp, on, &p->fasync);
-	mutex_unlock(&i2o_cfg_mutex);
-	return ret;
-}
-
-static int cfg_release(struct inode *inode, struct file *file)
-{
-	ulong id = (ulong) file->private_data;
-	struct i2o_cfg_info *p, **q;
-	unsigned long flags;
-
-	mutex_lock(&i2o_cfg_mutex);
-	spin_lock_irqsave(&i2o_config_lock, flags);
-	for (q = &open_files; (p = *q) != NULL; q = &p->next) {
-		if (p->q_id == id) {
-			*q = p->next;
-			kfree(p);
-			break;
-		}
-	}
-	spin_unlock_irqrestore(&i2o_config_lock, flags);
-	mutex_unlock(&i2o_cfg_mutex);
-
-	return 0;
-}
-
-static const struct file_operations config_fops = {
-	.owner = THIS_MODULE,
-	.llseek = no_llseek,
-	.unlocked_ioctl = i2o_cfg_ioctl,
-#ifdef CONFIG_COMPAT
-	.compat_ioctl = i2o_cfg_compat_ioctl,
-#endif
-	.open = cfg_open,
-	.release = cfg_release,
-	.fasync = cfg_fasync,
-};
-
-static struct miscdevice i2o_miscdev = {
-	I2O_MINOR,
-	"i2octl",
-	&config_fops
-};
-
-static int __init i2o_config_old_init(void)
-{
-	spin_lock_init(&i2o_config_lock);
-
-	if (misc_register(&i2o_miscdev) < 0) {
-		osm_err("can't register device.\n");
-		return -EBUSY;
-	}
-
-	return 0;
-}
-
-static void i2o_config_old_exit(void)
-{
-	misc_deregister(&i2o_miscdev);
-}
-
-MODULE_AUTHOR("Red Hat Software");
diff --git a/drivers/staging/i2o/i2o_proc.c b/drivers/staging/i2o/i2o_proc.c
deleted file mode 100644
index 780fee3224ea..000000000000
--- a/drivers/staging/i2o/i2o_proc.c
+++ /dev/null
@@ -1,2049 +0,0 @@
-/*
- *	procfs handler for Linux I2O subsystem
- *
- *	(c) Copyright 1999	Deepak Saxena
- *
- *	Originally written by Deepak Saxena(deepak@plexity.net)
- *
- *	This program is free software; you can redistribute it and/or modify it
- *	under the terms of the GNU General Public License as published by the
- *	Free Software Foundation; either version 2 of the License, or (at your
- *	option) any later version.
- *
- *	This is an initial test release. The code is based on the design of the
- *	ide procfs system (drivers/block/ide-proc.c). Some code taken from
- *	i2o-core module by Alan Cox.
- *
- *	DISCLAIMER: This code is still under development/test and may cause
- *	your system to behave unpredictably.  Use at your own discretion.
- *
- *
- *	Fixes/additions:
- *		Juha Sievänen (Juha.Sievanen@cs.Helsinki.FI),
- *		Auvo Häkkinen (Auvo.Hakkinen@cs.Helsinki.FI)
- *		University of Helsinki, Department of Computer Science
- *			LAN entries
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>
- *			Changes for new I2O API
- */
-
-#define OSM_NAME	"proc-osm"
-#define OSM_VERSION	"1.316"
-#define OSM_DESCRIPTION	"I2O ProcFS OSM"
-
-#define I2O_MAX_MODULES 4
-// FIXME!
-#define FMT_U64_HEX "0x%08x%08x"
-#define U64_VAL(pu64) *((u32*)(pu64)+1), *((u32*)(pu64))
-
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/pci.h>
-#include "i2o.h"
-#include <linux/slab.h>
-#include <linux/proc_fs.h>
-#include <linux/seq_file.h>
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/errno.h>
-#include <linux/spinlock.h>
-#include <linux/workqueue.h>
-#include <linux/uaccess.h>
-
-#include <asm/io.h>
-#include <asm/byteorder.h>
-
-/* Structure used to define /proc entries */
-typedef struct _i2o_proc_entry_t {
-	char *name;		/* entry name */
-	umode_t mode;		/* mode */
-	const struct file_operations *fops;	/* open function */
-} i2o_proc_entry;
-
-/* global I2O /proc/i2o entry */
-static struct proc_dir_entry *i2o_proc_dir_root;
-
-/* proc OSM driver struct */
-static struct i2o_driver i2o_proc_driver = {
-	.name = OSM_NAME,
-};
-
-static int print_serial_number(struct seq_file *seq, u8 * serialno, int max_len)
-{
-	int i;
-
-	/* 19990419 -sralston
-	 *      The I2O v1.5 (and v2.0 so far) "official specification"
-	 *      got serial numbers WRONG!
-	 *      Apparently, and despite what Section 3.4.4 says and
-	 *      Figure 3-35 shows (pg 3-39 in the pdf doc),
-	 *      the convention / consensus seems to be:
-	 *        + First byte is SNFormat
-	 *        + Second byte is SNLen (but only if SNFormat==7 (?))
-	 *        + (v2.0) SCSI+BS may use IEEE Registered (64 or 128 bit) format
-	 */
-	switch (serialno[0]) {
-	case I2O_SNFORMAT_BINARY:	/* Binary */
-		seq_printf(seq, "0x");
-		for (i = 0; i < serialno[1]; i++)
-			seq_printf(seq, "%02X", serialno[2 + i]);
-		break;
-
-	case I2O_SNFORMAT_ASCII:	/* ASCII */
-		if (serialno[1] < ' ') {	/* printable or SNLen? */
-			/* sanity */
-			max_len =
-			    (max_len < serialno[1]) ? max_len : serialno[1];
-			serialno[1 + max_len] = '\0';
-
-			/* just print it */
-			seq_printf(seq, "%s", &serialno[2]);
-		} else {
-			/* print chars for specified length */
-			for (i = 0; i < serialno[1]; i++)
-				seq_printf(seq, "%c", serialno[2 + i]);
-		}
-		break;
-
-	case I2O_SNFORMAT_UNICODE:	/* UNICODE */
-		seq_printf(seq, "UNICODE Format.  Can't Display\n");
-		break;
-
-	case I2O_SNFORMAT_LAN48_MAC:	/* LAN-48 MAC Address */
-		seq_printf(seq, "LAN-48 MAC address @ %pM", &serialno[2]);
-		break;
-
-	case I2O_SNFORMAT_WAN:	/* WAN MAC Address */
-		/* FIXME: Figure out what a WAN access address looks like?? */
-		seq_printf(seq, "WAN Access Address");
-		break;
-
-/* plus new in v2.0 */
-	case I2O_SNFORMAT_LAN64_MAC:	/* LAN-64 MAC Address */
-		/* FIXME: Figure out what a LAN-64 address really looks like?? */
-		seq_printf(seq,
-			   "LAN-64 MAC address @ [?:%02X:%02X:?] %pM",
-			   serialno[8], serialno[9], &serialno[2]);
-		break;
-
-	case I2O_SNFORMAT_DDM:	/* I2O DDM */
-		seq_printf(seq,
-			   "DDM: Tid=%03Xh, Rsvd=%04Xh, OrgId=%04Xh",
-			   *(u16 *) & serialno[2],
-			   *(u16 *) & serialno[4], *(u16 *) & serialno[6]);
-		break;
-
-	case I2O_SNFORMAT_IEEE_REG64:	/* IEEE Registered (64-bit) */
-	case I2O_SNFORMAT_IEEE_REG128:	/* IEEE Registered (128-bit) */
-		/* FIXME: Figure if this is even close?? */
-		seq_printf(seq,
-			   "IEEE NodeName(hi,lo)=(%08Xh:%08Xh), PortName(hi,lo)=(%08Xh:%08Xh)\n",
-			   *(u32 *) & serialno[2],
-			   *(u32 *) & serialno[6],
-			   *(u32 *) & serialno[10], *(u32 *) & serialno[14]);
-		break;
-
-	case I2O_SNFORMAT_UNKNOWN:	/* Unknown 0    */
-	case I2O_SNFORMAT_UNKNOWN2:	/* Unknown 0xff */
-	default:
-		seq_printf(seq, "Unknown data format (0x%02x)", serialno[0]);
-		break;
-	}
-
-	return 0;
-}
-
-/**
- *	i2o_get_class_name - 	do i2o class name lookup
- *	@class: class number
- *
- *	Return a descriptive string for an i2o class.
- */
-static const char *i2o_get_class_name(int class)
-{
-	int idx = 16;
-	static char *i2o_class_name[] = {
-		"Executive",
-		"Device Driver Module",
-		"Block Device",
-		"Tape Device",
-		"LAN Interface",
-		"WAN Interface",
-		"Fibre Channel Port",
-		"Fibre Channel Device",
-		"SCSI Device",
-		"ATE Port",
-		"ATE Device",
-		"Floppy Controller",
-		"Floppy Device",
-		"Secondary Bus Port",
-		"Peer Transport Agent",
-		"Peer Transport",
-		"Unknown"
-	};
-
-	switch (class & 0xfff) {
-	case I2O_CLASS_EXECUTIVE:
-		idx = 0;
-		break;
-	case I2O_CLASS_DDM:
-		idx = 1;
-		break;
-	case I2O_CLASS_RANDOM_BLOCK_STORAGE:
-		idx = 2;
-		break;
-	case I2O_CLASS_SEQUENTIAL_STORAGE:
-		idx = 3;
-		break;
-	case I2O_CLASS_LAN:
-		idx = 4;
-		break;
-	case I2O_CLASS_WAN:
-		idx = 5;
-		break;
-	case I2O_CLASS_FIBRE_CHANNEL_PORT:
-		idx = 6;
-		break;
-	case I2O_CLASS_FIBRE_CHANNEL_PERIPHERAL:
-		idx = 7;
-		break;
-	case I2O_CLASS_SCSI_PERIPHERAL:
-		idx = 8;
-		break;
-	case I2O_CLASS_ATE_PORT:
-		idx = 9;
-		break;
-	case I2O_CLASS_ATE_PERIPHERAL:
-		idx = 10;
-		break;
-	case I2O_CLASS_FLOPPY_CONTROLLER:
-		idx = 11;
-		break;
-	case I2O_CLASS_FLOPPY_DEVICE:
-		idx = 12;
-		break;
-	case I2O_CLASS_BUS_ADAPTER:
-		idx = 13;
-		break;
-	case I2O_CLASS_PEER_TRANSPORT_AGENT:
-		idx = 14;
-		break;
-	case I2O_CLASS_PEER_TRANSPORT:
-		idx = 15;
-		break;
-	}
-
-	return i2o_class_name[idx];
-}
-
-#define SCSI_TABLE_SIZE	13
-static char *scsi_devices[] = {
-	"Direct-Access Read/Write",
-	"Sequential-Access Storage",
-	"Printer",
-	"Processor",
-	"WORM Device",
-	"CD-ROM Device",
-	"Scanner Device",
-	"Optical Memory Device",
-	"Medium Changer Device",
-	"Communications Device",
-	"Graphics Art Pre-Press Device",
-	"Graphics Art Pre-Press Device",
-	"Array Controller Device"
-};
-
-static char *chtostr(char *tmp, u8 *chars, int n)
-{
-	tmp[0] = 0;
-	return strncat(tmp, (char *)chars, n);
-}
-
-static int i2o_report_query_status(struct seq_file *seq, int block_status,
-				   char *group)
-{
-	switch (block_status) {
-	case -ETIMEDOUT:
-		seq_printf(seq, "Timeout reading group %s.\n", group);
-		break;
-	case -ENOMEM:
-		seq_puts(seq, "No free memory to read the table.\n");
-		break;
-	case -I2O_PARAMS_STATUS_INVALID_GROUP_ID:
-		seq_printf(seq, "Group %s not supported.\n", group);
-		break;
-	default:
-		seq_printf(seq,
-			   "Error reading group %s. BlockStatus 0x%02X\n",
-			   group, -block_status);
-		break;
-	}
-
-	return 0;
-}
-
-static char *bus_strings[] = {
-	"Local Bus",
-	"ISA",
-	"EISA",
-	"PCI",
-	"PCMCIA",
-	"NUBUS",
-	"CARDBUS"
-};
-
-static int i2o_seq_show_hrt(struct seq_file *seq, void *v)
-{
-	struct i2o_controller *c = (struct i2o_controller *)seq->private;
-	i2o_hrt *hrt = (i2o_hrt *) c->hrt.virt;
-	u32 bus;
-	int i;
-
-	if (hrt->hrt_version) {
-		seq_printf(seq,
-			   "HRT table for controller is too new a version.\n");
-		return 0;
-	}
-
-	seq_printf(seq, "HRT has %d entries of %d bytes each.\n",
-		   hrt->num_entries, hrt->entry_len << 2);
-
-	for (i = 0; i < hrt->num_entries; i++) {
-		seq_printf(seq, "Entry %d:\n", i);
-		seq_printf(seq, "   Adapter ID: %0#10x\n",
-			   hrt->hrt_entry[i].adapter_id);
-		seq_printf(seq, "   Controlling tid: %0#6x\n",
-			   hrt->hrt_entry[i].parent_tid);
-
-		if (hrt->hrt_entry[i].bus_type != 0x80) {
-			bus = hrt->hrt_entry[i].bus_type;
-			seq_printf(seq, "   %s Information\n",
-				   bus_strings[bus]);
-
-			switch (bus) {
-			case I2O_BUS_LOCAL:
-				seq_printf(seq, "     IOBase: %0#6x,",
-					   hrt->hrt_entry[i].bus.local_bus.
-					   LbBaseIOPort);
-				seq_printf(seq, " MemoryBase: %0#10x\n",
-					   hrt->hrt_entry[i].bus.local_bus.
-					   LbBaseMemoryAddress);
-				break;
-
-			case I2O_BUS_ISA:
-				seq_printf(seq, "     IOBase: %0#6x,",
-					   hrt->hrt_entry[i].bus.isa_bus.
-					   IsaBaseIOPort);
-				seq_printf(seq, " MemoryBase: %0#10x,",
-					   hrt->hrt_entry[i].bus.isa_bus.
-					   IsaBaseMemoryAddress);
-				seq_printf(seq, " CSN: %0#4x,",
-					   hrt->hrt_entry[i].bus.isa_bus.CSN);
-				break;
-
-			case I2O_BUS_EISA:
-				seq_printf(seq, "     IOBase: %0#6x,",
-					   hrt->hrt_entry[i].bus.eisa_bus.
-					   EisaBaseIOPort);
-				seq_printf(seq, " MemoryBase: %0#10x,",
-					   hrt->hrt_entry[i].bus.eisa_bus.
-					   EisaBaseMemoryAddress);
-				seq_printf(seq, " Slot: %0#4x,",
-					   hrt->hrt_entry[i].bus.eisa_bus.
-					   EisaSlotNumber);
-				break;
-
-			case I2O_BUS_PCI:
-				seq_printf(seq, "     Bus: %0#4x",
-					   hrt->hrt_entry[i].bus.pci_bus.
-					   PciBusNumber);
-				seq_printf(seq, " Dev: %0#4x",
-					   hrt->hrt_entry[i].bus.pci_bus.
-					   PciDeviceNumber);
-				seq_printf(seq, " Func: %0#4x",
-					   hrt->hrt_entry[i].bus.pci_bus.
-					   PciFunctionNumber);
-				seq_printf(seq, " Vendor: %0#6x",
-					   hrt->hrt_entry[i].bus.pci_bus.
-					   PciVendorID);
-				seq_printf(seq, " Device: %0#6x\n",
-					   hrt->hrt_entry[i].bus.pci_bus.
-					   PciDeviceID);
-				break;
-
-			default:
-				seq_printf(seq, "      Unsupported Bus Type\n");
-			}
-		} else
-			seq_printf(seq, "   Unknown Bus Type\n");
-	}
-
-	return 0;
-}
-
-static int i2o_seq_show_lct(struct seq_file *seq, void *v)
-{
-	struct i2o_controller *c = (struct i2o_controller *)seq->private;
-	i2o_lct *lct = (i2o_lct *) c->lct;
-	int entries;
-	int i;
-
-#define BUS_TABLE_SIZE 3
-	static char *bus_ports[] = {
-		"Generic Bus",
-		"SCSI Bus",
-		"Fibre Channel Bus"
-	};
-
-	entries = (lct->table_size - 3) / 9;
-
-	seq_printf(seq, "LCT contains %d %s\n", entries,
-		   entries == 1 ? "entry" : "entries");
-	if (lct->boot_tid)
-		seq_printf(seq, "Boot Device @ ID %d\n", lct->boot_tid);
-
-	seq_printf(seq, "Current Change Indicator: %#10x\n", lct->change_ind);
-
-	for (i = 0; i < entries; i++) {
-		seq_printf(seq, "Entry %d\n", i);
-		seq_printf(seq, "  Class, SubClass  : %s",
-			   i2o_get_class_name(lct->lct_entry[i].class_id));
-
-		/*
-		 *      Classes which we'll print subclass info for
-		 */
-		switch (lct->lct_entry[i].class_id & 0xFFF) {
-		case I2O_CLASS_RANDOM_BLOCK_STORAGE:
-			switch (lct->lct_entry[i].sub_class) {
-			case 0x00:
-				seq_printf(seq, ", Direct-Access Read/Write");
-				break;
-
-			case 0x04:
-				seq_printf(seq, ", WORM Drive");
-				break;
-
-			case 0x05:
-				seq_printf(seq, ", CD-ROM Drive");
-				break;
-
-			case 0x07:
-				seq_printf(seq, ", Optical Memory Device");
-				break;
-
-			default:
-				seq_printf(seq, ", Unknown (0x%02x)",
-					   lct->lct_entry[i].sub_class);
-				break;
-			}
-			break;
-
-		case I2O_CLASS_LAN:
-			switch (lct->lct_entry[i].sub_class & 0xFF) {
-			case 0x30:
-				seq_printf(seq, ", Ethernet");
-				break;
-
-			case 0x40:
-				seq_printf(seq, ", 100base VG");
-				break;
-
-			case 0x50:
-				seq_printf(seq, ", IEEE 802.5/Token-Ring");
-				break;
-
-			case 0x60:
-				seq_printf(seq, ", ANSI X3T9.5 FDDI");
-				break;
-
-			case 0x70:
-				seq_printf(seq, ", Fibre Channel");
-				break;
-
-			default:
-				seq_printf(seq, ", Unknown Sub-Class (0x%02x)",
-					   lct->lct_entry[i].sub_class & 0xFF);
-				break;
-			}
-			break;
-
-		case I2O_CLASS_SCSI_PERIPHERAL:
-			if (lct->lct_entry[i].sub_class < SCSI_TABLE_SIZE)
-				seq_printf(seq, ", %s",
-					   scsi_devices[lct->lct_entry[i].
-							sub_class]);
-			else
-				seq_printf(seq, ", Unknown Device Type");
-			break;
-
-		case I2O_CLASS_BUS_ADAPTER:
-			if (lct->lct_entry[i].sub_class < BUS_TABLE_SIZE)
-				seq_printf(seq, ", %s",
-					   bus_ports[lct->lct_entry[i].
-						     sub_class]);
-			else
-				seq_printf(seq, ", Unknown Bus Type");
-			break;
-		}
-		seq_printf(seq, "\n");
-
-		seq_printf(seq, "  Local TID        : 0x%03x\n",
-			   lct->lct_entry[i].tid);
-		seq_printf(seq, "  User TID         : 0x%03x\n",
-			   lct->lct_entry[i].user_tid);
-		seq_printf(seq, "  Parent TID       : 0x%03x\n",
-			   lct->lct_entry[i].parent_tid);
-		seq_printf(seq, "  Identity Tag     : 0x%x%x%x%x%x%x%x%x\n",
-			   lct->lct_entry[i].identity_tag[0],
-			   lct->lct_entry[i].identity_tag[1],
-			   lct->lct_entry[i].identity_tag[2],
-			   lct->lct_entry[i].identity_tag[3],
-			   lct->lct_entry[i].identity_tag[4],
-			   lct->lct_entry[i].identity_tag[5],
-			   lct->lct_entry[i].identity_tag[6],
-			   lct->lct_entry[i].identity_tag[7]);
-		seq_printf(seq, "  Change Indicator : %0#10x\n",
-			   lct->lct_entry[i].change_ind);
-		seq_printf(seq, "  Event Capab Mask : %0#10x\n",
-			   lct->lct_entry[i].device_flags);
-	}
-
-	return 0;
-}
-
-static int i2o_seq_show_status(struct seq_file *seq, void *v)
-{
-	struct i2o_controller *c = (struct i2o_controller *)seq->private;
-	char prodstr[25];
-	int version;
-	i2o_status_block *sb = c->status_block.virt;
-
-	i2o_status_get(c);	// reread the status block
-
-	seq_printf(seq, "Organization ID        : %0#6x\n", sb->org_id);
-
-	version = sb->i2o_version;
-
-/* FIXME for Spec 2.0
-	if (version == 0x02) {
-		seq_printf(seq, "Lowest I2O version supported: ");
-		switch(workspace[2]) {
-			case 0x00:
-				seq_printf(seq, "1.0\n");
-				break;
-			case 0x01:
-				seq_printf(seq, "1.5\n");
-				break;
-			case 0x02:
-				seq_printf(seq, "2.0\n");
-				break;
-		}
-
-		seq_printf(seq, "Highest I2O version supported: ");
-		switch(workspace[3]) {
-			case 0x00:
-				seq_printf(seq, "1.0\n");
-				break;
-			case 0x01:
-				seq_printf(seq, "1.5\n");
-				break;
-			case 0x02:
-				seq_printf(seq, "2.0\n");
-				break;
-		}
-	}
-*/
-	seq_printf(seq, "IOP ID                 : %0#5x\n", sb->iop_id);
-	seq_printf(seq, "Host Unit ID           : %0#6x\n", sb->host_unit_id);
-	seq_printf(seq, "Segment Number         : %0#5x\n", sb->segment_number);
-
-	seq_printf(seq, "I2O version            : ");
-	switch (version) {
-	case 0x00:
-		seq_printf(seq, "1.0\n");
-		break;
-	case 0x01:
-		seq_printf(seq, "1.5\n");
-		break;
-	case 0x02:
-		seq_printf(seq, "2.0\n");
-		break;
-	default:
-		seq_printf(seq, "Unknown version\n");
-	}
-
-	seq_printf(seq, "IOP State              : ");
-	switch (sb->iop_state) {
-	case 0x01:
-		seq_printf(seq, "INIT\n");
-		break;
-
-	case 0x02:
-		seq_printf(seq, "RESET\n");
-		break;
-
-	case 0x04:
-		seq_printf(seq, "HOLD\n");
-		break;
-
-	case 0x05:
-		seq_printf(seq, "READY\n");
-		break;
-
-	case 0x08:
-		seq_printf(seq, "OPERATIONAL\n");
-		break;
-
-	case 0x10:
-		seq_printf(seq, "FAILED\n");
-		break;
-
-	case 0x11:
-		seq_printf(seq, "FAULTED\n");
-		break;
-
-	default:
-		seq_printf(seq, "Unknown\n");
-		break;
-	}
-
-	seq_printf(seq, "Messenger Type         : ");
-	switch (sb->msg_type) {
-	case 0x00:
-		seq_printf(seq, "Memory mapped\n");
-		break;
-	case 0x01:
-		seq_printf(seq, "Memory mapped only\n");
-		break;
-	case 0x02:
-		seq_printf(seq, "Remote only\n");
-		break;
-	case 0x03:
-		seq_printf(seq, "Memory mapped and remote\n");
-		break;
-	default:
-		seq_printf(seq, "Unknown\n");
-	}
-
-	seq_printf(seq, "Inbound Frame Size     : %d bytes\n",
-		   sb->inbound_frame_size << 2);
-	seq_printf(seq, "Max Inbound Frames     : %d\n",
-		   sb->max_inbound_frames);
-	seq_printf(seq, "Current Inbound Frames : %d\n",
-		   sb->cur_inbound_frames);
-	seq_printf(seq, "Max Outbound Frames    : %d\n",
-		   sb->max_outbound_frames);
-
-	/* Spec doesn't say if NULL terminated or not... */
-	memcpy(prodstr, sb->product_id, 24);
-	prodstr[24] = '\0';
-	seq_printf(seq, "Product ID             : %s\n", prodstr);
-	seq_printf(seq, "Expected LCT Size      : %d bytes\n",
-		   sb->expected_lct_size);
-
-	seq_printf(seq, "IOP Capabilities\n");
-	seq_printf(seq, "    Context Field Size Support : ");
-	switch (sb->iop_capabilities & 0x0000003) {
-	case 0:
-		seq_printf(seq, "Supports only 32-bit context fields\n");
-		break;
-	case 1:
-		seq_printf(seq, "Supports only 64-bit context fields\n");
-		break;
-	case 2:
-		seq_printf(seq, "Supports 32-bit and 64-bit context fields, "
-			   "but not concurrently\n");
-		break;
-	case 3:
-		seq_printf(seq, "Supports 32-bit and 64-bit context fields "
-			   "concurrently\n");
-		break;
-	default:
-		seq_printf(seq, "0x%08x\n", sb->iop_capabilities);
-	}
-	seq_printf(seq, "    Current Context Field Size : ");
-	switch (sb->iop_capabilities & 0x0000000C) {
-	case 0:
-		seq_printf(seq, "not configured\n");
-		break;
-	case 4:
-		seq_printf(seq, "Supports only 32-bit context fields\n");
-		break;
-	case 8:
-		seq_printf(seq, "Supports only 64-bit context fields\n");
-		break;
-	case 12:
-		seq_printf(seq, "Supports both 32-bit or 64-bit context fields "
-			   "concurrently\n");
-		break;
-	default:
-		seq_printf(seq, "\n");
-	}
-	seq_printf(seq, "    Inbound Peer Support       : %s\n",
-		   (sb->
-		    iop_capabilities & 0x00000010) ? "Supported" :
-		   "Not supported");
-	seq_printf(seq, "    Outbound Peer Support      : %s\n",
-		   (sb->
-		    iop_capabilities & 0x00000020) ? "Supported" :
-		   "Not supported");
-	seq_printf(seq, "    Peer to Peer Support       : %s\n",
-		   (sb->
-		    iop_capabilities & 0x00000040) ? "Supported" :
-		   "Not supported");
-
-	seq_printf(seq, "Desired private memory size   : %d kB\n",
-		   sb->desired_mem_size >> 10);
-	seq_printf(seq, "Allocated private memory size : %d kB\n",
-		   sb->current_mem_size >> 10);
-	seq_printf(seq, "Private memory base address   : %0#10x\n",
-		   sb->current_mem_base);
-	seq_printf(seq, "Desired private I/O size      : %d kB\n",
-		   sb->desired_io_size >> 10);
-	seq_printf(seq, "Allocated private I/O size    : %d kB\n",
-		   sb->current_io_size >> 10);
-	seq_printf(seq, "Private I/O base address      : %0#10x\n",
-		   sb->current_io_base);
-
-	return 0;
-}
-
-static int i2o_seq_show_hw(struct seq_file *seq, void *v)
-{
-	struct i2o_controller *c = (struct i2o_controller *)seq->private;
-	static u32 work32[5];
-	static u8 *work8 = (u8 *) work32;
-	static u16 *work16 = (u16 *) work32;
-	int token;
-	u32 hwcap;
-
-	static char *cpu_table[] = {
-		"Intel 80960 series",
-		"AMD2900 series",
-		"Motorola 68000 series",
-		"ARM series",
-		"MIPS series",
-		"Sparc series",
-		"PowerPC series",
-		"Intel x86 series"
-	};
-
-	token =
-	    i2o_parm_field_get(c->exec, 0x0000, -1, &work32, sizeof(work32));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token, "0x0000 IOP Hardware");
-		return 0;
-	}
-
-	seq_printf(seq, "I2O Vendor ID    : %0#6x\n", work16[0]);
-	seq_printf(seq, "Product ID       : %0#6x\n", work16[1]);
-	seq_printf(seq, "CPU              : ");
-	if (work8[16] > 8)
-		seq_printf(seq, "Unknown\n");
-	else
-		seq_printf(seq, "%s\n", cpu_table[work8[16]]);
-	/* Anyone using ProcessorVersion? */
-
-	seq_printf(seq, "RAM              : %dkB\n", work32[1] >> 10);
-	seq_printf(seq, "Non-Volatile Mem : %dkB\n", work32[2] >> 10);
-
-	hwcap = work32[3];
-	seq_printf(seq, "Capabilities : 0x%08x\n", hwcap);
-	seq_printf(seq, "   [%s] Self booting\n",
-		   (hwcap & 0x00000001) ? "+" : "-");
-	seq_printf(seq, "   [%s] Upgradable IRTOS\n",
-		   (hwcap & 0x00000002) ? "+" : "-");
-	seq_printf(seq, "   [%s] Supports downloading DDMs\n",
-		   (hwcap & 0x00000004) ? "+" : "-");
-	seq_printf(seq, "   [%s] Supports installing DDMs\n",
-		   (hwcap & 0x00000008) ? "+" : "-");
-	seq_printf(seq, "   [%s] Battery-backed RAM\n",
-		   (hwcap & 0x00000010) ? "+" : "-");
-
-	return 0;
-}
-
-/* Executive group 0003h - Executing DDM List (table) */
-static int i2o_seq_show_ddm_table(struct seq_file *seq, void *v)
-{
-	struct i2o_controller *c = (struct i2o_controller *)seq->private;
-	int token;
-	int i;
-
-	typedef struct _i2o_exec_execute_ddm_table {
-		u16 ddm_tid;
-		u8 module_type;
-		u8 reserved;
-		u16 i2o_vendor_id;
-		u16 module_id;
-		u8 module_name_version[28];
-		u32 data_size;
-		u32 code_size;
-	} i2o_exec_execute_ddm_table;
-
-	struct {
-		u16 result_count;
-		u16 pad;
-		u16 block_size;
-		u8 block_status;
-		u8 error_info_size;
-		u16 row_count;
-		u16 more_flag;
-		i2o_exec_execute_ddm_table ddm_table[I2O_MAX_MODULES];
-	} *result;
-
-	i2o_exec_execute_ddm_table ddm_table;
-	char tmp[28 + 1];
-
-	result = kmalloc(sizeof(*result), GFP_KERNEL);
-	if (!result)
-		return -ENOMEM;
-
-	token = i2o_parm_table_get(c->exec, I2O_PARAMS_TABLE_GET, 0x0003, -1,
-				   NULL, 0, result, sizeof(*result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token,
-					"0x0003 Executing DDM List");
-		goto out;
-	}
-
-	seq_printf(seq,
-		   "Tid   Module_type     Vendor Mod_id  Module_name             Vrs  Data_size Code_size\n");
-	ddm_table = result->ddm_table[0];
-
-	for (i = 0; i < result->row_count; ddm_table = result->ddm_table[++i]) {
-		seq_printf(seq, "0x%03x ", ddm_table.ddm_tid & 0xFFF);
-
-		switch (ddm_table.module_type) {
-		case 0x01:
-			seq_printf(seq, "Downloaded DDM  ");
-			break;
-		case 0x22:
-			seq_printf(seq, "Embedded DDM    ");
-			break;
-		default:
-			seq_printf(seq, "                ");
-		}
-
-		seq_printf(seq, "%-#7x", ddm_table.i2o_vendor_id);
-		seq_printf(seq, "%-#8x", ddm_table.module_id);
-		seq_printf(seq, "%-29s",
-			   chtostr(tmp, ddm_table.module_name_version, 28));
-		seq_printf(seq, "%9d  ", ddm_table.data_size);
-		seq_printf(seq, "%8d", ddm_table.code_size);
-
-		seq_printf(seq, "\n");
-	}
-      out:
-	kfree(result);
-	return 0;
-}
-
-/* Executive group 0004h - Driver Store (scalar) */
-static int i2o_seq_show_driver_store(struct seq_file *seq, void *v)
-{
-	struct i2o_controller *c = (struct i2o_controller *)seq->private;
-	u32 work32[8];
-	int token;
-
-	token =
-	    i2o_parm_field_get(c->exec, 0x0004, -1, &work32, sizeof(work32));
-	if (token < 0) {
-		i2o_report_query_status(seq, token, "0x0004 Driver Store");
-		return 0;
-	}
-
-	seq_printf(seq, "Module limit  : %d\n"
-		   "Module count  : %d\n"
-		   "Current space : %d kB\n"
-		   "Free space    : %d kB\n",
-		   work32[0], work32[1], work32[2] >> 10, work32[3] >> 10);
-
-	return 0;
-}
-
-/* Executive group 0005h - Driver Store Table (table) */
-static int i2o_seq_show_drivers_stored(struct seq_file *seq, void *v)
-{
-	typedef struct _i2o_driver_store {
-		u16 stored_ddm_index;
-		u8 module_type;
-		u8 reserved;
-		u16 i2o_vendor_id;
-		u16 module_id;
-		u8 module_name_version[28];
-		u8 date[8];
-		u32 module_size;
-		u32 mpb_size;
-		u32 module_flags;
-	} i2o_driver_store_table;
-
-	struct i2o_controller *c = (struct i2o_controller *)seq->private;
-	int token;
-	int i;
-
-	typedef struct {
-		u16 result_count;
-		u16 pad;
-		u16 block_size;
-		u8 block_status;
-		u8 error_info_size;
-		u16 row_count;
-		u16 more_flag;
-		i2o_driver_store_table dst[I2O_MAX_MODULES];
-	} i2o_driver_result_table;
-
-	i2o_driver_result_table *result;
-	i2o_driver_store_table *dst;
-	char tmp[28 + 1];
-
-	result = kmalloc(sizeof(i2o_driver_result_table), GFP_KERNEL);
-	if (result == NULL)
-		return -ENOMEM;
-
-	token = i2o_parm_table_get(c->exec, I2O_PARAMS_TABLE_GET, 0x0005, -1,
-				   NULL, 0, result, sizeof(*result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token,
-					"0x0005 DRIVER STORE TABLE");
-		kfree(result);
-		return 0;
-	}
-
-	seq_printf(seq,
-		   "#  Module_type     Vendor Mod_id  Module_name             Vrs"
-		   "Date     Mod_size Par_size Flags\n");
-	for (i = 0, dst = &result->dst[0]; i < result->row_count;
-	     dst = &result->dst[++i]) {
-		seq_printf(seq, "%-3d", dst->stored_ddm_index);
-		switch (dst->module_type) {
-		case 0x01:
-			seq_printf(seq, "Downloaded DDM  ");
-			break;
-		case 0x22:
-			seq_printf(seq, "Embedded DDM    ");
-			break;
-		default:
-			seq_printf(seq, "                ");
-		}
-
-		seq_printf(seq, "%-#7x", dst->i2o_vendor_id);
-		seq_printf(seq, "%-#8x", dst->module_id);
-		seq_printf(seq, "%-29s",
-			   chtostr(tmp, dst->module_name_version, 28));
-		seq_printf(seq, "%-9s", chtostr(tmp, dst->date, 8));
-		seq_printf(seq, "%8d ", dst->module_size);
-		seq_printf(seq, "%8d ", dst->mpb_size);
-		seq_printf(seq, "0x%04x", dst->module_flags);
-		seq_printf(seq, "\n");
-	}
-
-	kfree(result);
-	return 0;
-}
-
-/* Generic group F000h - Params Descriptor (table) */
-static int i2o_seq_show_groups(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-	int i;
-	u8 properties;
-
-	typedef struct _i2o_group_info {
-		u16 group_number;
-		u16 field_count;
-		u16 row_count;
-		u8 properties;
-		u8 reserved;
-	} i2o_group_info;
-
-	struct {
-		u16 result_count;
-		u16 pad;
-		u16 block_size;
-		u8 block_status;
-		u8 error_info_size;
-		u16 row_count;
-		u16 more_flag;
-		i2o_group_info group[256];
-	} *result;
-
-	result = kmalloc(sizeof(*result), GFP_KERNEL);
-	if (!result)
-		return -ENOMEM;
-
-	token = i2o_parm_table_get(d, I2O_PARAMS_TABLE_GET, 0xF000, -1, NULL, 0,
-				   result, sizeof(*result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token, "0xF000 Params Descriptor");
-		goto out;
-	}
-
-	seq_printf(seq,
-		   "#  Group   FieldCount RowCount Type   Add Del Clear\n");
-
-	for (i = 0; i < result->row_count; i++) {
-		seq_printf(seq, "%-3d", i);
-		seq_printf(seq, "0x%04X ", result->group[i].group_number);
-		seq_printf(seq, "%10d ", result->group[i].field_count);
-		seq_printf(seq, "%8d ", result->group[i].row_count);
-
-		properties = result->group[i].properties;
-		if (properties & 0x1)
-			seq_printf(seq, "Table  ");
-		else
-			seq_printf(seq, "Scalar ");
-		if (properties & 0x2)
-			seq_printf(seq, " + ");
-		else
-			seq_printf(seq, " - ");
-		if (properties & 0x4)
-			seq_printf(seq, "  + ");
-		else
-			seq_printf(seq, "  - ");
-		if (properties & 0x8)
-			seq_printf(seq, "  + ");
-		else
-			seq_printf(seq, "  - ");
-
-		seq_printf(seq, "\n");
-	}
-
-	if (result->more_flag)
-		seq_printf(seq, "There is more...\n");
-      out:
-	kfree(result);
-	return 0;
-}
-
-/* Generic group F001h - Physical Device Table (table) */
-static int i2o_seq_show_phys_device(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-	int i;
-
-	struct {
-		u16 result_count;
-		u16 pad;
-		u16 block_size;
-		u8 block_status;
-		u8 error_info_size;
-		u16 row_count;
-		u16 more_flag;
-		u32 adapter_id[64];
-	} result;
-
-	token = i2o_parm_table_get(d, I2O_PARAMS_TABLE_GET, 0xF001, -1, NULL, 0,
-				   &result, sizeof(result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token,
-					"0xF001 Physical Device Table");
-		return 0;
-	}
-
-	if (result.row_count)
-		seq_printf(seq, "#  AdapterId\n");
-
-	for (i = 0; i < result.row_count; i++) {
-		seq_printf(seq, "%-2d", i);
-		seq_printf(seq, "%#7x\n", result.adapter_id[i]);
-	}
-
-	if (result.more_flag)
-		seq_printf(seq, "There is more...\n");
-
-	return 0;
-}
-
-/* Generic group F002h - Claimed Table (table) */
-static int i2o_seq_show_claimed(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-	int i;
-
-	struct {
-		u16 result_count;
-		u16 pad;
-		u16 block_size;
-		u8 block_status;
-		u8 error_info_size;
-		u16 row_count;
-		u16 more_flag;
-		u16 claimed_tid[64];
-	} result;
-
-	token = i2o_parm_table_get(d, I2O_PARAMS_TABLE_GET, 0xF002, -1, NULL, 0,
-				   &result, sizeof(result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token, "0xF002 Claimed Table");
-		return 0;
-	}
-
-	if (result.row_count)
-		seq_printf(seq, "#  ClaimedTid\n");
-
-	for (i = 0; i < result.row_count; i++) {
-		seq_printf(seq, "%-2d", i);
-		seq_printf(seq, "%#7x\n", result.claimed_tid[i]);
-	}
-
-	if (result.more_flag)
-		seq_printf(seq, "There is more...\n");
-
-	return 0;
-}
-
-/* Generic group F003h - User Table (table) */
-static int i2o_seq_show_users(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-	int i;
-
-	typedef struct _i2o_user_table {
-		u16 instance;
-		u16 user_tid;
-		u8 claim_type;
-		u8 reserved1;
-		u16 reserved2;
-	} i2o_user_table;
-
-	struct {
-		u16 result_count;
-		u16 pad;
-		u16 block_size;
-		u8 block_status;
-		u8 error_info_size;
-		u16 row_count;
-		u16 more_flag;
-		i2o_user_table user[64];
-	} *result;
-
-	result = kmalloc(sizeof(*result), GFP_KERNEL);
-	if (!result)
-		return -ENOMEM;
-
-	token = i2o_parm_table_get(d, I2O_PARAMS_TABLE_GET, 0xF003, -1, NULL, 0,
-				   result, sizeof(*result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token, "0xF003 User Table");
-		goto out;
-	}
-
-	seq_printf(seq, "#  Instance UserTid ClaimType\n");
-
-	for (i = 0; i < result->row_count; i++) {
-		seq_printf(seq, "%-3d", i);
-		seq_printf(seq, "%#8x ", result->user[i].instance);
-		seq_printf(seq, "%#7x ", result->user[i].user_tid);
-		seq_printf(seq, "%#9x\n", result->user[i].claim_type);
-	}
-
-	if (result->more_flag)
-		seq_printf(seq, "There is more...\n");
-      out:
-	kfree(result);
-	return 0;
-}
-
-/* Generic group F005h - Private message extensions (table) (optional) */
-static int i2o_seq_show_priv_msgs(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-	int i;
-
-	typedef struct _i2o_private {
-		u16 ext_instance;
-		u16 organization_id;
-		u16 x_function_code;
-	} i2o_private;
-
-	struct {
-		u16 result_count;
-		u16 pad;
-		u16 block_size;
-		u8 block_status;
-		u8 error_info_size;
-		u16 row_count;
-		u16 more_flag;
-		i2o_private extension[64];
-	} result;
-
-	token = i2o_parm_table_get(d, I2O_PARAMS_TABLE_GET, 0xF000, -1, NULL, 0,
-				   &result, sizeof(result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token,
-					"0xF005 Private Message Extensions (optional)");
-		return 0;
-	}
-
-	seq_printf(seq, "Instance#  OrgId  FunctionCode\n");
-
-	for (i = 0; i < result.row_count; i++) {
-		seq_printf(seq, "%0#9x ", result.extension[i].ext_instance);
-		seq_printf(seq, "%0#6x ", result.extension[i].organization_id);
-		seq_printf(seq, "%0#6x", result.extension[i].x_function_code);
-
-		seq_printf(seq, "\n");
-	}
-
-	if (result.more_flag)
-		seq_printf(seq, "There is more...\n");
-
-	return 0;
-}
-
-/* Generic group F006h - Authorized User Table (table) */
-static int i2o_seq_show_authorized_users(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-	int i;
-
-	struct {
-		u16 result_count;
-		u16 pad;
-		u16 block_size;
-		u8 block_status;
-		u8 error_info_size;
-		u16 row_count;
-		u16 more_flag;
-		u32 alternate_tid[64];
-	} result;
-
-	token = i2o_parm_table_get(d, I2O_PARAMS_TABLE_GET, 0xF006, -1, NULL, 0,
-				   &result, sizeof(result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token,
-					"0xF006 Autohorized User Table");
-		return 0;
-	}
-
-	if (result.row_count)
-		seq_printf(seq, "#  AlternateTid\n");
-
-	for (i = 0; i < result.row_count; i++) {
-		seq_printf(seq, "%-2d", i);
-		seq_printf(seq, "%#7x ", result.alternate_tid[i]);
-	}
-
-	if (result.more_flag)
-		seq_printf(seq, "There is more...\n");
-
-	return 0;
-}
-
-/* Generic group F100h - Device Identity (scalar) */
-static int i2o_seq_show_dev_identity(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	static u32 work32[128];	// allow for "stuff" + up to 256 byte (max) serial number
-	// == (allow) 512d bytes (max)
-	static u16 *work16 = (u16 *) work32;
-	int token;
-	char tmp[16 + 1];
-
-	token = i2o_parm_field_get(d, 0xF100, -1, &work32, sizeof(work32));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token, "0xF100 Device Identity");
-		return 0;
-	}
-
-	seq_printf(seq, "Device Class  : %s\n", i2o_get_class_name(work16[0]));
-	seq_printf(seq, "Owner TID     : %0#5x\n", work16[2]);
-	seq_printf(seq, "Parent TID    : %0#5x\n", work16[3]);
-	seq_printf(seq, "Vendor info   : %s\n",
-		   chtostr(tmp, (u8 *) (work32 + 2), 16));
-	seq_printf(seq, "Product info  : %s\n",
-		   chtostr(tmp, (u8 *) (work32 + 6), 16));
-	seq_printf(seq, "Description   : %s\n",
-		   chtostr(tmp, (u8 *) (work32 + 10), 16));
-	seq_printf(seq, "Product rev.  : %s\n",
-		   chtostr(tmp, (u8 *) (work32 + 14), 8));
-
-	seq_printf(seq, "Serial number : ");
-	print_serial_number(seq, (u8 *) (work32 + 16),
-			    /* allow for SNLen plus
-			     * possible trailing '\0'
-			     */
-			    sizeof(work32) - (16 * sizeof(u32)) - 2);
-	seq_printf(seq, "\n");
-
-	return 0;
-}
-
-static int i2o_seq_show_dev_name(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-
-	seq_printf(seq, "%s\n", dev_name(&d->device));
-
-	return 0;
-}
-
-/* Generic group F101h - DDM Identity (scalar) */
-static int i2o_seq_show_ddm_identity(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-
-	struct {
-		u16 ddm_tid;
-		u8 module_name[24];
-		u8 module_rev[8];
-		u8 sn_format;
-		u8 serial_number[12];
-		u8 pad[256];	// allow up to 256 byte (max) serial number
-	} result;
-
-	char tmp[24 + 1];
-
-	token = i2o_parm_field_get(d, 0xF101, -1, &result, sizeof(result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token, "0xF101 DDM Identity");
-		return 0;
-	}
-
-	seq_printf(seq, "Registering DDM TID : 0x%03x\n", result.ddm_tid);
-	seq_printf(seq, "Module name         : %s\n",
-		   chtostr(tmp, result.module_name, 24));
-	seq_printf(seq, "Module revision     : %s\n",
-		   chtostr(tmp, result.module_rev, 8));
-
-	seq_printf(seq, "Serial number       : ");
-	print_serial_number(seq, result.serial_number, sizeof(result) - 36);
-	/* allow for SNLen plus possible trailing '\0' */
-
-	seq_printf(seq, "\n");
-
-	return 0;
-}
-
-/* Generic group F102h - User Information (scalar) */
-static int i2o_seq_show_uinfo(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-
-	struct {
-		u8 device_name[64];
-		u8 service_name[64];
-		u8 physical_location[64];
-		u8 instance_number[4];
-	} result;
-
-	char tmp[64 + 1];
-
-	token = i2o_parm_field_get(d, 0xF102, -1, &result, sizeof(result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token, "0xF102 User Information");
-		return 0;
-	}
-
-	seq_printf(seq, "Device name     : %s\n",
-		   chtostr(tmp, result.device_name, 64));
-	seq_printf(seq, "Service name    : %s\n",
-		   chtostr(tmp, result.service_name, 64));
-	seq_printf(seq, "Physical name   : %s\n",
-		   chtostr(tmp, result.physical_location, 64));
-	seq_printf(seq, "Instance number : %s\n",
-		   chtostr(tmp, result.instance_number, 4));
-
-	return 0;
-}
-
-/* Generic group F103h - SGL Operating Limits (scalar) */
-static int i2o_seq_show_sgl_limits(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	static u32 work32[12];
-	static u16 *work16 = (u16 *) work32;
-	static u8 *work8 = (u8 *) work32;
-	int token;
-
-	token = i2o_parm_field_get(d, 0xF103, -1, &work32, sizeof(work32));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token,
-					"0xF103 SGL Operating Limits");
-		return 0;
-	}
-
-	seq_printf(seq, "SGL chain size        : %d\n", work32[0]);
-	seq_printf(seq, "Max SGL chain size    : %d\n", work32[1]);
-	seq_printf(seq, "SGL chain size target : %d\n", work32[2]);
-	seq_printf(seq, "SGL frag count        : %d\n", work16[6]);
-	seq_printf(seq, "Max SGL frag count    : %d\n", work16[7]);
-	seq_printf(seq, "SGL frag count target : %d\n", work16[8]);
-
-/* FIXME
-	if (d->i2oversion == 0x02)
-	{
-*/
-	seq_printf(seq, "SGL data alignment    : %d\n", work16[8]);
-	seq_printf(seq, "SGL addr limit        : %d\n", work8[20]);
-	seq_printf(seq, "SGL addr sizes supported : ");
-	if (work8[21] & 0x01)
-		seq_printf(seq, "32 bit ");
-	if (work8[21] & 0x02)
-		seq_printf(seq, "64 bit ");
-	if (work8[21] & 0x04)
-		seq_printf(seq, "96 bit ");
-	if (work8[21] & 0x08)
-		seq_printf(seq, "128 bit ");
-	seq_printf(seq, "\n");
-/*
-	}
-*/
-
-	return 0;
-}
-
-/* Generic group F200h - Sensors (scalar) */
-static int i2o_seq_show_sensors(struct seq_file *seq, void *v)
-{
-	struct i2o_device *d = (struct i2o_device *)seq->private;
-	int token;
-
-	struct {
-		u16 sensor_instance;
-		u8 component;
-		u16 component_instance;
-		u8 sensor_class;
-		u8 sensor_type;
-		u8 scaling_exponent;
-		u32 actual_reading;
-		u32 minimum_reading;
-		u32 low2lowcat_treshold;
-		u32 lowcat2low_treshold;
-		u32 lowwarn2low_treshold;
-		u32 low2lowwarn_treshold;
-		u32 norm2lowwarn_treshold;
-		u32 lowwarn2norm_treshold;
-		u32 nominal_reading;
-		u32 hiwarn2norm_treshold;
-		u32 norm2hiwarn_treshold;
-		u32 high2hiwarn_treshold;
-		u32 hiwarn2high_treshold;
-		u32 hicat2high_treshold;
-		u32 hi2hicat_treshold;
-		u32 maximum_reading;
-		u8 sensor_state;
-		u16 event_enable;
-	} result;
-
-	token = i2o_parm_field_get(d, 0xF200, -1, &result, sizeof(result));
-
-	if (token < 0) {
-		i2o_report_query_status(seq, token,
-					"0xF200 Sensors (optional)");
-		return 0;
-	}
-
-	seq_printf(seq, "Sensor instance       : %d\n", result.sensor_instance);
-
-	seq_printf(seq, "Component             : %d = ", result.component);
-	switch (result.component) {
-	case 0:
-		seq_printf(seq, "Other");
-		break;
-	case 1:
-		seq_printf(seq, "Planar logic Board");
-		break;
-	case 2:
-		seq_printf(seq, "CPU");
-		break;
-	case 3:
-		seq_printf(seq, "Chassis");
-		break;
-	case 4:
-		seq_printf(seq, "Power Supply");
-		break;
-	case 5:
-		seq_printf(seq, "Storage");
-		break;
-	case 6:
-		seq_printf(seq, "External");
-		break;
-	}
-	seq_printf(seq, "\n");
-
-	seq_printf(seq, "Component instance    : %d\n",
-		   result.component_instance);
-	seq_printf(seq, "Sensor class          : %s\n",
-		   result.sensor_class ? "Analog" : "Digital");
-
-	seq_printf(seq, "Sensor type           : %d = ", result.sensor_type);
-	switch (result.sensor_type) {
-	case 0:
-		seq_printf(seq, "Other\n");
-		break;
-	case 1:
-		seq_printf(seq, "Thermal\n");
-		break;
-	case 2:
-		seq_printf(seq, "DC voltage (DC volts)\n");
-		break;
-	case 3:
-		seq_printf(seq, "AC voltage (AC volts)\n");
-		break;
-	case 4:
-		seq_printf(seq, "DC current (DC amps)\n");
-		break;
-	case 5:
-		seq_printf(seq, "AC current (AC volts)\n");
-		break;
-	case 6:
-		seq_printf(seq, "Door open\n");
-		break;
-	case 7:
-		seq_printf(seq, "Fan operational\n");
-		break;
-	}
-
-	seq_printf(seq, "Scaling exponent      : %d\n",
-		   result.scaling_exponent);
-	seq_printf(seq, "Actual reading        : %d\n", result.actual_reading);
-	seq_printf(seq, "Minimum reading       : %d\n", result.minimum_reading);
-	seq_printf(seq, "Low2LowCat treshold   : %d\n",
-		   result.low2lowcat_treshold);
-	seq_printf(seq, "LowCat2Low treshold   : %d\n",
-		   result.lowcat2low_treshold);
-	seq_printf(seq, "LowWarn2Low treshold  : %d\n",
-		   result.lowwarn2low_treshold);
-	seq_printf(seq, "Low2LowWarn treshold  : %d\n",
-		   result.low2lowwarn_treshold);
-	seq_printf(seq, "Norm2LowWarn treshold : %d\n",
-		   result.norm2lowwarn_treshold);
-	seq_printf(seq, "LowWarn2Norm treshold : %d\n",
-		   result.lowwarn2norm_treshold);
-	seq_printf(seq, "Nominal reading       : %d\n", result.nominal_reading);
-	seq_printf(seq, "HiWarn2Norm treshold  : %d\n",
-		   result.hiwarn2norm_treshold);
-	seq_printf(seq, "Norm2HiWarn treshold  : %d\n",
-		   result.norm2hiwarn_treshold);
-	seq_printf(seq, "High2HiWarn treshold  : %d\n",
-		   result.high2hiwarn_treshold);
-	seq_printf(seq, "HiWarn2High treshold  : %d\n",
-		   result.hiwarn2high_treshold);
-	seq_printf(seq, "HiCat2High treshold   : %d\n",
-		   result.hicat2high_treshold);
-	seq_printf(seq, "High2HiCat treshold   : %d\n",
-		   result.hi2hicat_treshold);
-	seq_printf(seq, "Maximum reading       : %d\n", result.maximum_reading);
-
-	seq_printf(seq, "Sensor state          : %d = ", result.sensor_state);
-	switch (result.sensor_state) {
-	case 0:
-		seq_printf(seq, "Normal\n");
-		break;
-	case 1:
-		seq_printf(seq, "Abnormal\n");
-		break;
-	case 2:
-		seq_printf(seq, "Unknown\n");
-		break;
-	case 3:
-		seq_printf(seq, "Low Catastrophic (LoCat)\n");
-		break;
-	case 4:
-		seq_printf(seq, "Low (Low)\n");
-		break;
-	case 5:
-		seq_printf(seq, "Low Warning (LoWarn)\n");
-		break;
-	case 6:
-		seq_printf(seq, "High Warning (HiWarn)\n");
-		break;
-	case 7:
-		seq_printf(seq, "High (High)\n");
-		break;
-	case 8:
-		seq_printf(seq, "High Catastrophic (HiCat)\n");
-		break;
-	}
-
-	seq_printf(seq, "Event_enable : 0x%02X\n", result.event_enable);
-	seq_printf(seq, "    [%s] Operational state change. \n",
-		   (result.event_enable & 0x01) ? "+" : "-");
-	seq_printf(seq, "    [%s] Low catastrophic. \n",
-		   (result.event_enable & 0x02) ? "+" : "-");
-	seq_printf(seq, "    [%s] Low reading. \n",
-		   (result.event_enable & 0x04) ? "+" : "-");
-	seq_printf(seq, "    [%s] Low warning. \n",
-		   (result.event_enable & 0x08) ? "+" : "-");
-	seq_printf(seq,
-		   "    [%s] Change back to normal from out of range state. \n",
-		   (result.event_enable & 0x10) ? "+" : "-");
-	seq_printf(seq, "    [%s] High warning. \n",
-		   (result.event_enable & 0x20) ? "+" : "-");
-	seq_printf(seq, "    [%s] High reading. \n",
-		   (result.event_enable & 0x40) ? "+" : "-");
-	seq_printf(seq, "    [%s] High catastrophic. \n",
-		   (result.event_enable & 0x80) ? "+" : "-");
-
-	return 0;
-}
-
-static int i2o_seq_open_hrt(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_hrt, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_lct(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_lct, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_status(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_status, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_hw(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_hw, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_ddm_table(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_ddm_table, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_driver_store(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_driver_store, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_drivers_stored(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_drivers_stored, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_groups(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_groups, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_phys_device(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_phys_device, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_claimed(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_claimed, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_users(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_users, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_priv_msgs(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_priv_msgs, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_authorized_users(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_authorized_users,
-			   PDE_DATA(inode));
-};
-
-static int i2o_seq_open_dev_identity(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_dev_identity, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_ddm_identity(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_ddm_identity, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_uinfo(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_uinfo, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_sgl_limits(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_sgl_limits, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_sensors(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_sensors, PDE_DATA(inode));
-};
-
-static int i2o_seq_open_dev_name(struct inode *inode, struct file *file)
-{
-	return single_open(file, i2o_seq_show_dev_name, PDE_DATA(inode));
-};
-
-static const struct file_operations i2o_seq_fops_lct = {
-	.open = i2o_seq_open_lct,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_hrt = {
-	.open = i2o_seq_open_hrt,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_status = {
-	.open = i2o_seq_open_status,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_hw = {
-	.open = i2o_seq_open_hw,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_ddm_table = {
-	.open = i2o_seq_open_ddm_table,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_driver_store = {
-	.open = i2o_seq_open_driver_store,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_drivers_stored = {
-	.open = i2o_seq_open_drivers_stored,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_groups = {
-	.open = i2o_seq_open_groups,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_phys_device = {
-	.open = i2o_seq_open_phys_device,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_claimed = {
-	.open = i2o_seq_open_claimed,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_users = {
-	.open = i2o_seq_open_users,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_priv_msgs = {
-	.open = i2o_seq_open_priv_msgs,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_authorized_users = {
-	.open = i2o_seq_open_authorized_users,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_dev_name = {
-	.open = i2o_seq_open_dev_name,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_dev_identity = {
-	.open = i2o_seq_open_dev_identity,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_ddm_identity = {
-	.open = i2o_seq_open_ddm_identity,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_uinfo = {
-	.open = i2o_seq_open_uinfo,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_sgl_limits = {
-	.open = i2o_seq_open_sgl_limits,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-static const struct file_operations i2o_seq_fops_sensors = {
-	.open = i2o_seq_open_sensors,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
-};
-
-/*
- * IOP specific entries...write field just in case someone
- * ever wants one.
- */
-static i2o_proc_entry i2o_proc_generic_iop_entries[] = {
-	{"hrt", S_IFREG | S_IRUGO, &i2o_seq_fops_hrt},
-	{"lct", S_IFREG | S_IRUGO, &i2o_seq_fops_lct},
-	{"status", S_IFREG | S_IRUGO, &i2o_seq_fops_status},
-	{"hw", S_IFREG | S_IRUGO, &i2o_seq_fops_hw},
-	{"ddm_table", S_IFREG | S_IRUGO, &i2o_seq_fops_ddm_table},
-	{"driver_store", S_IFREG | S_IRUGO, &i2o_seq_fops_driver_store},
-	{"drivers_stored", S_IFREG | S_IRUGO, &i2o_seq_fops_drivers_stored},
-	{NULL, 0, NULL}
-};
-
-/*
- * Device specific entries
- */
-static i2o_proc_entry generic_dev_entries[] = {
-	{"groups", S_IFREG | S_IRUGO, &i2o_seq_fops_groups},
-	{"phys_dev", S_IFREG | S_IRUGO, &i2o_seq_fops_phys_device},
-	{"claimed", S_IFREG | S_IRUGO, &i2o_seq_fops_claimed},
-	{"users", S_IFREG | S_IRUGO, &i2o_seq_fops_users},
-	{"priv_msgs", S_IFREG | S_IRUGO, &i2o_seq_fops_priv_msgs},
-	{"authorized_users", S_IFREG | S_IRUGO, &i2o_seq_fops_authorized_users},
-	{"dev_identity", S_IFREG | S_IRUGO, &i2o_seq_fops_dev_identity},
-	{"ddm_identity", S_IFREG | S_IRUGO, &i2o_seq_fops_ddm_identity},
-	{"user_info", S_IFREG | S_IRUGO, &i2o_seq_fops_uinfo},
-	{"sgl_limits", S_IFREG | S_IRUGO, &i2o_seq_fops_sgl_limits},
-	{"sensors", S_IFREG | S_IRUGO, &i2o_seq_fops_sensors},
-	{NULL, 0, NULL}
-};
-
-/*
- *  Storage unit specific entries (SCSI Periph, BS) with device names
- */
-static i2o_proc_entry rbs_dev_entries[] = {
-	{"dev_name", S_IFREG | S_IRUGO, &i2o_seq_fops_dev_name},
-	{NULL, 0, NULL}
-};
-
-/**
- *	i2o_proc_create_entries - Creates proc dir entries
- *	@dir: proc dir entry under which the entries should be placed
- *	@i2o_pe: pointer to the entries which should be added
- *	@data: pointer to I2O controller or device
- *
- *	Create proc dir entries for a I2O controller or I2O device.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_proc_create_entries(struct proc_dir_entry *dir,
-				   i2o_proc_entry * i2o_pe, void *data)
-{
-	struct proc_dir_entry *tmp;
-
-	while (i2o_pe->name) {
-		tmp = proc_create_data(i2o_pe->name, i2o_pe->mode, dir,
-				       i2o_pe->fops, data);
-		if (!tmp)
-			return -1;
-
-		i2o_pe++;
-	}
-
-	return 0;
-}
-
-/**
- *	i2o_proc_device_add - Add an I2O device to the proc dir
- *	@dir: proc dir entry to which the device should be added
- *	@dev: I2O device which should be added
- *
- *	Add an I2O device to the proc dir entry dir and create the entries for
- *	the device depending on the class of the I2O device.
- */
-static void i2o_proc_device_add(struct proc_dir_entry *dir,
-				struct i2o_device *dev)
-{
-	char buff[10];
-	struct proc_dir_entry *devdir;
-	i2o_proc_entry *i2o_pe = NULL;
-
-	sprintf(buff, "%03x", dev->lct_data.tid);
-
-	osm_debug("adding device /proc/i2o/%s/%s\n", dev->iop->name, buff);
-
-	devdir = proc_mkdir_data(buff, 0, dir, dev);
-	if (!devdir) {
-		osm_warn("Could not allocate procdir!\n");
-		return;
-	}
-
-	i2o_proc_create_entries(devdir, generic_dev_entries, dev);
-
-	/* Inform core that we want updates about this device's status */
-	switch (dev->lct_data.class_id) {
-	case I2O_CLASS_SCSI_PERIPHERAL:
-	case I2O_CLASS_RANDOM_BLOCK_STORAGE:
-		i2o_pe = rbs_dev_entries;
-		break;
-	default:
-		break;
-	}
-	if (i2o_pe)
-		i2o_proc_create_entries(devdir, i2o_pe, dev);
-}
-
-/**
- *	i2o_proc_iop_add - Add an I2O controller to the i2o proc tree
- *	@dir: parent proc dir entry
- *	@c: I2O controller which should be added
- *
- *	Add the entries to the parent proc dir entry. Also each device is added
- *	to the controllers proc dir entry.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_proc_iop_add(struct proc_dir_entry *dir,
-			    struct i2o_controller *c)
-{
-	struct proc_dir_entry *iopdir;
-	struct i2o_device *dev;
-
-	osm_debug("adding IOP /proc/i2o/%s\n", c->name);
-
-	iopdir = proc_mkdir_data(c->name, 0, dir, c);
-	if (!iopdir)
-		return -1;
-
-	i2o_proc_create_entries(iopdir, i2o_proc_generic_iop_entries, c);
-
-	list_for_each_entry(dev, &c->devices, list)
-	    i2o_proc_device_add(iopdir, dev);
-
-	return 0;
-}
-
-/**
- *	i2o_proc_fs_create - Create the i2o proc fs.
- *
- *	Iterate over each I2O controller and create the entries for it.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int __init i2o_proc_fs_create(void)
-{
-	struct i2o_controller *c;
-
-	i2o_proc_dir_root = proc_mkdir("i2o", NULL);
-	if (!i2o_proc_dir_root)
-		return -1;
-
-	list_for_each_entry(c, &i2o_controllers, list)
-	    i2o_proc_iop_add(i2o_proc_dir_root, c);
-
-	return 0;
-};
-
-/**
- *	i2o_proc_fs_destroy - Cleanup the all i2o proc entries
- *
- *	Iterate over each I2O controller and remove the entries for it.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int __exit i2o_proc_fs_destroy(void)
-{
-	remove_proc_subtree("i2o", NULL);
-
-	return 0;
-};
-
-/**
- *	i2o_proc_init - Init function for procfs
- *
- *	Registers Proc OSM and creates procfs entries.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int __init i2o_proc_init(void)
-{
-	int rc;
-
-	printk(KERN_INFO OSM_DESCRIPTION " v" OSM_VERSION "\n");
-
-	rc = i2o_driver_register(&i2o_proc_driver);
-	if (rc)
-		return rc;
-
-	rc = i2o_proc_fs_create();
-	if (rc) {
-		i2o_driver_unregister(&i2o_proc_driver);
-		return rc;
-	}
-
-	return 0;
-};
-
-/**
- *	i2o_proc_exit - Exit function for procfs
- *
- *	Unregisters Proc OSM and removes procfs entries.
- */
-static void __exit i2o_proc_exit(void)
-{
-	i2o_driver_unregister(&i2o_proc_driver);
-	i2o_proc_fs_destroy();
-};
-
-MODULE_AUTHOR("Deepak Saxena");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(OSM_DESCRIPTION);
-MODULE_VERSION(OSM_VERSION);
-
-module_init(i2o_proc_init);
-module_exit(i2o_proc_exit);
diff --git a/drivers/staging/i2o/i2o_scsi.c b/drivers/staging/i2o/i2o_scsi.c
deleted file mode 100644
index 1b11dcb3faea..000000000000
--- a/drivers/staging/i2o/i2o_scsi.c
+++ /dev/null
@@ -1,814 +0,0 @@
-/*
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation; either version 2, or (at your option) any
- * later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * For the avoidance of doubt the "preferred form" of this code is one which
- * is in an open non patent encumbered format. Where cryptographic key signing
- * forms part of the process of creating an executable the information
- * including keys needed to generate an equivalently functional executable
- * are deemed to be part of the source code.
- *
- *  Complications for I2O scsi
- *
- *	o	Each (bus,lun) is a logical device in I2O. We keep a map
- *		table. We spoof failed selection for unmapped units
- *	o	Request sense buffers can come back for free.
- *	o	Scatter gather is a bit dynamic. We have to investigate at
- *		setup time.
- *	o	Some of our resources are dynamically shared. The i2o core
- *		needs a message reservation protocol to avoid swap v net
- *		deadlocking. We need to back off queue requests.
- *
- *	In general the firmware wants to help. Where its help isn't performance
- *	useful we just ignore the aid. Its not worth the code in truth.
- *
- * Fixes/additions:
- *	Steve Ralston:
- *		Scatter gather now works
- *	Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *		Minor fixes for 2.6.
- *
- * To Do:
- *	64bit cleanups
- *	Fix the resource management problems.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/string.h>
-#include <linux/ioport.h>
-#include <linux/jiffies.h>
-#include <linux/interrupt.h>
-#include <linux/timer.h>
-#include <linux/delay.h>
-#include <linux/proc_fs.h>
-#include <linux/prefetch.h>
-#include <linux/pci.h>
-#include <linux/blkdev.h>
-#include "i2o.h"
-#include <linux/scatterlist.h>
-
-#include <asm/dma.h>
-#include <asm/io.h>
-#include <linux/atomic.h>
-
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_cmnd.h>
-#include <scsi/sg.h>
-
-#define OSM_NAME	"scsi-osm"
-#define OSM_VERSION	"1.316"
-#define OSM_DESCRIPTION	"I2O SCSI Peripheral OSM"
-
-static struct i2o_driver i2o_scsi_driver;
-
-static unsigned int i2o_scsi_max_id = 16;
-static unsigned int i2o_scsi_max_lun = 255;
-
-struct i2o_scsi_host {
-	struct Scsi_Host *scsi_host;	/* pointer to the SCSI host */
-	struct i2o_controller *iop;	/* pointer to the I2O controller */
-	u64 lun;	/* lun's used for block devices */
-	struct i2o_device *channel[0];	/* channel->i2o_dev mapping table */
-};
-
-static struct scsi_host_template i2o_scsi_host_template;
-
-#define I2O_SCSI_CAN_QUEUE	4
-
-/* SCSI OSM class handling definition */
-static struct i2o_class_id i2o_scsi_class_id[] = {
-	{I2O_CLASS_SCSI_PERIPHERAL},
-	{I2O_CLASS_END}
-};
-
-static struct i2o_scsi_host *i2o_scsi_host_alloc(struct i2o_controller *c)
-{
-	struct i2o_scsi_host *i2o_shost;
-	struct i2o_device *i2o_dev;
-	struct Scsi_Host *scsi_host;
-	int max_channel = 0;
-	u8 type;
-	int i;
-	size_t size;
-	u16 body_size = 6;
-
-#ifdef CONFIG_I2O_EXT_ADAPTEC
-	if (c->adaptec)
-		body_size = 8;
-#endif
-
-	list_for_each_entry(i2o_dev, &c->devices, list)
-	    if (i2o_dev->lct_data.class_id == I2O_CLASS_BUS_ADAPTER) {
-		if (!i2o_parm_field_get(i2o_dev, 0x0000, 0, &type, 1)
-		    && (type == 0x01))	/* SCSI bus */
-			max_channel++;
-	}
-
-	if (!max_channel) {
-		osm_warn("no channels found on %s\n", c->name);
-		return ERR_PTR(-EFAULT);
-	}
-
-	size = max_channel * sizeof(struct i2o_device *)
-	    + sizeof(struct i2o_scsi_host);
-
-	scsi_host = scsi_host_alloc(&i2o_scsi_host_template, size);
-	if (!scsi_host) {
-		osm_warn("Could not allocate SCSI host\n");
-		return ERR_PTR(-ENOMEM);
-	}
-
-	scsi_host->max_channel = max_channel - 1;
-	scsi_host->max_id = i2o_scsi_max_id;
-	scsi_host->max_lun = i2o_scsi_max_lun;
-	scsi_host->this_id = c->unit;
-	scsi_host->sg_tablesize = i2o_sg_tablesize(c, body_size);
-
-	i2o_shost = (struct i2o_scsi_host *)scsi_host->hostdata;
-	i2o_shost->scsi_host = scsi_host;
-	i2o_shost->iop = c;
-	i2o_shost->lun = 1;
-
-	i = 0;
-	list_for_each_entry(i2o_dev, &c->devices, list)
-	    if (i2o_dev->lct_data.class_id == I2O_CLASS_BUS_ADAPTER) {
-		if (!i2o_parm_field_get(i2o_dev, 0x0000, 0, &type, 1)
-		    && (type == 0x01))	/* only SCSI bus */
-			i2o_shost->channel[i++] = i2o_dev;
-
-		if (i >= max_channel)
-			break;
-	}
-
-	return i2o_shost;
-};
-
-/**
- *	i2o_scsi_get_host - Get an I2O SCSI host
- *	@c: I2O controller to for which to get the SCSI host
- *
- *	If the I2O controller already exists as SCSI host, the SCSI host
- *	is returned, otherwise the I2O controller is added to the SCSI
- *	core.
- *
- *	Returns pointer to the I2O SCSI host on success or NULL on failure.
- */
-static struct i2o_scsi_host *i2o_scsi_get_host(struct i2o_controller *c)
-{
-	return c->driver_data[i2o_scsi_driver.context];
-};
-
-/**
- *	i2o_scsi_remove - Remove I2O device from SCSI core
- *	@dev: device which should be removed
- *
- *	Removes the I2O device from the SCSI core again.
- *
- *	Returns 0 on success.
- */
-static int i2o_scsi_remove(struct device *dev)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-	struct i2o_controller *c = i2o_dev->iop;
-	struct i2o_scsi_host *i2o_shost;
-	struct scsi_device *scsi_dev;
-
-	osm_info("device removed (TID: %03x)\n", i2o_dev->lct_data.tid);
-
-	i2o_shost = i2o_scsi_get_host(c);
-
-	shost_for_each_device(scsi_dev, i2o_shost->scsi_host)
-	    if (scsi_dev->hostdata == i2o_dev) {
-		sysfs_remove_link(&i2o_dev->device.kobj, "scsi");
-		scsi_remove_device(scsi_dev);
-		scsi_device_put(scsi_dev);
-		break;
-	}
-
-	return 0;
-};
-
-/**
- *	i2o_scsi_probe - verify if dev is a I2O SCSI device and install it
- *	@dev: device to verify if it is a I2O SCSI device
- *
- *	Retrieve channel, id and lun for I2O device. If everything goes well
- *	register the I2O device as SCSI device on the I2O SCSI controller.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_scsi_probe(struct device *dev)
-{
-	struct i2o_device *i2o_dev = to_i2o_device(dev);
-	struct i2o_controller *c = i2o_dev->iop;
-	struct i2o_scsi_host *i2o_shost;
-	struct Scsi_Host *scsi_host;
-	struct i2o_device *parent;
-	struct scsi_device *scsi_dev;
-	u32 id = -1;
-	u64 lun = -1;
-	int channel = -1;
-	int i, rc;
-
-	i2o_shost = i2o_scsi_get_host(c);
-	if (!i2o_shost)
-		return -EFAULT;
-
-	scsi_host = i2o_shost->scsi_host;
-
-	switch (i2o_dev->lct_data.class_id) {
-	case I2O_CLASS_RANDOM_BLOCK_STORAGE:
-	case I2O_CLASS_EXECUTIVE:
-#ifdef CONFIG_I2O_EXT_ADAPTEC
-		if (c->adaptec) {
-			u8 type;
-			struct i2o_device *d = i2o_shost->channel[0];
-
-			if (!i2o_parm_field_get(d, 0x0000, 0, &type, 1)
-			    && (type == 0x01))	/* SCSI bus */
-				if (!i2o_parm_field_get(d, 0x0200, 4, &id, 4)) {
-					channel = 0;
-					if (i2o_dev->lct_data.class_id ==
-					    I2O_CLASS_RANDOM_BLOCK_STORAGE)
-						lun =
-						    cpu_to_le64(i2o_shost->
-								lun++);
-					else
-						lun = 0;
-				}
-		}
-#endif
-		break;
-
-	case I2O_CLASS_SCSI_PERIPHERAL:
-		if (i2o_parm_field_get(i2o_dev, 0x0000, 3, &id, 4))
-			return -EFAULT;
-
-		if (i2o_parm_field_get(i2o_dev, 0x0000, 4, &lun, 8))
-			return -EFAULT;
-
-		parent = i2o_iop_find_device(c, i2o_dev->lct_data.parent_tid);
-		if (!parent) {
-			osm_warn("can not find parent of device %03x\n",
-				 i2o_dev->lct_data.tid);
-			return -EFAULT;
-		}
-
-		for (i = 0; i <= i2o_shost->scsi_host->max_channel; i++)
-			if (i2o_shost->channel[i] == parent)
-				channel = i;
-		break;
-
-	default:
-		return -EFAULT;
-	}
-
-	if (channel == -1) {
-		osm_warn("can not find channel of device %03x\n",
-			 i2o_dev->lct_data.tid);
-		return -EFAULT;
-	}
-
-	if (le32_to_cpu(id) >= scsi_host->max_id) {
-		osm_warn("SCSI device id (%d) >= max_id of I2O host (%d)",
-			 le32_to_cpu(id), scsi_host->max_id);
-		return -EFAULT;
-	}
-
-	if (le64_to_cpu(lun) >= scsi_host->max_lun) {
-		osm_warn("SCSI device lun (%llu) >= max_lun of I2O host (%llu)",
-			 le64_to_cpu(lun), scsi_host->max_lun);
-		return -EFAULT;
-	}
-
-	scsi_dev =
-	    __scsi_add_device(i2o_shost->scsi_host, channel, le32_to_cpu(id),
-			      le64_to_cpu(lun), i2o_dev);
-
-	if (IS_ERR(scsi_dev)) {
-		osm_warn("can not add SCSI device %03x\n",
-			 i2o_dev->lct_data.tid);
-		return PTR_ERR(scsi_dev);
-	}
-
-	rc = sysfs_create_link(&i2o_dev->device.kobj,
-			       &scsi_dev->sdev_gendev.kobj, "scsi");
-	if (rc)
-		goto err;
-
-	osm_info("device added (TID: %03x) channel: %d, id: %d, lun: %llu\n",
-		 i2o_dev->lct_data.tid, channel, le32_to_cpu(id),
-		 le64_to_cpu(lun));
-
-	return 0;
-
-err:
-	scsi_remove_device(scsi_dev);
-	return rc;
-};
-
-static const char *i2o_scsi_info(struct Scsi_Host *SChost)
-{
-	struct i2o_scsi_host *hostdata;
-	hostdata = (struct i2o_scsi_host *)SChost->hostdata;
-	return hostdata->iop->name;
-}
-
-/**
- *	i2o_scsi_reply - SCSI OSM message reply handler
- *	@c: controller issuing the reply
- *	@m: message id for flushing
- *	@msg: the message from the controller
- *
- *	Process reply messages (interrupts in normal scsi controller think).
- *	We can get a variety of messages to process. The normal path is
- *	scsi command completions. We must also deal with IOP failures,
- *	the reply to a bus reset and the reply to a LUN query.
- *
- *	Returns 0 on success and if the reply should not be flushed or > 0
- *	on success and if the reply should be flushed. Returns negative error
- *	code on failure and if the reply should be flushed.
- */
-static int i2o_scsi_reply(struct i2o_controller *c, u32 m,
-			  struct i2o_message *msg)
-{
-	struct scsi_cmnd *cmd;
-	u32 error;
-	struct device *dev;
-
-	cmd = i2o_cntxt_list_get(c, le32_to_cpu(msg->u.s.tcntxt));
-	if (unlikely(!cmd)) {
-		osm_err("NULL reply received!\n");
-		return -1;
-	}
-
-	/*
-	 *      Low byte is device status, next is adapter status,
-	 *      (then one byte reserved), then request status.
-	 */
-	error = le32_to_cpu(msg->body[0]);
-
-	osm_debug("Completed %0x%p\n", cmd);
-
-	cmd->result = error & 0xff;
-	/*
-	 * if DeviceStatus is not SCSI_SUCCESS copy over the sense data and let
-	 * the SCSI layer handle the error
-	 */
-	if (cmd->result)
-		memcpy(cmd->sense_buffer, &msg->body[3],
-		       min(SCSI_SENSE_BUFFERSIZE, 40));
-
-	/* only output error code if AdapterStatus is not HBA_SUCCESS */
-	if ((error >> 8) & 0xff)
-		osm_err("SCSI error %08x\n", error);
-
-	dev = &c->pdev->dev;
-
-	scsi_dma_unmap(cmd);
-
-	cmd->scsi_done(cmd);
-
-	return 1;
-};
-
-/**
- *	i2o_scsi_notify_device_add - Retrieve notifications of added devices
- *	@i2o_dev: the I2O device which was added
- *
- *	If a I2O device is added we catch the notification, because I2O classes
- *	other than SCSI peripheral will not be received through
- *	i2o_scsi_probe().
- */
-static void i2o_scsi_notify_device_add(struct i2o_device *i2o_dev)
-{
-	switch (i2o_dev->lct_data.class_id) {
-	case I2O_CLASS_EXECUTIVE:
-	case I2O_CLASS_RANDOM_BLOCK_STORAGE:
-		i2o_scsi_probe(&i2o_dev->device);
-		break;
-
-	default:
-		break;
-	}
-};
-
-/**
- *	i2o_scsi_notify_device_remove - Retrieve notifications of removed devices
- *	@i2o_dev: the I2O device which was removed
- *
- *	If a I2O device is removed, we catch the notification to remove the
- *	corresponding SCSI device.
- */
-static void i2o_scsi_notify_device_remove(struct i2o_device *i2o_dev)
-{
-	switch (i2o_dev->lct_data.class_id) {
-	case I2O_CLASS_EXECUTIVE:
-	case I2O_CLASS_RANDOM_BLOCK_STORAGE:
-		i2o_scsi_remove(&i2o_dev->device);
-		break;
-
-	default:
-		break;
-	}
-};
-
-/**
- *	i2o_scsi_notify_controller_add - Retrieve notifications of added controllers
- *	@c: the controller which was added
- *
- *	If a I2O controller is added, we catch the notification to add a
- *	corresponding Scsi_Host.
- */
-static void i2o_scsi_notify_controller_add(struct i2o_controller *c)
-{
-	struct i2o_scsi_host *i2o_shost;
-	int rc;
-
-	i2o_shost = i2o_scsi_host_alloc(c);
-	if (IS_ERR(i2o_shost)) {
-		osm_err("Could not initialize SCSI host\n");
-		return;
-	}
-
-	rc = scsi_add_host(i2o_shost->scsi_host, &c->device);
-	if (rc) {
-		osm_err("Could not add SCSI host\n");
-		scsi_host_put(i2o_shost->scsi_host);
-		return;
-	}
-
-	c->driver_data[i2o_scsi_driver.context] = i2o_shost;
-
-	osm_debug("new I2O SCSI host added\n");
-};
-
-/**
- *	i2o_scsi_notify_controller_remove - Retrieve notifications of removed controllers
- *	@c: the controller which was removed
- *
- *	If a I2O controller is removed, we catch the notification to remove the
- *	corresponding Scsi_Host.
- */
-static void i2o_scsi_notify_controller_remove(struct i2o_controller *c)
-{
-	struct i2o_scsi_host *i2o_shost;
-	i2o_shost = i2o_scsi_get_host(c);
-	if (!i2o_shost)
-		return;
-
-	c->driver_data[i2o_scsi_driver.context] = NULL;
-
-	scsi_remove_host(i2o_shost->scsi_host);
-	scsi_host_put(i2o_shost->scsi_host);
-	osm_debug("I2O SCSI host removed\n");
-};
-
-/* SCSI OSM driver struct */
-static struct i2o_driver i2o_scsi_driver = {
-	.name = OSM_NAME,
-	.reply = i2o_scsi_reply,
-	.classes = i2o_scsi_class_id,
-	.notify_device_add = i2o_scsi_notify_device_add,
-	.notify_device_remove = i2o_scsi_notify_device_remove,
-	.notify_controller_add = i2o_scsi_notify_controller_add,
-	.notify_controller_remove = i2o_scsi_notify_controller_remove,
-	.driver = {
-		   .probe = i2o_scsi_probe,
-		   .remove = i2o_scsi_remove,
-		   },
-};
-
-/**
- *	i2o_scsi_queuecommand - queue a SCSI command
- *	@SCpnt: scsi command pointer
- *	@done: callback for completion
- *
- *	Issue a scsi command asynchronously. Return 0 on success or 1 if
- *	we hit an error (normally message queue congestion). The only
- *	minor complication here is that I2O deals with the device addressing
- *	so we have to map the bus/dev/lun back to an I2O handle as well
- *	as faking absent devices ourself.
- *
- *	Locks: takes the controller lock on error path only
- */
-
-static int i2o_scsi_queuecommand_lck(struct scsi_cmnd *SCpnt,
-				 void (*done) (struct scsi_cmnd *))
-{
-	struct i2o_controller *c;
-	struct i2o_device *i2o_dev;
-	int tid;
-	struct i2o_message *msg;
-	/*
-	 * ENABLE_DISCONNECT
-	 * SIMPLE_TAG
-	 * RETURN_SENSE_DATA_IN_REPLY_MESSAGE_FRAME
-	 */
-	u32 scsi_flags = 0x20a00000;
-	u32 sgl_offset;
-	u32 *mptr;
-	u32 cmd = I2O_CMD_SCSI_EXEC << 24;
-	int rc = 0;
-
-	/*
-	 *      Do the incoming paperwork
-	 */
-	i2o_dev = SCpnt->device->hostdata;
-
-	SCpnt->scsi_done = done;
-
-	if (unlikely(!i2o_dev)) {
-		osm_warn("no I2O device in request\n");
-		SCpnt->result = DID_NO_CONNECT << 16;
-		done(SCpnt);
-		goto exit;
-	}
-	c = i2o_dev->iop;
-	tid = i2o_dev->lct_data.tid;
-
-	osm_debug("qcmd: Tid = %03x\n", tid);
-	osm_debug("Real scsi messages.\n");
-
-	/*
-	 *      Put together a scsi execscb message
-	 */
-	switch (SCpnt->sc_data_direction) {
-	case PCI_DMA_NONE:
-		/* DATA NO XFER */
-		sgl_offset = SGL_OFFSET_0;
-		break;
-
-	case PCI_DMA_TODEVICE:
-		/* DATA OUT (iop-->dev) */
-		scsi_flags |= 0x80000000;
-		sgl_offset = SGL_OFFSET_10;
-		break;
-
-	case PCI_DMA_FROMDEVICE:
-		/* DATA IN  (iop<--dev) */
-		scsi_flags |= 0x40000000;
-		sgl_offset = SGL_OFFSET_10;
-		break;
-
-	default:
-		/* Unknown - kill the command */
-		SCpnt->result = DID_NO_CONNECT << 16;
-		done(SCpnt);
-		goto exit;
-	}
-
-	/*
-	 *      Obtain an I2O message. If there are none free then
-	 *      throw it back to the scsi layer
-	 */
-
-	msg = i2o_msg_get(c);
-	if (IS_ERR(msg)) {
-		rc = SCSI_MLQUEUE_HOST_BUSY;
-		goto exit;
-	}
-
-	mptr = &msg->body[0];
-
-#if 0 /* this code can't work */
-#ifdef CONFIG_I2O_EXT_ADAPTEC
-	if (c->adaptec) {
-		u32 adpt_flags = 0;
-
-		if (SCpnt->sc_request && SCpnt->sc_request->upper_private_data) {
-			i2o_sg_io_hdr_t __user *usr_ptr =
-			    ((Sg_request *) (SCpnt->sc_request->
-					     upper_private_data))->header.
-			    usr_ptr;
-
-			if (usr_ptr)
-				get_user(adpt_flags, &usr_ptr->flags);
-		}
-
-		switch (i2o_dev->lct_data.class_id) {
-		case I2O_CLASS_EXECUTIVE:
-		case I2O_CLASS_RANDOM_BLOCK_STORAGE:
-			/* interpret flag has to be set for executive */
-			adpt_flags ^= I2O_DPT_SG_FLAG_INTERPRET;
-			break;
-
-		default:
-			break;
-		}
-
-		/*
-		 * for Adaptec controllers we use the PRIVATE command, because
-		 * the normal SCSI EXEC doesn't support all SCSI commands on
-		 * all controllers (for example READ CAPACITY).
-		 */
-		if (sgl_offset == SGL_OFFSET_10)
-			sgl_offset = SGL_OFFSET_12;
-		cmd = I2O_CMD_PRIVATE << 24;
-		*mptr++ = cpu_to_le32(I2O_VENDOR_DPT << 16 | I2O_CMD_SCSI_EXEC);
-		*mptr++ = cpu_to_le32(adpt_flags | tid);
-	}
-#endif
-#endif
-
-	msg->u.head[1] = cpu_to_le32(cmd | HOST_TID << 12 | tid);
-	msg->u.s.icntxt = cpu_to_le32(i2o_scsi_driver.context);
-
-	/* We want the SCSI control block back */
-	msg->u.s.tcntxt = cpu_to_le32(i2o_cntxt_list_add(c, SCpnt));
-
-	/* LSI_920_PCI_QUIRK
-	 *
-	 *      Intermittant observations of msg frame word data corruption
-	 *      observed on msg[4] after:
-	 *        WRITE, READ-MODIFY-WRITE
-	 *      operations.  19990606 -sralston
-	 *
-	 *      (Hence we build this word via tag. Its good practice anyway
-	 *       we don't want fetches over PCI needlessly)
-	 */
-
-	/* Attach tags to the devices */
-	/* FIXME: implement
-	   if(SCpnt->device->tagged_supported) {
-	   if(SCpnt->tag == HEAD_OF_QUEUE_TAG)
-	   scsi_flags |= 0x01000000;
-	   else if(SCpnt->tag == ORDERED_QUEUE_TAG)
-	   scsi_flags |= 0x01800000;
-	   }
-	 */
-
-	*mptr++ = cpu_to_le32(scsi_flags | SCpnt->cmd_len);
-
-	/* Write SCSI command into the message - always 16 byte block */
-	memcpy(mptr, SCpnt->cmnd, 16);
-	mptr += 4;
-
-	if (sgl_offset != SGL_OFFSET_0) {
-		/* write size of data addressed by SGL */
-		*mptr++ = cpu_to_le32(scsi_bufflen(SCpnt));
-
-		/* Now fill in the SGList and command */
-
-		if (scsi_sg_count(SCpnt)) {
-			if (!i2o_dma_map_sg(c, scsi_sglist(SCpnt),
-					    scsi_sg_count(SCpnt),
-					    SCpnt->sc_data_direction, &mptr))
-				goto nomem;
-		}
-	}
-
-	/* Stick the headers on */
-	msg->u.head[0] =
-	    cpu_to_le32(I2O_MESSAGE_SIZE(mptr - &msg->u.head[0]) | sgl_offset);
-
-	/* Queue the message */
-	i2o_msg_post(c, msg);
-
-	osm_debug("Issued %0x%p\n", SCpnt);
-
-	return 0;
-
-      nomem:
-	rc = -ENOMEM;
-	i2o_msg_nop(c, msg);
-
-      exit:
-	return rc;
-}
-
-static DEF_SCSI_QCMD(i2o_scsi_queuecommand)
-
-/**
- *	i2o_scsi_abort - abort a running command
- *	@SCpnt: command to abort
- *
- *	Ask the I2O controller to abort a command. This is an asynchrnous
- *	process and our callback handler will see the command complete with an
- *	aborted message if it succeeds.
- *
- *	Returns 0 if the command is successfully aborted or negative error code
- *	on failure.
- */
-static int i2o_scsi_abort(struct scsi_cmnd *SCpnt)
-{
-	struct i2o_device *i2o_dev;
-	struct i2o_controller *c;
-	struct i2o_message *msg;
-	int tid;
-	int status = FAILED;
-
-	osm_warn("Aborting command block.\n");
-
-	i2o_dev = SCpnt->device->hostdata;
-	c = i2o_dev->iop;
-	tid = i2o_dev->lct_data.tid;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return SCSI_MLQUEUE_HOST_BUSY;
-
-	msg->u.head[0] = cpu_to_le32(FIVE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_SCSI_ABORT << 24 | HOST_TID << 12 | tid);
-	msg->body[0] = cpu_to_le32(i2o_cntxt_list_get_ptr(c, SCpnt));
-
-	if (!i2o_msg_post_wait(c, msg, I2O_TIMEOUT_SCSI_SCB_ABORT))
-		status = SUCCESS;
-
-	return status;
-}
-
-/**
- *	i2o_scsi_bios_param	-	Invent disk geometry
- *	@sdev: scsi device
- *	@dev: block layer device
- *	@capacity: size in sectors
- *	@ip: geometry array
- *
- *	This is anyone's guess quite frankly. We use the same rules everyone
- *	else appears to and hope. It seems to work.
- */
-
-static int i2o_scsi_bios_param(struct scsi_device *sdev,
-			       struct block_device *dev, sector_t capacity,
-			       int *ip)
-{
-	int size;
-
-	size = capacity;
-	ip[0] = 64;		/* heads                        */
-	ip[1] = 32;		/* sectors                      */
-	if ((ip[2] = size >> 11) > 1024) {	/* cylinders, test for big disk */
-		ip[0] = 255;	/* heads                        */
-		ip[1] = 63;	/* sectors                      */
-		ip[2] = size / (255 * 63);	/* cylinders                    */
-	}
-	return 0;
-}
-
-static struct scsi_host_template i2o_scsi_host_template = {
-	.proc_name = OSM_NAME,
-	.name = OSM_DESCRIPTION,
-	.info = i2o_scsi_info,
-	.queuecommand = i2o_scsi_queuecommand,
-	.eh_abort_handler = i2o_scsi_abort,
-	.bios_param = i2o_scsi_bios_param,
-	.can_queue = I2O_SCSI_CAN_QUEUE,
-	.sg_tablesize = 8,
-	.cmd_per_lun = 6,
-	.use_clustering = ENABLE_CLUSTERING,
-};
-
-/**
- *	i2o_scsi_init - SCSI OSM initialization function
- *
- *	Register SCSI OSM into I2O core.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int __init i2o_scsi_init(void)
-{
-	int rc;
-
-	printk(KERN_INFO OSM_DESCRIPTION " v" OSM_VERSION "\n");
-
-	/* Register SCSI OSM into I2O core */
-	rc = i2o_driver_register(&i2o_scsi_driver);
-	if (rc) {
-		osm_err("Could not register SCSI driver\n");
-		return rc;
-	}
-
-	return 0;
-};
-
-/**
- *	i2o_scsi_exit - SCSI OSM exit function
- *
- *	Unregisters SCSI OSM from I2O core.
- */
-static void __exit i2o_scsi_exit(void)
-{
-	/* Unregister I2O SCSI OSM from I2O core */
-	i2o_driver_unregister(&i2o_scsi_driver);
-};
-
-MODULE_AUTHOR("Red Hat Software");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(OSM_DESCRIPTION);
-MODULE_VERSION(OSM_VERSION);
-
-module_init(i2o_scsi_init);
-module_exit(i2o_scsi_exit);
diff --git a/drivers/staging/i2o/iop.c b/drivers/staging/i2o/iop.c
deleted file mode 100644
index 23bdbe4aa480..000000000000
--- a/drivers/staging/i2o/iop.c
+++ /dev/null
@@ -1,1255 +0,0 @@
-/*
- *	Functions to handle I2O controllers and I2O message handling
- *
- *	Copyright (C) 1999-2002	Red Hat Software
- *
- *	Written by Alan Cox, Building Number Three Ltd
- *
- *	This program is free software; you can redistribute it and/or modify it
- *	under the terms of the GNU General Public License as published by the
- *	Free Software Foundation; either version 2 of the License, or (at your
- *	option) any later version.
- *
- *	A lot of the I2O message side code from this is taken from the
- *	Red Creek RCPCI45 adapter driver by Red Creek Communications
- *
- *	Fixes/additions:
- *		Philipp Rumpf
- *		Juha Sievänen <Juha.Sievanen@cs.Helsinki.FI>
- *		Auvo Häkkinen <Auvo.Hakkinen@cs.Helsinki.FI>
- *		Deepak Saxena <deepak@plexity.net>
- *		Boji T Kannanthanam <boji.t.kannanthanam@intel.com>
- *		Alan Cox <alan@lxorguk.ukuu.org.uk>:
- *			Ported to Linux 2.5.
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *			Minor fixes for 2.6.
- */
-
-#include <linux/module.h>
-#include "i2o.h"
-#include <linux/delay.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include "core.h"
-
-#define OSM_NAME	"i2o"
-#define OSM_VERSION	"1.325"
-#define OSM_DESCRIPTION	"I2O subsystem"
-
-/* global I2O controller list */
-LIST_HEAD(i2o_controllers);
-
-/*
- * global I2O System Table. Contains information about all the IOPs in the
- * system. Used to inform IOPs about each others existence.
- */
-static struct i2o_dma i2o_systab;
-
-static int i2o_hrt_get(struct i2o_controller *c);
-
-/**
- *	i2o_msg_get_wait - obtain an I2O message from the IOP
- *	@c: I2O controller
- *	@wait: how long to wait until timeout
- *
- *	This function waits up to wait seconds for a message slot to be
- *	available.
- *
- *	On a success the message is returned and the pointer to the message is
- *	set in msg. The returned message is the physical page frame offset
- *	address from the read port (see the i2o spec). If no message is
- *	available returns I2O_QUEUE_EMPTY and msg is leaved untouched.
- */
-struct i2o_message *i2o_msg_get_wait(struct i2o_controller *c, int wait)
-{
-	unsigned long timeout = jiffies + wait * HZ;
-	struct i2o_message *msg;
-
-	while (IS_ERR(msg = i2o_msg_get(c))) {
-		if (time_after(jiffies, timeout)) {
-			osm_debug("%s: Timeout waiting for message frame.\n",
-				  c->name);
-			return ERR_PTR(-ETIMEDOUT);
-		}
-		schedule_timeout_uninterruptible(1);
-	}
-
-	return msg;
-};
-
-#if BITS_PER_LONG == 64
-/**
- *      i2o_cntxt_list_add - Append a pointer to context list and return a id
- *	@c: controller to which the context list belong
- *	@ptr: pointer to add to the context list
- *
- *	Because the context field in I2O is only 32-bit large, on 64-bit the
- *	pointer is to large to fit in the context field. The i2o_cntxt_list
- *	functions therefore map pointers to context fields.
- *
- *	Returns context id > 0 on success or 0 on failure.
- */
-u32 i2o_cntxt_list_add(struct i2o_controller * c, void *ptr)
-{
-	struct i2o_context_list_element *entry;
-	unsigned long flags;
-
-	if (!ptr)
-		osm_err("%s: couldn't add NULL pointer to context list!\n",
-			c->name);
-
-	entry = kmalloc(sizeof(*entry), GFP_ATOMIC);
-	if (!entry) {
-		osm_err("%s: Could not allocate memory for context list element"
-			"\n", c->name);
-		return 0;
-	}
-
-	entry->ptr = ptr;
-	entry->timestamp = jiffies;
-	INIT_LIST_HEAD(&entry->list);
-
-	spin_lock_irqsave(&c->context_list_lock, flags);
-
-	if (unlikely(atomic_inc_and_test(&c->context_list_counter)))
-		atomic_inc(&c->context_list_counter);
-
-	entry->context = atomic_read(&c->context_list_counter);
-
-	list_add(&entry->list, &c->context_list);
-
-	spin_unlock_irqrestore(&c->context_list_lock, flags);
-
-	osm_debug("%s: Add context to list %p -> %d\n", c->name, ptr, context);
-
-	return entry->context;
-};
-
-/**
- *      i2o_cntxt_list_remove - Remove a pointer from the context list
- *	@c: controller to which the context list belong
- *	@ptr: pointer which should be removed from the context list
- *
- *	Removes a previously added pointer from the context list and returns
- *	the matching context id.
- *
- *	Returns context id on success or 0 on failure.
- */
-u32 i2o_cntxt_list_remove(struct i2o_controller * c, void *ptr)
-{
-	struct i2o_context_list_element *entry;
-	u32 context = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&c->context_list_lock, flags);
-	list_for_each_entry(entry, &c->context_list, list)
-	    if (entry->ptr == ptr) {
-		list_del(&entry->list);
-		context = entry->context;
-		kfree(entry);
-		break;
-	}
-	spin_unlock_irqrestore(&c->context_list_lock, flags);
-
-	if (!context)
-		osm_warn("%s: Could not remove nonexistent ptr %p\n", c->name,
-			 ptr);
-
-	osm_debug("%s: remove ptr from context list %d -> %p\n", c->name,
-		  context, ptr);
-
-	return context;
-};
-
-/**
- *      i2o_cntxt_list_get - Get a pointer from the context list and remove it
- *	@c: controller to which the context list belong
- *	@context: context id to which the pointer belong
- *
- *	Returns pointer to the matching context id on success or NULL on
- *	failure.
- */
-void *i2o_cntxt_list_get(struct i2o_controller *c, u32 context)
-{
-	struct i2o_context_list_element *entry;
-	unsigned long flags;
-	void *ptr = NULL;
-
-	spin_lock_irqsave(&c->context_list_lock, flags);
-	list_for_each_entry(entry, &c->context_list, list)
-	    if (entry->context == context) {
-		list_del(&entry->list);
-		ptr = entry->ptr;
-		kfree(entry);
-		break;
-	}
-	spin_unlock_irqrestore(&c->context_list_lock, flags);
-
-	if (!ptr)
-		osm_warn("%s: context id %d not found\n", c->name, context);
-
-	osm_debug("%s: get ptr from context list %d -> %p\n", c->name, context,
-		  ptr);
-
-	return ptr;
-};
-
-/**
- *      i2o_cntxt_list_get_ptr - Get a context id from the context list
- *	@c: controller to which the context list belong
- *	@ptr: pointer to which the context id should be fetched
- *
- *	Returns context id which matches to the pointer on success or 0 on
- *	failure.
- */
-u32 i2o_cntxt_list_get_ptr(struct i2o_controller * c, void *ptr)
-{
-	struct i2o_context_list_element *entry;
-	u32 context = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&c->context_list_lock, flags);
-	list_for_each_entry(entry, &c->context_list, list)
-	    if (entry->ptr == ptr) {
-		context = entry->context;
-		break;
-	}
-	spin_unlock_irqrestore(&c->context_list_lock, flags);
-
-	if (!context)
-		osm_warn("%s: Could not find nonexistent ptr %p\n", c->name,
-			 ptr);
-
-	osm_debug("%s: get context id from context list %p -> %d\n", c->name,
-		  ptr, context);
-
-	return context;
-};
-#endif
-
-/**
- *	i2o_iop_find - Find an I2O controller by id
- *	@unit: unit number of the I2O controller to search for
- *
- *	Lookup the I2O controller on the controller list.
- *
- *	Returns pointer to the I2O controller on success or NULL if not found.
- */
-struct i2o_controller *i2o_find_iop(int unit)
-{
-	struct i2o_controller *c;
-
-	list_for_each_entry(c, &i2o_controllers, list) {
-		if (c->unit == unit)
-			return c;
-	}
-
-	return NULL;
-};
-
-/**
- *	i2o_iop_find_device - Find a I2O device on an I2O controller
- *	@c: I2O controller where the I2O device hangs on
- *	@tid: TID of the I2O device to search for
- *
- *	Searches the devices of the I2O controller for a device with TID tid and
- *	returns it.
- *
- *	Returns a pointer to the I2O device if found, otherwise NULL.
- */
-struct i2o_device *i2o_iop_find_device(struct i2o_controller *c, u16 tid)
-{
-	struct i2o_device *dev;
-
-	list_for_each_entry(dev, &c->devices, list)
-	    if (dev->lct_data.tid == tid)
-		return dev;
-
-	return NULL;
-};
-
-/**
- *	i2o_quiesce_controller - quiesce controller
- *	@c: controller
- *
- *	Quiesce an IOP. Causes IOP to make external operation quiescent
- *	(i2o 'READY' state). Internal operation of the IOP continues normally.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_iop_quiesce(struct i2o_controller *c)
-{
-	struct i2o_message *msg;
-	i2o_status_block *sb = c->status_block.virt;
-	int rc;
-
-	i2o_status_get(c);
-
-	/* SysQuiesce discarded if IOP not in READY or OPERATIONAL state */
-	if ((sb->iop_state != ADAPTER_STATE_READY) &&
-	    (sb->iop_state != ADAPTER_STATE_OPERATIONAL))
-		return 0;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FOUR_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_SYS_QUIESCE << 24 | HOST_TID << 12 |
-			ADAPTER_TID);
-
-	/* Long timeout needed for quiesce if lots of devices */
-	if ((rc = i2o_msg_post_wait(c, msg, 240)))
-		osm_info("%s: Unable to quiesce (status=%#x).\n", c->name, -rc);
-	else
-		osm_debug("%s: Quiesced.\n", c->name);
-
-	i2o_status_get(c);	// Entered READY state
-
-	return rc;
-};
-
-/**
- *	i2o_iop_enable - move controller from ready to OPERATIONAL
- *	@c: I2O controller
- *
- *	Enable IOP. This allows the IOP to resume external operations and
- *	reverses the effect of a quiesce. Returns zero or an error code if
- *	an error occurs.
- */
-static int i2o_iop_enable(struct i2o_controller *c)
-{
-	struct i2o_message *msg;
-	i2o_status_block *sb = c->status_block.virt;
-	int rc;
-
-	i2o_status_get(c);
-
-	/* Enable only allowed on READY state */
-	if (sb->iop_state != ADAPTER_STATE_READY)
-		return -EINVAL;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FOUR_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_SYS_ENABLE << 24 | HOST_TID << 12 |
-			ADAPTER_TID);
-
-	/* How long of a timeout do we need? */
-	if ((rc = i2o_msg_post_wait(c, msg, 240)))
-		osm_err("%s: Could not enable (status=%#x).\n", c->name, -rc);
-	else
-		osm_debug("%s: Enabled.\n", c->name);
-
-	i2o_status_get(c);	// entered OPERATIONAL state
-
-	return rc;
-};
-
-/**
- *	i2o_iop_quiesce_all - Quiesce all I2O controllers on the system
- *
- *	Quiesce all I2O controllers which are connected to the system.
- */
-static inline void i2o_iop_quiesce_all(void)
-{
-	struct i2o_controller *c, *tmp;
-
-	list_for_each_entry_safe(c, tmp, &i2o_controllers, list) {
-		if (!c->no_quiesce)
-			i2o_iop_quiesce(c);
-	}
-};
-
-/**
- *	i2o_iop_enable_all - Enables all controllers on the system
- *
- *	Enables all I2O controllers which are connected to the system.
- */
-static inline void i2o_iop_enable_all(void)
-{
-	struct i2o_controller *c, *tmp;
-
-	list_for_each_entry_safe(c, tmp, &i2o_controllers, list)
-	    i2o_iop_enable(c);
-};
-
-/**
- *	i2o_clear_controller - Bring I2O controller into HOLD state
- *	@c: controller
- *
- *	Clear an IOP to HOLD state, ie. terminate external operations, clear all
- *	input queues and prepare for a system restart. IOP's internal operation
- *	continues normally and the outbound queue is alive. The IOP is not
- *	expected to rebuild its LCT.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_iop_clear(struct i2o_controller *c)
-{
-	struct i2o_message *msg;
-	int rc;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	/* Quiesce all IOPs first */
-	i2o_iop_quiesce_all();
-
-	msg->u.head[0] = cpu_to_le32(FOUR_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_ADAPTER_CLEAR << 24 | HOST_TID << 12 |
-			ADAPTER_TID);
-
-	if ((rc = i2o_msg_post_wait(c, msg, 30)))
-		osm_info("%s: Unable to clear (status=%#x).\n", c->name, -rc);
-	else
-		osm_debug("%s: Cleared.\n", c->name);
-
-	/* Enable all IOPs */
-	i2o_iop_enable_all();
-
-	return rc;
-}
-
-/**
- *	i2o_iop_init_outbound_queue - setup the outbound message queue
- *	@c: I2O controller
- *
- *	Clear and (re)initialize IOP's outbound queue and post the message
- *	frames to the IOP.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_iop_init_outbound_queue(struct i2o_controller *c)
-{
-	u32 m;
-	volatile u8 *status = c->status.virt;
-	struct i2o_message *msg;
-	ulong timeout;
-	int i;
-
-	osm_debug("%s: Initializing Outbound Queue...\n", c->name);
-
-	memset(c->status.virt, 0, 4);
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(EIGHT_WORD_MSG_SIZE | SGL_OFFSET_6);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_OUTBOUND_INIT << 24 | HOST_TID << 12 |
-			ADAPTER_TID);
-	msg->u.s.icntxt = cpu_to_le32(i2o_exec_driver.context);
-	msg->u.s.tcntxt = cpu_to_le32(0x00000000);
-	msg->body[0] = cpu_to_le32(PAGE_SIZE);
-	/* Outbound msg frame size in words and Initcode */
-	msg->body[1] = cpu_to_le32(I2O_OUTBOUND_MSG_FRAME_SIZE << 16 | 0x80);
-	msg->body[2] = cpu_to_le32(0xd0000004);
-	msg->body[3] = cpu_to_le32(i2o_dma_low(c->status.phys));
-	msg->body[4] = cpu_to_le32(i2o_dma_high(c->status.phys));
-
-	i2o_msg_post(c, msg);
-
-	timeout = jiffies + I2O_TIMEOUT_INIT_OUTBOUND_QUEUE * HZ;
-	while (*status <= I2O_CMD_IN_PROGRESS) {
-		if (time_after(jiffies, timeout)) {
-			osm_warn("%s: Timeout Initializing\n", c->name);
-			return -ETIMEDOUT;
-		}
-		schedule_timeout_uninterruptible(1);
-	}
-
-	m = c->out_queue.phys;
-
-	/* Post frames */
-	for (i = 0; i < I2O_MAX_OUTBOUND_MSG_FRAMES; i++) {
-		i2o_flush_reply(c, m);
-		udelay(1);	/* Promise */
-		m += I2O_OUTBOUND_MSG_FRAME_SIZE * sizeof(u32);
-	}
-
-	return 0;
-}
-
-/**
- *	i2o_iop_reset - reset an I2O controller
- *	@c: controller to reset
- *
- *	Reset the IOP into INIT state and wait until IOP gets into RESET state.
- *	Terminate all external operations, clear IOP's inbound and outbound
- *	queues, terminate all DDMs, and reload the IOP's operating environment
- *	and all local DDMs. The IOP rebuilds its LCT.
- */
-static int i2o_iop_reset(struct i2o_controller *c)
-{
-	volatile u8 *status = c->status.virt;
-	struct i2o_message *msg;
-	unsigned long timeout;
-	i2o_status_block *sb = c->status_block.virt;
-	int rc = 0;
-
-	osm_debug("%s: Resetting controller\n", c->name);
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	memset(c->status_block.virt, 0, 8);
-
-	/* Quiesce all IOPs first */
-	i2o_iop_quiesce_all();
-
-	msg->u.head[0] = cpu_to_le32(EIGHT_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_ADAPTER_RESET << 24 | HOST_TID << 12 |
-			ADAPTER_TID);
-	msg->u.s.icntxt = cpu_to_le32(i2o_exec_driver.context);
-	msg->u.s.tcntxt = cpu_to_le32(0x00000000);
-	msg->body[0] = cpu_to_le32(0x00000000);
-	msg->body[1] = cpu_to_le32(0x00000000);
-	msg->body[2] = cpu_to_le32(i2o_dma_low(c->status.phys));
-	msg->body[3] = cpu_to_le32(i2o_dma_high(c->status.phys));
-
-	i2o_msg_post(c, msg);
-
-	/* Wait for a reply */
-	timeout = jiffies + I2O_TIMEOUT_RESET * HZ;
-	while (!*status) {
-		if (time_after(jiffies, timeout))
-			break;
-
-		schedule_timeout_uninterruptible(1);
-	}
-
-	switch (*status) {
-	case I2O_CMD_REJECTED:
-		osm_warn("%s: IOP reset rejected\n", c->name);
-		rc = -EPERM;
-		break;
-
-	case I2O_CMD_IN_PROGRESS:
-		/*
-		 * Once the reset is sent, the IOP goes into the INIT state
-		 * which is indeterminate. We need to wait until the IOP has
-		 * rebooted before we can let the system talk to it. We read
-		 * the inbound Free_List until a message is available. If we
-		 * can't read one in the given amount of time, we assume the
-		 * IOP could not reboot properly.
-		 */
-		osm_debug("%s: Reset in progress, waiting for reboot...\n",
-			  c->name);
-
-		while (IS_ERR(msg = i2o_msg_get_wait(c, I2O_TIMEOUT_RESET))) {
-			if (time_after(jiffies, timeout)) {
-				osm_err("%s: IOP reset timeout.\n", c->name);
-				rc = PTR_ERR(msg);
-				goto exit;
-			}
-			schedule_timeout_uninterruptible(1);
-		}
-		i2o_msg_nop(c, msg);
-
-		/* from here all quiesce commands are safe */
-		c->no_quiesce = 0;
-
-		/* verify if controller is in state RESET */
-		i2o_status_get(c);
-
-		if (!c->promise && (sb->iop_state != ADAPTER_STATE_RESET))
-			osm_warn("%s: reset completed, but adapter not in RESET"
-				 " state.\n", c->name);
-		else
-			osm_debug("%s: reset completed.\n", c->name);
-
-		break;
-
-	default:
-		osm_err("%s: IOP reset timeout.\n", c->name);
-		rc = -ETIMEDOUT;
-		break;
-	}
-
-      exit:
-	/* Enable all IOPs */
-	i2o_iop_enable_all();
-
-	return rc;
-};
-
-/**
- *	i2o_iop_activate - Bring controller up to HOLD
- *	@c: controller
- *
- *	This function brings an I2O controller into HOLD state. The adapter
- *	is reset if necessary and then the queues and resource table are read.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_iop_activate(struct i2o_controller *c)
-{
-	i2o_status_block *sb = c->status_block.virt;
-	int rc;
-	int state;
-
-	/* In INIT state, Wait Inbound Q to initialize (in i2o_status_get) */
-	/* In READY state, Get status */
-
-	rc = i2o_status_get(c);
-	if (rc) {
-		osm_info("%s: Unable to obtain status, attempting a reset.\n",
-			 c->name);
-		rc = i2o_iop_reset(c);
-		if (rc)
-			return rc;
-	}
-
-	if (sb->i2o_version > I2OVER15) {
-		osm_err("%s: Not running version 1.5 of the I2O Specification."
-			"\n", c->name);
-		return -ENODEV;
-	}
-
-	switch (sb->iop_state) {
-	case ADAPTER_STATE_FAULTED:
-		osm_err("%s: hardware fault\n", c->name);
-		return -EFAULT;
-
-	case ADAPTER_STATE_READY:
-	case ADAPTER_STATE_OPERATIONAL:
-	case ADAPTER_STATE_HOLD:
-	case ADAPTER_STATE_FAILED:
-		osm_debug("%s: already running, trying to reset...\n", c->name);
-		rc = i2o_iop_reset(c);
-		if (rc)
-			return rc;
-	}
-
-	/* preserve state */
-	state = sb->iop_state;
-
-	rc = i2o_iop_init_outbound_queue(c);
-	if (rc)
-		return rc;
-
-	/* if adapter was not in RESET state clear now */
-	if (state != ADAPTER_STATE_RESET)
-		i2o_iop_clear(c);
-
-	i2o_status_get(c);
-
-	if (sb->iop_state != ADAPTER_STATE_HOLD) {
-		osm_err("%s: failed to bring IOP into HOLD state\n", c->name);
-		return -EIO;
-	}
-
-	return i2o_hrt_get(c);
-};
-
-static void i2o_res_alloc(struct i2o_controller *c, unsigned long flags)
-{
-	i2o_status_block *sb = c->status_block.virt;
-	struct resource *res = &c->mem_resource;
-	resource_size_t size, align;
-	int err;
-
-	res->name = c->pdev->bus->name;
-	res->flags = flags;
-	res->start = 0;
-	res->end = 0;
-	osm_info("%s: requires private memory resources.\n", c->name);
-
-	if (flags & IORESOURCE_MEM) {
-		size = sb->desired_mem_size;
-		align = 1 << 20;	/* unspecified, use 1Mb and play safe */
-	} else {
-		size = sb->desired_io_size;
-		align = 1 << 12;	/* unspecified, use 4Kb and play safe */
-	}
-
-	err = pci_bus_alloc_resource(c->pdev->bus, res, size, align, 0, 0,
-				     NULL, NULL);
-	if (err < 0)
-		return;
-
-	if (flags & IORESOURCE_MEM) {
-		c->mem_alloc = 1;
-		sb->current_mem_size = resource_size(res);
-		sb->current_mem_base = res->start;
-	} else if (flags & IORESOURCE_IO) {
-		c->io_alloc = 1;
-		sb->current_io_size = resource_size(res);
-		sb->current_io_base = res->start;
-	}
-	osm_info("%s: allocated PCI space %pR\n", c->name, res);
-}
-
-/**
- *	i2o_iop_systab_set - Set the I2O System Table of the specified IOP
- *	@c: I2O controller to which the system table should be send
- *
- *	Before the systab could be set i2o_systab_build() must be called.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_iop_systab_set(struct i2o_controller *c)
-{
-	struct i2o_message *msg;
-	i2o_status_block *sb = c->status_block.virt;
-	struct device *dev = &c->pdev->dev;
-	int rc;
-
-	if (sb->current_mem_size < sb->desired_mem_size)
-		i2o_res_alloc(c, IORESOURCE_MEM);
-
-	if (sb->current_io_size < sb->desired_io_size)
-		i2o_res_alloc(c, IORESOURCE_IO);
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	i2o_systab.phys = dma_map_single(dev, i2o_systab.virt, i2o_systab.len,
-					 PCI_DMA_TODEVICE);
-	if (!i2o_systab.phys) {
-		i2o_msg_nop(c, msg);
-		return -ENOMEM;
-	}
-
-	msg->u.head[0] = cpu_to_le32(I2O_MESSAGE_SIZE(12) | SGL_OFFSET_6);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_SYS_TAB_SET << 24 | HOST_TID << 12 |
-			ADAPTER_TID);
-
-	/*
-	 * Provide three SGL-elements:
-	 * System table (SysTab), Private memory space declaration and
-	 * Private i/o space declaration
-	 */
-
-	msg->body[0] = cpu_to_le32(c->unit + 2);
-	msg->body[1] = cpu_to_le32(0x00000000);
-	msg->body[2] = cpu_to_le32(0x54000000 | i2o_systab.len);
-	msg->body[3] = cpu_to_le32(i2o_systab.phys);
-	msg->body[4] = cpu_to_le32(0x54000000 | sb->current_mem_size);
-	msg->body[5] = cpu_to_le32(sb->current_mem_base);
-	msg->body[6] = cpu_to_le32(0xd4000000 | sb->current_io_size);
-	msg->body[6] = cpu_to_le32(sb->current_io_base);
-
-	rc = i2o_msg_post_wait(c, msg, 120);
-
-	dma_unmap_single(dev, i2o_systab.phys, i2o_systab.len,
-			 PCI_DMA_TODEVICE);
-
-	if (rc < 0)
-		osm_err("%s: Unable to set SysTab (status=%#x).\n", c->name,
-			-rc);
-	else
-		osm_debug("%s: SysTab set.\n", c->name);
-
-	return rc;
-}
-
-/**
- *	i2o_iop_online - Bring a controller online into OPERATIONAL state.
- *	@c: I2O controller
- *
- *	Send the system table and enable the I2O controller.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_iop_online(struct i2o_controller *c)
-{
-	int rc;
-
-	rc = i2o_iop_systab_set(c);
-	if (rc)
-		return rc;
-
-	/* In READY state */
-	osm_debug("%s: Attempting to enable...\n", c->name);
-	rc = i2o_iop_enable(c);
-	if (rc)
-		return rc;
-
-	return 0;
-};
-
-/**
- *	i2o_iop_remove - Remove the I2O controller from the I2O core
- *	@c: I2O controller
- *
- *	Remove the I2O controller from the I2O core. If devices are attached to
- *	the controller remove these also and finally reset the controller.
- */
-void i2o_iop_remove(struct i2o_controller *c)
-{
-	struct i2o_device *dev, *tmp;
-
-	osm_debug("%s: deleting controller\n", c->name);
-
-	i2o_driver_notify_controller_remove_all(c);
-
-	list_del(&c->list);
-
-	list_for_each_entry_safe(dev, tmp, &c->devices, list)
-	    i2o_device_remove(dev);
-
-	device_del(&c->device);
-
-	/* Ask the IOP to switch to RESET state */
-	i2o_iop_reset(c);
-}
-
-/**
- *	i2o_systab_build - Build system table
- *
- *	The system table contains information about all the IOPs in the system
- *	(duh) and is used by the Executives on the IOPs to establish peer2peer
- *	connections. We're not supporting peer2peer at the moment, but this
- *	will be needed down the road for things like lan2lan forwarding.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_systab_build(void)
-{
-	struct i2o_controller *c, *tmp;
-	int num_controllers = 0;
-	u32 change_ind = 0;
-	int count = 0;
-	struct i2o_sys_tbl *systab = i2o_systab.virt;
-
-	list_for_each_entry_safe(c, tmp, &i2o_controllers, list)
-	    num_controllers++;
-
-	if (systab) {
-		change_ind = systab->change_ind;
-		kfree(i2o_systab.virt);
-	}
-
-	/* Header + IOPs */
-	i2o_systab.len = sizeof(struct i2o_sys_tbl) + num_controllers *
-	    sizeof(struct i2o_sys_tbl_entry);
-
-	systab = i2o_systab.virt = kzalloc(i2o_systab.len, GFP_KERNEL);
-	if (!systab) {
-		osm_err("unable to allocate memory for System Table\n");
-		return -ENOMEM;
-	}
-
-	systab->version = I2OVERSION;
-	systab->change_ind = change_ind + 1;
-
-	list_for_each_entry_safe(c, tmp, &i2o_controllers, list) {
-		i2o_status_block *sb;
-
-		if (count >= num_controllers) {
-			osm_err("controller added while building system table"
-				"\n");
-			break;
-		}
-
-		sb = c->status_block.virt;
-
-		/*
-		 * Get updated IOP state so we have the latest information
-		 *
-		 * We should delete the controller at this point if it
-		 * doesn't respond since if it's not on the system table
-		 * it is techninically not part of the I2O subsystem...
-		 */
-		if (unlikely(i2o_status_get(c))) {
-			osm_err("%s: Deleting b/c could not get status while "
-				"attempting to build system table\n", c->name);
-			i2o_iop_remove(c);
-			continue;	// try the next one
-		}
-
-		systab->iops[count].org_id = sb->org_id;
-		systab->iops[count].iop_id = c->unit + 2;
-		systab->iops[count].seg_num = 0;
-		systab->iops[count].i2o_version = sb->i2o_version;
-		systab->iops[count].iop_state = sb->iop_state;
-		systab->iops[count].msg_type = sb->msg_type;
-		systab->iops[count].frame_size = sb->inbound_frame_size;
-		systab->iops[count].last_changed = change_ind;
-		systab->iops[count].iop_capabilities = sb->iop_capabilities;
-		systab->iops[count].inbound_low =
-		    i2o_dma_low(c->base.phys + I2O_IN_PORT);
-		systab->iops[count].inbound_high =
-		    i2o_dma_high(c->base.phys + I2O_IN_PORT);
-
-		count++;
-	}
-
-	systab->num_entries = count;
-
-	return 0;
-};
-
-/**
- *	i2o_parse_hrt - Parse the hardware resource table.
- *	@c: I2O controller
- *
- *	We don't do anything with it except dumping it (in debug mode).
- *
- *	Returns 0.
- */
-static int i2o_parse_hrt(struct i2o_controller *c)
-{
-	i2o_dump_hrt(c);
-	return 0;
-};
-
-/**
- *	i2o_status_get - Get the status block from the I2O controller
- *	@c: I2O controller
- *
- *	Issue a status query on the controller. This updates the attached
- *	status block. The status block could then be accessed through
- *	c->status_block.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int i2o_status_get(struct i2o_controller *c)
-{
-	struct i2o_message *msg;
-	volatile u8 *status_block;
-	unsigned long timeout;
-
-	status_block = (u8 *) c->status_block.virt;
-	memset(c->status_block.virt, 0, sizeof(i2o_status_block));
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(NINE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_STATUS_GET << 24 | HOST_TID << 12 |
-			ADAPTER_TID);
-	msg->u.s.icntxt = cpu_to_le32(i2o_exec_driver.context);
-	msg->u.s.tcntxt = cpu_to_le32(0x00000000);
-	msg->body[0] = cpu_to_le32(0x00000000);
-	msg->body[1] = cpu_to_le32(0x00000000);
-	msg->body[2] = cpu_to_le32(i2o_dma_low(c->status_block.phys));
-	msg->body[3] = cpu_to_le32(i2o_dma_high(c->status_block.phys));
-	msg->body[4] = cpu_to_le32(sizeof(i2o_status_block));	/* always 88 bytes */
-
-	i2o_msg_post(c, msg);
-
-	/* Wait for a reply */
-	timeout = jiffies + I2O_TIMEOUT_STATUS_GET * HZ;
-	while (status_block[87] != 0xFF) {
-		if (time_after(jiffies, timeout)) {
-			osm_err("%s: Get status timeout.\n", c->name);
-			return -ETIMEDOUT;
-		}
-
-		schedule_timeout_uninterruptible(1);
-	}
-
-#ifdef DEBUG
-	i2o_debug_state(c);
-#endif
-
-	return 0;
-}
-
-/*
- *	i2o_hrt_get - Get the Hardware Resource Table from the I2O controller
- *	@c: I2O controller from which the HRT should be fetched
- *
- *	The HRT contains information about possible hidden devices but is
- *	mostly useless to us.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_hrt_get(struct i2o_controller *c)
-{
-	int rc;
-	int i;
-	i2o_hrt *hrt = c->hrt.virt;
-	u32 size = sizeof(i2o_hrt);
-	struct device *dev = &c->pdev->dev;
-
-	for (i = 0; i < I2O_HRT_GET_TRIES; i++) {
-		struct i2o_message *msg;
-
-		msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-		if (IS_ERR(msg))
-			return PTR_ERR(msg);
-
-		msg->u.head[0] = cpu_to_le32(SIX_WORD_MSG_SIZE | SGL_OFFSET_4);
-		msg->u.head[1] =
-		    cpu_to_le32(I2O_CMD_HRT_GET << 24 | HOST_TID << 12 |
-				ADAPTER_TID);
-		msg->body[0] = cpu_to_le32(0xd0000000 | c->hrt.len);
-		msg->body[1] = cpu_to_le32(c->hrt.phys);
-
-		rc = i2o_msg_post_wait_mem(c, msg, 20, &c->hrt);
-
-		if (rc < 0) {
-			osm_err("%s: Unable to get HRT (status=%#x)\n", c->name,
-				-rc);
-			return rc;
-		}
-
-		size = hrt->num_entries * hrt->entry_len << 2;
-		if (size > c->hrt.len) {
-			if (i2o_dma_realloc(dev, &c->hrt, size))
-				return -ENOMEM;
-			else
-				hrt = c->hrt.virt;
-		} else
-			return i2o_parse_hrt(c);
-	}
-
-	osm_err("%s: Unable to get HRT after %d tries, giving up\n", c->name,
-		I2O_HRT_GET_TRIES);
-
-	return -EBUSY;
-}
-
-/**
- *	i2o_iop_release - release the memory for a I2O controller
- *	@dev: I2O controller which should be released
- *
- *	Release the allocated memory. This function is called if refcount of
- *	device reaches 0 automatically.
- */
-static void i2o_iop_release(struct device *dev)
-{
-	struct i2o_controller *c = to_i2o_controller(dev);
-
-	i2o_iop_free(c);
-};
-
-/**
- *	i2o_iop_alloc - Allocate and initialize a i2o_controller struct
- *
- *	Allocate the necessary memory for a i2o_controller struct and
- *	initialize the lists and message mempool.
- *
- *	Returns a pointer to the I2O controller or a negative error code on
- *	failure.
- */
-struct i2o_controller *i2o_iop_alloc(void)
-{
-	static int unit;	/* 0 and 1 are NULL IOP and Local Host */
-	struct i2o_controller *c;
-	char poolname[32];
-
-	c = kzalloc(sizeof(*c), GFP_KERNEL);
-	if (!c) {
-		osm_err("i2o: Insufficient memory to allocate a I2O controller."
-			"\n");
-		return ERR_PTR(-ENOMEM);
-	}
-
-	c->unit = unit++;
-	sprintf(c->name, "iop%d", c->unit);
-
-	snprintf(poolname, sizeof(poolname), "i2o_%s_msg_inpool", c->name);
-	if (i2o_pool_alloc
-	    (&c->in_msg, poolname, I2O_INBOUND_MSG_FRAME_SIZE * 4 + sizeof(u32),
-	     I2O_MSG_INPOOL_MIN)) {
-		kfree(c);
-		return ERR_PTR(-ENOMEM);
-	};
-
-	INIT_LIST_HEAD(&c->devices);
-	spin_lock_init(&c->lock);
-	mutex_init(&c->lct_lock);
-
-	device_initialize(&c->device);
-
-	c->device.release = &i2o_iop_release;
-
-	dev_set_name(&c->device, "iop%d", c->unit);
-
-#if BITS_PER_LONG == 64
-	spin_lock_init(&c->context_list_lock);
-	atomic_set(&c->context_list_counter, 0);
-	INIT_LIST_HEAD(&c->context_list);
-#endif
-
-	return c;
-};
-
-/**
- *	i2o_iop_add - Initialize the I2O controller and add him to the I2O core
- *	@c: controller
- *
- *	Initialize the I2O controller and if no error occurs add him to the I2O
- *	core.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int i2o_iop_add(struct i2o_controller *c)
-{
-	int rc;
-
-	rc = device_add(&c->device);
-	if (rc) {
-		osm_err("%s: could not add controller\n", c->name);
-		goto iop_reset;
-	}
-
-	osm_info("%s: Activating I2O controller...\n", c->name);
-	osm_info("%s: This may take a few minutes if there are many devices\n",
-		 c->name);
-
-	rc = i2o_iop_activate(c);
-	if (rc) {
-		osm_err("%s: could not activate controller\n", c->name);
-		goto device_del;
-	}
-
-	osm_debug("%s: building sys table...\n", c->name);
-
-	rc = i2o_systab_build();
-	if (rc)
-		goto device_del;
-
-	osm_debug("%s: online controller...\n", c->name);
-
-	rc = i2o_iop_online(c);
-	if (rc)
-		goto device_del;
-
-	osm_debug("%s: getting LCT...\n", c->name);
-
-	rc = i2o_exec_lct_get(c);
-	if (rc)
-		goto device_del;
-
-	list_add(&c->list, &i2o_controllers);
-
-	i2o_driver_notify_controller_add_all(c);
-
-	osm_info("%s: Controller added\n", c->name);
-
-	return 0;
-
-      device_del:
-	device_del(&c->device);
-
-      iop_reset:
-	i2o_iop_reset(c);
-
-	return rc;
-};
-
-/**
- *	i2o_event_register - Turn on/off event notification for a I2O device
- *	@dev: I2O device which should receive the event registration request
- *	@drv: driver which want to get notified
- *	@tcntxt: transaction context to use with this notifier
- *	@evt_mask: mask of events
- *
- *	Create and posts an event registration message to the task. No reply
- *	is waited for, or expected. If you do not want further notifications,
- *	call the i2o_event_register again with a evt_mask of 0.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int i2o_event_register(struct i2o_device *dev, struct i2o_driver *drv,
-		       int tcntxt, u32 evt_mask)
-{
-	struct i2o_controller *c = dev->iop;
-	struct i2o_message *msg;
-
-	msg = i2o_msg_get_wait(c, I2O_TIMEOUT_MESSAGE_GET);
-	if (IS_ERR(msg))
-		return PTR_ERR(msg);
-
-	msg->u.head[0] = cpu_to_le32(FIVE_WORD_MSG_SIZE | SGL_OFFSET_0);
-	msg->u.head[1] =
-	    cpu_to_le32(I2O_CMD_UTIL_EVT_REGISTER << 24 | HOST_TID << 12 | dev->
-			lct_data.tid);
-	msg->u.s.icntxt = cpu_to_le32(drv->context);
-	msg->u.s.tcntxt = cpu_to_le32(tcntxt);
-	msg->body[0] = cpu_to_le32(evt_mask);
-
-	i2o_msg_post(c, msg);
-
-	return 0;
-};
-
-/**
- *	i2o_iop_init - I2O main initialization function
- *
- *	Initialize the I2O drivers (OSM) functions, register the Executive OSM,
- *	initialize the I2O PCI part and finally initialize I2O device stuff.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int __init i2o_iop_init(void)
-{
-	int rc = 0;
-
-	printk(KERN_INFO OSM_DESCRIPTION " v" OSM_VERSION "\n");
-
-	rc = i2o_driver_init();
-	if (rc)
-		goto exit;
-
-	rc = i2o_exec_init();
-	if (rc)
-		goto driver_exit;
-
-	rc = i2o_pci_init();
-	if (rc)
-		goto exec_exit;
-
-	return 0;
-
-      exec_exit:
-	i2o_exec_exit();
-
-      driver_exit:
-	i2o_driver_exit();
-
-      exit:
-	return rc;
-}
-
-/**
- *	i2o_iop_exit - I2O main exit function
- *
- *	Removes I2O controllers from PCI subsystem and shut down OSMs.
- */
-static void __exit i2o_iop_exit(void)
-{
-	i2o_pci_exit();
-	i2o_exec_exit();
-	i2o_driver_exit();
-};
-
-module_init(i2o_iop_init);
-module_exit(i2o_iop_exit);
-
-MODULE_AUTHOR("Red Hat Software");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(OSM_DESCRIPTION);
-MODULE_VERSION(OSM_VERSION);
-
-#if BITS_PER_LONG == 64
-EXPORT_SYMBOL(i2o_cntxt_list_add);
-EXPORT_SYMBOL(i2o_cntxt_list_get);
-EXPORT_SYMBOL(i2o_cntxt_list_remove);
-EXPORT_SYMBOL(i2o_cntxt_list_get_ptr);
-#endif
-EXPORT_SYMBOL(i2o_msg_get_wait);
-EXPORT_SYMBOL(i2o_find_iop);
-EXPORT_SYMBOL(i2o_iop_find_device);
-EXPORT_SYMBOL(i2o_event_register);
-EXPORT_SYMBOL(i2o_status_get);
-EXPORT_SYMBOL(i2o_controllers);
diff --git a/drivers/staging/i2o/memory.c b/drivers/staging/i2o/memory.c
deleted file mode 100644
index 78b702c18537..000000000000
--- a/drivers/staging/i2o/memory.c
+++ /dev/null
@@ -1,312 +0,0 @@
-/*
- *	Functions to handle I2O memory
- *
- *	Pulled from the inlines in i2o headers and uninlined
- *
- *
- *	This program is free software; you can redistribute it and/or modify it
- *	under 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 "i2o.h"
-#include <linux/delay.h>
-#include <linux/string.h>
-#include <linux/slab.h>
-#include "core.h"
-
-/* Protects our 32/64bit mask switching */
-static DEFINE_MUTEX(mem_lock);
-
-/**
- *	i2o_sg_tablesize - Calculate the maximum number of elements in a SGL
- *	@c: I2O controller for which the calculation should be done
- *	@body_size: maximum body size used for message in 32-bit words.
- *
- *	Return the maximum number of SG elements in a SG list.
- */
-u16 i2o_sg_tablesize(struct i2o_controller *c, u16 body_size)
-{
-	i2o_status_block *sb = c->status_block.virt;
-	u16 sg_count =
-	    (sb->inbound_frame_size - sizeof(struct i2o_message) / 4) -
-	    body_size;
-
-	if (c->pae_support) {
-		/*
-		 * for 64-bit a SG attribute element must be added and each
-		 * SG element needs 12 bytes instead of 8.
-		 */
-		sg_count -= 2;
-		sg_count /= 3;
-	} else
-		sg_count /= 2;
-
-	if (c->short_req && (sg_count > 8))
-		sg_count = 8;
-
-	return sg_count;
-}
-EXPORT_SYMBOL_GPL(i2o_sg_tablesize);
-
-
-/**
- *	i2o_dma_map_single - Map pointer to controller and fill in I2O message.
- *	@c: I2O controller
- *	@ptr: pointer to the data which should be mapped
- *	@size: size of data in bytes
- *	@direction: DMA_TO_DEVICE / DMA_FROM_DEVICE
- *	@sg_ptr: pointer to the SG list inside the I2O message
- *
- *	This function does all necessary DMA handling and also writes the I2O
- *	SGL elements into the I2O message. For details on DMA handling see also
- *	dma_map_single(). The pointer sg_ptr will only be set to the end of the
- *	SG list if the allocation was successful.
- *
- *	Returns DMA address which must be checked for failures using
- *	dma_mapping_error().
- */
-dma_addr_t i2o_dma_map_single(struct i2o_controller *c, void *ptr,
-					    size_t size,
-					    enum dma_data_direction direction,
-					    u32 ** sg_ptr)
-{
-	u32 sg_flags;
-	u32 *mptr = *sg_ptr;
-	dma_addr_t dma_addr;
-
-	switch (direction) {
-	case DMA_TO_DEVICE:
-		sg_flags = 0xd4000000;
-		break;
-	case DMA_FROM_DEVICE:
-		sg_flags = 0xd0000000;
-		break;
-	default:
-		return 0;
-	}
-
-	dma_addr = dma_map_single(&c->pdev->dev, ptr, size, direction);
-	if (!dma_mapping_error(&c->pdev->dev, dma_addr)) {
-#ifdef CONFIG_I2O_EXT_ADAPTEC_DMA64
-		if ((sizeof(dma_addr_t) > 4) && c->pae_support) {
-			*mptr++ = cpu_to_le32(0x7C020002);
-			*mptr++ = cpu_to_le32(PAGE_SIZE);
-		}
-#endif
-
-		*mptr++ = cpu_to_le32(sg_flags | size);
-		*mptr++ = cpu_to_le32(i2o_dma_low(dma_addr));
-#ifdef CONFIG_I2O_EXT_ADAPTEC_DMA64
-		if ((sizeof(dma_addr_t) > 4) && c->pae_support)
-			*mptr++ = cpu_to_le32(i2o_dma_high(dma_addr));
-#endif
-		*sg_ptr = mptr;
-	}
-	return dma_addr;
-}
-EXPORT_SYMBOL_GPL(i2o_dma_map_single);
-
-/**
- *	i2o_dma_map_sg - Map a SG List to controller and fill in I2O message.
- *	@c: I2O controller
- *	@sg: SG list to be mapped
- *	@sg_count: number of elements in the SG list
- *	@direction: DMA_TO_DEVICE / DMA_FROM_DEVICE
- *	@sg_ptr: pointer to the SG list inside the I2O message
- *
- *	This function does all necessary DMA handling and also writes the I2O
- *	SGL elements into the I2O message. For details on DMA handling see also
- *	dma_map_sg(). The pointer sg_ptr will only be set to the end of the SG
- *	list if the allocation was successful.
- *
- *	Returns 0 on failure or 1 on success.
- */
-int i2o_dma_map_sg(struct i2o_controller *c, struct scatterlist *sg,
-	    int sg_count, enum dma_data_direction direction, u32 ** sg_ptr)
-{
-	u32 sg_flags;
-	u32 *mptr = *sg_ptr;
-
-	switch (direction) {
-	case DMA_TO_DEVICE:
-		sg_flags = 0x14000000;
-		break;
-	case DMA_FROM_DEVICE:
-		sg_flags = 0x10000000;
-		break;
-	default:
-		return 0;
-	}
-
-	sg_count = dma_map_sg(&c->pdev->dev, sg, sg_count, direction);
-	if (!sg_count)
-		return 0;
-
-#ifdef CONFIG_I2O_EXT_ADAPTEC_DMA64
-	if ((sizeof(dma_addr_t) > 4) && c->pae_support) {
-		*mptr++ = cpu_to_le32(0x7C020002);
-		*mptr++ = cpu_to_le32(PAGE_SIZE);
-	}
-#endif
-
-	while (sg_count-- > 0) {
-		if (!sg_count)
-			sg_flags |= 0xC0000000;
-		*mptr++ = cpu_to_le32(sg_flags | sg_dma_len(sg));
-		*mptr++ = cpu_to_le32(i2o_dma_low(sg_dma_address(sg)));
-#ifdef CONFIG_I2O_EXT_ADAPTEC_DMA64
-		if ((sizeof(dma_addr_t) > 4) && c->pae_support)
-			*mptr++ = cpu_to_le32(i2o_dma_high(sg_dma_address(sg)));
-#endif
-		sg = sg_next(sg);
-	}
-	*sg_ptr = mptr;
-
-	return 1;
-}
-EXPORT_SYMBOL_GPL(i2o_dma_map_sg);
-
-/**
- *	i2o_dma_alloc - Allocate DMA memory
- *	@dev: struct device pointer to the PCI device of the I2O controller
- *	@addr: i2o_dma struct which should get the DMA buffer
- *	@len: length of the new DMA memory
- *
- *	Allocate a coherent DMA memory and write the pointers into addr.
- *
- *	Returns 0 on success or -ENOMEM on failure.
- */
-int i2o_dma_alloc(struct device *dev, struct i2o_dma *addr, size_t len)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	int dma_64 = 0;
-
-	mutex_lock(&mem_lock);
-	if ((sizeof(dma_addr_t) > 4) && (pdev->dma_mask == DMA_BIT_MASK(64))) {
-		dma_64 = 1;
-		if (pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
-			mutex_unlock(&mem_lock);
-			return -ENOMEM;
-		}
-	}
-
-	addr->virt = dma_alloc_coherent(dev, len, &addr->phys, GFP_KERNEL);
-
-	if ((sizeof(dma_addr_t) > 4) && dma_64)
-		if (pci_set_dma_mask(pdev, DMA_BIT_MASK(64)))
-			printk(KERN_WARNING "i2o: unable to set 64-bit DMA");
-	mutex_unlock(&mem_lock);
-
-	if (!addr->virt)
-		return -ENOMEM;
-
-	memset(addr->virt, 0, len);
-	addr->len = len;
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(i2o_dma_alloc);
-
-
-/**
- *	i2o_dma_free - Free DMA memory
- *	@dev: struct device pointer to the PCI device of the I2O controller
- *	@addr: i2o_dma struct which contains the DMA buffer
- *
- *	Free a coherent DMA memory and set virtual address of addr to NULL.
- */
-void i2o_dma_free(struct device *dev, struct i2o_dma *addr)
-{
-	if (addr->virt) {
-		if (addr->phys)
-			dma_free_coherent(dev, addr->len, addr->virt,
-					  addr->phys);
-		else
-			kfree(addr->virt);
-		addr->virt = NULL;
-	}
-}
-EXPORT_SYMBOL_GPL(i2o_dma_free);
-
-
-/**
- *	i2o_dma_realloc - Realloc DMA memory
- *	@dev: struct device pointer to the PCI device of the I2O controller
- *	@addr: pointer to a i2o_dma struct DMA buffer
- *	@len: new length of memory
- *
- *	If there was something allocated in the addr, free it first. If len > 0
- *	than try to allocate it and write the addresses back to the addr
- *	structure. If len == 0 set the virtual address to NULL.
- *
- *	Returns the 0 on success or negative error code on failure.
- */
-int i2o_dma_realloc(struct device *dev, struct i2o_dma *addr, size_t len)
-{
-	i2o_dma_free(dev, addr);
-
-	if (len)
-		return i2o_dma_alloc(dev, addr, len);
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(i2o_dma_realloc);
-
-/*
- *	i2o_pool_alloc - Allocate an slab cache and mempool
- *	@mempool: pointer to struct i2o_pool to write data into.
- *	@name: name which is used to identify cache
- *	@size: size of each object
- *	@min_nr: minimum number of objects
- *
- *	First allocates a slab cache with name and size. Then allocates a
- *	mempool which uses the slab cache for allocation and freeing.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-int i2o_pool_alloc(struct i2o_pool *pool, const char *name,
-				 size_t size, int min_nr)
-{
-	pool->name = kstrdup(name, GFP_KERNEL);
-	if (!pool->name)
-		goto exit;
-
-	pool->slab =
-	    kmem_cache_create(pool->name, size, 0, SLAB_HWCACHE_ALIGN, NULL);
-	if (!pool->slab)
-		goto free_name;
-
-	pool->mempool = mempool_create_slab_pool(min_nr, pool->slab);
-	if (!pool->mempool)
-		goto free_slab;
-
-	return 0;
-
-free_slab:
-	kmem_cache_destroy(pool->slab);
-
-free_name:
-	kfree(pool->name);
-
-exit:
-	return -ENOMEM;
-}
-EXPORT_SYMBOL_GPL(i2o_pool_alloc);
-
-/*
- *	i2o_pool_free - Free slab cache and mempool again
- *	@mempool: pointer to struct i2o_pool which should be freed
- *
- *	Note that you have to return all objects to the mempool again before
- *	calling i2o_pool_free().
- */
-void i2o_pool_free(struct i2o_pool *pool)
-{
-	mempool_destroy(pool->mempool);
-	kmem_cache_destroy(pool->slab);
-	kfree(pool->name);
-};
-EXPORT_SYMBOL_GPL(i2o_pool_free);
diff --git a/drivers/staging/i2o/pci.c b/drivers/staging/i2o/pci.c
deleted file mode 100644
index 49804c9cf74f..000000000000
--- a/drivers/staging/i2o/pci.c
+++ /dev/null
@@ -1,500 +0,0 @@
-/*
- *	PCI handling of I2O controller
- *
- * 	Copyright (C) 1999-2002	Red Hat Software
- *
- *	Written by Alan Cox, Building Number Three Ltd
- *
- *	This program is free software; you can redistribute it and/or modify it
- *	under the terms of the GNU General Public License as published by the
- *	Free Software Foundation; either version 2 of the License, or (at your
- *	option) any later version.
- *
- *	A lot of the I2O message side code from this is taken from the Red
- *	Creek RCPCI45 adapter driver by Red Creek Communications
- *
- *	Fixes/additions:
- *		Philipp Rumpf
- *		Juha Sievänen <Juha.Sievanen@cs.Helsinki.FI>
- *		Auvo Häkkinen <Auvo.Hakkinen@cs.Helsinki.FI>
- *		Deepak Saxena <deepak@plexity.net>
- *		Boji T Kannanthanam <boji.t.kannanthanam@intel.com>
- *		Alan Cox <alan@lxorguk.ukuu.org.uk>:
- *			Ported to Linux 2.5.
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *			Minor fixes for 2.6.
- *		Markus Lidel <Markus.Lidel@shadowconnect.com>:
- *			Support for sysfs included.
- */
-
-#include <linux/pci.h>
-#include <linux/interrupt.h>
-#include <linux/slab.h>
-#include "i2o.h"
-#include <linux/module.h>
-#include "core.h"
-
-#define OSM_DESCRIPTION	"I2O-subsystem"
-
-/* PCI device id table for all I2O controllers */
-static struct pci_device_id i2o_pci_ids[] = {
-	{PCI_DEVICE_CLASS(PCI_CLASS_INTELLIGENT_I2O << 8, 0xffff00)},
-	{PCI_DEVICE(PCI_VENDOR_ID_DPT, 0xa511)},
-	{.vendor = PCI_VENDOR_ID_INTEL,.device = 0x1962,
-	 .subvendor = PCI_VENDOR_ID_PROMISE,.subdevice = PCI_ANY_ID},
-	{0}
-};
-
-/**
- *	i2o_pci_free - Frees the DMA memory for the I2O controller
- *	@c: I2O controller to free
- *
- *	Remove all allocated DMA memory and unmap memory IO regions. If MTRR
- *	is enabled, also remove it again.
- */
-static void i2o_pci_free(struct i2o_controller *c)
-{
-	struct device *dev;
-
-	dev = &c->pdev->dev;
-
-	i2o_dma_free(dev, &c->out_queue);
-	i2o_dma_free(dev, &c->status_block);
-	kfree(c->lct);
-	i2o_dma_free(dev, &c->dlct);
-	i2o_dma_free(dev, &c->hrt);
-	i2o_dma_free(dev, &c->status);
-
-	if (c->raptor && c->in_queue.virt)
-		iounmap(c->in_queue.virt);
-
-	if (c->base.virt)
-		iounmap(c->base.virt);
-
-	pci_release_regions(c->pdev);
-}
-
-/**
- *	i2o_pci_alloc - Allocate DMA memory, map IO memory for I2O controller
- *	@c: I2O controller
- *
- *	Allocate DMA memory for a PCI (or in theory AGP) I2O controller. All
- *	IO mappings are also done here. If MTRR is enabled, also do add memory
- *	regions here.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_pci_alloc(struct i2o_controller *c)
-{
-	struct pci_dev *pdev = c->pdev;
-	struct device *dev = &pdev->dev;
-	int i;
-
-	if (pci_request_regions(pdev, OSM_DESCRIPTION)) {
-		printk(KERN_ERR "%s: device already claimed\n", c->name);
-		return -ENODEV;
-	}
-
-	for (i = 0; i < 6; i++) {
-		/* Skip I/O spaces */
-		if (!(pci_resource_flags(pdev, i) & IORESOURCE_IO)) {
-			if (!c->base.phys) {
-				c->base.phys = pci_resource_start(pdev, i);
-				c->base.len = pci_resource_len(pdev, i);
-
-				/*
-				 * If we know what card it is, set the size
-				 * correctly. Code is taken from dpt_i2o.c
-				 */
-				if (pdev->device == 0xa501) {
-					if (pdev->subsystem_device >= 0xc032 &&
-					    pdev->subsystem_device <= 0xc03b) {
-						if (c->base.len > 0x400000)
-							c->base.len = 0x400000;
-					} else {
-						if (c->base.len > 0x100000)
-							c->base.len = 0x100000;
-					}
-				}
-				if (!c->raptor)
-					break;
-			} else {
-				c->in_queue.phys = pci_resource_start(pdev, i);
-				c->in_queue.len = pci_resource_len(pdev, i);
-				break;
-			}
-		}
-	}
-
-	if (i == 6) {
-		printk(KERN_ERR "%s: I2O controller has no memory regions"
-		       " defined.\n", c->name);
-		i2o_pci_free(c);
-		return -EINVAL;
-	}
-
-	/* Map the I2O controller */
-	if (c->raptor) {
-		printk(KERN_INFO "%s: PCI I2O controller\n", c->name);
-		printk(KERN_INFO "     BAR0 at 0x%08lX size=%ld\n",
-		       (unsigned long)c->base.phys, (unsigned long)c->base.len);
-		printk(KERN_INFO "     BAR1 at 0x%08lX size=%ld\n",
-		       (unsigned long)c->in_queue.phys,
-		       (unsigned long)c->in_queue.len);
-	} else
-		printk(KERN_INFO "%s: PCI I2O controller at %08lX size=%ld\n",
-		       c->name, (unsigned long)c->base.phys,
-		       (unsigned long)c->base.len);
-
-	c->base.virt = ioremap_nocache(c->base.phys, c->base.len);
-	if (!c->base.virt) {
-		printk(KERN_ERR "%s: Unable to map controller.\n", c->name);
-		i2o_pci_free(c);
-		return -ENOMEM;
-	}
-
-	if (c->raptor) {
-		c->in_queue.virt =
-		    ioremap_nocache(c->in_queue.phys, c->in_queue.len);
-		if (!c->in_queue.virt) {
-			printk(KERN_ERR "%s: Unable to map controller.\n",
-			       c->name);
-			i2o_pci_free(c);
-			return -ENOMEM;
-		}
-	} else
-		c->in_queue = c->base;
-
-	c->irq_status = c->base.virt + I2O_IRQ_STATUS;
-	c->irq_mask = c->base.virt + I2O_IRQ_MASK;
-	c->in_port = c->base.virt + I2O_IN_PORT;
-	c->out_port = c->base.virt + I2O_OUT_PORT;
-
-	/* Motorola/Freescale chip does not follow spec */
-	if (pdev->vendor == PCI_VENDOR_ID_MOTOROLA && pdev->device == 0x18c0) {
-		/* Check if CPU is enabled */
-		if (be32_to_cpu(readl(c->base.virt + 0x10000)) & 0x10000000) {
-			printk(KERN_INFO "%s: MPC82XX needs CPU running to "
-			       "service I2O.\n", c->name);
-			i2o_pci_free(c);
-			return -ENODEV;
-		} else {
-			c->irq_status += I2O_MOTOROLA_PORT_OFFSET;
-			c->irq_mask += I2O_MOTOROLA_PORT_OFFSET;
-			c->in_port += I2O_MOTOROLA_PORT_OFFSET;
-			c->out_port += I2O_MOTOROLA_PORT_OFFSET;
-			printk(KERN_INFO "%s: MPC82XX workarounds activated.\n",
-			       c->name);
-		}
-	}
-
-	if (i2o_dma_alloc(dev, &c->status, 8)) {
-		i2o_pci_free(c);
-		return -ENOMEM;
-	}
-
-	if (i2o_dma_alloc(dev, &c->hrt, sizeof(i2o_hrt))) {
-		i2o_pci_free(c);
-		return -ENOMEM;
-	}
-
-	if (i2o_dma_alloc(dev, &c->dlct, 8192)) {
-		i2o_pci_free(c);
-		return -ENOMEM;
-	}
-
-	if (i2o_dma_alloc(dev, &c->status_block, sizeof(i2o_status_block))) {
-		i2o_pci_free(c);
-		return -ENOMEM;
-	}
-
-	if (i2o_dma_alloc(dev, &c->out_queue,
-		I2O_MAX_OUTBOUND_MSG_FRAMES * I2O_OUTBOUND_MSG_FRAME_SIZE *
-				sizeof(u32))) {
-		i2o_pci_free(c);
-		return -ENOMEM;
-	}
-
-	pci_set_drvdata(pdev, c);
-
-	return 0;
-}
-
-/**
- *	i2o_pci_interrupt - Interrupt handler for I2O controller
- *	@irq: interrupt line
- *	@dev_id: pointer to the I2O controller
- *
- *	Handle an interrupt from a PCI based I2O controller. This turns out
- *	to be rather simple. We keep the controller pointer in the cookie.
- */
-static irqreturn_t i2o_pci_interrupt(int irq, void *dev_id)
-{
-	struct i2o_controller *c = dev_id;
-	u32 m;
-	irqreturn_t rc = IRQ_NONE;
-
-	while (readl(c->irq_status) & I2O_IRQ_OUTBOUND_POST) {
-		m = readl(c->out_port);
-		if (m == I2O_QUEUE_EMPTY) {
-			/*
-			 * Old 960 steppings had a bug in the I2O unit that
-			 * caused the queue to appear empty when it wasn't.
-			 */
-			m = readl(c->out_port);
-			if (unlikely(m == I2O_QUEUE_EMPTY))
-				break;
-		}
-
-		/* dispatch it */
-		if (i2o_driver_dispatch(c, m))
-			/* flush it if result != 0 */
-			i2o_flush_reply(c, m);
-
-		rc = IRQ_HANDLED;
-	}
-
-	return rc;
-}
-
-/**
- *	i2o_pci_irq_enable - Allocate interrupt for I2O controller
- *	@c: i2o_controller that the request is for
- *
- *	Allocate an interrupt for the I2O controller, and activate interrupts
- *	on the I2O controller.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_pci_irq_enable(struct i2o_controller *c)
-{
-	struct pci_dev *pdev = c->pdev;
-	int rc;
-
-	writel(0xffffffff, c->irq_mask);
-
-	if (pdev->irq) {
-		rc = request_irq(pdev->irq, i2o_pci_interrupt, IRQF_SHARED,
-				 c->name, c);
-		if (rc < 0) {
-			printk(KERN_ERR "%s: unable to allocate interrupt %d."
-			       "\n", c->name, pdev->irq);
-			return rc;
-		}
-	}
-
-	writel(0x00000000, c->irq_mask);
-
-	printk(KERN_INFO "%s: Installed at IRQ %d\n", c->name, pdev->irq);
-
-	return 0;
-}
-
-/**
- *	i2o_pci_irq_disable - Free interrupt for I2O controller
- *	@c: I2O controller
- *
- *	Disable interrupts in I2O controller and then free interrupt.
- */
-static void i2o_pci_irq_disable(struct i2o_controller *c)
-{
-	writel(0xffffffff, c->irq_mask);
-
-	if (c->pdev->irq > 0)
-		free_irq(c->pdev->irq, c);
-}
-
-/**
- *	i2o_pci_probe - Probe the PCI device for an I2O controller
- *	@pdev: PCI device to test
- *	@id: id which matched with the PCI device id table
- *
- *	Probe the PCI device for any device which is a memory of the
- *	Intelligent, I2O class or an Adaptec Zero Channel Controller. We
- *	attempt to set up each such device and register it with the core.
- *
- *	Returns 0 on success or negative error code on failure.
- */
-static int i2o_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
-{
-	struct i2o_controller *c;
-	int rc;
-	struct pci_dev *i960 = NULL;
-
-	printk(KERN_INFO "i2o: Checking for PCI I2O controllers...\n");
-
-	if ((pdev->class & 0xff) > 1) {
-		printk(KERN_WARNING "i2o: %s does not support I2O 1.5 "
-		       "(skipping).\n", pci_name(pdev));
-		return -ENODEV;
-	}
-
-	rc = pci_enable_device(pdev);
-	if (rc) {
-		printk(KERN_WARNING "i2o: couldn't enable device %s\n",
-		       pci_name(pdev));
-		return rc;
-	}
-
-	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
-		printk(KERN_WARNING "i2o: no suitable DMA found for %s\n",
-		       pci_name(pdev));
-		rc = -ENODEV;
-		goto disable;
-	}
-
-	pci_set_master(pdev);
-
-	c = i2o_iop_alloc();
-	if (IS_ERR(c)) {
-		printk(KERN_ERR "i2o: couldn't allocate memory for %s\n",
-		       pci_name(pdev));
-		rc = PTR_ERR(c);
-		goto disable;
-	} else
-		printk(KERN_INFO "%s: controller found (%s)\n", c->name,
-		       pci_name(pdev));
-
-	c->pdev = pdev;
-	c->device.parent = &pdev->dev;
-
-	/* Cards that fall apart if you hit them with large I/O loads... */
-	if (pdev->vendor == PCI_VENDOR_ID_NCR && pdev->device == 0x0630) {
-		c->short_req = 1;
-		printk(KERN_INFO "%s: Symbios FC920 workarounds activated.\n",
-		       c->name);
-	}
-
-	if (pdev->subsystem_vendor == PCI_VENDOR_ID_PROMISE) {
-		/*
-		 * Expose the ship behind i960 for initialization, or it will
-		 * failed
-		 */
-		i960 = pci_get_slot(c->pdev->bus,
-				  PCI_DEVFN(PCI_SLOT(c->pdev->devfn), 0));
-
-		if (i960) {
-			pci_write_config_word(i960, 0x42, 0);
-			pci_dev_put(i960);
-		}
-
-		c->promise = 1;
-		c->limit_sectors = 1;
-	}
-
-	if (pdev->subsystem_vendor == PCI_VENDOR_ID_DPT)
-		c->adaptec = 1;
-
-	/* Cards that go bananas if you quiesce them before you reset them. */
-	if (pdev->vendor == PCI_VENDOR_ID_DPT) {
-		c->no_quiesce = 1;
-		if (pdev->device == 0xa511)
-			c->raptor = 1;
-
-		if (pdev->subsystem_device == 0xc05a) {
-			c->limit_sectors = 1;
-			printk(KERN_INFO
-			       "%s: limit sectors per request to %d\n", c->name,
-			       I2O_MAX_SECTORS_LIMITED);
-		}
-#ifdef CONFIG_I2O_EXT_ADAPTEC_DMA64
-		if (sizeof(dma_addr_t) > 4) {
-			if (pci_set_dma_mask(pdev, DMA_BIT_MASK(64)))
-				printk(KERN_INFO "%s: 64-bit DMA unavailable\n",
-				       c->name);
-			else {
-				c->pae_support = 1;
-				printk(KERN_INFO "%s: using 64-bit DMA\n",
-				       c->name);
-			}
-		}
-#endif
-	}
-
-	rc = i2o_pci_alloc(c);
-	if (rc) {
-		printk(KERN_ERR "%s: DMA / IO allocation for I2O controller "
-		       "failed\n", c->name);
-		goto free_controller;
-	}
-
-	if (i2o_pci_irq_enable(c)) {
-		printk(KERN_ERR "%s: unable to enable interrupts for I2O "
-		       "controller\n", c->name);
-		goto free_pci;
-	}
-
-	rc = i2o_iop_add(c);
-	if (rc)
-		goto uninstall;
-
-	if (i960)
-		pci_write_config_word(i960, 0x42, 0x03ff);
-
-	return 0;
-
-      uninstall:
-	i2o_pci_irq_disable(c);
-
-      free_pci:
-	i2o_pci_free(c);
-
-      free_controller:
-	i2o_iop_free(c);
-
-      disable:
-	pci_disable_device(pdev);
-
-	return rc;
-}
-
-/**
- *	i2o_pci_remove - Removes a I2O controller from the system
- *	@pdev: I2O controller which should be removed
- *
- *	Reset the I2O controller, disable interrupts and remove all allocated
- *	resources.
- */
-static void i2o_pci_remove(struct pci_dev *pdev)
-{
-	struct i2o_controller *c;
-	c = pci_get_drvdata(pdev);
-
-	i2o_iop_remove(c);
-	i2o_pci_irq_disable(c);
-	i2o_pci_free(c);
-
-	pci_disable_device(pdev);
-
-	printk(KERN_INFO "%s: Controller removed.\n", c->name);
-
-	put_device(&c->device);
-};
-
-/* PCI driver for I2O controller */
-static struct pci_driver i2o_pci_driver = {
-	.name = "PCI_I2O",
-	.id_table = i2o_pci_ids,
-	.probe = i2o_pci_probe,
-	.remove = i2o_pci_remove,
-};
-
-/**
- *	i2o_pci_init - registers I2O PCI driver in PCI subsystem
- *
- *	Returns > 0 on success or negative error code on failure.
- */
-int __init i2o_pci_init(void)
-{
-	return pci_register_driver(&i2o_pci_driver);
-};
-
-/**
- *	i2o_pci_exit - unregisters I2O PCI driver from PCI subsystem
- */
-void __exit i2o_pci_exit(void)
-{
-	pci_unregister_driver(&i2o_pci_driver);
-};
-
-MODULE_DEVICE_TABLE(pci, i2o_pci_ids);
diff --git a/drivers/staging/iio/Documentation/device.txt b/drivers/staging/iio/Documentation/device.txt
index 8be32e5a0af1..54ef0deed28f 100644
--- a/drivers/staging/iio/Documentation/device.txt
+++ b/drivers/staging/iio/Documentation/device.txt
@@ -52,7 +52,7 @@ Then fill in the following:
 	* info->write_event_value:
 		Write the value associated with on sensor event detectors. E.g.
 		a threshold above which an interrupt occurs.  Note that the
-		meaning of the value to be set is event type dependant.
+		meaning of the value to be set is event type dependent.
 
 - indio_dev->modes:
 	Specify whether direct access and / or ring buffer access is supported.
diff --git a/drivers/staging/iio/accel/Kconfig b/drivers/staging/iio/accel/Kconfig
index 07b7ffa00ab5..fa67da9408b6 100644
--- a/drivers/staging/iio/accel/Kconfig
+++ b/drivers/staging/iio/accel/Kconfig
@@ -79,7 +79,7 @@ config LIS3L02DQ
 	depends on SPI
 	select IIO_TRIGGER if IIO_BUFFER
 	depends on !IIO_BUFFER || IIO_KFIFO_BUF
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Say Y here to build SPI support for the ST microelectronics
 	  accelerometer. The driver supplies direct access via sysfs files
diff --git a/drivers/staging/iio/accel/sca3000_ring.c b/drivers/staging/iio/accel/sca3000_ring.c
index 8589eade1057..23685e74917e 100644
--- a/drivers/staging/iio/accel/sca3000_ring.c
+++ b/drivers/staging/iio/accel/sca3000_ring.c
@@ -258,6 +258,8 @@ static const struct iio_buffer_access_funcs sca3000_ring_access_funcs = {
 	.read_first_n = &sca3000_read_first_n_hw_rb,
 	.data_available = sca3000_ring_buf_data_available,
 	.release = sca3000_ring_release,
+
+	.modes = INDIO_BUFFER_HARDWARE,
 };
 
 int sca3000_configure_ring(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index d0016ce6e658..94ae4232ee77 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -5,7 +5,7 @@ menu "Analog to digital converters"
 
 config AD7606
 	tristate "Analog Devices AD7606 ADC driver"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	help
@@ -39,7 +39,7 @@ config AD7606_IFACE_SPI
 config AD7780
 	tristate "Analog Devices AD7780 and similar ADCs driver"
 	depends on SPI
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	select AD_SIGMA_DELTA
 	help
 	  Say yes here to build support for Analog Devices AD7170, AD7171,
@@ -52,7 +52,7 @@ config AD7780
 config AD7816
 	tristate "Analog Devices AD7816/7/8 temperature sensor and ADC driver"
 	depends on SPI
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Say yes here to build support for Analog Devices AD7816/7/8
 	  temperature sensors and ADC.
diff --git a/drivers/staging/iio/adc/ad7606_par.c b/drivers/staging/iio/adc/ad7606_par.c
index 9e24b4d4455f..1d48ae381d16 100644
--- a/drivers/staging/iio/adc/ad7606_par.c
+++ b/drivers/staging/iio/adc/ad7606_par.c
@@ -119,7 +119,7 @@ static const struct dev_pm_ops ad7606_pm_ops = {
 #define AD7606_PAR_PM_OPS NULL
 #endif  /* CONFIG_PM */
 
-static struct platform_device_id ad7606_driver_ids[] = {
+static const struct platform_device_id ad7606_driver_ids[] = {
 	{
 		.name		= "ad7606-8",
 		.driver_data	= ID_AD7606_8,
diff --git a/drivers/staging/iio/addac/Kconfig b/drivers/staging/iio/addac/Kconfig
index 0ed7e13e2283..ba18b8432d9c 100644
--- a/drivers/staging/iio/addac/Kconfig
+++ b/drivers/staging/iio/addac/Kconfig
@@ -5,7 +5,7 @@ menu "Analog digital bi-direction converters"
 
 config ADT7316
 	tristate "Analog Devices ADT7316/7/8 ADT7516/7/9 temperature sensor, ADC and DAC driver"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Say yes here to build support for Analog Devices ADT7316, ADT7317, ADT7318
 	  and ADT7516, ADT7517, ADT7519 temperature sensors, ADC and DAC.
diff --git a/drivers/staging/iio/iio_dummy_evgen.c b/drivers/staging/iio/iio_dummy_evgen.c
index 0c9c86d7b509..c54d5b5443a6 100644
--- a/drivers/staging/iio/iio_dummy_evgen.c
+++ b/drivers/staging/iio/iio_dummy_evgen.c
@@ -128,13 +128,11 @@ EXPORT_SYMBOL_GPL(iio_dummy_evgen_get_irq);
  *
  * Used by client driver instances to give the irqs back when they disconnect
  */
-int iio_dummy_evgen_release_irq(int irq)
+void iio_dummy_evgen_release_irq(int irq)
 {
 	mutex_lock(&iio_evgen->lock);
 	iio_evgen->inuse[irq - iio_evgen->base] = false;
 	mutex_unlock(&iio_evgen->lock);
-
-	return 0;
 }
 EXPORT_SYMBOL_GPL(iio_dummy_evgen_release_irq);
 
diff --git a/drivers/staging/iio/iio_dummy_evgen.h b/drivers/staging/iio/iio_dummy_evgen.h
index 2ac293ab7c8f..d044b946e74a 100644
--- a/drivers/staging/iio/iio_dummy_evgen.h
+++ b/drivers/staging/iio/iio_dummy_evgen.h
@@ -8,6 +8,6 @@ struct iio_dummy_regs {
 
 struct iio_dummy_regs *iio_dummy_evgen_get_regs(int irq);
 int iio_dummy_evgen_get_irq(void);
-int iio_dummy_evgen_release_irq(int irq);
+void iio_dummy_evgen_release_irq(int irq);
 
 #endif /* _IIO_DUMMY_EVGEN_H_ */
diff --git a/drivers/staging/iio/iio_simple_dummy.c b/drivers/staging/iio/iio_simple_dummy.c
index b47bf9fb64be..1629a8a6bf26 100644
--- a/drivers/staging/iio/iio_simple_dummy.c
+++ b/drivers/staging/iio/iio_simple_dummy.c
@@ -30,7 +30,7 @@
  * dummy devices are registered.
  */
 static unsigned instances = 1;
-module_param(instances, int, 0);
+module_param(instances, uint, 0);
 
 /* Pointer array used to fake bus elements */
 static struct iio_dev **iio_dummy_devs;
@@ -665,9 +665,8 @@ error_ret:
  *
  * Parameters follow those of iio_dummy_probe for buses.
  */
-static int iio_dummy_remove(int index)
+static void iio_dummy_remove(int index)
 {
-	int ret;
 	/*
 	 * Get a pointer to the device instance iio_dev structure
 	 * from the bus subsystem. E.g.
@@ -685,15 +684,10 @@ static int iio_dummy_remove(int index)
 	/* Buffered capture related cleanup */
 	iio_simple_dummy_unconfigure_buffer(indio_dev);
 
-	ret = iio_simple_dummy_events_unregister(indio_dev);
-	if (ret)
-		goto error_ret;
+	iio_simple_dummy_events_unregister(indio_dev);
 
 	/* Free all structures */
 	iio_device_free(indio_dev);
-
-error_ret:
-	return ret;
 }
 
 /**
@@ -722,9 +716,16 @@ static __init int iio_dummy_init(void)
 	for (i = 0; i < instances; i++) {
 		ret = iio_dummy_probe(i);
 		if (ret < 0)
-			return ret;
+			goto error_remove_devs;
 	}
 	return 0;
+
+error_remove_devs:
+	while (i--)
+		iio_dummy_remove(i);
+
+	kfree(iio_dummy_devs);
+	return ret;
 }
 module_init(iio_dummy_init);
 
diff --git a/drivers/staging/iio/iio_simple_dummy.h b/drivers/staging/iio/iio_simple_dummy.h
index 34989bf248a7..e877a99540ab 100644
--- a/drivers/staging/iio/iio_simple_dummy.h
+++ b/drivers/staging/iio/iio_simple_dummy.h
@@ -25,7 +25,7 @@ struct iio_dummy_regs;
  * @accel_calibscale:		cache for acceleration calibscale
  * @lock:			lock to ensure state is consistent
  * @event_irq:			irq number for event line (faked)
- * @event_val:			cache for event theshold value
+ * @event_val:			cache for event threshold value
  * @event_en:			cache of whether event is enabled
  */
 struct iio_dummy_state {
@@ -79,7 +79,7 @@ int iio_simple_dummy_write_event_value(struct iio_dev *indio_dev,
 				       int val2);
 
 int iio_simple_dummy_events_register(struct iio_dev *indio_dev);
-int iio_simple_dummy_events_unregister(struct iio_dev *indio_dev);
+void iio_simple_dummy_events_unregister(struct iio_dev *indio_dev);
 
 #else /* Stubs for when events are disabled at compile time */
 
@@ -89,11 +89,9 @@ iio_simple_dummy_events_register(struct iio_dev *indio_dev)
 	return 0;
 };
 
-static inline int
+static inline void
 iio_simple_dummy_events_unregister(struct iio_dev *indio_dev)
-{
-	return 0;
-};
+{ };
 
 #endif /* CONFIG_IIO_SIMPLE_DUMMY_EVENTS*/
 
diff --git a/drivers/staging/iio/iio_simple_dummy_events.c b/drivers/staging/iio/iio_simple_dummy_events.c
index a5cd3bb219fe..ecc563cb6cb9 100644
--- a/drivers/staging/iio/iio_simple_dummy_events.c
+++ b/drivers/staging/iio/iio_simple_dummy_events.c
@@ -84,6 +84,7 @@ int iio_simple_dummy_write_event_config(struct iio_dev *indio_dev,
 		default:
 			return -EINVAL;
 		}
+		break;
 	case IIO_STEPS:
 		switch (type) {
 		case IIO_EV_TYPE_CHANGE:
@@ -92,6 +93,7 @@ int iio_simple_dummy_write_event_config(struct iio_dev *indio_dev,
 		default:
 			return -EINVAL;
 		}
+		break;
 	default:
 		return -EINVAL;
 	}
@@ -255,13 +257,11 @@ error_ret:
  * iio_simple_dummy_events_unregister() - tidy up interrupt handling on remove
  * @indio_dev: device instance data
  */
-int iio_simple_dummy_events_unregister(struct iio_dev *indio_dev)
+void iio_simple_dummy_events_unregister(struct iio_dev *indio_dev)
 {
 	struct iio_dummy_state *st = iio_priv(indio_dev);
 
 	free_irq(st->event_irq, indio_dev);
 	/* Not part of normal driver */
 	iio_dummy_evgen_release_irq(st->event_irq);
-
-	return 0;
 }
diff --git a/drivers/staging/iio/light/isl29018.c b/drivers/staging/iio/light/isl29018.c
index a3489187aeb0..e646c5d24004 100644
--- a/drivers/staging/iio/light/isl29018.c
+++ b/drivers/staging/iio/light/isl29018.c
@@ -66,58 +66,115 @@
 #define ISL29035_BOUT_SHIFT		0x07
 #define ISL29035_BOUT_MASK		(0x01 << ISL29035_BOUT_SHIFT)
 
+#define ISL29018_INT_TIME_AVAIL		"0.090000 0.005630 0.000351 0.000021"
+#define ISL29023_INT_TIME_AVAIL		"0.090000 0.005600 0.000352 0.000022"
+#define ISL29035_INT_TIME_AVAIL		"0.105000 0.006500 0.000410 0.000025"
+
+static const char * const int_time_avail[] = {
+	ISL29018_INT_TIME_AVAIL,
+	ISL29023_INT_TIME_AVAIL,
+	ISL29035_INT_TIME_AVAIL,
+};
+
+enum isl29018_int_time {
+	ISL29018_INT_TIME_16,
+	ISL29018_INT_TIME_12,
+	ISL29018_INT_TIME_8,
+	ISL29018_INT_TIME_4,
+};
+
+static const unsigned int isl29018_int_utimes[3][4] = {
+	{90000, 5630, 351, 21},
+	{90000, 5600, 352, 22},
+	{105000, 6500, 410, 25},
+};
+
+static const struct isl29018_scale {
+	unsigned int scale;
+	unsigned int uscale;
+} isl29018_scales[4][4] = {
+	{ {0, 15258}, {0, 61035}, {0, 244140}, {0, 976562} },
+	{ {0, 244140}, {0, 976562}, {3, 906250}, {15, 625000} },
+	{ {3, 906250}, {15, 625000}, {62, 500000}, {250, 0} },
+	{ {62, 500000}, {250, 0}, {1000, 0}, {4000, 0} }
+};
+
 struct isl29018_chip {
 	struct device		*dev;
 	struct regmap		*regmap;
 	struct mutex		lock;
 	int			type;
-	unsigned int		lux_scale;
-	unsigned int		lux_uscale;
-	unsigned int		range;
-	unsigned int		adc_bit;
+	unsigned int		calibscale;
+	unsigned int		ucalibscale;
+	unsigned int		int_time;
+	struct isl29018_scale	scale;
 	int			prox_scheme;
 	bool			suspended;
 };
 
-static int isl29018_set_range(struct isl29018_chip *chip, unsigned long range,
-		unsigned int *new_range)
+static int isl29018_set_integration_time(struct isl29018_chip *chip,
+					 unsigned int utime)
 {
-	static const unsigned long supp_ranges[] = {1000, 4000, 16000, 64000};
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(supp_ranges); ++i) {
-		if (range <= supp_ranges[i]) {
-			*new_range = (unsigned int)supp_ranges[i];
+	int i, ret;
+	unsigned int int_time, new_int_time;
+	struct isl29018_scale new_scale;
+
+	for (i = 0; i < ARRAY_SIZE(isl29018_int_utimes[chip->type]); ++i) {
+		if (utime == isl29018_int_utimes[chip->type][i]) {
+			new_int_time = i;
+			new_scale = isl29018_scales[new_int_time][0];
 			break;
 		}
 	}
 
-	if (i >= ARRAY_SIZE(supp_ranges))
+	if (i >= ARRAY_SIZE(isl29018_int_utimes[chip->type]))
 		return -EINVAL;
 
-	return regmap_update_bits(chip->regmap, ISL29018_REG_ADD_COMMANDII,
-			COMMANDII_RANGE_MASK, i << COMMANDII_RANGE_SHIFT);
+	ret = regmap_update_bits(chip->regmap, ISL29018_REG_ADD_COMMANDII,
+				 COMMANDII_RESOLUTION_MASK,
+				 i << COMMANDII_RESOLUTION_SHIFT);
+	if (ret < 0)
+		return ret;
+
+	/* keep the same range when integration time changes */
+	int_time = chip->int_time;
+	for (i = 0; i < ARRAY_SIZE(isl29018_scales[int_time]); ++i) {
+		if (chip->scale.scale == isl29018_scales[int_time][i].scale &&
+		    chip->scale.uscale == isl29018_scales[int_time][i].uscale) {
+			chip->scale = isl29018_scales[new_int_time][i];
+			break;
+		}
+	}
+	chip->int_time = new_int_time;
+
+	return 0;
 }
 
-static int isl29018_set_resolution(struct isl29018_chip *chip,
-			unsigned long adcbit, unsigned int *conf_adc_bit)
+static int isl29018_set_scale(struct isl29018_chip *chip, int scale, int uscale)
 {
-	static const unsigned long supp_adcbit[] = {16, 12, 8, 4};
-	int i;
+	int i, ret;
+	struct isl29018_scale new_scale;
 
-	for (i = 0; i < ARRAY_SIZE(supp_adcbit); ++i) {
-		if (adcbit >= supp_adcbit[i]) {
-			*conf_adc_bit = (unsigned int)supp_adcbit[i];
+	for (i = 0; i < ARRAY_SIZE(isl29018_scales[chip->int_time]); ++i) {
+		if (scale == isl29018_scales[chip->int_time][i].scale &&
+		    uscale == isl29018_scales[chip->int_time][i].uscale) {
+			new_scale = isl29018_scales[chip->int_time][i];
 			break;
 		}
 	}
 
-	if (i >= ARRAY_SIZE(supp_adcbit))
+	if (i >= ARRAY_SIZE(isl29018_scales[chip->int_time]))
 		return -EINVAL;
 
-	return regmap_update_bits(chip->regmap, ISL29018_REG_ADD_COMMANDII,
-			COMMANDII_RESOLUTION_MASK,
-			i << COMMANDII_RESOLUTION_SHIFT);
+	ret = regmap_update_bits(chip->regmap, ISL29018_REG_ADD_COMMANDII,
+				 COMMANDII_RANGE_MASK,
+				 i << COMMANDII_RANGE_SHIFT);
+	if (ret < 0)
+		return ret;
+
+	chip->scale = new_scale;
+
+	return 0;
 }
 
 static int isl29018_read_sensor_input(struct isl29018_chip *chip, int mode)
@@ -156,22 +213,17 @@ static int isl29018_read_sensor_input(struct isl29018_chip *chip, int mode)
 static int isl29018_read_lux(struct isl29018_chip *chip, int *lux)
 {
 	int lux_data;
-	unsigned int data_x_range, lux_unshifted;
+	unsigned int data_x_range;
 
 	lux_data = isl29018_read_sensor_input(chip, COMMMAND1_OPMODE_ALS_ONCE);
 
 	if (lux_data < 0)
 		return lux_data;
 
-	/* To support fractional scaling, separate the unshifted lux
-	 * into two calculations: int scaling and micro-scaling.
-	 * lux_uscale ranges from 0-999999, so about 20 bits.  Split
-	 * the /1,000,000 in two to reduce the risk of over/underflow.
-	 */
-	data_x_range = lux_data * chip->range;
-	lux_unshifted = data_x_range * chip->lux_scale;
-	lux_unshifted += data_x_range / 1000 * chip->lux_uscale / 1000;
-	*lux = lux_unshifted >> chip->adc_bit;
+	data_x_range = lux_data * chip->scale.scale +
+		       lux_data * chip->scale.uscale / 1000000;
+	*lux = data_x_range * chip->calibscale +
+	       data_x_range * chip->ucalibscale / 1000000;
 
 	return 0;
 }
@@ -229,86 +281,37 @@ static int isl29018_read_proximity_ir(struct isl29018_chip *chip, int scheme,
 	return 0;
 }
 
-/* Sysfs interface */
-/* range */
-static ssize_t show_range(struct device *dev,
+static ssize_t show_scale_available(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
+	int i, len = 0;
 
-	return sprintf(buf, "%u\n", chip->range);
-}
-
-static ssize_t store_range(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct isl29018_chip *chip = iio_priv(indio_dev);
-	int status;
-	unsigned long lval;
-	unsigned int new_range;
-
-	if (kstrtoul(buf, 10, &lval))
-		return -EINVAL;
-
-	if (!(lval == 1000UL || lval == 4000UL ||
-			lval == 16000UL || lval == 64000UL)) {
-		dev_err(dev, "The range is not supported\n");
-		return -EINVAL;
-	}
+	for (i = 0; i < ARRAY_SIZE(isl29018_scales[chip->int_time]); ++i)
+		len += sprintf(buf + len, "%d.%06d ",
+			       isl29018_scales[chip->int_time][i].scale,
+			       isl29018_scales[chip->int_time][i].uscale);
 
-	mutex_lock(&chip->lock);
-	status = isl29018_set_range(chip, lval, &new_range);
-	if (status < 0) {
-		mutex_unlock(&chip->lock);
-		dev_err(dev,
-			"Error in setting max range with err %d\n", status);
-		return status;
-	}
-	chip->range = new_range;
-	mutex_unlock(&chip->lock);
+	buf[len - 1] = '\n';
 
-	return count;
+	return len;
 }
 
-/* resolution */
-static ssize_t show_resolution(struct device *dev,
+static ssize_t show_int_time_available(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct isl29018_chip *chip = iio_priv(indio_dev);
+	int i, len = 0;
 
-	return sprintf(buf, "%u\n", chip->adc_bit);
-}
+	for (i = 0; i < ARRAY_SIZE(isl29018_int_utimes[chip->type]); ++i)
+		len += sprintf(buf + len, "0.%06d ",
+			       isl29018_int_utimes[chip->type][i]);
 
-static ssize_t store_resolution(struct device *dev,
-		struct device_attribute *attr, const char *buf, size_t count)
-{
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct isl29018_chip *chip = iio_priv(indio_dev);
-	int status;
-	unsigned int val;
-	unsigned int new_adc_bit;
-
-	if (kstrtouint(buf, 10, &val))
-		return -EINVAL;
-	if (!(val == 4 || val == 8 || val == 12 || val == 16)) {
-		dev_err(dev, "The resolution is not supported\n");
-		return -EINVAL;
-	}
-
-	mutex_lock(&chip->lock);
-	status = isl29018_set_resolution(chip, val, &new_adc_bit);
-	if (status < 0) {
-		mutex_unlock(&chip->lock);
-		dev_err(dev, "Error in setting resolution\n");
-		return status;
-	}
-	chip->adc_bit = new_adc_bit;
-	mutex_unlock(&chip->lock);
+	buf[len - 1] = '\n';
 
-	return count;
+	return len;
 }
 
 /* proximity scheme */
@@ -357,11 +360,29 @@ static int isl29018_write_raw(struct iio_dev *indio_dev,
 	int ret = -EINVAL;
 
 	mutex_lock(&chip->lock);
-	if (mask == IIO_CHAN_INFO_CALIBSCALE && chan->type == IIO_LIGHT) {
-		chip->lux_scale = val;
-		/* With no write_raw_get_fmt(), val2 is a MICRO fraction. */
-		chip->lux_uscale = val2;
-		ret = 0;
+	switch (mask) {
+	case IIO_CHAN_INFO_CALIBSCALE:
+		if (chan->type == IIO_LIGHT) {
+			chip->calibscale = val;
+			chip->ucalibscale = val2;
+			ret = 0;
+		}
+		break;
+	case IIO_CHAN_INFO_INT_TIME:
+		if (chan->type == IIO_LIGHT) {
+			if (val != 0) {
+				mutex_unlock(&chip->lock);
+				return -EINVAL;
+			}
+			ret = isl29018_set_integration_time(chip, val2);
+		}
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		if (chan->type == IIO_LIGHT)
+			ret = isl29018_set_scale(chip, val, val2);
+		break;
+	default:
+		break;
 	}
 	mutex_unlock(&chip->lock);
 
@@ -402,10 +423,24 @@ static int isl29018_read_raw(struct iio_dev *indio_dev,
 		if (!ret)
 			ret = IIO_VAL_INT;
 		break;
+	case IIO_CHAN_INFO_INT_TIME:
+		if (chan->type == IIO_LIGHT) {
+			*val = 0;
+			*val2 = isl29018_int_utimes[chip->type][chip->int_time];
+			ret = IIO_VAL_INT_PLUS_MICRO;
+		}
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		if (chan->type == IIO_LIGHT) {
+			*val = chip->scale.scale;
+			*val2 = chip->scale.uscale;
+			ret = IIO_VAL_INT_PLUS_MICRO;
+		}
+		break;
 	case IIO_CHAN_INFO_CALIBSCALE:
 		if (chan->type == IIO_LIGHT) {
-			*val = chip->lux_scale;
-			*val2 = chip->lux_uscale;
+			*val = chip->calibscale;
+			*val2 = chip->ucalibscale;
 			ret = IIO_VAL_INT_PLUS_MICRO;
 		}
 		break;
@@ -421,7 +456,9 @@ static int isl29018_read_raw(struct iio_dev *indio_dev,
 	.indexed = 1,							\
 	.channel = 0,							\
 	.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) |		\
-	BIT(IIO_CHAN_INFO_CALIBSCALE),					\
+	BIT(IIO_CHAN_INFO_CALIBSCALE) |					\
+	BIT(IIO_CHAN_INFO_SCALE) |					\
+	BIT(IIO_CHAN_INFO_INT_TIME),					\
 }
 
 #define ISL29018_IR_CHANNEL {						\
@@ -447,32 +484,27 @@ static const struct iio_chan_spec isl29023_channels[] = {
 	ISL29018_IR_CHANNEL,
 };
 
-static IIO_DEVICE_ATTR(range, S_IRUGO | S_IWUSR, show_range, store_range, 0);
-static IIO_CONST_ATTR(range_available, "1000 4000 16000 64000");
-static IIO_CONST_ATTR(adc_resolution_available, "4 8 12 16");
-static IIO_DEVICE_ATTR(adc_resolution, S_IRUGO | S_IWUSR,
-					show_resolution, store_resolution, 0);
+static IIO_DEVICE_ATTR(in_illuminance_integration_time_available, S_IRUGO,
+		       show_int_time_available, NULL, 0);
+static IIO_DEVICE_ATTR(in_illuminance_scale_available, S_IRUGO,
+		      show_scale_available, NULL, 0);
 static IIO_DEVICE_ATTR(proximity_on_chip_ambient_infrared_suppression,
 					S_IRUGO | S_IWUSR,
 					show_prox_infrared_suppression,
 					store_prox_infrared_suppression, 0);
 
 #define ISL29018_DEV_ATTR(name) (&iio_dev_attr_##name.dev_attr.attr)
-#define ISL29018_CONST_ATTR(name) (&iio_const_attr_##name.dev_attr.attr)
+
 static struct attribute *isl29018_attributes[] = {
-	ISL29018_DEV_ATTR(range),
-	ISL29018_CONST_ATTR(range_available),
-	ISL29018_DEV_ATTR(adc_resolution),
-	ISL29018_CONST_ATTR(adc_resolution_available),
+	ISL29018_DEV_ATTR(in_illuminance_scale_available),
+	ISL29018_DEV_ATTR(in_illuminance_integration_time_available),
 	ISL29018_DEV_ATTR(proximity_on_chip_ambient_infrared_suppression),
 	NULL
 };
 
 static struct attribute *isl29023_attributes[] = {
-	ISL29018_DEV_ATTR(range),
-	ISL29018_CONST_ATTR(range_available),
-	ISL29018_DEV_ATTR(adc_resolution),
-	ISL29018_CONST_ATTR(adc_resolution_available),
+	ISL29018_DEV_ATTR(in_illuminance_scale_available),
+	ISL29018_DEV_ATTR(in_illuminance_integration_time_available),
 	NULL
 };
 
@@ -516,8 +548,6 @@ enum {
 static int isl29018_chip_init(struct isl29018_chip *chip)
 {
 	int status;
-	unsigned int new_adc_bit;
-	unsigned int new_range;
 
 	if (chip->type == isl29035) {
 		status = isl29035_detect(chip);
@@ -566,14 +596,19 @@ static int isl29018_chip_init(struct isl29018_chip *chip)
 	usleep_range(1000, 2000);	/* per data sheet, page 10 */
 
 	/* set defaults */
-	status = isl29018_set_range(chip, chip->range, &new_range);
+	status = isl29018_set_scale(chip, chip->scale.scale,
+				    chip->scale.uscale);
 	if (status < 0) {
 		dev_err(chip->dev, "Init of isl29018 fails\n");
 		return status;
 	}
 
-	status = isl29018_set_resolution(chip, chip->adc_bit,
-						&new_adc_bit);
+	status = isl29018_set_integration_time(chip,
+			isl29018_int_utimes[chip->type][chip->int_time]);
+	if (status < 0) {
+		dev_err(chip->dev, "Init of isl29018 fails\n");
+		return status;
+	}
 
 	return 0;
 }
@@ -701,10 +736,10 @@ static int isl29018_probe(struct i2c_client *client,
 	mutex_init(&chip->lock);
 
 	chip->type = dev_id;
-	chip->lux_scale = 1;
-	chip->lux_uscale = 0;
-	chip->range = 1000;
-	chip->adc_bit = 16;
+	chip->calibscale = 1;
+	chip->ucalibscale = 0;
+	chip->int_time = ISL29018_INT_TIME_16;
+	chip->scale = isl29018_scales[chip->int_time][0];
 	chip->suspended = false;
 
 	chip->regmap = devm_regmap_init_i2c(client,
diff --git a/drivers/staging/iio/resolver/Kconfig b/drivers/staging/iio/resolver/Kconfig
index c7a742ec1227..1c7e2860d6b7 100644
--- a/drivers/staging/iio/resolver/Kconfig
+++ b/drivers/staging/iio/resolver/Kconfig
@@ -16,7 +16,7 @@ config AD2S90
 config AD2S1200
 	tristate "Analog Devices ad2s1200/ad2s1205 driver"
 	depends on SPI
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Say yes here to build support for Analog Devices spi resolver
 	  to digital converters, ad2s1200 and ad2s1205, provides direct access
@@ -28,7 +28,7 @@ config AD2S1200
 config AD2S1210
 	tristate "Analog Devices ad2s1210 driver"
 	depends on SPI
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  Say yes here to build support for Analog Devices spi resolver
 	  to digital converters, ad2s1210, provides direct access via sysfs.
diff --git a/drivers/staging/lustre/TODO b/drivers/staging/lustre/TODO
index 0512594b5199..f194417d0af7 100644
--- a/drivers/staging/lustre/TODO
+++ b/drivers/staging/lustre/TODO
@@ -1,6 +1,6 @@
 * Possible remaining coding style fix.
 * Remove deadcode.
-* Seperate client/server functionality. Functions only used by server can be
+* Separate client/server functionality. Functions only used by server can be
   removed from client.
 * Clean up libcfs layer. Ideally we can remove include/linux/libcfs entirely.
 * Clean up CLIO layer. Lustre client readahead/writeback control needs to better
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs.h b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
index 4410d7fdc1b4..5dd9cdfae30c 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
@@ -37,34 +37,11 @@
 #ifndef __LIBCFS_LIBCFS_H__
 #define __LIBCFS_LIBCFS_H__
 
-#if !__GNUC__
-#define __attribute__(x)
-#endif
-
 #include "linux/libcfs.h"
 #include <linux/gfp.h>
 
 #include "curproc.h"
 
-#ifndef offsetof
-# define offsetof(typ, memb) ((long)(long_ptr_t)((char *)&(((typ *)0)->memb)))
-#endif
-
-#ifndef ARRAY_SIZE
-#define ARRAY_SIZE(a) ((sizeof(a)) / (sizeof((a)[0])))
-#endif
-
-#if !defined(swap)
-#define swap(x, y) do { typeof(x) z = x; x = y; y = z; } while (0)
-#endif
-
-#if !defined(container_of)
-/* given a pointer @ptr to the field @member embedded into type (usually
- * struct) @type, return pointer to the embedding instance of @type. */
-#define container_of(ptr, type, member) \
-	((type *)((char *)(ptr)-(char *)(&((type *)0)->member)))
-#endif
-
 static inline int __is_po2(unsigned long long val)
 {
 	return !(val & (val - 1));
@@ -88,23 +65,6 @@ static inline int __is_po2(unsigned long long val)
 int libcfs_arch_init(void);
 void libcfs_arch_cleanup(void);
 
-/* libcfs tcpip */
-int libcfs_ipif_query(char *name, int *up, __u32 *ip, __u32 *mask);
-int libcfs_ipif_enumerate(char ***names);
-void libcfs_ipif_free_enumeration(char **names, int n);
-int libcfs_sock_listen(struct socket **sockp, __u32 ip, int port, int backlog);
-int libcfs_sock_accept(struct socket **newsockp, struct socket *sock);
-void libcfs_sock_abort_accept(struct socket *sock);
-int libcfs_sock_connect(struct socket **sockp, int *fatal,
-			__u32 local_ip, int local_port,
-			__u32 peer_ip, int peer_port);
-int libcfs_sock_setbuf(struct socket *socket, int txbufsize, int rxbufsize);
-int libcfs_sock_getbuf(struct socket *socket, int *txbufsize, int *rxbufsize);
-int libcfs_sock_getaddr(struct socket *socket, int remote, __u32 *ip, int *port);
-int libcfs_sock_write(struct socket *sock, void *buffer, int nob, int timeout);
-int libcfs_sock_read(struct socket *sock, void *buffer, int nob, int timeout);
-void libcfs_sock_release(struct socket *sock);
-
 /* need both kernel and user-land acceptor */
 #define LNET_ACCEPTOR_MIN_RESERVED_PORT    512
 #define LNET_ACCEPTOR_MAX_RESERVED_PORT    1023
@@ -184,4 +144,8 @@ static inline void *__container_of(void *ptr, unsigned long shift)
 
 #define _LIBCFS_H
 
+void *libcfs_kvzalloc(size_t size, gfp_t flags);
+void *libcfs_kvzalloc_cpt(struct cfs_cpt_table *cptab, int cpt, size_t size,
+			  gfp_t flags);
+
 #endif /* _LIBCFS_H */
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_ioctl.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_ioctl.h
index 3ee38782ad8c..f5d741f25ffd 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_ioctl.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_ioctl.h
@@ -134,7 +134,7 @@ struct libcfs_ioctl_handler {
 #define IOC_LIBCFS_DEL_PEER		_IOWR('e', 74, long)
 #define IOC_LIBCFS_ADD_PEER		_IOWR('e', 75, long)
 #define IOC_LIBCFS_GET_PEER		_IOWR('e', 76, long)
-#define IOC_LIBCFS_GET_TXDESC	      _IOWR('e', 77, long)
+/* ioctl 77 is free for use */
 #define IOC_LIBCFS_ADD_INTERFACE	   _IOWR('e', 78, long)
 #define IOC_LIBCFS_DEL_INTERFACE	   _IOWR('e', 79, long)
 #define IOC_LIBCFS_GET_INTERFACE	   _IOWR('e', 80, long)
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
index fef882530455..ed37d26eb20d 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
@@ -42,14 +42,10 @@
 #ifndef __LIBCFS_PRIVATE_H__
 #define __LIBCFS_PRIVATE_H__
 
-/* XXX this layering violation is for nidstrings */
-#include "../lnet/types.h"
-
 #ifndef DEBUG_SUBSYSTEM
 # define DEBUG_SUBSYSTEM S_UNDEFINED
 #endif
 
-
 /*
  * When this is on, LASSERT macro includes check for assignment used instead
  * of equality check, but doesn't have unlikely(). Turn this on from time to
@@ -410,36 +406,6 @@ int cfs_percpt_atomic_summary(atomic_t **refs);
  */
 #define CLASSERT(cond) do {switch (42) {case (cond): case 0: break; } } while (0)
 
-/* support decl needed both by kernel and liblustre */
-int	 libcfs_isknown_lnd(int type);
-char       *libcfs_lnd2modname(int type);
-char       *libcfs_lnd2str(int type);
-int	 libcfs_str2lnd(const char *str);
-char       *libcfs_net2str(__u32 net);
-char       *libcfs_nid2str(lnet_nid_t nid);
-__u32       libcfs_str2net(const char *str);
-lnet_nid_t  libcfs_str2nid(const char *str);
-int	 libcfs_str2anynid(lnet_nid_t *nid, const char *str);
-char       *libcfs_id2str(lnet_process_id_t id);
-void	cfs_free_nidlist(struct list_head *list);
-int	 cfs_parse_nidlist(char *str, int len, struct list_head *list);
-int	 cfs_match_nid(lnet_nid_t nid, struct list_head *list);
-
-/** \addtogroup lnet_addr
- * @{ */
-/* how an LNET NID encodes net:address */
-/** extract the address part of an lnet_nid_t */
-#define LNET_NIDADDR(nid)      ((__u32)((nid) & 0xffffffff))
-/** extract the network part of an lnet_nid_t */
-#define LNET_NIDNET(nid)       ((__u32)(((nid) >> 32)) & 0xffffffff)
-/** make an lnet_nid_t from a network part and an address part */
-#define LNET_MKNID(net, addr)   ((((__u64)(net))<<32)|((__u64)(addr)))
-/* how net encodes type:number */
-#define LNET_NETNUM(net)       ((net) & 0xffff)
-#define LNET_NETTYP(net)       (((net) >> 16) & 0xffff)
-#define LNET_MKNET(typ, num)    ((((__u32)(typ))<<16)|((__u32)(num)))
-/** @} lnet_addr */
-
 /* max value for numeric network address */
 #define MAX_NUMERIC_VALUE 0xffffffff
 
@@ -496,19 +462,6 @@ static inline size_t cfs_round_strlen(char *fset)
 	return (size_t)cfs_size_round((int)strlen(fset) + 1);
 }
 
-/* roundup \a val to power2 */
-static inline unsigned int cfs_power2_roundup(unsigned int val)
-{
-	if (val != LOWEST_BIT_SET(val)) { /* not a power of 2 already */
-		do {
-			val &= ~LOWEST_BIT_SET(val);
-		} while (val != LOWEST_BIT_SET(val));
-		/* ...and round up */
-		val <<= 1;
-	}
-	return val;
-}
-
 #define LOGL(var, len, ptr)				       \
 do {							    \
 	if (var)						\
@@ -532,25 +485,4 @@ do {						    \
 	ptr += cfs_size_round(len + 1);		 \
 } while (0)
 
-/**
- *  Lustre Network Driver types.
- */
-enum {
-	/* Only add to these values (i.e. don't ever change or redefine them):
-	 * network addresses depend on them... */
-	QSWLND    = 1,
-	SOCKLND   = 2,
-	GMLND     = 3, /* obsolete, keep it so that libcfs_nid2str works */
-	PTLLND    = 4,
-	O2IBLND   = 5,
-	CIBLND    = 6,
-	OPENIBLND = 7,
-	IIBLND    = 8,
-	LOLND     = 9,
-	RALND     = 10,
-	VIBLND    = 11,
-	MXLND     = 12,
-	GNILND    = 13,
-};
-
 #endif
diff --git a/drivers/staging/lustre/include/linux/libcfs/linux/libcfs.h b/drivers/staging/lustre/include/linux/libcfs/linux/libcfs.h
index 4fe50841e8e3..3e2502a69bbd 100644
--- a/drivers/staging/lustre/include/linux/libcfs/linux/libcfs.h
+++ b/drivers/staging/lustre/include/linux/libcfs/linux/libcfs.h
@@ -61,7 +61,6 @@
 #include <linux/moduleparam.h>
 #include <linux/mutex.h>
 #include <linux/notifier.h>
-#include <linux/proc_fs.h>
 #include <linux/random.h>
 #include <linux/rbtree.h>
 #include <linux/rwsem.h>
diff --git a/drivers/staging/lustre/include/linux/lnet/api-support.h b/drivers/staging/lustre/include/linux/lnet/api-support.h
deleted file mode 100644
index 8f7fa28b517c..000000000000
--- a/drivers/staging/lustre/include/linux/lnet/api-support.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LNET_API_SUPPORT_H__
-#define __LNET_API_SUPPORT_H__
-
-#include "linux/api-support.h"
-
-#include "../libcfs/libcfs.h"
-#include "types.h"
-#include "lnet.h"
-
-#endif
diff --git a/drivers/staging/lustre/include/linux/lnet/api.h b/drivers/staging/lustre/include/linux/lnet/api.h
index cd865175703f..9493d5e236c5 100644
--- a/drivers/staging/lustre/include/linux/lnet/api.h
+++ b/drivers/staging/lustre/include/linux/lnet/api.h
@@ -15,21 +15,19 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
 /*
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
+ *
+ * Copyright (c) 2011 - 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
+ * Lustre is a trademark of Seagate, Inc.
  */
 
 #ifndef __LNET_API_H__
@@ -41,9 +39,8 @@
  *
  * LNet is an asynchronous message-passing API, which provides an unreliable
  * connectionless service that can't guarantee any order. It supports OFA IB,
- * TCP/IP, and Cray Portals, and routes between heterogeneous networks.
+ * TCP/IP, and Cray Interconnects, and routes between heterogeneous networks.
  *
- * LNet can run both in OS kernel space and in userspace as a library.
  * @{
  */
 
@@ -52,9 +49,6 @@
 /** \defgroup lnet_init_fini Initialization and cleanup
  * The LNet must be properly initialized before any LNet calls can be made.
  * @{ */
-int LNetInit(void);
-void LNetFini(void);
-
 int LNetNIInit(lnet_pid_t requested_pid);
 int LNetNIFini(void);
 /** @} lnet_init_fini */
@@ -98,16 +92,16 @@ void LNetSnprintHandle(char *str, int str_len, lnet_handle_any_t handle);
  * @{ */
 int LNetMEAttach(unsigned int      portal,
 		 lnet_process_id_t match_id_in,
-		 __u64	     match_bits_in,
-		 __u64	     ignore_bits_in,
+		 __u64		   match_bits_in,
+		 __u64		   ignore_bits_in,
 		 lnet_unlink_t     unlink_in,
 		 lnet_ins_pos_t    pos_in,
 		 lnet_handle_me_t *handle_out);
 
 int LNetMEInsert(lnet_handle_me_t  current_in,
 		 lnet_process_id_t match_id_in,
-		 __u64	     match_bits_in,
-		 __u64	     ignore_bits_in,
+		 __u64		   match_bits_in,
+		 __u64		   ignore_bits_in,
 		 lnet_unlink_t     unlink_in,
 		 lnet_ins_pos_t    position_in,
 		 lnet_handle_me_t *handle_out);
@@ -128,13 +122,13 @@ int LNetMEUnlink(lnet_handle_me_t current_in);
  * associated with a MD: LNetMDUnlink().
  * @{ */
 int LNetMDAttach(lnet_handle_me_t  current_in,
-		 lnet_md_t	 md_in,
+		 lnet_md_t	   md_in,
 		 lnet_unlink_t     unlink_in,
 		 lnet_handle_md_t *handle_out);
 
-int LNetMDBind(lnet_md_t	 md_in,
-	       lnet_unlink_t     unlink_in,
-	       lnet_handle_md_t *handle_out);
+int LNetMDBind(lnet_md_t	   md_in,
+	       lnet_unlink_t       unlink_in,
+	       lnet_handle_md_t   *handle_out);
 
 int LNetMDUnlink(lnet_handle_md_t md_in);
 /** @} lnet_md */
@@ -174,10 +168,10 @@ int LNetEQWait(lnet_handle_eq_t  eventq_in,
 	       lnet_event_t     *event_out);
 
 int LNetEQPoll(lnet_handle_eq_t *eventqs_in,
-	       int	       neq_in,
-	       int	       timeout_ms,
+	       int		 neq_in,
+	       int		 timeout_ms,
 	       lnet_event_t     *event_out,
-	       int	      *which_eq_out);
+	       int		*which_eq_out);
 /** @} lnet_eq */
 
 /** \defgroup lnet_data Data movement operations
@@ -185,20 +179,20 @@ int LNetEQPoll(lnet_handle_eq_t *eventqs_in,
  * The LNet API provides two data movement operations: LNetPut()
  * and LNetGet().
  * @{ */
-int LNetPut(lnet_nid_t	self,
+int LNetPut(lnet_nid_t	      self,
 	    lnet_handle_md_t  md_in,
 	    lnet_ack_req_t    ack_req_in,
 	    lnet_process_id_t target_in,
 	    unsigned int      portal_in,
-	    __u64	     match_bits_in,
+	    __u64	      match_bits_in,
 	    unsigned int      offset_in,
-	    __u64	     hdr_data_in);
+	    __u64	      hdr_data_in);
 
-int LNetGet(lnet_nid_t	self,
+int LNetGet(lnet_nid_t	      self,
 	    lnet_handle_md_t  md_in,
 	    lnet_process_id_t target_in,
 	    unsigned int      portal_in,
-	    __u64	     match_bits_in,
+	    __u64	      match_bits_in,
 	    unsigned int      offset_in);
 /** @} lnet_data */
 
@@ -209,7 +203,6 @@ int LNetGet(lnet_nid_t	self,
 int LNetSetLazyPortal(int portal);
 int LNetClearLazyPortal(int portal);
 int LNetCtl(unsigned int cmd, void *arg);
-int LNetSetAsync(lnet_process_id_t id, int nasync);
 
 /** @} lnet_misc */
 
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
index 0038d29a37fe..a9c9a077c77d 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
@@ -15,11 +15,7 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
@@ -27,56 +23,46 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012 - 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
+ * Lustre is a trademark of Seagate, Inc.
  *
  * lnet/include/lnet/lib-lnet.h
- *
- * Top level include for library side routines
  */
 
 #ifndef __LNET_LIB_LNET_H__
 #define __LNET_LIB_LNET_H__
 
-#include "linux/lib-lnet.h"
 #include "../libcfs/libcfs.h"
-#include "types.h"
+#include "api.h"
 #include "lnet.h"
 #include "lib-types.h"
 
-extern lnet_t  the_lnet;			/* THE network */
-
-#if  defined(LNET_USE_LIB_FREELIST)
-/* 1 CPT, simplify implementation... */
-# define LNET_CPT_MAX_BITS      0
-
-#else /* KERNEL and no freelist */
+extern lnet_t	the_lnet;	/* THE network */
 
-# if (BITS_PER_LONG == 32)
+#if (BITS_PER_LONG == 32)
 /* 2 CPTs, allowing more CPTs might make us under memory pressure */
-#  define LNET_CPT_MAX_BITS     1
+#define LNET_CPT_MAX_BITS	1
 
-# else /* 64-bit system */
+#else /* 64-bit system */
 /*
  * 256 CPTs for thousands of CPUs, allowing more CPTs might make us
  * under risk of consuming all lh_cookie.
  */
-#  define LNET_CPT_MAX_BITS     8
-# endif /* BITS_PER_LONG == 32 */
-#endif
+#define LNET_CPT_MAX_BITS	8
+#endif /* BITS_PER_LONG == 32 */
 
 /* max allowed CPT number */
-#define LNET_CPT_MAX	    (1 << LNET_CPT_MAX_BITS)
+#define LNET_CPT_MAX		(1 << LNET_CPT_MAX_BITS)
 
-#define LNET_CPT_NUMBER	 (the_lnet.ln_cpt_number)
-#define LNET_CPT_BITS	   (the_lnet.ln_cpt_bits)
-#define LNET_CPT_MASK	   ((1ULL << LNET_CPT_BITS) - 1)
+#define LNET_CPT_NUMBER		(the_lnet.ln_cpt_number)
+#define LNET_CPT_BITS		(the_lnet.ln_cpt_bits)
+#define LNET_CPT_MASK		((1ULL << LNET_CPT_BITS) - 1)
 
 /** exclusive lock */
-#define LNET_LOCK_EX	    CFS_PERCPT_LOCK_EX
+#define LNET_LOCK_EX		CFS_PERCPT_LOCK_EX
 
 static inline int lnet_is_wire_handle_none(lnet_handle_wire_t *wh)
 {
@@ -172,196 +158,12 @@ lnet_net_lock_current(void)
 #define lnet_eq_wait_unlock()	spin_unlock(&the_lnet.ln_eq_wait_lock)
 #define lnet_ni_lock(ni)	spin_lock(&(ni)->ni_lock)
 #define lnet_ni_unlock(ni)	spin_unlock(&(ni)->ni_lock)
-#define LNET_MUTEX_LOCK(m)	mutex_lock(m)
-#define LNET_MUTEX_UNLOCK(m)	mutex_unlock(m)
-
-#define MAX_PORTALS     64
-
-/* these are only used by code with LNET_USE_LIB_FREELIST, but we still
- * exported them to !LNET_USE_LIB_FREELIST for easy implementation */
-#define LNET_FL_MAX_MES		2048
-#define LNET_FL_MAX_MDS		2048
-#define LNET_FL_MAX_EQS		512
-#define LNET_FL_MAX_MSGS	2048    /* Outstanding messages */
-
-#ifdef LNET_USE_LIB_FREELIST
-
-int lnet_freelist_init(lnet_freelist_t *fl, int n, int size);
-void lnet_freelist_fini(lnet_freelist_t *fl);
-
-static inline void *
-lnet_freelist_alloc(lnet_freelist_t *fl)
-{
-	/* ALWAYS called with liblock held */
-	lnet_freeobj_t *o;
-
-	if (list_empty(&fl->fl_list))
-		return NULL;
-
-	o = list_entry(fl->fl_list.next, lnet_freeobj_t, fo_list);
-	list_del(&o->fo_list);
-	return (void *)&o->fo_contents;
-}
-
-static inline void
-lnet_freelist_free(lnet_freelist_t *fl, void *obj)
-{
-	/* ALWAYS called with liblock held */
-	lnet_freeobj_t *o = list_entry(obj, lnet_freeobj_t, fo_contents);
-
-	list_add(&o->fo_list, &fl->fl_list);
-}
-
-static inline lnet_eq_t *
-lnet_eq_alloc(void)
-{
-	/* NEVER called with resource lock held */
-	struct lnet_res_container *rec = &the_lnet.ln_eq_container;
-	lnet_eq_t		  *eq;
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-
-	lnet_res_lock(0);
-	eq = (lnet_eq_t *)lnet_freelist_alloc(&rec->rec_freelist);
-	lnet_res_unlock(0);
-
-	return eq;
-}
-
-static inline void
-lnet_eq_free_locked(lnet_eq_t *eq)
-{
-	/* ALWAYS called with resource lock held */
-	struct lnet_res_container *rec = &the_lnet.ln_eq_container;
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-	lnet_freelist_free(&rec->rec_freelist, eq);
-}
-
-static inline void
-lnet_eq_free(lnet_eq_t *eq)
-{
-	lnet_res_lock(0);
-	lnet_eq_free_locked(eq);
-	lnet_res_unlock(0);
-}
-
-static inline lnet_libmd_t *
-lnet_md_alloc(lnet_md_t *umd)
-{
-	/* NEVER called with resource lock held */
-	struct lnet_res_container *rec = the_lnet.ln_md_containers[0];
-	lnet_libmd_t		  *md;
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-
-	lnet_res_lock(0);
-	md = (lnet_libmd_t *)lnet_freelist_alloc(&rec->rec_freelist);
-	lnet_res_unlock(0);
-
-	if (md != NULL)
-		INIT_LIST_HEAD(&md->md_list);
-
-	return md;
-}
-
-static inline void
-lnet_md_free_locked(lnet_libmd_t *md)
-{
-	/* ALWAYS called with resource lock held */
-	struct lnet_res_container *rec = the_lnet.ln_md_containers[0];
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-	lnet_freelist_free(&rec->rec_freelist, md);
-}
-
-static inline void
-lnet_md_free(lnet_libmd_t *md)
-{
-	lnet_res_lock(0);
-	lnet_md_free_locked(md);
-	lnet_res_unlock(0);
-}
-
-static inline lnet_me_t *
-lnet_me_alloc(void)
-{
-	/* NEVER called with resource lock held */
-	struct lnet_res_container *rec = the_lnet.ln_me_containers[0];
-	lnet_me_t		  *me;
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-
-	lnet_res_lock(0);
-	me = (lnet_me_t *)lnet_freelist_alloc(&rec->rec_freelist);
-	lnet_res_unlock(0);
-
-	return me;
-}
-
-static inline void
-lnet_me_free_locked(lnet_me_t *me)
-{
-	/* ALWAYS called with resource lock held */
-	struct lnet_res_container *rec = the_lnet.ln_me_containers[0];
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-	lnet_freelist_free(&rec->rec_freelist, me);
-}
-
-static inline void
-lnet_me_free(lnet_me_t *me)
-{
-	lnet_res_lock(0);
-	lnet_me_free_locked(me);
-	lnet_res_unlock(0);
-}
-
-static inline lnet_msg_t *
-lnet_msg_alloc(void)
-{
-	/* NEVER called with network lock held */
-	struct lnet_msg_container *msc = the_lnet.ln_msg_containers[0];
-	lnet_msg_t		  *msg;
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-
-	lnet_net_lock(0);
-	msg = (lnet_msg_t *)lnet_freelist_alloc(&msc->msc_freelist);
-	lnet_net_unlock(0);
 
-	if (msg != NULL) {
-		/* NULL pointers, clear flags etc */
-		memset(msg, 0, sizeof(*msg));
-	}
-	return msg;
-}
-
-static inline void
-lnet_msg_free_locked(lnet_msg_t *msg)
-{
-	/* ALWAYS called with network lock held */
-	struct lnet_msg_container *msc = the_lnet.ln_msg_containers[0];
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-	LASSERT(!msg->msg_onactivelist);
-	lnet_freelist_free(&msc->msc_freelist, msg);
-}
-
-static inline void
-lnet_msg_free(lnet_msg_t *msg)
-{
-	lnet_net_lock(0);
-	lnet_msg_free_locked(msg);
-	lnet_net_unlock(0);
-}
-
-#else /* !LNET_USE_LIB_FREELIST */
+#define MAX_PORTALS		64
 
 static inline lnet_eq_t *
 lnet_eq_alloc(void)
 {
-	/* NEVER called with liblock held */
 	lnet_eq_t *eq;
 
 	LIBCFS_ALLOC(eq, sizeof(*eq));
@@ -371,17 +173,15 @@ lnet_eq_alloc(void)
 static inline void
 lnet_eq_free(lnet_eq_t *eq)
 {
-	/* ALWAYS called with resource lock held */
 	LIBCFS_FREE(eq, sizeof(*eq));
 }
 
 static inline lnet_libmd_t *
 lnet_md_alloc(lnet_md_t *umd)
 {
-	/* NEVER called with liblock held */
 	lnet_libmd_t *md;
-	unsigned int  size;
-	unsigned int  niov;
+	unsigned int size;
+	unsigned int niov;
 
 	if ((umd->options & LNET_MD_KIOV) != 0) {
 		niov = umd->length;
@@ -407,8 +207,7 @@ lnet_md_alloc(lnet_md_t *umd)
 static inline void
 lnet_md_free(lnet_libmd_t *md)
 {
-	/* ALWAYS called with resource lock held */
-	unsigned int  size;
+	unsigned int size;
 
 	if ((md->md_options & LNET_MD_KIOV) != 0)
 		size = offsetof(lnet_libmd_t, md_iov.kiov[md->md_niov]);
@@ -421,7 +220,6 @@ lnet_md_free(lnet_libmd_t *md)
 static inline lnet_me_t *
 lnet_me_alloc(void)
 {
-	/* NEVER called with liblock held */
 	lnet_me_t *me;
 
 	LIBCFS_ALLOC(me, sizeof(*me));
@@ -431,14 +229,12 @@ lnet_me_alloc(void)
 static inline void
 lnet_me_free(lnet_me_t *me)
 {
-	/* ALWAYS called with resource lock held */
 	LIBCFS_FREE(me, sizeof(*me));
 }
 
 static inline lnet_msg_t *
 lnet_msg_alloc(void)
 {
-	/* NEVER called with liblock held */
 	lnet_msg_t *msg;
 
 	LIBCFS_ALLOC(msg, sizeof(*msg));
@@ -450,18 +246,10 @@ lnet_msg_alloc(void)
 static inline void
 lnet_msg_free(lnet_msg_t *msg)
 {
-	/* ALWAYS called with network lock held */
 	LASSERT(!msg->msg_onactivelist);
 	LIBCFS_FREE(msg, sizeof(*msg));
 }
 
-#define lnet_eq_free_locked(eq)		lnet_eq_free(eq)
-#define lnet_md_free_locked(md)		lnet_md_free(md)
-#define lnet_me_free_locked(me)		lnet_me_free(me)
-#define lnet_msg_free_locked(msg)	lnet_msg_free(msg)
-
-#endif /* LNET_USE_LIB_FREELIST */
-
 lnet_libhandle_t *lnet_res_lh_lookup(struct lnet_res_container *rec,
 				     __u64 cookie);
 void lnet_res_lh_initialize(struct lnet_res_container *rec,
@@ -469,7 +257,6 @@ void lnet_res_lh_initialize(struct lnet_res_container *rec,
 static inline void
 lnet_res_lh_invalidate(lnet_libhandle_t *lh)
 {
-	/* ALWAYS called with resource lock held */
 	/* NB: cookie is still useful, don't reset it */
 	list_del(&lh->lh_hash_chain);
 }
@@ -488,7 +275,6 @@ lnet_eq2handle(lnet_handle_eq_t *handle, lnet_eq_t *eq)
 static inline lnet_eq_t *
 lnet_handle2eq(lnet_handle_eq_t *handle)
 {
-	/* ALWAYS called with resource lock held */
 	lnet_libhandle_t *lh;
 
 	lh = lnet_res_lh_lookup(&the_lnet.ln_eq_container, handle->cookie);
@@ -509,7 +295,7 @@ lnet_handle2md(lnet_handle_md_t *handle)
 {
 	/* ALWAYS called with resource lock held */
 	lnet_libhandle_t *lh;
-	int		 cpt;
+	int cpt;
 
 	cpt = lnet_cpt_of_cookie(handle->cookie);
 	lh = lnet_res_lh_lookup(the_lnet.ln_md_containers[cpt],
@@ -525,7 +311,7 @@ lnet_wire_handle2md(lnet_handle_wire_t *wh)
 {
 	/* ALWAYS called with resource lock held */
 	lnet_libhandle_t *lh;
-	int		 cpt;
+	int cpt;
 
 	if (wh->wh_interface_cookie != the_lnet.ln_interface_cookie)
 		return NULL;
@@ -550,7 +336,7 @@ lnet_handle2me(lnet_handle_me_t *handle)
 {
 	/* ALWAYS called with resource lock held */
 	lnet_libhandle_t *lh;
-	int		 cpt;
+	int cpt;
 
 	cpt = lnet_cpt_of_cookie(handle->cookie);
 	lh = lnet_res_lh_lookup(the_lnet.ln_me_containers[cpt],
@@ -644,6 +430,9 @@ lnet_ni_t *lnet_nid2ni_locked(lnet_nid_t nid, int cpt);
 lnet_ni_t *lnet_net2ni_locked(__u32 net, int cpt);
 lnet_ni_t *lnet_net2ni(__u32 net);
 
+int lnet_init(void);
+void lnet_fini(void);
+
 int lnet_notify(lnet_ni_t *ni, lnet_nid_t peer, int alive, unsigned long when);
 void lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive,
 			unsigned long when);
@@ -737,7 +526,9 @@ void lnet_recv(lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
 	       unsigned int offset, unsigned int mlen, unsigned int rlen);
 lnet_msg_t *lnet_create_reply_msg(lnet_ni_t *ni, lnet_msg_t *get_msg);
 void lnet_set_reply_msg_len(lnet_ni_t *ni, lnet_msg_t *msg, unsigned int len);
+
 void lnet_finalize(lnet_ni_t *ni, lnet_msg_t *msg, int rc);
+
 void lnet_drop_delayed_msg_list(struct list_head *head, char *reason);
 void lnet_recv_delayed_msg_list(struct list_head *head);
 
@@ -830,7 +621,6 @@ void lnet_md_deconstruct(lnet_libmd_t *lmd, lnet_md_t *umd);
 
 void lnet_register_lnd(lnd_t *lnd);
 void lnet_unregister_lnd(lnd_t *lnd);
-int lnet_set_ip_niaddr(lnet_ni_t *ni);
 
 int lnet_connect(struct socket **sockp, lnet_nid_t peer_nid,
 		 __u32 local_ip, __u32 peer_ip, int peer_port);
@@ -846,7 +636,22 @@ int lnet_acceptor_port(void);
 int lnet_acceptor_start(void);
 void lnet_acceptor_stop(void);
 
-void lnet_get_tunables(void);
+int lnet_ipif_query(char *name, int *up, __u32 *ip, __u32 *mask);
+int lnet_ipif_enumerate(char ***names);
+void lnet_ipif_free_enumeration(char **names, int n);
+int lnet_sock_setbuf(struct socket *socket, int txbufsize, int rxbufsize);
+int lnet_sock_getbuf(struct socket *socket, int *txbufsize, int *rxbufsize);
+int lnet_sock_getaddr(struct socket *socket, bool remote, __u32 *ip, int *port);
+int lnet_sock_write(struct socket *sock, void *buffer, int nob, int timeout);
+int lnet_sock_read(struct socket *sock, void *buffer, int nob, int timeout);
+
+int lnet_sock_listen(struct socket **sockp, __u32 ip, int port, int backlog);
+int lnet_sock_accept(struct socket **newsockp, struct socket *sock);
+int lnet_sock_connect(struct socket **sockp, int *fatal,
+		      __u32 local_ip, int local_port,
+		      __u32 peer_ip, int peer_port);
+void libcfs_sock_release(struct socket *sock);
+
 int lnet_peers_start_down(void);
 int lnet_peer_buffer_credits(lnet_ni_t *ni);
 
@@ -872,12 +677,12 @@ void lnet_peer_tables_destroy(void);
 int lnet_peer_tables_create(void);
 void lnet_debug_peer(lnet_nid_t nid);
 
-static inline void lnet_peer_set_alive(lnet_peer_t *lp)
+static inline void
+lnet_peer_set_alive(lnet_peer_t *lp)
 {
 	lp->lp_last_alive = lp->lp_last_query = get_seconds();
 	if (!lp->lp_alive)
 		lnet_notify_locked(lp, 0, 1, lp->lp_last_alive);
 }
 
-
 #endif
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-types.h b/drivers/staging/lustre/include/linux/lnet/lib-types.h
index 50537668f59d..81a63dbdea25 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-types.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-types.h
@@ -15,11 +15,7 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
@@ -27,155 +23,41 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012 - 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
+ * Lustre is a trademark of Seagate, Inc.
  *
  * lnet/include/lnet/lib-types.h
- *
- * Types used by the library side routines that do not need to be
- * exposed to the user application
  */
 
 #ifndef __LNET_LIB_TYPES_H__
 #define __LNET_LIB_TYPES_H__
 
-#include "linux/lib-types.h"
+#include <linux/kthread.h>
+#include <linux/uio.h>
+#include <linux/types.h>
+#include <net/sock.h>
 
-#include "../libcfs/libcfs.h"
-#include <linux/list.h>
 #include "types.h"
 
-#define WIRE_ATTR       __attribute__((packed))
-
-/* Packed version of lnet_process_id_t to transfer via network */
-typedef struct {
-	lnet_nid_t nid;
-	lnet_pid_t pid;   /* node id / process id */
-} WIRE_ATTR lnet_process_id_packed_t;
-
-/* The wire handle's interface cookie only matches one network interface in
- * one epoch (i.e. new cookie when the interface restarts or the node
- * reboots).  The object cookie only matches one object on that interface
- * during that object's lifetime (i.e. no cookie re-use). */
-typedef struct {
-	__u64 wh_interface_cookie;
-	__u64 wh_object_cookie;
-} WIRE_ATTR lnet_handle_wire_t;
-
-typedef enum {
-	LNET_MSG_ACK = 0,
-	LNET_MSG_PUT,
-	LNET_MSG_GET,
-	LNET_MSG_REPLY,
-	LNET_MSG_HELLO,
-} lnet_msg_type_t;
-
-/* The variant fields of the portals message header are aligned on an 8
- * byte boundary in the message header.  Note that all types used in these
- * wire structs MUST be fixed size and the smaller types are placed at the
- * end. */
-typedef struct lnet_ack {
-	lnet_handle_wire_t  dst_wmd;
-	__u64	       match_bits;
-	__u32	       mlength;
-} WIRE_ATTR lnet_ack_t;
-
-typedef struct lnet_put {
-	lnet_handle_wire_t  ack_wmd;
-	__u64	       match_bits;
-	__u64	       hdr_data;
-	__u32	       ptl_index;
-	__u32	       offset;
-} WIRE_ATTR lnet_put_t;
-
-typedef struct lnet_get {
-	lnet_handle_wire_t  return_wmd;
-	__u64	       match_bits;
-	__u32	       ptl_index;
-	__u32	       src_offset;
-	__u32	       sink_length;
-} WIRE_ATTR lnet_get_t;
-
-typedef struct lnet_reply {
-	lnet_handle_wire_t  dst_wmd;
-} WIRE_ATTR lnet_reply_t;
-
-typedef struct lnet_hello {
-	__u64	      incarnation;
-	__u32	      type;
-} WIRE_ATTR lnet_hello_t;
-
-typedef struct {
-	lnet_nid_t	  dest_nid;
-	lnet_nid_t	  src_nid;
-	lnet_pid_t	  dest_pid;
-	lnet_pid_t	  src_pid;
-	__u32	       type;	       /* lnet_msg_type_t */
-	__u32	       payload_length;     /* payload data to follow */
-	/*<------__u64 aligned------->*/
-	union {
-		lnet_ack_t   ack;
-		lnet_put_t   put;
-		lnet_get_t   get;
-		lnet_reply_t reply;
-		lnet_hello_t hello;
-	} msg;
-} WIRE_ATTR lnet_hdr_t;
-
-/* A HELLO message contains a magic number and protocol version
- * code in the header's dest_nid, the peer's NID in the src_nid, and
- * LNET_MSG_HELLO in the type field.  All other common fields are zero
- * (including payload_size; i.e. no payload).
- * This is for use by byte-stream LNDs (e.g. TCP/IP) to check the peer is
- * running the same protocol and to find out its NID. These LNDs should
- * exchange HELLO messages when a connection is first established.  Individual
- * LNDs can put whatever else they fancy in lnet_hdr_t::msg.
- */
-typedef struct {
-	__u32   magic;			  /* LNET_PROTO_TCP_MAGIC */
-	__u16   version_major;		  /* increment on incompatible change */
-	__u16   version_minor;		  /* increment on compatible change */
-} WIRE_ATTR lnet_magicversion_t;
-
-/* PROTO MAGIC for LNDs */
-#define LNET_PROTO_IB_MAGIC		 0x0be91b91
-#define LNET_PROTO_RA_MAGIC		 0x0be91b92
-#define LNET_PROTO_QSW_MAGIC		0x0be91b93
-#define LNET_PROTO_GNI_MAGIC		0xb00fbabe /* ask Kim */
-#define LNET_PROTO_TCP_MAGIC		0xeebc0ded
-#define LNET_PROTO_PTL_MAGIC		0x50746C4E /* 'PtlN' unique magic */
-#define LNET_PROTO_MX_MAGIC		 0x4d583130 /* 'MX10'! */
-#define LNET_PROTO_ACCEPTOR_MAGIC	   0xacce7100
-#define LNET_PROTO_PING_MAGIC	       0x70696E67 /* 'ping' */
-
-/* Placeholder for a future "unified" protocol across all LNDs */
-/* Current LNDs that receive a request with this magic will respond with a
- * "stub" reply using their current protocol */
-#define LNET_PROTO_MAGIC		    0x45726963 /* ! */
-
-#define LNET_PROTO_TCP_VERSION_MAJOR	1
-#define LNET_PROTO_TCP_VERSION_MINOR	0
-
-/* Acceptor connection request */
-typedef struct {
-	__u32       acr_magic;		  /* PTL_ACCEPTOR_PROTO_MAGIC */
-	__u32       acr_version;		/* protocol version */
-	__u64       acr_nid;		    /* target NID */
-} WIRE_ATTR lnet_acceptor_connreq_t;
-
-#define LNET_PROTO_ACCEPTOR_VERSION       1
+/* Max payload size */
+#define LNET_MAX_PAYLOAD      CONFIG_LNET_MAX_PAYLOAD
+#if (LNET_MAX_PAYLOAD < LNET_MTU)
+# error "LNET_MAX_PAYLOAD too small - error in configure --with-max-payload-mb"
+#elif (LNET_MAX_PAYLOAD > (PAGE_SIZE * LNET_MAX_IOV))
+# error "LNET_MAX_PAYLOAD too large - error in configure --with-max-payload-mb"
+#endif
 
 /* forward refs */
 struct lnet_libmd;
 
 typedef struct lnet_msg {
-	struct list_head	    msg_activelist;
-	struct list_head	    msg_list;	   /* Q for credits/MD */
+	struct list_head	msg_activelist;
+	struct list_head	msg_list;	   /* Q for credits/MD */
 
-	lnet_process_id_t     msg_target;
+	lnet_process_id_t	msg_target;
 	/* where is it from, it's only for building event */
 	lnet_nid_t		msg_from;
 	__u32			msg_type;
@@ -195,127 +77,114 @@ typedef struct lnet_msg {
 	/* ready for pending on RX delay list */
 	unsigned int		msg_rx_ready_delay:1;
 
-	unsigned int	  msg_vmflush:1;      /* VM trying to free memory */
-	unsigned int	  msg_target_is_router:1; /* sending to a router */
-	unsigned int	  msg_routing:1;      /* being forwarded */
-	unsigned int	  msg_ack:1;	  /* ack on finalize (PUT) */
-	unsigned int	  msg_sending:1;      /* outgoing message */
-	unsigned int	  msg_receiving:1;    /* being received */
-	unsigned int	  msg_txcredit:1;     /* taken an NI send credit */
-	unsigned int	  msg_peertxcredit:1; /* taken a peer send credit */
-	unsigned int	  msg_rtrcredit:1;    /* taken a global router credit */
-	unsigned int	  msg_peerrtrcredit:1; /* taken a peer router credit */
-	unsigned int	  msg_onactivelist:1; /* on the activelist */
-
-	struct lnet_peer     *msg_txpeer;	 /* peer I'm sending to */
-	struct lnet_peer     *msg_rxpeer;	 /* peer I received from */
-
-	void		 *msg_private;
-	struct lnet_libmd    *msg_md;
-
-	unsigned int	  msg_len;
-	unsigned int	  msg_wanted;
-	unsigned int	  msg_offset;
-	unsigned int	  msg_niov;
-	struct kvec	 *msg_iov;
-	lnet_kiov_t	  *msg_kiov;
-
-	lnet_event_t	  msg_ev;
-	lnet_hdr_t	    msg_hdr;
+	unsigned int	msg_vmflush:1;		/* VM trying to free memory */
+	unsigned int	msg_target_is_router:1; /* sending to a router */
+	unsigned int	msg_routing:1;		/* being forwarded */
+	unsigned int	msg_ack:1;		/* ack on finalize (PUT) */
+	unsigned int	msg_sending:1;		/* outgoing message */
+	unsigned int	msg_receiving:1;	/* being received */
+	unsigned int	msg_txcredit:1;		/* taken an NI send credit */
+	unsigned int	msg_peertxcredit:1;	/* taken a peer send credit */
+	unsigned int	msg_rtrcredit:1;	/* taken a global
+						   router credit */
+	unsigned int	msg_peerrtrcredit:1;	/* taken a peer router credit */
+	unsigned int	msg_onactivelist:1;	/* on the activelist */
+
+	struct lnet_peer	*msg_txpeer;	 /* peer I'm sending to */
+	struct lnet_peer	*msg_rxpeer;	 /* peer I received from */
+
+	void			*msg_private;
+	struct lnet_libmd	*msg_md;
+
+	unsigned int		 msg_len;
+	unsigned int		 msg_wanted;
+	unsigned int		 msg_offset;
+	unsigned int		 msg_niov;
+	struct kvec		*msg_iov;
+	lnet_kiov_t		*msg_kiov;
+
+	lnet_event_t		 msg_ev;
+	lnet_hdr_t		 msg_hdr;
 } lnet_msg_t;
 
 typedef struct lnet_libhandle {
-	struct list_head	    lh_hash_chain;
-	__u64		 lh_cookie;
+	struct list_head	lh_hash_chain;
+	__u64			lh_cookie;
 } lnet_libhandle_t;
 
 #define lh_entry(ptr, type, member) \
 	((type *)((char *)(ptr)-(char *)(&((type *)0)->member)))
 
 typedef struct lnet_eq {
-	struct list_head		eq_list;
-	lnet_libhandle_t	eq_lh;
-	lnet_seq_t		eq_enq_seq;
-	lnet_seq_t		eq_deq_seq;
-	unsigned int		eq_size;
-	lnet_eq_handler_t	eq_callback;
-	lnet_event_t		*eq_events;
+	struct list_head	  eq_list;
+	lnet_libhandle_t	  eq_lh;
+	lnet_seq_t		  eq_enq_seq;
+	lnet_seq_t		  eq_deq_seq;
+	unsigned int		  eq_size;
+	lnet_eq_handler_t	  eq_callback;
+	lnet_event_t		 *eq_events;
 	int			**eq_refs;	/* percpt refcount for EQ */
 } lnet_eq_t;
 
 typedef struct lnet_me {
-	struct list_head	     me_list;
-	lnet_libhandle_t       me_lh;
-	lnet_process_id_t      me_match_id;
-	unsigned int	   me_portal;
-	unsigned int	   me_pos;		/* hash offset in mt_hash */
-	__u64		  me_match_bits;
-	__u64		  me_ignore_bits;
-	lnet_unlink_t	  me_unlink;
-	struct lnet_libmd     *me_md;
+	struct list_head	 me_list;
+	lnet_libhandle_t	 me_lh;
+	lnet_process_id_t	 me_match_id;
+	unsigned int		 me_portal;
+	unsigned int		 me_pos;	/* hash offset in mt_hash */
+	__u64			 me_match_bits;
+	__u64			 me_ignore_bits;
+	lnet_unlink_t		 me_unlink;
+	struct lnet_libmd	*me_md;
 } lnet_me_t;
 
 typedef struct lnet_libmd {
-	struct list_head	    md_list;
-	lnet_libhandle_t      md_lh;
-	lnet_me_t	    *md_me;
-	char		 *md_start;
-	unsigned int	  md_offset;
-	unsigned int	  md_length;
-	unsigned int	  md_max_size;
-	int		   md_threshold;
-	int		   md_refcount;
-	unsigned int	  md_options;
-	unsigned int	  md_flags;
-	void		 *md_user_ptr;
-	lnet_eq_t	    *md_eq;
-	unsigned int	  md_niov;		/* # frags */
+	struct list_head	 md_list;
+	lnet_libhandle_t	 md_lh;
+	lnet_me_t		*md_me;
+	char			*md_start;
+	unsigned int		 md_offset;
+	unsigned int		 md_length;
+	unsigned int		 md_max_size;
+	int			 md_threshold;
+	int			 md_refcount;
+	unsigned int		 md_options;
+	unsigned int		 md_flags;
+	void			*md_user_ptr;
+	lnet_eq_t		*md_eq;
+	unsigned int		 md_niov;	/* # frags */
 	union {
-		struct kvec   iov[LNET_MAX_IOV];
-		lnet_kiov_t   kiov[LNET_MAX_IOV];
+		struct kvec	iov[LNET_MAX_IOV];
+		lnet_kiov_t	kiov[LNET_MAX_IOV];
 	} md_iov;
 } lnet_libmd_t;
 
-#define LNET_MD_FLAG_ZOMBIE	   (1 << 0)
-#define LNET_MD_FLAG_AUTO_UNLINK      (1 << 1)
-#define LNET_MD_FLAG_ABORTED	 (1 << 2)
-
-#ifdef LNET_USE_LIB_FREELIST
-typedef struct {
-	void		  *fl_objs;	  /* single contiguous array of objects */
-	int		    fl_nobjs;	 /* the number of them */
-	int		    fl_objsize;       /* the size (including overhead) of each of them */
-	struct list_head	     fl_list;	  /* where they are enqueued */
-} lnet_freelist_t;
-
-typedef struct {
-	struct list_head	     fo_list;	     /* enqueue on fl_list */
-	void		  *fo_contents;	 /* aligned contents */
-} lnet_freeobj_t;
-#endif
+#define LNET_MD_FLAG_ZOMBIE		(1 << 0)
+#define LNET_MD_FLAG_AUTO_UNLINK	(1 << 1)
+#define LNET_MD_FLAG_ABORTED		(1 << 2)
 
 typedef struct {
 	/* info about peers we are trying to fail */
-	struct list_head	     tp_list;	     /* ln_test_peers */
-	lnet_nid_t	     tp_nid;	      /* matching nid */
-	unsigned int	   tp_threshold;	/* # failures to simulate */
+	struct list_head	tp_list;	/* ln_test_peers */
+	lnet_nid_t		tp_nid;		/* matching nid */
+	unsigned int		tp_threshold;	/* # failures to simulate */
 } lnet_test_peer_t;
 
-#define LNET_COOKIE_TYPE_MD    1
-#define LNET_COOKIE_TYPE_ME    2
-#define LNET_COOKIE_TYPE_EQ    3
-#define LNET_COOKIE_TYPE_BITS  2
+#define LNET_COOKIE_TYPE_MD	1
+#define LNET_COOKIE_TYPE_ME	2
+#define LNET_COOKIE_TYPE_EQ	3
+#define LNET_COOKIE_TYPE_BITS	2
 #define LNET_COOKIE_MASK	((1ULL << LNET_COOKIE_TYPE_BITS) - 1ULL)
 
-struct lnet_ni;				  /* forward ref */
+struct lnet_ni;			/* forward ref */
 
 typedef struct lnet_lnd {
 	/* fields managed by portals */
-	struct list_head	    lnd_list;	     /* stash in the LND table */
-	int		   lnd_refcount;	 /* # active instances */
+	struct list_head	lnd_list;	/* stash in the LND table */
+	int			lnd_refcount;	/* # active instances */
 
 	/* fields initialised by the LND */
-	unsigned int	  lnd_type;
+	__u32			lnd_type;
 
 	int  (*lnd_startup)(struct lnet_ni *ni);
 	void (*lnd_shutdown)(struct lnet_ni *ni);
@@ -347,7 +216,8 @@ typedef struct lnet_lnd {
 	int (*lnd_recv)(struct lnet_ni *ni, void *private, lnet_msg_t *msg,
 			int delayed, unsigned int niov,
 			struct kvec *iov, lnet_kiov_t *kiov,
-			unsigned int offset, unsigned int mlen, unsigned int rlen);
+			unsigned int offset, unsigned int mlen,
+			unsigned int rlen);
 
 	/* lnet_parse() has had to delay processing of this message
 	 * (e.g. waiting for a forwarding buffer or send credits).  Give the
@@ -355,60 +225,49 @@ typedef struct lnet_lnd {
 	 * for success and do NOT give back a receive credit; that has to wait
 	 * until lnd_recv() gets called.  On failure return < 0 and
 	 * release resources; lnd_recv() will not be called. */
-	int (*lnd_eager_recv)(struct lnet_ni *ni, void *private, lnet_msg_t *msg,
-			      void **new_privatep);
+	int (*lnd_eager_recv)(struct lnet_ni *ni, void *private,
+			      lnet_msg_t *msg, void **new_privatep);
 
 	/* notification of peer health */
 	void (*lnd_notify)(struct lnet_ni *ni, lnet_nid_t peer, int alive);
 
 	/* query of peer aliveness */
-	void (*lnd_query)(struct lnet_ni *ni, lnet_nid_t peer, unsigned long *when);
+	void (*lnd_query)(struct lnet_ni *ni, lnet_nid_t peer,
+			  unsigned long *when);
 
 	/* accept a new connection */
 	int (*lnd_accept)(struct lnet_ni *ni, struct socket *sock);
-
 } lnd_t;
 
-#define LNET_NI_STATUS_UP      0x15aac0de
-#define LNET_NI_STATUS_DOWN    0xdeadface
-#define LNET_NI_STATUS_INVALID 0x00000000
-typedef struct {
-	lnet_nid_t ns_nid;
-	__u32      ns_status;
-	__u32      ns_unused;
-} WIRE_ATTR lnet_ni_status_t;
-
 struct lnet_tx_queue {
 	int			tq_credits;	/* # tx credits free */
 	int			tq_credits_min;	/* lowest it's been */
 	int			tq_credits_max;	/* total # tx credits */
-	struct list_head		tq_delayed;	/* delayed TXs */
+	struct list_head	tq_delayed;	/* delayed TXs */
 };
 
-#define LNET_MAX_INTERFACES   16
-
 typedef struct lnet_ni {
-	spinlock_t		ni_lock;
-	struct list_head		ni_list;	/* chain on ln_nis */
-	struct list_head		ni_cptlist;	/* chain on ln_nis_cpt */
-	int			ni_maxtxcredits; /* # tx credits  */
+	spinlock_t		  ni_lock;
+	struct list_head	  ni_list;	/* chain on ln_nis */
+	struct list_head	  ni_cptlist;	/* chain on ln_nis_cpt */
+	int			  ni_maxtxcredits; /* # tx credits  */
 	/* # per-peer send credits */
-	int			ni_peertxcredits;
+	int			  ni_peertxcredits;
 	/* # per-peer router buffer credits */
-	int			ni_peerrtrcredits;
+	int			  ni_peerrtrcredits;
 	/* seconds to consider peer dead */
-	int			ni_peertimeout;
-	int			ni_ncpts;	/* number of CPTs */
-	__u32			*ni_cpts;	/* bond NI on some CPTs */
-	lnet_nid_t		ni_nid;		/* interface's NID */
-	void			*ni_data;	/* instance-specific data */
-	lnd_t			*ni_lnd;	/* procedural interface */
+	int			  ni_peertimeout;
+	int			  ni_ncpts;	/* number of CPTs */
+	__u32			 *ni_cpts;	/* bond NI on some CPTs */
+	lnet_nid_t		  ni_nid;	/* interface's NID */
+	void			 *ni_data;	/* instance-specific data */
+	lnd_t			 *ni_lnd;	/* procedural interface */
 	struct lnet_tx_queue	**ni_tx_queues;	/* percpt TX queues */
 	int			**ni_refs;	/* percpt reference count */
-	long			ni_last_alive;	/* when I was last alive */
-	lnet_ni_status_t	*ni_status;	/* my health status */
+	long			  ni_last_alive;/* when I was last alive */
+	lnet_ni_status_t	 *ni_status;	/* my health status */
 	/* equivalent interfaces to use */
-	char			*ni_interfaces[LNET_MAX_INTERFACES];
+	char			 *ni_interfaces[LNET_MAX_INTERFACES];
 } lnet_ni_t;
 
 #define LNET_PROTO_PING_MATCHBITS	0x8000000000000000LL
@@ -422,68 +281,70 @@ typedef struct lnet_ni {
 #define LNET_PING_FEAT_MASK		(LNET_PING_FEAT_BASE | \
 					 LNET_PING_FEAT_NI_STATUS)
 
-typedef struct {
-	__u32			pi_magic;
-	__u32			pi_features;
-	lnet_pid_t		pi_pid;
-	__u32			pi_nnis;
-	lnet_ni_status_t	pi_ni[0];
-} WIRE_ATTR lnet_ping_info_t;
-
 /* router checker data, per router */
 #define LNET_MAX_RTR_NIS   16
 #define LNET_PINGINFO_SIZE offsetof(lnet_ping_info_t, pi_ni[LNET_MAX_RTR_NIS])
 typedef struct {
 	/* chain on the_lnet.ln_zombie_rcd or ln_deathrow_rcd */
-	struct list_head		rcd_list;
-	lnet_handle_md_t	rcd_mdh;	/* ping buffer MD */
+	struct list_head	 rcd_list;
+	lnet_handle_md_t	 rcd_mdh;	/* ping buffer MD */
 	struct lnet_peer	*rcd_gateway;	/* reference to gateway */
 	lnet_ping_info_t	*rcd_pinginfo;	/* ping buffer */
 } lnet_rc_data_t;
 
 typedef struct lnet_peer {
-	struct list_head	lp_hashlist;	  /* chain on peer hash */
-	struct list_head	lp_txq;	       /* messages blocking for tx credits */
-	struct list_head	lp_rtrq;	      /* messages blocking for router credits */
-	struct list_head	lp_rtr_list;	  /* chain on router list */
-	int	       lp_txcredits;	 /* # tx credits available */
-	int	       lp_mintxcredits;      /* low water mark */
-	int	       lp_rtrcredits;	/* # router credits */
-	int	       lp_minrtrcredits;     /* low water mark */
-	unsigned int      lp_alive:1;	   /* alive/dead? */
-	unsigned int      lp_notify:1;	  /* notification outstanding? */
-	unsigned int      lp_notifylnd:1;       /* outstanding notification for LND? */
-	unsigned int      lp_notifying:1;       /* some thread is handling notification */
-	unsigned int      lp_ping_notsent;      /* SEND event outstanding from ping */
-	int	       lp_alive_count;       /* # times router went dead<->alive */
-	long	      lp_txqnob;	    /* bytes queued for sending */
-	unsigned long	lp_timestamp;	 /* time of last aliveness news */
-	unsigned long	lp_ping_timestamp;    /* time of last ping attempt */
-	unsigned long	lp_ping_deadline;     /* != 0 if ping reply expected */
-	unsigned long	lp_last_alive;	/* when I was last alive */
-	unsigned long	lp_last_query;	/* when lp_ni was queried last time */
-	lnet_ni_t	*lp_ni;		/* interface peer is on */
-	lnet_nid_t	lp_nid;	       /* peer's NID */
-	int	       lp_refcount;	  /* # refs */
-	int			lp_cpt;		/* CPT this peer attached on */
+	struct list_head	 lp_hashlist;	/* chain on peer hash */
+	struct list_head	 lp_txq;	/* messages blocking for
+						   tx credits */
+	struct list_head	 lp_rtrq;	/* messages blocking for
+						   router credits */
+	struct list_head	 lp_rtr_list;	/* chain on router list */
+	int			 lp_txcredits;	/* # tx credits available */
+	int			 lp_mintxcredits;  /* low water mark */
+	int			 lp_rtrcredits;	   /* # router credits */
+	int			 lp_minrtrcredits; /* low water mark */
+	unsigned int		 lp_alive:1;	   /* alive/dead? */
+	unsigned int		 lp_notify:1;	/* notification outstanding? */
+	unsigned int		 lp_notifylnd:1;/* outstanding notification
+						   for LND? */
+	unsigned int		 lp_notifying:1; /* some thread is handling
+						    notification */
+	unsigned int		 lp_ping_notsent;/* SEND event outstanding
+						    from ping */
+	int			 lp_alive_count; /* # times router went
+						    dead<->alive */
+	long			 lp_txqnob;	 /* bytes queued for sending */
+	unsigned long		 lp_timestamp;	 /* time of last aliveness
+						    news */
+	unsigned long		 lp_ping_timestamp;/* time of last ping
+						      attempt */
+	unsigned long		 lp_ping_deadline; /* != 0 if ping reply
+						      expected */
+	unsigned long		 lp_last_alive;	/* when I was last alive */
+	unsigned long		 lp_last_query;	/* when lp_ni was queried
+						   last time */
+	lnet_ni_t		*lp_ni;		/* interface peer is on */
+	lnet_nid_t		 lp_nid;	/* peer's NID */
+	int			 lp_refcount;	/* # refs */
+	int			 lp_cpt;	/* CPT this peer attached on */
 	/* # refs from lnet_route_t::lr_gateway */
-	int			lp_rtr_refcount;
+	int			 lp_rtr_refcount;
 	/* returned RC ping features */
-	unsigned int		lp_ping_feats;
-	struct list_head		lp_routes;	/* routers on this peer */
+	unsigned int		 lp_ping_feats;
+	struct list_head	 lp_routes;	/* routers on this peer */
 	lnet_rc_data_t		*lp_rcd;	/* router checker state */
 } lnet_peer_t;
 
 /* peer hash size */
-#define LNET_PEER_HASH_BITS     9
-#define LNET_PEER_HASH_SIZE     (1 << LNET_PEER_HASH_BITS)
+#define LNET_PEER_HASH_BITS	9
+#define LNET_PEER_HASH_SIZE	(1 << LNET_PEER_HASH_BITS)
 
 /* peer hash table */
 struct lnet_peer_table {
-	int			pt_version;	/* /proc validity stamp */
-	int			pt_number;	/* # peers extant */
-	struct list_head		pt_deathrow;	/* zombie peers */
-	struct list_head		*pt_hash;	/* NID->peer hash */
+	int			 pt_version;	/* /proc validity stamp */
+	int			 pt_number;	/* # peers extant */
+	struct list_head	 pt_deathrow;	/* zombie peers */
+	struct list_head	*pt_hash;	/* NID->peer hash */
 };
 
 /* peer aliveness is enabled only on routers for peers in a network where the
@@ -492,14 +353,14 @@ struct lnet_peer_table {
 					 (lp)->lp_ni->ni_peertimeout > 0)
 
 typedef struct {
-	struct list_head		lr_list;	/* chain on net */
-	struct list_head		lr_gwlist;	/* chain on gateway */
+	struct list_head	 lr_list;	/* chain on net */
+	struct list_head	 lr_gwlist;	/* chain on gateway */
 	lnet_peer_t		*lr_gateway;	/* router node */
-	__u32			lr_net;		/* remote network number */
-	int			lr_seq;		/* sequence for round-robin */
-	unsigned int		lr_downis;	/* number of down NIs */
-	unsigned int		lr_hops;	/* how far I am */
-	unsigned int            lr_priority;    /* route priority */
+	__u32			 lr_net;	/* remote network number */
+	int			 lr_seq;	/* sequence for round-robin */
+	unsigned int		 lr_downis;	/* number of down NIs */
+	unsigned int		 lr_hops;	/* how far I am */
+	unsigned int             lr_priority;	/* route priority */
 } lnet_route_t;
 
 #define LNET_REMOTE_NETS_HASH_DEFAULT	(1U << 7)
@@ -507,43 +368,37 @@ typedef struct {
 #define LNET_REMOTE_NETS_HASH_SIZE	(1 << the_lnet.ln_remote_nets_hbits)
 
 typedef struct {
-	struct list_head	      lrn_list;       /* chain on ln_remote_nets_hash */
-	struct list_head	      lrn_routes;     /* routes to me */
-	__u32		   lrn_net;	/* my net number */
+	struct list_head	lrn_list;	/* chain on
+						   ln_remote_nets_hash */
+	struct list_head	lrn_routes;	/* routes to me */
+	__u32			lrn_net;	/* my net number */
 } lnet_remotenet_t;
 
+/** lnet message has credit and can be submitted to lnd for send/receive */
+#define LNET_CREDIT_OK		0
+/** lnet message is waiting for credit */
+#define LNET_CREDIT_WAIT	1
+
 typedef struct {
-	struct list_head rbp_bufs;	     /* my free buffer pool */
-	struct list_head rbp_msgs;	     /* messages blocking for a buffer */
-	int	rbp_npages;	   /* # pages in each buffer */
-	int	rbp_nbuffers;	 /* # buffers */
-	int	rbp_credits;	  /* # free buffers / blocked messages */
-	int	rbp_mincredits;       /* low water mark */
+	struct list_head	rbp_bufs;	/* my free buffer pool */
+	struct list_head	rbp_msgs;	/* messages blocking
+						   for a buffer */
+	int			rbp_npages;	/* # pages in each buffer */
+	int			rbp_nbuffers;	/* # buffers */
+	int			rbp_credits;	/* # free buffers /
+						     blocked messages */
+	int			rbp_mincredits;	/* low water mark */
 } lnet_rtrbufpool_t;
 
 typedef struct {
-	struct list_head	     rb_list;	     /* chain on rbp_bufs */
-	lnet_rtrbufpool_t     *rb_pool;	     /* owning pool */
-	lnet_kiov_t	    rb_kiov[0];	  /* the buffer space */
+	struct list_head	 rb_list;	/* chain on rbp_bufs */
+	lnet_rtrbufpool_t	*rb_pool;	/* owning pool */
+	lnet_kiov_t		 rb_kiov[0];	/* the buffer space */
 } lnet_rtrbuf_t;
 
-typedef struct {
-	__u32	msgs_alloc;
-	__u32	msgs_max;
-	__u32	errors;
-	__u32	send_count;
-	__u32	recv_count;
-	__u32	route_count;
-	__u32	drop_count;
-	__u64	send_length;
-	__u64	recv_length;
-	__u64	route_length;
-	__u64	drop_length;
-} WIRE_ATTR lnet_counters_t;
-
-#define LNET_PEER_HASHSIZE   503		/* prime! */
-
-#define LNET_NRBPOOLS	 3		 /* # different router buffer pools */
+#define LNET_PEER_HASHSIZE	503	/* prime! */
+
+#define LNET_NRBPOOLS		3	/* # different router buffer pools */
 
 enum {
 	/* Didn't match anything */
@@ -553,15 +408,16 @@ enum {
 	/* Must be discarded */
 	LNET_MATCHMD_DROP	= (1 << 2),
 	/* match and buffer is exhausted */
-	LNET_MATCHMD_EXHAUSTED  = (1 << 3),
+	LNET_MATCHMD_EXHAUSTED	= (1 << 3),
 	/* match or drop */
-	LNET_MATCHMD_FINISH     = (LNET_MATCHMD_OK | LNET_MATCHMD_DROP),
+	LNET_MATCHMD_FINISH	= (LNET_MATCHMD_OK | LNET_MATCHMD_DROP),
 };
 
 /* Options for lnet_portal_t::ptl_options */
-#define LNET_PTL_LAZY	       (1 << 0)
-#define LNET_PTL_MATCH_UNIQUE       (1 << 1)    /* unique match, for RDMA */
-#define LNET_PTL_MATCH_WILDCARD     (1 << 2)    /* wildcard match, request portal */
+#define LNET_PTL_LAZY		(1 << 0)
+#define LNET_PTL_MATCH_UNIQUE	(1 << 1)	/* unique match, for RDMA */
+#define LNET_PTL_MATCH_WILDCARD	(1 << 2)	/* wildcard match,
+						   request portal */
 
 /* parameter for matching operations (GET, PUT) */
 struct lnet_match_info {
@@ -590,14 +446,14 @@ struct lnet_match_info {
 /* portal match table */
 struct lnet_match_table {
 	/* reserved for upcoming patches, CPU partition ID */
-	unsigned int		mt_cpt;
-	unsigned int		mt_portal;      /* portal index */
+	unsigned int		 mt_cpt;
+	unsigned int		 mt_portal;	/* portal index */
 	/* match table is set as "enabled" if there's non-exhausted MD
 	 * attached on mt_mhash, it's only valid for wildcard portal */
-	unsigned int		mt_enabled;
+	unsigned int		 mt_enabled;
 	/* bitmap to flag whether MEs on mt_hash are exhausted or not */
-	__u64			mt_exhausted[LNET_MT_EXHAUSTED_BMAP];
-	struct list_head		*mt_mhash;      /* matching hash */
+	__u64			 mt_exhausted[LNET_MT_EXHAUSTED_BMAP];
+	struct list_head	*mt_mhash;	/* matching hash */
 };
 
 /* these are only useful for wildcard portal */
@@ -611,22 +467,22 @@ struct lnet_match_table {
 #define	LNET_PTL_ROTOR_HASH_RT	3
 
 typedef struct lnet_portal {
-	spinlock_t		ptl_lock;
-	unsigned int		ptl_index;	/* portal ID, reserved */
+	spinlock_t		  ptl_lock;
+	unsigned int		  ptl_index;	/* portal ID, reserved */
 	/* flags on this portal: lazy, unique... */
-	unsigned int		ptl_options;
+	unsigned int		  ptl_options;
 	/* list of messages which are stealing buffer */
-	struct list_head		ptl_msg_stealing;
+	struct list_head	  ptl_msg_stealing;
 	/* messages blocking for MD */
-	struct list_head		ptl_msg_delayed;
+	struct list_head	  ptl_msg_delayed;
 	/* Match table for each CPT */
 	struct lnet_match_table	**ptl_mtables;
 	/* spread rotor of incoming "PUT" */
-	unsigned int		ptl_rotor;
+	unsigned int		  ptl_rotor;
 	/* # active entries for this portal */
-	int		     ptl_mt_nmaps;
+	int			  ptl_mt_nmaps;
 	/* array of active entries' cpu-partition-id */
-	int		     ptl_mt_maps[0];
+	int			  ptl_mt_maps[0];
 } lnet_portal_t;
 
 #define LNET_LH_HASH_BITS	12
@@ -635,28 +491,22 @@ typedef struct lnet_portal {
 
 /* resource container (ME, MD, EQ) */
 struct lnet_res_container {
-	unsigned int		rec_type;	/* container type */
-	__u64			rec_lh_cookie;	/* cookie generator */
-	struct list_head		rec_active;	/* active resource list */
-	struct list_head		*rec_lh_hash;	/* handle hash */
-#ifdef LNET_USE_LIB_FREELIST
-	lnet_freelist_t		rec_freelist;	/* freelist for resources */
-#endif
+	unsigned int		 rec_type;	/* container type */
+	__u64			 rec_lh_cookie;	/* cookie generator */
+	struct list_head	 rec_active;	/* active resource list */
+	struct list_head	*rec_lh_hash;	/* handle hash */
 };
 
 /* message container */
 struct lnet_msg_container {
-	int			msc_init;	/* initialized or not */
+	int			  msc_init;	/* initialized or not */
 	/* max # threads finalizing */
-	int			msc_nfinalizers;
+	int			  msc_nfinalizers;
 	/* msgs waiting to complete finalizing */
-	struct list_head		msc_finalizing;
-	struct list_head		msc_active;	/* active message list */
+	struct list_head	  msc_finalizing;
+	struct list_head	  msc_active;	/* active message list */
 	/* threads doing finalization */
 	void			**msc_finalizers;
-#ifdef LNET_USE_LIB_FREELIST
-	lnet_freelist_t		msc_freelist;	/* freelist for messages */
-#endif
 };
 
 /* Router Checker states */
@@ -666,15 +516,15 @@ struct lnet_msg_container {
 
 typedef struct {
 	/* CPU partition table of LNet */
-	struct cfs_cpt_table		*ln_cpt_table;
+	struct cfs_cpt_table		 *ln_cpt_table;
 	/* number of CPTs in ln_cpt_table */
-	unsigned int			ln_cpt_number;
-	unsigned int			ln_cpt_bits;
+	unsigned int			  ln_cpt_number;
+	unsigned int			  ln_cpt_bits;
 
 	/* protect LNet resources (ME/MD/EQ) */
-	struct cfs_percpt_lock		*ln_res_lock;
+	struct cfs_percpt_lock		 *ln_res_lock;
 	/* # portals */
-	int				ln_nportals;
+	int				  ln_nportals;
 	/* the vector of portals */
 	lnet_portal_t			**ln_portals;
 	/* percpt ME containers */
@@ -683,77 +533,78 @@ typedef struct {
 	struct lnet_res_container	**ln_md_containers;
 
 	/* Event Queue container */
-	struct lnet_res_container	ln_eq_container;
-	wait_queue_head_t			ln_eq_waitq;
-	spinlock_t			ln_eq_wait_lock;
-	unsigned int			ln_remote_nets_hbits;
+	struct lnet_res_container	  ln_eq_container;
+	wait_queue_head_t		  ln_eq_waitq;
+	spinlock_t			  ln_eq_wait_lock;
+	unsigned int			  ln_remote_nets_hbits;
 
 	/* protect NI, peer table, credits, routers, rtrbuf... */
-	struct cfs_percpt_lock		*ln_net_lock;
+	struct cfs_percpt_lock		 *ln_net_lock;
 	/* percpt message containers for active/finalizing/freed message */
 	struct lnet_msg_container	**ln_msg_containers;
 	lnet_counters_t			**ln_counters;
 	struct lnet_peer_table		**ln_peer_tables;
 	/* failure simulation */
-	struct list_head			ln_test_peers;
+	struct list_head		  ln_test_peers;
 
-	struct list_head			ln_nis;		/* LND instances */
+	struct list_head		  ln_nis;	/* LND instances */
 	/* NIs bond on specific CPT(s) */
-	struct list_head			ln_nis_cpt;
+	struct list_head		  ln_nis_cpt;
 	/* dying LND instances */
-	struct list_head			ln_nis_zombie;
-	lnet_ni_t			*ln_loni;	/* the loopback NI */
+	struct list_head		  ln_nis_zombie;
+	lnet_ni_t			 *ln_loni;	/* the loopback NI */
 	/* NI to wait for events in */
-	lnet_ni_t			*ln_eq_waitni;
+	lnet_ni_t			 *ln_eq_waitni;
 
 	/* remote networks with routes to them */
-	struct list_head			*ln_remote_nets_hash;
+	struct list_head		 *ln_remote_nets_hash;
 	/* validity stamp */
-	__u64				ln_remote_nets_version;
+	__u64				  ln_remote_nets_version;
 	/* list of all known routers */
-	struct list_head			ln_routers;
+	struct list_head		  ln_routers;
 	/* validity stamp */
-	__u64				ln_routers_version;
+	__u64				  ln_routers_version;
 	/* percpt router buffer pools */
 	lnet_rtrbufpool_t		**ln_rtrpools;
 
-	lnet_handle_md_t		ln_ping_target_md;
-	lnet_handle_eq_t		ln_ping_target_eq;
-	lnet_ping_info_t		*ln_ping_info;
+	lnet_handle_md_t		  ln_ping_target_md;
+	lnet_handle_eq_t		  ln_ping_target_eq;
+	lnet_ping_info_t		 *ln_ping_info;
 
 	/* router checker startup/shutdown state */
-	int				ln_rc_state;
+	int				  ln_rc_state;
 	/* router checker's event queue */
-	lnet_handle_eq_t		ln_rc_eqh;
+	lnet_handle_eq_t		  ln_rc_eqh;
 	/* rcd still pending on net */
-	struct list_head			ln_rcd_deathrow;
+	struct list_head		  ln_rcd_deathrow;
 	/* rcd ready for free */
-	struct list_head			ln_rcd_zombie;
+	struct list_head		  ln_rcd_zombie;
 	/* serialise startup/shutdown */
-	struct semaphore		ln_rc_signal;
+	struct semaphore		  ln_rc_signal;
 
-	struct mutex			ln_api_mutex;
-	struct mutex			ln_lnd_mutex;
-	int				ln_init;	/* LNetInit() called? */
+	struct mutex			  ln_api_mutex;
+	struct mutex			  ln_lnd_mutex;
+	int				  ln_init;	/* lnet_init()
+							   called? */
 	/* Have I called LNetNIInit myself? */
-	int				ln_niinit_self;
+	int				  ln_niinit_self;
 	/* LNetNIInit/LNetNIFini counter */
-	int				ln_refcount;
+	int				  ln_refcount;
 	/* shutdown in progress */
-	int				ln_shutdown;
+	int				  ln_shutdown;
 
-	int				ln_routing;	/* am I a router? */
-	lnet_pid_t			ln_pid;		/* requested pid */
+	int				  ln_routing;	/* am I a router? */
+	lnet_pid_t			  ln_pid;	/* requested pid */
 	/* uniquely identifies this ni in this epoch */
-	__u64				ln_interface_cookie;
+	__u64				  ln_interface_cookie;
 	/* registered LNDs */
-	struct list_head			ln_lnds;
+	struct list_head		  ln_lnds;
 
 	/* space for network names */
-	char				*ln_network_tokens;
-	int				ln_network_tokens_nob;
+	char				 *ln_network_tokens;
+	int				  ln_network_tokens_nob;
 	/* test protocol compatibility flags */
-	int				ln_testprotocompat;
+	int				  ln_testprotocompat;
 
 } lnet_t;
 
diff --git a/drivers/staging/lustre/include/linux/lnet/linux/api-support.h b/drivers/staging/lustre/include/linux/lnet/linux/api-support.h
deleted file mode 100644
index e237ad6af422..000000000000
--- a/drivers/staging/lustre/include/linux/lnet/linux/api-support.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LINUX_API_SUPPORT_H__
-#define __LINUX_API_SUPPORT_H__
-
-#ifndef __LNET_API_SUPPORT_H__
-#error Do not #include this file directly. #include <lnet /api-support.h> instead
-#endif
-
-#endif
diff --git a/drivers/staging/lustre/include/linux/lnet/linux/lib-lnet.h b/drivers/staging/lustre/include/linux/lnet/linux/lib-lnet.h
deleted file mode 100644
index 0f8f04d1ecff..000000000000
--- a/drivers/staging/lustre/include/linux/lnet/linux/lib-lnet.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LNET_LINUX_LIB_LNET_H__
-#define __LNET_LINUX_LIB_LNET_H__
-
-#ifndef __LNET_LIB_LNET_H__
-#error Do not #include this file directly. #include <linux/lnet/lib-lnet.h> instead
-#endif
-
-# include <asm/page.h>
-# include <linux/string.h>
-# include <asm/io.h>
-#include "../../libcfs/libcfs.h"
-
-static inline __u64
-lnet_page2phys(struct page *p)
-{
-	/* compiler optimizer will elide unused branches */
-
-	switch (sizeof(typeof(page_to_phys(p)))) {
-	case 4:
-		/* page_to_phys returns a 32 bit physical address.  This must
-		 * be a 32 bit machine with <= 4G memory and we must ensure we
-		 * don't sign extend when converting to 64 bits. */
-		return (unsigned long)page_to_phys(p);
-
-	case 8:
-		/* page_to_phys returns a 64 bit physical address :) */
-		return page_to_phys(p);
-
-	default:
-		LBUG();
-		return 0;
-	}
-}
-
-#define LNET_ROUTER
-
-#endif /* __LNET_LINUX_LIB_LNET_H__ */
diff --git a/drivers/staging/lustre/include/linux/lnet/linux/lib-types.h b/drivers/staging/lustre/include/linux/lnet/linux/lib-types.h
deleted file mode 100644
index 669e8c038534..000000000000
--- a/drivers/staging/lustre/include/linux/lnet/linux/lib-types.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LNET_LINUX_LIB_TYPES_H__
-#define __LNET_LINUX_LIB_TYPES_H__
-
-#ifndef __LNET_LIB_TYPES_H__
-#error Do not #include this file directly. #include <linux/lnet/lib-types.h> instead
-#endif
-
-# include <linux/uio.h>
-# include <linux/types.h>
-
-#endif
diff --git a/drivers/staging/lustre/include/linux/lnet/linux/lnet.h b/drivers/staging/lustre/include/linux/lnet/linux/lnet.h
deleted file mode 100644
index 1e888f1efc45..000000000000
--- a/drivers/staging/lustre/include/linux/lnet/linux/lnet.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LNET_LINUX_LNET_H__
-#define __LNET_LINUX_LNET_H__
-
-#ifndef __LNET_H__
-#error Do not #include this file directly. #include <linux/lnet/lnet.h> instead
-#endif
-
-/*
- * lnet.h
- *
- * User application interface file
- */
-
-#include <linux/uio.h>
-#include <linux/types.h>
-
-#define cfs_tcp_sendpage(sk, page, offset, size, flags) \
-	tcp_sendpage(sk, page, offset, size, flags)
-
-#endif
diff --git a/drivers/staging/lustre/include/linux/lnet/lnet-sysctl.h b/drivers/staging/lustre/include/linux/lnet/lnet-sysctl.h
deleted file mode 100644
index 2dee1b97fb88..000000000000
--- a/drivers/staging/lustre/include/linux/lnet/lnet-sysctl.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LNET_SYSCTL_H__
-#define __LNET_SYSCTL_H__
-
-#if defined(CONFIG_SYSCTL)
-
-#define CTL_KRANAL      201
-#define CTL_O2IBLND     205
-#define CTL_PTLLND      206
-#define CTL_QSWNAL      207
-#define CTL_SOCKLND     208
-#define CTL_GNILND      210
-
-#endif
-
-#endif
diff --git a/drivers/staging/lustre/include/linux/lnet/lnet.h b/drivers/staging/lustre/include/linux/lnet/lnet.h
index 75c0ab9193cc..5d1559a26638 100644
--- a/drivers/staging/lustre/include/linux/lnet/lnet.h
+++ b/drivers/staging/lustre/include/linux/lnet/lnet.h
@@ -15,21 +15,19 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
 /*
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
+ *
+ * Copyright (c) 2012 - 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
+ * Lustre is a trademark of Seagate, Inc.
  */
 
 #ifndef __LNET_H__
@@ -40,12 +38,7 @@
  *
  * User application interface file
  */
-#include "linux/lnet.h"
-
 #include "types.h"
-#include "api.h"
-
-#define LNET_NIDSTR_COUNT  1024    /* # of nidstrings */
-#define LNET_NIDSTR_SIZE   32      /* size of each one (see below for usage) */
+#include "nidstr.h"
 
 #endif
diff --git a/drivers/staging/lustre/include/linux/lnet/lnetctl.h b/drivers/staging/lustre/include/linux/lnet/lnetctl.h
index 98181d389396..bdd69b2af909 100644
--- a/drivers/staging/lustre/include/linux/lnet/lnetctl.h
+++ b/drivers/staging/lustre/include/linux/lnet/lnetctl.h
@@ -14,12 +14,11 @@
  *   along with Portals; if not, write to the Free Software
  *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  *
- * header for libptlctl.a
+ * header for lnet ioctl
  */
-#ifndef _PTLCTL_H_
-#define _PTLCTL_H_
+#ifndef _LNETCTL_H_
+#define _LNETCTL_H_
 
-#include "../libcfs/libcfs.h"
 #include "types.h"
 
 #define LNET_DEV_ID 0
diff --git a/drivers/staging/lustre/include/linux/lnet/lnetst.h b/drivers/staging/lustre/include/linux/lnet/lnetst.h
index 885f708d4031..fd1e0fd3696f 100644
--- a/drivers/staging/lustre/include/linux/lnet/lnetst.h
+++ b/drivers/staging/lustre/include/linux/lnet/lnetst.h
@@ -15,11 +15,7 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
@@ -27,23 +23,21 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, Intel Corporation.
+ * Copyright (c) 2011 - 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
+ * Lustre is a trademark of Seagate, Inc.
  *
  * lnet/include/lnet/lnetst.h
  *
- * Author: Liang Zhen <liangzhen@clusterfs.com>
+ * Author: Liang Zhen <liang.zhen@intel.com>
  */
 
 #ifndef __LNET_ST_H__
 #define __LNET_ST_H__
 
-#include "../libcfs/libcfs.h"
-#include "lnet.h"
-#include "lib-types.h"
+#include <linux/types.h>
 
 #define LST_FEAT_NONE		(0)
 #define LST_FEAT_BULK_LEN	(1 << 0)	/* enable variable page size */
@@ -51,99 +45,107 @@
 #define LST_FEATS_EMPTY		(LST_FEAT_NONE)
 #define LST_FEATS_MASK		(LST_FEAT_NONE | LST_FEAT_BULK_LEN)
 
-#define LST_NAME_SIZE	   32	      /* max name buffer length */
-
-#define LSTIO_DEBUG	     0xC00	   /* debug */
-#define LSTIO_SESSION_NEW       0xC01	   /* create session */
-#define LSTIO_SESSION_END       0xC02	   /* end session */
-#define LSTIO_SESSION_INFO      0xC03	   /* query session */
-#define LSTIO_GROUP_ADD	 0xC10	   /* add group */
-#define LSTIO_GROUP_LIST	0xC11	   /* list all groups in session */
-#define LSTIO_GROUP_INFO	0xC12	   /* query default information of specified group */
-#define LSTIO_GROUP_DEL	 0xC13	   /* delete group */
-#define LSTIO_NODES_ADD	 0xC14	   /* add nodes to specified group */
-#define LSTIO_GROUP_UPDATE      0xC15	   /* update group */
-#define LSTIO_BATCH_ADD	 0xC20	   /* add batch */
-#define LSTIO_BATCH_START       0xC21	   /* start batch */
-#define LSTIO_BATCH_STOP	0xC22	   /* stop batch */
-#define LSTIO_BATCH_DEL	 0xC23	   /* delete batch */
-#define LSTIO_BATCH_LIST	0xC24	   /* show all batches in the session */
-#define LSTIO_BATCH_INFO	0xC25	   /* show defail of specified batch */
-#define LSTIO_TEST_ADD	  0xC26	   /* add test (to batch) */
-#define LSTIO_BATCH_QUERY       0xC27	   /* query batch status */
-#define LSTIO_STAT_QUERY	0xC30	   /* get stats */
-
-typedef struct {
-	lnet_nid_t	      ses_nid;		/* nid of console node */
-	__u64		   ses_stamp;	      /* time stamp */
-} lst_sid_t;					    /*** session id */
+#define LST_NAME_SIZE		32	/* max name buffer length */
+
+#define LSTIO_DEBUG		0xC00	/* debug */
+#define LSTIO_SESSION_NEW	0xC01	/* create session */
+#define LSTIO_SESSION_END	0xC02	/* end session */
+#define LSTIO_SESSION_INFO	0xC03	/* query session */
+#define LSTIO_GROUP_ADD		0xC10	/* add group */
+#define LSTIO_GROUP_LIST	0xC11	/* list all groups in session */
+#define LSTIO_GROUP_INFO	0xC12	/* query default information of
+					 * specified group */
+#define LSTIO_GROUP_DEL		0xC13	/* delete group */
+#define LSTIO_NODES_ADD		0xC14	/* add nodes to specified group */
+#define LSTIO_GROUP_UPDATE      0xC15	/* update group */
+#define LSTIO_BATCH_ADD		0xC20	/* add batch */
+#define LSTIO_BATCH_START	0xC21	/* start batch */
+#define LSTIO_BATCH_STOP	0xC22	/* stop batch */
+#define LSTIO_BATCH_DEL		0xC23	/* delete batch */
+#define LSTIO_BATCH_LIST	0xC24	/* show all batches in the session */
+#define LSTIO_BATCH_INFO	0xC25	/* show defail of specified batch */
+#define LSTIO_TEST_ADD		0xC26	/* add test (to batch) */
+#define LSTIO_BATCH_QUERY	0xC27	/* query batch status */
+#define LSTIO_STAT_QUERY	0xC30	/* get stats */
+
+typedef struct {
+	lnet_nid_t	ses_nid;	/* nid of console node */
+	__u64		ses_stamp;	/* time stamp */
+} lst_sid_t;				/*** session id */
 
 extern lst_sid_t LST_INVALID_SID;
 
 typedef struct {
-	__u64		   bat_id;		 /* unique id in session */
-} lst_bid_t;					    /*** batch id (group of tests) */
+	__u64	bat_id;		/* unique id in session */
+} lst_bid_t;			/*** batch id (group of tests) */
 
 /* Status of test node */
-#define LST_NODE_ACTIVE	 0x1		     /* node in this session */
-#define LST_NODE_BUSY	   0x2		     /* node is taken by other session */
-#define LST_NODE_DOWN	   0x4		     /* node is down */
-#define LST_NODE_UNKNOWN	0x8		     /* node not in session */
+#define LST_NODE_ACTIVE		0x1	/* node in this session */
+#define LST_NODE_BUSY		0x2	/* node is taken by other session */
+#define LST_NODE_DOWN		0x4	/* node is down */
+#define LST_NODE_UNKNOWN	0x8	/* node not in session */
 
 typedef struct {
-	lnet_process_id_t       nde_id;		 /* id of node */
-	int		     nde_state;	      /* state of node */
-} lstcon_node_ent_t;				    /*** node entry, for list_group command */
+	lnet_process_id_t       nde_id;		/* id of node */
+	int			nde_state;	/* state of node */
+} lstcon_node_ent_t;		/*** node entry, for list_group command */
 
 typedef struct {
-	int		     nle_nnode;	      /* # of nodes */
-	int		     nle_nactive;	    /* # of active nodes */
-	int		     nle_nbusy;	      /* # of busy nodes */
-	int		     nle_ndown;	      /* # of down nodes */
-	int		     nle_nunknown;	   /* # of unknown nodes */
-} lstcon_ndlist_ent_t;				  /*** node_list entry, for list_batch command */
+	int	nle_nnode;	/* # of nodes */
+	int	nle_nactive;	/* # of active nodes */
+	int	nle_nbusy;	/* # of busy nodes */
+	int	nle_ndown;	/* # of down nodes */
+	int	nle_nunknown;	/* # of unknown nodes */
+} lstcon_ndlist_ent_t;		/*** node_list entry, for list_batch command */
 
 typedef struct {
-	int		     tse_type;	       /* test type */
-	int		     tse_loop;	       /* loop count */
-	int		     tse_concur;	     /* concurrency of test */
-} lstcon_test_ent_t;				    /*** test summary entry, for list_batch command */
+	int	tse_type;       /* test type */
+	int	tse_loop;       /* loop count */
+	int	tse_concur;     /* concurrency of test */
+} lstcon_test_ent_t;		/*** test summary entry, for
+				 *** list_batch command */
 
 typedef struct {
-	int		     bae_state;	      /* batch status */
-	int		     bae_timeout;	    /* batch timeout */
-	int		     bae_ntest;	      /* # of tests in the batch */
-} lstcon_batch_ent_t;				   /*** batch summary entry, for list_batch command */
+	int	bae_state;	/* batch status */
+	int	bae_timeout;	/* batch timeout */
+	int	bae_ntest;	/* # of tests in the batch */
+} lstcon_batch_ent_t;		/*** batch summary entry, for
+				 *** list_batch command */
 
 typedef struct {
-	lstcon_ndlist_ent_t     tbe_cli_nle;	    /* client (group) node_list entry */
-	lstcon_ndlist_ent_t     tbe_srv_nle;	    /* server (group) node_list entry */
+	lstcon_ndlist_ent_t     tbe_cli_nle;	/* client (group) node_list
+						 * entry */
+	lstcon_ndlist_ent_t     tbe_srv_nle;	/* server (group) node_list
+						 * entry */
 	union {
-		lstcon_test_ent_t  tbe_test;	    /* test entry */
-		lstcon_batch_ent_t tbe_batch;	   /* batch entry */
+		lstcon_test_ent_t  tbe_test;	/* test entry */
+		lstcon_batch_ent_t tbe_batch;	/* batch entry */
 	} u;
-} lstcon_test_batch_ent_t;			      /*** test/batch verbose information entry,
-							 *** for list_batch command */
+} lstcon_test_batch_ent_t;	/*** test/batch verbose information entry,
+				 *** for list_batch command */
 
 typedef struct {
-	struct list_head	      rpe_link;	       /* link chain */
-	lnet_process_id_t       rpe_peer;	       /* peer's id */
-	struct timeval	  rpe_stamp;	      /* time stamp of RPC */
-	int		     rpe_state;	      /* peer's state */
-	int		     rpe_rpc_errno;	  /* RPC errno */
+	struct list_head	rpe_link;	/* link chain */
+	lnet_process_id_t	rpe_peer;	/* peer's id */
+	struct timeval		rpe_stamp;	/* time stamp of RPC */
+	int			rpe_state;	/* peer's state */
+	int			rpe_rpc_errno;	/* RPC errno */
 
-	lst_sid_t	       rpe_sid;		/* peer's session id */
-	int		     rpe_fwk_errno;	  /* framework errno */
-	int		     rpe_priv[4];	    /* private data */
-	char		    rpe_payload[0];	 /* private reply payload */
+	lst_sid_t		rpe_sid;	/* peer's session id */
+	int			rpe_fwk_errno;	/* framework errno */
+	int			rpe_priv[4];	/* private data */
+	char			rpe_payload[0];	/* private reply payload */
 } lstcon_rpc_ent_t;
 
 typedef struct {
-	int		     trs_rpc_stat[4];	/* RPCs stat (0: total, 1: failed, 2: finished, 4: reserved */
-	int		     trs_rpc_errno;	  /* RPC errno */
-	int		     trs_fwk_stat[8];	/* framework stat */
-	int		     trs_fwk_errno;	  /* errno of the first remote error */
-	void		   *trs_fwk_private;	/* private framework stat */
+	int	trs_rpc_stat[4];	/* RPCs stat (0: total
+						      1: failed
+						      2: finished
+						      4: reserved */
+	int	trs_rpc_errno;		/* RPC errno */
+	int	trs_fwk_stat[8];	/* framework stat */
+	int	trs_fwk_errno;		/* errno of the first remote error */
+	void	*trs_fwk_private;	/* private framework stat */
 } lstcon_trans_stat_t;
 
 static inline int
@@ -238,237 +240,265 @@ lstcon_statqry_stat_failure(lstcon_trans_stat_t *stat, int inc)
 
 /* create a session */
 typedef struct {
-	int		     lstio_ses_key;	  /* IN: local key */
-	int		     lstio_ses_timeout;      /* IN: session timeout */
-	int		     lstio_ses_force;	/* IN: force create ? */
+	int		 lstio_ses_key;		/* IN: local key */
+	int		 lstio_ses_timeout;	/* IN: session timeout */
+	int		 lstio_ses_force;	/* IN: force create ? */
 	/** IN: session features */
-	unsigned		lstio_ses_feats;
-	lst_sid_t	      *lstio_ses_idp;	  /* OUT: session id */
-	int		     lstio_ses_nmlen;	/* IN: name length */
-	char		   *lstio_ses_namep;	/* IN: session name */
+	unsigned	 lstio_ses_feats;
+	lst_sid_t	*lstio_ses_idp;		/* OUT: session id */
+	int		 lstio_ses_nmlen;	/* IN: name length */
+	char		*lstio_ses_namep;	/* IN: session name */
 } lstio_session_new_args_t;
 
 /* query current session */
 typedef struct {
-	lst_sid_t	      *lstio_ses_idp;	  /* OUT: session id */
-	int		    *lstio_ses_keyp;	 /* OUT: local key */
+	lst_sid_t		*lstio_ses_idp;		/* OUT: session id */
+	int			*lstio_ses_keyp;	/* OUT: local key */
 	/** OUT: session features */
-	unsigned	       *lstio_ses_featp;
-	lstcon_ndlist_ent_t    *lstio_ses_ndinfo;       /* OUT: */
-	int		     lstio_ses_nmlen;	/* IN: name length */
-	char		   *lstio_ses_namep;	/* OUT: session name */
+	unsigned		*lstio_ses_featp;
+	lstcon_ndlist_ent_t	*lstio_ses_ndinfo;	/* OUT: */
+	int			 lstio_ses_nmlen;	/* IN: name length */
+	char			*lstio_ses_namep;	/* OUT: session name */
 } lstio_session_info_args_t;
 
 /* delete a session */
 typedef struct {
-	int		     lstio_ses_key;	  /* IN: session key */
+	int			lstio_ses_key;	/* IN: session key */
 } lstio_session_end_args_t;
 
-#define LST_OPC_SESSION	 1
-#define LST_OPC_GROUP	   2
-#define LST_OPC_NODES	   3
+#define LST_OPC_SESSION		1
+#define LST_OPC_GROUP		2
+#define LST_OPC_NODES		3
 #define LST_OPC_BATCHCLI	4
 #define LST_OPC_BATCHSRV	5
 
 typedef struct {
-	int		     lstio_dbg_key;	  /* IN: session key */
-	int		     lstio_dbg_type;	 /* IN: debug sessin|batch|group|nodes list */
-	int		     lstio_dbg_flags;	/* IN: reserved debug flags */
-	int		     lstio_dbg_timeout;      /* IN: timeout of debug */
-
-	int		     lstio_dbg_nmlen;	/* IN: len of name */
-	char		   *lstio_dbg_namep;	/* IN: name of group|batch */
-	int		     lstio_dbg_count;	/* IN: # of test nodes to debug */
-	lnet_process_id_t      *lstio_dbg_idsp;	 /* IN: id of test nodes */
-	struct list_head	     *lstio_dbg_resultp;      /* OUT: list head of result buffer */
+	int			 lstio_dbg_key;		/* IN: session key */
+	int			 lstio_dbg_type;	/* IN: debug
+								session|batch|
+								group|nodes
+								list */
+	int			 lstio_dbg_flags;	/* IN: reserved debug
+							       flags */
+	int			 lstio_dbg_timeout;	/* IN: timeout of
+							       debug */
+	int			 lstio_dbg_nmlen;	/* IN: len of name */
+	char			*lstio_dbg_namep;	/* IN: name of
+							       group|batch */
+	int			 lstio_dbg_count;	/* IN: # of test nodes
+							       to debug */
+	lnet_process_id_t	*lstio_dbg_idsp;	/* IN: id of test
+							       nodes */
+	struct list_head	*lstio_dbg_resultp;	/* OUT: list head of
+								result buffer */
 } lstio_debug_args_t;
 
 typedef struct {
-	int		     lstio_grp_key;	  /* IN: session key */
-	int		     lstio_grp_nmlen;	/* IN: name length */
-	char		   *lstio_grp_namep;	/* IN: group name */
+	int	 lstio_grp_key;		/* IN: session key */
+	int	 lstio_grp_nmlen;	/* IN: name length */
+	char	*lstio_grp_namep;	/* IN: group name */
 } lstio_group_add_args_t;
 
 typedef struct {
-	int		     lstio_grp_key;	  /* IN: session key */
-	int		     lstio_grp_nmlen;	/* IN: name length */
-	char		   *lstio_grp_namep;	/* IN: group name */
+	int	 lstio_grp_key;		/* IN: session key */
+	int	 lstio_grp_nmlen;	/* IN: name length */
+	char	*lstio_grp_namep;	/* IN: group name */
 } lstio_group_del_args_t;
 
-#define LST_GROUP_CLEAN	 1		       /* remove inactive nodes in the group */
-#define LST_GROUP_REFRESH       2		       /* refresh inactive nodes in the group */
-#define LST_GROUP_RMND	  3		       /* delete nodes from the group */
-
-typedef struct {
-	int		     lstio_grp_key;	  /* IN: session key */
-	int		     lstio_grp_opc;	  /* IN: OPC */
-	int		     lstio_grp_args;	 /* IN: arguments */
-	int		     lstio_grp_nmlen;	/* IN: name length */
-	char		   *lstio_grp_namep;	/* IN: group name */
-	int		     lstio_grp_count;	/* IN: # of nodes id */
-	lnet_process_id_t      *lstio_grp_idsp;	 /* IN: array of nodes */
-	struct list_head	     *lstio_grp_resultp;      /* OUT: list head of result buffer */
+#define LST_GROUP_CLEAN		1	/* remove inactive nodes in the group */
+#define LST_GROUP_REFRESH	2	/* refresh inactive nodes
+					 * in the group */
+#define LST_GROUP_RMND		3	/* delete nodes from the group */
+
+typedef struct {
+	int			 lstio_grp_key;		/* IN: session key */
+	int			 lstio_grp_opc;		/* IN: OPC */
+	int			 lstio_grp_args;	/* IN: arguments */
+	int			 lstio_grp_nmlen;	/* IN: name length */
+	char			*lstio_grp_namep;	/* IN: group name */
+	int			 lstio_grp_count;	/* IN: # of nodes id */
+	lnet_process_id_t	*lstio_grp_idsp;	/* IN: array of nodes */
+	struct list_head	*lstio_grp_resultp;	/* OUT: list head of
+								result buffer */
 } lstio_group_update_args_t;
 
 typedef struct {
-	int		     lstio_grp_key;	  /* IN: session key */
-	int		     lstio_grp_nmlen;	/* IN: name length */
-	char		   *lstio_grp_namep;	/* IN: group name */
-	int		     lstio_grp_count;	/* IN: # of nodes */
+	int			 lstio_grp_key;		/* IN: session key */
+	int			 lstio_grp_nmlen;	/* IN: name length */
+	char			*lstio_grp_namep;	/* IN: group name */
+	int			 lstio_grp_count;	/* IN: # of nodes */
 	/** OUT: session features */
-	unsigned	       *lstio_grp_featp;
-	lnet_process_id_t      *lstio_grp_idsp;	 /* IN: nodes */
-	struct list_head	     *lstio_grp_resultp;      /* OUT: list head of result buffer */
+	unsigned		*lstio_grp_featp;
+	lnet_process_id_t	*lstio_grp_idsp;	/* IN: nodes */
+	struct list_head	*lstio_grp_resultp;	/* OUT: list head of
+								result buffer */
 } lstio_group_nodes_args_t;
 
 typedef struct {
-	int		     lstio_grp_key;	  /* IN: session key */
-	int		     lstio_grp_idx;	  /* IN: group idx */
-	int		     lstio_grp_nmlen;	/* IN: name len */
-	char		   *lstio_grp_namep;	/* OUT: name */
+	int	 lstio_grp_key;		/* IN: session key */
+	int	 lstio_grp_idx;		/* IN: group idx */
+	int	 lstio_grp_nmlen;	/* IN: name len */
+	char	*lstio_grp_namep;	/* OUT: name */
 } lstio_group_list_args_t;
 
 typedef struct {
-	int		     lstio_grp_key;	  /* IN: session key */
-	int		     lstio_grp_nmlen;	/* IN: name len */
-	char		   *lstio_grp_namep;	/* IN: name */
-	lstcon_ndlist_ent_t    *lstio_grp_entp;	 /* OUT: description of group */
-
-	int		    *lstio_grp_idxp;	 /* IN/OUT: node index */
-	int		    *lstio_grp_ndentp;       /* IN/OUT: # of nodent */
-	lstcon_node_ent_t      *lstio_grp_dentsp;       /* OUT: nodent array */
+	int			 lstio_grp_key;		/* IN: session key */
+	int			 lstio_grp_nmlen;	/* IN: name len */
+	char			*lstio_grp_namep;	/* IN: name */
+	lstcon_ndlist_ent_t	*lstio_grp_entp;	/* OUT: description of
+								group */
+	int			*lstio_grp_idxp;	/* IN/OUT: node index */
+	int			*lstio_grp_ndentp;	/* IN/OUT: # of nodent */
+	lstcon_node_ent_t	*lstio_grp_dentsp;	/* OUT: nodent array */
 } lstio_group_info_args_t;
 
-#define LST_DEFAULT_BATCH       "batch"		 /* default batch name */
+#define LST_DEFAULT_BATCH	"batch"			/* default batch name */
 
 typedef struct {
-	int		     lstio_bat_key;	  /* IN: session key */
-	int		     lstio_bat_nmlen;	/* IN: name length */
-	char		   *lstio_bat_namep;	/* IN: batch name */
+	int	 lstio_bat_key;		/* IN: session key */
+	int	 lstio_bat_nmlen;	/* IN: name length */
+	char	*lstio_bat_namep;	/* IN: batch name */
 } lstio_batch_add_args_t;
 
 typedef struct {
-	int		     lstio_bat_key;	  /* IN: session key */
-	int		     lstio_bat_nmlen;	/* IN: name length */
-	char		   *lstio_bat_namep;	/* IN: batch name */
+	int	 lstio_bat_key;		/* IN: session key */
+	int	 lstio_bat_nmlen;	/* IN: name length */
+	char	*lstio_bat_namep;	/* IN: batch name */
 } lstio_batch_del_args_t;
 
 typedef struct {
-	int		     lstio_bat_key;	  /* IN: session key */
-	int		     lstio_bat_timeout;      /* IN: timeout for the batch */
-	int		     lstio_bat_nmlen;	/* IN: name length */
-	char		   *lstio_bat_namep;	/* IN: batch name */
-	struct list_head	     *lstio_bat_resultp;      /* OUT: list head of result buffer */
+	int			 lstio_bat_key;		/* IN: session key */
+	int			 lstio_bat_timeout;	/* IN: timeout for
+							       the batch */
+	int			 lstio_bat_nmlen;	/* IN: name length */
+	char			*lstio_bat_namep;	/* IN: batch name */
+	struct list_head	*lstio_bat_resultp;	/* OUT: list head of
+								result buffer */
 } lstio_batch_run_args_t;
 
 typedef struct {
-	int		     lstio_bat_key;	  /* IN: session key */
-	int		     lstio_bat_force;	/* IN: abort unfinished test RPC */
-	int		     lstio_bat_nmlen;	/* IN: name length */
-	char		   *lstio_bat_namep;	/* IN: batch name */
-	struct list_head	     *lstio_bat_resultp;      /* OUT: list head of result buffer */
+	int			 lstio_bat_key;		/* IN: session key */
+	int			 lstio_bat_force;	/* IN: abort unfinished
+							       test RPC */
+	int			 lstio_bat_nmlen;	/* IN: name length */
+	char			*lstio_bat_namep;	/* IN: batch name */
+	struct list_head	*lstio_bat_resultp;	/* OUT: list head of
+								result buffer */
 } lstio_batch_stop_args_t;
 
 typedef struct {
-	int		     lstio_bat_key;	  /* IN: session key */
-	int		     lstio_bat_testidx;      /* IN: test index */
-	int		     lstio_bat_client;       /* IN: is test client? */
-	int		     lstio_bat_timeout;      /* IN: timeout for waiting */
-	int		     lstio_bat_nmlen;	/* IN: name length */
-	char		   *lstio_bat_namep;	/* IN: batch name */
-	struct list_head	     *lstio_bat_resultp;      /* OUT: list head of result buffer */
+	int			 lstio_bat_key;		/* IN: session key */
+	int			 lstio_bat_testidx;	/* IN: test index */
+	int			 lstio_bat_client;	/* IN: we testing
+							       client? */
+	int			 lstio_bat_timeout;	/* IN: timeout for
+							       waiting */
+	int			 lstio_bat_nmlen;	/* IN: name length */
+	char			*lstio_bat_namep;	/* IN: batch name */
+	struct list_head	*lstio_bat_resultp;	/* OUT: list head of
+								result buffer */
 } lstio_batch_query_args_t;
 
 typedef struct {
-	int		     lstio_bat_key;	  /* IN: session key */
-	int		     lstio_bat_idx;	  /* IN: index */
-	int		     lstio_bat_nmlen;	/* IN: name length */
-	char		   *lstio_bat_namep;	/* IN: batch name */
+	int	 lstio_bat_key;		/* IN: session key */
+	int	 lstio_bat_idx;		/* IN: index */
+	int	 lstio_bat_nmlen;	/* IN: name length */
+	char	*lstio_bat_namep;	/* IN: batch name */
 } lstio_batch_list_args_t;
 
 typedef struct {
-	int		     lstio_bat_key;	  /* IN: session key */
-	int		     lstio_bat_nmlen;	/* IN: name length */
-	char		   *lstio_bat_namep;	/* IN: name */
-	int		     lstio_bat_server;       /* IN: query server or not */
-	int		     lstio_bat_testidx;      /* IN: test index */
-	lstcon_test_batch_ent_t *lstio_bat_entp;	/* OUT: batch ent */
+	int			 lstio_bat_key;		/* IN: session key */
+	int			 lstio_bat_nmlen;	/* IN: name length */
+	char			*lstio_bat_namep;	/* IN: name */
+	int			 lstio_bat_server;	/* IN: query server
+							       or not */
+	int			 lstio_bat_testidx;	/* IN: test index */
+	lstcon_test_batch_ent_t	*lstio_bat_entp;	/* OUT: batch ent */
 
-	int		    *lstio_bat_idxp;	 /* IN/OUT: index of node */
-	int		    *lstio_bat_ndentp;       /* IN/OUT: # of nodent */
-	lstcon_node_ent_t      *lstio_bat_dentsp;       /* array of nodent */
+	int			*lstio_bat_idxp;	/* IN/OUT: index of node */
+	int			*lstio_bat_ndentp;	/* IN/OUT: # of nodent */
+	lstcon_node_ent_t	*lstio_bat_dentsp;	/* array of nodent */
 } lstio_batch_info_args_t;
 
 /* add stat in session */
 typedef struct {
-	int		     lstio_sta_key;	  /* IN: session key */
-	int		     lstio_sta_timeout;      /* IN: timeout for stat request */
-	int		     lstio_sta_nmlen;	/* IN: group name length */
-	char		   *lstio_sta_namep;	/* IN: group name */
-	int		     lstio_sta_count;	/* IN: # of pid */
-	lnet_process_id_t      *lstio_sta_idsp;	 /* IN: pid */
-	struct list_head	     *lstio_sta_resultp;      /* OUT: list head of result buffer */
+	int			 lstio_sta_key;		/* IN: session key */
+	int			 lstio_sta_timeout;	/* IN: timeout for
+							       stat request */
+	int			 lstio_sta_nmlen;	/* IN: group name
+							       length */
+	char			*lstio_sta_namep;	/* IN: group name */
+	int			 lstio_sta_count;	/* IN: # of pid */
+	lnet_process_id_t	*lstio_sta_idsp;	/* IN: pid */
+	struct list_head	*lstio_sta_resultp;	/* OUT: list head of
+								result buffer */
 } lstio_stat_args_t;
 
 typedef enum {
-	LST_TEST_BULK   = 1,
-	LST_TEST_PING   = 2
+	LST_TEST_BULK	= 1,
+	LST_TEST_PING	= 2
 } lst_test_type_t;
 
 /* create a test in a batch */
-#define LST_MAX_CONCUR	  1024		    /* Max concurrency of test */
-
-typedef struct {
-	int		     lstio_tes_key;	  /* IN: session key */
-	int		     lstio_tes_bat_nmlen;    /* IN: batch name len */
-	char		   *lstio_tes_bat_name;     /* IN: batch name */
-	int		     lstio_tes_type;	 /* IN: test type */
-	int		     lstio_tes_oneside;      /* IN: one sided test */
-	int		     lstio_tes_loop;	 /* IN: loop count */
-	int		     lstio_tes_concur;       /* IN: concurrency */
-
-	int		     lstio_tes_dist;	 /* IN: node distribution in destination groups */
-	int		     lstio_tes_span;	 /* IN: node span in destination groups */
-	int		     lstio_tes_sgrp_nmlen;   /* IN: source group name length */
-	char		   *lstio_tes_sgrp_name;    /* IN: group name */
-	int		     lstio_tes_dgrp_nmlen;   /* IN: destination group name length */
-	char		   *lstio_tes_dgrp_name;    /* IN: group name */
-
-	int		     lstio_tes_param_len;    /* IN: param buffer len */
-	void		   *lstio_tes_param;	/* IN: parameter for specified test:
-							       lstio_bulk_param_t,
-							       lstio_ping_param_t,
-							       ... more */
-	int		    *lstio_tes_retp;	 /* OUT: private returned value */
-	struct list_head	     *lstio_tes_resultp;      /* OUT: list head of result buffer */
+#define LST_MAX_CONCUR	1024	/* Max concurrency of test */
+
+typedef struct {
+	int		  lstio_tes_key;	/* IN: session key */
+	int		  lstio_tes_bat_nmlen;	/* IN: batch name len */
+	char		 *lstio_tes_bat_name;	/* IN: batch name */
+	int		  lstio_tes_type;	/* IN: test type */
+	int		  lstio_tes_oneside;	/* IN: one sided test */
+	int		  lstio_tes_loop;	/* IN: loop count */
+	int		  lstio_tes_concur;	/* IN: concurrency */
+
+	int		  lstio_tes_dist;	/* IN: node distribution in
+						       destination groups */
+	int		  lstio_tes_span;	/* IN: node span in
+						       destination groups */
+	int		  lstio_tes_sgrp_nmlen;	/* IN: source group
+						       name length */
+	char		 *lstio_tes_sgrp_name;	/* IN: group name */
+	int		  lstio_tes_dgrp_nmlen;	/* IN: destination group
+						       name length */
+	char		 *lstio_tes_dgrp_name;	/* IN: group name */
+
+	int		  lstio_tes_param_len;	/* IN: param buffer len */
+	void		 *lstio_tes_param;	/* IN: parameter for specified
+						       test:
+						       lstio_bulk_param_t,
+						       lstio_ping_param_t,
+						       ... more */
+	int		 *lstio_tes_retp;	/* OUT: private returned
+							value */
+	struct list_head *lstio_tes_resultp;	/* OUT: list head of
+							result buffer */
 } lstio_test_args_t;
 
 typedef enum {
-	LST_BRW_READ    = 1,
-	LST_BRW_WRITE   = 2
+	LST_BRW_READ	= 1,
+	LST_BRW_WRITE	= 2
 } lst_brw_type_t;
 
 typedef enum {
-	LST_BRW_CHECK_NONE   = 1,
-	LST_BRW_CHECK_SIMPLE = 2,
-	LST_BRW_CHECK_FULL   = 3
+	LST_BRW_CHECK_NONE	= 1,
+	LST_BRW_CHECK_SIMPLE	= 2,
+	LST_BRW_CHECK_FULL	= 3
 } lst_brw_flags_t;
 
 typedef struct {
-	int		     blk_opc;		/* bulk operation code */
-	int		     blk_size;	       /* size (bytes) */
-	int		     blk_time;	       /* time of running the test*/
-	int		     blk_flags;	      /* reserved flags */
+	int	blk_opc;	/* bulk operation code */
+	int	blk_size;       /* size (bytes) */
+	int	blk_time;       /* time of running the test*/
+	int	blk_flags;      /* reserved flags */
 } lst_test_bulk_param_t;
 
 typedef struct {
-	int		     png_size;	       /* size of ping message */
-	int		     png_time;	       /* time */
-	int		     png_loop;	       /* loop */
-	int		     png_flags;	      /* reserved flags */
+	int	png_size;	/* size of ping message */
+	int	png_time;	/* time */
+	int	png_loop;	/* loop */
+	int	png_flags;	/* reserved flags */
 } lst_test_ping_param_t;
 
-/* more tests */
 typedef struct {
 	__u32 errors;
 	__u32 rpcs_sent;
diff --git a/drivers/staging/lustre/include/linux/lnet/nidstr.h b/drivers/staging/lustre/include/linux/lnet/nidstr.h
new file mode 100644
index 000000000000..a627be9fcdd0
--- /dev/null
+++ b/drivers/staging/lustre/include/linux/lnet/nidstr.h
@@ -0,0 +1,77 @@
+/*
+ * GPL HEADER START
+ *
+ * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 only,
+ * 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 version 2 for more details (a copy is included
+ * in the LICENSE file that accompanied this code).
+ *
+ * You should have received a copy of the GNU General Public License
+ * version 2 along with this program; If not, see
+ * http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * GPL HEADER END
+ */
+/*
+ * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
+ * Use is subject to license terms.
+ *
+ * Copyright (c) 2011 - 2015, Intel Corporation.
+ */
+#ifndef _LNET_NIDSTRINGS_H
+#define _LNET_NIDSTRINGS_H
+
+#include "types.h"
+
+/**
+ *  Lustre Network Driver types.
+ */
+enum {
+	/* Only add to these values (i.e. don't ever change or redefine them):
+	 * network addresses depend on them... */
+	QSWLND		= 1,
+	SOCKLND		= 2,
+	GMLND		= 3,
+	PTLLND		= 4,
+	O2IBLND		= 5,
+	CIBLND		= 6,
+	OPENIBLND	= 7,
+	IIBLND		= 8,
+	LOLND		= 9,
+	RALND		= 10,
+	VIBLND		= 11,
+	MXLND		= 12,
+	GNILND		= 13,
+	GNIIPLND	= 14,
+};
+
+struct list_head;
+
+#define LNET_NIDSTR_COUNT  1024    /* # of nidstrings */
+#define LNET_NIDSTR_SIZE   32      /* size of each one (see below for usage) */
+
+int libcfs_isknown_lnd(int type);
+char *libcfs_lnd2modname(int type);
+char *libcfs_lnd2str(int type);
+int libcfs_str2lnd(const char *str);
+char *libcfs_net2str(__u32 net);
+char *libcfs_nid2str(lnet_nid_t nid);
+__u32 libcfs_str2net(const char *str);
+lnet_nid_t libcfs_str2nid(const char *str);
+int libcfs_str2anynid(lnet_nid_t *nid, const char *str);
+char *libcfs_id2str(lnet_process_id_t id);
+void cfs_free_nidlist(struct list_head *list);
+int cfs_parse_nidlist(char *str, int len, struct list_head *list);
+int cfs_match_nid(lnet_nid_t nid, struct list_head *list);
+bool cfs_nidrange_is_contiguous(struct list_head *nidlist);
+void cfs_nidrange_find_min_max(struct list_head *nidlist, char *min_nid,
+			       char *max_nid, size_t nidstr_length);
+
+#endif /* _LNET_NIDSTRINGS_H */
diff --git a/drivers/staging/lustre/include/linux/lnet/ptllnd.h b/drivers/staging/lustre/include/linux/lnet/ptllnd.h
deleted file mode 100644
index c91d65329995..000000000000
--- a/drivers/staging/lustre/include/linux/lnet/ptllnd.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lnet/include/lnet/ptllnd.h
- *
- * Author: PJ Kirner <pjkirner@clusterfs.com>
- */
-
-/*
- * The PTLLND was designed to support Portals with
- * Lustre and non-lustre UNLINK semantics.
- * However for now the two targets are Cray Portals
- * on the XT3 and Lustre Portals (for testing) both
- * have Lustre UNLINK semantics, so this is defined
- * by default.
- */
-#define LUSTRE_PORTALS_UNLINK_SEMANTICS
-
-#ifdef _USING_LUSTRE_PORTALS_
-
-/* NIDs are 64-bits on Lustre Portals */
-#define FMT_NID "%llu"
-#define FMT_PID "%d"
-
-/* When using Lustre Portals Lustre completion semantics are imlicit*/
-#define PTL_MD_LUSTRE_COMPLETION_SEMANTICS      0
-
-#else /* _USING_CRAY_PORTALS_ */
-
-/* NIDs are integers on Cray Portals */
-#define FMT_NID "%u"
-#define FMT_PID "%d"
-
-/* When using Cray Portals this is defined in the Cray Portals Header*/
-/*#define PTL_MD_LUSTRE_COMPLETION_SEMANTICS */
-
-/* Can compare handles directly on Cray Portals */
-#define PtlHandleIsEqual(a, b) ((a) == (b))
-
-/* Different error types on Cray Portals*/
-#define ptl_err_t ptl_ni_fail_t
-
-/*
- * The Cray Portals has no maximum number of IOVs.  The
- * maximum is limited only by memory and size of the
- * int parameters (2^31-1).
- * Lustre only really require that the underyling
- * implementation to support at least LNET_MAX_IOV,
- * so for Cray portals we can safely just use that
- * value here.
- *
- */
-#define PTL_MD_MAX_IOV	  LNET_MAX_IOV
-
-#endif
-
-#define FMT_PTLID "ptlid:"FMT_PID"-"FMT_NID
-
-/* Align incoming small request messages to an 8 byte boundary if this is
- * supported to avoid alignment issues on some architectures */
-#ifndef PTL_MD_LOCAL_ALIGN8
-# define PTL_MD_LOCAL_ALIGN8 0
-#endif
diff --git a/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h b/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h
deleted file mode 100644
index 808f37b64a4f..000000000000
--- a/drivers/staging/lustre/include/linux/lnet/ptllnd_wire.h
+++ /dev/null
@@ -1,119 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lnet/include/lnet/ptllnd_wire.h
- *
- * Author: PJ Kirner <pjkirner@clusterfs.com>
- */
-
-/* Minimum buffer size that any peer will post to receive ptllnd messages */
-#define PTLLND_MIN_BUFFER_SIZE  256
-
-/************************************************************************
- * Tunable defaults that {u,k}lnds/ptllnd should have in common.
- */
-
-#define PTLLND_PORTAL	   9	  /* The same portal PTLPRC used when talking to cray portals */
-#define PTLLND_PID	      9	  /* The Portals PID */
-#define PTLLND_PEERCREDITS      8	  /* concurrent sends to 1 peer */
-
-/* Default buffer size for kernel ptllnds (guaranteed eager) */
-#define PTLLND_MAX_KLND_MSG_SIZE 512
-
-/* Default buffer size for catamount ptllnds (not guaranteed eager) - large
- * enough to avoid RDMA for anything sent while control is not in liblustre */
-#define PTLLND_MAX_ULND_MSG_SIZE 512
-
-/************************************************************************
- * Portals LND Wire message format.
- * These are sent in sender's byte order (i.e. receiver flips).
- */
-
-#define PTL_RESERVED_MATCHBITS  0x100	/* below this value is reserved
-					 * above is for bulk data transfer */
-#define LNET_MSG_MATCHBITS       0      /* the value for the message channel */
-
-typedef struct {
-	lnet_hdr_t	kptlim_hdr;	     /* portals header */
-	char	      kptlim_payload[0];      /* piggy-backed payload */
-} WIRE_ATTR kptl_immediate_msg_t;
-
-typedef struct {
-	lnet_hdr_t	kptlrm_hdr;	     /* portals header */
-	__u64	     kptlrm_matchbits;       /* matchbits */
-} WIRE_ATTR kptl_rdma_msg_t;
-
-typedef struct {
-	__u64	     kptlhm_matchbits;       /* matchbits */
-	__u32	     kptlhm_max_msg_size;    /* max message size */
-} WIRE_ATTR kptl_hello_msg_t;
-
-typedef struct {
-	/* First 2 fields fixed FOR ALL TIME */
-	__u32	   ptlm_magic;     /* I'm a Portals LND message */
-	__u16	   ptlm_version;   /* this is my version number */
-	__u8	    ptlm_type;      /* the message type */
-	__u8	    ptlm_credits;   /* returned credits */
-	__u32	   ptlm_nob;       /* # bytes in whole message */
-	__u32	   ptlm_cksum;     /* checksum (0 == no checksum) */
-	__u64	   ptlm_srcnid;    /* sender's NID */
-	__u64	   ptlm_srcstamp;  /* sender's incarnation */
-	__u64	   ptlm_dstnid;    /* destination's NID */
-	__u64	   ptlm_dststamp;  /* destination's incarnation */
-	__u32	   ptlm_srcpid;    /* sender's PID */
-	__u32	   ptlm_dstpid;    /* destination's PID */
-
-	 union {
-		kptl_immediate_msg_t    immediate;
-		kptl_rdma_msg_t	 rdma;
-		kptl_hello_msg_t	hello;
-	} WIRE_ATTR ptlm_u;
-
-} kptl_msg_t;
-
-/* kptl_msg_t::ptlm_credits is only a __u8 */
-#define PTLLND_MSG_MAX_CREDITS ((typeof(((kptl_msg_t *)0)->ptlm_credits)) - 1)
-
-#define PTLLND_MSG_MAGIC		LNET_PROTO_PTL_MAGIC
-#define PTLLND_MSG_VERSION	      0x04
-
-#define PTLLND_RDMA_OK		  0x00
-#define PTLLND_RDMA_FAIL		0x01
-
-#define PTLLND_MSG_TYPE_INVALID	 0x00
-#define PTLLND_MSG_TYPE_PUT	     0x01
-#define PTLLND_MSG_TYPE_GET	     0x02
-#define PTLLND_MSG_TYPE_IMMEDIATE       0x03    /* No bulk data xfer*/
-#define PTLLND_MSG_TYPE_NOOP	    0x04
-#define PTLLND_MSG_TYPE_HELLO	   0x05
-#define PTLLND_MSG_TYPE_NAK	     0x06
diff --git a/drivers/staging/lustre/include/linux/lnet/socklnd.h b/drivers/staging/lustre/include/linux/lnet/socklnd.h
index 389038b122c5..599c9f6628fb 100644
--- a/drivers/staging/lustre/include/linux/lnet/socklnd.h
+++ b/drivers/staging/lustre/include/linux/lnet/socklnd.h
@@ -15,57 +15,52 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
 /*
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
+ *
+ * Copyright (c) 2012 - 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
+ * Lustre is a trademark of Seagate, Inc.
  *
  * lnet/include/lnet/socklnd.h
- *
- * #defines shared between socknal implementation and utilities
  */
 #ifndef __LNET_LNET_SOCKLND_H__
 #define __LNET_LNET_SOCKLND_H__
 
 #include "types.h"
-#include "lib-types.h"
 
 #define SOCKLND_CONN_NONE     (-1)
 #define SOCKLND_CONN_ANY	0
-#define SOCKLND_CONN_CONTROL    1
-#define SOCKLND_CONN_BULK_IN    2
-#define SOCKLND_CONN_BULK_OUT   3
-#define SOCKLND_CONN_NTYPES     4
+#define SOCKLND_CONN_CONTROL	1
+#define SOCKLND_CONN_BULK_IN	2
+#define SOCKLND_CONN_BULK_OUT	3
+#define SOCKLND_CONN_NTYPES	4
 
 #define SOCKLND_CONN_ACK	SOCKLND_CONN_BULK_IN
 
 typedef struct {
-	__u32		   kshm_magic;     /* magic number of socklnd message */
-	__u32		   kshm_version;   /* version of socklnd message */
-	lnet_nid_t	      kshm_src_nid;   /* sender's nid */
-	lnet_nid_t	      kshm_dst_nid;   /* destination nid */
-	lnet_pid_t	      kshm_src_pid;   /* sender's pid */
-	lnet_pid_t	      kshm_dst_pid;   /* destination pid */
-	__u64		   kshm_src_incarnation; /* sender's incarnation */
-	__u64		   kshm_dst_incarnation; /* destination's incarnation */
-	__u32		   kshm_ctype;     /* connection type */
-	__u32		   kshm_nips;      /* # IP addrs */
-	__u32		   kshm_ips[0];    /* IP addrs */
+	__u32		kshm_magic;	/* magic number of socklnd message */
+	__u32		kshm_version;	/* version of socklnd message */
+	lnet_nid_t      kshm_src_nid;	/* sender's nid */
+	lnet_nid_t	kshm_dst_nid;	/* destination nid */
+	lnet_pid_t	kshm_src_pid;	/* sender's pid */
+	lnet_pid_t	kshm_dst_pid;	/* destination pid */
+	__u64		kshm_src_incarnation; /* sender's incarnation */
+	__u64		kshm_dst_incarnation; /* destination's incarnation */
+	__u32		kshm_ctype;	/* connection type */
+	__u32		kshm_nips;	/* # IP addrs */
+	__u32		kshm_ips[0];	/* IP addrs */
 } WIRE_ATTR ksock_hello_msg_t;
 
 typedef struct {
-	lnet_hdr_t	      ksnm_hdr;       /* lnet hdr */
+	lnet_hdr_t	ksnm_hdr;	/* lnet hdr */
 
 	/*
 	 * ksnm_payload is removed because of winnt compiler's limitation:
@@ -76,28 +71,29 @@ typedef struct {
 } WIRE_ATTR ksock_lnet_msg_t;
 
 typedef struct {
-	__u32		   ksm_type;       /* type of socklnd message */
-	__u32		   ksm_csum;       /* checksum if != 0 */
-	__u64		   ksm_zc_cookies[2]; /* Zero-Copy request/ACK cookie */
+	__u32	ksm_type;		/* type of socklnd message */
+	__u32	ksm_csum;		/* checksum if != 0 */
+	__u64	ksm_zc_cookies[2];	/* Zero-Copy request/ACK cookie */
 	union {
-		ksock_lnet_msg_t lnetmsg;       /* lnet message, it's empty if it's NOOP */
+		ksock_lnet_msg_t lnetmsg;/* lnet message, it's empty if
+					  * it's NOOP */
 	} WIRE_ATTR ksm_u;
 } WIRE_ATTR ksock_msg_t;
 
 static inline void
 socklnd_init_msg(ksock_msg_t *msg, int type)
 {
-	msg->ksm_csum	   = 0;
-	msg->ksm_type	   = type;
-	msg->ksm_zc_cookies[0]  = msg->ksm_zc_cookies[1]  = 0;
+	msg->ksm_csum = 0;
+	msg->ksm_type = type;
+	msg->ksm_zc_cookies[0] = msg->ksm_zc_cookies[1]  = 0;
 }
 
-#define KSOCK_MSG_NOOP	  0xc0	    /* ksm_u empty */
-#define KSOCK_MSG_LNET	  0xc1	    /* lnet msg */
+#define KSOCK_MSG_NOOP	0xC0	/* ksm_u empty */
+#define KSOCK_MSG_LNET	0xC1	/* lnet msg */
 
 /* We need to know this number to parse hello msg from ksocklnd in
  * other LND (usocklnd, for example) */
-#define KSOCK_PROTO_V2	  2
-#define KSOCK_PROTO_V3	  3
+#define KSOCK_PROTO_V2	2
+#define KSOCK_PROTO_V3	3
 
 #endif
diff --git a/drivers/staging/lustre/include/linux/lnet/types.h b/drivers/staging/lustre/include/linux/lnet/types.h
index 68d8139a2b11..940f73f266d1 100644
--- a/drivers/staging/lustre/include/linux/lnet/types.h
+++ b/drivers/staging/lustre/include/linux/lnet/types.h
@@ -15,11 +15,7 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
@@ -27,28 +23,28 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012 - 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
+ * Lustre is a trademark of Seagate, Inc.
  */
 
 #ifndef __LNET_TYPES_H__
 #define __LNET_TYPES_H__
 
+#include <linux/types.h>
+
 /** \addtogroup lnet
  * @{ */
 
-#include "../libcfs/libcfs.h"
-
 /** \addtogroup lnet_addr
  * @{ */
 
 /** Portal reserved for LNet's own use.
  * \see lustre/include/lustre/lustre_idl.h for Lustre portal assignments.
  */
-#define LNET_RESERVED_PORTAL      0
+#define LNET_RESERVED_PORTAL	0
 
 /**
  * Address of an end-point in an LNet network.
@@ -68,16 +64,201 @@ typedef __u64 lnet_nid_t;
 typedef __u32 lnet_pid_t;
 
 /** wildcard NID that matches any end-point address */
-#define LNET_NID_ANY      ((lnet_nid_t) -1)
+#define LNET_NID_ANY	((lnet_nid_t) -1)
 /** wildcard PID that matches any lnet_pid_t */
-#define LNET_PID_ANY      ((lnet_pid_t) -1)
+#define LNET_PID_ANY	((lnet_pid_t) -1)
 
 #define LNET_PID_RESERVED 0xf0000000 /* reserved bits in PID */
 #define LNET_PID_USERFLAG 0x80000000 /* set in userspace peers */
+#define LNET_PID_LUSTRE	  12345
 
-#define LNET_TIME_FOREVER    (-1)
+#define LNET_TIME_FOREVER (-1)
 
-/**
+/* how an LNET NID encodes net:address */
+/** extract the address part of an lnet_nid_t */
+
+static inline __u32 LNET_NIDADDR(lnet_nid_t nid)
+{
+	return nid & 0xffffffff;
+}
+
+static inline __u32 LNET_NIDNET(lnet_nid_t nid)
+{
+	return (nid >> 32) & 0xffffffff;
+}
+
+static inline lnet_nid_t LNET_MKNID(__u32 net, __u32 addr)
+{
+	return (((__u64)net) << 32) | addr;
+}
+
+static inline __u32 LNET_NETNUM(__u32 net)
+{
+	return net & 0xffff;
+}
+
+static inline __u32 LNET_NETTYP(__u32 net)
+{
+	return (net >> 16) & 0xffff;
+}
+
+static inline __u32 LNET_MKNET(__u32 type, __u32 num)
+{
+	return (type << 16) | num;
+}
+
+#define WIRE_ATTR	__packed
+
+/* Packed version of lnet_process_id_t to transfer via network */
+typedef struct {
+	/* node id / process id */
+	lnet_nid_t	nid;
+	lnet_pid_t	pid;
+} WIRE_ATTR lnet_process_id_packed_t;
+
+/* The wire handle's interface cookie only matches one network interface in
+ * one epoch (i.e. new cookie when the interface restarts or the node
+ * reboots).  The object cookie only matches one object on that interface
+ * during that object's lifetime (i.e. no cookie re-use). */
+typedef struct {
+	__u64	wh_interface_cookie;
+	__u64	wh_object_cookie;
+} WIRE_ATTR lnet_handle_wire_t;
+
+typedef enum {
+	LNET_MSG_ACK = 0,
+	LNET_MSG_PUT,
+	LNET_MSG_GET,
+	LNET_MSG_REPLY,
+	LNET_MSG_HELLO,
+} lnet_msg_type_t;
+
+/* The variant fields of the portals message header are aligned on an 8
+ * byte boundary in the message header.  Note that all types used in these
+ * wire structs MUST be fixed size and the smaller types are placed at the
+ * end. */
+typedef struct lnet_ack {
+	lnet_handle_wire_t	dst_wmd;
+	__u64			match_bits;
+	__u32			mlength;
+} WIRE_ATTR lnet_ack_t;
+
+typedef struct lnet_put {
+	lnet_handle_wire_t	ack_wmd;
+	__u64			match_bits;
+	__u64			hdr_data;
+	__u32			ptl_index;
+	__u32			offset;
+} WIRE_ATTR lnet_put_t;
+
+typedef struct lnet_get {
+	lnet_handle_wire_t	return_wmd;
+	__u64			match_bits;
+	__u32			ptl_index;
+	__u32			src_offset;
+	__u32			sink_length;
+} WIRE_ATTR lnet_get_t;
+
+typedef struct lnet_reply {
+	lnet_handle_wire_t	dst_wmd;
+} WIRE_ATTR lnet_reply_t;
+
+typedef struct lnet_hello {
+	__u64			incarnation;
+	__u32			type;
+} WIRE_ATTR lnet_hello_t;
+
+typedef struct {
+	lnet_nid_t	dest_nid;
+	lnet_nid_t	src_nid;
+	lnet_pid_t	dest_pid;
+	lnet_pid_t	src_pid;
+	__u32		type;		/* lnet_msg_type_t */
+	__u32		payload_length;	/* payload data to follow */
+	/*<------__u64 aligned------->*/
+	union {
+		lnet_ack_t	ack;
+		lnet_put_t	put;
+		lnet_get_t	get;
+		lnet_reply_t	reply;
+		lnet_hello_t	hello;
+	} msg;
+} WIRE_ATTR lnet_hdr_t;
+
+/* A HELLO message contains a magic number and protocol version
+ * code in the header's dest_nid, the peer's NID in the src_nid, and
+ * LNET_MSG_HELLO in the type field.  All other common fields are zero
+ * (including payload_size; i.e. no payload).
+ * This is for use by byte-stream LNDs (e.g. TCP/IP) to check the peer is
+ * running the same protocol and to find out its NID. These LNDs should
+ * exchange HELLO messages when a connection is first established.  Individual
+ * LNDs can put whatever else they fancy in lnet_hdr_t::msg.
+ */
+typedef struct {
+	__u32	magic;		/* LNET_PROTO_TCP_MAGIC */
+	__u16	version_major;	/* increment on incompatible change */
+	__u16	version_minor;	/* increment on compatible change */
+} WIRE_ATTR lnet_magicversion_t;
+
+/* PROTO MAGIC for LNDs */
+#define LNET_PROTO_IB_MAGIC		0x0be91b91
+#define LNET_PROTO_GNI_MAGIC		0xb00fbabe /* ask Kim */
+#define LNET_PROTO_TCP_MAGIC		0xeebc0ded
+#define LNET_PROTO_ACCEPTOR_MAGIC	0xacce7100
+#define LNET_PROTO_PING_MAGIC		0x70696E67 /* 'ping' */
+
+/* Placeholder for a future "unified" protocol across all LNDs */
+/* Current LNDs that receive a request with this magic will respond with a
+ * "stub" reply using their current protocol */
+#define LNET_PROTO_MAGIC		0x45726963 /* ! */
+
+#define LNET_PROTO_TCP_VERSION_MAJOR	1
+#define LNET_PROTO_TCP_VERSION_MINOR	0
+
+/* Acceptor connection request */
+typedef struct {
+	__u32	acr_magic;		/* PTL_ACCEPTOR_PROTO_MAGIC */
+	__u32	acr_version;		/* protocol version */
+	__u64	acr_nid;		/* target NID */
+} WIRE_ATTR lnet_acceptor_connreq_t;
+
+#define LNET_PROTO_ACCEPTOR_VERSION	1
+
+typedef struct {
+	lnet_nid_t	ns_nid;
+	__u32		ns_status;
+	__u32		ns_unused;
+} WIRE_ATTR lnet_ni_status_t;
+
+typedef struct {
+	__u32			pi_magic;
+	__u32			pi_features;
+	lnet_pid_t		pi_pid;
+	__u32			pi_nnis;
+	lnet_ni_status_t	pi_ni[0];
+} WIRE_ATTR lnet_ping_info_t;
+
+typedef struct lnet_counters {
+	__u32	msgs_alloc;
+	__u32	msgs_max;
+	__u32	errors;
+	__u32	send_count;
+	__u32	recv_count;
+	__u32	route_count;
+	__u32	drop_count;
+	__u64	send_length;
+	__u64	recv_length;
+	__u64	route_length;
+	__u64	drop_length;
+} WIRE_ATTR lnet_counters_t;
+
+#define LNET_NI_STATUS_UP      0x15aac0de
+#define LNET_NI_STATUS_DOWN    0xdeadface
+#define LNET_NI_STATUS_INVALID 0x00000000
+
+#define LNET_MAX_INTERFACES    16
+
+/*
  * Objects maintained by the LNet are accessed through handles. Handle types
  * have names of the form lnet_handle_xx_t, where xx is one of the two letter
  * object type codes ('eq' for event queue, 'md' for memory descriptor, and
@@ -195,8 +376,8 @@ typedef struct {
 	 * one must start on page boundary, and all but the last must end on
 	 * page boundary.
 	 */
-	void	    *start;
-	unsigned int     length;
+	void		*start;
+	unsigned int	 length;
 	/**
 	 * Specifies the maximum number of operations that can be performed
 	 * on the memory descriptor. An operation is any action that could
@@ -207,7 +388,7 @@ typedef struct {
 	 * there is no bound on the number of operations that may be applied
 	 * to a MD.
 	 */
-	int	      threshold;
+	int		 threshold;
 	/**
 	 * Specifies the largest incoming request that the memory descriptor
 	 * should respond to. When the unused portion of a MD (length -
@@ -215,7 +396,7 @@ typedef struct {
 	 * does not respond to further operations. This value is only used
 	 * if the LNET_MD_MAX_SIZE option is set.
 	 */
-	int	      max_size;
+	int		 max_size;
 	/**
 	 * Specifies the behavior of the memory descriptor. A bitwise OR
 	 * of the following values can be used:
@@ -252,14 +433,14 @@ typedef struct {
 	 *   region (i.e. sum of all fragment lengths) must not be less than
 	 *   \a max_size.
 	 */
-	unsigned int     options;
+	unsigned int	 options;
 	/**
 	 * A user-specified value that is associated with the memory
 	 * descriptor. The value does not need to be a pointer, but must fit
 	 * in the space used by a pointer. This value is recorded in events
 	 * associated with operations on this MD.
 	 */
-	void	    *user_ptr;
+	void		*user_ptr;
 	/**
 	 * A handle for the event queue used to log the operations performed on
 	 * the memory region. If this argument is a NULL handle (i.e. nullified
@@ -276,44 +457,33 @@ typedef struct {
 #define LNET_MTU	(1 << LNET_MTU_BITS)
 
 /** limit on the number of fragments in discontiguous MDs */
-#define LNET_MAX_IOV    256
-
-/* Max payload size */
-# define LNET_MAX_PAYLOAD	CONFIG_LNET_MAX_PAYLOAD
-# if (LNET_MAX_PAYLOAD < LNET_MTU)
-#  error "LNET_MAX_PAYLOAD too small - error in configure --with-max-payload-mb"
-# else
-#  if (LNET_MAX_PAYLOAD > (PAGE_SIZE * LNET_MAX_IOV))
-/*  PAGE_SIZE is a constant: check with cpp! */
-#   error "LNET_MAX_PAYLOAD too large - error in configure --with-max-payload-mb"
-#  endif
-# endif
+#define LNET_MAX_IOV	256
 
 /**
  * Options for the MD structure. See lnet_md_t::options.
  */
-#define LNET_MD_OP_PUT	       (1 << 0)
+#define LNET_MD_OP_PUT		(1 << 0)
 /** See lnet_md_t::options. */
-#define LNET_MD_OP_GET	       (1 << 1)
+#define LNET_MD_OP_GET		(1 << 1)
 /** See lnet_md_t::options. */
 #define LNET_MD_MANAGE_REMOTE	(1 << 2)
-/* unused			    (1 << 3) */
+/* unused			(1 << 3) */
 /** See lnet_md_t::options. */
-#define LNET_MD_TRUNCATE	     (1 << 4)
+#define LNET_MD_TRUNCATE	(1 << 4)
 /** See lnet_md_t::options. */
-#define LNET_MD_ACK_DISABLE	  (1 << 5)
+#define LNET_MD_ACK_DISABLE	(1 << 5)
 /** See lnet_md_t::options. */
 #define LNET_MD_IOVEC		(1 << 6)
 /** See lnet_md_t::options. */
-#define LNET_MD_MAX_SIZE	     (1 << 7)
+#define LNET_MD_MAX_SIZE	(1 << 7)
 /** See lnet_md_t::options. */
-#define LNET_MD_KIOV		 (1 << 8)
+#define LNET_MD_KIOV		(1 << 8)
 
 /* For compatibility with Cray Portals */
-#define LNET_MD_PHYS			 0
+#define LNET_MD_PHYS		0
 
 /** Infinite threshold on MD operations. See lnet_md_t::threshold */
-#define LNET_MD_THRESH_INF       (-1)
+#define LNET_MD_THRESH_INF	(-1)
 
 /* NB lustre portals uses struct iovec internally! */
 typedef struct iovec lnet_md_iovec_t;
@@ -323,15 +493,15 @@ typedef struct iovec lnet_md_iovec_t;
  */
 typedef struct {
 	/** Pointer to the page where the fragment resides */
-	struct page      *kiov_page;
+	struct page	*kiov_page;
 	/** Length in bytes of the fragment */
-	unsigned int     kiov_len;
+	unsigned int	 kiov_len;
 	/**
 	 * Starting offset of the fragment within the page. Note that the
 	 * end of the fragment must not pass the end of the page; i.e.,
 	 * kiov_len + kiov_offset <= PAGE_CACHE_SIZE.
 	 */
-	unsigned int     kiov_offset;
+	unsigned int	 kiov_offset;
 } lnet_kiov_t;
 /** @} lnet_md */
 
@@ -379,7 +549,7 @@ typedef enum {
 	LNET_EVENT_UNLINK,
 } lnet_event_kind_t;
 
-#define LNET_SEQ_BASETYPE       long
+#define LNET_SEQ_BASETYPE	long
 typedef unsigned LNET_SEQ_BASETYPE lnet_seq_t;
 #define LNET_SEQ_GT(a, b)	(((signed LNET_SEQ_BASETYPE)((a) - (b))) > 0)
 
@@ -388,23 +558,23 @@ typedef unsigned LNET_SEQ_BASETYPE lnet_seq_t;
  */
 typedef struct {
 	/** The identifier (nid, pid) of the target. */
-	lnet_process_id_t   target;
+	lnet_process_id_t	target;
 	/** The identifier (nid, pid) of the initiator. */
-	lnet_process_id_t   initiator;
+	lnet_process_id_t	initiator;
 	/**
 	 * The NID of the immediate sender. If the request has been forwarded
 	 * by routers, this is the NID of the last hop; otherwise it's the
 	 * same as the initiator.
 	 */
-	lnet_nid_t	  sender;
+	lnet_nid_t		sender;
 	/** Indicates the type of the event. */
-	lnet_event_kind_t   type;
+	lnet_event_kind_t	type;
 	/** The portal table index specified in the request */
-	unsigned int	pt_index;
+	unsigned int		pt_index;
 	/** A copy of the match bits specified in the request. */
-	__u64	       match_bits;
+	__u64			match_bits;
 	/** The length (in bytes) specified in the request. */
-	unsigned int	rlength;
+	unsigned int		rlength;
 	/**
 	 * The length (in bytes) of the data that was manipulated by the
 	 * operation. For truncated operations, the manipulated length will be
@@ -412,47 +582,47 @@ typedef struct {
 	 * see lnet_md_t). For all other operations, the manipulated length
 	 * will be the length of the requested operation, i.e. rlength.
 	 */
-	unsigned int	mlength;
+	unsigned int		mlength;
 	/**
 	 * The handle to the MD associated with the event. The handle may be
 	 * invalid if the MD has been unlinked.
 	 */
-	lnet_handle_md_t    md_handle;
+	lnet_handle_md_t	md_handle;
 	/**
 	 * A snapshot of the state of the MD immediately after the event has
 	 * been processed. In particular, the threshold field in md will
 	 * reflect the value of the threshold after the operation occurred.
 	 */
-	lnet_md_t	   md;
+	lnet_md_t		md;
 	/**
 	 * 64 bits of out-of-band user data. Only valid for LNET_EVENT_PUT.
 	 * \see LNetPut
 	 */
-	__u64	       hdr_data;
+	__u64			hdr_data;
 	/**
 	 * Indicates the completion status of the operation. It's 0 for
 	 * successful operations, otherwise it's an error code.
 	 */
-	int		 status;
+	int			status;
 	/**
 	 * Indicates whether the MD has been unlinked. Note that:
 	 * - An event with unlinked set is the last event on the MD.
 	 * - This field is also set for an explicit LNET_EVENT_UNLINK event.
 	 * \see LNetMDUnlink
 	 */
-	int		 unlinked;
+	int			unlinked;
 	/**
 	 * The displacement (in bytes) into the memory region that the
 	 * operation used. The offset can be determined by the operation for
 	 * a remote managed MD or by the local MD.
 	 * \see lnet_md_t::options
 	 */
-	unsigned int	offset;
+	unsigned int		offset;
 	/**
 	 * The sequence number for this event. Sequence numbers are unique
 	 * to each event.
 	 */
-	volatile lnet_seq_t sequence;
+	volatile lnet_seq_t	sequence;
 } lnet_event_t;
 
 /**
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index 3bad441de8dc..4eb24a11b02a 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -38,25 +38,26 @@
  * Author: Eric Barton <eric@bartonsoftware.com>
  */
 
-#include "o2iblnd.h"
 #include <asm/div64.h>
+#include <asm/page.h>
+#include "o2iblnd.h"
 
 static lnd_t the_o2iblnd = {
-	.lnd_type       = O2IBLND,
-	.lnd_startup    = kiblnd_startup,
-	.lnd_shutdown   = kiblnd_shutdown,
-	.lnd_ctl	= kiblnd_ctl,
-	.lnd_query      = kiblnd_query,
-	.lnd_send       = kiblnd_send,
-	.lnd_recv       = kiblnd_recv,
+	.lnd_type     = O2IBLND,
+	.lnd_startup  = kiblnd_startup,
+	.lnd_shutdown = kiblnd_shutdown,
+	.lnd_ctl      = kiblnd_ctl,
+	.lnd_query    = kiblnd_query,
+	.lnd_send     = kiblnd_send,
+	.lnd_recv     = kiblnd_recv,
 };
 
-kib_data_t	      kiblnd_data;
+kib_data_t kiblnd_data;
 
 static __u32 kiblnd_cksum(void *ptr, int nob)
 {
-	char  *c  = ptr;
-	__u32  sum = 0;
+	char *c = ptr;
+	__u32 sum = 0;
 
 	while (nob-- > 0)
 		sum = ((sum << 1) | (sum >> 31)) + *c++;
@@ -138,10 +139,10 @@ static int kiblnd_msgtype2size(int type)
 
 static int kiblnd_unpack_rd(kib_msg_t *msg, int flip)
 {
-	kib_rdma_desc_t   *rd;
-	int		nob;
-	int		n;
-	int		i;
+	kib_rdma_desc_t *rd;
+	int nob;
+	int n;
+	int i;
 
 	LASSERT(msg->ibm_type == IBLND_MSG_GET_REQ ||
 		 msg->ibm_type == IBLND_MSG_PUT_ACK);
@@ -210,10 +211,10 @@ void kiblnd_pack_msg(lnet_ni_t *ni, kib_msg_t *msg, int version,
 int kiblnd_unpack_msg(kib_msg_t *msg, int nob)
 {
 	const int hdr_size = offsetof(kib_msg_t, ibm_u);
-	__u32     msg_cksum;
-	__u16     version;
-	int       msg_nob;
-	int       flip;
+	__u32 msg_cksum;
+	__u16 version;
+	int msg_nob;
+	int flip;
 
 	/* 6 bytes are enough to have received magic + version */
 	if (nob < 6) {
@@ -320,10 +321,10 @@ int kiblnd_unpack_msg(kib_msg_t *msg, int nob)
 
 int kiblnd_create_peer(lnet_ni_t *ni, kib_peer_t **peerp, lnet_nid_t nid)
 {
-	kib_peer_t	*peer;
-	kib_net_t	*net = ni->ni_data;
-	int		cpt = lnet_cpt_of_nid(nid);
-	unsigned long   flags;
+	kib_peer_t *peer;
+	kib_net_t *net = ni->ni_data;
+	int cpt = lnet_cpt_of_nid(nid);
+	unsigned long flags;
 
 	LASSERT(net != NULL);
 	LASSERT(nid != LNET_NID_ANY);
@@ -385,9 +386,9 @@ kib_peer_t *kiblnd_find_peer_locked(lnet_nid_t nid)
 {
 	/* the caller is responsible for accounting the additional reference
 	 * that this creates */
-	struct list_head       *peer_list = kiblnd_nid2peerlist(nid);
-	struct list_head       *tmp;
-	kib_peer_t       *peer;
+	struct list_head *peer_list = kiblnd_nid2peerlist(nid);
+	struct list_head *tmp;
+	kib_peer_t *peer;
 
 	list_for_each(tmp, peer_list) {
 
@@ -422,10 +423,10 @@ void kiblnd_unlink_peer_locked(kib_peer_t *peer)
 static int kiblnd_get_peer_info(lnet_ni_t *ni, int index,
 				lnet_nid_t *nidp, int *count)
 {
-	kib_peer_t	    *peer;
-	struct list_head	    *ptmp;
-	int		    i;
-	unsigned long	  flags;
+	kib_peer_t *peer;
+	struct list_head *ptmp;
+	int i;
+	unsigned long flags;
 
 	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
@@ -459,9 +460,9 @@ static int kiblnd_get_peer_info(lnet_ni_t *ni, int index,
 
 static void kiblnd_del_peer_locked(kib_peer_t *peer)
 {
-	struct list_head	   *ctmp;
-	struct list_head	   *cnxt;
-	kib_conn_t	   *conn;
+	struct list_head *ctmp;
+	struct list_head *cnxt;
+	kib_conn_t *conn;
 
 	if (list_empty(&peer->ibp_conns)) {
 		kiblnd_unlink_peer_locked(peer);
@@ -480,14 +481,14 @@ static void kiblnd_del_peer_locked(kib_peer_t *peer)
 static int kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
 {
 	LIST_HEAD(zombies);
-	struct list_head	    *ptmp;
-	struct list_head	    *pnxt;
-	kib_peer_t	    *peer;
-	int		    lo;
-	int		    hi;
-	int		    i;
-	unsigned long	  flags;
-	int		    rc = -ENOENT;
+	struct list_head *ptmp;
+	struct list_head *pnxt;
+	kib_peer_t *peer;
+	int lo;
+	int hi;
+	int i;
+	unsigned long flags;
+	int rc = -ENOENT;
 
 	write_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
@@ -532,12 +533,12 @@ static int kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
 
 static kib_conn_t *kiblnd_get_conn_by_idx(lnet_ni_t *ni, int index)
 {
-	kib_peer_t	    *peer;
-	struct list_head	    *ptmp;
-	kib_conn_t	    *conn;
-	struct list_head	    *ctmp;
-	int		    i;
-	unsigned long	  flags;
+	kib_peer_t *peer;
+	struct list_head *ptmp;
+	kib_conn_t *conn;
+	struct list_head *ctmp;
+	int i;
+	unsigned long flags;
 
 	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
@@ -593,7 +594,7 @@ int kiblnd_translate_mtu(int value)
 
 static void kiblnd_setup_mtu_locked(struct rdma_cm_id *cmid)
 {
-	int	   mtu;
+	int mtu;
 
 	/* XXX There is no path record for iWARP, set by netdev->change_mtu? */
 	if (cmid->route.path_rec == NULL)
@@ -607,11 +608,11 @@ static void kiblnd_setup_mtu_locked(struct rdma_cm_id *cmid)
 
 static int kiblnd_get_completion_vector(kib_conn_t *conn, int cpt)
 {
-	cpumask_t	*mask;
-	int		vectors;
-	int		off;
-	int		i;
-	lnet_nid_t	nid = conn->ibc_peer->ibp_nid;
+	cpumask_t *mask;
+	int vectors;
+	int off;
+	int i;
+	lnet_nid_t nid = conn->ibc_peer->ibp_nid;
 
 	vectors = conn->ibc_cmid->device->num_comp_vectors;
 	if (vectors <= 1)
@@ -642,17 +643,18 @@ kib_conn_t *kiblnd_create_conn(kib_peer_t *peer, struct rdma_cm_id *cmid,
 	 * she must dispose of 'cmid'.  (Actually I'd block forever if I tried
 	 * to destroy 'cmid' here since I'm called from the CM which still has
 	 * its ref on 'cmid'). */
-	rwlock_t		*glock = &kiblnd_data.kib_global_lock;
-	kib_net_t	      *net = peer->ibp_ni->ni_data;
-	kib_dev_t	      *dev;
+	rwlock_t *glock = &kiblnd_data.kib_global_lock;
+	kib_net_t *net = peer->ibp_ni->ni_data;
+	kib_dev_t *dev;
 	struct ib_qp_init_attr *init_qp_attr;
-	struct kib_sched_info	*sched;
-	kib_conn_t		*conn;
-	struct ib_cq		*cq;
-	unsigned long		flags;
-	int			cpt;
-	int			rc;
-	int			i;
+	struct kib_sched_info *sched;
+	struct ib_cq_init_attr cq_attr = {};
+	kib_conn_t *conn;
+	struct ib_cq *cq;
+	unsigned long flags;
+	int cpt;
+	int rc;
+	int i;
 
 	LASSERT(net != NULL);
 	LASSERT(!in_interrupt());
@@ -742,10 +744,11 @@ kib_conn_t *kiblnd_create_conn(kib_peer_t *peer, struct rdma_cm_id *cmid,
 
 	kiblnd_map_rx_descs(conn);
 
+	cq_attr.cqe = IBLND_CQ_ENTRIES(version);
+	cq_attr.comp_vector = kiblnd_get_completion_vector(conn, cpt);
 	cq = ib_create_cq(cmid->device,
 			  kiblnd_cq_completion, kiblnd_cq_event, conn,
-			  IBLND_CQ_ENTRIES(version),
-			  kiblnd_get_completion_vector(conn, cpt));
+			  &cq_attr);
 	if (IS_ERR(cq)) {
 		CERROR("Can't create CQ: %ld, cqe: %d\n",
 		       PTR_ERR(cq), IBLND_CQ_ENTRIES(version));
@@ -837,8 +840,8 @@ kib_conn_t *kiblnd_create_conn(kib_peer_t *peer, struct rdma_cm_id *cmid,
 void kiblnd_destroy_conn(kib_conn_t *conn)
 {
 	struct rdma_cm_id *cmid = conn->ibc_cmid;
-	kib_peer_t	*peer = conn->ibc_peer;
-	int		rc;
+	kib_peer_t *peer = conn->ibc_peer;
+	int rc;
 
 	LASSERT(!in_interrupt());
 	LASSERT(atomic_read(&conn->ibc_refcount) == 0);
@@ -904,10 +907,10 @@ void kiblnd_destroy_conn(kib_conn_t *conn)
 
 int kiblnd_close_peer_conns_locked(kib_peer_t *peer, int why)
 {
-	kib_conn_t	     *conn;
-	struct list_head	     *ctmp;
-	struct list_head	     *cnxt;
-	int		     count = 0;
+	kib_conn_t *conn;
+	struct list_head *ctmp;
+	struct list_head *cnxt;
+	int count = 0;
 
 	list_for_each_safe(ctmp, cnxt, &peer->ibp_conns) {
 		conn = list_entry(ctmp, kib_conn_t, ibc_list);
@@ -926,10 +929,10 @@ int kiblnd_close_peer_conns_locked(kib_peer_t *peer, int why)
 int kiblnd_close_stale_conns_locked(kib_peer_t *peer,
 				     int version, __u64 incarnation)
 {
-	kib_conn_t	     *conn;
-	struct list_head	     *ctmp;
-	struct list_head	     *cnxt;
-	int		     count = 0;
+	kib_conn_t *conn;
+	struct list_head *ctmp;
+	struct list_head *cnxt;
+	int count = 0;
 
 	list_for_each_safe(ctmp, cnxt, &peer->ibp_conns) {
 		conn = list_entry(ctmp, kib_conn_t, ibc_list);
@@ -953,14 +956,14 @@ int kiblnd_close_stale_conns_locked(kib_peer_t *peer,
 
 static int kiblnd_close_matching_conns(lnet_ni_t *ni, lnet_nid_t nid)
 {
-	kib_peer_t	     *peer;
-	struct list_head	     *ptmp;
-	struct list_head	     *pnxt;
-	int		     lo;
-	int		     hi;
-	int		     i;
-	unsigned long	   flags;
-	int		     count = 0;
+	kib_peer_t *peer;
+	struct list_head *ptmp;
+	struct list_head *pnxt;
+	int lo;
+	int hi;
+	int i;
+	unsigned long flags;
+	int count = 0;
 
 	write_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
@@ -1001,17 +1004,17 @@ static int kiblnd_close_matching_conns(lnet_ni_t *ni, lnet_nid_t nid)
 int kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 {
 	struct libcfs_ioctl_data *data = arg;
-	int		       rc = -EINVAL;
+	int rc = -EINVAL;
 
 	switch (cmd) {
 	case IOC_LIBCFS_GET_PEER: {
-		lnet_nid_t   nid = 0;
-		int	  count = 0;
+		lnet_nid_t nid = 0;
+		int count = 0;
 
 		rc = kiblnd_get_peer_info(ni, data->ioc_count,
 					  &nid, &count);
-		data->ioc_nid    = nid;
-		data->ioc_count  = count;
+		data->ioc_nid   = nid;
+		data->ioc_count = count;
 		break;
 	}
 
@@ -1053,11 +1056,11 @@ int kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 
 void kiblnd_query(lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 {
-	unsigned long	last_alive = 0;
-	unsigned long	now = cfs_time_current();
-	rwlock_t	*glock = &kiblnd_data.kib_global_lock;
-	kib_peer_t	*peer;
-	unsigned long	flags;
+	unsigned long last_alive = 0;
+	unsigned long now = cfs_time_current();
+	rwlock_t *glock = &kiblnd_data.kib_global_lock;
+	kib_peer_t *peer;
+	unsigned long flags;
 
 	read_lock_irqsave(glock, flags);
 
@@ -1086,8 +1089,8 @@ void kiblnd_query(lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 
 void kiblnd_free_pages(kib_pages_t *p)
 {
-	int	npages = p->ibp_npages;
-	int	i;
+	int npages = p->ibp_npages;
+	int i;
 
 	for (i = 0; i < npages; i++) {
 		if (p->ibp_pages[i] != NULL)
@@ -1099,8 +1102,8 @@ void kiblnd_free_pages(kib_pages_t *p)
 
 int kiblnd_alloc_pages(kib_pages_t **pp, int cpt, int npages)
 {
-	kib_pages_t	*p;
-	int		i;
+	kib_pages_t *p;
+	int i;
 
 	LIBCFS_CPT_ALLOC(p, lnet_cpt_table(), cpt,
 			 offsetof(kib_pages_t, ibp_pages[npages]));
@@ -1130,7 +1133,7 @@ int kiblnd_alloc_pages(kib_pages_t **pp, int cpt, int npages)
 void kiblnd_unmap_rx_descs(kib_conn_t *conn)
 {
 	kib_rx_t *rx;
-	int       i;
+	int i;
 
 	LASSERT(conn->ibc_rxs != NULL);
 	LASSERT(conn->ibc_hdev != NULL);
@@ -1153,14 +1156,13 @@ void kiblnd_unmap_rx_descs(kib_conn_t *conn)
 
 void kiblnd_map_rx_descs(kib_conn_t *conn)
 {
-	kib_rx_t       *rx;
-	struct page    *pg;
-	int	     pg_off;
-	int	     ipg;
-	int	     i;
+	kib_rx_t *rx;
+	struct page *pg;
+	int pg_off;
+	int ipg;
+	int i;
 
-	for (pg_off = ipg = i = 0;
-	     i < IBLND_RX_MSGS(conn->ibc_version); i++) {
+	for (pg_off = ipg = i = 0; i < IBLND_RX_MSGS(conn->ibc_version); i++) {
 		pg = conn->ibc_rx_pages->ibp_pages[ipg];
 		rx = &conn->ibc_rxs[i];
 
@@ -1177,7 +1179,7 @@ void kiblnd_map_rx_descs(kib_conn_t *conn)
 
 		CDEBUG(D_NET, "rx %d: %p %#llx(%#llx)\n",
 		       i, rx->rx_msg, rx->rx_msgaddr,
-		       lnet_page2phys(pg) + pg_off);
+		       (__u64)(page_to_phys(pg) + pg_off));
 
 		pg_off += IBLND_MSG_SIZE;
 		LASSERT(pg_off <= PAGE_SIZE);
@@ -1192,9 +1194,9 @@ void kiblnd_map_rx_descs(kib_conn_t *conn)
 
 static void kiblnd_unmap_tx_pool(kib_tx_pool_t *tpo)
 {
-	kib_hca_dev_t  *hdev = tpo->tpo_hdev;
-	kib_tx_t       *tx;
-	int	     i;
+	kib_hca_dev_t *hdev = tpo->tpo_hdev;
+	kib_tx_t *tx;
+	int i;
 
 	LASSERT(tpo->tpo_pool.po_allocated == 0);
 
@@ -1216,8 +1218,8 @@ static void kiblnd_unmap_tx_pool(kib_tx_pool_t *tpo)
 static kib_hca_dev_t *kiblnd_current_hdev(kib_dev_t *dev)
 {
 	kib_hca_dev_t *hdev;
-	unsigned long  flags;
-	int	    i = 0;
+	unsigned long flags;
+	int i = 0;
 
 	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 	while (dev->ibd_failover) {
@@ -1240,15 +1242,15 @@ static kib_hca_dev_t *kiblnd_current_hdev(kib_dev_t *dev)
 
 static void kiblnd_map_tx_pool(kib_tx_pool_t *tpo)
 {
-	kib_pages_t    *txpgs = tpo->tpo_tx_pages;
-	kib_pool_t     *pool  = &tpo->tpo_pool;
-	kib_net_t      *net   = pool->po_owner->ps_net;
-	kib_dev_t      *dev;
-	struct page    *page;
-	kib_tx_t       *tx;
-	int	     page_offset;
-	int	     ipage;
-	int	     i;
+	kib_pages_t *txpgs = tpo->tpo_tx_pages;
+	kib_pool_t *pool = &tpo->tpo_pool;
+	kib_net_t *net = pool->po_owner->ps_net;
+	kib_dev_t *dev;
+	struct page *page;
+	kib_tx_t *tx;
+	int page_offset;
+	int ipage;
+	int i;
 
 	LASSERT(net != NULL);
 
@@ -1291,7 +1293,7 @@ static void kiblnd_map_tx_pool(kib_tx_pool_t *tpo)
 
 struct ib_mr *kiblnd_find_dma_mr(kib_hca_dev_t *hdev, __u64 addr, __u64 size)
 {
-	__u64   index;
+	__u64 index;
 
 	LASSERT(hdev->ibh_mrs[0] != NULL);
 
@@ -1311,7 +1313,7 @@ struct ib_mr *kiblnd_find_rd_dma_mr(kib_hca_dev_t *hdev, kib_rdma_desc_t *rd)
 {
 	struct ib_mr *prev_mr;
 	struct ib_mr *mr;
-	int	   i;
+	int i;
 
 	LASSERT(hdev->ibh_mrs[0] != NULL);
 
@@ -1382,18 +1384,18 @@ static int kiblnd_create_fmr_pool(kib_fmr_poolset_t *fps,
 				  kib_fmr_pool_t **pp_fpo)
 {
 	/* FMR pool for RDMA */
-	kib_dev_t	       *dev = fps->fps_net->ibn_dev;
-	kib_fmr_pool_t	  *fpo;
+	kib_dev_t *dev = fps->fps_net->ibn_dev;
+	kib_fmr_pool_t *fpo;
 	struct ib_fmr_pool_param param = {
 		.max_pages_per_fmr = LNET_MAX_PAYLOAD/PAGE_SIZE,
-		.page_shift	= PAGE_SHIFT,
-		.access	    = (IB_ACCESS_LOCAL_WRITE |
-				      IB_ACCESS_REMOTE_WRITE),
-		.pool_size	   = fps->fps_pool_size,
+		.page_shift        = PAGE_SHIFT,
+		.access            = (IB_ACCESS_LOCAL_WRITE |
+		                      IB_ACCESS_REMOTE_WRITE),
+		.pool_size         = fps->fps_pool_size,
 		.dirty_watermark   = fps->fps_flush_trigger,
 		.flush_function    = NULL,
-		.flush_arg	 = NULL,
-		.cache	     = !!*kiblnd_tunables.kib_fmr_cache};
+		.flush_arg         = NULL,
+		.cache             = !!*kiblnd_tunables.kib_fmr_cache};
 	int rc;
 
 	LIBCFS_CPT_ALLOC(fpo, lnet_cpt_table(), fps->fps_cpt, sizeof(*fpo));
@@ -1454,7 +1456,7 @@ static int kiblnd_init_fmr_poolset(kib_fmr_poolset_t *fps, int cpt,
 				   int flush_trigger)
 {
 	kib_fmr_pool_t *fpo;
-	int	     rc;
+	int rc;
 
 	memset(fps, 0, sizeof(kib_fmr_poolset_t));
 
@@ -1485,11 +1487,11 @@ static int kiblnd_fmr_pool_is_idle(kib_fmr_pool_t *fpo, unsigned long now)
 void kiblnd_fmr_pool_unmap(kib_fmr_t *fmr, int status)
 {
 	LIST_HEAD(zombies);
-	kib_fmr_pool_t    *fpo = fmr->fmr_pool;
+	kib_fmr_pool_t *fpo = fmr->fmr_pool;
 	kib_fmr_poolset_t *fps = fpo->fpo_owner;
-	unsigned long	 now = cfs_time_current();
-	kib_fmr_pool_t    *tmp;
-	int		rc;
+	unsigned long now = cfs_time_current();
+	kib_fmr_pool_t *tmp;
+	int rc;
 
 	rc = ib_fmr_pool_unmap(fmr->fmr_pfmr);
 	LASSERT(rc == 0);
@@ -1525,9 +1527,9 @@ int kiblnd_fmr_pool_map(kib_fmr_poolset_t *fps, __u64 *pages, int npages,
 			__u64 iov, kib_fmr_t *fmr)
 {
 	struct ib_pool_fmr *pfmr;
-	kib_fmr_pool_t     *fpo;
-	__u64	       version;
-	int		 rc;
+	kib_fmr_pool_t *fpo;
+	__u64 version;
+	int rc;
 
  again:
 	spin_lock(&fps->fps_lock);
@@ -1658,13 +1660,13 @@ static int kiblnd_init_poolset(kib_poolset_t *ps, int cpt,
 			       kib_ps_node_init_t nd_init,
 			       kib_ps_node_fini_t nd_fini)
 {
-	kib_pool_t	*pool;
-	int		rc;
+	kib_pool_t *pool;
+	int rc;
 
 	memset(ps, 0, sizeof(kib_poolset_t));
 
-	ps->ps_cpt	    = cpt;
-	ps->ps_net	  = net;
+	ps->ps_cpt          = cpt;
+	ps->ps_net          = net;
 	ps->ps_pool_create  = po_create;
 	ps->ps_pool_destroy = po_destroy;
 	ps->ps_node_init    = nd_init;
@@ -1698,9 +1700,9 @@ static int kiblnd_pool_is_idle(kib_pool_t *pool, unsigned long now)
 void kiblnd_pool_free_node(kib_pool_t *pool, struct list_head *node)
 {
 	LIST_HEAD(zombies);
-	kib_poolset_t  *ps = pool->po_owner;
-	kib_pool_t     *tmp;
-	unsigned long      now = cfs_time_current();
+	kib_poolset_t *ps = pool->po_owner;
+	kib_pool_t *tmp;
+	unsigned long now = cfs_time_current();
 
 	spin_lock(&ps->ps_lock);
 
@@ -1727,9 +1729,9 @@ void kiblnd_pool_free_node(kib_pool_t *pool, struct list_head *node)
 
 struct list_head *kiblnd_pool_alloc_node(kib_poolset_t *ps)
 {
-	struct list_head	    *node;
-	kib_pool_t	    *pool;
-	int		    rc;
+	struct list_head *node;
+	kib_pool_t *pool;
+	int rc;
 
  again:
 	spin_lock(&ps->ps_lock);
@@ -1789,8 +1791,8 @@ struct list_head *kiblnd_pool_alloc_node(kib_poolset_t *ps)
 
 void kiblnd_pmr_pool_unmap(kib_phys_mr_t *pmr)
 {
-	kib_pmr_pool_t      *ppo = pmr->pmr_pool;
-	struct ib_mr	*mr  = pmr->pmr_mr;
+	kib_pmr_pool_t *ppo = pmr->pmr_pool;
+	struct ib_mr *mr = pmr->pmr_mr;
 
 	pmr->pmr_mr = NULL;
 	kiblnd_pool_free_node(&ppo->ppo_pool, &pmr->pmr_list);
@@ -1802,9 +1804,9 @@ int kiblnd_pmr_pool_map(kib_pmr_poolset_t *pps, kib_hca_dev_t *hdev,
 		    kib_rdma_desc_t *rd, __u64 *iova, kib_phys_mr_t **pp_pmr)
 {
 	kib_phys_mr_t *pmr;
-	struct list_head    *node;
-	int	    rc;
-	int	    i;
+	struct list_head *node;
+	int rc;
+	int i;
 
 	node = kiblnd_pool_alloc_node(&pps->pps_poolset);
 	if (node == NULL) {
@@ -1846,7 +1848,7 @@ int kiblnd_pmr_pool_map(kib_pmr_poolset_t *pps, kib_hca_dev_t *hdev,
 static void kiblnd_destroy_pmr_pool(kib_pool_t *pool)
 {
 	kib_pmr_pool_t *ppo = container_of(pool, kib_pmr_pool_t, ppo_pool);
-	kib_phys_mr_t  *pmr;
+	kib_phys_mr_t *pmr;
 	kib_phys_mr_t *tmp;
 
 	LASSERT(pool->po_allocated == 0);
@@ -1881,10 +1883,10 @@ static inline int kiblnd_pmr_pool_size(int ncpts)
 static int kiblnd_create_pmr_pool(kib_poolset_t *ps, int size,
 				  kib_pool_t **pp_po)
 {
-	struct kib_pmr_pool	*ppo;
-	struct kib_pool		*pool;
-	kib_phys_mr_t		*pmr;
-	int			i;
+	struct kib_pmr_pool *ppo;
+	struct kib_pool *pool;
+	kib_phys_mr_t *pmr;
+	int i;
 
 	LIBCFS_CPT_ALLOC(ppo, lnet_cpt_table(),
 			 ps->ps_cpt, sizeof(kib_pmr_pool_t));
@@ -1923,8 +1925,8 @@ static int kiblnd_create_pmr_pool(kib_poolset_t *ps, int size,
 
 static void kiblnd_destroy_tx_pool(kib_pool_t *pool)
 {
-	kib_tx_pool_t  *tpo = container_of(pool, kib_tx_pool_t, tpo_pool);
-	int	     i;
+	kib_tx_pool_t *tpo = container_of(pool, kib_tx_pool_t, tpo_pool);
+	int i;
 
 	LASSERT(pool->po_allocated == 0);
 
@@ -1979,9 +1981,9 @@ static int kiblnd_tx_pool_size(int ncpts)
 static int kiblnd_create_tx_pool(kib_poolset_t *ps, int size,
 				 kib_pool_t **pp_po)
 {
-	int	    i;
-	int	    npg;
-	kib_pool_t    *pool;
+	int i;
+	int npg;
+	kib_pool_t *pool;
 	kib_tx_pool_t *tpo;
 
 	LIBCFS_CPT_ALLOC(tpo, lnet_cpt_table(), ps->ps_cpt, sizeof(*tpo));
@@ -2064,19 +2066,19 @@ static void kiblnd_tx_init(kib_pool_t *pool, struct list_head *node)
 {
 	kib_tx_poolset_t *tps = container_of(pool->po_owner, kib_tx_poolset_t,
 					     tps_poolset);
-	kib_tx_t	 *tx  = list_entry(node, kib_tx_t, tx_list);
+	kib_tx_t *tx  = list_entry(node, kib_tx_t, tx_list);
 
 	tx->tx_cookie = tps->tps_next_tx_cookie++;
 }
 
 static void kiblnd_net_fini_pools(kib_net_t *net)
 {
-	int	i;
+	int i;
 
 	cfs_cpt_for_each(i, lnet_cpt_table()) {
-		kib_tx_poolset_t	*tps;
-		kib_fmr_poolset_t	*fps;
-		kib_pmr_poolset_t	*pps;
+		kib_tx_poolset_t *tps;
+		kib_fmr_poolset_t *fps;
+		kib_pmr_poolset_t *pps;
 
 		if (net->ibn_tx_ps != NULL) {
 			tps = net->ibn_tx_ps[i];
@@ -2112,16 +2114,15 @@ static void kiblnd_net_fini_pools(kib_net_t *net)
 
 static int kiblnd_net_init_pools(kib_net_t *net, __u32 *cpts, int ncpts)
 {
-	unsigned long	flags;
-	int		cpt;
-	int		rc;
-	int		i;
+	unsigned long flags;
+	int cpt;
+	int rc;
+	int i;
 
 	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 	if (*kiblnd_tunables.kib_map_on_demand == 0 &&
 	    net->ibn_dev->ibd_hdev->ibh_nmrs == 1) {
-		read_unlock_irqrestore(&kiblnd_data.kib_global_lock,
-					   flags);
+		read_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 		goto create_tx_pool;
 	}
 
@@ -2241,7 +2242,7 @@ static int kiblnd_net_init_pools(kib_net_t *net, __u32 *cpts, int ncpts)
 static int kiblnd_hdev_get_attr(kib_hca_dev_t *hdev)
 {
 	struct ib_device_attr *attr;
-	int		    rc;
+	int rc;
 
 	/* It's safe to assume a HCA can handle a page size
 	 * matching that of the native system */
@@ -2284,7 +2285,7 @@ static int kiblnd_hdev_get_attr(kib_hca_dev_t *hdev)
 
 static void kiblnd_hdev_cleanup_mrs(kib_hca_dev_t *hdev)
 {
-	int     i;
+	int i;
 
 	if (hdev->ibh_nmrs == 0 || hdev->ibh_mrs == NULL)
 		return;
@@ -2317,12 +2318,11 @@ void kiblnd_hdev_destroy(kib_hca_dev_t *hdev)
 static int kiblnd_hdev_setup_mrs(kib_hca_dev_t *hdev)
 {
 	struct ib_mr *mr;
-	int	   i;
-	int	   rc;
-	__u64	 mm_size;
-	__u64	 mr_size;
-	int	   acflags = IB_ACCESS_LOCAL_WRITE |
-				IB_ACCESS_REMOTE_WRITE;
+	int i;
+	int rc;
+	__u64 mm_size;
+	__u64 mr_size;
+	int acflags = IB_ACCESS_LOCAL_WRITE | IB_ACCESS_REMOTE_WRITE;
 
 	rc = kiblnd_hdev_get_attr(hdev);
 	if (rc != 0)
@@ -2371,11 +2371,11 @@ static int kiblnd_hdev_setup_mrs(kib_hca_dev_t *hdev)
 
 	for (i = 0; i < hdev->ibh_nmrs; i++) {
 		struct ib_phys_buf ipb;
-		__u64	      iova;
+		__u64 iova;
 
 		ipb.size = hdev->ibh_mr_size;
 		ipb.addr = i * mr_size;
-		iova     = ipb.addr;
+		iova = ipb.addr;
 
 		mr = ib_reg_phys_mr(hdev->ibh_pd, &ipb, 1, acflags, &iova);
 		if (IS_ERR(mr)) {
@@ -2406,10 +2406,10 @@ static int kiblnd_dummy_callback(struct rdma_cm_id *cmid,
 
 static int kiblnd_dev_need_failover(kib_dev_t *dev)
 {
-	struct rdma_cm_id  *cmid;
-	struct sockaddr_in  srcaddr;
-	struct sockaddr_in  dstaddr;
-	int		 rc;
+	struct rdma_cm_id *cmid;
+	struct sockaddr_in srcaddr;
+	struct sockaddr_in dstaddr;
+	int rc;
 
 	if (dev->ibd_hdev == NULL || /* initializing */
 	    dev->ibd_hdev->ibh_cmid == NULL || /* listener is dead */
@@ -2435,7 +2435,7 @@ static int kiblnd_dev_need_failover(kib_dev_t *dev)
 	}
 
 	memset(&srcaddr, 0, sizeof(srcaddr));
-	srcaddr.sin_family      = AF_INET;
+	srcaddr.sin_family = AF_INET;
 	srcaddr.sin_addr.s_addr = (__force u32)htonl(dev->ibd_ifip);
 
 	memset(&dstaddr, 0, sizeof(dstaddr));
@@ -2464,15 +2464,14 @@ int kiblnd_dev_failover(kib_dev_t *dev)
 	LIST_HEAD(zombie_tpo);
 	LIST_HEAD(zombie_ppo);
 	LIST_HEAD(zombie_fpo);
-	struct rdma_cm_id  *cmid  = NULL;
-	kib_hca_dev_t      *hdev  = NULL;
-	kib_hca_dev_t      *old;
-	struct ib_pd       *pd;
-	kib_net_t	  *net;
-	struct sockaddr_in  addr;
-	unsigned long       flags;
-	int		 rc = 0;
-	int		    i;
+	struct rdma_cm_id *cmid  = NULL;
+	kib_hca_dev_t *hdev  = NULL;
+	struct ib_pd *pd;
+	kib_net_t *net;
+	struct sockaddr_in addr;
+	unsigned long flags;
+	int rc = 0;
+	int i;
 
 	LASSERT(*kiblnd_tunables.kib_dev_failover > 1 ||
 		 dev->ibd_can_failover ||
@@ -2558,9 +2557,7 @@ int kiblnd_dev_failover(kib_dev_t *dev)
 
 	write_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
-	old = dev->ibd_hdev;
-	dev->ibd_hdev = hdev; /* take over the refcount */
-	hdev = old;
+	swap(dev->ibd_hdev, hdev); /* take over the refcount */
 
 	list_for_each_entry(net, &dev->ibd_nets, ibn_list) {
 		cfs_cpt_for_each(i, lnet_cpt_table()) {
@@ -2614,13 +2611,13 @@ void kiblnd_destroy_dev(kib_dev_t *dev)
 static kib_dev_t *kiblnd_create_dev(char *ifname)
 {
 	struct net_device *netdev;
-	kib_dev_t	 *dev;
-	__u32	      netmask;
-	__u32	      ip;
-	int		up;
-	int		rc;
+	kib_dev_t *dev;
+	__u32 netmask;
+	__u32 ip;
+	int up;
+	int rc;
 
-	rc = libcfs_ipif_query(ifname, &up, &ip, &netmask);
+	rc = lnet_ipif_query(ifname, &up, &ip, &netmask);
 	if (rc != 0) {
 		CERROR("Can't query IPoIB interface %s: %d\n",
 		       ifname, rc);
@@ -2665,8 +2662,8 @@ static kib_dev_t *kiblnd_create_dev(char *ifname)
 
 static void kiblnd_base_shutdown(void)
 {
-	struct kib_sched_info	*sched;
-	int			i;
+	struct kib_sched_info *sched;
+	int i;
 
 	LASSERT(list_empty(&kiblnd_data.kib_devs));
 
@@ -2732,10 +2729,10 @@ static void kiblnd_base_shutdown(void)
 
 void kiblnd_shutdown(lnet_ni_t *ni)
 {
-	kib_net_t	*net = ni->ni_data;
-	rwlock_t     *g_lock = &kiblnd_data.kib_global_lock;
-	int	       i;
-	unsigned long     flags;
+	kib_net_t *net = ni->ni_data;
+	rwlock_t *g_lock = &kiblnd_data.kib_global_lock;
+	int i;
+	unsigned long flags;
 
 	LASSERT(kiblnd_data.kib_init == IBLND_INIT_ALL);
 
@@ -2804,9 +2801,9 @@ out:
 
 static int kiblnd_base_startup(void)
 {
-	struct kib_sched_info	*sched;
-	int			rc;
-	int			i;
+	struct kib_sched_info *sched;
+	int rc;
+	int i;
 
 	LASSERT(kiblnd_data.kib_init == IBLND_INIT_NOTHING);
 
@@ -2821,8 +2818,7 @@ static int kiblnd_base_startup(void)
 
 	kiblnd_data.kib_peer_hash_size = IBLND_PEER_HASH_SIZE;
 	LIBCFS_ALLOC(kiblnd_data.kib_peers,
-		     sizeof(struct list_head) *
-			    kiblnd_data.kib_peer_hash_size);
+		     sizeof(struct list_head) * kiblnd_data.kib_peer_hash_size);
 	if (kiblnd_data.kib_peers == NULL)
 		goto failed;
 	for (i = 0; i < kiblnd_data.kib_peer_hash_size; i++)
@@ -2840,7 +2836,7 @@ static int kiblnd_base_startup(void)
 		goto failed;
 
 	cfs_percpt_for_each(sched, i, kiblnd_data.kib_scheds) {
-		int	nthrs;
+		int nthrs;
 
 		spin_lock_init(&sched->ibs_lock);
 		INIT_LIST_HEAD(&sched->ibs_conns);
@@ -2893,9 +2889,9 @@ static int kiblnd_base_startup(void)
 
 static int kiblnd_start_schedulers(struct kib_sched_info *sched)
 {
-	int	rc = 0;
-	int	nthrs;
-	int	i;
+	int rc = 0;
+	int nthrs;
+	int i;
 
 	if (sched->ibs_nthreads == 0) {
 		if (*kiblnd_tunables.kib_nscheds > 0) {
@@ -2913,8 +2909,8 @@ static int kiblnd_start_schedulers(struct kib_sched_info *sched)
 	}
 
 	for (i = 0; i < nthrs; i++) {
-		long	id;
-		char	name[20];
+		long id;
+		char name[20];
 
 		id = KIB_THREAD_ID(sched->ibs_cpt, sched->ibs_nthreads + i);
 		snprintf(name, sizeof(name), "kiblnd_sd_%02ld_%02ld",
@@ -2935,9 +2931,9 @@ static int kiblnd_start_schedulers(struct kib_sched_info *sched)
 static int kiblnd_dev_start_threads(kib_dev_t *dev, int newdev, __u32 *cpts,
 				    int ncpts)
 {
-	int	cpt;
-	int	rc;
-	int	i;
+	int cpt;
+	int rc;
+	int i;
 
 	for (i = 0; i < ncpts; i++) {
 		struct kib_sched_info *sched;
@@ -2960,10 +2956,10 @@ static int kiblnd_dev_start_threads(kib_dev_t *dev, int newdev, __u32 *cpts,
 
 static kib_dev_t *kiblnd_dev_search(char *ifname)
 {
-	kib_dev_t	*alias = NULL;
-	kib_dev_t	*dev;
-	char		*colon;
-	char		*colon2;
+	kib_dev_t *alias = NULL;
+	kib_dev_t *dev;
+	char *colon;
+	char *colon2;
 
 	colon = strchr(ifname, ':');
 	list_for_each_entry(dev, &kiblnd_data.kib_devs, ibd_list) {
@@ -2992,13 +2988,13 @@ static kib_dev_t *kiblnd_dev_search(char *ifname)
 
 int kiblnd_startup(lnet_ni_t *ni)
 {
-	char		     *ifname;
-	kib_dev_t		*ibdev = NULL;
-	kib_net_t		*net;
-	struct timeval	    tv;
-	unsigned long	     flags;
-	int		       rc;
-	int			  newdev;
+	char *ifname;
+	kib_dev_t *ibdev = NULL;
+	kib_net_t *net;
+	struct timeval tv;
+	unsigned long flags;
+	int rc;
+	int newdev;
 
 	LASSERT(ni->ni_lnd == &the_o2iblnd);
 
@@ -3091,7 +3087,7 @@ static void __exit kiblnd_module_fini(void)
 
 static int __init kiblnd_module_init(void)
 {
-	int    rc;
+	int rc;
 
 	CLASSERT(sizeof(kib_msg_t) <= IBLND_MSG_SIZE);
 	CLASSERT(offsetof(kib_msg_t,
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
index cd664d025f41..f5d1d9f8f1ed 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
@@ -65,7 +65,6 @@
 #include "../../../include/linux/libcfs/libcfs.h"
 #include "../../../include/linux/lnet/lnet.h"
 #include "../../../include/linux/lnet/lib-lnet.h"
-#include "../../../include/linux/lnet/lnet-sysctl.h"
 
 #include <rdma/rdma_cm.h>
 #include <rdma/ib_cm.h>
@@ -80,42 +79,47 @@
 #define IBLND_N_SCHED_HIGH		4
 
 typedef struct {
-	int	      *kib_dev_failover;     /* HCA failover */
-	unsigned int     *kib_service;	  /* IB service number */
-	int	      *kib_min_reconnect_interval; /* first failed connection retry... */
-	int	      *kib_max_reconnect_interval; /* ...exponentially increasing to this */
-	int	      *kib_cksum;	    /* checksum kib_msg_t? */
-	int	      *kib_timeout;	  /* comms timeout (seconds) */
-	int	      *kib_keepalive;	/* keepalive timeout (seconds) */
-	int	      *kib_ntx;	      /* # tx descs */
-	int	      *kib_credits;	  /* # concurrent sends */
-	int	      *kib_peertxcredits;    /* # concurrent sends to 1 peer */
-	int	      *kib_peerrtrcredits;   /* # per-peer router buffer credits */
-	int	      *kib_peercredits_hiw;  /* # when eagerly to return credits */
-	int	      *kib_peertimeout;      /* seconds to consider peer dead */
-	char	    **kib_default_ipif;     /* default IPoIB interface */
-	int	      *kib_retry_count;
-	int	      *kib_rnr_retry_count;
-	int	      *kib_concurrent_sends; /* send work queue sizing */
-	int		 *kib_ib_mtu;		/* IB MTU */
-	int	      *kib_map_on_demand;    /* map-on-demand if RD has more fragments
-						 * than this value, 0 disable map-on-demand */
-	int	      *kib_pmr_pool_size;    /* # physical MR in pool */
-	int	      *kib_fmr_pool_size;    /* # FMRs in pool */
-	int	      *kib_fmr_flush_trigger; /* When to trigger FMR flush */
-	int	      *kib_fmr_cache;	/* enable FMR pool cache? */
-	int	      *kib_require_priv_port;/* accept only privileged ports */
-	int	      *kib_use_priv_port;    /* use privileged port for active connect */
-	/* # threads on each CPT */
-	int		 *kib_nscheds;
+	int          *kib_dev_failover;      /* HCA failover */
+	unsigned int *kib_service;           /* IB service number */
+	int          *kib_min_reconnect_interval; /* first failed connection
+						   * retry... */
+	int          *kib_max_reconnect_interval; /* ...exponentially increasing
+						   * to this */
+	int          *kib_cksum;             /* checksum kib_msg_t? */
+	int          *kib_timeout;           /* comms timeout (seconds) */
+	int          *kib_keepalive;         /* keepalive timeout (seconds) */
+	int          *kib_ntx;               /* # tx descs */
+	int          *kib_credits;           /* # concurrent sends */
+	int          *kib_peertxcredits;     /* # concurrent sends to 1 peer */
+	int          *kib_peerrtrcredits;    /* # per-peer router buffer
+					      * credits */
+	int          *kib_peercredits_hiw;   /* # when eagerly to return
+					      * credits */
+	int          *kib_peertimeout;       /* seconds to consider peer dead */
+	char         **kib_default_ipif;     /* default IPoIB interface */
+	int          *kib_retry_count;
+	int          *kib_rnr_retry_count;
+	int          *kib_concurrent_sends;  /* send work queue sizing */
+	int          *kib_ib_mtu;            /* IB MTU */
+	int          *kib_map_on_demand;     /* map-on-demand if RD has more
+					      * fragments than this value, 0
+					      * disable map-on-demand */
+	int          *kib_pmr_pool_size;     /* # physical MR in pool */
+	int          *kib_fmr_pool_size;     /* # FMRs in pool */
+	int          *kib_fmr_flush_trigger; /* When to trigger FMR flush */
+	int          *kib_fmr_cache;         /* enable FMR pool cache? */
+	int          *kib_require_priv_port; /* accept only privileged ports */
+	int          *kib_use_priv_port;     /* use privileged port for active
+					      * connect */
+	int          *kib_nscheds;           /* # threads on each CPT */
 } kib_tunables_t;
 
 extern kib_tunables_t  kiblnd_tunables;
 
-#define IBLND_MSG_QUEUE_SIZE_V1      8	  /* V1 only : # messages/RDMAs in-flight */
-#define IBLND_CREDIT_HIGHWATER_V1    7	  /* V1 only : when eagerly to return credits */
+#define IBLND_MSG_QUEUE_SIZE_V1   8 /* V1 only : # messages/RDMAs in-flight */
+#define IBLND_CREDIT_HIGHWATER_V1 7 /* V1 only : when eagerly to return credits */
 
-#define IBLND_CREDITS_DEFAULT	8	  /* default # of peer credits */
+#define IBLND_CREDITS_DEFAULT     8 /* default # of peer credits */
 #define IBLND_CREDITS_MAX	  ((typeof(((kib_msg_t*) 0)->ibm_credits)) - 1)  /* Max # of peer credits */
 
 #define IBLND_MSG_QUEUE_SIZE(v)    ((v) == IBLND_MSG_VERSION_1 ? \
@@ -186,34 +190,36 @@ struct kib_hca_dev;
 #endif
 
 typedef struct {
-	struct list_head	   ibd_list;	  /* chain on kib_devs */
-	struct list_head	   ibd_fail_list;     /* chain on kib_failed_devs */
-	__u32		ibd_ifip;	  /* IPoIB interface IP */
-	/** IPoIB interface name */
-	char		 ibd_ifname[KIB_IFNAME_SIZE];
-	int		  ibd_nnets;	 /* # nets extant */
-
-	unsigned long	   ibd_next_failover;
-	int		  ibd_failed_failover; /* # failover failures */
-	unsigned int	 ibd_failover;      /* failover in progress */
-	unsigned int	 ibd_can_failover;  /* IPoIB interface is a bonding master */
-	struct list_head	   ibd_nets;
-	struct kib_hca_dev  *ibd_hdev;
+	struct list_head   ibd_list;            /* chain on kib_devs */
+	struct list_head   ibd_fail_list;       /* chain on kib_failed_devs */
+	__u32              ibd_ifip;            /* IPoIB interface IP */
+
+	/* IPoIB interface name */
+	char               ibd_ifname[KIB_IFNAME_SIZE];
+	int                ibd_nnets;           /* # nets extant */
+
+	unsigned long      ibd_next_failover;
+	int                ibd_failed_failover; /* # failover failures */
+	unsigned int       ibd_failover;        /* failover in progress */
+	unsigned int       ibd_can_failover;    /* IPoIB interface is a bonding
+						 * master */
+	struct list_head   ibd_nets;
+	struct kib_hca_dev *ibd_hdev;
 } kib_dev_t;
 
 typedef struct kib_hca_dev {
-	struct rdma_cm_id   *ibh_cmid;	  /* listener cmid */
-	struct ib_device    *ibh_ibdev;	 /* IB device */
-	int		  ibh_page_shift;    /* page shift of current HCA */
-	int		  ibh_page_size;     /* page size of current HCA */
-	__u64		ibh_page_mask;     /* page mask of current HCA */
-	int		  ibh_mr_shift;      /* bits shift of max MR size */
-	__u64		ibh_mr_size;       /* size of MR */
-	int		  ibh_nmrs;	  /* # of global MRs */
-	struct ib_mr       **ibh_mrs;	   /* global MR */
-	struct ib_pd	*ibh_pd;	    /* PD */
-	kib_dev_t	   *ibh_dev;	   /* owner */
-	atomic_t	 ibh_ref;	   /* refcount */
+	struct rdma_cm_id  *ibh_cmid;           /* listener cmid */
+	struct ib_device   *ibh_ibdev;          /* IB device */
+	int                ibh_page_shift;      /* page shift of current HCA */
+	int                ibh_page_size;       /* page size of current HCA */
+	__u64              ibh_page_mask;       /* page mask of current HCA */
+	int                ibh_mr_shift;        /* bits shift of max MR size */
+	__u64              ibh_mr_size;         /* size of MR */
+	int                ibh_nmrs;            /* # of global MRs */
+	struct ib_mr       **ibh_mrs;           /* global MR */
+	struct ib_pd       *ibh_pd;             /* PD */
+	kib_dev_t          *ibh_dev;            /* owner */
+	atomic_t           ibh_ref;             /* refcount */
 } kib_hca_dev_t;
 
 /** # of seconds to keep pool alive */
@@ -222,19 +228,19 @@ typedef struct kib_hca_dev {
 #define IBLND_POOL_RETRY	1
 
 typedef struct {
-	int		     ibp_npages;	     /* # pages */
-	struct page	    *ibp_pages[0];	   /* page array */
+	int                ibp_npages;          /* # pages */
+	struct page        *ibp_pages[0];       /* page array */
 } kib_pages_t;
 
 struct kib_pmr_pool;
 
 typedef struct {
-	struct list_head	      pmr_list;	       /* chain node */
-	struct ib_phys_buf     *pmr_ipb;		/* physical buffer */
-	struct ib_mr	   *pmr_mr;		 /* IB MR */
-	struct kib_pmr_pool    *pmr_pool;	       /* owner of this MR */
-	__u64		   pmr_iova;	       /* Virtual I/O address */
-	int		     pmr_refcount;	   /* reference count */
+	struct list_head    pmr_list;           /* chain node */
+	struct ib_phys_buf  *pmr_ipb;           /* physical buffer */
+	struct ib_mr        *pmr_mr;            /* IB MR */
+	struct kib_pmr_pool *pmr_pool;          /* owner of this MR */
+	__u64               pmr_iova;           /* Virtual I/O address */
+	int                 pmr_refcount;       /* reference count */
 } kib_phys_mr_t;
 
 struct kib_pool;
@@ -251,97 +257,99 @@ struct kib_net;
 #define IBLND_POOL_NAME_LEN     32
 
 typedef struct kib_poolset {
-	spinlock_t		ps_lock;		/* serialize */
-	struct kib_net	 *ps_net;		 /* network it belongs to */
-	char		    ps_name[IBLND_POOL_NAME_LEN]; /* pool set name */
-	struct list_head	      ps_pool_list;	   /* list of pools */
-	struct list_head	      ps_failed_pool_list;    /* failed pool list */
-	unsigned long	      ps_next_retry;	  /* time stamp for retry if failed to allocate */
-	int		     ps_increasing;	  /* is allocating new pool */
-	int		     ps_pool_size;	   /* new pool size */
-	int			ps_cpt;			/* CPT id */
-
-	kib_ps_pool_create_t    ps_pool_create;	 /* create a new pool */
-	kib_ps_pool_destroy_t   ps_pool_destroy;	/* destroy a pool */
-	kib_ps_node_init_t      ps_node_init;	   /* initialize new allocated node */
-	kib_ps_node_fini_t      ps_node_fini;	   /* finalize node */
+	spinlock_t            ps_lock;            /* serialize */
+	struct kib_net        *ps_net;            /* network it belongs to */
+	char                  ps_name[IBLND_POOL_NAME_LEN]; /* pool set name */
+	struct list_head      ps_pool_list;       /* list of pools */
+	struct list_head      ps_failed_pool_list;/* failed pool list */
+	unsigned long         ps_next_retry;      /* time stamp for retry if
+						   * failed to allocate */
+	int                   ps_increasing;      /* is allocating new pool */
+	int                   ps_pool_size;       /* new pool size */
+	int                   ps_cpt;             /* CPT id */
+
+	kib_ps_pool_create_t  ps_pool_create;     /* create a new pool */
+	kib_ps_pool_destroy_t ps_pool_destroy;    /* destroy a pool */
+	kib_ps_node_init_t    ps_node_init;       /* initialize new allocated
+						   * node */
+	kib_ps_node_fini_t    ps_node_fini;       /* finalize node */
 } kib_poolset_t;
 
 typedef struct kib_pool {
-	struct list_head	      po_list;		/* chain on pool list */
-	struct list_head	      po_free_list;	   /* pre-allocated node */
-	kib_poolset_t	  *po_owner;	       /* pool_set of this pool */
-	unsigned long	      po_deadline;	    /* deadline of this pool */
-	int		     po_allocated;	   /* # of elements in use */
-	int		     po_failed;	      /* pool is created on failed HCA */
-	int		     po_size;		/* # of pre-allocated elements */
+	struct list_head      po_list;         /* chain on pool list */
+	struct list_head      po_free_list;    /* pre-allocated node */
+	kib_poolset_t         *po_owner;       /* pool_set of this pool */
+	unsigned long         po_deadline;     /* deadline of this pool */
+	int                   po_allocated;    /* # of elements in use */
+	int                   po_failed;       /* pool is created on failed
+						* HCA */
+	int                   po_size;         /* # of pre-allocated elements */
 } kib_pool_t;
 
 typedef struct {
-	kib_poolset_t	   tps_poolset;	    /* pool-set */
-	__u64		   tps_next_tx_cookie;     /* cookie of TX */
+	kib_poolset_t         tps_poolset;        /* pool-set */
+	__u64                 tps_next_tx_cookie; /* cookie of TX */
 } kib_tx_poolset_t;
 
 typedef struct {
-	kib_pool_t	      tpo_pool;	       /* pool */
-	struct kib_hca_dev     *tpo_hdev;	       /* device for this pool */
-	struct kib_tx	  *tpo_tx_descs;	   /* all the tx descriptors */
-	kib_pages_t	    *tpo_tx_pages;	   /* premapped tx msg pages */
+	kib_pool_t            tpo_pool;           /* pool */
+	struct kib_hca_dev    *tpo_hdev;          /* device for this pool */
+	struct kib_tx         *tpo_tx_descs;      /* all the tx descriptors */
+	kib_pages_t           *tpo_tx_pages;      /* premapped tx msg pages */
 } kib_tx_pool_t;
 
 typedef struct {
-	kib_poolset_t	   pps_poolset;	    /* pool-set */
+	kib_poolset_t         pps_poolset;        /* pool-set */
 } kib_pmr_poolset_t;
 
 typedef struct kib_pmr_pool {
-	struct kib_hca_dev     *ppo_hdev;	       /* device for this pool */
-	kib_pool_t	      ppo_pool;	       /* pool */
+	struct kib_hca_dev    *ppo_hdev;          /* device for this pool */
+	kib_pool_t            ppo_pool;           /* pool */
 } kib_pmr_pool_t;
 
 typedef struct {
-	spinlock_t		fps_lock;		/* serialize */
-	struct kib_net	 *fps_net;		/* IB network */
-	struct list_head	      fps_pool_list;	  /* FMR pool list */
-	struct list_head	      fps_failed_pool_list;   /* FMR pool list */
-	__u64		   fps_version;	    /* validity stamp */
-	int			fps_cpt;		/* CPT id */
-	int			fps_pool_size;
-	int			fps_flush_trigger;
-	/* is allocating new pool */
-	int			fps_increasing;
-	/* time stamp for retry if failed to allocate */
-	unsigned long		fps_next_retry;
+	spinlock_t            fps_lock;            /* serialize */
+	struct kib_net        *fps_net;            /* IB network */
+	struct list_head      fps_pool_list;       /* FMR pool list */
+	struct list_head      fps_failed_pool_list;/* FMR pool list */
+	__u64                 fps_version;         /* validity stamp */
+	int                   fps_cpt;             /* CPT id */
+	int                   fps_pool_size;
+	int                   fps_flush_trigger;
+	int                   fps_increasing;      /* is allocating new pool */
+	unsigned long         fps_next_retry;      /* time stamp for retry if
+						    * failed to allocate */
 } kib_fmr_poolset_t;
 
 typedef struct {
-	struct list_head	      fpo_list;	       /* chain on pool list */
-	struct kib_hca_dev     *fpo_hdev;	       /* device for this pool */
-	kib_fmr_poolset_t      *fpo_owner;	      /* owner of this pool */
-	struct ib_fmr_pool     *fpo_fmr_pool;	   /* IB FMR pool */
-	unsigned long	      fpo_deadline;	   /* deadline of this pool */
-	int		     fpo_failed;	     /* fmr pool is failed */
-	int		     fpo_map_count;	  /* # of mapped FMR */
+	struct list_head      fpo_list;            /* chain on pool list */
+	struct kib_hca_dev    *fpo_hdev;           /* device for this pool */
+	kib_fmr_poolset_t     *fpo_owner;          /* owner of this pool */
+	struct ib_fmr_pool    *fpo_fmr_pool;       /* IB FMR pool */
+	unsigned long         fpo_deadline;        /* deadline of this pool */
+	int                   fpo_failed;          /* fmr pool is failed */
+	int                   fpo_map_count;       /* # of mapped FMR */
 } kib_fmr_pool_t;
 
 typedef struct {
-	struct ib_pool_fmr     *fmr_pfmr;	       /* IB pool fmr */
-	kib_fmr_pool_t	 *fmr_pool;	       /* pool of FMR */
+	struct ib_pool_fmr    *fmr_pfmr;           /* IB pool fmr */
+	kib_fmr_pool_t        *fmr_pool;           /* pool of FMR */
 } kib_fmr_t;
 
 typedef struct kib_net {
-	struct list_head	   ibn_list;	  /* chain on kib_dev_t::ibd_nets */
-	__u64		ibn_incarnation;   /* my epoch */
-	int		  ibn_init;	  /* initialisation state */
-	int		  ibn_shutdown;      /* shutting down? */
+	struct list_head      ibn_list;       /* chain on kib_dev_t::ibd_nets */
+	__u64                 ibn_incarnation;/* my epoch */
+	int                   ibn_init;       /* initialisation state */
+	int                   ibn_shutdown;   /* shutting down? */
 
-	atomic_t		ibn_npeers;	/* # peers extant */
-	atomic_t		ibn_nconns;	/* # connections extant */
+	atomic_t              ibn_npeers;     /* # peers extant */
+	atomic_t              ibn_nconns;     /* # connections extant */
 
-	kib_tx_poolset_t	**ibn_tx_ps;	/* tx pool-set */
-	kib_fmr_poolset_t	**ibn_fmr_ps;	/* fmr pool-set */
-	kib_pmr_poolset_t	**ibn_pmr_ps;	/* pmr pool-set */
+	kib_tx_poolset_t      **ibn_tx_ps;    /* tx pool-set */
+	kib_fmr_poolset_t     **ibn_fmr_ps;   /* fmr pool-set */
+	kib_pmr_poolset_t     **ibn_pmr_ps;   /* pmr pool-set */
 
-	kib_dev_t		*ibn_dev;	/* underlying IB device */
+	kib_dev_t             *ibn_dev;       /* underlying IB device */
 } kib_net_t;
 
 #define KIB_THREAD_SHIFT		16
@@ -350,51 +358,45 @@ typedef struct kib_net {
 #define KIB_THREAD_TID(id)		((id) & ((1UL << KIB_THREAD_SHIFT) - 1))
 
 struct kib_sched_info {
-	/* serialise */
-	spinlock_t		ibs_lock;
-	/* schedulers sleep here */
-	wait_queue_head_t		ibs_waitq;
-	/* conns to check for rx completions */
-	struct list_head		ibs_conns;
-	/* number of scheduler threads */
-	int			ibs_nthreads;
-	/* max allowed scheduler threads */
-	int			ibs_nthreads_max;
-	int			ibs_cpt;	/* CPT id */
+	spinlock_t         ibs_lock;     /* serialise */
+	wait_queue_head_t  ibs_waitq;    /* schedulers sleep here */
+	struct list_head   ibs_conns;    /* conns to check for rx completions */
+	int                ibs_nthreads; /* number of scheduler threads */
+	int                ibs_nthreads_max; /* max allowed scheduler threads */
+	int                ibs_cpt;      /* CPT id */
 };
 
 typedef struct {
-	int			kib_init;	/* initialisation state */
-	int			kib_shutdown;	/* shut down? */
-	struct list_head		kib_devs;	/* IB devices extant */
-	/* list head of failed devices */
-	struct list_head		kib_failed_devs;
-	/* schedulers sleep here */
-	wait_queue_head_t		kib_failover_waitq;
-	atomic_t		kib_nthreads;	/* # live threads */
-	/* stabilize net/dev/peer/conn ops */
-	rwlock_t		kib_global_lock;
-	/* hash table of all my known peers */
-	struct list_head		*kib_peers;
-	/* size of kib_peers */
-	int			kib_peer_hash_size;
-	/* the connd task (serialisation assertions) */
-	void			*kib_connd;
-	/* connections to setup/teardown */
-	struct list_head		kib_connd_conns;
-	/* connections with zero refcount */
-	struct list_head		kib_connd_zombies;
-	/* connection daemon sleeps here */
-	wait_queue_head_t		kib_connd_waitq;
-	spinlock_t		kib_connd_lock;	/* serialise */
-	struct ib_qp_attr	kib_error_qpa;	/* QP->ERROR */
-	/* percpt data for schedulers */
-	struct kib_sched_info	**kib_scheds;
+	int                   kib_init;           /* initialisation state */
+	int                   kib_shutdown;       /* shut down? */
+	struct list_head      kib_devs;           /* IB devices extant */
+	struct list_head      kib_failed_devs;    /* list head of failed
+						   * devices */
+	wait_queue_head_t     kib_failover_waitq; /* schedulers sleep here */
+	atomic_t              kib_nthreads;       /* # live threads */
+	rwlock_t              kib_global_lock;    /* stabilize net/dev/peer/conn
+						   * ops */
+	struct list_head      *kib_peers;         /* hash table of all my known
+						   * peers */
+	int                   kib_peer_hash_size; /* size of kib_peers */
+	void                  *kib_connd;         /* the connd task
+						   * (serialisation assertions)
+						   */
+	struct list_head      kib_connd_conns;    /* connections to
+						   * setup/teardown */
+	struct list_head      kib_connd_zombies;  /* connections with zero
+						   * refcount */
+	wait_queue_head_t     kib_connd_waitq;    /* connection daemon sleeps
+						   * here */
+	spinlock_t            kib_connd_lock;     /* serialise */
+	struct ib_qp_attr     kib_error_qpa;      /* QP->ERROR */
+	struct kib_sched_info **kib_scheds;       /* percpt data for schedulers
+						   */
 } kib_data_t;
 
-#define IBLND_INIT_NOTHING	 0
-#define IBLND_INIT_DATA	    1
-#define IBLND_INIT_ALL	     2
+#define IBLND_INIT_NOTHING 0
+#define IBLND_INIT_DATA    1
+#define IBLND_INIT_ALL     2
 
 /************************************************************************
  * IB Wire message format.
@@ -402,228 +404,243 @@ typedef struct {
  */
 
 typedef struct kib_connparams {
-	__u16	     ibcp_queue_depth;
-	__u16	     ibcp_max_frags;
-	__u32	     ibcp_max_msg_size;
+	__u16        ibcp_queue_depth;
+	__u16        ibcp_max_frags;
+	__u32        ibcp_max_msg_size;
 } WIRE_ATTR kib_connparams_t;
 
 typedef struct {
-	lnet_hdr_t	ibim_hdr;	     /* portals header */
-	char	      ibim_payload[0];      /* piggy-backed payload */
+	lnet_hdr_t   ibim_hdr;        /* portals header */
+	char         ibim_payload[0]; /* piggy-backed payload */
 } WIRE_ATTR kib_immediate_msg_t;
 
 typedef struct {
-	__u32	     rf_nob;	       /* # bytes this frag */
-	__u64	     rf_addr;	      /* CAVEAT EMPTOR: misaligned!! */
+	__u32        rf_nob;          /* # bytes this frag */
+	__u64        rf_addr;         /* CAVEAT EMPTOR: misaligned!! */
 } WIRE_ATTR kib_rdma_frag_t;
 
 typedef struct {
-	__u32	     rd_key;	       /* local/remote key */
-	__u32	     rd_nfrags;	    /* # fragments */
-	kib_rdma_frag_t   rd_frags[0];	  /* buffer frags */
+	__u32           rd_key;       /* local/remote key */
+	__u32           rd_nfrags;    /* # fragments */
+	kib_rdma_frag_t rd_frags[0];  /* buffer frags */
 } WIRE_ATTR kib_rdma_desc_t;
 
 typedef struct {
-	lnet_hdr_t	ibprm_hdr;	    /* portals header */
-	__u64	     ibprm_cookie;	 /* opaque completion cookie */
+	lnet_hdr_t      ibprm_hdr;    /* portals header */
+	__u64           ibprm_cookie; /* opaque completion cookie */
 } WIRE_ATTR kib_putreq_msg_t;
 
 typedef struct {
-	__u64	     ibpam_src_cookie;     /* reflected completion cookie */
-	__u64	     ibpam_dst_cookie;     /* opaque completion cookie */
-	kib_rdma_desc_t   ibpam_rd;	     /* sender's sink buffer */
+	__u64           ibpam_src_cookie; /* reflected completion cookie */
+	__u64           ibpam_dst_cookie; /* opaque completion cookie */
+	kib_rdma_desc_t ibpam_rd;         /* sender's sink buffer */
 } WIRE_ATTR kib_putack_msg_t;
 
 typedef struct {
-	lnet_hdr_t	ibgm_hdr;	     /* portals header */
-	__u64	     ibgm_cookie;	  /* opaque completion cookie */
-	kib_rdma_desc_t   ibgm_rd;	      /* rdma descriptor */
+	lnet_hdr_t      ibgm_hdr;     /* portals header */
+	__u64           ibgm_cookie;  /* opaque completion cookie */
+	kib_rdma_desc_t ibgm_rd;      /* rdma descriptor */
 } WIRE_ATTR kib_get_msg_t;
 
 typedef struct {
-	__u64	     ibcm_cookie;	  /* opaque completion cookie */
-	__s32	     ibcm_status;	  /* < 0 failure: >= 0 length */
+	__u64           ibcm_cookie;  /* opaque completion cookie */
+	__s32           ibcm_status;  /* < 0 failure: >= 0 length */
 } WIRE_ATTR kib_completion_msg_t;
 
 typedef struct {
 	/* First 2 fields fixed FOR ALL TIME */
-	__u32	     ibm_magic;	    /* I'm an ibnal message */
-	__u16	     ibm_version;	  /* this is my version number */
-
-	__u8	      ibm_type;	     /* msg type */
-	__u8	      ibm_credits;	  /* returned credits */
-	__u32	     ibm_nob;	      /* # bytes in whole message */
-	__u32	     ibm_cksum;	    /* checksum (0 == no checksum) */
-	__u64	     ibm_srcnid;	   /* sender's NID */
-	__u64	     ibm_srcstamp;	 /* sender's incarnation */
-	__u64	     ibm_dstnid;	   /* destination's NID */
-	__u64	     ibm_dststamp;	 /* destination's incarnation */
+	__u32           ibm_magic;    /* I'm an ibnal message */
+	__u16           ibm_version;  /* this is my version number */
+
+	__u8            ibm_type;     /* msg type */
+	__u8            ibm_credits;  /* returned credits */
+	__u32           ibm_nob;      /* # bytes in whole message */
+	__u32           ibm_cksum;    /* checksum (0 == no checksum) */
+	__u64           ibm_srcnid;   /* sender's NID */
+	__u64           ibm_srcstamp; /* sender's incarnation */
+	__u64           ibm_dstnid;   /* destination's NID */
+	__u64           ibm_dststamp; /* destination's incarnation */
 
 	union {
-		kib_connparams_t      connparams;
-		kib_immediate_msg_t   immediate;
-		kib_putreq_msg_t      putreq;
-		kib_putack_msg_t      putack;
-		kib_get_msg_t	 get;
-		kib_completion_msg_t  completion;
+		kib_connparams_t     connparams;
+		kib_immediate_msg_t  immediate;
+		kib_putreq_msg_t     putreq;
+		kib_putack_msg_t     putack;
+		kib_get_msg_t        get;
+		kib_completion_msg_t completion;
 	} WIRE_ATTR ibm_u;
 } WIRE_ATTR kib_msg_t;
 
-#define IBLND_MSG_MAGIC LNET_PROTO_IB_MAGIC	/* unique magic */
+#define IBLND_MSG_MAGIC     LNET_PROTO_IB_MAGIC /* unique magic */
 
-#define IBLND_MSG_VERSION_1	 0x11
-#define IBLND_MSG_VERSION_2	 0x12
-#define IBLND_MSG_VERSION	   IBLND_MSG_VERSION_2
+#define IBLND_MSG_VERSION_1 0x11
+#define IBLND_MSG_VERSION_2 0x12
+#define IBLND_MSG_VERSION   IBLND_MSG_VERSION_2
 
-#define IBLND_MSG_CONNREQ	   0xc0	/* connection request */
-#define IBLND_MSG_CONNACK	   0xc1	/* connection acknowledge */
-#define IBLND_MSG_NOOP	      0xd0	/* nothing (just credits) */
-#define IBLND_MSG_IMMEDIATE	 0xd1	/* immediate */
-#define IBLND_MSG_PUT_REQ	   0xd2	/* putreq (src->sink) */
-#define IBLND_MSG_PUT_NAK	   0xd3	/* completion (sink->src) */
-#define IBLND_MSG_PUT_ACK	   0xd4	/* putack (sink->src) */
-#define IBLND_MSG_PUT_DONE	  0xd5	/* completion (src->sink) */
-#define IBLND_MSG_GET_REQ	   0xd6	/* getreq (sink->src) */
-#define IBLND_MSG_GET_DONE	  0xd7	/* completion (src->sink: all OK) */
+#define IBLND_MSG_CONNREQ   0xc0	/* connection request */
+#define IBLND_MSG_CONNACK   0xc1	/* connection acknowledge */
+#define IBLND_MSG_NOOP      0xd0	/* nothing (just credits) */
+#define IBLND_MSG_IMMEDIATE 0xd1	/* immediate */
+#define IBLND_MSG_PUT_REQ   0xd2	/* putreq (src->sink) */
+#define IBLND_MSG_PUT_NAK   0xd3	/* completion (sink->src) */
+#define IBLND_MSG_PUT_ACK   0xd4	/* putack (sink->src) */
+#define IBLND_MSG_PUT_DONE  0xd5	/* completion (src->sink) */
+#define IBLND_MSG_GET_REQ   0xd6	/* getreq (sink->src) */
+#define IBLND_MSG_GET_DONE  0xd7	/* completion (src->sink: all OK) */
 
 typedef struct {
-	__u32	    ibr_magic;	     /* sender's magic */
-	__u16	    ibr_version;	   /* sender's version */
-	__u8	     ibr_why;	       /* reject reason */
-	__u8	     ibr_padding;	   /* padding */
-	__u64	    ibr_incarnation;       /* incarnation of peer */
-	kib_connparams_t ibr_cp;		/* connection parameters */
+	__u32            ibr_magic;       /* sender's magic */
+	__u16            ibr_version;     /* sender's version */
+	__u8             ibr_why;         /* reject reason */
+	__u8             ibr_padding;     /* padding */
+	__u64            ibr_incarnation; /* incarnation of peer */
+	kib_connparams_t ibr_cp;          /* connection parameters */
 } WIRE_ATTR kib_rej_t;
 
 /* connection rejection reasons */
-#define IBLND_REJECT_CONN_RACE       1	  /* You lost connection race */
-#define IBLND_REJECT_NO_RESOURCES    2	  /* Out of memory/conns etc */
-#define IBLND_REJECT_FATAL	   3	  /* Anything else */
-
-#define IBLND_REJECT_CONN_UNCOMPAT   4	  /* incompatible version peer */
-#define IBLND_REJECT_CONN_STALE      5	  /* stale peer */
-
-#define IBLND_REJECT_RDMA_FRAGS      6	  /* Fatal: peer's rdma frags can't match mine */
-#define IBLND_REJECT_MSG_QUEUE_SIZE  7	  /* Fatal: peer's msg queue size can't match mine */
+#define IBLND_REJECT_CONN_RACE      1 /* You lost connection race */
+#define IBLND_REJECT_NO_RESOURCES   2 /* Out of memory/conns etc */
+#define IBLND_REJECT_FATAL          3 /* Anything else */
+#define IBLND_REJECT_CONN_UNCOMPAT  4 /* incompatible version peer */
+#define IBLND_REJECT_CONN_STALE     5 /* stale peer */
+#define IBLND_REJECT_RDMA_FRAGS     6 /* Fatal: peer's rdma frags can't match
+				       * mine */
+#define IBLND_REJECT_MSG_QUEUE_SIZE 7 /* Fatal: peer's msg queue size can't
+				       * match mine */
 
 /***********************************************************************/
 
-typedef struct kib_rx			   /* receive message */
+typedef struct kib_rx                         /* receive message */
 {
-	struct list_head		rx_list;      /* queue for attention */
-	struct kib_conn	  *rx_conn;      /* owning conn */
-	int		       rx_nob;       /* # bytes received (-1 while posted) */
-	enum ib_wc_status	 rx_status;    /* completion status */
-	kib_msg_t		*rx_msg;       /* message buffer (host vaddr) */
-	__u64		     rx_msgaddr;   /* message buffer (I/O addr) */
-	DECLARE_PCI_UNMAP_ADDR   (rx_msgunmap); /* for dma_unmap_single() */
-	struct ib_recv_wr	 rx_wrq;       /* receive work item... */
-	struct ib_sge	     rx_sge;       /* ...and its memory */
+	struct list_head       rx_list;       /* queue for attention */
+	struct kib_conn        *rx_conn;      /* owning conn */
+	int                    rx_nob;        /* # bytes received (-1 while
+					       * posted) */
+	enum ib_wc_status      rx_status;     /* completion status */
+	kib_msg_t              *rx_msg;       /* message buffer (host vaddr) */
+	__u64                  rx_msgaddr;    /* message buffer (I/O addr) */
+	DECLARE_PCI_UNMAP_ADDR (rx_msgunmap); /* for dma_unmap_single() */
+	struct ib_recv_wr      rx_wrq;        /* receive work item... */
+	struct ib_sge          rx_sge;        /* ...and its memory */
 } kib_rx_t;
 
-#define IBLND_POSTRX_DONT_POST    0	     /* don't post */
-#define IBLND_POSTRX_NO_CREDIT    1	     /* post: no credits */
-#define IBLND_POSTRX_PEER_CREDIT  2	     /* post: give peer back 1 credit */
-#define IBLND_POSTRX_RSRVD_CREDIT 3	     /* post: give myself back 1 reserved credit */
+#define IBLND_POSTRX_DONT_POST    0   /* don't post */
+#define IBLND_POSTRX_NO_CREDIT    1   /* post: no credits */
+#define IBLND_POSTRX_PEER_CREDIT  2   /* post: give peer back 1 credit */
+#define IBLND_POSTRX_RSRVD_CREDIT 3   /* post: give myself back 1 reserved
+				       * credit */
 
-typedef struct kib_tx			   /* transmit message */
+typedef struct kib_tx                         /* transmit message */
 {
-	struct list_head		tx_list;      /* queue on idle_txs ibc_tx_queue etc. */
-	kib_tx_pool_t	    *tx_pool;      /* pool I'm from */
-	struct kib_conn	  *tx_conn;      /* owning conn */
-	short		     tx_sending;   /* # tx callbacks outstanding */
-	short		     tx_queued;    /* queued for sending */
-	short		     tx_waiting;   /* waiting for peer */
-	int		       tx_status;    /* LNET completion status */
-	unsigned long	     tx_deadline;  /* completion deadline */
-	__u64		     tx_cookie;    /* completion cookie */
-	lnet_msg_t	       *tx_lntmsg[2]; /* lnet msgs to finalize on completion */
-	kib_msg_t		*tx_msg;       /* message buffer (host vaddr) */
-	__u64		     tx_msgaddr;   /* message buffer (I/O addr) */
-	DECLARE_PCI_UNMAP_ADDR   (tx_msgunmap); /* for dma_unmap_single() */
-	int		       tx_nwrq;      /* # send work items */
-	struct ib_send_wr	*tx_wrq;       /* send work items... */
-	struct ib_sge	    *tx_sge;       /* ...and their memory */
-	kib_rdma_desc_t	  *tx_rd;	/* rdma descriptor */
-	int		       tx_nfrags;    /* # entries in... */
-	struct scatterlist       *tx_frags;     /* dma_map_sg descriptor */
-	__u64		    *tx_pages;     /* rdma phys page addrs */
+	struct list_head       tx_list;       /* queue on idle_txs ibc_tx_queue
+					       * etc. */
+	kib_tx_pool_t          *tx_pool;      /* pool I'm from */
+	struct kib_conn        *tx_conn;      /* owning conn */
+	short                  tx_sending;    /* # tx callbacks outstanding */
+	short                  tx_queued;     /* queued for sending */
+	short                  tx_waiting;    /* waiting for peer */
+	int                    tx_status;     /* LNET completion status */
+	unsigned long          tx_deadline;   /* completion deadline */
+	__u64                  tx_cookie;     /* completion cookie */
+	lnet_msg_t             *tx_lntmsg[2]; /* lnet msgs to finalize on
+					       * completion */
+	kib_msg_t              *tx_msg;       /* message buffer (host vaddr) */
+	__u64                  tx_msgaddr;    /* message buffer (I/O addr) */
+	DECLARE_PCI_UNMAP_ADDR (tx_msgunmap); /* for dma_unmap_single() */
+	int                    tx_nwrq;       /* # send work items */
+	struct ib_send_wr      *tx_wrq;       /* send work items... */
+	struct ib_sge          *tx_sge;       /* ...and their memory */
+	kib_rdma_desc_t        *tx_rd;        /* rdma descriptor */
+	int                    tx_nfrags;     /* # entries in... */
+	struct scatterlist     *tx_frags;     /* dma_map_sg descriptor */
+	__u64                  *tx_pages;     /* rdma phys page addrs */
 	union {
-		kib_phys_mr_t      *pmr;	/* MR for physical buffer */
-		kib_fmr_t	   fmr;	/* FMR */
-	}			 tx_u;
-	int		       tx_dmadir;    /* dma direction */
+		kib_phys_mr_t  *pmr;          /* MR for physical buffer */
+		kib_fmr_t      fmr;           /* FMR */
+	}                      tx_u;
+	int                    tx_dmadir;     /* dma direction */
 } kib_tx_t;
 
 typedef struct kib_connvars {
-	/* connection-in-progress variables */
-	kib_msg_t		 cv_msg;
+	kib_msg_t cv_msg; /* connection-in-progress variables */
 } kib_connvars_t;
 
 typedef struct kib_conn {
-	struct kib_sched_info *ibc_sched;	/* scheduler information */
-	struct kib_peer     *ibc_peer;	  /* owning peer */
-	kib_hca_dev_t       *ibc_hdev;	  /* HCA bound on */
-	struct list_head	   ibc_list;	  /* stash on peer's conn list */
-	struct list_head	   ibc_sched_list;    /* schedule for attention */
-	__u16		ibc_version;       /* version of connection */
-	__u64		ibc_incarnation;   /* which instance of the peer */
-	atomic_t	 ibc_refcount;      /* # users */
-	int		  ibc_state;	 /* what's happening */
-	int		  ibc_nsends_posted; /* # uncompleted sends */
-	int		  ibc_noops_posted;  /* # uncompleted NOOPs */
-	int		  ibc_credits;       /* # credits I have */
-	int		  ibc_outstanding_credits; /* # credits to return */
-	int		  ibc_reserved_credits;/* # ACK/DONE msg credits */
-	int		  ibc_comms_error;   /* set on comms error */
-	unsigned int	     ibc_nrx:16;	/* receive buffers owned */
-	unsigned int	     ibc_scheduled:1;   /* scheduled for attention */
-	unsigned int	     ibc_ready:1;       /* CQ callback fired */
-	/* time of last send */
-	unsigned long	ibc_last_send;
-	/** link chain for kiblnd_check_conns only */
-	struct list_head	   ibc_connd_list;
-	/** rxs completed before ESTABLISHED */
-	struct list_head	   ibc_early_rxs;
-	/** IBLND_MSG_NOOPs for IBLND_MSG_VERSION_1 */
-	struct list_head	   ibc_tx_noops;
-	struct list_head	   ibc_tx_queue;       /* sends that need a credit */
-	struct list_head	   ibc_tx_queue_nocred;/* sends that don't need a credit */
-	struct list_head	   ibc_tx_queue_rsrvd; /* sends that need to reserve an ACK/DONE msg */
-	struct list_head	   ibc_active_txs;     /* active tx awaiting completion */
-	spinlock_t	     ibc_lock;		 /* serialise */
-	kib_rx_t	    *ibc_rxs;	    /* the rx descs */
-	kib_pages_t	 *ibc_rx_pages;       /* premapped rx msg pages */
-
-	struct rdma_cm_id   *ibc_cmid;	   /* CM id */
-	struct ib_cq	*ibc_cq;	     /* completion queue */
-
-	kib_connvars_t      *ibc_connvars;       /* in-progress connection state */
+	struct kib_sched_info *ibc_sched;           /* scheduler information */
+	struct kib_peer       *ibc_peer;            /* owning peer */
+	kib_hca_dev_t         *ibc_hdev;            /* HCA bound on */
+	struct list_head      ibc_list;             /* stash on peer's conn
+						     * list */
+	struct list_head      ibc_sched_list;       /* schedule for attention */
+	__u16                 ibc_version;          /* version of connection */
+	__u64                 ibc_incarnation;      /* which instance of the
+						     * peer */
+	atomic_t              ibc_refcount;         /* # users */
+	int                   ibc_state;            /* what's happening */
+	int                   ibc_nsends_posted;    /* # uncompleted sends */
+	int                   ibc_noops_posted;     /* # uncompleted NOOPs */
+	int                   ibc_credits;          /* # credits I have */
+	int                   ibc_outstanding_credits; /* # credits to return */
+	int                   ibc_reserved_credits; /* # ACK/DONE msg credits */
+	int                   ibc_comms_error;      /* set on comms error */
+	unsigned int          ibc_nrx:16;           /* receive buffers owned */
+	unsigned int          ibc_scheduled:1;      /* scheduled for attention
+						     */
+	unsigned int          ibc_ready:1;          /* CQ callback fired */
+	unsigned long         ibc_last_send;        /* time of last send */
+	struct list_head      ibc_connd_list;       /* link chain for
+						     * kiblnd_check_conns only
+						     */
+	struct list_head      ibc_early_rxs;        /* rxs completed before
+						     * ESTABLISHED */
+	struct list_head      ibc_tx_noops;         /* IBLND_MSG_NOOPs for
+						     * IBLND_MSG_VERSION_1 */
+	struct list_head      ibc_tx_queue;         /* sends that need a credit
+						     */
+	struct list_head      ibc_tx_queue_nocred;  /* sends that don't need a
+						     * credit */
+	struct list_head      ibc_tx_queue_rsrvd;   /* sends that need to
+						     * reserve an ACK/DONE msg
+						     */
+	struct list_head      ibc_active_txs;       /* active tx awaiting
+						     * completion */
+	spinlock_t            ibc_lock;             /* serialise */
+	kib_rx_t              *ibc_rxs;             /* the rx descs */
+	kib_pages_t           *ibc_rx_pages;        /* premapped rx msg pages */
+
+	struct rdma_cm_id     *ibc_cmid;            /* CM id */
+	struct ib_cq          *ibc_cq;              /* completion queue */
+
+	kib_connvars_t        *ibc_connvars;        /* in-progress connection
+						     * state */
 } kib_conn_t;
 
-#define IBLND_CONN_INIT	       0	 /* being initialised */
-#define IBLND_CONN_ACTIVE_CONNECT     1	 /* active sending req */
-#define IBLND_CONN_PASSIVE_WAIT       2	 /* passive waiting for rtu */
-#define IBLND_CONN_ESTABLISHED	3	 /* connection established */
-#define IBLND_CONN_CLOSING	    4	 /* being closed */
-#define IBLND_CONN_DISCONNECTED       5	 /* disconnected */
+#define IBLND_CONN_INIT           0	 /* being initialised */
+#define IBLND_CONN_ACTIVE_CONNECT 1	 /* active sending req */
+#define IBLND_CONN_PASSIVE_WAIT   2	 /* passive waiting for rtu */
+#define IBLND_CONN_ESTABLISHED    3	 /* connection established */
+#define IBLND_CONN_CLOSING        4	 /* being closed */
+#define IBLND_CONN_DISCONNECTED   5	 /* disconnected */
 
 typedef struct kib_peer {
-	struct list_head	   ibp_list;	   /* stash on global peer list */
-	lnet_nid_t	   ibp_nid;	    /* who's on the other end(s) */
-	lnet_ni_t	   *ibp_ni;	     /* LNet interface */
-	atomic_t	 ibp_refcount;       /* # users */
-	struct list_head	   ibp_conns;	  /* all active connections */
-	struct list_head	   ibp_tx_queue;       /* msgs waiting for a conn */
-	__u16		ibp_version;	/* version of peer */
-	__u64		ibp_incarnation;    /* incarnation of peer */
-	int		  ibp_connecting;     /* current active connection attempts */
-	int		  ibp_accepting;      /* current passive connection attempts */
-	int		  ibp_error;	  /* errno on closing this peer */
-	unsigned long	   ibp_last_alive;     /* when (in jiffies) I was last alive */
+	struct list_head ibp_list;        /* stash on global peer list */
+	lnet_nid_t       ibp_nid;         /* who's on the other end(s) */
+	lnet_ni_t        *ibp_ni;         /* LNet interface */
+	atomic_t         ibp_refcount;    /* # users */
+	struct list_head ibp_conns;       /* all active connections */
+	struct list_head ibp_tx_queue;    /* msgs waiting for a conn */
+	__u16            ibp_version;     /* version of peer */
+	__u64            ibp_incarnation; /* incarnation of peer */
+	int              ibp_connecting;  /* current active connection attempts
+					   */
+	int              ibp_accepting;   /* current passive connection attempts
+					   */
+	int              ibp_error;       /* errno on closing this peer */
+	unsigned long    ibp_last_alive;  /* when (in jiffies) I was last alive
+					   */
 } kib_peer_t;
 
-extern kib_data_t      kiblnd_data;
+extern kib_data_t kiblnd_data;
 
 extern void kiblnd_hdev_destroy(kib_hca_dev_t *hdev);
 
@@ -941,8 +958,8 @@ static inline unsigned int kiblnd_sg_dma_len(struct ib_device *dev,
  * right because OFED1.2 defines it as const, to use it we have to add
  * (void *) cast to overcome "const" */
 
-#define KIBLND_CONN_PARAM(e)	    ((e)->param.conn.private_data)
-#define KIBLND_CONN_PARAM_LEN(e)	((e)->param.conn.private_data_len)
+#define KIBLND_CONN_PARAM(e)     ((e)->param.conn.private_data)
+#define KIBLND_CONN_PARAM_LEN(e) ((e)->param.conn.private_data_len)
 
 
 struct ib_mr *kiblnd_find_rd_dma_mr(kib_hca_dev_t *hdev,
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index dbf3749831f9..477aa8b76f32 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -44,9 +44,9 @@ static void
 kiblnd_tx_done(lnet_ni_t *ni, kib_tx_t *tx)
 {
 	lnet_msg_t *lntmsg[2];
-	kib_net_t  *net = ni->ni_data;
-	int	 rc;
-	int	 i;
+	kib_net_t *net = ni->ni_data;
+	int rc;
+	int i;
 
 	LASSERT(net != NULL);
 	LASSERT(!in_interrupt());
@@ -102,10 +102,10 @@ kiblnd_txlist_done(lnet_ni_t *ni, struct list_head *txlist, int status)
 static kib_tx_t *
 kiblnd_get_idle_tx(lnet_ni_t *ni, lnet_nid_t target)
 {
-	kib_net_t		*net = (kib_net_t *)ni->ni_data;
-	struct list_head		*node;
-	kib_tx_t		*tx;
-	kib_tx_poolset_t	*tps;
+	kib_net_t *net = (kib_net_t *)ni->ni_data;
+	struct list_head *node;
+	kib_tx_t *tx;
+	kib_tx_poolset_t *tps;
 
 	tps = net->ibn_tx_ps[lnet_cpt_of_nid(target)];
 	node = kiblnd_pool_alloc_node(&tps->tps_poolset);
@@ -130,9 +130,9 @@ kiblnd_get_idle_tx(lnet_ni_t *ni, lnet_nid_t target)
 static void
 kiblnd_drop_rx(kib_rx_t *rx)
 {
-	kib_conn_t		*conn	= rx->rx_conn;
-	struct kib_sched_info	*sched	= conn->ibc_sched;
-	unsigned long		flags;
+	kib_conn_t *conn = rx->rx_conn;
+	struct kib_sched_info *sched = conn->ibc_sched;
+	unsigned long flags;
 
 	spin_lock_irqsave(&sched->ibs_lock, flags);
 	LASSERT(conn->ibc_nrx > 0);
@@ -145,11 +145,11 @@ kiblnd_drop_rx(kib_rx_t *rx)
 int
 kiblnd_post_rx(kib_rx_t *rx, int credit)
 {
-	kib_conn_t	 *conn = rx->rx_conn;
-	kib_net_t	  *net = conn->ibc_peer->ibp_ni->ni_data;
-	struct ib_recv_wr  *bad_wrq = NULL;
-	struct ib_mr       *mr;
-	int		 rc;
+	kib_conn_t *conn = rx->rx_conn;
+	kib_net_t *net = conn->ibc_peer->ibp_ni->ni_data;
+	struct ib_recv_wr *bad_wrq = NULL;
+	struct ib_mr *mr;
+	int rc;
 
 	LASSERT(net != NULL);
 	LASSERT(!in_interrupt());
@@ -164,10 +164,10 @@ kiblnd_post_rx(kib_rx_t *rx, int credit)
 	rx->rx_sge.addr   = rx->rx_msgaddr;
 	rx->rx_sge.length = IBLND_MSG_SIZE;
 
-	rx->rx_wrq.next = NULL;
+	rx->rx_wrq.next    = NULL;
 	rx->rx_wrq.sg_list = &rx->rx_sge;
 	rx->rx_wrq.num_sge = 1;
-	rx->rx_wrq.wr_id = kiblnd_ptr2wreqid(rx, IBLND_WID_RX);
+	rx->rx_wrq.wr_id   = kiblnd_ptr2wreqid(rx, IBLND_WID_RX);
 
 	LASSERT(conn->ibc_state >= IBLND_CONN_INIT);
 	LASSERT(rx->rx_nob >= 0);	      /* not posted */
@@ -212,7 +212,7 @@ kiblnd_post_rx(kib_rx_t *rx, int credit)
 static kib_tx_t *
 kiblnd_find_waiting_tx_locked(kib_conn_t *conn, int txtype, __u64 cookie)
 {
-	struct list_head   *tmp;
+	struct list_head *tmp;
 
 	list_for_each(tmp, &conn->ibc_active_txs) {
 		kib_tx_t *tx = list_entry(tmp, kib_tx_t, tx_list);
@@ -237,9 +237,9 @@ kiblnd_find_waiting_tx_locked(kib_conn_t *conn, int txtype, __u64 cookie)
 static void
 kiblnd_handle_completion(kib_conn_t *conn, int txtype, int status, __u64 cookie)
 {
-	kib_tx_t    *tx;
-	lnet_ni_t   *ni = conn->ibc_peer->ibp_ni;
-	int	  idle;
+	kib_tx_t *tx;
+	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	int idle;
 
 	spin_lock(&conn->ibc_lock);
 
@@ -276,8 +276,8 @@ kiblnd_handle_completion(kib_conn_t *conn, int txtype, int status, __u64 cookie)
 static void
 kiblnd_send_completion(kib_conn_t *conn, int type, int status, __u64 cookie)
 {
-	lnet_ni_t   *ni = conn->ibc_peer->ibp_ni;
-	kib_tx_t    *tx = kiblnd_get_idle_tx(ni, conn->ibc_peer->ibp_nid);
+	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	kib_tx_t *tx = kiblnd_get_idle_tx(ni, conn->ibc_peer->ibp_nid);
 
 	if (tx == NULL) {
 		CERROR("Can't get tx for completion %x for %s\n",
@@ -295,14 +295,14 @@ kiblnd_send_completion(kib_conn_t *conn, int type, int status, __u64 cookie)
 static void
 kiblnd_handle_rx(kib_rx_t *rx)
 {
-	kib_msg_t    *msg = rx->rx_msg;
-	kib_conn_t   *conn = rx->rx_conn;
-	lnet_ni_t    *ni = conn->ibc_peer->ibp_ni;
-	int	   credits = msg->ibm_credits;
-	kib_tx_t     *tx;
-	int	   rc = 0;
-	int	   rc2;
-	int	   post_credit;
+	kib_msg_t *msg = rx->rx_msg;
+	kib_conn_t *conn = rx->rx_conn;
+	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	int credits = msg->ibm_credits;
+	kib_tx_t *tx;
+	int rc = 0;
+	int rc2;
+	int post_credit;
 
 	LASSERT(conn->ibc_state >= IBLND_CONN_ESTABLISHED);
 
@@ -456,12 +456,12 @@ kiblnd_handle_rx(kib_rx_t *rx)
 static void
 kiblnd_rx_complete(kib_rx_t *rx, int status, int nob)
 {
-	kib_msg_t    *msg = rx->rx_msg;
-	kib_conn_t   *conn = rx->rx_conn;
-	lnet_ni_t    *ni = conn->ibc_peer->ibp_ni;
-	kib_net_t    *net = ni->ni_data;
-	int	   rc;
-	int	   err = -EIO;
+	kib_msg_t *msg = rx->rx_msg;
+	kib_conn_t *conn = rx->rx_conn;
+	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	kib_net_t *net = ni->ni_data;
+	int rc;
+	int err = -EIO;
 
 	LASSERT(net != NULL);
 	LASSERT(rx->rx_nob < 0);	       /* was posted */
@@ -502,8 +502,8 @@ kiblnd_rx_complete(kib_rx_t *rx, int status, int nob)
 	/* racing with connection establishment/teardown! */
 
 	if (conn->ibc_state < IBLND_CONN_ESTABLISHED) {
-		rwlock_t  *g_lock = &kiblnd_data.kib_global_lock;
-		unsigned long  flags;
+		rwlock_t *g_lock = &kiblnd_data.kib_global_lock;
+		unsigned long flags;
 
 		write_lock_irqsave(g_lock, flags);
 		/* must check holding global lock to eliminate race */
@@ -550,19 +550,19 @@ kiblnd_kvaddr_to_page(unsigned long vaddr)
 static int
 kiblnd_fmr_map_tx(kib_net_t *net, kib_tx_t *tx, kib_rdma_desc_t *rd, int nob)
 {
-	kib_hca_dev_t		*hdev;
-	__u64			*pages = tx->tx_pages;
-	kib_fmr_poolset_t	*fps;
-	int			npages;
-	int			size;
-	int			cpt;
-	int			rc;
-	int			i;
+	kib_hca_dev_t *hdev;
+	__u64 *pages = tx->tx_pages;
+	kib_fmr_poolset_t *fps;
+	int npages;
+	int size;
+	int cpt;
+	int rc;
+	int i;
 
 	LASSERT(tx->tx_pool != NULL);
 	LASSERT(tx->tx_pool->tpo_pool.po_owner != NULL);
 
-	hdev  = tx->tx_pool->tpo_hdev;
+	hdev = tx->tx_pool->tpo_hdev;
 
 	for (i = 0, npages = 0; i < rd->rd_nfrags; i++) {
 		for (size = 0; size <  rd->rd_frags[i].rf_nob;
@@ -586,7 +586,7 @@ kiblnd_fmr_map_tx(kib_net_t *net, kib_tx_t *tx, kib_rdma_desc_t *rd, int nob)
 	rd->rd_key = (rd != tx->tx_rd) ? tx->tx_u.fmr.fmr_pfmr->fmr->rkey :
 					 tx->tx_u.fmr.fmr_pfmr->fmr->lkey;
 	rd->rd_frags[0].rf_addr &= ~hdev->ibh_page_mask;
-	rd->rd_frags[0].rf_nob   = nob;
+	rd->rd_frags[0].rf_nob = nob;
 	rd->rd_nfrags = 1;
 
 	return 0;
@@ -595,11 +595,11 @@ kiblnd_fmr_map_tx(kib_net_t *net, kib_tx_t *tx, kib_rdma_desc_t *rd, int nob)
 static int
 kiblnd_pmr_map_tx(kib_net_t *net, kib_tx_t *tx, kib_rdma_desc_t *rd, int nob)
 {
-	kib_hca_dev_t		*hdev;
-	kib_pmr_poolset_t	*pps;
-	__u64			iova;
-	int			cpt;
-	int			rc;
+	kib_hca_dev_t *hdev;
+	kib_pmr_poolset_t *pps;
+	__u64 iova;
+	int cpt;
+	int rc;
 
 	LASSERT(tx->tx_pool != NULL);
 	LASSERT(tx->tx_pool->tpo_pool.po_owner != NULL);
@@ -623,7 +623,7 @@ kiblnd_pmr_map_tx(kib_net_t *net, kib_tx_t *tx, kib_rdma_desc_t *rd, int nob)
 					 tx->tx_u.pmr->pmr_mr->lkey;
 	rd->rd_nfrags = 1;
 	rd->rd_frags[0].rf_addr = iova;
-	rd->rd_frags[0].rf_nob  = nob;
+	rd->rd_frags[0].rf_nob = nob;
 
 	return 0;
 }
@@ -631,7 +631,7 @@ kiblnd_pmr_map_tx(kib_net_t *net, kib_tx_t *tx, kib_rdma_desc_t *rd, int nob)
 void
 kiblnd_unmap_tx(lnet_ni_t *ni, kib_tx_t *tx)
 {
-	kib_net_t  *net = ni->ni_data;
+	kib_net_t *net = ni->ni_data;
 
 	LASSERT(net != NULL);
 
@@ -655,20 +655,19 @@ int
 kiblnd_map_tx(lnet_ni_t *ni, kib_tx_t *tx,
 	      kib_rdma_desc_t *rd, int nfrags)
 {
-	kib_hca_dev_t      *hdev  = tx->tx_pool->tpo_hdev;
-	kib_net_t	  *net   = ni->ni_data;
-	struct ib_mr       *mr    = NULL;
-	__u32	       nob;
-	int		 i;
+	kib_hca_dev_t *hdev = tx->tx_pool->tpo_hdev;
+	kib_net_t *net = ni->ni_data;
+	struct ib_mr *mr    = NULL;
+	__u32 nob;
+	int i;
 
 	/* If rd is not tx_rd, it's going to get sent to a peer and I'm the
 	 * RDMA sink */
 	tx->tx_dmadir = (rd != tx->tx_rd) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
 	tx->tx_nfrags = nfrags;
 
-	rd->rd_nfrags =
-		kiblnd_dma_map_sg(hdev->ibh_ibdev,
-				  tx->tx_frags, tx->tx_nfrags, tx->tx_dmadir);
+	rd->rd_nfrags = kiblnd_dma_map_sg(hdev->ibh_ibdev, tx->tx_frags,
+					  tx->tx_nfrags, tx->tx_dmadir);
 
 	for (i = 0, nob = 0; i < rd->rd_nfrags; i++) {
 		rd->rd_frags[i].rf_nob  = kiblnd_sg_dma_len(
@@ -699,12 +698,12 @@ static int
 kiblnd_setup_rd_iov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 		    unsigned int niov, struct kvec *iov, int offset, int nob)
 {
-	kib_net_t	  *net = ni->ni_data;
-	struct page	*page;
+	kib_net_t *net = ni->ni_data;
+	struct page *page;
 	struct scatterlist *sg;
-	unsigned long       vaddr;
-	int		 fragnob;
-	int		 page_offset;
+	unsigned long vaddr;
+	int fragnob;
+	int page_offset;
 
 	LASSERT(nob > 0);
 	LASSERT(niov > 0);
@@ -752,9 +751,9 @@ static int
 kiblnd_setup_rd_kiov(lnet_ni_t *ni, kib_tx_t *tx, kib_rdma_desc_t *rd,
 		      int nkiov, lnet_kiov_t *kiov, int offset, int nob)
 {
-	kib_net_t	  *net = ni->ni_data;
+	kib_net_t *net = ni->ni_data;
 	struct scatterlist *sg;
-	int		 fragnob;
+	int fragnob;
 
 	CDEBUG(D_NET, "niov %d offset %d nob %d\n", nkiov, offset, nob);
 
@@ -793,11 +792,11 @@ kiblnd_post_tx_locked(kib_conn_t *conn, kib_tx_t *tx, int credit)
 	__releases(conn->ibc_lock)
 	__acquires(conn->ibc_lock)
 {
-	kib_msg_t	 *msg = tx->tx_msg;
-	kib_peer_t	*peer = conn->ibc_peer;
-	int		ver = conn->ibc_version;
-	int		rc;
-	int		done;
+	kib_msg_t *msg = tx->tx_msg;
+	kib_peer_t *peer = conn->ibc_peer;
+	int ver = conn->ibc_version;
+	int rc;
+	int done;
 	struct ib_send_wr *bad_wrq;
 
 	LASSERT(tx->tx_queued);
@@ -878,8 +877,7 @@ kiblnd_post_tx_locked(kib_conn_t *conn, kib_tx_t *tx, int credit)
 		/* close_conn will launch failover */
 		rc = -ENETDOWN;
 	} else {
-		rc = ib_post_send(conn->ibc_cmid->qp,
-				  tx->tx_wrq, &bad_wrq);
+		rc = ib_post_send(conn->ibc_cmid->qp, tx->tx_wrq, &bad_wrq);
 	}
 
 	conn->ibc_last_send = jiffies;
@@ -925,9 +923,9 @@ kiblnd_post_tx_locked(kib_conn_t *conn, kib_tx_t *tx, int credit)
 void
 kiblnd_check_sends(kib_conn_t *conn)
 {
-	int	ver = conn->ibc_version;
+	int ver = conn->ibc_version;
 	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
-	kib_tx_t  *tx;
+	kib_tx_t *tx;
 
 	/* Don't send anything until after the connection is established */
 	if (conn->ibc_state < IBLND_CONN_ESTABLISHED) {
@@ -997,9 +995,9 @@ kiblnd_check_sends(kib_conn_t *conn)
 static void
 kiblnd_tx_complete(kib_tx_t *tx, int status)
 {
-	int	   failed = (status != IB_WC_SUCCESS);
-	kib_conn_t   *conn = tx->tx_conn;
-	int	   idle;
+	int failed = (status != IB_WC_SUCCESS);
+	kib_conn_t *conn = tx->tx_conn;
+	int idle;
 
 	LASSERT(tx->tx_sending > 0);
 
@@ -1051,11 +1049,11 @@ kiblnd_tx_complete(kib_tx_t *tx, int status)
 void
 kiblnd_init_tx_msg(lnet_ni_t *ni, kib_tx_t *tx, int type, int body_nob)
 {
-	kib_hca_dev_t     *hdev = tx->tx_pool->tpo_hdev;
-	struct ib_sge     *sge = &tx->tx_sge[tx->tx_nwrq];
+	kib_hca_dev_t *hdev = tx->tx_pool->tpo_hdev;
+	struct ib_sge *sge = &tx->tx_sge[tx->tx_nwrq];
 	struct ib_send_wr *wrq = &tx->tx_wrq[tx->tx_nwrq];
-	int		nob = offsetof(kib_msg_t, ibm_u) + body_nob;
-	struct ib_mr      *mr;
+	int nob = offsetof(kib_msg_t, ibm_u) + body_nob;
+	struct ib_mr *mr;
 
 	LASSERT(tx->tx_nwrq >= 0);
 	LASSERT(tx->tx_nwrq < IBLND_MAX_RDMA_FRAGS + 1);
@@ -1086,14 +1084,14 @@ int
 kiblnd_init_rdma(kib_conn_t *conn, kib_tx_t *tx, int type,
 		  int resid, kib_rdma_desc_t *dstrd, __u64 dstcookie)
 {
-	kib_msg_t	 *ibmsg = tx->tx_msg;
-	kib_rdma_desc_t   *srcrd = tx->tx_rd;
-	struct ib_sge     *sge = &tx->tx_sge[0];
+	kib_msg_t *ibmsg = tx->tx_msg;
+	kib_rdma_desc_t *srcrd = tx->tx_rd;
+	struct ib_sge *sge = &tx->tx_sge[0];
 	struct ib_send_wr *wrq = &tx->tx_wrq[0];
-	int		rc  = resid;
-	int		srcidx;
-	int		dstidx;
-	int		wrknob;
+	int rc  = resid;
+	int srcidx;
+	int dstidx;
+	int wrknob;
 
 	LASSERT(!in_interrupt());
 	LASSERT(tx->tx_nwrq == 0);
@@ -1144,7 +1142,7 @@ kiblnd_init_rdma(kib_conn_t *conn, kib_tx_t *tx, int type,
 		wrq->send_flags = 0;
 
 		wrq->wr.rdma.remote_addr = kiblnd_rd_frag_addr(dstrd, dstidx);
-		wrq->wr.rdma.rkey	= kiblnd_rd_frag_key(dstrd, dstidx);
+		wrq->wr.rdma.rkey        = kiblnd_rd_frag_key(dstrd, dstidx);
 
 		srcidx = kiblnd_rd_consume_frag(srcrd, srcidx, wrknob);
 		dstidx = kiblnd_rd_consume_frag(dstrd, dstidx, wrknob);
@@ -1170,7 +1168,7 @@ kiblnd_init_rdma(kib_conn_t *conn, kib_tx_t *tx, int type,
 void
 kiblnd_queue_tx_locked(kib_tx_t *tx, kib_conn_t *conn)
 {
-	struct list_head   *q;
+	struct list_head *q;
 
 	LASSERT(tx->tx_nwrq > 0);	      /* work items set up */
 	LASSERT(!tx->tx_queued);	       /* not queued for sending already */
@@ -1271,11 +1269,11 @@ static void
 kiblnd_connect_peer(kib_peer_t *peer)
 {
 	struct rdma_cm_id *cmid;
-	kib_dev_t	 *dev;
-	kib_net_t	 *net = peer->ibp_ni->ni_data;
+	kib_dev_t *dev;
+	kib_net_t *net = peer->ibp_ni->ni_data;
 	struct sockaddr_in srcaddr;
 	struct sockaddr_in dstaddr;
-	int		rc;
+	int rc;
 
 	LASSERT(net != NULL);
 	LASSERT(peer->ibp_connecting > 0);
@@ -1335,12 +1333,12 @@ kiblnd_connect_peer(kib_peer_t *peer)
 void
 kiblnd_launch_tx(lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
 {
-	kib_peer_t	*peer;
-	kib_peer_t	*peer2;
-	kib_conn_t	*conn;
-	rwlock_t	*g_lock = &kiblnd_data.kib_global_lock;
-	unsigned long      flags;
-	int		rc;
+	kib_peer_t *peer;
+	kib_peer_t *peer2;
+	kib_conn_t *conn;
+	rwlock_t *g_lock = &kiblnd_data.kib_global_lock;
+	unsigned long flags;
+	int rc;
 
 	/* If I get here, I've committed to send, so I complete the tx with
 	 * failure on any problems */
@@ -1456,20 +1454,20 @@ kiblnd_launch_tx(lnet_ni_t *ni, kib_tx_t *tx, lnet_nid_t nid)
 int
 kiblnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 {
-	lnet_hdr_t       *hdr = &lntmsg->msg_hdr;
-	int	       type = lntmsg->msg_type;
+	lnet_hdr_t *hdr = &lntmsg->msg_hdr;
+	int type = lntmsg->msg_type;
 	lnet_process_id_t target = lntmsg->msg_target;
-	int	       target_is_router = lntmsg->msg_target_is_router;
-	int	       routing = lntmsg->msg_routing;
-	unsigned int      payload_niov = lntmsg->msg_niov;
-	struct kvec      *payload_iov = lntmsg->msg_iov;
-	lnet_kiov_t      *payload_kiov = lntmsg->msg_kiov;
-	unsigned int      payload_offset = lntmsg->msg_offset;
-	unsigned int      payload_nob = lntmsg->msg_len;
-	kib_msg_t	*ibmsg;
-	kib_tx_t	 *tx;
-	int	       nob;
-	int	       rc;
+	int target_is_router = lntmsg->msg_target_is_router;
+	int routing = lntmsg->msg_routing;
+	unsigned int payload_niov = lntmsg->msg_niov;
+	struct kvec *payload_iov = lntmsg->msg_iov;
+	lnet_kiov_t *payload_kiov = lntmsg->msg_kiov;
+	unsigned int payload_offset = lntmsg->msg_offset;
+	unsigned int payload_nob = lntmsg->msg_len;
+	kib_msg_t *ibmsg;
+	kib_tx_t *tx;
+	int nob;
+	int rc;
 
 	/* NB 'private' is different depending on what we're sending.... */
 
@@ -1628,13 +1626,13 @@ static void
 kiblnd_reply(lnet_ni_t *ni, kib_rx_t *rx, lnet_msg_t *lntmsg)
 {
 	lnet_process_id_t target = lntmsg->msg_target;
-	unsigned int      niov = lntmsg->msg_niov;
-	struct kvec      *iov = lntmsg->msg_iov;
-	lnet_kiov_t      *kiov = lntmsg->msg_kiov;
-	unsigned int      offset = lntmsg->msg_offset;
-	unsigned int      nob = lntmsg->msg_len;
-	kib_tx_t	 *tx;
-	int	       rc;
+	unsigned int niov = lntmsg->msg_niov;
+	struct kvec *iov = lntmsg->msg_iov;
+	lnet_kiov_t *kiov = lntmsg->msg_kiov;
+	unsigned int offset = lntmsg->msg_offset;
+	unsigned int nob = lntmsg->msg_len;
+	kib_tx_t *tx;
+	int rc;
 
 	tx = kiblnd_get_idle_tx(ni, rx->rx_conn->ibc_peer->ibp_nid);
 	if (tx == NULL) {
@@ -1691,14 +1689,14 @@ kiblnd_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg, int delayed,
 	     unsigned int niov, struct kvec *iov, lnet_kiov_t *kiov,
 	     unsigned int offset, unsigned int mlen, unsigned int rlen)
 {
-	kib_rx_t    *rx = private;
-	kib_msg_t   *rxmsg = rx->rx_msg;
-	kib_conn_t  *conn = rx->rx_conn;
-	kib_tx_t    *tx;
-	kib_msg_t   *txmsg;
-	int	  nob;
-	int	  post_credit = IBLND_POSTRX_PEER_CREDIT;
-	int	  rc = 0;
+	kib_rx_t *rx = private;
+	kib_msg_t *rxmsg = rx->rx_msg;
+	kib_conn_t *conn = rx->rx_conn;
+	kib_tx_t *tx;
+	kib_msg_t *txmsg;
+	int nob;
+	int post_credit = IBLND_POSTRX_PEER_CREDIT;
+	int rc = 0;
 
 	LASSERT(mlen <= rlen);
 	LASSERT(!in_interrupt());
@@ -1828,8 +1826,8 @@ kiblnd_peer_alive(kib_peer_t *peer)
 static void
 kiblnd_peer_notify(kib_peer_t *peer)
 {
-	int	   error = 0;
-	unsigned long    last_alive = 0;
+	int error = 0;
+	unsigned long last_alive = 0;
 	unsigned long flags;
 
 	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
@@ -1860,9 +1858,9 @@ kiblnd_close_conn_locked(kib_conn_t *conn, int error)
 	 * connection to be finished off by the connd.  Otherwise the connd is
 	 * already dealing with it (either to set it up or tear it down).
 	 * Caller holds kib_global_lock exclusively in irq context */
-	kib_peer_t       *peer = conn->ibc_peer;
-	kib_dev_t	*dev;
-	unsigned long     flags;
+	kib_peer_t *peer = conn->ibc_peer;
+	kib_dev_t *dev;
+	unsigned long flags;
 
 	LASSERT(error != 0 || conn->ibc_state >= IBLND_CONN_ESTABLISHED);
 
@@ -1934,8 +1932,8 @@ kiblnd_close_conn(kib_conn_t *conn, int error)
 static void
 kiblnd_handle_early_rxs(kib_conn_t *conn)
 {
-	unsigned long    flags;
-	kib_rx_t	*rx;
+	unsigned long flags;
+	kib_rx_t *rx;
 	kib_rx_t *tmp;
 
 	LASSERT(!in_interrupt());
@@ -1957,9 +1955,9 @@ static void
 kiblnd_abort_txs(kib_conn_t *conn, struct list_head *txs)
 {
 	LIST_HEAD(zombies);
-	struct list_head	  *tmp;
-	struct list_head	  *nxt;
-	kib_tx_t	    *tx;
+	struct list_head *tmp;
+	struct list_head *nxt;
+	kib_tx_t *tx;
 
 	spin_lock(&conn->ibc_lock);
 
@@ -2018,7 +2016,7 @@ void
 kiblnd_peer_connect_failed(kib_peer_t *peer, int active, int error)
 {
 	LIST_HEAD(zombies);
-	unsigned long     flags;
+	unsigned long flags;
 
 	LASSERT(error != 0);
 	LASSERT(!in_interrupt());
@@ -2071,12 +2069,12 @@ kiblnd_peer_connect_failed(kib_peer_t *peer, int active, int error)
 void
 kiblnd_connreq_done(kib_conn_t *conn, int status)
 {
-	kib_peer_t	*peer = conn->ibc_peer;
-	kib_tx_t	  *tx;
+	kib_peer_t *peer = conn->ibc_peer;
+	kib_tx_t *tx;
 	kib_tx_t *tmp;
-	struct list_head	 txs;
-	unsigned long      flags;
-	int		active;
+	struct list_head txs;
+	unsigned long flags;
+	int active;
 
 	active = (conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT);
 
@@ -2166,7 +2164,7 @@ kiblnd_connreq_done(kib_conn_t *conn, int status)
 static void
 kiblnd_reject(struct rdma_cm_id *cmid, kib_rej_t *rej)
 {
-	int	  rc;
+	int rc;
 
 	rc = rdma_reject(cmid, rej, sizeof(*rej));
 
@@ -2177,22 +2175,22 @@ kiblnd_reject(struct rdma_cm_id *cmid, kib_rej_t *rej)
 static int
 kiblnd_passive_connect(struct rdma_cm_id *cmid, void *priv, int priv_nob)
 {
-	rwlock_t		*g_lock = &kiblnd_data.kib_global_lock;
-	kib_msg_t	     *reqmsg = priv;
-	kib_msg_t	     *ackmsg;
-	kib_dev_t	     *ibdev;
-	kib_peer_t	    *peer;
-	kib_peer_t	    *peer2;
-	kib_conn_t	    *conn;
-	lnet_ni_t	     *ni  = NULL;
-	kib_net_t	     *net = NULL;
-	lnet_nid_t	     nid;
+	rwlock_t *g_lock = &kiblnd_data.kib_global_lock;
+	kib_msg_t *reqmsg = priv;
+	kib_msg_t *ackmsg;
+	kib_dev_t *ibdev;
+	kib_peer_t *peer;
+	kib_peer_t *peer2;
+	kib_conn_t *conn;
+	lnet_ni_t *ni  = NULL;
+	kib_net_t *net = NULL;
+	lnet_nid_t nid;
 	struct rdma_conn_param cp;
-	kib_rej_t	      rej;
-	int		    version = IBLND_MSG_VERSION;
-	unsigned long	  flags;
-	int		    rc;
-	struct sockaddr_in    *peer_addr;
+	kib_rej_t rej;
+	int version = IBLND_MSG_VERSION;
+	unsigned long flags;
+	int rc;
+	struct sockaddr_in *peer_addr;
 	LASSERT(!in_interrupt());
 
 	/* cmid inherits 'context' from the corresponding listener id */
@@ -2200,8 +2198,8 @@ kiblnd_passive_connect(struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	LASSERT(ibdev != NULL);
 
 	memset(&rej, 0, sizeof(rej));
-	rej.ibr_magic		= IBLND_MSG_MAGIC;
-	rej.ibr_why		  = IBLND_REJECT_FATAL;
+	rej.ibr_magic = IBLND_MSG_MAGIC;
+	rej.ibr_why = IBLND_REJECT_FATAL;
 	rej.ibr_cp.ibcp_max_msg_size = IBLND_MSG_SIZE;
 
 	peer_addr = (struct sockaddr_in *)&(cmid->route.addr.dst_addr);
@@ -2243,7 +2241,7 @@ kiblnd_passive_connect(struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	}
 
 	nid = reqmsg->ibm_srcnid;
-	ni  = lnet_net2ni(LNET_NIDNET(reqmsg->ibm_dstnid));
+	ni = lnet_net2ni(LNET_NIDNET(reqmsg->ibm_dstnid));
 
 	if (ni != NULL) {
 		net = (kib_net_t *)ni->ni_data;
@@ -2394,7 +2392,7 @@ kiblnd_passive_connect(struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	 * CM callback doesn't destroy cmid. */
 
 	conn->ibc_incarnation      = reqmsg->ibm_srcstamp;
-	conn->ibc_credits	  = IBLND_MSG_QUEUE_SIZE(version);
+	conn->ibc_credits          = IBLND_MSG_QUEUE_SIZE(version);
 	conn->ibc_reserved_credits = IBLND_MSG_QUEUE_SIZE(version);
 	LASSERT(conn->ibc_credits + conn->ibc_reserved_credits + IBLND_OOB_MSGS(version)
 		 <= IBLND_RX_MSGS(version));
@@ -2412,12 +2410,12 @@ kiblnd_passive_connect(struct rdma_cm_id *cmid, void *priv, int priv_nob)
 
 	memset(&cp, 0, sizeof(cp));
 	cp.private_data	= ackmsg;
-	cp.private_data_len    = ackmsg->ibm_nob;
+	cp.private_data_len = ackmsg->ibm_nob;
 	cp.responder_resources = 0;	     /* No atomic ops or RDMA reads */
-	cp.initiator_depth     = 0;
+	cp.initiator_depth = 0;
 	cp.flow_control	= 1;
-	cp.retry_count	 = *kiblnd_tunables.kib_retry_count;
-	cp.rnr_retry_count     = *kiblnd_tunables.kib_rnr_retry_count;
+	cp.retry_count = *kiblnd_tunables.kib_retry_count;
+	cp.rnr_retry_count = *kiblnd_tunables.kib_rnr_retry_count;
 
 	CDEBUG(D_NET, "Accept %s\n", libcfs_nid2str(nid));
 
@@ -2439,7 +2437,7 @@ kiblnd_passive_connect(struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	if (ni != NULL)
 		lnet_ni_decref(ni);
 
-	rej.ibr_version = version;
+	rej.ibr_version             = version;
 	rej.ibr_cp.ibcp_queue_depth = IBLND_MSG_QUEUE_SIZE(version);
 	rej.ibr_cp.ibcp_max_frags   = IBLND_RDMA_FRAGS(version);
 	kiblnd_reject(cmid, &rej);
@@ -2451,10 +2449,10 @@ static void
 kiblnd_reconnect(kib_conn_t *conn, int version,
 		  __u64 incarnation, int why, kib_connparams_t *cp)
 {
-	kib_peer_t    *peer = conn->ibc_peer;
-	char	  *reason;
-	int	    retry = 0;
-	unsigned long  flags;
+	kib_peer_t *peer = conn->ibc_peer;
+	char *reason;
+	int retry = 0;
+	unsigned long flags;
 
 	LASSERT(conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT);
 	LASSERT(peer->ibp_connecting > 0);     /* 'conn' at least */
@@ -2513,7 +2511,7 @@ kiblnd_reconnect(kib_conn_t *conn, int version,
 static void
 kiblnd_rejected(kib_conn_t *conn, int reason, void *priv, int priv_nob)
 {
-	kib_peer_t    *peer = conn->ibc_peer;
+	kib_peer_t *peer = conn->ibc_peer;
 
 	LASSERT(!in_interrupt());
 	LASSERT(conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT);
@@ -2532,10 +2530,10 @@ kiblnd_rejected(kib_conn_t *conn, int reason, void *priv, int priv_nob)
 
 	case IB_CM_REJ_CONSUMER_DEFINED:
 		if (priv_nob >= offsetof(kib_rej_t, ibr_padding)) {
-			kib_rej_t	*rej	 = priv;
-			kib_connparams_t *cp	  = NULL;
-			int	       flip	= 0;
-			__u64	     incarnation = -1;
+			kib_rej_t *rej = priv;
+			kib_connparams_t *cp = NULL;
+			int flip = 0;
+			__u64 incarnation = -1;
 
 			/* NB. default incarnation is -1 because:
 			 * a) V1 will ignore dst incarnation in connreq.
@@ -2652,13 +2650,13 @@ kiblnd_rejected(kib_conn_t *conn, int reason, void *priv, int priv_nob)
 static void
 kiblnd_check_connreply(kib_conn_t *conn, void *priv, int priv_nob)
 {
-	kib_peer_t    *peer = conn->ibc_peer;
-	lnet_ni_t     *ni   = peer->ibp_ni;
-	kib_net_t     *net  = ni->ni_data;
-	kib_msg_t     *msg  = priv;
-	int	    ver  = conn->ibc_version;
-	int	    rc   = kiblnd_unpack_msg(msg, priv_nob);
-	unsigned long  flags;
+	kib_peer_t *peer = conn->ibc_peer;
+	lnet_ni_t *ni = peer->ibp_ni;
+	kib_net_t *net = ni->ni_data;
+	kib_msg_t *msg = priv;
+	int ver = conn->ibc_version;
+	int rc = kiblnd_unpack_msg(msg, priv_nob);
+	unsigned long flags;
 
 	LASSERT(net != NULL);
 
@@ -2726,8 +2724,8 @@ kiblnd_check_connreply(kib_conn_t *conn, void *priv, int priv_nob)
 		goto failed;
 	}
 
-	conn->ibc_incarnation      = msg->ibm_srcstamp;
-	conn->ibc_credits	  =
+	conn->ibc_incarnation = msg->ibm_srcstamp;
+	conn->ibc_credits =
 	conn->ibc_reserved_credits = IBLND_MSG_QUEUE_SIZE(ver);
 	LASSERT(conn->ibc_credits + conn->ibc_reserved_credits + IBLND_OOB_MSGS(ver)
 		 <= IBLND_RX_MSGS(ver));
@@ -2749,20 +2747,20 @@ kiblnd_check_connreply(kib_conn_t *conn, void *priv, int priv_nob)
 static int
 kiblnd_active_connect(struct rdma_cm_id *cmid)
 {
-	kib_peer_t	      *peer = (kib_peer_t *)cmid->context;
-	kib_conn_t	      *conn;
-	kib_msg_t	       *msg;
-	struct rdma_conn_param   cp;
-	int		      version;
-	__u64		    incarnation;
-	unsigned long	    flags;
-	int		      rc;
+	kib_peer_t *peer = (kib_peer_t *)cmid->context;
+	kib_conn_t *conn;
+	kib_msg_t *msg;
+	struct rdma_conn_param cp;
+	int version;
+	__u64 incarnation;
+	unsigned long flags;
+	int rc;
 
 	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
 
 	incarnation = peer->ibp_incarnation;
-	version     = (peer->ibp_version == 0) ? IBLND_MSG_VERSION :
-						 peer->ibp_version;
+	version = (peer->ibp_version == 0) ? IBLND_MSG_VERSION :
+					     peer->ibp_version;
 
 	read_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
 
@@ -2793,8 +2791,8 @@ kiblnd_active_connect(struct rdma_cm_id *cmid)
 	cp.private_data_len    = msg->ibm_nob;
 	cp.responder_resources = 0;	     /* No atomic ops or RDMA reads */
 	cp.initiator_depth     = 0;
-	cp.flow_control	= 1;
-	cp.retry_count	 = *kiblnd_tunables.kib_retry_count;
+	cp.flow_control        = 1;
+	cp.retry_count         = *kiblnd_tunables.kib_retry_count;
 	cp.rnr_retry_count     = *kiblnd_tunables.kib_rnr_retry_count;
 
 	LASSERT(cmid->context == (void *)conn);
@@ -2814,9 +2812,9 @@ kiblnd_active_connect(struct rdma_cm_id *cmid)
 int
 kiblnd_cm_callback(struct rdma_cm_id *cmid, struct rdma_cm_event *event)
 {
-	kib_peer_t  *peer;
-	kib_conn_t  *conn;
-	int	  rc;
+	kib_peer_t *peer;
+	kib_conn_t *conn;
+	int rc;
 
 	switch (event->event) {
 	default:
@@ -2983,8 +2981,8 @@ kiblnd_cm_callback(struct rdma_cm_id *cmid, struct rdma_cm_event *event)
 static int
 kiblnd_check_txs_locked(kib_conn_t *conn, struct list_head *txs)
 {
-	kib_tx_t	  *tx;
-	struct list_head	*ttmp;
+	kib_tx_t *tx;
+	struct list_head *ttmp;
 
 	list_for_each(ttmp, txs) {
 		tx = list_entry(ttmp, kib_tx_t, tx_list);
@@ -3022,13 +3020,13 @@ kiblnd_check_conns(int idx)
 {
 	LIST_HEAD(closes);
 	LIST_HEAD(checksends);
-	struct list_head    *peers = &kiblnd_data.kib_peers[idx];
-	struct list_head    *ptmp;
-	kib_peer_t    *peer;
-	kib_conn_t    *conn;
+	struct list_head *peers = &kiblnd_data.kib_peers[idx];
+	struct list_head *ptmp;
+	kib_peer_t *peer;
+	kib_conn_t *conn;
 	kib_conn_t *tmp;
-	struct list_head    *ctmp;
-	unsigned long  flags;
+	struct list_head *ctmp;
+	unsigned long flags;
 
 	/* NB. We expect to have a look at all the peers and not find any
 	 * RDMAs to time out, so we just use a shared lock while we
@@ -3114,14 +3112,14 @@ kiblnd_disconnect_conn(kib_conn_t *conn)
 int
 kiblnd_connd(void *arg)
 {
-	wait_queue_t     wait;
-	unsigned long      flags;
-	kib_conn_t	*conn;
-	int		timeout;
-	int		i;
-	int		dropped_lock;
-	int		peer_index = 0;
-	unsigned long      deadline = jiffies;
+	wait_queue_t wait;
+	unsigned long flags;
+	kib_conn_t *conn;
+	int timeout;
+	int i;
+	int dropped_lock;
+	int peer_index = 0;
+	unsigned long deadline = jiffies;
 
 	cfs_block_allsigs();
 
@@ -3169,7 +3167,7 @@ kiblnd_connd(void *arg)
 		if (timeout <= 0) {
 			const int n = 4;
 			const int p = 1;
-			int       chunk = kiblnd_data.kib_peer_hash_size;
+			int chunk = kiblnd_data.kib_peer_hash_size;
 
 			spin_unlock_irqrestore(&kiblnd_data.kib_connd_lock, flags);
 			dropped_lock = 1;
@@ -3273,9 +3271,9 @@ kiblnd_cq_completion(struct ib_cq *cq, void *arg)
 	 * consuming my CQ I could be called after all completions have
 	 * occurred.  But in this case, ibc_nrx == 0 && ibc_nsends_posted == 0
 	 * and this CQ is about to be destroyed so I NOOP. */
-	kib_conn_t		*conn = (kib_conn_t *)arg;
-	struct kib_sched_info	*sched = conn->ibc_sched;
-	unsigned long		flags;
+	kib_conn_t *conn = (kib_conn_t *)arg;
+	struct kib_sched_info *sched = conn->ibc_sched;
+	unsigned long flags;
 
 	LASSERT(cq == conn->ibc_cq);
 
@@ -3309,15 +3307,15 @@ kiblnd_cq_event(struct ib_event *event, void *arg)
 int
 kiblnd_scheduler(void *arg)
 {
-	long			id = (long)arg;
-	struct kib_sched_info	*sched;
-	kib_conn_t		*conn;
-	wait_queue_t		wait;
-	unsigned long		flags;
-	struct ib_wc		wc;
-	int			did_something;
-	int			busy_loops = 0;
-	int			rc;
+	long id = (long)arg;
+	struct kib_sched_info *sched;
+	kib_conn_t *conn;
+	wait_queue_t wait;
+	unsigned long flags;
+	struct ib_wc wc;
+	int did_something;
+	int busy_loops = 0;
+	int rc;
 
 	cfs_block_allsigs();
 
@@ -3432,11 +3430,11 @@ kiblnd_scheduler(void *arg)
 int
 kiblnd_failover_thread(void *arg)
 {
-	rwlock_t		*glock = &kiblnd_data.kib_global_lock;
-	kib_dev_t	 *dev;
-	wait_queue_t     wait;
-	unsigned long      flags;
-	int		rc;
+	rwlock_t *glock = &kiblnd_data.kib_global_lock;
+	kib_dev_t *dev;
+	wait_queue_t wait;
+	unsigned long flags;
+	int rc;
 
 	LASSERT(*kiblnd_tunables.kib_dev_failover != 0);
 
@@ -3446,8 +3444,8 @@ kiblnd_failover_thread(void *arg)
 	write_lock_irqsave(glock, flags);
 
 	while (!kiblnd_data.kib_shutdown) {
-		int     do_failover = 0;
-		int     long_sleep;
+		int do_failover = 0;
+		int long_sleep;
 
 		list_for_each_entry(dev, &kiblnd_data.kib_failed_devs,
 				    ibd_fail_list) {
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
index eedf01afd57f..b0e00361cfce 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
@@ -150,30 +150,30 @@ module_param(use_privileged_port, int, 0644);
 MODULE_PARM_DESC(use_privileged_port, "use privileged port when initiating connection");
 
 kib_tunables_t kiblnd_tunables = {
-	.kib_dev_failover	   = &dev_failover,
-	.kib_service		= &service,
-	.kib_cksum		  = &cksum,
-	.kib_timeout		= &timeout,
-	.kib_keepalive	      = &keepalive,
-	.kib_ntx		    = &ntx,
-	.kib_credits		= &credits,
-	.kib_peertxcredits	  = &peer_credits,
-	.kib_peercredits_hiw	= &peer_credits_hiw,
-	.kib_peerrtrcredits	 = &peer_buffer_credits,
-	.kib_peertimeout	    = &peer_timeout,
-	.kib_default_ipif	   = &ipif_name,
-	.kib_retry_count	    = &retry_count,
-	.kib_rnr_retry_count	= &rnr_retry_count,
-	.kib_concurrent_sends       = &concurrent_sends,
-	.kib_ib_mtu		 = &ib_mtu,
-	.kib_map_on_demand	  = &map_on_demand,
-	.kib_fmr_pool_size	  = &fmr_pool_size,
-	.kib_fmr_flush_trigger      = &fmr_flush_trigger,
-	.kib_fmr_cache	      = &fmr_cache,
-	.kib_pmr_pool_size	  = &pmr_pool_size,
-	.kib_require_priv_port      = &require_privileged_port,
-	.kib_use_priv_port	    = &use_privileged_port,
-	.kib_nscheds		    = &nscheds
+	.kib_dev_failover      = &dev_failover,
+	.kib_service           = &service,
+	.kib_cksum             = &cksum,
+	.kib_timeout           = &timeout,
+	.kib_keepalive         = &keepalive,
+	.kib_ntx               = &ntx,
+	.kib_credits           = &credits,
+	.kib_peertxcredits     = &peer_credits,
+	.kib_peercredits_hiw   = &peer_credits_hiw,
+	.kib_peerrtrcredits    = &peer_buffer_credits,
+	.kib_peertimeout       = &peer_timeout,
+	.kib_default_ipif      = &ipif_name,
+	.kib_retry_count       = &retry_count,
+	.kib_rnr_retry_count   = &rnr_retry_count,
+	.kib_concurrent_sends  = &concurrent_sends,
+	.kib_ib_mtu            = &ib_mtu,
+	.kib_map_on_demand     = &map_on_demand,
+	.kib_fmr_pool_size     = &fmr_pool_size,
+	.kib_fmr_flush_trigger = &fmr_flush_trigger,
+	.kib_fmr_cache         = &fmr_cache,
+	.kib_pmr_pool_size     = &pmr_pool_size,
+	.kib_require_priv_port = &require_privileged_port,
+	.kib_use_priv_port     = &use_privileged_port,
+	.kib_nscheds           = &nscheds
 };
 
 int
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/Makefile b/drivers/staging/lustre/lnet/klnds/socklnd/Makefile
index f3fb8778c3ad..c011581d3453 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/Makefile
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/Makefile
@@ -1,3 +1,3 @@
 obj-$(CONFIG_LNET) += ksocklnd.o
 
-ksocklnd-y := socklnd.o socklnd_cb.o socklnd_proto.o socklnd_modparams.o socklnd_lib-linux.o
+ksocklnd-y := socklnd.o socklnd_cb.o socklnd_proto.o socklnd_modparams.o socklnd_lib.o
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
index 7586b7e4040b..4128a92218a9 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
@@ -49,8 +49,8 @@ ksock_nal_data_t ksocknal_data;
 static ksock_interface_t *
 ksocknal_ip2iface(lnet_ni_t *ni, __u32 ip)
 {
-	ksock_net_t       *net = ni->ni_data;
-	int		i;
+	ksock_net_t *net = ni->ni_data;
+	int i;
 	ksock_interface_t *iface;
 
 	for (i = 0; i < net->ksnn_ninterfaces; i++) {
@@ -102,8 +102,8 @@ ksocknal_destroy_route(ksock_route_t *route)
 static int
 ksocknal_create_peer(ksock_peer_t **peerp, lnet_ni_t *ni, lnet_process_id_t id)
 {
-	ksock_net_t   *net = ni->ni_data;
-	ksock_peer_t  *peer;
+	ksock_net_t *net = ni->ni_data;
+	ksock_peer_t *peer;
 
 	LASSERT(id.nid != LNET_NID_ANY);
 	LASSERT(id.pid != LNET_PID_ANY);
@@ -149,7 +149,7 @@ ksocknal_create_peer(ksock_peer_t **peerp, lnet_ni_t *ni, lnet_process_id_t id)
 void
 ksocknal_destroy_peer(ksock_peer_t *peer)
 {
-	ksock_net_t    *net = peer->ksnp_ni->ni_data;
+	ksock_net_t *net = peer->ksnp_ni->ni_data;
 
 	CDEBUG(D_NET, "peer %s %p deleted\n",
 		libcfs_id2str(peer->ksnp_id), peer);
@@ -175,9 +175,9 @@ ksocknal_destroy_peer(ksock_peer_t *peer)
 ksock_peer_t *
 ksocknal_find_peer_locked(lnet_ni_t *ni, lnet_process_id_t id)
 {
-	struct list_head       *peer_list = ksocknal_nid2peerlist(id.nid);
-	struct list_head       *tmp;
-	ksock_peer_t     *peer;
+	struct list_head *peer_list = ksocknal_nid2peerlist(id.nid);
+	struct list_head *tmp;
+	ksock_peer_t *peer;
 
 	list_for_each(tmp, peer_list) {
 
@@ -203,7 +203,7 @@ ksocknal_find_peer_locked(lnet_ni_t *ni, lnet_process_id_t id)
 ksock_peer_t *
 ksocknal_find_peer(lnet_ni_t *ni, lnet_process_id_t id)
 {
-	ksock_peer_t     *peer;
+	ksock_peer_t *peer;
 
 	read_lock(&ksocknal_data.ksnd_global_lock);
 	peer = ksocknal_find_peer_locked(ni, id);
@@ -217,8 +217,8 @@ ksocknal_find_peer(lnet_ni_t *ni, lnet_process_id_t id)
 static void
 ksocknal_unlink_peer_locked(ksock_peer_t *peer)
 {
-	int		i;
-	__u32	      ip;
+	int i;
+	__u32 ip;
 	ksock_interface_t *iface;
 
 	for (i = 0; i < peer->ksnp_n_passive_ips; i++) {
@@ -249,13 +249,13 @@ ksocknal_get_peer_info(lnet_ni_t *ni, int index,
 			lnet_process_id_t *id, __u32 *myip, __u32 *peer_ip,
 			int *port, int *conn_count, int *share_count)
 {
-	ksock_peer_t      *peer;
-	struct list_head	*ptmp;
-	ksock_route_t     *route;
-	struct list_head	*rtmp;
-	int		i;
-	int		j;
-	int		rc = -ENOENT;
+	ksock_peer_t *peer;
+	struct list_head *ptmp;
+	ksock_route_t *route;
+	struct list_head *rtmp;
+	int i;
+	int j;
+	int rc = -ENOENT;
 
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
@@ -322,8 +322,8 @@ ksocknal_get_peer_info(lnet_ni_t *ni, int index,
 static void
 ksocknal_associate_route_conn_locked(ksock_route_t *route, ksock_conn_t *conn)
 {
-	ksock_peer_t      *peer = route->ksnr_peer;
-	int		type = conn->ksnc_type;
+	ksock_peer_t *peer = route->ksnr_peer;
+	int type = conn->ksnc_type;
 	ksock_interface_t *iface;
 
 	conn->ksnc_route = route;
@@ -366,9 +366,9 @@ ksocknal_associate_route_conn_locked(ksock_route_t *route, ksock_conn_t *conn)
 static void
 ksocknal_add_route_locked(ksock_peer_t *peer, ksock_route_t *route)
 {
-	struct list_head	*tmp;
-	ksock_conn_t      *conn;
-	ksock_route_t     *route2;
+	struct list_head *tmp;
+	ksock_conn_t *conn;
+	ksock_route_t *route2;
 
 	LASSERT(!peer->ksnp_closing);
 	LASSERT(route->ksnr_peer == NULL);
@@ -407,11 +407,11 @@ ksocknal_add_route_locked(ksock_peer_t *peer, ksock_route_t *route)
 static void
 ksocknal_del_route_locked(ksock_route_t *route)
 {
-	ksock_peer_t      *peer = route->ksnr_peer;
+	ksock_peer_t *peer = route->ksnr_peer;
 	ksock_interface_t *iface;
-	ksock_conn_t      *conn;
-	struct list_head	*ctmp;
-	struct list_head	*cnxt;
+	ksock_conn_t *conn;
+	struct list_head *ctmp;
+	struct list_head *cnxt;
 
 	LASSERT(!route->ksnr_deleted);
 
@@ -447,12 +447,12 @@ ksocknal_del_route_locked(ksock_route_t *route)
 int
 ksocknal_add_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 {
-	struct list_head	*tmp;
-	ksock_peer_t      *peer;
-	ksock_peer_t      *peer2;
-	ksock_route_t     *route;
-	ksock_route_t     *route2;
-	int		rc;
+	struct list_head *tmp;
+	ksock_peer_t *peer;
+	ksock_peer_t *peer2;
+	ksock_route_t *route;
+	ksock_route_t *route2;
+	int rc;
 
 	if (id.nid == LNET_NID_ANY ||
 	    id.pid == LNET_PID_ANY)
@@ -509,11 +509,11 @@ ksocknal_add_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
 static void
 ksocknal_del_peer_locked(ksock_peer_t *peer, __u32 ip)
 {
-	ksock_conn_t     *conn;
-	ksock_route_t    *route;
-	struct list_head       *tmp;
-	struct list_head       *nxt;
-	int	       nshared;
+	ksock_conn_t *conn;
+	ksock_route_t *route;
+	struct list_head *tmp;
+	struct list_head *nxt;
+	int nshared;
 
 	LASSERT(!peer->ksnp_closing);
 
@@ -565,13 +565,13 @@ static int
 ksocknal_del_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 {
 	LIST_HEAD(zombies);
-	struct list_head	*ptmp;
-	struct list_head	*pnxt;
-	ksock_peer_t      *peer;
-	int		lo;
-	int		hi;
-	int		i;
-	int		rc = -ENOENT;
+	struct list_head *ptmp;
+	struct list_head *pnxt;
+	ksock_peer_t *peer;
+	int lo;
+	int hi;
+	int i;
+	int rc = -ENOENT;
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
@@ -623,11 +623,11 @@ ksocknal_del_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 static ksock_conn_t *
 ksocknal_get_conn_by_idx(lnet_ni_t *ni, int index)
 {
-	ksock_peer_t      *peer;
-	struct list_head	*ptmp;
-	ksock_conn_t      *conn;
-	struct list_head	*ctmp;
-	int		i;
+	ksock_peer_t *peer;
+	struct list_head *ptmp;
+	ksock_conn_t *conn;
+	struct list_head *ctmp;
+	int i;
 
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
@@ -661,8 +661,8 @@ static ksock_sched_t *
 ksocknal_choose_scheduler_locked(unsigned int cpt)
 {
 	struct ksock_sched_info	*info = ksocknal_data.ksnd_sched_info[cpt];
-	ksock_sched_t		*sched;
-	int			i;
+	ksock_sched_t *sched;
+	int i;
 
 	LASSERT(info->ksi_nthreads > 0);
 
@@ -683,9 +683,9 @@ ksocknal_choose_scheduler_locked(unsigned int cpt)
 static int
 ksocknal_local_ipvec(lnet_ni_t *ni, __u32 *ipaddrs)
 {
-	ksock_net_t       *net = ni->ni_data;
-	int		i;
-	int		nip;
+	ksock_net_t *net = ni->ni_data;
+	int i;
+	int nip;
 
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
@@ -711,12 +711,12 @@ ksocknal_local_ipvec(lnet_ni_t *ni, __u32 *ipaddrs)
 static int
 ksocknal_match_peerip(ksock_interface_t *iface, __u32 *ips, int nips)
 {
-	int   best_netmatch = 0;
-	int   best_xor      = 0;
-	int   best	  = -1;
-	int   this_xor;
-	int   this_netmatch;
-	int   i;
+	int best_netmatch = 0;
+	int best_xor      = 0;
+	int best	  = -1;
+	int this_xor;
+	int this_netmatch;
+	int i;
 
 	for (i = 0; i < nips; i++) {
 		if (ips[i] == 0)
@@ -743,19 +743,19 @@ ksocknal_match_peerip(ksock_interface_t *iface, __u32 *ips, int nips)
 static int
 ksocknal_select_ips(ksock_peer_t *peer, __u32 *peerips, int n_peerips)
 {
-	rwlock_t		*global_lock = &ksocknal_data.ksnd_global_lock;
-	ksock_net_t	*net = peer->ksnp_ni->ni_data;
-	ksock_interface_t  *iface;
-	ksock_interface_t  *best_iface;
-	int		 n_ips;
-	int		 i;
-	int		 j;
-	int		 k;
-	__u32	       ip;
-	__u32	       xor;
-	int		 this_netmatch;
-	int		 best_netmatch;
-	int		 best_npeers;
+	rwlock_t *global_lock = &ksocknal_data.ksnd_global_lock;
+	ksock_net_t *net = peer->ksnp_ni->ni_data;
+	ksock_interface_t *iface;
+	ksock_interface_t *best_iface;
+	int n_ips;
+	int i;
+	int j;
+	int k;
+	__u32 ip;
+	__u32 xor;
+	int this_netmatch;
+	int best_netmatch;
+	int best_npeers;
 
 	/* CAVEAT EMPTOR: We do all our interface matching with an
 	 * exclusive hold of global lock at IRQ priority.  We're only
@@ -846,19 +846,19 @@ static void
 ksocknal_create_routes(ksock_peer_t *peer, int port,
 		       __u32 *peer_ipaddrs, int npeer_ipaddrs)
 {
-	ksock_route_t       *newroute = NULL;
-	rwlock_t		*global_lock = &ksocknal_data.ksnd_global_lock;
-	lnet_ni_t	   *ni = peer->ksnp_ni;
-	ksock_net_t	 *net = ni->ni_data;
-	struct list_head	  *rtmp;
-	ksock_route_t       *route;
-	ksock_interface_t   *iface;
-	ksock_interface_t   *best_iface;
-	int		  best_netmatch;
-	int		  this_netmatch;
-	int		  best_nroutes;
-	int		  i;
-	int		  j;
+	ksock_route_t *newroute = NULL;
+	rwlock_t *global_lock = &ksocknal_data.ksnd_global_lock;
+	lnet_ni_t *ni = peer->ksnp_ni;
+	ksock_net_t *net = ni->ni_data;
+	struct list_head *rtmp;
+	ksock_route_t *route;
+	ksock_interface_t *iface;
+	ksock_interface_t *best_iface;
+	int best_netmatch;
+	int this_netmatch;
+	int best_nroutes;
+	int i;
+	int j;
 
 	/* CAVEAT EMPTOR: We do all our interface matching with an
 	 * exclusive hold of global lock at IRQ priority.  We're only
@@ -963,12 +963,12 @@ ksocknal_create_routes(ksock_peer_t *peer, int port,
 int
 ksocknal_accept(lnet_ni_t *ni, struct socket *sock)
 {
-	ksock_connreq_t    *cr;
-	int		 rc;
-	__u32	       peer_ip;
-	int		 peer_port;
+	ksock_connreq_t *cr;
+	int rc;
+	__u32 peer_ip;
+	int peer_port;
 
-	rc = libcfs_sock_getaddr(sock, 1, &peer_ip, &peer_port);
+	rc = lnet_sock_getaddr(sock, 1, &peer_ip, &peer_port);
 	LASSERT(rc == 0);		      /* we succeeded before */
 
 	LIBCFS_ALLOC(cr, sizeof(*cr));
@@ -994,7 +994,7 @@ ksocknal_accept(lnet_ni_t *ni, struct socket *sock)
 static int
 ksocknal_connecting(ksock_peer_t *peer, __u32 ipaddr)
 {
-	ksock_route_t   *route;
+	ksock_route_t *route;
 
 	list_for_each_entry(route, &peer->ksnp_routes, ksnr_list) {
 
@@ -1008,23 +1008,23 @@ int
 ksocknal_create_conn(lnet_ni_t *ni, ksock_route_t *route,
 		      struct socket *sock, int type)
 {
-	rwlock_t		*global_lock = &ksocknal_data.ksnd_global_lock;
+	rwlock_t *global_lock = &ksocknal_data.ksnd_global_lock;
 	LIST_HEAD(zombies);
-	lnet_process_id_t  peerid;
-	struct list_head	*tmp;
-	__u64	      incarnation;
-	ksock_conn_t      *conn;
-	ksock_conn_t      *conn2;
-	ksock_peer_t      *peer = NULL;
-	ksock_peer_t      *peer2;
-	ksock_sched_t     *sched;
+	lnet_process_id_t peerid;
+	struct list_head *tmp;
+	__u64 incarnation;
+	ksock_conn_t *conn;
+	ksock_conn_t *conn2;
+	ksock_peer_t *peer = NULL;
+	ksock_peer_t *peer2;
+	ksock_sched_t *sched;
 	ksock_hello_msg_t *hello;
-	int		   cpt;
-	ksock_tx_t	*tx;
-	ksock_tx_t	*txtmp;
-	int		rc;
-	int		active;
-	char	      *warn = NULL;
+	int cpt;
+	ksock_tx_t *tx;
+	ksock_tx_t *txtmp;
+	int rc;
+	int active;
+	char *warn = NULL;
 
 	active = (route != NULL);
 
@@ -1378,15 +1378,15 @@ ksocknal_create_conn(lnet_ni_t *ni, ksock_route_t *route,
 	ksocknal_txlist_done(ni, &zombies, 1);
 	ksocknal_peer_decref(peer);
 
- failed_1:
+failed_1:
 	if (hello != NULL)
 		LIBCFS_FREE(hello, offsetof(ksock_hello_msg_t,
 					    kshm_ips[LNET_MAX_INTERFACES]));
 
 	LIBCFS_FREE(conn, sizeof(*conn));
 
- failed_0:
-	libcfs_sock_release(sock);
+failed_0:
+	sock_release(sock);
 	return rc;
 }
 
@@ -1396,10 +1396,10 @@ ksocknal_close_conn_locked(ksock_conn_t *conn, int error)
 	/* This just does the immmediate housekeeping, and queues the
 	 * connection for the reaper to terminate.
 	 * Caller holds ksnd_global_lock exclusively in irq context */
-	ksock_peer_t      *peer = conn->ksnc_peer;
-	ksock_route_t     *route;
-	ksock_conn_t      *conn2;
-	struct list_head	*tmp;
+	ksock_peer_t *peer = conn->ksnc_peer;
+	ksock_route_t *route;
+	ksock_conn_t *conn2;
+	struct list_head *tmp;
 
 	LASSERT(peer->ksnp_error == 0);
 	LASSERT(!conn->ksnc_closing);
@@ -1479,7 +1479,7 @@ ksocknal_close_conn_locked(ksock_conn_t *conn, int error)
 void
 ksocknal_peer_failed(ksock_peer_t *peer)
 {
-	int	notify = 0;
+	int notify = 0;
 	unsigned long last_alive = 0;
 
 	/* There has been a connection failure or comms error; but I'll only
@@ -1506,9 +1506,9 @@ ksocknal_peer_failed(ksock_peer_t *peer)
 void
 ksocknal_finalize_zcreq(ksock_conn_t *conn)
 {
-	ksock_peer_t     *peer = conn->ksnc_peer;
-	ksock_tx_t       *tx;
-	ksock_tx_t       *tmp;
+	ksock_peer_t *peer = conn->ksnc_peer;
+	ksock_tx_t *tx;
+	ksock_tx_t *tmp;
 	LIST_HEAD(zlist);
 
 	/* NB safe to finalize TXs because closing of socket will
@@ -1546,9 +1546,9 @@ ksocknal_terminate_conn(ksock_conn_t *conn)
 	 * disengage the socket from its callbacks and close it.
 	 * ksnc_refcount will eventually hit zero, and then the reaper will
 	 * destroy it. */
-	ksock_peer_t     *peer = conn->ksnc_peer;
-	ksock_sched_t    *sched = conn->ksnc_scheduler;
-	int	       failed = 0;
+	ksock_peer_t *peer = conn->ksnc_peer;
+	ksock_sched_t *sched = conn->ksnc_scheduler;
+	int failed = 0;
 
 	LASSERT(conn->ksnc_closing);
 
@@ -1617,7 +1617,7 @@ ksocknal_queue_zombie_conn(ksock_conn_t *conn)
 void
 ksocknal_destroy_conn(ksock_conn_t *conn)
 {
-	unsigned long      last_rcv;
+	unsigned long last_rcv;
 
 	/* Final coup-de-grace of the reaper */
 	CDEBUG(D_NET, "connection %p\n", conn);
@@ -1677,10 +1677,10 @@ ksocknal_destroy_conn(ksock_conn_t *conn)
 int
 ksocknal_close_peer_conns_locked(ksock_peer_t *peer, __u32 ipaddr, int why)
 {
-	ksock_conn_t       *conn;
-	struct list_head	 *ctmp;
-	struct list_head	 *cnxt;
-	int		 count = 0;
+	ksock_conn_t *conn;
+	struct list_head *ctmp;
+	struct list_head *cnxt;
+	int count = 0;
 
 	list_for_each_safe(ctmp, cnxt, &peer->ksnp_conns) {
 		conn = list_entry(ctmp, ksock_conn_t, ksnc_list);
@@ -1698,9 +1698,9 @@ ksocknal_close_peer_conns_locked(ksock_peer_t *peer, __u32 ipaddr, int why)
 int
 ksocknal_close_conn_and_siblings(ksock_conn_t *conn, int why)
 {
-	ksock_peer_t     *peer = conn->ksnc_peer;
-	__u32	     ipaddr = conn->ksnc_ipaddr;
-	int	       count;
+	ksock_peer_t *peer = conn->ksnc_peer;
+	__u32 ipaddr = conn->ksnc_ipaddr;
+	int count;
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
@@ -1714,13 +1714,13 @@ ksocknal_close_conn_and_siblings(ksock_conn_t *conn, int why)
 int
 ksocknal_close_matching_conns(lnet_process_id_t id, __u32 ipaddr)
 {
-	ksock_peer_t       *peer;
-	struct list_head	 *ptmp;
-	struct list_head	 *pnxt;
-	int		 lo;
-	int		 hi;
-	int		 i;
-	int		 count = 0;
+	ksock_peer_t *peer;
+	struct list_head *ptmp;
+	struct list_head *pnxt;
+	int lo;
+	int hi;
+	int i;
+	int count = 0;
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
@@ -1762,7 +1762,7 @@ ksocknal_notify(lnet_ni_t *ni, lnet_nid_t gw_nid, int alive)
 {
 	/* The router is telling me she's been notified of a change in
 	 * gateway state.... */
-	lnet_process_id_t  id = {0};
+	lnet_process_id_t id = {0};
 
 	id.nid = gw_nid;
 	id.pid = LNET_PID_ANY;
@@ -1783,20 +1783,20 @@ ksocknal_notify(lnet_ni_t *ni, lnet_nid_t gw_nid, int alive)
 void
 ksocknal_query(lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 {
-	int		connect = 1;
-	unsigned long	 last_alive = 0;
-	unsigned long	 now = cfs_time_current();
-	ksock_peer_t      *peer = NULL;
-	rwlock_t		*glock = &ksocknal_data.ksnd_global_lock;
-	lnet_process_id_t  id = {.nid = nid, .pid = LUSTRE_SRV_LNET_PID};
+	int connect = 1;
+	unsigned long last_alive = 0;
+	unsigned long now = cfs_time_current();
+	ksock_peer_t *peer = NULL;
+	rwlock_t *glock = &ksocknal_data.ksnd_global_lock;
+	lnet_process_id_t id = {.nid = nid, .pid = LUSTRE_SRV_LNET_PID};
 
 	read_lock(glock);
 
 	peer = ksocknal_find_peer_locked(ni, id);
 	if (peer != NULL) {
-		struct list_head       *tmp;
-		ksock_conn_t     *conn;
-		int	       bufnob;
+		struct list_head *tmp;
+		ksock_conn_t *conn;
+		int bufnob;
 
 		list_for_each(tmp, &peer->ksnp_conns) {
 			conn = list_entry(tmp, ksock_conn_t, ksnc_list);
@@ -1844,10 +1844,10 @@ ksocknal_query(lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
 static void
 ksocknal_push_peer(ksock_peer_t *peer)
 {
-	int	       index;
-	int	       i;
-	struct list_head       *tmp;
-	ksock_conn_t     *conn;
+	int index;
+	int i;
+	struct list_head *tmp;
+	ksock_conn_t *conn;
 
 	for (index = 0; ; index++) {
 		read_lock(&ksocknal_data.ksnd_global_lock);
@@ -1877,12 +1877,12 @@ ksocknal_push_peer(ksock_peer_t *peer)
 static int
 ksocknal_push(lnet_ni_t *ni, lnet_process_id_t id)
 {
-	ksock_peer_t      *peer;
-	struct list_head	*tmp;
-	int		index;
-	int		i;
-	int		j;
-	int		rc = -ENOENT;
+	ksock_peer_t *peer;
+	struct list_head *tmp;
+	int index;
+	int i;
+	int j;
+	int rc = -ENOENT;
 
 	for (i = 0; i < ksocknal_data.ksnd_peer_hash_size; i++) {
 		for (j = 0; ; j++) {
@@ -1926,15 +1926,15 @@ ksocknal_push(lnet_ni_t *ni, lnet_process_id_t id)
 static int
 ksocknal_add_interface(lnet_ni_t *ni, __u32 ipaddress, __u32 netmask)
 {
-	ksock_net_t       *net = ni->ni_data;
+	ksock_net_t *net = ni->ni_data;
 	ksock_interface_t *iface;
-	int		rc;
-	int		i;
-	int		j;
-	struct list_head	*ptmp;
-	ksock_peer_t      *peer;
-	struct list_head	*rtmp;
-	ksock_route_t     *route;
+	int rc;
+	int i;
+	int j;
+	struct list_head *ptmp;
+	ksock_peer_t *peer;
+	struct list_head *rtmp;
+	ksock_route_t *route;
 
 	if (ipaddress == 0 ||
 	    netmask == 0)
@@ -1988,12 +1988,12 @@ ksocknal_add_interface(lnet_ni_t *ni, __u32 ipaddress, __u32 netmask)
 static void
 ksocknal_peer_del_interface_locked(ksock_peer_t *peer, __u32 ipaddr)
 {
-	struct list_head	 *tmp;
-	struct list_head	 *nxt;
-	ksock_route_t      *route;
-	ksock_conn_t       *conn;
-	int		 i;
-	int		 j;
+	struct list_head *tmp;
+	struct list_head *nxt;
+	ksock_route_t *route;
+	ksock_conn_t *conn;
+	int i;
+	int j;
 
 	for (i = 0; i < peer->ksnp_n_passive_ips; i++)
 		if (peer->ksnp_passive_ips[i] == ipaddr) {
@@ -2029,14 +2029,14 @@ ksocknal_peer_del_interface_locked(ksock_peer_t *peer, __u32 ipaddr)
 static int
 ksocknal_del_interface(lnet_ni_t *ni, __u32 ipaddress)
 {
-	ksock_net_t       *net = ni->ni_data;
-	int		rc = -ENOENT;
-	struct list_head	*tmp;
-	struct list_head	*nxt;
-	ksock_peer_t      *peer;
-	__u32	      this_ip;
-	int		i;
-	int		j;
+	ksock_net_t *net = ni->ni_data;
+	int rc = -ENOENT;
+	struct list_head *tmp;
+	struct list_head *nxt;
+	ksock_peer_t *peer;
+	__u32 this_ip;
+	int i;
+	int j;
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
@@ -2114,11 +2114,11 @@ ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 					      data->ioc_u32[0]); /* IP address */
 
 	case IOC_LIBCFS_GET_PEER: {
-		__u32	    myip = 0;
-		__u32	    ip = 0;
-		int	      port = 0;
-		int	      conn_count = 0;
-		int	      share_count = 0;
+		__u32 myip = 0;
+		__u32 ip = 0;
+		int port = 0;
+		int conn_count = 0;
+		int share_count = 0;
 
 		rc = ksocknal_get_peer_info(ni, data->ioc_count,
 					    &id, &myip, &ip, &port,
@@ -2150,9 +2150,9 @@ ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 					  data->ioc_u32[0]); /* IP */
 
 	case IOC_LIBCFS_GET_CONN: {
-		int	   txmem;
-		int	   rxmem;
-		int	   nagle;
+		int txmem;
+		int rxmem;
+		int nagle;
 		ksock_conn_t *conn = ksocknal_get_conn_by_idx(ni, data->ioc_count);
 
 		if (conn == NULL)
@@ -2207,8 +2207,8 @@ ksocknal_free_buffers(void)
 	LASSERT(atomic_read(&ksocknal_data.ksnd_nactive_txs) == 0);
 
 	if (ksocknal_data.ksnd_sched_info != NULL) {
-		struct ksock_sched_info	*info;
-		int			i;
+		struct ksock_sched_info *info;
+		int i;
 
 		cfs_percpt_for_each(info, i, ksocknal_data.ksnd_sched_info) {
 			if (info->ksi_scheds != NULL) {
@@ -2227,8 +2227,8 @@ ksocknal_free_buffers(void)
 	spin_lock(&ksocknal_data.ksnd_tx_lock);
 
 	if (!list_empty(&ksocknal_data.ksnd_idle_noop_txs)) {
-		struct list_head	zlist;
-		ksock_tx_t	*tx;
+		struct list_head zlist;
+		ksock_tx_t *tx;
 
 		list_add(&zlist, &ksocknal_data.ksnd_idle_noop_txs);
 		list_del_init(&ksocknal_data.ksnd_idle_noop_txs);
@@ -2248,9 +2248,9 @@ static void
 ksocknal_base_shutdown(void)
 {
 	struct ksock_sched_info *info;
-	ksock_sched_t		*sched;
-	int			i;
-	int			j;
+	ksock_sched_t *sched;
+	int i;
+	int j;
 
 	CDEBUG(D_MALLOC, "before NAL cleanup: kmem %d\n",
 	       atomic_read(&libcfs_kmemory));
@@ -2351,8 +2351,8 @@ static int
 ksocknal_base_startup(void)
 {
 	struct ksock_sched_info	*info;
-	int			rc;
-	int			i;
+	int rc;
+	int i;
 
 	LASSERT(ksocknal_data.ksnd_init == SOCKNAL_INIT_NOTHING);
 	LASSERT(ksocknal_data.ksnd_nnets == 0);
@@ -2398,8 +2398,8 @@ ksocknal_base_startup(void)
 		goto failed;
 
 	cfs_percpt_for_each(info, i, ksocknal_data.ksnd_sched_info) {
-		ksock_sched_t	*sched;
-		int		nthrs;
+		ksock_sched_t *sched;
+		int nthrs;
 
 		nthrs = cfs_cpt_weight(lnet_cpt_table(), i);
 		if (*ksocknal_tunables.ksnd_nscheds > 0) {
@@ -2430,9 +2430,9 @@ ksocknal_base_startup(void)
 		}
 	}
 
-	ksocknal_data.ksnd_connd_starting	 = 0;
-	ksocknal_data.ksnd_connd_failed_stamp     = 0;
-	ksocknal_data.ksnd_connd_starting_stamp   = get_seconds();
+	ksocknal_data.ksnd_connd_starting       = 0;
+	ksocknal_data.ksnd_connd_failed_stamp   = 0;
+	ksocknal_data.ksnd_connd_starting_stamp = get_seconds();
 	/* must have at least 2 connds to remain responsive to accepts while
 	 * connecting */
 	if (*ksocknal_tunables.ksnd_nconnds < SOCKNAL_CONND_RESV + 1)
@@ -2482,9 +2482,9 @@ ksocknal_base_startup(void)
 static void
 ksocknal_debug_peerhash(lnet_ni_t *ni)
 {
-	ksock_peer_t	*peer = NULL;
-	struct list_head	*tmp;
-	int		i;
+	ksock_peer_t *peer = NULL;
+	struct list_head *tmp;
+	int i;
 
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
@@ -2536,12 +2536,12 @@ ksocknal_debug_peerhash(lnet_ni_t *ni)
 void
 ksocknal_shutdown(lnet_ni_t *ni)
 {
-	ksock_net_t      *net = ni->ni_data;
-	int	       i;
+	ksock_net_t *net = ni->ni_data;
+	int i;
 	lnet_process_id_t anyid = {0};
 
-	anyid.nid =  LNET_NID_ANY;
-	anyid.pid =  LNET_PID_ANY;
+	anyid.nid = LNET_NID_ANY;
+	anyid.pid = LNET_PID_ANY;
 
 	LASSERT(ksocknal_data.ksnd_init == SOCKNAL_INIT_ALL);
 	LASSERT(ksocknal_data.ksnd_nnets > 0);
@@ -2588,27 +2588,27 @@ ksocknal_shutdown(lnet_ni_t *ni)
 static int
 ksocknal_enumerate_interfaces(ksock_net_t *net)
 {
-	char      **names;
-	int	 i;
-	int	 j;
-	int	 rc;
-	int	 n;
+	char **names;
+	int i;
+	int j;
+	int rc;
+	int n;
 
-	n = libcfs_ipif_enumerate(&names);
+	n = lnet_ipif_enumerate(&names);
 	if (n <= 0) {
 		CERROR("Can't enumerate interfaces: %d\n", n);
 		return n;
 	}
 
 	for (i = j = 0; i < n; i++) {
-		int	up;
-		__u32      ip;
-		__u32      mask;
+		int up;
+		__u32 ip;
+		__u32 mask;
 
 		if (!strcmp(names[i], "lo")) /* skip the loopback IF */
 			continue;
 
-		rc = libcfs_ipif_query(names[i], &up, &ip, &mask);
+		rc = lnet_ipif_query(names[i], &up, &ip, &mask);
 		if (rc != 0) {
 			CWARN("Can't get interface %s info: %d\n",
 			      names[i], rc);
@@ -2634,7 +2634,7 @@ ksocknal_enumerate_interfaces(ksock_net_t *net)
 		j++;
 	}
 
-	libcfs_ipif_free_enumeration(names, n);
+	lnet_ipif_free_enumeration(names, n);
 
 	if (j == 0)
 		CERROR("Can't find any usable interfaces\n");
@@ -2645,15 +2645,15 @@ ksocknal_enumerate_interfaces(ksock_net_t *net)
 static int
 ksocknal_search_new_ipif(ksock_net_t *net)
 {
-	int	new_ipif = 0;
-	int	i;
+	int new_ipif = 0;
+	int i;
 
 	for (i = 0; i < net->ksnn_ninterfaces; i++) {
-		char		*ifnam = &net->ksnn_interfaces[i].ksni_name[0];
-		char		*colon = strchr(ifnam, ':');
-		int		found  = 0;
-		ksock_net_t	*tmp;
-		int		j;
+		char *ifnam = &net->ksnn_interfaces[i].ksni_name[0];
+		char *colon = strchr(ifnam, ':');
+		int found  = 0;
+		ksock_net_t *tmp;
+		int j;
 
 		if (colon != NULL) /* ignore alias device */
 			*colon = 0;
@@ -2687,9 +2687,9 @@ ksocknal_search_new_ipif(ksock_net_t *net)
 static int
 ksocknal_start_schedulers(struct ksock_sched_info *info)
 {
-	int	nthrs;
-	int	rc = 0;
-	int	i;
+	int nthrs;
+	int rc = 0;
+	int i;
 
 	if (info->ksi_nthreads == 0) {
 		if (*ksocknal_tunables.ksnd_nscheds > 0) {
@@ -2708,9 +2708,9 @@ ksocknal_start_schedulers(struct ksock_sched_info *info)
 	}
 
 	for (i = 0; i < nthrs; i++) {
-		long		id;
-		char		name[20];
-		ksock_sched_t	*sched;
+		long id;
+		char name[20];
+		ksock_sched_t *sched;
 		id = KSOCK_THREAD_ID(info->ksi_cpt, info->ksi_nthreads + i);
 		sched = &info->ksi_scheds[KSOCK_THREAD_SID(id)];
 		snprintf(name, sizeof(name), "socknal_sd%02d_%02d",
@@ -2733,14 +2733,14 @@ ksocknal_start_schedulers(struct ksock_sched_info *info)
 static int
 ksocknal_net_start_threads(ksock_net_t *net, __u32 *cpts, int ncpts)
 {
-	int	newif = ksocknal_search_new_ipif(net);
-	int	rc;
-	int	i;
+	int newif = ksocknal_search_new_ipif(net);
+	int rc;
+	int i;
 
 	LASSERT(ncpts > 0 && ncpts <= cfs_cpt_number(lnet_cpt_table()));
 
 	for (i = 0; i < ncpts; i++) {
-		struct ksock_sched_info	*info;
+		struct ksock_sched_info *info;
 		int cpt = (cpts == NULL) ? i : cpts[i];
 
 		LASSERT(cpt < cfs_cpt_number(lnet_cpt_table()));
@@ -2759,9 +2759,9 @@ ksocknal_net_start_threads(ksock_net_t *net, __u32 *cpts, int ncpts)
 int
 ksocknal_startup(lnet_ni_t *ni)
 {
-	ksock_net_t  *net;
-	int	   rc;
-	int	   i;
+	ksock_net_t *net;
+	int rc;
+	int i;
 
 	LASSERT(ni->ni_lnd == &the_ksocklnd);
 
@@ -2791,13 +2791,12 @@ ksocknal_startup(lnet_ni_t *ni)
 		net->ksnn_ninterfaces = 1;
 	} else {
 		for (i = 0; i < LNET_MAX_INTERFACES; i++) {
-			int    up;
+			int up;
 
 			if (ni->ni_interfaces[i] == NULL)
 				break;
 
-			rc = libcfs_ipif_query(
-				ni->ni_interfaces[i], &up,
+			rc = lnet_ipif_query(ni->ni_interfaces[i], &up,
 				&net->ksnn_interfaces[i].ksni_ipaddr,
 				&net->ksnn_interfaces[i].ksni_netmask);
 
@@ -2851,7 +2850,7 @@ ksocknal_module_fini(void)
 static int __init
 ksocknal_module_init(void)
 {
-	int    rc;
+	int rc;
 
 	/* check ksnr_connected/connecting field large enough */
 	CLASSERT(SOCKLND_CONN_NTYPES <= 4);
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
index c54c9955164e..7125eb955ae5 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
@@ -25,26 +25,49 @@
  *
  */
 
+#ifndef _SOCKLND_SOCKLND_H_
+#define _SOCKLND_SOCKLND_H_
+
 #define DEBUG_PORTAL_ALLOC
 #define DEBUG_SUBSYSTEM S_LND
 
-#include "socklnd_lib-linux.h"
+#include <asm/irq.h>
+#include <linux/crc32.h>
+#include <linux/errno.h>
+#include <linux/if.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/kmod.h>
+#include <linux/list.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/stat.h>
+#include <linux/string.h>
+#include <linux/syscalls.h>
+#include <linux/sysctl.h>
+#include <linux/uio.h>
+#include <linux/unistd.h>
+#include <net/sock.h>
+#include <net/tcp.h>
 
 #include "../../../include/linux/libcfs/libcfs.h"
 #include "../../../include/linux/lnet/lnet.h"
 #include "../../../include/linux/lnet/lib-lnet.h"
 #include "../../../include/linux/lnet/socklnd.h"
-#include "../../../include/linux/lnet/lnet-sysctl.h"
 
-#define SOCKNAL_PEER_HASH_SIZE  101	     /* # peer lists */
-#define SOCKNAL_RESCHED	 100	     /* # scheduler loops before reschedule */
-#define SOCKNAL_INSANITY_RECONN 5000	    /* connd is trying on reconn infinitely */
-#define SOCKNAL_ENOMEM_RETRY    CFS_TICK	/* jiffies between retries */
+/* assume one thread for each connection type */
+#define SOCKNAL_NSCHEDS		3
+#define SOCKNAL_NSCHEDS_HIGH	(SOCKNAL_NSCHEDS << 1)
+
+#define SOCKNAL_PEER_HASH_SIZE  101   /* # peer lists */
+#define SOCKNAL_RESCHED         100   /* # scheduler loops before reschedule */
+#define SOCKNAL_INSANITY_RECONN 5000  /* connd is trying on reconn infinitely */
+#define SOCKNAL_ENOMEM_RETRY    CFS_TICK /* jiffies between retries */
 
-#define SOCKNAL_SINGLE_FRAG_TX      0	   /* disable multi-fragment sends */
-#define SOCKNAL_SINGLE_FRAG_RX      0	   /* disable multi-fragment receives */
+#define SOCKNAL_SINGLE_FRAG_TX  0     /* disable multi-fragment sends */
+#define SOCKNAL_SINGLE_FRAG_RX  0     /* disable multi-fragment receives */
 
-#define SOCKNAL_VERSION_DEBUG       0	   /* enable protocol version debugging */
+#define SOCKNAL_VERSION_DEBUG   0     /* enable protocol version debugging */
 
 /* risk kmap deadlock on multi-frag I/O (backs off to single-frag if disabled).
  * no risk if we're not running on a CONFIG_HIGHMEM platform. */
@@ -58,33 +81,31 @@ struct ksock_sched_info;
 
 typedef struct				  /* per scheduler state */
 {
-	spinlock_t		kss_lock;	/* serialise */
-	struct list_head		kss_rx_conns;	/* conn waiting to be read */
-	/* conn waiting to be written */
-	struct list_head		kss_tx_conns;
-	/* zombie noop tx list */
-	struct list_head		kss_zombie_noop_txs;
-	wait_queue_head_t		kss_waitq;	/* where scheduler sleeps */
-	/* # connections assigned to this scheduler */
-	int			kss_nconns;
-	struct ksock_sched_info	*kss_info;	/* owner of it */
-	struct page		*kss_rx_scratch_pgs[LNET_MAX_IOV];
-	struct kvec		kss_scratch_iov[LNET_MAX_IOV];
+	spinlock_t              kss_lock;       /* serialise */
+	struct list_head        kss_rx_conns;   /* conn waiting to be read */
+	struct list_head        kss_tx_conns;   /* conn waiting to be written */
+	struct list_head        kss_zombie_noop_txs; /* zombie noop tx list */
+	wait_queue_head_t       kss_waitq;	/* where scheduler sleeps */
+	int                     kss_nconns;     /* # connections assigned to
+						 * this scheduler */
+	struct ksock_sched_info *kss_info;	/* owner of it */
+	struct page             *kss_rx_scratch_pgs[LNET_MAX_IOV];
+	struct kvec             kss_scratch_iov[LNET_MAX_IOV];
 } ksock_sched_t;
 
 struct ksock_sched_info {
-	int			ksi_nthreads_max; /* max allowed threads */
-	int			ksi_nthreads;	/* number of threads */
-	int			ksi_cpt;	/* CPT id */
-	ksock_sched_t		*ksi_scheds;	/* array of schedulers */
+	int                     ksi_nthreads_max; /* max allowed threads */
+	int                     ksi_nthreads;     /* number of threads */
+	int                     ksi_cpt;          /* CPT id */
+	ksock_sched_t           *ksi_scheds;      /* array of schedulers */
 };
 
-#define KSOCK_CPT_SHIFT			16
-#define KSOCK_THREAD_ID(cpt, sid)	(((cpt) << KSOCK_CPT_SHIFT) | (sid))
-#define KSOCK_THREAD_CPT(id)		((id) >> KSOCK_CPT_SHIFT)
-#define KSOCK_THREAD_SID(id)		((id) & ((1UL << KSOCK_CPT_SHIFT) - 1))
+#define KSOCK_CPT_SHIFT           16
+#define KSOCK_THREAD_ID(cpt, sid) (((cpt) << KSOCK_CPT_SHIFT) | (sid))
+#define KSOCK_THREAD_CPT(id)      ((id) >> KSOCK_CPT_SHIFT)
+#define KSOCK_THREAD_SID(id)      ((id) & ((1UL << KSOCK_CPT_SHIFT) - 1))
 
-typedef struct				  /* in-use interface */
+typedef struct                                  /* in-use interface */
 {
 	__u32		ksni_ipaddr;		/* interface's IP address */
 	__u32		ksni_netmask;		/* interface's network mask */
@@ -94,35 +115,48 @@ typedef struct				  /* in-use interface */
 } ksock_interface_t;
 
 typedef struct {
-	/* "stuck" socket timeout (seconds) */
-	int	      *ksnd_timeout;
-	/* # scheduler threads in each pool while starting */
-	int		 *ksnd_nscheds;
-	int	      *ksnd_nconnds;	 /* # connection daemons */
-	int	      *ksnd_nconnds_max;     /* max # connection daemons */
-	int	      *ksnd_min_reconnectms; /* first connection retry after (ms)... */
-	int	      *ksnd_max_reconnectms; /* ...exponentially increasing to this */
-	int	      *ksnd_eager_ack;       /* make TCP ack eagerly? */
-	int	      *ksnd_typed_conns;     /* drive sockets by type? */
-	int	      *ksnd_min_bulk;	/* smallest "large" message */
-	int	      *ksnd_tx_buffer_size;  /* socket tx buffer size */
-	int	      *ksnd_rx_buffer_size;  /* socket rx buffer size */
-	int	      *ksnd_nagle;	   /* enable NAGLE? */
-	int	      *ksnd_round_robin;     /* round robin for multiple interfaces */
-	int	      *ksnd_keepalive;       /* # secs for sending keepalive NOOP */
-	int	      *ksnd_keepalive_idle;  /* # idle secs before 1st probe */
-	int	      *ksnd_keepalive_count; /* # probes */
-	int	      *ksnd_keepalive_intvl; /* time between probes */
-	int	      *ksnd_credits;	 /* # concurrent sends */
-	int	      *ksnd_peertxcredits;   /* # concurrent sends to 1 peer */
-	int	      *ksnd_peerrtrcredits;  /* # per-peer router buffer credits */
-	int	      *ksnd_peertimeout;     /* seconds to consider peer dead */
-	int	      *ksnd_enable_csum;     /* enable check sum */
-	int	      *ksnd_inject_csum_error; /* set non-zero to inject checksum error */
-	int	      *ksnd_nonblk_zcack;    /* always send zc-ack on non-blocking connection */
-	unsigned int     *ksnd_zc_min_payload;  /* minimum zero copy payload size */
-	int	      *ksnd_zc_recv;	 /* enable ZC receive (for Chelsio TOE) */
-	int	      *ksnd_zc_recv_min_nfrags; /* minimum # of fragments to enable ZC receive */
+	int          *ksnd_timeout;            /* "stuck" socket timeout
+						* (seconds) */
+	int          *ksnd_nscheds;            /* # scheduler threads in each
+						* pool while starting */
+	int          *ksnd_nconnds;            /* # connection daemons */
+	int          *ksnd_nconnds_max;        /* max # connection daemons */
+	int          *ksnd_min_reconnectms;    /* first connection retry after
+						* (ms)... */
+	int          *ksnd_max_reconnectms;    /* ...exponentially increasing to
+						* this */
+	int          *ksnd_eager_ack;          /* make TCP ack eagerly? */
+	int          *ksnd_typed_conns;        /* drive sockets by type? */
+	int          *ksnd_min_bulk;           /* smallest "large" message */
+	int          *ksnd_tx_buffer_size;     /* socket tx buffer size */
+	int          *ksnd_rx_buffer_size;     /* socket rx buffer size */
+	int          *ksnd_nagle;              /* enable NAGLE? */
+	int          *ksnd_round_robin;        /* round robin for multiple
+						* interfaces */
+	int          *ksnd_keepalive;          /* # secs for sending keepalive
+						* NOOP */
+	int          *ksnd_keepalive_idle;     /* # idle secs before 1st probe
+						*/
+	int          *ksnd_keepalive_count;    /* # probes */
+	int          *ksnd_keepalive_intvl;    /* time between probes */
+	int          *ksnd_credits;            /* # concurrent sends */
+	int          *ksnd_peertxcredits;      /* # concurrent sends to 1 peer
+						*/
+	int          *ksnd_peerrtrcredits;     /* # per-peer router buffer
+						* credits */
+	int          *ksnd_peertimeout;        /* seconds to consider peer dead
+						*/
+	int          *ksnd_enable_csum;        /* enable check sum */
+	int          *ksnd_inject_csum_error;  /* set non-zero to inject
+						* checksum error */
+	int          *ksnd_nonblk_zcack;       /* always send zc-ack on
+						* non-blocking connection */
+	unsigned int *ksnd_zc_min_payload;     /* minimum zero copy payload
+						* size */
+	int          *ksnd_zc_recv;            /* enable ZC receive (for
+						* Chelsio TOE) */
+	int          *ksnd_zc_recv_min_nfrags; /* minimum # of fragments to
+						* enable ZC receive */
 } ksock_tunables_t;
 
 typedef struct {
@@ -141,55 +175,67 @@ typedef struct {
 #define SOCKNAL_CONND_RESV     1
 
 typedef struct {
-	int			ksnd_init;	/* initialisation state */
-	int			ksnd_nnets;	/* # networks set up */
-	struct list_head		ksnd_nets;	/* list of nets */
-	/* stabilize peer/conn ops */
-	rwlock_t		ksnd_global_lock;
-	/* hash table of all my known peers */
-	struct list_head		*ksnd_peers;
-	int			ksnd_peer_hash_size; /* size of ksnd_peers */
-
-	int			ksnd_nthreads;	/* # live threads */
-	int			ksnd_shuttingdown; /* tell threads to exit */
-	/* schedulers information */
-	struct ksock_sched_info	**ksnd_sched_info;
-
-	atomic_t      ksnd_nactive_txs;    /* #active txs */
-
-	struct list_head	ksnd_deathrow_conns; /* conns to close: reaper_lock*/
-	struct list_head	ksnd_zombie_conns;   /* conns to free: reaper_lock */
-	struct list_head	ksnd_enomem_conns;   /* conns to retry: reaper_lock*/
-	wait_queue_head_t       ksnd_reaper_waitq;   /* reaper sleeps here */
-	unsigned long	ksnd_reaper_waketime;/* when reaper will wake */
-	spinlock_t	  ksnd_reaper_lock;	/* serialise */
-
-	int	       ksnd_enomem_tx;      /* test ENOMEM sender */
-	int	       ksnd_stall_tx;       /* test sluggish sender */
-	int	       ksnd_stall_rx;       /* test sluggish receiver */
-
-	struct list_head	ksnd_connd_connreqs; /* incoming connection requests */
-	struct list_head	ksnd_connd_routes;   /* routes waiting to be connected */
-	wait_queue_head_t       ksnd_connd_waitq;    /* connds sleep here */
-	int	       ksnd_connd_connecting;/* # connds connecting */
-	/** time stamp of the last failed connecting attempt */
-	long	      ksnd_connd_failed_stamp;
-	/** # starting connd */
-	unsigned	  ksnd_connd_starting;
-	/** time stamp of the last starting connd */
-	long	      ksnd_connd_starting_stamp;
-	/** # running connd */
-	unsigned	  ksnd_connd_running;
-	spinlock_t	  ksnd_connd_lock;	/* serialise */
-
-	struct list_head	  ksnd_idle_noop_txs;	/* list head for freed noop tx */
-	spinlock_t	  ksnd_tx_lock;		/* serialise, g_lock unsafe */
+	int                     ksnd_init;              /* initialisation state
+							 */
+	int                     ksnd_nnets;             /* # networks set up */
+	struct list_head        ksnd_nets;              /* list of nets */
+	rwlock_t                ksnd_global_lock;       /* stabilize peer/conn
+							 * ops */
+	struct list_head        *ksnd_peers;            /* hash table of all my
+							 * known peers */
+	int                     ksnd_peer_hash_size;    /* size of ksnd_peers */
+
+	int                     ksnd_nthreads;          /* # live threads */
+	int                     ksnd_shuttingdown;      /* tell threads to exit
+							 */
+	struct ksock_sched_info **ksnd_sched_info;      /* schedulers info */
+
+	atomic_t                ksnd_nactive_txs;       /* #active txs */
+
+	struct list_head        ksnd_deathrow_conns;    /* conns to close:
+							 * reaper_lock*/
+	struct list_head        ksnd_zombie_conns;      /* conns to free:
+							 * reaper_lock */
+	struct list_head        ksnd_enomem_conns;      /* conns to retry:
+							 * reaper_lock*/
+	wait_queue_head_t       ksnd_reaper_waitq;      /* reaper sleeps here */
+	unsigned long	        ksnd_reaper_waketime;   /* when reaper will wake
+							 */
+	spinlock_t              ksnd_reaper_lock;       /* serialise */
+
+	int                     ksnd_enomem_tx;         /* test ENOMEM sender */
+	int                     ksnd_stall_tx;          /* test sluggish sender
+							 */
+	int                     ksnd_stall_rx;          /* test sluggish
+							 * receiver */
+
+	struct list_head        ksnd_connd_connreqs;    /* incoming connection
+							 * requests */
+	struct list_head        ksnd_connd_routes;      /* routes waiting to be
+							 * connected */
+	wait_queue_head_t       ksnd_connd_waitq;       /* connds sleep here */
+	int                     ksnd_connd_connecting;  /* # connds connecting
+							 */
+	long                    ksnd_connd_failed_stamp;/* time stamp of the
+							 * last failed
+							 * connecting attempt */
+	unsigned                ksnd_connd_starting;    /* # starting connd */
+	long                    ksnd_connd_starting_stamp;/* time stamp of the
+							   * last starting connd
+							   */
+	unsigned                ksnd_connd_running;     /* # running connd */
+	spinlock_t              ksnd_connd_lock;        /* serialise */
+
+	struct list_head        ksnd_idle_noop_txs;     /* list head for freed
+							 * noop tx */
+	spinlock_t              ksnd_tx_lock;           /* serialise, g_lock
+							 * unsafe */
 
 } ksock_nal_data_t;
 
-#define SOCKNAL_INIT_NOTHING    0
-#define SOCKNAL_INIT_DATA       1
-#define SOCKNAL_INIT_ALL	2
+#define SOCKNAL_INIT_NOTHING 0
+#define SOCKNAL_INIT_DATA    1
+#define SOCKNAL_INIT_ALL     2
 
 /* A packet just assembled for transmission is represented by 1 or more
  * struct iovec fragments (the first frag contains the portals header),
@@ -200,43 +246,45 @@ typedef struct {
  * received into either struct iovec or lnet_kiov_t fragments, depending on
  * what the header matched or whether the message needs forwarding. */
 
-struct ksock_conn;			      /* forward ref */
-struct ksock_peer;			      /* forward ref */
-struct ksock_route;			     /* forward ref */
-struct ksock_proto;			     /* forward ref */
+struct ksock_conn;  /* forward ref */
+struct ksock_peer;  /* forward ref */
+struct ksock_route; /* forward ref */
+struct ksock_proto; /* forward ref */
 
-typedef struct				  /* transmit packet */
+typedef struct                             /* transmit packet */
 {
-	struct list_head     tx_list;	/* queue on conn for transmission etc */
-	struct list_head     tx_zc_list;     /* queue on peer for ZC request */
-	atomic_t   tx_refcount;    /* tx reference count */
-	int	    tx_nob;	 /* # packet bytes */
-	int	    tx_resid;       /* residual bytes */
-	int	    tx_niov;	/* # packet iovec frags */
-	struct kvec  *tx_iov;	 /* packet iovec frags */
-	int	    tx_nkiov;       /* # packet page frags */
-	unsigned short tx_zc_aborted;  /* aborted ZC request */
-	unsigned short tx_zc_capable:1; /* payload is large enough for ZC */
-	unsigned short tx_zc_checked:1; /* Have I checked if I should ZC? */
-	unsigned short tx_nonblk:1;    /* it's a non-blocking ACK */
-	lnet_kiov_t   *tx_kiov;	/* packet page frags */
-	struct ksock_conn  *tx_conn;	/* owning conn */
-	lnet_msg_t    *tx_lnetmsg;     /* lnet message for lnet_finalize() */
-	unsigned long     tx_deadline;    /* when (in jiffies) tx times out */
-	ksock_msg_t    tx_msg;	 /* socklnd message buffer */
-	int	    tx_desc_size;   /* size of this descriptor */
+	struct list_head  tx_list;         /* queue on conn for transmission etc
+					    */
+	struct list_head  tx_zc_list;      /* queue on peer for ZC request */
+	atomic_t          tx_refcount;     /* tx reference count */
+	int               tx_nob;          /* # packet bytes */
+	int               tx_resid;        /* residual bytes */
+	int               tx_niov;         /* # packet iovec frags */
+	struct kvec       *tx_iov;         /* packet iovec frags */
+	int               tx_nkiov;        /* # packet page frags */
+	unsigned short    tx_zc_aborted;   /* aborted ZC request */
+	unsigned short    tx_zc_capable:1; /* payload is large enough for ZC */
+	unsigned short    tx_zc_checked:1; /* Have I checked if I should ZC? */
+	unsigned short    tx_nonblk:1;     /* it's a non-blocking ACK */
+	lnet_kiov_t       *tx_kiov;        /* packet page frags */
+	struct ksock_conn *tx_conn;        /* owning conn */
+	lnet_msg_t        *tx_lnetmsg;     /* lnet message for lnet_finalize()
+					    */
+	unsigned long     tx_deadline;     /* when (in jiffies) tx times out */
+	ksock_msg_t       tx_msg;          /* socklnd message buffer */
+	int               tx_desc_size;    /* size of this descriptor */
 	union {
 		struct {
-			struct kvec iov;       /* virt hdr */
-			lnet_kiov_t  kiov[0];   /* paged payload */
-		}		  paged;
+			struct kvec iov;     /* virt hdr */
+			lnet_kiov_t kiov[0]; /* paged payload */
+		} paged;
 		struct {
-			struct kvec iov[1];    /* virt hdr + payload */
-		}		  virt;
-	}		       tx_frags;
+			struct kvec iov[1];  /* virt hdr + payload */
+		} virt;
+	} tx_frags;
 } ksock_tx_t;
 
-#define KSOCK_NOOP_TX_SIZE  ((int)offsetof(ksock_tx_t, tx_frags.paged.kiov[0]))
+#define KSOCK_NOOP_TX_SIZE ((int)offsetof(ksock_tx_t, tx_frags.paged.kiov[0]))
 
 /* network zero copy callback descriptor embedded in ksock_tx_t */
 
@@ -247,153 +295,205 @@ typedef union {
 	lnet_kiov_t      kiov[LNET_MAX_IOV];
 } ksock_rxiovspace_t;
 
-#define SOCKNAL_RX_KSM_HEADER   1	       /* reading ksock message header */
-#define SOCKNAL_RX_LNET_HEADER  2	       /* reading lnet message header */
-#define SOCKNAL_RX_PARSE	3	       /* Calling lnet_parse() */
-#define SOCKNAL_RX_PARSE_WAIT   4	       /* waiting to be told to read the body */
-#define SOCKNAL_RX_LNET_PAYLOAD 5	       /* reading lnet payload (to deliver here) */
-#define SOCKNAL_RX_SLOP	 6	       /* skipping body */
+#define SOCKNAL_RX_KSM_HEADER   1 /* reading ksock message header */
+#define SOCKNAL_RX_LNET_HEADER  2 /* reading lnet message header */
+#define SOCKNAL_RX_PARSE        3 /* Calling lnet_parse() */
+#define SOCKNAL_RX_PARSE_WAIT   4 /* waiting to be told to read the body */
+#define SOCKNAL_RX_LNET_PAYLOAD 5 /* reading lnet payload (to deliver here) */
+#define SOCKNAL_RX_SLOP         6 /* skipping body */
 
 typedef struct ksock_conn {
-	struct ksock_peer  *ksnc_peer;	 /* owning peer */
-	struct ksock_route *ksnc_route;	/* owning route */
-	struct list_head	  ksnc_list;	 /* stash on peer's conn list */
-	struct socket       *ksnc_sock;	 /* actual socket */
-	void	       *ksnc_saved_data_ready; /* socket's original data_ready() callback */
-	void	       *ksnc_saved_write_space; /* socket's original write_space() callback */
-	atomic_t	ksnc_conn_refcount; /* conn refcount */
-	atomic_t	ksnc_sock_refcount; /* sock refcount */
-	ksock_sched_t      *ksnc_scheduler;  /* who schedules this connection */
-	__u32	       ksnc_myipaddr;   /* my IP */
-	__u32	       ksnc_ipaddr;     /* peer's IP */
-	int		 ksnc_port;       /* peer's port */
-	signed int	  ksnc_type:3;     /* type of connection,
-					      * should be signed value */
-	unsigned int	    ksnc_closing:1;  /* being shut down */
-	unsigned int	    ksnc_flip:1;     /* flip or not, only for V2.x */
-	unsigned int	    ksnc_zc_capable:1; /* enable to ZC */
-	struct ksock_proto *ksnc_proto;      /* protocol for the connection */
+	struct ksock_peer  *ksnc_peer;        /* owning peer */
+	struct ksock_route *ksnc_route;       /* owning route */
+	struct list_head   ksnc_list;         /* stash on peer's conn list */
+	struct socket      *ksnc_sock;        /* actual socket */
+	void               *ksnc_saved_data_ready;  /* socket's original
+						     * data_ready() callback */
+	void               *ksnc_saved_write_space; /* socket's original
+						     * write_space() callback */
+	atomic_t           ksnc_conn_refcount;/* conn refcount */
+	atomic_t           ksnc_sock_refcount;/* sock refcount */
+	ksock_sched_t      *ksnc_scheduler;   /* who schedules this connection
+					       */
+	__u32              ksnc_myipaddr;     /* my IP */
+	__u32              ksnc_ipaddr;       /* peer's IP */
+	int                ksnc_port;         /* peer's port */
+	signed int         ksnc_type:3;       /* type of connection, should be
+					       * signed value */
+	unsigned int       ksnc_closing:1;    /* being shut down */
+	unsigned int       ksnc_flip:1;       /* flip or not, only for V2.x */
+	unsigned int       ksnc_zc_capable:1; /* enable to ZC */
+	struct ksock_proto *ksnc_proto;       /* protocol for the connection */
 
 	/* reader */
-	struct list_head  ksnc_rx_list;     /* where I enq waiting input or a forwarding descriptor */
-	unsigned long	    ksnc_rx_deadline; /* when (in jiffies) receive times out */
-	__u8		  ksnc_rx_started;  /* started receiving a message */
-	__u8		  ksnc_rx_ready;    /* data ready to read */
-	__u8		  ksnc_rx_scheduled;/* being progressed */
-	__u8		  ksnc_rx_state;    /* what is being read */
-	int		   ksnc_rx_nob_left; /* # bytes to next hdr/body */
-	int		   ksnc_rx_nob_wanted; /* bytes actually wanted */
-	int		   ksnc_rx_niov;     /* # iovec frags */
-	struct kvec 	 *ksnc_rx_iov;      /* the iovec frags */
-	int		   ksnc_rx_nkiov;    /* # page frags */
-	lnet_kiov_t	  *ksnc_rx_kiov;     /* the page frags */
-	ksock_rxiovspace_t    ksnc_rx_iov_space;/* space for frag descriptors */
-	__u32		 ksnc_rx_csum;     /* partial checksum for incoming data */
-	void		 *ksnc_cookie;      /* rx lnet_finalize passthru arg */
-	ksock_msg_t	   ksnc_msg;	 /* incoming message buffer:
-						 * V2.x message takes the
-						 * whole struct
-						 * V1.x message is a bare
-						 * lnet_hdr_t, it's stored in
-						 * ksnc_msg.ksm_u.lnetmsg */
+	struct list_head   ksnc_rx_list;      /* where I enq waiting input or a
+					       * forwarding descriptor */
+	unsigned long      ksnc_rx_deadline;  /* when (in jiffies) receive times
+					       * out */
+	__u8               ksnc_rx_started;   /* started receiving a message */
+	__u8               ksnc_rx_ready;     /* data ready to read */
+	__u8               ksnc_rx_scheduled; /* being progressed */
+	__u8               ksnc_rx_state;     /* what is being read */
+	int                ksnc_rx_nob_left;  /* # bytes to next hdr/body */
+	int                ksnc_rx_nob_wanted;/* bytes actually wanted */
+	int                ksnc_rx_niov;      /* # iovec frags */
+	struct kvec        *ksnc_rx_iov;      /* the iovec frags */
+	int                ksnc_rx_nkiov;     /* # page frags */
+	lnet_kiov_t        *ksnc_rx_kiov;     /* the page frags */
+	ksock_rxiovspace_t ksnc_rx_iov_space; /* space for frag descriptors */
+	__u32              ksnc_rx_csum;      /* partial checksum for incoming
+					       * data */
+	void               *ksnc_cookie;      /* rx lnet_finalize passthru arg
+					       */
+	ksock_msg_t        ksnc_msg;          /* incoming message buffer:
+					       * V2.x message takes the
+					       * whole struct
+					       * V1.x message is a bare
+					       * lnet_hdr_t, it's stored in
+					       * ksnc_msg.ksm_u.lnetmsg */
 
 	/* WRITER */
-	struct list_head	    ksnc_tx_list;     /* where I enq waiting for output space */
-	struct list_head	    ksnc_tx_queue;    /* packets waiting to be sent */
-	ksock_tx_t	   *ksnc_tx_carrier;  /* next TX that can carry a LNet message or ZC-ACK */
-	unsigned long	    ksnc_tx_deadline; /* when (in jiffies) tx times out */
-	int		   ksnc_tx_bufnob;     /* send buffer marker */
-	atomic_t	  ksnc_tx_nob;	/* # bytes queued */
-	int		   ksnc_tx_ready;      /* write space */
-	int		   ksnc_tx_scheduled;  /* being progressed */
-	unsigned long	    ksnc_tx_last_post;  /* time stamp of the last posted TX */
+	struct list_head   ksnc_tx_list;      /* where I enq waiting for output
+					       * space */
+	struct list_head   ksnc_tx_queue;     /* packets waiting to be sent */
+	ksock_tx_t         *ksnc_tx_carrier;  /* next TX that can carry a LNet
+					       * message or ZC-ACK */
+	unsigned long      ksnc_tx_deadline;  /* when (in jiffies) tx times out
+					       */
+	int                ksnc_tx_bufnob;    /* send buffer marker */
+	atomic_t           ksnc_tx_nob;       /* # bytes queued */
+	int		   ksnc_tx_ready;     /* write space */
+	int		   ksnc_tx_scheduled; /* being progressed */
+	unsigned long      ksnc_tx_last_post; /* time stamp of the last posted
+					       * TX */
 } ksock_conn_t;
 
 typedef struct ksock_route {
-	struct list_head	    ksnr_list;	/* chain on peer route list */
-	struct list_head	    ksnr_connd_list;  /* chain on ksnr_connd_routes */
-	struct ksock_peer    *ksnr_peer;	/* owning peer */
-	atomic_t	  ksnr_refcount;    /* # users */
-	unsigned long	    ksnr_timeout;     /* when (in jiffies) reconnection can happen next */
-	long	ksnr_retry_interval; /* how long between retries */
-	__u32		 ksnr_myipaddr;    /* my IP */
-	__u32		 ksnr_ipaddr;      /* IP address to connect to */
-	int		   ksnr_port;	/* port to connect to */
-	unsigned int	  ksnr_scheduled:1; /* scheduled for attention */
-	unsigned int	  ksnr_connecting:1;/* connection establishment in progress */
-	unsigned int	  ksnr_connected:4; /* connections established by type */
-	unsigned int	  ksnr_deleted:1;   /* been removed from peer? */
-	unsigned int	  ksnr_share_count; /* created explicitly? */
-	int		   ksnr_conn_count;  /* # conns established by this route */
+	struct list_head  ksnr_list;           /* chain on peer route list */
+	struct list_head  ksnr_connd_list;     /* chain on ksnr_connd_routes */
+	struct ksock_peer *ksnr_peer;          /* owning peer */
+	atomic_t          ksnr_refcount;       /* # users */
+	unsigned long     ksnr_timeout;        /* when (in jiffies) reconnection
+						* can happen next */
+	long              ksnr_retry_interval; /* how long between retries */
+	__u32             ksnr_myipaddr;       /* my IP */
+	__u32             ksnr_ipaddr;         /* IP address to connect to */
+	int               ksnr_port;           /* port to connect to */
+	unsigned int      ksnr_scheduled:1;    /* scheduled for attention */
+	unsigned int      ksnr_connecting:1;   /* connection establishment in
+						* progress */
+	unsigned int      ksnr_connected:4;    /* connections established by
+						* type */
+	unsigned int      ksnr_deleted:1;      /* been removed from peer? */
+	unsigned int      ksnr_share_count;    /* created explicitly? */
+	int               ksnr_conn_count;     /* # conns established by this
+						* route */
 } ksock_route_t;
 
-#define SOCKNAL_KEEPALIVE_PING	  1       /* cookie for keepalive ping */
+#define SOCKNAL_KEEPALIVE_PING 1 /* cookie for keepalive ping */
 
 typedef struct ksock_peer {
-	struct list_head	    ksnp_list;	/* stash on global peer list */
-	unsigned long	    ksnp_last_alive;  /* when (in jiffies) I was last alive */
-	lnet_process_id_t     ksnp_id;       /* who's on the other end(s) */
-	atomic_t	  ksnp_refcount; /* # users */
-	int		   ksnp_sharecount;  /* lconf usage counter */
-	int		   ksnp_closing;  /* being closed */
-	int		   ksnp_accepting;/* # passive connections pending */
-	int		   ksnp_error;    /* errno on closing last conn */
-	__u64		 ksnp_zc_next_cookie;/* ZC completion cookie */
-	__u64		 ksnp_incarnation;   /* latest known peer incarnation */
-	struct ksock_proto   *ksnp_proto;    /* latest known peer protocol */
-	struct list_head	    ksnp_conns;    /* all active connections */
-	struct list_head	    ksnp_routes;   /* routes */
-	struct list_head	    ksnp_tx_queue; /* waiting packets */
-	spinlock_t	      ksnp_lock;	/* serialize, g_lock unsafe */
-	struct list_head	    ksnp_zc_req_list;   /* zero copy requests wait for ACK  */
-	unsigned long	    ksnp_send_keepalive; /* time to send keepalive */
-	lnet_ni_t	    *ksnp_ni;       /* which network */
-	int		   ksnp_n_passive_ips; /* # of... */
-	__u32		 ksnp_passive_ips[LNET_MAX_INTERFACES]; /* preferred local interfaces */
+	struct list_head   ksnp_list;           /* stash on global peer list */
+	unsigned long      ksnp_last_alive;     /* when (in jiffies) I was last
+						 * alive */
+	lnet_process_id_t  ksnp_id;             /* who's on the other end(s) */
+	atomic_t           ksnp_refcount;       /* # users */
+	int                ksnp_sharecount;     /* lconf usage counter */
+	int                ksnp_closing;        /* being closed */
+	int                ksnp_accepting;      /* # passive connections pending
+						 */
+	int                ksnp_error;          /* errno on closing last conn */
+	__u64              ksnp_zc_next_cookie; /* ZC completion cookie */
+	__u64              ksnp_incarnation;    /* latest known peer incarnation
+						 */
+	struct ksock_proto *ksnp_proto;         /* latest known peer protocol */
+	struct list_head   ksnp_conns;          /* all active connections */
+	struct list_head   ksnp_routes;         /* routes */
+	struct list_head   ksnp_tx_queue;       /* waiting packets */
+	spinlock_t         ksnp_lock;           /* serialize, g_lock unsafe */
+	struct list_head   ksnp_zc_req_list;    /* zero copy requests wait for
+						 * ACK  */
+	unsigned long      ksnp_send_keepalive; /* time to send keepalive */
+	lnet_ni_t          *ksnp_ni;            /* which network */
+	int                ksnp_n_passive_ips;  /* # of... */
+
+	/* preferred local interfaces */
+	__u32              ksnp_passive_ips[LNET_MAX_INTERFACES];
 } ksock_peer_t;
 
 typedef struct ksock_connreq {
-	struct list_head	    ksncr_list;     /* stash on ksnd_connd_connreqs */
-	lnet_ni_t	    *ksncr_ni;       /* chosen NI */
-	struct socket	 *ksncr_sock;     /* accepted socket */
+	struct list_head ksncr_list;  /* stash on ksnd_connd_connreqs */
+	lnet_ni_t        *ksncr_ni;   /* chosen NI */
+	struct socket    *ksncr_sock; /* accepted socket */
 } ksock_connreq_t;
 
 extern ksock_nal_data_t ksocknal_data;
 extern ksock_tunables_t ksocknal_tunables;
 
-#define SOCKNAL_MATCH_NO	0	/* TX can't match type of connection */
-#define SOCKNAL_MATCH_YES       1	/* TX matches type of connection */
-#define SOCKNAL_MATCH_MAY       2	/* TX can be sent on the connection, but not preferred */
+#define SOCKNAL_MATCH_NO  0 /* TX can't match type of connection */
+#define SOCKNAL_MATCH_YES 1 /* TX matches type of connection */
+#define SOCKNAL_MATCH_MAY 2 /* TX can be sent on the connection, but not
+			     * preferred */
 
 typedef struct ksock_proto {
-	int	   pro_version;					      /* version number of protocol */
-	int	 (*pro_send_hello)(ksock_conn_t *, ksock_hello_msg_t *);     /* handshake function */
-	int	 (*pro_recv_hello)(ksock_conn_t *, ksock_hello_msg_t *, int);/* handshake function */
-	void	(*pro_pack)(ksock_tx_t *);				  /* message pack */
-	void	(*pro_unpack)(ksock_msg_t *);			       /* message unpack */
-	ksock_tx_t *(*pro_queue_tx_msg)(ksock_conn_t *, ksock_tx_t *);	  /* queue tx on the connection */
-	int	 (*pro_queue_tx_zcack)(ksock_conn_t *, ksock_tx_t *, __u64); /* queue ZC ack on the connection */
-	int	 (*pro_handle_zcreq)(ksock_conn_t *, __u64, int);	    /* handle ZC request */
-	int	 (*pro_handle_zcack)(ksock_conn_t *, __u64, __u64);	  /* handle ZC ACK */
-	int	 (*pro_match_tx)(ksock_conn_t *, ksock_tx_t *, int);	 /* msg type matches the connection type:
-										 * return value:
-										 *   return MATCH_NO  : no
-										 *   return MATCH_YES : matching type
-										 *   return MATCH_MAY : can be backup */
+	/* version number of protocol */
+	int        pro_version;
+
+	/* handshake function */
+	int        (*pro_send_hello)(ksock_conn_t *, ksock_hello_msg_t *);
+
+	/* handshake function */
+	int        (*pro_recv_hello)(ksock_conn_t *, ksock_hello_msg_t *, int);
+
+	/* message pack */
+	void       (*pro_pack)(ksock_tx_t *);
+
+	/* message unpack */
+	void       (*pro_unpack)(ksock_msg_t *);
+
+	/* queue tx on the connection */
+	ksock_tx_t *(*pro_queue_tx_msg)(ksock_conn_t *, ksock_tx_t *);
+
+	/* queue ZC ack on the connection */
+	int        (*pro_queue_tx_zcack)(ksock_conn_t *, ksock_tx_t *, __u64);
+
+	/* handle ZC request */
+	int        (*pro_handle_zcreq)(ksock_conn_t *, __u64, int);
+
+	/* handle ZC ACK */
+	int        (*pro_handle_zcack)(ksock_conn_t *, __u64, __u64);
+
+	/* msg type matches the connection type:
+	 * return value:
+	 *   return MATCH_NO  : no
+	 *   return MATCH_YES : matching type
+	 *   return MATCH_MAY : can be backup */
+	int        (*pro_match_tx)(ksock_conn_t *, ksock_tx_t *, int);
 } ksock_proto_t;
 
 extern ksock_proto_t ksocknal_protocol_v1x;
 extern ksock_proto_t ksocknal_protocol_v2x;
 extern ksock_proto_t ksocknal_protocol_v3x;
 
-#define KSOCK_PROTO_V1_MAJOR    LNET_PROTO_TCP_VERSION_MAJOR
-#define KSOCK_PROTO_V1_MINOR    LNET_PROTO_TCP_VERSION_MINOR
-#define KSOCK_PROTO_V1	  KSOCK_PROTO_V1_MAJOR
+#define KSOCK_PROTO_V1_MAJOR LNET_PROTO_TCP_VERSION_MAJOR
+#define KSOCK_PROTO_V1_MINOR LNET_PROTO_TCP_VERSION_MINOR
+#define KSOCK_PROTO_V1       KSOCK_PROTO_V1_MAJOR
 
 #ifndef CPU_MASK_NONE
 #define CPU_MASK_NONE   0UL
 #endif
 
+static inline __u32 ksocknal_csum(__u32 crc, unsigned char const *p, size_t len)
+{
+#if 1
+	return crc32_le(crc, p, len);
+#else
+	while (len-- > 0)
+		crc = ((crc + 0x100) & ~0xff) | ((crc + *p++) & 0xff) ;
+	return crc;
+#endif
+}
+
 static inline int
 ksocknal_route_mask(void)
 {
@@ -434,7 +534,7 @@ ksocknal_conn_decref(ksock_conn_t *conn)
 static inline int
 ksocknal_connsock_addref(ksock_conn_t *conn)
 {
-	int   rc = -ESHUTDOWN;
+	int rc = -ESHUTDOWN;
 
 	read_lock(&ksocknal_data.ksnd_global_lock);
 	if (!conn->ksnc_closing) {
@@ -453,7 +553,7 @@ ksocknal_connsock_decref(ksock_conn_t *conn)
 	LASSERT(atomic_read(&conn->ksnc_sock_refcount) > 0);
 	if (atomic_dec_and_test(&conn->ksnc_sock_refcount)) {
 		LASSERT(conn->ksnc_closing);
-		libcfs_sock_release(conn->ksnc_sock);
+		sock_release(conn->ksnc_sock);
 		conn->ksnc_sock = NULL;
 		ksocknal_finalize_zcreq(conn);
 	}
@@ -586,3 +686,5 @@ extern void ksocknal_lib_csum_tx(ksock_tx_t *tx);
 
 extern int ksocknal_lib_memory_pressure(ksock_conn_t *conn);
 extern int ksocknal_lib_bind_thread_to_cpu(int id);
+
+#endif /* _SOCKLND_SOCKLND_H_ */
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
index fa7ad883bda9..fe2a83a540cd 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
@@ -75,13 +75,13 @@ ksocknal_alloc_tx_noop(__u64 cookie, int nonblk)
 		return NULL;
 	}
 
-	tx->tx_conn     = NULL;
-	tx->tx_lnetmsg  = NULL;
-	tx->tx_kiov     = NULL;
-	tx->tx_nkiov    = 0;
-	tx->tx_iov      = tx->tx_frags.virt.iov;
-	tx->tx_niov     = 1;
-	tx->tx_nonblk   = nonblk;
+	tx->tx_conn    = NULL;
+	tx->tx_lnetmsg = NULL;
+	tx->tx_kiov    = NULL;
+	tx->tx_nkiov   = 0;
+	tx->tx_iov     = tx->tx_frags.virt.iov;
+	tx->tx_niov    = 1;
+	tx->tx_nonblk  = nonblk;
 
 	socklnd_init_msg(&tx->tx_msg, KSOCK_MSG_NOOP);
 	tx->tx_msg.ksm_zc_cookies[1] = cookie;
@@ -110,11 +110,11 @@ ksocknal_free_tx (ksock_tx_t *tx)
 static int
 ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 {
-	struct kvec  *iov = tx->tx_iov;
-	int    nob;
-	int    rc;
+	struct kvec *iov = tx->tx_iov;
+	int nob;
+	int rc;
 
-	LASSERT (tx->tx_niov > 0);
+	LASSERT(tx->tx_niov > 0);
 
 	/* Never touch tx->tx_iov inside ksocknal_lib_send_iov() */
 	rc = ksocknal_lib_send_iov(conn, tx);
@@ -128,7 +128,7 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 
 	/* "consume" iov */
 	do {
-		LASSERT (tx->tx_niov > 0);
+		LASSERT(tx->tx_niov > 0);
 
 		if (nob < (int) iov->iov_len) {
 			iov->iov_base = (void *)((char *)iov->iov_base + nob);
@@ -147,12 +147,12 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 static int
 ksocknal_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 {
-	lnet_kiov_t    *kiov = tx->tx_kiov;
-	int     nob;
-	int     rc;
+	lnet_kiov_t *kiov = tx->tx_kiov;
+	int nob;
+	int rc;
 
-	LASSERT (tx->tx_niov == 0);
-	LASSERT (tx->tx_nkiov > 0);
+	LASSERT(tx->tx_niov == 0);
+	LASSERT(tx->tx_nkiov > 0);
 
 	/* Never touch tx->tx_kiov inside ksocknal_lib_send_kiov() */
 	rc = ksocknal_lib_send_kiov(conn, tx);
@@ -185,15 +185,15 @@ ksocknal_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 static int
 ksocknal_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 {
-	int      rc;
-	int      bufnob;
+	int rc;
+	int bufnob;
 
 	if (ksocknal_data.ksnd_stall_tx != 0) {
 		set_current_state(TASK_UNINTERRUPTIBLE);
 		schedule_timeout(cfs_time_seconds(ksocknal_data.ksnd_stall_tx));
 	}
 
-	LASSERT (tx->tx_resid != 0);
+	LASSERT(tx->tx_resid != 0);
 
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0) {
@@ -252,10 +252,10 @@ static int
 ksocknal_recv_iov (ksock_conn_t *conn)
 {
 	struct kvec *iov = conn->ksnc_rx_iov;
-	int     nob;
-	int     rc;
+	int nob;
+	int rc;
 
-	LASSERT (conn->ksnc_rx_niov > 0);
+	LASSERT(conn->ksnc_rx_niov > 0);
 
 	/* Never touch conn->ksnc_rx_iov or change connection
 	 * status inside ksocknal_lib_recv_iov */
@@ -277,7 +277,7 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 	conn->ksnc_rx_nob_left -= nob;
 
 	do {
-		LASSERT (conn->ksnc_rx_niov > 0);
+		LASSERT(conn->ksnc_rx_niov > 0);
 
 		if (nob < (int)iov->iov_len) {
 			iov->iov_len -= nob;
@@ -296,10 +296,10 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 static int
 ksocknal_recv_kiov (ksock_conn_t *conn)
 {
-	lnet_kiov_t   *kiov = conn->ksnc_rx_kiov;
-	int     nob;
-	int     rc;
-	LASSERT (conn->ksnc_rx_nkiov > 0);
+	lnet_kiov_t *kiov = conn->ksnc_rx_kiov;
+	int nob;
+	int rc;
+	LASSERT(conn->ksnc_rx_nkiov > 0);
 
 	/* Never touch conn->ksnc_rx_kiov or change connection
 	 * status inside ksocknal_lib_recv_iov */
@@ -321,7 +321,7 @@ ksocknal_recv_kiov (ksock_conn_t *conn)
 	conn->ksnc_rx_nob_left -= nob;
 
 	do {
-		LASSERT (conn->ksnc_rx_nkiov > 0);
+		LASSERT(conn->ksnc_rx_nkiov > 0);
 
 		if (nob < (int) kiov->kiov_len) {
 			kiov->kiov_offset += nob;
@@ -343,7 +343,7 @@ ksocknal_receive (ksock_conn_t *conn)
 	/* Return 1 on success, 0 on EOF, < 0 on error.
 	 * Caller checks ksnc_rx_nob_wanted to determine
 	 * progress/completion. */
-	int     rc;
+	int rc;
 
 	if (ksocknal_data.ksnd_stall_rx != 0) {
 		set_current_state(TASK_UNINTERRUPTIBLE);
@@ -388,8 +388,8 @@ ksocknal_receive (ksock_conn_t *conn)
 void
 ksocknal_tx_done (lnet_ni_t *ni, ksock_tx_t *tx)
 {
-	lnet_msg_t  *lnetmsg = tx->tx_lnetmsg;
-	int	  rc = (tx->tx_resid == 0 && !tx->tx_zc_aborted) ? 0 : -EIO;
+	lnet_msg_t *lnetmsg = tx->tx_lnetmsg;
+	int rc = (tx->tx_resid == 0 && !tx->tx_zc_aborted) ? 0 : -EIO;
 
 	LASSERT(ni != NULL || tx->tx_conn != NULL);
 
@@ -410,7 +410,7 @@ ksocknal_txlist_done (lnet_ni_t *ni, struct list_head *txlist, int error)
 	ksock_tx_t *tx;
 
 	while (!list_empty (txlist)) {
-		tx = list_entry (txlist->next, ksock_tx_t, tx_list);
+		tx = list_entry(txlist->next, ksock_tx_t, tx_list);
 
 		if (error && tx->tx_lnetmsg != NULL) {
 			CNETERR("Deleting packet type %d len %d %s->%s\n",
@@ -422,18 +422,18 @@ ksocknal_txlist_done (lnet_ni_t *ni, struct list_head *txlist, int error)
 			CNETERR("Deleting noop packet\n");
 		}
 
-		list_del (&tx->tx_list);
+		list_del(&tx->tx_list);
 
-		LASSERT (atomic_read(&tx->tx_refcount) == 1);
-		ksocknal_tx_done (ni, tx);
+		LASSERT(atomic_read(&tx->tx_refcount) == 1);
+		ksocknal_tx_done(ni, tx);
 	}
 }
 
 static void
 ksocknal_check_zc_req(ksock_tx_t *tx)
 {
-	ksock_conn_t   *conn = tx->tx_conn;
-	ksock_peer_t   *peer = conn->ksnc_peer;
+	ksock_conn_t *conn = tx->tx_conn;
+	ksock_peer_t *peer = conn->ksnc_peer;
 
 	/* Set tx_msg.ksm_zc_cookies[0] to a unique non-zero cookie and add tx
 	 * to ksnp_zc_req_list if some fragment of this message should be sent
@@ -441,8 +441,8 @@ ksocknal_check_zc_req(ksock_tx_t *tx)
 	 * she has received this message to tell us we can signal completion.
 	 * tx_msg.ksm_zc_cookies[0] remains non-zero while tx is on
 	 * ksnp_zc_req_list. */
-	LASSERT (tx->tx_msg.ksm_type != KSOCK_MSG_NOOP);
-	LASSERT (tx->tx_zc_capable);
+	LASSERT(tx->tx_msg.ksm_type != KSOCK_MSG_NOOP);
+	LASSERT(tx->tx_zc_capable);
 
 	tx->tx_zc_checked = 1;
 
@@ -461,7 +461,7 @@ ksocknal_check_zc_req(ksock_tx_t *tx)
 	tx->tx_deadline =
 		cfs_time_shift(*ksocknal_tunables.ksnd_timeout);
 
-	LASSERT (tx->tx_msg.ksm_zc_cookies[0] == 0);
+	LASSERT(tx->tx_msg.ksm_zc_cookies[0] == 0);
 
 	tx->tx_msg.ksm_zc_cookies[0] = peer->ksnp_zc_next_cookie++;
 
@@ -476,7 +476,7 @@ ksocknal_check_zc_req(ksock_tx_t *tx)
 static void
 ksocknal_uncheck_zc_req(ksock_tx_t *tx)
 {
-	ksock_peer_t   *peer = tx->tx_conn->ksnc_peer;
+	ksock_peer_t *peer = tx->tx_conn->ksnc_peer;
 
 	LASSERT(tx->tx_msg.ksm_type != KSOCK_MSG_NOOP);
 	LASSERT(tx->tx_zc_capable);
@@ -502,14 +502,14 @@ ksocknal_uncheck_zc_req(ksock_tx_t *tx)
 static int
 ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 {
-	int	    rc;
+	int rc;
 
 	if (tx->tx_zc_capable && !tx->tx_zc_checked)
 		ksocknal_check_zc_req(tx);
 
 	rc = ksocknal_transmit (conn, tx);
 
-	CDEBUG (D_NET, "send(%d) %d\n", tx->tx_resid, rc);
+	CDEBUG(D_NET, "send(%d) %d\n", tx->tx_resid, rc);
 
 	if (tx->tx_resid == 0) {
 		/* Sent everything OK */
@@ -546,7 +546,7 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	}
 
 	/* Actual error */
-	LASSERT (rc < 0);
+	LASSERT(rc < 0);
 
 	if (!conn->ksnc_closing) {
 		switch (rc) {
@@ -582,9 +582,9 @@ ksocknal_launch_connection_locked (ksock_route_t *route)
 
 	/* called holding write lock on ksnd_global_lock */
 
-	LASSERT (!route->ksnr_scheduled);
-	LASSERT (!route->ksnr_connecting);
-	LASSERT ((ksocknal_route_mask() & ~route->ksnr_connected) != 0);
+	LASSERT(!route->ksnr_scheduled);
+	LASSERT(!route->ksnr_connecting);
+	LASSERT((ksocknal_route_mask() & ~route->ksnr_connected) != 0);
 
 	route->ksnr_scheduled = 1;	      /* scheduling conn for connd */
 	ksocknal_route_addref(route);	   /* extra ref for connd */
@@ -617,22 +617,22 @@ ksocknal_launch_all_connections_locked (ksock_peer_t *peer)
 ksock_conn_t *
 ksocknal_find_conn_locked(ksock_peer_t *peer, ksock_tx_t *tx, int nonblk)
 {
-	struct list_head       *tmp;
-	ksock_conn_t     *conn;
-	ksock_conn_t     *typed = NULL;
-	ksock_conn_t     *fallback = NULL;
-	int	       tnob     = 0;
-	int	       fnob     = 0;
+	struct list_head *tmp;
+	ksock_conn_t *conn;
+	ksock_conn_t *typed = NULL;
+	ksock_conn_t *fallback = NULL;
+	int tnob = 0;
+	int fnob = 0;
 
 	list_for_each (tmp, &peer->ksnp_conns) {
 		ksock_conn_t *c  = list_entry(tmp, ksock_conn_t, ksnc_list);
-		int	   nob = atomic_read(&c->ksnc_tx_nob) +
-				    c->ksnc_sock->sk->sk_wmem_queued;
-		int	   rc;
+		int nob = atomic_read(&c->ksnc_tx_nob) +
+                                      c->ksnc_sock->sk->sk_wmem_queued;
+		int rc;
 
-		LASSERT (!c->ksnc_closing);
-		LASSERT (c->ksnc_proto != NULL &&
-			 c->ksnc_proto->pro_match_tx != NULL);
+		LASSERT(!c->ksnc_closing);
+		LASSERT(c->ksnc_proto != NULL &&
+			c->ksnc_proto->pro_match_tx != NULL);
 
 		rc = c->ksnc_proto->pro_match_tx(c, tx, nonblk);
 
@@ -656,7 +656,7 @@ ksocknal_find_conn_locked(ksock_peer_t *peer, ksock_tx_t *tx, int nonblk)
 			    (fnob == nob && *ksocknal_tunables.ksnd_round_robin &&
 			     cfs_time_after(fallback->ksnc_tx_last_post, c->ksnc_tx_last_post))) {
 				fallback = c;
-				fnob     = nob;
+				fnob = nob;
 			}
 			break;
 		}
@@ -685,9 +685,9 @@ void
 ksocknal_queue_tx_locked (ksock_tx_t *tx, ksock_conn_t *conn)
 {
 	ksock_sched_t *sched = conn->ksnc_scheduler;
-	ksock_msg_t   *msg = &tx->tx_msg;
-	ksock_tx_t    *ztx = NULL;
-	int	    bufnob = 0;
+	ksock_msg_t *msg = &tx->tx_msg;
+	ksock_tx_t *ztx = NULL;
+	int bufnob = 0;
 
 	/* called holding global lock (read or irq-write) and caller may
 	 * not have dropped this lock between finding conn and calling me,
@@ -708,11 +708,11 @@ ksocknal_queue_tx_locked (ksock_tx_t *tx, ksock_conn_t *conn)
 	 *
 	 * We always expect at least 1 mapped fragment containing the
 	 * complete ksocknal message header. */
-	LASSERT (lnet_iov_nob (tx->tx_niov, tx->tx_iov) +
-		 lnet_kiov_nob(tx->tx_nkiov, tx->tx_kiov) ==
-		 (unsigned int)tx->tx_nob);
-	LASSERT (tx->tx_niov >= 1);
-	LASSERT (tx->tx_resid == tx->tx_nob);
+	LASSERT(lnet_iov_nob (tx->tx_niov, tx->tx_iov) +
+		lnet_kiov_nob(tx->tx_nkiov, tx->tx_kiov) ==
+		(unsigned int)tx->tx_nob);
+	LASSERT(tx->tx_niov >= 1);
+	LASSERT(tx->tx_resid == tx->tx_nob);
 
 	CDEBUG (D_NET, "Packet %p type %d, nob %d niov %d nkiov %d\n",
 		tx, (tx->tx_lnetmsg != NULL) ? tx->tx_lnetmsg->msg_hdr.type:
@@ -739,8 +739,8 @@ ksocknal_queue_tx_locked (ksock_tx_t *tx, ksock_conn_t *conn)
 	if (msg->ksm_type == KSOCK_MSG_NOOP) {
 		/* The packet is noop ZC ACK, try to piggyback the ack_cookie
 		 * on a normal packet so I don't need to send it */
-		LASSERT (msg->ksm_zc_cookies[1] != 0);
-		LASSERT (conn->ksnc_proto->pro_queue_tx_zcack != NULL);
+		LASSERT(msg->ksm_zc_cookies[1] != 0);
+		LASSERT(conn->ksnc_proto->pro_queue_tx_zcack != NULL);
 
 		if (conn->ksnc_proto->pro_queue_tx_zcack(conn, tx, 0))
 			ztx = tx; /* ZC ACK piggybacked on ztx release tx later */
@@ -748,8 +748,8 @@ ksocknal_queue_tx_locked (ksock_tx_t *tx, ksock_conn_t *conn)
 	} else {
 		/* It's a normal packet - can it piggback a noop zc-ack that
 		 * has been queued already? */
-		LASSERT (msg->ksm_zc_cookies[1] == 0);
-		LASSERT (conn->ksnc_proto->pro_queue_tx_msg != NULL);
+		LASSERT(msg->ksm_zc_cookies[1] == 0);
+		LASSERT(conn->ksnc_proto->pro_queue_tx_msg != NULL);
 
 		ztx = conn->ksnc_proto->pro_queue_tx_msg(conn, tx);
 		/* ztx will be released later */
@@ -777,14 +777,14 @@ ksocknal_queue_tx_locked (ksock_tx_t *tx, ksock_conn_t *conn)
 ksock_route_t *
 ksocknal_find_connectable_route_locked (ksock_peer_t *peer)
 {
-	unsigned long     now = cfs_time_current();
-	struct list_head    *tmp;
+	unsigned long now = cfs_time_current();
+	struct list_head *tmp;
 	ksock_route_t *route;
 
 	list_for_each (tmp, &peer->ksnp_routes) {
 		route = list_entry (tmp, ksock_route_t, ksnr_list);
 
-		LASSERT (!route->ksnr_connecting || route->ksnr_scheduled);
+		LASSERT(!route->ksnr_connecting || route->ksnr_scheduled);
 
 		if (route->ksnr_scheduled)      /* connections being established */
 			continue;
@@ -813,13 +813,13 @@ ksocknal_find_connectable_route_locked (ksock_peer_t *peer)
 ksock_route_t *
 ksocknal_find_connecting_route_locked (ksock_peer_t *peer)
 {
-	struct list_head	*tmp;
-	ksock_route_t     *route;
+	struct list_head *tmp;
+	ksock_route_t *route;
 
 	list_for_each (tmp, &peer->ksnp_routes) {
 		route = list_entry (tmp, ksock_route_t, ksnr_list);
 
-		LASSERT (!route->ksnr_connecting || route->ksnr_scheduled);
+		LASSERT(!route->ksnr_connecting || route->ksnr_scheduled);
 
 		if (route->ksnr_scheduled)
 			return route;
@@ -831,13 +831,13 @@ ksocknal_find_connecting_route_locked (ksock_peer_t *peer)
 int
 ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 {
-	ksock_peer_t     *peer;
-	ksock_conn_t     *conn;
-	rwlock_t     *g_lock;
-	int	       retry;
-	int	       rc;
+	ksock_peer_t *peer;
+	ksock_conn_t *conn;
+	rwlock_t *g_lock;
+	int retry;
+	int rc;
 
-	LASSERT (tx->tx_conn == NULL);
+	LASSERT(tx->tx_conn == NULL);
 
 	g_lock = &ksocknal_data.ksnd_global_lock;
 
@@ -922,17 +922,17 @@ ksocknal_launch_packet (lnet_ni_t *ni, ksock_tx_t *tx, lnet_process_id_t id)
 int
 ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 {
-	int	       mpflag = 1;
-	int	       type = lntmsg->msg_type;
+	int mpflag = 1;
+	int type = lntmsg->msg_type;
 	lnet_process_id_t target = lntmsg->msg_target;
-	unsigned int      payload_niov = lntmsg->msg_niov;
-	struct kvec      *payload_iov = lntmsg->msg_iov;
-	lnet_kiov_t      *payload_kiov = lntmsg->msg_kiov;
-	unsigned int      payload_offset = lntmsg->msg_offset;
-	unsigned int      payload_nob = lntmsg->msg_len;
-	ksock_tx_t       *tx;
-	int	       desc_size;
-	int	       rc;
+	unsigned int payload_niov = lntmsg->msg_niov;
+	struct kvec *payload_iov = lntmsg->msg_iov;
+	lnet_kiov_t *payload_kiov = lntmsg->msg_kiov;
+	unsigned int payload_offset = lntmsg->msg_offset;
+	unsigned int payload_nob = lntmsg->msg_len;
+	ksock_tx_t *tx;
+	int desc_size;
+	int rc;
 
 	/* NB 'private' is different depending on what we're sending.
 	 * Just ignore it... */
@@ -940,8 +940,8 @@ ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 	CDEBUG(D_NET, "sending %u bytes in %d frags to %s\n",
 	       payload_nob, payload_niov, libcfs_id2str(target));
 
-	LASSERT (payload_nob == 0 || payload_niov > 0);
-	LASSERT (payload_niov <= LNET_MAX_IOV);
+	LASSERT(payload_nob == 0 || payload_niov > 0);
+	LASSERT(payload_niov <= LNET_MAX_IOV);
 	/* payload is either all vaddrs or all pages */
 	LASSERT (!(payload_kiov != NULL && payload_iov != NULL));
 	LASSERT (!in_interrupt ());
@@ -1028,9 +1028,9 @@ ksocknal_new_packet (ksock_conn_t *conn, int nob_to_skip)
 {
 	static char ksocknal_slop_buffer[4096];
 
-	int	    nob;
-	unsigned int   niov;
-	int	    skipped;
+	int nob;
+	unsigned int niov;
+	int skipped;
 
 	LASSERT(conn->ksnc_proto != NULL);
 
@@ -1063,7 +1063,7 @@ ksocknal_new_packet (ksock_conn_t *conn, int nob_to_skip)
 
 			conn->ksnc_rx_iov = (struct kvec *)&conn->ksnc_rx_iov_space;
 			conn->ksnc_rx_iov[0].iov_base = &conn->ksnc_msg.ksm_u.lnetmsg;
-			conn->ksnc_rx_iov[0].iov_len  = sizeof (lnet_hdr_t);
+			conn->ksnc_rx_iov[0].iov_len = sizeof (lnet_hdr_t);
 			break;
 
 		default:
@@ -1108,18 +1108,18 @@ ksocknal_new_packet (ksock_conn_t *conn, int nob_to_skip)
 static int
 ksocknal_process_receive (ksock_conn_t *conn)
 {
-	lnet_hdr_t	*lhdr;
+	lnet_hdr_t *lhdr;
 	lnet_process_id_t *id;
-	int		rc;
+	int rc;
 
 	LASSERT (atomic_read(&conn->ksnc_conn_refcount) > 0);
 
 	/* NB: sched lock NOT held */
 	/* SOCKNAL_RX_LNET_HEADER is here for backward compatibility */
-	LASSERT (conn->ksnc_rx_state == SOCKNAL_RX_KSM_HEADER ||
-		 conn->ksnc_rx_state == SOCKNAL_RX_LNET_PAYLOAD ||
-		 conn->ksnc_rx_state == SOCKNAL_RX_LNET_HEADER ||
-		 conn->ksnc_rx_state == SOCKNAL_RX_SLOP);
+	LASSERT(conn->ksnc_rx_state == SOCKNAL_RX_KSM_HEADER ||
+		conn->ksnc_rx_state == SOCKNAL_RX_LNET_PAYLOAD ||
+		conn->ksnc_rx_state == SOCKNAL_RX_LNET_HEADER ||
+		conn->ksnc_rx_state == SOCKNAL_RX_SLOP);
  again:
 	if (conn->ksnc_rx_nob_wanted != 0) {
 		rc = ksocknal_receive(conn);
@@ -1229,7 +1229,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 		if ((conn->ksnc_peer->ksnp_id.pid & LNET_PID_USERFLAG) != 0) {
 			/* Userspace peer */
 			lhdr = &conn->ksnc_msg.ksm_u.lnetmsg.ksnm_hdr;
-			id   = &conn->ksnc_peer->ksnp_id;
+			id = &conn->ksnc_peer->ksnp_id;
 
 			/* Substitute process ID assigned at connection time */
 			lhdr->src_pid = cpu_to_le32(id->pid);
@@ -1277,7 +1277,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 			LASSERT(conn->ksnc_proto != &ksocknal_protocol_v1x);
 
 			lhdr = &conn->ksnc_msg.ksm_u.lnetmsg.ksnm_hdr;
-			id   = &conn->ksnc_peer->ksnp_id;
+			id = &conn->ksnc_peer->ksnp_id;
 
 			rc = conn->ksnc_proto->pro_handle_zcreq(conn,
 					conn->ksnc_msg.ksm_zc_cookies[0],
@@ -1305,7 +1305,7 @@ ksocknal_process_receive (ksock_conn_t *conn)
 	}
 
 	/* Not Reached */
-	LBUG ();
+	LBUG();
 	return -EINVAL;		       /* keep gcc happy */
 }
 
@@ -1314,15 +1314,15 @@ ksocknal_recv (lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
 	       unsigned int niov, struct kvec *iov, lnet_kiov_t *kiov,
 	       unsigned int offset, unsigned int mlen, unsigned int rlen)
 {
-	ksock_conn_t  *conn = (ksock_conn_t *)private;
+	ksock_conn_t *conn = (ksock_conn_t *)private;
 	ksock_sched_t *sched = conn->ksnc_scheduler;
 
-	LASSERT (mlen <= rlen);
-	LASSERT (niov <= LNET_MAX_IOV);
+	LASSERT(mlen <= rlen);
+	LASSERT(niov <= LNET_MAX_IOV);
 
 	conn->ksnc_cookie = msg;
 	conn->ksnc_rx_nob_wanted = mlen;
-	conn->ksnc_rx_nob_left   = rlen;
+	conn->ksnc_rx_nob_left = rlen;
 
 	if (mlen == 0 || iov != NULL) {
 		conn->ksnc_rx_nkiov = 0;
@@ -1333,18 +1333,18 @@ ksocknal_recv (lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
 					 niov, iov, offset, mlen);
 	} else {
 		conn->ksnc_rx_niov = 0;
-		conn->ksnc_rx_iov  = NULL;
+		conn->ksnc_rx_iov = NULL;
 		conn->ksnc_rx_kiov = conn->ksnc_rx_iov_space.kiov;
 		conn->ksnc_rx_nkiov =
 			lnet_extract_kiov(LNET_MAX_IOV, conn->ksnc_rx_kiov,
 					  niov, kiov, offset, mlen);
 	}
 
-	LASSERT (mlen ==
-		 lnet_iov_nob (conn->ksnc_rx_niov, conn->ksnc_rx_iov) +
-		 lnet_kiov_nob (conn->ksnc_rx_nkiov, conn->ksnc_rx_kiov));
+	LASSERT(mlen ==
+		lnet_iov_nob(conn->ksnc_rx_niov, conn->ksnc_rx_iov) +
+		lnet_kiov_nob(conn->ksnc_rx_nkiov, conn->ksnc_rx_kiov));
 
-	LASSERT (conn->ksnc_rx_scheduled);
+	LASSERT(conn->ksnc_rx_scheduled);
 
 	spin_lock_bh(&sched->kss_lock);
 
@@ -1370,7 +1370,7 @@ ksocknal_recv (lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
 static inline int
 ksocknal_sched_cansleep(ksock_sched_t *sched)
 {
-	int	   rc;
+	int rc;
 
 	spin_lock_bh(&sched->kss_lock);
 
@@ -1384,13 +1384,13 @@ ksocknal_sched_cansleep(ksock_sched_t *sched)
 
 int ksocknal_scheduler(void *arg)
 {
-	struct ksock_sched_info	*info;
-	ksock_sched_t		*sched;
-	ksock_conn_t		*conn;
-	ksock_tx_t		*tx;
-	int			rc;
-	int			nloops = 0;
-	long			id = (long)arg;
+	struct ksock_sched_info *info;
+	ksock_sched_t *sched;
+	ksock_conn_t *conn;
+	ksock_tx_t *tx;
+	int rc;
+	int nloops = 0;
+	long id = (long)arg;
 
 	info = ksocknal_data.ksnd_sched_info[KSOCK_THREAD_CPT(id)];
 	sched = &info->ksi_scheds[KSOCK_THREAD_SID(id)];
@@ -1455,7 +1455,7 @@ int ksocknal_scheduler(void *arg)
 		}
 
 		if (!list_empty (&sched->kss_tx_conns)) {
-			LIST_HEAD    (zlist);
+			LIST_HEAD(zlist);
 
 			if (!list_empty(&sched->kss_zombie_noop_txs)) {
 				list_add(&zlist,
@@ -1513,9 +1513,9 @@ int ksocknal_scheduler(void *arg)
 				/* Do nothing; after a short timeout, this
 				 * conn will be reposted on kss_tx_conns. */
 			} else if (conn->ksnc_tx_ready &&
-				   !list_empty (&conn->ksnc_tx_queue)) {
+				   !list_empty(&conn->ksnc_tx_queue)) {
 				/* reschedule for tx */
-				list_add_tail (&conn->ksnc_tx_list,
+				list_add_tail(&conn->ksnc_tx_list,
 						   &sched->kss_tx_conns);
 			} else {
 				conn->ksnc_tx_scheduled = 0;
@@ -1606,7 +1606,7 @@ void ksocknal_write_callback (ksock_conn_t *conn)
 static ksock_proto_t *
 ksocknal_parse_proto_version (ksock_hello_msg_t *hello)
 {
-	__u32   version = 0;
+	__u32 version = 0;
 
 	if (hello->kshm_magic == LNET_PROTO_MAGIC)
 		version = hello->kshm_version;
@@ -1634,8 +1634,8 @@ ksocknal_parse_proto_version (ksock_hello_msg_t *hello)
 	if (hello->kshm_magic == le32_to_cpu(LNET_PROTO_TCP_MAGIC)) {
 		lnet_magicversion_t *hmv = (lnet_magicversion_t *)hello;
 
-		CLASSERT (sizeof (lnet_magicversion_t) ==
-			  offsetof (ksock_hello_msg_t, kshm_src_nid));
+		CLASSERT(sizeof (lnet_magicversion_t) ==
+			 offsetof (ksock_hello_msg_t, kshm_src_nid));
 
 		if (hmv->version_major == cpu_to_le16 (KSOCK_PROTO_V1_MAJOR) &&
 		    hmv->version_minor == cpu_to_le16 (KSOCK_PROTO_V1_MINOR))
@@ -1650,19 +1650,19 @@ ksocknal_send_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 		     lnet_nid_t peer_nid, ksock_hello_msg_t *hello)
 {
 	/* CAVEAT EMPTOR: this byte flips 'ipaddrs' */
-	ksock_net_t	 *net = (ksock_net_t *)ni->ni_data;
+	ksock_net_t *net = (ksock_net_t *)ni->ni_data;
 
-	LASSERT (hello->kshm_nips <= LNET_MAX_INTERFACES);
+	LASSERT(hello->kshm_nips <= LNET_MAX_INTERFACES);
 
 	/* rely on caller to hold a ref on socket so it wouldn't disappear */
-	LASSERT (conn->ksnc_proto != NULL);
+	LASSERT(conn->ksnc_proto != NULL);
 
-	hello->kshm_src_nid	 = ni->ni_nid;
-	hello->kshm_dst_nid	 = peer_nid;
-	hello->kshm_src_pid	 = the_lnet.ln_pid;
+	hello->kshm_src_nid = ni->ni_nid;
+	hello->kshm_dst_nid = peer_nid;
+	hello->kshm_src_pid = the_lnet.ln_pid;
 
 	hello->kshm_src_incarnation = net->ksnn_incarnation;
-	hello->kshm_ctype	   = conn->ksnc_type;
+	hello->kshm_ctype = conn->ksnc_type;
 
 	return conn->ksnc_proto->pro_send_hello(conn, hello);
 }
@@ -1693,21 +1693,21 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 	 *	EALREADY   lost connection race
 	 *	EPROTO     protocol version mismatch
 	 */
-	struct socket	*sock = conn->ksnc_sock;
-	int		  active = (conn->ksnc_proto != NULL);
-	int		  timeout;
-	int		  proto_match;
-	int		  rc;
-	ksock_proto_t       *proto;
-	lnet_process_id_t    recv_id;
+	struct socket *sock = conn->ksnc_sock;
+	int active = (conn->ksnc_proto != NULL);
+	int timeout;
+	int proto_match;
+	int rc;
+	ksock_proto_t *proto;
+	lnet_process_id_t recv_id;
 
 	/* socket type set on active connections - not set on passive */
-	LASSERT (!active == !(conn->ksnc_type != SOCKLND_CONN_NONE));
+	LASSERT(!active == !(conn->ksnc_type != SOCKLND_CONN_NONE));
 
 	timeout = active ? *ksocknal_tunables.ksnd_timeout :
 			    lnet_acceptor_timeout();
 
-	rc = libcfs_sock_read(sock, &hello->kshm_magic, sizeof (hello->kshm_magic), timeout);
+	rc = lnet_sock_read(sock, &hello->kshm_magic, sizeof (hello->kshm_magic), timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading HELLO from %pI4h\n",
 			rc, &conn->ksnc_ipaddr);
@@ -1726,12 +1726,12 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 		return -EPROTO;
 	}
 
-	rc = libcfs_sock_read(sock, &hello->kshm_version,
-			      sizeof(hello->kshm_version), timeout);
+	rc = lnet_sock_read(sock, &hello->kshm_version,
+			    sizeof(hello->kshm_version), timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading HELLO from %pI4h\n",
 			rc, &conn->ksnc_ipaddr);
-		LASSERT (rc < 0);
+		LASSERT(rc < 0);
 		return rc;
 	}
 
@@ -1765,7 +1765,7 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 	if (rc != 0) {
 		CERROR("Error %d reading or checking hello from from %pI4h\n",
 		       rc, &conn->ksnc_ipaddr);
-		LASSERT (rc < 0);
+		LASSERT(rc < 0);
 		return rc;
 	}
 
@@ -1830,22 +1830,22 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 static int
 ksocknal_connect (ksock_route_t *route)
 {
-	LIST_HEAD    (zombies);
-	ksock_peer_t     *peer = route->ksnr_peer;
-	int	       type;
-	int	       wanted;
-	struct socket     *sock;
-	unsigned long	deadline;
-	int	       retry_later = 0;
-	int	       rc = 0;
+	LIST_HEAD(zombies);
+	ksock_peer_t *peer = route->ksnr_peer;
+	int type;
+	int wanted;
+	struct socket *sock;
+	unsigned long deadline;
+	int retry_later = 0;
+	int rc = 0;
 
 	deadline = cfs_time_add(cfs_time_current(),
 				cfs_time_seconds(*ksocknal_tunables.ksnd_timeout));
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
-	LASSERT (route->ksnr_scheduled);
-	LASSERT (!route->ksnr_connecting);
+	LASSERT(route->ksnr_scheduled);
+	LASSERT(!route->ksnr_connecting);
 
 	route->ksnr_connecting = 1;
 
@@ -2101,7 +2101,7 @@ static ksock_route_t *
 ksocknal_connd_get_route_locked(signed long *timeout_p)
 {
 	ksock_route_t *route;
-	unsigned long     now;
+	unsigned long now;
 
 	now = cfs_time_current();
 
@@ -2124,13 +2124,13 @@ ksocknal_connd_get_route_locked(signed long *timeout_p)
 int
 ksocknal_connd (void *arg)
 {
-	spinlock_t    *connd_lock = &ksocknal_data.ksnd_connd_lock;
-	ksock_connreq_t   *cr;
-	wait_queue_t     wait;
-	int		nloops = 0;
-	int		cons_retry = 0;
+	spinlock_t *connd_lock = &ksocknal_data.ksnd_connd_lock;
+	ksock_connreq_t *cr;
+	wait_queue_t wait;
+	int nloops = 0;
+	int cons_retry = 0;
 
-	cfs_block_allsigs ();
+	cfs_block_allsigs();
 
 	init_waitqueue_entry(&wait, current);
 
@@ -2144,7 +2144,7 @@ ksocknal_connd (void *arg)
 		ksock_route_t *route = NULL;
 		long sec = get_seconds();
 		long timeout = MAX_SCHEDULE_TIMEOUT;
-		int  dropped_lock = 0;
+		int dropped_lock = 0;
 
 		if (ksocknal_connd_check_stop(sec, &timeout)) {
 			/* wakeup another one to check stop */
@@ -2236,15 +2236,15 @@ static ksock_conn_t *
 ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 {
 	/* We're called with a shared lock on ksnd_global_lock */
-	ksock_conn_t      *conn;
-	struct list_head	*ctmp;
+	ksock_conn_t *conn;
+	struct list_head *ctmp;
 
 	list_for_each (ctmp, &peer->ksnp_conns) {
-		int     error;
+		int error;
 		conn = list_entry (ctmp, ksock_conn_t, ksnc_list);
 
 		/* Don't need the {get,put}connsock dance to deref ksnc_sock */
-		LASSERT (!conn->ksnc_closing);
+		LASSERT(!conn->ksnc_closing);
 
 		/* SOCK_ERROR will reset error code of socket in
 		 * some platform (like Darwin8.x) */
@@ -2313,8 +2313,8 @@ ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 static inline void
 ksocknal_flush_stale_txs(ksock_peer_t *peer)
 {
-	ksock_tx_t	*tx;
-	LIST_HEAD      (stale_txs);
+	ksock_tx_t *tx;
+	LIST_HEAD(stale_txs);
 
 	write_lock_bh(&ksocknal_data.ksnd_global_lock);
 
@@ -2338,9 +2338,9 @@ ksocknal_flush_stale_txs(ksock_peer_t *peer)
 static int
 ksocknal_send_keepalive_locked(ksock_peer_t *peer)
 {
-	ksock_sched_t  *sched;
-	ksock_conn_t   *conn;
-	ksock_tx_t     *tx;
+	ksock_sched_t *sched;
+	ksock_conn_t *conn;
+	ksock_tx_t *tx;
 
 	if (list_empty(&peer->ksnp_conns)) /* last_alive will be updated by create_conn */
 		return 0;
@@ -2399,10 +2399,10 @@ ksocknal_send_keepalive_locked(ksock_peer_t *peer)
 static void
 ksocknal_check_peer_timeouts (int idx)
 {
-	struct list_head       *peers = &ksocknal_data.ksnd_peers[idx];
-	ksock_peer_t     *peer;
-	ksock_conn_t     *conn;
-	ksock_tx_t       *tx;
+	struct list_head *peers = &ksocknal_data.ksnd_peers[idx];
+	ksock_peer_t *peer;
+	ksock_conn_t *conn;
+	ksock_tx_t *tx;
 
  again:
 	/* NB. We expect to have a look at all the peers and not find any
@@ -2411,9 +2411,9 @@ ksocknal_check_peer_timeouts (int idx)
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
 	list_for_each_entry(peer, peers, ksnp_list) {
-		unsigned long  deadline = 0;
-		int	 resid = 0;
-		int	 n     = 0;
+		unsigned long deadline = 0;
+		int resid = 0;
+		int n = 0;
 
 		if (ksocknal_send_keepalive_locked(peer) != 0) {
 			read_unlock(&ksocknal_data.ksnd_global_lock);
@@ -2476,8 +2476,8 @@ ksocknal_check_peer_timeouts (int idx)
 		tx = list_entry(peer->ksnp_zc_req_list.next,
 				    ksock_tx_t, tx_zc_list);
 		deadline = tx->tx_deadline;
-		resid    = tx->tx_resid;
-		conn     = tx->tx_conn;
+		resid = tx->tx_resid;
+		conn = tx->tx_conn;
 		ksocknal_conn_addref(conn);
 
 		spin_unlock(&peer->ksnp_lock);
@@ -2499,17 +2499,17 @@ ksocknal_check_peer_timeouts (int idx)
 int
 ksocknal_reaper (void *arg)
 {
-	wait_queue_t     wait;
-	ksock_conn_t      *conn;
-	ksock_sched_t     *sched;
-	struct list_head	 enomem_conns;
-	int		nenomem_conns;
-	long     timeout;
-	int		i;
-	int		peer_index = 0;
-	unsigned long	 deadline = cfs_time_current();
-
-	cfs_block_allsigs ();
+	wait_queue_t wait;
+	ksock_conn_t *conn;
+	ksock_sched_t *sched;
+	struct list_head enomem_conns;
+	int nenomem_conns;
+	long timeout;
+	int i;
+	int peer_index = 0;
+	unsigned long deadline = cfs_time_current();
+
+	cfs_block_allsigs();
 
 	INIT_LIST_HEAD(&enomem_conns);
 	init_waitqueue_entry(&wait, current);
@@ -2580,7 +2580,7 @@ ksocknal_reaper (void *arg)
 					       cfs_time_current())) <= 0) {
 			const int n = 4;
 			const int p = 1;
-			int       chunk = ksocknal_data.ksnd_peer_hash_size;
+			int chunk = ksocknal_data.ksnd_peer_hash_size;
 
 			/* Time to check for timeouts on a few more peers: I do
 			 * checks every 'p' seconds on a proportion of the peer
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h
deleted file mode 100644
index f5563881b25c..000000000000
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#define DEBUG_PORTAL_ALLOC
-
-#ifndef __LINUX_SOCKNAL_LIB_H__
-#define __LINUX_SOCKNAL_LIB_H__
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/mm.h>
-#include <linux/string.h>
-#include <linux/stat.h>
-#include <linux/errno.h>
-#include <linux/unistd.h>
-#include <net/sock.h>
-#include <net/tcp.h>
-#include <linux/uio.h>
-#include <linux/if.h>
-#include <linux/uaccess.h>
-
-#include <asm/irq.h>
-
-#include <linux/fs.h>
-#include <linux/file.h>
-#include <linux/list.h>
-#include <linux/kmod.h>
-#include <linux/sysctl.h>
-#include <asm/div64.h>
-#include <linux/syscalls.h>
-
-#include "../../../include/linux/libcfs/libcfs.h"
-
-#include <linux/crc32.h>
-static inline __u32 ksocknal_csum(__u32 crc, unsigned char const *p, size_t len)
-{
-#if 1
-	return crc32_le(crc, p, len);
-#else
-	while (len-- > 0)
-		crc = ((crc + 0x100) & ~0xff) | ((crc + *p++) & 0xff) ;
-	return crc;
-#endif
-}
-
-#define SOCKNAL_WSPACE(sk)       sk_stream_wspace(sk)
-#define SOCKNAL_MIN_WSPACE(sk)   sk_stream_min_wspace(sk)
-
-/* assume one thread for each connection type */
-#define SOCKNAL_NSCHEDS		3
-#define SOCKNAL_NSCHEDS_HIGH	(SOCKNAL_NSCHEDS << 1)
-
-#endif
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c
index f5e8ab06070c..340706110c21 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c
@@ -39,9 +39,8 @@
 int
 ksocknal_lib_get_conn_addrs(ksock_conn_t *conn)
 {
-	int rc = libcfs_sock_getaddr(conn->ksnc_sock, 1,
-				     &conn->ksnc_ipaddr,
-				     &conn->ksnc_port);
+	int rc = lnet_sock_getaddr(conn->ksnc_sock, 1, &conn->ksnc_ipaddr,
+				   &conn->ksnc_port);
 
 	/* Didn't need the {get,put}connsock dance to deref ksnc_sock... */
 	LASSERT(!conn->ksnc_closing);
@@ -51,8 +50,7 @@ ksocknal_lib_get_conn_addrs(ksock_conn_t *conn)
 		return rc;
 	}
 
-	rc = libcfs_sock_getaddr(conn->ksnc_sock, 0,
-				 &conn->ksnc_myipaddr, NULL);
+	rc = lnet_sock_getaddr(conn->ksnc_sock, 0, &conn->ksnc_myipaddr, NULL);
 	if (rc != 0) {
 		CERROR("Error %d getting sock local IP\n", rc);
 		return rc;
@@ -64,7 +62,7 @@ ksocknal_lib_get_conn_addrs(ksock_conn_t *conn)
 int
 ksocknal_lib_zc_capable(ksock_conn_t *conn)
 {
-	int  caps = conn->ksnc_sock->sk->sk_route_caps;
+	int caps = conn->ksnc_sock->sk->sk_route_caps;
 
 	if (conn->ksnc_proto == &ksocknal_protocol_v1x)
 		return 0;
@@ -78,8 +76,8 @@ int
 ksocknal_lib_send_iov(ksock_conn_t *conn, ksock_tx_t *tx)
 {
 	struct socket *sock = conn->ksnc_sock;
-	int	    nob;
-	int	    rc;
+	int nob;
+	int rc;
 
 	if (*ksocknal_tunables.ksnd_enable_csum	&& /* checksum enabled */
 	    conn->ksnc_proto == &ksocknal_protocol_v2x && /* V2.x connection  */
@@ -92,15 +90,15 @@ ksocknal_lib_send_iov(ksock_conn_t *conn, ksock_tx_t *tx)
 
 	{
 #if SOCKNAL_SINGLE_FRAG_TX
-		struct kvec    scratch;
-		struct kvec   *scratchiov = &scratch;
-		unsigned int    niov = 1;
+		struct kvec scratch;
+		struct kvec *scratchiov = &scratch;
+		unsigned int niov = 1;
 #else
-		struct kvec   *scratchiov = conn->ksnc_scheduler->kss_scratch_iov;
-		unsigned int    niov = tx->tx_niov;
+		struct kvec *scratchiov = conn->ksnc_scheduler->kss_scratch_iov;
+		unsigned int niov = tx->tx_niov;
 #endif
 		struct msghdr msg = {.msg_flags = MSG_DONTWAIT};
-		int  i;
+		int i;
 
 		for (nob = i = 0; i < niov; i++) {
 			scratchiov[i] = tx->tx_iov[i];
@@ -120,9 +118,9 @@ int
 ksocknal_lib_send_kiov(ksock_conn_t *conn, ksock_tx_t *tx)
 {
 	struct socket *sock = conn->ksnc_sock;
-	lnet_kiov_t   *kiov = tx->tx_kiov;
-	int	    rc;
-	int	    nob;
+	lnet_kiov_t *kiov = tx->tx_kiov;
+	int rc;
+	int nob;
 
 	/* Not NOOP message */
 	LASSERT(tx->tx_lnetmsg != NULL);
@@ -131,11 +129,11 @@ ksocknal_lib_send_kiov(ksock_conn_t *conn, ksock_tx_t *tx)
 	 * or leave them alone. */
 	if (tx->tx_msg.ksm_zc_cookies[0] != 0) {
 		/* Zero copy is enabled */
-		struct sock   *sk = sock->sk;
-		struct page   *page = kiov->kiov_page;
-		int	    offset = kiov->kiov_offset;
-		int	    fragsize = kiov->kiov_len;
-		int	    msgflg = MSG_DONTWAIT;
+		struct sock *sk = sock->sk;
+		struct page *page = kiov->kiov_page;
+		int offset = kiov->kiov_offset;
+		int fragsize = kiov->kiov_len;
+		int msgflg = MSG_DONTWAIT;
 
 		CDEBUG(D_NET, "page %p + offset %x for %d\n",
 			       page, offset, kiov->kiov_len);
@@ -148,23 +146,22 @@ ksocknal_lib_send_kiov(ksock_conn_t *conn, ksock_tx_t *tx)
 			rc = sk->sk_prot->sendpage(sk, page,
 						   offset, fragsize, msgflg);
 		} else {
-			rc = cfs_tcp_sendpage(sk, page, offset, fragsize,
-					      msgflg);
+			rc = tcp_sendpage(sk, page, offset, fragsize, msgflg);
 		}
 	} else {
 #if SOCKNAL_SINGLE_FRAG_TX || !SOCKNAL_RISK_KMAP_DEADLOCK
-		struct kvec  scratch;
+		struct kvec scratch;
 		struct kvec *scratchiov = &scratch;
-		unsigned int  niov = 1;
+		unsigned int niov = 1;
 #else
 #ifdef CONFIG_HIGHMEM
 #warning "XXX risk of kmap deadlock on multiple frags..."
 #endif
 		struct kvec *scratchiov = conn->ksnc_scheduler->kss_scratch_iov;
-		unsigned int  niov = tx->tx_nkiov;
+		unsigned int niov = tx->tx_nkiov;
 #endif
 		struct msghdr msg = {.msg_flags = MSG_DONTWAIT};
-		int	   i;
+		int i;
 
 		for (nob = i = 0; i < niov; i++) {
 			scratchiov[i].iov_base = kmap(kiov[i].kiov_page) +
@@ -187,7 +184,7 @@ ksocknal_lib_send_kiov(ksock_conn_t *conn, ksock_tx_t *tx)
 void
 ksocknal_lib_eager_ack(ksock_conn_t *conn)
 {
-	int	    opt = 1;
+	int opt = 1;
 	struct socket *sock = conn->ksnc_sock;
 
 	/* Remind the socket to ACK eagerly.  If I don't, the socket might
@@ -203,23 +200,23 @@ int
 ksocknal_lib_recv_iov(ksock_conn_t *conn)
 {
 #if SOCKNAL_SINGLE_FRAG_RX
-	struct kvec  scratch;
+	struct kvec scratch;
 	struct kvec *scratchiov = &scratch;
-	unsigned int  niov = 1;
+	unsigned int niov = 1;
 #else
 	struct kvec *scratchiov = conn->ksnc_scheduler->kss_scratch_iov;
-	unsigned int  niov = conn->ksnc_rx_niov;
+	unsigned int niov = conn->ksnc_rx_niov;
 #endif
 	struct kvec *iov = conn->ksnc_rx_iov;
 	struct msghdr msg = {
-		.msg_flags      = 0
+		.msg_flags = 0
 	};
-	int	  nob;
-	int	  i;
-	int	  rc;
-	int	  fragnob;
-	int	  sum;
-	__u32	saved_csum;
+	int nob;
+	int i;
+	int rc;
+	int fragnob;
+	int sum;
+	__u32 saved_csum;
 
 	/* NB we can't trust socket ops to either consume our iovs
 	 * or leave them alone. */
@@ -271,9 +268,9 @@ static void *
 ksocknal_lib_kiov_vmap(lnet_kiov_t *kiov, int niov,
 		       struct kvec *iov, struct page **pages)
 {
-	void	     *addr;
-	int	       nob;
-	int	       i;
+	void *addr;
+	int nob;
+	int i;
 
 	if (!*ksocknal_tunables.ksnd_zc_recv || pages == NULL)
 		return NULL;
@@ -307,29 +304,29 @@ int
 ksocknal_lib_recv_kiov(ksock_conn_t *conn)
 {
 #if SOCKNAL_SINGLE_FRAG_RX || !SOCKNAL_RISK_KMAP_DEADLOCK
-	struct kvec   scratch;
-	struct kvec  *scratchiov = &scratch;
-	struct page  **pages      = NULL;
-	unsigned int   niov       = 1;
+	struct kvec scratch;
+	struct kvec *scratchiov = &scratch;
+	struct page **pages = NULL;
+	unsigned int niov = 1;
 #else
 #ifdef CONFIG_HIGHMEM
 #warning "XXX risk of kmap deadlock on multiple frags..."
 #endif
-	struct kvec  *scratchiov = conn->ksnc_scheduler->kss_scratch_iov;
-	struct page  **pages      = conn->ksnc_scheduler->kss_rx_scratch_pgs;
-	unsigned int   niov       = conn->ksnc_rx_nkiov;
+	struct kvec *scratchiov = conn->ksnc_scheduler->kss_scratch_iov;
+	struct page **pages = conn->ksnc_scheduler->kss_rx_scratch_pgs;
+	unsigned int niov = conn->ksnc_rx_nkiov;
 #endif
 	lnet_kiov_t   *kiov = conn->ksnc_rx_kiov;
 	struct msghdr msg = {
-		.msg_flags      = 0
+		.msg_flags = 0
 	};
-	int	  nob;
-	int	  i;
-	int	  rc;
-	void	*base;
-	void	*addr;
-	int	  sum;
-	int	  fragnob;
+	int nob;
+	int i;
+	int rc;
+	void *base;
+	void *addr;
+	int sum;
+	int fragnob;
 	int n;
 
 	/* NB we can't trust socket ops to either consume our iovs
@@ -357,10 +354,10 @@ ksocknal_lib_recv_kiov(ksock_conn_t *conn)
 		for (i = 0, sum = rc; sum > 0; i++, sum -= fragnob) {
 			LASSERT(i < niov);
 
-			/* Dang! have to kmap again because I have nowhere to stash the
-			 * mapped address.  But by doing it while the page is still
-			 * mapped, the kernel just bumps the map count and returns me
-			 * the address it stashed. */
+			/* Dang! have to kmap again because I have nowhere to
+                         * stash the mapped address.  But by doing it while the
+                         * page is still mapped, the kernel just bumps the map
+                         * count and returns me the address it stashed. */
 			base = kmap(kiov[i].kiov_page) + kiov[i].kiov_offset;
 			fragnob = kiov[i].kiov_len;
 			if (fragnob > sum)
@@ -386,9 +383,9 @@ ksocknal_lib_recv_kiov(ksock_conn_t *conn)
 void
 ksocknal_lib_csum_tx(ksock_tx_t *tx)
 {
-	int	  i;
-	__u32	csum;
-	void	*base;
+	int i;
+	__u32 csum;
+	void *base;
 
 	LASSERT(tx->tx_iov[0].iov_base == &tx->tx_msg);
 	LASSERT(tx->tx_conn != NULL);
@@ -426,8 +423,8 @@ int
 ksocknal_lib_get_conn_tunables(ksock_conn_t *conn, int *txmem, int *rxmem, int *nagle)
 {
 	struct socket *sock = conn->ksnc_sock;
-	int	    len;
-	int	    rc;
+	int len;
+	int rc;
 
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0) {
@@ -436,7 +433,7 @@ ksocknal_lib_get_conn_tunables(ksock_conn_t *conn, int *txmem, int *rxmem, int *
 		return -ESHUTDOWN;
 	}
 
-	rc = libcfs_sock_getbuf(sock, txmem, rxmem);
+	rc = lnet_sock_getbuf(sock, txmem, rxmem);
 	if (rc == 0) {
 		len = sizeof(*nagle);
 		rc = kernel_getsockopt(sock, SOL_TCP, TCP_NODELAY,
@@ -456,13 +453,13 @@ ksocknal_lib_get_conn_tunables(ksock_conn_t *conn, int *txmem, int *rxmem, int *
 int
 ksocknal_lib_setup_sock(struct socket *sock)
 {
-	int	     rc;
-	int	     option;
-	int	     keep_idle;
-	int	     keep_intvl;
-	int	     keep_count;
-	int	     do_keepalive;
-	struct linger   linger;
+	int rc;
+	int option;
+	int keep_idle;
+	int keep_intvl;
+	int keep_count;
+	int do_keepalive;
+	struct linger linger;
 
 	sock->sk->sk_allocation = GFP_NOFS;
 
@@ -498,9 +495,8 @@ ksocknal_lib_setup_sock(struct socket *sock)
 		}
 	}
 
-	rc = libcfs_sock_setbuf(sock,
-				*ksocknal_tunables.ksnd_tx_buffer_size,
-				*ksocknal_tunables.ksnd_rx_buffer_size);
+	rc = lnet_sock_setbuf(sock, *ksocknal_tunables.ksnd_tx_buffer_size,
+			      *ksocknal_tunables.ksnd_rx_buffer_size);
 	if (rc != 0) {
 		CERROR("Can't set buffer tx %d, rx %d buffers: %d\n",
 			*ksocknal_tunables.ksnd_tx_buffer_size,
@@ -555,11 +551,11 @@ ksocknal_lib_setup_sock(struct socket *sock)
 void
 ksocknal_lib_push_conn(ksock_conn_t *conn)
 {
-	struct sock    *sk;
+	struct sock *sk;
 	struct tcp_sock *tp;
-	int	     nonagle;
-	int	     val = 1;
-	int	     rc;
+	int nonagle;
+	int val = 1;
+	int rc;
 
 	rc = ksocknal_connsock_addref(conn);
 	if (rc != 0)			    /* being shut down */
@@ -592,7 +588,7 @@ extern void ksocknal_write_callback(ksock_conn_t *conn);
 static void
 ksocknal_data_ready(struct sock *sk)
 {
-	ksock_conn_t  *conn;
+	ksock_conn_t *conn;
 
 	/* interleave correctly with closing sockets... */
 	LASSERT(!in_irq());
@@ -611,17 +607,17 @@ ksocknal_data_ready(struct sock *sk)
 static void
 ksocknal_write_space(struct sock *sk)
 {
-	ksock_conn_t  *conn;
-	int	    wspace;
-	int	    min_wpace;
+	ksock_conn_t *conn;
+	int wspace;
+	int min_wpace;
 
 	/* interleave correctly with closing sockets... */
 	LASSERT(!in_irq());
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
 	conn = sk->sk_user_data;
-	wspace = SOCKNAL_WSPACE(sk);
-	min_wpace = SOCKNAL_MIN_WSPACE(sk);
+	wspace = sk_stream_wspace(sk);
+	min_wpace = sk_stream_min_wspace(sk);
 
 	CDEBUG(D_NET, "sk %p wspace %d low water %d conn %p%s%s%s\n",
 	       sk, wspace, min_wpace, conn,
@@ -689,7 +685,7 @@ ksocknal_lib_reset_callback(struct socket *sock, ksock_conn_t *conn)
 int
 ksocknal_lib_memory_pressure(ksock_conn_t *conn)
 {
-	int	    rc = 0;
+	int rc = 0;
 	ksock_sched_t *sched;
 
 	sched = conn->ksnc_scheduler;
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
index 86b88db1cf20..c3ac67698125 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
@@ -145,40 +145,37 @@ ksock_tunables_t ksocknal_tunables;
 
 int ksocknal_tunables_init(void)
 {
-
 	/* initialize ksocknal_tunables structure */
-	ksocknal_tunables.ksnd_timeout	    = &sock_timeout;
-	ksocknal_tunables.ksnd_nscheds		  = &nscheds;
-	ksocknal_tunables.ksnd_nconnds	    = &nconnds;
-	ksocknal_tunables.ksnd_nconnds_max	= &nconnds_max;
+	ksocknal_tunables.ksnd_timeout            = &sock_timeout;
+	ksocknal_tunables.ksnd_nscheds            = &nscheds;
+	ksocknal_tunables.ksnd_nconnds            = &nconnds;
+	ksocknal_tunables.ksnd_nconnds_max        = &nconnds_max;
 	ksocknal_tunables.ksnd_min_reconnectms    = &min_reconnectms;
 	ksocknal_tunables.ksnd_max_reconnectms    = &max_reconnectms;
-	ksocknal_tunables.ksnd_eager_ack	  = &eager_ack;
-	ksocknal_tunables.ksnd_typed_conns	= &typed_conns;
-	ksocknal_tunables.ksnd_min_bulk	   = &min_bulk;
+	ksocknal_tunables.ksnd_eager_ack          = &eager_ack;
+	ksocknal_tunables.ksnd_typed_conns        = &typed_conns;
+	ksocknal_tunables.ksnd_min_bulk           = &min_bulk;
 	ksocknal_tunables.ksnd_tx_buffer_size     = &tx_buffer_size;
 	ksocknal_tunables.ksnd_rx_buffer_size     = &rx_buffer_size;
-	ksocknal_tunables.ksnd_nagle	      = &nagle;
-	ksocknal_tunables.ksnd_round_robin	= &round_robin;
-	ksocknal_tunables.ksnd_keepalive	  = &keepalive;
+	ksocknal_tunables.ksnd_nagle              = &nagle;
+	ksocknal_tunables.ksnd_round_robin        = &round_robin;
+	ksocknal_tunables.ksnd_keepalive          = &keepalive;
 	ksocknal_tunables.ksnd_keepalive_idle     = &keepalive_idle;
 	ksocknal_tunables.ksnd_keepalive_count    = &keepalive_count;
 	ksocknal_tunables.ksnd_keepalive_intvl    = &keepalive_intvl;
-	ksocknal_tunables.ksnd_credits	    = &credits;
+	ksocknal_tunables.ksnd_credits            = &credits;
 	ksocknal_tunables.ksnd_peertxcredits      = &peer_credits;
 	ksocknal_tunables.ksnd_peerrtrcredits     = &peer_buffer_credits;
-	ksocknal_tunables.ksnd_peertimeout	= &peer_timeout;
-	ksocknal_tunables.ksnd_enable_csum	= &enable_csum;
+	ksocknal_tunables.ksnd_peertimeout        = &peer_timeout;
+	ksocknal_tunables.ksnd_enable_csum        = &enable_csum;
 	ksocknal_tunables.ksnd_inject_csum_error  = &inject_csum_error;
 	ksocknal_tunables.ksnd_nonblk_zcack       = &nonblk_zcack;
 	ksocknal_tunables.ksnd_zc_min_payload     = &zc_min_payload;
-	ksocknal_tunables.ksnd_zc_recv	    = &zc_recv;
+	ksocknal_tunables.ksnd_zc_recv            = &zc_recv;
 	ksocknal_tunables.ksnd_zc_recv_min_nfrags = &zc_recv_min_nfrags;
 
-
-
 #if SOCKNAL_VERSION_DEBUG
-	ksocknal_tunables.ksnd_protocol	   = &protocol;
+	ksocknal_tunables.ksnd_protocol           = &protocol;
 #endif
 
 	if (*ksocknal_tunables.ksnd_zc_min_payload < (2 << 10))
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
index 8596581f54ff..986bce4c9f3b 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
@@ -52,7 +52,7 @@ ksocknal_queue_tx_msg_v1(ksock_conn_t *conn, ksock_tx_t *tx_msg)
 void
 ksocknal_next_tx_carrier(ksock_conn_t *conn)
 {
-	ksock_tx_t     *tx = conn->ksnc_tx_carrier;
+	ksock_tx_t *tx = conn->ksnc_tx_carrier;
 
 	/* Called holding BH lock: conn->ksnc_scheduler->kss_lock */
 	LASSERT(!list_empty(&conn->ksnc_tx_queue));
@@ -119,7 +119,7 @@ ksocknal_queue_tx_zcack_v2(ksock_conn_t *conn,
 static ksock_tx_t *
 ksocknal_queue_tx_msg_v2(ksock_conn_t *conn, ksock_tx_t *tx_msg)
 {
-	ksock_tx_t  *tx  = conn->ksnc_tx_carrier;
+	ksock_tx_t *tx  = conn->ksnc_tx_carrier;
 
 	/*
 	 * Enqueue tx_msg:
@@ -361,10 +361,10 @@ ksocknal_match_tx_v3(ksock_conn_t *conn, ksock_tx_t *tx, int nonblk)
 static int
 ksocknal_handle_zcreq(ksock_conn_t *c, __u64 cookie, int remote)
 {
-	ksock_peer_t   *peer = c->ksnc_peer;
-	ksock_conn_t   *conn;
-	ksock_tx_t     *tx;
-	int	     rc;
+	ksock_peer_t *peer = c->ksnc_peer;
+	ksock_conn_t *conn;
+	ksock_tx_t *tx;
+	int rc;
 
 	read_lock(&ksocknal_data.ksnd_global_lock);
 
@@ -405,11 +405,11 @@ ksocknal_handle_zcreq(ksock_conn_t *c, __u64 cookie, int remote)
 static int
 ksocknal_handle_zcack(ksock_conn_t *conn, __u64 cookie1, __u64 cookie2)
 {
-	ksock_peer_t      *peer = conn->ksnc_peer;
-	ksock_tx_t	*tx;
-	ksock_tx_t	*tmp;
+	ksock_peer_t *peer = conn->ksnc_peer;
+	ksock_tx_t *tx;
+	ksock_tx_t *tmp;
 	LIST_HEAD(zlist);
-	int		count;
+	int count;
 
 	if (cookie1 == 0)
 		cookie1 = cookie2;
@@ -452,11 +452,11 @@ ksocknal_handle_zcack(ksock_conn_t *conn, __u64 cookie1, __u64 cookie2)
 static int
 ksocknal_send_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 {
-	struct socket	*sock = conn->ksnc_sock;
-	lnet_hdr_t	  *hdr;
+	struct socket *sock = conn->ksnc_sock;
+	lnet_hdr_t *hdr;
 	lnet_magicversion_t *hmv;
-	int		  rc;
-	int		  i;
+	int rc;
+	int i;
 
 	CLASSERT(sizeof(lnet_magicversion_t) == offsetof(lnet_hdr_t, src_nid));
 
@@ -470,7 +470,7 @@ ksocknal_send_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 
 	/* Re-organize V2.x message header to V1.x (lnet_hdr_t)
 	 * header and send out */
-	hmv->magic	 = cpu_to_le32 (LNET_PROTO_TCP_MAGIC);
+	hmv->magic         = cpu_to_le32 (LNET_PROTO_TCP_MAGIC);
 	hmv->version_major = cpu_to_le16 (KSOCK_PROTO_V1_MAJOR);
 	hmv->version_minor = cpu_to_le16 (KSOCK_PROTO_V1_MINOR);
 
@@ -488,16 +488,14 @@ ksocknal_send_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 		LNET_UNLOCK();
 	}
 
-	hdr->src_nid	= cpu_to_le64 (hello->kshm_src_nid);
-	hdr->src_pid	= cpu_to_le32 (hello->kshm_src_pid);
-	hdr->type	   = cpu_to_le32 (LNET_MSG_HELLO);
+	hdr->src_nid = cpu_to_le64 (hello->kshm_src_nid);
+	hdr->src_pid = cpu_to_le32 (hello->kshm_src_pid);
+	hdr->type = cpu_to_le32 (LNET_MSG_HELLO);
 	hdr->payload_length = cpu_to_le32 (hello->kshm_nips * sizeof(__u32));
 	hdr->msg.hello.type = cpu_to_le32 (hello->kshm_ctype);
 	hdr->msg.hello.incarnation = cpu_to_le64 (hello->kshm_src_incarnation);
 
-	rc = libcfs_sock_write(sock, hdr, sizeof(*hdr),
-			       lnet_acceptor_timeout());
-
+	rc = lnet_sock_write(sock, hdr, sizeof(*hdr), lnet_acceptor_timeout());
 	if (rc != 0) {
 		CNETERR("Error %d sending HELLO hdr to %pI4h/%d\n",
 			rc, &conn->ksnc_ipaddr, conn->ksnc_port);
@@ -511,9 +509,9 @@ ksocknal_send_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 		hello->kshm_ips[i] = __cpu_to_le32 (hello->kshm_ips[i]);
 	}
 
-	rc = libcfs_sock_write(sock, hello->kshm_ips,
-			       hello->kshm_nips * sizeof(__u32),
-			       lnet_acceptor_timeout());
+	rc = lnet_sock_write(sock, hello->kshm_ips,
+			     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,
@@ -529,7 +527,7 @@ static int
 ksocknal_send_hello_v2(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 {
 	struct socket *sock = conn->ksnc_sock;
-	int	     rc;
+	int rc;
 
 	hello->kshm_magic   = LNET_PROTO_MAGIC;
 	hello->kshm_version = conn->ksnc_proto->pro_version;
@@ -544,9 +542,8 @@ ksocknal_send_hello_v2(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 		LNET_UNLOCK();
 	}
 
-	rc = libcfs_sock_write(sock, hello, offsetof(ksock_hello_msg_t, kshm_ips),
-			       lnet_acceptor_timeout());
-
+	rc = lnet_sock_write(sock, hello, offsetof(ksock_hello_msg_t, kshm_ips),
+			     lnet_acceptor_timeout());
 	if (rc != 0) {
 		CNETERR("Error %d sending HELLO hdr to %pI4h/%d\n",
 			rc, &conn->ksnc_ipaddr, conn->ksnc_port);
@@ -556,9 +553,9 @@ ksocknal_send_hello_v2(ksock_conn_t *conn, ksock_hello_msg_t *hello)
 	if (hello->kshm_nips == 0)
 		return 0;
 
-	rc = libcfs_sock_write(sock, hello->kshm_ips,
-			       hello->kshm_nips * sizeof(__u32),
-			       lnet_acceptor_timeout());
+	rc = lnet_sock_write(sock, hello->kshm_ips,
+			     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,
@@ -572,10 +569,10 @@ static int
 ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,
 		       int timeout)
 {
-	struct socket	*sock = conn->ksnc_sock;
-	lnet_hdr_t	  *hdr;
-	int		  rc;
-	int		  i;
+	struct socket *sock = conn->ksnc_sock;
+	lnet_hdr_t *hdr;
+	int rc;
+	int i;
 
 	LIBCFS_ALLOC(hdr, sizeof(*hdr));
 	if (hdr == NULL) {
@@ -583,9 +580,9 @@ ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,
 		return -ENOMEM;
 	}
 
-	rc = libcfs_sock_read(sock, &hdr->src_nid,
-			      sizeof(*hdr) - offsetof(lnet_hdr_t, src_nid),
-			      timeout);
+	rc = lnet_sock_read(sock, &hdr->src_nid,
+			    sizeof(*hdr) - offsetof(lnet_hdr_t, src_nid),
+			    timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading rest of HELLO hdr from %pI4h\n",
 			rc, &conn->ksnc_ipaddr);
@@ -602,12 +599,12 @@ ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,
 		goto out;
 	}
 
-	hello->kshm_src_nid	 = le64_to_cpu(hdr->src_nid);
-	hello->kshm_src_pid	 = le32_to_cpu(hdr->src_pid);
+	hello->kshm_src_nid         = le64_to_cpu(hdr->src_nid);
+	hello->kshm_src_pid         = le32_to_cpu(hdr->src_pid);
 	hello->kshm_src_incarnation = le64_to_cpu(hdr->msg.hello.incarnation);
-	hello->kshm_ctype	   = le32_to_cpu(hdr->msg.hello.type);
-	hello->kshm_nips	    = le32_to_cpu(hdr->payload_length) /
-					 sizeof(__u32);
+	hello->kshm_ctype           = le32_to_cpu(hdr->msg.hello.type);
+	hello->kshm_nips            = le32_to_cpu(hdr->payload_length) /
+						  sizeof(__u32);
 
 	if (hello->kshm_nips > LNET_MAX_INTERFACES) {
 		CERROR("Bad nips %d from ip %pI4h\n",
@@ -619,8 +616,8 @@ ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,
 	if (hello->kshm_nips == 0)
 		goto out;
 
-	rc = libcfs_sock_read(sock, hello->kshm_ips,
-			      hello->kshm_nips * sizeof(__u32), timeout);
+	rc = lnet_sock_read(sock, hello->kshm_ips,
+			    hello->kshm_nips * sizeof(__u32), timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading IPs from ip %pI4h\n",
 			rc, &conn->ksnc_ipaddr);
@@ -647,19 +644,19 @@ out:
 static int
 ksocknal_recv_hello_v2(ksock_conn_t *conn, ksock_hello_msg_t *hello, int timeout)
 {
-	struct socket   *sock = conn->ksnc_sock;
-	int		rc;
-	int		i;
+	struct socket *sock = conn->ksnc_sock;
+	int rc;
+	int i;
 
 	if (hello->kshm_magic == LNET_PROTO_MAGIC)
 		conn->ksnc_flip = 0;
 	else
 		conn->ksnc_flip = 1;
 
-	rc = libcfs_sock_read(sock, &hello->kshm_src_nid,
-			      offsetof(ksock_hello_msg_t, kshm_ips) -
-				       offsetof(ksock_hello_msg_t, kshm_src_nid),
-			      timeout);
+	rc = lnet_sock_read(sock, &hello->kshm_src_nid,
+			    offsetof(ksock_hello_msg_t, kshm_ips) -
+				     offsetof(ksock_hello_msg_t, kshm_src_nid),
+			    timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading HELLO from %pI4h\n",
 			rc, &conn->ksnc_ipaddr);
@@ -687,8 +684,8 @@ ksocknal_recv_hello_v2(ksock_conn_t *conn, ksock_hello_msg_t *hello, int timeout
 	if (hello->kshm_nips == 0)
 		return 0;
 
-	rc = libcfs_sock_read(sock, hello->kshm_ips,
-			      hello->kshm_nips * sizeof(__u32), timeout);
+	rc = lnet_sock_read(sock, hello->kshm_ips,
+			    hello->kshm_nips * sizeof(__u32), timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading IPs from ip %pI4h\n",
 			rc, &conn->ksnc_ipaddr);
@@ -746,9 +743,9 @@ ksocknal_pack_msg_v2(ksock_tx_t *tx)
 static void
 ksocknal_unpack_msg_v1(ksock_msg_t *msg)
 {
-	msg->ksm_csum	   = 0;
-	msg->ksm_type	   = KSOCK_MSG_LNET;
-	msg->ksm_zc_cookies[0]  = msg->ksm_zc_cookies[1]  = 0;
+	msg->ksm_csum = 0;
+	msg->ksm_type = KSOCK_MSG_LNET;
+	msg->ksm_zc_cookies[0] = msg->ksm_zc_cookies[1]  = 0;
 }
 
 static void
@@ -758,40 +755,40 @@ ksocknal_unpack_msg_v2(ksock_msg_t *msg)
 }
 
 ksock_proto_t  ksocknal_protocol_v1x = {
-	.pro_version	    = KSOCK_PROTO_V1,
-	.pro_send_hello	 = ksocknal_send_hello_v1,
-	.pro_recv_hello	 = ksocknal_recv_hello_v1,
-	.pro_pack	       = ksocknal_pack_msg_v1,
-	.pro_unpack	     = ksocknal_unpack_msg_v1,
-	.pro_queue_tx_msg       = ksocknal_queue_tx_msg_v1,
-	.pro_handle_zcreq       = NULL,
-	.pro_handle_zcack       = NULL,
-	.pro_queue_tx_zcack     = NULL,
-	.pro_match_tx	   = ksocknal_match_tx
+	.pro_version        = KSOCK_PROTO_V1,
+	.pro_send_hello     = ksocknal_send_hello_v1,
+	.pro_recv_hello     = ksocknal_recv_hello_v1,
+	.pro_pack           = ksocknal_pack_msg_v1,
+	.pro_unpack         = ksocknal_unpack_msg_v1,
+	.pro_queue_tx_msg   = ksocknal_queue_tx_msg_v1,
+	.pro_handle_zcreq   = NULL,
+	.pro_handle_zcack   = NULL,
+	.pro_queue_tx_zcack = NULL,
+	.pro_match_tx       = ksocknal_match_tx
 };
 
 ksock_proto_t  ksocknal_protocol_v2x = {
-	.pro_version	    = KSOCK_PROTO_V2,
-	.pro_send_hello	 = ksocknal_send_hello_v2,
-	.pro_recv_hello	 = ksocknal_recv_hello_v2,
-	.pro_pack	       = ksocknal_pack_msg_v2,
-	.pro_unpack	     = ksocknal_unpack_msg_v2,
-	.pro_queue_tx_msg       = ksocknal_queue_tx_msg_v2,
-	.pro_queue_tx_zcack     = ksocknal_queue_tx_zcack_v2,
-	.pro_handle_zcreq       = ksocknal_handle_zcreq,
-	.pro_handle_zcack       = ksocknal_handle_zcack,
-	.pro_match_tx	   = ksocknal_match_tx
+	.pro_version        = KSOCK_PROTO_V2,
+	.pro_send_hello     = ksocknal_send_hello_v2,
+	.pro_recv_hello     = ksocknal_recv_hello_v2,
+	.pro_pack           = ksocknal_pack_msg_v2,
+	.pro_unpack         = ksocknal_unpack_msg_v2,
+	.pro_queue_tx_msg   = ksocknal_queue_tx_msg_v2,
+	.pro_queue_tx_zcack = ksocknal_queue_tx_zcack_v2,
+	.pro_handle_zcreq   = ksocknal_handle_zcreq,
+	.pro_handle_zcack   = ksocknal_handle_zcack,
+	.pro_match_tx       = ksocknal_match_tx
 };
 
 ksock_proto_t  ksocknal_protocol_v3x = {
-	.pro_version	    = KSOCK_PROTO_V3,
-	.pro_send_hello	 = ksocknal_send_hello_v2,
-	.pro_recv_hello	 = ksocknal_recv_hello_v2,
-	.pro_pack	       = ksocknal_pack_msg_v2,
-	.pro_unpack	     = ksocknal_unpack_msg_v2,
-	.pro_queue_tx_msg       = ksocknal_queue_tx_msg_v2,
-	.pro_queue_tx_zcack     = ksocknal_queue_tx_zcack_v3,
-	.pro_handle_zcreq       = ksocknal_handle_zcreq,
-	.pro_handle_zcack       = ksocknal_handle_zcack,
-	.pro_match_tx	   = ksocknal_match_tx_v3
+	.pro_version        = KSOCK_PROTO_V3,
+	.pro_send_hello     = ksocknal_send_hello_v2,
+	.pro_recv_hello     = ksocknal_recv_hello_v2,
+	.pro_pack           = ksocknal_pack_msg_v2,
+	.pro_unpack         = ksocknal_unpack_msg_v2,
+	.pro_queue_tx_msg   = ksocknal_queue_tx_msg_v2,
+	.pro_queue_tx_zcack = ksocknal_queue_tx_zcack_v3,
+	.pro_handle_zcreq   = ksocknal_handle_zcreq,
+	.pro_handle_zcack   = ksocknal_handle_zcack,
+	.pro_match_tx       = ksocknal_match_tx_v3
 };
diff --git a/drivers/staging/lustre/lnet/lnet/Makefile b/drivers/staging/lustre/lnet/lnet/Makefile
index 336b8ea4fdf6..52492fb10f85 100644
--- a/drivers/staging/lustre/lnet/lnet/Makefile
+++ b/drivers/staging/lustre/lnet/lnet/Makefile
@@ -1,5 +1,6 @@
 obj-$(CONFIG_LNET) += lnet.o
 
-lnet-y := api-ni.o config.o lib-me.o lib-msg.o lib-eq.o	\
-	  lib-md.o lib-ptl.o lib-move.o module.o lo.o router.o		\
-	  router_proc.o acceptor.o peer.o
+lnet-y := api-ni.o config.o					\
+	  lib-me.o lib-msg.o lib-eq.o lib-md.o lib-ptl.o	\
+	  lib-socket.o lib-move.o module.o lo.o			\
+	  router.o router_proc.o acceptor.o peer.o
diff --git a/drivers/staging/lustre/lnet/lnet/acceptor.c b/drivers/staging/lustre/lnet/lnet/acceptor.c
index 72fd1bf70ca0..99f8396f3822 100644
--- a/drivers/staging/lustre/lnet/lnet/acceptor.c
+++ b/drivers/staging/lustre/lnet/lnet/acceptor.c
@@ -35,9 +35,9 @@
  */
 
 #define DEBUG_SUBSYSTEM S_LNET
+#include <linux/completion.h>
 #include "../../include/linux/lnet/lib-lnet.h"
 
-
 static int   accept_port    = 988;
 static int   accept_backlog = 127;
 static int   accept_timeout = 5;
@@ -143,10 +143,10 @@ lnet_connect(struct socket **sockp, lnet_nid_t peer_nid,
 	    __u32 local_ip, __u32 peer_ip, int peer_port)
 {
 	lnet_acceptor_connreq_t cr;
-	struct socket	   *sock;
-	int		     rc;
-	int		     port;
-	int		     fatal;
+	struct socket *sock;
+	int rc;
+	int port;
+	int fatal;
 
 	CLASSERT(sizeof(cr) <= 16);	    /* not too big to be on the stack */
 
@@ -155,9 +155,8 @@ lnet_connect(struct socket **sockp, lnet_nid_t peer_nid,
 	     --port) {
 		/* Iterate through reserved ports. */
 
-		rc = libcfs_sock_connect(&sock, &fatal,
-					 local_ip, port,
-					 peer_ip, peer_port);
+		rc = lnet_sock_connect(&sock, &fatal, local_ip, port, peer_ip,
+				       peer_port);
 		if (rc != 0) {
 			if (fatal)
 				goto failed;
@@ -184,8 +183,7 @@ lnet_connect(struct socket **sockp, lnet_nid_t peer_nid,
 			lnet_net_unlock(LNET_LOCK_EX);
 		}
 
-		rc = libcfs_sock_write(sock, &cr, sizeof(cr),
-				       accept_timeout);
+		rc = lnet_sock_write(sock, &cr, sizeof(cr), accept_timeout);
 		if (rc != 0)
 			goto failed_sock;
 
@@ -197,7 +195,7 @@ lnet_connect(struct socket **sockp, lnet_nid_t peer_nid,
 	goto failed;
 
  failed_sock:
-	libcfs_sock_release(sock);
+	sock_release(sock);
  failed:
 	lnet_connect_console_error(rc, peer_nid, peer_ip, peer_port);
 	return rc;
@@ -211,16 +209,16 @@ static int
 lnet_accept(struct socket *sock, __u32 magic)
 {
 	lnet_acceptor_connreq_t cr;
-	__u32		   peer_ip;
-	int		     peer_port;
-	int		     rc;
-	int		     flip;
-	lnet_ni_t	      *ni;
-	char		   *str;
+	__u32 peer_ip;
+	int peer_port;
+	int rc;
+	int flip;
+	lnet_ni_t *ni;
+	char *str;
 
 	LASSERT(sizeof(cr) <= 16);	     /* not too big for the stack */
 
-	rc = libcfs_sock_getaddr(sock, 1, &peer_ip, &peer_port);
+	rc = lnet_sock_getaddr(sock, 1, &peer_ip, &peer_port);
 	LASSERT(rc == 0);		      /* we succeeded before */
 
 	if (!lnet_accept_magic(magic, LNET_PROTO_ACCEPTOR_MAGIC)) {
@@ -234,8 +232,8 @@ lnet_accept(struct socket *sock, __u32 magic)
 			memset(&cr, 0, sizeof(cr));
 			cr.acr_magic = LNET_PROTO_ACCEPTOR_MAGIC;
 			cr.acr_version = LNET_PROTO_ACCEPTOR_VERSION;
-			rc = libcfs_sock_write(sock, &cr, sizeof(cr),
-					       accept_timeout);
+			rc = lnet_sock_write(sock, &cr, sizeof(cr),
+					     accept_timeout);
 
 			if (rc != 0)
 				CERROR("Error sending magic+version in response to LNET magic from %pI4h: %d\n",
@@ -245,8 +243,6 @@ lnet_accept(struct socket *sock, __u32 magic)
 
 		if (magic == le32_to_cpu(LNET_PROTO_TCP_MAGIC))
 			str = "'old' socknal/tcpnal";
-		else if (lnet_accept_magic(magic, LNET_PROTO_RA_MAGIC))
-			str = "'old' ranal";
 		else
 			str = "unrecognised";
 
@@ -257,9 +253,8 @@ lnet_accept(struct socket *sock, __u32 magic)
 
 	flip = (magic != LNET_PROTO_ACCEPTOR_MAGIC);
 
-	rc = libcfs_sock_read(sock, &cr.acr_version,
-			      sizeof(cr.acr_version),
-			      accept_timeout);
+	rc = lnet_sock_read(sock, &cr.acr_version, sizeof(cr.acr_version),
+			    accept_timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading connection request version from %pI4h\n",
 			rc, &peer_ip);
@@ -280,19 +275,17 @@ lnet_accept(struct socket *sock, __u32 magic)
 		cr.acr_magic = LNET_PROTO_ACCEPTOR_MAGIC;
 		cr.acr_version = LNET_PROTO_ACCEPTOR_VERSION;
 
-		rc = libcfs_sock_write(sock, &cr, sizeof(cr),
-				       accept_timeout);
-
+		rc = lnet_sock_write(sock, &cr, sizeof(cr), accept_timeout);
 		if (rc != 0)
 			CERROR("Error sending magic+version in response to version %d from %pI4h: %d\n",
 			       peer_version, &peer_ip, rc);
 		return -EPROTO;
 	}
 
-	rc = libcfs_sock_read(sock, &cr.acr_nid,
-			      sizeof(cr) -
-			      offsetof(lnet_acceptor_connreq_t, acr_nid),
-			      accept_timeout);
+	rc = lnet_sock_read(sock, &cr.acr_nid,
+			    sizeof(cr) -
+			    offsetof(lnet_acceptor_connreq_t, acr_nid),
+			    accept_timeout);
 	if (rc != 0) {
 		CERROR("Error %d reading connection request from %pI4h\n",
 			rc, &peer_ip);
@@ -333,18 +326,18 @@ static int
 lnet_acceptor(void *arg)
 {
 	struct socket *newsock;
-	int	    rc;
-	__u32	  magic;
-	__u32	  peer_ip;
-	int	    peer_port;
-	int	    secure = (int)((long_ptr_t)arg);
+	int rc;
+	__u32 magic;
+	__u32 peer_ip;
+	int peer_port;
+	int secure = (int)((long_ptr_t)arg);
 
 	LASSERT(lnet_acceptor_state.pta_sock == NULL);
 
 	cfs_block_allsigs();
 
-	rc = libcfs_sock_listen(&lnet_acceptor_state.pta_sock,
-				0, accept_port, accept_backlog);
+	rc = lnet_sock_listen(&lnet_acceptor_state.pta_sock, 0, accept_port,
+			      accept_backlog);
 	if (rc != 0) {
 		if (rc == -EADDRINUSE)
 			LCONSOLE_ERROR_MSG(0x122, "Can't start acceptor on port %d: port already in use\n",
@@ -367,7 +360,7 @@ lnet_acceptor(void *arg)
 
 	while (!lnet_acceptor_state.pta_shutdown) {
 
-		rc = libcfs_sock_accept(&newsock, lnet_acceptor_state.pta_sock);
+		rc = lnet_sock_accept(&newsock, lnet_acceptor_state.pta_sock);
 		if (rc != 0) {
 			if (rc != -EAGAIN) {
 				CWARN("Accept error %d: pausing...\n", rc);
@@ -377,13 +370,13 @@ lnet_acceptor(void *arg)
 			continue;
 		}
 
-		/* maybe we're waken up with libcfs_sock_abort_accept() */
+		/* maybe the LNet acceptor thread has been waken */
 		if (lnet_acceptor_state.pta_shutdown) {
-			libcfs_sock_release(newsock);
+			sock_release(newsock);
 			break;
 		}
 
-		rc = libcfs_sock_getaddr(newsock, 1, &peer_ip, &peer_port);
+		rc = lnet_sock_getaddr(newsock, 1, &peer_ip, &peer_port);
 		if (rc != 0) {
 			CERROR("Can't determine new connection's address\n");
 			goto failed;
@@ -395,8 +388,8 @@ lnet_acceptor(void *arg)
 			goto failed;
 		}
 
-		rc = libcfs_sock_read(newsock, &magic, sizeof(magic),
-				      accept_timeout);
+		rc = lnet_sock_read(newsock, &magic, sizeof(magic),
+				    accept_timeout);
 		if (rc != 0) {
 			CERROR("Error %d reading connection request from %pI4h\n",
 				rc, &peer_ip);
@@ -410,10 +403,10 @@ lnet_acceptor(void *arg)
 		continue;
 
 failed:
-		libcfs_sock_release(newsock);
+		sock_release(newsock);
 	}
 
-	libcfs_sock_release(lnet_acceptor_state.pta_sock);
+	sock_release(lnet_acceptor_state.pta_sock);
 	lnet_acceptor_state.pta_sock = NULL;
 
 	CDEBUG(D_NET, "Acceptor stopping\n");
@@ -444,7 +437,7 @@ accept2secure(const char *acc, long *sec)
 int
 lnet_acceptor_start(void)
 {
-	int  rc;
+	int rc;
 	long rc2;
 	long secure;
 
@@ -493,7 +486,7 @@ lnet_acceptor_stop(void)
 		return;
 
 	lnet_acceptor_state.pta_shutdown = 1;
-	libcfs_sock_abort_accept(lnet_acceptor_state.pta_sock);
+	wake_up_all(sk_sleep(lnet_acceptor_state.pta_sock->sk));
 
 	/* block until acceptor signals exit */
 	wait_for_completion(&lnet_acceptor_state.pta_signal);
diff --git a/drivers/staging/lustre/lnet/lnet/api-ni.c b/drivers/staging/lustre/lnet/lnet/api-ni.c
index 4a14e5109821..d14fe70a56df 100644
--- a/drivers/staging/lustre/lnet/lnet/api-ni.c
+++ b/drivers/staging/lustre/lnet/lnet/api-ni.c
@@ -35,13 +35,14 @@
  */
 
 #define DEBUG_SUBSYSTEM S_LNET
-#include "../../include/linux/lnet/lib-lnet.h"
 #include <linux/log2.h>
 #include <linux/ktime.h>
 
+#include "../../include/linux/lnet/lib-lnet.h"
+
 #define D_LNI D_CONSOLE
 
-lnet_t      the_lnet;			   /* THE state of the network */
+lnet_t the_lnet;			   /* THE state of the network */
 EXPORT_SYMBOL(the_lnet);
 
 
@@ -70,8 +71,8 @@ lnet_get_routes(void)
 static char *
 lnet_get_networks(void)
 {
-	char   *nets;
-	int     rc;
+	char *nets;
+	int rc;
 
 	if (*networks != 0 && *ip2nets != 0) {
 		LCONSOLE_ERROR_MSG(0x101, "Please specify EITHER 'networks' or 'ip2nets' but not both at once\n");
@@ -98,17 +99,11 @@ lnet_init_locks(void)
 	mutex_init(&the_lnet.ln_api_mutex);
 }
 
-static void
-lnet_fini_locks(void)
-{
-}
-
-
 static int
 lnet_create_remote_nets_table(void)
 {
-	int		i;
-	struct list_head	*hash;
+	int i;
+	struct list_head *hash;
 
 	LASSERT(the_lnet.ln_remote_nets_hash == NULL);
 	LASSERT(the_lnet.ln_remote_nets_hbits > 0);
@@ -153,8 +148,6 @@ lnet_destroy_locks(void)
 		cfs_percpt_lock_free(the_lnet.ln_net_lock);
 		the_lnet.ln_net_lock = NULL;
 	}
-
-	lnet_fini_locks();
 }
 
 static int
@@ -273,8 +266,8 @@ static void lnet_assert_wire_constants(void)
 static lnd_t *
 lnet_find_lnd_by_type(int type)
 {
-	lnd_t	      *lnd;
-	struct list_head	 *tmp;
+	lnd_t *lnd;
+	struct list_head *tmp;
 
 	/* holding lnd mutex */
 	list_for_each(tmp, &the_lnet.ln_lnds) {
@@ -290,7 +283,7 @@ lnet_find_lnd_by_type(int type)
 void
 lnet_register_lnd(lnd_t *lnd)
 {
-	LNET_MUTEX_LOCK(&the_lnet.ln_lnd_mutex);
+	mutex_lock(&the_lnet.ln_lnd_mutex);
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(libcfs_isknown_lnd(lnd->lnd_type));
@@ -301,14 +294,14 @@ lnet_register_lnd(lnd_t *lnd)
 
 	CDEBUG(D_NET, "%s LND registered\n", libcfs_lnd2str(lnd->lnd_type));
 
-	LNET_MUTEX_UNLOCK(&the_lnet.ln_lnd_mutex);
+	mutex_unlock(&the_lnet.ln_lnd_mutex);
 }
 EXPORT_SYMBOL(lnet_register_lnd);
 
 void
 lnet_unregister_lnd(lnd_t *lnd)
 {
-	LNET_MUTEX_LOCK(&the_lnet.ln_lnd_mutex);
+	mutex_lock(&the_lnet.ln_lnd_mutex);
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(lnet_find_lnd_by_type(lnd->lnd_type) == lnd);
@@ -317,7 +310,7 @@ lnet_unregister_lnd(lnd_t *lnd)
 	list_del(&lnd->lnd_list);
 	CDEBUG(D_NET, "%s LND unregistered\n", libcfs_lnd2str(lnd->lnd_type));
 
-	LNET_MUTEX_UNLOCK(&the_lnet.ln_lnd_mutex);
+	mutex_unlock(&the_lnet.ln_lnd_mutex);
 }
 EXPORT_SYMBOL(lnet_unregister_lnd);
 
@@ -325,7 +318,7 @@ void
 lnet_counters_get(lnet_counters_t *counters)
 {
 	lnet_counters_t *ctr;
-	int		i;
+	int i;
 
 	memset(counters, 0, sizeof(*counters));
 
@@ -353,7 +346,7 @@ void
 lnet_counters_reset(void)
 {
 	lnet_counters_t *counters;
-	int		i;
+	int i;
 
 	lnet_net_lock(LNET_LOCK_EX);
 
@@ -364,56 +357,6 @@ lnet_counters_reset(void)
 }
 EXPORT_SYMBOL(lnet_counters_reset);
 
-#ifdef LNET_USE_LIB_FREELIST
-
-int
-lnet_freelist_init(lnet_freelist_t *fl, int n, int size)
-{
-	char *space;
-
-	LASSERT(n > 0);
-
-	size += offsetof(lnet_freeobj_t, fo_contents);
-
-	LIBCFS_ALLOC(space, n * size);
-	if (space == NULL)
-		return -ENOMEM;
-
-	INIT_LIST_HEAD(&fl->fl_list);
-	fl->fl_objs = space;
-	fl->fl_nobjs = n;
-	fl->fl_objsize = size;
-
-	do {
-		memset(space, 0, size);
-		list_add((struct list_head *)space, &fl->fl_list);
-		space += size;
-	} while (--n != 0);
-
-	return 0;
-}
-
-void
-lnet_freelist_fini(lnet_freelist_t *fl)
-{
-	struct list_head       *el;
-	int	       count;
-
-	if (fl->fl_nobjs == 0)
-		return;
-
-	count = 0;
-	for (el = fl->fl_list.next; el != &fl->fl_list; el = el->next)
-		count++;
-
-	LASSERT(count == fl->fl_nobjs);
-
-	LIBCFS_FREE(fl->fl_objs, fl->fl_nobjs * fl->fl_objsize);
-	memset(fl, 0, sizeof(*fl));
-}
-
-#endif /* LNET_USE_LIB_FREELIST */
-
 static __u64
 lnet_create_interface_cookie(void)
 {
@@ -441,7 +384,7 @@ lnet_res_type2str(int type)
 static void
 lnet_res_container_cleanup(struct lnet_res_container *rec)
 {
-	int	count = 0;
+	int count = 0;
 
 	if (rec->rec_type == 0) /* not set yet, it's uninitialized */
 		return;
@@ -470,9 +413,6 @@ lnet_res_container_cleanup(struct lnet_res_container *rec)
 		       count, lnet_res_type2str(rec->rec_type));
 	}
 
-#ifdef LNET_USE_LIB_FREELIST
-	lnet_freelist_fini(&rec->rec_freelist);
-#endif
 	if (rec->rec_lh_hash != NULL) {
 		LIBCFS_FREE(rec->rec_lh_hash,
 			    LNET_LH_HASH_SIZE * sizeof(rec->rec_lh_hash[0]));
@@ -483,23 +423,15 @@ lnet_res_container_cleanup(struct lnet_res_container *rec)
 }
 
 static int
-lnet_res_container_setup(struct lnet_res_container *rec,
-			 int cpt, int type, int objnum, int objsz)
+lnet_res_container_setup(struct lnet_res_container *rec, int cpt, int type)
 {
-	int	rc = 0;
-	int	i;
+	int rc = 0;
+	int i;
 
 	LASSERT(rec->rec_type == 0);
 
 	rec->rec_type = type;
 	INIT_LIST_HEAD(&rec->rec_active);
-
-#ifdef LNET_USE_LIB_FREELIST
-	memset(&rec->rec_freelist, 0, sizeof(rec->rec_freelist));
-	rc = lnet_freelist_init(&rec->rec_freelist, objnum, objsz);
-	if (rc != 0)
-		goto out;
-#endif
 	rec->rec_lh_cookie = (cpt << LNET_COOKIE_TYPE_BITS) | type;
 
 	/* Arbitrary choice of hash table size */
@@ -525,8 +457,8 @@ out:
 static void
 lnet_res_containers_destroy(struct lnet_res_container **recs)
 {
-	struct lnet_res_container	*rec;
-	int				i;
+	struct lnet_res_container *rec;
+	int i;
 
 	cfs_percpt_for_each(rec, i, recs)
 		lnet_res_container_cleanup(rec);
@@ -535,12 +467,12 @@ lnet_res_containers_destroy(struct lnet_res_container **recs)
 }
 
 static struct lnet_res_container **
-lnet_res_containers_create(int type, int objnum, int objsz)
+lnet_res_containers_create(int type)
 {
-	struct lnet_res_container	**recs;
-	struct lnet_res_container	*rec;
-	int				rc;
-	int				i;
+	struct lnet_res_container **recs;
+	struct lnet_res_container *rec;
+	int rc;
+	int i;
 
 	recs = cfs_percpt_alloc(lnet_cpt_table(), sizeof(*rec));
 	if (recs == NULL) {
@@ -550,7 +482,7 @@ lnet_res_containers_create(int type, int objnum, int objsz)
 	}
 
 	cfs_percpt_for_each(rec, i, recs) {
-		rc = lnet_res_container_setup(rec, i, type, objnum, objsz);
+		rc = lnet_res_container_setup(rec, i, type);
 		if (rc != 0) {
 			lnet_res_containers_destroy(recs);
 			return NULL;
@@ -564,9 +496,9 @@ lnet_libhandle_t *
 lnet_res_lh_lookup(struct lnet_res_container *rec, __u64 cookie)
 {
 	/* ALWAYS called with lnet_res_lock held */
-	struct list_head		*head;
-	lnet_libhandle_t	*lh;
-	unsigned int		hash;
+	struct list_head *head;
+	lnet_libhandle_t *lh;
+	unsigned int hash;
 
 	if ((cookie & LNET_COOKIE_MASK) != rec->rec_type)
 		return NULL;
@@ -586,8 +518,8 @@ void
 lnet_res_lh_initialize(struct lnet_res_container *rec, lnet_libhandle_t *lh)
 {
 	/* ALWAYS called with lnet_res_lock held */
-	unsigned int	ibits = LNET_COOKIE_TYPE_BITS + LNET_CPT_BITS;
-	unsigned int	hash;
+	unsigned int ibits = LNET_COOKIE_TYPE_BITS + LNET_CPT_BITS;
+	unsigned int hash;
 
 	lh->lh_cookie = rec->rec_lh_cookie;
 	rec->rec_lh_cookie += 1 << ibits;
@@ -605,7 +537,7 @@ lnet_prepare(lnet_pid_t requested_pid)
 {
 	/* Prepare to bring up the network */
 	struct lnet_res_container **recs;
-	int			  rc = 0;
+	int rc = 0;
 
 	LASSERT(the_lnet.ln_refcount == 0);
 
@@ -643,13 +575,11 @@ lnet_prepare(lnet_pid_t requested_pid)
 		goto failed;
 
 	rc = lnet_res_container_setup(&the_lnet.ln_eq_container, 0,
-				      LNET_COOKIE_TYPE_EQ, LNET_FL_MAX_EQS,
-				      sizeof(lnet_eq_t));
+				      LNET_COOKIE_TYPE_EQ);
 	if (rc != 0)
 		goto failed;
 
-	recs = lnet_res_containers_create(LNET_COOKIE_TYPE_ME, LNET_FL_MAX_MES,
-					  sizeof(lnet_me_t));
+	recs = lnet_res_containers_create(LNET_COOKIE_TYPE_ME);
 	if (recs == NULL) {
 		rc = -ENOMEM;
 		goto failed;
@@ -657,8 +587,7 @@ lnet_prepare(lnet_pid_t requested_pid)
 
 	the_lnet.ln_me_containers = recs;
 
-	recs = lnet_res_containers_create(LNET_COOKIE_TYPE_MD, LNET_FL_MAX_MDS,
-					  sizeof(lnet_libmd_t));
+	recs = lnet_res_containers_create(LNET_COOKIE_TYPE_MD);
 	if (recs == NULL) {
 		rc = -ENOMEM;
 		goto failed;
@@ -725,8 +654,8 @@ lnet_unprepare(void)
 lnet_ni_t  *
 lnet_net2ni_locked(__u32 net, int cpt)
 {
-	struct list_head	*tmp;
-	lnet_ni_t	*ni;
+	struct list_head *tmp;
+	lnet_ni_t *ni;
 
 	LASSERT(cpt != LNET_LOCK_EX);
 
@@ -758,8 +687,8 @@ EXPORT_SYMBOL(lnet_net2ni);
 static unsigned int
 lnet_nid_cpt_hash(lnet_nid_t nid, unsigned int number)
 {
-	__u64		key = nid;
-	unsigned int	val;
+	__u64 key = nid;
+	unsigned int val;
 
 	LASSERT(number >= 1 && number <= LNET_CPT_NUMBER);
 
@@ -801,8 +730,8 @@ lnet_cpt_of_nid_locked(lnet_nid_t nid)
 int
 lnet_cpt_of_nid(lnet_nid_t nid)
 {
-	int	cpt;
-	int	cpt2;
+	int cpt;
+	int cpt2;
 
 	if (LNET_CPT_NUMBER == 1)
 		return 0; /* the only one */
@@ -821,8 +750,8 @@ EXPORT_SYMBOL(lnet_cpt_of_nid);
 int
 lnet_islocalnet(__u32 net)
 {
-	struct lnet_ni	*ni;
-	int		cpt;
+	struct lnet_ni *ni;
+	int cpt;
 
 	cpt = lnet_net_lock_current();
 
@@ -838,8 +767,8 @@ lnet_islocalnet(__u32 net)
 lnet_ni_t  *
 lnet_nid2ni_locked(lnet_nid_t nid, int cpt)
 {
-	struct lnet_ni	*ni;
-	struct list_head	*tmp;
+	struct lnet_ni *ni;
+	struct list_head *tmp;
 
 	LASSERT(cpt != LNET_LOCK_EX);
 
@@ -858,8 +787,8 @@ lnet_nid2ni_locked(lnet_nid_t nid, int cpt)
 int
 lnet_islocalnid(lnet_nid_t nid)
 {
-	struct lnet_ni	*ni;
-	int		cpt;
+	struct lnet_ni *ni;
+	int cpt;
 
 	cpt = lnet_net_lock_current();
 	ni = lnet_nid2ni_locked(nid, cpt);
@@ -874,10 +803,10 @@ int
 lnet_count_acceptor_nis(void)
 {
 	/* Return the # of NIs that need the acceptor. */
-	int		count = 0;
-	struct list_head	*tmp;
-	struct lnet_ni	*ni;
-	int		cpt;
+	int count = 0;
+	struct list_head *tmp;
+	struct lnet_ni *ni;
+	int cpt;
 
 	cpt = lnet_net_lock_current();
 	list_for_each(tmp, &the_lnet.ln_nis) {
@@ -895,7 +824,7 @@ lnet_count_acceptor_nis(void)
 static int
 lnet_ni_tq_credits(lnet_ni_t *ni)
 {
-	int	credits;
+	int credits;
 
 	LASSERT(ni->ni_ncpts >= 1);
 
@@ -912,9 +841,9 @@ lnet_ni_tq_credits(lnet_ni_t *ni)
 static void
 lnet_shutdown_lndnis(void)
 {
-	int		i;
-	int		islo;
-	lnet_ni_t	 *ni;
+	int i;
+	int islo;
+	lnet_ni_t *ni;
 
 	/* NB called holding the global mutex */
 
@@ -968,8 +897,8 @@ lnet_shutdown_lndnis(void)
 	 * and shut them down in guaranteed thread context */
 	i = 2;
 	while (!list_empty(&the_lnet.ln_nis_zombie)) {
-		int	*ref;
-		int	j;
+		int *ref;
+		int j;
 
 		ni = list_entry(the_lnet.ln_nis_zombie.next,
 				    lnet_ni_t, ni_list);
@@ -1029,15 +958,15 @@ lnet_shutdown_lndnis(void)
 static int
 lnet_startup_lndnis(void)
 {
-	lnd_t			*lnd;
-	struct lnet_ni		*ni;
-	struct lnet_tx_queue	*tq;
-	struct list_head		nilist;
-	int			i;
-	int		rc = 0;
-	int		lnd_type;
-	int		nicount = 0;
-	char	      *nets = lnet_get_networks();
+	lnd_t *lnd;
+	struct lnet_ni *ni;
+	struct lnet_tx_queue *tq;
+	struct list_head nilist;
+	int i;
+	int rc = 0;
+	int lnd_type;
+	int nicount = 0;
+	char *nets = lnet_get_networks();
 
 	INIT_LIST_HEAD(&nilist);
 
@@ -1063,18 +992,18 @@ lnet_startup_lndnis(void)
 			goto failed;
 		}
 
-		LNET_MUTEX_LOCK(&the_lnet.ln_lnd_mutex);
+		mutex_lock(&the_lnet.ln_lnd_mutex);
 		lnd = lnet_find_lnd_by_type(lnd_type);
 
 		if (lnd == NULL) {
-			LNET_MUTEX_UNLOCK(&the_lnet.ln_lnd_mutex);
+			mutex_unlock(&the_lnet.ln_lnd_mutex);
 			rc = request_module("%s",
 						libcfs_lnd2modname(lnd_type));
-			LNET_MUTEX_LOCK(&the_lnet.ln_lnd_mutex);
+			mutex_lock(&the_lnet.ln_lnd_mutex);
 
 			lnd = lnet_find_lnd_by_type(lnd_type);
 			if (lnd == NULL) {
-				LNET_MUTEX_UNLOCK(&the_lnet.ln_lnd_mutex);
+				mutex_unlock(&the_lnet.ln_lnd_mutex);
 				CERROR("Can't load LND %s, module %s, rc=%d\n",
 				       libcfs_lnd2str(lnd_type),
 				       libcfs_lnd2modname(lnd_type), rc);
@@ -1090,7 +1019,7 @@ lnet_startup_lndnis(void)
 
 		rc = (lnd->lnd_startup)(ni);
 
-		LNET_MUTEX_UNLOCK(&the_lnet.ln_lnd_mutex);
+		mutex_unlock(&the_lnet.ln_lnd_mutex);
 
 		if (rc != 0) {
 			LCONSOLE_ERROR_MSG(0x105, "Error %d starting up LNI %s\n",
@@ -1172,16 +1101,16 @@ lnet_startup_lndnis(void)
  * Initialize LNet library.
  *
  * Only userspace program needs to call this function - it's automatically
- * called in the kernel at module loading time. Caller has to call LNetFini()
- * after a call to LNetInit(), if and only if the latter returned 0. It must
+ * called in the kernel at module loading time. Caller has to call lnet_fini()
+ * after a call to lnet_init(), if and only if the latter returned 0. It must
  * be called exactly once.
  *
  * \return 0 on success, and -ve on failures.
  */
 int
-LNetInit(void)
+lnet_init(void)
 {
-	int	rc;
+	int rc;
 
 	lnet_assert_wire_constants();
 	LASSERT(!the_lnet.ln_init);
@@ -1232,7 +1161,7 @@ LNetInit(void)
 	lnet_register_lnd(&the_lolnd);
 	return 0;
 }
-EXPORT_SYMBOL(LNetInit);
+EXPORT_SYMBOL(lnet_init);
 
 /**
  * Finalize LNet library.
@@ -1240,11 +1169,11 @@ EXPORT_SYMBOL(LNetInit);
  * Only userspace program needs to call this function. It can be called
  * at most once.
  *
- * \pre LNetInit() called with success.
+ * \pre lnet_init() called with success.
  * \pre All LNet users called LNetNIFini() for matching LNetNIInit() calls.
  */
 void
-LNetFini(void)
+lnet_fini(void)
 {
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount == 0);
@@ -1256,12 +1185,12 @@ LNetFini(void)
 
 	the_lnet.ln_init = 0;
 }
-EXPORT_SYMBOL(LNetFini);
+EXPORT_SYMBOL(lnet_fini);
 
 /**
  * Set LNet PID and start LNet interfaces, routing, and forwarding.
  *
- * Userspace program should call this after a successful call to LNetInit().
+ * Userspace program should call this after a successful call to lnet_init().
  * Users must call this function at least once before any other functions.
  * For each successful call there must be a corresponding call to
  * LNetNIFini(). For subsequent calls to LNetNIInit(), \a requested_pid is
@@ -1277,10 +1206,10 @@ EXPORT_SYMBOL(LNetFini);
 int
 LNetNIInit(lnet_pid_t requested_pid)
 {
-	int	 im_a_router = 0;
-	int	 rc;
+	int im_a_router = 0;
+	int rc;
 
-	LNET_MUTEX_LOCK(&the_lnet.ln_api_mutex);
+	mutex_lock(&the_lnet.ln_api_mutex);
 
 	LASSERT(the_lnet.ln_init);
 	CDEBUG(D_OTHER, "refs %d\n", the_lnet.ln_refcount);
@@ -1290,8 +1219,6 @@ LNetNIInit(lnet_pid_t requested_pid)
 		goto out;
 	}
 
-	lnet_get_tunables();
-
 	if (requested_pid == LNET_PID_ANY) {
 		/* Don't instantiate LNET just for me */
 		rc = -ENETDOWN;
@@ -1351,7 +1278,7 @@ LNetNIInit(lnet_pid_t requested_pid)
  failed0:
 	LASSERT(rc < 0);
  out:
-	LNET_MUTEX_UNLOCK(&the_lnet.ln_api_mutex);
+	mutex_unlock(&the_lnet.ln_api_mutex);
 	return rc;
 }
 EXPORT_SYMBOL(LNetNIInit);
@@ -1368,7 +1295,7 @@ EXPORT_SYMBOL(LNetNIInit);
 int
 LNetNIFini(void)
 {
-	LNET_MUTEX_LOCK(&the_lnet.ln_api_mutex);
+	mutex_lock(&the_lnet.ln_api_mutex);
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -1391,7 +1318,7 @@ LNetNIFini(void)
 		lnet_unprepare();
 	}
 
-	LNET_MUTEX_UNLOCK(&the_lnet.ln_api_mutex);
+	mutex_unlock(&the_lnet.ln_api_mutex);
 	return 0;
 }
 EXPORT_SYMBOL(LNetNIFini);
@@ -1413,9 +1340,9 @@ int
 LNetCtl(unsigned int cmd, void *arg)
 {
 	struct libcfs_ioctl_data *data = arg;
-	lnet_process_id_t	 id = {0};
-	lnet_ni_t		*ni;
-	int		       rc;
+	lnet_process_id_t id = {0};
+	lnet_ni_t *ni;
+	int rc;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -1531,10 +1458,10 @@ EXPORT_SYMBOL(LNetCtl);
 int
 LNetGetId(unsigned int index, lnet_process_id_t *id)
 {
-	struct lnet_ni	*ni;
-	struct list_head	*tmp;
-	int		cpt;
-	int		rc = -ENOENT;
+	struct lnet_ni *ni;
+	struct list_head *tmp;
+	int cpt;
+	int rc = -ENOENT;
 
 	LASSERT(the_lnet.ln_init);
 
@@ -1575,11 +1502,11 @@ EXPORT_SYMBOL(LNetSnprintHandle);
 static int
 lnet_create_ping_info(void)
 {
-	int	       i;
-	int	       n;
-	int	       rc;
-	unsigned int      infosz;
-	lnet_ni_t	*ni;
+	int i;
+	int n;
+	int rc;
+	unsigned int infosz;
+	lnet_ni_t *ni;
 	lnet_process_id_t id;
 	lnet_ping_info_t *pinfo;
 
@@ -1633,7 +1560,7 @@ lnet_create_ping_info(void)
 static void
 lnet_destroy_ping_info(void)
 {
-	struct lnet_ni	*ni;
+	struct lnet_ni *ni;
 
 	lnet_net_lock(0);
 
@@ -1654,12 +1581,12 @@ lnet_destroy_ping_info(void)
 int
 lnet_ping_target_init(void)
 {
-	lnet_md_t	 md = { NULL };
-	lnet_handle_me_t  meh;
+	lnet_md_t md = { NULL };
+	lnet_handle_me_t meh;
 	lnet_process_id_t id;
-	int	       rc;
-	int	       rc2;
-	int	       infosz;
+	int rc;
+	int rc2;
+	int infosz;
 
 	rc = lnet_create_ping_info();
 	if (rc != 0)
@@ -1722,11 +1649,11 @@ lnet_ping_target_init(void)
 void
 lnet_ping_target_fini(void)
 {
-	lnet_event_t    event;
-	int	     rc;
-	int	     which;
-	int	     timeout_ms = 1000;
-	sigset_t    blocked = cfs_block_allsigs();
+	lnet_event_t event;
+	int rc;
+	int which;
+	int timeout_ms = 1000;
+	sigset_t blocked = cfs_block_allsigs();
 
 	LNetMDUnlink(the_lnet.ln_ping_target_md);
 	/* NB md could be busy; this just starts the unlink */
@@ -1759,22 +1686,22 @@ lnet_ping_target_fini(void)
 int
 lnet_ping(lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_ids)
 {
-	lnet_handle_eq_t     eqh;
-	lnet_handle_md_t     mdh;
-	lnet_event_t	 event;
-	lnet_md_t	    md = { NULL };
-	int		  which;
-	int		  unlinked = 0;
-	int		  replied = 0;
-	const int	    a_long_time = 60000; /* mS */
-	int		  infosz = offsetof(lnet_ping_info_t, pi_ni[n_ids]);
-	lnet_ping_info_t    *info;
-	lnet_process_id_t    tmpid;
-	int		  i;
-	int		  nob;
-	int		  rc;
-	int		  rc2;
-	sigset_t	 blocked;
+	lnet_handle_eq_t eqh;
+	lnet_handle_md_t mdh;
+	lnet_event_t event;
+	lnet_md_t md = { NULL };
+	int which;
+	int unlinked = 0;
+	int replied = 0;
+	const int a_long_time = 60000; /* mS */
+	int infosz = offsetof(lnet_ping_info_t, pi_ni[n_ids]);
+	lnet_ping_info_t *info;
+	lnet_process_id_t tmpid;
+	int i;
+	int nob;
+	int rc;
+	int rc2;
+	sigset_t blocked;
 
 	if (n_ids <= 0 ||
 	    id.nid == LNET_NID_ANY ||
diff --git a/drivers/staging/lustre/lnet/lnet/config.c b/drivers/staging/lustre/lnet/lnet/config.c
index 2dc4c4a1afd0..9c576ce2f455 100644
--- a/drivers/staging/lustre/lnet/lnet/config.c
+++ b/drivers/staging/lustre/lnet/lnet/config.c
@@ -38,9 +38,9 @@
 #include "../../include/linux/lnet/lib-lnet.h"
 
 struct lnet_text_buf_t {	    /* tmp struct for parsing routes */
-	struct list_head	 ltb_list;	/* stash on lists */
-	int		ltb_size;	/* allocated size */
-	char	       ltb_text[0];     /* text buffer */
+	struct list_head ltb_list;	/* stash on lists */
+	int ltb_size;	/* allocated size */
+	char ltb_text[0];     /* text buffer */
 };
 
 static int lnet_tbnob;			/* track text buf allocation */
@@ -80,8 +80,8 @@ lnet_issep(char c)
 static int
 lnet_net_unique(__u32 net, struct list_head *nilist)
 {
-	struct list_head       *tmp;
-	lnet_ni_t	*ni;
+	struct list_head *tmp;
+	lnet_ni_t *ni;
 
 	list_for_each(tmp, nilist) {
 		ni = list_entry(tmp, lnet_ni_t, ni_list);
@@ -111,10 +111,10 @@ lnet_ni_free(struct lnet_ni *ni)
 static lnet_ni_t *
 lnet_ni_alloc(__u32 net, struct cfs_expr_list *el, struct list_head *nilist)
 {
-	struct lnet_tx_queue	*tq;
-	struct lnet_ni		*ni;
-	int			rc;
-	int			i;
+	struct lnet_tx_queue *tq;
+	struct lnet_ni *ni;
+	int rc;
+	int i;
 
 	if (!lnet_net_unique(net, nilist)) {
 		LCONSOLE_ERROR_MSG(0x111, "Duplicate network specified: %s\n",
@@ -178,13 +178,13 @@ int
 lnet_parse_networks(struct list_head *nilist, char *networks)
 {
 	struct cfs_expr_list *el = NULL;
-	int		tokensize = strlen(networks) + 1;
-	char		*tokens;
-	char		*str;
-	char		*tmp;
-	struct lnet_ni	*ni;
-	__u32		net;
-	int		nnets = 0;
+	int tokensize = strlen(networks) + 1;
+	char *tokens;
+	char *str;
+	char *tmp;
+	struct lnet_ni *ni;
+	__u32 net;
+	int nnets = 0;
 
 	if (strlen(networks) > LNET_SINGLE_TEXTBUF_NOB) {
 		/* _WAY_ conservative */
@@ -210,12 +210,12 @@ lnet_parse_networks(struct list_head *nilist, char *networks)
 		goto failed;
 
 	while (str != NULL && *str != 0) {
-		char	*comma = strchr(str, ',');
-		char	*bracket = strchr(str, '(');
-		char	*square = strchr(str, '[');
-		char	*iface;
-		int	niface;
-		int	rc;
+		char *comma = strchr(str, ',');
+		char *bracket = strchr(str, '(');
+		char *square = strchr(str, '[');
+		char *iface;
+		int niface;
+		int rc;
 
 		/* NB we don't check interface conflicts here; it's the LNDs
 		 * responsibility (if it cares at all) */
@@ -369,7 +369,7 @@ static struct lnet_text_buf_t *
 lnet_new_text_buf(int str_len)
 {
 	struct lnet_text_buf_t *ltb;
-	int	      nob;
+	int nob;
 
 	/* NB allocate space for the terminating 0 */
 	nob = offsetof(struct lnet_text_buf_t, ltb_text[str_len + 1]);
@@ -404,7 +404,7 @@ lnet_free_text_buf(struct lnet_text_buf_t *ltb)
 static void
 lnet_free_text_bufs(struct list_head *tbs)
 {
-	struct lnet_text_buf_t  *ltb;
+	struct lnet_text_buf_t *ltb;
 
 	while (!list_empty(tbs)) {
 		ltb = list_entry(tbs->next, struct lnet_text_buf_t, ltb_list);
@@ -417,11 +417,11 @@ lnet_free_text_bufs(struct list_head *tbs)
 static int
 lnet_str2tbs_sep(struct list_head *tbs, char *str)
 {
-	struct list_head	pending;
-	char	     *sep;
-	int	       nob;
-	int	       i;
-	struct lnet_text_buf_t  *ltb;
+	struct list_head pending;
+	char *sep;
+	int nob;
+	int i;
+	struct lnet_text_buf_t *ltb;
 
 	INIT_LIST_HEAD(&pending);
 
@@ -477,8 +477,8 @@ lnet_expand1tb(struct list_head *list,
 	       char *str, char *sep1, char *sep2,
 	       char *item, int itemlen)
 {
-	int	      len1 = (int)(sep1 - str);
-	int	      len2 = strlen(sep2 + 1);
+	int len1 = (int)(sep1 - str);
+	int len2 = strlen(sep2 + 1);
 	struct lnet_text_buf_t *ltb;
 
 	LASSERT(*sep1 == '[');
@@ -500,18 +500,18 @@ lnet_expand1tb(struct list_head *list,
 static int
 lnet_str2tbs_expand(struct list_head *tbs, char *str)
 {
-	char	      num[16];
-	struct list_head	pending;
-	char	     *sep;
-	char	     *sep2;
-	char	     *parsed;
-	char	     *enditem;
-	int	       lo;
-	int	       hi;
-	int	       stride;
-	int	       i;
-	int	       nob;
-	int	       scanned;
+	char num[16];
+	struct list_head pending;
+	char *sep;
+	char *sep2;
+	char *parsed;
+	char *enditem;
+	int lo;
+	int hi;
+	int stride;
+	int i;
+	int nob;
+	int scanned;
 
 	INIT_LIST_HEAD(&pending);
 
@@ -584,8 +584,8 @@ lnet_str2tbs_expand(struct list_head *tbs, char *str)
 static int
 lnet_parse_hops(char *str, unsigned int *hops)
 {
-	int     len = strlen(str);
-	int     nob = len;
+	int len = strlen(str);
+	int nob = len;
 
 	return (sscanf(str, "%u%n", hops, &nob) >= 1 &&
 		nob == len &&
@@ -597,9 +597,9 @@ lnet_parse_hops(char *str, unsigned int *hops)
 static int
 lnet_parse_priority(char *str, unsigned int *priority, char **token)
 {
-	int   nob;
+	int nob;
 	char *sep;
-	int   len;
+	int len;
 
 	sep = strchr(str, LNET_PRIORITY_SEPARATOR);
 	if (sep == NULL) {
@@ -628,23 +628,23 @@ static int
 lnet_parse_route(char *str, int *im_a_router)
 {
 	/* static scratch buffer OK (single threaded) */
-	static char       cmd[LNET_SINGLE_TEXTBUF_NOB];
-
-	struct list_head	nets;
-	struct list_head	gateways;
-	struct list_head       *tmp1;
-	struct list_head       *tmp2;
-	__u32	     net;
-	lnet_nid_t	nid;
-	struct lnet_text_buf_t  *ltb;
-	int	       rc;
-	char	     *sep;
-	char	     *token = str;
-	int	       ntokens = 0;
-	int	       myrc = -1;
-	unsigned int      hops;
-	int	       got_hops = 0;
-	unsigned int	  priority = 0;
+	static char cmd[LNET_SINGLE_TEXTBUF_NOB];
+
+	struct list_head nets;
+	struct list_head gateways;
+	struct list_head *tmp1;
+	struct list_head *tmp2;
+	__u32 net;
+	lnet_nid_t nid;
+	struct lnet_text_buf_t *ltb;
+	int rc;
+	char *sep;
+	char *token = str;
+	int ntokens = 0;
+	int myrc = -1;
+	unsigned int hops;
+	int got_hops = 0;
+	unsigned int priority = 0;
 
 	INIT_LIST_HEAD(&gateways);
 	INIT_LIST_HEAD(&nets);
@@ -772,7 +772,7 @@ lnet_parse_route(char *str, int *im_a_router)
 static int
 lnet_parse_route_tbs(struct list_head *tbs, int *im_a_router)
 {
-	struct lnet_text_buf_t   *ltb;
+	struct lnet_text_buf_t *ltb;
 
 	while (!list_empty(tbs)) {
 		ltb = list_entry(tbs->next, struct lnet_text_buf_t, ltb_list);
@@ -792,8 +792,8 @@ lnet_parse_route_tbs(struct list_head *tbs, int *im_a_router)
 int
 lnet_parse_routes(char *routes, int *im_a_router)
 {
-	struct list_head	tbs;
-	int	       rc = 0;
+	struct list_head tbs;
+	int rc = 0;
 
 	*im_a_router = 0;
 
@@ -814,8 +814,8 @@ static int
 lnet_match_network_token(char *token, int len, __u32 *ipaddrs, int nip)
 {
 	LIST_HEAD(list);
-	int		rc;
-	int		i;
+	int rc;
+	int i;
 
 	rc = cfs_ip_addr_parse(token, len, &list);
 	if (rc != 0)
@@ -834,13 +834,13 @@ lnet_match_network_tokens(char *net_entry, __u32 *ipaddrs, int nip)
 {
 	static char tokens[LNET_SINGLE_TEXTBUF_NOB];
 
-	int   matched = 0;
-	int   ntokens = 0;
-	int   len;
+	int matched = 0;
+	int ntokens = 0;
+	int len;
 	char *net = NULL;
 	char *sep;
 	char *token;
-	int   rc;
+	int rc;
 
 	LASSERT(strlen(net_entry) < sizeof(tokens));
 
@@ -889,8 +889,8 @@ lnet_match_network_tokens(char *net_entry, __u32 *ipaddrs, int nip)
 static __u32
 lnet_netspec2net(char *netspec)
 {
-	char   *bracket = strchr(netspec, '(');
-	__u32   net;
+	char *bracket = strchr(netspec, '(');
+	__u32 net;
 
 	if (bracket != NULL)
 		*bracket = 0;
@@ -906,15 +906,15 @@ lnet_netspec2net(char *netspec)
 static int
 lnet_splitnets(char *source, struct list_head *nets)
 {
-	int	       offset = 0;
-	int	       offset2;
-	int	       len;
-	struct lnet_text_buf_t  *tb;
-	struct lnet_text_buf_t  *tb2;
-	struct list_head       *t;
-	char	     *sep;
-	char	     *bracket;
-	__u32	     net;
+	int offset = 0;
+	int offset2;
+	int len;
+	struct lnet_text_buf_t *tb;
+	struct lnet_text_buf_t *tb2;
+	struct list_head *t;
+	char *sep;
+	char *bracket;
+	__u32 net;
 
 	LASSERT(!list_empty(nets));
 	LASSERT(nets->next == nets->prev);     /* single entry */
@@ -986,22 +986,22 @@ lnet_splitnets(char *source, struct list_head *nets)
 static int
 lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 {
-	static char	networks[LNET_SINGLE_TEXTBUF_NOB];
-	static char	source[LNET_SINGLE_TEXTBUF_NOB];
-
-	struct list_head	  raw_entries;
-	struct list_head	  matched_nets;
-	struct list_head	  current_nets;
-	struct list_head	 *t;
-	struct list_head	 *t2;
-	struct lnet_text_buf_t    *tb;
-	struct lnet_text_buf_t    *tb2;
-	__u32	       net1;
-	__u32	       net2;
-	int		 len;
-	int		 count;
-	int		 dup;
-	int		 rc;
+	static char networks[LNET_SINGLE_TEXTBUF_NOB];
+	static char source[LNET_SINGLE_TEXTBUF_NOB];
+
+	struct list_head raw_entries;
+	struct list_head matched_nets;
+	struct list_head current_nets;
+	struct list_head *t;
+	struct list_head *t2;
+	struct lnet_text_buf_t *tb;
+	struct lnet_text_buf_t *tb2;
+	__u32 net1;
+	__u32 net2;
+	int len;
+	int count;
+	int dup;
+	int rc;
 
 	INIT_LIST_HEAD(&raw_entries);
 	if (lnet_str2tbs_sep(&raw_entries, ip2nets) < 0) {
@@ -1112,15 +1112,15 @@ lnet_ipaddr_free_enumeration(__u32 *ipaddrs, int nip)
 static int
 lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 {
-	int	up;
-	__u32      netmask;
-	__u32     *ipaddrs;
-	__u32     *ipaddrs2;
-	int	nip;
-	char     **ifnames;
-	int	nif = libcfs_ipif_enumerate(&ifnames);
-	int	i;
-	int	rc;
+	int up;
+	__u32 netmask;
+	__u32 *ipaddrs;
+	__u32 *ipaddrs2;
+	int nip;
+	char **ifnames;
+	int nif = lnet_ipif_enumerate(&ifnames);
+	int i;
+	int rc;
 
 	if (nif <= 0)
 		return nif;
@@ -1128,7 +1128,7 @@ lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 	LIBCFS_ALLOC(ipaddrs, nif * sizeof(*ipaddrs));
 	if (ipaddrs == NULL) {
 		CERROR("Can't allocate ipaddrs[%d]\n", nif);
-		libcfs_ipif_free_enumeration(ifnames, nif);
+		lnet_ipif_free_enumeration(ifnames, nif);
 		return -ENOMEM;
 	}
 
@@ -1136,8 +1136,7 @@ lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 		if (!strcmp(ifnames[i], "lo"))
 			continue;
 
-		rc = libcfs_ipif_query(ifnames[i], &up,
-				       &ipaddrs[nip], &netmask);
+		rc = lnet_ipif_query(ifnames[i], &up, &ipaddrs[nip], &netmask);
 		if (rc != 0) {
 			CWARN("Can't query interface %s: %d\n",
 			      ifnames[i], rc);
@@ -1153,7 +1152,7 @@ lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 		nip++;
 	}
 
-	libcfs_ipif_free_enumeration(ifnames, nif);
+	lnet_ipif_free_enumeration(ifnames, nif);
 
 	if (nip == nif) {
 		*ipaddrsp = ipaddrs;
@@ -1178,9 +1177,9 @@ lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 int
 lnet_parse_ip2nets(char **networksp, char *ip2nets)
 {
-	__u32     *ipaddrs = NULL;
-	int	nip = lnet_ipaddr_enumerate(&ipaddrs);
-	int	rc;
+	__u32 *ipaddrs = NULL;
+	int nip = lnet_ipaddr_enumerate(&ipaddrs);
+	int rc;
 
 	if (nip < 0) {
 		LCONSOLE_ERROR_MSG(0x117,
@@ -1211,82 +1210,3 @@ lnet_parse_ip2nets(char **networksp, char *ip2nets)
 
 	return 0;
 }
-
-int
-lnet_set_ip_niaddr(lnet_ni_t *ni)
-{
-	__u32  net = LNET_NIDNET(ni->ni_nid);
-	char **names;
-	int    n;
-	__u32  ip;
-	__u32  netmask;
-	int    up;
-	int    i;
-	int    rc;
-
-	/* Convenience for LNDs that use the IP address of a local interface as
-	 * the local address part of their NID */
-
-	if (ni->ni_interfaces[0] != NULL) {
-
-		CLASSERT(LNET_MAX_INTERFACES > 1);
-
-		if (ni->ni_interfaces[1] != NULL) {
-			CERROR("Net %s doesn't support multiple interfaces\n",
-			       libcfs_net2str(net));
-			return -EPERM;
-		}
-
-		rc = libcfs_ipif_query(ni->ni_interfaces[0],
-				       &up, &ip, &netmask);
-		if (rc != 0) {
-			CERROR("Net %s can't query interface %s: %d\n",
-			       libcfs_net2str(net), ni->ni_interfaces[0], rc);
-			return -EPERM;
-		}
-
-		if (!up) {
-			CERROR("Net %s can't use interface %s: it's down\n",
-			       libcfs_net2str(net), ni->ni_interfaces[0]);
-			return -ENETDOWN;
-		}
-
-		ni->ni_nid = LNET_MKNID(net, ip);
-		return 0;
-	}
-
-	n = libcfs_ipif_enumerate(&names);
-	if (n <= 0) {
-		CERROR("Net %s can't enumerate interfaces: %d\n",
-		       libcfs_net2str(net), n);
-		return 0;
-	}
-
-	for (i = 0; i < n; i++) {
-		if (!strcmp(names[i], "lo")) /* skip the loopback IF */
-			continue;
-
-		rc = libcfs_ipif_query(names[i], &up, &ip, &netmask);
-
-		if (rc != 0) {
-			CWARN("Net %s can't query interface %s: %d\n",
-			      libcfs_net2str(net), names[i], rc);
-			continue;
-		}
-
-		if (!up) {
-			CWARN("Net %s ignoring interface %s (down)\n",
-			      libcfs_net2str(net), names[i]);
-			continue;
-		}
-
-		libcfs_ipif_free_enumeration(names, n);
-		ni->ni_nid = LNET_MKNID(net, ip);
-		return 0;
-	}
-
-	CERROR("Net %s can't find any interfaces\n", libcfs_net2str(net));
-	libcfs_ipif_free_enumeration(names, n);
-	return -ENOENT;
-}
-EXPORT_SYMBOL(lnet_set_ip_niaddr);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-eq.c b/drivers/staging/lustre/lnet/lnet/lib-eq.c
index 5470148f5b64..f19ce9ae6a9a 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-eq.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-eq.c
@@ -70,7 +70,7 @@ int
 LNetEQAlloc(unsigned int count, lnet_eq_handler_t callback,
 	    lnet_handle_eq_t *handle)
 {
-	lnet_eq_t     *eq;
+	lnet_eq_t *eq;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -79,7 +79,7 @@ LNetEQAlloc(unsigned int count, lnet_eq_handler_t callback,
 	 * overflow, they don't skip entries, so the queue has the same
 	 * apparent capacity at all times */
 
-	count = cfs_power2_roundup(count);
+	count = roundup_pow_of_two(count);
 
 	if (callback != LNET_EQ_HANDLER_NONE && count != 0)
 		CWARN("EQ callback is guaranteed to get every event, do you still want to set eqcount %d for polling event which will have locking overhead? Please contact with developer to confirm\n", count);
@@ -151,13 +151,13 @@ EXPORT_SYMBOL(LNetEQAlloc);
 int
 LNetEQFree(lnet_handle_eq_t eqh)
 {
-	struct lnet_eq	*eq;
-	lnet_event_t	*events = NULL;
-	int		**refs = NULL;
-	int		*ref;
-	int		rc = 0;
-	int		size = 0;
-	int		i;
+	struct lnet_eq *eq;
+	lnet_event_t *events = NULL;
+	int **refs = NULL;
+	int *ref;
+	int rc = 0;
+	int size = 0;
+	int i;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -185,13 +185,13 @@ LNetEQFree(lnet_handle_eq_t eqh)
 	}
 
 	/* stash for free after lock dropped */
-	events	= eq->eq_events;
-	size	= eq->eq_size;
-	refs	= eq->eq_refs;
+	events = eq->eq_events;
+	size = eq->eq_size;
+	refs = eq->eq_refs;
 
 	lnet_res_lh_invalidate(&eq->eq_lh);
 	list_del(&eq->eq_list);
-	lnet_eq_free_locked(eq);
+	lnet_eq_free(eq);
  out:
 	lnet_eq_wait_unlock();
 	lnet_res_unlock(LNET_LOCK_EX);
@@ -237,9 +237,9 @@ lnet_eq_enqueue_event(lnet_eq_t *eq, lnet_event_t *ev)
 static int
 lnet_eq_dequeue_event(lnet_eq_t *eq, lnet_event_t *ev)
 {
-	int		new_index = eq->eq_deq_seq & (eq->eq_size - 1);
-	lnet_event_t	*new_event = &eq->eq_events[new_index];
-	int		rc;
+	int new_index = eq->eq_deq_seq & (eq->eq_size - 1);
+	lnet_event_t *new_event = &eq->eq_events[new_index];
+	int rc;
 
 	/* must called with lnet_eq_wait_lock hold */
 	if (LNET_SEQ_GT(eq->eq_deq_seq, new_event->sequence))
@@ -323,10 +323,10 @@ static int
 lnet_eq_wait_locked(int *timeout_ms)
 __must_hold(&the_lnet.ln_eq_wait_lock)
 {
-	int		tms = *timeout_ms;
-	int		wait;
-	wait_queue_t  wl;
-	unsigned long      now;
+	int tms = *timeout_ms;
+	int wait;
+	wait_queue_t wl;
+	unsigned long now;
 
 	if (tms == 0)
 		return -1; /* don't want to wait and no new event */
@@ -392,9 +392,9 @@ int
 LNetEQPoll(lnet_handle_eq_t *eventqs, int neq, int timeout_ms,
 	   lnet_event_t *event, int *which)
 {
-	int	wait = 1;
-	int	rc;
-	int	i;
+	int wait = 1;
+	int rc;
+	int i;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-md.c b/drivers/staging/lustre/lnet/lnet/lib-md.c
index 89d660fefd48..758f5bedef7e 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-md.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-md.c
@@ -74,7 +74,7 @@ lnet_md_unlink(lnet_libmd_t *md)
 	CDEBUG(D_NET, "Unlinking md %p\n", md);
 
 	if (md->md_eq != NULL) {
-		int	cpt = lnet_cpt_of_cookie(md->md_lh.lh_cookie);
+		int cpt = lnet_cpt_of_cookie(md->md_lh.lh_cookie);
 
 		LASSERT(*md->md_eq->eq_refs[cpt] > 0);
 		(*md->md_eq->eq_refs[cpt])--;
@@ -82,15 +82,15 @@ lnet_md_unlink(lnet_libmd_t *md)
 
 	LASSERT(!list_empty(&md->md_list));
 	list_del_init(&md->md_list);
-	lnet_md_free_locked(md);
+	lnet_md_free(md);
 }
 
 static int
 lnet_md_build(lnet_libmd_t *lmd, lnet_md_t *umd, int unlink)
 {
-	int	  i;
+	int i;
 	unsigned int niov;
-	int	  total_length = 0;
+	int total_length = 0;
 
 	lmd->md_me = NULL;
 	lmd->md_start = umd->start;
@@ -268,10 +268,10 @@ LNetMDAttach(lnet_handle_me_t meh, lnet_md_t umd,
 {
 	LIST_HEAD(matches);
 	LIST_HEAD(drops);
-	struct lnet_me		*me;
-	struct lnet_libmd	*md;
-	int			cpt;
-	int			rc;
+	struct lnet_me *me;
+	struct lnet_libmd *md;
+	int cpt;
+	int rc;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -320,7 +320,7 @@ LNetMDAttach(lnet_handle_me_t meh, lnet_md_t umd,
 	return 0;
 
  failed:
-	lnet_md_free_locked(md);
+	lnet_md_free(md);
 
 	lnet_res_unlock(cpt);
 	return rc;
@@ -346,9 +346,9 @@ EXPORT_SYMBOL(LNetMDAttach);
 int
 LNetMDBind(lnet_md_t umd, lnet_unlink_t unlink, lnet_handle_md_t *handle)
 {
-	lnet_libmd_t	*md;
-	int		cpt;
-	int		rc;
+	lnet_libmd_t *md;
+	int cpt;
+	int rc;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -381,7 +381,7 @@ LNetMDBind(lnet_md_t umd, lnet_unlink_t unlink, lnet_handle_md_t *handle)
 	return 0;
 
  failed:
-	lnet_md_free_locked(md);
+	lnet_md_free(md);
 
 	lnet_res_unlock(cpt);
 	return rc;
@@ -421,9 +421,9 @@ EXPORT_SYMBOL(LNetMDBind);
 int
 LNetMDUnlink(lnet_handle_md_t mdh)
 {
-	lnet_event_t	ev;
-	lnet_libmd_t	*md;
-	int		cpt;
+	lnet_event_t ev;
+	lnet_libmd_t *md;
+	int cpt;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-me.c b/drivers/staging/lustre/lnet/lnet/lib-me.c
index a3f929244711..42fc99ef9f80 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-me.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-me.c
@@ -80,8 +80,8 @@ LNetMEAttach(unsigned int portal,
 	     lnet_handle_me_t *handle)
 {
 	struct lnet_match_table *mtable;
-	struct lnet_me		*me;
-	struct list_head		*head;
+	struct lnet_me *me;
+	struct list_head *head;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -151,10 +151,10 @@ LNetMEInsert(lnet_handle_me_t current_meh,
 	     lnet_unlink_t unlink, lnet_ins_pos_t pos,
 	     lnet_handle_me_t *handle)
 {
-	struct lnet_me		*current_me;
-	struct lnet_me		*new_me;
-	struct lnet_portal	*ptl;
-	int			cpt;
+	struct lnet_me *current_me;
+	struct lnet_me *new_me;
+	struct lnet_portal *ptl;
+	int cpt;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -172,7 +172,7 @@ LNetMEInsert(lnet_handle_me_t current_meh,
 
 	current_me = lnet_handle2me(&current_meh);
 	if (current_me == NULL) {
-		lnet_me_free_locked(new_me);
+		lnet_me_free(new_me);
 
 		lnet_res_unlock(cpt);
 		return -ENOENT;
@@ -183,7 +183,7 @@ LNetMEInsert(lnet_handle_me_t current_meh,
 	ptl = the_lnet.ln_portals[current_me->me_portal];
 	if (lnet_ptl_is_unique(ptl)) {
 		/* nosense to insertion on unique portal */
-		lnet_me_free_locked(new_me);
+		lnet_me_free(new_me);
 		lnet_res_unlock(cpt);
 		return -EPERM;
 	}
@@ -228,10 +228,10 @@ EXPORT_SYMBOL(LNetMEInsert);
 int
 LNetMEUnlink(lnet_handle_me_t meh)
 {
-	lnet_me_t	*me;
-	lnet_libmd_t	*md;
-	lnet_event_t	ev;
-	int		cpt;
+	lnet_me_t *me;
+	lnet_libmd_t *md;
+	lnet_event_t ev;
+	int cpt;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -276,7 +276,7 @@ lnet_me_unlink(lnet_me_t *me)
 	}
 
 	lnet_res_lh_invalidate(&me->me_lh);
-	lnet_me_free_locked(me);
+	lnet_me_free(me);
 }
 
 #if 0
diff --git a/drivers/staging/lustre/lnet/lnet/lib-move.c b/drivers/staging/lustre/lnet/lnet/lib-move.c
index c2fb70e5fc4e..433faae9a2ff 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-move.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-move.c
@@ -49,10 +49,10 @@ MODULE_PARM_DESC(local_nid_dist_zero, "Reserved");
 int
 lnet_fail_nid(lnet_nid_t nid, unsigned int threshold)
 {
-	lnet_test_peer_t  *tp;
-	struct list_head	*el;
-	struct list_head	*next;
-	struct list_head	 cull;
+	lnet_test_peer_t *tp;
+	struct list_head *el;
+	struct list_head *next;
+	struct list_head cull;
 
 	LASSERT(the_lnet.ln_init);
 
@@ -103,10 +103,10 @@ static int
 fail_peer(lnet_nid_t nid, int outgoing)
 {
 	lnet_test_peer_t *tp;
-	struct list_head       *el;
-	struct list_head       *next;
-	struct list_head	cull;
-	int	       fail = 0;
+	struct list_head *el;
+	struct list_head *next;
+	struct list_head cull;
+	int fail = 0;
 
 	INIT_LIST_HEAD(&cull);
 
@@ -175,7 +175,7 @@ lnet_copy_iov2iov(unsigned int ndiov, struct kvec *diov, unsigned int doffset,
 		   unsigned int nob)
 {
 	/* NB diov, siov are READ-ONLY */
-	unsigned int  this_nob;
+	unsigned int this_nob;
 
 	if (nob == 0)
 		return;
@@ -236,8 +236,8 @@ lnet_extract_iov(int dst_niov, struct kvec *dst,
 	/* Initialise 'dst' to the subset of 'src' starting at 'offset',
 	 * for exactly 'len' bytes, and return the number of entries.
 	 * NB not destructive to 'src' */
-	unsigned int    frag_len;
-	unsigned int    niov;
+	unsigned int frag_len;
+	unsigned int niov;
 
 	if (len == 0)			   /* no data => */
 		return 0;		     /* no frags */
@@ -279,7 +279,7 @@ EXPORT_SYMBOL(lnet_extract_iov);
 unsigned int
 lnet_kiov_nob(unsigned int niov, lnet_kiov_t *kiov)
 {
-	unsigned int  nob = 0;
+	unsigned int nob = 0;
 
 	while (niov-- > 0)
 		nob += (kiov++)->kiov_len;
@@ -294,9 +294,9 @@ lnet_copy_kiov2kiov(unsigned int ndiov, lnet_kiov_t *diov, unsigned int doffset,
 		    unsigned int nob)
 {
 	/* NB diov, siov are READ-ONLY */
-	unsigned int    this_nob;
-	char	   *daddr = NULL;
-	char	   *saddr = NULL;
+	unsigned int this_nob;
+	char *daddr = NULL;
+	char *saddr = NULL;
 
 	if (nob == 0)
 		return;
@@ -376,8 +376,8 @@ lnet_copy_kiov2iov(unsigned int niov, struct kvec *iov, unsigned int iovoffset,
 		   unsigned int kiovoffset, unsigned int nob)
 {
 	/* NB iov, kiov are READ-ONLY */
-	unsigned int    this_nob;
-	char	   *addr = NULL;
+	unsigned int this_nob;
+	char *addr = NULL;
 
 	if (nob == 0)
 		return;
@@ -447,8 +447,8 @@ lnet_copy_iov2kiov(unsigned int nkiov, lnet_kiov_t *kiov,
 		   unsigned int nob)
 {
 	/* NB kiov, iov are READ-ONLY */
-	unsigned int    this_nob;
-	char	   *addr = NULL;
+	unsigned int this_nob;
+	char *addr = NULL;
 
 	if (nob == 0)
 		return;
@@ -518,8 +518,8 @@ lnet_extract_kiov(int dst_niov, lnet_kiov_t *dst,
 	/* Initialise 'dst' to the subset of 'src' starting at 'offset',
 	 * for exactly 'len' bytes, and return the number of entries.
 	 * NB not destructive to 'src' */
-	unsigned int    frag_len;
-	unsigned int    niov;
+	unsigned int frag_len;
+	unsigned int niov;
 
 	if (len == 0)			   /* no data => */
 		return 0;		     /* no frags */
@@ -565,10 +565,10 @@ 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)
 {
-	unsigned int  niov = 0;
+	unsigned int niov = 0;
 	struct kvec *iov = NULL;
-	lnet_kiov_t  *kiov = NULL;
-	int	   rc;
+	lnet_kiov_t *kiov = NULL;
+	int rc;
 
 	LASSERT(!in_interrupt());
 	LASSERT(mlen == 0 || msg != NULL);
@@ -642,8 +642,8 @@ lnet_prep_send(lnet_msg_t *msg, int type, lnet_process_id_t target,
 static void
 lnet_ni_send(lnet_ni_t *ni, lnet_msg_t *msg)
 {
-	void   *priv = msg->msg_private;
-	int     rc;
+	void *priv = msg->msg_private;
+	int rc;
 
 	LASSERT(!in_interrupt());
 	LASSERT(LNET_NETTYP(LNET_NIDNET(ni->ni_nid)) == LOLND ||
@@ -657,7 +657,7 @@ lnet_ni_send(lnet_ni_t *ni, lnet_msg_t *msg)
 static int
 lnet_ni_eager_recv(lnet_ni_t *ni, lnet_msg_t *msg)
 {
-	int	rc;
+	int rc;
 
 	LASSERT(!msg->msg_sending);
 	LASSERT(msg->msg_receiving);
@@ -700,7 +700,7 @@ lnet_ni_query_locked(lnet_ni_t *ni, lnet_peer_t *lp)
 static inline int
 lnet_peer_is_alive(lnet_peer_t *lp, unsigned long now)
 {
-	int	alive;
+	int alive;
 	unsigned long deadline;
 
 	LASSERT(lnet_peer_aliveness_enabled(lp));
@@ -785,10 +785,10 @@ lnet_peer_alive_locked(lnet_peer_t *lp)
 static int
 lnet_post_send_locked(lnet_msg_t *msg, int do_send)
 {
-	lnet_peer_t		*lp = msg->msg_txpeer;
-	lnet_ni_t		*ni = lp->lp_ni;
-	int			cpt = msg->msg_tx_cpt;
-	struct lnet_tx_queue	*tq = ni->ni_tx_queues[cpt];
+	lnet_peer_t *lp = msg->msg_txpeer;
+	lnet_ni_t *ni = lp->lp_ni;
+	int cpt = msg->msg_tx_cpt;
+	struct lnet_tx_queue *tq = ni->ni_tx_queues[cpt];
 
 	/* non-lnet_send() callers have checked before */
 	LASSERT(!do_send || msg->msg_tx_delayed);
@@ -871,8 +871,8 @@ lnet_post_send_locked(lnet_msg_t *msg, int do_send)
 static lnet_rtrbufpool_t *
 lnet_msg2bufpool(lnet_msg_t *msg)
 {
-	lnet_rtrbufpool_t	*rbp;
-	int			cpt;
+	lnet_rtrbufpool_t *rbp;
+	int cpt;
 
 	LASSERT(msg->msg_rx_committed);
 
@@ -894,9 +894,9 @@ 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
 	 * sets do_recv FALSE and I don't do the unlock/send/lock bit.  I
 	 * return EAGAIN if msg blocked and 0 if received or OK to receive */
-	lnet_peer_t	 *lp = msg->msg_rxpeer;
-	lnet_rtrbufpool_t   *rbp;
-	lnet_rtrbuf_t       *rb;
+	lnet_peer_t *lp = msg->msg_rxpeer;
+	lnet_rtrbufpool_t *rbp;
+	lnet_rtrbuf_t *rb;
 
 	LASSERT(msg->msg_iov == NULL);
 	LASSERT(msg->msg_kiov == NULL);
@@ -967,11 +967,11 @@ lnet_post_routed_recv_locked(lnet_msg_t *msg, int do_recv)
 void
 lnet_return_tx_credits_locked(lnet_msg_t *msg)
 {
-	lnet_peer_t	*txpeer = msg->msg_txpeer;
-	lnet_msg_t	*msg2;
+	lnet_peer_t *txpeer = msg->msg_txpeer;
+	lnet_msg_t *msg2;
 
 	if (msg->msg_txcredit) {
-		struct lnet_ni	     *ni = txpeer->lp_ni;
+		struct lnet_ni *ni = txpeer->lp_ni;
 		struct lnet_tx_queue *tq = ni->ni_tx_queues[msg->msg_tx_cpt];
 
 		/* give back NI txcredits */
@@ -1025,12 +1025,12 @@ lnet_return_tx_credits_locked(lnet_msg_t *msg)
 void
 lnet_return_rx_credits_locked(lnet_msg_t *msg)
 {
-	lnet_peer_t	*rxpeer = msg->msg_rxpeer;
-	lnet_msg_t	*msg2;
+	lnet_peer_t *rxpeer = msg->msg_rxpeer;
+	lnet_msg_t *msg2;
 
 	if (msg->msg_rtrcredit) {
 		/* give back global router credits */
-		lnet_rtrbuf_t     *rb;
+		lnet_rtrbuf_t *rb;
 		lnet_rtrbufpool_t *rbp;
 
 		/* NB If a msg ever blocks for a buffer in rbp_msgs, it stays
@@ -1122,13 +1122,13 @@ lnet_compare_routes(lnet_route_t *r1, lnet_route_t *r2)
 static lnet_peer_t *
 lnet_find_route_locked(lnet_ni_t *ni, lnet_nid_t target, lnet_nid_t rtr_nid)
 {
-	lnet_remotenet_t	*rnet;
-	lnet_route_t		*rtr;
-	lnet_route_t		*rtr_best;
-	lnet_route_t		*rtr_last;
-	struct lnet_peer	*lp_best;
-	struct lnet_peer	*lp;
-	int			rc;
+	lnet_remotenet_t *rnet;
+	lnet_route_t *rtr;
+	lnet_route_t *rtr_best;
+	lnet_route_t *rtr_last;
+	struct lnet_peer *lp_best;
+	struct lnet_peer *lp;
+	int rc;
 
 	/* If @rtr_nid is not LNET_NID_ANY, return the gateway with
 	 * rtr_nid nid, otherwise find the best gateway I can use */
@@ -1182,13 +1182,13 @@ lnet_find_route_locked(lnet_ni_t *ni, lnet_nid_t target, lnet_nid_t rtr_nid)
 int
 lnet_send(lnet_nid_t src_nid, lnet_msg_t *msg, lnet_nid_t rtr_nid)
 {
-	lnet_nid_t		dst_nid = msg->msg_target.nid;
-	struct lnet_ni		*src_ni;
-	struct lnet_ni		*local_ni;
-	struct lnet_peer	*lp;
-	int			cpt;
-	int			cpt2;
-	int			rc;
+	lnet_nid_t dst_nid = msg->msg_target.nid;
+	struct lnet_ni *src_ni;
+	struct lnet_ni *local_ni;
+	struct lnet_peer *lp;
+	int cpt;
+	int cpt2;
+	int rc;
 
 	/* NB: rtr_nid is set to LNET_NID_ANY for all current use-cases,
 	 * but we might want to use pre-determined router for ACK/REPLY
@@ -1364,7 +1364,7 @@ lnet_drop_message(lnet_ni_t *ni, int cpt, void *private, unsigned int nob)
 static void
 lnet_recv_put(lnet_ni_t *ni, lnet_msg_t *msg)
 {
-	lnet_hdr_t	*hdr = &msg->msg_hdr;
+	lnet_hdr_t *hdr = &msg->msg_hdr;
 
 	if (msg->msg_wanted != 0)
 		lnet_setpayloadbuffer(msg);
@@ -1383,9 +1383,9 @@ lnet_recv_put(lnet_ni_t *ni, lnet_msg_t *msg)
 static int
 lnet_parse_put(lnet_ni_t *ni, lnet_msg_t *msg)
 {
-	lnet_hdr_t		*hdr = &msg->msg_hdr;
-	struct lnet_match_info	info;
-	int			rc;
+	lnet_hdr_t *hdr = &msg->msg_hdr;
+	struct lnet_match_info info;
+	int rc;
 
 	/* Convert put fields to host byte order */
 	hdr->msg.put.match_bits	= le64_to_cpu(hdr->msg.put.match_bits);
@@ -1433,24 +1433,24 @@ lnet_parse_put(lnet_ni_t *ni, lnet_msg_t *msg)
 static int
 lnet_parse_get(lnet_ni_t *ni, lnet_msg_t *msg, int rdma_get)
 {
-	struct lnet_match_info	info;
-	lnet_hdr_t		*hdr = &msg->msg_hdr;
-	lnet_handle_wire_t	reply_wmd;
-	int			rc;
+	struct lnet_match_info info;
+	lnet_hdr_t *hdr = &msg->msg_hdr;
+	lnet_handle_wire_t reply_wmd;
+	int rc;
 
 	/* Convert get fields to host byte order */
-	hdr->msg.get.match_bits	  = le64_to_cpu(hdr->msg.get.match_bits);
-	hdr->msg.get.ptl_index	  = le32_to_cpu(hdr->msg.get.ptl_index);
-	hdr->msg.get.sink_length  = le32_to_cpu(hdr->msg.get.sink_length);
-	hdr->msg.get.src_offset	  = le32_to_cpu(hdr->msg.get.src_offset);
-
-	info.mi_id.nid	= hdr->src_nid;
-	info.mi_id.pid	= hdr->src_pid;
-	info.mi_opc	= LNET_MD_OP_GET;
-	info.mi_portal	= hdr->msg.get.ptl_index;
-	info.mi_rlength	= hdr->msg.get.sink_length;
-	info.mi_roffset	= hdr->msg.get.src_offset;
-	info.mi_mbits	= hdr->msg.get.match_bits;
+	hdr->msg.get.match_bits  = le64_to_cpu(hdr->msg.get.match_bits);
+	hdr->msg.get.ptl_index   = le32_to_cpu(hdr->msg.get.ptl_index);
+	hdr->msg.get.sink_length = le32_to_cpu(hdr->msg.get.sink_length);
+	hdr->msg.get.src_offset  = le32_to_cpu(hdr->msg.get.src_offset);
+
+	info.mi_id.nid  = hdr->src_nid;
+	info.mi_id.pid  = hdr->src_pid;
+	info.mi_opc     = LNET_MD_OP_GET;
+	info.mi_portal  = hdr->msg.get.ptl_index;
+	info.mi_rlength = hdr->msg.get.sink_length;
+	info.mi_roffset = hdr->msg.get.src_offset;
+	info.mi_mbits   = hdr->msg.get.match_bits;
 
 	rc = lnet_ptl_match_md(&info, msg);
 	if (rc == LNET_MATCHMD_DROP) {
@@ -1497,13 +1497,13 @@ lnet_parse_get(lnet_ni_t *ni, lnet_msg_t *msg, int rdma_get)
 static int
 lnet_parse_reply(lnet_ni_t *ni, lnet_msg_t *msg)
 {
-	void	     *private = msg->msg_private;
-	lnet_hdr_t       *hdr = &msg->msg_hdr;
+	void *private = msg->msg_private;
+	lnet_hdr_t *hdr = &msg->msg_hdr;
 	lnet_process_id_t src = {0};
-	lnet_libmd_t     *md;
-	int	       rlength;
-	int	       mlength;
-	int			cpt;
+	lnet_libmd_t *md;
+	int rlength;
+	int mlength;
+	int cpt;
 
 	cpt = lnet_cpt_of_cookie(hdr->msg.reply.dst_wmd.wh_object_cookie);
 	lnet_res_lock(cpt);
@@ -1562,10 +1562,10 @@ lnet_parse_reply(lnet_ni_t *ni, lnet_msg_t *msg)
 static int
 lnet_parse_ack(lnet_ni_t *ni, lnet_msg_t *msg)
 {
-	lnet_hdr_t       *hdr = &msg->msg_hdr;
+	lnet_hdr_t *hdr = &msg->msg_hdr;
 	lnet_process_id_t src = {0};
-	lnet_libmd_t     *md;
-	int			cpt;
+	lnet_libmd_t *md;
+	int cpt;
 
 	src.nid = hdr->src_nid;
 	src.pid = hdr->src_pid;
@@ -1612,7 +1612,7 @@ lnet_parse_ack(lnet_ni_t *ni, lnet_msg_t *msg)
 static int
 lnet_parse_forward_locked(lnet_ni_t *ni, lnet_msg_t *msg)
 {
-	int	rc = 0;
+	int rc = 0;
 
 	if (msg->msg_rxpeer->lp_rtrcredits <= 0 ||
 	    lnet_msg2bufpool(msg)->rbp_credits <= 0) {
@@ -1713,15 +1713,15 @@ int
 lnet_parse(lnet_ni_t *ni, lnet_hdr_t *hdr, lnet_nid_t from_nid,
 	   void *private, int rdma_req)
 {
-	int		rc = 0;
-	int		cpt;
-	int		for_me;
-	struct lnet_msg	*msg;
-	lnet_pid_t     dest_pid;
-	lnet_nid_t     dest_nid;
-	lnet_nid_t     src_nid;
-	__u32	  payload_length;
-	__u32	  type;
+	int rc = 0;
+	int cpt;
+	int for_me;
+	struct lnet_msg *msg;
+	lnet_pid_t dest_pid;
+	lnet_nid_t dest_nid;
+	lnet_nid_t src_nid;
+	__u32 payload_length;
+	__u32 type;
 
 	LASSERT(!in_interrupt());
 
@@ -1945,8 +1945,8 @@ void
 lnet_drop_delayed_msg_list(struct list_head *head, char *reason)
 {
 	while (!list_empty(head)) {
-		lnet_process_id_t	id = {0};
-		lnet_msg_t		*msg;
+		lnet_process_id_t id = {0};
+		lnet_msg_t *msg;
 
 		msg = list_entry(head->next, lnet_msg_t, msg_list);
 		list_del(&msg->msg_list);
@@ -1986,8 +1986,8 @@ void
 lnet_recv_delayed_msg_list(struct list_head *head)
 {
 	while (!list_empty(head)) {
-		lnet_msg_t	  *msg;
-		lnet_process_id_t  id;
+		lnet_msg_t *msg;
+		lnet_process_id_t id;
 
 		msg = list_entry(head->next, lnet_msg_t, msg_list);
 		list_del(&msg->msg_list);
@@ -2063,10 +2063,10 @@ LNetPut(lnet_nid_t self, lnet_handle_md_t mdh, lnet_ack_req_t ack,
 	__u64 match_bits, unsigned int offset,
 	__u64 hdr_data)
 {
-	struct lnet_msg		*msg;
-	struct lnet_libmd	*md;
-	int			cpt;
-	int			rc;
+	struct lnet_msg *msg;
+	struct lnet_libmd *md;
+	int cpt;
+	int rc;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -2153,10 +2153,10 @@ lnet_create_reply_msg(lnet_ni_t *ni, lnet_msg_t *getmsg)
 	 * CAVEAT EMPTOR: 'getmsg' is the original GET, which is freed when
 	 * lnet_finalize() is called on it, so the LND must call this first */
 
-	struct lnet_msg		*msg = lnet_msg_alloc();
-	struct lnet_libmd	*getmd = getmsg->msg_md;
-	lnet_process_id_t	peer_id = getmsg->msg_target;
-	int			cpt;
+	struct lnet_msg *msg = lnet_msg_alloc();
+	struct lnet_libmd *getmd = getmsg->msg_md;
+	lnet_process_id_t peer_id = getmsg->msg_target;
+	int cpt;
 
 	LASSERT(!getmsg->msg_target_is_router);
 	LASSERT(!getmsg->msg_routing);
@@ -2263,10 +2263,10 @@ LNetGet(lnet_nid_t self, lnet_handle_md_t mdh,
 	lnet_process_id_t target, unsigned int portal,
 	__u64 match_bits, unsigned int offset)
 {
-	struct lnet_msg		*msg;
-	struct lnet_libmd	*md;
-	int			cpt;
-	int			rc;
+	struct lnet_msg *msg;
+	struct lnet_libmd *md;
+	int cpt;
+	int rc;
 
 	LASSERT(the_lnet.ln_init);
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -2353,14 +2353,14 @@ EXPORT_SYMBOL(LNetGet);
 int
 LNetDist(lnet_nid_t dstnid, lnet_nid_t *srcnidp, __u32 *orderp)
 {
-	struct list_head		*e;
-	struct lnet_ni		*ni;
-	lnet_remotenet_t	*rnet;
-	__u32			dstnet = LNET_NIDNET(dstnid);
-	int			hops;
-	int			cpt;
-	__u32			order = 2;
-	struct list_head		*rn_list;
+	struct list_head *e;
+	struct lnet_ni *ni;
+	lnet_remotenet_t *rnet;
+	__u32 dstnet = LNET_NIDNET(dstnid);
+	int hops;
+	int cpt;
+	__u32 order = 2;
+	struct list_head *rn_list;
 
 	/* if !local_nid_dist_zero, I don't return a distance of 0 ever
 	 * (when lustre sees a distance of 0, it substitutes 0@lo), so I
@@ -2434,27 +2434,3 @@ LNetDist(lnet_nid_t dstnid, lnet_nid_t *srcnidp, __u32 *orderp)
 	return -EHOSTUNREACH;
 }
 EXPORT_SYMBOL(LNetDist);
-
-/**
- * Set the number of asynchronous messages expected from a target process.
- *
- * This function is only meaningful for userspace callers. It's a no-op when
- * called from kernel.
- *
- * Asynchronous messages are those that can come from a target when the
- * userspace process is not waiting for IO to complete; e.g., AST callbacks
- * from Lustre servers. Specifying the expected number of such messages
- * allows them to be eagerly received when user process is not running in
- * LNet; otherwise network errors may occur.
- *
- * \param id Process ID of the target process.
- * \param nasync Number of asynchronous messages expected from the target.
- *
- * \return 0 on success, and an error code otherwise.
- */
-int
-LNetSetAsync(lnet_process_id_t id, int nasync)
-{
-	return 0;
-}
-EXPORT_SYMBOL(LNetSetAsync);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-msg.c b/drivers/staging/lustre/lnet/lnet/lib-msg.c
index a46ccbf6608f..43977e8dffbb 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-msg.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-msg.c
@@ -60,8 +60,8 @@ lnet_build_unlink_event(lnet_libmd_t *md, lnet_event_t *ev)
 void
 lnet_build_msg_event(lnet_msg_t *msg, lnet_event_kind_t ev_type)
 {
-	lnet_hdr_t	*hdr = &msg->msg_hdr;
-	lnet_event_t	*ev  = &msg->msg_ev;
+	lnet_hdr_t *hdr = &msg->msg_hdr;
+	lnet_event_t *ev  = &msg->msg_ev;
 
 	LASSERT(!msg->msg_routing);
 
@@ -73,7 +73,7 @@ lnet_build_msg_event(lnet_msg_t *msg, lnet_event_kind_t ev_type)
 		ev->target.pid    = le32_to_cpu(hdr->dest_pid);
 		ev->initiator.nid = LNET_NID_ANY;
 		ev->initiator.pid = the_lnet.ln_pid;
-		ev->sender	  = LNET_NID_ANY;
+		ev->sender        = LNET_NID_ANY;
 
 	} else {
 		/* event for passive message */
@@ -82,9 +82,9 @@ lnet_build_msg_event(lnet_msg_t *msg, lnet_event_kind_t ev_type)
 		ev->initiator.pid = hdr->src_pid;
 		ev->initiator.nid = hdr->src_nid;
 		ev->rlength       = hdr->payload_length;
-		ev->sender	  = msg->msg_from;
-		ev->mlength	  = msg->msg_wanted;
-		ev->offset	  = msg->msg_offset;
+		ev->sender        = msg->msg_from;
+		ev->mlength       = msg->msg_wanted;
+		ev->offset        = msg->msg_offset;
 	}
 
 	switch (ev_type) {
@@ -137,7 +137,7 @@ void
 lnet_msg_commit(lnet_msg_t *msg, int cpt)
 {
 	struct lnet_msg_container *container = the_lnet.ln_msg_containers[cpt];
-	lnet_counters_t		  *counters  = the_lnet.ln_counters[cpt];
+	lnet_counters_t *counters  = the_lnet.ln_counters[cpt];
 
 	/* routed message can be committed for both receiving and sending */
 	LASSERT(!msg->msg_tx_committed);
@@ -170,7 +170,7 @@ static void
 lnet_msg_decommit_tx(lnet_msg_t *msg, int status)
 {
 	lnet_counters_t	*counters;
-	lnet_event_t	*ev = &msg->msg_ev;
+	lnet_event_t *ev = &msg->msg_ev;
 
 	LASSERT(msg->msg_tx_committed);
 	if (status != 0)
@@ -219,8 +219,8 @@ lnet_msg_decommit_tx(lnet_msg_t *msg, int status)
 static void
 lnet_msg_decommit_rx(lnet_msg_t *msg, int status)
 {
-	lnet_counters_t	*counters;
-	lnet_event_t	*ev = &msg->msg_ev;
+	lnet_counters_t *counters;
+	lnet_event_t *ev = &msg->msg_ev;
 
 	LASSERT(!msg->msg_tx_committed); /* decommitted or never committed */
 	LASSERT(msg->msg_rx_committed);
@@ -273,7 +273,7 @@ lnet_msg_decommit_rx(lnet_msg_t *msg, int status)
 void
 lnet_msg_decommit(lnet_msg_t *msg, int cpt, int status)
 {
-	int	cpt2 = cpt;
+	int cpt2 = cpt;
 
 	LASSERT(msg->msg_tx_committed || msg->msg_rx_committed);
 	LASSERT(msg->msg_onactivelist);
@@ -335,8 +335,8 @@ lnet_msg_attach_md(lnet_msg_t *msg, lnet_libmd_t *md,
 void
 lnet_msg_detach_md(lnet_msg_t *msg, int status)
 {
-	lnet_libmd_t	*md = msg->msg_md;
-	int		unlink;
+	lnet_libmd_t *md = msg->msg_md;
+	int unlink;
 
 	/* Now it's safe to drop my caller's ref */
 	md->md_refcount--;
@@ -359,8 +359,8 @@ static int
 lnet_complete_msg_locked(lnet_msg_t *msg, int cpt)
 {
 	lnet_handle_wire_t ack_wmd;
-	int		rc;
-	int		status = msg->msg_ev.status;
+	int rc;
+	int status = msg->msg_ev.status;
 
 	LASSERT(msg->msg_onactivelist);
 
@@ -427,18 +427,18 @@ lnet_complete_msg_locked(lnet_msg_t *msg, int cpt)
 	}
 
 	lnet_msg_decommit(msg, cpt, status);
-	lnet_msg_free_locked(msg);
+	lnet_msg_free(msg);
 	return 0;
 }
 
 void
 lnet_finalize(lnet_ni_t *ni, lnet_msg_t *msg, int status)
 {
-	struct lnet_msg_container	*container;
-	int				my_slot;
-	int				cpt;
-	int				rc;
-	int				i;
+	struct lnet_msg_container *container;
+	int my_slot;
+	int cpt;
+	int rc;
+	int i;
 
 	LASSERT(!in_interrupt());
 
@@ -534,7 +534,7 @@ EXPORT_SYMBOL(lnet_finalize);
 void
 lnet_msg_container_cleanup(struct lnet_msg_container *container)
 {
-	int     count = 0;
+	int count = 0;
 
 	if (container->msc_init == 0)
 		return;
@@ -568,7 +568,7 @@ lnet_msg_container_cleanup(struct lnet_msg_container *container)
 int
 lnet_msg_container_setup(struct lnet_msg_container *container, int cpt)
 {
-	int	rc;
+	int rc;
 
 	container->msc_init = 1;
 
@@ -608,7 +608,7 @@ void
 lnet_msg_containers_destroy(void)
 {
 	struct lnet_msg_container *container;
-	int     i;
+	int i;
 
 	if (the_lnet.ln_msg_containers == NULL)
 		return;
@@ -624,8 +624,8 @@ int
 lnet_msg_containers_create(void)
 {
 	struct lnet_msg_container *container;
-	int	rc;
-	int	i;
+	int rc;
+	int i;
 
 	the_lnet.ln_msg_containers = cfs_percpt_alloc(lnet_cpt_table(),
 						      sizeof(*container));
diff --git a/drivers/staging/lustre/lnet/lnet/lib-ptl.c b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
index 3ba0da919b41..84707c5cb464 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-ptl.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
@@ -39,7 +39,7 @@
 #include "../../include/linux/lnet/lib-lnet.h"
 
 /* NB: add /proc interfaces in upcoming patches */
-int	portal_rotor	= LNET_PTL_ROTOR_HASH_RT;
+int portal_rotor = LNET_PTL_ROTOR_HASH_RT;
 module_param(portal_rotor, int, 0644);
 MODULE_PARM_DESC(portal_rotor, "redirect PUTs to different cpu-partitions");
 
@@ -47,8 +47,8 @@ static int
 lnet_ptl_match_type(unsigned int index, lnet_process_id_t match_id,
 		    __u64 mbits, __u64 ignore_bits)
 {
-	struct lnet_portal	*ptl = the_lnet.ln_portals[index];
-	int			unique;
+	struct lnet_portal *ptl = the_lnet.ln_portals[index];
+	int unique;
 
 	unique = ignore_bits == 0 &&
 		 match_id.nid != LNET_NID_ANY &&
@@ -89,7 +89,7 @@ static void
 lnet_ptl_enable_mt(struct lnet_portal *ptl, int cpt)
 {
 	struct lnet_match_table	*mtable = ptl->ptl_mtables[cpt];
-	int			i;
+	int i;
 
 	/* with hold of both lnet_res_lock(cpt) and lnet_ptl_lock */
 	LASSERT(lnet_ptl_is_wildcard(ptl));
@@ -114,7 +114,7 @@ static void
 lnet_ptl_disable_mt(struct lnet_portal *ptl, int cpt)
 {
 	struct lnet_match_table	*mtable = ptl->ptl_mtables[cpt];
-	int			i;
+	int i;
 
 	/* with hold of both lnet_res_lock(cpt) and lnet_ptl_lock */
 	LASSERT(lnet_ptl_is_wildcard(ptl));
@@ -141,9 +141,9 @@ lnet_try_match_md(lnet_libmd_t *md,
 {
 	/* ALWAYS called holding the lnet_res_lock, and can't lnet_res_unlock;
 	 * lnet_match_blocked_msg() relies on this to avoid races */
-	unsigned int	offset;
-	unsigned int	mlength;
-	lnet_me_t	*me = md->md_me;
+	unsigned int offset;
+	unsigned int mlength;
+	lnet_me_t *me = md->md_me;
 
 	/* MD exhausted */
 	if (lnet_md_exhausted(md))
@@ -227,7 +227,7 @@ struct lnet_match_table *
 lnet_mt_of_attach(unsigned int index, lnet_process_id_t id,
 		  __u64 mbits, __u64 ignore_bits, lnet_ins_pos_t pos)
 {
-	struct lnet_portal	*ptl;
+	struct lnet_portal *ptl;
 	struct lnet_match_table	*mtable;
 
 	/* NB: called w/o lock */
@@ -261,11 +261,11 @@ static struct lnet_match_table *
 lnet_mt_of_match(struct lnet_match_info *info, struct lnet_msg *msg)
 {
 	struct lnet_match_table	*mtable;
-	struct lnet_portal	*ptl;
-	unsigned int		nmaps;
-	unsigned int		rotor;
-	unsigned int		cpt;
-	bool			routed;
+	struct lnet_portal *ptl;
+	unsigned int nmaps;
+	unsigned int rotor;
+	unsigned int cpt;
+	bool routed;
 
 	/* NB: called w/o lock */
 	LASSERT(info->mi_portal < the_lnet.ln_nportals);
@@ -312,8 +312,8 @@ lnet_mt_of_match(struct lnet_match_info *info, struct lnet_msg *msg)
 static int
 lnet_mt_test_exhausted(struct lnet_match_table *mtable, int pos)
 {
-	__u64	*bmap;
-	int	i;
+	__u64 *bmap;
+	int i;
 
 	if (!lnet_ptl_is_wildcard(the_lnet.ln_portals[mtable->mt_portal]))
 		return 0;
@@ -337,7 +337,7 @@ lnet_mt_test_exhausted(struct lnet_match_table *mtable, int pos)
 static void
 lnet_mt_set_exhausted(struct lnet_match_table *mtable, int pos, int exhausted)
 {
-	__u64	*bmap;
+	__u64 *bmap;
 
 	LASSERT(lnet_ptl_is_wildcard(the_lnet.ln_portals[mtable->mt_portal]));
 	LASSERT(pos <= LNET_MT_HASH_IGNORE);
@@ -373,11 +373,11 @@ int
 lnet_mt_match_md(struct lnet_match_table *mtable,
 		 struct lnet_match_info *info, struct lnet_msg *msg)
 {
-	struct list_head		*head;
-	lnet_me_t		*me;
-	lnet_me_t		*tmp;
-	int			exhausted = 0;
-	int			rc;
+	struct list_head *head;
+	lnet_me_t *me;
+	lnet_me_t *tmp;
+	int exhausted = 0;
+	int rc;
 
 	/* any ME with ignore bits? */
 	if (!list_empty(&mtable->mt_mhash[LNET_MT_HASH_IGNORE]))
@@ -428,7 +428,7 @@ lnet_mt_match_md(struct lnet_match_table *mtable,
 static int
 lnet_ptl_match_early(struct lnet_portal *ptl, struct lnet_msg *msg)
 {
-	int	rc;
+	int rc;
 
 	/* message arrived before any buffer posting on this portal,
 	 * simply delay or drop this message */
@@ -461,9 +461,9 @@ static int
 lnet_ptl_match_delay(struct lnet_portal *ptl,
 		     struct lnet_match_info *info, struct lnet_msg *msg)
 {
-	int	first = ptl->ptl_mt_maps[0]; /* read w/o lock */
-	int	rc = 0;
-	int	i;
+	int first = ptl->ptl_mt_maps[0]; /* read w/o lock */
+	int rc = 0;
+	int i;
 
 	/* steal buffer from other CPTs, and delay it if nothing to steal,
 	 * this function is more expensive than a regular match, but we
@@ -472,7 +472,7 @@ lnet_ptl_match_delay(struct lnet_portal *ptl,
 
 	for (i = 0; i < LNET_CPT_NUMBER; i++) {
 		struct lnet_match_table *mtable;
-		int			cpt;
+		int cpt;
 
 		cpt = (first + i) % LNET_CPT_NUMBER;
 		mtable = ptl->ptl_mtables[cpt];
@@ -536,8 +536,8 @@ int
 lnet_ptl_match_md(struct lnet_match_info *info, struct lnet_msg *msg)
 {
 	struct lnet_match_table	*mtable;
-	struct lnet_portal	*ptl;
-	int			rc;
+	struct lnet_portal *ptl;
+	int rc;
 
 	CDEBUG(D_NET, "Request from %s of length %d into portal %d MB=%#llx\n",
 	       libcfs_id2str(info->mi_id), info->mi_rlength, info->mi_portal,
@@ -622,13 +622,13 @@ void
 lnet_ptl_attach_md(lnet_me_t *me, lnet_libmd_t *md,
 		   struct list_head *matches, struct list_head *drops)
 {
-	struct lnet_portal	*ptl = the_lnet.ln_portals[me->me_portal];
+	struct lnet_portal *ptl = the_lnet.ln_portals[me->me_portal];
 	struct lnet_match_table	*mtable;
-	struct list_head		*head;
-	lnet_msg_t		*tmp;
-	lnet_msg_t		*msg;
-	int			exhausted = 0;
-	int			cpt;
+	struct list_head *head;
+	lnet_msg_t *tmp;
+	lnet_msg_t *msg;
+	int exhausted = 0;
+	int cpt;
 
 	LASSERT(md->md_refcount == 0); /* a brand new MD */
 
@@ -647,20 +647,20 @@ lnet_ptl_attach_md(lnet_me_t *me, lnet_libmd_t *md,
 	head = &ptl->ptl_msg_stealing;
  again:
 	list_for_each_entry_safe(msg, tmp, head, msg_list) {
-		struct lnet_match_info	info;
-		lnet_hdr_t		*hdr;
-		int			rc;
+		struct lnet_match_info info;
+		lnet_hdr_t *hdr;
+		int rc;
 
 		LASSERT(msg->msg_rx_delayed || head == &ptl->ptl_msg_stealing);
 
-		hdr   = &msg->msg_hdr;
-		info.mi_id.nid	= hdr->src_nid;
-		info.mi_id.pid	= hdr->src_pid;
-		info.mi_opc	= LNET_MD_OP_PUT;
-		info.mi_portal	= hdr->msg.put.ptl_index;
-		info.mi_rlength	= hdr->payload_length;
-		info.mi_roffset	= hdr->msg.put.offset;
-		info.mi_mbits	= hdr->msg.put.match_bits;
+		hdr = &msg->msg_hdr;
+		info.mi_id.nid  = hdr->src_nid;
+		info.mi_id.pid  = hdr->src_pid;
+		info.mi_opc     = LNET_MD_OP_PUT;
+		info.mi_portal  = hdr->msg.put.ptl_index;
+		info.mi_rlength = hdr->payload_length;
+		info.mi_roffset = hdr->msg.put.offset;
+		info.mi_mbits   = hdr->msg.put.match_bits;
 
 		rc = lnet_try_match_md(md, &info, msg);
 
@@ -715,7 +715,7 @@ static void
 lnet_ptl_cleanup(struct lnet_portal *ptl)
 {
 	struct lnet_match_table	*mtable;
-	int			i;
+	int i;
 
 	if (ptl->ptl_mtables == NULL) /* uninitialized portal */
 		return;
@@ -723,9 +723,9 @@ lnet_ptl_cleanup(struct lnet_portal *ptl)
 	LASSERT(list_empty(&ptl->ptl_msg_delayed));
 	LASSERT(list_empty(&ptl->ptl_msg_stealing));
 	cfs_percpt_for_each(mtable, i, ptl->ptl_mtables) {
-		struct list_head	*mhash;
-		lnet_me_t	*me;
-		int		j;
+		struct list_head *mhash;
+		lnet_me_t *me;
+		int j;
 
 		if (mtable->mt_mhash == NULL) /* uninitialized match-table */
 			continue;
@@ -753,9 +753,9 @@ static int
 lnet_ptl_setup(struct lnet_portal *ptl, int index)
 {
 	struct lnet_match_table	*mtable;
-	struct list_head		*mhash;
-	int			i;
-	int			j;
+	struct list_head *mhash;
+	int i;
+	int j;
 
 	ptl->ptl_mtables = cfs_percpt_alloc(lnet_cpt_table(),
 					    sizeof(struct lnet_match_table));
@@ -798,7 +798,7 @@ lnet_ptl_setup(struct lnet_portal *ptl, int index)
 void
 lnet_portals_destroy(void)
 {
-	int	i;
+	int i;
 
 	if (the_lnet.ln_portals == NULL)
 		return;
@@ -813,8 +813,8 @@ lnet_portals_destroy(void)
 int
 lnet_portals_create(void)
 {
-	int	size;
-	int	i;
+	int size;
+	int i;
 
 	size = offsetof(struct lnet_portal, ptl_mt_maps[LNET_CPT_NUMBER]);
 
@@ -898,8 +898,8 @@ EXPORT_SYMBOL(LNetSetLazyPortal);
 int
 LNetClearLazyPortal(int portal)
 {
-	struct lnet_portal	*ptl;
-	LIST_HEAD		(zombies);
+	struct lnet_portal *ptl;
+	LIST_HEAD(zombies);
 
 	if (portal < 0 || portal >= the_lnet.ln_nportals)
 		return -EINVAL;
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c b/drivers/staging/lustre/lnet/lnet/lib-socket.c
index f2462e7f04bc..6f7ef4c737cd 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-socket.c
@@ -15,11 +15,7 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
@@ -27,33 +23,49 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
+ * Lustre is a trademark of Seagate, Inc.
  */
 #define DEBUG_SUBSYSTEM S_LNET
 
-#include "../../../include/linux/libcfs/libcfs.h"
-
 #include <linux/if.h>
 #include <linux/in.h>
+#include <linux/net.h>
 #include <linux/file.h>
+#include <linux/pagemap.h>
 /* For sys_open & sys_close */
 #include <linux/syscalls.h>
+#include <net/sock.h>
+
+#include "../../include/linux/libcfs/libcfs.h"
+#include "../../include/linux/lnet/lib-lnet.h"
+
+static int
+kernel_sock_unlocked_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	mm_segment_t oldfs = get_fs();
+	int err;
+
+	set_fs(KERNEL_DS);
+	err = filp->f_op->unlocked_ioctl(filp, cmd, arg);
+	set_fs(oldfs);
+
+	return err;
+}
 
 static int
-libcfs_sock_ioctl(int cmd, unsigned long arg)
+lnet_sock_ioctl(int cmd, unsigned long arg)
 {
-	mm_segment_t	oldmm = get_fs();
-	struct socket  *sock;
-	int		rc;
-	struct file    *sock_filp;
+	struct file *sock_filp;
+	struct socket *sock;
+	int rc;
 
-	rc = sock_create (PF_INET, SOCK_STREAM, 0, &sock);
+	rc = sock_create(PF_INET, SOCK_STREAM, 0, &sock);
 	if (rc != 0) {
-		CERROR ("Can't create socket: %d\n", rc);
+		CERROR("Can't create socket: %d\n", rc);
 		return rc;
 	}
 
@@ -64,10 +76,7 @@ libcfs_sock_ioctl(int cmd, unsigned long arg)
 		goto out;
 	}
 
-	set_fs(KERNEL_DS);
-	if (sock_filp->f_op->unlocked_ioctl)
-		rc = sock_filp->f_op->unlocked_ioctl(sock_filp, cmd, arg);
-	set_fs(oldmm);
+	rc = kernel_sock_unlocked_ioctl(sock_filp, cmd, arg);
 
 	fput(sock_filp);
 out:
@@ -75,12 +84,12 @@ out:
 }
 
 int
-libcfs_ipif_query (char *name, int *up, __u32 *ip, __u32 *mask)
+lnet_ipif_query(char *name, int *up, __u32 *ip, __u32 *mask)
 {
-	struct ifreq   ifr;
-	int	    nob;
-	int	    rc;
-	__u32	  val;
+	struct ifreq ifr;
+	int nob;
+	int rc;
+	__u32 val;
 
 	nob = strnlen(name, IFNAMSIZ);
 	if (nob == IFNAMSIZ) {
@@ -88,11 +97,10 @@ libcfs_ipif_query (char *name, int *up, __u32 *ip, __u32 *mask)
 		return -EINVAL;
 	}
 
-	CLASSERT (sizeof(ifr.ifr_name) >= IFNAMSIZ);
+	CLASSERT(sizeof(ifr.ifr_name) >= IFNAMSIZ);
 
 	strcpy(ifr.ifr_name, name);
-	rc = libcfs_sock_ioctl(SIOCGIFFLAGS, (unsigned long)&ifr);
-
+	rc = lnet_sock_ioctl(SIOCGIFFLAGS, (unsigned long)&ifr);
 	if (rc != 0) {
 		CERROR("Can't get flags for interface %s\n", name);
 		return rc;
@@ -104,13 +112,11 @@ libcfs_ipif_query (char *name, int *up, __u32 *ip, __u32 *mask)
 		*ip = *mask = 0;
 		return 0;
 	}
-
 	*up = 1;
 
 	strcpy(ifr.ifr_name, name);
 	ifr.ifr_addr.sa_family = AF_INET;
-	rc = libcfs_sock_ioctl(SIOCGIFADDR, (unsigned long)&ifr);
-
+	rc = lnet_sock_ioctl(SIOCGIFADDR, (unsigned long)&ifr);
 	if (rc != 0) {
 		CERROR("Can't get IP address for interface %s\n", name);
 		return rc;
@@ -121,8 +127,7 @@ libcfs_ipif_query (char *name, int *up, __u32 *ip, __u32 *mask)
 
 	strcpy(ifr.ifr_name, name);
 	ifr.ifr_addr.sa_family = AF_INET;
-	rc = libcfs_sock_ioctl(SIOCGIFNETMASK, (unsigned long)&ifr);
-
+	rc = lnet_sock_ioctl(SIOCGIFNETMASK, (unsigned long)&ifr);
 	if (rc != 0) {
 		CERROR("Can't get netmask for interface %s\n", name);
 		return rc;
@@ -133,23 +138,21 @@ libcfs_ipif_query (char *name, int *up, __u32 *ip, __u32 *mask)
 
 	return 0;
 }
-
-EXPORT_SYMBOL(libcfs_ipif_query);
+EXPORT_SYMBOL(lnet_ipif_query);
 
 int
-libcfs_ipif_enumerate (char ***namesp)
+lnet_ipif_enumerate(char ***namesp)
 {
 	/* Allocate and fill in 'names', returning # interfaces/error */
-	char	   **names;
-	int	     toobig;
-	int	     nalloc;
-	int	     nfound;
-	struct ifreq   *ifr;
-	struct ifconf   ifc;
-	int	     rc;
-	int	     nob;
-	int	     i;
-
+	char **names;
+	int toobig;
+	int nalloc;
+	int nfound;
+	struct ifreq *ifr;
+	struct ifconf ifc;
+	int rc;
+	int nob;
+	int i;
 
 	nalloc = 16;	/* first guess at max interfaces */
 	toobig = 0;
@@ -163,7 +166,8 @@ libcfs_ipif_enumerate (char ***namesp)
 
 		LIBCFS_ALLOC(ifr, nalloc * sizeof(*ifr));
 		if (ifr == NULL) {
-			CERROR ("ENOMEM enumerating up to %d interfaces\n", nalloc);
+			CERROR("ENOMEM enumerating up to %d interfaces\n",
+			       nalloc);
 			rc = -ENOMEM;
 			goto out0;
 		}
@@ -171,17 +175,16 @@ libcfs_ipif_enumerate (char ***namesp)
 		ifc.ifc_buf = (char *)ifr;
 		ifc.ifc_len = nalloc * sizeof(*ifr);
 
-		rc = libcfs_sock_ioctl(SIOCGIFCONF, (unsigned long)&ifc);
-
+		rc = lnet_sock_ioctl(SIOCGIFCONF, (unsigned long)&ifc);
 		if (rc < 0) {
-			CERROR ("Error %d enumerating interfaces\n", rc);
+			CERROR("Error %d enumerating interfaces\n", rc);
 			goto out1;
 		}
 
-		LASSERT (rc == 0);
+		LASSERT(rc == 0);
 
 		nfound = ifc.ifc_len/sizeof(*ifr);
-		LASSERT (nfound <= nalloc);
+		LASSERT(nfound <= nalloc);
 
 		if (nfound < nalloc || toobig)
 			break;
@@ -200,8 +203,7 @@ libcfs_ipif_enumerate (char ***namesp)
 	}
 
 	for (i = 0; i < nfound; i++) {
-
-		nob = strnlen (ifr[i].ifr_name, IFNAMSIZ);
+		nob = strnlen(ifr[i].ifr_name, IFNAMSIZ);
 		if (nob == IFNAMSIZ) {
 			/* no space for terminating NULL */
 			CERROR("interface name %.*s too long (%d max)\n",
@@ -223,41 +225,39 @@ libcfs_ipif_enumerate (char ***namesp)
 	*namesp = names;
 	rc = nfound;
 
- out2:
+out2:
 	if (rc < 0)
-		libcfs_ipif_free_enumeration(names, nfound);
- out1:
+		lnet_ipif_free_enumeration(names, nfound);
+out1:
 	LIBCFS_FREE(ifr, nalloc * sizeof(*ifr));
- out0:
+out0:
 	return rc;
 }
-
-EXPORT_SYMBOL(libcfs_ipif_enumerate);
+EXPORT_SYMBOL(lnet_ipif_enumerate);
 
 void
-libcfs_ipif_free_enumeration (char **names, int n)
+lnet_ipif_free_enumeration(char **names, int n)
 {
-	int      i;
+	int i;
 
-	LASSERT (n > 0);
+	LASSERT(n > 0);
 
 	for (i = 0; i < n && names[i] != NULL; i++)
 		LIBCFS_FREE(names[i], IFNAMSIZ);
 
 	LIBCFS_FREE(names, n * sizeof(*names));
 }
-
-EXPORT_SYMBOL(libcfs_ipif_free_enumeration);
+EXPORT_SYMBOL(lnet_ipif_free_enumeration);
 
 int
-libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
+lnet_sock_write(struct socket *sock, void *buffer, int nob, int timeout)
 {
-	int	    rc;
-	long	   ticks = timeout * HZ;
-	unsigned long  then;
+	int rc;
+	long ticks = timeout * HZ;
+	unsigned long then;
 	struct timeval tv;
 
-	LASSERT (nob > 0);
+	LASSERT(nob > 0);
 	/* Caller may pass a zero timeout if she thinks the socket buffer is
 	 * empty enough to take the whole message immediately */
 
@@ -277,7 +277,7 @@ libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 				.tv_usec = ((ticks % HZ) * 1000000) / HZ
 			};
 			rc = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO,
-					     (char *)&tv, sizeof(tv));
+					       (char *)&tv, sizeof(tv));
 			if (rc != 0) {
 				CERROR("Can't set socket send timeout %ld.%06d: %d\n",
 				       (long)tv.tv_sec, (int)tv.tv_usec, rc);
@@ -296,7 +296,7 @@ libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 			return rc;
 
 		if (rc == 0) {
-			CERROR ("Unexpected zero rc\n");
+			CERROR("Unexpected zero rc\n");
 			return -ECONNABORTED;
 		}
 
@@ -306,21 +306,20 @@ libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 		buffer = ((char *)buffer) + rc;
 		nob -= rc;
 	}
-
 	return 0;
 }
-EXPORT_SYMBOL(libcfs_sock_write);
+EXPORT_SYMBOL(lnet_sock_write);
 
 int
-libcfs_sock_read (struct socket *sock, void *buffer, int nob, int timeout)
+lnet_sock_read(struct socket *sock, void *buffer, int nob, int timeout)
 {
-	int	    rc;
-	long	   ticks = timeout * HZ;
-	unsigned long  then;
+	int rc;
+	long ticks = timeout * HZ;
+	unsigned long then;
 	struct timeval tv;
 
-	LASSERT (nob > 0);
-	LASSERT (ticks > 0);
+	LASSERT(nob > 0);
+	LASSERT(ticks > 0);
 
 	for (;;) {
 		struct kvec  iov = {
@@ -328,7 +327,7 @@ libcfs_sock_read (struct socket *sock, void *buffer, int nob, int timeout)
 			.iov_len  = nob
 		};
 		struct msghdr msg = {
-			.msg_flags      = 0
+			.msg_flags = 0
 		};
 
 		/* Set receive timeout to remaining time */
@@ -337,7 +336,7 @@ libcfs_sock_read (struct socket *sock, void *buffer, int nob, int timeout)
 			.tv_usec = ((ticks % HZ) * 1000000) / HZ
 		};
 		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO,
-				     (char *)&tv, sizeof(tv));
+				       (char *)&tv, sizeof(tv));
 		if (rc != 0) {
 			CERROR("Can't set socket recv timeout %ld.%06d: %d\n",
 			       (long)tv.tv_sec, (int)tv.tv_usec, rc);
@@ -364,31 +363,30 @@ libcfs_sock_read (struct socket *sock, void *buffer, int nob, int timeout)
 			return -ETIMEDOUT;
 	}
 }
-
-EXPORT_SYMBOL(libcfs_sock_read);
+EXPORT_SYMBOL(lnet_sock_read);
 
 static int
-libcfs_sock_create (struct socket **sockp, int *fatal,
-		    __u32 local_ip, int local_port)
+lnet_sock_create(struct socket **sockp, int *fatal, __u32 local_ip,
+		 int local_port)
 {
-	struct sockaddr_in  locaddr;
-	struct socket      *sock;
-	int		 rc;
-	int		 option;
+	struct sockaddr_in locaddr;
+	struct socket *sock;
+	int rc;
+	int option;
 
 	/* All errors are fatal except bind failure if the port is in use */
 	*fatal = 1;
 
-	rc = sock_create (PF_INET, SOCK_STREAM, 0, &sock);
+	rc = sock_create(PF_INET, SOCK_STREAM, 0, &sock);
 	*sockp = sock;
 	if (rc != 0) {
-		CERROR ("Can't create socket: %d\n", rc);
+		CERROR("Can't create socket: %d\n", rc);
 		return rc;
 	}
 
 	option = 1;
 	rc = kernel_setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
-			     (char *)&option, sizeof (option));
+			       (char *)&option, sizeof(option));
 	if (rc != 0) {
 		CERROR("Can't set SO_REUSEADDR for socket: %d\n", rc);
 		goto failed;
@@ -401,8 +399,8 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 		locaddr.sin_addr.s_addr = (local_ip == 0) ?
 					  INADDR_ANY : htonl(local_ip);
 
-		rc = sock->ops->bind(sock, (struct sockaddr *)&locaddr,
-				     sizeof(locaddr));
+		rc = kernel_bind(sock, (struct sockaddr *)&locaddr,
+				 sizeof(locaddr));
 		if (rc == -EADDRINUSE) {
 			CDEBUG(D_NET, "Port %d already in use\n", local_port);
 			*fatal = 0;
@@ -414,27 +412,26 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 			goto failed;
 		}
 	}
-
 	return 0;
 
- failed:
+failed:
 	sock_release(sock);
 	return rc;
 }
 
 int
-libcfs_sock_setbuf (struct socket *sock, int txbufsize, int rxbufsize)
+lnet_sock_setbuf(struct socket *sock, int txbufsize, int rxbufsize)
 {
-	int		 option;
-	int		 rc;
+	int option;
+	int rc;
 
 	if (txbufsize != 0) {
 		option = txbufsize;
 		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDBUF,
-				     (char *)&option, sizeof (option));
+				       (char *)&option, sizeof(option));
 		if (rc != 0) {
-			CERROR ("Can't set send buffer %d: %d\n",
-				option, rc);
+			CERROR("Can't set send buffer %d: %d\n",
+			       option, rc);
 			return rc;
 		}
 	}
@@ -442,70 +439,65 @@ libcfs_sock_setbuf (struct socket *sock, int txbufsize, int rxbufsize)
 	if (rxbufsize != 0) {
 		option = rxbufsize;
 		rc = kernel_setsockopt(sock, SOL_SOCKET, SO_RCVBUF,
-				      (char *)&option, sizeof (option));
+				      (char *)&option, sizeof(option));
 		if (rc != 0) {
-			CERROR ("Can't set receive buffer %d: %d\n",
-				option, rc);
+			CERROR("Can't set receive buffer %d: %d\n",
+			       option, rc);
 			return rc;
 		}
 	}
-
 	return 0;
 }
-
-EXPORT_SYMBOL(libcfs_sock_setbuf);
+EXPORT_SYMBOL(lnet_sock_setbuf);
 
 int
-libcfs_sock_getaddr (struct socket *sock, int remote, __u32 *ip, int *port)
+lnet_sock_getaddr(struct socket *sock, bool remote, __u32 *ip, int *port)
 {
 	struct sockaddr_in sin;
-	int		len = sizeof (sin);
-	int		rc;
+	int len = sizeof(sin);
+	int rc;
 
-	rc = sock->ops->getname (sock, (struct sockaddr *)&sin, &len,
-				 remote ? 2 : 0);
+	if (remote)
+		rc = kernel_getpeername(sock, (struct sockaddr *)&sin, &len);
+	else
+		rc = kernel_getsockname(sock, (struct sockaddr *)&sin, &len);
 	if (rc != 0) {
-		CERROR ("Error %d getting sock %s IP/port\n",
-			rc, remote ? "peer" : "local");
+		CERROR("Error %d getting sock %s IP/port\n",
+		       rc, remote ? "peer" : "local");
 		return rc;
 	}
 
 	if (ip != NULL)
-		*ip = ntohl (sin.sin_addr.s_addr);
+		*ip = ntohl(sin.sin_addr.s_addr);
 
 	if (port != NULL)
-		*port = ntohs (sin.sin_port);
+		*port = ntohs(sin.sin_port);
 
 	return 0;
 }
-
-EXPORT_SYMBOL(libcfs_sock_getaddr);
+EXPORT_SYMBOL(lnet_sock_getaddr);
 
 int
-libcfs_sock_getbuf (struct socket *sock, int *txbufsize, int *rxbufsize)
+lnet_sock_getbuf(struct socket *sock, int *txbufsize, int *rxbufsize)
 {
-
-	if (txbufsize != NULL) {
+	if (txbufsize != NULL)
 		*txbufsize = sock->sk->sk_sndbuf;
-	}
 
-	if (rxbufsize != NULL) {
+	if (rxbufsize != NULL)
 		*rxbufsize = sock->sk->sk_rcvbuf;
-	}
 
 	return 0;
 }
-
-EXPORT_SYMBOL(libcfs_sock_getbuf);
+EXPORT_SYMBOL(lnet_sock_getbuf);
 
 int
-libcfs_sock_listen (struct socket **sockp,
-		    __u32 local_ip, int local_port, int backlog)
+lnet_sock_listen(struct socket **sockp, __u32 local_ip, int local_port,
+		 int backlog)
 {
-	int      fatal;
-	int      rc;
+	int fatal;
+	int rc;
 
-	rc = libcfs_sock_create(sockp, &fatal, local_ip, local_port);
+	rc = lnet_sock_create(sockp, &fatal, local_ip, local_port);
 	if (rc != 0) {
 		if (!fatal)
 			CERROR("Can't create socket: port %d already in use\n",
@@ -513,7 +505,7 @@ libcfs_sock_listen (struct socket **sockp,
 		return rc;
 	}
 
-	rc = (*sockp)->ops->listen(*sockp, backlog);
+	rc = kernel_listen(*sockp, backlog);
 	if (rc == 0)
 		return 0;
 
@@ -521,15 +513,14 @@ libcfs_sock_listen (struct socket **sockp,
 	sock_release(*sockp);
 	return rc;
 }
-
-EXPORT_SYMBOL(libcfs_sock_listen);
+EXPORT_SYMBOL(lnet_sock_listen);
 
 int
-libcfs_sock_accept (struct socket **newsockp, struct socket *sock)
+lnet_sock_accept(struct socket **newsockp, struct socket *sock)
 {
-	wait_queue_t   wait;
+	wait_queue_t wait;
 	struct socket *newsock;
-	int	    rc;
+	int rc;
 
 	init_waitqueue_entry(&wait, current);
 
@@ -560,41 +551,30 @@ libcfs_sock_accept (struct socket **newsockp, struct socket *sock)
 	*newsockp = newsock;
 	return 0;
 
- failed:
+failed:
 	sock_release(newsock);
 	return rc;
 }
-
-EXPORT_SYMBOL(libcfs_sock_accept);
-
-void
-libcfs_sock_abort_accept (struct socket *sock)
-{
-	wake_up_all(sk_sleep(sock->sk));
-}
-
-EXPORT_SYMBOL(libcfs_sock_abort_accept);
+EXPORT_SYMBOL(lnet_sock_accept);
 
 int
-libcfs_sock_connect (struct socket **sockp, int *fatal,
-		     __u32 local_ip, int local_port,
-		     __u32 peer_ip, int peer_port)
+lnet_sock_connect(struct socket **sockp, int *fatal, __u32 local_ip,
+		  int local_port, __u32 peer_ip, int peer_port)
 {
-	struct sockaddr_in  srvaddr;
-	int		 rc;
+	struct sockaddr_in srvaddr;
+	int rc;
 
-	rc = libcfs_sock_create(sockp, fatal, local_ip, local_port);
+	rc = lnet_sock_create(sockp, fatal, local_ip, local_port);
 	if (rc != 0)
 		return rc;
 
-	memset (&srvaddr, 0, sizeof (srvaddr));
+	memset(&srvaddr, 0, sizeof(srvaddr));
 	srvaddr.sin_family = AF_INET;
 	srvaddr.sin_port = htons(peer_port);
 	srvaddr.sin_addr.s_addr = htonl(peer_ip);
 
-	rc = (*sockp)->ops->connect(*sockp,
-				    (struct sockaddr *)&srvaddr, sizeof(srvaddr),
-				    0);
+	rc = kernel_connect(*sockp, (struct sockaddr *)&srvaddr,
+			    sizeof(srvaddr), 0);
 	if (rc == 0)
 		return 0;
 
@@ -605,19 +585,10 @@ libcfs_sock_connect (struct socket **sockp, int *fatal,
 	*fatal = !(rc == -EADDRNOTAVAIL);
 
 	CDEBUG_LIMIT(*fatal ? D_NETERROR : D_NET,
-	       "Error %d connecting %pI4h/%d -> %pI4h/%d\n", rc,
-	       &local_ip, local_port, &peer_ip, peer_port);
+		     "Error %d connecting %pI4h/%d -> %pI4h/%d\n", rc,
+		     &local_ip, local_port, &peer_ip, peer_port);
 
 	sock_release(*sockp);
 	return rc;
 }
-
-EXPORT_SYMBOL(libcfs_sock_connect);
-
-void
-libcfs_sock_release (struct socket *sock)
-{
-	sock_release(sock);
-}
-
-EXPORT_SYMBOL(libcfs_sock_release);
+EXPORT_SYMBOL(lnet_sock_connect);
diff --git a/drivers/staging/lustre/lnet/lnet/lo.c b/drivers/staging/lustre/lnet/lnet/lo.c
index f708c2e649d7..2a137f46800f 100644
--- a/drivers/staging/lustre/lnet/lnet/lo.c
+++ b/drivers/staging/lustre/lnet/lnet/lo.c
@@ -111,7 +111,7 @@ lnd_t the_lolnd = {
 	/* .lnd_type       = */ LOLND,
 	/* .lnd_startup    = */ lolnd_startup,
 	/* .lnd_shutdown   = */ lolnd_shutdown,
-	/* .lnt_ctl	= */ NULL,
+	/* .lnt_ctl        = */ NULL,
 	/* .lnd_send       = */ lolnd_send,
 	/* .lnd_recv       = */ lolnd_recv,
 	/* .lnd_eager_recv = */ NULL,
diff --git a/drivers/staging/lustre/lnet/lnet/module.c b/drivers/staging/lustre/lnet/lnet/module.c
index 72b7fbc83718..576201a8390c 100644
--- a/drivers/staging/lustre/lnet/lnet/module.c
+++ b/drivers/staging/lustre/lnet/lnet/module.c
@@ -47,9 +47,9 @@ static int
 lnet_configure(void *arg)
 {
 	/* 'arg' only there so I can be passed to cfs_create_thread() */
-	int    rc = 0;
+	int rc = 0;
 
-	LNET_MUTEX_LOCK(&lnet_config_mutex);
+	mutex_lock(&lnet_config_mutex);
 
 	if (!the_lnet.ln_niinit_self) {
 		rc = LNetNIInit(LUSTRE_SRV_LNET_PID);
@@ -59,34 +59,34 @@ lnet_configure(void *arg)
 		}
 	}
 
-	LNET_MUTEX_UNLOCK(&lnet_config_mutex);
+	mutex_unlock(&lnet_config_mutex);
 	return rc;
 }
 
 static int
 lnet_unconfigure(void)
 {
-	int   refcount;
+	int refcount;
 
-	LNET_MUTEX_LOCK(&lnet_config_mutex);
+	mutex_lock(&lnet_config_mutex);
 
 	if (the_lnet.ln_niinit_self) {
 		the_lnet.ln_niinit_self = 0;
 		LNetNIFini();
 	}
 
-	LNET_MUTEX_LOCK(&the_lnet.ln_api_mutex);
+	mutex_lock(&the_lnet.ln_api_mutex);
 	refcount = the_lnet.ln_refcount;
-	LNET_MUTEX_UNLOCK(&the_lnet.ln_api_mutex);
+	mutex_unlock(&the_lnet.ln_api_mutex);
 
-	LNET_MUTEX_UNLOCK(&lnet_config_mutex);
+	mutex_unlock(&lnet_config_mutex);
 	return (refcount == 0) ? 0 : -EBUSY;
 }
 
 static int
 lnet_ioctl(unsigned int cmd, struct libcfs_ioctl_data *data)
 {
-	int   rc;
+	int rc;
 
 	switch (cmd) {
 	case IOC_LIBCFS_CONFIGURE:
@@ -113,13 +113,13 @@ static DECLARE_IOCTL_HANDLER(lnet_ioctl_handler, lnet_ioctl);
 static int __init
 init_lnet(void)
 {
-	int		  rc;
+	int rc;
 
 	mutex_init(&lnet_config_mutex);
 
-	rc = LNetInit();
+	rc = lnet_init();
 	if (rc != 0) {
-		CERROR("LNetInit: error %d\n", rc);
+		CERROR("lnet_init: error %d\n", rc);
 		return rc;
 	}
 
@@ -143,11 +143,11 @@ fini_lnet(void)
 	rc = libcfs_deregister_ioctl(&lnet_ioctl_handler);
 	LASSERT(rc == 0);
 
-	LNetFini();
+	lnet_fini();
 }
 
 MODULE_AUTHOR("Peter J. Braam <braam@clusterfs.com>");
-MODULE_DESCRIPTION("Portals v3.1");
+MODULE_DESCRIPTION("LNet v3.1");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("1.0.0");
 
diff --git a/drivers/staging/lustre/lnet/lnet/peer.c b/drivers/staging/lustre/lnet/lnet/peer.c
index 45b5742f1bd2..1fceed3c8fc0 100644
--- a/drivers/staging/lustre/lnet/lnet/peer.c
+++ b/drivers/staging/lustre/lnet/lnet/peer.c
@@ -43,10 +43,10 @@
 int
 lnet_peer_tables_create(void)
 {
-	struct lnet_peer_table	*ptable;
-	struct list_head		*hash;
-	int			i;
-	int			j;
+	struct lnet_peer_table *ptable;
+	struct list_head *hash;
+	int i;
+	int j;
 
 	the_lnet.ln_peer_tables = cfs_percpt_alloc(lnet_cpt_table(),
 						   sizeof(*ptable));
@@ -77,10 +77,10 @@ lnet_peer_tables_create(void)
 void
 lnet_peer_tables_destroy(void)
 {
-	struct lnet_peer_table	*ptable;
-	struct list_head		*hash;
-	int			i;
-	int			j;
+	struct lnet_peer_table *ptable;
+	struct list_head *hash;
+	int i;
+	int j;
 
 	if (the_lnet.ln_peer_tables == NULL)
 		return;
@@ -106,9 +106,9 @@ lnet_peer_tables_destroy(void)
 void
 lnet_peer_tables_cleanup(void)
 {
-	struct lnet_peer_table	*ptable;
-	int			i;
-	int			j;
+	struct lnet_peer_table *ptable;
+	int i;
+	int j;
 
 	LASSERT(the_lnet.ln_shutdown);	/* i.e. no new peers */
 
@@ -133,7 +133,7 @@ lnet_peer_tables_cleanup(void)
 
 	cfs_percpt_for_each(ptable, i, the_lnet.ln_peer_tables) {
 		LIST_HEAD(deathrow);
-		lnet_peer_t	*lp;
+		lnet_peer_t *lp;
 
 		lnet_net_lock(i);
 
@@ -186,8 +186,8 @@ lnet_destroy_peer_locked(lnet_peer_t *lp)
 lnet_peer_t *
 lnet_find_peer_locked(struct lnet_peer_table *ptable, lnet_nid_t nid)
 {
-	struct list_head	*peers;
-	lnet_peer_t	*lp;
+	struct list_head *peers;
+	lnet_peer_t *lp;
 
 	LASSERT(!the_lnet.ln_shutdown);
 
@@ -205,11 +205,11 @@ lnet_find_peer_locked(struct lnet_peer_table *ptable, lnet_nid_t nid)
 int
 lnet_nid2peer_locked(lnet_peer_t **lpp, lnet_nid_t nid, int cpt)
 {
-	struct lnet_peer_table	*ptable;
-	lnet_peer_t		*lp = NULL;
-	lnet_peer_t		*lp2;
-	int			cpt2;
-	int			rc = 0;
+	struct lnet_peer_table *ptable;
+	lnet_peer_t *lp = NULL;
+	lnet_peer_t *lp2;
+	int cpt2;
+	int rc = 0;
 
 	*lpp = NULL;
 	if (the_lnet.ln_shutdown) /* it's shutting down */
@@ -287,8 +287,8 @@ lnet_nid2peer_locked(lnet_peer_t **lpp, lnet_nid_t nid, int cpt)
 		goto out;
 	}
 
-	lp->lp_txcredits    =
-	lp->lp_mintxcredits = lp->lp_ni->ni_peertxcredits;
+	lp->lp_txcredits     =
+	lp->lp_mintxcredits  = lp->lp_ni->ni_peertxcredits;
 	lp->lp_rtrcredits    =
 	lp->lp_minrtrcredits = lnet_peer_buffer_credits(lp->lp_ni);
 
@@ -308,10 +308,10 @@ out:
 void
 lnet_debug_peer(lnet_nid_t nid)
 {
-	char		*aliveness = "NA";
-	lnet_peer_t	*lp;
-	int		rc;
-	int		cpt;
+	char *aliveness = "NA";
+	lnet_peer_t *lp;
+	int rc;
+	int cpt;
 
 	cpt = lnet_cpt_of_nid(nid);
 	lnet_net_lock(cpt);
diff --git a/drivers/staging/lustre/lnet/lnet/router.c b/drivers/staging/lustre/lnet/lnet/router.c
index 8510bae4822a..4fbae5ef44a9 100644
--- a/drivers/staging/lustre/lnet/lnet/router.c
+++ b/drivers/staging/lustre/lnet/lnet/router.c
@@ -24,8 +24,6 @@
 #define DEBUG_SUBSYSTEM S_LNET
 #include "../../include/linux/lnet/lib-lnet.h"
 
-#if  defined(LNET_ROUTER)
-
 #define LNET_NRB_TINY_MIN	512	/* min value for each CPT */
 #define LNET_NRB_TINY		(LNET_NRB_TINY_MIN * 4)
 #define LNET_NRB_SMALL_MIN	4096	/* min value for each CPT */
@@ -70,15 +68,6 @@ lnet_peer_buffer_credits(lnet_ni_t *ni)
 
 /* forward ref's */
 static int lnet_router_checker(void *);
-#else
-
-int
-lnet_peer_buffer_credits(lnet_ni_t *ni)
-{
-	return 0;
-}
-
-#endif
 
 static int check_routers_before_use;
 module_param(check_routers_before_use, int, 0444);
@@ -139,8 +128,8 @@ lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive,
 static void
 lnet_ni_notify_locked(lnet_ni_t *ni, lnet_peer_t *lp)
 {
-	int	alive;
-	int	notifylnd;
+	int alive;
+	int notifylnd;
 
 	/* Notify only in 1 thread at any time to ensure ordered notification.
 	 * NB individual events can be missed; the only guarantee is that you
@@ -152,7 +141,7 @@ lnet_ni_notify_locked(lnet_ni_t *ni, lnet_peer_t *lp)
 	lp->lp_notifying = 1;
 
 	while (lp->lp_notify) {
-		alive     = lp->lp_alive;
+		alive = lp->lp_alive;
 		notifylnd = lp->lp_notifylnd;
 
 		lp->lp_notifylnd = 0;
@@ -228,9 +217,9 @@ lnet_rtr_decref_locked(lnet_peer_t *lp)
 lnet_remotenet_t *
 lnet_find_net_locked(__u32 net)
 {
-	lnet_remotenet_t	*rnet;
-	struct list_head		*tmp;
-	struct list_head		*rn_list;
+	lnet_remotenet_t *rnet;
+	struct list_head *tmp;
+	struct list_head *rn_list;
 
 	LASSERT(!the_lnet.ln_shutdown);
 
@@ -276,9 +265,9 @@ static void lnet_shuffle_seed(void)
 static void
 lnet_add_route_to_rnet(lnet_remotenet_t *rnet, lnet_route_t *route)
 {
-	unsigned int      len = 0;
-	unsigned int      offset = 0;
-	struct list_head       *e;
+	unsigned int len = 0;
+	unsigned int offset = 0;
+	struct list_head *e;
 
 	lnet_shuffle_seed();
 
@@ -304,13 +293,13 @@ int
 lnet_add_route(__u32 net, unsigned int hops, lnet_nid_t gateway,
 	       unsigned int priority)
 {
-	struct list_head	  *e;
-	lnet_remotenet_t    *rnet;
-	lnet_remotenet_t    *rnet2;
-	lnet_route_t	*route;
-	lnet_ni_t	   *ni;
-	int		  add_route;
-	int		  rc;
+	struct list_head *e;
+	lnet_remotenet_t *rnet;
+	lnet_remotenet_t *rnet2;
+	lnet_route_t *route;
+	lnet_ni_t *ni;
+	int add_route;
+	int rc;
 
 	CDEBUG(D_NET, "Add route: net %s hops %u priority %u gw %s\n",
 	       libcfs_net2str(net), hops, priority, libcfs_nid2str(gateway));
@@ -416,14 +405,14 @@ lnet_add_route(__u32 net, unsigned int hops, lnet_nid_t gateway,
 int
 lnet_check_routes(void)
 {
-	lnet_remotenet_t	*rnet;
-	lnet_route_t		*route;
-	lnet_route_t		*route2;
-	struct list_head		*e1;
-	struct list_head		*e2;
-	int			cpt;
-	struct list_head		*rn_list;
-	int			i;
+	lnet_remotenet_t *rnet;
+	lnet_route_t *route;
+	lnet_route_t *route2;
+	struct list_head *e1;
+	struct list_head *e2;
+	int cpt;
+	struct list_head *rn_list;
+	int i;
 
 	cpt = lnet_net_lock_current();
 
@@ -434,9 +423,9 @@ lnet_check_routes(void)
 
 			route2 = NULL;
 			list_for_each(e2, &rnet->lrn_routes) {
-				lnet_nid_t	nid1;
-				lnet_nid_t	nid2;
-				int		net;
+				lnet_nid_t nid1;
+				lnet_nid_t nid2;
+				int net;
 
 				route = list_entry(e2, lnet_route_t,
 						       lr_list);
@@ -472,14 +461,14 @@ lnet_check_routes(void)
 int
 lnet_del_route(__u32 net, lnet_nid_t gw_nid)
 {
-	struct lnet_peer	*gateway;
-	lnet_remotenet_t	*rnet;
-	lnet_route_t		*route;
-	struct list_head		*e1;
-	struct list_head		*e2;
-	int			rc = -ENOENT;
-	struct list_head		*rn_list;
-	int			idx = 0;
+	struct lnet_peer *gateway;
+	lnet_remotenet_t *rnet;
+	lnet_route_t *route;
+	struct list_head *e1;
+	struct list_head *e2;
+	int rc = -ENOENT;
+	struct list_head *rn_list;
+	int idx = 0;
 
 	CDEBUG(D_NET, "Del route: net %s : gw %s\n",
 	       libcfs_net2str(net), libcfs_nid2str(gw_nid));
@@ -554,13 +543,13 @@ int
 lnet_get_route(int idx, __u32 *net, __u32 *hops,
 	       lnet_nid_t *gateway, __u32 *alive, __u32 *priority)
 {
-	struct list_head		*e1;
-	struct list_head		*e2;
-	lnet_remotenet_t	*rnet;
-	lnet_route_t		*route;
-	int			cpt;
-	int			i;
-	struct list_head		*rn_list;
+	struct list_head *e1;
+	struct list_head *e2;
+	lnet_remotenet_t *rnet;
+	lnet_route_t *route;
+	int cpt;
+	int i;
+	struct list_head *rn_list;
 
 	cpt = lnet_net_lock_current();
 
@@ -574,11 +563,11 @@ lnet_get_route(int idx, __u32 *net, __u32 *hops,
 						       lr_list);
 
 				if (idx-- == 0) {
-					*net	  = rnet->lrn_net;
-					*hops	  = route->lr_hops;
+					*net      = rnet->lrn_net;
+					*hops     = route->lr_hops;
 					*priority = route->lr_priority;
 					*gateway  = route->lr_gateway->lp_nid;
-					*alive	  = route->lr_gateway->lp_alive;
+					*alive    = route->lr_gateway->lp_alive;
 					lnet_net_unlock(cpt);
 					return 0;
 				}
@@ -593,7 +582,7 @@ lnet_get_route(int idx, __u32 *net, __u32 *hops,
 void
 lnet_swap_pinginfo(lnet_ping_info_t *info)
 {
-	int	       i;
+	int i;
 	lnet_ni_status_t *stat;
 
 	__swab32s(&info->pi_magic);
@@ -614,9 +603,9 @@ lnet_swap_pinginfo(lnet_ping_info_t *info)
 static void
 lnet_parse_rc_info(lnet_rc_data_t *rcd)
 {
-	lnet_ping_info_t	*info = rcd->rcd_pinginfo;
-	struct lnet_peer	*gw   = rcd->rcd_gateway;
-	lnet_route_t		*rtr;
+	lnet_ping_info_t *info = rcd->rcd_pinginfo;
+	struct lnet_peer *gw = rcd->rcd_gateway;
+	lnet_route_t *rtr;
 
 	if (!gw->lp_alive)
 		return;
@@ -643,14 +632,14 @@ lnet_parse_rc_info(lnet_rc_data_t *rcd)
 		return; /* can't carry NI status info */
 
 	list_for_each_entry(rtr, &gw->lp_routes, lr_gwlist) {
-		int	ptl_status = LNET_NI_STATUS_INVALID;
-		int	down = 0;
-		int	up = 0;
-		int	i;
+		int ptl_status = LNET_NI_STATUS_INVALID;
+		int down = 0;
+		int up = 0;
+		int i;
 
 		for (i = 0; i < info->pi_nnis && i < LNET_MAX_RTR_NIS; i++) {
 			lnet_ni_status_t *stat = &info->pi_ni[i];
-			lnet_nid_t	 nid = stat->ns_nid;
+			lnet_nid_t nid = stat->ns_nid;
 
 			if (nid == LNET_NID_ANY) {
 				CDEBUG(D_NET, "%s: unexpected LNET_NID_ANY\n",
@@ -699,8 +688,8 @@ lnet_parse_rc_info(lnet_rc_data_t *rcd)
 static void
 lnet_router_checker_event(lnet_event_t *event)
 {
-	lnet_rc_data_t		*rcd = event->md.user_ptr;
-	struct lnet_peer	*lp;
+	lnet_rc_data_t *rcd = event->md.user_ptr;
+	struct lnet_peer *lp;
 
 	LASSERT(rcd != NULL);
 
@@ -752,14 +741,14 @@ lnet_router_checker_event(lnet_event_t *event)
 static void
 lnet_wait_known_routerstate(void)
 {
-	lnet_peer_t	 *rtr;
-	struct list_head	  *entry;
-	int		  all_known;
+	lnet_peer_t *rtr;
+	struct list_head *entry;
+	int all_known;
 
 	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
 
 	for (;;) {
-		int	cpt = lnet_net_lock_current();
+		int cpt = lnet_net_lock_current();
 
 		all_known = 1;
 		list_for_each(entry, &the_lnet.ln_routers) {
@@ -799,9 +788,9 @@ lnet_router_ni_update_locked(lnet_peer_t *gw, __u32 net)
 static void
 lnet_update_ni_status_locked(void)
 {
-	lnet_ni_t	*ni;
-	long		now;
-	int		timeout;
+	lnet_ni_t *ni;
+	long now;
+	int timeout;
 
 	LASSERT(the_lnet.ln_routing);
 
@@ -860,10 +849,10 @@ lnet_destroy_rc_data(lnet_rc_data_t *rcd)
 static lnet_rc_data_t *
 lnet_create_rc_data_locked(lnet_peer_t *gateway)
 {
-	lnet_rc_data_t		*rcd = NULL;
-	lnet_ping_info_t	*pi;
-	int			rc;
-	int			i;
+	lnet_rc_data_t *rcd = NULL;
+	lnet_ping_info_t *pi;
+	int rc;
+	int i;
 
 	lnet_net_unlock(gateway->lp_cpt);
 
@@ -943,8 +932,8 @@ static void
 lnet_ping_router_locked(lnet_peer_t *rtr)
 {
 	lnet_rc_data_t *rcd = NULL;
-	unsigned long      now = cfs_time_current();
-	int	     secs;
+	unsigned long now = cfs_time_current();
+	int secs;
 
 	lnet_peer_addref_locked(rtr);
 
@@ -979,9 +968,9 @@ lnet_ping_router_locked(lnet_peer_t *rtr)
 	if (secs != 0 && !rtr->lp_ping_notsent &&
 	    cfs_time_after(now, cfs_time_add(rtr->lp_ping_timestamp,
 					     cfs_time_seconds(secs)))) {
-		int	       rc;
+		int rc;
 		lnet_process_id_t id;
-		lnet_handle_md_t  mdh;
+		lnet_handle_md_t mdh;
 
 		id.nid = rtr->lp_nid;
 		id.pid = LUSTRE_SRV_LNET_PID;
@@ -1013,8 +1002,8 @@ lnet_ping_router_locked(lnet_peer_t *rtr)
 int
 lnet_router_checker_start(void)
 {
-	int	  rc;
-	int	  eqsz;
+	int rc;
+	int eqsz;
 
 	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_SHUTDOWN);
 
@@ -1085,11 +1074,11 @@ lnet_router_checker_stop(void)
 static void
 lnet_prune_rc_data(int wait_unlink)
 {
-	lnet_rc_data_t		*rcd;
-	lnet_rc_data_t		*tmp;
-	lnet_peer_t		*lp;
-	struct list_head		head;
-	int			i = 2;
+	lnet_rc_data_t *rcd;
+	lnet_rc_data_t *tmp;
+	lnet_peer_t *lp;
+	struct list_head head;
+	int i = 2;
 
 	if (likely(the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING &&
 		   list_empty(&the_lnet.ln_rcd_deathrow) &&
@@ -1163,23 +1152,20 @@ lnet_prune_rc_data(int wait_unlink)
 	lnet_net_unlock(LNET_LOCK_EX);
 }
 
-
-#if  defined(LNET_ROUTER)
-
 static int
 lnet_router_checker(void *arg)
 {
-	lnet_peer_t       *rtr;
-	struct list_head	*entry;
+	lnet_peer_t *rtr;
+	struct list_head *entry;
 
 	cfs_block_allsigs();
 
 	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
 
 	while (the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING) {
-		__u64	version;
-		int	cpt;
-		int	cpt2;
+		__u64 version;
+		int cpt;
+		int cpt2;
 
 		cpt = lnet_net_lock_current();
 rescan:
@@ -1245,11 +1231,11 @@ lnet_destroy_rtrbuf(lnet_rtrbuf_t *rb, int npages)
 static lnet_rtrbuf_t *
 lnet_new_rtrbuf(lnet_rtrbufpool_t *rbp, int cpt)
 {
-	int	    npages = rbp->rbp_npages;
-	int	    sz = offsetof(lnet_rtrbuf_t, rb_kiov[npages]);
-	struct page   *page;
+	int npages = rbp->rbp_npages;
+	int sz = offsetof(lnet_rtrbuf_t, rb_kiov[npages]);
+	struct page *page;
 	lnet_rtrbuf_t *rb;
-	int	    i;
+	int i;
 
 	LIBCFS_CPT_ALLOC(rb, lnet_cpt_table(), cpt, sz);
 	if (rb == NULL)
@@ -1280,9 +1266,9 @@ lnet_new_rtrbuf(lnet_rtrbufpool_t *rbp, int cpt)
 static void
 lnet_rtrpool_free_bufs(lnet_rtrbufpool_t *rbp)
 {
-	int		npages = rbp->rbp_npages;
-	int		nbuffers = 0;
-	lnet_rtrbuf_t	*rb;
+	int npages = rbp->rbp_npages;
+	int nbuffers = 0;
+	lnet_rtrbuf_t *rb;
 
 	if (rbp->rbp_nbuffers == 0) /* not initialized or already freed */
 		return;
@@ -1310,7 +1296,7 @@ static int
 lnet_rtrpool_alloc_bufs(lnet_rtrbufpool_t *rbp, int nbufs, int cpt)
 {
 	lnet_rtrbuf_t *rb;
-	int	    i;
+	int i;
 
 	if (rbp->rbp_nbuffers != 0) {
 		LASSERT(rbp->rbp_nbuffers == nbufs);
@@ -1355,7 +1341,7 @@ void
 lnet_rtrpools_free(void)
 {
 	lnet_rtrbufpool_t *rtrp;
-	int		  i;
+	int i;
 
 	if (the_lnet.ln_rtrpools == NULL) /* uninitialized or freed */
 		return;
@@ -1373,7 +1359,7 @@ lnet_rtrpools_free(void)
 static int
 lnet_nrb_tiny_calculate(int npages)
 {
-	int	nrbs = LNET_NRB_TINY;
+	int nrbs = LNET_NRB_TINY;
 
 	if (tiny_router_buffers < 0) {
 		LCONSOLE_ERROR_MSG(0x10c,
@@ -1392,7 +1378,7 @@ lnet_nrb_tiny_calculate(int npages)
 static int
 lnet_nrb_small_calculate(int npages)
 {
-	int	nrbs = LNET_NRB_SMALL;
+	int nrbs = LNET_NRB_SMALL;
 
 	if (small_router_buffers < 0) {
 		LCONSOLE_ERROR_MSG(0x10c,
@@ -1411,7 +1397,7 @@ lnet_nrb_small_calculate(int npages)
 static int
 lnet_nrb_large_calculate(int npages)
 {
-	int	nrbs = LNET_NRB_LARGE;
+	int nrbs = LNET_NRB_LARGE;
 
 	if (large_router_buffers < 0) {
 		LCONSOLE_ERROR_MSG(0x10c,
@@ -1431,13 +1417,13 @@ int
 lnet_rtrpools_alloc(int im_a_router)
 {
 	lnet_rtrbufpool_t *rtrp;
-	int	large_pages;
-	int	small_pages = 1;
-	int	nrb_tiny;
-	int	nrb_small;
-	int	nrb_large;
-	int	rc;
-	int	i;
+	int large_pages;
+	int small_pages = 1;
+	int nrb_tiny;
+	int nrb_small;
+	int nrb_large;
+	int rc;
+	int i;
 
 	large_pages = (LNET_MTU + PAGE_CACHE_SIZE - 1) >> PAGE_CACHE_SHIFT;
 
@@ -1507,9 +1493,9 @@ lnet_rtrpools_alloc(int im_a_router)
 int
 lnet_notify(lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
 {
-	struct lnet_peer	*lp = NULL;
-	unsigned long		now = cfs_time_current();
-	int			cpt = lnet_cpt_of_nid(nid);
+	struct lnet_peer *lp = NULL;
+	unsigned long now = cfs_time_current();
+	int cpt = lnet_cpt_of_nid(nid);
 
 	LASSERT(!in_interrupt ());
 
@@ -1573,134 +1559,3 @@ lnet_notify(lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
 	return 0;
 }
 EXPORT_SYMBOL(lnet_notify);
-
-void
-lnet_get_tunables(void)
-{
-}
-
-#else
-
-int
-lnet_notify(lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
-{
-	return -EOPNOTSUPP;
-}
-
-void
-lnet_router_checker(void)
-{
-	static time_t last;
-	static int    running;
-
-	time_t	    now = get_seconds();
-	int	       interval = now - last;
-	int	       rc;
-	__u64	     version;
-	lnet_peer_t      *rtr;
-
-	/* It's no use to call me again within a sec - all intervals and
-	 * timeouts are measured in seconds */
-	if (last != 0 && interval < 2)
-		return;
-
-	if (last != 0 &&
-	    interval > max(live_router_check_interval,
-			   dead_router_check_interval))
-		CNETERR("Checker(%d/%d) not called for %d seconds\n",
-			live_router_check_interval, dead_router_check_interval,
-			interval);
-
-	LASSERT(LNET_CPT_NUMBER == 1);
-
-	lnet_net_lock(0);
-	LASSERT(!running); /* recursion check */
-	running = 1;
-	lnet_net_unlock(0);
-
-	last = now;
-
-	if (the_lnet.ln_rc_state == LNET_RC_STATE_STOPPING)
-		lnet_prune_rc_data(0); /* unlink all rcd and nowait */
-
-	/* consume all pending events */
-	while (1) {
-		int	  i;
-		lnet_event_t ev;
-
-		/* NB ln_rc_eqh must be the 1st in 'eventqs' otherwise the
-		 * recursion breaker in LNetEQPoll would fail */
-		rc = LNetEQPoll(&the_lnet.ln_rc_eqh, 1, 0, &ev, &i);
-		if (rc == 0)   /* no event pending */
-			break;
-
-		/* NB a lost SENT prevents me from pinging a router again */
-		if (rc == -EOVERFLOW) {
-			CERROR("Dropped an event!!!\n");
-			abort();
-		}
-
-		LASSERT(rc == 1);
-
-		lnet_router_checker_event(&ev);
-	}
-
-	if (the_lnet.ln_rc_state == LNET_RC_STATE_STOPPING) {
-		lnet_prune_rc_data(1); /* release rcd */
-		the_lnet.ln_rc_state = LNET_RC_STATE_SHUTDOWN;
-		running = 0;
-		return;
-	}
-
-	LASSERT(the_lnet.ln_rc_state == LNET_RC_STATE_RUNNING);
-
-	lnet_net_lock(0);
-
-	version = the_lnet.ln_routers_version;
-	list_for_each_entry(rtr, &the_lnet.ln_routers, lp_rtr_list) {
-		lnet_ping_router_locked(rtr);
-		LASSERT(version == the_lnet.ln_routers_version);
-	}
-
-	lnet_net_unlock(0);
-
-	running = 0; /* lock only needed for the recursion check */
-}
-
-/* NB lnet_peers_start_down depends on me,
- * so must be called before any peer creation */
-void
-lnet_get_tunables(void)
-{
-	char *s;
-
-	s = getenv("LNET_ROUTER_PING_TIMEOUT");
-	if (s != NULL)
-		router_ping_timeout = atoi(s);
-
-	s = getenv("LNET_LIVE_ROUTER_CHECK_INTERVAL");
-	if (s != NULL)
-		live_router_check_interval = atoi(s);
-
-	s = getenv("LNET_DEAD_ROUTER_CHECK_INTERVAL");
-	if (s != NULL)
-		dead_router_check_interval = atoi(s);
-
-	/* This replaces old lnd_notify mechanism */
-	check_routers_before_use = 1;
-	if (dead_router_check_interval <= 0)
-		dead_router_check_interval = 30;
-}
-
-void
-lnet_rtrpools_free(void)
-{
-}
-
-int
-lnet_rtrpools_alloc(int im_a_arouter)
-{
-	return 0;
-}
-
-#endif
diff --git a/drivers/staging/lustre/lnet/lnet/router_proc.c b/drivers/staging/lustre/lnet/lnet/router_proc.c
index c055afc86eb4..ee902dc43823 100644
--- a/drivers/staging/lustre/lnet/lnet/router_proc.c
+++ b/drivers/staging/lustre/lnet/lnet/router_proc.c
@@ -112,11 +112,11 @@ static int proc_call_handler(void *data, int write, loff_t *ppos,
 static int __proc_lnet_stats(void *data, int write,
 			     loff_t pos, void __user *buffer, int nob)
 {
-	int	      rc;
+	int rc;
 	lnet_counters_t *ctrs;
-	int	      len;
-	char	    *tmpstr;
-	const int	tmpsiz = 256; /* 7 %u and 4 %llu */
+	int len;
+	char *tmpstr;
+	const int tmpsiz = 256; /* 7 %u and 4 %llu */
 
 	if (write) {
 		lnet_counters_reset();
@@ -167,13 +167,13 @@ static int proc_lnet_stats(struct ctl_table *table, int write,
 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;
-	char		*s;
-	int		rc = 0;
-	int		len;
-	int		ver;
-	int		off;
+	const int tmpsiz = 256;
+	char *tmpstr;
+	char *s;
+	int rc = 0;
+	int len;
+	int ver;
+	int off;
 
 	CLASSERT(sizeof(loff_t) >= 4);
 
@@ -205,13 +205,13 @@ static int proc_lnet_routes(struct ctl_table *table, int write,
 		lnet_net_unlock(0);
 		*ppos = LNET_PROC_POS_MAKE(0, ver, 0, off);
 	} else {
-		struct list_head		*n;
-		struct list_head		*r;
-		lnet_route_t		*route = NULL;
-		lnet_remotenet_t	*rnet  = NULL;
-		int			skip  = off - 1;
-		struct list_head		*rn_list;
-		int			i;
+		struct list_head *n;
+		struct list_head *r;
+		lnet_route_t *route = NULL;
+		lnet_remotenet_t *rnet  = NULL;
+		int skip  = off - 1;
+		struct list_head *rn_list;
+		int i;
 
 		lnet_net_lock(0);
 
@@ -251,11 +251,11 @@ static int proc_lnet_routes(struct ctl_table *table, int write,
 		}
 
 		if (route != NULL) {
-			__u32        net	= rnet->lrn_net;
-			unsigned int hops	= route->lr_hops;
-			unsigned int priority	= route->lr_priority;
-			lnet_nid_t   nid	= route->lr_gateway->lp_nid;
-			int          alive	= route->lr_gateway->lp_alive;
+			__u32 net = rnet->lrn_net;
+			unsigned int hops = route->lr_hops;
+			unsigned int priority = route->lr_priority;
+			lnet_nid_t nid = route->lr_gateway->lp_nid;
+			int alive = route->lr_gateway->lp_alive;
 
 			s += snprintf(s, tmpstr + tmpsiz - s,
 				      "%-8s %4u %8u %7s %s\n",
@@ -293,13 +293,13 @@ static int proc_lnet_routes(struct ctl_table *table, int write,
 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;
-	char      *s;
-	const int  tmpsiz = 256;
-	int	len;
-	int	ver;
-	int	off;
+	int rc = 0;
+	char *tmpstr;
+	char *s;
+	const int tmpsiz = 256;
+	int len;
+	int ver;
+	int off;
 
 	off = LNET_PROC_HOFF_GET(*ppos);
 	ver = LNET_PROC_VER_GET(*ppos);
@@ -328,9 +328,9 @@ static int proc_lnet_routers(struct ctl_table *table, int write,
 		lnet_net_unlock(0);
 		*ppos = LNET_PROC_POS_MAKE(0, ver, 0, off);
 	} else {
-		struct list_head		*r;
-		struct lnet_peer	*peer = NULL;
-		int			skip = off - 1;
+		struct list_head *r;
+		struct lnet_peer *peer = NULL;
+		int skip = off - 1;
 
 		lnet_net_lock(0);
 
@@ -360,14 +360,14 @@ static int proc_lnet_routers(struct ctl_table *table, int write,
 			lnet_nid_t nid = peer->lp_nid;
 			unsigned long now = cfs_time_current();
 			unsigned long deadline = peer->lp_ping_deadline;
-			int nrefs     = peer->lp_refcount;
-			int nrtrrefs  = peer->lp_rtr_refcount;
+			int nrefs = peer->lp_refcount;
+			int nrtrrefs = peer->lp_rtr_refcount;
 			int alive_cnt = peer->lp_alive_count;
-			int alive     = peer->lp_alive;
-			int pingsent  = !peer->lp_ping_notsent;
+			int alive = peer->lp_alive;
+			int pingsent = !peer->lp_ping_notsent;
 			int last_ping = cfs_duration_sec(cfs_time_sub(now,
 						     peer->lp_ping_timestamp));
-			int down_ni   = 0;
+			int down_ni = 0;
 			lnet_route_t *rtr;
 
 			if ((peer->lp_ping_feats &
@@ -428,16 +428,16 @@ static int proc_lnet_routers(struct ctl_table *table, int write,
 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;
-	char			*tmpstr;
-	char			*s;
-	int			cpt  = LNET_PROC_CPT_GET(*ppos);
-	int			ver  = LNET_PROC_VER_GET(*ppos);
-	int			hash = LNET_PROC_HASH_GET(*ppos);
-	int			hoff = LNET_PROC_HOFF_GET(*ppos);
-	int			rc = 0;
-	int			len;
+	const int tmpsiz  = 256;
+	struct lnet_peer_table *ptable;
+	char *tmpstr;
+	char *s;
+	int cpt  = LNET_PROC_CPT_GET(*ppos);
+	int ver  = LNET_PROC_VER_GET(*ppos);
+	int hash = LNET_PROC_HASH_GET(*ppos);
+	int hoff = LNET_PROC_HOFF_GET(*ppos);
+	int rc = 0;
+	int len;
 
 	CLASSERT(LNET_PROC_HASH_BITS >= LNET_PEER_HASH_BITS);
 	LASSERT(!write);
@@ -465,9 +465,9 @@ static int proc_lnet_peers(struct ctl_table *table, int write,
 
 		hoff++;
 	} else {
-		struct lnet_peer	*peer;
-		struct list_head		*p;
-		int			skip;
+		struct lnet_peer *peer;
+		struct list_head *p;
+		int skip;
  again:
 		p = NULL;
 		peer = NULL;
@@ -521,23 +521,23 @@ static int proc_lnet_peers(struct ctl_table *table, int write,
 		}
 
 		if (peer != NULL) {
-			lnet_nid_t nid       = peer->lp_nid;
-			int	nrefs     = peer->lp_refcount;
-			int	lastalive = -1;
-			char      *aliveness = "NA";
-			int	maxcr     = peer->lp_ni->ni_peertxcredits;
-			int	txcr      = peer->lp_txcredits;
-			int	mintxcr   = peer->lp_mintxcredits;
-			int	rtrcr     = peer->lp_rtrcredits;
-			int	minrtrcr  = peer->lp_minrtrcredits;
-			int	txqnob    = peer->lp_txqnob;
+			lnet_nid_t nid = peer->lp_nid;
+			int nrefs = peer->lp_refcount;
+			int lastalive = -1;
+			char *aliveness = "NA";
+			int maxcr = peer->lp_ni->ni_peertxcredits;
+			int txcr = peer->lp_txcredits;
+			int mintxcr = peer->lp_mintxcredits;
+			int rtrcr = peer->lp_rtrcredits;
+			int minrtrcr = peer->lp_minrtrcredits;
+			int txqnob = peer->lp_txqnob;
 
 			if (lnet_isrouter(peer) ||
 			    lnet_peer_aliveness_enabled(peer))
 				aliveness = peer->lp_alive ? "up" : "down";
 
 			if (lnet_peer_aliveness_enabled(peer)) {
-				unsigned long     now = cfs_time_current();
+				unsigned long now = cfs_time_current();
 				long delta;
 
 				delta = cfs_time_sub(now, peer->lp_last_alive);
@@ -595,13 +595,13 @@ static int proc_lnet_peers(struct ctl_table *table, int write,
 static int __proc_lnet_buffers(void *data, int write,
 			       loff_t pos, void __user *buffer, int nob)
 {
-	char	    *s;
-	char	    *tmpstr;
-	int		tmpsiz;
-	int		idx;
-	int		len;
-	int		rc;
-	int		i;
+	char *s;
+	char *tmpstr;
+	int tmpsiz;
+	int idx;
+	int len;
+	int rc;
+	int i;
 
 	LASSERT(!write);
 
@@ -660,11 +660,11 @@ static int proc_lnet_buffers(struct ctl_table *table, int write,
 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;
-	char      *tmpstr;
-	char      *s;
-	int	len;
+	int tmpsiz = 128 * LNET_CPT_NUMBER;
+	int rc = 0;
+	char *tmpstr;
+	char *s;
+	int len;
 
 	LASSERT(!write);
 
@@ -684,9 +684,9 @@ static int proc_lnet_nis(struct ctl_table *table, int write,
 			      "rtr", "max", "tx", "min");
 		LASSERT(tmpstr + tmpsiz - s > 0);
 	} else {
-		struct list_head	*n;
-		lnet_ni_t	 *ni   = NULL;
-		int		skip = *ppos - 1;
+		struct list_head *n;
+		lnet_ni_t *ni   = NULL;
+		int skip = *ppos - 1;
 
 		lnet_net_lock(0);
 
@@ -705,12 +705,12 @@ static int proc_lnet_nis(struct ctl_table *table, int write,
 		}
 
 		if (ni != NULL) {
-			struct lnet_tx_queue	*tq;
-			char	*stat;
-			long	now = get_seconds();
-			int	last_alive = -1;
-			int	i;
-			int	j;
+			struct lnet_tx_queue *tq;
+			char *stat;
+			long now = get_seconds();
+			int last_alive = -1;
+			int i;
+			int j;
 
 			if (the_lnet.ln_routing)
 				last_alive = now - ni->ni_last_alive;
@@ -777,9 +777,9 @@ static int proc_lnet_nis(struct ctl_table *table, int write,
 }
 
 struct lnet_portal_rotors {
-	int	     pr_value;
-	const char      *pr_name;
-	const char	*pr_desc;
+	int pr_value;
+	const char *pr_name;
+	const char *pr_desc;
 };
 
 static struct lnet_portal_rotors	portal_rotors[] = {
@@ -815,11 +815,11 @@ extern int portal_rotor;
 static int __proc_lnet_portal_rotor(void *data, int write,
 				    loff_t pos, void __user *buffer, int nob)
 {
-	const int	buf_len	= 128;
-	char		*buf;
-	char		*tmp;
-	int		rc;
-	int		i;
+	const int buf_len = 128;
+	char *buf;
+	char *tmp;
+	int rc;
+	int i;
 
 	LIBCFS_ALLOC(buf, buf_len);
 	if (buf == NULL)
@@ -887,38 +887,38 @@ static struct ctl_table lnet_table[] = {
 	 * to go via /proc for portability.
 	 */
 	{
-		.procname = "stats",
-		.mode     = 0644,
+		.procname     = "stats",
+		.mode         = 0644,
 		.proc_handler = &proc_lnet_stats,
 	},
 	{
-		.procname = "routes",
-		.mode     = 0444,
+		.procname     = "routes",
+		.mode         = 0444,
 		.proc_handler = &proc_lnet_routes,
 	},
 	{
-		.procname = "routers",
-		.mode     = 0444,
+		.procname     = "routers",
+		.mode         = 0444,
 		.proc_handler = &proc_lnet_routers,
 	},
 	{
-		.procname = "peers",
-		.mode     = 0444,
+		.procname     = "peers",
+		.mode         = 0444,
 		.proc_handler = &proc_lnet_peers,
 	},
 	{
-		.procname = "buffers",
-		.mode     = 0444,
+		.procname     = "buffers",
+		.mode         = 0444,
 		.proc_handler = &proc_lnet_buffers,
 	},
 	{
-		.procname = "nis",
-		.mode     = 0444,
+		.procname     = "nis",
+		.mode         = 0444,
 		.proc_handler = &proc_lnet_nis,
 	},
 	{
-		.procname = "portal_rotor",
-		.mode     = 0644,
+		.procname     = "portal_rotor",
+		.mode         = 0644,
 		.proc_handler = &proc_lnet_portal_rotor,
 	},
 	{
diff --git a/drivers/staging/lustre/lnet/selftest/brw_test.c b/drivers/staging/lustre/lnet/selftest/brw_test.c
index 658f4584fff8..de11f1bc8be7 100644
--- a/drivers/staging/lustre/lnet/selftest/brw_test.c
+++ b/drivers/staging/lustre/lnet/selftest/brw_test.c
@@ -91,7 +91,7 @@ brw_client_init(sfw_test_instance_t *tsi)
 		len   = npg * PAGE_CACHE_SIZE;
 
 	} else {
-		test_bulk_req_v1_t  *breq = &tsi->tsi_u.bulk_v1;
+		test_bulk_req_v1_t *breq = &tsi->tsi_u.bulk_v1;
 
 		/* I should never get this step if it's unknown feature
 		 * because make_session will reject unknown feature */
@@ -223,7 +223,7 @@ bad_data:
 static void
 brw_fill_bulk(srpc_bulk_t *bk, int pattern, __u64 magic)
 {
-	int	 i;
+	int i;
 	struct page *pg;
 
 	for (i = 0; i < bk->bk_niov; i++) {
@@ -235,7 +235,7 @@ brw_fill_bulk(srpc_bulk_t *bk, int pattern, __u64 magic)
 static int
 brw_check_bulk(srpc_bulk_t *bk, int pattern, __u64 magic)
 {
-	int	 i;
+	int i;
 	struct page *pg;
 
 	for (i = 0; i < bk->bk_niov; i++) {
@@ -254,16 +254,16 @@ static int
 brw_client_prep_rpc(sfw_test_unit_t *tsu,
 		     lnet_process_id_t dest, srpc_client_rpc_t **rpcpp)
 {
-	srpc_bulk_t	 *bulk = tsu->tsu_private;
+	srpc_bulk_t *bulk = tsu->tsu_private;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
-	sfw_session_t	    *sn = tsi->tsi_batch->bat_session;
-	srpc_client_rpc_t   *rpc;
-	srpc_brw_reqst_t    *req;
-	int		     flags;
-	int		     npg;
-	int		     len;
-	int		     opc;
-	int		     rc;
+	sfw_session_t *sn = tsi->tsi_batch->bat_session;
+	srpc_client_rpc_t *rpc;
+	srpc_brw_reqst_t *req;
+	int flags;
+	int npg;
+	int len;
+	int opc;
+	int rc;
 
 	LASSERT(sn != NULL);
 	LASSERT(bulk != NULL);
@@ -277,7 +277,7 @@ brw_client_prep_rpc(sfw_test_unit_t *tsu,
 		len   = npg * PAGE_CACHE_SIZE;
 
 	} else {
-		test_bulk_req_v1_t  *breq = &tsi->tsi_u.bulk_v1;
+		test_bulk_req_v1_t *breq = &tsi->tsi_u.bulk_v1;
 
 		/* I should never get this step if it's unknown feature
 		 * because make_session will reject unknown feature */
@@ -311,12 +311,12 @@ brw_client_prep_rpc(sfw_test_unit_t *tsu,
 static void
 brw_client_done_rpc(sfw_test_unit_t *tsu, srpc_client_rpc_t *rpc)
 {
-	__u64		magic = BRW_MAGIC;
+	__u64 magic = BRW_MAGIC;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
-	sfw_session_t       *sn = tsi->tsi_batch->bat_session;
-	srpc_msg_t	  *msg = &rpc->crpc_replymsg;
-	srpc_brw_reply_t    *reply = &msg->msg_body.brw_reply;
-	srpc_brw_reqst_t    *reqst = &rpc->crpc_reqstmsg.msg_body.brw_reqst;
+	sfw_session_t *sn = tsi->tsi_batch->bat_session;
+	srpc_msg_t *msg = &rpc->crpc_replymsg;
+	srpc_brw_reply_t *reply = &msg->msg_body.brw_reply;
+	srpc_brw_reqst_t *reqst = &rpc->crpc_reqstmsg.msg_body.brw_reqst;
 
 	LASSERT(sn != NULL);
 
@@ -380,10 +380,10 @@ brw_server_rpc_done(srpc_server_rpc_t *rpc)
 static int
 brw_bulk_ready(srpc_server_rpc_t *rpc, int status)
 {
-	__u64	     magic = BRW_MAGIC;
+	__u64 magic = BRW_MAGIC;
 	srpc_brw_reply_t *reply = &rpc->srpc_replymsg.msg_body.brw_reply;
 	srpc_brw_reqst_t *reqst;
-	srpc_msg_t       *reqstmsg;
+	srpc_msg_t *reqstmsg;
 
 	LASSERT(rpc->srpc_bulk != NULL);
 	LASSERT(rpc->srpc_reqstbuf != NULL);
@@ -416,13 +416,13 @@ brw_bulk_ready(srpc_server_rpc_t *rpc, int status)
 static int
 brw_server_handle(struct srpc_server_rpc *rpc)
 {
-	struct srpc_service	*sv = rpc->srpc_scd->scd_svc;
-	srpc_msg_t       *replymsg = &rpc->srpc_replymsg;
-	srpc_msg_t       *reqstmsg = &rpc->srpc_reqstbuf->buf_msg;
+	struct srpc_service *sv = rpc->srpc_scd->scd_svc;
+	srpc_msg_t *replymsg = &rpc->srpc_replymsg;
+	srpc_msg_t *reqstmsg = &rpc->srpc_reqstbuf->buf_msg;
 	srpc_brw_reply_t *reply = &replymsg->msg_body.brw_reply;
 	srpc_brw_reqst_t *reqst = &reqstmsg->msg_body.brw_reqst;
-	int		  npg;
-	int	       rc;
+	int npg;
+	int rc;
 
 	LASSERT(sv->sv_id == SRPC_SERVICE_BRW);
 
@@ -490,17 +490,17 @@ brw_server_handle(struct srpc_server_rpc *rpc)
 sfw_test_client_ops_t brw_test_client;
 void brw_init_test_client(void)
 {
-	brw_test_client.tso_init       = brw_client_init;
-	brw_test_client.tso_fini       = brw_client_fini;
-	brw_test_client.tso_prep_rpc   = brw_client_prep_rpc;
-	brw_test_client.tso_done_rpc   = brw_client_done_rpc;
+	brw_test_client.tso_init     = brw_client_init;
+	brw_test_client.tso_fini     = brw_client_fini;
+	brw_test_client.tso_prep_rpc = brw_client_prep_rpc;
+	brw_test_client.tso_done_rpc = brw_client_done_rpc;
 };
 
 srpc_service_t brw_test_service;
 void brw_init_test_service(void)
 {
 
-	brw_test_service.sv_id	 = SRPC_SERVICE_BRW;
+	brw_test_service.sv_id         = SRPC_SERVICE_BRW;
 	brw_test_service.sv_name       = "brw_test";
 	brw_test_service.sv_handler    = brw_server_handle;
 	brw_test_service.sv_bulk_ready = brw_bulk_ready;
diff --git a/drivers/staging/lustre/lnet/selftest/conctl.c b/drivers/staging/lustre/lnet/selftest/conctl.c
index 045fe295ad54..1a7870e91f23 100644
--- a/drivers/staging/lustre/lnet/selftest/conctl.c
+++ b/drivers/staging/lustre/lnet/selftest/conctl.c
@@ -48,11 +48,11 @@
 static int
 lst_session_new_ioctl(lstio_session_new_args_t *args)
 {
-	char      *name;
-	int	rc;
+	char *name;
+	int rc;
 
 	if (args->lstio_ses_idp   == NULL || /* address for output sid */
-	    args->lstio_ses_key   == 0 || /* no key is specified */
+	    args->lstio_ses_key   == 0 ||    /* no key is specified */
 	    args->lstio_ses_namep == NULL || /* session name */
 	    args->lstio_ses_nmlen <= 0 ||
 	    args->lstio_ses_nmlen > LST_NAME_SIZE)
@@ -96,12 +96,12 @@ lst_session_info_ioctl(lstio_session_info_args_t *args)
 {
 	/* no checking of key */
 
-	if (args->lstio_ses_idp   == NULL || /* address for output sid */
-	    args->lstio_ses_keyp  == NULL || /* address for output key */
+	if (args->lstio_ses_idp    == NULL || /* address for output sid */
+	    args->lstio_ses_keyp   == NULL || /* address for output key */
 	    args->lstio_ses_featp  == NULL || /* address for output features */
 	    args->lstio_ses_ndinfo == NULL || /* address for output ndinfo */
-	    args->lstio_ses_namep == NULL || /* address for output name */
-	    args->lstio_ses_nmlen <= 0 ||
+	    args->lstio_ses_namep  == NULL || /* address for output name */
+	    args->lstio_ses_nmlen  <= 0 ||
 	    args->lstio_ses_nmlen > LST_NAME_SIZE)
 		return -EINVAL;
 
@@ -197,8 +197,8 @@ out:
 static int
 lst_group_add_ioctl(lstio_group_add_args_t *args)
 {
-	char	   *name;
-	int	     rc;
+	char *name;
+	int rc;
 
 	if (args->lstio_grp_key != console_session.ses_key)
 		return -EACCES;
@@ -324,8 +324,8 @@ static int
 lst_nodes_add_ioctl(lstio_group_nodes_args_t *args)
 {
 	unsigned feats;
-	int     rc;
-	char   *name;
+	int rc;
+	char *name;
 
 	if (args->lstio_grp_key != console_session.ses_key)
 		return -EACCES;
@@ -385,10 +385,10 @@ lst_group_list_ioctl(lstio_group_list_args_t *args)
 static int
 lst_group_info_ioctl(lstio_group_info_args_t *args)
 {
-	char	   *name;
-	int	     ndent;
-	int	     index;
-	int	     rc;
+	char *name;
+	int ndent;
+	int index;
+	int rc;
 
 	if (args->lstio_grp_key != console_session.ses_key)
 		return -EACCES;
@@ -449,8 +449,8 @@ lst_group_info_ioctl(lstio_group_info_args_t *args)
 static int
 lst_batch_add_ioctl(lstio_batch_add_args_t *args)
 {
-	int	     rc;
-	char	   *name;
+	int rc;
+	char *name;
 
 	if (args->lstio_bat_key != console_session.ses_key)
 		return -EACCES;
@@ -483,8 +483,8 @@ lst_batch_add_ioctl(lstio_batch_add_args_t *args)
 static int
 lst_batch_run_ioctl(lstio_batch_run_args_t *args)
 {
-	int	     rc;
-	char	   *name;
+	int rc;
+	char *name;
 
 	if (args->lstio_bat_key != console_session.ses_key)
 		return -EACCES;
@@ -518,8 +518,8 @@ lst_batch_run_ioctl(lstio_batch_run_args_t *args)
 static int
 lst_batch_stop_ioctl(lstio_batch_stop_args_t *args)
 {
-	int	     rc;
-	char	   *name;
+	int rc;
+	char *name;
 
 	if (args->lstio_bat_key != console_session.ses_key)
 		return -EACCES;
@@ -613,10 +613,10 @@ lst_batch_list_ioctl(lstio_batch_list_args_t *args)
 static int
 lst_batch_info_ioctl(lstio_batch_info_args_t *args)
 {
-	char	   *name;
-	int	     rc;
-	int	     index;
-	int	     ndent;
+	char *name;
+	int rc;
+	int index;
+	int ndent;
 
 	if (args->lstio_bat_key != console_session.ses_key)
 		return -EACCES;
@@ -678,8 +678,8 @@ lst_batch_info_ioctl(lstio_batch_info_args_t *args)
 static int
 lst_stat_query_ioctl(lstio_stat_args_t *args)
 {
-	int	     rc;
-	char	   *name;
+	int rc;
+	char *name;
 
 	/* TODO: not finished */
 	if (args->lstio_sta_key != console_session.ses_key)
diff --git a/drivers/staging/lustre/lnet/selftest/conrpc.c b/drivers/staging/lustre/lnet/selftest/conrpc.c
index 77f02b76128e..a1a4e08f7391 100644
--- a/drivers/staging/lustre/lnet/selftest/conrpc.c
+++ b/drivers/staging/lustre/lnet/selftest/conrpc.c
@@ -117,8 +117,8 @@ static int
 lstcon_rpc_prep(lstcon_node_t *nd, int service, unsigned feats,
 		int bulk_npg, int bulk_len, lstcon_rpc_t **crpcpp)
 {
-	lstcon_rpc_t  *crpc = NULL;
-	int	    rc;
+	lstcon_rpc_t *crpc = NULL;
+	int rc;
 
 	spin_lock(&console_session.ses_rpc_lock);
 
@@ -151,7 +151,7 @@ void
 lstcon_rpc_put(lstcon_rpc_t *crpc)
 {
 	srpc_bulk_t *bulk = &crpc->crp_rpc->crpc_bulk;
-	int	  i;
+	int i;
 
 	LASSERT(list_empty(&crpc->crp_link));
 
@@ -336,8 +336,8 @@ lstcon_rpc_trans_check(lstcon_rpc_trans_t *trans)
 int
 lstcon_rpc_trans_postwait(lstcon_rpc_trans_t *trans, int timeout)
 {
-	lstcon_rpc_t  *crpc;
-	int	    rc;
+	lstcon_rpc_t *crpc;
+	int rc;
 
 	if (list_empty(&trans->tas_rpcs_list))
 		return 0;
@@ -386,8 +386,8 @@ lstcon_rpc_trans_postwait(lstcon_rpc_trans_t *trans, int timeout)
 static int
 lstcon_rpc_get_reply(lstcon_rpc_t *crpc, srpc_msg_t **msgpp)
 {
-	lstcon_node_t	*nd  = crpc->crp_node;
-	srpc_client_rpc_t    *rpc = crpc->crp_rpc;
+	lstcon_node_t *nd  = crpc->crp_node;
+	srpc_client_rpc_t *rpc = crpc->crp_rpc;
 	srpc_generic_reply_t *rep;
 
 	LASSERT(nd != NULL && rpc != NULL);
@@ -423,9 +423,9 @@ lstcon_rpc_get_reply(lstcon_rpc_t *crpc, srpc_msg_t **msgpp)
 void
 lstcon_rpc_trans_stat(lstcon_rpc_trans_t *trans, lstcon_trans_stat_t *stat)
 {
-	lstcon_rpc_t      *crpc;
-	srpc_msg_t	*rep;
-	int		error;
+	lstcon_rpc_t  *crpc;
+	srpc_msg_t *rep;
+	int error;
 
 	LASSERT(stat != NULL);
 
@@ -470,16 +470,16 @@ lstcon_rpc_trans_interpreter(lstcon_rpc_trans_t *trans,
 			     struct list_head *head_up,
 			     lstcon_rpc_readent_func_t readent)
 {
-	struct list_head	    tmp;
-	struct list_head	   *next;
-	lstcon_rpc_ent_t     *ent;
+	struct list_head tmp;
+	struct list_head *next;
+	lstcon_rpc_ent_t *ent;
 	srpc_generic_reply_t *rep;
-	lstcon_rpc_t	 *crpc;
-	srpc_msg_t	   *msg;
-	lstcon_node_t	*nd;
-	long	dur;
-	struct timeval	tv;
-	int		   error;
+	lstcon_rpc_t *crpc;
+	srpc_msg_t *msg;
+	lstcon_node_t *nd;
+	long dur;
+	struct timeval tv;
+	int error;
 
 	LASSERT(head_up != NULL);
 
@@ -544,9 +544,9 @@ void
 lstcon_rpc_trans_destroy(lstcon_rpc_trans_t *trans)
 {
 	srpc_client_rpc_t *rpc;
-	lstcon_rpc_t      *crpc;
-	lstcon_rpc_t      *tmp;
-	int		count = 0;
+	lstcon_rpc_t *crpc;
+	lstcon_rpc_t *tmp;
+	int count = 0;
 
 	list_for_each_entry_safe(crpc, tmp, &trans->tas_rpcs_list,
 				 crp_link) {
@@ -601,7 +601,7 @@ lstcon_sesrpc_prep(lstcon_node_t *nd, int transop,
 {
 	srpc_mksn_reqst_t *msrq;
 	srpc_rmsn_reqst_t *rsrq;
-	int		rc;
+	int rc;
 
 	switch (transop) {
 	case LST_TRANS_SESNEW:
@@ -638,7 +638,7 @@ int
 lstcon_dbgrpc_prep(lstcon_node_t *nd, unsigned feats, lstcon_rpc_t **crpc)
 {
 	srpc_debug_reqst_t *drq;
-	int		    rc;
+	int rc;
 
 	rc = lstcon_rpc_prep(nd, SRPC_SERVICE_DEBUG, feats, 0, 0, crpc);
 	if (rc != 0)
@@ -707,7 +707,7 @@ static lnet_process_id_packed_t *
 lstcon_next_id(int idx, int nkiov, lnet_kiov_t *kiov)
 {
 	lnet_process_id_packed_t *pid;
-	int		       i;
+	int i;
 
 	i = idx / SFW_ID_PER_PAGE;
 
@@ -723,11 +723,11 @@ lstcon_dstnodes_prep(lstcon_group_t *grp, int idx,
 		     int dist, int span, int nkiov, lnet_kiov_t *kiov)
 {
 	lnet_process_id_packed_t *pid;
-	lstcon_ndlink_t	  *ndl;
-	lstcon_node_t	    *nd;
-	int		       start;
-	int		       end;
-	int		       i = 0;
+	lstcon_ndlink_t *ndl;
+	lstcon_node_t *nd;
+	int start;
+	int end;
+	int i = 0;
 
 	LASSERT(dist >= 1);
 	LASSERT(span >= 1);
@@ -777,8 +777,8 @@ lstcon_pingrpc_prep(lst_test_ping_param_t *param, srpc_test_reqst_t *req)
 {
 	test_ping_req_t *prq = &req->tsr_u.ping;
 
-	prq->png_size   = param->png_size;
-	prq->png_flags  = param->png_flags;
+	prq->png_size  = param->png_size;
+	prq->png_flags = param->png_flags;
 	/* TODO dest */
 	return 0;
 }
@@ -788,9 +788,10 @@ 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;
 
-	brq->blk_opc    = param->blk_opc;
-	brq->blk_npg    = (param->blk_size + PAGE_CACHE_SIZE - 1) / PAGE_CACHE_SIZE;
-	brq->blk_flags  = param->blk_flags;
+	brq->blk_opc   = param->blk_opc;
+	brq->blk_npg   = (param->blk_size + PAGE_CACHE_SIZE - 1) /
+			  PAGE_CACHE_SIZE;
+	brq->blk_flags = param->blk_flags;
 
 	return 0;
 }
@@ -816,7 +817,7 @@ lstcon_testrpc_prep(lstcon_node_t *nd, int transop, unsigned feats,
 	lstcon_group_t    *dgrp = test->tes_dst_grp;
 	srpc_test_reqst_t *trq;
 	srpc_bulk_t       *bulk;
-	int		i;
+	int                i;
 	int		   npg = 0;
 	int		   nob = 0;
 	int		   rc  = 0;
@@ -835,8 +836,10 @@ lstcon_testrpc_prep(lstcon_node_t *nd, int transop, unsigned feats,
 	trq  = &(*crpc)->crp_rpc->crpc_reqstmsg.msg_body.tes_reqst;
 
 	if (transop == LST_TRANS_TSBSRVADD) {
-		int ndist = (sgrp->grp_nnode + test->tes_dist - 1) / test->tes_dist;
-		int nspan = (dgrp->grp_nnode + test->tes_span - 1) / test->tes_span;
+		int ndist = (sgrp->grp_nnode + test->tes_dist - 1) /
+			    test->tes_dist;
+		int nspan = (dgrp->grp_nnode + test->tes_span - 1) /
+			    test->tes_span;
 		int nmax = (ndist + nspan - 1) / nspan;
 
 		trq->tsr_ndest = 0;
@@ -851,7 +854,8 @@ lstcon_testrpc_prep(lstcon_node_t *nd, int transop, unsigned feats,
 			LASSERT(nob > 0);
 
 			len = (feats & LST_FEAT_BULK_LEN) == 0 ?
-			      PAGE_CACHE_SIZE : min_t(int, nob, PAGE_CACHE_SIZE);
+			      PAGE_CACHE_SIZE :
+			      min_t(int, nob, PAGE_CACHE_SIZE);
 			nob -= len;
 
 			bulk->bk_iovs[i].kiov_offset = 0;
@@ -883,8 +887,8 @@ lstcon_testrpc_prep(lstcon_node_t *nd, int transop, unsigned feats,
 		trq->tsr_loop  = test->tes_loop;
 	}
 
-	trq->tsr_sid	= console_session.ses_id;
-	trq->tsr_bid	= test->tes_hdr.tsb_id;
+	trq->tsr_sid        = console_session.ses_id;
+	trq->tsr_bid        = test->tes_hdr.tsb_id;
 	trq->tsr_concur     = test->tes_concur;
 	trq->tsr_is_client  = (transop == LST_TRANS_TSBCLIADD) ? 1 : 0;
 	trq->tsr_stop_onerr = !!test->tes_stop_onerr;
@@ -966,7 +970,7 @@ lstcon_rpc_stat_reply(lstcon_rpc_trans_t *trans, srpc_msg_t *msg,
 	srpc_batch_reply_t *bat_rep;
 	srpc_test_reply_t  *test_rep;
 	srpc_stat_reply_t  *stat_rep;
-	int		 rc = 0;
+	int                rc = 0;
 
 	switch (trans->tas_opc) {
 	case LST_TRANS_SESNEW:
@@ -1084,11 +1088,11 @@ lstcon_rpc_trans_ndlist(struct list_head *ndlist,
 			lstcon_rpc_trans_t **transpp)
 {
 	lstcon_rpc_trans_t *trans;
-	lstcon_ndlink_t    *ndl;
-	lstcon_node_t      *nd;
-	lstcon_rpc_t       *rpc;
-	unsigned	    feats;
-	int		 rc;
+	lstcon_ndlink_t *ndl;
+	lstcon_node_t *nd;
+	lstcon_rpc_t *rpc;
+	unsigned feats;
+	int rc;
 
 	/* Creating session RPG for list of nodes */
 
@@ -1165,16 +1169,16 @@ lstcon_rpc_trans_ndlist(struct list_head *ndlist,
 static void
 lstcon_rpc_pinger(void *arg)
 {
-	stt_timer_t	*ptimer = (stt_timer_t *)arg;
+	stt_timer_t *ptimer = (stt_timer_t *)arg;
 	lstcon_rpc_trans_t *trans;
-	lstcon_rpc_t       *crpc;
-	srpc_msg_t	 *rep;
+	lstcon_rpc_t *crpc;
+	srpc_msg_t *rep;
 	srpc_debug_reqst_t *drq;
-	lstcon_ndlink_t    *ndl;
-	lstcon_node_t      *nd;
-	time_t	      intv;
-	int		 count = 0;
-	int		 rc;
+	lstcon_ndlink_t *ndl;
+	lstcon_node_t *nd;
+	time_t intv;
+	int count = 0;
+	int rc;
 
 	/* RPC pinger is a special case of transaction,
 	 * it's called by timer at 8 seconds interval.
@@ -1283,8 +1287,8 @@ lstcon_rpc_pinger(void *arg)
 int
 lstcon_rpc_pinger_start(void)
 {
-	stt_timer_t    *ptimer;
-	int	     rc;
+	stt_timer_t *ptimer;
+	int rc;
 
 	LASSERT(list_empty(&console_session.ses_rpc_freelist));
 	LASSERT(atomic_read(&console_session.ses_rpc_counter) == 0);
@@ -1324,9 +1328,9 @@ void
 lstcon_rpc_cleanup_wait(void)
 {
 	lstcon_rpc_trans_t *trans;
-	lstcon_rpc_t       *crpc;
-	struct list_head	 *pacer;
-	struct list_head	  zlist;
+	lstcon_rpc_t *crpc;
+	struct list_head *pacer;
+	struct list_head zlist;
 
 	/* Called with hold of global mutex */
 
diff --git a/drivers/staging/lustre/lnet/selftest/conrpc.h b/drivers/staging/lustre/lnet/selftest/conrpc.h
index 2353889c6eac..7d33cf9e9d99 100644
--- a/drivers/staging/lustre/lnet/selftest/conrpc.h
+++ b/drivers/staging/lustre/lnet/selftest/conrpc.h
@@ -64,31 +64,29 @@ struct lstcon_test;
 struct lstcon_node;
 
 typedef struct lstcon_rpc {
-	struct list_head	       crp_link;       /* chain on rpc transaction */
+	struct list_head        crp_link;       /* chain on rpc transaction */
 	srpc_client_rpc_t       *crp_rpc;	/* client rpc */
-	struct lstcon_node      *crp_node;       /* destination node */
+	struct lstcon_node      *crp_node;      /* destination node */
 	struct lstcon_rpc_trans *crp_trans;     /* conrpc transaction */
 
-	unsigned int		 crp_posted:1;   /* rpc is posted */
-	unsigned int		 crp_finished:1; /* rpc is finished */
-	unsigned int		 crp_unpacked:1; /* reply is unpacked */
+	unsigned int            crp_posted:1;   /* rpc is posted */
+	unsigned int            crp_finished:1; /* rpc is finished */
+	unsigned int            crp_unpacked:1; /* reply is unpacked */
 	/** RPC is embedded in other structure and can't free it */
-	unsigned int		 crp_embedded:1;
-	int		      crp_status;     /* console rpc errors */
-	unsigned long	       crp_stamp;      /* replied time stamp */
+	unsigned int            crp_embedded:1;
+	int                     crp_status;     /* console rpc errors */
+	unsigned long           crp_stamp;      /* replied time stamp */
 } lstcon_rpc_t;
 
 typedef struct lstcon_rpc_trans {
-	struct list_head	    tas_olink;     /* link chain on owner list */
-	struct list_head	    tas_link;      /* link chain on global list */
-	int		   tas_opc;       /* operation code of transaction */
-	/* features mask is uptodate */
-	unsigned	      tas_feats_updated;
-	/* test features mask */
-	unsigned	      tas_features;
-	wait_queue_head_t	   tas_waitq;     /* wait queue head */
-	atomic_t	  tas_remaining; /* # of un-scheduled rpcs */
-	struct list_head	    tas_rpcs_list; /* queued requests */
+	struct list_head  tas_olink;         /* link chain on owner list */
+	struct list_head  tas_link;          /* link chain on global list */
+	int               tas_opc;           /* operation code of transaction */
+	unsigned          tas_feats_updated; /* features mask is uptodate */
+	unsigned          tas_features;      /* test features mask */
+	wait_queue_head_t tas_waitq;         /* wait queue head */
+	atomic_t          tas_remaining;     /* # of un-scheduled rpcs */
+	struct list_head  tas_rpcs_list;     /* queued requests */
 } lstcon_rpc_trans_t;
 
 #define LST_TRANS_PRIVATE       0x1000
diff --git a/drivers/staging/lustre/lnet/selftest/console.c b/drivers/staging/lustre/lnet/selftest/console.c
index 2b5f53c7a730..f47c8f27f975 100644
--- a/drivers/staging/lustre/lnet/selftest/console.c
+++ b/drivers/staging/lustre/lnet/selftest/console.c
@@ -59,7 +59,7 @@ do {							\
 	(p)->nle_nnode++;				\
 } while (0)
 
-lstcon_session_t	console_session;
+lstcon_session_t console_session;
 
 static void
 lstcon_node_get(lstcon_node_t *nd)
@@ -73,7 +73,7 @@ static int
 lstcon_node_find(lnet_process_id_t id, lstcon_node_t **ndpp, int create)
 {
 	lstcon_ndlink_t *ndl;
-	unsigned int     idx = LNET_NIDADDR(id.nid) % LST_GLOBAL_HASHSIZE;
+	unsigned int idx = LNET_NIDADDR(id.nid) % LST_GLOBAL_HASHSIZE;
 
 	LASSERT(id.nid != LNET_NID_ANY);
 
@@ -117,7 +117,7 @@ lstcon_node_find(lnet_process_id_t id, lstcon_node_t **ndpp, int create)
 static void
 lstcon_node_put(lstcon_node_t *nd)
 {
-	lstcon_ndlink_t  *ndl;
+	lstcon_ndlink_t *ndl;
 
 	LASSERT(nd->nd_ref > 0);
 
@@ -140,10 +140,10 @@ static int
 lstcon_ndlink_find(struct list_head *hash,
 		   lnet_process_id_t id, lstcon_ndlink_t **ndlpp, int create)
 {
-	unsigned int     idx = LNET_NIDADDR(id.nid) % LST_NODE_HASHSIZE;
+	unsigned int idx = LNET_NIDADDR(id.nid) % LST_NODE_HASHSIZE;
 	lstcon_ndlink_t *ndl;
-	lstcon_node_t   *nd;
-	int	      rc;
+	lstcon_node_t *nd;
+	int rc;
 
 	if (id.nid == LNET_NID_ANY)
 		return -EINVAL;
@@ -197,7 +197,7 @@ static int
 lstcon_group_alloc(char *name, lstcon_group_t **grpp)
 {
 	lstcon_group_t *grp;
-	int	     i;
+	int i;
 
 	LIBCFS_ALLOC(grp, offsetof(lstcon_group_t,
 				   grp_ndl_hash[LST_NODE_HASHSIZE]));
@@ -243,7 +243,7 @@ lstcon_group_drain(lstcon_group_t *grp, int keep)
 static void
 lstcon_group_decref(lstcon_group_t *grp)
 {
-	int     i;
+	int i;
 
 	if (--grp->grp_ref > 0)
 		return;
@@ -264,7 +264,7 @@ lstcon_group_decref(lstcon_group_t *grp)
 static int
 lstcon_group_find(const char *name, lstcon_group_t **grpp)
 {
-	lstcon_group_t   *grp;
+	lstcon_group_t *grp;
 
 	list_for_each_entry(grp, &console_session.ses_grp_list, grp_link) {
 		if (strncmp(grp->grp_name, name, LST_NAME_SIZE) != 0)
@@ -288,7 +288,7 @@ static int
 lstcon_group_ndlink_find(lstcon_group_t *grp, lnet_process_id_t id,
 			 lstcon_ndlink_t **ndlpp, int create)
 {
-	int     rc;
+	int rc;
 
 	rc = lstcon_ndlink_find(&grp->grp_ndl_hash[0], id, ndlpp, create);
 	if (rc != 0)
@@ -404,12 +404,12 @@ lstcon_group_nodes_add(lstcon_group_t *grp,
 		       int count, lnet_process_id_t *ids_up,
 		       unsigned *featp, struct list_head *result_up)
 {
-	lstcon_rpc_trans_t      *trans;
-	lstcon_ndlink_t	 *ndl;
-	lstcon_group_t	  *tmp;
-	lnet_process_id_t	id;
-	int		      i;
-	int		      rc;
+	lstcon_rpc_trans_t *trans;
+	lstcon_ndlink_t *ndl;
+	lstcon_group_t *tmp;
+	lnet_process_id_t id;
+	int i;
+	int rc;
 
 	rc = lstcon_group_alloc(NULL, &tmp);
 	if (rc != 0) {
@@ -471,12 +471,12 @@ lstcon_group_nodes_remove(lstcon_group_t *grp,
 			  int count, lnet_process_id_t *ids_up,
 			  struct list_head *result_up)
 {
-	lstcon_rpc_trans_t     *trans;
-	lstcon_ndlink_t	*ndl;
-	lstcon_group_t	 *tmp;
-	lnet_process_id_t       id;
-	int		     rc;
-	int		     i;
+	lstcon_rpc_trans_t *trans;
+	lstcon_ndlink_t *ndl;
+	lstcon_group_t *tmp;
+	lnet_process_id_t id;
+	int rc;
+	int i;
 
 	/* End session and remove node from the group */
 
@@ -525,7 +525,7 @@ int
 lstcon_group_add(char *name)
 {
 	lstcon_group_t *grp;
-	int	     rc;
+	int rc;
 
 	rc = (lstcon_group_find(name, &grp) == 0)? -EEXIST: 0;
 	if (rc != 0) {
@@ -549,8 +549,8 @@ int
 lstcon_nodes_add(char *name, int count, lnet_process_id_t *ids_up,
 		 unsigned *featp, struct list_head *result_up)
 {
-	lstcon_group_t	 *grp;
-	int		     rc;
+	lstcon_group_t *grp;
+	int rc;
 
 	LASSERT(count > 0);
 	LASSERT(ids_up != NULL);
@@ -580,8 +580,8 @@ int
 lstcon_group_del(char *name)
 {
 	lstcon_rpc_trans_t *trans;
-	lstcon_group_t     *grp;
-	int		 rc;
+	lstcon_group_t *grp;
+	int rc;
 
 	rc = lstcon_group_find(name, &grp);
 	if (rc != 0) {
@@ -621,7 +621,7 @@ int
 lstcon_group_clean(char *name, int args)
 {
 	lstcon_group_t *grp = NULL;
-	int	     rc;
+	int rc;
 
 	rc = lstcon_group_find(name, &grp);
 	if (rc != 0) {
@@ -654,7 +654,7 @@ lstcon_nodes_remove(char *name, int count,
 		    lnet_process_id_t *ids_up, struct list_head *result_up)
 {
 	lstcon_group_t *grp = NULL;
-	int	     rc;
+	int rc;
 
 	rc = lstcon_group_find(name, &grp);
 	if (rc != 0) {
@@ -682,9 +682,9 @@ lstcon_nodes_remove(char *name, int count,
 int
 lstcon_group_refresh(char *name, struct list_head *result_up)
 {
-	lstcon_rpc_trans_t      *trans;
-	lstcon_group_t	  *grp;
-	int		      rc;
+	lstcon_rpc_trans_t *trans;
+	lstcon_group_t *grp;
+	int rc;
 
 	rc = lstcon_group_find(name, &grp);
 	if (rc != 0) {
@@ -743,10 +743,10 @@ static int
 lstcon_nodes_getent(struct list_head *head, int *index_p,
 		    int *count_p, lstcon_node_ent_t *dents_up)
 {
-	lstcon_ndlink_t  *ndl;
-	lstcon_node_t    *nd;
-	int	       count = 0;
-	int	       index = 0;
+	lstcon_ndlink_t *ndl;
+	lstcon_node_t *nd;
+	int count = 0;
+	int index = 0;
 
 	LASSERT(index_p != NULL && count_p != NULL);
 	LASSERT(dents_up != NULL);
@@ -784,9 +784,9 @@ lstcon_group_info(char *name, lstcon_ndlist_ent_t *gents_p,
 		  int *index_p, int *count_p, lstcon_node_ent_t *dents_up)
 {
 	lstcon_ndlist_ent_t *gentp;
-	lstcon_group_t      *grp;
-	lstcon_ndlink_t     *ndl;
-	int		  rc;
+	lstcon_group_t *grp;
+	lstcon_ndlink_t *ndl;
+	int rc;
 
 	rc = lstcon_group_find(name, &grp);
 	if (rc != 0) {
@@ -828,7 +828,7 @@ lstcon_group_info(char *name, lstcon_ndlist_ent_t *gents_p,
 static int
 lstcon_batch_find(const char *name, lstcon_batch_t **batpp)
 {
-	lstcon_batch_t   *bat;
+	lstcon_batch_t *bat;
 
 	list_for_each_entry(bat, &console_session.ses_bat_list, bat_link) {
 		if (strncmp(bat->bat_name, name, LST_NAME_SIZE) == 0) {
@@ -843,9 +843,9 @@ lstcon_batch_find(const char *name, lstcon_batch_t **batpp)
 int
 lstcon_batch_add(char *name)
 {
-	lstcon_batch_t   *bat;
-	int	       i;
-	int	       rc;
+	lstcon_batch_t *bat;
+	int i;
+	int rc;
 
 	rc = (lstcon_batch_find(name, &bat) == 0)? -EEXIST: 0;
 	if (rc != 0) {
@@ -903,7 +903,7 @@ lstcon_batch_add(char *name)
 int
 lstcon_batch_list(int index, int len, char *name_up)
 {
-	lstcon_batch_t    *bat;
+	lstcon_batch_t *bat;
 
 	LASSERT(name_up != NULL);
 	LASSERT(index >= 0);
@@ -924,12 +924,12 @@ lstcon_batch_info(char *name, lstcon_test_batch_ent_t *ent_up, int server,
 		  lstcon_node_ent_t *dents_up)
 {
 	lstcon_test_batch_ent_t *entp;
-	struct list_head	      *clilst;
-	struct list_head	      *srvlst;
-	lstcon_test_t	   *test = NULL;
-	lstcon_batch_t	  *bat;
-	lstcon_ndlink_t	 *ndl;
-	int		      rc;
+	struct list_head *clilst;
+	struct list_head *srvlst;
+	lstcon_test_t *test = NULL;
+	lstcon_batch_t *bat;
+	lstcon_ndlink_t *ndl;
+	int rc;
 
 	rc = lstcon_batch_find(name, &bat);
 	if (rc != 0) {
@@ -1018,7 +1018,7 @@ lstcon_batch_op(lstcon_batch_t *bat, int transop,
 		struct list_head *result_up)
 {
 	lstcon_rpc_trans_t *trans;
-	int		 rc;
+	int rc;
 
 	rc = lstcon_rpc_trans_ndlist(&bat->bat_cli_list,
 				     &bat->bat_trans_list, transop,
@@ -1041,7 +1041,7 @@ int
 lstcon_batch_run(char *name, int timeout, struct list_head *result_up)
 {
 	lstcon_batch_t *bat;
-	int	     rc;
+	int rc;
 
 	if (lstcon_batch_find(name, &bat) != 0) {
 		CDEBUG(D_NET, "Can't find batch %s\n", name);
@@ -1063,7 +1063,7 @@ int
 lstcon_batch_stop(char *name, int force, struct list_head *result_up)
 {
 	lstcon_batch_t *bat;
-	int	     rc;
+	int rc;
 
 	if (lstcon_batch_find(name, &bat) != 0) {
 		CDEBUG(D_NET, "Can't find batch %s\n", name);
@@ -1084,9 +1084,9 @@ lstcon_batch_stop(char *name, int force, struct list_head *result_up)
 static void
 lstcon_batch_destroy(lstcon_batch_t *bat)
 {
-	lstcon_ndlink_t    *ndl;
-	lstcon_test_t      *test;
-	int		 i;
+	lstcon_ndlink_t *ndl;
+	lstcon_test_t *test;
+	int i;
 
 	list_del(&bat->bat_link);
 
@@ -1137,11 +1137,11 @@ lstcon_batch_destroy(lstcon_batch_t *bat)
 static int
 lstcon_testrpc_condition(int transop, lstcon_node_t *nd, void *arg)
 {
-	lstcon_test_t    *test;
-	lstcon_batch_t   *batch;
-	lstcon_ndlink_t  *ndl;
-	struct list_head       *hash;
-	struct list_head       *head;
+	lstcon_test_t *test;
+	lstcon_batch_t *batch;
+	lstcon_ndlink_t *ndl;
+	struct list_head *hash;
+	struct list_head *head;
 
 	test = (lstcon_test_t *)arg;
 	LASSERT(test != NULL);
@@ -1181,10 +1181,10 @@ lstcon_testrpc_condition(int transop, lstcon_node_t *nd, void *arg)
 static int
 lstcon_test_nodes_add(lstcon_test_t *test, struct list_head *result_up)
 {
-	lstcon_rpc_trans_t     *trans;
-	lstcon_group_t	 *grp;
-	int		     transop;
-	int		     rc;
+	lstcon_rpc_trans_t *trans;
+	lstcon_group_t *grp;
+	int transop;
+	int rc;
 
 	LASSERT(test->tes_src_grp != NULL);
 	LASSERT(test->tes_dst_grp != NULL);
@@ -1251,8 +1251,8 @@ lstcon_verify_batch(const char *name, lstcon_batch_t **batch)
 static int
 lstcon_verify_group(const char *name, lstcon_group_t **grp)
 {
-	int			rc;
-	lstcon_ndlink_t		*ndl;
+	int rc;
+	lstcon_ndlink_t *ndl;
 
 	rc = lstcon_group_find(name, grp);
 	if (rc != 0) {
@@ -1398,13 +1398,13 @@ lstcon_test_batch_query(char *name, int testidx, int client,
 			int timeout, struct list_head *result_up)
 {
 	lstcon_rpc_trans_t *trans;
-	struct list_head	 *translist;
-	struct list_head	 *ndlist;
-	lstcon_tsb_hdr_t   *hdr;
-	lstcon_batch_t     *batch;
-	lstcon_test_t      *test = NULL;
-	int		 transop;
-	int		 rc;
+	struct list_head *translist;
+	struct list_head *ndlist;
+	lstcon_tsb_hdr_t *hdr;
+	lstcon_batch_t *batch;
+	lstcon_test_t *test = NULL;
+	int transop;
+	int rc;
 
 	rc = lstcon_batch_find(name, &batch);
 	if (rc != 0) {
@@ -1460,9 +1460,9 @@ lstcon_statrpc_readent(int transop, srpc_msg_t *msg,
 		       lstcon_rpc_ent_t *ent_up)
 {
 	srpc_stat_reply_t *rep = &msg->msg_body.stat_reply;
-	sfw_counters_t    *sfwk_stat;
-	srpc_counters_t   *srpc_stat;
-	lnet_counters_t   *lnet_stat;
+	sfw_counters_t *sfwk_stat;
+	srpc_counters_t *srpc_stat;
+	lnet_counters_t *lnet_stat;
 
 	if (rep->str_status != 0)
 		return 0;
@@ -1483,9 +1483,9 @@ static int
 lstcon_ndlist_stat(struct list_head *ndlist,
 		   int timeout, struct list_head *result_up)
 {
-	struct list_head	  head;
+	struct list_head head;
 	lstcon_rpc_trans_t *trans;
-	int		 rc;
+	int rc;
 
 	INIT_LIST_HEAD(&head);
 
@@ -1508,8 +1508,8 @@ lstcon_ndlist_stat(struct list_head *ndlist,
 int
 lstcon_group_stat(char *grp_name, int timeout, struct list_head *result_up)
 {
-	lstcon_group_t     *grp;
-	int		 rc;
+	lstcon_group_t *grp;
+	int rc;
 
 	rc = lstcon_group_find(grp_name, &grp);
 	if (rc != 0) {
@@ -1528,11 +1528,11 @@ int
 lstcon_nodes_stat(int count, lnet_process_id_t *ids_up,
 		  int timeout, struct list_head *result_up)
 {
-	lstcon_ndlink_t	 *ndl;
-	lstcon_group_t	  *tmp;
-	lnet_process_id_t	id;
-	int		      i;
-	int		      rc;
+	lstcon_ndlink_t *ndl;
+	lstcon_group_t *tmp;
+	lnet_process_id_t id;
+	int i;
+	int rc;
 
 	rc = lstcon_group_alloc(NULL, &tmp);
 	if (rc != 0) {
@@ -1604,7 +1604,7 @@ lstcon_batch_debug(int timeout, char *name,
 		   int client, struct list_head *result_up)
 {
 	lstcon_batch_t *bat;
-	int	     rc;
+	int rc;
 
 	rc = lstcon_batch_find(name, &bat);
 	if (rc != 0)
@@ -1622,7 +1622,7 @@ lstcon_group_debug(int timeout, char *name,
 		   struct list_head *result_up)
 {
 	lstcon_group_t *grp;
-	int	     rc;
+	int rc;
 
 	rc = lstcon_group_find(name, &grp);
 	if (rc != 0)
@@ -1640,11 +1640,11 @@ lstcon_nodes_debug(int timeout,
 		   int count, lnet_process_id_t *ids_up,
 		   struct list_head *result_up)
 {
-	lnet_process_id_t  id;
-	lstcon_ndlink_t   *ndl;
-	lstcon_group_t    *grp;
-	int		i;
-	int		rc;
+	lnet_process_id_t id;
+	lstcon_ndlink_t *ndl;
+	lstcon_group_t *grp;
+	int i;
+	int rc;
 
 	rc = lstcon_group_alloc(NULL, &grp);
 	if (rc != 0) {
@@ -1689,7 +1689,7 @@ lstcon_session_match(lst_sid_t sid)
 static void
 lstcon_new_session_id(lst_sid_t *sid)
 {
-	lnet_process_id_t      id;
+	lnet_process_id_t id;
 
 	LASSERT(console_session.ses_state == LST_SESSION_NONE);
 
@@ -1704,8 +1704,8 @@ int
 lstcon_session_new(char *name, int key, unsigned feats,
 		   int timeout, int force, lst_sid_t *sid_up)
 {
-	int     rc = 0;
-	int     i;
+	int rc = 0;
+	int i;
 
 	if (console_session.ses_state != LST_SESSION_NONE) {
 		/* session exists */
@@ -1733,9 +1733,9 @@ lstcon_session_new(char *name, int key, unsigned feats,
 
 	lstcon_new_session_id(&console_session.ses_id);
 
-	console_session.ses_key	    = key;
-	console_session.ses_state   = LST_SESSION_ACTIVE;
-	console_session.ses_force   = !!force;
+	console_session.ses_key = key;
+	console_session.ses_state = LST_SESSION_ACTIVE;
+	console_session.ses_force = !!force;
 	console_session.ses_features = feats;
 	console_session.ses_feats_updated = 0;
 	console_session.ses_timeout = (timeout <= 0) ?
@@ -1770,8 +1770,8 @@ lstcon_session_info(lst_sid_t *sid_up, int *key_up, unsigned *featp,
 		    lstcon_ndlist_ent_t *ndinfo_up, char *name_up, int len)
 {
 	lstcon_ndlist_ent_t *entp;
-	lstcon_ndlink_t     *ndl;
-	int		  rc = 0;
+	lstcon_ndlink_t *ndl;
+	int rc = 0;
 
 	if (console_session.ses_state != LST_SESSION_ACTIVE)
 		return -ESRCH;
@@ -1802,9 +1802,9 @@ int
 lstcon_session_end(void)
 {
 	lstcon_rpc_trans_t *trans;
-	lstcon_group_t     *grp;
-	lstcon_batch_t     *bat;
-	int		 rc = 0;
+	lstcon_group_t *grp;
+	lstcon_batch_t *bat;
+	int rc = 0;
 
 	LASSERT(console_session.ses_state == LST_SESSION_ACTIVE);
 
@@ -1894,13 +1894,13 @@ lstcon_session_feats_check(unsigned feats)
 static int
 lstcon_acceptor_handle(srpc_server_rpc_t *rpc)
 {
-	srpc_msg_t	*rep  = &rpc->srpc_replymsg;
-	srpc_msg_t	*req  = &rpc->srpc_reqstbuf->buf_msg;
+	srpc_msg_t *rep  = &rpc->srpc_replymsg;
+	srpc_msg_t *req  = &rpc->srpc_reqstbuf->buf_msg;
 	srpc_join_reqst_t *jreq = &req->msg_body.join_reqst;
 	srpc_join_reply_t *jrep = &rep->msg_body.join_reply;
-	lstcon_group_t    *grp  = NULL;
-	lstcon_ndlink_t   *ndl;
-	int		rc   = 0;
+	lstcon_group_t *grp  = NULL;
+	lstcon_ndlink_t *ndl;
+	int rc   = 0;
 
 	sfw_unpack_message(req);
 
@@ -1978,9 +1978,9 @@ srpc_service_t lstcon_acceptor_service;
 static void lstcon_init_acceptor_service(void)
 {
 	/* initialize selftest console acceptor service table */
-	lstcon_acceptor_service.sv_name    = "join session";
-	lstcon_acceptor_service.sv_handler = lstcon_acceptor_handle;
-	lstcon_acceptor_service.sv_id      = SRPC_SERVICE_JOIN;
+	lstcon_acceptor_service.sv_name     = "join session";
+	lstcon_acceptor_service.sv_handler  = lstcon_acceptor_handle;
+	lstcon_acceptor_service.sv_id       = SRPC_SERVICE_JOIN;
 	lstcon_acceptor_service.sv_wi_total = SFW_FRWK_WI_MAX;
 }
 
@@ -1992,19 +1992,19 @@ static DECLARE_IOCTL_HANDLER(lstcon_ioctl_handler, lstcon_ioctl_entry);
 int
 lstcon_console_init(void)
 {
-	int     i;
-	int     rc;
+	int i;
+	int rc;
 
 	memset(&console_session, 0, sizeof(lstcon_session_t));
 
-	console_session.ses_id		    = LST_INVALID_SID;
-	console_session.ses_state	    = LST_SESSION_NONE;
-	console_session.ses_timeout	    = 0;
-	console_session.ses_force	    = 0;
-	console_session.ses_expired	    = 0;
-	console_session.ses_feats_updated   = 0;
-	console_session.ses_features	    = LST_FEATS_MASK;
-	console_session.ses_laststamp	    = get_seconds();
+	console_session.ses_id		  = LST_INVALID_SID;
+	console_session.ses_state	  = LST_SESSION_NONE;
+	console_session.ses_timeout	  = 0;
+	console_session.ses_force	  = 0;
+	console_session.ses_expired	  = 0;
+	console_session.ses_feats_updated = 0;
+	console_session.ses_features	  = LST_FEATS_MASK;
+	console_session.ses_laststamp	  = get_seconds();
 
 	mutex_init(&console_session.ses_mutex);
 
@@ -2062,7 +2062,7 @@ out:
 int
 lstcon_console_fini(void)
 {
-	int     i;
+	int i;
 
 	libcfs_deregister_ioctl(&lstcon_ioctl_handler);
 
diff --git a/drivers/staging/lustre/lnet/selftest/console.h b/drivers/staging/lustre/lnet/selftest/console.h
index e41ca89f10ba..c4cf0aed80e1 100644
--- a/drivers/staging/lustre/lnet/selftest/console.h
+++ b/drivers/staging/lustre/lnet/selftest/console.h
@@ -52,119 +52,121 @@
 #include "conrpc.h"
 
 typedef struct lstcon_node {
-	lnet_process_id_t    nd_id;	  /* id of the node */
-	int		  nd_ref;	 /* reference count */
-	int		  nd_state;       /* state of the node */
-	int		  nd_timeout;     /* session timeout */
-	unsigned long	   nd_stamp;       /* timestamp of last replied RPC */
-	struct lstcon_rpc    nd_ping;	/* ping rpc */
-} lstcon_node_t;				/*** node descriptor */
+	lnet_process_id_t nd_id;      /* id of the node */
+	int               nd_ref;     /* reference count */
+	int               nd_state;   /* state of the node */
+	int               nd_timeout; /* session timeout */
+	unsigned long     nd_stamp;   /* timestamp of last replied RPC */
+	struct lstcon_rpc nd_ping;    /* ping rpc */
+} lstcon_node_t; /* node descriptor */
 
 typedef struct {
-	struct list_head	   ndl_link;       /* chain on list */
-	struct list_head	   ndl_hlink;      /* chain on hash */
-	lstcon_node_t       *ndl_node;       /* pointer to node */
-} lstcon_ndlink_t;			      /*** node link descriptor */
+	struct list_head ndl_link;    /* chain on list */
+	struct list_head ndl_hlink;   /* chain on hash */
+	lstcon_node_t    *ndl_node;   /* pointer to node */
+} lstcon_ndlink_t; /* node link descriptor */
 
 typedef struct {
-	struct list_head	   grp_link;       /* chain on global group list */
-	int		  grp_ref;	/* reference count */
-	int		  grp_userland;   /* has userland nodes */
-	int		  grp_nnode;      /* # of nodes */
-	char		 grp_name[LST_NAME_SIZE]; /* group name */
+	struct list_head grp_link;                /* chain on global group list
+						   */
+	int              grp_ref;                 /* reference count */
+	int              grp_userland;            /* has userland nodes */
+	int              grp_nnode;               /* # of nodes */
+	char             grp_name[LST_NAME_SIZE]; /* group name */
 
-	struct list_head	   grp_trans_list; /* transaction list */
-	struct list_head	   grp_ndl_list;   /* nodes list */
-	struct list_head	   grp_ndl_hash[0];/* hash table for nodes */
-} lstcon_group_t;		    /*** (alias of nodes) group descriptor */
+	struct list_head grp_trans_list;          /* transaction list */
+	struct list_head grp_ndl_list;            /* nodes list */
+	struct list_head grp_ndl_hash[0];         /* hash table for nodes */
+} lstcon_group_t; /* (alias of nodes) group descriptor */
 
-#define LST_BATCH_IDLE	  0xB0	    /* idle batch */
-#define LST_BATCH_RUNNING       0xB1	    /* running batch */
+#define LST_BATCH_IDLE    0xB0	    /* idle batch */
+#define LST_BATCH_RUNNING 0xB1	    /* running batch */
 
 typedef struct lstcon_tsb_hdr {
-	lst_bid_t	       tsb_id;	 /* batch ID */
-	int		     tsb_index;      /* test index */
+	lst_bid_t        tsb_id;	 /* batch ID */
+	int              tsb_index;      /* test index */
 } lstcon_tsb_hdr_t;
 
 typedef struct {
-	lstcon_tsb_hdr_t	bat_hdr;	/* test_batch header */
-	struct list_head	      bat_link;       /* chain on session's batches list */
-	int		     bat_ntest;      /* # of test */
-	int		     bat_state;      /* state of the batch */
-	int		     bat_arg;	/* parameter for run|stop, timeout for run, force for stop */
-	char		    bat_name[LST_NAME_SIZE]; /* name of batch */
-
-	struct list_head	      bat_test_list;  /* list head of tests (lstcon_test_t) */
-	struct list_head	      bat_trans_list; /* list head of transaction */
-	struct list_head	      bat_cli_list;   /* list head of client nodes (lstcon_node_t) */
-	struct list_head	     *bat_cli_hash;   /* hash table of client nodes */
-	struct list_head	      bat_srv_list;   /* list head of server nodes */
-	struct list_head	     *bat_srv_hash;   /* hash table of server nodes */
-} lstcon_batch_t;			     /*** (tests ) batch descriptor */
+	lstcon_tsb_hdr_t bat_hdr;         /* test_batch header */
+	struct list_head bat_link;        /* chain on session's batches list */
+	int              bat_ntest;       /* # of test */
+	int              bat_state;       /* state of the batch */
+	int              bat_arg;         /* parameter for run|stop, timeout
+					   * for run, force for stop */
+	char             bat_name[LST_NAME_SIZE];/* name of batch */
+
+	struct list_head bat_test_list;   /* list head of tests (lstcon_test_t)
+					   */
+	struct list_head bat_trans_list;  /* list head of transaction */
+	struct list_head bat_cli_list;    /* list head of client nodes
+					   * (lstcon_node_t) */
+	struct list_head *bat_cli_hash;   /* hash table of client nodes */
+	struct list_head bat_srv_list;    /* list head of server nodes */
+	struct list_head *bat_srv_hash;   /* hash table of server nodes */
+} lstcon_batch_t; /* (tests ) batch descriptor */
 
 typedef struct lstcon_test {
-	lstcon_tsb_hdr_t      tes_hdr;	/* test batch header */
-	struct list_head	    tes_link;       /* chain on batch's tests list */
-	lstcon_batch_t       *tes_batch;      /* pointer to batch */
+	lstcon_tsb_hdr_t tes_hdr;        /* test batch header */
+	struct list_head tes_link;       /* chain on batch's tests list */
+	lstcon_batch_t   *tes_batch;     /* pointer to batch */
 
-	int		   tes_type;       /* type of the test, i.e: bulk, ping */
-	int		   tes_stop_onerr; /* stop on error */
-	int		   tes_oneside;    /* one-sided test */
-	int		   tes_concur;     /* concurrency */
-	int		   tes_loop;       /* loop count */
-	int		   tes_dist;       /* nodes distribution of target group */
-	int		   tes_span;       /* nodes span of target group */
-	int		   tes_cliidx;     /* client index, used for RPC creating */
+	int              tes_type;       /* type of the test, i.e: bulk, ping */
+	int              tes_stop_onerr; /* stop on error */
+	int              tes_oneside;    /* one-sided test */
+	int              tes_concur;     /* concurrency */
+	int              tes_loop;       /* loop count */
+	int              tes_dist;       /* nodes distribution of target group */
+	int              tes_span;       /* nodes span of target group */
+	int              tes_cliidx;     /* client index, used for RPC creating */
 
-	struct list_head  tes_trans_list; /* transaction list */
-	lstcon_group_t       *tes_src_grp;    /* group run the test */
-	lstcon_group_t       *tes_dst_grp;    /* target group */
+	struct list_head tes_trans_list; /* transaction list */
+	lstcon_group_t   *tes_src_grp;   /* group run the test */
+	lstcon_group_t   *tes_dst_grp;   /* target group */
 
-	int		   tes_paramlen;   /* test parameter length */
-	char		  tes_param[0];   /* test parameter */
-} lstcon_test_t;				/*** a single test descriptor */
+	int              tes_paramlen;   /* test parameter length */
+	char             tes_param[0];   /* test parameter */
+} lstcon_test_t; /* a single test descriptor */
 
-#define LST_GLOBAL_HASHSIZE     503	     /* global nodes hash table size */
-#define LST_NODE_HASHSIZE       239	     /* node hash table (for batch or group) */
+#define LST_GLOBAL_HASHSIZE 503	     /* global nodes hash table size */
+#define LST_NODE_HASHSIZE   239	     /* node hash table (for batch or group) */
 
-#define LST_SESSION_NONE	0x0	     /* no session */
-#define LST_SESSION_ACTIVE      0x1	     /* working session */
+#define LST_SESSION_NONE    0x0	     /* no session */
+#define LST_SESSION_ACTIVE  0x1	     /* working session */
 
-#define LST_CONSOLE_TIMEOUT     300	     /* default console timeout */
+#define LST_CONSOLE_TIMEOUT 300	     /* default console timeout */
 
 typedef struct {
-	struct mutex		ses_mutex;      /* only 1 thread in session */
-	lst_sid_t	       ses_id;	 /* global session id */
-	int		     ses_key;	/* local session key */
-	int		     ses_state;      /* state of session */
-	int		     ses_timeout;    /* timeout in seconds */
-	time_t		  ses_laststamp;  /* last operation stamp (seconds) */
-	/** tests features of the session */
-	unsigned		ses_features;
-	/** features are synced with remote test nodes */
-	unsigned		ses_feats_updated:1;
-	/** force creating */
-	unsigned		ses_force:1;
-	/** session is shutting down */
-	unsigned		ses_shutdown:1;
-	/** console is timedout */
-	unsigned		ses_expired:1;
-	__u64		   ses_id_cookie;  /* batch id cookie */
-	char		    ses_name[LST_NAME_SIZE];  /* session name */
-	lstcon_rpc_trans_t     *ses_ping;       /* session pinger */
-	stt_timer_t	     ses_ping_timer; /* timer for pinger */
-	lstcon_trans_stat_t     ses_trans_stat; /* transaction stats */
-
-	struct list_head	      ses_trans_list; /* global list of transaction */
-	struct list_head	      ses_grp_list;   /* global list of groups */
-	struct list_head	      ses_bat_list;   /* global list of batches */
-	struct list_head	      ses_ndl_list;   /* global list of nodes */
-	struct list_head	     *ses_ndl_hash;   /* hash table of nodes */
-
-	spinlock_t	  ses_rpc_lock;   /* serialize */
-	atomic_t	    ses_rpc_counter;/* # of initialized RPCs */
-	struct list_head	      ses_rpc_freelist; /* idle console rpc */
-} lstcon_session_t;			     /*** session descriptor */
+	struct mutex        ses_mutex;        /* only 1 thread in session */
+	lst_sid_t           ses_id;           /* global session id */
+	int                 ses_key;          /* local session key */
+	int                 ses_state;        /* state of session */
+	int                 ses_timeout;      /* timeout in seconds */
+	time_t              ses_laststamp;    /* last operation stamp (seconds)
+					       */
+	unsigned            ses_features;     /* tests features of the session
+					       */
+	unsigned            ses_feats_updated:1; /* features are synced with
+						  * remote test nodes */
+	unsigned            ses_force:1;      /* force creating */
+	unsigned            ses_shutdown:1;   /* session is shutting down */
+	unsigned            ses_expired:1;    /* console is timedout */
+	__u64               ses_id_cookie;    /* batch id cookie */
+	char                ses_name[LST_NAME_SIZE];/* session name */
+	lstcon_rpc_trans_t  *ses_ping;        /* session pinger */
+	stt_timer_t         ses_ping_timer;   /* timer for pinger */
+	lstcon_trans_stat_t ses_trans_stat;   /* transaction stats */
+
+	struct list_head    ses_trans_list;   /* global list of transaction */
+	struct list_head    ses_grp_list;     /* global list of groups */
+	struct list_head    ses_bat_list;     /* global list of batches */
+	struct list_head    ses_ndl_list;     /* global list of nodes */
+	struct list_head    *ses_ndl_hash;    /* hash table of nodes */
+
+	spinlock_t          ses_rpc_lock;     /* serialize */
+	atomic_t            ses_rpc_counter;  /* # of initialized RPCs */
+	struct list_head    ses_rpc_freelist; /* idle console rpc */
+} lstcon_session_t; /* session descriptor */
 
 extern lstcon_session_t	 console_session;
 
diff --git a/drivers/staging/lustre/lnet/selftest/framework.c b/drivers/staging/lustre/lnet/selftest/framework.c
index a93a90de0f85..7c5185a2a795 100644
--- a/drivers/staging/lustre/lnet/selftest/framework.c
+++ b/drivers/staging/lustre/lnet/selftest/framework.c
@@ -53,20 +53,20 @@ static int rpc_timeout = 64;
 module_param(rpc_timeout, int, 0644);
 MODULE_PARM_DESC(rpc_timeout, "rpc timeout in seconds (64 by default, 0 == never)");
 
-#define sfw_unpack_id(id)	       \
-do {				    \
+#define sfw_unpack_id(id)          \
+do {                               \
 	__swab64s(&(id).nid);	   \
 	__swab32s(&(id).pid);	   \
 } while (0)
 
-#define sfw_unpack_sid(sid)	     \
-do {				    \
+#define sfw_unpack_sid(sid)             \
+do {                                    \
 	__swab64s(&(sid).ses_nid);      \
 	__swab64s(&(sid).ses_stamp);    \
 } while (0)
 
-#define sfw_unpack_fw_counters(fc)	\
-do {				      \
+#define sfw_unpack_fw_counters(fc)        \
+do {                                      \
 	__swab32s(&(fc).running_ms);      \
 	__swab32s(&(fc).active_batches);  \
 	__swab32s(&(fc).zombie_sessions); \
@@ -75,7 +75,7 @@ do {				      \
 } while (0)
 
 #define sfw_unpack_rpc_counters(rc)     \
-do {				    \
+do {                                    \
 	__swab32s(&(rc).errors);	\
 	__swab32s(&(rc).rpcs_sent);     \
 	__swab32s(&(rc).rpcs_rcvd);     \
@@ -86,7 +86,7 @@ do {				    \
 } while (0)
 
 #define sfw_unpack_lnet_counters(lc)    \
-do {				    \
+do {                                    \
 	__swab32s(&(lc).errors);	\
 	__swab32s(&(lc).msgs_max);      \
 	__swab32s(&(lc).msgs_alloc);    \
@@ -104,14 +104,14 @@ do {				    \
 #define sfw_batch_active(b)     (atomic_read(&(b)->bat_nactive) != 0)
 
 static struct smoketest_framework {
-	struct list_head	 fw_zombie_rpcs;     /* RPCs to be recycled */
-	struct list_head	 fw_zombie_sessions; /* stopping sessions */
-	struct list_head	 fw_tests;	   /* registered test cases */
-	atomic_t       fw_nzombies;	/* # zombie sessions */
-	spinlock_t	   fw_lock;		/* serialise */
-	sfw_session_t	  *fw_session;		/* _the_ session */
-	int		   fw_shuttingdown;	/* shutdown in progress */
-	srpc_server_rpc_t *fw_active_srpc;	/* running RPC */
+	struct list_head  fw_zombie_rpcs;     /* RPCs to be recycled */
+	struct list_head  fw_zombie_sessions; /* stopping sessions */
+	struct list_head  fw_tests;           /* registered test cases */
+	atomic_t          fw_nzombies;        /* # zombie sessions */
+	spinlock_t        fw_lock;            /* serialise */
+	sfw_session_t     *fw_session;        /* _the_ session */
+	int               fw_shuttingdown;    /* shutdown in progress */
+	srpc_server_rpc_t *fw_active_srpc;    /* running RPC */
 } sfw_data;
 
 /* forward ref's */
@@ -160,7 +160,7 @@ static void
 sfw_add_session_timer(void)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
-	stt_timer_t   *timer = &sn->sn_timer;
+	stt_timer_t *timer = &sn->sn_timer;
 
 	LASSERT(!sfw_data.fw_shuttingdown);
 
@@ -199,8 +199,8 @@ sfw_deactivate_session(void)
 	__must_hold(&sfw_data.fw_lock)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
-	int	    nactive = 0;
-	sfw_batch_t   *tsb;
+	int nactive = 0;
+	sfw_batch_t *tsb;
 	sfw_test_case_t *tsc;
 
 	if (sn == NULL) return;
@@ -273,7 +273,7 @@ sfw_init_session(sfw_session_t *sn, lst_sid_t sid,
 	strlcpy(&sn->sn_name[0], name, sizeof(sn->sn_name));
 
 	sn->sn_timer_active = 0;
-	sn->sn_id	   = sid;
+	sn->sn_id           = sid;
 	sn->sn_features	    = features;
 	sn->sn_timeout      = session_timeout;
 	sn->sn_started      = cfs_time_current();
@@ -287,8 +287,8 @@ sfw_init_session(sfw_session_t *sn, lst_sid_t sid,
 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;
+	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",
@@ -327,7 +327,7 @@ static sfw_batch_t *
 sfw_find_batch(lst_bid_t bid)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
-	sfw_batch_t   *bat;
+	sfw_batch_t *bat;
 
 	LASSERT(sn != NULL);
 
@@ -343,7 +343,7 @@ static sfw_batch_t *
 sfw_bid2batch(lst_bid_t bid)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
-	sfw_batch_t   *bat;
+	sfw_batch_t *bat;
 
 	LASSERT(sn != NULL);
 
@@ -368,10 +368,10 @@ sfw_bid2batch(lst_bid_t bid)
 static int
 sfw_get_stats(srpc_stat_reqst_t *request, srpc_stat_reply_t *reply)
 {
-	sfw_session_t  *sn = sfw_data.fw_session;
+	sfw_session_t *sn = sfw_data.fw_session;
 	sfw_counters_t *cnt = &reply->str_fw;
-	sfw_batch_t    *bat;
-	struct timeval  tv;
+	sfw_batch_t *bat;
+	struct timeval tv;
 
 	reply->str_sid = (sn == NULL) ? LST_INVALID_SID : sn->sn_id;
 
@@ -412,9 +412,9 @@ int
 sfw_make_session(srpc_mksn_reqst_t *request, srpc_mksn_reply_t *reply)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
-	srpc_msg_t    *msg = container_of(request, srpc_msg_t,
+	srpc_msg_t *msg = container_of(request, srpc_msg_t,
 					  msg_body.mksn_reqst);
-	int	       cplen = 0;
+	int cplen = 0;
 
 	if (request->mksn_sid.ses_nid == LNET_NID_ANY) {
 		reply->mksn_sid = (sn == NULL) ? LST_INVALID_SID : sn->sn_id;
@@ -533,7 +533,7 @@ sfw_debug_session(srpc_debug_reqst_t *request, srpc_debug_reply_t *reply)
 static void
 sfw_test_rpc_fini(srpc_client_rpc_t *rpc)
 {
-	sfw_test_unit_t     *tsu = rpc->crpc_priv;
+	sfw_test_unit_t *tsu = rpc->crpc_priv;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
 
 	/* Called with hold of tsi->tsi_lock */
@@ -544,9 +544,9 @@ sfw_test_rpc_fini(srpc_client_rpc_t *rpc)
 static inline int
 sfw_test_buffers(sfw_test_instance_t *tsi)
 {
-	struct sfw_test_case	*tsc = sfw_find_test_case(tsi->tsi_service);
-	struct srpc_service	*svc = tsc->tsc_srv_service;
-	int			nbuf;
+	struct sfw_test_case *tsc = sfw_find_test_case(tsi->tsi_service);
+	struct srpc_service *svc = tsc->tsc_srv_service;
+	int nbuf;
 
 	nbuf = min(svc->sv_wi_total, tsi->tsi_loop) / svc->sv_ncpts;
 	return max(SFW_TEST_WI_MIN, nbuf + SFW_TEST_WI_EXTRA);
@@ -555,10 +555,10 @@ sfw_test_buffers(sfw_test_instance_t *tsi)
 static int
 sfw_load_test(struct sfw_test_instance *tsi)
 {
-	struct sfw_test_case	*tsc;
-	struct srpc_service	*svc;
-	int			nbuf;
-	int			rc;
+	struct sfw_test_case *tsc;
+	struct srpc_service *svc;
+	int nbuf;
+	int rc;
 
 	LASSERT(tsi != NULL);
 	tsc = sfw_find_test_case(tsi->tsi_service);
@@ -611,7 +611,7 @@ static void
 sfw_destroy_test_instance(sfw_test_instance_t *tsi)
 {
 	srpc_client_rpc_t *rpc;
-	sfw_test_unit_t   *tsu;
+	sfw_test_unit_t *tsu;
 
 	if (!tsi->tsi_is_client) goto clean;
 
@@ -728,14 +728,14 @@ sfw_unpack_addtest_req(srpc_msg_t *msg)
 static int
 sfw_add_test_instance(sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 {
-	srpc_msg_t	  *msg = &rpc->srpc_reqstbuf->buf_msg;
-	srpc_test_reqst_t   *req = &msg->msg_body.tes_reqst;
-	srpc_bulk_t	 *bk = rpc->srpc_bulk;
-	int		  ndest = req->tsr_ndest;
-	sfw_test_unit_t     *tsu;
+	srpc_msg_t *msg = &rpc->srpc_reqstbuf->buf_msg;
+	srpc_test_reqst_t *req = &msg->msg_body.tes_reqst;
+	srpc_bulk_t *bk = rpc->srpc_bulk;
+	int ndest = req->tsr_ndest;
+	sfw_test_unit_t *tsu;
 	sfw_test_instance_t *tsi;
-	int		  i;
-	int		  rc;
+	int i;
+	int rc;
 
 	LIBCFS_ALLOC(tsi, sizeof(*tsi));
 	if (tsi == NULL) {
@@ -751,9 +751,9 @@ sfw_add_test_instance(sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 	INIT_LIST_HEAD(&tsi->tsi_active_rpcs);
 
 	tsi->tsi_stopping      = 0;
-	tsi->tsi_batch	 = tsb;
-	tsi->tsi_loop	  = req->tsr_loop;
-	tsi->tsi_concur	= req->tsr_concur;
+	tsi->tsi_batch         = tsb;
+	tsi->tsi_loop          = req->tsr_loop;
+	tsi->tsi_concur        = req->tsr_concur;
 	tsi->tsi_service       = req->tsr_service;
 	tsi->tsi_is_client     = !!(req->tsr_is_client);
 	tsi->tsi_stoptsu_onerr = !!(req->tsr_stop_onerr);
@@ -782,8 +782,8 @@ sfw_add_test_instance(sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
 
 	for (i = 0; i < ndest; i++) {
 		lnet_process_id_packed_t *dests;
-		lnet_process_id_packed_t  id;
-		int		       j;
+		lnet_process_id_packed_t id;
+		int j;
 
 		dests = page_address(bk->bk_iovs[i / SFW_ID_PER_PAGE].kiov_page);
 		LASSERT(dests != NULL);  /* my pages are within KVM always */
@@ -824,8 +824,8 @@ static void
 sfw_test_unit_done(sfw_test_unit_t *tsu)
 {
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
-	sfw_batch_t	 *tsb = tsi->tsi_batch;
-	sfw_session_t       *sn = tsb->bat_session;
+	sfw_batch_t *tsb = tsi->tsi_batch;
+	sfw_session_t *sn = tsb->bat_session;
 
 	LASSERT(sfw_test_active(tsi));
 
@@ -866,9 +866,9 @@ sfw_test_unit_done(sfw_test_unit_t *tsu)
 static void
 sfw_test_rpc_done(srpc_client_rpc_t *rpc)
 {
-	sfw_test_unit_t     *tsu = rpc->crpc_priv;
+	sfw_test_unit_t *tsu = rpc->crpc_priv;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
-	int		  done = 0;
+	int done = 0;
 
 	tsi->tsi_ops->tso_done_rpc(tsu, rpc);
 
@@ -904,7 +904,7 @@ sfw_create_test_rpc(sfw_test_unit_t *tsu, lnet_process_id_t peer,
 		    unsigned features, int nblk, int blklen,
 		    srpc_client_rpc_t **rpcpp)
 {
-	srpc_client_rpc_t   *rpc = NULL;
+	srpc_client_rpc_t *rpc = NULL;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
 
 	spin_lock(&tsi->tsi_lock);
@@ -945,9 +945,9 @@ sfw_create_test_rpc(sfw_test_unit_t *tsu, lnet_process_id_t peer,
 static int
 sfw_run_test(swi_workitem_t *wi)
 {
-	sfw_test_unit_t     *tsu = wi->swi_workitem.wi_data;
+	sfw_test_unit_t *tsu = wi->swi_workitem.wi_data;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
-	srpc_client_rpc_t   *rpc = NULL;
+	srpc_client_rpc_t *rpc = NULL;
 
 	LASSERT(wi == &tsu->tsu_worker);
 
@@ -995,8 +995,8 @@ test_done:
 static int
 sfw_run_batch(sfw_batch_t *tsb)
 {
-	swi_workitem_t      *wi;
-	sfw_test_unit_t     *tsu;
+	swi_workitem_t *wi;
+	sfw_test_unit_t *tsu;
 	sfw_test_instance_t *tsi;
 
 	if (sfw_batch_active(tsb)) {
@@ -1032,7 +1032,7 @@ int
 sfw_stop_batch(sfw_batch_t *tsb, int force)
 {
 	sfw_test_instance_t *tsi;
-	srpc_client_rpc_t   *rpc;
+	srpc_client_rpc_t *rpc;
 
 	if (!sfw_batch_active(tsb)) {
 		CDEBUG(D_NET, "Batch %llu inactive\n", tsb->bat_id.bat_id);
@@ -1118,11 +1118,11 @@ sfw_alloc_pages(struct srpc_server_rpc *rpc, int cpt, int npages, int len,
 static int
 sfw_add_test(srpc_server_rpc_t *rpc)
 {
-	sfw_session_t     *sn = sfw_data.fw_session;
+	sfw_session_t *sn = sfw_data.fw_session;
 	srpc_test_reply_t *reply = &rpc->srpc_replymsg.msg_body.tes_reply;
 	srpc_test_reqst_t *request;
-	int		rc;
-	sfw_batch_t       *bat;
+	int rc;
+	sfw_batch_t *bat;
 
 	request = &rpc->srpc_reqstbuf->buf_msg.msg_body.tes_reqst;
 	reply->tsr_sid = (sn == NULL) ? LST_INVALID_SID : sn->sn_id;
@@ -1160,8 +1160,8 @@ sfw_add_test(srpc_server_rpc_t *rpc)
 
 	if (request->tsr_is_client && rpc->srpc_bulk == NULL) {
 		/* rpc will be resumed later in sfw_bulk_ready */
-		int	npg = sfw_id_pages(request->tsr_ndest);
-		int	len;
+		int npg = sfw_id_pages(request->tsr_ndest);
+		int len;
 
 		if ((sn->sn_features & LST_FEAT_BULK_LEN) == 0) {
 			len = npg * PAGE_CACHE_SIZE;
@@ -1189,8 +1189,8 @@ static int
 sfw_control_batch(srpc_batch_reqst_t *request, srpc_batch_reply_t *reply)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
-	int	    rc = 0;
-	sfw_batch_t   *bat;
+	int rc = 0;
+	sfw_batch_t *bat;
 
 	reply->bar_sid = (sn == NULL) ? LST_INVALID_SID : sn->sn_id;
 
@@ -1229,11 +1229,11 @@ sfw_control_batch(srpc_batch_reqst_t *request, srpc_batch_reply_t *reply)
 static int
 sfw_handle_server_rpc(struct srpc_server_rpc *rpc)
 {
-	struct srpc_service	*sv = rpc->srpc_scd->scd_svc;
-	srpc_msg_t     *reply	= &rpc->srpc_replymsg;
-	srpc_msg_t     *request	= &rpc->srpc_reqstbuf->buf_msg;
-	unsigned	features = LST_FEATS_MASK;
-	int		rc = 0;
+	struct srpc_service *sv = rpc->srpc_scd->scd_svc;
+	srpc_msg_t *reply = &rpc->srpc_replymsg;
+	srpc_msg_t *request = &rpc->srpc_reqstbuf->buf_msg;
+	unsigned features = LST_FEATS_MASK;
+	int rc = 0;
 
 	LASSERT(sfw_data.fw_active_srpc == NULL);
 	LASSERT(sv->sv_id <= SRPC_FRAMEWORK_SERVICE_MAX_ID);
@@ -1334,8 +1334,8 @@ sfw_handle_server_rpc(struct srpc_server_rpc *rpc)
 static int
 sfw_bulk_ready(struct srpc_server_rpc *rpc, int status)
 {
-	struct srpc_service	*sv = rpc->srpc_scd->scd_svc;
-	int			rc;
+	struct srpc_service *sv = rpc->srpc_scd->scd_svc;
+	int rc;
 
 	LASSERT(rpc->srpc_bulk != NULL);
 	LASSERT(sv->sv_id == SRPC_SERVICE_TEST);
@@ -1640,10 +1640,10 @@ extern void brw_init_test_service(void);
 int
 sfw_startup(void)
 {
-	int	      i;
-	int	      rc;
-	int	      error;
-	srpc_service_t  *sv;
+	int i;
+	int rc;
+	int error;
+	srpc_service_t *sv;
 	sfw_test_case_t *tsc;
 
 
@@ -1735,9 +1735,9 @@ sfw_startup(void)
 void
 sfw_shutdown(void)
 {
-	srpc_service_t	*sv;
+	srpc_service_t *sv;
 	sfw_test_case_t	*tsc;
-	int		 i;
+	int i;
 
 	spin_lock(&sfw_data.fw_lock);
 
diff --git a/drivers/staging/lustre/lnet/selftest/module.c b/drivers/staging/lustre/lnet/selftest/module.c
index 7ad62f167cea..09b8f4649796 100644
--- a/drivers/staging/lustre/lnet/selftest/module.c
+++ b/drivers/staging/lustre/lnet/selftest/module.c
@@ -39,7 +39,7 @@
 #include "selftest.h"
 
 enum {
-	LST_INIT_NONE		= 0,
+	LST_INIT_NONE = 0,
 	LST_INIT_WI_SERIAL,
 	LST_INIT_WI_TEST,
 	LST_INIT_RPC,
@@ -58,7 +58,7 @@ struct cfs_wi_sched **lst_sched_test;
 static void
 lnet_selftest_fini(void)
 {
-	int	i;
+	int i;
 
 	switch (lst_init_step) {
 	case LST_INIT_CONSOLE:
@@ -92,9 +92,9 @@ lnet_selftest_fini(void)
 static int
 lnet_selftest_init(void)
 {
-	int	nscheds;
-	int	rc;
-	int	i;
+	int nscheds;
+	int rc;
+	int i;
 
 	rc = cfs_wi_sched_create("lst_s", lnet_cpt_table(), CFS_CPT_ANY,
 				 1, &lst_sched_serial);
diff --git a/drivers/staging/lustre/lnet/selftest/ping_test.c b/drivers/staging/lustre/lnet/selftest/ping_test.c
index 644069a9fe4e..1dab9984c58e 100644
--- a/drivers/staging/lustre/lnet/selftest/ping_test.c
+++ b/drivers/staging/lustre/lnet/selftest/ping_test.c
@@ -73,7 +73,7 @@ static void
 ping_client_fini(sfw_test_instance_t *tsi)
 {
 	sfw_session_t *sn = tsi->tsi_batch->bat_session;
-	int	    errors;
+	int errors;
 
 	LASSERT(sn != NULL);
 	LASSERT(tsi->tsi_is_client);
@@ -89,11 +89,11 @@ static int
 ping_client_prep_rpc(sfw_test_unit_t *tsu,
 		     lnet_process_id_t dest, srpc_client_rpc_t **rpc)
 {
-	srpc_ping_reqst_t   *req;
+	srpc_ping_reqst_t *req;
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
-	sfw_session_t       *sn  = tsi->tsi_batch->bat_session;
-	struct timeval       tv;
-	int		     rc;
+	sfw_session_t *sn = tsi->tsi_batch->bat_session;
+	struct timeval tv;
+	int rc;
 
 	LASSERT(sn != NULL);
 	LASSERT((sn->sn_features & ~LST_FEATS_MASK) == 0);
@@ -121,10 +121,10 @@ static void
 ping_client_done_rpc(sfw_test_unit_t *tsu, srpc_client_rpc_t *rpc)
 {
 	sfw_test_instance_t *tsi = tsu->tsu_instance;
-	sfw_session_t       *sn = tsi->tsi_batch->bat_session;
-	srpc_ping_reqst_t   *reqst = &rpc->crpc_reqstmsg.msg_body.ping_reqst;
-	srpc_ping_reply_t   *reply = &rpc->crpc_replymsg.msg_body.ping_reply;
-	struct timeval       tv;
+	sfw_session_t *sn = tsi->tsi_batch->bat_session;
+	srpc_ping_reqst_t *reqst = &rpc->crpc_reqstmsg.msg_body.ping_reqst;
+	srpc_ping_reply_t *reply = &rpc->crpc_replymsg.msg_body.ping_reply;
+	struct timeval tv;
 
 	LASSERT(sn != NULL);
 
@@ -171,9 +171,9 @@ ping_client_done_rpc(sfw_test_unit_t *tsu, srpc_client_rpc_t *rpc)
 static int
 ping_server_handle(struct srpc_server_rpc *rpc)
 {
-	struct srpc_service	*sv  = rpc->srpc_scd->scd_svc;
-	srpc_msg_t	*reqstmsg = &rpc->srpc_reqstbuf->buf_msg;
-	srpc_msg_t	  *replymsg = &rpc->srpc_replymsg;
+	struct srpc_service *sv  = rpc->srpc_scd->scd_svc;
+	srpc_msg_t *reqstmsg = &rpc->srpc_reqstbuf->buf_msg;
+	srpc_msg_t *replymsg = &rpc->srpc_replymsg;
 	srpc_ping_reqst_t *req = &reqstmsg->msg_body.ping_reqst;
 	srpc_ping_reply_t *rep = &rpc->srpc_replymsg.msg_body.ping_reply;
 
diff --git a/drivers/staging/lustre/lnet/selftest/rpc.c b/drivers/staging/lustre/lnet/selftest/rpc.c
index 080788ab749e..6ae133138b17 100644
--- a/drivers/staging/lustre/lnet/selftest/rpc.c
+++ b/drivers/staging/lustre/lnet/selftest/rpc.c
@@ -90,7 +90,7 @@ void srpc_set_counters(const srpc_counters_t *cnt)
 static int
 srpc_add_bulk_page(srpc_bulk_t *bk, struct page *pg, int i, int nob)
 {
-	nob = min(nob, (int)PAGE_CACHE_SIZE);
+	nob = min_t(int, nob, PAGE_CACHE_SIZE);
 
 	LASSERT(nob > 0);
 	LASSERT(i >= 0 && i < bk->bk_niov);
@@ -104,7 +104,7 @@ srpc_add_bulk_page(srpc_bulk_t *bk, struct page *pg, int i, int nob)
 void
 srpc_free_bulk(srpc_bulk_t *bk)
 {
-	int	 i;
+	int i;
 	struct page *pg;
 
 	LASSERT(bk != NULL);
@@ -124,8 +124,8 @@ srpc_free_bulk(srpc_bulk_t *bk)
 srpc_bulk_t *
 srpc_alloc_bulk(int cpt, unsigned bulk_npg, unsigned bulk_len, int sink)
 {
-	srpc_bulk_t  *bk;
-	int	      i;
+	srpc_bulk_t *bk;
+	int i;
 
 	LASSERT(bulk_npg > 0 && bulk_npg <= LNET_MAX_IOV);
 
@@ -143,7 +143,7 @@ srpc_alloc_bulk(int cpt, unsigned bulk_npg, unsigned bulk_len, int sink)
 
 	for (i = 0; i < bulk_npg; i++) {
 		struct page *pg;
-		int	    nob;
+		int nob;
 
 		pg = alloc_pages_node(cfs_cpt_spread_node(lnet_cpt_table(), cpt),
 				      GFP_IOFS, 0);
@@ -193,11 +193,11 @@ srpc_init_server_rpc(struct srpc_server_rpc *rpc,
 static void
 srpc_service_fini(struct srpc_service *svc)
 {
-	struct srpc_service_cd	*scd;
-	struct srpc_server_rpc	*rpc;
-	struct srpc_buffer	*buf;
-	struct list_head		*q;
-	int			i;
+	struct srpc_service_cd *scd;
+	struct srpc_server_rpc *rpc;
+	struct srpc_buffer *buf;
+	struct list_head *q;
+	int i;
 
 	if (svc->sv_cpt_data == NULL)
 		return;
@@ -249,11 +249,11 @@ int srpc_add_buffer(struct swi_workitem *wi);
 static int
 srpc_service_init(struct srpc_service *svc)
 {
-	struct srpc_service_cd	*scd;
-	struct srpc_server_rpc	*rpc;
-	int			nrpcs;
-	int			i;
-	int			j;
+	struct srpc_service_cd *scd;
+	struct srpc_server_rpc *rpc;
+	int nrpcs;
+	int i;
+	int j;
 
 	svc->sv_shuttingdown = 0;
 
@@ -357,8 +357,8 @@ 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)
 {
-	int		 rc;
-	lnet_md_t	 md;
+	int rc;
+	lnet_md_t md;
 	lnet_handle_me_t meh;
 
 	rc = LNetMEAttach(portal, peer, matchbits, 0, LNET_UNLINK,
@@ -397,7 +397,7 @@ 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)
 {
-	int       rc;
+	int rc;
 	lnet_md_t md;
 
 	md.user_ptr  = ev;
@@ -471,9 +471,9 @@ static int
 srpc_service_post_buffer(struct srpc_service_cd *scd, struct srpc_buffer *buf)
 	__must_hold(&scd->scd_lock)
 {
-	struct srpc_service	*sv = scd->scd_svc;
-	struct srpc_msg		*msg = &buf->buf_msg;
-	int			rc;
+	struct srpc_service *sv = scd->scd_svc;
+	struct srpc_msg *msg = &buf->buf_msg;
+	int rc;
 
 	LNetInvalidateHandle(&buf->buf_mdh);
 	list_add(&buf->buf_list, &scd->scd_buf_posted);
@@ -519,9 +519,9 @@ srpc_service_post_buffer(struct srpc_service_cd *scd, struct srpc_buffer *buf)
 int
 srpc_add_buffer(struct swi_workitem *wi)
 {
-	struct srpc_service_cd	*scd = wi->swi_workitem.wi_data;
-	struct srpc_buffer	*buf;
-	int			rc = 0;
+	struct srpc_service_cd *scd = wi->swi_workitem.wi_data;
+	struct srpc_buffer *buf;
+	int rc = 0;
 
 	/* it's called by workitem scheduler threads, these threads
 	 * should have been set CPT affinity, so buffers will be posted
@@ -579,9 +579,9 @@ srpc_add_buffer(struct swi_workitem *wi)
 int
 srpc_service_add_buffers(struct srpc_service *sv, int nbuffer)
 {
-	struct srpc_service_cd	*scd;
-	int			rc = 0;
-	int			i;
+	struct srpc_service_cd *scd;
+	int rc = 0;
+	int i;
 
 	LASSERTF(nbuffer > 0, "nbuffer must be positive: %d\n", nbuffer);
 
@@ -633,9 +633,9 @@ srpc_service_add_buffers(struct srpc_service *sv, int nbuffer)
 void
 srpc_service_remove_buffers(struct srpc_service *sv, int nbuffer)
 {
-	struct srpc_service_cd	*scd;
-	int			num;
-	int			i;
+	struct srpc_service_cd *scd;
+	int num;
+	int i;
 
 	LASSERT(!sv->sv_shuttingdown);
 
@@ -653,9 +653,9 @@ srpc_service_remove_buffers(struct srpc_service *sv, int nbuffer)
 int
 srpc_finish_service(struct srpc_service *sv)
 {
-	struct srpc_service_cd	*scd;
-	struct srpc_server_rpc	*rpc;
-	int			i;
+	struct srpc_service_cd *scd;
+	struct srpc_server_rpc *rpc;
+	int i;
 
 	LASSERT(sv->sv_shuttingdown); /* srpc_shutdown_service called */
 
@@ -731,9 +731,9 @@ srpc_service_recycle_buffer(struct srpc_service_cd *scd, srpc_buffer_t *buf)
 void
 srpc_abort_service(struct srpc_service *sv)
 {
-	struct srpc_service_cd	*scd;
-	struct srpc_server_rpc	*rpc;
-	int			i;
+	struct srpc_service_cd *scd;
+	struct srpc_server_rpc *rpc;
+	int i;
 
 	CDEBUG(D_NET, "Aborting service: id %d, name %s\n",
 	       sv->sv_id, sv->sv_name);
@@ -756,10 +756,10 @@ srpc_abort_service(struct srpc_service *sv)
 void
 srpc_shutdown_service(srpc_service_t *sv)
 {
-	struct srpc_service_cd	*scd;
-	struct srpc_server_rpc	*rpc;
-	srpc_buffer_t		*buf;
-	int			i;
+	struct srpc_service_cd *scd;
+	struct srpc_server_rpc *rpc;
+	srpc_buffer_t *buf;
+	int i;
 
 	CDEBUG(D_NET, "Shutting down service: id %d, name %s\n",
 	       sv->sv_id, sv->sv_name);
@@ -792,7 +792,7 @@ static int
 srpc_send_request(srpc_client_rpc_t *rpc)
 {
 	srpc_event_t *ev = &rpc->crpc_reqstev;
-	int	   rc;
+	int rc;
 
 	ev->ev_fired = 0;
 	ev->ev_data  = rpc;
@@ -812,8 +812,8 @@ static int
 srpc_prepare_reply(srpc_client_rpc_t *rpc)
 {
 	srpc_event_t *ev = &rpc->crpc_replyev;
-	__u64	*id = &rpc->crpc_reqstmsg.msg_body.reqst.rpyid;
-	int	   rc;
+	__u64 *id = &rpc->crpc_reqstmsg.msg_body.reqst.rpyid;
+	int rc;
 
 	ev->ev_fired = 0;
 	ev->ev_data  = rpc;
@@ -835,11 +835,11 @@ srpc_prepare_reply(srpc_client_rpc_t *rpc)
 static int
 srpc_prepare_bulk(srpc_client_rpc_t *rpc)
 {
-	srpc_bulk_t  *bk = &rpc->crpc_bulk;
+	srpc_bulk_t *bk = &rpc->crpc_bulk;
 	srpc_event_t *ev = &rpc->crpc_bulkev;
 	__u64	*id = &rpc->crpc_reqstmsg.msg_body.reqst.bulkid;
-	int	   rc;
-	int	   opt;
+	int rc;
+	int opt;
 
 	LASSERT(bk->bk_niov <= LNET_MAX_IOV);
 
@@ -868,11 +868,11 @@ srpc_prepare_bulk(srpc_client_rpc_t *rpc)
 static int
 srpc_do_bulk(srpc_server_rpc_t *rpc)
 {
-	srpc_event_t  *ev = &rpc->srpc_ev;
-	srpc_bulk_t   *bk = rpc->srpc_bulk;
-	__u64	  id = rpc->srpc_reqstbuf->buf_msg.msg_body.reqst.bulkid;
-	int	    rc;
-	int	    opt;
+	srpc_event_t *ev = &rpc->srpc_ev;
+	srpc_bulk_t *bk = rpc->srpc_bulk;
+	__u64 id = rpc->srpc_reqstbuf->buf_msg.msg_body.reqst.bulkid;
+	int rc;
+	int opt;
 
 	LASSERT(bk != NULL);
 
@@ -896,9 +896,9 @@ srpc_do_bulk(srpc_server_rpc_t *rpc)
 static void
 srpc_server_rpc_done(srpc_server_rpc_t *rpc, int status)
 {
-	struct srpc_service_cd	*scd = rpc->srpc_scd;
-	struct srpc_service	*sv  = scd->scd_svc;
-	srpc_buffer_t		*buffer;
+	struct srpc_service_cd *scd = rpc->srpc_scd;
+	struct srpc_service *sv  = scd->scd_svc;
+	srpc_buffer_t *buffer;
 
 	LASSERT(status != 0 || rpc->srpc_wi.swi_state == SWI_STATE_DONE);
 
@@ -959,11 +959,11 @@ srpc_server_rpc_done(srpc_server_rpc_t *rpc, int status)
 int
 srpc_handle_rpc(swi_workitem_t *wi)
 {
-	struct srpc_server_rpc	*rpc = wi->swi_workitem.wi_data;
-	struct srpc_service_cd	*scd = rpc->srpc_scd;
-	struct srpc_service	*sv = scd->scd_svc;
-	srpc_event_t		*ev = &rpc->srpc_ev;
-	int			rc = 0;
+	struct srpc_server_rpc *rpc = wi->swi_workitem.wi_data;
+	struct srpc_service_cd *scd = rpc->srpc_scd;
+	struct srpc_service *sv = scd->scd_svc;
+	srpc_event_t *ev = &rpc->srpc_ev;
+	int rc = 0;
 
 	LASSERT(wi == &rpc->srpc_wi);
 
@@ -989,7 +989,7 @@ srpc_handle_rpc(swi_workitem_t *wi)
 	default:
 		LBUG();
 	case SWI_STATE_NEWBORN: {
-		srpc_msg_t	   *msg;
+		srpc_msg_t *msg;
 		srpc_generic_reply_t *reply;
 
 		msg = &rpc->srpc_reqstbuf->buf_msg;
@@ -1173,10 +1173,10 @@ srpc_client_rpc_done(srpc_client_rpc_t *rpc, int status)
 int
 srpc_send_rpc(swi_workitem_t *wi)
 {
-	int		rc = 0;
+	int rc = 0;
 	srpc_client_rpc_t *rpc;
-	srpc_msg_t	*reply;
-	int		do_bulk;
+	srpc_msg_t *reply;
+	int do_bulk;
 
 	LASSERT(wi != NULL);
 
@@ -1359,13 +1359,13 @@ srpc_post_rpc(srpc_client_rpc_t *rpc)
 int
 srpc_send_reply(struct srpc_server_rpc *rpc)
 {
-	srpc_event_t		*ev = &rpc->srpc_ev;
-	struct srpc_msg		*msg = &rpc->srpc_replymsg;
-	struct srpc_buffer	*buffer = rpc->srpc_reqstbuf;
-	struct srpc_service_cd	*scd = rpc->srpc_scd;
-	struct srpc_service	*sv = scd->scd_svc;
-	__u64			rpyid;
-	int			rc;
+	srpc_event_t *ev = &rpc->srpc_ev;
+	struct srpc_msg *msg = &rpc->srpc_replymsg;
+	struct srpc_buffer *buffer = rpc->srpc_reqstbuf;
+	struct srpc_service_cd *scd = rpc->srpc_scd;
+	struct srpc_service *sv = scd->scd_svc;
+	__u64 rpyid;
+	int rc;
 
 	LASSERT(buffer != NULL);
 	rpyid = buffer->buf_msg.msg_body.reqst.rpyid;
@@ -1403,14 +1403,14 @@ srpc_send_reply(struct srpc_server_rpc *rpc)
 static void
 srpc_lnet_ev_handler(lnet_event_t *ev)
 {
-	struct srpc_service_cd	*scd;
-	srpc_event_t      *rpcev = ev->md.user_ptr;
+	struct srpc_service_cd *scd;
+	srpc_event_t *rpcev = ev->md.user_ptr;
 	srpc_client_rpc_t *crpc;
 	srpc_server_rpc_t *srpc;
-	srpc_buffer_t     *buffer;
-	srpc_service_t    *sv;
-	srpc_msg_t	*msg;
-	srpc_msg_type_t    type;
+	srpc_buffer_t *buffer;
+	srpc_service_t *sv;
+	srpc_msg_t *msg;
+	srpc_msg_type_t type;
 
 	LASSERT(!in_interrupt());
 
diff --git a/drivers/staging/lustre/lnet/selftest/rpc.h b/drivers/staging/lustre/lnet/selftest/rpc.h
index fbeb75fe5922..b7b00c6b1004 100644
--- a/drivers/staging/lustre/lnet/selftest/rpc.h
+++ b/drivers/staging/lustre/lnet/selftest/rpc.h
@@ -79,60 +79,61 @@ typedef struct {
 } WIRE_ATTR srpc_generic_reqst_t;
 
 typedef struct {
-	__u32		   status;
-	lst_sid_t	       sid;
+	__u32                   status;
+	lst_sid_t               sid;
 } WIRE_ATTR srpc_generic_reply_t;
 
 /* FRAMEWORK RPCs */
 typedef struct {
-	__u64			mksn_rpyid;      /* reply buffer matchbits */
-	lst_sid_t	       mksn_sid;	/* session id */
-	__u32			mksn_force;      /* use brute force */
+	__u64                   mksn_rpyid;     /* reply buffer matchbits */
+	lst_sid_t               mksn_sid;	/* session id */
+	__u32			mksn_force;     /* use brute force */
 	char			mksn_name[LST_NAME_SIZE];
-} WIRE_ATTR srpc_mksn_reqst_t;			/* make session request */
+} WIRE_ATTR srpc_mksn_reqst_t; /* make session request */
 
 typedef struct {
-	__u32		   mksn_status;      /* session status */
-	lst_sid_t	       mksn_sid;	 /* session id */
-	__u32		   mksn_timeout;     /* session timeout */
-	char			mksn_name[LST_NAME_SIZE];
+	__u32                   mksn_status;    /* session status */
+	lst_sid_t               mksn_sid;       /* session id */
+	__u32                   mksn_timeout;   /* session timeout */
+	char                    mksn_name[LST_NAME_SIZE];
 } WIRE_ATTR srpc_mksn_reply_t; /* make session reply */
 
 typedef struct {
-	__u64			rmsn_rpyid;      /* reply buffer matchbits */
-	lst_sid_t		rmsn_sid;	/* session id */
+	__u64                   rmsn_rpyid;     /* reply buffer matchbits */
+	lst_sid_t               rmsn_sid;       /* session id */
 } WIRE_ATTR srpc_rmsn_reqst_t; /* remove session request */
 
 typedef struct {
-	__u32			rmsn_status;
-	lst_sid_t		rmsn_sid;	/* session id */
+	__u32                   rmsn_status;
+	lst_sid_t               rmsn_sid;       /* session id */
 } WIRE_ATTR srpc_rmsn_reply_t; /* remove session reply */
 
 typedef struct {
-	__u64			join_rpyid;     /* reply buffer matchbits */
-	lst_sid_t	       join_sid;       /* session id to join */
-	char		    join_group[LST_NAME_SIZE]; /* group name */
+	__u64                   join_rpyid;     /* reply buffer matchbits */
+	lst_sid_t               join_sid;       /* session id to join */
+	char                    join_group[LST_NAME_SIZE]; /* group name */
 } WIRE_ATTR srpc_join_reqst_t;
 
 typedef struct {
-	__u32		   join_status;    /* returned status */
-	lst_sid_t	       join_sid;       /* session id */
-	__u32			join_timeout;   /* # seconds' inactivity to expire */
-	char		    join_session[LST_NAME_SIZE]; /* session name */
+	__u32                   join_status;    /* returned status */
+	lst_sid_t               join_sid;       /* session id */
+	__u32			join_timeout;   /* # seconds' inactivity to
+						 * expire */
+	char                    join_session[LST_NAME_SIZE]; /* session name */
 } WIRE_ATTR srpc_join_reply_t;
 
 typedef struct {
-	__u64		   dbg_rpyid;      /* reply buffer matchbits */
-	lst_sid_t	       dbg_sid;	/* session id */
-	__u32		   dbg_flags;      /* bitmap of debug */
+	__u64                   dbg_rpyid;      /* reply buffer matchbits */
+	lst_sid_t               dbg_sid;        /* session id */
+	__u32                   dbg_flags;      /* bitmap of debug */
 } WIRE_ATTR srpc_debug_reqst_t;
 
 typedef struct {
-	__u32		   dbg_status;     /* returned code */
-	lst_sid_t	       dbg_sid;	/* session id */
-	__u32		   dbg_timeout;    /* session timeout */
-	__u32		   dbg_nbatch;     /* # of batches in the node */
-	char		    dbg_name[LST_NAME_SIZE]; /* session name */
+	__u32                   dbg_status;     /* returned code */
+	lst_sid_t               dbg_sid;        /* session id */
+	__u32                   dbg_timeout;    /* session timeout */
+	__u32                   dbg_nbatch;     /* # of batches in the node */
+	char                    dbg_name[LST_NAME_SIZE]; /* session name */
 } WIRE_ATTR srpc_debug_reply_t;
 
 #define SRPC_BATCH_OPC_RUN      1
@@ -140,55 +141,51 @@ typedef struct {
 #define SRPC_BATCH_OPC_QUERY    3
 
 typedef struct {
-	__u64		   bar_rpyid;      /* reply buffer matchbits */
-	lst_sid_t	       bar_sid;	/* session id */
-	lst_bid_t	       bar_bid;	/* batch id */
-	__u32		   bar_opc;	/* create/start/stop batch */
-	__u32		   bar_testidx;    /* index of test */
-	__u32		   bar_arg;	/* parameters */
+	__u64              bar_rpyid;      /* reply buffer matchbits */
+	lst_sid_t          bar_sid;        /* session id */
+	lst_bid_t          bar_bid;        /* batch id */
+	__u32              bar_opc;        /* create/start/stop batch */
+	__u32              bar_testidx;    /* index of test */
+	__u32              bar_arg;        /* parameters */
 } WIRE_ATTR srpc_batch_reqst_t;
 
 typedef struct {
-	__u32		   bar_status;     /* status of request */
-	lst_sid_t	       bar_sid;	/* session id */
-	__u32		   bar_active;     /* # of active tests in batch/test */
-	__u32		   bar_time;       /* remained time */
+	__u32              bar_status;     /* status of request */
+	lst_sid_t          bar_sid;        /* session id */
+	__u32              bar_active;     /* # of active tests in batch/test */
+	__u32              bar_time;       /* remained time */
 } WIRE_ATTR srpc_batch_reply_t;
 
 typedef struct {
-	__u64		   str_rpyid;      /* reply buffer matchbits */
-	lst_sid_t	       str_sid;	/* session id */
-	__u32		   str_type;       /* type of stat */
+	__u64              str_rpyid;      /* reply buffer matchbits */
+	lst_sid_t          str_sid;        /* session id */
+	__u32              str_type;       /* type of stat */
 } WIRE_ATTR srpc_stat_reqst_t;
 
 typedef struct {
-	__u32		   str_status;
-	lst_sid_t	       str_sid;
-	sfw_counters_t	  str_fw;
-	srpc_counters_t	 str_rpc;
-	lnet_counters_t	 str_lnet;
+	__u32              str_status;
+	lst_sid_t          str_sid;
+	sfw_counters_t     str_fw;
+	srpc_counters_t    str_rpc;
+	lnet_counters_t    str_lnet;
 } WIRE_ATTR srpc_stat_reply_t;
 
 typedef struct {
-	__u32		   blk_opc;	/* bulk operation code */
-	__u32		   blk_npg;	/* # of pages */
-	__u32		   blk_flags;      /* reserved flags */
+	__u32              blk_opc;        /* bulk operation code */
+	__u32              blk_npg;        /* # of pages */
+	__u32              blk_flags;      /* reserved flags */
 } WIRE_ATTR test_bulk_req_t;
 
 typedef struct {
-	/** bulk operation code */
-	__u16			blk_opc;
-	/** data check flags */
-	__u16			blk_flags;
-	/** data length */
-	__u32			blk_len;
-	/** reserved: offset */
-	__u32		   blk_offset;
+	__u16              blk_opc;        /* bulk operation code */
+	__u16              blk_flags;      /* data check flags */
+	__u32              blk_len;        /* data length */
+	__u32              blk_offset;     /* reserved: offset */
 } WIRE_ATTR test_bulk_req_v1_t;
 
 typedef struct {
-	__u32			png_size;       /* size of ping message */
-	__u32			png_flags;      /* reserved flags */
+	__u32              png_size;       /* size of ping message */
+	__u32              png_flags;      /* reserved flags */
 } WIRE_ATTR test_ping_req_t;
 
 typedef struct {
@@ -197,8 +194,8 @@ typedef struct {
 	lst_sid_t		tsr_sid;	/* session id */
 	lst_bid_t		tsr_bid;	/* batch id */
 	__u32			tsr_service;    /* test type: bulk|ping|... */
-	/* test client loop count or # server buffers needed */
-	__u32			tsr_loop;
+	__u32			tsr_loop;       /* test client loop count or
+						 * # server buffers needed */
 	__u32			tsr_concur;     /* concurrency of test */
 	__u8			tsr_is_client;  /* is test client or not */
 	__u8			tsr_stop_onerr; /* stop on error */
@@ -234,8 +231,8 @@ typedef struct {
 typedef struct {
 	__u64		   brw_rpyid;      /* reply buffer matchbits */
 	__u64		   brw_bulkid;     /* bulk buffer matchbits */
-	__u32		   brw_rw;	 /* read or write */
-	__u32		   brw_len;	/* bulk data len */
+	__u32		   brw_rw;         /* read or write */
+	__u32		   brw_len;        /* bulk data len */
 	__u32		   brw_flags;      /* bulk data patterns */
 } WIRE_ATTR srpc_brw_reqst_t; /* bulk r/w request */
 
@@ -243,20 +240,16 @@ typedef struct {
 	__u32		   brw_status;
 } WIRE_ATTR srpc_brw_reply_t; /* bulk r/w reply */
 
-#define SRPC_MSG_MAGIC		  0xeeb0f00d
-#define SRPC_MSG_VERSION		1
+#define SRPC_MSG_MAGIC   0xeeb0f00d
+#define SRPC_MSG_VERSION 1
 
 typedef struct srpc_msg {
-	/** magic number */
-	__u32	msg_magic;
-	/** message version number */
-	__u32	msg_version;
-	/** type of message body: srpc_msg_type_t */
-	__u32	msg_type;
+	__u32	msg_magic;     /* magic number */
+	__u32	msg_version;   /* message version number */
+	__u32	msg_type;      /* type of message body: srpc_msg_type_t */
 	__u32	msg_reserved0;
 	__u32	msg_reserved1;
-	/** test session features */
-	__u32	msg_ses_feats;
+	__u32	msg_ses_feats; /* test session features */
 	union {
 		srpc_generic_reqst_t reqst;
 		srpc_generic_reply_t reply;
diff --git a/drivers/staging/lustre/lnet/selftest/selftest.h b/drivers/staging/lustre/lnet/selftest/selftest.h
index d48701834b18..7939e4e04d90 100644
--- a/drivers/staging/lustre/lnet/selftest/selftest.h
+++ b/drivers/staging/lustre/lnet/selftest/selftest.h
@@ -57,14 +57,14 @@
 #endif
 
 
-#define SWI_STATE_NEWBORN		  0
-#define SWI_STATE_REPLY_SUBMITTED	  1
-#define SWI_STATE_REPLY_SENT	       2
-#define SWI_STATE_REQUEST_SUBMITTED	3
-#define SWI_STATE_REQUEST_SENT	     4
-#define SWI_STATE_REPLY_RECEIVED	   5
-#define SWI_STATE_BULK_STARTED	     6
-#define SWI_STATE_DONE		     10
+#define SWI_STATE_NEWBORN           0
+#define SWI_STATE_REPLY_SUBMITTED   1
+#define SWI_STATE_REPLY_SENT        2
+#define SWI_STATE_REQUEST_SUBMITTED 3
+#define SWI_STATE_REQUEST_SENT      4
+#define SWI_STATE_REPLY_RECEIVED    5
+#define SWI_STATE_BULK_STARTED      6
+#define SWI_STATE_DONE             10
 
 /* forward refs */
 struct srpc_service;
@@ -75,24 +75,24 @@ struct sfw_test_instance;
 /* services below SRPC_FRAMEWORK_SERVICE_MAX_ID are framework
  * services, e.g. create/modify session.
  */
-#define SRPC_SERVICE_DEBUG	      0
-#define SRPC_SERVICE_MAKE_SESSION       1
-#define SRPC_SERVICE_REMOVE_SESSION     2
-#define SRPC_SERVICE_BATCH	      3
-#define SRPC_SERVICE_TEST	       4
-#define SRPC_SERVICE_QUERY_STAT	 5
-#define SRPC_SERVICE_JOIN	       6
-#define SRPC_FRAMEWORK_SERVICE_MAX_ID   10
+#define SRPC_SERVICE_DEBUG             0
+#define SRPC_SERVICE_MAKE_SESSION      1
+#define SRPC_SERVICE_REMOVE_SESSION    2
+#define SRPC_SERVICE_BATCH             3
+#define SRPC_SERVICE_TEST              4
+#define SRPC_SERVICE_QUERY_STAT        5
+#define SRPC_SERVICE_JOIN              6
+#define SRPC_FRAMEWORK_SERVICE_MAX_ID 10
 /* other services start from SRPC_FRAMEWORK_SERVICE_MAX_ID+1 */
-#define SRPC_SERVICE_BRW		11
-#define SRPC_SERVICE_PING	       12
-#define SRPC_SERVICE_MAX_ID	     12
+#define SRPC_SERVICE_BRW              11
+#define SRPC_SERVICE_PING             12
+#define SRPC_SERVICE_MAX_ID           12
 
-#define SRPC_REQUEST_PORTAL	     50
+#define SRPC_REQUEST_PORTAL           50
 /* a lazy portal for framework RPC requests */
-#define SRPC_FRAMEWORK_REQUEST_PORTAL   51
+#define SRPC_FRAMEWORK_REQUEST_PORTAL 51
 /* all reply/bulk RDMAs go to this portal */
-#define SRPC_RDMA_PORTAL		52
+#define SRPC_RDMA_PORTAL              52
 
 static inline srpc_msg_type_t
 srpc_service2request (int service)
@@ -136,7 +136,8 @@ srpc_service2reply (int service)
 }
 
 typedef enum {
-	SRPC_BULK_REQ_RCVD   = 1, /* passive bulk request(PUT sink/GET source) received */
+	SRPC_BULK_REQ_RCVD   = 1, /* passive bulk request(PUT sink/GET source)
+				   * received */
 	SRPC_BULK_PUT_SENT   = 2, /* active bulk PUT sent (source) */
 	SRPC_BULK_GET_RPLD   = 3, /* active bulk GET replied (sink) */
 	SRPC_REPLY_RCVD      = 4, /* incoming reply received */
@@ -149,114 +150,114 @@ typedef enum {
 typedef struct {
 	srpc_event_type_t ev_type;   /* what's up */
 	lnet_event_kind_t ev_lnet;   /* LNet event type */
-	int	       ev_fired;  /* LNet event fired? */
-	int	       ev_status; /* LNet event status */
-	void	     *ev_data;   /* owning server/client RPC */
+	int               ev_fired;  /* LNet event fired? */
+	int               ev_status; /* LNet event status */
+	void              *ev_data;  /* owning server/client RPC */
 } srpc_event_t;
 
 typedef struct {
-	int	      bk_len;  /* len of bulk data */
+	int              bk_len;     /* len of bulk data */
 	lnet_handle_md_t bk_mdh;
-	int	      bk_sink; /* sink/source */
-	int	      bk_niov; /* # iov in bk_iovs */
+	int              bk_sink;    /* sink/source */
+	int              bk_niov;    /* # iov in bk_iovs */
 	lnet_kiov_t      bk_iovs[0];
 } srpc_bulk_t; /* bulk descriptor */
 
 /* message buffer descriptor */
 typedef struct srpc_buffer {
-	struct list_head	   buf_list; /* chain on srpc_service::*_msgq */
-	srpc_msg_t	   buf_msg;
-	lnet_handle_md_t     buf_mdh;
-	lnet_nid_t	   buf_self;
-	lnet_process_id_t    buf_peer;
+	struct list_head  buf_list; /* chain on srpc_service::*_msgq */
+	srpc_msg_t        buf_msg;
+	lnet_handle_md_t  buf_mdh;
+	lnet_nid_t        buf_self;
+	lnet_process_id_t buf_peer;
 } srpc_buffer_t;
 
 struct swi_workitem;
 typedef int (*swi_action_t) (struct swi_workitem *);
 
 typedef struct swi_workitem {
-	struct cfs_wi_sched	*swi_sched;
-	cfs_workitem_t       swi_workitem;
-	swi_action_t	 swi_action;
-	int		  swi_state;
+	struct cfs_wi_sched *swi_sched;
+	cfs_workitem_t      swi_workitem;
+	swi_action_t        swi_action;
+	int                 swi_state;
 } swi_workitem_t;
 
 /* server-side state of a RPC */
 typedef struct srpc_server_rpc {
 	/* chain on srpc_service::*_rpcq */
-	struct list_head		srpc_list;
+	struct list_head       srpc_list;
 	struct srpc_service_cd *srpc_scd;
-	swi_workitem_t       srpc_wi;
-	srpc_event_t	 srpc_ev;      /* bulk/reply event */
-	lnet_nid_t	   srpc_self;
-	lnet_process_id_t    srpc_peer;
-	srpc_msg_t	   srpc_replymsg;
-	lnet_handle_md_t     srpc_replymdh;
-	srpc_buffer_t       *srpc_reqstbuf;
-	srpc_bulk_t	 *srpc_bulk;
-
-	unsigned int	 srpc_aborted; /* being given up */
-	int		  srpc_status;
-	void	       (*srpc_done)(struct srpc_server_rpc *);
+	swi_workitem_t         srpc_wi;
+	srpc_event_t           srpc_ev;      /* bulk/reply event */
+	lnet_nid_t             srpc_self;
+	lnet_process_id_t      srpc_peer;
+	srpc_msg_t             srpc_replymsg;
+	lnet_handle_md_t       srpc_replymdh;
+	srpc_buffer_t          *srpc_reqstbuf;
+	srpc_bulk_t            *srpc_bulk;
+
+	unsigned int           srpc_aborted; /* being given up */
+	int                    srpc_status;
+	void                   (*srpc_done)(struct srpc_server_rpc *);
 } srpc_server_rpc_t;
 
 /* client-side state of a RPC */
 typedef struct srpc_client_rpc {
-	struct list_head		crpc_list;	/* chain on user's lists */
-	spinlock_t		crpc_lock;	/* serialize */
-	int		  crpc_service;
-	atomic_t	 crpc_refcount;
-	int		  crpc_timeout; /* # seconds to wait for reply */
-	stt_timer_t	  crpc_timer;
-	swi_workitem_t       crpc_wi;
-	lnet_process_id_t    crpc_dest;
-
-	void	       (*crpc_done)(struct srpc_client_rpc *);
-	void	       (*crpc_fini)(struct srpc_client_rpc *);
-	int		  crpc_status;    /* completion status */
-	void		*crpc_priv;      /* caller data */
+	struct list_head  crpc_list;      /* chain on user's lists */
+	spinlock_t        crpc_lock;      /* serialize */
+	int               crpc_service;
+	atomic_t          crpc_refcount;
+	int               crpc_timeout;   /* # seconds to wait for reply */
+	stt_timer_t       crpc_timer;
+	swi_workitem_t    crpc_wi;
+	lnet_process_id_t crpc_dest;
+
+	void              (*crpc_done)(struct srpc_client_rpc *);
+	void              (*crpc_fini)(struct srpc_client_rpc *);
+	int               crpc_status;    /* completion status */
+	void              *crpc_priv;     /* caller data */
 
 	/* state flags */
-	unsigned int	 crpc_aborted:1; /* being given up */
-	unsigned int	 crpc_closed:1;  /* completed */
+	unsigned int      crpc_aborted:1; /* being given up */
+	unsigned int      crpc_closed:1;  /* completed */
 
 	/* RPC events */
-	srpc_event_t	 crpc_bulkev;    /* bulk event */
-	srpc_event_t	 crpc_reqstev;   /* request event */
-	srpc_event_t	 crpc_replyev;   /* reply event */
+	srpc_event_t      crpc_bulkev;    /* bulk event */
+	srpc_event_t      crpc_reqstev;   /* request event */
+	srpc_event_t      crpc_replyev;   /* reply event */
 
 	/* bulk, request(reqst), and reply exchanged on wire */
-	srpc_msg_t	   crpc_reqstmsg;
-	srpc_msg_t	   crpc_replymsg;
-	lnet_handle_md_t     crpc_reqstmdh;
-	lnet_handle_md_t     crpc_replymdh;
-	srpc_bulk_t	  crpc_bulk;
+	srpc_msg_t        crpc_reqstmsg;
+	srpc_msg_t        crpc_replymsg;
+	lnet_handle_md_t  crpc_reqstmdh;
+	lnet_handle_md_t  crpc_replymdh;
+	srpc_bulk_t       crpc_bulk;
 } srpc_client_rpc_t;
 
-#define srpc_client_rpc_size(rpc)				       \
+#define srpc_client_rpc_size(rpc)                                        \
 offsetof(srpc_client_rpc_t, crpc_bulk.bk_iovs[(rpc)->crpc_bulk.bk_niov])
 
-#define srpc_client_rpc_addref(rpc)				     \
-do {								    \
-	CDEBUG(D_NET, "RPC[%p] -> %s (%d)++\n",			 \
-	       (rpc), libcfs_id2str((rpc)->crpc_dest),		  \
-	       atomic_read(&(rpc)->crpc_refcount));		 \
-	LASSERT(atomic_read(&(rpc)->crpc_refcount) > 0);	    \
-	atomic_inc(&(rpc)->crpc_refcount);			  \
+#define srpc_client_rpc_addref(rpc)                                      \
+do {                                                                     \
+	CDEBUG(D_NET, "RPC[%p] -> %s (%d)++\n",                          \
+	       (rpc), libcfs_id2str((rpc)->crpc_dest),                   \
+	       atomic_read(&(rpc)->crpc_refcount));                      \
+	LASSERT(atomic_read(&(rpc)->crpc_refcount) > 0);                 \
+	atomic_inc(&(rpc)->crpc_refcount);                               \
 } while (0)
 
-#define srpc_client_rpc_decref(rpc)				     \
-do {								    \
-	CDEBUG(D_NET, "RPC[%p] -> %s (%d)--\n",			 \
-	       (rpc), libcfs_id2str((rpc)->crpc_dest),		  \
-	       atomic_read(&(rpc)->crpc_refcount));		 \
-	LASSERT(atomic_read(&(rpc)->crpc_refcount) > 0);	    \
-	if (atomic_dec_and_test(&(rpc)->crpc_refcount))	     \
-		srpc_destroy_client_rpc(rpc);			   \
+#define srpc_client_rpc_decref(rpc)                                      \
+do {                                                                     \
+	CDEBUG(D_NET, "RPC[%p] -> %s (%d)--\n",                          \
+	       (rpc), libcfs_id2str((rpc)->crpc_dest),                   \
+	       atomic_read(&(rpc)->crpc_refcount));                      \
+	LASSERT(atomic_read(&(rpc)->crpc_refcount) > 0);                 \
+	if (atomic_dec_and_test(&(rpc)->crpc_refcount))                  \
+		srpc_destroy_client_rpc(rpc);                            \
 } while (0)
 
-#define srpc_event_pending(rpc)   ((rpc)->crpc_bulkev.ev_fired == 0 ||  \
-				   (rpc)->crpc_reqstev.ev_fired == 0 || \
+#define srpc_event_pending(rpc)   ((rpc)->crpc_bulkev.ev_fired == 0 ||   \
+				   (rpc)->crpc_reqstev.ev_fired == 0 ||  \
 				   (rpc)->crpc_replyev.ev_fired == 0)
 
 /* CPU partition data of srpc service */
@@ -268,9 +269,9 @@ struct srpc_service_cd {
 	/** event buffer */
 	srpc_event_t		scd_ev;
 	/** free RPC descriptors */
-	struct list_head		scd_rpc_free;
+	struct list_head        scd_rpc_free;
 	/** in-flight RPCs */
-	struct list_head		scd_rpc_active;
+	struct list_head        scd_rpc_active;
 	/** workitem for posting buffer */
 	swi_workitem_t		scd_buf_wi;
 	/** CPT id */
@@ -278,7 +279,7 @@ struct srpc_service_cd {
 	/** error code for scd_buf_wi */
 	int			scd_buf_err;
 	/** timestamp for scd_buf_err */
-	unsigned long	   scd_buf_err_stamp;
+	unsigned long           scd_buf_err_stamp;
 	/** total # request buffers */
 	int			scd_buf_total;
 	/** # posted request buffers */
@@ -290,9 +291,9 @@ struct srpc_service_cd {
 	/** increase/decrease some buffers */
 	int			scd_buf_adjust;
 	/** posted message buffers */
-	struct list_head		scd_buf_posted;
+	struct list_head        scd_buf_posted;
 	/** blocked for RPC descriptor */
-	struct list_head		scd_buf_blocked;
+	struct list_head        scd_buf_blocked;
 };
 
 /* number of server workitems (mini-thread) for testing service */
@@ -318,40 +319,42 @@ typedef struct srpc_service {
 	 * - sv_handler: process incoming RPC request
 	 * - sv_bulk_ready: notify bulk data
 	 */
-	int	      (*sv_handler) (srpc_server_rpc_t *);
-	int	      (*sv_bulk_ready) (srpc_server_rpc_t *, int);
+	int                     (*sv_handler) (srpc_server_rpc_t *);
+	int                     (*sv_bulk_ready) (srpc_server_rpc_t *, int);
 } srpc_service_t;
 
 typedef struct {
-	struct list_head	sn_list;    /* chain on fw_zombie_sessions */
-	lst_sid_t	 sn_id;      /* unique identifier */
-	unsigned int      sn_timeout; /* # seconds' inactivity to expire */
-	int	       sn_timer_active;
-	unsigned int	  sn_features;
-	stt_timer_t       sn_timer;
-	struct list_head	sn_batches; /* list of batches */
-	char	      sn_name[LST_NAME_SIZE];
-	atomic_t      sn_refcount;
-	atomic_t      sn_brw_errors;
-	atomic_t      sn_ping_errors;
-	unsigned long	sn_started;
+	struct list_head sn_list;    /* chain on fw_zombie_sessions */
+	lst_sid_t        sn_id;      /* unique identifier */
+	unsigned int     sn_timeout; /* # seconds' inactivity to expire */
+	int              sn_timer_active;
+	unsigned int     sn_features;
+	stt_timer_t      sn_timer;
+	struct list_head sn_batches; /* list of batches */
+	char             sn_name[LST_NAME_SIZE];
+	atomic_t         sn_refcount;
+	atomic_t         sn_brw_errors;
+	atomic_t         sn_ping_errors;
+	unsigned long    sn_started;
 } sfw_session_t;
 
 #define sfw_sid_equal(sid0, sid1)     ((sid0).ses_nid == (sid1).ses_nid && \
 				       (sid0).ses_stamp == (sid1).ses_stamp)
 
 typedef struct {
-	struct list_head	bat_list;      /* chain on sn_batches */
-	lst_bid_t	 bat_id;	/* batch id */
-	int	       bat_error;     /* error code of batch */
-	sfw_session_t    *bat_session;   /* batch's session */
-	atomic_t      bat_nactive;   /* # of active tests */
-	struct list_head	bat_tests;     /* test instances */
+	struct list_head bat_list;      /* chain on sn_batches */
+	lst_bid_t        bat_id;	/* batch id */
+	int              bat_error;     /* error code of batch */
+	sfw_session_t    *bat_session;  /* batch's session */
+	atomic_t         bat_nactive;   /* # of active tests */
+	struct list_head bat_tests;     /* test instances */
 } sfw_batch_t;
 
 typedef struct {
-	int  (*tso_init)(struct sfw_test_instance *tsi); /* initialize test client */
-	void (*tso_fini)(struct sfw_test_instance *tsi); /* finalize test client */
+	int  (*tso_init)(struct sfw_test_instance *tsi); /* initialize test
+							  * client */
+	void (*tso_fini)(struct sfw_test_instance *tsi); /* finalize test
+							  * client */
 	int  (*tso_prep_rpc)(struct sfw_test_unit *tsu,
 			     lnet_process_id_t dest,
 			     srpc_client_rpc_t **rpc);   /* prep a tests rpc */
@@ -360,29 +363,31 @@ typedef struct {
 } sfw_test_client_ops_t;
 
 typedef struct sfw_test_instance {
-	struct list_head	      tsi_list;	 /* chain on batch */
-	int		     tsi_service;      /* test type */
-	sfw_batch_t	    *tsi_batch;	/* batch */
-	sfw_test_client_ops_t  *tsi_ops;	  /* test client operations */
+	struct list_head           tsi_list;            /* chain on batch */
+	int                        tsi_service;         /* test type */
+	sfw_batch_t                *tsi_batch;          /* batch */
+	sfw_test_client_ops_t      *tsi_ops;            /* test client operation
+							 */
 
 	/* public parameter for all test units */
-	unsigned int		tsi_is_client:1;     /* is test client */
-	unsigned int		tsi_stoptsu_onerr:1; /* stop tsu on error */
-	int		     tsi_concur;	  /* concurrency */
-	int		     tsi_loop;	    /* loop count */
+	unsigned int               tsi_is_client:1;     /* is test client */
+	unsigned int               tsi_stoptsu_onerr:1; /* stop tsu on error */
+	int                        tsi_concur;          /* concurrency */
+	int                        tsi_loop;            /* loop count */
 
 	/* status of test instance */
-	spinlock_t		tsi_lock;	  /* serialize */
-	unsigned int		tsi_stopping:1;   /* test is stopping */
-	atomic_t	    tsi_nactive;      /* # of active test unit */
-	struct list_head	      tsi_units;	/* test units */
-	struct list_head	      tsi_free_rpcs;    /* free rpcs */
-	struct list_head	      tsi_active_rpcs;  /* active rpcs */
+	spinlock_t                 tsi_lock;            /* serialize */
+	unsigned int               tsi_stopping:1;      /* test is stopping */
+	atomic_t                   tsi_nactive;         /* # of active test
+							 * unit */
+	struct list_head           tsi_units;           /* test units */
+	struct list_head           tsi_free_rpcs;       /* free rpcs */
+	struct list_head           tsi_active_rpcs;     /* active rpcs */
 
 	union {
-		test_ping_req_t		ping;	  /* ping parameter */
-		test_bulk_req_t		bulk_v0;  /* bulk parameter */
-		test_bulk_req_v1_t	bulk_v1;  /* bulk v1 parameter */
+		test_ping_req_t	   ping;    /* ping parameter */
+		test_bulk_req_t    bulk_v0; /* bulk parameter */
+		test_bulk_req_v1_t bulk_v1; /* bulk v1 parameter */
 	} tsi_u;
 } sfw_test_instance_t;
 
@@ -394,18 +399,18 @@ typedef struct sfw_test_instance {
 #define sfw_id_pages(n)    (((n) + SFW_ID_PER_PAGE - 1) / SFW_ID_PER_PAGE)
 
 typedef struct sfw_test_unit {
-	struct list_head	    tsu_list;	 /* chain on lst_test_instance */
-	lnet_process_id_t     tsu_dest;	 /* id of dest node */
-	int		   tsu_loop;	 /* loop count of the test */
-	sfw_test_instance_t  *tsu_instance;     /* pointer to test instance */
-	void		 *tsu_private;      /* private data */
-	swi_workitem_t	tsu_worker;       /* workitem of the test unit */
+	struct list_head    tsu_list;      /* chain on lst_test_instance */
+	lnet_process_id_t   tsu_dest;      /* id of dest node */
+	int                 tsu_loop;      /* loop count of the test */
+	sfw_test_instance_t *tsu_instance; /* pointer to test instance */
+	void                *tsu_private;  /* private data */
+	swi_workitem_t      tsu_worker;    /* workitem of the test unit */
 } sfw_test_unit_t;
 
 typedef struct sfw_test_case {
-	struct list_head	      tsc_list;	 /* chain on fw_tests */
-	srpc_service_t	 *tsc_srv_service;  /* test service */
-	sfw_test_client_ops_t  *tsc_cli_ops;      /* ops of test client */
+	struct list_head      tsc_list;         /* chain on fw_tests */
+	srpc_service_t        *tsc_srv_service; /* test service */
+	sfw_test_client_ops_t *tsc_cli_ops;     /* ops of test client */
 } sfw_test_case_t;
 
 srpc_client_rpc_t *
@@ -501,9 +506,9 @@ void srpc_shutdown(void);
 static inline void
 srpc_destroy_client_rpc (srpc_client_rpc_t *rpc)
 {
-	LASSERT (rpc != NULL);
-	LASSERT (!srpc_event_pending(rpc));
-	LASSERT (atomic_read(&rpc->crpc_refcount) == 0);
+	LASSERT(rpc != NULL);
+	LASSERT(!srpc_event_pending(rpc));
+	LASSERT(atomic_read(&rpc->crpc_refcount) == 0);
 
 	if (rpc->crpc_fini == NULL) {
 		LIBCFS_FREE(rpc, srpc_client_rpc_size(rpc));
@@ -520,7 +525,7 @@ srpc_init_client_rpc (srpc_client_rpc_t *rpc, lnet_process_id_t peer,
 		      void (*rpc_done)(srpc_client_rpc_t *),
 		      void (*rpc_fini)(srpc_client_rpc_t *), void *priv)
 {
-	LASSERT (nbulkiov <= LNET_MAX_IOV);
+	LASSERT(nbulkiov <= LNET_MAX_IOV);
 
 	memset(rpc, 0, offsetof(srpc_client_rpc_t,
 				crpc_bulk.bk_iovs[nbulkiov]));
@@ -531,13 +536,13 @@ srpc_init_client_rpc (srpc_client_rpc_t *rpc, lnet_process_id_t peer,
 	spin_lock_init(&rpc->crpc_lock);
 	atomic_set(&rpc->crpc_refcount, 1); /* 1 ref for caller */
 
-	rpc->crpc_dest	 = peer;
-	rpc->crpc_priv	 = priv;
+	rpc->crpc_dest         = peer;
+	rpc->crpc_priv         = priv;
 	rpc->crpc_service      = service;
 	rpc->crpc_bulk.bk_len  = bulklen;
 	rpc->crpc_bulk.bk_niov = nbulkiov;
-	rpc->crpc_done	 = rpc_done;
-	rpc->crpc_fini	 = rpc_fini;
+	rpc->crpc_done         = rpc_done;
+	rpc->crpc_fini         = rpc_fini;
 	LNetInvalidateHandle(&rpc->crpc_reqstmdh);
 	LNetInvalidateHandle(&rpc->crpc_replymdh);
 	LNetInvalidateHandle(&rpc->crpc_bulk.bk_mdh);
diff --git a/drivers/staging/lustre/lnet/selftest/timer.c b/drivers/staging/lustre/lnet/selftest/timer.c
index 441f9472a834..6133b54f4a82 100644
--- a/drivers/staging/lustre/lnet/selftest/timer.c
+++ b/drivers/staging/lustre/lnet/selftest/timer.c
@@ -50,7 +50,7 @@
  * sorted by increasing expiry time. The number of slots is 2**7 (128),
  * to cover a time period of 1024 seconds into the future before wrapping.
  */
-#define STTIMER_MINPOLL	3   /* log2 min poll interval (8 s) */
+#define STTIMER_MINPOLL        3   /* log2 min poll interval (8 s) */
 #define STTIMER_SLOTTIME       (1 << STTIMER_MINPOLL)
 #define STTIMER_SLOTTIMEMASK   (~(STTIMER_SLOTTIME - 1))
 #define STTIMER_NSLOTS	       (1 << 7)
@@ -58,13 +58,13 @@
 						    (STTIMER_NSLOTS - 1))])
 
 static struct st_timer_data {
-	spinlock_t	 stt_lock;
-	/* start time of the slot processed previously */
-	unsigned long       stt_prev_slot;
-	struct list_head       stt_hash[STTIMER_NSLOTS];
-	int	      stt_shuttingdown;
-	wait_queue_head_t      stt_waitq;
-	int	      stt_nthreads;
+	spinlock_t        stt_lock;
+	unsigned long     stt_prev_slot; /* start time of the slot processed
+					  * previously */
+	struct list_head  stt_hash[STTIMER_NSLOTS];
+	int               stt_shuttingdown;
+	wait_queue_head_t stt_waitq;
+	int               stt_nthreads;
 } stt_data;
 
 void
@@ -124,7 +124,7 @@ stt_del_timer(stt_timer_t *timer)
 static int
 stt_expire_list(struct list_head *slot, unsigned long now)
 {
-	int	  expired = 0;
+	int expired = 0;
 	stt_timer_t *timer;
 
 	while (!list_empty(slot)) {
@@ -148,7 +148,7 @@ stt_expire_list(struct list_head *slot, unsigned long now)
 static int
 stt_check_timers(unsigned long *last)
 {
-	int	expired = 0;
+	int expired = 0;
 	unsigned long now;
 	unsigned long this_slot;
 
diff --git a/drivers/staging/lustre/lnet/selftest/timer.h b/drivers/staging/lustre/lnet/selftest/timer.h
index d727c1e2b0ce..2a8803d89de4 100644
--- a/drivers/staging/lustre/lnet/selftest/timer.h
+++ b/drivers/staging/lustre/lnet/selftest/timer.h
@@ -39,15 +39,15 @@
 #define __SELFTEST_TIMER_H__
 
 typedef struct {
-	struct list_head	stt_list;
-	unsigned long	stt_expires;
-	void	    (*stt_func) (void *);
-	void	     *stt_data;
+	struct list_head stt_list;
+	unsigned long    stt_expires;
+	void             (*stt_func) (void *);
+	void             *stt_data;
 } stt_timer_t;
 
-void stt_add_timer (stt_timer_t *timer);
-int stt_del_timer (stt_timer_t *timer);
-int stt_startup (void);
-void stt_shutdown (void);
+void stt_add_timer(stt_timer_t *timer);
+int stt_del_timer(stt_timer_t *timer);
+int stt_startup(void);
+void stt_shutdown(void);
 
 #endif /* __SELFTEST_TIMER_H__ */
diff --git a/drivers/staging/lustre/lustre/fid/Makefile b/drivers/staging/lustre/lustre/fid/Makefile
index 5513ce416a35..b7ef314b4b84 100644
--- a/drivers/staging/lustre/lustre/fid/Makefile
+++ b/drivers/staging/lustre/lustre/fid/Makefile
@@ -1,3 +1,2 @@
 obj-$(CONFIG_LUSTRE_FS) += fid.o
-fid-y := fid_request.o fid_lib.o
-fid-$(CONFIG_PROC_FS) += lproc_fid.o
+fid-y := fid_request.o fid_lib.o lproc_fid.o
diff --git a/drivers/staging/lustre/lustre/fid/fid_internal.h b/drivers/staging/lustre/lustre/fid/fid_internal.h
index b5e8da8956f2..84daee1154dc 100644
--- a/drivers/staging/lustre/lustre/fid/fid_internal.h
+++ b/drivers/staging/lustre/lustre/fid/fid_internal.h
@@ -47,10 +47,6 @@
 int seq_client_alloc_super(struct lu_client_seq *seq,
 			   const struct lu_env *env);
 
-#if defined(CONFIG_PROC_FS)
-extern struct lprocfs_vars seq_client_proc_list[];
-#endif
-
-extern struct proc_dir_entry *seq_type_proc_dir;
+extern struct lprocfs_vars seq_client_debugfs_list[];
 
 #endif /* __FID_INTERNAL_H */
diff --git a/drivers/staging/lustre/lustre/fid/fid_request.c b/drivers/staging/lustre/lustre/fid/fid_request.c
index 063441abfcfc..1362783b7eab 100644
--- a/drivers/staging/lustre/lustre/fid/fid_request.c
+++ b/drivers/staging/lustre/lustre/fid/fid_request.c
@@ -53,6 +53,8 @@
 #include "../include/lustre_mdc.h"
 #include "fid_internal.h"
 
+static struct dentry *seq_debugfs_dir;
+
 static int seq_client_rpc(struct lu_client_seq *seq,
 			  struct lu_seq_range *output, __u32 opc,
 			  const char *opcname)
@@ -201,10 +203,9 @@ static int seq_client_alloc_seq(const struct lu_env *env,
 			CERROR("%s: Can't allocate new meta-sequence, rc %d\n",
 			       seq->lcs_name, rc);
 			return rc;
-		} else {
-			CDEBUG(D_INFO, "%s: New range - "DRANGE"\n",
-			       seq->lcs_name, PRANGE(&seq->lcs_space));
 		}
+		CDEBUG(D_INFO, "%s: New range - "DRANGE"\n",
+		       seq->lcs_name, PRANGE(&seq->lcs_space));
 	} else {
 		rc = 0;
 	}
@@ -400,37 +401,32 @@ void seq_client_flush(struct lu_client_seq *seq)
 }
 EXPORT_SYMBOL(seq_client_flush);
 
-static void seq_client_proc_fini(struct lu_client_seq *seq)
+static void seq_client_debugfs_fini(struct lu_client_seq *seq)
 {
-#if defined(CONFIG_PROC_FS)
-	if (seq->lcs_proc_dir) {
-		if (!IS_ERR(seq->lcs_proc_dir))
-			lprocfs_remove(&seq->lcs_proc_dir);
-		seq->lcs_proc_dir = NULL;
-	}
-#endif /* CONFIG_PROC_FS */
+	if (!IS_ERR_OR_NULL(seq->lcs_debugfs_entry))
+		ldebugfs_remove(&seq->lcs_debugfs_entry);
 }
 
-static int seq_client_proc_init(struct lu_client_seq *seq)
+static int seq_client_debugfs_init(struct lu_client_seq *seq)
 {
-#if defined(CONFIG_PROC_FS)
 	int rc;
 
-	seq->lcs_proc_dir = lprocfs_register(seq->lcs_name,
-					     seq_type_proc_dir,
-					     NULL, NULL);
+	seq->lcs_debugfs_entry = ldebugfs_register(seq->lcs_name,
+						   seq_debugfs_dir,
+						   NULL, NULL);
 
-	if (IS_ERR(seq->lcs_proc_dir)) {
-		CERROR("%s: LProcFS failed in seq-init\n",
-		       seq->lcs_name);
-		rc = PTR_ERR(seq->lcs_proc_dir);
+	if (IS_ERR_OR_NULL(seq->lcs_debugfs_entry)) {
+		CERROR("%s: LdebugFS failed in seq-init\n", seq->lcs_name);
+		rc = seq->lcs_debugfs_entry ? PTR_ERR(seq->lcs_debugfs_entry)
+					    : -ENOMEM;
+		seq->lcs_debugfs_entry = NULL;
 		return rc;
 	}
 
-	rc = lprocfs_add_vars(seq->lcs_proc_dir,
-			      seq_client_proc_list, seq);
+	rc = ldebugfs_add_vars(seq->lcs_debugfs_entry,
+			       seq_client_debugfs_list, seq);
 	if (rc) {
-		CERROR("%s: Can't init sequence manager proc, rc %d\n",
+		CERROR("%s: Can't init sequence manager debugfs, rc %d\n",
 		       seq->lcs_name, rc);
 		goto out_cleanup;
 	}
@@ -438,12 +434,8 @@ static int seq_client_proc_init(struct lu_client_seq *seq)
 	return 0;
 
 out_cleanup:
-	seq_client_proc_fini(seq);
+	seq_client_debugfs_fini(seq);
 	return rc;
-
-#else /* CONFIG_PROC_FS */
-	return 0;
-#endif
 }
 
 int seq_client_init(struct lu_client_seq *seq,
@@ -478,7 +470,7 @@ int seq_client_init(struct lu_client_seq *seq,
 	snprintf(seq->lcs_name, sizeof(seq->lcs_name),
 		 "cli-%s", prefix);
 
-	rc = seq_client_proc_init(seq);
+	rc = seq_client_debugfs_init(seq);
 	if (rc)
 		seq_client_fini(seq);
 	return rc;
@@ -487,7 +479,7 @@ EXPORT_SYMBOL(seq_client_init);
 
 void seq_client_fini(struct lu_client_seq *seq)
 {
-	seq_client_proc_fini(seq);
+	seq_client_debugfs_fini(seq);
 
 	if (seq->lcs_exp != NULL) {
 		class_export_put(seq->lcs_exp);
@@ -505,11 +497,11 @@ int client_fid_init(struct obd_device *obd,
 	char *prefix;
 	int rc;
 
-	OBD_ALLOC_PTR(cli->cl_seq);
+	cli->cl_seq = kzalloc(sizeof(*cli->cl_seq), GFP_NOFS);
 	if (cli->cl_seq == NULL)
 		return -ENOMEM;
 
-	OBD_ALLOC(prefix, MAX_OBD_NAME + 5);
+	prefix = kzalloc(MAX_OBD_NAME + 5, GFP_NOFS);
 	if (prefix == NULL) {
 		rc = -ENOMEM;
 		goto out_free_seq;
@@ -519,13 +511,13 @@ int client_fid_init(struct obd_device *obd,
 
 	/* Init client side sequence-manager */
 	rc = seq_client_init(cli->cl_seq, exp, type, prefix, NULL);
-	OBD_FREE(prefix, MAX_OBD_NAME + 5);
+	kfree(prefix);
 	if (rc)
 		goto out_free_seq;
 
 	return rc;
 out_free_seq:
-	OBD_FREE_PTR(cli->cl_seq);
+	kfree(cli->cl_seq);
 	cli->cl_seq = NULL;
 	return rc;
 }
@@ -537,7 +529,7 @@ int client_fid_fini(struct obd_device *obd)
 
 	if (cli->cl_seq != NULL) {
 		seq_client_fini(cli->cl_seq);
-		OBD_FREE_PTR(cli->cl_seq);
+		kfree(cli->cl_seq);
 		cli->cl_seq = NULL;
 	}
 
@@ -545,22 +537,18 @@ int client_fid_fini(struct obd_device *obd)
 }
 EXPORT_SYMBOL(client_fid_fini);
 
-struct proc_dir_entry *seq_type_proc_dir;
-
 static int __init fid_mod_init(void)
 {
-	seq_type_proc_dir = lprocfs_register(LUSTRE_SEQ_NAME,
-					     proc_lustre_root,
-					     NULL, NULL);
-	return PTR_ERR_OR_ZERO(seq_type_proc_dir);
+	seq_debugfs_dir = ldebugfs_register(LUSTRE_SEQ_NAME,
+					    debugfs_lustre_root,
+					    NULL, NULL);
+	return PTR_ERR_OR_ZERO(seq_debugfs_dir);
 }
 
 static void __exit fid_mod_exit(void)
 {
-	if (seq_type_proc_dir != NULL && !IS_ERR(seq_type_proc_dir)) {
-		lprocfs_remove(&seq_type_proc_dir);
-		seq_type_proc_dir = NULL;
-	}
+	if (!IS_ERR_OR_NULL(seq_debugfs_dir))
+		ldebugfs_remove(&seq_debugfs_dir);
 }
 
 MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
diff --git a/drivers/staging/lustre/lustre/fid/lproc_fid.c b/drivers/staging/lustre/lustre/fid/lproc_fid.c
index 783939dbd4db..cc2201c25339 100644
--- a/drivers/staging/lustre/lustre/fid/lproc_fid.c
+++ b/drivers/staging/lustre/lustre/fid/lproc_fid.c
@@ -59,8 +59,9 @@
  * Note: this function is only used for testing, it is no safe for production
  * use.
  */
-static int lprocfs_fid_write_common(const char __user *buffer, size_t count,
-				    struct lu_seq_range *range)
+static int
+ldebugfs_fid_write_common(const char __user *buffer, size_t count,
+			  struct lu_seq_range *range)
 {
 	struct lu_seq_range tmp;
 	int rc;
@@ -92,10 +93,11 @@ static int lprocfs_fid_write_common(const char __user *buffer, size_t count,
 	return count;
 }
 
-/* Client side procfs stuff */
-static ssize_t lprocfs_fid_space_seq_write(struct file *file,
-					   const char __user *buffer,
-					   size_t count, loff_t *off)
+/* Client side debugfs stuff */
+static ssize_t
+ldebugfs_fid_space_seq_write(struct file *file,
+			     const char __user *buffer,
+			     size_t count, loff_t *off)
 {
 	struct lu_client_seq *seq;
 	int rc;
@@ -104,7 +106,7 @@ static ssize_t lprocfs_fid_space_seq_write(struct file *file,
 	LASSERT(seq != NULL);
 
 	mutex_lock(&seq->lcs_mutex);
-	rc = lprocfs_fid_write_common(buffer, count, &seq->lcs_space);
+	rc = ldebugfs_fid_write_common(buffer, count, &seq->lcs_space);
 
 	if (rc == 0) {
 		CDEBUG(D_INFO, "%s: Space: "DRANGE"\n",
@@ -117,7 +119,7 @@ static ssize_t lprocfs_fid_space_seq_write(struct file *file,
 }
 
 static int
-lprocfs_fid_space_seq_show(struct seq_file *m, void *unused)
+ldebugfs_fid_space_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_seq *seq = (struct lu_client_seq *)m->private;
 
@@ -130,9 +132,10 @@ lprocfs_fid_space_seq_show(struct seq_file *m, void *unused)
 	return 0;
 }
 
-static ssize_t lprocfs_fid_width_seq_write(struct file *file,
-					   const char __user *buffer,
-					   size_t count, loff_t *off)
+static ssize_t
+ldebugfs_fid_width_seq_write(struct file *file,
+			     const char __user *buffer,
+			     size_t count, loff_t *off)
 {
 	struct lu_client_seq *seq;
 	__u64  max;
@@ -154,10 +157,8 @@ static ssize_t lprocfs_fid_width_seq_write(struct file *file,
 	if (val <= max && val > 0) {
 		seq->lcs_width = val;
 
-		if (rc == 0) {
-			CDEBUG(D_INFO, "%s: Sequence size: %llu\n",
-			       seq->lcs_name, seq->lcs_width);
-		}
+		CDEBUG(D_INFO, "%s: Sequence size: %llu\n", seq->lcs_name,
+		       seq->lcs_width);
 	}
 
 	mutex_unlock(&seq->lcs_mutex);
@@ -166,7 +167,7 @@ static ssize_t lprocfs_fid_width_seq_write(struct file *file,
 }
 
 static int
-lprocfs_fid_width_seq_show(struct seq_file *m, void *unused)
+ldebugfs_fid_width_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_seq *seq = (struct lu_client_seq *)m->private;
 
@@ -180,7 +181,7 @@ lprocfs_fid_width_seq_show(struct seq_file *m, void *unused)
 }
 
 static int
-lprocfs_fid_fid_seq_show(struct seq_file *m, void *unused)
+ldebugfs_fid_fid_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_seq *seq = (struct lu_client_seq *)m->private;
 
@@ -194,7 +195,7 @@ lprocfs_fid_fid_seq_show(struct seq_file *m, void *unused)
 }
 
 static int
-lprocfs_fid_server_seq_show(struct seq_file *m, void *unused)
+ldebugfs_fid_server_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_seq *seq = (struct lu_client_seq *)m->private;
 	struct client_obd *cli;
@@ -211,15 +212,15 @@ lprocfs_fid_server_seq_show(struct seq_file *m, void *unused)
 	return 0;
 }
 
-LPROC_SEQ_FOPS(lprocfs_fid_space);
-LPROC_SEQ_FOPS(lprocfs_fid_width);
-LPROC_SEQ_FOPS_RO(lprocfs_fid_server);
-LPROC_SEQ_FOPS_RO(lprocfs_fid_fid);
+LPROC_SEQ_FOPS(ldebugfs_fid_space);
+LPROC_SEQ_FOPS(ldebugfs_fid_width);
+LPROC_SEQ_FOPS_RO(ldebugfs_fid_server);
+LPROC_SEQ_FOPS_RO(ldebugfs_fid_fid);
 
-struct lprocfs_vars seq_client_proc_list[] = {
-	{ "space", &lprocfs_fid_space_fops },
-	{ "width", &lprocfs_fid_width_fops },
-	{ "server", &lprocfs_fid_server_fops },
-	{ "fid", &lprocfs_fid_fid_fops },
+struct lprocfs_vars seq_client_debugfs_list[] = {
+	{ "space", &ldebugfs_fid_space_fops },
+	{ "width", &ldebugfs_fid_width_fops },
+	{ "server", &ldebugfs_fid_server_fops },
+	{ "fid", &ldebugfs_fid_fid_fops },
 	{ NULL }
 };
diff --git a/drivers/staging/lustre/lustre/fld/Makefile b/drivers/staging/lustre/lustre/fld/Makefile
index 2bbf08433dca..646e315d1aa8 100644
--- a/drivers/staging/lustre/lustre/fld/Makefile
+++ b/drivers/staging/lustre/lustre/fld/Makefile
@@ -1,3 +1,2 @@
 obj-$(CONFIG_LUSTRE_FS) += fld.o
-fld-y := fld_request.o fld_cache.o
-fld-$(CONFIG_PROC_FS) += lproc_fld.o
+fld-y := fld_request.o fld_cache.o lproc_fld.o
diff --git a/drivers/staging/lustre/lustre/fld/fld_cache.c b/drivers/staging/lustre/lustre/fld/fld_cache.c
index 0d0a73745065..ec2fc4339a2e 100644
--- a/drivers/staging/lustre/lustre/fld/fld_cache.c
+++ b/drivers/staging/lustre/lustre/fld/fld_cache.c
@@ -69,7 +69,7 @@ struct fld_cache *fld_cache_init(const char *name,
 	LASSERT(name != NULL);
 	LASSERT(cache_threshold < cache_size);
 
-	OBD_ALLOC_PTR(cache);
+	cache = kzalloc(sizeof(*cache), GFP_NOFS);
 	if (cache == NULL)
 		return ERR_PTR(-ENOMEM);
 
@@ -116,7 +116,7 @@ void fld_cache_fini(struct fld_cache *cache)
 	CDEBUG(D_INFO, "  Cache reqs: %llu\n", cache->fci_stat.fst_cache);
 	CDEBUG(D_INFO, "  Cache hits: %llu%%\n", pct);
 
-	OBD_FREE_PTR(cache);
+	kfree(cache);
 }
 
 /**
@@ -128,7 +128,7 @@ void fld_cache_entry_delete(struct fld_cache *cache,
 	list_del(&node->fce_list);
 	list_del(&node->fce_lru);
 	cache->fci_cache_count--;
-	OBD_FREE_PTR(node);
+	kfree(node);
 }
 
 /**
@@ -268,7 +268,7 @@ static void fld_cache_punch_hole(struct fld_cache *cache,
 
 	OBD_ALLOC_GFP(fldt, sizeof(*fldt), GFP_ATOMIC);
 	if (!fldt) {
-		OBD_FREE_PTR(f_new);
+		kfree(f_new);
 		/* overlap is not allowed, so dont mess up list. */
 		return;
 	}
@@ -315,7 +315,7 @@ static void fld_cache_overlap_handle(struct fld_cache *cache,
 		f_curr->fce_range.lsr_end = max(f_curr->fce_range.lsr_end,
 						new_end);
 
-		OBD_FREE_PTR(f_new);
+		kfree(f_new);
 		fld_fix_new_list(cache);
 
 	} else if (new_start <= f_curr->fce_range.lsr_start &&
@@ -324,7 +324,7 @@ static void fld_cache_overlap_handle(struct fld_cache *cache,
 		 *	 e.g. whole range migrated. update fld cache entry */
 
 		f_curr->fce_range = *range;
-		OBD_FREE_PTR(f_new);
+		kfree(f_new);
 		fld_fix_new_list(cache);
 
 	} else if (f_curr->fce_range.lsr_start < new_start &&
@@ -364,7 +364,7 @@ struct fld_cache_entry
 
 	LASSERT(range_is_sane(range));
 
-	OBD_ALLOC_PTR(f_new);
+	f_new = kzalloc(sizeof(*f_new), GFP_NOFS);
 	if (!f_new)
 		return ERR_PTR(-ENOMEM);
 
@@ -440,7 +440,7 @@ int fld_cache_insert(struct fld_cache *cache,
 	rc = fld_cache_insert_nolock(cache, flde);
 	write_unlock(&cache->fci_lock);
 	if (rc)
-		OBD_FREE_PTR(flde);
+		kfree(flde);
 
 	return rc;
 }
diff --git a/drivers/staging/lustre/lustre/fld/fld_internal.h b/drivers/staging/lustre/lustre/fld/fld_internal.h
index 68bec7658463..844576b9bc6f 100644
--- a/drivers/staging/lustre/lustre/fld/fld_internal.h
+++ b/drivers/staging/lustre/lustre/fld/fld_internal.h
@@ -142,10 +142,7 @@ extern struct lu_fld_hash fld_hash[];
 int fld_client_rpc(struct obd_export *exp,
 		   struct lu_seq_range *range, __u32 fld_op);
 
-#if defined(CONFIG_PROC_FS)
-extern struct lprocfs_vars fld_client_proc_list[];
-#endif
-
+extern struct lprocfs_vars fld_client_debugfs_list[];
 
 struct fld_cache *fld_cache_init(const char *name,
 				 int cache_size, int cache_threshold);
diff --git a/drivers/staging/lustre/lustre/fld/fld_request.c b/drivers/staging/lustre/lustre/fld/fld_request.c
index 6ac225e90ee0..c3b47f2346df 100644
--- a/drivers/staging/lustre/lustre/fld/fld_request.c
+++ b/drivers/staging/lustre/lustre/fld/fld_request.c
@@ -221,7 +221,7 @@ int fld_client_add_target(struct lu_client_fld *fld,
 	CDEBUG(D_INFO, "%s: Adding target %s (idx %llu)\n",
 			fld->lcf_name, name, tar->ft_idx);
 
-	OBD_ALLOC_PTR(target);
+	target = kzalloc(sizeof(*target), GFP_NOFS);
 	if (target == NULL)
 		return -ENOMEM;
 
@@ -229,7 +229,7 @@ int fld_client_add_target(struct lu_client_fld *fld,
 	list_for_each_entry(tmp, &fld->lcf_targets, ft_chain) {
 		if (tmp->ft_idx == tar->ft_idx) {
 			spin_unlock(&fld->lcf_lock);
-			OBD_FREE_PTR(target);
+			kfree(target);
 			CERROR("Target %s exists in FLD and known as %s:#%llu\n",
 			       name, fld_target_name(tmp), tmp->ft_idx);
 			return -EEXIST;
@@ -268,7 +268,7 @@ int fld_client_del_target(struct lu_client_fld *fld, __u64 idx)
 			if (target->ft_exp != NULL)
 				class_export_put(target->ft_exp);
 
-			OBD_FREE_PTR(target);
+			kfree(target);
 			return 0;
 		}
 	}
@@ -277,58 +277,44 @@ int fld_client_del_target(struct lu_client_fld *fld, __u64 idx)
 }
 EXPORT_SYMBOL(fld_client_del_target);
 
-static struct proc_dir_entry *fld_type_proc_dir;
+static struct dentry *fld_debugfs_dir;
 
-#if defined(CONFIG_PROC_FS)
-static int fld_client_proc_init(struct lu_client_fld *fld)
+static int fld_client_debugfs_init(struct lu_client_fld *fld)
 {
 	int rc;
 
-	fld->lcf_proc_dir = lprocfs_register(fld->lcf_name,
-					     fld_type_proc_dir,
-					     NULL, NULL);
+	fld->lcf_debugfs_entry = ldebugfs_register(fld->lcf_name,
+						   fld_debugfs_dir,
+						   NULL, NULL);
 
-	if (IS_ERR(fld->lcf_proc_dir)) {
-		CERROR("%s: LProcFS failed in fld-init\n",
-		       fld->lcf_name);
-		rc = PTR_ERR(fld->lcf_proc_dir);
+	if (IS_ERR_OR_NULL(fld->lcf_debugfs_entry)) {
+		CERROR("%s: LdebugFS failed in fld-init\n", fld->lcf_name);
+		rc = fld->lcf_debugfs_entry ? PTR_ERR(fld->lcf_debugfs_entry)
+					    : -ENOMEM;
+		fld->lcf_debugfs_entry = NULL;
 		return rc;
 	}
 
-	rc = lprocfs_add_vars(fld->lcf_proc_dir,
-			      fld_client_proc_list, fld);
+	rc = ldebugfs_add_vars(fld->lcf_debugfs_entry,
+			       fld_client_debugfs_list, fld);
 	if (rc) {
-		CERROR("%s: Can't init FLD proc, rc %d\n",
-		       fld->lcf_name, rc);
+		CERROR("%s: Can't init FLD debufs, rc %d\n", fld->lcf_name, rc);
 		goto out_cleanup;
 	}
 
 	return 0;
 
 out_cleanup:
-	fld_client_proc_fini(fld);
+	fld_client_debugfs_fini(fld);
 	return rc;
 }
 
-void fld_client_proc_fini(struct lu_client_fld *fld)
+void fld_client_debugfs_fini(struct lu_client_fld *fld)
 {
-	if (fld->lcf_proc_dir) {
-		if (!IS_ERR(fld->lcf_proc_dir))
-			lprocfs_remove(&fld->lcf_proc_dir);
-		fld->lcf_proc_dir = NULL;
-	}
-}
-#else
-static int fld_client_proc_init(struct lu_client_fld *fld)
-{
-	return 0;
+	if (!IS_ERR_OR_NULL(fld->lcf_debugfs_entry))
+		ldebugfs_remove(&fld->lcf_debugfs_entry);
 }
-
-void fld_client_proc_fini(struct lu_client_fld *fld)
-{
-}
-#endif
-EXPORT_SYMBOL(fld_client_proc_fini);
+EXPORT_SYMBOL(fld_client_debugfs_fini);
 
 static inline int hash_is_sane(int hash)
 {
@@ -372,7 +358,7 @@ int fld_client_init(struct lu_client_fld *fld,
 		goto out;
 	}
 
-	rc = fld_client_proc_init(fld);
+	rc = fld_client_debugfs_init(fld);
 	if (rc)
 		goto out;
 out:
@@ -396,7 +382,7 @@ void fld_client_fini(struct lu_client_fld *fld)
 		list_del(&target->ft_chain);
 		if (target->ft_exp != NULL)
 			class_export_put(target->ft_exp);
-		OBD_FREE_PTR(target);
+		kfree(target);
 	}
 	spin_unlock(&fld->lcf_lock);
 
@@ -504,18 +490,16 @@ EXPORT_SYMBOL(fld_client_flush);
 
 static int __init fld_mod_init(void)
 {
-	fld_type_proc_dir = lprocfs_register(LUSTRE_FLD_NAME,
-					     proc_lustre_root,
-					     NULL, NULL);
-	return PTR_ERR_OR_ZERO(fld_type_proc_dir);
+	fld_debugfs_dir = ldebugfs_register(LUSTRE_FLD_NAME,
+					    debugfs_lustre_root,
+					    NULL, NULL);
+	return PTR_ERR_OR_ZERO(fld_debugfs_dir);
 }
 
 static void __exit fld_mod_exit(void)
 {
-	if (fld_type_proc_dir != NULL && !IS_ERR(fld_type_proc_dir)) {
-		lprocfs_remove(&fld_type_proc_dir);
-		fld_type_proc_dir = NULL;
-	}
+	if (!IS_ERR_OR_NULL(fld_debugfs_dir))
+		ldebugfs_remove(&fld_debugfs_dir);
 }
 
 MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
diff --git a/drivers/staging/lustre/lustre/fld/lproc_fld.c b/drivers/staging/lustre/lustre/fld/lproc_fld.c
index f53fdcfae34e..da822101e005 100644
--- a/drivers/staging/lustre/lustre/fld/lproc_fld.c
+++ b/drivers/staging/lustre/lustre/fld/lproc_fld.c
@@ -56,7 +56,7 @@
 #include "fld_internal.h"
 
 static int
-fld_proc_targets_seq_show(struct seq_file *m, void *unused)
+fld_debugfs_targets_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_fld *fld = (struct lu_client_fld *)m->private;
 	struct lu_fld_target *target;
@@ -73,7 +73,7 @@ fld_proc_targets_seq_show(struct seq_file *m, void *unused)
 }
 
 static int
-fld_proc_hash_seq_show(struct seq_file *m, void *unused)
+fld_debugfs_hash_seq_show(struct seq_file *m, void *unused)
 {
 	struct lu_client_fld *fld = (struct lu_client_fld *)m->private;
 
@@ -87,9 +87,9 @@ fld_proc_hash_seq_show(struct seq_file *m, void *unused)
 }
 
 static ssize_t
-fld_proc_hash_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+fld_debugfs_hash_seq_write(struct file *file,
+			   const char __user *buffer,
+			   size_t count, loff_t *off)
 {
 	struct lu_client_fld *fld;
 	struct lu_fld_hash *hash = NULL;
@@ -128,8 +128,8 @@ fld_proc_hash_seq_write(struct file *file,
 }
 
 static ssize_t
-fld_proc_cache_flush_write(struct file *file, const char __user *buffer,
-			       size_t count, loff_t *pos)
+fld_debugfs_cache_flush_write(struct file *file, const char __user *buffer,
+			      size_t count, loff_t *pos)
 {
 	struct lu_client_fld *fld = file->private_data;
 
@@ -142,31 +142,26 @@ fld_proc_cache_flush_write(struct file *file, const char __user *buffer,
 	return count;
 }
 
-static int fld_proc_cache_flush_open(struct inode *inode, struct file *file)
-{
-	file->private_data = PDE_DATA(inode);
-	return 0;
-}
-
-static int fld_proc_cache_flush_release(struct inode *inode, struct file *file)
+static int
+fld_debugfs_cache_flush_release(struct inode *inode, struct file *file)
 {
 	file->private_data = NULL;
 	return 0;
 }
 
-static struct file_operations fld_proc_cache_flush_fops = {
+static struct file_operations fld_debugfs_cache_flush_fops = {
 	.owner		= THIS_MODULE,
-	.open		= fld_proc_cache_flush_open,
-	.write		= fld_proc_cache_flush_write,
-	.release	= fld_proc_cache_flush_release,
+	.open           = simple_open,
+	.write		= fld_debugfs_cache_flush_write,
+	.release	= fld_debugfs_cache_flush_release,
 };
 
-LPROC_SEQ_FOPS_RO(fld_proc_targets);
-LPROC_SEQ_FOPS(fld_proc_hash);
+LPROC_SEQ_FOPS_RO(fld_debugfs_targets);
+LPROC_SEQ_FOPS(fld_debugfs_hash);
 
-struct lprocfs_vars fld_client_proc_list[] = {
-	{ "targets", &fld_proc_targets_fops },
-	{ "hash", &fld_proc_hash_fops },
-	{ "cache_flush", &fld_proc_cache_flush_fops },
+struct lprocfs_vars fld_client_debugfs_list[] = {
+	{ "targets",	 &fld_debugfs_targets_fops },
+	{ "hash",	 &fld_debugfs_hash_fops },
+	{ "cache_flush", &fld_debugfs_cache_flush_fops },
 	{ NULL }
 };
diff --git a/drivers/staging/lustre/lustre/include/dt_object.h b/drivers/staging/lustre/lustre/include/dt_object.h
index be4c7d95e788..abae31b41e74 100644
--- a/drivers/staging/lustre/lustre/include/dt_object.h
+++ b/drivers/staging/lustre/lustre/include/dt_object.h
@@ -58,7 +58,6 @@
 #include "../../include/linux/libcfs/libcfs.h"
 
 struct seq_file;
-struct proc_dir_entry;
 struct lustre_cfg;
 
 struct thandle;
@@ -1481,7 +1480,6 @@ static inline struct dt_thread_info *dt_info(const struct lu_env *env)
 int dt_global_init(void);
 void dt_global_fini(void);
 
-#if defined (CONFIG_PROC_FS)
 int lprocfs_dt_rd_blksize(char *page, char **start, off_t off,
 			  int count, int *eof, void *data);
 int lprocfs_dt_rd_kbytestotal(char *page, char **start, off_t off,
@@ -1494,6 +1492,5 @@ int lprocfs_dt_rd_filestotal(char *page, char **start, off_t off,
 			     int count, int *eof, void *data);
 int lprocfs_dt_rd_filesfree(char *page, char **start, off_t off,
 			    int count, int *eof, void *data);
-#endif /* CONFIG_PROC_FS */
 
 #endif /* __LUSTRE_DT_OBJECT_H */
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h b/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
index 3925db160650..513c81f43d6e 100644
--- a/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
+++ b/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
@@ -189,22 +189,7 @@ static inline int ll_quota_off(struct super_block *sb, int off, int remount)
 #endif
 
 
-
-/*
- * After 3.1, kernel's nameidata.intent.open.flags is different
- * with lustre's lookup_intent.it_flags, as lustre's it_flags'
- * lower bits equal to FMODE_xxx while kernel doesn't transliterate
- * lower bits of nameidata.intent.open.flags to FMODE_xxx.
- * */
 #include <linux/version.h>
-static inline int ll_namei_to_lookup_intent_flag(int flag)
-{
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 1, 0)
-	flag = (flag & ~O_ACCMODE) | OPEN_FMODE(flag);
-#endif
-	return flag;
-}
-
 #include <linux/fs.h>
 
 # define ll_umode_t	umode_t
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_lite.h b/drivers/staging/lustre/lustre/include/linux/lustre_lite.h
index a7658a99a08d..45651caf42cc 100644
--- a/drivers/staging/lustre/lustre/include/linux/lustre_lite.h
+++ b/drivers/staging/lustre/lustre/include/linux/lustre_lite.h
@@ -44,7 +44,6 @@
 
 #include <linux/fs.h>
 #include <linux/dcache.h>
-#include <linux/proc_fs.h>
 
 #include "../obd_class.h"
 #include "../lustre_net.h"
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_patchless_compat.h b/drivers/staging/lustre/lustre/include/linux/lustre_patchless_compat.h
index d72605864b0a..14562788e4e0 100644
--- a/drivers/staging/lustre/lustre/include/linux/lustre_patchless_compat.h
+++ b/drivers/staging/lustre/lustre/include/linux/lustre_patchless_compat.h
@@ -55,9 +55,7 @@ truncate_complete_page(struct address_space *mapping, struct page *page)
 	if (PagePrivate(page))
 		page->mapping->a_ops->invalidatepage(page, 0, PAGE_CACHE_SIZE);
 
-	if (TestClearPageDirty(page))
-		account_page_cleaned(page, mapping);
-
+	cancel_dirty_page(page);
 	ClearPageMappedToDisk(page);
 	ll_delete_from_page_cache(page);
 }
diff --git a/drivers/staging/lustre/lustre/include/linux/obd.h b/drivers/staging/lustre/lustre/include/linux/obd.h
index 9cd8683573ce..2817e88e014a 100644
--- a/drivers/staging/lustre/lustre/include/linux/obd.h
+++ b/drivers/staging/lustre/lustre/include/linux/obd.h
@@ -43,11 +43,11 @@
 
 #include "../obd_support.h"
 
-# include <linux/fs.h>
-# include <linux/list.h>
-# include <linux/sched.h>  /* for struct task_struct, for current.h */
-# include <linux/proc_fs.h>
-# include <linux/mount.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/sched.h>  /* for struct task_struct, for current.h */
+#include <linux/mount.h>
+
 #include "../lustre_intent.h"
 
 struct ll_iattr {
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index d030847e51ba..8ede2a00ca4f 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -42,7 +42,7 @@
 #ifndef _LPROCFS_SNMP_H
 #define _LPROCFS_SNMP_H
 
-#include <linux/proc_fs.h>
+#include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/spinlock.h>
 #include <linux/types.h>
@@ -60,8 +60,8 @@ struct lprocfs_vars {
 };
 
 struct lprocfs_static_vars {
-	struct lprocfs_vars *module_vars;
 	struct lprocfs_vars *obd_vars;
+	struct attribute_group *sysfs_vars;
 };
 
 /* if we find more consumers this could be generalized */
@@ -348,7 +348,8 @@ enum {
 
 #define EXTRA_FIRST_OPC LDLM_GLIMPSE_ENQUEUE
 /* class_obd.c */
-extern struct proc_dir_entry *proc_lustre_root;
+extern struct dentry *debugfs_lustre_root;
+extern struct kobject *lustre_kobj;
 
 struct obd_device;
 struct obd_histogram;
@@ -378,8 +379,6 @@ 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)
-
 extern int lprocfs_stats_alloc_one(struct lprocfs_stats *stats,
 				   unsigned int cpuid);
 /*
@@ -422,9 +421,8 @@ static inline int lprocfs_stats_lock(struct lprocfs_stats *stats, int opc,
 			else
 				spin_lock(&stats->ls_lock);
 			return 1;
-		} else {
-			return stats->ls_biggest_alloc_num;
 		}
+		return stats->ls_biggest_alloc_num;
 	}
 }
 
@@ -556,53 +554,45 @@ extern void lprocfs_counter_init(struct lprocfs_stats *stats, int index,
 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,
-				   struct proc_dir_entry *entry);
-extern int lprocfs_exp_setup(struct obd_export *exp,
-			     lnet_nid_t *peer_nid, int *newnid);
 extern int lprocfs_exp_cleanup(struct obd_export *exp);
-extern struct proc_dir_entry *lprocfs_add_simple(struct proc_dir_entry *root,
-						char *name,
-						void *data,
-						struct file_operations *fops);
-extern struct proc_dir_entry *
-lprocfs_add_symlink(const char *name, struct proc_dir_entry *parent,
+extern struct dentry *ldebugfs_add_simple(struct dentry *root,
+					  char *name,
+					  void *data,
+					  struct file_operations *fops);
+extern struct dentry *
+ldebugfs_add_symlink(const char *name, struct dentry *parent,
 		    const char *format, ...);
-extern void lprocfs_free_per_client_stats(struct obd_device *obd);
-extern int
-lprocfs_nid_stats_clear_write(struct file *file, const char *buffer,
-			      unsigned long count, void *data);
-extern int lprocfs_nid_stats_clear_read(struct seq_file *m, void *data);
 
-extern int lprocfs_register_stats(struct proc_dir_entry *root, const char *name,
-				  struct lprocfs_stats *stats);
+extern int ldebugfs_register_stats(struct dentry *parent,
+				   const char *name,
+				   struct lprocfs_stats *stats);
 
 /* lprocfs_status.c */
-extern int lprocfs_add_vars(struct proc_dir_entry *root,
-			    struct lprocfs_vars *var,
-			    void *data);
+extern int ldebugfs_add_vars(struct dentry *parent,
+			     struct lprocfs_vars *var,
+			     void *data);
 
-extern struct proc_dir_entry *lprocfs_register(const char *name,
-					      struct proc_dir_entry *parent,
-					      struct lprocfs_vars *list,
-					      void *data);
+extern struct dentry *ldebugfs_register(const char *name,
+					struct dentry *parent,
+					struct lprocfs_vars *list,
+					void *data);
 
-extern void lprocfs_remove(struct proc_dir_entry **root);
-extern void lprocfs_remove_proc_entry(const char *name,
-				      struct proc_dir_entry *parent);
+extern void ldebugfs_remove(struct dentry **entryp);
 
-extern int lprocfs_obd_setup(struct obd_device *obd, struct lprocfs_vars *list);
+extern int lprocfs_obd_setup(struct obd_device *obd, struct lprocfs_vars *list,
+			     struct attribute_group *attrs);
 extern int lprocfs_obd_cleanup(struct obd_device *obd);
 
-extern int lprocfs_seq_create(struct proc_dir_entry *parent, const char *name,
-			      umode_t mode,
-			      const struct file_operations *seq_fops,
-			      void *data);
-extern int lprocfs_obd_seq_create(struct obd_device *dev, const char *name,
-				  umode_t mode,
-				  const struct file_operations *seq_fops,
-				  void *data);
+extern int ldebugfs_seq_create(struct dentry *parent,
+			       const char *name,
+			       umode_t mode,
+			       const struct file_operations *seq_fops,
+			       void *data);
+extern int ldebugfs_obd_seq_create(struct obd_device *dev,
+				   const char *name,
+				   umode_t mode,
+				   const struct file_operations *seq_fops,
+				   void *data);
 
 /* Generic callbacks */
 
@@ -613,15 +603,12 @@ extern int lprocfs_wr_atomic(struct file *file, const char __user *buffer,
 extern int lprocfs_rd_uint(struct seq_file *m, void *data);
 extern int lprocfs_wr_uint(struct file *file, const char __user *buffer,
 			   unsigned long count, void *data);
-extern int lprocfs_rd_uuid(struct seq_file *m, void *data);
 extern int lprocfs_rd_name(struct seq_file *m, void *data);
 extern int lprocfs_rd_server_uuid(struct seq_file *m, void *data);
 extern int lprocfs_rd_conn_uuid(struct seq_file *m, void *data);
 extern int lprocfs_rd_import(struct seq_file *m, void *data);
 extern int lprocfs_rd_state(struct seq_file *m, void *data);
 extern int lprocfs_rd_connect_flags(struct seq_file *m, void *data);
-extern int lprocfs_rd_num_exports(struct seq_file *m, void *data);
-extern int lprocfs_rd_numrefs(struct seq_file *m, void *data);
 
 struct adaptive_timeout;
 extern int lprocfs_at_hist_helper(struct seq_file *m,
@@ -640,12 +627,6 @@ extern int lprocfs_wr_pinger_recov(struct file *file, const char __user *buffer,
 				   size_t count, loff_t *off);
 
 /* Statfs helpers */
-extern int lprocfs_rd_blksize(struct seq_file *m, void *data);
-extern int lprocfs_rd_kbytestotal(struct seq_file *m, void *data);
-extern int lprocfs_rd_kbytesfree(struct seq_file *m, void *data);
-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 __user *buffer, unsigned long count,
 				int *val);
@@ -685,11 +666,11 @@ extern int lprocfs_seq_release(struct inode *, struct file *);
 /* write the name##_seq_show function, call LPROC_SEQ_FOPS_RO for read-only
   proc entries; otherwise, you will define name##_seq_write function also for
   a read-write proc entry, and then call LPROC_SEQ_SEQ instead. Finally,
-  call lprocfs_obd_seq_create(obd, filename, 0444, &name#_fops, data); */
+  call ldebugfs_obd_seq_create(obd, filename, 0444, &name#_fops, data); */
 #define __LPROC_SEQ_FOPS(name, custom_seq_write)			\
 static int name##_single_open(struct inode *inode, struct file *file)	\
 {									\
-	return single_open(file, name##_seq_show, PDE_DATA(inode));	\
+	return single_open(file, name##_seq_show, inode->i_private);	\
 }									\
 static struct file_operations name##_fops = {				\
 	.owner   = THIS_MODULE,					    \
@@ -734,7 +715,7 @@ static struct file_operations name##_fops = {				\
 	}								\
 	static int name##_##type##_open(struct inode *inode, struct file *file) \
 	{								\
-		return single_open(file, NULL, PDE_DATA(inode));	\
+		return single_open(file, NULL, inode->i_private);	\
 	}								\
 	static struct file_operations name##_##type##_fops = {	\
 		.open	= name##_##type##_open,				\
@@ -742,6 +723,27 @@ static struct file_operations name##_fops = {				\
 		.release = lprocfs_single_release,			\
 	}
 
+struct lustre_attr {
+	struct attribute attr;
+	ssize_t (*show)(struct kobject *kobj, struct attribute *attr,
+			char *buf);
+	ssize_t (*store)(struct kobject *kobj, struct attribute *attr,
+			 const char *buf, size_t len);
+};
+
+#define LUSTRE_ATTR(name, mode, show, store) \
+static struct lustre_attr lustre_attr_##name = __ATTR(name, mode, show, store)
+
+#define LUSTRE_RO_ATTR(name) LUSTRE_ATTR(name, 0444, name##_show, NULL)
+#define LUSTRE_RW_ATTR(name) LUSTRE_ATTR(name, 0644, name##_show, name##_store)
+
+ssize_t lustre_attr_show(struct kobject *kobj, struct attribute *attr,
+			 char *buf);
+ssize_t lustre_attr_store(struct kobject *kobj, struct attribute *attr,
+			  const char *buf, size_t len);
+
+extern const struct sysfs_ops lustre_sysfs_ops;
+
 /* lproc_ptlrpc.c */
 struct ptlrpc_request;
 extern void target_print_req(void *seq_file, struct ptlrpc_request *req);
@@ -810,206 +812,4 @@ extern int lprocfs_quota_rd_qs_factor(char *page, char **start, loff_t off,
 extern int lprocfs_quota_wr_qs_factor(struct file *file,
 				      const char *buffer,
 				      unsigned long count, void *data);
-#else
-/* CONFIG_PROC_FS is not defined */
-
-#define proc_lustre_root NULL
-
-static inline void lprocfs_counter_add(struct lprocfs_stats *stats,
-				       int index, long amount)
-{ return; }
-static inline void lprocfs_counter_incr(struct lprocfs_stats *stats,
-					int index)
-{ return; }
-static inline void lprocfs_counter_sub(struct lprocfs_stats *stats,
-				       int index, long amount)
-{ return; }
-static inline void lprocfs_counter_decr(struct lprocfs_stats *stats,
-					int index)
-{ return; }
-static inline void lprocfs_counter_init(struct lprocfs_stats *stats,
-					int index, unsigned conf,
-					const char *name, const char *units)
-{ return; }
-
-static inline __u64 lc_read_helper(struct lprocfs_counter *lc,
-				   enum lprocfs_fields_flags field)
-{ return 0; }
-
-/* NB: we return !NULL to satisfy error checker */
-static inline struct lprocfs_stats *
-lprocfs_alloc_stats(unsigned int num, enum lprocfs_stats_flags flags)
-{ return (struct lprocfs_stats *)1; }
-static inline void lprocfs_clear_stats(struct lprocfs_stats *stats)
-{ return; }
-static inline void lprocfs_free_stats(struct lprocfs_stats **stats)
-{ return; }
-static inline int lprocfs_register_stats(struct proc_dir_entry *root,
-					 const char *name,
-					 struct lprocfs_stats *stats)
-{ return 0; }
-static inline void lprocfs_init_ops_stats(int num_private_stats,
-					  struct lprocfs_stats *stats)
-{ return; }
-static inline void lprocfs_init_mps_stats(int num_private_stats,
-					  struct lprocfs_stats *stats)
-{ return; }
-static inline void lprocfs_init_ldlm_stats(struct lprocfs_stats *ldlm_stats)
-{ return; }
-static inline int lprocfs_alloc_obd_stats(struct obd_device *obddev,
-					  unsigned int num_private_stats)
-{ return 0; }
-static inline int lprocfs_alloc_md_stats(struct obd_device *obddev,
-					 unsigned int num_private_stats)
-{ return 0; }
-static inline void lprocfs_free_obd_stats(struct obd_device *obddev)
-{ return; }
-static inline void lprocfs_free_md_stats(struct obd_device *obddev)
-{ return; }
-
-struct obd_export;
-static inline int lprocfs_add_clear_entry(struct obd_export *exp)
-{ return 0; }
-static inline int lprocfs_exp_setup(struct obd_export *exp,
-				    lnet_nid_t *peer_nid,
-				    int *newnid)
-{ return 0; }
-static inline int lprocfs_exp_cleanup(struct obd_export *exp)
-{ return 0; }
-static inline struct proc_dir_entry *
-lprocfs_add_simple(struct proc_dir_entry *root, char *name,
-		   void *data, struct file_operations *fops)
-{return 0; }
-static inline struct proc_dir_entry *
-lprocfs_add_symlink(const char *name, struct proc_dir_entry *parent,
-		    const char *format, ...)
-{return NULL; }
-static inline void lprocfs_free_per_client_stats(struct obd_device *obd)
-{ return; }
-static inline
-int lprocfs_nid_stats_clear_write(struct file *file, const char *buffer,
-				  unsigned long count, void *data)
-{return count;}
-static inline
-int lprocfs_nid_stats_clear_read(struct seq_file *m, void *data)
-{ return 0; }
-
-static inline struct proc_dir_entry *
-lprocfs_register(const char *name, struct proc_dir_entry *parent,
-		 struct lprocfs_vars *list, void *data)
-{ return NULL; }
-static inline int lprocfs_add_vars(struct proc_dir_entry *root,
-				   struct lprocfs_vars *var,
-				   void *data)
-{ return 0; }
-static inline void lprocfs_remove(struct proc_dir_entry **root)
-{ return; }
-static inline void lprocfs_remove_proc_entry(const char *name,
-					     struct proc_dir_entry *parent)
-{ return; }
-static inline int lprocfs_obd_setup(struct obd_device *dev,
-				    struct lprocfs_vars *list)
-{ return 0; }
-static inline int lprocfs_obd_cleanup(struct obd_device *dev)
-{ return 0; }
-static inline int lprocfs_rd_u64(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_rd_uuid(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_rd_name(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_rd_server_uuid(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_rd_conn_uuid(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_rd_import(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_rd_pinger_recov(struct seq_file *m, void *n)
-{ return 0; }
-static inline int lprocfs_rd_state(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_rd_connect_flags(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_rd_num_exports(struct seq_file *m, void *data)
-{ return 0; }
-extern inline int lprocfs_rd_numrefs(struct seq_file *m, void *data)
-{ return 0; }
-struct adaptive_timeout;
-static inline int lprocfs_at_hist_helper(struct seq_file *m,
-					 struct adaptive_timeout *at)
-{ return 0; }
-static inline int lprocfs_rd_timeouts(struct seq_file *m, void *data)
-{ return 0; }
-static inline int lprocfs_wr_timeouts(struct file *file,
-				const char __user *buffer,
-				unsigned long count, void *data)
-{ return 0; }
-static inline int lprocfs_wr_evict_client(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
-{ return 0; }
-static inline int lprocfs_wr_ping(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
-{ return 0; }
-static inline int lprocfs_wr_import(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
-{ return 0; }
-static inline int lprocfs_wr_pinger_recov(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
-{ return 0; }
-
-/* Statfs helpers */
-static inline
-int lprocfs_rd_blksize(struct seq_file *m, void *data)
-{ return 0; }
-static inline
-int lprocfs_rd_kbytestotal(struct seq_file *m, void *data)
-{ return 0; }
-static inline
-int lprocfs_rd_kbytesfree(struct seq_file *m, void *data)
-{ return 0; }
-static inline
-int lprocfs_rd_kbytesavail(struct seq_file *m, void *data)
-{ return 0; }
-static inline
-int lprocfs_rd_filestotal(struct seq_file *m, void *data)
-{ return 0; }
-static inline
-int lprocfs_rd_filesfree(struct seq_file *m, void *data)
-{ return 0; }
-static inline
-void lprocfs_oh_tally(struct obd_histogram *oh, unsigned int value)
-{ return; }
-static inline
-void lprocfs_oh_tally_log2(struct obd_histogram *oh, unsigned int value)
-{ return; }
-static inline
-void lprocfs_oh_clear(struct obd_histogram *oh)
-{ return; }
-static inline
-unsigned long lprocfs_oh_sum(struct obd_histogram *oh)
-{ return 0; }
-static inline
-void lprocfs_stats_collect(struct lprocfs_stats *stats, int idx,
-			   struct lprocfs_counter *cnt)
-{ return; }
-static inline
-__u64 lprocfs_stats_collector(struct lprocfs_stats *stats, int idx,
-			       enum lprocfs_fields_flags field)
-{ return (__u64)0; }
-
-#define LPROC_SEQ_FOPS_RO(name)
-#define LPROC_SEQ_FOPS(name)
-#define LPROC_SEQ_FOPS_RO_TYPE(name, type)
-#define LPROC_SEQ_FOPS_RW_TYPE(name, type)
-#define LPROC_SEQ_FOPS_WR_ONLY(name, type)
-
-/* lproc_ptlrpc.c */
-#define target_print_req NULL
-
-#endif /* CONFIG_PROC_FS */
-
 #endif /* LPROCFS_SNMP_H */
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index c8cc48f00026..e1d72a7a5c2d 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -43,7 +43,6 @@
 #include "lu_ref.h"
 
 struct seq_file;
-struct proc_dir_entry;
 struct lustre_cfg;
 struct lprocfs_stats;
 
@@ -277,7 +276,6 @@ struct lu_device {
 	 * Stack this device belongs to.
 	 */
 	struct lu_site		    *ld_site;
-	struct proc_dir_entry	     *ld_proc_entry;
 
 	/** \todo XXX: temporary back pointer into obd. */
 	struct obd_device		 *ld_obd;
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
index 305ecbee9b78..4d72d6ed26b0 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
@@ -92,6 +92,7 @@
 #define _LUSTRE_IDL_H_
 
 #include "../../../include/linux/libcfs/libcfs.h"
+#include "../../../include/linux/lnet/types.h"
 
 /* Defn's shared with user-space. */
 #include "lustre_user.h"
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
index 89794fdfec9d..e095ada40ed2 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
@@ -243,7 +243,6 @@ struct ost_id {
 
 #define LL_IOC_LMV_SETSTRIPE	    _IOWR('f', 240, struct lmv_user_md)
 #define LL_IOC_LMV_GETSTRIPE	    _IOWR('f', 241, struct lmv_user_md)
-#define LL_IOC_REMOVE_ENTRY	    _IOWR('f', 242, __u64)
 #define LL_IOC_SET_LEASE		_IOWR('f', 243, long)
 #define LL_IOC_GET_LEASE		_IO('f', 244)
 #define LL_IOC_HSM_IMPORT		_IOWR('f', 245, struct hsm_user_import)
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm.h b/drivers/staging/lustre/lustre/include/lustre_dlm.h
index bac9902b56bb..f6f4c037fb30 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm.h
@@ -60,11 +60,13 @@
 struct obd_ops;
 struct obd_device;
 
+extern struct kset *ldlm_ns_kset;
+extern struct kset *ldlm_svc_kset;
+
 #define OBD_LDLM_DEVICENAME  "ldlm"
 
 #define LDLM_DEFAULT_LRU_SIZE (100 * num_online_cpus())
 #define LDLM_DEFAULT_MAX_ALIVE (cfs_time_seconds(36000))
-#define LDLM_CTIME_AGE_LIMIT (10)
 #define LDLM_DEFAULT_PARALLEL_AST_LIMIT 1024
 
 /**
@@ -232,8 +234,8 @@ struct ldlm_pool_ops {
  * This feature is commonly referred to as lru_resize.
  */
 struct ldlm_pool {
-	/** Pool proc directory. */
-	struct proc_dir_entry	*pl_proc_dir;
+	/** Pool debugfs directory. */
+	struct dentry		*pl_debugfs_entry;
 	/** Pool name, must be long enough to hold compound proc entry name. */
 	char			pl_name[100];
 	/** Lock for protecting SLV/CLV updates. */
@@ -263,6 +265,10 @@ struct ldlm_pool {
 	int			pl_grant_plan;
 	/** Pool statistics. */
 	struct lprocfs_stats	*pl_stats;
+
+	/* sysfs object */
+	struct kobject		 pl_kobj;
+	struct completion	 pl_kobj_unregister;
 };
 
 typedef int (*ldlm_res_policy)(struct ldlm_namespace *, struct ldlm_lock **,
@@ -302,14 +308,6 @@ typedef enum {
 	LDLM_NAMESPACE_MODEST = 1 << 1
 } ldlm_appetite_t;
 
-/**
- * Default values for the "max_nolock_size", "contention_time" and
- * "contended_locks" namespace tunables.
- */
-#define NS_DEFAULT_MAX_NOLOCK_BYTES 0
-#define NS_DEFAULT_CONTENTION_SECONDS 2
-#define NS_DEFAULT_CONTENDED_LOCKS 32
-
 struct ldlm_ns_bucket {
 	/** back pointer to namespace */
 	struct ldlm_namespace      *nsb_namespace;
@@ -390,8 +388,8 @@ struct ldlm_namespace {
 	/** Client side original connect flags supported by server. */
 	__u64			ns_orig_connect_flags;
 
-	/* namespace proc dir entry */
-	struct proc_dir_entry	*ns_proc_dir_entry;
+	/* namespace debugfs dir entry */
+	struct dentry		*ns_debugfs_entry;
 
 	/**
 	 * Position in global namespace list linking all namespaces on
@@ -421,18 +419,6 @@ struct ldlm_namespace {
 	unsigned int		ns_max_unused;
 	/** Maximum allowed age (last used time) for locks in the LRU */
 	unsigned int		ns_max_age;
-	/**
-	 * Server only: number of times we evicted clients due to lack of reply
-	 * to ASTs.
-	 */
-	unsigned int		ns_timeouts;
-	/**
-	 * Number of seconds since the file change time after which the
-	 * MDT will return an UPDATE lock along with a LOOKUP lock.
-	 * This allows the client to start caching negative dentries
-	 * for a directory and may save an RPC for a later stat.
-	 */
-	unsigned int		ns_ctime_age_limit;
 
 	/**
 	 * Used to rate-limit ldlm_namespace_dump calls.
@@ -466,27 +452,6 @@ struct ldlm_namespace {
 	/** Definition of how eagerly unused locks will be released from LRU */
 	ldlm_appetite_t		ns_appetite;
 
-	/**
-	 * If more than \a ns_contended_locks are found, the resource is
-	 * considered to be contended. Lock enqueues might specify that no
-	 * contended locks should be granted
-	 */
-	unsigned		ns_contended_locks;
-
-	/**
-	 * The resources in this namespace remember contended state during
-	 * \a ns_contention_time, in seconds.
-	 */
-	unsigned		ns_contention_time;
-
-	/**
-	 * Limit size of contended extent locks, in bytes.
-	 * If extended lock is requested for more then this many bytes and
-	 * caller instructs us not to grant contended locks, we would disregard
-	 * such a request.
-	 */
-	unsigned		ns_max_nolock_size;
-
 	/** Limit of parallel AST RPC count. */
 	unsigned		ns_max_parallel_ast;
 
@@ -501,6 +466,9 @@ struct ldlm_namespace {
 	 * recalculation of LDLM pool statistics should be skipped.
 	 */
 	unsigned		ns_stopping:1;
+
+	struct kobject		ns_kobj; /* sysfs object */
+	struct completion	ns_kobj_unregister;
 };
 
 /**
@@ -1283,13 +1251,8 @@ void ldlm_namespace_register(struct ldlm_namespace *ns, ldlm_side_t client);
 void ldlm_namespace_unregister(struct ldlm_namespace *ns, ldlm_side_t client);
 void ldlm_namespace_get(struct ldlm_namespace *ns);
 void ldlm_namespace_put(struct ldlm_namespace *ns);
-#if defined (CONFIG_PROC_FS)
-int ldlm_proc_setup(void);
-void ldlm_proc_cleanup(void);
-#else
-static inline int ldlm_proc_setup(void) { return 0; }
-static inline void ldlm_proc_cleanup(void) {}
-#endif
+int ldlm_debugfs_setup(void);
+void ldlm_debugfs_cleanup(void);
 
 /* resource.c - internal */
 struct ldlm_resource *ldlm_resource_get(struct ldlm_namespace *ns,
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm_flags.h b/drivers/staging/lustre/lustre/include/lustre_dlm_flags.h
index 16dcdbfae689..d4cc09635271 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm_flags.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm_flags.h
@@ -1,17 +1,10 @@
 /*  -*- buffer-read-only: t -*- vi: set ro:
  *
- * DO NOT EDIT THIS FILE   (lustre_dlm_flags.h)
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 only,
+ * as published by the Free Software Foundation.
  *
- * It has been AutoGen-ed
- * From the definitions    lustre_dlm_flags.def
- * and the template file   lustre_dlm_flags.tpl
- *
- * lustre is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * lustre is distributed in the hope that it will be useful, but
+ * Lustre is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  * See the GNU General Public License for more details.
@@ -22,8 +15,6 @@
 /**
  * \file lustre_dlm_flags.h
  * The flags and collections of flags (masks) for \see struct ldlm_lock.
- * This file is derived from flag definitions in lustre_dlm_flags.def.
- * The format is defined in the lustre_dlm_flags.tpl template file.
  *
  * \addtogroup LDLM Lustre Distributed Lock Manager
  * @{
diff --git a/drivers/staging/lustre/lustre/include/lustre_export.h b/drivers/staging/lustre/lustre/include/lustre_export.h
index 9c06a49f12a4..3b992b42fd91 100644
--- a/drivers/staging/lustre/lustre/include/lustre_export.h
+++ b/drivers/staging/lustre/lustre/include/lustre_export.h
@@ -106,34 +106,6 @@ struct mgs_export_data {
 	spinlock_t		med_lock;	/* protect med_clients */
 };
 
-/**
- * per-NID statistics structure.
- * It tracks access patterns to this export on a per-client-NID basis
- */
-struct nid_stat {
-	lnet_nid_t	       nid;
-	struct hlist_node	 nid_hash;
-	struct list_head	       nid_list;
-	struct obd_device       *nid_obd;
-	struct proc_dir_entry   *nid_proc;
-	struct lprocfs_stats    *nid_stats;
-	struct lprocfs_stats    *nid_ldlm_stats;
-	atomic_t	     nid_exp_ref_count; /* for obd_nid_stats_hash
-							   exp_nid_stats */
-};
-
-#define nidstat_getref(nidstat)						\
-do {									   \
-	atomic_inc(&(nidstat)->nid_exp_ref_count);			 \
-} while (0)
-
-#define nidstat_putref(nidstat)						\
-do {									   \
-	atomic_dec(&(nidstat)->nid_exp_ref_count);			 \
-	LASSERTF(atomic_read(&(nidstat)->nid_exp_ref_count) >= 0,	  \
-		 "stat %p nid_exp_ref_count < 0\n", nidstat);		  \
-} while (0)
-
 enum obd_option {
 	OBD_OPT_FORCE =	 0x0001,
 	OBD_OPT_FAILOVER =      0x0002,
@@ -190,7 +162,6 @@ struct obd_export {
 	 * exp_lock protect its change
 	 */
 	struct obd_import	*exp_imp_reverse;
-	struct nid_stat	  *exp_nid_stats;
 	struct lprocfs_stats     *exp_md_stats;
 	/** Active connection */
 	struct ptlrpc_connection *exp_connection;
diff --git a/drivers/staging/lustre/lustre/include/lustre_fid.h b/drivers/staging/lustre/lustre/include/lustre_fid.h
index 0a0929fd9023..c7c8fe4cdbcc 100644
--- a/drivers/staging/lustre/lustre/include/lustre_fid.h
+++ b/drivers/staging/lustre/lustre/include/lustre_fid.h
@@ -346,7 +346,7 @@ struct lu_client_seq {
 	struct lu_seq_range	 lcs_space;
 
 	/* Seq related proc */
-	struct proc_dir_entry   *lcs_proc_dir;
+	struct dentry		*lcs_debugfs_entry;
 
 	/* This holds last allocated fid in last obtained seq */
 	struct lu_fid	   lcs_fid;
@@ -392,9 +392,6 @@ struct lu_server_seq {
 	/* /seq file object device */
 	struct dt_object       *lss_obj;
 
-	/* Seq related proc */
-	struct proc_dir_entry   *lss_proc_dir;
-
 	/* LUSTRE_SEQ_SERVER or LUSTRE_SEQ_CONTROLLER */
 	enum lu_mgr_type       lss_type;
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_fld.h b/drivers/staging/lustre/lustre/include/lustre_fld.h
index 5ee4b1ed0995..c1f08dee3bd6 100644
--- a/drivers/staging/lustre/lustre/include/lustre_fld.h
+++ b/drivers/staging/lustre/lustre/include/lustre_fld.h
@@ -71,10 +71,6 @@ struct lu_fld_target {
 
 struct lu_server_fld {
 	/**
-	 * Fld dir proc entry. */
-	struct proc_dir_entry    *lsf_proc_dir;
-
-	/**
 	 * /fld file object device */
 	struct dt_object	*lsf_obj;
 
@@ -99,8 +95,8 @@ struct lu_server_fld {
 
 struct lu_client_fld {
 	/**
-	 * Client side proc entry. */
-	struct proc_dir_entry    *lcf_proc_dir;
+	 * Client side debugfs entry. */
+	struct dentry		*lcf_debugfs_entry;
 
 	/**
 	 * List of exports client FLD knows about. */
@@ -123,10 +119,10 @@ struct lu_client_fld {
 	struct fld_cache	*lcf_cache;
 
 	/**
-	 * Client fld proc entry name. */
-	char		     lcf_name[LUSTRE_MDT_MAXNAMELEN];
+	 * Client fld debugfs entry name. */
+	char			 lcf_name[LUSTRE_MDT_MAXNAMELEN];
 
-	int		      lcf_flags;
+	int			 lcf_flags;
 };
 
 /* Client methods */
@@ -153,7 +149,7 @@ int fld_client_add_target(struct lu_client_fld *fld,
 int fld_client_del_target(struct lu_client_fld *fld,
 			  __u64 idx);
 
-void fld_client_proc_fini(struct lu_client_fld *fld);
+void fld_client_debugfs_fini(struct lu_client_fld *fld);
 
 /** @} fld */
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_lib.h b/drivers/staging/lustre/lustre/include/lustre_lib.h
index bf135630c39a..43ee9f0eb4d4 100644
--- a/drivers/staging/lustre/lustre/include/lustre_lib.h
+++ b/drivers/staging/lustre/lustre/include/lustre_lib.h
@@ -549,19 +549,13 @@ do {									   \
 		__blocked = cfs_block_sigsinv(0);			      \
 									       \
 	for (;;) {							     \
-		unsigned       __wstate;				       \
-									       \
-		__wstate = info->lwi_on_signal != NULL &&		      \
-			   (__timeout == 0 || __allow_intr) ?		  \
-			TASK_INTERRUPTIBLE : TASK_UNINTERRUPTIBLE;	       \
-									       \
-		set_current_state(TASK_INTERRUPTIBLE);		 \
-									       \
 		if (condition)						 \
 			break;						 \
 									       \
+		set_current_state(TASK_INTERRUPTIBLE);			       \
+									       \
 		if (__timeout == 0) {					  \
-			schedule();						\
+			schedule();					       \
 		} else {						       \
 			long interval = info->lwi_interval?	  \
 					     min_t(long,	     \
@@ -582,6 +576,8 @@ do {									   \
 			}						      \
 		}							      \
 									       \
+		set_current_state(TASK_RUNNING);			       \
+									       \
 		if (condition)						 \
 			break;						 \
 		if (cfs_signal_pending()) {				    \
@@ -605,7 +601,6 @@ do {									   \
 									       \
 	cfs_restore_sigs(__blocked);					   \
 									       \
-	set_current_state(TASK_RUNNING);			       \
 	remove_wait_queue(&wq, &__wait);					   \
 } while (0)
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_net.h b/drivers/staging/lustre/lustre/include/lustre_net.h
index e2805bd1acf1..77a7de98fc8e 100644
--- a/drivers/staging/lustre/lustre/include/lustre_net.h
+++ b/drivers/staging/lustre/lustre/include/lustre_net.h
@@ -56,8 +56,8 @@
  */
 
 #include "../../include/linux/libcfs/libcfs.h"
-// #include <obd.h>
-#include "../../include/linux/lnet/lnet.h"
+#include "../../include/linux/lnet/nidstr.h"
+#include "../../include/linux/lnet/api.h"
 #include "lustre/lustre_idl.h"
 #include "lustre_ha.h"
 #include "lustre_sec.h"
@@ -1978,8 +1978,8 @@ struct ptlrpc_service {
 	int				srv_nthrs_cpt_init;
 	/** limit of threads number for each partition */
 	int				srv_nthrs_cpt_limit;
-	/** Root of /proc dir tree for this service */
-	struct proc_dir_entry	   *srv_procroot;
+	/** Root of debugfs dir tree for this service */
+	struct dentry		   *srv_debugfs_entry;
 	/** Pointer to statistic data for this service */
 	struct lprocfs_stats	   *srv_stats;
 	/** # hp per lp reqs to handle */
@@ -2016,6 +2016,10 @@ struct ptlrpc_service {
 	int				srv_cpt_bits;
 	/** CPT table this service is running over */
 	struct cfs_cpt_table		*srv_cptable;
+
+	/* sysfs object */
+	struct kobject			 srv_kobj;
+	struct completion		 srv_kobj_unregister;
 	/**
 	 * partition data for ptlrpc service
 	 */
@@ -2525,7 +2529,8 @@ void ptlrpc_schedule_difficult_reply(struct ptlrpc_reply_state *rs);
 int ptlrpc_hpreq_handler(struct ptlrpc_request *req);
 struct ptlrpc_service *ptlrpc_register_service(
 				struct ptlrpc_service_conf *conf,
-				struct proc_dir_entry *proc_entry);
+				struct kset *parent,
+				struct dentry *debugfs_entry);
 void ptlrpc_stop_all_threads(struct ptlrpc_service *svc);
 
 int ptlrpc_start_threads(struct ptlrpc_service *svc);
@@ -2947,15 +2952,9 @@ void ptlrpcd_decref(void);
  * @{
  */
 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);
 void ptlrpc_lprocfs_brw(struct ptlrpc_request *req, int bytes);
-#else
-static inline void ptlrpc_lprocfs_register_obd(struct obd_device *obd) {}
-static inline void ptlrpc_lprocfs_unregister_obd(struct obd_device *obd) {}
-static inline void ptlrpc_lprocfs_brw(struct ptlrpc_request *req, int bytes) {}
-#endif
 /** @} */
 
 /* ptlrpc/llog_client.c */
diff --git a/drivers/staging/lustre/lustre/include/lustre_quota.h b/drivers/staging/lustre/lustre/include/lustre_quota.h
deleted file mode 100644
index 2643f28070a2..000000000000
--- a/drivers/staging/lustre/lustre/include/lustre_quota.h
+++ /dev/null
@@ -1,241 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; if not, write to the
- * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA 021110-1307, USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2011, 2012, Intel Corporation.
- * Use is subject to license terms.
- */
-
-#ifndef _LUSTRE_QUOTA_H
-#define _LUSTRE_QUOTA_H
-
-/** \defgroup quota quota
- *
- */
-
-#include <linux/fs.h>
-#include <linux/quota.h>
-#include <linux/quotaops.h>
-
-#include "dt_object.h"
-#include "lustre_fid.h"
-#include "lustre_dlm.h"
-
-#ifndef MAX_IQ_TIME
-#define MAX_IQ_TIME  604800     /* (7*24*60*60) 1 week */
-#endif
-
-#ifndef MAX_DQ_TIME
-#define MAX_DQ_TIME  604800     /* (7*24*60*60) 1 week */
-#endif
-
-struct lquota_id_info;
-struct lquota_trans;
-
-/* Gather all quota record type in an union that can be used to read any records
- * from disk. All fields of these records must be 64-bit aligned, otherwise the
- * OSD layer may swab them incorrectly. */
-union lquota_rec {
-	struct lquota_glb_rec	lqr_glb_rec;
-	struct lquota_slv_rec	lqr_slv_rec;
-	struct lquota_acct_rec	lqr_acct_rec;
-};
-
-/* Index features supported by the global index objects
- * Only used for migration purpose and should be removed once on-disk migration
- * is no longer needed */
-extern struct dt_index_features dt_quota_iusr_features;
-extern struct dt_index_features dt_quota_busr_features;
-extern struct dt_index_features dt_quota_igrp_features;
-extern struct dt_index_features dt_quota_bgrp_features;
-
-/* Name used in the configuration logs to identify the default metadata pool
- * (composed of all the MDTs, with pool ID 0) and the default data pool (all
- * the OSTs, with pool ID 0 too). */
-#define QUOTA_METAPOOL_NAME   "mdt="
-#define QUOTA_DATAPOOL_NAME   "ost="
-
-/*
- * Quota Master Target support
- */
-
-/* Request handlers for quota master operations.
- * This is used by the MDT to pass quota/lock requests to the quota master
- * target. This won't be needed any more once the QMT is a real target and
- * does not rely any more on the MDT service threads and namespace. */
-struct qmt_handlers {
-	/* Handle quotactl request from client. */
-	int (*qmth_quotactl)(const struct lu_env *, struct lu_device *,
-			     struct obd_quotactl *);
-
-	/* Handle dqacq/dqrel request from slave. */
-	int (*qmth_dqacq)(const struct lu_env *, struct lu_device *,
-			  struct ptlrpc_request *);
-
-	/* LDLM intent policy associated with quota locks */
-	int (*qmth_intent_policy)(const struct lu_env *, struct lu_device *,
-				  struct ptlrpc_request *, struct ldlm_lock **,
-				  int);
-
-	/* Initialize LVB of ldlm resource associated with quota objects */
-	int (*qmth_lvbo_init)(struct lu_device *, struct ldlm_resource *);
-
-	/* Update LVB of ldlm resource associated with quota objects */
-	int (*qmth_lvbo_update)(struct lu_device *, struct ldlm_resource *,
-				struct ptlrpc_request *, int);
-
-	/* Return size of LVB to be packed in ldlm message */
-	int (*qmth_lvbo_size)(struct lu_device *, struct ldlm_lock *);
-
-	/* Fill request buffer with lvb */
-	int (*qmth_lvbo_fill)(struct lu_device *, struct ldlm_lock *, void *,
-			      int);
-
-	/* Free lvb associated with ldlm resource */
-	int (*qmth_lvbo_free)(struct lu_device *, struct ldlm_resource *);
-};
-
-/* actual handlers are defined in lustre/quota/qmt_handler.c */
-extern struct qmt_handlers qmt_hdls;
-
-/*
- * Quota enforcement support on slaves
- */
-
-struct qsd_instance;
-
-/* The quota slave feature is implemented under the form of a library.
- * The API is the following:
- *
- * - qsd_init(): the user (mostly the OSD layer) should first allocate a qsd
- *	       instance via qsd_init(). This creates all required structures
- *	       to manage quota enforcement for this target and performs all
- *	       low-level initialization which does not involve any lustre
- *	       object. qsd_init() should typically be called when the OSD
- *	       is being set up.
- *
- * - qsd_prepare(): This sets up on-disk objects associated with the quota slave
- *		  feature and initiates the quota reintegration procedure if
- *		  needed. qsd_prepare() should typically be called when
- *		  ->ldo_prepare is invoked.
- *
- * - qsd_start(): a qsd instance should be started once recovery is completed
- *		(i.e. when ->ldo_recovery_complete is called). This is used
- *		to notify the qsd layer that quota should now be enforced
- *		again via the qsd_op_begin/end functions. The last step of the
- *		reintegration procedure (namely usage reconciliation) will be
- *		completed during start.
- *
- * - qsd_fini(): is used to release a qsd_instance structure allocated with
- *	       qsd_init(). This releases all quota slave objects and frees the
- *	       structures associated with the qsd_instance.
- *
- * - qsd_op_begin(): is used to enforce quota, it must be called in the
- *		   declaration of each operation. qsd_op_end() should then be
- *		   invoked later once all operations have been completed in
- *		   order to release/adjust the quota space.
- *		   Running qsd_op_begin() before qsd_start() isn't fatal and
- *		   will return success.
- *		   Once qsd_start() has been run, qsd_op_begin() will block
- *		   until the reintegration procedure is completed.
- *
- * - qsd_op_end(): performs the post operation quota processing. This must be
- *		 called after the operation transaction stopped.
- *		 While qsd_op_begin() must be invoked each time a new
- *		 operation is declared, qsd_op_end() should be called only
- *		 once for the whole transaction.
- *
- * - qsd_op_adjust(): triggers pre-acquire/release if necessary.
- *
- * Below are the function prototypes to be used by OSD layer to manage quota
- * enforcement. Arguments are documented where each function is defined.  */
-
-struct qsd_instance *qsd_init(const struct lu_env *, char *, struct dt_device *,
-			      struct proc_dir_entry *);
-int qsd_prepare(const struct lu_env *, struct qsd_instance *);
-int qsd_start(const struct lu_env *, struct qsd_instance *);
-void qsd_fini(const struct lu_env *, struct qsd_instance *);
-int qsd_op_begin(const struct lu_env *, struct qsd_instance *,
-		 struct lquota_trans *, struct lquota_id_info *, int *);
-void qsd_op_end(const struct lu_env *, struct qsd_instance *,
-		struct lquota_trans *);
-void qsd_op_adjust(const struct lu_env *, struct qsd_instance *,
-		   union lquota_id *, int);
-/* This is exported for the ldiskfs quota migration only,
- * see convert_quota_file() */
-int lquota_disk_write_glb(const struct lu_env *, struct dt_object *,
-			  __u64, struct lquota_glb_rec *);
-
-/*
- * Quota information attached to a transaction
- */
-
-struct lquota_entry;
-
-struct lquota_id_info {
-	/* quota identifier */
-	union lquota_id		 lqi_id;
-
-	/* USRQUOTA or GRPQUOTA for now, could be expanded for
-	 * directory quota or other types later.  */
-	int			 lqi_type;
-
-	/* inodes or kbytes to be consumed or released, it could
-	 * be negative when releasing space.  */
-	long long		 lqi_space;
-
-	/* quota slave entry structure associated with this ID */
-	struct lquota_entry	*lqi_qentry;
-
-	/* whether we are reporting blocks or inodes */
-	bool			 lqi_is_blk;
-};
-
-/* Since we enforce only inode quota in meta pool (MDTs), and block quota in
- * data pool (OSTs), there are at most 4 quota ids being enforced in a single
- * transaction, which is chown transaction:
- * original uid and gid, new uid and gid.
- *
- * This value might need to be revised when directory quota is added.  */
-#define QUOTA_MAX_TRANSIDS    4
-
-/* all qids involved in a single transaction */
-struct lquota_trans {
-	unsigned short		lqt_id_cnt;
-	struct lquota_id_info	lqt_ids[QUOTA_MAX_TRANSIDS];
-};
-
-/* flags for quota local enforcement */
-#define QUOTA_FL_OVER_USRQUOTA  0x01
-#define QUOTA_FL_OVER_GRPQUOTA  0x02
-#define QUOTA_FL_SYNC	   0x04
-
-#define IS_LQUOTA_RES(res)						\
-	(res->lr_name.name[LUSTRE_RES_ID_SEQ_OFF] == FID_SEQ_QUOTA ||	\
-	 res->lr_name.name[LUSTRE_RES_ID_SEQ_OFF] == FID_SEQ_QUOTA_GLB)
-
-/* helper function used by MDT & OFD to retrieve quota accounting information
- * on slave */
-int lquotactl_slv(const struct lu_env *, struct dt_device *,
-		  struct obd_quotactl *);
-/** @} quota */
-#endif /* _LUSTRE_QUOTA_H */
diff --git a/drivers/staging/lustre/lustre/include/lustre_sec.h b/drivers/staging/lustre/lustre/include/lustre_sec.h
index dff70a5b9bc4..707ff69717c6 100644
--- a/drivers/staging/lustre/lustre/include/lustre_sec.h
+++ b/drivers/staging/lustre/lustre/include/lustre_sec.h
@@ -1061,15 +1061,7 @@ const char *sec2target_str(struct ptlrpc_sec *sec);
 /*
  * lprocfs
  */
-#if defined (CONFIG_PROC_FS)
-struct proc_dir_entry;
-extern struct proc_dir_entry *sptlrpc_proc_root;
 int sptlrpc_lprocfs_cliobd_attach(struct obd_device *dev);
-#else
-#define sptlrpc_proc_root	NULL
-static inline int sptlrpc_lprocfs_cliobd_attach(struct obd_device *dev)
-{ return 0; }
-#endif
 
 /*
  * server side
diff --git a/drivers/staging/lustre/lustre/include/obd.h b/drivers/staging/lustre/lustre/include/obd.h
index 2a88b806fca5..55452e562bd4 100644
--- a/drivers/staging/lustre/lustre/include/obd.h
+++ b/drivers/staging/lustre/lustre/include/obd.h
@@ -244,11 +244,12 @@ struct obd_type {
 	struct list_head typ_chain;
 	struct obd_ops *typ_dt_ops;
 	struct md_ops *typ_md_ops;
-	struct proc_dir_entry *typ_procroot;
+	struct dentry *typ_debugfs_entry;
 	char *typ_name;
 	int  typ_refcnt;
 	struct lu_device_type *typ_lu;
 	spinlock_t obd_type_lock;
+	struct kobject *typ_kobj;
 };
 
 struct brw_page {
@@ -544,7 +545,7 @@ struct pool_desc {
 	struct lov_qos_rr     pool_rr;		/* round robin qos */
 	struct hlist_node      pool_hash;	      /* access by poolname */
 	struct list_head	    pool_list;	      /* serial access */
-	struct proc_dir_entry *pool_proc_entry;	/* file in /proc */
+	struct dentry		*pool_debugfs_entry;	/* file in /proc */
 	struct obd_device    *pool_lobd;	/* obd of the lov/lod to which
 						*  this pool belongs */
 };
@@ -565,13 +566,15 @@ struct lov_obd {
 	int		     lov_pool_count;
 	struct cfs_hash	     *lov_pools_hash_body; /* used for key access */
 	struct list_head	lov_pool_list; /* used for sequential access */
-	struct proc_dir_entry   *lov_pool_proc_entry;
+	struct dentry		*lov_pool_debugfs_entry;
 	enum lustre_sec_part    lov_sp_me;
 
 	/* Cached LRU pages from upper layer */
 	void		       *lov_cache;
 
 	struct rw_semaphore     lov_notify_lock;
+
+	struct kobject		*lov_tgts_kobj;
 };
 
 struct lmv_tgt_desc {
@@ -610,6 +613,7 @@ struct lmv_obd {
 	struct lmv_tgt_desc	**tgts;
 
 	struct obd_connect_data	conn_data;
+	struct kobject		*lmv_tgts_kobj;
 };
 
 struct niobuf_local {
@@ -841,9 +845,6 @@ struct obd_device {
 	struct cfs_hash	     *obd_uuid_hash;
 	/* nid-export hash body */
 	struct cfs_hash	     *obd_nid_hash;
-	/* nid stats body */
-	struct cfs_hash	     *obd_nid_stats_hash;
-	struct list_head	      obd_nid_stats;
 	atomic_t	    obd_refcount;
 	wait_queue_head_t	     obd_refcount_waitq;
 	struct list_head	      obd_exports;
@@ -913,10 +914,8 @@ struct obd_device {
 	unsigned int	   md_cntr_base;
 	struct lprocfs_stats  *md_stats;
 
-	struct proc_dir_entry  *obd_proc_entry;
-	void		  *obd_proc_private; /* type private PDEs */
-	struct proc_dir_entry  *obd_proc_exports_entry;
-	struct proc_dir_entry  *obd_svc_procroot;
+	struct dentry		*obd_debugfs_entry;
+	struct dentry		*obd_svc_debugfs_entry;
 	struct lprocfs_stats  *obd_svc_stats;
 	atomic_t	   obd_evict_inprogress;
 	wait_queue_head_t	    obd_evict_inprogress_waitq;
@@ -936,6 +935,9 @@ struct obd_device {
 	struct lu_ref	  obd_reference;
 
 	int		       obd_conn_inprogress;
+
+	struct kobject		obd_kobj; /* sysfs object */
+	struct completion	obd_kobj_unregister;
 };
 
 #define OBD_LLOG_FL_SENDNOW     0x0001
diff --git a/drivers/staging/lustre/lustre/include/obd_class.h b/drivers/staging/lustre/lustre/include/obd_class.h
index 34b5fa3f081c..36ed78127830 100644
--- a/drivers/staging/lustre/lustre/include/obd_class.h
+++ b/drivers/staging/lustre/lustre/include/obd_class.h
@@ -75,8 +75,7 @@ struct lu_device_type;
 extern struct list_head obd_types;
 struct obd_export *class_conn2export(struct lustre_handle *);
 int class_register_type(struct obd_ops *, struct md_ops *,
-			struct lprocfs_vars *, const char *nm,
-			struct lu_device_type *ldt);
+			const char *nm, struct lu_device_type *ldt);
 int class_unregister_type(const char *nm);
 
 struct obd_device *class_newdev(const char *type_name, const char *name);
@@ -140,14 +139,7 @@ int class_add_conn(struct obd_device *obd, struct lustre_cfg *lcfg);
 int class_add_uuid(const char *uuid, __u64 nid);
 
 /*obdecho*/
-#if defined (CONFIG_PROC_FS)
 extern void lprocfs_echo_init_vars(struct lprocfs_static_vars *lvars);
-#else
-static inline void lprocfs_echo_init_vars(struct lprocfs_static_vars *lvars)
-{
-	memset(lvars, 0, sizeof(*lvars));
-}
-#endif
 
 #define CFG_F_START     0x01   /* Set when we start updating from a log */
 #define CFG_F_MARKER    0x02   /* We are within a maker */
@@ -357,7 +349,6 @@ static inline int obd_check_dev_active(struct obd_device *obd)
 	return rc;
 }
 
-#if defined (CONFIG_PROC_FS)
 #define OBD_COUNTER_OFFSET(op)				  \
 	((offsetof(struct obd_ops, o_ ## op) -		  \
 	  offsetof(struct obd_ops, o_iocontrol))		\
@@ -379,10 +370,6 @@ static inline int obd_check_dev_active(struct obd_device *obd)
 			OBD_COUNTER_OFFSET(op);			      \
 		LASSERT(coffset < (export)->exp_obd->obd_stats->ls_num);     \
 		lprocfs_counter_incr((export)->exp_obd->obd_stats, coffset); \
-		if ((export)->exp_nid_stats != NULL &&		       \
-		    (export)->exp_nid_stats->nid_stats != NULL)	      \
-			lprocfs_counter_incr(				\
-				(export)->exp_nid_stats->nid_stats, coffset);\
 	}
 
 #define MD_COUNTER_OFFSET(op)				   \
@@ -411,27 +398,6 @@ static inline int obd_check_dev_active(struct obd_device *obd)
 				(export)->exp_md_stats, coffset);	    \
 	}
 
-#else
-#define OBD_COUNTER_OFFSET(op)
-#define OBD_COUNTER_INCREMENT(obd, op)
-#define EXP_COUNTER_INCREMENT(exp, op)
-#define MD_COUNTER_INCREMENT(obd, op)
-#define EXP_MD_COUNTER_INCREMENT(exp, op)
-#endif
-
-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
-						  ,LPROCFS_STATS_FLAG_NOPERCPU);
-	if (tmp->nid_ldlm_stats == NULL)
-		return -ENOMEM;
-
-	lprocfs_init_ldlm_stats(tmp->nid_ldlm_stats);
-
-	return lprocfs_register_stats(tmp->nid_proc, "ldlm_stats",
-				      tmp->nid_ldlm_stats);
-}
 
 #define OBD_CHECK_MD_OP(obd, op, err)			   \
 do {							    \
diff --git a/drivers/staging/lustre/lustre/include/obd_support.h b/drivers/staging/lustre/lustre/include/obd_support.h
index 2991d2ee780b..73e2d4880b9b 100644
--- a/drivers/staging/lustre/lustre/include/obd_support.h
+++ b/drivers/staging/lustre/lustre/include/obd_support.h
@@ -509,7 +509,6 @@ extern atomic_t libcfs_kmemory;
 
 extern void obd_update_maxusage(void);
 
-#if defined (CONFIG_PROC_FS)
 #define obd_memory_add(size)						  \
 	lprocfs_counter_add(obd_memory, OBD_MEMORY_STAT, (long)(size))
 #define obd_memory_sub(size)						  \
@@ -530,46 +529,6 @@ extern void obd_update_maxusage(void);
 extern __u64 obd_memory_max(void);
 extern __u64 obd_pages_max(void);
 
-#else
-
-extern __u64 obd_alloc;
-extern __u64 obd_pages;
-
-extern __u64 obd_max_alloc;
-extern __u64 obd_max_pages;
-
-static inline void obd_memory_add(long size)
-{
-	obd_alloc += size;
-	if (obd_alloc > obd_max_alloc)
-		obd_max_alloc = obd_alloc;
-}
-
-static inline void obd_memory_sub(long size)
-{
-	obd_alloc -= size;
-}
-
-static inline void obd_pages_add(int order)
-{
-	obd_pages += 1<< order;
-	if (obd_pages > obd_max_pages)
-		obd_max_pages = obd_pages;
-}
-
-static inline void obd_pages_sub(int order)
-{
-	obd_pages -= 1<< order;
-}
-
-#define obd_memory_sum() (obd_alloc)
-#define obd_pages_sum()  (obd_pages)
-
-#define obd_memory_max() (obd_max_alloc)
-#define obd_pages_max() (obd_max_pages)
-
-#endif
-
 #define OBD_DEBUG_MEMUSAGE (1)
 
 #if OBD_DEBUG_MEMUSAGE
@@ -676,37 +635,20 @@ do {									      \
 	 __OBD_VMALLOC_VEROBSE(ptr, cptab, cpt, size)
 
 
-/* Allocations above this size are considered too big and could not be done
- * atomically.
- *
- * Be very careful when changing this value, especially when decreasing it,
- * since vmalloc in Linux doesn't perform well on multi-cores system, calling
- * vmalloc in critical path would hurt performance badly. See LU-66.
- */
-#define OBD_ALLOC_BIG (4 * PAGE_CACHE_SIZE)
-
 #define OBD_ALLOC_LARGE(ptr, size)					    \
 do {									  \
-	if (size > OBD_ALLOC_BIG)					     \
-		OBD_VMALLOC(ptr, size);				       \
-	else								  \
-		OBD_ALLOC(ptr, size);					 \
+	ptr = libcfs_kvzalloc(size, GFP_NOFS);				  \
 } while (0)
 
 #define OBD_CPT_ALLOC_LARGE(ptr, cptab, cpt, size)			      \
 do {									      \
-	if (size > OBD_ALLOC_BIG)					      \
-		OBD_CPT_VMALLOC(ptr, cptab, cpt, size);			      \
-	else								      \
-		OBD_CPT_ALLOC(ptr, cptab, cpt, size);			      \
+	ptr = libcfs_kvzalloc_cpt(cptab, cpt, size, GFP_NOFS);		      \
 } while (0)
 
 #define OBD_FREE_LARGE(ptr, size)					     \
 do {									  \
-	if (size > OBD_ALLOC_BIG)					     \
-		OBD_VFREE(ptr, size);					 \
-	else								  \
-		OBD_FREE(ptr, size);					  \
+	(void)(size);							\
+	kvfree(ptr);							  \
 } while (0)
 
 
diff --git a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
index ab6cb419302f..e0c1ccafbd63 100644
--- a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
+++ b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
@@ -202,7 +202,7 @@ struct lu_device *ccc_device_alloc(const struct lu_env *env,
 	struct cl_site    *site;
 	int rc;
 
-	OBD_ALLOC_PTR(vdv);
+	vdv = kzalloc(sizeof(*vdv), GFP_NOFS);
 	if (vdv == NULL)
 		return ERR_PTR(-ENOMEM);
 
@@ -211,7 +211,7 @@ struct lu_device *ccc_device_alloc(const struct lu_env *env,
 	ccc2lu_dev(vdv)->ld_ops = luops;
 	vdv->cdv_cl.cd_ops = clops;
 
-	OBD_ALLOC_PTR(site);
+	site = kzalloc(sizeof(*site), GFP_NOFS);
 	if (site != NULL) {
 		rc = cl_site_init(site, &vdv->cdv_cl);
 		if (rc == 0)
@@ -219,7 +219,7 @@ struct lu_device *ccc_device_alloc(const struct lu_env *env,
 		else {
 			LASSERT(lud->ld_site == NULL);
 			CERROR("Cannot init lu_site, rc %d.\n", rc);
-			OBD_FREE_PTR(site);
+			kfree(site);
 		}
 	} else
 		rc = -ENOMEM;
@@ -239,10 +239,10 @@ struct lu_device *ccc_device_free(const struct lu_env *env,
 
 	if (d->ld_site != NULL) {
 		cl_site_fini(site);
-		OBD_FREE_PTR(site);
+		kfree(site);
 	}
 	cl_device_fini(lu2cl_dev(d));
-	OBD_FREE_PTR(vdv);
+	kfree(vdv);
 	return next;
 }
 
@@ -836,25 +836,24 @@ int ccc_prep_size(const struct lu_env *env, struct cl_object *obj,
 					*exceed = 1;
 			}
 			return result;
-		} else {
-			/*
-			 * region is within kms and, hence, within real file
-			 * size (A). We need to increase i_size to cover the
-			 * read region so that generic_file_read() will do its
-			 * job, but that doesn't mean the kms size is
-			 * _correct_, it is only the _minimum_ size. If
-			 * someone does a stat they will get the correct size
-			 * which will always be >= the kms value here.
-			 * b=11081
-			 */
-			if (cl_isize_read(inode) < kms) {
-				cl_isize_write_nolock(inode, kms);
-				CDEBUG(D_VFSTRACE,
-				       DFID" updating i_size %llu\n",
-				       PFID(lu_object_fid(&obj->co_lu)),
-				       (__u64)cl_isize_read(inode));
+		}
+		/*
+		 * region is within kms and, hence, within real file
+		 * size (A). We need to increase i_size to cover the
+		 * read region so that generic_file_read() will do its
+		 * job, but that doesn't mean the kms size is
+		 * _correct_, it is only the _minimum_ size. If
+		 * someone does a stat they will get the correct size
+		 * which will always be >= the kms value here.
+		 * b=11081
+		 */
+		if (cl_isize_read(inode) < kms) {
+			cl_isize_write_nolock(inode, kms);
+			CDEBUG(D_VFSTRACE,
+					DFID" updating i_size %llu\n",
+					PFID(lu_object_fid(&obj->co_lu)),
+					(__u64)cl_isize_read(inode));
 
-			}
 		}
 	}
 	ccc_object_size_unlock(obj);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
index 70b909f55861..6601e6b12c32 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
@@ -197,8 +197,7 @@ void ldlm_destroy_flock_export(struct obd_export *exp);
 void l_check_ns_lock(struct ldlm_namespace *ns);
 void l_check_no_ns_lock(struct ldlm_namespace *ns);
 
-extern struct proc_dir_entry *ldlm_svc_proc_dir;
-extern struct proc_dir_entry *ldlm_type_proc_dir;
+extern struct dentry *ldlm_svc_debugfs_dir;
 
 struct ldlm_state {
 	struct ptlrpc_service *ldlm_cb_service;
@@ -238,40 +237,85 @@ enum ldlm_policy_res {
 
 typedef enum ldlm_policy_res ldlm_policy_res_t;
 
-#define LDLM_POOL_PROC_READER_SEQ_SHOW(var, type)			    \
-	static int lprocfs_##var##_seq_show(struct seq_file *m, void *v) \
+#define LDLM_POOL_SYSFS_PRINT_int(v) sprintf(buf, "%d\n", v)
+#define LDLM_POOL_SYSFS_SET_int(a, b) { a = b; }
+#define LDLM_POOL_SYSFS_PRINT_u64(v) sprintf(buf, "%lld\n", v)
+#define LDLM_POOL_SYSFS_SET_u64(a, b) { a = b; }
+#define LDLM_POOL_SYSFS_PRINT_atomic(v) sprintf(buf, "%d\n", atomic_read(&v))
+#define LDLM_POOL_SYSFS_SET_atomic(a, b) atomic_set(&a, b)
+
+#define LDLM_POOL_SYSFS_READER_SHOW(var, type)				    \
+	static ssize_t var##_show(struct kobject *kobj,			    \
+				  struct attribute *attr,		    \
+				  char *buf)				    \
 	{								    \
-		struct ldlm_pool *pl = m->private;			    \
+		struct ldlm_pool *pl = container_of(kobj, struct ldlm_pool, \
+						    pl_kobj);		    \
 		type tmp;						    \
 									    \
 		spin_lock(&pl->pl_lock);				    \
 		tmp = pl->pl_##var;					    \
 		spin_unlock(&pl->pl_lock);				    \
 									    \
-		return lprocfs_rd_uint(m, &tmp);			    \
+		return LDLM_POOL_SYSFS_PRINT_##type(tmp);		    \
 	}								    \
 	struct __##var##__dummy_read {; } /* semicolon catcher */
 
-#define LDLM_POOL_PROC_WRITER(var, type)				    \
-	static int lprocfs_wr_##var(struct file *file,			    \
-				const char __user *buffer,		    \
-				unsigned long count, void *data)	    \
+#define LDLM_POOL_SYSFS_WRITER_STORE(var, type)				    \
+	static ssize_t var##_store(struct kobject *kobj,		    \
+				     struct attribute *attr,		    \
+				     const char *buffer,		    \
+				     size_t count)			    \
 	{								    \
-		struct ldlm_pool *pl = data;				    \
-		type tmp;						    \
+		struct ldlm_pool *pl = container_of(kobj, struct ldlm_pool, \
+						    pl_kobj);		    \
+		unsigned long tmp;					    \
 		int rc;							    \
 									    \
-		rc = lprocfs_wr_uint(file, buffer, count, &tmp);	    \
+		rc = kstrtoul(buffer, 10, &tmp);			    \
 		if (rc < 0) {						    \
-			CERROR("Can't parse user input, rc = %d\n", rc);    \
 			return rc;					    \
 		}							    \
 									    \
 		spin_lock(&pl->pl_lock);				    \
-		pl->pl_##var = tmp;					    \
+		LDLM_POOL_SYSFS_SET_##type(pl->pl_##var, tmp);		    \
 		spin_unlock(&pl->pl_lock);				    \
 									    \
-		return rc;						    \
+		return count;						    \
+	}								    \
+	struct __##var##__dummy_write {; } /* semicolon catcher */
+
+#define LDLM_POOL_SYSFS_READER_NOLOCK_SHOW(var, type)			    \
+	static ssize_t var##_show(struct kobject *kobj,		    \
+				    struct attribute *attr,		    \
+				    char *buf)				    \
+	{								    \
+		struct ldlm_pool *pl = container_of(kobj, struct ldlm_pool, \
+						    pl_kobj);		    \
+									    \
+		return LDLM_POOL_SYSFS_PRINT_##type(pl->pl_##var);	    \
+	}								    \
+	struct __##var##__dummy_read {; } /* semicolon catcher */
+
+#define LDLM_POOL_SYSFS_WRITER_NOLOCK_STORE(var, type)			    \
+	static ssize_t var##_store(struct kobject *kobj,		    \
+				     struct attribute *attr,		    \
+				     const char *buffer,		    \
+				     size_t count)			    \
+	{								    \
+		struct ldlm_pool *pl = container_of(kobj, struct ldlm_pool, \
+						    pl_kobj);		    \
+		unsigned long tmp;					    \
+		int rc;							    \
+									    \
+		rc = kstrtoul(buffer, 10, &tmp);			    \
+		if (rc < 0) {						    \
+			return rc;					    \
+		}							    \
+									    \
+		LDLM_POOL_SYSFS_SET_##type(pl->pl_##var, tmp);		    \
+									    \
+		return count;						    \
 	}								    \
 	struct __##var##__dummy_write {; } /* semicolon catcher */
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
index c5c86e73ca52..764f98684d74 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
@@ -73,7 +73,7 @@ static int import_set_conn(struct obd_import *imp, struct obd_uuid *uuid,
 	}
 
 	if (create) {
-		OBD_ALLOC(imp_conn, sizeof(*imp_conn));
+		imp_conn = kzalloc(sizeof(*imp_conn), GFP_NOFS);
 		if (!imp_conn) {
 			rc = -ENOMEM;
 			goto out_put;
@@ -119,8 +119,7 @@ static int import_set_conn(struct obd_import *imp, struct obd_uuid *uuid,
 	spin_unlock(&imp->imp_lock);
 	return 0;
 out_free:
-	if (imp_conn)
-		OBD_FREE(imp_conn, sizeof(*imp_conn));
+	kfree(imp_conn);
 out_put:
 	ptlrpc_connection_put(ptlrpc_conn);
 	return rc;
@@ -179,7 +178,7 @@ int client_import_del_conn(struct obd_import *imp, struct obd_uuid *uuid)
 
 		list_del(&imp_conn->oic_item);
 		ptlrpc_connection_put(imp_conn->oic_conn);
-		OBD_FREE(imp_conn, sizeof(*imp_conn));
+		kfree(imp_conn);
 		CDEBUG(D_HA, "imp %p@%s: remove connection %s\n",
 		       imp, imp->imp_obd->obd_name, uuid->uuid);
 		rc = 0;
@@ -668,10 +667,9 @@ int target_send_reply_msg(struct ptlrpc_request *req, int rc, int fail_id)
 		DEBUG_REQ(D_NET, req, "processing error (%d)", rc);
 		req->rq_status = rc;
 		return ptlrpc_send_error(req, 1);
-	} else {
-		DEBUG_REQ(D_NET, req, "sending reply");
 	}
 
+	DEBUG_REQ(D_NET, req, "sending reply");
 	return ptlrpc_send_reply(req, PTLRPC_REPLY_MAYBE_DIFFICULT);
 }
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
index 84b111eb48fa..bb2246d3b22b 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
@@ -208,8 +208,7 @@ void ldlm_lock_put(struct ldlm_lock *lock)
 			lock->l_export = NULL;
 		}
 
-		if (lock->l_lvb_data != NULL)
-			OBD_FREE(lock->l_lvb_data, lock->l_lvb_len);
+		kfree(lock->l_lvb_data);
 
 		ldlm_interval_free(ldlm_interval_detach(lock));
 		lu_ref_fini(&lock->l_reference);
@@ -932,7 +931,9 @@ static void search_granted_lock(struct list_head *queue,
 			prev->mode_link = &mode_end->l_sl_mode;
 			prev->policy_link = &req->l_sl_policy;
 			return;
-		} else if (lock->l_resource->lr_type == LDLM_IBITS) {
+		}
+
+		if (lock->l_resource->lr_type == LDLM_IBITS) {
 			for (;;) {
 				policy_end =
 					list_entry(lock->l_sl_policy.prev,
@@ -968,11 +969,10 @@ static void search_granted_lock(struct list_head *queue,
 			prev->mode_link = &mode_end->l_sl_mode;
 			prev->policy_link = &req->l_sl_policy;
 			return;
-		} else {
-			LDLM_ERROR(lock,
-				   "is not LDLM_PLAIN or LDLM_IBITS lock");
-			LBUG();
 		}
+
+		LDLM_ERROR(lock, "is not LDLM_PLAIN or LDLM_IBITS lock");
+		LBUG();
 	}
 
 	/* insert point is last lock on the queue,
@@ -1527,7 +1527,7 @@ struct ldlm_lock *ldlm_lock_create(struct ldlm_namespace *ns,
 
 	if (lvb_len) {
 		lock->l_lvb_len = lvb_len;
-		OBD_ALLOC(lock->l_lvb_data, lvb_len);
+		lock->l_lvb_data = kzalloc(lvb_len, GFP_NOFS);
 		if (lock->l_lvb_data == NULL)
 			goto out;
 	}
@@ -1791,7 +1791,7 @@ int ldlm_work_gl_ast_lock(struct ptlrpc_request_set *rqset, void *opaq)
 	LDLM_LOCK_RELEASE(lock);
 
 	if ((gl_work->gl_flags & LDLM_GL_WORK_NOFREE) == 0)
-		OBD_FREE_PTR(gl_work);
+		kfree(gl_work);
 
 	return rc;
 }
@@ -1812,7 +1812,7 @@ int ldlm_run_ast_work(struct ldlm_namespace *ns, struct list_head *rpc_list,
 	if (list_empty(rpc_list))
 		return 0;
 
-	OBD_ALLOC_PTR(arg);
+	arg = kzalloc(sizeof(*arg), GFP_NOFS);
 	if (arg == NULL)
 		return -ENOMEM;
 
@@ -1857,7 +1857,7 @@ int ldlm_run_ast_work(struct ldlm_namespace *ns, struct list_head *rpc_list,
 	rc = atomic_read(&arg->restart) ? -ERESTART : 0;
 	goto out;
 out:
-	OBD_FREE_PTR(arg);
+	kfree(arg);
 	return rc;
 }
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
index 08a91f5d91b1..b7b6ca1196b7 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
@@ -58,6 +58,10 @@ MODULE_PARM_DESC(ldlm_cpts, "CPU partitions ldlm threads should run on");
 static struct mutex	ldlm_ref_mutex;
 static int ldlm_refcount;
 
+struct kobject *ldlm_kobj;
+struct kset *ldlm_ns_kset;
+struct kset *ldlm_svc_kset;
+
 struct ldlm_cb_async_args {
 	struct ldlm_cb_set_arg *ca_set_arg;
 	struct ldlm_lock       *ca_lock;
@@ -220,7 +224,7 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 						     * variable length */
 			void *lvb_data;
 
-			OBD_ALLOC(lvb_data, lvb_len);
+			lvb_data = kzalloc(lvb_len, GFP_NOFS);
 			if (lvb_data == NULL) {
 				LDLM_ERROR(lock, "No memory: %d.\n", lvb_len);
 				rc = -ENOMEM;
@@ -448,7 +452,7 @@ static int ldlm_bl_to_thread(struct ldlm_namespace *ns,
 	if (cancel_flags & LCF_ASYNC) {
 		struct ldlm_bl_work_item *blwi;
 
-		OBD_ALLOC(blwi, sizeof(*blwi));
+		blwi = kzalloc(sizeof(*blwi), GFP_NOFS);
 		if (blwi == NULL)
 			return -ENOMEM;
 		init_blwi(blwi, ns, ld, cancels, count, lock, cancel_flags);
@@ -849,7 +853,7 @@ static int ldlm_bl_thread_main(void *arg)
 			memory_pressure_clr();
 
 		if (blwi->blwi_flags & LCF_ASYNC)
-			OBD_FREE(blwi, sizeof(*blwi));
+			kfree(blwi);
 		else
 			complete(&blwi->blwi_comp);
 	}
@@ -1002,6 +1006,42 @@ void ldlm_destroy_export(struct obd_export *exp)
 }
 EXPORT_SYMBOL(ldlm_destroy_export);
 
+extern unsigned int ldlm_cancel_unused_locks_before_replay;
+
+static ssize_t cancel_unused_locks_before_replay_show(struct kobject *kobj,
+						      struct attribute *attr,
+						      char *buf)
+{
+	return sprintf(buf, "%d\n", ldlm_cancel_unused_locks_before_replay);
+}
+static ssize_t cancel_unused_locks_before_replay_store(struct kobject *kobj,
+						       struct attribute *attr,
+						       const char *buffer,
+						       size_t count)
+{
+	int rc;
+	unsigned long val;
+
+	rc = kstrtoul(buffer, 10, &val);
+	if (rc)
+		return rc;
+
+	ldlm_cancel_unused_locks_before_replay = val;
+
+	return count;
+}
+LUSTRE_RW_ATTR(cancel_unused_locks_before_replay);
+
+/* These are for root of /sys/fs/lustre/ldlm */
+static struct attribute *ldlm_attrs[] = {
+	&lustre_attr_cancel_unused_locks_before_replay.attr,
+	NULL,
+};
+
+static struct attribute_group ldlm_attr_group = {
+	.attrs = ldlm_attrs,
+};
+
 static int ldlm_setup(void)
 {
 	static struct ptlrpc_service_conf	conf;
@@ -1012,11 +1052,33 @@ static int ldlm_setup(void)
 	if (ldlm_state != NULL)
 		return -EALREADY;
 
-	OBD_ALLOC(ldlm_state, sizeof(*ldlm_state));
+	ldlm_state = kzalloc(sizeof(*ldlm_state), GFP_NOFS);
 	if (ldlm_state == NULL)
 		return -ENOMEM;
 
-	rc = ldlm_proc_setup();
+	ldlm_kobj = kobject_create_and_add("ldlm", lustre_kobj);
+	if (!ldlm_kobj) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	rc = sysfs_create_group(ldlm_kobj, &ldlm_attr_group);
+	if (rc)
+		goto out;
+
+	ldlm_ns_kset = kset_create_and_add("namespaces", NULL, ldlm_kobj);
+	if (!ldlm_ns_kset) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	ldlm_svc_kset = kset_create_and_add("services", NULL, ldlm_kobj);
+	if (!ldlm_svc_kset) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	rc = ldlm_debugfs_setup();
 	if (rc != 0)
 		goto out;
 
@@ -1050,7 +1112,8 @@ static int ldlm_setup(void)
 		},
 	};
 	ldlm_state->ldlm_cb_service =
-			ptlrpc_register_service(&conf, ldlm_svc_proc_dir);
+			ptlrpc_register_service(&conf, ldlm_svc_kset,
+						ldlm_svc_debugfs_dir);
 	if (IS_ERR(ldlm_state->ldlm_cb_service)) {
 		CERROR("failed to start service\n");
 		rc = PTR_ERR(ldlm_state->ldlm_cb_service);
@@ -1059,7 +1122,7 @@ static int ldlm_setup(void)
 	}
 
 
-	OBD_ALLOC(blp, sizeof(*blp));
+	blp = kzalloc(sizeof(*blp), GFP_NOFS);
 	if (blp == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -1088,7 +1151,6 @@ static int ldlm_setup(void)
 			goto out;
 	}
 
-
 	rc = ldlm_pools_init();
 	if (rc) {
 		CERROR("Failed to initialize LDLM pools: %d\n", rc);
@@ -1129,16 +1191,22 @@ static int ldlm_cleanup(void)
 			wait_for_completion(&blp->blp_comp);
 		}
 
-		OBD_FREE(blp, sizeof(*blp));
+		kfree(blp);
 	}
 
 	if (ldlm_state->ldlm_cb_service != NULL)
 		ptlrpc_unregister_service(ldlm_state->ldlm_cb_service);
 
-	ldlm_proc_cleanup();
+	if (ldlm_ns_kset)
+		kset_unregister(ldlm_ns_kset);
+	if (ldlm_svc_kset)
+		kset_unregister(ldlm_svc_kset);
+	if (ldlm_kobj)
+		kobject_put(ldlm_kobj);
 
+	ldlm_debugfs_cleanup();
 
-	OBD_FREE(ldlm_state, sizeof(*ldlm_state));
+	kfree(ldlm_state);
 	ldlm_state = NULL;
 
 	return 0;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
index a9f4833e03e5..1605b9c69271 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
@@ -90,10 +90,10 @@
  * pl_server_lock_volume - Current server lock volume (calculated);
  *
  * As it may be seen from list above, we have few possible tunables which may
- * affect behavior much. They all may be modified via proc. However, they also
+ * affect behavior much. They all may be modified via sysfs. However, they also
  * give a possibility for constructing few pre-defined behavior policies. If
  * none of predefines is suitable for a working pattern being used, new one may
- * be "constructed" via proc tunables.
+ * be "constructed" via sysfs tunables.
  */
 
 #define DEBUG_SUBSYSTEM S_LDLM
@@ -654,7 +654,6 @@ int ldlm_pool_setup(struct ldlm_pool *pl, int limit)
 }
 EXPORT_SYMBOL(ldlm_pool_setup);
 
-#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,9 +695,12 @@ static int lprocfs_pool_state_seq_show(struct seq_file *m, void *unused)
 }
 LPROC_SEQ_FOPS_RO(lprocfs_pool_state);
 
-static int lprocfs_grant_speed_seq_show(struct seq_file *m, void *unused)
+static ssize_t grant_speed_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct ldlm_pool *pl = m->private;
+	struct ldlm_pool *pl = container_of(kobj, struct ldlm_pool,
+					    pl_kobj);
+
 	int	       grant_speed;
 
 	spin_lock(&pl->pl_lock);
@@ -706,63 +708,109 @@ static int lprocfs_grant_speed_seq_show(struct seq_file *m, void *unused)
 	grant_speed = atomic_read(&pl->pl_grant_rate) -
 			atomic_read(&pl->pl_cancel_rate);
 	spin_unlock(&pl->pl_lock);
-	return lprocfs_rd_uint(m, &grant_speed);
+	return sprintf(buf, "%d\n", grant_speed);
 }
+LUSTRE_RO_ATTR(grant_speed);
 
-LDLM_POOL_PROC_READER_SEQ_SHOW(grant_plan, int);
-LPROC_SEQ_FOPS_RO(lprocfs_grant_plan);
+LDLM_POOL_SYSFS_READER_SHOW(grant_plan, int);
+LUSTRE_RO_ATTR(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 __user *buf,
-					       size_t len, loff_t *off)
-{
-	struct seq_file *seq = file->private_data;
+LDLM_POOL_SYSFS_READER_SHOW(recalc_period, int);
+LDLM_POOL_SYSFS_WRITER_STORE(recalc_period, int);
+LUSTRE_RW_ATTR(recalc_period);
 
-	return lprocfs_wr_recalc_period(file, buf, len, seq->private);
-}
-LPROC_SEQ_FOPS(lprocfs_recalc_period);
+LDLM_POOL_SYSFS_READER_NOLOCK_SHOW(server_lock_volume, u64);
+LUSTRE_RO_ATTR(server_lock_volume);
 
-LPROC_SEQ_FOPS_RO_TYPE(ldlm_pool, u64);
-LPROC_SEQ_FOPS_RO_TYPE(ldlm_pool, atomic);
-LPROC_SEQ_FOPS_RW_TYPE(ldlm_pool_rw, atomic);
+LDLM_POOL_SYSFS_READER_NOLOCK_SHOW(limit, atomic);
+LDLM_POOL_SYSFS_WRITER_NOLOCK_STORE(limit, atomic);
+LUSTRE_RW_ATTR(limit);
 
-LPROC_SEQ_FOPS_RO(lprocfs_grant_speed);
+LDLM_POOL_SYSFS_READER_NOLOCK_SHOW(granted, atomic);
+LUSTRE_RO_ATTR(granted);
+
+LDLM_POOL_SYSFS_READER_NOLOCK_SHOW(cancel_rate, atomic);
+LUSTRE_RO_ATTR(cancel_rate);
+
+LDLM_POOL_SYSFS_READER_NOLOCK_SHOW(grant_rate, atomic);
+LUSTRE_RO_ATTR(grant_rate);
+
+LDLM_POOL_SYSFS_READER_NOLOCK_SHOW(lock_volume_factor, atomic);
+LDLM_POOL_SYSFS_WRITER_NOLOCK_STORE(lock_volume_factor, atomic);
+LUSTRE_RW_ATTR(lock_volume_factor);
 
 #define LDLM_POOL_ADD_VAR(name, var, ops)			\
 	do {							\
 		snprintf(var_name, MAX_STRING_SIZE, #name);	\
 		pool_vars[0].data = var;			\
 		pool_vars[0].fops = ops;			\
-		lprocfs_add_vars(pl->pl_proc_dir, pool_vars, NULL);\
+		ldebugfs_add_vars(pl->pl_debugfs_entry, pool_vars, NULL);\
 	} while (0)
 
-static int ldlm_pool_proc_init(struct ldlm_pool *pl)
+/* These are for pools in /sys/fs/lustre/ldlm/namespaces/.../pool */
+static struct attribute *ldlm_pl_attrs[] = {
+	&lustre_attr_grant_speed.attr,
+	&lustre_attr_grant_plan.attr,
+	&lustre_attr_recalc_period.attr,
+	&lustre_attr_server_lock_volume.attr,
+	&lustre_attr_limit.attr,
+	&lustre_attr_granted.attr,
+	&lustre_attr_cancel_rate.attr,
+	&lustre_attr_grant_rate.attr,
+	&lustre_attr_lock_volume_factor.attr,
+	NULL,
+};
+
+static void ldlm_pl_release(struct kobject *kobj)
+{
+	struct ldlm_pool *pl = container_of(kobj, struct ldlm_pool,
+					    pl_kobj);
+	complete(&pl->pl_kobj_unregister);
+}
+
+static struct kobj_type ldlm_pl_ktype = {
+	.default_attrs	= ldlm_pl_attrs,
+	.sysfs_ops	= &lustre_sysfs_ops,
+	.release	= ldlm_pl_release,
+};
+
+static int ldlm_pool_sysfs_init(struct ldlm_pool *pl)
+{
+	struct ldlm_namespace *ns = ldlm_pl2ns(pl);
+	int err;
+
+	init_completion(&pl->pl_kobj_unregister);
+	err = kobject_init_and_add(&pl->pl_kobj, &ldlm_pl_ktype, &ns->ns_kobj,
+				   "pool");
+
+	return err;
+}
+
+static int ldlm_pool_debugfs_init(struct ldlm_pool *pl)
 {
 	struct ldlm_namespace *ns = ldlm_pl2ns(pl);
-	struct proc_dir_entry *parent_ns_proc;
+	struct dentry *debugfs_ns_parent;
 	struct lprocfs_vars pool_vars[2];
 	char *var_name = NULL;
 	int rc = 0;
 
-	OBD_ALLOC(var_name, MAX_STRING_SIZE + 1);
+	var_name = kzalloc(MAX_STRING_SIZE + 1, GFP_NOFS);
 	if (!var_name)
 		return -ENOMEM;
 
-	parent_ns_proc = ns->ns_proc_dir_entry;
-	if (parent_ns_proc == NULL) {
-		CERROR("%s: proc entry is not initialized\n",
+	debugfs_ns_parent = ns->ns_debugfs_entry;
+	if (IS_ERR_OR_NULL(debugfs_ns_parent)) {
+		CERROR("%s: debugfs entry is not initialized\n",
 		       ldlm_ns_name(ns));
 		rc = -EINVAL;
 		goto out_free_name;
 	}
-	pl->pl_proc_dir = lprocfs_register("pool", parent_ns_proc,
-					   NULL, NULL);
-	if (IS_ERR(pl->pl_proc_dir)) {
-		CERROR("LProcFS failed in ldlm-pool-init\n");
-		rc = PTR_ERR(pl->pl_proc_dir);
-		pl->pl_proc_dir = NULL;
+	pl->pl_debugfs_entry = ldebugfs_register("pool", debugfs_ns_parent,
+						 NULL, NULL);
+	if (IS_ERR(pl->pl_debugfs_entry)) {
+		CERROR("LdebugFS failed in ldlm-pool-init\n");
+		rc = PTR_ERR(pl->pl_debugfs_entry);
+		pl->pl_debugfs_entry = NULL;
 		goto out_free_name;
 	}
 
@@ -770,20 +818,7 @@ static int ldlm_pool_proc_init(struct ldlm_pool *pl)
 	memset(pool_vars, 0, sizeof(pool_vars));
 	pool_vars[0].name = var_name;
 
-	LDLM_POOL_ADD_VAR("server_lock_volume", &pl->pl_server_lock_volume,
-			  &ldlm_pool_u64_fops);
-	LDLM_POOL_ADD_VAR("limit", &pl->pl_limit, &ldlm_pool_rw_atomic_fops);
-	LDLM_POOL_ADD_VAR("granted", &pl->pl_granted, &ldlm_pool_atomic_fops);
-	LDLM_POOL_ADD_VAR("grant_speed", pl, &lprocfs_grant_speed_fops);
-	LDLM_POOL_ADD_VAR("cancel_rate", &pl->pl_cancel_rate,
-			  &ldlm_pool_atomic_fops);
-	LDLM_POOL_ADD_VAR("grant_rate", &pl->pl_grant_rate,
-			  &ldlm_pool_atomic_fops);
-	LDLM_POOL_ADD_VAR("grant_plan", pl, &lprocfs_grant_plan_fops);
-	LDLM_POOL_ADD_VAR("recalc_period", pl, &lprocfs_recalc_period_fops);
-	LDLM_POOL_ADD_VAR("lock_volume_factor", &pl->pl_lock_volume_factor,
-			  &ldlm_pool_rw_atomic_fops);
-	LDLM_POOL_ADD_VAR("state", pl, &lprocfs_pool_state_fops);
+	LDLM_POOL_ADD_VAR(state, pl, &lprocfs_pool_state_fops);
 
 	pl->pl_stats = lprocfs_alloc_stats(LDLM_POOL_LAST_STAT -
 					   LDLM_POOL_FIRST_STAT, 0);
@@ -825,32 +860,31 @@ static int ldlm_pool_proc_init(struct ldlm_pool *pl)
 	lprocfs_counter_init(pl->pl_stats, LDLM_POOL_TIMING_STAT,
 			     LPROCFS_CNTR_AVGMINMAX | LPROCFS_CNTR_STDDEV,
 			     "recalc_timing", "sec");
-	rc = lprocfs_register_stats(pl->pl_proc_dir, "stats", pl->pl_stats);
+	rc = ldebugfs_register_stats(pl->pl_debugfs_entry, "stats",
+				     pl->pl_stats);
 
 out_free_name:
-	OBD_FREE(var_name, MAX_STRING_SIZE + 1);
+	kfree(var_name);
 	return rc;
 }
 
-static void ldlm_pool_proc_fini(struct ldlm_pool *pl)
+static void ldlm_pool_sysfs_fini(struct ldlm_pool *pl)
+{
+	kobject_put(&pl->pl_kobj);
+	wait_for_completion(&pl->pl_kobj_unregister);
+}
+
+static void ldlm_pool_debugfs_fini(struct ldlm_pool *pl)
 {
 	if (pl->pl_stats != NULL) {
 		lprocfs_free_stats(&pl->pl_stats);
 		pl->pl_stats = NULL;
 	}
-	if (pl->pl_proc_dir != NULL) {
-		lprocfs_remove(&pl->pl_proc_dir);
-		pl->pl_proc_dir = NULL;
+	if (pl->pl_debugfs_entry != NULL) {
+		ldebugfs_remove(&pl->pl_debugfs_entry);
+		pl->pl_debugfs_entry = NULL;
 	}
 }
-#else /* !CONFIG_PROC_FS */
-static int ldlm_pool_proc_init(struct ldlm_pool *pl)
-{
-	return 0;
-}
-
-static void ldlm_pool_proc_fini(struct ldlm_pool *pl) {}
-#endif /* CONFIG_PROC_FS */
 
 int ldlm_pool_init(struct ldlm_pool *pl, struct ldlm_namespace *ns,
 		   int idx, ldlm_side_t client)
@@ -881,7 +915,11 @@ int ldlm_pool_init(struct ldlm_pool *pl, struct ldlm_namespace *ns,
 		pl->pl_recalc_period = LDLM_POOL_CLI_DEF_RECALC_PERIOD;
 	}
 	pl->pl_client_lock_volume = 0;
-	rc = ldlm_pool_proc_init(pl);
+	rc = ldlm_pool_debugfs_init(pl);
+	if (rc)
+		return rc;
+
+	rc = ldlm_pool_sysfs_init(pl);
 	if (rc)
 		return rc;
 
@@ -893,7 +931,8 @@ EXPORT_SYMBOL(ldlm_pool_init);
 
 void ldlm_pool_fini(struct ldlm_pool *pl)
 {
-	ldlm_pool_proc_fini(pl);
+	ldlm_pool_sysfs_fini(pl);
+	ldlm_pool_debugfs_fini(pl);
 
 	/*
 	 * Pool should not be used after this point. We can't free it here as
@@ -1362,8 +1401,7 @@ static int ldlm_pools_thread_main(void *arg)
 
 		if (thread_test_and_clear_flags(thread, SVC_STOPPING))
 			break;
-		else
-			thread_test_and_clear_flags(thread, SVC_EVENT);
+		thread_test_and_clear_flags(thread, SVC_EVENT);
 	}
 
 	thread_set_flags(thread, SVC_STOPPED);
@@ -1383,7 +1421,7 @@ static int ldlm_pools_thread_start(void)
 	if (ldlm_pools_thread != NULL)
 		return -EALREADY;
 
-	OBD_ALLOC_PTR(ldlm_pools_thread);
+	ldlm_pools_thread = kzalloc(sizeof(*ldlm_pools_thread), GFP_NOFS);
 	if (ldlm_pools_thread == NULL)
 		return -ENOMEM;
 
@@ -1394,7 +1432,7 @@ static int ldlm_pools_thread_start(void)
 			   "ldlm_poold");
 	if (IS_ERR(task)) {
 		CERROR("Can't start pool thread, error %ld\n", PTR_ERR(task));
-		OBD_FREE(ldlm_pools_thread, sizeof(*ldlm_pools_thread));
+		kfree(ldlm_pools_thread);
 		ldlm_pools_thread = NULL;
 		return PTR_ERR(task);
 	}
@@ -1417,7 +1455,7 @@ static void ldlm_pools_thread_stop(void)
 	 * in pools thread.
 	 */
 	wait_for_completion(&ldlm_pools_comp);
-	OBD_FREE_PTR(ldlm_pools_thread);
+	kfree(ldlm_pools_thread);
 	ldlm_pools_thread = NULL;
 }
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
index 4f713183145b..6245a2c36a0f 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
@@ -1462,7 +1462,7 @@ static ldlm_policy_res_t ldlm_cancel_lrur_policy(struct ldlm_namespace *ns,
 			      lock->l_last_used));
 	lv = lvf * la * unused;
 
-	/* Inform pool about current CLV to see it via proc. */
+	/* Inform pool about current CLV to see it via debugfs. */
 	ldlm_pool_set_clv(pl, lv);
 
 	/* Stop when SLV is not yet come from server or lv is smaller than
@@ -1472,7 +1472,7 @@ static ldlm_policy_res_t ldlm_cancel_lrur_policy(struct ldlm_namespace *ns,
 }
 
 /**
- * Callback function for proc used policy. Makes decision whether to keep
+ * Callback function for debugfs used policy. Makes decision whether to keep
  * \a lock in LRU for current \a LRU size \a unused, added in current scan \a
  * added and number of locks to be preferably canceled \a count.
  *
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index f750d42a7ad5..cdb63665a113 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -61,19 +61,17 @@ LIST_HEAD(ldlm_cli_active_namespace_list);
 /* Client namespaces that don't have any locks in them */
 LIST_HEAD(ldlm_cli_inactive_namespace_list);
 
-struct proc_dir_entry *ldlm_type_proc_dir = NULL;
-static struct proc_dir_entry *ldlm_ns_proc_dir = NULL;
-struct proc_dir_entry *ldlm_svc_proc_dir = NULL;
-
-extern unsigned int ldlm_cancel_unused_locks_before_replay;
+static struct dentry *ldlm_debugfs_dir;
+static struct dentry *ldlm_ns_debugfs_dir;
+struct dentry *ldlm_svc_debugfs_dir;
 
 /* during debug dump certain amount of granted locks for one resource to avoid
  * DDOS. */
 unsigned int ldlm_dump_granted_max = 256;
 
-#if defined(CONFIG_PROC_FS)
-static ssize_t lprocfs_wr_dump_ns(struct file *file, const char __user *buffer,
-				  size_t count, loff_t *off)
+static ssize_t
+lprocfs_wr_dump_ns(struct file *file, const char __user *buffer,
+		   size_t count, loff_t *off)
 {
 	ldlm_dump_all_namespaces(LDLM_NAMESPACE_SERVER, D_DLMTRACE);
 	ldlm_dump_all_namespaces(LDLM_NAMESPACE_CLIENT, D_DLMTRACE);
@@ -82,81 +80,82 @@ static ssize_t lprocfs_wr_dump_ns(struct file *file, const char __user *buffer,
 LPROC_SEQ_FOPS_WR_ONLY(ldlm, dump_ns);
 
 LPROC_SEQ_FOPS_RW_TYPE(ldlm_rw, uint);
-LPROC_SEQ_FOPS_RO_TYPE(ldlm, uint);
 
-int ldlm_proc_setup(void)
+static struct lprocfs_vars ldlm_debugfs_list[] = {
+	{ "dump_namespaces", &ldlm_dump_ns_fops, NULL, 0222 },
+	{ "dump_granted_max", &ldlm_rw_uint_fops, &ldlm_dump_granted_max },
+	{ NULL }
+};
+
+int ldlm_debugfs_setup(void)
 {
 	int rc;
-	struct lprocfs_vars list[] = {
-		{ "dump_namespaces", &ldlm_dump_ns_fops, NULL, 0222 },
-		{ "dump_granted_max", &ldlm_rw_uint_fops,
-		  &ldlm_dump_granted_max },
-		{ "cancel_unused_locks_before_replay", &ldlm_rw_uint_fops,
-		  &ldlm_cancel_unused_locks_before_replay },
-		{ NULL } };
-	LASSERT(ldlm_ns_proc_dir == NULL);
-
-	ldlm_type_proc_dir = lprocfs_register(OBD_LDLM_DEVICENAME,
-					      proc_lustre_root,
-					      NULL, NULL);
-	if (IS_ERR(ldlm_type_proc_dir)) {
+
+	ldlm_debugfs_dir = ldebugfs_register(OBD_LDLM_DEVICENAME,
+					     debugfs_lustre_root,
+					     NULL, NULL);
+	if (IS_ERR_OR_NULL(ldlm_debugfs_dir)) {
 		CERROR("LProcFS failed in ldlm-init\n");
-		rc = PTR_ERR(ldlm_type_proc_dir);
+		rc = ldlm_debugfs_dir ? PTR_ERR(ldlm_debugfs_dir) : -ENOMEM;
 		goto err;
 	}
 
-	ldlm_ns_proc_dir = lprocfs_register("namespaces",
-					    ldlm_type_proc_dir,
-					    NULL, NULL);
-	if (IS_ERR(ldlm_ns_proc_dir)) {
+	ldlm_ns_debugfs_dir = ldebugfs_register("namespaces",
+						ldlm_debugfs_dir,
+						NULL, NULL);
+	if (IS_ERR_OR_NULL(ldlm_ns_debugfs_dir)) {
 		CERROR("LProcFS failed in ldlm-init\n");
-		rc = PTR_ERR(ldlm_ns_proc_dir);
+		rc = ldlm_ns_debugfs_dir ? PTR_ERR(ldlm_ns_debugfs_dir)
+					 : -ENOMEM;
 		goto err_type;
 	}
 
-	ldlm_svc_proc_dir = lprocfs_register("services",
-					    ldlm_type_proc_dir,
-					    NULL, NULL);
-	if (IS_ERR(ldlm_svc_proc_dir)) {
+	ldlm_svc_debugfs_dir = ldebugfs_register("services",
+						 ldlm_debugfs_dir,
+						 NULL, NULL);
+	if (IS_ERR_OR_NULL(ldlm_svc_debugfs_dir)) {
 		CERROR("LProcFS failed in ldlm-init\n");
-		rc = PTR_ERR(ldlm_svc_proc_dir);
+		rc = ldlm_svc_debugfs_dir ? PTR_ERR(ldlm_svc_debugfs_dir)
+					  : -ENOMEM;
 		goto err_ns;
 	}
 
-	rc = lprocfs_add_vars(ldlm_type_proc_dir, list, NULL);
+	rc = ldebugfs_add_vars(ldlm_debugfs_dir, ldlm_debugfs_list, NULL);
 
 	return 0;
 
 err_ns:
-	lprocfs_remove(&ldlm_ns_proc_dir);
+	ldebugfs_remove(&ldlm_ns_debugfs_dir);
 err_type:
-	lprocfs_remove(&ldlm_type_proc_dir);
+	ldebugfs_remove(&ldlm_debugfs_dir);
 err:
-	ldlm_svc_proc_dir = NULL;
-	ldlm_type_proc_dir = NULL;
-	ldlm_ns_proc_dir = NULL;
+	ldlm_svc_debugfs_dir = NULL;
+	ldlm_ns_debugfs_dir = NULL;
+	ldlm_debugfs_dir = NULL;
 	return rc;
 }
 
-void ldlm_proc_cleanup(void)
+void ldlm_debugfs_cleanup(void)
 {
-	if (ldlm_svc_proc_dir)
-		lprocfs_remove(&ldlm_svc_proc_dir);
+	if (!IS_ERR_OR_NULL(ldlm_svc_debugfs_dir))
+		ldebugfs_remove(&ldlm_svc_debugfs_dir);
 
-	if (ldlm_ns_proc_dir)
-		lprocfs_remove(&ldlm_ns_proc_dir);
+	if (!IS_ERR_OR_NULL(ldlm_ns_debugfs_dir))
+		ldebugfs_remove(&ldlm_ns_debugfs_dir);
 
-	if (ldlm_type_proc_dir)
-		lprocfs_remove(&ldlm_type_proc_dir);
+	if (!IS_ERR_OR_NULL(ldlm_debugfs_dir))
+		ldebugfs_remove(&ldlm_debugfs_dir);
 
-	ldlm_svc_proc_dir = NULL;
-	ldlm_type_proc_dir = NULL;
-	ldlm_ns_proc_dir = NULL;
+	ldlm_svc_debugfs_dir = NULL;
+	ldlm_ns_debugfs_dir = NULL;
+	ldlm_debugfs_dir = NULL;
 }
 
-static int lprocfs_ns_resources_seq_show(struct seq_file *m, void *v)
+static ssize_t resource_count_show(struct kobject *kobj, struct attribute *attr,
+				   char *buf)
 {
-	struct ldlm_namespace *ns  = m->private;
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
 	__u64		  res = 0;
 	struct cfs_hash_bd	  bd;
 	int		    i;
@@ -164,46 +163,56 @@ static int lprocfs_ns_resources_seq_show(struct seq_file *m, void *v)
 	/* result is not strictly consistent */
 	cfs_hash_for_each_bucket(ns->ns_rs_hash, &bd, i)
 		res += cfs_hash_bd_count_get(&bd);
-	return lprocfs_rd_u64(m, &res);
+	return sprintf(buf, "%lld\n", res);
 }
-LPROC_SEQ_FOPS_RO(lprocfs_ns_resources);
+LUSTRE_RO_ATTR(resource_count);
 
-static int lprocfs_ns_locks_seq_show(struct seq_file *m, void *v)
+static ssize_t lock_count_show(struct kobject *kobj, struct attribute *attr,
+			       char *buf)
 {
-	struct ldlm_namespace *ns = m->private;
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
 	__u64		  locks;
 
 	locks = lprocfs_stats_collector(ns->ns_stats, LDLM_NSS_LOCKS,
 					LPROCFS_FIELDS_FLAGS_SUM);
-	return lprocfs_rd_u64(m, &locks);
+	return sprintf(buf, "%lld\n", locks);
+}
+LUSTRE_RO_ATTR(lock_count);
+
+static ssize_t lock_unused_count_show(struct kobject *kobj,
+				      struct attribute *attr,
+				      char *buf)
+{
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
+
+	return sprintf(buf, "%d\n", ns->ns_nr_unused);
 }
-LPROC_SEQ_FOPS_RO(lprocfs_ns_locks);
+LUSTRE_RO_ATTR(lock_unused_count);
 
-static int lprocfs_lru_size_seq_show(struct seq_file *m, void *v)
+static ssize_t lru_size_show(struct kobject *kobj, struct attribute *attr,
+			     char *buf)
 {
-	struct ldlm_namespace *ns = m->private;
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
 	__u32 *nr = &ns->ns_max_unused;
 
 	if (ns_connect_lru_resize(ns))
 		nr = &ns->ns_nr_unused;
-	return lprocfs_rd_uint(m, nr);
+	return sprintf(buf, "%u", *nr);
 }
 
-static ssize_t lprocfs_lru_size_seq_write(struct file *file,
-					const char __user *buffer,
-					size_t count, loff_t *off)
+static ssize_t lru_size_store(struct kobject *kobj, struct attribute *attr,
+			      const char *buffer, size_t count)
 {
-	struct ldlm_namespace *ns = ((struct seq_file *)file->private_data)->private;
-	char dummy[MAX_STRING_SIZE + 1];
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
 	unsigned long tmp;
 	int lru_resize;
 	int err;
 
-	dummy[MAX_STRING_SIZE] = '\0';
-	if (copy_from_user(dummy, buffer, MAX_STRING_SIZE))
-		return -EFAULT;
-
-	if (strncmp(dummy, "clear", 5) == 0) {
+	if (strncmp(buffer, "clear", 5) == 0) {
 		CDEBUG(D_DLMTRACE,
 		       "dropping all unused locks from namespace %s\n",
 		       ldlm_ns_name(ns));
@@ -229,9 +238,9 @@ static ssize_t lprocfs_lru_size_seq_write(struct file *file,
 		return count;
 	}
 
-	err = kstrtoul(dummy, 10, &tmp);
+	err = kstrtoul(buffer, 10, &tmp);
 	if (err != 0) {
-		CERROR("invalid value written\n");
+		CERROR("lru_size: invalid value written\n");
 		return -EINVAL;
 	}
 	lru_resize = (tmp == 0);
@@ -277,25 +286,56 @@ static ssize_t lprocfs_lru_size_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(lprocfs_lru_size);
+LUSTRE_RW_ATTR(lru_size);
 
-static int lprocfs_elc_seq_show(struct seq_file *m, void *v)
+static ssize_t lru_max_age_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct ldlm_namespace *ns = m->private;
-	unsigned int supp = ns_connect_cancelset(ns);
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
 
-	return lprocfs_rd_uint(m, &supp);
+	return sprintf(buf, "%u", ns->ns_max_age);
 }
 
-static ssize_t lprocfs_elc_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t lru_max_age_store(struct kobject *kobj, struct attribute *attr,
+				 const char *buffer, size_t count)
 {
-	struct ldlm_namespace *ns = ((struct seq_file *)file->private_data)->private;
-	unsigned int supp = -1;
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
+	unsigned long tmp;
+	int err;
+
+	err = kstrtoul(buffer, 10, &tmp);
+	if (err != 0)
+		return -EINVAL;
+
+	ns->ns_max_age = tmp;
+
+	return count;
+}
+LUSTRE_RW_ATTR(lru_max_age);
+
+static ssize_t early_lock_cancel_show(struct kobject *kobj,
+				      struct attribute *attr,
+				      char *buf)
+{
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
+
+	return sprintf(buf, "%d\n", ns_connect_cancelset(ns));
+}
+
+static ssize_t early_lock_cancel_store(struct kobject *kobj,
+				       struct attribute *attr,
+				       const char *buffer,
+				       size_t count)
+{
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
+	unsigned long supp = -1;
 	int rc;
 
-	rc = lprocfs_wr_uint(file, buffer, count, &supp);
+	rc = kstrtoul(buffer, 10, &supp);
 	if (rc < 0)
 		return rc;
 
@@ -305,91 +345,88 @@ static ssize_t lprocfs_elc_seq_write(struct file *file,
 		ns->ns_connect_flags |= OBD_CONNECT_CANCELSET;
 	return count;
 }
-LPROC_SEQ_FOPS(lprocfs_elc);
+LUSTRE_RW_ATTR(early_lock_cancel);
+
+/* These are for namespaces in /sys/fs/lustre/ldlm/namespaces/ */
+static struct attribute *ldlm_ns_attrs[] = {
+	&lustre_attr_resource_count.attr,
+	&lustre_attr_lock_count.attr,
+	&lustre_attr_lock_unused_count.attr,
+	&lustre_attr_lru_size.attr,
+	&lustre_attr_lru_max_age.attr,
+	&lustre_attr_early_lock_cancel.attr,
+	NULL,
+};
+
+static void ldlm_ns_release(struct kobject *kobj)
+{
+	struct ldlm_namespace *ns = container_of(kobj, struct ldlm_namespace,
+						 ns_kobj);
+	complete(&ns->ns_kobj_unregister);
+}
 
-void ldlm_namespace_proc_unregister(struct ldlm_namespace *ns)
+static struct kobj_type ldlm_ns_ktype = {
+	.default_attrs  = ldlm_ns_attrs,
+	.sysfs_ops      = &lustre_sysfs_ops,
+	.release        = ldlm_ns_release,
+};
+
+static void ldlm_namespace_debugfs_unregister(struct ldlm_namespace *ns)
 {
-	if (ns->ns_proc_dir_entry == NULL)
+	if (IS_ERR_OR_NULL(ns->ns_debugfs_entry))
 		CERROR("dlm namespace %s has no procfs dir?\n",
 		       ldlm_ns_name(ns));
 	else
-		lprocfs_remove(&ns->ns_proc_dir_entry);
+		ldebugfs_remove(&ns->ns_debugfs_entry);
 
 	if (ns->ns_stats != NULL)
 		lprocfs_free_stats(&ns->ns_stats);
 }
 
-#define LDLM_NS_ADD_VAR(name, var, ops)				\
-	do {							\
-		snprintf(lock_name, MAX_STRING_SIZE, name);	\
-		lock_vars[0].data = var;			\
-		lock_vars[0].fops = ops;			\
-		lprocfs_add_vars(ns_pde, lock_vars, NULL);	\
-	} while (0)
-
-int ldlm_namespace_proc_register(struct ldlm_namespace *ns)
+void ldlm_namespace_sysfs_unregister(struct ldlm_namespace *ns)
 {
-	struct lprocfs_vars lock_vars[2];
-	char lock_name[MAX_STRING_SIZE + 1];
-	struct proc_dir_entry *ns_pde;
+	kobject_put(&ns->ns_kobj);
+	wait_for_completion(&ns->ns_kobj_unregister);
+}
 
-	LASSERT(ns != NULL);
-	LASSERT(ns->ns_rs_hash != NULL);
+int ldlm_namespace_sysfs_register(struct ldlm_namespace *ns)
+{
+	int err;
 
-	if (ns->ns_proc_dir_entry != NULL) {
-		ns_pde = ns->ns_proc_dir_entry;
-	} else {
-		ns_pde = proc_mkdir(ldlm_ns_name(ns), ldlm_ns_proc_dir);
-		if (ns_pde == NULL)
-			return -ENOMEM;
-		ns->ns_proc_dir_entry = ns_pde;
-	}
+	ns->ns_kobj.kset = ldlm_ns_kset;
+	init_completion(&ns->ns_kobj_unregister);
+	err = kobject_init_and_add(&ns->ns_kobj, &ldlm_ns_ktype, NULL,
+				   "%s", ldlm_ns_name(ns));
 
 	ns->ns_stats = lprocfs_alloc_stats(LDLM_NSS_LAST, 0);
-	if (ns->ns_stats == NULL)
+	if (ns->ns_stats == NULL) {
+		kobject_put(&ns->ns_kobj);
 		return -ENOMEM;
+	}
 
 	lprocfs_counter_init(ns->ns_stats, LDLM_NSS_LOCKS,
 			     LPROCFS_CNTR_AVGMINMAX, "locks", "locks");
 
-	lock_name[MAX_STRING_SIZE] = '\0';
-
-	memset(lock_vars, 0, sizeof(lock_vars));
-	lock_vars[0].name = lock_name;
+	return err;
+}
 
-	LDLM_NS_ADD_VAR("resource_count", ns, &lprocfs_ns_resources_fops);
-	LDLM_NS_ADD_VAR("lock_count", ns, &lprocfs_ns_locks_fops);
+static int ldlm_namespace_debugfs_register(struct ldlm_namespace *ns)
+{
+	struct dentry *ns_entry;
 
-	if (ns_is_client(ns)) {
-		LDLM_NS_ADD_VAR("lock_unused_count", &ns->ns_nr_unused,
-				&ldlm_uint_fops);
-		LDLM_NS_ADD_VAR("lru_size", ns, &lprocfs_lru_size_fops);
-		LDLM_NS_ADD_VAR("lru_max_age", &ns->ns_max_age,
-				&ldlm_rw_uint_fops);
-		LDLM_NS_ADD_VAR("early_lock_cancel", ns, &lprocfs_elc_fops);
+	if (!IS_ERR_OR_NULL(ns->ns_debugfs_entry)) {
+		ns_entry = ns->ns_debugfs_entry;
 	} else {
-		LDLM_NS_ADD_VAR("ctime_age_limit", &ns->ns_ctime_age_limit,
-				&ldlm_rw_uint_fops);
-		LDLM_NS_ADD_VAR("lock_timeouts", &ns->ns_timeouts,
-				&ldlm_uint_fops);
-		LDLM_NS_ADD_VAR("max_nolock_bytes", &ns->ns_max_nolock_size,
-				&ldlm_rw_uint_fops);
-		LDLM_NS_ADD_VAR("contention_seconds", &ns->ns_contention_time,
-				&ldlm_rw_uint_fops);
-		LDLM_NS_ADD_VAR("contended_locks", &ns->ns_contended_locks,
-				&ldlm_rw_uint_fops);
-		LDLM_NS_ADD_VAR("max_parallel_ast", &ns->ns_max_parallel_ast,
-				&ldlm_rw_uint_fops);
+		ns_entry = debugfs_create_dir(ldlm_ns_name(ns),
+					      ldlm_ns_debugfs_dir);
+		if (ns_entry == NULL)
+			return -ENOMEM;
+		ns->ns_debugfs_entry = ns_entry;
 	}
+
 	return 0;
 }
 #undef MAX_STRING_SIZE
-#else /* CONFIG_PROC_FS */
-
-#define ldlm_namespace_proc_unregister(ns)      ({; })
-#define ldlm_namespace_proc_register(ns)	({0; })
-
-#endif /* CONFIG_PROC_FS */
 
 static unsigned ldlm_res_hop_hash(struct cfs_hash *hs,
 				  const void *key, unsigned mask)
@@ -590,7 +627,7 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 			break;
 	}
 
-	OBD_ALLOC_PTR(ns);
+	ns = kzalloc(sizeof(*ns), GFP_NOFS);
 	if (!ns)
 		goto out_ref;
 
@@ -623,25 +660,26 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 	atomic_set(&ns->ns_bref, 0);
 	init_waitqueue_head(&ns->ns_waitq);
 
-	ns->ns_max_nolock_size    = NS_DEFAULT_MAX_NOLOCK_BYTES;
-	ns->ns_contention_time    = NS_DEFAULT_CONTENTION_SECONDS;
-	ns->ns_contended_locks    = NS_DEFAULT_CONTENDED_LOCKS;
-
 	ns->ns_max_parallel_ast   = LDLM_DEFAULT_PARALLEL_AST_LIMIT;
 	ns->ns_nr_unused	  = 0;
 	ns->ns_max_unused	 = LDLM_DEFAULT_LRU_SIZE;
 	ns->ns_max_age	    = LDLM_DEFAULT_MAX_ALIVE;
-	ns->ns_ctime_age_limit    = LDLM_CTIME_AGE_LIMIT;
-	ns->ns_timeouts	   = 0;
 	ns->ns_orig_connect_flags = 0;
 	ns->ns_connect_flags      = 0;
 	ns->ns_stopping	   = 0;
-	rc = ldlm_namespace_proc_register(ns);
+
+	rc = ldlm_namespace_sysfs_register(ns);
 	if (rc != 0) {
-		CERROR("Can't initialize ns proc, rc %d\n", rc);
+		CERROR("Can't initialize ns sysfs, rc %d\n", rc);
 		goto out_hash;
 	}
 
+	rc = ldlm_namespace_debugfs_register(ns);
+	if (rc != 0) {
+		CERROR("Can't initialize ns proc, rc %d\n", rc);
+		goto out_sysfs;
+	}
+
 	idx = ldlm_namespace_nr_read(client);
 	rc = ldlm_pool_init(&ns->ns_pool, ns, idx, client);
 	if (rc) {
@@ -652,12 +690,14 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 	ldlm_namespace_register(ns, client);
 	return ns;
 out_proc:
-	ldlm_namespace_proc_unregister(ns);
+	ldlm_namespace_debugfs_unregister(ns);
+out_sysfs:
+	ldlm_namespace_sysfs_unregister(ns);
 	ldlm_namespace_cleanup(ns, 0);
 out_hash:
 	cfs_hash_putref(ns->ns_rs_hash);
 out_ns:
-	OBD_FREE_PTR(ns);
+	kfree(ns);
 out_ref:
 	ldlm_put_ref();
 	return NULL;
@@ -898,13 +938,13 @@ void ldlm_namespace_free_post(struct ldlm_namespace *ns)
 	 * Removing it after @dir may cause oops. */
 	ldlm_pool_fini(&ns->ns_pool);
 
-	ldlm_namespace_proc_unregister(ns);
+	ldlm_namespace_debugfs_unregister(ns);
 	cfs_hash_putref(ns->ns_rs_hash);
 	/* Namespace \a ns should be not on list at this time, otherwise
 	 * this will cause issues related to using freed \a ns in poold
 	 * thread. */
 	LASSERT(list_empty(&ns->ns_list_chain));
-	OBD_FREE_PTR(ns);
+	kfree(ns);
 	ldlm_put_ref();
 }
 
@@ -915,7 +955,7 @@ void ldlm_namespace_free_post(struct ldlm_namespace *ns)
  * proc1: destroy import
  *	class_disconnect_export(grab cl_sem) ->
  *	      -> ldlm_namespace_free ->
- *	      -> lprocfs_remove(grab _lprocfs_lock).
+ *	      -> ldebugfs_remove(grab _lprocfs_lock).
  * proc2: read proc info
  *	lprocfs_fops_read(grab _lprocfs_lock) ->
  *	      -> osc_rd_active, etc(grab cl_sem).
@@ -1137,10 +1177,8 @@ ldlm_resource_get(struct ldlm_namespace *ns, struct ldlm_resource *parent,
 			CERROR("%s: lvbo_init failed for resource %#llx:%#llx: rc = %d\n",
 			       ns->ns_obd->obd_name, name->name[0],
 			       name->name[1], rc);
-			if (res->lr_lvb_data) {
-				OBD_FREE(res->lr_lvb_data, res->lr_lvb_len);
-				res->lr_lvb_data = NULL;
-			}
+			kfree(res->lr_lvb_data);
+			res->lr_lvb_data = NULL;
 			res->lr_lvb_len = rc;
 			mutex_unlock(&res->lr_lvb_mutex);
 			ldlm_resource_putref(res);
diff --git a/drivers/staging/lustre/lustre/libcfs/Makefile b/drivers/staging/lustre/lustre/libcfs/Makefile
index 2996a48a31fb..ec98f44a10dd 100644
--- a/drivers/staging/lustre/lustre/libcfs/Makefile
+++ b/drivers/staging/lustre/lustre/libcfs/Makefile
@@ -2,11 +2,11 @@ obj-$(CONFIG_LUSTRE_FS) += libcfs.o
 
 libcfs-linux-objs := linux-tracefile.o linux-debug.o
 libcfs-linux-objs += linux-prim.o linux-cpu.o
-libcfs-linux-objs += linux-tcpip.o
 libcfs-linux-objs += linux-curproc.o
 libcfs-linux-objs += linux-module.o
 libcfs-linux-objs += linux-crypto.o
 libcfs-linux-objs += linux-crypto-adler.o
+libcfs-linux-objs += linux-mem.o
 
 libcfs-linux-objs := $(addprefix linux/,$(libcfs-linux-objs))
 
diff --git a/drivers/staging/lustre/lustre/libcfs/fail.c b/drivers/staging/lustre/lustre/libcfs/fail.c
index 92444b0fe2a3..7b7fc215e633 100644
--- a/drivers/staging/lustre/lustre/libcfs/fail.c
+++ b/drivers/staging/lustre/lustre/libcfs/fail.c
@@ -41,7 +41,7 @@ EXPORT_SYMBOL(cfs_fail_loc);
 unsigned int cfs_fail_val = 0;
 EXPORT_SYMBOL(cfs_fail_val);
 
-wait_queue_head_t cfs_race_waitq;
+DECLARE_WAIT_QUEUE_HEAD(cfs_race_waitq);
 EXPORT_SYMBOL(cfs_race_waitq);
 
 int cfs_race_state;
diff --git a/drivers/staging/lustre/lustre/libcfs/hash.c b/drivers/staging/lustre/lustre/libcfs/hash.c
index a55567e0de9e..0ed063145230 100644
--- a/drivers/staging/lustre/lustre/libcfs/hash.c
+++ b/drivers/staging/lustre/lustre/libcfs/hash.c
@@ -427,31 +427,31 @@ cfs_hash_dd_hnode_del(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 }
 
 static cfs_hash_hlist_ops_t cfs_hash_hh_hops = {
-       .hop_hhead      = cfs_hash_hh_hhead,
-       .hop_hhead_size = cfs_hash_hh_hhead_size,
-       .hop_hnode_add  = cfs_hash_hh_hnode_add,
-       .hop_hnode_del  = cfs_hash_hh_hnode_del,
+	.hop_hhead      = cfs_hash_hh_hhead,
+	.hop_hhead_size = cfs_hash_hh_hhead_size,
+	.hop_hnode_add  = cfs_hash_hh_hnode_add,
+	.hop_hnode_del  = cfs_hash_hh_hnode_del,
 };
 
 static cfs_hash_hlist_ops_t cfs_hash_hd_hops = {
-       .hop_hhead      = cfs_hash_hd_hhead,
-       .hop_hhead_size = cfs_hash_hd_hhead_size,
-       .hop_hnode_add  = cfs_hash_hd_hnode_add,
-       .hop_hnode_del  = cfs_hash_hd_hnode_del,
+	.hop_hhead      = cfs_hash_hd_hhead,
+	.hop_hhead_size = cfs_hash_hd_hhead_size,
+	.hop_hnode_add  = cfs_hash_hd_hnode_add,
+	.hop_hnode_del  = cfs_hash_hd_hnode_del,
 };
 
 static cfs_hash_hlist_ops_t cfs_hash_dh_hops = {
-       .hop_hhead      = cfs_hash_dh_hhead,
-       .hop_hhead_size = cfs_hash_dh_hhead_size,
-       .hop_hnode_add  = cfs_hash_dh_hnode_add,
-       .hop_hnode_del  = cfs_hash_dh_hnode_del,
+	.hop_hhead      = cfs_hash_dh_hhead,
+	.hop_hhead_size = cfs_hash_dh_hhead_size,
+	.hop_hnode_add  = cfs_hash_dh_hnode_add,
+	.hop_hnode_del  = cfs_hash_dh_hnode_del,
 };
 
 static cfs_hash_hlist_ops_t cfs_hash_dd_hops = {
-       .hop_hhead      = cfs_hash_dd_hhead,
-       .hop_hhead_size = cfs_hash_dd_hhead_size,
-       .hop_hnode_add  = cfs_hash_dd_hnode_add,
-       .hop_hnode_del  = cfs_hash_dd_hnode_del,
+	.hop_hhead      = cfs_hash_dd_hhead,
+	.hop_hhead_size = cfs_hash_dd_hhead_size,
+	.hop_hnode_add  = cfs_hash_dd_hnode_add,
+	.hop_hnode_del  = cfs_hash_dd_hnode_del,
 };
 
 static void
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
index cc3ab351943e..f9262243f935 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
@@ -87,7 +87,7 @@ static void cfs_cpu_core_siblings(int cpu, cpumask_t *mask)
 /* return cpumask of HTs in the same core */
 static void cfs_cpu_ht_siblings(int cpu, cpumask_t *mask)
 {
-	cpumask_copy(mask, topology_thread_cpumask(cpu));
+	cpumask_copy(mask, topology_sibling_cpumask(cpu));
 }
 
 static void cfs_node_to_cpumask(int node, cpumask_t *mask)
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-mem.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-mem.c
new file mode 100644
index 000000000000..025e2f0028ab
--- /dev/null
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-mem.c
@@ -0,0 +1,59 @@
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 only,
+ * 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 version 2 for more details (a copy is included
+ * in the LICENSE file that accompanied this code).
+ *
+ * You should have received a copy of the GNU General Public License
+ * version 2 along with this program; If not, see
+ * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
+ *
+ */
+/*
+ * This file creates a memory allocation primitive for Lustre, that
+ * allows to fallback to vmalloc allocations should regular kernel allocations
+ * fail due to size or system memory fragmentation.
+ *
+ * Author: Oleg Drokin <green@linuxhacker.ru>
+ *
+ */
+/*
+ * This file is part of Lustre, http://www.lustre.org/
+ * Lustre is a trademark of Seagate Technology.
+ */
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+
+#include "../../../include/linux/libcfs/libcfs.h"
+
+void *libcfs_kvzalloc(size_t size, gfp_t flags)
+{
+	void *ret;
+
+	ret = kzalloc(size, flags | __GFP_NOWARN);
+	if (!ret)
+		ret = __vmalloc(size, flags | __GFP_ZERO, PAGE_KERNEL);
+	return ret;
+}
+EXPORT_SYMBOL(libcfs_kvzalloc);
+
+void *libcfs_kvzalloc_cpt(struct cfs_cpt_table *cptab, int cpt, size_t size,
+			  gfp_t flags)
+{
+	void *ret;
+
+	ret = kzalloc_node(size, flags | __GFP_NOWARN,
+			   cfs_cpt_spread_node(cptab, cpt));
+	if (!ret) {
+		WARN_ON(!(flags & (__GFP_FS|__GFP_HIGH)));
+		ret = vmalloc_node(size, cfs_cpt_spread_node(cptab, cpt));
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(libcfs_kvzalloc_cpt);
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-tracefile.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-tracefile.c
index c8e293002e07..eb10e3b478aa 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-tracefile.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-tracefile.c
@@ -49,7 +49,7 @@ static unsigned int pages_factor[CFS_TCD_TYPE_MAX] = {
 
 char *cfs_trace_console_buffers[NR_CPUS][CFS_TCD_TYPE_MAX];
 
-struct rw_semaphore cfs_tracefile_sem;
+static DECLARE_RWSEM(cfs_tracefile_sem);
 
 int cfs_tracefile_init_arch(void)
 {
@@ -57,8 +57,6 @@ int cfs_tracefile_init_arch(void)
 	int    j;
 	struct cfs_trace_cpu_data *tcd;
 
-	init_rwsem(&cfs_tracefile_sem);
-
 	/* initialize trace_data */
 	memset(cfs_trace_data, 0, sizeof(cfs_trace_data));
 	for (i = 0; i < CFS_TCD_TYPE_MAX; i++) {
@@ -102,11 +100,10 @@ void cfs_tracefile_fini_arch(void)
 	int    j;
 
 	for (i = 0; i < num_possible_cpus(); i++)
-		for (j = 0; j < 3; j++)
-			if (cfs_trace_console_buffers[i][j] != NULL) {
-				kfree(cfs_trace_console_buffers[i][j]);
-				cfs_trace_console_buffers[i][j] = NULL;
-			}
+		for (j = 0; j < 3; j++) {
+			kfree(cfs_trace_console_buffers[i][j]);
+			cfs_trace_console_buffers[i][j] = NULL;
+		}
 
 	for (i = 0; cfs_trace_data[i] != NULL; i++) {
 		kfree(cfs_trace_data[i]);
diff --git a/drivers/staging/lustre/lustre/libcfs/module.c b/drivers/staging/lustre/lustre/libcfs/module.c
index f0ee76abfd5a..e60b2e9b9194 100644
--- a/drivers/staging/lustre/lustre/libcfs/module.c
+++ b/drivers/staging/lustre/lustre/libcfs/module.c
@@ -49,7 +49,6 @@
 #include <linux/file.h>
 #include <linux/list.h>
 
-#include <linux/proc_fs.h>
 #include <linux/sysctl.h>
 
 # define DEBUG_SUBSYSTEM S_LNET
@@ -67,8 +66,6 @@ MODULE_DESCRIPTION("Portals v3.1");
 MODULE_LICENSE("GPL");
 
 extern struct miscdevice libcfs_dev;
-extern struct rw_semaphore cfs_tracefile_sem;
-extern struct mutex cfs_trace_thread_mutex;
 extern struct cfs_wi_sched *cfs_sched_rehash;
 extern void libcfs_init_nidstrings(void);
 
@@ -249,8 +246,8 @@ static int libcfs_psdev_release(unsigned long flags, void *args)
 	return 0;
 }
 
-static struct rw_semaphore ioctl_list_sem;
-static struct list_head ioctl_list;
+static DECLARE_RWSEM(ioctl_list_sem);
+static LIST_HEAD(ioctl_list);
 
 int libcfs_register_ioctl(struct libcfs_ioctl_handler *hand)
 {
@@ -393,11 +390,6 @@ static int init_libcfs_module(void)
 
 	libcfs_arch_init();
 	libcfs_init_nidstrings();
-	init_rwsem(&cfs_tracefile_sem);
-	mutex_init(&cfs_trace_thread_mutex);
-	init_rwsem(&ioctl_list_sem);
-	INIT_LIST_HEAD(&ioctl_list);
-	init_waitqueue_head(&cfs_race_waitq);
 
 	rc = libcfs_debug_init(5 * 1024 * 1024);
 	if (rc < 0) {
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.c b/drivers/staging/lustre/lustre/libcfs/tracefile.c
index c86394f7f4d9..6ee2adcf8890 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.c
@@ -52,7 +52,7 @@ union cfs_trace_data_union (*cfs_trace_data[TCD_MAX_TYPES])[NR_CPUS] __cacheline
 char cfs_tracefile[TRACEFILE_NAME_SIZE];
 long long cfs_tracefile_size = CFS_TRACEFILE_SIZE;
 static struct tracefiled_ctl trace_tctl;
-struct mutex cfs_trace_thread_mutex;
+static DEFINE_MUTEX(cfs_trace_thread_mutex);
 static int thread_running;
 
 static atomic_t cfs_tage_allocated = ATOMIC_INIT(0);
diff --git a/drivers/staging/lustre/lustre/llite/Makefile b/drivers/staging/lustre/lustre/llite/Makefile
index 7d70115d5bc7..2cbc46838fdd 100644
--- a/drivers/staging/lustre/lustre/llite/Makefile
+++ b/drivers/staging/lustre/lustre/llite/Makefile
@@ -5,7 +5,6 @@ lustre-y := dcache.o dir.o file.o llite_close.o llite_lib.o llite_nfs.o \
 	    xattr.o xattr_cache.o remote_perm.o llite_rmtacl.o llite_capa.o \
 	    rw26.o super25.o statahead.o \
 	    ../lclient/glimpse.o ../lclient/lcommon_cl.o ../lclient/lcommon_misc.o \
-	    vvp_dev.o vvp_page.o vvp_lock.o vvp_io.o vvp_object.o
+	    vvp_dev.o vvp_page.o vvp_lock.o vvp_io.o vvp_object.o lproc_llite.o
 
-lustre-$(CONFIG_PROC_FS) += lproc_llite.o
 llite_lloop-y := lloop.o
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index 5af01351306d..7b008a64707d 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -52,7 +52,7 @@ static void free_dentry_data(struct rcu_head *head)
 	struct ll_dentry_data *lld;
 
 	lld = container_of(head, struct ll_dentry_data, lld_rcu_head);
-	OBD_FREE_PTR(lld);
+	kfree(lld);
 }
 
 /* should NOT be called with the dcache lock, see fs/dcache.c */
@@ -67,7 +67,7 @@ static void ll_release(struct dentry *de)
 
 	if (lld->lld_it) {
 		ll_intent_release(lld->lld_it);
-		OBD_FREE(lld->lld_it, sizeof(*lld->lld_it));
+		kfree(lld->lld_it);
 	}
 
 	de->d_fsdata = NULL;
@@ -194,7 +194,7 @@ int ll_d_init(struct dentry *de)
 				de->d_fsdata = lld;
 				__d_lustre_invalidate(de);
 			} else {
-				OBD_FREE_PTR(lld);
+				kfree(lld);
 			}
 			spin_unlock(&de->d_lock);
 		} else {
diff --git a/drivers/staging/lustre/lustre/llite/dir.c b/drivers/staging/lustre/lustre/llite/dir.c
index a5bc694dcb64..3d746a94f92e 100644
--- a/drivers/staging/lustre/lustre/llite/dir.c
+++ b/drivers/staging/lustre/lustre/llite/dir.c
@@ -239,7 +239,7 @@ static int ll_dir_filler(void *_hash, struct page *page0)
 	ll_pagevec_lru_add_file(&lru_pvec);
 
 	if (page_pool != &page0)
-		OBD_FREE(page_pool, sizeof(struct page *) * max_pages);
+		kfree(page_pool);
 	return rc;
 }
 
@@ -650,7 +650,7 @@ static int ll_send_mgc_param(struct obd_export *mgc, char *string)
 				sizeof(struct mgs_send_param), msp, NULL);
 	if (rc)
 		CERROR("Failed to set parameter: %d\n", rc);
-	OBD_FREE_PTR(msp);
+	kfree(msp);
 
 	return rc;
 }
@@ -754,10 +754,8 @@ int ll_dir_setstripe(struct inode *inode, struct lov_user_md *lump,
 		char *buf;
 
 		param = kzalloc(MGS_PARAM_MAXLEN, GFP_NOFS);
-		if (!param) {
-			rc = -ENOMEM;
-			goto end;
-		}
+		if (!param)
+			return -ENOMEM;
 
 		buf = param;
 		/* Get fsname and assume devname to be -MDT0000. */
@@ -786,8 +784,7 @@ int ll_dir_setstripe(struct inode *inode, struct lov_user_md *lump,
 		rc = ll_send_mgc_param(mgc->u.cli.cl_mgc_mgsexp, param);
 
 end:
-		if (param != NULL)
-			OBD_FREE(param, MGS_PARAM_MAXLEN);
+		kfree(param);
 	}
 	return rc;
 }
@@ -1072,7 +1069,7 @@ static int copy_and_ioctl(int cmd, struct obd_export *exp,
 
 	rc = obd_iocontrol(cmd, exp, size, copy, NULL);
 out:
-	OBD_FREE(copy, size);
+	kfree(copy);
 
 	return rc;
 }
@@ -1163,7 +1160,7 @@ static int quotactl_ioctl(struct ll_sb_info *sbi, struct if_quotactl *qctl)
 				oqctl->qc_cmd = Q_QUOTAOFF;
 				obd_quotactl(sbi->ll_md_exp, oqctl);
 			}
-			OBD_FREE_PTR(oqctl);
+			kfree(oqctl);
 			return rc;
 		}
 		/* If QIF_SPACE is not set, client should collect the
@@ -1206,39 +1203,44 @@ static int quotactl_ioctl(struct ll_sb_info *sbi, struct if_quotactl *qctl)
 				oqctl->qc_dqblk.dqb_valid &= ~QIF_SPACE;
 			}
 
-			OBD_FREE_PTR(oqctl_tmp);
+			kfree(oqctl_tmp);
 		}
 out:
 		QCTL_COPY(qctl, oqctl);
-		OBD_FREE_PTR(oqctl);
+		kfree(oqctl);
 	}
 
 	return rc;
 }
 
-static char *
-ll_getname(const char __user *filename)
+/* This function tries to get a single name component,
+ * to send to the server. No actual path traversal involved,
+ * so we limit to NAME_MAX */
+static char *ll_getname(const char __user *filename)
 {
 	int ret = 0, len;
-	char *tmp = __getname();
+	char *tmp;
 
+	tmp = kzalloc(NAME_MAX + 1, GFP_KERNEL);
 	if (!tmp)
 		return ERR_PTR(-ENOMEM);
 
-	len = strncpy_from_user(tmp, filename, PATH_MAX);
-	if (len == 0)
+	len = strncpy_from_user(tmp, filename, NAME_MAX + 1);
+	if (len < 0)
+		ret = len;
+	else if (len == 0)
 		ret = -ENOENT;
-	else if (len > PATH_MAX)
+	else if (len > NAME_MAX && tmp[NAME_MAX] != 0)
 		ret = -ENAMETOOLONG;
 
 	if (ret) {
-		__putname(tmp);
+		kfree(tmp);
 		tmp =  ERR_PTR(ret);
 	}
 	return tmp;
 }
 
-#define ll_putname(filename) __putname(filename)
+#define ll_putname(filename) kfree(filename)
 
 static long ll_dir_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
@@ -1436,37 +1438,7 @@ lmv_out_free:
 			goto free_lmv;
 		}
 free_lmv:
-		if (tmp)
-			OBD_FREE(tmp, lum_size);
-		return rc;
-	}
-	case LL_IOC_REMOVE_ENTRY: {
-		char		*filename = NULL;
-		int		 namelen = 0;
-		int		 rc;
-
-		/* Here is a little hack to avoid sending REINT_RMENTRY to
-		 * unsupported server, which might crash the server(LU-2730),
-		 * Because both LVB_TYPE and REINT_RMENTRY will be supported
-		 * on 2.4, we use OBD_CONNECT_LVB_TYPE to detect whether the
-		 * server will support REINT_RMENTRY XXX*/
-		if (!(exp_connect_flags(sbi->ll_md_exp) & OBD_CONNECT_LVB_TYPE))
-			return -ENOTSUPP;
-
-		filename = ll_getname((const char *)arg);
-		if (IS_ERR(filename))
-			return PTR_ERR(filename);
-
-		namelen = strlen(filename);
-		if (namelen < 1) {
-			rc = -EINVAL;
-			goto out_rmdir;
-		}
-
-		rc = ll_rmdir_entry(inode, filename, namelen);
-out_rmdir:
-		if (filename)
-			ll_putname(filename);
+		kfree(tmp);
 		return rc;
 	}
 	case LL_IOC_LOV_SWAP_LAYOUTS:
@@ -1576,7 +1548,7 @@ out_req:
 		if (rc)
 			return rc;
 
-		OBD_ALLOC_LARGE(lmm, lmmsize);
+		lmm = libcfs_kvzalloc(lmmsize, GFP_NOFS);
 		if (lmm == NULL)
 			return -ENOMEM;
 		if (copy_from_user(lmm, lum, lmmsize)) {
@@ -1629,7 +1601,7 @@ out_req:
 free_lsm:
 		obd_free_memmd(sbi->ll_dt_exp, &lsm);
 free_lmm:
-		OBD_FREE_LARGE(lmm, lmmsize);
+		kvfree(lmm);
 		return rc;
 	}
 	case OBD_IOC_LLOG_CATINFO: {
@@ -1657,7 +1629,7 @@ free_lmm:
 		if (rc < 0)
 			CDEBUG(D_INFO, "obd_quotacheck failed: rc %d\n", rc);
 
-		OBD_FREE_PTR(oqctl);
+		kfree(oqctl);
 		return error ?: rc;
 	}
 	case OBD_IOC_POLL_QUOTACHECK: {
@@ -1691,7 +1663,7 @@ free_lmm:
 			goto out_poll;
 		}
 out_poll:
-		OBD_FREE_PTR(check);
+		kfree(check);
 		return rc;
 	}
 	case LL_IOC_QUOTACTL: {
@@ -1712,7 +1684,7 @@ out_poll:
 			rc = -EFAULT;
 
 out_quotactl:
-		OBD_FREE_PTR(qctl);
+		kfree(qctl);
 		return rc;
 	}
 	case OBD_IOC_GETDTNAME:
@@ -1775,19 +1747,13 @@ out_quotactl:
 		struct hsm_user_request	*hur;
 		ssize_t			 totalsize;
 
-		hur = kzalloc(sizeof(*hur), GFP_NOFS);
-		if (!hur)
-			return -ENOMEM;
-
-		/* We don't know the true size yet; copy the fixed-size part */
-		if (copy_from_user(hur, (void *)arg, sizeof(*hur))) {
-			OBD_FREE_PTR(hur);
-			return -EFAULT;
-		}
+		hur = memdup_user((void *)arg, sizeof(*hur));
+		if (IS_ERR(hur))
+			return PTR_ERR(hur);
 
 		/* Compute the whole struct size */
 		totalsize = hur_len(hur);
-		OBD_FREE_PTR(hur);
+		kfree(hur);
 		if (totalsize < 0)
 			return -E2BIG;
 
@@ -1795,13 +1761,13 @@ out_quotactl:
 		if (totalsize >= MDS_MAXREQSIZE / 3)
 			return -E2BIG;
 
-		OBD_ALLOC_LARGE(hur, totalsize);
+		hur = libcfs_kvzalloc(totalsize, GFP_NOFS);
 		if (hur == NULL)
 			return -ENOMEM;
 
 		/* Copy the whole struct */
 		if (copy_from_user(hur, (void *)arg, totalsize)) {
-			OBD_FREE_LARGE(hur, totalsize);
+			kvfree(hur);
 			return -EFAULT;
 		}
 
@@ -1828,7 +1794,7 @@ out_quotactl:
 					   hur, NULL);
 		}
 
-		OBD_FREE_LARGE(hur, totalsize);
+		kvfree(hur);
 
 		return rc;
 	}
@@ -1861,38 +1827,30 @@ out_quotactl:
 		struct hsm_copy	*copy;
 		int		 rc;
 
-		copy = kzalloc(sizeof(*copy), GFP_NOFS);
-		if (!copy)
-			return -ENOMEM;
-		if (copy_from_user(copy, (char *)arg, sizeof(*copy))) {
-			OBD_FREE_PTR(copy);
-			return -EFAULT;
-		}
+		copy = memdup_user((char *)arg, sizeof(*copy));
+		if (IS_ERR(copy))
+			return PTR_ERR(copy);
 
 		rc = ll_ioc_copy_start(inode->i_sb, copy);
 		if (copy_to_user((char *)arg, copy, sizeof(*copy)))
 			rc = -EFAULT;
 
-		OBD_FREE_PTR(copy);
+		kfree(copy);
 		return rc;
 	}
 	case LL_IOC_HSM_COPY_END: {
 		struct hsm_copy	*copy;
 		int		 rc;
 
-		copy = kzalloc(sizeof(*copy), GFP_NOFS);
-		if (!copy)
-			return -ENOMEM;
-		if (copy_from_user(copy, (char *)arg, sizeof(*copy))) {
-			OBD_FREE_PTR(copy);
-			return -EFAULT;
-		}
+		copy = memdup_user((char *)arg, sizeof(*copy));
+		if (IS_ERR(copy))
+			return PTR_ERR(copy);
 
 		rc = ll_ioc_copy_end(inode->i_sb, copy);
 		if (copy_to_user((char *)arg, copy, sizeof(*copy)))
 			rc = -EFAULT;
 
-		OBD_FREE_PTR(copy);
+		kfree(copy);
 		return rc;
 	}
 	default:
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index 4b44c634fcc3..3075db211106 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -213,7 +213,7 @@ out:
 		md_clear_open_replay_data(md_exp, och);
 		/* Free @och if it is not waiting for DONE_WRITING. */
 		och->och_fh.cookie = DEAD_HANDLE_MAGIC;
-		OBD_FREE_PTR(och);
+		kfree(och);
 	}
 	if (req) /* This is close request */
 		ptlrpc_req_finished(req);
@@ -693,7 +693,7 @@ restart:
 out_och_free:
 	if (rc) {
 		if (och_p && *och_p) {
-			OBD_FREE(*och_p, sizeof(struct obd_client_handle));
+			kfree(*och_p);
 			*och_p = NULL; /* OBD_FREE writes some magic there */
 			(*och_usecount)--;
 		}
@@ -875,7 +875,7 @@ out_close:
 out_release_it:
 	ll_intent_release(&it);
 out:
-	OBD_FREE_PTR(och);
+	kfree(och);
 	return ERR_PTR(rc);
 }
 
@@ -1282,7 +1282,7 @@ static int ll_lov_recreate(struct inode *inode, struct ost_id *oi, u32 ost_idx)
 	lsm_size = sizeof(*lsm) + (sizeof(struct lov_oinfo) *
 		   (lsm->lsm_stripe_count));
 
-	OBD_ALLOC_LARGE(lsm2, lsm_size);
+	lsm2 = libcfs_kvzalloc(lsm_size, GFP_NOFS);
 	if (lsm2 == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -1300,7 +1300,7 @@ static int ll_lov_recreate(struct inode *inode, struct ost_id *oi, u32 ost_idx)
 	rc = obd_create(NULL, exp, oa, &lsm2, &oti);
 	ll_inode_size_unlock(inode);
 
-	OBD_FREE_LARGE(lsm2, lsm_size);
+	kvfree(lsm2);
 	goto out;
 out:
 	ccc_inode_lsm_put(inode, lsm);
@@ -1477,12 +1477,12 @@ static int ll_lov_setea(struct inode *inode, struct file *file,
 	if (!capable(CFS_CAP_SYS_ADMIN))
 		return -EPERM;
 
-	OBD_ALLOC_LARGE(lump, lum_size);
+	lump = libcfs_kvzalloc(lum_size, GFP_NOFS);
 	if (lump == NULL)
 		return -ENOMEM;
 
 	if (copy_from_user(lump, (struct lov_user_md *)arg, lum_size)) {
-		OBD_FREE_LARGE(lump, lum_size);
+		kvfree(lump);
 		return -EFAULT;
 	}
 
@@ -1490,7 +1490,7 @@ static int ll_lov_setea(struct inode *inode, struct file *file,
 				     lum_size);
 	cl_lov_delay_create_clear(&file->f_flags);
 
-	OBD_FREE_LARGE(lump, lum_size);
+	kvfree(lump);
 	return rc;
 }
 
@@ -1779,7 +1779,7 @@ int ll_fid2path(struct inode *inode, void __user *arg)
 		rc = -EFAULT;
 
 gf_free:
-	OBD_FREE(gfout, outsize);
+	kfree(gfout);
 	return rc;
 }
 
@@ -1802,7 +1802,7 @@ static int ll_ioctl_fiemap(struct inode *inode, unsigned long arg)
 	num_bytes = sizeof(*fiemap_s) + (extent_count *
 					 sizeof(struct ll_fiemap_extent));
 
-	OBD_ALLOC_LARGE(fiemap_s, num_bytes);
+	fiemap_s = libcfs_kvzalloc(num_bytes, GFP_NOFS);
 	if (fiemap_s == NULL)
 		return -ENOMEM;
 
@@ -1839,7 +1839,7 @@ static int ll_ioctl_fiemap(struct inode *inode, unsigned long arg)
 		rc = -EFAULT;
 
 error:
-	OBD_FREE_LARGE(fiemap_s, num_bytes);
+	kvfree(fiemap_s);
 	return rc;
 }
 
@@ -1883,7 +1883,7 @@ int ll_data_version(struct inode *inode, __u64 *data_version,
 			*data_version = obdo->o_data_version;
 	}
 
-	OBD_FREE_PTR(obdo);
+	kfree(obdo);
 out:
 	ccc_inode_lsm_put(inode, lsm);
 	return rc;
@@ -2109,8 +2109,7 @@ putgl:
 	}
 
 free:
-	if (llss != NULL)
-		OBD_FREE_PTR(llss);
+	kfree(llss);
 
 	return rc;
 }
@@ -2152,22 +2151,20 @@ static int ll_hsm_import(struct inode *inode, struct file *file,
 
 	/* set HSM flags */
 	hss = kzalloc(sizeof(*hss), GFP_NOFS);
-	if (!hss) {
-		rc = -ENOMEM;
-		goto out;
-	}
+	if (!hss)
+		return -ENOMEM;
 
 	hss->hss_valid = HSS_SETMASK | HSS_ARCHIVE_ID;
 	hss->hss_archive_id = hui->hui_archive_id;
 	hss->hss_setmask = HS_ARCHIVED | HS_EXISTS | HS_RELEASED;
 	rc = ll_hsm_state_set(inode, hss);
 	if (rc != 0)
-		goto out;
+		goto free_hss;
 
 	attr = kzalloc(sizeof(*attr), GFP_NOFS);
 	if (!attr) {
 		rc = -ENOMEM;
-		goto out;
+		goto free_hss;
 	}
 
 	attr->ia_mode = hui->hui_mode & (S_IRWXU | S_IRWXG | S_IRWXO);
@@ -2193,13 +2190,9 @@ static int ll_hsm_import(struct inode *inode, struct file *file,
 
 	mutex_unlock(&inode->i_mutex);
 
-out:
-	if (hss != NULL)
-		OBD_FREE_PTR(hss);
-
-	if (attr != NULL)
-		OBD_FREE_PTR(attr);
-
+	kfree(attr);
+free_hss:
+	kfree(hss);
 	return rc;
 }
 
@@ -2350,7 +2343,7 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		op_data = ll_prep_md_op_data(NULL, inode, NULL, NULL, 0, 0,
 					     LUSTRE_OPC_ANY, hus);
 		if (IS_ERR(op_data)) {
-			OBD_FREE_PTR(hus);
+			kfree(hus);
 			return PTR_ERR(op_data);
 		}
 
@@ -2361,25 +2354,20 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			rc = -EFAULT;
 
 		ll_finish_md_op_data(op_data);
-		OBD_FREE_PTR(hus);
+		kfree(hus);
 		return rc;
 	}
 	case LL_IOC_HSM_STATE_SET: {
 		struct hsm_state_set	*hss;
 		int			 rc;
 
-		hss = kzalloc(sizeof(*hss), GFP_NOFS);
-		if (!hss)
-			return -ENOMEM;
-
-		if (copy_from_user(hss, (char *)arg, sizeof(*hss))) {
-			OBD_FREE_PTR(hss);
-			return -EFAULT;
-		}
+		hss = memdup_user((char *)arg, sizeof(*hss));
+		if (IS_ERR(hss))
+			return PTR_ERR(hss);
 
 		rc = ll_hsm_state_set(inode, hss);
 
-		OBD_FREE_PTR(hss);
+		kfree(hss);
 		return rc;
 	}
 	case LL_IOC_HSM_ACTION: {
@@ -2394,7 +2382,7 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		op_data = ll_prep_md_op_data(NULL, inode, NULL, NULL, 0, 0,
 					     LUSTRE_OPC_ANY, hca);
 		if (IS_ERR(op_data)) {
-			OBD_FREE_PTR(hca);
+			kfree(hca);
 			return PTR_ERR(op_data);
 		}
 
@@ -2405,7 +2393,7 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			rc = -EFAULT;
 
 		ll_finish_md_op_data(op_data);
-		OBD_FREE_PTR(hca);
+		kfree(hca);
 		return rc;
 	}
 	case LL_IOC_SET_LEASE: {
@@ -2495,18 +2483,13 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	case LL_IOC_HSM_IMPORT: {
 		struct hsm_user_import *hui;
 
-		hui = kzalloc(sizeof(*hui), GFP_NOFS);
-		if (!hui)
-			return -ENOMEM;
-
-		if (copy_from_user(hui, (void *)arg, sizeof(*hui))) {
-			OBD_FREE_PTR(hui);
-			return -EFAULT;
-		}
+		hui = memdup_user((void *)arg, sizeof(*hui));
+		if (IS_ERR(hui))
+			return PTR_ERR(hui);
 
 		rc = ll_hsm_import(inode, file, hui);
 
-		OBD_FREE_PTR(hui);
+		kfree(hui);
 		return rc;
 	}
 	default: {
@@ -3062,7 +3045,7 @@ static int ll_fiemap(struct inode *inode, struct fiemap_extent_info *fieinfo,
 
 	num_bytes = sizeof(*fiemap) + (extent_count *
 				       sizeof(struct ll_fiemap_extent));
-	OBD_ALLOC_LARGE(fiemap, num_bytes);
+	fiemap = libcfs_kvzalloc(num_bytes, GFP_NOFS);
 
 	if (fiemap == NULL)
 		return -ENOMEM;
@@ -3084,7 +3067,7 @@ static int ll_fiemap(struct inode *inode, struct fiemap_extent_info *fieinfo,
 		       fiemap->fm_mapped_extents *
 		       sizeof(struct ll_fiemap_extent));
 
-	OBD_FREE_LARGE(fiemap, num_bytes);
+	kvfree(fiemap);
 	return rc;
 }
 
@@ -3246,12 +3229,10 @@ void ll_iocontrol_unregister(void *magic)
 	down_write(&llioc.ioc_sem);
 	list_for_each_entry(tmp, &llioc.ioc_head, iocd_list) {
 		if (tmp == magic) {
-			unsigned int size = tmp->iocd_size;
-
 			list_del(&tmp->iocd_list);
 			up_write(&llioc.ioc_sem);
 
-			OBD_FREE(tmp, size);
+			kfree(tmp);
 			return;
 		}
 	}
@@ -3375,7 +3356,7 @@ static int ll_layout_fetch(struct inode *inode, struct ldlm_lock *lock)
 		goto out;
 	}
 
-	OBD_ALLOC_LARGE(lvbdata, lmmsize);
+	lvbdata = libcfs_kvzalloc(lmmsize, GFP_NOFS);
 	if (lvbdata == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -3384,7 +3365,7 @@ static int ll_layout_fetch(struct inode *inode, struct ldlm_lock *lock)
 	memcpy(lvbdata, lmm, lmmsize);
 	lock_res_and_lock(lock);
 	if (lock->l_lvb_data != NULL)
-		OBD_FREE_LARGE(lock->l_lvb_data, lock->l_lvb_len);
+		kvfree(lock->l_lvb_data);
 
 	lock->l_lvb_data = lvbdata;
 	lock->l_lvb_len = lmmsize;
@@ -3619,6 +3600,6 @@ int ll_layout_restore(struct inode *inode)
 	hur->hur_request.hr_itemcount = 1;
 	rc = obd_iocontrol(LL_IOC_HSM_REQUEST, cl_i2sbi(inode)->ll_md_exp,
 			   len, hur, NULL);
-	OBD_FREE(hur, len);
+	kfree(hur);
 	return rc;
 }
diff --git a/drivers/staging/lustre/lustre/llite/llite_capa.c b/drivers/staging/lustre/lustre/llite/llite_capa.c
index aec9a44120c0..a6268718b76e 100644
--- a/drivers/staging/lustre/lustre/llite/llite_capa.c
+++ b/drivers/staging/lustre/lustre/llite/llite_capa.c
@@ -140,6 +140,7 @@ static void sort_add_capa(struct obd_capa *ocapa, struct list_head *head)
 static inline int obd_capa_open_count(struct obd_capa *oc)
 {
 	struct ll_inode_info *lli = ll_i2info(oc->u.cli.inode);
+
 	return atomic_read(&lli->lli_open_count);
 }
 
diff --git a/drivers/staging/lustre/lustre/llite/llite_close.c b/drivers/staging/lustre/lustre/llite/llite_close.c
index a94ba02ccf02..7bdae723fedd 100644
--- a/drivers/staging/lustre/lustre/llite/llite_close.c
+++ b/drivers/staging/lustre/lustre/llite/llite_close.c
@@ -305,7 +305,7 @@ out:
 	ll_finish_md_op_data(op_data);
 	if (och) {
 		md_clear_open_replay_data(ll_i2sbi(inode)->ll_md_exp, och);
-		OBD_FREE_PTR(och);
+		kfree(och);
 	}
 }
 
@@ -374,7 +374,7 @@ int ll_close_thread_start(struct ll_close_queue **lcq_ret)
 
 	task = kthread_run(ll_close_thread, lcq, "ll_close");
 	if (IS_ERR(task)) {
-		OBD_FREE(lcq, sizeof(*lcq));
+		kfree(lcq);
 		return PTR_ERR(task);
 	}
 
@@ -389,5 +389,5 @@ void ll_close_thread_shutdown(struct ll_close_queue *lcq)
 	atomic_inc(&lcq->lcq_stop);
 	wake_up(&lcq->lcq_waitq);
 	wait_for_completion(&lcq->lcq_comp);
-	OBD_FREE(lcq, sizeof(*lcq));
+	kfree(lcq);
 }
diff --git a/drivers/staging/lustre/lustre/llite/llite_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index 5f918e3c4683..f097d4d167d5 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -57,12 +57,6 @@
 #define VM_FAULT_RETRY 0
 #endif
 
-/* Kernel 3.1 kills LOOKUP_CONTINUE, LOOKUP_PARENT is equivalent to it.
- * seem kernel commit 49084c3bb2055c401f3493c13edae14d49128ca0 */
-#ifndef LOOKUP_CONTINUE
-#define LOOKUP_CONTINUE LOOKUP_PARENT
-#endif
-
 /** Only used on client-side for indicating the tail of dir hash/offset. */
 #define LL_DIR_END_OFF	  0x7fffffffffffffffULL
 #define LL_DIR_END_OFF_32BIT    0x7fffffffUL
@@ -471,7 +465,7 @@ struct ll_sb_info {
 	struct obd_uuid	   ll_sb_uuid;
 	struct obd_export	*ll_md_exp;
 	struct obd_export	*ll_dt_exp;
-	struct proc_dir_entry*    ll_proc_root;
+	struct dentry		*ll_debugfs_entry;
 	struct lu_fid	     ll_root_fid; /* root object fid */
 
 	int		       ll_flags;
@@ -493,9 +487,6 @@ struct ll_sb_info {
 	unsigned int	      ll_namelen;
 	struct file_operations   *ll_fop;
 
-	/* =0 - hold lock over whole read/write
-	 * >0 - max. chunk to be read/written w/o lock re-acquiring */
-	unsigned long	     ll_max_rw_chunk;
 	unsigned int	      ll_md_brw_size; /* used by readdir */
 
 	struct lu_site	   *ll_site;
@@ -524,10 +515,11 @@ struct ll_sb_info {
 	struct rmtacl_ctl_table   ll_rct;
 	struct eacl_table	 ll_et;
 	__kernel_fsid_t		  ll_fsid;
+	struct kobject		 ll_kobj; /* sysfs object */
+	struct super_block	*ll_sb; /* struct super_block (for sysfs code)*/
+	struct completion	 ll_kobj_unregister;
 };
 
-#define LL_DEFAULT_MAX_RW_CHUNK      (32 * 1024 * 1024)
-
 struct ll_ra_read {
 	pgoff_t	     lrr_start;
 	pgoff_t	     lrr_count;
@@ -644,7 +636,8 @@ struct lov_stripe_md;
 
 extern spinlock_t inode_lock;
 
-extern struct proc_dir_entry *proc_lustre_fs_root;
+extern struct dentry *llite_root;
+extern struct kset *llite_kset;
 
 static inline struct inode *ll_info2i(struct ll_inode_info *lli)
 {
@@ -670,30 +663,14 @@ void ll_ra_read_ex(struct file *f, struct ll_ra_read *rar);
 struct ll_ra_read *ll_ra_read_get(struct file *f);
 
 /* llite/lproc_llite.c */
-#if defined (CONFIG_PROC_FS)
-int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
-				struct super_block *sb, char *osc, char *mdc);
-void lprocfs_unregister_mountpoint(struct ll_sb_info *sbi);
+int ldebugfs_register_mountpoint(struct dentry *parent,
+				 struct super_block *sb, char *osc, char *mdc);
+void ldebugfs_unregister_mountpoint(struct ll_sb_info *sbi);
 void ll_stats_ops_tally(struct ll_sb_info *sbi, int op, int count);
 void lprocfs_llite_init_vars(struct lprocfs_static_vars *lvars);
 void ll_rw_stats_tally(struct ll_sb_info *sbi, pid_t pid,
 		       struct ll_file_data *file, loff_t pos,
 		       size_t count, int rw);
-#else
-static inline int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
-			struct super_block *sb, char *osc, char *mdc){return 0;}
-static inline void lprocfs_unregister_mountpoint(struct ll_sb_info *sbi) {}
-static inline
-void ll_stats_ops_tally(struct ll_sb_info *sbi, int op, int count) {}
-static inline void lprocfs_llite_init_vars(struct lprocfs_static_vars *lvars)
-{
-	memset(lvars, 0, sizeof(*lvars));
-}
-static inline void ll_rw_stats_tally(struct ll_sb_info *sbi, pid_t pid,
-				     struct ll_file_data *file, loff_t pos,
-				     size_t count, int rw) {}
-#endif
-
 
 /* llite/dir.c */
 void ll_release_page(struct page *page, int remove);
@@ -727,11 +704,7 @@ int ll_readahead(const struct lu_env *env, struct cl_io *io,
 		 struct ll_readahead_state *ras, struct address_space *mapping,
 		 struct cl_page_list *queue, int flags);
 
-#ifndef MS_HAS_NEW_AOPS
 extern const struct address_space_operations ll_aops;
-#else
-extern const struct address_space_operations_ext ll_aops;
-#endif
 
 /* llite/file.c */
 extern struct file_operations ll_file_operations;
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index a27af7882170..25139885b5a7 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -57,7 +57,8 @@
 #include "llite_internal.h"
 
 struct kmem_cache *ll_file_data_slab;
-struct proc_dir_entry *proc_lustre_fs_root;
+struct dentry *llite_root;
+struct kset *llite_kset;
 
 static LIST_HEAD(ll_super_blocks);
 static DEFINE_SPINLOCK(ll_sb_lock);
@@ -66,7 +67,7 @@ static DEFINE_SPINLOCK(ll_sb_lock);
 #define log2(n) ffz(~(n))
 #endif
 
-static struct ll_sb_info *ll_init_sbi(void)
+static struct ll_sb_info *ll_init_sbi(struct super_block *sb)
 {
 	struct ll_sb_info *sbi = NULL;
 	unsigned long pages;
@@ -134,6 +135,8 @@ static struct ll_sb_info *ll_init_sbi(void)
 	atomic_set(&sbi->ll_agl_total, 0);
 	sbi->ll_flags |= LL_SBI_AGL_ENABLED;
 
+	sbi->ll_sb = sb;
+
 	return sbi;
 }
 
@@ -145,7 +148,7 @@ static void ll_free_sbi(struct super_block *sb)
 		spin_lock(&ll_sb_lock);
 		list_del(&sbi->ll_list);
 		spin_unlock(&ll_sb_lock);
-		OBD_FREE(sbi, sizeof(*sbi));
+		kfree(sbi);
 	}
 }
 
@@ -177,15 +180,14 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 
 	osfs = kzalloc(sizeof(*osfs), GFP_NOFS);
 	if (!osfs) {
-		OBD_FREE_PTR(data);
+		kfree(data);
 		return -ENOMEM;
 	}
 
-	if (proc_lustre_fs_root) {
-		err = lprocfs_register_mountpoint(proc_lustre_fs_root, sb,
-						  dt, md);
+	if (llite_root != NULL) {
+		err = ldebugfs_register_mountpoint(llite_root, sb, dt, md);
 		if (err < 0)
-			CERROR("could not register mount in /proc/fs/lustre\n");
+			CERROR("could not register mount in <debugfs>/lustre/llite\n");
 	}
 
 	/* indicate the features supported by this client */
@@ -227,14 +229,6 @@ 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;
 
-#ifdef HAVE_MS_FLOCK_LOCK
-	/* force vfs to use lustre handler for flock() calls - bug 10743 */
-	sb->s_flags |= MS_FLOCK_LOCK;
-#endif
-#ifdef MS_HAS_NEW_AOPS
-	sb->s_flags |= MS_HAS_NEW_AOPS;
-#endif
-
 	if (sbi->ll_flags & LL_SBI_FLOCK)
 		sbi->ll_fop = &ll_file_operations_flock;
 	else if (sbi->ll_flags & LL_SBI_LOCALFLOCK)
@@ -292,11 +286,15 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 		char *buf;
 
 		buf = kzalloc(PAGE_CACHE_SIZE, GFP_KERNEL);
+		if (!buf) {
+			err = -ENOMEM;
+			goto out_md_fid;
+		}
 		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",
 				   sbi->ll_md_exp->exp_obd->obd_name, buf);
-		OBD_FREE(buf, PAGE_CACHE_SIZE);
+		kfree(buf);
 		err = -EPROTO;
 		goto out_md_fid;
 	}
@@ -316,7 +314,6 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	sb->s_magic = LL_SUPER_MAGIC;
 	sb->s_maxbytes = MAX_LFS_FILESIZE;
 	sbi->ll_namelen = osfs->os_namelen;
-	sbi->ll_max_rw_chunk = LL_DEFAULT_MAX_RW_CHUNK;
 
 	if ((sbi->ll_flags & LL_SBI_USER_XATTR) &&
 	    !(data->ocd_connect_flags & OBD_CONNECT_XATTR)) {
@@ -408,7 +405,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	if (!OBD_FAIL_CHECK(OBD_FAIL_OSC_CONNECT_CKSUM)) {
 		/* OBD_CONNECT_CKSUM should always be set, even if checksums are
 		 * disabled by default, because it can still be enabled on the
-		 * fly via /proc. As a consequence, we still need to come to an
+		 * fly via /sys. As a consequence, we still need to come to an
 		 * agreement on the supported algorithms at connect time */
 		data->ocd_connect_flags |= OBD_CONNECT_CKSUM;
 
@@ -501,7 +498,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	err = md_getattr(sbi->ll_md_exp, op_data, &request);
 	if (oc)
 		capa_put(oc);
-	OBD_FREE_PTR(op_data);
+	kfree(op_data);
 	if (err) {
 		CERROR("%s: md_getattr failed for root: rc = %d\n",
 		       sbi->ll_md_exp->exp_obd->obd_name, err);
@@ -582,10 +579,8 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 		get_uuid2fsid(uuid->uuid, strlen(uuid->uuid), &sbi->ll_fsid);
 	}
 
-	if (data != NULL)
-		OBD_FREE_PTR(data);
-	if (osfs != NULL)
-		OBD_FREE_PTR(osfs);
+	kfree(data);
+	kfree(osfs);
 
 	return err;
 out_root:
@@ -603,11 +598,9 @@ out_md:
 	obd_disconnect(sbi->ll_md_exp);
 	sbi->ll_md_exp = NULL;
 out:
-	if (data != NULL)
-		OBD_FREE_PTR(data);
-	if (osfs != NULL)
-		OBD_FREE_PTR(osfs);
-	lprocfs_unregister_mountpoint(sbi);
+	kfree(data);
+	kfree(osfs);
+	ldebugfs_unregister_mountpoint(sbi);
 	return err;
 }
 
@@ -688,7 +681,7 @@ static void client_common_put_super(struct super_block *sb)
 	 * see LU-2543. */
 	obd_zombie_barrier();
 
-	lprocfs_unregister_mountpoint(sbi);
+	ldebugfs_unregister_mountpoint(sbi);
 
 	obd_fid_fini(sbi->ll_md_exp->exp_obd);
 	obd_disconnect(sbi->ll_md_exp);
@@ -916,8 +909,6 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 	char  *dt = NULL, *md = NULL;
 	char  *profilenm = get_profile_name(sb);
 	struct config_llog_instance *cfg;
-	/* %p for void* in printf needs 16+2 characters: 0xffffffffffffffff */
-	const int instlen = sizeof(cfg->cfg_instance) * 2 + 2;
 	int    err;
 
 	CDEBUG(D_VFSTRACE, "VFS Op: sb %p\n", sb);
@@ -929,10 +920,10 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 	try_module_get(THIS_MODULE);
 
 	/* client additional sb info */
-	lsi->lsi_llsbi = sbi = ll_init_sbi();
+	lsi->lsi_llsbi = sbi = ll_init_sbi(sb);
 	if (!sbi) {
 		module_put(THIS_MODULE);
-		OBD_FREE_PTR(cfg);
+		kfree(cfg);
 		return -ENOMEM;
 	}
 
@@ -993,16 +984,14 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 	err = client_common_fill_super(sb, md, dt, mnt);
 
 out_free:
-	if (md)
-		OBD_FREE(md, strlen(lprof->lp_md) + instlen + 2);
-	if (dt)
-		OBD_FREE(dt, strlen(lprof->lp_dt) + instlen + 2);
+	kfree(md);
+	kfree(dt);
 	if (err)
 		ll_put_super(sb);
 	else if (sbi->ll_flags & LL_SBI_VERBOSE)
 		LCONSOLE_WARN("Mounted %s\n", profilenm);
 
-	OBD_FREE_PTR(cfg);
+	kfree(cfg);
 	return err;
 } /* ll_fill_super */
 
@@ -1126,8 +1115,7 @@ void ll_clear_inode(struct inode *inode)
 		ll_md_real_close(inode, FMODE_READ);
 
 	if (S_ISLNK(inode->i_mode) && lli->lli_symlink_name) {
-		OBD_FREE(lli->lli_symlink_name,
-			 strlen(lli->lli_symlink_name) + 1);
+		kfree(lli->lli_symlink_name);
 		lli->lli_symlink_name = NULL;
 	}
 
@@ -1957,7 +1945,7 @@ void ll_umount_begin(struct super_block *sb)
 		obd_iocontrol(IOC_OSC_SET_ACTIVE, sbi->ll_dt_exp,
 			      sizeof(*ioc_data), ioc_data, NULL);
 
-		OBD_FREE_PTR(ioc_data);
+		kfree(ioc_data);
 	}
 
 	/* Really, we'd like to wait until there are no requests outstanding,
@@ -2236,7 +2224,7 @@ void ll_finish_md_op_data(struct md_op_data *op_data)
 {
 	capa_put(op_data->op_capa1);
 	capa_put(op_data->op_capa2);
-	OBD_FREE_PTR(op_data);
+	kfree(op_data);
 }
 
 int ll_show_options(struct seq_file *seq, struct dentry *dentry)
diff --git a/drivers/staging/lustre/lustre/llite/llite_nfs.c b/drivers/staging/lustre/lustre/llite/llite_nfs.c
index db43b81386f7..8d1c253d4669 100644
--- a/drivers/staging/lustre/lustre/llite/llite_nfs.c
+++ b/drivers/staging/lustre/lustre/llite/llite_nfs.c
@@ -116,7 +116,7 @@ struct inode *search_inode_for_lustre(struct super_block *sb,
 
 	/* mds_fid2dentry ignores f_type */
 	rc = md_getattr(sbi->ll_md_exp, op_data, &req);
-	OBD_FREE_PTR(op_data);
+	kfree(op_data);
 	if (rc) {
 		CERROR("can't get object attrs, fid "DFID", rc %d\n",
 		       PFID(fid), rc);
diff --git a/drivers/staging/lustre/lustre/llite/llite_rmtacl.c b/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
index f4da156f3874..c8a450b5cb18 100644
--- a/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
+++ b/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
@@ -94,7 +94,7 @@ static void rce_free(struct rmtacl_ctl_entry *rce)
 	if (!list_empty(&rce->rce_list))
 		list_del(&rce->rce_list);
 
-	OBD_FREE_PTR(rce);
+	kfree(rce);
 }
 
 static struct rmtacl_ctl_entry *__rct_search(struct rmtacl_ctl_table *rct,
@@ -205,7 +205,7 @@ void ee_free(struct eacl_entry *ee)
 	if (ee->ee_acl)
 		lustre_ext_acl_xattr_free(ee->ee_acl);
 
-	OBD_FREE_PTR(ee);
+	kfree(ee);
 }
 
 static struct eacl_entry *__et_search_del(struct eacl_table *et, pid_t key,
diff --git a/drivers/staging/lustre/lustre/llite/lloop.c b/drivers/staging/lustre/lustre/llite/lloop.c
index 413a8408e3f5..cc00fd10fbcf 100644
--- a/drivers/staging/lustre/lustre/llite/lloop.c
+++ b/drivers/staging/lustre/lustre/llite/lloop.c
@@ -840,9 +840,9 @@ out_mem4:
 out_mem3:
 	while (i--)
 		put_disk(disks[i]);
-	OBD_FREE(disks, max_loop * sizeof(*disks));
+	kfree(disks);
 out_mem2:
-	OBD_FREE(loop_dev, max_loop * sizeof(*loop_dev));
+	kfree(loop_dev);
 out_mem1:
 	unregister_blkdev(lloop_major, "lloop");
 	ll_iocontrol_unregister(ll_iocontrol_magic);
@@ -863,8 +863,8 @@ static void lloop_exit(void)
 
 	unregister_blkdev(lloop_major, "lloop");
 
-	OBD_FREE(disks, max_loop * sizeof(*disks));
-	OBD_FREE(loop_dev, max_loop * sizeof(*loop_dev));
+	kfree(disks);
+	kfree(loop_dev);
 }
 
 module_init(lloop_init);
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index 83a9b85474e1..486dca6077de 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -48,31 +48,33 @@ static struct file_operations ll_rw_extents_stats_fops;
 static struct file_operations ll_rw_extents_stats_pp_fops;
 static struct file_operations ll_rw_offset_stats_fops;
 
-static int ll_blksize_seq_show(struct seq_file *m, void *v)
+static ssize_t blocksize_show(struct kobject *kobj, struct attribute *attr,
+			      char *buf)
 {
-	struct super_block *sb = (struct super_block *)m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	struct obd_statfs osfs;
 	int rc;
 
-	LASSERT(sb != NULL);
-	rc = ll_statfs_internal(sb, &osfs,
+	rc = ll_statfs_internal(sbi->ll_sb, &osfs,
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc)
-		seq_printf(m, "%u\n", osfs.os_bsize);
+		return sprintf(buf, "%u\n", osfs.os_bsize);
 
 	return rc;
 }
-LPROC_SEQ_FOPS_RO(ll_blksize);
+LUSTRE_RO_ATTR(blocksize);
 
-static int ll_kbytestotal_seq_show(struct seq_file *m, void *v)
+static ssize_t kbytestotal_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct super_block *sb = (struct super_block *)m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	struct obd_statfs osfs;
 	int rc;
 
-	LASSERT(sb != NULL);
-	rc = ll_statfs_internal(sb, &osfs,
+	rc = ll_statfs_internal(sbi->ll_sb, &osfs,
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc) {
@@ -82,21 +84,22 @@ static int ll_kbytestotal_seq_show(struct seq_file *m, void *v)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		seq_printf(m, "%llu\n", result);
+		rc = sprintf(buf, "%llu\n", result);
 	}
 
 	return rc;
 }
-LPROC_SEQ_FOPS_RO(ll_kbytestotal);
+LUSTRE_RO_ATTR(kbytestotal);
 
-static int ll_kbytesfree_seq_show(struct seq_file *m, void *v)
+static ssize_t kbytesfree_show(struct kobject *kobj, struct attribute *attr,
+			       char *buf)
 {
-	struct super_block *sb = (struct super_block *)m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	struct obd_statfs osfs;
 	int rc;
 
-	LASSERT(sb != NULL);
-	rc = ll_statfs_internal(sb, &osfs,
+	rc = ll_statfs_internal(sbi->ll_sb, &osfs,
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc) {
@@ -106,21 +109,22 @@ static int ll_kbytesfree_seq_show(struct seq_file *m, void *v)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		seq_printf(m, "%llu\n", result);
+		rc = sprintf(buf, "%llu\n", result);
 	}
 
 	return rc;
 }
-LPROC_SEQ_FOPS_RO(ll_kbytesfree);
+LUSTRE_RO_ATTR(kbytesfree);
 
-static int ll_kbytesavail_seq_show(struct seq_file *m, void *v)
+static ssize_t kbytesavail_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct super_block *sb = (struct super_block *)m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	struct obd_statfs osfs;
 	int rc;
 
-	LASSERT(sb != NULL);
-	rc = ll_statfs_internal(sb, &osfs,
+	rc = ll_statfs_internal(sbi->ll_sb, &osfs,
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc) {
@@ -130,81 +134,79 @@ static int ll_kbytesavail_seq_show(struct seq_file *m, void *v)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		seq_printf(m, "%llu\n", result);
+		rc = sprintf(buf, "%llu\n", result);
 	}
 
 	return rc;
 }
-LPROC_SEQ_FOPS_RO(ll_kbytesavail);
+LUSTRE_RO_ATTR(kbytesavail);
 
-static int ll_filestotal_seq_show(struct seq_file *m, void *v)
+static ssize_t filestotal_show(struct kobject *kobj, struct attribute *attr,
+			       char *buf)
 {
-	struct super_block *sb = (struct super_block *)m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	struct obd_statfs osfs;
 	int rc;
 
-	LASSERT(sb != NULL);
-	rc = ll_statfs_internal(sb, &osfs,
+	rc = ll_statfs_internal(sbi->ll_sb, &osfs,
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc)
-		seq_printf(m, "%llu\n", osfs.os_files);
+		return sprintf(buf, "%llu\n", osfs.os_files);
 
 	return rc;
 }
-LPROC_SEQ_FOPS_RO(ll_filestotal);
+LUSTRE_RO_ATTR(filestotal);
 
-static int ll_filesfree_seq_show(struct seq_file *m, void *v)
+static ssize_t filesfree_show(struct kobject *kobj, struct attribute *attr,
+			      char *buf)
 {
-	struct super_block *sb = (struct super_block *)m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	struct obd_statfs osfs;
 	int rc;
 
-	LASSERT(sb != NULL);
-	rc = ll_statfs_internal(sb, &osfs,
+	rc = ll_statfs_internal(sbi->ll_sb, &osfs,
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				OBD_STATFS_NODELAY);
 	if (!rc)
-		seq_printf(m, "%llu\n", osfs.os_ffree);
+		return sprintf(buf, "%llu\n", osfs.os_ffree);
 
 	return rc;
 }
-LPROC_SEQ_FOPS_RO(ll_filesfree);
+LUSTRE_RO_ATTR(filesfree);
 
-static int ll_client_type_seq_show(struct seq_file *m, void *v)
+static ssize_t client_type_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct ll_sb_info *sbi = ll_s2sbi((struct super_block *)m->private);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	LASSERT(sbi != NULL);
-
-	if (sbi->ll_flags & LL_SBI_RMT_CLIENT)
-		seq_puts(m, "remote client\n");
-	else
-		seq_puts(m, "local client\n");
-
-	return 0;
+	return sprintf(buf, "%s client\n",
+			sbi->ll_flags & LL_SBI_RMT_CLIENT ? "remote" : "local");
 }
-LPROC_SEQ_FOPS_RO(ll_client_type);
+LUSTRE_RO_ATTR(client_type);
 
-static int ll_fstype_seq_show(struct seq_file *m, void *v)
+static ssize_t fstype_show(struct kobject *kobj, struct attribute *attr,
+			   char *buf)
 {
-	struct super_block *sb = (struct super_block *)m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	LASSERT(sb != NULL);
-	seq_printf(m, "%s\n", sb->s_type->name);
-	return 0;
+	return sprintf(buf, "%s\n", sbi->ll_sb->s_type->name);
 }
-LPROC_SEQ_FOPS_RO(ll_fstype);
+LUSTRE_RO_ATTR(fstype);
 
-static int ll_sb_uuid_seq_show(struct seq_file *m, void *v)
+static ssize_t uuid_show(struct kobject *kobj, struct attribute *attr,
+			 char *buf)
 {
-	struct super_block *sb = (struct super_block *)m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	LASSERT(sb != NULL);
-	seq_printf(m, "%s\n", ll_s2sbi(sb)->ll_sb_uuid.uuid);
-	return 0;
+	return sprintf(buf, "%s\n", sbi->ll_sb_uuid.uuid);
 }
-LPROC_SEQ_FOPS_RO(ll_sb_uuid);
+LUSTRE_RO_ATTR(uuid);
 
 static int ll_site_stats_seq_show(struct seq_file *m, void *v)
 {
@@ -218,10 +220,11 @@ static int ll_site_stats_seq_show(struct seq_file *m, void *v)
 }
 LPROC_SEQ_FOPS_RO(ll_site_stats);
 
-static int ll_max_readahead_mb_seq_show(struct seq_file *m, void *v)
+static ssize_t max_read_ahead_mb_show(struct kobject *kobj,
+				      struct attribute *attr, char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	long pages_number;
 	int mult;
 
@@ -230,23 +233,27 @@ static int ll_max_readahead_mb_seq_show(struct seq_file *m, void *v)
 	spin_unlock(&sbi->ll_lock);
 
 	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	return lprocfs_seq_read_frac_helper(m, pages_number, mult);
+	return lprocfs_read_frac_helper(buf, PAGE_SIZE, pages_number, mult);
 }
 
-static ssize_t ll_max_readahead_mb_seq_write(struct file *file,
-					     const char __user *buffer,
-					     size_t count, loff_t *off)
+static ssize_t max_read_ahead_mb_store(struct kobject *kobj,
+				       struct attribute *attr,
+				       const char *buffer,
+				       size_t count)
 {
-	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int mult, rc, pages_number;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long pages_number;
 
-	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	rc = lprocfs_write_frac_helper(buffer, count, &pages_number, mult);
+	rc = kstrtoul(buffer, 10, &pages_number);
 	if (rc)
 		return rc;
 
-	if (pages_number < 0 || pages_number > totalram_pages / 2) {
+	pages_number *= 1 << (20 - PAGE_CACHE_SHIFT); /* MB -> pages */
+
+	if (pages_number > totalram_pages / 2) {
+
 		CERROR("can't set file readahead more than %lu MB\n",
 		       totalram_pages >> (20 - PAGE_CACHE_SHIFT + 1)); /*1/2 of RAM*/
 		return -ERANGE;
@@ -258,12 +265,14 @@ static ssize_t ll_max_readahead_mb_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ll_max_readahead_mb);
+LUSTRE_RW_ATTR(max_read_ahead_mb);
 
-static int ll_max_readahead_per_file_mb_seq_show(struct seq_file *m, void *v)
+static ssize_t max_read_ahead_per_file_mb_show(struct kobject *kobj,
+					       struct attribute *attr,
+					       char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	long pages_number;
 	int mult;
 
@@ -272,24 +281,24 @@ static int ll_max_readahead_per_file_mb_seq_show(struct seq_file *m, void *v)
 	spin_unlock(&sbi->ll_lock);
 
 	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	return lprocfs_seq_read_frac_helper(m, pages_number, mult);
+	return lprocfs_read_frac_helper(buf, PAGE_SIZE, pages_number, mult);
 }
 
-static ssize_t ll_max_readahead_per_file_mb_seq_write(struct file *file,
-						  const char __user *buffer,
-						  size_t count, loff_t *off)
+static ssize_t max_read_ahead_per_file_mb_store(struct kobject *kobj,
+						struct attribute *attr,
+						const char *buffer,
+						size_t count)
 {
-	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int mult, rc, pages_number;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long pages_number;
 
-	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	rc = lprocfs_write_frac_helper(buffer, count, &pages_number, mult);
+	rc = kstrtoul(buffer, 10, &pages_number);
 	if (rc)
 		return rc;
 
-	if (pages_number < 0 ||
-		pages_number > sbi->ll_ra_info.ra_max_pages) {
+	if (pages_number > sbi->ll_ra_info.ra_max_pages) {
 		CERROR("can't set file readahead more than max_read_ahead_mb %lu MB\n",
 		       sbi->ll_ra_info.ra_max_pages);
 		return -ERANGE;
@@ -301,12 +310,14 @@ static ssize_t ll_max_readahead_per_file_mb_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ll_max_readahead_per_file_mb);
+LUSTRE_RW_ATTR(max_read_ahead_per_file_mb);
 
-static int ll_max_read_ahead_whole_mb_seq_show(struct seq_file *m, void *unused)
+static ssize_t max_read_ahead_whole_mb_show(struct kobject *kobj,
+					    struct attribute *attr,
+					    char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	long pages_number;
 	int mult;
 
@@ -315,26 +326,26 @@ static int ll_max_read_ahead_whole_mb_seq_show(struct seq_file *m, void *unused)
 	spin_unlock(&sbi->ll_lock);
 
 	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	return lprocfs_seq_read_frac_helper(m, pages_number, mult);
+	return lprocfs_read_frac_helper(buf, PAGE_SIZE, pages_number, mult);
 }
 
-static ssize_t ll_max_read_ahead_whole_mb_seq_write(struct file *file,
-						const char __user *buffer,
-						size_t count, loff_t *off)
+static ssize_t max_read_ahead_whole_mb_store(struct kobject *kobj,
+					     struct attribute *attr,
+					     const char  *buffer,
+					     size_t count)
 {
-	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int mult, rc, pages_number;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long pages_number;
 
-	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	rc = lprocfs_write_frac_helper(buffer, count, &pages_number, mult);
+	rc = kstrtoul(buffer, 10, &pages_number);
 	if (rc)
 		return rc;
 
 	/* Cap this at the current max readahead window size, the readahead
 	 * 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) {
+	if (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));
 		return -ERANGE;
@@ -346,7 +357,7 @@ static ssize_t ll_max_read_ahead_whole_mb_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ll_max_read_ahead_whole_mb);
+LUSTRE_RW_ATTR(max_read_ahead_whole_mb);
 
 static int ll_max_cached_mb_seq_show(struct seq_file *m, void *v)
 {
@@ -469,28 +480,30 @@ out:
 }
 LPROC_SEQ_FOPS(ll_max_cached_mb);
 
-static int ll_checksum_seq_show(struct seq_file *m, void *v)
+static ssize_t checksum_pages_show(struct kobject *kobj, struct attribute *attr,
+				   char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	seq_printf(m, "%u\n", (sbi->ll_flags & LL_SBI_CHECKSUM) ? 1 : 0);
-	return 0;
+	return sprintf(buf, "%u\n", (sbi->ll_flags & LL_SBI_CHECKSUM) ? 1 : 0);
 }
 
-static ssize_t ll_checksum_seq_write(struct file *file,
-				     const char __user *buffer,
-				     size_t count, loff_t *off)
+static ssize_t checksum_pages_store(struct kobject *kobj,
+				    struct attribute *attr,
+				    const char *buffer,
+				    size_t count)
 {
-	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int val, rc;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long val;
 
 	if (!sbi->ll_dt_exp)
 		/* Not set up yet */
 		return -EAGAIN;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 	if (val)
@@ -505,154 +518,146 @@ static ssize_t ll_checksum_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ll_checksum);
-
-static int ll_max_rw_chunk_seq_show(struct seq_file *m, void *v)
-{
-	struct super_block *sb = m->private;
-
-	seq_printf(m, "%lu\n", ll_s2sbi(sb)->ll_max_rw_chunk);
-	return 0;
-}
-
-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;
-
-	rc = lprocfs_write_helper(buffer, count, &val);
-	if (rc)
-		return rc;
-	ll_s2sbi(sb)->ll_max_rw_chunk = val;
-	return count;
-}
-LPROC_SEQ_FOPS(ll_max_rw_chunk);
+LUSTRE_RW_ATTR(checksum_pages);
 
-static int ll_rd_track_id(struct seq_file *m, enum stats_track_type type)
+static ssize_t ll_rd_track_id(struct kobject *kobj, char *buf,
+			      enum stats_track_type type)
 {
-	struct super_block *sb = m->private;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	if (ll_s2sbi(sb)->ll_stats_track_type == type)
-		seq_printf(m, "%d\n", ll_s2sbi(sb)->ll_stats_track_id);
-	else if (ll_s2sbi(sb)->ll_stats_track_type == STATS_TRACK_ALL)
-		seq_puts(m, "0 (all)\n");
+	if (sbi->ll_stats_track_type == type)
+		return sprintf(buf, "%d\n", sbi->ll_stats_track_id);
+	else if (sbi->ll_stats_track_type == STATS_TRACK_ALL)
+		return sprintf(buf, "0 (all)\n");
 	else
-		seq_puts(m, "untracked\n");
-
-	return 0;
+		return sprintf(buf, "untracked\n");
 }
 
-static int ll_wr_track_id(const char __user *buffer, unsigned long count,
-			  void *data, enum stats_track_type type)
+static ssize_t ll_wr_track_id(struct kobject *kobj, const char *buffer,
+			      size_t count,
+			      enum stats_track_type type)
 {
-	struct super_block *sb = data;
-	int rc, pid;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long pid;
 
-	rc = lprocfs_write_helper(buffer, count, &pid);
+	rc = kstrtoul(buffer, 10, &pid);
 	if (rc)
 		return rc;
-	ll_s2sbi(sb)->ll_stats_track_id = pid;
+	sbi->ll_stats_track_id = pid;
 	if (pid == 0)
-		ll_s2sbi(sb)->ll_stats_track_type = STATS_TRACK_ALL;
+		sbi->ll_stats_track_type = STATS_TRACK_ALL;
 	else
-		ll_s2sbi(sb)->ll_stats_track_type = type;
-	lprocfs_clear_stats(ll_s2sbi(sb)->ll_stats);
+		sbi->ll_stats_track_type = type;
+	lprocfs_clear_stats(sbi->ll_stats);
 	return count;
 }
 
-static int ll_track_pid_seq_show(struct seq_file *m, void *v)
+static ssize_t stats_track_pid_show(struct kobject *kobj,
+				    struct attribute *attr,
+				    char *buf)
 {
-	return ll_rd_track_id(m, STATS_TRACK_PID);
+	return ll_rd_track_id(kobj, buf, STATS_TRACK_PID);
 }
 
-static ssize_t ll_track_pid_seq_write(struct file *file,
-				      const char __user *buffer,
-				      size_t count, loff_t *off)
+static ssize_t stats_track_pid_store(struct kobject *kobj,
+				     struct attribute *attr,
+				     const char *buffer,
+				     size_t count)
 {
-	struct seq_file *seq = file->private_data;
-	return ll_wr_track_id(buffer, count, seq->private, STATS_TRACK_PID);
+	return ll_wr_track_id(kobj, buffer, count, STATS_TRACK_PID);
 }
-LPROC_SEQ_FOPS(ll_track_pid);
+LUSTRE_RW_ATTR(stats_track_pid);
 
-static int ll_track_ppid_seq_show(struct seq_file *m, void *v)
+static ssize_t stats_track_ppid_show(struct kobject *kobj,
+				     struct attribute *attr,
+				     char *buf)
 {
-	return ll_rd_track_id(m, STATS_TRACK_PPID);
+	return ll_rd_track_id(kobj, buf, STATS_TRACK_PPID);
 }
 
-static ssize_t ll_track_ppid_seq_write(struct file *file,
-				       const char __user *buffer,
-				       size_t count, loff_t *off)
+static ssize_t stats_track_ppid_store(struct kobject *kobj,
+				      struct attribute *attr,
+				      const char *buffer,
+				      size_t count)
 {
-	struct seq_file *seq = file->private_data;
-	return ll_wr_track_id(buffer, count, seq->private, STATS_TRACK_PPID);
+	return ll_wr_track_id(kobj, buffer, count, STATS_TRACK_PPID);
 }
-LPROC_SEQ_FOPS(ll_track_ppid);
+LUSTRE_RW_ATTR(stats_track_ppid);
 
-static int ll_track_gid_seq_show(struct seq_file *m, void *v)
+static ssize_t stats_track_gid_show(struct kobject *kobj,
+				    struct attribute *attr,
+				    char *buf)
 {
-	return ll_rd_track_id(m, STATS_TRACK_GID);
+	return ll_rd_track_id(kobj, buf, STATS_TRACK_GID);
 }
 
-static ssize_t ll_track_gid_seq_write(struct file *file,
-				      const char __user *buffer,
-				      size_t count, loff_t *off)
+static ssize_t stats_track_gid_store(struct kobject *kobj,
+				     struct attribute *attr,
+				     const char *buffer,
+				     size_t count)
 {
-	struct seq_file *seq = file->private_data;
-	return ll_wr_track_id(buffer, count, seq->private, STATS_TRACK_GID);
+	return ll_wr_track_id(kobj, buffer, count, STATS_TRACK_GID);
 }
-LPROC_SEQ_FOPS(ll_track_gid);
+LUSTRE_RW_ATTR(stats_track_gid);
 
-static int ll_statahead_max_seq_show(struct seq_file *m, void *v)
+static ssize_t statahead_max_show(struct kobject *kobj,
+				  struct attribute *attr,
+				  char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	seq_printf(m, "%u\n", sbi->ll_sa_max);
-	return 0;
+	return sprintf(buf, "%u\n", sbi->ll_sa_max);
 }
 
-static ssize_t ll_statahead_max_seq_write(struct file *file,
-					  const char __user *buffer,
-					  size_t count, loff_t *off)
+static ssize_t statahead_max_store(struct kobject *kobj,
+				   struct attribute *attr,
+				   const char *buffer,
+				   size_t count)
 {
-	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int val, rc;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
-	if (val >= 0 && val <= LL_SA_RPC_MAX)
+	if (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",
+		CERROR("Bad statahead_max value %lu. Valid values are in the range [0, %d]\n",
 		       val, LL_SA_RPC_MAX);
 
 	return count;
 }
-LPROC_SEQ_FOPS(ll_statahead_max);
+LUSTRE_RW_ATTR(statahead_max);
 
-static int ll_statahead_agl_seq_show(struct seq_file *m, void *v)
+static ssize_t statahead_agl_show(struct kobject *kobj,
+				  struct attribute *attr,
+				  char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	seq_printf(m, "%u\n", sbi->ll_flags & LL_SBI_AGL_ENABLED ? 1 : 0);
-	return 0;
+	return sprintf(buf, "%u\n", sbi->ll_flags & LL_SBI_AGL_ENABLED ? 1 : 0);
 }
 
-static ssize_t ll_statahead_agl_seq_write(struct file *file,
-					  const char __user *buffer,
-					  size_t count, loff_t *off)
+static ssize_t statahead_agl_store(struct kobject *kobj,
+				   struct attribute *attr,
+				   const char *buffer,
+				   size_t count)
 {
-	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int val, rc;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -663,7 +668,7 @@ static ssize_t ll_statahead_agl_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ll_statahead_agl);
+LUSTRE_RW_ATTR(statahead_agl);
 
 static int ll_statahead_stats_seq_show(struct seq_file *m, void *v)
 {
@@ -681,24 +686,27 @@ static int ll_statahead_stats_seq_show(struct seq_file *m, void *v)
 }
 LPROC_SEQ_FOPS_RO(ll_statahead_stats);
 
-static int ll_lazystatfs_seq_show(struct seq_file *m, void *v)
+static ssize_t lazystatfs_show(struct kobject *kobj,
+			       struct attribute *attr,
+			       char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	seq_printf(m, "%u\n", sbi->ll_flags & LL_SBI_LAZYSTATFS ? 1 : 0);
-	return 0;
+	return sprintf(buf, "%u\n", sbi->ll_flags & LL_SBI_LAZYSTATFS ? 1 : 0);
 }
 
-static ssize_t ll_lazystatfs_seq_write(struct file *file,
-				       const char __user *buffer,
-				       size_t count, loff_t *off)
+static ssize_t lazystatfs_store(struct kobject *kobj,
+				struct attribute *attr,
+				const char *buffer,
+				size_t count)
 {
-	struct super_block *sb = ((struct seq_file *)file->private_data)->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int val, rc;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -709,12 +717,14 @@ static ssize_t ll_lazystatfs_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ll_lazystatfs);
+LUSTRE_RW_ATTR(lazystatfs);
 
-static int ll_max_easize_seq_show(struct seq_file *m, void *v)
+static ssize_t max_easize_show(struct kobject *kobj,
+			       struct attribute *attr,
+			       char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	unsigned int ealen;
 	int rc;
 
@@ -722,15 +732,16 @@ static int ll_max_easize_seq_show(struct seq_file *m, void *v)
 	if (rc)
 		return rc;
 
-	seq_printf(m, "%u\n", ealen);
-	return 0;
+	return sprintf(buf, "%u\n", ealen);
 }
-LPROC_SEQ_FOPS_RO(ll_max_easize);
+LUSTRE_RO_ATTR(max_easize);
 
-static int ll_default_easize_seq_show(struct seq_file *m, void *v)
+static ssize_t default_easize_show(struct kobject *kobj,
+				   struct attribute *attr,
+				   char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 	unsigned int ealen;
 	int rc;
 
@@ -738,42 +749,9 @@ static int ll_default_easize_seq_show(struct seq_file *m, void *v)
 	if (rc)
 		return rc;
 
-	seq_printf(m, "%u\n", ealen);
-	return 0;
-}
-LPROC_SEQ_FOPS_RO(ll_default_easize);
-
-static int ll_max_cookiesize_seq_show(struct seq_file *m, void *v)
-{
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	unsigned int cookielen;
-	int rc;
-
-	rc = ll_get_max_cookiesize(sbi, &cookielen);
-	if (rc)
-		return rc;
-
-	seq_printf(m, "%u\n", cookielen);
-	return 0;
-}
-LPROC_SEQ_FOPS_RO(ll_max_cookiesize);
-
-static int ll_default_cookiesize_seq_show(struct seq_file *m, void *v)
-{
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	unsigned int cookielen;
-	int rc;
-
-	rc = ll_get_default_cookiesize(sbi, &cookielen);
-	if (rc)
-		return rc;
-
-	seq_printf(m, "%u\n", cookielen);
-	return 0;
+	return sprintf(buf, "%u\n", ealen);
 }
-LPROC_SEQ_FOPS_RO(ll_default_cookiesize);
+LUSTRE_RO_ATTR(default_easize);
 
 static int ll_sbi_flags_seq_show(struct seq_file *m, void *v)
 {
@@ -799,26 +777,27 @@ static int ll_sbi_flags_seq_show(struct seq_file *m, void *v)
 }
 LPROC_SEQ_FOPS_RO(ll_sbi_flags);
 
-static int ll_xattr_cache_seq_show(struct seq_file *m, void *v)
+static ssize_t xattr_cache_show(struct kobject *kobj,
+				struct attribute *attr,
+				char *buf)
 {
-	struct super_block *sb = m->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
 
-	seq_printf(m, "%u\n", sbi->ll_xattr_cache_enabled);
-
-	return 0;
+	return sprintf(buf, "%u\n", sbi->ll_xattr_cache_enabled);
 }
 
-static ssize_t ll_xattr_cache_seq_write(struct file *file,
-					const char __user *buffer,
-					size_t count, loff_t *off)
+static ssize_t xattr_cache_store(struct kobject *kobj,
+				 struct attribute *attr,
+				 const char *buffer,
+				 size_t count)
 {
-	struct seq_file *seq = file->private_data;
-	struct super_block *sb = seq->private;
-	struct ll_sb_info *sbi = ll_s2sbi(sb);
-	int val, rc;
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -832,46 +811,59 @@ static ssize_t ll_xattr_cache_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ll_xattr_cache);
+LUSTRE_RW_ATTR(xattr_cache);
 
 static struct lprocfs_vars lprocfs_llite_obd_vars[] = {
-	{ "uuid",	  &ll_sb_uuid_fops,	  NULL, 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 },
-	{ "kbytestotal",  &ll_kbytestotal_fops,   NULL, 0 },
-	{ "kbytesfree",   &ll_kbytesfree_fops,    NULL, 0 },
-	{ "kbytesavail",  &ll_kbytesavail_fops,   NULL, 0 },
-	{ "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 }, */
-	{ "max_read_ahead_mb", &ll_max_readahead_mb_fops, NULL },
-	{ "max_read_ahead_per_file_mb", &ll_max_readahead_per_file_mb_fops,
-		NULL },
-	{ "max_read_ahead_whole_mb", &ll_max_read_ahead_whole_mb_fops, NULL },
 	{ "max_cached_mb",    &ll_max_cached_mb_fops, NULL },
-	{ "checksum_pages",   &ll_checksum_fops, NULL },
-	{ "max_rw_chunk",     &ll_max_rw_chunk_fops, NULL },
-	{ "stats_track_pid",  &ll_track_pid_fops, NULL },
-	{ "stats_track_ppid", &ll_track_ppid_fops, NULL },
-	{ "stats_track_gid",  &ll_track_gid_fops, NULL },
-	{ "statahead_max",    &ll_statahead_max_fops, NULL },
-	{ "statahead_agl",    &ll_statahead_agl_fops, NULL },
 	{ "statahead_stats",  &ll_statahead_stats_fops, NULL, 0 },
-	{ "lazystatfs",       &ll_lazystatfs_fops, NULL },
-	{ "max_easize",       &ll_max_easize_fops, NULL, 0 },
-	{ "default_easize",   &ll_default_easize_fops, NULL, 0 },
-	{ "max_cookiesize",   &ll_max_cookiesize_fops, NULL, 0 },
-	{ "default_cookiesize", &ll_default_cookiesize_fops, NULL, 0 },
 	{ "sbi_flags",	      &ll_sbi_flags_fops, NULL, 0 },
-	{ "xattr_cache",      &ll_xattr_cache_fops, NULL, 0 },
 	{ NULL }
 };
 
 #define MAX_STRING_SIZE 128
 
+static struct attribute *llite_attrs[] = {
+	&lustre_attr_blocksize.attr,
+	&lustre_attr_kbytestotal.attr,
+	&lustre_attr_kbytesfree.attr,
+	&lustre_attr_kbytesavail.attr,
+	&lustre_attr_filestotal.attr,
+	&lustre_attr_filesfree.attr,
+	&lustre_attr_client_type.attr,
+	&lustre_attr_fstype.attr,
+	&lustre_attr_uuid.attr,
+	&lustre_attr_max_read_ahead_mb.attr,
+	&lustre_attr_max_read_ahead_per_file_mb.attr,
+	&lustre_attr_max_read_ahead_whole_mb.attr,
+	&lustre_attr_checksum_pages.attr,
+	&lustre_attr_stats_track_pid.attr,
+	&lustre_attr_stats_track_ppid.attr,
+	&lustre_attr_stats_track_gid.attr,
+	&lustre_attr_statahead_max.attr,
+	&lustre_attr_statahead_agl.attr,
+	&lustre_attr_lazystatfs.attr,
+	&lustre_attr_max_easize.attr,
+	&lustre_attr_default_easize.attr,
+	&lustre_attr_xattr_cache.attr,
+	NULL,
+};
+
+static void llite_sb_release(struct kobject *kobj)
+{
+	struct ll_sb_info *sbi = container_of(kobj, struct ll_sb_info,
+					      ll_kobj);
+	complete(&sbi->ll_kobj_unregister);
+}
+
+static struct kobj_type llite_ktype = {
+	.default_attrs	= llite_attrs,
+	.sysfs_ops	= &lustre_sysfs_ops,
+	.release	= llite_sb_release,
+};
+
 static const struct llite_file_opcode {
 	__u32       opcode;
 	__u32       type;
@@ -958,24 +950,18 @@ static const char *ra_stat_string[] = {
 	[RA_STAT_WRONG_GRAB_PAGE] = "wrong page from grab_cache_page",
 };
 
-LPROC_SEQ_FOPS_RO_TYPE(llite, name);
-LPROC_SEQ_FOPS_RO_TYPE(llite, uuid);
-
-int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
-				struct super_block *sb, char *osc, char *mdc)
+int ldebugfs_register_mountpoint(struct dentry *parent,
+				 struct super_block *sb, char *osc, char *mdc)
 {
-	struct lprocfs_vars lvars[2];
 	struct lustre_sb_info *lsi = s2lsi(sb);
 	struct ll_sb_info *sbi = ll_s2sbi(sb);
 	struct obd_device *obd;
-	struct proc_dir_entry *dir;
+	struct dentry *dir;
 	char name[MAX_STRING_SIZE + 1], *ptr;
 	int err, id, len, rc;
 
-	memset(lvars, 0, sizeof(lvars));
 
 	name[MAX_STRING_SIZE] = '\0';
-	lvars[0].name = name;
 
 	LASSERT(sbi != NULL);
 	LASSERT(mdc != NULL);
@@ -991,30 +977,32 @@ int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
 	snprintf(name, MAX_STRING_SIZE, "%.*s-%p", len,
 		 lsi->lsi_lmd->lmd_profile, sb);
 
-	sbi->ll_proc_root = lprocfs_register(name, parent, NULL, NULL);
-	if (IS_ERR(sbi->ll_proc_root)) {
-		err = PTR_ERR(sbi->ll_proc_root);
-		sbi->ll_proc_root = NULL;
+	dir = ldebugfs_register(name, parent, NULL, NULL);
+	if (IS_ERR_OR_NULL(dir)) {
+		err = dir ? PTR_ERR(dir) : -ENOMEM;
+		sbi->ll_debugfs_entry = NULL;
 		return err;
 	}
+	sbi->ll_debugfs_entry = dir;
 
-	rc = lprocfs_seq_create(sbi->ll_proc_root, "dump_page_cache", 0444,
-				&vvp_dump_pgcache_file_ops, sbi);
+	rc = ldebugfs_seq_create(sbi->ll_debugfs_entry, "dump_page_cache", 0444,
+				 &vvp_dump_pgcache_file_ops, sbi);
 	if (rc)
 		CWARN("Error adding the dump_page_cache file\n");
 
-	rc = lprocfs_seq_create(sbi->ll_proc_root, "extents_stats", 0644,
-				&ll_rw_extents_stats_fops, sbi);
+	rc = ldebugfs_seq_create(sbi->ll_debugfs_entry, "extents_stats", 0644,
+				 &ll_rw_extents_stats_fops, sbi);
 	if (rc)
 		CWARN("Error adding the extent_stats file\n");
 
-	rc = lprocfs_seq_create(sbi->ll_proc_root, "extents_stats_per_process",
-				0644, &ll_rw_extents_stats_pp_fops, sbi);
+	rc = ldebugfs_seq_create(sbi->ll_debugfs_entry,
+				  "extents_stats_per_process",
+				 0644, &ll_rw_extents_stats_pp_fops, sbi);
 	if (rc)
 		CWARN("Error adding the extents_stats_per_process file\n");
 
-	rc = lprocfs_seq_create(sbi->ll_proc_root, "offset_stats", 0644,
-				&ll_rw_offset_stats_fops, sbi);
+	rc = ldebugfs_seq_create(sbi->ll_debugfs_entry, "offset_stats", 0644,
+				 &ll_rw_offset_stats_fops, sbi);
 	if (rc)
 		CWARN("Error adding the offset_stats file\n");
 
@@ -1040,7 +1028,8 @@ int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
 				     (type & LPROCFS_CNTR_AVGMINMAX),
 				     llite_opcode_table[id].opname, ptr);
 	}
-	err = lprocfs_register_stats(sbi->ll_proc_root, "stats", sbi->ll_stats);
+	err = ldebugfs_register_stats(sbi->ll_debugfs_entry, "stats",
+				     sbi->ll_stats);
 	if (err)
 		goto out;
 
@@ -1054,76 +1043,53 @@ int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
 	for (id = 0; id < ARRAY_SIZE(ra_stat_string); id++)
 		lprocfs_counter_init(sbi->ll_ra_stats, id, 0,
 				     ra_stat_string[id], "pages");
-	err = lprocfs_register_stats(sbi->ll_proc_root, "read_ahead_stats",
+
+	err = ldebugfs_register_stats(sbi->ll_debugfs_entry, "read_ahead_stats",
 				     sbi->ll_ra_stats);
 	if (err)
 		goto out;
 
 
-	err = lprocfs_add_vars(sbi->ll_proc_root, lprocfs_llite_obd_vars, sb);
+	err = ldebugfs_add_vars(sbi->ll_debugfs_entry,
+				lprocfs_llite_obd_vars, sb);
 	if (err)
 		goto out;
 
-	/* MDC info */
-	obd = class_name2obd(mdc);
-
-	LASSERT(obd != NULL);
-	LASSERT(obd->obd_magic == OBD_DEVICE_MAGIC);
-	LASSERT(obd->obd_type->typ_name != NULL);
-
-	dir = proc_mkdir(obd->obd_type->typ_name, sbi->ll_proc_root);
-	if (dir == NULL) {
-		err = -ENOMEM;
-		goto out;
-	}
-
-	snprintf(name, MAX_STRING_SIZE, "common_name");
-	lvars[0].fops = &llite_name_fops;
-	err = lprocfs_add_vars(dir, lvars, obd);
+	sbi->ll_kobj.kset = llite_kset;
+	init_completion(&sbi->ll_kobj_unregister);
+	err = kobject_init_and_add(&sbi->ll_kobj, &llite_ktype, NULL,
+				   "%s", name);
 	if (err)
 		goto out;
 
-	snprintf(name, MAX_STRING_SIZE, "uuid");
-	lvars[0].fops = &llite_uuid_fops;
-	err = lprocfs_add_vars(dir, lvars, obd);
+	/* MDC info */
+	obd = class_name2obd(mdc);
+
+	err = sysfs_create_link(&sbi->ll_kobj, &obd->obd_kobj,
+				obd->obd_type->typ_name);
 	if (err)
 		goto out;
 
 	/* OSC */
 	obd = class_name2obd(osc);
 
-	LASSERT(obd != NULL);
-	LASSERT(obd->obd_magic == OBD_DEVICE_MAGIC);
-	LASSERT(obd->obd_type->typ_name != NULL);
-
-	dir = proc_mkdir(obd->obd_type->typ_name, sbi->ll_proc_root);
-	if (dir == NULL) {
-		err = -ENOMEM;
-		goto out;
-	}
-
-	snprintf(name, MAX_STRING_SIZE, "common_name");
-	lvars[0].fops = &llite_name_fops;
-	err = lprocfs_add_vars(dir, lvars, obd);
-	if (err)
-		goto out;
-
-	snprintf(name, MAX_STRING_SIZE, "uuid");
-	lvars[0].fops = &llite_uuid_fops;
-	err = lprocfs_add_vars(dir, lvars, obd);
+	err = sysfs_create_link(&sbi->ll_kobj, &obd->obd_kobj,
+				obd->obd_type->typ_name);
 out:
 	if (err) {
-		lprocfs_remove(&sbi->ll_proc_root);
+		ldebugfs_remove(&sbi->ll_debugfs_entry);
 		lprocfs_free_stats(&sbi->ll_ra_stats);
 		lprocfs_free_stats(&sbi->ll_stats);
 	}
 	return err;
 }
 
-void lprocfs_unregister_mountpoint(struct ll_sb_info *sbi)
+void ldebugfs_unregister_mountpoint(struct ll_sb_info *sbi)
 {
-	if (sbi->ll_proc_root) {
-		lprocfs_remove(&sbi->ll_proc_root);
+	if (sbi->ll_debugfs_entry) {
+		ldebugfs_remove(&sbi->ll_debugfs_entry);
+		kobject_put(&sbi->ll_kobj);
+		wait_for_completion(&sbi->ll_kobj_unregister);
 		lprocfs_free_stats(&sbi->ll_ra_stats);
 		lprocfs_free_stats(&sbi->ll_stats);
 	}
@@ -1531,6 +1497,5 @@ LPROC_SEQ_FOPS(ll_rw_offset_stats);
 
 void lprocfs_llite_init_vars(struct lprocfs_static_vars *lvars)
 {
-    lvars->module_vars  = NULL;
     lvars->obd_vars     = lprocfs_llite_obd_vars;
 }
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index 5a25dcd10126..72ce6e72845f 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -665,7 +665,7 @@ static int ll_atomic_open(struct inode *dir, struct dentry *dentry,
 
 out_release:
 	ll_intent_release(it);
-	OBD_FREE(it, sizeof(*it));
+	kfree(it);
 
 	return rc;
 }
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index c6c824356464..b17b7cea582c 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -200,12 +200,12 @@ static inline int ll_get_user_pages(int rw, unsigned long user_addr,
 	*max_pages = (user_addr + size + PAGE_CACHE_SIZE - 1) >> PAGE_CACHE_SHIFT;
 	*max_pages -= user_addr >> PAGE_CACHE_SHIFT;
 
-	OBD_ALLOC_LARGE(*pages, *max_pages * sizeof(**pages));
+	*pages = libcfs_kvzalloc(*max_pages * sizeof(**pages), GFP_NOFS);
 	if (*pages) {
 		result = get_user_pages_fast(user_addr, *max_pages,
 					     (rw == READ), *pages);
 		if (unlikely(result <= 0))
-			OBD_FREE_LARGE(*pages, *max_pages * sizeof(**pages));
+			kvfree(*pages);
 	}
 
 	return result;
@@ -517,7 +517,6 @@ static int ll_migratepage(struct address_space *mapping,
 }
 #endif
 
-#ifndef MS_HAS_NEW_AOPS
 const struct address_space_operations ll_aops = {
 	.readpage	= ll_readpage,
 	.direct_IO      = ll_direct_IO_26,
@@ -532,22 +531,3 @@ const struct address_space_operations ll_aops = {
 	.migratepage    = ll_migratepage,
 #endif
 };
-#else
-const struct address_space_operations_ext ll_aops = {
-	.orig_aops.readpage       = ll_readpage,
-/*	.orig_aops.readpages      = ll_readpages, */
-	.orig_aops.direct_IO      = ll_direct_IO_26,
-	.orig_aops.writepage      = ll_writepage,
-	.orig_aops.writepages     = ll_writepages,
-	.orig_aops.set_page_dirty = ll_set_page_dirty,
-	.orig_aops.prepare_write  = ll_prepare_write,
-	.orig_aops.commit_write   = ll_commit_write,
-	.orig_aops.invalidatepage = ll_invalidatepage,
-	.orig_aops.releasepage    = ll_releasepage,
-#ifdef CONFIG_MIGRATION
-	.orig_aops.migratepage    = ll_migratepage,
-#endif
-	.write_begin    = ll_write_begin,
-	.write_end      = ll_write_end
-};
-#endif
diff --git a/drivers/staging/lustre/lustre/llite/statahead.c b/drivers/staging/lustre/lustre/llite/statahead.c
index 7f8071242f23..f97371dd8539 100644
--- a/drivers/staging/lustre/lustre/llite/statahead.c
+++ b/drivers/staging/lustre/lustre/llite/statahead.c
@@ -312,7 +312,7 @@ static void ll_sa_entry_cleanup(struct ll_statahead_info *sai,
 		entry->se_minfo = NULL;
 		ll_intent_release(&minfo->mi_it);
 		iput(minfo->mi_dir);
-		OBD_FREE_PTR(minfo);
+		kfree(minfo);
 	}
 
 	if (req) {
@@ -336,7 +336,7 @@ static void ll_sa_entry_put(struct ll_statahead_info *sai,
 		ll_sa_entry_cleanup(sai, entry);
 		iput(entry->se_inode);
 
-		OBD_FREE(entry, entry->se_size);
+		kfree(entry);
 		atomic_dec(&sai->sai_cache_count);
 	}
 }
@@ -544,7 +544,7 @@ static void ll_sai_put(struct ll_statahead_info *sai)
 		LASSERT(agl_list_empty(sai));
 
 		iput(inode);
-		OBD_FREE_PTR(sai);
+		kfree(sai);
 	}
 }
 
@@ -772,7 +772,7 @@ out:
 	if (rc != 0) {
 		ll_intent_release(it);
 		iput(dir);
-		OBD_FREE_PTR(minfo);
+		kfree(minfo);
 	}
 	if (sai != NULL)
 		ll_sai_put(sai);
@@ -786,8 +786,8 @@ static void sa_args_fini(struct md_enqueue_info *minfo,
 	iput(minfo->mi_dir);
 	capa_put(minfo->mi_data.op_capa1);
 	capa_put(minfo->mi_data.op_capa2);
-	OBD_FREE_PTR(minfo);
-	OBD_FREE_PTR(einfo);
+	kfree(minfo);
+	kfree(einfo);
 }
 
 /**
@@ -816,15 +816,15 @@ static int sa_args_init(struct inode *dir, struct inode *child,
 
 	minfo = kzalloc(sizeof(*minfo), GFP_NOFS);
 	if (!minfo) {
-		OBD_FREE_PTR(einfo);
+		kfree(einfo);
 		return -ENOMEM;
 	}
 
 	op_data = ll_prep_md_op_data(&minfo->mi_data, dir, child, qstr->name,
 				     qstr->len, 0, LUSTRE_OPC_ANY, NULL);
 	if (IS_ERR(op_data)) {
-		OBD_FREE_PTR(einfo);
-		OBD_FREE_PTR(minfo);
+		kfree(einfo);
+		kfree(minfo);
 		return PTR_ERR(op_data);
 	}
 
@@ -1719,8 +1719,7 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 	return -EAGAIN;
 
 out:
-	if (sai != NULL)
-		OBD_FREE_PTR(sai);
+	kfree(sai);
 	spin_lock(&lli->lli_sa_lock);
 	lli->lli_opendir_key = NULL;
 	lli->lli_opendir_pid = 0;
diff --git a/drivers/staging/lustre/lustre/llite/super25.c b/drivers/staging/lustre/lustre/llite/super25.c
index a494f6271fa0..e4020ce8cb7b 100644
--- a/drivers/staging/lustre/lustre/llite/super25.c
+++ b/drivers/staging/lustre/lustre/llite/super25.c
@@ -89,7 +89,6 @@ void lustre_register_client_process_config(int (*cpc)(struct lustre_cfg *lcfg));
 
 static int __init init_lustre_lite(void)
 {
-	struct proc_dir_entry *entry;
 	lnet_process_id_t lnet_id;
 	struct timeval tv;
 	int i, rc, seed[2];
@@ -128,15 +127,18 @@ static int __init init_lustre_lite(void)
 	if (ll_rmtperm_hash_cachep == NULL)
 		goto out_cache;
 
-	entry = lprocfs_register("llite", proc_lustre_root, NULL, NULL);
-	if (IS_ERR(entry)) {
-		rc = PTR_ERR(entry);
-		CERROR("cannot register '/proc/fs/lustre/llite': rc = %d\n",
-		       rc);
+	llite_root = debugfs_create_dir("llite", debugfs_lustre_root);
+	if (IS_ERR_OR_NULL(llite_root)) {
+		rc = llite_root ? PTR_ERR(llite_root) : -ENOMEM;
+		llite_root = NULL;
 		goto out_cache;
 	}
 
-	proc_lustre_fs_root = entry;
+	llite_kset = kset_create_and_add("llite", NULL, lustre_kobj);
+	if (!llite_kset) {
+		rc = -ENOMEM;
+		goto out_debugfs;
+	}
 
 	cfs_get_random_bytes(seed, sizeof(seed));
 
@@ -155,7 +157,7 @@ static int __init init_lustre_lite(void)
 	setup_timer(&ll_capa_timer, ll_capa_timer_callback, 0);
 	rc = ll_capa_thread_start();
 	if (rc != 0)
-		goto out_proc;
+		goto out_sysfs;
 
 	rc = vvp_global_init();
 	if (rc != 0)
@@ -176,8 +178,10 @@ out_vvp:
 out_capa:
 	del_timer(&ll_capa_timer);
 	ll_capa_thread_stop();
-out_proc:
-	lprocfs_remove(&proc_lustre_fs_root);
+out_sysfs:
+	kset_unregister(llite_kset);
+out_debugfs:
+	debugfs_remove(llite_root);
 out_cache:
 	if (ll_inode_cachep != NULL)
 		kmem_cache_destroy(ll_inode_cachep);
@@ -200,7 +204,8 @@ static void __exit exit_lustre_lite(void)
 	lustre_register_kill_super_cb(NULL);
 	lustre_register_client_process_config(NULL);
 
-	lprocfs_remove(&proc_lustre_fs_root);
+	debugfs_remove(llite_root);
+	kset_unregister(llite_kset);
 
 	ll_xattr_fini();
 	vvp_global_fini();
diff --git a/drivers/staging/lustre/lustre/llite/symlink.c b/drivers/staging/lustre/lustre/llite/symlink.c
index 3711e671a4df..69b203651905 100644
--- a/drivers/staging/lustre/lustre/llite/symlink.c
+++ b/drivers/staging/lustre/lustre/llite/symlink.c
@@ -118,7 +118,7 @@ failed:
 	return rc;
 }
 
-static void *ll_follow_link(struct dentry *dentry, struct nameidata *nd)
+static const char *ll_follow_link(struct dentry *dentry, void **cookie)
 {
 	struct inode *inode = d_inode(dentry);
 	struct ptlrpc_request *request = NULL;
@@ -126,32 +126,22 @@ static void *ll_follow_link(struct dentry *dentry, struct nameidata *nd)
 	char *symname = NULL;
 
 	CDEBUG(D_VFSTRACE, "VFS Op\n");
-	/* Limit the recursive symlink depth to 5 instead of default
-	 * 8 links when kernel has 4k stack to prevent stack overflow.
-	 * For 8k stacks we need to limit it to 7 for local servers. */
-	if (THREAD_SIZE < 8192 && current->link_count >= 6) {
-		rc = -ELOOP;
-	} else if (THREAD_SIZE == 8192 && current->link_count >= 8) {
-		rc = -ELOOP;
-	} else {
-		ll_inode_size_lock(inode);
-		rc = ll_readlink_internal(inode, &request, &symname);
-		ll_inode_size_unlock(inode);
-	}
+	ll_inode_size_lock(inode);
+	rc = ll_readlink_internal(inode, &request, &symname);
+	ll_inode_size_unlock(inode);
 	if (rc) {
 		ptlrpc_req_finished(request);
-		request = NULL;
-		symname = ERR_PTR(rc);
+		return ERR_PTR(rc);
 	}
 
-	nd_set_link(nd, symname);
 	/* symname may contain a pointer to the request message buffer,
 	 * we delay request releasing until ll_put_link then.
 	 */
-	return request;
+	*cookie = request;
+	return symname;
 }
 
-static void ll_put_link(struct dentry *dentry, struct nameidata *nd, void *cookie)
+static void ll_put_link(struct inode *unused, void *cookie)
 {
 	ptlrpc_req_finished(cookie);
 }
diff --git a/drivers/staging/lustre/lustre/llite/vvp_dev.c b/drivers/staging/lustre/lustre/llite/vvp_dev.c
index fde41d7c5e3d..b8f6a8779fd3 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_dev.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_dev.c
@@ -253,7 +253,7 @@ int cl_sb_fini(struct super_block *sb)
 
 /****************************************************************************
  *
- * /proc/fs/lustre/llite/$MNT/dump_page_cache
+ * debugfs/lustre/llite/$MNT/dump_page_cache
  *
  ****************************************************************************/
 
@@ -517,7 +517,7 @@ static void vvp_pgcache_stop(struct seq_file *f, void *v)
 	/* Nothing to do */
 }
 
-static struct seq_operations vvp_pgcache_ops = {
+static const struct seq_operations vvp_pgcache_ops = {
 	.start = vvp_pgcache_start,
 	.next  = vvp_pgcache_next,
 	.stop  = vvp_pgcache_stop,
@@ -526,16 +526,17 @@ static struct seq_operations vvp_pgcache_ops = {
 
 static int vvp_dump_pgcache_seq_open(struct inode *inode, struct file *filp)
 {
-	struct ll_sb_info     *sbi = PDE_DATA(inode);
-	struct seq_file       *seq;
-	int		    result;
+	struct seq_file *seq;
+	int rc;
 
-	result = seq_open(filp, &vvp_pgcache_ops);
-	if (result == 0) {
-		seq = filp->private_data;
-		seq->private = sbi;
-	}
-	return result;
+	rc = seq_open(filp, &vvp_pgcache_ops);
+	if (rc)
+		return rc;
+
+	seq = filp->private_data;
+	seq->private = inode->i_private;
+
+	return 0;
 }
 
 const struct file_operations vvp_dump_pgcache_file_ops = {
diff --git a/drivers/staging/lustre/lustre/llite/xattr.c b/drivers/staging/lustre/lustre/llite/xattr.c
index e0fcbe1395fd..362a87d0d0d3 100644
--- a/drivers/staging/lustre/lustre/llite/xattr.c
+++ b/drivers/staging/lustre/lustre/llite/xattr.c
@@ -188,11 +188,11 @@ int ll_setxattr_common(struct inode *inode, const char *name,
 		valid |= rce_ops2valid(rce->rce_ops);
 	}
 #endif
-		oc = ll_mdscapa_get(inode);
-		rc = md_setxattr(sbi->ll_md_exp, ll_inode2fid(inode), oc,
-				valid, name, pv, size, 0, flags,
-				ll_i2suppgid(inode), &req);
-		capa_put(oc);
+	oc = ll_mdscapa_get(inode);
+	rc = md_setxattr(sbi->ll_md_exp, ll_inode2fid(inode), oc,
+			 valid, name, pv, size, 0, flags,
+			 ll_i2suppgid(inode), &req);
+	capa_put(oc);
 #ifdef CONFIG_FS_POSIX_ACL
 	if (new_value != NULL)
 		lustre_posix_acl_xattr_free(new_value, size);
diff --git a/drivers/staging/lustre/lustre/llite/xattr_cache.c b/drivers/staging/lustre/lustre/llite/xattr_cache.c
index 69ea92adf4f1..6956dec53fcc 100644
--- a/drivers/staging/lustre/lustre/llite/xattr_cache.c
+++ b/drivers/staging/lustre/lustre/llite/xattr_cache.c
@@ -144,7 +144,7 @@ static int ll_xattr_cache_add(struct list_head *cache,
 
 	return 0;
 err_value:
-	OBD_FREE(xattr->xe_name, xattr->xe_namelen);
+	kfree(xattr->xe_name);
 err_name:
 	OBD_SLAB_FREE_PTR(xattr, xattr_kmem);
 
@@ -170,8 +170,8 @@ static int ll_xattr_cache_del(struct list_head *cache,
 
 	if (ll_xattr_cache_find(cache, xattr_name, &xattr) == 0) {
 		list_del(&xattr->xe_list);
-		OBD_FREE(xattr->xe_name, xattr->xe_namelen);
-		OBD_FREE(xattr->xe_value, xattr->xe_vallen);
+		kfree(xattr->xe_name);
+		kfree(xattr->xe_value);
 		OBD_SLAB_FREE_PTR(xattr, xattr_kmem);
 
 		return 0;
diff --git a/drivers/staging/lustre/lustre/lmv/Makefile b/drivers/staging/lustre/lustre/lmv/Makefile
index a7a15369af15..1a24299791d7 100644
--- a/drivers/staging/lustre/lustre/lmv/Makefile
+++ b/drivers/staging/lustre/lustre/lmv/Makefile
@@ -1,3 +1,2 @@
 obj-$(CONFIG_LUSTRE_FS) += lmv.o
-lmv-y := lmv_obd.o lmv_intent.o lmv_fld.o
-lmv-$(CONFIG_PROC_FS) += lproc_lmv.o
+lmv-y := lmv_obd.o lmv_intent.o lmv_fld.o lproc_lmv.o
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_intent.c b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
index d22d57b4ff38..cb35f6341fb2 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_intent.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
@@ -99,7 +99,7 @@ static int lmv_intent_remote(struct obd_export *exp, void *lmm,
 		goto out;
 	}
 
-	OBD_ALLOC_PTR(op_data);
+	op_data = kzalloc(sizeof(*op_data), GFP_NOFS);
 	if (op_data == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -142,7 +142,7 @@ static int lmv_intent_remote(struct obd_export *exp, void *lmm,
 	it->d.lustre.it_lock_mode = pmode;
 
 out_free_op_data:
-	OBD_FREE_PTR(op_data);
+	kfree(op_data);
 out:
 	if (rc && pmode)
 		ldlm_lock_decref(&plock, pmode);
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_internal.h b/drivers/staging/lustre/lustre/lmv/lmv_internal.h
index 852d78721ca9..b808728daee7 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_internal.h
+++ b/drivers/staging/lustre/lustre/lmv/lmv_internal.h
@@ -144,14 +144,8 @@ struct lmv_tgt_desc
 *lmv_locate_mds(struct lmv_obd *lmv, struct md_op_data *op_data,
 		struct lu_fid *fid);
 /* lproc_lmv.c */
-#if defined(CONFIG_PROC_FS)
 void lprocfs_lmv_init_vars(struct lprocfs_static_vars *lvars);
-#else
-static inline void lprocfs_lmv_init_vars(struct lprocfs_static_vars *lvars)
-{
-	memset(lvars, 0, sizeof(*lvars));
-}
-#endif
+
 extern struct file_operations lmv_proc_target_fops;
 
 #endif
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index b9459faf8645..ac5053cd5da5 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -199,7 +199,6 @@ static int lmv_connect(const struct lu_env *env,
 		       struct obd_uuid *cluuid, struct obd_connect_data *data,
 		       void *localdata)
 {
-	struct proc_dir_entry *lmv_proc_dir;
 	struct lmv_obd	*lmv = &obd->u.lmv;
 	struct lustre_handle  conn = { 0 };
 	int		    rc = 0;
@@ -230,19 +229,8 @@ static int lmv_connect(const struct lu_env *env,
 	if (data)
 		lmv->conn_data = *data;
 
-	if (obd->obd_proc_private != NULL) {
-		lmv_proc_dir = obd->obd_proc_private;
-	} else {
-		lmv_proc_dir = lprocfs_register("target_obds", obd->obd_proc_entry,
-						NULL, NULL);
-		if (IS_ERR(lmv_proc_dir)) {
-			CERROR("could not register /proc/fs/lustre/%s/%s/target_obds.",
-			       obd->obd_type->typ_name, obd->obd_name);
-			lmv_proc_dir = NULL;
-		}
-		obd->obd_proc_private = lmv_proc_dir;
-	}
-
+	lmv->lmv_tgts_kobj = kobject_create_and_add("target_obds",
+						    &obd->obd_kobj);
 	/*
 	 * All real clients should perform actual connection right away, because
 	 * it is possible, that LMV will not have opportunity to connect targets
@@ -252,10 +240,8 @@ static int lmv_connect(const struct lu_env *env,
 	if (data->ocd_connect_flags & OBD_CONNECT_REAL)
 		rc = lmv_check_connect(obd);
 
-	if (rc && lmv_proc_dir) {
-		lprocfs_remove(&lmv_proc_dir);
-		obd->obd_proc_private = NULL;
-	}
+	if (rc && lmv->lmv_tgts_kobj)
+		kobject_put(lmv->lmv_tgts_kobj);
 
 	return rc;
 }
@@ -337,7 +323,6 @@ static int lmv_init_ea_size(struct obd_export *exp, int easize,
 
 static int lmv_connect_mdc(struct obd_device *obd, struct lmv_tgt_desc *tgt)
 {
-	struct proc_dir_entry   *lmv_proc_dir;
 	struct lmv_obd	  *lmv = &obd->u.lmv;
 	struct obd_uuid	 *cluuid = &lmv->cluuid;
 	struct obd_uuid	  lmv_mdc_uuid = { "LMV_MDC_UUID" };
@@ -415,25 +400,10 @@ static int lmv_connect_mdc(struct obd_device *obd, struct lmv_tgt_desc *tgt)
 		mdc_obd->obd_name, mdc_obd->obd_uuid.uuid,
 		atomic_read(&obd->obd_refcount));
 
-	lmv_proc_dir = obd->obd_proc_private;
-	if (lmv_proc_dir) {
-		struct proc_dir_entry *mdc_symlink;
-
-		LASSERT(mdc_obd->obd_type != NULL);
-		LASSERT(mdc_obd->obd_type->typ_name != NULL);
-		mdc_symlink = lprocfs_add_symlink(mdc_obd->obd_name,
-						  lmv_proc_dir,
-						  "../../../%s/%s",
-						  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.",
-			       obd->obd_type->typ_name, obd->obd_name,
-			       mdc_obd->obd_name);
-			lprocfs_remove(&lmv_proc_dir);
-			obd->obd_proc_private = NULL;
-		}
-	}
+	if (lmv->lmv_tgts_kobj)
+		/* Even if we failed to create the link, that's fine */
+		rc = sysfs_create_link(lmv->lmv_tgts_kobj, &mdc_obd->obd_kobj,
+				       mdc_obd->obd_name);
 	return 0;
 }
 
@@ -442,7 +412,7 @@ static void lmv_del_target(struct lmv_obd *lmv, int index)
 	if (lmv->tgts[index] == NULL)
 		return;
 
-	OBD_FREE_PTR(lmv->tgts[index]);
+	kfree(lmv->tgts[index]);
 	lmv->tgts[index] = NULL;
 	return;
 }
@@ -488,7 +458,7 @@ static int lmv_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 
 		while (newsize < index + 1)
 			newsize <<= 1;
-		OBD_ALLOC(newtgts, sizeof(*newtgts) * newsize);
+		newtgts = kcalloc(newsize, sizeof(*newtgts), GFP_NOFS);
 		if (newtgts == NULL) {
 			lmv_init_unlock(lmv);
 			return -ENOMEM;
@@ -504,14 +474,13 @@ static int lmv_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 		lmv->tgts = newtgts;
 		lmv->tgts_size = newsize;
 		smp_rmb();
-		if (old)
-			OBD_FREE(old, sizeof(*old) * oldsize);
+		kfree(old);
 
 		CDEBUG(D_CONFIG, "tgts: %p size: %d\n", lmv->tgts,
 		       lmv->tgts_size);
 	}
 
-	OBD_ALLOC_PTR(tgt);
+	tgt = kzalloc(sizeof(*tgt), GFP_NOFS);
 	if (!tgt) {
 		lmv_init_unlock(lmv);
 		return -ENOMEM;
@@ -611,7 +580,6 @@ int lmv_check_connect(struct obd_device *obd)
 
 static int lmv_disconnect_mdc(struct obd_device *obd, struct lmv_tgt_desc *tgt)
 {
-	struct proc_dir_entry  *lmv_proc_dir;
 	struct lmv_obd	 *lmv = &obd->u.lmv;
 	struct obd_device      *mdc_obd;
 	int		     rc;
@@ -627,9 +595,9 @@ static int lmv_disconnect_mdc(struct obd_device *obd, struct lmv_tgt_desc *tgt)
 		mdc_obd->obd_no_recov = obd->obd_no_recov;
 	}
 
-	lmv_proc_dir = obd->obd_proc_private;
-	if (lmv_proc_dir)
-		lprocfs_remove_proc_entry(mdc_obd->obd_name, lmv_proc_dir);
+	if (lmv->lmv_tgts_kobj)
+		sysfs_remove_link(lmv->lmv_tgts_kobj,
+				  mdc_obd->obd_name);
 
 	rc = obd_fid_fini(tgt->ltd_exp->exp_obd);
 	if (rc)
@@ -677,11 +645,8 @@ static int lmv_disconnect(struct obd_export *exp)
 		lmv_disconnect_mdc(obd, lmv->tgts[i]);
 	}
 
-	if (obd->obd_proc_private)
-		lprocfs_remove((struct proc_dir_entry **)&obd->obd_proc_private);
-	else
-		CERROR("/proc/fs/lustre/%s/%s/target_obds missing\n",
-		       obd->obd_type->typ_name, obd->obd_name);
+	if (lmv->lmv_tgts_kobj)
+		kobject_put(lmv->lmv_tgts_kobj);
 
 out_local:
 	/*
@@ -750,7 +715,7 @@ repeat_fid2path:
 	/* sigh, has to go to another MDT to do path building further */
 	if (remote_gf == NULL) {
 		remote_gf_size = sizeof(*remote_gf) + PATH_MAX;
-		OBD_ALLOC(remote_gf, remote_gf_size);
+		remote_gf = kzalloc(remote_gf_size, GFP_NOFS);
 		if (remote_gf == NULL) {
 			rc = -ENOMEM;
 			goto out_fid2path;
@@ -780,8 +745,7 @@ repeat_fid2path:
 	goto repeat_fid2path;
 
 out_fid2path:
-	if (remote_gf != NULL)
-		OBD_FREE(remote_gf, remote_gf_size);
+	kfree(remote_gf);
 	return rc;
 }
 
@@ -984,7 +948,7 @@ static int lmv_iocontrol(unsigned int cmd, struct obd_export *exp,
 			return -EAGAIN;
 
 		LASSERT(tgt && tgt->ltd_exp);
-		OBD_ALLOC_PTR(oqctl);
+		oqctl = kzalloc(sizeof(*oqctl), GFP_NOFS);
 		if (!oqctl)
 			return -ENOMEM;
 
@@ -995,7 +959,7 @@ static int lmv_iocontrol(unsigned int cmd, struct obd_export *exp,
 			qctl->qc_valid = QC_MDTIDX;
 			qctl->obd_uuid = tgt->ltd_uuid;
 		}
-		OBD_FREE_PTR(oqctl);
+		kfree(oqctl);
 		break;
 	}
 	case OBD_IOC_CHANGELOG_SEND:
@@ -1081,7 +1045,7 @@ static int lmv_iocontrol(unsigned int cmd, struct obd_export *exp,
 				reqlen = offsetof(typeof(*hur),
 						  hur_user_item[nr])
 					 + hur->hur_request.hr_data_len;
-				OBD_ALLOC_LARGE(req, reqlen);
+				req = libcfs_kvzalloc(reqlen, GFP_NOFS);
 				if (req == NULL)
 					return -ENOMEM;
 
@@ -1091,7 +1055,7 @@ static int lmv_iocontrol(unsigned int cmd, struct obd_export *exp,
 						    reqlen, req, uarg);
 				if (rc1 != 0 && rc == 0)
 					rc = rc1;
-				OBD_FREE_LARGE(req, reqlen);
+				kvfree(req);
 			}
 		}
 		break;
@@ -1311,7 +1275,7 @@ int lmv_fid_alloc(struct obd_export *exp, struct lu_fid *fid,
 static int lmv_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 {
 	struct lmv_obd	     *lmv = &obd->u.lmv;
-	struct lprocfs_static_vars  lvars;
+	struct lprocfs_static_vars  lvars = { NULL };
 	struct lmv_desc	    *desc;
 	int			 rc;
 
@@ -1327,7 +1291,7 @@ static int lmv_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 		return -EINVAL;
 	}
 
-	OBD_ALLOC(lmv->tgts, sizeof(*lmv->tgts) * 32);
+	lmv->tgts = kcalloc(32, sizeof(*lmv->tgts), GFP_NOFS);
 	if (lmv->tgts == NULL)
 		return -ENOMEM;
 	lmv->tgts_size = 32;
@@ -1345,16 +1309,12 @@ static int lmv_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 
 	lprocfs_lmv_init_vars(&lvars);
 
-	lprocfs_obd_setup(obd, lvars.obd_vars);
-#if defined (CONFIG_PROC_FS)
-	{
-		rc = lprocfs_seq_create(obd->obd_proc_entry, "target_obd",
-					0444, &lmv_proc_target_fops, obd);
-		if (rc)
-			CWARN("%s: error adding LMV target_obd file: rc = %d\n",
-			       obd->obd_name, rc);
-       }
-#endif
+	lprocfs_obd_setup(obd, lvars.obd_vars, lvars.sysfs_vars);
+	rc = ldebugfs_seq_create(obd->obd_debugfs_entry, "target_obd",
+				 0444, &lmv_proc_target_fops, obd);
+	if (rc)
+		CWARN("%s: error adding LMV target_obd file: rc = %d\n",
+		      obd->obd_name, rc);
 	rc = fld_client_init(&lmv->lmv_fld, obd->obd_name,
 			     LUSTRE_CLI_FLD_HASH_DHT);
 	if (rc) {
@@ -1380,7 +1340,7 @@ static int lmv_cleanup(struct obd_device *obd)
 				continue;
 			lmv_del_target(lmv, i);
 		}
-		OBD_FREE(lmv->tgts, sizeof(*lmv->tgts) * lmv->tgts_size);
+		kfree(lmv->tgts);
 		lmv->tgts_size = 0;
 	}
 	return 0;
@@ -1437,7 +1397,7 @@ static int lmv_statfs(const struct lu_env *env, struct obd_export *exp,
 	if (rc)
 		return rc;
 
-	OBD_ALLOC(temp, sizeof(*temp));
+	temp = kzalloc(sizeof(*temp), GFP_NOFS);
 	if (temp == NULL)
 		return -ENOMEM;
 
@@ -1473,7 +1433,7 @@ static int lmv_statfs(const struct lu_env *env, struct obd_export *exp,
 	}
 
 out_free_temp:
-	OBD_FREE(temp, sizeof(*temp));
+	kfree(temp);
 	return rc;
 }
 
@@ -1769,7 +1729,7 @@ lmv_enqueue_remote(struct obd_export *exp, struct ldlm_enqueue_info *einfo,
 		goto out;
 	}
 
-	OBD_ALLOC_PTR(rdata);
+	rdata = kzalloc(sizeof(*rdata), GFP_NOFS);
 	if (rdata == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -1780,7 +1740,7 @@ lmv_enqueue_remote(struct obd_export *exp, struct ldlm_enqueue_info *einfo,
 
 	rc = md_enqueue(tgt->ltd_exp, einfo, it, rdata, lockh,
 			lmm, lmmsize, NULL, extra_lock_flags);
-	OBD_FREE_PTR(rdata);
+	kfree(rdata);
 out:
 	ldlm_lock_decref(&plock, pmode);
 	return rc;
@@ -2313,7 +2273,7 @@ static int lmv_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 		 * stack. */
 		break;
 	case OBD_CLEANUP_EXPORTS:
-		fld_client_proc_fini(&lmv->lmv_fld);
+		fld_client_debugfs_fini(&lmv->lmv_fld);
 		lprocfs_obd_cleanup(obd);
 		break;
 	default:
@@ -2440,13 +2400,13 @@ static int lmv_packmd(struct obd_export *exp, struct lov_mds_md **lmmp,
 		return mea_size;
 
 	if (*lmmp && !lsm) {
-		OBD_FREE_LARGE(*lmmp, mea_size);
+		kvfree(*lmmp);
 		*lmmp = NULL;
 		return 0;
 	}
 
 	if (*lmmp == NULL) {
-		OBD_ALLOC_LARGE(*lmmp, mea_size);
+		*lmmp = libcfs_kvzalloc(mea_size, GFP_NOFS);
 		if (*lmmp == NULL)
 			return -ENOMEM;
 	}
@@ -2489,14 +2449,14 @@ static int lmv_unpackmd(struct obd_export *exp, struct lov_stripe_md **lsmp,
 		return mea_size;
 
 	if (*lsmp != NULL && lmm == NULL) {
-		OBD_FREE_LARGE(*tmea, mea_size);
+		kvfree(*tmea);
 		*lsmp = NULL;
 		return 0;
 	}
 
 	LASSERT(mea_size == lmm_size);
 
-	OBD_ALLOC_LARGE(*tmea, mea_size);
+	*tmea = libcfs_kvzalloc(mea_size, GFP_NOFS);
 	if (*tmea == NULL)
 		return -ENOMEM;
 
@@ -2875,7 +2835,7 @@ static int __init lmv_init(void)
 	lprocfs_lmv_init_vars(&lvars);
 
 	rc = class_register_type(&lmv_obd_ops, &lmv_md_ops,
-				 lvars.module_vars, LUSTRE_LMV_NAME, NULL);
+				 LUSTRE_LMV_NAME, NULL);
 	return rc;
 }
 
diff --git a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
index 22e5c315faa4..311fc1b70c4d 100644
--- a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
+++ b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
@@ -42,17 +42,17 @@
 #include "../include/obd_class.h"
 #include "lmv_internal.h"
 
-static int lmv_numobd_seq_show(struct seq_file *m, void *v)
+static ssize_t numobd_show(struct kobject *kobj, struct attribute *attr,
+			   char *buf)
 {
-	struct obd_device       *dev = (struct obd_device *)m->private;
-	struct lmv_desc	 *desc;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	struct lmv_desc *desc;
 
-	LASSERT(dev != NULL);
 	desc = &dev->u.lmv.desc;
-	seq_printf(m, "%u\n", desc->ld_tgt_count);
-	return 0;
+	return sprintf(buf, "%u\n", desc->ld_tgt_count);
 }
-LPROC_SEQ_FOPS_RO(lmv_numobd);
+LUSTRE_RO_ATTR(numobd);
 
 static const char *placement_name[] = {
 	[PLACEMENT_CHAR_POLICY] = "CHAR",
@@ -77,66 +77,61 @@ static const char *placement_policy2name(enum placement_policy placement)
 	return placement_name[placement];
 }
 
-static int lmv_placement_seq_show(struct seq_file *m, void *v)
+static ssize_t placement_show(struct kobject *kobj, struct attribute *attr,
+			      char *buf)
 {
-	struct obd_device       *dev = (struct obd_device *)m->private;
-	struct lmv_obd	  *lmv;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	struct lmv_obd *lmv;
 
-	LASSERT(dev != NULL);
 	lmv = &dev->u.lmv;
-	seq_printf(m, "%s\n", placement_policy2name(lmv->lmv_placement));
-	return 0;
+	return sprintf(buf, "%s\n", placement_policy2name(lmv->lmv_placement));
 }
 
 #define MAX_POLICY_STRING_SIZE 64
 
-static ssize_t lmv_placement_seq_write(struct file *file,
-					const char __user *buffer,
-					size_t count, loff_t *off)
+static ssize_t placement_store(struct kobject *kobj, struct attribute *attr,
+			       const char *buffer,
+			       size_t count)
 {
-	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
-	char		     dummy[MAX_POLICY_STRING_SIZE + 1];
-	int		      len = count;
-	enum placement_policy       policy;
-	struct lmv_obd	  *lmv;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	char dummy[MAX_POLICY_STRING_SIZE + 1];
+	enum placement_policy policy;
+	struct lmv_obd *lmv = &dev->u.lmv;
 
-	if (copy_from_user(dummy, buffer, MAX_POLICY_STRING_SIZE))
-		return -EFAULT;
+	memcpy(dummy, buffer, MAX_POLICY_STRING_SIZE);
 
-	LASSERT(dev != NULL);
-	lmv = &dev->u.lmv;
+	if (count > MAX_POLICY_STRING_SIZE)
+		count = MAX_POLICY_STRING_SIZE;
 
-	if (len > MAX_POLICY_STRING_SIZE)
-		len = MAX_POLICY_STRING_SIZE;
+	if (dummy[count - 1] == '\n')
+		count--;
+	dummy[count] = '\0';
 
-	if (dummy[len - 1] == '\n')
-		len--;
-	dummy[len] = '\0';
-
-	policy = placement_name2policy(dummy, len);
+	policy = placement_name2policy(dummy, count);
 	if (policy != PLACEMENT_INVAL_POLICY) {
 		spin_lock(&lmv->lmv_lock);
 		lmv->lmv_placement = policy;
 		spin_unlock(&lmv->lmv_lock);
 	} else {
-		CERROR("Invalid placement policy \"%s\"!\n", dummy);
 		return -EINVAL;
 	}
 	return count;
 }
-LPROC_SEQ_FOPS(lmv_placement);
+LUSTRE_RW_ATTR(placement);
 
-static int lmv_activeobd_seq_show(struct seq_file *m, void *v)
+static ssize_t activeobd_show(struct kobject *kobj, struct attribute *attr,
+			      char *buf)
 {
-	struct obd_device       *dev = (struct obd_device *)m->private;
-	struct lmv_desc	 *desc;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	struct lmv_desc *desc;
 
-	LASSERT(dev != NULL);
 	desc = &dev->u.lmv.desc;
-	seq_printf(m, "%u\n", desc->ld_active_tgt_count);
-	return 0;
+	return sprintf(buf, "%u\n", desc->ld_active_tgt_count);
 }
-LPROC_SEQ_FOPS_RO(lmv_activeobd);
+LUSTRE_RO_ATTR(activeobd);
 
 static int lmv_desc_uuid_seq_show(struct seq_file *m, void *v)
 {
@@ -182,7 +177,7 @@ static int lmv_tgt_seq_show(struct seq_file *p, void *v)
 	return 0;
 }
 
-static struct seq_operations lmv_tgt_sops = {
+static const struct seq_operations lmv_tgt_sops = {
 	.start		 = lmv_tgt_seq_start,
 	.stop		  = lmv_tgt_seq_stop,
 	.next		  = lmv_tgt_seq_next,
@@ -199,29 +194,16 @@ static int lmv_target_seq_open(struct inode *inode, struct file *file)
 		return rc;
 
 	seq = file->private_data;
-	seq->private = PDE_DATA(inode);
+	seq->private = inode->i_private;
 
 	return 0;
 }
 
-LPROC_SEQ_FOPS_RO_TYPE(lmv, uuid);
-
 static struct lprocfs_vars lprocfs_lmv_obd_vars[] = {
-	{ "numobd",	  &lmv_numobd_fops,	  NULL, 0 },
-	{ "placement",	  &lmv_placement_fops,    NULL, 0 },
-	{ "activeobd",	  &lmv_activeobd_fops,    NULL, 0 },
-	{ "uuid",	  &lmv_uuid_fops,	  NULL, 0 },
 	{ "desc_uuid",	  &lmv_desc_uuid_fops,    NULL, 0 },
 	{ NULL }
 };
 
-LPROC_SEQ_FOPS_RO_TYPE(lmv, numrefs);
-
-static struct lprocfs_vars lprocfs_lmv_module_vars[] = {
-	{ "num_refs",	   &lmv_numrefs_fops, NULL, 0 },
-	{ NULL }
-};
-
 struct file_operations lmv_proc_target_fops = {
 	.owner		= THIS_MODULE,
 	.open		 = lmv_target_seq_open,
@@ -230,8 +212,19 @@ struct file_operations lmv_proc_target_fops = {
 	.release	      = seq_release,
 };
 
+static struct attribute *lmv_attrs[] = {
+	&lustre_attr_activeobd.attr,
+	&lustre_attr_numobd.attr,
+	&lustre_attr_placement.attr,
+	NULL,
+};
+
+static struct attribute_group lmv_attr_group = {
+	.attrs = lmv_attrs,
+};
+
 void lprocfs_lmv_init_vars(struct lprocfs_static_vars *lvars)
 {
-	lvars->module_vars    = lprocfs_lmv_module_vars;
+	lvars->sysfs_vars     = &lmv_attr_group;
 	lvars->obd_vars       = lprocfs_lmv_obd_vars;
 }
diff --git a/drivers/staging/lustre/lustre/lov/Makefile b/drivers/staging/lustre/lustre/lov/Makefile
index 6fe56a24b165..e4cc0db21014 100644
--- a/drivers/staging/lustre/lustre/lov/Makefile
+++ b/drivers/staging/lustre/lustre/lov/Makefile
@@ -2,5 +2,4 @@ obj-$(CONFIG_LUSTRE_FS) += lov.o
 lov-y := lov_obd.o lov_pack.o lov_offset.o lov_merge.o \
 	 lov_request.o lov_ea.o lov_dev.o lov_object.o lov_page.o  \
 	 lov_lock.o lov_io.o lovsub_dev.o lovsub_object.o lovsub_page.o      \
-	 lovsub_lock.o lovsub_io.o lov_pool.o
-lov-$(CONFIG_PROC_FS) += lproc_lov.o
+	 lovsub_lock.o lovsub_io.o lov_pool.o lproc_lov.o
diff --git a/drivers/staging/lustre/lustre/lov/lov_dev.c b/drivers/staging/lustre/lustre/lov/lov_dev.c
index 711b837ddba2..504b24a468fc 100644
--- a/drivers/staging/lustre/lustre/lov/lov_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lov_dev.c
@@ -285,10 +285,10 @@ static void lov_emerg_free(struct lov_device_emerg **emrg, int nr)
 			LASSERT(em->emrg_page_list.pl_nr == 0);
 			if (em->emrg_env != NULL)
 				cl_env_put(em->emrg_env, &em->emrg_refcheck);
-			OBD_FREE_PTR(em);
+			kfree(em);
 		}
 	}
-	OBD_FREE(emrg, nr * sizeof(emrg[0]));
+	kfree(emrg);
 }
 
 static struct lu_device *lov_device_free(const struct lu_env *env,
@@ -298,11 +298,10 @@ static struct lu_device *lov_device_free(const struct lu_env *env,
 	const int	  nr = ld->ld_target_nr;
 
 	cl_device_fini(lu2cl_dev(d));
-	if (ld->ld_target != NULL)
-		OBD_FREE(ld->ld_target, nr * sizeof(ld->ld_target[0]));
+	kfree(ld->ld_target);
 	if (ld->ld_emrg != NULL)
 		lov_emerg_free(ld->ld_emrg, nr);
-	OBD_FREE_PTR(ld);
+	kfree(ld);
 	return NULL;
 }
 
@@ -323,13 +322,13 @@ static struct lov_device_emerg **lov_emerg_alloc(int nr)
 	int i;
 	int result;
 
-	OBD_ALLOC(emerg, nr * sizeof(emerg[0]));
+	emerg = kcalloc(nr, sizeof(emerg[0]), GFP_NOFS);
 	if (emerg == NULL)
 		return ERR_PTR(-ENOMEM);
 	for (result = i = 0; i < nr && result == 0; i++) {
 		struct lov_device_emerg *em;
 
-		OBD_ALLOC_PTR(em);
+		em = kzalloc(sizeof(*em), GFP_NOFS);
 		if (em != NULL) {
 			emerg[i] = em;
 			cl_page_list_init(&em->emrg_page_list);
@@ -369,12 +368,12 @@ static int lov_expand_targets(const struct lu_env *env, struct lov_device *dev)
 		if (IS_ERR(emerg))
 			return PTR_ERR(emerg);
 
-		OBD_ALLOC(newd, tgt_size * sz);
+		newd = kcalloc(tgt_size, sz, GFP_NOFS);
 		if (newd != NULL) {
 			mutex_lock(&dev->ld_mutex);
 			if (sub_size > 0) {
 				memcpy(newd, dev->ld_target, sub_size * sz);
-				OBD_FREE(dev->ld_target, sub_size * sz);
+				kfree(dev->ld_target);
 			}
 			dev->ld_target    = newd;
 			dev->ld_target_nr = tgt_size;
@@ -478,7 +477,7 @@ static struct lu_device *lov_device_alloc(const struct lu_env *env,
 	struct obd_device *obd;
 	int rc;
 
-	OBD_ALLOC_PTR(ld);
+	ld = kzalloc(sizeof(*ld), GFP_NOFS);
 	if (ld == NULL)
 		return ERR_PTR(-ENOMEM);
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_ea.c b/drivers/staging/lustre/lustre/lov/lov_ea.c
index 2bcfaeaff6fa..3f51b573e1fb 100644
--- a/drivers/staging/lustre/lustre/lov/lov_ea.c
+++ b/drivers/staging/lustre/lustre/lov/lov_ea.c
@@ -95,7 +95,7 @@ struct lov_stripe_md *lsm_alloc_plain(__u16 stripe_count, int *size)
 	oinfo_ptrs_size = sizeof(struct lov_oinfo *) * stripe_count;
 	*size = sizeof(struct lov_stripe_md) + oinfo_ptrs_size;
 
-	OBD_ALLOC_LARGE(lsm, *size);
+	lsm = libcfs_kvzalloc(*size, GFP_NOFS);
 	if (!lsm)
 		return NULL;
 
@@ -111,7 +111,7 @@ struct lov_stripe_md *lsm_alloc_plain(__u16 stripe_count, int *size)
 err:
 	while (--i >= 0)
 		OBD_SLAB_FREE(lsm->lsm_oinfo[i], lov_oinfo_slab, sizeof(*loi));
-	OBD_FREE_LARGE(lsm, *size);
+	kvfree(lsm);
 	return NULL;
 }
 
@@ -123,8 +123,7 @@ void lsm_free_plain(struct lov_stripe_md *lsm)
 	for (i = 0; i < stripe_count; i++)
 		OBD_SLAB_FREE(lsm->lsm_oinfo[i], lov_oinfo_slab,
 			      sizeof(struct lov_oinfo));
-	OBD_FREE_LARGE(lsm, sizeof(struct lov_stripe_md) +
-		       stripe_count * sizeof(struct lov_oinfo *));
+	kvfree(lsm);
 }
 
 static void lsm_unpackmd_common(struct lov_stripe_md *lsm,
diff --git a/drivers/staging/lustre/lustre/lov/lov_internal.h b/drivers/staging/lustre/lustre/lov/lov_internal.h
index b644acc9b034..dde9656d4dd6 100644
--- a/drivers/staging/lustre/lustre/lov/lov_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_internal.h
@@ -71,13 +71,6 @@
 })
 #endif
 
-struct lov_lock_handles {
-	struct portals_handle   llh_handle;
-	atomic_t	    llh_refcount;
-	int		     llh_stripe_count;
-	struct lustre_handle    llh_handles[0];
-};
-
 struct lov_request {
 	struct obd_info	  rq_oi;
 	struct lov_request_set  *rq_rqset;
@@ -88,7 +81,6 @@ struct lov_request {
 	int		      rq_stripe;     /* stripe number */
 	int		      rq_complete;
 	int		      rq_rc;
-	int		      rq_buflen;     /* length of sub_md */
 
 	u32		      rq_oabufs;
 	u32		      rq_pgaidx;
@@ -109,9 +101,6 @@ struct lov_request_set {
 	struct llog_cookie		*set_cookies;
 	int				set_cookie_sent;
 	struct obd_trans_info		*set_oti;
-	u32				set_oabufs;
-	struct brw_page			*set_pga;
-	struct lov_lock_handles		*set_lockh;
 	struct list_head			set_list;
 	wait_queue_head_t			set_waitq;
 	spinlock_t			set_lock;
@@ -136,32 +125,6 @@ static inline void lov_put_reqset(struct lov_request_set *set)
 		lov_finish_set(set);
 }
 
-static inline struct lov_lock_handles *
-lov_handle2llh(struct lustre_handle *handle)
-{
-	LASSERT(handle != NULL);
-	return class_handle2object(handle->cookie);
-}
-
-static inline void lov_llh_put(struct lov_lock_handles *llh)
-{
-	CDEBUG(D_INFO, "PUTting llh %p : new refcount %d\n", llh,
-	       atomic_read(&llh->llh_refcount) - 1);
-	LASSERT(atomic_read(&llh->llh_refcount) > 0 &&
-		atomic_read(&llh->llh_refcount) < 0x5a5a);
-	if (atomic_dec_and_test(&llh->llh_refcount)) {
-		class_handle_unhash(&llh->llh_handle);
-		/* The structure may be held by other threads because RCU.
-		 *   -jxiong */
-		if (atomic_read(&llh->llh_refcount))
-			return;
-
-		OBD_FREE_RCU(llh, sizeof(*llh) +
-			     sizeof(*llh->llh_handles) * llh->llh_stripe_count,
-			     &llh->llh_handle);
-	}
-}
-
 #define lov_uuid2str(lv, index) \
 	(char *)((lv)->lov_tgts[index]->ltd_uuid.uuid)
 
@@ -265,15 +228,8 @@ void lsm_free_plain(struct lov_stripe_md *lsm);
 void dump_lsm(unsigned int level, const struct lov_stripe_md *lsm);
 
 /* lproc_lov.c */
-#if defined (CONFIG_PROC_FS)
 extern const struct file_operations lov_proc_target_fops;
 void lprocfs_lov_init_vars(struct lprocfs_static_vars *lvars);
-#else
-static inline void lprocfs_lov_init_vars(struct lprocfs_static_vars *lvars)
-{
-	memset(lvars, 0, sizeof(*lvars));
-}
-#endif
 
 /* lov_cl.c */
 extern struct lu_device_type lov_device_type;
diff --git a/drivers/staging/lustre/lustre/lov/lov_io.c b/drivers/staging/lustre/lustre/lov/lov_io.c
index cf96e0d01e22..11c1081b1d3d 100644
--- a/drivers/staging/lustre/lustre/lov/lov_io.c
+++ b/drivers/staging/lustre/lustre/lov/lov_io.c
@@ -70,7 +70,7 @@ static void lov_io_sub_fini(const struct lu_env *env, struct lov_io *lio,
 		if (sub->sub_stripe == lio->lis_single_subio_index)
 			lio->lis_single_subio_index = -1;
 		else if (!sub->sub_borrowed)
-			OBD_FREE_PTR(sub->sub_io);
+			kfree(sub->sub_io);
 		sub->sub_io = NULL;
 	}
 	if (sub->sub_env != NULL && !IS_ERR(sub->sub_env)) {
@@ -179,7 +179,8 @@ static int lov_io_sub_init(const struct lu_env *env, struct lov_io *lio,
 				sub->sub_io = &lio->lis_single_subio;
 				lio->lis_single_subio_index = stripe;
 			} else {
-				OBD_ALLOC_PTR(sub->sub_io);
+				sub->sub_io = kzalloc(sizeof(*sub->sub_io),
+						      GFP_NOFS);
 				if (sub->sub_io == NULL)
 					result = -ENOMEM;
 			}
@@ -285,8 +286,10 @@ static int lov_io_subio_init(const struct lu_env *env, struct lov_io *lio,
 	 * Need to be optimized, we can't afford to allocate a piece of memory
 	 * when writing a page. -jay
 	 */
-	OBD_ALLOC_LARGE(lio->lis_subs,
-			lsm->lsm_stripe_count * sizeof(lio->lis_subs[0]));
+	lio->lis_subs =
+		libcfs_kvzalloc(lsm->lsm_stripe_count *
+				sizeof(lio->lis_subs[0]),
+				GFP_NOFS);
 	if (lio->lis_subs != NULL) {
 		lio->lis_nr_subios = lio->lis_stripe_count;
 		lio->lis_single_subio_index = -1;
@@ -359,8 +362,7 @@ static void lov_io_fini(const struct lu_env *env, const struct cl_io_slice *ios)
 	if (lio->lis_subs != NULL) {
 		for (i = 0; i < lio->lis_nr_subios; i++)
 			lov_io_sub_fini(env, lio, &lio->lis_subs[i]);
-		OBD_FREE_LARGE(lio->lis_subs,
-			 lio->lis_nr_subios * sizeof(lio->lis_subs[0]));
+		kvfree(lio->lis_subs);
 		lio->lis_nr_subios = 0;
 	}
 
@@ -603,8 +605,10 @@ static int lov_io_submit(const struct lu_env *env,
 
 	LASSERT(lio->lis_subs != NULL);
 	if (alloc) {
-		OBD_ALLOC_LARGE(stripes_qin,
-				sizeof(*stripes_qin) * lio->lis_nr_subios);
+		stripes_qin =
+			libcfs_kvzalloc(sizeof(*stripes_qin) *
+					lio->lis_nr_subios,
+					GFP_NOFS);
 		if (stripes_qin == NULL)
 			return -ENOMEM;
 
@@ -657,8 +661,7 @@ static int lov_io_submit(const struct lu_env *env,
 	}
 
 	if (alloc) {
-		OBD_FREE_LARGE(stripes_qin,
-			 sizeof(*stripes_qin) * lio->lis_nr_subios);
+		kvfree(stripes_qin);
 	} else {
 		int i;
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_lock.c b/drivers/staging/lustre/lustre/lov/lov_lock.c
index f2eca565bdda..a6938085ff24 100644
--- a/drivers/staging/lustre/lustre/lov/lov_lock.c
+++ b/drivers/staging/lustre/lustre/lov/lov_lock.c
@@ -314,7 +314,7 @@ static int lov_lock_sub_init(const struct lu_env *env,
 			nr++;
 	}
 	LASSERT(nr > 0);
-	OBD_ALLOC_LARGE(lck->lls_sub, nr * sizeof(lck->lls_sub[0]));
+	lck->lls_sub = libcfs_kvzalloc(nr * sizeof(lck->lls_sub[0]), GFP_NOFS);
 	if (lck->lls_sub == NULL)
 		return -ENOMEM;
 
@@ -441,8 +441,7 @@ static void lov_lock_fini(const struct lu_env *env,
 			 * a reference on its parent.
 			 */
 			LASSERT(lck->lls_sub[i].sub_lock == NULL);
-		OBD_FREE_LARGE(lck->lls_sub,
-			       lck->lls_nr * sizeof(lck->lls_sub[0]));
+		kvfree(lck->lls_sub);
 	}
 	OBD_SLAB_FREE_PTR(lck, lov_lock_kmem);
 }
diff --git a/drivers/staging/lustre/lustre/lov/lov_obd.c b/drivers/staging/lustre/lustre/lov/lov_obd.c
index 02781576637e..96c55acd52ae 100644
--- a/drivers/staging/lustre/lustre/lov/lov_obd.c
+++ b/drivers/staging/lustre/lustre/lov/lov_obd.c
@@ -127,7 +127,6 @@ int lov_connect_obd(struct obd_device *obd, __u32 index, int activate,
 	struct obd_device *tgt_obd;
 	static struct obd_uuid lov_osc_uuid = { "LOV_OSC_UUID" };
 	struct obd_import *imp;
-	struct proc_dir_entry *lov_proc_dir;
 	int rc;
 
 	if (!lov->lov_tgts[index])
@@ -186,28 +185,10 @@ int lov_connect_obd(struct obd_device *obd, __u32 index, int activate,
 	CDEBUG(D_CONFIG, "Connected tgt idx %d %s (%s) %sactive\n", index,
 	       obd_uuid2str(tgt_uuid), tgt_obd->obd_name, activate ? "":"in");
 
-	lov_proc_dir = obd->obd_proc_private;
-	if (lov_proc_dir) {
-		struct obd_device *osc_obd = lov->lov_tgts[index]->ltd_exp->exp_obd;
-		struct proc_dir_entry *osc_symlink;
-
-		LASSERT(osc_obd != NULL);
-		LASSERT(osc_obd->obd_magic == OBD_DEVICE_MAGIC);
-		LASSERT(osc_obd->obd_type->typ_name != NULL);
-
-		osc_symlink = lprocfs_add_symlink(osc_obd->obd_name,
-						  lov_proc_dir,
-						  "../../../%s/%s",
-						  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);
-			lprocfs_remove(&lov_proc_dir);
-			obd->obd_proc_private = NULL;
-		}
-	}
+	if (lov->lov_tgts_kobj)
+		/* Even if we failed, that's ok */
+		rc = sysfs_create_link(lov->lov_tgts_kobj, &tgt_obd->obd_kobj,
+				       tgt_obd->obd_name);
 
 	return 0;
 }
@@ -239,6 +220,10 @@ static int lov_connect(const struct lu_env *env,
 		lov->lov_ocd = *data;
 
 	obd_getref(obd);
+
+	lov->lov_tgts_kobj = kobject_create_and_add("target_obds",
+						    &obd->obd_kobj);
+
 	for (i = 0; i < lov->desc.ld_tgt_count; i++) {
 		tgt = lov->lov_tgts[i];
 		if (!tgt || obd_uuid_empty(&tgt->ltd_uuid))
@@ -268,7 +253,6 @@ static int lov_connect(const struct lu_env *env,
 
 static int lov_disconnect_obd(struct obd_device *obd, struct lov_tgt_desc *tgt)
 {
-	struct proc_dir_entry *lov_proc_dir;
 	struct lov_obd *lov = &obd->u.lov;
 	struct obd_device *osc_obd;
 	int rc;
@@ -284,10 +268,10 @@ static int lov_disconnect_obd(struct obd_device *obd, struct lov_tgt_desc *tgt)
 	}
 
 	if (osc_obd) {
-		lov_proc_dir = obd->obd_proc_private;
-		if (lov_proc_dir) {
-			lprocfs_remove_proc_entry(osc_obd->obd_name, lov_proc_dir);
-		}
+		if (lov->lov_tgts_kobj)
+			sysfs_remove_link(lov->lov_tgts_kobj,
+					  osc_obd->obd_name);
+
 		/* Pass it on to our clients.
 		 * XXX This should be an argument to disconnect,
 		 * XXX not a back-door flag on the OBD.  Ah well.
@@ -337,6 +321,10 @@ static int lov_disconnect(struct obd_export *exp)
 			lov_del_target(obd, i, NULL, lov->lov_tgts[i]->ltd_gen);
 		}
 	}
+
+	if (lov->lov_tgts_kobj)
+		kobject_put(lov->lov_tgts_kobj);
+
 	obd_putref(obd);
 
 out:
@@ -554,7 +542,7 @@ static int lov_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 		newsize = max_t(__u32, lov->lov_tgt_size, 2);
 		while (newsize < index + 1)
 			newsize <<= 1;
-		OBD_ALLOC(newtgts, sizeof(*newtgts) * newsize);
+		newtgts = kcalloc(newsize, sizeof(*newtgts), GFP_NOFS);
 		if (newtgts == NULL) {
 			mutex_unlock(&lov->lov_lock);
 			return -ENOMEM;
@@ -570,14 +558,13 @@ static int lov_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 		lov->lov_tgts = newtgts;
 		lov->lov_tgt_size = newsize;
 		smp_rmb();
-		if (old)
-			OBD_FREE(old, sizeof(*old) * oldsize);
+		kfree(old);
 
 		CDEBUG(D_CONFIG, "tgts: %p size: %d\n",
 		       lov->lov_tgts, lov->lov_tgt_size);
 	}
 
-	OBD_ALLOC_PTR(tgt);
+	tgt = kzalloc(sizeof(*tgt), GFP_NOFS);
 	if (!tgt) {
 		mutex_unlock(&lov->lov_lock);
 		return -ENOMEM;
@@ -586,7 +573,7 @@ static int lov_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 	rc = lov_ost_pool_add(&lov->lov_packed, index, lov->lov_tgt_size);
 	if (rc) {
 		mutex_unlock(&lov->lov_lock);
-		OBD_FREE_PTR(tgt);
+		kfree(tgt);
 		return rc;
 	}
 
@@ -712,7 +699,7 @@ static void __lov_del_obd(struct obd_device *obd, struct lov_tgt_desc *tgt)
 	if (tgt->ltd_exp)
 		lov_disconnect_obd(obd, tgt);
 
-	OBD_FREE_PTR(tgt);
+	kfree(tgt);
 
 	/* Manual cleanup - no cleanup logs to clean up the osc's.  We must
 	   do it ourselves. And we can't do it from lov_cleanup,
@@ -822,21 +809,16 @@ int lov_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 		goto out;
 
 	lprocfs_lov_init_vars(&lvars);
-	lprocfs_obd_setup(obd, lvars.obd_vars);
-#if defined (CONFIG_PROC_FS)
-	{
-		int rc1;
+	lprocfs_obd_setup(obd, lvars.obd_vars, lvars.sysfs_vars);
 
-		rc1 = lprocfs_seq_create(obd->obd_proc_entry, "target_obd",
-					0444, &lov_proc_target_fops, obd);
-		if (rc1)
-			CWARN("Error adding the target_obd file\n");
-	}
-#endif
-	lov->lov_pool_proc_entry = lprocfs_register("pools",
-						    obd->obd_proc_entry,
-						    NULL, NULL);
+	rc = ldebugfs_seq_create(obd->obd_debugfs_entry, "target_obd",
+				 0444, &lov_proc_target_fops, obd);
+	if (rc)
+		CWARN("Error adding the target_obd file\n");
 
+	lov->lov_pool_debugfs_entry = ldebugfs_register("pools",
+						     obd->obd_debugfs_entry,
+						     NULL, NULL);
 	return 0;
 
 out:
@@ -903,8 +885,7 @@ static int lov_cleanup(struct obd_device *obd)
 			lov_del_target(obd, i, NULL, 0);
 		}
 		obd_putref(obd);
-		OBD_FREE(lov->lov_tgts, sizeof(*lov->lov_tgts) *
-			 lov->lov_tgt_size);
+		kfree(lov->lov_tgts);
 		lov->lov_tgt_size = 0;
 	}
 	return 0;
@@ -994,7 +975,7 @@ static int lov_recreate(struct obd_export *exp, struct obdo *src_oa,
 	LASSERT(src_oa->o_valid & OBD_MD_FLFLAGS &&
 		src_oa->o_flags & OBD_FL_RECREATE_OBJS);
 
-	OBD_ALLOC(obj_mdp, sizeof(*obj_mdp));
+	obj_mdp = kzalloc(sizeof(*obj_mdp), GFP_NOFS);
 	if (obj_mdp == NULL)
 		return -ENOMEM;
 
@@ -1032,7 +1013,7 @@ static int lov_recreate(struct obd_export *exp, struct obdo *src_oa,
 	rc = obd_create(NULL, lov->lov_tgts[ost_idx]->ltd_exp,
 			src_oa, &obj_mdp, oti);
 out:
-	OBD_FREE(obj_mdp, sizeof(*obj_mdp));
+	kfree(obj_mdp);
 	return rc;
 }
 
@@ -1419,7 +1400,7 @@ static int lov_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		__u32 flags;
 
 		memcpy(&index, data->ioc_inlbuf2, sizeof(__u32));
-		if ((index >= count))
+		if (index >= count)
 			return -ENODEV;
 
 		if (!lov->lov_tgts[index])
@@ -1532,7 +1513,7 @@ static int lov_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 			return -EAGAIN;
 
 		LASSERT(tgt && tgt->ltd_exp);
-		OBD_ALLOC_PTR(oqctl);
+		oqctl = kzalloc(sizeof(*oqctl), GFP_NOFS);
 		if (!oqctl)
 			return -ENOMEM;
 
@@ -1543,7 +1524,7 @@ static int lov_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 			qctl->qc_valid = QC_OSTIDX;
 			qctl->obd_uuid = tgt->ltd_uuid;
 		}
-		OBD_FREE_PTR(oqctl);
+		kfree(oqctl);
 		break;
 	}
 	default: {
@@ -1756,7 +1737,7 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 	if (fiemap_count_to_size(fm_key->fiemap.fm_extent_count) < buffer_size)
 		buffer_size = fiemap_count_to_size(fm_key->fiemap.fm_extent_count);
 
-	OBD_ALLOC_LARGE(fm_local, buffer_size);
+	fm_local = libcfs_kvzalloc(buffer_size, GFP_NOFS);
 	if (fm_local == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -1962,7 +1943,7 @@ skip_last_device_calc:
 	fiemap->fm_mapped_extents = current_extent;
 
 out:
-	OBD_FREE_LARGE(fm_local, buffer_size);
+	kvfree(fm_local);
 	return rc;
 }
 
@@ -2367,7 +2348,7 @@ static int __init lov_init(void)
 	}
 	lprocfs_lov_init_vars(&lvars);
 
-	rc = class_register_type(&lov_obd_ops, NULL, lvars.module_vars,
+	rc = class_register_type(&lov_obd_ops, NULL,
 				 LUSTRE_LOV_NAME, &lov_device_type);
 
 	if (rc) {
diff --git a/drivers/staging/lustre/lustre/lov/lov_object.c b/drivers/staging/lustre/lustre/lov/lov_object.c
index a22342fa792d..4d7cd924a27e 100644
--- a/drivers/staging/lustre/lustre/lov/lov_object.c
+++ b/drivers/staging/lustre/lustre/lov/lov_object.c
@@ -218,7 +218,8 @@ static int lov_init_raid0(const struct lu_env *env,
 	r0->lo_nr  = lsm->lsm_stripe_count;
 	LASSERT(r0->lo_nr <= lov_targets_nr(dev));
 
-	OBD_ALLOC_LARGE(r0->lo_sub, r0->lo_nr * sizeof(r0->lo_sub[0]));
+	r0->lo_sub = libcfs_kvzalloc(r0->lo_nr * sizeof(r0->lo_sub[0]),
+				     GFP_NOFS);
 	if (r0->lo_sub != NULL) {
 		result = 0;
 		subconf->coc_inode = conf->coc_inode;
@@ -375,7 +376,7 @@ static void lov_fini_raid0(const struct lu_env *env, struct lov_object *lov,
 	struct lov_layout_raid0 *r0 = &state->raid0;
 
 	if (r0->lo_sub != NULL) {
-		OBD_FREE_LARGE(r0->lo_sub, r0->lo_nr * sizeof(r0->lo_sub[0]));
+		kvfree(r0->lo_sub);
 		r0->lo_sub = NULL;
 	}
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_pack.c b/drivers/staging/lustre/lustre/lov/lov_pack.c
index 5356d5324176..6b1c135c9ab0 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pack.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pack.c
@@ -192,13 +192,13 @@ int lov_packmd(struct obd_export *exp, struct lov_mds_md **lmmp,
 	if (*lmmp && !lsm) {
 		stripe_count = le16_to_cpu((*lmmp)->lmm_stripe_count);
 		lmm_size = lov_mds_md_size(stripe_count, lmm_magic);
-		OBD_FREE_LARGE(*lmmp, lmm_size);
+		kvfree(*lmmp);
 		*lmmp = NULL;
 		return 0;
 	}
 
 	if (!*lmmp) {
-		OBD_ALLOC_LARGE(*lmmp, lmm_size);
+		*lmmp = libcfs_kvzalloc(lmm_size, GFP_NOFS);
 		if (!*lmmp)
 			return -ENOMEM;
 	}
@@ -285,7 +285,7 @@ static int lov_verify_lmm(void *lmm, int lmm_bytes, __u16 *stripe_count)
 		CERROR("bad disk LOV MAGIC: 0x%08X; dumping LMM (size=%d):\n",
 		       le32_to_cpu(*(__u32 *)lmm), lmm_bytes);
 		sz = lmm_bytes * 2 + 1;
-		OBD_ALLOC_LARGE(buffer, sz);
+		buffer = libcfs_kvzalloc(sz, GFP_NOFS);
 		if (buffer != NULL) {
 			int i;
 
@@ -293,7 +293,7 @@ static int lov_verify_lmm(void *lmm, int lmm_bytes, __u16 *stripe_count)
 				sprintf(buffer+2*i, "%.2X", ((char *)lmm)[i]);
 			buffer[sz - 1] = '\0';
 			CERROR("%s\n", buffer);
-			OBD_FREE_LARGE(buffer, sz);
+			kvfree(buffer);
 		}
 		return -EINVAL;
 	}
@@ -367,9 +367,11 @@ int lov_unpackmd(struct obd_export *exp,  struct lov_stripe_md **lsmp,
 		if (rc)
 			return rc;
 		magic = le32_to_cpu(lmm->lmm_magic);
+		pattern = le32_to_cpu(lmm->lmm_pattern);
 	} else {
 		magic = LOV_MAGIC;
 		stripe_count = lov_get_stripecnt(lov, magic, 0);
+		pattern = LOV_PATTERN_RAID0;
 	}
 
 	/* If we aren't passed an lsmp struct, we just want the size */
@@ -384,7 +386,6 @@ int lov_unpackmd(struct obd_export *exp,  struct lov_stripe_md **lsmp,
 		return 0;
 	}
 
-	pattern = le32_to_cpu(lmm->lmm_pattern);
 	lsm_size = lov_alloc_memmd(lsmp, stripe_count, pattern, magic);
 	if (lsm_size < 0)
 		return lsm_size;
diff --git a/drivers/staging/lustre/lustre/lov/lov_pool.c b/drivers/staging/lustre/lustre/lov/lov_pool.c
index d96163de773f..1e4d3fbee323 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pool.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pool.c
@@ -64,10 +64,10 @@ void lov_pool_putref(struct pool_desc *pool)
 	if (atomic_dec_and_test(&pool->pool_refcount)) {
 		LASSERT(hlist_unhashed(&pool->pool_hash));
 		LASSERT(list_empty(&pool->pool_list));
-		LASSERT(pool->pool_proc_entry == NULL);
+		LASSERT(pool->pool_debugfs_entry == NULL);
 		lov_ost_pool_free(&(pool->pool_rr.lqr_pool));
 		lov_ost_pool_free(&(pool->pool_obds));
-		OBD_FREE_PTR(pool);
+		kfree(pool);
 	}
 }
 
@@ -152,7 +152,6 @@ cfs_hash_ops_t pool_hash_operations = {
 
 };
 
-#if defined (CONFIG_PROC_FS)
 /* ifdef needed for liblustre support */
 /*
  * pool /proc seq_file methods
@@ -210,7 +209,7 @@ static void *pool_proc_start(struct seq_file *s, loff_t *pos)
 		return NULL;
 	}
 
-	OBD_ALLOC_PTR(iter);
+	iter = kzalloc(sizeof(*iter), GFP_NOFS);
 	if (!iter)
 		return ERR_PTR(-ENOMEM);
 	iter->magic = POOL_IT_MAGIC;
@@ -246,7 +245,7 @@ static void pool_proc_stop(struct seq_file *s, void *v)
 		 * will work */
 		s->private = iter->pool;
 		lov_pool_putref(iter->pool);
-		OBD_FREE_PTR(iter);
+		kfree(iter);
 	}
 	return;
 }
@@ -269,7 +268,7 @@ static int pool_proc_show(struct seq_file *s, void *v)
 	return 0;
 }
 
-static struct seq_operations pool_proc_ops = {
+static const struct seq_operations pool_proc_ops = {
 	.start	  = pool_proc_start,
 	.next	   = pool_proc_next,
 	.stop	   = pool_proc_stop,
@@ -283,7 +282,7 @@ static int pool_proc_open(struct inode *inode, struct file *file)
 	rc = seq_open(file, &pool_proc_ops);
 	if (!rc) {
 		struct seq_file *s = file->private_data;
-		s->private = PDE_DATA(inode);
+		s->private = inode->i_private;
 	}
 	return rc;
 }
@@ -294,7 +293,6 @@ static struct file_operations pool_proc_operations = {
 	.llseek	 = seq_lseek,
 	.release	= seq_release,
 };
-#endif /* CONFIG_PROC_FS */
 
 void lov_dump_pool(int level, struct pool_desc *pool)
 {
@@ -327,7 +325,7 @@ int lov_ost_pool_init(struct ost_pool *op, unsigned int count)
 	op->op_count = 0;
 	init_rwsem(&op->op_rw_sem);
 	op->op_size = count;
-	OBD_ALLOC(op->op_array, op->op_size * sizeof(op->op_array[0]));
+	op->op_array = kcalloc(op->op_size, sizeof(op->op_array[0]), GFP_NOFS);
 	if (op->op_array == NULL) {
 		op->op_size = 0;
 		return -ENOMEM;
@@ -347,13 +345,13 @@ int lov_ost_pool_extend(struct ost_pool *op, unsigned int min_count)
 		return 0;
 
 	new_size = max(min_count, 2 * op->op_size);
-	OBD_ALLOC(new, new_size * sizeof(op->op_array[0]));
+	new = kcalloc(new_size, sizeof(op->op_array[0]), GFP_NOFS);
 	if (new == NULL)
 		return -ENOMEM;
 
 	/* copy old array to new one */
 	memcpy(new, op->op_array, op->op_size * sizeof(op->op_array[0]));
-	OBD_FREE(op->op_array, op->op_size * sizeof(op->op_array[0]));
+	kfree(op->op_array);
 	op->op_array = new;
 	op->op_size = new_size;
 	return 0;
@@ -411,7 +409,7 @@ int lov_ost_pool_free(struct ost_pool *op)
 
 	down_write(&op->op_rw_sem);
 
-	OBD_FREE(op->op_array, op->op_size * sizeof(op->op_array[0]));
+	kfree(op->op_array);
 	op->op_array = NULL;
 	op->op_count = 0;
 	op->op_size = 0;
@@ -432,7 +430,7 @@ int lov_pool_new(struct obd_device *obd, char *poolname)
 	if (strlen(poolname) > LOV_MAXPOOLNAME)
 		return -ENAMETOOLONG;
 
-	OBD_ALLOC_PTR(new_pool);
+	new_pool = kzalloc(sizeof(*new_pool), GFP_NOFS);
 	if (new_pool == NULL)
 		return -ENOMEM;
 
@@ -454,20 +452,21 @@ int lov_pool_new(struct obd_device *obd, char *poolname)
 
 	INIT_HLIST_NODE(&new_pool->pool_hash);
 
-#if defined (CONFIG_PROC_FS)
 	/* we need this assert seq_file is not implemented for liblustre */
 	/* get ref for /proc file */
 	lov_pool_getref(new_pool);
-	new_pool->pool_proc_entry = lprocfs_add_simple(lov->lov_pool_proc_entry,
-						       poolname, new_pool,
-						       &pool_proc_operations);
-	if (IS_ERR(new_pool->pool_proc_entry)) {
-		CWARN("Cannot add proc pool entry "LOV_POOLNAMEF"\n", poolname);
-		new_pool->pool_proc_entry = NULL;
+	new_pool->pool_debugfs_entry = ldebugfs_add_simple(
+						lov->lov_pool_debugfs_entry,
+						poolname, new_pool,
+						&pool_proc_operations);
+	if (IS_ERR_OR_NULL(new_pool->pool_debugfs_entry)) {
+		CWARN("Cannot add debugfs pool entry "LOV_POOLNAMEF"\n",
+		      poolname);
+		new_pool->pool_debugfs_entry = NULL;
 		lov_pool_putref(new_pool);
 	}
-	CDEBUG(D_INFO, "pool %p - proc %p\n", new_pool, new_pool->pool_proc_entry);
-#endif
+	CDEBUG(D_INFO, "pool %p - proc %p\n",
+		new_pool, new_pool->pool_debugfs_entry);
 
 	spin_lock(&obd->obd_dev_lock);
 	list_add_tail(&new_pool->pool_list, &lov->lov_pool_list);
@@ -493,12 +492,12 @@ out_err:
 	lov->lov_pool_count--;
 	spin_unlock(&obd->obd_dev_lock);
 
-	lprocfs_remove(&new_pool->pool_proc_entry);
+	ldebugfs_remove(&new_pool->pool_debugfs_entry);
 
 	lov_ost_pool_free(&new_pool->pool_rr.lqr_pool);
 out_free_pool_obds:
 	lov_ost_pool_free(&new_pool->pool_obds);
-	OBD_FREE_PTR(new_pool);
+	kfree(new_pool);
 	return rc;
 }
 
@@ -514,9 +513,9 @@ int lov_pool_del(struct obd_device *obd, char *poolname)
 	if (pool == NULL)
 		return -ENOENT;
 
-	if (pool->pool_proc_entry != NULL) {
-		CDEBUG(D_INFO, "proc entry %p\n", pool->pool_proc_entry);
-		lprocfs_remove(&pool->pool_proc_entry);
+	if (!IS_ERR_OR_NULL(pool->pool_debugfs_entry)) {
+		CDEBUG(D_INFO, "proc entry %p\n", pool->pool_debugfs_entry);
+		ldebugfs_remove(&pool->pool_debugfs_entry);
 		lov_pool_putref(pool);
 	}
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_request.c b/drivers/staging/lustre/lustre/lov/lov_request.c
index 933e2d1f8127..f4de8b84c5c2 100644
--- a/drivers/staging/lustre/lustre/lov/lov_request.c
+++ b/drivers/staging/lustre/lustre/lov/lov_request.c
@@ -68,22 +68,10 @@ void lov_finish_set(struct lov_request_set *set)
 
 		if (req->rq_oi.oi_oa)
 			OBDO_FREE(req->rq_oi.oi_oa);
-		if (req->rq_oi.oi_md)
-			OBD_FREE_LARGE(req->rq_oi.oi_md, req->rq_buflen);
-		if (req->rq_oi.oi_osfs)
-			OBD_FREE(req->rq_oi.oi_osfs,
-				 sizeof(*req->rq_oi.oi_osfs));
-		OBD_FREE(req, sizeof(*req));
+		kfree(req->rq_oi.oi_osfs);
+		kfree(req);
 	}
-
-	if (set->set_pga) {
-		int len = set->set_oabufs * sizeof(*set->set_pga);
-		OBD_FREE_LARGE(set->set_pga, len);
-	}
-	if (set->set_lockh)
-		lov_llh_put(set->set_lockh);
-
-	OBD_FREE(set, sizeof(*set));
+	kfree(set);
 }
 
 int lov_set_finished(struct lov_request_set *set, int idempotent)
@@ -286,7 +274,7 @@ int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 	struct lov_obd *lov = &exp->exp_obd->u.lov;
 	int rc = 0, i;
 
-	OBD_ALLOC(set, sizeof(*set));
+	set = kzalloc(sizeof(*set), GFP_NOFS);
 	if (set == NULL)
 		return -ENOMEM;
 	lov_init_set(set);
@@ -312,7 +300,7 @@ int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 			continue;
 		}
 
-		OBD_ALLOC(req, sizeof(*req));
+		req = kzalloc(sizeof(*req), GFP_NOFS);
 		if (req == NULL) {
 			rc = -ENOMEM;
 			goto out_set;
@@ -323,7 +311,7 @@ int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 
 		OBDO_ALLOC(req->rq_oi.oi_oa);
 		if (req->rq_oi.oi_oa == NULL) {
-			OBD_FREE(req, sizeof(*req));
+			kfree(req);
 			rc = -ENOMEM;
 			goto out_set;
 		}
@@ -369,7 +357,7 @@ int lov_prep_destroy_set(struct obd_export *exp, struct obd_info *oinfo,
 	struct lov_obd *lov = &exp->exp_obd->u.lov;
 	int rc = 0, i;
 
-	OBD_ALLOC(set, sizeof(*set));
+	set = kzalloc(sizeof(*set), GFP_NOFS);
 	if (set == NULL)
 		return -ENOMEM;
 	lov_init_set(set);
@@ -395,7 +383,7 @@ int lov_prep_destroy_set(struct obd_export *exp, struct obd_info *oinfo,
 			continue;
 		}
 
-		OBD_ALLOC(req, sizeof(*req));
+		req = kzalloc(sizeof(*req), GFP_NOFS);
 		if (req == NULL) {
 			rc = -ENOMEM;
 			goto out_set;
@@ -406,7 +394,7 @@ int lov_prep_destroy_set(struct obd_export *exp, struct obd_info *oinfo,
 
 		OBDO_ALLOC(req->rq_oi.oi_oa);
 		if (req->rq_oi.oi_oa == NULL) {
-			OBD_FREE(req, sizeof(*req));
+			kfree(req);
 			rc = -ENOMEM;
 			goto out_set;
 		}
@@ -488,7 +476,7 @@ int lov_prep_setattr_set(struct obd_export *exp, struct obd_info *oinfo,
 	struct lov_obd *lov = &exp->exp_obd->u.lov;
 	int rc = 0, i;
 
-	OBD_ALLOC(set, sizeof(*set));
+	set = kzalloc(sizeof(*set), GFP_NOFS);
 	if (set == NULL)
 		return -ENOMEM;
 	lov_init_set(set);
@@ -511,7 +499,7 @@ int lov_prep_setattr_set(struct obd_export *exp, struct obd_info *oinfo,
 			continue;
 		}
 
-		OBD_ALLOC(req, sizeof(*req));
+		req = kzalloc(sizeof(*req), GFP_NOFS);
 		if (req == NULL) {
 			rc = -ENOMEM;
 			goto out_set;
@@ -521,7 +509,7 @@ int lov_prep_setattr_set(struct obd_export *exp, struct obd_info *oinfo,
 
 		OBDO_ALLOC(req->rq_oi.oi_oa);
 		if (req->rq_oi.oi_oa == NULL) {
-			OBD_FREE(req, sizeof(*req));
+			kfree(req);
 			rc = -ENOMEM;
 			goto out_set;
 		}
@@ -619,8 +607,7 @@ void lov_update_statfs(struct obd_statfs *osfs, struct obd_statfs *lov_sfs,
 				if (tmp & 1) {
 					if (quit)
 						break;
-					else
-						quit = 1;
+					quit = 1;
 					shift = 0;
 				}
 				tmp >>= 1;
@@ -716,7 +703,7 @@ int lov_prep_statfs_set(struct obd_device *obd, struct obd_info *oinfo,
 	struct lov_obd *lov = &obd->u.lov;
 	int rc = 0, i;
 
-	OBD_ALLOC(set, sizeof(*set));
+	set = kzalloc(sizeof(*set), GFP_NOFS);
 	if (set == NULL)
 		return -ENOMEM;
 	lov_init_set(set);
@@ -742,15 +729,16 @@ int lov_prep_statfs_set(struct obd_device *obd, struct obd_info *oinfo,
 			continue;
 		}
 
-		OBD_ALLOC(req, sizeof(*req));
+		req = kzalloc(sizeof(*req), GFP_NOFS);
 		if (req == NULL) {
 			rc = -ENOMEM;
 			goto out_set;
 		}
 
-		OBD_ALLOC(req->rq_oi.oi_osfs, sizeof(*req->rq_oi.oi_osfs));
+		req->rq_oi.oi_osfs = kzalloc(sizeof(*req->rq_oi.oi_osfs),
+					     GFP_NOFS);
 		if (req->rq_oi.oi_osfs == NULL) {
-			OBD_FREE(req, sizeof(*req));
+			kfree(req);
 			rc = -ENOMEM;
 			goto out_set;
 		}
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_dev.c b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
index 42336f13a76f..90d9ec386a1a 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
@@ -136,7 +136,7 @@ static struct lu_device *lovsub_device_free(const struct lu_env *env,
 		lu_site_print(env, d->ld_site, &msgdata, lu_cdebug_printer);
 	}
 	cl_device_fini(lu2cl_dev(d));
-	OBD_FREE_PTR(lsd);
+	kfree(lsd);
 	return next;
 }
 
@@ -172,7 +172,7 @@ static struct lu_device *lovsub_device_alloc(const struct lu_env *env,
 	struct lu_device     *d;
 	struct lovsub_device *lsd;
 
-	OBD_ALLOC_PTR(lsd);
+	lsd = kzalloc(sizeof(*lsd), GFP_NOFS);
 	if (lsd != NULL) {
 		int result;
 
diff --git a/drivers/staging/lustre/lustre/lov/lproc_lov.c b/drivers/staging/lustre/lustre/lov/lproc_lov.c
index 174cbf5c138f..380b8271bf24 100644
--- a/drivers/staging/lustre/lustre/lov/lproc_lov.c
+++ b/drivers/staging/lustre/lustre/lov/lproc_lov.c
@@ -166,29 +166,29 @@ static ssize_t lov_stripecount_seq_write(struct file *file,
 }
 LPROC_SEQ_FOPS(lov_stripecount);
 
-static int lov_numobd_seq_show(struct seq_file *m, void *v)
+static ssize_t numobd_show(struct kobject *kobj, struct attribute *attr,
+			   char *buf)
 {
-	struct obd_device *dev = (struct obd_device *)m->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct lov_desc *desc;
 
-	LASSERT(dev != NULL);
 	desc = &dev->u.lov.desc;
-	seq_printf(m, "%u\n", desc->ld_tgt_count);
-	return 0;
+	return sprintf(buf, "%u\n", desc->ld_tgt_count);
 }
-LPROC_SEQ_FOPS_RO(lov_numobd);
+LUSTRE_RO_ATTR(numobd);
 
-static int lov_activeobd_seq_show(struct seq_file *m, void *v)
+static ssize_t activeobd_show(struct kobject *kobj, struct attribute *attr,
+			      char *buf)
 {
-	struct obd_device *dev = (struct obd_device *)m->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct lov_desc *desc;
 
-	LASSERT(dev != NULL);
 	desc = &dev->u.lov.desc;
-	seq_printf(m, "%u\n", desc->ld_active_tgt_count);
-	return 0;
+	return sprintf(buf, "%u\n", desc->ld_active_tgt_count);
 }
-LPROC_SEQ_FOPS_RO(lov_activeobd);
+LUSTRE_RO_ATTR(activeobd);
 
 static int lov_desc_uuid_seq_show(struct seq_file *m, void *v)
 {
@@ -258,48 +258,34 @@ static int lov_target_seq_open(struct inode *inode, struct file *file)
 		return rc;
 
 	seq = file->private_data;
-	seq->private = PDE_DATA(inode);
+	seq->private = inode->i_private;
 	return 0;
 }
 
-LPROC_SEQ_FOPS_RO_TYPE(lov, uuid);
-LPROC_SEQ_FOPS_RO_TYPE(lov, filestotal);
-LPROC_SEQ_FOPS_RO_TYPE(lov, filesfree);
-LPROC_SEQ_FOPS_RO_TYPE(lov, blksize);
-LPROC_SEQ_FOPS_RO_TYPE(lov, kbytestotal);
-LPROC_SEQ_FOPS_RO_TYPE(lov, kbytesfree);
-LPROC_SEQ_FOPS_RO_TYPE(lov, kbytesavail);
-
 static struct lprocfs_vars lprocfs_lov_obd_vars[] = {
-	{ "uuid",	  &lov_uuid_fops,	  NULL, 0 },
 	{ "stripesize",   &lov_stripesize_fops,   NULL },
 	{ "stripeoffset", &lov_stripeoffset_fops, NULL },
 	{ "stripecount",  &lov_stripecount_fops,  NULL },
 	{ "stripetype",   &lov_stripetype_fops,   NULL },
-	{ "numobd",       &lov_numobd_fops,	  NULL, 0 },
-	{ "activeobd",    &lov_activeobd_fops,	  NULL, 0 },
-	{ "filestotal",   &lov_filestotal_fops,   NULL, 0 },
-	{ "filesfree",    &lov_filesfree_fops,    NULL, 0 },
 	/*{ "filegroups", lprocfs_rd_filegroups,  NULL, 0 },*/
-	{ "blocksize",    &lov_blksize_fops,      NULL, 0 },
-	{ "kbytestotal",  &lov_kbytestotal_fops,  NULL, 0 },
-	{ "kbytesfree",   &lov_kbytesfree_fops,   NULL, 0 },
-	{ "kbytesavail",  &lov_kbytesavail_fops,  NULL, 0 },
 	{ "desc_uuid",    &lov_desc_uuid_fops,    NULL, 0 },
 	{ NULL }
 };
 
-LPROC_SEQ_FOPS_RO_TYPE(lov, numrefs);
+static struct attribute *lov_attrs[] = {
+	&lustre_attr_activeobd.attr,
+	&lustre_attr_numobd.attr,
+	NULL,
+};
 
-static struct lprocfs_vars lprocfs_lov_module_vars[] = {
-	{ "num_refs",     &lov_numrefs_fops,     NULL, 0 },
-	{ NULL }
+static struct attribute_group lov_attr_group = {
+	.attrs = lov_attrs,
 };
 
 void lprocfs_lov_init_vars(struct lprocfs_static_vars *lvars)
 {
-    lvars->module_vars  = lprocfs_lov_module_vars;
-    lvars->obd_vars     = lprocfs_lov_obd_vars;
+	lvars->sysfs_vars = &lov_attr_group;
+	lvars->obd_vars = lprocfs_lov_obd_vars;
 }
 
 const struct file_operations lov_proc_target_fops = {
diff --git a/drivers/staging/lustre/lustre/mdc/Makefile b/drivers/staging/lustre/lustre/mdc/Makefile
index 2516551a6dc3..99ba9ff0d83a 100644
--- a/drivers/staging/lustre/lustre/mdc/Makefile
+++ b/drivers/staging/lustre/lustre/mdc/Makefile
@@ -1,3 +1,2 @@
 obj-$(CONFIG_LUSTRE_FS) += mdc.o
-mdc-y := mdc_request.o mdc_reint.o mdc_lib.o mdc_locks.o
-mdc-$(CONFIG_PROC_FS) += lproc_mdc.o
+mdc-y := mdc_request.o mdc_reint.o mdc_lib.o mdc_locks.o lproc_mdc.o
diff --git a/drivers/staging/lustre/lustre/mdc/lproc_mdc.c b/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
index acfe08e459c0..1c95f87a0e2a 100644
--- a/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
+++ b/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
@@ -40,29 +40,34 @@
 #include "../include/lprocfs_status.h"
 #include "mdc_internal.h"
 
-static int mdc_max_rpcs_in_flight_seq_show(struct seq_file *m, void *v)
+static ssize_t max_rpcs_in_flight_show(struct kobject *kobj,
+				       struct attribute *attr,
+				       char *buf)
 {
-	struct obd_device *dev = m->private;
+	int len;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	seq_printf(m, "%u\n", cli->cl_max_rpcs_in_flight);
+	len = sprintf(buf, "%u\n", cli->cl_max_rpcs_in_flight);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	return 0;
+	return len;
 }
 
-static ssize_t mdc_max_rpcs_in_flight_seq_write(struct file *file,
-						const char __user *buffer,
-						size_t count,
-						loff_t *off)
+static ssize_t max_rpcs_in_flight_store(struct kobject *kobj,
+					struct attribute *attr,
+					const char *buffer,
+					size_t count)
 {
-	struct obd_device *dev =
-			((struct seq_file *)file->private_data)->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
-	int val, rc;
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -75,11 +80,11 @@ static ssize_t mdc_max_rpcs_in_flight_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(mdc_max_rpcs_in_flight);
+LUSTRE_RW_ATTR(max_rpcs_in_flight);
 
 static int mdc_kuc_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, NULL, PDE_DATA(inode));
+	return single_open(file, NULL, inode->i_private);
 }
 
 /* temporary for testing */
@@ -106,7 +111,7 @@ static ssize_t mdc_kuc_write(struct file *file,
 	len = sizeof(*lh) + sizeof(*hal) + MTI_NAME_MAXLEN +
 		/* for mockup below */ 2 * cfs_size_round(sizeof(*hai));
 
-	OBD_ALLOC(lh, len);
+	lh = kzalloc(len, GFP_NOFS);
 	if (!lh)
 		return -ENOMEM;
 
@@ -141,13 +146,13 @@ static ssize_t mdc_kuc_write(struct file *file,
 		rc = libcfs_kkuc_msg_put(fp, lh);
 		fput(fp);
 	}
-	OBD_FREE(lh, len);
+	kfree(lh);
 	if (rc < 0)
 		return rc;
 	return count;
 }
 
-struct file_operations mdc_kuc_fops = {
+static struct file_operations mdc_kuc_fops = {
 	.open		= mdc_kuc_open,
 	.write		= mdc_kuc_write,
 	.release	= single_release,
@@ -155,49 +160,39 @@ struct file_operations mdc_kuc_fops = {
 
 LPROC_SEQ_FOPS_WR_ONLY(mdc, ping);
 
-LPROC_SEQ_FOPS_RO_TYPE(mdc, uuid);
 LPROC_SEQ_FOPS_RO_TYPE(mdc, connect_flags);
-LPROC_SEQ_FOPS_RO_TYPE(mdc, blksize);
-LPROC_SEQ_FOPS_RO_TYPE(mdc, kbytestotal);
-LPROC_SEQ_FOPS_RO_TYPE(mdc, kbytesfree);
-LPROC_SEQ_FOPS_RO_TYPE(mdc, kbytesavail);
-LPROC_SEQ_FOPS_RO_TYPE(mdc, filestotal);
-LPROC_SEQ_FOPS_RO_TYPE(mdc, filesfree);
 LPROC_SEQ_FOPS_RO_TYPE(mdc, server_uuid);
 LPROC_SEQ_FOPS_RO_TYPE(mdc, conn_uuid);
 LPROC_SEQ_FOPS_RO_TYPE(mdc, timeouts);
 LPROC_SEQ_FOPS_RO_TYPE(mdc, state);
 
-static int mdc_obd_max_pages_per_rpc_seq_show(struct seq_file *m, void *v)
+/*
+ * Note: below sysfs entry is provided, but not currently in use, instead
+ * sbi->sb_md_brw_size is used, the per obd variable should be used
+ * when DNE is enabled, and dir pages are managed in MDC layer.
+ * Don't forget to enable sysfs store function then.
+ */
+static ssize_t max_pages_per_rpc_show(struct kobject *kobj,
+				      struct attribute *attr,
+				      char *buf)
 {
-	return lprocfs_obd_rd_max_pages_per_rpc(m, m->private);
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	struct client_obd *cli = &dev->u.cli;
+
+	return sprintf(buf, "%d\n", cli->cl_max_pages_per_rpc);
 }
-LPROC_SEQ_FOPS_RO(mdc_obd_max_pages_per_rpc);
+LUSTRE_RO_ATTR(max_pages_per_rpc);
 
 LPROC_SEQ_FOPS_RW_TYPE(mdc, import);
 LPROC_SEQ_FOPS_RW_TYPE(mdc, pinger_recov);
 
 static struct lprocfs_vars lprocfs_mdc_obd_vars[] = {
-	{ "uuid",		&mdc_uuid_fops,			NULL, 0 },
 	{ "ping",		&mdc_ping_fops,			NULL, 0222 },
 	{ "connect_flags",	&mdc_connect_flags_fops,	NULL, 0 },
-	{ "blocksize",		&mdc_blksize_fops,		NULL, 0 },
-	{ "kbytestotal",	&mdc_kbytestotal_fops,		NULL, 0 },
-	{ "kbytesfree",		&mdc_kbytesfree_fops,		NULL, 0 },
-	{ "kbytesavail",	&mdc_kbytesavail_fops,		NULL, 0 },
-	{ "filestotal",		&mdc_filestotal_fops,		NULL, 0 },
-	{ "filesfree",		&mdc_filesfree_fops,		NULL, 0 },
 	/*{ "filegroups",	lprocfs_rd_filegroups,		NULL, 0 },*/
 	{ "mds_server_uuid",	&mdc_server_uuid_fops,		NULL, 0 },
 	{ "mds_conn_uuid",	&mdc_conn_uuid_fops,		NULL, 0 },
-	/*
-	 * FIXME: below proc entry is provided, but not in used, instead
-	 * sbi->sb_md_brw_size is used, the per obd variable should be used
-	 * when CMD is enabled, and dir pages are managed in MDC layer.
-	 * Remember to enable proc write function.
-	 */
-	{ "max_pages_per_rpc",	&mdc_obd_max_pages_per_rpc_fops, NULL, 0 },
-	{ "max_rpcs_in_flight",	&mdc_max_rpcs_in_flight_fops,	NULL, 0 },
 	{ "timeouts",		&mdc_timeouts_fops,		NULL, 0 },
 	{ "import",		&mdc_import_fops,		NULL, 0 },
 	{ "state",		&mdc_state_fops,		NULL, 0 },
@@ -206,15 +201,18 @@ static struct lprocfs_vars lprocfs_mdc_obd_vars[] = {
 	{ NULL }
 };
 
-LPROC_SEQ_FOPS_RO_TYPE(mdc, numrefs);
+static struct attribute *mdc_attrs[] = {
+	&lustre_attr_max_rpcs_in_flight.attr,
+	&lustre_attr_max_pages_per_rpc.attr,
+	NULL,
+};
 
-static struct lprocfs_vars lprocfs_mdc_module_vars[] = {
-	{ "num_refs",		&mdc_numrefs_fops,		NULL, 0 },
-	{ NULL }
+static struct attribute_group mdc_attr_group = {
+	.attrs = mdc_attrs,
 };
 
 void lprocfs_mdc_init_vars(struct lprocfs_static_vars *lvars)
 {
-	lvars->module_vars  = lprocfs_mdc_module_vars;
+	lvars->sysfs_vars   = &mdc_attr_group;
 	lvars->obd_vars     = lprocfs_mdc_obd_vars;
 }
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_internal.h b/drivers/staging/lustre/lustre/mdc/mdc_internal.h
index 81780c943a08..4d149435e949 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_internal.h
+++ b/drivers/staging/lustre/lustre/mdc/mdc_internal.h
@@ -40,14 +40,7 @@
 #include "../include/lustre_mdc.h"
 #include "../include/lustre_mds.h"
 
-#if defined CONFIG_PROC_FS
 void lprocfs_mdc_init_vars(struct lprocfs_static_vars *lvars);
-#else
-static inline void lprocfs_mdc_init_vars(struct lprocfs_static_vars *lvars)
-{
-	memset(lvars, 0, sizeof(*lvars));
-}
-#endif
 
 void mdc_pack_body(struct ptlrpc_request *req, const struct lu_fid *fid,
 		   struct obd_capa *oc, __u64 valid, int ea_size,
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_locks.c b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
index d1c224ecd2b7..bcb6c00c49ff 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_locks.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
@@ -746,7 +746,7 @@ static int mdc_finish_enqueue(struct obd_export *exp,
 		LDLM_DEBUG(lock, "layout lock returned by: %s, lvb_len: %d\n",
 			ldlm_it2str(it->it_op), lvb_len);
 
-		OBD_ALLOC_LARGE(lmm, lvb_len);
+		lmm = libcfs_kvzalloc(lvb_len, GFP_NOFS);
 		if (lmm == NULL) {
 			LDLM_LOCK_PUT(lock);
 			return -ENOMEM;
@@ -763,7 +763,7 @@ static int mdc_finish_enqueue(struct obd_export *exp,
 		}
 		unlock_res_and_lock(lock);
 		if (lmm != NULL)
-			OBD_FREE_LARGE(lmm, lvb_len);
+			kvfree(lmm);
 	}
 	if (lock != NULL)
 		LDLM_LOCK_PUT(lock);
@@ -1251,7 +1251,7 @@ static int mdc_intent_getattr_async_interpret(const struct lu_env *env,
 	rc = mdc_finish_intent_lock(exp, req, &minfo->mi_data, it, lockh);
 
 out:
-	OBD_FREE_PTR(einfo);
+	kfree(einfo);
 	minfo->mi_cb(req, minfo, rc);
 	return 0;
 }
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_request.c b/drivers/staging/lustre/lustre/mdc/mdc_request.c
index f8ef5fe5e771..7f208a6621e6 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_request.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_request.c
@@ -75,11 +75,11 @@ static int mdc_unpack_capa(struct obd_export *exp, struct ptlrpc_request *req,
 	if (IS_ERR(c)) {
 		CDEBUG(D_INFO, "alloc capa failed!\n");
 		return PTR_ERR(c);
-	} else {
-		c->c_capa = *capa;
-		*oc = c;
-		return 0;
 	}
+
+	c->c_capa = *capa;
+	*oc = c;
+	return 0;
 }
 
 static inline int mdc_queue_wait(struct ptlrpc_request *req)
@@ -1201,7 +1201,7 @@ static int mdc_ioc_fid2path(struct obd_export *exp, struct getinfo_fid2path *gf)
 
 	/* Key is KEY_FID2PATH + getinfo_fid2path description */
 	keylen = cfs_size_round(sizeof(KEY_FID2PATH)) + sizeof(*gf);
-	OBD_ALLOC(key, keylen);
+	key = kzalloc(keylen, GFP_NOFS);
 	if (key == NULL)
 		return -ENOMEM;
 	memcpy(key, KEY_FID2PATH, sizeof(KEY_FID2PATH));
@@ -1234,7 +1234,7 @@ static int mdc_ioc_fid2path(struct obd_export *exp, struct getinfo_fid2path *gf)
 	       PFID(&gf->gf_fid), gf->gf_recno, gf->gf_linkno, gf->gf_path);
 
 out:
-	OBD_FREE(key, keylen);
+	kfree(key);
 	return rc;
 }
 
@@ -1604,7 +1604,7 @@ static int mdc_changelog_send_thread(void *csdata)
 	CDEBUG(D_CHANGELOG, "changelog to fp=%p start %llu\n",
 	       cs->cs_fp, cs->cs_startrec);
 
-	OBD_ALLOC(cs->cs_buf, KUC_CHANGELOG_MSG_MAXSIZE);
+	cs->cs_buf = kzalloc(KUC_CHANGELOG_MSG_MAXSIZE, GFP_NOFS);
 	if (cs->cs_buf == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -1644,9 +1644,8 @@ out:
 		llog_cat_close(NULL, llh);
 	if (ctxt)
 		llog_ctxt_put(ctxt);
-	if (cs->cs_buf)
-		OBD_FREE(cs->cs_buf, KUC_CHANGELOG_MSG_MAXSIZE);
-	OBD_FREE_PTR(cs);
+	kfree(cs->cs_buf);
+	kfree(cs);
 	return rc;
 }
 
@@ -1657,7 +1656,7 @@ static int mdc_ioc_changelog_send(struct obd_device *obd,
 	int rc;
 
 	/* Freed in mdc_changelog_send_thread */
-	OBD_ALLOC_PTR(cs);
+	cs = kzalloc(sizeof(*cs), GFP_NOFS);
 	if (!cs)
 		return -ENOMEM;
 
@@ -1679,7 +1678,7 @@ static int mdc_ioc_changelog_send(struct obd_device *obd,
 	}
 
 	CERROR("Failed to start changelog thread: %d\n", rc);
-	OBD_FREE_PTR(cs);
+	kfree(cs);
 	return rc;
 }
 
@@ -1819,10 +1818,7 @@ static int mdc_ioc_swap_layouts(struct obd_export *exp,
 	ptlrpc_request_set_replen(req);
 
 	rc = ptlrpc_queue_wait(req);
-	if (rc)
-		goto out;
 
-out:
 	ptlrpc_req_finished(req);
 	return rc;
 }
@@ -1937,7 +1933,7 @@ static int mdc_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		struct if_quotactl *qctl = karg;
 		struct obd_quotactl *oqctl;
 
-		OBD_ALLOC_PTR(oqctl);
+		oqctl = kzalloc(sizeof(*oqctl), GFP_NOFS);
 		if (oqctl == NULL) {
 			rc = -ENOMEM;
 			goto out;
@@ -1951,7 +1947,7 @@ static int mdc_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 			qctl->obd_uuid = obd->u.cli.cl_target_uuid;
 		}
 
-		OBD_FREE_PTR(oqctl);
+		kfree(oqctl);
 		goto out;
 	}
 	case LL_IOC_GET_CONNECT_FLAGS:
@@ -2094,7 +2090,6 @@ static int mdc_hsm_copytool_send(int len, void *val)
 {
 	struct kuc_hdr		*lh = (struct kuc_hdr *)val;
 	struct hsm_action_list	*hal = (struct hsm_action_list *)(lh + 1);
-	int			 rc;
 
 	if (len < sizeof(*lh) + sizeof(*hal)) {
 		CERROR("Short HSM message %d < %d\n", len,
@@ -2115,9 +2110,7 @@ static int mdc_hsm_copytool_send(int len, void *val)
 	       lh->kuc_msglen, hal->hal_count, hal->hal_fsname);
 
 	/* Broadcast to HSM listeners */
-	rc = libcfs_kkuc_group_put(KUC_GRP_HSM, lh);
-
-	return rc;
+	return libcfs_kkuc_group_put(KUC_GRP_HSM, lh);
 }
 
 /**
@@ -2430,14 +2423,14 @@ static int mdc_setup(struct obd_device *obd, struct lustre_cfg *cfg)
 	struct lprocfs_static_vars lvars = { NULL };
 	int rc;
 
-	OBD_ALLOC(cli->cl_rpc_lock, sizeof(*cli->cl_rpc_lock));
+	cli->cl_rpc_lock = kzalloc(sizeof(*cli->cl_rpc_lock), GFP_NOFS);
 	if (!cli->cl_rpc_lock)
 		return -ENOMEM;
 	mdc_init_rpc_lock(cli->cl_rpc_lock);
 
 	ptlrpcd_addref();
 
-	OBD_ALLOC(cli->cl_close_lock, sizeof(*cli->cl_close_lock));
+	cli->cl_close_lock = kzalloc(sizeof(*cli->cl_close_lock), GFP_NOFS);
 	if (!cli->cl_close_lock) {
 		rc = -ENOMEM;
 		goto err_rpc_lock;
@@ -2448,7 +2441,7 @@ static int mdc_setup(struct obd_device *obd, struct lustre_cfg *cfg)
 	if (rc)
 		goto err_close_lock;
 	lprocfs_mdc_init_vars(&lvars);
-	lprocfs_obd_setup(obd, lvars.obd_vars);
+	lprocfs_obd_setup(obd, lvars.obd_vars, lvars.sysfs_vars);
 	sptlrpc_lprocfs_cliobd_attach(obd);
 	ptlrpc_lprocfs_register_obd(obd);
 
@@ -2465,9 +2458,9 @@ static int mdc_setup(struct obd_device *obd, struct lustre_cfg *cfg)
 	return rc;
 
 err_close_lock:
-	OBD_FREE(cli->cl_close_lock, sizeof(*cli->cl_close_lock));
+	kfree(cli->cl_close_lock);
 err_rpc_lock:
-	OBD_FREE(cli->cl_rpc_lock, sizeof(*cli->cl_rpc_lock));
+	kfree(cli->cl_rpc_lock);
 	ptlrpcd_decref();
 	return rc;
 }
@@ -2525,8 +2518,8 @@ static int mdc_cleanup(struct obd_device *obd)
 {
 	struct client_obd *cli = &obd->u.cli;
 
-	OBD_FREE(cli->cl_rpc_lock, sizeof(*cli->cl_rpc_lock));
-	OBD_FREE(cli->cl_close_lock, sizeof(*cli->cl_close_lock));
+	kfree(cli->cl_rpc_lock);
+	kfree(cli->cl_close_lock);
 
 	ptlrpcd_decref();
 
@@ -2714,7 +2707,7 @@ static int __init mdc_init(void)
 
 	lprocfs_mdc_init_vars(&lvars);
 
-	return class_register_type(&mdc_obd_ops, &mdc_md_ops, lvars.module_vars,
+	return class_register_type(&mdc_obd_ops, &mdc_md_ops,
 				 LUSTRE_MDC_NAME, NULL);
 }
 
diff --git a/drivers/staging/lustre/lustre/mgc/Makefile b/drivers/staging/lustre/lustre/mgc/Makefile
index cc6e9f51a8e8..8ea29a89cf50 100644
--- a/drivers/staging/lustre/lustre/mgc/Makefile
+++ b/drivers/staging/lustre/lustre/mgc/Makefile
@@ -1,3 +1,2 @@
 obj-$(CONFIG_LUSTRE_FS) += mgc.o
-mgc-y := mgc_request.o
-mgc-$(CONFIG_PROC_FS) += lproc_mgc.o
+mgc-y := mgc_request.o lproc_mgc.o
diff --git a/drivers/staging/lustre/lustre/mgc/lproc_mgc.c b/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
index c4ea38e5f077..34a9317d6d63 100644
--- a/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
+++ b/drivers/staging/lustre/lustre/mgc/lproc_mgc.c
@@ -40,7 +40,6 @@
 #include "../include/lprocfs_status.h"
 #include "mgc_internal.h"
 
-LPROC_SEQ_FOPS_RO_TYPE(mgc, uuid);
 LPROC_SEQ_FOPS_RO_TYPE(mgc, connect_flags);
 LPROC_SEQ_FOPS_RO_TYPE(mgc, server_uuid);
 LPROC_SEQ_FOPS_RO_TYPE(mgc, conn_uuid);
@@ -56,7 +55,6 @@ static int mgc_ir_state_seq_show(struct seq_file *m, void *v)
 LPROC_SEQ_FOPS_RO(mgc_ir_state);
 
 static struct lprocfs_vars lprocfs_mgc_obd_vars[] = {
-	{ "uuid",	     &mgc_uuid_fops,	  NULL, 0 },
 	{ "ping",	     &mgc_ping_fops,      NULL, 0222 },
 	{ "connect_flags",   &mgc_connect_flags_fops, NULL, 0 },
 	{ "mgs_server_uuid", &mgc_server_uuid_fops,   NULL, 0 },
@@ -67,14 +65,7 @@ static struct lprocfs_vars lprocfs_mgc_obd_vars[] = {
 	{ NULL }
 };
 
-LPROC_SEQ_FOPS_RO_TYPE(mgc, numrefs);
-static struct lprocfs_vars lprocfs_mgc_module_vars[] = {
-	{ "num_refs",	&mgc_numrefs_fops,       NULL, 0 },
-	{ NULL }
-};
-
 void lprocfs_mgc_init_vars(struct lprocfs_static_vars *lvars)
 {
-	lvars->module_vars = lprocfs_mgc_module_vars;
 	lvars->obd_vars    = lprocfs_mgc_obd_vars;
 }
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_internal.h b/drivers/staging/lustre/lustre/mgc/mgc_internal.h
index a6f8b3ced2e7..82fb8f46e037 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_internal.h
+++ b/drivers/staging/lustre/lustre/mgc/mgc_internal.h
@@ -44,19 +44,8 @@
 #include "../include/lustre_log.h"
 #include "../include/lustre_export.h"
 
-#if defined (CONFIG_PROC_FS)
 void lprocfs_mgc_init_vars(struct lprocfs_static_vars *lvars);
 int lprocfs_mgc_rd_ir_state(struct seq_file *m, void *data);
-#else
-static inline void lprocfs_mgc_init_vars(struct lprocfs_static_vars *lvars)
-{
-	memset(lvars, 0, sizeof(*lvars));
-}
-static inline int lprocfs_mgc_rd_ir_state(struct seq_file *m, void *data)
-{
-	return 0;
-}
-#endif  /* CONFIG_PROC_FS */
 
 int mgc_process_log(struct obd_device *mgc, struct config_llog_data *cld);
 
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_request.c b/drivers/staging/lustre/lustre/mgc/mgc_request.c
index 7947aec5c847..174dfc32876b 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_request.c
+++ b/drivers/staging/lustre/lustre/mgc/mgc_request.c
@@ -149,7 +149,7 @@ static void config_log_put(struct config_llog_data *cld)
 			sptlrpc_conf_log_stop(cld->cld_logname);
 
 		class_export_put(cld->cld_mgcexp);
-		OBD_FREE(cld, sizeof(*cld) + strlen(cld->cld_logname) + 1);
+		kfree(cld);
 	}
 }
 
@@ -198,7 +198,7 @@ struct config_llog_data *do_config_log_add(struct obd_device *obd,
 	CDEBUG(D_MGC, "do adding config log %s:%p\n", logname,
 	       cfg ? cfg->cfg_instance : NULL);
 
-	OBD_ALLOC(cld, sizeof(*cld) + strlen(logname) + 1);
+	cld = kzalloc(sizeof(*cld) + strlen(logname) + 1, GFP_NOFS);
 	if (!cld)
 		return ERR_PTR(-ENOMEM);
 
@@ -448,7 +448,6 @@ static int config_log_end(char *logname, struct config_llog_instance *cfg)
 	return rc;
 }
 
-#if defined (CONFIG_PROC_FS)
 int lprocfs_mgc_rd_ir_state(struct seq_file *m, void *data)
 {
 	struct obd_device       *obd = data;
@@ -477,7 +476,6 @@ int lprocfs_mgc_rd_ir_state(struct seq_file *m, void *data)
 	LPROCFS_CLIMP_EXIT(obd);
 	return 0;
 }
-#endif
 
 /* reenqueue any lost locks */
 #define RQ_RUNNING 0x1
@@ -722,7 +720,7 @@ static int mgc_cleanup(struct obd_device *obd)
 
 static int mgc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 {
-	struct lprocfs_static_vars lvars;
+	struct lprocfs_static_vars lvars = { NULL };
 	int rc;
 
 	ptlrpcd_addref();
@@ -738,7 +736,7 @@ static int mgc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 	}
 
 	lprocfs_mgc_init_vars(&lvars);
-	lprocfs_obd_setup(obd, lvars.obd_vars);
+	lprocfs_obd_setup(obd, lvars.obd_vars, lvars.sysfs_vars);
 	sptlrpc_lprocfs_cliobd_attach(obd);
 
 	if (atomic_inc_return(&mgc_count) == 1) {
@@ -1129,14 +1127,14 @@ static int mgc_apply_recover_logs(struct obd_device *mgc,
 	LASSERT(cfg->cfg_instance != NULL);
 	LASSERT(cfg->cfg_sb == cfg->cfg_instance);
 
-	OBD_ALLOC(inst, PAGE_CACHE_SIZE);
+	inst = kzalloc(PAGE_CACHE_SIZE, GFP_NOFS);
 	if (inst == NULL)
 		return -ENOMEM;
 
 	if (!IS_SERVER(lsi)) {
 		pos = snprintf(inst, PAGE_CACHE_SIZE, "%p", cfg->cfg_instance);
 		if (pos >= PAGE_CACHE_SIZE) {
-			OBD_FREE(inst, PAGE_CACHE_SIZE);
+			kfree(inst);
 			return -E2BIG;
 		}
 	} else {
@@ -1144,7 +1142,7 @@ static int mgc_apply_recover_logs(struct obd_device *mgc,
 		rc = server_name2svname(lsi->lsi_svname, inst, NULL,
 					PAGE_CACHE_SIZE);
 		if (rc) {
-			OBD_FREE(inst, PAGE_CACHE_SIZE);
+			kfree(inst);
 			return -EINVAL;
 		}
 		pos = strlen(inst);
@@ -1302,7 +1300,7 @@ static int mgc_apply_recover_logs(struct obd_device *mgc,
 		/* continue, even one with error */
 	}
 
-	OBD_FREE(inst, PAGE_CACHE_SIZE);
+	kfree(inst);
 	return rc;
 }
 
@@ -1336,7 +1334,7 @@ static int mgc_process_recover_log(struct obd_device *obd,
 	if (cfg->cfg_last_idx == 0) /* the first time */
 		nrpages = CONFIG_READ_NRPAGES_INIT;
 
-	OBD_ALLOC(pages, sizeof(*pages) * nrpages);
+	pages = kcalloc(nrpages, sizeof(*pages), GFP_NOFS);
 	if (pages == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -1466,7 +1464,7 @@ out:
 				break;
 			__free_page(pages[i]);
 		}
-		OBD_FREE(pages, sizeof(*pages) * nrpages);
+		kfree(pages);
 	}
 	return rc;
 }
@@ -1494,7 +1492,7 @@ static int mgc_process_cfg_log(struct obd_device *mgc,
 	if (cld->cld_cfg.cfg_sb)
 		lsi = s2lsi(cld->cld_cfg.cfg_sb);
 
-	OBD_ALLOC_PTR(env);
+	env = kzalloc(sizeof(*env), GFP_NOFS);
 	if (env == NULL)
 		return -ENOMEM;
 
@@ -1540,7 +1538,7 @@ out_pop:
 
 	lu_env_fini(env);
 out_free:
-	OBD_FREE_PTR(env);
+	kfree(env);
 	return rc;
 }
 
@@ -1745,7 +1743,7 @@ struct obd_ops mgc_obd_ops = {
 
 static int __init mgc_init(void)
 {
-	return class_register_type(&mgc_obd_ops, NULL, NULL,
+	return class_register_type(&mgc_obd_ops, NULL,
 				   LUSTRE_MGC_NAME, NULL);
 }
 
diff --git a/drivers/staging/lustre/lustre/obdclass/Makefile b/drivers/staging/lustre/lustre/obdclass/Makefile
index e894681797c2..d0f70b41acf6 100644
--- a/drivers/staging/lustre/lustre/obdclass/Makefile
+++ b/drivers/staging/lustre/lustre/obdclass/Makefile
@@ -6,6 +6,4 @@ obdclass-y := linux/linux-module.o linux/linux-obdo.o linux/linux-sysctl.o \
 	      lustre_handles.o lustre_peer.o \
 	      statfs_pack.o obdo.o obd_config.o obd_mount.o \
 	      lu_object.o dt_object.o capa.o cl_object.o   \
-	      cl_page.o cl_lock.o cl_io.o lu_ref.o acl.o
-
-obdclass-$(CONFIG_PROC_FS) += lprocfs_counters.o
+	      cl_page.o cl_lock.o cl_io.o lu_ref.o acl.o lprocfs_counters.o
diff --git a/drivers/staging/lustre/lustre/obdclass/acl.c b/drivers/staging/lustre/lustre/obdclass/acl.c
index 9a69f6b35a0e..bc3fc4780cb9 100644
--- a/drivers/staging/lustre/lustre/obdclass/acl.c
+++ b/drivers/staging/lustre/lustre/obdclass/acl.c
@@ -104,12 +104,12 @@ static int lustre_posix_acl_xattr_reduce_space(posix_acl_xattr_header **header,
 	if (unlikely(old_count <= new_count))
 		return old_size;
 
-	OBD_ALLOC(new, new_size);
+	new = kzalloc(new_size, GFP_NOFS);
 	if (unlikely(new == NULL))
 		return -ENOMEM;
 
 	memcpy(new, *header, new_size);
-	OBD_FREE(*header, old_size);
+	kfree(*header);
 	*header = new;
 	return new_size;
 }
@@ -120,18 +120,17 @@ static int lustre_ext_acl_xattr_reduce_space(ext_acl_xattr_header **header,
 {
 	int ext_count = le32_to_cpu((*header)->a_count);
 	int ext_size = CFS_ACL_XATTR_SIZE(ext_count, ext_acl_xattr);
-	int old_size = CFS_ACL_XATTR_SIZE(old_count, ext_acl_xattr);
 	ext_acl_xattr_header *new;
 
 	if (unlikely(old_count <= ext_count))
 		return 0;
 
-	OBD_ALLOC(new, ext_size);
+	new = kzalloc(ext_size, GFP_NOFS);
 	if (unlikely(new == NULL))
 		return -ENOMEM;
 
 	memcpy(new, *header, ext_size);
-	OBD_FREE(*header, old_size);
+	kfree(*header);
 	*header = new;
 	return 0;
 }
@@ -152,7 +151,7 @@ lustre_posix_acl_xattr_2ext(posix_acl_xattr_header *header, int size)
 	else
 		count = CFS_ACL_XATTR_COUNT(size, posix_acl_xattr);
 	esize = CFS_ACL_XATTR_SIZE(count, ext_acl_xattr);
-	OBD_ALLOC(new, esize);
+	new = kzalloc(esize, GFP_NOFS);
 	if (unlikely(new == NULL))
 		return ERR_PTR(-ENOMEM);
 
@@ -183,7 +182,7 @@ int lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, size_t size,
 	if (size < sizeof(*new))
 		return -EINVAL;
 
-	OBD_ALLOC(new, size);
+	new = kzalloc(size, GFP_NOFS);
 	if (unlikely(new == NULL))
 		return -ENOMEM;
 
@@ -232,7 +231,7 @@ int lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, size_t size,
 
 _out:
 	if (rc) {
-		OBD_FREE(new, size);
+		kfree(new);
 		size = rc;
 	}
 	return size;
@@ -244,7 +243,7 @@ EXPORT_SYMBOL(lustre_posix_acl_xattr_filter);
  */
 void lustre_posix_acl_xattr_free(posix_acl_xattr_header *header, int size)
 {
-	OBD_FREE(header, size);
+	kfree(header);
 }
 EXPORT_SYMBOL(lustre_posix_acl_xattr_free);
 
@@ -253,8 +252,7 @@ EXPORT_SYMBOL(lustre_posix_acl_xattr_free);
  */
 void lustre_ext_acl_xattr_free(ext_acl_xattr_header *header)
 {
-	OBD_FREE(header, CFS_ACL_XATTR_SIZE(le32_to_cpu(header->a_count), \
-					    ext_acl_xattr));
+	kfree(header);
 }
 EXPORT_SYMBOL(lustre_ext_acl_xattr_free);
 
@@ -309,7 +307,7 @@ int lustre_acl_xattr_merge2posix(posix_acl_xattr_header *posix_header, int size,
 		/* there are only base ACL entries at most. */
 		posix_count = 3;
 		posix_size = CFS_ACL_XATTR_SIZE(posix_count, posix_acl_xattr);
-		OBD_ALLOC(new, posix_size);
+		new = kzalloc(posix_size, GFP_NOFS);
 		if (unlikely(new == NULL))
 			return -ENOMEM;
 
@@ -360,7 +358,7 @@ int lustre_acl_xattr_merge2posix(posix_acl_xattr_header *posix_header, int size,
 		posix_count = ori_posix_count + ext_count;
 		posix_size =
 			CFS_ACL_XATTR_SIZE(posix_count, posix_acl_xattr);
-		OBD_ALLOC(new, posix_size);
+		new = kzalloc(posix_size, GFP_NOFS);
 		if (unlikely(new == NULL))
 			return -ENOMEM;
 
@@ -402,7 +400,7 @@ int lustre_acl_xattr_merge2posix(posix_acl_xattr_header *posix_header, int size,
 
 _out:
 	if (rc) {
-		OBD_FREE(new, posix_size);
+		kfree(new);
 		posix_size = rc;
 	}
 	return posix_size;
@@ -432,7 +430,7 @@ lustre_acl_xattr_merge2ext(posix_acl_xattr_header *posix_header, int size,
 	ext_count = posix_count + ori_ext_count;
 	ext_size = CFS_ACL_XATTR_SIZE(ext_count, ext_acl_xattr);
 
-	OBD_ALLOC(new, ext_size);
+	new = kzalloc(ext_size, GFP_NOFS);
 	if (unlikely(new == NULL))
 		return ERR_PTR(-ENOMEM);
 
@@ -538,7 +536,7 @@ lustre_acl_xattr_merge2ext(posix_acl_xattr_header *posix_header, int size,
 
 out:
 	if (rc) {
-		OBD_FREE(new, ext_size);
+		kfree(new);
 		new = ERR_PTR(rc);
 	}
 	return new;
diff --git a/drivers/staging/lustre/lustre/obdclass/capa.c b/drivers/staging/lustre/lustre/obdclass/capa.c
index d206b1046a18..d8d1a66ad68e 100644
--- a/drivers/staging/lustre/lustre/obdclass/capa.c
+++ b/drivers/staging/lustre/lustre/obdclass/capa.c
@@ -87,7 +87,7 @@ struct hlist_head *init_capa_hash(void)
 	struct hlist_head *hash;
 	int nr_hash, i;
 
-	OBD_ALLOC(hash, PAGE_CACHE_SIZE);
+	hash = kzalloc(PAGE_CACHE_SIZE, GFP_NOFS);
 	if (!hash)
 		return NULL;
 
@@ -129,7 +129,7 @@ void cleanup_capa_hash(struct hlist_head *hash)
 	}
 	spin_unlock(&capa_lock);
 
-	OBD_FREE(hash, PAGE_CACHE_SIZE);
+	kfree(hash);
 }
 EXPORT_SYMBOL(cleanup_capa_hash);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_io.c b/drivers/staging/lustre/lustre/obdclass/cl_io.c
index 3141b6043708..fd1a4c5421e8 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_io.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_io.c
@@ -612,7 +612,7 @@ EXPORT_SYMBOL(cl_io_lock_add);
 static void cl_free_io_lock_link(const struct lu_env *env,
 				 struct cl_io_lock_link *link)
 {
-	OBD_FREE_PTR(link);
+	kfree(link);
 }
 
 /**
@@ -624,7 +624,7 @@ int cl_io_lock_alloc_add(const struct lu_env *env, struct cl_io *io,
 	struct cl_io_lock_link *link;
 	int result;
 
-	OBD_ALLOC_PTR(link);
+	link = kzalloc(sizeof(*link), GFP_NOFS);
 	if (link != NULL) {
 		link->cill_descr     = *descr;
 		link->cill_fini      = cl_free_io_lock_link;
@@ -1387,9 +1387,9 @@ static void cl_req_free(const struct lu_env *env, struct cl_req *req)
 				cl_object_put(env, obj);
 			}
 		}
-		OBD_FREE(req->crq_o, req->crq_nrobjs * sizeof(req->crq_o[0]));
+		kfree(req->crq_o);
 	}
-	OBD_FREE_PTR(req);
+	kfree(req);
 }
 
 static int cl_req_init(const struct lu_env *env, struct cl_req *req,
@@ -1448,7 +1448,7 @@ struct cl_req *cl_req_alloc(const struct lu_env *env, struct cl_page *page,
 
 	LINVRNT(nr_objects > 0);
 
-	OBD_ALLOC_PTR(req);
+	req = kzalloc(sizeof(*req), GFP_NOFS);
 	if (req != NULL) {
 		int result;
 
@@ -1456,7 +1456,8 @@ struct cl_req *cl_req_alloc(const struct lu_env *env, struct cl_page *page,
 		INIT_LIST_HEAD(&req->crq_pages);
 		INIT_LIST_HEAD(&req->crq_layers);
 
-		OBD_ALLOC(req->crq_o, nr_objects * sizeof(req->crq_o[0]));
+		req->crq_o = kcalloc(nr_objects, sizeof(req->crq_o[0]),
+				     GFP_NOFS);
 		if (req->crq_o != NULL) {
 			req->crq_nrobjs = nr_objects;
 			result = cl_req_init(env, req, page);
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_object.c b/drivers/staging/lustre/lustre/obdclass/cl_object.c
index f13d1fbffd9d..163fe0cd7f9a 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_object.c
@@ -906,10 +906,8 @@ struct lu_env *cl_env_nested_get(struct cl_env_nest *nest)
 	if (env != NULL) {
 		if (!cl_io_is_going(env))
 			return env;
-		else {
-			cl_env_put(env, &nest->cen_refcheck);
-			nest->cen_cookie = cl_env_reenter();
-		}
+		cl_env_put(env, &nest->cen_refcheck);
+		nest->cen_cookie = cl_env_reenter();
 	}
 	env = cl_env_get(&nest->cen_refcheck);
 	if (IS_ERR(env)) {
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_page.c b/drivers/staging/lustre/lustre/obdclass/cl_page.c
index b7dd04808060..a7f3032f34dd 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_page.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_page.c
@@ -62,12 +62,6 @@ static void cl_page_delete0(const struct lu_env *env, struct cl_page *pg,
 # define PINVRNT(env, page, exp) \
 	((void)sizeof(env), (void)sizeof(page), (void)sizeof !!(exp))
 
-/* Disable page statistic by default due to huge performance penalty. */
-#define CS_PAGE_INC(o, item)
-#define CS_PAGE_DEC(o, item)
-#define CS_PAGESTATE_INC(o, state)
-#define CS_PAGESTATE_DEC(o, state)
-
 /**
  * Internal version of cl_page_top, it should be called if the page is
  * known to be not freed, says with page referenced, or radix tree lock held,
@@ -248,7 +242,6 @@ EXPORT_SYMBOL(cl_page_gang_lookup);
 static void cl_page_free(const struct lu_env *env, struct cl_page *page)
 {
 	struct cl_object *obj  = page->cp_obj;
-	int pagesize = cl_object_header(obj)->coh_page_bufsize;
 
 	PASSERT(env, page, list_empty(&page->cp_batch));
 	PASSERT(env, page, page->cp_owner == NULL);
@@ -265,12 +258,10 @@ static void cl_page_free(const struct lu_env *env, struct cl_page *page)
 		list_del_init(page->cp_layers.next);
 		slice->cpl_ops->cpo_fini(env, slice);
 	}
-	CS_PAGE_DEC(obj, total);
-	CS_PAGESTATE_DEC(obj, page->cp_state);
 	lu_object_ref_del_at(&obj->co_lu, &page->cp_obj_ref, "cl_page", page);
 	cl_object_put(env, obj);
 	lu_ref_fini(&page->cp_reference);
-	OBD_FREE(page, pagesize);
+	kfree(page);
 }
 
 /**
@@ -324,11 +315,6 @@ static struct cl_page *cl_page_alloc(const struct lu_env *env,
 				}
 			}
 		}
-		if (result == 0) {
-			CS_PAGE_INC(o, total);
-			CS_PAGE_INC(o, create);
-			CS_PAGESTATE_DEC(o, CPS_CACHED);
-		}
 	} else {
 		page = ERR_PTR(-ENOMEM);
 	}
@@ -361,7 +347,6 @@ static struct cl_page *cl_page_find0(const struct lu_env *env,
 	might_sleep();
 
 	hdr = cl_object_header(o);
-	CS_PAGE_INC(o, lookup);
 
 	CDEBUG(D_PAGE, "%lu@"DFID" %p %lx %d\n",
 	       idx, PFID(&hdr->coh_lu.loh_fid), vmpage, vmpage->private, type);
@@ -388,7 +373,6 @@ static struct cl_page *cl_page_find0(const struct lu_env *env,
 	}
 
 	if (page != NULL) {
-		CS_PAGE_INC(o, hit);
 		return page;
 	}
 
@@ -555,8 +539,6 @@ static void cl_page_state_set0(const struct lu_env *env,
 		PASSERT(env, page,
 			equi(state == CPS_OWNED, page->cp_owner != NULL));
 
-		CS_PAGESTATE_DEC(page->cp_obj, page->cp_state);
-		CS_PAGESTATE_INC(page->cp_obj, state);
 		cl_page_state_set_trust(page, state);
 	}
 }
diff --git a/drivers/staging/lustre/lustre/obdclass/class_obd.c b/drivers/staging/lustre/lustre/obdclass/class_obd.c
index d4b74b670c43..1bc37566b3a5 100644
--- a/drivers/staging/lustre/lustre/obdclass/class_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/class_obd.c
@@ -231,7 +231,7 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 			err = -EINVAL;
 			goto out;
 		}
-		OBD_ALLOC(lcfg, data->ioc_plen1);
+		lcfg = kzalloc(data->ioc_plen1, GFP_NOFS);
 		if (lcfg == NULL) {
 			err = -ENOMEM;
 			goto out;
@@ -243,7 +243,7 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 		if (!err)
 			err = class_process_config(lcfg);
 
-		OBD_FREE(lcfg, data->ioc_plen1);
+		kfree(lcfg);
 		goto out;
 	}
 
@@ -506,15 +506,8 @@ int obd_init_checks(void)
 	return ret;
 }
 
-#if defined (CONFIG_PROC_FS)
 extern int class_procfs_init(void);
 extern int class_procfs_clean(void);
-#else
-static inline int class_procfs_init(void)
-{ return 0; }
-static inline int class_procfs_clean(void)
-{ return 0; }
-#endif
 
 static int __init init_obdclass(void)
 {
@@ -529,24 +522,22 @@ static int __init init_obdclass(void)
 	spin_lock_init(&obd_types_lock);
 	obd_zombie_impexp_init();
 
-	if (IS_ENABLED(CONFIG_PROC_FS)) {
-		obd_memory = lprocfs_alloc_stats(OBD_STATS_NUM,
-						 LPROCFS_STATS_FLAG_NONE |
-						 LPROCFS_STATS_FLAG_IRQ_SAFE);
+	obd_memory = lprocfs_alloc_stats(OBD_STATS_NUM,
+					 LPROCFS_STATS_FLAG_NONE |
+					 LPROCFS_STATS_FLAG_IRQ_SAFE);
 
-		if (obd_memory == NULL) {
-			CERROR("kmalloc of 'obd_memory' failed\n");
-			return -ENOMEM;
-		}
-
-		lprocfs_counter_init(obd_memory, OBD_MEMORY_STAT,
-				     LPROCFS_CNTR_AVGMINMAX,
-				     "memused", "bytes");
-		lprocfs_counter_init(obd_memory, OBD_MEMORY_PAGES_STAT,
-				     LPROCFS_CNTR_AVGMINMAX,
-				     "pagesused", "pages");
+	if (obd_memory == NULL) {
+		CERROR("kmalloc of 'obd_memory' failed\n");
+		return -ENOMEM;
 	}
 
+	lprocfs_counter_init(obd_memory, OBD_MEMORY_STAT,
+			     LPROCFS_CNTR_AVGMINMAX,
+			     "memused", "bytes");
+	lprocfs_counter_init(obd_memory, OBD_MEMORY_PAGES_STAT,
+			     LPROCFS_CNTR_AVGMINMAX,
+			     "pagesused", "pages");
+
 	err = obd_init_checks();
 	if (err == -EOVERFLOW)
 		return err;
@@ -620,7 +611,6 @@ void obd_update_maxusage(void)
 }
 EXPORT_SYMBOL(obd_update_maxusage);
 
-#if defined (CONFIG_PROC_FS)
 __u64 obd_memory_max(void)
 {
 	__u64 ret;
@@ -644,7 +634,6 @@ __u64 obd_pages_max(void)
 	return ret;
 }
 EXPORT_SYMBOL(obd_pages_max);
-#endif
 
 /* liblustre doesn't call cleanup_obdclass, apparently.  we carry on in this
  * ifdef to the end of the file to cover module and versioning goo.*/
diff --git a/drivers/staging/lustre/lustre/obdclass/dt_object.c b/drivers/staging/lustre/lustre/obdclass/dt_object.c
index b1eee0a6dc9a..b67b0feb03e0 100644
--- a/drivers/staging/lustre/lustre/obdclass/dt_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/dt_object.c
@@ -49,8 +49,6 @@
 /* fid_be_to_cpu() */
 #include "../include/lustre_fid.h"
 
-#include "../include/lustre_quota.h"
-
 /* context key constructor/destructor: dt_global_key_init, dt_global_key_fini */
 LU_KEY_INIT(dt_global, struct dt_thread_info);
 LU_KEY_FINI(dt_global, struct dt_thread_info);
@@ -237,7 +235,8 @@ EXPORT_SYMBOL(dt_locate_at);
 /**
  * find a object named \a entry in given \a dfh->dfh_o directory.
  */
-static int dt_find_entry(const struct lu_env *env, const char *entry, void *data)
+static int dt_find_entry(const struct lu_env *env,
+			 const char *entry, void *data)
 {
 	struct dt_find_hint  *dfh = data;
 	struct dt_device     *dt = dfh->dfh_dt;
@@ -330,9 +329,9 @@ static struct dt_object *dt_reg_open(const struct lu_env *env,
 	int result;
 
 	result = dt_lookup_dir(env, p, name, fid);
-	if (result == 0){
+	if (result == 0)
 		o = dt_locate(env, dt, fid);
-	} else
+	else
 		o = ERR_PTR(result);
 
 	return o;
@@ -923,7 +922,7 @@ int dt_index_read(const struct lu_env *env, struct dt_device *dev,
 	ii->ii_version = dt_version_get(env, obj);
 
 	/* walk the index and fill lu_idxpages with key/record pairs */
-	rc = dt_index_walk(env, obj, rdpg, dt_index_page_build ,ii);
+	rc = dt_index_walk(env, obj, rdpg, dt_index_page_build, ii);
 	dt_read_unlock(env, obj);
 
 	if (rc == 0) {
@@ -939,8 +938,6 @@ out:
 }
 EXPORT_SYMBOL(dt_index_read);
 
-#if defined (CONFIG_PROC_FS)
-
 int lprocfs_dt_rd_blksize(char *page, char **start, off_t off,
 			  int count, int *eof, void *data)
 {
@@ -1055,5 +1052,3 @@ int lprocfs_dt_rd_filesfree(char *page, char **start, off_t off,
 	return rc;
 }
 EXPORT_SYMBOL(lprocfs_dt_rd_filesfree);
-
-#endif /* CONFIG_PROC_FS */
diff --git a/drivers/staging/lustre/lustre/obdclass/genops.c b/drivers/staging/lustre/lustre/obdclass/genops.c
index 66b56784f674..978c3c5c460a 100644
--- a/drivers/staging/lustre/lustre/obdclass/genops.c
+++ b/drivers/staging/lustre/lustre/obdclass/genops.c
@@ -156,7 +156,7 @@ EXPORT_SYMBOL(class_put_type);
 #define CLASS_MAX_NAME 1024
 
 int class_register_type(struct obd_ops *dt_ops, struct md_ops *md_ops,
-			struct lprocfs_vars *vars, const char *name,
+			const char *name,
 			struct lu_device_type *ldt)
 {
 	struct obd_type *type;
@@ -171,13 +171,13 @@ int class_register_type(struct obd_ops *dt_ops, struct md_ops *md_ops,
 	}
 
 	rc = -ENOMEM;
-	OBD_ALLOC(type, sizeof(*type));
+	type = kzalloc(sizeof(*type), GFP_NOFS);
 	if (type == NULL)
 		return rc;
 
-	OBD_ALLOC_PTR(type->typ_dt_ops);
-	OBD_ALLOC_PTR(type->typ_md_ops);
-	OBD_ALLOC(type->typ_name, strlen(name) + 1);
+	type->typ_dt_ops = kzalloc(sizeof(*type->typ_dt_ops), GFP_NOFS);
+	type->typ_md_ops = kzalloc(sizeof(*type->typ_md_ops), GFP_NOFS);
+	type->typ_name = kzalloc(strlen(name) + 1, GFP_NOFS);
 
 	if (type->typ_dt_ops == NULL ||
 	    type->typ_md_ops == NULL ||
@@ -191,11 +191,19 @@ int class_register_type(struct obd_ops *dt_ops, struct md_ops *md_ops,
 	strcpy(type->typ_name, name);
 	spin_lock_init(&type->obd_type_lock);
 
-	type->typ_procroot = lprocfs_register(type->typ_name, proc_lustre_root,
-					      vars, type);
-	if (IS_ERR(type->typ_procroot)) {
-		rc = PTR_ERR(type->typ_procroot);
-		type->typ_procroot = NULL;
+	type->typ_debugfs_entry = ldebugfs_register(type->typ_name,
+						    debugfs_lustre_root,
+						    NULL, type);
+	if (IS_ERR_OR_NULL(type->typ_debugfs_entry)) {
+		rc = type->typ_debugfs_entry ? PTR_ERR(type->typ_debugfs_entry)
+					     : -ENOMEM;
+		type->typ_debugfs_entry = NULL;
+		goto failed;
+	}
+
+	type->typ_kobj = kobject_create_and_add(type->typ_name, lustre_kobj);
+	if (!type->typ_kobj) {
+		rc = -ENOMEM;
 		goto failed;
 	}
 
@@ -213,13 +221,12 @@ int class_register_type(struct obd_ops *dt_ops, struct md_ops *md_ops,
 	return 0;
 
  failed:
-	if (type->typ_name != NULL)
-		OBD_FREE(type->typ_name, strlen(name) + 1);
-	if (type->typ_md_ops != NULL)
-		OBD_FREE_PTR(type->typ_md_ops);
-	if (type->typ_dt_ops != NULL)
-		OBD_FREE_PTR(type->typ_dt_ops);
-	OBD_FREE(type, sizeof(*type));
+	if (type->typ_kobj)
+		kobject_put(type->typ_kobj);
+	kfree(type->typ_name);
+	kfree(type->typ_md_ops);
+	kfree(type->typ_dt_ops);
+	kfree(type);
 	return rc;
 }
 EXPORT_SYMBOL(class_register_type);
@@ -237,14 +244,16 @@ int class_unregister_type(const char *name)
 		CERROR("type %s has refcount (%d)\n", name, type->typ_refcnt);
 		/* This is a bad situation, let's make the best of it */
 		/* Remove ops, but leave the name for debugging */
-		OBD_FREE_PTR(type->typ_dt_ops);
-		OBD_FREE_PTR(type->typ_md_ops);
+		kfree(type->typ_dt_ops);
+		kfree(type->typ_md_ops);
 		return -EBUSY;
 	}
 
-	if (type->typ_procroot) {
-		lprocfs_remove(&type->typ_procroot);
-	}
+	if (type->typ_kobj)
+		kobject_put(type->typ_kobj);
+
+	if (!IS_ERR_OR_NULL(type->typ_debugfs_entry))
+		ldebugfs_remove(&type->typ_debugfs_entry);
 
 	if (type->typ_lu)
 		lu_device_type_fini(type->typ_lu);
@@ -252,12 +261,10 @@ int class_unregister_type(const char *name)
 	spin_lock(&obd_types_lock);
 	list_del(&type->typ_chain);
 	spin_unlock(&obd_types_lock);
-	OBD_FREE(type->typ_name, strlen(name) + 1);
-	if (type->typ_dt_ops != NULL)
-		OBD_FREE_PTR(type->typ_dt_ops);
-	if (type->typ_md_ops != NULL)
-		OBD_FREE_PTR(type->typ_md_ops);
-	OBD_FREE(type, sizeof(*type));
+	kfree(type->typ_name);
+	kfree(type->typ_dt_ops);
+	kfree(type->typ_md_ops);
+	kfree(type);
 	return 0;
 } /* class_unregister_type */
 EXPORT_SYMBOL(class_unregister_type);
@@ -819,7 +826,7 @@ struct obd_export *class_new_export(struct obd_device *obd,
 	struct cfs_hash *hash = NULL;
 	int rc = 0;
 
-	OBD_ALLOC_PTR(export);
+	export = kzalloc(sizeof(*export), GFP_NOFS);
 	if (!export)
 		return ERR_PTR(-ENOMEM);
 
@@ -904,7 +911,7 @@ exit_err:
 	class_handle_unhash(&export->exp_handle);
 	LASSERT(hlist_unhashed(&export->exp_uuid_hash));
 	obd_destroy_export(export);
-	OBD_FREE_PTR(export);
+	kfree(export);
 	return ERR_PTR(rc);
 }
 EXPORT_SYMBOL(class_new_export);
@@ -945,7 +952,7 @@ static void class_import_destroy(struct obd_import *imp)
 					  struct obd_import_conn, oic_item);
 		list_del_init(&imp_conn->oic_item);
 		ptlrpc_put_connection_superhack(imp_conn->oic_conn);
-		OBD_FREE(imp_conn, sizeof(*imp_conn));
+		kfree(imp_conn);
 	}
 
 	LASSERT(imp->imp_sec == NULL);
@@ -1008,7 +1015,7 @@ struct obd_import *class_new_import(struct obd_device *obd)
 {
 	struct obd_import *imp;
 
-	OBD_ALLOC(imp, sizeof(*imp));
+	imp = kzalloc(sizeof(*imp), GFP_NOFS);
 	if (imp == NULL)
 		return NULL;
 
@@ -1811,7 +1818,7 @@ void *kuc_alloc(int payload_len, int transport, int type)
 	struct kuc_hdr *lh;
 	int len = kuc_len(payload_len);
 
-	OBD_ALLOC(lh, len);
+	lh = kzalloc(len, GFP_NOFS);
 	if (lh == NULL)
 		return ERR_PTR(-ENOMEM);
 
@@ -1828,6 +1835,6 @@ EXPORT_SYMBOL(kuc_alloc);
 inline void kuc_free(void *p, int payload_len)
 {
 	struct kuc_hdr *lh = kuc_ptr(p);
-	OBD_FREE(lh, kuc_len(payload_len));
+	kfree(lh);
 }
 EXPORT_SYMBOL(kuc_free);
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
index 06944b863d16..84f75dce0d4c 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
@@ -53,7 +53,6 @@
 #include <linux/fcntl.h>
 #include <linux/delay.h>
 #include <linux/skbuff.h>
-#include <linux/proc_fs.h>
 #include <linux/fs.h>
 #include <linux/poll.h>
 #include <linux/list.h>
@@ -64,6 +63,7 @@
 #include <linux/uaccess.h>
 #include <linux/miscdevice.h>
 #include <linux/seq_file.h>
+#include <linux/kobject.h>
 
 #include "../../../include/linux/libcfs/libcfs.h"
 #include "../../../include/linux/lnet/lnetctl.h"
@@ -107,7 +107,7 @@ int obd_ioctl_getdata(char **buf, int *len, void *arg)
 	 * system, the high lock contention will hurt performance badly,
 	 * obdfilter-survey is an example, which relies on ioctl. So we'd
 	 * better avoid vmalloc on ioctl path. LU-66 */
-	OBD_ALLOC_LARGE(*buf, hdr.ioc_len);
+	*buf = libcfs_kvzalloc(hdr.ioc_len, GFP_NOFS);
 	if (*buf == NULL) {
 		CERROR("Cannot allocate control buffer of len %d\n",
 		       hdr.ioc_len);
@@ -153,7 +153,7 @@ int obd_ioctl_getdata(char **buf, int *len, void *arg)
 	return 0;
 
 free_buf:
-	OBD_FREE_LARGE(*buf, hdr.ioc_len);
+	kvfree(*buf);
 	return err;
 }
 EXPORT_SYMBOL(obd_ioctl_getdata);
@@ -216,29 +216,27 @@ struct miscdevice obd_psdev = {
 };
 
 
-#if defined (CONFIG_PROC_FS)
-static int obd_proc_version_seq_show(struct seq_file *m, void *v)
+static ssize_t version_show(struct kobject *kobj, struct attribute *attr,
+			    char *buf)
 {
-	seq_printf(m, "lustre: %s\nkernel: %s\nbuild:  %s\n",
-		   LUSTRE_VERSION_STRING, "patchless_client", BUILD_VERSION);
-	return 0;
+	return sprintf(buf, "%s\n", LUSTRE_VERSION_STRING);
 }
-LPROC_SEQ_FOPS_RO(obd_proc_version);
 
-int obd_proc_pinger_seq_show(struct seq_file *m, void *v)
+static ssize_t pinger_show(struct kobject *kobj, struct attribute *attr,
+			   char *buf)
 {
-	seq_printf(m, "%s\n", "on");
-	return 0;
+	return sprintf(buf, "%s\n", "on");
 }
-LPROC_SEQ_FOPS_RO(obd_proc_pinger);
 
-static int obd_proc_health_seq_show(struct seq_file *m, void *v)
+static ssize_t health_show(struct kobject *kobj, struct attribute *attr,
+			   char *buf)
 {
 	bool healthy = true;
 	int i;
+	size_t len = 0;
 
 	if (libcfs_catastrophe)
-		seq_printf(m, "LBUG\n");
+		return sprintf(buf, "LBUG\n");
 
 	read_lock(&obd_dev_lock);
 	for (i = 0; i < class_devno_max(); i++) {
@@ -256,8 +254,6 @@ static int obd_proc_health_seq_show(struct seq_file *m, void *v)
 		read_unlock(&obd_dev_lock);
 
 		if (obd_health_check(NULL, obd)) {
-			seq_printf(m, "device %s reported unhealthy\n",
-				   obd->obd_name);
 			healthy = false;
 		}
 		class_decref(obd, __func__, current);
@@ -266,32 +262,29 @@ static int obd_proc_health_seq_show(struct seq_file *m, void *v)
 	read_unlock(&obd_dev_lock);
 
 	if (healthy)
-		seq_puts(m, "healthy\n");
+		len = sprintf(buf, "healthy\n");
 	else
-		seq_puts(m, "NOT HEALTHY\n");
+		len = sprintf(buf, "NOT HEALTHY\n");
 
-	return 0;
+	return len;
 }
-LPROC_SEQ_FOPS_RO(obd_proc_health);
 
-static int obd_proc_jobid_var_seq_show(struct seq_file *m, void *v)
+static ssize_t jobid_var_show(struct kobject *kobj, struct attribute *attr,
+			      char *buf)
 {
-	seq_printf(m, "%s\n", obd_jobid_var);
-	return 0;
+	return snprintf(buf, PAGE_SIZE, "%s\n", obd_jobid_var);
 }
 
-static ssize_t obd_proc_jobid_var_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t jobid_var_store(struct kobject *kobj, struct attribute *attr,
+			       const char *buffer,
+			       size_t count)
 {
 	if (!count || count > JOBSTATS_JOBID_VAR_MAX_LEN)
 		return -EINVAL;
 
 	memset(obd_jobid_var, 0, JOBSTATS_JOBID_VAR_MAX_LEN + 1);
 
-	/* This might leave the var invalid on error, which is probably fine.*/
-	if (copy_from_user(obd_jobid_var, buffer, count))
-		return -EFAULT;
+	memcpy(obd_jobid_var, buffer, count);
 
 	/* Trim the trailing '\n' if any */
 	if (obd_jobid_var[count - 1] == '\n')
@@ -299,23 +292,21 @@ static ssize_t obd_proc_jobid_var_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(obd_proc_jobid_var);
 
-static int obd_proc_jobid_name_seq_show(struct seq_file *m, void *v)
+static ssize_t jobid_name_show(struct kobject *kobj, struct attribute *attr,
+			       char *buf)
 {
-	seq_printf(m, "%s\n", obd_jobid_var);
-	return 0;
+	return snprintf(buf, PAGE_SIZE, "%s\n", obd_jobid_node);
 }
 
-static ssize_t obd_proc_jobid_name_seq_write(struct file *file,
-					     const char __user *buffer,
-					     size_t count, loff_t *off)
+static ssize_t jobid_name_store(struct kobject *kobj, struct attribute *attr,
+				const char *buffer,
+				size_t count)
 {
 	if (!count || count > JOBSTATS_JOBID_SIZE)
 		return -EINVAL;
 
-	if (copy_from_user(obd_jobid_node, buffer, count))
-		return -EFAULT;
+	memcpy(obd_jobid_node, buffer, count);
 
 	obd_jobid_node[count] = 0;
 
@@ -325,20 +316,24 @@ static ssize_t obd_proc_jobid_name_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(obd_proc_jobid_name);
-
-/* Root for /proc/fs/lustre */
-struct proc_dir_entry *proc_lustre_root = NULL;
-EXPORT_SYMBOL(proc_lustre_root);
-
-struct lprocfs_vars lprocfs_base[] = {
-	{ "version", &obd_proc_version_fops },
-	{ "pinger", &obd_proc_pinger_fops },
-	{ "health_check", &obd_proc_health_fops },
-	{ "jobid_var", &obd_proc_jobid_var_fops },
-	{ .name =	"jobid_name",
-	  .fops =	&obd_proc_jobid_name_fops},
-	{ NULL }
+
+/* Root for /sys/kernel/debug/lustre */
+struct dentry *debugfs_lustre_root;
+EXPORT_SYMBOL_GPL(debugfs_lustre_root);
+
+LUSTRE_RO_ATTR(version);
+LUSTRE_RO_ATTR(pinger);
+LUSTRE_RO_ATTR(health);
+LUSTRE_RW_ATTR(jobid_var);
+LUSTRE_RW_ATTR(jobid_name);
+
+static struct attribute *lustre_attrs[] = {
+	&lustre_attr_version.attr,
+	&lustre_attr_pinger.attr,
+	&lustre_attr_health.attr,
+	&lustre_attr_jobid_name.attr,
+	&lustre_attr_jobid_var.attr,
+	NULL,
 };
 
 static void *obd_device_list_seq_start(struct seq_file *p, loff_t *pos)
@@ -406,7 +401,7 @@ static int obd_device_list_open(struct inode *inode, struct file *file)
 		return rc;
 
 	seq = file->private_data;
-	seq->private = PDE_DATA(inode);
+	seq->private = inode->i_private;
 
 	return 0;
 }
@@ -419,31 +414,57 @@ struct file_operations obd_device_list_fops = {
 	.release = seq_release,
 };
 
+struct kobject *lustre_kobj;
+EXPORT_SYMBOL_GPL(lustre_kobj);
+
+static struct attribute_group lustre_attr_group = {
+	.attrs = lustre_attrs,
+};
+
 int class_procfs_init(void)
 {
 	int rc = 0;
+	struct dentry *file;
+
+	lustre_kobj = kobject_create_and_add("lustre", fs_kobj);
+	if (lustre_kobj == NULL)
+		goto out;
+
+	/* Create the files associated with this kobject */
+	rc = sysfs_create_group(lustre_kobj, &lustre_attr_group);
+	if (rc) {
+		kobject_put(lustre_kobj);
+		goto out;
+	}
 
-	proc_lustre_root = lprocfs_register("fs/lustre", NULL,
-					    lprocfs_base, NULL);
-	if (IS_ERR(proc_lustre_root)) {
-		rc = PTR_ERR(proc_lustre_root);
-		proc_lustre_root = NULL;
+	debugfs_lustre_root = debugfs_create_dir("lustre", NULL);
+	if (IS_ERR_OR_NULL(debugfs_lustre_root)) {
+		rc = debugfs_lustre_root ? PTR_ERR(debugfs_lustre_root)
+					 : -ENOMEM;
+		debugfs_lustre_root = NULL;
+		kobject_put(lustre_kobj);
 		goto out;
 	}
 
-	rc = lprocfs_seq_create(proc_lustre_root, "devices", 0444,
-				&obd_device_list_fops, NULL);
+	file = debugfs_create_file("devices", 0444, debugfs_lustre_root, NULL,
+				   &obd_device_list_fops);
+	if (IS_ERR_OR_NULL(file)) {
+		rc = file ? PTR_ERR(file) : -ENOMEM;
+		kobject_put(lustre_kobj);
+		goto out;
+	}
 out:
-	if (rc)
-		CERROR("error adding /proc/fs/lustre/devices file\n");
-	return 0;
+	return rc;
 }
 
 int class_procfs_clean(void)
 {
-	if (proc_lustre_root) {
-		lprocfs_remove(&proc_lustre_root);
-	}
+	if (debugfs_lustre_root != NULL)
+		debugfs_remove_recursive(debugfs_lustre_root);
+
+	debugfs_lustre_root = NULL;
+
+	kobject_put(lustre_kobj);
+
 	return 0;
 }
-#endif /* CONFIG_PROC_FS */
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
index 4b62d25764ad..54f0a81f7b51 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
@@ -38,7 +38,6 @@
 #include <linux/sysctl.h>
 #include <linux/sched.h>
 #include <linux/mm.h>
-#include <linux/proc_fs.h>
 #include <linux/slab.h>
 #include <linux/stat.h>
 #include <linux/ctype.h>
@@ -55,30 +54,6 @@
 static struct ctl_table_header *obd_table_header;
 #endif
 
-
-#define OBD_SYSCTL 300
-
-enum {
-	OBD_TIMEOUT = 3,	/* RPC timeout before recovery/intr */
-	OBD_DUMP_ON_TIMEOUT,    /* dump kernel debug log upon eviction */
-	OBD_MEMUSED,	    /* bytes currently OBD_ALLOCated */
-	OBD_PAGESUSED,	  /* pages currently OBD_PAGE_ALLOCated */
-	OBD_MAXMEMUSED,	 /* maximum bytes OBD_ALLOCated concurrently */
-	OBD_MAXPAGESUSED,       /* maximum pages OBD_PAGE_ALLOCated concurrently */
-	OBD_SYNCFILTER,	 /* XXX temporary, as we play with sync osts.. */
-	OBD_LDLM_TIMEOUT,       /* LDLM timeout for ASTs before client eviction */
-	OBD_DUMP_ON_EVICTION,   /* dump kernel debug log upon eviction */
-	OBD_DEBUG_PEER_ON_TIMEOUT, /* dump peer debug when RPC times out */
-	OBD_ALLOC_FAIL_RATE,    /* memory allocation random failure rate */
-	OBD_MAX_DIRTY_PAGES,    /* maximum dirty pages */
-	OBD_AT_MIN,	     /* Adaptive timeouts params */
-	OBD_AT_MAX,
-	OBD_AT_EXTRA,
-	OBD_AT_EARLY_MARGIN,
-	OBD_AT_HISTORY,
-};
-
-
 #ifdef CONFIG_SYSCTL
 static int proc_set_timeout(struct ctl_table *table, int write,
 			void __user *buffer, size_t *lenp, loff_t *ppos)
diff --git a/drivers/staging/lustre/lustre/obdclass/llog.c b/drivers/staging/lustre/lustre/obdclass/llog.c
index 114be4a78ccf..4fa52d1b79d1 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog.c
@@ -60,7 +60,7 @@ static struct llog_handle *llog_alloc_handle(void)
 {
 	struct llog_handle *loghandle;
 
-	OBD_ALLOC_PTR(loghandle);
+	loghandle = kzalloc(sizeof(*loghandle), GFP_NOFS);
 	if (loghandle == NULL)
 		return NULL;
 
@@ -88,9 +88,9 @@ static void llog_free_handle(struct llog_handle *loghandle)
 	else if (loghandle->lgh_hdr->llh_flags & LLOG_F_IS_CAT)
 		LASSERT(list_empty(&loghandle->u.chd.chd_head));
 	LASSERT(sizeof(*(loghandle->lgh_hdr)) == LLOG_CHUNK_SIZE);
-	OBD_FREE(loghandle->lgh_hdr, LLOG_CHUNK_SIZE);
+	kfree(loghandle->lgh_hdr);
 out:
-	OBD_FREE_PTR(loghandle);
+	kfree(loghandle);
 }
 
 void llog_handle_get(struct llog_handle *loghandle)
@@ -207,7 +207,7 @@ int llog_init_handle(const struct lu_env *env, struct llog_handle *handle,
 
 	LASSERT(handle->lgh_hdr == NULL);
 
-	OBD_ALLOC_PTR(llh);
+	llh = kzalloc(sizeof(*llh), GFP_NOFS);
 	if (llh == NULL)
 		return -ENOMEM;
 	handle->lgh_hdr = llh;
@@ -261,7 +261,7 @@ int llog_init_handle(const struct lu_env *env, struct llog_handle *handle,
 	}
 out:
 	if (rc) {
-		OBD_FREE_PTR(llh);
+		kfree(llh);
 		handle->lgh_hdr = NULL;
 	}
 	return rc;
@@ -283,7 +283,7 @@ static int llog_process_thread(void *arg)
 
 	LASSERT(llh);
 
-	OBD_ALLOC(buf, LLOG_CHUNK_SIZE);
+	buf = kzalloc(LLOG_CHUNK_SIZE, GFP_NOFS);
 	if (!buf) {
 		lpi->lpi_rc = -ENOMEM;
 		return 0;
@@ -400,7 +400,7 @@ out:
 	if (cd != NULL)
 		cd->lpcd_last_idx = last_called_index;
 
-	OBD_FREE(buf, LLOG_CHUNK_SIZE);
+	kfree(buf);
 	lpi->lpi_rc = rc;
 	return 0;
 }
@@ -434,7 +434,7 @@ int llog_process_or_fork(const struct lu_env *env,
 	struct llog_process_info *lpi;
 	int		      rc;
 
-	OBD_ALLOC_PTR(lpi);
+	lpi = kzalloc(sizeof(*lpi), GFP_NOFS);
 	if (lpi == NULL) {
 		CERROR("cannot alloc pointer\n");
 		return -ENOMEM;
@@ -454,7 +454,7 @@ int llog_process_or_fork(const struct lu_env *env,
 		if (IS_ERR_VALUE(rc)) {
 			CERROR("%s: cannot start thread: rc = %d\n",
 			       loghandle->lgh_ctxt->loc_obd->obd_name, rc);
-			OBD_FREE_PTR(lpi);
+			kfree(lpi);
 			return rc;
 		}
 		wait_for_completion(&lpi->lpi_completion);
@@ -463,7 +463,7 @@ int llog_process_or_fork(const struct lu_env *env,
 		llog_process_thread(lpi);
 	}
 	rc = lpi->lpi_rc;
-	OBD_FREE_PTR(lpi);
+	kfree(lpi);
 	return rc;
 }
 EXPORT_SYMBOL(llog_process_or_fork);
@@ -484,7 +484,7 @@ int llog_reverse_process(const struct lu_env *env,
 	void *buf;
 	int rc = 0, first_index = 1, index, idx;
 
-	OBD_ALLOC(buf, LLOG_CHUNK_SIZE);
+	buf = kzalloc(LLOG_CHUNK_SIZE, GFP_NOFS);
 	if (!buf)
 		return -ENOMEM;
 
@@ -563,8 +563,7 @@ int llog_reverse_process(const struct lu_env *env,
 	}
 
 out:
-	if (buf)
-		OBD_FREE(buf, LLOG_CHUNK_SIZE);
+	kfree(buf);
 	return rc;
 }
 EXPORT_SYMBOL(llog_reverse_process);
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_cat.c b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
index c8f6ab006124..48dbbcf97702 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_cat.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
@@ -279,9 +279,8 @@ static struct llog_handle *llog_cat_current_log(struct llog_handle *cathandle,
 		    loghandle->lgh_last_idx < LLOG_BITMAP_SIZE(llh) - 1) {
 			up_read(&cathandle->lgh_lock);
 			return loghandle;
-		} else {
-			up_write(&loghandle->lgh_lock);
 		}
+		up_write(&loghandle->lgh_lock);
 	}
 	up_read(&cathandle->lgh_lock);
 
@@ -299,9 +298,8 @@ static struct llog_handle *llog_cat_current_log(struct llog_handle *cathandle,
 		if (loghandle->lgh_last_idx < LLOG_BITMAP_SIZE(llh) - 1) {
 			up_write(&cathandle->lgh_lock);
 			return loghandle;
-		} else {
-			up_write(&loghandle->lgh_lock);
 		}
+		up_write(&loghandle->lgh_lock);
 	}
 
 	CDEBUG(D_INODE, "use next log\n");
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_obd.c b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
index 978d886a1103..81ab27e7376f 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
@@ -46,7 +46,7 @@ static struct llog_ctxt *llog_new_ctxt(struct obd_device *obd)
 {
 	struct llog_ctxt *ctxt;
 
-	OBD_ALLOC_PTR(ctxt);
+	ctxt = kzalloc(sizeof(*ctxt), GFP_NOFS);
 	if (!ctxt)
 		return NULL;
 
@@ -66,7 +66,7 @@ static void llog_ctxt_destroy(struct llog_ctxt *ctxt)
 		class_import_put(ctxt->loc_imp);
 		ctxt->loc_imp = NULL;
 	}
-	OBD_FREE_PTR(ctxt);
+	kfree(ctxt);
 }
 
 int __llog_ctxt_put(const struct lu_env *env, struct llog_ctxt *ctxt)
diff --git a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
index c171c6c6c457..17e7c1807863 100644
--- a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
+++ b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
@@ -221,8 +221,6 @@ int lprocfs_write_frac_helper(const char __user *buffer, unsigned long count,
 }
 EXPORT_SYMBOL(lprocfs_write_frac_helper);
 
-#if defined (CONFIG_PROC_FS)
-
 static int lprocfs_no_percpu_stats;
 module_param(lprocfs_no_percpu_stats, int, 0644);
 MODULE_PARM_DESC(lprocfs_no_percpu_stats, "Do not alloc percpu data for lprocfs stats");
@@ -243,11 +241,11 @@ EXPORT_SYMBOL(lprocfs_seq_release);
 
 /* lprocfs API calls */
 
-struct proc_dir_entry *lprocfs_add_simple(struct proc_dir_entry *root,
-				     char *name, void *data,
-				     struct file_operations *fops)
+struct dentry *ldebugfs_add_simple(struct dentry *root,
+				   char *name, void *data,
+				   struct file_operations *fops)
 {
-	struct proc_dir_entry *proc;
+	struct dentry *entry;
 	umode_t mode = 0;
 
 	if (root == NULL || name == NULL || fops == NULL)
@@ -257,26 +255,26 @@ struct proc_dir_entry *lprocfs_add_simple(struct proc_dir_entry *root,
 		mode = 0444;
 	if (fops->write)
 		mode |= 0200;
-	proc = proc_create_data(name, mode, root, fops, data);
-	if (!proc) {
-		CERROR("LprocFS: No memory to create /proc entry %s", name);
-		return ERR_PTR(-ENOMEM);
+	entry = debugfs_create_file(name, mode, root, data, fops);
+	if (IS_ERR_OR_NULL(entry)) {
+		CERROR("LprocFS: No memory to create <debugfs> entry %s", name);
+		return entry ?: ERR_PTR(-ENOMEM);
 	}
-	return proc;
+	return entry;
 }
-EXPORT_SYMBOL(lprocfs_add_simple);
+EXPORT_SYMBOL(ldebugfs_add_simple);
 
-struct proc_dir_entry *lprocfs_add_symlink(const char *name,
-			struct proc_dir_entry *parent, const char *format, ...)
+struct dentry *ldebugfs_add_symlink(const char *name, struct dentry *parent,
+				    const char *format, ...)
 {
-	struct proc_dir_entry *entry;
+	struct dentry *entry;
 	char *dest;
 	va_list ap;
 
 	if (parent == NULL || format == NULL)
 		return NULL;
 
-	OBD_ALLOC_WAIT(dest, MAX_STRING_SIZE + 1);
+	dest = kzalloc(MAX_STRING_SIZE + 1, GFP_KERNEL);
 	if (dest == NULL)
 		return NULL;
 
@@ -284,37 +282,29 @@ struct proc_dir_entry *lprocfs_add_symlink(const char *name,
 	vsnprintf(dest, MAX_STRING_SIZE, format, ap);
 	va_end(ap);
 
-	entry = proc_symlink(name, parent, dest);
-	if (entry == NULL)
-		CERROR("LprocFS: Could not create symbolic link from %s to %s",
+	entry = debugfs_create_symlink(name, parent, dest);
+	if (IS_ERR_OR_NULL(entry)) {
+		CERROR("LdebugFS: Could not create symbolic link from %s to %s",
 			name, dest);
+		entry = NULL;
+	}
 
-	OBD_FREE(dest, MAX_STRING_SIZE + 1);
+	kfree(dest);
 	return entry;
 }
-EXPORT_SYMBOL(lprocfs_add_symlink);
+EXPORT_SYMBOL(ldebugfs_add_symlink);
 
 static struct file_operations lprocfs_generic_fops = { };
 
-/**
- * Add /proc entries.
- *
- * \param root [in]  The parent proc entry on which new entry will be added.
- * \param list [in]  Array of proc entries to be added.
- * \param data [in]  The argument to be passed when entries read/write routines
- *		   are called through /proc file.
- *
- * \retval 0   on success
- *	 < 0 on error
- */
-int lprocfs_add_vars(struct proc_dir_entry *root, struct lprocfs_vars *list,
-		     void *data)
+int ldebugfs_add_vars(struct dentry *parent,
+		      struct lprocfs_vars *list,
+		      void *data)
 {
-	if (root == NULL || list == NULL)
+	if (IS_ERR_OR_NULL(parent) || IS_ERR_OR_NULL(list))
 		return -EINVAL;
 
 	while (list->name != NULL) {
-		struct proc_dir_entry *proc;
+		struct dentry *entry;
 		umode_t mode = 0;
 
 		if (list->proc_mode != 0000) {
@@ -325,54 +315,50 @@ int lprocfs_add_vars(struct proc_dir_entry *root, struct lprocfs_vars *list,
 			if (list->fops->write)
 				mode |= 0200;
 		}
-		proc = proc_create_data(list->name, mode, root,
-					list->fops ?: &lprocfs_generic_fops,
-					list->data ?: data);
-		if (proc == NULL)
-			return -ENOMEM;
+		entry = debugfs_create_file(list->name, mode, parent,
+					    list->data ?: data,
+					    list->fops ?: &lprocfs_generic_fops
+					   );
+		if (IS_ERR_OR_NULL(entry))
+			return entry ? PTR_ERR(entry) : -ENOMEM;
 		list++;
 	}
 	return 0;
 }
-EXPORT_SYMBOL(lprocfs_add_vars);
-
-void lprocfs_remove(struct proc_dir_entry **rooth)
-{
-	proc_remove(*rooth);
-	*rooth = NULL;
-}
-EXPORT_SYMBOL(lprocfs_remove);
+EXPORT_SYMBOL(ldebugfs_add_vars);
 
-void lprocfs_remove_proc_entry(const char *name, struct proc_dir_entry *parent)
+void ldebugfs_remove(struct dentry **entryp)
 {
-	LASSERT(parent != NULL);
-	remove_proc_entry(name, parent);
+	debugfs_remove(*entryp);
+	*entryp = NULL;
 }
-EXPORT_SYMBOL(lprocfs_remove_proc_entry);
+EXPORT_SYMBOL(ldebugfs_remove);
 
-struct proc_dir_entry *lprocfs_register(const char *name,
-					struct proc_dir_entry *parent,
-					struct lprocfs_vars *list, void *data)
+struct dentry *ldebugfs_register(const char *name,
+				 struct dentry *parent,
+				 struct lprocfs_vars *list, void *data)
 {
-	struct proc_dir_entry *entry;
+	struct dentry *entry;
 
-	entry = proc_mkdir(name, parent);
-	if (entry == NULL) {
-		entry = ERR_PTR(-ENOMEM);
+	entry = debugfs_create_dir(name, parent);
+	if (IS_ERR_OR_NULL(entry)) {
+		entry = entry ?: ERR_PTR(-ENOMEM);
 		goto out;
 	}
 
-	if (list != NULL) {
-		int rc = lprocfs_add_vars(entry, list, data);
+	if (!IS_ERR_OR_NULL(list)) {
+		int rc;
+
+		rc = ldebugfs_add_vars(entry, list, data);
 		if (rc != 0) {
-			lprocfs_remove(&entry);
+			debugfs_remove(entry);
 			entry = ERR_PTR(rc);
 		}
 	}
 out:
 	return entry;
 }
-EXPORT_SYMBOL(lprocfs_register);
+EXPORT_SYMBOL(ldebugfs_register);
 
 /* Generic callbacks */
 int lprocfs_rd_uint(struct seq_file *m, void *data)
@@ -437,15 +423,15 @@ int lprocfs_wr_atomic(struct file *file, const char __user *buffer,
 }
 EXPORT_SYMBOL(lprocfs_wr_atomic);
 
-int lprocfs_rd_uuid(struct seq_file *m, void *data)
+static ssize_t uuid_show(struct kobject *kobj, struct attribute *attr,
+			 char *buf)
 {
-	struct obd_device *obd = data;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 
-	LASSERT(obd != NULL);
-	seq_printf(m, "%s\n", obd->obd_uuid.uuid);
-	return 0;
+	return sprintf(buf, "%s\n", obd->obd_uuid.uuid);
 }
-EXPORT_SYMBOL(lprocfs_rd_uuid);
+LUSTRE_RO_ATTR(uuid);
 
 int lprocfs_rd_name(struct seq_file *m, void *data)
 {
@@ -457,23 +443,27 @@ int lprocfs_rd_name(struct seq_file *m, void *data)
 }
 EXPORT_SYMBOL(lprocfs_rd_name);
 
-int lprocfs_rd_blksize(struct seq_file *m, void *data)
+static ssize_t blocksize_show(struct kobject *kobj, struct attribute *attr,
+			      char *buf)
 {
-	struct obd_device *obd = data;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct obd_statfs  osfs;
 	int rc = obd_statfs(NULL, obd->obd_self_export, &osfs,
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 			    OBD_STATFS_NODELAY);
 	if (!rc)
-		seq_printf(m, "%u\n", osfs.os_bsize);
+		return sprintf(buf, "%u\n", osfs.os_bsize);
 
 	return rc;
 }
-EXPORT_SYMBOL(lprocfs_rd_blksize);
+LUSTRE_RO_ATTR(blocksize);
 
-int lprocfs_rd_kbytestotal(struct seq_file *m, void *data)
+static ssize_t kbytestotal_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct obd_device *obd = data;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct obd_statfs  osfs;
 	int rc = obd_statfs(NULL, obd->obd_self_export, &osfs,
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
@@ -485,16 +475,18 @@ int lprocfs_rd_kbytestotal(struct seq_file *m, void *data)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		seq_printf(m, "%llu\n", result);
+		return sprintf(buf, "%llu\n", result);
 	}
 
 	return rc;
 }
-EXPORT_SYMBOL(lprocfs_rd_kbytestotal);
+LUSTRE_RO_ATTR(kbytestotal);
 
-int lprocfs_rd_kbytesfree(struct seq_file *m, void *data)
+static ssize_t kbytesfree_show(struct kobject *kobj, struct attribute *attr,
+			       char *buf)
 {
-	struct obd_device *obd = data;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct obd_statfs  osfs;
 	int rc = obd_statfs(NULL, obd->obd_self_export, &osfs,
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
@@ -506,16 +498,18 @@ int lprocfs_rd_kbytesfree(struct seq_file *m, void *data)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		seq_printf(m, "%llu\n", result);
+		return sprintf(buf, "%llu\n", result);
 	}
 
 	return rc;
 }
-EXPORT_SYMBOL(lprocfs_rd_kbytesfree);
+LUSTRE_RO_ATTR(kbytesfree);
 
-int lprocfs_rd_kbytesavail(struct seq_file *m, void *data)
+static ssize_t kbytesavail_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct obd_device *obd = data;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct obd_statfs  osfs;
 	int rc = obd_statfs(NULL, obd->obd_self_export, &osfs,
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
@@ -527,40 +521,44 @@ int lprocfs_rd_kbytesavail(struct seq_file *m, void *data)
 		while (blk_size >>= 1)
 			result <<= 1;
 
-		seq_printf(m, "%llu\n", result);
+		return sprintf(buf, "%llu\n", result);
 	}
 
 	return rc;
 }
-EXPORT_SYMBOL(lprocfs_rd_kbytesavail);
+LUSTRE_RO_ATTR(kbytesavail);
 
-int lprocfs_rd_filestotal(struct seq_file *m, void *data)
+static ssize_t filestotal_show(struct kobject *kobj, struct attribute *attr,
+			       char *buf)
 {
-	struct obd_device *obd = data;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct obd_statfs  osfs;
 	int rc = obd_statfs(NULL, obd->obd_self_export, &osfs,
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 			    OBD_STATFS_NODELAY);
 	if (!rc)
-		seq_printf(m, "%llu\n", osfs.os_files);
+		return sprintf(buf, "%llu\n", osfs.os_files);
 
 	return rc;
 }
-EXPORT_SYMBOL(lprocfs_rd_filestotal);
+LUSTRE_RO_ATTR(filestotal);
 
-int lprocfs_rd_filesfree(struct seq_file *m, void *data)
+static ssize_t filesfree_show(struct kobject *kobj, struct attribute *attr,
+			      char *buf)
 {
-	struct obd_device *obd = data;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct obd_statfs  osfs;
 	int rc = obd_statfs(NULL, obd->obd_self_export, &osfs,
 			    cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 			    OBD_STATFS_NODELAY);
 	if (!rc)
-		seq_printf(m, "%llu\n", osfs.os_ffree);
+		return sprintf(buf, "%llu\n", osfs.os_ffree);
 
 	return rc;
 }
-EXPORT_SYMBOL(lprocfs_rd_filesfree);
+LUSTRE_RO_ATTR(filesfree);
 
 int lprocfs_rd_server_uuid(struct seq_file *m, void *data)
 {
@@ -930,43 +928,62 @@ int lprocfs_rd_connect_flags(struct seq_file *m, void *data)
 }
 EXPORT_SYMBOL(lprocfs_rd_connect_flags);
 
-int lprocfs_rd_num_exports(struct seq_file *m, void *data)
-{
-	struct obd_device *obd = data;
-
-	LASSERT(obd != NULL);
-	seq_printf(m, "%u\n", obd->obd_num_exports);
-	return 0;
-}
-EXPORT_SYMBOL(lprocfs_rd_num_exports);
+static struct attribute *obd_def_attrs[] = {
+	&lustre_attr_blocksize.attr,
+	&lustre_attr_kbytestotal.attr,
+	&lustre_attr_kbytesfree.attr,
+	&lustre_attr_kbytesavail.attr,
+	&lustre_attr_filestotal.attr,
+	&lustre_attr_filesfree.attr,
+	&lustre_attr_uuid.attr,
+	NULL,
+};
 
-int lprocfs_rd_numrefs(struct seq_file *m, void *data)
+static void obd_sysfs_release(struct kobject *kobj)
 {
-	struct obd_type *class = (struct obd_type *) data;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 
-	LASSERT(class != NULL);
-	seq_printf(m, "%d\n", class->typ_refcnt);
-	return 0;
+	complete(&obd->obd_kobj_unregister);
 }
-EXPORT_SYMBOL(lprocfs_rd_numrefs);
 
-int lprocfs_obd_setup(struct obd_device *obd, struct lprocfs_vars *list)
+static struct kobj_type obd_ktype = {
+	.default_attrs	= obd_def_attrs,
+	.sysfs_ops	= &lustre_sysfs_ops,
+	.release	= obd_sysfs_release,
+};
+
+int lprocfs_obd_setup(struct obd_device *obd, struct lprocfs_vars *list,
+		      struct attribute_group *attrs)
 {
 	int rc = 0;
 
-	LASSERT(obd != NULL);
-	LASSERT(obd->obd_magic == OBD_DEVICE_MAGIC);
-	LASSERT(obd->obd_type->typ_procroot != NULL);
-
-	obd->obd_proc_entry = lprocfs_register(obd->obd_name,
-					       obd->obd_type->typ_procroot,
-					       list, obd);
-	if (IS_ERR(obd->obd_proc_entry)) {
-		rc = PTR_ERR(obd->obd_proc_entry);
+	init_completion(&obd->obd_kobj_unregister);
+	rc = kobject_init_and_add(&obd->obd_kobj, &obd_ktype,
+				  obd->obd_type->typ_kobj,
+				  "%s", obd->obd_name);
+	if (rc)
+		return rc;
+
+	if (attrs) {
+		rc = sysfs_create_group(&obd->obd_kobj, attrs);
+		if (rc) {
+			kobject_put(&obd->obd_kobj);
+			return rc;
+		}
+	}
+
+	obd->obd_debugfs_entry = ldebugfs_register(obd->obd_name,
+						   obd->obd_type->typ_debugfs_entry,
+						   list, obd);
+	if (IS_ERR_OR_NULL(obd->obd_debugfs_entry)) {
+		rc = obd->obd_debugfs_entry ? PTR_ERR(obd->obd_debugfs_entry)
+					    : -ENOMEM;
 		CERROR("error %d setting up lprocfs for %s\n",
 		       rc, obd->obd_name);
-		obd->obd_proc_entry = NULL;
+		obd->obd_debugfs_entry = NULL;
 	}
+
 	return rc;
 }
 EXPORT_SYMBOL(lprocfs_obd_setup);
@@ -975,58 +992,16 @@ int lprocfs_obd_cleanup(struct obd_device *obd)
 {
 	if (!obd)
 		return -EINVAL;
-	if (obd->obd_proc_exports_entry) {
-		/* Should be no exports left */
-		lprocfs_remove(&obd->obd_proc_exports_entry);
-		obd->obd_proc_exports_entry = NULL;
-	}
-	if (obd->obd_proc_entry) {
-		lprocfs_remove(&obd->obd_proc_entry);
-		obd->obd_proc_entry = NULL;
-	}
-	return 0;
-}
-EXPORT_SYMBOL(lprocfs_obd_cleanup);
-
-static void lprocfs_free_client_stats(struct nid_stat *client_stat)
-{
-	CDEBUG(D_CONFIG, "stat %p - data %p/%p\n", client_stat,
-	       client_stat->nid_proc, client_stat->nid_stats);
-
-	LASSERTF(atomic_read(&client_stat->nid_exp_ref_count) == 0,
-		 "nid %s:count %d\n", libcfs_nid2str(client_stat->nid),
-		 atomic_read(&client_stat->nid_exp_ref_count));
-
-	if (client_stat->nid_proc)
-		lprocfs_remove(&client_stat->nid_proc);
 
-	if (client_stat->nid_stats)
-		lprocfs_free_stats(&client_stat->nid_stats);
+	if (!IS_ERR_OR_NULL(obd->obd_debugfs_entry))
+		ldebugfs_remove(&obd->obd_debugfs_entry);
 
-	if (client_stat->nid_ldlm_stats)
-		lprocfs_free_stats(&client_stat->nid_ldlm_stats);
+	kobject_put(&obd->obd_kobj);
+	wait_for_completion(&obd->obd_kobj_unregister);
 
-	OBD_FREE_PTR(client_stat);
-	return;
-
-}
-
-void lprocfs_free_per_client_stats(struct obd_device *obd)
-{
-	struct cfs_hash *hash = obd->obd_nid_stats_hash;
-	struct nid_stat *stat;
-
-	/* we need extra list - because hash_exit called to early */
-	/* not need locking because all clients is died */
-	while (!list_empty(&obd->obd_nid_stats)) {
-		stat = list_entry(obd->obd_nid_stats.next,
-				      struct nid_stat, nid_list);
-		list_del_init(&stat->nid_list);
-		cfs_hash_del(hash, &stat->nid, &stat->nid_hash);
-		lprocfs_free_client_stats(stat);
-	}
+	return 0;
 }
-EXPORT_SYMBOL(lprocfs_free_per_client_stats);
+EXPORT_SYMBOL(lprocfs_obd_cleanup);
 
 int lprocfs_stats_alloc_one(struct lprocfs_stats *stats, unsigned int cpuid)
 {
@@ -1257,8 +1232,10 @@ static int lprocfs_stats_seq_open(struct inode *inode, struct file *file)
 	rc = seq_open(file, &lprocfs_stats_seq_sops);
 	if (rc)
 		return rc;
+
 	seq = file->private_data;
-	seq->private = PDE_DATA(inode);
+	seq->private = inode->i_private;
+
 	return 0;
 }
 
@@ -1271,20 +1248,21 @@ struct file_operations lprocfs_stats_seq_fops = {
 	.release = lprocfs_seq_release,
 };
 
-int lprocfs_register_stats(struct proc_dir_entry *root, const char *name,
+int ldebugfs_register_stats(struct dentry *parent, const char *name,
 			   struct lprocfs_stats *stats)
 {
-	struct proc_dir_entry *entry;
-	LASSERT(root != NULL);
+	struct dentry *entry;
 
-	entry = proc_create_data(name, 0644, root,
-				 &lprocfs_stats_seq_fops, stats);
-	if (entry == NULL)
-		return -ENOMEM;
+	LASSERT(!IS_ERR_OR_NULL(parent));
+
+	entry = debugfs_create_file(name, 0644, parent, stats,
+				    &lprocfs_stats_seq_fops);
+	if (IS_ERR_OR_NULL(entry))
+		return entry ? PTR_ERR(entry) : -ENOMEM;
 
 	return 0;
 }
-EXPORT_SYMBOL(lprocfs_register_stats);
+EXPORT_SYMBOL(ldebugfs_register_stats);
 
 void lprocfs_counter_init(struct lprocfs_stats *stats, int index,
 			  unsigned conf, const char *name, const char *units)
@@ -1388,7 +1366,7 @@ int lprocfs_alloc_obd_stats(struct obd_device *obd, unsigned num_private_stats)
 	int rc, i;
 
 	LASSERT(obd->obd_stats == NULL);
-	LASSERT(obd->obd_proc_entry != NULL);
+	LASSERT(obd->obd_debugfs_entry != NULL);
 	LASSERT(obd->obd_cntr_base == 0);
 
 	num_stats = ((int)sizeof(*obd->obd_type->typ_dt_ops) / sizeof(void *)) +
@@ -1409,7 +1387,7 @@ int lprocfs_alloc_obd_stats(struct obd_device *obd, unsigned 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);
+	rc = ldebugfs_register_stats(obd->obd_debugfs_entry, "stats", stats);
 	if (rc < 0) {
 		lprocfs_free_stats(&stats);
 	} else {
@@ -1479,7 +1457,7 @@ int lprocfs_alloc_md_stats(struct obd_device *obd,
 	int rc, i;
 
 	LASSERT(obd->md_stats == NULL);
-	LASSERT(obd->obd_proc_entry != NULL);
+	LASSERT(obd->obd_debugfs_entry != NULL);
 	LASSERT(obd->md_cntr_base == 0);
 
 	num_stats = 1 + MD_COUNTER_OFFSET(revalidate_lock) +
@@ -1497,7 +1475,7 @@ int lprocfs_alloc_md_stats(struct obd_device *obd,
 			LBUG();
 		}
 	}
-	rc = lprocfs_register_stats(obd->obd_proc_entry, "md_stats", stats);
+	rc = ldebugfs_register_stats(obd->obd_debugfs_entry, "md_stats", stats);
 	if (rc < 0) {
 		lprocfs_free_stats(&stats);
 	} else {
@@ -1543,241 +1521,8 @@ void lprocfs_init_ldlm_stats(struct lprocfs_stats *ldlm_stats)
 }
 EXPORT_SYMBOL(lprocfs_init_ldlm_stats);
 
-int lprocfs_exp_print_uuid(struct cfs_hash *hs, struct cfs_hash_bd *bd,
-			   struct hlist_node *hnode, void *data)
-
-{
-	struct obd_export *exp = cfs_hash_object(hs, hnode);
-	struct seq_file *m = (struct seq_file *)data;
-
-	if (exp->exp_nid_stats)
-		seq_printf(m, "%s\n", obd_uuid2str(&exp->exp_client_uuid));
-
-	return 0;
-}
-
-static int
-lproc_exp_uuid_seq_show(struct seq_file *m, void *unused)
-{
-	struct nid_stat *stats = (struct nid_stat *)m->private;
-	struct obd_device *obd = stats->nid_obd;
-
-	cfs_hash_for_each_key(obd->obd_nid_hash, &stats->nid,
-			      lprocfs_exp_print_uuid, m);
-	return 0;
-}
-
-LPROC_SEQ_FOPS_RO(lproc_exp_uuid);
-
-struct exp_hash_cb_data {
-	struct seq_file *m;
-	bool		first;
-};
-
-int lprocfs_exp_print_hash(struct cfs_hash *hs, struct cfs_hash_bd *bd,
-			   struct hlist_node *hnode, void *cb_data)
-
-{
-	struct exp_hash_cb_data *data = (struct exp_hash_cb_data *)cb_data;
-	struct obd_export       *exp = cfs_hash_object(hs, hnode);
-
-	if (exp->exp_lock_hash != NULL) {
-		if (data->first) {
-			cfs_hash_debug_header(data->m);
-			data->first = false;
-		}
-		cfs_hash_debug_str(hs, data->m);
-	}
-
-	return 0;
-}
-
-static int
-lproc_exp_hash_seq_show(struct seq_file *m, void *unused)
-{
-	struct nid_stat *stats = (struct nid_stat *)m->private;
-	struct obd_device *obd = stats->nid_obd;
-	struct exp_hash_cb_data cb_data = {
-		.m = m,
-		.first = true
-	};
-
-	cfs_hash_for_each_key(obd->obd_nid_hash, &stats->nid,
-			      lprocfs_exp_print_hash, &cb_data);
-	return 0;
-}
-
-LPROC_SEQ_FOPS_RO(lproc_exp_hash);
-
-int lprocfs_nid_stats_clear_read(struct seq_file *m, void *data)
-{
-	seq_printf(m, "%s\n",
-		   "Write into this file to clear all nid stats and stale nid entries");
-	return 0;
-}
-EXPORT_SYMBOL(lprocfs_nid_stats_clear_read);
-
-static int lprocfs_nid_stats_clear_write_cb(void *obj, void *data)
-{
-	struct nid_stat *stat = obj;
-
-	CDEBUG(D_INFO, "refcnt %d\n", atomic_read(&stat->nid_exp_ref_count));
-	if (atomic_read(&stat->nid_exp_ref_count) == 1) {
-		/* object has only hash references. */
-		spin_lock(&stat->nid_obd->obd_nid_lock);
-		list_move(&stat->nid_list, data);
-		spin_unlock(&stat->nid_obd->obd_nid_lock);
-		return 1;
-	}
-	/* we has reference to object - only clear data*/
-	if (stat->nid_stats)
-		lprocfs_clear_stats(stat->nid_stats);
-
-	return 0;
-}
-
-int lprocfs_nid_stats_clear_write(struct file *file, const char *buffer,
-				  unsigned long count, void *data)
-{
-	struct obd_device *obd = (struct obd_device *)data;
-	struct nid_stat *client_stat;
-	LIST_HEAD(free_list);
-
-	cfs_hash_cond_del(obd->obd_nid_stats_hash,
-			  lprocfs_nid_stats_clear_write_cb, &free_list);
-
-	while (!list_empty(&free_list)) {
-		client_stat = list_entry(free_list.next, struct nid_stat,
-					     nid_list);
-		list_del_init(&client_stat->nid_list);
-		lprocfs_free_client_stats(client_stat);
-	}
-
-	return count;
-}
-EXPORT_SYMBOL(lprocfs_nid_stats_clear_write);
-
-int lprocfs_exp_setup(struct obd_export *exp, lnet_nid_t *nid, int *newnid)
-{
-	struct nid_stat *new_stat, *old_stat;
-	struct obd_device *obd = NULL;
-	struct proc_dir_entry *entry;
-	char *buffer = NULL;
-	int rc = 0;
-
-	*newnid = 0;
-
-	if (!exp || !exp->exp_obd || !exp->exp_obd->obd_proc_exports_entry ||
-	    !exp->exp_obd->obd_nid_stats_hash)
-		return -EINVAL;
-
-	/* not test against zero because eric say:
-	 * You may only test nid against another nid, or LNET_NID_ANY.
-	 * Anything else is nonsense.*/
-	if (!nid || *nid == LNET_NID_ANY)
-		return 0;
-
-	obd = exp->exp_obd;
-
-	CDEBUG(D_CONFIG, "using hash %p\n", obd->obd_nid_stats_hash);
-
-	OBD_ALLOC_PTR(new_stat);
-	if (new_stat == NULL)
-		return -ENOMEM;
-
-	new_stat->nid	       = *nid;
-	new_stat->nid_obd	   = exp->exp_obd;
-	/* we need set default refcount to 1 to balance obd_disconnect */
-	atomic_set(&new_stat->nid_exp_ref_count, 1);
-
-	old_stat = cfs_hash_findadd_unique(obd->obd_nid_stats_hash,
-					   nid, &new_stat->nid_hash);
-	CDEBUG(D_INFO, "Found stats %p for nid %s - ref %d\n",
-	       old_stat, libcfs_nid2str(*nid),
-	       atomic_read(&new_stat->nid_exp_ref_count));
-
-	/* We need to release old stats because lprocfs_exp_cleanup() hasn't
-	 * been and will never be called. */
-	if (exp->exp_nid_stats) {
-		nidstat_putref(exp->exp_nid_stats);
-		exp->exp_nid_stats = NULL;
-	}
-
-	/* Return -EALREADY here so that we know that the /proc
-	 * entry already has been created */
-	if (old_stat != new_stat) {
-		exp->exp_nid_stats = old_stat;
-		rc = -EALREADY;
-		goto destroy_new;
-	}
-	/* not found - create */
-	OBD_ALLOC(buffer, LNET_NIDSTR_SIZE);
-	if (buffer == NULL) {
-		rc = -ENOMEM;
-		goto destroy_new;
-	}
-
-	memcpy(buffer, libcfs_nid2str(*nid), LNET_NIDSTR_SIZE);
-	new_stat->nid_proc = lprocfs_register(buffer,
-					      obd->obd_proc_exports_entry,
-					      NULL, NULL);
-	OBD_FREE(buffer, LNET_NIDSTR_SIZE);
-
-	if (IS_ERR(new_stat->nid_proc)) {
-		CERROR("Error making export directory for nid %s\n",
-		       libcfs_nid2str(*nid));
-		rc = PTR_ERR(new_stat->nid_proc);
-		new_stat->nid_proc = NULL;
-		goto destroy_new_ns;
-	}
-
-	entry = lprocfs_add_simple(new_stat->nid_proc, "uuid",
-				   new_stat, &lproc_exp_uuid_fops);
-	if (IS_ERR(entry)) {
-		CWARN("Error adding the NID stats file\n");
-		rc = PTR_ERR(entry);
-		goto destroy_new_ns;
-	}
-
-	entry = lprocfs_add_simple(new_stat->nid_proc, "hash",
-				   new_stat, &lproc_exp_hash_fops);
-	if (IS_ERR(entry)) {
-		CWARN("Error adding the hash file\n");
-		rc = PTR_ERR(entry);
-		goto destroy_new_ns;
-	}
-
-	exp->exp_nid_stats = new_stat;
-	*newnid = 1;
-	/* protect competitive add to list, not need locking on destroy */
-	spin_lock(&obd->obd_nid_lock);
-	list_add(&new_stat->nid_list, &obd->obd_nid_stats);
-	spin_unlock(&obd->obd_nid_lock);
-
-	return rc;
-
-destroy_new_ns:
-	if (new_stat->nid_proc != NULL)
-		lprocfs_remove(&new_stat->nid_proc);
-	cfs_hash_del(obd->obd_nid_stats_hash, nid, &new_stat->nid_hash);
-
-destroy_new:
-	nidstat_putref(new_stat);
-	OBD_FREE_PTR(new_stat);
-	return rc;
-}
-EXPORT_SYMBOL(lprocfs_exp_setup);
-
 int lprocfs_exp_cleanup(struct obd_export *exp)
 {
-	struct nid_stat *stat = exp->exp_nid_stats;
-
-	if (!stat || !exp->exp_obd)
-		return 0;
-
-	nidstat_putref(exp->exp_nid_stats);
-	exp->exp_nid_stats = NULL;
-
 	return 0;
 }
 EXPORT_SYMBOL(lprocfs_exp_cleanup);
@@ -1972,35 +1717,35 @@ char *lprocfs_find_named_value(const char *buffer, const char *name,
 }
 EXPORT_SYMBOL(lprocfs_find_named_value);
 
-int lprocfs_seq_create(struct proc_dir_entry *parent,
+int ldebugfs_seq_create(struct dentry *parent,
 		       const char *name,
 		       umode_t mode,
 		       const struct file_operations *seq_fops,
 		       void *data)
 {
-	struct proc_dir_entry *entry;
+	struct dentry *entry;
 
 	/* Disallow secretly (un)writable entries. */
 	LASSERT((seq_fops->write == NULL) == ((mode & 0222) == 0));
-	entry = proc_create_data(name, mode, parent, seq_fops, data);
 
-	if (entry == NULL)
-		return -ENOMEM;
+	entry = debugfs_create_file(name, mode, parent, data, seq_fops);
+	if (IS_ERR_OR_NULL(entry))
+		return entry ? PTR_ERR(entry) : -ENOMEM;
 
 	return 0;
 }
-EXPORT_SYMBOL(lprocfs_seq_create);
+EXPORT_SYMBOL(ldebugfs_seq_create);
 
-int lprocfs_obd_seq_create(struct obd_device *dev,
-			   const char *name,
-			   umode_t mode,
-			   const struct file_operations *seq_fops,
-			   void *data)
+int ldebugfs_obd_seq_create(struct obd_device *dev,
+			    const char *name,
+			    umode_t mode,
+			    const struct file_operations *seq_fops,
+			    void *data)
 {
-	return lprocfs_seq_create(dev->obd_proc_entry, name,
-				  mode, seq_fops, data);
+	return ldebugfs_seq_create(dev->obd_debugfs_entry, name,
+				   mode, seq_fops, data);
 }
-EXPORT_SYMBOL(lprocfs_obd_seq_create);
+EXPORT_SYMBOL(ldebugfs_obd_seq_create);
 
 void lprocfs_oh_tally(struct obd_histogram *oh, unsigned int value)
 {
@@ -2056,4 +1801,26 @@ int lprocfs_obd_rd_max_pages_per_rpc(struct seq_file *m, void *data)
 }
 EXPORT_SYMBOL(lprocfs_obd_rd_max_pages_per_rpc);
 
-#endif
+ssize_t lustre_attr_show(struct kobject *kobj,
+			 struct attribute *attr, char *buf)
+{
+	struct lustre_attr *a = container_of(attr, struct lustre_attr, attr);
+
+	return a->show ? a->show(kobj, attr, buf) : 0;
+}
+EXPORT_SYMBOL_GPL(lustre_attr_show);
+
+ssize_t lustre_attr_store(struct kobject *kobj, struct attribute *attr,
+			  const char *buf, size_t len)
+{
+	struct lustre_attr *a = container_of(attr, struct lustre_attr, attr);
+
+	return a->store ? a->store(kobj, attr, buf, len) : len;
+}
+EXPORT_SYMBOL_GPL(lustre_attr_store);
+
+const struct sysfs_ops lustre_sysfs_ops = {
+	.show  = lustre_attr_show,
+	.store = lustre_attr_store,
+};
+EXPORT_SYMBOL_GPL(lustre_sysfs_ops);
diff --git a/drivers/staging/lustre/lustre/obdclass/lu_object.c b/drivers/staging/lustre/lustre/obdclass/lu_object.c
index 20c0779951fd..4d9b6333eeae 100644
--- a/drivers/staging/lustre/lustre/obdclass/lu_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/lu_object.c
@@ -1532,7 +1532,7 @@ static void keys_fini(struct lu_context *ctx)
 	for (i = 0; i < ARRAY_SIZE(lu_keys); ++i)
 		key_fini(ctx, i);
 
-	OBD_FREE(ctx->lc_value, ARRAY_SIZE(lu_keys) * sizeof(ctx->lc_value[0]));
+	kfree(ctx->lc_value);
 	ctx->lc_value = NULL;
 }
 
@@ -1581,8 +1581,8 @@ static int keys_fill(struct lu_context *ctx)
 
 static int keys_init(struct lu_context *ctx)
 {
-	OBD_ALLOC(ctx->lc_value,
-		  ARRAY_SIZE(lu_keys) * sizeof(ctx->lc_value[0]));
+	ctx->lc_value = kcalloc(ARRAY_SIZE(lu_keys), sizeof(ctx->lc_value[0]),
+				GFP_NOFS);
 	if (likely(ctx->lc_value != NULL))
 		return keys_fill(ctx);
 
@@ -1989,14 +1989,10 @@ void lu_global_fini(void)
 
 static __u32 ls_stats_read(struct lprocfs_stats *stats, int idx)
 {
-#if defined (CONFIG_PROC_FS)
 	struct lprocfs_counter ret;
 
 	lprocfs_stats_collect(stats, idx, &ret);
 	return (__u32)ret.lc_count;
-#else
-	return 0;
-#endif
 }
 
 /**
@@ -2125,7 +2121,7 @@ void lu_buf_free(struct lu_buf *buf)
 	LASSERT(buf);
 	if (buf->lb_buf) {
 		LASSERT(buf->lb_len > 0);
-		OBD_FREE_LARGE(buf->lb_buf, buf->lb_len);
+		kvfree(buf->lb_buf);
 		buf->lb_buf = NULL;
 		buf->lb_len = 0;
 	}
@@ -2137,7 +2133,7 @@ void lu_buf_alloc(struct lu_buf *buf, int size)
 	LASSERT(buf);
 	LASSERT(buf->lb_buf == NULL);
 	LASSERT(buf->lb_len == 0);
-	OBD_ALLOC_LARGE(buf->lb_buf, size);
+	buf->lb_buf = libcfs_kvzalloc(size, GFP_NOFS);
 	if (likely(buf->lb_buf))
 		buf->lb_len = size;
 }
@@ -2175,14 +2171,14 @@ int lu_buf_check_and_grow(struct lu_buf *buf, int len)
 	if (len <= buf->lb_len)
 		return 0;
 
-	OBD_ALLOC_LARGE(ptr, len);
+	ptr = libcfs_kvzalloc(len, GFP_NOFS);
 	if (ptr == NULL)
 		return -ENOMEM;
 
 	/* Free the old buf */
 	if (buf->lb_buf != NULL) {
 		memcpy(ptr, buf->lb_buf, buf->lb_len);
-		OBD_FREE_LARGE(buf->lb_buf, buf->lb_len);
+		kvfree(buf->lb_buf);
 	}
 
 	buf->lb_buf = ptr;
diff --git a/drivers/staging/lustre/lustre/obdclass/lustre_handles.c b/drivers/staging/lustre/lustre/obdclass/lustre_handles.c
index f720e3183295..35a94a8f4fd3 100644
--- a/drivers/staging/lustre/lustre/obdclass/lustre_handles.c
+++ b/drivers/staging/lustre/lustre/obdclass/lustre_handles.c
@@ -186,7 +186,7 @@ void class_handle_free_cb(struct rcu_head *rcu)
 	if (h->h_ops->hop_free != NULL)
 		h->h_ops->hop_free(ptr, h->h_size);
 	else
-		OBD_FREE(ptr, h->h_size);
+		kfree(ptr);
 }
 EXPORT_SYMBOL(class_handle_free_cb);
 
@@ -198,7 +198,8 @@ int class_handle_init(void)
 
 	LASSERT(handle_hash == NULL);
 
-	OBD_ALLOC_LARGE(handle_hash, sizeof(*bucket) * HANDLE_HASH_SIZE);
+	handle_hash = libcfs_kvzalloc(sizeof(*bucket) * HANDLE_HASH_SIZE,
+				      GFP_NOFS);
 	if (handle_hash == NULL)
 		return -ENOMEM;
 
@@ -249,7 +250,7 @@ void class_handle_cleanup(void)
 
 	count = cleanup_all_handles();
 
-	OBD_FREE_LARGE(handle_hash, sizeof(*handle_hash) * HANDLE_HASH_SIZE);
+	kvfree(handle_hash);
 	handle_hash = NULL;
 
 	if (count != 0)
diff --git a/drivers/staging/lustre/lustre/obdclass/lustre_peer.c b/drivers/staging/lustre/lustre/obdclass/lustre_peer.c
index 64b2f35e224f..5cc6435cc47a 100644
--- a/drivers/staging/lustre/lustre/obdclass/lustre_peer.c
+++ b/drivers/staging/lustre/lustre/obdclass/lustre_peer.c
@@ -104,7 +104,7 @@ int class_add_uuid(const char *uuid, __u64 nid)
 	if (strlen(uuid) > UUID_MAX - 1)
 		return -EOVERFLOW;
 
-	OBD_ALLOC_PTR(data);
+	data = kzalloc(sizeof(*data), GFP_NOFS);
 	if (data == NULL)
 		return -ENOMEM;
 
@@ -136,7 +136,7 @@ int class_add_uuid(const char *uuid, __u64 nid)
 	if (found) {
 		CDEBUG(D_INFO, "found uuid %s %s cnt=%d\n", uuid,
 		       libcfs_nid2str(nid), entry->un_nid_count);
-		OBD_FREE(data, sizeof(*data));
+		kfree(data);
 	} else {
 		CDEBUG(D_INFO, "add uuid %s %s\n", uuid, libcfs_nid2str(nid));
 	}
@@ -180,7 +180,7 @@ int class_del_uuid(const char *uuid)
 		       libcfs_nid2str(data->un_nids[0]),
 		       data->un_nid_count);
 
-		OBD_FREE(data, sizeof(*data));
+		kfree(data);
 	}
 
 	return 0;
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_config.c b/drivers/staging/lustre/lustre/obdclass/obd_config.c
index 6ce9adc2f11c..fbdb748a36b9 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_config.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_config.c
@@ -49,7 +49,6 @@
 
 static cfs_hash_ops_t uuid_hash_ops;
 static cfs_hash_ops_t nid_hash_ops;
-static cfs_hash_ops_t nid_stat_hash_ops;
 
 /*********** string parsing utils *********/
 
@@ -383,7 +382,6 @@ int class_attach(struct lustre_cfg *lcfg)
 	INIT_LIST_HEAD(&obd->obd_unlinked_exports);
 	INIT_LIST_HEAD(&obd->obd_delayed_exports);
 	INIT_LIST_HEAD(&obd->obd_exports_timed);
-	INIT_LIST_HEAD(&obd->obd_nid_stats);
 	spin_lock_init(&obd->obd_nid_lock);
 	spin_lock_init(&obd->obd_dev_lock);
 	mutex_init(&obd->obd_dev_mutex);
@@ -486,7 +484,6 @@ int class_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 	obd->obd_starting = 1;
 	obd->obd_uuid_hash = NULL;
 	obd->obd_nid_hash = NULL;
-	obd->obd_nid_stats_hash = NULL;
 	spin_unlock(&obd->obd_dev_lock);
 
 	/* create an uuid-export lustre hash */
@@ -515,19 +512,6 @@ int class_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 		goto err_hash;
 	}
 
-	/* create a nid-stats lustre hash */
-	obd->obd_nid_stats_hash = cfs_hash_create("NID_STATS",
-						  HASH_NID_STATS_CUR_BITS,
-						  HASH_NID_STATS_MAX_BITS,
-						  HASH_NID_STATS_BKT_BITS, 0,
-						  CFS_HASH_MIN_THETA,
-						  CFS_HASH_MAX_THETA,
-						  &nid_stat_hash_ops, CFS_HASH_DEFAULT);
-	if (!obd->obd_nid_stats_hash) {
-		err = -ENOMEM;
-		goto err_hash;
-	}
-
 	exp = class_new_export(obd, &obd->obd_uuid);
 	if (IS_ERR(exp)) {
 		err = PTR_ERR(exp);
@@ -567,10 +551,6 @@ err_hash:
 		cfs_hash_putref(obd->obd_nid_hash);
 		obd->obd_nid_hash = NULL;
 	}
-	if (obd->obd_nid_stats_hash) {
-		cfs_hash_putref(obd->obd_nid_stats_hash);
-		obd->obd_nid_stats_hash = NULL;
-	}
 	obd->obd_starting = 0;
 	CERROR("setup %s failed (%d)\n", obd->obd_name, err);
 	return err;
@@ -694,12 +674,6 @@ int class_cleanup(struct obd_device *obd, struct lustre_cfg *lcfg)
 		obd->obd_nid_hash = NULL;
 	}
 
-	/* destroy a nid-stats hash body */
-	if (obd->obd_nid_stats_hash) {
-		cfs_hash_putref(obd->obd_nid_stats_hash);
-		obd->obd_nid_stats_hash = NULL;
-	}
-
 	class_decref(obd, "setup", obd);
 	obd->obd_set_up = 0;
 
@@ -860,48 +834,43 @@ int class_add_profile(int proflen, char *prof, int osclen, char *osc,
 
 	CDEBUG(D_CONFIG, "Add profile %s\n", prof);
 
-	OBD_ALLOC(lprof, sizeof(*lprof));
+	lprof = kzalloc(sizeof(*lprof), GFP_NOFS);
 	if (lprof == NULL)
 		return -ENOMEM;
 	INIT_LIST_HEAD(&lprof->lp_list);
 
 	LASSERT(proflen == (strlen(prof) + 1));
-	OBD_ALLOC(lprof->lp_profile, proflen);
+	lprof->lp_profile = kmemdup(prof, proflen, GFP_NOFS);
 	if (lprof->lp_profile == NULL) {
 		err = -ENOMEM;
-		goto out;
+		goto free_lprof;
 	}
-	memcpy(lprof->lp_profile, prof, proflen);
 
 	LASSERT(osclen == (strlen(osc) + 1));
-	OBD_ALLOC(lprof->lp_dt, osclen);
+	lprof->lp_dt = kmemdup(osc, osclen, GFP_NOFS);
 	if (lprof->lp_dt == NULL) {
 		err = -ENOMEM;
-		goto out;
+		goto free_lp_profile;
 	}
-	memcpy(lprof->lp_dt, osc, osclen);
 
 	if (mdclen > 0) {
 		LASSERT(mdclen == (strlen(mdc) + 1));
-		OBD_ALLOC(lprof->lp_md, mdclen);
+		lprof->lp_md = kmemdup(mdc, mdclen, GFP_NOFS);
 		if (lprof->lp_md == NULL) {
 			err = -ENOMEM;
-			goto out;
+			goto free_lp_dt;
 		}
-		memcpy(lprof->lp_md, mdc, mdclen);
 	}
 
 	list_add(&lprof->lp_list, &lustre_profile_list);
 	return err;
 
-out:
-	if (lprof->lp_md)
-		OBD_FREE(lprof->lp_md, mdclen);
-	if (lprof->lp_dt)
-		OBD_FREE(lprof->lp_dt, osclen);
-	if (lprof->lp_profile)
-		OBD_FREE(lprof->lp_profile, proflen);
-	OBD_FREE(lprof, sizeof(*lprof));
+free_lp_dt:
+	kfree(lprof->lp_dt);
+free_lp_profile:
+	kfree(lprof->lp_profile);
+free_lprof:
+	kfree(lprof);
 	return err;
 }
 
@@ -914,11 +883,10 @@ void class_del_profile(const char *prof)
 	lprof = class_get_profile(prof);
 	if (lprof) {
 		list_del(&lprof->lp_list);
-		OBD_FREE(lprof->lp_profile, strlen(lprof->lp_profile) + 1);
-		OBD_FREE(lprof->lp_dt, strlen(lprof->lp_dt) + 1);
-		if (lprof->lp_md)
-			OBD_FREE(lprof->lp_md, strlen(lprof->lp_md) + 1);
-		OBD_FREE(lprof, sizeof(*lprof));
+		kfree(lprof->lp_profile);
+		kfree(lprof->lp_dt);
+		kfree(lprof->lp_md);
+		kfree(lprof);
 	}
 }
 EXPORT_SYMBOL(class_del_profile);
@@ -930,11 +898,10 @@ void class_del_profiles(void)
 
 	list_for_each_entry_safe(lprof, n, &lustre_profile_list, lp_list) {
 		list_del(&lprof->lp_list);
-		OBD_FREE(lprof->lp_profile, strlen(lprof->lp_profile) + 1);
-		OBD_FREE(lprof->lp_dt, strlen(lprof->lp_dt) + 1);
-		if (lprof->lp_md)
-			OBD_FREE(lprof->lp_md, strlen(lprof->lp_md) + 1);
-		OBD_FREE(lprof, sizeof(*lprof));
+		kfree(lprof->lp_profile);
+		kfree(lprof->lp_dt);
+		kfree(lprof->lp_md);
+		kfree(lprof);
 	}
 }
 EXPORT_SYMBOL(class_del_profiles);
@@ -1011,7 +978,7 @@ struct lustre_cfg *lustre_cfg_rename(struct lustre_cfg *cfg,
 
 	new_len = LUSTRE_CFG_BUFLEN(cfg, 1) + strlen(new_name) - name_len;
 
-	OBD_ALLOC(new_param, new_len);
+	new_param = kzalloc(new_len, GFP_NOFS);
 	if (new_param == NULL)
 		return ERR_PTR(-ENOMEM);
 
@@ -1019,9 +986,9 @@ struct lustre_cfg *lustre_cfg_rename(struct lustre_cfg *cfg,
 	if (value != NULL)
 		strcat(new_param, value);
 
-	OBD_ALLOC_PTR(bufs);
+	bufs = kzalloc(sizeof(*bufs), GFP_NOFS);
 	if (bufs == NULL) {
-		OBD_FREE(new_param, new_len);
+		kfree(new_param);
 		return ERR_PTR(-ENOMEM);
 	}
 
@@ -1031,8 +998,8 @@ struct lustre_cfg *lustre_cfg_rename(struct lustre_cfg *cfg,
 
 	new_cfg = lustre_cfg_new(cfg->lcfg_command, bufs);
 
-	OBD_FREE(new_param, new_len);
-	OBD_FREE_PTR(bufs);
+	kfree(new_param);
+	kfree(bufs);
 	if (new_cfg == NULL)
 		return ERR_PTR(-ENOMEM);
 
@@ -1493,7 +1460,7 @@ int class_config_llog_handler(const struct lu_env *env,
 			inst = 1;
 			inst_len = LUSTRE_CFG_BUFLEN(lcfg, 0) +
 				   sizeof(clli->cfg_instance) * 2 + 4;
-			OBD_ALLOC(inst_name, inst_len);
+			inst_name = kzalloc(inst_len, GFP_NOFS);
 			if (inst_name == NULL) {
 				rc = -ENOMEM;
 				goto out;
@@ -1556,7 +1523,7 @@ int class_config_llog_handler(const struct lu_env *env,
 		lustre_cfg_free(lcfg_new);
 
 		if (inst)
-			OBD_FREE(inst_name, inst_len);
+			kfree(inst_name);
 		break;
 	}
 	default:
@@ -1671,7 +1638,7 @@ int class_config_dump_handler(const struct lu_env *env,
 	char	*outstr;
 	int	 rc = 0;
 
-	OBD_ALLOC(outstr, 256);
+	outstr = kzalloc(256, GFP_NOFS);
 	if (outstr == NULL)
 		return -ENOMEM;
 
@@ -1683,7 +1650,7 @@ int class_config_dump_handler(const struct lu_env *env,
 		rc = -EINVAL;
 	}
 
-	OBD_FREE(outstr, 256);
+	kfree(outstr);
 	return rc;
 }
 
@@ -1897,57 +1864,3 @@ static cfs_hash_ops_t nid_hash_ops = {
 	.hs_get	 = nid_export_get,
 	.hs_put_locked  = nid_export_put_locked,
 };
-
-
-/*
- * nid<->nidstats hash operations
- */
-
-static void *
-nidstats_key(struct hlist_node *hnode)
-{
-	struct nid_stat *ns;
-
-	ns = hlist_entry(hnode, struct nid_stat, nid_hash);
-
-	return &ns->nid;
-}
-
-static int
-nidstats_keycmp(const void *key, struct hlist_node *hnode)
-{
-	return *(lnet_nid_t *)nidstats_key(hnode) == *(lnet_nid_t *)key;
-}
-
-static void *
-nidstats_object(struct hlist_node *hnode)
-{
-	return hlist_entry(hnode, struct nid_stat, nid_hash);
-}
-
-static void
-nidstats_get(struct cfs_hash *hs, struct hlist_node *hnode)
-{
-	struct nid_stat *ns;
-
-	ns = hlist_entry(hnode, struct nid_stat, nid_hash);
-	nidstat_getref(ns);
-}
-
-static void
-nidstats_put_locked(struct cfs_hash *hs, struct hlist_node *hnode)
-{
-	struct nid_stat *ns;
-
-	ns = hlist_entry(hnode, struct nid_stat, nid_hash);
-	nidstat_putref(ns);
-}
-
-static cfs_hash_ops_t nid_stat_hash_ops = {
-	.hs_hash	= nid_hash,
-	.hs_key	 = nidstats_key,
-	.hs_keycmp      = nidstats_keycmp,
-	.hs_object      = nidstats_object,
-	.hs_get	 = nidstats_get,
-	.hs_put_locked  = nidstats_put_locked,
-};
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_mount.c b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
index 3437b2ecfc02..ce4a71f7171a 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_mount.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
@@ -84,7 +84,7 @@ int lustre_process_log(struct super_block *sb, char *logname,
 	LASSERT(mgc);
 	LASSERT(cfg);
 
-	OBD_ALLOC_PTR(bufs);
+	bufs = kzalloc(sizeof(*bufs), GFP_NOFS);
 	if (bufs == NULL)
 		return -ENOMEM;
 
@@ -97,7 +97,7 @@ int lustre_process_log(struct super_block *sb, char *logname,
 	rc = obd_process_config(mgc, sizeof(*lcfg), lcfg);
 	lustre_cfg_free(lcfg);
 
-	OBD_FREE_PTR(bufs);
+	kfree(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",
@@ -147,7 +147,7 @@ int do_lcfg(char *cfgname, lnet_nid_t nid, int cmd,
 	    char *s1, char *s2, char *s3, char *s4)
 {
 	struct lustre_cfg_bufs bufs;
-	struct lustre_cfg    * lcfg = NULL;
+	struct lustre_cfg     *lcfg = NULL;
 	int rc;
 
 	CDEBUG(D_TRACE, "lcfg %s %#x %s %s %s %s\n", cfgname,
@@ -247,8 +247,8 @@ int lustre_start_mgc(struct super_block *sb)
 	mutex_lock(&mgc_start_lock);
 
 	len = strlen(LUSTRE_MGC_OBDNAME) + strlen(libcfs_nid2str(nid)) + 1;
-	OBD_ALLOC(mgcname, len);
-	OBD_ALLOC(niduuid, len + 2);
+	mgcname = kzalloc(len, GFP_NOFS);
+	niduuid = kzalloc(len + 2, GFP_NOFS);
 	if (!mgcname || !niduuid) {
 		rc = -ENOMEM;
 		goto out_free;
@@ -257,7 +257,7 @@ int lustre_start_mgc(struct super_block *sb)
 
 	mgssec = lsi->lsi_lmd->lmd_mgssec ? lsi->lsi_lmd->lmd_mgssec : "";
 
-	OBD_ALLOC_PTR(data);
+	data = kzalloc(sizeof(*data), GFP_NOFS);
 	if (data == NULL) {
 		rc = -ENOMEM;
 		goto out_free;
@@ -375,7 +375,7 @@ int lustre_start_mgc(struct super_block *sb)
 	lsi->lsi_lmd->lmd_mgs_failnodes = 1;
 
 	/* Random uuid for MGC allows easier reconnects */
-	OBD_ALLOC_PTR(uuid);
+	uuid = kzalloc(sizeof(*uuid), GFP_NOFS);
 	if (!uuid) {
 		rc = -ENOMEM;
 		goto out_free;
@@ -388,7 +388,7 @@ int lustre_start_mgc(struct super_block *sb)
 	rc = lustre_start_simple(mgcname, LUSTRE_MGC_NAME,
 				 (char *)uuid->uuid, LUSTRE_MGS_OBDNAME,
 				 niduuid, NULL, NULL);
-	OBD_FREE_PTR(uuid);
+	kfree(uuid);
 	if (rc)
 		goto out_free;
 
@@ -464,12 +464,9 @@ out:
 out_free:
 	mutex_unlock(&mgc_start_lock);
 
-	if (data)
-		OBD_FREE_PTR(data);
-	if (mgcname)
-		OBD_FREE(mgcname, len);
-	if (niduuid)
-		OBD_FREE(niduuid, len + 2);
+	kfree(data);
+	kfree(mgcname);
+	kfree(niduuid);
 	return rc;
 }
 
@@ -513,7 +510,7 @@ static int lustre_stop_mgc(struct super_block *sb)
 	/* Save the obdname for cleaning the nid uuids, which are
 	   obdname_XX */
 	len = strlen(obd->obd_name) + 6;
-	OBD_ALLOC(niduuid, len);
+	niduuid = kzalloc(len, GFP_NOFS);
 	if (niduuid) {
 		strcpy(niduuid, obd->obd_name);
 		ptr = niduuid + strlen(niduuid);
@@ -538,8 +535,7 @@ static int lustre_stop_mgc(struct super_block *sb)
 			       niduuid, rc);
 	}
 out:
-	if (niduuid)
-		OBD_FREE(niduuid, len);
+	kfree(niduuid);
 
 	/* class_import_put will get rid of the additional connections */
 	mutex_unlock(&mgc_start_lock);
@@ -552,12 +548,12 @@ struct lustre_sb_info *lustre_init_lsi(struct super_block *sb)
 {
 	struct lustre_sb_info *lsi;
 
-	OBD_ALLOC_PTR(lsi);
+	lsi = kzalloc(sizeof(*lsi), GFP_NOFS);
 	if (!lsi)
 		return NULL;
-	OBD_ALLOC_PTR(lsi->lsi_lmd);
+	lsi->lsi_lmd = kzalloc(sizeof(*lsi->lsi_lmd), GFP_NOFS);
 	if (!lsi->lsi_lmd) {
-		OBD_FREE_PTR(lsi);
+		kfree(lsi);
 		return NULL;
 	}
 
@@ -585,36 +581,21 @@ static int lustre_free_lsi(struct super_block *sb)
 	LASSERT(atomic_read(&lsi->lsi_mounts) == 0);
 
 	if (lsi->lsi_lmd != NULL) {
-		if (lsi->lsi_lmd->lmd_dev != NULL)
-			OBD_FREE(lsi->lsi_lmd->lmd_dev,
-				 strlen(lsi->lsi_lmd->lmd_dev) + 1);
-		if (lsi->lsi_lmd->lmd_profile != NULL)
-			OBD_FREE(lsi->lsi_lmd->lmd_profile,
-				 strlen(lsi->lsi_lmd->lmd_profile) + 1);
-		if (lsi->lsi_lmd->lmd_mgssec != NULL)
-			OBD_FREE(lsi->lsi_lmd->lmd_mgssec,
-				 strlen(lsi->lsi_lmd->lmd_mgssec) + 1);
-		if (lsi->lsi_lmd->lmd_opts != NULL)
-			OBD_FREE(lsi->lsi_lmd->lmd_opts,
-				 strlen(lsi->lsi_lmd->lmd_opts) + 1);
+		kfree(lsi->lsi_lmd->lmd_dev);
+		kfree(lsi->lsi_lmd->lmd_profile);
+		kfree(lsi->lsi_lmd->lmd_mgssec);
+		kfree(lsi->lsi_lmd->lmd_opts);
 		if (lsi->lsi_lmd->lmd_exclude_count)
-			OBD_FREE(lsi->lsi_lmd->lmd_exclude,
-				 sizeof(lsi->lsi_lmd->lmd_exclude[0]) *
-				 lsi->lsi_lmd->lmd_exclude_count);
-		if (lsi->lsi_lmd->lmd_mgs != NULL)
-			OBD_FREE(lsi->lsi_lmd->lmd_mgs,
-				 strlen(lsi->lsi_lmd->lmd_mgs) + 1);
-		if (lsi->lsi_lmd->lmd_osd_type != NULL)
-			OBD_FREE(lsi->lsi_lmd->lmd_osd_type,
-				 strlen(lsi->lsi_lmd->lmd_osd_type) + 1);
-		if (lsi->lsi_lmd->lmd_params != NULL)
-			OBD_FREE(lsi->lsi_lmd->lmd_params, 4096);
-
-		OBD_FREE(lsi->lsi_lmd, sizeof(*lsi->lsi_lmd));
+			kfree(lsi->lsi_lmd->lmd_exclude);
+		kfree(lsi->lsi_lmd->lmd_mgs);
+		kfree(lsi->lsi_lmd->lmd_osd_type);
+		kfree(lsi->lsi_lmd->lmd_params);
+
+		kfree(lsi->lsi_lmd);
 	}
 
 	LASSERT(lsi->lsi_llsbi == NULL);
-	OBD_FREE(lsi, sizeof(*lsi));
+	kfree(lsi);
 	s2lsi_nocast(sb) = NULL;
 
 	return 0;
@@ -846,7 +827,7 @@ static int lmd_make_exclusion(struct lustre_mount_data *lmd, const char *ptr)
 	devmax = strlen(ptr) / 8 + 1;
 
 	/* temp storage until we figure out how many we have */
-	OBD_ALLOC(exclude_list, sizeof(index) * devmax);
+	exclude_list = kcalloc(devmax, sizeof(index), GFP_NOFS);
 	if (!exclude_list)
 		return -ENOMEM;
 
@@ -875,8 +856,8 @@ static int lmd_make_exclusion(struct lustre_mount_data *lmd, const char *ptr)
 
 	if (lmd->lmd_exclude_count) {
 		/* permanent, freed in lustre_free_lsi */
-		OBD_ALLOC(lmd->lmd_exclude, sizeof(index) *
-			  lmd->lmd_exclude_count);
+		lmd->lmd_exclude = kcalloc(lmd->lmd_exclude_count,
+					   sizeof(index), GFP_NOFS);
 		if (lmd->lmd_exclude) {
 			memcpy(lmd->lmd_exclude, exclude_list,
 			       sizeof(index) * lmd->lmd_exclude_count);
@@ -885,7 +866,7 @@ static int lmd_make_exclusion(struct lustre_mount_data *lmd, const char *ptr)
 			lmd->lmd_exclude_count = 0;
 		}
 	}
-	OBD_FREE(exclude_list, sizeof(index) * devmax);
+	kfree(exclude_list);
 	return rc;
 }
 
@@ -894,10 +875,8 @@ static int lmd_parse_mgssec(struct lustre_mount_data *lmd, char *ptr)
 	char   *tail;
 	int     length;
 
-	if (lmd->lmd_mgssec != NULL) {
-		OBD_FREE(lmd->lmd_mgssec, strlen(lmd->lmd_mgssec) + 1);
-		lmd->lmd_mgssec = NULL;
-	}
+	kfree(lmd->lmd_mgssec);
+	lmd->lmd_mgssec = NULL;
 
 	tail = strchr(ptr, ',');
 	if (tail == NULL)
@@ -905,7 +884,7 @@ static int lmd_parse_mgssec(struct lustre_mount_data *lmd, char *ptr)
 	else
 		length = tail - ptr;
 
-	OBD_ALLOC(lmd->lmd_mgssec, length + 1);
+	lmd->lmd_mgssec = kzalloc(length + 1, GFP_NOFS);
 	if (lmd->lmd_mgssec == NULL)
 		return -ENOMEM;
 
@@ -922,10 +901,8 @@ static int lmd_parse_string(char **handle, char *ptr)
 	if ((handle == NULL) || (ptr == NULL))
 		return -EINVAL;
 
-	if (*handle != NULL) {
-		OBD_FREE(*handle, strlen(*handle) + 1);
-		*handle = NULL;
-	}
+	kfree(*handle);
+	*handle = NULL;
 
 	tail = strchr(ptr, ',');
 	if (tail == NULL)
@@ -933,7 +910,7 @@ static int lmd_parse_string(char **handle, char *ptr)
 	else
 		length = tail - ptr;
 
-	OBD_ALLOC(*handle, length + 1);
+	*handle = kzalloc(length + 1, GFP_NOFS);
 	if (*handle == NULL)
 		return -ENOMEM;
 
@@ -963,7 +940,7 @@ static int lmd_parse_mgs(struct lustre_mount_data *lmd, char **ptr)
 	if (lmd->lmd_mgs != NULL)
 		oldlen = strlen(lmd->lmd_mgs) + 1;
 
-	OBD_ALLOC(mgsnid, oldlen + length + 1);
+	mgsnid = kzalloc(oldlen + length + 1, GFP_NOFS);
 	if (mgsnid == NULL)
 		return -ENOMEM;
 
@@ -971,7 +948,7 @@ static int lmd_parse_mgs(struct lustre_mount_data *lmd, char **ptr)
 		/* Multiple mgsnid= are taken to mean failover locations */
 		memcpy(mgsnid, lmd->lmd_mgs, oldlen);
 		mgsnid[oldlen - 1] = ':';
-		OBD_FREE(lmd->lmd_mgs, oldlen);
+		kfree(lmd->lmd_mgs);
 	}
 	memcpy(mgsnid + oldlen, *ptr, length);
 	mgsnid[oldlen + length] = '\0';
@@ -1005,7 +982,7 @@ static int lmd_parse(char *options, struct lustre_mount_data *lmd)
 	}
 	lmd->lmd_magic = LMD_MAGIC;
 
-	OBD_ALLOC(lmd->lmd_params, 4096);
+	lmd->lmd_params = kzalloc(4096, GFP_NOFS);
 	if (lmd->lmd_params == NULL)
 		return -ENOMEM;
 	lmd->lmd_params[0] = '\0';
@@ -1143,14 +1120,14 @@ static int lmd_parse(char *options, struct lustre_mount_data *lmd)
 		/* Remove leading /s from fsname */
 		while (*++s1 == '/') ;
 		/* Freed in lustre_free_lsi */
-		OBD_ALLOC(lmd->lmd_profile, strlen(s1) + 8);
+		lmd->lmd_profile = kzalloc(strlen(s1) + 8, GFP_NOFS);
 		if (!lmd->lmd_profile)
 			return -ENOMEM;
 		sprintf(lmd->lmd_profile, "%s-client", s1);
 	}
 
 	/* Freed in lustre_free_lsi */
-	OBD_ALLOC(lmd->lmd_dev, strlen(devname) + 1);
+	lmd->lmd_dev = kzalloc(strlen(devname) + 1, GFP_NOFS);
 	if (!lmd->lmd_dev)
 		return -ENOMEM;
 	strcpy(lmd->lmd_dev, devname);
@@ -1161,7 +1138,7 @@ static int lmd_parse(char *options, struct lustre_mount_data *lmd)
 		*s1-- = 0;
 	if (*options != 0) {
 		/* Freed in lustre_free_lsi */
-		OBD_ALLOC(lmd->lmd_opts, strlen(options) + 1);
+		lmd->lmd_opts = kzalloc(strlen(options) + 1, GFP_NOFS);
 		if (!lmd->lmd_opts)
 			return -ENOMEM;
 		strcpy(lmd->lmd_opts, options);
diff --git a/drivers/staging/lustre/lustre/obdecho/Makefile b/drivers/staging/lustre/lustre/obdecho/Makefile
index 672028fc7f6e..a659a37a7e93 100644
--- a/drivers/staging/lustre/lustre/obdecho/Makefile
+++ b/drivers/staging/lustre/lustre/obdecho/Makefile
@@ -1,2 +1,2 @@
 obj-$(CONFIG_LUSTRE_FS) += obdecho.o
-obdecho-y := echo_client.o lproc_echo.o
+obdecho-y := echo_client.o
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index d542e06d6cd3..0222fd2e4757 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -479,13 +479,13 @@ static int echo_alloc_memmd(struct echo_device *ed,
 	lsm_size = lov_stripe_md_size(1);
 
 	LASSERT(*lsmp == NULL);
-	OBD_ALLOC(*lsmp, lsm_size);
+	*lsmp = kzalloc(lsm_size, GFP_NOFS);
 	if (*lsmp == NULL)
 		return -ENOMEM;
 
-	OBD_ALLOC((*lsmp)->lsm_oinfo[0], sizeof(struct lov_oinfo));
+	(*lsmp)->lsm_oinfo[0] = kzalloc(sizeof(struct lov_oinfo), GFP_NOFS);
 	if ((*lsmp)->lsm_oinfo[0] == NULL) {
-		OBD_FREE(*lsmp, lsm_size);
+		kfree(*lsmp);
 		return -ENOMEM;
 	}
 
@@ -507,8 +507,8 @@ static int echo_free_memmd(struct echo_device *ed, struct lov_stripe_md **lsmp)
 	lsm_size = lov_stripe_md_size(1);
 
 	LASSERT(*lsmp != NULL);
-	OBD_FREE((*lsmp)->lsm_oinfo[0], sizeof(struct lov_oinfo));
-	OBD_FREE(*lsmp, lsm_size);
+	kfree((*lsmp)->lsm_oinfo[0]);
+	kfree(*lsmp);
 	*lsmp = NULL;
 	return 0;
 }
@@ -700,7 +700,7 @@ static struct lu_device *echo_device_alloc(const struct lu_env *env,
 	int rc;
 	int cleanup = 0;
 
-	OBD_ALLOC_PTR(ed);
+	ed = kzalloc(sizeof(*ed), GFP_NOFS);
 	if (ed == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -798,7 +798,7 @@ out:
 	case 2:
 		cl_device_fini(&ed->ed_cl);
 	case 1:
-		OBD_FREE_PTR(ed);
+		kfree(ed);
 	case 0:
 	default:
 		break;
@@ -895,7 +895,7 @@ static struct lu_device *echo_device_free(const struct lu_env *env,
 	LASSERT(ed->ed_site == lu2cl_site(d->ld_site));
 	echo_site_fini(env, ed);
 	cl_device_fini(&ed->ed_cl);
-	OBD_FREE_PTR(ed);
+	kfree(ed);
 
 	return NULL;
 }
@@ -1577,13 +1577,13 @@ static int echo_client_kbrw(struct echo_device *ed, int rw, struct obdo *oa,
 	if (rw == OBD_BRW_WRITE)
 		brw_flags = OBD_BRW_ASYNC;
 
-	OBD_ALLOC(pga, npages * sizeof(*pga));
+	pga = kcalloc(npages, sizeof(*pga), GFP_NOFS);
 	if (pga == NULL)
 		return -ENOMEM;
 
-	OBD_ALLOC(pages, npages * sizeof(*pages));
+	pages = kcalloc(npages, sizeof(*pages), GFP_NOFS);
 	if (pages == NULL) {
-		OBD_FREE(pga, npages * sizeof(*pga));
+		kfree(pga);
 		return -ENOMEM;
 	}
 
@@ -1632,8 +1632,8 @@ static int echo_client_kbrw(struct echo_device *ed, int rw, struct obdo *oa,
 		}
 		OBD_PAGE_FREE(pgp->pg);
 	}
-	OBD_FREE(pga, npages * sizeof(*pga));
-	OBD_FREE(pages, npages * sizeof(*pages));
+	kfree(pga);
+	kfree(pages);
 	return rc;
 }
 
@@ -1659,8 +1659,8 @@ static int echo_client_prep_commit(const struct lu_env *env,
 	npages = batch >> PAGE_CACHE_SHIFT;
 	tot_pages = count >> PAGE_CACHE_SHIFT;
 
-	OBD_ALLOC(lnb, npages * sizeof(struct niobuf_local));
-	OBD_ALLOC(rnb, npages * sizeof(struct niobuf_remote));
+	lnb = kcalloc(npages, sizeof(struct niobuf_local), GFP_NOFS);
+	rnb = kcalloc(npages, sizeof(struct niobuf_remote), GFP_NOFS);
 
 	if (lnb == NULL || rnb == NULL) {
 		ret = -ENOMEM;
@@ -1737,10 +1737,8 @@ static int echo_client_prep_commit(const struct lu_env *env,
 	}
 
 out:
-	if (lnb)
-		OBD_FREE(lnb, npages * sizeof(struct niobuf_local));
-	if (rnb)
-		OBD_FREE(rnb, npages * sizeof(struct niobuf_remote));
+	kfree(lnb);
+	kfree(rnb);
 	return ret;
 }
 
@@ -1879,7 +1877,7 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 	if (rc < 0)
 		return rc;
 
-	OBD_ALLOC_PTR(env);
+	env = kzalloc(sizeof(*env), GFP_NOFS);
 	if (env == NULL)
 		return -ENOMEM;
 
@@ -2010,7 +2008,7 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 
 out:
 	lu_env_fini(env);
-	OBD_FREE_PTR(env);
+	kfree(env);
 
 	/* XXX this should be in a helper also called by target_send_reply */
 	for (ack_lock = dummy_oti.oti_ack_locks, i = 0; i < 4;
@@ -2050,7 +2048,7 @@ static int echo_client_setup(const struct lu_env *env,
 	ec->ec_unique = 0;
 	ec->ec_nstripes = 0;
 
-	OBD_ALLOC(ocd, sizeof(*ocd));
+	ocd = kzalloc(sizeof(*ocd), GFP_NOFS);
 	if (ocd == NULL) {
 		CERROR("Can't alloc ocd connecting to %s\n",
 		       lustre_cfg_string(lcfg, 1));
@@ -2074,7 +2072,7 @@ static int echo_client_setup(const struct lu_env *env,
 		spin_unlock(&tgt->obd_dev_lock);
 	}
 
-	OBD_FREE(ocd, sizeof(*ocd));
+	kfree(ocd);
 
 	if (rc != 0) {
 		CERROR("fail to connect to device %s\n",
@@ -2143,15 +2141,11 @@ static struct obd_ops echo_client_obd_ops = {
 
 int echo_client_init(void)
 {
-	struct lprocfs_static_vars lvars = { NULL };
 	int rc;
 
-	lprocfs_echo_init_vars(&lvars);
-
 	rc = lu_kmem_init(echo_caches);
 	if (rc == 0) {
 		rc = class_register_type(&echo_client_obd_ops, NULL,
-					 lvars.module_vars,
 					 LUSTRE_ECHO_CLIENT_NAME,
 					 &echo_device_type);
 		if (rc)
@@ -2168,15 +2162,10 @@ void echo_client_exit(void)
 
 static int __init obdecho_init(void)
 {
-	struct lprocfs_static_vars lvars;
-
 	LCONSOLE_INFO("Echo OBD driver; http://www.lustre.org/\n");
 
 	LASSERT(PAGE_CACHE_SIZE % OBD_ECHO_BLOCK_SIZE == 0);
 
-	lprocfs_echo_init_vars(&lvars);
-
-
 	return echo_client_init();
 }
 
diff --git a/drivers/staging/lustre/lustre/obdecho/lproc_echo.c b/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
deleted file mode 100644
index 0beb97db7c7d..000000000000
--- a/drivers/staging/lustre/lustre/obdecho/lproc_echo.c
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-#define DEBUG_SUBSYSTEM S_ECHO
-
-#include "../include/lprocfs_status.h"
-#include "../include/obd_class.h"
-
-#if defined(CONFIG_PROC_FS)
-LPROC_SEQ_FOPS_RO_TYPE(echo, uuid);
-static struct lprocfs_vars lprocfs_echo_obd_vars[] = {
-	{ "uuid",	 &echo_uuid_fops,	NULL, 0 },
-	{ NULL }
-};
-
-LPROC_SEQ_FOPS_RO_TYPE(echo, numrefs);
-static struct lprocfs_vars lprocfs_echo_module_vars[] = {
-	{ "num_refs",     &echo_numrefs_fops,     NULL, 0 },
-	{ NULL }
-};
-
-void lprocfs_echo_init_vars(struct lprocfs_static_vars *lvars)
-{
-	lvars->module_vars  = lprocfs_echo_module_vars;
-	lvars->obd_vars     = lprocfs_echo_obd_vars;
-}
-#endif /* CONFIG_PROC_FS */
diff --git a/drivers/staging/lustre/lustre/osc/Makefile b/drivers/staging/lustre/lustre/osc/Makefile
index 54927fba4eb4..37cdeea9ac49 100644
--- a/drivers/staging/lustre/lustre/osc/Makefile
+++ b/drivers/staging/lustre/lustre/osc/Makefile
@@ -1,4 +1,3 @@
 obj-$(CONFIG_LUSTRE_FS) += osc.o
 osc-y := osc_request.o osc_dev.o osc_object.o \
-	 osc_page.o osc_lock.o osc_io.o osc_quota.o osc_cache.o
-osc-$(CONFIG_PROC_FS) += lproc_osc.o
+	 osc_page.o osc_lock.o osc_io.o osc_quota.o osc_cache.o lproc_osc.o
diff --git a/drivers/staging/lustre/lustre/osc/lproc_osc.c b/drivers/staging/lustre/lustre/osc/lproc_osc.c
index 15a66209831c..ff6d2e2ffdab 100644
--- a/drivers/staging/lustre/lustre/osc/lproc_osc.c
+++ b/drivers/staging/lustre/lustre/osc/lproc_osc.c
@@ -42,25 +42,25 @@
 #include <linux/seq_file.h>
 #include "osc_internal.h"
 
-static int osc_active_seq_show(struct seq_file *m, void *v)
+static ssize_t active_show(struct kobject *kobj, struct attribute *attr,
+			   char *buf)
 {
-	struct obd_device *dev = m->private;
-
-	LPROCFS_CLIMP_CHECK(dev);
-	seq_printf(m, "%d\n", !dev->u.cli.cl_import->imp_deactive);
-	LPROCFS_CLIMP_EXIT(dev);
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 
-	return 0;
+	return sprintf(buf, "%d\n", !dev->u.cli.cl_import->imp_deactive);
 }
 
-static ssize_t osc_active_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t active_store(struct kobject *kobj, struct attribute *attr,
+			    const char *buffer,
+			    size_t count)
 {
-	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
-	int val, rc;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 	if (val < 0 || val > 1)
@@ -70,41 +70,43 @@ static ssize_t osc_active_seq_write(struct file *file,
 	if (dev->u.cli.cl_import->imp_deactive == val)
 		rc = ptlrpc_set_import_active(dev->u.cli.cl_import, val);
 	else
-		CDEBUG(D_CONFIG, "activate %d: ignoring repeat request\n", val);
+		CDEBUG(D_CONFIG, "activate %ld: ignoring repeat request\n",
+		       val);
 
 	return count;
 }
-LPROC_SEQ_FOPS(osc_active);
+LUSTRE_RW_ATTR(active);
 
-static int osc_max_rpcs_in_flight_seq_show(struct seq_file *m, void *v)
+static ssize_t max_rpcs_in_flight_show(struct kobject *kobj,
+				       struct attribute *attr,
+				       char *buf)
 {
-	struct obd_device *dev = m->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
 
-	client_obd_list_lock(&cli->cl_loi_list_lock);
-	seq_printf(m, "%u\n", cli->cl_max_rpcs_in_flight);
-	client_obd_list_unlock(&cli->cl_loi_list_lock);
-
-	return 0;
+	return sprintf(buf, "%u\n", cli->cl_max_rpcs_in_flight);
 }
 
-static ssize_t osc_max_rpcs_in_flight_seq_write(struct file *file,
-			const char __user *buffer,
-			size_t count, loff_t *off)
+static ssize_t max_rpcs_in_flight_store(struct kobject *kobj,
+					struct attribute *attr,
+					const char *buffer,
+					size_t count)
 {
-	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
 	struct ptlrpc_request_pool *pool = cli->cl_import->imp_rq_pool;
-	int val, rc;
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
 	if (val < 1 || val > OSC_MAX_RIF_MAX)
 		return -ERANGE;
 
-	LPROCFS_CLIMP_CHECK(dev);
 	if (pool && val > cli->cl_max_rpcs_in_flight)
 		pool->prp_populate(pool, val-cli->cl_max_rpcs_in_flight);
 
@@ -112,14 +114,16 @@ static ssize_t osc_max_rpcs_in_flight_seq_write(struct file *file,
 	cli->cl_max_rpcs_in_flight = val;
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	LPROCFS_CLIMP_EXIT(dev);
 	return count;
 }
-LPROC_SEQ_FOPS(osc_max_rpcs_in_flight);
+LUSTRE_RW_ATTR(max_rpcs_in_flight);
 
-static int osc_max_dirty_mb_seq_show(struct seq_file *m, void *v)
+static ssize_t max_dirty_mb_show(struct kobject *kobj,
+				 struct attribute *attr,
+				 char *buf)
 {
-	struct obd_device *dev = m->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
 	long val;
 	int mult;
@@ -129,22 +133,26 @@ static int osc_max_dirty_mb_seq_show(struct seq_file *m, void *v)
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
 	mult = 1 << 20;
-	return lprocfs_seq_read_frac_helper(m, val, mult);
+	return lprocfs_read_frac_helper(buf, PAGE_SIZE, val, mult);
 }
 
-static ssize_t osc_max_dirty_mb_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t max_dirty_mb_store(struct kobject *kobj,
+				  struct attribute *attr,
+				  const char *buffer,
+				  size_t count)
 {
-	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
-	int pages_number, mult, rc;
+	int rc;
+	unsigned long pages_number;
 
-	mult = 1 << (20 - PAGE_CACHE_SHIFT);
-	rc = lprocfs_write_frac_helper(buffer, count, &pages_number, mult);
+	rc = kstrtoul(buffer, 10, &pages_number);
 	if (rc)
 		return rc;
 
+	pages_number *= 1 << (20 - PAGE_CACHE_SHIFT); /* MB -> pages */
+
 	if (pages_number <= 0 ||
 	    pages_number > OSC_MAX_DIRTY_MB_MAX << (20 - PAGE_CACHE_SHIFT) ||
 	    pages_number > totalram_pages / 4) /* 1/4 of RAM */
@@ -157,7 +165,7 @@ static ssize_t osc_max_dirty_mb_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(osc_max_dirty_mb);
+LUSTRE_RW_ATTR(max_dirty_mb);
 
 static int osc_cached_mb_seq_show(struct seq_file *m, void *v)
 {
@@ -210,44 +218,51 @@ static ssize_t osc_cached_mb_seq_write(struct file *file,
 }
 LPROC_SEQ_FOPS(osc_cached_mb);
 
-static int osc_cur_dirty_bytes_seq_show(struct seq_file *m, void *v)
+static ssize_t cur_dirty_bytes_show(struct kobject *kobj,
+				    struct attribute *attr,
+				    char *buf)
 {
-	struct obd_device *dev = m->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
+	int len;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	seq_printf(m, "%lu\n", cli->cl_dirty);
+	len = sprintf(buf, "%lu\n", cli->cl_dirty);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	return 0;
+	return len;
 }
-LPROC_SEQ_FOPS_RO(osc_cur_dirty_bytes);
+LUSTRE_RO_ATTR(cur_dirty_bytes);
 
-static int osc_cur_grant_bytes_seq_show(struct seq_file *m, void *v)
+static ssize_t cur_grant_bytes_show(struct kobject *kobj,
+				    struct attribute *attr,
+				    char *buf)
 {
-	struct obd_device *dev = m->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
+	int len;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	seq_printf(m, "%lu\n", cli->cl_avail_grant);
+	len = sprintf(buf, "%lu\n", cli->cl_avail_grant);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	return 0;
+	return len;
 }
 
-static ssize_t osc_cur_grant_bytes_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t cur_grant_bytes_store(struct kobject *kobj,
+				     struct attribute *attr,
+				     const char *buffer,
+				     size_t count)
 {
-	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &obd->u.cli;
-	int		rc;
-	__u64	      val;
-
-	if (obd == NULL)
-		return 0;
+	int rc;
+	unsigned long long val;
 
-	rc = lprocfs_write_u64_helper(buffer, count, &val);
+	rc = kstrtoull(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -255,54 +270,56 @@ static ssize_t osc_cur_grant_bytes_seq_write(struct file *file,
 	client_obd_list_lock(&cli->cl_loi_list_lock);
 	if (val >= cli->cl_avail_grant) {
 		client_obd_list_unlock(&cli->cl_loi_list_lock);
-		return 0;
+		return -EINVAL;
 	}
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	LPROCFS_CLIMP_CHECK(obd);
 	if (cli->cl_import->imp_state == LUSTRE_IMP_FULL)
 		rc = osc_shrink_grant_to_target(cli, val);
-	LPROCFS_CLIMP_EXIT(obd);
 	if (rc)
 		return rc;
 	return count;
 }
-LPROC_SEQ_FOPS(osc_cur_grant_bytes);
+LUSTRE_RW_ATTR(cur_grant_bytes);
 
-static int osc_cur_lost_grant_bytes_seq_show(struct seq_file *m, void *v)
+static ssize_t cur_lost_grant_bytes_show(struct kobject *kobj,
+					 struct attribute *attr,
+					 char *buf)
 {
-	struct obd_device *dev = m->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
+	int len;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	seq_printf(m, "%lu\n", cli->cl_lost_grant);
+	len = sprintf(buf, "%lu\n", cli->cl_lost_grant);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	return 0;
+	return len;
 }
-LPROC_SEQ_FOPS_RO(osc_cur_lost_grant_bytes);
+LUSTRE_RO_ATTR(cur_lost_grant_bytes);
 
-static int osc_grant_shrink_interval_seq_show(struct seq_file *m, void *v)
+static ssize_t grant_shrink_interval_show(struct kobject *kobj,
+					  struct attribute *attr,
+					  char *buf)
 {
-	struct obd_device *obd = m->private;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 
-	if (obd == NULL)
-		return 0;
-	seq_printf(m, "%d\n", obd->u.cli.cl_grant_shrink_interval);
-	return 0;
+	return sprintf(buf, "%d\n", obd->u.cli.cl_grant_shrink_interval);
 }
 
-static ssize_t osc_grant_shrink_interval_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t grant_shrink_interval_store(struct kobject *kobj,
+					   struct attribute *attr,
+					   const char *buffer,
+					   size_t count)
 {
-	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
-	int val, rc;
-
-	if (obd == NULL)
-		return 0;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -313,30 +330,29 @@ static ssize_t osc_grant_shrink_interval_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(osc_grant_shrink_interval);
+LUSTRE_RW_ATTR(grant_shrink_interval);
 
-static int osc_checksum_seq_show(struct seq_file *m, void *v)
+static ssize_t checksums_show(struct kobject *kobj,
+			      struct attribute *attr,
+			      char *buf)
 {
-	struct obd_device *obd = m->private;
-
-	if (obd == NULL)
-		return 0;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 
-	seq_printf(m, "%d\n", obd->u.cli.cl_checksum ? 1 : 0);
-	return 0;
+	return sprintf(buf, "%d\n", obd->u.cli.cl_checksum ? 1 : 0);
 }
 
-static ssize_t osc_checksum_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t checksums_store(struct kobject *kobj,
+			       struct attribute *attr,
+			       const char *buffer,
+			       size_t count)
 {
-	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
-	int val, rc;
-
-	if (obd == NULL)
-		return 0;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -344,7 +360,7 @@ static ssize_t osc_checksum_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(osc_checksum);
+LUSTRE_RW_ATTR(checksums);
 
 static int osc_checksum_type_seq_show(struct seq_file *m, void *v)
 {
@@ -400,22 +416,27 @@ static ssize_t osc_checksum_type_seq_write(struct file *file,
 }
 LPROC_SEQ_FOPS(osc_checksum_type);
 
-static int osc_resend_count_seq_show(struct seq_file *m, void *v)
+static ssize_t resend_count_show(struct kobject *kobj,
+				 struct attribute *attr,
+				 char *buf)
 {
-	struct obd_device *obd = m->private;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 
-	seq_printf(m, "%u\n", atomic_read(&obd->u.cli.cl_resends));
-	return 0;
+	return sprintf(buf, "%u\n", atomic_read(&obd->u.cli.cl_resends));
 }
 
-static ssize_t osc_resend_count_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t resend_count_store(struct kobject *kobj,
+				  struct attribute *attr,
+				  const char *buffer,
+				  size_t count)
 {
-	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
-	int val, rc;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	int rc;
+	unsigned long val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoul(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -426,75 +447,94 @@ static ssize_t osc_resend_count_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(osc_resend_count);
+LUSTRE_RW_ATTR(resend_count);
 
-static int osc_contention_seconds_seq_show(struct seq_file *m, void *v)
+static ssize_t contention_seconds_show(struct kobject *kobj,
+				       struct attribute *attr,
+				       char *buf)
 {
-	struct obd_device *obd = m->private;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct osc_device *od  = obd2osc_dev(obd);
 
-	seq_printf(m, "%u\n", od->od_contention_time);
-	return 0;
+	return sprintf(buf, "%u\n", od->od_contention_time);
 }
 
-static ssize_t osc_contention_seconds_seq_write(struct file *file,
-					const char __user *buffer,
-					size_t count, loff_t *off)
+static ssize_t contention_seconds_store(struct kobject *kobj,
+					struct attribute *attr,
+					const char *buffer,
+					size_t count)
 {
-	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct osc_device *od  = obd2osc_dev(obd);
 
 	return lprocfs_write_helper(buffer, count, &od->od_contention_time) ?:
 		count;
 }
-LPROC_SEQ_FOPS(osc_contention_seconds);
+LUSTRE_RW_ATTR(contention_seconds);
 
-static int osc_lockless_truncate_seq_show(struct seq_file *m, void *v)
+static ssize_t lockless_truncate_show(struct kobject *kobj,
+				      struct attribute *attr,
+				      char *buf)
 {
-	struct obd_device *obd = m->private;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct osc_device *od  = obd2osc_dev(obd);
 
-	seq_printf(m, "%u\n", od->od_lockless_truncate);
-	return 0;
+	return sprintf(buf, "%u\n", od->od_lockless_truncate);
 }
 
-static ssize_t osc_lockless_truncate_seq_write(struct file *file,
-					const char __user *buffer,
-					size_t count, loff_t *off)
+static ssize_t lockless_truncate_store(struct kobject *kobj,
+				       struct attribute *attr,
+				       const char *buffer,
+				       size_t count)
 {
-	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct osc_device *od  = obd2osc_dev(obd);
 
 	return lprocfs_write_helper(buffer, count, &od->od_lockless_truncate) ?:
 		count;
 }
-LPROC_SEQ_FOPS(osc_lockless_truncate);
+LUSTRE_RW_ATTR(lockless_truncate);
 
-static int osc_destroys_in_flight_seq_show(struct seq_file *m, void *v)
+static ssize_t destroys_in_flight_show(struct kobject *kobj,
+				       struct attribute *attr,
+				       char *buf)
 {
-	struct obd_device *obd = m->private;
+	struct obd_device *obd = container_of(kobj, struct obd_device,
+					      obd_kobj);
 
-	seq_printf(m, "%u\n", atomic_read(&obd->u.cli.cl_destroy_in_flight));
-	return 0;
+	return sprintf(buf, "%u\n",
+		       atomic_read(&obd->u.cli.cl_destroy_in_flight));
 }
-LPROC_SEQ_FOPS_RO(osc_destroys_in_flight);
+LUSTRE_RO_ATTR(destroys_in_flight);
 
-static int osc_obd_max_pages_per_rpc_seq_show(struct seq_file *m, void *v)
+static ssize_t max_pages_per_rpc_show(struct kobject *kobj,
+				      struct attribute *attr,
+				      char *buf)
 {
-	return lprocfs_obd_rd_max_pages_per_rpc(m, m->private);
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
+	struct client_obd *cli = &dev->u.cli;
+
+	return sprintf(buf, "%d\n", cli->cl_max_pages_per_rpc);
 }
 
-static ssize_t osc_obd_max_pages_per_rpc_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t max_pages_per_rpc_store(struct kobject *kobj,
+				       struct attribute *attr,
+				       const char *buffer,
+				       size_t count)
 {
-	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
+	struct obd_device *dev = container_of(kobj, struct obd_device,
+					      obd_kobj);
 	struct client_obd *cli = &dev->u.cli;
 	struct obd_connect_data *ocd = &cli->cl_import->imp_connect_data;
 	int chunk_mask, rc;
-	__u64 val;
+	unsigned long long val;
 
-	rc = lprocfs_write_u64_helper(buffer, count, &val);
+	rc = kstrtoull(buffer, 10, &val);
 	if (rc)
 		return rc;
 
@@ -502,32 +542,21 @@ static ssize_t osc_obd_max_pages_per_rpc_seq_write(struct file *file,
 	if (val >= ONE_MB_BRW_SIZE)
 		val >>= PAGE_CACHE_SHIFT;
 
-	LPROCFS_CLIMP_CHECK(dev);
-
 	chunk_mask = ~((1 << (cli->cl_chunkbits - PAGE_CACHE_SHIFT)) - 1);
 	/* max_pages_per_rpc must be chunk aligned */
 	val = (val + ~chunk_mask) & chunk_mask;
 	if (val == 0 || val > ocd->ocd_brw_size >> PAGE_CACHE_SHIFT) {
-		LPROCFS_CLIMP_EXIT(dev);
 		return -ERANGE;
 	}
 	client_obd_list_lock(&cli->cl_loi_list_lock);
 	cli->cl_max_pages_per_rpc = val;
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	LPROCFS_CLIMP_EXIT(dev);
 	return count;
 }
-LPROC_SEQ_FOPS(osc_obd_max_pages_per_rpc);
+LUSTRE_RW_ATTR(max_pages_per_rpc);
 
-LPROC_SEQ_FOPS_RO_TYPE(osc, uuid);
 LPROC_SEQ_FOPS_RO_TYPE(osc, connect_flags);
-LPROC_SEQ_FOPS_RO_TYPE(osc, blksize);
-LPROC_SEQ_FOPS_RO_TYPE(osc, kbytestotal);
-LPROC_SEQ_FOPS_RO_TYPE(osc, kbytesfree);
-LPROC_SEQ_FOPS_RO_TYPE(osc, kbytesavail);
-LPROC_SEQ_FOPS_RO_TYPE(osc, filestotal);
-LPROC_SEQ_FOPS_RO_TYPE(osc, filesfree);
 LPROC_SEQ_FOPS_RO_TYPE(osc, server_uuid);
 LPROC_SEQ_FOPS_RO_TYPE(osc, conn_uuid);
 LPROC_SEQ_FOPS_RO_TYPE(osc, timeouts);
@@ -539,46 +568,20 @@ LPROC_SEQ_FOPS_RW_TYPE(osc, import);
 LPROC_SEQ_FOPS_RW_TYPE(osc, pinger_recov);
 
 static struct lprocfs_vars lprocfs_osc_obd_vars[] = {
-	{ "uuid",	     &osc_uuid_fops,	NULL, 0 },
 	{ "ping",	     &osc_ping_fops,    NULL, 0222 },
 	{ "connect_flags",   &osc_connect_flags_fops, NULL, 0 },
-	{ "blocksize",       &osc_blksize_fops,     NULL, 0 },
-	{ "kbytestotal",     &osc_kbytestotal_fops, NULL, 0 },
-	{ "kbytesfree",      &osc_kbytesfree_fops,  NULL, 0 },
-	{ "kbytesavail",     &osc_kbytesavail_fops, NULL, 0 },
-	{ "filestotal",      &osc_filestotal_fops,  NULL, 0 },
-	{ "filesfree",       &osc_filesfree_fops,   NULL, 0 },
 	/*{ "filegroups",      lprocfs_rd_filegroups,  NULL, 0 },*/
 	{ "ost_server_uuid", &osc_server_uuid_fops, NULL, 0 },
 	{ "ost_conn_uuid",   &osc_conn_uuid_fops, NULL, 0 },
-	{ "active",	     &osc_active_fops, NULL },
-	{ "max_pages_per_rpc", &osc_obd_max_pages_per_rpc_fops, NULL },
-	{ "max_rpcs_in_flight", &osc_max_rpcs_in_flight_fops, NULL },
-	{ "destroys_in_flight", &osc_destroys_in_flight_fops, NULL, 0 },
-	{ "max_dirty_mb",    &osc_max_dirty_mb_fops, NULL },
 	{ "osc_cached_mb",   &osc_cached_mb_fops, NULL },
-	{ "cur_dirty_bytes", &osc_cur_dirty_bytes_fops, NULL, 0 },
-	{ "cur_grant_bytes", &osc_cur_grant_bytes_fops, NULL },
-	{ "cur_lost_grant_bytes", &osc_cur_lost_grant_bytes_fops, NULL, 0},
-	{ "grant_shrink_interval", &osc_grant_shrink_interval_fops, NULL },
-	{ "checksums",       &osc_checksum_fops, NULL },
 	{ "checksum_type",   &osc_checksum_type_fops, NULL },
-	{ "resend_count",    &osc_resend_count_fops, NULL},
 	{ "timeouts",	     &osc_timeouts_fops, NULL, 0 },
-	{ "contention_seconds", &osc_contention_seconds_fops, NULL },
-	{ "lockless_truncate",  &osc_lockless_truncate_fops, NULL },
 	{ "import",		&osc_import_fops, NULL },
 	{ "state",		&osc_state_fops, NULL, 0 },
 	{ "pinger_recov",	&osc_pinger_recov_fops, NULL },
 	{ NULL }
 };
 
-LPROC_SEQ_FOPS_RO_TYPE(osc, numrefs);
-static struct lprocfs_vars lprocfs_osc_module_vars[] = {
-	{ "num_refs",	&osc_numrefs_fops,     NULL, 0 },
-	{ NULL }
-};
-
 #define pct(a, b) (b ? a * 100 / b : 0)
 
 static int osc_rpc_stats_seq_show(struct seq_file *seq, void *v)
@@ -679,8 +682,8 @@ static int osc_rpc_stats_seq_show(struct seq_file *seq, void *v)
 #undef pct
 
 static ssize_t osc_rpc_stats_seq_write(struct file *file,
-				const char __user *buf,
-				size_t len, loff_t *off)
+				       const char __user *buf,
+				       size_t len, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	struct obd_device *dev = seq->private;
@@ -718,8 +721,8 @@ static int osc_stats_seq_show(struct seq_file *seq, void *v)
 }
 
 static ssize_t osc_stats_seq_write(struct file *file,
-				const char __user *buf,
-				size_t len, loff_t *off)
+				   const char __user *buf,
+				   size_t len, loff_t *off)
 {
 	struct seq_file *seq = file->private_data;
 	struct obd_device *dev = seq->private;
@@ -735,17 +738,38 @@ int lproc_osc_attach_seqstat(struct obd_device *dev)
 {
 	int rc;
 
-	rc = lprocfs_seq_create(dev->obd_proc_entry, "osc_stats", 0644,
-				&osc_stats_fops, dev);
+	rc = ldebugfs_seq_create(dev->obd_debugfs_entry, "osc_stats", 0644,
+				 &osc_stats_fops, dev);
 	if (rc == 0)
-		rc = lprocfs_obd_seq_create(dev, "rpc_stats", 0644,
-					    &osc_rpc_stats_fops, dev);
+		rc = ldebugfs_obd_seq_create(dev, "rpc_stats", 0644,
+					     &osc_rpc_stats_fops, dev);
 
 	return rc;
 }
 
+static struct attribute *osc_attrs[] = {
+	&lustre_attr_active.attr,
+	&lustre_attr_checksums.attr,
+	&lustre_attr_contention_seconds.attr,
+	&lustre_attr_cur_dirty_bytes.attr,
+	&lustre_attr_cur_grant_bytes.attr,
+	&lustre_attr_cur_lost_grant_bytes.attr,
+	&lustre_attr_destroys_in_flight.attr,
+	&lustre_attr_grant_shrink_interval.attr,
+	&lustre_attr_lockless_truncate.attr,
+	&lustre_attr_max_dirty_mb.attr,
+	&lustre_attr_max_pages_per_rpc.attr,
+	&lustre_attr_max_rpcs_in_flight.attr,
+	&lustre_attr_resend_count.attr,
+	NULL,
+};
+
+static struct attribute_group osc_attr_group = {
+	.attrs = osc_attrs,
+};
+
 void lprocfs_osc_init_vars(struct lprocfs_static_vars *lvars)
 {
-	lvars->module_vars = lprocfs_osc_module_vars;
+	lvars->sysfs_vars  = &osc_attr_group;
 	lvars->obd_vars    = lprocfs_osc_obd_vars;
 }
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index d44b3d4ffe4d..5592d32a1a95 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -112,8 +112,8 @@ static const char *oes_strings[] = {
 		/* ----- extent part 0 ----- */				      \
 		__ext, EXTPARA(__ext),					      \
 		/* ----- part 1 ----- */				      \
-		atomic_read(&__ext->oe_refc),			      \
-		atomic_read(&__ext->oe_users),			      \
+		atomic_read(&__ext->oe_refc),				      \
+		atomic_read(&__ext->oe_users),				      \
 		list_empty_marker(&__ext->oe_link),			      \
 		oes_strings[__ext->oe_state], ext_flags(__ext, __buf),	      \
 		__ext->oe_obj,						      \
@@ -297,12 +297,12 @@ out:
 #define sanity_check_nolock(ext) \
 	osc_extent_sanity_check0(ext, __func__, __LINE__)
 
-#define sanity_check(ext) ({						   \
-	int __res;							     \
+#define sanity_check(ext) ({						\
+	int __res;							\
 	osc_object_lock((ext)->oe_obj);					\
-	__res = sanity_check_nolock(ext);				      \
-	osc_object_unlock((ext)->oe_obj);				      \
-	__res;								 \
+	__res = sanity_check_nolock(ext);				\
+	osc_object_unlock((ext)->oe_obj);				\
+	__res;								\
 })
 
 
@@ -411,7 +411,7 @@ static void osc_extent_put_trust(struct osc_extent *ext)
 static struct osc_extent *osc_extent_search(struct osc_object *obj,
 					    pgoff_t index)
 {
-	struct rb_node    *n = obj->oo_root.rb_node;
+	struct rb_node *n = obj->oo_root.rb_node;
 	struct osc_extent *tmp, *p = NULL;
 
 	LASSERT(osc_object_is_locked(obj));
@@ -447,8 +447,8 @@ static struct osc_extent *osc_extent_lookup(struct osc_object *obj,
 /* caller must have held object lock. */
 static void osc_extent_insert(struct osc_object *obj, struct osc_extent *ext)
 {
-	struct rb_node   **n      = &obj->oo_root.rb_node;
-	struct rb_node    *parent = NULL;
+	struct rb_node **n = &obj->oo_root.rb_node;
+	struct rb_node *parent = NULL;
 	struct osc_extent *tmp;
 
 	LASSERT(ext->oe_intree == 0);
@@ -544,19 +544,19 @@ static int osc_extent_merge(const struct lu_env *env, struct osc_extent *cur,
 	LASSERT(cur->oe_osclock == victim->oe_osclock);
 	ppc_bits = osc_cli(obj)->cl_chunkbits - PAGE_CACHE_SHIFT;
 	chunk_start = cur->oe_start >> ppc_bits;
-	chunk_end   = cur->oe_end   >> ppc_bits;
-	if (chunk_start   != (victim->oe_end >> ppc_bits) + 1 &&
+	chunk_end = cur->oe_end >> ppc_bits;
+	if (chunk_start != (victim->oe_end >> ppc_bits) + 1 &&
 	    chunk_end + 1 != victim->oe_start >> ppc_bits)
 		return -ERANGE;
 
 	OSC_EXTENT_DUMP(D_CACHE, victim, "will be merged by %p.\n", cur);
 
-	cur->oe_start     = min(cur->oe_start, victim->oe_start);
-	cur->oe_end       = max(cur->oe_end,   victim->oe_end);
-	cur->oe_grants   += victim->oe_grants;
+	cur->oe_start = min(cur->oe_start, victim->oe_start);
+	cur->oe_end = max(cur->oe_end, victim->oe_end);
+	cur->oe_grants += victim->oe_grants;
 	cur->oe_nr_pages += victim->oe_nr_pages;
 	/* only the following bits are needed to merge */
-	cur->oe_urgent   |= victim->oe_urgent;
+	cur->oe_urgent |= victim->oe_urgent;
 	cur->oe_memalloc |= victim->oe_memalloc;
 	list_splice_init(&victim->oe_pages, &cur->oe_pages);
 	list_del_init(&victim->oe_link);
@@ -624,18 +624,18 @@ struct osc_extent *osc_extent_find(const struct lu_env *env,
 
 {
 	struct client_obd *cli = osc_cli(obj);
-	struct cl_lock    *lock;
+	struct cl_lock *lock;
 	struct osc_extent *cur;
 	struct osc_extent *ext;
 	struct osc_extent *conflict = NULL;
 	struct osc_extent *found = NULL;
-	pgoff_t    chunk;
-	pgoff_t    max_end;
-	int	max_pages; /* max_pages_per_rpc */
-	int	chunksize;
-	int	ppc_bits; /* pages per chunk bits */
-	int	chunk_mask;
-	int	rc;
+	pgoff_t chunk;
+	pgoff_t max_end;
+	int max_pages; /* max_pages_per_rpc */
+	int chunksize;
+	int ppc_bits; /* pages per chunk bits */
+	int chunk_mask;
+	int rc;
 
 	cur = osc_extent_alloc(obj);
 	if (cur == NULL)
@@ -646,10 +646,10 @@ struct osc_extent *osc_extent_find(const struct lu_env *env,
 	LASSERT(lock->cll_descr.cld_mode >= CLM_WRITE);
 
 	LASSERT(cli->cl_chunkbits >= PAGE_CACHE_SHIFT);
-	ppc_bits   = cli->cl_chunkbits - PAGE_CACHE_SHIFT;
+	ppc_bits = cli->cl_chunkbits - PAGE_CACHE_SHIFT;
 	chunk_mask = ~((1 << ppc_bits) - 1);
-	chunksize  = 1 << cli->cl_chunkbits;
-	chunk      = index >> ppc_bits;
+	chunksize = 1 << cli->cl_chunkbits;
+	chunk = index >> ppc_bits;
 
 	/* align end to rpc edge, rpc size may not be a power 2 integer. */
 	max_pages = cli->cl_max_pages_per_rpc;
@@ -659,15 +659,15 @@ struct osc_extent *osc_extent_find(const struct lu_env *env,
 
 	/* initialize new extent by parameters so far */
 	cur->oe_max_end = max_end;
-	cur->oe_start   = index & chunk_mask;
-	cur->oe_end     = ((index + ~chunk_mask + 1) & chunk_mask) - 1;
+	cur->oe_start = index & chunk_mask;
+	cur->oe_end = ((index + ~chunk_mask + 1) & chunk_mask) - 1;
 	if (cur->oe_start < lock->cll_descr.cld_start)
 		cur->oe_start = lock->cll_descr.cld_start;
 	if (cur->oe_end > max_end)
 		cur->oe_end = max_end;
 	cur->oe_osclock = lock;
-	cur->oe_grants  = 0;
-	cur->oe_mppr    = max_pages;
+	cur->oe_grants = 0;
+	cur->oe_mppr = max_pages;
 
 	/* grants has been allocated by caller */
 	LASSERTF(*grants >= chunksize + cli->cl_extent_tax,
@@ -681,7 +681,7 @@ restart:
 		ext = first_extent(obj);
 	while (ext != NULL) {
 		loff_t ext_chk_start = ext->oe_start >> ppc_bits;
-		loff_t ext_chk_end   = ext->oe_end   >> ppc_bits;
+		loff_t ext_chk_end = ext->oe_end >> ppc_bits;
 
 		LASSERT(sanity_check_nolock(ext) == 0);
 		if (chunk > ext_chk_end + 1)
@@ -755,14 +755,14 @@ restart:
 			EASSERT((ext->oe_start & ~chunk_mask) == 0, ext);
 
 			/* pull ext's start back to cover cur */
-			ext->oe_start   = cur->oe_start;
+			ext->oe_start = cur->oe_start;
 			ext->oe_grants += chunksize;
 			*grants -= chunksize;
 
 			found = osc_extent_hold(ext);
 		} else if (chunk == ext_chk_end + 1) {
 			/* rear merge */
-			ext->oe_end     = cur->oe_end;
+			ext->oe_end = cur->oe_end;
 			ext->oe_grants += chunksize;
 			*grants -= chunksize;
 
@@ -943,21 +943,21 @@ static int osc_extent_wait(const struct lu_env *env, struct osc_extent *ext,
  * @size, then partial truncate happens.
  */
 static int osc_extent_truncate(struct osc_extent *ext, pgoff_t trunc_index,
-				bool partial)
+			       bool partial)
 {
-	struct cl_env_nest     nest;
-	struct lu_env	 *env;
-	struct cl_io	  *io;
-	struct osc_object     *obj = ext->oe_obj;
-	struct client_obd     *cli = osc_cli(obj);
+	struct cl_env_nest nest;
+	struct lu_env *env;
+	struct cl_io *io;
+	struct osc_object *obj = ext->oe_obj;
+	struct client_obd *cli = osc_cli(obj);
 	struct osc_async_page *oap;
 	struct osc_async_page *tmp;
-	int		    pages_in_chunk = 0;
-	int		    ppc_bits    = cli->cl_chunkbits - PAGE_CACHE_SHIFT;
-	__u64		  trunc_chunk = trunc_index >> ppc_bits;
-	int		    grants   = 0;
-	int		    nr_pages = 0;
-	int		    rc       = 0;
+	int pages_in_chunk = 0;
+	int ppc_bits = cli->cl_chunkbits - PAGE_CACHE_SHIFT;
+	__u64 trunc_chunk = trunc_index >> ppc_bits;
+	int grants = 0;
+	int nr_pages = 0;
+	int rc = 0;
 
 	LASSERT(sanity_check(ext) == 0);
 	EASSERT(ext->oe_state == OES_TRUNC, ext);
@@ -976,8 +976,8 @@ static int osc_extent_truncate(struct osc_extent *ext, pgoff_t trunc_index,
 	/* discard all pages with index greater then trunc_index */
 	list_for_each_entry_safe(oap, tmp, &ext->oe_pages,
 				     oap_pending_item) {
-		struct cl_page  *sub  = oap2cl_page(oap);
-		struct cl_page  *page = cl_page_top(sub);
+		struct cl_page *sub = oap2cl_page(oap);
+		struct cl_page *page = cl_page_top(sub);
 
 		LASSERT(list_empty(&oap->oap_rpc_item));
 
@@ -1022,7 +1022,7 @@ static int osc_extent_truncate(struct osc_extent *ext, pgoff_t trunc_index,
 		grants = ext->oe_grants;
 		ext->oe_grants = 0;
 	} else { /* calculate how many grants we can free */
-		int     chunks = (ext->oe_end >> ppc_bits) - trunc_chunk;
+		int chunks = (ext->oe_end >> ppc_bits) - trunc_chunk;
 		pgoff_t last_index;
 
 
@@ -1038,10 +1038,10 @@ static int osc_extent_truncate(struct osc_extent *ext, pgoff_t trunc_index,
 		}
 
 		/* this is what we can free from this extent */
-		grants	  = chunks << cli->cl_chunkbits;
+		grants = chunks << cli->cl_chunkbits;
 		ext->oe_grants -= grants;
-		last_index      = ((trunc_chunk + 1) << ppc_bits) - 1;
-		ext->oe_end     = min(last_index, ext->oe_max_end);
+		last_index = ((trunc_chunk + 1) << ppc_bits) - 1;
+		ext->oe_end = min(last_index, ext->oe_max_end);
 		LASSERT(ext->oe_end >= ext->oe_start);
 		LASSERT(ext->oe_grants > 0);
 	}
@@ -1236,8 +1236,8 @@ static inline int osc_is_ready(struct osc_object *osc)
 static int osc_make_ready(const struct lu_env *env, struct osc_async_page *oap,
 			  int cmd)
 {
-	struct osc_page *opg  = oap2osc_page(oap);
-	struct cl_page  *page = cl_page_top(oap2cl_page(oap));
+	struct osc_page *opg = oap2osc_page(oap);
+	struct cl_page *page = cl_page_top(oap2cl_page(oap));
 	int result;
 
 	LASSERT(cmd == OBD_BRW_WRITE); /* no cached reads */
@@ -1251,10 +1251,10 @@ static int osc_make_ready(const struct lu_env *env, struct osc_async_page *oap,
 static int osc_refresh_count(const struct lu_env *env,
 			     struct osc_async_page *oap, int cmd)
 {
-	struct osc_page  *opg = oap2osc_page(oap);
-	struct cl_page   *page = oap2cl_page(oap);
+	struct osc_page *opg = oap2osc_page(oap);
+	struct cl_page *page = oap2cl_page(oap);
 	struct cl_object *obj;
-	struct cl_attr   *attr = &osc_env_info(env)->oti_attr;
+	struct cl_attr *attr = &osc_env_info(env)->oti_attr;
 
 	int result;
 	loff_t kms;
@@ -1283,10 +1283,10 @@ static int osc_refresh_count(const struct lu_env *env,
 static int osc_completion(const struct lu_env *env, struct osc_async_page *oap,
 			  int cmd, int rc)
 {
-	struct osc_page   *opg  = oap2osc_page(oap);
-	struct cl_page    *page = cl_page_top(oap2cl_page(oap));
-	struct osc_object *obj  = cl2osc(opg->ops_cl.cpl_obj);
-	enum cl_req_type   crt;
+	struct osc_page *opg = oap2osc_page(oap);
+	struct cl_page *page = cl_page_top(oap2cl_page(oap));
+	struct osc_object *obj = cl2osc(opg->ops_cl.cpl_obj);
+	enum cl_req_type crt;
 	int srvlock;
 
 	cmd &= ~OBD_BRW_NOQUOTA;
@@ -1318,7 +1318,7 @@ static int osc_completion(const struct lu_env *env, struct osc_async_page *oap,
 
 	/* statistic */
 	if (rc == 0 && srvlock) {
-		struct lu_device *ld    = opg->ops_cl.cpl_obj->co_lu.lo_dev;
+		struct lu_device *ld = opg->ops_cl.cpl_obj->co_lu.lo_dev;
 		struct osc_stats *stats = &lu2osc_dev(ld)->od_stats;
 		int bytes = oap->oap_count;
 
@@ -1396,7 +1396,7 @@ static int osc_reserve_grant(struct client_obd *cli, unsigned int bytes)
 	int rc = -EDQUOT;
 
 	if (cli->cl_avail_grant >= bytes) {
-		cli->cl_avail_grant    -= bytes;
+		cli->cl_avail_grant -= bytes;
 		cli->cl_reserved_grant += bytes;
 		rc = 0;
 	}
@@ -1527,7 +1527,7 @@ static int osc_enter_cache(const struct lu_env *env, struct client_obd *cli,
 			   struct osc_async_page *oap, int bytes)
 {
 	struct osc_object *osc = oap->oap_obj;
-	struct lov_oinfo  *loi = osc->oo_oinfo;
+	struct lov_oinfo *loi = osc->oo_oinfo;
 	struct osc_cache_waiter ocw;
 	struct l_wait_info lwi = LWI_INTR(LWI_ON_SIGNAL_NOOP, NULL);
 	int rc = -EDQUOT;
@@ -1782,7 +1782,7 @@ static void osc_ap_completion(const struct lu_env *env, struct client_obd *cli,
 			      struct osc_async_page *oap, int sent, int rc)
 {
 	struct osc_object *osc = oap->oap_obj;
-	struct lov_oinfo  *loi = osc->oo_oinfo;
+	struct lov_oinfo *loi = osc->oo_oinfo;
 	__u64 xid = 0;
 
 	if (oap->oap_request != NULL) {
@@ -2049,7 +2049,7 @@ osc_send_read_rpc(const struct lu_env *env, struct client_obd *cli,
 
 #define list_to_obj(list, item) ({					      \
 	struct list_head *__tmp = (list)->next;				      \
-	list_del_init(__tmp);					      \
+	list_del_init(__tmp);						      \
 	list_entry(__tmp, struct osc_object, oo_##item);		      \
 })
 
@@ -2179,7 +2179,7 @@ static int osc_io_unplug0(const struct lu_env *env, struct client_obd *cli,
 }
 
 static int osc_io_unplug_async(const struct lu_env *env,
-				struct client_obd *cli, struct osc_object *osc)
+			       struct client_obd *cli, struct osc_object *osc)
 {
 	/* XXX: policy is no use actually. */
 	return osc_io_unplug0(env, cli, osc, PDL_POLICY_ROUND, 1);
@@ -2194,7 +2194,7 @@ void osc_io_unplug(const struct lu_env *env, struct client_obd *cli,
 int osc_prep_async_page(struct osc_object *osc, struct osc_page *ops,
 			struct page *page, loff_t offset)
 {
-	struct obd_export     *exp = osc_export(osc);
+	struct obd_export *exp = osc_export(osc);
 	struct osc_async_page *oap = &ops->ops_oap;
 
 	if (!page)
@@ -2224,16 +2224,16 @@ int osc_queue_async_io(const struct lu_env *env, struct cl_io *io,
 		       struct osc_page *ops)
 {
 	struct osc_io *oio = osc_env_io(env);
-	struct osc_extent     *ext = NULL;
+	struct osc_extent *ext = NULL;
 	struct osc_async_page *oap = &ops->ops_oap;
-	struct client_obd     *cli = oap->oap_cli;
-	struct osc_object     *osc = oap->oap_obj;
+	struct client_obd *cli = oap->oap_cli;
+	struct osc_object *osc = oap->oap_obj;
 	pgoff_t index;
-	int    grants = 0;
-	int    brw_flags = OBD_BRW_ASYNC;
-	int    cmd = OBD_BRW_WRITE;
-	int    need_release = 0;
-	int    rc = 0;
+	int grants = 0;
+	int brw_flags = OBD_BRW_ASYNC;
+	int cmd = OBD_BRW_WRITE;
+	int need_release = 0;
+	int rc = 0;
 
 	if (oap->oap_magic != OAP_MAGIC)
 		return -EINVAL;
@@ -2256,7 +2256,7 @@ int osc_queue_async_io(const struct lu_env *env, struct cl_io *io,
 	/* check if the file's owner/group is over quota */
 	if (!(cmd & OBD_BRW_NOQUOTA)) {
 		struct cl_object *obj;
-		struct cl_attr   *attr;
+		struct cl_attr *attr;
 		unsigned int qid[MAXQUOTAS];
 
 		obj = cl_object_top(&osc->oo_cl);
@@ -2386,7 +2386,7 @@ int osc_teardown_async_page(const struct lu_env *env,
 			    struct osc_object *obj, struct osc_page *ops)
 {
 	struct osc_async_page *oap = &ops->ops_oap;
-	struct osc_extent     *ext = NULL;
+	struct osc_extent *ext = NULL;
 	int rc = 0;
 
 	LASSERT(oap->oap_magic == OAP_MAGIC);
@@ -2425,10 +2425,10 @@ int osc_teardown_async_page(const struct lu_env *env,
 int osc_flush_async_page(const struct lu_env *env, struct cl_io *io,
 			 struct osc_page *ops)
 {
-	struct osc_extent *ext   = NULL;
-	struct osc_object *obj   = cl2osc(ops->ops_cl.cpl_obj);
-	struct cl_page    *cp    = ops->ops_cl.cpl_page;
-	pgoff_t	    index = cp->cp_index;
+	struct osc_extent *ext = NULL;
+	struct osc_object *obj = cl2osc(ops->ops_cl.cpl_obj);
+	struct cl_page *cp = ops->ops_cl.cpl_page;
+	pgoff_t	index = cp->cp_index;
 	struct osc_async_page *oap = &ops->ops_oap;
 	bool unplug = false;
 	int rc = 0;
@@ -2507,14 +2507,14 @@ out:
 int osc_cancel_async_page(const struct lu_env *env, struct osc_page *ops)
 {
 	struct osc_async_page *oap = &ops->ops_oap;
-	struct osc_object     *obj = oap->oap_obj;
-	struct client_obd     *cli = osc_cli(obj);
-	struct osc_extent     *ext;
-	struct osc_extent     *found = NULL;
-	struct list_head	    *plist;
+	struct osc_object *obj = oap->oap_obj;
+	struct client_obd *cli = osc_cli(obj);
+	struct osc_extent *ext;
+	struct osc_extent *found = NULL;
+	struct list_head *plist;
 	pgoff_t index = oap2cl_page(oap)->cp_index;
-	int     rc = -EBUSY;
-	int     cmd;
+	int rc = -EBUSY;
+	int cmd;
 
 	LASSERT(!oap->oap_interrupted);
 	oap->oap_interrupted = 1;
@@ -2523,10 +2523,10 @@ int osc_cancel_async_page(const struct lu_env *env, struct osc_page *ops)
 	osc_object_lock(obj);
 	if (oap->oap_cmd & OBD_BRW_WRITE) {
 		plist = &obj->oo_urgent_exts;
-		cmd   = OBD_BRW_WRITE;
+		cmd = OBD_BRW_WRITE;
 	} else {
 		plist = &obj->oo_reading_exts;
-		cmd   = OBD_BRW_READ;
+		cmd = OBD_BRW_READ;
 	}
 	list_for_each_entry(ext, plist, oe_link) {
 		if (ext->oe_start <= index && ext->oe_end >= index) {
@@ -2564,13 +2564,13 @@ int osc_cancel_async_page(const struct lu_env *env, struct osc_page *ops)
 int osc_queue_sync_pages(const struct lu_env *env, struct osc_object *obj,
 			 struct list_head *list, int cmd, int brw_flags)
 {
-	struct client_obd     *cli = osc_cli(obj);
-	struct osc_extent     *ext;
+	struct client_obd *cli = osc_cli(obj);
+	struct osc_extent *ext;
 	struct osc_async_page *oap, *tmp;
-	int     page_count = 0;
-	int     mppr       = cli->cl_max_pages_per_rpc;
-	pgoff_t start      = CL_PAGE_EOF;
-	pgoff_t end	= 0;
+	int page_count = 0;
+	int mppr = cli->cl_max_pages_per_rpc;
+	pgoff_t start = CL_PAGE_EOF;
+	pgoff_t end = 0;
 
 	list_for_each_entry(oap, list, oap_pending_item) {
 		struct cl_page *cp = oap2cl_page(oap);
@@ -2785,7 +2785,7 @@ int osc_cache_wait_range(const struct lu_env *env, struct osc_object *obj,
 {
 	struct osc_extent *ext;
 	pgoff_t index = start;
-	int     result = 0;
+	int result = 0;
 
 again:
 	osc_object_lock(obj);
diff --git a/drivers/staging/lustre/lustre/osc/osc_dev.c b/drivers/staging/lustre/lustre/osc/osc_dev.c
index 4935fc7c0706..9222c9f4faae 100644
--- a/drivers/staging/lustre/lustre/osc/osc_dev.c
+++ b/drivers/staging/lustre/lustre/osc/osc_dev.c
@@ -118,7 +118,7 @@ static struct lu_device *osc2lu_dev(struct osc_device *osc)
  */
 
 static void *osc_key_init(const struct lu_context *ctx,
-			 struct lu_context_key *key)
+			  struct lu_context_key *key)
 {
 	struct osc_thread_info *info;
 
@@ -204,7 +204,7 @@ static struct lu_device *osc_device_free(const struct lu_env *env,
 	struct osc_device *od = lu2osc_dev(d);
 
 	cl_device_fini(lu2cl_dev(d));
-	OBD_FREE_PTR(od);
+	kfree(od);
 	return NULL;
 }
 
@@ -217,7 +217,7 @@ static struct lu_device *osc_device_alloc(const struct lu_env *env,
 	struct obd_device *obd;
 	int rc;
 
-	OBD_ALLOC_PTR(od);
+	od = kzalloc(sizeof(*od), GFP_NOFS);
 	if (od == NULL)
 		return ERR_PTR(-ENOMEM);
 
@@ -248,14 +248,14 @@ static const struct lu_device_type_operations osc_device_type_ops = {
 	.ldto_device_alloc = osc_device_alloc,
 	.ldto_device_free  = osc_device_free,
 
-	.ldto_device_init    = osc_device_init,
-	.ldto_device_fini    = osc_device_fini
+	.ldto_device_init = osc_device_init,
+	.ldto_device_fini = osc_device_fini
 };
 
 struct lu_device_type osc_device_type = {
-	.ldt_tags     = LU_DEVICE_CL,
-	.ldt_name     = LUSTRE_OSC_NAME,
-	.ldt_ops      = &osc_device_type_ops,
+	.ldt_tags = LU_DEVICE_CL,
+	.ldt_name = LUSTRE_OSC_NAME,
+	.ldt_ops = &osc_device_type_ops,
 	.ldt_ctx_tags = LCT_CL_THREAD
 };
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_internal.h b/drivers/staging/lustre/lustre/osc/osc_internal.h
index af96c7bc7764..470698b0dd75 100644
--- a/drivers/staging/lustre/lustre/osc/osc_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_internal.h
@@ -136,16 +136,8 @@ extern spinlock_t osc_ast_guard;
 int osc_cleanup(struct obd_device *obd);
 int osc_setup(struct obd_device *obd, struct lustre_cfg *lcfg);
 
-#if defined (CONFIG_PROC_FS)
 int lproc_osc_attach_seqstat(struct obd_device *dev);
 void lprocfs_osc_init_vars(struct lprocfs_static_vars *lvars);
-#else
-static inline int lproc_osc_attach_seqstat(struct obd_device *dev) {return 0;}
-static inline void lprocfs_osc_init_vars(struct lprocfs_static_vars *lvars)
-{
-	memset(lvars, 0, sizeof(*lvars));
-}
-#endif
 
 extern struct lu_device_type osc_device_type;
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_io.c b/drivers/staging/lustre/lustre/osc/osc_io.c
index 3c7300b0651d..fa24e9ed1831 100644
--- a/drivers/staging/lustre/lustre/osc/osc_io.c
+++ b/drivers/staging/lustre/lustre/osc/osc_io.c
@@ -100,16 +100,16 @@ static int osc_io_submit(const struct lu_env *env,
 			 const struct cl_io_slice *ios,
 			 enum cl_req_type crt, struct cl_2queue *queue)
 {
-	struct cl_page    *page;
-	struct cl_page    *tmp;
-	struct client_obd *cli  = NULL;
-	struct osc_object *osc  = NULL; /* to keep gcc happy */
-	struct osc_page   *opg;
-	struct cl_io      *io;
+	struct cl_page *page;
+	struct cl_page *tmp;
+	struct client_obd *cli = NULL;
+	struct osc_object *osc = NULL; /* to keep gcc happy */
+	struct osc_page *opg;
+	struct cl_io *io;
 	LIST_HEAD(list);
 
-	struct cl_page_list *qin      = &queue->c2_qin;
-	struct cl_page_list *qout     = &queue->c2_qout;
+	struct cl_page_list *qin = &queue->c2_qin;
+	struct cl_page_list *qout = &queue->c2_qout;
 	int queued = 0;
 	int result = 0;
 	int cmd;
@@ -189,8 +189,8 @@ static int osc_io_submit(const struct lu_env *env,
 static void osc_page_touch_at(const struct lu_env *env,
 			      struct cl_object *obj, pgoff_t idx, unsigned to)
 {
-	struct lov_oinfo  *loi  = cl2osc(obj)->oo_oinfo;
-	struct cl_attr    *attr = &osc_env_info(env)->oti_attr;
+	struct lov_oinfo *loi = cl2osc(obj)->oo_oinfo;
+	struct cl_attr *attr = &osc_env_info(env)->oti_attr;
 	int valid;
 	__u64 kms;
 
@@ -233,8 +233,8 @@ static void osc_page_touch_at(const struct lu_env *env,
 static void osc_page_touch(const struct lu_env *env,
 			   struct osc_page *opage, unsigned to)
 {
-	struct cl_page    *page = opage->ops_cl.cpl_page;
-	struct cl_object  *obj  = opage->ops_cl.cpl_obj;
+	struct cl_page *page = opage->ops_cl.cpl_page;
+	struct cl_object *obj = opage->ops_cl.cpl_obj;
 
 	osc_page_touch_at(env, obj, page->cp_index, to);
 }
@@ -260,7 +260,7 @@ static int osc_io_prepare_write(const struct lu_env *env,
 {
 	struct osc_device *dev = lu2osc_dev(slice->cpl_obj->co_lu.lo_dev);
 	struct obd_import *imp = class_exp2cliimp(dev->od_exp);
-	struct osc_io     *oio = cl2osc_io(env, ios);
+	struct osc_io *oio = cl2osc_io(env, ios);
 	int result = 0;
 
 	/*
@@ -284,9 +284,9 @@ static int osc_io_commit_write(const struct lu_env *env,
 			       const struct cl_page_slice *slice,
 			       unsigned from, unsigned to)
 {
-	struct osc_io	 *oio = cl2osc_io(env, ios);
-	struct osc_page       *opg = cl2osc_page(slice);
-	struct osc_object     *obj = cl2osc(opg->ops_cl.cpl_obj);
+	struct osc_io *oio = cl2osc_io(env, ios);
+	struct osc_page *opg = cl2osc_page(slice);
+	struct osc_object *obj = cl2osc(opg->ops_cl.cpl_obj);
 	struct osc_async_page *oap = &opg->ops_oap;
 
 	LASSERT(to > 0);
@@ -311,10 +311,10 @@ static int osc_io_commit_write(const struct lu_env *env,
 static int osc_io_fault_start(const struct lu_env *env,
 			      const struct cl_io_slice *ios)
 {
-	struct cl_io       *io;
+	struct cl_io *io;
 	struct cl_fault_io *fio;
 
-	io  = ios->cis_io;
+	io = ios->cis_io;
 	fio = &io->u.ci_fault;
 	CDEBUG(D_INFO, "%lu %d %d\n",
 	       fio->ft_index, fio->ft_writable, fio->ft_nob);
@@ -375,11 +375,11 @@ static void osc_trunc_check(const struct lu_env *env, struct cl_io *io,
 			    struct osc_io *oio, __u64 size)
 {
 	struct cl_object *clob;
-	int     partial;
+	int partial;
 	pgoff_t start;
 
-	clob    = oio->oi_cl.cis_obj;
-	start   = cl_index(clob, size);
+	clob = oio->oi_cl.cis_obj;
+	start = cl_index(clob, size);
 	partial = cl_offset(clob, start) < size;
 
 	/*
@@ -392,17 +392,17 @@ static void osc_trunc_check(const struct lu_env *env, struct cl_io *io,
 static int osc_io_setattr_start(const struct lu_env *env,
 				const struct cl_io_slice *slice)
 {
-	struct cl_io	    *io     = slice->cis_io;
-	struct osc_io	   *oio    = cl2osc_io(env, slice);
-	struct cl_object	*obj    = slice->cis_obj;
-	struct lov_oinfo	*loi    = cl2osc(obj)->oo_oinfo;
-	struct cl_attr	  *attr   = &osc_env_info(env)->oti_attr;
-	struct obdo	     *oa     = &oio->oi_oa;
+	struct cl_io *io = slice->cis_io;
+	struct osc_io *oio = cl2osc_io(env, slice);
+	struct cl_object *obj = slice->cis_obj;
+	struct lov_oinfo *loi = cl2osc(obj)->oo_oinfo;
+	struct cl_attr *attr = &osc_env_info(env)->oti_attr;
+	struct obdo *oa = &oio->oi_oa;
 	struct osc_async_cbargs *cbargs = &oio->oi_cbarg;
-	__u64		    size   = io->u.ci_setattr.sa_attr.lvb_size;
-	unsigned int	     ia_valid = io->u.ci_setattr.sa_valid;
-	int		      result = 0;
-	struct obd_info	  oinfo = { { { 0 } } };
+	__u64 size = io->u.ci_setattr.sa_attr.lvb_size;
+	unsigned int ia_valid = io->u.ci_setattr.sa_valid;
+	int result = 0;
+	struct obd_info oinfo = { { { 0 } } };
 
 	/* truncate cache dirty pages first */
 	if (cl_io_is_trunc(io))
@@ -477,8 +477,8 @@ static int osc_io_setattr_start(const struct lu_env *env,
 static void osc_io_setattr_end(const struct lu_env *env,
 			       const struct cl_io_slice *slice)
 {
-	struct cl_io     *io  = slice->cis_io;
-	struct osc_io    *oio = cl2osc_io(env, slice);
+	struct cl_io *io = slice->cis_io;
+	struct osc_io *oio = cl2osc_io(env, slice);
 	struct cl_object *obj = slice->cis_obj;
 	struct osc_async_cbargs *cbargs = &oio->oi_cbarg;
 	int result = 0;
@@ -512,8 +512,8 @@ static void osc_io_setattr_end(const struct lu_env *env,
 static int osc_io_read_start(const struct lu_env *env,
 			     const struct cl_io_slice *slice)
 {
-	struct cl_object *obj   = slice->cis_obj;
-	struct cl_attr   *attr  = &osc_env_info(env)->oti_attr;
+	struct cl_object *obj = slice->cis_obj;
+	struct cl_attr *attr = &osc_env_info(env)->oti_attr;
 	int rc = 0;
 
 	if (!slice->cis_io->ci_noatime) {
@@ -528,8 +528,8 @@ static int osc_io_read_start(const struct lu_env *env,
 static int osc_io_write_start(const struct lu_env *env,
 			      const struct cl_io_slice *slice)
 {
-	struct cl_object *obj   = slice->cis_obj;
-	struct cl_attr   *attr  = &osc_env_info(env)->oti_attr;
+	struct cl_object *obj = slice->cis_obj;
+	struct cl_attr *attr = &osc_env_info(env)->oti_attr;
 	int rc = 0;
 
 	OBD_FAIL_TIMEOUT(OBD_FAIL_OSC_DELAY_SETTIME, 1);
@@ -544,10 +544,10 @@ static int osc_io_write_start(const struct lu_env *env,
 static int osc_fsync_ost(const struct lu_env *env, struct osc_object *obj,
 			 struct cl_fsync_io *fio)
 {
-	struct osc_io    *oio   = osc_env_io(env);
-	struct obdo      *oa    = &oio->oi_oa;
-	struct obd_info  *oinfo = &oio->oi_info;
-	struct lov_oinfo *loi   = obj->oo_oinfo;
+	struct osc_io *oio = osc_env_io(env);
+	struct obdo *oa = &oio->oi_oa;
+	struct obd_info *oinfo = &oio->oi_info;
+	struct lov_oinfo *loi = obj->oo_oinfo;
 	struct osc_async_cbargs *cbargs = &oio->oi_cbarg;
 	int rc = 0;
 
@@ -575,13 +575,13 @@ static int osc_fsync_ost(const struct lu_env *env, struct osc_object *obj,
 static int osc_io_fsync_start(const struct lu_env *env,
 			      const struct cl_io_slice *slice)
 {
-	struct cl_io       *io  = slice->cis_io;
+	struct cl_io *io = slice->cis_io;
 	struct cl_fsync_io *fio = &io->u.ci_fsync;
-	struct cl_object   *obj = slice->cis_obj;
-	struct osc_object  *osc = cl2osc(obj);
-	pgoff_t start  = cl_index(obj, fio->fi_start);
-	pgoff_t end    = cl_index(obj, fio->fi_end);
-	int     result = 0;
+	struct cl_object *obj = slice->cis_obj;
+	struct osc_object *osc = cl2osc(obj);
+	pgoff_t start = cl_index(obj, fio->fi_start);
+	pgoff_t end = cl_index(obj, fio->fi_end);
+	int result = 0;
 
 	if (fio->fi_end == OBD_OBJECT_EOF)
 		end = CL_PAGE_EOF;
@@ -615,15 +615,15 @@ static void osc_io_fsync_end(const struct lu_env *env,
 			     const struct cl_io_slice *slice)
 {
 	struct cl_fsync_io *fio = &slice->cis_io->u.ci_fsync;
-	struct cl_object   *obj = slice->cis_obj;
+	struct cl_object *obj = slice->cis_obj;
 	pgoff_t start = cl_index(obj, fio->fi_start);
-	pgoff_t end   = cl_index(obj, fio->fi_end);
+	pgoff_t end = cl_index(obj, fio->fi_end);
 	int result = 0;
 
 	if (fio->fi_mode == CL_FSYNC_LOCAL) {
 		result = osc_cache_wait_range(env, cl2osc(obj), start, end);
 	} else if (fio->fi_mode == CL_FSYNC_ALL) {
-		struct osc_io	   *oio    = cl2osc_io(env, slice);
+		struct osc_io *oio = cl2osc_io(env, slice);
 		struct osc_async_cbargs *cbargs = &oio->oi_cbarg;
 
 		wait_for_completion(&cbargs->opc_sync);
@@ -717,17 +717,17 @@ static void osc_req_attr_set(const struct lu_env *env,
 			     struct cl_req_attr *attr, u64 flags)
 {
 	struct lov_oinfo *oinfo;
-	struct cl_req    *clerq;
-	struct cl_page   *apage; /* _some_ page in @clerq */
-	struct cl_lock   *lock;  /* _some_ lock protecting @apage */
-	struct osc_lock  *olck;
-	struct osc_page  *opg;
-	struct obdo      *oa;
-	struct ost_lvb   *lvb;
-
-	oinfo	= cl2osc(obj)->oo_oinfo;
-	lvb	= &oinfo->loi_lvb;
-	oa	= attr->cra_oa;
+	struct cl_req *clerq;
+	struct cl_page *apage; /* _some_ page in @clerq */
+	struct cl_lock *lock;  /* _some_ lock protecting @apage */
+	struct osc_lock *olck;
+	struct osc_page *opg;
+	struct obdo *oa;
+	struct ost_lvb *lvb;
+
+	oinfo = cl2osc(obj)->oo_oinfo;
+	lvb = &oinfo->loi_lvb;
+	oa = attr->cra_oa;
 
 	if ((flags & OBD_MD_FLMTIME) != 0) {
 		oa->o_mtime = lvb->lvb_mtime;
@@ -759,7 +759,7 @@ static void osc_req_attr_set(const struct lu_env *env,
 		lock = cl_lock_at_page(env, apage->cp_obj, apage, NULL, 1, 1);
 		if (lock == NULL) {
 			struct cl_object_header *head;
-			struct cl_lock	  *scan;
+			struct cl_lock *scan;
 
 			head = cl_object_header(apage->cp_obj);
 			list_for_each_entry(scan, &head->coh_locks,
diff --git a/drivers/staging/lustre/lustre/osc/osc_lock.c b/drivers/staging/lustre/lustre/osc/osc_lock.c
index 350ad49550ab..70b1b43f692b 100644
--- a/drivers/staging/lustre/lustre/osc/osc_lock.c
+++ b/drivers/staging/lustre/lustre/osc/osc_lock.c
@@ -89,9 +89,9 @@ static struct ldlm_lock *osc_handle_ptr(struct lustre_handle *handle)
  */
 static int osc_lock_invariant(struct osc_lock *ols)
 {
-	struct ldlm_lock *lock	      = osc_handle_ptr(&ols->ols_handle);
-	struct ldlm_lock *olock	      = ols->ols_lock;
-	int		  handle_used = lustre_handle_is_used(&ols->ols_handle);
+	struct ldlm_lock *lock = osc_handle_ptr(&ols->ols_handle);
+	struct ldlm_lock *olock = ols->ols_lock;
+	int handle_used = lustre_handle_is_used(&ols->ols_handle);
 
 	if (ergo(osc_lock_is_lockless(ols),
 		 ols->ols_locklessable && ols->ols_lock == NULL))
@@ -164,7 +164,7 @@ static void osc_lock_detach(const struct lu_env *env, struct osc_lock *olck)
 	lock_res_and_lock(dlmlock);
 	if (dlmlock->l_granted_mode == dlmlock->l_req_mode) {
 		struct cl_object *obj = olck->ols_cl.cls_obj;
-		struct cl_attr *attr  = &osc_env_info(env)->oti_attr;
+		struct cl_attr *attr = &osc_env_info(env)->oti_attr;
 		__u64 old_kms;
 
 		cl_object_attr_lock(obj);
@@ -237,7 +237,7 @@ static int osc_lock_unuse(const struct lu_env *env,
 static void osc_lock_fini(const struct lu_env *env,
 			  struct cl_lock_slice *slice)
 {
-	struct osc_lock  *ols = cl2osc_lock(slice);
+	struct osc_lock *ols = cl2osc_lock(slice);
 
 	LINVRNT(osc_lock_invariant(ols));
 	/*
@@ -337,25 +337,25 @@ static void osc_ast_data_put(const struct lu_env *env, struct osc_lock *olck)
 static void osc_lock_lvb_update(const struct lu_env *env, struct osc_lock *olck,
 				int rc)
 {
-	struct ost_lvb    *lvb;
-	struct cl_object  *obj;
-	struct lov_oinfo  *oinfo;
-	struct cl_attr    *attr;
-	unsigned	   valid;
+	struct ost_lvb *lvb;
+	struct cl_object *obj;
+	struct lov_oinfo *oinfo;
+	struct cl_attr *attr;
+	unsigned valid;
 
 	if (!(olck->ols_flags & LDLM_FL_LVB_READY))
 		return;
 
-	lvb   = &olck->ols_lvb;
-	obj   = olck->ols_cl.cls_obj;
+	lvb = &olck->ols_lvb;
+	obj = olck->ols_cl.cls_obj;
 	oinfo = cl2osc(obj)->oo_oinfo;
-	attr  = &osc_env_info(env)->oti_attr;
+	attr = &osc_env_info(env)->oti_attr;
 	valid = CAT_BLOCKS | CAT_ATIME | CAT_CTIME | CAT_MTIME | CAT_SIZE;
 	cl_lvb2attr(attr, lvb);
 
 	cl_object_attr_lock(obj);
 	if (rc == 0) {
-		struct ldlm_lock  *dlmlock;
+		struct ldlm_lock *dlmlock;
 		__u64 size;
 
 		dlmlock = olck->ols_lock;
@@ -401,23 +401,23 @@ static void osc_lock_lvb_update(const struct lu_env *env, struct osc_lock *olck,
 static void osc_lock_granted(const struct lu_env *env, struct osc_lock *olck,
 			     struct ldlm_lock *dlmlock, int rc)
 {
-	struct ldlm_extent   *ext;
-	struct cl_lock       *lock;
+	struct ldlm_extent *ext;
+	struct cl_lock *lock;
 	struct cl_lock_descr *descr;
 
 	LASSERT(dlmlock->l_granted_mode == dlmlock->l_req_mode);
 
 	if (olck->ols_state < OLS_GRANTED) {
-		lock  = olck->ols_cl.cls_lock;
-		ext   = &dlmlock->l_policy_data.l_extent;
+		lock = olck->ols_cl.cls_lock;
+		ext = &dlmlock->l_policy_data.l_extent;
 		descr = &osc_env_info(env)->oti_descr;
 		descr->cld_obj = lock->cll_descr.cld_obj;
 
 		/* XXX check that ->l_granted_mode is valid. */
-		descr->cld_mode  = osc_ldlm2cl_lock(dlmlock->l_granted_mode);
+		descr->cld_mode = osc_ldlm2cl_lock(dlmlock->l_granted_mode);
 		descr->cld_start = cl_index(descr->cld_obj, ext->start);
-		descr->cld_end   = cl_index(descr->cld_obj, ext->end);
-		descr->cld_gid   = ext->gid;
+		descr->cld_end = cl_index(descr->cld_obj, ext->end);
+		descr->cld_gid = ext->gid;
 		/*
 		 * tell upper layers the extent of the lock that was actually
 		 * granted
@@ -482,11 +482,11 @@ static void osc_lock_upcall0(const struct lu_env *env, struct osc_lock *olck)
  */
 static int osc_lock_upcall(void *cookie, int errcode)
 {
-	struct osc_lock	 *olck  = cookie;
-	struct cl_lock_slice    *slice = &olck->ols_cl;
-	struct cl_lock	  *lock  = slice->cls_lock;
-	struct lu_env	   *env;
-	struct cl_env_nest       nest;
+	struct osc_lock	*olck = cookie;
+	struct cl_lock_slice *slice = &olck->ols_cl;
+	struct cl_lock *lock = slice->cls_lock;
+	struct lu_env *env;
+	struct cl_env_nest nest;
 
 	env = cl_env_nested_get(&nest);
 	if (!IS_ERR(env)) {
@@ -626,7 +626,7 @@ static int osc_dlm_blocking_ast0(const struct lu_env *env,
 				 void *data, int flag)
 {
 	struct osc_lock *olck;
-	struct cl_lock  *lock;
+	struct cl_lock *lock;
 	int result;
 	int cancel;
 
@@ -733,9 +733,9 @@ static int osc_ldlm_blocking_ast(struct ldlm_lock *dlmlock,
 				 struct ldlm_lock_desc *new, void *data,
 				 int flag)
 {
-	struct lu_env     *env;
+	struct lu_env *env;
 	struct cl_env_nest nest;
-	int		result;
+	int result;
 
 	/*
 	 * This can be called in the context of outer IO, e.g.,
@@ -774,9 +774,9 @@ static int osc_ldlm_completion_ast(struct ldlm_lock *dlmlock,
 				   __u64 flags, void *data)
 {
 	struct cl_env_nest nest;
-	struct lu_env     *env;
-	struct osc_lock   *olck;
-	struct cl_lock    *lock;
+	struct lu_env *env;
+	struct osc_lock *olck;
+	struct cl_lock *lock;
 	int result;
 	int dlmrc;
 
@@ -830,15 +830,15 @@ static int osc_ldlm_completion_ast(struct ldlm_lock *dlmlock,
 
 static int osc_ldlm_glimpse_ast(struct ldlm_lock *dlmlock, void *data)
 {
-	struct ptlrpc_request  *req  = data;
+	struct ptlrpc_request *req = data;
 	struct osc_lock	*olck;
-	struct cl_lock	 *lock;
-	struct cl_object       *obj;
-	struct cl_env_nest      nest;
-	struct lu_env	  *env;
-	struct ost_lvb	 *lvb;
-	struct req_capsule     *cap;
-	int		     result;
+	struct cl_lock *lock;
+	struct cl_object *obj;
+	struct cl_env_nest nest;
+	struct lu_env *env;
+	struct ost_lvb *lvb;
+	struct req_capsule *cap;
+	int result;
 
 	LASSERT(lustre_msg_get_opc(req->rq_reqmsg) == LDLM_GL_CALLBACK);
 
@@ -916,11 +916,11 @@ static void osc_lock_build_einfo(const struct lu_env *env,
 		 */
 		mode = CLM_READ;
 
-	einfo->ei_type   = LDLM_EXTENT;
-	einfo->ei_mode   = osc_cl_lock2ldlm(mode);
-	einfo->ei_cb_bl  = osc_ldlm_blocking_ast;
-	einfo->ei_cb_cp  = osc_ldlm_completion_ast;
-	einfo->ei_cb_gl  = osc_ldlm_glimpse_ast;
+	einfo->ei_type = LDLM_EXTENT;
+	einfo->ei_mode = osc_cl_lock2ldlm(mode);
+	einfo->ei_cb_bl = osc_ldlm_blocking_ast;
+	einfo->ei_cb_cp = osc_ldlm_completion_ast;
+	einfo->ei_cb_gl = osc_ldlm_glimpse_ast;
 	einfo->ei_cbdata = lock; /* value to be put into ->l_ast_data */
 }
 
@@ -948,9 +948,9 @@ static void osc_lock_to_lockless(const struct lu_env *env,
 		ols->ols_locklessable = 1;
 		slice->cls_ops = &osc_lock_lockless_ops;
 	} else {
-		struct osc_io *oio     = osc_env_io(env);
-		struct cl_io  *io      = oio->oi_cl.cis_io;
-		struct cl_object *obj  = slice->cls_obj;
+		struct osc_io *oio = osc_env_io(env);
+		struct cl_io *io = oio->oi_cl.cis_io;
+		struct cl_object *obj = slice->cls_obj;
 		struct osc_object *oob = cl2osc(obj);
 		const struct osc_device *osd = lu2osc_dev(obj->co_lu.lo_dev);
 		struct obd_connect_data *ocd;
@@ -1006,13 +1006,13 @@ static int osc_lock_compatible(const struct osc_lock *qing,
 static int osc_lock_enqueue_wait(const struct lu_env *env,
 				 const struct osc_lock *olck)
 {
-	struct cl_lock	  *lock    = olck->ols_cl.cls_lock;
-	struct cl_lock_descr    *descr   = &lock->cll_descr;
-	struct cl_object_header *hdr     = cl_object_header(descr->cld_obj);
-	struct cl_lock	  *scan;
-	struct cl_lock	  *conflict = NULL;
-	int lockless		     = osc_lock_is_lockless(olck);
-	int rc			   = 0;
+	struct cl_lock *lock = olck->ols_cl.cls_lock;
+	struct cl_lock_descr *descr = &lock->cll_descr;
+	struct cl_object_header *hdr = cl_object_header(descr->cld_obj);
+	struct cl_lock *scan;
+	struct cl_lock *conflict = NULL;
+	int lockless = osc_lock_is_lockless(olck);
+	int rc = 0;
 
 	LASSERT(cl_lock_is_mutexed(lock));
 
@@ -1102,8 +1102,8 @@ static int osc_lock_enqueue(const struct lu_env *env,
 			    const struct cl_lock_slice *slice,
 			    struct cl_io *unused, __u32 enqflags)
 {
-	struct osc_lock	  *ols     = cl2osc_lock(slice);
-	struct cl_lock	   *lock    = ols->ols_cl.cls_lock;
+	struct osc_lock *ols = cl2osc_lock(slice);
+	struct cl_lock *lock = ols->ols_cl.cls_lock;
 	int result;
 
 	LASSERT(cl_lock_is_mutexed(lock));
@@ -1116,10 +1116,10 @@ static int osc_lock_enqueue(const struct lu_env *env,
 	result = osc_lock_enqueue_wait(env, ols);
 	if (result == 0) {
 		if (!osc_lock_is_lockless(ols)) {
-			struct osc_object	*obj = cl2osc(slice->cls_obj);
-			struct osc_thread_info   *info = osc_env_info(env);
-			struct ldlm_res_id       *resname = &info->oti_resname;
-			ldlm_policy_data_t       *policy = &info->oti_policy;
+			struct osc_object *obj = cl2osc(slice->cls_obj);
+			struct osc_thread_info *info = osc_env_info(env);
+			struct ldlm_res_id *resname = &info->oti_resname;
+			ldlm_policy_data_t *policy = &info->oti_policy;
 			struct ldlm_enqueue_info *einfo = &ols->ols_einfo;
 
 			/* lock will be passed as upcall cookie,
@@ -1164,7 +1164,7 @@ static int osc_lock_wait(const struct lu_env *env,
 			 const struct cl_lock_slice *slice)
 {
 	struct osc_lock *olck = cl2osc_lock(slice);
-	struct cl_lock  *lock = olck->ols_cl.cls_lock;
+	struct cl_lock *lock = olck->ols_cl.cls_lock;
 
 	LINVRNT(osc_lock_invariant(olck));
 
@@ -1176,8 +1176,7 @@ static int osc_lock_wait(const struct lu_env *env,
 				/* It is from enqueue RPC reply upcall for
 				 * updating state. Do not re-enqueue. */
 				return -ENAVAIL;
-			else
-				olck->ols_state = OLS_NEW;
+			olck->ols_state = OLS_NEW;
 		} else {
 			LASSERT(lock->cll_error);
 			return lock->cll_error;
@@ -1245,14 +1244,14 @@ static int osc_lock_use(const struct lu_env *env,
 
 static int osc_lock_flush(struct osc_lock *ols, int discard)
 {
-	struct cl_lock       *lock  = ols->ols_cl.cls_lock;
-	struct cl_env_nest    nest;
-	struct lu_env	*env;
+	struct cl_lock *lock = ols->ols_cl.cls_lock;
+	struct cl_env_nest nest;
+	struct lu_env *env;
 	int result = 0;
 
 	env = cl_env_nested_get(&nest);
 	if (!IS_ERR(env)) {
-		struct osc_object    *obj   = cl2osc(ols->ols_cl.cls_obj);
+		struct osc_object *obj = cl2osc(ols->ols_cl.cls_obj);
 		struct cl_lock_descr *descr = &lock->cll_descr;
 		int rc = 0;
 
@@ -1298,11 +1297,11 @@ static int osc_lock_flush(struct osc_lock *ols, int discard)
 static void osc_lock_cancel(const struct lu_env *env,
 			    const struct cl_lock_slice *slice)
 {
-	struct cl_lock   *lock    = slice->cls_lock;
-	struct osc_lock  *olck    = cl2osc_lock(slice);
+	struct cl_lock *lock = slice->cls_lock;
+	struct osc_lock *olck = cl2osc_lock(slice);
 	struct ldlm_lock *dlmlock = olck->ols_lock;
-	int	       result  = 0;
-	int	       discard;
+	int result = 0;
+	int discard;
 
 	LASSERT(cl_lock_is_mutexed(lock));
 	LINVRNT(osc_lock_invariant(olck));
@@ -1482,7 +1481,7 @@ static int osc_lock_lockless_unuse(const struct lu_env *env,
 static void osc_lock_lockless_cancel(const struct lu_env *env,
 				     const struct cl_lock_slice *slice)
 {
-	struct osc_lock   *ols  = cl2osc_lock(slice);
+	struct osc_lock *ols = cl2osc_lock(slice);
 	int result;
 
 	result = osc_lock_flush(ols, 0);
@@ -1496,7 +1495,7 @@ static int osc_lock_lockless_wait(const struct lu_env *env,
 				  const struct cl_lock_slice *slice)
 {
 	struct osc_lock *olck = cl2osc_lock(slice);
-	struct cl_lock  *lock = olck->ols_cl.cls_lock;
+	struct cl_lock *lock = olck->ols_cl.cls_lock;
 
 	LINVRNT(osc_lock_invariant(olck));
 	LASSERT(olck->ols_state >= OLS_UPCALL_RECEIVED);
@@ -1512,7 +1511,7 @@ static void osc_lock_lockless_state(const struct lu_env *env,
 
 	LINVRNT(osc_lock_invariant(lock));
 	if (state == CLS_HELD) {
-		struct osc_io *oio  = osc_env_io(env);
+		struct osc_io *oio = osc_env_io(env);
 
 		LASSERT(ergo(lock->ols_owner, lock->ols_owner == oio));
 		lock->ols_owner = oio;
@@ -1591,7 +1590,7 @@ int osc_lock_init(const struct lu_env *env,
 int osc_dlm_lock_pageref(struct ldlm_lock *dlm)
 {
 	struct osc_lock *olock;
-	int	      rc = 0;
+	int rc = 0;
 
 	spin_lock(&osc_ast_guard);
 	olock = dlm->l_ast_data;
diff --git a/drivers/staging/lustre/lustre/osc/osc_object.c b/drivers/staging/lustre/lustre/osc/osc_object.c
index 92c202f70395..c628a250ebd6 100644
--- a/drivers/staging/lustre/lustre/osc/osc_object.c
+++ b/drivers/staging/lustre/lustre/osc/osc_object.c
@@ -72,7 +72,7 @@ static struct osc_object *lu2osc(const struct lu_object *obj)
 static int osc_object_init(const struct lu_env *env, struct lu_object *obj,
 			   const struct lu_object_conf *conf)
 {
-	struct osc_object	   *osc   = lu2osc(obj);
+	struct osc_object *osc = lu2osc(obj);
 	const struct cl_object_conf *cconf = lu2cl_conf(conf);
 	int i;
 
@@ -136,9 +136,9 @@ int osc_lvb_print(const struct lu_env *env, void *cookie,
 static int osc_object_print(const struct lu_env *env, void *cookie,
 			    lu_printer_t p, const struct lu_object *obj)
 {
-	struct osc_object   *osc   = lu2osc(obj);
-	struct lov_oinfo    *oinfo = osc->oo_oinfo;
-	struct osc_async_rc *ar    = &oinfo->loi_ar;
+	struct osc_object *osc = lu2osc(obj);
+	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 ",
 	     POSTID(&oinfo->loi_oi), oinfo->loi_ost_idx,
@@ -163,7 +163,7 @@ int osc_attr_set(const struct lu_env *env, struct cl_object *obj,
 		 const struct cl_attr *attr, unsigned valid)
 {
 	struct lov_oinfo *oinfo = cl2osc(obj)->oo_oinfo;
-	struct ost_lvb   *lvb   = &oinfo->loi_lvb;
+	struct ost_lvb *lvb = &oinfo->loi_lvb;
 
 	if (valid & CAT_SIZE)
 		lvb->lvb_size = attr->cat_size;
@@ -188,7 +188,7 @@ static int osc_object_glimpse(const struct lu_env *env,
 {
 	struct lov_oinfo *oinfo = cl2osc(obj)->oo_oinfo;
 
-	lvb->lvb_size   = oinfo->loi_kms;
+	lvb->lvb_size = oinfo->loi_kms;
 	lvb->lvb_blocks = oinfo->loi_lvb.lvb_blocks;
 	return 0;
 }
@@ -208,9 +208,9 @@ void osc_object_clear_contended(struct osc_object *obj)
 
 int osc_object_is_contended(struct osc_object *obj)
 {
-	struct osc_device *dev  = lu2osc_dev(obj->oo_cl.co_lu.lo_dev);
+	struct osc_device *dev = lu2osc_dev(obj->oo_cl.co_lu.lo_dev);
 	int osc_contention_time = dev->od_contention_time;
-	unsigned long cur_time     = cfs_time_current();
+	unsigned long cur_time = cfs_time_current();
 	unsigned long retry_time;
 
 	if (OBD_FAIL_CHECK(OBD_FAIL_OSC_OBJECT_CONTENTION))
@@ -255,7 +255,7 @@ struct lu_object *osc_object_alloc(const struct lu_env *env,
 				   struct lu_device *dev)
 {
 	struct osc_object *osc;
-	struct lu_object  *obj;
+	struct lu_object *obj;
 
 	OBD_SLAB_ALLOC_PTR_GFP(osc, osc_object_kmem, GFP_NOFS);
 	if (osc != NULL) {
diff --git a/drivers/staging/lustre/lustre/osc/osc_page.c b/drivers/staging/lustre/lustre/osc/osc_page.c
index 76ba58b09c5d..43dfa73dd3a6 100644
--- a/drivers/staging/lustre/lustre/osc/osc_page.c
+++ b/drivers/staging/lustre/lustre/osc/osc_page.c
@@ -216,7 +216,7 @@ static int osc_page_cache_add(const struct lu_env *env,
 			      const struct cl_page_slice *slice,
 			      struct cl_io *io)
 {
-	struct osc_io   *oio = osc_env_io(env);
+	struct osc_io *oio = osc_env_io(env);
 	struct osc_page *opg = cl2osc_page(slice);
 	int result;
 
@@ -247,7 +247,7 @@ void osc_index2policy(ldlm_policy_data_t *policy, const struct cl_object *obj,
 {
 	memset(policy, 0, sizeof(*policy));
 	policy->l_extent.start = cl_offset(obj, start);
-	policy->l_extent.end   = cl_offset(obj, end + 1) - 1;
+	policy->l_extent.end = cl_offset(obj, end + 1) - 1;
 }
 
 static int osc_page_addref_lock(const struct lu_env *env,
@@ -255,7 +255,7 @@ static int osc_page_addref_lock(const struct lu_env *env,
 				struct cl_lock *lock)
 {
 	struct osc_lock *olock;
-	int	      rc;
+	int rc;
 
 	LASSERT(opg->ops_lock == NULL);
 
@@ -274,7 +274,7 @@ static int osc_page_addref_lock(const struct lu_env *env,
 static void osc_page_putref_lock(const struct lu_env *env,
 				 struct osc_page *opg)
 {
-	struct cl_lock  *lock = opg->ops_lock;
+	struct cl_lock *lock = opg->ops_lock;
 	struct osc_lock *olock;
 
 	LASSERT(lock != NULL);
@@ -291,7 +291,7 @@ static int osc_page_is_under_lock(const struct lu_env *env,
 				  struct cl_io *unused)
 {
 	struct cl_lock *lock;
-	int	     result = -ENODATA;
+	int result = -ENODATA;
 
 	lock = cl_lock_at_page(env, slice->cpl_obj, slice->cpl_page,
 			       NULL, 1, 0);
@@ -317,7 +317,7 @@ static void osc_page_completion_read(const struct lu_env *env,
 				     const struct cl_page_slice *slice,
 				     int ioret)
 {
-	struct osc_page   *opg = cl2osc_page(slice);
+	struct osc_page *opg = cl2osc_page(slice);
 	struct osc_object *obj = cl2osc(opg->ops_cl.cpl_obj);
 
 	if (likely(opg->ops_lock))
@@ -329,7 +329,7 @@ static void osc_page_completion_write(const struct lu_env *env,
 				      const struct cl_page_slice *slice,
 				      int ioret)
 {
-	struct osc_page   *opg = cl2osc_page(slice);
+	struct osc_page *opg = cl2osc_page(slice);
 	struct osc_object *obj = cl2osc(slice->cpl_obj);
 
 	osc_lru_add(osc_cli(obj), opg);
@@ -364,10 +364,10 @@ static int osc_page_print(const struct lu_env *env,
 			  const struct cl_page_slice *slice,
 			  void *cookie, lu_printer_t printer)
 {
-	struct osc_page       *opg = cl2osc_page(slice);
+	struct osc_page *opg = cl2osc_page(slice);
 	struct osc_async_page *oap = &opg->ops_oap;
-	struct osc_object     *obj = cl2osc(slice->cpl_obj);
-	struct client_obd     *cli = &osc_export(obj)->exp_obd->u.cli;
+	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",
 			  opg,
@@ -408,7 +408,7 @@ static int osc_page_print(const struct lu_env *env,
 static void osc_page_delete(const struct lu_env *env,
 			    const struct cl_page_slice *slice)
 {
-	struct osc_page   *opg = cl2osc_page(slice);
+	struct osc_page *opg = cl2osc_page(slice);
 	struct osc_object *obj = cl2osc(opg->ops_cl.cpl_obj);
 	int rc;
 
@@ -437,13 +437,13 @@ static void osc_page_delete(const struct lu_env *env,
 void osc_page_clip(const struct lu_env *env, const struct cl_page_slice *slice,
 		   int from, int to)
 {
-	struct osc_page       *opg = cl2osc_page(slice);
+	struct osc_page *opg = cl2osc_page(slice);
 	struct osc_async_page *oap = &opg->ops_oap;
 
 	LINVRNT(osc_page_protected(env, opg, CLM_READ, 0));
 
 	opg->ops_from = from;
-	opg->ops_to   = to;
+	opg->ops_to = to;
 	spin_lock(&oap->oap_lock);
 	oap->oap_async_flags |= ASYNC_COUNT_STABLE;
 	spin_unlock(&oap->oap_lock);
@@ -502,11 +502,11 @@ int osc_page_init(const struct lu_env *env, struct cl_object *obj,
 		struct cl_page *page, struct page *vmpage)
 {
 	struct osc_object *osc = cl2osc(obj);
-	struct osc_page   *opg = cl_object_page_slice(obj, page);
+	struct osc_page *opg = cl_object_page_slice(obj, page);
 	int result;
 
 	opg->ops_from = 0;
-	opg->ops_to   = PAGE_CACHE_SIZE;
+	opg->ops_to = PAGE_CACHE_SIZE;
 
 	result = osc_prep_async_page(osc, opg, vmpage,
 					cl_offset(obj, page->cp_index));
@@ -540,7 +540,7 @@ void osc_page_submit(const struct lu_env *env, struct osc_page *opg,
 		     enum cl_req_type crt, int brw_flags)
 {
 	struct osc_async_page *oap = &opg->ops_oap;
-	struct osc_object     *obj = oap->oap_obj;
+	struct osc_object *obj = oap->oap_obj;
 
 	LINVRNT(osc_page_protected(env, opg,
 				   crt == CRT_WRITE ? CLM_WRITE : CLM_READ, 1));
@@ -550,9 +550,9 @@ void osc_page_submit(const struct lu_env *env, struct osc_page *opg,
 	LASSERT(oap->oap_async_flags & ASYNC_READY);
 	LASSERT(oap->oap_async_flags & ASYNC_COUNT_STABLE);
 
-	oap->oap_cmd       = crt == CRT_WRITE ? OBD_BRW_WRITE : OBD_BRW_READ;
-	oap->oap_page_off  = opg->ops_from;
-	oap->oap_count     = opg->ops_to - opg->ops_from;
+	oap->oap_cmd = crt == CRT_WRITE ? OBD_BRW_WRITE : OBD_BRW_READ;
+	oap->oap_page_off = opg->ops_from;
+	oap->oap_count = opg->ops_to - opg->ops_from;
 	oap->oap_brw_flags = OBD_BRW_SYNC | brw_flags;
 
 	if (!client_is_remote(osc_export(obj)) &&
diff --git a/drivers/staging/lustre/lustre/osc/osc_quota.c b/drivers/staging/lustre/lustre/osc/osc_quota.c
index 6690f149a04c..2ff253f458f8 100644
--- a/drivers/staging/lustre/lustre/osc/osc_quota.c
+++ b/drivers/staging/lustre/lustre/osc/osc_quota.c
@@ -232,7 +232,7 @@ int osc_quota_setup(struct obd_device *obd)
 
 int osc_quota_cleanup(struct obd_device *obd)
 {
-	struct client_obd     *cli = &obd->u.cli;
+	struct client_obd *cli = &obd->u.cli;
 	int type;
 
 	for (type = 0; type < MAXQUOTAS; type++)
@@ -245,8 +245,8 @@ int osc_quotactl(struct obd_device *unused, struct obd_export *exp,
 		 struct obd_quotactl *oqctl)
 {
 	struct ptlrpc_request *req;
-	struct obd_quotactl   *oqc;
-	int		    rc;
+	struct obd_quotactl *oqc;
+	int rc;
 
 	req = ptlrpc_request_alloc_pack(class_exp2cliimp(exp),
 					&RQF_OST_QUOTACTL, LUSTRE_OST_VERSION,
@@ -285,10 +285,10 @@ int osc_quotactl(struct obd_device *unused, struct obd_export *exp,
 int osc_quotacheck(struct obd_device *unused, struct obd_export *exp,
 		   struct obd_quotactl *oqctl)
 {
-	struct client_obd       *cli = &exp->exp_obd->u.cli;
-	struct ptlrpc_request   *req;
-	struct obd_quotactl     *body;
-	int		      rc;
+	struct client_obd *cli = &exp->exp_obd->u.cli;
+	struct ptlrpc_request *req;
+	struct obd_quotactl *body;
+	int rc;
 
 	req = ptlrpc_request_alloc_pack(class_exp2cliimp(exp),
 					&RQF_OST_QUOTACHECK, LUSTRE_OST_VERSION,
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index d7a9b650df09..f84b4c78a8a0 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -110,7 +110,7 @@ static int osc_packmd(struct obd_export *exp, struct lov_mds_md **lmmp,
 		return lmm_size;
 
 	if (*lmmp != NULL && lsm == NULL) {
-		OBD_FREE(*lmmp, lmm_size);
+		kfree(*lmmp);
 		*lmmp = NULL;
 		return 0;
 	} else if (unlikely(lsm != NULL && ostid_id(&lsm->lsm_oi) == 0)) {
@@ -118,7 +118,7 @@ static int osc_packmd(struct obd_export *exp, struct lov_mds_md **lmmp,
 	}
 
 	if (*lmmp == NULL) {
-		OBD_ALLOC(*lmmp, lmm_size);
+		*lmmp = kzalloc(lmm_size, GFP_NOFS);
 		if (*lmmp == NULL)
 			return -ENOMEM;
 	}
@@ -157,19 +157,20 @@ static int osc_unpackmd(struct obd_export *exp, struct lov_stripe_md **lsmp,
 		return lsm_size;
 
 	if (*lsmp != NULL && lmm == NULL) {
-		OBD_FREE((*lsmp)->lsm_oinfo[0], sizeof(struct lov_oinfo));
-		OBD_FREE(*lsmp, lsm_size);
+		kfree((*lsmp)->lsm_oinfo[0]);
+		kfree(*lsmp);
 		*lsmp = NULL;
 		return 0;
 	}
 
 	if (*lsmp == NULL) {
-		OBD_ALLOC(*lsmp, lsm_size);
+		*lsmp = kzalloc(lsm_size, GFP_NOFS);
 		if (unlikely(*lsmp == NULL))
 			return -ENOMEM;
-		OBD_ALLOC((*lsmp)->lsm_oinfo[0], sizeof(struct lov_oinfo));
+		(*lsmp)->lsm_oinfo[0] = kzalloc(sizeof(struct lov_oinfo),
+						GFP_NOFS);
 		if (unlikely((*lsmp)->lsm_oinfo[0] == NULL)) {
-			OBD_FREE(*lsmp, lsm_size);
+			kfree(*lsmp);
 			return -ENOMEM;
 		}
 		loi_init((*lsmp)->lsm_oinfo[0]);
@@ -263,7 +264,7 @@ static int osc_getattr_async(struct obd_export *exp, struct obd_info *oinfo,
 {
 	struct ptlrpc_request *req;
 	struct osc_async_args *aa;
-	int		    rc;
+	int rc;
 
 	req = ptlrpc_request_alloc(class_exp2cliimp(exp), &RQF_OST_GETATTR);
 	if (req == NULL)
@@ -293,8 +294,8 @@ static int osc_getattr(const struct lu_env *env, struct obd_export *exp,
 		       struct obd_info *oinfo)
 {
 	struct ptlrpc_request *req;
-	struct ost_body       *body;
-	int		    rc;
+	struct ost_body *body;
+	int rc;
 
 	req = ptlrpc_request_alloc(class_exp2cliimp(exp), &RQF_OST_GETATTR);
 	if (req == NULL)
@@ -337,8 +338,8 @@ static int osc_setattr(const struct lu_env *env, struct obd_export *exp,
 		       struct obd_info *oinfo, struct obd_trans_info *oti)
 {
 	struct ptlrpc_request *req;
-	struct ost_body       *body;
-	int		    rc;
+	struct ost_body *body;
+	int rc;
 
 	LASSERT(oinfo->oi_oa->o_valid & OBD_MD_FLGROUP);
 
@@ -402,9 +403,9 @@ int osc_setattr_async_base(struct obd_export *exp, struct obd_info *oinfo,
 			   obd_enqueue_update_f upcall, void *cookie,
 			   struct ptlrpc_request_set *rqset)
 {
-	struct ptlrpc_request   *req;
+	struct ptlrpc_request *req;
 	struct osc_setattr_args *sa;
-	int		      rc;
+	int rc;
 
 	req = ptlrpc_request_alloc(class_exp2cliimp(exp), &RQF_OST_SETATTR);
 	if (req == NULL)
@@ -459,9 +460,9 @@ int osc_real_create(struct obd_export *exp, struct obdo *oa,
 		    struct lov_stripe_md **ea, struct obd_trans_info *oti)
 {
 	struct ptlrpc_request *req;
-	struct ost_body       *body;
-	struct lov_stripe_md  *lsm;
-	int		    rc;
+	struct ost_body *body;
+	struct lov_stripe_md *lsm;
+	int rc;
 
 	LASSERT(oa);
 	LASSERT(ea);
@@ -547,10 +548,10 @@ int osc_punch_base(struct obd_export *exp, struct obd_info *oinfo,
 		   obd_enqueue_update_f upcall, void *cookie,
 		   struct ptlrpc_request_set *rqset)
 {
-	struct ptlrpc_request   *req;
+	struct ptlrpc_request *req;
 	struct osc_setattr_args *sa;
-	struct ost_body	 *body;
-	int		      rc;
+	struct ost_body *body;
+	int rc;
 
 	req = ptlrpc_request_alloc(class_exp2cliimp(exp), &RQF_OST_PUNCH);
 	if (req == NULL)
@@ -576,7 +577,7 @@ int osc_punch_base(struct obd_export *exp, struct obd_info *oinfo,
 	req->rq_interpret_reply = (ptlrpc_interpterer_t)osc_setattr_interpret;
 	CLASSERT (sizeof(*sa) <= sizeof(req->rq_async_args));
 	sa = ptlrpc_req_async_args(req);
-	sa->sa_oa     = oinfo->oi_oa;
+	sa->sa_oa = oinfo->oi_oa;
 	sa->sa_upcall = upcall;
 	sa->sa_cookie = cookie;
 	if (rqset == PTLRPCD_SET)
@@ -615,9 +616,9 @@ int osc_sync_base(struct obd_export *exp, struct obd_info *oinfo,
 		  struct ptlrpc_request_set *rqset)
 {
 	struct ptlrpc_request *req;
-	struct ost_body       *body;
+	struct ost_body *body;
 	struct osc_fsync_args *fa;
-	int		    rc;
+	int rc;
 
 	req = ptlrpc_request_alloc(class_exp2cliimp(exp), &RQF_OST_SYNC);
 	if (req == NULL)
@@ -756,9 +757,9 @@ static int osc_destroy(const struct lu_env *env, struct obd_export *exp,
 		       struct obd_trans_info *oti, struct obd_export *md_export,
 		       void *capa)
 {
-	struct client_obd     *cli = &exp->exp_obd->u.cli;
+	struct client_obd *cli = &exp->exp_obd->u.cli;
 	struct ptlrpc_request *req;
-	struct ost_body       *body;
+	struct ost_body *body;
 	LIST_HEAD(cancels);
 	int rc, count;
 
@@ -946,7 +947,7 @@ static int osc_shrink_grant(struct client_obd *cli)
 
 int osc_shrink_grant_to_target(struct client_obd *cli, __u64 target_bytes)
 {
-	int			rc = 0;
+	int rc = 0;
 	struct ost_body	*body;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
@@ -962,7 +963,7 @@ int osc_shrink_grant_to_target(struct client_obd *cli, __u64 target_bytes)
 	}
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
-	OBD_ALLOC_PTR(body);
+	body = kzalloc(sizeof(*body), GFP_NOFS);
 	if (!body)
 		return -ENOMEM;
 
@@ -984,7 +985,7 @@ int osc_shrink_grant_to_target(struct client_obd *cli, __u64 target_bytes)
 				sizeof(*body), body, NULL);
 	if (rc != 0)
 		__osc_update_grant(cli, body->oa.o_grant);
-	OBD_FREE_PTR(body);
+	kfree(body);
 	return rc;
 }
 
@@ -1006,8 +1007,8 @@ static int osc_should_shrink_grant(struct client_obd *client)
 		if (client->cl_import->imp_state == LUSTRE_IMP_FULL &&
 		    client->cl_avail_grant > brw_size)
 			return 1;
-		else
-			osc_update_next_shrink(client);
+
+		osc_update_next_shrink(client);
 	}
 	return 0;
 }
@@ -1130,8 +1131,8 @@ static int check_write_rcs(struct ptlrpc_request *req,
 			   int requested_nob, int niocount,
 			   u32 page_count, struct brw_page **pga)
 {
-	int     i;
-	__u32   *remote_rcs;
+	int i;
+	__u32 *remote_rcs;
 
 	remote_rcs = req_capsule_server_sized_get(&req->rq_pill, &RMF_RCS,
 						  sizeof(*remote_rcs) *
@@ -1181,15 +1182,15 @@ static inline int can_merge_pages(struct brw_page *p1, struct brw_page *p2)
 }
 
 static u32 osc_checksum_bulk(int nob, u32 pg_count,
-				   struct brw_page **pga, int opc,
-				   cksum_type_t cksum_type)
+			     struct brw_page **pga, int opc,
+			     cksum_type_t cksum_type)
 {
-	__u32				cksum;
-	int				i = 0;
-	struct cfs_crypto_hash_desc	*hdesc;
-	unsigned int			bufsize;
-	int				err;
-	unsigned char			cfs_alg = cksum_obd2cfs(cksum_type);
+	__u32 cksum;
+	int i = 0;
+	struct cfs_crypto_hash_desc *hdesc;
+	unsigned int bufsize;
+	int err;
+	unsigned char cfs_alg = cksum_obd2cfs(cksum_type);
 
 	LASSERT(pg_count > 0);
 
@@ -1249,14 +1250,14 @@ static int osc_brw_prep_request(int cmd, struct client_obd *cli,
 				struct obd_capa *ocapa, int reserve,
 				int resend)
 {
-	struct ptlrpc_request   *req;
+	struct ptlrpc_request *req;
 	struct ptlrpc_bulk_desc *desc;
-	struct ost_body	 *body;
-	struct obd_ioobj	*ioobj;
-	struct niobuf_remote    *niobuf;
+	struct ost_body	*body;
+	struct obd_ioobj *ioobj;
+	struct niobuf_remote *niobuf;
 	int niocount, i, requested_nob, opc, rc;
 	struct osc_brw_async_args *aa;
-	struct req_capsule      *pill;
+	struct req_capsule *pill;
 	struct brw_page *pg_prev;
 
 	if (OBD_FAIL_CHECK(OBD_FAIL_OSC_BRW_PREP_REQ))
@@ -1358,8 +1359,8 @@ static int osc_brw_prep_request(int cmd, struct client_obd *cli,
 			niobuf->len += pg->count;
 		} else {
 			niobuf->offset = pg->off;
-			niobuf->len    = pg->count;
-			niobuf->flags  = pg->flag;
+			niobuf->len = pg->count;
+			niobuf->flags = pg->flag;
 		}
 		pg_prev = pg;
 	}
@@ -1580,9 +1581,9 @@ static int osc_brw_fini_request(struct ptlrpc_request *req, int rc)
 
 	if (body->oa.o_valid & OBD_MD_FLCKSUM) {
 		static int cksum_counter;
-		__u32      server_cksum = body->oa.o_cksum;
-		char      *via;
-		char      *router;
+		__u32 server_cksum = body->oa.o_cksum;
+		char *via;
+		char *router;
 		cksum_type_t cksum_type;
 
 		cksum_type = cksum_type_unpack(body->oa.o_valid &OBD_MD_FLFLAGS?
@@ -1748,7 +1749,7 @@ static void sort_brw_pages(struct brw_page **array, int num)
 static void osc_release_ppga(struct brw_page **ppga, u32 count)
 {
 	LASSERT(ppga != NULL);
-	OBD_FREE(ppga, sizeof(*ppga) * count);
+	kfree(ppga);
 }
 
 static int brw_interpret(const struct lu_env *env,
@@ -1757,7 +1758,7 @@ static int brw_interpret(const struct lu_env *env,
 	struct osc_brw_async_args *aa = data;
 	struct osc_extent *ext;
 	struct osc_extent *tmp;
-	struct cl_object  *obj = NULL;
+	struct cl_object *obj = NULL;
 	struct client_obd *cli = aa->aa_cli;
 
 	rc = osc_brw_fini_request(req, rc);
@@ -1861,26 +1862,25 @@ static int brw_interpret(const struct lu_env *env,
 int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 		  struct list_head *ext_list, int cmd, pdl_policy_t pol)
 {
-	struct ptlrpc_request		*req = NULL;
-	struct osc_extent		*ext;
-	struct brw_page			**pga = NULL;
-	struct osc_brw_async_args	*aa = NULL;
-	struct obdo			*oa = NULL;
-	struct osc_async_page		*oap;
-	struct osc_async_page		*tmp;
-	struct cl_req			*clerq = NULL;
-	enum cl_req_type		crt = (cmd & OBD_BRW_WRITE) ? CRT_WRITE :
-								      CRT_READ;
-	struct ldlm_lock		*lock = NULL;
-	struct cl_req_attr		*crattr = NULL;
-	u64				starting_offset = OBD_OBJECT_EOF;
-	u64				ending_offset = 0;
-	int				mpflag = 0;
-	int				mem_tight = 0;
-	int				page_count = 0;
-	int				i;
-	int				rc;
-	struct ost_body			*body;
+	struct ptlrpc_request *req = NULL;
+	struct osc_extent *ext;
+	struct brw_page **pga = NULL;
+	struct osc_brw_async_args *aa = NULL;
+	struct obdo *oa = NULL;
+	struct osc_async_page *oap;
+	struct osc_async_page *tmp;
+	struct cl_req *clerq = NULL;
+	enum cl_req_type crt = (cmd & OBD_BRW_WRITE) ? CRT_WRITE : CRT_READ;
+	struct ldlm_lock *lock = NULL;
+	struct cl_req_attr *crattr = NULL;
+	u64 starting_offset = OBD_OBJECT_EOF;
+	u64 ending_offset = 0;
+	int mpflag = 0;
+	int mem_tight = 0;
+	int page_count = 0;
+	int i;
+	int rc;
+	struct ost_body *body;
 	LIST_HEAD(rpc_list);
 
 	LASSERT(!list_empty(ext_list));
@@ -1908,13 +1908,13 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 	if (mem_tight)
 		mpflag = cfs_memory_pressure_get_and_set();
 
-	OBD_ALLOC(crattr, sizeof(*crattr));
+	crattr = kzalloc(sizeof(*crattr), GFP_NOFS);
 	if (crattr == NULL) {
 		rc = -ENOMEM;
 		goto out;
 	}
 
-	OBD_ALLOC(pga, sizeof(*pga) * page_count);
+	pga = kcalloc(page_count, sizeof(*pga), GFP_NOFS);
 	if (pga == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -2055,7 +2055,7 @@ out:
 
 	if (crattr != NULL) {
 		capa_put(crattr->cra_capa);
-		OBD_FREE(crattr, sizeof(*crattr));
+		kfree(crattr);
 	}
 
 	if (rc != 0) {
@@ -2063,8 +2063,7 @@ out:
 
 		if (oa)
 			OBDO_FREE(oa);
-		if (pga)
-			OBD_FREE(pga, sizeof(*pga) * page_count);
+		kfree(pga);
 		/* this should happen rarely and is pretty bad, it makes the
 		 * pending list not follow the dirty order */
 		while (!list_empty(ext_list)) {
@@ -2300,7 +2299,9 @@ int osc_enqueue_base(struct obd_export *exp, struct ldlm_res_id *res_id,
 			ldlm_lock_decref(lockh, mode);
 			LDLM_LOCK_PUT(matched);
 			return -ECANCELED;
-		} else if (osc_set_lock_data_with_check(matched, einfo)) {
+		}
+
+		if (osc_set_lock_data_with_check(matched, einfo)) {
 			*flags |= LDLM_FL_LVB_READY;
 			/* addref the lock only if not async requests and PW
 			 * lock is matched whereas we asked for PR. */
@@ -2325,10 +2326,10 @@ int osc_enqueue_base(struct obd_export *exp, struct ldlm_res_id *res_id,
 				ldlm_lock_decref(lockh, einfo->ei_mode);
 			LDLM_LOCK_PUT(matched);
 			return ELDLM_OK;
-		} else {
-			ldlm_lock_decref(lockh, mode);
-			LDLM_LOCK_PUT(matched);
 		}
+
+		ldlm_lock_decref(lockh, mode);
+		LDLM_LOCK_PUT(matched);
 	}
 
  no_match:
@@ -2480,10 +2481,10 @@ static int osc_statfs_async(struct obd_export *exp,
 			    struct obd_info *oinfo, __u64 max_age,
 			    struct ptlrpc_request_set *rqset)
 {
-	struct obd_device     *obd = class_exp2obd(exp);
+	struct obd_device *obd = class_exp2obd(exp);
 	struct ptlrpc_request *req;
 	struct osc_async_args *aa;
-	int		    rc;
+	int rc;
 
 	/* We could possibly pass max_age in the request (as an absolute
 	 * timestamp or a "seconds.usec ago") so the target can avoid doing
@@ -2522,10 +2523,10 @@ static int osc_statfs_async(struct obd_export *exp,
 static int osc_statfs(const struct lu_env *env, struct obd_export *exp,
 		      struct obd_statfs *osfs, __u64 max_age, __u32 flags)
 {
-	struct obd_device     *obd = class_exp2obd(exp);
-	struct obd_statfs     *msfs;
+	struct obd_device *obd = class_exp2obd(exp);
+	struct obd_statfs *msfs;
 	struct ptlrpc_request *req;
-	struct obd_import     *imp = NULL;
+	struct obd_import *imp = NULL;
 	int rc;
 
 	/*Since the request might also come from lprocfs, so we need
@@ -2617,7 +2618,7 @@ static int osc_getstripe(struct lov_stripe_md *lsm, struct lov_user_md *lump)
 	 * because lov_user_md_vX and lov_mds_md_vX have the same size */
 	if (lum.lmm_stripe_count > 0) {
 		lum_size = lov_mds_md_size(lum.lmm_stripe_count, lum.lmm_magic);
-		OBD_ALLOC(lumk, lum_size);
+		lumk = kzalloc(lum_size, GFP_NOFS);
 		if (!lumk)
 			return -ENOMEM;
 
@@ -2639,7 +2640,7 @@ static int osc_getstripe(struct lov_stripe_md *lsm, struct lov_user_md *lump)
 		rc = -EFAULT;
 
 	if (lumk != &lum)
-		OBD_FREE(lumk, lum_size);
+		kfree(lumk);
 
 	return rc;
 }
@@ -2749,9 +2750,9 @@ static int osc_get_info(const struct lu_env *env, struct obd_export *exp,
 		return 0;
 	} else if (KEY_IS(KEY_LAST_ID)) {
 		struct ptlrpc_request *req;
-		u64		*reply;
-		char		  *tmp;
-		int		    rc;
+		u64 *reply;
+		char *tmp;
+		int rc;
 
 		req = ptlrpc_request_alloc(class_exp2cliimp(exp),
 					   &RQF_OST_GET_INFO_LAST_ID);
@@ -2788,14 +2789,14 @@ static int osc_get_info(const struct lu_env *env, struct obd_export *exp,
 	} else if (KEY_IS(KEY_FIEMAP)) {
 		struct ll_fiemap_info_key *fm_key =
 				(struct ll_fiemap_info_key *)key;
-		struct ldlm_res_id	 res_id;
-		ldlm_policy_data_t	 policy;
-		struct lustre_handle	 lockh;
-		ldlm_mode_t		 mode = 0;
-		struct ptlrpc_request	*req;
-		struct ll_user_fiemap	*reply;
-		char			*tmp;
-		int			 rc;
+		struct ldlm_res_id res_id;
+		ldlm_policy_data_t policy;
+		struct lustre_handle lockh;
+		ldlm_mode_t mode = 0;
+		struct ptlrpc_request *req;
+		struct ll_user_fiemap *reply;
+		char *tmp;
+		int rc;
 
 		if (!(fm_key->fiemap.fm_flags & FIEMAP_FLAG_SYNC))
 			goto skip_locking;
@@ -2881,10 +2882,10 @@ static int osc_set_info_async(const struct lu_env *env, struct obd_export *exp,
 			      void *val, struct ptlrpc_request_set *set)
 {
 	struct ptlrpc_request *req;
-	struct obd_device     *obd = exp->exp_obd;
-	struct obd_import     *imp = class_exp2cliimp(exp);
-	char		  *tmp;
-	int		    rc;
+	struct obd_device *obd = exp->exp_obd;
+	struct obd_import *imp = class_exp2cliimp(exp);
+	char *tmp;
+	int rc;
 
 	OBD_FAIL_TIMEOUT(OBD_FAIL_OSC_SHUTDOWN, 10);
 
@@ -3071,8 +3072,8 @@ static int osc_import_event(struct obd_device *obd,
 	}
 	case IMP_EVENT_INVALIDATE: {
 		struct ldlm_namespace *ns = obd->obd_namespace;
-		struct lu_env	 *env;
-		int		    refcheck;
+		struct lu_env *env;
+		int refcheck;
 
 		env = cl_env_get(&refcheck);
 		if (!IS_ERR(env)) {
@@ -3159,9 +3160,9 @@ static int brw_queue_work(const struct lu_env *env, void *data)
 int osc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 {
 	struct lprocfs_static_vars lvars = { NULL };
-	struct client_obd	  *cli = &obd->u.cli;
-	void		       *handler;
-	int			rc;
+	struct client_obd *cli = &obd->u.cli;
+	void *handler;
+	int rc;
 
 	rc = ptlrpcd_addref();
 	if (rc)
@@ -3184,7 +3185,7 @@ int osc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 
 	cli->cl_grant_shrink_interval = GRANT_SHRINK_INTERVAL;
 	lprocfs_osc_init_vars(&lvars);
-	if (lprocfs_obd_setup(obd, lvars.obd_vars) == 0) {
+	if (lprocfs_obd_setup(obd, lvars.obd_vars, lvars.sysfs_vars) == 0) {
 		lproc_osc_attach_seqstat(obd);
 		sptlrpc_lprocfs_cliobd_attach(obd);
 		ptlrpc_lprocfs_register_obd(obd);
@@ -3351,7 +3352,7 @@ static int __init osc_init(void)
 
 	lprocfs_osc_init_vars(&lvars);
 
-	rc = class_register_type(&osc_obd_ops, NULL, lvars.module_vars,
+	rc = class_register_type(&osc_obd_ops, NULL,
 				 LUSTRE_OSC_NAME, &osc_device_type);
 	if (rc) {
 		lu_kmem_fini(osc_caches);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/Makefile b/drivers/staging/lustre/lustre/ptlrpc/Makefile
index fb50cd4c65b6..24bbac19ddd1 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/Makefile
+++ b/drivers/staging/lustre/lustre/ptlrpc/Makefile
@@ -15,6 +15,5 @@ ptlrpc_objs += pers.o lproc_ptlrpc.o wiretest.o layout.o
 ptlrpc_objs += sec.o sec_bulk.o sec_gc.o sec_config.o
 ptlrpc_objs += sec_null.o sec_plain.o nrs.o nrs_fifo.o
 
-ptlrpc-y := $(ldlm_objs) $(ptlrpc_objs)
-ptlrpc-$(CONFIG_PROC_FS) += sec_lproc.o
+ptlrpc-y := $(ldlm_objs) $(ptlrpc_objs) sec_lproc.o
 ptlrpc-$(CONFIG_LUSTRE_TRANSLATE_ERRNOS) += errno.o
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index 0357f1d4532f..a12cd66b2365 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -57,8 +57,8 @@ void ptlrpc_init_client(int req_portal, int rep_portal, char *name,
 			struct ptlrpc_client *cl)
 {
 	cl->cli_request_portal = req_portal;
-	cl->cli_reply_portal   = rep_portal;
-	cl->cli_name	   = name;
+	cl->cli_reply_portal = rep_portal;
+	cl->cli_name = name;
 }
 EXPORT_SYMBOL(ptlrpc_init_client);
 
@@ -68,9 +68,9 @@ EXPORT_SYMBOL(ptlrpc_init_client);
 struct ptlrpc_connection *ptlrpc_uuid_to_connection(struct obd_uuid *uuid)
 {
 	struct ptlrpc_connection *c;
-	lnet_nid_t		self;
-	lnet_process_id_t	 peer;
-	int		       err;
+	lnet_nid_t self;
+	lnet_process_id_t peer;
+	int err;
 
 	/* ptlrpc_uuid_to_peer() initializes its 2nd parameter
 	 * before accessing its values. */
@@ -103,7 +103,8 @@ struct ptlrpc_bulk_desc *ptlrpc_new_bulk(unsigned npages, unsigned max_brw,
 	struct ptlrpc_bulk_desc *desc;
 	int i;
 
-	OBD_ALLOC(desc, offsetof(struct ptlrpc_bulk_desc, bd_iov[npages]));
+	desc = kzalloc(offsetof(struct ptlrpc_bulk_desc, bd_iov[npages]),
+		       GFP_NOFS);
 	if (!desc)
 		return NULL;
 
@@ -147,7 +148,7 @@ struct ptlrpc_bulk_desc *ptlrpc_prep_bulk_imp(struct ptlrpc_request *req,
 	desc->bd_import = class_import_get(imp);
 	desc->bd_req = req;
 
-	desc->bd_cbid.cbid_fn  = client_bulk_callback;
+	desc->bd_cbid.cbid_fn = client_bulk_callback;
 	desc->bd_cbid.cbid_arg = desc;
 
 	/* This makes req own desc, and free it when she frees herself */
@@ -205,8 +206,7 @@ void __ptlrpc_free_bulk(struct ptlrpc_bulk_desc *desc, int unpin)
 			page_cache_release(desc->bd_iov[i].kiov_page);
 	}
 
-	OBD_FREE(desc, offsetof(struct ptlrpc_bulk_desc,
-				bd_iov[desc->bd_max_iov]));
+	kfree(desc);
 }
 EXPORT_SYMBOL(__ptlrpc_free_bulk);
 
@@ -343,8 +343,8 @@ static int unpack_reply(struct ptlrpc_request *req)
 static int ptlrpc_at_recv_early_reply(struct ptlrpc_request *req)
 {
 	struct ptlrpc_request *early_req;
-	time_t		 olddl;
-	int		    rc;
+	time_t olddl;
+	int rc;
 
 	req->rq_early = 0;
 	spin_unlock(&req->rq_lock);
@@ -390,7 +390,7 @@ static int ptlrpc_at_recv_early_reply(struct ptlrpc_request *req)
 	return rc;
 }
 
-struct kmem_cache *request_cache;
+static struct kmem_cache *request_cache;
 
 int ptlrpc_request_cache_init(void)
 {
@@ -435,11 +435,11 @@ void ptlrpc_free_rq_pool(struct ptlrpc_request_pool *pool)
 		list_del(&req->rq_list);
 		LASSERT(req->rq_reqbuf);
 		LASSERT(req->rq_reqbuf_len == pool->prp_rq_size);
-		OBD_FREE_LARGE(req->rq_reqbuf, pool->prp_rq_size);
+		kvfree(req->rq_reqbuf);
 		ptlrpc_request_cache_free(req);
 	}
 	spin_unlock(&pool->prp_lock);
-	OBD_FREE(pool, sizeof(*pool));
+	kfree(pool);
 }
 EXPORT_SYMBOL(ptlrpc_free_rq_pool);
 
@@ -469,7 +469,7 @@ void ptlrpc_add_rqs_to_pool(struct ptlrpc_request_pool *pool, int num_rq)
 		req = ptlrpc_request_cache_alloc(GFP_NOFS);
 		if (!req)
 			return;
-		OBD_ALLOC_LARGE(msg, size);
+		msg = libcfs_kvzalloc(size, GFP_NOFS);
 		if (!msg) {
 			ptlrpc_request_cache_free(req);
 			return;
@@ -498,7 +498,7 @@ ptlrpc_init_rq_pool(int num_rq, int msgsize,
 {
 	struct ptlrpc_request_pool *pool;
 
-	OBD_ALLOC(pool, sizeof(struct ptlrpc_request_pool));
+	pool = kzalloc(sizeof(struct ptlrpc_request_pool), GFP_NOFS);
 	if (!pool)
 		return NULL;
 
@@ -514,7 +514,7 @@ ptlrpc_init_rq_pool(int num_rq, int msgsize,
 
 	if (list_empty(&pool->prp_req_list)) {
 		/* have not allocated a single request for the pool */
-		OBD_FREE(pool, sizeof(struct ptlrpc_request_pool));
+		kfree(pool);
 		pool = NULL;
 	}
 	return pool;
@@ -580,8 +580,8 @@ static int __ptlrpc_request_bufs_pack(struct ptlrpc_request *request,
 				      int count, __u32 *lengths, char **bufs,
 				      struct ptlrpc_cli_ctx *ctx)
 {
-	struct obd_import  *imp = request->rq_import;
-	int		 rc;
+	struct obd_import *imp = request->rq_import;
+	int rc;
 
 	if (unlikely(ctx))
 		request->rq_cli_ctx = sptlrpc_cli_ctx_get(ctx);
@@ -605,10 +605,10 @@ static int __ptlrpc_request_bufs_pack(struct ptlrpc_request *request,
 	request->rq_type = PTL_RPC_MSG_REQUEST;
 	request->rq_export = NULL;
 
-	request->rq_req_cbid.cbid_fn  = request_out_callback;
+	request->rq_req_cbid.cbid_fn = request_out_callback;
 	request->rq_req_cbid.cbid_arg = request;
 
-	request->rq_reply_cbid.cbid_fn  = reply_in_callback;
+	request->rq_reply_cbid.cbid_fn = reply_in_callback;
 	request->rq_reply_cbid.cbid_arg = request;
 
 	request->rq_reply_deadline = 0;
@@ -761,8 +761,8 @@ EXPORT_SYMBOL(ptlrpc_request_alloc);
  * initialize its buffer structure according to capsule template \a format.
  */
 struct ptlrpc_request *ptlrpc_request_alloc_pool(struct obd_import *imp,
-					    struct ptlrpc_request_pool *pool,
-					    const struct req_format *format)
+						 struct ptlrpc_request_pool *pool,
+						 const struct req_format *format)
 {
 	return ptlrpc_request_alloc_internal(imp, pool, format);
 }
@@ -789,11 +789,11 @@ EXPORT_SYMBOL(ptlrpc_request_free);
  * Returns allocated request or NULL on error.
  */
 struct ptlrpc_request *ptlrpc_request_alloc_pack(struct obd_import *imp,
-						const struct req_format *format,
-						__u32 version, int opcode)
+						 const struct req_format *format,
+						 __u32 version, int opcode)
 {
 	struct ptlrpc_request *req = ptlrpc_request_alloc(imp, format);
-	int		    rc;
+	int rc;
 
 	if (req) {
 		rc = ptlrpc_request_pack(req, version, opcode);
@@ -820,7 +820,7 @@ ptlrpc_prep_req_pool(struct obd_import *imp,
 		     struct ptlrpc_request_pool *pool)
 {
 	struct ptlrpc_request *request;
-	int		    rc;
+	int rc;
 
 	request = __ptlrpc_request_alloc(imp, pool);
 	if (!request)
@@ -856,7 +856,7 @@ struct ptlrpc_request_set *ptlrpc_prep_set(void)
 {
 	struct ptlrpc_request_set *set;
 
-	OBD_ALLOC(set, sizeof(*set));
+	set = kzalloc(sizeof(*set), GFP_NOFS);
 	if (!set)
 		return NULL;
 	atomic_set(&set->set_refcount, 1);
@@ -868,9 +868,9 @@ struct ptlrpc_request_set *ptlrpc_prep_set(void)
 	INIT_LIST_HEAD(&set->set_new_requests);
 	INIT_LIST_HEAD(&set->set_cblist);
 	set->set_max_inflight = UINT_MAX;
-	set->set_producer     = NULL;
+	set->set_producer = NULL;
 	set->set_producer_arg = NULL;
-	set->set_rc	   = 0;
+	set->set_rc = 0;
 
 	return set;
 }
@@ -894,9 +894,9 @@ struct ptlrpc_request_set *ptlrpc_prep_fcset(int max, set_producer_func func,
 	if (!set)
 		return NULL;
 
-	set->set_max_inflight  = max;
-	set->set_producer      = func;
-	set->set_producer_arg  = arg;
+	set->set_max_inflight = max;
+	set->set_producer = func;
+	set->set_producer_arg = arg;
 
 	return set;
 }
@@ -912,10 +912,10 @@ EXPORT_SYMBOL(ptlrpc_prep_fcset);
  */
 void ptlrpc_set_destroy(struct ptlrpc_request_set *set)
 {
-	struct list_head       *tmp;
-	struct list_head       *next;
-	int	       expected_phase;
-	int	       n = 0;
+	struct list_head *tmp;
+	struct list_head *next;
+	int expected_phase;
+	int n = 0;
 
 	/* Requests on the set should either all be completed, or all be new */
 	expected_phase = (atomic_read(&set->set_remaining) == 0) ?
@@ -970,7 +970,7 @@ int ptlrpc_set_add_cb(struct ptlrpc_request_set *set,
 {
 	struct ptlrpc_set_cbdata *cbdata;
 
-	OBD_ALLOC_PTR(cbdata);
+	cbdata = kzalloc(sizeof(*cbdata), GFP_NOFS);
 	if (cbdata == NULL)
 		return -ENOMEM;
 
@@ -1013,7 +1013,7 @@ EXPORT_SYMBOL(ptlrpc_set_add_req);
  * Currently only used for ptlrpcd.
  */
 void ptlrpc_set_add_new_req(struct ptlrpcd_ctl *pc,
-			   struct ptlrpc_request *req)
+			    struct ptlrpc_request *req)
 {
 	struct ptlrpc_request_set *set = pc->pc_set;
 	int count, i;
@@ -1400,7 +1400,7 @@ static int after_reply(struct ptlrpc_request *req)
  */
 static int ptlrpc_send_new_req(struct ptlrpc_request *req)
 {
-	struct obd_import     *imp = req->rq_import;
+	struct obd_import *imp = req->rq_import;
 	int rc;
 
 	LASSERT(req->rq_phase == RQ_PHASE_NEW);
@@ -1669,7 +1669,7 @@ int ptlrpc_check_set(const struct lu_env *env, struct ptlrpc_request_set *set)
 					continue;
 				}
 
-				if (status != 0)  {
+				if (status != 0) {
 					req->rq_status = status;
 					ptlrpc_rqphase_move(req,
 						RQ_PHASE_INTERPRET);
@@ -1969,8 +1969,8 @@ int ptlrpc_expire_one_request(struct ptlrpc_request *req, int async_unlink)
 int ptlrpc_expired_set(void *data)
 {
 	struct ptlrpc_request_set *set = data;
-	struct list_head		*tmp;
-	time_t		     now = get_seconds();
+	struct list_head *tmp;
+	time_t now = get_seconds();
 
 	LASSERT(set != NULL);
 
@@ -2052,11 +2052,11 @@ EXPORT_SYMBOL(ptlrpc_interrupted_set);
  */
 int ptlrpc_set_next_timeout(struct ptlrpc_request_set *set)
 {
-	struct list_head	    *tmp;
-	time_t		 now = get_seconds();
-	int		    timeout = 0;
+	struct list_head *tmp;
+	time_t now = get_seconds();
+	int timeout = 0;
 	struct ptlrpc_request *req;
-	int		    deadline;
+	int deadline;
 
 	list_for_each(tmp, &set->set_requests) {
 		req = list_entry(tmp, struct ptlrpc_request, rq_set_chain);
@@ -2105,10 +2105,10 @@ EXPORT_SYMBOL(ptlrpc_set_next_timeout);
  */
 int ptlrpc_set_wait(struct ptlrpc_request_set *set)
 {
-	struct list_head	    *tmp;
+	struct list_head *tmp;
 	struct ptlrpc_request *req;
-	struct l_wait_info     lwi;
-	int		    rc, timeout;
+	struct l_wait_info lwi;
+	int rc, timeout;
 
 	if (set->set_producer)
 		(void)ptlrpc_set_producer(set);
@@ -2214,7 +2214,7 @@ int ptlrpc_set_wait(struct ptlrpc_request_set *set)
 			err = cbdata->psc_interpret(set, cbdata->psc_data, rc);
 			if (err && !rc)
 				rc = err;
-			OBD_FREE_PTR(cbdata);
+			kfree(cbdata);
 		}
 	}
 
@@ -2353,8 +2353,8 @@ EXPORT_SYMBOL(ptlrpc_req_xid);
  */
 int ptlrpc_unregister_reply(struct ptlrpc_request *request, int async)
 {
-	int		rc;
-	wait_queue_head_t       *wq;
+	int rc;
+	wait_queue_head_t *wq;
 	struct l_wait_info lwi;
 
 	/*
@@ -2471,7 +2471,7 @@ void ptlrpc_free_committed(struct obd_import *imp)
 {
 	struct ptlrpc_request *req, *saved;
 	struct ptlrpc_request *last_req = NULL; /* temporary fire escape */
-	bool		       skip_committed_list = true;
+	bool skip_committed_list = true;
 
 	LASSERT(imp != NULL);
 	assert_spin_locked(&imp->imp_lock);
@@ -3023,8 +3023,8 @@ EXPORT_SYMBOL(ptlrpc_sample_next_xid);
  *    have delay before it really runs by ptlrpcd thread.
  */
 struct ptlrpc_work_async_args {
-	int   (*cb)(const struct lu_env *, void *);
-	void   *cbdata;
+	int (*cb)(const struct lu_env *, void *);
+	void *cbdata;
 };
 
 static void ptlrpcd_add_work_req(struct ptlrpc_request *req)
@@ -3113,7 +3113,7 @@ void *ptlrpcd_alloc_work(struct obd_import *imp,
 
 	CLASSERT(sizeof(*args) <= sizeof(req->rq_async_args));
 	args = ptlrpc_req_async_args(req);
-	args->cb     = cb;
+	args->cb = cb;
 	args->cbdata = cbdata;
 
 	return req;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/connection.c b/drivers/staging/lustre/lustre/ptlrpc/connection.c
index 7e27397ce384..ffe36e22245f 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/connection.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/connection.c
@@ -54,7 +54,7 @@ ptlrpc_connection_get(lnet_process_id_t peer, lnet_nid_t self,
 	if (conn)
 		goto out;
 
-	OBD_ALLOC_PTR(conn);
+	conn = kzalloc(sizeof(*conn), GFP_NOFS);
 	if (!conn)
 		return NULL;
 
@@ -76,7 +76,7 @@ ptlrpc_connection_get(lnet_process_id_t peer, lnet_nid_t self,
 	/* coverity[overrun-buffer-val] */
 	conn2 = cfs_hash_findadd_unique(conn_hash, &peer, &conn->c_hash);
 	if (conn != conn2) {
-		OBD_FREE_PTR(conn);
+		kfree(conn);
 		conn = conn2;
 	}
 out:
@@ -227,7 +227,7 @@ conn_exit(struct cfs_hash *hs, struct hlist_node *hnode)
 	LASSERTF(atomic_read(&conn->c_refcount) == 0,
 		 "Busy connection with %d refs\n",
 		 atomic_read(&conn->c_refcount));
-	OBD_FREE_PTR(conn);
+	kfree(conn);
 }
 
 static cfs_hash_ops_t conn_hash_ops = {
diff --git a/drivers/staging/lustre/lustre/ptlrpc/events.c b/drivers/staging/lustre/lustre/ptlrpc/events.c
index 7f8644e01112..8cb1929fd31d 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/events.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/events.c
@@ -53,7 +53,7 @@ lnet_handle_eq_t   ptlrpc_eq_h;
  */
 void request_out_callback(lnet_event_t *ev)
 {
-	struct ptlrpc_cb_id   *cbid = ev->md.user_ptr;
+	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_request *req = cbid->cbid_arg;
 
 	LASSERT(ev->type == LNET_EVENT_SEND ||
@@ -86,7 +86,7 @@ void request_out_callback(lnet_event_t *ev)
  */
 void reply_in_callback(lnet_event_t *ev)
 {
-	struct ptlrpc_cb_id   *cbid = ev->md.user_ptr;
+	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_request *req = cbid->cbid_arg;
 
 	DEBUG_REQ(D_NET, req, "type %d, status %d", ev->type, ev->status);
@@ -172,9 +172,9 @@ out_wake:
  */
 void client_bulk_callback(lnet_event_t *ev)
 {
-	struct ptlrpc_cb_id     *cbid = ev->md.user_ptr;
+	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_bulk_desc *desc = cbid->cbid_arg;
-	struct ptlrpc_request   *req;
+	struct ptlrpc_request *req;
 
 	LASSERT((desc->bd_type == BULK_PUT_SINK &&
 		 ev->type == LNET_EVENT_PUT) ||
@@ -245,9 +245,9 @@ void client_bulk_callback(lnet_event_t *ev)
 static void ptlrpc_req_add_history(struct ptlrpc_service_part *svcpt,
 				   struct ptlrpc_request *req)
 {
-	__u64	sec = req->rq_arrival_time.tv_sec;
-	__u32	usec = req->rq_arrival_time.tv_usec >> 4; /* usec / 16 */
-	__u64	new_seq;
+	__u64 sec = req->rq_arrival_time.tv_sec;
+	__u32 usec = req->rq_arrival_time.tv_usec >> 4; /* usec / 16 */
+	__u64 new_seq;
 
 	/* set sequence ID for request and add it to history list,
 	 * it must be called with hold svcpt::scp_lock */
@@ -281,11 +281,11 @@ static void ptlrpc_req_add_history(struct ptlrpc_service_part *svcpt,
  */
 void request_in_callback(lnet_event_t *ev)
 {
-	struct ptlrpc_cb_id		  *cbid = ev->md.user_ptr;
+	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_request_buffer_desc *rqbd = cbid->cbid_arg;
-	struct ptlrpc_service_part	  *svcpt = rqbd->rqbd_svcpt;
-	struct ptlrpc_service	     *service = svcpt->scp_service;
-	struct ptlrpc_request	     *req;
+	struct ptlrpc_service_part *svcpt = rqbd->rqbd_svcpt;
+	struct ptlrpc_service *service = svcpt->scp_service;
+	struct ptlrpc_request *req;
 
 	LASSERT(ev->type == LNET_EVENT_PUT ||
 		ev->type == LNET_EVENT_UNLINK);
@@ -380,7 +380,7 @@ void request_in_callback(lnet_event_t *ev)
  */
 void reply_out_callback(lnet_event_t *ev)
 {
-	struct ptlrpc_cb_id	  *cbid = ev->md.user_ptr;
+	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_reply_state *rs = cbid->cbid_arg;
 	struct ptlrpc_service_part *svcpt = rs->rs_svcpt;
 
@@ -433,17 +433,17 @@ static void ptlrpc_master_callback(lnet_event_t *ev)
 }
 
 int ptlrpc_uuid_to_peer(struct obd_uuid *uuid,
-			 lnet_process_id_t *peer, lnet_nid_t *self)
+			lnet_process_id_t *peer, lnet_nid_t *self)
 {
-	int	       best_dist = 0;
-	__u32	     best_order = 0;
-	int	       count = 0;
-	int	       rc = -ENOENT;
-	int	       portals_compatibility;
-	int	       dist;
-	__u32	     order;
-	lnet_nid_t	dst_nid;
-	lnet_nid_t	src_nid;
+	int best_dist = 0;
+	__u32 best_order = 0;
+	int count = 0;
+	int rc = -ENOENT;
+	int portals_compatibility;
+	int dist;
+	__u32 order;
+	lnet_nid_t dst_nid;
+	lnet_nid_t src_nid;
 
 	portals_compatibility = LNetCtl(IOC_LIBCFS_PORTALS_COMPATIBILITY, NULL);
 
@@ -487,10 +487,10 @@ int ptlrpc_uuid_to_peer(struct obd_uuid *uuid,
 
 void ptlrpc_ni_fini(void)
 {
-	wait_queue_head_t	 waitq;
-	struct l_wait_info  lwi;
-	int		 rc;
-	int		 retries;
+	wait_queue_head_t waitq;
+	struct l_wait_info lwi;
+	int rc;
+	int retries;
 
 	/* Wait for the event queue to become idle since there may still be
 	 * messages in flight with pending events (i.e. the fire-and-forget
@@ -523,7 +523,7 @@ void ptlrpc_ni_fini(void)
 
 lnet_pid_t ptl_get_pid(void)
 {
-	lnet_pid_t	pid;
+	lnet_pid_t pid;
 
 	pid = LUSTRE_SRV_LNET_PID;
 	return pid;
@@ -531,8 +531,8 @@ lnet_pid_t ptl_get_pid(void)
 
 int ptlrpc_ni_init(void)
 {
-	int	      rc;
-	lnet_pid_t       pid;
+	int rc;
+	lnet_pid_t pid;
 
 	pid = ptl_get_pid();
 	CDEBUG(D_NET, "My pid is: %x\n", pid);
@@ -563,7 +563,7 @@ int ptlrpc_ni_init(void)
 
 int ptlrpc_init_portals(void)
 {
-	int   rc = ptlrpc_ni_init();
+	int rc = ptlrpc_ni_init();
 
 	if (rc != 0) {
 		CERROR("network initialisation failed\n");
diff --git a/drivers/staging/lustre/lustre/ptlrpc/import.c b/drivers/staging/lustre/lustre/ptlrpc/import.c
index d5fc689c008b..c9b8481dd384 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/import.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/import.c
@@ -119,7 +119,7 @@ int ptlrpc_init_import(struct obd_import *imp)
 	spin_lock(&imp->imp_lock);
 
 	imp->imp_generation++;
-	imp->imp_state =  LUSTRE_IMP_NEW;
+	imp->imp_state = LUSTRE_IMP_NEW;
 
 	spin_unlock(&imp->imp_lock);
 
@@ -369,7 +369,7 @@ void ptlrpc_invalidate_import(struct obd_import *imp)
 						   imp_unregistering));
 			}
 			spin_unlock(&imp->imp_lock);
-		  }
+		}
 	} while (rc != 0);
 
 	/*
@@ -559,7 +559,7 @@ static int import_select_connection(struct obd_import *imp)
 		ptlrpc_connection_put(imp->imp_connection);
 	imp->imp_connection = ptlrpc_connection_addref(imp_conn->oic_conn);
 
-	dlmexp =  class_conn2export(&imp->imp_dlm_handle);
+	dlmexp = class_conn2export(&imp->imp_dlm_handle);
 	LASSERT(dlmexp != NULL);
 	if (dlmexp->exp_connection)
 		ptlrpc_connection_put(dlmexp->exp_connection);
@@ -1490,7 +1490,7 @@ int ptlrpc_disconnect_import(struct obd_import *imp, int noclose)
 					INITIAL_CONNECT_TIMEOUT);
 
 		IMPORT_SET_STATE(imp, LUSTRE_IMP_CONNECTING);
-		req->rq_send_state =  LUSTRE_IMP_CONNECTING;
+		req->rq_send_state = LUSTRE_IMP_CONNECTING;
 		ptlrpc_request_set_replen(req);
 		rc = ptlrpc_queue_wait(req);
 		ptlrpc_req_finished(req);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/layout.c b/drivers/staging/lustre/lustre/ptlrpc/layout.c
index a42335e26de9..d14c20008850 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/layout.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/layout.c
@@ -807,11 +807,11 @@ struct req_capsule;
 /*
  * Request fields.
  */
-#define DEFINE_MSGF(name, flags, size, swabber, dumper) {       \
-	.rmf_name    = (name),				  \
-	.rmf_flags   = (flags),				 \
-	.rmf_size    = (size),				  \
-	.rmf_swabber = (void (*)(void *))(swabber),	      \
+#define DEFINE_MSGF(name, flags, size, swabber, dumper) {	\
+	.rmf_name    = (name),					\
+	.rmf_flags   = (flags),					\
+	.rmf_size    = (size),					\
+	.rmf_swabber = (void (*)(void *))(swabber),		\
 	.rmf_dumper  = (void (*)(void *))(dumper)		\
 }
 
@@ -1164,25 +1164,25 @@ EXPORT_SYMBOL(RMF_SWAP_LAYOUTS);
 
 struct req_format {
 	const char *rf_name;
-	int	 rf_idx;
+	int rf_idx;
 	struct {
-		int			  nr;
+		int nr;
 		const struct req_msg_field **d;
 	} rf_fields[RCL_NR];
 };
 
-#define DEFINE_REQ_FMT(name, client, client_nr, server, server_nr) {    \
-	.rf_name   = name,					      \
-	.rf_fields = {						  \
+#define DEFINE_REQ_FMT(name, client, client_nr, server, server_nr) {	\
+	.rf_name = name,						\
+	.rf_fields = {							\
 		[RCL_CLIENT] = {					\
 			.nr = client_nr,				\
-			.d  = client				    \
-		},						      \
+			.d = client					\
+		},							\
 		[RCL_SERVER] = {					\
 			.nr = server_nr,				\
-			.d  = server				    \
-		}						       \
-	}							       \
+			.d = server					\
+		}							\
+	}								\
 }
 
 #define DEFINE_REQ_FMT0(name, client, server)				  \
@@ -1769,10 +1769,10 @@ EXPORT_SYMBOL(req_capsule_set);
  * field of a \a pill's \a rc_fmt's RMF's.
  */
 int req_capsule_filled_sizes(struct req_capsule *pill,
-			   enum req_location loc)
+			     enum req_location loc)
 {
 	const struct req_format *fmt = pill->rc_fmt;
-	int		      i;
+	int i;
 
 	LASSERT(fmt != NULL);
 
@@ -1806,8 +1806,8 @@ EXPORT_SYMBOL(req_capsule_filled_sizes);
 int req_capsule_server_pack(struct req_capsule *pill)
 {
 	const struct req_format *fmt;
-	int		      count;
-	int		      rc;
+	int count;
+	int rc;
 
 	LASSERT(pill->rc_loc == RCL_SERVER);
 	fmt = pill->rc_fmt;
@@ -1857,11 +1857,11 @@ swabber_dumper_helper(struct req_capsule *pill,
 		      int offset,
 		      void *value, int len, int dump, void (*swabber)(void *))
 {
-	void    *p;
-	int     i;
-	int     n;
-	int     do_swab;
-	int     inout = loc == RCL_CLIENT;
+	void *p;
+	int i;
+	int n;
+	int do_swab;
+	int inout = loc == RCL_CLIENT;
 
 	swabber = swabber ?: field->rmf_swabber;
 
@@ -1936,10 +1936,10 @@ static void *__req_capsule_get(struct req_capsule *pill,
 			       int dump)
 {
 	const struct req_format *fmt;
-	struct lustre_msg       *msg;
-	void		    *value;
-	int		      len;
-	int		      offset;
+	struct lustre_msg *msg;
+	void *value;
+	int len;
+	int offset;
 
 	void *(*getter)(struct lustre_msg *m, int n, int minlen);
 
@@ -2000,10 +2000,10 @@ static void *__req_capsule_get(struct req_capsule *pill,
  */
 static void __req_capsule_dump(struct req_capsule *pill, enum req_location loc)
 {
-	const struct    req_format *fmt;
-	const struct    req_msg_field *field;
-	int	     len;
-	int	     i;
+	const struct req_format *fmt;
+	const struct req_msg_field *field;
+	int len;
+	int i;
 
 	fmt = pill->rc_fmt;
 
@@ -2350,9 +2350,9 @@ void req_capsule_shrink(struct req_capsule *pill,
 			enum req_location loc)
 {
 	const struct req_format *fmt;
-	struct lustre_msg       *msg;
-	int		      len;
-	int		      offset;
+	struct lustre_msg *msg;
+	int len;
+	int offset;
 
 	fmt = pill->rc_fmt;
 	LASSERT(fmt != NULL);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/llog_client.c b/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
index e9baf5bbee3a..1c701e0a0bc7 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
@@ -49,29 +49,29 @@
 #include "../include/lustre_net.h"
 #include <linux/list.h>
 
-#define LLOG_CLIENT_ENTRY(ctxt, imp) do {			     \
-	mutex_lock(&ctxt->loc_mutex);			     \
-	if (ctxt->loc_imp) {					  \
-		imp = class_import_get(ctxt->loc_imp);		\
-	} else {						      \
-		CERROR("ctxt->loc_imp == NULL for context idx %d."    \
-		       "Unable to complete MDS/OSS recovery,"	 \
-		       "but I'll try again next time.  Not fatal.\n", \
-		       ctxt->loc_idx);				\
-		imp = NULL;					   \
-		mutex_unlock(&ctxt->loc_mutex);		   \
-		return (-EINVAL);				     \
-	}							     \
-	mutex_unlock(&ctxt->loc_mutex);			   \
+#define LLOG_CLIENT_ENTRY(ctxt, imp) do {				\
+	mutex_lock(&ctxt->loc_mutex);					\
+	if (ctxt->loc_imp) {						\
+		imp = class_import_get(ctxt->loc_imp);			\
+	} else {							\
+		CERROR("ctxt->loc_imp == NULL for context idx %d."	\
+		       "Unable to complete MDS/OSS recovery,"		\
+		       "but I'll try again next time.  Not fatal.\n",	\
+		       ctxt->loc_idx);					\
+		imp = NULL;						\
+		mutex_unlock(&ctxt->loc_mutex);				\
+		return (-EINVAL);					\
+	}								\
+	mutex_unlock(&ctxt->loc_mutex);					\
 } while (0)
 
-#define LLOG_CLIENT_EXIT(ctxt, imp) do {			      \
-	mutex_lock(&ctxt->loc_mutex);			     \
-	if (ctxt->loc_imp != imp)				     \
-		CWARN("loc_imp has changed from %p to %p\n",	  \
-		       ctxt->loc_imp, imp);			   \
-	class_import_put(imp);					\
-	mutex_unlock(&ctxt->loc_mutex);			   \
+#define LLOG_CLIENT_EXIT(ctxt, imp) do {				\
+	mutex_lock(&ctxt->loc_mutex);					\
+	if (ctxt->loc_imp != imp)					\
+		CWARN("loc_imp has changed from %p to %p\n",		\
+		       ctxt->loc_imp, imp);				\
+	class_import_put(imp);						\
+	mutex_unlock(&ctxt->loc_mutex);					\
 } while (0)
 
 /* This is a callback from the llog_* functions.
@@ -80,11 +80,11 @@ static int llog_client_open(const struct lu_env *env,
 			    struct llog_handle *lgh, struct llog_logid *logid,
 			    char *name, enum llog_open_param open_param)
 {
-	struct obd_import     *imp;
-	struct llogd_body     *body;
-	struct llog_ctxt      *ctxt = lgh->lgh_ctxt;
+	struct obd_import *imp;
+	struct llogd_body *body;
+	struct llog_ctxt *ctxt = lgh->lgh_ctxt;
 	struct ptlrpc_request *req = NULL;
-	int		    rc;
+	int rc;
 
 	LLOG_CLIENT_ENTRY(ctxt, imp);
 
@@ -145,10 +145,10 @@ out:
 static int llog_client_destroy(const struct lu_env *env,
 			       struct llog_handle *loghandle)
 {
-	struct obd_import     *imp;
+	struct obd_import *imp;
 	struct ptlrpc_request *req = NULL;
-	struct llogd_body     *body;
-	int		    rc;
+	struct llogd_body *body;
+	int rc;
 
 	LLOG_CLIENT_ENTRY(loghandle->lgh_ctxt, imp);
 	req = ptlrpc_request_alloc_pack(imp, &RQF_LLOG_ORIGIN_HANDLE_DESTROY,
@@ -182,11 +182,11 @@ static int llog_client_next_block(const struct lu_env *env,
 				  int *cur_idx, int next_idx,
 				  __u64 *cur_offset, void *buf, int len)
 {
-	struct obd_import     *imp;
+	struct obd_import *imp;
 	struct ptlrpc_request *req = NULL;
-	struct llogd_body     *body;
-	void		  *ptr;
-	int		    rc;
+	struct llogd_body *body;
+	void *ptr;
+	int rc;
 
 	LLOG_CLIENT_ENTRY(loghandle->lgh_ctxt, imp);
 	req = ptlrpc_request_alloc_pack(imp, &RQF_LLOG_ORIGIN_HANDLE_NEXT_BLOCK,
@@ -240,11 +240,11 @@ static int llog_client_prev_block(const struct lu_env *env,
 				  struct llog_handle *loghandle,
 				  int prev_idx, void *buf, int len)
 {
-	struct obd_import     *imp;
+	struct obd_import *imp;
 	struct ptlrpc_request *req = NULL;
-	struct llogd_body     *body;
-	void		  *ptr;
-	int		    rc;
+	struct llogd_body *body;
+	void *ptr;
+	int rc;
 
 	LLOG_CLIENT_ENTRY(loghandle->lgh_ctxt, imp);
 	req = ptlrpc_request_alloc_pack(imp, &RQF_LLOG_ORIGIN_HANDLE_PREV_BLOCK,
@@ -292,12 +292,12 @@ err_exit:
 static int llog_client_read_header(const struct lu_env *env,
 				   struct llog_handle *handle)
 {
-	struct obd_import     *imp;
+	struct obd_import *imp;
 	struct ptlrpc_request *req = NULL;
-	struct llogd_body     *body;
-	struct llog_log_hdr   *hdr;
-	struct llog_rec_hdr   *llh_hdr;
-	int		    rc;
+	struct llogd_body *body;
+	struct llog_log_hdr *hdr;
+	struct llog_rec_hdr *llh_hdr;
+	int rc;
 
 	LLOG_CLIENT_ENTRY(handle->lgh_ctxt, imp);
 	req = ptlrpc_request_alloc_pack(imp, &RQF_LLOG_ORIGIN_HANDLE_READ_HEADER,
diff --git a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
index 9533ab976a33..aaaabbf5f1b9 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
@@ -181,19 +181,19 @@ static const char *ll_eopcode2str(__u32 opcode)
 	return ll_eopcode_table[opcode].opname;
 }
 
-#if defined(CONFIG_PROC_FS)
-static void ptlrpc_lprocfs_register(struct proc_dir_entry *root, char *dir,
-				    char *name,
-				    struct proc_dir_entry **procroot_ret,
-				    struct lprocfs_stats **stats_ret)
+static void
+ptlrpc_ldebugfs_register(struct dentry *root, char *dir,
+			 char *name,
+			 struct dentry **debugfs_root_ret,
+			 struct lprocfs_stats **stats_ret)
 {
-	struct proc_dir_entry *svc_procroot;
+	struct dentry *svc_debugfs_entry;
 	struct lprocfs_stats *svc_stats;
 	int i, rc;
 	unsigned int svc_counter_config = LPROCFS_CNTR_AVGMINMAX |
 					  LPROCFS_CNTR_STDDEV;
 
-	LASSERT(*procroot_ret == NULL);
+	LASSERT(*debugfs_root_ret == NULL);
 	LASSERT(*stats_ret == NULL);
 
 	svc_stats = lprocfs_alloc_stats(EXTRA_MAX_OPCODES+LUSTRE_MAX_OPCODES,
@@ -201,14 +201,14 @@ static void ptlrpc_lprocfs_register(struct proc_dir_entry *root, char *dir,
 	if (svc_stats == NULL)
 		return;
 
-	if (dir) {
-		svc_procroot = lprocfs_register(dir, root, NULL, NULL);
-		if (IS_ERR(svc_procroot)) {
+	if (dir != NULL) {
+		svc_debugfs_entry = ldebugfs_register(dir, root, NULL, NULL);
+		if (IS_ERR(svc_debugfs_entry)) {
 			lprocfs_free_stats(&svc_stats);
 			return;
 		}
 	} else {
-		svc_procroot = root;
+		svc_debugfs_entry = root;
 	}
 
 	lprocfs_counter_init(svc_stats, PTLRPC_REQWAIT_CNTR,
@@ -244,14 +244,14 @@ static void ptlrpc_lprocfs_register(struct proc_dir_entry *root, char *dir,
 				     ll_opcode2str(opcode), "usec");
 	}
 
-	rc = lprocfs_register_stats(svc_procroot, name, svc_stats);
+	rc = ldebugfs_register_stats(svc_debugfs_entry, name, svc_stats);
 	if (rc < 0) {
-		if (dir)
-			lprocfs_remove(&svc_procroot);
+		if (dir != NULL)
+			ldebugfs_remove(&svc_debugfs_entry);
 		lprocfs_free_stats(&svc_stats);
 	} else {
-		if (dir)
-			*procroot_ret = svc_procroot;
+		if (dir != NULL)
+			*debugfs_root_ret = svc_debugfs_entry;
 		*stats_ret = svc_stats;
 	}
 }
@@ -261,8 +261,8 @@ ptlrpc_lprocfs_req_history_len_seq_show(struct seq_file *m, void *v)
 {
 	struct ptlrpc_service *svc = m->private;
 	struct ptlrpc_service_part *svcpt;
-	int	total = 0;
-	int	i;
+	int total = 0;
+	int i;
 
 	ptlrpc_service_for_each_part(svcpt, i, svc)
 		total += svcpt->scp_hist_nrqbds;
@@ -277,8 +277,8 @@ ptlrpc_lprocfs_req_history_max_seq_show(struct seq_file *m, void *n)
 {
 	struct ptlrpc_service *svc = m->private;
 	struct ptlrpc_service_part *svcpt;
-	int	total = 0;
-	int	i;
+	int total = 0;
+	int i;
 
 	ptlrpc_service_for_each_part(svcpt, i, svc)
 		total += svc->srv_hist_nrqbds_cpt_max;
@@ -289,13 +289,13 @@ ptlrpc_lprocfs_req_history_max_seq_show(struct seq_file *m, void *n)
 
 static ssize_t
 ptlrpc_lprocfs_req_history_max_seq_write(struct file *file,
-					const char __user *buffer,
-					size_t count, loff_t *off)
+					 const char __user *buffer,
+					 size_t count, loff_t *off)
 {
 	struct ptlrpc_service *svc = ((struct seq_file *)file->private_data)->private;
-	int			    bufpages;
-	int			    val;
-	int			    rc;
+	int bufpages;
+	int val;
+	int rc;
 
 	rc = lprocfs_write_helper(buffer, count, &val);
 	if (rc < 0)
@@ -324,23 +324,23 @@ ptlrpc_lprocfs_req_history_max_seq_write(struct file *file,
 }
 LPROC_SEQ_FOPS(ptlrpc_lprocfs_req_history_max);
 
-static int
-ptlrpc_lprocfs_threads_min_seq_show(struct seq_file *m, void *n)
+
+static ssize_t threads_min_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct ptlrpc_service *svc = m->private;
+	struct ptlrpc_service *svc = container_of(kobj, struct ptlrpc_service,
+						  srv_kobj);
 
-	seq_printf(m, "%d\n", svc->srv_nthrs_cpt_init * svc->srv_ncpts);
-	return 0;
+	return sprintf(buf, "%d\n", svc->srv_nthrs_cpt_init * svc->srv_ncpts);
 }
 
-static ssize_t
-ptlrpc_lprocfs_threads_min_seq_write(struct file *file,
-					const char __user *buffer,
-					size_t count, loff_t *off)
+static ssize_t threads_min_store(struct kobject *kobj, struct attribute *attr,
+				 const char *buffer, size_t count)
 {
-	struct ptlrpc_service *svc = ((struct seq_file *)file->private_data)->private;
-	int	val;
-	int	rc = lprocfs_write_helper(buffer, count, &val);
+	struct ptlrpc_service *svc = container_of(kobj, struct ptlrpc_service,
+						  srv_kobj);
+	unsigned long val;
+	int rc = kstrtoul(buffer, 10, &val);
 
 	if (rc < 0)
 		return rc;
@@ -360,41 +360,41 @@ ptlrpc_lprocfs_threads_min_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ptlrpc_lprocfs_threads_min);
+LUSTRE_RW_ATTR(threads_min);
 
-static int
-ptlrpc_lprocfs_threads_started_seq_show(struct seq_file *m, void *n)
+static ssize_t threads_started_show(struct kobject *kobj,
+				    struct attribute *attr,
+				    char *buf)
 {
-	struct ptlrpc_service *svc = m->private;
+	struct ptlrpc_service *svc = container_of(kobj, struct ptlrpc_service,
+						  srv_kobj);
 	struct ptlrpc_service_part *svcpt;
-	int	total = 0;
-	int	i;
+	int total = 0;
+	int i;
 
 	ptlrpc_service_for_each_part(svcpt, i, svc)
 		total += svcpt->scp_nthrs_running;
 
-	seq_printf(m, "%d\n", total);
-	return 0;
+	return sprintf(buf, "%d\n", total);
 }
-LPROC_SEQ_FOPS_RO(ptlrpc_lprocfs_threads_started);
+LUSTRE_RO_ATTR(threads_started);
 
-static int
-ptlrpc_lprocfs_threads_max_seq_show(struct seq_file *m, void *n)
+static ssize_t threads_max_show(struct kobject *kobj, struct attribute *attr,
+				char *buf)
 {
-	struct ptlrpc_service *svc = m->private;
+	struct ptlrpc_service *svc = container_of(kobj, struct ptlrpc_service,
+						  srv_kobj);
 
-	seq_printf(m, "%d\n", svc->srv_nthrs_cpt_limit * svc->srv_ncpts);
-	return 0;
+	return sprintf(buf, "%d\n", svc->srv_nthrs_cpt_limit * svc->srv_ncpts);
 }
 
-static ssize_t
-ptlrpc_lprocfs_threads_max_seq_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
+static ssize_t threads_max_store(struct kobject *kobj, struct attribute *attr,
+				 const char *buffer, size_t count)
 {
-	struct ptlrpc_service *svc = ((struct seq_file *)file->private_data)->private;
-	int	val;
-	int	rc = lprocfs_write_helper(buffer, count, &val);
+	struct ptlrpc_service *svc = container_of(kobj, struct ptlrpc_service,
+						  srv_kobj);
+	unsigned long val;
+	int rc = kstrtoul(buffer, 10, &val);
 
 	if (rc < 0)
 		return rc;
@@ -414,7 +414,7 @@ ptlrpc_lprocfs_threads_max_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ptlrpc_lprocfs_threads_max);
+LUSTRE_RW_ATTR(threads_max);
 
 /**
  * \addtogoup nrs
@@ -478,17 +478,17 @@ void nrs_policy_get_info_locked(struct ptlrpc_nrs_policy *policy,
  */
 static int ptlrpc_lprocfs_nrs_seq_show(struct seq_file *m, void *n)
 {
-	struct ptlrpc_service	       *svc = m->private;
-	struct ptlrpc_service_part     *svcpt;
-	struct ptlrpc_nrs	       *nrs;
-	struct ptlrpc_nrs_policy       *policy;
-	struct ptlrpc_nrs_pol_info     *infos;
-	struct ptlrpc_nrs_pol_info	tmp;
-	unsigned			num_pols;
-	unsigned			pol_idx = 0;
-	bool				hp = false;
-	int				i;
-	int				rc = 0;
+	struct ptlrpc_service *svc = m->private;
+	struct ptlrpc_service_part *svcpt;
+	struct ptlrpc_nrs *nrs;
+	struct ptlrpc_nrs_policy *policy;
+	struct ptlrpc_nrs_pol_info *infos;
+	struct ptlrpc_nrs_pol_info tmp;
+	unsigned num_pols;
+	unsigned pol_idx = 0;
+	bool hp = false;
+	int i;
+	int rc = 0;
 
 	/**
 	 * Serialize NRS core lprocfs operations with policy registration/
@@ -507,10 +507,10 @@ static int ptlrpc_lprocfs_nrs_seq_show(struct seq_file *m, void *n)
 	num_pols = svc->srv_parts[0]->scp_nrs_reg.nrs_num_pols;
 	spin_unlock(&nrs->nrs_lock);
 
-	OBD_ALLOC(infos, num_pols * sizeof(*infos));
+	infos = kcalloc(num_pols, sizeof(*infos), GFP_NOFS);
 	if (infos == NULL) {
 		rc = -ENOMEM;
-		goto out;
+		goto unlock;
 	}
 again:
 
@@ -617,10 +617,8 @@ again:
 		goto again;
 	}
 
-out:
-	if (infos)
-		OBD_FREE(infos, num_pols * sizeof(*infos));
-
+	kfree(infos);
+unlock:
 	mutex_unlock(&nrs_core.nrs_mutex);
 
 	return rc;
@@ -640,26 +638,22 @@ out:
  * regular and high-priority (if the service has one) NRS head.
  */
 static ssize_t ptlrpc_lprocfs_nrs_seq_write(struct file *file,
-					const char __user *buffer,
-					size_t count, loff_t *off)
+					    const char __user *buffer,
+					    size_t count, loff_t *off)
 {
 	struct ptlrpc_service *svc = ((struct seq_file *)file->private_data)->private;
-	enum ptlrpc_nrs_queue_type	queue = PTLRPC_NRS_QUEUE_BOTH;
-	char			       *cmd;
-	char			       *cmd_copy = NULL;
-	char			       *token;
-	int				rc = 0;
+	enum ptlrpc_nrs_queue_type queue = PTLRPC_NRS_QUEUE_BOTH;
+	char *cmd;
+	char *cmd_copy = NULL;
+	char *token;
+	int rc = 0;
 
-	if (count >= LPROCFS_NRS_WR_MAX_CMD) {
-		rc = -EINVAL;
-		goto out;
-	}
+	if (count >= LPROCFS_NRS_WR_MAX_CMD)
+		return -EINVAL;
 
-	OBD_ALLOC(cmd, LPROCFS_NRS_WR_MAX_CMD);
-	if (cmd == NULL) {
-		rc = -ENOMEM;
-		goto out;
-	}
+	cmd = kzalloc(LPROCFS_NRS_WR_MAX_CMD, GFP_NOFS);
+	if (cmd == NULL)
+		return -ENOMEM;
 	/**
 	 * strsep() modifies its argument, so keep a copy
 	 */
@@ -716,8 +710,7 @@ default_queue:
 
 	mutex_unlock(&nrs_core.nrs_mutex);
 out:
-	if (cmd_copy)
-		OBD_FREE(cmd_copy, LPROCFS_NRS_WR_MAX_CMD);
+	kfree(cmd_copy);
 
 	return rc < 0 ? rc : count;
 }
@@ -736,8 +729,8 @@ ptlrpc_lprocfs_svc_req_history_seek(struct ptlrpc_service_part *svcpt,
 				    struct ptlrpc_srh_iterator *srhi,
 				    __u64 seq)
 {
-	struct list_head		*e;
-	struct ptlrpc_request	*req;
+	struct list_head *e;
+	struct ptlrpc_request *req;
 
 	if (srhi->srhi_req != NULL &&
 	    srhi->srhi_seq > svcpt->scp_hist_seq_culled &&
@@ -825,7 +818,7 @@ ptlrpc_lprocfs_svc_req_history_start(struct seq_file *s, loff_t *pos)
 		return NULL;
 	}
 
-	OBD_ALLOC(srhi, sizeof(*srhi));
+	srhi = kzalloc(sizeof(*srhi), GFP_NOFS);
 	if (srhi == NULL)
 		return NULL;
 
@@ -851,7 +844,7 @@ ptlrpc_lprocfs_svc_req_history_start(struct seq_file *s, loff_t *pos)
 		}
 	}
 
-	OBD_FREE(srhi, sizeof(*srhi));
+	kfree(srhi);
 	return NULL;
 }
 
@@ -860,20 +853,19 @@ ptlrpc_lprocfs_svc_req_history_stop(struct seq_file *s, void *iter)
 {
 	struct ptlrpc_srh_iterator *srhi = iter;
 
-	if (srhi != NULL)
-		OBD_FREE(srhi, sizeof(*srhi));
+	kfree(srhi);
 }
 
 static void *
 ptlrpc_lprocfs_svc_req_history_next(struct seq_file *s,
 				    void *iter, loff_t *pos)
 {
-	struct ptlrpc_service		*svc = s->private;
-	struct ptlrpc_srh_iterator	*srhi = iter;
-	struct ptlrpc_service_part	*svcpt;
-	__u64				seq;
-	int				rc;
-	int				i;
+	struct ptlrpc_service *svc = s->private;
+	struct ptlrpc_srh_iterator *srhi = iter;
+	struct ptlrpc_service_part *svcpt;
+	__u64 seq;
+	int rc;
+	int i;
 
 	for (i = srhi->srhi_idx; i < svc->srv_ncpts; i++) {
 		svcpt = svc->srv_parts[i];
@@ -895,7 +887,7 @@ ptlrpc_lprocfs_svc_req_history_next(struct seq_file *s,
 		}
 	}
 
-	OBD_FREE(srhi, sizeof(*srhi));
+	kfree(srhi);
 	return NULL;
 }
 
@@ -931,11 +923,11 @@ EXPORT_SYMBOL(target_print_req);
 
 static int ptlrpc_lprocfs_svc_req_history_show(struct seq_file *s, void *iter)
 {
-	struct ptlrpc_service		*svc = s->private;
-	struct ptlrpc_srh_iterator	*srhi = iter;
-	struct ptlrpc_service_part	*svcpt;
-	struct ptlrpc_request		*req;
-	int				rc;
+	struct ptlrpc_service *svc = s->private;
+	struct ptlrpc_srh_iterator *srhi = iter;
+	struct ptlrpc_service_part *svcpt;
+	struct ptlrpc_request *req;
+	int rc;
 
 	LASSERT(srhi->srhi_idx < svc->srv_ncpts);
 
@@ -980,28 +972,28 @@ ptlrpc_lprocfs_svc_req_history_open(struct inode *inode, struct file *file)
 		.next  = ptlrpc_lprocfs_svc_req_history_next,
 		.show  = ptlrpc_lprocfs_svc_req_history_show,
 	};
-	struct seq_file       *seqf;
-	int		    rc;
+	struct seq_file *seqf;
+	int rc;
 
 	rc = seq_open(file, &sops);
 	if (rc)
 		return rc;
 
 	seqf = file->private_data;
-	seqf->private = PDE_DATA(inode);
+	seqf->private = inode->i_private;
 	return 0;
 }
 
 /* See also lprocfs_rd_timeouts */
 static int ptlrpc_lprocfs_timeouts_seq_show(struct seq_file *m, void *n)
 {
-	struct ptlrpc_service		*svc = m->private;
-	struct ptlrpc_service_part	*svcpt;
-	struct dhms			ts;
-	time_t				worstt;
-	unsigned int			cur;
-	unsigned int			worst;
-	int				i;
+	struct ptlrpc_service *svc = m->private;
+	struct ptlrpc_service_part *svcpt;
+	struct dhms ts;
+	time_t worstt;
+	unsigned int cur;
+	unsigned int worst;
+	int i;
 
 	if (AT_OFF) {
 		seq_printf(m, "adaptive timeouts off, using obd_timeout %u\n",
@@ -1026,23 +1018,26 @@ static int ptlrpc_lprocfs_timeouts_seq_show(struct seq_file *m, void *n)
 }
 LPROC_SEQ_FOPS_RO(ptlrpc_lprocfs_timeouts);
 
-static int ptlrpc_lprocfs_hp_ratio_seq_show(struct seq_file *m, void *v)
+static ssize_t high_priority_ratio_show(struct kobject *kobj,
+					struct attribute *attr,
+					char *buf)
 {
-	struct ptlrpc_service *svc = m->private;
-	seq_printf(m, "%d", svc->srv_hpreq_ratio);
-	return 0;
+	struct ptlrpc_service *svc = container_of(kobj, struct ptlrpc_service,
+						  srv_kobj);
+	return sprintf(buf, "%d\n", svc->srv_hpreq_ratio);
 }
 
-static ssize_t ptlrpc_lprocfs_hp_ratio_seq_write(struct file *file,
-					     const char __user *buffer,
-					     size_t count,
-					     loff_t *off)
+static ssize_t high_priority_ratio_store(struct kobject *kobj,
+					 struct attribute *attr,
+					 const char *buffer,
+					 size_t count)
 {
-	struct ptlrpc_service *svc = ((struct seq_file *)file->private_data)->private;
-	int	rc;
-	int	val;
+	struct ptlrpc_service *svc = container_of(kobj, struct ptlrpc_service,
+						  srv_kobj);
+	int rc;
+	int val;
 
-	rc = lprocfs_write_helper(buffer, count, &val);
+	rc = kstrtoint(buffer, 10, &val);
 	if (rc < 0)
 		return rc;
 
@@ -1055,30 +1050,62 @@ static ssize_t ptlrpc_lprocfs_hp_ratio_seq_write(struct file *file,
 
 	return count;
 }
-LPROC_SEQ_FOPS(ptlrpc_lprocfs_hp_ratio);
+LUSTRE_RW_ATTR(high_priority_ratio);
+
+static struct attribute *ptlrpc_svc_attrs[] = {
+	&lustre_attr_threads_min.attr,
+	&lustre_attr_threads_started.attr,
+	&lustre_attr_threads_max.attr,
+	&lustre_attr_high_priority_ratio.attr,
+	NULL,
+};
+
+static void ptlrpc_sysfs_svc_release(struct kobject *kobj)
+{
+	struct ptlrpc_service *svc = container_of(kobj, struct ptlrpc_service,
+						  srv_kobj);
+
+	complete(&svc->srv_kobj_unregister);
+}
+
+static struct kobj_type ptlrpc_svc_ktype = {
+	.default_attrs	= ptlrpc_svc_attrs,
+	.sysfs_ops	= &lustre_sysfs_ops,
+	.release	= ptlrpc_sysfs_svc_release,
+};
+
+void ptlrpc_sysfs_unregister_service(struct ptlrpc_service *svc)
+{
+	/* Let's see if we had a chance at initialization first */
+	if (svc->srv_kobj.kset) {
+		kobject_put(&svc->srv_kobj);
+		wait_for_completion(&svc->srv_kobj_unregister);
+	}
+}
+
+int ptlrpc_sysfs_register_service(struct kset *parent,
+				  struct ptlrpc_service *svc)
+{
+	int rc;
+
+	svc->srv_kobj.kset = parent;
+	init_completion(&svc->srv_kobj_unregister);
+	rc = kobject_init_and_add(&svc->srv_kobj, &ptlrpc_svc_ktype, NULL,
+				  "%s", svc->srv_name);
+
+	return rc;
+}
 
-void ptlrpc_lprocfs_register_service(struct proc_dir_entry *entry,
-				     struct ptlrpc_service *svc)
+void ptlrpc_ldebugfs_register_service(struct dentry *entry,
+				      struct ptlrpc_service *svc)
 {
 	struct lprocfs_vars lproc_vars[] = {
-		{.name       = "high_priority_ratio",
-		 .fops	     = &ptlrpc_lprocfs_hp_ratio_fops,
-		 .data       = svc},
 		{.name       = "req_buffer_history_len",
 		 .fops	     = &ptlrpc_lprocfs_req_history_len_fops,
 		 .data       = svc},
 		{.name       = "req_buffer_history_max",
 		 .fops	     = &ptlrpc_lprocfs_req_history_max_fops,
 		 .data       = svc},
-		{.name       = "threads_min",
-		 .fops	     = &ptlrpc_lprocfs_threads_min_fops,
-		 .data       = svc},
-		{.name       = "threads_max",
-		 .fops	     = &ptlrpc_lprocfs_threads_max_fops,
-		 .data       = svc},
-		{.name       = "threads_started",
-		 .fops	     = &ptlrpc_lprocfs_threads_started_fops,
-		 .data       = svc},
 		{.name       = "timeouts",
 		 .fops	     = &ptlrpc_lprocfs_timeouts_fops,
 		 .data       = svc},
@@ -1097,26 +1124,26 @@ void ptlrpc_lprocfs_register_service(struct proc_dir_entry *entry,
 
 	int rc;
 
-	ptlrpc_lprocfs_register(entry, svc->srv_name,
-				"stats", &svc->srv_procroot,
-				&svc->srv_stats);
+	ptlrpc_ldebugfs_register(entry, svc->srv_name,
+				 "stats", &svc->srv_debugfs_entry,
+				 &svc->srv_stats);
 
-	if (svc->srv_procroot == NULL)
+	if (svc->srv_debugfs_entry == NULL)
 		return;
 
-	lprocfs_add_vars(svc->srv_procroot, lproc_vars, NULL);
+	ldebugfs_add_vars(svc->srv_debugfs_entry, lproc_vars, NULL);
 
-	rc = lprocfs_seq_create(svc->srv_procroot, "req_history",
-				0400, &req_history_fops, svc);
+	rc = ldebugfs_seq_create(svc->srv_debugfs_entry, "req_history",
+				 0400, &req_history_fops, svc);
 	if (rc)
 		CWARN("Error adding the req_history file\n");
 }
 
 void ptlrpc_lprocfs_register_obd(struct obd_device *obddev)
 {
-	ptlrpc_lprocfs_register(obddev->obd_proc_entry, NULL, "stats",
-				&obddev->obd_svc_procroot,
-				&obddev->obd_svc_stats);
+	ptlrpc_ldebugfs_register(obddev->obd_debugfs_entry, NULL, "stats",
+				 &obddev->obd_svc_debugfs_entry,
+				 &obddev->obd_svc_stats);
 }
 EXPORT_SYMBOL(ptlrpc_lprocfs_register_obd);
 
@@ -1164,8 +1191,8 @@ EXPORT_SYMBOL(ptlrpc_lprocfs_brw);
 
 void ptlrpc_lprocfs_unregister_service(struct ptlrpc_service *svc)
 {
-	if (svc->srv_procroot != NULL)
-		lprocfs_remove(&svc->srv_procroot);
+	if (svc->srv_debugfs_entry != NULL)
+		ldebugfs_remove(&svc->srv_debugfs_entry);
 
 	if (svc->srv_stats)
 		lprocfs_free_stats(&svc->srv_stats);
@@ -1173,8 +1200,8 @@ void ptlrpc_lprocfs_unregister_service(struct ptlrpc_service *svc)
 
 void ptlrpc_lprocfs_unregister_obd(struct obd_device *obd)
 {
-	if (obd->obd_svc_procroot)
-		lprocfs_remove(&obd->obd_svc_procroot);
+	if (!IS_ERR_OR_NULL(obd->obd_svc_debugfs_entry))
+		ldebugfs_remove(&obd->obd_svc_debugfs_entry);
 
 	if (obd->obd_svc_stats)
 		lprocfs_free_stats(&obd->obd_svc_stats);
@@ -1188,10 +1215,10 @@ int lprocfs_wr_evict_client(struct file *file, const char __user *buffer,
 			    size_t count, loff_t *off)
 {
 	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
-	char	      *kbuf;
-	char	      *tmpbuf;
+	char *kbuf;
+	char *tmpbuf;
 
-	OBD_ALLOC(kbuf, BUFLEN);
+	kbuf = kzalloc(BUFLEN, GFP_NOFS);
 	if (kbuf == NULL)
 		return -ENOMEM;
 
@@ -1209,7 +1236,7 @@ int lprocfs_wr_evict_client(struct file *file, const char __user *buffer,
 	/* Kludge code(deadlock situation): the lprocfs lock has been held
 	 * since the client is evicted by writing client's
 	 * uuid/nid to procfs "evict_client" entry. However,
-	 * obd_export_evict_by_uuid() will call lprocfs_remove() to destroy
+	 * obd_export_evict_by_uuid() will call ldebugfs_remove() to destroy
 	 * the proc entries under the being destroyed export{}, so I have
 	 * to drop the lock at first here.
 	 * - jay, jxiong@clusterfs.com */
@@ -1225,7 +1252,7 @@ int lprocfs_wr_evict_client(struct file *file, const char __user *buffer,
 	class_decref(obd, __func__, current);
 
 out:
-	OBD_FREE(kbuf, BUFLEN);
+	kfree(kbuf);
 	return count;
 }
 EXPORT_SYMBOL(lprocfs_wr_evict_client);
@@ -1237,7 +1264,7 @@ int lprocfs_wr_ping(struct file *file, const char __user *buffer,
 {
 	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
 	struct ptlrpc_request *req;
-	int		    rc;
+	int rc;
 
 	LPROCFS_CLIMP_CHECK(obd);
 	req = ptlrpc_prep_ping(obd->u.cli.cl_import);
@@ -1275,7 +1302,7 @@ int lprocfs_wr_import(struct file *file, const char __user *buffer,
 	if (count > PAGE_CACHE_SIZE - 1 || count <= prefix_len)
 		return -EINVAL;
 
-	OBD_ALLOC(kbuf, count + 1);
+	kbuf = kzalloc(count + 1, GFP_NOFS);
 	if (kbuf == NULL)
 		return -ENOMEM;
 
@@ -1319,7 +1346,7 @@ int lprocfs_wr_import(struct file *file, const char __user *buffer,
 		ptlrpc_recover_import(imp, uuid, 1);
 
 out:
-	OBD_FREE(kbuf, count + 1);
+	kfree(kbuf);
 	return count;
 }
 EXPORT_SYMBOL(lprocfs_wr_import);
@@ -1362,5 +1389,3 @@ int lprocfs_wr_pinger_recov(struct file *file, const char __user *buffer,
 
 }
 EXPORT_SYMBOL(lprocfs_wr_pinger_recov);
-
-#endif /* CONFIG_PROC_FS */
diff --git a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
index 2fa2585584a3..92c746b44462 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
@@ -52,17 +52,17 @@ static int ptl_send_buf(lnet_handle_md_t *mdh, void *base, int len,
 			struct ptlrpc_connection *conn, int portal, __u64 xid,
 			unsigned int offset)
 {
-	int	      rc;
-	lnet_md_t	 md;
+	int rc;
+	lnet_md_t md;
 
 	LASSERT(portal != 0);
 	LASSERT(conn != NULL);
 	CDEBUG(D_INFO, "conn=%p id %s\n", conn, libcfs_id2str(conn->c_peer));
-	md.start     = base;
-	md.length    = len;
+	md.start = base;
+	md.length = len;
 	md.threshold = (ack == LNET_ACK_REQ) ? 2 : 1;
-	md.options   = PTLRPC_MD_OPTIONS;
-	md.user_ptr  = cbid;
+	md.options = PTLRPC_MD_OPTIONS;
+	md.user_ptr = cbid;
 	md.eq_handle = ptlrpc_eq_h;
 
 	if (unlikely(ack == LNET_ACK_REQ &&
@@ -120,8 +120,8 @@ int ptlrpc_register_bulk(struct ptlrpc_request *req)
 	int posted_md;
 	int total_md;
 	__u64 xid;
-	lnet_handle_me_t  me_h;
-	lnet_md_t	 md;
+	lnet_handle_me_t me_h;
+	lnet_md_t md;
 
 	if (OBD_FAIL_CHECK(OBD_FAIL_PTLRPC_BULK_GET_NET))
 		return 0;
@@ -243,9 +243,9 @@ EXPORT_SYMBOL(ptlrpc_register_bulk);
 int ptlrpc_unregister_bulk(struct ptlrpc_request *req, int async)
 {
 	struct ptlrpc_bulk_desc *desc = req->rq_bulk;
-	wait_queue_head_t	     *wq;
-	struct l_wait_info       lwi;
-	int		      rc;
+	wait_queue_head_t *wq;
+	struct l_wait_info lwi;
+	int rc;
 
 	LASSERT(!in_interrupt());     /* might sleep */
 
@@ -301,8 +301,8 @@ EXPORT_SYMBOL(ptlrpc_unregister_bulk);
 
 static void ptlrpc_at_set_reply(struct ptlrpc_request *req, int flags)
 {
-	struct ptlrpc_service_part	*svcpt = req->rq_rqbd->rqbd_svcpt;
-	struct ptlrpc_service		*svc = svcpt->scp_service;
+	struct ptlrpc_service_part *svcpt = req->rq_rqbd->rqbd_svcpt;
+	struct ptlrpc_service *svc = svcpt->scp_service;
 	int service_time = max_t(int, get_seconds() -
 				 req->rq_arrival_time.tv_sec, 1);
 
@@ -353,8 +353,8 @@ static void ptlrpc_at_set_reply(struct ptlrpc_request *req, int flags)
 int ptlrpc_send_reply(struct ptlrpc_request *req, int flags)
 {
 	struct ptlrpc_reply_state *rs = req->rq_reply_state;
-	struct ptlrpc_connection  *conn;
-	int			rc;
+	struct ptlrpc_connection *conn;
+	int rc;
 
 	/* We must already have a reply buffer (only ptlrpc_error() may be
 	 * called without one). The reply generated by sptlrpc layer (e.g.
@@ -491,8 +491,8 @@ int ptl_send_rpc(struct ptlrpc_request *request, int noreply)
 	int rc2;
 	int mpflag = 0;
 	struct ptlrpc_connection *connection;
-	lnet_handle_me_t  reply_me_h;
-	lnet_md_t	 reply_md;
+	lnet_handle_me_t reply_me_h;
+	lnet_md_t reply_md;
 	struct obd_device *obd = request->rq_import->imp_obd;
 
 	if (OBD_FAIL_CHECK(OBD_FAIL_PTLRPC_DROP_RPC))
@@ -594,15 +594,15 @@ int ptl_send_rpc(struct ptlrpc_request *request, int noreply)
 	spin_unlock(&request->rq_lock);
 
 	if (!noreply) {
-		reply_md.start     = request->rq_repbuf;
-		reply_md.length    = request->rq_repbuf_len;
+		reply_md.start = request->rq_repbuf;
+		reply_md.length = request->rq_repbuf_len;
 		/* Allow multiple early replies */
 		reply_md.threshold = LNET_MD_THRESH_INF;
 		/* Manage remote for early replies */
-		reply_md.options   = PTLRPC_MD_OPTIONS | LNET_MD_OP_PUT |
+		reply_md.options = PTLRPC_MD_OPTIONS | LNET_MD_OP_PUT |
 			LNET_MD_MANAGE_REMOTE |
 			LNET_MD_TRUNCATE; /* allow to make EOVERFLOW error */;
-		reply_md.user_ptr  = &request->rq_reply_cbid;
+		reply_md.user_ptr = &request->rq_reply_cbid;
 		reply_md.eq_handle = ptlrpc_eq_h;
 
 		/* We must see the unlink callback to unset rq_reply_unlink,
@@ -682,11 +682,11 @@ EXPORT_SYMBOL(ptl_send_rpc);
  */
 int ptlrpc_register_rqbd(struct ptlrpc_request_buffer_desc *rqbd)
 {
-	struct ptlrpc_service	  *service = rqbd->rqbd_svcpt->scp_service;
-	static lnet_process_id_t  match_id = {LNET_NID_ANY, LNET_PID_ANY};
-	int			  rc;
-	lnet_md_t		 md;
-	lnet_handle_me_t	  me_h;
+	struct ptlrpc_service *service = rqbd->rqbd_svcpt->scp_service;
+	static lnet_process_id_t match_id = {LNET_NID_ANY, LNET_PID_ANY};
+	int rc;
+	lnet_md_t md;
+	lnet_handle_me_t me_h;
 
 	CDEBUG(D_NET, "LNetMEAttach: portal %d\n",
 	       service->srv_req_portal);
@@ -709,12 +709,12 @@ int ptlrpc_register_rqbd(struct ptlrpc_request_buffer_desc *rqbd)
 	LASSERT(rqbd->rqbd_refcount == 0);
 	rqbd->rqbd_refcount = 1;
 
-	md.start     = rqbd->rqbd_buffer;
-	md.length    = service->srv_buf_size;
-	md.max_size  = service->srv_max_req_size;
+	md.start = rqbd->rqbd_buffer;
+	md.length = service->srv_buf_size;
+	md.max_size = service->srv_max_req_size;
 	md.threshold = LNET_MD_THRESH_INF;
-	md.options   = PTLRPC_MD_OPTIONS | LNET_MD_OP_PUT | LNET_MD_MAX_SIZE;
-	md.user_ptr  = &rqbd->rqbd_cbid;
+	md.options = PTLRPC_MD_OPTIONS | LNET_MD_OP_PUT | LNET_MD_MAX_SIZE;
+	md.user_ptr = &rqbd->rqbd_cbid;
 	md.eq_handle = ptlrpc_eq_h;
 
 	rc = LNetMDAttach(me_h, md, LNET_UNLINK, &rqbd->rqbd_md_h);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs.c b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
index 81ad7473242e..9516acadb7a1 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
@@ -188,8 +188,8 @@ static void nrs_policy_stop_primary(struct ptlrpc_nrs *nrs)
  */
 static int nrs_policy_start_locked(struct ptlrpc_nrs_policy *policy)
 {
-	struct ptlrpc_nrs      *nrs = policy->pol_nrs;
-	int			rc = 0;
+	struct ptlrpc_nrs *nrs = policy->pol_nrs;
+	int rc = 0;
 
 	/**
 	 * Don't allow multiple starting which is too complex, and has no real
@@ -377,15 +377,15 @@ static void nrs_resource_put(struct ptlrpc_nrs_resource *res)
  */
 static
 struct ptlrpc_nrs_resource *nrs_resource_get(struct ptlrpc_nrs_policy *policy,
-					      struct ptlrpc_nrs_request *nrq,
-					      bool moving_req)
+					     struct ptlrpc_nrs_request *nrq,
+					     bool moving_req)
 {
 	/**
 	 * Set to NULL to traverse the resource hierarchy from the top.
 	 */
 	struct ptlrpc_nrs_resource *res = NULL;
 	struct ptlrpc_nrs_resource *tmp = NULL;
-	int			    rc;
+	int rc;
 
 	while (1) {
 		rc = policy->pol_desc->pd_ops->op_res_get(policy, nrq, res,
@@ -432,8 +432,8 @@ static void nrs_resource_get_safe(struct ptlrpc_nrs *nrs,
 				  struct ptlrpc_nrs_resource **resp,
 				  bool moving_req)
 {
-	struct ptlrpc_nrs_policy   *primary = NULL;
-	struct ptlrpc_nrs_policy   *fallback = NULL;
+	struct ptlrpc_nrs_policy *primary = NULL;
+	struct ptlrpc_nrs_policy *fallback = NULL;
 
 	memset(resp, 0, sizeof(resp[0]) * NRS_RES_MAX);
 
@@ -478,14 +478,14 @@ static void nrs_resource_get_safe(struct ptlrpc_nrs *nrs,
  *
  * \param resp	the resource hierarchy that is being released
  *
- * \see ptlrpcnrs_req_hp_move()
+ * \see ptlrpc_nrs_req_hp_move()
  * \see ptlrpc_nrs_req_finalize()
  */
 static void nrs_resource_put_safe(struct ptlrpc_nrs_resource **resp)
 {
 	struct ptlrpc_nrs_policy *pols[NRS_RES_MAX];
-	struct ptlrpc_nrs	 *nrs = NULL;
-	int			  i;
+	struct ptlrpc_nrs *nrs = NULL;
+	int i;
 
 	for (i = 0; i < NRS_RES_MAX; i++) {
 		if (resp[i] != NULL) {
@@ -530,7 +530,7 @@ static void nrs_resource_put_safe(struct ptlrpc_nrs_resource **resp)
  */
 static inline
 struct ptlrpc_nrs_request *nrs_request_get(struct ptlrpc_nrs_policy *policy,
-					    bool peek, bool force)
+					   bool peek, bool force)
 {
 	struct ptlrpc_nrs_request *nrq;
 
@@ -556,8 +556,8 @@ struct ptlrpc_nrs_request *nrs_request_get(struct ptlrpc_nrs_policy *policy,
 static inline void nrs_request_enqueue(struct ptlrpc_nrs_request *nrq)
 {
 	struct ptlrpc_nrs_policy *policy;
-	int			  rc;
-	int			  i;
+	int rc;
+	int i;
 
 	/**
 	 * Try in descending order, because the primary policy (if any) is
@@ -628,8 +628,8 @@ static inline void nrs_request_stop(struct ptlrpc_nrs_request *nrq)
 static int nrs_policy_ctl(struct ptlrpc_nrs *nrs, char *name,
 			  enum ptlrpc_nrs_ctl opc, void *arg)
 {
-	struct ptlrpc_nrs_policy       *policy;
-	int				rc = 0;
+	struct ptlrpc_nrs_policy *policy;
+	int rc = 0;
 
 	spin_lock(&nrs->nrs_lock);
 
@@ -715,7 +715,7 @@ static int nrs_policy_unregister(struct ptlrpc_nrs *nrs, char *name)
 	nrs_policy_fini(policy);
 
 	LASSERT(policy->pol_private == NULL);
-	OBD_FREE_PTR(policy);
+	kfree(policy);
 
 	return 0;
 }
@@ -733,10 +733,10 @@ static int nrs_policy_unregister(struct ptlrpc_nrs *nrs, char *name)
 static int nrs_policy_register(struct ptlrpc_nrs *nrs,
 			       struct ptlrpc_nrs_pol_desc *desc)
 {
-	struct ptlrpc_nrs_policy       *policy;
-	struct ptlrpc_nrs_policy       *tmp;
-	struct ptlrpc_service_part     *svcpt = nrs->nrs_svcpt;
-	int				rc;
+	struct ptlrpc_nrs_policy *policy;
+	struct ptlrpc_nrs_policy *tmp;
+	struct ptlrpc_service_part *svcpt = nrs->nrs_svcpt;
+	int rc;
 
 	LASSERT(svcpt != NULL);
 	LASSERT(desc->pd_ops != NULL);
@@ -746,22 +746,23 @@ static int nrs_policy_register(struct ptlrpc_nrs *nrs,
 	LASSERT(desc->pd_ops->op_req_dequeue != NULL);
 	LASSERT(desc->pd_compat != NULL);
 
-	OBD_CPT_ALLOC_GFP(policy, svcpt->scp_service->srv_cptable,
-			  svcpt->scp_cpt, sizeof(*policy), GFP_NOFS);
+	policy = kzalloc_node(sizeof(*policy), GFP_NOFS,
+			cfs_cpt_spread_node(svcpt->scp_service->srv_cptable,
+					    svcpt->scp_cpt));
 	if (policy == NULL)
 		return -ENOMEM;
 
-	policy->pol_nrs     = nrs;
-	policy->pol_desc    = desc;
-	policy->pol_state   = NRS_POL_STATE_STOPPED;
-	policy->pol_flags   = desc->pd_flags;
+	policy->pol_nrs = nrs;
+	policy->pol_desc = desc;
+	policy->pol_state = NRS_POL_STATE_STOPPED;
+	policy->pol_flags = desc->pd_flags;
 
 	INIT_LIST_HEAD(&policy->pol_list);
 	INIT_LIST_HEAD(&policy->pol_list_queued);
 
 	rc = nrs_policy_init(policy);
 	if (rc != 0) {
-		OBD_FREE_PTR(policy);
+		kfree(policy);
 		return rc;
 	}
 
@@ -776,7 +777,7 @@ static int nrs_policy_register(struct ptlrpc_nrs *nrs,
 
 		spin_unlock(&nrs->nrs_lock);
 		nrs_policy_fini(policy);
-		OBD_FREE_PTR(policy);
+		kfree(policy);
 
 		return -EEXIST;
 	}
@@ -803,7 +804,7 @@ static int nrs_policy_register(struct ptlrpc_nrs *nrs,
  */
 static void ptlrpc_nrs_req_add_nolock(struct ptlrpc_request *req)
 {
-	struct ptlrpc_nrs_policy       *policy;
+	struct ptlrpc_nrs_policy *policy;
 
 	LASSERT(req->rq_nrq.nr_initialized);
 	LASSERT(!req->rq_nrq.nr_enqueued);
@@ -828,7 +829,7 @@ static void ptlrpc_nrs_req_add_nolock(struct ptlrpc_request *req)
  */
 static void ptlrpc_nrs_hpreq_add_nolock(struct ptlrpc_request *req)
 {
-	int	opc = lustre_msg_get_opc(req->rq_reqmsg);
+	int opc = lustre_msg_get_opc(req->rq_reqmsg);
 
 	spin_lock(&req->rq_lock);
 	req->rq_hp = 1;
@@ -871,9 +872,9 @@ static int nrs_register_policies_locked(struct ptlrpc_nrs *nrs)
 {
 	struct ptlrpc_nrs_pol_desc *desc;
 	/* for convenience */
-	struct ptlrpc_service_part	 *svcpt = nrs->nrs_svcpt;
-	struct ptlrpc_service		 *svc = svcpt->scp_service;
-	int				  rc = -EINVAL;
+	struct ptlrpc_service_part *svcpt = nrs->nrs_svcpt;
+	struct ptlrpc_service *svc = svcpt->scp_service;
+	int rc = -EINVAL;
 
 	LASSERT(mutex_is_locked(&nrs_core.nrs_mutex));
 
@@ -911,7 +912,7 @@ static int nrs_register_policies_locked(struct ptlrpc_nrs *nrs)
 static int nrs_svcpt_setup_locked0(struct ptlrpc_nrs *nrs,
 				   struct ptlrpc_service_part *svcpt)
 {
-	enum ptlrpc_nrs_queue_type	queue;
+	enum ptlrpc_nrs_queue_type queue;
 
 	LASSERT(mutex_is_locked(&nrs_core.nrs_mutex));
 
@@ -942,8 +943,8 @@ static int nrs_svcpt_setup_locked0(struct ptlrpc_nrs *nrs,
  */
 static int nrs_svcpt_setup_locked(struct ptlrpc_service_part *svcpt)
 {
-	struct ptlrpc_nrs	       *nrs;
-	int				rc;
+	struct ptlrpc_nrs *nrs;
+	int rc;
 
 	LASSERT(mutex_is_locked(&nrs_core.nrs_mutex));
 
@@ -961,9 +962,10 @@ static int nrs_svcpt_setup_locked(struct ptlrpc_service_part *svcpt)
 	if (svcpt->scp_service->srv_ops.so_hpreq_handler == NULL)
 		goto out;
 
-	OBD_CPT_ALLOC_PTR(svcpt->scp_nrs_hp,
-			  svcpt->scp_service->srv_cptable,
-			  svcpt->scp_cpt);
+	svcpt->scp_nrs_hp =
+		kzalloc_node(sizeof(*svcpt->scp_nrs_hp), GFP_NOFS,
+			cfs_cpt_spread_node(svcpt->scp_service->srv_cptable,
+					    svcpt->scp_cpt));
 	if (svcpt->scp_nrs_hp == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -986,11 +988,11 @@ out:
  */
 static void nrs_svcpt_cleanup_locked(struct ptlrpc_service_part *svcpt)
 {
-	struct ptlrpc_nrs	       *nrs;
-	struct ptlrpc_nrs_policy       *policy;
-	struct ptlrpc_nrs_policy       *tmp;
-	int				rc;
-	bool				hp = false;
+	struct ptlrpc_nrs *nrs;
+	struct ptlrpc_nrs_policy *policy;
+	struct ptlrpc_nrs_policy *tmp;
+	int rc;
+	bool hp = false;
 
 	LASSERT(mutex_is_locked(&nrs_core.nrs_mutex));
 
@@ -1013,7 +1015,7 @@ again:
 	}
 
 	if (hp)
-		OBD_FREE_PTR(nrs);
+		kfree(nrs);
 }
 
 /**
@@ -1026,7 +1028,7 @@ again:
  */
 static struct ptlrpc_nrs_pol_desc *nrs_policy_find_desc_locked(const char *name)
 {
-	struct ptlrpc_nrs_pol_desc     *tmp;
+	struct ptlrpc_nrs_pol_desc *tmp;
 
 	list_for_each_entry(tmp, &nrs_core.nrs_policies, pd_list) {
 		if (strncmp(tmp->pd_name, name, NRS_POL_NAME_MAX) == 0)
@@ -1049,11 +1051,11 @@ static struct ptlrpc_nrs_pol_desc *nrs_policy_find_desc_locked(const char *name)
  */
 static int nrs_policy_unregister_locked(struct ptlrpc_nrs_pol_desc *desc)
 {
-	struct ptlrpc_nrs	       *nrs;
-	struct ptlrpc_service	       *svc;
-	struct ptlrpc_service_part     *svcpt;
-	int				i;
-	int				rc = 0;
+	struct ptlrpc_nrs *nrs;
+	struct ptlrpc_service *svc;
+	struct ptlrpc_service_part *svcpt;
+	int i;
+	int rc = 0;
 
 	LASSERT(mutex_is_locked(&nrs_core.nrs_mutex));
 	LASSERT(mutex_is_locked(&ptlrpc_all_services_mutex));
@@ -1113,9 +1115,9 @@ again:
  */
 int ptlrpc_nrs_policy_register(struct ptlrpc_nrs_pol_conf *conf)
 {
-	struct ptlrpc_service	       *svc;
-	struct ptlrpc_nrs_pol_desc     *desc;
-	int				rc = 0;
+	struct ptlrpc_service *svc;
+	struct ptlrpc_nrs_pol_desc *desc;
+	int rc = 0;
 
 	LASSERT(conf != NULL);
 	LASSERT(conf->nc_ops != NULL);
@@ -1153,19 +1155,19 @@ int ptlrpc_nrs_policy_register(struct ptlrpc_nrs_pol_conf *conf)
 		goto fail;
 	}
 
-	OBD_ALLOC_PTR(desc);
+	desc = kzalloc(sizeof(*desc), GFP_NOFS);
 	if (desc == NULL) {
 		rc = -ENOMEM;
 		goto fail;
 	}
 
 	strncpy(desc->pd_name, conf->nc_name, NRS_POL_NAME_MAX);
-	desc->pd_ops		 = conf->nc_ops;
-	desc->pd_compat		 = conf->nc_compat;
+	desc->pd_ops = conf->nc_ops;
+	desc->pd_compat = conf->nc_compat;
 	desc->pd_compat_svc_name = conf->nc_compat_svc_name;
 	if ((conf->nc_flags & PTLRPC_NRS_FL_REG_EXTERN) != 0)
-		desc->pd_owner	 = conf->nc_owner;
-	desc->pd_flags		 = conf->nc_flags;
+		desc->pd_owner = conf->nc_owner;
+	desc->pd_flags = conf->nc_flags;
 	atomic_set(&desc->pd_refs, 0);
 
 	/**
@@ -1185,17 +1187,17 @@ int ptlrpc_nrs_policy_register(struct ptlrpc_nrs_pol_conf *conf)
 	mutex_lock(&ptlrpc_all_services_mutex);
 
 	list_for_each_entry(svc, &ptlrpc_all_services, srv_list) {
-		struct ptlrpc_service_part     *svcpt;
-		int				i;
-		int				rc2;
+		struct ptlrpc_service_part *svcpt;
+		int i;
+		int rc2;
 
 		if (!nrs_policy_compatible(svc, desc) ||
 		    unlikely(svc->srv_is_stopping))
 			continue;
 
 		ptlrpc_service_for_each_part(svcpt, i, svc) {
-			struct ptlrpc_nrs      *nrs;
-			bool			hp = false;
+			struct ptlrpc_nrs *nrs;
+			bool hp = false;
 again:
 			nrs = nrs_svcpt2nrs(svcpt, hp);
 			rc = nrs_policy_register(nrs, desc);
@@ -1210,7 +1212,7 @@ again:
 				 */
 				LASSERT(rc2 == 0);
 				mutex_unlock(&ptlrpc_all_services_mutex);
-				OBD_FREE_PTR(desc);
+				kfree(desc);
 				goto fail;
 			}
 
@@ -1233,7 +1235,7 @@ again:
 				 */
 				LASSERT(rc2 == 0);
 				mutex_unlock(&ptlrpc_all_services_mutex);
-				OBD_FREE_PTR(desc);
+				kfree(desc);
 				goto fail;
 			}
 		}
@@ -1265,8 +1267,8 @@ EXPORT_SYMBOL(ptlrpc_nrs_policy_register);
  */
 int ptlrpc_nrs_policy_unregister(struct ptlrpc_nrs_pol_conf *conf)
 {
-	struct ptlrpc_nrs_pol_desc	*desc;
-	int				 rc;
+	struct ptlrpc_nrs_pol_desc *desc;
+	int rc;
 
 	LASSERT(conf != NULL);
 
@@ -1301,7 +1303,7 @@ int ptlrpc_nrs_policy_unregister(struct ptlrpc_nrs_pol_conf *conf)
 	       conf->nc_name);
 
 	list_del(&desc->pd_list);
-	OBD_FREE_PTR(desc);
+	kfree(desc);
 
 fail:
 	mutex_unlock(&ptlrpc_all_services_mutex);
@@ -1329,10 +1331,10 @@ EXPORT_SYMBOL(ptlrpc_nrs_policy_unregister);
  */
 int ptlrpc_service_nrs_setup(struct ptlrpc_service *svc)
 {
-	struct ptlrpc_service_part	       *svcpt;
-	const struct ptlrpc_nrs_pol_desc       *desc;
-	int					i;
-	int					rc = 0;
+	struct ptlrpc_service_part *svcpt;
+	const struct ptlrpc_nrs_pol_desc *desc;
+	int i;
+	int rc = 0;
 
 	mutex_lock(&nrs_core.nrs_mutex);
 
@@ -1374,9 +1376,9 @@ failed:
  */
 void ptlrpc_service_nrs_cleanup(struct ptlrpc_service *svc)
 {
-	struct ptlrpc_service_part	     *svcpt;
-	const struct ptlrpc_nrs_pol_desc     *desc;
-	int				      i;
+	struct ptlrpc_service_part *svcpt;
+	const struct ptlrpc_nrs_pol_desc *desc;
+	int i;
 
 	mutex_lock(&nrs_core.nrs_mutex);
 
@@ -1415,7 +1417,7 @@ void ptlrpc_service_nrs_cleanup(struct ptlrpc_service *svc)
 void ptlrpc_nrs_req_initialize(struct ptlrpc_service_part *svcpt,
 			       struct ptlrpc_request *req, bool hp)
 {
-	struct ptlrpc_nrs	*nrs = nrs_svcpt2nrs(svcpt, hp);
+	struct ptlrpc_nrs *nrs = nrs_svcpt2nrs(svcpt, hp);
 
 	memset(&req->rq_nrq, 0, sizeof(req->rq_nrq));
 	nrs_resource_get_safe(nrs, &req->rq_nrq, req->rq_nrq.nr_res_ptrs,
@@ -1523,8 +1525,8 @@ struct ptlrpc_request *
 ptlrpc_nrs_req_get_nolock0(struct ptlrpc_service_part *svcpt, bool hp,
 			   bool peek, bool force)
 {
-	struct ptlrpc_nrs	  *nrs = nrs_svcpt2nrs(svcpt, hp);
-	struct ptlrpc_nrs_policy  *policy;
+	struct ptlrpc_nrs *nrs = nrs_svcpt2nrs(svcpt, hp);
+	struct ptlrpc_nrs_policy *policy;
 	struct ptlrpc_nrs_request *nrq;
 
 	/**
@@ -1594,10 +1596,10 @@ bool ptlrpc_nrs_req_pending_nolock(struct ptlrpc_service_part *svcpt, bool hp)
  */
 void ptlrpc_nrs_req_hp_move(struct ptlrpc_request *req)
 {
-	struct ptlrpc_service_part	*svcpt = req->rq_rqbd->rqbd_svcpt;
-	struct ptlrpc_nrs_request	*nrq = &req->rq_nrq;
-	struct ptlrpc_nrs_resource	*res1[NRS_RES_MAX];
-	struct ptlrpc_nrs_resource	*res2[NRS_RES_MAX];
+	struct ptlrpc_service_part *svcpt = req->rq_rqbd->rqbd_svcpt;
+	struct ptlrpc_nrs_request *nrq = &req->rq_nrq;
+	struct ptlrpc_nrs_resource *res1[NRS_RES_MAX];
+	struct ptlrpc_nrs_resource *res2[NRS_RES_MAX];
 
 	/**
 	 * Obtain the high-priority NRS head resources.
@@ -1659,9 +1661,9 @@ int ptlrpc_nrs_policy_control(const struct ptlrpc_service *svc,
 			      enum ptlrpc_nrs_queue_type queue, char *name,
 			      enum ptlrpc_nrs_ctl opc, bool single, void *arg)
 {
-	struct ptlrpc_service_part     *svcpt;
-	int				i;
-	int				rc = 0;
+	struct ptlrpc_service_part *svcpt;
+	int i;
+	int rc = 0;
 
 	LASSERT(opc != PTLRPC_NRS_CTL_INVALID);
 
@@ -1709,7 +1711,7 @@ extern struct ptlrpc_nrs_pol_conf nrs_conf_fifo;
  */
 int ptlrpc_nrs_init(void)
 {
-	int	rc;
+	int rc;
 
 	mutex_init(&nrs_core.nrs_mutex);
 	INIT_LIST_HEAD(&nrs_core.nrs_policies);
@@ -1747,7 +1749,7 @@ void ptlrpc_nrs_fini(void)
 	list_for_each_entry_safe(desc, tmp, &nrs_core.nrs_policies,
 				     pd_list) {
 		list_del_init(&desc->pd_list);
-		OBD_FREE_PTR(desc);
+		kfree(desc);
 	}
 }
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs_fifo.c b/drivers/staging/lustre/lustre/ptlrpc/nrs_fifo.c
index eb40c01db612..8e21f0cdc8f8 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs_fifo.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/nrs_fifo.c
@@ -80,7 +80,9 @@ static int nrs_fifo_start(struct ptlrpc_nrs_policy *policy)
 {
 	struct nrs_fifo_head *head;
 
-	OBD_CPT_ALLOC_PTR(head, nrs_pol2cptab(policy), nrs_pol2cptid(policy));
+	head = kzalloc_node(sizeof(*head), GFP_NOFS,
+			    cfs_cpt_spread_node(nrs_pol2cptab(policy),
+						nrs_pol2cptid(policy)));
 	if (head == NULL)
 		return -ENOMEM;
 
@@ -105,7 +107,7 @@ static void nrs_fifo_stop(struct ptlrpc_nrs_policy *policy)
 	LASSERT(head != NULL);
 	LASSERT(list_empty(&head->fh_list));
 
-	OBD_FREE_PTR(head);
+	kfree(head);
 }
 
 /**
@@ -158,9 +160,9 @@ static int nrs_fifo_res_get(struct ptlrpc_nrs_policy *policy,
  */
 static
 struct ptlrpc_nrs_request *nrs_fifo_req_get(struct ptlrpc_nrs_policy *policy,
-					     bool peek, bool force)
+					    bool peek, bool force)
 {
-	struct nrs_fifo_head	  *head = policy->pol_private;
+	struct nrs_fifo_head *head = policy->pol_private;
 	struct ptlrpc_nrs_request *nrq;
 
 	nrq = unlikely(list_empty(&head->fh_list)) ? NULL :
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
index b51af9bf37b7..2787bfd67165 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
@@ -279,8 +279,8 @@ lustre_get_emerg_rs(struct ptlrpc_service_part *svcpt)
 
 	/* See if we have anything in a pool, and wait if nothing */
 	while (list_empty(&svcpt->scp_rep_idle)) {
-		struct l_wait_info	lwi;
-		int			rc;
+		struct l_wait_info lwi;
+		int rc;
 
 		spin_unlock(&svcpt->scp_rep_lock);
 		/* If we cannot get anything for some long time, we better
@@ -321,7 +321,7 @@ int lustre_pack_reply_v2(struct ptlrpc_request *req, int count,
 			 __u32 *lens, char **bufs, int flags)
 {
 	struct ptlrpc_reply_state *rs;
-	int			msg_len, rc;
+	int msg_len, rc;
 
 	LASSERT(req->rq_reply_state == NULL);
 
@@ -440,8 +440,8 @@ EXPORT_SYMBOL(lustre_msg_buf);
 int lustre_shrink_msg_v2(struct lustre_msg_v2 *msg, int segment,
 			 unsigned int newlen, int move_data)
 {
-	char   *tail = NULL, *newpos;
-	int     tail_len = 0, n;
+	char *tail = NULL, *newpos;
+	int tail_len = 0, n;
 
 	LASSERT(msg);
 	LASSERT(msg->lm_bufcount > segment);
@@ -1577,8 +1577,8 @@ int do_set_info_async(struct obd_import *imp,
 		      struct ptlrpc_request_set *set)
 {
 	struct ptlrpc_request *req;
-	char		  *tmp;
-	int		    rc;
+	char *tmp;
+	int rc;
 
 	req = ptlrpc_request_alloc(imp, &RQF_OBD_SET_INFO);
 	if (req == NULL)
@@ -1688,7 +1688,7 @@ void lustre_swab_connect(struct obd_connect_data *ocd)
 	CLASSERT(offsetof(typeof(*ocd), paddingF) != 0);
 }
 
-void lustre_swab_obdo(struct obdo  *o)
+void lustre_swab_obdo(struct obdo *o)
 {
 	__swab64s(&o->o_valid);
 	lustre_swab_ost_id(&o->o_oi);
@@ -2179,7 +2179,7 @@ EXPORT_SYMBOL(lustre_swab_lov_user_md_objects);
 
 void lustre_swab_ldlm_res_id(struct ldlm_res_id *id)
 {
-	int  i;
+	int i;
 
 	for (i = 0; i < RES_NAME_SIZE; i++)
 		__swab64s(&id->name[i]);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pinger.c b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
index 9dbda9332dd8..d05c37c1fd30 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pinger.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
@@ -91,7 +91,7 @@ int ptlrpc_obd_ping(struct obd_device *obd)
 }
 EXPORT_SYMBOL(ptlrpc_obd_ping);
 
-int ptlrpc_ping(struct obd_import *imp)
+static int ptlrpc_ping(struct obd_import *imp)
 {
 	struct ptlrpc_request *req;
 
@@ -110,7 +110,7 @@ int ptlrpc_ping(struct obd_import *imp)
 	return 0;
 }
 
-void ptlrpc_update_next_ping(struct obd_import *imp, int soon)
+static void ptlrpc_update_next_ping(struct obd_import *imp, int soon)
 {
 	int time = soon ? PING_INTERVAL_SHORT : PING_INTERVAL;
 	if (imp->imp_state == LUSTRE_IMP_DISCON) {
@@ -141,7 +141,7 @@ static inline int ptlrpc_next_reconnect(struct obd_import *imp)
 		return cfs_time_shift(obd_timeout);
 }
 
-long pinger_check_timeout(unsigned long time)
+static long pinger_check_timeout(unsigned long time)
 {
 	struct timeout_item *item;
 	unsigned long timeout = PING_INTERVAL;
@@ -289,12 +289,10 @@ static int ptlrpc_pinger_main(void *arg)
 				     thread_is_stopping(thread) ||
 				     thread_is_event(thread),
 				     &lwi);
-			if (thread_test_and_clear_flags(thread, SVC_STOPPING)) {
+			if (thread_test_and_clear_flags(thread, SVC_STOPPING))
 				break;
-			} else {
-				/* woken after adding import to reset timer */
-				thread_test_and_clear_flags(thread, SVC_EVENT);
-			}
+			/* woken after adding import to reset timer */
+			thread_test_and_clear_flags(thread, SVC_EVENT);
 		}
 	}
 
@@ -422,12 +420,12 @@ 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,
-					timeout_cb_t cb, void *data)
+static struct timeout_item *ptlrpc_new_timeout(int time,
+	enum timeout_event event, timeout_cb_t cb, void *data)
 {
 	struct timeout_item *ti;
 
-	OBD_ALLOC_PTR(ti);
+	ti = kzalloc(sizeof(*ti), GFP_NOFS);
 	if (!ti)
 		return NULL;
 
@@ -514,7 +512,7 @@ int ptlrpc_del_timeout_client(struct list_head *obd_list,
 	LASSERTF(ti != NULL, "ti is NULL !\n");
 	if (list_empty(&ti->ti_obd_list)) {
 		list_del(&ti->ti_chain);
-		OBD_FREE_PTR(ti);
+		kfree(ti);
 	}
 	mutex_unlock(&pinger_mutex);
 	return 0;
@@ -529,7 +527,7 @@ int ptlrpc_pinger_remove_timeouts(void)
 	list_for_each_entry_safe(item, tmp, &timeout_list, ti_chain) {
 		LASSERT(list_empty(&item->ti_obd_list));
 		list_del(&item->ti_chain);
-		OBD_FREE_PTR(item);
+		kfree(item);
 	}
 	mutex_unlock(&pinger_mutex);
 	return 0;
@@ -546,9 +544,9 @@ void ptlrpc_pinger_wake_up(void)
 #define PET_TERMINATE 2
 
 static int pet_refcount;
-static int	       pet_state;
-static wait_queue_head_t       pet_waitq;
-LIST_HEAD(pet_list);
+static int pet_state;
+static wait_queue_head_t pet_waitq;
+static LIST_HEAD(pet_list);
 static DEFINE_SPINLOCK(pet_lock);
 
 int ping_evictor_wake(struct obd_export *exp)
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
index a66dc3c6da41..6dc3998dcd24 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
@@ -76,19 +76,16 @@ void ptlrpc_initiate_recovery(struct obd_import *imp);
 int lustre_unpack_req_ptlrpc_body(struct ptlrpc_request *req, int offset);
 int lustre_unpack_rep_ptlrpc_body(struct ptlrpc_request *req, int offset);
 
-#if defined(CONFIG_PROC_FS)
-void ptlrpc_lprocfs_register_service(struct proc_dir_entry *proc_entry,
-				     struct ptlrpc_service *svc);
+int ptlrpc_sysfs_register_service(struct kset *parent,
+				  struct ptlrpc_service *svc);
+void ptlrpc_sysfs_unregister_service(struct ptlrpc_service *svc);
+
+void ptlrpc_ldebugfs_register_service(struct dentry *debugfs_entry,
+				      struct ptlrpc_service *svc);
 void ptlrpc_lprocfs_unregister_service(struct ptlrpc_service *svc);
 void ptlrpc_lprocfs_rpc_sent(struct ptlrpc_request *req, long amount);
 void ptlrpc_lprocfs_do_request_stat(struct ptlrpc_request *req,
 				     long q_usec, long work_usec);
-#else
-#define ptlrpc_lprocfs_register_service(params...) do {} while (0)
-#define ptlrpc_lprocfs_unregister_service(params...) do {} while (0)
-#define ptlrpc_lprocfs_rpc_sent(params...) do {} while (0)
-#define ptlrpc_lprocfs_do_request_stat(params...) do {} while (0)
-#endif /* CONFIG_PROC_FS */
 
 /* NRS */
 
@@ -263,14 +260,8 @@ void sptlrpc_enc_pool_fini(void);
 int sptlrpc_proc_enc_pool_seq_show(struct seq_file *m, void *v);
 
 /* sec_lproc.c */
-#if defined(CONFIG_PROC_FS)
 int  sptlrpc_lproc_init(void);
 void sptlrpc_lproc_fini(void);
-#else
-static inline int sptlrpc_lproc_init(void)
-{ return 0; }
-static inline void sptlrpc_lproc_fini(void) {}
-#endif
 
 /* sec_gc.c */
 int sptlrpc_gc_init(void);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
index 0c178ec0e487..e591cff323ec 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
@@ -68,9 +68,9 @@
 #include "ptlrpc_internal.h"
 
 struct ptlrpcd {
-	int		pd_size;
-	int		pd_index;
-	int		pd_nthreads;
+	int pd_size;
+	int pd_index;
+	int pd_nthreads;
 	struct ptlrpcd_ctl pd_thread_rcv;
 	struct ptlrpcd_ctl pd_threads[0];
 };
@@ -528,8 +528,9 @@ static int ptlrpcd_bind(int index, int max)
 	}
 
 	if (rc == 0 && pc->pc_npartners > 0) {
-		OBD_ALLOC(pc->pc_partners,
-			  sizeof(struct ptlrpcd_ctl *) * pc->pc_npartners);
+		pc->pc_partners = kcalloc(pc->pc_npartners,
+					  sizeof(struct ptlrpcd_ctl *),
+					  GFP_NOFS);
 		if (pc->pc_partners == NULL) {
 			pc->pc_npartners = 0;
 			rc = -ENOMEM;
@@ -699,8 +700,7 @@ out:
 	if (pc->pc_npartners > 0) {
 		LASSERT(pc->pc_partners != NULL);
 
-		OBD_FREE(pc->pc_partners,
-			 sizeof(struct ptlrpcd_ctl *) * pc->pc_npartners);
+		kfree(pc->pc_partners);
 		pc->pc_partners = NULL;
 	}
 	pc->pc_npartners = 0;
@@ -717,7 +717,7 @@ static void ptlrpcd_fini(void)
 			ptlrpcd_free(&ptlrpcds->pd_threads[i]);
 		ptlrpcd_stop(&ptlrpcds->pd_thread_rcv, 0);
 		ptlrpcd_free(&ptlrpcds->pd_thread_rcv);
-		OBD_FREE(ptlrpcds, ptlrpcds->pd_size);
+		kfree(ptlrpcds);
 		ptlrpcds = NULL;
 	}
 }
@@ -738,7 +738,7 @@ static int ptlrpcd_init(void)
 		nthreads &= ~1; /* make sure it is even */
 
 	size = offsetof(struct ptlrpcd, pd_threads[nthreads]);
-	OBD_ALLOC(ptlrpcds, size);
+	ptlrpcds = kzalloc(size, GFP_NOFS);
 	if (ptlrpcds == NULL) {
 		rc = -ENOMEM;
 		goto out;
@@ -781,7 +781,7 @@ out:
 			ptlrpcd_free(&ptlrpcds->pd_threads[j]);
 		ptlrpcd_stop(&ptlrpcds->pd_thread_rcv, 0);
 		ptlrpcd_free(&ptlrpcds->pd_thread_rcv);
-		OBD_FREE(ptlrpcds, size);
+		kfree(ptlrpcds);
 		ptlrpcds = NULL;
 	}
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec.c b/drivers/staging/lustre/lustre/ptlrpc/sec.c
index 21e9dc9d5580..b9821db22904 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec.c
@@ -113,10 +113,10 @@ static
 struct ptlrpc_sec_policy *sptlrpc_wireflavor2policy(__u32 flavor)
 {
 	static DEFINE_MUTEX(load_mutex);
-	static atomic_t       loaded = ATOMIC_INIT(0);
+	static atomic_t loaded = ATOMIC_INIT(0);
 	struct ptlrpc_sec_policy *policy;
-	__u16		     number = SPTLRPC_FLVR_POLICY(flavor);
-	__u16		     flag = 0;
+	__u16 number = SPTLRPC_FLVR_POLICY(flavor);
+	__u16 flag = 0;
 
 	if (number >= SPTLRPC_POLICY_MAX)
 		return NULL;
@@ -339,7 +339,7 @@ int sptlrpc_cli_ctx_display(struct ptlrpc_cli_ctx *ctx, char *buf, int bufsize)
 
 static int import_sec_check_expire(struct obd_import *imp)
 {
-	int     adapt = 0;
+	int adapt = 0;
 
 	spin_lock(&imp->imp_lock);
 	if (imp->imp_sec_expire &&
@@ -359,7 +359,7 @@ static int import_sec_check_expire(struct obd_import *imp)
 static int import_sec_validate_get(struct obd_import *imp,
 				   struct ptlrpc_sec **sec)
 {
-	int     rc;
+	int rc;
 
 	if (unlikely(imp->imp_sec_expire)) {
 		rc = import_sec_check_expire(imp);
@@ -447,10 +447,10 @@ int sptlrpc_req_ctx_switch(struct ptlrpc_request *req,
 			   struct ptlrpc_cli_ctx *oldctx,
 			   struct ptlrpc_cli_ctx *newctx)
 {
-	struct sptlrpc_flavor   old_flvr;
-	char		   *reqmsg = NULL; /* to workaround old gcc */
-	int		     reqmsg_size;
-	int		     rc = 0;
+	struct sptlrpc_flavor old_flvr;
+	char *reqmsg = NULL; /* to workaround old gcc */
+	int reqmsg_size;
+	int rc = 0;
 
 	LASSERT(req->rq_reqmsg);
 	LASSERT(req->rq_reqlen);
@@ -469,7 +469,7 @@ int sptlrpc_req_ctx_switch(struct ptlrpc_request *req,
 	/* save request message */
 	reqmsg_size = req->rq_reqlen;
 	if (reqmsg_size != 0) {
-		OBD_ALLOC_LARGE(reqmsg, reqmsg_size);
+		reqmsg = libcfs_kvzalloc(reqmsg_size, GFP_NOFS);
 		if (reqmsg == NULL)
 			return -ENOMEM;
 		memcpy(reqmsg, req->rq_reqmsg, reqmsg_size);
@@ -497,7 +497,7 @@ int sptlrpc_req_ctx_switch(struct ptlrpc_request *req,
 			req->rq_flvr = old_flvr;
 		}
 
-		OBD_FREE_LARGE(reqmsg, reqmsg_size);
+		kvfree(reqmsg);
 	}
 	return rc;
 }
@@ -514,7 +514,7 @@ int sptlrpc_req_replace_dead_ctx(struct ptlrpc_request *req)
 {
 	struct ptlrpc_cli_ctx *oldctx = req->rq_cli_ctx;
 	struct ptlrpc_cli_ctx *newctx;
-	int		    rc;
+	int rc;
 
 	LASSERT(oldctx);
 
@@ -629,10 +629,10 @@ void req_off_ctx_list(struct ptlrpc_request *req, struct ptlrpc_cli_ctx *ctx)
  */
 int sptlrpc_req_refresh_ctx(struct ptlrpc_request *req, long timeout)
 {
-	struct ptlrpc_cli_ctx  *ctx = req->rq_cli_ctx;
-	struct ptlrpc_sec      *sec;
-	struct l_wait_info      lwi;
-	int		     rc;
+	struct ptlrpc_cli_ctx *ctx = req->rq_cli_ctx;
+	struct ptlrpc_sec *sec;
+	struct l_wait_info lwi;
+	int rc;
 
 	LASSERT(ctx);
 
@@ -866,7 +866,7 @@ void sptlrpc_request_out_callback(struct ptlrpc_request *req)
 	if (req->rq_pool || !req->rq_reqbuf)
 		return;
 
-	OBD_FREE(req->rq_reqbuf, req->rq_reqbuf_len);
+	kfree(req->rq_reqbuf);
 	req->rq_reqbuf = NULL;
 	req->rq_reqbuf_len = 0;
 }
@@ -878,7 +878,7 @@ void sptlrpc_request_out_callback(struct ptlrpc_request *req)
  */
 int sptlrpc_import_check_ctx(struct obd_import *imp)
 {
-	struct ptlrpc_sec     *sec;
+	struct ptlrpc_sec *sec;
 	struct ptlrpc_cli_ctx *ctx;
 	struct ptlrpc_request *req = NULL;
 	int rc;
@@ -974,7 +974,7 @@ int sptlrpc_cli_wrap_request(struct ptlrpc_request *req)
 static int do_cli_unwrap_reply(struct ptlrpc_request *req)
 {
 	struct ptlrpc_cli_ctx *ctx = req->rq_cli_ctx;
-	int		    rc;
+	int rc;
 
 	LASSERT(ctx);
 	LASSERT(ctx->cc_sec);
@@ -1082,10 +1082,10 @@ int sptlrpc_cli_unwrap_reply(struct ptlrpc_request *req)
 int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 				   struct ptlrpc_request **req_ret)
 {
-	struct ptlrpc_request  *early_req;
-	char		   *early_buf;
-	int		     early_bufsz, early_size;
-	int		     rc;
+	struct ptlrpc_request *early_req;
+	char *early_buf;
+	int early_bufsz, early_size;
+	int rc;
 
 	early_req = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (early_req == NULL)
@@ -1093,7 +1093,7 @@ int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 
 	early_size = req->rq_nob_received;
 	early_bufsz = size_roundup_power2(early_size);
-	OBD_ALLOC_LARGE(early_buf, early_bufsz);
+	early_buf = libcfs_kvzalloc(early_bufsz, GFP_NOFS);
 	if (early_buf == NULL) {
 		rc = -ENOMEM;
 		goto err_req;
@@ -1163,7 +1163,7 @@ int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 err_ctx:
 	sptlrpc_cli_ctx_put(early_req->rq_cli_ctx, 1);
 err_buf:
-	OBD_FREE_LARGE(early_buf, early_bufsz);
+	kvfree(early_buf);
 err_req:
 	ptlrpc_request_cache_free(early_req);
 	return rc;
@@ -1181,7 +1181,7 @@ void sptlrpc_cli_finish_early_reply(struct ptlrpc_request *early_req)
 	LASSERT(early_req->rq_repmsg);
 
 	sptlrpc_cli_ctx_put(early_req->rq_cli_ctx, 1);
-	OBD_FREE_LARGE(early_req->rq_repbuf, early_req->rq_repbuf_len);
+	kvfree(early_req->rq_repbuf);
 	ptlrpc_request_cache_free(early_req);
 }
 
@@ -1273,13 +1273,13 @@ EXPORT_SYMBOL(sptlrpc_sec_put);
  */
 static
 struct ptlrpc_sec *sptlrpc_sec_create(struct obd_import *imp,
-				       struct ptlrpc_svc_ctx *svc_ctx,
-				       struct sptlrpc_flavor *sf,
-				       enum lustre_sec_part sp)
+				      struct ptlrpc_svc_ctx *svc_ctx,
+				      struct sptlrpc_flavor *sf,
+				      enum lustre_sec_part sp)
 {
 	struct ptlrpc_sec_policy *policy;
-	struct ptlrpc_sec	*sec;
-	char		      str[32];
+	struct ptlrpc_sec *sec;
+	char str[32];
 
 	if (svc_ctx) {
 		LASSERT(imp->imp_dlm_fake == 1);
@@ -1369,7 +1369,7 @@ static void sptlrpc_import_sec_adapt_inplace(struct obd_import *imp,
 					     struct ptlrpc_sec *sec,
 					     struct sptlrpc_flavor *sf)
 {
-	char    str1[32], str2[32];
+	char str1[32], str2[32];
 
 	if (sec->ps_flvr.sf_flags != sf->sf_flags)
 		CDEBUG(D_SEC, "changing sec flags: %s -> %s\n",
@@ -1394,12 +1394,12 @@ int sptlrpc_import_sec_adapt(struct obd_import *imp,
 			     struct ptlrpc_svc_ctx *svc_ctx,
 			     struct sptlrpc_flavor *flvr)
 {
-	struct ptlrpc_connection   *conn;
-	struct sptlrpc_flavor       sf;
-	struct ptlrpc_sec	  *sec, *newsec;
-	enum lustre_sec_part	sp;
-	char			str[24];
-	int			 rc = 0;
+	struct ptlrpc_connection *conn;
+	struct sptlrpc_flavor sf;
+	struct ptlrpc_sec *sec, *newsec;
+	enum lustre_sec_part sp;
+	char str[24];
+	int rc = 0;
 
 	might_sleep();
 
@@ -1436,7 +1436,7 @@ int sptlrpc_import_sec_adapt(struct obd_import *imp,
 
 	sec = sptlrpc_import_sec_ref(imp);
 	if (sec) {
-		char    str2[24];
+		char str2[24];
 
 		if (flavor_equal(&sf, &sec->ps_flvr))
 			goto out;
@@ -1585,8 +1585,8 @@ void sptlrpc_cli_free_reqbuf(struct ptlrpc_request *req)
 void _sptlrpc_enlarge_msg_inplace(struct lustre_msg *msg,
 				  int segment, int newsize)
 {
-	void   *src, *dst;
-	int     oldsize, oldmsg_size, movesize;
+	void *src, *dst;
+	int oldsize, oldmsg_size, movesize;
 
 	LASSERT(segment < msg->lm_bufcount);
 	LASSERT(msg->lm_buflens[segment] <= newsize);
@@ -1635,9 +1635,9 @@ EXPORT_SYMBOL(_sptlrpc_enlarge_msg_inplace);
 int sptlrpc_cli_enlarge_reqbuf(struct ptlrpc_request *req,
 			       int segment, int newsize)
 {
-	struct ptlrpc_cli_ctx    *ctx = req->rq_cli_ctx;
-	struct ptlrpc_sec_cops   *cops;
-	struct lustre_msg	*msg = req->rq_reqmsg;
+	struct ptlrpc_cli_ctx *ctx = req->rq_cli_ctx;
+	struct ptlrpc_sec_cops *cops;
+	struct lustre_msg *msg = req->rq_reqmsg;
 
 	LASSERT(ctx);
 	LASSERT(msg);
@@ -1748,7 +1748,7 @@ static int flavor_allowed(struct sptlrpc_flavor *exp,
 int sptlrpc_target_export_check(struct obd_export *exp,
 				struct ptlrpc_request *req)
 {
-	struct sptlrpc_flavor   flavor;
+	struct sptlrpc_flavor flavor;
 
 	if (exp == NULL)
 		return 0;
@@ -1926,8 +1926,8 @@ EXPORT_SYMBOL(sptlrpc_target_export_check);
 void sptlrpc_target_update_exp_flavor(struct obd_device *obd,
 				      struct sptlrpc_rule_set *rset)
 {
-	struct obd_export       *exp;
-	struct sptlrpc_flavor    new_flvr;
+	struct obd_export *exp;
+	struct sptlrpc_flavor new_flvr;
 
 	LASSERT(obd);
 
@@ -2019,8 +2019,8 @@ static int sptlrpc_svc_check_from(struct ptlrpc_request *req, int svc_rc)
 int sptlrpc_svc_unwrap_request(struct ptlrpc_request *req)
 {
 	struct ptlrpc_sec_policy *policy;
-	struct lustre_msg	*msg = req->rq_reqbuf;
-	int		       rc;
+	struct lustre_msg *msg = req->rq_reqbuf;
+	int rc;
 
 	LASSERT(msg);
 	LASSERT(req->rq_reqmsg == NULL);
@@ -2231,8 +2231,8 @@ int sptlrpc_cli_unwrap_bulk_read(struct ptlrpc_request *req,
 				 struct ptlrpc_bulk_desc *desc,
 				 int nob)
 {
-	struct ptlrpc_cli_ctx  *ctx;
-	int		     rc;
+	struct ptlrpc_cli_ctx *ctx;
+	int rc;
 
 	LASSERT(req->rq_bulk_read && !req->rq_bulk_write);
 
@@ -2256,8 +2256,8 @@ EXPORT_SYMBOL(sptlrpc_cli_unwrap_bulk_read);
 int sptlrpc_cli_unwrap_bulk_write(struct ptlrpc_request *req,
 				  struct ptlrpc_bulk_desc *desc)
 {
-	struct ptlrpc_cli_ctx  *ctx;
-	int		     rc;
+	struct ptlrpc_cli_ctx *ctx;
+	int rc;
 
 	LASSERT(!req->rq_bulk_read && req->rq_bulk_write);
 
@@ -2329,7 +2329,7 @@ EXPORT_SYMBOL(sptlrpc_pack_user_desc);
 int sptlrpc_unpack_user_desc(struct lustre_msg *msg, int offset, int swabbed)
 {
 	struct ptlrpc_user_desc *pud;
-	int		      i;
+	int i;
 
 	pud = lustre_msg_buf(msg, offset, sizeof(*pud));
 	if (!pud)
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
index c05a8554d737..69d73c430696 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
@@ -62,7 +62,7 @@
 #define POINTERS_PER_PAGE	(PAGE_CACHE_SIZE / sizeof(void *))
 #define PAGES_PER_POOL		(POINTERS_PER_PAGE)
 
-#define IDLE_IDX_MAX	    (100)
+#define IDLE_IDX_MAX	 (100)
 #define IDLE_IDX_WEIGHT	 (3)
 
 #define CACHE_QUIESCENT_PERIOD  (20)
@@ -173,8 +173,8 @@ int sptlrpc_proc_enc_pool_seq_show(struct seq_file *m, void *v)
 
 static void enc_pools_release_free_pages(long npages)
 {
-	int     p_idx, g_idx;
-	int     p_idx_max1, p_idx_max2;
+	int p_idx, g_idx;
+	int p_idx_max1, p_idx_max2;
 
 	LASSERT(npages > 0);
 	LASSERT(npages <= page_pools.epp_free_pages);
@@ -210,7 +210,7 @@ static void enc_pools_release_free_pages(long npages)
 	/* free unused pools */
 	while (p_idx_max1 < p_idx_max2) {
 		LASSERT(page_pools.epp_pools[p_idx_max2]);
-		OBD_FREE(page_pools.epp_pools[p_idx_max2], PAGE_CACHE_SIZE);
+		kfree(page_pools.epp_pools[p_idx_max2]);
 		page_pools.epp_pools[p_idx_max2] = NULL;
 		p_idx_max2--;
 	}
@@ -284,7 +284,7 @@ int npages_to_npools(unsigned long npages)
 static unsigned long enc_pools_cleanup(struct page ***pools, int npools)
 {
 	unsigned long cleaned = 0;
-	int	   i, j;
+	int i, j;
 
 	for (i = 0; i < npools; i++) {
 		if (pools[i]) {
@@ -294,7 +294,7 @@ static unsigned long enc_pools_cleanup(struct page ***pools, int npools)
 					cleaned++;
 				}
 			}
-			OBD_FREE(pools[i], PAGE_CACHE_SIZE);
+			kfree(pools[i]);
 			pools[i] = NULL;
 		}
 	}
@@ -311,9 +311,9 @@ static unsigned long enc_pools_cleanup(struct page ***pools, int npools)
  */
 static void enc_pools_insert(struct page ***pools, int npools, int npages)
 {
-	int     freeslot;
-	int     op_idx, np_idx, og_idx, ng_idx;
-	int     cur_npools, end_npools;
+	int freeslot;
+	int op_idx, np_idx, og_idx, ng_idx;
+	int cur_npools, end_npools;
 
 	LASSERT(npages > 0);
 	LASSERT(page_pools.epp_total_pages+npages <= page_pools.epp_max_pages);
@@ -393,9 +393,9 @@ static void enc_pools_insert(struct page ***pools, int npools, int npages)
 static int enc_pools_add_pages(int npages)
 {
 	static DEFINE_MUTEX(add_pages_mutex);
-	struct page   ***pools;
-	int	     npools, alloced = 0;
-	int	     i, j, rc = -ENOMEM;
+	struct page ***pools;
+	int npools, alloced = 0;
+	int i, j, rc = -ENOMEM;
 
 	if (npages < PTLRPC_MAX_BRW_PAGES)
 		npages = PTLRPC_MAX_BRW_PAGES;
@@ -409,12 +409,12 @@ static int enc_pools_add_pages(int npages)
 	page_pools.epp_st_grows++;
 
 	npools = npages_to_npools(npages);
-	OBD_ALLOC(pools, npools * sizeof(*pools));
+	pools = kcalloc(npools, sizeof(*pools), GFP_NOFS);
 	if (pools == NULL)
 		goto out;
 
 	for (i = 0; i < npools; i++) {
-		OBD_ALLOC(pools[i], PAGE_CACHE_SIZE);
+		pools[i] = kzalloc(PAGE_CACHE_SIZE, GFP_NOFS);
 		if (pools[i] == NULL)
 			goto out_pools;
 
@@ -435,7 +435,7 @@ static int enc_pools_add_pages(int npages)
 
 out_pools:
 	enc_pools_cleanup(pools, npools);
-	OBD_FREE(pools, npools * sizeof(*pools));
+	kfree(pools);
 out:
 	if (rc) {
 		page_pools.epp_st_grow_fails++;
@@ -494,12 +494,12 @@ static int enc_pools_should_grow(int page_needed, long now)
  */
 int sptlrpc_enc_pool_get_pages(struct ptlrpc_bulk_desc *desc)
 {
-	wait_queue_t  waitlink;
-	unsigned long   this_idle = -1;
-	unsigned long      tick = 0;
-	long	    now;
-	int	     p_idx, g_idx;
-	int	     i;
+	wait_queue_t waitlink;
+	unsigned long this_idle = -1;
+	unsigned long tick = 0;
+	long now;
+	int p_idx, g_idx;
+	int i;
 
 	LASSERT(desc->bd_iov_count > 0);
 	LASSERT(desc->bd_iov_count <= page_pools.epp_max_pages);
@@ -508,8 +508,8 @@ int sptlrpc_enc_pool_get_pages(struct ptlrpc_bulk_desc *desc)
 	if (desc->bd_enc_iov != NULL)
 		return 0;
 
-	OBD_ALLOC(desc->bd_enc_iov,
-		  desc->bd_iov_count * sizeof(*desc->bd_enc_iov));
+	desc->bd_enc_iov = kcalloc(desc->bd_iov_count,
+				   sizeof(*desc->bd_enc_iov), GFP_NOFS);
 	if (desc->bd_enc_iov == NULL)
 		return -ENOMEM;
 
@@ -609,8 +609,8 @@ EXPORT_SYMBOL(sptlrpc_enc_pool_get_pages);
 
 void sptlrpc_enc_pool_put_pages(struct ptlrpc_bulk_desc *desc)
 {
-	int     p_idx, g_idx;
-	int     i;
+	int p_idx, g_idx;
+	int i;
 
 	if (desc->bd_enc_iov == NULL)
 		return;
@@ -646,8 +646,7 @@ void sptlrpc_enc_pool_put_pages(struct ptlrpc_bulk_desc *desc)
 
 	spin_unlock(&page_pools.epp_lock);
 
-	OBD_FREE(desc->bd_enc_iov,
-		 desc->bd_iov_count * sizeof(*desc->bd_enc_iov));
+	kfree(desc->bd_enc_iov);
 	desc->bd_enc_iov = NULL;
 }
 EXPORT_SYMBOL(sptlrpc_enc_pool_put_pages);
@@ -659,7 +658,7 @@ EXPORT_SYMBOL(sptlrpc_enc_pool_put_pages);
  */
 int sptlrpc_enc_pool_add_user(void)
 {
-	int     need_grow = 0;
+	int need_grow = 0;
 
 	spin_lock(&page_pools.epp_lock);
 	if (page_pools.epp_growing == 0 && page_pools.epp_total_pages == 0) {
@@ -690,9 +689,10 @@ EXPORT_SYMBOL(sptlrpc_enc_pool_del_user);
 static inline void enc_pools_alloc(void)
 {
 	LASSERT(page_pools.epp_max_pools);
-	OBD_ALLOC_LARGE(page_pools.epp_pools,
-			page_pools.epp_max_pools *
-			sizeof(*page_pools.epp_pools));
+	page_pools.epp_pools =
+		libcfs_kvzalloc(page_pools.epp_max_pools *
+				sizeof(*page_pools.epp_pools),
+				GFP_NOFS);
 }
 
 static inline void enc_pools_free(void)
@@ -700,9 +700,7 @@ static inline void enc_pools_free(void)
 	LASSERT(page_pools.epp_max_pools);
 	LASSERT(page_pools.epp_pools);
 
-	OBD_FREE_LARGE(page_pools.epp_pools,
-		       page_pools.epp_max_pools *
-		       sizeof(*page_pools.epp_pools));
+	kvfree(page_pools.epp_pools);
 }
 
 static struct shrinker pools_shrinker = {
@@ -843,11 +841,11 @@ EXPORT_SYMBOL(bulk_sec_desc_unpack);
 int sptlrpc_get_bulk_checksum(struct ptlrpc_bulk_desc *desc, __u8 alg,
 			      void *buf, int buflen)
 {
-	struct cfs_crypto_hash_desc	*hdesc;
-	int				hashsize;
-	char				hashbuf[64];
-	unsigned int			bufsize;
-	int				i, err;
+	struct cfs_crypto_hash_desc *hdesc;
+	int hashsize;
+	char hashbuf[64];
+	unsigned int bufsize;
+	int i, err;
 
 	LASSERT(alg > BULK_HASH_ALG_NULL && alg < BULK_HASH_ALG_MAX);
 	LASSERT(buflen >= 4);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
index 56ba9e4e5297..31da43e8b3c6 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
@@ -94,8 +94,8 @@ EXPORT_SYMBOL(sptlrpc_target_sec_part);
  */
 int sptlrpc_parse_flavor(const char *str, struct sptlrpc_flavor *flvr)
 {
-	char	    buf[32];
-	char	   *bulk, *alg;
+	char buf[32];
+	char *bulk, *alg;
 
 	memset(flvr, 0, sizeof(*flvr));
 
@@ -182,8 +182,8 @@ static void sptlrpc_rule_init(struct sptlrpc_rule *rule)
  */
 int sptlrpc_parse_rule(char *param, struct sptlrpc_rule *rule)
 {
-	char	   *flavor, *dir;
-	int	     rc;
+	char *flavor, *dir;
+	int rc;
 
 	sptlrpc_rule_init(rule);
 
@@ -242,8 +242,7 @@ void sptlrpc_rule_set_free(struct sptlrpc_rule_set *rset)
 		(rset->srs_nrule == 0 && rset->srs_rules == NULL));
 
 	if (rset->srs_nslot) {
-		OBD_FREE(rset->srs_rules,
-			 rset->srs_nslot * sizeof(*rset->srs_rules));
+		kfree(rset->srs_rules);
 		sptlrpc_rule_set_init(rset);
 	}
 }
@@ -265,7 +264,7 @@ int sptlrpc_rule_set_expand(struct sptlrpc_rule_set *rset)
 	nslot = rset->srs_nslot + 8;
 
 	/* better use realloc() if available */
-	OBD_ALLOC(rules, nslot * sizeof(*rset->srs_rules));
+	rules = kcalloc(nslot, sizeof(*rset->srs_rules), GFP_NOFS);
 	if (rules == NULL)
 		return -ENOMEM;
 
@@ -274,8 +273,7 @@ int sptlrpc_rule_set_expand(struct sptlrpc_rule_set *rset)
 		memcpy(rules, rset->srs_rules,
 		       rset->srs_nrule * sizeof(*rset->srs_rules));
 
-		OBD_FREE(rset->srs_rules,
-			 rset->srs_nslot * sizeof(*rset->srs_rules));
+		kfree(rset->srs_rules);
 	}
 
 	rset->srs_rules = rules;
@@ -311,9 +309,9 @@ static inline int rule_match_net(struct sptlrpc_rule *r1,
 int sptlrpc_rule_set_merge(struct sptlrpc_rule_set *rset,
 			   struct sptlrpc_rule *rule)
 {
-	struct sptlrpc_rule      *p = rset->srs_rules;
-	int		       spec_dir, spec_net;
-	int		       rc, n, match = 0;
+	struct sptlrpc_rule *p = rset->srs_rules;
+	int spec_dir, spec_net;
+	int rc, n, match = 0;
 
 	might_sleep();
 
@@ -405,8 +403,8 @@ int sptlrpc_rule_set_choose(struct sptlrpc_rule_set *rset,
 			    lnet_nid_t nid,
 			    struct sptlrpc_flavor *sf)
 {
-	struct sptlrpc_rule    *r;
-	int		     n;
+	struct sptlrpc_rule *r;
+	int n;
 
 	for (n = 0; n < rset->srs_nrule; n++) {
 		r = &rset->srs_rules[n];
@@ -435,7 +433,7 @@ EXPORT_SYMBOL(sptlrpc_rule_set_choose);
 void sptlrpc_rule_set_dump(struct sptlrpc_rule_set *rset)
 {
 	struct sptlrpc_rule *r;
-	int     n;
+	int n;
 
 	for (n = 0; n < rset->srs_nrule; n++) {
 		r = &rset->srs_rules[n];
@@ -476,8 +474,8 @@ static inline int is_hex(char c)
 
 static void target2fsname(const char *tgt, char *fsname, int buflen)
 {
-	const char     *ptr;
-	int	     len;
+	const char *ptr;
+	int len;
 
 	ptr = strrchr(tgt, '-');
 	if (ptr) {
@@ -509,7 +507,7 @@ static void sptlrpc_conf_free_rsets(struct sptlrpc_conf *conf)
 				     &conf->sc_tgts, sct_list) {
 		sptlrpc_rule_set_free(&conf_tgt->sct_rset);
 		list_del(&conf_tgt->sct_list);
-		OBD_FREE_PTR(conf_tgt);
+		kfree(conf_tgt);
 	}
 	LASSERT(list_empty(&conf->sc_tgts));
 
@@ -523,7 +521,7 @@ static void sptlrpc_conf_free(struct sptlrpc_conf *conf)
 
 	sptlrpc_conf_free_rsets(conf);
 	list_del(&conf->sc_list);
-	OBD_FREE_PTR(conf);
+	kfree(conf);
 }
 
 static
@@ -541,7 +539,7 @@ struct sptlrpc_conf_tgt *sptlrpc_conf_get_tgt(struct sptlrpc_conf *conf,
 	if (!create)
 		return NULL;
 
-	OBD_ALLOC_PTR(conf_tgt);
+	conf_tgt = kzalloc(sizeof(*conf_tgt), GFP_NOFS);
 	if (conf_tgt) {
 		strlcpy(conf_tgt->sct_name, name, sizeof(conf_tgt->sct_name));
 		sptlrpc_rule_set_init(&conf_tgt->sct_rset);
@@ -565,7 +563,7 @@ struct sptlrpc_conf *sptlrpc_conf_get(const char *fsname,
 	if (!create)
 		return NULL;
 
-	OBD_ALLOC_PTR(conf);
+	conf = kzalloc(sizeof(*conf), GFP_NOFS);
 	if (conf == NULL)
 		return NULL;
 
@@ -585,8 +583,8 @@ static int sptlrpc_conf_merge_rule(struct sptlrpc_conf *conf,
 				   const char *target,
 				   struct sptlrpc_rule *rule)
 {
-	struct sptlrpc_conf_tgt  *conf_tgt;
-	struct sptlrpc_rule_set  *rule_set;
+	struct sptlrpc_conf_tgt *conf_tgt;
+	struct sptlrpc_rule_set *rule_set;
 
 	/* fsname == target means general rules for the whole fs */
 	if (strcmp(conf->sc_fsname, target) == 0) {
@@ -612,10 +610,10 @@ static int sptlrpc_conf_merge_rule(struct sptlrpc_conf *conf,
 static int __sptlrpc_process_config(struct lustre_cfg *lcfg,
 				    struct sptlrpc_conf *conf)
 {
-	char		   *target, *param;
-	char		    fsname[MTI_NAME_MAXLEN];
-	struct sptlrpc_rule     rule;
-	int		     rc;
+	char *target, *param;
+	char fsname[MTI_NAME_MAXLEN];
+	struct sptlrpc_rule rule;
+	int rc;
 
 	target = lustre_cfg_string(lcfg, 1);
 	if (target == NULL) {
@@ -673,8 +671,8 @@ EXPORT_SYMBOL(sptlrpc_process_config);
 
 static int logname2fsname(const char *logname, char *buf, int buflen)
 {
-	char   *ptr;
-	int     len;
+	char *ptr;
+	int len;
 
 	ptr = strrchr(logname, '-');
 	if (ptr == NULL || strcmp(ptr, "-sptlrpc")) {
@@ -692,7 +690,7 @@ static int logname2fsname(const char *logname, char *buf, int buflen)
 void sptlrpc_conf_log_update_begin(const char *logname)
 {
 	struct sptlrpc_conf *conf;
-	char		 fsname[16];
+	char fsname[16];
 
 	if (logname2fsname(logname, fsname, sizeof(fsname)))
 		return;
@@ -718,7 +716,7 @@ EXPORT_SYMBOL(sptlrpc_conf_log_update_begin);
 void sptlrpc_conf_log_update_end(const char *logname)
 {
 	struct sptlrpc_conf *conf;
-	char		 fsname[16];
+	char fsname[16];
 
 	if (logname2fsname(logname, fsname, sizeof(fsname)))
 		return;
@@ -743,7 +741,7 @@ EXPORT_SYMBOL(sptlrpc_conf_log_update_end);
 
 void sptlrpc_conf_log_start(const char *logname)
 {
-	char		 fsname[16];
+	char fsname[16];
 
 	if (logname2fsname(logname, fsname, sizeof(fsname)))
 		return;
@@ -757,7 +755,7 @@ EXPORT_SYMBOL(sptlrpc_conf_log_start);
 void sptlrpc_conf_log_stop(const char *logname)
 {
 	struct sptlrpc_conf *conf;
-	char		 fsname[16];
+	char fsname[16];
 
 	if (logname2fsname(logname, fsname, sizeof(fsname)))
 		return;
@@ -801,10 +799,10 @@ void sptlrpc_conf_choose_flavor(enum lustre_sec_part from,
 				lnet_nid_t nid,
 				struct sptlrpc_flavor *sf)
 {
-	struct sptlrpc_conf     *conf;
+	struct sptlrpc_conf *conf;
 	struct sptlrpc_conf_tgt *conf_tgt;
-	char		     name[MTI_NAME_MAXLEN];
-	int		      len, rc = 0;
+	char name[MTI_NAME_MAXLEN];
+	int len, rc = 0;
 
 	target2fsname(target->uuid, name, sizeof(name));
 
@@ -860,7 +858,7 @@ EXPORT_SYMBOL(sptlrpc_target_choose_flavor);
  */
 void sptlrpc_conf_client_adapt(struct obd_device *obd)
 {
-	struct obd_import  *imp;
+	struct obd_import *imp;
 
 	LASSERT(strcmp(obd->obd_type->typ_name, LUSTRE_MDC_NAME) == 0 ||
 		strcmp(obd->obd_type->typ_name, LUSTRE_OSC_NAME) == 0);
@@ -882,7 +880,7 @@ void sptlrpc_conf_client_adapt(struct obd_device *obd)
 }
 EXPORT_SYMBOL(sptlrpc_conf_client_adapt);
 
-int  sptlrpc_conf_init(void)
+int sptlrpc_conf_init(void)
 {
 	mutex_init(&sptlrpc_conf_lock);
 	return 0;
@@ -890,7 +888,7 @@ int  sptlrpc_conf_init(void)
 
 void sptlrpc_conf_fini(void)
 {
-	struct sptlrpc_conf  *conf, *conf_next;
+	struct sptlrpc_conf *conf, *conf_next;
 
 	mutex_lock(&sptlrpc_conf_lock);
 	list_for_each_entry_safe(conf, conf_next, &sptlrpc_confs, sc_list) {
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_gc.c b/drivers/staging/lustre/lustre/ptlrpc/sec_gc.c
index 81de68edb04e..cdad608bdb8d 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_gc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_gc.c
@@ -164,7 +164,7 @@ static void sec_do_gc(struct ptlrpc_sec *sec)
 static int sec_gc_main(void *arg)
 {
 	struct ptlrpc_thread *thread = (struct ptlrpc_thread *) arg;
-	struct l_wait_info    lwi;
+	struct l_wait_info lwi;
 
 	unshare_fs_struct();
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_lproc.c b/drivers/staging/lustre/lustre/ptlrpc/sec_lproc.c
index 0d08145a6c7e..68fcac14b3ee 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_lproc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_lproc.c
@@ -51,10 +51,6 @@
 
 #include "ptlrpc_internal.h"
 
-
-struct proc_dir_entry *sptlrpc_proc_root = NULL;
-EXPORT_SYMBOL(sptlrpc_proc_root);
-
 static char *sec_flags2str(unsigned long flags, char *buf, int bufsize)
 {
 	buf[0] = '\0';
@@ -78,7 +74,7 @@ static int sptlrpc_info_lprocfs_seq_show(struct seq_file *seq, void *v)
 	struct obd_device *dev = seq->private;
 	struct client_obd *cli = &dev->u.cli;
 	struct ptlrpc_sec *sec = NULL;
-	char	       str[32];
+	char str[32];
 
 	LASSERT(strcmp(dev->obd_type->typ_name, LUSTRE_OSC_NAME) == 0 ||
 		strcmp(dev->obd_type->typ_name, LUSTRE_MDC_NAME) == 0 ||
@@ -138,7 +134,7 @@ LPROC_SEQ_FOPS_RO(sptlrpc_ctxs_lprocfs);
 
 int sptlrpc_lprocfs_cliobd_attach(struct obd_device *dev)
 {
-	int     rc;
+	int rc;
 
 	if (strcmp(dev->obd_type->typ_name, LUSTRE_OSC_NAME) != 0 &&
 	    strcmp(dev->obd_type->typ_name, LUSTRE_MDC_NAME) != 0 &&
@@ -148,16 +144,16 @@ int sptlrpc_lprocfs_cliobd_attach(struct obd_device *dev)
 		return -EINVAL;
 	}
 
-	rc = lprocfs_obd_seq_create(dev, "srpc_info", 0444,
-				    &sptlrpc_info_lprocfs_fops, dev);
+	rc = ldebugfs_obd_seq_create(dev, "srpc_info", 0444,
+				     &sptlrpc_info_lprocfs_fops, dev);
 	if (rc) {
 		CERROR("create proc entry srpc_info for %s: %d\n",
 		       dev->obd_name, rc);
 		return rc;
 	}
 
-	rc = lprocfs_obd_seq_create(dev, "srpc_contexts", 0444,
-				    &sptlrpc_ctxs_lprocfs_fops, dev);
+	rc = ldebugfs_obd_seq_create(dev, "srpc_contexts", 0444,
+				     &sptlrpc_ctxs_lprocfs_fops, dev);
 	if (rc) {
 		CERROR("create proc entry srpc_contexts for %s: %d\n",
 		       dev->obd_name, rc);
@@ -174,17 +170,20 @@ static struct lprocfs_vars sptlrpc_lprocfs_vars[] = {
 	{ NULL }
 };
 
+static struct dentry *sptlrpc_debugfs_dir;
+
 int sptlrpc_lproc_init(void)
 {
-	int     rc;
+	int rc;
 
-	LASSERT(sptlrpc_proc_root == NULL);
+	LASSERT(sptlrpc_debugfs_dir == NULL);
 
-	sptlrpc_proc_root = lprocfs_register("sptlrpc", proc_lustre_root,
-					     sptlrpc_lprocfs_vars, NULL);
-	if (IS_ERR(sptlrpc_proc_root)) {
-		rc = PTR_ERR(sptlrpc_proc_root);
-		sptlrpc_proc_root = NULL;
+	sptlrpc_debugfs_dir = ldebugfs_register("sptlrpc", debugfs_lustre_root,
+						sptlrpc_lprocfs_vars, NULL);
+	if (IS_ERR_OR_NULL(sptlrpc_debugfs_dir)) {
+		rc = sptlrpc_debugfs_dir ? PTR_ERR(sptlrpc_debugfs_dir)
+					 : -ENOMEM;
+		sptlrpc_debugfs_dir = NULL;
 		return rc;
 	}
 	return 0;
@@ -192,8 +191,6 @@ int sptlrpc_lproc_init(void)
 
 void sptlrpc_lproc_fini(void)
 {
-	if (sptlrpc_proc_root) {
-		lprocfs_remove(&sptlrpc_proc_root);
-		sptlrpc_proc_root = NULL;
-	}
+	if (!IS_ERR_OR_NULL(sptlrpc_debugfs_dir))
+		ldebugfs_remove(&sptlrpc_debugfs_dir);
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_null.c b/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
index 4e132435b450..ce1c563d0175 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
@@ -92,7 +92,7 @@ int null_ctx_sign(struct ptlrpc_cli_ctx *ctx, struct ptlrpc_request *req)
 static
 int null_ctx_verify(struct ptlrpc_cli_ctx *ctx, struct ptlrpc_request *req)
 {
-	__u32   cksums, cksumc;
+	__u32 cksums, cksumc;
 
 	LASSERT(req->rq_repdata);
 
@@ -159,7 +159,7 @@ int null_alloc_reqbuf(struct ptlrpc_sec *sec,
 		int alloc_size = size_roundup_power2(msgsize);
 
 		LASSERT(!req->rq_pool);
-		OBD_ALLOC_LARGE(req->rq_reqbuf, alloc_size);
+		req->rq_reqbuf = libcfs_kvzalloc(alloc_size, GFP_NOFS);
 		if (!req->rq_reqbuf)
 			return -ENOMEM;
 
@@ -186,7 +186,7 @@ void null_free_reqbuf(struct ptlrpc_sec *sec,
 			 "req %p: reqlen %d should smaller than buflen %d\n",
 			 req, req->rq_reqlen, req->rq_reqbuf_len);
 
-		OBD_FREE_LARGE(req->rq_reqbuf, req->rq_reqbuf_len);
+		kvfree(req->rq_reqbuf);
 		req->rq_reqbuf = NULL;
 		req->rq_reqbuf_len = 0;
 	}
@@ -202,7 +202,7 @@ int null_alloc_repbuf(struct ptlrpc_sec *sec,
 
 	msgsize = size_roundup_power2(msgsize);
 
-	OBD_ALLOC_LARGE(req->rq_repbuf, msgsize);
+	req->rq_repbuf = libcfs_kvzalloc(msgsize, GFP_NOFS);
 	if (!req->rq_repbuf)
 		return -ENOMEM;
 
@@ -216,7 +216,7 @@ void null_free_repbuf(struct ptlrpc_sec *sec,
 {
 	LASSERT(req->rq_repbuf);
 
-	OBD_FREE_LARGE(req->rq_repbuf, req->rq_repbuf_len);
+	kvfree(req->rq_repbuf);
 	req->rq_repbuf = NULL;
 	req->rq_repbuf_len = 0;
 }
@@ -226,9 +226,9 @@ int null_enlarge_reqbuf(struct ptlrpc_sec *sec,
 			struct ptlrpc_request *req,
 			int segment, int newsize)
 {
-	struct lustre_msg      *newbuf;
-	struct lustre_msg      *oldbuf = req->rq_reqmsg;
-	int		     oldsize, newmsg_size, alloc_size;
+	struct lustre_msg *newbuf;
+	struct lustre_msg *oldbuf = req->rq_reqmsg;
+	int oldsize, newmsg_size, alloc_size;
 
 	LASSERT(req->rq_reqbuf);
 	LASSERT(req->rq_reqbuf == req->rq_reqmsg);
@@ -247,7 +247,7 @@ int null_enlarge_reqbuf(struct ptlrpc_sec *sec,
 	if (req->rq_reqbuf_len < newmsg_size) {
 		alloc_size = size_roundup_power2(newmsg_size);
 
-		OBD_ALLOC_LARGE(newbuf, alloc_size);
+		newbuf = libcfs_kvzalloc(alloc_size, GFP_NOFS);
 		if (newbuf == NULL)
 			return -ENOMEM;
 
@@ -261,7 +261,7 @@ int null_enlarge_reqbuf(struct ptlrpc_sec *sec,
 			spin_lock(&req->rq_import->imp_lock);
 		memcpy(newbuf, req->rq_reqbuf, req->rq_reqlen);
 
-		OBD_FREE_LARGE(req->rq_reqbuf, req->rq_reqbuf_len);
+		kvfree(req->rq_reqbuf);
 		req->rq_reqbuf = req->rq_reqmsg = newbuf;
 		req->rq_reqbuf_len = alloc_size;
 
@@ -316,7 +316,7 @@ int null_alloc_rs(struct ptlrpc_request *req, int msgsize)
 		/* pre-allocated */
 		LASSERT(rs->rs_size >= rs_size);
 	} else {
-		OBD_ALLOC_LARGE(rs, rs_size);
+		rs = libcfs_kvzalloc(rs_size, GFP_NOFS);
 		if (rs == NULL)
 			return -ENOMEM;
 
@@ -341,7 +341,7 @@ void null_free_rs(struct ptlrpc_reply_state *rs)
 	atomic_dec(&rs->rs_svc_ctx->sc_refcount);
 
 	if (!rs->rs_prealloc)
-		OBD_FREE_LARGE(rs, rs->rs_size);
+		kvfree(rs);
 }
 
 static
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c b/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
index a79cd53010a4..53ce0d14bd46 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
@@ -136,7 +136,7 @@ static int plain_verify_bulk_csum(struct ptlrpc_bulk_desc *desc,
 				  struct plain_bulk_token *tokenr)
 {
 	struct plain_bulk_token tokenv;
-	int		     rc;
+	int rc;
 
 	if (hash_alg == BULK_HASH_ALG_NULL)
 		return 0;
@@ -154,8 +154,8 @@ static int plain_verify_bulk_csum(struct ptlrpc_bulk_desc *desc,
 
 static void corrupt_bulk_data(struct ptlrpc_bulk_desc *desc)
 {
-	char	   *ptr;
-	unsigned int    off, i;
+	char *ptr;
+	unsigned int off, i;
 
 	for (i = 0; i < desc->bd_iov_count; i++) {
 		if (desc->bd_iov[i].kiov_len == 0)
@@ -190,7 +190,7 @@ int plain_ctx_validate(struct ptlrpc_cli_ctx *ctx)
 static
 int plain_ctx_sign(struct ptlrpc_cli_ctx *ctx, struct ptlrpc_request *req)
 {
-	struct lustre_msg   *msg = req->rq_reqbuf;
+	struct lustre_msg *msg = req->rq_reqbuf;
 	struct plain_header *phdr;
 
 	msg->lm_secflvr = req->rq_flvr.sf_rpc;
@@ -214,10 +214,10 @@ int plain_ctx_sign(struct ptlrpc_cli_ctx *ctx, struct ptlrpc_request *req)
 static
 int plain_ctx_verify(struct ptlrpc_cli_ctx *ctx, struct ptlrpc_request *req)
 {
-	struct lustre_msg   *msg = req->rq_repdata;
+	struct lustre_msg *msg = req->rq_repdata;
 	struct plain_header *phdr;
-	__u32		cksum;
-	int		  swabbed;
+	__u32 cksum;
+	int swabbed;
 
 	if (msg->lm_bufcount != PLAIN_PACK_SEGMENTS) {
 		CERROR("unexpected reply buf count %u\n", msg->lm_bufcount);
@@ -290,8 +290,8 @@ int plain_cli_wrap_bulk(struct ptlrpc_cli_ctx *ctx,
 			struct ptlrpc_bulk_desc *desc)
 {
 	struct ptlrpc_bulk_sec_desc *bsd;
-	struct plain_bulk_token     *token;
-	int			  rc;
+	struct plain_bulk_token *token;
+	int rc;
 
 	LASSERT(req->rq_pack_bulk);
 	LASSERT(req->rq_reqbuf->lm_bufcount == PLAIN_PACK_SEGMENTS);
@@ -333,9 +333,9 @@ int plain_cli_unwrap_bulk(struct ptlrpc_cli_ctx *ctx,
 			  struct ptlrpc_bulk_desc *desc)
 {
 	struct ptlrpc_bulk_sec_desc *bsdv;
-	struct plain_bulk_token     *tokenv;
-	int			  rc;
-	int			  i, nob;
+	struct plain_bulk_token *tokenv;
+	int rc;
+	int i, nob;
 
 	LASSERT(req->rq_pack_bulk);
 	LASSERT(req->rq_reqbuf->lm_bufcount == PLAIN_PACK_SEGMENTS);
@@ -374,9 +374,9 @@ int plain_cli_unwrap_bulk(struct ptlrpc_cli_ctx *ctx,
 static
 struct ptlrpc_cli_ctx *plain_sec_install_ctx(struct plain_sec *plsec)
 {
-	struct ptlrpc_cli_ctx  *ctx, *ctx_new;
+	struct ptlrpc_cli_ctx *ctx, *ctx_new;
 
-	OBD_ALLOC_PTR(ctx_new);
+	ctx_new = kzalloc(sizeof(*ctx_new), GFP_NOFS);
 
 	write_lock(&plsec->pls_lock);
 
@@ -384,8 +384,7 @@ struct ptlrpc_cli_ctx *plain_sec_install_ctx(struct plain_sec *plsec)
 	if (ctx) {
 		atomic_inc(&ctx->cc_refcount);
 
-		if (ctx_new)
-			OBD_FREE_PTR(ctx_new);
+		kfree(ctx_new);
 	} else if (ctx_new) {
 		ctx = ctx_new;
 
@@ -414,7 +413,7 @@ struct ptlrpc_cli_ctx *plain_sec_install_ctx(struct plain_sec *plsec)
 static
 void plain_destroy_sec(struct ptlrpc_sec *sec)
 {
-	struct plain_sec       *plsec = sec2plsec(sec);
+	struct plain_sec *plsec = sec2plsec(sec);
 
 	LASSERT(sec->ps_policy == &plain_policy);
 	LASSERT(sec->ps_import);
@@ -424,7 +423,7 @@ void plain_destroy_sec(struct ptlrpc_sec *sec)
 
 	class_import_put(sec->ps_import);
 
-	OBD_FREE_PTR(plsec);
+	kfree(plsec);
 }
 
 static
@@ -438,13 +437,13 @@ struct ptlrpc_sec *plain_create_sec(struct obd_import *imp,
 				    struct ptlrpc_svc_ctx *svc_ctx,
 				    struct sptlrpc_flavor *sf)
 {
-	struct plain_sec       *plsec;
-	struct ptlrpc_sec      *sec;
-	struct ptlrpc_cli_ctx  *ctx;
+	struct plain_sec *plsec;
+	struct ptlrpc_sec *sec;
+	struct ptlrpc_cli_ctx *ctx;
 
 	LASSERT(SPTLRPC_FLVR_POLICY(sf->sf_rpc) == SPTLRPC_POLICY_PLAIN);
 
-	OBD_ALLOC_PTR(plsec);
+	plsec = kzalloc(sizeof(*plsec), GFP_NOFS);
 	if (plsec == NULL)
 		return NULL;
 
@@ -484,8 +483,8 @@ struct ptlrpc_cli_ctx *plain_lookup_ctx(struct ptlrpc_sec *sec,
 					struct vfs_cred *vcred,
 					int create, int remove_dead)
 {
-	struct plain_sec       *plsec = sec2plsec(sec);
-	struct ptlrpc_cli_ctx  *ctx;
+	struct plain_sec *plsec = sec2plsec(sec);
+	struct ptlrpc_cli_ctx *ctx;
 
 	read_lock(&plsec->pls_lock);
 	ctx = plsec->pls_ctx;
@@ -508,7 +507,7 @@ void plain_release_ctx(struct ptlrpc_sec *sec,
 	LASSERT(atomic_read(&ctx->cc_refcount) == 0);
 	LASSERT(ctx->cc_sec == sec);
 
-	OBD_FREE_PTR(ctx);
+	kfree(ctx);
 
 	atomic_dec(&sec->ps_nctx);
 	sptlrpc_sec_put(sec);
@@ -518,8 +517,8 @@ static
 int plain_flush_ctx_cache(struct ptlrpc_sec *sec,
 			  uid_t uid, int grace, int force)
 {
-	struct plain_sec       *plsec = sec2plsec(sec);
-	struct ptlrpc_cli_ctx  *ctx;
+	struct plain_sec *plsec = sec2plsec(sec);
+	struct ptlrpc_cli_ctx *ctx;
 
 	/* do nothing unless caller want to flush for 'all' */
 	if (uid != -1)
@@ -541,7 +540,7 @@ int plain_alloc_reqbuf(struct ptlrpc_sec *sec,
 		       int msgsize)
 {
 	__u32 buflens[PLAIN_PACK_SEGMENTS] = { 0, };
-	int   alloc_len;
+	int alloc_len;
 
 	buflens[PLAIN_PACK_HDR_OFF] = sizeof(struct plain_header);
 	buflens[PLAIN_PACK_MSG_OFF] = msgsize;
@@ -560,7 +559,7 @@ int plain_alloc_reqbuf(struct ptlrpc_sec *sec,
 		LASSERT(!req->rq_pool);
 
 		alloc_len = size_roundup_power2(alloc_len);
-		OBD_ALLOC_LARGE(req->rq_reqbuf, alloc_len);
+		req->rq_reqbuf = libcfs_kvzalloc(alloc_len, GFP_NOFS);
 		if (!req->rq_reqbuf)
 			return -ENOMEM;
 
@@ -585,7 +584,7 @@ void plain_free_reqbuf(struct ptlrpc_sec *sec,
 		       struct ptlrpc_request *req)
 {
 	if (!req->rq_pool) {
-		OBD_FREE_LARGE(req->rq_reqbuf, req->rq_reqbuf_len);
+		kvfree(req->rq_reqbuf);
 		req->rq_reqbuf = NULL;
 		req->rq_reqbuf_len = 0;
 	}
@@ -614,7 +613,7 @@ int plain_alloc_repbuf(struct ptlrpc_sec *sec,
 
 	alloc_len = size_roundup_power2(alloc_len);
 
-	OBD_ALLOC_LARGE(req->rq_repbuf, alloc_len);
+	req->rq_repbuf = libcfs_kvzalloc(alloc_len, GFP_NOFS);
 	if (!req->rq_repbuf)
 		return -ENOMEM;
 
@@ -626,7 +625,7 @@ static
 void plain_free_repbuf(struct ptlrpc_sec *sec,
 		       struct ptlrpc_request *req)
 {
-	OBD_FREE_LARGE(req->rq_repbuf, req->rq_repbuf_len);
+	kvfree(req->rq_repbuf);
 	req->rq_repbuf = NULL;
 	req->rq_repbuf_len = 0;
 }
@@ -636,9 +635,9 @@ int plain_enlarge_reqbuf(struct ptlrpc_sec *sec,
 			 struct ptlrpc_request *req,
 			 int segment, int newsize)
 {
-	struct lustre_msg      *newbuf;
-	int		     oldsize;
-	int		     newmsg_size, newbuf_size;
+	struct lustre_msg *newbuf;
+	int oldsize;
+	int newmsg_size, newbuf_size;
 
 	LASSERT(req->rq_reqbuf);
 	LASSERT(req->rq_reqbuf_len >= req->rq_reqlen);
@@ -665,7 +664,7 @@ int plain_enlarge_reqbuf(struct ptlrpc_sec *sec,
 	if (req->rq_reqbuf_len < newbuf_size) {
 		newbuf_size = size_roundup_power2(newbuf_size);
 
-		OBD_ALLOC_LARGE(newbuf, newbuf_size);
+		newbuf = libcfs_kvzalloc(newbuf_size, GFP_NOFS);
 		if (newbuf == NULL)
 			return -ENOMEM;
 
@@ -680,7 +679,7 @@ int plain_enlarge_reqbuf(struct ptlrpc_sec *sec,
 
 		memcpy(newbuf, req->rq_reqbuf, req->rq_reqbuf_len);
 
-		OBD_FREE_LARGE(req->rq_reqbuf, req->rq_reqbuf_len);
+		kvfree(req->rq_reqbuf);
 		req->rq_reqbuf = newbuf;
 		req->rq_reqbuf_len = newbuf_size;
 		req->rq_reqmsg = lustre_msg_buf(req->rq_reqbuf,
@@ -710,9 +709,9 @@ static struct ptlrpc_svc_ctx plain_svc_ctx = {
 static
 int plain_accept(struct ptlrpc_request *req)
 {
-	struct lustre_msg   *msg = req->rq_reqbuf;
+	struct lustre_msg *msg = req->rq_reqbuf;
 	struct plain_header *phdr;
-	int		  swabbed;
+	int swabbed;
 
 	LASSERT(SPTLRPC_FLVR_POLICY(req->rq_flvr.sf_rpc) ==
 		SPTLRPC_POLICY_PLAIN);
@@ -781,9 +780,9 @@ int plain_accept(struct ptlrpc_request *req)
 static
 int plain_alloc_rs(struct ptlrpc_request *req, int msgsize)
 {
-	struct ptlrpc_reply_state   *rs;
-	__u32			buflens[PLAIN_PACK_SEGMENTS] = { 0, };
-	int			  rs_size = sizeof(*rs);
+	struct ptlrpc_reply_state *rs;
+	__u32 buflens[PLAIN_PACK_SEGMENTS] = { 0, };
+	int rs_size = sizeof(*rs);
 
 	LASSERT(msgsize % 8 == 0);
 
@@ -801,7 +800,7 @@ int plain_alloc_rs(struct ptlrpc_request *req, int msgsize)
 		/* pre-allocated */
 		LASSERT(rs->rs_size >= rs_size);
 	} else {
-		OBD_ALLOC_LARGE(rs, rs_size);
+		rs = libcfs_kvzalloc(rs_size, GFP_NOFS);
 		if (rs == NULL)
 			return -ENOMEM;
 
@@ -827,16 +826,16 @@ void plain_free_rs(struct ptlrpc_reply_state *rs)
 	atomic_dec(&rs->rs_svc_ctx->sc_refcount);
 
 	if (!rs->rs_prealloc)
-		OBD_FREE_LARGE(rs, rs->rs_size);
+		kvfree(rs);
 }
 
 static
 int plain_authorize(struct ptlrpc_request *req)
 {
 	struct ptlrpc_reply_state *rs = req->rq_reply_state;
-	struct lustre_msg_v2      *msg = rs->rs_repbuf;
-	struct plain_header       *phdr;
-	int			len;
+	struct lustre_msg_v2 *msg = rs->rs_repbuf;
+	struct plain_header *phdr;
+	int len;
 
 	LASSERT(rs);
 	LASSERT(msg);
@@ -871,7 +870,7 @@ int plain_authorize(struct ptlrpc_request *req)
 			lustre_msg_buf(msg, PLAIN_PACK_MSG_OFF, 0),
 			lustre_msg_buflen(msg, PLAIN_PACK_MSG_OFF),
 			NULL, 0, (unsigned char *)&msg->lm_cksum, &hsize);
-			req->rq_reply_off = 0;
+		req->rq_reply_off = 0;
 	}
 
 	return 0;
@@ -881,10 +880,10 @@ static
 int plain_svc_unwrap_bulk(struct ptlrpc_request *req,
 			  struct ptlrpc_bulk_desc *desc)
 {
-	struct ptlrpc_reply_state   *rs = req->rq_reply_state;
+	struct ptlrpc_reply_state *rs = req->rq_reply_state;
 	struct ptlrpc_bulk_sec_desc *bsdr, *bsdv;
-	struct plain_bulk_token     *tokenr;
-	int			  rc;
+	struct plain_bulk_token *tokenr;
+	int rc;
 
 	LASSERT(req->rq_bulk_write);
 	LASSERT(req->rq_pack_bulk);
@@ -915,10 +914,10 @@ static
 int plain_svc_wrap_bulk(struct ptlrpc_request *req,
 			struct ptlrpc_bulk_desc *desc)
 {
-	struct ptlrpc_reply_state   *rs = req->rq_reply_state;
+	struct ptlrpc_reply_state *rs = req->rq_reply_state;
 	struct ptlrpc_bulk_sec_desc *bsdr, *bsdv;
-	struct plain_bulk_token     *tokenv;
-	int			  rc;
+	struct plain_bulk_token *tokenv;
+	int rc;
 
 	LASSERT(req->rq_bulk_read);
 	LASSERT(req->rq_pack_bulk);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/service.c b/drivers/staging/lustre/lustre/ptlrpc/service.c
index 8e61421515cb..9117f1c15a8e 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/service.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/service.c
@@ -72,10 +72,12 @@ struct mutex ptlrpc_all_services_mutex;
 struct ptlrpc_request_buffer_desc *
 ptlrpc_alloc_rqbd(struct ptlrpc_service_part *svcpt)
 {
-	struct ptlrpc_service		  *svc = svcpt->scp_service;
+	struct ptlrpc_service *svc = svcpt->scp_service;
 	struct ptlrpc_request_buffer_desc *rqbd;
 
-	OBD_CPT_ALLOC_PTR(rqbd, svc->srv_cptable, svcpt->scp_cpt);
+	rqbd = kzalloc_node(sizeof(*rqbd), GFP_NOFS,
+			    cfs_cpt_spread_node(svc->srv_cptable,
+						svcpt->scp_cpt));
 	if (rqbd == NULL)
 		return NULL;
 
@@ -87,7 +89,7 @@ ptlrpc_alloc_rqbd(struct ptlrpc_service_part *svcpt)
 	OBD_CPT_ALLOC_LARGE(rqbd->rqbd_buffer, svc->srv_cptable,
 			    svcpt->scp_cpt, svc->srv_buf_size);
 	if (rqbd->rqbd_buffer == NULL) {
-		OBD_FREE_PTR(rqbd);
+		kfree(rqbd);
 		return NULL;
 	}
 
@@ -112,17 +114,17 @@ ptlrpc_free_rqbd(struct ptlrpc_request_buffer_desc *rqbd)
 	svcpt->scp_nrqbds_total--;
 	spin_unlock(&svcpt->scp_lock);
 
-	OBD_FREE_LARGE(rqbd->rqbd_buffer, svcpt->scp_service->srv_buf_size);
-	OBD_FREE_PTR(rqbd);
+	kvfree(rqbd->rqbd_buffer);
+	kfree(rqbd);
 }
 
 int
 ptlrpc_grow_req_bufs(struct ptlrpc_service_part *svcpt, int post)
 {
-	struct ptlrpc_service		  *svc = svcpt->scp_service;
+	struct ptlrpc_service *svc = svcpt->scp_service;
 	struct ptlrpc_request_buffer_desc *rqbd;
-	int				rc = 0;
-	int				i;
+	int rc = 0;
+	int i;
 
 	if (svcpt->scp_rqbd_allocating)
 		goto try_post;
@@ -184,7 +186,7 @@ ptlrpc_save_lock(struct ptlrpc_request *req,
 		 struct lustre_handle *lock, int mode, int no_ack)
 {
 	struct ptlrpc_reply_state *rs = req->rq_reply_state;
-	int			idx;
+	int idx;
 
 	LASSERT(rs != NULL);
 	LASSERT(rs->rs_nlocks < RS_MAX_LOCKS);
@@ -273,8 +275,8 @@ static void rs_batch_init(struct rs_batch *b)
 static struct ptlrpc_hr_thread *
 ptlrpc_hr_select(struct ptlrpc_service_part *svcpt)
 {
-	struct ptlrpc_hr_partition	*hrp;
-	unsigned int			rotor;
+	struct ptlrpc_hr_partition *hrp;
+	unsigned int rotor;
 
 	if (svcpt->scp_cpt >= 0 &&
 	    svcpt->scp_service->srv_cptable == ptlrpc_hr.hr_cpt_table) {
@@ -429,8 +431,8 @@ static int
 ptlrpc_server_post_idle_rqbds(struct ptlrpc_service_part *svcpt)
 {
 	struct ptlrpc_request_buffer_desc *rqbd;
-	int				  rc;
-	int				  posted = 0;
+	int rc;
+	int posted = 0;
 
 	for (;;) {
 		spin_lock(&svcpt->scp_lock);
@@ -487,11 +489,11 @@ static void
 ptlrpc_server_nthreads_check(struct ptlrpc_service *svc,
 			     struct ptlrpc_service_conf *conf)
 {
-	struct ptlrpc_service_thr_conf	*tc = &conf->psc_thr;
-	unsigned			init;
-	unsigned			total;
-	unsigned			nthrs;
-	int				weight;
+	struct ptlrpc_service_thr_conf *tc = &conf->psc_thr;
+	unsigned init;
+	unsigned total;
+	unsigned nthrs;
+	int weight;
 
 	/*
 	 * Common code for estimating & validating threads number.
@@ -515,7 +517,7 @@ ptlrpc_server_nthreads_check(struct ptlrpc_service *svc,
 		 * be up to 8 * nthrs_max */
 		total = min(tc->tc_nthrs_max * 8, tc->tc_nthrs_user);
 		nthrs = total / svc->srv_ncpts;
-		init  = max(init, nthrs);
+		init = max(init, nthrs);
 		goto out;
 	}
 
@@ -529,7 +531,7 @@ ptlrpc_server_nthreads_check(struct ptlrpc_service *svc,
 
 	nthrs = tc->tc_nthrs_base;
 	if (svc->srv_ncpts == 1) {
-		int	i;
+		int i;
 
 		/* NB: Increase the base number if it's single partition
 		 * and total number of cores/HTs is larger or equal to 4.
@@ -541,7 +543,7 @@ ptlrpc_server_nthreads_check(struct ptlrpc_service *svc,
 	}
 
 	if (tc->tc_thr_factor != 0) {
-		int	  factor = tc->tc_thr_factor;
+		int factor = tc->tc_thr_factor;
 		const int fade = 4;
 
 		/*
@@ -557,7 +559,7 @@ ptlrpc_server_nthreads_check(struct ptlrpc_service *svc,
 		 * there are.
 		 */
 		/* weight is # of HTs */
-		if (cpumask_weight(topology_thread_cpumask(0)) > 1) {
+		if (cpumask_weight(topology_sibling_cpumask(0)) > 1) {
 			/* depress thread factor for hyper-thread */
 			factor = factor - (factor >> 1) + (factor >> 3);
 		}
@@ -593,9 +595,9 @@ ptlrpc_service_part_init(struct ptlrpc_service *svc,
 			 struct ptlrpc_service_part *svcpt, int cpt)
 {
 	struct ptlrpc_at_array	*array;
-	int			size;
-	int			index;
-	int			rc;
+	int size;
+	int index;
+	int rc;
 
 	svcpt->scp_cpt = cpt;
 	INIT_LIST_HEAD(&svcpt->scp_threads);
@@ -625,23 +627,25 @@ ptlrpc_service_part_init(struct ptlrpc_service *svc,
 	array = &svcpt->scp_at_array;
 
 	size = at_est2timeout(at_max);
-	array->paa_size     = size;
-	array->paa_count    = 0;
+	array->paa_size = size;
+	array->paa_count = 0;
 	array->paa_deadline = -1;
 
 	/* allocate memory for scp_at_array (ptlrpc_at_array) */
-	OBD_CPT_ALLOC(array->paa_reqs_array,
-		      svc->srv_cptable, cpt, sizeof(struct list_head) * size);
+	array->paa_reqs_array =
+		kzalloc_node(sizeof(struct list_head) * size, GFP_NOFS,
+			     cfs_cpt_spread_node(svc->srv_cptable, cpt));
 	if (array->paa_reqs_array == NULL)
 		return -ENOMEM;
 
 	for (index = 0; index < size; index++)
 		INIT_LIST_HEAD(&array->paa_reqs_array[index]);
 
-	OBD_CPT_ALLOC(array->paa_reqs_count,
-		      svc->srv_cptable, cpt, sizeof(__u32) * size);
+	array->paa_reqs_count =
+		kzalloc_node(sizeof(__u32) * size, GFP_NOFS,
+			     cfs_cpt_spread_node(svc->srv_cptable, cpt));
 	if (array->paa_reqs_count == NULL)
-		goto failed;
+		goto free_reqs_array;
 
 	cfs_timer_init(&svcpt->scp_at_timer, ptlrpc_at_timer, svcpt);
 	/* At SOW, service time should be quick; 10s seems generous. If client
@@ -655,21 +659,16 @@ ptlrpc_service_part_init(struct ptlrpc_service *svc,
 	/* We shouldn't be under memory pressure at startup, so
 	 * fail if we can't allocate all our buffers at this time. */
 	if (rc != 0)
-		goto failed;
+		goto free_reqs_count;
 
 	return 0;
 
- failed:
-	if (array->paa_reqs_count != NULL) {
-		OBD_FREE(array->paa_reqs_count, sizeof(__u32) * size);
-		array->paa_reqs_count = NULL;
-	}
-
-	if (array->paa_reqs_array != NULL) {
-		OBD_FREE(array->paa_reqs_array,
-			 sizeof(struct list_head) * array->paa_size);
-		array->paa_reqs_array = NULL;
-	}
+free_reqs_count:
+	kfree(array->paa_reqs_count);
+	array->paa_reqs_count = NULL;
+free_reqs_array:
+	kfree(array->paa_reqs_array);
+	array->paa_reqs_array = NULL;
 
 	return -ENOMEM;
 }
@@ -681,17 +680,18 @@ ptlrpc_service_part_init(struct ptlrpc_service *svc,
  */
 struct ptlrpc_service *
 ptlrpc_register_service(struct ptlrpc_service_conf *conf,
-			struct proc_dir_entry *proc_entry)
-{
-	struct ptlrpc_service_cpt_conf	*cconf = &conf->psc_cpt;
-	struct ptlrpc_service		*service;
-	struct ptlrpc_service_part	*svcpt;
-	struct cfs_cpt_table		*cptable;
-	__u32				*cpts = NULL;
-	int				ncpts;
-	int				cpt;
-	int				rc;
-	int				i;
+			struct kset *parent,
+			struct dentry *debugfs_entry)
+{
+	struct ptlrpc_service_cpt_conf *cconf = &conf->psc_cpt;
+	struct ptlrpc_service *service;
+	struct ptlrpc_service_part *svcpt;
+	struct cfs_cpt_table *cptable;
+	__u32 *cpts = NULL;
+	int ncpts;
+	int cpt;
+	int rc;
+	int i;
 
 	LASSERT(conf->psc_buf.bc_nbufs > 0);
 	LASSERT(conf->psc_buf.bc_buf_size >=
@@ -707,7 +707,7 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 	} else {
 		ncpts = cfs_cpt_number(cptable);
 		if (cconf->cc_pattern != NULL) {
-			struct cfs_expr_list	*el;
+			struct cfs_expr_list *el;
 
 			rc = cfs_expr_list_parse(cconf->cc_pattern,
 						 strlen(cconf->cc_pattern),
@@ -723,24 +723,23 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 			if (rc <= 0) {
 				CERROR("%s: failed to parse CPT array %s: %d\n",
 				       conf->psc_name, cconf->cc_pattern, rc);
-				if (cpts != NULL)
-					OBD_FREE(cpts, sizeof(*cpts) * ncpts);
+				kfree(cpts);
 				return ERR_PTR(rc < 0 ? rc : -EINVAL);
 			}
 			ncpts = rc;
 		}
 	}
 
-	OBD_ALLOC(service, offsetof(struct ptlrpc_service, srv_parts[ncpts]));
+	service = kzalloc(offsetof(struct ptlrpc_service, srv_parts[ncpts]),
+			  GFP_NOFS);
 	if (service == NULL) {
-		if (cpts != NULL)
-			OBD_FREE(cpts, sizeof(*cpts) * ncpts);
+		kfree(cpts);
 		return ERR_PTR(-ENOMEM);
 	}
 
-	service->srv_cptable		= cptable;
-	service->srv_cpts		= cpts;
-	service->srv_ncpts		= ncpts;
+	service->srv_cptable = cptable;
+	service->srv_cpts = cpts;
+	service->srv_ncpts = ncpts;
 
 	service->srv_cpt_bits = 0; /* it's zero already, easy to read... */
 	while ((1 << service->srv_cpt_bits) < cfs_cpt_number(cptable))
@@ -748,18 +747,18 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 
 	/* public members */
 	spin_lock_init(&service->srv_lock);
-	service->srv_name		= conf->psc_name;
-	service->srv_watchdog_factor	= conf->psc_watchdog_factor;
+	service->srv_name = conf->psc_name;
+	service->srv_watchdog_factor = conf->psc_watchdog_factor;
 	INIT_LIST_HEAD(&service->srv_list); /* for safety of cleanup */
 
 	/* buffer configuration */
-	service->srv_nbuf_per_group	= test_req_buffer_pressure ?
+	service->srv_nbuf_per_group = test_req_buffer_pressure ?
 					  1 : conf->psc_buf.bc_nbufs;
-	service->srv_max_req_size	= conf->psc_buf.bc_req_max_size +
+	service->srv_max_req_size = conf->psc_buf.bc_req_max_size +
 					  SPTLRPC_MAX_PAYLOAD;
-	service->srv_buf_size		= conf->psc_buf.bc_buf_size;
-	service->srv_rep_portal		= conf->psc_buf.bc_rep_portal;
-	service->srv_req_portal		= conf->psc_buf.bc_req_portal;
+	service->srv_buf_size = conf->psc_buf.bc_buf_size;
+	service->srv_rep_portal	= conf->psc_buf.bc_rep_portal;
+	service->srv_req_portal	= conf->psc_buf.bc_req_portal;
 
 	/* Increase max reply size to next power of two */
 	service->srv_max_reply_size = 1;
@@ -767,10 +766,10 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 	       conf->psc_buf.bc_rep_max_size + SPTLRPC_MAX_PAYLOAD)
 		service->srv_max_reply_size <<= 1;
 
-	service->srv_thread_name	= conf->psc_thr.tc_thr_name;
-	service->srv_ctx_tags		= conf->psc_thr.tc_ctx_tags;
-	service->srv_hpreq_ratio	= PTLRPC_SVC_HP_RATIO;
-	service->srv_ops		= conf->psc_ops;
+	service->srv_thread_name = conf->psc_thr.tc_thr_name;
+	service->srv_ctx_tags = conf->psc_thr.tc_ctx_tags;
+	service->srv_hpreq_ratio = PTLRPC_SVC_HP_RATIO;
+	service->srv_ops = conf->psc_ops;
 
 	for (i = 0; i < ncpts; i++) {
 		if (!conf->psc_thr.tc_cpu_affinity)
@@ -778,7 +777,8 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 		else
 			cpt = cpts != NULL ? cpts[i] : i;
 
-		OBD_CPT_ALLOC(svcpt, cptable, cpt, sizeof(*svcpt));
+		svcpt = kzalloc_node(sizeof(*svcpt), GFP_NOFS,
+				     cfs_cpt_spread_node(cptable, cpt));
 		if (svcpt == NULL) {
 			rc = -ENOMEM;
 			goto failed;
@@ -799,8 +799,14 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 	list_add(&service->srv_list, &ptlrpc_all_services);
 	mutex_unlock(&ptlrpc_all_services_mutex);
 
-	if (proc_entry != NULL)
-		ptlrpc_lprocfs_register_service(proc_entry, service);
+	if (parent) {
+		rc = ptlrpc_sysfs_register_service(parent, service);
+		if (rc)
+			goto failed;
+	}
+
+	if (!IS_ERR_OR_NULL(debugfs_entry))
+		ptlrpc_ldebugfs_register_service(debugfs_entry, service);
 
 	rc = ptlrpc_service_nrs_setup(service);
 	if (rc != 0)
@@ -853,11 +859,11 @@ static void ptlrpc_server_free_request(struct ptlrpc_request *req)
 void ptlrpc_server_drop_request(struct ptlrpc_request *req)
 {
 	struct ptlrpc_request_buffer_desc *rqbd = req->rq_rqbd;
-	struct ptlrpc_service_part	  *svcpt = rqbd->rqbd_svcpt;
-	struct ptlrpc_service		  *svc = svcpt->scp_service;
-	int				refcount;
-	struct list_head			*tmp;
-	struct list_head			*nxt;
+	struct ptlrpc_service_part *svcpt = rqbd->rqbd_svcpt;
+	struct ptlrpc_service *svc = svcpt->scp_service;
+	int refcount;
+	struct list_head *tmp;
+	struct list_head *nxt;
 
 	if (!atomic_dec_and_test(&req->rq_refcount))
 		return;
@@ -1304,7 +1310,7 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 	reqcopy = ptlrpc_request_cache_alloc(GFP_NOFS);
 	if (reqcopy == NULL)
 		return -ENOMEM;
-	OBD_ALLOC_LARGE(reqmsg, req->rq_reqlen);
+	reqmsg = libcfs_kvzalloc(req->rq_reqlen, GFP_NOFS);
 	if (!reqmsg) {
 		rc = -ENOMEM;
 		goto out_free;
@@ -1368,7 +1374,7 @@ out_put:
 	class_export_put(reqcopy->rq_export);
 out:
 	sptlrpc_svc_ctx_decref(reqcopy);
-	OBD_FREE_LARGE(reqmsg, req->rq_reqlen);
+	kvfree(reqmsg);
 out_free:
 	ptlrpc_request_cache_free(reqcopy);
 	return rc;
@@ -1381,7 +1387,7 @@ static int ptlrpc_at_check_timed(struct ptlrpc_service_part *svcpt)
 	struct ptlrpc_at_array *array = &svcpt->scp_at_array;
 	struct ptlrpc_request *rq, *n;
 	struct list_head work_list;
-	__u32  index, count;
+	__u32 index, count;
 	time_t deadline;
 	time_t now = get_seconds();
 	long delay;
@@ -1726,10 +1732,10 @@ static int
 ptlrpc_server_handle_req_in(struct ptlrpc_service_part *svcpt,
 			    struct ptlrpc_thread *thread)
 {
-	struct ptlrpc_service	*svc = svcpt->scp_service;
-	struct ptlrpc_request	*req;
-	__u32			deadline;
-	int			rc;
+	struct ptlrpc_service *svc = svcpt->scp_service;
+	struct ptlrpc_request *req;
+	__u32 deadline;
+	int rc;
 
 	spin_lock(&svcpt->scp_lock);
 	if (list_empty(&svcpt->scp_req_incoming)) {
@@ -1870,11 +1876,11 @@ ptlrpc_server_handle_request(struct ptlrpc_service_part *svcpt,
 {
 	struct ptlrpc_service *svc = svcpt->scp_service;
 	struct ptlrpc_request *request;
-	struct timeval	 work_start;
-	struct timeval	 work_end;
-	long		   timediff;
-	int		    rc;
-	int		    fail_opc = 0;
+	struct timeval work_start;
+	struct timeval work_end;
+	long timediff;
+	int rc;
+	int fail_opc = 0;
 
 	request = ptlrpc_server_request_get(svcpt, false);
 	if (request == NULL)
@@ -2026,10 +2032,10 @@ static int
 ptlrpc_handle_rs(struct ptlrpc_reply_state *rs)
 {
 	struct ptlrpc_service_part *svcpt = rs->rs_svcpt;
-	struct ptlrpc_service     *svc = svcpt->scp_service;
-	struct obd_export	 *exp;
-	int			nlocks;
-	int			been_handled;
+	struct ptlrpc_service *svc = svcpt->scp_service;
+	struct obd_export *exp;
+	int nlocks;
+	int been_handled;
 
 	exp = rs->rs_export;
 
@@ -2256,10 +2262,10 @@ ptlrpc_wait_event(struct ptlrpc_service_part *svcpt,
  */
 static int ptlrpc_main(void *arg)
 {
-	struct ptlrpc_thread		*thread = (struct ptlrpc_thread *)arg;
-	struct ptlrpc_service_part	*svcpt = thread->t_svcpt;
-	struct ptlrpc_service		*svc = svcpt->scp_service;
-	struct ptlrpc_reply_state	*rs;
+	struct ptlrpc_thread *thread = (struct ptlrpc_thread *)arg;
+	struct ptlrpc_service_part *svcpt = thread->t_svcpt;
+	struct ptlrpc_service *svc = svcpt->scp_service;
+	struct ptlrpc_reply_state *rs;
 	struct group_info *ginfo = NULL;
 	struct lu_env *env;
 	int counter = 0, rc = 0;
@@ -2291,7 +2297,7 @@ static int ptlrpc_main(void *arg)
 			goto out;
 	}
 
-	OBD_ALLOC_PTR(env);
+	env = kzalloc(sizeof(*env), GFP_NOFS);
 	if (env == NULL) {
 		rc = -ENOMEM;
 		goto out_srv_fini;
@@ -2317,7 +2323,7 @@ static int ptlrpc_main(void *arg)
 	}
 
 	/* Alloc reply state structure for this one */
-	OBD_ALLOC_LARGE(rs, svc->srv_max_reply_size);
+	rs = libcfs_kvzalloc(svc->srv_max_reply_size, GFP_NOFS);
 	if (!rs) {
 		rc = -ENOMEM;
 		goto out_srv_fini;
@@ -2414,7 +2420,7 @@ out_srv_fini:
 
 	if (env != NULL) {
 		lu_context_fini(&env->le_ctx);
-		OBD_FREE_PTR(env);
+		kfree(env);
 	}
 out:
 	CDEBUG(D_RPCTRACE, "service thread [ %p : %u ] %d exiting: rc %d\n",
@@ -2458,11 +2464,11 @@ static int hrt_dont_sleep(struct ptlrpc_hr_thread *hrt,
  */
 static int ptlrpc_hr_main(void *arg)
 {
-	struct ptlrpc_hr_thread		*hrt = (struct ptlrpc_hr_thread *)arg;
-	struct ptlrpc_hr_partition	*hrp = hrt->hrt_partition;
-	LIST_HEAD			(replies);
-	char				threadname[20];
-	int				rc;
+	struct ptlrpc_hr_thread	*hrt = (struct ptlrpc_hr_thread *)arg;
+	struct ptlrpc_hr_partition *hrp = hrt->hrt_partition;
+	LIST_HEAD	(replies);
+	char threadname[20];
+	int rc;
 
 	snprintf(threadname, sizeof(threadname), "ptlrpc_hr%02d_%03d",
 		 hrp->hrp_cpt, hrt->hrt_id);
@@ -2499,9 +2505,9 @@ static int ptlrpc_hr_main(void *arg)
 
 static void ptlrpc_stop_hr_threads(void)
 {
-	struct ptlrpc_hr_partition	*hrp;
-	int				i;
-	int				j;
+	struct ptlrpc_hr_partition *hrp;
+	int i;
+	int j;
 
 	ptlrpc_hr.hr_stopping = 1;
 
@@ -2523,12 +2529,12 @@ static void ptlrpc_stop_hr_threads(void)
 
 static int ptlrpc_start_hr_threads(void)
 {
-	struct ptlrpc_hr_partition	*hrp;
-	int				i;
-	int				j;
+	struct ptlrpc_hr_partition *hrp;
+	int i;
+	int j;
 
 	cfs_percpt_for_each(hrp, i, ptlrpc_hr.hr_partitions) {
-		int	rc = 0;
+		int rc = 0;
 
 		for (j = 0; j < hrp->hrp_nthrs; j++) {
 			struct	ptlrpc_hr_thread *hrt = &hrp->hrp_thrs[j];
@@ -2555,9 +2561,9 @@ static int ptlrpc_start_hr_threads(void)
 
 static void ptlrpc_svcpt_stop_threads(struct ptlrpc_service_part *svcpt)
 {
-	struct l_wait_info	lwi = { 0 };
-	struct ptlrpc_thread	*thread;
-	LIST_HEAD		(zombie);
+	struct l_wait_info lwi = { 0 };
+	struct ptlrpc_thread *thread;
+	LIST_HEAD	(zombie);
 
 	CDEBUG(D_INFO, "Stopping threads for service %s\n",
 	       svcpt->scp_service->srv_name);
@@ -2596,7 +2602,7 @@ static void ptlrpc_svcpt_stop_threads(struct ptlrpc_service_part *svcpt)
 		thread = list_entry(zombie.next,
 					struct ptlrpc_thread, t_link);
 		list_del(&thread->t_link);
-		OBD_FREE_PTR(thread);
+		kfree(thread);
 	}
 }
 
@@ -2606,7 +2612,7 @@ static void ptlrpc_svcpt_stop_threads(struct ptlrpc_service_part *svcpt)
 void ptlrpc_stop_all_threads(struct ptlrpc_service *svc)
 {
 	struct ptlrpc_service_part *svcpt;
-	int			   i;
+	int i;
 
 	ptlrpc_service_for_each_part(svcpt, i, svc) {
 		if (svcpt->scp_service != NULL)
@@ -2617,9 +2623,9 @@ EXPORT_SYMBOL(ptlrpc_stop_all_threads);
 
 int ptlrpc_start_threads(struct ptlrpc_service *svc)
 {
-	int	rc = 0;
-	int	i;
-	int	j;
+	int rc = 0;
+	int i;
+	int j;
 
 	/* We require 2 threads min, see note in ptlrpc_server_handle_request */
 	LASSERT(svc->srv_nthrs_cpt_init >= PTLRPC_NTHRS_INIT);
@@ -2648,10 +2654,10 @@ EXPORT_SYMBOL(ptlrpc_start_threads);
 
 int ptlrpc_start_thread(struct ptlrpc_service_part *svcpt, int wait)
 {
-	struct l_wait_info	lwi = { 0 };
-	struct ptlrpc_thread	*thread;
-	struct ptlrpc_service	*svc;
-	int			rc;
+	struct l_wait_info lwi = { 0 };
+	struct ptlrpc_thread *thread;
+	struct ptlrpc_service *svc;
+	int rc;
 
 	LASSERT(svcpt != NULL);
 
@@ -2670,7 +2676,9 @@ int ptlrpc_start_thread(struct ptlrpc_service_part *svcpt, int wait)
 	     svcpt->scp_nthrs_running == svc->srv_nthrs_cpt_init - 1))
 		return -EMFILE;
 
-	OBD_CPT_ALLOC_PTR(thread, svc->srv_cptable, svcpt->scp_cpt);
+	thread = kzalloc_node(sizeof(*thread), GFP_NOFS,
+			      cfs_cpt_spread_node(svc->srv_cptable,
+						  svcpt->scp_cpt));
 	if (thread == NULL)
 		return -ENOMEM;
 	init_waitqueue_head(&thread->t_ctl_waitq);
@@ -2678,7 +2686,7 @@ int ptlrpc_start_thread(struct ptlrpc_service_part *svcpt, int wait)
 	spin_lock(&svcpt->scp_lock);
 	if (!ptlrpc_threads_increasable(svcpt)) {
 		spin_unlock(&svcpt->scp_lock);
-		OBD_FREE_PTR(thread);
+		kfree(thread);
 		return -EMFILE;
 	}
 
@@ -2687,7 +2695,7 @@ int ptlrpc_start_thread(struct ptlrpc_service_part *svcpt, int wait)
 		 * might require unique and contiguous t_id */
 		LASSERT(svcpt->scp_nthrs_starting == 1);
 		spin_unlock(&svcpt->scp_lock);
-		OBD_FREE_PTR(thread);
+		kfree(thread);
 		if (wait) {
 			CDEBUG(D_INFO, "Waiting for creating thread %s #%d\n",
 			       svc->srv_thread_name, svcpt->scp_thr_nextid);
@@ -2733,7 +2741,7 @@ int ptlrpc_start_thread(struct ptlrpc_service_part *svcpt, int wait)
 		} else {
 			list_del(&thread->t_link);
 			spin_unlock(&svcpt->scp_lock);
-			OBD_FREE_PTR(thread);
+			kfree(thread);
 		}
 		return rc;
 	}
@@ -2751,12 +2759,12 @@ int ptlrpc_start_thread(struct ptlrpc_service_part *svcpt, int wait)
 
 int ptlrpc_hr_init(void)
 {
-	struct ptlrpc_hr_partition	*hrp;
-	struct ptlrpc_hr_thread		*hrt;
-	int				rc;
-	int				i;
-	int				j;
-	int				weight;
+	struct ptlrpc_hr_partition *hrp;
+	struct ptlrpc_hr_thread	*hrt;
+	int rc;
+	int i;
+	int j;
+	int weight;
 
 	memset(&ptlrpc_hr, 0, sizeof(ptlrpc_hr));
 	ptlrpc_hr.hr_cpt_table = cfs_cpt_table;
@@ -2768,7 +2776,7 @@ int ptlrpc_hr_init(void)
 
 	init_waitqueue_head(&ptlrpc_hr.hr_waitq);
 
-	weight = cpumask_weight(topology_thread_cpumask(0));
+	weight = cpumask_weight(topology_sibling_cpumask(0));
 
 	cfs_percpt_for_each(hrp, i, ptlrpc_hr.hr_partitions) {
 		hrp->hrp_cpt = i;
@@ -2780,8 +2788,10 @@ int ptlrpc_hr_init(void)
 		hrp->hrp_nthrs /= weight;
 
 		LASSERT(hrp->hrp_nthrs > 0);
-		OBD_CPT_ALLOC(hrp->hrp_thrs, ptlrpc_hr.hr_cpt_table, i,
-			      hrp->hrp_nthrs * sizeof(*hrt));
+		hrp->hrp_thrs =
+			kzalloc_node(hrp->hrp_nthrs * sizeof(*hrt), GFP_NOFS,
+				cfs_cpt_spread_node(ptlrpc_hr.hr_cpt_table,
+						    i));
 		if (hrp->hrp_thrs == NULL) {
 			rc = -ENOMEM;
 			goto out;
@@ -2807,8 +2817,8 @@ out:
 
 void ptlrpc_hr_fini(void)
 {
-	struct ptlrpc_hr_partition	*hrp;
-	int				i;
+	struct ptlrpc_hr_partition *hrp;
+	int i;
 
 	if (ptlrpc_hr.hr_partitions == NULL)
 		return;
@@ -2817,8 +2827,7 @@ void ptlrpc_hr_fini(void)
 
 	cfs_percpt_for_each(hrp, i, ptlrpc_hr.hr_partitions) {
 		if (hrp->hrp_thrs != NULL) {
-			OBD_FREE(hrp->hrp_thrs,
-				 hrp->hrp_nthrs * sizeof(hrp->hrp_thrs[0]));
+			kfree(hrp->hrp_thrs);
 		}
 	}
 
@@ -2849,8 +2858,8 @@ static void ptlrpc_wait_replies(struct ptlrpc_service_part *svcpt)
 static void
 ptlrpc_service_del_atimer(struct ptlrpc_service *svc)
 {
-	struct ptlrpc_service_part	*svcpt;
-	int				i;
+	struct ptlrpc_service_part *svcpt;
+	int i;
 
 	/* early disarm AT timer... */
 	ptlrpc_service_for_each_part(svcpt, i, svc) {
@@ -2862,11 +2871,11 @@ ptlrpc_service_del_atimer(struct ptlrpc_service *svc)
 static void
 ptlrpc_service_unlink_rqbd(struct ptlrpc_service *svc)
 {
-	struct ptlrpc_service_part	  *svcpt;
+	struct ptlrpc_service_part *svcpt;
 	struct ptlrpc_request_buffer_desc *rqbd;
-	struct l_wait_info		  lwi;
-	int				  rc;
-	int				  i;
+	struct l_wait_info lwi;
+	int rc;
+	int i;
 
 	/* All history will be culled when the next request buffer is
 	 * freed in ptlrpc_service_purge_all() */
@@ -2918,11 +2927,11 @@ ptlrpc_service_unlink_rqbd(struct ptlrpc_service *svc)
 static void
 ptlrpc_service_purge_all(struct ptlrpc_service *svc)
 {
-	struct ptlrpc_service_part		*svcpt;
-	struct ptlrpc_request_buffer_desc	*rqbd;
-	struct ptlrpc_request			*req;
-	struct ptlrpc_reply_state		*rs;
-	int					i;
+	struct ptlrpc_service_part *svcpt;
+	struct ptlrpc_request_buffer_desc *rqbd;
+	struct ptlrpc_request *req;
+	struct ptlrpc_reply_state *rs;
+	int i;
 
 	ptlrpc_service_for_each_part(svcpt, i, svc) {
 		if (svcpt->scp_service == NULL)
@@ -2978,7 +2987,7 @@ ptlrpc_service_purge_all(struct ptlrpc_service *svc)
 					    struct ptlrpc_reply_state,
 					    rs_list);
 			list_del(&rs->rs_list);
-			OBD_FREE_LARGE(rs, svc->srv_max_reply_size);
+			kvfree(rs);
 		}
 	}
 }
@@ -2986,9 +2995,9 @@ ptlrpc_service_purge_all(struct ptlrpc_service *svc)
 static void
 ptlrpc_service_free(struct ptlrpc_service *svc)
 {
-	struct ptlrpc_service_part	*svcpt;
-	struct ptlrpc_at_array		*array;
-	int				i;
+	struct ptlrpc_service_part *svcpt;
+	struct ptlrpc_at_array *array;
+	int i;
 
 	ptlrpc_service_for_each_part(svcpt, i, svc) {
 		if (svcpt->scp_service == NULL)
@@ -2998,27 +3007,19 @@ ptlrpc_service_free(struct ptlrpc_service *svc)
 		cfs_timer_disarm(&svcpt->scp_at_timer);
 		array = &svcpt->scp_at_array;
 
-		if (array->paa_reqs_array != NULL) {
-			OBD_FREE(array->paa_reqs_array,
-				 sizeof(struct list_head) * array->paa_size);
-			array->paa_reqs_array = NULL;
-		}
-
-		if (array->paa_reqs_count != NULL) {
-			OBD_FREE(array->paa_reqs_count,
-				 sizeof(__u32) * array->paa_size);
-			array->paa_reqs_count = NULL;
-		}
+		kfree(array->paa_reqs_array);
+		array->paa_reqs_array = NULL;
+		kfree(array->paa_reqs_count);
+		array->paa_reqs_count = NULL;
 	}
 
 	ptlrpc_service_for_each_part(svcpt, i, svc)
-		OBD_FREE_PTR(svcpt);
+		kfree(svcpt);
 
 	if (svc->srv_cpts != NULL)
 		cfs_expr_list_values_free(svc->srv_cpts, svc->srv_ncpts);
 
-	OBD_FREE(svc, offsetof(struct ptlrpc_service,
-			       srv_parts[svc->srv_ncpts]));
+	kfree(svc);
 }
 
 int ptlrpc_unregister_service(struct ptlrpc_service *service)
@@ -3039,6 +3040,7 @@ int ptlrpc_unregister_service(struct ptlrpc_service *service)
 	ptlrpc_service_nrs_cleanup(service);
 
 	ptlrpc_lprocfs_unregister_service(service);
+	ptlrpc_sysfs_unregister_service(service);
 
 	ptlrpc_service_free(service);
 
@@ -3054,9 +3056,9 @@ EXPORT_SYMBOL(ptlrpc_unregister_service);
  * to be shot, so it's intentionally non-aggressive. */
 int ptlrpc_svcpt_health_check(struct ptlrpc_service_part *svcpt)
 {
-	struct ptlrpc_request		*request = NULL;
-	struct timeval			right_now;
-	long				timediff;
+	struct ptlrpc_request *request = NULL;
+	struct timeval right_now;
+	long timediff;
 
 	do_gettimeofday(&right_now);
 
@@ -3088,8 +3090,8 @@ int ptlrpc_svcpt_health_check(struct ptlrpc_service_part *svcpt)
 int
 ptlrpc_service_health_check(struct ptlrpc_service *svc)
 {
-	struct ptlrpc_service_part	*svcpt;
-	int				i;
+	struct ptlrpc_service_part *svcpt;
+	int i;
 
 	if (svc == NULL)
 		return 0;
diff --git a/drivers/staging/lustre/sysfs-fs-lustre b/drivers/staging/lustre/sysfs-fs-lustre
new file mode 100644
index 000000000000..1e302e8516ce
--- /dev/null
+++ b/drivers/staging/lustre/sysfs-fs-lustre
@@ -0,0 +1,543 @@
+What:		/sys/fs/lustre/version
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows current running lustre version.
+
+What:		/sys/fs/lustre/pinger
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows if the lustre module has pinger support.
+		"on" means yes and "off" means no.
+
+What:		/sys/fs/lustre/health
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows whenever current system state believed to be "healthy",
+		"NOT HEALTHY", or "LBUG" whenever lustre has experienced
+		an internal assertion failure
+
+What:		/sys/fs/lustre/jobid_name
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Currently running job "name" for this node to be transferred
+		to Lustre servers for purposes of QoS and statistics gathering.
+		Writing into this file will change the name, reading outputs
+		currently set value.
+
+What:		/sys/fs/lustre/jobid_var
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Control file for lustre "jobstats" functionality, write new
+		value from the list below to change the mode:
+		disable - disable job name reporting to the servers (default)
+		procname_uid - form the job name as the current running
+			       command name and pid with a dot in between
+			       e.g. dd.1253
+		nodelocal - use jobid_name value from above.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/blocksize
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Biggest blocksize on object storage server for this filesystem.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/kbytestotal
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows total number of kilobytes of space on this filesystem
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/kbytesfree
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows total number of free kilobytes of space on this filesystem
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/kbytesavail
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows total number of free kilobytes of space on this filesystem
+		actually available for use (taking into account per-client
+		grants and filesystem reservations).
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/filestotal
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows total number of inodes on the filesystem.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/filesfree
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows estimated number of free inodes on the filesystem
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/client_type
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows whenever this filesystem considers this client to be
+		compute cluster-local or remote. Remote clients have
+		additional uid/gid convrting logic applied.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/fstype
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows filesystem type of the filesystem
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/uuid
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows this filesystem superblock uuid
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/max_read_ahead_mb
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Sets maximum number of megabytes in system memory to be
+		given to read-ahead cache.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/max_read_ahead_per_file_mb
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Sets maximum number of megabytes to read-ahead for a single file
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/max_read_ahead_whole_mb
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		For small reads, how many megabytes to actually request from
+		the server as initial read-ahead.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/checksum_pages
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Enables or disables per-page checksum at llite layer, before
+		the pages are actually given to lower level for network transfer
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/stats_track_pid
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Limit Lustre vfs operations gathering to just a single pid.
+		0 to track everything.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/stats_track_ppid
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Limit Lustre vfs operations gathering to just a single ppid.
+		0 to track everything.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/stats_track_gid
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Limit Lustre vfs operations gathering to just a single gid.
+		0 to track everything.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/statahead_max
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls maximum number of statahead requests to send when
+		sequential readdir+stat pattern is detected.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/statahead_agl
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls if AGL (async glimpse ahead - obtain object information
+		from OSTs in parallel with MDS during statahead) should be
+		enabled or disabled.
+		0 to disable, 1 to enable.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/lazystatfs
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls statfs(2) behaviour in the face of down servers.
+		If 0, always wait for all servers to come online,
+		if 1, ignote inactive servers.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/max_easize
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows maximum number of bytes file striping data could be
+		in current configuration of storage.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/default_easize
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows maximum observed file striping data seen by this
+		filesystem client instance.
+
+What:		/sys/fs/lustre/llite/<fsname>-<uuid>/xattr_cache
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls extended attributes client-side cache.
+		1 to enable, 0 to disable.
+
+What:		/sys/fs/lustre/ldlm/cancel_unused_locks_before_replay
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls if client should replay unused locks during recovery
+		If a client tends to have a lot of unused locks in LRU,
+		recovery times might become prolonged.
+		1 - just locally cancel unused locks (default)
+		0 - replay unused locks.
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/resource_count
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Displays number of lock resources (objects on which individual
+		locks are taken) currently allocated in this namespace.
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/lock_count
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Displays number or locks allocated in this namespace.
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/lru_size
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls and displays LRU size limit for unused locks for this
+		namespace.
+		0 - LRU size is unlimited, controlled by server resources
+		positive number - number of locks to allow in lock LRU list
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/lock_unused_count
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Display number of locks currently sitting in the LRU list
+		of this namespace
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/lru_max_age
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Maximum number of milliseconds a lock could sit in LRU list
+		before client would voluntarily cancel it as unused.
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/early_lock_cancel
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls "early lock cancellation" feature on this namespace
+		if supported by the server.
+		When enabled, tries to preemtively cancel locks that would be
+		cancelled by verious operations and bundle the cancellation
+		requests in the same RPC as the main operation, which results
+		in significant speedups due to reduced lock-pingpong RPCs.
+		0 - disabled
+		1 - enabled (default)
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/granted
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Displays number of granted locks in this namespace
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/grant_rate
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of granted locks in this namespace during last
+		time interval
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/cancel_rate
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of lock cancellations in this namespace during
+		last time interval
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/grant_speed
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Calculated speed of lock granting (grant_rate - cancel_rate)
+		in this namespace
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/grant_plan
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Estimated number of locks to be granted in the next time
+		interval in this namespace
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/limit
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls number of allowed locks in this pool.
+		When lru_size is 0, this is the actual limit then.
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/lock_volume_factor
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Multiplier for all lock volume calculations above.
+		Default is 1. Increase to make the client to more agressively
+		clean it's lock LRU list for this namespace.
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/server_lock_volume
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Calculated server lock volume.
+
+What:		/sys/fs/lustre/ldlm/namespaces/<name>/pool/recalc_period
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls length of time between recalculation of above
+		values (in seconds).
+
+What:		/sys/fs/lustre/ldlm/services/ldlm_cbd/threads_min
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls minimum number of ldlm callback threads to start.
+
+What:		/sys/fs/lustre/ldlm/services/ldlm_cbd/threads_max
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls maximum number of ldlm callback threads to start.
+
+What:		/sys/fs/lustre/ldlm/services/ldlm_cbd/threads_started
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows actual number of ldlm callback threads running.
+
+What:		/sys/fs/lustre/ldlm/services/ldlm_cbd/high_priority_ratio
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls what percentage of ldlm callback threads is dedicated
+		to "high priority" incoming requests.
+
+What:		/sys/fs/lustre/{obdtype}/{connection_name}/blocksize
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Blocksize on backend filesystem for service behind this obd
+		device (or biggest blocksize for compound devices like lov
+		and lmv)
+
+What:		/sys/fs/lustre/{obdtype}/{connection_name}/kbytestotal
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Total number of kilobytes of space on backend filesystem
+		for service behind this obd (or total amount for compound
+		devices like lov lmv)
+
+What:		/sys/fs/lustre/{obdtype}/{connection_name}/kbytesfree
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of free kilobytes on backend filesystem for service
+		behind this obd (or total amount for compound devices
+		like lov lmv)
+
+What:		/sys/fs/lustre/{obdtype}/{connection_name}/kbytesavail
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of kilobytes of free space on backend filesystem
+		for service behind this obd (or total amount for compound
+		devices like lov lmv) that is actually available for use
+		(taking into account per-client and filesystem reservations).
+
+What:		/sys/fs/lustre/{obdtype}/{connection_name}/filestotal
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of inodes on backend filesystem for service behind this
+		obd.
+
+What:		/sys/fs/lustre/{obdtype}/{connection_name}/filesfree
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of free inodes on backend filesystem for service
+		behind this obd.
+
+What:		/sys/fs/lustre/mdc/{connection_name}/max_pages_per_rpc
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Maximum number of readdir pages to fit into a single readdir
+		RPC.
+
+What:		/sys/fs/lustre/{mdc,osc}/{connection_name}/max_rpcs_in_flight
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Maximum number of parallel RPCs on the wire to allow on
+		this connection. Increasing this number would help on higher
+		latency links, but has a chance of overloading a server
+		if you have too many clients like this.
+		Default: 8
+
+What:		/sys/fs/lustre/osc/{connection_name}/max_pages_per_rpc
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Maximum number of pages to fit into a single RPC.
+		Typically bigger RPCs allow for better performance.
+		Default: however many pages to form 1M of data (256 pages
+		for 4K page sized platforms)
+
+What:		/sys/fs/lustre/osc/{connection_name}/active
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls accessibility of this connection. If set to 0,
+		fail all accesses immediately.
+
+What:		/sys/fs/lustre/osc/{connection_name}/checksums
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls whenever to checksum bulk RPC data over the wire
+		to this target.
+		1: enable (default) ; 0: disable
+
+What:		/sys/fs/lustre/osc/{connection_name}/contention_seconds
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls for how long to consider a file contended once
+		indicated as such by the server.
+		When a file is considered contended, all operations switch to
+		synchronous lockless mode to avoid cache and lock pingpong.
+
+What:		/sys/fs/lustre/osc/{connection_name}/cur_dirty_bytes
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Displays how many dirty bytes is presently in the cache for this
+		target.
+
+What:		/sys/fs/lustre/osc/{connection_name}/cur_grant_bytes
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows how many bytes we have as a "dirty cache" grant from the
+		server. Writing a value smaller than shown allows to release
+		some grant back to the server.
+		Dirty cache grant is a way Lustre ensures that cached successful
+		writes on client do not end up discarded by the server due to
+		lack of space later on.
+
+What:		/sys/fs/lustre/osc/{connection_name}/cur_lost_grant_bytes
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Shows how many granted bytes were released to the server due
+		to lack of write activity on this client.
+
+What:		/sys/fs/lustre/osc/{connection_name}/grant_shrink_interval
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of seconds with no write activity for this target
+		to start releasing dirty grant back to the server.
+
+What:		/sys/fs/lustre/osc/{connection_name}/destroys_in_flight
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of DESTROY RPCs currently in flight to this target.
+
+What:		/sys/fs/lustre/osc/{connection_name}/lockless_truncate
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls whether lockless truncate RPCs are allowed to this
+		target.
+		Lockless truncate causes server to perform the locking which
+		is beneficial if the truncate is not followed by a write
+		immediately.
+		1: enable ; 0: disable (default)
+
+What:		/sys/fs/lustre/osc/{connection_name}/max_dirty_mb
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls how much dirty data this client can accumulate
+		for this target. This is orthogonal to dirty grant and is
+		a hard limit even if the server would allow a bigger dirty
+		cache.
+		While allowing higher dirty cache is beneficial for write
+		performance, flushing write cache takes longer and as such
+		the node might be more prone to OOMs.
+		Having this value set too low might result in not being able
+		to sent too many parallel WRITE RPCs.
+		Default: 32
+
+What:		/sys/fs/lustre/osc/{connection_name}/resend_count
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Controls how many times to try and resend RPCs to this target
+		that failed with "recoverable" status, such as EAGAIN,
+		ENOMEM.
+
+What:		/sys/fs/lustre/lov/{connection_name}/numobd
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of OSC targets managed by this LOV instance.
+
+What:		/sys/fs/lustre/lov/{connection_name}/activeobd
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of OSC targets managed by this LOV instance that are
+		actually active.
+
+What:		/sys/fs/lustre/lmv/{connection_name}/numobd
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of MDC targets managed by this LMV instance.
+
+What:		/sys/fs/lustre/lmv/{connection_name}/activeobd
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Number of MDC targets managed by this LMV instance that are
+		actually active.
+
+What:		/sys/fs/lustre/lmv/{connection_name}/placement
+Date:		May 2015
+Contact:	"Oleg Drokin" <oleg.drokin@intel.com>
+Description:
+		Determines policy of inode placement in case of multiple
+		metadata servers:
+		CHAR - based on a hash of the file name used at creation time
+		       (Default)
+		NID - based on a hash of creating client network id.
diff --git a/drivers/staging/media/Kconfig b/drivers/staging/media/Kconfig
index 96498b7fc20e..14697686eea5 100644
--- a/drivers/staging/media/Kconfig
+++ b/drivers/staging/media/Kconfig
@@ -25,8 +25,6 @@ source "drivers/staging/media/cxd2099/Kconfig"
 
 source "drivers/staging/media/davinci_vpfe/Kconfig"
 
-source "drivers/staging/media/dt3155v4l/Kconfig"
-
 source "drivers/staging/media/mn88472/Kconfig"
 
 source "drivers/staging/media/mn88473/Kconfig"
diff --git a/drivers/staging/media/Makefile b/drivers/staging/media/Makefile
index a9006bcb4472..34c557b4c6d6 100644
--- a/drivers/staging/media/Makefile
+++ b/drivers/staging/media/Makefile
@@ -1,7 +1,6 @@
 obj-$(CONFIG_I2C_BCM2048)	+= bcm2048/
 obj-$(CONFIG_DVB_CXD2099)	+= cxd2099/
 obj-$(CONFIG_LIRC_STAGING)	+= lirc/
-obj-$(CONFIG_VIDEO_DT3155)	+= dt3155v4l/
 obj-$(CONFIG_VIDEO_DM365_VPFE)	+= davinci_vpfe/
 obj-$(CONFIG_VIDEO_OMAP4)	+= omap4iss/
 obj-$(CONFIG_DVB_MN88472)       += mn88472/
diff --git a/drivers/staging/media/bcm2048/radio-bcm2048.c b/drivers/staging/media/bcm2048/radio-bcm2048.c
index e9d0691b21d3..8bc68e2b4052 100644
--- a/drivers/staging/media/bcm2048/radio-bcm2048.c
+++ b/drivers/staging/media/bcm2048/radio-bcm2048.c
@@ -217,7 +217,7 @@
 #define BCM2048_FREQ_ERROR_FLOOR	-20
 #define BCM2048_FREQ_ERROR_ROOF		20
 
-/* -60 dB is reported as full signal strenght */
+/* -60 dB is reported as full signal strength */
 #define BCM2048_RSSI_LEVEL_BASE		-60
 #define BCM2048_RSSI_LEVEL_ROOF		-100
 #define BCM2048_RSSI_LEVEL_ROOF_NEG	100
@@ -2468,7 +2468,7 @@ static int bcm2048_vidioc_g_tuner(struct file *file, void *priv,
 		} else {
 			/*
 			 * RSSI level -60 dB is defined to report full
-			 * signal strenght
+			 * signal strength
 			 */
 			rssi = bcm2048_get_fm_rssi(bdev);
 			if (rssi >= BCM2048_RSSI_LEVEL_BASE) {
@@ -2593,7 +2593,7 @@ static int bcm2048_i2c_driver_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
 	struct bcm2048_device *bdev;
-	int err, skip_release = 0;
+	int err;
 
 	bdev = kzalloc(sizeof(*bdev), GFP_KERNEL);
 	if (!bdev) {
@@ -2646,7 +2646,6 @@ free_sysfs:
 	bcm2048_sysfs_unregister_properties(bdev, ARRAY_SIZE(attrs));
 free_registration:
 	video_unregister_device(&bdev->videodev);
-	skip_release = 1;
 free_irq:
 	if (client->irq)
 		free_irq(client->irq, bdev);
diff --git a/drivers/staging/media/davinci_vpfe/dm365_resizer.c b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
index b6498137de56..acb293ed9c91 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_resizer.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
@@ -321,6 +321,7 @@ static int resizer_configure_output_win(struct vpfe_resizer_device *resizer)
 
 	outformat = &resizer->resizer_a.formats[RESIZER_PAD_SOURCE];
 
+	memset(&output_specs, 0x0, sizeof(struct vpfe_rsz_output_spec));
 	output_specs.vst_y = param->user_config.vst;
 	if (outformat->code == MEDIA_BUS_FMT_YDYUYDYV8_1X16)
 		output_specs.vst_c = param->user_config.vst;
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.h b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.h
index 2632a806c4a8..8ad8d743f4e0 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.h
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.h
@@ -67,8 +67,6 @@ struct vpfe_device {
 	/* CCDC IRQs used when CCDC/ISIF output to SDRAM */
 	unsigned int			ccdc_irq0;
 	unsigned int			ccdc_irq1;
-	/* maximum video memory that is available*/
-	unsigned int			video_limit;
 	/* media device */
 	struct media_device		media_dev;
 	/* ccdc subdevice */
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.c b/drivers/staging/media/davinci_vpfe/vpfe_video.c
index 06d48d5eb0a0..87048a14c34d 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.c
@@ -27,9 +27,6 @@
 #include "vpfe.h"
 #include "vpfe_mc_capture.h"
 
-/* minimum number of buffers needed in cont-mode */
-#define MIN_NUM_BUFFERS			3
-
 static int debug;
 
 /* get v4l2 subdev pointer to external subdev which is active */
@@ -473,7 +470,7 @@ void vpfe_video_process_buffer_complete(struct vpfe_video_device *video)
 {
 	struct vpfe_pipeline *pipe = &video->pipe;
 
-	do_gettimeofday(&video->cur_frm->vb.v4l2_buf.timestamp);
+	v4l2_get_timestamp(&video->cur_frm->vb.v4l2_buf.timestamp);
 	vb2_buffer_done(&video->cur_frm->vb, VB2_BUF_STATE_DONE);
 	if (pipe->state == VPFE_PIPELINE_STREAM_CONTINUOUS)
 		video->cur_frm = video->next_frm;
@@ -1088,20 +1085,14 @@ vpfe_buffer_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
 	struct vpfe_fh *fh = vb2_get_drv_priv(vq);
 	struct vpfe_video_device *video = fh->video;
 	struct vpfe_device *vpfe_dev = video->vpfe_dev;
-	struct vpfe_pipeline *pipe = &video->pipe;
 	unsigned long size;
 
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_buffer_queue_setup\n");
 	size = video->fmt.fmt.pix.sizeimage;
 
-	if (vpfe_dev->video_limit) {
-		while (size * *nbuffers > vpfe_dev->video_limit)
-			(*nbuffers)--;
-	}
-	if (pipe->state == VPFE_PIPELINE_STREAM_CONTINUOUS) {
-		if (*nbuffers < MIN_NUM_BUFFERS)
-			*nbuffers = MIN_NUM_BUFFERS;
-	}
+	if (vq->num_buffers + *nbuffers < 3)
+		*nbuffers = 3 - vq->num_buffers;
+
 	*nplanes = 1;
 	sizes[0] = size;
 	alloc_ctxs[0] = video->alloc_ctx;
@@ -1346,6 +1337,7 @@ static int vpfe_reqbufs(struct file *file, void *priv,
 	q->ops = &video_qops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct vpfe_cap_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 
 	ret = vb2_queue_init(q);
 	if (ret) {
diff --git a/drivers/staging/media/dt3155v4l/Kconfig b/drivers/staging/media/dt3155v4l/Kconfig
deleted file mode 100644
index 2d496001b6e8..000000000000
--- a/drivers/staging/media/dt3155v4l/Kconfig
+++ /dev/null
@@ -1,29 +0,0 @@
-config VIDEO_DT3155
-	tristate "DT3155 frame grabber, Video4Linux interface"
-	depends on PCI && VIDEO_DEV && VIDEO_V4L2
-	depends on HAS_DMA
-	select VIDEOBUF2_DMA_CONTIG
-	default n
-	---help---
-	  Enables dt3155 device driver for the DataTranslation DT3155 frame grabber.
-	  Say Y here if you have this hardware.
-	  In doubt, say N.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called dt3155v4l.
-
-config DT3155_CCIR
-	bool "Selects CCIR/50Hz vertical refresh"
-	depends on VIDEO_DT3155
-	default y
-	---help---
-	  Select it for CCIR/50Hz (European region),
-	  or leave it unselected for RS-170/60Hz (North America).
-
-config DT3155_STREAMING
-	bool "Selects streaming capture method"
-	depends on VIDEO_DT3155
-	default y
-	---help---
-	  Select it if you want to use streaming of memory mapped buffers
-	  or leave it unselected if you want to use read method (one copy more).
diff --git a/drivers/staging/media/dt3155v4l/Makefile b/drivers/staging/media/dt3155v4l/Makefile
deleted file mode 100644
index ce7a3ec2faf3..000000000000
--- a/drivers/staging/media/dt3155v4l/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-obj-$(CONFIG_VIDEO_DT3155)	+= dt3155v4l.o
diff --git a/drivers/staging/media/dt3155v4l/dt3155v4l.c b/drivers/staging/media/dt3155v4l/dt3155v4l.c
deleted file mode 100644
index 52a8ffe560b1..000000000000
--- a/drivers/staging/media/dt3155v4l/dt3155v4l.c
+++ /dev/null
@@ -1,981 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 2006-2010 by Marin Mitov                                *
- *   mitov@issp.bas.bg                                                     *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-
-#include <linux/module.h>
-#include <linux/version.h>
-#include <linux/stringify.h>
-#include <linux/delay.h>
-#include <linux/kthread.h>
-#include <linux/slab.h>
-#include <media/v4l2-dev.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-common.h>
-#include <media/videobuf2-dma-contig.h>
-
-#include "dt3155v4l.h"
-
-#define DT3155_DEVICE_ID 0x1223
-
-/* DT3155_CHUNK_SIZE is 4M (2^22) 8 full size buffers */
-#define DT3155_CHUNK_SIZE (1U << 22)
-
-#define DT3155_COH_FLAGS (GFP_KERNEL | GFP_DMA32 | __GFP_COLD | __GFP_NOWARN)
-
-#define DT3155_BUF_SIZE (768 * 576)
-
-#ifdef CONFIG_DT3155_STREAMING
-#define DT3155_CAPTURE_METHOD V4L2_CAP_STREAMING
-#else
-#define DT3155_CAPTURE_METHOD V4L2_CAP_READWRITE
-#endif
-
-/*  global initializers (for all boards)  */
-#ifdef CONFIG_DT3155_CCIR
-static const u8 csr2_init = VT_50HZ;
-#define DT3155_CURRENT_NORM V4L2_STD_625_50
-static const unsigned int img_width = 768;
-static const unsigned int img_height = 576;
-static const unsigned int frames_per_sec = 25;
-static const struct v4l2_fmtdesc frame_std[] = {
-	{
-	.index = 0,
-	.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
-	.flags = 0,
-	.description = "CCIR/50Hz 8 bits gray",
-	.pixelformat = V4L2_PIX_FMT_GREY,
-	},
-};
-#else
-static const u8 csr2_init = VT_60HZ;
-#define DT3155_CURRENT_NORM V4L2_STD_525_60
-static const unsigned int img_width = 640;
-static const unsigned int img_height = 480;
-static const unsigned int frames_per_sec = 30;
-static const struct v4l2_fmtdesc frame_std[] = {
-	{
-	.index = 0,
-	.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
-	.flags = 0,
-	.description = "RS-170/60Hz 8 bits gray",
-	.pixelformat = V4L2_PIX_FMT_GREY,
-	},
-};
-#endif
-
-#define NUM_OF_FORMATS ARRAY_SIZE(frame_std)
-
-static u8 config_init = ACQ_MODE_EVEN;
-
-/**
- * read_i2c_reg - reads an internal i2c register
- *
- * @addr:	dt3155 mmio base address
- * @index:	index (internal address) of register to read
- * @data:	pointer to byte the read data will be placed in
- *
- * returns:	zero on success or error code
- *
- * This function starts reading the specified (by index) register
- * and busy waits for the process to finish. The result is placed
- * in a byte pointed by data.
- */
-static int
-read_i2c_reg(void __iomem *addr, u8 index, u8 *data)
-{
-	u32 tmp = index;
-
-	iowrite32((tmp<<17) | IIC_READ, addr + IIC_CSR2);
-	mmiowb();
-	udelay(45); /* wait at least 43 usec for NEW_CYCLE to clear */
-	if (ioread32(addr + IIC_CSR2) & NEW_CYCLE)
-		return -EIO; /* error: NEW_CYCLE not cleared */
-	tmp = ioread32(addr + IIC_CSR1);
-	if (tmp & DIRECT_ABORT) {
-		/* reset DIRECT_ABORT bit */
-		iowrite32(DIRECT_ABORT, addr + IIC_CSR1);
-		return -EIO; /* error: DIRECT_ABORT set */
-	}
-	*data = tmp>>24;
-	return 0;
-}
-
-/**
- * write_i2c_reg - writes to an internal i2c register
- *
- * @addr:	dt3155 mmio base address
- * @index:	index (internal address) of register to read
- * @data:	data to be written
- *
- * returns:	zero on success or error code
- *
- * This function starts writting the specified (by index) register
- * and busy waits for the process to finish.
- */
-static int
-write_i2c_reg(void __iomem *addr, u8 index, u8 data)
-{
-	u32 tmp = index;
-
-	iowrite32((tmp<<17) | IIC_WRITE | data, addr + IIC_CSR2);
-	mmiowb();
-	udelay(65); /* wait at least 63 usec for NEW_CYCLE to clear */
-	if (ioread32(addr + IIC_CSR2) & NEW_CYCLE)
-		return -EIO; /* error: NEW_CYCLE not cleared */
-	if (ioread32(addr + IIC_CSR1) & DIRECT_ABORT) {
-		/* reset DIRECT_ABORT bit */
-		iowrite32(DIRECT_ABORT, addr + IIC_CSR1);
-		return -EIO; /* error: DIRECT_ABORT set */
-	}
-	return 0;
-}
-
-/**
- * write_i2c_reg_nowait - writes to an internal i2c register
- *
- * @addr:	dt3155 mmio base address
- * @index:	index (internal address) of register to read
- * @data:	data to be written
- *
- * This function starts writting the specified (by index) register
- * and then returns.
- */
-static void write_i2c_reg_nowait(void __iomem *addr, u8 index, u8 data)
-{
-	u32 tmp = index;
-
-	iowrite32((tmp<<17) | IIC_WRITE | data, addr + IIC_CSR2);
-	mmiowb();
-}
-
-/**
- * wait_i2c_reg - waits the read/write to finish
- *
- * @addr:	dt3155 mmio base address
- *
- * returns:	zero on success or error code
- *
- * This function waits reading/writting to finish.
- */
-static int wait_i2c_reg(void __iomem *addr)
-{
-	if (ioread32(addr + IIC_CSR2) & NEW_CYCLE)
-		udelay(65); /* wait at least 63 usec for NEW_CYCLE to clear */
-	if (ioread32(addr + IIC_CSR2) & NEW_CYCLE)
-		return -EIO; /* error: NEW_CYCLE not cleared */
-	if (ioread32(addr + IIC_CSR1) & DIRECT_ABORT) {
-		/* reset DIRECT_ABORT bit */
-		iowrite32(DIRECT_ABORT, addr + IIC_CSR1);
-		return -EIO; /* error: DIRECT_ABORT set */
-	}
-	return 0;
-}
-
-static int
-dt3155_start_acq(struct dt3155_priv *pd)
-{
-	struct vb2_buffer *vb = pd->curr_buf;
-	dma_addr_t dma_addr;
-
-	dma_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
-	iowrite32(dma_addr, pd->regs + EVEN_DMA_START);
-	iowrite32(dma_addr + img_width, pd->regs + ODD_DMA_START);
-	iowrite32(img_width, pd->regs + EVEN_DMA_STRIDE);
-	iowrite32(img_width, pd->regs + ODD_DMA_STRIDE);
-	/* enable interrupts, clear all irq flags */
-	iowrite32(FLD_START_EN | FLD_END_ODD_EN | FLD_START |
-			FLD_END_EVEN | FLD_END_ODD, pd->regs + INT_CSR);
-	iowrite32(FIFO_EN | SRST | FLD_CRPT_ODD | FLD_CRPT_EVEN |
-		  FLD_DN_ODD | FLD_DN_EVEN | CAP_CONT_EVEN | CAP_CONT_ODD,
-							pd->regs + CSR1);
-	wait_i2c_reg(pd->regs);
-	write_i2c_reg(pd->regs, CONFIG, pd->config);
-	write_i2c_reg(pd->regs, EVEN_CSR, CSR_ERROR | CSR_DONE);
-	write_i2c_reg(pd->regs, ODD_CSR, CSR_ERROR | CSR_DONE);
-
-	/*  start the board  */
-	write_i2c_reg(pd->regs, CSR2, pd->csr2 | BUSY_EVEN | BUSY_ODD);
-	return 0; /* success  */
-}
-
-/*
- *	driver-specific callbacks (vb2_ops)
- */
-static int
-dt3155_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
-		unsigned int *num_buffers, unsigned int *num_planes,
-		unsigned int sizes[], void *alloc_ctxs[])
-
-{
-	struct dt3155_priv *pd = vb2_get_drv_priv(q);
-	void *ret;
-
-	if (*num_buffers == 0)
-		*num_buffers = 1;
-	*num_planes = 1;
-	sizes[0] = img_width * img_height;
-	if (pd->q->alloc_ctx[0])
-		return 0;
-	ret = vb2_dma_contig_init_ctx(&pd->pdev->dev);
-	if (IS_ERR(ret))
-		return PTR_ERR(ret);
-	pd->q->alloc_ctx[0] = ret;
-	return 0;
-}
-
-static void
-dt3155_wait_prepare(struct vb2_queue *q)
-{
-	struct dt3155_priv *pd = vb2_get_drv_priv(q);
-
-	mutex_unlock(pd->vdev.lock);
-}
-
-static void
-dt3155_wait_finish(struct vb2_queue *q)
-{
-	struct dt3155_priv *pd = vb2_get_drv_priv(q);
-
-	mutex_lock(pd->vdev.lock);
-}
-
-static int
-dt3155_buf_prepare(struct vb2_buffer *vb)
-{
-	vb2_set_plane_payload(vb, 0, img_width * img_height);
-	return 0;
-}
-
-static void
-dt3155_stop_streaming(struct vb2_queue *q)
-{
-	struct dt3155_priv *pd = vb2_get_drv_priv(q);
-	struct vb2_buffer *vb;
-
-	spin_lock_irq(&pd->lock);
-	while (!list_empty(&pd->dmaq)) {
-		vb = list_first_entry(&pd->dmaq, typeof(*vb), done_entry);
-		list_del(&vb->done_entry);
-		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
-	}
-	spin_unlock_irq(&pd->lock);
-	msleep(45); /* irq hendler will stop the hardware */
-}
-
-static void
-dt3155_buf_queue(struct vb2_buffer *vb)
-{
-	struct dt3155_priv *pd = vb2_get_drv_priv(vb->vb2_queue);
-
-	/*  pd->q->streaming = 1 when dt3155_buf_queue() is invoked  */
-	spin_lock_irq(&pd->lock);
-	if (pd->curr_buf)
-		list_add_tail(&vb->done_entry, &pd->dmaq);
-	else {
-		pd->curr_buf = vb;
-		dt3155_start_acq(pd);
-	}
-	spin_unlock_irq(&pd->lock);
-}
-/*
- *	end driver-specific callbacks
- */
-
-static const struct vb2_ops q_ops = {
-	.queue_setup = dt3155_queue_setup,
-	.wait_prepare = dt3155_wait_prepare,
-	.wait_finish = dt3155_wait_finish,
-	.buf_prepare = dt3155_buf_prepare,
-	.stop_streaming = dt3155_stop_streaming,
-	.buf_queue = dt3155_buf_queue,
-};
-
-static irqreturn_t
-dt3155_irq_handler_even(int irq, void *dev_id)
-{
-	struct dt3155_priv *ipd = dev_id;
-	struct vb2_buffer *ivb;
-	dma_addr_t dma_addr;
-	u32 tmp;
-
-	tmp = ioread32(ipd->regs + INT_CSR) & (FLD_START | FLD_END_ODD);
-	if (!tmp)
-		return IRQ_NONE;  /* not our irq */
-	if ((tmp & FLD_START) && !(tmp & FLD_END_ODD)) {
-		iowrite32(FLD_START_EN | FLD_END_ODD_EN | FLD_START,
-							ipd->regs + INT_CSR);
-		ipd->field_count++;
-		return IRQ_HANDLED; /* start of field irq */
-	}
-	if ((tmp & FLD_START) && (tmp & FLD_END_ODD))
-		ipd->stats.start_before_end++;
-	/*	check for corrupted fields     */
-/*	write_i2c_reg(ipd->regs, EVEN_CSR, CSR_ERROR | CSR_DONE);	*/
-/*	write_i2c_reg(ipd->regs, ODD_CSR, CSR_ERROR | CSR_DONE);	*/
-	tmp = ioread32(ipd->regs + CSR1) & (FLD_CRPT_EVEN | FLD_CRPT_ODD);
-	if (tmp) {
-		ipd->stats.corrupted_fields++;
-		iowrite32(FIFO_EN | SRST | FLD_CRPT_ODD | FLD_CRPT_EVEN |
-						FLD_DN_ODD | FLD_DN_EVEN |
-						CAP_CONT_EVEN | CAP_CONT_ODD,
-							ipd->regs + CSR1);
-		mmiowb();
-	}
-
-	spin_lock(&ipd->lock);
-	if (ipd->curr_buf) {
-		v4l2_get_timestamp(&ipd->curr_buf->v4l2_buf.timestamp);
-		ipd->curr_buf->v4l2_buf.sequence = (ipd->field_count) >> 1;
-		vb2_buffer_done(ipd->curr_buf, VB2_BUF_STATE_DONE);
-	}
-
-	if (!ipd->q->streaming || list_empty(&ipd->dmaq))
-		goto stop_dma;
-	ivb = list_first_entry(&ipd->dmaq, typeof(*ivb), done_entry);
-	list_del(&ivb->done_entry);
-	ipd->curr_buf = ivb;
-	dma_addr = vb2_dma_contig_plane_dma_addr(ivb, 0);
-	iowrite32(dma_addr, ipd->regs + EVEN_DMA_START);
-	iowrite32(dma_addr + img_width, ipd->regs + ODD_DMA_START);
-	iowrite32(img_width, ipd->regs + EVEN_DMA_STRIDE);
-	iowrite32(img_width, ipd->regs + ODD_DMA_STRIDE);
-	mmiowb();
-	/* enable interrupts, clear all irq flags */
-	iowrite32(FLD_START_EN | FLD_END_ODD_EN | FLD_START |
-			FLD_END_EVEN | FLD_END_ODD, ipd->regs + INT_CSR);
-	spin_unlock(&ipd->lock);
-	return IRQ_HANDLED;
-
-stop_dma:
-	ipd->curr_buf = NULL;
-	/* stop the board */
-	write_i2c_reg_nowait(ipd->regs, CSR2, ipd->csr2);
-	iowrite32(FIFO_EN | SRST | FLD_CRPT_ODD | FLD_CRPT_EVEN |
-		  FLD_DN_ODD | FLD_DN_EVEN, ipd->regs + CSR1);
-	/* disable interrupts, clear all irq flags */
-	iowrite32(FLD_START | FLD_END_EVEN | FLD_END_ODD, ipd->regs + INT_CSR);
-	spin_unlock(&ipd->lock);
-	return IRQ_HANDLED;
-}
-
-static int
-dt3155_open(struct file *filp)
-{
-	int ret = 0;
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	if (mutex_lock_interruptible(&pd->mux))
-		return -ERESTARTSYS;
-	if (!pd->users) {
-		pd->q = kzalloc(sizeof(*pd->q), GFP_KERNEL);
-		if (!pd->q) {
-			ret = -ENOMEM;
-			goto err_alloc_queue;
-		}
-		pd->q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		pd->q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-		pd->q->io_modes = VB2_READ | VB2_MMAP;
-		pd->q->ops = &q_ops;
-		pd->q->mem_ops = &vb2_dma_contig_memops;
-		pd->q->drv_priv = pd;
-		pd->curr_buf = NULL;
-		pd->field_count = 0;
-		ret = vb2_queue_init(pd->q);
-		if (ret < 0)
-			goto err_request_irq;
-		INIT_LIST_HEAD(&pd->dmaq);
-		spin_lock_init(&pd->lock);
-		/* disable all irqs, clear all irq flags */
-		iowrite32(FLD_START | FLD_END_EVEN | FLD_END_ODD,
-						pd->regs + INT_CSR);
-		ret = request_irq(pd->pdev->irq, dt3155_irq_handler_even,
-						IRQF_SHARED, DT3155_NAME, pd);
-		if (ret)
-			goto err_request_irq;
-	}
-	pd->users++;
-	mutex_unlock(&pd->mux);
-	return 0; /* success */
-err_request_irq:
-	kfree(pd->q);
-	pd->q = NULL;
-err_alloc_queue:
-	mutex_unlock(&pd->mux);
-	return ret;
-}
-
-static int
-dt3155_release(struct file *filp)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	mutex_lock(&pd->mux);
-	pd->users--;
-	BUG_ON(pd->users < 0);
-	if (!pd->users) {
-		vb2_queue_release(pd->q);
-		free_irq(pd->pdev->irq, pd);
-		if (pd->q->alloc_ctx[0])
-			vb2_dma_contig_cleanup_ctx(pd->q->alloc_ctx[0]);
-		kfree(pd->q);
-		pd->q = NULL;
-	}
-	mutex_unlock(&pd->mux);
-	return 0;
-}
-
-static ssize_t
-dt3155_read(struct file *filp, char __user *user, size_t size, loff_t *loff)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-	ssize_t res;
-
-	if (mutex_lock_interruptible(&pd->mux))
-		return -ERESTARTSYS;
-	res = vb2_read(pd->q, user, size, loff, filp->f_flags & O_NONBLOCK);
-	mutex_unlock(&pd->mux);
-	return res;
-}
-
-static unsigned int
-dt3155_poll(struct file *filp, struct poll_table_struct *polltbl)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-	unsigned int res;
-
-	mutex_lock(&pd->mux);
-	res = vb2_poll(pd->q, filp, polltbl);
-	mutex_unlock(&pd->mux);
-	return res;
-}
-
-static int
-dt3155_mmap(struct file *filp, struct vm_area_struct *vma)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-	int res;
-
-	if (mutex_lock_interruptible(&pd->mux))
-		return -ERESTARTSYS;
-	res = vb2_mmap(pd->q, vma);
-	mutex_unlock(&pd->mux);
-	return res;
-}
-
-static const struct v4l2_file_operations dt3155_fops = {
-	.owner = THIS_MODULE,
-	.open = dt3155_open,
-	.release = dt3155_release,
-	.read = dt3155_read,
-	.poll = dt3155_poll,
-	.unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
-	.mmap = dt3155_mmap,
-};
-
-static int
-dt3155_ioc_streamon(struct file *filp, void *p, enum v4l2_buf_type type)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	return vb2_streamon(pd->q, type);
-}
-
-static int
-dt3155_ioc_streamoff(struct file *filp, void *p, enum v4l2_buf_type type)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	return vb2_streamoff(pd->q, type);
-}
-
-static int
-dt3155_ioc_querycap(struct file *filp, void *p, struct v4l2_capability *cap)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	strcpy(cap->driver, DT3155_NAME);
-	strcpy(cap->card, DT3155_NAME " frame grabber");
-	sprintf(cap->bus_info, "PCI:%s", pci_name(pd->pdev));
-	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
-				DT3155_CAPTURE_METHOD;
-	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-	return 0;
-}
-
-static int
-dt3155_ioc_enum_fmt_vid_cap(struct file *filp, void *p, struct v4l2_fmtdesc *f)
-{
-	if (f->index >= NUM_OF_FORMATS)
-		return -EINVAL;
-	*f = frame_std[f->index];
-	return 0;
-}
-
-static int
-dt3155_ioc_g_fmt_vid_cap(struct file *filp, void *p, struct v4l2_format *f)
-{
-	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	f->fmt.pix.width = img_width;
-	f->fmt.pix.height = img_height;
-	f->fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
-	f->fmt.pix.field = V4L2_FIELD_NONE;
-	f->fmt.pix.bytesperline = f->fmt.pix.width;
-	f->fmt.pix.sizeimage = f->fmt.pix.width * f->fmt.pix.height;
-	f->fmt.pix.colorspace = 0;
-	f->fmt.pix.priv = 0;
-	return 0;
-}
-
-static int
-dt3155_ioc_try_fmt_vid_cap(struct file *filp, void *p, struct v4l2_format *f)
-{
-	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	if (f->fmt.pix.width == img_width &&
-		f->fmt.pix.height == img_height &&
-		f->fmt.pix.pixelformat == V4L2_PIX_FMT_GREY &&
-		f->fmt.pix.field == V4L2_FIELD_NONE &&
-		f->fmt.pix.bytesperline == f->fmt.pix.width &&
-		f->fmt.pix.sizeimage == f->fmt.pix.width * f->fmt.pix.height)
-			return 0;
-	else
-		return -EINVAL;
-}
-
-static int
-dt3155_ioc_s_fmt_vid_cap(struct file *filp, void *p, struct v4l2_format *f)
-{
-	return dt3155_ioc_g_fmt_vid_cap(filp, p, f);
-}
-
-static int
-dt3155_ioc_reqbufs(struct file *filp, void *p, struct v4l2_requestbuffers *b)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	return vb2_reqbufs(pd->q, b);
-}
-
-static int
-dt3155_ioc_querybuf(struct file *filp, void *p, struct v4l2_buffer *b)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	return vb2_querybuf(pd->q, b);
-}
-
-static int
-dt3155_ioc_qbuf(struct file *filp, void *p, struct v4l2_buffer *b)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	return vb2_qbuf(pd->q, b);
-}
-
-static int
-dt3155_ioc_dqbuf(struct file *filp, void *p, struct v4l2_buffer *b)
-{
-	struct dt3155_priv *pd = video_drvdata(filp);
-
-	return vb2_dqbuf(pd->q, b, filp->f_flags & O_NONBLOCK);
-}
-
-static int
-dt3155_ioc_querystd(struct file *filp, void *p, v4l2_std_id *norm)
-{
-	*norm = DT3155_CURRENT_NORM;
-	return 0;
-}
-
-static int
-dt3155_ioc_g_std(struct file *filp, void *p, v4l2_std_id *norm)
-{
-	*norm = DT3155_CURRENT_NORM;
-	return 0;
-}
-
-static int
-dt3155_ioc_s_std(struct file *filp, void *p, v4l2_std_id norm)
-{
-	if (norm & DT3155_CURRENT_NORM)
-		return 0;
-	return -EINVAL;
-}
-
-static int
-dt3155_ioc_enum_input(struct file *filp, void *p, struct v4l2_input *input)
-{
-	if (input->index)
-		return -EINVAL;
-	strcpy(input->name, "Coax in");
-	input->type = V4L2_INPUT_TYPE_CAMERA;
-	/*
-	 * FIXME: input->std = 0 according to v4l2 API
-	 * VIDIOC_G_STD, VIDIOC_S_STD, VIDIOC_QUERYSTD and VIDIOC_ENUMSTD
-	 * should return -EINVAL
-	 */
-	input->std = DT3155_CURRENT_NORM;
-	input->status = 0;/* FIXME: add sync detection & V4L2_IN_ST_NO_H_LOCK */
-	return 0;
-}
-
-static int
-dt3155_ioc_g_input(struct file *filp, void *p, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-static int
-dt3155_ioc_s_input(struct file *filp, void *p, unsigned int i)
-{
-	if (i)
-		return -EINVAL;
-	return 0;
-}
-
-static int
-dt3155_ioc_g_parm(struct file *filp, void *p, struct v4l2_streamparm *parms)
-{
-	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	parms->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
-	parms->parm.capture.capturemode = 0;
-	parms->parm.capture.timeperframe.numerator = 1001;
-	parms->parm.capture.timeperframe.denominator = frames_per_sec * 1000;
-	parms->parm.capture.extendedmode = 0;
-	parms->parm.capture.readbuffers = 1; /* FIXME: 2 buffers? */
-	return 0;
-}
-
-static int
-dt3155_ioc_s_parm(struct file *filp, void *p, struct v4l2_streamparm *parms)
-{
-	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-	parms->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
-	parms->parm.capture.capturemode = 0;
-	parms->parm.capture.timeperframe.numerator = 1001;
-	parms->parm.capture.timeperframe.denominator = frames_per_sec * 1000;
-	parms->parm.capture.extendedmode = 0;
-	parms->parm.capture.readbuffers = 1; /* FIXME: 2 buffers? */
-	return 0;
-}
-
-static const struct v4l2_ioctl_ops dt3155_ioctl_ops = {
-	.vidioc_streamon = dt3155_ioc_streamon,
-	.vidioc_streamoff = dt3155_ioc_streamoff,
-	.vidioc_querycap = dt3155_ioc_querycap,
-/*
-	.vidioc_g_priority = dt3155_ioc_g_priority,
-	.vidioc_s_priority = dt3155_ioc_s_priority,
-*/
-	.vidioc_enum_fmt_vid_cap = dt3155_ioc_enum_fmt_vid_cap,
-	.vidioc_try_fmt_vid_cap = dt3155_ioc_try_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap = dt3155_ioc_g_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap = dt3155_ioc_s_fmt_vid_cap,
-	.vidioc_reqbufs = dt3155_ioc_reqbufs,
-	.vidioc_querybuf = dt3155_ioc_querybuf,
-	.vidioc_qbuf = dt3155_ioc_qbuf,
-	.vidioc_dqbuf = dt3155_ioc_dqbuf,
-	.vidioc_querystd = dt3155_ioc_querystd,
-	.vidioc_g_std = dt3155_ioc_g_std,
-	.vidioc_s_std = dt3155_ioc_s_std,
-	.vidioc_enum_input = dt3155_ioc_enum_input,
-	.vidioc_g_input = dt3155_ioc_g_input,
-	.vidioc_s_input = dt3155_ioc_s_input,
-/*
-	.vidioc_queryctrl = dt3155_ioc_queryctrl,
-	.vidioc_g_ctrl = dt3155_ioc_g_ctrl,
-	.vidioc_s_ctrl = dt3155_ioc_s_ctrl,
-	.vidioc_querymenu = dt3155_ioc_querymenu,
-	.vidioc_g_ext_ctrls = dt3155_ioc_g_ext_ctrls,
-	.vidioc_s_ext_ctrls = dt3155_ioc_s_ext_ctrls,
-*/
-	.vidioc_g_parm = dt3155_ioc_g_parm,
-	.vidioc_s_parm = dt3155_ioc_s_parm,
-/*
-	.vidioc_cropcap = dt3155_ioc_cropcap,
-	.vidioc_g_crop = dt3155_ioc_g_crop,
-	.vidioc_s_crop = dt3155_ioc_s_crop,
-	.vidioc_enum_framesizes = dt3155_ioc_enum_framesizes,
-	.vidioc_enum_frameintervals = dt3155_ioc_enum_frameintervals,
-*/
-};
-
-static int
-dt3155_init_board(struct pci_dev *pdev)
-{
-	struct dt3155_priv *pd = pci_get_drvdata(pdev);
-	void *buf_cpu;
-	dma_addr_t buf_dma;
-	int i;
-	u8 tmp;
-
-	pci_set_master(pdev); /* dt3155 needs it */
-
-	/*  resetting the adapter  */
-	iowrite32(FLD_CRPT_ODD | FLD_CRPT_EVEN | FLD_DN_ODD | FLD_DN_EVEN,
-							pd->regs + CSR1);
-	mmiowb();
-	msleep(20);
-
-	/*  initializing adaper registers  */
-	iowrite32(FIFO_EN | SRST, pd->regs + CSR1);
-	mmiowb();
-	iowrite32(0xEEEEEE01, pd->regs + EVEN_PIXEL_FMT);
-	iowrite32(0xEEEEEE01, pd->regs + ODD_PIXEL_FMT);
-	iowrite32(0x00000020, pd->regs + FIFO_TRIGER);
-	iowrite32(0x00000103, pd->regs + XFER_MODE);
-	iowrite32(0, pd->regs + RETRY_WAIT_CNT);
-	iowrite32(0, pd->regs + INT_CSR);
-	iowrite32(1, pd->regs + EVEN_FLD_MASK);
-	iowrite32(1, pd->regs + ODD_FLD_MASK);
-	iowrite32(0, pd->regs + MASK_LENGTH);
-	iowrite32(0x0005007C, pd->regs + FIFO_FLAG_CNT);
-	iowrite32(0x01010101, pd->regs + IIC_CLK_DUR);
-	mmiowb();
-
-	/* verifying that we have a DT3155 board (not just a SAA7116 chip) */
-	read_i2c_reg(pd->regs, DT_ID, &tmp);
-	if (tmp != DT3155_ID)
-		return -ENODEV;
-
-	/* initialize AD LUT */
-	write_i2c_reg(pd->regs, AD_ADDR, 0);
-	for (i = 0; i < 256; i++)
-		write_i2c_reg(pd->regs, AD_LUT, i);
-
-	/* initialize ADC references */
-	/* FIXME: pos_ref & neg_ref depend on VT_50HZ */
-	write_i2c_reg(pd->regs, AD_ADDR, AD_CMD_REG);
-	write_i2c_reg(pd->regs, AD_CMD, VIDEO_CNL_1 | SYNC_CNL_1 | SYNC_LVL_3);
-	write_i2c_reg(pd->regs, AD_ADDR, AD_POS_REF);
-	write_i2c_reg(pd->regs, AD_CMD, 34);
-	write_i2c_reg(pd->regs, AD_ADDR, AD_NEG_REF);
-	write_i2c_reg(pd->regs, AD_CMD, 0);
-
-	/* initialize PM LUT */
-	write_i2c_reg(pd->regs, CONFIG, pd->config | PM_LUT_PGM);
-	for (i = 0; i < 256; i++) {
-		write_i2c_reg(pd->regs, PM_LUT_ADDR, i);
-		write_i2c_reg(pd->regs, PM_LUT_DATA, i);
-	}
-	write_i2c_reg(pd->regs, CONFIG, pd->config | PM_LUT_PGM | PM_LUT_SEL);
-	for (i = 0; i < 256; i++) {
-		write_i2c_reg(pd->regs, PM_LUT_ADDR, i);
-		write_i2c_reg(pd->regs, PM_LUT_DATA, i);
-	}
-	write_i2c_reg(pd->regs, CONFIG, pd->config); /*  ACQ_MODE_EVEN  */
-
-	/* select channel 1 for input and set sync level */
-	write_i2c_reg(pd->regs, AD_ADDR, AD_CMD_REG);
-	write_i2c_reg(pd->regs, AD_CMD, VIDEO_CNL_1 | SYNC_CNL_1 | SYNC_LVL_3);
-
-	/* allocate memory, and initialize the DMA machine */
-	buf_cpu = dma_alloc_coherent(&pdev->dev, DT3155_BUF_SIZE, &buf_dma,
-								GFP_KERNEL);
-	if (!buf_cpu)
-		return -ENOMEM;
-	iowrite32(buf_dma, pd->regs + EVEN_DMA_START);
-	iowrite32(buf_dma, pd->regs + ODD_DMA_START);
-	iowrite32(0, pd->regs + EVEN_DMA_STRIDE);
-	iowrite32(0, pd->regs + ODD_DMA_STRIDE);
-
-	/*  Perform a pseudo even field acquire    */
-	iowrite32(FIFO_EN | SRST | CAP_CONT_ODD, pd->regs + CSR1);
-	write_i2c_reg(pd->regs, CSR2, pd->csr2 | SYNC_SNTL);
-	write_i2c_reg(pd->regs, CONFIG, pd->config);
-	write_i2c_reg(pd->regs, EVEN_CSR, CSR_SNGL);
-	write_i2c_reg(pd->regs, CSR2, pd->csr2 | BUSY_EVEN | SYNC_SNTL);
-	msleep(100);
-	read_i2c_reg(pd->regs, CSR2, &tmp);
-	write_i2c_reg(pd->regs, EVEN_CSR, CSR_ERROR | CSR_SNGL | CSR_DONE);
-	write_i2c_reg(pd->regs, ODD_CSR, CSR_ERROR | CSR_SNGL | CSR_DONE);
-	write_i2c_reg(pd->regs, CSR2, pd->csr2);
-	iowrite32(FIFO_EN | SRST | FLD_DN_EVEN | FLD_DN_ODD, pd->regs + CSR1);
-
-	/*  deallocate memory  */
-	dma_free_coherent(&pdev->dev, DT3155_BUF_SIZE, buf_cpu, buf_dma);
-	if (tmp & BUSY_EVEN)
-		return -EIO;
-	return 0;
-}
-
-static struct video_device dt3155_vdev = {
-	.name = DT3155_NAME,
-	.fops = &dt3155_fops,
-	.ioctl_ops = &dt3155_ioctl_ops,
-	.minor = -1,
-	.release = video_device_release_empty,
-	.tvnorms = DT3155_CURRENT_NORM,
-};
-
-/* same as in drivers/base/dma-coherent.c */
-struct dma_coherent_mem {
-	void		*virt_base;
-	dma_addr_t	device_base;
-	int		size;
-	int		flags;
-	unsigned long	*bitmap;
-};
-
-static int
-dt3155_alloc_coherent(struct device *dev, size_t size, int flags)
-{
-	struct dma_coherent_mem *mem;
-	dma_addr_t dev_base;
-	int pages = size >> PAGE_SHIFT;
-	int bitmap_size = BITS_TO_LONGS(pages) * sizeof(long);
-
-	if ((flags & DMA_MEMORY_MAP) == 0)
-		goto out;
-	if (!size)
-		goto out;
-	if (dev->dma_mem)
-		goto out;
-
-	mem = kzalloc(sizeof(*mem), GFP_KERNEL);
-	if (!mem)
-		goto out;
-	mem->virt_base = dma_alloc_coherent(dev, size, &dev_base,
-							DT3155_COH_FLAGS);
-	if (!mem->virt_base)
-		goto err_alloc_coherent;
-	mem->bitmap = kzalloc(bitmap_size, GFP_KERNEL);
-	if (!mem->bitmap)
-		goto err_bitmap;
-
-	/* coherent_dma_mask is already set to 32 bits */
-	mem->device_base = dev_base;
-	mem->size = pages;
-	mem->flags = flags;
-	dev->dma_mem = mem;
-	return DMA_MEMORY_MAP;
-
-err_bitmap:
-	dma_free_coherent(dev, size, mem->virt_base, dev_base);
-err_alloc_coherent:
-	kfree(mem);
-out:
-	return 0;
-}
-
-static void
-dt3155_free_coherent(struct device *dev)
-{
-	struct dma_coherent_mem *mem = dev->dma_mem;
-
-	if (!mem)
-		return;
-	dev->dma_mem = NULL;
-	dma_free_coherent(dev, mem->size << PAGE_SHIFT,
-					mem->virt_base, mem->device_base);
-	kfree(mem->bitmap);
-	kfree(mem);
-}
-
-static int
-dt3155_probe(struct pci_dev *pdev, const struct pci_device_id *id)
-{
-	int err;
-	struct dt3155_priv *pd;
-
-	err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
-	if (err)
-		return -ENODEV;
-	pd = devm_kzalloc(&pdev->dev, sizeof(*pd), GFP_KERNEL);
-	if (!pd)
-		return -ENOMEM;
-
-	pd->vdev = dt3155_vdev;
-	pci_set_drvdata(pdev, pd);    /* for use in dt3155_remove() */
-	video_set_drvdata(&pd->vdev, pd);  /* for use in video_fops */
-	pd->users = 0;
-	pd->pdev = pdev;
-	INIT_LIST_HEAD(&pd->dmaq);
-	mutex_init(&pd->mux);
-	pd->vdev.lock = &pd->mux; /* for locking v4l2_file_operations */
-	spin_lock_init(&pd->lock);
-	pd->csr2 = csr2_init;
-	pd->config = config_init;
-	err = pci_enable_device(pdev);
-	if (err)
-		return err;
-	err = pci_request_region(pdev, 0, pci_name(pdev));
-	if (err)
-		goto err_req_region;
-	pd->regs = pci_iomap(pdev, 0, pci_resource_len(pd->pdev, 0));
-	if (!pd->regs) {
-		err = -ENOMEM;
-		goto err_pci_iomap;
-	}
-	err = dt3155_init_board(pdev);
-	if (err)
-		goto err_init_board;
-	err = video_register_device(&pd->vdev, VFL_TYPE_GRABBER, -1);
-	if (err)
-		goto err_init_board;
-	if (dt3155_alloc_coherent(&pdev->dev, DT3155_CHUNK_SIZE,
-							DMA_MEMORY_MAP))
-		dev_info(&pdev->dev, "preallocated 8 buffers\n");
-	dev_info(&pdev->dev, "/dev/video%i is ready\n", pd->vdev.minor);
-	return 0;  /*   success   */
-
-err_init_board:
-	pci_iounmap(pdev, pd->regs);
-err_pci_iomap:
-	pci_release_region(pdev, 0);
-err_req_region:
-	pci_disable_device(pdev);
-	return err;
-}
-
-static void
-dt3155_remove(struct pci_dev *pdev)
-{
-	struct dt3155_priv *pd = pci_get_drvdata(pdev);
-
-	dt3155_free_coherent(&pdev->dev);
-	video_unregister_device(&pd->vdev);
-	pci_iounmap(pdev, pd->regs);
-	pci_release_region(pdev, 0);
-	pci_disable_device(pdev);
-}
-
-static const struct pci_device_id pci_ids[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, DT3155_DEVICE_ID) },
-	{ 0, /* zero marks the end */ },
-};
-MODULE_DEVICE_TABLE(pci, pci_ids);
-
-static struct pci_driver pci_driver = {
-	.name = DT3155_NAME,
-	.id_table = pci_ids,
-	.probe = dt3155_probe,
-	.remove = dt3155_remove,
-};
-
-module_pci_driver(pci_driver);
-
-MODULE_DESCRIPTION("video4linux pci-driver for dt3155 frame grabber");
-MODULE_AUTHOR("Marin Mitov <mitov@issp.bas.bg>");
-MODULE_VERSION(DT3155_VERSION);
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/lirc/lirc_imon.c b/drivers/staging/media/lirc/lirc_imon.c
index 335b98a54237..62ec9f70dae4 100644
--- a/drivers/staging/media/lirc/lirc_imon.c
+++ b/drivers/staging/media/lirc/lirc_imon.c
@@ -693,10 +693,9 @@ static int imon_probe(struct usb_interface *interface,
 	int ifnum;
 	int lirc_minor = 0;
 	int num_endpts;
-	int retval = 0;
+	int retval = -ENOMEM;
 	int display_ep_found = 0;
 	int ir_ep_found = 0;
-	int alloc_status = 0;
 	int vfd_proto_6p = 0;
 	struct imon_context *context = NULL;
 	int i;
@@ -706,10 +705,8 @@ static int imon_probe(struct usb_interface *interface,
 	mutex_lock(&driver_lock);
 
 	context = kzalloc(sizeof(struct imon_context), GFP_KERNEL);
-	if (!context) {
-		alloc_status = 1;
-		goto alloc_status_switch;
-	}
+	if (!context)
+		goto driver_unlock;
 
 	/*
 	 * Try to auto-detect the type of display if the user hasn't set
@@ -775,8 +772,7 @@ static int imon_probe(struct usb_interface *interface,
 		dev_err(dev, "%s: no valid input (IR) endpoint found.\n",
 			__func__);
 		retval = -ENODEV;
-		alloc_status = 2;
-		goto alloc_status_switch;
+		goto free_context;
 	}
 
 	/* Determine if display requires 6 packets */
@@ -790,31 +786,26 @@ static int imon_probe(struct usb_interface *interface,
 
 	driver = kzalloc(sizeof(struct lirc_driver), GFP_KERNEL);
 	if (!driver) {
-		alloc_status = 2;
-		goto alloc_status_switch;
+		goto free_context;
 	}
 	rbuf = kmalloc(sizeof(struct lirc_buffer), GFP_KERNEL);
 	if (!rbuf) {
-		alloc_status = 3;
-		goto alloc_status_switch;
+		goto free_driver;
 	}
 	if (lirc_buffer_init(rbuf, BUF_CHUNK_SIZE, BUF_SIZE)) {
 		dev_err(dev, "%s: lirc_buffer_init failed\n", __func__);
-		alloc_status = 4;
-		goto alloc_status_switch;
+		goto free_rbuf;
 	}
 	rx_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!rx_urb) {
 		dev_err(dev, "%s: usb_alloc_urb failed for IR urb\n", __func__);
-		alloc_status = 5;
-		goto alloc_status_switch;
+		goto free_lirc_buf;
 	}
 	tx_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!tx_urb) {
 		dev_err(dev, "%s: usb_alloc_urb failed for display urb\n",
 		    __func__);
-		alloc_status = 6;
-		goto alloc_status_switch;
+		goto free_rx_urb;
 	}
 
 	mutex_init(&context->ctx_lock);
@@ -840,11 +831,11 @@ static int imon_probe(struct usb_interface *interface,
 	lirc_minor = lirc_register_driver(driver);
 	if (lirc_minor < 0) {
 		dev_err(dev, "%s: lirc_register_driver failed\n", __func__);
-		alloc_status = 7;
-		goto unlock;
-	} else
-		dev_info(dev, "Registered iMON driver (lirc minor: %d)\n",
-			 lirc_minor);
+		goto free_tx_urb;
+	}
+
+	dev_info(dev, "Registered iMON driver (lirc minor: %d)\n",
+			lirc_minor);
 
 	/* Needed while unregistering! */
 	driver->minor = lirc_minor;
@@ -872,11 +863,9 @@ static int imon_probe(struct usb_interface *interface,
 		context->rx_endpoint->bInterval);
 
 	retval = usb_submit_urb(context->rx_urb, GFP_KERNEL);
-
 	if (retval) {
 		dev_err(dev, "usb_submit_urb failed for intf0 (%d)\n", retval);
-		alloc_status = 8;
-		goto unlock;
+		goto unregister_lirc;
 	}
 
 	usb_set_intfdata(interface, context);
@@ -895,39 +884,31 @@ static int imon_probe(struct usb_interface *interface,
 	dev_info(dev, "iMON device (%04x:%04x, intf%d) on usb<%d:%d> initialized\n",
 		vendor, product, ifnum, usbdev->bus->busnum, usbdev->devnum);
 
-unlock:
-	mutex_unlock(&context->ctx_lock);
-alloc_status_switch:
-
-	switch (alloc_status) {
-	case 8:
-		lirc_unregister_driver(driver->minor);
-	case 7:
-		usb_free_urb(tx_urb);
-	case 6:
-		usb_free_urb(rx_urb);
-		/* fall-through */
-	case 5:
-		if (rbuf)
-			lirc_buffer_free(rbuf);
-		/* fall-through */
-	case 4:
-		kfree(rbuf);
-		/* fall-through */
-	case 3:
-		kfree(driver);
-		/* fall-through */
-	case 2:
-		kfree(context);
-		context = NULL;
-	case 1:
-		if (retval != -ENODEV)
-			retval = -ENOMEM;
-		break;
-	case 0:
-		retval = 0;
-	}
+	/* Everything went fine. Just unlock and return retval (with is 0) */
+	goto driver_unlock;
+
+unregister_lirc:
+	lirc_unregister_driver(driver->minor);
+
+free_tx_urb:
+	usb_free_urb(tx_urb);
+
+free_rx_urb:
+	usb_free_urb(rx_urb);
+
+free_lirc_buf:
+	lirc_buffer_free(rbuf);
+
+free_rbuf:
+	kfree(rbuf);
+
+free_driver:
+	kfree(driver);
+free_context:
+	kfree(context);
+	context = NULL;
 
+driver_unlock:
 	mutex_unlock(&driver_lock);
 
 	return retval;
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index 29087f66e2f4..4f326e97ad75 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -44,7 +44,7 @@
 #include <linux/ioport.h>
 #include <linux/kernel.h>
 #include <linux/serial_reg.h>
-#include <linux/time.h>
+#include <linux/ktime.h>
 #include <linux/string.h>
 #include <linux/types.h>
 #include <linux/wait.h>
@@ -127,9 +127,9 @@ static int threshold = 3;
 static DEFINE_SPINLOCK(timer_lock);
 static struct timer_list timerlist;
 /* time of last signal change detected */
-static struct timeval last_tv = {0, 0};
+static ktime_t last;
 /* time of last UART data ready interrupt */
-static struct timeval last_intr_tv = {0, 0};
+static ktime_t last_intr_time;
 static int last_value;
 
 static DECLARE_WAIT_QUEUE_HEAD(lirc_read_queue);
@@ -400,20 +400,6 @@ static void drop_chrdev(void)
 }
 
 /* SECTION: Hardware */
-static long delta(struct timeval *tv1, struct timeval *tv2)
-{
-	unsigned long deltv;
-
-	deltv = tv2->tv_sec - tv1->tv_sec;
-	if (deltv > 15)
-		deltv = 0xFFFFFF;
-	else
-		deltv = deltv*1000000 +
-			tv2->tv_usec -
-			tv1->tv_usec;
-	return deltv;
-}
-
 static void sir_timeout(unsigned long data)
 {
 	/*
@@ -432,12 +418,14 @@ static void sir_timeout(unsigned long data)
 		/* clear unread bits in UART and restart */
 		outb(UART_FCR_CLEAR_RCVR, io + UART_FCR);
 		/* determine 'virtual' pulse end: */
-		pulse_end = delta(&last_tv, &last_intr_tv);
+		pulse_end = min_t(unsigned long,
+				  ktime_us_delta(last, last_intr_time),
+				  PULSE_MASK);
 		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;
+		last = last_intr_time;
 	}
 	spin_unlock_irqrestore(&timer_lock, flags);
 }
@@ -445,9 +433,9 @@ static void sir_timeout(unsigned long data)
 static irqreturn_t sir_interrupt(int irq, void *dev_id)
 {
 	unsigned char data;
-	struct timeval curr_tv;
-	static unsigned long deltv;
-	unsigned long deltintrtv;
+	ktime_t curr_time;
+	static unsigned long delt;
+	unsigned long deltintr;
 	unsigned long flags;
 	int iir, lsr;
 
@@ -471,49 +459,46 @@ static irqreturn_t sir_interrupt(int irq, void *dev_id)
 			do {
 				del_timer(&timerlist);
 				data = inb(io + UART_RX);
-				do_gettimeofday(&curr_tv);
-				deltv = delta(&last_tv, &curr_tv);
-				deltintrtv = delta(&last_intr_tv, &curr_tv);
+				curr_time = ktime_get();
+				delt = min_t(unsigned long,
+					     ktime_us_delta(last, curr_time),
+					     PULSE_MASK);
+				deltintr = min_t(unsigned long,
+						 ktime_us_delta(last_intr_time,
+								curr_time),
+						 PULSE_MASK);
 				dev_dbg(driver.dev, "t %lu, d %d\n",
-						    deltintrtv, (int)data);
+						    deltintr, (int)data);
 				/*
 				 * if nothing came in last X cycles,
 				 * it was gap
 				 */
-				if (deltintrtv > TIME_CONST * threshold) {
+				if (deltintr > TIME_CONST * threshold) {
 					if (last_value) {
 						dev_dbg(driver.dev, "GAP\n");
 						/* simulate signal change */
 						add_read_queue(last_value,
-							       deltv -
-							       deltintrtv);
+							       delt -
+							       deltintr);
 						last_value = 0;
-						last_tv.tv_sec =
-							last_intr_tv.tv_sec;
-						last_tv.tv_usec =
-							last_intr_tv.tv_usec;
-						deltv = deltintrtv;
+						last = last_intr_time;
+						delt = deltintr;
 					}
 				}
 				data = 1;
 				if (data ^ last_value) {
 					/*
-					 * deltintrtv > 2*TIME_CONST, remember?
+					 * deltintr > 2*TIME_CONST, remember?
 					 * the other case is timeout
 					 */
 					add_read_queue(last_value,
-						       deltv-TIME_CONST);
+						       delt-TIME_CONST);
 					last_value = data;
-					last_tv = curr_tv;
-					if (last_tv.tv_usec >= TIME_CONST) {
-						last_tv.tv_usec -= TIME_CONST;
-					} else {
-						last_tv.tv_sec--;
-						last_tv.tv_usec += 1000000 -
-							TIME_CONST;
-					}
+					last = curr_time;
+					last = ktime_sub_us(last,
+							    TIME_CONST);
 				}
-				last_intr_tv = curr_tv;
+				last_intr_time = curr_time;
 				if (data) {
 					/*
 					 * start timer for end of
diff --git a/drivers/staging/media/mn88472/mn88472.c b/drivers/staging/media/mn88472/mn88472.c
index a4cfcf57c99c..a8d45f44765c 100644
--- a/drivers/staging/media/mn88472/mn88472.c
+++ b/drivers/staging/media/mn88472/mn88472.c
@@ -218,7 +218,7 @@ err:
 	return ret;
 }
 
-static int mn88472_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int mn88472_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct i2c_client *client = fe->demodulator_priv;
 	struct mn88472_dev *dev = i2c_get_clientdata(client);
@@ -344,12 +344,12 @@ static int mn88472_init(struct dvb_frontend *fe)
 	if (ret) {
 		dev_err(&client->dev,
 				"parity reg read failed=%d\n", ret);
-		goto err;
+		goto firmware_release;
 	}
 	if (tmp & 0x10) {
 		dev_err(&client->dev,
 				"firmware parity check failed=0x%x\n", tmp);
-		goto err;
+		goto firmware_release;
 	}
 	dev_err(&client->dev, "firmware parity check succeeded=0x%x\n", tmp);
 
diff --git a/drivers/staging/media/mn88472/mn88472_priv.h b/drivers/staging/media/mn88472/mn88472_priv.h
index 9ba8c8b3823e..1a0de9e46b66 100644
--- a/drivers/staging/media/mn88472/mn88472_priv.h
+++ b/drivers/staging/media/mn88472/mn88472_priv.h
@@ -29,7 +29,7 @@ struct mn88472_dev {
 	struct regmap *regmap[3];
 	struct dvb_frontend fe;
 	u16 i2c_wr_max;
-	fe_delivery_system_t delivery_system;
+	enum fe_delivery_system delivery_system;
 	bool warm; /* FW running */
 	u32 xtal;
 	int ts_mode;
diff --git a/drivers/staging/media/mn88473/mn88473.c b/drivers/staging/media/mn88473/mn88473.c
index 8b6736c70057..f9146a146d07 100644
--- a/drivers/staging/media/mn88473/mn88473.c
+++ b/drivers/staging/media/mn88473/mn88473.c
@@ -167,7 +167,7 @@ err:
 	return ret;
 }
 
-static int mn88473_read_status(struct dvb_frontend *fe, fe_status_t *status)
+static int mn88473_read_status(struct dvb_frontend *fe, enum fe_status *status)
 {
 	struct i2c_client *client = fe->demodulator_priv;
 	struct mn88473_dev *dev = i2c_get_clientdata(client);
diff --git a/drivers/staging/media/mn88473/mn88473_priv.h b/drivers/staging/media/mn88473/mn88473_priv.h
index ef6f01323ac9..54beb4241ccf 100644
--- a/drivers/staging/media/mn88473/mn88473_priv.h
+++ b/drivers/staging/media/mn88473/mn88473_priv.h
@@ -29,7 +29,7 @@ struct mn88473_dev {
 	struct regmap *regmap[3];
 	struct dvb_frontend fe;
 	u16 i2c_wr_max;
-	fe_delivery_system_t delivery_system;
+	enum fe_delivery_system delivery_system;
 	bool warm; /* FW running */
 	u32 xtal;
 };
diff --git a/drivers/staging/media/omap4iss/iss.c b/drivers/staging/media/omap4iss/iss.c
index 7ced940bd807..9bfb725b9986 100644
--- a/drivers/staging/media/omap4iss/iss.c
+++ b/drivers/staging/media/omap4iss/iss.c
@@ -1489,7 +1489,7 @@ static int iss_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id omap4iss_id_table[] = {
+static const struct platform_device_id omap4iss_id_table[] = {
 	{ "omap4iss", 0 },
 	{ },
 };
diff --git a/drivers/staging/media/omap4iss/iss_csi2.c b/drivers/staging/media/omap4iss/iss_csi2.c
index d7ff7698a067..bc83f8246101 100644
--- a/drivers/staging/media/omap4iss/iss_csi2.c
+++ b/drivers/staging/media/omap4iss/iss_csi2.c
@@ -828,8 +828,10 @@ static const struct iss_video_operations csi2_issvideo_ops = {
  */
 
 static struct v4l2_mbus_framefmt *
-__csi2_get_format(struct iss_csi2_device *csi2, struct v4l2_subdev_pad_config *cfg,
-		  unsigned int pad, enum v4l2_subdev_format_whence which)
+__csi2_get_format(struct iss_csi2_device *csi2,
+		  struct v4l2_subdev_pad_config *cfg,
+		  unsigned int pad,
+		  enum v4l2_subdev_format_whence which)
 {
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
 		return v4l2_subdev_get_try_format(&csi2->subdev, cfg, pad);
@@ -838,8 +840,10 @@ __csi2_get_format(struct iss_csi2_device *csi2, struct v4l2_subdev_pad_config *c
 }
 
 static void
-csi2_try_format(struct iss_csi2_device *csi2, struct v4l2_subdev_pad_config *cfg,
-		unsigned int pad, struct v4l2_mbus_framefmt *fmt,
+csi2_try_format(struct iss_csi2_device *csi2,
+		struct v4l2_subdev_pad_config *cfg,
+		unsigned int pad,
+		struct v4l2_mbus_framefmt *fmt,
 		enum v4l2_subdev_format_whence which)
 {
 	u32 pixelcode;
@@ -967,7 +971,8 @@ static int csi2_enum_frame_size(struct v4l2_subdev *sd,
  * @fmt: pointer to v4l2 subdev format structure
  * return -EINVAL or zero on success
  */
-static int csi2_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+static int csi2_get_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_pad_config *cfg,
 			   struct v4l2_subdev_format *fmt)
 {
 	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
@@ -988,7 +993,8 @@ static int csi2_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config
  * @fmt: pointer to v4l2 subdev format structure
  * return -EINVAL or zero on success
  */
-static int csi2_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+static int csi2_set_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_pad_config *cfg,
 			   struct v4l2_subdev_format *fmt)
 {
 	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
diff --git a/drivers/staging/media/omap4iss/iss_ipipe.c b/drivers/staging/media/omap4iss/iss_ipipe.c
index eaa82da30f50..f94a59299a83 100644
--- a/drivers/staging/media/omap4iss/iss_ipipe.c
+++ b/drivers/staging/media/omap4iss/iss_ipipe.c
@@ -24,8 +24,10 @@
 #include "iss_ipipe.h"
 
 static struct v4l2_mbus_framefmt *
-__ipipe_get_format(struct iss_ipipe_device *ipipe, struct v4l2_subdev_pad_config *cfg,
-		  unsigned int pad, enum v4l2_subdev_format_whence which);
+__ipipe_get_format(struct iss_ipipe_device *ipipe,
+		   struct v4l2_subdev_pad_config *cfg,
+		   unsigned int pad,
+		   enum v4l2_subdev_format_whence which);
 
 static const unsigned int ipipe_fmts[] = {
 	MEDIA_BUS_FMT_SGRBG10_1X10,
@@ -176,8 +178,10 @@ static int ipipe_set_stream(struct v4l2_subdev *sd, int enable)
 }
 
 static struct v4l2_mbus_framefmt *
-__ipipe_get_format(struct iss_ipipe_device *ipipe, struct v4l2_subdev_pad_config *cfg,
-		  unsigned int pad, enum v4l2_subdev_format_whence which)
+__ipipe_get_format(struct iss_ipipe_device *ipipe,
+		   struct v4l2_subdev_pad_config *cfg,
+		   unsigned int pad,
+		   enum v4l2_subdev_format_whence which)
 {
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
 		return v4l2_subdev_get_try_format(&ipipe->subdev, cfg, pad);
@@ -193,9 +197,11 @@ __ipipe_get_format(struct iss_ipipe_device *ipipe, struct v4l2_subdev_pad_config
  * @fmt: Format
  */
 static void
-ipipe_try_format(struct iss_ipipe_device *ipipe, struct v4l2_subdev_pad_config *cfg,
-		unsigned int pad, struct v4l2_mbus_framefmt *fmt,
-		enum v4l2_subdev_format_whence which)
+ipipe_try_format(struct iss_ipipe_device *ipipe,
+		 struct v4l2_subdev_pad_config *cfg,
+		 unsigned int pad,
+		 struct v4l2_mbus_framefmt *fmt,
+		 enum v4l2_subdev_format_whence which)
 {
 	struct v4l2_mbus_framefmt *format;
 	unsigned int width = fmt->width;
@@ -306,8 +312,9 @@ static int ipipe_enum_frame_size(struct v4l2_subdev *sd,
  * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
  * to the format type.
  */
-static int ipipe_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
-			   struct v4l2_subdev_format *fmt)
+static int ipipe_get_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *fmt)
 {
 	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
@@ -329,8 +336,9 @@ static int ipipe_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_confi
  * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
  * to the format type.
  */
-static int ipipe_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
-			   struct v4l2_subdev_format *fmt)
+static int ipipe_set_format(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_pad_config *cfg,
+			    struct v4l2_subdev_format *fmt)
 {
 	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.c b/drivers/staging/media/omap4iss/iss_ipipeif.c
index 530ac8426b5b..c0da13d55865 100644
--- a/drivers/staging/media/omap4iss/iss_ipipeif.c
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.c
@@ -518,8 +518,9 @@ static int ipipeif_enum_frame_size(struct v4l2_subdev *sd,
  * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
  * to the format type.
  */
-static int ipipeif_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
-			   struct v4l2_subdev_format *fmt)
+static int ipipeif_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
 {
 	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
@@ -541,8 +542,9 @@ static int ipipeif_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_con
  * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
  * to the format type.
  */
-static int ipipeif_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
-			   struct v4l2_subdev_format *fmt)
+static int ipipeif_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
 {
 	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
diff --git a/drivers/staging/media/omap4iss/iss_resizer.c b/drivers/staging/media/omap4iss/iss_resizer.c
index 5f69012c4deb..5030cf3cd34c 100644
--- a/drivers/staging/media/omap4iss/iss_resizer.c
+++ b/drivers/staging/media/omap4iss/iss_resizer.c
@@ -580,8 +580,9 @@ static int resizer_enum_frame_size(struct v4l2_subdev *sd,
  * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
  * to the format type.
  */
-static int resizer_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
-			   struct v4l2_subdev_format *fmt)
+static int resizer_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *fmt)
 {
 	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *format;
@@ -603,7 +604,8 @@ static int resizer_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_con
  * Return 0 on success or -EINVAL if the pad is invalid or doesn't correspond
  * to the format type.
  */
-static int resizer_set_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+static int resizer_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
 			      struct v4l2_subdev_format *fmt)
 {
 	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index 1bdc8d001e65..164634d61ac5 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -916,7 +916,7 @@ static int tegra_nvec_remove(struct platform_device *pdev)
 	nvec_unregister_notifier(nvec, &nvec->nvec_status_notifier);
 	cancel_work_sync(&nvec->rx_work);
 	cancel_work_sync(&nvec->tx_work);
-	/* FIXME: needs check wether nvec is responsible for power off */
+	/* FIXME: needs check whether nvec is responsible for power off */
 	pm_power_off = NULL;
 
 	return 0;
diff --git a/drivers/staging/nvec/nvec_ps2.c b/drivers/staging/nvec/nvec_ps2.c
index 6ebbc82323c3..0922dd3a08d3 100644
--- a/drivers/staging/nvec/nvec_ps2.c
+++ b/drivers/staging/nvec/nvec_ps2.c
@@ -169,8 +169,8 @@ static int nvec_mouse_resume(struct device *dev)
 }
 #endif
 
-static const SIMPLE_DEV_PM_OPS(nvec_mouse_pm_ops, nvec_mouse_suspend,
-				nvec_mouse_resume);
+static SIMPLE_DEV_PM_OPS(nvec_mouse_pm_ops, nvec_mouse_suspend,
+			 nvec_mouse_resume);
 
 static struct platform_driver nvec_mouse_driver = {
 	.probe  = nvec_mouse_probe,
diff --git a/drivers/staging/octeon-usb/octeon-hcd.c b/drivers/staging/octeon-usb/octeon-hcd.c
index 9e5476e352b4..9bd73ea5a1fa 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.c
+++ b/drivers/staging/octeon-usb/octeon-hcd.c
@@ -499,15 +499,21 @@ static int octeon_alloc_temp_buffer(struct urb *urb, gfp_t mem_flags)
 static void octeon_free_temp_buffer(struct urb *urb)
 {
 	struct octeon_temp_buffer *temp;
+	size_t length;
 
 	if (!(urb->transfer_flags & URB_ALIGNED_TEMP_BUFFER))
 		return;
 
 	temp = container_of(urb->transfer_buffer, struct octeon_temp_buffer,
 			    data);
-	if (usb_urb_dir_in(urb))
-		memcpy(temp->orig_buffer, urb->transfer_buffer,
-		       urb->actual_length);
+	if (usb_urb_dir_in(urb)) {
+		if (usb_pipeisoc(urb->pipe))
+			length = urb->transfer_buffer_length;
+		else
+			length = urb->actual_length;
+
+		memcpy(temp->orig_buffer, urb->transfer_buffer, length);
+	}
 	urb->transfer_buffer = temp->orig_buffer;
 	urb->transfer_flags &= ~URB_ALIGNED_TEMP_BUFFER;
 	kfree(temp);
@@ -1233,7 +1239,7 @@ static int cvmx_usb_fill_tx_hw(struct cvmx_usb_state *usb,
 							usb->index) ^ 4;
 		int words = available;
 
-		/* Limit the amount of data to waht the SW fifo has */
+		/* Limit the amount of data to what the SW fifo has */
 		if (fifo->entry[i].size <= available) {
 			words = fifo->entry[i].size;
 			fifo->tail++;
@@ -1843,7 +1849,7 @@ static void cvmx_usb_start_channel(struct cvmx_usb_state *usb, int channel,
 		transaction->xfersize = usbc_hctsiz.s.xfersize;
 		transaction->pktcnt = usbc_hctsiz.s.pktcnt;
 	}
-	/* Remeber when we start a split transaction */
+	/* Remember when we start a split transaction */
 	if (cvmx_usb_pipe_needs_split(usb, pipe))
 		usb->active_split = transaction;
 	USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
diff --git a/drivers/staging/octeon-usb/octeon-hcd.h b/drivers/staging/octeon-usb/octeon-hcd.h
index 3e351ab7465a..70e7fa5e37d9 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.h
+++ b/drivers/staging/octeon-usb/octeon-hcd.h
@@ -1693,7 +1693,7 @@ union cvmx_usbnx_usbp_ctl_status {
 	 * struct cvmx_usbnx_usbp_ctl_status_s
 	 * @txrisetune: HS Transmitter Rise/Fall Time Adjustment
 	 * @txvreftune: HS DC Voltage Level Adjustment
-	 * @txfslstune: FS/LS Source Impedence Adjustment
+	 * @txfslstune: FS/LS Source Impedance Adjustment
 	 * @txhsxvtune: Transmitter High-Speed Crossover Adjustment
 	 * @sqrxtune: Squelch Threshold Adjustment
 	 * @compdistune: Disconnect Threshold Adjustment
diff --git a/drivers/staging/octeon/ethernet-defines.h b/drivers/staging/octeon/ethernet-defines.h
index 2a98a2153e16..f92e0c478e16 100644
--- a/drivers/staging/octeon/ethernet-defines.h
+++ b/drivers/staging/octeon/ethernet-defines.h
@@ -1,46 +1,15 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
 
 /*
  * A few defines are used to control the operation of this driver:
- *  CONFIG_CAVIUM_RESERVE32
- *      This kernel config options controls the amount of memory configured
- *      in a wired TLB entry for all processes to share. If this is set, the
- *      driver will use this memory instead of kernel memory for pools. This
- *      allows 32bit userspace application to access the buffers, but also
- *      requires all received packets to be copied.
- *  USE_SKBUFFS_IN_HW
- *      Tells the driver to populate the packet buffers with kernel skbuffs.
- *      This allows the driver to receive packets without copying them. It also
- *      means that 32bit userspace can't access the packet buffers.
- *  USE_HW_TCPUDP_CHECKSUM
- *      Controls if the Octeon TCP/UDP checksum engine is used for packet
- *      output. If this is zero, the kernel will perform the checksum in
- *      software.
  *  USE_ASYNC_IOBDMA
  *      Use asynchronous IO access to hardware. This uses Octeon's asynchronous
  *      IOBDMAs to issue IO accesses without stalling. Set this to zero
@@ -57,39 +26,14 @@
 
 #include <asm/octeon/cvmx-config.h>
 
-
-#define OCTEON_ETHERNET_VERSION "1.9"
-
-#ifndef CONFIG_CAVIUM_RESERVE32
-#define CONFIG_CAVIUM_RESERVE32 0
-#endif
-
-#define USE_SKBUFFS_IN_HW           1
 #ifdef CONFIG_NETFILTER
 #define REUSE_SKBUFFS_WITHOUT_FREE  0
 #else
 #define REUSE_SKBUFFS_WITHOUT_FREE  1
 #endif
 
-#define USE_HW_TCPUDP_CHECKSUM      1
-
-/* Enable Random Early Dropping under load */
-#define USE_RED                     1
 #define USE_ASYNC_IOBDMA            (CONFIG_CAVIUM_OCTEON_CVMSEG_SIZE > 0)
 
-/*
- * Allow SW based preamble removal at 10Mbps to workaround PHYs giving
- * us bad preambles.
- */
-#define USE_10MBPS_PREAMBLE_WORKAROUND 1
-/*
- * Use this to have all FPA frees also tell the L2 not to write data
- * to memory.
- */
-#define DONT_WRITEBACK(x)           (x)
-/* Use this to not have FPA frees control L2 */
-/*#define DONT_WRITEBACK(x)         0   */
-
 /* Maximum number of SKBs to try to free per xmit packet. */
 #define MAX_OUT_QUEUE_DEPTH 1000
 
diff --git a/drivers/staging/octeon/ethernet-mdio.c b/drivers/staging/octeon/ethernet-mdio.c
index 40dab11e5333..fd9b3d899c1f 100644
--- a/drivers/staging/octeon/ethernet-mdio.c
+++ b/drivers/staging/octeon/ethernet-mdio.c
@@ -1,35 +1,19 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
+
 #include <linux/kernel.h>
 #include <linux/ethtool.h>
 #include <linux/phy.h>
 #include <linux/ratelimit.h>
 #include <linux/of_mdio.h>
-
+#include <generated/utsrelease.h>
 #include <net/dst.h>
 
 #include <asm/octeon/octeon.h>
@@ -39,15 +23,14 @@
 #include "ethernet-mdio.h"
 #include "ethernet-util.h"
 
-#include <asm/octeon/cvmx-helper-board.h>
-
+#include <asm/octeon/cvmx-gmxx-defs.h>
 #include <asm/octeon/cvmx-smix-defs.h>
 
 static void cvm_oct_get_drvinfo(struct net_device *dev,
 				struct ethtool_drvinfo *info)
 {
-	strlcpy(info->driver, "cavium-ethernet", sizeof(info->driver));
-	strlcpy(info->version, OCTEON_ETHERNET_VERSION, sizeof(info->version));
+	strlcpy(info->driver, KBUILD_MODNAME, sizeof(info->driver));
+	strlcpy(info->version, UTS_RELEASE, sizeof(info->version));
 	strlcpy(info->bus_info, "Builtin", sizeof(info->bus_info));
 }
 
@@ -116,14 +99,14 @@ int cvm_oct_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	return phy_mii_ioctl(priv->phydev, rq, cmd);
 }
 
-static void cvm_oct_note_carrier(struct octeon_ethernet *priv,
-				 cvmx_helper_link_info_t li)
+void cvm_oct_note_carrier(struct octeon_ethernet *priv,
+			  cvmx_helper_link_info_t li)
 {
 	if (li.s.link_up) {
-		pr_notice_ratelimited("%s: %u Mbps %s duplex, port %d\n",
+		pr_notice_ratelimited("%s: %u Mbps %s duplex, port %d, queue %d\n",
 				      netdev_name(priv->netdev), li.s.speed,
 				      (li.s.full_duplex) ? "Full" : "Half",
-				      priv->port);
+				      priv->port, priv->queue);
 	} else {
 		pr_notice_ratelimited("%s: Link down\n",
 				      netdev_name(priv->netdev));
@@ -150,7 +133,14 @@ void cvm_oct_adjust_link(struct net_device *dev)
 int cvm_oct_common_stop(struct net_device *dev)
 {
 	struct octeon_ethernet *priv = netdev_priv(dev);
+	int interface = INTERFACE(priv->port);
 	cvmx_helper_link_info_t link_info;
+	union cvmx_gmxx_prtx_cfg gmx_cfg;
+	int index = INDEX(priv->port);
+
+	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
+	gmx_cfg.s.en = 0;
+	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
 
 	priv->poll = NULL;
 
diff --git a/drivers/staging/octeon/ethernet-mdio.h b/drivers/staging/octeon/ethernet-mdio.h
index 6191b0850646..a530b55f27d8 100644
--- a/drivers/staging/octeon/ethernet-mdio.h
+++ b/drivers/staging/octeon/ethernet-mdio.h
@@ -1,29 +1,13 @@
-/*********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-*********************************************************************/
+ */
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
diff --git a/drivers/staging/octeon/ethernet-mem.c b/drivers/staging/octeon/ethernet-mem.c
index 964da860f4c4..5a5cdb3cd740 100644
--- a/drivers/staging/octeon/ethernet-mem.c
+++ b/drivers/staging/octeon/ethernet-mem.c
@@ -1,29 +1,13 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2010 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
+
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
 #include <linux/slab.h>
@@ -54,7 +38,7 @@ static int cvm_oct_fill_hw_skbuff(int pool, int size, int elements)
 			break;
 		skb_reserve(skb, 256 - (((unsigned long)skb->data) & 0x7f));
 		*(struct sk_buff **)(skb->data - sizeof(void *)) = skb;
-		cvmx_fpa_free(skb->data, pool, DONT_WRITEBACK(size / 128));
+		cvmx_fpa_free(skb->data, pool, size / 128);
 		freed--;
 	}
 	return elements - freed;
@@ -160,7 +144,7 @@ int cvm_oct_mem_fill_fpa(int pool, int size, int elements)
 {
 	int freed;
 
-	if (USE_SKBUFFS_IN_HW && pool == CVMX_FPA_PACKET_POOL)
+	if (pool == CVMX_FPA_PACKET_POOL)
 		freed = cvm_oct_fill_hw_skbuff(pool, size, elements);
 	else
 		freed = cvm_oct_fill_hw_memory(pool, size, elements);
@@ -169,7 +153,7 @@ int cvm_oct_mem_fill_fpa(int pool, int size, int elements)
 
 void cvm_oct_mem_empty_fpa(int pool, int size, int elements)
 {
-	if (USE_SKBUFFS_IN_HW && pool == CVMX_FPA_PACKET_POOL)
+	if (pool == CVMX_FPA_PACKET_POOL)
 		cvm_oct_free_hw_skbuff(pool, size, elements);
 	else
 		cvm_oct_free_hw_memory(pool, size, elements);
diff --git a/drivers/staging/octeon/ethernet-mem.h b/drivers/staging/octeon/ethernet-mem.h
index 713f2edc8b4f..62d07c426f89 100644
--- a/drivers/staging/octeon/ethernet-mem.h
+++ b/drivers/staging/octeon/ethernet-mem.h
@@ -1,29 +1,12 @@
-/*********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-********************************************************************/
+ */
 
 int cvm_oct_mem_fill_fpa(int pool, int size, int elements);
 void cvm_oct_mem_empty_fpa(int pool, int size, int elements);
diff --git a/drivers/staging/octeon/ethernet-rgmii.c b/drivers/staging/octeon/ethernet-rgmii.c
index e36f9bc69543..beb7aac9c289 100644
--- a/drivers/staging/octeon/ethernet-rgmii.c
+++ b/drivers/staging/octeon/ethernet-rgmii.c
@@ -1,29 +1,13 @@
-/*********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
+
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
 #include <linux/interrupt.h>
@@ -48,6 +32,37 @@ static DEFINE_SPINLOCK(global_register_lock);
 
 static int number_rgmii_ports;
 
+static void cvm_oct_set_hw_preamble(struct octeon_ethernet *priv, bool enable)
+{
+	union cvmx_gmxx_rxx_frm_ctl gmxx_rxx_frm_ctl;
+	union cvmx_ipd_sub_port_fcs ipd_sub_port_fcs;
+	union cvmx_gmxx_rxx_int_reg gmxx_rxx_int_reg;
+	int interface = INTERFACE(priv->port);
+	int index = INDEX(priv->port);
+
+	/* Set preamble checking. */
+	gmxx_rxx_frm_ctl.u64 = cvmx_read_csr(CVMX_GMXX_RXX_FRM_CTL(index,
+								   interface));
+	gmxx_rxx_frm_ctl.s.pre_chk = enable;
+	cvmx_write_csr(CVMX_GMXX_RXX_FRM_CTL(index, interface),
+		       gmxx_rxx_frm_ctl.u64);
+
+	/* Set FCS stripping. */
+	ipd_sub_port_fcs.u64 = cvmx_read_csr(CVMX_IPD_SUB_PORT_FCS);
+	if (enable)
+		ipd_sub_port_fcs.s.port_bit |= 1ull << priv->port;
+	else
+		ipd_sub_port_fcs.s.port_bit &=
+					0xffffffffull ^ (1ull << priv->port);
+	cvmx_write_csr(CVMX_IPD_SUB_PORT_FCS, ipd_sub_port_fcs.u64);
+
+	/* Clear any error bits. */
+	gmxx_rxx_int_reg.u64 = cvmx_read_csr(CVMX_GMXX_RXX_INT_REG(index,
+								   interface));
+	cvmx_write_csr(CVMX_GMXX_RXX_INT_REG(index, interface),
+		       gmxx_rxx_int_reg.u64);
+}
+
 static void cvm_oct_rgmii_poll(struct net_device *dev)
 {
 	struct octeon_ethernet *priv = netdev_priv(dev);
@@ -68,14 +83,7 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 
 	link_info = cvmx_helper_link_get(priv->port);
 	if (link_info.u64 == priv->link_info) {
-
-		/*
-		 * If the 10Mbps preamble workaround is supported and we're
-		 * at 10Mbps we may need to do some special checking.
-		 */
-		if (USE_10MBPS_PREAMBLE_WORKAROUND &&
-				(link_info.s.speed == 10)) {
-
+		if (link_info.s.speed == 10) {
 			/*
 			 * Read the GMXX_RXX_INT_REG[PCTERR] bit and
 			 * see if we are getting preamble errors.
@@ -88,7 +96,6 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 			    cvmx_read_csr(CVMX_GMXX_RXX_INT_REG
 					  (index, interface));
 			if (gmxx_rxx_int_reg.s.pcterr) {
-
 				/*
 				 * We are getting preamble errors at
 				 * 10Mbps.  Most likely the PHY is
@@ -97,30 +104,7 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 				 * packets we need to disable preamble
 				 * checking and do it in software.
 				 */
-				union cvmx_gmxx_rxx_frm_ctl gmxx_rxx_frm_ctl;
-				union cvmx_ipd_sub_port_fcs ipd_sub_port_fcs;
-
-				/* Disable preamble checking */
-				gmxx_rxx_frm_ctl.u64 =
-				    cvmx_read_csr(CVMX_GMXX_RXX_FRM_CTL
-						  (index, interface));
-				gmxx_rxx_frm_ctl.s.pre_chk = 0;
-				cvmx_write_csr(CVMX_GMXX_RXX_FRM_CTL
-					       (index, interface),
-					       gmxx_rxx_frm_ctl.u64);
-
-				/* Disable FCS stripping */
-				ipd_sub_port_fcs.u64 =
-				    cvmx_read_csr(CVMX_IPD_SUB_PORT_FCS);
-				ipd_sub_port_fcs.s.port_bit &=
-				    0xffffffffull ^ (1ull << priv->port);
-				cvmx_write_csr(CVMX_IPD_SUB_PORT_FCS,
-					       ipd_sub_port_fcs.u64);
-
-				/* Clear any error bits */
-				cvmx_write_csr(CVMX_GMXX_RXX_INT_REG
-					       (index, interface),
-					       gmxx_rxx_int_reg.u64);
+				cvm_oct_set_hw_preamble(priv, false);
 				printk_ratelimited("%s: Using 10Mbps with software preamble removal\n",
 						   dev->name);
 			}
@@ -133,34 +117,12 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 		return;
 	}
 
-	/* If the 10Mbps preamble workaround is allowed we need to on
+	/* Since the 10Mbps preamble workaround is allowed we need to enable
 	   preamble checking, FCS stripping, and clear error bits on
 	   every speed change. If errors occur during 10Mbps operation
 	   the above code will change this stuff */
-	if (USE_10MBPS_PREAMBLE_WORKAROUND) {
-
-		union cvmx_gmxx_rxx_frm_ctl gmxx_rxx_frm_ctl;
-		union cvmx_ipd_sub_port_fcs ipd_sub_port_fcs;
-		union cvmx_gmxx_rxx_int_reg gmxx_rxx_int_reg;
-		int interface = INTERFACE(priv->port);
-		int index = INDEX(priv->port);
-
-		/* Enable preamble checking */
-		gmxx_rxx_frm_ctl.u64 =
-		    cvmx_read_csr(CVMX_GMXX_RXX_FRM_CTL(index, interface));
-		gmxx_rxx_frm_ctl.s.pre_chk = 1;
-		cvmx_write_csr(CVMX_GMXX_RXX_FRM_CTL(index, interface),
-			       gmxx_rxx_frm_ctl.u64);
-		/* Enable FCS stripping */
-		ipd_sub_port_fcs.u64 = cvmx_read_csr(CVMX_IPD_SUB_PORT_FCS);
-		ipd_sub_port_fcs.s.port_bit |= 1ull << priv->port;
-		cvmx_write_csr(CVMX_IPD_SUB_PORT_FCS, ipd_sub_port_fcs.u64);
-		/* Clear any error bits */
-		gmxx_rxx_int_reg.u64 =
-		    cvmx_read_csr(CVMX_GMXX_RXX_INT_REG(index, interface));
-		cvmx_write_csr(CVMX_GMXX_RXX_INT_REG(index, interface),
-			       gmxx_rxx_int_reg.u64);
-	}
+	cvm_oct_set_hw_preamble(priv, true);
+
 	if (priv->phydev == NULL) {
 		link_info = cvmx_helper_link_autoconf(priv->port);
 		priv->link_info = link_info.u64;
@@ -176,172 +138,75 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 		if (link_info.s.link_up) {
 			if (!netif_carrier_ok(dev))
 				netif_carrier_on(dev);
-			if (priv->queue != -1)
-				printk_ratelimited("%s: %u Mbps %s duplex, port %2d, queue %2d\n",
-						   dev->name, link_info.s.speed,
-						   (link_info.s.full_duplex) ?
-						   "Full" : "Half",
-						   priv->port, priv->queue);
-			else
-				printk_ratelimited("%s: %u Mbps %s duplex, port %2d, POW\n",
-						   dev->name, link_info.s.speed,
-						   (link_info.s.full_duplex) ?
-						   "Full" : "Half",
-						   priv->port);
-		} else {
-			if (netif_carrier_ok(dev))
-				netif_carrier_off(dev);
-			printk_ratelimited("%s: Link down\n", dev->name);
+		} else if (netif_carrier_ok(dev)) {
+			netif_carrier_off(dev);
 		}
+		cvm_oct_note_carrier(priv, link_info);
 	}
 }
 
-static irqreturn_t cvm_oct_rgmii_rml_interrupt(int cpl, void *dev_id)
+static int cmv_oct_rgmii_gmx_interrupt(int interface)
 {
-	union cvmx_npi_rsl_int_blocks rsl_int_blocks;
 	int index;
-	irqreturn_t return_status = IRQ_NONE;
+	int count = 0;
 
-	rsl_int_blocks.u64 = cvmx_read_csr(CVMX_NPI_RSL_INT_BLOCKS);
-
-	/* Check and see if this interrupt was caused by the GMX0 block */
-	if (rsl_int_blocks.s.gmx0) {
-
-		int interface = 0;
-		/* Loop through every port of this interface */
-		for (index = 0;
-		     index < cvmx_helper_ports_on_interface(interface);
-		     index++) {
+	/* Loop through every port of this interface */
+	for (index = 0;
+	     index < cvmx_helper_ports_on_interface(interface);
+	     index++) {
+		union cvmx_gmxx_rxx_int_reg gmx_rx_int_reg;
 
-			/* Read the GMX interrupt status bits */
-			union cvmx_gmxx_rxx_int_reg gmx_rx_int_reg;
-
-			gmx_rx_int_reg.u64 =
-			    cvmx_read_csr(CVMX_GMXX_RXX_INT_REG
+		/* Read the GMX interrupt status bits */
+		gmx_rx_int_reg.u64 = cvmx_read_csr(CVMX_GMXX_RXX_INT_REG
 					  (index, interface));
-			gmx_rx_int_reg.u64 &=
-			    cvmx_read_csr(CVMX_GMXX_RXX_INT_EN
+		gmx_rx_int_reg.u64 &= cvmx_read_csr(CVMX_GMXX_RXX_INT_EN
 					  (index, interface));
-			/* Poll the port if inband status changed */
-			if (gmx_rx_int_reg.s.phy_dupx
-			    || gmx_rx_int_reg.s.phy_link
-			    || gmx_rx_int_reg.s.phy_spd) {
 
-				struct net_device *dev =
+		/* Poll the port if inband status changed */
+		if (gmx_rx_int_reg.s.phy_dupx || gmx_rx_int_reg.s.phy_link ||
+		    gmx_rx_int_reg.s.phy_spd) {
+			struct net_device *dev =
 				    cvm_oct_device[cvmx_helper_get_ipd_port
 						   (interface, index)];
-				struct octeon_ethernet *priv = netdev_priv(dev);
-
-				if (dev &&
-				!atomic_read(&cvm_oct_poll_queue_stopping))
-					queue_work(cvm_oct_poll_queue,
-						&priv->port_work);
-
-				gmx_rx_int_reg.u64 = 0;
-				gmx_rx_int_reg.s.phy_dupx = 1;
-				gmx_rx_int_reg.s.phy_link = 1;
-				gmx_rx_int_reg.s.phy_spd = 1;
-				cvmx_write_csr(CVMX_GMXX_RXX_INT_REG
-					       (index, interface),
-					       gmx_rx_int_reg.u64);
-				return_status = IRQ_HANDLED;
-			}
+			struct octeon_ethernet *priv = netdev_priv(dev);
+
+			if (dev && !atomic_read(&cvm_oct_poll_queue_stopping))
+				queue_work(cvm_oct_poll_queue,
+					   &priv->port_work);
+
+			gmx_rx_int_reg.u64 = 0;
+			gmx_rx_int_reg.s.phy_dupx = 1;
+			gmx_rx_int_reg.s.phy_link = 1;
+			gmx_rx_int_reg.s.phy_spd = 1;
+			cvmx_write_csr(CVMX_GMXX_RXX_INT_REG(index, interface),
+				       gmx_rx_int_reg.u64);
+			count++;
 		}
 	}
+	return count;
+}
 
-	/* Check and see if this interrupt was caused by the GMX1 block */
-	if (rsl_int_blocks.s.gmx1) {
+static irqreturn_t cvm_oct_rgmii_rml_interrupt(int cpl, void *dev_id)
+{
+	union cvmx_npi_rsl_int_blocks rsl_int_blocks;
+	int count = 0;
 
-		int interface = 1;
-		/* Loop through every port of this interface */
-		for (index = 0;
-		     index < cvmx_helper_ports_on_interface(interface);
-		     index++) {
+	rsl_int_blocks.u64 = cvmx_read_csr(CVMX_NPI_RSL_INT_BLOCKS);
 
-			/* Read the GMX interrupt status bits */
-			union cvmx_gmxx_rxx_int_reg gmx_rx_int_reg;
+	/* Check and see if this interrupt was caused by the GMX0 block */
+	if (rsl_int_blocks.s.gmx0)
+		count += cmv_oct_rgmii_gmx_interrupt(0);
 
-			gmx_rx_int_reg.u64 =
-			    cvmx_read_csr(CVMX_GMXX_RXX_INT_REG
-					  (index, interface));
-			gmx_rx_int_reg.u64 &=
-			    cvmx_read_csr(CVMX_GMXX_RXX_INT_EN
-					  (index, interface));
-			/* Poll the port if inband status changed */
-			if (gmx_rx_int_reg.s.phy_dupx
-			    || gmx_rx_int_reg.s.phy_link
-			    || gmx_rx_int_reg.s.phy_spd) {
+	/* Check and see if this interrupt was caused by the GMX1 block */
+	if (rsl_int_blocks.s.gmx1)
+		count += cmv_oct_rgmii_gmx_interrupt(1);
 
-				struct net_device *dev =
-				    cvm_oct_device[cvmx_helper_get_ipd_port
-						   (interface, index)];
-				struct octeon_ethernet *priv = netdev_priv(dev);
-
-				if (dev &&
-				!atomic_read(&cvm_oct_poll_queue_stopping))
-					queue_work(cvm_oct_poll_queue,
-						&priv->port_work);
-
-				gmx_rx_int_reg.u64 = 0;
-				gmx_rx_int_reg.s.phy_dupx = 1;
-				gmx_rx_int_reg.s.phy_link = 1;
-				gmx_rx_int_reg.s.phy_spd = 1;
-				cvmx_write_csr(CVMX_GMXX_RXX_INT_REG
-					       (index, interface),
-					       gmx_rx_int_reg.u64);
-				return_status = IRQ_HANDLED;
-			}
-		}
-	}
-	return return_status;
+	return count ? IRQ_HANDLED : IRQ_NONE;
 }
 
 int cvm_oct_rgmii_open(struct net_device *dev)
 {
-	union cvmx_gmxx_prtx_cfg gmx_cfg;
-	struct octeon_ethernet *priv = netdev_priv(dev);
-	int interface = INTERFACE(priv->port);
-	int index = INDEX(priv->port);
-	cvmx_helper_link_info_t link_info;
-	int rv;
-
-	rv = cvm_oct_phy_setup_device(dev);
-	if (rv)
-		return rv;
-
-	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
-	gmx_cfg.s.en = 1;
-	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
-
-	if (!octeon_is_simulation()) {
-		if (priv->phydev) {
-			int r = phy_read_status(priv->phydev);
-
-			if (r == 0 && priv->phydev->link == 0)
-				netif_carrier_off(dev);
-			cvm_oct_adjust_link(dev);
-		} else {
-			link_info = cvmx_helper_link_get(priv->port);
-			if (!link_info.s.link_up)
-				netif_carrier_off(dev);
-			priv->poll = cvm_oct_rgmii_poll;
-		}
-	}
-
-	return 0;
-}
-
-int cvm_oct_rgmii_stop(struct net_device *dev)
-{
-	union cvmx_gmxx_prtx_cfg gmx_cfg;
-	struct octeon_ethernet *priv = netdev_priv(dev);
-	int interface = INTERFACE(priv->port);
-	int index = INDEX(priv->port);
-
-	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
-	gmx_cfg.s.en = 0;
-	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
-	return cvm_oct_common_stop(dev);
+	return cvm_oct_common_open(dev, cvm_oct_rgmii_poll, false);
 }
 
 static void cvm_oct_rgmii_immediate_poll(struct work_struct *work)
@@ -357,7 +222,6 @@ int cvm_oct_rgmii_init(struct net_device *dev)
 	int r;
 
 	cvm_oct_common_init(dev);
-	dev->netdev_ops->ndo_stop(dev);
 	INIT_WORK(&priv->port_work, cvm_oct_rgmii_immediate_poll);
 	/*
 	 * Due to GMX errata in CN3XXX series chips, it is necessary
diff --git a/drivers/staging/octeon/ethernet-rx.c b/drivers/staging/octeon/ethernet-rx.c
index 22667dbb10d8..22853d33da05 100644
--- a/drivers/staging/octeon/ethernet-rx.c
+++ b/drivers/staging/octeon/ethernet-rx.c
@@ -1,29 +1,13 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2010 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/cache.h>
@@ -93,11 +77,8 @@ static inline int cvm_oct_check_rcv_error(cvmx_wqe_t *work)
 		 * instead of 60+4FCS.  Note these packets still get
 		 * counted as frame errors.
 		 */
-	} else
-	    if (USE_10MBPS_PREAMBLE_WORKAROUND
-		&& ((work->word2.snoip.err_code == 5)
-		    || (work->word2.snoip.err_code == 7))) {
-
+	} else if (work->word2.snoip.err_code == 5 ||
+		   work->word2.snoip.err_code == 7) {
 		/*
 		 * We received a packet with either an alignment error
 		 * or a FCS error. This may be signalling that we are
@@ -233,7 +214,7 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 		}
 		rx_count++;
 
-		skb_in_hw = USE_SKBUFFS_IN_HW && work->word2.s.bufs == 1;
+		skb_in_hw = work->word2.s.bufs == 1;
 		if (likely(skb_in_hw)) {
 			skb = *pskb;
 			prefetch(&skb->head);
@@ -394,7 +375,7 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 		 * Check to see if the skbuff and work share the same
 		 * packet buffer.
 		 */
-		if (USE_SKBUFFS_IN_HW && likely(packet_not_copied)) {
+		if (likely(packet_not_copied)) {
 			/*
 			 * This buffer needs to be replaced, increment
 			 * the number of buffers we need to free by
@@ -403,8 +384,7 @@ static int cvm_oct_napi_poll(struct napi_struct *napi, int budget)
 			cvmx_fau_atomic_add32(FAU_NUM_PACKET_BUFFERS_TO_FREE,
 					      1);
 
-			cvmx_fpa_free(work, CVMX_FPA_WQE_POOL,
-				      DONT_WRITEBACK(1));
+			cvmx_fpa_free(work, CVMX_FPA_WQE_POOL, 1);
 		} else {
 			cvm_oct_free_work(work);
 		}
diff --git a/drivers/staging/octeon/ethernet-rx.h b/drivers/staging/octeon/ethernet-rx.h
index 9240c85ce241..a5973fd015fc 100644
--- a/drivers/staging/octeon/ethernet-rx.h
+++ b/drivers/staging/octeon/ethernet-rx.h
@@ -1,29 +1,13 @@
-/*********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-*********************************************************************/
+ */
+
 #include <asm/octeon/cvmx-fau.h>
 
 void cvm_oct_poll_controller(struct net_device *dev);
diff --git a/drivers/staging/octeon/ethernet-sgmii.c b/drivers/staging/octeon/ethernet-sgmii.c
index 21a7a17acb79..8bceb769166c 100644
--- a/drivers/staging/octeon/ethernet-sgmii.c
+++ b/drivers/staging/octeon/ethernet-sgmii.c
@@ -1,29 +1,13 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
+
 #include <linux/phy.h>
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
@@ -41,101 +25,15 @@
 
 #include <asm/octeon/cvmx-gmxx-defs.h>
 
-static void cvm_oct_sgmii_poll(struct net_device *dev)
-{
-	struct octeon_ethernet *priv = netdev_priv(dev);
-	cvmx_helper_link_info_t link_info;
-
-	link_info = cvmx_helper_link_get(priv->port);
-	if (link_info.u64 == priv->link_info)
-		return;
-
-	link_info = cvmx_helper_link_autoconf(priv->port);
-	priv->link_info = link_info.u64;
-
-	/* Tell Linux */
-	if (link_info.s.link_up) {
-
-		if (!netif_carrier_ok(dev))
-			netif_carrier_on(dev);
-		if (priv->queue != -1)
-			printk_ratelimited
-			    ("%s: %u Mbps %s duplex, port %2d, queue %2d\n",
-			     dev->name, link_info.s.speed,
-			     (link_info.s.full_duplex) ? "Full" : "Half",
-			     priv->port, priv->queue);
-		else
-			printk_ratelimited
-				("%s: %u Mbps %s duplex, port %2d, POW\n",
-				 dev->name, link_info.s.speed,
-				 (link_info.s.full_duplex) ? "Full" : "Half",
-				 priv->port);
-	} else {
-		if (netif_carrier_ok(dev))
-			netif_carrier_off(dev);
-		printk_ratelimited("%s: Link down\n", dev->name);
-	}
-}
-
 int cvm_oct_sgmii_open(struct net_device *dev)
 {
-	union cvmx_gmxx_prtx_cfg gmx_cfg;
-	struct octeon_ethernet *priv = netdev_priv(dev);
-	int interface = INTERFACE(priv->port);
-	int index = INDEX(priv->port);
-	cvmx_helper_link_info_t link_info;
-	int rv;
-
-	rv = cvm_oct_phy_setup_device(dev);
-	if (rv)
-		return rv;
-
-	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
-	gmx_cfg.s.en = 1;
-	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
-
-	if (octeon_is_simulation())
-		return 0;
-
-	if (priv->phydev) {
-		int r = phy_read_status(priv->phydev);
-
-		if (r == 0 && priv->phydev->link == 0)
-			netif_carrier_off(dev);
-		cvm_oct_adjust_link(dev);
-	} else {
-		link_info = cvmx_helper_link_get(priv->port);
-		if (!link_info.s.link_up)
-			netif_carrier_off(dev);
-		priv->poll = cvm_oct_sgmii_poll;
-		cvm_oct_sgmii_poll(dev);
-	}
-	return 0;
-}
-
-int cvm_oct_sgmii_stop(struct net_device *dev)
-{
-	union cvmx_gmxx_prtx_cfg gmx_cfg;
-	struct octeon_ethernet *priv = netdev_priv(dev);
-	int interface = INTERFACE(priv->port);
-	int index = INDEX(priv->port);
-
-	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
-	gmx_cfg.s.en = 0;
-	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
-	return cvm_oct_common_stop(dev);
+	return cvm_oct_common_open(dev, cvm_oct_link_poll, true);
 }
 
 int cvm_oct_sgmii_init(struct net_device *dev)
 {
 	cvm_oct_common_init(dev);
-	dev->netdev_ops->ndo_stop(dev);
 
 	/* FIXME: Need autoneg logic */
 	return 0;
 }
-
-void cvm_oct_sgmii_uninit(struct net_device *dev)
-{
-	cvm_oct_common_uninit(dev);
-}
diff --git a/drivers/staging/octeon/ethernet-spi.c b/drivers/staging/octeon/ethernet-spi.c
index 5108bc0bb573..2ae1944b3a1b 100644
--- a/drivers/staging/octeon/ethernet-spi.c
+++ b/drivers/staging/octeon/ethernet-spi.c
@@ -1,29 +1,13 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
+
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
 #include <linux/interrupt.h>
@@ -44,141 +28,104 @@
 static int number_spi_ports;
 static int need_retrain[2] = { 0, 0 };
 
-static irqreturn_t cvm_oct_spi_rml_interrupt(int cpl, void *dev_id)
+static void cvm_oct_spxx_int_pr(union cvmx_spxx_int_reg spx_int_reg, int index)
 {
-	irqreturn_t return_status = IRQ_NONE;
-	union cvmx_npi_rsl_int_blocks rsl_int_blocks;
-
-	/* Check and see if this interrupt was caused by the GMX block */
-	rsl_int_blocks.u64 = cvmx_read_csr(CVMX_NPI_RSL_INT_BLOCKS);
-	if (rsl_int_blocks.s.spx1) {	/* 19 - SPX1_INT_REG & STX1_INT_REG */
-
-		union cvmx_spxx_int_reg spx_int_reg;
-		union cvmx_stxx_int_reg stx_int_reg;
-
-		spx_int_reg.u64 = cvmx_read_csr(CVMX_SPXX_INT_REG(1));
-		cvmx_write_csr(CVMX_SPXX_INT_REG(1), spx_int_reg.u64);
-		if (!need_retrain[1]) {
-
-			spx_int_reg.u64 &= cvmx_read_csr(CVMX_SPXX_INT_MSK(1));
-			if (spx_int_reg.s.spf)
-				pr_err("SPI1: SRX Spi4 interface down\n");
-			if (spx_int_reg.s.calerr)
-				pr_err("SPI1: SRX Spi4 Calendar table parity error\n");
-			if (spx_int_reg.s.syncerr)
-				pr_err("SPI1: SRX Consecutive Spi4 DIP4 errors have exceeded SPX_ERR_CTL[ERRCNT]\n");
-			if (spx_int_reg.s.diperr)
-				pr_err("SPI1: SRX Spi4 DIP4 error\n");
-			if (spx_int_reg.s.tpaovr)
-				pr_err("SPI1: SRX Selected port has hit TPA overflow\n");
-			if (spx_int_reg.s.rsverr)
-				pr_err("SPI1: SRX Spi4 reserved control word detected\n");
-			if (spx_int_reg.s.drwnng)
-				pr_err("SPI1: SRX Spi4 receive FIFO drowning/overflow\n");
-			if (spx_int_reg.s.clserr)
-				pr_err("SPI1: SRX Spi4 packet closed on non-16B alignment without EOP\n");
-			if (spx_int_reg.s.spiovr)
-				pr_err("SPI1: SRX Spi4 async FIFO overflow\n");
-			if (spx_int_reg.s.abnorm)
-				pr_err("SPI1: SRX Abnormal packet termination (ERR bit)\n");
-			if (spx_int_reg.s.prtnxa)
-				pr_err("SPI1: SRX Port out of range\n");
-		}
-
-		stx_int_reg.u64 = cvmx_read_csr(CVMX_STXX_INT_REG(1));
-		cvmx_write_csr(CVMX_STXX_INT_REG(1), stx_int_reg.u64);
-		if (!need_retrain[1]) {
+	if (spx_int_reg.s.spf)
+		pr_err("SPI%d: SRX Spi4 interface down\n", index);
+	if (spx_int_reg.s.calerr)
+		pr_err("SPI%d: SRX Spi4 Calendar table parity error\n", index);
+	if (spx_int_reg.s.syncerr)
+		pr_err("SPI%d: SRX Consecutive Spi4 DIP4 errors have exceeded SPX_ERR_CTL[ERRCNT]\n",
+		       index);
+	if (spx_int_reg.s.diperr)
+		pr_err("SPI%d: SRX Spi4 DIP4 error\n", index);
+	if (spx_int_reg.s.tpaovr)
+		pr_err("SPI%d: SRX Selected port has hit TPA overflow\n",
+		       index);
+	if (spx_int_reg.s.rsverr)
+		pr_err("SPI%d: SRX Spi4 reserved control word detected\n",
+		       index);
+	if (spx_int_reg.s.drwnng)
+		pr_err("SPI%d: SRX Spi4 receive FIFO drowning/overflow\n",
+		       index);
+	if (spx_int_reg.s.clserr)
+		pr_err("SPI%d: SRX Spi4 packet closed on non-16B alignment without EOP\n",
+		       index);
+	if (spx_int_reg.s.spiovr)
+		pr_err("SPI%d: SRX Spi4 async FIFO overflow\n", index);
+	if (spx_int_reg.s.abnorm)
+		pr_err("SPI%d: SRX Abnormal packet termination (ERR bit)\n",
+		       index);
+	if (spx_int_reg.s.prtnxa)
+		pr_err("SPI%d: SRX Port out of range\n", index);
+}
 
-			stx_int_reg.u64 &= cvmx_read_csr(CVMX_STXX_INT_MSK(1));
-			if (stx_int_reg.s.syncerr)
-				pr_err("SPI1: STX Interface encountered a fatal error\n");
-			if (stx_int_reg.s.frmerr)
-				pr_err("SPI1: STX FRMCNT has exceeded STX_DIP_CNT[MAXFRM]\n");
-			if (stx_int_reg.s.unxfrm)
-				pr_err("SPI1: STX Unexpected framing sequence\n");
-			if (stx_int_reg.s.nosync)
-				pr_err("SPI1: STX ERRCNT has exceeded STX_DIP_CNT[MAXDIP]\n");
-			if (stx_int_reg.s.diperr)
-				pr_err("SPI1: STX DIP2 error on the Spi4 Status channel\n");
-			if (stx_int_reg.s.datovr)
-				pr_err("SPI1: STX Spi4 FIFO overflow error\n");
-			if (stx_int_reg.s.ovrbst)
-				pr_err("SPI1: STX Transmit packet burst too big\n");
-			if (stx_int_reg.s.calpar1)
-				pr_err("SPI1: STX Calendar Table Parity Error Bank1\n");
-			if (stx_int_reg.s.calpar0)
-				pr_err("SPI1: STX Calendar Table Parity Error Bank0\n");
-		}
+static void cvm_oct_stxx_int_pr(union cvmx_stxx_int_reg stx_int_reg, int index)
+{
+	if (stx_int_reg.s.syncerr)
+		pr_err("SPI%d: STX Interface encountered a fatal error\n",
+		       index);
+	if (stx_int_reg.s.frmerr)
+		pr_err("SPI%d: STX FRMCNT has exceeded STX_DIP_CNT[MAXFRM]\n",
+		       index);
+	if (stx_int_reg.s.unxfrm)
+		pr_err("SPI%d: STX Unexpected framing sequence\n", index);
+	if (stx_int_reg.s.nosync)
+		pr_err("SPI%d: STX ERRCNT has exceeded STX_DIP_CNT[MAXDIP]\n",
+		       index);
+	if (stx_int_reg.s.diperr)
+		pr_err("SPI%d: STX DIP2 error on the Spi4 Status channel\n",
+		       index);
+	if (stx_int_reg.s.datovr)
+		pr_err("SPI%d: STX Spi4 FIFO overflow error\n", index);
+	if (stx_int_reg.s.ovrbst)
+		pr_err("SPI%d: STX Transmit packet burst too big\n", index);
+	if (stx_int_reg.s.calpar1)
+		pr_err("SPI%d: STX Calendar Table Parity Error Bank%d\n",
+		       index, 1);
+	if (stx_int_reg.s.calpar0)
+		pr_err("SPI%d: STX Calendar Table Parity Error Bank%d\n",
+		       index, 0);
+}
 
-		cvmx_write_csr(CVMX_SPXX_INT_MSK(1), 0);
-		cvmx_write_csr(CVMX_STXX_INT_MSK(1), 0);
-		need_retrain[1] = 1;
-		return_status = IRQ_HANDLED;
+static irqreturn_t cvm_oct_spi_spx_int(int index)
+{
+	union cvmx_spxx_int_reg spx_int_reg;
+	union cvmx_stxx_int_reg stx_int_reg;
+
+	spx_int_reg.u64 = cvmx_read_csr(CVMX_SPXX_INT_REG(index));
+	cvmx_write_csr(CVMX_SPXX_INT_REG(index), spx_int_reg.u64);
+	if (!need_retrain[index]) {
+		spx_int_reg.u64 &= cvmx_read_csr(CVMX_SPXX_INT_MSK(index));
+		cvm_oct_spxx_int_pr(spx_int_reg, index);
 	}
 
-	if (rsl_int_blocks.s.spx0) {	/* 18 - SPX0_INT_REG & STX0_INT_REG */
-		union cvmx_spxx_int_reg spx_int_reg;
-		union cvmx_stxx_int_reg stx_int_reg;
+	stx_int_reg.u64 = cvmx_read_csr(CVMX_STXX_INT_REG(index));
+	cvmx_write_csr(CVMX_STXX_INT_REG(index), stx_int_reg.u64);
+	if (!need_retrain[index]) {
+		stx_int_reg.u64 &= cvmx_read_csr(CVMX_STXX_INT_MSK(index));
+		cvm_oct_stxx_int_pr(stx_int_reg, index);
+	}
 
-		spx_int_reg.u64 = cvmx_read_csr(CVMX_SPXX_INT_REG(0));
-		cvmx_write_csr(CVMX_SPXX_INT_REG(0), spx_int_reg.u64);
-		if (!need_retrain[0]) {
+	cvmx_write_csr(CVMX_SPXX_INT_MSK(index), 0);
+	cvmx_write_csr(CVMX_STXX_INT_MSK(index), 0);
+	need_retrain[index] = 1;
 
-			spx_int_reg.u64 &= cvmx_read_csr(CVMX_SPXX_INT_MSK(0));
-			if (spx_int_reg.s.spf)
-				pr_err("SPI0: SRX Spi4 interface down\n");
-			if (spx_int_reg.s.calerr)
-				pr_err("SPI0: SRX Spi4 Calendar table parity error\n");
-			if (spx_int_reg.s.syncerr)
-				pr_err("SPI0: SRX Consecutive Spi4 DIP4 errors have exceeded SPX_ERR_CTL[ERRCNT]\n");
-			if (spx_int_reg.s.diperr)
-				pr_err("SPI0: SRX Spi4 DIP4 error\n");
-			if (spx_int_reg.s.tpaovr)
-				pr_err("SPI0: SRX Selected port has hit TPA overflow\n");
-			if (spx_int_reg.s.rsverr)
-				pr_err("SPI0: SRX Spi4 reserved control word detected\n");
-			if (spx_int_reg.s.drwnng)
-				pr_err("SPI0: SRX Spi4 receive FIFO drowning/overflow\n");
-			if (spx_int_reg.s.clserr)
-				pr_err("SPI0: SRX Spi4 packet closed on non-16B alignment without EOP\n");
-			if (spx_int_reg.s.spiovr)
-				pr_err("SPI0: SRX Spi4 async FIFO overflow\n");
-			if (spx_int_reg.s.abnorm)
-				pr_err("SPI0: SRX Abnormal packet termination (ERR bit)\n");
-			if (spx_int_reg.s.prtnxa)
-				pr_err("SPI0: SRX Port out of range\n");
-		}
+	return IRQ_HANDLED;
+}
 
-		stx_int_reg.u64 = cvmx_read_csr(CVMX_STXX_INT_REG(0));
-		cvmx_write_csr(CVMX_STXX_INT_REG(0), stx_int_reg.u64);
-		if (!need_retrain[0]) {
+static irqreturn_t cvm_oct_spi_rml_interrupt(int cpl, void *dev_id)
+{
+	irqreturn_t return_status = IRQ_NONE;
+	union cvmx_npi_rsl_int_blocks rsl_int_blocks;
 
-			stx_int_reg.u64 &= cvmx_read_csr(CVMX_STXX_INT_MSK(0));
-			if (stx_int_reg.s.syncerr)
-				pr_err("SPI0: STX Interface encountered a fatal error\n");
-			if (stx_int_reg.s.frmerr)
-				pr_err("SPI0: STX FRMCNT has exceeded STX_DIP_CNT[MAXFRM]\n");
-			if (stx_int_reg.s.unxfrm)
-				pr_err("SPI0: STX Unexpected framing sequence\n");
-			if (stx_int_reg.s.nosync)
-				pr_err("SPI0: STX ERRCNT has exceeded STX_DIP_CNT[MAXDIP]\n");
-			if (stx_int_reg.s.diperr)
-				pr_err("SPI0: STX DIP2 error on the Spi4 Status channel\n");
-			if (stx_int_reg.s.datovr)
-				pr_err("SPI0: STX Spi4 FIFO overflow error\n");
-			if (stx_int_reg.s.ovrbst)
-				pr_err("SPI0: STX Transmit packet burst too big\n");
-			if (stx_int_reg.s.calpar1)
-				pr_err("SPI0: STX Calendar Table Parity Error Bank1\n");
-			if (stx_int_reg.s.calpar0)
-				pr_err("SPI0: STX Calendar Table Parity Error Bank0\n");
-		}
+	/* Check and see if this interrupt was caused by the GMX block */
+	rsl_int_blocks.u64 = cvmx_read_csr(CVMX_NPI_RSL_INT_BLOCKS);
+	if (rsl_int_blocks.s.spx1) /* 19 - SPX1_INT_REG & STX1_INT_REG */
+		return_status = cvm_oct_spi_spx_int(1);
 
-		cvmx_write_csr(CVMX_SPXX_INT_MSK(0), 0);
-		cvmx_write_csr(CVMX_STXX_INT_MSK(0), 0);
-		need_retrain[0] = 1;
-		return_status = IRQ_HANDLED;
-	}
+	if (rsl_int_blocks.s.spx0) /* 18 - SPX0_INT_REG & STX0_INT_REG */
+		return_status = cvm_oct_spi_spx_int(0);
 
 	return return_status;
 }
diff --git a/drivers/staging/octeon/ethernet-tx.c b/drivers/staging/octeon/ethernet-tx.c
index 5b9ac1f6d6f0..7c1c1b052b7d 100644
--- a/drivers/staging/octeon/ethernet-tx.c
+++ b/drivers/staging/octeon/ethernet-tx.c
@@ -1,29 +1,13 @@
-/*********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2010 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-*********************************************************************/
+ */
+
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
@@ -411,7 +395,7 @@ int cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev)
 dont_put_skbuff_in_hw:
 
 	/* Check if we can use the hardware checksumming */
-	if (USE_HW_TCPUDP_CHECKSUM && (skb->protocol == htons(ETH_P_IP)) &&
+	if ((skb->protocol == htons(ETH_P_IP)) &&
 	    (ip_hdr(skb)->version == 4) && (ip_hdr(skb)->ihl == 5) &&
 	    ((ip_hdr(skb)->frag_off == 0) || (ip_hdr(skb)->frag_off == htons(1 << 14)))
 	    && ((ip_hdr(skb)->protocol == IPPROTO_TCP)
@@ -576,7 +560,7 @@ int cvm_oct_xmit_pow(struct sk_buff *skb, struct net_device *dev)
 	if (unlikely(packet_buffer == NULL)) {
 		printk_ratelimited("%s: Failed to allocate a packet buffer\n",
 				   dev->name);
-		cvmx_fpa_free(work, CVMX_FPA_WQE_POOL, DONT_WRITEBACK(1));
+		cvmx_fpa_free(work, CVMX_FPA_WQE_POOL, 1);
 		priv->stats.tx_dropped++;
 		dev_kfree_skb_any(skb);
 		return 0;
diff --git a/drivers/staging/octeon/ethernet-tx.h b/drivers/staging/octeon/ethernet-tx.h
index 547680c6c371..84848e4c1664 100644
--- a/drivers/staging/octeon/ethernet-tx.h
+++ b/drivers/staging/octeon/ethernet-tx.h
@@ -1,29 +1,12 @@
-/*********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-*********************************************************************/
+ */
 
 int cvm_oct_xmit(struct sk_buff *skb, struct net_device *dev);
 int cvm_oct_xmit_pow(struct sk_buff *skb, struct net_device *dev);
diff --git a/drivers/staging/octeon/ethernet-util.h b/drivers/staging/octeon/ethernet-util.h
index 0f9b4a18fc27..1ba789a7741b 100644
--- a/drivers/staging/octeon/ethernet-util.h
+++ b/drivers/staging/octeon/ethernet-util.h
@@ -1,29 +1,12 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-*********************************************************************/
+ */
 
 /**
  * cvm_oct_get_buffer_ptr - convert packet data address to pointer
diff --git a/drivers/staging/octeon/ethernet-xaui.c b/drivers/staging/octeon/ethernet-xaui.c
index fd9d103d8e56..4b47bcfaabb1 100644
--- a/drivers/staging/octeon/ethernet-xaui.c
+++ b/drivers/staging/octeon/ethernet-xaui.c
@@ -1,29 +1,13 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
+
 #include <linux/phy.h>
 #include <linux/kernel.h>
 #include <linux/netdevice.h>
@@ -41,89 +25,9 @@
 
 #include <asm/octeon/cvmx-gmxx-defs.h>
 
-static void cvm_oct_xaui_poll(struct net_device *dev)
-{
-	struct octeon_ethernet *priv = netdev_priv(dev);
-	cvmx_helper_link_info_t link_info;
-
-	link_info = cvmx_helper_link_get(priv->port);
-	if (link_info.u64 == priv->link_info)
-		return;
-
-	link_info = cvmx_helper_link_autoconf(priv->port);
-	priv->link_info = link_info.u64;
-
-	/* Tell Linux */
-	if (link_info.s.link_up) {
-
-		if (!netif_carrier_ok(dev))
-			netif_carrier_on(dev);
-		if (priv->queue != -1)
-			printk_ratelimited
-				("%s: %u Mbps %s duplex, port %2d, queue %2d\n",
-				 dev->name, link_info.s.speed,
-				 (link_info.s.full_duplex) ? "Full" : "Half",
-				 priv->port, priv->queue);
-		else
-			printk_ratelimited
-				("%s: %u Mbps %s duplex, port %2d, POW\n",
-				 dev->name, link_info.s.speed,
-				 (link_info.s.full_duplex) ? "Full" : "Half",
-				 priv->port);
-	} else {
-		if (netif_carrier_ok(dev))
-			netif_carrier_off(dev);
-		printk_ratelimited("%s: Link down\n", dev->name);
-	}
-}
-
 int cvm_oct_xaui_open(struct net_device *dev)
 {
-	union cvmx_gmxx_prtx_cfg gmx_cfg;
-	struct octeon_ethernet *priv = netdev_priv(dev);
-	int interface = INTERFACE(priv->port);
-	int index = INDEX(priv->port);
-	cvmx_helper_link_info_t link_info;
-	int rv;
-
-	rv = cvm_oct_phy_setup_device(dev);
-	if (rv)
-		return rv;
-
-	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
-	gmx_cfg.s.en = 1;
-	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
-
-	if (octeon_is_simulation())
-		return 0;
-
-	if (priv->phydev) {
-		int r = phy_read_status(priv->phydev);
-
-		if (r == 0 && priv->phydev->link == 0)
-			netif_carrier_off(dev);
-		cvm_oct_adjust_link(dev);
-	} else {
-		link_info = cvmx_helper_link_get(priv->port);
-		if (!link_info.s.link_up)
-			netif_carrier_off(dev);
-		priv->poll = cvm_oct_xaui_poll;
-		cvm_oct_xaui_poll(dev);
-	}
-	return 0;
-}
-
-int cvm_oct_xaui_stop(struct net_device *dev)
-{
-	union cvmx_gmxx_prtx_cfg gmx_cfg;
-	struct octeon_ethernet *priv = netdev_priv(dev);
-	int interface = INTERFACE(priv->port);
-	int index = INDEX(priv->port);
-
-	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
-	gmx_cfg.s.en = 0;
-	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
-	return cvm_oct_common_stop(dev);
+	return cvm_oct_common_open(dev, cvm_oct_link_poll, true);
 }
 
 int cvm_oct_xaui_init(struct net_device *dev)
@@ -131,14 +35,8 @@ int cvm_oct_xaui_init(struct net_device *dev)
 	struct octeon_ethernet *priv = netdev_priv(dev);
 
 	cvm_oct_common_init(dev);
-	dev->netdev_ops->ndo_stop(dev);
 	if (!octeon_is_simulation() && priv->phydev == NULL)
-		priv->poll = cvm_oct_xaui_poll;
+		priv->poll = cvm_oct_link_poll;
 
 	return 0;
 }
-
-void cvm_oct_xaui_uninit(struct net_device *dev)
-{
-	cvm_oct_common_uninit(dev);
-}
diff --git a/drivers/staging/octeon/ethernet.c b/drivers/staging/octeon/ethernet.c
index fbbe866485c7..f9dba23a3759 100644
--- a/drivers/staging/octeon/ethernet.c
+++ b/drivers/staging/octeon/ethernet.c
@@ -1,29 +1,13 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2007 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
+
 #include <linux/platform_device.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -180,10 +164,7 @@ static void cvm_oct_configure_common_hw(void)
 	}
 #endif
 
-	if (USE_RED)
-		cvmx_helper_setup_red(num_packet_buffers / 4,
-				      num_packet_buffers / 8);
-
+	cvmx_helper_setup_red(num_packet_buffers / 4, num_packet_buffers / 8);
 }
 
 /**
@@ -206,11 +187,10 @@ int cvm_oct_free_work(void *work_queue_entry)
 		if (unlikely(!segment_ptr.s.i))
 			cvmx_fpa_free(cvm_oct_get_buffer_ptr(segment_ptr),
 				      segment_ptr.s.pool,
-				      DONT_WRITEBACK(CVMX_FPA_PACKET_POOL_SIZE /
-						     128));
+				      CVMX_FPA_PACKET_POOL_SIZE / 128);
 		segment_ptr = next_ptr;
 	}
-	cvmx_fpa_free(work, CVMX_FPA_WQE_POOL, DONT_WRITEBACK(1));
+	cvmx_fpa_free(work, CVMX_FPA_WQE_POOL, 1);
 
 	return 0;
 }
@@ -468,11 +448,8 @@ int cvm_oct_common_init(struct net_device *dev)
 	    && (always_use_pow || strstr(pow_send_list, dev->name)))
 		priv->queue = -1;
 
-	if (priv->queue != -1) {
-		dev->features |= NETIF_F_SG;
-		if (USE_HW_TCPUDP_CHECKSUM)
-			dev->features |= NETIF_F_IP_CSUM;
-	}
+	if (priv->queue != -1)
+		dev->features |= NETIF_F_SG | NETIF_F_IP_CSUM;
 
 	/* We do our own locking, Linux doesn't need to */
 	dev->features |= NETIF_F_LLTX;
@@ -488,6 +465,9 @@ int cvm_oct_common_init(struct net_device *dev)
 	memset(dev->netdev_ops->ndo_get_stats(dev), 0,
 	       sizeof(struct net_device_stats));
 
+	if (dev->netdev_ops->ndo_stop)
+		dev->netdev_ops->ndo_stop(dev);
+
 	return 0;
 }
 
@@ -499,6 +479,66 @@ void cvm_oct_common_uninit(struct net_device *dev)
 		phy_disconnect(priv->phydev);
 }
 
+int cvm_oct_common_open(struct net_device *dev,
+			void (*link_poll)(struct net_device *), bool poll_now)
+{
+	union cvmx_gmxx_prtx_cfg gmx_cfg;
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	int interface = INTERFACE(priv->port);
+	int index = INDEX(priv->port);
+	cvmx_helper_link_info_t link_info;
+	int rv;
+
+	rv = cvm_oct_phy_setup_device(dev);
+	if (rv)
+		return rv;
+
+	gmx_cfg.u64 = cvmx_read_csr(CVMX_GMXX_PRTX_CFG(index, interface));
+	gmx_cfg.s.en = 1;
+	cvmx_write_csr(CVMX_GMXX_PRTX_CFG(index, interface), gmx_cfg.u64);
+
+	if (octeon_is_simulation())
+		return 0;
+
+	if (priv->phydev) {
+		int r = phy_read_status(priv->phydev);
+
+		if (r == 0 && priv->phydev->link == 0)
+			netif_carrier_off(dev);
+		cvm_oct_adjust_link(dev);
+	} else {
+		link_info = cvmx_helper_link_get(priv->port);
+		if (!link_info.s.link_up)
+			netif_carrier_off(dev);
+		priv->poll = link_poll;
+		if (poll_now)
+			link_poll(dev);
+	}
+
+	return 0;
+}
+
+void cvm_oct_link_poll(struct net_device *dev)
+{
+	struct octeon_ethernet *priv = netdev_priv(dev);
+	cvmx_helper_link_info_t link_info;
+
+	link_info = cvmx_helper_link_get(priv->port);
+	if (link_info.u64 == priv->link_info)
+		return;
+
+	link_info = cvmx_helper_link_autoconf(priv->port);
+	priv->link_info = link_info.u64;
+
+	if (link_info.s.link_up) {
+		if (!netif_carrier_ok(dev))
+			netif_carrier_on(dev);
+	} else if (netif_carrier_ok(dev)) {
+		netif_carrier_off(dev);
+	}
+	cvm_oct_note_carrier(priv, link_info);
+}
+
 static const struct net_device_ops cvm_oct_npi_netdev_ops = {
 	.ndo_init		= cvm_oct_common_init,
 	.ndo_uninit		= cvm_oct_common_uninit,
@@ -514,9 +554,9 @@ static const struct net_device_ops cvm_oct_npi_netdev_ops = {
 };
 static const struct net_device_ops cvm_oct_xaui_netdev_ops = {
 	.ndo_init		= cvm_oct_xaui_init,
-	.ndo_uninit		= cvm_oct_xaui_uninit,
+	.ndo_uninit		= cvm_oct_common_uninit,
 	.ndo_open		= cvm_oct_xaui_open,
-	.ndo_stop		= cvm_oct_xaui_stop,
+	.ndo_stop		= cvm_oct_common_stop,
 	.ndo_start_xmit		= cvm_oct_xmit,
 	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
 	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
@@ -529,9 +569,9 @@ static const struct net_device_ops cvm_oct_xaui_netdev_ops = {
 };
 static const struct net_device_ops cvm_oct_sgmii_netdev_ops = {
 	.ndo_init		= cvm_oct_sgmii_init,
-	.ndo_uninit		= cvm_oct_sgmii_uninit,
+	.ndo_uninit		= cvm_oct_common_uninit,
 	.ndo_open		= cvm_oct_sgmii_open,
-	.ndo_stop		= cvm_oct_sgmii_stop,
+	.ndo_stop		= cvm_oct_common_stop,
 	.ndo_start_xmit		= cvm_oct_xmit,
 	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
 	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
@@ -559,7 +599,7 @@ static const struct net_device_ops cvm_oct_rgmii_netdev_ops = {
 	.ndo_init		= cvm_oct_rgmii_init,
 	.ndo_uninit		= cvm_oct_rgmii_uninit,
 	.ndo_open		= cvm_oct_rgmii_open,
-	.ndo_stop		= cvm_oct_rgmii_stop,
+	.ndo_stop		= cvm_oct_common_stop,
 	.ndo_start_xmit		= cvm_oct_xmit,
 	.ndo_set_rx_mode	= cvm_oct_common_set_multicast_list,
 	.ndo_set_mac_address	= cvm_oct_common_set_mac_address,
@@ -625,7 +665,6 @@ static int cvm_oct_probe(struct platform_device *pdev)
 	struct device_node *pip;
 
 	octeon_mdiobus_force_mod_depencency();
-	pr_notice("cavium-ethernet %s\n", OCTEON_ETHERNET_VERSION);
 
 	pip = pdev->dev.of_node;
 	if (!pip) {
diff --git a/drivers/staging/octeon/octeon-ethernet.h b/drivers/staging/octeon/octeon-ethernet.h
index f48dc766fada..e9d3e9a7e8a7 100644
--- a/drivers/staging/octeon/octeon-ethernet.h
+++ b/drivers/staging/octeon/octeon-ethernet.h
@@ -1,29 +1,12 @@
-/**********************************************************************
- * Author: Cavium Networks
- *
- * Contact: support@caviumnetworks.com
- * This file is part of the OCTEON SDK
+/*
+ * This file is based on code from OCTEON SDK by Cavium Networks.
  *
  * Copyright (c) 2003-2010 Cavium Networks
  *
  * This file is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License, Version 2, as
  * published by the Free Software Foundation.
- *
- * This file is distributed in the hope that it will be useful, but
- * AS-IS and WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE, TITLE, or
- * NONINFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this file; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
- * or visit http://www.gnu.org/licenses/.
- *
- * This file may also be available under a different license from Cavium.
- * Contact Cavium Networks for more information
-**********************************************************************/
+ */
 
 /*
  * External interface for the Cavium Octeon ethernet driver.
@@ -33,6 +16,8 @@
 
 #include <linux/of.h>
 
+#include <asm/octeon/cvmx-helper-board.h>
+
 /**
  * This is the definition of the Ethernet driver's private
  * driver state stored in netdev_priv(dev).
@@ -71,24 +56,24 @@ int cvm_oct_free_work(void *work_queue_entry);
 extern int cvm_oct_rgmii_init(struct net_device *dev);
 extern void cvm_oct_rgmii_uninit(struct net_device *dev);
 extern int cvm_oct_rgmii_open(struct net_device *dev);
-extern int cvm_oct_rgmii_stop(struct net_device *dev);
 
 extern int cvm_oct_sgmii_init(struct net_device *dev);
-extern void cvm_oct_sgmii_uninit(struct net_device *dev);
 extern int cvm_oct_sgmii_open(struct net_device *dev);
-extern int cvm_oct_sgmii_stop(struct net_device *dev);
 
 extern int cvm_oct_spi_init(struct net_device *dev);
 extern void cvm_oct_spi_uninit(struct net_device *dev);
 extern int cvm_oct_xaui_init(struct net_device *dev);
-extern void cvm_oct_xaui_uninit(struct net_device *dev);
 extern int cvm_oct_xaui_open(struct net_device *dev);
-extern int cvm_oct_xaui_stop(struct net_device *dev);
 
 extern int cvm_oct_common_init(struct net_device *dev);
 extern void cvm_oct_common_uninit(struct net_device *dev);
 void cvm_oct_adjust_link(struct net_device *dev);
 int cvm_oct_common_stop(struct net_device *dev);
+int cvm_oct_common_open(struct net_device *dev,
+			void (*link_poll)(struct net_device *), bool poll_now);
+void cvm_oct_note_carrier(struct octeon_ethernet *priv,
+			  cvmx_helper_link_info_t li);
+void cvm_oct_link_poll(struct net_device *dev);
 
 extern int always_use_pow;
 extern int pow_send_group;
diff --git a/drivers/staging/ozwpan/ozhcd.c b/drivers/staging/ozwpan/ozhcd.c
index 5ff4716b72c3..784b5ecfa849 100644
--- a/drivers/staging/ozwpan/ozhcd.c
+++ b/drivers/staging/ozwpan/ozhcd.c
@@ -746,8 +746,8 @@ void oz_hcd_pd_reset(void *hpd, void *hport)
 /*
  * Context: softirq
  */
-void oz_hcd_get_desc_cnf(void *hport, u8 req_id, int status, const u8 *desc,
-			int length, int offset, int total_size)
+void oz_hcd_get_desc_cnf(void *hport, u8 req_id, u8 status, const u8 *desc,
+			u8 length, u16 offset, u16 total_size)
 {
 	struct oz_port *port = hport;
 	struct urb *urb;
@@ -759,8 +759,8 @@ void oz_hcd_get_desc_cnf(void *hport, u8 req_id, int status, const u8 *desc,
 	if (!urb)
 		return;
 	if (status == 0) {
-		int copy_len;
-		int required_size = urb->transfer_buffer_length;
+		unsigned int copy_len;
+		unsigned int required_size = urb->transfer_buffer_length;
 
 		if (required_size > total_size)
 			required_size = total_size;
diff --git a/drivers/staging/ozwpan/ozusbif.h b/drivers/staging/ozwpan/ozusbif.h
index 4249fa374012..d2a6085345be 100644
--- a/drivers/staging/ozwpan/ozusbif.h
+++ b/drivers/staging/ozwpan/ozusbif.h
@@ -29,8 +29,8 @@ void oz_usb_request_heartbeat(void *hpd);
 
 /* Confirmation functions.
  */
-void oz_hcd_get_desc_cnf(void *hport, u8 req_id, int status,
-	const u8 *desc, int length, int offset, int total_size);
+void oz_hcd_get_desc_cnf(void *hport, u8 req_id, u8 status,
+	const u8 *desc, u8 length, u16 offset, u16 total_size);
 void oz_hcd_control_cnf(void *hport, u8 req_id, u8 rcode,
 	const u8 *data, int data_len);
 
diff --git a/drivers/staging/ozwpan/ozusbsvc1.c b/drivers/staging/ozwpan/ozusbsvc1.c
index d434d8c6fff6..301fee8625ed 100644
--- a/drivers/staging/ozwpan/ozusbsvc1.c
+++ b/drivers/staging/ozwpan/ozusbsvc1.c
@@ -326,7 +326,11 @@ static void oz_usb_handle_ep_data(struct oz_usb_ctx *usb_ctx,
 			struct oz_multiple_fixed *body =
 				(struct oz_multiple_fixed *)data_hdr;
 			u8 *data = body->data;
-			int n = (len - sizeof(struct oz_multiple_fixed)+1)
+			unsigned int n;
+			if (!body->unit_size ||
+				len < sizeof(struct oz_multiple_fixed) - 1)
+				break;
+			n = (len - (sizeof(struct oz_multiple_fixed) - 1))
 				/ body->unit_size;
 			while (n--) {
 				oz_hcd_data_ind(usb_ctx->hport, body->endpoint,
@@ -338,12 +342,16 @@ static void oz_usb_handle_ep_data(struct oz_usb_ctx *usb_ctx,
 	case OZ_DATA_F_ISOC_FIXED: {
 			struct oz_isoc_fixed *body =
 				(struct oz_isoc_fixed *)data_hdr;
-			int data_len = len-sizeof(struct oz_isoc_fixed)+1;
+			int data_len;
 			int unit_size = body->unit_size;
 			u8 *data = body->data;
 			int count;
 			int i;
 
+			if (len < sizeof(struct oz_isoc_fixed) - 1)
+				break;
+			data_len = len - (sizeof(struct oz_isoc_fixed) - 1);
+
 			if (!unit_size)
 				break;
 			count = data_len/unit_size;
@@ -390,10 +398,15 @@ void oz_usb_rx(struct oz_pd *pd, struct oz_elt *elt)
 	case OZ_GET_DESC_RSP: {
 			struct oz_get_desc_rsp *body =
 				(struct oz_get_desc_rsp *)usb_hdr;
-			int data_len = elt->length -
-					sizeof(struct oz_get_desc_rsp) + 1;
-			u16 offs = le16_to_cpu(get_unaligned(&body->offset));
-			u16 total_size =
+			u16 offs, total_size;
+			u8 data_len;
+
+			if (elt->length < sizeof(struct oz_get_desc_rsp) - 1)
+				break;
+			data_len = elt->length -
+					(sizeof(struct oz_get_desc_rsp) - 1);
+			offs = le16_to_cpu(get_unaligned(&body->offset));
+			total_size =
 				le16_to_cpu(get_unaligned(&body->total_size));
 			oz_dbg(ON, "USB_REQ_GET_DESCRIPTOR - cnf\n");
 			oz_hcd_get_desc_cnf(usb_ctx->hport, body->req_id,
@@ -418,6 +431,11 @@ void oz_usb_rx(struct oz_pd *pd, struct oz_elt *elt)
 	case OZ_VENDOR_CLASS_RSP: {
 			struct oz_vendor_class_rsp *body =
 				(struct oz_vendor_class_rsp *)usb_hdr;
+
+			if (elt->length <
+			    sizeof(struct oz_vendor_class_rsp) - 1)
+				break;
+
 			oz_hcd_control_cnf(usb_ctx->hport, body->req_id,
 				body->rcode, body->data, elt->length-
 				sizeof(struct oz_vendor_class_rsp)+1);
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index ea54fb4ec837..bda208da514e 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -775,12 +775,10 @@ static void pin_to_bits(int pin, unsigned char *d_val, unsigned char *c_val)
 /* sleeps that many milliseconds with a reschedule */
 static void long_sleep(int ms)
 {
-	if (in_interrupt()) {
+	if (in_interrupt())
 		mdelay(ms);
-	} else {
-		__set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout((ms * HZ + 999) / 1000);
-	}
+	else
+		schedule_timeout_interruptible(msecs_to_jiffies(ms));
 }
 
 /* send a serial byte to the LCD panel. The caller is responsible for locking
@@ -2190,6 +2188,8 @@ static struct notifier_block panel_notifier = {
 
 static void panel_attach(struct parport *port)
 {
+	struct pardev_cb panel_cb;
+
 	if (port->number != parport)
 		return;
 
@@ -2199,10 +2199,11 @@ static void panel_attach(struct parport *port)
 		return;
 	}
 
-	pprt = parport_register_device(port, "panel", NULL, NULL,  /* pf, kf */
-				       NULL,
-				       /*PARPORT_DEV_EXCL */
-				       0, (void *)&pprt);
+	memset(&panel_cb, 0, sizeof(panel_cb));
+	panel_cb.private = &pprt;
+	/* panel_cb.flags = 0 should be PARPORT_DEV_EXCL? */
+
+	pprt = parport_register_dev_model(port, "panel", &panel_cb, 0);
 	if (pprt == NULL) {
 		pr_err("%s: port->number=%d parport=%d, parport_register_device() failed\n",
 		       __func__, port->number, parport);
@@ -2250,28 +2251,35 @@ static void panel_detach(struct parport *port)
 		       __func__, port->number, parport);
 		return;
 	}
+	if (scan_timer.function != NULL)
+		del_timer_sync(&scan_timer);
 
-	unregister_reboot_notifier(&panel_notifier);
+	if (pprt != NULL) {
+		if (keypad.enabled) {
+			misc_deregister(&keypad_dev);
+			keypad_initialized = 0;
+		}
 
-	if (keypad.enabled && keypad_initialized) {
-		misc_deregister(&keypad_dev);
-		keypad_initialized = 0;
-	}
+		if (lcd.enabled) {
+			panel_lcd_print("\x0cLCD driver " PANEL_VERSION
+					"\nunloaded.\x1b[Lc\x1b[Lb\x1b[L-");
+			misc_deregister(&lcd_dev);
+			lcd.initialized = false;
+		}
 
-	if (lcd.enabled && lcd.initialized) {
-		misc_deregister(&lcd_dev);
-		lcd.initialized = false;
+		/* TODO: free all input signals */
+		parport_release(pprt);
+		parport_unregister_device(pprt);
+		pprt = NULL;
+		unregister_reboot_notifier(&panel_notifier);
 	}
-
-	parport_release(pprt);
-	parport_unregister_device(pprt);
-	pprt = NULL;
 }
 
 static struct parport_driver panel_driver = {
 	.name = "panel",
-	.attach = panel_attach,
+	.match_port = panel_attach,
 	.detach = panel_detach,
+	.devmodel = true,
 };
 
 /* init function */
@@ -2398,28 +2406,6 @@ static int __init panel_init_module(void)
 
 static void __exit panel_cleanup_module(void)
 {
-
-	if (scan_timer.function != NULL)
-		del_timer_sync(&scan_timer);
-
-	if (pprt != NULL) {
-		if (keypad.enabled) {
-			misc_deregister(&keypad_dev);
-			keypad_initialized = 0;
-		}
-
-		if (lcd.enabled) {
-			panel_lcd_print("\x0cLCD driver " PANEL_VERSION
-					"\nunloaded.\x1b[Lc\x1b[Lb\x1b[L-");
-			misc_deregister(&lcd_dev);
-			lcd.initialized = false;
-		}
-
-		/* TODO: free all input signals */
-		parport_release(pprt);
-		parport_unregister_device(pprt);
-		pprt = NULL;
-	}
 	parport_unregister_driver(&panel_driver);
 }
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index e65ee6e858a8..581af88e3024 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -19,6 +19,8 @@
  ******************************************************************************/
 #define _RTW_AP_C_
 
+#include <linux/ieee80211.h>
+
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <wifi.h>
@@ -81,12 +83,14 @@ static void update_BCNTIM(struct adapter *padapter)
 		u8 *pbackup_remainder_ie = NULL;
 		uint offset, tmp_len, tim_ielen, tim_ie_offset, remainder_ielen;
 
-		p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen, pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
+		p = rtw_get_ie(pie + _FIXED_IE_LENGTH_, _TIM_IE_, &tim_ielen,
+				pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_);
 		if (p != NULL && tim_ielen > 0) {
 			tim_ielen += 2;
 			premainder_ie = p+tim_ielen;
 			tim_ie_offset = (int)(p - pie);
-			remainder_ielen = pnetwork_mlmeext->IELength - tim_ie_offset - tim_ielen;
+			remainder_ielen = pnetwork_mlmeext->IELength -
+						tim_ie_offset - tim_ielen;
 			/* append TIM IE from dst_ie offset */
 			dst_ie = p;
 		} else {
@@ -97,7 +101,10 @@ static void update_BCNTIM(struct adapter *padapter)
 			offset += pnetwork_mlmeext->Ssid.SsidLength + 2;
 
 			/*  get supported rates len */
-			p = rtw_get_ie(pie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &tmp_len, (pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_));
+			p = rtw_get_ie(pie + _BEACON_IE_OFFSET_,
+					_SUPPORTEDRATES_IE_, &tmp_len,
+					(pnetwork_mlmeext->IELength -
+						_BEACON_IE_OFFSET_));
 			if (p !=  NULL)
 				offset += tmp_len+2;
 
@@ -106,7 +113,8 @@ static void update_BCNTIM(struct adapter *padapter)
 
 			premainder_ie = pie + offset;
 
-			remainder_ielen = pnetwork_mlmeext->IELength - offset - tim_ielen;
+			remainder_ielen = pnetwork_mlmeext->IELength -
+						offset - tim_ielen;
 
 			/* append TIM IE from offset */
 			dst_ie = pie + offset;
@@ -115,11 +123,13 @@ static void update_BCNTIM(struct adapter *padapter)
 		if (remainder_ielen > 0) {
 			pbackup_remainder_ie = rtw_malloc(remainder_ielen);
 			if (pbackup_remainder_ie && premainder_ie)
-				memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+				memcpy(pbackup_remainder_ie,
+						premainder_ie, remainder_ielen);
 		}
 		*dst_ie++ = _TIM_IE_;
 
-		if ((pstapriv->tim_bitmap&0xff00) && (pstapriv->tim_bitmap&0x00fc))
+		if ((pstapriv->tim_bitmap&0xff00) &&
+				(pstapriv->tim_bitmap&0x00fc))
 			tim_ielen = 5;
 		else
 			tim_ielen = 4;
@@ -154,7 +164,8 @@ static void update_BCNTIM(struct adapter *padapter)
 	set_tx_beacon_cmd(padapter);
 }
 
-void rtw_add_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork, u8 index, u8 *data, u8 len)
+void rtw_add_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork,
+			u8 index, u8 *data, u8 len)
 {
 	struct ndis_802_11_var_ie *pIE;
 	u8 bmatch = false;
@@ -168,7 +179,8 @@ void rtw_add_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork, u8
 
 		if (pIE->ElementID > index) {
 			break;
-		} else if (pIE->ElementID == index) { /*  already exist the same IE */
+		/*  already exist the same IE */
+		} else if (pIE->ElementID == index) {
 			p = (u8 *)pIE;
 			ielen = pIE->Length;
 			bmatch = true;
@@ -197,7 +209,8 @@ void rtw_add_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork, u8
 	if (remainder_ielen > 0) {
 		pbackup_remainder_ie = rtw_malloc(remainder_ielen);
 		if (pbackup_remainder_ie && premainder_ie)
-			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+			memcpy(pbackup_remainder_ie,
+					premainder_ie, remainder_ielen);
 	}
 
 	*dst_ie++ = index;
@@ -217,7 +230,8 @@ void rtw_add_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork, u8
 	pnetwork->IELength = offset + remainder_ielen;
 }
 
-void rtw_remove_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork, u8 index)
+void rtw_remove_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork,
+				u8 index)
 {
 	u8 *p, *dst_ie = NULL, *premainder_ie = NULL;
 	u8 *pbackup_remainder_ie = NULL;
@@ -241,7 +255,8 @@ void rtw_remove_bcn_ie(struct adapter *padapter, struct wlan_bssid_ex *pnetwork,
 	if (remainder_ielen > 0) {
 		pbackup_remainder_ie = rtw_malloc(remainder_ielen);
 		if (pbackup_remainder_ie && premainder_ie)
-			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
+			memcpy(pbackup_remainder_ie,
+					premainder_ie, remainder_ielen);
 	}
 
 	/* copy remainder IE */
@@ -259,8 +274,10 @@ static u8 chk_sta_is_alive(struct sta_info *psta)
 {
 	u8 ret = false;
 
-	if ((psta->sta_stats.last_rx_data_pkts + psta->sta_stats.last_rx_ctrl_pkts) ==
-	    (psta->sta_stats.rx_data_pkts + psta->sta_stats.rx_ctrl_pkts))
+	if ((psta->sta_stats.last_rx_data_pkts +
+			psta->sta_stats.last_rx_ctrl_pkts) ==
+			(psta->sta_stats.rx_data_pkts +
+			psta->sta_stats.rx_ctrl_pkts))
 		;
 	else
 		ret = true;
@@ -342,13 +359,18 @@ void	expire_timeout_chk(struct adapter *padapter)
 
 			if (psta->state & WIFI_SLEEP_STATE) {
 				if (!(psta->state & WIFI_STA_ALIVE_CHK_STATE)) {
-					/* to check if alive by another methods if station is at ps mode. */
+					/* to check if alive by another methods
+					 * if station is at ps mode.
+					 */
 					psta->expire_to = pstapriv->expire_to;
 					psta->state |= WIFI_STA_ALIVE_CHK_STATE;
 
-					/* to update bcn with tim_bitmap for this station */
+					/* to update bcn with tim_bitmap
+					 * for this station
+					 */
 					pstapriv->tim_bitmap |= BIT(psta->aid);
-					update_beacon(padapter, _TIM_IE_, NULL, false);
+					update_beacon(padapter, _TIM_IE_,
+							NULL, false);
 
 					if (!pmlmeext->active_keep_alive_check)
 						continue;
@@ -827,7 +849,7 @@ static void start_bss_network(struct adapter *padapter, u8 *pbuf)
 
 		/* issue beacon frame */
 		if (send_beacon(padapter) == _FAIL)
-			DBG_88E("issue_beacon, fail!\n");
+			DBG_88E("send_beacon, fail!\n");
 	}
 
 	/* update bc/mc sta_info */
diff --git a/drivers/staging/rtl8188eu/core/rtw_efuse.c b/drivers/staging/rtl8188eu/core/rtw_efuse.c
index b66746160223..dbaba2c6cce5 100644
--- a/drivers/staging/rtl8188eu/core/rtw_efuse.c
+++ b/drivers/staging/rtl8188eu/core/rtw_efuse.c
@@ -31,7 +31,7 @@
 
 enum{
 		VOLTAGE_V25						= 0x03,
-		LDOE25_SHIFT						= 28 ,
+		LDOE25_SHIFT						= 28,
 	};
 
 /*
diff --git a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
index 969150a48661..8c05cb021c46 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
@@ -642,21 +642,18 @@ u16 rtw_get_cur_max_rate(struct adapter *adapter)
 */
 int rtw_set_country(struct adapter *adapter, const char *country_code)
 {
+	int i;
 	int channel_plan = RT_CHANNEL_DOMAIN_WORLD_WIDE_5G;
 
 	DBG_88E("%s country_code:%s\n", __func__, country_code);
+	for (i = 0; i < ARRAY_SIZE(channel_table); i++) {
+		if (0 == strcmp(channel_table[i].name, country_code)) {
+			channel_plan = channel_table[i].channel_plan;
+			break;
+		}
+	}
 
-	/* TODO: should have a table to match country code and RT_CHANNEL_DOMAIN */
-	/* TODO: should consider 2-character and 3-character country code */
-	if (0 == strcmp(country_code, "US"))
-		channel_plan = RT_CHANNEL_DOMAIN_FCC;
-	else if (0 == strcmp(country_code, "EU"))
-		channel_plan = RT_CHANNEL_DOMAIN_ETSI;
-	else if (0 == strcmp(country_code, "JP"))
-		channel_plan = RT_CHANNEL_DOMAIN_MKK;
-	else if (0 == strcmp(country_code, "CN"))
-		channel_plan = RT_CHANNEL_DOMAIN_CHINA;
-	else
+	if (i == ARRAY_SIZE(channel_table))
 		DBG_88E("%s unknown country_code:%s\n", __func__, country_code);
 
 	return rtw_set_chplan_cmd(adapter, channel_plan, 1);
diff --git a/drivers/staging/rtl8188eu/core/rtw_led.c b/drivers/staging/rtl8188eu/core/rtw_led.c
index 94405dc44220..14461cf34037 100644
--- a/drivers/staging/rtl8188eu/core/rtw_led.c
+++ b/drivers/staging/rtl8188eu/core/rtw_led.c
@@ -477,7 +477,7 @@ void LedControl8188eu(struct adapter *padapter, enum LED_CTL_MODE LedAction)
 {
 	struct led_priv *ledpriv = &(padapter->ledpriv);
 
-       if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped) ||
+	if ((padapter->bSurpriseRemoved) || (padapter->bDriverStopped) ||
 	   (!padapter->hw_init_completed))
 		return;
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
index 6c91aa58d924..05584515c5b4 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -141,7 +141,7 @@ struct	wlan_network *_rtw_alloc_network(struct	mlme_priv *pmlmepriv)/* _queue *f
 	}
 	plist = free_queue->queue.next;
 
-	pnetwork = container_of(plist , struct wlan_network, list);
+	pnetwork = container_of(plist, struct wlan_network, list);
 
 	list_del_init(&pnetwork->list);
 
@@ -219,7 +219,7 @@ struct wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr)
 	plist = phead->next;
 
 	while (plist != phead) {
-		pnetwork = container_of(plist, struct wlan_network , list);
+		pnetwork = container_of(plist, struct wlan_network, list);
 		if (!memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN))
 			break;
 		plist = plist->next;
@@ -638,7 +638,6 @@ exit:
 void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
 {
 	struct	mlme_priv *pmlmepriv = &(adapter->mlmepriv);
-	struct mlme_ext_priv *pmlmeext;
 
 	spin_lock_bh(&pmlmepriv->lock);
 
@@ -720,15 +719,13 @@ void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
 	spin_unlock_bh(&pmlmepriv->lock);
 
 	rtw_os_xmit_schedule(adapter);
-
-	pmlmeext = &adapter->mlmeextpriv;
 }
 
-void rtw_dummy_event_callback(struct adapter *adapter , u8 *pbuf)
+void rtw_dummy_event_callback(struct adapter *adapter, u8 *pbuf)
 {
 }
 
-void rtw_fwdbg_event_callback(struct adapter *adapter , u8 *pbuf)
+void rtw_fwdbg_event_callback(struct adapter *adapter, u8 *pbuf)
 {
 }
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
index be9e34a0daef..a0b8f665fa2f 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -19,6 +19,8 @@
  ******************************************************************************/
 #define _RTW_MLME_EXT_C_
 
+#include <linux/ieee80211.h>
+
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <wifi.h>
@@ -27,42 +29,6 @@
 #include <mlme_osdep.h>
 #include <recv_osdep.h>
 
-static struct mlme_handler mlme_sta_tbl[] = {
-	{WIFI_ASSOCREQ,		"OnAssocReq",	&OnAssocReq},
-	{WIFI_ASSOCRSP,		"OnAssocRsp",	&OnAssocRsp},
-	{WIFI_REASSOCREQ,	"OnReAssocReq",	&OnAssocReq},
-	{WIFI_REASSOCRSP,	"OnReAssocRsp",	&OnAssocRsp},
-	{WIFI_PROBEREQ,		"OnProbeReq",	&OnProbeReq},
-	{WIFI_PROBERSP,		"OnProbeRsp",		&OnProbeRsp},
-
-	/*----------------------------------------------------------
-					below 2 are reserved
-	-----------------------------------------------------------*/
-	{0,					"DoReserved",		&DoReserved},
-	{0,					"DoReserved",		&DoReserved},
-	{WIFI_BEACON,		"OnBeacon",		&OnBeacon},
-	{WIFI_ATIM,			"OnATIM",		&OnAtim},
-	{WIFI_DISASSOC,		"OnDisassoc",		&OnDisassoc},
-	{WIFI_AUTH,			"OnAuth",		&OnAuthClient},
-	{WIFI_DEAUTH,		"OnDeAuth",		&OnDeAuth},
-	{WIFI_ACTION,		"OnAction",		&OnAction},
-};
-
-static struct action_handler OnAction_tbl[] = {
-	{RTW_WLAN_CATEGORY_SPECTRUM_MGMT,	 "ACTION_SPECTRUM_MGMT", on_action_spct},
-	{RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
-	{RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
-	{RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
-	{RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
-	{RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
-	{RTW_WLAN_CATEGORY_FT, "ACTION_FT",	&DoReserved},
-	{RTW_WLAN_CATEGORY_HT,	"ACTION_HT",	&OnAction_ht},
-	{RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
-	{RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
-	{RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
-};
-
-
 static u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
 
 /**************************************************
@@ -193,1628 +159,6 @@ int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch)
 	return i;
 }
 
-/****************************************************************************
-
-Following are the initialization functions for WiFi MLME
-
-*****************************************************************************/
-
-int init_hw_mlme_ext(struct adapter *padapter)
-{
-	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-
-	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
-	return _SUCCESS;
-}
-
-static void init_mlme_ext_priv_value(struct adapter *padapter)
-{
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	unsigned char	mixed_datarate[NumRates] = {
-		_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
-		_9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_,
-		 _48M_RATE_, _54M_RATE_, 0xff
-	};
-	unsigned char	mixed_basicrate[NumRates] = {
-		_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
-		_12M_RATE_, _24M_RATE_, 0xff,
-	};
-
-	atomic_set(&pmlmeext->event_seq, 0);
-	pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */
-
-	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_bwmode = pmlmeext->cur_bwmode;
-	pmlmeext->oper_ch_offset = pmlmeext->cur_ch_offset;
-	pmlmeext->retry = 0;
-
-	pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
-
-	memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
-	memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
-
-	pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
-
-	pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
-	pmlmeext->sitesurvey_res.channel_idx = 0;
-	pmlmeext->sitesurvey_res.bss_cnt = 0;
-	pmlmeext->scan_abort = false;
-
-	pmlmeinfo->state = WIFI_FW_NULL_STATE;
-	pmlmeinfo->reauth_count = 0;
-	pmlmeinfo->reassoc_count = 0;
-	pmlmeinfo->link_count = 0;
-	pmlmeinfo->auth_seq = 0;
-	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
-	pmlmeinfo->key_index = 0;
-	pmlmeinfo->iv = 0;
-
-	pmlmeinfo->enc_algo = _NO_PRIVACY_;
-	pmlmeinfo->authModeToggle = 0;
-
-	memset(pmlmeinfo->chg_txt, 0, 128);
-
-	pmlmeinfo->slotTime = SHORT_SLOT_TIME;
-	pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
-
-	pmlmeinfo->dialogToken = 0;
-
-	pmlmeext->action_public_rxseq = 0xffff;
-	pmlmeext->action_public_dialog_token = 0xff;
-}
-
-static int has_channel(struct rt_channel_info *channel_set,
-					   u8 chanset_size,
-					   u8 chan) {
-	int i;
-
-	for (i = 0; i < chanset_size; i++) {
-		if (channel_set[i].ChannelNum == chan)
-			return 1;
-	}
-	return 0;
-}
-
-static void init_channel_list(struct adapter *padapter, struct rt_channel_info *channel_set,
-							  u8 chanset_size,
-							  struct p2p_channels *channel_list) {
-	struct p2p_oper_class_map op_class[] = {
-		{ IEEE80211G,  81,   1,  13,  1, BW20 },
-		{ IEEE80211G,  82,  14,  14,  1, BW20 },
-		{ -1, 0, 0, 0, 0, BW20 }
-	};
-
-	int cla, op;
-
-	cla = 0;
-
-	for (op = 0; op_class[op].op_class; op++) {
-		u8 ch;
-		struct p2p_oper_class_map *o = &op_class[op];
-		struct p2p_reg_class *reg = NULL;
-
-		for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
-			if (!has_channel(channel_set, chanset_size, ch)) {
-				continue;
-			}
-
-			if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
-				continue;
-
-			if ((0 == (padapter->registrypriv.cbw40_enable & BIT(1))) &&
-			    ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
-				continue;
-
-			if (reg == NULL) {
-				reg = &channel_list->reg_class[cla];
-				cla++;
-				reg->reg_class = o->op_class;
-				reg->channels = 0;
-			}
-			reg->channel[reg->channels] = ch;
-			reg->channels++;
-		}
-	}
-	channel_list->reg_classes = cla;
-}
-
-static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, struct rt_channel_info *channel_set)
-{
-	u8 index, chanset_size = 0;
-	u8 b2_4GBand = false;
-	u8 Index2G = 0;
-
-	memset(channel_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
-
-	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
-		DBG_88E("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
-		return chanset_size;
-	}
-
-	if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
-		b2_4GBand = true;
-		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
-			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
-		else
-			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
-	}
-
-	if (b2_4GBand) {
-		for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) {
-			channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
-
-			if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */
-			    (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)) {
-				if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
-					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
-				else if ((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
-					channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
-			} else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
-				   RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) {/*  channel 12~13, passive scan */
-				if (channel_set[chanset_size].ChannelNum <= 11)
-					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
-				else
-					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
-			} else {
-				channel_set[chanset_size].ScanType = SCAN_ACTIVE;
-			}
-
-			chanset_size++;
-		}
-	}
-	return chanset_size;
-}
-
-int	init_mlme_ext_priv(struct adapter *padapter)
-{
-	struct registry_priv *pregistrypriv = &padapter->registrypriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-
-	pmlmeext->padapter = padapter;
-
-	init_mlme_ext_priv_value(padapter);
-	pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
-
-	init_mlme_ext_timer(padapter);
-
-#ifdef CONFIG_88EU_AP_MODE
-	init_mlme_ap_info(padapter);
-#endif
-
-	pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan, pmlmeext->channel_set);
-	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
-
-	pmlmeext->chan_scan_time = SURVEY_TO;
-	pmlmeext->mlmeext_init = true;
-
-
-	pmlmeext->active_keep_alive_check = true;
-
-	return _SUCCESS;
-}
-
-void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
-{
-	struct adapter *padapter = pmlmeext->padapter;
-
-	if (!padapter)
-		return;
-
-	if (padapter->bDriverStopped) {
-		del_timer_sync(&pmlmeext->survey_timer);
-		del_timer_sync(&pmlmeext->link_timer);
-	}
-}
-
-static void _mgt_dispatcher(struct adapter *padapter, struct mlme_handler *ptable, struct recv_frame *precv_frame)
-{
-	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	u8 *pframe = precv_frame->rx_data;
-
-	  if (ptable->func) {
-	 /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
-		if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
-		    memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
-			return;
-		ptable->func(padapter, precv_frame);
-	}
-}
-
-void mgt_dispatcher(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	int index;
-	struct mlme_handler *ptable;
-#ifdef CONFIG_88EU_AP_MODE
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-#endif /* CONFIG_88EU_AP_MODE */
-	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	u8 *pframe = precv_frame->rx_data;
-	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
-
-	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
-		 ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
-		  GetFrameType(pframe), GetFrameSubType(pframe)));
-
-	if (GetFrameType(pframe) != WIFI_MGT_TYPE) {
-		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
-		return;
-	}
-
-	/* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
-	if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
-	    memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
-		return;
-
-	ptable = mlme_sta_tbl;
-
-	index = GetFrameSubType(pframe) >> 4;
-
-	if (index > 13) {
-		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Currently we do not support reserved sub-fr-type=%d\n", index));
-		return;
-	}
-	ptable += index;
-
-	if (psta != NULL) {
-		if (GetRetry(pframe)) {
-			if (precv_frame->attrib.seq_num ==
-			    psta->RxMgmtFrameSeqNum) {
-				/* drop the duplicate management frame */
-				DBG_88E("Drop duplicate management frame with seq_num=%d.\n",
-					precv_frame->attrib.seq_num);
-				return;
-			}
-		}
-		psta->RxMgmtFrameSeqNum = precv_frame->attrib.seq_num;
-	}
-
-#ifdef CONFIG_88EU_AP_MODE
-	switch (GetFrameSubType(pframe)) {
-	case WIFI_AUTH:
-		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
-			ptable->func = &OnAuth;
-		else
-			ptable->func = &OnAuthClient;
-		/* fall through */
-	case WIFI_ASSOCREQ:
-	case WIFI_REASSOCREQ:
-	case WIFI_PROBEREQ:
-	case WIFI_BEACON:
-	case WIFI_ACTION:
-		_mgt_dispatcher(padapter, ptable, precv_frame);
-		break;
-	default:
-		_mgt_dispatcher(padapter, ptable, precv_frame);
-		break;
-	}
-#else
-	_mgt_dispatcher(padapter, ptable, precv_frame);
-#endif
-}
-
-/****************************************************************************
-
-Following are the callback functions for each subtype of the management frames
-
-*****************************************************************************/
-
-unsigned int OnProbeReq(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	unsigned int	ielen;
-	unsigned char	*p;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
-	u8 *pframe = precv_frame->rx_data;
-	uint len = precv_frame->len;
-	u8 is_valid_p2p_probereq = false;
-
-	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
-		return _SUCCESS;
-
-	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
-	    !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE))
-		return _SUCCESS;
-
-	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
-			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
-
-	/* check (wildcard) SSID */
-	if (p != NULL) {
-		if (is_valid_p2p_probereq)
-			goto _issue_probersp;
-
-		if ((ielen != 0 && memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength)) ||
-		    (ielen == 0 && pmlmeinfo->hidden_ssid_mode))
-			return _SUCCESS;
-
-_issue_probersp:
-
-		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
-		    pmlmepriv->cur_network.join_res)
-			issue_probersp(padapter, get_sa(pframe), is_valid_p2p_probereq);
-	}
-	return _SUCCESS;
-}
-
-unsigned int OnProbeRsp(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-
-	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
-		report_survey_event(padapter, precv_frame);
-		return _SUCCESS;
-	}
-
-	return _SUCCESS;
-}
-
-unsigned int OnBeacon(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	int cam_idx;
-	struct sta_info	*psta;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct sta_priv	*pstapriv = &padapter->stapriv;
-	u8 *pframe = precv_frame->rx_data;
-	uint len = precv_frame->len;
-	struct wlan_bssid_ex *pbss;
-	int ret = _SUCCESS;
-	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
-
-	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
-		report_survey_event(padapter, precv_frame);
-		return _SUCCESS;
-	}
-
-	if (!memcmp(GetAddr3Ptr(pframe), pnetwork->MacAddress, ETH_ALEN)) {
-		if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
-			/* we should update current network before auth, or some IE is wrong */
-			pbss = (struct wlan_bssid_ex *)rtw_malloc(sizeof(struct wlan_bssid_ex));
-			if (pbss) {
-				if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
-					update_network(&(pmlmepriv->cur_network.network), pbss, padapter, true);
-					rtw_get_bcn_info(&(pmlmepriv->cur_network));
-				}
-				kfree(pbss);
-			}
-
-			/* check the vendor of the assoc AP */
-			pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));
-
-			/* update TSF Value */
-			update_TSF(pmlmeext, pframe, len);
-
-			/* start auth */
-			start_clnt_auth(padapter);
-
-			return _SUCCESS;
-		}
-
-		if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
-			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
-			if (psta != NULL) {
-				ret = rtw_check_bcn_info(padapter, pframe, len);
-				if (!ret) {
-						DBG_88E_LEVEL(_drv_info_, "ap has changed, disconnect now\n ");
-						receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 65535);
-						return _SUCCESS;
-				}
-				/* update WMM, ERP in the beacon */
-				/* todo: the timer is used instead of the number of the beacon received */
-				if ((sta_rx_pkts(psta) & 0xf) == 0)
-					update_beacon_info(padapter, pframe, len, psta);
-			}
-		} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
-			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
-			if (psta != NULL) {
-				/* update WMM, ERP in the beacon */
-				/* todo: the timer is used instead of the number of the beacon received */
-				if ((sta_rx_pkts(psta) & 0xf) == 0)
-					update_beacon_info(padapter, pframe, len, psta);
-			} else {
-				/* allocate a new CAM entry for IBSS station */
-				cam_idx = allocate_fw_sta_entry(padapter);
-				if (cam_idx == NUM_STA)
-					goto _END_ONBEACON_;
-
-				/* get supported rate */
-				if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
-					pmlmeinfo->FW_sta_info[cam_idx].status = 0;
-					goto _END_ONBEACON_;
-				}
-
-				/* update TSF Value */
-				update_TSF(pmlmeext, pframe, len);
-
-				/* report sta add event */
-				report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
-			}
-		}
-	}
-
-_END_ONBEACON_:
-
-	return _SUCCESS;
-}
-
-unsigned int OnAuth(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-#ifdef CONFIG_88EU_AP_MODE
-	unsigned int	auth_mode, ie_len;
-	u16 seq;
-	unsigned char	*sa, *p;
-	u16 algorithm;
-	int	status;
-	static struct sta_info stat;
-	struct	sta_info	*pstat = NULL;
-	struct	sta_priv *pstapriv = &padapter->stapriv;
-	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	u8 *pframe = precv_frame->rx_data;
-	uint len = precv_frame->len;
-
-	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
-		return _FAIL;
-
-	DBG_88E("+OnAuth\n");
-
-	sa = GetAddr2Ptr(pframe);
-
-	auth_mode = psecuritypriv->dot11AuthAlgrthm;
-	seq = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + 2));
-	algorithm = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN));
-
-	DBG_88E("auth alg=%x, seq=%X\n", algorithm, seq);
-
-	if (auth_mode == 2 && psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
-	    psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
-		auth_mode = 0;
-
-	if ((algorithm > 0 && auth_mode == 0) ||	/*  rx a shared-key auth but shared not enabled */
-	    (algorithm == 0 && auth_mode == 1)) {	/*  rx a open-system auth but shared-key is enabled */
-		DBG_88E("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
-			algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
-
-		status = _STATS_NO_SUPP_ALG_;
-
-		goto auth_fail;
-	}
-
-	if (!rtw_access_ctrl(padapter, sa)) {
-		status = _STATS_UNABLE_HANDLE_STA_;
-		goto auth_fail;
-	}
-
-	pstat = rtw_get_stainfo(pstapriv, sa);
-	if (pstat == NULL) {
-		/*  allocate a new one */
-		DBG_88E("going to alloc stainfo for sa=%pM\n", sa);
-		pstat = rtw_alloc_stainfo(pstapriv, sa);
-		if (pstat == NULL) {
-			DBG_88E(" Exceed the upper limit of supported clients...\n");
-			status = _STATS_UNABLE_HANDLE_STA_;
-			goto auth_fail;
-		}
-
-		pstat->state = WIFI_FW_AUTH_NULL;
-		pstat->auth_seq = 0;
-	} else {
-		spin_lock_bh(&pstapriv->asoc_list_lock);
-		if (!list_empty(&pstat->asoc_list)) {
-			list_del_init(&pstat->asoc_list);
-			pstapriv->asoc_list_cnt--;
-		}
-		spin_unlock_bh(&pstapriv->asoc_list_lock);
-
-		if (seq == 1) {
-			/* TODO: STA re_auth and auth timeout */
-		}
-	}
-
-	spin_lock_bh(&pstapriv->auth_list_lock);
-	if (list_empty(&pstat->auth_list)) {
-		list_add_tail(&pstat->auth_list, &pstapriv->auth_list);
-		pstapriv->auth_list_cnt++;
-	}
-	spin_unlock_bh(&pstapriv->auth_list_lock);
-
-	if (pstat->auth_seq == 0)
-		pstat->expire_to = pstapriv->auth_to;
-
-	if ((pstat->auth_seq + 1) != seq) {
-		DBG_88E("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
-			seq, pstat->auth_seq+1);
-		status = _STATS_OUT_OF_AUTH_SEQ_;
-		goto auth_fail;
-	}
-
-	if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
-		if (seq == 1) {
-			pstat->state &= ~WIFI_FW_AUTH_NULL;
-			pstat->state |= WIFI_FW_AUTH_SUCCESS;
-			pstat->expire_to = pstapriv->assoc_to;
-			pstat->authalg = algorithm;
-		} else {
-			DBG_88E("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
-				seq, pstat->auth_seq+1);
-			status = _STATS_OUT_OF_AUTH_SEQ_;
-			goto auth_fail;
-		}
-	} else { /*  shared system or auto authentication */
-		if (seq == 1) {
-			/* prepare for the challenging txt... */
-
-			pstat->state &= ~WIFI_FW_AUTH_NULL;
-			pstat->state |= WIFI_FW_AUTH_STATE;
-			pstat->authalg = algorithm;
-			pstat->auth_seq = 2;
-		} else if (seq == 3) {
-			/* checking for challenging txt... */
-			DBG_88E("checking for challenging txt...\n");
-
-			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
-					len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
-
-			if ((p == NULL) || (ie_len <= 0)) {
-				DBG_88E("auth rejected because challenge failure!(1)\n");
-				status = _STATS_CHALLENGE_FAIL_;
-				goto auth_fail;
-			}
-
-			if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
-				pstat->state &= (~WIFI_FW_AUTH_STATE);
-				pstat->state |= WIFI_FW_AUTH_SUCCESS;
-				/*  challenging txt is correct... */
-				pstat->expire_to =  pstapriv->assoc_to;
-			} else {
-				DBG_88E("auth rejected because challenge failure!\n");
-				status = _STATS_CHALLENGE_FAIL_;
-				goto auth_fail;
-			}
-		} else {
-			DBG_88E("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
-				seq, pstat->auth_seq+1);
-			status = _STATS_OUT_OF_AUTH_SEQ_;
-			goto auth_fail;
-		}
-	}
-
-	/*  Now, we are going to issue_auth... */
-	pstat->auth_seq = seq + 1;
-
-#ifdef CONFIG_88EU_AP_MODE
-	issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
-#endif
-
-	if (pstat->state & WIFI_FW_AUTH_SUCCESS)
-		pstat->auth_seq = 0;
-
-	return _SUCCESS;
-
-auth_fail:
-
-	if (pstat)
-		rtw_free_stainfo(padapter , pstat);
-
-	pstat = &stat;
-	memset((char *)pstat, '\0', sizeof(stat));
-	pstat->auth_seq = 2;
-	memcpy(pstat->hwaddr, sa, 6);
-
-#ifdef CONFIG_88EU_AP_MODE
-	issue_auth(padapter, pstat, (unsigned short)status);
-#endif
-
-#endif
-	return _FAIL;
-}
-
-unsigned int OnAuthClient(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	unsigned int	seq, len, status, offset;
-	unsigned char	*p;
-	unsigned int	go2asoc = 0;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	u8 *pframe = precv_frame->rx_data;
-	uint pkt_len = precv_frame->len;
-
-	DBG_88E("%s\n", __func__);
-
-	/* check A1 matches or not */
-	if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
-		return _SUCCESS;
-
-	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
-		return _SUCCESS;
-
-	offset = (GetPrivacy(pframe)) ? 4 : 0;
-
-	seq	= le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 2));
-	status	= le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 4));
-
-	if (status != 0) {
-		DBG_88E("clnt auth fail, status: %d\n", status);
-		if (status == 13) { /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
-			if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
-				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
-			else
-				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
-		}
-
-		set_link_timer(pmlmeext, 1);
-		goto authclnt_fail;
-	}
-
-	if (seq == 2) {
-		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
-			 /*  legendary shared system */
-			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
-				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
-
-			if (p == NULL)
-				goto authclnt_fail;
-
-			memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
-			pmlmeinfo->auth_seq = 3;
-			issue_auth(padapter, NULL, 0);
-			set_link_timer(pmlmeext, REAUTH_TO);
-
-			return _SUCCESS;
-		} else {
-			/*  open system */
-			go2asoc = 1;
-		}
-	} else if (seq == 4) {
-		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
-			go2asoc = 1;
-		else
-			goto authclnt_fail;
-	} else {
-		/*  this is also illegal */
-		goto authclnt_fail;
-	}
-
-	if (go2asoc) {
-		DBG_88E_LEVEL(_drv_info_, "auth success, start assoc\n");
-		start_clnt_assoc(padapter);
-		return _SUCCESS;
-	}
-authclnt_fail:
-	return _FAIL;
-}
-
-unsigned int OnAssocReq(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-#ifdef CONFIG_88EU_AP_MODE
-	u16 capab_info;
-	struct rtw_ieee802_11_elems elems;
-	struct sta_info	*pstat;
-	unsigned char		reassoc, *p, *pos, *wpa_ie;
-	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
-	int		i, ie_len, wpa_ie_len, left;
-	unsigned char		supportRate[16];
-	int					supportRateNum;
-	unsigned short		status = _STATS_SUCCESSFUL_;
-	unsigned short		frame_type, ie_offset = 0;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	u8 *pframe = precv_frame->rx_data;
-	uint pkt_len = precv_frame->len;
-
-	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
-		return _FAIL;
-
-	frame_type = GetFrameSubType(pframe);
-	if (frame_type == WIFI_ASSOCREQ) {
-		reassoc = 0;
-		ie_offset = _ASOCREQ_IE_OFFSET_;
-	} else { /*  WIFI_REASSOCREQ */
-		reassoc = 1;
-		ie_offset = _REASOCREQ_IE_OFFSET_;
-	}
-
-
-	if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
-		DBG_88E("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
-		       "\n", reassoc, (unsigned long)pkt_len);
-		return _FAIL;
-	}
-
-	pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
-	if (pstat == NULL) {
-		status = _RSON_CLS2_;
-		goto asoc_class2_error;
-	}
-
-	capab_info = get_unaligned_le16(pframe + WLAN_HDR_A3_LEN);
-
-	left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
-	pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
-
-
-	DBG_88E("%s\n", __func__);
-
-	/*  check if this stat has been successfully authenticated/assocated */
-	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
-		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
-			status = _RSON_CLS2_;
-			goto asoc_class2_error;
-		} else {
-			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
-			pstat->state |= WIFI_FW_ASSOC_STATE;
-		}
-	} else {
-		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
-		pstat->state |= WIFI_FW_ASSOC_STATE;
-	}
-	pstat->capability = capab_info;
-	/* now parse all ieee802_11 ie to point to elems */
-	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
-	    !elems.ssid) {
-		DBG_88E("STA %pM sent invalid association request\n",
-			pstat->hwaddr);
-		status = _STATS_FAILURE_;
-		goto OnAssocReqFail;
-	}
-
-
-	/*  now we should check all the fields... */
-	/*  checking SSID */
-	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
-		pkt_len - WLAN_HDR_A3_LEN - ie_offset);
-	if (p == NULL)
-		status = _STATS_FAILURE_;
-
-	if (ie_len == 0) { /*  broadcast ssid, however it is not allowed in assocreq */
-		status = _STATS_FAILURE_;
-	} else {
-		/*  check if ssid match */
-		if (memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
-			status = _STATS_FAILURE_;
-
-		if (ie_len != cur->Ssid.SsidLength)
-			status = _STATS_FAILURE_;
-	}
-
-	if (_STATS_SUCCESSFUL_ != status)
-		goto OnAssocReqFail;
-
-	/*  check if the supported rate is ok */
-	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
-	if (p == NULL) {
-		DBG_88E("Rx a sta assoc-req which supported rate is empty!\n");
-		/*  use our own rate set as statoin used */
-		/* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
-		/* supportRateNum = AP_BSSRATE_LEN; */
-
-		status = _STATS_FAILURE_;
-		goto OnAssocReqFail;
-	} else {
-		memcpy(supportRate, p+2, ie_len);
-		supportRateNum = ie_len;
-
-		p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
-				pkt_len - WLAN_HDR_A3_LEN - ie_offset);
-		if (p !=  NULL) {
-			if (supportRateNum <= sizeof(supportRate)) {
-				memcpy(supportRate+supportRateNum, p+2, ie_len);
-				supportRateNum += ie_len;
-			}
-		}
-	}
-
-	/* todo: mask supportRate between AP & STA -> move to update raid */
-	/* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
-
-	/* update station supportRate */
-	pstat->bssratelen = supportRateNum;
-	memcpy(pstat->bssrateset, supportRate, supportRateNum);
-	UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
-
-	/* check RSN/WPA/WPS */
-	pstat->dot8021xalg = 0;
-	pstat->wpa_psk = 0;
-	pstat->wpa_group_cipher = 0;
-	pstat->wpa2_group_cipher = 0;
-	pstat->wpa_pairwise_cipher = 0;
-	pstat->wpa2_pairwise_cipher = 0;
-	memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
-	if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
-		int group_cipher = 0, pairwise_cipher = 0;
-
-		wpa_ie = elems.rsn_ie;
-		wpa_ie_len = elems.rsn_ie_len;
-
-		if (rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
-			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
-			pstat->wpa_psk |= BIT(1);
-
-			pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
-			pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
-
-			if (!pstat->wpa2_group_cipher)
-				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
-
-			if (!pstat->wpa2_pairwise_cipher)
-				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
-		} else {
-			status = WLAN_STATUS_INVALID_IE;
-		}
-	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
-		int group_cipher = 0, pairwise_cipher = 0;
-
-		wpa_ie = elems.wpa_ie;
-		wpa_ie_len = elems.wpa_ie_len;
-
-		if (rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
-			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
-			pstat->wpa_psk |= BIT(0);
-
-			pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
-			pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
-
-			if (!pstat->wpa_group_cipher)
-				status = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
-
-			if (!pstat->wpa_pairwise_cipher)
-				status = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
-		} else {
-			status = WLAN_STATUS_INVALID_IE;
-		}
-	} else {
-		wpa_ie = NULL;
-		wpa_ie_len = 0;
-	}
-
-	if (_STATS_SUCCESSFUL_ != status)
-		goto OnAssocReqFail;
-
-	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
-	if (wpa_ie == NULL) {
-		if (elems.wps_ie) {
-			DBG_88E("STA included WPS IE in "
-				   "(Re)Association Request - assume WPS is "
-				   "used\n");
-			pstat->flags |= WLAN_STA_WPS;
-			/* wpabuf_free(sta->wps_ie); */
-			/* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
-			/*				elems.wps_ie_len - 4); */
-		} else {
-			DBG_88E("STA did not include WPA/RSN IE "
-				   "in (Re)Association Request - possible WPS "
-				   "use\n");
-			pstat->flags |= WLAN_STA_MAYBE_WPS;
-		}
-
-
-		/*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
-		/*  that the selected registrar of AP is _FLASE */
-		if ((psecuritypriv->wpa_psk > 0) && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
-			if (pmlmepriv->wps_beacon_ie) {
-				u8 selected_registrar = 0;
-
-				rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
-
-				if (!selected_registrar) {
-					DBG_88E("selected_registrar is false , or AP is not ready to do WPS\n");
-
-					status = _STATS_UNABLE_HANDLE_STA_;
-
-					goto OnAssocReqFail;
-				}
-			}
-		}
-	} else {
-		int copy_len;
-
-		if (psecuritypriv->wpa_psk == 0) {
-			DBG_88E("STA %pM: WPA/RSN IE in association "
-			"request, but AP don't support WPA/RSN\n", pstat->hwaddr);
-
-			status = WLAN_STATUS_INVALID_IE;
-
-			goto OnAssocReqFail;
-		}
-
-		if (elems.wps_ie) {
-			DBG_88E("STA included WPS IE in "
-				   "(Re)Association Request - WPS is "
-				   "used\n");
-			pstat->flags |= WLAN_STA_WPS;
-			copy_len = 0;
-		} else {
-			copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)) : (wpa_ie_len+2);
-		}
-		if (copy_len > 0)
-			memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
-	}
-	/*  check if there is WMM IE & support WWM-PS */
-	pstat->flags &= ~WLAN_STA_WME;
-	pstat->qos_option = 0;
-	pstat->qos_info = 0;
-	pstat->has_legacy_ac = true;
-	pstat->uapsd_vo = 0;
-	pstat->uapsd_vi = 0;
-	pstat->uapsd_be = 0;
-	pstat->uapsd_bk = 0;
-	if (pmlmepriv->qospriv.qos_option) {
-		p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
-		for (;;) {
-			p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
-			if (p != NULL) {
-				if (!memcmp(p+2, WMM_IE, 6)) {
-					pstat->flags |= WLAN_STA_WME;
-
-					pstat->qos_option = 1;
-					pstat->qos_info = *(p+8);
-
-					pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
-
-					if ((pstat->qos_info&0xf) != 0xf)
-						pstat->has_legacy_ac = true;
-					else
-						pstat->has_legacy_ac = false;
-
-					if (pstat->qos_info&0xf) {
-						if (pstat->qos_info&BIT(0))
-							pstat->uapsd_vo = BIT(0)|BIT(1);
-						else
-							pstat->uapsd_vo = 0;
-
-						if (pstat->qos_info&BIT(1))
-							pstat->uapsd_vi = BIT(0)|BIT(1);
-						else
-							pstat->uapsd_vi = 0;
-
-						if (pstat->qos_info&BIT(2))
-							pstat->uapsd_bk = BIT(0)|BIT(1);
-						else
-							pstat->uapsd_bk = 0;
-
-						if (pstat->qos_info&BIT(3))
-							pstat->uapsd_be = BIT(0)|BIT(1);
-						else
-							pstat->uapsd_be = 0;
-					}
-					break;
-				}
-			} else {
-				break;
-			}
-			p = p + ie_len + 2;
-		}
-	}
-
-	/* save HT capabilities in the sta object */
-	memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
-	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) {
-		pstat->flags |= WLAN_STA_HT;
-
-		pstat->flags |= WLAN_STA_WME;
-
-		memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
-	} else {
-		pstat->flags &= ~WLAN_STA_HT;
-	}
-	if ((!pmlmepriv->htpriv.ht_option) && (pstat->flags&WLAN_STA_HT)) {
-		status = _STATS_FAILURE_;
-		goto OnAssocReqFail;
-	}
-
-	if ((pstat->flags & WLAN_STA_HT) &&
-	    ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
-	    (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) {
-		DBG_88E("HT: %pM tried to "
-			"use TKIP with HT association\n", pstat->hwaddr);
-
-		/* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
-		/* goto OnAssocReqFail; */
-	}
-
-	pstat->flags |= WLAN_STA_NONERP;
-	for (i = 0; i < pstat->bssratelen; i++) {
-		if ((pstat->bssrateset[i] & 0x7f) > 22) {
-			pstat->flags &= ~WLAN_STA_NONERP;
-			break;
-		}
-	}
-
-	if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
-		pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
-	else
-		pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
-
-
-
-	if (status != _STATS_SUCCESSFUL_)
-		goto OnAssocReqFail;
-
-	/* TODO: identify_proprietary_vendor_ie(); */
-	/*  Realtek proprietary IE */
-	/*  identify if this is Broadcom sta */
-	/*  identify if this is ralink sta */
-	/*  Customer proprietary IE */
-
-	/* get a unique AID */
-	if (pstat->aid > 0) {
-		DBG_88E("  old AID %d\n", pstat->aid);
-	} else {
-		for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
-			if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
-				break;
-
-		/* if (pstat->aid > NUM_STA) { */
-		if (pstat->aid > pstapriv->max_num_sta) {
-			pstat->aid = 0;
-
-			DBG_88E("  no room for more AIDs\n");
-
-			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
-
-			goto OnAssocReqFail;
-		} else {
-			pstapriv->sta_aid[pstat->aid - 1] = pstat;
-			DBG_88E("allocate new AID=(%d)\n", pstat->aid);
-		}
-	}
-
-	pstat->state &= (~WIFI_FW_ASSOC_STATE);
-	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
-
-	spin_lock_bh(&pstapriv->auth_list_lock);
-	if (!list_empty(&pstat->auth_list)) {
-		list_del_init(&pstat->auth_list);
-		pstapriv->auth_list_cnt--;
-	}
-	spin_unlock_bh(&pstapriv->auth_list_lock);
-
-	spin_lock_bh(&pstapriv->asoc_list_lock);
-	if (list_empty(&pstat->asoc_list)) {
-		pstat->expire_to = pstapriv->expire_to;
-		list_add_tail(&pstat->asoc_list, &pstapriv->asoc_list);
-		pstapriv->asoc_list_cnt++;
-	}
-	spin_unlock_bh(&pstapriv->asoc_list_lock);
-
-	/*  now the station is qualified to join our BSS... */
-	if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) {
-#ifdef CONFIG_88EU_AP_MODE
-		/* 1 bss_cap_update & sta_info_update */
-		bss_cap_update_on_sta_join(padapter, pstat);
-		sta_info_update(padapter, pstat);
-
-		/* issue assoc rsp before notify station join event. */
-		if (frame_type == WIFI_ASSOCREQ)
-			issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
-		else
-			issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
-
-		/* 2 - report to upper layer */
-		DBG_88E("indicate_sta_join_event to upper layer - hostapd\n");
-		rtw_indicate_sta_assoc_event(padapter, pstat);
-
-		/* 3-(1) report sta add event */
-		report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
-#endif
-	}
-
-	return _SUCCESS;
-
-asoc_class2_error:
-
-#ifdef CONFIG_88EU_AP_MODE
-	issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
-#endif
-
-	return _FAIL;
-
-OnAssocReqFail:
-
-
-#ifdef CONFIG_88EU_AP_MODE
-	pstat->aid = 0;
-	if (frame_type == WIFI_ASSOCREQ)
-		issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
-	else
-		issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
-#endif
-
-
-#endif /* CONFIG_88EU_AP_MODE */
-
-	return _FAIL;
-}
-
-unsigned int OnAssocRsp(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	uint i;
-	int res;
-	unsigned short	status;
-	struct ndis_802_11_var_ie *pIE;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	/* struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); */
-	u8 *pframe = precv_frame->rx_data;
-	uint pkt_len = precv_frame->len;
-
-	DBG_88E("%s\n", __func__);
-
-	/* check A1 matches or not */
-	if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
-		return _SUCCESS;
-
-	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
-		return _SUCCESS;
-
-	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
-		return _SUCCESS;
-
-	del_timer_sync(&pmlmeext->link_timer);
-
-	/* status */
-	status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
-	if (status > 0) {
-		DBG_88E("assoc reject, status code: %d\n", status);
-		pmlmeinfo->state = WIFI_FW_NULL_STATE;
-		res = -4;
-		goto report_assoc_result;
-	}
-
-	/* get capabilities */
-	pmlmeinfo->capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
-
-	/* set slot time */
-	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;
-
-	/* AID */
-	pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
-	res = pmlmeinfo->aid;
-
-	/* following are moved to join event callback function */
-	/* to handle HT, WMM, rate adaptive, update MAC reg */
-	/* for not to handle the synchronous IO in the tasklet */
-	for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;) {
-		pIE = (struct ndis_802_11_var_ie *)(pframe + i);
-
-		switch (pIE->ElementID) {
-		case _VENDOR_SPECIFIC_IE_:
-			if (!memcmp(pIE->data, WMM_PARA_OUI, 6)) /* WMM */
-				WMM_param_handler(padapter, pIE);
-			break;
-		case _HT_CAPABILITY_IE_:	/* HT caps */
-			HT_caps_handler(padapter, pIE);
-			break;
-		case _HT_EXTRA_INFO_IE_:	/* HT info */
-			HT_info_handler(padapter, pIE);
-			break;
-		case _ERPINFO_IE_:
-			ERP_IE_handler(padapter, pIE);
-		default:
-			break;
-		}
-
-		i += (pIE->Length + 2);
-	}
-
-	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
-	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
-
-	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
-	UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
-
-report_assoc_result:
-	if (res > 0) {
-		rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
-	} else {
-		rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
-	}
-
-	report_join_res(padapter, res);
-
-	return _SUCCESS;
-}
-
-unsigned int OnDeAuth(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	unsigned short	reason;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	u8 *pframe = precv_frame->rx_data;
-	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
-
-	/* check A3 */
-	if (memcmp(GetAddr3Ptr(pframe), pnetwork->MacAddress, ETH_ALEN))
-		return _SUCCESS;
-
-	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
-
-	DBG_88E("%s Reason code(%d)\n", __func__, reason);
-
-#ifdef CONFIG_88EU_AP_MODE
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
-		struct sta_info *psta;
-		struct sta_priv *pstapriv = &padapter->stapriv;
-
-		DBG_88E_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
-			      reason, GetAddr2Ptr(pframe));
-
-		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
-		if (psta) {
-			u8 updated = 0;
-
-			spin_lock_bh(&pstapriv->asoc_list_lock);
-			if (!list_empty(&psta->asoc_list)) {
-				list_del_init(&psta->asoc_list);
-				pstapriv->asoc_list_cnt--;
-				updated = ap_free_sta(padapter, psta, false, reason);
-			}
-			spin_unlock_bh(&pstapriv->asoc_list_lock);
-
-			associated_clients_update(padapter, updated);
-		}
-
-
-		return _SUCCESS;
-	} else
-#endif
-	{
-		DBG_88E_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM\n",
-			      reason, GetAddr3Ptr(pframe));
-
-		receive_disconnect(padapter, GetAddr3Ptr(pframe) , reason);
-	}
-	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
-	return _SUCCESS;
-}
-
-unsigned int OnDisassoc(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	u16 reason;
-	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	u8 *pframe = precv_frame->rx_data;
-	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
-
-	/* check A3 */
-	if (memcmp(GetAddr3Ptr(pframe), pnetwork->MacAddress, ETH_ALEN))
-		return _SUCCESS;
-
-	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
-
-	DBG_88E("%s Reason code(%d)\n", __func__, reason);
-
-#ifdef CONFIG_88EU_AP_MODE
-	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
-		struct sta_info *psta;
-		struct sta_priv *pstapriv = &padapter->stapriv;
-
-		DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
-			      reason, GetAddr2Ptr(pframe));
-
-		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
-		if (psta) {
-			u8 updated = 0;
-
-			spin_lock_bh(&pstapriv->asoc_list_lock);
-			if (!list_empty(&psta->asoc_list)) {
-				list_del_init(&psta->asoc_list);
-				pstapriv->asoc_list_cnt--;
-				updated = ap_free_sta(padapter, psta, false, reason);
-			}
-			spin_unlock_bh(&pstapriv->asoc_list_lock);
-
-			associated_clients_update(padapter, updated);
-		}
-
-		return _SUCCESS;
-	} else
-#endif
-	{
-		DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
-			      reason, GetAddr3Ptr(pframe));
-
-		receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
-	}
-	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
-	return _SUCCESS;
-}
-
-unsigned int OnAtim(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	DBG_88E("%s\n", __func__);
-	return _SUCCESS;
-}
-
-unsigned int on_action_spct(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	struct sta_info *psta = NULL;
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	u8 *pframe = precv_frame->rx_data;
-	u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
-	u8 category;
-	u8 action;
-
-	DBG_88E(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
-
-	psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
-
-	if (!psta)
-		goto exit;
-
-	category = frame_body[0];
-	if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
-		goto exit;
-
-	action = frame_body[1];
-	switch (action) {
-	case RTW_WLAN_ACTION_SPCT_MSR_REQ:
-	case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
-	case RTW_WLAN_ACTION_SPCT_TPC_REQ:
-	case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
-		break;
-	case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
-		break;
-	default:
-		break;
-	}
-
-exit:
-	return _FAIL;
-}
-
-unsigned int OnAction_qos(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	return _SUCCESS;
-}
-
-unsigned int OnAction_dls(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	return _SUCCESS;
-}
-
-unsigned int OnAction_back(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	u8 *addr;
-	struct sta_info *psta = NULL;
-	struct recv_reorder_ctrl *preorder_ctrl;
-	unsigned char		*frame_body;
-	unsigned char		category, action;
-	unsigned short	tid, status, reason_code = 0;
-	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
-	u8 *pframe = precv_frame->rx_data;
-	struct sta_priv *pstapriv = &padapter->stapriv;
-	/* check RA matches or not */
-	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe),
-		   ETH_ALEN))/* for if1, sta/ap mode */
-		return _SUCCESS;
-
-	DBG_88E("%s\n", __func__);
-
-	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
-		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
-			return _SUCCESS;
-
-	addr = GetAddr2Ptr(pframe);
-	psta = rtw_get_stainfo(pstapriv, addr);
-
-	if (psta == NULL)
-		return _SUCCESS;
-
-	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
-
-	category = frame_body[0];
-	if (category == RTW_WLAN_CATEGORY_BACK) { /*  representing Block Ack */
-		if (!pmlmeinfo->HT_enable)
-			return _SUCCESS;
-		action = frame_body[1];
-		DBG_88E("%s, action=%d\n", __func__, action);
-		switch (action) {
-		case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
-			memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
-			process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr);
-
-			if (pmlmeinfo->bAcceptAddbaReq)
-				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
-			else
-				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
-			break;
-		case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
-			status = get_unaligned_le16(&frame_body[3]);
-			tid = (frame_body[5] >> 2) & 0x7;
-			if (status == 0) {	/* successful */
-				DBG_88E("agg_enable for TID=%d\n", tid);
-				psta->htpriv.agg_enable_bitmap |= 1 << tid;
-				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
-			} else {
-				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
-			}
-			break;
-		case RTW_WLAN_ACTION_DELBA: /* DELBA */
-			if ((frame_body[3] & BIT(3)) == 0) {
-				psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
-				psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
-				reason_code = get_unaligned_le16(&frame_body[4]);
-			} else if ((frame_body[3] & BIT(3)) == BIT(3)) {
-				tid = (frame_body[3] >> 4) & 0x0F;
-				preorder_ctrl =  &psta->recvreorder_ctrl[tid];
-				preorder_ctrl->enable = false;
-				preorder_ctrl->indicate_seq = 0xffff;
-			}
-			DBG_88E("%s(): DELBA: %x(%x)\n", __func__, pmlmeinfo->agg_enable_bitmap, reason_code);
-			/* todo: how to notify the host while receiving DELETE BA */
-			break;
-		default:
-			break;
-		}
-	}
-	return _SUCCESS;
-}
-
-static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
-{
-	struct adapter *adapter = recv_frame->adapter;
-	struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
-	u8 *frame = recv_frame->rx_data;
-	u16 seq_ctrl = ((recv_frame->attrib.seq_num&0xffff) << 4) |
-		(recv_frame->attrib.frag_num & 0xf);
-
-	if (GetRetry(frame)) {
-		if (token >= 0) {
-			if ((seq_ctrl == mlmeext->action_public_rxseq) && (token == mlmeext->action_public_dialog_token)) {
-				DBG_88E(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x, token:%d\n",
-					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
-				return _FAIL;
-			}
-		} else {
-			if (seq_ctrl == mlmeext->action_public_rxseq) {
-				DBG_88E(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x\n",
-					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
-				return _FAIL;
-			}
-		}
-	}
-
-	mlmeext->action_public_rxseq = seq_ctrl;
-
-	if (token >= 0)
-		mlmeext->action_public_dialog_token = token;
-
-	return _SUCCESS;
-}
-
-static unsigned int on_action_public_p2p(struct recv_frame *precv_frame)
-{
-	u8 *pframe = precv_frame->rx_data;
-	u8 *frame_body;
-	u8 dialogToken = 0;
-	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
-
-	dialogToken = frame_body[7];
-
-	if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
-		return _FAIL;
-
-	return _SUCCESS;
-}
-
-static unsigned int on_action_public_vendor(struct recv_frame *precv_frame)
-{
-	unsigned int ret = _FAIL;
-	u8 *pframe = precv_frame->rx_data;
-	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
-
-	if (!memcmp(frame_body + 2, P2P_OUI, 4))
-		ret = on_action_public_p2p(precv_frame);
-
-	return ret;
-}
-
-static unsigned int on_action_public_default(struct recv_frame *precv_frame, u8 action)
-{
-	unsigned int ret = _FAIL;
-	u8 *pframe = precv_frame->rx_data;
-	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
-	u8 token;
-
-	token = frame_body[2];
-
-	if (rtw_action_public_decache(precv_frame, token) == _FAIL)
-		goto exit;
-
-	ret = _SUCCESS;
-
-exit:
-	return ret;
-}
-
-unsigned int on_action_public(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	unsigned int ret = _FAIL;
-	u8 *pframe = precv_frame->rx_data;
-	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
-	u8 category, action;
-
-	/* check RA matches or not */
-	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
-		goto exit;
-
-	category = frame_body[0];
-	if (category != RTW_WLAN_CATEGORY_PUBLIC)
-		goto exit;
-
-	action = frame_body[1];
-	switch (action) {
-	case ACT_PUBLIC_VENDOR:
-		ret = on_action_public_vendor(precv_frame);
-		break;
-	default:
-		ret = on_action_public_default(precv_frame, action);
-		break;
-	}
-
-exit:
-	return ret;
-}
-
-unsigned int OnAction_ht(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	return _SUCCESS;
-}
-
-unsigned int OnAction_wmm(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	return _SUCCESS;
-}
-
-unsigned int OnAction_p2p(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	return _SUCCESS;
-}
-
-unsigned int OnAction(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	int i;
-	unsigned char	category;
-	struct action_handler *ptable;
-	unsigned char	*frame_body;
-	u8 *pframe = precv_frame->rx_data;
-
-	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
-
-	category = frame_body[0];
-
-	for (i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++) {
-		ptable = &OnAction_tbl[i];
-		if (category == ptable->num)
-			ptable->func(padapter, precv_frame);
-	}
-	return _SUCCESS;
-}
-
-unsigned int DoReserved(struct adapter *padapter, struct recv_frame *precv_frame)
-{
-	return _SUCCESS;
-}
-
 struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
 {
 	struct xmit_frame			*pmgntframe;
@@ -1886,7 +230,8 @@ void update_mgntframe_attrib(struct adapter *padapter, struct pkt_attrib *pattri
 	pattrib->retry_ctrl = true;
 }
 
-void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe)
+static void dump_mgntframe(struct adapter *padapter,
+			   struct xmit_frame *pmgntframe)
 {
 	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
 		return;
@@ -1894,7 +239,9 @@ void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe)
 	rtw_hal_mgnt_xmit(padapter, pmgntframe);
 }
 
-s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntframe, int timeout_ms)
+static s32 dump_mgntframe_and_wait(struct adapter *padapter,
+				   struct xmit_frame *pmgntframe,
+				   int timeout_ms)
 {
 	s32 ret = _FAIL;
 	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
@@ -1914,7 +261,8 @@ s32 dump_mgntframe_and_wait(struct adapter *padapter, struct xmit_frame *pmgntfr
 	return ret;
 }
 
-s32 dump_mgntframe_and_wait_ack(struct adapter *padapter, struct xmit_frame *pmgntframe)
+static s32 dump_mgntframe_and_wait_ack(struct adapter *padapter,
+				       struct xmit_frame *pmgntframe)
 {
 	s32 ret = _FAIL;
 	u32 timeout_ms = 500;/*   500ms */
@@ -1970,7 +318,7 @@ static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
 	return len_diff;
 }
 
-void issue_beacon(struct adapter *padapter, int timeout_ms)
+static void issue_beacon(struct adapter *padapter, int timeout_ms)
 {
 	struct xmit_frame	*pmgntframe;
 	struct pkt_attrib	*pattrib;
@@ -2111,7 +459,7 @@ _issue_bcn:
 		dump_mgntframe(padapter, pmgntframe);
 }
 
-void issue_probersp(struct adapter *padapter, unsigned char *da, u8 is_valid_p2p_probereq)
+static void issue_probersp(struct adapter *padapter, unsigned char *da)
 {
 	struct xmit_frame			*pmgntframe;
 	struct pkt_attrib			*pattrib;
@@ -2353,13 +701,15 @@ exit:
 	return ret;
 }
 
-inline void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da)
+static inline void issue_probereq(struct adapter *padapter,
+				  struct ndis_802_11_ssid *pssid, u8 *da)
 {
 	_issue_probereq(padapter, pssid, da, false);
 }
 
-int issue_probereq_ex(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da,
-	int try_cnt, int wait_ms)
+static int issue_probereq_ex(struct adapter *padapter,
+			     struct ndis_802_11_ssid *pssid, u8 *da,
+			     int try_cnt, int wait_ms)
 {
 	int ret;
 	int i = 0;
@@ -2398,7 +748,8 @@ exit:
 }
 
 /*  if psta == NULL, indicate we are station(client) now... */
-void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short status)
+static void issue_auth(struct adapter *padapter, struct sta_info *psta,
+		       unsigned short status)
 {
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
@@ -2537,9 +888,10 @@ void issue_auth(struct adapter *padapter, struct sta_info *psta, unsigned short
 }
 
 
-void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_info *pstat, int pkt_type)
-{
 #ifdef CONFIG_88EU_AP_MODE
+static void issue_asocrsp(struct adapter *padapter, unsigned short status,
+			  struct sta_info *pstat, int pkt_type)
+{
 	struct xmit_frame	*pmgntframe;
 	struct rtw_ieee80211_hdr	*pwlanhdr;
 	struct pkt_attrib *pattrib;
@@ -2659,10 +1011,10 @@ void issue_asocrsp(struct adapter *padapter, unsigned short status, struct sta_i
 
 	pattrib->last_txcmdsz = pattrib->pktlen;
 	dump_mgntframe(padapter, pmgntframe);
-#endif
 }
+#endif /* CONFIG_88EU_AP_MODE */
 
-void issue_assocreq(struct adapter *padapter)
+static void issue_assocreq(struct adapter *padapter)
 {
 	int ret = _FAIL;
 	struct xmit_frame	*pmgntframe;
@@ -3169,8 +1521,9 @@ int issue_deauth(struct adapter *padapter, unsigned char *da, unsigned short rea
 	return _issue_deauth(padapter, da, reason, false);
 }
 
-int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason, int try_cnt,
-	int wait_ms)
+static int issue_deauth_ex(struct adapter *padapter, u8 *da,
+			   unsigned short reason, int try_cnt,
+			   int wait_ms)
 {
 	int ret;
 	int i = 0;
@@ -3267,7 +1620,8 @@ void issue_action_spct_ch_switch(struct adapter *padapter, u8 *ra, u8 new_ch, u8
 	dump_mgntframe(padapter, pmgntframe);
 }
 
-void issue_action_BA(struct adapter *padapter, unsigned char *raddr, unsigned char action, unsigned short status)
+static void issue_action_BA(struct adapter *padapter, unsigned char *raddr,
+			    unsigned char action, unsigned short status)
 {
 	u8 category = RTW_WLAN_CATEGORY_BACK;
 	u16 start_seq;
@@ -3619,7 +1973,7 @@ Following are some utility functions for WiFi MLME
 
 *****************************************************************************/
 
-void site_survey(struct adapter *padapter)
+static void site_survey(struct adapter *padapter)
 {
 	unsigned char		survey_channel = 0, val8;
 	enum rt_scan_type ScanType = SCAN_PASSIVE;
@@ -3723,7 +2077,9 @@ void site_survey(struct adapter *padapter)
 }
 
 /* collect bss info from Beacon and Probe request/response frames. */
-u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, struct wlan_bssid_ex *bssid)
+static u8 collect_bss_info(struct adapter *padapter,
+			   struct recv_frame *precv_frame,
+			   struct wlan_bssid_ex *bssid)
 {
 	int	i;
 	u32	len;
@@ -3889,7 +2245,7 @@ u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame, st
 	return _SUCCESS;
 }
 
-void start_create_ibss(struct adapter *padapter)
+static void start_create_ibss(struct adapter *padapter)
 {
 	unsigned short	caps;
 	u8 val8;
@@ -3940,7 +2296,7 @@ void start_create_ibss(struct adapter *padapter)
 	}
 }
 
-void start_clnt_join(struct adapter *padapter)
+static void start_clnt_join(struct adapter *padapter)
 {
 	unsigned short	caps;
 	u8 val8;
@@ -3995,7 +2351,7 @@ void start_clnt_join(struct adapter *padapter)
 	}
 }
 
-void start_clnt_auth(struct adapter *padapter)
+static void start_clnt_auth(struct adapter *padapter)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
@@ -4026,7 +2382,7 @@ void start_clnt_auth(struct adapter *padapter)
 }
 
 
-void start_clnt_assoc(struct adapter *padapter)
+static void start_clnt_assoc(struct adapter *padapter)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
@@ -4041,7 +2397,9 @@ void start_clnt_assoc(struct adapter *padapter)
 	set_link_timer(pmlmeext, REASSOC_TO);
 }
 
-unsigned int receive_disconnect(struct adapter *padapter, unsigned char *MacAddr, unsigned short reason)
+static unsigned int receive_disconnect(struct adapter *padapter,
+				       unsigned char *MacAddr,
+				       unsigned short reason)
 {
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
@@ -4221,6 +2579,1665 @@ static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid
 
 /****************************************************************************
 
+Following are the callback functions for each subtype of the management frames
+
+*****************************************************************************/
+
+static unsigned int OnProbeReq(struct adapter *padapter,
+			       struct recv_frame *precv_frame)
+{
+	unsigned int	ielen;
+	unsigned char	*p;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
+	u8 *pframe = precv_frame->rx_data;
+	uint len = precv_frame->len;
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE))
+		return _SUCCESS;
+
+	if (!check_fwstate(pmlmepriv, _FW_LINKED) &&
+	    !check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE|WIFI_AP_STATE))
+		return _SUCCESS;
+
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ielen,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+	/* check (wildcard) SSID */
+	if (p != NULL) {
+		if ((ielen != 0 && memcmp((void *)(p+2), (void *)cur->Ssid.Ssid, cur->Ssid.SsidLength)) ||
+		    (ielen == 0 && pmlmeinfo->hidden_ssid_mode))
+			return _SUCCESS;
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) &&
+		    pmlmepriv->cur_network.join_res)
+			issue_probersp(padapter, get_sa(pframe));
+	}
+	return _SUCCESS;
+}
+
+static unsigned int OnProbeRsp(struct adapter *padapter,
+			       struct recv_frame *precv_frame)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	return _SUCCESS;
+}
+
+static unsigned int OnBeacon(struct adapter *padapter,
+			     struct recv_frame *precv_frame)
+{
+	int cam_idx;
+	struct sta_info	*psta;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->rx_data;
+	uint len = precv_frame->len;
+	struct wlan_bssid_ex *pbss;
+	int ret = _SUCCESS;
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		report_survey_event(padapter, precv_frame);
+		return _SUCCESS;
+	}
+
+	if (!memcmp(GetAddr3Ptr(pframe), pnetwork->MacAddress, ETH_ALEN)) {
+		if (pmlmeinfo->state & WIFI_FW_AUTH_NULL) {
+			/* we should update current network before auth, or some IE is wrong */
+			pbss = (struct wlan_bssid_ex *)rtw_malloc(sizeof(struct wlan_bssid_ex));
+			if (pbss) {
+				if (collect_bss_info(padapter, precv_frame, pbss) == _SUCCESS) {
+					update_network(&(pmlmepriv->cur_network.network), pbss, padapter, true);
+					rtw_get_bcn_info(&(pmlmepriv->cur_network));
+				}
+				kfree(pbss);
+			}
+
+			/* check the vendor of the assoc AP */
+			pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pframe+sizeof(struct rtw_ieee80211_hdr_3addr), len-sizeof(struct rtw_ieee80211_hdr_3addr));
+
+			/* update TSF Value */
+			update_TSF(pmlmeext, pframe, len);
+
+			/* start auth */
+			start_clnt_auth(padapter);
+
+			return _SUCCESS;
+		}
+
+		if (((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) && (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta != NULL) {
+				ret = rtw_check_bcn_info(padapter, pframe, len);
+				if (!ret) {
+						DBG_88E_LEVEL(_drv_info_, "ap has changed, disconnect now\n ");
+						receive_disconnect(padapter, pmlmeinfo->network.MacAddress , 65535);
+						return _SUCCESS;
+				}
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+					update_beacon_info(padapter, pframe, len, psta);
+			}
+		} else if ((pmlmeinfo->state&0x03) == WIFI_FW_ADHOC_STATE) {
+			psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+			if (psta != NULL) {
+				/* update WMM, ERP in the beacon */
+				/* todo: the timer is used instead of the number of the beacon received */
+				if ((sta_rx_pkts(psta) & 0xf) == 0)
+					update_beacon_info(padapter, pframe, len, psta);
+			} else {
+				/* allocate a new CAM entry for IBSS station */
+				cam_idx = allocate_fw_sta_entry(padapter);
+				if (cam_idx == NUM_STA)
+					goto _END_ONBEACON_;
+
+				/* get supported rate */
+				if (update_sta_support_rate(padapter, (pframe + WLAN_HDR_A3_LEN + _BEACON_IE_OFFSET_), (len - WLAN_HDR_A3_LEN - _BEACON_IE_OFFSET_), cam_idx) == _FAIL) {
+					pmlmeinfo->FW_sta_info[cam_idx].status = 0;
+					goto _END_ONBEACON_;
+				}
+
+				/* update TSF Value */
+				update_TSF(pmlmeext, pframe, len);
+
+				/* report sta add event */
+				report_add_sta_event(padapter, GetAddr2Ptr(pframe), cam_idx);
+			}
+		}
+	}
+
+_END_ONBEACON_:
+
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_88EU_AP_MODE
+static unsigned int OnAuth(struct adapter *padapter,
+			   struct recv_frame *precv_frame)
+{
+	unsigned int	auth_mode, ie_len;
+	u16 seq;
+	unsigned char	*sa, *p;
+	u16 algorithm;
+	int	status;
+	static struct sta_info stat;
+	struct	sta_info	*pstat = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->rx_data;
+	uint len = precv_frame->len;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	DBG_88E("+OnAuth\n");
+
+	sa = GetAddr2Ptr(pframe);
+
+	auth_mode = psecuritypriv->dot11AuthAlgrthm;
+	seq = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + 2));
+	algorithm = le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN));
+
+	DBG_88E("auth alg=%x, seq=%X\n", algorithm, seq);
+
+	if (auth_mode == 2 && psecuritypriv->dot11PrivacyAlgrthm != _WEP40_ &&
+	    psecuritypriv->dot11PrivacyAlgrthm != _WEP104_)
+		auth_mode = 0;
+
+	if ((algorithm > 0 && auth_mode == 0) ||	/*  rx a shared-key auth but shared not enabled */
+	    (algorithm == 0 && auth_mode == 1)) {	/*  rx a open-system auth but shared-key is enabled */
+		DBG_88E("auth rejected due to bad alg [alg=%d, auth_mib=%d] %02X%02X%02X%02X%02X%02X\n",
+			algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
+
+		status = _STATS_NO_SUPP_ALG_;
+
+		goto auth_fail;
+	}
+
+	if (!rtw_access_ctrl(padapter, sa)) {
+		status = _STATS_UNABLE_HANDLE_STA_;
+		goto auth_fail;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, sa);
+	if (pstat == NULL) {
+		/*  allocate a new one */
+		DBG_88E("going to alloc stainfo for sa=%pM\n", sa);
+		pstat = rtw_alloc_stainfo(pstapriv, sa);
+		if (pstat == NULL) {
+			DBG_88E(" Exceed the upper limit of supported clients...\n");
+			status = _STATS_UNABLE_HANDLE_STA_;
+			goto auth_fail;
+		}
+
+		pstat->state = WIFI_FW_AUTH_NULL;
+		pstat->auth_seq = 0;
+	} else {
+		spin_lock_bh(&pstapriv->asoc_list_lock);
+		if (!list_empty(&pstat->asoc_list)) {
+			list_del_init(&pstat->asoc_list);
+			pstapriv->asoc_list_cnt--;
+		}
+		spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+		if (seq == 1) {
+			/* TODO: STA re_auth and auth timeout */
+		}
+	}
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (list_empty(&pstat->auth_list)) {
+		list_add_tail(&pstat->auth_list, &pstapriv->auth_list);
+		pstapriv->auth_list_cnt++;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	if (pstat->auth_seq == 0)
+		pstat->expire_to = pstapriv->auth_to;
+
+	if ((pstat->auth_seq + 1) != seq) {
+		DBG_88E("(1)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+			seq, pstat->auth_seq+1);
+		status = _STATS_OUT_OF_AUTH_SEQ_;
+		goto auth_fail;
+	}
+
+	if (algorithm == 0 && (auth_mode == 0 || auth_mode == 2)) {
+		if (seq == 1) {
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_SUCCESS;
+			pstat->expire_to = pstapriv->assoc_to;
+			pstat->authalg = algorithm;
+		} else {
+			DBG_88E("(2)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+				seq, pstat->auth_seq+1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	} else { /*  shared system or auto authentication */
+		if (seq == 1) {
+			/* prepare for the challenging txt... */
+
+			pstat->state &= ~WIFI_FW_AUTH_NULL;
+			pstat->state |= WIFI_FW_AUTH_STATE;
+			pstat->authalg = algorithm;
+			pstat->auth_seq = 2;
+		} else if (seq == 3) {
+			/* checking for challenging txt... */
+			DBG_88E("checking for challenging txt...\n");
+
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_ , _CHLGETXT_IE_, (int *)&ie_len,
+					len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
+
+			if ((p == NULL) || (ie_len <= 0)) {
+				DBG_88E("auth rejected because challenge failure!(1)\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+
+			if (!memcmp((void *)(p + 2), pstat->chg_txt, 128)) {
+				pstat->state &= (~WIFI_FW_AUTH_STATE);
+				pstat->state |= WIFI_FW_AUTH_SUCCESS;
+				/*  challenging txt is correct... */
+				pstat->expire_to =  pstapriv->assoc_to;
+			} else {
+				DBG_88E("auth rejected because challenge failure!\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+		} else {
+			DBG_88E("(3)auth rejected because out of seq [rx_seq=%d, exp_seq=%d]!\n",
+				seq, pstat->auth_seq+1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			goto auth_fail;
+		}
+	}
+
+	/*  Now, we are going to issue_auth... */
+	pstat->auth_seq = seq + 1;
+
+	issue_auth(padapter, pstat, (unsigned short)(_STATS_SUCCESSFUL_));
+
+	if (pstat->state & WIFI_FW_AUTH_SUCCESS)
+		pstat->auth_seq = 0;
+
+	return _SUCCESS;
+
+auth_fail:
+
+	if (pstat)
+		rtw_free_stainfo(padapter, pstat);
+
+	pstat = &stat;
+	memset((char *)pstat, '\0', sizeof(stat));
+	pstat->auth_seq = 2;
+	memcpy(pstat->hwaddr, sa, 6);
+
+	issue_auth(padapter, pstat, (unsigned short)status);
+
+	return _FAIL;
+}
+#endif /* CONFIG_88EU_AP_MODE */
+
+static unsigned int OnAuthClient(struct adapter *padapter,
+				 struct recv_frame *precv_frame)
+{
+	unsigned int	seq, len, status, offset;
+	unsigned char	*p;
+	unsigned int	go2asoc = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->rx_data;
+	uint pkt_len = precv_frame->len;
+
+	DBG_88E("%s\n", __func__);
+
+	/* check A1 matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & WIFI_FW_AUTH_STATE))
+		return _SUCCESS;
+
+	offset = (GetPrivacy(pframe)) ? 4 : 0;
+
+	seq	= le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 2));
+	status	= le16_to_cpu(*(__le16 *)((size_t)pframe + WLAN_HDR_A3_LEN + offset + 4));
+
+	if (status != 0) {
+		DBG_88E("clnt auth fail, status: %d\n", status);
+		if (status == 13) { /*  pmlmeinfo->auth_algo == dot11AuthAlgrthm_Auto) */
+			if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+			else
+				pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared;
+		}
+
+		set_link_timer(pmlmeext, 1);
+		goto authclnt_fail;
+	}
+
+	if (seq == 2) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared) {
+			 /*  legendary shared system */
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&len,
+				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
+
+			if (p == NULL)
+				goto authclnt_fail;
+
+			memcpy((void *)(pmlmeinfo->chg_txt), (void *)(p + 2), len);
+			pmlmeinfo->auth_seq = 3;
+			issue_auth(padapter, NULL, 0);
+			set_link_timer(pmlmeext, REAUTH_TO);
+
+			return _SUCCESS;
+		} else {
+			/*  open system */
+			go2asoc = 1;
+		}
+	} else if (seq == 4) {
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_Shared)
+			go2asoc = 1;
+		else
+			goto authclnt_fail;
+	} else {
+		/*  this is also illegal */
+		goto authclnt_fail;
+	}
+
+	if (go2asoc) {
+		DBG_88E_LEVEL(_drv_info_, "auth success, start assoc\n");
+		start_clnt_assoc(padapter);
+		return _SUCCESS;
+	}
+authclnt_fail:
+	return _FAIL;
+}
+
+static unsigned int OnAssocReq(struct adapter *padapter,
+			       struct recv_frame *precv_frame)
+{
+#ifdef CONFIG_88EU_AP_MODE
+	u16 capab_info;
+	struct rtw_ieee802_11_elems elems;
+	struct sta_info	*pstat;
+	unsigned char		reassoc, *p, *pos, *wpa_ie;
+	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+	int		i, ie_len, wpa_ie_len, left;
+	unsigned char		supportRate[16];
+	int					supportRateNum;
+	unsigned short		status = _STATS_SUCCESSFUL_;
+	unsigned short		frame_type, ie_offset = 0;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->rx_data;
+	uint pkt_len = precv_frame->len;
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return _FAIL;
+
+	frame_type = GetFrameSubType(pframe);
+	if (frame_type == WIFI_ASSOCREQ) {
+		reassoc = 0;
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	} else { /*  WIFI_REASSOCREQ */
+		reassoc = 1;
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+	}
+
+
+	if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
+		DBG_88E("handle_assoc(reassoc=%d) - too short payload (len=%lu)"
+		       "\n", reassoc, (unsigned long)pkt_len);
+		return _FAIL;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+	if (pstat == NULL) {
+		status = _RSON_CLS2_;
+		goto asoc_class2_error;
+	}
+
+	capab_info = get_unaligned_le16(pframe + WLAN_HDR_A3_LEN);
+
+	left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
+	pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
+
+
+	DBG_88E("%s\n", __func__);
+
+	/*  check if this stat has been successfully authenticated/assocated */
+	if (!((pstat->state) & WIFI_FW_AUTH_SUCCESS)) {
+		if (!((pstat->state) & WIFI_FW_ASSOC_SUCCESS)) {
+			status = _RSON_CLS2_;
+			goto asoc_class2_error;
+		} else {
+			pstat->state &= (~WIFI_FW_ASSOC_SUCCESS);
+			pstat->state |= WIFI_FW_ASSOC_STATE;
+		}
+	} else {
+		pstat->state &= (~WIFI_FW_AUTH_SUCCESS);
+		pstat->state |= WIFI_FW_ASSOC_STATE;
+	}
+	pstat->capability = capab_info;
+	/* now parse all ieee802_11 ie to point to elems */
+	if (rtw_ieee802_11_parse_elems(pos, left, &elems, 1) == ParseFailed ||
+	    !elems.ssid) {
+		DBG_88E("STA %pM sent invalid association request\n",
+			pstat->hwaddr);
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+
+	/*  now we should check all the fields... */
+	/*  checking SSID */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SSID_IE_, &ie_len,
+		pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (p == NULL)
+		status = _STATS_FAILURE_;
+
+	if (ie_len == 0) { /*  broadcast ssid, however it is not allowed in assocreq */
+		status = _STATS_FAILURE_;
+	} else {
+		/*  check if ssid match */
+		if (memcmp((void *)(p+2), cur->Ssid.Ssid, cur->Ssid.SsidLength))
+			status = _STATS_FAILURE_;
+
+		if (ie_len != cur->Ssid.SsidLength)
+			status = _STATS_FAILURE_;
+	}
+
+	if (_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	/*  check if the supported rate is ok */
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _SUPPORTEDRATES_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+	if (p == NULL) {
+		DBG_88E("Rx a sta assoc-req which supported rate is empty!\n");
+		/*  use our own rate set as statoin used */
+		/* memcpy(supportRate, AP_BSSRATE, AP_BSSRATE_LEN); */
+		/* supportRateNum = AP_BSSRATE_LEN; */
+
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	} else {
+		memcpy(supportRate, p+2, ie_len);
+		supportRateNum = ie_len;
+
+		p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + ie_offset, _EXT_SUPPORTEDRATES_IE_ , &ie_len,
+				pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+		if (p !=  NULL) {
+			if (supportRateNum <= sizeof(supportRate)) {
+				memcpy(supportRate+supportRateNum, p+2, ie_len);
+				supportRateNum += ie_len;
+			}
+		}
+	}
+
+	/* todo: mask supportRate between AP & STA -> move to update raid */
+	/* get_matched_rate(pmlmeext, supportRate, &supportRateNum, 0); */
+
+	/* update station supportRate */
+	pstat->bssratelen = supportRateNum;
+	memcpy(pstat->bssrateset, supportRate, supportRateNum);
+	UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
+
+	/* check RSN/WPA/WPS */
+	pstat->dot8021xalg = 0;
+	pstat->wpa_psk = 0;
+	pstat->wpa_group_cipher = 0;
+	pstat->wpa2_group_cipher = 0;
+	pstat->wpa_pairwise_cipher = 0;
+	pstat->wpa2_pairwise_cipher = 0;
+	memset(pstat->wpa_ie, 0, sizeof(pstat->wpa_ie));
+	if ((psecuritypriv->wpa_psk & BIT(1)) && elems.rsn_ie) {
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.rsn_ie;
+		wpa_ie_len = elems.rsn_ie_len;
+
+		if (rtw_parse_wpa2_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+			pstat->wpa_psk |= BIT(1);
+
+			pstat->wpa2_group_cipher = group_cipher&psecuritypriv->wpa2_group_cipher;
+			pstat->wpa2_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa2_pairwise_cipher;
+
+			if (!pstat->wpa2_group_cipher)
+				status = WLAN_STATUS_INVALID_GROUP_CIPHER;
+
+			if (!pstat->wpa2_pairwise_cipher)
+				status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
+		} else {
+			status = WLAN_STATUS_INVALID_IE;
+		}
+	} else if ((psecuritypriv->wpa_psk & BIT(0)) && elems.wpa_ie) {
+		int group_cipher = 0, pairwise_cipher = 0;
+
+		wpa_ie = elems.wpa_ie;
+		wpa_ie_len = elems.wpa_ie_len;
+
+		if (rtw_parse_wpa_ie(wpa_ie-2, wpa_ie_len+2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
+			pstat->dot8021xalg = 1;/* psk,  todo:802.1x */
+			pstat->wpa_psk |= BIT(0);
+
+			pstat->wpa_group_cipher = group_cipher&psecuritypriv->wpa_group_cipher;
+			pstat->wpa_pairwise_cipher = pairwise_cipher&psecuritypriv->wpa_pairwise_cipher;
+
+			if (!pstat->wpa_group_cipher)
+				status = WLAN_STATUS_INVALID_GROUP_CIPHER;
+
+			if (!pstat->wpa_pairwise_cipher)
+				status = WLAN_STATUS_INVALID_PAIRWISE_CIPHER;
+		} else {
+			status = WLAN_STATUS_INVALID_IE;
+		}
+	} else {
+		wpa_ie = NULL;
+		wpa_ie_len = 0;
+	}
+
+	if (_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
+	if (wpa_ie == NULL) {
+		if (elems.wps_ie) {
+			DBG_88E("STA included WPS IE in "
+				   "(Re)Association Request - assume WPS is "
+				   "used\n");
+			pstat->flags |= WLAN_STA_WPS;
+			/* wpabuf_free(sta->wps_ie); */
+			/* sta->wps_ie = wpabuf_alloc_copy(elems.wps_ie + 4, */
+			/*				elems.wps_ie_len - 4); */
+		} else {
+			DBG_88E("STA did not include WPA/RSN IE "
+				   "in (Re)Association Request - possible WPS "
+				   "use\n");
+			pstat->flags |= WLAN_STA_MAYBE_WPS;
+		}
+
+
+		/*  AP support WPA/RSN, and sta is going to do WPS, but AP is not ready */
+		/*  that the selected registrar of AP is _FLASE */
+		if ((psecuritypriv->wpa_psk > 0) && (pstat->flags & (WLAN_STA_WPS|WLAN_STA_MAYBE_WPS))) {
+			if (pmlmepriv->wps_beacon_ie) {
+				u8 selected_registrar = 0;
+
+				rtw_get_wps_attr_content(pmlmepriv->wps_beacon_ie, pmlmepriv->wps_beacon_ie_len, WPS_ATTR_SELECTED_REGISTRAR , &selected_registrar, NULL);
+
+				if (!selected_registrar) {
+					DBG_88E("selected_registrar is false , or AP is not ready to do WPS\n");
+
+					status = _STATS_UNABLE_HANDLE_STA_;
+
+					goto OnAssocReqFail;
+				}
+			}
+		}
+	} else {
+		int copy_len;
+
+		if (psecuritypriv->wpa_psk == 0) {
+			DBG_88E("STA %pM: WPA/RSN IE in association "
+			"request, but AP don't support WPA/RSN\n", pstat->hwaddr);
+
+			status = WLAN_STATUS_INVALID_IE;
+
+			goto OnAssocReqFail;
+		}
+
+		if (elems.wps_ie) {
+			DBG_88E("STA included WPS IE in "
+				   "(Re)Association Request - WPS is "
+				   "used\n");
+			pstat->flags |= WLAN_STA_WPS;
+			copy_len = 0;
+		} else {
+			copy_len = ((wpa_ie_len+2) > sizeof(pstat->wpa_ie)) ? (sizeof(pstat->wpa_ie)) : (wpa_ie_len+2);
+		}
+		if (copy_len > 0)
+			memcpy(pstat->wpa_ie, wpa_ie-2, copy_len);
+	}
+	/*  check if there is WMM IE & support WWM-PS */
+	pstat->flags &= ~WLAN_STA_WME;
+	pstat->qos_option = 0;
+	pstat->qos_info = 0;
+	pstat->has_legacy_ac = true;
+	pstat->uapsd_vo = 0;
+	pstat->uapsd_vi = 0;
+	pstat->uapsd_be = 0;
+	pstat->uapsd_bk = 0;
+	if (pmlmepriv->qospriv.qos_option) {
+		p = pframe + WLAN_HDR_A3_LEN + ie_offset; ie_len = 0;
+		for (;;) {
+			p = rtw_get_ie(p, _VENDOR_SPECIFIC_IE_, &ie_len, pkt_len - WLAN_HDR_A3_LEN - ie_offset);
+			if (p != NULL) {
+				if (!memcmp(p+2, WMM_IE, 6)) {
+					pstat->flags |= WLAN_STA_WME;
+
+					pstat->qos_option = 1;
+					pstat->qos_info = *(p+8);
+
+					pstat->max_sp_len = (pstat->qos_info>>5)&0x3;
+
+					if ((pstat->qos_info&0xf) != 0xf)
+						pstat->has_legacy_ac = true;
+					else
+						pstat->has_legacy_ac = false;
+
+					if (pstat->qos_info&0xf) {
+						if (pstat->qos_info&BIT(0))
+							pstat->uapsd_vo = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_vo = 0;
+
+						if (pstat->qos_info&BIT(1))
+							pstat->uapsd_vi = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_vi = 0;
+
+						if (pstat->qos_info&BIT(2))
+							pstat->uapsd_bk = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_bk = 0;
+
+						if (pstat->qos_info&BIT(3))
+							pstat->uapsd_be = BIT(0)|BIT(1);
+						else
+							pstat->uapsd_be = 0;
+					}
+					break;
+				}
+			} else {
+				break;
+			}
+			p = p + ie_len + 2;
+		}
+	}
+
+	/* save HT capabilities in the sta object */
+	memset(&pstat->htpriv.ht_cap, 0, sizeof(struct rtw_ieee80211_ht_cap));
+	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) {
+		pstat->flags |= WLAN_STA_HT;
+
+		pstat->flags |= WLAN_STA_WME;
+
+		memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
+	} else {
+		pstat->flags &= ~WLAN_STA_HT;
+	}
+	if ((!pmlmepriv->htpriv.ht_option) && (pstat->flags&WLAN_STA_HT)) {
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+	if ((pstat->flags & WLAN_STA_HT) &&
+	    ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
+	    (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) {
+		DBG_88E("HT: %pM tried to "
+			"use TKIP with HT association\n", pstat->hwaddr);
+
+		/* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
+		/* goto OnAssocReqFail; */
+	}
+
+	pstat->flags |= WLAN_STA_NONERP;
+	for (i = 0; i < pstat->bssratelen; i++) {
+		if ((pstat->bssrateset[i] & 0x7f) > 22) {
+			pstat->flags &= ~WLAN_STA_NONERP;
+			break;
+		}
+	}
+
+	if (pstat->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		pstat->flags |= WLAN_STA_SHORT_PREAMBLE;
+	else
+		pstat->flags &= ~WLAN_STA_SHORT_PREAMBLE;
+
+
+
+	if (status != _STATS_SUCCESSFUL_)
+		goto OnAssocReqFail;
+
+	/* TODO: identify_proprietary_vendor_ie(); */
+	/*  Realtek proprietary IE */
+	/*  identify if this is Broadcom sta */
+	/*  identify if this is ralink sta */
+	/*  Customer proprietary IE */
+
+	/* get a unique AID */
+	if (pstat->aid > 0) {
+		DBG_88E("  old AID %d\n", pstat->aid);
+	} else {
+		for (pstat->aid = 1; pstat->aid <= NUM_STA; pstat->aid++)
+			if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
+				break;
+
+		/* if (pstat->aid > NUM_STA) { */
+		if (pstat->aid > pstapriv->max_num_sta) {
+			pstat->aid = 0;
+
+			DBG_88E("  no room for more AIDs\n");
+
+			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+
+			goto OnAssocReqFail;
+		} else {
+			pstapriv->sta_aid[pstat->aid - 1] = pstat;
+			DBG_88E("allocate new AID=(%d)\n", pstat->aid);
+		}
+	}
+
+	pstat->state &= (~WIFI_FW_ASSOC_STATE);
+	pstat->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	spin_lock_bh(&pstapriv->auth_list_lock);
+	if (!list_empty(&pstat->auth_list)) {
+		list_del_init(&pstat->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	spin_unlock_bh(&pstapriv->auth_list_lock);
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	if (list_empty(&pstat->asoc_list)) {
+		pstat->expire_to = pstapriv->expire_to;
+		list_add_tail(&pstat->asoc_list, &pstapriv->asoc_list);
+		pstapriv->asoc_list_cnt++;
+	}
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+	/*  now the station is qualified to join our BSS... */
+	if (pstat && (pstat->state & WIFI_FW_ASSOC_SUCCESS) && (_STATS_SUCCESSFUL_ == status)) {
+		/* 1 bss_cap_update & sta_info_update */
+		bss_cap_update_on_sta_join(padapter, pstat);
+		sta_info_update(padapter, pstat);
+
+		/* issue assoc rsp before notify station join event. */
+		if (frame_type == WIFI_ASSOCREQ)
+			issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+		else
+			issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+		/* 2 - report to upper layer */
+		DBG_88E("indicate_sta_join_event to upper layer - hostapd\n");
+		rtw_indicate_sta_assoc_event(padapter, pstat);
+
+		/* 3-(1) report sta add event */
+		report_add_sta_event(padapter, pstat->hwaddr, pstat->aid);
+	}
+
+	return _SUCCESS;
+
+asoc_class2_error:
+
+	issue_deauth(padapter, (void *)GetAddr2Ptr(pframe), status);
+
+	return _FAIL;
+
+OnAssocReqFail:
+
+	pstat->aid = 0;
+	if (frame_type == WIFI_ASSOCREQ)
+		issue_asocrsp(padapter, status, pstat, WIFI_ASSOCRSP);
+	else
+		issue_asocrsp(padapter, status, pstat, WIFI_REASSOCRSP);
+
+#endif /* CONFIG_88EU_AP_MODE */
+
+	return _FAIL;
+}
+
+static unsigned int OnAssocRsp(struct adapter *padapter,
+			       struct recv_frame *precv_frame)
+{
+	uint i;
+	int res;
+	unsigned short	status;
+	struct ndis_802_11_var_ie *pIE;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	/* struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network); */
+	u8 *pframe = precv_frame->rx_data;
+	uint pkt_len = precv_frame->len;
+
+	DBG_88E("%s\n", __func__);
+
+	/* check A1 matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), get_da(pframe), ETH_ALEN))
+		return _SUCCESS;
+
+	if (!(pmlmeinfo->state & (WIFI_FW_AUTH_SUCCESS | WIFI_FW_ASSOC_STATE)))
+		return _SUCCESS;
+
+	if (pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS)
+		return _SUCCESS;
+
+	del_timer_sync(&pmlmeext->link_timer);
+
+	/* status */
+	status = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 2));
+	if (status > 0) {
+		DBG_88E("assoc reject, status code: %d\n", status);
+		pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		res = -4;
+		goto report_assoc_result;
+	}
+
+	/* get capabilities */
+	pmlmeinfo->capability = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	/* set slot time */
+	pmlmeinfo->slotTime = (pmlmeinfo->capability & BIT(10)) ? 9 : 20;
+
+	/* AID */
+	pmlmeinfo->aid = (int)(le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN + 4))&0x3fff);
+	res = pmlmeinfo->aid;
+
+	/* following are moved to join event callback function */
+	/* to handle HT, WMM, rate adaptive, update MAC reg */
+	/* for not to handle the synchronous IO in the tasklet */
+	for (i = (6 + WLAN_HDR_A3_LEN); i < pkt_len;) {
+		pIE = (struct ndis_802_11_var_ie *)(pframe + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if (!memcmp(pIE->data, WMM_PARA_OUI, 6)) /* WMM */
+				WMM_param_handler(padapter, pIE);
+			break;
+		case _HT_CAPABILITY_IE_:	/* HT caps */
+			HT_caps_handler(padapter, pIE);
+			break;
+		case _HT_EXTRA_INFO_IE_:	/* HT info */
+			HT_info_handler(padapter, pIE);
+			break;
+		case _ERPINFO_IE_:
+			ERP_IE_handler(padapter, pIE);
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	pmlmeinfo->state &= (~WIFI_FW_ASSOC_STATE);
+	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
+
+	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
+	UpdateBrateTbl(padapter, pmlmeinfo->network.SupportedRates);
+
+report_assoc_result:
+	if (res > 0) {
+		rtw_buf_update(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len, pframe, pkt_len);
+	} else {
+		rtw_buf_free(&pmlmepriv->assoc_rsp, &pmlmepriv->assoc_rsp_len);
+	}
+
+	report_join_res(padapter, res);
+
+	return _SUCCESS;
+}
+
+static unsigned int OnDeAuth(struct adapter *padapter,
+			     struct recv_frame *precv_frame)
+{
+	unsigned short	reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->rx_data;
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+
+	/* check A3 */
+	if (memcmp(GetAddr3Ptr(pframe), pnetwork->MacAddress, ETH_ALEN))
+		return _SUCCESS;
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	DBG_88E("%s Reason code(%d)\n", __func__, reason);
+
+#ifdef CONFIG_88EU_AP_MODE
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		DBG_88E_LEVEL(_drv_always_, "ap recv deauth reason code(%d) sta:%pM\n",
+			      reason, GetAddr2Ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		if (psta) {
+			u8 updated = 0;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			if (!list_empty(&psta->asoc_list)) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, reason);
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			associated_clients_update(padapter, updated);
+		}
+
+
+		return _SUCCESS;
+	} else
+#endif
+	{
+		DBG_88E_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM\n",
+			      reason, GetAddr3Ptr(pframe));
+
+		receive_disconnect(padapter, GetAddr3Ptr(pframe) , reason);
+	}
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+}
+
+static unsigned int OnDisassoc(struct adapter *padapter,
+			       struct recv_frame *precv_frame)
+{
+	u16 reason;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->rx_data;
+	struct wlan_bssid_ex *pnetwork = &(pmlmeinfo->network);
+
+	/* check A3 */
+	if (memcmp(GetAddr3Ptr(pframe), pnetwork->MacAddress, ETH_ALEN))
+		return _SUCCESS;
+
+	reason = le16_to_cpu(*(__le16 *)(pframe + WLAN_HDR_A3_LEN));
+
+	DBG_88E("%s Reason code(%d)\n", __func__, reason);
+
+#ifdef CONFIG_88EU_AP_MODE
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
+			      reason, GetAddr2Ptr(pframe));
+
+		psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+		if (psta) {
+			u8 updated = 0;
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			if (!list_empty(&psta->asoc_list)) {
+				list_del_init(&psta->asoc_list);
+				pstapriv->asoc_list_cnt--;
+				updated = ap_free_sta(padapter, psta, false, reason);
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			associated_clients_update(padapter, updated);
+		}
+
+		return _SUCCESS;
+	} else
+#endif
+	{
+		DBG_88E_LEVEL(_drv_always_, "ap recv disassoc reason code(%d) sta:%pM\n",
+			      reason, GetAddr3Ptr(pframe));
+
+		receive_disconnect(padapter, GetAddr3Ptr(pframe), reason);
+	}
+	pmlmepriv->LinkDetectInfo.bBusyTraffic = false;
+	return _SUCCESS;
+}
+
+static unsigned int OnAtim(struct adapter *padapter,
+			   struct recv_frame *precv_frame)
+{
+	DBG_88E("%s\n", __func__);
+	return _SUCCESS;
+}
+
+static unsigned int on_action_spct(struct adapter *padapter,
+				   struct recv_frame *precv_frame)
+{
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body = (u8 *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+	u8 category;
+	u8 action;
+
+	DBG_88E(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+	psta = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+
+	if (!psta)
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_SPECTRUM_MGMT)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case RTW_WLAN_ACTION_SPCT_MSR_REQ:
+	case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
+	case RTW_WLAN_ACTION_SPCT_TPC_REQ:
+	case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
+		break;
+	case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
+		break;
+	default:
+		break;
+	}
+
+exit:
+	return _FAIL;
+}
+
+static unsigned int OnAction_qos(struct adapter *padapter,
+				 struct recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+static unsigned int OnAction_dls(struct adapter *padapter,
+				 struct recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+static unsigned int OnAction_back(struct adapter *padapter,
+				  struct recv_frame *precv_frame)
+{
+	u8 *addr;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	unsigned char		*frame_body;
+	unsigned char		category, action;
+	unsigned short	tid, status, reason_code = 0;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 *pframe = precv_frame->rx_data;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	/* check RA matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe),
+		   ETH_ALEN))/* for if1, sta/ap mode */
+		return _SUCCESS;
+
+	DBG_88E("%s\n", __func__);
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		if (!(pmlmeinfo->state & WIFI_FW_ASSOC_SUCCESS))
+			return _SUCCESS;
+
+	addr = GetAddr2Ptr(pframe);
+	psta = rtw_get_stainfo(pstapriv, addr);
+
+	if (psta == NULL)
+		return _SUCCESS;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+	if (category == RTW_WLAN_CATEGORY_BACK) { /*  representing Block Ack */
+		if (!pmlmeinfo->HT_enable)
+			return _SUCCESS;
+		action = frame_body[1];
+		DBG_88E("%s, action=%d\n", __func__, action);
+		switch (action) {
+		case RTW_WLAN_ACTION_ADDBA_REQ: /* ADDBA request */
+			memcpy(&(pmlmeinfo->ADDBA_req), &(frame_body[2]), sizeof(struct ADDBA_request));
+			process_addba_req(padapter, (u8 *)&(pmlmeinfo->ADDBA_req), addr);
+
+			if (pmlmeinfo->bAcceptAddbaReq)
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
+			else
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
+			break;
+		case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
+			status = get_unaligned_le16(&frame_body[3]);
+			tid = (frame_body[5] >> 2) & 0x7;
+			if (status == 0) {	/* successful */
+				DBG_88E("agg_enable for TID=%d\n", tid);
+				psta->htpriv.agg_enable_bitmap |= 1 << tid;
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+			} else {
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+			}
+			break;
+		case RTW_WLAN_ACTION_DELBA: /* DELBA */
+			if ((frame_body[3] & BIT(3)) == 0) {
+				psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+				psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+				reason_code = get_unaligned_le16(&frame_body[4]);
+			} else if ((frame_body[3] & BIT(3)) == BIT(3)) {
+				tid = (frame_body[3] >> 4) & 0x0F;
+				preorder_ctrl =  &psta->recvreorder_ctrl[tid];
+				preorder_ctrl->enable = false;
+				preorder_ctrl->indicate_seq = 0xffff;
+			}
+			DBG_88E("%s(): DELBA: %x(%x)\n", __func__, pmlmeinfo->agg_enable_bitmap, reason_code);
+			/* todo: how to notify the host while receiving DELETE BA */
+			break;
+		default:
+			break;
+		}
+	}
+	return _SUCCESS;
+}
+
+static s32 rtw_action_public_decache(struct recv_frame *recv_frame, s32 token)
+{
+	struct adapter *adapter = recv_frame->adapter;
+	struct mlme_ext_priv *mlmeext = &(adapter->mlmeextpriv);
+	u8 *frame = recv_frame->rx_data;
+	u16 seq_ctrl = ((recv_frame->attrib.seq_num&0xffff) << 4) |
+		(recv_frame->attrib.frag_num & 0xf);
+
+	if (GetRetry(frame)) {
+		if (token >= 0) {
+			if ((seq_ctrl == mlmeext->action_public_rxseq) && (token == mlmeext->action_public_dialog_token)) {
+				DBG_88E(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x, token:%d\n",
+					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
+				return _FAIL;
+			}
+		} else {
+			if (seq_ctrl == mlmeext->action_public_rxseq) {
+				DBG_88E(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x\n",
+					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq);
+				return _FAIL;
+			}
+		}
+	}
+
+	mlmeext->action_public_rxseq = seq_ctrl;
+
+	if (token >= 0)
+		mlmeext->action_public_dialog_token = token;
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_p2p(struct recv_frame *precv_frame)
+{
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body;
+	u8 dialogToken = 0;
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	dialogToken = frame_body[7];
+
+	if (rtw_action_public_decache(precv_frame, dialogToken) == _FAIL)
+		return _FAIL;
+
+	return _SUCCESS;
+}
+
+static unsigned int on_action_public_vendor(struct recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+
+	if (!memcmp(frame_body + 2, P2P_OUI, 4))
+		ret = on_action_public_p2p(precv_frame);
+
+	return ret;
+}
+
+static unsigned int on_action_public_default(struct recv_frame *precv_frame, u8 action)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 token;
+
+	token = frame_body[2];
+
+	if (rtw_action_public_decache(precv_frame, token) == _FAIL)
+		goto exit;
+
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+
+static unsigned int on_action_public(struct adapter *padapter,
+				     struct recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	u8 *pframe = precv_frame->rx_data;
+	u8 *frame_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
+	u8 category, action;
+
+	/* check RA matches or not */
+	if (memcmp(myid(&(padapter->eeprompriv)), GetAddr1Ptr(pframe), ETH_ALEN))
+		goto exit;
+
+	category = frame_body[0];
+	if (category != RTW_WLAN_CATEGORY_PUBLIC)
+		goto exit;
+
+	action = frame_body[1];
+	switch (action) {
+	case ACT_PUBLIC_VENDOR:
+		ret = on_action_public_vendor(precv_frame);
+		break;
+	default:
+		ret = on_action_public_default(precv_frame, action);
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+static unsigned int OnAction_ht(struct adapter *padapter,
+				struct recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+static unsigned int OnAction_wmm(struct adapter *padapter,
+				 struct recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+static unsigned int OnAction_p2p(struct adapter *padapter,
+				 struct recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+static unsigned int DoReserved(struct adapter *padapter,
+			       struct recv_frame *precv_frame)
+{
+	return _SUCCESS;
+}
+
+static struct action_handler OnAction_tbl[] = {
+	{RTW_WLAN_CATEGORY_SPECTRUM_MGMT,	 "ACTION_SPECTRUM_MGMT", on_action_spct},
+	{RTW_WLAN_CATEGORY_QOS, "ACTION_QOS", &OnAction_qos},
+	{RTW_WLAN_CATEGORY_DLS, "ACTION_DLS", &OnAction_dls},
+	{RTW_WLAN_CATEGORY_BACK, "ACTION_BACK", &OnAction_back},
+	{RTW_WLAN_CATEGORY_PUBLIC, "ACTION_PUBLIC", on_action_public},
+	{RTW_WLAN_CATEGORY_RADIO_MEASUREMENT, "ACTION_RADIO_MEASUREMENT", &DoReserved},
+	{RTW_WLAN_CATEGORY_FT, "ACTION_FT",	&DoReserved},
+	{RTW_WLAN_CATEGORY_HT,	"ACTION_HT",	&OnAction_ht},
+	{RTW_WLAN_CATEGORY_SA_QUERY, "ACTION_SA_QUERY", &DoReserved},
+	{RTW_WLAN_CATEGORY_WMM, "ACTION_WMM", &OnAction_wmm},
+	{RTW_WLAN_CATEGORY_P2P, "ACTION_P2P", &OnAction_p2p},
+};
+
+static unsigned int OnAction(struct adapter *padapter,
+			     struct recv_frame *precv_frame)
+{
+	int i;
+	unsigned char	category;
+	struct action_handler *ptable;
+	unsigned char	*frame_body;
+	u8 *pframe = precv_frame->rx_data;
+
+	frame_body = (unsigned char *)(pframe + sizeof(struct rtw_ieee80211_hdr_3addr));
+
+	category = frame_body[0];
+
+	for (i = 0; i < sizeof(OnAction_tbl)/sizeof(struct action_handler); i++) {
+		ptable = &OnAction_tbl[i];
+		if (category == ptable->num)
+			ptable->func(padapter, precv_frame);
+	}
+	return _SUCCESS;
+}
+
+/****************************************************************************
+
+Following are the initialization functions for WiFi MLME
+
+*****************************************************************************/
+
+static struct mlme_handler mlme_sta_tbl[] = {
+	{WIFI_ASSOCREQ,		"OnAssocReq",	&OnAssocReq},
+	{WIFI_ASSOCRSP,		"OnAssocRsp",	&OnAssocRsp},
+	{WIFI_REASSOCREQ,	"OnReAssocReq",	&OnAssocReq},
+	{WIFI_REASSOCRSP,	"OnReAssocRsp",	&OnAssocRsp},
+	{WIFI_PROBEREQ,		"OnProbeReq",	&OnProbeReq},
+	{WIFI_PROBERSP,		"OnProbeRsp",		&OnProbeRsp},
+
+	/*----------------------------------------------------------
+					below 2 are reserved
+	-----------------------------------------------------------*/
+	{0,					"DoReserved",		&DoReserved},
+	{0,					"DoReserved",		&DoReserved},
+	{WIFI_BEACON,		"OnBeacon",		&OnBeacon},
+	{WIFI_ATIM,			"OnATIM",		&OnAtim},
+	{WIFI_DISASSOC,		"OnDisassoc",		&OnDisassoc},
+	{WIFI_AUTH,			"OnAuth",		&OnAuthClient},
+	{WIFI_DEAUTH,		"OnDeAuth",		&OnDeAuth},
+	{WIFI_ACTION,		"OnAction",		&OnAction},
+};
+
+int init_hw_mlme_ext(struct adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+	return _SUCCESS;
+}
+
+static void init_mlme_ext_priv_value(struct adapter *padapter)
+{
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	unsigned char	mixed_datarate[NumRates] = {
+		_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
+		_9M_RATE_, _12M_RATE_, _18M_RATE_, _24M_RATE_, _36M_RATE_,
+		 _48M_RATE_, _54M_RATE_, 0xff
+	};
+	unsigned char	mixed_basicrate[NumRates] = {
+		_1M_RATE_, _2M_RATE_, _5M_RATE_, _11M_RATE_, _6M_RATE_,
+		_12M_RATE_, _24M_RATE_, 0xff,
+	};
+
+	atomic_set(&pmlmeext->event_seq, 0);
+	pmlmeext->mgnt_seq = 0;/* reset to zero when disconnect at client mode */
+
+	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_bwmode = pmlmeext->cur_bwmode;
+	pmlmeext->oper_ch_offset = pmlmeext->cur_ch_offset;
+	pmlmeext->retry = 0;
+
+	pmlmeext->cur_wireless_mode = padapter->registrypriv.wireless_mode;
+
+	memcpy(pmlmeext->datarate, mixed_datarate, NumRates);
+	memcpy(pmlmeext->basicrate, mixed_basicrate, NumRates);
+
+	pmlmeext->tx_rate = IEEE80211_CCK_RATE_1MB;
+
+	pmlmeext->sitesurvey_res.state = SCAN_DISABLE;
+	pmlmeext->sitesurvey_res.channel_idx = 0;
+	pmlmeext->sitesurvey_res.bss_cnt = 0;
+	pmlmeext->scan_abort = false;
+
+	pmlmeinfo->state = WIFI_FW_NULL_STATE;
+	pmlmeinfo->reauth_count = 0;
+	pmlmeinfo->reassoc_count = 0;
+	pmlmeinfo->link_count = 0;
+	pmlmeinfo->auth_seq = 0;
+	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Open;
+	pmlmeinfo->key_index = 0;
+	pmlmeinfo->iv = 0;
+
+	pmlmeinfo->enc_algo = _NO_PRIVACY_;
+	pmlmeinfo->authModeToggle = 0;
+
+	memset(pmlmeinfo->chg_txt, 0, 128);
+
+	pmlmeinfo->slotTime = SHORT_SLOT_TIME;
+	pmlmeinfo->preamble_mode = PREAMBLE_AUTO;
+
+	pmlmeinfo->dialogToken = 0;
+
+	pmlmeext->action_public_rxseq = 0xffff;
+	pmlmeext->action_public_dialog_token = 0xff;
+}
+
+static int has_channel(struct rt_channel_info *channel_set,
+					   u8 chanset_size,
+					   u8 chan) {
+	int i;
+
+	for (i = 0; i < chanset_size; i++) {
+		if (channel_set[i].ChannelNum == chan)
+			return 1;
+	}
+	return 0;
+}
+
+static void init_channel_list(struct adapter *padapter, struct rt_channel_info *channel_set,
+							  u8 chanset_size,
+							  struct p2p_channels *channel_list) {
+	struct p2p_oper_class_map op_class[] = {
+		{ IEEE80211G,  81,   1,  13,  1, BW20 },
+		{ IEEE80211G,  82,  14,  14,  1, BW20 },
+		{ -1, 0, 0, 0, 0, BW20 }
+	};
+
+	int cla, op;
+
+	cla = 0;
+
+	for (op = 0; op_class[op].op_class; op++) {
+		u8 ch;
+		struct p2p_oper_class_map *o = &op_class[op];
+		struct p2p_reg_class *reg = NULL;
+
+		for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
+			if (!has_channel(channel_set, chanset_size, ch)) {
+				continue;
+			}
+
+			if ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
+				continue;
+
+			if ((0 == (padapter->registrypriv.cbw40_enable & BIT(1))) &&
+			    ((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
+				continue;
+
+			if (reg == NULL) {
+				reg = &channel_list->reg_class[cla];
+				cla++;
+				reg->reg_class = o->op_class;
+				reg->channels = 0;
+			}
+			reg->channel[reg->channels] = ch;
+			reg->channels++;
+		}
+	}
+	channel_list->reg_classes = cla;
+}
+
+static u8 init_channel_set(struct adapter *padapter, u8 ChannelPlan, struct rt_channel_info *channel_set)
+{
+	u8 index, chanset_size = 0;
+	u8 b2_4GBand = false;
+	u8 Index2G = 0;
+
+	memset(channel_set, 0, sizeof(struct rt_channel_info) * MAX_CHANNEL_NUM);
+
+	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
+		DBG_88E("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
+		return chanset_size;
+	}
+
+	if (padapter->registrypriv.wireless_mode & WIRELESS_11G) {
+		b2_4GBand = true;
+		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+			Index2G = RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE.Index2G;
+		else
+			Index2G = RTW_ChannelPlanMap[ChannelPlan].Index2G;
+	}
+
+	if (b2_4GBand) {
+		for (index = 0; index < RTW_ChannelPlan2G[Index2G].Len; index++) {
+			channel_set[chanset_size].ChannelNum = RTW_ChannelPlan2G[Index2G].Channel[index];
+
+			if ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */
+			    (RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN_2G == ChannelPlan)) {
+				if (channel_set[chanset_size].ChannelNum >= 1 && channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else if ((channel_set[chanset_size].ChannelNum  >= 12 && channel_set[chanset_size].ChannelNum  <= 14))
+					channel_set[chanset_size].ScanType  = SCAN_PASSIVE;
+			} else if (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
+				   RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) {/*  channel 12~13, passive scan */
+				if (channel_set[chanset_size].ChannelNum <= 11)
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+			} else {
+				channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+			}
+
+			chanset_size++;
+		}
+	}
+	return chanset_size;
+}
+
+int	init_mlme_ext_priv(struct adapter *padapter)
+{
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmlmeext->padapter = padapter;
+
+	init_mlme_ext_priv_value(padapter);
+	pmlmeinfo->bAcceptAddbaReq = pregistrypriv->bAcceptAddbaReq;
+
+	init_mlme_ext_timer(padapter);
+
+#ifdef CONFIG_88EU_AP_MODE
+	init_mlme_ap_info(padapter);
+#endif
+
+	pmlmeext->max_chan_nums = init_channel_set(padapter, pmlmepriv->ChannelPlan, pmlmeext->channel_set);
+	init_channel_list(padapter, pmlmeext->channel_set, pmlmeext->max_chan_nums, &pmlmeext->channel_list);
+
+	pmlmeext->chan_scan_time = SURVEY_TO;
+	pmlmeext->mlmeext_init = true;
+
+
+	pmlmeext->active_keep_alive_check = true;
+
+	return _SUCCESS;
+}
+
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
+{
+	struct adapter *padapter = pmlmeext->padapter;
+
+	if (!padapter)
+		return;
+
+	if (padapter->bDriverStopped) {
+		del_timer_sync(&pmlmeext->survey_timer);
+		del_timer_sync(&pmlmeext->link_timer);
+	}
+}
+
+static void _mgt_dispatcher(struct adapter *padapter, struct mlme_handler *ptable, struct recv_frame *precv_frame)
+{
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 *pframe = precv_frame->rx_data;
+
+	  if (ptable->func) {
+	 /* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+		if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+		    memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+			return;
+		ptable->func(padapter, precv_frame);
+	}
+}
+
+void mgt_dispatcher(struct adapter *padapter, struct recv_frame *precv_frame)
+{
+	int index;
+	struct mlme_handler *ptable;
+#ifdef CONFIG_88EU_AP_MODE
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+#endif /* CONFIG_88EU_AP_MODE */
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 *pframe = precv_frame->rx_data;
+	struct sta_info *psta = rtw_get_stainfo(&padapter->stapriv, GetAddr2Ptr(pframe));
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
+		  GetFrameType(pframe), GetFrameSubType(pframe)));
+
+	if (GetFrameType(pframe) != WIFI_MGT_TYPE) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
+		return;
+	}
+
+	/* receive the frames that ra(a1) is my address or ra(a1) is bc address. */
+	if (memcmp(GetAddr1Ptr(pframe), myid(&padapter->eeprompriv), ETH_ALEN) &&
+	    memcmp(GetAddr1Ptr(pframe), bc_addr, ETH_ALEN))
+		return;
+
+	ptable = mlme_sta_tbl;
+
+	index = GetFrameSubType(pframe) >> 4;
+
+	if (index > 13) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Currently we do not support reserved sub-fr-type=%d\n", index));
+		return;
+	}
+	ptable += index;
+
+	if (psta != NULL) {
+		if (GetRetry(pframe)) {
+			if (precv_frame->attrib.seq_num ==
+			    psta->RxMgmtFrameSeqNum) {
+				/* drop the duplicate management frame */
+				DBG_88E("Drop duplicate management frame with seq_num=%d.\n",
+					precv_frame->attrib.seq_num);
+				return;
+			}
+		}
+		psta->RxMgmtFrameSeqNum = precv_frame->attrib.seq_num;
+	}
+
+#ifdef CONFIG_88EU_AP_MODE
+	switch (GetFrameSubType(pframe)) {
+	case WIFI_AUTH:
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+			ptable->func = &OnAuth;
+		else
+			ptable->func = &OnAuthClient;
+		/* fall through */
+	case WIFI_ASSOCREQ:
+	case WIFI_REASSOCREQ:
+	case WIFI_PROBEREQ:
+	case WIFI_BEACON:
+	case WIFI_ACTION:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	default:
+		_mgt_dispatcher(padapter, ptable, precv_frame);
+		break;
+	}
+#else
+	_mgt_dispatcher(padapter, ptable, precv_frame);
+#endif
+}
+
+/****************************************************************************
+
 Following are the functions to report events
 
 *****************************************************************************/
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
index cda725a8f9cd..8501eb898824 100644
--- a/drivers/staging/rtl8188eu/core/rtw_recv.c
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -19,6 +19,8 @@
  ******************************************************************************/
 #define _RTW_RECV_C_
 
+#include <linux/ieee80211.h>
+
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <recv_osdep.h>
diff --git a/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c b/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
index dc9d0ddf6b3a..b340e4a9d540 100644
--- a/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
+++ b/drivers/staging/rtl8188eu/core/rtw_sta_mgt.c
@@ -54,14 +54,12 @@ static void _rtw_init_stainfo(struct sta_info *psta)
 
 	psta->bpairwise_key_installed = false;
 
-#ifdef CONFIG_88EU_AP_MODE
 	psta->nonerp_set = 0;
 	psta->no_short_slot_time_set = 0;
 	psta->no_short_preamble_set = 0;
 	psta->no_ht_gf_set = 0;
 	psta->no_ht_set = 0;
 	psta->ht_20mhz_set = 0;
-#endif
 
 	psta->under_exist_checking = 0;
 
@@ -145,32 +143,6 @@ inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int
 	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
 }
 
-/*  this function is used to free the memory of lock || sema for all stainfos */
-static void rtw_mfree_all_stainfo(struct sta_priv *pstapriv)
-{
-	struct list_head *plist, *phead;
-	struct sta_info *psta = NULL;
-
-
-	spin_lock_bh(&pstapriv->sta_hash_lock);
-
-	phead = get_list_head(&pstapriv->free_sta_queue);
-	plist = phead->next;
-
-	while (phead != plist) {
-		psta = container_of(plist, struct sta_info , list);
-		plist = plist->next;
-	}
-
-	spin_unlock_bh(&pstapriv->sta_hash_lock);
-
-}
-
-static void rtw_mfree_sta_priv_lock(struct sta_priv *pstapriv)
-{
-	 rtw_mfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */
-}
-
 u32	_rtw_free_sta_priv(struct	sta_priv *pstapriv)
 {
 	struct list_head *phead, *plist;
@@ -199,8 +171,6 @@ u32	_rtw_free_sta_priv(struct	sta_priv *pstapriv)
 		spin_unlock_bh(&pstapriv->sta_hash_lock);
 		/*===============================*/
 
-		rtw_mfree_sta_priv_lock(pstapriv);
-
 		if (pstapriv->pallocated_stainfo_buf)
 			vfree(pstapriv->pallocated_stainfo_buf);
 	}
@@ -259,7 +229,7 @@ struct	sta_info *rtw_alloc_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
 
 		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_info_,
 			 ("alloc number_%d stainfo  with hwaddr = %pM\n",
-			 pstapriv->asoc_sta_count , hwaddr));
+			 pstapriv->asoc_sta_count, hwaddr));
 
 		init_addba_retry_timer(pstapriv->padapter, psta);
 
@@ -293,7 +263,7 @@ exit:
 }
 
 /*  using pstapriv->sta_hash_lock to protect */
-u32	rtw_free_stainfo(struct adapter *padapter , struct sta_info *psta)
+u32	rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta)
 {
 	int i;
 	struct __queue *pfree_sta_queue;
@@ -442,12 +412,12 @@ void rtw_free_all_stainfo(struct adapter *padapter)
 		plist = phead->next;
 
 		while (phead != plist) {
-			psta = container_of(plist, struct sta_info , hash_list);
+			psta = container_of(plist, struct sta_info, hash_list);
 
 			plist = plist->next;
 
 			if (pbcmc_stainfo != psta)
-				rtw_free_stainfo(padapter , psta);
+				rtw_free_stainfo(padapter, psta);
 		}
 	}
 	spin_unlock_bh(&pstapriv->sta_hash_lock);
diff --git a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
index 2b371757cbfe..32300df7b996 100644
--- a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
@@ -603,7 +603,7 @@ void WMMOnAssocRsp(struct adapter *padapter)
 	inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
 
 	if (pregpriv->wifi_spec == 1) {
-		u32	j, tmp, change_inx = false;
+		u32	j, change_inx = false;
 
 		/* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
 		for (i = 0; i < 4; i++) {
@@ -618,14 +618,8 @@ void WMMOnAssocRsp(struct adapter *padapter)
 				}
 
 				if (change_inx) {
-					tmp = edca[i];
-					edca[i] = edca[j];
-					edca[j] = tmp;
-
-					tmp = inx[i];
-					inx[i] = inx[j];
-					inx[j] = tmp;
-
+					swap(edca[i], edca[j]);
+					swap(inx[i], inx[j]);
 					change_inx = false;
 				}
 			}
diff --git a/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c b/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
index 082f0ca198ef..15a176596305 100644
--- a/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
+++ b/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
@@ -87,7 +87,7 @@ static u8 DROPING_NECESSARY[RATESIZE] = {
 
 static u8 PendingForRateUpFail[5] = {2, 10, 24, 40, 60};
 static u16 DynamicTxRPTTiming[6] = {
-	0x186a, 0x30d4, 0x493e, 0x61a8, 0x7a12 , 0x927c}; /*  200ms-1200ms */
+	0x186a, 0x30d4, 0x493e, 0x61a8, 0x7a12, 0x927c}; /*  200ms-1200ms */
 
 /*  End Rate adaptive parameters */
 
diff --git a/drivers/staging/rtl8188eu/hal/hal_intf.c b/drivers/staging/rtl8188eu/hal/hal_intf.c
index 4bdbed28774e..5edb5c41c8e7 100644
--- a/drivers/staging/rtl8188eu/hal/hal_intf.c
+++ b/drivers/staging/rtl8188eu/hal/hal_intf.c
@@ -202,6 +202,7 @@ s32 rtw_hal_xmit(struct adapter *adapt, struct xmit_frame *pxmitframe)
 s32 rtw_hal_mgnt_xmit(struct adapter *adapt, struct xmit_frame *pmgntframe)
 {
 	s32 ret = _FAIL;
+
 	if (adapt->HalFunc.mgnt_xmit)
 		ret = adapt->HalFunc.mgnt_xmit(adapt, pmgntframe);
 	return ret;
@@ -236,6 +237,7 @@ void rtw_hal_update_ra_mask(struct adapter *adapt, u32 mac_id, u8 rssi_level)
 #ifdef CONFIG_88EU_AP_MODE
 		struct sta_info *psta = NULL;
 		struct sta_priv *pstapriv = &adapt->stapriv;
+
 		if ((mac_id-1) > 0)
 			psta = pstapriv->sta_aid[(mac_id-1) - 1];
 		if (psta)
diff --git a/drivers/staging/rtl8188eu/hal/phy.c b/drivers/staging/rtl8188eu/hal/phy.c
index 6e4c3ee0399a..2eafa503f343 100644
--- a/drivers/staging/rtl8188eu/hal/phy.c
+++ b/drivers/staging/rtl8188eu/hal/phy.c
@@ -629,7 +629,7 @@ void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
 
 static u8 phy_path_a_iqk(struct adapter *adapt, bool config_pathb)
 {
-	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
+	u32 reg_eac, reg_e94, reg_e9c;
 	u8 result = 0x00;
 
 	/* 1 Tx IQK */
@@ -651,7 +651,6 @@ static u8 phy_path_a_iqk(struct adapter *adapt, bool config_pathb)
 	reg_eac = phy_query_bb_reg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
 	reg_e94 = phy_query_bb_reg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
 	reg_e9c = phy_query_bb_reg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
-	reg_ea4 = phy_query_bb_reg(adapt, rRx_Power_Before_IQK_A_2, bMaskDWord);
 
 	if (!(reg_eac & BIT28) &&
 	    (((reg_e94 & 0x03FF0000)>>16) != 0x142) &&
@@ -1316,8 +1315,7 @@ void rtl88eu_phy_iq_calibrate(struct adapter *adapt, bool recovery)
 	s32 result[4][8];
 	u8 i, final, chn_index;
 	bool pathaok, pathbok;
-	s32 reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
-	    reg_ecc;
+	s32 reg_e94, reg_e9c, reg_ea4, reg_eb4, reg_ebc, reg_ec4;
 	bool is12simular, is13simular, is23simular;
 	bool singletone = false, carrier_sup = false;
 	u32 iqk_bb_reg_92c[IQK_BB_REG_NUM] = {
@@ -1389,18 +1387,15 @@ void rtl88eu_phy_iq_calibrate(struct adapter *adapt, bool recovery)
 		reg_e94 = result[i][0];
 		reg_e9c = result[i][1];
 		reg_ea4 = result[i][2];
-		reg_eac = result[i][3];
 		reg_eb4 = result[i][4];
 		reg_ebc = result[i][5];
 		reg_ec4 = result[i][6];
-		reg_ecc = result[i][7];
 	}
 
 	if (final != 0xff) {
 		reg_e94 = result[final][0];
 		reg_e9c = result[final][1];
 		reg_ea4 = result[final][2];
-		reg_eac = result[final][3];
 		reg_eb4 = result[final][4];
 		reg_ebc = result[final][5];
 		dm_odm->RFCalibrateInfo.RegE94 = reg_e94;
@@ -1408,7 +1403,6 @@ void rtl88eu_phy_iq_calibrate(struct adapter *adapt, bool recovery)
 		dm_odm->RFCalibrateInfo.RegEB4 = reg_eb4;
 		dm_odm->RFCalibrateInfo.RegEBC = reg_ebc;
 		reg_ec4 = result[final][6];
-		reg_ecc = result[final][7];
 		pathaok = true;
 		pathbok = true;
 	} else {
diff --git a/drivers/staging/rtl8188eu/hal/pwrseqcmd.c b/drivers/staging/rtl8188eu/hal/pwrseqcmd.c
index 73e1f8b36b37..3e60b23819ae 100644
--- a/drivers/staging/rtl8188eu/hal/pwrseqcmd.c
+++ b/drivers/staging/rtl8188eu/hal/pwrseqcmd.c
@@ -37,7 +37,7 @@ u8 rtl88eu_pwrseqcmdparsing(struct adapter *padapter, u8 cut_vers, u8 fab_vers,
 	do {
 		pwrcfgcmd = pwrseqcmd[aryidx];
 
-		RT_TRACE(_module_hal_init_c_ , _drv_info_,
+		RT_TRACE(_module_hal_init_c_, _drv_info_,
 			 ("rtl88eu_pwrseqcmdparsing: offset(%#x) cut_msk(%#x)"
 			  "fab_msk(%#x) interface_msk(%#x) base(%#x) cmd(%#x)"
 			  "msk(%#x) value(%#x)\n",
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
index 01566210bbd2..fca590949409 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
@@ -151,7 +151,6 @@ void rtl8188e_InitHalDm(struct adapter *Adapter)
 
 void rtl8188e_HalDmWatchDog(struct adapter *Adapter)
 {
-	bool fw_cur_in_ps = false;
 	bool fw_ps_awake = true;
 	u8 hw_init_completed = false;
 	struct hal_data_8188e *hal_data = GET_HAL_DATA(Adapter);
@@ -163,7 +162,6 @@ void rtl8188e_HalDmWatchDog(struct adapter *Adapter)
 	if (!hw_init_completed)
 		goto skip_dm;
 
-	fw_cur_in_ps = Adapter->pwrctrlpriv.bFwCurrentInPSMode;
 	rtw_hal_get_hwreg(Adapter, HW_VAR_FWLPS_RF_ON, (u8 *)(&fw_ps_awake));
 
 	/*  Fw is under p2p powersaving mode, driver should stop dynamic mechanism. */
diff --git a/drivers/staging/rtl8188eu/hal/usb_halinit.c b/drivers/staging/rtl8188eu/hal/usb_halinit.c
index 7b01d5aa6b23..872622214264 100644
--- a/drivers/staging/rtl8188eu/hal/usb_halinit.c
+++ b/drivers/staging/rtl8188eu/hal/usb_halinit.c
@@ -2077,7 +2077,6 @@ static void UpdateHalRAMask8188EUsb(struct adapter *adapt, u32 mac_id, u8 rssi_l
 		break;
 	}
 
-	rate_bitmap = 0x0fffffff;
 	rate_bitmap = ODM_Get_Rate_Bitmap(&haldata->odmpriv, mac_id, mask, rssi_level);
 	DBG_88E("%s => mac_id:%d, networkType:0x%02x, mask:0x%08x\n\t ==> rssi_level:%d, rate_bitmap:0x%08x\n",
 		__func__, mac_id, networkType, mask, rssi_level, rate_bitmap);
diff --git a/drivers/staging/rtl8188eu/include/drv_types.h b/drivers/staging/rtl8188eu/include/drv_types.h
index c81317906adc..bcc74dcd8207 100644
--- a/drivers/staging/rtl8188eu/include/drv_types.h
+++ b/drivers/staging/rtl8188eu/include/drv_types.h
@@ -175,7 +175,6 @@ static inline struct device *dvobj_to_dev(struct dvobj_priv *dvobj)
 };
 
 struct adapter {
-	int	pid[3];/* process id from UI, 0:wps, 1:hostapd, 2:dhcpcd */
 	u16	chip_type;
 
 	struct dvobj_priv *dvobj;
diff --git a/drivers/staging/rtl8188eu/include/ieee80211.h b/drivers/staging/rtl8188eu/include/ieee80211.h
index 8fd35dcdbb94..b129ad148b47 100644
--- a/drivers/staging/rtl8188eu/include/ieee80211.h
+++ b/drivers/staging/rtl8188eu/include/ieee80211.h
@@ -493,34 +493,7 @@ struct ieee80211_snap_hdr {
 #define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
 #define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
 
-/* Status codes */
-#define WLAN_STATUS_SUCCESS 0
-#define WLAN_STATUS_UNSPECIFIED_FAILURE 1
-#define WLAN_STATUS_CAPS_UNSUPPORTED 10
-#define WLAN_STATUS_REASSOC_NO_ASSOC 11
-#define WLAN_STATUS_ASSOC_DENIED_UNSPEC 12
-#define WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG 13
-#define WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION 14
-#define WLAN_STATUS_CHALLENGE_FAIL 15
-#define WLAN_STATUS_AUTH_TIMEOUT 16
-#define WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA 17
-#define WLAN_STATUS_ASSOC_DENIED_RATES 18
-/* 802.11b */
-#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
-#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
-#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
-
-/* Reason codes */
-#define WLAN_REASON_UNSPECIFIED 1
-#define WLAN_REASON_PREV_AUTH_NOT_VALID 2
-#define WLAN_REASON_DEAUTH_LEAVING 3
-#define WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY 4
-#define WLAN_REASON_DISASSOC_AP_BUSY 5
-#define WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA 6
-#define WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA 7
-#define WLAN_REASON_DISASSOC_STA_HAS_LEFT 8
-#define WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH 9
-#define WLAN_REASON_JOIN_WRONG_CHANNEL       65534
+/* Non standard?  Not in <linux/ieee80211.h> */
 #define WLAN_REASON_EXPIRATION_CHK 65535
 
 /* Information Element IDs */
diff --git a/drivers/staging/rtl8188eu/include/odm.h b/drivers/staging/rtl8188eu/include/odm.h
index 525eb100cd15..73ef9c7596c1 100644
--- a/drivers/staging/rtl8188eu/include/odm.h
+++ b/drivers/staging/rtl8188eu/include/odm.h
@@ -918,14 +918,6 @@ enum ODM_RF_CONTENT {
 	odm_radiod_txt = 0x1003
 };
 
-enum odm_bb_config_type {
-    CONFIG_BB_PHY_REG,
-    CONFIG_BB_AGC_TAB,
-    CONFIG_BB_AGC_TAB_2G,
-    CONFIG_BB_AGC_TAB_5G,
-    CONFIG_BB_PHY_REG_PG,
-};
-
 /*  Status code */
 enum rt_status {
 	RT_STATUS_SUCCESS,
@@ -1068,7 +1060,7 @@ enum dm_swas {
 
 extern	u32 OFDMSwingTable[OFDM_TABLE_SIZE_92D];
 extern	u8 CCKSwingTable_Ch1_Ch13[CCK_TABLE_SIZE][8];
-extern	u8 CCKSwingTable_Ch14 [CCK_TABLE_SIZE][8];
+extern	u8 CCKSwingTable_Ch14[CCK_TABLE_SIZE][8];
 
 /*  check Sta pointer valid or not */
 #define IS_STA_VALID(pSta)		(pSta)
diff --git a/drivers/staging/rtl8188eu/include/odm_HWConfig.h b/drivers/staging/rtl8188eu/include/odm_HWConfig.h
index 1de4e6399435..62a00498e473 100644
--- a/drivers/staging/rtl8188eu/include/odm_HWConfig.h
+++ b/drivers/staging/rtl8188eu/include/odm_HWConfig.h
@@ -120,7 +120,4 @@ void ODM_MacStatusQuery(struct odm_dm_struct *pDM_Odm,
 			bool	bPacketToSelf,
 			bool	bPacketBeacon);
 
-enum HAL_STATUS ODM_ConfigBBWithHeaderFile(struct odm_dm_struct *pDM_Odm,
-					   enum odm_bb_config_type ConfigType);
-
 #endif
diff --git a/drivers/staging/rtl8188eu/include/osdep_intf.h b/drivers/staging/rtl8188eu/include/osdep_intf.h
index efa786887962..1521744d626c 100644
--- a/drivers/staging/rtl8188eu/include/osdep_intf.h
+++ b/drivers/staging/rtl8188eu/include/osdep_intf.h
@@ -31,7 +31,6 @@ u8 rtw_init_drv_sw(struct adapter *padapter);
 u8 rtw_free_drv_sw(struct adapter *padapter);
 u8 rtw_reset_drv_sw(struct adapter *padapter);
 
-u32 rtw_start_drv_threads(struct adapter *padapter);
 void rtw_stop_drv_threads (struct adapter *padapter);
 void rtw_cancel_all_timer(struct adapter *padapter);
 
@@ -40,7 +39,6 @@ int rtw_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 int rtw_init_netdev_name(struct net_device *pnetdev, const char *ifname);
 struct net_device *rtw_init_netdev(struct adapter *padapter);
 u16 rtw_recv_select_queue(struct sk_buff *skb);
-void rtw_proc_init_one(struct net_device *dev);
 void rtw_proc_remove_one(struct net_device *dev);
 
 int pm_netdev_open(struct net_device *pnetdev, u8 bnormal);
diff --git a/drivers/staging/rtl8188eu/include/osdep_service.h b/drivers/staging/rtl8188eu/include/osdep_service.h
index 515e949629e2..00472e0c00a0 100644
--- a/drivers/staging/rtl8188eu/include/osdep_service.h
+++ b/drivers/staging/rtl8188eu/include/osdep_service.h
@@ -157,8 +157,6 @@ void rtw_free_netdev(struct net_device *netdev);
 #define FUNC_ADPT_FMT "%s(%s)"
 #define FUNC_ADPT_ARG(adapter) __func__, adapter->pnetdev->name
 
-#define rtw_signal_process(pid, sig) kill_pid(find_vpid((pid)), (sig), 1)
-
 u64 rtw_modular64(u64 x, u64 y);
 
 /* Macros for handling unaligned memory accesses */
diff --git a/drivers/staging/rtl8188eu/include/recv_osdep.h b/drivers/staging/rtl8188eu/include/recv_osdep.h
index 5aabd3984e58..0809963ce6aa 100644
--- a/drivers/staging/rtl8188eu/include/recv_osdep.h
+++ b/drivers/staging/rtl8188eu/include/recv_osdep.h
@@ -44,8 +44,5 @@ int rtw_os_recv_resource_alloc(struct adapter *adapt,
 int rtw_os_recvbuf_resource_alloc(struct adapter *adapt, struct recv_buf *buf);
 
 void rtw_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl);
-int _netdev_open(struct net_device *pnetdev);
-int netdev_open(struct net_device *pnetdev);
-int netdev_close(struct net_device *pnetdev);
 
 #endif /*  */
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_hal.h b/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
index b8c42eed98c4..7d8e022925e0 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
@@ -159,7 +159,7 @@ struct txpowerinfo24g {
 /*  | 1byte|----8bytes----|1byte|--5bytes--| */
 /*  |         |            Reserved(14bytes)	      | */
 
-/*  PG data exclude header, dummy 6 bytes frome CP test and reserved 1byte. */
+/*  PG data exclude header, dummy 6 bytes from CP test and reserved 1byte. */
 #define EFUSE_OOB_PROTECT_BYTES			15
 
 #define		HWSET_MAX_SIZE_88E		512
@@ -177,7 +177,7 @@ struct txpowerinfo24g {
 /*  9bytes + 1byt + 5bytes and pre 1byte. */
 /*  For worst case: */
 /*  | 2byte|----8bytes----|1byte|--7bytes--| 92D */
-/*  PG data exclude header, dummy 7 bytes frome CP test and reserved 1byte. */
+/*  PG data exclude header, dummy 7 bytes from CP test and reserved 1byte. */
 #define		EFUSE_OOB_PROTECT_BYTES_88E	18
 #define		EFUSE_PROTECT_BYTES_BANK_88E	16
 
@@ -188,14 +188,6 @@ struct txpowerinfo24g {
 
 #define EFUSE_PROTECT_BYTES_BANK	16
 
-/*  For RTL8723 WiFi/BT/GPS multi-function configuration. */
-enum rt_multi_func {
-	RT_MULTI_FUNC_NONE = 0x00,
-	RT_MULTI_FUNC_WIFI = 0x01,
-	RT_MULTI_FUNC_BT = 0x02,
-	RT_MULTI_FUNC_GPS = 0x04,
-};
-
 /*  For RTL8723 regulator mode. */
 enum rt_regulator_mode {
 	RT_SWITCHING_REGULATOR = 0,
@@ -378,11 +370,6 @@ struct hal_data_8188e {
 	((struct hal_data_8188e *)((__pAdapter)->HalData))
 #define GET_RF_TYPE(priv)		(GET_HAL_DATA(priv)->rf_type)
 
-#define INCLUDE_MULTI_FUNC_BT(_Adapter)				\
-	(GET_HAL_DATA(_Adapter)->MultiFunc & RT_MULTI_FUNC_BT)
-#define INCLUDE_MULTI_FUNC_GPS(_Adapter)			\
-	(GET_HAL_DATA(_Adapter)->MultiFunc & RT_MULTI_FUNC_GPS)
-
 /*  rtl8188e_hal_init.c */
 void _8051Reset88E(struct adapter *padapter);
 void rtl8188e_InitializeFirmwareVars(struct adapter *padapter);
diff --git a/drivers/staging/rtl8188eu/include/rtw_ap.h b/drivers/staging/rtl8188eu/include/rtw_ap.h
index 923340159798..6128ccce91ba 100644
--- a/drivers/staging/rtl8188eu/include/rtw_ap.h
+++ b/drivers/staging/rtl8188eu/include/rtw_ap.h
@@ -47,7 +47,6 @@ void rtw_set_macaddr_acl(struct adapter *padapter, int mode);
 int rtw_acl_add_sta(struct adapter *padapter, u8 *addr);
 int rtw_acl_remove_sta(struct adapter *padapter, u8 *addr);
 
-#ifdef CONFIG_88EU_AP_MODE
 void associated_clients_update(struct adapter *padapter, u8 updated);
 void bss_cap_update_on_sta_join(struct adapter *padapter, struct sta_info *psta);
 u8 bss_cap_update_on_sta_leave(struct adapter *padapter, struct sta_info *psta);
@@ -59,7 +58,6 @@ int rtw_sta_flush(struct adapter *padapter);
 int rtw_ap_inform_ch_switch(struct adapter *padapter, u8 new_ch, u8 ch_offset);
 void start_ap_mode(struct adapter *padapter);
 void stop_ap_mode(struct adapter *padapter);
-#endif
 #endif /* end of CONFIG_88EU_AP_MODE */
 
 #endif
diff --git a/drivers/staging/rtl8188eu/include/rtw_ioctl.h b/drivers/staging/rtl8188eu/include/rtw_ioctl.h
index f3aa924f2029..ee2cb54a7552 100644
--- a/drivers/staging/rtl8188eu/include/rtw_ioctl.h
+++ b/drivers/staging/rtl8188eu/include/rtw_ioctl.h
@@ -117,6 +117,4 @@ int drv_set_info(struct  net_device *MiniportAdapterContext,
 		 u32 informationbufferlength, u32 *bytesread,
 		 u32 *bytesneeded);
 
-extern int ui_pid[3];
-
 #endif /*  #ifndef __INC_CEINFO_ */
diff --git a/drivers/staging/rtl8188eu/include/rtw_led.h b/drivers/staging/rtl8188eu/include/rtw_led.h
index 7a5303d50d49..f2054ef70358 100644
--- a/drivers/staging/rtl8188eu/include/rtw_led.h
+++ b/drivers/staging/rtl8188eu/include/rtw_led.h
@@ -30,7 +30,7 @@ enum LED_CTL_MODE {
 	LED_CTL_LINK,
 	LED_CTL_NO_LINK,
 	LED_CTL_TX,
-	LED_CTL_RX ,
+	LED_CTL_RX,
 	LED_CTL_SITE_SURVEY,
 	LED_CTL_POWER_OFF,
 	LED_CTL_START_TO_LINK,
diff --git a/drivers/staging/rtl8188eu/include/rtw_mlme.h b/drivers/staging/rtl8188eu/include/rtw_mlme.h
index 3f7d1e631ef9..8c7e8a36aa13 100644
--- a/drivers/staging/rtl8188eu/include/rtw_mlme.h
+++ b/drivers/staging/rtl8188eu/include/rtw_mlme.h
@@ -65,7 +65,7 @@
 #define _FW_UNDER_SURVEY	WIFI_SITE_MONITOR
 
 enum dot11AuthAlgrthmNum {
-	dot11AuthAlgrthm_Open = 0,
+	dot11AuthAlgrthm_Open = 0, /* open system */
 	dot11AuthAlgrthm_Shared,
 	dot11AuthAlgrthm_8021X,
 	dot11AuthAlgrthm_Auto,
diff --git a/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h b/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
index 2bebf46b053a..2417809f3aef 100644
--- a/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8188eu/include/rtw_mlme_ext.h
@@ -191,6 +191,14 @@ struct rt_channel_plan_map {
 	unsigned char	Index2G;
 };
 
+static const struct {
+	int channel_plan;
+	char *name;
+} channel_table[] = { { RT_CHANNEL_DOMAIN_FCC, "US" },
+	{ RT_CHANNEL_DOMAIN_ETSI, "EU" },
+	{ RT_CHANNEL_DOMAIN_MKK, "JP" },
+	{ RT_CHANNEL_DOMAIN_CHINA, "CN"} };
+
 enum Associated_AP {
 	atherosAP	= 0,
 	broadcomAP	= 1,
@@ -489,9 +497,6 @@ void CAM_empty_entry(struct adapter *Adapter, u8 ucIndex);
 int allocate_fw_sta_entry(struct adapter *padapter);
 void flush_all_cam_entry(struct adapter *padapter);
 
-void site_survey(struct adapter *padapter);
-u8 collect_bss_info(struct adapter *padapter, struct recv_frame *precv_frame,
-		    struct wlan_bssid_ex *bssid);
 void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *src,
 		    struct adapter *adapter, bool update_ie);
 
@@ -533,9 +538,6 @@ unsigned int update_MSC_rate(struct HT_caps_element *pHT_caps);
 void Update_RA_Entry(struct adapter *padapter, u32 mac_id);
 void set_sta_rate(struct adapter *padapter, struct sta_info *psta);
 
-unsigned int receive_disconnect(struct adapter *padapter,
-				unsigned char *macaddr, unsigned short reason);
-
 unsigned char get_highest_rate_idx(u32 mask);
 int support_short_GI(struct adapter *padapter, struct HT_caps_element *caps);
 unsigned int is_ap_in_tkip(struct adapter *padapter);
@@ -558,86 +560,18 @@ unsigned int setup_beacon_frame(struct adapter *padapter,
 void update_mgnt_tx_rate(struct adapter *padapter, u8 rate);
 void update_mgntframe_attrib(struct adapter *padapter,
 			     struct pkt_attrib *pattrib);
-void dump_mgntframe(struct adapter *padapter, struct xmit_frame *pmgntframe);
-s32 dump_mgntframe_and_wait(struct adapter *padapter,
-			    struct xmit_frame *pmgntframe, int timeout_ms);
-s32 dump_mgntframe_and_wait_ack(struct adapter *padapter,
-				struct xmit_frame *pmgntframe);
-
-void issue_beacon(struct adapter *padapter, int timeout_ms);
-void issue_probersp(struct adapter *padapter, unsigned char *da,
-		    u8 is_valid_p2p_probereq);
-void issue_assocreq(struct adapter *padapter);
-void issue_asocrsp(struct adapter *padapter, unsigned short status,
-		   struct sta_info *pstat, int pkt_type);
-void issue_auth(struct adapter *padapter, struct sta_info *psta,
-		unsigned short status);
-void issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid,
-		    u8 *da);
-s32 issue_probereq_ex(struct adapter *adapter, struct ndis_802_11_ssid *pssid,
-		      u8 *da, int try_cnt, int wait_ms);
+
 int issue_nulldata(struct adapter *padapter, unsigned char *da,
 		   unsigned int power_mode, int try_cnt, int wait_ms);
 int issue_qos_nulldata(struct adapter *padapter, unsigned char *da,
 		       u16 tid, int try_cnt, int wait_ms);
 int issue_deauth(struct adapter *padapter, unsigned char *da,
 		 unsigned short reason);
-int issue_deauth_ex(struct adapter *padapter, u8 *da, unsigned short reason,
-		    int try_cnt, int wait_ms);
 void issue_action_spct_ch_switch(struct adapter *padapter, u8 *ra, u8 new_ch,
 				 u8 ch_offset);
-void issue_action_BA(struct adapter *padapter, unsigned char *raddr,
-		     unsigned char action, unsigned short status);
 unsigned int send_delba(struct adapter *padapter, u8 initiator, u8 *addr);
 unsigned int send_beacon(struct adapter *padapter);
 
-void start_clnt_assoc(struct adapter *padapter);
-void start_clnt_auth(struct adapter *padapter);
-void start_clnt_join(struct adapter *padapter);
-void start_create_ibss(struct adapter *padapter);
-
-unsigned int OnAssocReq(struct adapter *padapter,
-			struct recv_frame *precv_frame);
-unsigned int OnAssocRsp(struct adapter *padapter,
-			struct recv_frame *precv_frame);
-unsigned int OnProbeReq(struct adapter *padapter,
-			struct recv_frame *precv_frame);
-unsigned int OnProbeRsp(struct adapter *padapter,
-			struct recv_frame *precv_frame);
-unsigned int DoReserved(struct adapter *padapter,
-			struct recv_frame *precv_frame);
-unsigned int OnBeacon(struct adapter *padapter,
-		      struct recv_frame *precv_frame);
-unsigned int OnAtim(struct adapter *padapter,
-		    struct recv_frame *precv_frame);
-unsigned int OnDisassoc(struct adapter *padapter,
-			struct recv_frame *precv_frame);
-unsigned int OnAuth(struct adapter *padapter,
-		    struct recv_frame *precv_frame);
-unsigned int OnAuthClient(struct adapter *padapter,
-			  struct recv_frame *precv_frame);
-unsigned int OnDeAuth(struct adapter *padapter,
-		      struct recv_frame *precv_frame);
-unsigned int OnAction(struct adapter *padapter,
-		      struct recv_frame *precv_frame);
-
-unsigned int on_action_spct(struct adapter *padapter,
-			    struct recv_frame *precv_frame);
-unsigned int OnAction_qos(struct adapter *padapter,
-			  struct recv_frame *precv_frame);
-unsigned int OnAction_dls(struct adapter *padapter,
-			  struct recv_frame *precv_frame);
-unsigned int OnAction_back(struct adapter *padapter,
-			   struct recv_frame *precv_frame);
-unsigned int on_action_public(struct adapter *padapter,
-			      struct recv_frame *precv_frame);
-unsigned int OnAction_ht(struct adapter *padapter,
-			 struct recv_frame *precv_frame);
-unsigned int OnAction_wmm(struct adapter *padapter,
-			  struct recv_frame *precv_frame);
-unsigned int OnAction_p2p(struct adapter *padapter,
-			  struct recv_frame *precv_frame);
-
 void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res);
 void mlmeext_sta_del_event_callback(struct adapter *padapter);
 void mlmeext_sta_add_event_callback(struct adapter *padapter,
@@ -751,7 +685,7 @@ enum rtw_c2h_event {
 	GEN_EVT_CODE(_Survey),	 /*8*/
 	GEN_EVT_CODE(_SurveyDone),	 /*9*/
 
-	GEN_EVT_CODE(_JoinBss) , /*10*/
+	GEN_EVT_CODE(_JoinBss), /*10*/
 	GEN_EVT_CODE(_AddSTA),
 	GEN_EVT_CODE(_DelSTA),
 	GEN_EVT_CODE(_AtimDone),
diff --git a/drivers/staging/rtl8188eu/include/rtw_security.h b/drivers/staging/rtl8188eu/include/rtw_security.h
index e9723a72af5e..abe7e21e6e20 100644
--- a/drivers/staging/rtl8188eu/include/rtw_security.h
+++ b/drivers/staging/rtl8188eu/include/rtw_security.h
@@ -328,7 +328,7 @@ static const unsigned long K[64] = {
 #define RORc(x, y) \
 	(((((unsigned long)(x) & 0xFFFFFFFFUL) >> (unsigned long)((y)&31)) | \
 	 ((unsigned long)(x) << (unsigned long)(32-((y)&31)))) & 0xFFFFFFFFUL)
-#define Ch(x, y , z)       (z ^ (x & (y ^ z)))
+#define Ch(x, y, z)       (z ^ (x & (y ^ z)))
 #define Maj(x, y, z)      (((x | y) & z) | (x & y))
 #define S(x, n)         RORc((x), (n))
 #define R(x, n)         (((x)&0xFFFFFFFFUL)>>(n))
diff --git a/drivers/staging/rtl8188eu/include/wifi.h b/drivers/staging/rtl8188eu/include/wifi.h
index a89275e0e0e0..a08a2e045e59 100644
--- a/drivers/staging/rtl8188eu/include/wifi.h
+++ b/drivers/staging/rtl8188eu/include/wifi.h
@@ -130,35 +130,6 @@ enum WIFI_REASON_CODE	{
 	_RSON_TDLS_TEAR_UN_RSN_		= 26,
 };
 
-/* Reason codes (IEEE 802.11-2007, 7.3.1.7, Table 7-22)
-
-#define WLAN_REASON_UNSPECIFIED 1
-#define WLAN_REASON_PREV_AUTH_NOT_VALID 2
-#define WLAN_REASON_DEAUTH_LEAVING 3
-#define WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY 4
-#define WLAN_REASON_DISASSOC_AP_BUSY 5
-#define WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA 6
-#define WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA 7
-#define WLAN_REASON_DISASSOC_STA_HAS_LEFT 8
-#define WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH 9 */
-/* IEEE 802.11h */
-#define WLAN_REASON_PWR_CAPABILITY_NOT_VALID 10
-#define WLAN_REASON_SUPPORTED_CHANNEL_NOT_VALID 11
-
-/* IEEE 802.11i
-#define WLAN_REASON_INVALID_IE 13
-#define WLAN_REASON_MICHAEL_MIC_FAILURE 14
-#define WLAN_REASON_4WAY_HANDSHAKE_TIMEOUT 15
-#define WLAN_REASON_GROUP_KEY_UPDATE_TIMEOUT 16
-#define WLAN_REASON_IE_IN_4WAY_DIFFERS 17
-#define WLAN_REASON_GROUP_CIPHER_NOT_VALID 18
-#define WLAN_REASON_PAIRWISE_CIPHER_NOT_VALID 19
-#define WLAN_REASON_AKMP_NOT_VALID 20
-#define WLAN_REASON_UNSUPPORTED_RSN_IE_VERSION 21
-#define WLAN_REASON_INVALID_RSN_IE_CAPAB 22
-#define WLAN_REASON_IEEE_802_1X_AUTH_FAILED 23
-#define WLAN_REASON_CIPHER_SUITE_REJECTED 24 */
-
 enum WIFI_STATUS_CODE {
 	_STATS_SUCCESSFUL_		= 0,
 	_STATS_FAILURE_			= 1,
@@ -173,54 +144,6 @@ enum WIFI_STATUS_CODE {
 	_STATS_RATE_FAIL_		= 18,
 };
 
-/* Status codes (IEEE 802.11-2007, 7.3.1.9, Table 7-23)
-#define WLAN_STATUS_SUCCESS 0
-#define WLAN_STATUS_UNSPECIFIED_FAILURE 1
-#define WLAN_STATUS_CAPS_UNSUPPORTED 10
-#define WLAN_STATUS_REASSOC_NO_ASSOC 11
-#define WLAN_STATUS_ASSOC_DENIED_UNSPEC 12
-#define WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG 13
-#define WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION 14
-#define WLAN_STATUS_CHALLENGE_FAIL 15
-#define WLAN_STATUS_AUTH_TIMEOUT 16
-#define WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA 17
-#define WLAN_STATUS_ASSOC_DENIED_RATES 18 */
-
-/* entended */
-/* IEEE 802.11b */
-#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
-#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
-#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
-/* IEEE 802.11h */
-#define WLAN_STATUS_SPEC_MGMT_REQUIRED 22
-#define WLAN_STATUS_PWR_CAPABILITY_NOT_VALID 23
-#define WLAN_STATUS_SUPPORTED_CHANNEL_NOT_VALID 24
-/* IEEE 802.11g */
-#define WLAN_STATUS_ASSOC_DENIED_NO_SHORT_SLOT_TIME 25
-#define WLAN_STATUS_ASSOC_DENIED_NO_ER_PBCC 26
-#define WLAN_STATUS_ASSOC_DENIED_NO_DSSS_OFDM 27
-/* IEEE 802.11w */
-#define WLAN_STATUS_ASSOC_REJECTED_TEMPORARILY 30
-#define WLAN_STATUS_ROBUST_MGMT_FRAME_POLICY_VIOLATION 31
-/* IEEE 802.11i */
-#define WLAN_STATUS_INVALID_IE 40
-#define WLAN_STATUS_GROUP_CIPHER_NOT_VALID 41
-#define WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID 42
-#define WLAN_STATUS_AKMP_NOT_VALID 43
-#define WLAN_STATUS_UNSUPPORTED_RSN_IE_VERSION 44
-#define WLAN_STATUS_INVALID_RSN_IE_CAPAB 45
-#define WLAN_STATUS_CIPHER_REJECTED_PER_POLICY 46
-#define WLAN_STATUS_TS_NOT_CREATED 47
-#define WLAN_STATUS_DIRECT_LINK_NOT_ALLOWED 48
-#define WLAN_STATUS_DEST_STA_NOT_PRESENT 49
-#define WLAN_STATUS_DEST_STA_NOT_QOS_STA 50
-#define WLAN_STATUS_ASSOC_DENIED_LISTEN_INT_TOO_LARGE 51
-/* IEEE 802.11r */
-#define WLAN_STATUS_INVALID_FT_ACTION_FRAME_COUNT 52
-#define WLAN_STATUS_INVALID_PMKID 53
-#define WLAN_STATUS_INVALID_MDIE 54
-#define WLAN_STATUS_INVALID_FTIE 55
-
 enum WIFI_REG_DOMAIN {
 	DOMAIN_FCC	= 1,
 	DOMAIN_IC	= 2,
diff --git a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
index 96c1c2d4a112..38dba1435c1e 100644
--- a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
@@ -19,6 +19,8 @@
  ******************************************************************************/
 #define _IOCTL_LINUX_C_
 
+#include <linux/ieee80211.h>
+
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <wlan_bssdef.h>
@@ -1625,7 +1627,7 @@ static int rtw_wx_set_enc(struct net_device *dev,
 		padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
 		padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
 		padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
-		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
 		authmode = Ndis802_11AuthModeOpen;
 		padapter->securitypriv.ndisauthtype = authmode;
 
@@ -1664,7 +1666,7 @@ static int rtw_wx_set_enc(struct net_device *dev,
 		DBG_88E("rtw_wx_set_enc():erq->flags = 0x%x\n", erq->flags);
 
 		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;/* Ndis802_11EncryptionDisabled; */
-		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
 		padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
 		padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
 		authmode = Ndis802_11AuthModeOpen;
@@ -1855,7 +1857,7 @@ static int rtw_wx_set_auth(struct net_device *dev,
 			padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled;
 			padapter->securitypriv.dot11PrivacyAlgrthm = _NO_PRIVACY_;
 			padapter->securitypriv.dot118021XGrpPrivacy = _NO_PRIVACY_;
-			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
 			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
 		}
 
diff --git a/drivers/staging/rtl8188eu/os_dep/mlme_linux.c b/drivers/staging/rtl8188eu/os_dep/mlme_linux.c
index baff1e2661d5..218adaa574b5 100644
--- a/drivers/staging/rtl8188eu/os_dep/mlme_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/mlme_linux.c
@@ -41,8 +41,6 @@ void rtw_os_indicate_connect(struct adapter *adapter)
 {
 	rtw_indicate_wx_assoc_event(adapter);
 	netif_carrier_on(adapter->pnetdev);
-	if (adapter->pid[2] != 0)
-		rtw_signal_process(adapter->pid[2], SIGALRM);
 }
 
 void rtw_os_indicate_scan_done(struct adapter *padapter, bool aborted)
@@ -82,7 +80,7 @@ void rtw_reset_securitypriv(struct adapter *adapter)
 		/* reset values in securitypriv */
 		struct security_priv *psec_priv = &adapter->securitypriv;
 
-		psec_priv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;  /* open system */
+		psec_priv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
 		psec_priv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
 		psec_priv->dot11PrivacyKeyIndex = 0;
 		psec_priv->dot118021XGrpPrivacy = _NO_PRIVACY_;
diff --git a/drivers/staging/rtl8188eu/os_dep/os_intfs.c b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
index 750c87b46365..a14e79f31abf 100644
--- a/drivers/staging/rtl8188eu/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
@@ -41,7 +41,8 @@ MODULE_VERSION(DRIVERVERSION);
 static int rtw_chip_version;
 static int rtw_rfintfs = HWPI;
 static int rtw_lbkmode;/* RTL8712_AIR_TRX; */
-static int rtw_network_mode = Ndis802_11IBSS;/* Ndis802_11Infrastructure; infra, ad-hoc, auto */
+/* Ndis802_11Infrastructure; infra, ad-hoc, auto */
+static int rtw_network_mode = Ndis802_11IBSS;
 static int rtw_channel = 1;/* ad-hoc support requirement */
 static int rtw_wireless_mode = WIRELESS_11BG_24N;
 static int rtw_vrtl_carrier_sense = AUTO_VCS;
@@ -81,21 +82,37 @@ static int rtw_uapsd_acvi_en;
 static int rtw_uapsd_acvo_en;
 
 static int rtw_ht_enable = 1;
-static int rtw_cbw40_enable = 3; /*  0 :disable, bit(0): enable 2.4g, bit(1): enable 5g */
+/* 0 :disable, bit(0): enable 2.4g, bit(1): enable 5g */
+static int rtw_cbw40_enable = 3;
 static int rtw_ampdu_enable = 1;/* for enable tx_ampdu */
-static int rtw_rx_stbc = 1;/*  0: disable, bit(0):enable 2.4g, bit(1):enable 5g, default is set to enable 2.4GHZ for IOT issue with bufflao's AP at 5GHZ */
+
+/* 0: disable
+ * bit(0):enable 2.4g
+ * bit(1):enable 5g
+ * default is set to enable 2.4GHZ for IOT issue with bufflao's AP at 5GHZ
+ */
+static int rtw_rx_stbc = 1;
 static int rtw_ampdu_amsdu;/*  0: disabled, 1:enabled, 2:auto */
 
-static int rtw_lowrate_two_xmit = 1;/* Use 2 path Tx to transmit MCS0~7 and legacy mode */
+/* Use 2 path Tx to transmit MCS0~7 and legacy mode */
+static int rtw_lowrate_two_xmit = 1;
 
 static int rtw_rf_config = RF_819X_MAX_TYPE;  /* auto */
 static int rtw_low_power;
 static int rtw_wifi_spec;
 static int rtw_channel_plan = RT_CHANNEL_DOMAIN_MAX;
-static int rtw_AcceptAddbaReq = true;/*  0:Reject AP's Add BA req, 1:Accept AP's Add BA req. */
+/* 0:Reject AP's Add BA req, 1:Accept AP's Add BA req. */
+static int rtw_AcceptAddbaReq = true;
 
 static int rtw_antdiv_cfg = 2; /*  0:OFF , 1:ON, 2:decide by Efuse config */
-static int rtw_antdiv_type; /* 0:decide by efuse  1: for 88EE, 1Tx and 1RxCG are diversity.(2 Ant with SPDT), 2:  for 88EE, 1Tx and 2Rx are diversity.(2 Ant, Tx and RxCG are both on aux port, RxCS is on main port), 3: for 88EE, 1Tx and 1RxCG are fixed.(1Ant, Tx and RxCG are both on aux port) */
+
+/* 0: decide by efuse
+ * 1: for 88EE, 1Tx and 1RxCG are diversity (2 Ant with SPDT)
+ * 2: for 88EE, 1Tx and 2Rx are diversity (2 Ant, Tx and RxCG are both on aux
+ *    port, RxCS is on main port)
+ * 3: for 88EE, 1Tx and 1RxCG are fixed (1Ant, Tx and RxCG are both on aux port)
+ */
+static int rtw_antdiv_type;
 
 static int rtw_enusbss;/* 0:disable, 1:enable */
 
@@ -117,7 +134,8 @@ static char *if2name = "wlan%d";
 module_param(if2name, charp, 0644);
 MODULE_PARM_DESC(if2name, "The default name to allocate for second interface");
 
-char *rtw_initmac;  /*  temp mac address if users want to use instead of the mac address in Efuse */
+/* temp mac address if users want to use instead of the mac address in Efuse */
+char *rtw_initmac;
 
 module_param(rtw_initmac, charp, 0644);
 module_param(rtw_channel_plan, int, 0644);
@@ -167,17 +185,20 @@ MODULE_PARM_DESC(rtw_notch_filter, "0:Disable, 1:Enable, 2:Enable only for P2P")
 module_param_named(debug, rtw_debug, int, 0444);
 MODULE_PARM_DESC(debug, "Set debug level (1-9) (default 1)");
 
+static int netdev_open(struct net_device *pnetdev);
+static int netdev_close(struct net_device *pnetdev);
+
 /* dummy routines */
 void rtw_proc_remove_one(struct net_device *dev)
 {
 }
 
-void rtw_proc_init_one(struct net_device *dev)
+static void rtw_proc_init_one(struct net_device *dev)
 {
 }
 
 #if 0	/* TODO: Convert these to /sys */
-void rtw_proc_init_one(struct net_device *dev)
+static void rtw_proc_init_one(struct net_device *dev)
 {
 	struct proc_dir_entry *dir_dev = NULL;
 	struct proc_dir_entry *entry = NULL;
@@ -187,13 +208,16 @@ void rtw_proc_init_one(struct net_device *dev)
 	if (rtw_proc == NULL) {
 		memcpy(rtw_proc_name, DRV_NAME, sizeof(DRV_NAME));
 
-		rtw_proc = create_proc_entry(rtw_proc_name, S_IFDIR, init_net.proc_net);
+		rtw_proc = create_proc_entry(rtw_proc_name, S_IFDIR,
+					     init_net.proc_net);
 		if (rtw_proc == NULL) {
 			DBG_88E(KERN_ERR "Unable to create rtw_proc directory\n");
 			return;
 		}
 
-		entry = create_proc_read_entry("ver_info", S_IFREG | S_IRUGO, rtw_proc, proc_get_drv_version, dev);
+		entry = create_proc_read_entry("ver_info", S_IFREG | S_IRUGO,
+					       rtw_proc, proc_get_drv_version,
+					       dev);
 		if (!entry) {
 			pr_info("Unable to create_proc_read_entry!\n");
 			return;
@@ -206,11 +230,9 @@ void rtw_proc_init_one(struct net_device *dev)
 					  rtw_proc);
 		dir_dev = padapter->dir_dev;
 		if (dir_dev == NULL) {
-			if (rtw_proc_cnt == 0) {
-				if (rtw_proc) {
-					remove_proc_entry(rtw_proc_name, init_net.proc_net);
-					rtw_proc = NULL;
-				}
+			if (rtw_proc_cnt == 0 && rtw_proc) {
+				remove_proc_entry(rtw_proc_name, init_net.proc_net);
+				rtw_proc = NULL;
 			}
 
 			pr_info("Unable to create dir_dev directory\n");
@@ -360,15 +382,17 @@ void rtw_proc_init_one(struct net_device *dev)
 
 	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
 	if ((RF_1T2R == rf_type) || (RF_1T1R == rf_type)) {
-		entry = create_proc_read_entry("rf_reg_dump3", S_IFREG | S_IRUGO,
-					   dir_dev, proc_get_rf_reg_dump3, dev);
+		entry = create_proc_read_entry("rf_reg_dump3",
+					       S_IFREG | S_IRUGO, dir_dev,
+					       proc_get_rf_reg_dump3, dev);
 		if (!entry) {
 			pr_info("Unable to create_proc_read_entry!\n");
 			return;
 		}
 
-		entry = create_proc_read_entry("rf_reg_dump4", S_IFREG | S_IRUGO,
-					   dir_dev, proc_get_rf_reg_dump4, dev);
+		entry = create_proc_read_entry("rf_reg_dump4",
+					       S_IFREG | S_IRUGO, dir_dev,
+					       proc_get_rf_reg_dump4, dev);
 		if (!entry) {
 			pr_info("Unable to create_proc_read_entry!\n");
 			return;
@@ -510,11 +534,10 @@ void rtw_proc_remove_one(struct net_device *dev)
 }
 #endif
 
-static uint loadparam(struct adapter *padapter,  struct  net_device *pnetdev)
+static void loadparam(struct adapter *padapter, struct net_device *pnetdev)
 {
 	struct registry_priv  *registry_par = &padapter->registrypriv;
 
-
 	GlobalDebugLevel = rtw_debug;
 	registry_par->chip_version = (u8)rtw_chip_version;
 	registry_par->rfintfs = (u8)rtw_rfintfs;
@@ -569,8 +592,8 @@ static uint loadparam(struct adapter *padapter,  struct  net_device *pnetdev)
 	registry_par->bAcceptAddbaReq = (u8)rtw_AcceptAddbaReq;
 	registry_par->antdiv_cfg = (u8)rtw_antdiv_cfg;
 	registry_par->antdiv_type = (u8)rtw_antdiv_type;
-	registry_par->hwpdn_mode = (u8)rtw_hwpdn_mode;/* 0:disable, 1:enable, 2:by EFUSE config */
-	registry_par->hwpwrp_detect = (u8)rtw_hwpwrp_detect;/* 0:disable, 1:enable */
+	registry_par->hwpdn_mode = (u8)rtw_hwpdn_mode;
+	registry_par->hwpwrp_detect = (u8)rtw_hwpwrp_detect;
 	registry_par->hw_wps_pbc = (u8)rtw_hw_wps_pbc;
 
 	registry_par->max_roaming_times = (u8)rtw_max_roaming_times;
@@ -581,7 +604,6 @@ 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 _SUCCESS;
 }
 
 static int rtw_net_set_mac_address(struct net_device *pnetdev, void *p)
@@ -601,8 +623,8 @@ static struct net_device_stats *rtw_net_get_stats(struct net_device *pnetdev)
 	struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
 	struct recv_priv *precvpriv = &(padapter->recvpriv);
 
-	padapter->stats.tx_packets = pxmitpriv->tx_pkts;/* pxmitpriv->tx_pkts++; */
-	padapter->stats.rx_packets = precvpriv->rx_pkts;/* precvpriv->rx_pkts++; */
+	padapter->stats.tx_packets = pxmitpriv->tx_pkts;
+	padapter->stats.rx_packets = precvpriv->rx_pkts;
 	padapter->stats.tx_dropped = pxmitpriv->tx_drop;
 	padapter->stats.rx_dropped = precvpriv->rx_drop;
 	padapter->stats.tx_bytes = pxmitpriv->tx_bytes;
@@ -725,32 +747,33 @@ struct net_device *rtw_init_netdev(struct adapter *old_padapter)
 	pnetdev->watchdog_timeo = HZ*3; /* 3 second timeout */
 	pnetdev->wireless_handlers = (struct iw_handler_def *)&rtw_handlers_def;
 
-	/* step 2. */
 	loadparam(padapter, pnetdev);
 
 	return pnetdev;
 }
 
-u32 rtw_start_drv_threads(struct adapter *padapter)
+static int rtw_start_drv_threads(struct adapter *padapter)
 {
-	u32 _status = _SUCCESS;
+	int err = 0;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_start_drv_threads\n"));
 
-	padapter->cmdThread = kthread_run(rtw_cmd_thread, padapter, "RTW_CMD_THREAD");
+	padapter->cmdThread = kthread_run(rtw_cmd_thread, padapter,
+					  "RTW_CMD_THREAD");
 	if (IS_ERR(padapter->cmdThread))
-		_status = _FAIL;
+		err = PTR_ERR(padapter->cmdThread);
 	else
-		_rtw_down_sema(&padapter->cmdpriv.terminate_cmdthread_sema); /* wait for cmd_thread to run */
+		/* wait for cmd_thread to run */
+		_rtw_down_sema(&padapter->cmdpriv.terminate_cmdthread_sema);
 
-	return _status;
+	return err;
 }
 
 void rtw_stop_drv_threads(struct adapter *padapter)
 {
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_stop_drv_threads\n"));
 
-	/* Below is to termindate rtw_cmd_thread & event_thread... */
+	/* Below is to terminate rtw_cmd_thread & event_thread... */
 	up(&padapter->cmdpriv.cmd_queue_sema);
 	if (padapter->cmdThread)
 		_rtw_down_sema(&padapter->cmdpriv.terminate_cmdthread_sema);
@@ -781,7 +804,7 @@ static u8 rtw_init_default_value(struct adapter *padapter)
 	psecuritypriv->binstallGrpkey = _FAIL;
 	psecuritypriv->sw_encrypt = pregistrypriv->software_encrypt;
 	psecuritypriv->sw_decrypt = pregistrypriv->software_decrypt;
-	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open; /* open system */
+	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
 	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
 	psecuritypriv->dot11PrivacyKeyIndex = 0;
 	psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
@@ -931,7 +954,8 @@ u8 rtw_free_drv_sw(struct adapter *padapter)
 	rtw_free_mlme_priv(&padapter->mlmepriv);
 	_rtw_free_xmit_priv(&padapter->xmitpriv);
 
-	_rtw_free_sta_priv(&padapter->stapriv); /* will free bcmc_stainfo here */
+	/* will free bcmc_stainfo here */
+	_rtw_free_sta_priv(&padapter->stapriv);
 
 	_rtw_free_recv_priv(&padapter->recvpriv);
 
@@ -952,9 +976,10 @@ u8 rtw_free_drv_sw(struct adapter *padapter)
 	return _SUCCESS;
 }
 
-int _netdev_open(struct net_device *pnetdev)
+static int _netdev_open(struct net_device *pnetdev)
 {
 	uint status;
+	int err;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
 	struct pwrctrl_priv *pwrctrlpriv = &padapter->pwrctrlpriv;
 
@@ -978,8 +1003,8 @@ int _netdev_open(struct net_device *pnetdev)
 
 		pr_info("MAC Address = %pM\n", pnetdev->dev_addr);
 
-		status = rtw_start_drv_threads(padapter);
-		if (status == _FAIL) {
+		err = rtw_start_drv_threads(padapter);
+		if (err) {
 			pr_info("Initialize driver software resource Failed!\n");
 			goto netdev_open_error;
 		}
@@ -1023,7 +1048,7 @@ netdev_open_error:
 	return -1;
 }
 
-int netdev_open(struct net_device *pnetdev)
+static int netdev_open(struct net_device *pnetdev)
 {
 	int ret;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
@@ -1037,6 +1062,7 @@ int netdev_open(struct net_device *pnetdev)
 static int  ips_netdrv_open(struct adapter *padapter)
 {
 	int status = _SUCCESS;
+
 	padapter->net_closed = false;
 	DBG_88E("===> %s.........\n", __func__);
 
@@ -1069,6 +1095,7 @@ int rtw_ips_pwr_up(struct adapter *padapter)
 {
 	int result;
 	u32 start_time = jiffies;
+
 	DBG_88E("===>  rtw_ips_pwr_up..............\n");
 	rtw_reset_drv_sw(padapter);
 
@@ -1083,6 +1110,7 @@ int rtw_ips_pwr_up(struct adapter *padapter)
 void rtw_ips_pwr_down(struct adapter *padapter)
 {
 	u32 start_time = jiffies;
+
 	DBG_88E("===> rtw_ips_pwr_down...................\n");
 
 	padapter->net_closed = true;
@@ -1118,7 +1146,7 @@ int pm_netdev_open(struct net_device *pnetdev, u8 bnormal)
 	return status;
 }
 
-int netdev_close(struct net_device *pnetdev)
+static int netdev_close(struct net_device *pnetdev)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
 	struct hal_data_8188e *rtlhal = GET_HAL_DATA(padapter);
diff --git a/drivers/staging/rtl8188eu/os_dep/osdep_service.c b/drivers/staging/rtl8188eu/os_dep/osdep_service.c
index abcb3a8589ef..acb4eb120b76 100644
--- a/drivers/staging/rtl8188eu/os_dep/osdep_service.c
+++ b/drivers/staging/rtl8188eu/os_dep/osdep_service.c
@@ -134,7 +134,7 @@ void rtw_buf_free(u8 **buf, u32 *buf_len)
 
 void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len)
 {
-	u32 ori_len = 0, dup_len = 0;
+	u32 dup_len = 0;
 	u8 *ori = NULL;
 	u8 *dup = NULL;
 
@@ -153,7 +153,6 @@ void rtw_buf_update(u8 **buf, u32 *buf_len, u8 *src, u32 src_len)
 
 keep_ori:
 	ori = *buf;
-	ori_len = *buf_len;
 
 	/* replace buf with dup */
 	*buf_len = 0;
diff --git a/drivers/staging/rtl8188eu/os_dep/rtw_android.c b/drivers/staging/rtl8188eu/os_dep/rtw_android.c
index 99ce077007f4..5f3337c281ee 100644
--- a/drivers/staging/rtl8188eu/os_dep/rtw_android.c
+++ b/drivers/staging/rtl8188eu/os_dep/rtw_android.c
@@ -79,7 +79,7 @@ int rtw_android_cmdstr_to_num(char *cmdstr)
 {
 	int cmd_num;
 	for (cmd_num = 0; cmd_num < ANDROID_WIFI_CMD_MAX; cmd_num++)
-		if (0 == strncasecmp(cmdstr , android_wifi_cmd_str[cmd_num],
+		if (0 == strncasecmp(cmdstr, android_wifi_cmd_str[cmd_num],
 				  strlen(android_wifi_cmd_str[cmd_num])))
 			break;
 	return cmd_num;
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_intf.c b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
index ef3c73e38172..d0d4335b444c 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
@@ -32,8 +32,6 @@
 #include <usb_hal.h>
 #include <rtw_ioctl.h>
 
-int ui_pid[3] = {0, 0, 0};
-
 #define USB_VENDER_ID_REALTEK		0x0bda
 
 /* DID_USB_v916_20130116 */
@@ -330,11 +328,6 @@ static int rtw_resume_process(struct adapter *padapter)
 
 	_exit_pwrlock(&pwrpriv->lock);
 
-	if (padapter->pid[1] != 0) {
-		DBG_88E("pid[1]:%d\n", padapter->pid[1]);
-		rtw_signal_process(padapter->pid[1], SIGUSR2);
-	}
-
 	rtw_roaming(padapter, NULL);
 
 	ret = 0;
@@ -511,11 +504,6 @@ static int rtw_drv_init(struct usb_interface *pusb_intf, const struct usb_device
 		goto free_dvobj;
 	}
 
-	if (ui_pid[1] != 0) {
-		DBG_88E("ui_pid[1]:%d\n", ui_pid[1]);
-		rtw_signal_process(ui_pid[1], SIGUSR2);
-	}
-
 	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-871x_drv - drv_init, success!\n"));
 
 	status = _SUCCESS;
diff --git a/drivers/staging/rtl8192e/dot11d.h b/drivers/staging/rtl8192e/dot11d.h
index aad3394392fe..69e0f8f7e3f8 100644
--- a/drivers/staging/rtl8192e/dot11d.h
+++ b/drivers/staging/rtl8192e/dot11d.h
@@ -74,8 +74,8 @@ static inline void cpMacAddr(unsigned char *des, unsigned char *src)
 	(GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen > 0)
 
 #define IS_EQUAL_CIE_SRC(__pIeeeDev, __pTa)		\
-	 ether_addr_equal_unaligned(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, \
-	__pTa)
+	 ether_addr_equal_unaligned( \
+		GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa)
 #define UPDATE_CIE_SRC(__pIeeeDev, __pTa)		\
 	cpMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa)
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h b/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
index b7bb71fa9ecd..d0b08301b88f 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
@@ -144,19 +144,6 @@ enum rf_optype {
 	RF_OP_MAX
 };
 
-
-enum power_save_mode {
-	POWER_SAVE_MODE_ACTIVE,
-	POWER_SAVE_MODE_SAVE,
-};
-
-enum interface_select_8190pci {
-	INTF_SEL1_MINICARD		= 0,
-	INTF_SEL0_PCIE			= 1,
-	INTF_SEL2_RSV			= 2,
-	INTF_SEL3_RSV			= 3,
-};
-
 struct bb_reg_definition {
 	u32 rfintfs;
 	u32 rfintfi;
@@ -178,33 +165,6 @@ struct bb_reg_definition {
 	u32 rfLSSIReadBackPi;
 };
 
-struct tx_fwinfo {
-	u8			TxRate:7;
-	u8			CtsEnable:1;
-	u8			RtsRate:7;
-	u8			RtsEnable:1;
-	u8			TxHT:1;
-	u8			Short:1;
-	u8			TxBandwidth:1;
-	u8			TxSubCarrier:2;
-	u8			STBC:2;
-	u8			AllowAggregation:1;
-	u8			RtsHT:1;
-	u8			RtsShort:1;
-	u8			RtsBandwidth:1;
-	u8			RtsSubcarrier:2;
-	u8			RtsSTBC:2;
-	u8			EnableCPUDur:1;
-
-	u32			RxMF:2;
-	u32			RxAMD:3;
-	u32			Reserved1:3;
-	u32			TxAGCOffset:4;
-	u32			TxAGCSign:1;
-	u32			Tx_INFO_RSVD:6;
-	u32			PacketID:13;
-};
-
 struct tx_fwinfo_8190pci {
 	u8			TxRate:7;
 	u8			CtsEnable:1;
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
index 01d2201afc94..facc6f1f302b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
@@ -47,8 +47,8 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev,
 						0x0e, bMask12Bits, 0x021);
 
 			} else {
-				RT_TRACE(COMP_ERR,
-					 "PHY_SetRF8256Bandwidth(): unknown hardware version\n");
+				netdev_warn(dev, "%s(): Unknown HW version.\n",
+					    __func__);
 			}
 
 			break;
@@ -66,16 +66,15 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev,
 						 0x0e, bMask12Bits, 0x0e1);
 
 			} else {
-				RT_TRACE(COMP_ERR,
-					 "PHY_SetRF8256Bandwidth(): unknown hardware version\n");
+				netdev_warn(dev, "%s(): Unknown HW version.\n",
+					    __func__);
 			}
 
 
 			break;
 		default:
-			RT_TRACE(COMP_ERR,
-				 "PHY_SetRF8256Bandwidth(): unknown Bandwidth: %#X\n",
-				 Bandwidth);
+			netdev_err(dev, "%s(): Unknown bandwidth: %#X\n",
+				   __func__, Bandwidth);
 			break;
 
 		}
@@ -139,9 +138,8 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 		rtStatus = rtl8192_phy_checkBBAndRF(dev, HW90_BLOCK_RF,
 						(enum rf90_radio_path)eRFPath);
 		if (!rtStatus) {
-			RT_TRACE(COMP_ERR,
-				 "PHY_RF8256_Config():Check Radio[%d] Fail!!\n",
-				 eRFPath);
+			netdev_err(dev, "%s(): Failed to check RF Path %d.\n",
+				   __func__, eRFPath);
 			goto phy_RF8256_Config_ParaFile_Fail;
 		}
 
@@ -227,9 +225,9 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 		}
 
 		if (ret) {
-			RT_TRACE(COMP_ERR,
-				 "phy_RF8256_Config_ParaFile():Radio[%d] Fail!!",
-				 eRFPath);
+			netdev_err(dev,
+				   "%s(): Failed to initialize RF Path %d.\n",
+				   __func__, eRFPath);
 			goto phy_RF8256_Config_ParaFile_Fail;
 		}
 
@@ -239,7 +237,6 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 	return true;
 
 phy_RF8256_Config_ParaFile_Fail:
-	RT_TRACE(COMP_ERR, "PHY Initialization failed\n");
 	return false;
 }
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
index ecdd2e5c6bac..ebd08a16685e 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
@@ -88,293 +88,3 @@ bool cmpk_message_handle_tx(
 Failed:
 	return rt_status;
 }
-
-static	void
-cmpk_count_txstatistic(
-	struct net_device *dev,
-	struct cmpk_txfb *pstx_fb)
-{
-	struct r8192_priv *priv = rtllib_priv(dev);
-#ifdef ENABLE_PS
-	enum rt_rf_power_state rtState;
-
-	pAdapter->HalFunc.GetHwRegHandler(pAdapter, HW_VAR_RF_STATE,
-					  (pu1Byte)(&rtState));
-
-	if (rtState == eRfOff)
-		return;
-#endif
-
-	if (pstx_fb->tok) {
-		priv->stats.txfeedbackok++;
-		priv->stats.txoktotal++;
-		priv->stats.txokbytestotal += pstx_fb->pkt_length;
-		priv->stats.txokinperiod++;
-
-		if (pstx_fb->pkt_type == PACKET_MULTICAST) {
-			priv->stats.txmulticast++;
-			priv->stats.txbytesmulticast += pstx_fb->pkt_length;
-		} else if (pstx_fb->pkt_type == PACKET_BROADCAST) {
-			priv->stats.txbroadcast++;
-			priv->stats.txbytesbroadcast += pstx_fb->pkt_length;
-		} else {
-			priv->stats.txunicast++;
-			priv->stats.txbytesunicast += pstx_fb->pkt_length;
-		}
-	} else {
-		priv->stats.txfeedbackfail++;
-		priv->stats.txerrtotal++;
-		priv->stats.txerrbytestotal += pstx_fb->pkt_length;
-
-		if (pstx_fb->pkt_type == PACKET_MULTICAST)
-			priv->stats.txerrmulticast++;
-		else if (pstx_fb->pkt_type == PACKET_BROADCAST)
-			priv->stats.txerrbroadcast++;
-		else
-			priv->stats.txerrunicast++;
-	}
-
-	priv->stats.txretrycount += pstx_fb->retry_cnt;
-	priv->stats.txfeedbackretry += pstx_fb->retry_cnt;
-}
-
-static void cmpk_handle_tx_feedback(struct net_device *dev, u8 *pmsg)
-{
-	struct r8192_priv *priv = rtllib_priv(dev);
-	struct cmpk_txfb rx_tx_fb;
-
-	priv->stats.txfeedback++;
-
-
-	memcpy((u8 *)&rx_tx_fb, pmsg, sizeof(struct cmpk_txfb));
-	cmpk_count_txstatistic(dev, &rx_tx_fb);
-}
-
-static void cmdpkt_beacontimerinterrupt_819xusb(struct net_device *dev)
-{
-	struct r8192_priv *priv = rtllib_priv(dev);
-
-	if ((priv->rtllib->current_network.mode == IEEE_A)  ||
-	    (priv->rtllib->current_network.mode == IEEE_N_5G) ||
-	    ((priv->rtllib->current_network.mode == IEEE_N_24G)  &&
-	    (!priv->rtllib->pHTInfo->bCurSuppCCK)))
-		DMESG("send beacon frame  tx rate is 6Mbpm\n");
-	else
-		DMESG("send beacon frame  tx rate is 1Mbpm\n");
-}
-
-static void cmpk_handle_interrupt_status(struct net_device *dev, u8 *pmsg)
-{
-	struct cmpk_intr_sta rx_intr_status;
-	struct r8192_priv *priv = rtllib_priv(dev);
-
-	DMESG("---> cmpk_Handle_Interrupt_Status()\n");
-
-	rx_intr_status.length = pmsg[1];
-	if (rx_intr_status.length != (sizeof(struct cmpk_intr_sta) - 2)) {
-		DMESG("cmpk_Handle_Interrupt_Status: wrong length!\n");
-		return;
-	}
-
-	if (priv->rtllib->iw_mode == IW_MODE_ADHOC) {
-		rx_intr_status.interrupt_status = *((u32 *)(pmsg + 4));
-
-		DMESG("interrupt status = 0x%x\n",
-		      rx_intr_status.interrupt_status);
-
-		if (rx_intr_status.interrupt_status & ISR_TxBcnOk) {
-			priv->rtllib->bibsscoordinator = true;
-			priv->stats.txbeaconokint++;
-		} else if (rx_intr_status.interrupt_status & ISR_TxBcnErr) {
-			priv->rtllib->bibsscoordinator = false;
-			priv->stats.txbeaconerr++;
-		}
-
-		if (rx_intr_status.interrupt_status & ISR_BcnTimerIntr)
-			cmdpkt_beacontimerinterrupt_819xusb(dev);
-	}
-
-	DMESG("<---- cmpk_handle_interrupt_status()\n");
-
-}
-
-static	void cmpk_handle_query_config_rx(struct net_device *dev, u8 *pmsg)
-{
-	cmpk_query_cfg_t	rx_query_cfg;
-
-
-	rx_query_cfg.cfg_action = (pmsg[4] & 0x80000000)>>31;
-	rx_query_cfg.cfg_type = (pmsg[4] & 0x60) >> 5;
-	rx_query_cfg.cfg_size = (pmsg[4] & 0x18) >> 3;
-	rx_query_cfg.cfg_page = (pmsg[6] & 0x0F) >> 0;
-	rx_query_cfg.cfg_offset	 = pmsg[7];
-	rx_query_cfg.value = (pmsg[8] << 24) | (pmsg[9] << 16) |
-			     (pmsg[10] << 8) | (pmsg[11] << 0);
-	rx_query_cfg.mask = (pmsg[12] << 24) | (pmsg[13] << 16) |
-			    (pmsg[14] << 8) | (pmsg[15] << 0);
-
-}
-
-static void cmpk_count_tx_status(struct net_device *dev,
-				 struct cmpk_tx_status *pstx_status)
-{
-	struct r8192_priv *priv = rtllib_priv(dev);
-
-#ifdef ENABLE_PS
-
-	enum rt_rf_power_state rtstate;
-
-	pAdapter->HalFunc.GetHwRegHandler(pAdapter, HW_VAR_RF_STATE,
-					  (pu1Byte)(&rtState));
-
-	if (rtState == eRfOff)
-		return;
-#endif
-
-	priv->stats.txfeedbackok	+= pstx_status->txok;
-	priv->stats.txoktotal		+= pstx_status->txok;
-
-	priv->stats.txfeedbackfail	+= pstx_status->txfail;
-	priv->stats.txerrtotal		+= pstx_status->txfail;
-
-	priv->stats.txretrycount		+= pstx_status->txretry;
-	priv->stats.txfeedbackretry	+= pstx_status->txretry;
-
-
-	priv->stats.txmulticast	+= pstx_status->txmcok;
-	priv->stats.txbroadcast	+= pstx_status->txbcok;
-	priv->stats.txunicast		+= pstx_status->txucok;
-
-	priv->stats.txerrmulticast	+= pstx_status->txmcfail;
-	priv->stats.txerrbroadcast	+= pstx_status->txbcfail;
-	priv->stats.txerrunicast	+= pstx_status->txucfail;
-
-	priv->stats.txbytesmulticast	+= pstx_status->txmclength;
-	priv->stats.txbytesbroadcast	+= pstx_status->txbclength;
-	priv->stats.txbytesunicast		+= pstx_status->txuclength;
-
-	priv->stats.last_packet_rate		= pstx_status->rate;
-}
-
-static	void cmpk_handle_tx_status(struct net_device *dev, u8 *pmsg)
-{
-	struct cmpk_tx_status rx_tx_sts;
-
-	memcpy((void *)&rx_tx_sts, (void *)pmsg, sizeof(struct cmpk_tx_status));
-	cmpk_count_tx_status(dev, &rx_tx_sts);
-}
-
-static	void cmpk_handle_tx_rate_history(struct net_device *dev, u8 *pmsg)
-{
-	struct cmpk_tx_rahis *ptxrate;
-	u8 i, j;
-	u16				length = sizeof(struct cmpk_tx_rahis);
-	u32 *ptemp;
-	struct r8192_priv *priv = rtllib_priv(dev);
-
-#ifdef ENABLE_PS
-	pAdapter->HalFunc.GetHwRegHandler(pAdapter, HW_VAR_RF_STATE,
-					 (pu1Byte)(&rtState));
-
-	if (rtState == eRfOff)
-		return;
-#endif
-
-	ptemp = (u32 *)pmsg;
-
-	for (i = 0; i < (length / 4); i++) {
-		u16	 temp1, temp2;
-
-		temp1 = ptemp[i] & 0x0000FFFF;
-		temp2 = ptemp[i] >> 16;
-		ptemp[i] = (temp1 << 16) | temp2;
-	}
-
-	ptxrate = (struct cmpk_tx_rahis *)pmsg;
-
-	if (ptxrate == NULL)
-		return;
-
-	for (i = 0; i < 16; i++) {
-		if (i < 4)
-			priv->stats.txrate.cck[i] += ptxrate->cck[i];
-
-		if (i < 8)
-			priv->stats.txrate.ofdm[i] += ptxrate->ofdm[i];
-
-		for (j = 0; j < 4; j++)
-			priv->stats.txrate.ht_mcs[j][i] +=
-							 ptxrate->ht_mcs[j][i];
-	}
-}
-
-u32 cmpk_message_handle_rx(struct net_device *dev,
-			   struct rtllib_rx_stats *pstats)
-{
-	int			total_length;
-	u8			cmd_length, exe_cnt = 0;
-	u8			element_id;
-	u8			*pcmd_buff;
-
-	RT_TRACE(COMP_CMDPKT, "---->cmpk_message_handle_rx()\n");
-
-	if (pstats == NULL)
-		return 0;
-
-	total_length = pstats->Length;
-
-	pcmd_buff = pstats->virtual_address;
-
-	element_id = pcmd_buff[0];
-
-	while (total_length > 0 || exe_cnt++ > 100) {
-		element_id = pcmd_buff[0];
-
-		switch (element_id) {
-		case RX_TX_FEEDBACK:
-			RT_TRACE(COMP_CMDPKT,
-				 "---->cmpk_message_handle_rx():RX_TX_FEEDBACK\n");
-			cmpk_handle_tx_feedback(dev, pcmd_buff);
-			cmd_length = CMPK_RX_TX_FB_SIZE;
-			break;
-		case RX_INTERRUPT_STATUS:
-			RT_TRACE(COMP_CMDPKT,
-				 "---->cmpk_message_handle_rx():RX_INTERRUPT_STATUS\n");
-			cmpk_handle_interrupt_status(dev, pcmd_buff);
-			cmd_length = sizeof(struct cmpk_intr_sta);
-			break;
-		case BOTH_QUERY_CONFIG:
-			RT_TRACE(COMP_CMDPKT,
-				 "---->cmpk_message_handle_rx():BOTH_QUERY_CONFIG\n");
-			cmpk_handle_query_config_rx(dev, pcmd_buff);
-			cmd_length = CMPK_BOTH_QUERY_CONFIG_SIZE;
-			break;
-		case RX_TX_STATUS:
-			RT_TRACE(COMP_CMDPKT,
-				 "---->cmpk_message_handle_rx():RX_TX_STATUS\n");
-			cmpk_handle_tx_status(dev, pcmd_buff);
-			cmd_length = CMPK_RX_TX_STS_SIZE;
-			break;
-		case RX_TX_PER_PKT_FEEDBACK:
-			RT_TRACE(COMP_CMDPKT,
-				 "---->cmpk_message_handle_rx():RX_TX_PER_PKT_FEEDBACK\n");
-			cmd_length = CMPK_RX_TX_FB_SIZE;
-			break;
-		case RX_TX_RATE_HISTORY:
-			RT_TRACE(COMP_CMDPKT,
-				 "---->cmpk_message_handle_rx():RX_TX_HISTORY\n");
-			cmpk_handle_tx_rate_history(dev, pcmd_buff);
-			cmd_length = CMPK_TX_RAHIS_SIZE;
-			break;
-		default:
-
-			RT_TRACE(COMP_CMDPKT,
-				 "---->cmpk_message_handle_rx():unknown CMD Element\n");
-			return 1;
-		}
-
-		total_length -= cmd_length;
-		pcmd_buff    += cmd_length;
-	}
-	return	1;
-}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
index 2693682644df..f714d5100059 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
@@ -18,142 +18,8 @@
 ******************************************************************************/
 #ifndef R819XUSB_CMDPKT_H
 #define R819XUSB_CMDPKT_H
-#define	CMPK_RX_TX_FB_SIZE		sizeof(struct cmpk_txfb)
-#define CMPK_TX_SET_CONFIG_SIZE		sizeof(struct cmpk_set_cfg)
-#define CMPK_BOTH_QUERY_CONFIG_SIZE	sizeof(struct cmpk_set_cfg)
-#define CMPK_RX_TX_STS_SIZE		sizeof(struct cmpk_tx_status)
-#define CMPK_RX_DBG_MSG_SIZE		sizeof(struct cmpk_rx_dbginfo)
-#define	CMPK_TX_RAHIS_SIZE		sizeof(struct cmpk_tx_rahis)
 
-#define ISR_TxBcnOk			BIT27
-#define ISR_TxBcnErr			BIT26
-#define ISR_BcnTimerIntr		BIT13
-
-
-struct cmpk_txfb {
-	u8	element_id;
-	u8	length;
-	u8	TID:4;
-	u8	fail_reason:3;
-	u8	tok:1;
-	u8	reserve1:4;
-	u8	pkt_type:2;
-	u8	bandwidth:1;
-	u8	qos_pkt:1;
-
-	u8	reserve2;
-	u8	retry_cnt;
-	u16	pkt_id;
-
-	u16	seq_num;
-	u8	s_rate;
-	u8	f_rate;
-
-	u8	s_rts_rate;
-	u8	f_rts_rate;
-	u16	pkt_length;
-
-	u16	reserve3;
-	u16	duration;
-};
-
-struct cmpk_intr_sta {
-	u8	element_id;
-	u8	length;
-	u16	reserve;
-	u32	interrupt_status;
-};
-
-
-struct cmpk_set_cfg {
-	u8	element_id;
-	u8	length;
-	u16	reserve1;
-	u8	cfg_reserve1:3;
-	u8	cfg_size:2;
-	u8	cfg_type:2;
-	u8	cfg_action:1;
-	u8	cfg_reserve2;
-	u8	cfg_page:4;
-	u8	cfg_reserve3:4;
-	u8	cfg_offset;
-	u32	value;
-	u32	mask;
-};
-
-#define		cmpk_query_cfg_t	struct cmpk_set_cfg
-
-struct cmpk_tx_status {
-	u16	reserve1;
-	u8	length;
-	u8	element_id;
-
-	u16	txfail;
-	u16	txok;
-
-	u16	txmcok;
-	u16	txretry;
-
-	u16  txucok;
-	u16	txbcok;
-
-	u16	txbcfail;
-	u16	txmcfail;
-
-	u16	reserve2;
-	u16	txucfail;
-
-	u32	txmclength;
-	u32	txbclength;
-	u32	txuclength;
-
-	u16	reserve3_23;
-	u8	reserve3_1;
-	u8	rate;
-} __packed;
-
-struct cmpk_rx_dbginfo {
-	u16	reserve1;
-	u8	length;
-	u8	element_id;
-
-
-};
-
-struct cmpk_tx_rahis {
-	u8	element_id;
-	u8	length;
-	u16	reserved1;
-
-	u16	cck[4];
-
-	u16	ofdm[8];
-
-
-
-
-
-	u16	ht_mcs[4][16];
-
-} __packed;
-
-enum cmpk_element {
-	RX_TX_FEEDBACK = 0,
-	RX_INTERRUPT_STATUS		= 1,
-	TX_SET_CONFIG			= 2,
-	BOTH_QUERY_CONFIG		= 3,
-	RX_TX_STATUS			= 4,
-	RX_DBGINFO_FEEDBACK		= 5,
-	RX_TX_PER_PKT_FEEDBACK		= 6,
-	RX_TX_RATE_HISTORY		= 7,
-	RX_CMD_ELE_MAX
-};
-
-extern  u32 cmpk_message_handle_rx(struct net_device *dev,
-				   struct rtllib_rx_stats *pstats);
 extern bool cmpk_message_handle_tx(struct net_device *dev,
 				   u8 *codevirtualaddress, u32 packettype,
 				   u32 buffer_len);
-
-
 #endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
index 2869602436ef..f6661bbae7a8 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
@@ -30,7 +30,8 @@
 #include "rtl_dm.h"
 #include "rtl_wx.h"
 
-static int WDCAPARA_ADD[] = {EDCAPARA_BE, EDCAPARA_BK, EDCAPARA_VI, EDCAPARA_VO};
+static int WDCAPARA_ADD[] = {EDCAPARA_BE, EDCAPARA_BK, EDCAPARA_VI,
+			     EDCAPARA_VO};
 
 void rtl8192e_start_beacon(struct net_device *dev)
 {
@@ -187,22 +188,21 @@ void rtl8192e_SetHwReg(struct net_device *dev, u8 variable, u8 *val)
 		u8		u1bAIFS;
 		u32		u4bAcParam;
 		u8 mode = priv->rtllib->mode;
-		struct rtllib_qos_parameters *qos_parameters =
+		struct rtllib_qos_parameters *qop =
 			 &priv->rtllib->current_network.qos_data.parameters;
 
-		u1bAIFS = qos_parameters->aifs[pAcParam] *
+		u1bAIFS = qop->aifs[pAcParam] *
 			  ((mode&(IEEE_G|IEEE_N_24G)) ? 9 : 20) + aSifsTime;
 
 		dm_init_edca_turbo(dev);
 
-		u4bAcParam = (((le16_to_cpu(
-					qos_parameters->tx_op_limit[pAcParam])) <<
-			     AC_PARAM_TXOP_LIMIT_OFFSET) |
-			     ((le16_to_cpu(qos_parameters->cw_max[pAcParam])) <<
-			     AC_PARAM_ECW_MAX_OFFSET) |
-			     ((le16_to_cpu(qos_parameters->cw_min[pAcParam])) <<
-			     AC_PARAM_ECW_MIN_OFFSET) |
-			     (((u32)u1bAIFS) << AC_PARAM_AIFS_OFFSET));
+		u4bAcParam = (le16_to_cpu(qop->tx_op_limit[pAcParam]) <<
+			      AC_PARAM_TXOP_LIMIT_OFFSET) |
+				((le16_to_cpu(qop->cw_max[pAcParam])) <<
+				 AC_PARAM_ECW_MAX_OFFSET) |
+				((le16_to_cpu(qop->cw_min[pAcParam])) <<
+				 AC_PARAM_ECW_MIN_OFFSET) |
+				(((u32)u1bAIFS) << AC_PARAM_AIFS_OFFSET);
 
 		RT_TRACE(COMP_DBG, "%s():HW_VAR_AC_PARAM eACI:%x:%x\n",
 			 __func__, eACI, u4bAcParam);
@@ -316,19 +316,18 @@ void rtl8192e_SetHwReg(struct net_device *dev, u8 variable, u8 *val)
 static void rtl8192_read_eeprom_info(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
-
+	const u8 bMac_Tmp_Addr[ETH_ALEN] = {0x00, 0xe0, 0x4c, 0x00, 0x00, 0x01};
 	u8 tempval;
 	u8 ICVer8192, ICVer8256;
 	u16 i, usValue, IC_Version;
 	u16 EEPROMId;
-	u8 bMac_Tmp_Addr[6] = {0x00, 0xe0, 0x4c, 0x00, 0x00, 0x01};
 
 	RT_TRACE(COMP_INIT, "====> rtl8192_read_eeprom_info\n");
 
 	EEPROMId = eprom_read(dev, 0);
 	if (EEPROMId != RTL8190_EEPROM_ID) {
-		RT_TRACE(COMP_ERR, "EEPROM ID is invalid:%x, %x\n",
-			 EEPROMId, RTL8190_EEPROM_ID);
+		netdev_err(dev, "%s(): Invalid EEPROM ID: %x\n", __func__,
+			   EEPROMId);
 		priv->AutoloadFailFlag = true;
 	} else {
 		priv->AutoloadFailFlag = false;
@@ -383,7 +382,7 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 			*(u16 *)(&dev->dev_addr[i]) = usValue;
 		}
 	} else {
-		memcpy(dev->dev_addr, bMac_Tmp_Addr, 6);
+		ether_addr_copy(dev->dev_addr, bMac_Tmp_Addr);
 	}
 
 	RT_TRACE(COMP_INIT, "Permanent Address = %pM\n",
@@ -737,9 +736,8 @@ start:
 	else if (priv->pFirmware->firmware_status == FW_STATUS_5_READY)
 		ulRegRead |= CPU_GEN_FIRMWARE_RESET;
 	else
-		RT_TRACE(COMP_ERR,
-			 "ERROR in %s(): undefined firmware state(%d)\n",
-			 __func__,   priv->pFirmware->firmware_status);
+		netdev_err(dev, "%s(): undefined firmware state: %d.\n",
+			   __func__, priv->pFirmware->firmware_status);
 
 	write_nic_dword(dev, CPU_GEN, ulRegRead);
 
@@ -755,7 +753,7 @@ start:
 	RT_TRACE(COMP_INIT, "BB Config Start!\n");
 	rtStatus = rtl8192_BBConfig(dev);
 	if (!rtStatus) {
-		RT_TRACE(COMP_ERR, "BB Config failed\n");
+		netdev_warn(dev, "%s(): Failed to configure BB\n", __func__);
 		return rtStatus;
 	}
 	RT_TRACE(COMP_INIT, "BB Config Finished!\n");
@@ -769,8 +767,8 @@ start:
 		else if (priv->LoopbackMode == RTL819X_MAC_LOOPBACK)
 			ulRegRead |= CPU_CCK_LOOPBACK;
 		else
-			RT_TRACE(COMP_ERR,
-				 "Serious error: wrong loopback mode setting\n");
+			netdev_err(dev, "%s: Invalid loopback mode setting.\n",
+				   __func__);
 
 		write_nic_dword(dev, CPU_GEN, ulRegRead);
 
@@ -868,7 +866,7 @@ start:
 		RT_TRACE(COMP_INIT, "RF Config Started!\n");
 		rtStatus = rtl8192_phy_RFConfig(dev);
 		if (!rtStatus) {
-			RT_TRACE(COMP_ERR, "RF Config failed\n");
+			netdev_info(dev, "RF Config failed\n");
 			return rtStatus;
 		}
 		RT_TRACE(COMP_INIT, "RF Config Finished!\n");
@@ -918,8 +916,7 @@ start:
 			tmpRegC = rtl8192_QueryBBReg(dev,
 				  rOFDM0_XCTxIQImbalance, bMaskDWord);
 			for (i = 0; i < TxBBGainTableLength; i++) {
-				if (tmpRegA ==
-				    priv->txbbgain_table[i].txbbgain_value) {
+				if (tmpRegA == dm_tx_bb_gain[i]) {
 					priv->rfa_txpowertrackingindex = (u8)i;
 					priv->rfa_txpowertrackingindex_real =
 						 (u8)i;
@@ -933,7 +930,7 @@ start:
 				  rCCK0_TxFilter1, bMaskByte2);
 
 			for (i = 0; i < CCKTxBBGainTableLength; i++) {
-				if (TempCCk == priv->cck_txbbgain_table[i].ccktxbb_valuearray[0]) {
+				if (TempCCk == dm_cck_tx_bb_gain[i][0]) {
 					priv->CCKPresentAttentuation_20Mdefault = (u8)i;
 					break;
 				}
@@ -1139,7 +1136,8 @@ static u8 MRateToHwRate8190Pci(u8 rate)
 	return ret;
 }
 
-static u8 rtl8192_MapHwQueueToFirmwareQueue(u8 QueueID, u8 priority)
+static u8 rtl8192_MapHwQueueToFirmwareQueue(struct net_device *dev, u8 QueueID,
+					    u8 priority)
 {
 	u8 QueueSelect = 0x0;
 
@@ -1172,9 +1170,8 @@ static u8 rtl8192_MapHwQueueToFirmwareQueue(u8 QueueID, u8 priority)
 		QueueSelect = QSLT_HIGH;
 		break;
 	default:
-		RT_TRACE(COMP_ERR,
-			 "TransmitTCB(): Impossible Queue Selection: %d\n",
-			 QueueID);
+		netdev_warn(dev, "%s(): Impossible Queue Selection: %d\n",
+			    __func__, QueueID);
 		break;
 	}
 	return QueueSelect;
@@ -1198,7 +1195,7 @@ void  rtl8192_tx_fill_desc(struct net_device *dev, struct tx_desc *pdesc,
 						cb_desc);
 
 	if (pci_dma_mapping_error(priv->pdev, mapping))
-		RT_TRACE(COMP_ERR, "DMA Mapping error\n");
+		netdev_err(dev, "%s(): DMA Mapping error\n", __func__);
 	if (cb_desc->bAMPDUEnable) {
 		pTxFwInfo->AllowAggregation = 1;
 		pTxFwInfo->RxMF = cb_desc->ampdu_factor;
@@ -1274,7 +1271,7 @@ void  rtl8192_tx_fill_desc(struct net_device *dev, struct tx_desc *pdesc,
 
 	pdesc->PktId = 0x0;
 
-	pdesc->QueueSelect = rtl8192_MapHwQueueToFirmwareQueue(
+	pdesc->QueueSelect = rtl8192_MapHwQueueToFirmwareQueue(dev,
 						cb_desc->queue_index,
 						cb_desc->priority);
 	pdesc->TxFWInfoSize = sizeof(struct tx_fwinfo_8190pci);
@@ -1298,7 +1295,7 @@ void  rtl8192_tx_fill_cmd_desc(struct net_device *dev,
 			 PCI_DMA_TODEVICE);
 
 	if (pci_dma_mapping_error(priv->pdev, mapping))
-		RT_TRACE(COMP_ERR, "DMA Mapping error\n");
+		netdev_err(dev, "%s(): DMA Mapping error\n", __func__);
 	memset(entry, 0, 12);
 	entry->LINIP = cb_desc->bLastIniPkt;
 	entry->FirstSeg = 1;
@@ -2202,14 +2199,6 @@ rtl8192_InitializeVariables(struct net_device  *dev)
 	priv->ShortRetryLimit = 0x30;
 	priv->LongRetryLimit = 0x30;
 
-	priv->EarlyRxThreshold = 7;
-	priv->pwrGroupCnt = 0;
-
-	priv->bIgnoreSilentReset = false;
-	priv->enable_gpio0 = 0;
-
-	priv->TransmitConfig = 0;
-
 	priv->ReceiveConfig = RCR_ADD3	|
 		RCR_AMF | RCR_ADF |
 		RCR_AICV |
@@ -2224,9 +2213,6 @@ rtl8192_InitializeVariables(struct net_device  *dev)
 			    IMR_RDU | IMR_RXFOVW | IMR_TXFOVW |
 			    IMR_BcnInt | IMR_TBDOK | IMR_TBDER);
 
-
-	priv->MidHighPwrTHR_L1 = 0x3B;
-	priv->MidHighPwrTHR_L2 = 0x40;
 	priv->PwrDomainProtect = false;
 
 	priv->bfirst_after_down = false;
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index c465f8749acd..17d2a1540cc8 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -116,7 +116,7 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 	}
 
 	if (!(CPU_status&CPU_GEN_PUT_CODE_OK)) {
-		RT_TRACE(COMP_ERR, "Download Firmware: Put code fail!\n");
+		netdev_err(dev, "Firmware download failed.\n");
 		goto CPUCheckMainCodeOKAndTurnOnCPU_Fail;
 	} else {
 		RT_TRACE(COMP_FIRMWARE, "Download Firmware: Put code ok!\n");
@@ -135,15 +135,16 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 		mdelay(2);
 	}
 
-	if (!(CPU_status&CPU_GEN_BOOT_RDY))
+	if (!(CPU_status&CPU_GEN_BOOT_RDY)) {
+		netdev_err(dev, "Firmware boot failed.\n");
 		goto CPUCheckMainCodeOKAndTurnOnCPU_Fail;
-	else
-		RT_TRACE(COMP_FIRMWARE, "Download Firmware: Boot ready!\n");
+	}
+
+	RT_TRACE(COMP_FIRMWARE, "Download Firmware: Boot ready!\n");
 
 	return rt_status;
 
 CPUCheckMainCodeOKAndTurnOnCPU_Fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
 	rt_status = false;
 	return rt_status;
 }
@@ -171,7 +172,6 @@ static bool CPUcheck_firmware_ready(struct net_device *dev)
 	return rt_status;
 
 CPUCheckFirmwareReady_Fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
 	rt_status = false;
 	return rt_status;
 
@@ -230,7 +230,7 @@ bool init_firmware(struct net_device *dev)
 
 	u32	file_length = 0;
 	u8	*mapped_file = NULL;
-	u8	init_step = 0;
+	u8	i = 0;
 	enum opt_rst_type rst_opt = OPT_SYSTEM_RESET;
 	enum firmware_init_step starting_state = FW_INIT_STEP0_BOOT;
 
@@ -250,10 +250,9 @@ bool init_firmware(struct net_device *dev)
 			 "PlatformInitFirmware: undefined firmware state\n");
 	}
 
-	for (init_step = starting_state; init_step <= FW_INIT_STEP2_DATA;
-	     init_step++) {
+	for (i = starting_state; i <= FW_INIT_STEP2_DATA; i++) {
 		if (rst_opt == OPT_SYSTEM_RESET) {
-			if (pfirmware->firmware_buf_size[init_step] == 0) {
+			if (pfirmware->firmware_buf_size[i] == 0) {
 				const char *fw_name[3] = {
 					RTL8192E_BOOT_IMG_FW,
 					RTL8192E_MAIN_IMG_FW,
@@ -263,7 +262,7 @@ bool init_firmware(struct net_device *dev)
 				int rc;
 
 				rc = request_firmware(&fw_entry,
-						      fw_name[init_step],
+						      fw_name[i],
 						      &priv->pdev->dev);
 				if (rc < 0) {
 					RT_TRACE(COMP_FIRMWARE,
@@ -271,24 +270,24 @@ bool init_firmware(struct net_device *dev)
 					goto download_firmware_fail;
 				}
 				if (fw_entry->size >
-				    sizeof(pfirmware->firmware_buf[init_step])) {
+				    sizeof(pfirmware->firmware_buf[i])) {
 					RT_TRACE(COMP_FIRMWARE,
 						 "img file size exceed the container struct buffer fail!\n");
 					goto download_firmware_fail;
 				}
 
-				if (init_step != FW_INIT_STEP1_MAIN) {
-					memcpy(pfirmware->firmware_buf[init_step],
+				if (i != FW_INIT_STEP1_MAIN) {
+					memcpy(pfirmware->firmware_buf[i],
 					       fw_entry->data, fw_entry->size);
-					pfirmware->firmware_buf_size[init_step] =
+					pfirmware->firmware_buf_size[i] =
 						fw_entry->size;
 
 				} else {
-					memset(pfirmware->firmware_buf[init_step],
+					memset(pfirmware->firmware_buf[i],
 					       0, 128);
-					memcpy(&pfirmware->firmware_buf[init_step][128],
+					memcpy(&pfirmware->firmware_buf[i][128],
 					       fw_entry->data, fw_entry->size);
-					pfirmware->firmware_buf_size[init_step] =
+					pfirmware->firmware_buf_size[i] =
 						fw_entry->size + 128;
 				}
 
@@ -297,14 +296,14 @@ bool init_firmware(struct net_device *dev)
 			}
 		}
 
-		mapped_file = pfirmware->firmware_buf[init_step];
-		file_length = pfirmware->firmware_buf_size[init_step];
+		mapped_file = pfirmware->firmware_buf[i];
+		file_length = pfirmware->firmware_buf_size[i];
 
 		rt_status = fw_download_code(dev, mapped_file, file_length);
 		if (!rt_status)
 			goto download_firmware_fail;
 
-		if (!firmware_check_ready(dev, init_step))
+		if (!firmware_check_ready(dev, i))
 			goto download_firmware_fail;
 	}
 
@@ -312,7 +311,7 @@ bool init_firmware(struct net_device *dev)
 	return rt_status;
 
 download_firmware_fail:
-	RT_TRACE(COMP_ERR, "ERR in %s()\n", __func__);
+	netdev_err(dev, "%s: Failed to initialize firmware.\n", __func__);
 	rt_status = false;
 	return rt_status;
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
index 94fa16b4993d..d79e54203199 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
@@ -52,11 +52,6 @@ enum firmware_status {
 	FW_STATUS_5_READY = 5,
 };
 
-struct fw_seg_container {
-	u16	seg_size;
-	u8	*seg_ptr;
-};
-
 struct rt_firmware {
 	enum firmware_status firmware_status;
 	u16		  cmdpacket_frag_thresold;
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
index d804876dd92f..5bd3b3530aa6 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
@@ -23,12 +23,6 @@
 
 #include <linux/types.h>
 
-#define BootArrayLengthPciE 344
-extern u8 Rtl8192PciEFwBootArray[BootArrayLengthPciE];
-#define MainArrayLengthPciE 43012
-extern u8 Rtl8192PciEFwMainArray[MainArrayLengthPciE];
-#define DataArrayLengthPciE 848
-extern u8 Rtl8192PciEFwDataArray[DataArrayLengthPciE];
 #define PHY_REGArrayLengthPciE 1
 extern u32 Rtl8192PciEPHY_REGArray[PHY_REGArrayLengthPciE];
 #define PHY_REG_1T2RArrayLengthPciE 296
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index 4664a4fd1e48..fba7654160e8 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -504,13 +504,15 @@ bool rtl8192_phy_checkBBAndRF(struct net_device *dev,
 	WriteAddr[HW90_BLOCK_RF] = 0x3;
 	RT_TRACE(COMP_PHY, "=======>%s(), CheckBlock:%d\n", __func__,
 		 CheckBlock);
+
+	if (CheckBlock == HW90_BLOCK_MAC) {
+		netdev_warn(dev, "%s(): No checks available for MAC block.\n",
+			    __func__);
+		return ret;
+	}
+
 	for (i = 0; i < CheckTimes; i++) {
 		switch (CheckBlock) {
-		case HW90_BLOCK_MAC:
-			RT_TRACE(COMP_ERR,
-				 "PHY_CheckBBRFOK(): Never Write 0x100 here!");
-			break;
-
 		case HW90_BLOCK_PHY0:
 		case HW90_BLOCK_PHY1:
 			write_nic_dword(dev, WriteAddr[CheckBlock],
@@ -537,9 +539,7 @@ bool rtl8192_phy_checkBBAndRF(struct net_device *dev,
 
 
 		if (dwRegRead != WriteData[i]) {
-			RT_TRACE(COMP_ERR,
-				 "====>error=====dwRegRead: %x, WriteData: %x\n",
-				 dwRegRead, WriteData[i]);
+			netdev_warn(dev, "%s(): Check failed.\n", __func__);
 			ret = false;
 			break;
 		}
@@ -628,8 +628,8 @@ void rtl8192_phy_getTxPower(struct net_device *dev)
 	priv->DefaultInitialGain[3] = read_nic_byte(dev, rOFDM0_XDAGCCore1);
 	RT_TRACE(COMP_INIT,
 		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x)\n",
-		priv->DefaultInitialGain[0], priv->DefaultInitialGain[1],
-		priv->DefaultInitialGain[2], priv->DefaultInitialGain[3]);
+		 priv->DefaultInitialGain[0], priv->DefaultInitialGain[1],
+		 priv->DefaultInitialGain[2], priv->DefaultInitialGain[3]);
 
 	priv->framesync = read_nic_byte(dev, rOFDM0_RxDetector3);
 	priv->framesyncC34 = read_nic_dword(dev, rOFDM0_RxDetector2);
@@ -685,8 +685,7 @@ void rtl8192_phy_setTxPower(struct net_device *dev, u8 channel)
 	case RF_8258:
 		break;
 	default:
-		RT_TRACE(COMP_ERR, "unknown rf chip in function %s()\n",
-			 __func__);
+		netdev_err(dev, "Invalid RF Chip ID.\n");
 		break;
 	}
 }
@@ -709,7 +708,7 @@ bool rtl8192_phy_RFConfig(struct net_device *dev)
 		break;
 
 	default:
-		RT_TRACE(COMP_ERR, "error chip id\n");
+		netdev_err(dev, "Invalid RF Chip ID.\n");
 		break;
 	}
 	return rtStatus;
@@ -802,13 +801,13 @@ static void rtl8192_SetTxPowerLevel(struct net_device *dev, u8 channel)
 	case RF_8258:
 		break;
 	default:
-		RT_TRACE(COMP_ERR,
-			 "unknown rf chip ID in rtl8192_SetTxPowerLevel()\n");
+		netdev_warn(dev, "%s(): Invalid RF Chip ID\n", __func__);
 		break;
 	}
 }
 
-static u8 rtl8192_phy_SetSwChnlCmdArray(struct sw_chnl_cmd *CmdTable,
+static u8 rtl8192_phy_SetSwChnlCmdArray(struct net_device *dev,
+					struct sw_chnl_cmd *CmdTable,
 					u32 CmdTableIdx, u32 CmdTableSz,
 					enum sw_chnl_cmd_id CmdID,
 					u32 Para1, u32 Para2, u32 msDelay)
@@ -816,14 +815,11 @@ static u8 rtl8192_phy_SetSwChnlCmdArray(struct sw_chnl_cmd *CmdTable,
 	struct sw_chnl_cmd *pCmd;
 
 	if (CmdTable == NULL) {
-		RT_TRACE(COMP_ERR,
-			 "phy_SetSwChnlCmdArray(): CmdTable cannot be NULL.\n");
+		netdev_err(dev, "%s(): CmdTable cannot be NULL.\n", __func__);
 		return false;
 	}
 	if (CmdTableIdx >= CmdTableSz) {
-		RT_TRACE(COMP_ERR,
-			 "phy_SetSwChnlCmdArray(): Access invalid index, please check size of the table, CmdTableIdx:%d, CmdTableSz:%d\n",
-			 CmdTableIdx, CmdTableSz);
+		netdev_err(dev, "%s(): Invalid index requested.\n", __func__);
 		return false;
 	}
 
@@ -851,24 +847,23 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 		  __func__, *stage, *step, channel);
 
 	if (!rtllib_legal_channel(priv->rtllib, channel)) {
-		RT_TRACE(COMP_ERR, "=============>set to illegal channel:%d\n",
-			 channel);
+		netdev_err(dev, "Invalid channel requested: %d\n", channel);
 		return true;
 	}
 
 	{
 		PreCommonCmdCnt = 0;
-		rtl8192_phy_SetSwChnlCmdArray(ieee->PreCommonCmd,
+		rtl8192_phy_SetSwChnlCmdArray(dev, ieee->PreCommonCmd,
 					PreCommonCmdCnt++,
 					MAX_PRECMD_CNT, CmdID_SetTxPowerLevel,
 					0, 0, 0);
-		rtl8192_phy_SetSwChnlCmdArray(ieee->PreCommonCmd,
+		rtl8192_phy_SetSwChnlCmdArray(dev, ieee->PreCommonCmd,
 					PreCommonCmdCnt++,
 					MAX_PRECMD_CNT, CmdID_End, 0, 0, 0);
 
 		PostCommonCmdCnt = 0;
 
-		rtl8192_phy_SetSwChnlCmdArray(ieee->PostCommonCmd,
+		rtl8192_phy_SetSwChnlCmdArray(dev, ieee->PostCommonCmd,
 					PostCommonCmdCnt++,
 					MAX_POSTCMD_CNT, CmdID_End, 0, 0, 0);
 
@@ -876,32 +871,32 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 		switch (priv->rf_chip) {
 		case RF_8225:
 			if (!(channel >= 1 && channel <= 14)) {
-				RT_TRACE(COMP_ERR,
-					 "illegal channel for Zebra 8225: %d\n",
-					 channel);
+				netdev_err(dev,
+					   "Invalid channel requested for 8225: %d\n",
+					   channel);
 				return false;
 			}
-			rtl8192_phy_SetSwChnlCmdArray(ieee->RfDependCmd,
+			rtl8192_phy_SetSwChnlCmdArray(dev, ieee->RfDependCmd,
 				RfDependCmdCnt++, MAX_RFDEPENDCMD_CNT,
 				CmdID_RF_WriteReg, rZebra1_Channel,
 				RF_CHANNEL_TABLE_ZEBRA[channel], 10);
-			rtl8192_phy_SetSwChnlCmdArray(ieee->RfDependCmd,
+			rtl8192_phy_SetSwChnlCmdArray(dev, ieee->RfDependCmd,
 				RfDependCmdCnt++, MAX_RFDEPENDCMD_CNT,
 				CmdID_End, 0, 0, 0);
 			break;
 
 		case RF_8256:
 			if (!(channel >= 1 && channel <= 14)) {
-				RT_TRACE(COMP_ERR,
-					 "illegal channel for Zebra 8256: %d\n",
-					 channel);
+				netdev_err(dev,
+					   "Invalid channel requested for 8256: %d\n",
+					   channel);
 				return false;
 			}
-			rtl8192_phy_SetSwChnlCmdArray(ieee->RfDependCmd,
+			rtl8192_phy_SetSwChnlCmdArray(dev, ieee->RfDependCmd,
 				 RfDependCmdCnt++, MAX_RFDEPENDCMD_CNT,
 				CmdID_RF_WriteReg, rZebra1_Channel, channel,
 				 10);
-			rtl8192_phy_SetSwChnlCmdArray(ieee->RfDependCmd,
+			rtl8192_phy_SetSwChnlCmdArray(dev, ieee->RfDependCmd,
 
 						      RfDependCmdCnt++,
 						      MAX_RFDEPENDCMD_CNT,
@@ -912,8 +907,7 @@ static u8 rtl8192_phy_SwChnlStepByStep(struct net_device *dev, u8 channel,
 			break;
 
 		default:
-			RT_TRACE(COMP_ERR, "Unknown RFChipID: %d\n",
-				 priv->rf_chip);
+			netdev_warn(dev, "Unknown RF Chip ID\n");
 			return false;
 		}
 
@@ -1002,7 +996,7 @@ void rtl8192_SwChnl_WorkItem(struct net_device *dev)
 	RT_TRACE(COMP_TRACE, "=====>--%s(), set chan:%d, priv:%p\n", __func__,
 		 priv->chan, priv);
 
-	rtl8192_phy_FinishSwChnlNow(dev , priv->chan);
+	rtl8192_phy_FinishSwChnlNow(dev, priv->chan);
 
 	RT_TRACE(COMP_TRACE, "<== SwChnlCallback819xUsbWorkItem()\n");
 }
@@ -1013,7 +1007,7 @@ u8 rtl8192_phy_SwChnl(struct net_device *dev, u8 channel)
 
 	RT_TRACE(COMP_PHY, "=====>%s()\n", __func__);
 	if (!priv->up) {
-		RT_TRACE(COMP_ERR, "%s(): ERR !! driver is not up\n", __func__);
+		netdev_err(dev, "%s(): Driver is not initialized\n", __func__);
 		return false;
 	}
 	if (priv->SwChnlInProgress)
@@ -1024,20 +1018,26 @@ u8 rtl8192_phy_SwChnl(struct net_device *dev, u8 channel)
 	case WIRELESS_MODE_A:
 	case WIRELESS_MODE_N_5G:
 		if (channel <= 14) {
-			RT_TRACE(COMP_ERR, "WIRELESS_MODE_A but channel<=14");
+			netdev_warn(dev,
+				    "Channel %d not available in 802.11a.\n",
+				    channel);
 			return false;
 		}
 		break;
 	case WIRELESS_MODE_B:
 		if (channel > 14) {
-			RT_TRACE(COMP_ERR, "WIRELESS_MODE_B but channel>14");
+			netdev_warn(dev,
+				    "Channel %d not available in 802.11b.\n",
+				    channel);
 			return false;
 		}
 		break;
 	case WIRELESS_MODE_G:
 	case WIRELESS_MODE_N_24G:
 		if (channel > 14) {
-			RT_TRACE(COMP_ERR, "WIRELESS_MODE_G but channel>14");
+			netdev_warn(dev,
+				    "Channel %d not available in 802.11g.\n",
+				    channel);
 			return false;
 		}
 		break;
@@ -1180,7 +1180,7 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 		return;
 	}
 	if (!priv->up) {
-		RT_TRACE(COMP_ERR, "%s(): ERR!! driver is not up\n", __func__);
+		netdev_err(dev, "%s(): Driver is not initialized\n", __func__);
 		return;
 	}
 	regBwOpMode = read_nic_byte(dev, BW_OPMODE);
@@ -1197,9 +1197,8 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 		break;
 
 	default:
-		RT_TRACE(COMP_ERR,
-			 "SetChannelBandwidth819xUsb(): unknown Bandwidth: %#X\n",
-			 priv->CurrentChannelBW);
+		netdev_err(dev, "%s(): unknown Bandwidth: %#X\n", __func__,
+			   priv->CurrentChannelBW);
 		break;
 	}
 
@@ -1239,9 +1238,8 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 		rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, 0x00100000, 0);
 		break;
 	default:
-		RT_TRACE(COMP_ERR,
-			 "SetChannelBandwidth819xUsb(): unknown Bandwidth: %#X\n",
-			 priv->CurrentChannelBW);
+		netdev_err(dev, "%s(): unknown Bandwidth: %#X\n", __func__,
+			   priv->CurrentChannelBW);
 		break;
 
 	}
@@ -1261,7 +1259,8 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 		break;
 
 	default:
-		RT_TRACE(COMP_ERR, "Unknown RFChipID: %d\n", priv->rf_chip);
+		netdev_info(dev, "%s(): Unknown RFChipID: %d\n", __func__,
+			    priv->rf_chip);
 		break;
 	}
 
@@ -1452,9 +1451,9 @@ static bool SetRFPowerState8190(struct net_device *dev,
 				} while (!rtstatus && (InitilizeCount > 0));
 
 				if (!rtstatus) {
-					RT_TRACE(COMP_ERR,
-						 "%s():Initialize Adapter fail,return\n",
-						 __func__);
+					netdev_err(dev,
+						   "%s(): Failed to initialize Adapter.\n",
+						   __func__);
 					priv->SetRFPowerStateInProgress = false;
 					return false;
 				}
@@ -1555,16 +1554,16 @@ static bool SetRFPowerState8190(struct net_device *dev,
 
 		default:
 			bResult = false;
-			RT_TRACE(COMP_ERR,
-				 "SetRFPowerState8190(): unknown state to set: 0x%X!!!\n",
-				 eRFPowerState);
+			netdev_warn(dev,
+				    "%s(): Unknown state requested: 0x%X.\n",
+				    __func__, eRFPowerState);
 			break;
 		}
 
 		break;
 
 	default:
-		RT_TRACE(COMP_ERR, "SetRFPowerState8190(): Unknown RF type\n");
+		netdev_warn(dev, "%s(): Unknown RF type\n", __func__);
 		break;
 	}
 
@@ -1576,8 +1575,7 @@ static bool SetRFPowerState8190(struct net_device *dev,
 			break;
 
 		default:
-			RT_TRACE(COMP_ERR,
-				 "SetRFPowerState8190(): Unknown RF type\n");
+			netdev_warn(dev, "%s(): Unknown RF type\n", __func__);
 			break;
 		}
 	}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
index 7318f8857af2..18bc58240fbe 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
@@ -41,13 +41,7 @@
 #define Rtl819XPHY_REGArray			Rtl8192PciEPHY_REGArray
 #define Rtl819XPHY_REG_1T2RArray		Rtl8192PciEPHY_REG_1T2RArray
 
-extern u32 rtl819XMACPHY_Array_PG[];
-extern u32 rtl819XPHY_REG_1T2RArray[];
 extern u32 rtl819XAGCTAB_Array[];
-extern u32 rtl819XRadioA_Array[];
-extern u32 rtl819XRadioB_Array[];
-extern u32 rtl819XRadioC_Array[];
-extern u32 rtl819XRadioD_Array[];
 
 enum hw90_block {
 	HW90_BLOCK_MAC = 0,
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
index 7899dd538dcd..8a1d91e05da9 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
@@ -3,7 +3,7 @@
  *
  * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  * more details.
  *
  * You should have received a copy of the GNU General Public License along with
@@ -20,48 +20,48 @@
 #define _R819XU_PHYREG_H
 
 
-#define   RF_DATA				0x1d4
-
-#define rPMAC_Reset				0x100
-#define rPMAC_TxStart				0x104
-#define rPMAC_TxLegacySIG			0x108
-#define rPMAC_TxHTSIG1				0x10c
-#define rPMAC_TxHTSIG2				0x110
-#define rPMAC_PHYDebug				0x114
-#define rPMAC_TxPacketNum			0x118
-#define rPMAC_TxIdle				0x11c
-#define rPMAC_TxMACHeader0		0x120
-#define rPMAC_TxMACHeader1		0x124
-#define rPMAC_TxMACHeader2		0x128
-#define rPMAC_TxMACHeader3		0x12c
-#define rPMAC_TxMACHeader4		0x130
-#define rPMAC_TxMACHeader5		0x134
-#define rPMAC_TxDataType			0x138
-#define rPMAC_TxRandomSeed			0x13c
-#define rPMAC_CCKPLCPPreamble		0x140
-#define rPMAC_CCKPLCPHeader			0x144
-#define rPMAC_CCKCRC16				0x148
-#define rPMAC_OFDMRxCRC32OK		0x170
-#define rPMAC_OFDMRxCRC32Er		0x174
-#define rPMAC_OFDMRxParityEr			0x178
-#define rPMAC_OFDMRxCRC8Er		0x17c
-#define rPMAC_CCKCRxRC16Er			0x180
-#define rPMAC_CCKCRxRC32Er			0x184
-#define rPMAC_CCKCRxRC32OK			0x188
-#define rPMAC_TxStatus				0x18c
-
-#define	MCS_TXAGC				0x340
-#define	CCK_TXAGC				0x348
-
-/*---------------------0x400~0x4ff----------------------*/
-#define	MacBlkCtrl				0x403
-
-#define rFPGA0_RFMOD				0x800
-#define rFPGA0_TxInfo				0x804
-#define rFPGA0_PSDFunction			0x808
-#define rFPGA0_TxGainStage			0x80c
-#define rFPGA0_RFTiming1			0x810
-#define rFPGA0_RFTiming2			0x814
+#define RF_DATA			0x1d4
+
+#define rPMAC_Reset		0x100
+#define rPMAC_TxStart		0x104
+#define rPMAC_TxLegacySIG	0x108
+#define rPMAC_TxHTSIG1		0x10c
+#define rPMAC_TxHTSIG2		0x110
+#define rPMAC_PHYDebug		0x114
+#define rPMAC_TxPacketNum	0x118
+#define rPMAC_TxIdle		0x11c
+#define rPMAC_TxMACHeader0	0x120
+#define rPMAC_TxMACHeader1	0x124
+#define rPMAC_TxMACHeader2	0x128
+#define rPMAC_TxMACHeader3	0x12c
+#define rPMAC_TxMACHeader4	0x130
+#define rPMAC_TxMACHeader5	0x134
+#define rPMAC_TxDataType	0x138
+#define rPMAC_TxRandomSeed	0x13c
+#define rPMAC_CCKPLCPPreamble	0x140
+#define rPMAC_CCKPLCPHeader	0x144
+#define rPMAC_CCKCRC16		0x148
+#define rPMAC_OFDMRxCRC32OK	0x170
+#define rPMAC_OFDMRxCRC32Er	0x174
+#define rPMAC_OFDMRxParityEr	0x178
+#define rPMAC_OFDMRxCRC8Er	0x17c
+#define rPMAC_CCKCRxRC16Er	0x180
+#define rPMAC_CCKCRxRC32Er	0x184
+#define rPMAC_CCKCRxRC32OK	0x188
+#define rPMAC_TxStatus		0x18c
+
+#define MCS_TXAGC		0x340
+#define CCK_TXAGC		0x348
+
+/* Mac block on/off control register */
+#define MacBlkCtrl			0x403
+
+#define rFPGA0_RFMOD			0x800 /* RF mode & CCK TxSC */
+#define rFPGA0_TxInfo			0x804
+#define rFPGA0_PSDFunction		0x808
+#define rFPGA0_TxGainStage		0x80c
+#define rFPGA0_RFTiming1		0x810
+#define rFPGA0_RFTiming2		0x814
 #define rFPGA0_XA_HSSIParameter1	0x820
 #define rFPGA0_XA_HSSIParameter2	0x824
 #define rFPGA0_XB_HSSIParameter1	0x828
@@ -94,52 +94,57 @@
 #define rFPGA0_XB_LSSIReadBack		0x8a4
 #define rFPGA0_XC_LSSIReadBack		0x8a8
 #define rFPGA0_XD_LSSIReadBack		0x8ac
-#define rFPGA0_PSDReport			0x8b4
+#define rFPGA0_PSDReport		0x8b4
 #define rFPGA0_XAB_RFInterfaceRB	0x8e0
 #define rFPGA0_XCD_RFInterfaceRB	0x8e4
 
-#define rFPGA1_RFMOD				0x900
-#define rFPGA1_TxBlock				0x904
-#define rFPGA1_DebugSelect			0x908
-#define rFPGA1_TxInfo				0x90c
-
-#define rCCK0_System				0xa00
-#define rCCK0_AFESetting			0xa04
-#define rCCK0_CCA					0xa08
-#define rCCK0_RxAGC1				0xa0c
-#define rCCK0_RxAGC2				0xa10
-#define rCCK0_RxHP				0xa14
+/* Page 9 - RF mode & OFDM TxSC */
+#define rFPGA1_RFMOD			0x900
+#define rFPGA1_TxBlock			0x904
+#define rFPGA1_DebugSelect		0x908
+#define rFPGA1_TxInfo			0x90c
+
+#define rCCK0_System			0xa00
+#define rCCK0_AFESetting		0xa04
+#define rCCK0_CCA			0xa08
+/* AGC default value, saturation level */
+#define rCCK0_RxAGC1			0xa0c
+#define rCCK0_RxAGC2			0xa10 /* AGC & DAGC */
+#define rCCK0_RxHP			0xa14
+/* Timing recovery & channel estimation threshold */
 #define rCCK0_DSPParameter1		0xa18
-#define rCCK0_DSPParameter2		0xa1c
-#define rCCK0_TxFilter1				0xa20
-#define rCCK0_TxFilter2				0xa24
-#define rCCK0_DebugPort				0xa28
+#define rCCK0_DSPParameter2		0xa1c /* SQ threshold */
+#define rCCK0_TxFilter1			0xa20
+#define rCCK0_TxFilter2			0xa24
+#define rCCK0_DebugPort			0xa28 /* Debug port and TX filter 3 */
 #define rCCK0_FalseAlarmReport		0xa2c
-#define rCCK0_TRSSIReport			0xa50
-#define rCCK0_RxReport				0xa54
+#define rCCK0_TRSSIReport		0xa50
+#define rCCK0_RxReport			0xa54
 #define rCCK0_FACounterLower		0xa5c
 #define rCCK0_FACounterUpper		0xa58
 
-#define rOFDM0_LSTF				0xc00
+#define rOFDM0_LSTF			0xc00
 #define rOFDM0_TRxPathEnable		0xc04
-#define rOFDM0_TRMuxPar				0xc08
-#define rOFDM0_TRSWIsolation			0xc0c
-#define rOFDM0_XARxAFE				0xc10
-#define rOFDM0_XARxIQImbalance		0xc14
-#define rOFDM0_XBRxAFE				0xc18
+#define rOFDM0_TRMuxPar			0xc08
+#define rOFDM0_TRSWIsolation		0xc0c
+/* RxIQ DC offset, Rx digital filter, DC notch filter */
+#define rOFDM0_XARxAFE			0xc10
+#define rOFDM0_XARxIQImbalance		0xc14 /* RxIQ imbalance matrix */
+#define rOFDM0_XBRxAFE			0xc18
 #define rOFDM0_XBRxIQImbalance		0xc1c
-#define rOFDM0_XCRxAFE				0xc20
+#define rOFDM0_XCRxAFE			0xc20
 #define rOFDM0_XCRxIQImbalance		0xc24
-#define rOFDM0_XDRxAFE				0xc28
+#define rOFDM0_XDRxAFE			0xc28
 #define rOFDM0_XDRxIQImbalance		0xc2c
-#define rOFDM0_RxDetector1			0xc30
-#define rOFDM0_RxDetector2			0xc34
-#define rOFDM0_RxDetector3			0xc38
-#define rOFDM0_RxDetector4			0xc3c
-#define rOFDM0_RxDSP				0xc40
-#define rOFDM0_CFOandDAGC		0xc44
+#define rOFDM0_RxDetector1		0xc30 /* PD, BW & SBD */
+#define rOFDM0_RxDetector2		0xc34 /* SBD */
+#define rOFDM0_RxDetector3		0xc38 /* Frame Sync */
+/* PD, SBD, Frame Sync & Short-GI */
+#define rOFDM0_RxDetector4		0xc3c
+#define rOFDM0_RxDSP			0xc40 /* Rx Sync Path */
+#define rOFDM0_CFOandDAGC		0xc44 /* CFO & DAGC */
 #define rOFDM0_CCADropThreshold		0xc48
-#define rOFDM0_ECCAThreshold		0xc4c
+#define rOFDM0_ECCAThreshold		0xc4c /* Energy CCA */
 #define rOFDM0_XAAGCCore1		0xc50
 #define rOFDM0_XAAGCCore2		0xc54
 #define rOFDM0_XBAGCCore1		0xc58
@@ -151,702 +156,735 @@
 #define rOFDM0_AGCParameter1		0xc70
 #define rOFDM0_AGCParameter2		0xc74
 #define rOFDM0_AGCRSSITable		0xc78
-#define rOFDM0_HTSTFAGC				0xc7c
+#define rOFDM0_HTSTFAGC			0xc7c
 #define rOFDM0_XATxIQImbalance		0xc80
-#define rOFDM0_XATxAFE				0xc84
+#define rOFDM0_XATxAFE			0xc84
 #define rOFDM0_XBTxIQImbalance		0xc88
-#define rOFDM0_XBTxAFE				0xc8c
+#define rOFDM0_XBTxAFE			0xc8c
 #define rOFDM0_XCTxIQImbalance		0xc90
-#define rOFDM0_XCTxAFE				0xc94
+#define rOFDM0_XCTxAFE			0xc94
 #define rOFDM0_XDTxIQImbalance		0xc98
-#define rOFDM0_XDTxAFE				0xc9c
+#define rOFDM0_XDTxAFE			0xc9c
 #define rOFDM0_RxHPParameter		0xce0
 #define rOFDM0_TxPseudoNoiseWgt		0xce4
-#define rOFDM0_FrameSync			0xcf0
-#define rOFDM0_DFSReport			0xcf4
-#define rOFDM0_TxCoeff1				0xca4
-#define rOFDM0_TxCoeff2				0xca8
-#define rOFDM0_TxCoeff3				0xcac
-#define rOFDM0_TxCoeff4				0xcb0
-#define rOFDM0_TxCoeff5				0xcb4
-#define rOFDM0_TxCoeff6				0xcb8
+#define rOFDM0_FrameSync		0xcf0
+#define rOFDM0_DFSReport		0xcf4
+#define rOFDM0_TxCoeff1			0xca4
+#define rOFDM0_TxCoeff2			0xca8
+#define rOFDM0_TxCoeff3			0xcac
+#define rOFDM0_TxCoeff4			0xcb0
+#define rOFDM0_TxCoeff5			0xcb4
+#define rOFDM0_TxCoeff6			0xcb8
 
 
-#define rOFDM1_LSTF				0xd00
+#define rOFDM1_LSTF			0xd00
 #define rOFDM1_TRxPathEnable		0xd04
-#define rOFDM1_CFO				0xd08
-#define rOFDM1_CSI1				0xd10
-#define rOFDM1_SBD				0xd14
-#define rOFDM1_CSI2				0xd18
-#define rOFDM1_CFOTracking			0xd2c
+#define rOFDM1_CFO			0xd08
+#define rOFDM1_CSI1			0xd10
+#define rOFDM1_SBD			0xd14
+#define rOFDM1_CSI2			0xd18
+#define rOFDM1_CFOTracking		0xd2c
 #define rOFDM1_TRxMesaure1		0xd34
-#define rOFDM1_IntfDet				0xd3c
-#define rOFDM1_PseudoNoiseStateAB 0xd50
-#define rOFDM1_PseudoNoiseStateCD 0xd54
-#define rOFDM1_RxPseudoNoiseWgt   0xd58
-#define rOFDM_PHYCounter1			0xda0
-#define rOFDM_PHYCounter2			0xda4
-#define rOFDM_PHYCounter3			0xda8
-#define rOFDM_ShortCFOAB			0xdac
-#define rOFDM_ShortCFOCD			0xdb0
-#define rOFDM_LongCFOAB				0xdb4
-#define rOFDM_LongCFOCD				0xdb8
-#define rOFDM_TailCFOAB				0xdbc
-#define rOFDM_TailCFOCD				0xdc0
+#define rOFDM1_IntfDet			0xd3c
+#define rOFDM1_PseudoNoiseStateAB	0xd50
+#define rOFDM1_PseudoNoiseStateCD	0xd54
+#define rOFDM1_RxPseudoNoiseWgt		0xd58
+#define rOFDM_PHYCounter1		0xda0 /* cca, parity fail */
+#define rOFDM_PHYCounter2		0xda4 /* rate illegal, crc8 fail */
+#define rOFDM_PHYCounter3		0xda8 /* MCS not supported */
+#define rOFDM_ShortCFOAB		0xdac
+#define rOFDM_ShortCFOCD		0xdb0
+#define rOFDM_LongCFOAB			0xdb4
+#define rOFDM_LongCFOCD			0xdb8
+#define rOFDM_TailCFOAB			0xdbc
+#define rOFDM_TailCFOCD			0xdc0
 #define rOFDM_PWMeasure1		0xdc4
 #define rOFDM_PWMeasure2		0xdc8
-#define rOFDM_BWReport				0xdcc
-#define rOFDM_AGCReport				0xdd0
-#define rOFDM_RxSNR				0xdd4
-#define rOFDM_RxEVMCSI				0xdd8
-#define rOFDM_SIGReport				0xddc
+#define rOFDM_BWReport			0xdcc
+#define rOFDM_AGCReport			0xdd0
+#define rOFDM_RxSNR			0xdd4
+#define rOFDM_RxEVMCSI			0xdd8
+#define rOFDM_SIGReport			0xddc
 
-#define rTxAGC_Rate18_06			0xe00
-#define rTxAGC_Rate54_24			0xe04
-#define rTxAGC_CCK_Mcs32			0xe08
-#define rTxAGC_Mcs03_Mcs00			0xe10
-#define rTxAGC_Mcs07_Mcs04			0xe14
-#define rTxAGC_Mcs11_Mcs08			0xe18
-#define rTxAGC_Mcs15_Mcs12			0xe1c
+#define rTxAGC_Rate18_06		0xe00
+#define rTxAGC_Rate54_24		0xe04
+#define rTxAGC_CCK_Mcs32		0xe08
+#define rTxAGC_Mcs03_Mcs00		0xe10
+#define rTxAGC_Mcs07_Mcs04		0xe14
+#define rTxAGC_Mcs11_Mcs08		0xe18
+#define rTxAGC_Mcs15_Mcs12		0xe1c
 
 
 #define rZebra1_HSSIEnable		0x0
 #define rZebra1_TRxEnable1		0x1
 #define rZebra1_TRxEnable2		0x2
-#define rZebra1_AGC				0x4
+#define rZebra1_AGC			0x4
 #define rZebra1_ChargePump		0x5
-#define rZebra1_Channel				0x7
-#define rZebra1_TxGain				0x8
-#define rZebra1_TxLPF				0x9
-#define rZebra1_RxLPF				0xb
+#define rZebra1_Channel			0x7
+#define rZebra1_TxGain			0x8
+#define rZebra1_TxLPF			0x9
+#define rZebra1_RxLPF			0xb
 #define rZebra1_RxHPFCorner		0xc
 
-#define rGlobalCtrl				0
-#define rRTL8256_TxLPF				19
-#define rRTL8256_RxLPF				11
+/* Zebra 4 */
+#define rGlobalCtrl			0
+#define rRTL8256_TxLPF			19
+#define rRTL8256_RxLPF			11
 
-#define rRTL8258_TxLPF				0x11
-#define rRTL8258_RxLPF				0x13
+/* RTL8258 */
+#define rRTL8258_TxLPF			0x11
+#define rRTL8258_RxLPF			0x13
 #define rRTL8258_RSSILPF		0xa
 
-#define bBBResetB					0x100
-#define bGlobalResetB				0x200
-#define bOFDMTxStart				0x4
-#define bCCKTxStart					0x8
-#define bCRC32Debug				0x100
-#define bPMACLoopback				0x10
-#define bTxLSIG						0xffffff
-#define bOFDMTxRate				0xf
-#define bOFDMTxReserved				0x10
-#define bOFDMTxLength				0x1ffe0
-#define bOFDMTxParity				0x20000
-#define bTxHTSIG1					0xffffff
-#define bTxHTMCSRate				0x7f
-#define bTxHTBW						0x80
-#define bTxHTLength				0xffff00
-#define bTxHTSIG2					0xffffff
-#define bTxHTSmoothing				0x1
-#define bTxHTSounding				0x2
-#define bTxHTReserved				0x4
-#define bTxHTAggreation				0x8
-#define bTxHTSTBC					0x30
-#define bTxHTAdvanceCoding			0x40
-#define bTxHTShortGI				0x80
-#define bTxHTNumberHT_LTF			0x300
-#define bTxHTCRC8					0x3fc00
-#define bCounterReset				0x10000
-#define bNumOfOFDMTx				0xffff
-#define bNumOfCCKTx				0xffff0000
-#define bTxIdleInterval					0xffff
-#define bOFDMService				0xffff0000
-#define bTxMACHeader				0xffffffff
-#define bTxDataInit					0xff
-#define bTxHTMode				0x100
-#define bTxDataType				0x30000
-#define bTxRandomSeed				0xffffffff
-#define bCCKTxPreamble				0x1
-#define bCCKTxSFD					0xffff0000
-#define bCCKTxSIG					0xff
-#define bCCKTxService				0xff00
-#define bCCKLengthExt				0x8000
-#define bCCKTxLength				0xffff0000
-#define bCCKTxCRC16				0xffff
-#define bCCKTxStatus				0x1
-#define bOFDMTxStatus				0x2
-
-#define bRFMOD						0x1
-#define bJapanMode				0x2
-#define bCCKTxSC					0x30
-#define bCCKEn						0x1000000
-#define bOFDMEn						0x2000000
-#define bOFDMRxADCPhase				0x10000
-#define bOFDMTxDACPhase				0x40000
-#define bXATxAGC					0x3f
-#define bXBTxAGC					0xf00
-#define bXCTxAGC					0xf000
-#define bXDTxAGC					0xf0000
-#define bPAStart					0xf0000000
-#define bTRStart					0x00f00000
-#define bRFStart					0x0000f000
-#define bBBStart					0x000000f0
-#define bBBCCKStart				0x0000000f
-#define bPAEnd						0xf
-#define bTREnd						0x0f000000
-#define bRFEnd						0x000f0000
-#define bCCAMask					0x000000f0
-#define bR2RCCAMask				0x00000f00
-#define bHSSI_R2TDelay				0xf8000000
-#define bHSSI_T2RDelay				0xf80000
-#define bContTxHSSI				0x400
-#define bIGFromCCK				0x200
-#define bAGCAddress				0x3f
-#define bRxHPTx						0x7000
-#define bRxHPT2R					0x38000
-#define bRxHPCCKIni				0xc0000
-#define bAGCTxCode				0xc00000
-#define bAGCRxCode				0x300000
-#define b3WireDataLength			0x800
-#define b3WireAddressLength			0x400
-#define b3WireRFPowerDown			0x1
-#define b5GPAPEPolarity				0x40000000
-#define b2GPAPEPolarity				0x80000000
-#define bRFSW_TxDefaultAnt			0x3
-#define bRFSW_TxOptionAnt			0x30
-#define bRFSW_RxDefaultAnt			0x300
-#define bRFSW_RxOptionAnt			0x3000
-#define bRFSI_3WireData				0x1
-#define bRFSI_3WireClock			0x2
-#define bRFSI_3WireLoad				0x4
-#define bRFSI_3WireRW				0x8
-#define bRFSI_3Wire					0xf
-#define bRFSI_RFENV				0x10
-#define bRFSI_TRSW				0x20
-#define bRFSI_TRSWB				0x40
-#define bRFSI_ANTSW				0x100
-#define bRFSI_ANTSWB				0x200
-#define bRFSI_PAPE					0x400
-#define bRFSI_PAPE5G				0x800
-#define bBandSelect					0x1
-#define bHTSIG2_GI					0x80
-#define bHTSIG2_Smoothing			0x01
-#define bHTSIG2_Sounding			0x02
-#define bHTSIG2_Aggreaton			0x08
-#define bHTSIG2_STBC				0x30
-#define bHTSIG2_AdvCoding			0x40
+/* Bit Mask - Page 1*/
+#define bBBResetB			0x100
+#define bGlobalResetB			0x200
+#define bOFDMTxStart			0x4
+#define bCCKTxStart			0x8
+#define bCRC32Debug			0x100
+#define bPMACLoopback			0x10
+#define bTxLSIG				0xffffff
+#define bOFDMTxRate			0xf
+#define bOFDMTxReserved			0x10
+#define bOFDMTxLength			0x1ffe0
+#define bOFDMTxParity			0x20000
+#define bTxHTSIG1			0xffffff
+#define bTxHTMCSRate			0x7f
+#define bTxHTBW	0x80
+#define bTxHTLength			0xffff00
+#define bTxHTSIG2			0xffffff
+#define bTxHTSmoothing			0x1
+#define bTxHTSounding			0x2
+#define bTxHTReserved			0x4
+#define bTxHTAggreation			0x8
+#define bTxHTSTBC			0x30
+#define bTxHTAdvanceCoding		0x40
+#define bTxHTShortGI			0x80
+#define bTxHTNumberHT_LTF		0x300
+#define bTxHTCRC8			0x3fc00
+#define bCounterReset			0x10000
+#define bNumOfOFDMTx			0xffff
+#define bNumOfCCKTx			0xffff0000
+#define bTxIdleInterval			0xffff
+#define bOFDMService			0xffff0000
+#define bTxMACHeader			0xffffffff
+#define bTxDataInit			0xff
+#define bTxHTMode			0x100
+#define bTxDataType			0x30000
+#define bTxRandomSeed			0xffffffff
+#define bCCKTxPreamble			0x1
+#define bCCKTxSFD			0xffff0000
+#define bCCKTxSIG			0xff
+#define bCCKTxService			0xff00
+#define bCCKLengthExt			0x8000
+#define bCCKTxLength			0xffff0000
+#define bCCKTxCRC16			0xffff
+#define bCCKTxStatus			0x1
+#define bOFDMTxStatus			0x2
+/* Bit Mask - Page 8 */
+#define bRFMOD				0x1
+#define bJapanMode			0x2
+#define bCCKTxSC			0x30
+#define bCCKEn				0x1000000
+#define bOFDMEn				0x2000000
+#define bOFDMRxADCPhase			0x10000
+#define bOFDMTxDACPhase			0x40000
+#define bXATxAGC			0x3f
+#define bXBTxAGC			0xf00
+#define bXCTxAGC			0xf000
+#define bXDTxAGC			0xf0000
+#define bPAStart			0xf0000000
+#define bTRStart			0x00f00000
+#define bRFStart			0x0000f000
+#define bBBStart			0x000000f0
+#define bBBCCKStart			0x0000000f
+/* Bit Mask - rFPGA0_RFTiming2 */
+#define bPAEnd				0xf
+#define bTREnd				0x0f000000
+#define bRFEnd				0x000f0000
+/* T2R */
+#define bCCAMask			0x000000f0
+#define bR2RCCAMask			0x00000f00
+#define bHSSI_R2TDelay			0xf8000000
+#define bHSSI_T2RDelay			0xf80000
+/* Channel gain at continue TX. */
+#define bContTxHSSI			0x400
+#define bIGFromCCK			0x200
+#define bAGCAddress			0x3f
+#define bRxHPTx				0x7000
+#define bRxHPT2R			0x38000
+#define bRxHPCCKIni			0xc0000
+#define bAGCTxCode			0xc00000
+#define bAGCRxCode			0x300000
+#define b3WireDataLength		0x800
+#define b3WireAddressLength		0x400
+#define b3WireRFPowerDown		0x1
+/*#define bHWSISelect			0x8 */
+#define b5GPAPEPolarity			0x40000000
+#define b2GPAPEPolarity			0x80000000
+#define bRFSW_TxDefaultAnt		0x3
+#define bRFSW_TxOptionAnt		0x30
+#define bRFSW_RxDefaultAnt		0x300
+#define bRFSW_RxOptionAnt		0x3000
+#define bRFSI_3WireData			0x1
+#define bRFSI_3WireClock		0x2
+#define bRFSI_3WireLoad			0x4
+#define bRFSI_3WireRW			0x8
+/* 3-wire total control */
+#define bRFSI_3Wire			0xf
+#define bRFSI_RFENV			0x10
+#define bRFSI_TRSW			0x20
+#define bRFSI_TRSWB			0x40
+#define bRFSI_ANTSW			0x100
+#define bRFSI_ANTSWB			0x200
+#define bRFSI_PAPE			0x400
+#define bRFSI_PAPE5G			0x800
+#define bBandSelect			0x1
+#define bHTSIG2_GI			0x80
+#define bHTSIG2_Smoothing		0x01
+#define bHTSIG2_Sounding		0x02
+#define bHTSIG2_Aggreaton		0x08
+#define bHTSIG2_STBC			0x30
+#define bHTSIG2_AdvCoding		0x40
 #define bHTSIG2_NumOfHTLTF		0x300
-#define bHTSIG2_CRC8				0x3fc
-#define bHTSIG1_MCS				0x7f
-#define bHTSIG1_BandWidth			0x80
-#define bHTSIG1_HTLength			0xffff
-#define bLSIG_Rate					0xf
-#define bLSIG_Reserved				0x10
-#define bLSIG_Length				0x1fffe
-#define bLSIG_Parity					0x20
-#define bCCKRxPhase				0x4
-#define bLSSIReadAddress			0x3f000000
-#define bLSSIReadEdge				0x80000000
-#define bLSSIReadBackData			0xfff
-#define bLSSIReadOKFlag				0x1000
-#define bCCKSampleRate				0x8
-
-#define bRegulator0Standby			0x1
-#define bRegulatorPLLStandby			0x2
-#define bRegulator1Standby			0x4
-#define bPLLPowerUp				0x8
-#define bDPLLPowerUp				0x10
-#define bDA10PowerUp				0x20
-#define bAD7PowerUp				0x200
-#define bDA6PowerUp				0x2000
-#define bXtalPowerUp				0x4000
-#define b40MDClkPowerUP				0x8000
-#define bDA6DebugMode				0x20000
-#define bDA6Swing					0x380000
-#define bADClkPhase				0x4000000
-#define b80MClkDelay				0x18000000
-#define bAFEWatchDogEnable			0x20000000
-#define bXtalCap					0x0f000000
-#define bXtalCap01					0xc0000000
-#define bXtalCap23					0x3
-#define bXtalCap92x					0x0f000000
-#define bIntDifClkEnable			0x400
-#define bExtSigClkEnable			0x800
+#define bHTSIG2_CRC8			0x3fc
+#define bHTSIG1_MCS			0x7f
+#define bHTSIG1_BandWidth		0x80
+#define bHTSIG1_HTLength		0xffff
+#define bLSIG_Rate			0xf
+#define bLSIG_Reserved			0x10
+#define bLSIG_Length			0x1fffe
+#define bLSIG_Parity			0x20
+#define bCCKRxPhase			0x4
+#define bLSSIReadAddress		0x3f000000 /* LSSI "read" address */
+#define bLSSIReadEdge			0x80000000 /* LSSI "read" edge signal */
+#define bLSSIReadBackData		0xfff
+#define bLSSIReadOKFlag			0x1000
+#define bCCKSampleRate			0x8 /* 0: 44 MHz, 1: 88MHz */
+
+#define bRegulator0Standby		0x1
+#define bRegulatorPLLStandby		0x2
+#define bRegulator1Standby		0x4
+#define bPLLPowerUp			0x8
+#define bDPLLPowerUp			0x10
+#define bDA10PowerUp			0x20
+#define bAD7PowerUp			0x200
+#define bDA6PowerUp			0x2000
+#define bXtalPowerUp			0x4000
+#define b40MDClkPowerUP			0x8000
+#define bDA6DebugMode			0x20000
+#define bDA6Swing			0x380000
+#define bADClkPhase			0x4000000
+#define b80MClkDelay			0x18000000
+#define bAFEWatchDogEnable		0x20000000
+#define bXtalCap			0x0f000000
+#define bXtalCap01			0xc0000000
+#define bXtalCap23			0x3
+#define bXtalCap92x			0x0f000000
+#define bIntDifClkEnable		0x400
+#define bExtSigClkEnable		0x800
 #define bBandgapMbiasPowerUp		0x10000
-#define bAD11SHGain				0xc0000
-#define bAD11InputRange				0x700000
-#define bAD11OPCurrent				0x3800000
-#define bIPathLoopback				0x4000000
-#define bQPathLoopback				0x8000000
-#define bAFELoopback				0x10000000
-#define bDA10Swing				0x7e0
-#define bDA10Reverse				0x800
-#define bDAClkSource				0x1000
-#define bAD7InputRange				0x6000
-#define bAD7Gain					0x38000
-#define bAD7OutputCMMode			0x40000
-#define bAD7InputCMMode				0x380000
-#define bAD7Current					0xc00000
-#define bRegulatorAdjust			0x7000000
-#define bAD11PowerUpAtTx			0x1
-#define bDA10PSAtTx				0x10
-#define bAD11PowerUpAtRx			0x100
-#define bDA10PSAtRx				0x1000
-
-#define bCCKRxAGCFormat				0x200
-
-#define bPSDFFTSamplepPoint			0xc000
-#define bPSDAverageNum				0x3000
-#define bIQPathControl				0xc00
-#define bPSDFreq					0x3ff
-#define bPSDAntennaPath				0x30
-#define bPSDIQSwitch				0x40
-#define bPSDRxTrigger				0x400000
-#define bPSDTxTrigger				0x80000000
-#define bPSDSineToneScale			0x7f000000
-#define bPSDReport					0xffff
-
-#define bOFDMTxSC				0x30000000
-#define bCCKTxOn					0x1
-#define bOFDMTxOn				0x2
-#define bDebugPage				0xfff
-#define bDebugItem				0xff
-#define bAntL					0x10
-#define bAntNonHT					0x100
-#define bAntHT1					0x1000
-#define bAntHT2						0x10000
-#define bAntHT1S1					0x100000
-#define bAntNonHTS1				0x1000000
-
-#define bCCKBBMode				0x3
-#define bCCKTxPowerSaving			0x80
-#define bCCKRxPowerSaving			0x40
-#define bCCKSideBand				0x10
-#define bCCKScramble				0x8
-#define bCCKAntDiversity			0x8000
+#define bAD11SHGain			0xc0000
+#define bAD11InputRange			0x700000
+#define bAD11OPCurrent			0x3800000
+#define bIPathLoopback			0x4000000
+#define bQPathLoopback			0x8000000
+#define bAFELoopback			0x10000000
+#define bDA10Swing			0x7e0
+#define bDA10Reverse			0x800
+#define bDAClkSource			0x1000
+#define bAD7InputRange			0x6000
+#define bAD7Gain			0x38000
+#define bAD7OutputCMMode		0x40000
+#define bAD7InputCMMode			0x380000
+#define bAD7Current			0xc00000
+#define bRegulatorAdjust		0x7000000
+#define bAD11PowerUpAtTx		0x1
+#define bDA10PSAtTx			0x10
+#define bAD11PowerUpAtRx		0x100
+#define bDA10PSAtRx			0x1000
+
+#define bCCKRxAGCFormat			0x200
+
+#define bPSDFFTSamplepPoint		0xc000
+#define bPSDAverageNum			0x3000
+#define bIQPathControl			0xc00
+#define bPSDFreq			0x3ff
+#define bPSDAntennaPath			0x30
+#define bPSDIQSwitch			0x40
+#define bPSDRxTrigger			0x400000
+#define bPSDTxTrigger			0x80000000
+#define bPSDSineToneScale		0x7f000000
+#define bPSDReport			0xffff
+
+/* Page 8 */
+#define bOFDMTxSC			0x30000000
+#define bCCKTxOn			0x1
+#define bOFDMTxOn			0x2
+/* Reset debug page and also HWord, LWord */
+#define bDebugPage			0xfff
+/* Reset debug page and LWord */
+#define bDebugItem			0xff
+#define bAntL				0x10
+#define bAntNonHT			0x100
+#define bAntHT1				0x1000
+#define bAntHT2				0x10000
+#define bAntHT1S1			0x100000
+#define bAntNonHTS1			0x1000000
+
+/* Page a */
+#define bCCKBBMode			0x3
+#define bCCKTxPowerSaving		0x80
+#define bCCKRxPowerSaving		0x40
+#define bCCKSideBand			0x10
+#define bCCKScramble			0x8
+#define bCCKAntDiversity		0x8000
 #define bCCKCarrierRecovery		0x4000
-#define bCCKTxRate				0x3000
-#define bCCKDCCancel				0x0800
-#define bCCKISICancel				0x0400
-#define bCCKMatchFilter				0x0200
-#define bCCKEqualizer				0x0100
-#define bCCKPreambleDetect			0x800000
-#define bCCKFastFalseCCA			0x400000
-#define bCCKChEstStart				0x300000
-#define bCCKCCACount				0x080000
-#define bCCKcs_lim					0x070000
-#define bCCKBistMode				0x80000000
-#define bCCKCCAMask				0x40000000
+#define bCCKTxRate			0x3000
+#define bCCKDCCancel			0x0800
+#define bCCKISICancel			0x0400
+#define bCCKMatchFilter			0x0200
+#define bCCKEqualizer			0x0100
+#define bCCKPreambleDetect		0x800000
+#define bCCKFastFalseCCA		0x400000
+#define bCCKChEstStart			0x300000
+#define bCCKCCACount			0x080000
+#define bCCKcs_lim			0x070000
+#define bCCKBistMode			0x80000000
+#define bCCKCCAMask			0x40000000
 #define bCCKTxDACPhase			0x4
-#define bCCKRxADCPhase			0x20000000
+#define bCCKRxADCPhase			0x20000000 /* r_rx_clk */
 #define bCCKr_cp_mode0			0x0100
-#define bCCKTxDCOffset				0xf0
-#define bCCKRxDCOffset				0xf
-#define bCCKCCAMode				0xc000
-#define bCCKFalseCS_lim				0x3f00
-#define bCCKCS_ratio				0xc00000
-#define bCCKCorgBit_sel				0x300000
-#define bCCKPD_lim					0x0f0000
-#define bCCKNewCCA				0x80000000
-#define bCCKRxHPofIG				0x8000
-#define bCCKRxIG					0x7f00
-#define bCCKLNAPolarity				0x800000
-#define bCCKRx1stGain				0x7f0000
-#define bCCKRFExtend				0x20000000
-#define bCCKRxAGCSatLevel			0x1f000000
-#define bCCKRxAGCSatCount			0xe0
-#define bCCKRxRFSettle				0x1f
-#define bCCKFixedRxAGC				0x8000
-#define bCCKAntennaPolarity			0x2000
-#define bCCKTxFilterType			0x0c00
+#define bCCKTxDCOffset			0xf0
+#define bCCKRxDCOffset			0xf
+#define bCCKCCAMode			0xc000
+#define bCCKFalseCS_lim			0x3f00
+#define bCCKCS_ratio			0xc00000
+#define bCCKCorgBit_sel			0x300000
+#define bCCKPD_lim			0x0f0000
+#define bCCKNewCCA			0x80000000
+#define bCCKRxHPofIG			0x8000
+#define bCCKRxIG			0x7f00
+#define bCCKLNAPolarity			0x800000
+#define bCCKRx1stGain			0x7f0000
+/* CCK Rx Initial gain polarity */
+#define bCCKRFExtend			0x20000000
+#define bCCKRxAGCSatLevel		0x1f000000
+#define bCCKRxAGCSatCount		0xe0
+/* AGCSAmp_dly */
+#define bCCKRxRFSettle			0x1f
+#define bCCKFixedRxAGC			0x8000
+/*#define bCCKRxAGCFormat		0x4000  remove to HSSI register 0x824 */
+#define bCCKAntennaPolarity		0x2000
+#define bCCKTxFilterType		0x0c00
 #define bCCKRxAGCReportType		0x0300
-#define bCCKRxDAGCEn				0x80000000
-#define bCCKRxDAGCPeriod			0x20000000
+#define bCCKRxDAGCEn			0x80000000
+#define bCCKRxDAGCPeriod		0x20000000
 #define bCCKRxDAGCSatLevel		0x1f000000
-#define bCCKTimingRecovery			0x800000
-#define bCCKTxC0					0x3f0000
-#define bCCKTxC1					0x3f000000
-#define bCCKTxC2					0x3f
-#define bCCKTxC3					0x3f00
-#define bCCKTxC4					0x3f0000
-#define bCCKTxC5					0x3f000000
-#define bCCKTxC6					0x3f
-#define bCCKTxC7					0x3f00
-#define bCCKDebugPort				0xff0000
-#define bCCKDACDebug				0x0f000000
-#define bCCKFalseAlarmEnable			0x8000
-#define bCCKFalseAlarmRead			0x4000
-#define bCCKTRSSI					0x7f
-#define bCCKRxAGCReport				0xfe
-#define bCCKRxReport_AntSel			0x80000000
-#define bCCKRxReport_MFOff			0x40000000
+#define bCCKTimingRecovery		0x800000
+#define bCCKTxC0			0x3f0000
+#define bCCKTxC1			0x3f000000
+#define bCCKTxC2			0x3f
+#define bCCKTxC3			0x3f00
+#define bCCKTxC4			0x3f0000
+#define bCCKTxC5			0x3f000000
+#define bCCKTxC6			0x3f
+#define bCCKTxC7			0x3f00
+#define bCCKDebugPort			0xff0000
+#define bCCKDACDebug			0x0f000000
+#define bCCKFalseAlarmEnable		0x8000
+#define bCCKFalseAlarmRead		0x4000
+#define bCCKTRSSI			0x7f
+#define bCCKRxAGCReport			0xfe
+#define bCCKRxReport_AntSel		0x80000000
+#define bCCKRxReport_MFOff		0x40000000
 #define bCCKRxRxReport_SQLoss		0x20000000
-#define bCCKRxReport_Pktloss			0x10000000
+#define bCCKRxReport_Pktloss		0x10000000
 #define bCCKRxReport_Lockedbit		0x08000000
 #define bCCKRxReport_RateError		0x04000000
-#define bCCKRxReport_RxRate			0x03000000
+#define bCCKRxReport_RxRate		0x03000000
 #define bCCKRxFACounterLower		0xff
 #define bCCKRxFACounterUpper		0xff000000
-#define bCCKRxHPAGCStart			0xe000
-#define bCCKRxHPAGCFinal			0x1c00
+#define bCCKRxHPAGCStart		0xe000
+#define bCCKRxHPAGCFinal		0x1c00
 
 #define bCCKRxFalseAlarmEnable		0x8000
-#define bCCKFACounterFreeze			0x4000
-
-#define bCCKTxPathSel				0x10000000
-#define bCCKDefaultRxPath			0xc000000
-#define bCCKOptionRxPath			0x3000000
-
-#define bNumOfSTF					0x3
-#define bShift_L					0xc0
-#define bGI_TH						0xc
-#define bRxPathA					0x1
-#define bRxPathB					0x2
-#define bRxPathC					0x4
-#define bRxPathD					0x8
-#define bTxPathA					0x1
-#define bTxPathB					0x2
-#define bTxPathC					0x4
-#define bTxPathD					0x8
-#define bTRSSIFreq					0x200
-#define bADCBackoff					0x3000
-#define bDFIRBackoff					0xc000
-#define bTRSSILatchPhase			0x10000
-#define bRxIDCOffset					0xff
-#define bRxQDCOffset					0xff00
-#define bRxDFIRMode				0x1800000
-#define bRxDCNFType				0xe000000
-#define bRXIQImb_A					0x3ff
-#define bRXIQImb_B					0xfc00
-#define bRXIQImb_C					0x3f0000
-#define bRXIQImb_D					0xffc00000
-#define bDC_dc_Notch				0x60000
-#define bRxNBINotch					0x1f000000
-#define bPD_TH						0xf
-#define bPD_TH_Opt2				0xc000
-#define bPWED_TH					0x700
-#define bIfMF_Win_L					0x800
-#define bPD_Option					0x1000
-#define bMF_Win_L					0xe000
-#define bBW_Search_L				0x30000
-#define bwin_enh_L					0xc0000
-#define bBW_TH						0x700000
-#define bED_TH2						0x3800000
-#define bBW_option					0x4000000
-#define bRatio_TH					0x18000000
-#define bWindow_L					0xe0000000
-#define bSBD_Option					0x1
-#define bFrame_TH					0x1c
-#define bFS_Option					0x60
-#define bDC_Slope_check				0x80
-#define bFGuard_Counter_DC_L			0xe00
-#define bFrame_Weight_Short			0x7000
-#define bSub_Tune					0xe00000
-#define bFrame_DC_Length			0xe000000
-#define bSBD_start_offset			0x30000000
-#define bFrame_TH_2				0x7
-#define bFrame_GI2_TH				0x38
-#define bGI2_Sync_en				0x40
-#define bSarch_Short_Early			0x300
-#define bSarch_Short_Late			0xc00
-#define bSarch_GI2_Late				0x70000
-#define bCFOAntSum				0x1
-#define bCFOAcc						0x2
-#define bCFOStartOffset				0xc
-#define bCFOLookBack				0x70
-#define bCFOSumWeight				0x80
-#define bDAGCEnable					0x10000
-#define bTXIQImb_A					0x3ff
-#define bTXIQImb_B					0xfc00
-#define bTXIQImb_C					0x3f0000
-#define bTXIQImb_D					0xffc00000
-#define bTxIDCOffset					0xff
-#define bTxQDCOffset					0xff00
-#define bTxDFIRMode				0x10000
-#define bTxPesudoNoiseOn			0x4000000
-#define bTxPesudoNoise_A			0xff
-#define bTxPesudoNoise_B			0xff00
-#define bTxPesudoNoise_C			0xff0000
-#define bTxPesudoNoise_D			0xff000000
-#define bCCADropOption				0x20000
-#define bCCADropThres				0xfff00000
-#define bEDCCA_H					0xf
-#define bEDCCA_L					0xf0
-#define bLambda_ED               0x300
-#define bRxInitialGain           0x7f
-#define bRxAntDivEn              0x80
-#define bRxAGCAddressForLNA      0x7f00
-#define bRxHighPowerFlow         0x8000
-#define bRxAGCFreezeThres        0xc0000
-#define bRxFreezeStep_AGC1       0x300000
-#define bRxFreezeStep_AGC2       0xc00000
-#define bRxFreezeStep_AGC3       0x3000000
-#define bRxFreezeStep_AGC0       0xc000000
-#define bRxRssi_Cmp_En           0x10000000
-#define bRxQuickAGCEn            0x20000000
-#define bRxAGCFreezeThresMode    0x40000000
-#define bRxOverFlowCheckType     0x80000000
-#define bRxAGCShift              0x7f
-#define bTRSW_Tri_Only           0x80
-#define bPowerThres              0x300
-#define bRxAGCEn                 0x1
-#define bRxAGCTogetherEn         0x2
-#define bRxAGCMin                0x4
-#define bRxHP_Ini                0x7
-#define bRxHP_TRLNA              0x70
-#define bRxHP_RSSI               0x700
-#define bRxHP_BBP1               0x7000
-#define bRxHP_BBP2               0x70000
-#define bRxHP_BBP3               0x700000
-#define bRSSI_H                  0x7f0000
-#define bRSSI_Gen                0x7f000000
-#define bRxSettle_TRSW           0x7
-#define bRxSettle_LNA            0x38
-#define bRxSettle_RSSI           0x1c0
-#define bRxSettle_BBP            0xe00
-#define bRxSettle_RxHP           0x7000
-#define bRxSettle_AntSW_RSSI     0x38000
-#define bRxSettle_AntSW          0xc0000
-#define bRxProcessTime_DAGC      0x300000
-#define bRxSettle_HSSI           0x400000
-#define bRxProcessTime_BBPPW     0x800000
-#define bRxAntennaPowerShift     0x3000000
-#define bRSSITableSelect         0xc000000
-#define bRxHP_Final              0x7000000
-#define bRxHTSettle_BBP          0x7
-#define bRxHTSettle_HSSI         0x8
-#define bRxHTSettle_RxHP         0x70
-#define bRxHTSettle_BBPPW        0x80
-#define bRxHTSettle_Idle         0x300
-#define bRxHTSettle_Reserved     0x1c00
-#define bRxHTRxHPEn              0x8000
-#define bRxHTAGCFreezeThres      0x30000
-#define bRxHTAGCTogetherEn       0x40000
-#define bRxHTAGCMin              0x80000
-#define bRxHTAGCEn               0x100000
-#define bRxHTDAGCEn              0x200000
-#define bRxHTRxHP_BBP            0x1c00000
-#define bRxHTRxHP_Final          0xe0000000
-#define bRxPWRatioTH             0x3
-#define bRxPWRatioEn             0x4
-#define bRxMFHold                0x3800
-#define bRxPD_Delay_TH1          0x38
-#define bRxPD_Delay_TH2          0x1c0
-#define bRxPD_DC_COUNT_MAX       0x600
-#define bRxPD_Delay_TH           0x8000
-#define bRxProcess_Delay         0xf0000
-#define bRxSearchrange_GI2_Early 0x700000
-#define bRxFrame_Guard_Counter_L 0x3800000
-#define bRxSGI_Guard_L           0xc000000
-#define bRxSGI_Search_L          0x30000000
-#define bRxSGI_TH                0xc0000000
-#define bDFSCnt0                 0xff
-#define bDFSCnt1                 0xff00
-#define bDFSFlag                 0xf0000
-
-#define bMFWeightSum             0x300000
-#define bMinIdxTH                0x7f000000
-
-#define bDAFormat                0x40000
-
-#define bTxChEmuEnable           0x01000000
-
-#define bTRSWIsolation_A         0x7f
-#define bTRSWIsolation_B         0x7f00
-#define bTRSWIsolation_C         0x7f0000
-#define bTRSWIsolation_D         0x7f000000
-
-#define bExtLNAGain              0x7c00
-
-#define bSTBCEn                  0x4
-#define bAntennaMapping          0x10
-#define bNss                     0x20
-#define bCFOAntSumD              0x200
-#define bPHYCounterReset         0x8000000
-#define bCFOReportGet            0x4000000
-#define bOFDMContinueTx          0x10000000
-#define bOFDMSingleCarrier       0x20000000
-#define bOFDMSingleTone          0x40000000
-#define bHTDetect                0x100
-#define bCFOEn                   0x10000
-#define bCFOValue                0xfff00000
-#define bSigTone_Re              0x3f
-#define bSigTone_Im              0x7f00
-#define bCounter_CCA             0xffff
-#define bCounter_ParityFail      0xffff0000
-#define bCounter_RateIllegal     0xffff
-#define bCounter_CRC8Fail        0xffff0000
-#define bCounter_MCSNoSupport    0xffff
-#define bCounter_FastSync        0xffff
-#define bShortCFO                0xfff
-#define bShortCFOTLength         12
-#define bShortCFOFLength         11
-#define bLongCFO                 0x7ff
-#define bLongCFOTLength          11
-#define bLongCFOFLength          11
-#define bTailCFO                 0x1fff
-#define bTailCFOTLength          13
-#define bTailCFOFLength          12
-
-#define bmax_en_pwdB             0xffff
-#define bCC_power_dB             0xffff0000
-#define bnoise_pwdB              0xffff
-#define bPowerMeasTLength        10
-#define bPowerMeasFLength        3
-#define bRx_HT_BW                0x1
-#define bRxSC                    0x6
-#define bRx_HT                   0x8
-
-#define bNB_intf_det_on          0x1
-#define bIntf_win_len_cfg        0x30
-#define bNB_Intf_TH_cfg          0x1c0
-
-#define bRFGain                  0x3f
-#define bTableSel                0x40
-#define bTRSW                    0x80
-
-#define bRxSNR_A                 0xff
-#define bRxSNR_B                 0xff00
-#define bRxSNR_C                 0xff0000
-#define bRxSNR_D                 0xff000000
-#define bSNREVMTLength           8
-#define bSNREVMFLength           1
-
-#define bCSI1st                  0xff
-#define bCSI2nd                  0xff00
-#define bRxEVM1st                0xff0000
-#define bRxEVM2nd                0xff000000
-
-#define bSIGEVM                  0xff
-#define bPWDB                    0xff00
-#define bSGIEN                   0x10000
-
-#define bSFactorQAM1             0xf
-#define bSFactorQAM2             0xf0
-#define bSFactorQAM3             0xf00
-#define bSFactorQAM4             0xf000
-#define bSFactorQAM5             0xf0000
-#define bSFactorQAM6             0xf0000
-#define bSFactorQAM7             0xf00000
-#define bSFactorQAM8             0xf000000
-#define bSFactorQAM9             0xf0000000
-#define bCSIScheme               0x100000
-
-#define bNoiseLvlTopSet          0x3
-#define bChSmooth                0x4
-#define bChSmoothCfg1            0x38
-#define bChSmoothCfg2            0x1c0
-#define bChSmoothCfg3            0xe00
-#define bChSmoothCfg4            0x7000
-#define bMRCMode                 0x800000
-#define bTHEVMCfg                0x7000000
-
-#define bLoopFitType             0x1
-#define bUpdCFO                  0x40
-#define bUpdCFOOffData           0x80
-#define bAdvUpdCFO               0x100
-#define bAdvTimeCtrl             0x800
-#define bUpdClko                 0x1000
-#define bFC                      0x6000
-#define bTrackingMode            0x8000
-#define bPhCmpEnable             0x10000
-#define bUpdClkoLTF              0x20000
-#define bComChCFO                0x40000
-#define bCSIEstiMode             0x80000
-#define bAdvUpdEqz               0x100000
-#define bUChCfg                  0x7000000
-#define bUpdEqz                  0x8000000
-
-#define bTxAGCRate18_06			0x7f7f7f7f
-#define bTxAGCRate54_24			0x7f7f7f7f
+#define bCCKFACounterFreeze		0x4000
+
+#define bCCKTxPathSel			0x10000000
+#define bCCKDefaultRxPath		0xc000000
+#define bCCKOptionRxPath		0x3000000
+
+/* Page c */
+#define bNumOfSTF			0x3
+#define bShift_L			0xc0
+#define bGI_TH				0xc
+#define bRxPathA			0x1
+#define bRxPathB			0x2
+#define bRxPathC			0x4
+#define bRxPathD			0x8
+#define bTxPathA			0x1
+#define bTxPathB			0x2
+#define bTxPathC			0x4
+#define bTxPathD			0x8
+#define bTRSSIFreq			0x200
+#define bADCBackoff			0x3000
+#define bDFIRBackoff			0xc000
+#define bTRSSILatchPhase		0x10000
+#define bRxIDCOffset			0xff
+#define bRxQDCOffset			0xff00
+#define bRxDFIRMode			0x1800000
+#define bRxDCNFType			0xe000000
+#define bRXIQImb_A			0x3ff
+#define bRXIQImb_B			0xfc00
+#define bRXIQImb_C			0x3f0000
+#define bRXIQImb_D			0xffc00000
+#define bDC_dc_Notch			0x60000
+#define bRxNBINotch			0x1f000000
+#define bPD_TH				0xf
+#define bPD_TH_Opt2			0xc000
+#define bPWED_TH			0x700
+#define bIfMF_Win_L			0x800
+#define bPD_Option			0x1000
+#define bMF_Win_L			0xe000
+#define bBW_Search_L			0x30000
+#define bwin_enh_L			0xc0000
+#define bBW_TH				0x700000
+#define bED_TH2				0x3800000
+#define bBW_option			0x4000000
+#define bRatio_TH			0x18000000
+#define bWindow_L			0xe0000000
+#define bSBD_Option			0x1
+#define bFrame_TH			0x1c
+#define bFS_Option			0x60
+#define bDC_Slope_check			0x80
+#define bFGuard_Counter_DC_L		0xe00
+#define bFrame_Weight_Short		0x7000
+#define bSub_Tune			0xe00000
+#define bFrame_DC_Length		0xe000000
+#define bSBD_start_offset		0x30000000
+#define bFrame_TH_2			0x7
+#define bFrame_GI2_TH			0x38
+#define bGI2_Sync_en			0x40
+#define bSarch_Short_Early		0x300
+#define bSarch_Short_Late		0xc00
+#define bSarch_GI2_Late			0x70000
+#define bCFOAntSum			0x1
+#define bCFOAcc				0x2
+#define bCFOStartOffset			0xc
+#define bCFOLookBack			0x70
+#define bCFOSumWeight			0x80
+#define bDAGCEnable			0x10000
+#define bTXIQImb_A			0x3ff
+#define bTXIQImb_B			0xfc00
+#define bTXIQImb_C			0x3f0000
+#define bTXIQImb_D			0xffc00000
+#define bTxIDCOffset			0xff
+#define bTxQDCOffset			0xff00
+#define bTxDFIRMode			0x10000
+#define bTxPesudoNoiseOn		0x4000000
+#define bTxPesudoNoise_A		0xff
+#define bTxPesudoNoise_B		0xff00
+#define bTxPesudoNoise_C		0xff0000
+#define bTxPesudoNoise_D		0xff000000
+#define bCCADropOption			0x20000
+#define bCCADropThres			0xfff00000
+#define bEDCCA_H			0xf
+#define bEDCCA_L			0xf0
+#define bLambda_ED			0x300
+#define bRxInitialGain			0x7f
+#define bRxAntDivEn			0x80
+#define bRxAGCAddressForLNA		0x7f00
+#define bRxHighPowerFlow		0x8000
+#define bRxAGCFreezeThres		0xc0000
+#define bRxFreezeStep_AGC1		0x300000
+#define bRxFreezeStep_AGC2		0xc00000
+#define bRxFreezeStep_AGC3		0x3000000
+#define bRxFreezeStep_AGC0		0xc000000
+#define bRxRssi_Cmp_En			0x10000000
+#define bRxQuickAGCEn			0x20000000
+#define bRxAGCFreezeThresMode		0x40000000
+#define bRxOverFlowCheckType		0x80000000
+#define bRxAGCShift			0x7f
+#define bTRSW_Tri_Only			0x80
+#define bPowerThres			0x300
+#define bRxAGCEn			0x1
+#define bRxAGCTogetherEn		0x2
+#define bRxAGCMin			0x4
+#define bRxHP_Ini			0x7
+#define bRxHP_TRLNA			0x70
+#define bRxHP_RSSI			0x700
+#define bRxHP_BBP1			0x7000
+#define bRxHP_BBP2			0x70000
+#define bRxHP_BBP3			0x700000
+/* The threshold for high power */
+#define bRSSI_H				0x7f0000
+/* The threshold for ant diversity */
+#define bRSSI_Gen			0x7f000000
+#define bRxSettle_TRSW			0x7
+#define bRxSettle_LNA			0x38
+#define bRxSettle_RSSI			0x1c0
+#define bRxSettle_BBP			0xe00
+#define bRxSettle_RxHP			0x7000
+#define bRxSettle_AntSW_RSSI		0x38000
+#define bRxSettle_AntSW			0xc0000
+#define bRxProcessTime_DAGC		0x300000
+#define bRxSettle_HSSI			0x400000
+#define bRxProcessTime_BBPPW		0x800000
+#define bRxAntennaPowerShift		0x3000000
+#define bRSSITableSelect		0xc000000
+#define bRxHP_Final			0x7000000
+#define bRxHTSettle_BBP			0x7
+#define bRxHTSettle_HSSI		0x8
+#define bRxHTSettle_RxHP		0x70
+#define bRxHTSettle_BBPPW		0x80
+#define bRxHTSettle_Idle		0x300
+#define bRxHTSettle_Reserved		0x1c00
+#define bRxHTRxHPEn			0x8000
+#define bRxHTAGCFreezeThres		0x30000
+#define bRxHTAGCTogetherEn		0x40000
+#define bRxHTAGCMin			0x80000
+#define bRxHTAGCEn			0x100000
+#define bRxHTDAGCEn			0x200000
+#define bRxHTRxHP_BBP			0x1c00000
+#define bRxHTRxHP_Final			0xe0000000
+#define bRxPWRatioTH			0x3
+#define bRxPWRatioEn			0x4
+#define bRxMFHold			0x3800
+#define bRxPD_Delay_TH1			0x38
+#define bRxPD_Delay_TH2			0x1c0
+#define bRxPD_DC_COUNT_MAX		0x600
+/*#define bRxMF_Hold			0x3800*/
+#define bRxPD_Delay_TH			0x8000
+#define bRxProcess_Delay		0xf0000
+#define bRxSearchrange_GI2_Early	0x700000
+#define bRxFrame_Guard_Counter_L	0x3800000
+#define bRxSGI_Guard_L			0xc000000
+#define bRxSGI_Search_L			0x30000000
+#define bRxSGI_TH			0xc0000000
+#define bDFSCnt0			0xff
+#define bDFSCnt1			0xff00
+#define bDFSFlag			0xf0000
+
+#define bMFWeightSum		0x300000
+#define bMinIdxTH		0x7f000000
+
+#define bDAFormat		0x40000
+
+#define bTxChEmuEnable		0x01000000
+
+#define bTRSWIsolation_A	0x7f
+#define bTRSWIsolation_B	0x7f00
+#define bTRSWIsolation_C	0x7f0000
+#define bTRSWIsolation_D	0x7f000000
+
+#define bExtLNAGain		0x7c00
+
+/* Page d */
+#define bSTBCEn			0x4
+#define bAntennaMapping		0x10
+#define bNss			0x20
+#define bCFOAntSumD		0x200
+#define bPHYCounterReset	0x8000000
+#define bCFOReportGet		0x4000000
+#define bOFDMContinueTx		0x10000000
+#define bOFDMSingleCarrier	0x20000000
+#define bOFDMSingleTone		0x40000000
+/* #define bRxPath1		0x01
+ * #define bRxPath2		0x02
+ * #define bRxPath3		0x04
+ * #define bRxPath4		0x08
+ * #define bTxPath1		0x10
+ * #define bTxPath2		0x20
+*/
+#define bHTDetect		0x100
+#define bCFOEn			0x10000
+#define bCFOValue		0xfff00000
+#define bSigTone_Re		0x3f
+#define bSigTone_Im		0x7f00
+#define bCounter_CCA		0xffff
+#define bCounter_ParityFail	0xffff0000
+#define bCounter_RateIllegal	0xffff
+#define bCounter_CRC8Fail	0xffff0000
+#define bCounter_MCSNoSupport	0xffff
+#define bCounter_FastSync	0xffff
+#define bShortCFO		0xfff
+#define bShortCFOTLength	12 /* total */
+#define bShortCFOFLength	11 /* fraction */
+#define bLongCFO		0x7ff
+#define bLongCFOTLength		11
+#define bLongCFOFLength		11
+#define bTailCFO		0x1fff
+#define bTailCFOTLength		13
+#define bTailCFOFLength		12
+
+#define bmax_en_pwdB		0xffff
+#define bCC_power_dB		0xffff0000
+#define bnoise_pwdB		0xffff
+#define bPowerMeasTLength	10
+#define bPowerMeasFLength	3
+#define bRx_HT_BW		0x1
+#define bRxSC			0x6
+#define bRx_HT			0x8
+
+#define bNB_intf_det_on		0x1
+#define bIntf_win_len_cfg	0x30
+#define bNB_Intf_TH_cfg		0x1c0
+
+#define bRFGain			0x3f
+#define bTableSel		0x40
+#define bTRSW			0x80
+
+#define bRxSNR_A		0xff
+#define bRxSNR_B		0xff00
+#define bRxSNR_C		0xff0000
+#define bRxSNR_D		0xff000000
+#define bSNREVMTLength		8
+#define bSNREVMFLength		1
+
+#define bCSI1st			0xff
+#define bCSI2nd			0xff00
+#define bRxEVM1st		0xff0000
+#define bRxEVM2nd		0xff000000
+
+#define bSIGEVM			0xff
+#define bPWDB			0xff00
+#define bSGIEN			0x10000
+
+#define bSFactorQAM1		0xf
+#define bSFactorQAM2		0xf0
+#define bSFactorQAM3		0xf00
+#define bSFactorQAM4		0xf000
+#define bSFactorQAM5		0xf0000
+#define bSFactorQAM6		0xf0000
+#define bSFactorQAM7		0xf00000
+#define bSFactorQAM8		0xf000000
+#define bSFactorQAM9		0xf0000000
+#define bCSIScheme		0x100000
+
+#define bNoiseLvlTopSet		0x3
+#define bChSmooth		0x4
+#define bChSmoothCfg1		0x38
+#define bChSmoothCfg2		0x1c0
+#define bChSmoothCfg3		0xe00
+#define bChSmoothCfg4		0x7000
+#define bMRCMode		0x800000
+#define bTHEVMCfg		0x7000000
+
+#define bLoopFitType		0x1
+#define bUpdCFO			0x40
+#define bUpdCFOOffData		0x80
+#define bAdvUpdCFO		0x100
+#define bAdvTimeCtrl		0x800
+#define bUpdClko		0x1000
+#define bFC			0x6000
+#define bTrackingMode		0x8000
+#define bPhCmpEnable		0x10000
+#define bUpdClkoLTF		0x20000
+#define bComChCFO		0x40000
+#define bCSIEstiMode		0x80000
+#define bAdvUpdEqz		0x100000
+#define bUChCfg			0x7000000
+#define bUpdEqz			0x8000000
+
+/* Page e */
+#define bTxAGCRate18_06		0x7f7f7f7f
+#define bTxAGCRate54_24		0x7f7f7f7f
 #define bTxAGCRateMCS32		0x7f
-#define bTxAGCRateCCK			0x7f00
+#define bTxAGCRateCCK		0x7f00
 #define bTxAGCRateMCS3_MCS0	0x7f7f7f7f
 #define bTxAGCRateMCS7_MCS4	0x7f7f7f7f
 #define bTxAGCRateMCS11_MCS8	0x7f7f7f7f
 #define bTxAGCRateMCS15_MCS12	0x7f7f7f7f
 
-
-#define bRxPesudoNoiseOn         0x20000000
-#define bRxPesudoNoise_A         0xff
-#define bRxPesudoNoise_B         0xff00
-#define bRxPesudoNoise_C         0xff0000
-#define bRxPesudoNoise_D         0xff000000
-#define bPesudoNoiseState_A      0xffff
-#define bPesudoNoiseState_B      0xffff0000
-#define bPesudoNoiseState_C      0xffff
-#define bPesudoNoiseState_D      0xffff0000
-
-#define bZebra1_HSSIEnable        0x8
-#define bZebra1_TRxControl        0xc00
-#define bZebra1_TRxGainSetting    0x07f
-#define bZebra1_RxCorner          0xc00
-#define bZebra1_TxChargePump      0x38
-#define bZebra1_RxChargePump      0x7
-#define bZebra1_ChannelNum        0xf80
-#define bZebra1_TxLPFBW           0x400
-#define bZebra1_RxLPFBW           0x600
-
-#define bRTL8256RegModeCtrl1      0x100
-#define bRTL8256RegModeCtrl0      0x40
-#define bRTL8256_TxLPFBW          0x18
-#define bRTL8256_RxLPFBW          0x600
-
-#define bRTL8258_TxLPFBW          0xc
-#define bRTL8258_RxLPFBW          0xc00
-#define bRTL8258_RSSILPFBW        0xc0
-
-#define bByte0                    0x1
-#define bByte1                    0x2
-#define bByte2                    0x4
-#define bByte3                    0x8
-#define bWord0                    0x3
-#define bWord1                    0xc
-#define bDWord                    0xf
-
-#define bMaskByte0                0xff
-#define bMaskByte1                0xff00
-#define bMaskByte2                0xff0000
-#define bMaskByte3                0xff000000
-#define bMaskHWord                0xffff0000
-#define bMaskLWord                0x0000ffff
-#define bMaskDWord                0xffffffff
-
-#define bMask12Bits               0xfff
-
-#define bEnable                   0x1
-#define bDisable                  0x0
-
-#define LeftAntenna               0x0
-#define RightAntenna              0x1
-
-#define tCheckTxStatus            500
-#define tUpdateRxCounter          100
-
-#define rateCCK     0
-#define rateOFDM    1
-#define rateHT      2
-
-#define bPMAC_End                 0x1ff
-#define bFPGAPHY0_End             0x8ff
-#define bFPGAPHY1_End             0x9ff
-#define bCCKPHY0_End              0xaff
-#define bOFDMPHY0_End             0xcff
-#define bOFDMPHY1_End             0xdff
-
-
-#define bPMACControl              0x0
-#define bWMACControl              0x1
-#define bWNICControl              0x2
-
-#define PathA                     0x0
-#define PathB                     0x1
-#define PathC                     0x2
-#define PathD                     0x3
-
-#define	rRTL8256RxMixerPole		0xb
-#define		bZebraRxMixerPole		0x6
-#define		rRTL8256TxBBOPBias        0x9
-#define		bRTL8256TxBBOPBias       0x400
-#define		rRTL8256TxBBBW             19
-#define		bRTL8256TxBBBW			0x18
+#define bRxPesudoNoiseOn	0x20000000 /* Rx Pseduo noise */
+#define bRxPesudoNoise_A	0xff
+#define bRxPesudoNoise_B	0xff00
+#define bRxPesudoNoise_C	0xff0000
+#define bRxPesudoNoise_D	0xff000000
+#define bPesudoNoiseState_A	0xffff
+#define bPesudoNoiseState_B	0xffff0000
+#define bPesudoNoiseState_C	0xffff
+#define bPesudoNoiseState_D	0xffff0000
+
+/* RF Zebra 1 */
+#define bZebra1_HSSIEnable	0x8
+#define bZebra1_TRxControl	0xc00
+#define bZebra1_TRxGainSetting	0x07f
+#define bZebra1_RxCorner	0xc00
+#define bZebra1_TxChargePump	0x38
+#define bZebra1_RxChargePump	0x7
+#define bZebra1_ChannelNum	0xf80
+#define bZebra1_TxLPFBW	0x400
+#define bZebra1_RxLPFBW	0x600
+
+/* Zebra4 */
+#define bRTL8256RegModeCtrl1	0x100
+#define bRTL8256RegModeCtrl0	0x40
+#define bRTL8256_TxLPFBW	0x18
+#define bRTL8256_RxLPFBW	0x600
+
+/* RTL8258 */
+#define bRTL8258_TxLPFBW	0xc
+#define bRTL8258_RxLPFBW	0xc00
+#define bRTL8258_RSSILPFBW	0xc0
+
+/* byte enable for sb_write */
+#define bByte0	0x1
+#define bByte1	0x2
+#define bByte2	0x4
+#define bByte3	0x8
+#define bWord0	0x3
+#define bWord1	0xc
+#define bDWord	0xf
+
+/* for PutRegsetting & GetRegSetting BitMask */
+#define bMaskByte0	0xff
+#define bMaskByte1	0xff00
+#define bMaskByte2	0xff0000
+#define bMaskByte3	0xff000000
+#define bMaskHWord	0xffff0000
+#define bMaskLWord	0x0000ffff
+#define bMaskDWord	0xffffffff
+
+/* for PutRFRegsetting & GetRFRegSetting BitMask */
+#define bMask12Bits	0xfff
+
+#define bEnable		0x1
+#define bDisable	0x0
+
+#define LeftAntenna	0x0
+#define RightAntenna	0x1
+
+#define tCheckTxStatus		500 /* 500 ms */
+#define tUpdateRxCounter	100 /* 100 ms */
+
+#define rateCCK		0
+#define rateOFDM	1
+#define rateHT		2
+
+#define bPMAC_End	0x1ff /* define Register-End */
+#define bFPGAPHY0_End	0x8ff
+#define bFPGAPHY1_End	0x9ff
+#define bCCKPHY0_End	0xaff
+#define bOFDMPHY0_End	0xcff
+#define bOFDMPHY1_End	0xdff
+
+
+#define bPMACControl	0x0
+#define bWMACControl	0x1
+#define bWNICControl	0x2
+
+#define PathA	0x0
+#define PathB	0x1
+#define PathC	0x2
+#define PathD	0x3
+
+#define rRTL8256RxMixerPole	0xb
+#define bZebraRxMixerPole	0x6
+#define rRTL8256TxBBOPBias	0x9
+#define bRTL8256TxBBOPBias	0x400
+#define rRTL8256TxBBBW		19
+#define bRTL8256TxBBBW		0x18
 
 #endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h b/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
deleted file mode 100644
index 03eee3d059c6..000000000000
--- a/drivers/staging/rtl8192e/rtl8192e/r819xE_phyreg.h
+++ /dev/null
@@ -1,908 +0,0 @@
-#ifndef _R819XU_PHYREG_H
-#define _R819XU_PHYREG_H
-
-
-#define   RF_DATA				0x1d4					// FW will write RF data in the register.
-
-//Register   //duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF
-//page 1
-#define rPMAC_Reset               		0x100
-#define rPMAC_TxStart             		0x104
-#define rPMAC_TxLegacySIG         		0x108
-#define rPMAC_TxHTSIG1            		0x10c
-#define rPMAC_TxHTSIG2            		0x110
-#define rPMAC_PHYDebug            		0x114
-#define rPMAC_TxPacketNum         		0x118
-#define rPMAC_TxIdle              		0x11c
-#define rPMAC_TxMACHeader0       	0x120
-#define rPMAC_TxMACHeader1       	0x124
-#define rPMAC_TxMACHeader2       	0x128
-#define rPMAC_TxMACHeader3       	0x12c
-#define rPMAC_TxMACHeader4       	0x130
-#define rPMAC_TxMACHeader5       	0x134
-#define rPMAC_TxDataType          		0x138
-#define rPMAC_TxRandomSeed      		0x13c
-#define rPMAC_CCKPLCPPreamble  		0x140
-#define rPMAC_CCKPLCPHeader     		0x144
-#define rPMAC_CCKCRC16            		0x148
-#define rPMAC_OFDMRxCRC32OK  		0x170
-#define rPMAC_OFDMRxCRC32Er   		0x174
-#define rPMAC_OFDMRxParityEr    		0x178
-#define rPMAC_OFDMRxCRC8Er     		0x17c
-#define rPMAC_CCKCRxRC16Er       		0x180
-#define rPMAC_CCKCRxRC32Er       		0x184
-#define rPMAC_CCKCRxRC32OK      		0x188
-#define rPMAC_TxStatus            		0x18c
-
-//90P
-#define	MCS_TXAGC				0x340	// MCS AGC
-#define	CCK_TXAGC				0x348	// CCK AGC
-
-#define	MacBlkCtrl				0x403					// Mac block on/off control register
-
-//page8
-#define rFPGA0_RFMOD              		0x800  //RF mode & CCK TxSC
-#define rFPGA0_TxInfo             		0x804
-#define rFPGA0_PSDFunction        		0x808
-#define rFPGA0_TxGainStage        		0x80c
-#define rFPGA0_RFTiming1          		0x810
-#define rFPGA0_RFTiming2          		0x814
-//#define rFPGA0_XC_RFTiming        		0x818
-//#define rFPGA0_XD_RFTiming        		0x81c
-#define rFPGA0_XA_HSSIParameter1  	0x820
-#define rFPGA0_XA_HSSIParameter2  	0x824
-#define rFPGA0_XB_HSSIParameter1  	0x828
-#define rFPGA0_XB_HSSIParameter2  	0x82c
-#define rFPGA0_XC_HSSIParameter1  	0x830
-#define rFPGA0_XC_HSSIParameter2  	0x834
-#define rFPGA0_XD_HSSIParameter1  	0x838
-#define rFPGA0_XD_HSSIParameter2  	0x83c
-#define rFPGA0_XA_LSSIParameter   	0x840
-#define rFPGA0_XB_LSSIParameter   	0x844
-#define rFPGA0_XC_LSSIParameter   	0x848
-#define rFPGA0_XD_LSSIParameter   	0x84c
-#define rFPGA0_RFWakeUpParameter  	0x850
-#define rFPGA0_RFSleepUpParameter 	0x854
-#define rFPGA0_XAB_SwitchControl  	0x858
-#define rFPGA0_XCD_SwitchControl  	0x85c
-#define rFPGA0_XA_RFInterfaceOE   	0x860
-#define rFPGA0_XB_RFInterfaceOE   	0x864
-#define rFPGA0_XC_RFInterfaceOE   	0x868
-#define rFPGA0_XD_RFInterfaceOE   	0x86c
-#define rFPGA0_XAB_RFInterfaceSW  	0x870
-#define rFPGA0_XCD_RFInterfaceSW  	0x874
-#define rFPGA0_XAB_RFParameter    	0x878
-#define rFPGA0_XCD_RFParameter    	0x87c
-#define rFPGA0_AnalogParameter1   	0x880
-#define rFPGA0_AnalogParameter2   	0x884
-#define rFPGA0_AnalogParameter3   	0x888
-#define rFPGA0_AnalogParameter4   	0x88c
-#define rFPGA0_XA_LSSIReadBack    	0x8a0
-#define rFPGA0_XB_LSSIReadBack    	0x8a4
-#define rFPGA0_XC_LSSIReadBack    	0x8a8
-#define rFPGA0_XD_LSSIReadBack    	0x8ac
-#define rFPGA0_PSDReport		0x8b4
-#define rFPGA0_XAB_RFInterfaceRB  	0x8e0
-#define rFPGA0_XCD_RFInterfaceRB  	0x8e4
-
-/* Page 9 - RF mode & OFDM TxSC */
-#define rFPGA1_RFMOD      		0x900
-#define rFPGA1_TxBlock            	0x904
-#define rFPGA1_DebugSelect        	0x908
-#define rFPGA1_TxInfo             	0x90c
-
-/* Page a */
-#define rCCK0_System              	0xa00
-#define rCCK0_AFESetting          	0xa04
-#define rCCK0_CCA                 	0xa08
-/* AGC default value, saturation level */
-#define rCCK0_RxAGC1              	0xa0c
-/* AGC & DAGC */
-#define rCCK0_RxAGC2              	0xa10
-#define rCCK0_RxHP                	0xa14
-/* Timing recovery & channel estimation threshold */
-#define rCCK0_DSPParameter1       	0xa18
-/* SQ threshold */
-#define rCCK0_DSPParameter2       	0xa1c
-#define rCCK0_TxFilter1           	0xa20
-#define rCCK0_TxFilter2           	0xa24
-/* Debug port and TX filter 3 */
-#define rCCK0_DebugPort           	0xa28
-#define rCCK0_FalseAlarmReport    	0xa2c
-#define rCCK0_TRSSIReport         	0xa50
-#define rCCK0_RxReport            	0xa54
-#define rCCK0_FACounterLower      	0xa5c
-#define rCCK0_FACounterUpper      	0xa58
-
-/* Page c */
-#define rOFDM0_LSTF              	0xc00
-#define rOFDM0_TRxPathEnable      	0xc04
-#define rOFDM0_TRMuxPar           	0xc08
-#define rOFDM0_TRSWIsolation      	0xc0c
-/* RxIQ DC offset, Rx digital filter, DC notch filter */
-#define rOFDM0_XARxAFE            	0xc10
-/* RxIQ imblance matrix */
-#define rOFDM0_XARxIQImbalance    	0xc14
-#define rOFDM0_XBRxAFE            	0xc18
-#define rOFDM0_XBRxIQImbalance    	0xc1c
-#define rOFDM0_XCRxAFE           	0xc20
-#define rOFDM0_XCRxIQImbalance    	0xc24
-#define rOFDM0_XDRxAFE            	0xc28
-#define rOFDM0_XDRxIQImbalance    	0xc2c
-/* PD, BW & SBD */
-#define rOFDM0_RxDetector1        	0xc30
-/* SBD */
-#define rOFDM0_RxDetector2		0xc34
-/* Frame Sync */
-#define rOFDM0_RxDetector3         	0xc38
-/* PD, SBD, Frame Sync & Short-GI */
-#define rOFDM0_RxDetector4        	0xc3c
-/* Rx Sync Path */
-#define rOFDM0_RxDSP			0xc40
-/* CFO & DAGC */
-#define rOFDM0_CFOandDAGC         	0xc44
-/* CCA Drop threshold */
-#define rOFDM0_CCADropThreshold   	0xc48
-/* Energy CCA */
-#define rOFDM0_ECCAThreshold      	0xc4c
-#define rOFDM0_XAAGCCore1         	0xc50
-#define rOFDM0_XAAGCCore2         	0xc54
-#define rOFDM0_XBAGCCore1         	0xc58
-#define rOFDM0_XBAGCCore2         	0xc5c
-#define rOFDM0_XCAGCCore1         	0xc60
-#define rOFDM0_XCAGCCore2         	0xc64
-#define rOFDM0_XDAGCCore1         	0xc68
-#define rOFDM0_XDAGCCore2         	0xc6c
-#define rOFDM0_AGCParameter1      	0xc70
-#define rOFDM0_AGCParameter2      	0xc74
-#define rOFDM0_AGCRSSITable       	0xc78
-#define rOFDM0_HTSTFAGC           	0xc7c
-#define rOFDM0_XATxIQImbalance   	0xc80
-#define rOFDM0_XATxAFE            	0xc84
-#define rOFDM0_XBTxIQImbalance    	0xc88
-#define rOFDM0_XBTxAFE            	0xc8c
-#define rOFDM0_XCTxIQImbalance    	0xc90
-#define rOFDM0_XCTxAFE            	0xc94
-#define rOFDM0_XDTxIQImbalance    	0xc98
-#define rOFDM0_XDTxAFE            	0xc9c
-#define rOFDM0_RxHPParameter      	0xce0
-#define rOFDM0_TxPseudoNoiseWgt   	0xce4
-#define rOFDM0_FrameSync          	0xcf0
-#define rOFDM0_DFSReport          	0xcf4
-#define rOFDM0_TxCoeff1           	0xca4
-#define rOFDM0_TxCoeff2           	0xca8
-#define rOFDM0_TxCoeff3           	0xcac
-#define rOFDM0_TxCoeff4           	0xcb0
-#define rOFDM0_TxCoeff5           	0xcb4
-#define rOFDM0_TxCoeff6           	0xcb8
-
-
-/* Page d */
-#define rOFDM1_LSTF               	0xd00
-#define rOFDM1_TRxPathEnable      	0xd04
-#define rOFDM1_CFO                	0xd08
-#define rOFDM1_CSI1               	0xd10
-#define rOFDM1_SBD                	0xd14
-#define rOFDM1_CSI2               	0xd18
-#define rOFDM1_CFOTracking        	0xd2c
-#define rOFDM1_TRxMesaure1        	0xd34
-#define rOFDM1_IntfDet            	0xd3c
-#define rOFDM1_PseudoNoiseStateAB 	0xd50
-#define rOFDM1_PseudoNoiseStateCD 	0xd54
-#define rOFDM1_RxPseudoNoiseWgt   	0xd58
-/* cca, parity fail */
-#define rOFDM_PHYCounter1         	0xda0
-/* rate illegal, crc8 fail */
-#define rOFDM_PHYCounter2   		0xda4
-/* MCS not supported */
-#define rOFDM_PHYCounter3         	0xda8
-#define rOFDM_ShortCFOAB          	0xdac
-#define rOFDM_ShortCFOCD          	0xdb0
-#define rOFDM_LongCFOAB           	0xdb4
-#define rOFDM_LongCFOCD           	0xdb8
-#define rOFDM_TailCFOAB           	0xdbc
-#define rOFDM_TailCFOCD           	0xdc0
-#define rOFDM_PWMeasure1          	0xdc4
-#define rOFDM_PWMeasure2          	0xdc8
-#define rOFDM_BWReport            	0xdcc
-#define rOFDM_AGCReport           	0xdd0
-#define rOFDM_RxSNR               	0xdd4
-#define rOFDM_RxEVMCSI            	0xdd8
-#define rOFDM_SIGReport           	0xddc
-
-/* Page e */
-#define rTxAGC_Rate18_06		0xe00
-#define rTxAGC_Rate54_24		0xe04
-#define rTxAGC_CCK_Mcs32		0xe08
-#define rTxAGC_Mcs03_Mcs00		0xe10
-#define rTxAGC_Mcs07_Mcs04		0xe14
-#define rTxAGC_Mcs11_Mcs08		0xe18
-#define rTxAGC_Mcs15_Mcs12		0xe1c
-
-
-/* RF Zebra 1 */
-#define rZebra1_HSSIEnable            	0x0
-#define rZebra1_TRxEnable1            	0x1
-#define rZebra1_TRxEnable2           	0x2
-#define rZebra1_AGC                   	0x4
-#define rZebra1_ChargePump            	0x5
-#define rZebra1_Channel               	0x7
-#define rZebra1_TxGain               	0x8
-#define rZebra1_TxLPF                 	0x9
-#define rZebra1_RxLPF                 	0xb
-#define rZebra1_RxHPFCorner           	0xc
-
-/* Zebra 4 */
-#define rGlobalCtrl                   	0
-#define rRTL8256_TxLPF                	19
-#define rRTL8256_RxLPF                	11
-
-/* RTL8258 */
-#define rRTL8258_TxLPF                	0x11
-#define rRTL8258_RxLPF                  0x13
-#define rRTL8258_RSSILPF              	0xa
-
-/* Bit Mask */
-/* Page 1 */
-#define bBBResetB                 	0x100
-#define bGlobalResetB             	0x200
-#define bOFDMTxStart              	0x4
-#define bCCKTxStart               	0x8
-#define bCRC32Debug               	0x100
-#define bPMACLoopback             	0x10
-#define bTxLSIG                   	0xffffff
-#define bOFDMTxRate               	0xf
-#define bOFDMTxReserved           	0x10
-#define bOFDMTxLength             	0x1ffe0
-#define bOFDMTxParity             	0x20000
-#define bTxHTSIG1                 	0xffffff
-#define bTxHTMCSRate              	0x7f
-#define bTxHTBW                   	0x80
-#define bTxHTLength               	0xffff00
-#define bTxHTSIG2                 	0xffffff
-#define bTxHTSmoothing            	0x1
-#define bTxHTSounding             	0x2
-#define bTxHTReserved             	0x4
-#define bTxHTAggreation           	0x8
-#define bTxHTSTBC                 	0x30
-#define bTxHTAdvanceCoding        	0x40
-#define bTxHTShortGI              	0x80
-#define bTxHTNumberHT_LTF         	0x300
-#define bTxHTCRC8                 	0x3fc00
-#define bCounterReset             	0x10000
-#define bNumOfOFDMTx              	0xffff
-#define bNumOfCCKTx               	0xffff0000
-#define bTxIdleInterval           	0xffff
-#define bOFDMService              	0xffff0000
-#define bTxMACHeader              	0xffffffff
-#define bTxDataInit               	0xff
-#define bTxHTMode                 	0x100
-#define bTxDataType               	0x30000
-#define bTxRandomSeed             	0xffffffff
-#define bCCKTxPreamble           	0x1
-#define bCCKTxSFD                 	0xffff0000
-#define bCCKTxSIG                 	0xff
-#define bCCKTxService             	0xff00
-#define bCCKLengthExt             	0x8000
-#define bCCKTxLength              	0xffff0000
-#define bCCKTxCRC16               	0xffff
-#define bCCKTxStatus              	0x1
-#define bOFDMTxStatus             	0x2
-
-/* Page 8 */
-#define bRFMOD                    	0x1
-#define bJapanMode                	0x2
-#define bCCKTxSC                  	0x30
-#define bCCKEn                    	0x1000000
-#define bOFDMEn                   	0x2000000
-#define bOFDMRxADCPhase           	0x10000
-#define bOFDMTxDACPhase           	0x40000
-#define bXATxAGC                  	0x3f
-#define bXBTxAGC                  	0xf00
-#define bXCTxAGC                  	0xf000
-#define bXDTxAGC                  	0xf0000
-#define bPAStart                  	0xf0000000
-#define bTRStart                  	0x00f00000
-#define bRFStart                  	0x0000f000
-#define bBBStart                  	0x000000f0
-#define bBBCCKStart               	0x0000000f
-/* Reg x814 */
-#define bPAEnd                    	0xf
-#define bTREnd                    	0x0f000000
-#define bRFEnd                    	0x000f0000
-/* T2R */
-#define bCCAMask                  	0x000000f0
-#define bR2RCCAMask               	0x00000f00
-#define bHSSI_R2TDelay            	0xf8000000
-#define bHSSI_T2RDelay            	0xf80000
-/* Channel gain at continue TX. */
-#define bContTxHSSI               	0x400
-#define bIGFromCCK                	0x200
-#define bAGCAddress               	0x3f
-#define bRxHPTx                   	0x7000
-#define bRxHPT2R                  	0x38000
-#define bRxHPCCKIni               	0xc0000
-#define bAGCTxCode                	0xc00000
-#define bAGCRxCode                	0x300000
-#define b3WireDataLength          	0x800
-#define b3WireAddressLength       	0x400
-#define b3WireRFPowerDown         	0x1
-/*#define bHWSISelect               	0x8 */
-#define b5GPAPEPolarity           	0x40000000
-#define b2GPAPEPolarity           	0x80000000
-#define bRFSW_TxDefaultAnt        	0x3
-#define bRFSW_TxOptionAnt         	0x30
-#define bRFSW_RxDefaultAnt        	0x300
-#define bRFSW_RxOptionAnt         	0x3000
-#define bRFSI_3WireData           	0x1
-#define bRFSI_3WireClock          	0x2
-#define bRFSI_3WireLoad           	0x4
-#define bRFSI_3WireRW             	0x8
-/* 3-wire total control */
-#define bRFSI_3Wire               	0xf
-#define bRFSI_RFENV               	0x10
-#define bRFSI_TRSW                	0x20
-#define bRFSI_TRSWB               	0x40
-#define bRFSI_ANTSW               	0x100
-#define bRFSI_ANTSWB              	0x200
-#define bRFSI_PAPE                	0x400
-#define bRFSI_PAPE5G              	0x800
-#define bBandSelect               	0x1
-#define bHTSIG2_GI                	0x80
-#define bHTSIG2_Smoothing         	0x01
-#define bHTSIG2_Sounding          	0x02
-#define bHTSIG2_Aggreaton         	0x08
-#define bHTSIG2_STBC              	0x30
-#define bHTSIG2_AdvCoding         	0x40
-#define bHTSIG2_NumOfHTLTF        	0x300
-#define bHTSIG2_CRC8              	0x3fc
-#define bHTSIG1_MCS               	0x7f
-#define bHTSIG1_BandWidth         	0x80
-#define bHTSIG1_HTLength          	0xffff
-#define bLSIG_Rate                	0xf
-#define bLSIG_Reserved            	0x10
-#define bLSIG_Length              	0x1fffe
-#define bLSIG_Parity              	0x20
-#define bCCKRxPhase               	0x4
-/* LSSI "read" address */
-#define bLSSIReadAddress          	0x3f000000
-/* LSSI "read" edge signal */
-#define bLSSIReadEdge             	0x80000000
-#define bLSSIReadBackData         	0xfff
-#define bLSSIReadOKFlag           	0x1000
-/* 0: 44 MHz, 1: 88MHz */
-#define bCCKSampleRate            	0x8
-
-#define bRegulator0Standby        	0x1
-#define bRegulatorPLLStandby      	0x2
-#define bRegulator1Standby        	0x4
-#define bPLLPowerUp               	0x8
-#define bDPLLPowerUp              	0x10
-#define bDA10PowerUp              	0x20
-#define bAD7PowerUp               	0x200
-#define bDA6PowerUp               	0x2000
-#define bXtalPowerUp              	0x4000
-#define b40MDClkPowerUP           	0x8000
-#define bDA6DebugMode             	0x20000
-#define bDA6Swing                 	0x380000
-#define bADClkPhase               	0x4000000
-#define b80MClkDelay              	0x18000000
-#define bAFEWatchDogEnable        	0x20000000
-#define bXtalCap                	0x0f000000
-#define bXtalCap01                	0xc0000000
-#define bXtalCap23                	0x3
-#define bXtalCap92x			0x0f000000
-#define bIntDifClkEnable          	0x400
-#define bExtSigClkEnable         	0x800
-#define bBandgapMbiasPowerUp      	0x10000
-#define bAD11SHGain               	0xc0000
-#define bAD11InputRange           	0x700000
-#define bAD11OPCurrent            	0x3800000
-#define bIPathLoopback            	0x4000000
-#define bQPathLoopback            	0x8000000
-#define bAFELoopback              	0x10000000
-#define bDA10Swing                	0x7e0
-#define bDA10Reverse              	0x800
-#define bDAClkSource              	0x1000
-#define bAD7InputRange            	0x6000
-#define bAD7Gain                  	0x38000
-#define bAD7OutputCMMode          	0x40000
-#define bAD7InputCMMode           	0x380000
-#define bAD7Current               	0xc00000
-#define bRegulatorAdjust          	0x7000000
-#define bAD11PowerUpAtTx          	0x1
-#define bDA10PSAtTx               	0x10
-#define bAD11PowerUpAtRx          	0x100
-#define bDA10PSAtRx               	0x1000
-
-#define bCCKRxAGCFormat           	0x200
-
-#define bPSDFFTSamplepPoint       	0xc000
-#define bPSDAverageNum            	0x3000
-#define bIQPathControl            	0xc00
-#define bPSDFreq                  	0x3ff
-#define bPSDAntennaPath           	0x30
-#define bPSDIQSwitch              	0x40
-#define bPSDRxTrigger             	0x400000
-#define bPSDTxTrigger             	0x80000000
-#define bPSDSineToneScale        	0x7f000000
-#define bPSDReport                	0xffff
-
-/* Page 8 */
-#define bOFDMTxSC                 	0x30000000
-#define bCCKTxOn                  	0x1
-#define bOFDMTxOn                 	0x2
-/* Reset debug page and also HWord, LWord */
-#define bDebugPage                	0xfff
-/* Reset debug page and LWord */
-#define bDebugItem                	0xff
-#define bAntL              	       	0x10
-#define bAntNonHT           	      	0x100
-#define bAntHT1               		0x1000
-#define bAntHT2                   	0x10000
-#define bAntHT1S1                 	0x100000
-#define bAntNonHTS1               	0x1000000
-
-/* Page a */
-#define bCCKBBMode                	0x3
-#define bCCKTxPowerSaving         	0x80
-#define bCCKRxPowerSaving         	0x40
-#define bCCKSideBand              	0x10
-#define bCCKScramble              	0x8
-#define bCCKAntDiversity    		0x8000
-#define bCCKCarrierRecovery   	    	0x4000
-#define bCCKTxRate           		0x3000
-#define bCCKDCCancel             	0x0800
-#define bCCKISICancel             	0x0400
-#define bCCKMatchFilter           	0x0200
-#define bCCKEqualizer             	0x0100
-#define bCCKPreambleDetect       	0x800000
-#define bCCKFastFalseCCA          	0x400000
-#define bCCKChEstStart            	0x300000
-#define bCCKCCACount              	0x080000
-#define bCCKcs_lim                	0x070000
-#define bCCKBistMode              	0x80000000
-#define bCCKCCAMask             	0x40000000
-#define bCCKTxDACPhase         	   	0x4
-/* r_rx_clk */
-#define bCCKRxADCPhase         	 	0x20000000
-#define bCCKr_cp_mode0         	   	0x0100
-#define bCCKTxDCOffset           	0xf0
-#define bCCKRxDCOffset         		0xf
-#define bCCKCCAMode              	0xc000
-#define bCCKFalseCS_lim           	0x3f00
-#define bCCKCS_ratio              	0xc00000
-#define bCCKCorgBit_sel           	0x300000
-#define bCCKPD_lim                	0x0f0000
-#define bCCKNewCCA                	0x80000000
-#define bCCKRxHPofIG              	0x8000
-#define bCCKRxIG                  	0x7f00
-#define bCCKLNAPolarity           	0x800000
-#define bCCKRx1stGain             	0x7f0000
-/* CCK Rx Initial gain polarity */
-#define bCCKRFExtend              	0x20000000
-#define bCCKRxAGCSatLevel        	0x1f000000
-#define bCCKRxAGCSatCount       	0xe0
-/* AGCSAmp_dly */
-#define bCCKRxRFSettle            	0x1f
-#define bCCKFixedRxAGC           	0x8000
-/*#define bCCKRxAGCFormat         	0x4000  remove to HSSI register 0x824 */
-#define bCCKAntennaPolarity      	0x2000
-#define bCCKTxFilterType          	0x0c00
-#define bCCKRxAGCReportType   	   	0x0300
-#define bCCKRxDAGCEn              	0x80000000
-#define bCCKRxDAGCPeriod        	0x20000000
-#define bCCKRxDAGCSatLevel     	   	0x1f000000
-#define bCCKTimingRecovery       	0x800000
-#define bCCKTxC0                  	0x3f0000
-#define bCCKTxC1                  	0x3f000000
-#define bCCKTxC2                  	0x3f
-#define bCCKTxC3                  	0x3f00
-#define bCCKTxC4                  	0x3f0000
-#define bCCKTxC5                  	0x3f000000
-#define bCCKTxC6                  	0x3f
-#define bCCKTxC7                  	0x3f00
-#define bCCKDebugPort             	0xff0000
-#define bCCKDACDebug              	0x0f000000
-#define bCCKFalseAlarmEnable      	0x8000
-#define bCCKFalseAlarmRead        	0x4000
-#define bCCKTRSSI                 	0x7f
-#define bCCKRxAGCReport           	0xfe
-#define bCCKRxReport_AntSel       	0x80000000
-#define bCCKRxReport_MFOff        	0x40000000
-#define bCCKRxRxReport_SQLoss     	0x20000000
-#define bCCKRxReport_Pktloss      	0x10000000
-#define bCCKRxReport_Lockedbit    	0x08000000
-#define bCCKRxReport_RateError    	0x04000000
-#define bCCKRxReport_RxRate       	0x03000000
-#define bCCKRxFACounterLower      	0xff
-#define bCCKRxFACounterUpper      	0xff000000
-#define bCCKRxHPAGCStart          	0xe000
-#define bCCKRxHPAGCFinal          	0x1c00
-
-#define bCCKRxFalseAlarmEnable    	0x8000
-#define bCCKFACounterFreeze       	0x4000
-
-#define bCCKTxPathSel             	0x10000000
-#define bCCKDefaultRxPath         	0xc000000
-#define bCCKOptionRxPath          	0x3000000
-
-/* Page c */
-#define bNumOfSTF                	0x3
-#define bShift_L                 	0xc0
-#define bGI_TH                   	0xc
-#define bRxPathA                 	0x1
-#define bRxPathB                 	0x2
-#define bRxPathC                 	0x4
-#define bRxPathD                 	0x8
-#define bTxPathA                 	0x1
-#define bTxPathB                 	0x2
-#define bTxPathC                 	0x4
-#define bTxPathD                 	0x8
-#define bTRSSIFreq               	0x200
-#define bADCBackoff              	0x3000
-#define bDFIRBackoff             	0xc000
-#define bTRSSILatchPhase         	0x10000
-#define bRxIDCOffset             	0xff
-#define bRxQDCOffset             	0xff00
-#define bRxDFIRMode              	0x1800000
-#define bRxDCNFType              	0xe000000
-#define bRXIQImb_A               	0x3ff
-#define bRXIQImb_B               	0xfc00
-#define bRXIQImb_C               	0x3f0000
-#define bRXIQImb_D               	0xffc00000
-#define bDC_dc_Notch             	0x60000
-#define bRxNBINotch              	0x1f000000
-#define bPD_TH                   	0xf
-#define bPD_TH_Opt2              	0xc000
-#define bPWED_TH                 	0x700
-#define bIfMF_Win_L              	0x800
-#define bPD_Option               	0x1000
-#define bMF_Win_L                	0xe000
-#define bBW_Search_L             	0x30000
-#define bwin_enh_L               	0xc0000
-#define bBW_TH                   	0x700000
-#define bED_TH2                  	0x3800000
-#define bBW_option               	0x4000000
-#define bRatio_TH                	0x18000000
-#define bWindow_L                	0xe0000000
-#define bSBD_Option              	0x1
-#define bFrame_TH                	0x1c
-#define bFS_Option               	0x60
-#define bDC_Slope_check          	0x80
-#define bFGuard_Counter_DC_L     	0xe00
-#define bFrame_Weight_Short      	0x7000
-#define bSub_Tune                	0xe00000
-#define bFrame_DC_Length         	0xe000000
-#define bSBD_start_offset        	0x30000000
-#define bFrame_TH_2              	0x7
-#define bFrame_GI2_TH            	0x38
-#define bGI2_Sync_en             	0x40
-#define bSarch_Short_Early       	0x300
-#define bSarch_Short_Late        	0xc00
-#define bSarch_GI2_Late          	0x70000
-#define bCFOAntSum               	0x1
-#define bCFOAcc                  	0x2
-#define bCFOStartOffset          	0xc
-#define bCFOLookBack             	0x70
-#define bCFOSumWeight            	0x80
-#define bDAGCEnable              	0x10000
-#define bTXIQImb_A               	0x3ff
-#define bTXIQImb_B               	0xfc00
-#define bTXIQImb_C               	0x3f0000
-#define bTXIQImb_D               	0xffc00000
-#define bTxIDCOffset             	0xff
-#define bTxQDCOffset             	0xff00
-#define bTxDFIRMode              	0x10000
-#define bTxPesudoNoiseOn         	0x4000000
-#define bTxPesudoNoise_A         	0xff
-#define bTxPesudoNoise_B         	0xff00
-#define bTxPesudoNoise_C         	0xff0000
-#define bTxPesudoNoise_D         	0xff000000
-#define bCCADropOption           	0x20000
-#define bCCADropThres            	0xfff00000
-#define bEDCCA_H                 	0xf
-#define bEDCCA_L                 	0xf0
-#define bLambda_ED               	0x300
-#define bRxInitialGain           	0x7f
-#define bRxAntDivEn              	0x80
-#define bRxAGCAddressForLNA      	0x7f00
-#define bRxHighPowerFlow         	0x8000
-#define bRxAGCFreezeThres        	0xc0000
-#define bRxFreezeStep_AGC1       	0x300000
-#define bRxFreezeStep_AGC2       	0xc00000
-#define bRxFreezeStep_AGC3       	0x3000000
-#define bRxFreezeStep_AGC0       	0xc000000
-#define bRxRssi_Cmp_En           	0x10000000
-#define bRxQuickAGCEn            	0x20000000
-#define bRxAGCFreezeThresMode    	0x40000000
-#define bRxOverFlowCheckType     	0x80000000
-#define bRxAGCShift              	0x7f
-#define bTRSW_Tri_Only           	0x80
-#define bPowerThres              	0x300
-#define bRxAGCEn                	0x1
-#define bRxAGCTogetherEn         	0x2
-#define bRxAGCMin                	0x4
-#define bRxHP_Ini                	0x7
-#define bRxHP_TRLNA              	0x70
-#define bRxHP_RSSI               	0x700
-#define bRxHP_BBP1               	0x7000
-#define bRxHP_BBP2               	0x70000
-#define bRxHP_BBP3               	0x700000
-/* The threshold for high power */
-#define bRSSI_H                  	0x7f0000
-/* The threshold for ant diversity */
-#define bRSSI_Gen                	0x7f000000
-#define bRxSettle_TRSW           	0x7
-#define bRxSettle_LNA            	0x38
-#define bRxSettle_RSSI           	0x1c0
-#define bRxSettle_BBP            	0xe00
-#define bRxSettle_RxHP           	0x7000
-#define bRxSettle_AntSW_RSSI     	0x38000
-#define bRxSettle_AntSW          	0xc0000
-#define bRxProcessTime_DAGC      	0x300000
-#define bRxSettle_HSSI           	0x400000
-#define bRxProcessTime_BBPPW     	0x800000
-#define bRxAntennaPowerShift     	0x3000000
-#define bRSSITableSelect         	0xc000000
-#define bRxHP_Final              	0x7000000
-#define bRxHTSettle_BBP          	0x7
-#define bRxHTSettle_HSSI         	0x8
-#define bRxHTSettle_RxHP         	0x70
-#define bRxHTSettle_BBPPW        	0x80
-#define bRxHTSettle_Idle         	0x300
-#define bRxHTSettle_Reserved     	0x1c00
-#define bRxHTRxHPEn              	0x8000
-#define bRxHTAGCFreezeThres      	0x30000
-#define bRxHTAGCTogetherEn       	0x40000
-#define bRxHTAGCMin              	0x80000
-#define bRxHTAGCEn               	0x100000
-#define bRxHTDAGCEn              	0x200000
-#define bRxHTRxHP_BBP            	0x1c00000
-#define bRxHTRxHP_Final          	0xe0000000
-#define bRxPWRatioTH             	0x3
-#define bRxPWRatioEn             	0x4
-#define bRxMFHold                	0x3800
-#define bRxPD_Delay_TH1          	0x38
-#define bRxPD_Delay_TH2          	0x1c0
-#define bRxPD_DC_COUNT_MAX       	0x600
-/*#define bRxMF_Hold               	0x3800*/
-#define bRxPD_Delay_TH           0x8000
-#define bRxProcess_Delay         0xf0000
-#define bRxSearchrange_GI2_Early 0x700000
-#define bRxFrame_Guard_Counter_L 0x3800000
-#define bRxSGI_Guard_L           0xc000000
-#define bRxSGI_Search_L          0x30000000
-#define bRxSGI_TH                0xc0000000
-#define bDFSCnt0                 0xff
-#define bDFSCnt1                 0xff00
-#define bDFSFlag                 0xf0000
-
-#define bMFWeightSum             0x300000
-#define bMinIdxTH                0x7f000000
-
-#define bDAFormat                0x40000
-
-#define bTxChEmuEnable           0x01000000
-
-#define bTRSWIsolation_A         0x7f
-#define bTRSWIsolation_B         0x7f00
-#define bTRSWIsolation_C         0x7f0000
-#define bTRSWIsolation_D         0x7f000000
-
-#define bExtLNAGain              0x7c00
-
-/* Page d */
-#define bSTBCEn                  0x4
-#define bAntennaMapping          0x10
-#define bNss                     0x20
-#define bCFOAntSumD              0x200
-#define bPHYCounterReset         0x8000000
-#define bCFOReportGet            0x4000000
-#define bOFDMContinueTx          0x10000000
-#define bOFDMSingleCarrier       0x20000000
-#define bOFDMSingleTone          0x40000000
-/*#define bRxPath1                 0x01
-#define bRxPath2                 0x02
-#define bRxPath3                 0x04
-#define bRxPath4                 0x08
-#define bTxPath1                 0x10
-#define bTxPath2                 0x20*/
-#define bHTDetect                0x100
-#define bCFOEn                   0x10000
-#define bCFOValue                0xfff00000
-#define bSigTone_Re              0x3f
-#define bSigTone_Im              0x7f00
-#define bCounter_CCA             0xffff
-#define bCounter_ParityFail      0xffff0000
-#define bCounter_RateIllegal     0xffff
-#define bCounter_CRC8Fail        0xffff0000
-#define bCounter_MCSNoSupport    0xffff
-#define bCounter_FastSync        0xffff
-#define bShortCFO                0xfff
-/* total */
-#define bShortCFOTLength         12
-/* fraction */
-#define bShortCFOFLength         11
-#define bLongCFO                 0x7ff
-#define bLongCFOTLength          11
-#define bLongCFOFLength          11
-#define bTailCFO                 0x1fff
-#define bTailCFOTLength          13
-#define bTailCFOFLength          12
-
-#define bmax_en_pwdB             0xffff
-#define bCC_power_dB             0xffff0000
-#define bnoise_pwdB              0xffff
-#define bPowerMeasTLength        10
-#define bPowerMeasFLength        3
-#define bRx_HT_BW                0x1
-#define bRxSC                    0x6
-#define bRx_HT                   0x8
-
-#define bNB_intf_det_on          0x1
-#define bIntf_win_len_cfg        0x30
-#define bNB_Intf_TH_cfg          0x1c0
-
-#define bRFGain                  0x3f
-#define bTableSel                0x40
-#define bTRSW                    0x80
-
-#define bRxSNR_A                 0xff
-#define bRxSNR_B                 0xff00
-#define bRxSNR_C                 0xff0000
-#define bRxSNR_D                 0xff000000
-#define bSNREVMTLength           8
-#define bSNREVMFLength           1
-
-#define bCSI1st                  0xff
-#define bCSI2nd                  0xff00
-#define bRxEVM1st                0xff0000
-#define bRxEVM2nd                0xff000000
-
-#define bSIGEVM                  0xff
-#define bPWDB                    0xff00
-#define bSGIEN                   0x10000
-
-#define bSFactorQAM1             0xf
-#define bSFactorQAM2             0xf0
-#define bSFactorQAM3             0xf00
-#define bSFactorQAM4             0xf000
-#define bSFactorQAM5             0xf0000
-#define bSFactorQAM6             0xf0000
-#define bSFactorQAM7             0xf00000
-#define bSFactorQAM8             0xf000000
-#define bSFactorQAM9             0xf0000000
-#define bCSIScheme               0x100000
-
-#define bNoiseLvlTopSet          0x3
-#define bChSmooth                0x4
-#define bChSmoothCfg1            0x38
-#define bChSmoothCfg2            0x1c0
-#define bChSmoothCfg3            0xe00
-#define bChSmoothCfg4            0x7000
-#define bMRCMode                 0x800000
-#define bTHEVMCfg                0x7000000
-
-#define bLoopFitType             0x1
-#define bUpdCFO                  0x40
-#define bUpdCFOOffData           0x80
-#define bAdvUpdCFO               0x100
-#define bAdvTimeCtrl             0x800
-#define bUpdClko                 0x1000
-#define bFC                      0x6000
-#define bTrackingMode            0x8000
-#define bPhCmpEnable             0x10000
-#define bUpdClkoLTF              0x20000
-#define bComChCFO                0x40000
-#define bCSIEstiMode             0x80000
-#define bAdvUpdEqz               0x100000
-#define bUChCfg                  0x7000000
-#define bUpdEqz                  0x8000000
-
-/* Page e */
-#define bTxAGCRate18_06		0x7f7f7f7f
-#define bTxAGCRate54_24		0x7f7f7f7f
-#define bTxAGCRateMCS32		0x7f
-#define bTxAGCRateCCK		0x7f00
-#define bTxAGCRateMCS3_MCS0	0x7f7f7f7f
-#define bTxAGCRateMCS7_MCS4	0x7f7f7f7f
-#define bTxAGCRateMCS11_MCS8	0x7f7f7f7f
-#define bTxAGCRateMCS15_MCS12	0x7f7f7f7f
-
-
-/* Rx Pseduo noise */
-#define bRxPesudoNoiseOn         0x20000000
-#define bRxPesudoNoise_A         0xff
-#define bRxPesudoNoise_B         0xff00
-#define bRxPesudoNoise_C         0xff0000
-#define bRxPesudoNoise_D         0xff000000
-#define bPesudoNoiseState_A      0xffff
-#define bPesudoNoiseState_B      0xffff0000
-#define bPesudoNoiseState_C      0xffff
-#define bPesudoNoiseState_D      0xffff0000
-
-/* RF Zebra 1 */
-#define bZebra1_HSSIEnable        0x8
-#define bZebra1_TRxControl        0xc00
-#define bZebra1_TRxGainSetting    0x07f
-#define bZebra1_RxCorner          0xc00
-#define bZebra1_TxChargePump      0x38
-#define bZebra1_RxChargePump      0x7
-#define bZebra1_ChannelNum        0xf80
-#define bZebra1_TxLPFBW           0x400
-#define bZebra1_RxLPFBW           0x600
-
-/* Zebra4 */
-#define bRTL8256RegModeCtrl1      0x100
-#define bRTL8256RegModeCtrl0      0x40
-#define bRTL8256_TxLPFBW          0x18
-#define bRTL8256_RxLPFBW          0x600
-
-//RTL8258
-#define bRTL8258_TxLPFBW          0xc
-#define bRTL8258_RxLPFBW          0xc00
-#define bRTL8258_RSSILPFBW        0xc0
-
-/* byte enable for sb_write */
-#define bByte0                    0x1
-#define bByte1                    0x2
-#define bByte2                    0x4
-#define bByte3                    0x8
-#define bWord0                    0x3
-#define bWord1                    0xc
-#define bDWord                    0xf
-
-/* for PutRegsetting & GetRegSetting BitMask */
-#define bMaskByte0                0xff
-#define bMaskByte1                0xff00
-#define bMaskByte2                0xff0000
-#define bMaskByte3                0xff000000
-#define bMaskHWord                0xffff0000
-#define bMaskLWord                0x0000ffff
-#define bMaskDWord                0xffffffff
-
-/* for PutRFRegsetting & GetRFRegSetting BitMask */
-#define bMask12Bits               0xfff
-
-#define bEnable                   0x1
-#define bDisable                  0x0
-
-#define LeftAntenna               0x0
-#define RightAntenna              0x1
-
-/* 500 ms */
-#define tCheckTxStatus            500
-/* 100 ms */
-#define tUpdateRxCounter          100
-
-#define rateCCK     0
-#define rateOFDM    1
-#define rateHT      2
-
-/* define Register-End */
-#define bPMAC_End                 0x1ff
-#define bFPGAPHY0_End             0x8ff
-#define bFPGAPHY1_End             0x9ff
-#define bCCKPHY0_End              0xaff
-#define bOFDMPHY0_End             0xcff
-#define bOFDMPHY1_End             0xdff
-
-#define bPMACControl              0x0
-#define bWMACControl              0x1
-#define bWNICControl              0x2
-
-#define PathA                     0x0
-#define PathB                     0x1
-#define PathC                     0x2
-#define PathD                     0x3
-
-#define rRTL8256RxMixerPole	0xb
-#define bZebraRxMixerPole	0x6
-#define rRTL8256TxBBOPBias	0x9
-#define bRTL8256TxBBOPBias	0x400
-#define rRTL8256TxBBBW		19
-#define bRTL8256TxBBBW		0x18
-
-
-#endif	/* __INC_HAL8190PCIPHYREG_H */
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
index 41b025e250fe..f246222e5fc9 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
@@ -36,18 +36,6 @@ void CamResetAllEntry(struct net_device *dev)
 	write_nic_dword(dev, RWCAM, ulcommand);
 }
 
-void write_cam(struct net_device *dev, u8 addr, u32 data)
-{
-	write_nic_dword(dev, WCAMI, data);
-	write_nic_dword(dev, RWCAM, BIT31|BIT16|(addr&0xff));
-}
-
-u32 read_cam(struct net_device *dev, u8 addr)
-{
-	write_nic_dword(dev, RWCAM, 0x80000000|(addr&0xff));
-	return read_nic_dword(dev, 0xa8);
-}
-
 void EnableHWSecurityConfig8192(struct net_device *dev)
 {
 	u8 SECR_value = 0x0;
@@ -81,7 +69,7 @@ void EnableHWSecurityConfig8192(struct net_device *dev)
 }
 
 void set_swcam(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
-	       u8 *MacAddr, u8 DefaultKey, u32 *KeyContent, u8 is_mesh)
+	       const u8 *MacAddr, u8 DefaultKey, u32 *KeyContent, u8 is_mesh)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rtllib_device *ieee = priv->rtllib;
@@ -100,7 +88,7 @@ void set_swcam(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
 }
 
 void setKey(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
-	    u8 *MacAddr, u8 DefaultKey, u32 *KeyContent)
+	    const u8 *MacAddr, u8 DefaultKey, u32 *KeyContent)
 {
 	u32 TargetCommand = 0;
 	u32 TargetContent = 0;
@@ -113,8 +101,8 @@ void setKey(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
 	if (priv->rtllib->PowerSaveControl.bInactivePs) {
 		if (rtState == eRfOff) {
 			if (priv->rtllib->RfOffReason > RF_CHANGE_BY_IPS) {
-				RT_TRACE(COMP_ERR, "%s(): RF is OFF.\n",
-					__func__);
+				netdev_warn(dev, "%s(): RF is OFF.\n",
+					    __func__);
 				return;
 			}
 			down(&priv->rtllib->ips_sem);
@@ -124,7 +112,7 @@ void setKey(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
 	}
 	priv->rtllib->is_set_key = true;
 	if (EntryNo >= TOTAL_CAM_ENTRY)
-		RT_TRACE(COMP_ERR, "cam entry exceeds in setKey()\n");
+		netdev_info(dev, "%s(): Invalid CAM entry\n", __func__);
 
 	RT_TRACE(COMP_SEC,
 		 "====>to setKey(), dev:%p, EntryNo:%d, KeyIndex:%d,KeyType:%d, MacAddr %pM\n",
@@ -243,9 +231,9 @@ void CamRestoreAllEntry(struct net_device *dev)
 				       (u32 *)(&priv->rtllib->swcamtable[0].key_buf[0])
 				     );
 			} else {
-				RT_TRACE(COMP_ERR,
-					 "===>%s():ERR!! ADHOC TKIP ,but 0 entry is have no data\n",
-					 __func__);
+				netdev_warn(dev,
+					    "%s(): ADHOC TKIP: missing key entry.\n",
+					    __func__);
 				return;
 			}
 		}
@@ -267,9 +255,9 @@ void CamRestoreAllEntry(struct net_device *dev)
 					CAM_CONST_ADDR[0], 0,
 					(u32 *)(&priv->rtllib->swcamtable[0].key_buf[0]));
 			} else {
-				RT_TRACE(COMP_ERR,
-					 "===>%s():ERR!! ADHOC CCMP ,but 0 entry is have no data\n",
-					 __func__);
+				netdev_warn(dev,
+					    "%s(): ADHOC CCMP: missing key entry.\n",
+					    __func__);
 				return;
 			}
 		}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
index 3c4c0e61c181..f23ab46c77e7 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
@@ -31,14 +31,9 @@ struct net_device;
 void CamResetAllEntry(struct net_device *dev);
 void EnableHWSecurityConfig8192(struct net_device *dev);
 void setKey(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
-	    u8 *MacAddr, u8 DefaultKey, u32 *KeyContent);
+	    const u8 *MacAddr, u8 DefaultKey, u32 *KeyContent);
 void set_swcam(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
-	       u8 *MacAddr, u8 DefaultKey, u32 *KeyContent, u8 is_mesh);
-void CamPrintDbgReg(struct net_device *dev);
-
-u32 read_cam(struct net_device *dev, u8 addr);
-void write_cam(struct net_device *dev, u8 addr, u32 data);
-
+	       const u8 *MacAddr, u8 DefaultKey, u32 *KeyContent, u8 is_mesh);
 void CamRestoreAllEntry(struct net_device *dev);
 
 #endif
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index 352d381b7c4a..c6cdb43b864c 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -44,6 +44,7 @@
 #include <linux/uaccess.h>
 #include <linux/pci.h>
 #include <linux/vmalloc.h>
+#include <linux/ieee80211.h>
 #include "rtl_core.h"
 #include "r8192E_phy.h"
 #include "r8192E_phyreg.h"
@@ -342,8 +343,9 @@ bool MgntActSet_RF_State(struct net_device *dev,
 					mdelay(1);
 
 					if (RFWaitCounter > 100) {
-						RT_TRACE(COMP_ERR,
-							 "MgntActSet_RF_State(): Wait too logn to set RF\n");
+						netdev_warn(dev,
+							    "%s(): Timeout waiting for RF change.\n",
+							    __func__);
 						return false;
 					}
 				}
@@ -391,7 +393,7 @@ bool MgntActSet_RF_State(struct net_device *dev,
 				else
 					priv->blinked_ingpio = false;
 				rtllib_MgntDisconnect(priv->rtllib,
-						      disas_lv_ss);
+						      WLAN_REASON_DISASSOC_STA_HAS_LEFT);
 			}
 		}
 		if ((ChangeSource == RF_CHANGE_BY_HW) && !priv->bHwRadioOff)
@@ -438,22 +440,6 @@ bool MgntActSet_RF_State(struct net_device *dev,
 	return bActionAllowed;
 }
 
-
-static short rtl8192_get_nic_desc_num(struct net_device *dev, int prio)
-{
-	struct r8192_priv *priv = rtllib_priv(dev);
-	struct rtl8192_tx_ring *ring = &priv->tx_ring[prio];
-
-	/* For now, we reserved two free descriptor as a safety boundary
-	* between the tail and the head
-	*/
-	if ((prio == MGNT_QUEUE) && (skb_queue_len(&ring->queue) > 10))
-		RT_TRACE(COMP_DBG,
-			 "-----[%d]---------ring->idx=%d queue_len=%d---------\n",
-			 prio, ring->idx, skb_queue_len(&ring->queue));
-	return skb_queue_len(&ring->queue);
-}
-
 static short rtl8192_check_nic_enough_desc(struct net_device *dev, int prio)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
@@ -896,9 +882,9 @@ void rtl8192_SetWirelessMode(struct net_device *dev, u8 wireless_mode)
 		} else if ((bSupportMode & WIRELESS_MODE_B)) {
 			wireless_mode = WIRELESS_MODE_B;
 		} else {
-			RT_TRACE(COMP_ERR,
-				 "%s(), No valid wireless mode supported (%x)!!!\n",
-				 __func__, bSupportMode);
+			netdev_info(dev,
+				    "%s(): Unsupported mode requested. Fallback to 802.11b\n",
+				    __func__);
 			wireless_mode = WIRELESS_MODE_B;
 		}
 	}
@@ -945,8 +931,7 @@ static int _rtl8192_sta_up(struct net_device *dev, bool is_silent_reset)
 	priv->bfirst_init = true;
 	init_status = priv->ops->initialize_adapter(dev);
 	if (!init_status) {
-		RT_TRACE(COMP_ERR, "ERR!!! %s(): initialization is failed!\n",
-			 __func__);
+		netdev_err(dev, "%s(): Initialization failed!\n", __func__);
 		priv->bfirst_init = false;
 		return -1;
 	}
@@ -1045,7 +1030,6 @@ static void rtl8192_init_priv_handler(struct net_device *dev)
 	priv->rtllib->data_hard_stop		= rtl8192_data_hard_stop;
 	priv->rtllib->data_hard_resume		= rtl8192_data_hard_resume;
 	priv->rtllib->check_nic_enough_desc	= rtl8192_check_nic_enough_desc;
-	priv->rtllib->get_nic_desc_num		= rtl8192_get_nic_desc_num;
 	priv->rtllib->handle_assoc_response	= rtl8192_handle_assoc_response;
 	priv->rtllib->handle_beacon		= rtl8192_handle_beacon;
 	priv->rtllib->SetWirelessMode		= rtl8192_SetWirelessMode;
@@ -1075,8 +1059,6 @@ static void rtl8192_init_priv_handler(struct net_device *dev)
 	priv->rtllib->UpdateBeaconInterruptHandler = NULL;
 
 	priv->rtllib->ScanOperationBackupHandler = PHY_ScanOperationBackup8192;
-
-	priv->rtllib->rtllib_rfkill_poll = NULL;
 }
 
 static void rtl8192_init_priv_constant(struct net_device *dev)
@@ -1086,16 +1068,6 @@ static void rtl8192_init_priv_constant(struct net_device *dev)
 					&(priv->rtllib->PowerSaveControl);
 
 	pPSC->RegMaxLPSAwakeIntvl = 5;
-
-	priv->RegPciASPM = 2;
-
-	priv->RegDevicePciASPMSetting = 0x03;
-
-	priv->RegHostPciASPMSetting = 0x02;
-
-	priv->RegHwSwRfOffD3 = 2;
-
-	priv->RegSupportPciASPM = 2;
 }
 
 
@@ -1106,18 +1078,13 @@ static void rtl8192_init_priv_variable(struct net_device *dev)
 
 	priv->AcmMethod = eAcmWay2_SW;
 	priv->dot11CurrentPreambleMode = PREAMBLE_AUTO;
-	priv->rtllib->hwscan_sem_up = 1;
 	priv->rtllib->status = 0;
-	priv->H2CTxCmdSeq = 0;
-	priv->bDisableFrameBursting = false;
-	priv->bDMInitialGainEnable = true;
 	priv->polling_timer_on = 0;
 	priv->up_first_time = 1;
 	priv->blinked_ingpio = false;
 	priv->bDriverIsGoingToUnload = false;
 	priv->being_init_adapter = false;
 	priv->initialized_at_probe = false;
-	priv->sw_radio_on = true;
 	priv->bdisable_nic = false;
 	priv->bfirst_init = false;
 	priv->txringcount = 64;
@@ -1125,12 +1092,7 @@ static void rtl8192_init_priv_variable(struct net_device *dev)
 	priv->rxringcount = MAX_RX_COUNT;
 	priv->irq_enabled = 0;
 	priv->chan = 1;
-	priv->RegWirelessMode = WIRELESS_MODE_AUTO;
 	priv->RegChannelPlan = 0xf;
-	priv->nrxAMPDU_size = 0;
-	priv->nrxAMPDU_aggr_num = 0;
-	priv->last_rxdesc_tsf_high = 0;
-	priv->last_rxdesc_tsf_low = 0;
 	priv->rtllib->mode = WIRELESS_MODE_AUTO;
 	priv->rtllib->iw_mode = IW_MODE_INFRA;
 	priv->rtllib->bNetPromiscuousMode = false;
@@ -1176,12 +1138,6 @@ static void rtl8192_init_priv_variable(struct net_device *dev)
 	priv->rtllib->sta_sleep = LPS_IS_WAKE;
 	priv->rtllib->eRFPowerState = eRfOn;
 
-	priv->txpower_checkcnt = 0;
-	priv->thermal_readback_index = 0;
-	priv->txpower_tracking_callback_cnt = 0;
-	priv->ccktxpower_adjustcnt_ch14 = 0;
-	priv->ccktxpower_adjustcnt_not_ch14 = 0;
-
 	priv->rtllib->current_network.beacon_interval = DEFAULT_BEACONINTERVAL;
 	priv->rtllib->iw_mode = IW_MODE_INFRA;
 	priv->rtllib->active_scan = 1;
@@ -1191,20 +1147,15 @@ static void rtl8192_init_priv_variable(struct net_device *dev)
 	priv->rtllib->host_encrypt = 1;
 	priv->rtllib->host_decrypt = 1;
 
-	priv->rtllib->dot11PowerSaveMode = eActive;
 	priv->rtllib->fts = DEFAULT_FRAG_THRESHOLD;
-	priv->rtllib->MaxMssDensity = 0;
-	priv->rtllib->MinSpaceCfg = 0;
 
 	priv->card_type = PCI;
 
-	priv->AcmControl = 0;
 	priv->pFirmware = vzalloc(sizeof(struct rt_firmware));
 	if (!priv->pFirmware)
 		netdev_err(dev,
 			   "rtl8192e: Unable to allocate space for firmware\n");
 
-	skb_queue_head_init(&priv->rx_queue);
 	skb_queue_head_init(&priv->skb_queue);
 
 	for (i = 0; i < MAX_QUEUE_SIZE; i++)
@@ -1215,14 +1166,10 @@ static void rtl8192_init_priv_variable(struct net_device *dev)
 
 static void rtl8192_init_priv_lock(struct r8192_priv *priv)
 {
-	spin_lock_init(&priv->fw_scan_lock);
 	spin_lock_init(&priv->tx_lock);
-	spin_lock_init(&priv->irq_lock);
 	spin_lock_init(&priv->irq_th_lock);
 	spin_lock_init(&priv->rf_ps_lock);
 	spin_lock_init(&priv->ps_lock);
-	spin_lock_init(&priv->rf_lock);
-	spin_lock_init(&priv->rt_h2c_lock);
 	sema_init(&priv->wx_sem, 1);
 	sema_init(&priv->rf_sem, 1);
 	mutex_init(&priv->mutex);
@@ -1267,9 +1214,8 @@ static short rtl8192_get_channel_map(struct net_device *dev)
 
 	if ((priv->rf_chip != RF_8225) && (priv->rf_chip != RF_8256)
 			&& (priv->rf_chip != RF_6052)) {
-		RT_TRACE(COMP_ERR,
-			 "%s: unknown rf chip, can't set channel map\n",
-			 __func__);
+		netdev_err(dev, "%s: unknown rf chip, can't set channel map\n",
+			   __func__);
 		return -1;
 	}
 
@@ -1306,12 +1252,10 @@ static short rtl8192_init(struct net_device *dev)
 
 	init_hal_dm(dev);
 
-	init_timer(&priv->watch_dog_timer);
 	setup_timer(&priv->watch_dog_timer,
 		    watch_dog_timer_callback,
 		    (unsigned long) dev);
 
-	init_timer(&priv->gpio_polling_timer);
 	setup_timer(&priv->gpio_polling_timer,
 		    check_rfctrl_gpio_timer,
 		    (unsigned long)dev);
@@ -1498,9 +1442,8 @@ RESET_START:
 			LeisurePSLeave(dev);
 
 		if (priv->up) {
-			RT_TRACE(COMP_ERR,
-				 "%s():the driver is not up! return\n",
-				 __func__);
+			netdev_info(dev, "%s():the driver is not up.\n",
+				    __func__);
 			up(&priv->wx_sem);
 			return;
 		}
@@ -1533,7 +1476,7 @@ RESET_START:
 			SEM_UP_IEEE_WX(&ieee->wx_sem);
 		} else {
 			netdev_info(dev, "ieee->state is NOT LINKED\n");
-			rtllib_softmac_stop_protocol(priv->rtllib, 0 , true);
+			rtllib_softmac_stop_protocol(priv->rtllib, 0, true);
 		}
 
 		dm_backup_dynamic_mechanism_state(dev);
@@ -1554,9 +1497,8 @@ RESET_START:
 				reset_times++;
 				goto RESET_START;
 			} else {
-				RT_TRACE(COMP_ERR,
-					 " ERR!!! %s():  Reset Failed!!\n",
-					 __func__);
+				netdev_warn(dev, "%s():	Reset Failed\n",
+					    __func__);
 			}
 		}
 
@@ -1729,7 +1671,7 @@ void	rtl819x_watchdog_wqcallback(void *data)
 
 		if (priv->check_roaming_cnt > 0) {
 			if (ieee->eRFPowerState == eRfOff)
-				RT_TRACE(COMP_ERR, "========>%s()\n", __func__);
+				netdev_info(dev, "%s(): RF is off\n", __func__);
 
 			netdev_info(dev,
 				    "===>%s(): AP is power off, chan:%d, connect another one\n",
@@ -1884,8 +1826,9 @@ void rtl8192_hard_data_xmit(struct sk_buff *skb, struct net_device *dev,
 		return;
 	}
 
-	assert(queue_index != TXCMD_QUEUE);
-
+	if (queue_index != TXCMD_QUEUE)
+		netdev_warn(dev, "%s(): queue index != TXCMD_QUEUE\n",
+			    __func__);
 
 	memcpy((unsigned char *)(skb->cb), &dev, sizeof(dev));
 	skb_push(skb, priv->rtllib->tx_headroom);
@@ -1998,9 +1941,8 @@ short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
 	u32 fwinfo_size = 0;
 
 	if (priv->bdisable_nic) {
-		RT_TRACE(COMP_ERR,
-			 "%s: ERR!! Nic is disabled! Can't tx packet len=%d qidx=%d!!!\n",
-			 __func__, skb->len, tcb_desc->queue_index);
+		netdev_warn(dev, "%s: Nic is disabled! Can't tx packet.\n",
+			    __func__);
 		return skb->len;
 	}
 
@@ -2037,10 +1979,10 @@ short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
 
 	pdesc = &ring->desc[idx];
 	if ((pdesc->OWN == 1) && (tcb_desc->queue_index != BEACON_QUEUE)) {
-		RT_TRACE(COMP_ERR,
-			 "No more TX desc@%d, ring->idx = %d, idx = %d, skblen = 0x%x queuelen=%d",
-			 tcb_desc->queue_index, ring->idx, idx, skb->len,
-			 skb_queue_len(&ring->queue));
+		netdev_warn(dev,
+			    "No more TX desc@%d, ring->idx = %d, idx = %d, skblen = 0x%x queuelen=%d",
+			    tcb_desc->queue_index, ring->idx, idx, skb->len,
+			    skb_queue_len(&ring->queue));
 		spin_unlock_irqrestore(&priv->irq_th_lock, flags);
 		return skb->len;
 	}
@@ -2066,13 +2008,12 @@ static short rtl8192_alloc_rx_desc_ring(struct net_device *dev)
 	int i, rx_queue_idx;
 
 	for (rx_queue_idx = 0; rx_queue_idx < MAX_RX_QUEUE; rx_queue_idx++) {
-		priv->rx_ring[rx_queue_idx] =
-			pci_zalloc_consistent(priv->pdev,
+		priv->rx_ring[rx_queue_idx] = pci_zalloc_consistent(priv->pdev,
 					      sizeof(*priv->rx_ring[rx_queue_idx]) * priv->rxringcount,
 					      &priv->rx_ring_dma[rx_queue_idx]);
 		if (!priv->rx_ring[rx_queue_idx] ||
 		    (unsigned long)priv->rx_ring[rx_queue_idx] & 0xFF) {
-			RT_TRACE(COMP_ERR, "Cannot allocate RX ring\n");
+			netdev_warn(dev, "Cannot allocate RX ring\n");
 			return -ENOMEM;
 		}
 
@@ -2102,7 +2043,7 @@ static short rtl8192_alloc_rx_desc_ring(struct net_device *dev)
 			entry->OWN = 1;
 		}
 
-		if(entry)
+		if (entry)
 			entry->EOR = 1;
 	}
 	return 0;
@@ -2118,8 +2059,7 @@ static int rtl8192_alloc_tx_desc_ring(struct net_device *dev,
 
 	ring = pci_zalloc_consistent(priv->pdev, sizeof(*ring) * entries, &dma);
 	if (!ring || (unsigned long)ring & 0xFF) {
-		RT_TRACE(COMP_ERR, "Cannot allocate TX ring (prio = %d)\n",
-			 prio);
+		netdev_warn(dev, "Cannot allocate TX ring (prio = %d)\n", prio);
 		return -ENOMEM;
 	}
 
@@ -2310,7 +2250,7 @@ static void rtl8192_rx_normal(struct net_device *dev)
 
 	struct rtllib_rx_stats stats = {
 		.signal = 0,
-		.noise = -98,
+		.noise = (u8) -98,
 		.rate = 0,
 		.freq = RTLLIB_24GHZ_BAND,
 	};
@@ -2519,7 +2459,7 @@ void rtl8192_commit(struct net_device *dev)
 
 	if (priv->up == 0)
 		return;
-	rtllib_softmac_stop_protocol(priv->rtllib, 0 , true);
+	rtllib_softmac_stop_protocol(priv->rtllib, 0, true);
 	rtl8192_irq_disable(dev);
 	priv->ops->stop_adapter(dev, true);
 	_rtl8192_up(dev, false);
@@ -2572,8 +2512,7 @@ static int rtl8192_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	int ret = -1;
 	struct rtllib_device *ieee = priv->rtllib;
 	u32 key[4];
-	u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	u8 zero_addr[6] = {0};
+	const u8 broadcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	struct iw_point *p = &wrq->u.data;
 	struct ieee_param *ipw = NULL;
 
@@ -2610,8 +2549,7 @@ static int rtl8192_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 				}
 
 				if (ieee->pairwise_key_type) {
-					if (memcmp(ieee->ap_mac_addr, zero_addr,
-					    6) == 0)
+					if (is_zero_ether_addr(ieee->ap_mac_addr))
 						ieee->iw_mode = IW_MODE_ADHOC;
 					memcpy((u8 *)key, ipw->u.crypt.key, 16);
 					EnableHWSecurityConfig8192(dev);
@@ -2850,7 +2788,7 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 	RT_TRACE(COMP_INIT, "Configuring chip resources");
 
 	if (pci_enable_device(pdev)) {
-		RT_TRACE(COMP_ERR, "Failed to enable PCI device");
+		dev_err(&pdev->dev, "Failed to enable PCI device");
 		return -EIO;
 	}
 
@@ -2888,21 +2826,21 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 	pmem_flags = pci_resource_flags(pdev, 1);
 
 	if (!(pmem_flags & IORESOURCE_MEM)) {
-		RT_TRACE(COMP_ERR, "region #1 not a MMIO resource, aborting");
+		netdev_err(dev, "region #1 not a MMIO resource, aborting");
 		goto err_rel_rtllib;
 	}
 
 	dev_info(&pdev->dev, "Memory mapped space start: 0x%08lx\n",
 		 pmem_start);
 	if (!request_mem_region(pmem_start, pmem_len, DRV_NAME)) {
-		RT_TRACE(COMP_ERR, "request_mem_region failed!");
+		netdev_err(dev, "request_mem_region failed!");
 		goto err_rel_rtllib;
 	}
 
 
 	ioaddr = (unsigned long)ioremap_nocache(pmem_start, pmem_len);
 	if (ioaddr == (unsigned long)NULL) {
-		RT_TRACE(COMP_ERR, "ioremap failed!");
+		netdev_err(dev, "ioremap failed!");
 		goto err_rel_mem;
 	}
 
@@ -2938,7 +2876,7 @@ static int rtl8192_pci_probe(struct pci_dev *pdev,
 
 	RT_TRACE(COMP_INIT, "Driver probe completed1\n");
 	if (rtl8192_init(dev) != 0) {
-		RT_TRACE(COMP_ERR, "Initialization failed");
+		netdev_warn(dev, "Initialization failed");
 		goto err_free_irq;
 	}
 
@@ -3001,8 +2939,6 @@ static void rtl8192_pci_disconnect(struct pci_dev *pdev)
 		}
 		free_rtllib(dev);
 
-		kfree(priv->scan_cmd);
-
 		if (dev->mem_start != 0) {
 			iounmap((void __iomem *)dev->mem_start);
 			release_mem_region(pci_resource_start(pdev, 1),
@@ -3024,8 +2960,7 @@ bool NicIFEnableNIC(struct net_device *dev)
 					(&(priv->rtllib->PowerSaveControl));
 
 	if (!priv->up) {
-		RT_TRACE(COMP_ERR, "ERR!!! %s(): Driver is already down!\n",
-			 __func__);
+		netdev_warn(dev, "%s(): Driver is already down!\n", __func__);
 		priv->bdisable_nic = false;
 		return false;
 	}
@@ -3034,8 +2969,7 @@ bool NicIFEnableNIC(struct net_device *dev)
 	priv->bfirst_init = true;
 	init_status = priv->ops->initialize_adapter(dev);
 	if (!init_status) {
-		RT_TRACE(COMP_ERR, "ERR!!! %s(): initialization is failed!\n",
-			 __func__);
+		netdev_warn(dev, "%s(): Initialization failed!\n", __func__);
 		priv->bdisable_nic = false;
 		return false;
 	}
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index d365af6ebdc7..776d950655cb 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -68,74 +68,19 @@
 #define DRV_AUTHOR  "<wlanfae@realtek.com>"
 #define DRV_VERSION  "0014.0401.2010"
 
-#define IS_HARDWARE_TYPE_819xP(_priv)		\
-	((((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8190P) || \
-	(((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8192E))
 #define IS_HARDWARE_TYPE_8192SE(_priv)		\
 	(((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8192SE)
-#define IS_HARDWARE_TYPE_8192CE(_priv)		\
-	(((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8192CE)
-#define IS_HARDWARE_TYPE_8192CU(_priv)		\
-	(((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8192CU)
-#define IS_HARDWARE_TYPE_8192DE(_priv)		\
-	(((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8192DE)
-#define IS_HARDWARE_TYPE_8192DU(_priv)		\
-	(((struct r8192_priv *)rtllib_priv(dev))->card_8192 == NIC_8192DU)
 
 #define RTL_PCI_DEVICE(vend, dev, cfg) \
 	.vendor = (vend), .device = (dev), \
-	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID , \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID, \
 	.driver_data = (kernel_ulong_t)&(cfg)
 
-#define RTL_MAX_SCAN_SIZE 128
-
-#define RTL_RATE_MAX		30
-
 #define TOTAL_CAM_ENTRY		32
 #define CAM_CONTENT_COUNT	8
 
-#ifndef BIT
-#define BIT(_i)				(1<<(_i))
-#endif
-
-#define IS_ADAPTER_SENDS_BEACON(dev) 0
-
-#define HAL_MEMORY_MAPPED_IO_RANGE_8190PCI	0x1000
-#define HAL_HW_PCI_REVISION_ID_8190PCI			0x00
-#define HAL_MEMORY_MAPPED_IO_RANGE_8192PCIE	0x4000
 #define HAL_HW_PCI_REVISION_ID_8192PCIE		0x01
-#define HAL_MEMORY_MAPPED_IO_RANGE_8192SE	0x4000
 #define HAL_HW_PCI_REVISION_ID_8192SE	0x10
-#define HAL_HW_PCI_REVISION_ID_8192CE			0x1
-#define HAL_MEMORY_MAPPED_IO_RANGE_8192CE	0x4000
-#define HAL_HW_PCI_REVISION_ID_8192DE			0x0
-#define HAL_MEMORY_MAPPED_IO_RANGE_8192DE	0x4000
-
-#define HAL_HW_PCI_8180_DEVICE_ID			0x8180
-#define HAL_HW_PCI_8185_DEVICE_ID			0x8185
-#define HAL_HW_PCI_8188_DEVICE_ID			0x8188
-#define HAL_HW_PCI_8198_DEVICE_ID			0x8198
-#define HAL_HW_PCI_8190_DEVICE_ID			0x8190
-#define HAL_HW_PCI_8192_DEVICE_ID			0x8192
-#define HAL_HW_PCI_8192SE_DEVICE_ID			0x8192
-#define HAL_HW_PCI_8174_DEVICE_ID			0x8174
-#define HAL_HW_PCI_8173_DEVICE_ID			0x8173
-#define HAL_HW_PCI_8172_DEVICE_ID			0x8172
-#define HAL_HW_PCI_8171_DEVICE_ID			0x8171
-#define HAL_HW_PCI_0045_DEVICE_ID			0x0045
-#define HAL_HW_PCI_0046_DEVICE_ID			0x0046
-#define HAL_HW_PCI_0044_DEVICE_ID			0x0044
-#define HAL_HW_PCI_0047_DEVICE_ID			0x0047
-#define HAL_HW_PCI_700F_DEVICE_ID			0x700F
-#define HAL_HW_PCI_701F_DEVICE_ID			0x701F
-#define HAL_HW_PCI_DLINK_DEVICE_ID			0x3304
-#define HAL_HW_PCI_8192CET_DEVICE_ID			0x8191
-#define HAL_HW_PCI_8192CE_DEVICE_ID			0x8178
-#define HAL_HW_PCI_8191CE_DEVICE_ID			0x8177
-#define HAL_HW_PCI_8188CE_DEVICE_ID			0x8176
-#define HAL_HW_PCI_8192CU_DEVICE_ID			0x8191
-#define HAL_HW_PCI_8192DE_DEVICE_ID			0x092D
-#define HAL_HW_PCI_8192DU_DEVICE_ID			0x092D
 
 #define RTL819X_DEFAULT_RF_TYPE		RF_1T2R
 
@@ -150,16 +95,12 @@
 	(1600 + (MAX_802_11_HEADER_LENGTH + ENCRYPTION_MAX_OVERHEAD) *	\
 	 MAX_FRAGMENT_COUNT)
 
-#define scrclng				4
-
 #define DEFAULT_FRAG_THRESHOLD	2342U
 #define MIN_FRAG_THRESHOLD	256U
 #define DEFAULT_BEACONINTERVAL	0x64U
 
-#define DEFAULT_SSID		""
 #define DEFAULT_RETRY_RTS	7
 #define DEFAULT_RETRY_DATA	7
-#define PRISM_HDR_SIZE		64
 
 #define	PHY_RSSI_SLID_WIN_MAX			100
 
@@ -183,29 +124,6 @@
 
 extern int hwwep;
 
-enum RTL819x_PHY_PARAM {
-	RTL819X_PHY_MACPHY_REG			= 0,
-	RTL819X_PHY_MACPHY_REG_PG		= 1,
-	RTL8188C_PHY_MACREG			= 2,
-	RTL8192C_PHY_MACREG			= 3,
-	RTL819X_PHY_REG				= 4,
-	RTL819X_PHY_REG_1T2R			= 5,
-	RTL819X_PHY_REG_to1T1R			= 6,
-	RTL819X_PHY_REG_to1T2R			= 7,
-	RTL819X_PHY_REG_to2T2R			= 8,
-	RTL819X_PHY_REG_PG			= 9,
-	RTL819X_AGC_TAB				= 10,
-	RTL819X_PHY_RADIO_A			= 11,
-	RTL819X_PHY_RADIO_A_1T			= 12,
-	RTL819X_PHY_RADIO_A_2T			= 13,
-	RTL819X_PHY_RADIO_B			= 14,
-	RTL819X_PHY_RADIO_B_GM			= 15,
-	RTL819X_PHY_RADIO_C			= 16,
-	RTL819X_PHY_RADIO_D			= 17,
-	RTL819X_EEPROM_MAP			= 18,
-	RTL819X_EFUSE_MAP			= 19,
-};
-
 enum nic_t {
 	NIC_UNKNOWN     = 0,
 	NIC_8192E       = 1,
@@ -220,7 +138,6 @@ enum nic_t {
 enum rt_eeprom_type {
 	EEPROM_93C46,
 	EEPROM_93C56,
-	EEPROM_BOOT_EFUSE,
 };
 
 enum dcmg_txcmd_op {
@@ -242,19 +159,6 @@ enum rt_rf_type_819xu {
 	RF_PSEUDO_11N = 5,
 };
 
-enum rf_step {
-	RF_STEP_INIT = 0,
-	RF_STEP_NORMAL,
-	RF_STEP_MAX
-};
-
-enum rt_status {
-	RT_STATUS_SUCCESS,
-	RT_STATUS_FAILURE,
-	RT_STATUS_PENDING,
-	RT_STATUS_RESOURCE
-};
-
 enum rt_customer_id {
 	RT_CID_DEFAULT	  = 0,
 	RT_CID_8187_ALPHA0      = 1,
@@ -294,58 +198,9 @@ enum reset_type {
 	RESET_TYPE_SILENT = 0x02
 };
 
-enum ic_inferiority_8192s {
-	IC_INFERIORITY_A	    = 0,
-	IC_INFERIORITY_B	    = 1,
-};
-
-enum pci_bridge_vendor {
-	PCI_BRIDGE_VENDOR_INTEL = 0x0,
-	PCI_BRIDGE_VENDOR_ATI,
-	PCI_BRIDGE_VENDOR_AMD,
-	PCI_BRIDGE_VENDOR_SIS ,
-	PCI_BRIDGE_VENDOR_UNKNOWN,
-	PCI_BRIDGE_VENDOR_MAX ,
-};
-
-struct buffer {
-	struct buffer *next;
-	u32 *buf;
-	dma_addr_t dma;
-
-};
-
-struct rtl_reg_debug {
-	unsigned int  cmd;
-	struct {
-		unsigned char type;
-		unsigned char addr;
-		unsigned char page;
-		unsigned char length;
-	} head;
-	unsigned char buf[0xff];
-};
-
-struct rt_tx_rahis {
-	u32	     cck[4];
-	u32	     ofdm[8];
-	u32	     ht_mcs[4][16];
-};
-
-struct rt_smooth_data_4rf {
-	char	elements[4][100];
-	u32	index;
-	u32	TotalNum;
-	u32	TotalVal[4];
-};
-
 struct rt_stats {
-	unsigned long txrdu;
 	unsigned long rxrdu;
 	unsigned long rxok;
-	unsigned long rxframgment;
-	unsigned long rxurberr;
-	unsigned long rxstaterr;
 	unsigned long rxdatacrcerr;
 	unsigned long rxmgmtcrcerr;
 	unsigned long rxcrcerrmin;
@@ -353,8 +208,6 @@ struct rt_stats {
 	unsigned long rxcrcerrmax;
 	unsigned long received_rate_histogram[4][32];
 	unsigned long received_preamble_GI[2][32];
-	unsigned long	rx_AMPDUsize_histogram[5];
-	unsigned long rx_AMPDUnum_histogram[5];
 	unsigned long numpacket_matchbssid;
 	unsigned long numpacket_toself;
 	unsigned long num_process_phyinfo;
@@ -362,58 +215,24 @@ struct rt_stats {
 	unsigned long numqry_phystatusCCK;
 	unsigned long numqry_phystatusHT;
 	unsigned long received_bwtype[5];
-	unsigned long txnperr;
-	unsigned long txnpdrop;
-	unsigned long txresumed;
 	unsigned long rxoverflow;
 	unsigned long rxint;
-	unsigned long txnpokint;
 	unsigned long ints;
 	unsigned long shints;
 	unsigned long txoverflow;
-	unsigned long txlpokint;
-	unsigned long txlpdrop;
-	unsigned long txlperr;
 	unsigned long txbeokint;
-	unsigned long txbedrop;
-	unsigned long txbeerr;
 	unsigned long txbkokint;
-	unsigned long txbkdrop;
-	unsigned long txbkerr;
 	unsigned long txviokint;
-	unsigned long txvidrop;
-	unsigned long txvierr;
 	unsigned long txvookint;
-	unsigned long txvodrop;
-	unsigned long txvoerr;
 	unsigned long txbeaconokint;
-	unsigned long txbeacondrop;
 	unsigned long txbeaconerr;
 	unsigned long txmanageokint;
-	unsigned long txmanagedrop;
-	unsigned long txmanageerr;
 	unsigned long txcmdpktokint;
-	unsigned long txdatapkt;
-	unsigned long txfeedback;
-	unsigned long txfeedbackok;
-	unsigned long txoktotal;
-	unsigned long txokbytestotal;
-	unsigned long txokinperiod;
-	unsigned long txmulticast;
 	unsigned long txbytesmulticast;
-	unsigned long txbroadcast;
 	unsigned long txbytesbroadcast;
-	unsigned long txunicast;
 	unsigned long txbytesunicast;
 	unsigned long rxbytesunicast;
-	unsigned long txfeedbackfail;
-	unsigned long txerrtotal;
-	unsigned long txerrbytestotal;
-	unsigned long txerrmulticast;
-	unsigned long txerrbroadcast;
-	unsigned long txerrunicast;
 	unsigned long txretrycount;
-	unsigned long txfeedbackretry;
 	u8	last_packet_rate;
 	unsigned long slide_signal_strength[100];
 	unsigned long slide_evm[100];
@@ -426,10 +245,8 @@ struct rt_stats {
 	u8 rx_rssi_percentage[4];
 	u8 rx_evm_percentage[2];
 	long rxSNRdB[4];
-	struct rt_tx_rahis txrate;
 	u32 Slide_Beacon_pwdb[100];
 	u32 Slide_Beacon_Total;
-	struct rt_smooth_data_4rf cck_adc_pwdb;
 	u32	CurrentShowTxate;
 };
 
@@ -442,24 +259,6 @@ struct channel_access_setting {
 	u16 CWmaxIndex;
 };
 
-enum two_port_status {
-	TWO_PORT_STATUS__DEFAULT_ONLY,
-	TWO_PORT_STATUS__EXTENSION_ONLY,
-	TWO_PORT_STATUS__EXTENSION_FOLLOW_DEFAULT,
-	TWO_PORT_STATUS__DEFAULT_G_EXTENSION_N20,
-	TWO_PORT_STATUS__ADHOC,
-	TWO_PORT_STATUS__WITHOUT_ANY_ASSOCIATE
-};
-
-struct txbbgain_struct {
-	long	txbb_iq_amplifygain;
-	u32	txbbgain_value;
-};
-
-struct ccktxbbgain {
-	u8	ccktxbb_valuearray[8];
-};
-
 struct init_gain {
 	u8	xaagccore1;
 	u8	xbagccore1;
@@ -540,17 +339,11 @@ struct r8192_priv {
 	struct delayed_work		txpower_tracking_wq;
 	struct delayed_work		rfpath_check_wq;
 	struct delayed_work		gpio_change_rf_wq;
-	struct delayed_work		initialgain_operate_wq;
-	struct delayed_work		check_hw_scan_wq;
-	struct delayed_work		hw_scan_simu_wq;
-	struct delayed_work		start_hw_scan_wq;
 
 	struct workqueue_struct		*priv_wq;
 
 	struct channel_access_setting ChannelAccessSetting;
 
-	struct mp_adapter NdisAdapter;
-
 	struct rtl819x_ops			*ops;
 	struct rtllib_device			*rtllib;
 
@@ -562,16 +355,10 @@ struct r8192_priv {
 
 
 	enum rt_rf_type_819xu rf_chip;
-	enum ic_inferiority_8192s IC_Class;
 	enum ht_channel_width CurrentChannelBW;
 	struct bb_reg_definition PHYRegDef[4];
 	struct rate_adaptive rate_adaptive;
 
-	struct ccktxbbgain cck_txbbgain_table[CCKTxBBGainTableLength];
-	struct ccktxbbgain cck_txbbgain_ch14_table[CCKTxBBGainTableLength];
-
-	struct txbbgain_struct txbbgain_table[TxBBGainTableLength];
-
 	enum acm_method AcmMethod;
 
 	struct rt_firmware			*pFirmware;
@@ -581,17 +368,11 @@ struct r8192_priv {
 	struct timer_list			fsync_timer;
 	struct timer_list			gpio_polling_timer;
 
-	spinlock_t				fw_scan_lock;
-	spinlock_t				irq_lock;
 	spinlock_t				irq_th_lock;
 	spinlock_t				tx_lock;
 	spinlock_t				rf_ps_lock;
-	spinlock_t				rw_lock;
-	spinlock_t				rt_h2c_lock;
-	spinlock_t				rf_lock;
 	spinlock_t				ps_lock;
 
-	struct sk_buff_head		rx_queue;
 	struct sk_buff_head		skb_queue;
 
 	struct tasklet_struct		irq_rx_tasklet;
@@ -604,12 +385,9 @@ struct r8192_priv {
 
 	struct rt_stats stats;
 	struct iw_statistics			wstats;
-	struct proc_dir_entry		*dir_dev;
 
 	short (*rf_set_sens)(struct net_device *dev, short sens);
 	u8 (*rf_set_chan)(struct net_device *dev, u8 ch);
-	void (*rf_close)(struct net_device *dev);
-	void (*rf_init)(struct net_device *dev);
 
 	struct rx_desc *rx_ring[MAX_RX_QUEUE];
 	struct sk_buff	*rx_buf[MAX_RX_QUEUE][MAX_RX_COUNT];
@@ -620,29 +398,19 @@ struct r8192_priv {
 
 	u64		LastRxDescTSF;
 
-	u16		EarlyRxThreshold;
 	u32		ReceiveConfig;
-	u8		AcmControl;
-	u8		RFProgType;
 	u8		retry_data;
 	u8		retry_rts;
 	u16		rts;
 
 	struct rtl8192_tx_ring tx_ring[MAX_TX_QUEUE_COUNT];
 	int		 txringcount;
-	int		txbuffsize;
-	int		txfwbuffersize;
 	atomic_t	tx_pending[0x10];
 
 	u16		ShortRetryLimit;
 	u16		LongRetryLimit;
-	u32		TransmitConfig;
-	u8		RegCWinMin;
-	u8		keepAliveLevel;
 
-	bool		sw_radio_on;
 	bool		bHwRadioOff;
-	bool		pwrdown;
 	bool		blinked_ingpio;
 	u8		polling_timer_on;
 
@@ -655,17 +423,11 @@ struct r8192_priv {
 
 	struct work_struct qos_activate;
 
-	u8 bIbssCoordinator;
-
 	short	promisc;
-	short	crcmon;
-
-	int txbeaconcount;
 
 	short	chan;
 	short	sens;
 	short	max_sens;
-	u32 rx_prevlen;
 
 	u8 ScanDelay;
 	bool ps_force;
@@ -676,114 +438,43 @@ struct r8192_priv {
 	enum nic_t card_8192;
 	u8 card_8192_version;
 
-	short	enable_gpio0;
-
 	u8 rf_type;
 	u8 IC_Cut;
 	char nick[IW_ESSID_MAX_SIZE + 1];
-
-	u8 RegBcnCtrlVal;
-	bool bHwAntDiv;
-
-	bool bTKIPinNmodeFromReg;
-	bool bWEPinNmodeFromReg;
-
-	bool bLedOpenDrain;
-
 	u8 check_roaming_cnt;
 
-	bool bIgnoreSilentReset;
-	u32 SilentResetRxSoltNum;
 	u32 SilentResetRxSlotIndex;
 	u32 SilentResetRxStuckEvent[MAX_SILENT_RESET_RX_SLOT_NUM];
 
-	void *scan_cmd;
-	u8 hwscan_bw_40;
-
-	u16 nrxAMPDU_size;
-	u8 nrxAMPDU_aggr_num;
-
-	u32 last_rxdesc_tsf_high;
-	u32 last_rxdesc_tsf_low;
-
 	u16 basic_rate;
 	u8 short_preamble;
 	u8 dot11CurrentPreambleMode;
 	u8 slot_time;
 	u16 SifsTime;
 
-	u8 RegWirelessMode;
-
-	u8 firmware_version;
-	u16 FirmwareSubVersion;
-	u16 rf_pathmap;
 	bool AutoloadFailFlag;
 
-	u8 RegPciASPM;
-	u8 RegAMDPciASPM;
-	u8 RegHwSwRfOffD3;
-	u8 RegSupportPciASPM;
-	bool bSupportASPM;
-
-	u32 RfRegChnlVal[2];
-
-	u8 ShowRateMode;
-	u8 RATRTableBitmap;
-
-	u8 EfuseMap[2][HWSET_MAX_SIZE_92S];
-	u16 EfuseUsedBytes;
-	u8 EfuseUsedPercentage;
-
 	short	epromtype;
 	u16 eeprom_vid;
 	u16 eeprom_did;
-	u16 eeprom_svid;
-	u16 eeprom_smid;
 	u8 eeprom_CustomerID;
 	u16 eeprom_ChannelPlan;
-	u8 eeprom_version;
-
-	u8 EEPROMRegulatory;
-	u8 EEPROMPwrGroup[2][3];
-	u8 EEPROMOptional;
 
 	u8 EEPROMTxPowerLevelCCK[14];
 	u8 EEPROMTxPowerLevelOFDM24G[14];
-	u8 EEPROMTxPowerLevelOFDM5G[24];
 	u8 EEPROMRfACCKChnl1TxPwLevel[3];
 	u8 EEPROMRfAOfdmChnlTxPwLevel[3];
 	u8 EEPROMRfCCCKChnl1TxPwLevel[3];
 	u8 EEPROMRfCOfdmChnlTxPwLevel[3];
-	u16 EEPROMTxPowerDiff;
 	u16 EEPROMAntPwDiff;
 	u8 EEPROMThermalMeter;
-	u8 EEPROMPwDiff;
 	u8 EEPROMCrystalCap;
 
-	u8 EEPROMBluetoothCoexist;
-	u8 EEPROMBluetoothType;
-	u8 EEPROMBluetoothAntNum;
-	u8 EEPROMBluetoothAntIsolation;
-	u8 EEPROMBluetoothRadioShared;
-
-
-	u8 EEPROMSupportWoWLAN;
-	u8 EEPROMBoardType;
-	u8 EEPROM_Def_Ver;
-	u8 EEPROMHT2T_TxPwr[6];
-	u8 EEPROMTSSI_A;
-	u8 EEPROMTSSI_B;
-	u8 EEPROMTxPowerLevelCCK_V1[3];
 	u8 EEPROMLegacyHTTxPowerDiff;
 
-	u8 BluetoothCoexist;
-
 	u8 CrystalCap;
 	u8 ThermalMeter[2];
 
-	u16 FwCmdIOMap;
-	u32 FwCmdIOParam;
-
 	u8 SwChnlInProgress;
 	u8 SwChnlStage;
 	u8 SwChnlStep;
@@ -799,60 +490,16 @@ struct r8192_priv {
 
 	u16 RegChannelPlan;
 	u16 ChannelPlan;
-	bool bChnlPlanFromHW;
 
 	bool RegRfOff;
 	bool isRFOff;
 	bool bInPowerSaveMode;
 	u8 bHwRfOffAction;
 
-	bool aspm_clkreq_enable;
-	u32 pci_bridge_vendor;
-	u8 RegHostPciASPMSetting;
-	u8 RegDevicePciASPMSetting;
-
 	bool RFChangeInProgress;
 	bool SetRFPowerStateInProgress;
 	bool bdisable_nic;
 
-	u8 pwrGroupCnt;
-
-	u8 ThermalValue_LCK;
-	u8 ThermalValue_IQK;
-	bool bRfPiEnable;
-
-	u32 APKoutput[2][2];
-	bool bAPKdone;
-
-	long RegE94;
-	long RegE9C;
-	long RegEB4;
-	long RegEBC;
-
-	u32 RegC04;
-	u32 Reg874;
-	u32 RegC08;
-	u32 ADDA_backup[16];
-	u32 IQK_MAC_backup[3];
-
-	bool SetFwCmdInProgress;
-	u8 CurrentFwCmdIO;
-
-	u8 rssi_level;
-
-	bool bInformFWDriverControlDM;
-	u8 PwrGroupHT20[2][14];
-	u8 PwrGroupHT40[2][14];
-
-	u8 ThermalValue;
-	long EntryMinUndecoratedSmoothedPWDB;
-	long EntryMaxUndecoratedSmoothedPWDB;
-	u8 DynamicTxHighPowerLvl;
-	u8 LastDTPLvl;
-	u32 CurrentRATR0;
-	struct false_alarm_stats FalseAlmCnt;
-
-	u8 DMFlag;
 	u8 DM_Type;
 
 	u8 CckPwEnl;
@@ -862,54 +509,32 @@ struct r8192_priv {
 	u8 CCKPresentAttentuation_40Mdefault;
 	char CCKPresentAttentuation_difference;
 	char CCKPresentAttentuation;
-	u8 bCckHighPower;
 	long undecorated_smoothed_pwdb;
-	long undecorated_smoothed_cck_adc_pwdb[4];
 
 	u32 MCSTxPowerLevelOriginalOffset[6];
-	u32 CCKTxPowerLevelOriginalOffset;
 	u8 TxPowerLevelCCK[14];
 	u8 TxPowerLevelCCK_A[14];
 	u8 TxPowerLevelCCK_C[14];
 	u8		TxPowerLevelOFDM24G[14];
-	u8		TxPowerLevelOFDM5G[14];
 	u8		TxPowerLevelOFDM24G_A[14];
 	u8		TxPowerLevelOFDM24G_C[14];
 	u8		LegacyHTTxPowerDiff;
-	u8		TxPowerDiff;
 	s8		RF_C_TxPwDiff;
-	s8		RF_B_TxPwDiff;
-	u8		RfTxPwrLevelCck[2][14];
-	u8		RfTxPwrLevelOfdm1T[2][14];
-	u8		RfTxPwrLevelOfdm2T[2][14];
 	u8		AntennaTxPwDiff[3];
-	u8		TxPwrHt20Diff[2][14];
-	u8		TxPwrLegacyHtDiff[2][14];
-	u8		TxPwrSafetyFlag;
-	u8		HT2T_TxPwr_A[14];
-	u8		HT2T_TxPwr_B[14];
-	u8		CurrentCckTxPwrIdx;
-	u8		CurrentOfdm24GTxPwrIdx;
-
-	bool		bdynamic_txpower;
+
 	bool		bDynamicTxHighPower;
 	bool		bDynamicTxLowPower;
 	bool		bLastDTPFlag_High;
 	bool		bLastDTPFlag_Low;
 
-	bool		bstore_last_dtpflag;
-	bool		bstart_txctrl_bydtp;
-
 	u8		rfa_txpowertrackingindex;
 	u8		rfa_txpowertrackingindex_real;
 	u8		rfa_txpowertracking_default;
 	u8		rfc_txpowertrackingindex;
 	u8		rfc_txpowertrackingindex_real;
-	u8		rfc_txpowertracking_default;
 	bool		btxpower_tracking;
 	bool		bcck_in_ch14;
 
-	u8		TxPowerTrackControl;
 	u8		txpower_count;
 	bool		btxpower_trackingInit;
 
@@ -925,11 +550,6 @@ struct r8192_priv {
 	bool		bcurrent_turbo_EDCA;
 	bool		bis_cur_rdlstate;
 
-	bool		bCCKinCH14;
-
-	u8		MidHighPwrTHR_L1;
-	u8		MidHighPwrTHR_L2;
-
 	bool		bfsync_processing;
 	u32		rate_record;
 	u32		rateCountDiffRecord;
@@ -939,56 +559,21 @@ struct r8192_priv {
 	u32		framesyncC34;
 	u8		framesyncMonitor;
 
-	bool		bDMInitialGainEnable;
-	bool		MutualAuthenticationFail;
-
-	bool		bDisableFrameBursting;
-
 	u32		reset_count;
-	bool		bpbc_pressed;
-
-	u32		txpower_checkcnt;
-	u32		txpower_tracking_callback_cnt;
-	u8		thermal_read_val[40];
-	u8		thermal_readback_index;
-	u32		ccktxpower_adjustcnt_not_ch14;
-	u32		ccktxpower_adjustcnt_ch14;
 
 	enum reset_type ResetProgress;
 	bool		bForcedSilentReset;
 	bool		bDisableNormalResetCheck;
 	u16		TxCounter;
 	u16		RxCounter;
-	int		IrpPendingCount;
 	bool		bResetInProgress;
 	bool		force_reset;
 	bool		force_lps;
-	u8		InitialGainOperateType;
 
 	bool		chan_forced;
-	bool		bSingleCarrier;
-	bool		RegBoard;
-	bool		bCckContTx;
-	bool		bOfdmContTx;
-	bool		bStartContTx;
-	u8		RegPaModel;
-	u8		btMpCckTxPower;
-	u8		btMpOfdmTxPower;
-
-	u32		MptActType;
-	u32		MptIoOffset;
-	u32		MptIoValue;
-	u32		MptRfPath;
-
-	u32		MptBandWidth;
-	u32		MptRateIndex;
-	u8		MptChannelToSw;
-	u32	MptRCR;
 
 	u8		PwrDomainProtect;
 	u8		H2CTxCmdSeq;
-
-
 };
 
 extern const struct ethtool_ops rtl819x_ethtool_ops;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_crypto.h b/drivers/staging/rtl8192e/rtl8192e/rtl_crypto.h
deleted file mode 100644
index ee57c0f4fa69..000000000000
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_crypto.h
+++ /dev/null
@@ -1,382 +0,0 @@
-/*
- * Scatterlist Cryptographic API.
- *
- * Copyright (c) 2002 James Morris <jmorris@intercode.com.au>
- * Copyright (c) 2002 David S. Miller (davem@redhat.com)
- *
- * Portions derived from Cryptoapi, by Alexander Kjeldaas <astor@fast.no>
- * and Nettle, by Niels M鰈ler.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under 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_CRYPTO_H
-#define _LINUX_CRYPTO_H
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/list.h>
-#include <linux/string.h>
-#include <asm/page.h>
-#include <linux/errno.h>
-
-#define crypto_register_alg crypto_register_alg_rsl
-#define crypto_unregister_alg crypto_unregister_alg_rsl
-#define crypto_alloc_tfm crypto_alloc_tfm_rsl
-#define crypto_free_tfm crypto_free_tfm_rsl
-#define crypto_alg_available crypto_alg_available_rsl
-
-/*
- * Algorithm masks and types.
- */
-#define CRYPTO_ALG_TYPE_MASK		0x000000ff
-#define CRYPTO_ALG_TYPE_CIPHER		0x00000001
-#define CRYPTO_ALG_TYPE_DIGEST		0x00000002
-#define CRYPTO_ALG_TYPE_COMPRESS	0x00000004
-
-/*
- * Transform masks and values (for crt_flags).
- */
-#define CRYPTO_TFM_MODE_MASK		0x000000ff
-#define CRYPTO_TFM_REQ_MASK		0x000fff00
-#define CRYPTO_TFM_RES_MASK		0xfff00000
-
-#define CRYPTO_TFM_MODE_ECB		0x00000001
-#define CRYPTO_TFM_MODE_CBC		0x00000002
-#define CRYPTO_TFM_MODE_CFB		0x00000004
-#define CRYPTO_TFM_MODE_CTR		0x00000008
-
-#define CRYPTO_TFM_REQ_WEAK_KEY		0x00000100
-#define CRYPTO_TFM_RES_WEAK_KEY		0x00100000
-#define CRYPTO_TFM_RES_BAD_KEY_LEN	0x00200000
-#define CRYPTO_TFM_RES_BAD_KEY_SCHED	0x00400000
-#define CRYPTO_TFM_RES_BAD_BLOCK_LEN	0x00800000
-#define CRYPTO_TFM_RES_BAD_FLAGS	0x01000000
-
-/*
- * Miscellaneous stuff.
- */
-#define CRYPTO_UNSPEC			0
-#define CRYPTO_MAX_ALG_NAME		64
-
-struct scatterlist;
-
-/*
- * Algorithms: modular crypto algorithm implementations, managed
- * via crypto_register_alg() and crypto_unregister_alg().
- */
-struct cipher_alg {
-	unsigned int cia_min_keysize;
-	unsigned int cia_max_keysize;
-	int (*cia_setkey)(void *ctx, const u8 *key,
-			  unsigned int keylen, u32 *flags);
-	void (*cia_encrypt)(void *ctx, u8 *dst, const u8 *src);
-	void (*cia_decrypt)(void *ctx, u8 *dst, const u8 *src);
-};
-
-struct digest_alg {
-	unsigned int dia_digestsize;
-	void (*dia_init)(void *ctx);
-	void (*dia_update)(void *ctx, const u8 *data, unsigned int len);
-	void (*dia_final)(void *ctx, u8 *out);
-	int (*dia_setkey)(void *ctx, const u8 *key,
-			  unsigned int keylen, u32 *flags);
-};
-
-struct compress_alg {
-	int (*coa_init)(void *ctx);
-	void (*coa_exit)(void *ctx);
-	int (*coa_compress)(void *ctx, const u8 *src, unsigned int slen,
-			    u8 *dst, unsigned int *dlen);
-	int (*coa_decompress)(void *ctx, const u8 *src, unsigned int slen,
-			      u8 *dst, unsigned int *dlen);
-};
-
-#define cra_cipher	cra_u.cipher
-#define cra_digest	cra_u.digest
-#define cra_compress	cra_u.compress
-
-struct crypto_alg {
-	struct list_head cra_list;
-	u32 cra_flags;
-	unsigned int cra_blocksize;
-	unsigned int cra_ctxsize;
-	const char cra_name[CRYPTO_MAX_ALG_NAME];
-
-	union {
-		struct cipher_alg cipher;
-		struct digest_alg digest;
-		struct compress_alg compress;
-	} cra_u;
-
-	struct module *cra_module;
-};
-
-/*
- * Algorithm registration interface.
- */
-int crypto_register_alg(struct crypto_alg *alg);
-int crypto_unregister_alg(struct crypto_alg *alg);
-
-/*
- * Algorithm query interface.
- */
-int crypto_alg_available(const char *name, u32 flags);
-
-/*
- * Transforms: user-instantiated objects which encapsulate algorithms
- * and core processing logic.  Managed via crypto_alloc_tfm() and
- * crypto_free_tfm(), as well as the various helpers below.
- */
-struct crypto_tfm;
-
-struct cipher_tfm {
-	void *cit_iv;
-	unsigned int cit_ivsize;
-	u32 cit_mode;
-	int (*cit_setkey)(struct crypto_tfm *tfm,
-			  const u8 *key, unsigned int keylen);
-	int (*cit_encrypt)(struct crypto_tfm *tfm,
-			   struct scatterlist *dst,
-			   struct scatterlist *src,
-			   unsigned int nbytes);
-	int (*cit_encrypt_iv)(struct crypto_tfm *tfm,
-			      struct scatterlist *dst,
-			      struct scatterlist *src,
-			      unsigned int nbytes, u8 *iv);
-	int (*cit_decrypt)(struct crypto_tfm *tfm,
-			   struct scatterlist *dst,
-			   struct scatterlist *src,
-			   unsigned int nbytes);
-	int (*cit_decrypt_iv)(struct crypto_tfm *tfm,
-			   struct scatterlist *dst,
-			   struct scatterlist *src,
-			   unsigned int nbytes, u8 *iv);
-	void (*cit_xor_block)(u8 *dst, const u8 *src);
-};
-
-struct digest_tfm {
-	void (*dit_init)(struct crypto_tfm *tfm);
-	void (*dit_update)(struct crypto_tfm *tfm,
-			   struct scatterlist *sg, unsigned int nsg);
-	void (*dit_final)(struct crypto_tfm *tfm, u8 *out);
-	void (*dit_digest)(struct crypto_tfm *tfm, struct scatterlist *sg,
-			   unsigned int nsg, u8 *out);
-	int (*dit_setkey)(struct crypto_tfm *tfm,
-			  const u8 *key, unsigned int keylen);
-};
-
-struct compress_tfm {
-	int (*cot_compress)(struct crypto_tfm *tfm,
-			    const u8 *src, unsigned int slen,
-			    u8 *dst, unsigned int *dlen);
-	int (*cot_decompress)(struct crypto_tfm *tfm,
-			      const u8 *src, unsigned int slen,
-			      u8 *dst, unsigned int *dlen);
-};
-
-#define crt_cipher	crt_u.cipher
-#define crt_digest	crt_u.digest
-#define crt_compress	crt_u.compress
-
-struct crypto_tfm {
-
-	u32 crt_flags;
-
-	union {
-		struct cipher_tfm cipher;
-		struct digest_tfm digest;
-		struct compress_tfm compress;
-	} crt_u;
-
-	struct crypto_alg *__crt_alg;
-};
-
-/*
- * Transform user interface.
- */
-
-/*
- * crypto_alloc_tfm() will first attempt to locate an already loaded algorithm.
- * If that fails and the kernel supports dynamically loadable modules, it
- * will then attempt to load a module of the same name or alias.  A refcount
- * is grabbed on the algorithm which is then associated with the new transform.
- *
- * crypto_free_tfm() frees up the transform and any associated resources,
- * then drops the refcount on the associated algorithm.
- */
-struct crypto_tfm *crypto_alloc_tfm(const char *alg_name, u32 tfm_flags);
-void crypto_free_tfm(struct crypto_tfm *tfm);
-
-/*
- * Transform helpers which query the underlying algorithm.
- */
-static inline const char *crypto_tfm_alg_name(struct crypto_tfm *tfm)
-{
-	return tfm->__crt_alg->cra_name;
-}
-
-static inline const char *crypto_tfm_alg_modname(struct crypto_tfm *tfm)
-{
-	struct crypto_alg *alg = tfm->__crt_alg;
-
-	if (alg->cra_module)
-		return alg->cra_module->name;
-	else
-		return NULL;
-}
-
-static inline u32 crypto_tfm_alg_type(struct crypto_tfm *tfm)
-{
-	return tfm->__crt_alg->cra_flags & CRYPTO_ALG_TYPE_MASK;
-}
-
-static inline unsigned int crypto_tfm_alg_min_keysize(struct crypto_tfm *tfm)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	return tfm->__crt_alg->cra_cipher.cia_min_keysize;
-}
-
-static inline unsigned int crypto_tfm_alg_max_keysize(struct crypto_tfm *tfm)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	return tfm->__crt_alg->cra_cipher.cia_max_keysize;
-}
-
-static inline unsigned int crypto_tfm_alg_ivsize(struct crypto_tfm *tfm)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	return tfm->crt_cipher.cit_ivsize;
-}
-
-static inline unsigned int crypto_tfm_alg_blocksize(struct crypto_tfm *tfm)
-{
-	return tfm->__crt_alg->cra_blocksize;
-}
-
-static inline unsigned int crypto_tfm_alg_digestsize(struct crypto_tfm *tfm)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
-	return tfm->__crt_alg->cra_digest.dia_digestsize;
-}
-
-/*
- * API wrappers.
- */
-static inline void crypto_digest_init(struct crypto_tfm *tfm)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
-	tfm->crt_digest.dit_init(tfm);
-}
-
-static inline void crypto_digest_update(struct crypto_tfm *tfm,
-					struct scatterlist *sg,
-					unsigned int nsg)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
-	tfm->crt_digest.dit_update(tfm, sg, nsg);
-}
-
-static inline void crypto_digest_final(struct crypto_tfm *tfm, u8 *out)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
-	tfm->crt_digest.dit_final(tfm, out);
-}
-
-static inline void crypto_digest_digest(struct crypto_tfm *tfm,
-					struct scatterlist *sg,
-					unsigned int nsg, u8 *out)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
-	tfm->crt_digest.dit_digest(tfm, sg, nsg, out);
-}
-
-static inline int crypto_digest_setkey(struct crypto_tfm *tfm,
-				       const u8 *key, unsigned int keylen)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
-	if (tfm->crt_digest.dit_setkey == NULL)
-		return -ENOSYS;
-	return tfm->crt_digest.dit_setkey(tfm, key, keylen);
-}
-
-static inline int crypto_cipher_setkey(struct crypto_tfm *tfm,
-				       const u8 *key, unsigned int keylen)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	return tfm->crt_cipher.cit_setkey(tfm, key, keylen);
-}
-
-static inline int crypto_cipher_encrypt(struct crypto_tfm *tfm,
-					struct scatterlist *dst,
-					struct scatterlist *src,
-					unsigned int nbytes)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	return tfm->crt_cipher.cit_encrypt(tfm, dst, src, nbytes);
-}
-
-static inline int crypto_cipher_encrypt_iv(struct crypto_tfm *tfm,
-					   struct scatterlist *dst,
-					   struct scatterlist *src,
-					   unsigned int nbytes, u8 *iv)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	BUG_ON(tfm->crt_cipher.cit_mode == CRYPTO_TFM_MODE_ECB);
-	return tfm->crt_cipher.cit_encrypt_iv(tfm, dst, src, nbytes, iv);
-}
-
-static inline int crypto_cipher_decrypt(struct crypto_tfm *tfm,
-					struct scatterlist *dst,
-					struct scatterlist *src,
-					unsigned int nbytes)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	return tfm->crt_cipher.cit_decrypt(tfm, dst, src, nbytes);
-}
-
-static inline int crypto_cipher_decrypt_iv(struct crypto_tfm *tfm,
-					   struct scatterlist *dst,
-					   struct scatterlist *src,
-					   unsigned int nbytes, u8 *iv)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	BUG_ON(tfm->crt_cipher.cit_mode == CRYPTO_TFM_MODE_ECB);
-	return tfm->crt_cipher.cit_decrypt_iv(tfm, dst, src, nbytes, iv);
-}
-
-static inline void crypto_cipher_set_iv(struct crypto_tfm *tfm,
-					const u8 *src, unsigned int len)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	memcpy(tfm->crt_cipher.cit_iv, src, len);
-}
-
-static inline void crypto_cipher_get_iv(struct crypto_tfm *tfm,
-					u8 *dst, unsigned int len)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
-	memcpy(dst, tfm->crt_cipher.cit_iv, len);
-}
-
-static inline int crypto_comp_compress(struct crypto_tfm *tfm,
-				       const u8 *src, unsigned int slen,
-				       u8 *dst, unsigned int *dlen)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_COMPRESS);
-	return tfm->crt_compress.cot_compress(tfm, src, slen, dst, dlen);
-}
-
-static inline int crypto_comp_decompress(struct crypto_tfm *tfm,
-					 const u8 *src, unsigned int slen,
-					 u8 *dst, unsigned int *dlen)
-{
-	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_COMPRESS);
-	return tfm->crt_compress.cot_decompress(tfm, src, slen, dst, dlen);
-}
-
-#endif	/* _LINUX_CRYPTO_H */
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index df4bbcf38bae..3de7cc549794 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -60,14 +60,104 @@ static u32 edca_setting_UL[HT_IOT_PEER_MAX] = {
 
 #define RTK_UL_EDCA 0xa44f
 #define RTK_DL_EDCA 0x5e4322
+
+const u32 dm_tx_bb_gain[TxBBGainTableLength] = {
+	0x7f8001fe, /* 12 dB */
+	0x788001e2, /* 11 dB */
+	0x71c001c7,
+	0x6b8001ae,
+	0x65400195,
+	0x5fc0017f,
+	0x5a400169,
+	0x55400155,
+	0x50800142,
+	0x4c000130,
+	0x47c0011f,
+	0x43c0010f,
+	0x40000100,
+	0x3c8000f2,
+	0x390000e4,
+	0x35c000d7,
+	0x32c000cb,
+	0x300000c0,
+	0x2d4000b5,
+	0x2ac000ab,
+	0x288000a2,
+	0x26000098,
+	0x24000090,
+	0x22000088,
+	0x20000080,
+	0x1a00006c,
+	0x1c800072,
+	0x18000060,
+	0x19800066,
+	0x15800056,
+	0x26c0005b,
+	0x14400051,
+	0x24400051,
+	0x1300004c,
+	0x12000048,
+	0x11000044,
+	0x10000040, /* -24 dB */
+};
+
+const u8 dm_cck_tx_bb_gain[CCKTxBBGainTableLength][8] = {
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04},
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04},
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03},
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03},
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03},
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02},
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02},
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02},
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02},
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02},
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01},
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}
+};
+
+const u8 dm_cck_tx_bb_gain_ch14[CCKTxBBGainTableLength][8] = {
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00},
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00},
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},
+	{0x2d, 0x2d, 0x27, 0x17, 0x00, 0x00, 0x00, 0x00},
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},
+	{0x28, 0x28, 0x22, 0x14, 0x00, 0x00, 0x00, 0x00},
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00},
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00},
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00},
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00},
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00},
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00},
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00},
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}
+};
+
 /*---------------------------Define Local Constant---------------------------*/
 
 
 /*------------------------Define global variable-----------------------------*/
 struct dig_t dm_digtable;
-u8 dm_shadow[16][256] = {
-	{0}
-};
 
 struct drx_path_sel DM_RxPathSelTable;
 /*------------------------Define global variable-----------------------------*/
@@ -155,7 +245,8 @@ void init_hal_dm(struct net_device *dev)
 	if (IS_HARDWARE_TYPE_8192SE(dev))
 		dm_Init_WA_Broadcom_IOT(dev);
 
-	INIT_DELAYED_WORK_RSL(&priv->gpio_change_rf_wq, (void *)dm_CheckRfCtrlGPIO, dev);
+	INIT_DELAYED_WORK_RSL(&priv->gpio_change_rf_wq,
+			      (void *)dm_CheckRfCtrlGPIO, dev);
 }
 
 void deinit_hal_dm(struct net_device *dev)
@@ -195,8 +286,8 @@ void hal_dm_watchdog(struct net_device *dev)
 static void dm_check_ac_dc_power(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
-	static char *ac_dc_check_script_path = "/etc/acpi/wireless-rtl-ac-dc-power.sh";
-	char *argv[] = {ac_dc_check_script_path, DRV_NAME, NULL};
+	static char *ac_dc_script = "/etc/acpi/wireless-rtl-ac-dc-power.sh";
+	char *argv[] = {ac_dc_script, DRV_NAME, NULL};
 	static char *envp[] = {"HOME=/",
 			"TERM=linux",
 			"PATH=/usr/bin:/bin",
@@ -210,7 +301,7 @@ static void dm_check_ac_dc_power(struct net_device *dev)
 
 	if (priv->rtllib->state != RTLLIB_LINKED)
 		return;
-	call_usermodehelper(ac_dc_check_script_path, argv, envp, UMH_WAIT_PROC);
+	call_usermodehelper(ac_dc_script, argv, envp, UMH_WAIT_PROC);
 
 	return;
 };
@@ -220,7 +311,7 @@ void init_rate_adaptive(struct net_device *dev)
 {
 
 	struct r8192_priv *priv = rtllib_priv(dev);
-	struct rate_adaptive *pra = (struct rate_adaptive *)&priv->rate_adaptive;
+	struct rate_adaptive *pra = &priv->rate_adaptive;
 
 	pra->ratr_state = DM_RATR_STA_MAX;
 	pra->high2low_rssi_thresh_for_ra = RateAdaptiveTH_High;
@@ -261,14 +352,15 @@ static void dm_check_rate_adaptive(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rt_hi_throughput *pHTInfo = priv->rtllib->pHTInfo;
-	struct rate_adaptive *pra = (struct rate_adaptive *)&priv->rate_adaptive;
+	struct rate_adaptive *pra = &priv->rate_adaptive;
 	u32 currentRATR, targetRATR = 0;
 	u32 LowRSSIThreshForRA = 0, HighRSSIThreshForRA = 0;
 	bool bshort_gi_enabled = false;
 	static u8 ping_rssi_state;
 
 	if (!priv->up) {
-		RT_TRACE(COMP_RATE, "<---- dm_check_rate_adaptive(): driver is going to unload\n");
+		RT_TRACE(COMP_RATE,
+			 "<---- dm_check_rate_adaptive(): driver is going to unload\n");
 		return;
 	}
 
@@ -281,44 +373,52 @@ static void dm_check_rate_adaptive(struct net_device *dev)
 
 	if (priv->rtllib->state == RTLLIB_LINKED) {
 
-		bshort_gi_enabled = (pHTInfo->bCurTxBW40MHz && pHTInfo->bCurShortGI40MHz) ||
-			(!pHTInfo->bCurTxBW40MHz && pHTInfo->bCurShortGI20MHz);
-
+		bshort_gi_enabled = (pHTInfo->bCurTxBW40MHz &&
+				     pHTInfo->bCurShortGI40MHz) ||
+				    (!pHTInfo->bCurTxBW40MHz &&
+				     pHTInfo->bCurShortGI20MHz);
 
 		pra->upper_rssi_threshold_ratr =
-				(pra->upper_rssi_threshold_ratr & (~BIT31)) | ((bshort_gi_enabled) ? BIT31 : 0);
+				(pra->upper_rssi_threshold_ratr & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
 
 		pra->middle_rssi_threshold_ratr =
-				(pra->middle_rssi_threshold_ratr & (~BIT31)) | ((bshort_gi_enabled) ? BIT31 : 0);
+				(pra->middle_rssi_threshold_ratr & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
 
 		if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) {
 			pra->low_rssi_threshold_ratr =
-				(pra->low_rssi_threshold_ratr_40M & (~BIT31)) | ((bshort_gi_enabled) ? BIT31 : 0);
+				(pra->low_rssi_threshold_ratr_40M & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
 		} else {
 			pra->low_rssi_threshold_ratr =
-			(pra->low_rssi_threshold_ratr_20M & (~BIT31)) | ((bshort_gi_enabled) ? BIT31 : 0);
+				(pra->low_rssi_threshold_ratr_20M & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
 		}
 		pra->ping_rssi_ratr =
-				(pra->ping_rssi_ratr & (~BIT31)) | ((bshort_gi_enabled) ? BIT31 : 0);
+				(pra->ping_rssi_ratr & (~BIT31)) |
+				((bshort_gi_enabled) ? BIT31 : 0);
 
 		if (pra->ratr_state == DM_RATR_STA_HIGH) {
-			HighRSSIThreshForRA	= pra->high2low_rssi_thresh_for_ra;
-			LowRSSIThreshForRA	= (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+			HighRSSIThreshForRA = pra->high2low_rssi_thresh_for_ra;
+			LowRSSIThreshForRA = (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
 					(pra->low_rssi_thresh_for_ra40M) : (pra->low_rssi_thresh_for_ra20M);
 		} else if (pra->ratr_state == DM_RATR_STA_LOW) {
-			HighRSSIThreshForRA	= pra->high_rssi_thresh_for_ra;
-			LowRSSIThreshForRA	= (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+			HighRSSIThreshForRA = pra->high_rssi_thresh_for_ra;
+			LowRSSIThreshForRA = (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
 					(pra->low2high_rssi_thresh_for_ra40M) : (pra->low2high_rssi_thresh_for_ra20M);
 		} else {
-			HighRSSIThreshForRA	= pra->high_rssi_thresh_for_ra;
-			LowRSSIThreshForRA	= (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
+			HighRSSIThreshForRA = pra->high_rssi_thresh_for_ra;
+			LowRSSIThreshForRA = (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20) ?
 					(pra->low_rssi_thresh_for_ra40M) : (pra->low_rssi_thresh_for_ra20M);
 		}
 
-		if (priv->undecorated_smoothed_pwdb >= (long)HighRSSIThreshForRA) {
+		if (priv->undecorated_smoothed_pwdb >=
+		    (long)HighRSSIThreshForRA) {
 			pra->ratr_state = DM_RATR_STA_HIGH;
 			targetRATR = pra->upper_rssi_threshold_ratr;
-		} else if (priv->undecorated_smoothed_pwdb >= (long)LowRSSIThreshForRA) {
+		} else if (priv->undecorated_smoothed_pwdb >=
+			   (long)LowRSSIThreshForRA) {
 			pra->ratr_state = DM_RATR_STA_MIDDLE;
 			targetRATR = pra->middle_rssi_threshold_ratr;
 		} else {
@@ -327,8 +427,10 @@ static void dm_check_rate_adaptive(struct net_device *dev)
 		}
 
 		if (pra->ping_rssi_enable) {
-			if (priv->undecorated_smoothed_pwdb < (long)(pra->ping_rssi_thresh_for_ra+5)) {
-				if ((priv->undecorated_smoothed_pwdb < (long)pra->ping_rssi_thresh_for_ra) ||
+			if (priv->undecorated_smoothed_pwdb <
+			    (long)(pra->ping_rssi_thresh_for_ra+5)) {
+				if ((priv->undecorated_smoothed_pwdb <
+				     (long)pra->ping_rssi_thresh_for_ra) ||
 				    ping_rssi_state) {
 					pra->ratr_state = DM_RATR_STA_LOW;
 					targetRATR = pra->ping_rssi_ratr;
@@ -449,6 +551,97 @@ static u8	CCKSwingTable_Ch14[CCK_Table_length][8] = {
 #define		Tssi_Report_Value2			0x13e
 #define		FW_Busy_Flag				0x13f
 
+static void dm_tx_update_tssi_weak_signal(struct net_device *dev, u8 RF_Type)
+{
+	struct r8192_priv *p = rtllib_priv(dev);
+
+	if (RF_Type == RF_2T4R) {
+		if ((p->rfa_txpowertrackingindex > 0) &&
+		    (p->rfc_txpowertrackingindex > 0)) {
+			p->rfa_txpowertrackingindex--;
+			if (p->rfa_txpowertrackingindex_real > 4) {
+				p->rfa_txpowertrackingindex_real--;
+				rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+						 bMaskDWord,
+						 dm_tx_bb_gain[p->rfa_txpowertrackingindex_real]);
+			}
+
+			p->rfc_txpowertrackingindex--;
+			if (p->rfc_txpowertrackingindex_real > 4) {
+				p->rfc_txpowertrackingindex_real--;
+				rtl8192_setBBreg(dev,
+						 rOFDM0_XCTxIQImbalance,
+						 bMaskDWord,
+						 dm_tx_bb_gain[p->rfc_txpowertrackingindex_real]);
+			}
+		} else {
+			rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+					 bMaskDWord,
+					 dm_tx_bb_gain[4]);
+			rtl8192_setBBreg(dev,
+					 rOFDM0_XCTxIQImbalance,
+					 bMaskDWord, dm_tx_bb_gain[4]);
+		}
+	} else {
+		if (p->rfa_txpowertrackingindex > 0) {
+			p->rfa_txpowertrackingindex--;
+			if (p->rfa_txpowertrackingindex_real > 4) {
+				p->rfa_txpowertrackingindex_real--;
+				rtl8192_setBBreg(dev,
+						 rOFDM0_XATxIQImbalance,
+						 bMaskDWord,
+						 dm_tx_bb_gain[p->rfa_txpowertrackingindex_real]);
+			}
+		} else {
+			rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+					 bMaskDWord, dm_tx_bb_gain[4]);
+		}
+	}
+}
+
+static void dm_tx_update_tssi_strong_signal(struct net_device *dev, u8 RF_Type)
+{
+	struct r8192_priv *p = rtllib_priv(dev);
+
+	if (RF_Type == RF_2T4R) {
+		if ((p->rfa_txpowertrackingindex < TxBBGainTableLength - 1) &&
+		    (p->rfc_txpowertrackingindex < TxBBGainTableLength - 1)) {
+			p->rfa_txpowertrackingindex++;
+			p->rfa_txpowertrackingindex_real++;
+			rtl8192_setBBreg(dev,
+				 rOFDM0_XATxIQImbalance,
+				 bMaskDWord,
+				 dm_tx_bb_gain[p->rfa_txpowertrackingindex_real]);
+			p->rfc_txpowertrackingindex++;
+			p->rfc_txpowertrackingindex_real++;
+			rtl8192_setBBreg(dev,
+				 rOFDM0_XCTxIQImbalance,
+				 bMaskDWord,
+				 dm_tx_bb_gain[p->rfc_txpowertrackingindex_real]);
+		} else {
+			rtl8192_setBBreg(dev,
+				 rOFDM0_XATxIQImbalance,
+				 bMaskDWord,
+				 dm_tx_bb_gain[TxBBGainTableLength - 1]);
+			rtl8192_setBBreg(dev, rOFDM0_XCTxIQImbalance,
+					 bMaskDWord,
+					 dm_tx_bb_gain[TxBBGainTableLength - 1]);
+		}
+	} else {
+		if (p->rfa_txpowertrackingindex < (TxBBGainTableLength - 1)) {
+			p->rfa_txpowertrackingindex++;
+			p->rfa_txpowertrackingindex_real++;
+			rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+					 bMaskDWord,
+					 dm_tx_bb_gain[p->rfa_txpowertrackingindex_real]);
+		} else {
+			rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
+					 bMaskDWord,
+					 dm_tx_bb_gain[TxBBGainTableLength - 1]);
+		}
+	}
+}
+
 static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
@@ -538,7 +731,8 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 			if (viviflag) {
 				write_nic_byte(dev, Pw_Track_Flag, 0);
 				viviflag = false;
-				RT_TRACE(COMP_POWER_TRACKING, "we filted this data\n");
+				RT_TRACE(COMP_POWER_TRACKING,
+					 "we filted this data\n");
 				for (k = 0; k < 5; k++)
 					tmp_report[k] = 0;
 				break;
@@ -547,12 +741,13 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 			for (k = 0; k < 5; k++)
 				Avg_TSSI_Meas_from_driver += tmp_report[k];
 
-			Avg_TSSI_Meas_from_driver = Avg_TSSI_Meas_from_driver*100/5;
+			Avg_TSSI_Meas_from_driver *= 100 / 5;
 			RT_TRACE(COMP_POWER_TRACKING,
 				 "Avg_TSSI_Meas_from_driver = %d\n",
 				 Avg_TSSI_Meas_from_driver);
 			TSSI_13dBm = priv->TSSI_13dBm;
-			RT_TRACE(COMP_POWER_TRACKING, "TSSI_13dBm = %d\n", TSSI_13dBm);
+			RT_TRACE(COMP_POWER_TRACKING, "TSSI_13dBm = %d\n",
+				 TSSI_13dBm);
 
 			if (Avg_TSSI_Meas_from_driver > TSSI_13dBm)
 				delta = Avg_TSSI_Meas_from_driver - TSSI_13dBm;
@@ -579,92 +774,11 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 					 priv->CCKPresentAttentuation);
 				return;
 			}
-			if (Avg_TSSI_Meas_from_driver < TSSI_13dBm - E_FOR_TX_POWER_TRACK) {
-				if (RF_Type == RF_2T4R) {
-
-					if ((priv->rfa_txpowertrackingindex > 0) &&
-					    (priv->rfc_txpowertrackingindex > 0)) {
-						priv->rfa_txpowertrackingindex--;
-						if (priv->rfa_txpowertrackingindex_real > 4) {
-							priv->rfa_txpowertrackingindex_real--;
-							rtl8192_setBBreg(dev,
-								 rOFDM0_XATxIQImbalance,
-								 bMaskDWord,
-								 priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
-						}
-
-						priv->rfc_txpowertrackingindex--;
-						if (priv->rfc_txpowertrackingindex_real > 4) {
-							priv->rfc_txpowertrackingindex_real--;
-							rtl8192_setBBreg(dev,
-								 rOFDM0_XCTxIQImbalance,
-								 bMaskDWord,
-								 priv->txbbgain_table[priv->rfc_txpowertrackingindex_real].txbbgain_value);
-						}
-					} else {
-						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
-								 bMaskDWord,
-								 priv->txbbgain_table[4].txbbgain_value);
-						rtl8192_setBBreg(dev,
-								 rOFDM0_XCTxIQImbalance,
-								 bMaskDWord, priv->txbbgain_table[4].txbbgain_value);
-					}
-				} else {
-					if (priv->rfa_txpowertrackingindex > 0) {
-						priv->rfa_txpowertrackingindex--;
-						if (priv->rfa_txpowertrackingindex_real > 4) {
-							priv->rfa_txpowertrackingindex_real--;
-							rtl8192_setBBreg(dev,
-									 rOFDM0_XATxIQImbalance,
-									 bMaskDWord,
-									 priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
-						}
-					} else
-						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
-								 bMaskDWord, priv->txbbgain_table[4].txbbgain_value);
+			if (Avg_TSSI_Meas_from_driver < TSSI_13dBm - E_FOR_TX_POWER_TRACK)
+				dm_tx_update_tssi_weak_signal(dev, RF_Type);
+			else
+				dm_tx_update_tssi_strong_signal(dev, RF_Type);
 
-				}
-			} else {
-				if (RF_Type == RF_2T4R) {
-					if ((priv->rfa_txpowertrackingindex <
-					    TxBBGainTableLength - 1) &&
-					    (priv->rfc_txpowertrackingindex <
-					    TxBBGainTableLength - 1)) {
-						priv->rfa_txpowertrackingindex++;
-						priv->rfa_txpowertrackingindex_real++;
-						rtl8192_setBBreg(dev,
-							 rOFDM0_XATxIQImbalance,
-							 bMaskDWord,
-							 priv->txbbgain_table
-							 [priv->rfa_txpowertrackingindex_real].txbbgain_value);
-						priv->rfc_txpowertrackingindex++;
-						priv->rfc_txpowertrackingindex_real++;
-						rtl8192_setBBreg(dev,
-							 rOFDM0_XCTxIQImbalance,
-							 bMaskDWord,
-							 priv->txbbgain_table[priv->rfc_txpowertrackingindex_real].txbbgain_value);
-					} else {
-						rtl8192_setBBreg(dev,
-							 rOFDM0_XATxIQImbalance,
-							 bMaskDWord,
-							 priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
-						rtl8192_setBBreg(dev,
-							 rOFDM0_XCTxIQImbalance,
-							 bMaskDWord, priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
-					}
-				} else {
-					if (priv->rfa_txpowertrackingindex < (TxBBGainTableLength - 1)) {
-						priv->rfa_txpowertrackingindex++;
-						priv->rfa_txpowertrackingindex_real++;
-						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
-								 bMaskDWord,
-								 priv->txbbgain_table[priv->rfa_txpowertrackingindex_real].txbbgain_value);
-					} else
-						rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance,
-								 bMaskDWord,
-								 priv->txbbgain_table[TxBBGainTableLength - 1].txbbgain_value);
-				}
-			}
 			if (RF_Type == RF_2T4R) {
 				priv->CCKPresentAttentuation_difference
 					= priv->rfa_txpowertrackingindex - priv->rfa_txpowertracking_default;
@@ -712,11 +826,13 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 				 "priv->CCKPresentAttentuation = %d\n",
 				 priv->CCKPresentAttentuation);
 
-			if (priv->CCKPresentAttentuation_difference <= -12 || priv->CCKPresentAttentuation_difference >= 24) {
+			if (priv->CCKPresentAttentuation_difference <= -12 ||
+			    priv->CCKPresentAttentuation_difference >= 24) {
 				priv->rtllib->bdynamic_txpower_enable = true;
 				write_nic_byte(dev, Pw_Track_Flag, 0);
 				write_nic_byte(dev, FW_Busy_Flag, 0);
-				RT_TRACE(COMP_POWER_TRACKING, "tx power track--->limited\n");
+				RT_TRACE(COMP_POWER_TRACKING,
+					 "tx power track--->limited\n");
 				return;
 			}
 
@@ -741,12 +857,15 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device *dev)
 	int i = 0, CCKSwingNeedUpdate = 0;
 
 	if (!priv->btxpower_trackingInit) {
-		tmpRegA = rtl8192_QueryBBReg(dev, rOFDM0_XATxIQImbalance, bMaskDWord);
+		tmpRegA = rtl8192_QueryBBReg(dev, rOFDM0_XATxIQImbalance,
+					     bMaskDWord);
 		for (i = 0; i < OFDM_Table_Length; i++) {
 			if (tmpRegA == OFDMSwingTable[i]) {
 				priv->OFDM_index[0] = (u8)i;
-				RT_TRACE(COMP_POWER_TRACKING, "Initial reg0x%x = 0x%x, OFDM_index = 0x%x\n",
-					rOFDM0_XATxIQImbalance, tmpRegA, priv->OFDM_index[0]);
+				RT_TRACE(COMP_POWER_TRACKING,
+					 "Initial reg0x%x = 0x%x, OFDM_index = 0x%x\n",
+					 rOFDM0_XATxIQImbalance, tmpRegA,
+					 priv->OFDM_index[0]);
 			}
 		}
 
@@ -849,495 +968,6 @@ static void dm_InitializeTXPowerTracking_TSSI(struct net_device *dev)
 
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	priv->txbbgain_table[0].txbb_iq_amplifygain = 12;
-	priv->txbbgain_table[0].txbbgain_value = 0x7f8001fe;
-	priv->txbbgain_table[1].txbb_iq_amplifygain = 11;
-	priv->txbbgain_table[1].txbbgain_value = 0x788001e2;
-	priv->txbbgain_table[2].txbb_iq_amplifygain = 10;
-	priv->txbbgain_table[2].txbbgain_value = 0x71c001c7;
-	priv->txbbgain_table[3].txbb_iq_amplifygain = 9;
-	priv->txbbgain_table[3].txbbgain_value = 0x6b8001ae;
-	priv->txbbgain_table[4].txbb_iq_amplifygain = 8;
-	priv->txbbgain_table[4].txbbgain_value = 0x65400195;
-	priv->txbbgain_table[5].txbb_iq_amplifygain = 7;
-	priv->txbbgain_table[5].txbbgain_value = 0x5fc0017f;
-	priv->txbbgain_table[6].txbb_iq_amplifygain = 6;
-	priv->txbbgain_table[6].txbbgain_value = 0x5a400169;
-	priv->txbbgain_table[7].txbb_iq_amplifygain = 5;
-	priv->txbbgain_table[7].txbbgain_value = 0x55400155;
-	priv->txbbgain_table[8].txbb_iq_amplifygain = 4;
-	priv->txbbgain_table[8].txbbgain_value = 0x50800142;
-	priv->txbbgain_table[9].txbb_iq_amplifygain = 3;
-	priv->txbbgain_table[9].txbbgain_value = 0x4c000130;
-	priv->txbbgain_table[10].txbb_iq_amplifygain = 2;
-	priv->txbbgain_table[10].txbbgain_value = 0x47c0011f;
-	priv->txbbgain_table[11].txbb_iq_amplifygain = 1;
-	priv->txbbgain_table[11].txbbgain_value = 0x43c0010f;
-	priv->txbbgain_table[12].txbb_iq_amplifygain = 0;
-	priv->txbbgain_table[12].txbbgain_value = 0x40000100;
-	priv->txbbgain_table[13].txbb_iq_amplifygain = -1;
-	priv->txbbgain_table[13].txbbgain_value = 0x3c8000f2;
-	priv->txbbgain_table[14].txbb_iq_amplifygain = -2;
-	priv->txbbgain_table[14].txbbgain_value = 0x390000e4;
-	priv->txbbgain_table[15].txbb_iq_amplifygain = -3;
-	priv->txbbgain_table[15].txbbgain_value = 0x35c000d7;
-	priv->txbbgain_table[16].txbb_iq_amplifygain = -4;
-	priv->txbbgain_table[16].txbbgain_value = 0x32c000cb;
-	priv->txbbgain_table[17].txbb_iq_amplifygain = -5;
-	priv->txbbgain_table[17].txbbgain_value = 0x300000c0;
-	priv->txbbgain_table[18].txbb_iq_amplifygain = -6;
-	priv->txbbgain_table[18].txbbgain_value = 0x2d4000b5;
-	priv->txbbgain_table[19].txbb_iq_amplifygain = -7;
-	priv->txbbgain_table[19].txbbgain_value = 0x2ac000ab;
-	priv->txbbgain_table[20].txbb_iq_amplifygain = -8;
-	priv->txbbgain_table[20].txbbgain_value = 0x288000a2;
-	priv->txbbgain_table[21].txbb_iq_amplifygain = -9;
-	priv->txbbgain_table[21].txbbgain_value = 0x26000098;
-	priv->txbbgain_table[22].txbb_iq_amplifygain = -10;
-	priv->txbbgain_table[22].txbbgain_value = 0x24000090;
-	priv->txbbgain_table[23].txbb_iq_amplifygain = -11;
-	priv->txbbgain_table[23].txbbgain_value = 0x22000088;
-	priv->txbbgain_table[24].txbb_iq_amplifygain = -12;
-	priv->txbbgain_table[24].txbbgain_value = 0x20000080;
-	priv->txbbgain_table[25].txbb_iq_amplifygain = -13;
-	priv->txbbgain_table[25].txbbgain_value = 0x1a00006c;
-	priv->txbbgain_table[26].txbb_iq_amplifygain = -14;
-	priv->txbbgain_table[26].txbbgain_value = 0x1c800072;
-	priv->txbbgain_table[27].txbb_iq_amplifygain = -15;
-	priv->txbbgain_table[27].txbbgain_value = 0x18000060;
-	priv->txbbgain_table[28].txbb_iq_amplifygain = -16;
-	priv->txbbgain_table[28].txbbgain_value = 0x19800066;
-	priv->txbbgain_table[29].txbb_iq_amplifygain = -17;
-	priv->txbbgain_table[29].txbbgain_value = 0x15800056;
-	priv->txbbgain_table[30].txbb_iq_amplifygain = -18;
-	priv->txbbgain_table[30].txbbgain_value = 0x26c0005b;
-	priv->txbbgain_table[31].txbb_iq_amplifygain = -19;
-	priv->txbbgain_table[31].txbbgain_value = 0x14400051;
-	priv->txbbgain_table[32].txbb_iq_amplifygain = -20;
-	priv->txbbgain_table[32].txbbgain_value = 0x24400051;
-	priv->txbbgain_table[33].txbb_iq_amplifygain = -21;
-	priv->txbbgain_table[33].txbbgain_value = 0x1300004c;
-	priv->txbbgain_table[34].txbb_iq_amplifygain = -22;
-	priv->txbbgain_table[34].txbbgain_value = 0x12000048;
-	priv->txbbgain_table[35].txbb_iq_amplifygain = -23;
-	priv->txbbgain_table[35].txbbgain_value = 0x11000044;
-	priv->txbbgain_table[36].txbb_iq_amplifygain = -24;
-	priv->txbbgain_table[36].txbbgain_value = 0x10000040;
-
-	priv->cck_txbbgain_table[0].ccktxbb_valuearray[0] = 0x36;
-	priv->cck_txbbgain_table[0].ccktxbb_valuearray[1] = 0x35;
-	priv->cck_txbbgain_table[0].ccktxbb_valuearray[2] = 0x2e;
-	priv->cck_txbbgain_table[0].ccktxbb_valuearray[3] = 0x25;
-	priv->cck_txbbgain_table[0].ccktxbb_valuearray[4] = 0x1c;
-	priv->cck_txbbgain_table[0].ccktxbb_valuearray[5] = 0x12;
-	priv->cck_txbbgain_table[0].ccktxbb_valuearray[6] = 0x09;
-	priv->cck_txbbgain_table[0].ccktxbb_valuearray[7] = 0x04;
-
-	priv->cck_txbbgain_table[1].ccktxbb_valuearray[0] = 0x33;
-	priv->cck_txbbgain_table[1].ccktxbb_valuearray[1] = 0x32;
-	priv->cck_txbbgain_table[1].ccktxbb_valuearray[2] = 0x2b;
-	priv->cck_txbbgain_table[1].ccktxbb_valuearray[3] = 0x23;
-	priv->cck_txbbgain_table[1].ccktxbb_valuearray[4] = 0x1a;
-	priv->cck_txbbgain_table[1].ccktxbb_valuearray[5] = 0x11;
-	priv->cck_txbbgain_table[1].ccktxbb_valuearray[6] = 0x08;
-	priv->cck_txbbgain_table[1].ccktxbb_valuearray[7] = 0x04;
-
-	priv->cck_txbbgain_table[2].ccktxbb_valuearray[0] = 0x30;
-	priv->cck_txbbgain_table[2].ccktxbb_valuearray[1] = 0x2f;
-	priv->cck_txbbgain_table[2].ccktxbb_valuearray[2] = 0x29;
-	priv->cck_txbbgain_table[2].ccktxbb_valuearray[3] = 0x21;
-	priv->cck_txbbgain_table[2].ccktxbb_valuearray[4] = 0x19;
-	priv->cck_txbbgain_table[2].ccktxbb_valuearray[5] = 0x10;
-	priv->cck_txbbgain_table[2].ccktxbb_valuearray[6] = 0x08;
-	priv->cck_txbbgain_table[2].ccktxbb_valuearray[7] = 0x03;
-
-	priv->cck_txbbgain_table[3].ccktxbb_valuearray[0] = 0x2d;
-	priv->cck_txbbgain_table[3].ccktxbb_valuearray[1] = 0x2d;
-	priv->cck_txbbgain_table[3].ccktxbb_valuearray[2] = 0x27;
-	priv->cck_txbbgain_table[3].ccktxbb_valuearray[3] = 0x1f;
-	priv->cck_txbbgain_table[3].ccktxbb_valuearray[4] = 0x18;
-	priv->cck_txbbgain_table[3].ccktxbb_valuearray[5] = 0x0f;
-	priv->cck_txbbgain_table[3].ccktxbb_valuearray[6] = 0x08;
-	priv->cck_txbbgain_table[3].ccktxbb_valuearray[7] = 0x03;
-
-	priv->cck_txbbgain_table[4].ccktxbb_valuearray[0] = 0x2b;
-	priv->cck_txbbgain_table[4].ccktxbb_valuearray[1] = 0x2a;
-	priv->cck_txbbgain_table[4].ccktxbb_valuearray[2] = 0x25;
-	priv->cck_txbbgain_table[4].ccktxbb_valuearray[3] = 0x1e;
-	priv->cck_txbbgain_table[4].ccktxbb_valuearray[4] = 0x16;
-	priv->cck_txbbgain_table[4].ccktxbb_valuearray[5] = 0x0e;
-	priv->cck_txbbgain_table[4].ccktxbb_valuearray[6] = 0x07;
-	priv->cck_txbbgain_table[4].ccktxbb_valuearray[7] = 0x03;
-
-	priv->cck_txbbgain_table[5].ccktxbb_valuearray[0] = 0x28;
-	priv->cck_txbbgain_table[5].ccktxbb_valuearray[1] = 0x28;
-	priv->cck_txbbgain_table[5].ccktxbb_valuearray[2] = 0x22;
-	priv->cck_txbbgain_table[5].ccktxbb_valuearray[3] = 0x1c;
-	priv->cck_txbbgain_table[5].ccktxbb_valuearray[4] = 0x15;
-	priv->cck_txbbgain_table[5].ccktxbb_valuearray[5] = 0x0d;
-	priv->cck_txbbgain_table[5].ccktxbb_valuearray[6] = 0x07;
-	priv->cck_txbbgain_table[5].ccktxbb_valuearray[7] = 0x03;
-
-	priv->cck_txbbgain_table[6].ccktxbb_valuearray[0] = 0x26;
-	priv->cck_txbbgain_table[6].ccktxbb_valuearray[1] = 0x25;
-	priv->cck_txbbgain_table[6].ccktxbb_valuearray[2] = 0x21;
-	priv->cck_txbbgain_table[6].ccktxbb_valuearray[3] = 0x1b;
-	priv->cck_txbbgain_table[6].ccktxbb_valuearray[4] = 0x14;
-	priv->cck_txbbgain_table[6].ccktxbb_valuearray[5] = 0x0d;
-	priv->cck_txbbgain_table[6].ccktxbb_valuearray[6] = 0x06;
-	priv->cck_txbbgain_table[6].ccktxbb_valuearray[7] = 0x03;
-
-	priv->cck_txbbgain_table[7].ccktxbb_valuearray[0] = 0x24;
-	priv->cck_txbbgain_table[7].ccktxbb_valuearray[1] = 0x23;
-	priv->cck_txbbgain_table[7].ccktxbb_valuearray[2] = 0x1f;
-	priv->cck_txbbgain_table[7].ccktxbb_valuearray[3] = 0x19;
-	priv->cck_txbbgain_table[7].ccktxbb_valuearray[4] = 0x13;
-	priv->cck_txbbgain_table[7].ccktxbb_valuearray[5] = 0x0c;
-	priv->cck_txbbgain_table[7].ccktxbb_valuearray[6] = 0x06;
-	priv->cck_txbbgain_table[7].ccktxbb_valuearray[7] = 0x03;
-
-	priv->cck_txbbgain_table[8].ccktxbb_valuearray[0] = 0x22;
-	priv->cck_txbbgain_table[8].ccktxbb_valuearray[1] = 0x21;
-	priv->cck_txbbgain_table[8].ccktxbb_valuearray[2] = 0x1d;
-	priv->cck_txbbgain_table[8].ccktxbb_valuearray[3] = 0x18;
-	priv->cck_txbbgain_table[8].ccktxbb_valuearray[4] = 0x11;
-	priv->cck_txbbgain_table[8].ccktxbb_valuearray[5] = 0x0b;
-	priv->cck_txbbgain_table[8].ccktxbb_valuearray[6] = 0x06;
-	priv->cck_txbbgain_table[8].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[9].ccktxbb_valuearray[0] = 0x20;
-	priv->cck_txbbgain_table[9].ccktxbb_valuearray[1] = 0x20;
-	priv->cck_txbbgain_table[9].ccktxbb_valuearray[2] = 0x1b;
-	priv->cck_txbbgain_table[9].ccktxbb_valuearray[3] = 0x16;
-	priv->cck_txbbgain_table[9].ccktxbb_valuearray[4] = 0x11;
-	priv->cck_txbbgain_table[9].ccktxbb_valuearray[5] = 0x08;
-	priv->cck_txbbgain_table[9].ccktxbb_valuearray[6] = 0x05;
-	priv->cck_txbbgain_table[9].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[10].ccktxbb_valuearray[0] = 0x1f;
-	priv->cck_txbbgain_table[10].ccktxbb_valuearray[1] = 0x1e;
-	priv->cck_txbbgain_table[10].ccktxbb_valuearray[2] = 0x1a;
-	priv->cck_txbbgain_table[10].ccktxbb_valuearray[3] = 0x15;
-	priv->cck_txbbgain_table[10].ccktxbb_valuearray[4] = 0x10;
-	priv->cck_txbbgain_table[10].ccktxbb_valuearray[5] = 0x0a;
-	priv->cck_txbbgain_table[10].ccktxbb_valuearray[6] = 0x05;
-	priv->cck_txbbgain_table[10].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[11].ccktxbb_valuearray[0] = 0x1d;
-	priv->cck_txbbgain_table[11].ccktxbb_valuearray[1] = 0x1c;
-	priv->cck_txbbgain_table[11].ccktxbb_valuearray[2] = 0x18;
-	priv->cck_txbbgain_table[11].ccktxbb_valuearray[3] = 0x14;
-	priv->cck_txbbgain_table[11].ccktxbb_valuearray[4] = 0x0f;
-	priv->cck_txbbgain_table[11].ccktxbb_valuearray[5] = 0x0a;
-	priv->cck_txbbgain_table[11].ccktxbb_valuearray[6] = 0x05;
-	priv->cck_txbbgain_table[11].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[12].ccktxbb_valuearray[0] = 0x1b;
-	priv->cck_txbbgain_table[12].ccktxbb_valuearray[1] = 0x1a;
-	priv->cck_txbbgain_table[12].ccktxbb_valuearray[2] = 0x17;
-	priv->cck_txbbgain_table[12].ccktxbb_valuearray[3] = 0x13;
-	priv->cck_txbbgain_table[12].ccktxbb_valuearray[4] = 0x0e;
-	priv->cck_txbbgain_table[12].ccktxbb_valuearray[5] = 0x09;
-	priv->cck_txbbgain_table[12].ccktxbb_valuearray[6] = 0x04;
-	priv->cck_txbbgain_table[12].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[13].ccktxbb_valuearray[0] = 0x1a;
-	priv->cck_txbbgain_table[13].ccktxbb_valuearray[1] = 0x19;
-	priv->cck_txbbgain_table[13].ccktxbb_valuearray[2] = 0x16;
-	priv->cck_txbbgain_table[13].ccktxbb_valuearray[3] = 0x12;
-	priv->cck_txbbgain_table[13].ccktxbb_valuearray[4] = 0x0d;
-	priv->cck_txbbgain_table[13].ccktxbb_valuearray[5] = 0x09;
-	priv->cck_txbbgain_table[13].ccktxbb_valuearray[6] = 0x04;
-	priv->cck_txbbgain_table[13].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[14].ccktxbb_valuearray[0] = 0x18;
-	priv->cck_txbbgain_table[14].ccktxbb_valuearray[1] = 0x17;
-	priv->cck_txbbgain_table[14].ccktxbb_valuearray[2] = 0x15;
-	priv->cck_txbbgain_table[14].ccktxbb_valuearray[3] = 0x11;
-	priv->cck_txbbgain_table[14].ccktxbb_valuearray[4] = 0x0c;
-	priv->cck_txbbgain_table[14].ccktxbb_valuearray[5] = 0x08;
-	priv->cck_txbbgain_table[14].ccktxbb_valuearray[6] = 0x04;
-	priv->cck_txbbgain_table[14].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[15].ccktxbb_valuearray[0] = 0x17;
-	priv->cck_txbbgain_table[15].ccktxbb_valuearray[1] = 0x16;
-	priv->cck_txbbgain_table[15].ccktxbb_valuearray[2] = 0x13;
-	priv->cck_txbbgain_table[15].ccktxbb_valuearray[3] = 0x10;
-	priv->cck_txbbgain_table[15].ccktxbb_valuearray[4] = 0x0c;
-	priv->cck_txbbgain_table[15].ccktxbb_valuearray[5] = 0x08;
-	priv->cck_txbbgain_table[15].ccktxbb_valuearray[6] = 0x04;
-	priv->cck_txbbgain_table[15].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[16].ccktxbb_valuearray[0] = 0x16;
-	priv->cck_txbbgain_table[16].ccktxbb_valuearray[1] = 0x15;
-	priv->cck_txbbgain_table[16].ccktxbb_valuearray[2] = 0x12;
-	priv->cck_txbbgain_table[16].ccktxbb_valuearray[3] = 0x0f;
-	priv->cck_txbbgain_table[16].ccktxbb_valuearray[4] = 0x0b;
-	priv->cck_txbbgain_table[16].ccktxbb_valuearray[5] = 0x07;
-	priv->cck_txbbgain_table[16].ccktxbb_valuearray[6] = 0x04;
-	priv->cck_txbbgain_table[16].ccktxbb_valuearray[7] = 0x01;
-
-	priv->cck_txbbgain_table[17].ccktxbb_valuearray[0] = 0x14;
-	priv->cck_txbbgain_table[17].ccktxbb_valuearray[1] = 0x14;
-	priv->cck_txbbgain_table[17].ccktxbb_valuearray[2] = 0x11;
-	priv->cck_txbbgain_table[17].ccktxbb_valuearray[3] = 0x0e;
-	priv->cck_txbbgain_table[17].ccktxbb_valuearray[4] = 0x0b;
-	priv->cck_txbbgain_table[17].ccktxbb_valuearray[5] = 0x07;
-	priv->cck_txbbgain_table[17].ccktxbb_valuearray[6] = 0x03;
-	priv->cck_txbbgain_table[17].ccktxbb_valuearray[7] = 0x02;
-
-	priv->cck_txbbgain_table[18].ccktxbb_valuearray[0] = 0x13;
-	priv->cck_txbbgain_table[18].ccktxbb_valuearray[1] = 0x13;
-	priv->cck_txbbgain_table[18].ccktxbb_valuearray[2] = 0x10;
-	priv->cck_txbbgain_table[18].ccktxbb_valuearray[3] = 0x0d;
-	priv->cck_txbbgain_table[18].ccktxbb_valuearray[4] = 0x0a;
-	priv->cck_txbbgain_table[18].ccktxbb_valuearray[5] = 0x06;
-	priv->cck_txbbgain_table[18].ccktxbb_valuearray[6] = 0x03;
-	priv->cck_txbbgain_table[18].ccktxbb_valuearray[7] = 0x01;
-
-	priv->cck_txbbgain_table[19].ccktxbb_valuearray[0] = 0x12;
-	priv->cck_txbbgain_table[19].ccktxbb_valuearray[1] = 0x12;
-	priv->cck_txbbgain_table[19].ccktxbb_valuearray[2] = 0x0f;
-	priv->cck_txbbgain_table[19].ccktxbb_valuearray[3] = 0x0c;
-	priv->cck_txbbgain_table[19].ccktxbb_valuearray[4] = 0x09;
-	priv->cck_txbbgain_table[19].ccktxbb_valuearray[5] = 0x06;
-	priv->cck_txbbgain_table[19].ccktxbb_valuearray[6] = 0x03;
-	priv->cck_txbbgain_table[19].ccktxbb_valuearray[7] = 0x01;
-
-	priv->cck_txbbgain_table[20].ccktxbb_valuearray[0] = 0x11;
-	priv->cck_txbbgain_table[20].ccktxbb_valuearray[1] = 0x11;
-	priv->cck_txbbgain_table[20].ccktxbb_valuearray[2] = 0x0f;
-	priv->cck_txbbgain_table[20].ccktxbb_valuearray[3] = 0x0c;
-	priv->cck_txbbgain_table[20].ccktxbb_valuearray[4] = 0x09;
-	priv->cck_txbbgain_table[20].ccktxbb_valuearray[5] = 0x06;
-	priv->cck_txbbgain_table[20].ccktxbb_valuearray[6] = 0x03;
-	priv->cck_txbbgain_table[20].ccktxbb_valuearray[7] = 0x01;
-
-	priv->cck_txbbgain_table[21].ccktxbb_valuearray[0] = 0x10;
-	priv->cck_txbbgain_table[21].ccktxbb_valuearray[1] = 0x10;
-	priv->cck_txbbgain_table[21].ccktxbb_valuearray[2] = 0x0e;
-	priv->cck_txbbgain_table[21].ccktxbb_valuearray[3] = 0x0b;
-	priv->cck_txbbgain_table[21].ccktxbb_valuearray[4] = 0x08;
-	priv->cck_txbbgain_table[21].ccktxbb_valuearray[5] = 0x05;
-	priv->cck_txbbgain_table[21].ccktxbb_valuearray[6] = 0x03;
-	priv->cck_txbbgain_table[21].ccktxbb_valuearray[7] = 0x01;
-
-	priv->cck_txbbgain_table[22].ccktxbb_valuearray[0] = 0x0f;
-	priv->cck_txbbgain_table[22].ccktxbb_valuearray[1] = 0x0f;
-	priv->cck_txbbgain_table[22].ccktxbb_valuearray[2] = 0x0d;
-	priv->cck_txbbgain_table[22].ccktxbb_valuearray[3] = 0x0b;
-	priv->cck_txbbgain_table[22].ccktxbb_valuearray[4] = 0x08;
-	priv->cck_txbbgain_table[22].ccktxbb_valuearray[5] = 0x05;
-	priv->cck_txbbgain_table[22].ccktxbb_valuearray[6] = 0x03;
-	priv->cck_txbbgain_table[22].ccktxbb_valuearray[7] = 0x01;
-
-	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[0] = 0x36;
-	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[1] = 0x35;
-	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[2] = 0x2e;
-	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[3] = 0x1b;
-	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[0].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[0] = 0x33;
-	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[1] = 0x32;
-	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[2] = 0x2b;
-	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[3] = 0x19;
-	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[1].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[0] = 0x30;
-	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[1] = 0x2f;
-	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[2] = 0x29;
-	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[3] = 0x18;
-	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[2].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[0] = 0x2d;
-	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[1] = 0x2d;
-	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[2] = 0x27;
-	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[3] = 0x17;
-	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[3].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[0] = 0x2b;
-	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[1] = 0x2a;
-	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[2] = 0x25;
-	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[3] = 0x15;
-	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[4].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[0] = 0x28;
-	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[1] = 0x28;
-	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[2] = 0x22;
-	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[3] = 0x14;
-	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[5].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[0] = 0x26;
-	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[1] = 0x25;
-	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[2] = 0x21;
-	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[3] = 0x13;
-	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[6].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[0] = 0x24;
-	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[1] = 0x23;
-	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[2] = 0x1f;
-	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[3] = 0x12;
-	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[7].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[0] = 0x22;
-	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[1] = 0x21;
-	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[2] = 0x1d;
-	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[3] = 0x11;
-	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[8].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[0] = 0x20;
-	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[1] = 0x20;
-	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[2] = 0x1b;
-	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[3] = 0x10;
-	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[9].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[0] = 0x1f;
-	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[1] = 0x1e;
-	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[2] = 0x1a;
-	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[3] = 0x0f;
-	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[10].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[0] = 0x1d;
-	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[1] = 0x1c;
-	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[2] = 0x18;
-	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[3] = 0x0e;
-	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[11].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[0] = 0x1b;
-	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[1] = 0x1a;
-	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[2] = 0x17;
-	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[3] = 0x0e;
-	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[12].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[0] = 0x1a;
-	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[1] = 0x19;
-	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[2] = 0x16;
-	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[3] = 0x0d;
-	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[13].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[0] = 0x18;
-	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[1] = 0x17;
-	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[2] = 0x15;
-	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[3] = 0x0c;
-	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[14].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[0] = 0x17;
-	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[1] = 0x16;
-	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[2] = 0x13;
-	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[3] = 0x0b;
-	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[15].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[0] = 0x16;
-	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[1] = 0x15;
-	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[2] = 0x12;
-	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[3] = 0x0b;
-	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[16].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[0] = 0x14;
-	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[1] = 0x14;
-	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[2] = 0x11;
-	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[3] = 0x0a;
-	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[17].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[0] = 0x13;
-	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[1] = 0x13;
-	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[2] = 0x10;
-	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[3] = 0x0a;
-	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[18].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[0] = 0x12;
-	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[1] = 0x12;
-	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[2] = 0x0f;
-	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[3] = 0x09;
-	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[19].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[0] = 0x11;
-	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[1] = 0x11;
-	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[2] = 0x0f;
-	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[3] = 0x09;
-	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[20].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[0] = 0x10;
-	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[1] = 0x10;
-	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[2] = 0x0e;
-	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[3] = 0x08;
-	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[21].ccktxbb_valuearray[7] = 0x00;
-
-	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[0] = 0x0f;
-	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[1] = 0x0f;
-	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[2] = 0x0d;
-	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[3] = 0x08;
-	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[4] = 0x00;
-	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[5] = 0x00;
-	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[6] = 0x00;
-	priv->cck_txbbgain_ch14_table[22].ccktxbb_valuearray[7] = 0x00;
-
 	priv->btxpower_tracking = true;
 	priv->txpower_count       = 0;
 	priv->btxpower_trackingInit = false;
@@ -1383,7 +1013,8 @@ static void dm_CheckTXPowerTracking_TSSI(struct net_device *dev)
 
 
 	 if (tx_power_track_counter >= 180) {
-		queue_delayed_work_rsl(priv->priv_wq, &priv->txpower_tracking_wq, 0);
+		queue_delayed_work_rsl(priv->priv_wq,
+				       &priv->txpower_tracking_wq, 0);
 		tx_power_track_counter = 0;
 	}
 
@@ -1436,88 +1067,91 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 {
 	u32 TempVal;
 	struct r8192_priv *priv = rtllib_priv(dev);
+	u8 attenuation = (u8)priv->CCKPresentAttentuation;
 
 	TempVal = 0;
 	if (!bInCH14) {
-		TempVal = (u32)(priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[0] +
-			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[1]<<8));
+		TempVal = (u32)(dm_cck_tx_bb_gain[attenuation][0] +
+			  (dm_cck_tx_bb_gain[attenuation][1] << 8));
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
-		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));
+		TempVal = (u32)((dm_cck_tx_bb_gain[attenuation][2]) +
+			  (dm_cck_tx_bb_gain[attenuation][3] << 8) +
+			  (dm_cck_tx_bb_gain[attenuation][4] << 16)+
+			  (dm_cck_tx_bb_gain[attenuation][5] << 24));
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
-		TempVal = (u32)(priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[6] +
-			  (priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[7]<<8));
+		TempVal = (u32)(dm_cck_tx_bb_gain[attenuation][6] +
+			  (dm_cck_tx_bb_gain[attenuation][7] << 8));
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 	} else {
-		TempVal = (u32)(priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[0] +
-			  (priv->cck_txbbgain_ch14_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[1]<<8));
+		TempVal = (u32)((dm_cck_tx_bb_gain_ch14[attenuation][0]) +
+			  (dm_cck_tx_bb_gain_ch14[attenuation][1] << 8));
 
 		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
-		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));
+		TempVal = (u32)((dm_cck_tx_bb_gain_ch14[attenuation][2]) +
+			  (dm_cck_tx_bb_gain_ch14[attenuation][3] << 8) +
+			  (dm_cck_tx_bb_gain_ch14[attenuation][4] << 16)+
+			  (dm_cck_tx_bb_gain_ch14[attenuation][5] << 24));
 		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
-		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));
+		TempVal = (u32)((dm_cck_tx_bb_gain_ch14[attenuation][6]) +
+			  (dm_cck_tx_bb_gain_ch14[attenuation][7] << 8));
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 	}
-
-
 }
 
-static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH14)
+static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,
+					     bool bInCH14)
 {
 	u32 TempVal;
 	struct r8192_priv *priv = rtllib_priv(dev);
 
 	TempVal = 0;
 	if (!bInCH14) {
-		TempVal =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][0] +
-					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][1]<<8);
+		TempVal = CCKSwingTable_Ch1_Ch13[priv->CCK_index][0] +
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][1] << 8);
 		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 =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][2] +
-					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][3]<<8) +
-					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][4]<<16)+
-					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][5]<<24);
+		RT_TRACE(COMP_POWER_TRACKING,
+			 "CCK not chnl 14, reg 0x%x = 0x%x\n", rCCK0_TxFilter1,
+			 TempVal);
+		TempVal = CCKSwingTable_Ch1_Ch13[priv->CCK_index][2] +
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][3] << 8) +
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][4] << 16)+
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][5] << 24);
 		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 =	CCKSwingTable_Ch1_Ch13[priv->CCK_index][6] +
-					(CCKSwingTable_Ch1_Ch13[priv->CCK_index][7]<<8);
+		RT_TRACE(COMP_POWER_TRACKING,
+			 "CCK not chnl 14, reg 0x%x = 0x%x\n", rCCK0_TxFilter2,
+			 TempVal);
+		TempVal = CCKSwingTable_Ch1_Ch13[priv->CCK_index][6] +
+			  (CCKSwingTable_Ch1_Ch13[priv->CCK_index][7] << 8);
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
-		RT_TRACE(COMP_POWER_TRACKING, "CCK not chnl 14, reg 0x%x = 0x%x\n",
-			rCCK0_DebugPort, TempVal);
+		RT_TRACE(COMP_POWER_TRACKING,
+			 "CCK not chnl 14, reg 0x%x = 0x%x\n", rCCK0_DebugPort,
+			 TempVal);
 	} else {
-		TempVal =	CCKSwingTable_Ch14[priv->CCK_index][0] +
-					(CCKSwingTable_Ch14[priv->CCK_index][1]<<8);
+		TempVal = CCKSwingTable_Ch14[priv->CCK_index][0] +
+			  (CCKSwingTable_Ch14[priv->CCK_index][1] << 8);
 
 		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 =	CCKSwingTable_Ch14[priv->CCK_index][2] +
-					(CCKSwingTable_Ch14[priv->CCK_index][3]<<8) +
-					(CCKSwingTable_Ch14[priv->CCK_index][4]<<16)+
-					(CCKSwingTable_Ch14[priv->CCK_index][5]<<24);
+		TempVal = CCKSwingTable_Ch14[priv->CCK_index][2] +
+			  (CCKSwingTable_Ch14[priv->CCK_index][3] << 8) +
+			  (CCKSwingTable_Ch14[priv->CCK_index][4] << 16)+
+			  (CCKSwingTable_Ch14[priv->CCK_index][5] << 24);
 		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 =	CCKSwingTable_Ch14[priv->CCK_index][6] +
-					(CCKSwingTable_Ch14[priv->CCK_index][7]<<8);
+		TempVal = CCKSwingTable_Ch14[priv->CCK_index][6] +
+			  (CCKSwingTable_Ch14[priv->CCK_index][7]<<8);
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_DebugPort, TempVal);
 	}
-	}
+}
 
 void dm_cck_txpower_adjust(struct net_device *dev, bool  binch14)
 {
@@ -1535,26 +1169,30 @@ static void dm_txpower_reset_recovery(struct net_device *dev)
 
 	RT_TRACE(COMP_POWER_TRACKING, "Start Reset Recovery ==>\n");
 	rtl8192_setBBreg(dev, rOFDM0_XATxIQImbalance, bMaskDWord,
-			 priv->txbbgain_table[priv->rfa_txpowertrackingindex].txbbgain_value);
+			 dm_tx_bb_gain[priv->rfa_txpowertrackingindex]);
 	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in 0xc80 is %08x\n",
-		 priv->txbbgain_table[priv->rfa_txpowertrackingindex].txbbgain_value);
-	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in RFA_txPowerTrackingIndex is %x\n",
+		 dm_tx_bb_gain[priv->rfa_txpowertrackingindex]);
+	RT_TRACE(COMP_POWER_TRACKING,
+		 "Reset Recovery: Fill in RFA_txPowerTrackingIndex is %x\n",
 		 priv->rfa_txpowertrackingindex);
-	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery : RF A I/Q Amplify Gain is %ld\n",
-		 priv->txbbgain_table[priv->rfa_txpowertrackingindex].txbb_iq_amplifygain);
-	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: CCK Attenuation is %d dB\n",
+	RT_TRACE(COMP_POWER_TRACKING,
+		 "Reset Recovery : RF A I/Q Amplify Gain is %d\n",
+		 dm_tx_bb_gain_idx_to_amplify(priv->rfa_txpowertrackingindex));
+	RT_TRACE(COMP_POWER_TRACKING,
+		 "Reset Recovery: CCK Attenuation is %d dB\n",
 		 priv->CCKPresentAttentuation);
 	dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
 
 	rtl8192_setBBreg(dev, rOFDM0_XCTxIQImbalance, bMaskDWord,
-			 priv->txbbgain_table[priv->rfc_txpowertrackingindex].txbbgain_value);
+			 dm_tx_bb_gain[priv->rfc_txpowertrackingindex]);
 	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in 0xc90 is %08x\n",
-		 priv->txbbgain_table[priv->rfc_txpowertrackingindex].txbbgain_value);
-	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in RFC_txPowerTrackingIndex is %x\n",
+		 dm_tx_bb_gain[priv->rfc_txpowertrackingindex]);
+	RT_TRACE(COMP_POWER_TRACKING,
+		 "Reset Recovery: Fill in RFC_txPowerTrackingIndex is %x\n",
 		 priv->rfc_txpowertrackingindex);
-	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery : RF C I/Q Amplify Gain is %ld\n",
-		 priv->txbbgain_table[priv->rfc_txpowertrackingindex].txbb_iq_amplifygain);
-
+	RT_TRACE(COMP_POWER_TRACKING,
+		 "Reset Recovery : RF C I/Q Amplify Gain is %d\n",
+		 dm_tx_bb_gain_idx_to_amplify(priv->rfc_txpowertrackingindex));
 }
 
 void dm_restore_dynamic_mechanism_state(struct net_device *dev)
@@ -1564,7 +1202,8 @@ void dm_restore_dynamic_mechanism_state(struct net_device *dev)
 	u32 ratr_value;
 
 	if (!priv->up) {
-		RT_TRACE(COMP_RATE, "<---- dm_restore_dynamic_mechanism_state(): driver is going to unload\n");
+		RT_TRACE(COMP_RATE,
+			 "<---- dm_restore_dynamic_mechanism_state(): driver is going to unload\n");
 		return;
 	}
 
@@ -1594,18 +1233,28 @@ static void dm_bb_initialgain_restore(struct net_device *dev)
 		return;
 
 	rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x8);
-	rtl8192_setBBreg(dev, rOFDM0_XAAGCCore1, bit_mask, (u32)priv->initgain_backup.xaagccore1);
-	rtl8192_setBBreg(dev, rOFDM0_XBAGCCore1, bit_mask, (u32)priv->initgain_backup.xbagccore1);
-	rtl8192_setBBreg(dev, rOFDM0_XCAGCCore1, bit_mask, (u32)priv->initgain_backup.xcagccore1);
-	rtl8192_setBBreg(dev, rOFDM0_XDAGCCore1, bit_mask, (u32)priv->initgain_backup.xdagccore1);
+	rtl8192_setBBreg(dev, rOFDM0_XAAGCCore1, bit_mask,
+			 (u32)priv->initgain_backup.xaagccore1);
+	rtl8192_setBBreg(dev, rOFDM0_XBAGCCore1, bit_mask,
+			 (u32)priv->initgain_backup.xbagccore1);
+	rtl8192_setBBreg(dev, rOFDM0_XCAGCCore1, bit_mask,
+			 (u32)priv->initgain_backup.xcagccore1);
+	rtl8192_setBBreg(dev, rOFDM0_XDAGCCore1, bit_mask,
+			 (u32)priv->initgain_backup.xdagccore1);
 	bit_mask  = bMaskByte2;
-	rtl8192_setBBreg(dev, rCCK0_CCA, bit_mask, (u32)priv->initgain_backup.cca);
-
-	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc50 is %x\n", priv->initgain_backup.xaagccore1);
-	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc58 is %x\n", priv->initgain_backup.xbagccore1);
-	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc60 is %x\n", priv->initgain_backup.xcagccore1);
-	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc68 is %x\n", priv->initgain_backup.xdagccore1);
-	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xa0a is %x\n", priv->initgain_backup.cca);
+	rtl8192_setBBreg(dev, rCCK0_CCA, bit_mask,
+			 (u32)priv->initgain_backup.cca);
+
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc50 is %x\n",
+		 priv->initgain_backup.xaagccore1);
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc58 is %x\n",
+		 priv->initgain_backup.xbagccore1);
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc60 is %x\n",
+		 priv->initgain_backup.xcagccore1);
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xc68 is %x\n",
+		 priv->initgain_backup.xdagccore1);
+	RT_TRACE(COMP_DIG, "dm_BBInitialGainRestore 0xa0a is %x\n",
+		 priv->initgain_backup.cca);
 	rtl8192_setBBreg(dev, UFWP, bMaskByte1, 0x1);
 
 }
@@ -1638,58 +1287,17 @@ static void dm_bb_initialgain_backup(struct net_device *dev)
 	bit_mask  = bMaskByte2;
 	priv->initgain_backup.cca = (u8)rtl8192_QueryBBReg(dev, rCCK0_CCA, bit_mask);
 
-	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc50 is %x\n", priv->initgain_backup.xaagccore1);
-	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc58 is %x\n", priv->initgain_backup.xbagccore1);
-	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc60 is %x\n", priv->initgain_backup.xcagccore1);
-	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc68 is %x\n", priv->initgain_backup.xdagccore1);
-	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xa0a is %x\n", priv->initgain_backup.cca);
-
-}
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc50 is %x\n",
+		 priv->initgain_backup.xaagccore1);
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc58 is %x\n",
+		 priv->initgain_backup.xbagccore1);
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc60 is %x\n",
+		 priv->initgain_backup.xcagccore1);
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xc68 is %x\n",
+		 priv->initgain_backup.xdagccore1);
+	RT_TRACE(COMP_DIG, "BBInitialGainBackup 0xa0a is %x\n",
+		 priv->initgain_backup.cca);
 
-void dm_change_dynamic_initgain_thresh(struct net_device *dev,
-				       u32 dm_type, u32 dm_value)
-{
-	if (dm_type == DIG_TYPE_THRESH_HIGH) {
-		dm_digtable.rssi_high_thresh = dm_value;
-	} else if (dm_type == DIG_TYPE_THRESH_LOW) {
-		dm_digtable.rssi_low_thresh = dm_value;
-	} else if (dm_type == DIG_TYPE_THRESH_HIGHPWR_HIGH) {
-		dm_digtable.rssi_high_power_highthresh = dm_value;
-	} else if (dm_type == DIG_TYPE_THRESH_HIGHPWR_LOW) {
-		dm_digtable.rssi_high_power_lowthresh = dm_value;
-	} else if (dm_type == DIG_TYPE_ENABLE) {
-		dm_digtable.dig_state		= DM_STA_DIG_MAX;
-		dm_digtable.dig_enable_flag	= true;
-	} else if (dm_type == DIG_TYPE_DISABLE) {
-		dm_digtable.dig_state		= DM_STA_DIG_MAX;
-		dm_digtable.dig_enable_flag	= false;
-	} else if (dm_type == DIG_TYPE_DBG_MODE) {
-		if (dm_value >= DM_DBG_MAX)
-			dm_value = DM_DBG_OFF;
-		dm_digtable.dbg_mode		= (u8)dm_value;
-	} else if (dm_type == DIG_TYPE_RSSI) {
-		if (dm_value > 100)
-			dm_value = 30;
-		dm_digtable.rssi_val			= (long)dm_value;
-	} else if (dm_type == DIG_TYPE_ALGORITHM) {
-		if (dm_value >= DIG_ALGO_MAX)
-			dm_value = DIG_ALGO_BY_FALSE_ALARM;
-		if (dm_digtable.dig_algorithm != (u8)dm_value)
-			dm_digtable.dig_algorithm_switch = 1;
-		dm_digtable.dig_algorithm	= (u8)dm_value;
-	} else if (dm_type == DIG_TYPE_BACKOFF) {
-		if (dm_value > 30)
-			dm_value = 30;
-		dm_digtable.backoff_val		= (u8)dm_value;
-	} else if (dm_type == DIG_TYPE_RX_GAIN_MIN) {
-		if (dm_value == 0)
-			dm_value = 0x1;
-		dm_digtable.rx_gain_range_min = (u8)dm_value;
-	} else if (dm_type == DIG_TYPE_RX_GAIN_MAX) {
-		if (dm_value > 0x50)
-			dm_value = 0x50;
-		dm_digtable.rx_gain_range_max = (u8)dm_value;
-	}
 }
 
 static void dm_dig_init(struct net_device *dev)
@@ -1697,27 +1305,19 @@ static void dm_dig_init(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 
 	dm_digtable.dig_enable_flag	= true;
-	dm_digtable.Backoff_Enable_Flag = true;
 
 	dm_digtable.dig_algorithm = DIG_ALGO_BY_RSSI;
 
-	dm_digtable.Dig_TwoPort_Algorithm = DIG_TWO_PORT_ALGO_RSSI;
-	dm_digtable.Dig_Ext_Port_Stage = DIG_EXT_PORT_STAGE_MAX;
-	dm_digtable.dbg_mode = DM_DBG_OFF;
 	dm_digtable.dig_algorithm_switch = 0;
 
 	dm_digtable.dig_state		= DM_STA_DIG_MAX;
 	dm_digtable.dig_highpwr_state	= DM_STA_DIG_MAX;
-	dm_digtable.CurSTAConnectState = dm_digtable.PreSTAConnectState = DIG_STA_DISCONNECT;
-	dm_digtable.CurAPConnectState = dm_digtable.PreAPConnectState = DIG_AP_DISCONNECT;
-	dm_digtable.initialgain_lowerbound_state = false;
+	dm_digtable.CurSTAConnectState = DIG_STA_DISCONNECT;
+	dm_digtable.PreSTAConnectState = DIG_STA_DISCONNECT;
 
 	dm_digtable.rssi_low_thresh	= DM_DIG_THRESH_LOW;
 	dm_digtable.rssi_high_thresh	= DM_DIG_THRESH_HIGH;
 
-	dm_digtable.FALowThresh	= DM_FALSEALARM_THRESH_LOW;
-	dm_digtable.FAHighThresh	= DM_FALSEALARM_THRESH_HIGH;
-
 	dm_digtable.rssi_high_power_lowthresh = DM_DIG_HIGH_PWR_THRESH_LOW;
 	dm_digtable.rssi_high_power_highthresh = DM_DIG_HIGH_PWR_THRESH_HIGH;
 
@@ -1728,9 +1328,6 @@ static void dm_dig_init(struct net_device *dev)
 		dm_digtable.rx_gain_range_min = DM_DIG_MIN_Netcore;
 	else
 		dm_digtable.rx_gain_range_min = DM_DIG_MIN;
-
-	dm_digtable.BackoffVal_range_max = DM_DIG_BACKOFF_MAX;
-	dm_digtable.BackoffVal_range_min = DM_DIG_BACKOFF_MIN;
 }
 
 static void dm_ctrl_initgain_byrssi(struct net_device *dev)
@@ -1790,8 +1387,7 @@ static void dm_ctrl_initgain_byrssi_by_driverrssi(
 		dm_digtable.CurSTAConnectState = DIG_STA_DISCONNECT;
 
 
-	if (dm_digtable.dbg_mode == DM_DBG_OFF)
-		dm_digtable.rssi_val = priv->undecorated_smoothed_pwdb;
+	dm_digtable.rssi_val = priv->undecorated_smoothed_pwdb;
 	dm_initial_gain(dev);
 	dm_pd_th(dev);
 	dm_cs_ratio(dev);
@@ -1895,11 +1491,14 @@ static void dm_ctrl_initgain_byrssi_highpwr(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 	static u32 reset_cnt_highpwr;
 
-	if ((priv->undecorated_smoothed_pwdb > dm_digtable.rssi_high_power_lowthresh) &&
-		(priv->undecorated_smoothed_pwdb < dm_digtable.rssi_high_power_highthresh))
+	if ((priv->undecorated_smoothed_pwdb >
+	     dm_digtable.rssi_high_power_lowthresh) &&
+	    (priv->undecorated_smoothed_pwdb <
+	     dm_digtable.rssi_high_power_highthresh))
 		return;
 
-	if (priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_power_highthresh) {
+	if (priv->undecorated_smoothed_pwdb >=
+	    dm_digtable.rssi_high_power_highthresh) {
 		if (dm_digtable.dig_highpwr_state == DM_STA_DIG_ON &&
 			(priv->reset_count == reset_cnt_highpwr))
 			return;
@@ -1915,8 +1514,10 @@ static void dm_ctrl_initgain_byrssi_highpwr(struct net_device *dev)
 			return;
 		dm_digtable.dig_highpwr_state = DM_STA_DIG_OFF;
 
-		if (priv->undecorated_smoothed_pwdb < dm_digtable.rssi_high_power_lowthresh &&
-			 priv->undecorated_smoothed_pwdb >= dm_digtable.rssi_high_thresh) {
+		if ((priv->undecorated_smoothed_pwdb <
+		     dm_digtable.rssi_high_power_lowthresh) &&
+		    (priv->undecorated_smoothed_pwdb >=
+		    dm_digtable.rssi_high_thresh)) {
 			if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
 				write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x20);
 			else
@@ -1945,12 +1546,12 @@ static void dm_initial_gain(struct net_device *dev)
 
 	if (dm_digtable.PreSTAConnectState == dm_digtable.CurSTAConnectState) {
 		if (dm_digtable.CurSTAConnectState == DIG_STA_CONNECT) {
-			if ((dm_digtable.rssi_val+10-dm_digtable.backoff_val) > dm_digtable.rx_gain_range_max)
-				dm_digtable.cur_ig_value = dm_digtable.rx_gain_range_max;
-			else if ((dm_digtable.rssi_val+10-dm_digtable.backoff_val) < dm_digtable.rx_gain_range_min)
-				dm_digtable.cur_ig_value = dm_digtable.rx_gain_range_min;
-			else
-				dm_digtable.cur_ig_value = dm_digtable.rssi_val+10-dm_digtable.backoff_val;
+			long gain_range = dm_digtable.rssi_val + 10 -
+					  dm_digtable.backoff_val;
+			gain_range = clamp_t(long, gain_range,
+					     dm_digtable.rx_gain_range_min,
+					     dm_digtable.rx_gain_range_max);
+			dm_digtable.cur_ig_value = gain_range;
 		} else {
 			if (dm_digtable.cur_ig_value == 0)
 				dm_digtable.cur_ig_value = priv->DefaultInitialGain[0];
@@ -1996,15 +1597,23 @@ static void dm_pd_th(struct net_device *dev)
 
 	if (dm_digtable.PreSTAConnectState == dm_digtable.CurSTAConnectState) {
 		if (dm_digtable.CurSTAConnectState == DIG_STA_CONNECT) {
-			if (dm_digtable.rssi_val >= dm_digtable.rssi_high_power_highthresh)
-				dm_digtable.curpd_thstate = DIG_PD_AT_HIGH_POWER;
-			else if (dm_digtable.rssi_val <= dm_digtable.rssi_low_thresh)
-				dm_digtable.curpd_thstate = DIG_PD_AT_LOW_POWER;
-			else if ((dm_digtable.rssi_val >= dm_digtable.rssi_high_thresh) &&
-					(dm_digtable.rssi_val < dm_digtable.rssi_high_power_lowthresh))
-				dm_digtable.curpd_thstate = DIG_PD_AT_NORMAL_POWER;
+			if (dm_digtable.rssi_val >=
+			    dm_digtable.rssi_high_power_highthresh)
+				dm_digtable.curpd_thstate =
+							DIG_PD_AT_HIGH_POWER;
+			else if (dm_digtable.rssi_val <=
+				 dm_digtable.rssi_low_thresh)
+				dm_digtable.curpd_thstate =
+							DIG_PD_AT_LOW_POWER;
+			else if ((dm_digtable.rssi_val >=
+				  dm_digtable.rssi_high_thresh) &&
+				 (dm_digtable.rssi_val <
+				  dm_digtable.rssi_high_power_lowthresh))
+				dm_digtable.curpd_thstate =
+							DIG_PD_AT_NORMAL_POWER;
 			else
-				dm_digtable.curpd_thstate = dm_digtable.prepd_thstate;
+				dm_digtable.curpd_thstate =
+						dm_digtable.prepd_thstate;
 		} else {
 			dm_digtable.curpd_thstate = DIG_PD_AT_LOW_POWER;
 		}
@@ -2024,7 +1633,8 @@ static void dm_pd_th(struct net_device *dev)
 				write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x00);
 			else
 				write_nic_byte(dev, rOFDM0_RxDetector1, 0x42);
-		} else if (dm_digtable.curpd_thstate == DIG_PD_AT_NORMAL_POWER) {
+		} else if (dm_digtable.curpd_thstate ==
+			   DIG_PD_AT_NORMAL_POWER) {
 			if (priv->CurrentChannelBW != HT_CHANNEL_WIDTH_20)
 				write_nic_byte(dev, (rOFDM0_XATxAFE+3), 0x20);
 			else
@@ -2143,10 +1753,12 @@ static void dm_check_edca_turbo(struct net_device *dev)
 				if (!priv->bis_cur_rdlstate ||
 				    !priv->bcurrent_turbo_EDCA) {
 					if (priv->rtllib->mode == WIRELESS_MODE_G)
-						write_nic_dword(dev, EDCAPARA_BE,
+						write_nic_dword(dev,
+								EDCAPARA_BE,
 							 edca_setting_DL_GMode[pHTInfo->IOTPeer]);
 					else
-						write_nic_dword(dev, EDCAPARA_BE,
+						write_nic_dword(dev,
+								EDCAPARA_BE,
 							 edca_setting_DL[pHTInfo->IOTPeer]);
 					priv->bis_cur_rdlstate = true;
 				}
@@ -2154,12 +1766,15 @@ static void dm_check_edca_turbo(struct net_device *dev)
 			priv->bcurrent_turbo_EDCA = true;
 		} else {
 			if (curRxOkCnt > 4*curTxOkCnt) {
-				if (!priv->bis_cur_rdlstate || !priv->bcurrent_turbo_EDCA) {
+				if (!priv->bis_cur_rdlstate ||
+				    !priv->bcurrent_turbo_EDCA) {
 					if (priv->rtllib->mode == WIRELESS_MODE_G)
-						write_nic_dword(dev, EDCAPARA_BE,
+						write_nic_dword(dev,
+								EDCAPARA_BE,
 							 edca_setting_DL_GMode[pHTInfo->IOTPeer]);
 					else
-						write_nic_dword(dev, EDCAPARA_BE,
+						write_nic_dword(dev,
+								EDCAPARA_BE,
 							 edca_setting_DL[pHTInfo->IOTPeer]);
 					priv->bis_cur_rdlstate = true;
 				}
@@ -2179,7 +1794,8 @@ static void dm_check_edca_turbo(struct net_device *dev)
 		 if (priv->bcurrent_turbo_EDCA) {
 			u8 tmp = AC0_BE;
 
-			priv->rtllib->SetHwRegHandler(dev, HW_VAR_AC_PARAM, (u8 *)(&tmp));
+			priv->rtllib->SetHwRegHandler(dev, HW_VAR_AC_PARAM,
+						      (u8 *)(&tmp));
 			priv->bcurrent_turbo_EDCA = false;
 		}
 	}
@@ -2196,7 +1812,6 @@ static void dm_init_ctstoself(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv((struct net_device *)dev);
 
 	priv->rtllib->bCTSToSelfEnable = true;
-	priv->rtllib->CTSToSelfTH = CTSToSelfTHVal;
 }
 
 static void dm_ctstoself(struct net_device *dev)
@@ -2249,7 +1864,8 @@ void dm_CheckRfCtrlGPIO(void *data)
 	bool bActuallySet = false;
 	char *argv[3];
 	static char *RadioPowerPath = "/etc/acpi/events/RadioPower.sh";
-	static char *envp[] = {"HOME=/", "TERM=linux", "PATH=/usr/bin:/bin", NULL};
+	static char *envp[] = {"HOME=/", "TERM=linux", "PATH=/usr/bin:/bin",
+			       NULL};
 
 	bActuallySet = false;
 
@@ -2280,7 +1896,8 @@ void dm_CheckRfCtrlGPIO(void *data)
 	if (bActuallySet) {
 		mdelay(1000);
 		priv->bHwRfOffAction = 1;
-		MgntActSet_RF_State(dev, eRfPowerStateToSet, RF_CHANGE_BY_HW, true);
+		MgntActSet_RF_State(dev, eRfPowerStateToSet, RF_CHANGE_BY_HW,
+				    true);
 		if (priv->bHwRadioOff)
 			argv[1] = "RFOFF";
 		else
@@ -2326,7 +1943,6 @@ static void dm_init_rxpath_selection(struct net_device *dev)
 		DM_RxPathSelTable.cck_method = CCK_Rx_Version_2;
 	else
 		DM_RxPathSelTable.cck_method = CCK_Rx_Version_1;
-	DM_RxPathSelTable.DbgMode = DM_DBG_OFF;
 	DM_RxPathSelTable.disabledRF = 0;
 	for (i = 0; i < 4; i++) {
 		DM_RxPathSelTable.rf_rssi[i] = 50;
@@ -2369,8 +1985,7 @@ static void dm_rxpath_sel_byrssi(struct net_device *dev)
 		DM_RxPathSelTable.cck_method = CCK_Rx_Version_2;
 
 	for (i = 0; i < RF90_PATH_MAX; i++) {
-		if (!DM_RxPathSelTable.DbgMode)
-			DM_RxPathSelTable.rf_rssi[i] = priv->stats.rx_rssi_percentage[i];
+		DM_RxPathSelTable.rf_rssi[i] = priv->stats.rx_rssi_percentage[i];
 
 		if (priv->brfpath_rxenable[i]) {
 			rf_num++;
@@ -2564,7 +2179,6 @@ static void dm_init_fsync(struct net_device *dev)
 	priv->rtllib->fsync_state = Default_Fsync;
 	priv->framesyncMonitor = 1;
 
-	init_timer(&priv->fsync_timer);
 	setup_timer(&priv->fsync_timer, dm_fsync_timer_callback,
 		   (unsigned long) dev);
 }
@@ -2884,28 +2498,6 @@ void dm_check_fsync(struct net_device *dev)
 	}
 }
 
-void dm_shadow_init(struct net_device *dev)
-{
-	u8	page;
-	u16	offset;
-
-	for (page = 0; page < 5; page++)
-		for (offset = 0; offset < 256; offset++)
-			dm_shadow[page][offset] = read_nic_byte(dev,
-						  offset+page * 256);
-
-	for (page = 8; page < 11; page++)
-		for (offset = 0; offset < 256; offset++)
-			dm_shadow[page][offset] = read_nic_byte(dev,
-						  offset+page * 256);
-
-	for (page = 12; page < 15; page++)
-		for (offset = 0; offset < 256; offset++)
-			dm_shadow[page][offset] = read_nic_byte(dev,
-						  offset+page*256);
-
-}
-
 /*---------------------------Define function prototype------------------------*/
 static void dm_init_dynamic_txpower(struct net_device *dev)
 {
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
index 3f02e11cfc57..b037451c3ada 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
@@ -87,17 +87,11 @@
 struct dig_t {
 	u8		dig_enable_flag;
 	u8		dig_algorithm;
-	u8		Dig_TwoPort_Algorithm;
-	u8		Dig_Ext_Port_Stage;
-	u8		dbg_mode;
 	u8		dig_algorithm_switch;
 
 	long		rssi_low_thresh;
 	long		rssi_high_thresh;
 
-	u32		FALowThresh;
-	u32		FAHighThresh;
-
 	long		rssi_high_power_lowthresh;
 	long		rssi_high_power_highthresh;
 
@@ -105,8 +99,6 @@ struct dig_t {
 	u8		dig_highpwr_state;
 	u8		CurSTAConnectState;
 	u8		PreSTAConnectState;
-	u8		CurAPConnectState;
-	u8		PreAPConnectState;
 
 	u8		curpd_thstate;
 	u8		prepd_thstate;
@@ -116,13 +108,9 @@ struct dig_t {
 	u32		pre_ig_value;
 	u32		cur_ig_value;
 
-	u8		Backoff_Enable_Flag;
 	u8		backoff_val;
-	char		BackoffVal_range_max;
-	char		BackoffVal_range_min;
 	u8		rx_gain_range_max;
 	u8		rx_gain_range_min;
-	bool		initialgain_lowerbound_state;
 
 	long		rssi_val;
 };
@@ -141,23 +129,6 @@ enum dm_ratr_sta {
 	DM_RATR_STA_MAX
 };
 
-enum dm_dig_op_sta {
-	DIG_TYPE_THRESH_HIGH	= 0,
-	DIG_TYPE_THRESH_LOW	= 1,
-	DIG_TYPE_THRESH_HIGHPWR_HIGH	= 2,
-	DIG_TYPE_THRESH_HIGHPWR_LOW	= 3,
-	DIG_TYPE_DBG_MODE				= 4,
-	DIG_TYPE_RSSI						= 5,
-	DIG_TYPE_ALGORITHM				= 6,
-	DIG_TYPE_BACKOFF					= 7,
-	DIG_TYPE_PWDB_FACTOR			= 8,
-	DIG_TYPE_RX_GAIN_MIN				= 9,
-	DIG_TYPE_RX_GAIN_MAX				= 10,
-	DIG_TYPE_ENABLE			= 20,
-	DIG_TYPE_DISABLE		= 30,
-	DIG_OP_TYPE_MAX
-};
-
 enum dm_dig_alg {
 	DIG_ALGO_BY_FALSE_ALARM = 0,
 	DIG_ALGO_BY_RSSI	= 1,
@@ -166,26 +137,6 @@ enum dm_dig_alg {
 	DIG_ALGO_MAX
 };
 
-enum dm_dig_two_port_alg {
-	DIG_TWO_PORT_ALGO_RSSI = 0,
-	DIG_TWO_PORT_ALGO_FALSE_ALARM = 1,
-};
-
-
-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_dbg {
-	DIG_DBG_OFF = 0,
-	DIG_DBG_ON = 1,
-	DIG_DBG_MAX
-};
-
 enum dm_dig_connect {
 	DIG_STA_DISCONNECT = 0,
 	DIG_STA_CONNECT = 1,
@@ -211,7 +162,6 @@ enum dm_dig_cs_ratio {
 
 struct drx_path_sel {
 	u8		Enable;
-	u8		DbgMode;
 	u8		cck_method;
 	u8		cck_Rx_path;
 
@@ -232,12 +182,6 @@ enum dm_cck_rx_path_method {
 };
 
 
-enum dm_dbg {
-	DM_DBG_OFF = 0,
-	DM_DBG_ON = 1,
-	DM_DBG_MAX
-};
-
 struct dcmd_txcmd {
 	u32	Op;
 	u32	Length;
@@ -248,15 +192,21 @@ struct dcmd_txcmd {
 
 /*------------------------Export global variable----------------------------*/
 extern	struct dig_t dm_digtable;
-extern	u8		dm_shadow[16][256];
 extern struct drx_path_sel DM_RxPathSelTable;
 
-extern	u8			test_flag;
+/* Pre-calculated gain tables */
+extern const u32 dm_tx_bb_gain[TxBBGainTableLength];
+extern const u8 dm_cck_tx_bb_gain[CCKTxBBGainTableLength][8];
+extern const u8 dm_cck_tx_bb_gain_ch14[CCKTxBBGainTableLength][8];
+/* Maps table index to iq amplify gain (dB, 12 to -24dB) */
+#define dm_tx_bb_gain_idx_to_amplify(idx) (-idx + 12)
+
 /*------------------------Export global variable----------------------------*/
 
 
 /*--------------------------Exported Function prototype---------------------*/
 /*--------------------------Exported Function prototype---------------------*/
+
 extern  void    init_hal_dm(struct net_device *dev);
 extern  void deinit_hal_dm(struct net_device *dev);
 
@@ -270,47 +220,10 @@ extern  void dm_cck_txpower_adjust(struct net_device *dev, bool binch14);
 
 extern  void    dm_restore_dynamic_mechanism_state(struct net_device *dev);
 extern  void    dm_backup_dynamic_mechanism_state(struct net_device *dev);
-extern  void    dm_change_dynamic_initgain_thresh(struct net_device *dev,
-					u32	     dm_type,
-					u32	     dm_value);
-extern  void    DM_ChangeFsyncSetting(struct net_device *dev,
-					s32	     DM_Type,
-					s32	     DM_Value);
-extern  void dm_force_tx_fw_info(struct net_device *dev,
-					u32	     force_type,
-					u32	     force_value);
 extern  void    dm_init_edca_turbo(struct net_device *dev);
-extern  void    dm_rf_operation_test_callback(unsigned long data);
 extern  void    dm_rf_pathcheck_workitemcallback(void *data);
 extern  void dm_fsync_timer_callback(unsigned long data);
 extern  void dm_check_fsync(struct net_device *dev);
-extern  void    dm_shadow_init(struct net_device *dev);
 extern  void dm_initialize_txpower_tracking(struct net_device *dev);
 extern  void    dm_CheckRfCtrlGPIO(void *data);
-extern void dm_InitRateAdaptiveMask(struct net_device *dev);
-extern	void	init_hal_dm(struct net_device *dev);
-extern	void deinit_hal_dm(struct net_device *dev);
-extern void hal_dm_watchdog(struct net_device *dev);
-extern	void	init_rate_adaptive(struct net_device *dev);
-extern	void	dm_txpower_trackingcallback(void *data);
-extern	void	dm_restore_dynamic_mechanism_state(struct net_device *dev);
-extern	void	dm_backup_dynamic_mechanism_state(struct net_device *dev);
-extern	void	dm_change_dynamic_initgain_thresh(struct net_device *dev,
-					u32	dm_type,
-					u32	dm_value);
-extern	void	DM_ChangeFsyncSetting(struct net_device *dev,
-					s32		DM_Type,
-					s32		DM_Value);
-extern	void dm_force_tx_fw_info(struct net_device *dev,
-					u32		force_type,
-					u32		force_value);
-extern	void	dm_init_edca_turbo(struct net_device *dev);
-extern	void	dm_rf_operation_test_callback(unsigned long data);
-extern	void	dm_rf_pathcheck_workitemcallback(void *data);
-extern	void dm_fsync_timer_callback(unsigned long data);
-extern	void dm_check_fsync(struct net_device *dev);
-extern	void	dm_shadow_init(struct net_device *dev);
-extern	void dm_initialize_txpower_tracking(struct net_device *dev);
-extern  void    dm_CheckRfCtrlGPIO(void *data);
-
 #endif	/*__R8192UDM_H__ */
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
index 51f53be2de17..6bbd1c626e24 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.c
@@ -34,10 +34,8 @@ static void rtl8192_parse_pci_configuration(struct pci_dev *pdev,
 	u16 LinkCtrlReg;
 
 	pcie_capability_read_word(priv->pdev, PCI_EXP_LNKCTL, &LinkCtrlReg);
-	priv->NdisAdapter.LinkCtrlReg = (u8)LinkCtrlReg;
 
-	RT_TRACE(COMP_INIT, "Link Control Register =%x\n",
-		 priv->NdisAdapter.LinkCtrlReg);
+	RT_TRACE(COMP_INIT, "Link Control Register =%x\n", LinkCtrlReg);
 
 	pci_read_config_byte(pdev, 0x98, &tmp);
 	tmp |= BIT4;
@@ -62,7 +60,7 @@ bool rtl8192_pci_findadapter(struct pci_dev *pdev, struct net_device *dev)
 
 	priv->card_8192 = priv->ops->nic_type;
 
-	if (DeviceID == 0x8172) {
+	if (DeviceID == 0x8192) {
 		switch (RevisionID) {
 		case HAL_HW_PCI_REVISION_ID_8192PCIE:
 			dev_info(&pdev->dev,
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h
index 4b94653c50d9..e8d5527a5f04 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pci.h
@@ -28,23 +28,6 @@
 #include <linux/types.h>
 #include <linux/pci.h>
 
-struct mp_adapter {
-	u8		LinkCtrlReg;
-
-	u8		BusNumber;
-	u8		DevNumber;
-	u8		FuncNumber;
-
-	u8		PciBridgeBusNum;
-	u8		PciBridgeDevNum;
-	u8		PciBridgeFuncNum;
-	u8		PciBridgeVendor;
-	u16		PciBridgeVendorId;
-	u16		PciBridgeDeviceId;
-	u8		PciBridgePCIeHdrOffset;
-	u8		PciBridgeLinkCtrlReg;
-};
-
 struct net_device;
 bool rtl8192_pci_findadapter(struct pci_dev *pdev, struct net_device *dev);
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
index ca6ecfc8299e..e4908672421c 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
@@ -57,10 +57,8 @@ int rtl8192E_suspend(struct pci_dev *pdev, pm_message_t state)
 		write_nic_byte(dev, MacBlkCtrl, 0xa);
 	}
 out_pci_suspend:
-	netdev_info(dev, "r8192E support WOL call??????????????????????\n");
-	if (priv->rtllib->bSupportRemoteWakeUp)
-		RT_TRACE(COMP_POWER,
-			 "r8192E support WOL call!!!!!!!!!!!!!!!!!!.\n");
+	netdev_info(dev, "WOL is %s\n", priv->rtllib->bSupportRemoteWakeUp ?
+			    "Supported" : "Not supported");
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
 	pci_enable_wake(pdev, pci_choose_state(pdev, state),
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
index 0bbffec0c2ae..404cb83153d9 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
@@ -28,6 +28,7 @@
 #include "r8192E_phyreg.h"
 #include "r8190P_rtl8256.h" /* RTL8225 Radio frontend */
 #include "r8192E_cmdpkt.h"
+#include <linux/jiffies.h>
 
 static void rtl8192_hw_sleep_down(struct net_device *dev)
 {
@@ -93,19 +94,21 @@ void rtl8192_hw_to_sleep(struct net_device *dev, u64 time)
 
 	u32 tmp;
 	unsigned long flags;
+	unsigned long timeout;
 
 	spin_lock_irqsave(&priv->ps_lock, flags);
 
 	time -= msecs_to_jiffies(8 + 16 + 7);
 
-	if ((time - jiffies) <= msecs_to_jiffies(MIN_SLEEP_TIME)) {
+	timeout = jiffies + msecs_to_jiffies(MIN_SLEEP_TIME);
+	if (time_before((unsigned long)time, timeout)) {
 		spin_unlock_irqrestore(&priv->ps_lock, flags);
 		netdev_info(dev, "too short to sleep::%lld < %ld\n",
 			    time - jiffies, msecs_to_jiffies(MIN_SLEEP_TIME));
 		return;
 	}
-
-	if ((time - jiffies) > msecs_to_jiffies(MAX_SLEEP_TIME)) {
+	timeout = jiffies + msecs_to_jiffies(MAX_SLEEP_TIME);
+	if (time_after((unsigned long)time, timeout)) {
 		netdev_info(dev, "========>too long to sleep:%lld > %ld\n",
 			    time - jiffies, msecs_to_jiffies(MAX_SLEEP_TIME));
 		spin_unlock_irqrestore(&priv->ps_lock, flags);
@@ -199,8 +202,8 @@ void rtllib_ips_leave_wq(struct net_device *dev)
 	if (priv->rtllib->PowerSaveControl.bInactivePs) {
 		if (rtState == eRfOff) {
 			if (priv->rtllib->RfOffReason > RF_CHANGE_BY_IPS) {
-				RT_TRACE(COMP_ERR, "%s(): RF is OFF.\n",
-					 __func__);
+				netdev_warn(dev, "%s(): RF is OFF.\n",
+					    __func__);
 				return;
 			}
 			netdev_info(dev, "=========>%s(): IPSLeave\n",
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index 8d6a109e023b..f5e4961677d2 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -116,9 +116,8 @@ static int r8192_wx_set_power(struct net_device *dev,
 	struct r8192_priv *priv = rtllib_priv(dev);
 
 	if (priv->bHwRadioOff) {
-		RT_TRACE(COMP_ERR,
-			 "%s():Hw is Radio Off, we can't set Power,return\n",
-			 __func__);
+		netdev_warn(dev, "%s(): Can't set Power: Radio is Off.\n",
+			    __func__);
 		return 0;
 	}
 	down(&priv->wx_sem);
@@ -175,48 +174,6 @@ static int r8192_wx_force_reset(struct net_device *dev,
 
 }
 
-static int r8192_wx_force_mic_error(struct net_device *dev,
-		struct iw_request_info *info,
-		union iwreq_data *wrqu, char *extra)
-{
-	struct r8192_priv *priv = rtllib_priv(dev);
-	struct rtllib_device *ieee = priv->rtllib;
-
-	down(&priv->wx_sem);
-
-	RT_TRACE(COMP_DBG, "%s(): force mic error !\n", __func__);
-	ieee->force_mic_error = true;
-	up(&priv->wx_sem);
-	return 0;
-
-}
-
-#define MAX_ADHOC_PEER_NUM 64
-struct adhoc_peer_entry {
-	unsigned char MacAddr[ETH_ALEN];
-	unsigned char WirelessMode;
-	unsigned char bCurTxBW40MHz;
-};
-struct adhoc_peers_info {
-	struct adhoc_peer_entry Entry[MAX_ADHOC_PEER_NUM];
-	unsigned char num;
-};
-
-static int r8192_wx_get_adhoc_peers(struct net_device *dev,
-				    struct iw_request_info *info,
-				    union iwreq_data *wrqu, char *extra)
-{
-	return 0;
-}
-
-
-static int r8191se_wx_get_firm_version(struct net_device *dev,
-		struct iw_request_info *info,
-		struct iw_param *wrqu, char *extra)
-{
-	return 0;
-}
-
 static int r8192_wx_adapter_power_status(struct net_device *dev,
 		struct iw_request_info *info,
 		union iwreq_data *wrqu, char *extra)
@@ -247,28 +204,6 @@ static int r8192_wx_adapter_power_status(struct net_device *dev,
 	return 0;
 }
 
-static int r8192se_wx_set_radio(struct net_device *dev,
-	struct iw_request_info *info,
-	union iwreq_data *wrqu, char *extra)
-{
-	struct r8192_priv *priv = rtllib_priv(dev);
-
-	down(&priv->wx_sem);
-
-	netdev_info(dev, "%s(): set radio ! extra is %d\n", __func__, *extra);
-	if ((*extra != 0) && (*extra != 1)) {
-		RT_TRACE(COMP_ERR,
-			 "%s(): set radio an err value,must 0(radio off) or 1(radio on)\n",
-			 __func__);
-		up(&priv->wx_sem);
-		return -1;
-	}
-	priv->sw_radio_on = *extra;
-	up(&priv->wx_sem);
-	return 0;
-
-}
-
 static int r8192se_wx_set_lps_awake_interval(struct net_device *dev,
 	struct iw_request_info *info,
 	union iwreq_data *wrqu, char *extra)
@@ -342,8 +277,8 @@ static int r8192_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
 			if (rtState == eRfOff) {
 				if (priv->rtllib->RfOffReason >
 				    RF_CHANGE_BY_IPS) {
-					RT_TRACE(COMP_ERR, "%s(): RF is OFF.\n",
-						 __func__);
+					netdev_warn(dev, "%s(): RF is OFF.\n",
+						    __func__);
 					up(&priv->wx_sem);
 					return -1;
 				}
@@ -502,9 +437,8 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 			if (rtState == eRfOff) {
 				if (priv->rtllib->RfOffReason >
 				    RF_CHANGE_BY_IPS) {
-					RT_TRACE(COMP_ERR,
-						 "%s(): RF is OFF.\n",
-						 __func__);
+					netdev_warn(dev, "%s(): RF is OFF.\n",
+						    __func__);
 					up(&priv->wx_sem);
 					return -1;
 				}
@@ -613,7 +547,8 @@ static int r8192_wx_set_nick(struct net_device *dev,
 	if (wrqu->data.length > IW_ESSID_MAX_SIZE)
 		return -E2BIG;
 	down(&priv->wx_sem);
-	wrqu->data.length = min_t(size_t, wrqu->data.length, sizeof(priv->nick));
+	wrqu->data.length = min_t(size_t, wrqu->data.length,
+				  sizeof(priv->nick));
 	memset(priv->nick, 0, sizeof(priv->nick));
 	memcpy(priv->nick, extra, wrqu->data.length);
 	up(&priv->wx_sem);
@@ -987,8 +922,8 @@ static int r8192_wx_set_enc_ext(struct net_device *dev,
 
 	ret = rtllib_wx_set_encode_ext(ieee, info, wrqu, extra);
 	{
-		u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-		u8 zero[6] = {0};
+		const u8 broadcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+		const u8 zero[ETH_ALEN] = {0};
 		u32 key[4] = {0};
 		struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
 		struct iw_point *encoding = &wrqu->encoding;
@@ -1239,21 +1174,10 @@ static const struct iw_priv_args r8192_private_args[] = {
 		SIOCIWFIRSTPRIV + 0x3,
 		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "forcereset"
 	}, {
-		SIOCIWFIRSTPRIV + 0x4,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "force_mic_error"
-	}, {
-		SIOCIWFIRSTPRIV + 0x5,
-		IW_PRIV_TYPE_NONE, IW_PRIV_TYPE_INT|IW_PRIV_SIZE_FIXED|1,
-		"firm_ver"
-	}, {
 		SIOCIWFIRSTPRIV + 0x6,
 		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED|1, IW_PRIV_TYPE_NONE,
 		"set_power"
 	}, {
-		SIOCIWFIRSTPRIV + 0x9,
-		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED|1, IW_PRIV_TYPE_NONE,
-		"radio"
-	}, {
 		SIOCIWFIRSTPRIV + 0xa,
 		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED|1, IW_PRIV_TYPE_NONE,
 		"lps_interv"
@@ -1262,9 +1186,6 @@ static const struct iw_priv_args r8192_private_args[] = {
 		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED|1, IW_PRIV_TYPE_NONE,
 		"lps_force"
 	}, {
-		SIOCIWFIRSTPRIV + 0xc,
-		0, IW_PRIV_TYPE_CHAR|2047, "adhoc_peer_list"
-	}, {
 		SIOCIWFIRSTPRIV + 0x16,
 		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 3, 0, "setpromisc"
 	}, {
@@ -1279,15 +1200,15 @@ static iw_handler r8192_private_handler[] = {
 	(iw_handler)r8192_wx_set_scan_type,
 	(iw_handler)r8192_wx_set_rawtx,
 	(iw_handler)r8192_wx_force_reset,
-	(iw_handler)r8192_wx_force_mic_error,
-	(iw_handler)r8191se_wx_get_firm_version,
+	(iw_handler)NULL,
+	(iw_handler)NULL,
 	(iw_handler)r8192_wx_adapter_power_status,
 	(iw_handler)NULL,
 	(iw_handler)NULL,
-	(iw_handler)r8192se_wx_set_radio,
+	(iw_handler)NULL,
 	(iw_handler)r8192se_wx_set_lps_awake_interval,
 	(iw_handler)r8192se_wx_set_force_lps,
-	(iw_handler)r8192_wx_get_adhoc_peers,
+	(iw_handler)NULL,
 	(iw_handler)NULL,
 	(iw_handler)NULL,
 	(iw_handler)NULL,
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
index 58398517f5b3..771284019e08 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
@@ -20,11 +20,7 @@
 #ifndef R819x_WX_H
 #define R819x_WX_H
 
-struct net_device;
 struct iw_handler_def;
-struct iw_statistics;
 
 extern const struct iw_handler_def r8192_wx_handlers_def;
-u16 rtl8192_11n_user_show_rates(struct net_device *dev);
-
 #endif
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index 26258ea8de4a..60f536c295ab 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -18,6 +18,7 @@
 ******************************************************************************/
 #include <asm/byteorder.h>
 #include <asm/unaligned.h>
+#include <linux/etherdevice.h>
 #include "rtllib.h"
 #include "rtl819x_BA.h"
 
@@ -83,18 +84,16 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 	u8 *tag = NULL;
 	u16 len = ieee->tx_headroom + 9;
 
-	RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA,
-		     "========>%s(), frame(%d) sentd to: %pM, ieee->dev:%p\n",
-		     __func__, type, Dst, ieee->dev);
+	netdev_dbg(ieee->dev, "%s(): frame(%d) sentd to: %pM, ieee->dev:%p\n",
+		   __func__, type, Dst, ieee->dev);
+
 	if (pBA == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "pBA is NULL\n");
+		netdev_warn(ieee->dev, "pBA is NULL\n");
 		return NULL;
 	}
 	skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
-	if (skb == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
+	if (skb == NULL)
 		return NULL;
-	}
 
 	memset(skb->data, 0, sizeof(struct rtllib_hdr_3addr));
 
@@ -103,10 +102,10 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 	BAReq = (struct rtllib_hdr_3addr *)skb_put(skb,
 		 sizeof(struct rtllib_hdr_3addr));
 
-	memcpy(BAReq->addr1, Dst, ETH_ALEN);
-	memcpy(BAReq->addr2, ieee->dev->dev_addr, ETH_ALEN);
+	ether_addr_copy(BAReq->addr1, Dst);
+	ether_addr_copy(BAReq->addr2, ieee->dev->dev_addr);
 
-	memcpy(BAReq->addr3, ieee->current_network.bssid, ETH_ALEN);
+	ether_addr_copy(BAReq->addr3, ieee->current_network.bssid);
 	BAReq->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
 
 	tag = (u8 *)skb_put(skb, 9);
@@ -132,7 +131,10 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 		tag += 2;
 	}
 
-	RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("rtllib_ADDBA(): ", DUMP_PREFIX_NONE, skb->data,
+			     skb->len);
+#endif
 	return skb;
 }
 
@@ -147,9 +149,8 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 	u16 len = 6 + ieee->tx_headroom;
 
 	if (net_ratelimit())
-		RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA,
-			     "========>%s(), ReasonCode(%d) sentd to: %pM\n",
-			     __func__, ReasonCode, dst);
+		netdev_dbg(ieee->dev, "%s(): ReasonCode(%d) sentd to: %pM\n",
+			   __func__, ReasonCode, dst);
 
 	memset(&DelbaParamSet, 0, 2);
 
@@ -157,19 +158,17 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 	DelbaParamSet.field.TID	= pBA->BaParamSet.field.TID;
 
 	skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
-	if (skb == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
+	if (skb == NULL)
 		return NULL;
-	}
 
 	skb_reserve(skb, ieee->tx_headroom);
 
 	Delba = (struct rtllib_hdr_3addr *) skb_put(skb,
 		 sizeof(struct rtllib_hdr_3addr));
 
-	memcpy(Delba->addr1, dst, ETH_ALEN);
-	memcpy(Delba->addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(Delba->addr3, ieee->current_network.bssid, ETH_ALEN);
+	ether_addr_copy(Delba->addr1, dst);
+	ether_addr_copy(Delba->addr2, ieee->dev->dev_addr);
+	ether_addr_copy(Delba->addr3, ieee->current_network.bssid);
 	Delba->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);
 
 	tag = (u8 *)skb_put(skb, 6);
@@ -184,10 +183,10 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 	put_unaligned_le16(ReasonCode, tag);
 	tag += 2;
 
-	RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
-	if (net_ratelimit())
-		RTLLIB_DEBUG(RTLLIB_DL_TRACE | RTLLIB_DL_BA, "<=====%s()\n",
-			     __func__);
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("rtllib_DELBA(): ", DUMP_PREFIX_NONE, skb->data,
+			     skb->len);
+#endif
 	return skb;
 }
 
@@ -202,8 +201,7 @@ static void rtllib_send_ADDBAReq(struct rtllib_device *ieee, u8 *dst,
 		RT_TRACE(COMP_DBG, "====>to send ADDBAREQ!!!!!\n");
 		softmac_mgmt_xmit(skb, ieee);
 	} else {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "alloc skb error in function %s()\n", __func__);
+		netdev_dbg(ieee->dev, "Failed to generate ADDBAReq packet.\n");
 	}
 }
 
@@ -216,8 +214,7 @@ static void rtllib_send_ADDBARsp(struct rtllib_device *ieee, u8 *dst,
 	if (skb)
 		softmac_mgmt_xmit(skb, ieee);
 	else
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "alloc skb error in function %s()\n", __func__);
+		netdev_dbg(ieee->dev, "Failed to generate ADDBARsp packet.\n");
 }
 
 static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
@@ -230,8 +227,7 @@ static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
 	if (skb)
 		softmac_mgmt_xmit(skb, ieee);
 	else
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "alloc skb error in function %s()\n", __func__);
+		netdev_dbg(ieee->dev, "Failed to generate DELBA packet.\n");
 }
 
 int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
@@ -246,14 +242,16 @@ int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
 	struct rx_ts_record *pTS = NULL;
 
 	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 9) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     " Invalid skb len in BAREQ(%d / %d)\n",
-			     (int)skb->len,
-			     (int)(sizeof(struct rtllib_hdr_3addr) + 9));
+		netdev_warn(ieee->dev, "Invalid skb len in BAREQ(%d / %d)\n",
+			    (int)skb->len,
+			    (int)(sizeof(struct rtllib_hdr_3addr) + 9));
 		return -1;
 	}
 
-	RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("rtllib_rx_ADDBAReq(): ", DUMP_PREFIX_NONE,
+			     skb->data, skb->len);
+#endif
 
 	req = (struct rtllib_hdr_3addr *) skb->data;
 	tag = (u8 *)req;
@@ -269,24 +267,24 @@ int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
 	    (ieee->pHTInfo->bCurrentHTSupport == false) ||
 	    (ieee->pHTInfo->IOTAction & HT_IOT_ACT_REJECT_ADDBA_REQ)) {
 		rc = ADDBA_STATUS_REFUSED;
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "Failed to reply on ADDBA_REQ as some capability is not ready(%d, %d)\n",
-			     ieee->current_network.qos_data.active,
-			     ieee->pHTInfo->bCurrentHTSupport);
+		netdev_warn(ieee->dev,
+			    "Failed to reply on ADDBA_REQ as some capability is not ready(%d, %d)\n",
+			    ieee->current_network.qos_data.active,
+			    ieee->pHTInfo->bCurrentHTSupport);
 		goto OnADDBAReq_Fail;
 	}
 	if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst,
 	    (u8)(pBaParamSet->field.TID), RX_DIR, true)) {
 		rc = ADDBA_STATUS_REFUSED;
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't get TS in %s()\n", __func__);
+		netdev_warn(ieee->dev, "%s(): can't get TS\n", __func__);
 		goto OnADDBAReq_Fail;
 	}
 	pBA = &pTS->RxAdmittedBARecord;
 
 	if (pBaParamSet->field.BAPolicy == BA_POLICY_DELAYED) {
 		rc = ADDBA_STATUS_INVALID_PARAM;
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "BA Policy is not correct in %s()\n", __func__);
+		netdev_warn(ieee->dev, "%s(): BA Policy is not correct\n",
+			    __func__);
 		goto OnADDBAReq_Fail;
 	}
 
@@ -333,10 +331,9 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 	u16			ReasonCode;
 
 	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 9) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "Invalid skb len in BARSP(%d / %d)\n",
-			     (int)skb->len,
-			     (int)(sizeof(struct rtllib_hdr_3addr) + 9));
+		netdev_warn(ieee->dev, "Invalid skb len in BARSP(%d / %d)\n",
+			    (int)skb->len,
+			    (int)(sizeof(struct rtllib_hdr_3addr) + 9));
 		return -1;
 	}
 	rsp = (struct rtllib_hdr_3addr *)skb->data;
@@ -352,11 +349,11 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 	if (ieee->current_network.qos_data.active == 0  ||
 	    ieee->pHTInfo->bCurrentHTSupport == false ||
 	    ieee->pHTInfo->bCurrentAMPDUEnable == false) {
-		RTLLIB_DEBUG(RTLLIB_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);
+		netdev_warn(ieee->dev,
+			    "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;
 	}
@@ -364,7 +361,7 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 
 	if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst,
 		   (u8)(pBaParamSet->field.TID), TX_DIR, false)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't get TS in %s()\n", __func__);
+		netdev_warn(ieee->dev, "%s(): can't get TS\n", __func__);
 		ReasonCode = DELBA_REASON_UNKNOWN_BA;
 		goto OnADDBARsp_Reject;
 	}
@@ -375,19 +372,20 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 
 
 	if (pAdmittedBA->bValid == true) {
-		RTLLIB_DEBUG(RTLLIB_DL_BA,
-			     "OnADDBARsp(): Recv ADDBA Rsp. Drop because already admit it!\n");
+		netdev_dbg(ieee->dev, "%s(): ADDBA response already admitted\n",
+			   __func__);
 		return -1;
 	} else if ((pPendingBA->bValid == false) ||
 		   (*pDialogToken != pPendingBA->DialogToken)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "OnADDBARsp(): Recv ADDBA Rsp. BA invalid, DELBA!\n");
+		netdev_warn(ieee->dev,
+			    "%s(): ADDBA Rsp. BA invalid, DELBA!\n",
+			    __func__);
 		ReasonCode = DELBA_REASON_UNKNOWN_BA;
 		goto OnADDBARsp_Reject;
 	} else {
-		RTLLIB_DEBUG(RTLLIB_DL_BA,
-			     "OnADDBARsp(): Recv ADDBA Rsp. BA is admitted! Status code:%X\n",
-			     *pStatusCode);
+		netdev_dbg(ieee->dev,
+			   "%s(): Recv ADDBA Rsp. BA is admitted! Status code:%X\n",
+			   __func__, *pStatusCode);
 		DeActivateBAEntry(ieee, pPendingBA);
 	}
 
@@ -434,23 +432,25 @@ int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb)
 	u8 *dst = NULL;
 
 	if (skb->len < sizeof(struct rtllib_hdr_3addr) + 6) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "Invalid skb len in DELBA(%d / %d)\n",
-			     (int)skb->len,
-			     (int)(sizeof(struct rtllib_hdr_3addr) + 6));
+		netdev_warn(ieee->dev, "Invalid skb len in DELBA(%d / %d)\n",
+			    (int)skb->len,
+			    (int)(sizeof(struct rtllib_hdr_3addr) + 6));
 		return -1;
 	}
 
 	if (ieee->current_network.qos_data.active == 0  ||
 		ieee->pHTInfo->bCurrentHTSupport == false) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "received DELBA while QOS or HT is not supported(%d, %d)\n",
-			     ieee->current_network. qos_data.active,
-			     ieee->pHTInfo->bCurrentHTSupport);
+		netdev_warn(ieee->dev,
+			    "received DELBA while QOS or HT is not supported(%d, %d)\n",
+			    ieee->current_network. qos_data.active,
+			    ieee->pHTInfo->bCurrentHTSupport);
 		return -1;
 	}
 
-	RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA|RTLLIB_DL_BA, skb->data, skb->len);
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("rtllib_rx_DELBA(): ", DUMP_PREFIX_NONE, skb->data,
+			     skb->len);
+#endif
 	delba = (struct rtllib_hdr_3addr *)skb->data;
 	dst = (u8 *)(&delba->addr2[0]);
 	delba += sizeof(struct rtllib_hdr_3addr);
@@ -462,10 +462,10 @@ int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb)
 
 		if (!GetTs(ieee, (struct ts_common_info **)&pRxTs, dst,
 		    (u8)pDelBaParamSet->field.TID, RX_DIR, false)) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR,
-				     "can't get TS for RXTS in %s().dst: %pM TID:%d\n",
-				     __func__, dst,
-				     (u8)pDelBaParamSet->field.TID);
+			netdev_warn(ieee->dev,
+				    "%s(): can't get TS for RXTS. dst:%pM TID:%d\n",
+				    __func__, dst,
+				    (u8)pDelBaParamSet->field.TID);
 			return -1;
 		}
 
@@ -475,9 +475,8 @@ int rtllib_rx_DELBA(struct rtllib_device *ieee, struct sk_buff *skb)
 
 		if (!GetTs(ieee, (struct ts_common_info **)&pTxTs, dst,
 			   (u8)pDelBaParamSet->field.TID, TX_DIR, false)) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR,
-				     "can't get TS for TXTS in %s()\n",
-				     __func__);
+			netdev_warn(ieee->dev, "%s(): can't get TS for TXTS\n",
+				    __func__);
 			return -1;
 		}
 
diff --git a/drivers/staging/rtl8192e/rtl819x_HT.h b/drivers/staging/rtl8192e/rtl819x_HT.h
index f7076d7ddc53..0c263d9f7246 100644
--- a/drivers/staging/rtl8192e/rtl819x_HT.h
+++ b/drivers/staging/rtl8192e/rtl819x_HT.h
@@ -19,45 +19,12 @@
 #ifndef _RTL819XU_HTTYPE_H_
 #define _RTL819XU_HTTYPE_H_
 
-
-#define HT_OPMODE_NO_PROTECT		0
-#define HT_OPMODE_OPTIONAL		1
-#define HT_OPMODE_40MHZ_PROTECT	2
-#define HT_OPMODE_MIXED			3
-
 #define MIMO_PS_STATIC				0
 #define MIMO_PS_DYNAMIC			1
 #define MIMO_PS_NOLIMIT			3
 
-
-
 #define sHTCLng	4
 
-
-#define HT_SUPPORTED_MCS_1SS_BITMAP			0x000000ff
-#define HT_SUPPORTED_MCS_2SS_BITMAP			0x0000ff00
-#define HT_SUPPORTED_MCS_1SS_2SS_BITMAP			\
-	(HT_MCS_1SS_BITMAP | HT_MCS_1SS_2SS_BITMAP)
-
-enum ht_mcs_rate {
-	HT_MCS0   = 0x00000001,
-	HT_MCS1   = 0x00000002,
-	HT_MCS2   = 0x00000004,
-	HT_MCS3   = 0x00000008,
-	HT_MCS4   = 0x00000010,
-	HT_MCS5   = 0x00000020,
-	HT_MCS6   = 0x00000040,
-	HT_MCS7   = 0x00000080,
-	HT_MCS8   = 0x00000100,
-	HT_MCS9   = 0x00000200,
-	HT_MCS10 = 0x00000400,
-	HT_MCS11 = 0x00000800,
-	HT_MCS12 = 0x00001000,
-	HT_MCS13 = 0x00002000,
-	HT_MCS14 = 0x00004000,
-	HT_MCS15 = 0x00008000,
-};
-
 enum ht_channel_width {
 	HT_CHANNEL_WIDTH_20 = 0,
 	HT_CHANNEL_WIDTH_20_40 = 1,
@@ -70,34 +37,6 @@ enum ht_extchnl_offset {
 	HT_EXTCHNL_OFFSET_LOWER = 3,
 };
 
-enum chnl_op {
-	CHNLOP_NONE = 0,
-	CHNLOP_SCAN = 1,
-	CHNLOP_SWBW = 2,
-	CHNLOP_SWCHNL = 3,
-};
-
-enum ht_action {
-	ACT_RECOMMAND_WIDTH		= 0,
-	ACT_MIMO_PWR_SAVE		= 1,
-	ACT_PSMP					= 2,
-	ACT_SET_PCO_PHASE		= 3,
-	ACT_MIMO_CHL_MEASURE	= 4,
-	ACT_RECIPROCITY_CORRECT	= 5,
-	ACT_MIMO_CSI_MATRICS		= 6,
-	ACT_MIMO_NOCOMPR_STEER	= 7,
-	ACT_MIMO_COMPR_STEER		= 8,
-	ACT_ANTENNA_SELECT		= 9,
-};
-
-
-enum ht_bw40_sc {
-	SC_MODE_DUPLICATE = 0,
-	SC_MODE_LOWER = 1,
-	SC_MODE_UPPER = 2,
-	SC_MODE_FULL40MHZ = 3,
-};
-
 struct ht_capab_ele {
 
 	u8	AdvCoding:1;
@@ -158,12 +97,6 @@ struct ht_info_ele {
 	u8	BasicMSC[16];
 } __packed;
 
-struct mimops_ctrl {
-	u8	MimoPsEnable:1;
-	u8	MimoPsMode:1;
-	u8	Reserved:6;
-};
-
 enum ht_spec_ver {
 	HT_SPEC_VER_IEEE = 0,
 	HT_SPEC_VER_EWC = 1,
@@ -233,7 +166,6 @@ struct rt_hi_throughput {
 	u8				PeerBandwidth;
 
 	u8				bSwBwInProgress;
-	enum chnl_op ChnlOp;
 	u8				SwBwStep;
 
 	u8				bRegRT2RTAggregation;
@@ -260,43 +192,6 @@ struct rt_hi_throughput {
 	u8				bAcceptAddbaReq;
 } __packed;
 
-
-
-struct rt_htinfo_sta_entry {
-	u8			bEnableHT;
-
-	u8			bSupportCck;
-
-	u16			AMSDU_MaxSize;
-
-	u8			AMPDU_Factor;
-	u8			MPDU_Density;
-
-	u8			HTHighestOperaRate;
-
-	u8			bBw40MHz;
-
-	u8			bCurTxBW40MHz;
-
-	u8			bCurShortGI20MHz;
-
-	u8			bCurShortGI40MHz;
-
-	u8			MimoPs;
-
-	u8			McsRateSet[16];
-
-	u8                      bCurRxReorderEnable;
-
-	u16                     nAMSDU_MaxSize;
-
-};
-
-
-
-
-
-
 struct bss_ht {
 
 	u8				bdSupportHT;
@@ -315,31 +210,6 @@ struct bss_ht {
 	u8					bdHT1R;
 };
 
-struct mimo_rssi {
-	u32	EnableAntenna;
-	u32	AntennaA;
-	u32	AntennaB;
-	u32	AntennaC;
-	u32	AntennaD;
-	u32	Average;
-};
-
-struct mimo_evm {
-	u32	EVM1;
-	u32    EVM2;
-};
-
-struct false_alarm_stats {
-	u32	Cnt_Parity_Fail;
-	u32	Cnt_Rate_Illegal;
-	u32	Cnt_Crc8_fail;
-	u32	Cnt_Mcs_fail;
-	u32	Cnt_Ofdm_fail;
-	u32	Cnt_Cck_fail;
-	u32	Cnt_all;
-};
-
-
 extern u8 MCS_FILTER_ALL[16];
 extern u8 MCS_FILTER_1SS[16];
 
@@ -347,8 +217,6 @@ extern u8 MCS_FILTER_1SS[16];
 #define	RATE_ADPT_2SS_MASK		0xF0
 #define	RATE_ADPT_MCS32_MASK		0x01
 
-#define		IS_11N_MCS_RATE(rate)		(rate&0x80)
-
 enum ht_aggre_size {
 	HT_AGG_SIZE_8K = 0,
 	HT_AGG_SIZE_16K = 1,
@@ -371,10 +239,6 @@ enum ht_iot_peer {
 	HT_IOT_PEER_MAX = 11,
 };
 
-enum ht_iot_peer_subtype {
-	HT_IOT_PEER_ATHEROS_DIR635 = 0,
-};
-
 enum ht_iot_action {
 	HT_IOT_ACT_TX_USE_AMSDU_4K = 0x00000001,
 	HT_IOT_ACT_TX_USE_AMSDU_8K = 0x00000002,
diff --git a/drivers/staging/rtl8192e/rtl819x_HTProc.c b/drivers/staging/rtl8192e/rtl819x_HTProc.c
index 7f103114d5d2..b5c3647b0f80 100644
--- a/drivers/staging/rtl8192e/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_HTProc.c
@@ -217,8 +217,7 @@ static void HTIOTPeerDetermine(struct rtllib_device *ieee)
 	else
 		pHTInfo->IOTPeer = HT_IOT_PEER_UNKNOWN;
 
-	RTLLIB_DEBUG(RTLLIB_DL_IOT, "Joseph debug!! IOTPEER: %x\n",
-		     pHTInfo->IOTPeer);
+	netdev_dbg(ieee->dev, "IOTPEER: %x\n", pHTInfo->IOTPeer);
 }
 
 static u8 HTIOTActIsDisableMCS14(struct rtllib_device *ieee, u8 *PeerMacAddr)
@@ -237,7 +236,8 @@ static bool HTIOTActIsDisableMCSTwoSpatialStream(struct rtllib_device *ieee)
 	return false;
 }
 
-static u8 HTIOTActIsDisableEDCATurbo(struct rtllib_device *ieee, u8 *PeerMacAddr)
+static u8 HTIOTActIsDisableEDCATurbo(struct rtllib_device *ieee,
+				     u8 *PeerMacAddr)
 {
 	return false;
 }
@@ -291,8 +291,8 @@ void HTConstructCapabilityElement(struct rtllib_device *ieee, u8 *posHTCap,
 	struct ht_capab_ele *pCapELE = NULL;
 
 	if ((posHTCap == NULL) || (pHT == NULL)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "posHTCap or pHTInfo can't be null in HTConstructCapabilityElement()\n");
+		netdev_warn(ieee->dev,
+			    "%s(): posHTCap and pHTInfo are null\n", __func__);
 		return;
 	}
 	memset(posHTCap, 0, *len);
@@ -328,9 +328,9 @@ void HTConstructCapabilityElement(struct rtllib_device *ieee, u8 *posHTCap,
 	pCapELE->LSigTxopProtect = 0;
 
 
-	RTLLIB_DEBUG(RTLLIB_DL_HT,
-		     "TX HT cap/info ele BW=%d MaxAMSDUSize:%d DssCCk:%d\n",
-		     pCapELE->ChlWidth, pCapELE->MaxAMSDUSize, pCapELE->DssCCk);
+	netdev_dbg(ieee->dev,
+		   "TX HT cap/info ele BW=%d MaxAMSDUSize:%d DssCCk:%d\n",
+		   pCapELE->ChlWidth, pCapELE->MaxAMSDUSize, pCapELE->DssCCk);
 
 	if (IsEncrypt) {
 		pCapELE->MPDUDensity	= 7;
@@ -373,8 +373,9 @@ void HTConstructInfoElement(struct rtllib_device *ieee, u8 *posHTInfo,
 	struct ht_info_ele *pHTInfoEle = (struct ht_info_ele *)posHTInfo;
 
 	if ((posHTInfo == NULL) || (pHTInfoEle == NULL)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "posHTInfo or pHTInfoEle can't be null in HTConstructInfoElement()\n");
+		netdev_warn(ieee->dev,
+			    "%s(): posHTInfo and pHTInfoEle are null\n",
+			    __func__);
 		return;
 	}
 
@@ -413,8 +414,7 @@ void HTConstructRT2RTAggElement(struct rtllib_device *ieee, u8 *posRT2RTAgg,
 				u8 *len)
 {
 	if (posRT2RTAgg == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "posRT2RTAgg can't be null in HTConstructRT2RTAggElement()\n");
+		netdev_warn(ieee->dev, "%s(): posRT2RTAgg is null\n", __func__);
 		return;
 	}
 	memset(posRT2RTAgg, 0, *len);
@@ -437,8 +437,7 @@ static u8 HT_PickMCSRate(struct rtllib_device *ieee, u8 *pOperateMCS)
 	u8 i;
 
 	if (pOperateMCS == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "pOperateMCS can't be null in HT_PickMCSRate()\n");
+		netdev_warn(ieee->dev, "%s(): pOperateMCS is null\n", __func__);
 		return false;
 	}
 
@@ -472,8 +471,9 @@ u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
 	u8		availableMcsRate[16];
 
 	if (pMCSRateSet == NULL || pMCSFilter == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "pMCSRateSet or pMCSFilter can't be null in HTGetHighestMCSRate()\n");
+		netdev_warn(ieee->dev,
+			    "%s(): pMCSRateSet and pMCSFilter are null\n",
+			    __func__);
 		return false;
 	}
 	for (i = 0; i < 16; i++)
@@ -538,11 +538,10 @@ void HTOnAssocRsp(struct rtllib_device *ieee)
 	static u8 EWC11NHTInfo[] = {0x00, 0x90, 0x4c, 0x34};
 
 	if (pHTInfo->bCurrentHTSupport == false) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "<=== HTOnAssocRsp(): HT_DISABLE\n");
+		netdev_warn(ieee->dev, "%s(): HT_DISABLE\n", __func__);
 		return;
 	}
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "===> HTOnAssocRsp_wq(): HT_ENABLE\n");
+	netdev_dbg(ieee->dev, "%s(): HT_ENABLE\n", __func__);
 
 	if (!memcmp(pHTInfo->PeerHTCapBuf, EWC11NHTCap, sizeof(EWC11NHTCap)))
 		pPeerHTCap = (struct ht_capab_ele *)(&pHTInfo->PeerHTCapBuf[4]);
@@ -555,8 +554,11 @@ void HTOnAssocRsp(struct rtllib_device *ieee)
 	else
 		pPeerHTInfo = (struct ht_info_ele *)(pHTInfo->PeerHTInfoBuf);
 
-	RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA | RTLLIB_DL_HT, pPeerHTCap,
-			  sizeof(struct ht_capab_ele));
+
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("HTOnAssocRsp(): ", DUMP_PREFIX_NONE,
+			     pPeerHTCap, sizeof(struct ht_capab_ele));
+#endif
 	HTSetConnectBwMode(ieee, (enum ht_channel_width)(pPeerHTCap->ChlWidth),
 			  (enum ht_extchnl_offset)(pPeerHTInfo->ExtChlOffset));
 	pHTInfo->bCurTxBW40MHz = ((pPeerHTInfo->RecommemdedTxWidth == 1) ?
@@ -647,7 +649,7 @@ void HTInitializeHTInfo(struct rtllib_device *ieee)
 {
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
 
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "===========>%s()\n", __func__);
+	netdev_vdbg(ieee->dev, "%s()\n", __func__);
 	pHTInfo->bCurrentHTSupport = false;
 
 	pHTInfo->bCurBW40MHz = false;
@@ -674,7 +676,6 @@ void HTInitializeHTInfo(struct rtllib_device *ieee)
 		sizeof(pHTInfo->PeerHTInfoBuf));
 
 	pHTInfo->bSwBwInProgress = false;
-	pHTInfo->ChnlOp = CHNLOP_NONE;
 
 	pHTInfo->ePeerHTSpecVer = HT_SPEC_VER_IEEE;
 
@@ -717,7 +718,7 @@ void HTResetSelfAndSavePeerSetting(struct rtllib_device *ieee,
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
 	u8	bIOTAction = 0;
 
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "==============>%s()\n", __func__);
+	netdev_vdbg(ieee->dev, "%s()\n", __func__);
 	/* unmark bEnableHT flag here is the same reason why unmarked in
 	 * function rtllib_softmac_new_net. WB 2008.09.10
 	 */
@@ -841,8 +842,7 @@ u8 HTCCheck(struct rtllib_device *ieee, u8 *pFrame)
 {
 	if (ieee->pHTInfo->bCurrentHTSupport) {
 		if ((IsQoSDataFrame(pFrame) && Frame_Order(pFrame)) == 1) {
-			RTLLIB_DEBUG(RTLLIB_DL_HT,
-				     "HT CONTROL FILED EXIST!!\n");
+			netdev_dbg(ieee->dev, "HT CONTROL FILED EXIST!!\n");
 			return true;
 		}
 	}
@@ -853,7 +853,8 @@ static void HTSetConnectBwModeCallback(struct rtllib_device *ieee)
 {
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
 
-	RTLLIB_DEBUG(RTLLIB_DL_HT, "======>%s()\n", __func__);
+	netdev_vdbg(ieee->dev, "%s()\n", __func__);
+
 	if (pHTInfo->bCurBW40MHz) {
 		if (pHTInfo->CurSTAExtChnlOffset == HT_EXTCHNL_OFFSET_UPPER)
 			ieee->set_chan(ieee->dev,
diff --git a/drivers/staging/rtl8192e/rtl819x_Qos.h b/drivers/staging/rtl8192e/rtl819x_Qos.h
index 55ef7ec33f65..3aa35ced2b8b 100644
--- a/drivers/staging/rtl8192e/rtl819x_Qos.h
+++ b/drivers/staging/rtl8192e/rtl819x_Qos.h
@@ -91,122 +91,22 @@ union tspec_body {
 	} f;
 };
 
-struct wmm_tspec {
-	u8		ID;
-	u8		Length;
-	u8		OUI[3];
-	u8		OUI_Type;
-	u8		OUI_SubType;
-	u8		Version;
-	union tspec_body Body;
-};
-
 struct octet_string {
 	u8 *Octet;
 	u16 Length;
 };
 
-#define	MAX_WMMELE_LENGTH	64
-
-#define QOS_MODE u32
-
-#define QOS_DISABLE		0
-#define QOS_WMM		1
-#define QOS_WMMSA		2
-#define QOS_EDCA		4
-#define QOS_HCCA		8
-#define QOS_WMM_UAPSD		16
-
-#define WMM_PARAM_ELE_BODY_LEN	18
-
-#define MAX_STA_TS_COUNT			16
-#define MAX_AP_TS_COUNT			32
-#define QOS_TSTREAM_KEY_SIZE		13
-
-#define WMM_ACTION_CATEGORY_CODE	17
-#define WMM_PARAM_ELE_BODY_LEN	18
-
-#define MAX_TSPEC_TSID				15
-#define SESSION_REJECT_TSID			0xfe
-#define DEFAULT_TSID					0xff
-
-#define ADDTS_TIME_SLOT				100
-
-#define ACM_TIMEOUT				1000
-#define SESSION_REJECT_TIMEOUT		60000
-
 enum ack_policy {
 	eAckPlc0_ACK		= 0x00,
 	eAckPlc1_NoACK		= 0x01,
 };
 
-
-#define SET_WMM_QOS_INFO_FIELD(_pStart, _val)	\
-	WriteEF1Byte(_pStart, _val)
-
-#define GET_WMM_QOS_INFO_FIELD_PARAMETERSET_COUNT(_pStart) \
-	LE_BITS_TO_1BYTE(_pStart, 0, 4)
-#define SET_WMM_QOS_INFO_FIELD_PARAMETERSET_COUNT(_pStart, _val) \
-	SET_BITS_TO_LE_1BYTE(_pStart, 0, 4, _val)
-
-#define GET_WMM_QOS_INFO_FIELD_AP_UAPSD(_pStart) \
-	LE_BITS_TO_1BYTE(_pStart, 7, 1)
-#define SET_WMM_QOS_INFO_FIELD_AP_UAPSD(_pStart, _val) \
-	SET_BITS_TO_LE_1BYTE(_pStart, 7, 1, _val)
-
-#define GET_WMM_QOS_INFO_FIELD_STA_AC_VO_UAPSD(_pStart) \
-	LE_BITS_TO_1BYTE(_pStart, 0, 1)
-#define SET_WMM_QOS_INFO_FIELD_STA_AC_VO_UAPSD(_pStart, _val) \
-	SET_BITS_TO_LE_1BYTE(_pStart, 0, 1, _val)
-
-#define GET_WMM_QOS_INFO_FIELD_STA_AC_VI_UAPSD(_pStart) \
-	LE_BITS_TO_1BYTE(_pStart, 1, 1)
-#define SET_WMM_QOS_INFO_FIELD_STA_AC_VI_UAPSD(_pStart, _val) \
-	SET_BITS_TO_LE_1BYTE(_pStart, 1, 1, _val)
-
-#define GET_WMM_QOS_INFO_FIELD_STA_AC_BE_UAPSD(_pStart) \
-	LE_BITS_TO_1BYTE(_pStart, 2, 1)
-#define SET_WMM_QOS_INFO_FIELD_STA_AC_BE_UAPSD(_pStart, _val) \
-	SET_BITS_TO_LE_1BYTE(_pStart, 2, 1, _val)
-
-#define GET_WMM_QOS_INFO_FIELD_STA_AC_BK_UAPSD(_pStart) \
-	LE_BITS_TO_1BYTE(_pStart, 3, 1)
-#define SET_WMM_QOS_INFO_FIELD_STA_AC_BK_UAPSD(_pStart, _val) \
-	SET_BITS_TO_LE_1BYTE(_pStart, 3, 1, _val)
-
-#define GET_WMM_QOS_INFO_FIELD_STA_MAX_SP_LEN(_pStart) \
-	LE_BITS_TO_1BYTE(_pStart, 5, 2)
-#define SET_WMM_QOS_INFO_FIELD_STA_MAX_SP_LEN(_pStart, _val) \
-	SET_BITS_TO_LE_1BYTE(_pStart, 5, 2, _val)
-
-enum qos_ie_source {
-	QOSIE_SRC_ADDTSREQ,
-	QOSIE_SRC_ADDTSRSP,
-	QOSIE_SRC_REASOCREQ,
-	QOSIE_SRC_REASOCRSP,
-	QOSIE_SRC_DELTS,
-};
-
-
-#define AC_CODING u32
-
 #define AC0_BE	0
 #define AC1_BK	1
 #define AC2_VI	2
 #define AC3_VO	3
 #define AC_MAX	4
 
-
-#define AC_PARAM_SIZE	4
-
-#define WMM_PARAM_ELEMENT_SIZE	(8+(4*AC_PARAM_SIZE))
-
-enum qos_ele_subtype {
-	QOSELE_TYPE_INFO		= 0x00,
-	QOSELE_TYPE_PARAM	= 0x01,
-};
-
-
 enum direction_value {
 	DIR_UP			= 0,
 	DIR_DOWN		= 1,
@@ -227,22 +127,6 @@ struct acm {
 	u8		HwAcmCtl;
 };
 
-
-
-#define AC_UAPSD	u8
-
-#define	GET_VO_UAPSD(_apsd) ((_apsd) & BIT0)
-#define	SET_VO_UAPSD(_apsd) ((_apsd) |= BIT0)
-
-#define	GET_VI_UAPSD(_apsd) ((_apsd) & BIT1)
-#define	SET_VI_UAPSD(_apsd) ((_apsd) |= BIT1)
-
-#define	GET_BK_UAPSD(_apsd) ((_apsd) & BIT2)
-#define	SET_BK_UAPSD(_apsd) ((_apsd) |= BIT2)
-
-#define	GET_BE_UAPSD(_apsd) ((_apsd) & BIT3)
-#define	SET_BE_UAPSD(_apsd) ((_apsd) |= BIT3)
-
 union qos_tclas {
 
 	struct _TYPE_GENERAL {
@@ -255,8 +139,8 @@ union qos_tclas {
 		u8		Priority;
 		u8		ClassifierType;
 		u8		Mask;
-		u8		SrcAddr[6];
-		u8		DstAddr[6];
+		u8		SrcAddr[ETH_ALEN];
+		u8		DstAddr[ETH_ALEN];
 		u16		Type;
 	} TYPE0_ETH;
 
@@ -294,65 +178,6 @@ union qos_tclas {
 	} TYPE2_8021Q;
 };
 
-struct qos_tstream {
-
-	bool			bUsed;
-	u16			MsduLifetime;
-	bool			bEstablishing;
-	u8			TimeSlotCount;
-	u8			DialogToken;
-	struct wmm_tspec TSpec;
-	struct wmm_tspec OutStandingTSpec;
-	u8			NominalPhyRate;
-};
-
-struct sta_qos {
-	u8 WMMIEBuf[MAX_WMMELE_LENGTH];
-	u8 *WMMIE;
-
-	QOS_MODE QosCapability;
-	QOS_MODE CurrentQosMode;
-
-	AC_UAPSD b4ac_Uapsd;
-	AC_UAPSD Curr4acUapsd;
-	u8 bInServicePeriod;
-	u8 MaxSPLength;
-	int NumBcnBeforeTrigger;
-
-	u8 *pWMMInfoEle;
-	u8 WMMParamEle[WMM_PARAM_ELEMENT_SIZE];
-
-	struct acm acm[4];
-	enum acm_method AcmMethod;
-
-	struct qos_tstream StaTsArray[MAX_STA_TS_COUNT];
-	u8				DialogToken;
-	struct wmm_tspec TSpec;
-
-	u8				QBssWirelessMode;
-
-	bool				bNoAck;
-
-	bool				bEnableRxImmBA;
-
-};
-
-#define QBSS_LOAD_SIZE				5
-
-struct bss_qos {
-	QOS_MODE bdQoSMode;
-	u8 bdWMMIEBuf[MAX_WMMELE_LENGTH];
-	struct octet_string bdWMMIE;
-
-	enum qos_ele_subtype EleSubType;
-
-	u8 *pWMMInfoEle;
-	u8 *pWMMParamEle;
-
-	u8 QBssLoad[QBSS_LOAD_SIZE];
-	bool bQBssLoadValid;
-};
-
 #define IsACValid(ac)		((ac >= 0 && ac <= 7) ? true : false)
 
 
@@ -367,23 +192,4 @@ union aci_aifsn {
 	} f;
 };
 
-union ecw {
-	u8	charData;
-	struct {
-		u8	ECWmin:4;
-		u8	ECWmax:4;
-	} f;
-};
-
-union ac_param {
-	u32	longData;
-	u8	charData[4];
-
-	struct {
-		union aci_aifsn AciAifsn;
-		union ecw Ecw;
-		u16		TXOPLimit;
-	} f;
-};
-
 #endif
diff --git a/drivers/staging/rtl8192e/rtl819x_TS.h b/drivers/staging/rtl8192e/rtl819x_TS.h
index 8601b1ad217d..b8fed556928c 100644
--- a/drivers/staging/rtl8192e/rtl819x_TS.h
+++ b/drivers/staging/rtl8192e/rtl819x_TS.h
@@ -35,7 +35,7 @@ struct ts_common_info {
 	struct list_head		List;
 	struct timer_list		SetupTimer;
 	struct timer_list		InactTimer;
-	u8				Addr[6];
+	u8				Addr[ETH_ALEN];
 	union tspec_body TSpec;
 	union qos_tclas TClass[TCLAS_NUM];
 	u8				TClasProc;
@@ -67,7 +67,6 @@ struct rx_ts_record {
 	u8				num;
 };
 
-void _setup_timer(struct timer_list *, void *, unsigned long);
 
 
 #endif
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
index 7d77d056228d..05aea4321b9d 100644
--- a/drivers/staging/rtl8192e/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -49,8 +49,10 @@ static void RxPktPendingTimeout(unsigned long data)
 			if (index == 0)
 				pRxTs->RxIndicateSeq = pReorderEntry->SeqNum;
 
-			if (SN_LESS(pReorderEntry->SeqNum, pRxTs->RxIndicateSeq) ||
-				SN_EQUAL(pReorderEntry->SeqNum, pRxTs->RxIndicateSeq)) {
+			if (SN_LESS(pReorderEntry->SeqNum,
+				    pRxTs->RxIndicateSeq) ||
+			    SN_EQUAL(pReorderEntry->SeqNum,
+				     pRxTs->RxIndicateSeq)) {
 				list_del_init(&pReorderEntry->List);
 
 				if (SN_EQUAL(pReorderEntry->SeqNum,
@@ -58,9 +60,9 @@ static void RxPktPendingTimeout(unsigned long data)
 					pRxTs->RxIndicateSeq =
 					      (pRxTs->RxIndicateSeq + 1) % 4096;
 
-				RTLLIB_DEBUG(RTLLIB_DL_REORDER,
-					     "%s(): Indicate SeqNum: %d\n",
-					     __func__, pReorderEntry->SeqNum);
+				netdev_dbg(ieee->dev,
+					   "%s(): Indicate SeqNum: %d\n",
+					   __func__, pReorderEntry->SeqNum);
 				ieee->stats_IndicateArray[index] =
 							 pReorderEntry->prxb;
 				index++;
@@ -78,8 +80,9 @@ static void RxPktPendingTimeout(unsigned long data)
 		pRxTs->RxTimeoutIndicateSeq = 0xffff;
 
 		if (index > REORDER_WIN_SIZE) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR,
-				     "RxReorderIndicatePacket(): Rx Reorder struct buffer full!!\n");
+			netdev_warn(ieee->dev,
+				    "%s(): Rx Reorder struct buffer full\n",
+				    __func__);
 			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
 					       flags);
 			return;
@@ -91,7 +94,8 @@ static void RxPktPendingTimeout(unsigned long data)
 	if (bPktInBuf && (pRxTs->RxTimeoutIndicateSeq == 0xffff)) {
 		pRxTs->RxTimeoutIndicateSeq = pRxTs->RxIndicateSeq;
 		mod_timer(&pRxTs->RxPktPendingTimer,  jiffies +
-			  msecs_to_jiffies(ieee->pHTInfo->RxReorderPendingTime));
+			  msecs_to_jiffies(ieee->pHTInfo->RxReorderPendingTime)
+			  );
 	}
 	spin_unlock_irqrestore(&(ieee->reorder_spinlock), flags);
 }
@@ -104,8 +108,7 @@ static void TsAddBaProcess(unsigned long data)
 				     TxTsRecord[num]);
 
 	TsInitAddBA(ieee, pTxTs, BA_POLICY_IMMEDIATE, false);
-	RTLLIB_DEBUG(RTLLIB_DL_BA,
-		     "TsAddBaProcess(): ADDBA Req is started!!\n");
+	netdev_dbg(ieee->dev, "%s(): ADDBA Req is started\n", __func__);
 }
 
 static void ResetTsCommonInfo(struct ts_common_info *pTsCommonInfo)
@@ -144,29 +147,29 @@ void TSInitialize(struct rtllib_device *ieee)
 	struct rx_reorder_entry *pRxReorderEntry = ieee->RxReorderEntry;
 	u8				count = 0;
 
-	RTLLIB_DEBUG(RTLLIB_DL_TS, "==========>%s()\n", __func__);
+	netdev_vdbg(ieee->dev, "%s()\n", __func__);
 	INIT_LIST_HEAD(&ieee->Tx_TS_Admit_List);
 	INIT_LIST_HEAD(&ieee->Tx_TS_Pending_List);
 	INIT_LIST_HEAD(&ieee->Tx_TS_Unused_List);
 
 	for (count = 0; count < TOTAL_TS_NUM; count++) {
 		pTxTS->num = count;
-		_setup_timer(&pTxTS->TsCommonInfo.SetupTimer,
+		setup_timer(&pTxTS->TsCommonInfo.SetupTimer,
 			    TsSetupTimeOut,
 			    (unsigned long) pTxTS);
 
-		_setup_timer(&pTxTS->TsCommonInfo.InactTimer,
+		setup_timer(&pTxTS->TsCommonInfo.InactTimer,
 			    TsInactTimeout,
 			    (unsigned long) pTxTS);
 
-		_setup_timer(&pTxTS->TsAddBaTimer,
+		setup_timer(&pTxTS->TsAddBaTimer,
 			    TsAddBaProcess,
 			    (unsigned long) pTxTS);
 
-		_setup_timer(&pTxTS->TxPendingBARecord.Timer,
+		setup_timer(&pTxTS->TxPendingBARecord.Timer,
 			    BaSetupTimeOut,
 			    (unsigned long) pTxTS);
-		_setup_timer(&pTxTS->TxAdmittedBARecord.Timer,
+		setup_timer(&pTxTS->TxAdmittedBARecord.Timer,
 			    TxBaInactTimeout,
 			    (unsigned long) pTxTS);
 
@@ -183,19 +186,19 @@ void TSInitialize(struct rtllib_device *ieee)
 		pRxTS->num = count;
 		INIT_LIST_HEAD(&pRxTS->RxPendingPktList);
 
-		_setup_timer(&pRxTS->TsCommonInfo.SetupTimer,
+		setup_timer(&pRxTS->TsCommonInfo.SetupTimer,
 			    TsSetupTimeOut,
 			    (unsigned long) pRxTS);
 
-		_setup_timer(&pRxTS->TsCommonInfo.InactTimer,
+		setup_timer(&pRxTS->TsCommonInfo.InactTimer,
 			    TsInactTimeout,
 			    (unsigned long) pRxTS);
 
-		_setup_timer(&pRxTS->RxAdmittedBARecord.Timer,
+		setup_timer(&pRxTS->RxAdmittedBARecord.Timer,
 			    RxBaInactTimeout,
 			    (unsigned long) pRxTS);
 
-		_setup_timer(&pRxTS->RxPktPendingTimer,
+		setup_timer(&pRxTS->RxPktPendingTimer,
 			    RxPktPendingTimeout,
 			    (unsigned long) pRxTS);
 
@@ -269,10 +272,10 @@ static struct ts_common_info *SearchAdmitTRStream(struct rtllib_device *ieee,
 		if (!search_dir[dir])
 			continue;
 		list_for_each_entry(pRet, psearch_list, List) {
-			if (memcmp(pRet->Addr, Addr, 6) == 0)
-				if (pRet->TSpec.f.TSInfo.field.ucTSID == TID)
-					if (pRet->TSpec.f.TSInfo.field.ucDirection == dir)
-						break;
+			if (memcmp(pRet->Addr, Addr, 6) == 0 &&
+			    pRet->TSpec.f.TSInfo.field.ucTSID == TID &&
+			    pRet->TSpec.f.TSInfo.field.ucDirection == dir)
+				break;
 
 		}
 		if (&pRet->List  != psearch_list)
@@ -318,17 +321,15 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 	enum direction_value Dir;
 
 	if (is_multicast_ether_addr(Addr)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "ERR! get TS for Broadcast or Multicast\n");
+		netdev_warn(ieee->dev, "Get TS for Broadcast or Multicast\n");
 		return false;
 	}
 	if (ieee->current_network.qos_data.supported == 0) {
 		UP = 0;
 	} else {
 		if (!IsACValid(TID)) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR,
-				     "ERR! in %s(), TID(%d) is not valid\n",
-				     __func__, TID);
+			netdev_warn(ieee->dev, "%s(): TID(%d) is not valid\n",
+				    __func__, TID);
 			return false;
 		}
 
@@ -357,8 +358,7 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 		return true;
 
 	if (!bAddNewTs) {
-		RTLLIB_DEBUG(RTLLIB_DL_TS,
-			     "add new TS failed(tid:%d)\n", UP);
+		netdev_dbg(ieee->dev, "add new TS failed(tid:%d)\n", UP);
 		return false;
 	}
 
@@ -374,7 +374,6 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 				((TxRxSelect == TX_DIR) ? DIR_DOWN : DIR_UP) :
 				((TxRxSelect == TX_DIR) ? DIR_UP : DIR_DOWN);
 
-	RTLLIB_DEBUG(RTLLIB_DL_TS, "to add Ts\n");
 	if (!list_empty(pUnusedList)) {
 		(*ppTS) = list_entry(pUnusedList->next,
 			  struct ts_common_info, List);
@@ -393,9 +392,9 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 			ResetRxTsEntry(tmp);
 		}
 
-		RTLLIB_DEBUG(RTLLIB_DL_TS,
-			     "to init current TS, UP:%d, Dir:%d, addr: %pM ppTs=%p\n",
-			     UP, Dir, Addr, *ppTS);
+		netdev_dbg(ieee->dev,
+			   "to init current TS, UP:%d, Dir:%d, addr: %pM ppTs=%p\n",
+			   UP, Dir, Addr, *ppTS);
 		pTSInfo->field.ucTrafficType = 0;
 		pTSInfo->field.ucTSID = UP;
 		pTSInfo->field.ucDirection = Dir;
@@ -413,14 +412,14 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 		return true;
 	}
 
-	RTLLIB_DEBUG(RTLLIB_DL_ERR,
-		     "ERR!!in function %s() There is not enough dir=%d(0=up down=1) TS record to be used!!",
-		     __func__, Dir);
+	netdev_warn(ieee->dev,
+		    "There is not enough dir=%d(0=up down=1) TS record to be used!",
+		    Dir);
 	return false;
 }
 
-static void RemoveTsEntry(struct rtllib_device *ieee, struct ts_common_info *pTs,
-			  enum tr_select TxRxSelect)
+static void RemoveTsEntry(struct rtllib_device *ieee,
+			  struct ts_common_info *pTs, enum tr_select TxRxSelect)
 {
 	del_timer_sync(&pTs->SetupTimer);
 	del_timer_sync(&pTs->InactTimer);
@@ -437,9 +436,8 @@ static void RemoveTsEntry(struct rtllib_device *ieee, struct ts_common_info *pTs
 			pRxReorderEntry = (struct rx_reorder_entry *)
 					list_entry(pRxTS->RxPendingPktList.prev,
 					struct rx_reorder_entry, List);
-			RTLLIB_DEBUG(RTLLIB_DL_REORDER,
-				     "%s(): Delete SeqNum %d!\n", __func__,
-				     pRxReorderEntry->SeqNum);
+			netdev_dbg(ieee->dev,  "%s(): Delete SeqNum %d!\n",
+				   __func__, pRxReorderEntry->SeqNum);
 			list_del_init(&pRxReorderEntry->List);
 			{
 				int i = 0;
@@ -539,16 +537,13 @@ void TsStartAddBaProcess(struct rtllib_device *ieee, struct tx_ts_record *pTxTS)
 		pTxTS->bAddBaReqInProgress = true;
 
 		if (pTxTS->bAddBaReqDelayed) {
-			RTLLIB_DEBUG(RTLLIB_DL_BA,
-				     "TsStartAddBaProcess(): Delayed Start ADDBA after 60 sec!!\n");
+			netdev_dbg(ieee->dev, "Start ADDBA after 60 sec!!\n");
 			mod_timer(&pTxTS->TsAddBaTimer, jiffies +
 				  msecs_to_jiffies(TS_ADDBA_DELAY));
 		} else {
-			RTLLIB_DEBUG(RTLLIB_DL_BA,
-				     "TsStartAddBaProcess(): Immediately Start ADDBA now!!\n");
+			netdev_dbg(ieee->dev, "Immediately Start ADDBA\n");
 			mod_timer(&pTxTS->TsAddBaTimer, jiffies+10);
 		}
 	} else
-		RTLLIB_DEBUG(RTLLIB_DL_BA, "%s()==>BA timer is already added\n",
-			     __func__);
+		netdev_dbg(ieee->dev, "BA timer is already added\n");
 }
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index 3c8b708df5c3..fd38c6dd146b 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -261,67 +261,6 @@ struct sw_chnl_cmd {
 #define	MGN_MCS14_SG		0x9e
 #define	MGN_MCS15_SG		0x9f
 
-
-enum	_ReasonCode {
-	unspec_reason	= 0x1,
-	auth_not_valid	= 0x2,
-	deauth_lv_ss	= 0x3,
-	inactivity		= 0x4,
-	ap_overload	= 0x5,
-	class2_err		= 0x6,
-	class3_err		= 0x7,
-	disas_lv_ss	= 0x8,
-	asoc_not_auth	= 0x9,
-
-	mic_failure	= 0xe,
-
-	invalid_IE		= 0x0d,
-	four_way_tmout	= 0x0f,
-	two_way_tmout	= 0x10,
-	IE_dismatch	= 0x11,
-	invalid_Gcipher = 0x12,
-	invalid_Pcipher = 0x13,
-	invalid_AKMP	= 0x14,
-	unsup_RSNIEver = 0x15,
-	invalid_RSNIE	= 0x16,
-	auth_802_1x_fail = 0x17,
-	ciper_reject		= 0x18,
-
-	QoS_unspec		= 0x20,
-	QAP_bandwidth	= 0x21,
-	poor_condition	= 0x22,
-	no_facility	= 0x23,
-	req_declined	= 0x25,
-	invalid_param	= 0x26,
-	req_not_honored = 0x27,
-	TS_not_created	= 0x2F,
-	DL_not_allowed	= 0x30,
-	dest_not_exist	= 0x31,
-	dest_not_QSTA	= 0x32,
-};
-
-enum hal_def_variable {
-	HAL_DEF_TPC_ENABLE,
-	HAL_DEF_INIT_GAIN,
-	HAL_DEF_PROT_IMP_MODE,
-	HAL_DEF_HIGH_POWER_MECHANISM,
-	HAL_DEF_RATE_ADAPTIVE_MECHANISM,
-	HAL_DEF_ANTENNA_DIVERSITY_MECHANISM,
-	HAL_DEF_LED,
-	HAL_DEF_CW_MAX_MIN,
-
-	HAL_DEF_WOWLAN,
-	HAL_DEF_ENDPOINTS,
-	HAL_DEF_MIN_TX_POWER_DBM,
-	HAL_DEF_MAX_TX_POWER_DBM,
-	HW_DEF_EFUSE_REPG_SECTION1_FLAG,
-	HW_DEF_EFUSE_REPG_DATA,
-	HW_DEF_GPIO,
-	HAL_DEF_PCI_SUPPORT_ASPM,
-	HAL_DEF_THERMAL_VALUE,
-	HAL_DEF_USB_IN_TOKEN_REV,
-};
-
 enum hw_variables {
 	HW_VAR_ETHER_ADDR,
 	HW_VAR_MULTICAST_REG,
@@ -440,27 +379,14 @@ enum rt_op_mode {
 #define IEEE_PARAM_AUTH_ALGS			5
 #define IEEE_PARAM_IEEE_802_1X			6
 #define IEEE_PARAM_WPAX_SELECT			7
-#define IEEE_PROTO_WPA				1
-#define IEEE_PROTO_RSN				2
-#define IEEE_WPAX_USEGROUP			0
-#define IEEE_WPAX_WEP40				1
-#define IEEE_WPAX_TKIP				2
-#define IEEE_WPAX_WRAP				3
-#define IEEE_WPAX_CCMP				4
-#define IEEE_WPAX_WEP104			5
-
-#define IEEE_KEY_MGMT_IEEE8021X			1
-#define IEEE_KEY_MGMT_PSK			2
 
 #define IEEE_MLME_STA_DEAUTH			1
 #define IEEE_MLME_STA_DISASSOC			2
 
 
 #define IEEE_CRYPT_ERR_UNKNOWN_ALG		2
-#define IEEE_CRYPT_ERR_UNKNOWN_ADDR		3
 #define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED	4
 #define IEEE_CRYPT_ERR_KEY_SET_FAILED		5
-#define IEEE_CRYPT_ERR_TX_KEY_SET_FAILED	6
 #define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
 #define	IEEE_CRYPT_ALG_NAME_LEN			16
 
@@ -495,19 +421,8 @@ struct ieee_param {
 	} u;
 };
 
-
-#if WIRELESS_EXT < 17
-#define IW_QUAL_QUAL_INVALID   0x10
-#define IW_QUAL_LEVEL_INVALID  0x20
-#define IW_QUAL_NOISE_INVALID  0x40
-#define IW_QUAL_QUAL_UPDATED   0x1
-#define IW_QUAL_LEVEL_UPDATED  0x2
-#define IW_QUAL_NOISE_UPDATED  0x4
-#endif
-
 #define msleep_interruptible_rsl  msleep_interruptible
 
-#define RTLLIB_DATA_LEN		2304
 /* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
  * 6.2.1.1.2.
  *
@@ -521,23 +436,13 @@ struct ieee_param {
 #define RTLLIB_3ADDR_LEN 24
 #define RTLLIB_4ADDR_LEN 30
 #define RTLLIB_FCS_LEN    4
-#define RTLLIB_HLEN		  (RTLLIB_4ADDR_LEN)
-#define RTLLIB_FRAME_LEN	     (RTLLIB_DATA_LEN + RTLLIB_HLEN)
-#define RTLLIB_MGMT_HDR_LEN 24
-#define RTLLIB_DATA_HDR3_LEN 24
-#define RTLLIB_DATA_HDR4_LEN 30
 
 #define RTLLIB_SKBBUFFER_SIZE 2500
 
 #define MIN_FRAG_THRESHOLD     256U
 #define MAX_FRAG_THRESHOLD     2346U
-#define MAX_HT_DATA_FRAG_THRESHOLD 0x2000
-
-#define HT_AMSDU_SIZE_4K 3839
-#define HT_AMSDU_SIZE_8K 7935
 
 /* Frame control field constants */
-#define RTLLIB_FCTL_VERS		0x0003
 #define RTLLIB_FCTL_FTYPE		0x000c
 #define RTLLIB_FCTL_STYPE		0x00f0
 #define RTLLIB_FCTL_FRAMETYPE	0x00fc
@@ -574,9 +479,6 @@ struct ieee_param {
 #define RTLLIB_STYPE_RTS		0x00B0
 #define RTLLIB_STYPE_CTS		0x00C0
 #define RTLLIB_STYPE_ACK		0x00D0
-#define RTLLIB_STYPE_CFEND		0x00E0
-#define RTLLIB_STYPE_CFENDACK	0x00F0
-#define RTLLIB_STYPE_BLOCKACK   0x0094
 
 /* data */
 #define RTLLIB_STYPE_DATA		0x0000
@@ -584,9 +486,6 @@ struct ieee_param {
 #define RTLLIB_STYPE_DATA_CFPOLL	0x0020
 #define RTLLIB_STYPE_DATA_CFACKPOLL	0x0030
 #define RTLLIB_STYPE_NULLFUNC	0x0040
-#define RTLLIB_STYPE_CFACK		0x0050
-#define RTLLIB_STYPE_CFPOLL		0x0060
-#define RTLLIB_STYPE_CFACKPOLL	0x0070
 #define RTLLIB_STYPE_QOS_DATA	0x0080
 #define RTLLIB_STYPE_QOS_NULL	0x00C0
 
@@ -615,13 +514,6 @@ enum act_category {
 	ACT_CAT_WMM = 17,
 };
 
-enum ts_action {
-	ACT_ADDTSREQ = 0,
-	ACT_ADDTSRSP = 1,
-	ACT_DELTS    = 2,
-	ACT_SCHEDULE = 3,
-};
-
 enum ba_action {
 	ACT_ADDBAREQ = 0,
 	ACT_ADDBARSP = 1,
@@ -643,20 +535,11 @@ enum led_ctl_mode {
 	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,
-	LED_CTL_STOP_WPS_FAIL = 12,
-	 LED_CTL_STOP_WPS_FAIL_OVERLAP = 13,
 };
 
 enum rt_rf_type_def {
 	RF_1T2R = 0,
 	RF_2T4R,
-	RF_2T2R,
-	RF_1T1R,
-	RF_2T2R_GREEN,
-	RF_819X_MAX_TYPE
 };
 
 enum wireless_mode {
@@ -669,100 +552,12 @@ enum wireless_mode {
 	WIRELESS_MODE_N_5G = 0x20
 };
 
-enum wireless_network_type {
-	WIRELESS_11B = 1,
-	WIRELESS_11G = 2,
-	WIRELESS_11A = 4,
-	WIRELESS_11N = 8
-};
-
-#define OUI_SUBTYPE_WMM_INFO		0
-#define OUI_SUBTYPE_WMM_PARAM	1
-#define OUI_SUBTYPE_QOS_CAPABI	5
-
-/* debug macros */
-extern u32 rtllib_debug_level;
-#define RTLLIB_DEBUG(level, fmt, args...) \
-do {								\
-	if (rtllib_debug_level & (level))			\
-		printk(KERN_DEBUG "rtllib: " fmt, ## args);	\
-} while (0)
-
-#define RTLLIB_DEBUG_DATA(level, data, datalen)	\
-	do {							\
-		if ((rtllib_debug_level & (level)) == (level)) {	\
-			printk(KERN_DEBUG "rtllib: %s()\n", __func__);	\
-			print_hex_dump_bytes(KERN_DEBUG, DUMP_PREFIX_NONE, \
-					     data, datalen); \
-		}					\
-	} while (0)
-
-/* To use the debug system;
- *
- * If you are defining a new debug classification, simply add it to the #define
- * list here in the form of:
- *
- * #define RTLLIB_DL_xxxx VALUE
- *
- * shifting value to the left one bit from the previous entry.  xxxx should be
- * the name of the classification (for example, WEP)
- *
- * You then need to either add a RTLLIB_xxxx_DEBUG() macro definition for your
- * classification, or use RTLLIB_DEBUG(RTLLIB_DL_xxxx, ...) whenever you want
- * to send output to that classification.
- *
- * To add your debug level to the list of levels seen when you perform
- *
- * % cat /proc/net/ipw/debug_level
- *
- * you simply need to add your entry to the ipw_debug_levels array.
- */
-
-#define RTLLIB_DL_INFO	  (1<<0)
-#define RTLLIB_DL_WX	    (1<<1)
-#define RTLLIB_DL_SCAN	  (1<<2)
-#define RTLLIB_DL_STATE	 (1<<3)
-#define RTLLIB_DL_MGMT	  (1<<4)
-#define RTLLIB_DL_FRAG	  (1<<5)
-#define RTLLIB_DL_EAP	   (1<<6)
-#define RTLLIB_DL_DROP	  (1<<7)
-
-#define RTLLIB_DL_TX	    (1<<8)
-#define RTLLIB_DL_RX	    (1<<9)
-
-#define RTLLIB_DL_HT		   (1<<10)
-#define RTLLIB_DL_BA		   (1<<11)
-#define RTLLIB_DL_TS		   (1<<12)
-#define RTLLIB_DL_QOS	   (1<<13)
-#define RTLLIB_DL_REORDER	   (1<<14)
-#define RTLLIB_DL_IOT	   (1<<15)
-#define RTLLIB_DL_IPS	   (1<<16)
-#define RTLLIB_DL_TRACE	   (1<<29)
-#define RTLLIB_DL_DATA	   (1<<30)
-#define RTLLIB_DL_ERR	   (1<<31)
-#define RTLLIB_ERROR(f, a...) pr_err("rtllib: " f, ## a)
-#define RTLLIB_WARNING(f, a...) pr_warn("rtllib: " f, ## a)
-#define RTLLIB_DEBUG_INFO(f, a...)   RTLLIB_DEBUG(RTLLIB_DL_INFO, f, ## a)
-
-#define RTLLIB_DEBUG_WX(f, a...)     RTLLIB_DEBUG(RTLLIB_DL_WX, f, ## a)
-#define RTLLIB_DEBUG_SCAN(f, a...)   RTLLIB_DEBUG(RTLLIB_DL_SCAN, f, ## a)
-#define RTLLIB_DEBUG_STATE(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_STATE, f, ## a)
-#define RTLLIB_DEBUG_MGMT(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_MGMT, f, ## a)
-#define RTLLIB_DEBUG_FRAG(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_FRAG, f, ## a)
-#define RTLLIB_DEBUG_EAP(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_EAP, f, ## a)
-#define RTLLIB_DEBUG_DROP(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_DROP, f, ## a)
-#define RTLLIB_DEBUG_TX(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_TX, f, ## a)
-#define RTLLIB_DEBUG_RX(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_RX, f, ## a)
-#define RTLLIB_DEBUG_QOS(f, a...)  RTLLIB_DEBUG(RTLLIB_DL_QOS, f, ## a)
-
 #ifndef ETH_P_PAE
 #define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
 #define ETH_P_IP	0x0800		/* Internet Protocol packet	*/
 #define ETH_P_ARP	0x0806		/* Address Resolution packet	*/
 #endif /* ETH_P_PAE */
 
-#define ETH_P_PREAUTH 0x88C7 /* IEEE 802.11i pre-authentication */
-
 #ifndef ETH_P_80211_RAW
 #define ETH_P_80211_RAW (ETH_P_ECONET + 1)
 #endif
@@ -788,7 +583,6 @@ enum _REG_PREAMBLE_MODE {
 
 #define SNAP_SIZE sizeof(struct rtllib_snap_hdr)
 
-#define WLAN_FC_GET_VERS(fc) ((fc) & RTLLIB_FCTL_VERS)
 #define WLAN_FC_GET_TYPE(fc) ((fc) & RTLLIB_FCTL_FTYPE)
 #define WLAN_FC_GET_STYPE(fc) ((fc) & RTLLIB_FCTL_STYPE)
 #define WLAN_FC_MORE_DATA(fc) ((fc) & RTLLIB_FCTL_MOREDATA)
@@ -802,30 +596,15 @@ enum _REG_PREAMBLE_MODE {
 #define WLAN_AUTH_SHARED_KEY 1
 #define WLAN_AUTH_LEAP 128
 
-#define WLAN_AUTH_CHALLENGE_LEN 128
-
 #define WLAN_CAPABILITY_ESS (1<<0)
 #define WLAN_CAPABILITY_IBSS (1<<1)
-#define WLAN_CAPABILITY_CF_POLLABLE (1<<2)
-#define WLAN_CAPABILITY_CF_POLL_REQUEST (1<<3)
 #define WLAN_CAPABILITY_PRIVACY (1<<4)
 #define WLAN_CAPABILITY_SHORT_PREAMBLE (1<<5)
-#define WLAN_CAPABILITY_PBCC (1<<6)
-#define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
-#define WLAN_CAPABILITY_SPECTRUM_MGMT (1<<8)
-#define WLAN_CAPABILITY_QOS (1<<9)
 #define WLAN_CAPABILITY_SHORT_SLOT_TIME (1<<10)
-#define WLAN_CAPABILITY_DSSS_OFDM (1<<13)
-
-/* 802.11g ERP information element */
-#define WLAN_ERP_NON_ERP_PRESENT (1<<0)
-#define WLAN_ERP_USE_PROTECTION (1<<1)
-#define WLAN_ERP_BARKER_PREAMBLE (1<<2)
 
 #define RTLLIB_STATMASK_SIGNAL (1<<0)
 #define RTLLIB_STATMASK_RSSI (1<<1)
 #define RTLLIB_STATMASK_NOISE (1<<2)
-#define RTLLIB_STATMASK_RATE (1<<3)
 #define RTLLIB_STATMASK_WEMASK 0x7
 
 #define RTLLIB_CCK_MODULATION    (1<<0)
@@ -850,44 +629,6 @@ enum _REG_PREAMBLE_MODE {
 #define RTLLIB_OFDM_RATE_54MB		0x6C
 #define RTLLIB_BASIC_RATE_MASK		0x80
 
-#define RTLLIB_CCK_RATE_1MB_MASK		(1<<0)
-#define RTLLIB_CCK_RATE_2MB_MASK		(1<<1)
-#define RTLLIB_CCK_RATE_5MB_MASK		(1<<2)
-#define RTLLIB_CCK_RATE_11MB_MASK		(1<<3)
-#define RTLLIB_OFDM_RATE_6MB_MASK		(1<<4)
-#define RTLLIB_OFDM_RATE_9MB_MASK		(1<<5)
-#define RTLLIB_OFDM_RATE_12MB_MASK		(1<<6)
-#define RTLLIB_OFDM_RATE_18MB_MASK		(1<<7)
-#define RTLLIB_OFDM_RATE_24MB_MASK		(1<<8)
-#define RTLLIB_OFDM_RATE_36MB_MASK		(1<<9)
-#define RTLLIB_OFDM_RATE_48MB_MASK		(1<<10)
-#define RTLLIB_OFDM_RATE_54MB_MASK		(1<<11)
-
-#define RTLLIB_CCK_RATES_MASK		0x0000000F
-#define RTLLIB_CCK_BASIC_RATES_MASK	(RTLLIB_CCK_RATE_1MB_MASK | \
-	RTLLIB_CCK_RATE_2MB_MASK)
-#define RTLLIB_CCK_DEFAULT_RATES_MASK	(RTLLIB_CCK_BASIC_RATES_MASK | \
-	RTLLIB_CCK_RATE_5MB_MASK | \
-	RTLLIB_CCK_RATE_11MB_MASK)
-
-#define RTLLIB_OFDM_RATES_MASK		0x00000FF0
-#define RTLLIB_OFDM_BASIC_RATES_MASK	(RTLLIB_OFDM_RATE_6MB_MASK | \
-	RTLLIB_OFDM_RATE_12MB_MASK | \
-	RTLLIB_OFDM_RATE_24MB_MASK)
-#define RTLLIB_OFDM_DEFAULT_RATES_MASK	(RTLLIB_OFDM_BASIC_RATES_MASK | \
-	RTLLIB_OFDM_RATE_9MB_MASK  | \
-	RTLLIB_OFDM_RATE_18MB_MASK | \
-	RTLLIB_OFDM_RATE_36MB_MASK | \
-	RTLLIB_OFDM_RATE_48MB_MASK | \
-	RTLLIB_OFDM_RATE_54MB_MASK)
-#define RTLLIB_DEFAULT_RATES_MASK (RTLLIB_OFDM_DEFAULT_RATES_MASK | \
-				RTLLIB_CCK_DEFAULT_RATES_MASK)
-
-#define RTLLIB_NUM_OFDM_RATES	    8
-#define RTLLIB_NUM_CCK_RATES		    4
-#define RTLLIB_OFDM_SHIFT_MASK_A	 4
-
-
 /* this is stolen and modified from the madwifi driver*/
 #define RTLLIB_FC0_TYPE_MASK		0x0c
 #define RTLLIB_FC0_TYPE_DATA		0x08
@@ -943,7 +684,6 @@ struct rtllib_rx_stats {
 	u32 TimeStampHigh;
 	bool bShift;
 	bool bIsQosData;
-	u8   UserPriority;
 
 	u8    RxDrvInfoSize;
 	u8    RxBufShift;
@@ -957,17 +697,13 @@ struct rtllib_rx_stats {
 	bool  bPacketMatchBSSID;
 	bool  bIsCCK;
 	bool  bPacketToSelf;
-	u8 *virtual_address;
 	u16    packetlength;
 	u16    fraglength;
 	u16    fragoffset;
 	u16    ntotalfrag;
-	bool   bisrxaggrsubframe;
 	bool   bPacketBeacon;
 	bool   bToSelfBA;
-	char   cck_adc_pwdb[4];
 	u16    Seq_Num;
-	u8     nTotalAggPkt;
 };
 
 /* IEEE 802.11 requires that STA supports concurrent reception of at least
@@ -1225,15 +961,6 @@ struct rtllib_assoc_request_frame {
 	struct rtllib_info_element info_element[0];
 } __packed;
 
-struct rtllib_reassoc_request_frame {
-	struct rtllib_hdr_3addr header;
-	__le16 capability;
-	__le16 listen_interval;
-	u8 current_ap[ETH_ALEN];
-	/* SSID, supported rates, RSN */
-	struct rtllib_info_element info_element[0];
-} __packed;
-
 struct rtllib_assoc_response_frame {
 	struct rtllib_hdr_3addr header;
 	__le16 capability;
@@ -1253,12 +980,6 @@ struct rtllib_txb {
 	struct sk_buff *fragments[0];
 };
 
-#define MAX_TX_AGG_COUNT		  16
-struct rtllib_drv_agg_txb {
-	u8 nr_drv_agg_frames;
-	struct sk_buff *tx_agg_frames[MAX_TX_AGG_COUNT];
-} __packed;
-
 #define MAX_SUBFRAME_COUNT		  64
 struct rtllib_rxb {
 	u8 nr_subframes;
@@ -1279,9 +1000,6 @@ union frameqos {
 	} field;
 };
 
-/* SWEEP TABLE ENTRIES NUMBER*/
-#define MAX_SWEEP_TAB_ENTRIES		  42
-#define MAX_SWEEP_TAB_ENTRIES_PER_PACKET  7
 /* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
  * only use 8, and then use extended rates for the remaining supported
  * rates.  Other APs, however, stick all of their supported rates on the
@@ -1295,8 +1013,6 @@ union frameqos {
 #define RTLLIB_SOFTMAC_SCAN_TIME	   100
 #define RTLLIB_SOFTMAC_ASSOC_RETRY_TIME (HZ * 2)
 
-#define CRC_LENGTH		 4U
-
 #define MAX_WPA_IE_LEN 64
 #define MAX_WZC_IE_LEN 256
 
@@ -1310,12 +1026,6 @@ union frameqos {
 #define NETWORK_HAS_QOS_MASK	    (NETWORK_HAS_QOS_PARAMETERS | \
 					 NETWORK_HAS_QOS_INFORMATION)
 /* 802.11h */
-#define NETWORK_HAS_POWER_CONSTRAINT    (1<<5)
-#define NETWORK_HAS_CSA		 (1<<6)
-#define NETWORK_HAS_QUIET	       (1<<7)
-#define NETWORK_HAS_IBSS_DFS	    (1<<8)
-#define NETWORK_HAS_TPC_REPORT	  (1<<9)
-
 #define NETWORK_HAS_ERP_VALUE	   (1<<10)
 
 #define QOS_QUEUE_NUM		   4
@@ -1325,7 +1035,6 @@ union frameqos {
 #define QOS_OUI_INFO_SUB_TYPE	   0
 #define QOS_OUI_PARAM_SUB_TYPE	  1
 #define QOS_VERSION_1		   1
-#define QOS_AIFSN_MIN_VALUE	     2
 
 struct rtllib_qos_information_element {
 	u8 elementID;
@@ -1377,31 +1086,6 @@ struct rtllib_wmm_ac_param {
 	u16 ac_txop_limit;
 };
 
-struct rtllib_wmm_ts_info {
-	u8 ac_dir_tid;
-	u8 ac_up_psb;
-	u8 reserved;
-} __packed;
-
-struct rtllib_wmm_tspec_elem {
-	struct rtllib_wmm_ts_info ts_info;
-	u16 norm_msdu_size;
-	u16 max_msdu_size;
-	u32 min_serv_inter;
-	u32 max_serv_inter;
-	u32 inact_inter;
-	u32 suspen_inter;
-	u32 serv_start_time;
-	u32 min_data_rate;
-	u32 mean_data_rate;
-	u32 peak_data_rate;
-	u32 max_burst_size;
-	u32 delay_bound;
-	u32 min_phy_rate;
-	u16 surp_band_allow;
-	u16 medium_time;
-} __packed;
-
 enum eap_type {
 	EAP_PACKET = 0,
 	EAPOL_START,
@@ -1466,13 +1150,6 @@ struct rtllib_softmac_stats {
 	unsigned int txretrycount;
 };
 
-#define BEACON_PROBE_SSID_ID_POSITION 12
-
-struct rtllib_info_element_hdr {
-	u8 id;
-	u8 len;
-} __packed;
-
 /* These are the data types that can make up management packets
  *
  * u16 auth_algorithm;
@@ -1523,12 +1200,6 @@ enum {WMM_all_frame, WMM_two_frame, WMM_four_frame, WMM_six_frame};
 	((up) < 6) ? WME_AC_VI : \
 	WME_AC_VO)
 
-#define AC2UP(_ac)	(       \
-	((_ac) == WME_AC_VO) ? 6 : \
-	((_ac) == WME_AC_VI) ? 5 : \
-	((_ac) == WME_AC_BK) ? 1 : \
-	0)
-
 #define	ETHER_ADDR_LEN		6	/* length of an Ethernet address */
 #define ETHERNET_HEADER_SIZE    14      /* length of two Ethernet address
 					 * plus ether type*/
@@ -1539,14 +1210,6 @@ struct	ether_header {
 	u16 ether_type;
 } __packed;
 
-#ifndef ETHERTYPE_PAE
-#define	ETHERTYPE_PAE	0x888e		/* EAPOL PAE/802.1x */
-#endif
-#ifndef ETHERTYPE_IP
-#define	ETHERTYPE_IP	0x0800		/* IP protocol */
-#endif
-
-
 enum erp_t {
 	ERP_NonERPpresent	= 0x01,
 	ERP_UseProtection	= 0x02,
@@ -1570,7 +1233,7 @@ struct rtllib_network {
 	u16	CcxRmState[2];
 	bool	bMBssidValid;
 	u8	MBssidMask;
-	u8	MBssid[6];
+	u8	MBssid[ETH_ALEN];
 	bool	bWithCcxVerNum;
 	u8	BssCcxVerNumber;
 	/* These are network statistics */
@@ -1583,7 +1246,6 @@ struct rtllib_network {
 	unsigned long last_scanned;
 	u8  mode;
 	u32 flags;
-	u32 last_associate;
 	u32 time_stamp[2];
 	u16 beacon_interval;
 	u16 listen_interval;
@@ -1617,7 +1279,6 @@ struct rtllib_network {
 	bool unknown_cap_exist;
 	bool	berp_info_valid;
 	bool buseprotection;
-	bool bIsNetgear854T;
 	u8 SignalStrength;
 	u8 RSSI;
 	struct list_head list;
@@ -1666,17 +1327,7 @@ enum rtllib_state {
 
 #define CFG_RTLLIB_RESERVE_FCS (1<<0)
 #define CFG_RTLLIB_COMPUTE_FCS (1<<1)
-#define CFG_RTLLIB_RTS (1<<2)
 
-#define RTLLIB_24GHZ_MIN_CHANNEL 1
-#define RTLLIB_24GHZ_MAX_CHANNEL 14
-#define RTLLIB_24GHZ_CHANNELS (RTLLIB_24GHZ_MAX_CHANNEL - \
-				  RTLLIB_24GHZ_MIN_CHANNEL + 1)
-
-#define RTLLIB_52GHZ_MIN_CHANNEL 34
-#define RTLLIB_52GHZ_MAX_CHANNEL 165
-#define RTLLIB_52GHZ_CHANNELS (RTLLIB_52GHZ_MAX_CHANNEL - \
-				  RTLLIB_52GHZ_MIN_CHANNEL + 1)
 struct tx_pending {
 	int frag;
 	struct rtllib_txb *txb;
@@ -1717,81 +1368,21 @@ enum ips_callback_function {
 	IPS_CALLBACK_JOIN_REQUEST = 2,
 };
 
-enum rt_join_action {
-	RT_JOIN_INFRA   = 1,
-	RT_JOIN_IBSS  = 2,
-	RT_START_IBSS = 3,
-	RT_NO_ACTION  = 4,
-};
-
-struct ibss_parms {
-	u16   atimWin;
-};
-#define MAX_NUM_RATES	264
-
 enum rt_rf_power_state {
 	eRfOn,
 	eRfSleep,
 	eRfOff
 };
 
-#define	MAX_SUPPORT_WOL_PATTERN_NUM		8
-
-#define	MAX_WOL_BIT_MASK_SIZE		16
-#define	MAX_WOL_PATTERN_SIZE		128
-
-enum wol_pattern_type {
-	eNetBIOS = 0,
-	eIPv4IPv6ARP,
-	eIPv4IPv6TCPSYN,
-	eMACIDOnly,
-	eNoDefined,
-};
-
-struct rt_pm_wol_info {
-	u32	PatternId;
-	u32	Mask[4];
-	u16	CrcRemainder;
-	u8	WFMIndex;
-	enum wol_pattern_type PatternType;
-};
-
 struct rt_pwr_save_ctrl {
 
 	bool				bInactivePs;
 	bool				bIPSModeBackup;
-	bool				bHaltAdapterClkRQ;
 	bool				bSwRfProcessing;
 	enum rt_rf_power_state eInactivePowerState;
-	struct work_struct		InactivePsWorkItem;
-	struct timer_list	InactivePsTimer;
-
 	enum ips_callback_function ReturnPoint;
 
-	bool				bTmpBssDesc;
-	enum rt_join_action tmpJoinAction;
-	struct rtllib_network tmpBssDesc;
-
-	bool				bTmpScanOnly;
-	bool				bTmpActiveScan;
-	bool				bTmpFilterHiddenAP;
-	bool				bTmpUpdateParms;
-	u8				tmpSsidBuf[33];
-	struct octet_string tmpSsid2Scan;
-	bool				bTmpSsid2Scan;
-	u8				tmpNetworkType;
-	u8				tmpChannelNumber;
-	u16				tmpBcnPeriod;
-	u8				tmpDtimPeriod;
-	u16				tmpmCap;
-	struct octet_string tmpSuppRateSet;
-	u8				tmpSuppRateBuf[MAX_NUM_RATES];
-	bool				bTmpSuppRate;
-	struct ibss_parms tmpIbpm;
-	bool				bTmpIbpm;
-
 	bool				bLeisurePs;
-	u32				PowerProfile;
 	u8				LpsIdleCount;
 	u8				RegMaxLPSAwakeIntvl;
 	u8				LPSAwakeIntvl;
@@ -1800,18 +1391,6 @@ struct rt_pwr_save_ctrl {
 	u32				RegRfPsLevel;
 
 	bool				bFwCtrlLPS;
-	u8				FWCtrlPSMode;
-
-	bool				LinkReqInIPSRFOffPgs;
-	bool				BufConnectinfoBefore;
-
-
-	bool				bGpioRfSw;
-
-	u8				RegAMDPciASPM;
-
-	u8				oWLANMode;
-	struct rt_pm_wol_info PmWoLPatternInfo[MAX_SUPPORT_WOL_PATTERN_NUM];
 
 };
 
@@ -1871,13 +1450,6 @@ enum fw_cmd_io_type {
 	FW_CMD_TX_FEEDBACK_CCX_ENABLE = 21,
 	FW_CMD_LPS_ENTER = 22,
 	FW_CMD_LPS_LEAVE = 23,
-	FW_CMD_DIG_MODE_SS = 24,
-	FW_CMD_DIG_MODE_FA = 25,
-	FW_CMD_ADD_A2_ENTRY = 26,
-	FW_CMD_CTRL_DM_BY_DRIVER = 27,
-	FW_CMD_CTRL_DM_BY_DRIVER_NEW = 28,
-	FW_CMD_PAPE_CONTROL = 29,
-	FW_CMD_CHAN_SET = 30,
 };
 
 #define RT_MAX_LD_SLOT_NUM	10
@@ -1897,15 +1469,11 @@ struct rt_link_detect {
 	bool				bBusyTraffic;
 	bool				bHigherBusyTraffic;
 	bool				bHigherBusyRxTraffic;
-	u8				IdleCount;
-	u32				NumTxUnicastOkInPeriod;
-	u32				LastNumTxUnicast;
-	u32				LastNumRxUnicast;
 };
 
 struct sw_cam_table {
 
-	u8				macaddr[6];
+	u8				macaddr[ETH_ALEN];
 	bool				bused;
 	u8				key_buf[16];
 	u16				key_type;
@@ -1937,26 +1505,13 @@ struct rate_adaptive {
 	u8				PreRATRState;
 
 };
-enum ratr_table_mode_8192s {
-	RATR_INX_WIRELESS_NGB = 0,
-	RATR_INX_WIRELESS_NG = 1,
-	RATR_INX_WIRELESS_NB = 2,
-	RATR_INX_WIRELESS_N = 3,
-	RATR_INX_WIRELESS_GB = 4,
-	RATR_INX_WIRELESS_G = 5,
-	RATR_INX_WIRELESS_B = 6,
-	RATR_INX_WIRELESS_MC = 7,
-	RATR_INX_WIRELESS_A = 8,
-};
 
 #define	NUM_PMKID_CACHE		16
 struct rt_pmkid_list {
-	u8 bUsed;
-	u8 Bssid[6];
+	u8 Bssid[ETH_ALEN];
 	u8 PMKID[16];
 	u8 SsidBuf[33];
-	u8 *ssid_octet;
-	u16 ssid_length;
+	u8 bUsed;
 };
 
 struct rt_intel_promisc_mode {
@@ -1967,19 +1522,9 @@ struct rt_intel_promisc_mode {
 
 /*************** DRIVER STATUS   *****/
 #define STATUS_SCANNING			0
-#define STATUS_SCAN_HW			1
-#define STATUS_SCAN_ABORTING	2
-#define STATUS_SETTING_CHAN		3
 /*************** DRIVER STATUS   *****/
 
 enum {
-	NO_USE		= 0,
-	USED		= 1,
-	HW_SEC		= 2,
-	SW_SEC		= 3,
-};
-
-enum {
 	LPS_IS_WAKE = 0,
 	LPS_IS_SLEEP = 1,
 	LPS_WAIT_NULL_DATA_SEND = 2,
@@ -1993,37 +1538,23 @@ struct rtllib_device {
 	bool disable_mgnt_queue;
 
 	unsigned long status;
-	short hwscan_ch_bk;
-	enum ht_extchnl_offset chan_offset_bk;
-	enum ht_channel_width bandwidth_bk;
-	u8 hwscan_sem_up;
 	u8	CntAfterLink;
 
 	enum rt_op_mode OpMode;
 
-	u8 VersionID;
 	/* The last AssocReq/Resp IEs */
 	u8 *assocreq_ies, *assocresp_ies;
 	size_t assocreq_ies_len, assocresp_ies_len;
 
-	bool b_customer_lenovo_id;
-	bool	bForcedShowRxRate;
-	bool	bForcedShowRateStill;
-	u8	SystemQueryDataRateCount;
 	bool	bForcedBgMode;
-	bool bUseRAMask;
-	bool b1x1RecvCombine;
 	u8 RF_Type;
-	bool b1SSSupport;
 
 	u8 hwsec_active;
 	bool is_silent_reset;
-	bool force_mic_error;
 	bool is_roaming;
 	bool ieee_up;
 	bool cannot_notify;
 	bool bSupportRemoteWakeUp;
-	enum rt_ps_mode dot11PowerSaveMode;
 	bool actscanning;
 	bool FirstIe_InScan;
 	bool be_scan_inprogress;
@@ -2033,7 +1564,6 @@ struct rtllib_device {
 	bool is_set_key;
 	bool wx_set_enc;
 	struct rt_hi_throughput *pHTInfo;
-	spinlock_t bw_spinlock;
 
 	spinlock_t reorder_spinlock;
 	u8	Regdot11HTOperationalRateSet[16];
@@ -2042,12 +1572,9 @@ struct rtllib_device {
 	u8	RegHTSuppRateSet[16];
 	u8	HTCurrentOperaRate;
 	u8	HTHighestOperaRate;
-	u8	MinSpaceCfg;
-	u8	MaxMssDensity;
 	u8	bTxDisableRateFallBack;
 	u8	bTxUseDriverAssingedRate;
 	u8	bTxEnableFwCalcDur;
-	atomic_t	atm_chnlop;
 	atomic_t	atm_swbw;
 
 	struct list_head		Tx_TS_Admit_List;
@@ -2060,7 +1587,6 @@ struct rtllib_device {
 	struct rx_ts_record RxTsRecord[TOTAL_TS_NUM];
 	struct rx_reorder_entry RxReorderEntry[128];
 	struct list_head		RxReorder_Unused_List;
-	u8				ForcedPriority;
 
 
 	/* Bookkeeping structures */
@@ -2079,8 +1605,6 @@ struct rtllib_device {
 	bool bNetPromiscuousMode;
 	struct rt_intel_promisc_mode IntelPromiscuousModeInfo;
 
-	struct iw_spy_data spy_data;
-
 	spinlock_t lock;
 	spinlock_t wpax_suitlist_lock;
 
@@ -2098,21 +1622,11 @@ struct rtllib_device {
 
 	/* If the host performs {en,de}cryption, then set to 1 */
 	int host_encrypt;
-	int host_encrypt_msdu;
 	int host_decrypt;
-	/* host performs multicast decryption */
-	int host_mc_decrypt;
-
-	/* host should strip IV and ICV from protected frames */
-	/* meaningful only when hardware decryption is being used */
-	int host_strip_iv_icv;
 
-	int host_open_frag;
-	int host_build_iv;
 	int ieee802_1x; /* is IEEE 802.1X used */
 
 	/* WPA data */
-	bool bHalfNMode;
 	bool bHalfWirelessN24GMode;
 	int wpa_enabled;
 	int drop_unencrypted;
@@ -2122,15 +1636,13 @@ struct rtllib_device {
 	u8 *wpa_ie;
 	size_t wps_ie_len;
 	u8 *wps_ie;
-	u8 ap_mac_addr[6];
+	u8 ap_mac_addr[ETH_ALEN];
 	u16 pairwise_key_type;
 	u16 group_key_type;
 
 	struct lib80211_crypt_info crypt_info;
 
 	struct sw_cam_table swcamtable[TOTAL_CAM_ENTRY];
-	int bcrx_sta_key; /* use individual keys to override default keys even
-			   * with RX of broad/multicast frames */
 
 	struct rt_pmkid_list PMKIDList[NUM_PMKID_CACHE];
 
@@ -2159,21 +1671,15 @@ struct rtllib_device {
 	enum rtllib_state state;
 
 	int short_slot;
-	int reg_mode;
 	int mode;       /* A, B, G */
 	int modulation; /* CCK, OFDM */
 	int freq_band;  /* 2.4Ghz, 5.2Ghz, Mixed */
-	int abg_true;   /* ABG flag	      */
 
 	/* used for forcing the ibss workqueue to terminate
 	 * without wait for the syncro scan to terminate
 	 */
 	short sync_scan_hurryup;
 	u16 scan_watch_dog;
-	int perfect_rssi;
-	int worst_rssi;
-
-	u16 prev_seq_ctl;       /* used to drop duplicate frames */
 
 	/* map of allowed channels. 0 is dummy */
 	void *pDot11dInfo;
@@ -2185,7 +1691,6 @@ struct rtllib_device {
 
 	int rate;       /* current rate */
 	int basic_rate;
-	u32	currentRate;
 
 	short active_scan;
 
@@ -2231,7 +1736,6 @@ struct rtllib_device {
 	short ssid_set;
 
 	/* set on initialization */
-	u8  qos_support;
 	unsigned int wmm_acm;
 
 	/* for discarding duplicated packets in IBSS */
@@ -2251,21 +1755,13 @@ struct rtllib_device {
 	struct sk_buff *mgmt_queue_ring[MGMT_QUEUE_NUM];
 	int mgmt_queue_head;
 	int mgmt_queue_tail;
-#define RTLLIB_QUEUE_LIMIT 128
 	u8 AsocRetryCount;
-	unsigned int hw_header;
 	struct sk_buff_head skb_waitQ[MAX_QUEUE_SIZE];
 	struct sk_buff_head  skb_aggQ[MAX_QUEUE_SIZE];
-	struct sk_buff_head  skb_drv_aggQ[MAX_QUEUE_SIZE];
-	u32	sta_edca_param[4];
-	bool aggregation;
-	bool enable_rx_imm_BA;
-	bool bibsscoordinator;
 
 	bool	bdynamic_txpower_enable;
 
 	bool bCTSToSelfEnable;
-	u8	CTSToSelfTH;
 
 	u32	fsync_time_interval;
 	u32	fsync_rate_bitmap;
@@ -2283,7 +1779,6 @@ struct rtllib_device {
 	struct rt_link_detect LinkDetectInfo;
 	bool bIsAggregateFrame;
 	struct rt_pwr_save_ctrl PowerSaveControl;
-	u8 amsdu_in_process;
 
 	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
 	struct tx_pending tx_pending;
@@ -2298,7 +1793,6 @@ struct rtllib_device {
 	struct work_struct ips_leave_wq;
 	struct delayed_work associate_procedure_wq;
 	struct delayed_work softmac_scan_wq;
-	struct delayed_work softmac_hint11d_wq;
 	struct delayed_work associate_retry_wq;
 	struct delayed_work start_ibss_wq;
 	struct delayed_work hw_wakeup_wq;
@@ -2330,11 +1824,6 @@ struct rtllib_device {
 			       struct net_device *dev);
 
 	int (*reset_port)(struct net_device *dev);
-	int (*is_queue_full)(struct net_device *dev, int pri);
-
-	int (*handle_management)(struct net_device *dev,
-				 struct rtllib_network *network, u16 type);
-	int (*is_qos_active)(struct net_device *dev, struct sk_buff *skb);
 
 	/* Softmac-generated frames (management) are TXed via this
 	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
@@ -2370,25 +1859,6 @@ struct rtllib_device {
 	 */
 	void (*set_chan)(struct net_device *dev, short ch);
 
-	/* These are not used if the ieee stack takes care of
-	 * scanning (IEEE_SOFTMAC_SCAN feature set).
-	 * In this case only the set_chan is used.
-	 *
-	 * The syncro version is similar to the start_scan but
-	 * does not return until all channels has been scanned.
-	 * this is called in user context and should sleep,
-	 * it is called in a work_queue when switching to ad-hoc mode
-	 * or in behalf of iwlist scan when the card is associated
-	 * and root user ask for a scan.
-	 * the function stop_scan should stop both the syncro and
-	 * background scanning and can sleep.
-	 * The function start_scan should initiate the background
-	 * scanning and can't sleep.
-	 */
-	void (*scan_syncro)(struct net_device *dev);
-	void (*start_scan)(struct net_device *dev);
-	void (*stop_scan)(struct net_device *dev);
-
 	void (*rtllib_start_hw_scan)(struct net_device *dev);
 	void (*rtllib_stop_hw_scan)(struct net_device *dev);
 
@@ -2422,7 +1892,6 @@ struct rtllib_device {
 
 	/* check whether Tx hw resource available */
 	short (*check_nic_enough_desc)(struct net_device *dev, int queue_index);
-	short (*get_nic_desc_num)(struct net_device *dev, int queue_index);
 	void (*SetBWModeHandler)(struct net_device *dev,
 				 enum ht_channel_width Bandwidth,
 				 enum ht_extchnl_offset Offset);
@@ -2430,24 +1899,16 @@ struct rtllib_device {
 	void (*SetWirelessMode)(struct net_device *dev, u8 wireless_mode);
 	bool (*GetHalfNmodeSupportByAPsHandler)(struct net_device *dev);
 	u8   (*rtllib_ap_sec_type)(struct rtllib_device *ieee);
-	void (*HalUsbRxAggrHandler)(struct net_device *dev, bool Value);
 	void (*InitialGainHandler)(struct net_device *dev, u8 Operation);
 	bool (*SetFwCmdHandler)(struct net_device *dev,
 				enum fw_cmd_io_type FwCmdIO);
-	void (*UpdateHalRAMaskHandler)(struct net_device *dev, bool bMulticast,
-				       u8 macId, u8 MimoPs, u8 WirelessMode,
-				       u8 bCurTxBW40MHz, u8 rssi_level);
 	void (*UpdateBeaconInterruptHandler)(struct net_device *dev,
 					     bool start);
-	void (*UpdateInterruptMaskHandler)(struct net_device *dev, u32 AddMSR,
-					   u32 RemoveMSR);
-	u16  (*rtl_11n_user_show_rates)(struct net_device *dev);
 	void (*ScanOperationBackupHandler)(struct net_device *dev,
 					   u8 Operation);
 	void (*LedControlHandler)(struct net_device *dev,
 				  enum led_ctl_mode LedAction);
 	void (*SetHwRegHandler)(struct net_device *dev, u8 variable, u8 *val);
-	void (*GetHwRegHandler)(struct net_device *dev, u8 variable, u8 *val);
 
 	void (*AllowAllDestAddrHandler)(struct net_device *dev,
 					bool bAllowAllDA, bool WriteIntoReg);
@@ -2455,7 +1916,6 @@ struct rtllib_device {
 	void (*rtllib_ips_leave_wq)(struct net_device *dev);
 	void (*rtllib_ips_leave)(struct net_device *dev);
 	void (*LeisurePSLeave)(struct net_device *dev);
-	void (*rtllib_rfkill_poll)(struct net_device *dev);
 
 	/* This must be the last item so that it points to the data
 	 * allocated beyond this structure by alloc_rtllib
@@ -2524,30 +1984,6 @@ static inline int rtllib_is_empty_essid(const char *essid, int essid_len)
 	return 1;
 }
 
-static inline int rtllib_is_valid_mode(struct rtllib_device *ieee, int mode)
-{
-	/* It is possible for both access points and our device to support
-	 * combinations of modes, so as long as there is one valid combination
-	 * of ap/device supported modes, then return success
-	 */
-	if ((mode & IEEE_A) &&
-	    (ieee->modulation & RTLLIB_OFDM_MODULATION) &&
-	    (ieee->freq_band & RTLLIB_52GHZ_BAND))
-		return 1;
-
-	if ((mode & IEEE_G) &&
-	    (ieee->modulation & RTLLIB_OFDM_MODULATION) &&
-	    (ieee->freq_band & RTLLIB_24GHZ_BAND))
-		return 1;
-
-	if ((mode & IEEE_B) &&
-	    (ieee->modulation & RTLLIB_CCK_MODULATION) &&
-	    (ieee->freq_band & RTLLIB_24GHZ_BAND))
-		return 1;
-
-	return 0;
-}
-
 static inline int rtllib_get_hdrlen(u16 fc)
 {
 	int hdrlen = RTLLIB_3ADDR_LEN;
@@ -2623,8 +2059,6 @@ static inline int rtllib_is_cck_rate(u8 rate)
 extern void free_rtllib(struct net_device *dev);
 extern struct net_device *alloc_rtllib(int sizeof_priv);
 
-extern int rtllib_set_encryption(struct rtllib_device *ieee);
-
 /* rtllib_tx.c */
 
 extern int rtllib_encrypt_fragment(
@@ -2656,14 +2090,9 @@ extern int rtllib_wx_set_encode(struct rtllib_device *ieee,
 extern int rtllib_wx_get_encode(struct rtllib_device *ieee,
 				   struct iw_request_info *info,
 				   union iwreq_data *wrqu, char *key);
-#if WIRELESS_EXT >= 18
-extern int rtllib_wx_get_encode_ext(struct rtllib_device *ieee,
-			    struct iw_request_info *info,
-			    union iwreq_data *wrqu, char *extra);
 extern int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 			    struct iw_request_info *info,
 			    union iwreq_data *wrqu, char *extra);
-#endif
 extern int rtllib_wx_set_auth(struct rtllib_device *ieee,
 			       struct iw_request_info *info,
 			       struct iw_param *data, char *extra);
@@ -2674,7 +2103,6 @@ extern int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len);
 
 /* rtllib_softmac.c */
 extern short rtllib_is_54g(struct rtllib_network *net);
-extern short rtllib_is_shortslot(const struct rtllib_network *net);
 extern int rtllib_rx_frame_softmac(struct rtllib_device *ieee,
 				   struct sk_buff *skb,
 				   struct rtllib_rx_stats *rx_stats, u16 type,
@@ -2704,7 +2132,6 @@ 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);
 extern void rtllib_sta_ps_send_pspoll_frame(struct rtllib_device *ieee);
-extern void rtllib_check_all_nets(struct rtllib_device *ieee);
 extern void rtllib_start_protocol(struct rtllib_device *ieee);
 extern void rtllib_stop_protocol(struct rtllib_device *ieee, u8 shutdown);
 
@@ -2724,8 +2151,6 @@ extern void rtllib_softmac_start_protocol(struct rtllib_device *ieee,
 					  u8 mesh_flag);
 
 extern void rtllib_reset_queue(struct rtllib_device *ieee);
-extern void rtllib_wake_queue(struct rtllib_device *ieee);
-extern void rtllib_stop_queue(struct rtllib_device *ieee);
 extern void rtllib_wake_all_queues(struct rtllib_device *ieee);
 extern void rtllib_stop_all_queues(struct rtllib_device *ieee);
 extern struct sk_buff *rtllib_get_beacon(struct rtllib_device *ieee);
@@ -2743,11 +2168,6 @@ extern u16 rtllib_query_seqnum(struct rtllib_device *ieee,
 			       struct sk_buff *skb, u8 *dst);
 extern u8 rtllib_ap_sec_type(struct rtllib_device *ieee);
 
-/* rtllib_crypt_ccmp&tkip&wep.c */
-extern void rtllib_tkip_null(void);
-extern void rtllib_wep_null(void);
-extern void rtllib_ccmp_null(void);
-
 /* rtllib_softmac_wx.c */
 
 extern int rtllib_wx_get_wap(struct rtllib_device *ieee,
@@ -2877,16 +2297,6 @@ void rtllib_softmac_scan_syncro(struct rtllib_device *ieee, u8 is_mesh);
 
 extern const long rtllib_wlan_frequencies[];
 
-static inline void rtllib_increment_scans(struct rtllib_device *ieee)
-{
-	ieee->scans++;
-}
-
-static inline int rtllib_get_scans(struct rtllib_device *ieee)
-{
-	return ieee->scans;
-}
-
 static inline const char *escape_essid(const char *essid, u8 essid_len)
 {
 	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
@@ -2900,10 +2310,6 @@ static inline const char *escape_essid(const char *essid, u8 essid_len)
 	return escaped;
 }
 
-#define CONVERT_RATE(_ieee, _MGN_RATE)			\
-	((_MGN_RATE < MGN_MCS0) ? (_MGN_RATE) :		\
-	(HTMcsToDataRate(_ieee, (u8)_MGN_RATE)))
-
 /* fun with the built-in rtllib stack... */
 bool rtllib_MgntDisconnect(struct rtllib_device *rtllib, u8 asRsn);
 
@@ -2914,7 +2320,6 @@ bool rtllib_MgntDisconnect(struct rtllib_device *rtllib, u8 asRsn);
 extern void rtllib_update_active_chan_map(struct rtllib_device *ieee);
 extern void rtllib_FlushRxTsPendingPkts(struct rtllib_device *ieee,
 					struct rx_ts_record *pTS);
-extern int rtllib_data_xmit(struct sk_buff *skb, struct net_device *dev);
 extern int rtllib_parse_info_param(struct rtllib_device *ieee,
 		struct rtllib_info_element *info_element,
 		u16 length,
@@ -2929,47 +2334,7 @@ extern void HTUseDefaultSetting(struct rtllib_device *ieee);
 #define RT_ASOC_RETRY_LIMIT	5
 u8 MgntQuery_TxRateExcludeCCKRates(struct rtllib_device *ieee);
 extern void rtllib_TURBO_Info(struct rtllib_device *ieee, u8 **tag_p);
-#ifndef ENABLE_LOCK_DEBUG
-#define SPIN_LOCK_IEEE(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_IEEE(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_IEEE_REORDER(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_IEEE_REORDER(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_IEEE_WPAX(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_IEEE_WPAX(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_IEEE_MGNTTX(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_IEEE_MGNTTX(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_IEEE_BCN(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_IEEE_BCN(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_MSH_STAINFO(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_MSH_STAINFO(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_MSH_PREQ(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_MSH_PREQ(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_MSH_QUEUE(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_MSH_QUEUE(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_PRIV_RFPS(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_PRIV_RFPS(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_PRIV_IRQTH(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_PRIV_IRQTH(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_PRIV_TX(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_PRIV_TX(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_PRIV_D3(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_PRIV_D3(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_PRIV_RF(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_PRIV_RF(plock) spin_unlock_irqrestore((plock), flags)
-#define SPIN_LOCK_PRIV_PS(plock) spin_lock_irqsave((plock), flags)
-#define SPIN_UNLOCK_PRIV_PS(plock) spin_unlock_irqrestore((plock), flags)
 #define SEM_DOWN_IEEE_WX(psem) down(psem)
 #define SEM_UP_IEEE_WX(psem) up(psem)
-#define SEM_DOWN_IEEE_SCAN(psem) down(psem)
-#define SEM_UP_IEEE_SCAN(psem) up(psem)
-#define SEM_DOWN_IEEE_IPS(psem) down(psem)
-#define SEM_UP_IEEE_IPS(psem) up(psem)
-#define SEM_DOWN_PRIV_WX(psem) down(psem)
-#define SEM_UP_PRIV_WX(psem) up(psem)
-#define SEM_DOWN_PRIV_RF(psem) down(psem)
-#define SEM_UP_PRIV_RF(psem) up(psem)
-#define MUTEX_LOCK_PRIV(pmutex) mutex_lock(pmutex)
-#define MUTEX_UNLOCK_PRIV(pmutex) mutex_unlock(pmutex)
-#endif
 
 #endif /* RTLLIB_H */
diff --git a/drivers/staging/rtl8192e/rtllib_crypt.c b/drivers/staging/rtl8192e/rtllib_crypt.c
deleted file mode 100644
index 1e6ae9bead23..000000000000
--- a/drivers/staging/rtl8192e/rtllib_crypt.c
+++ /dev/null
@@ -1,254 +0,0 @@
-/*
- * Host AP crypto routines
- *
- * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
- * Portions Copyright (C) 2004, Intel Corporation <jketreno@linux.intel.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation. See README and COPYING for
- * more details.
- *
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/errno.h>
-
-#include "rtllib.h"
-
-struct rtllib_crypto_alg {
-	struct list_head list;
-	struct lib80211_crypto_ops *ops;
-};
-
-
-struct rtllib_crypto {
-	struct list_head algs;
-	spinlock_t lock;
-};
-
-static struct rtllib_crypto *hcrypt;
-
-void rtllib_crypt_deinit_entries(struct lib80211_crypt_info *info,
-					   int force)
-{
-	struct list_head *ptr, *n;
-	struct lib80211_crypt_data *entry;
-
-	for (ptr = info->crypt_deinit_list.next, n = ptr->next;
-	     ptr != &info->crypt_deinit_list; ptr = n, n = ptr->next) {
-		entry = list_entry(ptr, struct lib80211_crypt_data, list);
-
-		if (atomic_read(&entry->refcnt) != 0 && !force)
-			continue;
-
-		list_del(ptr);
-
-		if (entry->ops)
-			entry->ops->deinit(entry->priv);
-		kfree(entry);
-	}
-}
-EXPORT_SYMBOL(rtllib_crypt_deinit_entries);
-
-void rtllib_crypt_deinit_handler(unsigned long data)
-{
-	struct lib80211_crypt_info *info = (struct lib80211_crypt_info *)data;
-	unsigned long flags;
-
-	spin_lock_irqsave(info->lock, flags);
-	rtllib_crypt_deinit_entries(info, 0);
-	if (!list_empty(&info->crypt_deinit_list)) {
-		printk(KERN_DEBUG
-		       "%s: entries remaining in delayed crypt deletion list\n",
-		       info->name);
-		info->crypt_deinit_timer.expires = jiffies + HZ;
-		add_timer(&info->crypt_deinit_timer);
-	}
-	spin_unlock_irqrestore(info->lock, flags);
-
-}
-EXPORT_SYMBOL(rtllib_crypt_deinit_handler);
-
-void rtllib_crypt_delayed_deinit(struct lib80211_crypt_info *info,
-				 struct lib80211_crypt_data **crypt)
-{
-	struct lib80211_crypt_data *tmp;
-	unsigned long flags;
-
-	if (*crypt == NULL)
-		return;
-
-	tmp = *crypt;
-	*crypt = NULL;
-
-	/* must not run ops->deinit() while there may be pending encrypt or
-	 * decrypt operations. Use a list of delayed deinits to avoid needing
-	 * locking.
-	 */
-
-	spin_lock_irqsave(info->lock, flags);
-	list_add(&tmp->list, &info->crypt_deinit_list);
-	if (!timer_pending(&info->crypt_deinit_timer)) {
-		info->crypt_deinit_timer.expires = jiffies + HZ;
-		add_timer(&info->crypt_deinit_timer);
-	}
-	spin_unlock_irqrestore(info->lock, flags);
-}
-EXPORT_SYMBOL(rtllib_crypt_delayed_deinit);
-
-int rtllib_register_crypto_ops(struct lib80211_crypto_ops *ops)
-{
-	unsigned long flags;
-	struct rtllib_crypto_alg *alg;
-
-	if (hcrypt == NULL)
-		return -1;
-
-	alg = kzalloc(sizeof(*alg), GFP_KERNEL);
-	if (alg == NULL)
-		return -ENOMEM;
-
-	alg->ops = ops;
-
-	spin_lock_irqsave(&hcrypt->lock, flags);
-	list_add(&alg->list, &hcrypt->algs);
-	spin_unlock_irqrestore(&hcrypt->lock, flags);
-
-	printk(KERN_DEBUG "rtllib_crypt: registered algorithm '%s'\n",
-	       ops->name);
-
-	return 0;
-}
-EXPORT_SYMBOL(rtllib_register_crypto_ops);
-
-int rtllib_unregister_crypto_ops(struct lib80211_crypto_ops *ops)
-{
-	unsigned long flags;
-	struct list_head *ptr;
-	struct rtllib_crypto_alg *del_alg = NULL;
-
-	if (hcrypt == NULL)
-		return -1;
-
-	spin_lock_irqsave(&hcrypt->lock, flags);
-	for (ptr = hcrypt->algs.next; ptr != &hcrypt->algs; ptr = ptr->next) {
-		struct rtllib_crypto_alg *alg =
-			(struct rtllib_crypto_alg *) ptr;
-		if (alg->ops == ops) {
-			list_del(&alg->list);
-			del_alg = alg;
-			break;
-		}
-	}
-	spin_unlock_irqrestore(&hcrypt->lock, flags);
-
-	if (del_alg) {
-		printk(KERN_DEBUG "rtllib_crypt: unregistered algorithm '%s'\n",
-		       ops->name);
-		kfree(del_alg);
-	}
-
-	return del_alg ? 0 : -1;
-}
-EXPORT_SYMBOL(rtllib_unregister_crypto_ops);
-
-
-struct lib80211_crypto_ops *rtllib_get_crypto_ops(const char *name)
-{
-	unsigned long flags;
-	struct list_head *ptr;
-	struct rtllib_crypto_alg *found_alg = NULL;
-
-	if (hcrypt == NULL)
-		return NULL;
-
-	spin_lock_irqsave(&hcrypt->lock, flags);
-	for (ptr = hcrypt->algs.next; ptr != &hcrypt->algs; ptr = ptr->next) {
-		struct rtllib_crypto_alg *alg =
-			(struct rtllib_crypto_alg *) ptr;
-		if (strcmp(alg->ops->name, name) == 0) {
-			found_alg = alg;
-			break;
-		}
-	}
-	spin_unlock_irqrestore(&hcrypt->lock, flags);
-
-	if (found_alg)
-		return found_alg->ops;
-	else
-		return NULL;
-}
-EXPORT_SYMBOL(rtllib_get_crypto_ops);
-
-
-static void *rtllib_crypt_null_init(int keyidx) { return (void *) 1; }
-static void rtllib_crypt_null_deinit(void *priv) {}
-
-static struct lib80211_crypto_ops rtllib_crypt_null = {
-	.name			= "NULL",
-	.init			= rtllib_crypt_null_init,
-	.deinit			= rtllib_crypt_null_deinit,
-	.encrypt_mpdu		= NULL,
-	.decrypt_mpdu		= NULL,
-	.encrypt_msdu		= NULL,
-	.decrypt_msdu		= NULL,
-	.set_key		= NULL,
-	.get_key		= NULL,
-	.extra_mpdu_prefix_len	= 0,
-	.extra_mpdu_postfix_len	= 0,
-	.extra_msdu_prefix_len	= 0,
-	.extra_msdu_postfix_len	= 0,
-	.owner			= THIS_MODULE,
-};
-
-
-int __init rtllib_crypto_init(void)
-{
-	int ret = -ENOMEM;
-
-	hcrypt = kzalloc(sizeof(*hcrypt), GFP_KERNEL);
-	if (!hcrypt)
-		goto out;
-
-	INIT_LIST_HEAD(&hcrypt->algs);
-	spin_lock_init(&hcrypt->lock);
-
-	ret = lib80211_register_crypto_ops(&rtllib_crypt_null);
-	if (ret < 0) {
-		kfree(hcrypt);
-		hcrypt = NULL;
-	}
-out:
-	return ret;
-}
-
-
-void __exit rtllib_crypto_deinit(void)
-{
-	struct list_head *ptr, *n;
-
-	if (hcrypt == NULL)
-		return;
-
-	for (ptr = hcrypt->algs.next, n = ptr->next; ptr != &hcrypt->algs;
-	     ptr = n, n = ptr->next) {
-		struct rtllib_crypto_alg *alg =
-			(struct rtllib_crypto_alg *) ptr;
-		list_del(ptr);
-		printk(KERN_DEBUG
-		       "rtllib_crypt: unregistered algorithm '%s' (deinit)\n",
-		       alg->ops->name);
-		kfree(alg);
-	}
-
-	kfree(hcrypt);
-}
-
-module_init(rtllib_crypto_init);
-module_exit(rtllib_crypto_deinit);
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/rtl8192e/rtllib_crypt.h b/drivers/staging/rtl8192e/rtllib_crypt.h
deleted file mode 100644
index b8cf59f39a60..000000000000
--- a/drivers/staging/rtl8192e/rtllib_crypt.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*
- * Original code based on Host AP (software wireless LAN access point) driver
- * for Intersil Prism2/2.5/3.
- *
- * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
- * <jkmaline@cc.hut.fi>
- * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
- *
- * Adaption to a generic IEEE 802.11 stack by James Ketrenos
- * <jketreno@linux.intel.com>
- *
- * Copyright (c) 2004, Intel Corporation
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation. See README and COPYING for
- * more details.
- */
-
-/* This file defines the interface to the rtllib crypto module.
- */
-#ifndef RTLLIB_CRYPT_H
-#define RTLLIB_CRYPT_H
-
-#include <linux/skbuff.h>
-
-int rtllib_register_crypto_ops(struct lib80211_crypto_ops *ops);
-int rtllib_unregister_crypto_ops(struct lib80211_crypto_ops *ops);
-struct lib80211_crypto_ops *rtllib_get_crypto_ops(const char *name);
-void rtllib_crypt_deinit_entries(struct lib80211_crypt_info *info, int force);
-void rtllib_crypt_deinit_handler(unsigned long data);
-void rtllib_crypt_delayed_deinit(struct lib80211_crypt_info *info,
-				 struct lib80211_crypt_data **crypt);
-#endif
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
index 7d486e8887f8..496de4f6a7bc 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
@@ -69,7 +69,7 @@ static void *rtllib_ccmp_init(int key_idx)
 
 	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tfm)) {
-		pr_debug("rtllib_crypt_ccmp: could not allocate crypto API aes\n");
+		pr_debug("Could not allocate crypto API aes\n");
 		priv->tfm = NULL;
 		goto fail;
 	}
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
index 656b4b359c50..2096d78913bd 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
@@ -21,6 +21,7 @@
 #include <linux/crypto.h>
 #include <linux/scatterlist.h>
 #include <linux/crc32.h>
+#include <linux/etherdevice.h>
 
 #include "rtllib.h"
 
@@ -52,7 +53,8 @@ struct rtllib_tkip_data {
 	struct crypto_blkcipher *tx_tfm_arc4;
 	struct crypto_hash *tx_tfm_michael;
 	/* scratch buffers for virt_to_page() (crypto API) */
-	u8 rx_hdr[16], tx_hdr[16];
+	u8 rx_hdr[16];
+	u8 tx_hdr[16];
 };
 
 static void *rtllib_tkip_init(int key_idx)
@@ -66,8 +68,7 @@ static void *rtllib_tkip_init(int key_idx)
 	priv->tx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
 			CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tx_tfm_arc4)) {
-		printk(KERN_DEBUG
-		       "rtllib_crypt_tkip: could not allocate crypto API arc4\n");
+		pr_debug("Could not allocate crypto API arc4\n");
 		priv->tx_tfm_arc4 = NULL;
 		goto fail;
 	}
@@ -75,8 +76,7 @@ static void *rtllib_tkip_init(int key_idx)
 	priv->tx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
 			CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tx_tfm_michael)) {
-		printk(KERN_DEBUG
-		       "rtllib_crypt_tkip: could not allocate crypto API michael_mic\n");
+		pr_debug("Could not allocate crypto API michael_mic\n");
 		priv->tx_tfm_michael = NULL;
 		goto fail;
 	}
@@ -84,8 +84,7 @@ static void *rtllib_tkip_init(int key_idx)
 	priv->rx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
 			CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->rx_tfm_arc4)) {
-		printk(KERN_DEBUG
-		       "rtllib_crypt_tkip: could not allocate crypto API arc4\n");
+		pr_debug("Could not allocate crypto API arc4\n");
 		priv->rx_tfm_arc4 = NULL;
 		goto fail;
 	}
@@ -93,8 +92,7 @@ static void *rtllib_tkip_init(int key_idx)
 	priv->rx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
 			CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->rx_tfm_michael)) {
-		printk(KERN_DEBUG
-		       "rtllib_crypt_tkip: could not allocate crypto API michael_mic\n");
+		pr_debug("Could not allocate crypto API michael_mic\n");
 		priv->rx_tfm_michael = NULL;
 		goto fail;
 	}
@@ -401,24 +399,24 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	keyidx = pos[3];
 	if (!(keyidx & (1 << 5))) {
 		if (net_ratelimit()) {
-			printk(KERN_DEBUG
-			       "TKIP: received packet without ExtIV flag from %pM\n",
-			       hdr->addr2);
+			netdev_dbg(skb->dev,
+				   "Received packet without ExtIV flag from %pM\n",
+				   hdr->addr2);
 		}
 		return -2;
 	}
 	keyidx >>= 6;
 	if (tkey->key_idx != keyidx) {
-		printk(KERN_DEBUG
-		       "TKIP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
-		       tkey->key_idx, keyidx, priv);
+		netdev_dbg(skb->dev,
+			   "RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
+			   tkey->key_idx, keyidx, priv);
 		return -6;
 	}
 	if (!tkey->key_set) {
 		if (net_ratelimit()) {
-			printk(KERN_DEBUG
-			       "TKIP: received packet from %pM with keyid=%d that does not have a configured key\n",
-			       hdr->addr2, keyidx);
+			netdev_dbg(skb->dev,
+				   "Received packet from %pM with keyid=%d that does not have a configured key\n",
+				   hdr->addr2, keyidx);
 		}
 		return -3;
 	}
@@ -431,10 +429,10 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 		    (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) &&
 		    tkey->initialized) {
 			if (net_ratelimit()) {
-				printk(KERN_DEBUG
-				       "TKIP: replay detected: STA= %pM previous TSC %08x%04x received TSC %08x%04x\n",
-				       hdr->addr2, tkey->rx_iv32, tkey->rx_iv16,
-				       iv32, iv16);
+				netdev_dbg(skb->dev,
+					   "Replay detected: STA= %pM previous TSC %08x%04x received TSC %08x%04x\n",
+					   hdr->addr2, tkey->rx_iv32,
+					   tkey->rx_iv16, iv32, iv16);
 			}
 			tkey->dot11RSNAStatsTKIPReplays++;
 			return -4;
@@ -455,9 +453,9 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 		crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
 		if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
 			if (net_ratelimit()) {
-				printk(KERN_DEBUG
-				       ": TKIP: failed to decrypt received packet from %pM\n",
-				       hdr->addr2);
+				netdev_dbg(skb->dev,
+					   "Failed to decrypt received packet from %pM\n",
+					   hdr->addr2);
 			}
 			return -7;
 		}
@@ -477,9 +475,9 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 				tkey->rx_phase1_done = 0;
 			}
 			if (net_ratelimit()) {
-				printk(KERN_DEBUG
-				       "TKIP: ICV error detected: STA= %pM\n",
-				       hdr->addr2);
+				netdev_dbg(skb->dev,
+					   "ICV error detected: STA= %pM\n",
+					   hdr->addr2);
 			}
 			tkey->dot11RSNAStatsTKIPICVErrors++;
 			return -5;
@@ -532,20 +530,20 @@ static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
 	switch (le16_to_cpu(hdr11->frame_ctl) &
 		(RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS)) {
 	case RTLLIB_FCTL_TODS:
-		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
-		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
+		ether_addr_copy(hdr, hdr11->addr3); /* DA */
+		ether_addr_copy(hdr + ETH_ALEN, hdr11->addr2); /* SA */
 		break;
 	case RTLLIB_FCTL_FROMDS:
-		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
-		memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
+		ether_addr_copy(hdr, hdr11->addr1); /* DA */
+		ether_addr_copy(hdr + ETH_ALEN, hdr11->addr3); /* SA */
 		break;
 	case RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS:
-		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
-		memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
+		ether_addr_copy(hdr, hdr11->addr3); /* DA */
+		ether_addr_copy(hdr + ETH_ALEN, hdr11->addr4); /* SA */
 		break;
 	case 0:
-		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
-		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
+		ether_addr_copy(hdr, hdr11->addr1); /* DA */
+		ether_addr_copy(hdr + ETH_ALEN, hdr11->addr2); /* SA */
 		break;
 	}
 
@@ -564,9 +562,9 @@ static int rtllib_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
 	hdr = (struct rtllib_hdr_4addr *) skb->data;
 
 	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
-		printk(KERN_DEBUG
-		       "Invalid packet for Michael MIC add (tailroom=%d hdr_len=%d skb->len=%d)\n",
-		       skb_tailroom(skb), hdr_len, skb->len);
+		netdev_dbg(skb->dev,
+			   "Invalid packet for Michael MIC add (tailroom=%d hdr_len=%d skb->len=%d)\n",
+			   skb_tailroom(skb), hdr_len, skb->len);
 		return -1;
 	}
 
@@ -598,7 +596,7 @@ static void rtllib_michael_mic_failure(struct net_device *dev,
 	else
 		ev.flags |= IW_MICFAILURE_PAIRWISE;
 	ev.src_addr.sa_family = ARPHRD_ETHER;
-	memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
+	ether_addr_copy(ev.src_addr.sa_data, hdr->addr2);
 	memset(&wrqu, 0, sizeof(wrqu));
 	wrqu.data.length = sizeof(ev);
 	wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *) &ev);
@@ -628,12 +626,11 @@ static int rtllib_michael_mic_verify(struct sk_buff *skb, int keyidx,
 		struct rtllib_hdr_4addr *hdr;
 
 		hdr = (struct rtllib_hdr_4addr *) skb->data;
-		printk(KERN_DEBUG
-		       "%s: Michael MIC verification failed for MSDU from %pM keyidx=%d\n",
-		       skb->dev ? skb->dev->name : "N/A", hdr->addr2,
-		       keyidx);
-		printk(KERN_DEBUG "%d\n",
-		       memcmp(mic, skb->data + skb->len - 8, 8) != 0);
+		netdev_dbg(skb->dev,
+			   "Michael MIC verification failed for MSDU from %pM keyidx=%d\n",
+			   hdr->addr2, keyidx);
+		netdev_dbg(skb->dev, "%d\n",
+			   memcmp(mic, skb->data + skb->len - 8, 8) != 0);
 		if (skb->dev) {
 			pr_info("skb->dev != NULL\n");
 			rtllib_michael_mic_failure(skb->dev, hdr, keyidx);
diff --git a/drivers/staging/rtl8192e/rtllib_debug.h b/drivers/staging/rtl8192e/rtllib_debug.h
index 119729d31c74..42e88d69ae63 100644
--- a/drivers/staging/rtl8192e/rtllib_debug.h
+++ b/drivers/staging/rtl8192e/rtllib_debug.h
@@ -73,16 +73,7 @@ enum RTL_DEBUG {
 #define RT_TRACE(component, x, args...)		\
 do {			\
 	if (rt_global_debug_component & component) \
-		printk(KERN_DEBUG DRV_NAME ":" x "\n" , \
-		       ##args);\
-} while (0)
-
-#define assert(expr) \
-do {	\
-	if (!(expr)) {				  \
-		pr_info("Assertion failed! %s,%s,%s,line=%d\n", \
-		#expr, __FILE__, __func__, __LINE__);	  \
-	}	\
+		printk(KERN_DEBUG DRV_NAME ":" x "\n", ##args);\
 } while (0)
 
 #endif
diff --git a/drivers/staging/rtl8192e/rtllib_module.c b/drivers/staging/rtl8192e/rtllib_module.c
index 32cc8df9d3a7..224dc99af131 100644
--- a/drivers/staging/rtl8192e/rtllib_module.c
+++ b/drivers/staging/rtl8192e/rtllib_module.c
@@ -57,12 +57,6 @@ u32 rt_global_debug_component = COMP_ERR;
 EXPORT_SYMBOL(rt_global_debug_component);
 
 
-void _setup_timer(struct timer_list *ptimer, void *fun, unsigned long data)
-{
-	ptimer->function = fun;
-	ptimer->data = data;
-	init_timer(ptimer);
-}
 
 static inline int rtllib_networks_allocate(struct rtllib_device *ieee)
 {
@@ -103,11 +97,11 @@ struct net_device *alloc_rtllib(int sizeof_priv)
 	struct net_device *dev;
 	int i, err;
 
-	RTLLIB_DEBUG_INFO("Initializing...\n");
+	pr_debug("rtllib: Initializing...\n");
 
 	dev = alloc_etherdev(sizeof(struct rtllib_device) + sizeof_priv);
 	if (!dev) {
-		RTLLIB_ERROR("Unable to network device.\n");
+		pr_err("Unable to allocate net_device.\n");
 		return NULL;
 	}
 	ieee = (struct rtllib_device *)netdev_priv_rsl(dev);
@@ -116,8 +110,7 @@ struct net_device *alloc_rtllib(int sizeof_priv)
 
 	err = rtllib_networks_allocate(ieee);
 	if (err) {
-		RTLLIB_ERROR("Unable to allocate beacon storage: %d\n",
-				err);
+		pr_err("Unable to allocate beacon storage: %d\n", err);
 		goto failed;
 	}
 	rtllib_networks_initialize(ieee);
@@ -137,15 +130,12 @@ struct net_device *alloc_rtllib(int sizeof_priv)
 
 	spin_lock_init(&ieee->lock);
 	spin_lock_init(&ieee->wpax_suitlist_lock);
-	spin_lock_init(&ieee->bw_spinlock);
 	spin_lock_init(&ieee->reorder_spinlock);
-	atomic_set(&(ieee->atm_chnlop), 0);
 	atomic_set(&(ieee->atm_swbw), 0);
 
 	/* SAM FIXME */
 	lib80211_crypt_info_init(&ieee->crypt_info, "RTLLIB", &ieee->lock);
 
-	ieee->bHalfNMode = false;
 	ieee->wpa_enabled = 0;
 	ieee->tkip_countermeasures = 0;
 	ieee->drop_unencrypted = 0;
@@ -197,69 +187,13 @@ void free_rtllib(struct net_device *dev)
 }
 EXPORT_SYMBOL(free_rtllib);
 
-u32 rtllib_debug_level;
-static int debug = RTLLIB_DL_ERR;
-static struct proc_dir_entry *rtllib_proc;
-
-static int show_debug_level(struct seq_file *m, void *v)
-{
-	seq_printf(m, "0x%08X\n", rtllib_debug_level);
-
-	return 0;
-}
-
-static ssize_t write_debug_level(struct file *file, const char __user *buffer,
-			     size_t count, loff_t *ppos)
-{
-	unsigned long val;
-	int err = kstrtoul_from_user(buffer, count, 0, &val);
-
-	if (err)
-		return err;
-	rtllib_debug_level = val;
-	return count;
-}
-
-static int open_debug_level(struct inode *inode, struct file *file)
-{
-	return single_open(file, show_debug_level, NULL);
-}
-
-static const struct file_operations fops = {
-	.open = open_debug_level,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.write = write_debug_level,
-	.release = single_release,
-};
-
 static int __init rtllib_init(void)
 {
-	struct proc_dir_entry *e;
-
-	rtllib_debug_level = debug;
-	rtllib_proc = proc_mkdir(DRV_NAME, init_net.proc_net);
-	if (rtllib_proc == NULL) {
-		RTLLIB_ERROR("Unable to create " DRV_NAME
-				" proc directory\n");
-		return -EIO;
-	}
-	e = proc_create("debug_level", S_IRUGO | S_IWUSR, rtllib_proc, &fops);
-	if (!e) {
-		remove_proc_entry(DRV_NAME, init_net.proc_net);
-		rtllib_proc = NULL;
-		return -EIO;
-	}
 	return 0;
 }
 
 static void __exit rtllib_exit(void)
 {
-	if (rtllib_proc) {
-		remove_proc_entry("debug_level", rtllib_proc);
-		remove_proc_entry(DRV_NAME, init_net.proc_net);
-		rtllib_proc = NULL;
-	}
 }
 
 module_init(rtllib_init);
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index fe3e7e1273ff..da862c3da4ce 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -45,8 +45,9 @@
 #include "dot11d.h"
 
 static inline void rtllib_monitor_rx(struct rtllib_device *ieee,
-				struct sk_buff *skb, struct rtllib_rx_stats *rx_status,
-				size_t hdr_length)
+				     struct sk_buff *skb,
+				     struct rtllib_rx_stats *rx_status,
+				     size_t hdr_length)
 {
 	skb->dev = ieee->dev;
 	skb_reset_mac_header(skb);
@@ -69,9 +70,9 @@ rtllib_frag_cache_find(struct rtllib_device *ieee, unsigned int seq,
 		entry = &ieee->frag_cache[tid][i];
 		if (entry->skb != NULL &&
 		    time_after(jiffies, entry->first_frag_time + 2 * HZ)) {
-			RTLLIB_DEBUG_FRAG(
-				"expiring fragment cache entry seq=%u last_frag=%u\n",
-				entry->seq, entry->last_frag);
+			netdev_dbg(ieee->dev,
+				   "expiring fragment cache entry seq=%u last_frag=%u\n",
+				   entry->seq, entry->last_frag);
 			dev_kfree_skb_any(entry->skb);
 			entry->skb = NULL;
 		}
@@ -101,7 +102,8 @@ rtllib_frag_cache_get(struct rtllib_device *ieee,
 	struct rtllib_hdr_4addrqos *hdr_4addrqos;
 	u8 tid;
 
-	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) && RTLLIB_QOS_HAS_SEQ(fc)) {
+	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) &&
+	    RTLLIB_QOS_HAS_SEQ(fc)) {
 		hdr_4addrqos = (struct rtllib_hdr_4addrqos *)hdr;
 		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & RTLLIB_QCTL_TID;
 		tid = UP2AC(tid);
@@ -123,7 +125,8 @@ rtllib_frag_cache_get(struct rtllib_device *ieee,
 				    2 /* alignment */ +
 				    8 /* WEP */ +
 				    ETH_ALEN /* WDS */ +
-				    (RTLLIB_QOS_HAS_SEQ(fc) ? 2 : 0) /* QOS Control */);
+				    /* QOS Control */
+				    (RTLLIB_QOS_HAS_SEQ(fc) ? 2 : 0));
 		if (skb == NULL)
 			return NULL;
 
@@ -139,8 +142,8 @@ rtllib_frag_cache_get(struct rtllib_device *ieee,
 		entry->seq = seq;
 		entry->last_frag = frag;
 		entry->skb = skb;
-		memcpy(entry->src_addr, hdr->addr2, ETH_ALEN);
-		memcpy(entry->dst_addr, hdr->addr1, ETH_ALEN);
+		ether_addr_copy(entry->src_addr, hdr->addr2);
+		ether_addr_copy(entry->dst_addr, hdr->addr1);
 	} else {
 		/* received a fragment of a frame for which the head fragment
 		 * should have already been received
@@ -169,7 +172,8 @@ static int rtllib_frag_cache_invalidate(struct rtllib_device *ieee,
 	struct rtllib_hdr_4addrqos *hdr_4addrqos;
 	u8 tid;
 
-	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) && RTLLIB_QOS_HAS_SEQ(fc)) {
+	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) &&
+	    RTLLIB_QOS_HAS_SEQ(fc)) {
 		hdr_4addrqos = (struct rtllib_hdr_4addrqos *)hdr;
 		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & RTLLIB_QCTL_TID;
 		tid = UP2AC(tid);
@@ -187,8 +191,9 @@ static int rtllib_frag_cache_invalidate(struct rtllib_device *ieee,
 					  hdr->addr1);
 
 	if (entry == NULL) {
-		RTLLIB_DEBUG_FRAG(
-			"could not invalidate fragment cache entry (seq=%u)\n", seq);
+		netdev_dbg(ieee->dev,
+			   "Couldn't invalidate fragment cache entry (seq=%u)\n",
+			   seq);
 		return -1;
 	}
 
@@ -290,7 +295,8 @@ rtllib_rx_frame_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 		return 0;
 
 	if (ieee->hwsec_active) {
-		struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+		struct cb_desc *tcb_desc = (struct cb_desc *)
+						(skb->cb + MAX_DEV_ADDR_SIZE);
 
 		tcb_desc->bHwSec = 1;
 
@@ -305,11 +311,12 @@ rtllib_rx_frame_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 	res = crypt->ops->decrypt_mpdu(skb, hdrlen, crypt->priv);
 	atomic_dec(&crypt->refcnt);
 	if (res < 0) {
-		RTLLIB_DEBUG_DROP(
-			"decryption failed (SA= %pM) res=%d\n", hdr->addr2, res);
+		netdev_dbg(ieee->dev, "decryption failed (SA= %pM) res=%d\n",
+			   hdr->addr2, res);
 		if (res == -2)
-			RTLLIB_DEBUG_DROP("Decryption failed ICV mismatch (key %d)\n",
-					     skb->data[hdrlen + 3] >> 6);
+			netdev_dbg(ieee->dev,
+				   "Decryption failed ICV mismatch (key %d)\n",
+				   skb->data[hdrlen + 3] >> 6);
 		ieee->ieee_stats.rx_discards_undecryptable++;
 		return -1;
 	}
@@ -329,7 +336,8 @@ rtllib_rx_frame_decrypt_msdu(struct rtllib_device *ieee, struct sk_buff *skb,
 	if (crypt == NULL || crypt->ops->decrypt_msdu == NULL)
 		return 0;
 	if (ieee->hwsec_active) {
-		struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+		struct cb_desc *tcb_desc = (struct cb_desc *)
+						(skb->cb + MAX_DEV_ADDR_SIZE);
 
 		tcb_desc->bHwSec = 1;
 
@@ -344,8 +352,9 @@ rtllib_rx_frame_decrypt_msdu(struct rtllib_device *ieee, struct sk_buff *skb,
 	res = crypt->ops->decrypt_msdu(skb, keyidx, hdrlen, crypt->priv);
 	atomic_dec(&crypt->refcnt);
 	if (res < 0) {
-		printk(KERN_DEBUG "%s: MSDU decryption/MIC verification failed (SA= %pM keyidx=%d)\n",
-		       ieee->dev->name, hdr->addr2, keyidx);
+		netdev_dbg(ieee->dev,
+			   "MSDU decryption/MIC verification failed (SA= %pM keyidx=%d)\n",
+			   hdr->addr2, keyidx);
 		return -1;
 	}
 
@@ -368,7 +377,8 @@ static int is_duplicate_packet(struct rtllib_device *ieee,
 	struct rtllib_hdr_4addrqos *hdr_4addrqos;
 	u8 tid;
 
-	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) && RTLLIB_QOS_HAS_SEQ(fc)) {
+	if (((fc & RTLLIB_FCTL_DSTODS) == RTLLIB_FCTL_DSTODS) &&
+	    RTLLIB_QOS_HAS_SEQ(fc)) {
 		hdr_4addrqos = (struct rtllib_hdr_4addrqos *)header;
 		tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & RTLLIB_QCTL_TID;
 		tid = UP2AC(tid);
@@ -396,11 +406,12 @@ static int is_duplicate_packet(struct rtllib_device *ieee,
 				break;
 		}
 		if (p == &ieee->ibss_mac_hash[index]) {
-			entry = kmalloc(sizeof(struct ieee_ibss_seq), GFP_ATOMIC);
+			entry = kmalloc(sizeof(struct ieee_ibss_seq),
+					GFP_ATOMIC);
 			if (!entry)
 				return 0;
 
-			memcpy(entry->mac, mac, ETH_ALEN);
+			ether_addr_copy(entry->mac, mac);
 			entry->seq_num[tid] = seq;
 			entry->frag_num[tid] = frag;
 			entry->packet_time[tid] = jiffies;
@@ -466,10 +477,11 @@ static bool AddReorderEntry(struct rx_ts_record *pTS,
 	return true;
 }
 
-void rtllib_indicate_packets(struct rtllib_device *ieee, struct rtllib_rxb **prxbIndicateArray, u8 index)
+void rtllib_indicate_packets(struct rtllib_device *ieee,
+			     struct rtllib_rxb **prxbIndicateArray, u8 index)
 {
 	struct net_device_stats *stats = &ieee->stats;
-	u8 i = 0 , j = 0;
+	u8 i = 0, j = 0;
 	u16 ethertype;
 
 	for (j = 0; j < index; j++) {
@@ -481,9 +493,12 @@ void rtllib_indicate_packets(struct rtllib_device *ieee, struct rtllib_rxb **prx
 		/* convert hdr + possible LLC headers into Ethernet header */
 			ethertype = (sub_skb->data[6] << 8) | sub_skb->data[7];
 			if (sub_skb->len >= 8 &&
-			    ((memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) == 0 &&
-			    ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
-			    memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE) == 0)) {
+			    ((memcmp(sub_skb->data, rfc1042_header,
+				     SNAP_SIZE) == 0 &&
+			      ethertype != ETH_P_AARP &&
+			      ethertype != ETH_P_IPX) ||
+			    memcmp(sub_skb->data, bridge_tunnel_header,
+				   SNAP_SIZE) == 0)) {
 				/* remove RFC1042 or Bridge-Tunnel encapsulation
 				 * and replace EtherType
 				 */
@@ -505,11 +520,13 @@ void rtllib_indicate_packets(struct rtllib_device *ieee, struct rtllib_rxb **prx
 				stats->rx_bytes += sub_skb->len;
 
 				memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
-				sub_skb->protocol = eth_type_trans(sub_skb, ieee->dev);
+				sub_skb->protocol = eth_type_trans(sub_skb,
+								   ieee->dev);
 				sub_skb->dev = ieee->dev;
 				sub_skb->dev->stats.rx_packets++;
 				sub_skb->dev->stats.rx_bytes += sub_skb->len;
-				sub_skb->ip_summed = CHECKSUM_NONE; /* 802.11 crc not sufficient */
+				/* 802.11 crc not sufficient */
+				sub_skb->ip_summed = CHECKSUM_NONE;
 				ieee->last_rx_ps_time = jiffies;
 				netif_rx(sub_skb);
 			}
@@ -519,7 +536,8 @@ void rtllib_indicate_packets(struct rtllib_device *ieee, struct rtllib_rxb **prx
 	}
 }
 
-void rtllib_FlushRxTsPendingPkts(struct rtllib_device *ieee,	struct rx_ts_record *pTS)
+void rtllib_FlushRxTsPendingPkts(struct rtllib_device *ieee,
+				 struct rx_ts_record *pTS)
 {
 	struct rx_reorder_entry *pRxReorderEntry;
 	u8 RfdCnt = 0;
@@ -533,14 +551,18 @@ void rtllib_FlushRxTsPendingPkts(struct rtllib_device *ieee,	struct rx_ts_record
 			break;
 		}
 
-		pRxReorderEntry = (struct rx_reorder_entry *)list_entry(pTS->RxPendingPktList.prev, struct rx_reorder_entry, List);
-		RTLLIB_DEBUG(RTLLIB_DL_REORDER, "%s(): Indicate SeqNum %d!\n", __func__, pRxReorderEntry->SeqNum);
+		pRxReorderEntry = (struct rx_reorder_entry *)
+				  list_entry(pTS->RxPendingPktList.prev,
+					     struct rx_reorder_entry, List);
+		netdev_dbg(ieee->dev, "%s(): Indicate SeqNum %d!\n", __func__,
+			   pRxReorderEntry->SeqNum);
 		list_del_init(&pRxReorderEntry->List);
 
 		ieee->RfdArray[RfdCnt] = pRxReorderEntry->prxb;
 
 		RfdCnt = RfdCnt + 1;
-		list_add_tail(&pRxReorderEntry->List, &ieee->RxReorder_Unused_List);
+		list_add_tail(&pRxReorderEntry->List,
+			      &ieee->RxReorder_Unused_List);
 	}
 	rtllib_indicate_packets(ieee, ieee->RfdArray, RfdCnt);
 
@@ -559,8 +581,9 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 	bool bMatchWinStart = false, bPktInBuf = false;
 	unsigned long flags;
 
-	RTLLIB_DEBUG(RTLLIB_DL_REORDER, "%s(): Seq is %d, pTS->RxIndicateSeq is %d, WinSize is %d\n", __func__, SeqNum,
-		     pTS->RxIndicateSeq, WinSize);
+	netdev_dbg(ieee->dev,
+		   "%s(): Seq is %d, pTS->RxIndicateSeq is %d, WinSize is %d\n",
+		   __func__, SeqNum, pTS->RxIndicateSeq, WinSize);
 
 	spin_lock_irqsave(&(ieee->reorder_spinlock), flags);
 
@@ -571,8 +594,9 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 
 	/* Drop out the packet which SeqNum is smaller than WinStart */
 	if (SN_LESS(SeqNum, pTS->RxIndicateSeq)) {
-		RTLLIB_DEBUG(RTLLIB_DL_REORDER, "Packet Drop! IndicateSeq: %d, NewSeq: %d\n",
-				 pTS->RxIndicateSeq, SeqNum);
+		netdev_dbg(ieee->dev,
+			   "Packet Drop! IndicateSeq: %d, NewSeq: %d\n",
+			   pTS->RxIndicateSeq, SeqNum);
 		pHTInfo->RxReorderDropCounter++;
 		{
 			int i;
@@ -597,8 +621,11 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 		if (SeqNum >= (WinSize - 1))
 			pTS->RxIndicateSeq = SeqNum + 1 - WinSize;
 		else
-			pTS->RxIndicateSeq = 4095 - (WinSize - (SeqNum + 1)) + 1;
-		RTLLIB_DEBUG(RTLLIB_DL_REORDER, "Window Shift! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum);
+			pTS->RxIndicateSeq = 4095 -
+					     (WinSize - (SeqNum + 1)) + 1;
+		netdev_dbg(ieee->dev,
+			   "Window Shift! IndicateSeq: %d, NewSeq: %d\n",
+			   pTS->RxIndicateSeq, SeqNum);
 	}
 
 	/* Indication process.
@@ -613,8 +640,9 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 	 */
 	if (bMatchWinStart) {
 		/* Current packet is going to be indicated.*/
-		RTLLIB_DEBUG(RTLLIB_DL_REORDER, "Packets indication!! IndicateSeq: %d, NewSeq: %d\n",
-				pTS->RxIndicateSeq, SeqNum);
+		netdev_dbg(ieee->dev,
+			   "Packets indication! IndicateSeq: %d, NewSeq: %d\n",
+			   pTS->RxIndicateSeq, SeqNum);
 		ieee->prxbIndicateArray[0] = prxb;
 		index = 1;
 	} else {
@@ -625,28 +653,30 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 					struct rx_reorder_entry, List);
 			list_del_init(&pReorderEntry->List);
 
-			/* Make a reorder entry and insert into a the packet list.*/
+			/* Make a reorder entry and insert
+			 * into a the packet list.
+			 */
 			pReorderEntry->SeqNum = SeqNum;
 			pReorderEntry->prxb = prxb;
 
 			if (!AddReorderEntry(pTS, pReorderEntry)) {
-				RTLLIB_DEBUG(RTLLIB_DL_REORDER,
-					     "%s(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n",
-					    __func__, pTS->RxIndicateSeq,
-					    SeqNum);
+				int i;
+
+				netdev_dbg(ieee->dev,
+					   "%s(): Duplicate packet is dropped. IndicateSeq: %d, NewSeq: %d\n",
+					   __func__, pTS->RxIndicateSeq,
+					   SeqNum);
 				list_add_tail(&pReorderEntry->List,
-					      &ieee->RxReorder_Unused_List); {
-					int i;
+					      &ieee->RxReorder_Unused_List);
 
-					for (i = 0; i < prxb->nr_subframes; i++)
-						dev_kfree_skb(prxb->subframes[i]);
-					kfree(prxb);
-					prxb = NULL;
-				}
+				for (i = 0; i < prxb->nr_subframes; i++)
+					dev_kfree_skb(prxb->subframes[i]);
+				kfree(prxb);
+				prxb = NULL;
 			} else {
-				RTLLIB_DEBUG(RTLLIB_DL_REORDER,
-					 "Pkt insert into struct buffer!! IndicateSeq: %d, NewSeq: %d\n",
-					 pTS->RxIndicateSeq, SeqNum);
+				netdev_dbg(ieee->dev,
+					   "Pkt insert into struct buffer. IndicateSeq: %d, NewSeq: %d\n",
+					   pTS->RxIndicateSeq, SeqNum);
 			}
 		} else {
 			/* Packets are dropped if there are not enough reorder
@@ -654,7 +684,9 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 			 * indicate all the packets in struct buffer and get
 			 * reorder entries.
 			 */
-			RTLLIB_DEBUG(RTLLIB_DL_ERR, "RxReorderIndicatePacket(): There is no reorder entry!! Packet is dropped!!\n");
+			netdev_err(ieee->dev,
+				   "%s(): There is no reorder entry! Packet is dropped!\n",
+				   __func__);
 			{
 				int i;
 
@@ -668,15 +700,20 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 
 	/* Check if there is any packet need indicate.*/
 	while (!list_empty(&pTS->RxPendingPktList)) {
-		RTLLIB_DEBUG(RTLLIB_DL_REORDER, "%s(): start RREORDER indicate\n", __func__);
+		netdev_dbg(ieee->dev, "%s(): start RREORDER indicate\n",
+			   __func__);
 
-		pReorderEntry = (struct rx_reorder_entry *)list_entry(pTS->RxPendingPktList.prev,
-				 struct rx_reorder_entry, List);
+		pReorderEntry = (struct rx_reorder_entry *)
+					list_entry(pTS->RxPendingPktList.prev,
+						   struct rx_reorder_entry,
+						   List);
 		if (SN_LESS(pReorderEntry->SeqNum, pTS->RxIndicateSeq) ||
-				SN_EQUAL(pReorderEntry->SeqNum, pTS->RxIndicateSeq)) {
+		    SN_EQUAL(pReorderEntry->SeqNum, pTS->RxIndicateSeq)) {
 			/* This protect struct buffer from overflow. */
 			if (index >= REORDER_WIN_SIZE) {
-				RTLLIB_DEBUG(RTLLIB_DL_ERR, "RxReorderIndicatePacket(): Buffer overflow!!\n");
+				netdev_err(ieee->dev,
+					   "%s(): Buffer overflow!\n",
+					   __func__);
 				bPktInBuf = true;
 				break;
 			}
@@ -684,10 +721,12 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 			list_del_init(&pReorderEntry->List);
 
 			if (SN_EQUAL(pReorderEntry->SeqNum, pTS->RxIndicateSeq))
-				pTS->RxIndicateSeq = (pTS->RxIndicateSeq + 1) % 4096;
+				pTS->RxIndicateSeq = (pTS->RxIndicateSeq + 1) %
+						     4096;
 
 			ieee->prxbIndicateArray[index] = pReorderEntry->prxb;
-			RTLLIB_DEBUG(RTLLIB_DL_REORDER, "%s(): Indicate SeqNum %d!\n", __func__, pReorderEntry->SeqNum);
+			netdev_dbg(ieee->dev, "%s(): Indicate SeqNum %d!\n",
+				   __func__, pReorderEntry->SeqNum);
 			index++;
 
 			list_add_tail(&pReorderEntry->List,
@@ -707,7 +746,9 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 		pTS->RxTimeoutIndicateSeq = 0xffff;
 
 		if (index > REORDER_WIN_SIZE) {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR, "RxReorderIndicatePacket(): Rx Reorder struct buffer full!!\n");
+			netdev_err(ieee->dev,
+				   "%s(): Rx Reorder struct buffer full!\n",
+				   __func__);
 			spin_unlock_irqrestore(&(ieee->reorder_spinlock),
 					       flags);
 			return;
@@ -717,8 +758,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 	}
 
 	if (bPktInBuf && pTS->RxTimeoutIndicateSeq == 0xffff) {
-		RTLLIB_DEBUG(RTLLIB_DL_REORDER, "%s(): SET rx timeout timer\n",
-			     __func__);
+		netdev_dbg(ieee->dev, "%s(): SET rx timeout timer\n", __func__);
 		pTS->RxTimeoutIndicateSeq = pTS->RxIndicateSeq;
 		mod_timer(&pTS->RxPktPendingTimer, jiffies +
 			  msecs_to_jiffies(pHTInfo->RxReorderPendingTime));
@@ -834,7 +874,8 @@ static u8 parse_subframe(struct rtllib_device *ieee, struct sk_buff *skb,
 		sub_skb->dev = ieee->dev;
 		rxb->subframes[rxb->nr_subframes++] = sub_skb;
 		if (rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
-			RTLLIB_DEBUG_RX("ParseSubframe(): Too many Subframes! Packets dropped!\n");
+			netdev_dbg(ieee->dev,
+				   "ParseSubframe(): Too many Subframes! Packets dropped!\n");
 			break;
 		}
 		skb_pull(skb, nSubframe_Length);
@@ -896,7 +937,8 @@ static int rtllib_rx_check_duplicate(struct rtllib_device *ieee,
 		!ieee->current_network.qos_data.active ||
 		!IsDataFrame(skb->data) ||
 		IsLegacyDataFrame(skb->data)) {
-		if (!((type == RTLLIB_FTYPE_MGMT) && (stype == RTLLIB_STYPE_BEACON))) {
+		if (!((type == RTLLIB_FTYPE_MGMT) &&
+		      (stype == RTLLIB_STYPE_BEACON))) {
 			if (is_duplicate_packet(ieee, hdr))
 				return -1;
 		}
@@ -911,7 +953,8 @@ static int rtllib_rx_check_duplicate(struct rtllib_device *ieee,
 			pRxTS->RxLastFragNum = frag;
 			pRxTS->RxLastSeqNum = WLAN_GET_SEQ_SEQ(sc);
 		} else {
-			RTLLIB_DEBUG(RTLLIB_DL_ERR, "ERR!!%s(): No TS!! Skip the check!!\n", __func__);
+			netdev_warn(ieee->dev, "%s(): No TS! Skip the check!\n",
+				    __func__);
 			return -1;
 		}
 	}
@@ -927,24 +970,24 @@ static void rtllib_rx_extract_addr(struct rtllib_device *ieee,
 
 	switch (fc & (RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS)) {
 	case RTLLIB_FCTL_FROMDS:
-		memcpy(dst, hdr->addr1, ETH_ALEN);
-		memcpy(src, hdr->addr3, ETH_ALEN);
-		memcpy(bssid, hdr->addr2, ETH_ALEN);
+		ether_addr_copy(dst, hdr->addr1);
+		ether_addr_copy(src, hdr->addr3);
+		ether_addr_copy(bssid, hdr->addr2);
 		break;
 	case RTLLIB_FCTL_TODS:
-		memcpy(dst, hdr->addr3, ETH_ALEN);
-		memcpy(src, hdr->addr2, ETH_ALEN);
-		memcpy(bssid, hdr->addr1, ETH_ALEN);
+		ether_addr_copy(dst, hdr->addr3);
+		ether_addr_copy(src, hdr->addr2);
+		ether_addr_copy(bssid, hdr->addr1);
 		break;
 	case RTLLIB_FCTL_FROMDS | RTLLIB_FCTL_TODS:
-		memcpy(dst, hdr->addr3, ETH_ALEN);
-		memcpy(src, hdr->addr4, ETH_ALEN);
-		memcpy(bssid, ieee->current_network.bssid, ETH_ALEN);
+		ether_addr_copy(dst, hdr->addr3);
+		ether_addr_copy(src, hdr->addr4);
+		ether_addr_copy(bssid, ieee->current_network.bssid);
 		break;
 	case 0:
-		memcpy(dst, hdr->addr1, ETH_ALEN);
-		memcpy(src, hdr->addr2, ETH_ALEN);
-		memcpy(bssid, hdr->addr3, ETH_ALEN);
+		ether_addr_copy(dst, hdr->addr1);
+		ether_addr_copy(src, hdr->addr2);
+		ether_addr_copy(bssid, hdr->addr3);
 		break;
 	}
 }
@@ -984,9 +1027,9 @@ static int rtllib_rx_data_filter(struct rtllib_device *ieee, u16 fc,
 		    stype != RTLLIB_STYPE_DATA_CFACKPOLL &&
 		    stype != RTLLIB_STYPE_QOS_DATA) {
 			if (stype != RTLLIB_STYPE_NULLFUNC)
-				RTLLIB_DEBUG_DROP(
-					"RX: dropped data frame with no data (type=0x%02x, subtype=0x%02x)\n",
-					type, stype);
+				netdev_dbg(ieee->dev,
+					   "RX: dropped data frame with no data (type=0x%02x, subtype=0x%02x)\n",
+					   type, stype);
 			return -1;
 		}
 	}
@@ -998,7 +1041,8 @@ static int rtllib_rx_data_filter(struct rtllib_device *ieee, u16 fc,
 
 		/* {broad,multi}cast packets to our BSS go through */
 		if (is_multicast_ether_addr(dst)) {
-			if (memcmp(bssid, ieee->current_network.bssid, ETH_ALEN))
+			if (memcmp(bssid, ieee->current_network.bssid,
+				   ETH_ALEN))
 				return -1;
 		}
 	}
@@ -1030,8 +1074,9 @@ static int rtllib_rx_get_crypt(struct rtllib_device *ieee, struct sk_buff *skb,
 			 * frames silently instead of filling system log with
 			 * these reports.
 			 */
-			RTLLIB_DEBUG_DROP("Decryption failed (not set) (SA= %pM)\n",
-					     hdr->addr2);
+			netdev_dbg(ieee->dev,
+				   "Decryption failed (not set) (SA= %pM)\n",
+				   hdr->addr2);
 			ieee->ieee_stats.rx_discards_undecryptable++;
 			return -1;
 		}
@@ -1070,13 +1115,13 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 		int flen;
 		struct sk_buff *frag_skb = rtllib_frag_cache_get(ieee, hdr);
 
-		RTLLIB_DEBUG_FRAG("Rx Fragment received (%u)\n", frag);
+		netdev_dbg(ieee->dev, "Rx Fragment received (%u)\n", frag);
 
 		if (!frag_skb) {
-			RTLLIB_DEBUG(RTLLIB_DL_RX | RTLLIB_DL_FRAG,
-					"Rx cannot get skb from fragment cache (morefrag=%d seq=%u frag=%u)\n",
-					(fc & RTLLIB_FCTL_MOREFRAGS) != 0,
-					WLAN_GET_SEQ_SEQ(sc), frag);
+			netdev_dbg(ieee->dev,
+				   "Rx cannot get skb from fragment cache (morefrag=%d seq=%u frag=%u)\n",
+				   (fc & RTLLIB_FCTL_MOREFRAGS) != 0,
+				   WLAN_GET_SEQ_SEQ(sc), frag);
 			return -1;
 		}
 		flen = skb->len;
@@ -1141,12 +1186,13 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 			 */
 			struct eapol *eap = (struct eapol *)(skb->data +
 				24);
-			RTLLIB_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
-						eap_get_type(eap->type));
+			netdev_dbg(ieee->dev,
+				   "RX: IEEE 802.1X EAPOL frame: %s\n",
+				   eap_get_type(eap->type));
 		} else {
-			RTLLIB_DEBUG_DROP(
-				"encryption configured, but RX frame not encrypted (SA= %pM)\n",
-				hdr->addr2);
+			netdev_dbg(ieee->dev,
+				   "encryption configured, but RX frame not encrypted (SA= %pM)\n",
+				   hdr->addr2);
 			return -1;
 		}
 	}
@@ -1155,15 +1201,16 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 	    rtllib_is_eapol_frame(ieee, skb, hdrlen)) {
 			struct eapol *eap = (struct eapol *)(skb->data +
 				24);
-			RTLLIB_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
-						eap_get_type(eap->type));
+			netdev_dbg(ieee->dev,
+				   "RX: IEEE 802.1X EAPOL frame: %s\n",
+				   eap_get_type(eap->type));
 	}
 
 	if (crypt && !(fc & RTLLIB_FCTL_WEP) && !ieee->open_wep &&
 	    !rtllib_is_eapol_frame(ieee, skb, hdrlen)) {
-		RTLLIB_DEBUG_DROP(
-			"dropped unencrypted RX data frame from %pM (drop_unencrypted=1)\n",
-			hdr->addr2);
+		netdev_dbg(ieee->dev,
+			   "dropped unencrypted RX data frame from %pM (drop_unencrypted=1)\n",
+			   hdr->addr2);
 		return -1;
 	}
 
@@ -1173,7 +1220,8 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 	return 0;
 }
 
-static void rtllib_rx_check_leave_lps(struct rtllib_device *ieee, u8 unicast, u8 nr_subframes)
+static void rtllib_rx_check_leave_lps(struct rtllib_device *ieee, u8 unicast,
+				      u8 nr_subframes)
 {
 	if (unicast) {
 
@@ -1208,25 +1256,33 @@ static void rtllib_rx_indicate_pkt_legacy(struct rtllib_device *ieee,
 		struct sk_buff *sub_skb = rxb->subframes[i];
 
 		if (sub_skb) {
-			/* convert hdr + possible LLC headers into Ethernet header */
+			/* convert hdr + possible LLC headers
+			 * into Ethernet header
+			 */
 			ethertype = (sub_skb->data[6] << 8) | sub_skb->data[7];
 			if (sub_skb->len >= 8 &&
 				((memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) == 0 &&
 				ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
 				memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE) == 0)) {
-				/* remove RFC1042 or Bridge-Tunnel encapsulation and
-				 * replace EtherType
+				/* remove RFC1042 or Bridge-Tunnel encapsulation
+				 * and replace EtherType
 				 */
 				skb_pull(sub_skb, SNAP_SIZE);
-				memcpy(skb_push(sub_skb, ETH_ALEN), src, ETH_ALEN);
-				memcpy(skb_push(sub_skb, ETH_ALEN), dst, ETH_ALEN);
+				ether_addr_copy(skb_push(sub_skb, ETH_ALEN),
+						src);
+				ether_addr_copy(skb_push(sub_skb, ETH_ALEN),
+						dst);
 			} else {
 				u16 len;
-				/* Leave Ethernet header part of hdr and full payload */
+				/* Leave Ethernet header part of hdr
+				 * and full payload
+				 */
 				len = sub_skb->len;
 				memcpy(skb_push(sub_skb, 2), &len, 2);
-				memcpy(skb_push(sub_skb, ETH_ALEN), src, ETH_ALEN);
-				memcpy(skb_push(sub_skb, ETH_ALEN), dst, ETH_ALEN);
+				ether_addr_copy(skb_push(sub_skb, ETH_ALEN),
+						src);
+				ether_addr_copy(skb_push(sub_skb, ETH_ALEN),
+						dst);
 			}
 
 			ieee->stats.rx_packets++;
@@ -1241,7 +1297,8 @@ static void rtllib_rx_indicate_pkt_legacy(struct rtllib_device *ieee,
 			sub_skb->dev = dev;
 			sub_skb->dev->stats.rx_packets++;
 			sub_skb->dev->stats.rx_bytes += sub_skb->len;
-			sub_skb->ip_summed = CHECKSUM_NONE; /* 802.11 crc not sufficient */
+			/* 802.11 crc not sufficient */
+			sub_skb->ip_summed = CHECKSUM_NONE;
 			netif_rx(sub_skb);
 		}
 	}
@@ -1258,7 +1315,11 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	struct rx_ts_record *pTS = NULL;
 	u16 fc, sc, SeqNum = 0;
 	u8 type, stype, multicast = 0, unicast = 0, nr_subframes = 0, TID = 0;
-	u8 dst[ETH_ALEN], src[ETH_ALEN], bssid[ETH_ALEN] = {0}, *payload;
+	u8 *payload;
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	u8 bssid[ETH_ALEN] = {0};
+
 	size_t hdrlen = 0;
 	bool bToOtherSTA = false;
 	int ret = 0, i = 0;
@@ -1282,7 +1343,8 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	/*Filter pkt has too small length */
 	hdrlen = rtllib_rx_get_hdrlen(ieee, skb, rx_stats);
 	if (skb->len < hdrlen) {
-		netdev_info(dev, "%s():ERR!!! skb->len is smaller than hdrlen\n",
+		netdev_info(dev,
+			    "%s():ERR!!! skb->len is smaller than hdrlen\n",
 			    __func__);
 		goto rx_dropped;
 	}
@@ -1327,10 +1389,13 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 		goto rx_dropped;
 
 	/* Send pspoll based on moredata */
-	if ((ieee->iw_mode == IW_MODE_INFRA)  && (ieee->sta_sleep == LPS_IS_SLEEP)
-		&& (ieee->polling) && (!bToOtherSTA)) {
+	if ((ieee->iw_mode == IW_MODE_INFRA)  &&
+	    (ieee->sta_sleep == LPS_IS_SLEEP) &&
+	    (ieee->polling) && (!bToOtherSTA)) {
 		if (WLAN_FC_MORE_DATA(fc)) {
-			/* more data bit is set, let's request a new frame from the AP */
+			/* more data bit is set, let's request a new frame
+			 * from the AP
+			 */
 			rtllib_sta_ps_send_pspoll_frame(ieee);
 		} else {
 			ieee->polling =  false;
@@ -1356,7 +1421,8 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 		&& (!bToOtherSTA)) {
 		TID = Frame_QoSTID(skb->data);
 		SeqNum = WLAN_GET_SEQ_SEQ(sc);
-		GetTs(ieee, (struct ts_common_info **) &pTS, hdr->addr2, TID, RX_DIR, true);
+		GetTs(ieee, (struct ts_common_info **) &pTS, hdr->addr2, TID,
+		      RX_DIR, true);
 		if (TID != 0 && TID != 3)
 			ieee->bis_any_nonbepkts = true;
 	}
@@ -1371,7 +1437,9 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	/* to parse amsdu packets */
 	/* qos data packets & reserved bit is 1 */
 	if (parse_subframe(ieee, skb, rx_stats, rxb, src, dst) == 0) {
-		/* only to free rxb, and not submit the packets to upper layer */
+		/* only to free rxb, and not submit the packets
+		 * to upper layer
+		 */
 		for (i = 0; i < rxb->nr_subframes; i++)
 			dev_kfree_skb(rxb->subframes[i]);
 		kfree(rxb);
@@ -1393,7 +1461,8 @@ static int rtllib_rx_InfraAdhoc(struct rtllib_device *ieee, struct sk_buff *skb,
 	}
 
 	/* Indicate packets to upper layer or Rx Reorder */
-	if (ieee->pHTInfo->bCurRxReorderEnable == false || pTS == NULL || bToOtherSTA)
+	if (ieee->pHTInfo->bCurRxReorderEnable == false || pTS == NULL ||
+	    bToOtherSTA)
 		rtllib_rx_indicate_pkt_legacy(ieee, rx_stats, rxb, dst, src);
 	else
 		RxReorderIndicatePacket(ieee, rxb, pTS, SeqNum);
@@ -1522,8 +1591,9 @@ static int rtllib_verify_qos_info(struct rtllib_qos_information_element
 
 /* Parse a QoS parameter element */
 static int rtllib_read_qos_param_element(struct rtllib_qos_parameter_info
-					    *element_param, struct rtllib_info_element
-					    *info_element)
+							*element_param,
+					 struct rtllib_info_element
+							*info_element)
 {
 	int ret = 0;
 	u16 size = sizeof(struct rtllib_qos_parameter_info) - 2;
@@ -1545,10 +1615,10 @@ static int rtllib_read_qos_param_element(struct rtllib_qos_parameter_info
 }
 
 /* Parse a QoS information element */
-static int rtllib_read_qos_info_element(struct
-					   rtllib_qos_information_element
-					   *element_info, struct rtllib_info_element
-					   *info_element)
+static int rtllib_read_qos_info_element(struct rtllib_qos_information_element
+							*element_info,
+					struct rtllib_info_element
+							*info_element)
 {
 	int ret = 0;
 	u16 size = sizeof(struct rtllib_qos_information_element) - 2;
@@ -1558,7 +1628,8 @@ static int rtllib_read_qos_info_element(struct
 	if (info_element == NULL)
 		return -1;
 
-	if ((info_element->id == QOS_ELEMENT_ID) && (info_element->len == size)) {
+	if ((info_element->id == QOS_ELEMENT_ID) &&
+	    (info_element->len == size)) {
 		memcpy(element_info->qui, info_element->data,
 		       info_element->len);
 		element_info->elementID = info_element->id;
@@ -1568,14 +1639,14 @@ static int rtllib_read_qos_info_element(struct
 
 	if (ret == 0)
 		ret = rtllib_verify_qos_info(element_info,
-						QOS_OUI_INFO_SUB_TYPE);
+					     QOS_OUI_INFO_SUB_TYPE);
 	return ret;
 }
 
 
 /* Write QoS parameters from the ac parameters. */
 static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info *param_elm,
-		struct rtllib_qos_data *qos_data)
+					       struct rtllib_qos_data *qos_data)
 {
 	struct rtllib_qos_ac_parameter *ac_params;
 	struct rtllib_qos_parameters *qos_param = &(qos_data->parameters);
@@ -1619,11 +1690,13 @@ static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info
 		qos_param->aifs[aci] = (ac_params->aci_aifsn) & 0x0f;
 
 		/* WMM spec P.11: The minimum value for AIFSN shall be 2 */
-		qos_param->aifs[aci] = (qos_param->aifs[aci] < 2) ? 2 : qos_param->aifs[aci];
+		qos_param->aifs[aci] = max_t(u8, qos_param->aifs[aci], 2);
 
-		qos_param->cw_min[aci] = cpu_to_le16(ac_params->ecw_min_max & 0x0F);
+		qos_param->cw_min[aci] = cpu_to_le16(ac_params->ecw_min_max &
+						     0x0F);
 
-		qos_param->cw_max[aci] = cpu_to_le16((ac_params->ecw_min_max & 0xF0) >> 4);
+		qos_param->cw_max[aci] = cpu_to_le16((ac_params->ecw_min_max &
+						      0xF0) >> 4);
 
 		qos_param->flag[aci] =
 		    (ac_params->aci_aifsn & 0x10) ? 0x01 : 0x00;
@@ -1636,9 +1709,10 @@ static int rtllib_qos_convert_ac_to_parameters(struct rtllib_qos_parameter_info
  * parameters element. check the information element length to decide
  * which type to read
  */
-static int rtllib_parse_qos_info_param_IE(struct rtllib_info_element
+static int rtllib_parse_qos_info_param_IE(struct rtllib_device *ieee,
+					  struct rtllib_info_element
 					     *info_element,
-					     struct rtllib_network *network)
+					  struct rtllib_network *network)
 {
 	int rc = 0;
 	struct rtllib_qos_information_element qos_info_element;
@@ -1663,7 +1737,7 @@ static int rtllib_parse_qos_info_param_IE(struct rtllib_info_element
 	}
 
 	if (rc == 0) {
-		RTLLIB_DEBUG_QOS("QoS is supported\n");
+		netdev_dbg(ieee->dev, "QoS is supported\n");
 		network->qos_data.supported = 1;
 	}
 	return rc;
@@ -1713,15 +1787,19 @@ static inline void rtllib_extract_country_ie(
 {
 	if (IS_DOT11D_ENABLE(ieee)) {
 		if (info_element->len != 0) {
-			memcpy(network->CountryIeBuf, info_element->data, info_element->len);
+			memcpy(network->CountryIeBuf, info_element->data,
+			       info_element->len);
 			network->CountryIeLen = info_element->len;
 
 			if (!IS_COUNTRY_IE_VALID(ieee)) {
-				if (rtllib_act_scanning(ieee, false) && ieee->FirstIe_InScan)
+				if (rtllib_act_scanning(ieee, false) &&
+				    ieee->FirstIe_InScan)
 					netdev_info(ieee->dev,
 						    "Received beacon ContryIE, SSID: <%s>\n",
 						    network->ssid);
-				Dot11d_UpdateCountryIe(ieee, addr2, info_element->len, info_element->data);
+				Dot11d_UpdateCountryIe(ieee, addr2,
+						       info_element->len,
+						       info_element->data);
 			}
 		}
 
@@ -1731,6 +1809,234 @@ static inline void rtllib_extract_country_ie(
 
 }
 
+static void rtllib_parse_mife_generic(struct rtllib_device *ieee,
+				      struct rtllib_info_element *info_element,
+				      struct rtllib_network *network,
+				      u16 *tmp_htcap_len,
+				      u16 *tmp_htinfo_len)
+{
+	u16 ht_realtek_agg_len = 0;
+	u8  ht_realtek_agg_buf[MAX_IE_LEN];
+
+	if (!rtllib_parse_qos_info_param_IE(ieee, info_element, network))
+		return;
+	if (info_element->len >= 4 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x50 &&
+	    info_element->data[2] == 0xf2 &&
+	    info_element->data[3] == 0x01) {
+		network->wpa_ie_len = min(info_element->len + 2,
+					  MAX_WPA_IE_LEN);
+		memcpy(network->wpa_ie, info_element, network->wpa_ie_len);
+		return;
+	}
+	if (info_element->len == 7 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0xe0 &&
+	    info_element->data[2] == 0x4c &&
+	    info_element->data[3] == 0x01 &&
+	    info_element->data[4] == 0x02)
+		network->Turbo_Enable = 1;
+
+	if (*tmp_htcap_len == 0) {
+		if (info_element->len >= 4 &&
+		    info_element->data[0] == 0x00 &&
+		    info_element->data[1] == 0x90 &&
+		    info_element->data[2] == 0x4c &&
+		    info_element->data[3] == 0x033) {
+			*tmp_htcap_len = min_t(u8, info_element->len,
+					       MAX_IE_LEN);
+			if (*tmp_htcap_len != 0) {
+				network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
+				network->bssht.bdHTCapLen = min_t(u16, *tmp_htcap_len, sizeof(network->bssht.bdHTCapBuf));
+				memcpy(network->bssht.bdHTCapBuf,
+				       info_element->data,
+				       network->bssht.bdHTCapLen);
+			}
+		}
+		if (*tmp_htcap_len != 0) {
+			network->bssht.bdSupportHT = true;
+			network->bssht.bdHT1R = ((((struct ht_capab_ele *)(network->bssht.bdHTCapBuf))->MCS[1]) == 0);
+		} else {
+			network->bssht.bdSupportHT = false;
+			network->bssht.bdHT1R = false;
+		}
+	}
+
+
+	if (*tmp_htinfo_len == 0) {
+		if (info_element->len >= 4 &&
+		    info_element->data[0] == 0x00 &&
+		    info_element->data[1] == 0x90 &&
+		    info_element->data[2] == 0x4c &&
+		    info_element->data[3] == 0x034) {
+			*tmp_htinfo_len = min_t(u8, info_element->len,
+						MAX_IE_LEN);
+			if (*tmp_htinfo_len != 0) {
+				network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
+				network->bssht.bdHTInfoLen = min_t(u16, *tmp_htinfo_len, sizeof(network->bssht.bdHTInfoBuf));
+				memcpy(network->bssht.bdHTInfoBuf,
+				       info_element->data,
+				       network->bssht.bdHTInfoLen);
+			}
+
+		}
+	}
+
+	if (network->bssht.bdSupportHT) {
+		if (info_element->len >= 4 &&
+		    info_element->data[0] == 0x00 &&
+		    info_element->data[1] == 0xe0 &&
+		    info_element->data[2] == 0x4c &&
+		    info_element->data[3] == 0x02) {
+			ht_realtek_agg_len = min_t(u8, info_element->len,
+						   MAX_IE_LEN);
+			memcpy(ht_realtek_agg_buf, info_element->data,
+			       info_element->len);
+		}
+		if (ht_realtek_agg_len >= 5) {
+			network->realtek_cap_exit = true;
+			network->bssht.bdRT2RTAggregation = true;
+
+			if ((ht_realtek_agg_buf[4] == 1) &&
+			    (ht_realtek_agg_buf[5] & 0x02))
+				network->bssht.bdRT2RTLongSlotTime = true;
+
+			if ((ht_realtek_agg_buf[4] == 1) &&
+			    (ht_realtek_agg_buf[5] & RT_HT_CAP_USE_92SE))
+				network->bssht.RT2RT_HT_Mode |= RT_HT_CAP_USE_92SE;
+		}
+	}
+	if (ht_realtek_agg_len >= 5) {
+		if ((ht_realtek_agg_buf[5] & RT_HT_CAP_USE_SOFTAP))
+			network->bssht.RT2RT_HT_Mode |= RT_HT_CAP_USE_SOFTAP;
+	}
+
+	if ((info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x05 &&
+	     info_element->data[2] == 0xb5) ||
+	     (info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x0a &&
+	     info_element->data[2] == 0xf7) ||
+	     (info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x10 &&
+	     info_element->data[2] == 0x18)) {
+		network->broadcom_cap_exist = true;
+	}
+	if (info_element->len >= 3 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x0c &&
+	    info_element->data[2] == 0x43)
+		network->ralink_cap_exist = true;
+	if ((info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x03 &&
+	     info_element->data[2] == 0x7f) ||
+	     (info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x13 &&
+	     info_element->data[2] == 0x74))
+		network->atheros_cap_exist = true;
+
+	if ((info_element->len >= 3 &&
+	     info_element->data[0] == 0x00 &&
+	     info_element->data[1] == 0x50 &&
+	     info_element->data[2] == 0x43))
+		network->marvell_cap_exist = true;
+	if (info_element->len >= 3 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x40 &&
+	    info_element->data[2] == 0x96)
+		network->cisco_cap_exist = true;
+
+
+	if (info_element->len >= 3 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x0a &&
+	    info_element->data[2] == 0xf5)
+		network->airgo_cap_exist = true;
+
+	if (info_element->len > 4 &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x40 &&
+	    info_element->data[2] == 0x96 &&
+	    info_element->data[3] == 0x01) {
+		if (info_element->len == 6) {
+			memcpy(network->CcxRmState, &info_element[4], 2);
+			if (network->CcxRmState[0] != 0)
+				network->bCcxRmEnable = true;
+			else
+				network->bCcxRmEnable = false;
+			network->MBssidMask = network->CcxRmState[1] & 0x07;
+			if (network->MBssidMask != 0) {
+				network->bMBssidValid = true;
+				network->MBssidMask = 0xff <<
+						      (network->MBssidMask);
+				ether_addr_copy(network->MBssid,
+						network->bssid);
+				network->MBssid[5] &= network->MBssidMask;
+			} else {
+				network->bMBssidValid = false;
+			}
+		} else {
+			network->bCcxRmEnable = false;
+		}
+	}
+	if (info_element->len > 4  &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x40 &&
+	    info_element->data[2] == 0x96 &&
+	    info_element->data[3] == 0x03) {
+		if (info_element->len == 5) {
+			network->bWithCcxVerNum = true;
+			network->BssCcxVerNumber = info_element->data[4];
+		} else {
+			network->bWithCcxVerNum = false;
+			network->BssCcxVerNumber = 0;
+		}
+	}
+	if (info_element->len > 4  &&
+	    info_element->data[0] == 0x00 &&
+	    info_element->data[1] == 0x50 &&
+	    info_element->data[2] == 0xf2 &&
+	    info_element->data[3] == 0x04) {
+		netdev_dbg(ieee->dev, "MFIE_TYPE_WZC: %d bytes\n",
+			   info_element->len);
+		network->wzc_ie_len = min(info_element->len+2, MAX_WZC_IE_LEN);
+		memcpy(network->wzc_ie, info_element, network->wzc_ie_len);
+	}
+}
+
+static void rtllib_parse_mfie_ht_cap(struct rtllib_info_element *info_element,
+				     struct rtllib_network *network,
+				     u16 *tmp_htcap_len)
+{
+	struct bss_ht *ht = &network->bssht;
+
+	*tmp_htcap_len = min_t(u8, info_element->len, MAX_IE_LEN);
+	if (*tmp_htcap_len != 0) {
+		ht->bdHTSpecVer = HT_SPEC_VER_EWC;
+		ht->bdHTCapLen = min_t(u16, *tmp_htcap_len,
+				       sizeof(ht->bdHTCapBuf));
+		memcpy(ht->bdHTCapBuf, info_element->data, ht->bdHTCapLen);
+
+		ht->bdSupportHT = true;
+		ht->bdHT1R = ((((struct ht_capab_ele *)
+				ht->bdHTCapBuf))->MCS[1]) == 0;
+
+		ht->bdBandWidth = (enum ht_channel_width)
+					     (((struct ht_capab_ele *)
+					     (ht->bdHTCapBuf))->ChlWidth);
+	} else {
+		ht->bdSupportHT = false;
+		ht->bdHT1R = false;
+		ht->bdBandWidth = HT_CHANNEL_WIDTH_20;
+	}
+}
+
 int rtllib_parse_info_param(struct rtllib_device *ieee,
 		struct rtllib_info_element *info_element,
 		u16 length,
@@ -1741,17 +2047,15 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 	short offset;
 	u16	tmp_htcap_len = 0;
 	u16	tmp_htinfo_len = 0;
-	u16 ht_realtek_agg_len = 0;
-	u8  ht_realtek_agg_buf[MAX_IE_LEN];
 	char rates_str[64];
 	char *p;
 
 	while (length >= sizeof(*info_element)) {
 		if (sizeof(*info_element) + info_element->len > length) {
-			RTLLIB_DEBUG_MGMT("Info elem: parse failed: info_element->len + 2 > left : info_element->len+2=%zd left=%d, id=%d.\n",
-					     info_element->len +
-					     sizeof(*info_element),
-					     length, info_element->id);
+			netdev_dbg(ieee->dev,
+				   "Info elem: parse failed: info_element->len + 2 > left : info_element->len+2=%zd left=%d, id=%d.\n",
+				   info_element->len + sizeof(*info_element),
+				   length, info_element->id);
 			/* We stop processing but don't return an error here
 			 * because some misbehaviour APs break this rule. ie.
 			 * Orinoco AP1000.
@@ -1769,13 +2073,14 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 
 			network->ssid_len = min(info_element->len,
 						(u8) IW_ESSID_MAX_SIZE);
-			memcpy(network->ssid, info_element->data, network->ssid_len);
+			memcpy(network->ssid, info_element->data,
+			       network->ssid_len);
 			if (network->ssid_len < IW_ESSID_MAX_SIZE)
 				memset(network->ssid + network->ssid_len, 0,
 				       IW_ESSID_MAX_SIZE - network->ssid_len);
 
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_SSID: '%s' len=%d.\n",
-					     network->ssid, network->ssid_len);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_SSID: '%s' len=%d.\n",
+				   network->ssid, network->ssid_len);
 			break;
 
 		case MFIE_TYPE_RATES:
@@ -1802,8 +2107,8 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 				}
 			}
 
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_RATES: '%s' (%d)\n",
-					     rates_str, network->rates_len);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_RATES: '%s' (%d)\n",
+				   rates_str, network->rates_len);
 			break;
 
 		case MFIE_TYPE_RATES_EX:
@@ -1825,22 +2130,22 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 				}
 			}
 
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_RATES_EX: '%s' (%d)\n",
-					     rates_str, network->rates_ex_len);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_RATES_EX: '%s' (%d)\n",
+				   rates_str, network->rates_ex_len);
 			break;
 
 		case MFIE_TYPE_DS_SET:
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_DS_SET: %d\n",
-					     info_element->data[0]);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_DS_SET: %d\n",
+				   info_element->data[0]);
 			network->channel = info_element->data[0];
 			break;
 
 		case MFIE_TYPE_FH_SET:
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_FH_SET: ignored\n");
+			netdev_dbg(ieee->dev, "MFIE_TYPE_FH_SET: ignored\n");
 			break;
 
 		case MFIE_TYPE_CF_SET:
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_CF_SET: ignored\n");
+			netdev_dbg(ieee->dev, "MFIE_TYPE_CF_SET: ignored\n");
 			break;
 
 		case MFIE_TYPE_TIM:
@@ -1879,217 +2184,31 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 		case MFIE_TYPE_ERP:
 			network->erp_value = info_element->data[0];
 			network->flags |= NETWORK_HAS_ERP_VALUE;
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_ERP_SET: %d\n",
-					     network->erp_value);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_ERP_SET: %d\n",
+				   network->erp_value);
 			break;
 		case MFIE_TYPE_IBSS_SET:
 			network->atim_window = info_element->data[0];
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_IBSS_SET: %d\n",
-					     network->atim_window);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_IBSS_SET: %d\n",
+				   network->atim_window);
 			break;
 
 		case MFIE_TYPE_CHALLENGE:
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_CHALLENGE: ignored\n");
+			netdev_dbg(ieee->dev, "MFIE_TYPE_CHALLENGE: ignored\n");
 			break;
 
 		case MFIE_TYPE_GENERIC:
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_GENERIC: %d bytes\n",
-					     info_element->len);
-			if (!rtllib_parse_qos_info_param_IE(info_element,
-							       network))
-				break;
-			if (info_element->len >= 4 &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x50 &&
-			    info_element->data[2] == 0xf2 &&
-			    info_element->data[3] == 0x01) {
-				network->wpa_ie_len = min(info_element->len + 2,
-							  MAX_WPA_IE_LEN);
-				memcpy(network->wpa_ie, info_element,
-				       network->wpa_ie_len);
-				break;
-			}
-			if (info_element->len == 7 &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0xe0 &&
-			    info_element->data[2] == 0x4c &&
-			    info_element->data[3] == 0x01 &&
-			    info_element->data[4] == 0x02)
-				network->Turbo_Enable = 1;
-
-			if (tmp_htcap_len == 0) {
-				if (info_element->len >= 4 &&
-				   info_element->data[0] == 0x00 &&
-				   info_element->data[1] == 0x90 &&
-				   info_element->data[2] == 0x4c &&
-				   info_element->data[3] == 0x033) {
-
-						tmp_htcap_len = min_t(u8, info_element->len, MAX_IE_LEN);
-						if (tmp_htcap_len != 0) {
-							network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
-							network->bssht.bdHTCapLen = tmp_htcap_len > sizeof(network->bssht.bdHTCapBuf) ?
-								sizeof(network->bssht.bdHTCapBuf) : tmp_htcap_len;
-							memcpy(network->bssht.bdHTCapBuf, info_element->data, network->bssht.bdHTCapLen);
-						}
-				}
-				if (tmp_htcap_len != 0) {
-					network->bssht.bdSupportHT = true;
-					network->bssht.bdHT1R = ((((struct ht_capab_ele *)(network->bssht.bdHTCapBuf))->MCS[1]) == 0);
-				} else {
-					network->bssht.bdSupportHT = false;
-					network->bssht.bdHT1R = false;
-				}
-			}
-
-
-			if (tmp_htinfo_len == 0) {
-				if (info_element->len >= 4 &&
-				    info_element->data[0] == 0x00 &&
-				    info_element->data[1] == 0x90 &&
-				    info_element->data[2] == 0x4c &&
-				    info_element->data[3] == 0x034) {
-					tmp_htinfo_len = min_t(u8, info_element->len, MAX_IE_LEN);
-					if (tmp_htinfo_len != 0) {
-						network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
-						if (tmp_htinfo_len) {
-							network->bssht.bdHTInfoLen = tmp_htinfo_len > sizeof(network->bssht.bdHTInfoBuf) ?
-								sizeof(network->bssht.bdHTInfoBuf) : tmp_htinfo_len;
-							memcpy(network->bssht.bdHTInfoBuf, info_element->data, network->bssht.bdHTInfoLen);
-						}
+			netdev_dbg(ieee->dev, "MFIE_TYPE_GENERIC: %d bytes\n",
+				   info_element->len);
 
-					}
-
-				}
-			}
-
-			if (ieee->aggregation) {
-				if (network->bssht.bdSupportHT) {
-					if (info_element->len >= 4 &&
-					    info_element->data[0] == 0x00 &&
-					    info_element->data[1] == 0xe0 &&
-					    info_element->data[2] == 0x4c &&
-					    info_element->data[3] == 0x02) {
-						ht_realtek_agg_len = min_t(u8, info_element->len, MAX_IE_LEN);
-						memcpy(ht_realtek_agg_buf, info_element->data, info_element->len);
-					}
-					if (ht_realtek_agg_len >= 5) {
-						network->realtek_cap_exit = true;
-						network->bssht.bdRT2RTAggregation = true;
-
-						if ((ht_realtek_agg_buf[4] == 1) && (ht_realtek_agg_buf[5] & 0x02))
-							network->bssht.bdRT2RTLongSlotTime = true;
-
-						if ((ht_realtek_agg_buf[4] == 1) && (ht_realtek_agg_buf[5] & RT_HT_CAP_USE_92SE))
-							network->bssht.RT2RT_HT_Mode |= RT_HT_CAP_USE_92SE;
-					}
-				}
-				if (ht_realtek_agg_len >= 5) {
-					if ((ht_realtek_agg_buf[5] & RT_HT_CAP_USE_SOFTAP))
-						network->bssht.RT2RT_HT_Mode |= RT_HT_CAP_USE_SOFTAP;
-				}
-			}
-
-			if ((info_element->len >= 3 &&
-			     info_element->data[0] == 0x00 &&
-			     info_element->data[1] == 0x05 &&
-			     info_element->data[2] == 0xb5) ||
-			     (info_element->len >= 3 &&
-			     info_element->data[0] == 0x00 &&
-			     info_element->data[1] == 0x0a &&
-			     info_element->data[2] == 0xf7) ||
-			     (info_element->len >= 3 &&
-			     info_element->data[0] == 0x00 &&
-			     info_element->data[1] == 0x10 &&
-			     info_element->data[2] == 0x18)) {
-				network->broadcom_cap_exist = true;
-			}
-			if (info_element->len >= 3 &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x0c &&
-			    info_element->data[2] == 0x43)
-				network->ralink_cap_exist = true;
-			if ((info_element->len >= 3 &&
-			     info_element->data[0] == 0x00 &&
-			     info_element->data[1] == 0x03 &&
-			     info_element->data[2] == 0x7f) ||
-			     (info_element->len >= 3 &&
-			     info_element->data[0] == 0x00 &&
-			     info_element->data[1] == 0x13 &&
-			     info_element->data[2] == 0x74))
-				network->atheros_cap_exist = true;
-
-			if ((info_element->len >= 3 &&
-			     info_element->data[0] == 0x00 &&
-			     info_element->data[1] == 0x50 &&
-			     info_element->data[2] == 0x43))
-				network->marvell_cap_exist = true;
-			if (info_element->len >= 3 &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x40 &&
-			    info_element->data[2] == 0x96)
-				network->cisco_cap_exist = true;
-
-
-			if (info_element->len >= 3 &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x0a &&
-			    info_element->data[2] == 0xf5)
-				network->airgo_cap_exist = true;
-
-			if (info_element->len > 4 &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x40 &&
-			    info_element->data[2] == 0x96 &&
-			    info_element->data[3] == 0x01) {
-				if (info_element->len == 6) {
-					memcpy(network->CcxRmState, &info_element[4], 2);
-					if (network->CcxRmState[0] != 0)
-						network->bCcxRmEnable = true;
-					else
-						network->bCcxRmEnable = false;
-					network->MBssidMask = network->CcxRmState[1] & 0x07;
-					if (network->MBssidMask != 0) {
-						network->bMBssidValid = true;
-						network->MBssidMask = 0xff << (network->MBssidMask);
-						memcpy(network->MBssid, network->bssid, ETH_ALEN);
-						network->MBssid[5] &= network->MBssidMask;
-					} else {
-						network->bMBssidValid = false;
-					}
-				} else {
-					network->bCcxRmEnable = false;
-				}
-			}
-			if (info_element->len > 4  &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x40 &&
-			    info_element->data[2] == 0x96 &&
-			    info_element->data[3] == 0x03) {
-				if (info_element->len == 5) {
-					network->bWithCcxVerNum = true;
-					network->BssCcxVerNumber = info_element->data[4];
-				} else {
-					network->bWithCcxVerNum = false;
-					network->BssCcxVerNumber = 0;
-				}
-			}
-			if (info_element->len > 4  &&
-			    info_element->data[0] == 0x00 &&
-			    info_element->data[1] == 0x50 &&
-			    info_element->data[2] == 0xf2 &&
-			    info_element->data[3] == 0x04) {
-				RTLLIB_DEBUG_MGMT("MFIE_TYPE_WZC: %d bytes\n",
-						     info_element->len);
-				network->wzc_ie_len = min(info_element->len+2,
-							  MAX_WZC_IE_LEN);
-				memcpy(network->wzc_ie, info_element,
-						network->wzc_ie_len);
-			}
+			rtllib_parse_mife_generic(ieee, info_element, network,
+						  &tmp_htcap_len,
+						  &tmp_htinfo_len);
 			break;
 
 		case MFIE_TYPE_RSN:
-			RTLLIB_DEBUG_MGMT("MFIE_TYPE_RSN: %d bytes\n",
-					     info_element->len);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_RSN: %d bytes\n",
+				   info_element->len);
 			network->rsn_ie_len = min(info_element->len + 2,
 						  MAX_WPA_IE_LEN);
 			memcpy(network->rsn_ie, info_element,
@@ -2097,36 +2216,19 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 			break;
 
 		case MFIE_TYPE_HT_CAP:
-			RTLLIB_DEBUG_SCAN("MFIE_TYPE_HT_CAP: %d bytes\n",
-					     info_element->len);
-			tmp_htcap_len = min_t(u8, info_element->len, MAX_IE_LEN);
-			if (tmp_htcap_len != 0) {
-				network->bssht.bdHTSpecVer = HT_SPEC_VER_EWC;
-				network->bssht.bdHTCapLen = tmp_htcap_len > sizeof(network->bssht.bdHTCapBuf) ?
-					sizeof(network->bssht.bdHTCapBuf) : tmp_htcap_len;
-				memcpy(network->bssht.bdHTCapBuf,
-				       info_element->data,
-				       network->bssht.bdHTCapLen);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_HT_CAP: %d bytes\n",
+				   info_element->len);
 
-				network->bssht.bdSupportHT = true;
-				network->bssht.bdHT1R = ((((struct ht_capab_ele *)
-							network->bssht.bdHTCapBuf))->MCS[1]) == 0;
-
-				network->bssht.bdBandWidth = (enum ht_channel_width)
-							     (((struct ht_capab_ele *)
-							     (network->bssht.bdHTCapBuf))->ChlWidth);
-			} else {
-				network->bssht.bdSupportHT = false;
-				network->bssht.bdHT1R = false;
-				network->bssht.bdBandWidth = HT_CHANNEL_WIDTH_20;
-			}
+			rtllib_parse_mfie_ht_cap(info_element, network,
+						 &tmp_htcap_len);
 			break;
 
 
 		case MFIE_TYPE_HT_INFO:
-			RTLLIB_DEBUG_SCAN("MFIE_TYPE_HT_INFO: %d bytes\n",
-					     info_element->len);
-			tmp_htinfo_len = min_t(u8, info_element->len, MAX_IE_LEN);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_HT_INFO: %d bytes\n",
+				   info_element->len);
+			tmp_htinfo_len = min_t(u8, info_element->len,
+					       MAX_IE_LEN);
 			if (tmp_htinfo_len) {
 				network->bssht.bdHTSpecVer = HT_SPEC_VER_IEEE;
 				network->bssht.bdHTInfoLen = tmp_htinfo_len >
@@ -2140,8 +2242,8 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 			break;
 
 		case MFIE_TYPE_AIRONET:
-			RTLLIB_DEBUG_SCAN("MFIE_TYPE_AIRONET: %d bytes\n",
-					     info_element->len);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_AIRONET: %d bytes\n",
+				   info_element->len);
 			if (info_element->len > IE_CISCO_FLAG_POSITION) {
 				network->bWithAironetIE = true;
 
@@ -2163,17 +2265,17 @@ int rtllib_parse_info_param(struct rtllib_device *ieee,
 			break;
 
 		case MFIE_TYPE_COUNTRY:
-			RTLLIB_DEBUG_SCAN("MFIE_TYPE_COUNTRY: %d bytes\n",
-					     info_element->len);
+			netdev_dbg(ieee->dev, "MFIE_TYPE_COUNTRY: %d bytes\n",
+				   info_element->len);
 			rtllib_extract_country_ie(ieee, info_element, network,
 						  network->bssid);
 			break;
 /* TODO */
 		default:
-			RTLLIB_DEBUG_MGMT
-			    ("Unsupported info element: %s (%d)\n",
-			     get_info_element_string(info_element->id),
-			     info_element->id);
+			netdev_dbg(ieee->dev,
+				   "Unsupported info element: %s (%d)\n",
+				   get_info_element_string(info_element->id),
+				   info_element->id);
 			break;
 		}
 
@@ -2211,7 +2313,7 @@ static inline int rtllib_network_init(
 	memset(&network->qos_data, 0, sizeof(struct rtllib_qos_data));
 
 	/* Pull out fixed field data */
-	memcpy(network->bssid, beacon->header.addr3, ETH_ALEN);
+	ether_addr_copy(network->bssid, beacon->header.addr3);
 	network->capability = le16_to_cpu(beacon->capability);
 	network->last_scanned = jiffies;
 	network->time_stamp[0] = beacon->time_stamp[0];
@@ -2220,7 +2322,6 @@ static inline int rtllib_network_init(
 	/* Where to pull this? beacon->listen_interval;*/
 	network->listen_interval = 0x0A;
 	network->rates_len = network->rates_ex_len = 0;
-	network->last_associate = 0;
 	network->ssid_len = 0;
 	network->hidden_ssid_len = 0;
 	memset(network->hidden_ssid, 0, sizeof(network->hidden_ssid));
@@ -2271,10 +2372,9 @@ static inline int rtllib_network_init(
 	}
 
 	if (network->mode == 0) {
-		RTLLIB_DEBUG_SCAN("Filtered out '%s (%pM)' network.\n",
-				     escape_essid(network->ssid,
-						  network->ssid_len),
-				     network->bssid);
+		netdev_dbg(ieee->dev, "Filtered out '%s (%pM)' network.\n",
+			   escape_essid(network->ssid, network->ssid_len),
+			   network->bssid);
 		return 1;
 	}
 
@@ -2314,7 +2414,8 @@ static inline int is_same_network(struct rtllib_network *src,
 }
 
 
-static inline void update_network(struct rtllib_network *dst,
+static inline void update_network(struct rtllib_device *ieee,
+				  struct rtllib_network *dst,
 				  struct rtllib_network *src)
 {
 	int qos_active;
@@ -2388,17 +2489,16 @@ static inline void update_network(struct rtllib_network *dst,
 		       sizeof(struct rtllib_qos_data));
 	if (dst->qos_data.supported == 1) {
 		if (dst->ssid_len)
-			RTLLIB_DEBUG_QOS
-				("QoS the network %s is QoS supported\n",
-				dst->ssid);
+			netdev_dbg(ieee->dev,
+				   "QoS the network %s is QoS supported\n",
+				   dst->ssid);
 		else
-			RTLLIB_DEBUG_QOS
-				("QoS the network is QoS supported\n");
+			netdev_dbg(ieee->dev,
+				   "QoS the network is QoS supported\n");
 	}
 	dst->qos_data.active = qos_active;
 	dst->qos_data.old_param_count = old_param;
 
-	/* dst->last_associate is not overwritten */
 	dst->wmm_info = src->wmm_info;
 	if (src->wmm_param[0].ac_aci_acm_aifsn ||
 	   src->wmm_param[1].ac_aci_acm_aifsn ||
@@ -2424,9 +2524,9 @@ static inline void update_network(struct rtllib_network *dst,
 	dst->BssCcxVerNumber = src->BssCcxVerNumber;
 }
 
-static inline int is_beacon(__le16 fc)
+static inline int is_beacon(u16 fc)
 {
-	return (WLAN_FC_GET_STYPE(le16_to_cpu(fc)) == RTLLIB_STYPE_BEACON);
+	return (WLAN_FC_GET_STYPE(fc) == RTLLIB_STYPE_BEACON);
 }
 
 static int IsPassiveChannel(struct rtllib_device *rtllib, u8 channel)
@@ -2467,40 +2567,37 @@ static inline void rtllib_process_probe_response(
 	short renew;
 	struct rtllib_network *network = kzalloc(sizeof(struct rtllib_network),
 						 GFP_ATOMIC);
+	u16 frame_ctl = le16_to_cpu(beacon->header.frame_ctl);
 
 	if (!network)
 		return;
 
-	RTLLIB_DEBUG_SCAN(
-		"'%s' ( %pM ): %c%c%c%c %c%c%c%c-%c%c%c%c %c%c%c%c\n",
-		escape_essid(info_element->data, info_element->len),
-		beacon->header.addr3,
-		(le16_to_cpu(beacon->capability) & (1<<0xf)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0xe)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0xd)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0xc)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0xb)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0xa)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x9)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x8)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x7)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x6)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x5)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x4)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x3)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x2)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x1)) ? '1' : '0',
-		(le16_to_cpu(beacon->capability) & (1<<0x0)) ? '1' : '0');
+	netdev_dbg(ieee->dev,
+		   "'%s' ( %pM ): %c%c%c%c %c%c%c%c-%c%c%c%c %c%c%c%c\n",
+		   escape_essid(info_element->data, info_element->len),
+		   beacon->header.addr3,
+		   (le16_to_cpu(beacon->capability) & (1<<0xf)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xe)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xd)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xc)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xb)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0xa)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x9)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x8)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x7)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x6)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x5)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x4)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x3)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x2)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x1)) ? '1' : '0',
+		   (le16_to_cpu(beacon->capability) & (1<<0x0)) ? '1' : '0');
 
 	if (rtllib_network_init(ieee, beacon, network, stats)) {
-		RTLLIB_DEBUG_SCAN("Dropped '%s' ( %pM) via %s.\n",
-				  escape_essid(info_element->data,
-				  info_element->len),
-				  beacon->header.addr3,
-				  WLAN_FC_GET_STYPE(
-					  le16_to_cpu(beacon->header.frame_ctl)) ==
-				  RTLLIB_STYPE_PROBE_RESP ?
-				  "PROBE RESPONSE" : "BEACON");
+		netdev_dbg(ieee->dev, "Dropped '%s' ( %pM) via %s.\n",
+			   escape_essid(info_element->data, info_element->len),
+			   beacon->header.addr3,
+			   is_beacon(frame_ctl) ? "BEACON" : "PROBE RESPONSE");
 		goto free_network;
 	}
 
@@ -2508,8 +2605,7 @@ static inline void rtllib_process_probe_response(
 	if (!rtllib_legal_channel(ieee, network->channel))
 		goto free_network;
 
-	if (WLAN_FC_GET_STYPE(le16_to_cpu(beacon->header.frame_ctl)) ==
-	    RTLLIB_STYPE_PROBE_RESP) {
+	if (WLAN_FC_GET_STYPE(frame_ctl) == RTLLIB_STYPE_PROBE_RESP) {
 		if (IsPassiveChannel(ieee, network->channel)) {
 			netdev_info(ieee->dev,
 				    "GetScanInfo(): For Global Domain, filter probe response at channel(%d).\n",
@@ -2533,7 +2629,7 @@ static inline void rtllib_process_probe_response(
 	spin_lock_irqsave(&ieee->lock, flags);
 	if (is_same_network(&ieee->current_network, network,
 	   (network->ssid_len ? 1 : 0))) {
-		update_network(&ieee->current_network, network);
+		update_network(ieee, &ieee->current_network, network);
 		if ((ieee->current_network.mode == IEEE_N_24G ||
 		     ieee->current_network.mode == IEEE_G)
 		     && ieee->current_network.berp_info_valid) {
@@ -2542,7 +2638,7 @@ static inline void rtllib_process_probe_response(
 			else
 				ieee->current_network.buseprotection = false;
 		}
-		if (is_beacon(beacon->header.frame_ctl)) {
+		if (is_beacon(frame_ctl)) {
 			if (ieee->state >= RTLLIB_LINKED)
 				ieee->LinkDetectInfo.NumRecvBcnInPeriod++;
 		}
@@ -2564,10 +2660,10 @@ static inline void rtllib_process_probe_response(
 			/* If there are no more slots, expire the oldest */
 			list_del(&oldest->list);
 			target = oldest;
-			RTLLIB_DEBUG_SCAN("Expired '%s' ( %pM) from network list.\n",
-					     escape_essid(target->ssid,
-							  target->ssid_len),
-					     target->bssid);
+			netdev_dbg(ieee->dev,
+				   "Expired '%s' ( %pM) from network list.\n",
+				   escape_essid(target->ssid, target->ssid_len),
+				   target->bssid);
 		} else {
 			/* Otherwise just pull from the free list */
 			target = list_entry(ieee->network_free_list.next,
@@ -2575,26 +2671,20 @@ static inline void rtllib_process_probe_response(
 			list_del(ieee->network_free_list.next);
 		}
 
+		netdev_dbg(ieee->dev, "Adding '%s' ( %pM) via %s.\n",
+			   escape_essid(network->ssid, network->ssid_len),
+			   network->bssid,
+			   is_beacon(frame_ctl) ? "BEACON" : "PROBE RESPONSE");
 
-		RTLLIB_DEBUG_SCAN("Adding '%s' ( %pM) via %s.\n",
-				  escape_essid(network->ssid,
-				  network->ssid_len), network->bssid,
-				  WLAN_FC_GET_STYPE(
-					  le16_to_cpu(beacon->header.frame_ctl)) ==
-				  RTLLIB_STYPE_PROBE_RESP ?
-				  "PROBE RESPONSE" : "BEACON");
 		memcpy(target, network, sizeof(*target));
 		list_add_tail(&target->list, &ieee->network_list);
 		if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE)
 			rtllib_softmac_new_net(ieee, network);
 	} else {
-		RTLLIB_DEBUG_SCAN("Updating '%s' ( %pM) via %s.\n",
-				  escape_essid(target->ssid,
-				  target->ssid_len), target->bssid,
-				  WLAN_FC_GET_STYPE(
-					  le16_to_cpu(beacon->header.frame_ctl)) ==
-				  RTLLIB_STYPE_PROBE_RESP ?
-				  "PROBE RESPONSE" : "BEACON");
+		netdev_dbg(ieee->dev, "Updating '%s' ( %pM) via %s.\n",
+			   escape_essid(target->ssid, target->ssid_len),
+			   target->bssid,
+			   is_beacon(frame_ctl) ? "BEACON" : "PROBE RESPONSE");
 
 		/* we have an entry and we are going to update it. But this
 		 *  entry may be already expired. In this case we do the same
@@ -2609,13 +2699,13 @@ static inline void rtllib_process_probe_response(
 		    network->ssid_len) == 0) &&
 		    (ieee->state == RTLLIB_NOLINK))))
 			renew = 1;
-		update_network(target, network);
+		update_network(ieee, target, network);
 		if (renew && (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE))
 			rtllib_softmac_new_net(ieee, network);
 	}
 
 	spin_unlock_irqrestore(&ieee->lock, flags);
-	if (is_beacon(beacon->header.frame_ctl) &&
+	if (is_beacon(frame_ctl) &&
 	    is_same_network(&ieee->current_network, network,
 	    (network->ssid_len ? 1 : 0)) &&
 	    (ieee->state == RTLLIB_LINKED)) {
@@ -2642,9 +2732,8 @@ void rtllib_rx_mgt(struct rtllib_device *ieee,
 	switch (WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl))) {
 
 	case RTLLIB_STYPE_BEACON:
-		RTLLIB_DEBUG_MGMT("received BEACON (%d)\n",
-				  WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
-		RTLLIB_DEBUG_SCAN("Beacon\n");
+		netdev_dbg(ieee->dev, "received BEACON (%d)\n",
+			   WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
 		rtllib_process_probe_response(
 				ieee, (struct rtllib_probe_response *)header,
 				stats);
@@ -2657,17 +2746,14 @@ void rtllib_rx_mgt(struct rtllib_device *ieee,
 		break;
 
 	case RTLLIB_STYPE_PROBE_RESP:
-		RTLLIB_DEBUG_MGMT("received PROBE RESPONSE (%d)\n",
-			WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
-		RTLLIB_DEBUG_SCAN("Probe response\n");
+		netdev_dbg(ieee->dev, "received PROBE RESPONSE (%d)\n",
+			   WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
 		rtllib_process_probe_response(ieee,
 			      (struct rtllib_probe_response *)header, stats);
 		break;
 	case RTLLIB_STYPE_PROBE_REQ:
-		RTLLIB_DEBUG_MGMT("received PROBE RESQUEST (%d)\n",
-				  WLAN_FC_GET_STYPE(
-					  le16_to_cpu(header->frame_ctl)));
-		RTLLIB_DEBUG_SCAN("Probe request\n");
+		netdev_dbg(ieee->dev, "received PROBE RESQUEST (%d)\n",
+			   WLAN_FC_GET_STYPE(le16_to_cpu(header->frame_ctl)));
 		if ((ieee->softmac_features & IEEE_SOFTMAC_PROBERS) &&
 		    ((ieee->iw_mode == IW_MODE_ADHOC ||
 		    ieee->iw_mode == IW_MODE_MASTER) &&
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index 23b7a4c3b699..d320c31732f2 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -20,6 +20,7 @@
 #include <linux/delay.h>
 #include <linux/uaccess.h>
 #include <linux/etherdevice.h>
+#include <linux/ieee80211.h>
 #include "dot11d.h"
 
 short rtllib_is_54g(struct rtllib_network *net)
@@ -27,11 +28,6 @@ short rtllib_is_54g(struct rtllib_network *net)
 	return (net->rates_ex_len > 0) || (net->rates_len > 4);
 }
 
-short rtllib_is_shortslot(const struct rtllib_network *net)
-{
-	return net->capability & WLAN_CAPABILITY_SHORT_SLOT_TIME;
-}
-
 /* returns the total length needed for placing the RATE MFIE
  * tag and the EXTENDED RATE MFIE tag if needed.
  * It encludes two bytes per tag for the tag itself and its len
@@ -146,21 +142,6 @@ static void enqueue_mgmt(struct rtllib_device *ieee, struct sk_buff *skb)
 
 }
 
-static struct sk_buff *dequeue_mgmt(struct rtllib_device *ieee)
-{
-	struct sk_buff *ret;
-
-	if (ieee->mgmt_queue_tail == ieee->mgmt_queue_head)
-		return NULL;
-
-	ret = ieee->mgmt_queue_ring[ieee->mgmt_queue_tail];
-
-	ieee->mgmt_queue_tail =
-		(ieee->mgmt_queue_tail+1) % MGMT_QUEUE_NUM;
-
-	return ret;
-}
-
 static void init_mgmt_queue(struct rtllib_device *ieee)
 {
 	ieee->mgmt_queue_tail = ieee->mgmt_queue_head = 0;
@@ -271,9 +252,10 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct rtllib_device *ieee)
 			ieee->seq_ctrl[0]++;
 
 		/* check whether the managed packet queued greater than 5 */
-		if (!ieee->check_nic_enough_desc(ieee->dev, tcb_desc->queue_index) ||
-		    (skb_queue_len(&ieee->skb_waitQ[tcb_desc->queue_index]) != 0) ||
-		    (ieee->queue_stop)) {
+		if (!ieee->check_nic_enough_desc(ieee->dev,
+						 tcb_desc->queue_index) ||
+		    skb_queue_len(&ieee->skb_waitQ[tcb_desc->queue_index]) ||
+		    ieee->queue_stop) {
 			/* insert the skb packet to the management queue
 			 *
 			 * as for the completion function, it does not need
@@ -371,7 +353,7 @@ static inline struct sk_buff *rtllib_probe_req(struct rtllib_device *ieee)
 	req->header.duration_id = 0;
 
 	memset(req->header.addr1, 0xff, ETH_ALEN);
-	memcpy(req->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	ether_addr_copy(req->header.addr2, ieee->dev->dev_addr);
 	memset(req->header.addr3, 0xff, ETH_ALEN);
 
 	tag = (u8 *) skb_put(skb, len + 2 + rate_len);
@@ -510,10 +492,6 @@ void rtllib_send_probe_requests(struct rtllib_device *ieee, u8 is_mesh)
 	}
 }
 
-static void rtllib_softmac_hint11d_wq(void *data)
-{
-}
-
 void rtllib_update_active_chan_map(struct rtllib_device *ieee)
 {
 	memcpy(ieee->active_channel_map, GET_DOT11D_INFO(ieee)->channel_map,
@@ -814,9 +792,9 @@ inline struct sk_buff *rtllib_authentication_req(struct rtllib_network *beacon,
 		auth->header.frame_ctl |= cpu_to_le16(RTLLIB_FCTL_WEP);
 
 	auth->header.duration_id = cpu_to_le16(0x013a);
-	memcpy(auth->header.addr1, beacon->bssid, ETH_ALEN);
-	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(auth->header.addr3, beacon->bssid, ETH_ALEN);
+	ether_addr_copy(auth->header.addr1, beacon->bssid);
+	ether_addr_copy(auth->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(auth->header.addr3, beacon->bssid);
 	if (ieee->auth_mode == 0)
 		auth->algorithm = WLAN_AUTH_OPEN;
 	else if (ieee->auth_mode == 1)
@@ -831,7 +809,8 @@ inline struct sk_buff *rtllib_authentication_req(struct rtllib_network *beacon,
 	return skb;
 }
 
-static struct sk_buff *rtllib_probe_resp(struct rtllib_device *ieee, u8 *dest)
+static struct sk_buff *rtllib_probe_resp(struct rtllib_device *ieee,
+					 const u8 *dest)
 {
 	u8 *tag;
 	int beacon_size;
@@ -907,9 +886,9 @@ static struct sk_buff *rtllib_probe_resp(struct rtllib_device *ieee, u8 *dest)
 
 	beacon_buf = (struct rtllib_probe_response *) skb_put(skb,
 		     (beacon_size - ieee->tx_headroom));
-	memcpy(beacon_buf->header.addr1, dest, ETH_ALEN);
-	memcpy(beacon_buf->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(beacon_buf->header.addr3, ieee->current_network.bssid, ETH_ALEN);
+	ether_addr_copy(beacon_buf->header.addr1, dest);
+	ether_addr_copy(beacon_buf->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(beacon_buf->header.addr3, ieee->current_network.bssid);
 
 	beacon_buf->header.duration_id = 0;
 	beacon_buf->beacon_interval =
@@ -1004,9 +983,9 @@ static struct sk_buff *rtllib_assoc_resp(struct rtllib_device *ieee, u8 *dest)
 		skb_put(skb, sizeof(struct rtllib_assoc_response_frame));
 
 	assoc->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_ASSOC_RESP);
-	memcpy(assoc->header.addr1, dest, ETH_ALEN);
-	memcpy(assoc->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(assoc->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	ether_addr_copy(assoc->header.addr1, dest);
+	ether_addr_copy(assoc->header.addr3, ieee->dev->dev_addr);
+	ether_addr_copy(assoc->header.addr2, ieee->dev->dev_addr);
 	assoc->capability = cpu_to_le16(ieee->iw_mode == IW_MODE_MASTER ?
 		WLAN_CAPABILITY_ESS : WLAN_CAPABILITY_IBSS);
 
@@ -1061,9 +1040,9 @@ static struct sk_buff *rtllib_auth_resp(struct rtllib_device *ieee, int status,
 	auth->transaction = cpu_to_le16(2);
 	auth->algorithm = cpu_to_le16(WLAN_AUTH_OPEN);
 
-	memcpy(auth->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(auth->header.addr1, dest, ETH_ALEN);
+	ether_addr_copy(auth->header.addr3, ieee->dev->dev_addr);
+	ether_addr_copy(auth->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(auth->header.addr1, dest);
 	auth->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_AUTH);
 	return skb;
 
@@ -1084,9 +1063,9 @@ static struct sk_buff *rtllib_null_func(struct rtllib_device *ieee, short pwr)
 	hdr = (struct rtllib_hdr_3addr *)skb_put(skb,
 	      sizeof(struct rtllib_hdr_3addr));
 
-	memcpy(hdr->addr1, ieee->current_network.bssid, ETH_ALEN);
-	memcpy(hdr->addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(hdr->addr3, ieee->current_network.bssid, ETH_ALEN);
+	ether_addr_copy(hdr->addr1, ieee->current_network.bssid);
+	ether_addr_copy(hdr->addr2, ieee->dev->dev_addr);
+	ether_addr_copy(hdr->addr3, ieee->current_network.bssid);
 
 	hdr->frame_ctl = cpu_to_le16(RTLLIB_FTYPE_DATA |
 		RTLLIB_STYPE_NULLFUNC | RTLLIB_FCTL_TODS |
@@ -1111,8 +1090,8 @@ static struct sk_buff *rtllib_pspoll_func(struct rtllib_device *ieee)
 	hdr = (struct rtllib_pspoll_hdr *)skb_put(skb,
 	      sizeof(struct rtllib_pspoll_hdr));
 
-	memcpy(hdr->bssid, ieee->current_network.bssid, ETH_ALEN);
-	memcpy(hdr->ta, ieee->dev->dev_addr, ETH_ALEN);
+	ether_addr_copy(hdr->bssid, ieee->current_network.bssid);
+	ether_addr_copy(hdr->ta, ieee->dev->dev_addr);
 
 	hdr->aid = cpu_to_le16(ieee->assoc_id | 0xc000);
 	hdr->frame_ctl = cpu_to_le16(RTLLIB_FTYPE_CTL | RTLLIB_STYPE_PSPOLL |
@@ -1262,13 +1241,13 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 		skb_put(skb, sizeof(struct rtllib_assoc_request_frame) + 2);
 
 
-	hdr->header.frame_ctl = RTLLIB_STYPE_ASSOC_REQ;
+	hdr->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_ASSOC_REQ);
 	hdr->header.duration_id = cpu_to_le16(37);
-	memcpy(hdr->header.addr1, beacon->bssid, ETH_ALEN);
-	memcpy(hdr->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(hdr->header.addr3, beacon->bssid, ETH_ALEN);
+	ether_addr_copy(hdr->header.addr1, beacon->bssid);
+	ether_addr_copy(hdr->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(hdr->header.addr3, beacon->bssid);
 
-	memcpy(ieee->ap_mac_addr, beacon->bssid, ETH_ALEN);
+	ether_addr_copy(ieee->ap_mac_addr, beacon->bssid);
 
 	hdr->capability = cpu_to_le16(WLAN_CAPABILITY_ESS);
 	if (beacon->capability & WLAN_CAPABILITY_PRIVACY)
@@ -1437,10 +1416,10 @@ void rtllib_associate_abort(struct rtllib_device *ieee)
 	 * with, so we retry or just get back to NO_LINK and scanning
 	 */
 	if (ieee->state == RTLLIB_ASSOCIATING_AUTHENTICATING) {
-		RTLLIB_DEBUG_MGMT("Authentication failed\n");
+		netdev_dbg(ieee->dev, "Authentication failed\n");
 		ieee->softmac_stats.no_auth_rs++;
 	} else {
-		RTLLIB_DEBUG_MGMT("Association failed\n");
+		netdev_dbg(ieee->dev, "Association failed\n");
 		ieee->softmac_stats.no_ass_rs++;
 	}
 
@@ -1462,7 +1441,7 @@ static void rtllib_associate_step1(struct rtllib_device *ieee, u8 *daddr)
 	struct rtllib_network *beacon = &ieee->current_network;
 	struct sk_buff *skb;
 
-	RTLLIB_DEBUG_MGMT("Stopping scan\n");
+	netdev_dbg(ieee->dev, "Stopping scan\n");
 
 	ieee->softmac_stats.tx_auth_rq++;
 
@@ -1472,7 +1451,7 @@ static void rtllib_associate_step1(struct rtllib_device *ieee, u8 *daddr)
 		rtllib_associate_abort(ieee);
 	else {
 		ieee->state = RTLLIB_ASSOCIATING_AUTHENTICATING;
-		RTLLIB_DEBUG_MGMT("Sending authentication request\n");
+		netdev_dbg(ieee->dev, "Sending authentication request\n");
 		softmac_mgmt_xmit(skb, ieee);
 		if (!timer_pending(&ieee->associate_timer)) {
 			ieee->associate_timer.expires = jiffies + (HZ / 2);
@@ -1481,7 +1460,8 @@ static void rtllib_associate_step1(struct rtllib_device *ieee, u8 *daddr)
 	}
 }
 
-static void rtllib_auth_challenge(struct rtllib_device *ieee, u8 *challenge, int chlen)
+static void rtllib_auth_challenge(struct rtllib_device *ieee, u8 *challenge,
+				  int chlen)
 {
 	u8 *c;
 	struct sk_buff *skb;
@@ -1500,7 +1480,8 @@ static void rtllib_auth_challenge(struct rtllib_device *ieee, u8 *challenge, int
 		*(c++) = chlen;
 		memcpy(c, challenge, chlen);
 
-		RTLLIB_DEBUG_MGMT("Sending authentication challenge response\n");
+		netdev_dbg(ieee->dev,
+			   "Sending authentication challenge response\n");
 
 		rtllib_encrypt_fragment(ieee, skb,
 					sizeof(struct rtllib_hdr_3addr));
@@ -1518,7 +1499,7 @@ static void rtllib_associate_step2(struct rtllib_device *ieee)
 
 	del_timer_sync(&ieee->associate_timer);
 
-	RTLLIB_DEBUG_MGMT("Sending association request\n");
+	netdev_dbg(ieee->dev, "Sending association request\n");
 
 	ieee->softmac_stats.tx_ass_rq++;
 	skb = rtllib_association_req(beacon, ieee);
@@ -1730,14 +1711,13 @@ inline void rtllib_softmac_new_net(struct rtllib_device *ieee,
 			   !(ieee->softmac_features & IEEE_SOFTMAC_SCAN))
 				rtllib_stop_scan_syncro(ieee);
 
-			ieee->hwscan_ch_bk = ieee->current_network.channel;
 			HTResetIOTSetting(ieee->pHTInfo);
 			ieee->wmm_acm = 0;
 			if (ieee->iw_mode == IW_MODE_INFRA) {
 				/* Join the network for the first time */
 				ieee->AsocRetryCount = 0;
 				if ((ieee->current_network.qos_data.supported == 1) &&
-				   ieee->current_network.bssht.bdSupportHT)
+				    ieee->current_network.bssht.bdSupportHT)
 					HTResetSelfAndSavePeerSetting(ieee,
 						 &(ieee->current_network));
 				else
@@ -1752,14 +1732,19 @@ inline void rtllib_softmac_new_net(struct rtllib_device *ieee,
 					   &ieee->associate_procedure_wq, 0);
 			} else {
 				if (rtllib_is_54g(&ieee->current_network) &&
-					(ieee->modulation & RTLLIB_OFDM_MODULATION)) {
+				    (ieee->modulation &
+				     RTLLIB_OFDM_MODULATION)) {
 					ieee->rate = 108;
-					ieee->SetWirelessMode(ieee->dev, IEEE_G);
-					netdev_info(ieee->dev, "Using G rates\n");
+					ieee->SetWirelessMode(ieee->dev,
+							      IEEE_G);
+					netdev_info(ieee->dev,
+						    "Using G rates\n");
 				} else {
 					ieee->rate = 22;
-					ieee->SetWirelessMode(ieee->dev, IEEE_B);
-					netdev_info(ieee->dev, "Using B rates\n");
+					ieee->SetWirelessMode(ieee->dev,
+							      IEEE_B);
+					netdev_info(ieee->dev,
+						    "Using B rates\n");
 				}
 				memset(ieee->dot11HTOperationalRateSet, 0, 16);
 				ieee->state = RTLLIB_LINKED;
@@ -1791,14 +1776,15 @@ void rtllib_softmac_check_all_nets(struct rtllib_device *ieee)
 	spin_unlock_irqrestore(&ieee->lock, flags);
 }
 
-static inline u16 auth_parse(struct sk_buff *skb, u8 **challenge, int *chlen)
+static inline u16 auth_parse(struct net_device *dev, struct sk_buff *skb,
+			     u8 **challenge, int *chlen)
 {
 	struct rtllib_authentication *a;
 	u8 *t;
 
 	if (skb->len <  (sizeof(struct rtllib_authentication) -
 	    sizeof(struct rtllib_info_element))) {
-		RTLLIB_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
+		netdev_dbg(dev, "invalid len in auth resp: %d\n", skb->len);
 		return 0xcafe;
 	}
 	*challenge = NULL;
@@ -1813,22 +1799,21 @@ static inline u16 auth_parse(struct sk_buff *skb, u8 **challenge, int *chlen)
 				return -ENOMEM;
 		}
 	}
-	return cpu_to_le16(a->status);
+	return le16_to_cpu(a->status);
 }
 
-static int auth_rq_parse(struct sk_buff *skb, u8 *dest)
+static int auth_rq_parse(struct net_device *dev, struct sk_buff *skb, u8 *dest)
 {
 	struct rtllib_authentication *a;
 
 	if (skb->len <  (sizeof(struct rtllib_authentication) -
 	    sizeof(struct rtllib_info_element))) {
-		RTLLIB_DEBUG_MGMT("invalid len in auth request: %d\n",
-				  skb->len);
+		netdev_dbg(dev, "invalid len in auth request: %d\n", skb->len);
 		return -1;
 	}
 	a = (struct rtllib_authentication *) skb->data;
 
-	memcpy(dest, a->header.addr2, ETH_ALEN);
+	ether_addr_copy(dest, a->header.addr2);
 
 	if (le16_to_cpu(a->algorithm) != WLAN_AUTH_OPEN)
 		return  WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG;
@@ -1851,12 +1836,12 @@ static short probe_rq_parse(struct rtllib_device *ieee, struct sk_buff *skb,
 		return -1; /* corrupted */
 
 	bssid_match =
-	  (memcmp(header->addr3, ieee->current_network.bssid, ETH_ALEN) != 0) &&
+	  (!ether_addr_equal(header->addr3, ieee->current_network.bssid)) &&
 	  (!is_broadcast_ether_addr(header->addr3));
 	if (bssid_match)
 		return -1;
 
-	memcpy(src, header->addr2, ETH_ALEN);
+	ether_addr_copy(src, header->addr2);
 
 	skbend = (u8 *)skb->data + skb->len;
 
@@ -1882,20 +1867,19 @@ static short probe_rq_parse(struct rtllib_device *ieee, struct sk_buff *skb,
 	return !strncmp(ssid, ieee->current_network.ssid, ssidlen);
 }
 
-static int assoc_rq_parse(struct sk_buff *skb, u8 *dest)
+static int assoc_rq_parse(struct net_device *dev, struct sk_buff *skb, u8 *dest)
 {
 	struct rtllib_assoc_request_frame *a;
 
 	if (skb->len < (sizeof(struct rtllib_assoc_request_frame) -
 		sizeof(struct rtllib_info_element))) {
-
-		RTLLIB_DEBUG_MGMT("invalid len in auth request:%d\n", skb->len);
+		netdev_dbg(dev, "invalid len in auth request:%d\n", skb->len);
 		return -1;
 	}
 
 	a = (struct rtllib_assoc_request_frame *) skb->data;
 
-	memcpy(dest, a->header.addr2, ETH_ALEN);
+	ether_addr_copy(dest, a->header.addr2);
 
 	return 0;
 }
@@ -1907,7 +1891,8 @@ static inline u16 assoc_parse(struct rtllib_device *ieee, struct sk_buff *skb,
 	u16 status_code;
 
 	if (skb->len <  sizeof(struct rtllib_assoc_response_frame)) {
-		RTLLIB_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
+		netdev_dbg(ieee->dev, "Invalid len in auth resp: %d\n",
+			   skb->len);
 		return 0xcafe;
 	}
 
@@ -1947,7 +1932,7 @@ static inline void rtllib_rx_auth_rq(struct rtllib_device *ieee,
 
 	ieee->softmac_stats.rx_auth_rq++;
 
-	status = auth_rq_parse(skb, dest);
+	status = auth_rq_parse(ieee->dev, skb, dest);
 	if (status != -1)
 		rtllib_resp_to_auth(ieee, status, dest);
 }
@@ -1955,11 +1940,11 @@ static inline void rtllib_rx_auth_rq(struct rtllib_device *ieee,
 static inline void rtllib_rx_assoc_rq(struct rtllib_device *ieee,
 				      struct sk_buff *skb)
 {
-
 	u8 dest[ETH_ALEN];
 
+
 	ieee->softmac_stats.rx_ass_rq++;
-	if (assoc_rq_parse(skb, dest) != -1)
+	if (assoc_rq_parse(ieee->dev, skb, dest) != -1)
 		rtllib_resp_to_assoc_rq(ieee, dest);
 
 	netdev_info(ieee->dev, "New client associated: %pM\n", dest);
@@ -2020,7 +2005,7 @@ static short rtllib_sta_ps_sleep(struct rtllib_device *ieee, u64 *time)
 		if (ieee->bAwakePktSent) {
 			pPSC->LPSAwakeIntvl = 1;
 		} else {
-			u8		MaxPeriod = 1;
+			u8 MaxPeriod = 1;
 
 			if (pPSC->LPSAwakeIntvl == 0)
 				pPSC->LPSAwakeIntvl = 1;
@@ -2191,15 +2176,16 @@ void rtllib_ps_tx_ack(struct rtllib_device *ieee, short success)
 }
 EXPORT_SYMBOL(rtllib_ps_tx_ack);
 
-static void rtllib_process_action(struct rtllib_device *ieee, struct sk_buff *skb)
+static void rtllib_process_action(struct rtllib_device *ieee,
+				  struct sk_buff *skb)
 {
 	struct rtllib_hdr_3addr *header = (struct rtllib_hdr_3addr *) skb->data;
 	u8 *act = rtllib_get_payload((struct rtllib_hdr *)header);
 	u8 category = 0;
 
 	if (act == NULL) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "error to get payload of action frame\n");
+		netdev_warn(ieee->dev,
+			    "Error getting payload of action frame\n");
 		return;
 	}
 
@@ -2232,9 +2218,10 @@ inline int rtllib_rx_assoc_resp(struct rtllib_device *ieee, struct sk_buff *skb,
 	u8 *ies;
 	struct rtllib_assoc_response_frame *assoc_resp;
 	struct rtllib_hdr_3addr *header = (struct rtllib_hdr_3addr *) skb->data;
+	u16 frame_ctl = le16_to_cpu(header->frame_ctl);
 
-	RTLLIB_DEBUG_MGMT("received [RE]ASSOCIATION RESPONSE (%d)\n",
-			  WLAN_FC_GET_STYPE(header->frame_ctl));
+	netdev_dbg(ieee->dev, "received [RE]ASSOCIATION RESPONSE (%d)\n",
+		   WLAN_FC_GET_STYPE(frame_ctl));
 
 	if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
 	     ieee->state == RTLLIB_ASSOCIATING_AUTHENTICATED &&
@@ -2295,9 +2282,6 @@ inline int rtllib_rx_assoc_resp(struct rtllib_device *ieee, struct sk_buff *skb,
 			netdev_info(ieee->dev,
 				    "Association response status code 0x%x\n",
 				    errcode);
-			RTLLIB_DEBUG_MGMT(
-				"Association response status code 0x%x\n",
-				errcode);
 			if (ieee->AsocRetryCount < RT_ASOC_RETRY_LIMIT)
 				queue_delayed_work_rsl(ieee->wq,
 					 &ieee->associate_procedure_wq, 0);
@@ -2315,13 +2299,10 @@ static void rtllib_rx_auth_resp(struct rtllib_device *ieee, struct sk_buff *skb)
 	int chlen = 0;
 	bool bSupportNmode = true, bHalfSupportNmode = false;
 
-	errcode = auth_parse(skb, &challenge, &chlen);
+	errcode = auth_parse(ieee->dev, skb, &challenge, &chlen);
 
 	if (errcode) {
 		ieee->softmac_stats.rx_auth_rs_err++;
-		RTLLIB_DEBUG_MGMT("Authentication respose status code 0x%x",
-				  errcode);
-
 		netdev_info(ieee->dev,
 			    "Authentication respose status code 0x%x", errcode);
 		rtllib_associate_abort(ieee);
@@ -2371,7 +2352,8 @@ inline int rtllib_rx_auth(struct rtllib_device *ieee, struct sk_buff *skb,
 	if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) {
 		if (ieee->state == RTLLIB_ASSOCIATING_AUTHENTICATING &&
 		    (ieee->iw_mode == IW_MODE_INFRA)) {
-			RTLLIB_DEBUG_MGMT("Received authentication response");
+			netdev_dbg(ieee->dev,
+				   "Received authentication response");
 			rtllib_rx_auth_resp(ieee, skb);
 		} else if (ieee->iw_mode == IW_MODE_MASTER) {
 			rtllib_rx_auth_rq(ieee, skb);
@@ -2383,6 +2365,7 @@ inline int rtllib_rx_auth(struct rtllib_device *ieee, struct sk_buff *skb,
 inline int rtllib_rx_deauth(struct rtllib_device *ieee, struct sk_buff *skb)
 {
 	struct rtllib_hdr_3addr *header = (struct rtllib_hdr_3addr *) skb->data;
+	u16 frame_ctl;
 
 	if (memcmp(header->addr3, ieee->current_network.bssid, ETH_ALEN) != 0)
 		return 0;
@@ -2393,9 +2376,10 @@ inline int rtllib_rx_deauth(struct rtllib_device *ieee, struct sk_buff *skb)
 	if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
 	    ieee->state == RTLLIB_LINKED &&
 	    (ieee->iw_mode == IW_MODE_INFRA)) {
+		frame_ctl = le16_to_cpu(header->frame_ctl);
 		netdev_info(ieee->dev,
 			    "==========>received disassoc/deauth(%x) frame, reason code:%x\n",
-			    WLAN_FC_GET_STYPE(header->frame_ctl),
+			    WLAN_FC_GET_STYPE(frame_ctl),
 			    ((struct rtllib_disassoc *)skb->data)->reason);
 		ieee->state = RTLLIB_ASSOCIATING;
 		ieee->softmac_stats.reassoc++;
@@ -2421,11 +2405,13 @@ inline int rtllib_rx_frame_softmac(struct rtllib_device *ieee,
 				   u16 stype)
 {
 	struct rtllib_hdr_3addr *header = (struct rtllib_hdr_3addr *) skb->data;
+	u16 frame_ctl;
 
 	if (!ieee->proto_started)
 		return 0;
 
-	switch (WLAN_FC_GET_STYPE(header->frame_ctl)) {
+	frame_ctl = le16_to_cpu(header->frame_ctl);
+	switch (WLAN_FC_GET_STYPE(frame_ctl)) {
 	case RTLLIB_STYPE_ASSOC_RESP:
 	case RTLLIB_STYPE_REASSOC_RESP:
 		if (rtllib_rx_assoc_resp(ieee, skb, rx_stats) == 1)
@@ -2521,30 +2507,6 @@ void rtllib_softmac_xmit(struct rtllib_txb *txb, struct rtllib_device *ieee)
 
 }
 
-/* called with ieee->lock acquired */
-static void rtllib_resume_tx(struct rtllib_device *ieee)
-{
-	int i;
-
-	for (i = ieee->tx_pending.frag; i < ieee->tx_pending.txb->nr_frags;
-	     i++) {
-
-		if (ieee->queue_stop) {
-			ieee->tx_pending.frag = i;
-			return;
-		}
-
-		ieee->softmac_data_hard_start_xmit(
-			ieee->tx_pending.txb->fragments[i],
-			ieee->dev, ieee->rate);
-		ieee->stats.tx_packets++;
-	}
-
-	rtllib_txb_free(ieee->tx_pending.txb);
-	ieee->tx_pending.txb = NULL;
-}
-
-
 void rtllib_reset_queue(struct rtllib_device *ieee)
 {
 	unsigned long flags;
@@ -2561,59 +2523,6 @@ void rtllib_reset_queue(struct rtllib_device *ieee)
 }
 EXPORT_SYMBOL(rtllib_reset_queue);
 
-void rtllib_wake_queue(struct rtllib_device *ieee)
-{
-
-	unsigned long flags;
-	struct sk_buff *skb;
-	struct rtllib_hdr_3addr  *header;
-
-	spin_lock_irqsave(&ieee->lock, flags);
-	if (!ieee->queue_stop)
-		goto exit;
-
-	ieee->queue_stop = 0;
-
-	if (ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) {
-		while (!ieee->queue_stop && (skb = dequeue_mgmt(ieee))) {
-
-			header = (struct rtllib_hdr_3addr  *) skb->data;
-
-			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
-
-			if (ieee->seq_ctrl[0] == 0xFFF)
-				ieee->seq_ctrl[0] = 0;
-			else
-				ieee->seq_ctrl[0]++;
-
-			ieee->softmac_data_hard_start_xmit(skb, ieee->dev,
-							   ieee->basic_rate);
-		}
-	}
-	if (!ieee->queue_stop && ieee->tx_pending.txb)
-		rtllib_resume_tx(ieee);
-
-	if (!ieee->queue_stop && netif_queue_stopped(ieee->dev)) {
-		ieee->softmac_stats.swtxawake++;
-		netif_wake_queue(ieee->dev);
-	}
-
-exit:
-	spin_unlock_irqrestore(&ieee->lock, flags);
-}
-
-
-void rtllib_stop_queue(struct rtllib_device *ieee)
-{
-
-	if (!netif_queue_stopped(ieee->dev)) {
-		netif_stop_queue(ieee->dev);
-		ieee->softmac_stats.swtxstop++;
-	}
-	ieee->queue_stop = 1;
-
-}
-
 void rtllib_stop_all_queues(struct rtllib_device *ieee)
 {
 	unsigned int i;
@@ -2650,7 +2559,7 @@ void rtllib_start_master_bss(struct rtllib_device *ieee)
 		ieee->ssid_set = 1;
 	}
 
-	memcpy(ieee->current_network.bssid, ieee->dev->dev_addr, ETH_ALEN);
+	ether_addr_copy(ieee->current_network.bssid, ieee->dev->dev_addr);
 
 	ieee->set_chan(ieee->dev, ieee->current_network.channel);
 	ieee->state = RTLLIB_LINKED;
@@ -2910,7 +2819,7 @@ exit:
 
 struct sk_buff *rtllib_get_beacon_(struct rtllib_device *ieee)
 {
-	u8 broadcast_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	const u8 broadcast_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	struct sk_buff *skb;
 	struct rtllib_probe_response *b;
@@ -2983,7 +2892,7 @@ void rtllib_stop_protocol(struct rtllib_device *ieee, u8 shutdown)
 
 	if (ieee->state == RTLLIB_LINKED) {
 		if (ieee->iw_mode == IW_MODE_INFRA)
-			SendDisassociation(ieee, 1, deauth_lv_ss);
+			SendDisassociation(ieee, 1, WLAN_REASON_DEAUTH_LEAVING);
 		rtllib_disassociate(ieee);
 	}
 
@@ -3072,7 +2981,7 @@ void rtllib_softmac_init(struct rtllib_device *ieee)
 		ieee->seq_ctrl[i] = 0;
 	ieee->pDot11dInfo = kzalloc(sizeof(struct rt_dot11d_info), GFP_ATOMIC);
 	if (!ieee->pDot11dInfo)
-		RTLLIB_DEBUG(RTLLIB_DL_ERR, "can't alloc memory for DOT11D\n");
+		netdev_err(ieee->dev, "Can't alloc memory for DOT11D\n");
 	ieee->LinkDetectInfo.SlotIndex = 0;
 	ieee->LinkDetectInfo.SlotNum = 2;
 	ieee->LinkDetectInfo.NumRecvBcnInPeriod = 0;
@@ -3108,19 +3017,13 @@ void rtllib_softmac_init(struct rtllib_device *ieee)
 	ieee->is_set_key = false;
 	init_mgmt_queue(ieee);
 
-	ieee->sta_edca_param[0] = 0x0000A403;
-	ieee->sta_edca_param[1] = 0x0000A427;
-	ieee->sta_edca_param[2] = 0x005E4342;
-	ieee->sta_edca_param[3] = 0x002F3262;
-	ieee->aggregation = true;
-	ieee->enable_rx_imm_BA = true;
 	ieee->tx_pending.txb = NULL;
 
-	_setup_timer(&ieee->associate_timer,
+	setup_timer(&ieee->associate_timer,
 		    rtllib_associate_abort_cb,
 		    (unsigned long) ieee);
 
-	_setup_timer(&ieee->beacon_timer,
+	setup_timer(&ieee->beacon_timer,
 		    rtllib_send_beacon_cb,
 		    (unsigned long) ieee);
 
@@ -3137,8 +3040,6 @@ void rtllib_softmac_init(struct rtllib_device *ieee)
 			      (void *)rtllib_associate_procedure_wq, ieee);
 	INIT_DELAYED_WORK_RSL(&ieee->softmac_scan_wq,
 			      (void *)rtllib_softmac_scan_wq, ieee);
-	INIT_DELAYED_WORK_RSL(&ieee->softmac_hint11d_wq,
-			      (void *)rtllib_softmac_hint11d_wq, ieee);
 	INIT_DELAYED_WORK_RSL(&ieee->associate_retry_wq,
 			      (void *)rtllib_associate_retry_wq, ieee);
 	INIT_WORK_RSL(&ieee->wx_sync_scan_wq, (void *)rtllib_wx_sync_scan_wq,
@@ -3517,9 +3418,9 @@ inline struct sk_buff *rtllib_disauth_skb(struct rtllib_network *beacon,
 	disauth->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_DEAUTH);
 	disauth->header.duration_id = 0;
 
-	memcpy(disauth->header.addr1, beacon->bssid, ETH_ALEN);
-	memcpy(disauth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(disauth->header.addr3, beacon->bssid, ETH_ALEN);
+	ether_addr_copy(disauth->header.addr1, beacon->bssid);
+	ether_addr_copy(disauth->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(disauth->header.addr3, beacon->bssid);
 
 	disauth->reason = cpu_to_le16(asRsn);
 	return skb;
@@ -3544,9 +3445,9 @@ inline struct sk_buff *rtllib_disassociate_skb(struct rtllib_network *beacon,
 	disass->header.frame_ctl = cpu_to_le16(RTLLIB_STYPE_DISASSOC);
 	disass->header.duration_id = 0;
 
-	memcpy(disass->header.addr1, beacon->bssid, ETH_ALEN);
-	memcpy(disass->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
-	memcpy(disass->header.addr3, beacon->bssid, ETH_ALEN);
+	ether_addr_copy(disass->header.addr1, beacon->bssid);
+	ether_addr_copy(disass->header.addr2, ieee->dev->dev_addr);
+	ether_addr_copy(disass->header.addr3, beacon->bssid);
 
 	disass->reason = cpu_to_le16(asRsn);
 	return skb;
@@ -3676,8 +3577,8 @@ static void rtllib_MgntDisconnectIBSS(struct rtllib_device *rtllib)
 
 }
 
-static void rtllib_MlmeDisassociateRequest(struct rtllib_device *rtllib, u8 *asSta,
-				    u8 asRsn)
+static void rtllib_MlmeDisassociateRequest(struct rtllib_device *rtllib,
+					   u8 *asSta, u8 asRsn)
 {
 	u8 i;
 	u8	OpMode;
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
index 9715a793fd37..86f52ac7d33e 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -160,7 +160,7 @@ int rtllib_wx_set_wap(struct rtllib_device *ieee,
 
 	if (is_zero_ether_addr(temp->sa_data)) {
 		spin_lock_irqsave(&ieee->lock, flags);
-		memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
+		ether_addr_copy(ieee->current_network.bssid, temp->sa_data);
 		ieee->wap_set = 0;
 		spin_unlock_irqrestore(&ieee->lock, flags);
 		ret = -1;
@@ -177,7 +177,7 @@ int rtllib_wx_set_wap(struct rtllib_device *ieee,
 	spin_lock_irqsave(&ieee->lock, flags);
 
 	ieee->cannot_notify = false;
-	memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
+	ether_addr_copy(ieee->current_network.bssid, temp->sa_data);
 	ieee->wap_set = !is_zero_ether_addr(temp->sa_data);
 
 	spin_unlock_irqrestore(&ieee->lock, flags);
@@ -454,13 +454,7 @@ int rtllib_wx_set_essid(struct rtllib_device *ieee,
 
 	proto_started = ieee->proto_started;
 
-	len = (wrqu->essid.length < IW_ESSID_MAX_SIZE) ? wrqu->essid.length :
-	       IW_ESSID_MAX_SIZE;
-
-	if (len > IW_ESSID_MAX_SIZE) {
-		ret = -E2BIG;
-		goto out;
-	}
+	len = min_t(__u16, wrqu->essid.length, IW_ESSID_MAX_SIZE);
 
 	if (ieee->iw_mode == IW_MODE_MONITOR) {
 		ret = -1;
@@ -575,9 +569,9 @@ int rtllib_wx_set_power(struct rtllib_device *ieee,
 	if ((!ieee->sta_wake_up) ||
 	    (!ieee->enter_sleep_state) ||
 	    (!ieee->ps_is_queue_empty)) {
-		RTLLIB_DEBUG(RTLLIB_DL_ERR,
-			     "%s(): PS mode is tried to be use but driver missed a callback\n\n",
-			     __func__);
+		netdev_warn(ieee->dev,
+			    "%s(): PS mode is tried to be use but driver missed a callback\n",
+			    __func__);
 		return -1;
 	}
 
diff --git a/drivers/staging/rtl8192e/rtllib_tx.c b/drivers/staging/rtl8192e/rtllib_tx.c
index 3b159638bba2..e99ea5e67ef9 100644
--- a/drivers/staging/rtl8192e/rtllib_tx.c
+++ b/drivers/staging/rtl8192e/rtllib_tx.c
@@ -57,18 +57,19 @@
  *
  *
  * 802.11 frame_control for data frames - 2 bytes
- *      ,-----------------------------------------------------------------------------------------.
- * bits | 0  |  1  |  2  |  3  |  4  |  5  |  6  |  7  |  8  |  9  |  a  |  b  |  c  |  d  |  e   |
- *      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
- * val  | 0  |  0  |  0  |  1  |  x  |  0  |  0  |  0  |  1  |  0  |  x  |  x  |  x  |  x  |  x   |
- *      |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
- * desc | ^-ver-^  |  ^type-^  |  ^-----subtype-----^  | to  |from |more |retry| pwr |more |wep   |
- *      |          |           | x=0 data,x=1 data+ack | DS  | DS  |frag |     | mgm |data |      |
- *      '-----------------------------------------------------------------------------------------'
- *                                                   /\
- *                                                   |
- * 802.11 Data Frame                                 |
- *          ,--------- 'ctrl' expands to >-----------'
+ *      ,--------------------------------------------------------------------.
+ * bits | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 |  9 |  a |  b  |  c  |  d  | e  |
+ *      |---|---|---|---|---|---|---|---|---|----|----|-----|-----|-----|----|
+ * val  | 0 | 0 | 0 | 1 | x | 0 | 0 | 0 | 1 |  0 |  x |  x  |  x  |  x  | x  |
+ *      |---|---|---|---|---|---|---|---|---|----|----|-----|-----|-----|----|
+ * desc |  ver  | type  |  ^-subtype-^  |to |from|more|retry| pwr |more |wep |
+ *      |       |       | x=0 data      |DS | DS |frag|     | mgm |data |    |
+ *      |       |       | x=1 data+ack  |   |    |    |     |     |     |    |
+ *      '--------------------------------------------------------------------'
+ *                                           /\
+ *                                           |
+ * 802.11 Data Frame                         |
+ *          ,--------- 'ctrl' expands to >---'
  *          |
  *       ,--'---,-------------------------------------------------------------.
  * Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
@@ -112,15 +113,15 @@
  *       `-----------------------------------------'
  * Total: 18 non-data bytes
  *
- * In the event that fragmentation is required, the incoming payload is split into
- * N parts of size ieee->fts.  The first fragment contains the SNAP header and the
- * remaining packets are just data.
+ * In the event that fragmentation is required, the incoming payload is split
+ * into N parts of size ieee->fts.  The first fragment contains the SNAP header
+ * and the remaining packets are just data.
  *
- * If encryption is enabled, each fragment payload size is reduced by enough space
- * to add the prefix and postfix (IV and ICV totalling 8 bytes in the case of WEP)
- * So if you have 1500 bytes of payload with ieee->fts set to 500 without
- * encryption it will take 3 frames.  With WEP it will take 4 frames as the
- * payload of each frame is reduced to 492 bytes.
+ * If encryption is enabled, each fragment payload size is reduced by enough
+ * space to add the prefix and postfix (IV and ICV totalling 8 bytes in
+ * the case of WEP) So if you have 1500 bytes of payload with ieee->fts set to
+ * 500 without encryption it will take 3 frames.  With WEP it will take 4 frames
+ * as the payload of each frame is reduced to 492 bytes.
  *
  * SKB visualization
  *
@@ -260,7 +261,10 @@ static int rtllib_classify(struct sk_buff *skb, u8 bIsAmsdu)
 	if (eth->h_proto != htons(ETH_P_IP))
 		return 0;
 
-	RTLLIB_DEBUG_DATA(RTLLIB_DL_DATA, skb->data, skb->len);
+#ifdef VERBOSE_DEBUG
+	print_hex_dump_bytes("rtllib_classify(): ", DUMP_PREFIX_NONE, skb->data,
+			     skb->len);
+#endif
 	ip = ip_hdr(skb);
 	switch (ip->tos & 0xfc) {
 	case 0x20:
@@ -579,8 +583,9 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 		.seq_ctl = 0,
 		.qos_ctl = 0
 	};
-	u8 dest[ETH_ALEN], src[ETH_ALEN];
 	int qos_actived = ieee->current_network.qos_data.active;
+	u8 dest[ETH_ALEN];
+	u8 src[ETH_ALEN];
 	struct lib80211_crypt_data *crypt = NULL;
 	struct cb_desc *tcb_desc;
 	u8 bIsMulticast = false;
@@ -608,8 +613,8 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 			goto success;
 		}
 		/* Save source and destination addresses */
-		memcpy(dest, skb->data, ETH_ALEN);
-		memcpy(src, skb->data+ETH_ALEN, ETH_ALEN);
+		ether_addr_copy(dest, skb->data);
+		ether_addr_copy(src, skb->data + ETH_ALEN);
 
 		memset(skb->cb, 0, sizeof(skb->cb));
 		ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
@@ -669,8 +674,9 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 			struct eapol *eap = (struct eapol *)(skb->data +
 				sizeof(struct ethhdr) - SNAP_SIZE -
 				sizeof(u16));
-			RTLLIB_DEBUG_EAP("TX: IEEE 802.11 EAPOL frame: %s\n",
-				eap_get_type(eap->type));
+			netdev_dbg(ieee->dev,
+				   "TX: IEEE 802.11 EAPOL frame: %s\n",
+				   eap_get_type(eap->type));
 		}
 
 		/* Advance the SKB to the start of the payload */
@@ -694,22 +700,22 @@ int rtllib_xmit_inter(struct sk_buff *skb, struct net_device *dev)
 			/* To DS: Addr1 = BSSID, Addr2 = SA,
 			 * Addr3 = DA
 			 */
-			memcpy(&header.addr1, ieee->current_network.bssid,
-			       ETH_ALEN);
-			memcpy(&header.addr2, &src, ETH_ALEN);
+			ether_addr_copy(header.addr1,
+					ieee->current_network.bssid);
+			ether_addr_copy(header.addr2, src);
 			if (IsAmsdu)
-				memcpy(&header.addr3,
-				       ieee->current_network.bssid, ETH_ALEN);
+				ether_addr_copy(header.addr3,
+						ieee->current_network.bssid);
 			else
-				memcpy(&header.addr3, &dest, ETH_ALEN);
+				ether_addr_copy(header.addr3, dest);
 		} else if (ieee->iw_mode == IW_MODE_ADHOC) {
 			/* not From/To DS: Addr1 = DA, Addr2 = SA,
 			 * Addr3 = BSSID
 			 */
-			memcpy(&header.addr1, dest, ETH_ALEN);
-			memcpy(&header.addr2, src, ETH_ALEN);
-			memcpy(&header.addr3, ieee->current_network.bssid,
-			       ETH_ALEN);
+			ether_addr_copy(header.addr1, dest);
+			ether_addr_copy(header.addr2, src);
+			ether_addr_copy(header.addr3,
+					ieee->current_network.bssid);
 		}
 
 		bIsMulticast = is_multicast_ether_addr(header.addr1);
diff --git a/drivers/staging/rtl8192e/rtllib_wx.c b/drivers/staging/rtl8192e/rtllib_wx.c
index 6234aae5b069..967ba8bc80bb 100644
--- a/drivers/staging/rtl8192e/rtllib_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_wx.c
@@ -266,7 +266,7 @@ int rtllib_wx_get_scan(struct rtllib_device *ieee,
 	int i = 0;
 	int err = 0;
 
-	RTLLIB_DEBUG_WX("Getting scan\n");
+	netdev_dbg(ieee->dev, "Getting scan\n");
 	down(&ieee->wx_sem);
 	spin_lock_irqsave(&ieee->lock, flags);
 
@@ -281,11 +281,13 @@ int rtllib_wx_get_scan(struct rtllib_device *ieee,
 			ev = rtl819x_translate_scan(ieee, ev, stop, network,
 						    info);
 		else
-			RTLLIB_DEBUG_SCAN("Not showing network '%s ( %pM)' due to age (%lums).\n",
-				escape_essid(network->ssid,
-					     network->ssid_len),
-				network->bssid,
-				(jiffies - network->last_scanned) / (HZ / 100));
+			netdev_dbg(ieee->dev,
+				   "Network '%s ( %pM)' hidden due to age (%lums).\n",
+				   escape_essid(network->ssid,
+						network->ssid_len),
+				   network->bssid,
+				   (jiffies - network->last_scanned) /
+				   (HZ / 100));
 	}
 
 	spin_unlock_irqrestore(&ieee->lock, flags);
@@ -293,7 +295,7 @@ int rtllib_wx_get_scan(struct rtllib_device *ieee,
 	wrqu->data.length = ev -  extra;
 	wrqu->data.flags = 0;
 
-	RTLLIB_DEBUG_WX("exit: %d networks returned.\n", i);
+	netdev_dbg(ieee->dev, "%s(): %d networks returned.\n", __func__, i);
 
 	return err;
 }
@@ -311,7 +313,7 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 	int i, key, key_provided, len;
 	struct lib80211_crypt_data **crypt;
 
-	RTLLIB_DEBUG_WX("SET_ENCODE\n");
+	netdev_dbg(ieee->dev, "%s()\n", __func__);
 
 	key = erq->flags & IW_ENCODE_INDEX;
 	if (key) {
@@ -324,16 +326,16 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 		key = ieee->crypt_info.tx_keyidx;
 	}
 
-	RTLLIB_DEBUG_WX("Key: %d [%s]\n", key, key_provided ?
+	netdev_dbg(ieee->dev, "Key: %d [%s]\n", key, key_provided ?
 			   "provided" : "default");
 	crypt = &ieee->crypt_info.crypt[key];
 	if (erq->flags & IW_ENCODE_DISABLED) {
 		if (key_provided && *crypt) {
-			RTLLIB_DEBUG_WX("Disabling encryption on key %d.\n",
-					   key);
+			netdev_dbg(ieee->dev,
+				   "Disabling encryption on key %d.\n", key);
 			lib80211_crypt_delayed_deinit(&ieee->crypt_info, crypt);
 		} else
-			RTLLIB_DEBUG_WX("Disabling encryption.\n");
+			netdev_dbg(ieee->dev, "Disabling encryption.\n");
 
 		/* Check all the keys to see if any are still configured,
 		 * and if no key index was provided, de-init them all
@@ -405,9 +407,9 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 		if (len > erq->length)
 			memset(sec.keys[key] + erq->length, 0,
 			       len - erq->length);
-		RTLLIB_DEBUG_WX("Setting key %d to '%s' (%d:%d bytes)\n",
-				   key, escape_essid(sec.keys[key], len),
-				   erq->length, len);
+		netdev_dbg(ieee->dev, "Setting key %d to '%s' (%d:%d bytes)\n",
+			   key, escape_essid(sec.keys[key], len), erq->length,
+			   len);
 		sec.key_sizes[key] = len;
 		(*crypt)->ops->set_key(sec.keys[key], len, NULL,
 				       (*crypt)->priv);
@@ -436,8 +438,8 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 
 		/* No key data - just set the default TX key index */
 		if (key_provided) {
-			RTLLIB_DEBUG_WX("Setting key %d to default Tx key.\n",
-					key);
+			netdev_dbg(ieee->dev,
+				   "Setting key %d as default Tx key.\n", key);
 			ieee->crypt_info.tx_keyidx = key;
 			sec.active_key = key;
 			sec.flags |= SEC_ACTIVE_KEY;
@@ -449,7 +451,7 @@ int rtllib_wx_set_encode(struct rtllib_device *ieee,
 			  WLAN_AUTH_SHARED_KEY;
 	sec.auth_mode = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
 	sec.flags |= SEC_AUTH_MODE;
-	RTLLIB_DEBUG_WX("Auth: %s\n", sec.auth_mode == WLAN_AUTH_OPEN ?
+	netdev_dbg(ieee->dev, "Auth: %s\n", sec.auth_mode == WLAN_AUTH_OPEN ?
 			   "OPEN" : "SHARED KEY");
 
 	/* For now we just support WEP, so only set that security level...
@@ -485,7 +487,7 @@ int rtllib_wx_get_encode(struct rtllib_device *ieee,
 	int len, key;
 	struct lib80211_crypt_data *crypt;
 
-	RTLLIB_DEBUG_WX("GET_ENCODE\n");
+	netdev_dbg(ieee->dev, "%s()\n", __func__);
 
 	if (ieee->iw_mode == IW_MODE_MONITOR)
 		return -1;
@@ -508,7 +510,8 @@ int rtllib_wx_get_encode(struct rtllib_device *ieee,
 		return 0;
 	}
 	len = crypt->ops->get_key(keybuf, SCM_KEY_LEN, NULL, crypt->priv);
-	erq->length = (len >= 0 ? len : 0);
+
+	erq->length = max(len, 0);
 
 	erq->flags |= IW_ENCODE_ENABLED;
 
@@ -592,8 +595,7 @@ int rtllib_wx_set_encode_ext(struct rtllib_device *ieee,
 		module = "rtllib_crypt_ccmp";
 		break;
 	default:
-		RTLLIB_DEBUG_WX("%s: unknown crypto alg %d\n",
-				   dev->name, ext->alg);
+		netdev_dbg(ieee->dev, "Unknown crypto alg %d\n", ext->alg);
 		ret = -EINVAL;
 		goto done;
 	}
@@ -673,70 +675,13 @@ done:
 	 if (ieee->reset_on_keychange &&
 	    ieee->iw_mode != IW_MODE_INFRA &&
 	    ieee->reset_port && ieee->reset_port(dev)) {
-		RTLLIB_DEBUG_WX("%s: reset_port failed\n", dev->name);
+		netdev_dbg(ieee->dev, "Port reset failed\n");
 		return -EINVAL;
 	}
 	return ret;
 }
 EXPORT_SYMBOL(rtllib_wx_set_encode_ext);
 
-int rtllib_wx_get_encode_ext(struct rtllib_device *ieee,
-			       struct iw_request_info *info,
-			       union iwreq_data *wrqu, char *extra)
-{
-	struct iw_point *encoding = &wrqu->encoding;
-	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
-	struct lib80211_crypt_data *crypt;
-	int idx, max_key_len;
-
-	max_key_len = encoding->length - sizeof(*ext);
-	if (max_key_len < 0)
-		return -EINVAL;
-
-	idx = encoding->flags & IW_ENCODE_INDEX;
-	if (idx) {
-		if (idx < 1 || idx > NUM_WEP_KEYS)
-			return -EINVAL;
-		idx--;
-	} else {
-		idx = ieee->crypt_info.tx_keyidx;
-	}
-	if (!(ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) &&
-	    (ext->alg != IW_ENCODE_ALG_WEP))
-		if (idx != 0 || (ieee->iw_mode != IW_MODE_INFRA))
-			return -EINVAL;
-
-	crypt = ieee->crypt_info.crypt[idx];
-
-	encoding->flags = idx + 1;
-	memset(ext, 0, sizeof(*ext));
-
-	if (crypt == NULL || crypt->ops == NULL) {
-		ext->alg = IW_ENCODE_ALG_NONE;
-		ext->key_len = 0;
-		encoding->flags |= IW_ENCODE_DISABLED;
-	} else {
-		if (strcmp(crypt->ops->name, "R-WEP") == 0)
-			ext->alg = IW_ENCODE_ALG_WEP;
-		else if (strcmp(crypt->ops->name, "R-TKIP"))
-			ext->alg = IW_ENCODE_ALG_TKIP;
-		else if (strcmp(crypt->ops->name, "R-CCMP"))
-			ext->alg = IW_ENCODE_ALG_CCMP;
-		else
-			return -EINVAL;
-		ext->key_len = crypt->ops->get_key(ext->key, SCM_KEY_LEN,
-						   NULL, crypt->priv);
-		encoding->flags |= IW_ENCODE_ENABLED;
-		if (ext->key_len &&
-		    (ext->alg == IW_ENCODE_ALG_TKIP ||
-		     ext->alg == IW_ENCODE_ALG_CCMP))
-			ext->ext_flags |= IW_ENCODE_EXT_TX_SEQ_VALID;
-
-	}
-
-	return 0;
-}
-
 int rtllib_wx_set_mlme(struct rtllib_device *ieee,
 			       struct iw_request_info *info,
 			       union iwreq_data *wrqu, char *extra)
@@ -850,8 +795,7 @@ int rtllib_wx_set_gen_ie(struct rtllib_device *ieee, u8 *ie, size_t len)
 		if ((eid == MFIE_TYPE_GENERIC) && (!memcmp(&ie[2],
 		     wps_oui, 4))) {
 
-			ieee->wps_ie_len = (len < MAX_WZC_IE_LEN) ? (len) :
-					   (MAX_WZC_IE_LEN);
+			ieee->wps_ie_len = min_t(size_t, len, MAX_WZC_IE_LEN);
 			buf = kmemdup(ie, ieee->wps_ie_len, GFP_KERNEL);
 			if (buf == NULL)
 				return -ENOMEM;
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
index 0f53c6a97578..23af2aad458e 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -24,7 +24,7 @@
 #ifndef IEEE80211_H
 #define IEEE80211_H
 #include <linux/if_ether.h> /* ETH_ALEN */
-#include <linux/kernel.h>   /* ARRAY_SIZE */
+#include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/jiffies.h>
 #include <linux/timer.h>
@@ -34,6 +34,7 @@
 
 #include <linux/delay.h>
 #include <linux/wireless.h>
+#include <linux/ieee80211.h>
 
 #include "rtl819x_HT.h"
 #include "rtl819x_BA.h"
@@ -48,21 +49,6 @@
 #define IWEVCUSTOM 0x8c02
 #endif
 
-
-#ifndef container_of
-/**
- * container_of - cast a member of a structure out to the containing structure
- *
- * @ptr:        the pointer to the member.
- * @type:       the type of the container struct this is embedded in.
- * @member:     the name of the member within the struct.
- *
- */
-#define container_of(ptr, type, member) ({                      \
-	const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
-	(type *)( (char *)__mptr - offsetof(type,member) );})
-#endif
-
 #define KEY_TYPE_NA		0x0
 #define KEY_TYPE_WEP40		0x1
 #define KEY_TYPE_TKIP		0x2
@@ -187,54 +173,6 @@ typedef struct cb_desc {
 #define MGN_MCS14               0x8e
 #define MGN_MCS15               0x8f
 
-//----------------------------------------------------------------------------
-//		802.11 Management frame Reason Code field
-//----------------------------------------------------------------------------
-enum	_ReasonCode{
-	unspec_reason	= 0x1,
-	auth_not_valid	= 0x2,
-	deauth_lv_ss	= 0x3,
-	inactivity		= 0x4,
-	ap_overload	= 0x5,
-	class2_err		= 0x6,
-	class3_err		= 0x7,
-	disas_lv_ss	= 0x8,
-	asoc_not_auth	= 0x9,
-
-	//----MIC_CHECK
-	mic_failure	= 0xe,
-	//----END MIC_CHECK
-
-	// Reason code defined in 802.11i D10.0 p.28.
-	invalid_IE		= 0x0d,
-	four_way_tmout	= 0x0f,
-	two_way_tmout	= 0x10,
-	IE_dismatch	= 0x11,
-	invalid_Gcipher = 0x12,
-	invalid_Pcipher = 0x13,
-	invalid_AKMP	= 0x14,
-	unsup_RSNIEver = 0x15,
-	invalid_RSNIE	= 0x16,
-	auth_802_1x_fail= 0x17,
-	ciper_reject		= 0x18,
-
-	// Reason code defined in 7.3.1.7, 802.1e D13.0, p.42. Added by Annie, 2005-11-15.
-	QoS_unspec		= 0x20, // 32
-	QAP_bandwidth	= 0x21, // 33
-	poor_condition	= 0x22, // 34
-	no_facility	= 0x23, // 35
-							// Where is 36???
-	req_declined	= 0x25, // 37
-	invalid_param	= 0x26, // 38
-	req_not_honored= 0x27,	// 39
-	TS_not_created	= 0x2F, // 47
-	DL_not_allowed	= 0x30, // 48
-	dest_not_exist	= 0x31, // 49
-	dest_not_QSTA	= 0x32, // 50
-};
-
-
-
 #define aSifsTime ((priv->ieee80211->current_network.mode == IEEE_A || \
 		    priv->ieee80211->current_network.mode == IEEE_N_24G || \
 		    priv->ieee80211->current_network.mode == IEEE_N_5G) ? \
@@ -646,13 +584,6 @@ struct ieee80211_snap_hdr {
 #define WLAN_GET_SEQ_FRAG(seq) ((seq) & IEEE80211_SCTL_FRAG)
 #define WLAN_GET_SEQ_SEQ(seq)  (((seq) & IEEE80211_SCTL_SEQ) >> 4)
 
-/* Authentication algorithms */
-#define WLAN_AUTH_OPEN 0
-#define WLAN_AUTH_SHARED_KEY 1
-#define WLAN_AUTH_LEAP 2
-
-#define WLAN_AUTH_CHALLENGE_LEN 128
-
 #define WLAN_CAPABILITY_BSS (1<<0)
 #define WLAN_CAPABILITY_IBSS (1<<1)
 #define WLAN_CAPABILITY_CF_POLLABLE (1<<2)
@@ -671,69 +602,6 @@ struct ieee80211_snap_hdr {
 #define WLAN_ERP_USE_PROTECTION (1<<1)
 #define WLAN_ERP_BARKER_PREAMBLE (1<<2)
 
-/* Status codes */
-enum ieee80211_statuscode {
-	WLAN_STATUS_SUCCESS = 0,
-	WLAN_STATUS_UNSPECIFIED_FAILURE = 1,
-	WLAN_STATUS_CAPS_UNSUPPORTED = 10,
-	WLAN_STATUS_REASSOC_NO_ASSOC = 11,
-	WLAN_STATUS_ASSOC_DENIED_UNSPEC = 12,
-	WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG = 13,
-	WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION = 14,
-	WLAN_STATUS_CHALLENGE_FAIL = 15,
-	WLAN_STATUS_AUTH_TIMEOUT = 16,
-	WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA = 17,
-	WLAN_STATUS_ASSOC_DENIED_RATES = 18,
-	/* 802.11b */
-	WLAN_STATUS_ASSOC_DENIED_NOSHORTPREAMBLE = 19,
-	WLAN_STATUS_ASSOC_DENIED_NOPBCC = 20,
-	WLAN_STATUS_ASSOC_DENIED_NOAGILITY = 21,
-	/* 802.11h */
-	WLAN_STATUS_ASSOC_DENIED_NOSPECTRUM = 22,
-	WLAN_STATUS_ASSOC_REJECTED_BAD_POWER = 23,
-	WLAN_STATUS_ASSOC_REJECTED_BAD_SUPP_CHAN = 24,
-	/* 802.11g */
-	WLAN_STATUS_ASSOC_DENIED_NOSHORTTIME = 25,
-	WLAN_STATUS_ASSOC_DENIED_NODSSSOFDM = 26,
-	/* 802.11i */
-	WLAN_STATUS_INVALID_IE = 40,
-	WLAN_STATUS_INVALID_GROUP_CIPHER = 41,
-	WLAN_STATUS_INVALID_PAIRWISE_CIPHER = 42,
-	WLAN_STATUS_INVALID_AKMP = 43,
-	WLAN_STATUS_UNSUPP_RSN_VERSION = 44,
-	WLAN_STATUS_INVALID_RSN_IE_CAP = 45,
-	WLAN_STATUS_CIPHER_SUITE_REJECTED = 46,
-};
-
-/* Reason codes */
-enum ieee80211_reasoncode {
-	WLAN_REASON_UNSPECIFIED = 1,
-	WLAN_REASON_PREV_AUTH_NOT_VALID = 2,
-	WLAN_REASON_DEAUTH_LEAVING = 3,
-	WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY = 4,
-	WLAN_REASON_DISASSOC_AP_BUSY = 5,
-	WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA = 6,
-	WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA = 7,
-	WLAN_REASON_DISASSOC_STA_HAS_LEFT = 8,
-	WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH = 9,
-	/* 802.11h */
-	WLAN_REASON_DISASSOC_BAD_POWER = 10,
-	WLAN_REASON_DISASSOC_BAD_SUPP_CHAN = 11,
-	/* 802.11i */
-	WLAN_REASON_INVALID_IE = 13,
-	WLAN_REASON_MIC_FAILURE = 14,
-	WLAN_REASON_4WAY_HANDSHAKE_TIMEOUT = 15,
-	WLAN_REASON_GROUP_KEY_HANDSHAKE_TIMEOUT = 16,
-	WLAN_REASON_IE_DIFFERENT = 17,
-	WLAN_REASON_INVALID_GROUP_CIPHER = 18,
-	WLAN_REASON_INVALID_PAIRWISE_CIPHER = 19,
-	WLAN_REASON_INVALID_AKMP = 20,
-	WLAN_REASON_UNSUPP_RSN_VERSION = 21,
-	WLAN_REASON_INVALID_RSN_IE_CAP = 22,
-	WLAN_REASON_IEEE8021X_FAILED = 23,
-	WLAN_REASON_CIPHER_SUITE_REJECTED = 24,
-};
-
 #define IEEE80211_STATMASK_SIGNAL (1<<0)
 #define IEEE80211_STATMASK_RSSI (1<<1)
 #define IEEE80211_STATMASK_NOISE (1<<2)
@@ -961,10 +829,10 @@ struct ieee80211_device;
 struct ieee80211_security {
 	u16 active_key:2,
 	    enabled:1,
-	    auth_mode:2,
 	    auth_algo:4,
 	    unicast_uses_group:1,
 	    encrypt:1;
+	u8 auth_mode;
 	u8 key_sizes[WEP_KEYS];
 	u8 keys[WEP_KEYS][SCM_KEY_LEN];
 	u8 level;
@@ -1020,20 +888,20 @@ enum ieee80211_mfie {
 /* Minimal header; can be used for passing 802.11 frames with sufficient
  * information to determine what type of underlying data type is actually
  * stored in the data. */
-struct ieee80211_hdr {
+struct rtl_80211_hdr {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 payload[0];
 } __packed;
 
-struct ieee80211_hdr_1addr {
+struct rtl_80211_hdr_1addr {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 addr1[ETH_ALEN];
 	u8 payload[0];
 } __packed;
 
-struct ieee80211_hdr_2addr {
+struct rtl_80211_hdr_2addr {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 addr1[ETH_ALEN];
@@ -1041,7 +909,7 @@ struct ieee80211_hdr_2addr {
 	u8 payload[0];
 } __packed;
 
-struct ieee80211_hdr_3addr {
+struct rtl_80211_hdr_3addr {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 addr1[ETH_ALEN];
@@ -1051,7 +919,7 @@ struct ieee80211_hdr_3addr {
 	u8 payload[0];
 } __packed;
 
-struct ieee80211_hdr_4addr {
+struct rtl_80211_hdr_4addr {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 addr1[ETH_ALEN];
@@ -1062,7 +930,7 @@ struct ieee80211_hdr_4addr {
 	u8 payload[0];
 } __packed;
 
-struct ieee80211_hdr_3addrqos {
+struct rtl_80211_hdr_3addrqos {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 addr1[ETH_ALEN];
@@ -1073,7 +941,7 @@ struct ieee80211_hdr_3addrqos {
 	__le16 qos_ctl;
 } __packed;
 
-struct ieee80211_hdr_4addrqos {
+struct rtl_80211_hdr_4addrqos {
 	__le16 frame_ctl;
 	__le16 duration_id;
 	u8 addr1[ETH_ALEN];
@@ -1092,7 +960,7 @@ struct ieee80211_info_element {
 } __packed;
 
 struct ieee80211_authentication {
-	struct ieee80211_hdr_3addr header;
+	struct rtl_80211_hdr_3addr header;
 	__le16 algorithm;
 	__le16 transaction;
 	__le16 status;
@@ -1101,18 +969,18 @@ struct ieee80211_authentication {
 } __packed;
 
 struct ieee80211_disassoc {
-	struct ieee80211_hdr_3addr header;
+	struct rtl_80211_hdr_3addr header;
 	__le16 reason;
 } __packed;
 
 struct ieee80211_probe_request {
-	struct ieee80211_hdr_3addr header;
+	struct rtl_80211_hdr_3addr header;
 	/* SSID, supported rates */
 	struct ieee80211_info_element info_element[0];
 } __packed;
 
 struct ieee80211_probe_response {
-	struct ieee80211_hdr_3addr header;
+	struct rtl_80211_hdr_3addr header;
 	__le32 time_stamp[2];
 	__le16 beacon_interval;
 	__le16 capability;
@@ -1125,7 +993,7 @@ struct ieee80211_probe_response {
 #define ieee80211_beacon ieee80211_probe_response
 
 struct ieee80211_assoc_request_frame {
-	struct ieee80211_hdr_3addr header;
+	struct rtl_80211_hdr_3addr header;
 	__le16 capability;
 	__le16 listen_interval;
 	/* SSID, supported rates, RSN */
@@ -1133,7 +1001,7 @@ struct ieee80211_assoc_request_frame {
 } __packed;
 
 struct ieee80211_reassoc_request_frame {
-	struct ieee80211_hdr_3addr header;
+	struct rtl_80211_hdr_3addr header;
 	__le16 capability;
 	__le16 listen_interval;
 	u8 current_ap[ETH_ALEN];
@@ -1142,7 +1010,7 @@ struct ieee80211_reassoc_request_frame {
 } __packed;
 
 struct ieee80211_assoc_response_frame {
-	struct ieee80211_hdr_3addr header;
+	struct rtl_80211_hdr_3addr header;
 	__le16 capability;
 	__le16 status;
 	__le16 aid;
@@ -1276,12 +1144,6 @@ struct ieee80211_tim_parameters {
 } __packed;
 
 //#else
-struct ieee80211_wmm_ac_param {
-	u8 ac_aci_acm_aifsn;
-	u8 ac_ecwmin_ecwmax;
-	u16 ac_txop_limit;
-};
-
 struct ieee80211_wmm_ts_info {
 	u8 ac_dir_tid;
 	u8 ac_up_psb;
@@ -1329,9 +1191,9 @@ static inline const char *eap_get_type(int type)
 //added by amy for reorder
 static inline u8 Frame_QoSTID(u8 *buf)
 {
-	struct ieee80211_hdr_3addr *hdr;
+	struct rtl_80211_hdr_3addr *hdr;
 	u16 fc;
-	hdr = (struct ieee80211_hdr_3addr *)buf;
+	hdr = (struct rtl_80211_hdr_3addr *)buf;
 	fc = le16_to_cpu(hdr->frame_ctl);
 	return (u8)((frameqos *)(buf + (((fc & IEEE80211_FCTL_TODS)&&(fc & IEEE80211_FCTL_FROMDS))? 30 : 24)))->field.tid;
 }
@@ -2262,17 +2124,17 @@ static inline int ieee80211_get_hdrlen(u16 fc)
 	return hdrlen;
 }
 
-static inline u8 *ieee80211_get_payload(struct ieee80211_hdr *hdr)
+static inline u8 *ieee80211_get_payload(struct rtl_80211_hdr *hdr)
 {
 	switch (ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl))) {
 	case IEEE80211_1ADDR_LEN:
-		return ((struct ieee80211_hdr_1addr *)hdr)->payload;
+		return ((struct rtl_80211_hdr_1addr *)hdr)->payload;
 	case IEEE80211_2ADDR_LEN:
-		return ((struct ieee80211_hdr_2addr *)hdr)->payload;
+		return ((struct rtl_80211_hdr_2addr *)hdr)->payload;
 	case IEEE80211_3ADDR_LEN:
-		return ((struct ieee80211_hdr_3addr *)hdr)->payload;
+		return ((struct rtl_80211_hdr_3addr *)hdr)->payload;
 	case IEEE80211_4ADDR_LEN:
-		return ((struct ieee80211_hdr_4addr *)hdr)->payload;
+		return ((struct rtl_80211_hdr_4addr *)hdr)->payload;
 	}
 	return NULL;
 }
@@ -2328,7 +2190,7 @@ extern void ieee80211_txb_free(struct ieee80211_txb *);
 extern int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 			struct ieee80211_rx_stats *rx_stats);
 extern void ieee80211_rx_mgt(struct ieee80211_device *ieee,
-			     struct ieee80211_hdr_4addr *header,
+			     struct rtl_80211_hdr_4addr *header,
 			     struct ieee80211_rx_stats *stats);
 
 /* ieee80211_wx.c */
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
index 788704b800c4..a66141647f2d 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
@@ -112,7 +112,7 @@ static inline void xor_block(u8 *b, u8 *a, size_t len)
 
 
 static void ccmp_init_blocks(struct crypto_tfm *tfm,
-			     struct ieee80211_hdr_4addr *hdr,
+			     struct rtl_80211_hdr_4addr *hdr,
 			     u8 *pn, size_t dlen, u8 *b0, u8 *auth,
 			     u8 *s0)
 {
@@ -196,7 +196,7 @@ static int ieee80211_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	struct ieee80211_ccmp_data *key = priv;
 	int data_len, i;
 	u8 *pos;
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 	cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 
 	if (skb_headroom(skb) < CCMP_HDR_LEN ||
@@ -228,7 +228,7 @@ static int ieee80211_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	*pos++ = key->tx_pn[0];
 
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 	if (!tcb_desc->bHwSec)
 	{
 		int blocks, last, len;
@@ -270,7 +270,7 @@ static int ieee80211_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 {
 	struct ieee80211_ccmp_data *key = priv;
 	u8 keyidx, *pos;
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 	cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 	u8 pn[6];
 
@@ -279,7 +279,7 @@ static int ieee80211_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 		return -1;
 	}
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 	pos = skb->data + hdr_len;
 	keyidx = pos[3];
 	if (!(keyidx & (1 << 5))) {
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
index e815c81b45dc..1f80c52a49c4 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
@@ -306,7 +306,7 @@ static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	struct ieee80211_tkip_data *tkey = priv;
 		int len;
 	u8 *pos;
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 	cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 	struct blkcipher_desc desc = {.tfm = tkey->tx_tfm_arc4};
 	int ret = 0;
@@ -318,7 +318,7 @@ static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	    skb->len < hdr_len)
 		return -1;
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 
 	if (!tcb_desc->bHwSec)
 	{
@@ -390,7 +390,7 @@ static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	u8 keyidx, *pos;
 	u32 iv32;
 	u16 iv16;
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 	cb_desc *tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 	struct blkcipher_desc desc = {.tfm = tkey->rx_tfm_arc4};
 	u8 rc4key[16];
@@ -401,7 +401,7 @@ static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	if (skb->len < hdr_len + 8 + 4)
 		return -1;
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 	pos = skb->data + hdr_len;
 	keyidx = pos[3];
 	if (!(keyidx & (1 << 5))) {
@@ -523,9 +523,9 @@ static int michael_mic(struct crypto_hash *tfm_michael, u8 *key, u8 *hdr,
 
 static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
 {
-	struct ieee80211_hdr_4addr *hdr11;
+	struct rtl_80211_hdr_4addr *hdr11;
 
-	hdr11 = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr11 = (struct rtl_80211_hdr_4addr *) skb->data;
 	switch (le16_to_cpu(hdr11->frame_ctl) &
 		(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
 	case IEEE80211_FCTL_TODS:
@@ -556,9 +556,9 @@ static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len, void *pri
 {
 	struct ieee80211_tkip_data *tkey = priv;
 	u8 *pos;
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 
 	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
 		printk(KERN_DEBUG "Invalid packet for Michael MIC add "
@@ -585,7 +585,7 @@ static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len, void *pri
 }
 
 static void ieee80211_michael_mic_failure(struct net_device *dev,
-				       struct ieee80211_hdr_4addr *hdr,
+				       struct rtl_80211_hdr_4addr *hdr,
 				       int keyidx)
 {
 	union iwreq_data wrqu;
@@ -610,9 +610,9 @@ static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
 {
 	struct ieee80211_tkip_data *tkey = priv;
 	u8 mic[8];
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 
 	if (!tkey->key_set)
 		return -1;
@@ -629,8 +629,8 @@ static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
 				skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
 		return -1;
 	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
-		struct ieee80211_hdr_4addr *hdr;
-		hdr = (struct ieee80211_hdr_4addr *) skb->data;
+		struct rtl_80211_hdr_4addr *hdr;
+		hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 		       "MSDU from %pM keyidx=%d\n",
 		       skb->dev ? skb->dev->name : "N/A", hdr->addr2,
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
index 0a17f84bb809..681611dc93d3 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
@@ -19,7 +19,7 @@
 #include "ieee80211.h"
 
 #include <linux/crypto.h>
-    #include <linux/scatterlist.h>
+#include <linux/scatterlist.h>
 #include <linux/crc32.h>
 
 MODULE_AUTHOR("Jouni Malinen");
@@ -43,38 +43,24 @@ static void *prism2_wep_init(int keyidx)
 
 	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
 	if (priv == NULL)
-		goto fail;
+		return NULL;
 	priv->key_idx = keyidx;
 
 	priv->tx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->tx_tfm)) {
-		pr_debug("ieee80211_crypt_wep: could not allocate "
-		       "crypto API arc4\n");
-		priv->tx_tfm = NULL;
-		goto fail;
-	}
+	if (IS_ERR(priv->tx_tfm))
+		goto free_priv;
 	priv->rx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
-	if (IS_ERR(priv->rx_tfm)) {
-		pr_debug("ieee80211_crypt_wep: could not allocate "
-		       "crypto API arc4\n");
-		priv->rx_tfm = NULL;
-		goto fail;
-	}
+	if (IS_ERR(priv->rx_tfm))
+		goto free_tx;
 
 	/* start WEP IV from a random value */
 	get_random_bytes(&priv->iv, 4);
 
 	return priv;
-
-fail:
-	if (priv) {
-		if (priv->tx_tfm)
-			crypto_free_blkcipher(priv->tx_tfm);
-		if (priv->rx_tfm)
-			crypto_free_blkcipher(priv->rx_tfm);
-		kfree(priv);
-	}
-
+free_tx:
+	crypto_free_blkcipher(priv->tx_tfm);
+free_priv:
+	kfree(priv);
 	return NULL;
 }
 
@@ -142,9 +128,7 @@ static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	/* Copy rest of the WEP key (the secret part) */
 	memcpy(key + 3, wep->key, wep->key_len);
 
-	if (!tcb_desc->bHwSec)
-	{
-
+	if (!tcb_desc->bHwSec) {
 		/* Append little-endian CRC32 and encrypt it to produce ICV */
 		crc = ~crc32_le(~0, pos, len);
 		icv = skb_put(skb, 4);
@@ -201,8 +185,7 @@ static int prism2_wep_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	/* Apply RC4 to data and compute CRC32 over decrypted data */
 	plen = skb->len - hdr_len - 8;
 
-	if (!tcb_desc->bHwSec)
-	{
+	if (!tcb_desc->bHwSec) {
 		crypto_blkcipher_setkey(wep->rx_tfm, key, klen);
 		sg_init_one(&sg, pos, plen+4);
 
@@ -293,6 +276,4 @@ void __exit ieee80211_crypto_wep_exit(void)
 
 void ieee80211_wep_null(void)
 {
-//	printk("============>%s()\n", __func__);
-	return;
 }
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index 9fbb53d8c6bf..b374088c5ff8 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -47,7 +47,7 @@ static inline void ieee80211_monitor_rx(struct ieee80211_device *ieee,
 					struct sk_buff *skb,
 					struct ieee80211_rx_stats *rx_stats)
 {
-	struct ieee80211_hdr_4addr *hdr = (struct ieee80211_hdr_4addr *)skb->data;
+	struct rtl_80211_hdr_4addr *hdr = (struct rtl_80211_hdr_4addr *)skb->data;
 	u16 fc = le16_to_cpu(hdr->frame_ctl);
 
 	skb->dev = ieee->dev;
@@ -94,7 +94,7 @@ ieee80211_frag_cache_find(struct ieee80211_device *ieee, unsigned int seq,
 /* Called only as a tasklet (software IRQ) */
 static struct sk_buff *
 ieee80211_frag_cache_get(struct ieee80211_device *ieee,
-			 struct ieee80211_hdr_4addr *hdr)
+			 struct rtl_80211_hdr_4addr *hdr)
 {
 	struct sk_buff *skb = NULL;
 	u16 fc = le16_to_cpu(hdr->frame_ctl);
@@ -102,17 +102,17 @@ ieee80211_frag_cache_get(struct ieee80211_device *ieee,
 	unsigned int frag = WLAN_GET_SEQ_FRAG(sc);
 	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
 	struct ieee80211_frag_entry *entry;
-	struct ieee80211_hdr_3addrqos *hdr_3addrqos;
-	struct ieee80211_hdr_4addrqos *hdr_4addrqos;
+	struct rtl_80211_hdr_3addrqos *hdr_3addrqos;
+	struct rtl_80211_hdr_4addrqos *hdr_4addrqos;
 	u8 tid;
 
 	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
-	  hdr_4addrqos = (struct ieee80211_hdr_4addrqos *)hdr;
+	  hdr_4addrqos = (struct rtl_80211_hdr_4addrqos *)hdr;
 	  tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QCTL_TID;
 	  tid = UP2AC(tid);
 	  tid ++;
 	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
-	  hdr_3addrqos = (struct ieee80211_hdr_3addrqos *)hdr;
+	  hdr_3addrqos = (struct rtl_80211_hdr_3addrqos *)hdr;
 	  tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QCTL_TID;
 	  tid = UP2AC(tid);
 	  tid ++;
@@ -123,7 +123,7 @@ ieee80211_frag_cache_get(struct ieee80211_device *ieee,
 	if (frag == 0) {
 		/* Reserve enough space to fit maximum frame length */
 		skb = dev_alloc_skb(ieee->dev->mtu +
-				    sizeof(struct ieee80211_hdr_4addr) +
+				    sizeof(struct rtl_80211_hdr_4addr) +
 				    8 /* LLC */ +
 				    2 /* alignment */ +
 				    8 /* WEP */ +
@@ -163,23 +163,23 @@ ieee80211_frag_cache_get(struct ieee80211_device *ieee,
 
 /* Called only as a tasklet (software IRQ) */
 static int ieee80211_frag_cache_invalidate(struct ieee80211_device *ieee,
-					   struct ieee80211_hdr_4addr *hdr)
+					   struct rtl_80211_hdr_4addr *hdr)
 {
 	u16 fc = le16_to_cpu(hdr->frame_ctl);
 	u16 sc = le16_to_cpu(hdr->seq_ctl);
 	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
 	struct ieee80211_frag_entry *entry;
-	struct ieee80211_hdr_3addrqos *hdr_3addrqos;
-	struct ieee80211_hdr_4addrqos *hdr_4addrqos;
+	struct rtl_80211_hdr_3addrqos *hdr_3addrqos;
+	struct rtl_80211_hdr_4addrqos *hdr_4addrqos;
 	u8 tid;
 
 	if(((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
-	  hdr_4addrqos = (struct ieee80211_hdr_4addrqos *)hdr;
+	  hdr_4addrqos = (struct rtl_80211_hdr_4addrqos *)hdr;
 	  tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QCTL_TID;
 	  tid = UP2AC(tid);
 	  tid ++;
 	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
-	  hdr_3addrqos = (struct ieee80211_hdr_3addrqos *)hdr;
+	  hdr_3addrqos = (struct rtl_80211_hdr_3addrqos *)hdr;
 	  tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QCTL_TID;
 	  tid = UP2AC(tid);
 	  tid ++;
@@ -217,10 +217,10 @@ ieee80211_rx_frame_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb,
 	 * this is not mandatory.... but seems that the probe
 	 * response parser uses it
 	 */
-	struct ieee80211_hdr_3addr *hdr = (struct ieee80211_hdr_3addr *)skb->data;
+	struct rtl_80211_hdr_3addr *hdr = (struct rtl_80211_hdr_3addr *)skb->data;
 
 	rx_stats->len = skb->len;
-	ieee80211_rx_mgt(ieee,(struct ieee80211_hdr_4addr *)skb->data,rx_stats);
+	ieee80211_rx_mgt(ieee,(struct rtl_80211_hdr_4addr *)skb->data,rx_stats);
 	/* if ((ieee->state == IEEE80211_LINKED) && (memcmp(hdr->addr3, ieee->current_network.bssid, ETH_ALEN))) */
 	if ((memcmp(hdr->addr1, ieee->dev->dev_addr, ETH_ALEN)))/* use ADDR1 to perform address matching for Management frames */
 	{
@@ -298,13 +298,13 @@ static int ieee80211_is_eapol_frame(struct ieee80211_device *ieee,
 {
 	struct net_device *dev = ieee->dev;
 	u16 fc, ethertype;
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 	u8 *pos;
 
 	if (skb->len < 24)
 		return 0;
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 	fc = le16_to_cpu(hdr->frame_ctl);
 
 	/* check that the frame is unicast frame to us */
@@ -338,7 +338,7 @@ static inline int
 ieee80211_rx_frame_decrypt(struct ieee80211_device *ieee, struct sk_buff *skb,
 			   struct ieee80211_crypt_data *crypt)
 {
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 	int res, hdrlen;
 
 	if (crypt == NULL || crypt->ops->decrypt_mpdu == NULL)
@@ -348,7 +348,7 @@ ieee80211_rx_frame_decrypt(struct ieee80211_device *ieee, struct sk_buff *skb,
 		cb_desc *tcb_desc = (cb_desc *)(skb->cb+ MAX_DEV_ADDR_SIZE);
 		tcb_desc->bHwSec = 1;
 	}
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 	hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
 
 	if (ieee->tkip_countermeasures &&
@@ -385,7 +385,7 @@ static inline int
 ieee80211_rx_frame_decrypt_msdu(struct ieee80211_device *ieee, struct sk_buff *skb,
 			     int keyidx, struct ieee80211_crypt_data *crypt)
 {
-	struct ieee80211_hdr_4addr *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
 	int res, hdrlen;
 
 	if (crypt == NULL || crypt->ops->decrypt_msdu == NULL)
@@ -396,7 +396,7 @@ ieee80211_rx_frame_decrypt_msdu(struct ieee80211_device *ieee, struct sk_buff *s
 		tcb_desc->bHwSec = 1;
 	}
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 	hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
 
 	atomic_inc(&crypt->refcnt);
@@ -416,7 +416,7 @@ ieee80211_rx_frame_decrypt_msdu(struct ieee80211_device *ieee, struct sk_buff *s
 /* this function is stolen from ipw2200 driver*/
 #define IEEE_PACKET_RETRY_TIME (5*HZ)
 static int is_duplicate_packet(struct ieee80211_device *ieee,
-				      struct ieee80211_hdr_4addr *header)
+				      struct rtl_80211_hdr_4addr *header)
 {
 	u16 fc = le16_to_cpu(header->frame_ctl);
 	u16 sc = le16_to_cpu(header->seq_ctl);
@@ -424,19 +424,19 @@ static int is_duplicate_packet(struct ieee80211_device *ieee,
 	u16 frag = WLAN_GET_SEQ_FRAG(sc);
 	u16 *last_seq, *last_frag;
 	unsigned long *last_time;
-	struct ieee80211_hdr_3addrqos *hdr_3addrqos;
-	struct ieee80211_hdr_4addrqos *hdr_4addrqos;
+	struct rtl_80211_hdr_3addrqos *hdr_3addrqos;
+	struct rtl_80211_hdr_4addrqos *hdr_4addrqos;
 	u8 tid;
 
 
 	//TO2DS and QoS
 	if(((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
-	  hdr_4addrqos = (struct ieee80211_hdr_4addrqos *)header;
+	  hdr_4addrqos = (struct rtl_80211_hdr_4addrqos *)header;
 	  tid = le16_to_cpu(hdr_4addrqos->qos_ctl) & IEEE80211_QCTL_TID;
 	  tid = UP2AC(tid);
 	  tid ++;
 	} else if(IEEE80211_QOS_HAS_SEQ(fc)) { //QoS
-	  hdr_3addrqos = (struct ieee80211_hdr_3addrqos *)header;
+	  hdr_3addrqos = (struct rtl_80211_hdr_3addrqos *)header;
 	  tid = le16_to_cpu(hdr_3addrqos->qos_ctl) & IEEE80211_QCTL_TID;
 	  tid = UP2AC(tid);
 	  tid ++;
@@ -768,10 +768,10 @@ static u8 parse_subframe(struct sk_buff *skb,
 			 struct ieee80211_rx_stats *rx_stats,
 			 struct ieee80211_rxb *rxb, u8 *src, u8 *dst)
 {
-	struct ieee80211_hdr_3addr  *hdr = (struct ieee80211_hdr_3addr *)skb->data;
+	struct rtl_80211_hdr_3addr  *hdr = (struct rtl_80211_hdr_3addr *)skb->data;
 	u16		fc = le16_to_cpu(hdr->frame_ctl);
 
-	u16		LLCOffset= sizeof(struct ieee80211_hdr_3addr);
+	u16		LLCOffset= sizeof(struct rtl_80211_hdr_3addr);
 	u16		ChkLength;
 	bool		bIsAggregateFrame = false;
 	u16		nSubframe_Length;
@@ -888,8 +888,8 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 		 struct ieee80211_rx_stats *rx_stats)
 {
 	struct net_device *dev = ieee->dev;
-	struct ieee80211_hdr_4addr *hdr;
-	//struct ieee80211_hdr_3addrqos *hdr;
+	struct rtl_80211_hdr_4addr *hdr;
+	//struct rtl_80211_hdr_3addrqos *hdr;
 
 	size_t hdrlen;
 	u16 fc, type, stype, sc;
@@ -921,7 +921,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	int i;
 	struct ieee80211_rxb *rxb = NULL;
 	// cheat the the hdr type
-	hdr = (struct ieee80211_hdr_4addr *)skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
 	stats = &ieee->stats;
 
 	if (skb->len < 10) {
@@ -1156,7 +1156,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	}
 
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 
 	/* skb: hdr + (possibly fragmented) plaintext payload */
 	// PR: FIXME: hostap has additional conditions in the "if" below:
@@ -1209,7 +1209,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 		/* this was the last fragment and the frame will be
 		 * delivered, so remove skb from fragment cache */
 		skb = frag_skb;
-		hdr = (struct ieee80211_hdr_4addr *) skb->data;
+		hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 		ieee80211_frag_cache_invalidate(ieee, hdr);
 	}
 
@@ -1226,7 +1226,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	ieee->LinkDetectInfo.NumRecvDataInPeriod++;
 	ieee->LinkDetectInfo.NumRxOkInPeriod++;
 
-	hdr = (struct ieee80211_hdr_4addr *) skb->data;
+	hdr = (struct rtl_80211_hdr_4addr *) skb->data;
 	if (crypt && !(fc & IEEE80211_FCTL_WEP) && !ieee->open_wep) {
 		if (/*ieee->ieee802_1x &&*/
 		    ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
@@ -2366,10 +2366,10 @@ static inline void update_network(struct ieee80211_network *dst,
 
 	/* dst->last_associate is not overwritten */
 	dst->wmm_info = src->wmm_info; //sure to exist in beacon or probe response frame.
-	if (src->wmm_param[0].ac_aci_acm_aifsn|| \
-	   src->wmm_param[1].ac_aci_acm_aifsn|| \
-	   src->wmm_param[2].ac_aci_acm_aifsn|| \
-	   src->wmm_param[3].ac_aci_acm_aifsn) {
+	if (src->wmm_param[0].aci_aifsn|| \
+	   src->wmm_param[1].aci_aifsn|| \
+	   src->wmm_param[2].aci_aifsn|| \
+	   src->wmm_param[3].aci_aifsn) {
 	  memcpy(dst->wmm_param, src->wmm_param, WME_AC_PRAM_LEN);
 	}
 	//dst->QoS_Enable = src->QoS_Enable;
@@ -2612,7 +2612,7 @@ static inline void ieee80211_process_probe_response(
 }
 
 void ieee80211_rx_mgt(struct ieee80211_device *ieee,
-		      struct ieee80211_hdr_4addr *header,
+		      struct rtl_80211_hdr_4addr *header,
 		      struct ieee80211_rx_stats *stats)
 {
 	switch (WLAN_FC_GET_STYPE(header->frame_ctl)) {
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index c2388812d4fd..1b11acb96233 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -222,8 +222,8 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee
 {
 	unsigned long flags;
 	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
-	struct ieee80211_hdr_3addr  *header=
-		(struct ieee80211_hdr_3addr  *) skb->data;
+	struct rtl_80211_hdr_3addr  *header=
+		(struct rtl_80211_hdr_3addr  *) skb->data;
 
 	cb_desc *tcb_desc = (cb_desc *)(skb->cb + 8);
 	spin_lock_irqsave(&ieee->lock, flags);
@@ -289,8 +289,8 @@ inline void softmac_ps_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *i
 {
 
 	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
-	struct ieee80211_hdr_3addr  *header =
-		(struct ieee80211_hdr_3addr  *) skb->data;
+	struct rtl_80211_hdr_3addr  *header =
+		(struct rtl_80211_hdr_3addr  *) skb->data;
 
 
 	if(single){
@@ -660,10 +660,13 @@ inline struct sk_buff *ieee80211_authentication_req(struct ieee80211_network *be
 	auth = (struct ieee80211_authentication *)
 		skb_put(skb, sizeof(struct ieee80211_authentication));
 
-	auth->header.frame_ctl = IEEE80211_STYPE_AUTH;
-	if (challengelen) auth->header.frame_ctl |= IEEE80211_FCTL_WEP;
+	if (challengelen)
+		auth->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_AUTH
+				| IEEE80211_FCTL_WEP);
+	else
+		auth->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_AUTH);
 
-	auth->header.duration_id = 0x013a; //FIXME
+	auth->header.duration_id = cpu_to_le16(0x013a);
 
 	memcpy(auth->header.addr1, beacon->bssid, ETH_ALEN);
 	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
@@ -673,7 +676,7 @@ inline struct sk_buff *ieee80211_authentication_req(struct ieee80211_network *be
 	if(ieee->auth_mode == 0)
 		auth->algorithm = WLAN_AUTH_OPEN;
 	else if(ieee->auth_mode == 1)
-		auth->algorithm = WLAN_AUTH_SHARED_KEY;
+		auth->algorithm = cpu_to_le16(WLAN_AUTH_SHARED_KEY);
 	else if(ieee->auth_mode == 2)
 		auth->algorithm = WLAN_AUTH_OPEN;//0x80;
 	printk("=================>%s():auth->algorithm is %d\n",__func__,auth->algorithm);
@@ -928,14 +931,14 @@ static struct sk_buff *ieee80211_null_func(struct ieee80211_device *ieee,
 					   short pwr)
 {
 	struct sk_buff *skb;
-	struct ieee80211_hdr_3addr *hdr;
+	struct rtl_80211_hdr_3addr *hdr;
 
-	skb = dev_alloc_skb(sizeof(struct ieee80211_hdr_3addr));
+	skb = dev_alloc_skb(sizeof(struct rtl_80211_hdr_3addr));
 
 	if (!skb)
 		return NULL;
 
-	hdr = (struct ieee80211_hdr_3addr *)skb_put(skb,sizeof(struct ieee80211_hdr_3addr));
+	hdr = (struct rtl_80211_hdr_3addr *)skb_put(skb,sizeof(struct rtl_80211_hdr_3addr));
 
 	memcpy(hdr->addr1, ieee->current_network.bssid, ETH_ALEN);
 	memcpy(hdr->addr2, ieee->dev->dev_addr, ETH_ALEN);
@@ -1084,7 +1087,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 
 
 	hdr->header.frame_ctl = IEEE80211_STYPE_ASSOC_REQ;
-	hdr->header.duration_id= 37; //FIXME
+	hdr->header.duration_id = cpu_to_le16(37);
 	memcpy(hdr->header.addr1, beacon->bssid, ETH_ALEN);
 	memcpy(hdr->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
 	memcpy(hdr->header.addr3, beacon->bssid, ETH_ALEN);
@@ -1103,7 +1106,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 	if (wmm_info_len) //QOS
 		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_QOS);
 
-	hdr->listen_interval = 0xa; //FIXME
+	hdr->listen_interval = cpu_to_le16(0xa);
 
 	hdr->info_element[0].id = MFIE_TYPE_SSID;
 
@@ -1304,7 +1307,7 @@ static void ieee80211_auth_challenge(struct ieee80211_device *ieee,
 
 		IEEE80211_DEBUG_MGMT("Sending authentication challenge response\n");
 
-		ieee80211_encrypt_fragment(ieee, skb, sizeof(struct ieee80211_hdr_3addr  ));
+		ieee80211_encrypt_fragment(ieee, skb, sizeof(struct rtl_80211_hdr_3addr  ));
 
 		softmac_mgmt_xmit(skb, ieee);
 		mod_timer(&ieee->associate_timer, jiffies + (HZ/2));
@@ -1364,12 +1367,10 @@ static void ieee80211_associate_complete_wq(struct work_struct *work)
 		ieee->LinkDetectInfo.NumRecvDataInPeriod= 1;
 	}
 	ieee->link_change(ieee->dev);
-	if(ieee->is_silent_reset == 0){
+	if (!ieee->is_silent_reset) {
 		printk("============>normal associate\n");
-	notify_wx_assoc_event(ieee);
-	}
-	else if(ieee->is_silent_reset == 1)
-	{
+		notify_wx_assoc_event(ieee);
+	} else {
 		printk("==================>silent reset associate\n");
 		ieee->is_silent_reset = false;
 	}
@@ -1558,7 +1559,7 @@ static inline u16 auth_parse(struct sk_buff *skb, u8 **challenge, int *chlen)
 		}
 	}
 
-	return cpu_to_le16(a->status);
+	return le16_to_cpu(a->status);
 
 }
 
@@ -1588,17 +1589,17 @@ static short probe_rq_parse(struct ieee80211_device *ieee, struct sk_buff *skb,
 	u8 *ssid=NULL;
 	u8 ssidlen = 0;
 
-	struct ieee80211_hdr_3addr   *header =
-		(struct ieee80211_hdr_3addr   *) skb->data;
+	struct rtl_80211_hdr_3addr   *header =
+		(struct rtl_80211_hdr_3addr   *) skb->data;
 
-	if (skb->len < sizeof (struct ieee80211_hdr_3addr  ))
+	if (skb->len < sizeof (struct rtl_80211_hdr_3addr  ))
 		return -1; /* corrupted */
 
 	memcpy(src,header->addr2, ETH_ALEN);
 
 	skbend = (u8 *)skb->data + skb->len;
 
-	tag = skb->data + sizeof (struct ieee80211_hdr_3addr  );
+	tag = skb->data + sizeof (struct rtl_80211_hdr_3addr  );
 
 	while (tag+1 < skbend){
 		if (*tag == 0) {
@@ -1894,7 +1895,7 @@ EXPORT_SYMBOL(ieee80211_ps_tx_ack);
 static void ieee80211_process_action(struct ieee80211_device *ieee,
 				     struct sk_buff *skb)
 {
-	struct ieee80211_hdr *header = (struct ieee80211_hdr *)skb->data;
+	struct rtl_80211_hdr *header = (struct rtl_80211_hdr *)skb->data;
 	u8 *act = ieee80211_get_payload(header);
 	u8 tmp = 0;
 //	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
@@ -1985,7 +1986,7 @@ ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 			struct ieee80211_rx_stats *rx_stats, u16 type,
 			u16 stype)
 {
-	struct ieee80211_hdr_3addr *header = (struct ieee80211_hdr_3addr *) skb->data;
+	struct rtl_80211_hdr_3addr *header = (struct rtl_80211_hdr_3addr *) skb->data;
 	u16 errcode;
 	int aid;
 	struct ieee80211_assoc_response_frame *assoc_resp;
@@ -2153,7 +2154,7 @@ void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *
 	ieee80211_sta_wakeup(ieee, 0);
 
 	/* update the tx status */
-	ieee->stats.tx_bytes += txb->payload_size;
+	ieee->stats.tx_bytes += le16_to_cpu(txb->payload_size);
 	ieee->stats.tx_packets++;
 	tcb_desc = (cb_desc *)(txb->fragments[0]->cb + MAX_DEV_ADDR_SIZE);
 	if (tcb_desc->bMulticast) {
@@ -2243,7 +2244,7 @@ void ieee80211_wake_queue(struct ieee80211_device *ieee)
 
 	unsigned long flags;
 	struct sk_buff *skb;
-	struct ieee80211_hdr_3addr  *header;
+	struct rtl_80211_hdr_3addr  *header;
 
 	spin_lock_irqsave(&ieee->lock, flags);
 	if (! ieee->queue_stop) goto exit;
@@ -2253,7 +2254,7 @@ void ieee80211_wake_queue(struct ieee80211_device *ieee)
 	if (ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE) {
 		while (!ieee->queue_stop && (skb = dequeue_mgmt(ieee))){
 
-			header = (struct ieee80211_hdr_3addr  *) skb->data;
+			header = (struct rtl_80211_hdr_3addr  *) skb->data;
 
 			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
 
@@ -3126,7 +3127,7 @@ inline struct sk_buff *ieee80211_disassociate_skb(
 	memcpy(disass->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
 	memcpy(disass->header.addr3, beacon->bssid, ETH_ALEN);
 
-	disass->reason = asRsn;
+	disass->reason = cpu_to_le16(asRsn);
 	return skb;
 }
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index 9f68c652fb2b..5353a45ffdff 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -194,9 +194,9 @@ int ieee80211_encrypt_fragment(
 	if (ieee->tkip_countermeasures &&
 	    crypt && crypt->ops && strcmp(crypt->ops->name, "TKIP") == 0) {
 		if (net_ratelimit()) {
-			struct ieee80211_hdr_3addrqos *header;
+			struct rtl_80211_hdr_3addrqos *header;
 
-			header = (struct ieee80211_hdr_3addrqos *)frag->data;
+			header = (struct rtl_80211_hdr_3addrqos *)frag->data;
 			printk(KERN_DEBUG "%s: TKIP countermeasures: dropped "
 			       "TX packet to %pM\n",
 			       ieee->dev->name, header->addr1);
@@ -308,7 +308,7 @@ static void ieee80211_tx_query_agg_cap(struct ieee80211_device *ieee,
 {
 	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
 	PTX_TS_RECORD			pTxTs = NULL;
-	struct ieee80211_hdr_1addr *hdr = (struct ieee80211_hdr_1addr *)skb->data;
+	struct rtl_80211_hdr_1addr *hdr = (struct rtl_80211_hdr_1addr *)skb->data;
 
 	if (!pHTInfo->bCurrentHTSupport||!pHTInfo->bEnableHT)
 		return;
@@ -598,14 +598,14 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct ieee80211_device *ieee = netdev_priv(dev);
 	struct ieee80211_txb *txb = NULL;
-	struct ieee80211_hdr_3addrqos *frag_hdr;
+	struct rtl_80211_hdr_3addrqos *frag_hdr;
 	int i, bytes_per_frag, nr_frags, bytes_last_frag, frag_size;
 	unsigned long flags;
 	struct net_device_stats *stats = &ieee->stats;
 	int ether_type = 0, encrypt;
 	int bytes, fc, qos_ctl = 0, hdr_len;
 	struct sk_buff *skb_frag;
-	struct ieee80211_hdr_3addrqos header = { /* Ensure zero initialized */
+	struct rtl_80211_hdr_3addrqos header = { /* Ensure zero initialized */
 		.duration_id = 0,
 		.seq_ctl = 0,
 		.qos_ctl = 0
@@ -787,7 +787,7 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 			{
 				tcb_desc->bHwSec = 0;
 			}
-			frag_hdr = (struct ieee80211_hdr_3addrqos *)skb_put(skb_frag, hdr_len);
+			frag_hdr = (struct rtl_80211_hdr_3addrqos *)skb_put(skb_frag, hdr_len);
 			memcpy(frag_hdr, &header, hdr_len);
 
 			/* If this is not the last fragment, then add the MOREFRAGS
@@ -845,7 +845,7 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 			ieee->seq_ctrl[0]++;
 		}
 	}else{
-		if (unlikely(skb->len < sizeof(struct ieee80211_hdr_3addr))) {
+		if (unlikely(skb->len < sizeof(struct rtl_80211_hdr_3addr))) {
 			printk(KERN_WARNING "%s: skb too small (%d).\n",
 			ieee->dev->name, skb->len);
 			goto success;
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index 618d2cbc049e..9ff8e056ab7f 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -110,7 +110,7 @@ void ResetBaEntry(PBA_RECORD pBA)
 static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, PBA_RECORD pBA, u16 StatusCode, u8 type)
 {
 	struct sk_buff *skb = NULL;
-	 struct ieee80211_hdr_3addr *BAReq = NULL;
+	 struct rtl_80211_hdr_3addr *BAReq = NULL;
 	u8 *tag = NULL;
 	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))
@@ -120,17 +120,17 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "pBA is NULL\n");
 		return NULL;
 	}
-	skb = dev_alloc_skb(len + sizeof( struct ieee80211_hdr_3addr)); //need to add something others? FIXME
+	skb = dev_alloc_skb(len + sizeof( struct rtl_80211_hdr_3addr)); //need to add something others? FIXME
 	if (skb == NULL)
 	{
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
 		return NULL;
 	}
 
-	memset(skb->data, 0, sizeof( struct ieee80211_hdr_3addr));	//I wonder whether it's necessary. Apparently kernel will not do it when alloc a skb.
+	memset(skb->data, 0, sizeof( struct rtl_80211_hdr_3addr));	//I wonder whether it's necessary. Apparently kernel will not do it when alloc a skb.
 	skb_reserve(skb, ieee->tx_headroom);
 
-	BAReq = ( struct ieee80211_hdr_3addr *) skb_put(skb,sizeof( struct ieee80211_hdr_3addr));
+	BAReq = ( struct rtl_80211_hdr_3addr *) skb_put(skb,sizeof( struct rtl_80211_hdr_3addr));
 
 	memcpy(BAReq->addr1, Dst, ETH_ALEN);
 	memcpy(BAReq->addr2, ieee->dev->dev_addr, ETH_ALEN);
@@ -139,7 +139,7 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 
 	BAReq->frame_ctl = cpu_to_le16(IEEE80211_STYPE_MANAGE_ACT); //action frame
 
-	//tag += sizeof( struct ieee80211_hdr_3addr); //move to action field
+	//tag += sizeof( struct rtl_80211_hdr_3addr); //move to action field
 	tag = (u8 *)skb_put(skb, 9);
 	*tag ++= ACT_CAT_BA;
 	*tag ++= type;
@@ -195,7 +195,7 @@ static struct sk_buff *ieee80211_DELBA(
 {
 	DELBA_PARAM_SET	DelbaParamSet;
 	struct sk_buff *skb = NULL;
-	 struct ieee80211_hdr_3addr *Delba = NULL;
+	 struct rtl_80211_hdr_3addr *Delba = NULL;
 	u8 *tag = NULL;
 	//len = head len + DELBA Parameter Set(2) + Reason Code(2)
 	u16 len = 6 + ieee->tx_headroom;
@@ -208,16 +208,16 @@ static struct sk_buff *ieee80211_DELBA(
 	DelbaParamSet.field.Initiator	= (TxRxSelect==TX_DIR)?1:0;
 	DelbaParamSet.field.TID	= pBA->BaParamSet.field.TID;
 
-	skb = dev_alloc_skb(len + sizeof( struct ieee80211_hdr_3addr)); //need to add something others? FIXME
+	skb = dev_alloc_skb(len + sizeof( struct rtl_80211_hdr_3addr)); //need to add something others? FIXME
 	if (skb == NULL)
 	{
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
 		return NULL;
 	}
-//	memset(skb->data, 0, len+sizeof( struct ieee80211_hdr_3addr));
+//	memset(skb->data, 0, len+sizeof( struct rtl_80211_hdr_3addr));
 	skb_reserve(skb, ieee->tx_headroom);
 
-	Delba = ( struct ieee80211_hdr_3addr *) skb_put(skb,sizeof( struct ieee80211_hdr_3addr));
+	Delba = ( struct rtl_80211_hdr_3addr *) skb_put(skb,sizeof( struct rtl_80211_hdr_3addr));
 
 	memcpy(Delba->addr1, dst, ETH_ALEN);
 	memcpy(Delba->addr2, ieee->dev->dev_addr, ETH_ALEN);
@@ -333,7 +333,7 @@ static void ieee80211_send_DELBA(struct ieee80211_device *ieee, u8 *dst,
 ********************************************************************************************************************/
 int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee, struct sk_buff *skb)
 {
-	 struct ieee80211_hdr_3addr *req = NULL;
+	 struct rtl_80211_hdr_3addr *req = NULL;
 	u16 rc = 0;
 	u8 *dst = NULL, *pDialogToken = NULL, *tag = NULL;
 	PBA_RECORD pBA = NULL;
@@ -342,20 +342,20 @@ 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 rtl_80211_hdr_3addr) + 9) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR,
 				" Invalid skb len in BAREQ(%d / %zu)\n",
 				skb->len,
-				(sizeof(struct ieee80211_hdr_3addr) + 9));
+				(sizeof(struct rtl_80211_hdr_3addr) + 9));
 		return -1;
 	}
 
 	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
 
-	req = (struct ieee80211_hdr_3addr *) skb->data;
+	req = (struct rtl_80211_hdr_3addr *) skb->data;
 	tag = (u8 *)req;
 	dst = (u8 *)(&req->addr2[0]);
-	tag += sizeof(struct ieee80211_hdr_3addr);
+	tag += sizeof(struct rtl_80211_hdr_3addr);
 	pDialogToken = tag + 2;  //category+action
 	pBaParamSet = (PBA_PARAM_SET)(tag + 3);   //+DialogToken
 	pBaTimeoutVal = (u16 *)(tag + 5);
@@ -435,7 +435,7 @@ OnADDBAReq_Fail:
 ********************************************************************************************************************/
 int ieee80211_rx_ADDBARsp(struct ieee80211_device *ieee, struct sk_buff *skb)
 {
-	 struct ieee80211_hdr_3addr *rsp = NULL;
+	 struct rtl_80211_hdr_3addr *rsp = NULL;
 	PBA_RECORD		pPendingBA, pAdmittedBA;
 	PTX_TS_RECORD		pTS = NULL;
 	u8 *dst = NULL, *pDialogToken = NULL, *tag = NULL;
@@ -443,17 +443,17 @@ 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 rtl_80211_hdr_3addr) + 9) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR,
 				" Invalid skb len in BARSP(%d / %zu)\n",
 				skb->len,
-				(sizeof(struct ieee80211_hdr_3addr) + 9));
+				(sizeof(struct rtl_80211_hdr_3addr) + 9));
 		return -1;
 	}
-	rsp = (struct ieee80211_hdr_3addr *)skb->data;
+	rsp = (struct rtl_80211_hdr_3addr *)skb->data;
 	tag = (u8 *)rsp;
 	dst = (u8 *)(&rsp->addr2[0]);
-	tag += sizeof(struct ieee80211_hdr_3addr);
+	tag += sizeof(struct rtl_80211_hdr_3addr);
 	pDialogToken = tag + 2;
 	pStatusCode = (u16 *)(tag + 3);
 	pBaParamSet = (PBA_PARAM_SET)(tag + 5);
@@ -569,16 +569,16 @@ OnADDBARsp_Reject:
 ********************************************************************************************************************/
 int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb)
 {
-	 struct ieee80211_hdr_3addr *delba = NULL;
+	 struct rtl_80211_hdr_3addr *delba = NULL;
 	PDELBA_PARAM_SET	pDelBaParamSet = NULL;
 	u16			*pReasonCode = NULL;
 	u8			*dst = NULL;
 
-	if (skb->len < sizeof(struct ieee80211_hdr_3addr) + 6) {
+	if (skb->len < sizeof(struct rtl_80211_hdr_3addr) + 6) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR,
 				" Invalid skb len in DELBA(%d / %zu)\n",
 				skb->len,
-				(sizeof(struct ieee80211_hdr_3addr) + 6));
+				(sizeof(struct rtl_80211_hdr_3addr) + 6));
 		return -1;
 	}
 
@@ -590,9 +590,9 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb)
 	}
 
 	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
-	delba = (struct ieee80211_hdr_3addr *)skb->data;
+	delba = (struct rtl_80211_hdr_3addr *)skb->data;
 	dst = (u8 *)(&delba->addr2[0]);
-	delba += sizeof(struct ieee80211_hdr_3addr);
+	delba += sizeof(struct rtl_80211_hdr_3addr);
 	pDelBaParamSet = (PDELBA_PARAM_SET)(delba+2);
 	pReasonCode = (u16 *)(delba+4);
 
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index a4795afeeb9c..b852396d21e6 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -3209,7 +3209,7 @@ static void rtl819x_update_rxcounts(struct r8192_priv *priv, u32 *TotalRxBcnNum,
 }
 
 
-void rtl819x_watchdog_wqcallback(struct work_struct *work)
+static void rtl819x_watchdog_wqcallback(struct work_struct *work)
 {
 	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
 	struct r8192_priv *priv = container_of(dwork, struct r8192_priv, watch_dog_wq);
@@ -3273,13 +3273,13 @@ void rtl819x_watchdog_wqcallback(struct work_struct *work)
 
 }
 
-void watch_dog_timer_callback(unsigned long data)
+static void watch_dog_timer_callback(unsigned long data)
 {
 	struct r8192_priv *priv = ieee80211_priv((struct net_device *) data);
 	queue_delayed_work(priv->priv_wq, &priv->watch_dog_wq, 0);
 	mod_timer(&priv->watch_dog_timer, jiffies + MSECS(IEEE80211_WATCH_DOG_TIME));
 }
-int _rtl8192_up(struct net_device *dev)
+static int _rtl8192_up(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	int init_status = 0;
@@ -3330,7 +3330,7 @@ int rtl8192_up(struct net_device *dev)
 }
 
 
-int rtl8192_close(struct net_device *dev)
+static int rtl8192_close(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 	int ret;
@@ -3403,7 +3403,7 @@ void rtl8192_commit(struct net_device *dev)
 
 }
 
-void rtl8192_restart(struct work_struct *work)
+static void rtl8192_restart(struct work_struct *work)
 {
 	struct r8192_priv *priv = container_of(work, struct r8192_priv, reset_wq);
 	struct net_device *dev = priv->ieee80211->dev;
@@ -3711,10 +3711,10 @@ static void rtl8192_process_phyinfo(struct r8192_priv *priv, u8 *buffer,
 	static u32 slide_beacon_adc_pwdb_index, slide_beacon_adc_pwdb_statistics;
 	static u32 last_beacon_adc_pwdb;
 
-	struct ieee80211_hdr_3addr *hdr;
+	struct rtl_80211_hdr_3addr *hdr;
 	u16 sc;
 	unsigned int frag, seq;
-	hdr = (struct ieee80211_hdr_3addr *)buffer;
+	hdr = (struct rtl_80211_hdr_3addr *)buffer;
 	sc = le16_to_cpu(hdr->seq_ctl);
 	frag = WLAN_GET_SEQ_FRAG(sc);
 	seq = WLAN_GET_SEQ_SEQ(sc);
@@ -4205,7 +4205,7 @@ static void TranslateRxSignalStuff819xUsb(struct sk_buff *skb,
 	bool bpacket_match_bssid, bpacket_toself;
 	bool bPacketBeacon = false, bToSelfBA = false;
 	static struct ieee80211_rx_stats  previous_stats;
-	struct ieee80211_hdr_3addr *hdr;//by amy
+	struct rtl_80211_hdr_3addr *hdr;//by amy
 	u16 fc, type;
 
 	// Get Signal Quality for only RX data queue (but not command queue)
@@ -4216,7 +4216,7 @@ static void TranslateRxSignalStuff819xUsb(struct sk_buff *skb,
 	/* Get MAC frame start address. */
 	tmp_buf = (u8 *)skb->data;
 
-	hdr = (struct ieee80211_hdr_3addr *)tmp_buf;
+	hdr = (struct rtl_80211_hdr_3addr *)tmp_buf;
 	fc = le16_to_cpu(hdr->frame_ctl);
 	type = WLAN_FC_GET_TYPE(fc);
 	praddr = hdr->addr1;
@@ -4487,7 +4487,7 @@ static void rtl8192_rx_nomal(struct sk_buff *skb)
 		.freq = IEEE80211_24GHZ_BAND,
 	};
 	u32 rx_pkt_len = 0;
-	struct ieee80211_hdr_1addr *ieee80211_hdr = NULL;
+	struct rtl_80211_hdr_1addr *ieee80211_hdr = NULL;
 	bool unicast_packet = false;
 
 	/* 20 is for ps-poll */
@@ -4500,7 +4500,7 @@ static void rtl8192_rx_nomal(struct sk_buff *skb)
 		skb_trim(skb, skb->len - 4/*sCrcLng*/);
 
 		rx_pkt_len = skb->len;
-		ieee80211_hdr = (struct ieee80211_hdr_1addr *)skb->data;
+		ieee80211_hdr = (struct rtl_80211_hdr_1addr *)skb->data;
 		unicast_packet = false;
 		if (is_broadcast_ether_addr(ieee80211_hdr->addr1)) {
 			//TODO
@@ -4615,7 +4615,7 @@ static void rtl8192_rx_cmd(struct sk_buff *skb)
 	}
 }
 
-void rtl8192_irq_rx_tasklet(struct r8192_priv *priv)
+static void rtl8192_irq_rx_tasklet(struct r8192_priv *priv)
 {
 	struct sk_buff *skb;
 	struct rtl8192_rx_info *info;
@@ -4733,7 +4733,7 @@ fail:
 }
 
 //detach all the work and timer structure declared or inititialize in r8192U_init function.
-void rtl8192_cancel_deferred_work(struct r8192_priv *priv)
+static void rtl8192_cancel_deferred_work(struct r8192_priv *priv)
 {
 
 	cancel_work_sync(&priv->reset_wq);
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index 12dd19e1159b..7ca5d8fbc57f 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -120,7 +120,7 @@ static	void	dm_ctstoself(struct net_device *dev);
  *		Prepare SW resource for HW dynamic mechanism.
  *
  *	Assumption:
- *		This function is only invoked at driver intialization once.
+ *		This function is only invoked at driver initialization once.
  */
 void init_hal_dm(struct net_device *dev)
 {
@@ -563,7 +563,7 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 					break;
 				}
 			}
-			if (viviflag == true) {
+			if (viviflag) {
 				write_nic_byte(dev, 0x1ba, 0);
 				viviflag = false;
 				RT_TRACE(COMP_POWER_TRACKING, "we filtered the data\n");
@@ -766,7 +766,7 @@ void dm_txpower_trackingcallback(struct work_struct *work)
 	struct r8192_priv *priv = container_of(dwork, struct r8192_priv, txpower_tracking_wq);
 	struct net_device *dev = priv->ieee80211->dev;
 
-	if (priv->bDcut == true)
+	if (priv->bDcut)
 		dm_TXPowerTrackingCallback_TSSI(dev);
 	else
 		dm_TXPowerTrackingCallback_ThermalMeter(dev);
@@ -1301,7 +1301,7 @@ void dm_initialize_txpower_tracking(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	if (priv->bDcut == true)
+	if (priv->bDcut)
 		dm_InitializeTXPowerTracking_TSSI(dev);
 	else
 		dm_InitializeTXPowerTracking_ThermalMeter(dev);
@@ -1357,7 +1357,7 @@ static void dm_check_txpower_tracking(struct net_device *dev)
 #ifdef RTL8190P
 	dm_CheckTXPowerTracking_TSSI(dev);
 #else
-	if (priv->bDcut == true)
+	if (priv->bDcut)
 		dm_CheckTXPowerTracking_TSSI(dev);
 	else
 		dm_CheckTXPowerTracking_ThermalMeter(dev);
@@ -1467,7 +1467,7 @@ void dm_cck_txpower_adjust(struct net_device *dev, bool binch14)
 {	/*  dm_CCKTxPowerAdjust */
 	struct r8192_priv *priv = ieee80211_priv(dev);
 
-	if (priv->bDcut == true)
+	if (priv->bDcut)
 		dm_CCKTxPowerAdjust_TSSI(dev, binch14);
 	else
 		dm_CCKTxPowerAdjust_ThermalMeter(dev, binch14);
@@ -3062,7 +3062,7 @@ static void dm_dynamic_txpower(struct net_device *dev)
 			priv->bDynamicTxLowPower = false;
 		} else {
 			/* high power state check */
-			if (priv->undecorated_smoothed_pwdb < txlowpower_threshold && priv->bDynamicTxHighPower == true)
+			if (priv->undecorated_smoothed_pwdb < txlowpower_threshold && priv->bDynamicTxHighPower)
 				priv->bDynamicTxHighPower = false;
 
 			/* low power state check */
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.h b/drivers/staging/rtl8192u/r819xU_firmware.h
index cfa222350a9a..24b63f2ec509 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware.h
+++ b/drivers/staging/rtl8192u/r819xU_firmware.h
@@ -2,15 +2,8 @@
 #define __INC_FIRMWARE_H
 
 #define RTL8190_CPU_START_OFFSET	0x80
-/* TODO: this definition is TBD */
-//#define USB_HWDESC_HEADER_LEN	0
-
-/* It should be double word alignment */
-//#if DEV_BUS_TYPE==PCI_INTERFACE
-//#define GET_COMMAND_PACKET_FRAG_THRESHOLD(v)	4*(v/4) - 8
-//#else
-#define GET_COMMAND_PACKET_FRAG_THRESHOLD(v)	(4*(v/4) - 8 - USB_HWDESC_HEADER_LEN)
-//#endif
+#define GET_COMMAND_PACKET_FRAG_THRESHOLD(v) \
+	(4*(v/4) - 8 - USB_HWDESC_HEADER_LEN)
 
 typedef enum _firmware_init_step {
 	FW_INIT_STEP0_BOOT = 0,
diff --git a/drivers/staging/rtl8712/ieee80211.h b/drivers/staging/rtl8712/ieee80211.h
index 8269be80437a..6e813a9c1aa2 100644
--- a/drivers/staging/rtl8712/ieee80211.h
+++ b/drivers/staging/rtl8712/ieee80211.h
@@ -314,35 +314,6 @@ struct ieee80211_snap_hdr {
 #define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
 #define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
 
-/* Status codes */
-#define WLAN_STATUS_SUCCESS 0
-#define WLAN_STATUS_UNSPECIFIED_FAILURE 1
-#define WLAN_STATUS_CAPS_UNSUPPORTED 10
-#define WLAN_STATUS_REASSOC_NO_ASSOC 11
-#define WLAN_STATUS_ASSOC_DENIED_UNSPEC 12
-#define WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG 13
-#define WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION 14
-#define WLAN_STATUS_CHALLENGE_FAIL 15
-#define WLAN_STATUS_AUTH_TIMEOUT 16
-#define WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA 17
-#define WLAN_STATUS_ASSOC_DENIED_RATES 18
-/* 802.11b */
-#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
-#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
-#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
-
-/* Reason codes */
-#define WLAN_REASON_UNSPECIFIED 1
-#define WLAN_REASON_PREV_AUTH_NOT_VALID 2
-#define WLAN_REASON_DEAUTH_LEAVING 3
-#define WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY 4
-#define WLAN_REASON_DISASSOC_AP_BUSY 5
-#define WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA 6
-#define WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA 7
-#define WLAN_REASON_DISASSOC_STA_HAS_LEFT 8
-#define WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH 9
-
-
 /* Information Element IDs */
 #define WLAN_EID_SSID 0
 #define WLAN_EID_SUPP_RATES 1
diff --git a/drivers/staging/rtl8712/os_intfs.c b/drivers/staging/rtl8712/os_intfs.c
index 6e776e5433f6..5d551a1ba3dc 100644
--- a/drivers/staging/rtl8712/os_intfs.c
+++ b/drivers/staging/rtl8712/os_intfs.c
@@ -122,13 +122,11 @@ module_param(low_power, int, 0644);
 MODULE_PARM_DESC(ifname, " Net interface name, wlan%d=default");
 MODULE_PARM_DESC(initmac, "MAC-Address, default: use FUSE");
 
-static uint loadparam(struct _adapter *padapter, struct  net_device *pnetdev);
 static int netdev_open(struct net_device *pnetdev);
 static int netdev_close(struct net_device *pnetdev);
 
-static uint loadparam(struct _adapter *padapter, struct  net_device *pnetdev)
+static void loadparam(struct _adapter *padapter, struct  net_device *pnetdev)
 {
-	uint status = _SUCCESS;
 	struct registry_priv  *registry_par = &padapter->registrypriv;
 
 	registry_par->chip_version = (u8)chip_version;
@@ -172,7 +170,6 @@ static uint loadparam(struct _adapter *padapter, struct  net_device *pnetdev)
 	registry_par->low_power = (u8)low_power;
 	registry_par->wifi_test = (u8) wifi_test;
 	r8712_initmac = initmac;
-	return status;
 }
 
 static int r871x_net_set_mac_address(struct net_device *pnetdev, void *p)
@@ -181,7 +178,7 @@ static int r871x_net_set_mac_address(struct net_device *pnetdev, void *p)
 	struct sockaddr *addr = p;
 
 	if (padapter->bup == false)
-		memcpy(pnetdev->dev_addr, addr->sa_data, ETH_ALEN);
+		ether_addr_copy(pnetdev->dev_addr, addr->sa_data);
 	return 0;
 }
 
@@ -228,7 +225,6 @@ struct net_device *r8712_init_netdev(void)
 	pnetdev->watchdog_timeo = HZ; /* 1 second timeout */
 	pnetdev->wireless_handlers = (struct iw_handler_def *)
 				     &r871x_handlers_def;
-	/*step 2.*/
 	loadparam(padapter, pnetdev);
 	netif_carrier_off(pnetdev);
 	padapter->pid = 0;  /* Initial the PID value used for HW PBC.*/
diff --git a/drivers/staging/rtl8712/recv_linux.c b/drivers/staging/rtl8712/recv_linux.c
index 799a0f9a5b2d..4201ce776e0c 100644
--- a/drivers/staging/rtl8712/recv_linux.c
+++ b/drivers/staging/rtl8712/recv_linux.c
@@ -132,9 +132,9 @@ void r8712_recv_indicatepkt(struct _adapter *padapter,
 	return;
 _recv_indicatepkt_drop:
 	 /*enqueue back to free_recv_queue*/
-	 if (precv_frame)
+	if (precv_frame)
 		r8712_free_recvframe(precv_frame, pfree_recv_queue);
-	 precvpriv->rx_drop++;
+	precvpriv->rx_drop++;
 }
 
 static void _r8712_reordering_ctrl_timeout_handler (unsigned long data)
diff --git a/drivers/staging/rtl8712/rtl8712_led.c b/drivers/staging/rtl8712/rtl8712_led.c
index f1d47a0676c3..ada8d5dafd49 100644
--- a/drivers/staging/rtl8712/rtl8712_led.c
+++ b/drivers/staging/rtl8712/rtl8712_led.c
@@ -898,11 +898,11 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			  IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedNoLinkBlinkInProgress = true;
@@ -921,11 +921,11 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedLinkBlinkInProgress = true;
@@ -946,15 +946,15 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			if (IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				 pLed->bLedLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -975,11 +975,11 @@ static void SwLedControlMode1(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedLinkBlinkInProgress = false;
 			}
 			pLed->bLedBlinkInProgress = true;
@@ -998,19 +998,19 @@ static void SwLedControlMode1(struct _adapter *padapter,
 	case LED_CTL_START_WPS_BOTTON:
 		 if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				 pLed->bLedLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedScanBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedScanBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1025,23 +1025,23 @@ static void SwLedControlMode1(struct _adapter *padapter,
 		break;
 	case LED_CTL_STOP_WPS:
 		if (pLed->bLedNoLinkBlinkInProgress == true) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedNoLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedLinkBlinkInProgress == true) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			 pLed->bLedLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedBlinkInProgress == true) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress == true) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress)
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 		else
 			pLed->bLedWPSBlinkInProgress = true;
 		pLed->CurrLedState = LED_BLINK_WPS_STOP;
@@ -1057,7 +1057,7 @@ static void SwLedControlMode1(struct _adapter *padapter,
 		break;
 	case LED_CTL_STOP_WPS_FAIL:
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->bLedNoLinkBlinkInProgress = true;
@@ -1073,23 +1073,23 @@ static void SwLedControlMode1(struct _adapter *padapter,
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedNoLinkBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedNoLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedLinkBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		mod_timer(&pLed->BlinkTimer,
@@ -1116,7 +1116,7 @@ static void SwLedControlMode2(struct _adapter *padapter,
 				return;
 
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -1154,11 +1154,11 @@ static void SwLedControlMode2(struct _adapter *padapter,
 		pLed->CurrLedState = LED_ON;
 		pLed->BlinkingLedState = LED_ON;
 		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 
@@ -1170,11 +1170,11 @@ static void SwLedControlMode2(struct _adapter *padapter,
 	case LED_CTL_START_WPS_BOTTON:
 		if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedScanBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedScanBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1214,15 +1214,15 @@ static void SwLedControlMode2(struct _adapter *padapter,
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		mod_timer(&pLed->BlinkTimer,
@@ -1248,7 +1248,7 @@ static void SwLedControlMode3(struct _adapter *padapter,
 			if (IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -1286,11 +1286,11 @@ static void SwLedControlMode3(struct _adapter *padapter,
 		pLed->CurrLedState = LED_ON;
 		pLed->BlinkingLedState = LED_ON;
 		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		mod_timer(&pLed->BlinkTimer,
@@ -1300,11 +1300,11 @@ static void SwLedControlMode3(struct _adapter *padapter,
 	case LED_CTL_START_WPS_BOTTON:
 		if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedScanBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedScanBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1319,7 +1319,7 @@ static void SwLedControlMode3(struct _adapter *padapter,
 		break;
 	case LED_CTL_STOP_WPS:
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&(pLed->BlinkTimer));
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		} else
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1336,7 +1336,7 @@ static void SwLedControlMode3(struct _adapter *padapter,
 		break;
 	case LED_CTL_STOP_WPS_FAIL:
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->CurrLedState = LED_OFF;
@@ -1357,15 +1357,15 @@ static void SwLedControlMode3(struct _adapter *padapter,
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		mod_timer(&pLed->BlinkTimer,
@@ -1388,7 +1388,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 	case LED_CTL_START_TO_LINK:
 		if (pLed1->bLedWPSBlinkInProgress) {
 			pLed1->bLedWPSBlinkInProgress = false;
-			del_timer_sync(&pLed1->BlinkTimer);
+			del_timer(&pLed1->BlinkTimer);
 			pLed1->BlinkingLedState = LED_OFF;
 			pLed1->CurrLedState = LED_OFF;
 			if (pLed1->bLedOn)
@@ -1400,11 +1400,11 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			pLed->bLedStartToLinkBlinkInProgress = true;
@@ -1426,7 +1426,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 		if (LedAction == LED_CTL_LINK) {
 			if (pLed1->bLedWPSBlinkInProgress) {
 				pLed1->bLedWPSBlinkInProgress = false;
-				del_timer_sync(&pLed1->BlinkTimer);
+				del_timer(&pLed1->BlinkTimer);
 				pLed1->BlinkingLedState = LED_OFF;
 				pLed1->CurrLedState = LED_OFF;
 				if (pLed1->bLedOn)
@@ -1439,7 +1439,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedNoLinkBlinkInProgress = true;
@@ -1460,11 +1460,11 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			if (IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -1485,7 +1485,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			    IS_LED_WPS_BLINKING(pLed))
 				return;
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			pLed->bLedBlinkInProgress = true;
@@ -1503,7 +1503,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 	case LED_CTL_START_WPS_BOTTON:
 		if (pLed1->bLedWPSBlinkInProgress) {
 			pLed1->bLedWPSBlinkInProgress = false;
-			del_timer_sync(&(pLed1->BlinkTimer));
+			del_timer(&pLed1->BlinkTimer);
 			pLed1->BlinkingLedState = LED_OFF;
 			pLed1->CurrLedState = LED_OFF;
 			if (pLed1->bLedOn)
@@ -1512,15 +1512,15 @@ static void SwLedControlMode4(struct _adapter *padapter,
 		}
 		if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedNoLinkBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedNoLinkBlinkInProgress = false;
 			}
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			if (pLed->bLedScanBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedScanBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1538,7 +1538,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 		break;
 	case LED_CTL_STOP_WPS:	/*WPS connect success*/
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->bLedNoLinkBlinkInProgress = true;
@@ -1552,7 +1552,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 		break;
 	case LED_CTL_STOP_WPS_FAIL:	/*WPS authentication fail*/
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->bLedNoLinkBlinkInProgress = true;
@@ -1565,7 +1565,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		/*LED1 settings*/
 		if (pLed1->bLedWPSBlinkInProgress)
-			del_timer_sync(&pLed1->BlinkTimer);
+			del_timer(&pLed1->BlinkTimer);
 		else
 			pLed1->bLedWPSBlinkInProgress = true;
 		pLed1->CurrLedState = LED_BLINK_WPS_STOP;
@@ -1578,7 +1578,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 		break;
 	case LED_CTL_STOP_WPS_FAIL_OVERLAP:	/*WPS session overlap*/
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->bLedNoLinkBlinkInProgress = true;
@@ -1591,7 +1591,7 @@ static void SwLedControlMode4(struct _adapter *padapter,
 			  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 		/*LED1 settings*/
 		if (pLed1->bLedWPSBlinkInProgress)
-			del_timer_sync(&pLed1->BlinkTimer);
+			del_timer(&pLed1->BlinkTimer);
 		else
 			pLed1->bLedWPSBlinkInProgress = true;
 		pLed1->CurrLedState = LED_BLINK_WPS_STOP_OVERLAP;
@@ -1607,31 +1607,31 @@ static void SwLedControlMode4(struct _adapter *padapter,
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedNoLinkBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedNoLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedLinkBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedLinkBlinkInProgress = false;
 		}
 		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		if (pLed->bLedScanBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedScanBlinkInProgress = false;
 		}
 		if (pLed->bLedStartToLinkBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedStartToLinkBlinkInProgress = false;
 		}
 		if (pLed1->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed1->BlinkTimer);
+			del_timer(&pLed1->BlinkTimer);
 			pLed1->bLedWPSBlinkInProgress = false;
 		}
 		pLed1->BlinkingLedState = LED_UNKNOWN;
@@ -1671,7 +1671,7 @@ static void SwLedControlMode5(struct _adapter *padapter,
 			; /* dummy branch */
 		else if (pLed->bLedScanBlinkInProgress == false) {
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedScanBlinkInProgress = true;
@@ -1705,7 +1705,7 @@ static void SwLedControlMode5(struct _adapter *padapter,
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		SwLedOff(padapter, pLed);
@@ -1756,7 +1756,7 @@ static void SwLedControlMode6(struct _adapter *padapter,
 	case LED_CTL_START_WPS_BOTTON:
 		if (pLed->bLedWPSBlinkInProgress == false) {
 			if (pLed->bLedBlinkInProgress == true) {
-				del_timer_sync(&pLed->BlinkTimer);
+				del_timer(&pLed->BlinkTimer);
 				pLed->bLedBlinkInProgress = false;
 			}
 			pLed->bLedWPSBlinkInProgress = true;
@@ -1772,7 +1772,7 @@ static void SwLedControlMode6(struct _adapter *padapter,
 	case LED_CTL_STOP_WPS_FAIL:
 	case LED_CTL_STOP_WPS:
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		pLed->CurrLedState = LED_ON;
@@ -1784,11 +1784,11 @@ static void SwLedControlMode6(struct _adapter *padapter,
 		pLed->CurrLedState = LED_OFF;
 		pLed->BlinkingLedState = LED_OFF;
 		if (pLed->bLedBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedBlinkInProgress = false;
 		}
 		if (pLed->bLedWPSBlinkInProgress) {
-			del_timer_sync(&pLed->BlinkTimer);
+			del_timer(&pLed->BlinkTimer);
 			pLed->bLedWPSBlinkInProgress = false;
 		}
 		SwLedOff(padapter, pLed);
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index 50227b598e0c..fcb8c61b2884 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -1056,7 +1056,8 @@ static int recvbuf2recvframe(struct _adapter *padapter, struct sk_buff *pskb)
 		/* for first fragment packet, driver need allocate 1536 +
 		 * drvinfo_sz + RXDESC_SIZE to defrag packet. */
 		if ((mf == 1) && (frag == 0))
-			alloc_sz = 1658;/*1658+6=1664, 1664 is 128 alignment.*/
+			/*1658+6=1664, 1664 is 128 alignment.*/
+			alloc_sz = max_t(u16, tmp_len, 1658);
 		else
 			alloc_sz = tmp_len;
 		/* 2 is for IP header 4 bytes alignment in QoS packet case.
diff --git a/drivers/staging/rtl8712/rtl8712_xmit.c b/drivers/staging/rtl8712/rtl8712_xmit.c
index a3093ac1204b..86206d34a40e 100644
--- a/drivers/staging/rtl8712/rtl8712_xmit.c
+++ b/drivers/staging/rtl8712/rtl8712_xmit.c
@@ -188,7 +188,7 @@ static struct xmit_frame *dequeue_xframe_ex(struct xmit_priv *pxmitpriv,
 	struct  __queue *pframe_queue = NULL;
 	struct	xmit_frame *pxmitframe = NULL;
 	int i, inx[4];
-	int j, tmp, acirp_cnt[4];
+	int j, acirp_cnt[4];
 
 	/*entry indx: 0->vo, 1->vi, 2->be, 3->bk.*/
 	inx[0] = 0; acirp_cnt[0] = pxmitpriv->voq_cnt;
@@ -198,12 +198,8 @@ static struct xmit_frame *dequeue_xframe_ex(struct xmit_priv *pxmitpriv,
 	for (i = 0; i < 4; i++) {
 		for (j = i + 1; j < 4; j++) {
 			if (acirp_cnt[j] < acirp_cnt[i]) {
-				tmp = acirp_cnt[i];
-				acirp_cnt[i] = acirp_cnt[j];
-				acirp_cnt[j] = tmp;
-				tmp = inx[i];
-				inx[i] = inx[j];
-				inx[j] = tmp;
+				swap(acirp_cnt[i], acirp_cnt[j]);
+				swap(inx[i], inx[j]);
 			}
 		}
 	}
@@ -340,7 +336,7 @@ u8 r8712_append_mpdu_unit(struct xmit_buf *pxmitbuf,
 u8 r8712_xmitframe_aggr_1st(struct xmit_buf *pxmitbuf,
 			struct xmit_frame *pxmitframe)
 {
-	/* linux complete context doesnt need to protect */
+	/* linux complete context doesn't need to protect */
 	pxmitframe->pxmitbuf = pxmitbuf;
 	pxmitbuf->priv_data = pxmitframe;
 	pxmitframe->pxmit_urb[0] = pxmitbuf->pxmit_urb[0];
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.c b/drivers/staging/rtl8712/rtl871x_cmd.c
index 1a1c38f885d6..e35854d28f90 100644
--- a/drivers/staging/rtl8712/rtl871x_cmd.c
+++ b/drivers/staging/rtl8712/rtl871x_cmd.c
@@ -910,7 +910,7 @@ void r8712_createbss_cmd_callback(struct _adapter *padapter,
 	if (pcmd->res != H2C_SUCCESS)
 		mod_timer(&pmlmepriv->assoc_timer,
 			  jiffies + msecs_to_jiffies(1));
-	del_timer_sync(&pmlmepriv->assoc_timer);
+	del_timer(&pmlmepriv->assoc_timer);
 #ifdef __BIG_ENDIAN
 	/* endian_convert */
 	pnetwork->Length = le32_to_cpu(pnetwork->Length);
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index cb0b6387789f..3388f971fb48 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -813,8 +813,7 @@ static int r871x_wx_set_pmkid(struct net_device *dev,
 	case IW_PMKSA_ADD:
 		if (!memcmp(strIssueBssid, strZeroMacAddress, ETH_ALEN))
 			return intReturn;
-		else
-			intReturn = true;
+		intReturn = true;
 		blInserted = false;
 		/* overwrite PMKID */
 		for (j = 0; j < NUM_PMKID_CACHE; j++) {
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index fb2b195b90af..c044b0e55ba9 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -582,7 +582,7 @@ void r8712_surveydone_event_callback(struct _adapter *adapter, u8 *pbuf)
 	spin_lock_irqsave(&pmlmepriv->lock, irqL);
 
 	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true) {
-		del_timer_sync(&pmlmepriv->scan_to_timer);
+		del_timer(&pmlmepriv->scan_to_timer);
 
 		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
 	}
@@ -696,7 +696,7 @@ void r8712_ind_disconnect(struct _adapter *padapter)
 	}
 	if (padapter->pwrctrlpriv.pwr_mode !=
 	    padapter->registrypriv.power_mgnt) {
-		del_timer_sync(&pmlmepriv->dhcp_timer);
+		del_timer(&pmlmepriv->dhcp_timer);
 		r8712_set_ps_mode(padapter, padapter->registrypriv.power_mgnt,
 				  padapter->registrypriv.smart_ps);
 	}
@@ -910,7 +910,7 @@ void r8712_joinbss_event_callback(struct _adapter *adapter, u8 *pbuf)
 			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)
 				== true)
 				r8712_indicate_connect(adapter);
-			del_timer_sync(&pmlmepriv->assoc_timer);
+			del_timer(&pmlmepriv->assoc_timer);
 		} else
 			goto ignore_joinbss_callback;
 	} else {
diff --git a/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h b/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h
index 8e2586231ffd..2e9120a21a0b 100644
--- a/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h
+++ b/drivers/staging/rtl8712/rtl871x_mp_phy_regdef.h
@@ -603,7 +603,7 @@
 #define	bCCKRxIG		0x7f00
 #define	bCCKLNAPolarity		0x800000
 #define	bCCKRx1stGain		0x7f0000
-#define	bCCKRFExtend		0x20000000 /* CCK Rx inital gain polarity */
+#define	bCCKRFExtend		0x20000000 /* CCK Rx initial gain polarity */
 #define	bCCKRxAGCSatLevel	0x1f000000
 #define	bCCKRxAGCSatCount       0xe0
 #define	bCCKRxRFSettle          0x1f       /* AGCsamp_dly */
diff --git a/drivers/staging/rtl8712/rtl871x_pwrctrl.c b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
index aaa584435c87..9bc04f474d18 100644
--- a/drivers/staging/rtl8712/rtl871x_pwrctrl.c
+++ b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
@@ -103,7 +103,7 @@ void r8712_cpwm_int_hdl(struct _adapter *padapter,
 
 	if (pwrpriv->cpwm_tog == ((preportpwrstate->state) & 0x80))
 		return;
-	del_timer_sync(&padapter->pwrctrlpriv.rpwm_check_timer);
+	del_timer(&padapter->pwrctrlpriv.rpwm_check_timer);
 	_enter_pwrlock(&pwrpriv->lock);
 	pwrpriv->cpwm = (preportpwrstate->state) & 0xf;
 	if (pwrpriv->cpwm >= PS_STATE_S2) {
diff --git a/drivers/staging/rtl8712/rtl871x_sta_mgt.c b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
index 7bb96c47f188..6ae8cdc1bfd1 100644
--- a/drivers/staging/rtl8712/rtl871x_sta_mgt.c
+++ b/drivers/staging/rtl8712/rtl871x_sta_mgt.c
@@ -198,7 +198,7 @@ void r8712_free_stainfo(struct _adapter *padapter, struct sta_info *psta)
 	 * cancel reordering_ctrl_timer */
 	for (i = 0; i < 16; i++) {
 		preorder_ctrl = &psta->recvreorder_ctrl[i];
-		del_timer_sync(&preorder_ctrl->reordering_ctrl_timer);
+		del_timer(&preorder_ctrl->reordering_ctrl_timer);
 	}
 	spin_lock(&(pfree_sta_queue->lock));
 	/* insert into free_sta_queue; 20061114 */
@@ -270,12 +270,10 @@ void r8712_init_bcmc_stainfo(struct _adapter *padapter)
 
 struct sta_info *r8712_get_bcmc_stainfo(struct _adapter *padapter)
 {
-	struct sta_info *psta;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
-	psta = r8712_get_stainfo(pstapriv, bc_addr);
-	return psta;
+	return r8712_get_stainfo(pstapriv, bc_addr);
 }
 
 
diff --git a/drivers/staging/rtl8723au/core/rtw_ap.c b/drivers/staging/rtl8723au/core/rtw_ap.c
index 645668950e9c..65b209a20c29 100644
--- a/drivers/staging/rtl8723au/core/rtw_ap.c
+++ b/drivers/staging/rtl8723au/core/rtw_ap.c
@@ -1642,7 +1642,7 @@ u8 ap_free_sta23a(struct rtw_adapter *padapter, struct sta_info *psta, bool acti
 	return beacon_updated;
 }
 
-int rtw_ap_inform_ch_switch23a (struct rtw_adapter *padapter, u8 new_ch, u8 ch_offset)
+int rtw_ap_inform_ch_switch23a(struct rtw_adapter *padapter, u8 new_ch, u8 ch_offset)
 {
 	struct list_head *phead, *plist;
 	struct sta_info *psta = NULL;
@@ -1663,12 +1663,12 @@ int rtw_ap_inform_ch_switch23a (struct rtw_adapter *padapter, u8 new_ch, u8 ch_o
 	list_for_each(plist, phead) {
 		psta = container_of(plist, struct sta_info, asoc_list);
 
-		issue_action_spct_ch_switch23a (padapter, psta->hwaddr, new_ch, ch_offset);
+		issue_action_spct_ch_switch23a(padapter, psta->hwaddr, new_ch, ch_offset);
 		psta->expire_to = ((pstapriv->expire_to * 2) > 5) ? 5 : (pstapriv->expire_to * 2);
 	}
 	spin_unlock_bh(&pstapriv->asoc_list_lock);
 
-	issue_action_spct_ch_switch23a (padapter, bc_addr, new_ch, ch_offset);
+	issue_action_spct_ch_switch23a(padapter, bc_addr, new_ch, ch_offset);
 
 	return 0;
 }
@@ -1871,7 +1871,7 @@ void stop_ap_mode23a(struct rtw_adapter *padapter)
 	pmlmeext->bstart_bss = false;
 
 	/* reset and init security priv , this can refine with rtw_reset_securitypriv23a */
-	memset((unsigned char *)&padapter->securitypriv, 0, sizeof (struct security_priv));
+	memset((unsigned char *)&padapter->securitypriv, 0, sizeof(struct security_priv));
 	padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
 	padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
 
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
index 196beafde6f0..be9a3d560a43 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
@@ -51,8 +51,7 @@ static void issue_probereq(struct rtw_adapter *padapter,
 static int issue_probereq_ex(struct rtw_adapter *padapter,
 			     struct cfg80211_ssid *pssid,
 			     u8 *da, int try_cnt, int wait_ms);
-static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da,
-			   u8 is_valid_p2p_probereq);
+static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da);
 static void issue_auth(struct rtw_adapter *padapter, struct sta_info *psta,
 		       unsigned short status);
 static int issue_deauth_ex(struct rtw_adapter *padapter, u8 *da,
@@ -760,7 +759,7 @@ OnProbeReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 
 	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
 	    pmlmepriv->cur_network.join_res)
-		issue_probersp(padapter, mgmt->sa, false);
+		issue_probersp(padapter, mgmt->sa);
 
 out:
 	return _SUCCESS;
@@ -1718,7 +1717,6 @@ OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 	/*  now the station is qualified to join our BSS... */
 	if (pstat && pstat->state & WIFI_FW_ASSOC_SUCCESS &&
 	    status == WLAN_STATUS_SUCCESS) {
-#ifdef CONFIG_8723AU_AP_MODE
 		/* 1 bss_cap_update & sta_info_update23a */
 		bss_cap_update_on_sta_join23a(padapter, pstat);
 		sta_info_update23a(padapter, pstat);
@@ -1737,21 +1735,17 @@ OnAssocReq23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 
 		/* 3-(1) report sta add event */
 		report_add_sta_event23a(padapter, pstat->hwaddr, pstat->aid);
-#endif
 	}
 
 	return _SUCCESS;
 
 asoc_class2_error:
 
-#ifdef CONFIG_8723AU_AP_MODE
 	issue_deauth23a(padapter, mgmt->sa, status);
-#endif
 	return _FAIL;
 
 OnAssocReq23aFail:
 
-#ifdef CONFIG_8723AU_AP_MODE
 	pstat->aid = 0;
 	if (ieee80211_is_assoc_req(mgmt->frame_control))
 		issue_assocrsp(padapter, status, pstat,
@@ -1759,7 +1753,6 @@ OnAssocReq23aFail:
 	else
 		issue_assocrsp(padapter, status, pstat,
 			       IEEE80211_STYPE_REASSOC_RESP);
-#endif
 
 #endif /* CONFIG_8723AU_AP_MODE */
 
@@ -2503,8 +2496,7 @@ _issue_bcn:
 		dump_mgntframe23a(padapter, pmgntframe);
 }
 
-static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da,
-			   u8 is_valid_p2p_probereq)
+static void issue_probersp(struct rtw_adapter *padapter, unsigned char *da)
 {
 	struct xmit_frame *pmgntframe;
 	struct pkt_attrib *pattrib;
@@ -3803,8 +3795,6 @@ void issue_action_BA23a(struct rtw_adapter *padapter,
 
 	pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr) + 1;
 
-	status = cpu_to_le16(status);
-
 	switch (action) {
 	case WLAN_ACTION_ADDBA_REQ:
 		pattrib->pktlen += sizeof(mgmt->u.action.u.addba_req);
@@ -3908,8 +3898,8 @@ void issue_action_BA23a(struct rtw_adapter *padapter,
 		put_unaligned_le16(BA_para_set,
 				   &mgmt->u.action.u.addba_resp.capab);
 
-		put_unaligned_le16(pmlmeinfo->ADDBA_req.BA_timeout_value,
-				   &mgmt->u.action.u.addba_resp.timeout);
+		mgmt->u.action.u.addba_resp.timeout
+			= pmlmeinfo->ADDBA_req.BA_timeout_value;
 
 		pattrib->pktlen += 8;
 		break;
diff --git a/drivers/staging/rtl8723au/core/rtw_wlan_util.c b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
index 5280338aa387..3c1315fc02e5 100644
--- a/drivers/staging/rtl8723au/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
@@ -573,7 +573,7 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 	inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
 
 	if (pregpriv->wifi_spec == 1) {
-		u32 j, tmp, change_inx = false;
+		u32 j, change_inx = false;
 
 		/* entry indx: 0->vo, 1->vi, 2->be, 3->bk. */
 		for (i = 0; i < 4; i++) {
@@ -589,14 +589,8 @@ void WMMOnAssocRsp23a(struct rtw_adapter *padapter)
 				}
 
 				if (change_inx) {
-					tmp = edca[i];
-					edca[i] = edca[j];
-					edca[j] = tmp;
-
-					tmp = inx[i];
-					inx[i] = inx[j];
-					inx[j] = tmp;
-
+					swap(edca[i], edca[j]);
+					swap(inx[i], inx[j]);
 					change_inx = false;
 				}
 			}
diff --git a/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c b/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
index ae090ab11585..0a3d96e840cc 100644
--- a/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
+++ b/drivers/staging/rtl8723au/hal/HalPwrSeqCmd.c
@@ -92,7 +92,7 @@ u8 HalPwrSeqCmdParsing23a(struct rtw_adapter *padapter, u8 CutVersion,
 				value |= (GET_PWR_CFG_VALUE(PwrCfgCmd) &
 					  GET_PWR_CFG_MASK(PwrCfgCmd));
 
-				/*  Write the value back to sytem register */
+				/*  Write the value back to system register */
 				rtl8723au_write8(padapter, offset, value);
 				break;
 
diff --git a/drivers/staging/rtl8723au/hal/odm.c b/drivers/staging/rtl8723au/hal/odm.c
index ec543cfe1b45..f354f5e11a30 100644
--- a/drivers/staging/rtl8723au/hal/odm.c
+++ b/drivers/staging/rtl8723au/hal/odm.c
@@ -40,7 +40,7 @@ static u32 EDCAParam[HT_IOT_PEER_MAX][3] = {          /*  UL			DL */
 	{0x5ea42b, 0xa630, 0x5e431c}, /*  11:airgocap AP */
 };
 
-/*  EDCA Paramter for AP/ADSL   by Mingzhi 2011-11-22 */
+/*  EDCA Parameter for AP/ADSL   by Mingzhi 2011-11-22 */
 
 /*  Global var */
 u32 OFDMSwingTable23A[OFDM_TABLE_SIZE_92D] = {
@@ -388,7 +388,7 @@ void odm_CommonInfoSelfInit23a(struct dm_odm_t *pDM_Odm)
 		pDM_Odm->bCckHighPower = true;
 	else
 		pDM_Odm->bCckHighPower = false;
-		
+
 	pDM_Odm->RFPathRxEnable =
 		rtl8723au_read32(pDM_Odm->Adapter, rOFDM0_TRxPathEnable) & 0x0F;
 
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
index 11e1108d0c56..9733aa6ef908 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_cmd.c
@@ -55,7 +55,7 @@ int FillH2CCmd(struct rtw_adapter *padapter, u8 ElementID, u32 CmdLen,
 	u8 h2c_box_num;
 	u32 msgbox_addr;
 	u32 msgbox_ex_addr;
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(padapter);
+	struct hal_data_8723a *pHalData;
 	u32 h2c_cmd = 0;
 	u16 h2c_cmd_ex = 0;
 	int ret = _FAIL;
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
index 04d01833dc30..cb5076abda8b 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_hal_init.c
@@ -1396,7 +1396,7 @@ static void _DisableAnalog(struct rtw_adapter *padapter, bool bWithoutHWSM)
 		/* value16 |= (APDM_HOST | FSM_HSUS |/PFM_ALDN); */
 		/*  2010/08/31 According to Filen description, we need to
 		    use HW to shut down 8051 automatically. */
-		/*  Becasue suspend operatione need the asistance of 8051
+		/*  Because suspend operation need the asistance of 8051
 		    to wait for 3ms. */
 		value16 = APDM_HOST | AFSM_HSUS | PFM_ALDN;
 	} else {
@@ -1485,7 +1485,7 @@ void Hal_EfuseParseIDCode(struct rtw_adapter *padapter, u8 *hwinfo)
 	u16 EEPROMId;
 
 	/*  Checl 0x8129 again for making sure autoload status!! */
-	EEPROMId = le16_to_cpu(*((u16 *) hwinfo));
+	EEPROMId = le16_to_cpu(*((__le16 *) hwinfo));
 	if (EEPROMId != RTL_EEPROM_ID) {
 		DBG_8723A("EEPROM ID(%#x) is invalid!!\n", EEPROMId);
 		pEEPROM->bautoload_fail_flag = true;
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
index 46a30659c96f..7fa97808b951 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_phycfg.c
@@ -660,7 +660,7 @@ phy_BB8723a_Config_ParaFile(struct rtw_adapter *Adapter)
 
 	/*  */
 	/*  1. Read PHY_REG.TXT BB INIT!! */
-	/*  We will seperate as 88C / 92C according to chip version */
+	/*  We will separate as 88C / 92C according to chip version */
 	/*  */
 	ODM_ReadAndConfig_PHY_REG_1T_8723A(&pHalData->odmpriv);
 
diff --git a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
index 3e3f18634ffe..11d635d2eac8 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723a_rf6052.c
@@ -138,7 +138,7 @@ void rtl823a_phy_rf6052setccktxpower(struct rtw_adapter *Adapter,
 /*  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. */
+ *  and maintained independently. Thanks for Lanhsin's reminder. */
 		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level1) {
 			TxAGC[RF_PATH_A] = 0x10101010;
 			TxAGC[RF_PATH_B] = 0x10101010;
@@ -300,7 +300,7 @@ getTxPowerWriteValByRegulatory(struct rtw_adapter *Adapter, u8 Channel,
 /*  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. */
+    maintained independently. Thanks for Lanhsin's reminder. */
 
 		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level1)
 			writeVal = 0x14141414;
@@ -500,7 +500,6 @@ phy_RF6052_Config_ParaFile_Fail:
 int PHY_RF6052_Config8723A(struct rtw_adapter *Adapter)
 {
 	struct hal_data_8723a *pHalData = GET_HAL_DATA(Adapter);
-	int rtStatus = _SUCCESS;
 
 	/*  Initialize general global value */
 	/*  TODO: Extend RF_PATH_C and RF_PATH_D in the future */
@@ -510,8 +509,7 @@ int PHY_RF6052_Config8723A(struct rtw_adapter *Adapter)
 		pHalData->NumTotalRFPath = 2;
 
 	/*  Config BB and RF */
-	rtStatus = phy_RF6052_Config_ParaFile(Adapter);
-	return rtStatus;
+	return phy_RF6052_Config_ParaFile(Adapter);
 }
 
 /* End of HalRf6052.c */
diff --git a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
index 6bf87fe86644..14746dd8db78 100644
--- a/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
+++ b/drivers/staging/rtl8723au/hal/rtl8723au_xmit.c
@@ -252,7 +252,7 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz)
 	}
 
 	/*  (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 */
+	/*  mgnt frame should be controlled 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. */
diff --git a/drivers/staging/rtl8723au/hal/usb_halinit.c b/drivers/staging/rtl8723au/hal/usb_halinit.c
index 42ae29d26302..68156a13d00f 100644
--- a/drivers/staging/rtl8723au/hal/usb_halinit.c
+++ b/drivers/staging/rtl8723au/hal/usb_halinit.c
@@ -360,7 +360,7 @@ static void _InitWMACSetting(struct rtw_adapter *Adapter)
 	/*  2010.09.08 hpfan */
 	/*  Since ADF is removed from RCR, ps-poll will not be indicate
 	    to driver, */
-	/*  RxFilterMap should mask ps-poll to gurantee AP mode can
+	/*  RxFilterMap should mask ps-poll to guarantee AP mode can
 	    rx ps-poll. */
 	/* value16 = 0x400; */
 	/* rtl8723au_write16(Adapter, REG_RXFLTMAP1, value16); */
diff --git a/drivers/staging/rtl8723au/include/odm_debug.h b/drivers/staging/rtl8723au/include/odm_debug.h
index 83be5bab9e09..c4b375a6f409 100644
--- a/drivers/staging/rtl8723au/include/odm_debug.h
+++ b/drivers/staging/rtl8723au/include/odm_debug.h
@@ -22,7 +22,7 @@
 /*	Define the debug levels */
 /*  */
 /*	1.	DBG_TRACE and DBG_LOUD are used for normal cases. */
-/*	So that, they can help SW engineer to develope or trace states changed */
+/*	So that, they can help SW engineer to develop or trace states changed */
 /*	and also help HW enginner to trace every operation to and from HW, */
 /*	e.g IO, Tx, Rx. */
 /*  */
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_hal.h b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
index ad3a442bc000..77a0fd485b51 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_hal.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
@@ -80,12 +80,15 @@ struct rt_8723a_firmware_hdr {
 	/*  8-byte alinment required */
 
 	/*  LONG WORD 0 ---- */
-	u16		Signature;	/*  92C0: test chip; 92C, 88C0: test chip; 88C1: MP A-cut; 92C1: MP A-cut */
+	__le16		Signature;  /*
+				     * 92C0: test chip; 92C, 88C0: test chip;
+				     * 88C1: MP A-cut; 92C1: MP A-cut
+				     */
 	u8		Category;	/*  AP/NIC and USB/PCI */
 	u8		Function;	/*  Reserved for different FW function indcation, for further use when driver needs to download different FW in different conditions */
-	u16		Version;		/*  FW Version */
+	__le16		Version;		/*  FW Version */
 	u8		Subversion;	/*  FW Subversion, default 0x00 */
-	u16		Rsvd1;
+	u8		Rsvd1;
 
 
 	/*  LONG WORD 1 ---- */
@@ -93,16 +96,16 @@ struct rt_8723a_firmware_hdr {
 	u8		Date;	/*  Release time Date field */
 	u8		Hour;	/*  Release time Hour field */
 	u8		Minute;	/*  Release time Minute field */
-	u16		RamCodeSize;	/*  The size of RAM code */
-	u16		Rsvd2;
+	__le16		RamCodeSize;	/*  The size of RAM code */
+	__le16		Rsvd2;
 
 	/*  LONG WORD 2 ---- */
-	u32		SvnIdx;	/*  The SVN entry index */
-	u32		Rsvd3;
+	__le32		SvnIdx;	/*  The SVN entry index */
+	__le32		Rsvd3;
 
 	/*  LONG WORD 3 ---- */
-	u32		Rsvd4;
-	u32		Rsvd5;
+	__le32		Rsvd4;
+	__le32		Rsvd5;
 };
 
 #define DRIVER_EARLY_INT_TIME		0x05
@@ -193,7 +196,7 @@ enum ChannelPlan
 /*  |         |            Reserved(14bytes)	      | */
 /*  */
 
-/*  PG data exclude header, dummy 6 bytes frome CP test and reserved 1byte. */
+/*  PG data exclude header, dummy 6 bytes from CP test and reserved 1byte. */
 #define EFUSE_OOB_PROTECT_BYTES			15
 
 #define EFUSE_REAL_CONTENT_LEN_8723A	512
diff --git a/drivers/staging/rtl8723au/include/rtw_cmd.h b/drivers/staging/rtl8723au/include/rtw_cmd.h
index 775dcdc1e7b9..4dcc9253be51 100644
--- a/drivers/staging/rtl8723au/include/rtw_cmd.h
+++ b/drivers/staging/rtl8723au/include/rtw_cmd.h
@@ -657,7 +657,7 @@ struct TDLSoption_param {
 
 Result:
 0x00: success
-0x01: sucess, and check Response.
+0x01: success, and check Response.
 0x02: cmd ignored due to duplicated sequcne number
 0x03: cmd dropped due to invalid cmd code
 0x04: reserved.
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme.h b/drivers/staging/rtl8723au/include/rtw_mlme.h
index a6751f138336..dbd3a5f5c523 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme.h
@@ -50,11 +50,11 @@
 #define	WIFI_SITE_MONITOR	0x00000800
 
 #define	WIFI_MP_STATE		0x00010000
-#define	WIFI_MP_CTX_BACKGROUND	0x00020000	/*  in continous tx background */
-#define	WIFI_MP_CTX_ST		0x00040000	/*  in continous tx with single-tone */
-#define	WIFI_MP_CTX_BACKGROUND_PENDING	0x00080000	/*  pending in continous tx background due to out of skb */
-#define	WIFI_MP_CTX_CCK_HW	0x00100000	/*  in continous tx */
-#define	WIFI_MP_CTX_CCK_CS	0x00200000	/*  in continous tx with carrier suppression */
+#define	WIFI_MP_CTX_BACKGROUND	0x00020000	/*  in continuous tx background */
+#define	WIFI_MP_CTX_ST		0x00040000	/*  in continuous tx with single-tone */
+#define	WIFI_MP_CTX_BACKGROUND_PENDING	0x00080000	/*  pending in continuous tx background due to out of skb */
+#define	WIFI_MP_CTX_CCK_HW	0x00100000	/*  in continuous tx */
+#define	WIFI_MP_CTX_CCK_CS	0x00200000	/*  in continuous tx with carrier suppression */
 #define   WIFI_MP_LPBK_STATE	0x00400000
 
 #define _FW_UNDER_LINKING	WIFI_UNDER_LINKING
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
index ffb37b252fc1..ea2a6c914d38 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
@@ -103,7 +103,7 @@ extern unsigned char WMM_PARA_OUI23A[];
 /*  Note: */
 /*	We just add new channel plan when the new channel plan is different from any of the following */
 /*	channel plan. */
-/*	If you just wnat to customize the acitions(scan period or join actions) about one of the channel plan, */
+/*	If you just want to customize the actions(scan period or join actions) about one of the channel plan, */
 /*	customize them in struct rt_channel_info in the RT_CHANNEL_LIST. */
 /*  */
 enum  { /* _RT_CHANNEL_DOMAIN */
diff --git a/drivers/staging/rtl8723au/include/sta_info.h b/drivers/staging/rtl8723au/include/sta_info.h
index c756b4f7f5d5..e7260050e533 100644
--- a/drivers/staging/rtl8723au/include/sta_info.h
+++ b/drivers/staging/rtl8723au/include/sta_info.h
@@ -332,7 +332,7 @@ struct sta_priv {
 	 */
 	struct sta_info *sta_aid[NUM_STA];
 
-	u16 sta_dz_bitmap;/* only support 15 stations, staion aid bitmap
+	u16 sta_dz_bitmap;/* only support 15 stations, station aid bitmap
 			   * for sleeping sta. */
 	u16 tim_bitmap;/* only support 15 stations,
 			* aid=0~15 mapping bit0~bit15 */
diff --git a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
index bc95ce89af06..0ae2180a35b7 100644
--- a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
@@ -379,7 +379,7 @@ void rtw_cfg80211_indicate_disconnect(struct rtw_adapter *padapter)
 						GFP_ATOMIC);
 		} else {
 			cfg80211_disconnected(padapter->pnetdev, 0, NULL,
-					      0, GFP_ATOMIC);
+					      0, false, GFP_ATOMIC);
 		}
 	}
 }
@@ -1041,7 +1041,7 @@ static u16 rtw_get_cur_max_rate(struct rtw_adapter *adapter)
 		while (pcur_bss->SupportedRates[i] != 0 &&
 		       pcur_bss->SupportedRates[i] != 0xFF) {
 			rate = pcur_bss->SupportedRates[i] & 0x7F;
-			if (rate>max_rate)
+			if (rate > max_rate)
 				max_rate = rate;
 			i++;
 		}
diff --git a/drivers/staging/rtl8723au/os_dep/os_intfs.c b/drivers/staging/rtl8723au/os_dep/os_intfs.c
index 83696360c293..b8848c25beb4 100644
--- a/drivers/staging/rtl8723au/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8723au/os_dep/os_intfs.c
@@ -172,7 +172,7 @@ MODULE_PARM_DESC(debug, "Set debug level (1-9) (default 1)");
 
 static int netdev_close(struct net_device *pnetdev);
 
-static int loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
+static void loadparam(struct rtw_adapter *padapter,  struct net_device *pnetdev)
 {
 	struct registry_priv  *registry_par = &padapter->registrypriv;
 
@@ -233,7 +233,6 @@ 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 _SUCCESS;
 }
 
 static int rtw_net_set_mac_address(struct net_device *pnetdev, void *p)
@@ -372,7 +371,6 @@ struct net_device *rtw_init_netdev23a(struct rtw_adapter *old_padapter)
 
 	pnetdev->watchdog_timeo = HZ*3; /* 3 second timeout */
 
-	/* step 2. */
 	loadparam(padapter, pnetdev);
 	return pnetdev;
 }
diff --git a/drivers/staging/rts5208/rtsx.c b/drivers/staging/rts5208/rtsx.c
index d64b6ed9c0c9..b4e45436a4b9 100644
--- a/drivers/staging/rts5208/rtsx.c
+++ b/drivers/staging/rts5208/rtsx.c
@@ -230,7 +230,6 @@ static struct scsi_host_template rtsx_host_template = {
 
 	/* queue commands only, only one command per LUN */
 	.can_queue =			1,
-	.cmd_per_lun =			1,
 
 	/* unknown initiator id */
 	.this_id =			-1,
@@ -537,7 +536,7 @@ static int rtsx_polling_thread(void *__dev)
 	for (;;) {
 
 		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(POLLING_INTERVAL);
+		schedule_timeout(msecs_to_jiffies(POLLING_INTERVAL));
 
 		/* lock the device pointers */
 		mutex_lock(&(dev->dev_mutex));
diff --git a/drivers/staging/rts5208/rtsx.h b/drivers/staging/rts5208/rtsx.h
index 262441bcfc41..1396263e13e6 100644
--- a/drivers/staging/rts5208/rtsx.h
+++ b/drivers/staging/rts5208/rtsx.h
@@ -37,7 +37,7 @@
 #include <linux/cdrom.h>
 #include <linux/workqueue.h>
 #include <linux/timer.h>
-#include <linux/time.h>
+#include <linux/time64.h>
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
@@ -48,9 +48,6 @@
 
 #define CR_DRIVER_NAME		"rts5208"
 
-#define pci_get_bus_and_slot(bus, devfn)	\
-	pci_get_domain_bus_and_slot(0, (bus), (devfn))
-
 /*
  * macros for easy use
  */
@@ -148,21 +145,24 @@ static inline struct rtsx_dev *host_to_rtsx(struct Scsi_Host *host)
 
 static inline void get_current_time(u8 *timeval_buf, int buf_len)
 {
-	struct timeval tv;
+	struct timespec64 ts64;
+	u32 tv_usec;
 
 	if (!timeval_buf || (buf_len < 8))
 		return;
 
-	do_gettimeofday(&tv);
+	getnstimeofday64(&ts64);
+
+	tv_usec = ts64.tv_nsec/NSEC_PER_USEC;
 
-	timeval_buf[0] = (u8)(tv.tv_sec >> 24);
-	timeval_buf[1] = (u8)(tv.tv_sec >> 16);
-	timeval_buf[2] = (u8)(tv.tv_sec >> 8);
-	timeval_buf[3] = (u8)(tv.tv_sec);
-	timeval_buf[4] = (u8)(tv.tv_usec >> 24);
-	timeval_buf[5] = (u8)(tv.tv_usec >> 16);
-	timeval_buf[6] = (u8)(tv.tv_usec >> 8);
-	timeval_buf[7] = (u8)(tv.tv_usec);
+	timeval_buf[0] = (u8)(ts64.tv_sec >> 24);
+	timeval_buf[1] = (u8)(ts64.tv_sec >> 16);
+	timeval_buf[2] = (u8)(ts64.tv_sec >> 8);
+	timeval_buf[3] = (u8)(ts64.tv_sec);
+	timeval_buf[4] = (u8)(tv_usec >> 24);
+	timeval_buf[5] = (u8)(tv_usec >> 16);
+	timeval_buf[6] = (u8)(tv_usec >> 8);
+	timeval_buf[7] = (u8)(tv_usec);
 }
 
 /* The scsi_lock() and scsi_unlock() macros protect the sm_state and the
diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
index 0c1716ebc827..0df3b9d97134 100644
--- a/drivers/staging/rts5208/rtsx_chip.c
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -1143,137 +1143,281 @@ static void rtsx_monitor_aspm_config(struct rtsx_chip *chip)
 	}
 }
 
-void rtsx_polling_func(struct rtsx_chip *chip)
+static void rtsx_manage_ocp(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	if (!chip->ocp_int)
+		return;
+
+	rtsx_read_register(chip, OCPSTAT, &chip->ocp_stat);
+
+	if (chip->card_exist & SD_CARD)
+		sd_power_off_card3v3(chip);
+	else if (chip->card_exist & MS_CARD)
+		ms_power_off_card3v3(chip);
+	else if (chip->card_exist & XD_CARD)
+		xd_power_off_card3v3(chip);
+
+	chip->ocp_int = 0;
+#endif
+}
+
+static void rtsx_manage_sd_lock(struct rtsx_chip *chip)
 {
 #ifdef SUPPORT_SD_LOCK
 	struct sd_info *sd_card = &chip->sd_card;
-#endif
-	bool ss_allowed;
+	u8 val;
 
-	if (rtsx_chk_stat(chip, RTSX_STAT_SUSPEND))
+	if (!sd_card->sd_erase_status)
 		return;
 
-	if (rtsx_chk_stat(chip, RTSX_STAT_DELINK))
-		goto delink_stage;
+	if (chip->card_exist & SD_CARD) {
+		rtsx_read_register(chip, 0xFD30, &val);
+		if (val & 0x02) {
+			sd_card->sd_erase_status = SD_NOT_ERASE;
+			sd_card->sd_lock_notify = 1;
+			chip->need_reinit |= SD_CARD;
+		}
+	} else {
+		sd_card->sd_erase_status = SD_NOT_ERASE;
+	}
+#endif
+}
 
-	if (chip->polling_config) {
-		u8 val;
+static bool rtsx_is_ss_allowed(struct rtsx_chip *chip)
+{
+	u32 val;
 
-		rtsx_read_config_byte(chip, 0, &val);
+	if (!chip->ss_en || CHECK_PID(chip, 0x5288))
+		return false;
+
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
+		rtsx_read_cfg_dw(chip, 1, 0x04, &val);
+		if (val & 0x07)
+			return false;
 	}
 
-	if (rtsx_chk_stat(chip, RTSX_STAT_SS))
+	return true;
+}
+
+static void rtsx_manage_ss(struct rtsx_chip *chip)
+{
+	if (!rtsx_is_ss_allowed(chip) || chip->sd_io)
 		return;
 
-#ifdef SUPPORT_OCP
-	if (chip->ocp_int) {
-		rtsx_read_register(chip, OCPSTAT, &chip->ocp_stat);
+	if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
+		chip->ss_counter = 0;
+		return;
+	}
 
-		if (chip->card_exist & SD_CARD)
-			sd_power_off_card3v3(chip);
-		else if (chip->card_exist & MS_CARD)
-			ms_power_off_card3v3(chip);
-		else if (chip->card_exist & XD_CARD)
-			xd_power_off_card3v3(chip);
+	if (chip->ss_counter < (chip->ss_idle_period / POLLING_INTERVAL))
+		chip->ss_counter++;
+	else
+		rtsx_exclusive_enter_ss(chip);
+}
+
+static void rtsx_manage_aspm(struct rtsx_chip *chip)
+{
+	u8 data;
+
+	if (!CHECK_PID(chip, 0x5208))
+		return;
+
+	rtsx_monitor_aspm_config(chip);
 
-		chip->ocp_int = 0;
+#ifdef SUPPORT_SDIO_ASPM
+	if (!CHK_SDIO_EXIST(chip) || CHK_SDIO_IGNORED(chip) ||
+	    !chip->aspm_l0s_l1_en || !chip->dynamic_aspm)
+		return;
+
+	if (chip->sd_io) {
+		dynamic_configure_sdio_aspm(chip);
+		return;
 	}
+
+	if (chip->sdio_aspm)
+		return;
+
+	dev_dbg(rtsx_dev(chip), "SDIO enter ASPM!\n");
+	data = 0x30 | (chip->aspm_level[1] << 2);
+	rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFC, data);
+	chip->sdio_aspm = 1;
 #endif
+}
 
-#ifdef SUPPORT_SD_LOCK
-	if (sd_card->sd_erase_status) {
-		if (chip->card_exist & SD_CARD) {
-			u8 val;
-
-			rtsx_read_register(chip, 0xFD30, &val);
-			if (val & 0x02) {
-				sd_card->sd_erase_status = SD_NOT_ERASE;
-				sd_card->sd_lock_notify = 1;
-				chip->need_reinit |= SD_CARD;
-			}
-		} else {
-			sd_card->sd_erase_status = SD_NOT_ERASE;
-		}
+static void rtsx_manage_idle(struct rtsx_chip *chip)
+{
+	if (chip->idle_counter < IDLE_MAX_COUNT) {
+		chip->idle_counter++;
+		return;
 	}
+
+	if (rtsx_get_stat(chip) == RTSX_STAT_IDLE)
+		return;
+
+	dev_dbg(rtsx_dev(chip), "Idle state!\n");
+	rtsx_set_stat(chip, RTSX_STAT_IDLE);
+
+#if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
+	chip->led_toggle_counter = 0;
 #endif
+	rtsx_force_power_on(chip, SSC_PDCTL);
 
-	rtsx_init_cards(chip);
+	turn_off_led(chip, LED_GPIO);
 
-	if (chip->ss_en) {
-		ss_allowed = true;
+	if (chip->auto_power_down && !chip->card_ready && !chip->sd_io)
+		rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
+}
 
-		if (CHECK_PID(chip, 0x5288)) {
-			ss_allowed = false;
-		} else {
-			if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
-				u32 val;
+static void rtsx_manage_2lun_mode(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	u8 sd_oc, ms_oc;
 
-				rtsx_read_cfg_dw(chip, 1, 0x04, &val);
-				if (val & 0x07)
-					ss_allowed = false;
-			}
-		}
-	} else {
-		ss_allowed = false;
+	sd_oc = chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER);
+	ms_oc = chip->ocp_stat & (MS_OC_NOW | MS_OC_EVER);
+
+	if (sd_oc || ms_oc)
+		dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+			chip->ocp_stat);
+
+	if (sd_oc && (chip->card_exist & SD_CARD)) {
+		rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+		card_power_off(chip, SD_CARD);
+		chip->card_fail |= SD_CARD;
 	}
 
-	if (ss_allowed && !chip->sd_io) {
-		if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
-			chip->ss_counter = 0;
-		} else {
-			if (chip->ss_counter <
-				(chip->ss_idle_period / POLLING_INTERVAL)) {
-				chip->ss_counter++;
-			} else {
-				rtsx_exclusive_enter_ss(chip);
-				return;
-			}
-		}
+	if (ms_oc && (chip->card_exist & MS_CARD)) {
+		rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+		card_power_off(chip, MS_CARD);
+		chip->card_fail |= MS_CARD;
 	}
+#endif
+}
 
-	if (CHECK_PID(chip, 0x5208)) {
-		rtsx_monitor_aspm_config(chip);
+static void rtsx_manage_1lun_mode(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	if (!(chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)))
+		return;
 
-#ifdef SUPPORT_SDIO_ASPM
-		if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip) &&
-		    chip->aspm_l0s_l1_en && chip->dynamic_aspm) {
-			if (chip->sd_io) {
-				dynamic_configure_sdio_aspm(chip);
-			} else {
-				if (!chip->sdio_aspm) {
-					dev_dbg(rtsx_dev(chip), "SDIO enter ASPM!\n");
-					rtsx_write_register(chip,
-						ASPM_FORCE_CTL, 0xFC,
-						0x30 |
-						(chip->aspm_level[1] << 2));
-					chip->sdio_aspm = 1;
-				}
-			}
-		}
+	dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+		chip->ocp_stat);
+
+	if (chip->card_exist & SD_CARD) {
+		rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+		chip->card_fail |= SD_CARD;
+	} else if (chip->card_exist & MS_CARD) {
+		rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+		chip->card_fail |= MS_CARD;
+	} else if (chip->card_exist & XD_CARD) {
+		rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN, 0);
+		chip->card_fail |= XD_CARD;
+	}
+	card_power_off(chip, SD_CARD);
 #endif
+}
+
+static void rtsx_delink_stage1(struct rtsx_chip *chip, int enter_L1,
+			       int stage3_cnt)
+{
+	u8 val;
+
+	rtsx_set_stat(chip, RTSX_STAT_DELINK);
+
+	if (chip->asic_code && CHECK_PID(chip, 0x5208))
+		rtsx_set_phy_reg_bit(chip, 0x1C, 2);
+
+	if (chip->card_exist)
+		dev_dbg(rtsx_dev(chip), "False card inserted, do force delink\n");
+	else
+		dev_dbg(rtsx_dev(chip), "No card inserted, do delink\n");
+
+	if (enter_L1)
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 1);
+
+	if (chip->card_exist)
+		val = 0x02;
+	else
+		val = 0x0A;
+
+	rtsx_write_register(chip, CHANGE_LINK_STATE, val, val);
+
+	if (enter_L1)
+		rtsx_enter_L1(chip);
+
+	if (chip->card_exist)
+		chip->auto_delink_cnt = stage3_cnt + 1;
+}
+
+static void rtsx_delink_stage(struct rtsx_chip *chip)
+{
+	int delink_stage1_cnt, delink_stage2_cnt, delink_stage3_cnt;
+	int enter_L1;
+
+	if (!chip->auto_delink_en || !chip->auto_delink_allowed ||
+	    chip->card_ready || chip->card_ejected || chip->sd_io) {
+		chip->auto_delink_cnt = 0;
+		return;
 	}
 
-	if (chip->idle_counter < IDLE_MAX_COUNT) {
-		chip->idle_counter++;
-	} else {
-		if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
-			dev_dbg(rtsx_dev(chip), "Idle state!\n");
-			rtsx_set_stat(chip, RTSX_STAT_IDLE);
+	enter_L1 = chip->auto_delink_in_L1 &&
+		(chip->aspm_l0s_l1_en || chip->ss_en);
 
-#if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
-			chip->led_toggle_counter = 0;
-#endif
-			rtsx_force_power_on(chip, SSC_PDCTL);
+	delink_stage1_cnt = chip->delink_stage1_step;
+	delink_stage2_cnt = delink_stage1_cnt + chip->delink_stage2_step;
+	delink_stage3_cnt = delink_stage2_cnt + chip->delink_stage3_step;
 
-			turn_off_led(chip, LED_GPIO);
+	if (chip->auto_delink_cnt > delink_stage3_cnt)
+		return;
 
-			if (chip->auto_power_down && !chip->card_ready &&
-			    !chip->sd_io)
-				rtsx_force_power_down(chip,
-						      SSC_PDCTL | OC_PDCTL);
-		}
+	if (chip->auto_delink_cnt == delink_stage1_cnt)
+		rtsx_delink_stage1(chip, enter_L1, delink_stage3_cnt);
+
+	if (chip->auto_delink_cnt == delink_stage2_cnt) {
+		dev_dbg(rtsx_dev(chip), "Try to do force delink\n");
+
+		if (enter_L1)
+			rtsx_exit_L1(chip);
+
+		if (chip->asic_code && CHECK_PID(chip, 0x5208))
+			rtsx_set_phy_reg_bit(chip, 0x1C, 2);
+
+		rtsx_write_register(chip, CHANGE_LINK_STATE, 0x0A, 0x0A);
+	}
+
+	chip->auto_delink_cnt++;
+}
+
+void rtsx_polling_func(struct rtsx_chip *chip)
+{
+	if (rtsx_chk_stat(chip, RTSX_STAT_SUSPEND))
+		return;
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_DELINK))
+		goto delink_stage;
+
+	if (chip->polling_config) {
+		u8 val;
+
+		rtsx_read_config_byte(chip, 0, &val);
 	}
 
+	if (rtsx_chk_stat(chip, RTSX_STAT_SS))
+		return;
+
+	rtsx_manage_ocp(chip);
+
+	rtsx_manage_sd_lock(chip);
+
+	rtsx_init_cards(chip);
+
+	rtsx_manage_ss(chip);
+
+	rtsx_manage_aspm(chip);
+
+	rtsx_manage_idle(chip);
+
 	switch (rtsx_get_stat(chip)) {
 	case RTSX_STAT_RUN:
 #if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
@@ -1293,121 +1437,13 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		break;
 	}
 
-#ifdef SUPPORT_OCP
-	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
-		if (chip->ocp_stat &
-			(SD_OC_NOW | SD_OC_EVER | MS_OC_NOW | MS_OC_EVER))
-			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
-				chip->ocp_stat);
-
-		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);
-				card_power_off(chip, SD_CARD);
-				chip->card_fail |= SD_CARD;
-			}
-		}
-		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);
-				card_power_off(chip, MS_CARD);
-				chip->card_fail |= MS_CARD;
-			}
-		}
-	} else {
-		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
-			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
-				chip->ocp_stat);
-			if (chip->card_exist & SD_CARD) {
-				rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN,
-						    0);
-				chip->card_fail |= SD_CARD;
-			} else if (chip->card_exist & MS_CARD) {
-				rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
-						    0);
-				chip->card_fail |= MS_CARD;
-			} else if (chip->card_exist & XD_CARD) {
-				rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN,
-						    0);
-				chip->card_fail |= XD_CARD;
-			}
-			card_power_off(chip, SD_CARD);
-		}
-	}
-#endif
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
+		rtsx_manage_2lun_mode(chip);
+	else
+		rtsx_manage_1lun_mode(chip);
 
 delink_stage:
-	if (chip->auto_delink_en && chip->auto_delink_allowed &&
-	    !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;
-		int delink_stage2_cnt = delink_stage1_cnt +
-			chip->delink_stage2_step;
-		int delink_stage3_cnt = delink_stage2_cnt +
-			chip->delink_stage3_step;
-
-		if (chip->auto_delink_cnt <= delink_stage3_cnt) {
-			if (chip->auto_delink_cnt == delink_stage1_cnt) {
-				rtsx_set_stat(chip, RTSX_STAT_DELINK);
-
-				if (chip->asic_code && CHECK_PID(chip, 0x5208))
-					rtsx_set_phy_reg_bit(chip, 0x1C, 2);
-
-				if (chip->card_exist) {
-					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,
-							    CHANGE_LINK_STATE,
-							    0x0A, 0x0A);
-
-					if (enter_L1)
-						rtsx_enter_L1(chip);
-
-					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,
-							    CHANGE_LINK_STATE,
-							    0x02, 0x02);
-
-					if (enter_L1)
-						rtsx_enter_L1(chip);
-				}
-			}
-
-			if (chip->auto_delink_cnt == delink_stage2_cnt) {
-				dev_dbg(rtsx_dev(chip), "Try to do force delink\n");
-
-				if (enter_L1)
-					rtsx_exit_L1(chip);
-
-				if (chip->asic_code && CHECK_PID(chip, 0x5208))
-					rtsx_set_phy_reg_bit(chip, 0x1C, 2);
-
-				rtsx_write_register(chip, CHANGE_LINK_STATE,
-						    0x0A, 0x0A);
-			}
-
-			chip->auto_delink_cnt++;
-		}
-	} else {
-		chip->auto_delink_cnt = 0;
-	}
+	rtsx_delink_stage(chip);
 }
 
 void rtsx_undo_delink(struct rtsx_chip *chip)
diff --git a/drivers/staging/rts5208/rtsx_scsi.c b/drivers/staging/rts5208/rtsx_scsi.c
index 8a5d6a8e780f..60871f3022b1 100644
--- a/drivers/staging/rts5208/rtsx_scsi.c
+++ b/drivers/staging/rts5208/rtsx_scsi.c
@@ -915,6 +915,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		start_sec = ((u32)(srb->cmnd[1] & 0x1F) << 16) |
 			((u32)srb->cmnd[2] << 8) | ((u32)srb->cmnd[3]);
 		sec_cnt = srb->cmnd[4];
+		if (sec_cnt == 0)
+			sec_cnt = 256;
 	} else if ((srb->cmnd[0] == VENDOR_CMND) &&
 		(srb->cmnd[1] == SCSI_APP_CMD) &&
 		((srb->cmnd[2] == PP_READ10) || (srb->cmnd[2] == PP_WRITE10))) {
@@ -2904,9 +2906,11 @@ void led_shine(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if ((srb->cmnd[0] == READ_10) || (srb->cmnd[0] == WRITE_10))
 		sec_cnt = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
-	else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6))
+	else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6)) {
 		sec_cnt = srb->cmnd[4];
-	else
+		if (sec_cnt == 0)
+			sec_cnt = 256;
+	} else
 		return;
 
 	if (chip->rw_cap[lun] >= GPIO_TOGGLE_THRESHOLD) {
diff --git a/drivers/staging/rts5208/xd.c b/drivers/staging/rts5208/xd.c
index 8fd108e50509..10fea7bb8f30 100644
--- a/drivers/staging/rts5208/xd.c
+++ b/drivers/staging/rts5208/xd.c
@@ -1379,9 +1379,8 @@ static int xd_erase_block(struct rtsx_chip *chip, u32 phy_blk)
 				xd_set_err_code(chip, XD_PRG_ERROR);
 				rtsx_trace(chip);
 				return STATUS_FAIL;
-			} else {
-				xd_set_err_code(chip, XD_ERASE_FAIL);
 			}
+			xd_set_err_code(chip, XD_ERASE_FAIL);
 			retval = xd_reset_cmd(chip);
 			if (retval != STATUS_SUCCESS) {
 				rtsx_trace(chip);
diff --git a/drivers/staging/skein/skein_api.h b/drivers/staging/skein/skein_api.h
index 171b87549548..7da8b386a28c 100644
--- a/drivers/staging/skein/skein_api.h
+++ b/drivers/staging/skein/skein_api.h
@@ -121,7 +121,7 @@ struct skein_ctx {
  * @param size
  *     Which Skein size to use.
  * @return
- *     SKEIN_SUCESS of SKEIN_FAIL
+ *     SKEIN_SUCCESS of SKEIN_FAIL
  */
 int skein_ctx_prepare(struct skein_ctx *ctx, enum skein_size size);
 
@@ -136,7 +136,7 @@ int skein_ctx_prepare(struct skein_ctx *ctx, enum skein_size size);
  * @param hash_bit_len
  *     Number of MAC hash bits to compute
  * @return
- *     SKEIN_SUCESS of SKEIN_FAIL
+ *     SKEIN_SUCCESS of SKEIN_FAIL
  * @see skein_reset
  */
 int skein_init(struct skein_ctx *ctx, size_t hash_bit_len);
@@ -171,7 +171,7 @@ void skein_reset(struct skein_ctx *ctx);
  * @param hash_bit_len
  *     Number of MAC hash bits to compute
  * @return
- *     SKEIN_SUCESS of SKEIN_FAIL
+ *     SKEIN_SUCCESS of SKEIN_FAIL
  */
 int skein_mac_init(struct skein_ctx *ctx, const u8 *key, size_t key_len,
 		   size_t hash_bit_len);
diff --git a/drivers/staging/slicoss/TODO b/drivers/staging/slicoss/TODO
index 20cc9abdc466..9019729b7be6 100644
--- a/drivers/staging/slicoss/TODO
+++ b/drivers/staging/slicoss/TODO
@@ -25,7 +25,6 @@ TODO:
 	- state variables for things that are
 	  easily available and shouldn't be kept in card structure, cardnum, ...
 	  slotnumber, events, ...
-	- get rid of slic_spinlock wrapper
 	- volatile == bad design => bad code
 	- locking too fine grained, not designed just throw more locks
 	  at problem
diff --git a/drivers/staging/slicoss/slic.h b/drivers/staging/slicoss/slic.h
index 3a5aa882b957..00b8af63e7ff 100644
--- a/drivers/staging/slicoss/slic.h
+++ b/drivers/staging/slicoss/slic.h
@@ -56,11 +56,6 @@ static u32 OasisRcvUCodeLen = 512;
 static u32 GBRcvUCodeLen = 512;
 #define SECTION_SIZE 65536
 
-struct slic_spinlock {
-	spinlock_t	lock;
-	unsigned long	flags;
-};
-
 #define SLIC_RSPQ_PAGES_GB        10
 #define SLIC_RSPQ_BUFSINPAGE      (PAGE_SIZE / SLIC_RSPBUF_SIZE)
 
@@ -165,7 +160,7 @@ struct slic_cmdqueue {
 	struct slic_hostcmd *head;
 	struct slic_hostcmd *tail;
 	int count;
-	struct slic_spinlock lock;
+	spinlock_t lock;
 };
 
 #define SLIC_MAX_CARDS              32
@@ -346,7 +341,7 @@ struct physcard {
 };
 
 struct base_driver {
-	struct slic_spinlock driver_lock;
+	spinlock_t       driver_lock;
 	u32              num_slic_cards;
 	u32              num_slic_ports;
 	u32              num_slic_ports_active;
@@ -401,8 +396,8 @@ struct adapter {
 	uint                card_size;
 	uint                chipid;
 	struct net_device  *netdev;
-	struct slic_spinlock     adapter_lock;
-	struct slic_spinlock     reset_lock;
+	spinlock_t          adapter_lock;
+	spinlock_t          reset_lock;
 	struct pci_dev     *pcidev;
 	uint                busnumber;
 	uint                slotnumber;
@@ -419,7 +414,6 @@ struct adapter {
 	u32             intrregistered;
 	uint                isp_initialized;
 	uint                gennumber;
-	u32             curaddrupper;
 	struct slic_shmem      *pshmem;
 	dma_addr_t          phys_shmem;
 	u32             isrcopy;
@@ -441,8 +435,8 @@ struct adapter {
 	u32             pingtimerset;
 	struct timer_list   loadtimer;
 	u32             loadtimerset;
-	struct slic_spinlock     upr_lock;
-	struct slic_spinlock     bit64reglock;
+	spinlock_t               upr_lock;
+	spinlock_t               bit64reglock;
 	struct slic_rspqueue     rspqueue;
 	struct slic_rcvqueue     rcvqueue;
 	struct slic_cmdqueue     cmdq_free;
@@ -457,7 +451,7 @@ struct adapter {
 	/* Free object handles*/
 	struct slic_handle *pfree_slic_handles;
 	/* Object handle list lock*/
-	struct slic_spinlock     handle_lock;
+	spinlock_t          handle_lock;
 	ushort              slic_handle_ix;
 
 	u32             xmitq_full;
@@ -520,8 +514,6 @@ struct adapter {
 #define FLUSH		true
 #define DONT_FLUSH	false
 
-#define SIOCSLICDUMPCARD         (SIOCDEVPRIVATE+9)
 #define SIOCSLICSETINTAGG        (SIOCDEVPRIVATE+10)
-#define SIOCSLICTRACEDUMP        (SIOCDEVPRIVATE+11)
 
 #endif /*  __SLIC_DRIVER_H__ */
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index c2bda1d38e41..a609f3e67256 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -144,17 +144,14 @@ static inline void slic_reg64_write(struct adapter *adapter, void __iomem *reg,
 				    u32 value, void __iomem *regh, u32 paddrh,
 				    bool flush)
 {
-	spin_lock_irqsave(&adapter->bit64reglock.lock,
-				adapter->bit64reglock.flags);
-	if (paddrh != adapter->curaddrupper) {
-		adapter->curaddrupper = paddrh;
-		writel(paddrh, regh);
-	}
+	unsigned long flags;
+
+	spin_lock_irqsave(&adapter->bit64reglock, flags);
+	writel(paddrh, regh);
 	writel(value, reg);
 	if (flush)
 		mb();
-	spin_unlock_irqrestore(&adapter->bit64reglock.lock,
-				adapter->bit64reglock.flags);
+	spin_unlock_irqrestore(&adapter->bit64reglock, flags);
 }
 
 static void slic_mcast_set_bit(struct adapter *adapter, char *address)
@@ -936,9 +933,10 @@ static int slic_upr_request(struct adapter *adapter,
 		     u32 upr_data_h,
 		     u32 upr_buffer, u32 upr_buffer_h)
 {
+	unsigned long flags;
 	int rc;
 
-	spin_lock_irqsave(&adapter->upr_lock.lock, adapter->upr_lock.flags);
+	spin_lock_irqsave(&adapter->upr_lock, flags);
 	rc = slic_upr_queue_request(adapter,
 					upr_request,
 					upr_data,
@@ -948,8 +946,7 @@ static int slic_upr_request(struct adapter *adapter,
 
 	slic_upr_start(adapter);
 err_unlock_irq:
-	spin_unlock_irqrestore(&adapter->upr_lock.lock,
-				adapter->upr_lock.flags);
+	spin_unlock_irqrestore(&adapter->upr_lock, flags);
 	return rc;
 }
 
@@ -1029,12 +1026,12 @@ static void slic_upr_request_complete(struct adapter *adapter, u32 isr)
 {
 	struct sliccard *card = adapter->card;
 	struct slic_upr *upr;
+	unsigned long flags;
 
-	spin_lock_irqsave(&adapter->upr_lock.lock, adapter->upr_lock.flags);
+	spin_lock_irqsave(&adapter->upr_lock, flags);
 	upr = adapter->upr_list;
 	if (!upr) {
-		spin_unlock_irqrestore(&adapter->upr_lock.lock,
-					adapter->upr_lock.flags);
+		spin_unlock_irqrestore(&adapter->upr_lock, flags);
 		return;
 	}
 	adapter->upr_list = upr->next;
@@ -1127,8 +1124,7 @@ static void slic_upr_request_complete(struct adapter *adapter, u32 isr)
 	}
 	kfree(upr);
 	slic_upr_start(adapter);
-	spin_unlock_irqrestore(&adapter->upr_lock.lock,
-				adapter->upr_lock.flags);
+	spin_unlock_irqrestore(&adapter->upr_lock, flags);
 }
 
 static int slic_config_get(struct adapter *adapter, u32 config, u32 config_h)
@@ -1310,6 +1306,7 @@ static void slic_cmdq_addcmdpage(struct adapter *adapter, u32 *page)
 	u32 phys_addrl;
 	u32 phys_addrh;
 	struct slic_handle *pslic_handle;
+	unsigned long flags;
 
 	cmdaddr = page;
 	cmd = (struct slic_hostcmd *)cmdaddr;
@@ -1324,12 +1321,10 @@ static void slic_cmdq_addcmdpage(struct adapter *adapter, u32 *page)
 	while ((cmdcnt < SLIC_CMDQ_CMDSINPAGE) &&
 	       (adapter->slic_handle_ix < 256)) {
 		/* Allocate and initialize a SLIC_HANDLE for this command */
-		spin_lock_irqsave(&adapter->handle_lock.lock,
-				adapter->handle_lock.flags);
+		spin_lock_irqsave(&adapter->handle_lock, flags);
 		pslic_handle  =  adapter->pfree_slic_handles;
 		adapter->pfree_slic_handles = pslic_handle->next;
-		spin_unlock_irqrestore(&adapter->handle_lock.lock,
-				adapter->handle_lock.flags);
+		spin_unlock_irqrestore(&adapter->handle_lock, flags);
 		pslic_handle->type = SLIC_HANDLE_CMD;
 		pslic_handle->address = (void *) cmd;
 		pslic_handle->offset = (ushort) adapter->slic_handle_ix++;
@@ -1356,11 +1351,11 @@ static void slic_cmdq_addcmdpage(struct adapter *adapter, u32 *page)
 	tail->next_all = cmdq->head;
 	cmdq->head = prev;
 	cmdq = &adapter->cmdq_free;
-	spin_lock_irqsave(&cmdq->lock.lock, cmdq->lock.flags);
+	spin_lock_irqsave(&cmdq->lock, flags);
 	cmdq->count += cmdcnt;	/*  SLIC_CMDQ_CMDSINPAGE;   mooktodo */
 	tail->next = cmdq->head;
 	cmdq->head = prev;
-	spin_unlock_irqrestore(&cmdq->lock.lock, cmdq->lock.flags);
+	spin_unlock_irqrestore(&cmdq->lock, flags);
 }
 
 static int slic_cmdq_init(struct adapter *adapter)
@@ -1371,9 +1366,9 @@ static int slic_cmdq_init(struct adapter *adapter)
 	memset(&adapter->cmdq_all, 0, sizeof(struct slic_cmdqueue));
 	memset(&adapter->cmdq_free, 0, sizeof(struct slic_cmdqueue));
 	memset(&adapter->cmdq_done, 0, sizeof(struct slic_cmdqueue));
-	spin_lock_init(&adapter->cmdq_all.lock.lock);
-	spin_lock_init(&adapter->cmdq_free.lock.lock);
-	spin_lock_init(&adapter->cmdq_done.lock.lock);
+	spin_lock_init(&adapter->cmdq_all.lock);
+	spin_lock_init(&adapter->cmdq_free.lock);
+	spin_lock_init(&adapter->cmdq_done.lock);
 	memset(&adapter->cmdqmem, 0, sizeof(struct slic_cmdqmem));
 	adapter->slic_handle_ix = 1;
 	for (i = 0; i < SLIC_CMDQ_INITPAGES; i++) {
@@ -1394,11 +1389,10 @@ static void slic_cmdq_reset(struct adapter *adapter)
 	struct slic_hostcmd *hcmd;
 	struct sk_buff *skb;
 	u32 outstanding;
+	unsigned long flags;
 
-	spin_lock_irqsave(&adapter->cmdq_free.lock.lock,
-			adapter->cmdq_free.lock.flags);
-	spin_lock_irqsave(&adapter->cmdq_done.lock.lock,
-			adapter->cmdq_done.lock.flags);
+	spin_lock_irqsave(&adapter->cmdq_free.lock, flags);
+	spin_lock(&adapter->cmdq_done.lock);
 	outstanding = adapter->cmdq_all.count - adapter->cmdq_done.count;
 	outstanding -= adapter->cmdq_free.count;
 	hcmd = adapter->cmdq_all.head;
@@ -1429,40 +1423,40 @@ static void slic_cmdq_reset(struct adapter *adapter)
 			"free_count %d != all count %d\n",
 			adapter->cmdq_free.count, adapter->cmdq_all.count);
 	}
-	spin_unlock_irqrestore(&adapter->cmdq_done.lock.lock,
-				adapter->cmdq_done.lock.flags);
-	spin_unlock_irqrestore(&adapter->cmdq_free.lock.lock,
-				adapter->cmdq_free.lock.flags);
+	spin_unlock(&adapter->cmdq_done.lock);
+	spin_unlock_irqrestore(&adapter->cmdq_free.lock, flags);
 }
 
 static void slic_cmdq_getdone(struct adapter *adapter)
 {
 	struct slic_cmdqueue *done_cmdq = &adapter->cmdq_done;
 	struct slic_cmdqueue *free_cmdq = &adapter->cmdq_free;
+	unsigned long flags;
 
-	spin_lock_irqsave(&done_cmdq->lock.lock, done_cmdq->lock.flags);
+	spin_lock_irqsave(&done_cmdq->lock, flags);
 
 	free_cmdq->head = done_cmdq->head;
 	free_cmdq->count = done_cmdq->count;
 	done_cmdq->head = NULL;
 	done_cmdq->tail = NULL;
 	done_cmdq->count = 0;
-	spin_unlock_irqrestore(&done_cmdq->lock.lock, done_cmdq->lock.flags);
+	spin_unlock_irqrestore(&done_cmdq->lock, flags);
 }
 
 static struct slic_hostcmd *slic_cmdq_getfree(struct adapter *adapter)
 {
 	struct slic_cmdqueue *cmdq = &adapter->cmdq_free;
 	struct slic_hostcmd *cmd = NULL;
+	unsigned long flags;
 
 lock_and_retry:
-	spin_lock_irqsave(&cmdq->lock.lock, cmdq->lock.flags);
+	spin_lock_irqsave(&cmdq->lock, flags);
 retry:
 	cmd = cmdq->head;
 	if (cmd) {
 		cmdq->head = cmd->next;
 		cmdq->count--;
-		spin_unlock_irqrestore(&cmdq->lock.lock, cmdq->lock.flags);
+		spin_unlock_irqrestore(&cmdq->lock, flags);
 	} else {
 		slic_cmdq_getdone(adapter);
 		cmd = cmdq->head;
@@ -1471,8 +1465,7 @@ retry:
 		} else {
 			u32 *pageaddr;
 
-			spin_unlock_irqrestore(&cmdq->lock.lock,
-						cmdq->lock.flags);
+			spin_unlock_irqrestore(&cmdq->lock, flags);
 			pageaddr = slic_cmdqmem_addpage(adapter);
 			if (pageaddr) {
 				slic_cmdq_addcmdpage(adapter, pageaddr);
@@ -1488,14 +1481,14 @@ static void slic_cmdq_putdone_irq(struct adapter *adapter,
 {
 	struct slic_cmdqueue *cmdq = &adapter->cmdq_done;
 
-	spin_lock(&cmdq->lock.lock);
+	spin_lock(&cmdq->lock);
 	cmd->busy = 0;
 	cmd->next = cmdq->head;
 	cmdq->head = cmd;
 	cmdq->count++;
 	if ((adapter->xmitq_full) && (cmdq->count > 10))
 		netif_wake_queue(adapter->netdev);
-	spin_unlock(&cmdq->lock.lock);
+	spin_unlock(&cmdq->lock);
 }
 
 static int slic_rcvqueue_fill(struct adapter *adapter)
@@ -2250,21 +2243,20 @@ static void slic_adapter_freeresources(struct adapter *adapter)
 	adapter->rcv_unicasts = 0;
 }
 
-static int slic_adapter_allocresources(struct adapter *adapter)
+static int slic_adapter_allocresources(struct adapter *adapter,
+				       unsigned long *flags)
 {
 	if (!adapter->intrregistered) {
 		int retval;
 
-		spin_unlock_irqrestore(&slic_global.driver_lock.lock,
-					slic_global.driver_lock.flags);
+		spin_unlock_irqrestore(&slic_global.driver_lock, *flags);
 
 		retval = request_irq(adapter->netdev->irq,
 				     &slic_interrupt,
 				     IRQF_SHARED,
 				     adapter->netdev->name, adapter->netdev);
 
-		spin_lock_irqsave(&slic_global.driver_lock.lock,
-					slic_global.driver_lock.flags);
+		spin_lock_irqsave(&slic_global.driver_lock, *flags);
 
 		if (retval) {
 			dev_err(&adapter->netdev->dev,
@@ -2283,7 +2275,7 @@ static int slic_adapter_allocresources(struct adapter *adapter)
  *  Perform initialization of our slic interface.
  *
  */
-static int slic_if_init(struct adapter *adapter)
+static int slic_if_init(struct adapter *adapter, unsigned long *flags)
 {
 	struct sliccard *card = adapter->card;
 	struct net_device *dev = adapter->netdev;
@@ -2311,7 +2303,7 @@ static int slic_if_init(struct adapter *adapter)
 		if (dev->flags & IFF_MULTICAST)
 			adapter->macopts |= MAC_MCAST;
 	}
-	rc = slic_adapter_allocresources(adapter);
+	rc = slic_adapter_allocresources(adapter, flags);
 	if (rc) {
 		dev_err(&dev->dev, "slic_adapter_allocresources FAILED %x\n",
 			rc);
@@ -2336,11 +2328,11 @@ static int slic_if_init(struct adapter *adapter)
 	mdelay(1);
 
 	if (!adapter->isp_initialized) {
+		unsigned long flags;
 		pshmem = (struct slic_shmem *)(unsigned long)
 			 adapter->phys_shmem;
 
-		spin_lock_irqsave(&adapter->bit64reglock.lock,
-					adapter->bit64reglock.flags);
+		spin_lock_irqsave(&adapter->bit64reglock, flags);
 
 #if BITS_PER_LONG == 64
 		slic_reg32_write(&slic_regs->slic_addr_upper,
@@ -2352,8 +2344,7 @@ static int slic_if_init(struct adapter *adapter)
 		slic_reg32_write(&slic_regs->slic_isp, (u32)&pshmem->isr,
 				FLUSH);
 #endif
-		spin_unlock_irqrestore(&adapter->bit64reglock.lock,
-					adapter->bit64reglock.flags);
+		spin_unlock_irqrestore(&adapter->bit64reglock, flags);
 		adapter->isp_initialized = 1;
 	}
 
@@ -2396,18 +2387,18 @@ static int slic_entry_open(struct net_device *dev)
 {
 	struct adapter *adapter = netdev_priv(dev);
 	struct sliccard *card = adapter->card;
+	unsigned long flags;
 	int status;
 
 	netif_stop_queue(adapter->netdev);
 
-	spin_lock_irqsave(&slic_global.driver_lock.lock,
-				slic_global.driver_lock.flags);
+	spin_lock_irqsave(&slic_global.driver_lock, flags);
 	if (!adapter->activated) {
 		card->adapters_activated++;
 		slic_global.num_slic_ports_active++;
 		adapter->activated = 1;
 	}
-	status = slic_if_init(adapter);
+	status = slic_if_init(adapter, &flags);
 
 	if (status != 0) {
 		if (adapter->activated) {
@@ -2421,8 +2412,7 @@ static int slic_entry_open(struct net_device *dev)
 		card->master = adapter;
 
 spin_unlock:
-	spin_unlock_irqrestore(&slic_global.driver_lock.lock,
-			       slic_global.driver_lock.flags);
+	spin_unlock_irqrestore(&slic_global.driver_lock, flags);
 	return status;
 }
 
@@ -2481,9 +2471,9 @@ static int slic_entry_halt(struct net_device *dev)
 	struct adapter *adapter = netdev_priv(dev);
 	struct sliccard *card = adapter->card;
 	__iomem struct slic_regs *slic_regs = adapter->slic_regs;
+	unsigned long flags;
 
-	spin_lock_irqsave(&slic_global.driver_lock.lock,
-				slic_global.driver_lock.flags);
+	spin_lock_irqsave(&slic_global.driver_lock, flags);
 	netif_stop_queue(adapter->netdev);
 	adapter->state = ADAPT_DOWN;
 	adapter->linkstate = LINK_DOWN;
@@ -2512,8 +2502,7 @@ static int slic_entry_halt(struct net_device *dev)
 		slic_card_init(card, adapter);
 #endif
 
-	spin_unlock_irqrestore(&slic_global.driver_lock.lock,
-				slic_global.driver_lock.flags);
+	spin_unlock_irqrestore(&slic_global.driver_lock, flags);
 	return 0;
 }
 
@@ -2663,6 +2652,7 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 	unsigned char oemfruformat;
 	struct atk_fru *patkfru;
 	union oemfru *poemfru;
+	unsigned long flags;
 
 	/* Reset everything except PCI configuration space */
 	slic_soft_reset(adapter);
@@ -2693,14 +2683,12 @@ static int slic_card_init(struct sliccard *card, struct adapter *adapter)
 		pshmem = (struct slic_shmem *)(unsigned long)
 			 adapter->phys_shmem;
 
-		spin_lock_irqsave(&adapter->bit64reglock.lock,
-					adapter->bit64reglock.flags);
+		spin_lock_irqsave(&adapter->bit64reglock, flags);
 		slic_reg32_write(&slic_regs->slic_addr_upper,
 				 SLIC_GET_ADDR_HIGH(&pshmem->isr), DONT_FLUSH);
 		slic_reg32_write(&slic_regs->slic_isp,
 				 SLIC_GET_ADDR_LOW(&pshmem->isr), FLUSH);
-		spin_unlock_irqrestore(&adapter->bit64reglock.lock,
-					adapter->bit64reglock.flags);
+		spin_unlock_irqrestore(&adapter->bit64reglock, flags);
 
 		status = slic_config_get(adapter, phys_configl, phys_configh);
 		if (status) {
@@ -2854,7 +2842,7 @@ static void slic_init_driver(void)
 {
 	if (slic_first_init) {
 		slic_first_init = 0;
-		spin_lock_init(&slic_global.driver_lock.lock);
+		spin_lock_init(&slic_global.driver_lock);
 	}
 }
 
@@ -2880,11 +2868,11 @@ static void slic_init_adapter(struct net_device *netdev,
 	adapter->chipid = chip_idx;
 	adapter->port = 0;	/*adapter->functionnumber;*/
 	adapter->cardindex = adapter->port;
-	spin_lock_init(&adapter->upr_lock.lock);
-	spin_lock_init(&adapter->bit64reglock.lock);
-	spin_lock_init(&adapter->adapter_lock.lock);
-	spin_lock_init(&adapter->reset_lock.lock);
-	spin_lock_init(&adapter->handle_lock.lock);
+	spin_lock_init(&adapter->upr_lock);
+	spin_lock_init(&adapter->bit64reglock);
+	spin_lock_init(&adapter->adapter_lock);
+	spin_lock_init(&adapter->reset_lock);
+	spin_lock_init(&adapter->handle_lock);
 
 	adapter->card_size = 1;
 	/*
diff --git a/drivers/staging/sm750fb/Kconfig b/drivers/staging/sm750fb/Kconfig
index c40d088a4d3b..ccebc25c2ec1 100644
--- a/drivers/staging/sm750fb/Kconfig
+++ b/drivers/staging/sm750fb/Kconfig
@@ -1,6 +1,10 @@
 config FB_SM750
 	tristate "Silicon Motion SM750 framebuffer support"
 	depends on FB && PCI
+	select FB_MODE_HELPERS
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
 	help
 	  Frame buffer driver for the Silicon Motion SM750 chip
 	  with 2D accelearion and dual head support.
diff --git a/drivers/staging/sm750fb/TODO b/drivers/staging/sm750fb/TODO
index bc1617249395..a3a877d90066 100644
--- a/drivers/staging/sm750fb/TODO
+++ b/drivers/staging/sm750fb/TODO
@@ -1,7 +1,8 @@
 TODO:
-- lots of clechpatch cleanup
+- lots of checkpatch cleanup
 - use kernel coding style
 - refine the code and remove unused code
+- Implement hardware acceleration for imageblit if image->depth > 1
 - check on hardware effects of removal of USE_HW_I2C and USE_DVICHIP (these two
 	are supposed to be sample code which is given here if someone wants to
 	use those functionalities)
diff --git a/drivers/staging/sm750fb/ddk750_chip.c b/drivers/staging/sm750fb/ddk750_chip.c
index 7b28328c92f8..f4975d2d97ad 100644
--- a/drivers/staging/sm750fb/ddk750_chip.c
+++ b/drivers/staging/sm750fb/ddk750_chip.c
@@ -17,7 +17,7 @@ logical_chip_type_t getChipType(void)
 	char physicalRev;
 	logical_chip_type_t chip;
 
-	physicalID = devId750;//either 0x718 or 0x750
+	physicalID = devId750; /* either 0x718 or 0x750 */
 	physicalRev = revId750;
 
 	if (physicalID == 0x718)
@@ -257,7 +257,7 @@ int ddk750_initHw(initchip_param_t *pInitParam)
 
 	unsigned int ulReg;
 #if 0
-	//move the code to map regiter function.
+	/* move the code to map regiter function. */
 	if (getChipType() == SM718) {
 		/* turn on big endian bit*/
 		ulReg = PEEK32(0x74);
@@ -285,7 +285,7 @@ int ddk750_initHw(initchip_param_t *pInitParam)
 		ulReg = FIELD_SET(ulReg, VGA_CONFIGURATION, MODE, GRAPHIC);
 		POKE32(VGA_CONFIGURATION, ulReg);
 	} else {
-#if defined(__i386__) || defined( __x86_64__)
+#if defined(__i386__) || defined(__x86_64__)
 		/* set graphic mode via IO method */
 		outb_p(0x88, 0x3d4);
 		outb_p(0x06, 0x3d5);
@@ -382,7 +382,7 @@ int ddk750_initHw(initchip_param_t *pInitParam)
 
 unsigned int absDiff(unsigned int a, unsigned int b)
 {
-	if ( a > b )
+	if (a > b)
 		return(a - b);
 	else
 		return(b - a);
@@ -433,7 +433,7 @@ unsigned int calcPllValue(unsigned int request_orig, pll_value_t *pll)
 	unsigned int RN, quo, rem, fl_quo;
 	unsigned int input, request;
 	unsigned int tmpClock, ret;
-	pllcalparam * xparm;
+	pllcalparam *xparm;
 
 #if 1
 	if (getChipType() == SM750LE) {
@@ -487,8 +487,6 @@ unsigned int calcPllValue(unsigned int request_orig, pll_value_t *pll)
 			}
 		}
 	}
-
-	//printk("Finally:  pll->n[%lu],m[%lu],od[%lu],pod[%lu]\n",pll->N,pll->M,pll->OD,pll->POD);
 	return ret;
 }
 
@@ -580,14 +578,9 @@ pll_value_t *pPLL           /* Structure to hold the value to be set in PLL */
 	}
 
     /* Restore input frequency from Khz to hz unit */
-//    pPLL->inputFreq *= 1000;
 	ulRequestClk *= 1000;
 	pPLL->inputFreq = DEFAULT_INPUT_CLOCK; /* Default reference clock */
 
-    /* Output debug information */
-	//DDKDEBUGPRINT((DISPLAY_LEVEL, "calcPllValue: Requested Frequency = %d\n", ulRequestClk));
-	//DDKDEBUGPRINT((DISPLAY_LEVEL, "calcPllValue: Input CLK = %dHz, M=%d, N=%d, OD=%d, POD=%d\n", pPLL->inputFreq, pPLL->M, pPLL->N, pPLL->OD, pPLL->POD));
-
     /* Return actual frequency that the PLL can set */
 	ret = calcPLL(pPLL);
 	return ret;
@@ -616,7 +609,7 @@ unsigned int formatPllReg(pll_value_t *pPLL)
 	| FIELD_VALUE(0, PANEL_PLL_CTRL, N,      pPLL->N)
 	| FIELD_VALUE(0, PANEL_PLL_CTRL, M,      pPLL->M);
 
-    return ulPllReg;
+	return ulPllReg;
 }
 
 
diff --git a/drivers/staging/sm750fb/ddk750_chip.h b/drivers/staging/sm750fb/ddk750_chip.h
index 04cb0d559245..4e030e820cf3 100644
--- a/drivers/staging/sm750fb/ddk750_chip.h
+++ b/drivers/staging/sm750fb/ddk750_chip.h
@@ -10,10 +10,10 @@
 /* This is all the chips recognized by this library */
 typedef enum _logical_chip_type_t
 {
-    SM_UNKNOWN,
-    SM718,
-    SM750,
-    SM750LE,
+	SM_UNKNOWN,
+	SM718,
+	SM750,
+	SM750LE,
 }
 logical_chip_type_t;
 
@@ -30,51 +30,56 @@ clock_type_t;
 
 typedef struct _pll_value_t
 {
-    clock_type_t clockType;
-    unsigned long inputFreq; /* Input clock frequency to the PLL */
+	clock_type_t clockType;
+	unsigned long inputFreq; /* Input clock frequency to the PLL */
 
-    /* Use this when clockType = PANEL_PLL */
-    unsigned long M;
-    unsigned long N;
-    unsigned long OD;
-    unsigned long POD;
+	/* Use this when clockType = PANEL_PLL */
+	unsigned long M;
+	unsigned long N;
+	unsigned long OD;
+	unsigned long POD;
 }
 pll_value_t;
 
 /* input struct to initChipParam() function */
 typedef struct _initchip_param_t
 {
-    unsigned short powerMode;    /* Use power mode 0 or 1 */
-    unsigned short chipClock;    /* Speed of main chip clock in MHz unit
-                                    0 = keep the current clock setting
-                                    Others = the new main chip clock
-                                  */
-    unsigned short memClock;     /* Speed of memory clock in MHz unit
-                                    0 = keep the current clock setting
-                                    Others = the new memory clock
-                                  */
-    unsigned short masterClock;  /* Speed of master clock in MHz unit
-                                    0 = keep the current clock setting
-                                    Others = the new master clock
-                                  */
-    unsigned short setAllEngOff; /* 0 = leave all engine state untouched.
-                                    1 = make sure they are off: 2D, Overlay,
-                                    video alpha, alpha, hardware cursors
-                                 */
-    unsigned char resetMemory;   /* 0 = Do not reset the memory controller
-                                    1 = Reset the memory controller
-                                  */
+	unsigned short powerMode;    /* Use power mode 0 or 1 */
+	unsigned short chipClock;    /**
+				      * Speed of main chip clock in MHz unit
+				      * 0 = keep the current clock setting
+				      * Others = the new main chip clock
+				      */
+	unsigned short memClock;     /**
+				      * Speed of memory clock in MHz unit
+				      * 0 = keep the current clock setting
+				      * Others = the new memory clock
+				      */
+	unsigned short masterClock;  /**
+				      * Speed of master clock in MHz unit
+				      * 0 = keep the current clock setting
+				      * Others = the new master clock
+				      */
+	unsigned short setAllEngOff; /**
+				      * 0 = leave all engine state untouched.
+				      * 1 = make sure they are off: 2D, Overlay,
+				      * video alpha, alpha, hardware cursors
+				      */
+	unsigned char resetMemory;   /**
+				      * 0 = Do not reset the memory controller
+				      * 1 = Reset the memory controller
+				      */
 
-    /* More initialization parameter can be added if needed */
+	/* More initialization parameter can be added if needed */
 }
 initchip_param_t;
 
 
 logical_chip_type_t getChipType(void);
-unsigned int calcPllValue(unsigned int request,pll_value_t *pll);
-unsigned int calcPllValue2(unsigned int,pll_value_t *);
+unsigned int calcPllValue(unsigned int request, pll_value_t *pll);
+unsigned int calcPllValue2(unsigned int, pll_value_t *);
 unsigned int formatPllReg(pll_value_t *pPLL);
-void ddk750_set_mmio(void __iomem *,unsigned short,char);
+void ddk750_set_mmio(void __iomem *, unsigned short, char);
 unsigned int ddk750_getVMSize(void);
 int ddk750_initHw(initchip_param_t *);
 unsigned int getPllValue(clock_type_t clockType, pll_value_t *pPLL);
diff --git a/drivers/staging/sm750fb/ddk750_display.c b/drivers/staging/sm750fb/ddk750_display.c
index c84196ac055d..a3e672056ef8 100644
--- a/drivers/staging/sm750fb/ddk750_display.c
+++ b/drivers/staging/sm750fb/ddk750_display.c
@@ -4,9 +4,9 @@
 #include "ddk750_power.h"
 #include "ddk750_dvi.h"
 
-#define primaryWaitVerticalSync(delay) waitNextVerticalSync(0,delay)
+#define primaryWaitVerticalSync(delay) waitNextVerticalSync(0, delay)
 
-static void setDisplayControl(int ctrl,int dispState)
+static void setDisplayControl(int ctrl, int dispState)
 {
 	/* state != 0 means turn on both timing & plane en_bit */
 	unsigned long ulDisplayCtrlReg, ulReservedBits;
@@ -51,7 +51,7 @@ static void setDisplayControl(int ctrl,int dispState)
 				POKE32(PANEL_DISPLAY_CTRL, ulDisplayCtrlReg);
 			} while((PEEK32(PANEL_DISPLAY_CTRL) & ~ulReservedBits) !=
 					(ulDisplayCtrlReg & ~ulReservedBits));
-			printk("Set Panel Plane enbit:after tried %d times\n",cnt);
+			printk("Set Panel Plane enbit:after tried %d times\n", cnt);
 		}
 		else
 		{
@@ -106,7 +106,7 @@ static void setDisplayControl(int ctrl,int dispState)
 				POKE32(CRT_DISPLAY_CTRL, ulDisplayCtrlReg);
 			} while((PEEK32(CRT_DISPLAY_CTRL) & ~ulReservedBits) !=
 					(ulDisplayCtrlReg & ~ulReservedBits));
-				printk("Set Crt Plane enbit:after tried %d times\n",cnt);
+				printk("Set Crt Plane enbit:after tried %d times\n", cnt);
 		}
 		else
 		{
@@ -129,7 +129,7 @@ static void setDisplayControl(int ctrl,int dispState)
 }
 
 
-static void waitNextVerticalSync(int ctrl,int delay)
+static void waitNextVerticalSync(int ctrl, int delay)
 {
 	unsigned int status;
 	if(!ctrl){
@@ -201,31 +201,31 @@ static void waitNextVerticalSync(int ctrl,int delay)
 	}
 }
 
-static void swPanelPowerSequence(int disp,int delay)
+static void swPanelPowerSequence(int disp, int delay)
 {
 	unsigned int reg;
 
 	/* disp should be 1 to open sequence */
 	reg = PEEK32(PANEL_DISPLAY_CTRL);
-	reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,FPEN,disp);
-	POKE32(PANEL_DISPLAY_CTRL,reg);
+	reg = FIELD_VALUE(reg, PANEL_DISPLAY_CTRL, FPEN, disp);
+	POKE32(PANEL_DISPLAY_CTRL, reg);
 	primaryWaitVerticalSync(delay);
 
 
 	reg = PEEK32(PANEL_DISPLAY_CTRL);
-	reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,DATA,disp);
-	POKE32(PANEL_DISPLAY_CTRL,reg);
+	reg = FIELD_VALUE(reg, PANEL_DISPLAY_CTRL, DATA, disp);
+	POKE32(PANEL_DISPLAY_CTRL, reg);
 	primaryWaitVerticalSync(delay);
 
 	reg = PEEK32(PANEL_DISPLAY_CTRL);
-	reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,VBIASEN,disp);
-	POKE32(PANEL_DISPLAY_CTRL,reg);
+	reg = FIELD_VALUE(reg, PANEL_DISPLAY_CTRL, VBIASEN, disp);
+	POKE32(PANEL_DISPLAY_CTRL, reg);
 	primaryWaitVerticalSync(delay);
 
 
 	reg = PEEK32(PANEL_DISPLAY_CTRL);
-	reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,FPEN,disp);
-	POKE32(PANEL_DISPLAY_CTRL,reg);
+	reg = FIELD_VALUE(reg, PANEL_DISPLAY_CTRL, FPEN, disp);
+	POKE32(PANEL_DISPLAY_CTRL, reg);
 	primaryWaitVerticalSync(delay);
 
 }
@@ -236,33 +236,33 @@ void ddk750_setLogicalDispOut(disp_output_t output)
 	if(output & PNL_2_USAGE){
 		/* set panel path controller select */
 		reg = PEEK32(PANEL_DISPLAY_CTRL);
-		reg = FIELD_VALUE(reg,PANEL_DISPLAY_CTRL,SELECT,(output & PNL_2_MASK)>>PNL_2_OFFSET);
-		POKE32(PANEL_DISPLAY_CTRL,reg);
+		reg = FIELD_VALUE(reg, PANEL_DISPLAY_CTRL, SELECT, (output & PNL_2_MASK)>>PNL_2_OFFSET);
+		POKE32(PANEL_DISPLAY_CTRL, reg);
 	}
 
 	if(output & CRT_2_USAGE){
 		/* set crt path controller select */
 		reg = PEEK32(CRT_DISPLAY_CTRL);
-		reg = FIELD_VALUE(reg,CRT_DISPLAY_CTRL,SELECT,(output & CRT_2_MASK)>>CRT_2_OFFSET);
+		reg = FIELD_VALUE(reg, CRT_DISPLAY_CTRL, SELECT, (output & CRT_2_MASK)>>CRT_2_OFFSET);
 		/*se blank off */
-		reg = FIELD_SET(reg,CRT_DISPLAY_CTRL,BLANK,OFF);
-		POKE32(CRT_DISPLAY_CTRL,reg);
+		reg = FIELD_SET(reg, CRT_DISPLAY_CTRL, BLANK, OFF);
+		POKE32(CRT_DISPLAY_CTRL, reg);
 
 	}
 
 	if(output & PRI_TP_USAGE){
 		/* set primary timing and plane en_bit */
-		setDisplayControl(0,(output&PRI_TP_MASK)>>PRI_TP_OFFSET);
+		setDisplayControl(0, (output&PRI_TP_MASK)>>PRI_TP_OFFSET);
 	}
 
 	if(output & SEC_TP_USAGE){
 		/* set secondary timing and plane en_bit*/
-		setDisplayControl(1,(output&SEC_TP_MASK)>>SEC_TP_OFFSET);
+		setDisplayControl(1, (output&SEC_TP_MASK)>>SEC_TP_OFFSET);
 	}
 
 	if(output & PNL_SEQ_USAGE){
 		/* set  panel sequence */
-		swPanelPowerSequence((output&PNL_SEQ_MASK)>>PNL_SEQ_OFFSET,4);
+		swPanelPowerSequence((output&PNL_SEQ_MASK)>>PNL_SEQ_OFFSET, 4);
 	}
 
 	if(output & DAC_USAGE)
diff --git a/drivers/staging/sm750fb/ddk750_dvi.c b/drivers/staging/sm750fb/ddk750_dvi.c
index f5932bbf13e8..b2bf7e66d5cb 100644
--- a/drivers/staging/sm750fb/ddk750_dvi.c
+++ b/drivers/staging/sm750fb/ddk750_dvi.c
@@ -51,7 +51,7 @@ int dviInit(
                               vsyncEnable, deskewEnable, deskewSetting, continuousSyncEnable,
                               pllFilterEnable, pllFilterValue);
 	}
-	return -1;//error
+	return -1; /* error */
 }
 
 
@@ -66,7 +66,6 @@ unsigned short dviGetVendorID(void)
 {
     dvi_ctrl_device_t *pCurrentDviCtrl;
 
-    //pCurrentDviCtrl = getDviCtrl();
     pCurrentDviCtrl = g_dcftSupportedDviController;
     if (pCurrentDviCtrl != (dvi_ctrl_device_t *)0)
         return pCurrentDviCtrl->pfnGetVendorId();
@@ -86,7 +85,6 @@ unsigned short dviGetDeviceID(void)
 {
     dvi_ctrl_device_t *pCurrentDviCtrl;
 
-//    pCurrentDviCtrl = getDviCtrl();
 	pCurrentDviCtrl = g_dcftSupportedDviController;
     if (pCurrentDviCtrl != (dvi_ctrl_device_t *)0)
         return pCurrentDviCtrl->pfnGetDeviceId();
diff --git a/drivers/staging/sm750fb/ddk750_dvi.h b/drivers/staging/sm750fb/ddk750_dvi.h
index 50bcec29b2c0..83bbd6d62061 100644
--- a/drivers/staging/sm750fb/ddk750_dvi.h
+++ b/drivers/staging/sm750fb/ddk750_dvi.h
@@ -4,16 +4,17 @@
 /* dvi chip stuffs structros */
 
 typedef long (*PFN_DVICTRL_INIT)(
-    unsigned char edgeSelect,
-    unsigned char busSelect,
-    unsigned char dualEdgeClkSelect,
-    unsigned char hsyncEnable,
-    unsigned char vsyncEnable,
-    unsigned char deskewEnable,
-    unsigned char deskewSetting,
-    unsigned char continuousSyncEnable,
-    unsigned char pllFilterEnable,
-    unsigned char pllFilterValue);
+	unsigned char edgeSelect,
+	unsigned char busSelect,
+	unsigned char dualEdgeClkSelect,
+	unsigned char hsyncEnable,
+	unsigned char vsyncEnable,
+	unsigned char deskewEnable,
+	unsigned char deskewSetting,
+	unsigned char continuousSyncEnable,
+	unsigned char pllFilterEnable,
+	unsigned char pllFilterValue);
+
 typedef void (*PFN_DVICTRL_RESETCHIP)(void);
 typedef char* (*PFN_DVICTRL_GETCHIPSTRING)(void);
 typedef unsigned short (*PFN_DVICTRL_GETVENDORID)(void);
@@ -24,44 +25,39 @@ typedef unsigned char (*PFN_DVICTRL_ISCONNECTED)(void);
 typedef unsigned char (*PFN_DVICTRL_CHECKINTERRUPT)(void);
 typedef void (*PFN_DVICTRL_CLEARINTERRUPT)(void);
 
-
-
 /* Structure to hold all the function pointer to the DVI Controller. */
 typedef struct _dvi_ctrl_device_t
 {
-    PFN_DVICTRL_INIT                pfnInit;
-    PFN_DVICTRL_RESETCHIP           pfnResetChip;
-    PFN_DVICTRL_GETCHIPSTRING       pfnGetChipString;
-    PFN_DVICTRL_GETVENDORID         pfnGetVendorId;
-    PFN_DVICTRL_GETDEVICEID         pfnGetDeviceId;
-    PFN_DVICTRL_SETPOWER            pfnSetPower;
-    PFN_DVICTRL_HOTPLUGDETECTION    pfnEnableHotPlugDetection;
-    PFN_DVICTRL_ISCONNECTED         pfnIsConnected;
-    PFN_DVICTRL_CHECKINTERRUPT      pfnCheckInterrupt;
-    PFN_DVICTRL_CLEARINTERRUPT      pfnClearInterrupt;
+	PFN_DVICTRL_INIT		pfnInit;
+	PFN_DVICTRL_RESETCHIP		pfnResetChip;
+	PFN_DVICTRL_GETCHIPSTRING	pfnGetChipString;
+	PFN_DVICTRL_GETVENDORID		pfnGetVendorId;
+	PFN_DVICTRL_GETDEVICEID		pfnGetDeviceId;
+	PFN_DVICTRL_SETPOWER		pfnSetPower;
+	PFN_DVICTRL_HOTPLUGDETECTION	pfnEnableHotPlugDetection;
+	PFN_DVICTRL_ISCONNECTED		pfnIsConnected;
+	PFN_DVICTRL_CHECKINTERRUPT	pfnCheckInterrupt;
+	PFN_DVICTRL_CLEARINTERRUPT	pfnClearInterrupt;
 } dvi_ctrl_device_t;
-#define DVI_CTRL_SII164
-
 
+#define DVI_CTRL_SII164
 
 /* dvi functions prototype */
 int dviInit(
-    unsigned char edgeSelect,
-    unsigned char busSelect,
-    unsigned char dualEdgeClkSelect,
-    unsigned char hsyncEnable,
-    unsigned char vsyncEnable,
-    unsigned char deskewEnable,
-    unsigned char deskewSetting,
-    unsigned char continuousSyncEnable,
-    unsigned char pllFilterEnable,
-    unsigned char pllFilterValue
+	unsigned char edgeSelect,
+	unsigned char busSelect,
+	unsigned char dualEdgeClkSelect,
+	unsigned char hsyncEnable,
+	unsigned char vsyncEnable,
+	unsigned char deskewEnable,
+	unsigned char deskewSetting,
+	unsigned char continuousSyncEnable,
+	unsigned char pllFilterEnable,
+	unsigned char pllFilterValue
 );
 
 unsigned short dviGetVendorID(void);
 unsigned short dviGetDeviceID(void);
 
-
-
 #endif
 
diff --git a/drivers/staging/sm750fb/ddk750_help.c b/drivers/staging/sm750fb/ddk750_help.c
index c68ff3b5751a..1adcafcc5133 100644
--- a/drivers/staging/sm750fb/ddk750_help.c
+++ b/drivers/staging/sm750fb/ddk750_help.c
@@ -1,13 +1,11 @@
-//#include "ddk750_reg.h"
-//#include "ddk750_chip.h"
 #include "ddk750_help.h"
 
-void __iomem * mmio750 = NULL;
+void __iomem *mmio750 = NULL;
 char revId750 = 0;
 unsigned short devId750 = 0;
 
 /* after driver mapped io registers, use this function first */
-void ddk750_set_mmio(void __iomem * addr,unsigned short devId,char revId)
+void ddk750_set_mmio(void __iomem *addr, unsigned short devId, char revId)
 {
 	mmio750 = addr;
 	devId750 = devId;
diff --git a/drivers/staging/sm750fb/ddk750_help.h b/drivers/staging/sm750fb/ddk750_help.h
index 07c8264fac95..4285b056585a 100644
--- a/drivers/staging/sm750fb/ddk750_help.h
+++ b/drivers/staging/sm750fb/ddk750_help.h
@@ -13,13 +13,13 @@
 /* if 718 big endian turned on,be aware that don't use this driver for general use,only for ppc big-endian */
 #warning "big endian on target cpu and enable nature big endian support of 718 capability !"
 #define PEEK32(addr)  			__raw_readl(mmio750 + addr)
-#define POKE32(addr,data) 		__raw_writel(data, mmio750 + addr)
-#else /* software control endianess */
+#define POKE32(addr, data) 		__raw_writel(data, mmio750 + addr)
+#else /* software control endianness */
 #define PEEK32(addr) readl(addr + mmio750)
-#define POKE32(addr,data) writel(data, addr + mmio750)
+#define POKE32(addr, data) writel(data, addr + mmio750)
 #endif
 
-extern void __iomem * mmio750;
+extern void __iomem *mmio750;
 extern char revId750;
 extern unsigned short devId750;
 #else
diff --git a/drivers/staging/sm750fb/ddk750_hwi2c.h b/drivers/staging/sm750fb/ddk750_hwi2c.h
index ad311493c9fc..0b830ba65eec 100644
--- a/drivers/staging/sm750fb/ddk750_hwi2c.h
+++ b/drivers/staging/sm750fb/ddk750_hwi2c.h
@@ -5,6 +5,6 @@
 int hwI2CInit(unsigned char busSpeedMode);
 void hwI2CClose(void);
 
-unsigned char hwI2CReadReg(unsigned char deviceAddress,unsigned char registerIndex);
-int hwI2CWriteReg(unsigned char deviceAddress,unsigned char registerIndex,unsigned char data);
+unsigned char hwI2CReadReg(unsigned char deviceAddress, unsigned char registerIndex);
+int hwI2CWriteReg(unsigned char deviceAddress, unsigned char registerIndex, unsigned char data);
 #endif
diff --git a/drivers/staging/sm750fb/ddk750_mode.c b/drivers/staging/sm750fb/ddk750_mode.c
index 2e418fb6ffde..74313ff84e45 100644
--- a/drivers/staging/sm750fb/ddk750_mode.c
+++ b/drivers/staging/sm750fb/ddk750_mode.c
@@ -75,15 +75,15 @@ static unsigned long displayControlAdjust_SM750LE(mode_parameter_t *pModeParam,
 
 
 /* only timing related registers will be  programed */
-static int programModeRegisters(mode_parameter_t * pModeParam,pll_value_t * pll)
+static int programModeRegisters(mode_parameter_t *pModeParam, pll_value_t *pll)
 {
 	int ret = 0;
 	int cnt = 0;
-	unsigned int ulTmpValue,ulReg;
+	unsigned int ulTmpValue, ulReg;
 	if(pll->clockType == SECONDARY_PLL)
 	{
 		/* programe secondary pixel clock */
-		POKE32(CRT_PLL_CTRL,formatPllReg(pll));
+		POKE32(CRT_PLL_CTRL, formatPllReg(pll));
         POKE32(CRT_HORIZONTAL_TOTAL,
               FIELD_VALUE(0, CRT_HORIZONTAL_TOTAL, TOTAL, pModeParam->horizontal_total - 1)
             | FIELD_VALUE(0, CRT_HORIZONTAL_TOTAL, DISPLAY_END, pModeParam->horizontal_display_end - 1));
@@ -101,29 +101,29 @@ static int programModeRegisters(mode_parameter_t * pModeParam,pll_value_t * pll)
             | FIELD_VALUE(0, CRT_VERTICAL_SYNC, START, pModeParam->vertical_sync_start - 1));
 
 
-		ulTmpValue = FIELD_VALUE(0,CRT_DISPLAY_CTRL,VSYNC_PHASE,pModeParam->vertical_sync_polarity)|
-					  FIELD_VALUE(0,CRT_DISPLAY_CTRL,HSYNC_PHASE,pModeParam->horizontal_sync_polarity)|
-					  FIELD_SET(0,CRT_DISPLAY_CTRL,TIMING,ENABLE)|
-					  FIELD_SET(0,CRT_DISPLAY_CTRL,PLANE,ENABLE);
+		ulTmpValue = FIELD_VALUE(0, CRT_DISPLAY_CTRL, VSYNC_PHASE, pModeParam->vertical_sync_polarity)|
+					  FIELD_VALUE(0, CRT_DISPLAY_CTRL, HSYNC_PHASE, pModeParam->horizontal_sync_polarity)|
+					  FIELD_SET(0, CRT_DISPLAY_CTRL, TIMING, ENABLE)|
+					  FIELD_SET(0, CRT_DISPLAY_CTRL, PLANE, ENABLE);
 
 
 		if(getChipType() == SM750LE){
-			displayControlAdjust_SM750LE(pModeParam,ulTmpValue);
+			displayControlAdjust_SM750LE(pModeParam, ulTmpValue);
 		}else{
 			ulReg = PEEK32(CRT_DISPLAY_CTRL)
-					& FIELD_CLEAR(CRT_DISPLAY_CTRL,VSYNC_PHASE)
-					& FIELD_CLEAR(CRT_DISPLAY_CTRL,HSYNC_PHASE)
-					& FIELD_CLEAR(CRT_DISPLAY_CTRL,TIMING)
-					& FIELD_CLEAR(CRT_DISPLAY_CTRL,PLANE);
+					& FIELD_CLEAR(CRT_DISPLAY_CTRL, VSYNC_PHASE)
+					& FIELD_CLEAR(CRT_DISPLAY_CTRL, HSYNC_PHASE)
+					& FIELD_CLEAR(CRT_DISPLAY_CTRL, TIMING)
+					& FIELD_CLEAR(CRT_DISPLAY_CTRL, PLANE);
 
-			 POKE32(CRT_DISPLAY_CTRL,ulTmpValue|ulReg);
+			 POKE32(CRT_DISPLAY_CTRL, ulTmpValue|ulReg);
 		}
 
 	}
 	else if(pll->clockType == PRIMARY_PLL)
 	{
 		unsigned int ulReservedBits;
-		POKE32(PANEL_PLL_CTRL,formatPllReg(pll));
+		POKE32(PANEL_PLL_CTRL, formatPllReg(pll));
 
         POKE32(PANEL_HORIZONTAL_TOTAL,
               FIELD_VALUE(0, PANEL_HORIZONTAL_TOTAL, TOTAL, pModeParam->horizontal_total - 1)
@@ -141,16 +141,16 @@ static int programModeRegisters(mode_parameter_t * pModeParam,pll_value_t * pll)
               FIELD_VALUE(0, PANEL_VERTICAL_SYNC, HEIGHT, pModeParam->vertical_sync_height)
             | FIELD_VALUE(0, PANEL_VERTICAL_SYNC, START, pModeParam->vertical_sync_start - 1));
 
-		ulTmpValue = FIELD_VALUE(0,PANEL_DISPLAY_CTRL,VSYNC_PHASE,pModeParam->vertical_sync_polarity)|
-					FIELD_VALUE(0,PANEL_DISPLAY_CTRL,HSYNC_PHASE,pModeParam->horizontal_sync_polarity)|
-					FIELD_VALUE(0,PANEL_DISPLAY_CTRL,CLOCK_PHASE,pModeParam->clock_phase_polarity)|
-					FIELD_SET(0,PANEL_DISPLAY_CTRL,TIMING,ENABLE)|
-					FIELD_SET(0,PANEL_DISPLAY_CTRL,PLANE,ENABLE);
+		ulTmpValue = FIELD_VALUE(0, PANEL_DISPLAY_CTRL, VSYNC_PHASE, pModeParam->vertical_sync_polarity)|
+					FIELD_VALUE(0, PANEL_DISPLAY_CTRL, HSYNC_PHASE, pModeParam->horizontal_sync_polarity)|
+					FIELD_VALUE(0, PANEL_DISPLAY_CTRL, CLOCK_PHASE, pModeParam->clock_phase_polarity)|
+					FIELD_SET(0, PANEL_DISPLAY_CTRL, TIMING, ENABLE)|
+					FIELD_SET(0, PANEL_DISPLAY_CTRL, PLANE, ENABLE);
 
         ulReservedBits = FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_1_MASK, ENABLE) |
                          FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_2_MASK, ENABLE) |
                          FIELD_SET(0, PANEL_DISPLAY_CTRL, RESERVED_3_MASK, ENABLE)|
-                         FIELD_SET(0,PANEL_DISPLAY_CTRL,VSYNC,ACTIVE_LOW);
+                         FIELD_SET(0, PANEL_DISPLAY_CTRL, VSYNC, ACTIVE_LOW);
 
         ulReg = (PEEK32(PANEL_DISPLAY_CTRL) & ~ulReservedBits)
               & FIELD_CLEAR(PANEL_DISPLAY_CTRL, CLOCK_PHASE)
@@ -162,20 +162,20 @@ static int programModeRegisters(mode_parameter_t * pModeParam,pll_value_t * pll)
 
 		/* May a hardware bug or just my test chip (not confirmed).
 		* PANEL_DISPLAY_CTRL register seems requiring few writes
-		* before a value can be succesfully written in.
+		* before a value can be successfully written in.
 		* Added some masks to mask out the reserved bits.
 		* Note: This problem happens by design. The hardware will wait for the
 		*       next vertical sync to turn on/off the plane.
 		*/
 
-		POKE32(PANEL_DISPLAY_CTRL,ulTmpValue|ulReg);
+		POKE32(PANEL_DISPLAY_CTRL, ulTmpValue|ulReg);
 #if 1
 		while((PEEK32(PANEL_DISPLAY_CTRL) & ~ulReservedBits) != (ulTmpValue|ulReg))
 		{
 			cnt++;
 			if(cnt > 1000)
 				break;
-			POKE32(PANEL_DISPLAY_CTRL,ulTmpValue|ulReg);
+			POKE32(PANEL_DISPLAY_CTRL, ulTmpValue|ulReg);
 		}
 #endif
 	}
@@ -185,20 +185,20 @@ static int programModeRegisters(mode_parameter_t * pModeParam,pll_value_t * pll)
 	return ret;
 }
 
-int ddk750_setModeTiming(mode_parameter_t * parm,clock_type_t clock)
+int ddk750_setModeTiming(mode_parameter_t *parm, clock_type_t clock)
 {
 	pll_value_t pll;
 	unsigned int uiActualPixelClk;
 	pll.inputFreq = DEFAULT_INPUT_CLOCK;
 	pll.clockType = clock;
 
-	uiActualPixelClk = calcPllValue(parm->pixel_clock,&pll);
+	uiActualPixelClk = calcPllValue(parm->pixel_clock, &pll);
 	if(getChipType() == SM750LE){
 		/* set graphic mode via IO method */
-		outb_p(0x88,0x3d4);
-		outb_p(0x06,0x3d5);
+		outb_p(0x88, 0x3d4);
+		outb_p(0x06, 0x3d5);
 	}
-	programModeRegisters(parm,&pll);
+	programModeRegisters(parm, &pll);
 	return 0;
 }
 
diff --git a/drivers/staging/sm750fb/ddk750_mode.h b/drivers/staging/sm750fb/ddk750_mode.h
index 6f8df96a8b02..4e8fab3f17e4 100644
--- a/drivers/staging/sm750fb/ddk750_mode.h
+++ b/drivers/staging/sm750fb/ddk750_mode.h
@@ -37,7 +37,7 @@ typedef struct _mode_parameter_t
 }
 mode_parameter_t;
 
-int ddk750_setModeTiming(mode_parameter_t *,clock_type_t);
+int ddk750_setModeTiming(mode_parameter_t *, clock_type_t);
 
 
 #endif
diff --git a/drivers/staging/sm750fb/ddk750_power.c b/drivers/staging/sm750fb/ddk750_power.c
index cbb97676b33c..1e5f398aed10 100644
--- a/drivers/staging/sm750fb/ddk750_power.c
+++ b/drivers/staging/sm750fb/ddk750_power.c
@@ -7,10 +7,10 @@ void ddk750_setDPMS(DPMS_t state)
 	unsigned int value;
 	if(getChipType() == SM750LE){
 		value = PEEK32(CRT_DISPLAY_CTRL);
-		POKE32(CRT_DISPLAY_CTRL,FIELD_VALUE(value,CRT_DISPLAY_CTRL,DPMS,state));
+		POKE32(CRT_DISPLAY_CTRL, FIELD_VALUE(value, CRT_DISPLAY_CTRL, DPMS, state));
 	}else{
 		value = PEEK32(SYSTEM_CTRL);
-		value= FIELD_VALUE(value,SYSTEM_CTRL,DPMS,state);
+		value= FIELD_VALUE(value, SYSTEM_CTRL, DPMS, state);
 		POKE32(SYSTEM_CTRL, value);
 	}
 }
diff --git a/drivers/staging/sm750fb/ddk750_power.h b/drivers/staging/sm750fb/ddk750_power.h
index 71dc7f980069..4e00955a07dd 100644
--- a/drivers/staging/sm750fb/ddk750_power.h
+++ b/drivers/staging/sm750fb/ddk750_power.h
@@ -12,7 +12,7 @@ DPMS_t;
 
 #define setDAC(off) \
 		{	\
-		POKE32(MISC_CTRL,FIELD_VALUE(PEEK32(MISC_CTRL),	\
+		POKE32(MISC_CTRL, FIELD_VALUE(PEEK32(MISC_CTRL), \
 									MISC_CTRL,	\
 									DAC_POWER,	\
 									off));	\
diff --git a/drivers/staging/sm750fb/ddk750_reg.h b/drivers/staging/sm750fb/ddk750_reg.h
index 2016f97d2a3d..1a40dc2a2f75 100644
--- a/drivers/staging/sm750fb/ddk750_reg.h
+++ b/drivers/staging/sm750fb/ddk750_reg.h
@@ -1885,10 +1885,10 @@
 #define DISPLAY_CONTROL_750LE 	0x80288
 /* Palette RAM */
 
-/* Panel Pallete register starts at 0x080400 ~ 0x0807FC */
+/* Panel Palette register starts at 0x080400 ~ 0x0807FC */
 #define PANEL_PALETTE_RAM                             0x080400
 
-/* Panel Pallete register starts at 0x080C00 ~ 0x080FFC */
+/* Panel Palette register starts at 0x080C00 ~ 0x080FFC */
 #define CRT_PALETTE_RAM                               0x080C00
 
 /* 2D registers
diff --git a/drivers/staging/sm750fb/ddk750_sii164.c b/drivers/staging/sm750fb/ddk750_sii164.c
index 3d224d6a74ff..b6395b87fc21 100644
--- a/drivers/staging/sm750fb/ddk750_sii164.c
+++ b/drivers/staging/sm750fb/ddk750_sii164.c
@@ -125,10 +125,7 @@ long sii164InitChip(
     unsigned char pllFilterValue
 )
 {
-    //unsigned char ucRegIndex, ucRegValue;
-    //unsigned char ucDeviceAddress,
 	unsigned char config;
-    //unsigned long delayCount;
 
     /* Initialize the i2c bus */
 #ifdef USE_HW_I2C
@@ -141,10 +138,6 @@ long sii164InitChip(
     /* Check if SII164 Chip exists */
     if ((sii164GetVendorID() == SII164_VENDOR_ID) && (sii164GetDeviceID() == SII164_DEVICE_ID))
     {
-
-#ifdef DDKDEBUG
-        //sii164PrintRegisterValues();
-#endif
         /*
          *  Initialize SII164 controller chip.
          */
@@ -241,10 +234,6 @@ long sii164InitChip(
         config |= SII164_CONFIGURATION_POWER_NORMAL;
         i2cWriteReg(SII164_I2C_ADDRESS, SII164_CONFIGURATION, config);
 
-#ifdef DDKDEBUG
-        //sii164PrintRegisterValues();
-#endif
-
         return 0;
     }
 
@@ -256,7 +245,7 @@ long sii164InitChip(
 
 
 
-/* below sii164 function is not neccessary */
+/* below sii164 function is not necessary */
 
 #ifdef SII164_FULL_FUNCTIONS
 
@@ -388,7 +377,7 @@ unsigned char sii164IsConnected(void)
 
 /*
  *  sii164CheckInterrupt
- *      Checks if interrupt has occured.
+ *      Checks if interrupt has occurred.
  *
  *  Output:
  *      0   - No interrupt
diff --git a/drivers/staging/sm750fb/ddk750_swi2c.c b/drivers/staging/sm750fb/ddk750_swi2c.c
index 901b3737f1ed..5133bccf17bd 100644
--- a/drivers/staging/sm750fb/ddk750_swi2c.c
+++ b/drivers/staging/sm750fb/ddk750_swi2c.c
@@ -96,15 +96,14 @@ static void swI2CWait(void)
        it's more reliable than counter loop ..
        write 0x61 to 0x3ce and read from 0x3cf
        */
-	while(peekIO(0x3ce,0x61) & 0x10);
+	while (peekIO(0x3ce, 0x61) & 0x10);
 #else
-    int i, Temp;
+	int i, Temp;
 
-    for(i=0; i<600; i++)
-    {
-        Temp = i;
-        Temp += i;
-    }
+	for (i = 0; i < 600; i++) {
+		Temp = i;
+		Temp += i;
+	}
 #endif
 }
 
@@ -122,27 +121,24 @@ static void swI2CWait(void)
  */
 void swI2CSCL(unsigned char value)
 {
-    unsigned long ulGPIOData;
-    unsigned long ulGPIODirection;
-
-    ulGPIODirection = PEEK32(g_i2cClkGPIODataDirReg);
-    if (value)      /* High */
-    {
-        /* Set direction as input. This will automatically pull the signal up. */
-        ulGPIODirection &= ~(1 << g_i2cClockGPIO);
-        POKE32(g_i2cClkGPIODataDirReg, ulGPIODirection);
-    }
-    else            /* Low */
-    {
-        /* Set the signal down */
-        ulGPIOData = PEEK32(g_i2cClkGPIODataReg);
-        ulGPIOData &= ~(1 << g_i2cClockGPIO);
-        POKE32(g_i2cClkGPIODataReg, ulGPIOData);
-
-        /* Set direction as output */
-        ulGPIODirection |= (1 << g_i2cClockGPIO);
-        POKE32(g_i2cClkGPIODataDirReg, ulGPIODirection);
-    }
+	unsigned long ulGPIOData;
+	unsigned long ulGPIODirection;
+
+	ulGPIODirection = PEEK32(g_i2cClkGPIODataDirReg);
+	if (value) {    /* High */
+		/* Set direction as input. This will automatically pull the signal up. */
+		ulGPIODirection &= ~(1 << g_i2cClockGPIO);
+		POKE32(g_i2cClkGPIODataDirReg, ulGPIODirection);
+	} else {        /* Low */
+		/* Set the signal down */
+		ulGPIOData = PEEK32(g_i2cClkGPIODataReg);
+		ulGPIOData &= ~(1 << g_i2cClockGPIO);
+		POKE32(g_i2cClkGPIODataReg, ulGPIOData);
+
+		/* Set direction as output */
+		ulGPIODirection |= (1 << g_i2cClockGPIO);
+		POKE32(g_i2cClkGPIODataDirReg, ulGPIODirection);
+	}
 }
 
 /*
@@ -159,27 +155,24 @@ void swI2CSCL(unsigned char value)
  */
 void swI2CSDA(unsigned char value)
 {
-    unsigned long ulGPIOData;
-    unsigned long ulGPIODirection;
-
-    ulGPIODirection = PEEK32(g_i2cDataGPIODataDirReg);
-    if (value)      /* High */
-    {
-        /* Set direction as input. This will automatically pull the signal up. */
-        ulGPIODirection &= ~(1 << g_i2cDataGPIO);
-        POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
-    }
-    else            /* Low */
-    {
-        /* Set the signal down */
-        ulGPIOData = PEEK32(g_i2cDataGPIODataReg);
-        ulGPIOData &= ~(1 << g_i2cDataGPIO);
-        POKE32(g_i2cDataGPIODataReg, ulGPIOData);
-
-        /* Set direction as output */
-        ulGPIODirection |= (1 << g_i2cDataGPIO);
-        POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
-    }
+	unsigned long ulGPIOData;
+	unsigned long ulGPIODirection;
+
+	ulGPIODirection = PEEK32(g_i2cDataGPIODataDirReg);
+	if (value) {    /* High */
+		/* Set direction as input. This will automatically pull the signal up. */
+		ulGPIODirection &= ~(1 << g_i2cDataGPIO);
+		POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
+	} else {        /* Low */
+		/* Set the signal down */
+		ulGPIOData = PEEK32(g_i2cDataGPIODataReg);
+		ulGPIOData &= ~(1 << g_i2cDataGPIO);
+		POKE32(g_i2cDataGPIODataReg, ulGPIOData);
+
+		/* Set direction as output */
+		ulGPIODirection |= (1 << g_i2cDataGPIO);
+		POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
+	}
 }
 
 /*
@@ -190,23 +183,22 @@ void swI2CSDA(unsigned char value)
  */
 static unsigned char swI2CReadSDA(void)
 {
-    unsigned long ulGPIODirection;
-    unsigned long ulGPIOData;
-
-    /* Make sure that the direction is input (High) */
-    ulGPIODirection = PEEK32(g_i2cDataGPIODataDirReg);
-    if ((ulGPIODirection & (1 << g_i2cDataGPIO)) != (~(1 << g_i2cDataGPIO)))
-    {
-        ulGPIODirection &= ~(1 << g_i2cDataGPIO);
-        POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
-    }
-
-    /* Now read the SDA line */
-    ulGPIOData = PEEK32(g_i2cDataGPIODataReg);
-    if (ulGPIOData & (1 << g_i2cDataGPIO))
-        return 1;
-    else
-        return 0;
+	unsigned long ulGPIODirection;
+	unsigned long ulGPIOData;
+
+	/* Make sure that the direction is input (High) */
+	ulGPIODirection = PEEK32(g_i2cDataGPIODataDirReg);
+	if ((ulGPIODirection & (1 << g_i2cDataGPIO)) != (~(1 << g_i2cDataGPIO))) {
+		ulGPIODirection &= ~(1 << g_i2cDataGPIO);
+		POKE32(g_i2cDataGPIODataDirReg, ulGPIODirection);
+	}
+
+	/* Now read the SDA line */
+	ulGPIOData = PEEK32(g_i2cDataGPIODataReg);
+	if (ulGPIOData & (1 << g_i2cDataGPIO))
+		return 1;
+	else
+		return 0;
 }
 
 /*
@@ -214,7 +206,7 @@ static unsigned char swI2CReadSDA(void)
  */
 static void swI2CAck(void)
 {
-    return;  /* Single byte read is ok without it. */
+	return;  /* Single byte read is ok without it. */
 }
 
 /*
@@ -222,10 +214,10 @@ static void swI2CAck(void)
  */
 static void swI2CStart(void)
 {
-    /* Start I2C */
-    swI2CSDA(1);
-    swI2CSCL(1);
-    swI2CSDA(0);
+	/* Start I2C */
+	swI2CSDA(1);
+	swI2CSCL(1);
+	swI2CSDA(0);
 }
 
 /*
@@ -233,10 +225,10 @@ static void swI2CStart(void)
  */
 static void swI2CStop(void)
 {
-    /* Stop the I2C */
-    swI2CSCL(1);
-    swI2CSDA(0);
-    swI2CSDA(1);
+	/* Stop the I2C */
+	swI2CSCL(1);
+	swI2CSDA(0);
+	swI2CSDA(1);
 }
 
 /*
@@ -251,60 +243,58 @@ static void swI2CStop(void)
  */
 static long swI2CWriteByte(unsigned char data)
 {
-    unsigned char value = data;
-    int i;
-
-    /* Sending the data bit by bit */
-    for (i=0; i<8; i++)
-    {
-        /* Set SCL to low */
-        swI2CSCL(0);
-
-        /* Send data bit */
-        if ((value & 0x80) != 0)
-            swI2CSDA(1);
-        else
-            swI2CSDA(0);
-
-        swI2CWait();
-
-        /* Toggle clk line to one */
-        swI2CSCL(1);
-        swI2CWait();
-
-        /* Shift byte to be sent */
-        value = value << 1;
-    }
-
-    /* Set the SCL Low and SDA High (prepare to get input) */
-    swI2CSCL(0);
-    swI2CSDA(1);
-
-    /* Set the SCL High for ack */
-    swI2CWait();
-    swI2CSCL(1);
-    swI2CWait();
-
-    /* Read SDA, until SDA==0 */
-    for(i=0; i<0xff; i++)
-    {
-        if (!swI2CReadSDA())
-            break;
-
-        swI2CSCL(0);
-        swI2CWait();
-        swI2CSCL(1);
-        swI2CWait();
-    }
-
-    /* Set the SCL Low and SDA High */
-    swI2CSCL(0);
-    swI2CSDA(1);
-
-    if (i<0xff)
-        return 0;
-    else
-        return -1;
+	unsigned char value = data;
+	int i;
+
+	/* Sending the data bit by bit */
+	for (i = 0; i < 8; i++) {
+		/* Set SCL to low */
+		swI2CSCL(0);
+
+		/* Send data bit */
+		if ((value & 0x80) != 0)
+			swI2CSDA(1);
+		else
+			swI2CSDA(0);
+
+		swI2CWait();
+
+		/* Toggle clk line to one */
+		swI2CSCL(1);
+		swI2CWait();
+
+		/* Shift byte to be sent */
+		value = value << 1;
+	}
+
+	/* Set the SCL Low and SDA High (prepare to get input) */
+	swI2CSCL(0);
+	swI2CSDA(1);
+
+	/* Set the SCL High for ack */
+	swI2CWait();
+	swI2CSCL(1);
+	swI2CWait();
+
+	/* Read SDA, until SDA==0 */
+	for (i = 0; i < 0xff; i++) {
+		if (!swI2CReadSDA())
+			break;
+
+		swI2CSCL(0);
+		swI2CWait();
+		swI2CSCL(1);
+		swI2CWait();
+	}
+
+	/* Set the SCL Low and SDA High */
+	swI2CSCL(0);
+	swI2CSDA(1);
+
+	if (i < 0xff)
+		return 0;
+	else
+		return -1;
 }
 
 /*
@@ -319,32 +309,31 @@ static long swI2CWriteByte(unsigned char data)
  */
 static unsigned char swI2CReadByte(unsigned char ack)
 {
-    int i;
-    unsigned char data = 0;
+	int i;
+	unsigned char data = 0;
 
-    for(i=7; i>=0; i--)
-    {
-        /* Set the SCL to Low and SDA to High (Input) */
-        swI2CSCL(0);
-        swI2CSDA(1);
-        swI2CWait();
+	for (i = 7; i >= 0; i--) {
+		/* Set the SCL to Low and SDA to High (Input) */
+		swI2CSCL(0);
+		swI2CSDA(1);
+		swI2CWait();
 
-        /* Set the SCL High */
-        swI2CSCL(1);
-        swI2CWait();
+		/* Set the SCL High */
+		swI2CSCL(1);
+		swI2CWait();
 
-        /* Read data bits from SDA */
-        data |= (swI2CReadSDA() << i);
-    }
+		/* Read data bits from SDA */
+		data |= (swI2CReadSDA() << i);
+	}
 
-    if (ack)
-        swI2CAck();
+	if (ack)
+		swI2CAck();
 
-    /* Set the SCL Low and SDA High */
-    swI2CSCL(0);
-    swI2CSDA(1);
+	/* Set the SCL Low and SDA High */
+	swI2CSCL(0);
+	swI2CSDA(1);
 
-    return data;
+	return data;
 }
 
 /*
@@ -361,29 +350,29 @@ static unsigned char swI2CReadByte(unsigned char ack)
 static long swI2CInit_SM750LE(unsigned char i2cClkGPIO,
 			      unsigned char i2cDataGPIO)
 {
-    int i;
+	int i;
 
-    /* Initialize the GPIO pin for the i2c Clock Register */
-    g_i2cClkGPIODataReg = GPIO_DATA_SM750LE;
-    g_i2cClkGPIODataDirReg = GPIO_DATA_DIRECTION_SM750LE;
+	/* Initialize the GPIO pin for the i2c Clock Register */
+	g_i2cClkGPIODataReg = GPIO_DATA_SM750LE;
+	g_i2cClkGPIODataDirReg = GPIO_DATA_DIRECTION_SM750LE;
 
-    /* Initialize the Clock GPIO Offset */
-    g_i2cClockGPIO = i2cClkGPIO;
+	/* Initialize the Clock GPIO Offset */
+	g_i2cClockGPIO = i2cClkGPIO;
 
-    /* Initialize the GPIO pin for the i2c Data Register */
-    g_i2cDataGPIODataReg = GPIO_DATA_SM750LE;
-    g_i2cDataGPIODataDirReg = GPIO_DATA_DIRECTION_SM750LE;
+	/* Initialize the GPIO pin for the i2c Data Register */
+	g_i2cDataGPIODataReg = GPIO_DATA_SM750LE;
+	g_i2cDataGPIODataDirReg = GPIO_DATA_DIRECTION_SM750LE;
 
-    /* Initialize the Data GPIO Offset */
-    g_i2cDataGPIO = i2cDataGPIO;
+	/* Initialize the Data GPIO Offset */
+	g_i2cDataGPIO = i2cDataGPIO;
 
-    /* Note that SM750LE don't have GPIO MUX and power is always on */
+	/* Note that SM750LE don't have GPIO MUX and power is always on */
 
-    /* Clear the i2c lines. */
-    for(i=0; i<9; i++)
-        swI2CStop();
+	/* Clear the i2c lines. */
+	for (i = 0; i < 9; i++)
+		swI2CStop();
 
-    return 0;
+	return 0;
 }
 
 /*
@@ -398,49 +387,49 @@ static long swI2CInit_SM750LE(unsigned char i2cClkGPIO,
  *       0   - Success
  */
 long swI2CInit(
-    unsigned char i2cClkGPIO,
-    unsigned char i2cDataGPIO
+	unsigned char i2cClkGPIO,
+	unsigned char i2cDataGPIO
 )
 {
-    int i;
+	int i;
 
-    /* Return 0 if the GPIO pins to be used is out of range. The range is only from [0..63] */
-    if ((i2cClkGPIO > 31) || (i2cDataGPIO > 31))
-        return -1;
+	/* Return 0 if the GPIO pins to be used is out of range. The range is only from [0..63] */
+	if ((i2cClkGPIO > 31) || (i2cDataGPIO > 31))
+		return -1;
 
-    if (getChipType() == SM750LE)
-        return swI2CInit_SM750LE(i2cClkGPIO, i2cDataGPIO);
+	if (getChipType() == SM750LE)
+		return swI2CInit_SM750LE(i2cClkGPIO, i2cDataGPIO);
 
-    /* Initialize the GPIO pin for the i2c Clock Register */
-    g_i2cClkGPIOMuxReg = GPIO_MUX;
-    g_i2cClkGPIODataReg = GPIO_DATA;
-    g_i2cClkGPIODataDirReg = GPIO_DATA_DIRECTION;
+	/* Initialize the GPIO pin for the i2c Clock Register */
+	g_i2cClkGPIOMuxReg = GPIO_MUX;
+	g_i2cClkGPIODataReg = GPIO_DATA;
+	g_i2cClkGPIODataDirReg = GPIO_DATA_DIRECTION;
 
-    /* Initialize the Clock GPIO Offset */
-    g_i2cClockGPIO = i2cClkGPIO;
+	/* Initialize the Clock GPIO Offset */
+	g_i2cClockGPIO = i2cClkGPIO;
 
-    /* Initialize the GPIO pin for the i2c Data Register */
-    g_i2cDataGPIOMuxReg = GPIO_MUX;
-    g_i2cDataGPIODataReg = GPIO_DATA;
-    g_i2cDataGPIODataDirReg = GPIO_DATA_DIRECTION;
+	/* Initialize the GPIO pin for the i2c Data Register */
+	g_i2cDataGPIOMuxReg = GPIO_MUX;
+	g_i2cDataGPIODataReg = GPIO_DATA;
+	g_i2cDataGPIODataDirReg = GPIO_DATA_DIRECTION;
 
-    /* Initialize the Data GPIO Offset */
-    g_i2cDataGPIO = i2cDataGPIO;
+	/* Initialize the Data GPIO Offset */
+	g_i2cDataGPIO = i2cDataGPIO;
 
-    /* Enable the GPIO pins for the i2c Clock and Data (GPIO MUX) */
-    POKE32(g_i2cClkGPIOMuxReg,
-                      PEEK32(g_i2cClkGPIOMuxReg) & ~(1 << g_i2cClockGPIO));
-    POKE32(g_i2cDataGPIOMuxReg,
-                      PEEK32(g_i2cDataGPIOMuxReg) & ~(1 << g_i2cDataGPIO));
+	/* Enable the GPIO pins for the i2c Clock and Data (GPIO MUX) */
+	POKE32(g_i2cClkGPIOMuxReg,
+			PEEK32(g_i2cClkGPIOMuxReg) & ~(1 << g_i2cClockGPIO));
+	POKE32(g_i2cDataGPIOMuxReg,
+			PEEK32(g_i2cDataGPIOMuxReg) & ~(1 << g_i2cDataGPIO));
 
-    /* Enable GPIO power */
-    enableGPIO(1);
+	/* Enable GPIO power */
+	enableGPIO(1);
 
-    /* Clear the i2c lines. */
-    for(i=0; i<9; i++)
-        swI2CStop();
+	/* Clear the i2c lines. */
+	for (i = 0; i < 9; i++)
+		swI2CStop();
 
-    return 0;
+	return 0;
 }
 
 /*
@@ -455,30 +444,30 @@ long swI2CInit(
  *      Register value
  */
 unsigned char swI2CReadReg(
-    unsigned char deviceAddress,
-    unsigned char registerIndex
+	unsigned char deviceAddress,
+	unsigned char registerIndex
 )
 {
-    unsigned char data;
+	unsigned char data;
 
-    /* Send the Start signal */
-    swI2CStart();
+	/* Send the Start signal */
+	swI2CStart();
 
-    /* Send the device address */
-    swI2CWriteByte(deviceAddress);
+	/* Send the device address */
+	swI2CWriteByte(deviceAddress);
 
-    /* Send the register index */
-    swI2CWriteByte(registerIndex);
+	/* Send the register index */
+	swI2CWriteByte(registerIndex);
 
-    /* Get the bus again and get the data from the device read address */
-    swI2CStart();
-    swI2CWriteByte(deviceAddress + 1);
-    data = swI2CReadByte(1);
+	/* Get the bus again and get the data from the device read address */
+	swI2CStart();
+	swI2CWriteByte(deviceAddress + 1);
+	data = swI2CReadByte(1);
 
-    /* Stop swI2C and release the bus */
-    swI2CStop();
+	/* Stop swI2C and release the bus */
+	swI2CStop();
 
-    return data;
+	return data;
 }
 
 /*
@@ -495,28 +484,27 @@ unsigned char swI2CReadReg(
  *         -1   - Fail
  */
 long swI2CWriteReg(
-    unsigned char deviceAddress,
-    unsigned char registerIndex,
-    unsigned char data
+	unsigned char deviceAddress,
+	unsigned char registerIndex,
+	unsigned char data
 )
 {
-    long returnValue = 0;
+	long returnValue = 0;
 
-    /* Send the Start signal */
-    swI2CStart();
+	/* Send the Start signal */
+	swI2CStart();
 
-    /* Send the device address and read the data. All should return success
-       in order for the writing processed to be successful
-     */
-    if ((swI2CWriteByte(deviceAddress) != 0) ||
-        (swI2CWriteByte(registerIndex) != 0) ||
-        (swI2CWriteByte(data) != 0))
-    {
-        returnValue = -1;
-    }
+	/* Send the device address and read the data. All should return success
+	   in order for the writing processed to be successful
+	*/
+	if ((swI2CWriteByte(deviceAddress) != 0) ||
+	    (swI2CWriteByte(registerIndex) != 0) ||
+	    (swI2CWriteByte(data) != 0)) {
+		returnValue = -1;
+	}
 
-    /* Stop i2c and release the bus */
-    swI2CStop();
+	/* Stop i2c and release the bus */
+	swI2CStop();
 
-    return returnValue;
+	return returnValue;
 }
diff --git a/drivers/staging/sm750fb/ddk750_swi2c.h b/drivers/staging/sm750fb/ddk750_swi2c.h
index ec5463b98ddf..4af2b7aa1f17 100644
--- a/drivers/staging/sm750fb/ddk750_swi2c.h
+++ b/drivers/staging/sm750fb/ddk750_swi2c.h
@@ -29,8 +29,8 @@
  *       0   - Success
  */
 long swI2CInit(
-    unsigned char i2cClkGPIO,
-    unsigned char i2cDataGPIO
+	unsigned char i2cClkGPIO,
+	unsigned char i2cDataGPIO
 );
 
 /*
@@ -45,8 +45,8 @@ long swI2CInit(
  *      Register value
  */
 unsigned char swI2CReadReg(
-    unsigned char deviceAddress,
-    unsigned char registerIndex
+	unsigned char deviceAddress,
+	unsigned char registerIndex
 );
 
 /*
@@ -63,14 +63,14 @@ unsigned char swI2CReadReg(
  *         -1   - Fail
  */
 long swI2CWriteReg(
-    unsigned char deviceAddress,
-    unsigned char registerIndex,
-    unsigned char data
+	unsigned char deviceAddress,
+	unsigned char registerIndex,
+	unsigned char data
 );
 
 /*
- *  These two functions are used to toggle the data on the SCL and SDA I2C lines.
- *  The used of these two functions are not recommended unless it is necessary.
+ *  These two functions toggle the data on the SCL and SDA I2C lines.
+ *  The use of these two functions is not recommended unless it is necessary.
  */
 
 /*
diff --git a/drivers/staging/sm750fb/modedb.h b/drivers/staging/sm750fb/modedb.h
index c5275c6fffaf..83cb2e2ae51a 100644
--- a/drivers/staging/sm750fb/modedb.h
+++ b/drivers/staging/sm750fb/modedb.h
@@ -1,221 +1,233 @@
 
 static const struct fb_videomode modedb2[] = {
-    {
-	/* 640x400 @ 70 Hz, 31.5 kHz hsync */
-	NULL, 70, 640, 400, 39721, 40, 24, 39, 9, 96, 2,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 640x480 @ 60 Hz, 31.5 kHz hsync */
-	NULL, 60, 640, 480, 39721, 40, 24, 32, 11, 96, 2,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 800x600 @ 56 Hz, 35.15 kHz hsync */
-	NULL, 56, 800, 600, 27777, 128, 24, 22, 1, 72, 2,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1024x768 @ 87 Hz interlaced, 35.5 kHz hsync */
-	NULL, 87, 1024, 768, 22271, 56, 24, 33, 8, 160, 8,
-	0, FB_VMODE_INTERLACED
-    }, {
-	/* 640x400 @ 85 Hz, 37.86 kHz hsync */
-	NULL, 85, 640, 400, 31746, 96, 32, 41, 1, 64, 3,
-	FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 640x480 @ 72 Hz, 36.5 kHz hsync */
-	NULL, 72, 640, 480, 31746, 144, 40, 30, 8, 40, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 640x480 @ 75 Hz, 37.50 kHz hsync */
-	NULL, 75, 640, 480, 31746, 120, 16, 16, 1, 64, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 800x600 @ 60 Hz, 37.8 kHz hsync */
-	NULL, 60, 800, 600, 25000, 88, 40, 23, 1, 128, 4,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 640x480 @ 85 Hz, 43.27 kHz hsync */
-	NULL, 85, 640, 480, 27777, 80, 56, 25, 1, 56, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1152x864 @ 89 Hz interlaced, 44 kHz hsync */
-	NULL, 69, 1152, 864, 15384, 96, 16, 110, 1, 216, 10,
-	0, FB_VMODE_INTERLACED
-    }, {
-	/* 800x600 @ 72 Hz, 48.0 kHz hsync */
-	NULL, 72, 800, 600, 20000, 64, 56, 23, 37, 120, 6,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1024x768 @ 60 Hz, 48.4 kHz hsync */
-	NULL, 60, 1024, 768, 15384, 168, 8, 29, 3, 144, 6,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 640x480 @ 100 Hz, 53.01 kHz hsync */
-	NULL, 100, 640, 480, 21834, 96, 32, 36, 8, 96, 6,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1152x864 @ 60 Hz, 53.5 kHz hsync */
-	NULL, 60, 1152, 864, 11123, 208, 64, 16, 4, 256, 8,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 800x600 @ 85 Hz, 55.84 kHz hsync */
-	NULL, 85, 800, 600, 16460, 160, 64, 36, 16, 64, 5,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1024x768 @ 70 Hz, 56.5 kHz hsync */
-	NULL, 70, 1024, 768, 13333, 144, 24, 29, 3, 136, 6,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-        /*  1280x960-60 VESA */
-        NULL, 60, 1280, 960, 9259, 312, 96, 36, 1, 112, 3,
-        FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,  FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
-    }, {
-        /*  1280x1024-60 VESA */
-        NULL, 60, 1280, 1024, 9259, 248, 48, 38, 1, 112, 3,
-        FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
-    }, {
-	/* 1280x1024 @ 87 Hz interlaced, 51 kHz hsync */
-	NULL, 87, 1280, 1024, 12500, 56, 16, 128, 1, 216, 12,
-	0, FB_VMODE_INTERLACED
-    }, {
-	/* 800x600 @ 100 Hz, 64.02 kHz hsync */
-	NULL, 100, 800, 600, 14357, 160, 64, 30, 4, 64, 6,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1024x768 @ 76 Hz, 62.5 kHz hsync */
-	NULL, 76, 1024, 768, 11764, 208, 8, 36, 16, 120, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1152x864 @ 70 Hz, 62.4 kHz hsync */
-	NULL, 70, 1152, 864, 10869, 106, 56, 20, 1, 160, 10,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1280x1024 @ 61 Hz, 64.2 kHz hsync */
-	NULL, 61, 1280, 1024, 9090, 200, 48, 26, 1, 184, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1400x1050 @ 60Hz, 63.9 kHz hsync */
-	NULL, 68, 1400, 1050, 9259, 136, 40, 13, 1, 112, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1400x1050 @ 75,107 Hz, 82,392 kHz +hsync +vsync*/
-	NULL, 75, 1400, 1050, 9271, 120, 56, 13, 0, 112, 3,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1400x1050 @ 60 Hz, ? kHz +hsync +vsync*/
-        NULL, 60, 1400, 1050, 9259, 128, 40, 12, 0, 112, 3,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1024x768 @ 85 Hz, 70.24 kHz hsync */
-	NULL, 85, 1024, 768, 10111, 192, 32, 34, 14, 160, 6,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1152x864 @ 78 Hz, 70.8 kHz hsync */
-	NULL, 78, 1152, 864, 9090, 228, 88, 32, 0, 84, 12,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1280x1024 @ 70 Hz, 74.59 kHz hsync */
-	NULL, 70, 1280, 1024, 7905, 224, 32, 28, 8, 160, 8,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1600x1200 @ 60Hz, 75.00 kHz hsync */
-	NULL, 60, 1600, 1200, 6172, 304, 64, 46, 1, 192, 3,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1152x864 @ 84 Hz, 76.0 kHz hsync */
-	NULL, 84, 1152, 864, 7407, 184, 312, 32, 0, 128, 12,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1280x1024 @ 74 Hz, 78.85 kHz hsync */
-	NULL, 74, 1280, 1024, 7407, 256, 32, 34, 3, 144, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1024x768 @ 100Hz, 80.21 kHz hsync */
-	NULL, 100, 1024, 768, 8658, 192, 32, 21, 3, 192, 10,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1280x1024 @ 76 Hz, 81.13 kHz hsync */
-	NULL, 76, 1280, 1024, 7407, 248, 32, 34, 3, 104, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1600x1200 @ 70 Hz, 87.50 kHz hsync */
-	NULL, 70, 1600, 1200, 5291, 304, 64, 46, 1, 192, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1152x864 @ 100 Hz, 89.62 kHz hsync */
-	NULL, 100, 1152, 864, 7264, 224, 32, 17, 2, 128, 19,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1280x1024 @ 85 Hz, 91.15 kHz hsync */
-	NULL, 85, 1280, 1024, 6349, 224, 64, 44, 1, 160, 3,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1600x1200 @ 75 Hz, 93.75 kHz hsync */
-	NULL, 75, 1600, 1200, 4938, 304, 64, 46, 1, 192, 3,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1600x1200 @ 85 Hz, 105.77 kHz hsync */
-	NULL, 85, 1600, 1200, 4545, 272, 16, 37, 4, 192, 3,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1280x1024 @ 100 Hz, 107.16 kHz hsync */
-	NULL, 100, 1280, 1024, 5502, 256, 32, 26, 7, 128, 15,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1800x1440 @ 64Hz, 96.15 kHz hsync  */
-	NULL, 64, 1800, 1440, 4347, 304, 96, 46, 1, 192, 3,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 1800x1440 @ 70Hz, 104.52 kHz hsync  */
-	NULL, 70, 1800, 1440, 4000, 304, 96, 46, 1, 192, 3,
-	FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-    }, {
-	/* 512x384 @ 78 Hz, 31.50 kHz hsync */
-	NULL, 78, 512, 384, 49603, 48, 16, 16, 1, 64, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 512x384 @ 85 Hz, 34.38 kHz hsync */
-	NULL, 85, 512, 384, 45454, 48, 16, 16, 1, 64, 3,
-	0, FB_VMODE_NONINTERLACED
-    }, {
-	/* 320x200 @ 70 Hz, 31.5 kHz hsync, 8:5 aspect ratio */
-	NULL, 70, 320, 200, 79440, 16, 16, 20, 4, 48, 1,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 320x240 @ 60 Hz, 31.5 kHz hsync, 4:3 aspect ratio */
-	NULL, 60, 320, 240, 79440, 16, 16, 16, 5, 48, 1,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 320x240 @ 72 Hz, 36.5 kHz hsync */
-	NULL, 72, 320, 240, 63492, 16, 16, 16, 4, 48, 2,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 400x300 @ 56 Hz, 35.2 kHz hsync, 4:3 aspect ratio */
-	NULL, 56, 400, 300, 55555, 64, 16, 10, 1, 32, 1,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 400x300 @ 60 Hz, 37.8 kHz hsync */
-	NULL, 60, 400, 300, 50000, 48, 16, 11, 1, 64, 2,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 400x300 @ 72 Hz, 48.0 kHz hsync */
-	NULL, 72, 400, 300, 40000, 32, 24, 11, 19, 64, 3,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 480x300 @ 56 Hz, 35.2 kHz hsync, 8:5 aspect ratio */
-	NULL, 56, 480, 300, 46176, 80, 16, 10, 1, 40, 1,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 480x300 @ 60 Hz, 37.8 kHz hsync */
-	NULL, 60, 480, 300, 41858, 56, 16, 11, 1, 80, 2,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 480x300 @ 63 Hz, 39.6 kHz hsync */
-	NULL, 63, 480, 300, 40000, 56, 16, 11, 1, 80, 2,
-	0, FB_VMODE_DOUBLE
-    }, {
-	/* 480x300 @ 72 Hz, 48.0 kHz hsync */
-	NULL, 72, 480, 300, 33386, 40, 24, 11, 19, 80, 3,
-	0, FB_VMODE_DOUBLE
-    },
+	{
+		/* 640x400 @ 70 Hz, 31.5 kHz hsync */
+		NULL, 70, 640, 400, 39721, 40, 24, 39, 9, 96, 2,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 60 Hz, 31.5 kHz hsync */
+		NULL, 60, 640, 480, 39721, 40, 24, 32, 11, 96, 2,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 800x600 @ 56 Hz, 35.15 kHz hsync */
+		NULL, 56, 800, 600, 27777, 128, 24, 22, 1, 72, 2,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 87 Hz interlaced, 35.5 kHz hsync */
+		NULL, 87, 1024, 768, 22271, 56, 24, 33, 8, 160, 8,
+		0, FB_VMODE_INTERLACED
+	}, {
+		/* 640x400 @ 85 Hz, 37.86 kHz hsync */
+		NULL, 85, 640, 400, 31746, 96, 32, 41, 1, 64, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 72 Hz, 36.5 kHz hsync */
+		NULL, 72, 640, 480, 31746, 144, 40, 30, 8, 40, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 75 Hz, 37.50 kHz hsync */
+		NULL, 75, 640, 480, 31746, 120, 16, 16, 1, 64, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 800x600 @ 60 Hz, 37.8 kHz hsync */
+		NULL, 60, 800, 600, 25000, 88, 40, 23, 1, 128, 4,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 85 Hz, 43.27 kHz hsync */
+		NULL, 85, 640, 480, 27777, 80, 56, 25, 1, 56, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1152x864 @ 89 Hz interlaced, 44 kHz hsync */
+		NULL, 69, 1152, 864, 15384, 96, 16, 110, 1, 216, 10,
+		0, FB_VMODE_INTERLACED
+	}, {
+		/* 800x600 @ 72 Hz, 48.0 kHz hsync */
+		NULL, 72, 800, 600, 20000, 64, 56, 23, 37, 120, 6,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 60 Hz, 48.4 kHz hsync */
+		NULL, 60, 1024, 768, 15384, 168, 8, 29, 3, 144, 6,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 100 Hz, 53.01 kHz hsync */
+		NULL, 100, 640, 480, 21834, 96, 32, 36, 8, 96, 6,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1152x864 @ 60 Hz, 53.5 kHz hsync */
+		NULL, 60, 1152, 864, 11123, 208, 64, 16, 4, 256, 8,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 800x600 @ 85 Hz, 55.84 kHz hsync */
+		NULL, 85, 800, 600, 16460, 160, 64, 36, 16, 64, 5,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 70 Hz, 56.5 kHz hsync */
+		NULL, 70, 1024, 768, 13333, 144, 24, 29, 3, 136, 6,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/*  1280x960-60 VESA */
+		NULL, 60, 1280, 960, 9259, 312, 96, 36, 1, 112, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
+	}, {
+		/*  1280x1024-60 VESA */
+		NULL, 60, 1280, 1024, 9259, 248, 48, 38, 1, 112, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
+	}, {
+		/* 1280x1024 @ 87 Hz interlaced, 51 kHz hsync */
+		NULL, 87, 1280, 1024, 12500, 56, 16, 128, 1, 216, 12,
+		0, FB_VMODE_INTERLACED
+	}, {
+		/* 800x600 @ 100 Hz, 64.02 kHz hsync */
+		NULL, 100, 800, 600, 14357, 160, 64, 30, 4, 64, 6,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 76 Hz, 62.5 kHz hsync */
+		NULL, 76, 1024, 768, 11764, 208, 8, 36, 16, 120, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1152x864 @ 70 Hz, 62.4 kHz hsync */
+		NULL, 70, 1152, 864, 10869, 106, 56, 20, 1, 160, 10,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 61 Hz, 64.2 kHz hsync */
+		NULL, 61, 1280, 1024, 9090, 200, 48, 26, 1, 184, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1400x1050 @ 60Hz, 63.9 kHz hsync */
+		NULL, 68, 1400, 1050, 9259, 136, 40, 13, 1, 112, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1400x1050 @ 75,107 Hz, 82,392 kHz +hsync +vsync*/
+		NULL, 75, 1400, 1050, 9271, 120, 56, 13, 0, 112, 3,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 1400x1050 @ 60 Hz, ? kHz +hsync +vsync*/
+		NULL, 60, 1400, 1050, 9259, 128, 40, 12, 0, 112, 3,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 85 Hz, 70.24 kHz hsync */
+		NULL, 85, 1024, 768, 10111, 192, 32, 34, 14, 160, 6,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1152x864 @ 78 Hz, 70.8 kHz hsync */
+		NULL, 78, 1152, 864, 9090, 228, 88, 32, 0, 84, 12,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 70 Hz, 74.59 kHz hsync */
+		NULL, 70, 1280, 1024, 7905, 224, 32, 28, 8, 160, 8,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 60Hz, 75.00 kHz hsync */
+		NULL, 60, 1600, 1200, 6172, 304, 64, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 1152x864 @ 84 Hz, 76.0 kHz hsync */
+		NULL, 84, 1152, 864, 7407, 184, 312, 32, 0, 128, 12,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 74 Hz, 78.85 kHz hsync */
+		NULL, 74, 1280, 1024, 7407, 256, 32, 34, 3, 144, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 100Hz, 80.21 kHz hsync */
+		NULL, 100, 1024, 768, 8658, 192, 32, 21, 3, 192, 10,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 76 Hz, 81.13 kHz hsync */
+		NULL, 76, 1280, 1024, 7407, 248, 32, 34, 3, 104, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 70 Hz, 87.50 kHz hsync */
+		NULL, 70, 1600, 1200, 5291, 304, 64, 46, 1, 192, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1152x864 @ 100 Hz, 89.62 kHz hsync */
+		NULL, 100, 1152, 864, 7264, 224, 32, 17, 2, 128, 19,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 85 Hz, 91.15 kHz hsync */
+		NULL, 85, 1280, 1024, 6349, 224, 64, 44, 1, 160, 3,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 75 Hz, 93.75 kHz hsync */
+		NULL, 75, 1600, 1200, 4938, 304, 64, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 85 Hz, 105.77 kHz hsync */
+		NULL, 85, 1600, 1200, 4545, 272, 16, 37, 4, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 100 Hz, 107.16 kHz hsync */
+		NULL, 100, 1280, 1024, 5502, 256, 32, 26, 7, 128, 15,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1800x1440 @ 64Hz, 96.15 kHz hsync  */
+		NULL, 64, 1800, 1440, 4347, 304, 96, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 1800x1440 @ 70Hz, 104.52 kHz hsync  */
+		NULL, 70, 1800, 1440, 4000, 304, 96, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
+		FB_VMODE_NONINTERLACED
+	}, {
+		/* 512x384 @ 78 Hz, 31.50 kHz hsync */
+		NULL, 78, 512, 384, 49603, 48, 16, 16, 1, 64, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 512x384 @ 85 Hz, 34.38 kHz hsync */
+		NULL, 85, 512, 384, 45454, 48, 16, 16, 1, 64, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 320x200 @ 70 Hz, 31.5 kHz hsync, 8:5 aspect ratio */
+		NULL, 70, 320, 200, 79440, 16, 16, 20, 4, 48, 1,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 320x240 @ 60 Hz, 31.5 kHz hsync, 4:3 aspect ratio */
+		NULL, 60, 320, 240, 79440, 16, 16, 16, 5, 48, 1,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 320x240 @ 72 Hz, 36.5 kHz hsync */
+		NULL, 72, 320, 240, 63492, 16, 16, 16, 4, 48, 2,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 400x300 @ 56 Hz, 35.2 kHz hsync, 4:3 aspect ratio */
+		NULL, 56, 400, 300, 55555, 64, 16, 10, 1, 32, 1,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 400x300 @ 60 Hz, 37.8 kHz hsync */
+		NULL, 60, 400, 300, 50000, 48, 16, 11, 1, 64, 2,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 400x300 @ 72 Hz, 48.0 kHz hsync */
+		NULL, 72, 400, 300, 40000, 32, 24, 11, 19, 64, 3,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 480x300 @ 56 Hz, 35.2 kHz hsync, 8:5 aspect ratio */
+		NULL, 56, 480, 300, 46176, 80, 16, 10, 1, 40, 1,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 480x300 @ 60 Hz, 37.8 kHz hsync */
+		NULL, 60, 480, 300, 41858, 56, 16, 11, 1, 80, 2,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 480x300 @ 63 Hz, 39.6 kHz hsync */
+		NULL, 63, 480, 300, 40000, 56, 16, 11, 1, 80, 2,
+		0, FB_VMODE_DOUBLE
+	}, {
+		/* 480x300 @ 72 Hz, 48.0 kHz hsync */
+		NULL, 72, 480, 300, 33386, 40, 24, 11, 19, 80, 3,
+		0, FB_VMODE_DOUBLE
+	},
 };
 static const int nmodedb2 = sizeof(modedb2);
diff --git a/drivers/staging/sm750fb/readme b/drivers/staging/sm750fb/readme
index ab9af791653d..cfa45958b9d2 100644
--- a/drivers/staging/sm750fb/readme
+++ b/drivers/staging/sm750fb/readme
@@ -5,7 +5,7 @@ Introduction:
 	- 2D acceleration
 	- 16MB integrated video memory
 
-About the kernel module paramter of driver:
+About the kernel module parameter of driver:
 
 	Use 1280,8bpp index color and 60 hz mode:
 	insmod ./sm750fb.ko g_option="1280x1024-8@60"
@@ -20,16 +20,16 @@ About the kernel module paramter of driver:
 	and user can use con2fb to link fbX and ttyX
 
 	Notes:
-	1) if you build the driver with built-in method, the paramter
+	1) if you build the driver with built-in method, the parameter
 		you edited in the grub config file will be also the
-		same format as above modular method,but additionaly add
+		same format as above modular method,but additionally add
 		"video=sm750fb:"
 		ahead of parameters,so,it looks like:
 		video=sm750fb:noaccel,1280x1024@60,otherparam,etc...
 		it equal to modular method with below command:
 		insmod ./sm750fb.ko g_option="noaccel:1280x1024@60:otherparm:etc..."
 
-	2) if you put 800x600 into the paramter without bpp and
+	2) if you put 800x600 into the parameter without bpp and
 		refresh rate, kernel driver will defaulty use 16bpp and 60hz
 
 Important:
diff --git a/drivers/staging/sm750fb/sm750.c b/drivers/staging/sm750fb/sm750.c
index dbbb2f879a29..8e201f19cc0d 100644
--- a/drivers/staging/sm750fb/sm750.c
+++ b/drivers/staging/sm750fb/sm750.c
@@ -16,9 +16,6 @@
 #include<linux/vmalloc.h>
 #include<linux/pagemap.h>
 #include <linux/console.h>
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
 #include <asm/fb.h>
 #include "sm750.h"
 #include "sm750_hw.h"
@@ -47,9 +44,7 @@ typedef int (*PROC_SPEC_INITHW)(struct lynx_share*, struct pci_dev*);
 /* common var for all device */
 static int g_hwcursor = 1;
 static int g_noaccel;
-#ifdef CONFIG_MTRR
 static int g_nomtrr;
-#endif
 static const char *g_fbmode[] = {NULL, NULL};
 static const char *g_def_fbmode = "800x600-16@60";
 static char *g_settings = NULL;
@@ -93,9 +88,6 @@ static const struct fb_videomode lynx750_ext[] = {
 	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 	 FB_VMODE_NONINTERLACED},
 
-	{NULL, 60, 1360, 768, 11804, 208, 64, 23, 1, 144, 3,
-	 FB_SYNC_HOR_HIGH_ACT|FB_VMODE_NONINTERLACED},
-
 	/*	1360 x 768	[1.77083:1]	*/
 	{NULL,  60, 1360, 768, 11804, 208,  64, 23, 1, 144, 3,
 	 FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
@@ -207,7 +199,7 @@ static void lynxfb_ops_fillrect(struct fb_info *info,
 
 	/*
 	 * If not use spin_lock,system will die if user load driver
-	 * and immediatly unload driver frequently (dual)
+	 * and immediately unload driver frequently (dual)
 	 */
 	if (share->dual)
 		spin_lock(&share->slock);
@@ -239,7 +231,7 @@ static void lynxfb_ops_copyarea(struct fb_info *info,
 
 	/*
 	 * If not use spin_lock, system will die if user load driver
-	 * and immediatly unload driver frequently (dual)
+	 * and immediately unload driver frequently (dual)
 	 */
 	if (share->dual)
 		spin_lock(&share->slock);
@@ -279,11 +271,14 @@ static void lynxfb_ops_imageblit(struct fb_info *info,
 		}
 		goto _do_work;
 	}
+	/* TODO: Implement hardware acceleration for image->depth > 1 */
+	cfb_imageblit(info, image);
 	return;
+
 _do_work:
 	/*
 	 * If not use spin_lock, system will die if user load driver
-	 * and immediatly unload driver frequently (dual)
+	 * and immediately unload driver frequently (dual)
 	 */
 	if (share->dual)
 		spin_lock(&share->slock);
@@ -343,7 +338,7 @@ static int lynxfb_ops_set_par(struct fb_info *info)
 	line_length = var->xres_virtual * var->bits_per_pixel / 8;
 	line_length = PADDING(crtc->line_pad, line_length);
 	fix->line_length = line_length;
-	pr_err("fix->line_length = %d\n", fix->line_length);
+	pr_info("fix->line_length = %d\n", fix->line_length);
 
 	/* var->red,green,blue,transp are need to be set by driver
 	 * and these data should be set before setcolreg routine
@@ -479,7 +474,7 @@ static int lynxfb_resume(struct pci_dev *pdev)
 
 	ret = pci_set_power_state(pdev, PCI_D0);
 	if (ret) {
-		pr_err("error:%d occured in pci_set_power_state\n", ret);
+		pr_err("error:%d occurred in pci_set_power_state\n", ret);
 		return ret;
 	}
 
@@ -488,7 +483,7 @@ static int lynxfb_resume(struct pci_dev *pdev)
 		pci_restore_state(pdev);
 		ret = pci_enable_device(pdev);
 		if (ret) {
-			pr_err("error:%d occured in pci_enable_device\n", ret);
+			pr_err("error:%d occurred in pci_enable_device\n", ret);
 			return ret;
 		}
 		pci_set_master(pdev);
@@ -979,7 +974,7 @@ static int lynxfb_set_fbinfo(struct fb_info *info, int index)
 
 	ret = fb_alloc_cmap(&info->cmap, 256, 0);
 	if (ret < 0) {
-		pr_err("Could not allcate memory for cmap.\n");
+		pr_err("Could not allocate memory for cmap.\n");
 		goto exit;
 	}
 
@@ -1026,8 +1021,8 @@ static void sm750fb_setup(struct lynx_share *share, char *src)
 	}
 
 	while ((opt = strsep(&src, ":")) != NULL && *opt != 0) {
-		pr_err("opt=%s\n", opt);
-		pr_err("src=%s\n", src);
+		pr_info("opt=%s\n", opt);
+		pr_info("src=%s\n", src);
 
 		if (!strncmp(opt, "swap", strlen("swap")))
 			swap = 1;
@@ -1092,7 +1087,7 @@ NO_PARAM:
 }
 
 static int lynxfb_pci_probe(struct pci_dev *pdev,
-			    const struct pci_device_id * ent)
+			    const struct pci_device_id *ent)
 {
 	struct fb_info *info[] = {NULL, NULL};
 	struct lynx_share *share = NULL;
@@ -1126,11 +1121,8 @@ static int lynxfb_pci_probe(struct pci_dev *pdev,
 
 	pr_info("share->revid = %02x\n", share->revid);
 	share->pdev = pdev;
-#ifdef CONFIG_MTRR
 	share->mtrr_off = g_nomtrr;
 	share->mtrr.vram = 0;
-	share->mtrr.vram_added = 0;
-#endif
 	share->accel_off = g_noaccel;
 	share->dual = g_dualview;
 	spin_lock_init(&share->slock);
@@ -1158,22 +1150,9 @@ static int lynxfb_pci_probe(struct pci_dev *pdev,
 		goto err_map;
 	}
 
-#ifdef CONFIG_MTRR
-	if (!share->mtrr_off) {
-		pr_info("enable mtrr\n");
-		share->mtrr.vram = mtrr_add(share->vidmem_start,
-					    share->vidmem_size,
-					    MTRR_TYPE_WRCOMB, 1);
-
-		if (share->mtrr.vram < 0) {
-			/* don't block driver with the failure of MTRR */
-			pr_err("Unable to setup MTRR.\n");
-		} else {
-			share->mtrr.vram_added = 1;
-			pr_info("MTRR added succesfully\n");
-		}
-	}
-#endif
+	if (!share->mtrr_off)
+		share->mtrr.vram = arch_phys_wc_add(share->vidmem_start,
+						    share->vidmem_size);
 
 	memset_io(share->pvMem, 0, share->vidmem_size);
 
@@ -1274,12 +1253,7 @@ static void lynxfb_pci_remove(struct pci_dev *pdev)
 		/* release frame buffer */
 		framebuffer_release(info);
 	}
-#ifdef CONFIG_MTRR
-	if (share->mtrr.vram_added)
-		mtrr_del(share->mtrr.vram,
-			 share->vidmem_start,
-			 share->vidmem_size);
-#endif
+	arch_phys_wc_del(share->mtrr.vram);
 
 	iounmap(share->pvReg);
 	iounmap(share->pvMem);
@@ -1321,10 +1295,8 @@ static int __init lynxfb_setup(char *options)
 		/* options that mean for any lynx chips are configured here */
 		if (!strncmp(opt, "noaccel", strlen("noaccel")))
 			g_noaccel = 1;
-#ifdef CONFIG_MTRR
 		else if (!strncmp(opt, "nomtrr", strlen("nomtrr")))
 			g_nomtrr = 1;
-#endif
 		else if (!strncmp(opt, "dual", strlen("dual")))
 			g_dualview = 1;
 		else {
diff --git a/drivers/staging/sm750fb/sm750.h b/drivers/staging/sm750fb/sm750.h
index 0847d2bd95c8..cc80580bc823 100644
--- a/drivers/staging/sm750fb/sm750.h
+++ b/drivers/staging/sm750fb/sm750.h
@@ -7,12 +7,10 @@
 /* please use revision id to distinguish sm750le and sm750*/
 #define SPC_SM750 	0
 
-//#define SPC_SM750LE 8
-
 #define MB(x) ((x)<<20)
 #define MHZ(x) ((x) * 1000000)
 /* align should be 2,4,8,16 */
-#define PADDING(align,data) (((data)+(align)-1)&(~((align) -1)))
+#define PADDING(align, data) (((data)+(align)-1)&(~((align) -1)))
 extern int smi_indent;
 
 
@@ -27,15 +25,16 @@ struct lynx_accel{
 
 	int (*de_wait)(void);/* see if hardware ready to work */
 
-	int (*de_fillrect)(struct lynx_accel *,u32,u32,u32,
-							u32,u32,u32,u32,u32,u32);
+	int (*de_fillrect)(struct lynx_accel *, u32, u32, u32, u32,
+						u32, u32, u32, u32, u32);
 
-	int (*de_copyarea)(struct lynx_accel *,u32,u32,u32,u32,
-						u32,u32,u32,u32,
-						u32,u32,u32,u32);
+	int (*de_copyarea)(struct lynx_accel *, u32, u32, u32, u32,
+						u32, u32, u32, u32,
+						u32, u32, u32, u32);
 
-	int (*de_imageblit)(struct lynx_accel *,const char *,u32,u32,u32,
-						u32,u32,u32,u32,u32,u32,u32,u32,u32);
+	int (*de_imageblit)(struct lynx_accel *, const char *, u32, u32, u32, u32,
+							       u32, u32, u32, u32,
+							       u32, u32, u32, u32);
 
 };
 
@@ -46,25 +45,22 @@ struct lynx_share{
 	/* common members */
 	u16 devid;
 	u8 revid;
-	struct pci_dev * pdev;
-	struct fb_info * fbinfo[2];
+	struct pci_dev *pdev;
+	struct fb_info *fbinfo[2];
 	struct lynx_accel accel;
 	int accel_off;
 	int dual;
-#ifdef CONFIG_MTRR
 		int mtrr_off;
 		struct{
 			int vram;
-			int vram_added;
 		}mtrr;
-#endif
 	/* all smi graphic adaptor got below attributes */
 	unsigned long vidmem_start;
 	unsigned long vidreg_start;
 	__u32 vidmem_size;
 	__u32 vidreg_size;
-	void __iomem * pvReg;
-	unsigned char __iomem * pvMem;
+	void __iomem *pvReg;
+	unsigned char __iomem *pvMem;
 	/* locks*/
 	spinlock_t slock;
 	/* function pointers */
@@ -81,26 +77,26 @@ struct lynx_cursor{
 	int maxW;
 	int maxH;
 	/* base virtual address and offset  of cursor image */
-	char __iomem * vstart;
+	char __iomem *vstart;
 	int offset;
 	/* mmio addr of hw cursor */
 	volatile char __iomem * mmio;
 	/* the lynx_share of this adaptor */
-	struct lynx_share * share;
+	struct lynx_share *share;
 	/* proc_routines */
 	void (*enable)(struct lynx_cursor *);
 	void (*disable)(struct lynx_cursor *);
-	void (*setSize)(struct lynx_cursor *,int,int);
-	void (*setPos)(struct lynx_cursor *,int,int);
-	void (*setColor)(struct lynx_cursor *,u32,u32);
-	void (*setData)(struct lynx_cursor *,u16,const u8*,const u8*);
+	void (*setSize)(struct lynx_cursor *, int, int);
+	void (*setPos)(struct lynx_cursor *, int, int);
+	void (*setColor)(struct lynx_cursor *, u32, u32);
+	void (*setData)(struct lynx_cursor *, u16, const u8*, const u8*);
 };
 
 struct lynxfb_crtc{
-	unsigned char __iomem * vCursor;//virtual address of cursor
-	unsigned char __iomem * vScreen;//virtual address of on_screen
-	int oCursor;//cursor address offset in vidmem
-	int oScreen;//onscreen address offset in vidmem
+	unsigned char __iomem *vCursor; /* virtual address of cursor */
+	unsigned char __iomem *vScreen; /* virtual address of on_screen */
+	int oCursor; /* cursor address offset in vidmem */
+	int oScreen; /* onscreen address offset in vidmem */
 	int channel;/* which channel this crtc stands for*/
 	resource_size_t vidmem_size;/* this view's video memory max size */
 
@@ -110,14 +106,14 @@ struct lynxfb_crtc{
 	u16 ypanstep;
 	u16 ywrapstep;
 
-	void * priv;
+	void *priv;
 
 	int(*proc_setMode)(struct lynxfb_crtc*,
 						struct fb_var_screeninfo*,
 						struct fb_fix_screeninfo*);
 
-	int(*proc_checkMode)(struct lynxfb_crtc*,struct fb_var_screeninfo*);
-	int(*proc_setColReg)(struct lynxfb_crtc*,ushort,ushort,ushort,ushort);
+	int(*proc_checkMode)(struct lynxfb_crtc*, struct fb_var_screeninfo*);
+	int(*proc_setColReg)(struct lynxfb_crtc*, ushort, ushort, ushort, ushort);
 	void (*clear)(struct lynxfb_crtc*);
         /* pan display */
 	int (*proc_panDisplay)(struct lynxfb_crtc *,
@@ -136,20 +132,20 @@ struct lynxfb_output{
 		paths=3:means output for both panel and crt paths
 	*/
 
-	int * channel;
+	int *channel;
 	/* 	which channel these outputs linked with,for sm750:
 		*channel=0 means primary channel
 		*channel=1 means secondary channel
 		output->channel ==> &crtc->channel
 	*/
-	void * priv;
+	void *priv;
 
 	int(*proc_setMode)(struct lynxfb_output*,
 						struct fb_var_screeninfo*,
 						struct fb_fix_screeninfo*);
 
-	int(*proc_checkMode)(struct lynxfb_output*,struct fb_var_screeninfo*);
-	int(*proc_setBLANK)(struct lynxfb_output*,int);
+	int(*proc_checkMode)(struct lynxfb_output*, struct fb_var_screeninfo*);
+	int(*proc_setBLANK)(struct lynxfb_output*, int);
 	void  (*clear)(struct lynxfb_output*);
 };
 
@@ -159,8 +155,8 @@ struct lynxfb_par{
 	unsigned int pseudo_palette[256];
 	struct lynxfb_crtc crtc;
 	struct lynxfb_output output;
-	struct fb_info * info;
-	struct lynx_share * share;
+	struct fb_info *info;
+	struct lynx_share *share;
 };
 
 #ifndef offsetof
@@ -171,7 +167,7 @@ struct lynxfb_par{
 #define PS_TO_HZ(ps)	\
 			({ 	\
 			unsigned long long hz = 1000*1000*1000*1000ULL;	\
-			do_div(hz,ps);	\
+			do_div(hz, ps);	\
 			(unsigned long)hz;})
 
 static inline unsigned long ps_to_hz(unsigned int psvalue)
diff --git a/drivers/staging/sm750fb/sm750_accel.c b/drivers/staging/sm750fb/sm750_accel.c
index c5a372690737..6eee4cd582d1 100644
--- a/drivers/staging/sm750fb/sm750_accel.c
+++ b/drivers/staging/sm750fb/sm750_accel.c
@@ -18,81 +18,81 @@
 #include "sm750.h"
 #include "sm750_accel.h"
 #include "sm750_help.h"
-static inline void write_dpr(struct lynx_accel * accel,int offset,u32 regValue)
+static inline void write_dpr(struct lynx_accel *accel, int offset, u32 regValue)
 {
-	writel(regValue,accel->dprBase + offset);
+	writel(regValue, accel->dprBase + offset);
 }
 
-static inline u32 read_dpr(struct lynx_accel * accel,int offset)
+static inline u32 read_dpr(struct lynx_accel *accel, int offset)
 {
 	return readl(accel->dprBase + offset);
 }
 
-static inline void write_dpPort(struct lynx_accel * accel,u32 data)
+static inline void write_dpPort(struct lynx_accel *accel, u32 data)
 {
-	writel(data,accel->dpPortBase);
+	writel(data, accel->dpPortBase);
 }
 
-void hw_de_init(struct lynx_accel * accel)
+void hw_de_init(struct lynx_accel *accel)
 {
 	/* setup 2d engine registers */
-	u32 reg,clr;
+	u32 reg, clr;
 	
-	write_dpr(accel,DE_MASKS,0xFFFFFFFF);
+	write_dpr(accel, DE_MASKS, 0xFFFFFFFF);
 
 	/* dpr1c */
-	reg = FIELD_SET(0,DE_STRETCH_FORMAT,PATTERN_XY,NORMAL)|
-		FIELD_VALUE(0,DE_STRETCH_FORMAT,PATTERN_Y,0)|
-		FIELD_VALUE(0,DE_STRETCH_FORMAT,PATTERN_X,0)|
-		FIELD_SET(0,DE_STRETCH_FORMAT,ADDRESSING,XY)|
-		FIELD_VALUE(0,DE_STRETCH_FORMAT,SOURCE_HEIGHT,3);
-
-	clr = FIELD_CLEAR(DE_STRETCH_FORMAT,PATTERN_XY)&
-		FIELD_CLEAR(DE_STRETCH_FORMAT,PATTERN_Y)&
-		FIELD_CLEAR(DE_STRETCH_FORMAT,PATTERN_X)&
-		FIELD_CLEAR(DE_STRETCH_FORMAT,ADDRESSING)&
-		FIELD_CLEAR(DE_STRETCH_FORMAT,SOURCE_HEIGHT);
+	reg = FIELD_SET(0, DE_STRETCH_FORMAT, PATTERN_XY, NORMAL)|
+		FIELD_VALUE(0, DE_STRETCH_FORMAT, PATTERN_Y, 0)|
+		FIELD_VALUE(0, DE_STRETCH_FORMAT, PATTERN_X, 0)|
+		FIELD_SET(0, DE_STRETCH_FORMAT, ADDRESSING, XY)|
+		FIELD_VALUE(0, DE_STRETCH_FORMAT, SOURCE_HEIGHT, 3);
+
+	clr = FIELD_CLEAR(DE_STRETCH_FORMAT, PATTERN_XY)&
+		FIELD_CLEAR(DE_STRETCH_FORMAT, PATTERN_Y)&
+		FIELD_CLEAR(DE_STRETCH_FORMAT, PATTERN_X)&
+		FIELD_CLEAR(DE_STRETCH_FORMAT, ADDRESSING)&
+		FIELD_CLEAR(DE_STRETCH_FORMAT, SOURCE_HEIGHT);
 
 	/* DE_STRETCH bpp format need be initilized in setMode routine */
-	write_dpr(accel,DE_STRETCH_FORMAT,(read_dpr(accel,DE_STRETCH_FORMAT) & clr) | reg);
+	write_dpr(accel, DE_STRETCH_FORMAT, (read_dpr(accel, DE_STRETCH_FORMAT) & clr) | reg);
 
 	/* disable clipping and transparent */
-	write_dpr(accel,DE_CLIP_TL,0);//dpr2c
-	write_dpr(accel,DE_CLIP_BR,0);//dpr30
+	write_dpr(accel, DE_CLIP_TL, 0); /* dpr2c */
+	write_dpr(accel, DE_CLIP_BR, 0); /* dpr30 */
 
-	write_dpr(accel,DE_COLOR_COMPARE_MASK,0);//dpr24
-	write_dpr(accel,DE_COLOR_COMPARE,0);
+	write_dpr(accel, DE_COLOR_COMPARE_MASK, 0); /* dpr24 */
+	write_dpr(accel, DE_COLOR_COMPARE, 0);
 
-	reg = FIELD_SET(0,DE_CONTROL,TRANSPARENCY,DISABLE)|
-		FIELD_SET(0,DE_CONTROL,TRANSPARENCY_MATCH,OPAQUE)|
-		FIELD_SET(0,DE_CONTROL,TRANSPARENCY_SELECT,SOURCE);
+	reg = FIELD_SET(0, DE_CONTROL, TRANSPARENCY, DISABLE)|
+		FIELD_SET(0, DE_CONTROL, TRANSPARENCY_MATCH, OPAQUE)|
+		FIELD_SET(0, DE_CONTROL, TRANSPARENCY_SELECT, SOURCE);
 
-	clr = FIELD_CLEAR(DE_CONTROL,TRANSPARENCY)&
-		FIELD_CLEAR(DE_CONTROL,TRANSPARENCY_MATCH)&
-		FIELD_CLEAR(DE_CONTROL,TRANSPARENCY_SELECT);
+	clr = FIELD_CLEAR(DE_CONTROL, TRANSPARENCY)&
+		FIELD_CLEAR(DE_CONTROL, TRANSPARENCY_MATCH)&
+		FIELD_CLEAR(DE_CONTROL, TRANSPARENCY_SELECT);
 
 	/* dpr0c */
-	write_dpr(accel,DE_CONTROL,(read_dpr(accel,DE_CONTROL)&clr)|reg);
+	write_dpr(accel, DE_CONTROL, (read_dpr(accel, DE_CONTROL)&clr)|reg);
 }
 
 /* set2dformat only be called from setmode functions
  * but if you need dual framebuffer driver,need call set2dformat
  * every time you use 2d function */
 
-void hw_set2dformat(struct lynx_accel * accel,int fmt)
+void hw_set2dformat(struct lynx_accel *accel, int fmt)
 {
 	u32 reg;
 	
 	/* fmt=0,1,2 for 8,16,32,bpp on sm718/750/502 */
-	reg = read_dpr(accel,DE_STRETCH_FORMAT);
-	reg = FIELD_VALUE(reg,DE_STRETCH_FORMAT,PIXEL_FORMAT,fmt);
-	write_dpr(accel,DE_STRETCH_FORMAT,reg);
+	reg = read_dpr(accel, DE_STRETCH_FORMAT);
+	reg = FIELD_VALUE(reg, DE_STRETCH_FORMAT, PIXEL_FORMAT, fmt);
+	write_dpr(accel, DE_STRETCH_FORMAT, reg);
 }
 
-int hw_fillrect(struct lynx_accel * accel,
-				u32 base,u32 pitch,u32 Bpp,
-				u32 x,u32 y,u32 width,u32 height,
-				u32 color,u32 rop)
+int hw_fillrect(struct lynx_accel *accel,
+				u32 base, u32 pitch, u32 Bpp,
+				u32 x, u32 y, u32 width, u32 height,
+				u32 color, u32 rop)
 {
 	u32 deCtrl;
 
@@ -100,44 +100,44 @@ int hw_fillrect(struct lynx_accel * accel,
 	{
 		/* int time wait and always busy,seems hardware
 		 * got something error */
-		pr_debug("%s:De engine always bussy\n",__func__);
+		pr_debug("%s:De engine always bussy\n", __func__);
 		return -1;
 	}
 
-	write_dpr(accel,DE_WINDOW_DESTINATION_BASE,base);//dpr40
-	write_dpr(accel,DE_PITCH,
-			FIELD_VALUE(0,DE_PITCH,DESTINATION,pitch/Bpp)|
-			FIELD_VALUE(0,DE_PITCH,SOURCE,pitch/Bpp));//dpr10
+	write_dpr(accel, DE_WINDOW_DESTINATION_BASE, base); /* dpr40 */
+	write_dpr(accel, DE_PITCH,
+			FIELD_VALUE(0, DE_PITCH, DESTINATION, pitch/Bpp)|
+			FIELD_VALUE(0, DE_PITCH, SOURCE, pitch/Bpp)); /* dpr10 */
 
-	write_dpr(accel,DE_WINDOW_WIDTH,
-			FIELD_VALUE(0,DE_WINDOW_WIDTH,DESTINATION,pitch/Bpp)|
-			FIELD_VALUE(0,DE_WINDOW_WIDTH,SOURCE,pitch/Bpp));//dpr44
+	write_dpr(accel, DE_WINDOW_WIDTH,
+			FIELD_VALUE(0, DE_WINDOW_WIDTH, DESTINATION, pitch/Bpp)|
+			FIELD_VALUE(0, DE_WINDOW_WIDTH, SOURCE, pitch/Bpp)); /* dpr44 */
 
-	write_dpr(accel,DE_FOREGROUND,color);//DPR14
+	write_dpr(accel, DE_FOREGROUND, color); /* DPR14 */
 
-	write_dpr(accel,DE_DESTINATION,
-			FIELD_SET(0,DE_DESTINATION,WRAP,DISABLE)|
-			FIELD_VALUE(0,DE_DESTINATION,X,x)|
-			FIELD_VALUE(0,DE_DESTINATION,Y,y));//dpr4
+	write_dpr(accel, DE_DESTINATION,
+			FIELD_SET(0, DE_DESTINATION, WRAP, DISABLE)|
+			FIELD_VALUE(0, DE_DESTINATION, X, x)|
+			FIELD_VALUE(0, DE_DESTINATION, Y, y)); /* dpr4 */
 
-	write_dpr(accel,DE_DIMENSION,
-			FIELD_VALUE(0,DE_DIMENSION,X,width)|
-			FIELD_VALUE(0,DE_DIMENSION,Y_ET,height));//dpr8
+	write_dpr(accel, DE_DIMENSION,
+			FIELD_VALUE(0, DE_DIMENSION, X, width)|
+			FIELD_VALUE(0, DE_DIMENSION, Y_ET, height)); /* dpr8 */
 
 	deCtrl =
-		FIELD_SET(0,DE_CONTROL,STATUS,START)|
-		FIELD_SET(0,DE_CONTROL,DIRECTION,LEFT_TO_RIGHT)|
-		FIELD_SET(0,DE_CONTROL,LAST_PIXEL,ON)|
-		FIELD_SET(0,DE_CONTROL,COMMAND,RECTANGLE_FILL)|
-		FIELD_SET(0,DE_CONTROL,ROP_SELECT,ROP2)|
-		FIELD_VALUE(0,DE_CONTROL,ROP,rop);//dpr0xc
-
-	write_dpr(accel,DE_CONTROL,deCtrl);
+		FIELD_SET(0, DE_CONTROL, STATUS, START)|
+		FIELD_SET(0, DE_CONTROL, DIRECTION, LEFT_TO_RIGHT)|
+		FIELD_SET(0, DE_CONTROL, LAST_PIXEL, ON)|
+		FIELD_SET(0, DE_CONTROL, COMMAND, RECTANGLE_FILL)|
+		FIELD_SET(0, DE_CONTROL, ROP_SELECT, ROP2)|
+		FIELD_VALUE(0, DE_CONTROL, ROP, rop); /* dpr0xc */
+
+	write_dpr(accel, DE_CONTROL, deCtrl);
 	return 0;
 }
 
 int hw_copyarea(
-struct lynx_accel * accel,
+struct lynx_accel *accel,
 unsigned int sBase,  /* Address of source: offset in frame buffer */
 unsigned int sPitch, /* Pitch value of source surface in BYTE */
 unsigned int sx,
@@ -236,40 +236,40 @@ unsigned int rop2)   /* ROP value */
     /* 2D Source Base.
        It is an address offset (128 bit aligned) from the beginning of frame buffer.
     */
-    write_dpr(accel,DE_WINDOW_SOURCE_BASE, sBase);//dpr40
+    write_dpr(accel, DE_WINDOW_SOURCE_BASE, sBase); /* dpr40 */
 
     /* 2D Destination Base.
        It is an address offset (128 bit aligned) from the beginning of frame buffer.
     */
-    write_dpr(accel,DE_WINDOW_DESTINATION_BASE, dBase);//dpr44
+    write_dpr(accel, DE_WINDOW_DESTINATION_BASE, dBase); /* dpr44 */
 
 #if 0
     /* Program pitch (distance between the 1st points of two adjacent lines).
        Note that input pitch is BYTE value, but the 2D Pitch register uses
-       pixel values. Need Byte to pixel convertion.
+       pixel values. Need Byte to pixel conversion.
     */
 	if(Bpp == 3){
 			sx *= 3;
 			dx *= 3;
 			width *= 3;
-		write_dpr(accel,DE_PITCH,
+		write_dpr(accel, DE_PITCH,
 				FIELD_VALUE(0, DE_PITCH, DESTINATION, dPitch) |
-				FIELD_VALUE(0, DE_PITCH, SOURCE,      sPitch));//dpr10
+				FIELD_VALUE(0, DE_PITCH, SOURCE,      sPitch)); /* dpr10 */
 	}
 	else
 #endif
 	{
-		write_dpr(accel,DE_PITCH,
+		write_dpr(accel, DE_PITCH,
 				FIELD_VALUE(0, DE_PITCH, DESTINATION, (dPitch/Bpp)) |
-				FIELD_VALUE(0, DE_PITCH, SOURCE,      (sPitch/Bpp)));//dpr10
+				FIELD_VALUE(0, DE_PITCH, SOURCE,      (sPitch/Bpp))); /* dpr10 */
 	}
 
     /* Screen Window width in Pixels.
        2D engine uses this value to calculate the linear address in frame buffer for a given point.
     */
-    write_dpr(accel,DE_WINDOW_WIDTH,
+    write_dpr(accel, DE_WINDOW_WIDTH,
         FIELD_VALUE(0, DE_WINDOW_WIDTH, DESTINATION, (dPitch/Bpp)) |
-        FIELD_VALUE(0, DE_WINDOW_WIDTH, SOURCE,      (sPitch/Bpp)));//dpr3c
+        FIELD_VALUE(0, DE_WINDOW_WIDTH, SOURCE,      (sPitch/Bpp))); /* dpr3c */
 
 	if (accel->de_wait() != 0){
 		return -1;
@@ -277,17 +277,17 @@ unsigned int rop2)   /* ROP value */
 
     {
 
-        write_dpr(accel,DE_SOURCE,
+        write_dpr(accel, DE_SOURCE,
             FIELD_SET  (0, DE_SOURCE, WRAP, DISABLE) |
             FIELD_VALUE(0, DE_SOURCE, X_K1, sx)   |
-            FIELD_VALUE(0, DE_SOURCE, Y_K2, sy));//dpr0
-        write_dpr(accel,DE_DESTINATION,
+            FIELD_VALUE(0, DE_SOURCE, Y_K2, sy)); /* dpr0 */
+        write_dpr(accel, DE_DESTINATION,
             FIELD_SET  (0, DE_DESTINATION, WRAP, DISABLE) |
             FIELD_VALUE(0, DE_DESTINATION, X,    dx)  |
-            FIELD_VALUE(0, DE_DESTINATION, Y,    dy));//dpr04
-        write_dpr(accel,DE_DIMENSION,
+            FIELD_VALUE(0, DE_DESTINATION, Y,    dy)); /* dpr04 */
+        write_dpr(accel, DE_DIMENSION,
             FIELD_VALUE(0, DE_DIMENSION, X,    width) |
-            FIELD_VALUE(0, DE_DIMENSION, Y_ET, height));//dpr08
+            FIELD_VALUE(0, DE_DIMENSION, Y_ET, height)); /* dpr08 */
 
         de_ctrl =
             FIELD_VALUE(0, DE_CONTROL, ROP, rop2) |
@@ -297,17 +297,17 @@ unsigned int rop2)   /* ROP value */
             FIELD_SET(0, DE_CONTROL, DIRECTION, RIGHT_TO_LEFT)
             : FIELD_SET(0, DE_CONTROL, DIRECTION, LEFT_TO_RIGHT)) |
             FIELD_SET(0, DE_CONTROL, STATUS, START);
-		write_dpr(accel,DE_CONTROL,de_ctrl);//dpr0c
+		write_dpr(accel, DE_CONTROL, de_ctrl); /* dpr0c */
     }
 
     return 0;
 }
 
-static unsigned int deGetTransparency(struct lynx_accel * accel)
+static unsigned int deGetTransparency(struct lynx_accel *accel)
 {
     unsigned int de_ctrl;
 
-    de_ctrl = read_dpr(accel,DE_CONTROL);
+    de_ctrl = read_dpr(accel, DE_CONTROL);
 
     de_ctrl &=
         FIELD_MASK(DE_CONTROL_TRANSPARENCY_MATCH) |
@@ -346,65 +346,64 @@ int hw_imageblit(struct lynx_accel *accel,
 
 	if(accel->de_wait() != 0)
     {
-//		inf_msg("*** ImageBlit return -1 ***\n");
         return -1;
     }
 
     /* 2D Source Base.
        Use 0 for HOST Blt.
     */
-    write_dpr(accel,DE_WINDOW_SOURCE_BASE, 0);
+    write_dpr(accel, DE_WINDOW_SOURCE_BASE, 0);
 
     /* 2D Destination Base.
        It is an address offset (128 bit aligned) from the beginning of frame buffer.
     */
-    write_dpr(accel,DE_WINDOW_DESTINATION_BASE, dBase);
+    write_dpr(accel, DE_WINDOW_DESTINATION_BASE, dBase);
 #if 0
     /* Program pitch (distance between the 1st points of two adjacent lines).
        Note that input pitch is BYTE value, but the 2D Pitch register uses
-       pixel values. Need Byte to pixel convertion.
+       pixel values. Need Byte to pixel conversion.
     */
 	if(bytePerPixel == 3 ){
 		dx *= 3;
 		width *= 3;
 		startBit *= 3;
-		write_dpr(accel,DE_PITCH,
+		write_dpr(accel, DE_PITCH,
 				FIELD_VALUE(0, DE_PITCH, DESTINATION, dPitch) |
-				FIELD_VALUE(0, DE_PITCH, SOURCE,      dPitch));//dpr10
+				FIELD_VALUE(0, DE_PITCH, SOURCE,      dPitch)); /* dpr10 */
 
 	}
 	else
 #endif
 	{
-		write_dpr(accel,DE_PITCH,
+		write_dpr(accel, DE_PITCH,
 				FIELD_VALUE(0, DE_PITCH, DESTINATION, dPitch/bytePerPixel) |
-				FIELD_VALUE(0, DE_PITCH, SOURCE,      dPitch/bytePerPixel));//dpr10
+				FIELD_VALUE(0, DE_PITCH, SOURCE,      dPitch/bytePerPixel)); /* dpr10 */
 	}
 
     /* Screen Window width in Pixels.
        2D engine uses this value to calculate the linear address in frame buffer for a given point.
     */
-    write_dpr(accel,DE_WINDOW_WIDTH,
+    write_dpr(accel, DE_WINDOW_WIDTH,
         FIELD_VALUE(0, DE_WINDOW_WIDTH, DESTINATION, (dPitch/bytePerPixel)) |
         FIELD_VALUE(0, DE_WINDOW_WIDTH, SOURCE,      (dPitch/bytePerPixel)));
 
     /* Note: For 2D Source in Host Write, only X_K1_MONO field is needed, and Y_K2 field is not used.
              For mono bitmap, use startBit for X_K1. */
-    write_dpr(accel,DE_SOURCE,
+    write_dpr(accel, DE_SOURCE,
         FIELD_SET  (0, DE_SOURCE, WRAP, DISABLE)       |
-        FIELD_VALUE(0, DE_SOURCE, X_K1_MONO, startBit));//dpr00
+        FIELD_VALUE(0, DE_SOURCE, X_K1_MONO, startBit)); /* dpr00 */
 
-    write_dpr(accel,DE_DESTINATION,
+    write_dpr(accel, DE_DESTINATION,
         FIELD_SET  (0, DE_DESTINATION, WRAP, DISABLE) |
         FIELD_VALUE(0, DE_DESTINATION, X,    dx)    |
-        FIELD_VALUE(0, DE_DESTINATION, Y,    dy));//dpr04
+        FIELD_VALUE(0, DE_DESTINATION, Y,    dy)); /* dpr04 */
 
-    write_dpr(accel,DE_DIMENSION,
+    write_dpr(accel, DE_DIMENSION,
         FIELD_VALUE(0, DE_DIMENSION, X,    width) |
-        FIELD_VALUE(0, DE_DIMENSION, Y_ET, height));//dpr08
+        FIELD_VALUE(0, DE_DIMENSION, Y_ET, height)); /* dpr08 */
 
-    write_dpr(accel,DE_FOREGROUND, fColor);
-    write_dpr(accel,DE_BACKGROUND, bColor);
+    write_dpr(accel, DE_FOREGROUND, fColor);
+    write_dpr(accel, DE_BACKGROUND, bColor);
 
 	de_ctrl = FIELD_VALUE(0, DE_CONTROL, ROP, rop2)         |
 		FIELD_SET(0, DE_CONTROL, ROP_SELECT, ROP2)    |
@@ -412,7 +411,7 @@ int hw_imageblit(struct lynx_accel *accel,
 		FIELD_SET(0, DE_CONTROL, HOST, MONO)          |
 		FIELD_SET(0, DE_CONTROL, STATUS, START);
 
-	write_dpr(accel,DE_CONTROL, de_ctrl | deGetTransparency(accel));
+	write_dpr(accel, DE_CONTROL, de_ctrl | deGetTransparency(accel));
 
     /* Write MONO data (line by line) to 2D Engine data port */
     for (i=0; i<height; i++)
diff --git a/drivers/staging/sm750fb/sm750_accel.h b/drivers/staging/sm750fb/sm750_accel.h
index 3ee0bd89257b..d3d256c21206 100644
--- a/drivers/staging/sm750fb/sm750_accel.h
+++ b/drivers/staging/sm750fb/sm750_accel.h
@@ -113,7 +113,7 @@
 #define DE_CONTROL_TRANSPARENCY_ENABLE                  1
 #define DE_CONTROL_ROP                                  7:0
 
-// Pseudo fields.
+/* Pseudo fields. */
 
 #define DE_CONTROL_SHORT_STROKE_DIR                     27:24
 #define DE_CONTROL_SHORT_STROKE_DIR_225                 0
@@ -234,17 +234,17 @@
 #define BOTTOM_TO_TOP 1
 #define RIGHT_TO_LEFT 1
 
-void hw_set2dformat(struct lynx_accel * accel,int fmt);
+void hw_set2dformat(struct lynx_accel *accel, int fmt);
 
-void hw_de_init(struct lynx_accel * accel);
+void hw_de_init(struct lynx_accel *accel);
 
-int hw_fillrect(struct lynx_accel * accel,
-				u32 base,u32 pitch,u32 Bpp,
-				u32 x,u32 y,u32 width,u32 height,
-				u32 color,u32 rop);
+int hw_fillrect(struct lynx_accel *accel,
+				u32 base, u32 pitch, u32 Bpp,
+				u32 x, u32 y, u32 width, u32 height,
+				u32 color, u32 rop);
 
 int hw_copyarea(
-struct lynx_accel * accel,
+struct lynx_accel *accel,
 unsigned int sBase,  /* Address of source: offset in frame buffer */
 unsigned int sPitch, /* Pitch value of source surface in BYTE */
 unsigned int sx,
diff --git a/drivers/staging/sm750fb/sm750_cursor.c b/drivers/staging/sm750fb/sm750_cursor.c
index 68d5cbc3e513..405e24b6768f 100644
--- a/drivers/staging/sm750fb/sm750_cursor.c
+++ b/drivers/staging/sm750fb/sm750_cursor.c
@@ -23,8 +23,8 @@
 #define PEEK32(addr) \
 readl(cursor->mmio + (addr))
 
-#define POKE32(addr,data) \
-writel((data),cursor->mmio + (addr))
+#define POKE32(addr, data) \
+writel((data), cursor->mmio + (addr))
 
 /* cursor control for voyager and 718/750*/
 #define HWC_ADDRESS                         0x0
@@ -58,45 +58,45 @@ writel((data),cursor->mmio + (addr))
 
 
 /* hw_cursor_xxx works for voyager,718 and 750 */
-void hw_cursor_enable(struct lynx_cursor * cursor)
+void hw_cursor_enable(struct lynx_cursor *cursor)
 {
 	u32 reg;
-	reg = FIELD_VALUE(0,HWC_ADDRESS,ADDRESS,cursor->offset)|
-			FIELD_SET(0,HWC_ADDRESS,EXT,LOCAL)|
-			FIELD_SET(0,HWC_ADDRESS,ENABLE,ENABLE);
-	POKE32(HWC_ADDRESS,reg);
+	reg = FIELD_VALUE(0, HWC_ADDRESS, ADDRESS, cursor->offset)|
+			FIELD_SET(0, HWC_ADDRESS, EXT, LOCAL)|
+			FIELD_SET(0, HWC_ADDRESS, ENABLE, ENABLE);
+	POKE32(HWC_ADDRESS, reg);
 }
-void hw_cursor_disable(struct lynx_cursor * cursor)
+void hw_cursor_disable(struct lynx_cursor *cursor)
 {
-	POKE32(HWC_ADDRESS,0);
+	POKE32(HWC_ADDRESS, 0);
 }
 
-void hw_cursor_setSize(struct lynx_cursor * cursor,
-						int w,int h)
+void hw_cursor_setSize(struct lynx_cursor *cursor,
+						int w, int h)
 {
 	cursor->w = w;
 	cursor->h = h;
 }
-void hw_cursor_setPos(struct lynx_cursor * cursor,
-						int x,int y)
+void hw_cursor_setPos(struct lynx_cursor *cursor,
+						int x, int y)
 {
 	u32 reg;
-	reg = FIELD_VALUE(0,HWC_LOCATION,Y,y)|
-			FIELD_VALUE(0,HWC_LOCATION,X,x);
-	POKE32(HWC_LOCATION,reg);
+	reg = FIELD_VALUE(0, HWC_LOCATION, Y, y)|
+			FIELD_VALUE(0, HWC_LOCATION, X, x);
+	POKE32(HWC_LOCATION, reg);
 }
-void hw_cursor_setColor(struct lynx_cursor * cursor,
-						u32 fg,u32 bg)
+void hw_cursor_setColor(struct lynx_cursor *cursor,
+						u32 fg, u32 bg)
 {
-	POKE32(HWC_COLOR_12,(fg<<16)|(bg&0xffff));
-	POKE32(HWC_COLOR_3,0xffe0);
+	POKE32(HWC_COLOR_12, (fg<<16)|(bg&0xffff));
+	POKE32(HWC_COLOR_3, 0xffe0);
 }
 
-void hw_cursor_setData(struct lynx_cursor * cursor,
-			u16 rop,const u8* pcol,const u8* pmsk)
+void hw_cursor_setData(struct lynx_cursor *cursor,
+			u16 rop, const u8* pcol, const u8* pmsk)
 {
-	int i,j,count,pitch,offset;
-	u8 color,mask,opr;
+	int i, j, count, pitch, offset;
+	u8 color, mask, opr;
 	u16 data;
 	void __iomem *pbuffer, *pstart;
 
@@ -141,10 +141,10 @@ void hw_cursor_setData(struct lynx_cursor * cursor,
 		{
 
 			if(opr & (0x80 >> j))
-			{	//use fg color,id = 2
+			{	/* use fg color,id = 2 */
 				data |= 2 << (j*2);
 			}else{
-				//use bg color,id = 1
+				/* use bg color,id = 1 */
 				data |= 1 << (j*2);
 			}
 		}
@@ -183,10 +183,10 @@ void hw_cursor_setData(struct lynx_cursor * cursor,
 }
 
 
-void hw_cursor_setData2(struct lynx_cursor * cursor,
-			u16 rop,const u8* pcol,const u8* pmsk)
+void hw_cursor_setData2(struct lynx_cursor *cursor,
+			u16 rop, const u8* pcol, const u8* pmsk)
 {
-	int i,j,count,pitch,offset;
+	int i, j, count, pitch, offset;
 	u8 color, mask;
 	u16 data;
 	void __iomem *pbuffer, *pstart;
@@ -221,10 +221,10 @@ void hw_cursor_setData2(struct lynx_cursor * cursor,
 		{
 
 			if(opr & (0x80 >> j))
-			{	//use fg color,id = 2
+			{	/* use fg color,id = 2 */
 				data |= 2 << (j*2);
 			}else{
-				//use bg color,id = 1
+				/* use bg color,id = 1 */
 				data |= 1 << (j*2);
 			}
 		}
@@ -238,7 +238,6 @@ void hw_cursor_setData2(struct lynx_cursor * cursor,
 
 		/* assume pitch is 1,2,4,8,...*/
 		if(!(i&(pitch-1)))
-		//if((i+1) % pitch == 0)
 		{
 			/* need a return */
 			pstart += offset;
diff --git a/drivers/staging/sm750fb/sm750_cursor.h b/drivers/staging/sm750fb/sm750_cursor.h
index 8cede0721332..e1716a600239 100644
--- a/drivers/staging/sm750fb/sm750_cursor.h
+++ b/drivers/staging/sm750fb/sm750_cursor.h
@@ -2,16 +2,16 @@
 #define LYNX_CURSOR_H__
 
 /* hw_cursor_xxx works for voyager,718 and 750 */
-void hw_cursor_enable(struct lynx_cursor * cursor);
-void hw_cursor_disable(struct lynx_cursor * cursor);
-void hw_cursor_setSize(struct lynx_cursor * cursor,
-						int w,int h);
-void hw_cursor_setPos(struct lynx_cursor * cursor,
-						int x,int y);
-void hw_cursor_setColor(struct lynx_cursor * cursor,
-						u32 fg,u32 bg);
-void hw_cursor_setData(struct lynx_cursor * cursor,
-			u16 rop,const u8* data,const u8* mask);
-void hw_cursor_setData2(struct lynx_cursor * cursor,
-			u16 rop,const u8* data,const u8* mask);
+void hw_cursor_enable(struct lynx_cursor *cursor);
+void hw_cursor_disable(struct lynx_cursor *cursor);
+void hw_cursor_setSize(struct lynx_cursor *cursor,
+						int w, int h);
+void hw_cursor_setPos(struct lynx_cursor *cursor,
+						int x, int y);
+void hw_cursor_setColor(struct lynx_cursor *cursor,
+						u32 fg, u32 bg);
+void hw_cursor_setData(struct lynx_cursor *cursor,
+			u16 rop, const u8* data, const u8* mask);
+void hw_cursor_setData2(struct lynx_cursor *cursor,
+			u16 rop, const u8* data, const u8* mask);
 #endif
diff --git a/drivers/staging/sm750fb/sm750_help.h b/drivers/staging/sm750fb/sm750_help.h
index e0128d2a9ead..05777f72c166 100644
--- a/drivers/staging/sm750fb/sm750_help.h
+++ b/drivers/staging/sm750fb/sm750_help.h
@@ -1,29 +1,21 @@
 #ifndef LYNX_HELP_H__
 #define LYNX_HELP_H__
-/*****************************************************************************\
- *                                FIELD MACROS                               *
-\*****************************************************************************/
 
+/*  FIELD MACROS */
 #define _LSB(f)             (0 ? f)
 #define _MSB(f)             (1 ? f)
 #define _COUNT(f)           (_MSB(f) - _LSB(f) + 1)
 
 #define RAW_MASK(f)         (0xFFFFFFFF >> (32 - _COUNT(f)))
 #define GET_MASK(f)         (RAW_MASK(f) << _LSB(f))
-#define GET_FIELD(d,f)      (((d) >> _LSB(f)) & RAW_MASK(f))
-#define TEST_FIELD(d,f,v)   (GET_FIELD(d,f) == f ## _ ## v)
-#define SET_FIELD(d,f,v)    (((d) & ~GET_MASK(f)) | \
+#define GET_FIELD(d, f)     (((d) >> _LSB(f)) & RAW_MASK(f))
+#define TEST_FIELD(d, f, v) (GET_FIELD(d, f) == f ## _ ## v)
+#define SET_FIELD(d, f, v)  (((d) & ~GET_MASK(f)) | \
                             (((f ## _ ## v) & RAW_MASK(f)) << _LSB(f)))
-#define SET_FIELDV(d,f,v)   (((d) & ~GET_MASK(f)) | \
+#define SET_FIELDV(d, f, v) (((d) & ~GET_MASK(f)) | \
                             (((v) & RAW_MASK(f)) << _LSB(f)))
 
-
-////////////////////////////////////////////////////////////////////////////////
-//                                                                            //
-// Internal macros                                                            //
-//                                                                            //
-////////////////////////////////////////////////////////////////////////////////
-
+/* Internal macros */
 #define _F_START(f)             (0 ? f)
 #define _F_END(f)               (1 ? f)
 #define _F_SIZE(f)              (1 + _F_END(f) - _F_START(f))
@@ -31,13 +23,7 @@
 #define _F_NORMALIZE(v, f)      (((v) & _F_MASK(f)) >> _F_START(f))
 #define _F_DENORMALIZE(v, f)    (((v) << _F_START(f)) & _F_MASK(f))
 
-
-////////////////////////////////////////////////////////////////////////////////
-//                                                                            //
-// Global macros                                                              //
-//                                                                            //
-////////////////////////////////////////////////////////////////////////////////
-
+/* Global macros */
 #define FIELD_GET(x, reg, field) \
 ( \
     _F_NORMALIZE((x), reg ## _ ## field) \
@@ -60,13 +46,7 @@
     ~ _F_MASK(reg ## _ ## field) \
 )
 
-
-////////////////////////////////////////////////////////////////////////////////
-//                                                                            //
-// Field Macros                                                               //
-//                                                                            //
-////////////////////////////////////////////////////////////////////////////////
-
+/* Field Macros */
 #define FIELD_START(field)              (0 ? field)
 #define FIELD_END(field)                (1 ? field)
 #define FIELD_SIZE(field)               (1 + FIELD_END(field) - FIELD_START(field))
@@ -91,7 +71,7 @@
     (unsigned short) ((((r) & 0xF8) << 8) | (((g) & 0xFC) << 3) | (((b) & 0xF8) >> 3)) \
 )
 
-static inline unsigned int absDiff(unsigned int a,unsigned int b)
+static inline unsigned int absDiff(unsigned int a, unsigned int b)
 {
 	if(a<b)
 		return b-a;
@@ -100,7 +80,7 @@ static inline unsigned int absDiff(unsigned int a,unsigned int b)
 }
 
 /* n / d + 1 / 2 = (2n + d) / 2d */
-#define roundedDiv(num,denom)	((2 * (num) + (denom)) / (2 * (denom)))
+#define roundedDiv(num, denom)	((2 * (num) + (denom)) / (2 * (denom)))
 #define MB(x) ((x)<<20)
 #define KB(x) ((x)<<10)
 #define MHz(x) ((x) * 1000000)
diff --git a/drivers/staging/sm750fb/sm750_hw.c b/drivers/staging/sm750fb/sm750_hw.c
index 9f0d06da12fb..84381bc414e7 100644
--- a/drivers/staging/sm750fb/sm750_hw.c
+++ b/drivers/staging/sm750fb/sm750_hw.c
@@ -27,10 +27,10 @@
 int hw_sm750_map(struct lynx_share* share, struct pci_dev* pdev)
 {
 	int ret;
-	struct sm750_share * spec_share;
+	struct sm750_share *spec_share;
 	
 
-	spec_share = container_of(share, struct sm750_share,share);
+	spec_share = container_of(share, struct sm750_share, share);
 	ret = 0;
 
 	share->vidreg_start  = pci_resource_start(pdev, 1);
@@ -64,7 +64,7 @@ int hw_sm750_map(struct lynx_share* share, struct pci_dev* pdev)
 	share->accel.dprBase = share->pvReg + DE_BASE_ADDR_TYPE1;
 	share->accel.dpPortBase = share->pvReg + DE_PORT_ADDR_TYPE1;
 
-	ddk750_set_mmio(share->pvReg,share->devid, share->revid);
+	ddk750_set_mmio(share->pvReg, share->devid, share->revid);
 
 	share->vidmem_start = pci_resource_start(pdev, 0);
 	/* don't use pdev_resource[x].end - resource[x].start to
@@ -78,15 +78,14 @@ int hw_sm750_map(struct lynx_share* share, struct pci_dev* pdev)
 
 	/* reserve the vidmem space of smi adaptor */
 #if 0
-	if((ret = pci_request_region(pdev,0,_moduleName_)))
+	if((ret = pci_request_region(pdev, 0, _moduleName_)))
 	{
 		pr_err("Can not request PCI regions.\n");
 		goto exit;
 	}
 #endif
 
-	share->pvMem = ioremap(share->vidmem_start,
-							share->vidmem_size);
+	share->pvMem = ioremap_wc(share->vidmem_start, share->vidmem_size);
 
 	if(!share->pvMem){
 		pr_err("Map video memory failed\n");
@@ -101,12 +100,12 @@ exit:
 
 
 
-int hw_sm750_inithw(struct lynx_share* share, struct pci_dev * pdev)
+int hw_sm750_inithw(struct lynx_share *share, struct pci_dev *pdev)
 {
-	struct sm750_share * spec_share;
-	struct init_status * parm;
+	struct sm750_share *spec_share;
+	struct init_status *parm;
 	
-	spec_share = container_of(share, struct sm750_share,share);
+	spec_share = container_of(share, struct sm750_share, share);
 	parm = &spec_share->state.initParm;
 	if(parm->chip_clk == 0)
 		parm->chip_clk = (getChipType() == SM750LE)?
@@ -172,7 +171,7 @@ int hw_sm750_inithw(struct lynx_share* share, struct pci_dev * pdev)
 		/* Set up GPIO for software I2C to program DVI chip in the
 		   Xilinx SP605 board, in order to have video signal.
 		 */
-        swI2CInit(0,1);
+        swI2CInit(0, 1);
 
 
         /* Customer may NOT use CH7301 DVI chip, which has to be
@@ -194,14 +193,13 @@ int hw_sm750_inithw(struct lynx_share* share, struct pci_dev * pdev)
 	/* init 2d engine */
 	if(!share->accel_off){
 		hw_sm750_initAccel(share);
-//		share->accel.de_wait = hw_sm750_deWait;
 	}
 
 	return 0;
 }
 
 
-resource_size_t hw_sm750_getVMSize(struct lynx_share * share)
+resource_size_t hw_sm750_getVMSize(struct lynx_share *share)
 {
 	resource_size_t ret;
 	
@@ -267,10 +265,10 @@ void hw_sm750_output_clear(struct lynxfb_output* output)
 
 int hw_sm750_crtc_checkMode(struct lynxfb_crtc* crtc, struct fb_var_screeninfo* var)
 {
-	struct lynx_share * share;
+	struct lynx_share *share;
 	
 
-	share = container_of(crtc, struct lynxfb_par,crtc)->share;
+	share = container_of(crtc, struct lynxfb_par, crtc)->share;
 
 	switch (var->bits_per_pixel){
 		case 8:
@@ -298,12 +296,12 @@ int hw_sm750_crtc_setMode(struct lynxfb_crtc* crtc,
 								struct fb_var_screeninfo* var,
 								struct fb_fix_screeninfo* fix)
 {
-	int ret,fmt;
+	int ret, fmt;
 	u32 reg;
 	mode_parameter_t modparm;
 	clock_type_t clock;
-	struct lynx_share * share;
-	struct lynxfb_par * par;
+	struct lynx_share *share;
+	struct lynxfb_par *par;
 
 	
 	ret = 0;
@@ -329,7 +327,6 @@ int hw_sm750_crtc_setMode(struct lynxfb_crtc* crtc,
 #endif
 
 	/* set timing */
-//	modparm.pixel_clock = PS_TO_HZ(var->pixclock);
 	modparm.pixel_clock = ps_to_hz(var->pixclock);
 	modparm.vertical_sync_polarity = (var->sync & FB_SYNC_HOR_HIGH_ACT) ? POS:NEG;
 	modparm.horizontal_sync_polarity = (var->sync & FB_SYNC_VERT_HIGH_ACT) ? POS:NEG;
@@ -365,7 +362,7 @@ int hw_sm750_crtc_setMode(struct lynxfb_crtc* crtc,
 
 		reg = var->xres * (var->bits_per_pixel >> 3);
 		/* crtc->channel is not equal to par->index on numeric,be aware of that */
-		reg = PADDING(crtc->line_pad,reg);
+		reg = PADDING(crtc->line_pad, reg);
 
 		POKE32(PANEL_FB_WIDTH,
 			FIELD_VALUE(0, PANEL_FB_WIDTH, WIDTH, reg)|
@@ -383,7 +380,7 @@ int hw_sm750_crtc_setMode(struct lynxfb_crtc* crtc,
 
 		POKE32(PANEL_PLANE_BR,
 			FIELD_VALUE(0, PANEL_PLANE_BR, BOTTOM, var->yres - 1)|
-			FIELD_VALUE(0, PANEL_PLANE_BR,RIGHT, var->xres - 1));
+			FIELD_VALUE(0, PANEL_PLANE_BR, RIGHT, var->xres - 1));
 
 		/* set pixel format */
 		reg = PEEK32(PANEL_DISPLAY_CTRL);
@@ -424,17 +421,17 @@ void hw_sm750_crtc_clear(struct lynxfb_crtc* crtc)
 int hw_sm750_setColReg(struct lynxfb_crtc* crtc, ushort index,
 								ushort red, ushort green, ushort blue)
 {
-	static unsigned int add[]={PANEL_PALETTE_RAM,CRT_PALETTE_RAM};
+	static unsigned int add[]={PANEL_PALETTE_RAM, CRT_PALETTE_RAM};
 	POKE32(add[crtc->channel] + index*4, (red<<16)|(green<<8)|blue);
 	return 0;
 }
 
 int hw_sm750le_setBLANK(struct lynxfb_output * output, int blank){
-	int dpms,crtdb;
+	int dpms, crtdb;
 	
 	switch(blank)
 	{
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_UNBLANK:
 #else
 		case VESA_NO_BLANKING:
@@ -442,13 +439,13 @@ int hw_sm750le_setBLANK(struct lynxfb_output * output, int blank){
 			dpms = CRT_DISPLAY_CTRL_DPMS_0;
 			crtdb = CRT_DISPLAY_CTRL_BLANK_OFF;
 			break;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_NORMAL:
 			dpms = CRT_DISPLAY_CTRL_DPMS_0;
 			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
 			break;
 #endif
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_VSYNC_SUSPEND:
 #else
 		case VESA_VSYNC_SUSPEND:
@@ -456,7 +453,7 @@ int hw_sm750le_setBLANK(struct lynxfb_output * output, int blank){
 			dpms = CRT_DISPLAY_CTRL_DPMS_2;
 			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
 			break;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_HSYNC_SUSPEND:
 #else
 		case VESA_HSYNC_SUSPEND:
@@ -464,7 +461,7 @@ int hw_sm750le_setBLANK(struct lynxfb_output * output, int blank){
 			dpms = CRT_DISPLAY_CTRL_DPMS_1;
 			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
 			break;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_POWERDOWN:
 #else
 		case VESA_POWERDOWN:
@@ -483,7 +480,7 @@ int hw_sm750le_setBLANK(struct lynxfb_output * output, int blank){
 	return 0;
 }
 
-int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
+int hw_sm750_setBLANK(struct lynxfb_output* output, int blank)
 {
 	unsigned int dpms, pps, crtdb;
 	
@@ -491,7 +488,7 @@ int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
 
 	switch (blank)
 	{
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_UNBLANK:
 #else
 		case VESA_NO_BLANKING:
@@ -501,7 +498,7 @@ int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
 			pps = PANEL_DISPLAY_CTRL_DATA_ENABLE;
 			crtdb = CRT_DISPLAY_CTRL_BLANK_OFF;
 			break;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_NORMAL:
 			pr_info("flag = FB_BLANK_NORMAL \n");
 			dpms = SYSTEM_CTRL_DPMS_VPHP;
@@ -509,7 +506,7 @@ int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
 			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
 			break;
 #endif
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_VSYNC_SUSPEND:
 #else
 		case VESA_VSYNC_SUSPEND:
@@ -518,7 +515,7 @@ int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
 			pps = PANEL_DISPLAY_CTRL_DATA_DISABLE;
 			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
 			break;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_HSYNC_SUSPEND:
 #else
 		case VESA_HSYNC_SUSPEND:
@@ -527,7 +524,7 @@ int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
 			pps = PANEL_DISPLAY_CTRL_DATA_DISABLE;
 			crtdb = CRT_DISPLAY_CTRL_BLANK_ON;
 			break;
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,10)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 10)
 		case FB_BLANK_POWERDOWN:
 #else
 		case VESA_POWERDOWN:
@@ -540,8 +537,8 @@ int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
 
 	if(output->paths & sm750_crt){
 
-		POKE32(SYSTEM_CTRL,FIELD_VALUE(PEEK32(SYSTEM_CTRL), SYSTEM_CTRL, DPMS, dpms));
-		POKE32(CRT_DISPLAY_CTRL,FIELD_VALUE(PEEK32(CRT_DISPLAY_CTRL), CRT_DISPLAY_CTRL,BLANK, crtdb));
+		POKE32(SYSTEM_CTRL, FIELD_VALUE(PEEK32(SYSTEM_CTRL), SYSTEM_CTRL, DPMS, dpms));
+		POKE32(CRT_DISPLAY_CTRL, FIELD_VALUE(PEEK32(CRT_DISPLAY_CTRL), CRT_DISPLAY_CTRL, BLANK, crtdb));
 	}
 
 	if(output->paths & sm750_panel){
@@ -552,28 +549,28 @@ int hw_sm750_setBLANK(struct lynxfb_output* output,int blank)
 }
 
 
-void hw_sm750_initAccel(struct lynx_share * share)
+void hw_sm750_initAccel(struct lynx_share *share)
 {
 	u32 reg;
 	enable2DEngine(1);
 
 	if(getChipType() == SM750LE){
 		reg = PEEK32(DE_STATE1);
-		reg = FIELD_SET(reg, DE_STATE1, DE_ABORT,ON);
-		POKE32(DE_STATE1,reg);
+		reg = FIELD_SET(reg, DE_STATE1, DE_ABORT, ON);
+		POKE32(DE_STATE1, reg);
 
 		reg = PEEK32(DE_STATE1);
-		reg = FIELD_SET(reg, DE_STATE1, DE_ABORT,OFF);
+		reg = FIELD_SET(reg, DE_STATE1, DE_ABORT, OFF);
 		POKE32(DE_STATE1, reg);
 
 	}else{
 		/* engine reset */
 		reg = PEEK32(SYSTEM_CTRL);
-	    reg = FIELD_SET(reg, SYSTEM_CTRL, DE_ABORT,ON);
+	    reg = FIELD_SET(reg, SYSTEM_CTRL, DE_ABORT, ON);
 		POKE32(SYSTEM_CTRL, reg);
 
 		reg = PEEK32(SYSTEM_CTRL);
-		reg = FIELD_SET(reg, SYSTEM_CTRL, DE_ABORT,OFF);
+		reg = FIELD_SET(reg, SYSTEM_CTRL, DE_ABORT, OFF);
 		POKE32(SYSTEM_CTRL, reg);
 	}
 
@@ -603,9 +600,9 @@ int hw_sm750_deWait(void)
 	int i=0x10000000;
 	while(i--){
 		unsigned int dwVal = PEEK32(SYSTEM_CTRL);
-		if((FIELD_GET(dwVal,SYSTEM_CTRL,DE_STATUS) == SYSTEM_CTRL_DE_STATUS_IDLE) &&
-			(FIELD_GET(dwVal,SYSTEM_CTRL,DE_FIFO)  == SYSTEM_CTRL_DE_FIFO_EMPTY) &&
-			(FIELD_GET(dwVal,SYSTEM_CTRL,DE_MEM_FIFO) == SYSTEM_CTRL_DE_MEM_FIFO_EMPTY))
+		if((FIELD_GET(dwVal, SYSTEM_CTRL, DE_STATUS) == SYSTEM_CTRL_DE_STATUS_IDLE) &&
+			(FIELD_GET(dwVal, SYSTEM_CTRL, DE_FIFO)  == SYSTEM_CTRL_DE_FIFO_EMPTY) &&
+			(FIELD_GET(dwVal, SYSTEM_CTRL, DE_MEM_FIFO) == SYSTEM_CTRL_DE_MEM_FIFO_EMPTY))
 		{
 			return 0;
 		}
@@ -619,7 +616,7 @@ int hw_sm750_pan_display(struct lynxfb_crtc *crtc,
         const struct fb_info *info)
 {
     uint32_t total;
-    //check params
+    /* check params */
     if ((var->xoffset + var->xres > var->xres_virtual) ||
             (var->yoffset + var->yres > var->yres_virtual)) {
         return -EINVAL;
diff --git a/drivers/staging/sm750fb/sm750_hw.h b/drivers/staging/sm750fb/sm750_hw.h
index b05be5e99f51..93288b3a99d8 100644
--- a/drivers/staging/sm750fb/sm750_hw.h
+++ b/drivers/staging/sm750fb/sm750_hw.h
@@ -8,9 +8,6 @@
 #define SM750LE_REVISION_ID (unsigned char)0xfe
 #endif
 
-//#define DEFAULT_MEM_CLOCK	(DEFAULT_SM750_CHIP_CLOCK/1)
-//#define DEFAULT_MASTER_CLOCK	(DEFAULT_SM750_CHIP_CLOCK/3)
-
 
 enum sm750_pnltype{
 
@@ -65,7 +62,7 @@ struct sm750_state{
 };
 
 /* 	sm750_share stands for a presentation of two frame buffer
-	that use one sm750 adaptor, it is similiar to the super class of lynx_share
+	that use one sm750 adaptor, it is similar to the super class of lynx_share
 	in C++
 */
 
@@ -81,20 +78,20 @@ struct sm750_share{
 	*/
 };
 
-int hw_sm750_map(struct lynx_share* share,struct pci_dev* pdev);
-int hw_sm750_inithw(struct lynx_share*,struct pci_dev *);
+int hw_sm750_map(struct lynx_share* share, struct pci_dev* pdev);
+int hw_sm750_inithw(struct lynx_share*, struct pci_dev *);
 void hw_sm750_initAccel(struct lynx_share *);
 int hw_sm750_deWait(void);
 int hw_sm750le_deWait(void);
 
 resource_size_t hw_sm750_getVMSize(struct lynx_share *);
-int hw_sm750_output_checkMode(struct lynxfb_output*,struct fb_var_screeninfo*);
-int hw_sm750_output_setMode(struct lynxfb_output*,struct fb_var_screeninfo*,struct fb_fix_screeninfo*);
-int hw_sm750_crtc_checkMode(struct lynxfb_crtc*,struct fb_var_screeninfo*);
-int hw_sm750_crtc_setMode(struct lynxfb_crtc*,struct fb_var_screeninfo*,struct fb_fix_screeninfo*);
-int hw_sm750_setColReg(struct lynxfb_crtc*,ushort,ushort,ushort,ushort);
-int hw_sm750_setBLANK(struct lynxfb_output*,int);
-int hw_sm750le_setBLANK(struct lynxfb_output*,int);
+int hw_sm750_output_checkMode(struct lynxfb_output*, struct fb_var_screeninfo*);
+int hw_sm750_output_setMode(struct lynxfb_output*, struct fb_var_screeninfo*, struct fb_fix_screeninfo*);
+int hw_sm750_crtc_checkMode(struct lynxfb_crtc*, struct fb_var_screeninfo*);
+int hw_sm750_crtc_setMode(struct lynxfb_crtc*, struct fb_var_screeninfo*, struct fb_fix_screeninfo*);
+int hw_sm750_setColReg(struct lynxfb_crtc*, ushort, ushort, ushort, ushort);
+int hw_sm750_setBLANK(struct lynxfb_output*, int);
+int hw_sm750le_setBLANK(struct lynxfb_output*, int);
 void hw_sm750_crtc_clear(struct lynxfb_crtc*);
 void hw_sm750_output_clear(struct lynxfb_output*);
 int hw_sm750_pan_display(struct lynxfb_crtc *crtc,
diff --git a/drivers/staging/sm7xxfb/sm7xx.h b/drivers/staging/sm7xxfb/sm7xx.h
index c5d62534e4a4..4bed0946c1b1 100644
--- a/drivers/staging/sm7xxfb/sm7xx.h
+++ b/drivers/staging/sm7xxfb/sm7xx.h
@@ -98,7 +98,7 @@ static inline unsigned int smtc_seqr(int reg)
 /* The next structure holds all information relevant for a specific video mode.
  */
 
-struct ModeInit {
+struct modeinit {
 	int mmsizex;
 	int mmsizey;
 	int bpp;
@@ -115,665 +115,3 @@ struct ModeInit {
 	unsigned char init_cr30_cr4d[SIZE_CR30_CR4D];
 	unsigned char init_cr90_cra7[SIZE_CR90_CRA7];
 };
-
-/**********************************************************************
-			 SM712 Mode table.
- **********************************************************************/
-static struct ModeInit vgamode[] = {
-	{
-	 /*  mode#0: 640 x 480  16Bpp  60Hz */
-	 640, 480, 16, 60,
-	 /*  Init_MISC */
-	 0xE3,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x00, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xFF, 0xBE, 0xEF, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x30, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x32, 0x03, 0xA0, 0x09, 0xC0, 0x32, 0x32, 0x32,
-	  0x32, 0x32, 0x32, 0x32, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x32, 0x32, 0x32,
-	  0x04, 0x24, 0x63, 0x4F, 0x52, 0x0B, 0xDF, 0xEA,
-	  0x04, 0x50, 0x19, 0x32, 0x32, 0x00, 0x00, 0x32,
-	  0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x07, 0x82, 0x07, 0x04,
-	  0x00, 0x45, 0x30, 0x30, 0x40, 0x30,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x32,
-	  0xF7, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x32, 0x32,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
-	  0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xFF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0x5F, 0x4F, 0x4F, 0x00, 0x53, 0x1F, 0x0B, 0x3E,
-	  0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xEA, 0x0C, 0xDF, 0x50, 0x40, 0xDF, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x55, 0x03, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xFF, 0xFD,
-	  0x5F, 0x4F, 0x00, 0x54, 0x00, 0x0B, 0xDF, 0x00,
-	  0xEA, 0x0C, 0x2E, 0x00, 0x4F, 0xDF,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x56, 0xDD, 0x5E, 0xEA, 0x87, 0x44, 0x8F, 0x55,
-	  0x0A, 0x8F, 0x55, 0x0A, 0x00, 0x00, 0x18, 0x00,
-	  0x11, 0x10, 0x0B, 0x0A, 0x0A, 0x0A, 0x0A, 0x00,
-	  },
-	 },
-	{
-	 /*  mode#1: 640 x 480  24Bpp  60Hz */
-	 640, 480, 24, 60,
-	 /*  Init_MISC */
-	 0xE3,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x00, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xFF, 0xBE, 0xEF, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x30, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x32, 0x03, 0xA0, 0x09, 0xC0, 0x32, 0x32, 0x32,
-	  0x32, 0x32, 0x32, 0x32, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x32, 0x32, 0x32,
-	  0x04, 0x24, 0x63, 0x4F, 0x52, 0x0B, 0xDF, 0xEA,
-	  0x04, 0x50, 0x19, 0x32, 0x32, 0x00, 0x00, 0x32,
-	  0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x07, 0x82, 0x07, 0x04,
-	  0x00, 0x45, 0x30, 0x30, 0x40, 0x30,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x32,
-	  0xF7, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x32, 0x32,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
-	  0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xFF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0x5F, 0x4F, 0x4F, 0x00, 0x53, 0x1F, 0x0B, 0x3E,
-	  0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xEA, 0x0C, 0xDF, 0x50, 0x40, 0xDF, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x55, 0x03, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xFF, 0xFD,
-	  0x5F, 0x4F, 0x00, 0x54, 0x00, 0x0B, 0xDF, 0x00,
-	  0xEA, 0x0C, 0x2E, 0x00, 0x4F, 0xDF,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x56, 0xDD, 0x5E, 0xEA, 0x87, 0x44, 0x8F, 0x55,
-	  0x0A, 0x8F, 0x55, 0x0A, 0x00, 0x00, 0x18, 0x00,
-	  0x11, 0x10, 0x0B, 0x0A, 0x0A, 0x0A, 0x0A, 0x00,
-	  },
-	 },
-	{
-	 /*  mode#0: 640 x 480  32Bpp  60Hz */
-	 640, 480, 32, 60,
-	 /*  Init_MISC */
-	 0xE3,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x00, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xFF, 0xBE, 0xEF, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x30, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x32, 0x03, 0xA0, 0x09, 0xC0, 0x32, 0x32, 0x32,
-	  0x32, 0x32, 0x32, 0x32, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x32, 0x32, 0x32,
-	  0x04, 0x24, 0x63, 0x4F, 0x52, 0x0B, 0xDF, 0xEA,
-	  0x04, 0x50, 0x19, 0x32, 0x32, 0x00, 0x00, 0x32,
-	  0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x07, 0x82, 0x07, 0x04,
-	  0x00, 0x45, 0x30, 0x30, 0x40, 0x30,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x32,
-	  0xF7, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x32, 0x32,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
-	  0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xFF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0x5F, 0x4F, 0x4F, 0x00, 0x53, 0x1F, 0x0B, 0x3E,
-	  0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xEA, 0x0C, 0xDF, 0x50, 0x40, 0xDF, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x55, 0x03, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xFF, 0xFD,
-	  0x5F, 0x4F, 0x00, 0x54, 0x00, 0x0B, 0xDF, 0x00,
-	  0xEA, 0x0C, 0x2E, 0x00, 0x4F, 0xDF,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x56, 0xDD, 0x5E, 0xEA, 0x87, 0x44, 0x8F, 0x55,
-	  0x0A, 0x8F, 0x55, 0x0A, 0x00, 0x00, 0x18, 0x00,
-	  0x11, 0x10, 0x0B, 0x0A, 0x0A, 0x0A, 0x0A, 0x00,
-	  },
-	 },
-
-	{			/*  mode#2: 800 x 600  16Bpp  60Hz */
-	 800, 600, 16, 60,
-	 /*  Init_MISC */
-	 0x2B,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x03, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xFF, 0xBE, 0xEE, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x30, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x34, 0x03, 0x20, 0x09, 0xC0, 0x24, 0x24, 0x24,
-	  0x24, 0x24, 0x24, 0x24, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x38, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x24, 0x24, 0x24,
-	  0x04, 0x48, 0x83, 0x63, 0x68, 0x72, 0x57, 0x58,
-	  0x04, 0x55, 0x59, 0x24, 0x24, 0x00, 0x00, 0x24,
-	  0x01, 0x80, 0x7A, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x1C, 0x85, 0x35, 0x13,
-	  0x02, 0x45, 0x30, 0x35, 0x40, 0x20,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0x00, 0x00, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x24,
-	  0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x24, 0x24,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
-	  0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xBF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0x7F, 0x63, 0x63, 0x00, 0x68, 0x18, 0x72, 0xF0,
-	  0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0x58, 0x0C, 0x57, 0x64, 0x40, 0x57, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x03, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xBF, 0xFD,
-	  0x7F, 0x63, 0x00, 0x69, 0x18, 0x72, 0x57, 0x00,
-	  0x58, 0x0C, 0xE0, 0x20, 0x63, 0x57,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x56, 0x4B, 0x5E, 0x55, 0x86, 0x9D, 0x8E, 0xAA,
-	  0xDB, 0x2A, 0xDF, 0x33, 0x00, 0x00, 0x18, 0x00,
-	  0x20, 0x1F, 0x1A, 0x19, 0x0F, 0x0F, 0x0F, 0x00,
-	  },
-	 },
-	{			/*  mode#3: 800 x 600  24Bpp  60Hz */
-	 800, 600, 24, 60,
-	 0x2B,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x03, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xFF, 0xBE, 0xEE, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x30, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x36, 0x03, 0x20, 0x09, 0xC0, 0x36, 0x36, 0x36,
-	  0x36, 0x36, 0x36, 0x36, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x36, 0x36, 0x36,
-	  0x04, 0x48, 0x83, 0x63, 0x68, 0x72, 0x57, 0x58,
-	  0x04, 0x55, 0x59, 0x36, 0x36, 0x00, 0x00, 0x36,
-	  0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x1C, 0x85, 0x35, 0x13,
-	  0x02, 0x45, 0x30, 0x30, 0x40, 0x20,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x36,
-	  0xF7, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x36, 0x36,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
-	  0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xBF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0x7F, 0x63, 0x63, 0x00, 0x68, 0x18, 0x72, 0xF0,
-	  0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0x58, 0x0C, 0x57, 0x64, 0x40, 0x57, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x03, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xBF, 0xFD,
-	  0x7F, 0x63, 0x00, 0x69, 0x18, 0x72, 0x57, 0x00,
-	  0x58, 0x0C, 0xE0, 0x20, 0x63, 0x57,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x56, 0x4B, 0x5E, 0x55, 0x86, 0x9D, 0x8E, 0xAA,
-	  0xDB, 0x2A, 0xDF, 0x33, 0x00, 0x00, 0x18, 0x00,
-	  0x20, 0x1F, 0x1A, 0x19, 0x0F, 0x0F, 0x0F, 0x00,
-	  },
-	 },
-	{			/*  mode#7: 800 x 600  32Bpp  60Hz */
-	 800, 600, 32, 60,
-	 /*  Init_MISC */
-	 0x2B,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x03, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xFF, 0xBE, 0xEE, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x30, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x34, 0x03, 0x20, 0x09, 0xC0, 0x24, 0x24, 0x24,
-	  0x24, 0x24, 0x24, 0x24, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x38, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x24, 0x24, 0x24,
-	  0x04, 0x48, 0x83, 0x63, 0x68, 0x72, 0x57, 0x58,
-	  0x04, 0x55, 0x59, 0x24, 0x24, 0x00, 0x00, 0x24,
-	  0x01, 0x80, 0x7A, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x1C, 0x85, 0x35, 0x13,
-	  0x02, 0x45, 0x30, 0x35, 0x40, 0x20,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0x00, 0x00, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x24,
-	  0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x24, 0x24,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
-	  0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xBF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0x7F, 0x63, 0x63, 0x00, 0x68, 0x18, 0x72, 0xF0,
-	  0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0x58, 0x0C, 0x57, 0x64, 0x40, 0x57, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x03, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xBF, 0xFD,
-	  0x7F, 0x63, 0x00, 0x69, 0x18, 0x72, 0x57, 0x00,
-	  0x58, 0x0C, 0xE0, 0x20, 0x63, 0x57,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x56, 0x4B, 0x5E, 0x55, 0x86, 0x9D, 0x8E, 0xAA,
-	  0xDB, 0x2A, 0xDF, 0x33, 0x00, 0x00, 0x18, 0x00,
-	  0x20, 0x1F, 0x1A, 0x19, 0x0F, 0x0F, 0x0F, 0x00,
-	  },
-	 },
-	/* We use 1024x768 table to light 1024x600 panel for lemote */
-	{			/*  mode#4: 1024 x 600  16Bpp  60Hz  */
-	 1024, 600, 16, 60,
-	 /*  Init_MISC */
-	 0xEB,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x00, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xC8, 0x40, 0x14, 0x60, 0x00, 0x0A, 0x17, 0x20,
-	  0x51, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x30, 0x02, 0x00, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x22, 0x03, 0x24, 0x09, 0xC0, 0x22, 0x22, 0x22,
-	  0x22, 0x22, 0x22, 0x22, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x22, 0x22, 0x22,
-	  0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
-	  0x00, 0x60, 0x59, 0x22, 0x22, 0x00, 0x00, 0x22,
-	  0x01, 0x80, 0x7A, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x16, 0x02, 0x0D, 0x82, 0x09, 0x02,
-	  0x04, 0x45, 0x3F, 0x30, 0x40, 0x20,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
-	  0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
-	  0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
-	  0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
-	  0xA3, 0x7F, 0x00, 0x82, 0x0b, 0x6f, 0x57, 0x00,
-	  0x5c, 0x0f, 0xE0, 0xe0, 0x7F, 0x57,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
-	  0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
-	  0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
-	  },
-	 },
-	{			/*  mode#5: 1024 x 768  24Bpp  60Hz */
-	 1024, 768, 24, 60,
-	 /*  Init_MISC */
-	 0xEB,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x03, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xF3, 0xB6, 0xC0, 0xDD, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x30, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x38, 0x03, 0x20, 0x09, 0xC0, 0x3A, 0x3A, 0x3A,
-	  0x3A, 0x3A, 0x3A, 0x3A, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x00, 0x00, 0x3A,
-	  0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
-	  0x00, 0x60, 0x59, 0x3A, 0x3A, 0x00, 0x00, 0x3A,
-	  0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x3B, 0x0D, 0x09, 0x02,
-	  0x04, 0x45, 0x30, 0x30, 0x40, 0x20,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
-	  0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
-	  0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
-	  0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
-	  0xA3, 0x7F, 0x00, 0x86, 0x15, 0x24, 0xFF, 0x00,
-	  0x01, 0x07, 0xE5, 0x20, 0x7F, 0xFF,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
-	  0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
-	  0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
-	  },
-	 },
-	{			/*  mode#4: 1024 x 768  32Bpp  60Hz */
-	 1024, 768, 32, 60,
-	 /*  Init_MISC */
-	 0xEB,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x03, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xF3, 0xB6, 0xC0, 0xDD, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x32, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x38, 0x03, 0x20, 0x09, 0xC0, 0x3A, 0x3A, 0x3A,
-	  0x3A, 0x3A, 0x3A, 0x3A, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x00, 0x00, 0x3A,
-	  0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
-	  0x00, 0x60, 0x59, 0x3A, 0x3A, 0x00, 0x00, 0x3A,
-	  0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x3B, 0x0D, 0x09, 0x02,
-	  0x04, 0x45, 0x30, 0x30, 0x40, 0x20,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
-	  0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
-	  0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
-	  0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
-	  0x00, 0x00, 0x00, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
-	  0xA3, 0x7F, 0x00, 0x86, 0x15, 0x24, 0xFF, 0x00,
-	  0x01, 0x07, 0xE5, 0x20, 0x7F, 0xFF,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
-	  0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
-	  0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
-	  },
-	 },
-	{			/*  mode#6: 320 x 240  16Bpp  60Hz */
-	 320, 240, 16, 60,
-	 /*  Init_MISC */
-	 0xEB,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x03, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xF3, 0xB6, 0xC0, 0xDD, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x32, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x38, 0x03, 0x20, 0x09, 0xC0, 0x3A, 0x3A, 0x3A,
-	  0x3A, 0x3A, 0x3A, 0x3A, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x00, 0x00, 0x3A,
-	  0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
-	  0x00, 0x60, 0x59, 0x3A, 0x3A, 0x00, 0x00, 0x3A,
-	  0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x08, 0x43, 0x08, 0x43,
-	  0x04, 0x45, 0x30, 0x30, 0x40, 0x20,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
-	  0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
-	  0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
-	  0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
-	  0x00, 0x00, 0x30, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
-	  0x2E, 0x27, 0x00, 0x2b, 0x0c, 0x0F, 0xEF, 0x00,
-	  0xFe, 0x0f, 0x01, 0xC0, 0x27, 0xEF,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
-	  0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
-	  0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
-	  },
-	 },
-
-	{			/*  mode#8: 320 x 240  32Bpp  60Hz */
-	 320, 240, 32, 60,
-	 /*  Init_MISC */
-	 0xEB,
-	 {			/*  Init_SR0_SR4 */
-	  0x03, 0x01, 0x0F, 0x03, 0x0E,
-	  },
-	 {			/*  Init_SR10_SR24 */
-	  0xF3, 0xB6, 0xC0, 0xDD, 0x00, 0x0E, 0x17, 0x2C,
-	  0x99, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0xC4, 0x32, 0x02, 0x01, 0x01,
-	  },
-	 {			/*  Init_SR30_SR75 */
-	  0x38, 0x03, 0x20, 0x09, 0xC0, 0x3A, 0x3A, 0x3A,
-	  0x3A, 0x3A, 0x3A, 0x3A, 0x00, 0x00, 0x03, 0xFF,
-	  0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
-	  0x20, 0x0C, 0x44, 0x20, 0x00, 0x00, 0x00, 0x3A,
-	  0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
-	  0x00, 0x60, 0x59, 0x3A, 0x3A, 0x00, 0x00, 0x3A,
-	  0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
-	  0x50, 0x03, 0x74, 0x14, 0x08, 0x43, 0x08, 0x43,
-	  0x04, 0x45, 0x30, 0x30, 0x40, 0x20,
-	  },
-	 {			/*  Init_SR80_SR93 */
-	  0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
-	  0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
-	  0x00, 0x00, 0x00, 0x00,
-	  },
-	 {			/*  Init_SRA0_SRAF */
-	  0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
-	  0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
-	  },
-	 {			/*  Init_GR00_GR08 */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
-	  0xFF,
-	  },
-	 {			/*  Init_AR00_AR14 */
-	  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
-	  0x41, 0x00, 0x0F, 0x00, 0x00,
-	  },
-	 {			/*  Init_CR00_CR18 */
-	  0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
-	  0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	  0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
-	  0xFF,
-	  },
-	 {			/*  Init_CR30_CR4D */
-	  0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
-	  0x00, 0x00, 0x30, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
-	  0x2E, 0x27, 0x00, 0x2b, 0x0c, 0x0F, 0xEF, 0x00,
-	  0xFe, 0x0f, 0x01, 0xC0, 0x27, 0xEF,
-	  },
-	 {			/*  Init_CR90_CRA7 */
-	  0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
-	  0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
-	  0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
-	  },
-	 },
-};
-
-#define numvgamodes		ARRAY_SIZE(vgamode)
diff --git a/drivers/staging/sm7xxfb/sm7xxfb.c b/drivers/staging/sm7xxfb/sm7xxfb.c
index 77f51a075004..2ff4fe73d148 100644
--- a/drivers/staging/sm7xxfb/sm7xxfb.c
+++ b/drivers/staging/sm7xxfb/sm7xxfb.c
@@ -39,7 +39,7 @@
 */
 struct smtcfb_info {
 	struct pci_dev *pdev;
-	struct fb_info fb;
+	struct fb_info *fb;
 	u16 chip_id;
 	u8  chip_rev_id;
 
@@ -94,7 +94,7 @@ struct vesa_mode {
 	u16  lfb_depth;
 };
 
-static struct vesa_mode vesa_mode_table[] = {
+static const struct vesa_mode vesa_mode_table[] = {
 	{"0x301", 640,  480,  8},
 	{"0x303", 800,  600,  8},
 	{"0x305", 1024, 768,  8},
@@ -111,6 +111,666 @@ static struct vesa_mode vesa_mode_table[] = {
 	{"0x31B", 1280, 1024, 24},
 };
 
+/**********************************************************************
+			 SM712 Mode table.
+ **********************************************************************/
+static const struct modeinit vgamode[] = {
+	{
+		/*  mode#0: 640 x 480  16Bpp  60Hz */
+		640, 480, 16, 60,
+		/*  Init_MISC */
+		0xE3,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x00, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xFF, 0xBE, 0xEF, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x30, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x32, 0x03, 0xA0, 0x09, 0xC0, 0x32, 0x32, 0x32,
+			0x32, 0x32, 0x32, 0x32, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x32, 0x32, 0x32,
+			0x04, 0x24, 0x63, 0x4F, 0x52, 0x0B, 0xDF, 0xEA,
+			0x04, 0x50, 0x19, 0x32, 0x32, 0x00, 0x00, 0x32,
+			0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x07, 0x82, 0x07, 0x04,
+			0x00, 0x45, 0x30, 0x30, 0x40, 0x30,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x32,
+			0xF7, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x32, 0x32,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
+			0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xFF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0x5F, 0x4F, 0x4F, 0x00, 0x53, 0x1F, 0x0B, 0x3E,
+			0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xEA, 0x0C, 0xDF, 0x50, 0x40, 0xDF, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x55, 0x03, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xFF, 0xFD,
+			0x5F, 0x4F, 0x00, 0x54, 0x00, 0x0B, 0xDF, 0x00,
+			0xEA, 0x0C, 0x2E, 0x00, 0x4F, 0xDF,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x56, 0xDD, 0x5E, 0xEA, 0x87, 0x44, 0x8F, 0x55,
+			0x0A, 0x8F, 0x55, 0x0A, 0x00, 0x00, 0x18, 0x00,
+			0x11, 0x10, 0x0B, 0x0A, 0x0A, 0x0A, 0x0A, 0x00,
+		},
+	},
+	{
+		/*  mode#1: 640 x 480  24Bpp  60Hz */
+		640, 480, 24, 60,
+		/*  Init_MISC */
+		0xE3,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x00, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xFF, 0xBE, 0xEF, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x30, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x32, 0x03, 0xA0, 0x09, 0xC0, 0x32, 0x32, 0x32,
+			0x32, 0x32, 0x32, 0x32, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x32, 0x32, 0x32,
+			0x04, 0x24, 0x63, 0x4F, 0x52, 0x0B, 0xDF, 0xEA,
+			0x04, 0x50, 0x19, 0x32, 0x32, 0x00, 0x00, 0x32,
+			0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x07, 0x82, 0x07, 0x04,
+			0x00, 0x45, 0x30, 0x30, 0x40, 0x30,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x32,
+			0xF7, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x32, 0x32,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
+			0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xFF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0x5F, 0x4F, 0x4F, 0x00, 0x53, 0x1F, 0x0B, 0x3E,
+			0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xEA, 0x0C, 0xDF, 0x50, 0x40, 0xDF, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x55, 0x03, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xFF, 0xFD,
+			0x5F, 0x4F, 0x00, 0x54, 0x00, 0x0B, 0xDF, 0x00,
+			0xEA, 0x0C, 0x2E, 0x00, 0x4F, 0xDF,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x56, 0xDD, 0x5E, 0xEA, 0x87, 0x44, 0x8F, 0x55,
+			0x0A, 0x8F, 0x55, 0x0A, 0x00, 0x00, 0x18, 0x00,
+			0x11, 0x10, 0x0B, 0x0A, 0x0A, 0x0A, 0x0A, 0x00,
+		},
+	},
+	{
+		/*  mode#0: 640 x 480  32Bpp  60Hz */
+		640, 480, 32, 60,
+		/*  Init_MISC */
+		0xE3,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x00, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xFF, 0xBE, 0xEF, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x30, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x32, 0x03, 0xA0, 0x09, 0xC0, 0x32, 0x32, 0x32,
+			0x32, 0x32, 0x32, 0x32, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x32, 0x32, 0x32,
+			0x04, 0x24, 0x63, 0x4F, 0x52, 0x0B, 0xDF, 0xEA,
+			0x04, 0x50, 0x19, 0x32, 0x32, 0x00, 0x00, 0x32,
+			0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x07, 0x82, 0x07, 0x04,
+			0x00, 0x45, 0x30, 0x30, 0x40, 0x30,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x32,
+			0xF7, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x32, 0x32,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
+			0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xFF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0x5F, 0x4F, 0x4F, 0x00, 0x53, 0x1F, 0x0B, 0x3E,
+			0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xEA, 0x0C, 0xDF, 0x50, 0x40, 0xDF, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x55, 0x03, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xFF, 0xFD,
+			0x5F, 0x4F, 0x00, 0x54, 0x00, 0x0B, 0xDF, 0x00,
+			0xEA, 0x0C, 0x2E, 0x00, 0x4F, 0xDF,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x56, 0xDD, 0x5E, 0xEA, 0x87, 0x44, 0x8F, 0x55,
+			0x0A, 0x8F, 0x55, 0x0A, 0x00, 0x00, 0x18, 0x00,
+			0x11, 0x10, 0x0B, 0x0A, 0x0A, 0x0A, 0x0A, 0x00,
+		},
+	},
+
+	{	/*  mode#2: 800 x 600  16Bpp  60Hz */
+		800, 600, 16, 60,
+		/*  Init_MISC */
+		0x2B,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x03, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xFF, 0xBE, 0xEE, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x30, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x34, 0x03, 0x20, 0x09, 0xC0, 0x24, 0x24, 0x24,
+			0x24, 0x24, 0x24, 0x24, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x38, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x24, 0x24, 0x24,
+			0x04, 0x48, 0x83, 0x63, 0x68, 0x72, 0x57, 0x58,
+			0x04, 0x55, 0x59, 0x24, 0x24, 0x00, 0x00, 0x24,
+			0x01, 0x80, 0x7A, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x1C, 0x85, 0x35, 0x13,
+			0x02, 0x45, 0x30, 0x35, 0x40, 0x20,
+		},
+		{	/*  Init_SR80_SR93 */
+			0x00, 0x00, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x24,
+			0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x24, 0x24,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
+			0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xBF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0x7F, 0x63, 0x63, 0x00, 0x68, 0x18, 0x72, 0xF0,
+			0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x58, 0x0C, 0x57, 0x64, 0x40, 0x57, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x03, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xBF, 0xFD,
+			0x7F, 0x63, 0x00, 0x69, 0x18, 0x72, 0x57, 0x00,
+			0x58, 0x0C, 0xE0, 0x20, 0x63, 0x57,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x56, 0x4B, 0x5E, 0x55, 0x86, 0x9D, 0x8E, 0xAA,
+			0xDB, 0x2A, 0xDF, 0x33, 0x00, 0x00, 0x18, 0x00,
+			0x20, 0x1F, 0x1A, 0x19, 0x0F, 0x0F, 0x0F, 0x00,
+		},
+	},
+	{	/*  mode#3: 800 x 600  24Bpp  60Hz */
+		800, 600, 24, 60,
+		0x2B,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x03, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xFF, 0xBE, 0xEE, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x30, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x36, 0x03, 0x20, 0x09, 0xC0, 0x36, 0x36, 0x36,
+			0x36, 0x36, 0x36, 0x36, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x36, 0x36, 0x36,
+			0x04, 0x48, 0x83, 0x63, 0x68, 0x72, 0x57, 0x58,
+			0x04, 0x55, 0x59, 0x36, 0x36, 0x00, 0x00, 0x36,
+			0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x1C, 0x85, 0x35, 0x13,
+			0x02, 0x45, 0x30, 0x30, 0x40, 0x20,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x36,
+			0xF7, 0x00, 0x00, 0x00, 0xEF, 0xFF, 0x36, 0x36,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
+			0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xBF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0x7F, 0x63, 0x63, 0x00, 0x68, 0x18, 0x72, 0xF0,
+			0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x58, 0x0C, 0x57, 0x64, 0x40, 0x57, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x03, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xBF, 0xFD,
+			0x7F, 0x63, 0x00, 0x69, 0x18, 0x72, 0x57, 0x00,
+			0x58, 0x0C, 0xE0, 0x20, 0x63, 0x57,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x56, 0x4B, 0x5E, 0x55, 0x86, 0x9D, 0x8E, 0xAA,
+			0xDB, 0x2A, 0xDF, 0x33, 0x00, 0x00, 0x18, 0x00,
+			0x20, 0x1F, 0x1A, 0x19, 0x0F, 0x0F, 0x0F, 0x00,
+		},
+	},
+	{	/*  mode#7: 800 x 600  32Bpp  60Hz */
+		800, 600, 32, 60,
+		/*  Init_MISC */
+		0x2B,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x03, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xFF, 0xBE, 0xEE, 0xFF, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x30, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x34, 0x03, 0x20, 0x09, 0xC0, 0x24, 0x24, 0x24,
+			0x24, 0x24, 0x24, 0x24, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x38, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x24, 0x24, 0x24,
+			0x04, 0x48, 0x83, 0x63, 0x68, 0x72, 0x57, 0x58,
+			0x04, 0x55, 0x59, 0x24, 0x24, 0x00, 0x00, 0x24,
+			0x01, 0x80, 0x7A, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x1C, 0x85, 0x35, 0x13,
+			0x02, 0x45, 0x30, 0x35, 0x40, 0x20,
+		},
+		{	/*  Init_SR80_SR93 */
+			0x00, 0x00, 0x00, 0x6F, 0x7F, 0x7F, 0xFF, 0x24,
+			0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x24, 0x24,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFF, 0xBF, 0xFF, 0xFF, 0xED, 0xED, 0xED,
+			0x7B, 0xFF, 0xFF, 0xFF, 0xBF, 0xEF, 0xBF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0x7F, 0x63, 0x63, 0x00, 0x68, 0x18, 0x72, 0xF0,
+			0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x58, 0x0C, 0x57, 0x64, 0x40, 0x57, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x03, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xE7, 0xBF, 0xFD,
+			0x7F, 0x63, 0x00, 0x69, 0x18, 0x72, 0x57, 0x00,
+			0x58, 0x0C, 0xE0, 0x20, 0x63, 0x57,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x56, 0x4B, 0x5E, 0x55, 0x86, 0x9D, 0x8E, 0xAA,
+			0xDB, 0x2A, 0xDF, 0x33, 0x00, 0x00, 0x18, 0x00,
+			0x20, 0x1F, 0x1A, 0x19, 0x0F, 0x0F, 0x0F, 0x00,
+		},
+	},
+	/* We use 1024x768 table to light 1024x600 panel for lemote */
+	{	/*  mode#4: 1024 x 600  16Bpp  60Hz  */
+		1024, 600, 16, 60,
+		/*  Init_MISC */
+		0xEB,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x00, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xC8, 0x40, 0x14, 0x60, 0x00, 0x0A, 0x17, 0x20,
+			0x51, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x30, 0x02, 0x00, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x22, 0x03, 0x24, 0x09, 0xC0, 0x22, 0x22, 0x22,
+			0x22, 0x22, 0x22, 0x22, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x22, 0x22, 0x22,
+			0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
+			0x00, 0x60, 0x59, 0x22, 0x22, 0x00, 0x00, 0x22,
+			0x01, 0x80, 0x7A, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x16, 0x02, 0x0D, 0x82, 0x09, 0x02,
+			0x04, 0x45, 0x3F, 0x30, 0x40, 0x20,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
+			0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
+			0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
+			0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
+			0xA3, 0x7F, 0x00, 0x82, 0x0b, 0x6f, 0x57, 0x00,
+			0x5c, 0x0f, 0xE0, 0xe0, 0x7F, 0x57,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
+			0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
+			0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
+		},
+	},
+	{	/*  mode#5: 1024 x 768  24Bpp  60Hz */
+		1024, 768, 24, 60,
+		/*  Init_MISC */
+		0xEB,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x03, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xF3, 0xB6, 0xC0, 0xDD, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x30, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x38, 0x03, 0x20, 0x09, 0xC0, 0x3A, 0x3A, 0x3A,
+			0x3A, 0x3A, 0x3A, 0x3A, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x00, 0x00, 0x3A,
+			0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
+			0x00, 0x60, 0x59, 0x3A, 0x3A, 0x00, 0x00, 0x3A,
+			0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x3B, 0x0D, 0x09, 0x02,
+			0x04, 0x45, 0x30, 0x30, 0x40, 0x20,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
+			0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
+			0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
+			0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
+			0xA3, 0x7F, 0x00, 0x86, 0x15, 0x24, 0xFF, 0x00,
+			0x01, 0x07, 0xE5, 0x20, 0x7F, 0xFF,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
+			0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
+			0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
+		},
+	},
+	{	/*  mode#4: 1024 x 768  32Bpp  60Hz */
+		1024, 768, 32, 60,
+		/*  Init_MISC */
+		0xEB,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x03, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xF3, 0xB6, 0xC0, 0xDD, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x32, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x38, 0x03, 0x20, 0x09, 0xC0, 0x3A, 0x3A, 0x3A,
+			0x3A, 0x3A, 0x3A, 0x3A, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x00, 0x00, 0x3A,
+			0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
+			0x00, 0x60, 0x59, 0x3A, 0x3A, 0x00, 0x00, 0x3A,
+			0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x3B, 0x0D, 0x09, 0x02,
+			0x04, 0x45, 0x30, 0x30, 0x40, 0x20,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
+			0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
+			0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
+			0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
+			0x00, 0x00, 0x00, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
+			0xA3, 0x7F, 0x00, 0x86, 0x15, 0x24, 0xFF, 0x00,
+			0x01, 0x07, 0xE5, 0x20, 0x7F, 0xFF,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
+			0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
+			0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
+		},
+	},
+	{	/*  mode#6: 320 x 240  16Bpp  60Hz */
+		320, 240, 16, 60,
+		/*  Init_MISC */
+		0xEB,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x03, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xF3, 0xB6, 0xC0, 0xDD, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x32, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x38, 0x03, 0x20, 0x09, 0xC0, 0x3A, 0x3A, 0x3A,
+			0x3A, 0x3A, 0x3A, 0x3A, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x00, 0x00, 0x3A,
+			0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
+			0x00, 0x60, 0x59, 0x3A, 0x3A, 0x00, 0x00, 0x3A,
+			0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x08, 0x43, 0x08, 0x43,
+			0x04, 0x45, 0x30, 0x30, 0x40, 0x20,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
+			0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
+			0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
+			0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
+			0x00, 0x00, 0x30, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
+			0x2E, 0x27, 0x00, 0x2b, 0x0c, 0x0F, 0xEF, 0x00,
+			0xFe, 0x0f, 0x01, 0xC0, 0x27, 0xEF,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
+			0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
+			0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
+		},
+	},
+
+	{	/*  mode#8: 320 x 240  32Bpp  60Hz */
+		320, 240, 32, 60,
+		/*  Init_MISC */
+		0xEB,
+		{	/*  Init_SR0_SR4 */
+			0x03, 0x01, 0x0F, 0x03, 0x0E,
+		},
+		{	/*  Init_SR10_SR24 */
+			0xF3, 0xB6, 0xC0, 0xDD, 0x00, 0x0E, 0x17, 0x2C,
+			0x99, 0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0xC4, 0x32, 0x02, 0x01, 0x01,
+		},
+		{	/*  Init_SR30_SR75 */
+			0x38, 0x03, 0x20, 0x09, 0xC0, 0x3A, 0x3A, 0x3A,
+			0x3A, 0x3A, 0x3A, 0x3A, 0x00, 0x00, 0x03, 0xFF,
+			0x00, 0xFC, 0x00, 0x00, 0x20, 0x18, 0x00, 0xFC,
+			0x20, 0x0C, 0x44, 0x20, 0x00, 0x00, 0x00, 0x3A,
+			0x06, 0x68, 0xA7, 0x7F, 0x83, 0x24, 0xFF, 0x03,
+			0x00, 0x60, 0x59, 0x3A, 0x3A, 0x00, 0x00, 0x3A,
+			0x01, 0x80, 0x7E, 0x1A, 0x1A, 0x00, 0x00, 0x00,
+			0x50, 0x03, 0x74, 0x14, 0x08, 0x43, 0x08, 0x43,
+			0x04, 0x45, 0x30, 0x30, 0x40, 0x20,
+		},
+		{	/*  Init_SR80_SR93 */
+			0xFF, 0x07, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x3A,
+			0xF7, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x3A, 0x3A,
+			0x00, 0x00, 0x00, 0x00,
+		},
+		{	/*  Init_SRA0_SRAF */
+			0x00, 0xFB, 0x9F, 0x01, 0x00, 0xED, 0xED, 0xED,
+			0x7B, 0xFB, 0xFF, 0xFF, 0x97, 0xEF, 0xBF, 0xDF,
+		},
+		{	/*  Init_GR00_GR08 */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
+			0xFF,
+		},
+		{	/*  Init_AR00_AR14 */
+			0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+			0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+			0x41, 0x00, 0x0F, 0x00, 0x00,
+		},
+		{	/*  Init_CR00_CR18 */
+			0xA3, 0x7F, 0x7F, 0x00, 0x85, 0x16, 0x24, 0xF5,
+			0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			0x03, 0x09, 0xFF, 0x80, 0x40, 0xFF, 0x00, 0xE3,
+			0xFF,
+		},
+		{	/*  Init_CR30_CR4D */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x02, 0x20,
+			0x00, 0x00, 0x30, 0x40, 0x00, 0xFF, 0xBF, 0xFF,
+			0x2E, 0x27, 0x00, 0x2b, 0x0c, 0x0F, 0xEF, 0x00,
+			0xFe, 0x0f, 0x01, 0xC0, 0x27, 0xEF,
+		},
+		{	/*  Init_CR90_CRA7 */
+			0x55, 0xD9, 0x5D, 0xE1, 0x86, 0x1B, 0x8E, 0x26,
+			0xDA, 0x8D, 0xDE, 0x94, 0x00, 0x00, 0x18, 0x00,
+			0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x15, 0x03,
+		},
+	},
+};
+
 static struct screen_info smtc_scr_info;
 
 static char *mode_option;
@@ -249,41 +909,39 @@ static int smtc_setcolreg(unsigned regno, unsigned red, unsigned green,
 	if (regno > 255)
 		return 1;
 
-	switch (sfb->fb.fix.visual) {
+	switch (sfb->fb->fix.visual) {
 	case FB_VISUAL_DIRECTCOLOR:
 	case FB_VISUAL_TRUECOLOR:
 		/*
 		 * 16/32 bit true-colour, use pseudo-palette for 16 base color
 		 */
-		if (regno < 16) {
-			if (sfb->fb.var.bits_per_pixel == 16) {
-				u32 *pal = sfb->fb.pseudo_palette;
+		if (regno >= 16)
+			break;
+		if (sfb->fb->var.bits_per_pixel == 16) {
+			u32 *pal = sfb->fb->pseudo_palette;
 
-				val = chan_to_field(red, &sfb->fb.var.red);
-				val |= chan_to_field(green, &sfb->fb.var.green);
-				val |= chan_to_field(blue, &sfb->fb.var.blue);
+			val = chan_to_field(red, &sfb->fb->var.red);
+			val |= chan_to_field(green, &sfb->fb->var.green);
+			val |= chan_to_field(blue, &sfb->fb->var.blue);
 #ifdef __BIG_ENDIAN
-				pal[regno] =
-				    ((red & 0xf800) >> 8) |
-				    ((green & 0xe000) >> 13) |
-				    ((green & 0x1c00) << 3) |
-				    ((blue & 0xf800) >> 3);
+			pal[regno] = ((red & 0xf800) >> 8) |
+				     ((green & 0xe000) >> 13) |
+				     ((green & 0x1c00) << 3) |
+				     ((blue & 0xf800) >> 3);
 #else
-				pal[regno] = val;
+			pal[regno] = val;
 #endif
-			} else {
-				u32 *pal = sfb->fb.pseudo_palette;
+		} else {
+			u32 *pal = sfb->fb->pseudo_palette;
 
-				val = chan_to_field(red, &sfb->fb.var.red);
-				val |= chan_to_field(green, &sfb->fb.var.green);
-				val |= chan_to_field(blue, &sfb->fb.var.blue);
+			val = chan_to_field(red, &sfb->fb->var.red);
+			val |= chan_to_field(green, &sfb->fb->var.green);
+			val |= chan_to_field(blue, &sfb->fb->var.blue);
 #ifdef __BIG_ENDIAN
-				val =
-				    (val & 0xff00ff00 >> 8) |
-				    (val & 0x00ff00ff << 8);
+			val = (val & 0xff00ff00 >> 8) |
+			      (val & 0x00ff00ff << 8);
 #endif
-				pal[regno] = val;
-			}
+			pal[regno] = val;
 		}
 		break;
 
@@ -300,8 +958,8 @@ static int smtc_setcolreg(unsigned regno, unsigned red, unsigned green,
 }
 
 #ifdef __BIG_ENDIAN
-static ssize_t smtcfb_read(struct fb_info *info, char __user *buf, size_t
-				count, loff_t *ppos)
+static ssize_t smtcfb_read(struct fb_info *info, char __user *buf,
+			   size_t count, loff_t *ppos)
 {
 	unsigned long p = *ppos;
 
@@ -334,7 +992,7 @@ static ssize_t smtcfb_read(struct fb_info *info, char __user *buf, size_t
 	if (!buffer)
 		return -ENOMEM;
 
-	src = (u32 __iomem *) (info->screen_base + p);
+	src = (u32 __iomem *)(info->screen_base + p);
 
 	if (info->fbops->fb_sync)
 		info->fbops->fb_sync(info);
@@ -344,9 +1002,8 @@ static ssize_t smtcfb_read(struct fb_info *info, char __user *buf, size_t
 		dst = buffer;
 		for (i = c >> 2; i--;) {
 			*dst = fb_readl(src++);
-			*dst =
-			    (*dst & 0xff00ff00 >> 8) |
-			    (*dst & 0x00ff00ff << 8);
+			*dst = (*dst & 0xff00ff00 >> 8) |
+			       (*dst & 0x00ff00ff << 8);
 			dst++;
 		}
 		if (c & 3) {
@@ -379,9 +1036,8 @@ static ssize_t smtcfb_read(struct fb_info *info, char __user *buf, size_t
 	return (err) ? err : cnt;
 }
 
-static ssize_t
-smtcfb_write(struct fb_info *info, const char __user *buf, size_t count,
-	     loff_t *ppos)
+static ssize_t smtcfb_write(struct fb_info *info, const char __user *buf,
+			    size_t count, loff_t *ppos)
 {
 	unsigned long p = *ppos;
 
@@ -420,7 +1076,7 @@ smtcfb_write(struct fb_info *info, const char __user *buf, size_t count,
 	if (!buffer)
 		return -ENOMEM;
 
-	dst = (u32 __iomem *) (info->screen_base + p);
+	dst = (u32 __iomem *)(info->screen_base + p);
 
 	if (info->fbops->fb_sync)
 		info->fbops->fb_sync(info);
@@ -472,76 +1128,71 @@ static void sm7xx_set_timing(struct smtcfb_info *sfb)
 	u32 m_nscreenstride;
 
 	dev_dbg(&sfb->pdev->dev,
-		"sfb->width=%d sfb->height=%d sfb->fb.var.bits_per_pixel=%d sfb->hz=%d\n",
-		sfb->width, sfb->height, sfb->fb.var.bits_per_pixel, sfb->hz);
-
-	for (j = 0; j < numvgamodes; j++) {
-		if (vgamode[j].mmsizex == sfb->width &&
-		    vgamode[j].mmsizey == sfb->height &&
-		    vgamode[j].bpp == sfb->fb.var.bits_per_pixel &&
-		    vgamode[j].hz == sfb->hz) {
-			dev_dbg(&sfb->pdev->dev,
-				"vgamode[j].mmsizex=%d vgamode[j].mmSizeY=%d vgamode[j].bpp=%d vgamode[j].hz=%d\n",
-				vgamode[j].mmsizex, vgamode[j].mmsizey,
-				vgamode[j].bpp, vgamode[j].hz);
-
-			dev_dbg(&sfb->pdev->dev, "vgamode index=%d\n", j);
-
-			smtc_mmiowb(0x0, 0x3c6);
-
-			smtc_seqw(0, 0x1);
-
-			smtc_mmiowb(vgamode[j].init_misc, 0x3c2);
-
-			/* init SEQ register SR00 - SR04 */
-			for (i = 0; i < SIZE_SR00_SR04; i++)
-				smtc_seqw(i, vgamode[j].init_sr00_sr04[i]);
-
-			/* init SEQ register SR10 - SR24 */
-			for (i = 0; i < SIZE_SR10_SR24; i++)
-				smtc_seqw(i + 0x10,
-					  vgamode[j].init_sr10_sr24[i]);
-
-			/* init SEQ register SR30 - SR75 */
-			for (i = 0; i < SIZE_SR30_SR75; i++)
-				if ((i + 0x30) != 0x62 &&
-				    (i + 0x30) != 0x6a &&
-				    (i + 0x30) != 0x6b)
-					smtc_seqw(i + 0x30,
-						  vgamode[j].init_sr30_sr75[i]);
-
-			/* init SEQ register SR80 - SR93 */
-			for (i = 0; i < SIZE_SR80_SR93; i++)
-				smtc_seqw(i + 0x80,
-					  vgamode[j].init_sr80_sr93[i]);
-
-			/* init SEQ register SRA0 - SRAF */
-			for (i = 0; i < SIZE_SRA0_SRAF; i++)
-				smtc_seqw(i + 0xa0,
-					  vgamode[j].init_sra0_sraf[i]);
-
-			/* init Graphic register GR00 - GR08 */
-			for (i = 0; i < SIZE_GR00_GR08; i++)
-				smtc_grphw(i, vgamode[j].init_gr00_gr08[i]);
-
-			/* init Attribute register AR00 - AR14 */
-			for (i = 0; i < SIZE_AR00_AR14; i++)
-				smtc_attrw(i, vgamode[j].init_ar00_ar14[i]);
-
-			/* init CRTC register CR00 - CR18 */
-			for (i = 0; i < SIZE_CR00_CR18; i++)
-				smtc_crtcw(i, vgamode[j].init_cr00_cr18[i]);
-
-			/* init CRTC register CR30 - CR4D */
-			for (i = 0; i < SIZE_CR30_CR4D; i++)
-				smtc_crtcw(i + 0x30,
-					   vgamode[j].init_cr30_cr4d[i]);
-
-			/* init CRTC register CR90 - CRA7 */
-			for (i = 0; i < SIZE_CR90_CRA7; i++)
-				smtc_crtcw(i + 0x90,
-					   vgamode[j].init_cr90_cra7[i]);
-		}
+		"sfb->width=%d sfb->height=%d sfb->fb->var.bits_per_pixel=%d sfb->hz=%d\n",
+		sfb->width, sfb->height, sfb->fb->var.bits_per_pixel, sfb->hz);
+
+	for (j = 0; j < ARRAY_SIZE(vgamode); j++) {
+		if (vgamode[j].mmsizex != sfb->width ||
+		    vgamode[j].mmsizey != sfb->height ||
+		    vgamode[j].bpp != sfb->fb->var.bits_per_pixel ||
+		    vgamode[j].hz != sfb->hz)
+			continue;
+
+		dev_dbg(&sfb->pdev->dev,
+			"vgamode[j].mmsizex=%d vgamode[j].mmSizeY=%d vgamode[j].bpp=%d vgamode[j].hz=%d\n",
+			vgamode[j].mmsizex, vgamode[j].mmsizey,
+			vgamode[j].bpp, vgamode[j].hz);
+
+		dev_dbg(&sfb->pdev->dev, "vgamode index=%d\n", j);
+
+		smtc_mmiowb(0x0, 0x3c6);
+
+		smtc_seqw(0, 0x1);
+
+		smtc_mmiowb(vgamode[j].init_misc, 0x3c2);
+
+		/* init SEQ register SR00 - SR04 */
+		for (i = 0; i < SIZE_SR00_SR04; i++)
+			smtc_seqw(i, vgamode[j].init_sr00_sr04[i]);
+
+		/* init SEQ register SR10 - SR24 */
+		for (i = 0; i < SIZE_SR10_SR24; i++)
+			smtc_seqw(i + 0x10, vgamode[j].init_sr10_sr24[i]);
+
+		/* init SEQ register SR30 - SR75 */
+		for (i = 0; i < SIZE_SR30_SR75; i++)
+			if ((i + 0x30) != 0x62 && (i + 0x30) != 0x6a &&
+			    (i + 0x30) != 0x6b)
+				smtc_seqw(i + 0x30,
+					  vgamode[j].init_sr30_sr75[i]);
+
+		/* init SEQ register SR80 - SR93 */
+		for (i = 0; i < SIZE_SR80_SR93; i++)
+			smtc_seqw(i + 0x80, vgamode[j].init_sr80_sr93[i]);
+
+		/* init SEQ register SRA0 - SRAF */
+		for (i = 0; i < SIZE_SRA0_SRAF; i++)
+			smtc_seqw(i + 0xa0, vgamode[j].init_sra0_sraf[i]);
+
+		/* init Graphic register GR00 - GR08 */
+		for (i = 0; i < SIZE_GR00_GR08; i++)
+			smtc_grphw(i, vgamode[j].init_gr00_gr08[i]);
+
+		/* init Attribute register AR00 - AR14 */
+		for (i = 0; i < SIZE_AR00_AR14; i++)
+			smtc_attrw(i, vgamode[j].init_ar00_ar14[i]);
+
+		/* init CRTC register CR00 - CR18 */
+		for (i = 0; i < SIZE_CR00_CR18; i++)
+			smtc_crtcw(i, vgamode[j].init_cr00_cr18[i]);
+
+		/* init CRTC register CR30 - CR4D */
+		for (i = 0; i < SIZE_CR30_CR4D; i++)
+			smtc_crtcw(i + 0x30, vgamode[j].init_cr30_cr4d[i]);
+
+		/* init CRTC register CR90 - CRA7 */
+		for (i = 0; i < SIZE_CR90_CRA7; i++)
+			smtc_crtcw(i + 0x90, vgamode[j].init_cr90_cra7[i]);
 	}
 	smtc_mmiowb(0x67, 0x3c2);
 
@@ -550,9 +1201,8 @@ static void sm7xx_set_timing(struct smtcfb_info *sfb)
 	writel(0x0, sfb->vp_regs + 0x40);
 
 	/* set data width */
-	m_nscreenstride =
-		(sfb->width * sfb->fb.var.bits_per_pixel) / 64;
-	switch (sfb->fb.var.bits_per_pixel) {
+	m_nscreenstride = (sfb->width * sfb->fb->var.bits_per_pixel) / 64;
+	switch (sfb->fb->var.bits_per_pixel) {
 	case 8:
 		writel(0x0, sfb->vp_regs + 0x0);
 		break;
@@ -566,7 +1216,7 @@ static void sm7xx_set_timing(struct smtcfb_info *sfb)
 		writel(0x00030000, sfb->vp_regs + 0x0);
 		break;
 	}
-	writel((u32) (((m_nscreenstride + 2) << 16) | m_nscreenstride),
+	writel((u32)(((m_nscreenstride + 2) << 16) | m_nscreenstride),
 	       sfb->vp_regs + 0x10);
 }
 
@@ -583,52 +1233,52 @@ static void smtc_set_timing(struct smtcfb_info *sfb)
 
 static void smtcfb_setmode(struct smtcfb_info *sfb)
 {
-	switch (sfb->fb.var.bits_per_pixel) {
+	switch (sfb->fb->var.bits_per_pixel) {
 	case 32:
-		sfb->fb.fix.visual       = FB_VISUAL_TRUECOLOR;
-		sfb->fb.fix.line_length  = sfb->fb.var.xres * 4;
-		sfb->fb.var.red.length   = 8;
-		sfb->fb.var.green.length = 8;
-		sfb->fb.var.blue.length  = 8;
-		sfb->fb.var.red.offset   = 16;
-		sfb->fb.var.green.offset = 8;
-		sfb->fb.var.blue.offset  = 0;
+		sfb->fb->fix.visual       = FB_VISUAL_TRUECOLOR;
+		sfb->fb->fix.line_length  = sfb->fb->var.xres * 4;
+		sfb->fb->var.red.length   = 8;
+		sfb->fb->var.green.length = 8;
+		sfb->fb->var.blue.length  = 8;
+		sfb->fb->var.red.offset   = 16;
+		sfb->fb->var.green.offset = 8;
+		sfb->fb->var.blue.offset  = 0;
 		break;
 	case 24:
-		sfb->fb.fix.visual       = FB_VISUAL_TRUECOLOR;
-		sfb->fb.fix.line_length  = sfb->fb.var.xres * 3;
-		sfb->fb.var.red.length   = 8;
-		sfb->fb.var.green.length = 8;
-		sfb->fb.var.blue.length  = 8;
-		sfb->fb.var.red.offset   = 16;
-		sfb->fb.var.green.offset = 8;
-		sfb->fb.var.blue.offset  = 0;
+		sfb->fb->fix.visual       = FB_VISUAL_TRUECOLOR;
+		sfb->fb->fix.line_length  = sfb->fb->var.xres * 3;
+		sfb->fb->var.red.length   = 8;
+		sfb->fb->var.green.length = 8;
+		sfb->fb->var.blue.length  = 8;
+		sfb->fb->var.red.offset   = 16;
+		sfb->fb->var.green.offset = 8;
+		sfb->fb->var.blue.offset  = 0;
 		break;
 	case 8:
-		sfb->fb.fix.visual       = FB_VISUAL_PSEUDOCOLOR;
-		sfb->fb.fix.line_length  = sfb->fb.var.xres;
-		sfb->fb.var.red.length   = 3;
-		sfb->fb.var.green.length = 3;
-		sfb->fb.var.blue.length  = 2;
-		sfb->fb.var.red.offset   = 5;
-		sfb->fb.var.green.offset = 2;
-		sfb->fb.var.blue.offset  = 0;
+		sfb->fb->fix.visual       = FB_VISUAL_PSEUDOCOLOR;
+		sfb->fb->fix.line_length  = sfb->fb->var.xres;
+		sfb->fb->var.red.length   = 3;
+		sfb->fb->var.green.length = 3;
+		sfb->fb->var.blue.length  = 2;
+		sfb->fb->var.red.offset   = 5;
+		sfb->fb->var.green.offset = 2;
+		sfb->fb->var.blue.offset  = 0;
 		break;
 	case 16:
 	default:
-		sfb->fb.fix.visual       = FB_VISUAL_TRUECOLOR;
-		sfb->fb.fix.line_length  = sfb->fb.var.xres * 2;
-		sfb->fb.var.red.length   = 5;
-		sfb->fb.var.green.length = 6;
-		sfb->fb.var.blue.length  = 5;
-		sfb->fb.var.red.offset   = 11;
-		sfb->fb.var.green.offset = 5;
-		sfb->fb.var.blue.offset  = 0;
+		sfb->fb->fix.visual       = FB_VISUAL_TRUECOLOR;
+		sfb->fb->fix.line_length  = sfb->fb->var.xres * 2;
+		sfb->fb->var.red.length   = 5;
+		sfb->fb->var.green.length = 6;
+		sfb->fb->var.blue.length  = 5;
+		sfb->fb->var.red.offset   = 11;
+		sfb->fb->var.green.offset = 5;
+		sfb->fb->var.blue.offset  = 0;
 		break;
 	}
 
-	sfb->width  = sfb->fb.var.xres;
-	sfb->height = sfb->fb.var.yres;
+	sfb->width  = sfb->fb->var.xres;
+	sfb->height = sfb->fb->var.yres;
 	sfb->hz = 60;
 	smtc_set_timing(sfb);
 }
@@ -673,38 +1323,6 @@ static struct fb_ops smtcfb_ops = {
 };
 
 /*
- * alloc struct smtcfb_info and assign default values
- */
-static struct smtcfb_info *smtc_alloc_fb_info(struct pci_dev *pdev)
-{
-	struct smtcfb_info *sfb;
-
-	sfb = kzalloc(sizeof(*sfb), GFP_KERNEL);
-
-	if (!sfb)
-		return NULL;
-
-	sfb->pdev = pdev;
-
-	sfb->fb.flags          = FBINFO_FLAG_DEFAULT;
-	sfb->fb.fbops          = &smtcfb_ops;
-	sfb->fb.fix            = smtcfb_fix;
-	sfb->fb.var            = smtcfb_var;
-	sfb->fb.pseudo_palette = sfb->colreg;
-	sfb->fb.par            = sfb;
-
-	return sfb;
-}
-
-/*
- * free struct smtcfb_info
- */
-static void smtc_free_fb_info(struct smtcfb_info *sfb)
-{
-	kfree(sfb);
-}
-
-/*
  * Unmap in the memory mapped IO registers
  */
 
@@ -721,20 +1339,20 @@ static void smtc_unmap_mmio(struct smtcfb_info *sfb)
 static int smtc_map_smem(struct smtcfb_info *sfb,
 			 struct pci_dev *pdev, u_long smem_len)
 {
-	sfb->fb.fix.smem_start = pci_resource_start(pdev, 0);
+	sfb->fb->fix.smem_start = pci_resource_start(pdev, 0);
 
 #ifdef __BIG_ENDIAN
-	if (sfb->fb.var.bits_per_pixel == 32)
-		sfb->fb.fix.smem_start += 0x800000;
+	if (sfb->fb->var.bits_per_pixel == 32)
+		sfb->fb->fix.smem_start += 0x800000;
 #endif
 
-	sfb->fb.fix.smem_len = smem_len;
+	sfb->fb->fix.smem_len = smem_len;
 
-	sfb->fb.screen_base = sfb->lfb;
+	sfb->fb->screen_base = sfb->lfb;
 
-	if (!sfb->fb.screen_base) {
+	if (!sfb->fb->screen_base) {
 		dev_err(&pdev->dev,
-			"%s: unable to map screen memory\n", sfb->fb.fix.id);
+			"%s: unable to map screen memory\n", sfb->fb->fix.id);
 		return -ENOMEM;
 	}
 
@@ -747,9 +1365,9 @@ static int smtc_map_smem(struct smtcfb_info *sfb,
  */
 static void smtc_unmap_smem(struct smtcfb_info *sfb)
 {
-	if (sfb && sfb->fb.screen_base) {
-		iounmap(sfb->fb.screen_base);
-		sfb->fb.screen_base = NULL;
+	if (sfb && sfb->fb->screen_base) {
+		iounmap(sfb->fb->screen_base);
+		sfb->fb->screen_base = NULL;
 	}
 }
 
@@ -766,11 +1384,12 @@ static int smtcfb_pci_probe(struct pci_dev *pdev,
 			    const struct pci_device_id *ent)
 {
 	struct smtcfb_info *sfb;
+	struct fb_info *info;
 	u_long smem_size = 0x00800000;	/* default 8MB */
 	int err;
 	unsigned long mmio_base;
 
-	dev_info(&pdev->dev, "Silicon Motion display driver.");
+	dev_info(&pdev->dev, "Silicon Motion display driver.\n");
 
 	err = pci_enable_device(pdev);	/* enable SMTC chip */
 	if (err)
@@ -784,14 +1403,23 @@ static int smtcfb_pci_probe(struct pci_dev *pdev,
 
 	sprintf(smtcfb_fix.id, "sm%Xfb", ent->device);
 
-	sfb = smtc_alloc_fb_info(pdev);
-
-	if (!sfb) {
+	info = framebuffer_alloc(sizeof(*sfb), &pdev->dev);
+	if (!info) {
+		dev_err(&pdev->dev, "framebuffer_alloc failed\n");
 		err = -ENOMEM;
 		goto failed_free;
 	}
 
+	sfb = info->par;
+	sfb->fb = info;
 	sfb->chip_id = ent->device;
+	sfb->pdev = pdev;
+	info->flags = FBINFO_FLAG_DEFAULT;
+	info->fbops = &smtcfb_ops;
+	info->fix = smtcfb_fix;
+	info->var = smtcfb_var;
+	info->pseudo_palette = sfb->colreg;
+	info->par = sfb;
 
 	pci_set_drvdata(pdev, sfb);
 
@@ -799,19 +1427,19 @@ static int smtcfb_pci_probe(struct pci_dev *pdev,
 
 	/* get mode parameter from smtc_scr_info */
 	if (smtc_scr_info.lfb_width != 0) {
-		sfb->fb.var.xres = smtc_scr_info.lfb_width;
-		sfb->fb.var.yres = smtc_scr_info.lfb_height;
-		sfb->fb.var.bits_per_pixel = smtc_scr_info.lfb_depth;
+		sfb->fb->var.xres = smtc_scr_info.lfb_width;
+		sfb->fb->var.yres = smtc_scr_info.lfb_height;
+		sfb->fb->var.bits_per_pixel = smtc_scr_info.lfb_depth;
 	} else {
 		/* default resolution 1024x600 16bit mode */
-		sfb->fb.var.xres = SCREEN_X_RES;
-		sfb->fb.var.yres = SCREEN_Y_RES;
-		sfb->fb.var.bits_per_pixel = SCREEN_BPP;
+		sfb->fb->var.xres = SCREEN_X_RES;
+		sfb->fb->var.yres = SCREEN_Y_RES;
+		sfb->fb->var.bits_per_pixel = SCREEN_BPP;
 	}
 
 #ifdef __BIG_ENDIAN
-	if (sfb->fb.var.bits_per_pixel == 24)
-		sfb->fb.var.bits_per_pixel = (smtc_scr_info.lfb_depth = 32);
+	if (sfb->fb->var.bits_per_pixel == 24)
+		sfb->fb->var.bits_per_pixel = (smtc_scr_info.lfb_depth = 32);
 #endif
 	/* Map address and memory detection */
 	mmio_base = pci_resource_start(pdev, 0);
@@ -820,8 +1448,8 @@ static int smtcfb_pci_probe(struct pci_dev *pdev,
 	switch (sfb->chip_id) {
 	case 0x710:
 	case 0x712:
-		sfb->fb.fix.mmio_start = mmio_base + 0x00400000;
-		sfb->fb.fix.mmio_len = 0x00400000;
+		sfb->fb->fix.mmio_start = mmio_base + 0x00400000;
+		sfb->fb->fix.mmio_len = 0x00400000;
 		smem_size = SM712_VIDEOMEMORYSIZE;
 #ifdef __BIG_ENDIAN
 		sfb->lfb = ioremap(mmio_base, 0x00c00000);
@@ -833,15 +1461,15 @@ static int smtcfb_pci_probe(struct pci_dev *pdev,
 		sfb->dp_regs = sfb->lfb + 0x00408000;
 		sfb->vp_regs = sfb->lfb + 0x0040c000;
 #ifdef __BIG_ENDIAN
-		if (sfb->fb.var.bits_per_pixel == 32) {
+		if (sfb->fb->var.bits_per_pixel == 32) {
 			sfb->lfb += 0x800000;
-			dev_info(&pdev->dev, "sfb->lfb=%p", sfb->lfb);
+			dev_info(&pdev->dev, "sfb->lfb=%p\n", sfb->lfb);
 		}
 #endif
 		if (!smtc_regbaseaddress) {
 			dev_err(&pdev->dev,
-				"%s: unable to map memory mapped IO!",
-				sfb->fb.fix.id);
+				"%s: unable to map memory mapped IO!\n",
+				sfb->fb->fix.id);
 			err = -ENOMEM;
 			goto failed_fb;
 		}
@@ -854,13 +1482,13 @@ static int smtcfb_pci_probe(struct pci_dev *pdev,
 		smtc_seqw(0x17, 0x20);
 		/* enable word swap */
 #ifdef __BIG_ENDIAN
-		if (sfb->fb.var.bits_per_pixel == 32)
+		if (sfb->fb->var.bits_per_pixel == 32)
 			smtc_seqw(0x17, 0x30);
 #endif
 		break;
 	case 0x720:
-		sfb->fb.fix.mmio_start = mmio_base;
-		sfb->fb.fix.mmio_len = 0x00200000;
+		sfb->fb->fix.mmio_start = mmio_base;
+		sfb->fb->fix.mmio_len = 0x00200000;
 		smem_size = SM722_VIDEOMEMORYSIZE;
 		sfb->dp_regs = ioremap(mmio_base, 0x00a00000);
 		sfb->lfb = sfb->dp_regs + 0x00200000;
@@ -874,41 +1502,41 @@ static int smtcfb_pci_probe(struct pci_dev *pdev,
 		break;
 	default:
 		dev_err(&pdev->dev,
-			"No valid Silicon Motion display chip was detected!");
+			"No valid Silicon Motion display chip was detected!\n");
 
 		goto failed_fb;
 	}
 
 	/* can support 32 bpp */
-	if (15 == sfb->fb.var.bits_per_pixel)
-		sfb->fb.var.bits_per_pixel = 16;
+	if (15 == sfb->fb->var.bits_per_pixel)
+		sfb->fb->var.bits_per_pixel = 16;
 
-	sfb->fb.var.xres_virtual = sfb->fb.var.xres;
-	sfb->fb.var.yres_virtual = sfb->fb.var.yres;
+	sfb->fb->var.xres_virtual = sfb->fb->var.xres;
+	sfb->fb->var.yres_virtual = sfb->fb->var.yres;
 	err = smtc_map_smem(sfb, pdev, smem_size);
 	if (err)
 		goto failed;
 
 	smtcfb_setmode(sfb);
 
-	err = register_framebuffer(&sfb->fb);
+	err = register_framebuffer(info);
 	if (err < 0)
 		goto failed;
 
 	dev_info(&pdev->dev,
-		 "Silicon Motion SM%X Rev%X primary display mode %dx%d-%d Init Complete.",
-		 sfb->chip_id, sfb->chip_rev_id, sfb->fb.var.xres,
-		 sfb->fb.var.yres, sfb->fb.var.bits_per_pixel);
+		 "Silicon Motion SM%X Rev%X primary display mode %dx%d-%d Init Complete.\n",
+		 sfb->chip_id, sfb->chip_rev_id, sfb->fb->var.xres,
+		 sfb->fb->var.yres, sfb->fb->var.bits_per_pixel);
 
 	return 0;
 
 failed:
-	dev_err(&pdev->dev, "Silicon Motion, Inc. primary display init fail.");
+	dev_err(&pdev->dev, "Silicon Motion, Inc. primary display init fail.\n");
 
 	smtc_unmap_smem(sfb);
 	smtc_unmap_mmio(sfb);
 failed_fb:
-	smtc_free_fb_info(sfb);
+	framebuffer_release(info);
 
 failed_free:
 	pci_release_region(pdev, 0);
@@ -940,8 +1568,8 @@ static void smtcfb_pci_remove(struct pci_dev *pdev)
 	sfb = pci_get_drvdata(pdev);
 	smtc_unmap_smem(sfb);
 	smtc_unmap_mmio(sfb);
-	unregister_framebuffer(&sfb->fb);
-	smtc_free_fb_info(sfb);
+	unregister_framebuffer(sfb->fb);
+	framebuffer_release(sfb->fb);
 	pci_release_region(pdev, 0);
 	pci_disable_device(pdev);
 }
@@ -961,7 +1589,7 @@ static int smtcfb_pci_suspend(struct device *device)
 	smtc_seqw(0x69, (smtc_seqr(0x69) & 0xf7));
 
 	console_lock();
-	fb_set_suspend(&sfb->fb, 1);
+	fb_set_suspend(sfb->fb, 1);
 	console_unlock();
 
 	/* additionally turn off all function blocks including internal PLLs */
@@ -989,7 +1617,7 @@ static int smtcfb_pci_resume(struct device *device)
 		/* enable PCI burst */
 		smtc_seqw(0x17, 0x20);
 #ifdef __BIG_ENDIAN
-		if (sfb->fb.var.bits_per_pixel == 32)
+		if (sfb->fb->var.bits_per_pixel == 32)
 			smtc_seqw(0x17, 0x30);
 #endif
 		break;
@@ -1006,7 +1634,7 @@ static int smtcfb_pci_resume(struct device *device)
 	smtcfb_setmode(sfb);
 
 	console_lock();
-	fb_set_suspend(&sfb->fb, 0);
+	fb_set_suspend(sfb->fb, 0);
 	console_unlock();
 
 	return 0;
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index c955976414ee..6c4f9a1ed07f 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -1597,7 +1597,7 @@ static int count_highlight_color(struct vc_data *vc)
 static int get_highlight_color(struct vc_data *vc)
 {
 	int i, j;
-	unsigned int cptr[8], tmp;
+	unsigned int cptr[8];
 	int vc_num = vc->vc_num;
 
 	for (i = 0; i < 8; i++)
@@ -1606,11 +1606,8 @@ static int get_highlight_color(struct vc_data *vc)
 	for (i = 0; i < 7; i++)
 		for (j = i + 1; j < 8; j++)
 			if (speakup_console[vc_num]->ht.bgcount[cptr[i]] >
-			    speakup_console[vc_num]->ht.bgcount[cptr[j]]) {
-				tmp = cptr[i];
-				cptr[i] = cptr[j];
-				cptr[j] = tmp;
-			}
+			    speakup_console[vc_num]->ht.bgcount[cptr[j]])
+				swap(cptr[i], cptr[j]);
 
 	for (i = 0; i < 8; i++)
 		if (speakup_console[vc_num]->ht.bgcount[cptr[i]] != 0)
diff --git a/drivers/staging/unisys/Kconfig b/drivers/staging/unisys/Kconfig
index 19fcb3465509..778f9d05f98a 100644
--- a/drivers/staging/unisys/Kconfig
+++ b/drivers/staging/unisys/Kconfig
@@ -4,16 +4,14 @@
 menuconfig UNISYSSPAR
 	bool "Unisys SPAR driver support"
 	depends on X86_64
+	select PCI
+	select ACPI
 	---help---
 	Support for the Unisys SPAR drivers
 
 if UNISYSSPAR
 
-source "drivers/staging/unisys/visorutil/Kconfig"
-source "drivers/staging/unisys/visorchannel/Kconfig"
-source "drivers/staging/unisys/visorchipset/Kconfig"
-source "drivers/staging/unisys/uislib/Kconfig"
-source "drivers/staging/unisys/virtpci/Kconfig"
-source "drivers/staging/unisys/virthba/Kconfig"
+source "drivers/staging/unisys/visorbus/Kconfig"
+source "drivers/staging/unisys/visornic/Kconfig"
 
 endif # UNISYSSPAR
diff --git a/drivers/staging/unisys/Makefile b/drivers/staging/unisys/Makefile
index 68b9925e7d5e..a515ebc4f8ec 100644
--- a/drivers/staging/unisys/Makefile
+++ b/drivers/staging/unisys/Makefile
@@ -1,9 +1,5 @@
 #
 # Makefile for Unisys SPAR drivers
 #
-obj-$(CONFIG_UNISYS_VISORUTIL)		+= visorutil/
-obj-$(CONFIG_UNISYS_VISORCHANNEL)	+= visorchannel/
-obj-$(CONFIG_UNISYS_VISORCHIPSET)	+= visorchipset/
-obj-$(CONFIG_UNISYS_UISLIB)		+= uislib/
-obj-$(CONFIG_UNISYS_VIRTPCI)		+= virtpci/
-obj-$(CONFIG_UNISYS_VIRTHBA)		+= virthba/
+obj-$(CONFIG_UNISYS_VISORBUS)		+= visorbus/
+obj-$(CONFIG_UNISYS_VISORNIC)		+= visornic/
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlframework.h b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
deleted file mode 100644
index 33d9caf337c8..000000000000
--- a/drivers/staging/unisys/common-spar/include/channels/controlframework.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*
- * Module Name:
- *  controlframework.h
- *
- * Abstract: This file defines common structures in the unmanaged
- *	     Ultravisor (mostly EFI) space.
- *
- */
-
-#ifndef _CONTROL_FRAMEWORK_H_
-#define _CONTROL_FRAMEWORK_H_
-
-#include <linux/types.h>
-#include "channel.h"
-
-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
-				 * ShuttingDown */
-	u16 allocated:1;	/* Bit 4: memory (device/port number)
-				 * has been selected by Command */
-	u16 known:1;		/* Bit 5: has been introduced to the
-				 * service/guest partition */
-	u16 ready:1;		/* Bit 6: service/Guest partition has
-				 * responded to introduction */
-	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 */
-};
-
-static const struct spar_segment_state segment_state_running = {
-	1, 1, 1, 0, 1, 1, 1, 1
-};
-
-static const struct spar_segment_state segment_state_paused = {
-	1, 1, 1, 0, 1, 1, 1, 0
-};
-
-static const struct spar_segment_state segment_state_standby = {
-	1, 1, 0, 0, 1, 1, 1, 0
-};
-
-#endif				/* _CONTROL_FRAMEWORK_H_ not defined */
diff --git a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
deleted file mode 100644
index e8fb8678a8e2..000000000000
--- a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
+++ /dev/null
@@ -1,427 +0,0 @@
-/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*++
- *
- * Module Name:
- *
- * diagchannel.h
- *
- * Abstract:
- *
- * This file defines the DiagChannel protocol.  This protocol is used to aid in
- * preserving event data sent by external applications.  This protocol provides
- * a region for event data to reside in.  This data will eventually be sent to
- * the Boot Partition where it will be committed to memory and/or disk.  This
- * file contains platform-independent data that can be built using any
- * Supervisor build environment (Windows, Linux, EFI).
- *
-*/
-
-#ifndef _DIAG_CHANNEL_H_
-#define _DIAG_CHANNEL_H_
-
-#include <linux/uuid.h>
-#include "channel.h"
-
-/* {EEA7A573-DB82-447c-8716-EFBEAAAE4858} */
-#define SPAR_DIAG_CHANNEL_PROTOCOL_UUID \
-		UUID_LE(0xeea7a573, 0xdb82, 0x447c, \
-				0x87, 0x16, 0xef, 0xbe, 0xaa, 0xae, 0x48, 0x58)
-
-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 \
-		UUID_LE(0xe850f968, 0x3263, 0x4484, \
-				0x8c, 0xa5, 0x2a, 0x35, 0xd0, 0x87, 0xa5, 0xa8)
-
-#define ULTRA_DIAG_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_DIAG_CHANNEL_PROTOCOL_VERSIONID 2
-
-#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... */
-#define MAX_SUBSYSTEMS 64	/* Maximum number of subsystems allowed in
-				 * DiagChannel... */
-#define LOW_SUBSYSTEMS 32	/* Half of MAX_SUBSYSTEMS to allow 64-bit
-				 * math */
-#define SUBSYSTEM_DEBUG 0	/* Standard subsystem for debug events */
-#define SUBSYSTEM_DEFAULT 1	/* Default subsystem for legacy calls to
-				 * ReportEvent */
-
-/* few useful subsystem mask values */
-#define SUBSYSTEM_MASK_DEBUG	0x01	/* Standard subsystem for debug
-					 * events */
-#define SUBSYSTEM_MASK_DEFAULT  0x02	/* Default subsystem for legacy calls to
-					 * ReportEvents */
-
-/* Event parameter "Severity" is overloaded with Cause in byte 2 and Severity in
- * byte 0, bytes 1 and 3 are reserved */
-#define SEVERITY_MASK 0x0FF	/* mask out all but the Severity in byte 0 */
-#define CAUSE_MASK 0x0FF0000	/* mask out all but the cause in byte 2 */
-#define CAUSE_SHIFT_AMT 16	/* shift 2 bytes to place it in byte 2 */
-
-/* SubsystemSeverityFilter */
-#define SEVERITY_FILTER_MASK 0x0F /* mask out the Cause half, SeverityFilter is
-				   * in the lower nibble */
-#define CAUSE_FILTER_MASK 0xF0	/* mask out the Severity half, CauseFilter is in
-				 * the upper nibble */
-#define CAUSE_FILTER_SHIFT_AMT	4 /* shift amount to place it in lower or upper
-				   * nibble */
-
-/* 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... */
-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 */
-	 /* RESERVED 0x03 - 0x7 */
-
-	 /* Ultravisor Components */
-	 ULTRA_COMPONENT_BOOT = 0x08,
-	 ULTRA_COMPONENT_IDLE = 0x09,
-	 ULTRA_COMPONENT_CONTROL = 0x0A,
-	 ULTRA_COMPONENT_LOGGER = 0x0B,
-	 ULTRA_COMPONENT_ACPI = 0X0C,
-	 /* RESERVED 0x0D - 0x0F */
-
-	 /* sPAR Components */
-	 ULTRA_COMPONENT_COMMAND = 0x10,
-	 ULTRA_COMPONENT_IODRIVER = 0x11,
-	 ULTRA_COMPONENT_CONSOLE = 0x12,
-	 ULTRA_COMPONENT_OPERATIONS = 0x13,
-	 ULTRA_COMPONENT_MANAGEMENT = 0x14,
-	 ULTRA_COMPONENT_DIAG = 0x15,
-	 ULTRA_COMPONENT_HWDIAG = 0x16,
-	 ULTRA_COMPONENT_PSERVICES = 0x17,
-	 ULTRA_COMPONENT_PDIAG = 0x18
-	 /* RESERVED 0x18 - 0x1F */
-};
-
-/* 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.
- * LineNumber: Line number in module name where event originated.  Timestamp:
- * Date/time when event was received by ReportEvent, and written to DiagChannel.
- * Reserved: Padding to align structure on a 64-byte cache line boundary.
- * AdditionalInfo: Array of characters for additional event info (may be
- * empty).  */
-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 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 block_no_high;
-	u32 event_no;		/* filled in by DiagSwitch as pool blocks are
-				 * filled */
-	u32 event_no_high;
-
-	/* 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 additional_info[MAX_ADDITIONAL_INFO_SIZE];
-
-	/* 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 */
-};
-
-/* 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,
-* but info events rarely need to be logged except during debugging).  The values
-* DIAG_SEVERITY_ENUM_BEGIN and DIAG_SEVERITY_ENUM_END are not valid severity
-* values.  They exist merely to dilineate the list, so that future additions
-* won't require changes to the driver (i.e. when checking for out-of-range
-* severities in SetSeverity).  The values DIAG_SEVERITY_OVERRIDE and
-* DIAG_SEVERITY_SHUTOFF are not valid severity values for logging events but
-* 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.  */
-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 */
-		DIAG_SEVERITY_WARNING = DIAG_SEVERITY_INFO + 1,	/* 2 */
-		DIAG_SEVERITY_ERR = DIAG_SEVERITY_WARNING + 1,	/* 3 */
-		DIAG_SEVERITY_PRINT = DIAG_SEVERITY_ERR + 1,	/* 4 */
-		DIAG_SEVERITY_SHUTOFF = DIAG_SEVERITY_PRINT + 1, /* 5 */
-		DIAG_SEVERITY_ENUM_END = DIAG_SEVERITY_SHUTOFF,	/* 5 */
-		DIAG_SEVERITY_NONFATAL_ERR = DIAG_SEVERITY_ERR,
-		DIAG_SEVERITY_FATAL_ERR = DIAG_SEVERITY_PRINT
-};
-
-/* Event Cause enums
-*
-* Levels of cause for diagnostic events, in order from least to greatest cause
-* Internal errors are most urgent since ideally they should never exist
-* Invalid requests are preventable by avoiding invalid inputs
-* Operations errors depend on environmental factors which may impact which
-* requests are possible
-* Manifest provides intermediate value to capture firmware and configuration
-* version information
-* Trace provides suplimental debug information in release firmware
-* Unknown Log captures unclasified LogEvent calls.
-* Debug is the least urgent since it provides suplimental debug information only
-* in debug firmware
-* Unknown Debug captures unclassified DebugEvent calls.
-* 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.  */
-
-/* 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
-* 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
- * 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.  */
-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 */
-	DIAG_CAUSE_UNKNOWN_LOG = DIAG_CAUSE_DEBUG + 1,	/* 3 */
-	DIAG_CAUSE_TRACE = DIAG_CAUSE_UNKNOWN_LOG + 1,	/* 4 */
-	DIAG_CAUSE_MANIFEST = DIAG_CAUSE_TRACE + 1,	/* 5 */
-	DIAG_CAUSE_OPERATIONS_ERROR = DIAG_CAUSE_MANIFEST + 1,	/* 6 */
-	DIAG_CAUSE_INVALID_REQUEST = DIAG_CAUSE_OPERATIONS_ERROR + 1,	/* 7 */
-	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 */
-};
-
-/* Event Cause category defined into the byte 2 of Severity */
-#define CAUSE_DEBUG (DIAG_CAUSE_DEBUG << CAUSE_SHIFT_AMT)
-#define CAUSE_TRACE (DIAG_CAUSE_TRACE << CAUSE_SHIFT_AMT)
-#define CAUSE_MANIFEST (DIAG_CAUSE_MANIFEST << CAUSE_SHIFT_AMT)
-#define CAUSE_OPERATIONS_ERROR (DIAG_CAUSE_OPERATIONS_ERROR << CAUSE_SHIFT_AMT)
-#define CAUSE_INVALID_REQUEST (DIAG_CAUSE_INVALID_REQUEST << CAUSE_SHIFT_AMT)
-#define CAUSE_INTERNAL_ERROR (DIAG_CAUSE_INTERNAL_ERROR << CAUSE_SHIFT_AMT)
-#define CAUSE_FILE_XFER (DIAG_CAUSE_FILE_XFER << CAUSE_SHIFT_AMT)
-#define CAUSE_ENUM_END CAUSE_FILE_XFER
-
-/* Combine Cause and Severity categories into one */
-#define CAUSE_DEBUG_SEVERITY_VERBOSE \
-	(CAUSE_DEBUG | DIAG_SEVERITY_VERBOSE)
-#define CAUSE_TRACE_SEVERITY_VERBOSE \
-	(CAUSE_TRACE | DIAG_SEVERITY_VERBOSE)
-#define CAUSE_MANIFEST_SEVERITY_VERBOSE\
-	(CAUSE_MANIFEST | DIAG_SEVERITY_VERBOSE)
-#define CAUSE_OPERATIONS_SEVERITY_VERBOSE \
-	(CAUSE_OPERATIONS_ERROR | DIAG_SEVERITY_VERBOSE)
-#define CAUSE_INVALID_SEVERITY_VERBOSE \
-	(CAUSE_INVALID_REQUEST  | DIAG_SEVERITY_VERBOSE)
-#define CAUSE_INTERNAL_SEVERITY_VERBOSE \
-	(CAUSE_INTERNAL_ERROR   | DIAG_SEVERITY_VERBOSE)
-
-#define CAUSE_DEBUG_SEVERITY_INFO \
-	(CAUSE_DEBUG | DIAG_SEVERITY_INFO)
-#define CAUSE_TRACE_SEVERITY_INFO \
-	(CAUSE_TRACE | DIAG_SEVERITY_INFO)
-#define CAUSE_MANIFEST_SEVERITY_INFO \
-	(CAUSE_MANIFEST | DIAG_SEVERITY_INFO)
-#define CAUSE_OPERATIONS_SEVERITY_INFO \
-	(CAUSE_OPERATIONS_ERROR | DIAG_SEVERITY_INFO)
-#define CAUSE_INVALID_SEVERITY_INFO \
-	(CAUSE_INVALID_REQUEST  | DIAG_SEVERITY_INFO)
-#define CAUSE_INTERNAL_SEVERITY_INFO \
-	(CAUSE_INTERNAL_ERROR | DIAG_SEVERITY_INFO)
-
-#define CAUSE_DEBUG_SEVERITY_WARN \
-	(CAUSE_DEBUG | DIAG_SEVERITY_WARNING)
-#define CAUSE_TRACE_SEVERITY_WARN \
-	(CAUSE_TRACE | DIAG_SEVERITY_WARNING)
-#define CAUSE_MANIFEST_SEVERITY_WARN \
-	(CAUSE_MANIFEST | DIAG_SEVERITY_WARNING)
-#define CAUSE_OPERATIONS_SEVERITY_WARN \
-	(CAUSE_OPERATIONS_ERROR | DIAG_SEVERITY_WARNING)
-#define CAUSE_INVALID_SEVERITY_WARN \
-	(CAUSE_INVALID_REQUEST | DIAG_SEVERITY_WARNING)
-#define CAUSE_INTERNAL_SEVERITY_WARN \
-	(CAUSE_INTERNAL_ERROR | DIAG_SEVERITY_WARNING)
-
-#define CAUSE_DEBUG_SEVERITY_ERR \
-	(CAUSE_DEBUG | DIAG_SEVERITY_ERR)
-#define CAUSE_TRACE_SEVERITY_ERR \
-	(CAUSE_TRACE | DIAG_SEVERITY_ERR)
-#define CAUSE_MANIFEST_SEVERITY_ERR \
-	(CAUSE_MANIFEST | DIAG_SEVERITY_ERR)
-#define CAUSE_OPERATIONS_SEVERITY_ERR \
-	(CAUSE_OPERATIONS_ERROR | DIAG_SEVERITY_ERR)
-#define CAUSE_INVALID_SEVERITY_ERR \
-	(CAUSE_INVALID_REQUEST  | DIAG_SEVERITY_ERR)
-#define CAUSE_INTERNAL_SEVERITY_ERR \
-	(CAUSE_INTERNAL_ERROR   | DIAG_SEVERITY_ERR)
-
-#define CAUSE_DEBUG_SEVERITY_PRINT \
-	(CAUSE_DEBUG | DIAG_SEVERITY_PRINT)
-#define CAUSE_TRACE_SEVERITY_PRINT \
-	(CAUSE_TRACE | DIAG_SEVERITY_PRINT)
-#define CAUSE_MANIFEST_SEVERITY_PRINT \
-	(CAUSE_MANIFEST | DIAG_SEVERITY_PRINT)
-#define CAUSE_OPERATIONS_SEVERITY_PRINT \
-	(CAUSE_OPERATIONS_ERROR | DIAG_SEVERITY_PRINT)
-#define CAUSE_INVALID_SEVERITY_PRINT \
-	(CAUSE_INVALID_REQUEST | DIAG_SEVERITY_PRINT)
-#define CAUSE_INTERNAL_SEVERITY_PRINT \
-	(CAUSE_INTERNAL_ERROR | DIAG_SEVERITY_PRINT)
-#define CAUSE_FILE_XFER_SEVERITY_PRINT \
-	(CAUSE_FILE_XFER | DIAG_SEVERITY_PRINT)
-
-/* Structure: diag_channel_protocol_header
- *
- * Purpose: Contains attributes that make up the header specific to the
- * DIAG_CHANNEL area.
- *
- * Attributes:
- *
- * DiagLock: Diag Channel spinlock.
- *
- *IsChannelInitialized: 1 iff SignalInit was called for this channel; otherwise
- *			0, and assume the channel is not ready for use yet.
- *
- * Reserved: Padding to align the fields in this structure.
- *
- *SubsystemSeverityFilter: Level of severity on a subsystem basis that controls
- *			whether events are logged.  Any event's severity for a
- *			particular subsystem below this level will be discarded.
- */
-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: */
-/* ----------------------- */
-/* | Channel Header        |	Defined by ULTRA_CHANNEL_PROTOCOL */
-/* ----------------------- */
-/* | Signal Queue Header   |	Defined by SIGNAL_QUEUE_HEADER */
-/* ----------------------- */
-/* | DiagChannel Header    |	Defined by diag_channel_protocol_header */
-/* ----------------------- */
-/* | 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(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(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)
-
-/* For Control and Idle Partitions with larger (8 MB) diagnostic(root)
- * channels */
-#define DIAG_CH_LRG_SIZE (2 * DIAG_CH_SIZE)	/* 8 MB */
-
-/*
- * Structure: spar_diag_channel_protocol
- *
- * Purpose: Contains attributes that make up the DIAG_CHANNEL memory.
- *
- * Attributes:
- *
- * CommonChannelHeader:	Header info common to all channels.
- *
- * QueueHeader: Queue header common to all channels - used to determine where to
- * store event.
- *
- * DiagChannelHeader: Diagnostic channel header info (see
- * 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.
-*/
-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/diagnostics/appos_subsystems.h b/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h
deleted file mode 100644
index 18cc9ed2748b..000000000000
--- a/drivers/staging/unisys/common-spar/include/diagnostics/appos_subsystems.h
+++ /dev/null
@@ -1,310 +0,0 @@
-/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* Please note that this file is to be used ONLY for defining diagnostic
- * subsystem values for the appos (sPAR Linux service partitions) component.
- */
-#ifndef __APPOS_SUBSYSTEMS_H__
-#define __APPOS_SUBSYSTEMS_H__
-
-#ifdef __KERNEL__
-#include <linux/kernel.h>
-#include <linux/string.h>
-#else
-#include <stdio.h>
-#include <string.h>
-#endif
-
-static inline char *
-subsys_unknown_to_s(int subsys, char *s, int n)
-{
-	snprintf(s, n, "SUBSYS-%-2.2d", subsys);
-	s[n - 1] = '\0';
-	return s;
-}
-
-#define SUBSYS_TO_MASK(subsys)      (1ULL << (subsys))
-
-/* The first SUBSYS_APPOS_MAX subsystems are the same for each AppOS type
- * (IOVM, SMS, etc.) The rest have unique values for each AppOS type.
- */
-#define SUBSYS_APPOS_MAX 16
-
-#define	SUBSYS_APPOS_DEFAULT         1	/* or "other" */
-#define SUBSYS_APPOS_CHIPSET         2	/* controlvm and other */
-					/* low-level sPAR activity */
-#define SUBSYS_APPOS_BUS             3	/* sPAR bus */
-/* DAK #define SUBSYS_APPOS_DIAG            4  // diagnostics and dump */
-#define SUBSYS_APPOS_CHANNELACCESS   5	/* generic channel access */
-#define SUBSYS_APPOS_NICCLIENT       6	/* virtual NIC client */
-#define SUBSYS_APPOS_HBACLIENT       7	/* virtual HBA client */
-#define SUBSYS_APPOS_CONSOLESERIAL   8	/* sPAR virtual serial console */
-#define SUBSYS_APPOS_UISLIB          9	/*  */
-#define SUBSYS_APPOS_VRTCUPDD       10	/*  */
-#define SUBSYS_APPOS_WATCHDOG       11	/* watchdog timer and healthcheck */
-#define SUBSYS_APPOS_13             13	/* available */
-#define SUBSYS_APPOS_14             14	/* available */
-#define SUBSYS_APPOS_15             15	/* available */
-#define SUBSYS_APPOS_16             16	/* available */
-static inline char *
-subsys_generic_to_s(int subsys, char *s, int n)
-{
-	switch (subsys) {
-	case SUBSYS_APPOS_DEFAULT:
-		strncpy(s, "APPOS_DEFAULT", n);
-		break;
-	case SUBSYS_APPOS_CHIPSET:
-		strncpy(s, "APPOS_CHIPSET", n);
-		break;
-	case SUBSYS_APPOS_BUS:
-		strncpy(s, "APPOS_BUS", n);
-		break;
-	case SUBSYS_APPOS_CHANNELACCESS:
-		strncpy(s, "APPOS_CHANNELACCESS", n);
-		break;
-	case SUBSYS_APPOS_NICCLIENT:
-		strncpy(s, "APPOS_NICCLIENT", n);
-		break;
-	case SUBSYS_APPOS_HBACLIENT:
-		strncpy(s, "APPOS_HBACLIENT", n);
-		break;
-	case SUBSYS_APPOS_CONSOLESERIAL:
-		strncpy(s, "APPOS_CONSOLESERIAL", n);
-		break;
-	case SUBSYS_APPOS_UISLIB:
-		strncpy(s, "APPOS_UISLIB", n);
-		break;
-	case SUBSYS_APPOS_VRTCUPDD:
-		strncpy(s, "APPOS_VRTCUPDD", n);
-		break;
-	case SUBSYS_APPOS_WATCHDOG:
-		strncpy(s, "APPOS_WATCHDOG", n);
-		break;
-	case SUBSYS_APPOS_13:
-		strncpy(s, "APPOS_13", n);
-		break;
-	case SUBSYS_APPOS_14:
-		strncpy(s, "APPOS_14", n);
-		break;
-	case SUBSYS_APPOS_15:
-		strncpy(s, "APPOS_15", n);
-		break;
-	case SUBSYS_APPOS_16:
-		strncpy(s, "APPOS_16", n);
-		break;
-	default:
-		subsys_unknown_to_s(subsys, s, n);
-		break;
-	}
-	s[n - 1] = '\0';
-	return s;
-}
-
-/* CONSOLE */
-
-#define SUBSYS_CONSOLE_VIDEO        (SUBSYS_APPOS_MAX + 1)	/* 17 */
-#define SUBSYS_CONSOLE_KBDMOU       (SUBSYS_APPOS_MAX + 2)	/* 18 */
-#define SUBSYS_CONSOLE_04           (SUBSYS_APPOS_MAX + 4)
-#define SUBSYS_CONSOLE_05           (SUBSYS_APPOS_MAX + 5)
-#define SUBSYS_CONSOLE_06           (SUBSYS_APPOS_MAX + 6)
-#define SUBSYS_CONSOLE_07           (SUBSYS_APPOS_MAX + 7)
-#define SUBSYS_CONSOLE_08           (SUBSYS_APPOS_MAX + 8)
-#define SUBSYS_CONSOLE_09           (SUBSYS_APPOS_MAX + 9)
-#define SUBSYS_CONSOLE_10           (SUBSYS_APPOS_MAX + 10)
-#define SUBSYS_CONSOLE_11           (SUBSYS_APPOS_MAX + 11)
-#define SUBSYS_CONSOLE_12           (SUBSYS_APPOS_MAX + 12)
-#define SUBSYS_CONSOLE_13           (SUBSYS_APPOS_MAX + 13)
-#define SUBSYS_CONSOLE_14           (SUBSYS_APPOS_MAX + 14)
-#define SUBSYS_CONSOLE_15           (SUBSYS_APPOS_MAX + 15)
-#define SUBSYS_CONSOLE_16           (SUBSYS_APPOS_MAX + 16)
-#define SUBSYS_CONSOLE_17           (SUBSYS_APPOS_MAX + 17)
-#define SUBSYS_CONSOLE_18           (SUBSYS_APPOS_MAX + 18)
-#define SUBSYS_CONSOLE_19           (SUBSYS_APPOS_MAX + 19)
-#define SUBSYS_CONSOLE_20           (SUBSYS_APPOS_MAX + 20)
-#define SUBSYS_CONSOLE_21           (SUBSYS_APPOS_MAX + 21)
-#define SUBSYS_CONSOLE_22           (SUBSYS_APPOS_MAX + 22)
-#define SUBSYS_CONSOLE_23           (SUBSYS_APPOS_MAX + 23)
-#define SUBSYS_CONSOLE_24           (SUBSYS_APPOS_MAX + 24)
-#define SUBSYS_CONSOLE_25           (SUBSYS_APPOS_MAX + 25)
-#define SUBSYS_CONSOLE_26           (SUBSYS_APPOS_MAX + 26)
-#define SUBSYS_CONSOLE_27           (SUBSYS_APPOS_MAX + 27)
-#define SUBSYS_CONSOLE_28           (SUBSYS_APPOS_MAX + 28)
-#define SUBSYS_CONSOLE_29           (SUBSYS_APPOS_MAX + 29)
-#define SUBSYS_CONSOLE_30           (SUBSYS_APPOS_MAX + 30)
-#define SUBSYS_CONSOLE_31           (SUBSYS_APPOS_MAX + 31)
-#define SUBSYS_CONSOLE_32           (SUBSYS_APPOS_MAX + 32)
-#define SUBSYS_CONSOLE_33           (SUBSYS_APPOS_MAX + 33)
-#define SUBSYS_CONSOLE_34           (SUBSYS_APPOS_MAX + 34)
-#define SUBSYS_CONSOLE_35           (SUBSYS_APPOS_MAX + 35)
-#define SUBSYS_CONSOLE_36           (SUBSYS_APPOS_MAX + 36)
-#define SUBSYS_CONSOLE_37           (SUBSYS_APPOS_MAX + 37)
-#define SUBSYS_CONSOLE_38           (SUBSYS_APPOS_MAX + 38)
-#define SUBSYS_CONSOLE_39           (SUBSYS_APPOS_MAX + 39)
-#define SUBSYS_CONSOLE_40           (SUBSYS_APPOS_MAX + 40)
-#define SUBSYS_CONSOLE_41           (SUBSYS_APPOS_MAX + 41)
-#define SUBSYS_CONSOLE_42           (SUBSYS_APPOS_MAX + 42)
-#define SUBSYS_CONSOLE_43           (SUBSYS_APPOS_MAX + 43)
-#define SUBSYS_CONSOLE_44           (SUBSYS_APPOS_MAX + 44)
-#define SUBSYS_CONSOLE_45           (SUBSYS_APPOS_MAX + 45)
-#define SUBSYS_CONSOLE_46           (SUBSYS_APPOS_MAX + 46)
-
-static inline char *
-subsys_console_to_s(int subsys, char *s, int n)
-{
-	switch (subsys) {
-	case SUBSYS_CONSOLE_VIDEO:
-		strncpy(s, "CONSOLE_VIDEO", n);
-		break;
-	case SUBSYS_CONSOLE_KBDMOU:
-		strncpy(s, "CONSOLE_KBDMOU", n);
-		break;
-	case SUBSYS_CONSOLE_04:
-		strncpy(s, "CONSOLE_04", n);
-		break;
-	case SUBSYS_CONSOLE_05:
-		strncpy(s, "CONSOLE_05", n);
-		break;
-	case SUBSYS_CONSOLE_06:
-		strncpy(s, "CONSOLE_06", n);
-		break;
-	case SUBSYS_CONSOLE_07:
-		strncpy(s, "CONSOLE_07", n);
-		break;
-	case SUBSYS_CONSOLE_08:
-		strncpy(s, "CONSOLE_08", n);
-		break;
-	case SUBSYS_CONSOLE_09:
-		strncpy(s, "CONSOLE_09", n);
-		break;
-	case SUBSYS_CONSOLE_10:
-		strncpy(s, "CONSOLE_10", n);
-		break;
-	case SUBSYS_CONSOLE_11:
-		strncpy(s, "CONSOLE_11", n);
-		break;
-	case SUBSYS_CONSOLE_12:
-		strncpy(s, "CONSOLE_12", n);
-		break;
-	case SUBSYS_CONSOLE_13:
-		strncpy(s, "CONSOLE_13", n);
-		break;
-	case SUBSYS_CONSOLE_14:
-		strncpy(s, "CONSOLE_14", n);
-		break;
-	case SUBSYS_CONSOLE_15:
-		strncpy(s, "CONSOLE_15", n);
-		break;
-	case SUBSYS_CONSOLE_16:
-		strncpy(s, "CONSOLE_16", n);
-		break;
-	case SUBSYS_CONSOLE_17:
-		strncpy(s, "CONSOLE_17", n);
-		break;
-	case SUBSYS_CONSOLE_18:
-		strncpy(s, "CONSOLE_18", n);
-		break;
-	case SUBSYS_CONSOLE_19:
-		strncpy(s, "CONSOLE_19", n);
-		break;
-	case SUBSYS_CONSOLE_20:
-		strncpy(s, "CONSOLE_20", n);
-		break;
-	case SUBSYS_CONSOLE_21:
-		strncpy(s, "CONSOLE_21", n);
-		break;
-	case SUBSYS_CONSOLE_22:
-		strncpy(s, "CONSOLE_22", n);
-		break;
-	case SUBSYS_CONSOLE_23:
-		strncpy(s, "CONSOLE_23", n);
-		break;
-	case SUBSYS_CONSOLE_24:
-		strncpy(s, "CONSOLE_24", n);
-		break;
-	case SUBSYS_CONSOLE_25:
-		strncpy(s, "CONSOLE_25", n);
-		break;
-	case SUBSYS_CONSOLE_26:
-		strncpy(s, "CONSOLE_26", n);
-		break;
-	case SUBSYS_CONSOLE_27:
-		strncpy(s, "CONSOLE_27", n);
-		break;
-	case SUBSYS_CONSOLE_28:
-		strncpy(s, "CONSOLE_28", n);
-		break;
-	case SUBSYS_CONSOLE_29:
-		strncpy(s, "CONSOLE_29", n);
-		break;
-	case SUBSYS_CONSOLE_30:
-		strncpy(s, "CONSOLE_30", n);
-		break;
-	case SUBSYS_CONSOLE_31:
-		strncpy(s, "CONSOLE_31", n);
-		break;
-	case SUBSYS_CONSOLE_32:
-		strncpy(s, "CONSOLE_32", n);
-		break;
-	case SUBSYS_CONSOLE_33:
-		strncpy(s, "CONSOLE_33", n);
-		break;
-	case SUBSYS_CONSOLE_34:
-		strncpy(s, "CONSOLE_34", n);
-		break;
-	case SUBSYS_CONSOLE_35:
-		strncpy(s, "CONSOLE_35", n);
-		break;
-	case SUBSYS_CONSOLE_36:
-		strncpy(s, "CONSOLE_36", n);
-		break;
-	case SUBSYS_CONSOLE_37:
-		strncpy(s, "CONSOLE_37", n);
-		break;
-	case SUBSYS_CONSOLE_38:
-		strncpy(s, "CONSOLE_38", n);
-		break;
-	case SUBSYS_CONSOLE_39:
-		strncpy(s, "CONSOLE_39", n);
-		break;
-	case SUBSYS_CONSOLE_40:
-		strncpy(s, "CONSOLE_40", n);
-		break;
-	case SUBSYS_CONSOLE_41:
-		strncpy(s, "CONSOLE_41", n);
-		break;
-	case SUBSYS_CONSOLE_42:
-		strncpy(s, "CONSOLE_42", n);
-		break;
-	case SUBSYS_CONSOLE_43:
-		strncpy(s, "CONSOLE_43", n);
-		break;
-	case SUBSYS_CONSOLE_44:
-		strncpy(s, "CONSOLE_44", n);
-		break;
-	case SUBSYS_CONSOLE_45:
-		strncpy(s, "CONSOLE_45", n);
-		break;
-	case SUBSYS_CONSOLE_46:
-		strncpy(s, "CONSOLE_46", n);
-		break;
-	default:
-		subsys_unknown_to_s(subsys, s, n);
-		break;
-	}
-	s[n - 1] = '\0';
-	return s;
-}
-
-#endif
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel.h b/drivers/staging/unisys/include/channel.h
index 6fb6e5b3ddaf..da0b5387f884 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel.h
+++ b/drivers/staging/unisys/include/channel.h
@@ -114,41 +114,6 @@ ULTRA_CHANNELCLI_STRING(u32 v)
 	  (((o) == CHANNELCLI_BUSY) && ((n) == CHANNELCLI_OWNED)) || (0)) \
 	 ? (1) : (0))
 
-#define SPAR_CHANNEL_CLIENT_CHK_TRANSITION(old, new, id, log,	\
-					    file, line)			\
-	do {								\
-		if (!ULTRA_VALID_CHANNELCLI_TRANSITION(old, new))	\
-			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 SPAR_CHANNEL_CLIENT_TRANSITION(ch, id, newstate, log)		\
-	do {								\
-		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)
-
 /* Values for ULTRA_CHANNEL_PROTOCOL.CliErrorBoot: */
 /* throttling invalid boot channel statetransition error due to client
  * disabled */
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel_guid.h b/drivers/staging/unisys/include/channel_guid.h
index 706363fc3e9a..706363fc3e9a 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel_guid.h
+++ b/drivers/staging/unisys/include/channel_guid.h
diff --git a/drivers/staging/unisys/include/diagchannel.h b/drivers/staging/unisys/include/diagchannel.h
new file mode 100644
index 000000000000..d2d35685d69f
--- /dev/null
+++ b/drivers/staging/unisys/include/diagchannel.h
@@ -0,0 +1,43 @@
+/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or (at
+ * your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ */
+
+#ifndef _DIAG_CHANNEL_H_
+#define _DIAG_CHANNEL_H_
+
+#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...
+					 */
+
+/* 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,
+ * but info events rarely need to be logged except during debugging). The
+ * values DIAG_SEVERITY_ENUM_BEGIN and DIAG_SEVERITY_ENUM_END are not valid
+ * severity values.  They exist merely to dilineate the list, so that future
+ * additions won't require changes to the driver (i.e. when checking for
+ * out-of-range severities in SetSeverity). The values DIAG_SEVERITY_OVERRIDE
+ * and DIAG_SEVERITY_SHUTOFF are not valid severity values for logging events
+ * but they are valid for controlling the amount of event data. Changes made
+ * to the enum, need to be reflected in s-Par.
+ */
+enum diag_severity {
+		DIAG_SEVERITY_VERBOSE = 0,
+		DIAG_SEVERITY_INFO = 1,
+		DIAG_SEVERITY_WARNING = 2,
+		DIAG_SEVERITY_ERR = 3,
+		DIAG_SEVERITY_PRINT = 4,
+};
+
+#endif
diff --git a/drivers/staging/unisys/include/guestlinuxdebug.h b/drivers/staging/unisys/include/guestlinuxdebug.h
index 957a627d0527..82ee565395ba 100644
--- a/drivers/staging/unisys/include/guestlinuxdebug.h
+++ b/drivers/staging/unisys/include/guestlinuxdebug.h
@@ -22,7 +22,6 @@
 * ISSUE_IO_VMCALL_POSTCODE_SEVERITY */
 
 /******* INFO ON ISSUE_POSTCODE_LINUX() BELOW *******/
-#include "vmcallinterface.h"
 enum driver_pc {		/* POSTCODE driver identifier tuples */
 	/* visorchipset driver files */
 	VISOR_CHIPSET_PC = 0xA0,
@@ -135,7 +134,7 @@ enum event_pc {			/* POSTCODE event identifier tuples */
 #define POSTCODE_SEVERITY_ERR DIAG_SEVERITY_ERR
 #define POSTCODE_SEVERITY_WARNING DIAG_SEVERITY_WARNING
 #define POSTCODE_SEVERITY_INFO DIAG_SEVERITY_PRINT	/* TODO-> Info currently
-							 * doesnt show, so we
+							 * doesn't show, so we
 							 * set info=warning */
 /* example call of POSTCODE_LINUX_2(VISOR_CHIPSET_PC, POSTCODE_SEVERITY_ERR);
  * Please also note that the resulting postcode is in hex, so if you are
diff --git a/drivers/staging/unisys/common-spar/include/channels/iochannel.h b/drivers/staging/unisys/include/iochannel.h
index 3bd7579e1daf..a559812342dc 100644
--- a/drivers/staging/unisys/common-spar/include/channels/iochannel.h
+++ b/drivers/staging/unisys/include/iochannel.h
@@ -4,45 +4,34 @@
 #define __IOCHANNEL_H__
 
 /*
-* Everything needed for IOPart-GuestPart communication is define in
-* this file.  Note: Everything is OS-independent because this file is
-* used by Windows, Linux and possible EFI drivers.  */
+ * Everything needed for IOPart-GuestPart communication is define in
+ * 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:
-*       UNINIT(All Zeroes), CHANNEL_ATTACHING, CHANNEL_ATTACHED, CHANNEL_OPENED
-*
-* additional states will be used later.  No locking is needed to switch between
-* states due to the following rules:
-*
-*      1.  IOPart is only the only partition allowed to change from UNIT
-*      2.  IOPart is only the only partition allowed to change from
-*		CHANNEL_ATTACHING
-*      3.  GuestPart is only the only partition allowed to change from
-*		CHANNEL_ATTACHED
-*
-* The state changes are the following: IOPart sees the channel is in UNINIT,
-*        UNINIT -> CHANNEL_ATTACHING (performed only by IOPart)
-*        CHANNEL_ATTACHING -> CHANNEL_ATTACHED (performed only by IOPart)
-*        CHANNEL_ATTACHED -> CHANNEL_OPENED (performed only by GuestPart)
-*/
+ * Communication flow between the IOPart and GuestPart uses the channel headers
+ * channel state.  The following states are currently being used:
+ *       UNINIT(All Zeroes), CHANNEL_ATTACHING, CHANNEL_ATTACHED, CHANNEL_OPENED
+ *
+ * additional states will be used later.  No locking is needed to switch between
+ * states due to the following rules:
+ *
+ *      1.  IOPart is only the only partition allowed to change from UNIT
+ *      2.  IOPart is only the only partition allowed to change from
+ *		CHANNEL_ATTACHING
+ *      3.  GuestPart is only the only partition allowed to change from
+ *		CHANNEL_ATTACHED
+ *
+ * The state changes are the following: IOPart sees the channel is in UNINIT,
+ *        UNINIT -> CHANNEL_ATTACHING (performed only by IOPart)
+ *        CHANNEL_ATTACHING -> CHANNEL_ATTACHED (performed only by IOPart)
+ *        CHANNEL_ATTACHED -> CHANNEL_OPENED (performed only by GuestPart)
+ */
 
 #include <linux/uuid.h>
 
-#include "vmcallinterface.h"
-
-#define _ULTRA_CONTROLVM_CHANNEL_INLINE_
 #include <linux/dma-direction.h>
-#include "controlvmchannel.h"
-#include "vbuschannel.h"
-#undef _ULTRA_CONTROLVM_CHANNEL_INLINE_
 #include "channel.h"
-
-/*
- * CHANNEL Guids
- */
-
 #include "channel_guid.h"
 
 #define ULTRA_VHBA_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
@@ -51,10 +40,11 @@
 	ULTRA_CHANNEL_PROTOCOL_SIGNATURE
 
 /* Must increment these 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. */
+ * 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_VHBA_CHANNEL_PROTOCOL_VERSIONID 2
 #define ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID 2
 #define ULTRA_VSWITCH_CHANNEL_PROTOCOL_VERSIONID 1
@@ -72,55 +62,26 @@
 				   ULTRA_VNIC_CHANNEL_PROTOCOL_SIGNATURE))
 
 /*
-* Everything necessary to handle SCSI & NIC traffic between Guest Partition and
-* IO Partition is defined below.  */
+ * Everything necessary to handle SCSI & NIC traffic between Guest Partition and
+ * IO Partition is defined below.
+ */
 
 /*
-* Defines and enums.
-*/
+ * Defines and enums.
+ */
 
 #define MINNUM(a, b) (((a) < (b)) ? (a) : (b))
 #define MAXNUM(a, b) (((a) > (b)) ? (a) : (b))
 
 /* these define the two queues per data channel between iopart and
- * ioguestparts */
+ * ioguestparts
+ */
 #define IOCHAN_TO_IOPART 0 /* used by ioguestpart to 'insert' signals to
 			    * iopart */
-#define IOCHAN_FROM_GUESTPART 0 /* used by iopart to 'remove' signals from
-				 * ioguestpart - same queue as previous queue */
 
-#define IOCHAN_TO_GUESTPART 1 /* used by iopart to 'insert' signals to
-			       * ioguestpart */
 #define IOCHAN_FROM_IOPART 1 /* used by ioguestpart to 'remove' signals from
 			      * iopart - same queue as previous queue */
 
-/* these define the two queues per control channel between controlpart and "its"
- * guests, which includes the iopart  */
-#define CTRLCHAN_TO_CTRLGUESTPART 0 /* used by ctrlguestpart to 'insert' signals
-				     * to ctrlpart */
-#define CTLRCHAN_FROM_CTRLPART 0 /* used by ctrlpart to 'remove' signals from
-				  * ctrlquestpart - same queue as previous
-				  * queue */
-
-#define CTRLCHAN_TO_CTRLPART 1 /* used by ctrlpart to 'insert' signals to
-				* ctrlguestpart */
-#define CTRLCHAN_FROM_CTRLGUESTPART 1 /* used by ctrguestpart to 'remove'
-				       * signals from ctrlpart - same queue as
-				       * previous queue */
-
-/* these define the Event & Ack queues per control channel Events are generated
-* by CTRLGUESTPART and sent to CTRLPART; Acks are generated by CTRLPART and sent
-* to CTRLGUESTPART. */
-#define CTRLCHAN_EVENT_TO_CTRLPART 2 /* used by ctrlguestpart to 'insert' Events
-				      * to ctrlpart */
-#define CTRLCHAN_EVENT_FROM_CTRLGUESTPART 2 /* used by ctrlpart to 'remove'
-					     * Events from ctrlguestpart */
-
-#define CTRLCHAN_ACK_TO_CTRLGUESTPART 3	/* used by ctrlpart to 'insert' Acks to
-					 * ctrlguestpart */
-#define CTRLCHAN_ACK_FROM_CTRLPART 3 /* used by ctrlguestpart to 'remove' Events
-				      * from ctrlpart */
-
 /* size of cdb - i.e., scsi cmnd */
 #define MAX_CMND_SIZE 16
 
@@ -128,28 +89,6 @@
 
 #define MAX_PHYS_INFO 64
 
-/* Because GuestToGuestCopy is limited to 4KiB segments, and we have limited the
-* Emulex Driver to 256 scatter list segments via the lpfc_sg_seg_cnt parameter
-* to 256, the maximum I/O size is limited to 256 * 4 KiB = 1 MB */
-#define MAX_IO_SIZE   (1024*1024)	/* 1 MB */
-
-/* NOTE 1: lpfc defines its support for segments in
-* #define LPFC_SG_SEG_CNT 64
-*
-* NOTE 2: In Linux, frags array in skb is currently allocated to be
-* MAX_SKB_FRAGS size, which is 18 which is smaller than MAX_PHYS_INFO for
-* now.  */
-
-#ifndef MAX_SERIAL_NUM
-#define MAX_SERIAL_NUM		32
-#endif				/* MAX_SERIAL_NUM */
-
-#define MAX_SCSI_BUSES		1
-#define MAX_SCSI_TARGETS	8
-#define MAX_SCSI_LUNS		16
-#define MAX_SCSI_FROM_HOST	0xFFFFFFFF	/* Indicator to use Physical HBA
-						 * SCSI Host value */
-
 /* various types of network packets that can be sent in cmdrsp */
 enum net_types {
 	NET_RCV_POST = 0,	/* submit buffer to hold receiving
@@ -173,7 +112,7 @@ enum net_types {
 	/* uisnic -> virtnic */
 	NET_MACADDR,		/* indicates the client has requested to update
 				 * its MAC addr */
-	NET_MACADDR_ACK,	/* MAC address  */
+	NET_MACADDR_ACK,	/* MAC address */
 
 };
 
@@ -182,19 +121,12 @@ enum net_types {
 #define		ETH_MIN_DATA_SIZE 46	/* minimum eth data size */
 #define		ETH_MIN_PACKET_SIZE (ETH_HEADER_SIZE + ETH_MIN_DATA_SIZE)
 
-#define     ETH_DEF_DATA_SIZE 1500	/* default data size */
-#define     ETH_DEF_PACKET_SIZE (ETH_HEADER_SIZE + ETH_DEF_DATA_SIZE)
-
 #define		ETH_MAX_MTU 16384	/* maximum data size */
 
 #ifndef MAX_MACADDR_LEN
 #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 NIC_VENDOR_ID 0x0008000B
-
 /* various types of scsi task mgmt commands  */
 enum task_mgmt_types {
 	TASK_MGMT_ABORT_TASK = 1,
@@ -209,32 +141,16 @@ enum vdisk_mgmt_types {
 	VDISK_MGMT_RELEASE,
 };
 
-/* this is used in the vdest field  */
-#define VDEST_ALL 0xFFFF
-
-#define MIN_NUMSIGNALS 64
-#define MAX_NUMSIGNALS 4096
-
-/* MAX_NET_RCV_BUF specifies the number of rcv buffers that are created by each
-* guest's virtnic and posted to uisnic.  Uisnic, for each channel, keeps the rcv
-* buffers posted and uses them to receive data on behalf of the guest's virtnic.
-* NOTE: the num_rcv_bufs is configurable for each VNIC. So the following is
-* simply an upperlimit on what each VNIC can provide.  Setting it to half of the
-* NUMSIGNALS to prevent queue full deadlocks */
-#define MAX_NET_RCV_BUFS (MIN_NUMSIGNALS / 2)
-
-/*
- * structs with pragma pack  */
-
-/* ///////////// BEGIN PRAGMA PACK PUSH 1 ///////////////////////// */
-/* ///////////// ONLY STRUCT TYPE SHOULD BE BELOW */
-
-#pragma pack(push, 1)
+struct phys_info {
+	u64 pi_pfn;
+	u16 pi_off;
+	u16 pi_len;
+} __packed;
 
 struct guest_phys_info {
 	u64 address;
 	u64 length;
-};
+} __packed;
 
 #define GPI_ENTRIES_PER_PAGE (PAGE_SIZE / sizeof(struct guest_phys_info))
 
@@ -242,12 +158,12 @@ struct uisscsi_dest {
 	u32 channel;		/* channel == bus number */
 	u32 id;			/* id == target number */
 	u32 lun;		/* lun == logical unit number */
-};
+} __packed;
 
 struct vhba_wwnn {
 	u32 wwnn1;
 	u32 wwnn2;
-};
+} __packed;
 
 /* WARNING: Values stired in this structure must contain maximum counts (not
  * maximum values). */
@@ -264,7 +180,7 @@ struct vhba_config_max {	/* 20 bytes */
 				 * bus */
 	/* max io size is often determined by the resource of the hba. e.g */
 	/* max scatter gather list length * page size / sector size */
-};
+} __packed;
 
 struct uiscmdrsp_scsi {
 	void *scsicmd;		/* the handle to the cmd that was received -
@@ -287,13 +203,7 @@ struct uiscmdrsp_scsi {
 	u8 scsistat;		/* the scsi status */
 	u8 addlstat;		/* non-scsi status - covers cases like timeout
 				 * needed by windows guests */
-#define ADDL_RESET		1
-#define ADDL_TIMEOUT		2
-#define ADDL_INTERNAL_ERROR	3
 #define ADDL_SEL_TIMEOUT	4
-#define ADDL_CMD_TIMEOUT	5
-#define ADDL_BAD_TARGET		6
-#define ADDL_RETRY		7
 
 	/* the following fields are need to determine the result of command */
 	 u8 sensebuf[MAX_SENSE_SIZE];	/* sense info in case cmd failed; */
@@ -301,17 +211,19 @@ struct uiscmdrsp_scsi {
 	/* see that struct for details. */
 	void *vdisk; /* contains pointer to the vdisk so that we can clean up
 		      * when the IO completes. */
-	int no_disk_result;	/* used to return no disk inquiry result */
-	/* when no_disk_result is set to 1,  */
-	/* scsi.scsistat is SAM_STAT_GOOD */
-	/* scsi.addlstat is 0 */
-	/* scsi.linuxstat is SAM_STAT_GOOD */
-	/* That is, there is NO error. */
-};
-
-/*
-* Defines to support sending correct inquiry result when no disk is
-* configured.  */
+	int no_disk_result;
+	/* used to return no disk inquiry result
+	 * when no_disk_result is set to 1,
+	 * scsi.scsistat is SAM_STAT_GOOD
+	 * scsi.addlstat is 0
+	 * scsi.linuxstat is SAM_STAT_GOOD
+	 * That is, there is NO error.
+	 */
+} __packed;
+
+/* Defines to support sending correct inquiry result when no disk is
+ * configured.
+ */
 
 /* From SCSI SPC2 -
  *
@@ -324,26 +236,22 @@ struct uiscmdrsp_scsi {
  *connected to this logical unit.
  */
 
-#define DEV_NOT_PRESENT 0x7f	/* old name - compatibility */
 #define DEV_NOT_CAPABLE 0x7f	/* peripheral qualifier of 0x3  */
-    /* peripheral type of 0x1f */
-    /* specifies no device but target present */
+				/* peripheral type of 0x1f */
+				/* specifies no device but target present */
 
 #define DEV_DISK_CAPABLE_NOT_PRESENT 0x20 /* peripheral qualifier of 0x1 */
     /* peripheral type of 0 - disk */
     /* specifies device capable, but not present */
 
-#define DEV_PROC_CAPABLE_NOT_PRESENT 0x23 /* peripheral qualifier of 0x1 */
-    /* peripheral type of 3 - processor */
-    /* specifies device capable, but not present */
-
 #define DEV_HISUPPORT 0x10	/* HiSup = 1; shows support for report luns */
-    /* must be returned for lun 0. */
+				/* must be returned for lun 0. */
 
 /* NOTE: Linux code assumes inquiry contains 36 bytes. Without checking length
-* in buf[4] some linux code accesses bytes beyond 5 to retrieve vendor, product
-* & revision.  Yikes! So let us always send back 36 bytes, the minimum for
-* inquiry result. */
+ * in buf[4] some linux code accesses bytes beyond 5 to retrieve vendor, product
+ * & revision.  Yikes! So let us always send back 36 bytes, the minimum for
+ * inquiry result.
+ */
 #define NO_DISK_INQUIRY_RESULT_LEN 36
 
 #define MIN_INQUIRY_RESULT_LEN 5 /* we need at least 5 bytes minimum for inquiry
@@ -394,21 +302,21 @@ struct uiscmdrsp_scsi {
 	} while (0)
 
 /*
-* Struct & Defines to support sense information.
-*/
+ * 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).
-* When set, these fields will have the following values:
-* ErrorCode = 0x70		indicates current error
-* Valid = 1			indicates sense info is valid
-* SenseKey			contains sense key as defined by SCSI specs.
-* AdditionalSenseCode		contains sense key as defined by SCSI specs.
-* AdditionalSenseCodeQualifier	contains qualifier to sense code as defined by
-*				scsi docs.
-* AdditionalSenseLength		contains will be sizeof(sense_data)-8=10.
-*/
+ * initialized in exactly the manner that is recommended in Windows (hence the
+ * odd values).
+ * When set, these fields will have the following values:
+ * ErrorCode = 0x70		indicates current error
+ * Valid = 1			indicates sense info is valid
+ * SenseKey			contains sense key as defined by SCSI specs.
+ * AdditionalSenseCode		contains sense key as defined by SCSI specs.
+ * AdditionalSenseCodeQualifier	contains qualifier to sense code as defined by
+ *				scsi docs.
+ * AdditionalSenseLength	contains will be sizeof(sense_data)-8=10.
+ */
 struct sense_data {
 	u8 errorcode:7;
 	u8 valid:1;
@@ -425,38 +333,7 @@ struct sense_data {
 	u8 additional_sense_code_qualifier;
 	u8 fru_code;
 	u8 sense_key_specific[3];
-};
-
-/* some SCSI ADSENSE codes */
-#ifndef SCSI_ADSENSE_LUN_NOT_READY
-#define SCSI_ADSENSE_LUN_NOT_READY 0x04
-#endif	/*  */
-#ifndef SCSI_ADSENSE_ILLEGAL_COMMAND
-#define SCSI_ADSENSE_ILLEGAL_COMMAND 0x20
-#endif	/*  */
-#ifndef SCSI_ADSENSE_ILLEGAL_BLOCK
-#endif	/*  */
-#ifndef SCSI_ADSENSE_ILLEGAL_BLOCK
-#define SCSI_ADSENSE_ILLEGAL_BLOCK  0x21
-#endif	/*  */
-#ifndef SCSI_ADSENSE_INVALID_CDB
-#define SCSI_ADSENSE_INVALID_CDB    0x24
-#endif	/*  */
-#ifndef SCSI_ADSENSE_INVALID_LUN
-#define SCSI_ADSENSE_INVALID_LUN    0x25
-#endif	/*  */
-#ifndef SCSI_ADWRITE_PROTECT
-#define SCSI_ADWRITE_PROTECT        0x27
-#endif	/*  */
-#ifndef SCSI_ADSENSE_MEDIUM_CHANGED
-#define SCSI_ADSENSE_MEDIUM_CHANGED 0x28
-#endif	/*  */
-#ifndef SCSI_ADSENSE_BUS_RESET
-#define SCSI_ADSENSE_BUS_RESET      0x29
-#endif	/*  */
-#ifndef SCSI_ADSENSE_NO_MEDIA_IN_DEVICE
-#define SCSI_ADSENSE_NO_MEDIA_IN_DEVICE 0x3a
-#endif	/*  */
+} __packed;
 
 struct net_pkt_xmt {
 	int len;	/* full length of data in the packet */
@@ -484,34 +361,33 @@ struct net_pkt_xmt {
 	     * guest memory to get to the header. uisnic needs ethhdr to
 	     * determine how to route the packet.
 	     */
-};
+} __packed;
 
 struct net_pkt_xmtdone {
 	u32 xmt_done_result;	/* result of NET_XMIT */
-#define XMIT_SUCCESS 0
-#define XMIT_FAILED 1
-};
+} __packed;
 
 /* RCVPOST_BUF_SIZe must be at most page_size(4096) - cache_line_size (64) The
-* reason is because dev_skb_alloc which is used to generate RCV_POST skbs in
-* virtnic requires that there is "overhead" in the buffer, and pads 16 bytes.  I
-* prefer to use 1 full cache line size for "overhead" so that transfers are
-* better.  IOVM requires that a buffer be represented by 1 phys_info structure
-* which can only cover page_size. */
+ * reason is because dev_skb_alloc which is used to generate RCV_POST skbs in
+ * virtnic requires that there is "overhead" in the buffer, and pads 16 bytes. I
+ * prefer to use 1 full cache line size for "overhead" so that transfers are
+ * better.  IOVM requires that a buffer be represented by 1 phys_info structure
+ * which can only cover page_size.
+ */
 #define RCVPOST_BUF_SIZE 4032
 #define MAX_NET_RCV_CHAIN \
 	((ETH_MAX_MTU+ETH_HEADER_SIZE + RCVPOST_BUF_SIZE-1) / RCVPOST_BUF_SIZE)
 
 struct net_pkt_rcvpost {
 	    /* rcv buf size must be large enough to include ethernet data len +
-	    * ethernet header len - we are choosing 2K because it is guaranteed
-	    * to be describable */
+	     * ethernet header len - we are choosing 2K because it is guaranteed
+	     * to be describable */
 	    struct phys_info frag;	/* physical page information for the
 					 * single fragment 2K rcv buf */
 	    u64 unique_num;		/* This is used to make sure that
 					 * receive posts are returned to  */
-	    /* the Adapter which sent them origonally. */
-};
+	    /* the Adapter which we sent them originally. */
+} __packed;
 
 struct net_pkt_rcv {
 	/* the number of receive buffers that can be chained  */
@@ -525,34 +401,34 @@ struct net_pkt_rcv {
 	/* NOTE: first rcvbuf in the chain will also be provided in net.buf. */
 	u64 unique_num;
 	u32 rcvs_dropped_delta;
-};
+} __packed;
 
 struct net_pkt_enbdis {
 	void *context;
 	u16 enable;		/* 1 = enable, 0 = disable */
-};
+} __packed;
 
 struct net_pkt_macaddr {
 	void *context;
 	u8 macaddr[MAX_MACADDR_LEN];	/* 6 bytes */
-};
+} __packed;
 
 /* cmd rsp packet used for VNIC network traffic  */
 struct uiscmdrsp_net {
 	enum net_types type;
 	void *buf;
 	union {
-		struct net_pkt_xmt xmt;	/* used for NET_XMIT */
+		struct net_pkt_xmt xmt;		/* used for NET_XMIT */
 		struct net_pkt_xmtdone xmtdone;	/* used for NET_XMIT_DONE */
 		struct net_pkt_rcvpost rcvpost;	/* used for NET_RCV_POST */
-		struct net_pkt_rcv rcv;	/* used for NET_RCV */
+		struct net_pkt_rcv rcv;		/* used for NET_RCV */
 		struct net_pkt_enbdis enbdis;	/* used for NET_RCV_ENBDIS, */
-		/* NET_RCV_ENBDIS_ACK,  */
-		/* NET_RCV_PROMSIC, */
-		/* and NET_CONNECT_STATUS */
+						/* NET_RCV_ENBDIS_ACK,  */
+						/* NET_RCV_PROMSIC, */
+						/* and NET_CONNECT_STATUS */
 		struct net_pkt_macaddr macaddr;
 	};
-};
+} __packed;
 
 struct uiscmdrsp_scsitaskmgmt {
 	enum task_mgmt_types tasktype;
@@ -564,43 +440,45 @@ struct uiscmdrsp_scsitaskmgmt {
 	void *scsicmd;
 
 	    /* This is some handle that the guest has saved off for its own use.
-	    * Its value is preserved by iopart & returned as is in the task mgmt
-	    * rsp. */
+	     * Its value is preserved by iopart & returned as is in the task
+	     * mgmt rsp.
+	     */
 	void *notify;
 
-	    /* For linux guests, this is a pointer to wait_queue_head that a
+	   /* For linux guests, this is a pointer to wait_queue_head that a
 	    * thread is waiting on to see if the taskmgmt command has completed.
 	    * 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 thread waiting for taskmgmt
 	    * command completion.  Its value is preserved by iopart & returned
-	    * as is in the task mgmt rsp. */
+	    * as is in the task mgmt rsp.
+	    */
 	void *notifyresult;
 
 	    /* this is a handle to location in guest where the result of the
-	    * taskmgmt command (result field) is to saved off when the response
-	    * is handled.  Its value is preserved by iopart & returned as is in
-	    * the task mgmt rsp. */
+	     * taskmgmt command (result field) is to saved off when the response
+	     * is handled.  Its value is preserved by iopart & returned as is in
+	     * the task mgmt rsp.
+	     */
 	char result;
 
 	    /* result of taskmgmt command - set by IOPart - values are: */
 #define TASK_MGMT_FAILED  0
-#define TASK_MGMT_SUCCESS 1
-};
+} __packed;
 
 /* The following is used by uissd to send disk add/remove notifications to
  * Guest */
 /* Note that the vHba pointer is not used by the Client/Guest side. */
 struct uiscmdrsp_disknotify {
-	u8 add;		/* 0-remove, 1-add */
+	u8 add;			/* 0-remove, 1-add */
 	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 */
-};
+} __packed;
 
 /* The following is used by virthba/vSCSI to send the Acquire/Release commands
-* to the IOVM.  */
+ * to the IOVM. */
 struct uiscmdrsp_vdiskmgmt {
 	enum vdisk_mgmt_types vdisktype;
 
@@ -611,36 +489,38 @@ struct uiscmdrsp_vdiskmgmt {
 	void *scsicmd;
 
 	    /* This is some handle that the guest has saved off for its own use.
-	    * Its value is preserved by iopart & returned as is in the task mgmt
-	    * rsp. */
+	     * Its value is preserved by iopart & returned as is in the task
+	     * mgmt rsp.
+	     */
 	void *notify;
 
 	    /* For linux guests, this is a pointer to wait_queue_head that a
-	    * thread is waiting on to see if the taskmgmt command has completed.
-	    * 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 thread waiting for taskmgmt
-	    * command completion.  Its value is preserved by iopart & returned
-	    * as is in the task mgmt rsp. */
+	     * thread is waiting on to see if the tskmgmt command has completed.
+	     * For win32 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 thread waiting for taskmgmt
+	     * command completion.  Its value is preserved by iopart & returned
+	     * as is in the task mgmt rsp.
+	     */
 	void *notifyresult;
 
 	    /* this is a handle to location in guest where the result of the
-	    * taskmgmt command (result field) is to saved off when the response
-	    * is handled.  Its value is preserved by iopart & returned as is in
-	    * the task mgmt rsp. */
+	     * taskmgmt command (result field) is to saved off when the response
+	     * is handled.  Its value is preserved by iopart & returned as is in
+	     * the task mgmt rsp.
+	     */
 	char result;
 
 	    /* result of taskmgmt command - set by IOPart - values are: */
 #define VDISK_MGMT_FAILED  0
-#define VDISK_MGMT_SUCCESS 1
-};
+} __packed;
 
 /* keeping cmd & rsp info in one structure for now cmd rsp packet for scsi */
 struct uiscmdrsp {
 	char cmdtype;
 
-	    /* describes what type of information is in the struct */
+/* describes what type of information is in the struct */
 #define CMD_SCSI_TYPE		1
 #define CMD_NET_TYPE		2
 #define CMD_SCSITASKMGMT_TYPE	3
@@ -654,63 +534,44 @@ struct uiscmdrsp {
 		struct uiscmdrsp_vdiskmgmt vdiskmgmt;
 	};
 	void *private_data;	/* used to send the response when the cmd is
-				 * done (scsi & scsittaskmgmt).  */
+				 * done (scsi & scsittaskmgmt). */
 	struct uiscmdrsp *next;	/* General Purpose Queue Link */
 	struct uiscmdrsp *activeQ_next;	/* Used to track active commands */
-	struct uiscmdrsp *activeQ_prev;	/* Used to track active commands  */
-};
-
+	struct uiscmdrsp *activeQ_prev;	/* Used to track active commands */
+} __packed;
+
+struct iochannel_vhba {
+	struct vhba_wwnn wwnn;		/* 8 bytes */
+	struct vhba_config_max max;	/* 20 bytes */
+} __packed;				/* total = 28 bytes */
+struct iochannel_vnic {
+	u8 macaddr[6];			/* 6 bytes */
+	u32 num_rcv_bufs;		/* 4 bytes */
+	u32 mtu;			/* 4 bytes */
+	uuid_le zone_uuid;		/* 16 bytes */
+} __packed;
 /* 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 cmd_q and rsp_q SIGNAL_QUEUE_HEADERS. */
+ * this header there is a large region of memory which contains the command and
+ * 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 */
-			struct vhba_config_max max;	/* 20 bytes */
-		} vhba;		/* 28 */
-		struct {
-			u8 macaddr[MAX_MACADDR_LEN];	/* 6 bytes */
-			u32 num_rcv_bufs;	/* 4 */
-			u32 mtu;	/* 4 */
-			uuid_le zone_uuid;	/* 16 */
-		} vnic;		/* total     30 */
-	};
+		struct iochannel_vhba vhba;
+		struct iochannel_vnic vnic;
+	} __packed;
 
 #define MAX_CLIENTSTRING_LEN 1024
 	 u8 client_string[MAX_CLIENTSTRING_LEN];/* NULL terminated - so holds
 						 * max - 1 bytes */
-};
+} __packed;
 
-#pragma pack(pop)
-/* ///////////// END PRAGMA PACK PUSH 1 /////////////////////////// */
-
-/* define offsets to members of struct uiscmdrsp */
-#define OFFSET_CMDTYPE offsetof(struct uiscmdrsp, cmdtype)
-#define OFFSET_SCSI offsetof(struct uiscmdrsp, scsi)
-#define OFFSET_NET offsetof(struct uiscmdrsp, net)
-#define OFFSET_SCSITASKMGMT offsetof(struct uiscmdrsp, scsitaskmgmt)
-#define OFFSET_NEXT offsetof(struct uiscmdrsp, next)
-
-/* define offsets to members of struct uiscmdrsp_net */
-#define OFFSET_TYPE offsetof(struct uiscmdrsp_net, type)
-#define OFFSET_BUF offsetof(struct uiscmdrsp_net, buf)
-#define OFFSET_XMT offsetof(struct uiscmdrsp_net, xmt)
-#define OFFSET_XMT_DONE_RESULT offsetof(struct uiscmdrsp_net, xmtdone)
-#define OFFSET_RCVPOST offsetof(struct uiscmdrsp_net, rcvpost)
-#define OFFSET_RCV_DONE_LEN offsetof(struct uiscmdrsp_net, rcv)
-#define OFFSET_ENBDIS offsetof(struct uiscmdrsp_net, enbdis)
-
-/* define offsets to members of struct net_pkt_rcvpost */
-#define OFFSET_TOTALLEN offsetof(struct net_pkt_rcvpost, totallen)
-#define	OFFSET_FRAG offsetof(struct net_pkt_rcvpost, frag)
 
 /*
-* INLINE functions for initializing and accessing I/O data channels
-*/
+ * INLINE functions for initializing and accessing I/O data channels
+ */
 
 #define SIZEOF_PROTOCOL (COVER(sizeof(struct spar_io_channel_protocol), 64))
 #define SIZEOF_CMDRSP (COVER(sizeof(struct uiscmdrsp), 64))
@@ -719,16 +580,15 @@ struct spar_io_channel_protocol {
 				  2 * MIN_NUMSIGNALS * SIZEOF_CMDRSP, 4096)
 
 /*
-* INLINE function for expanding a guest's pfn-off-size into multiple 4K page
-* pfn-off-size entires.
-*/
+ * 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. */
 #define PI_PAGE_SIZE  0x1000
 #define PI_PAGE_MASK  0x0FFF
-#define PI_PAGE_SHIFT 12
 
 /* returns next non-zero index on success or zero on failure (i.e. out of
  * room)
diff --git a/drivers/staging/unisys/include/periodic_work.h b/drivers/staging/unisys/include/periodic_work.h
index 26ec10bdfe65..4e19c28dc3d0 100644
--- a/drivers/staging/unisys/include/periodic_work.h
+++ b/drivers/staging/unisys/include/periodic_work.h
@@ -18,7 +18,9 @@
 #ifndef __PERIODIC_WORK_H__
 #define __PERIODIC_WORK_H__
 
-#include "timskmod.h"
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+
 
 /* PERIODIC_WORK an opaque structure to users.
  * Fields are declared only in the implementation .c files.
@@ -31,8 +33,8 @@ struct periodic_work *visor_periodic_work_create(ulong jiffy_interval,
 					void *workfuncarg,
 					const char *devnam);
 void visor_periodic_work_destroy(struct periodic_work *pw);
-BOOL visor_periodic_work_nextperiod(struct periodic_work *pw);
-BOOL visor_periodic_work_start(struct periodic_work *pw);
-BOOL visor_periodic_work_stop(struct periodic_work *pw);
+bool visor_periodic_work_nextperiod(struct periodic_work *pw);
+bool visor_periodic_work_start(struct periodic_work *pw);
+bool visor_periodic_work_stop(struct periodic_work *pw);
 
 #endif
diff --git a/drivers/staging/unisys/include/procobjecttree.h b/drivers/staging/unisys/include/procobjecttree.h
deleted file mode 100644
index 809c6794290e..000000000000
--- a/drivers/staging/unisys/include/procobjecttree.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/* procobjecttree.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/** @file *********************************************************************
- *
- *  This describes the interfaces necessary for creating a tree of types,
- *  objects, and properties in /proc.
- *
- ******************************************************************************
- */
-
-#ifndef __PROCOBJECTTREE_H__
-#define __PROCOBJECTTREE_H__
-
-#include "timskmod.h"
-
-/* These are opaque structures to users.
- * Fields are declared only in the implementation .c files.
- */
-typedef struct MYPROCOBJECT_Tag MYPROCOBJECT;
-typedef struct MYPROCTYPE_Tag   MYPROCTYPE;
-
-MYPROCOBJECT *visor_proc_CreateObject(MYPROCTYPE *type, const char *name,
-				      void *context);
-void          visor_proc_DestroyObject(MYPROCOBJECT *obj);
-MYPROCTYPE   *visor_proc_CreateType(struct proc_dir_entry *procRootDir,
-				    const char **name,
-				    const char **propertyNames,
-				    void (*show_property)(struct seq_file *,
-							  void *, int));
-void          visor_proc_DestroyType(MYPROCTYPE *type);
-
-#endif
diff --git a/drivers/staging/unisys/include/sparstop.h b/drivers/staging/unisys/include/sparstop.h
deleted file mode 100644
index 05837399a741..000000000000
--- a/drivers/staging/unisys/include/sparstop.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/* sparstop.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __SPARSTOP_H__
-#define __SPARSTOP_H__
-
-#include "timskmod.h"
-#include "version.h"
-#include <linux/ctype.h>
-
-typedef void (*SPARSTOP_COMPLETE_FUNC) (void *context, int status);
-
-int sp_stop(void *context, SPARSTOP_COMPLETE_FUNC get_complete_func);
-void test_remove_stop_device(void);
-
-#endif
diff --git a/drivers/staging/unisys/include/timskmod.h b/drivers/staging/unisys/include/timskmod.h
deleted file mode 100644
index cde2494ad896..000000000000
--- a/drivers/staging/unisys/include/timskmod.h
+++ /dev/null
@@ -1,153 +0,0 @@
-/* timskmod.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __TIMSKMOD_H__
-#define __TIMSKMOD_H__
-
-#include <linux/version.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/device.h>
-#include <linux/kobject.h>
-#include <linux/sysfs.h>
-#include <linux/fs.h>
-#include <linux/string.h>
-#include <linux/sched.h>
-#include <linux/spinlock.h>
-#include <linux/slab.h>
-#include <linux/errno.h>
-#include <linux/interrupt.h>
-#include <linux/wait.h>
-#include <linux/vmalloc.h>
-#include <linux/proc_fs.h>
-#include <linux/cdev.h>
-#include <linux/types.h>
-#include <asm/irq.h>
-#include <linux/io.h>
-#include <asm/dma.h>
-#include <linux/uaccess.h>
-#include <linux/list.h>
-#include <linux/poll.h>
-/* #define EXPORT_SYMTAB */
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/fcntl.h>
-#include <linux/workqueue.h>
-#include <linux/kthread.h>
-#include <linux/seq_file.h>
-#include <linux/mm.h>
-
-/* #define DEBUG */
-#ifndef BOOL
-#define BOOL    int
-#endif
-#define FALSE   0
-#define TRUE    1
-#if !defined SUCCESS
-#define SUCCESS 0
-#endif
-#define MIN(a, b)     (((a) < (b)) ? (a) : (b))
-#define MAX(a, b)     (((a) > (b)) ? (a) : (b))
-#define STRUCTSEQUAL(x, y) (memcmp(&x, &y, sizeof(x)) == 0)
-#ifndef HOSTADDRESS
-#define HOSTADDRESS unsigned long long
-#endif
-
-#define sizeofmember(TYPE, MEMBER) (sizeof(((TYPE *)0)->MEMBER))
-/** "Covered quotient" function */
-#define COVQ(v, d)  (((v) + (d) - 1) / (d))
-#define SWAPPOINTERS(p1, p2)				\
-	do {						\
-		void *SWAPPOINTERS_TEMP = (void *)p1;	\
-		(void *)(p1) = (void *)(p2);            \
-		(void *)(p2) = SWAPPOINTERS_TEMP;	\
-	} while (0)
-
-#define WARNDRV(fmt, args...)   LOGWRN(fmt, ## args)
-#define SECUREDRV(fmt, args...) LOGWRN(fmt, ## args)
-
-#define PRINTKDEV(devname, fmt, args...)  LOGINFDEV(devname, fmt, ## args)
-#define TBDDEV(devname, fmt, args...)     LOGERRDEV(devname, fmt, ## args)
-#define HUHDEV(devname, fmt, args...)     LOGERRDEV(devname, fmt, ## args)
-#define ERRDEV(devname, fmt, args...)     LOGERRDEV(devname, fmt, ## args)
-#define ERRDEVX(devno, fmt, args...)	  LOGERRDEVX(devno, fmt, ## args)
-#define WARNDEV(devname, fmt, args...)    LOGWRNDEV(devname, fmt, ## args)
-#define SECUREDEV(devname, fmt, args...)  LOGWRNDEV(devname, fmt, ## args)
-#define INFODEV(devname, fmt, args...)    LOGINFDEV(devname, fmt, ## args)
-#define INFODEVX(devno, fmt, args...)     LOGINFDEVX(devno, fmt, ## args)
-
-/** Verifies the consistency of your PRIVATEDEVICEDATA structure using
- *  conventional "signature" fields:
- *  <p>
- *  - sig1 should contain the size of the structure
- *  - sig2 should contain a pointer to the beginning of the structure
- */
-#define DDLOOKSVALID(dd)                                 \
-		((dd != NULL)                             &&	\
-		 ((dd)->sig1 == sizeof(PRIVATEDEVICEDATA)) &&	\
-		 ((dd)->sig2 == dd))
-
-/** Verifies the consistency of your PRIVATEFILEDATA structure using
- *  conventional "signature" fields:
- *  <p>
- *  - sig1 should contain the size of the structure
- *  - sig2 should contain a pointer to the beginning of the structure
- */
-#define FDLOOKSVALID(fd)                               \
-	((fd != NULL)                           &&     \
-	 ((fd)->sig1 == sizeof(PRIVATEFILEDATA)) &&    \
-	 ((fd)->sig2 == fd))
-
-/** Sleep for an indicated number of seconds (for use in kernel mode).
- *  x - the number of seconds to sleep.
- */
-#define SLEEP(x)					     \
-	do { __set_current_state(TASK_INTERRUPTIBLE);        \
-		schedule_timeout((x)*HZ);		     \
-	} while (0)
-
-/** Sleep for an indicated number of jiffies (for use in kernel mode).
- *  x - the number of jiffies to sleep.
- */
-#define SLEEPJIFFIES(x)						    \
-	do { __set_current_state(TASK_INTERRUPTIBLE);		    \
-		schedule_timeout(x);				    \
-	} while (0)
-
-static inline struct cdev *cdev_alloc_init(struct module *owner,
-					   const struct file_operations *fops)
-{
-	struct cdev *cdev = NULL;
-
-	cdev = cdev_alloc();
-	if (!cdev)
-		return NULL;
-	cdev->ops = fops;
-	cdev->owner = owner;
-
-	/* Note that the memory allocated for cdev will be deallocated
-	 * when the usage count drops to 0, because it is controlled
-	 * by a kobject of type ktype_cdev_dynamic.  (This
-	 * deallocation could very well happen outside of our kernel
-	 * module, like via the cdev_put in __fput() for example.)
-	 */
-	return cdev;
-}
-
-extern int unisys_spar_platform;
-
-#endif
diff --git a/drivers/staging/unisys/include/uisqueue.h b/drivers/staging/unisys/include/uisqueue.h
deleted file mode 100644
index 08ba16ea840e..000000000000
--- a/drivers/staging/unisys/include/uisqueue.h
+++ /dev/null
@@ -1,396 +0,0 @@
-/* uisqueue.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*
- * Unisys IO Virtualization header NOTE: This file contains only Linux
- * specific structs.  All OS-independent structs are in iochannel.h.xx
- */
-
-#ifndef __UISQUEUE_H__
-#define __UISQUEUE_H__
-
-#include "linux/version.h"
-#include "iochannel.h"
-#include <linux/atomic.h>
-#include <linux/semaphore.h>
-#include <linux/uuid.h>
-
-#include "controlvmchannel.h"
-#include "controlvmcompletionstatus.h"
-
-struct uisqueue_info {
-	struct channel_header __iomem *chan;
-	/* channel containing queues in which scsi commands &
-	 * responses are queued
-	 */
-	u64 packets_sent;
-	u64 packets_received;
-	u64 interrupts_sent;
-	u64 interrupts_received;
-	u64 max_not_empty_cnt;
-	u64 total_wakeup_cnt;
-	u64 non_empty_wakeup_cnt;
-
-	struct {
-		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,
-					   unsigned char issue_irq_if_empty,
-					   u64 irq_handle,
-					   unsigned char io_termination);
-};
-
-/* uisqueue_put_cmdrsp_with_lock_client queues a commmand or response
- * to the specified queue, at the tail if the queue is full but
- * oktowait == 0, then it return 0 indicating failure.  otherwise it
- * wait for the queue to become non-full. If command is queued, return
- * 1 for success.
- */
-#define DONT_ISSUE_INTERRUPT 0
-#define ISSUE_INTERRUPT		 1
-
-#define DONT_WAIT			 0
-#define OK_TO_WAIT			 1
-#define UISLIB_LOCK_PREFIX \
-		".section .smp_locks,\"a\"\n"   \
-		_ASM_ALIGN "\n"                 \
-		_ASM_PTR "661f\n" /* address */ \
-		".previous\n"                   \
-		"661:\n\tlock; "
-
-unsigned long long uisqueue_interlocked_or(unsigned long long __iomem *tgt,
-					   unsigned long long set);
-unsigned long long uisqueue_interlocked_and(unsigned long long __iomem *tgt,
-					    unsigned long long set);
-
-int uisqueue_put_cmdrsp_with_lock_client(struct uisqueue_info *queueinfo,
-					 struct uiscmdrsp *cmdrsp,
-					 unsigned int queue,
-					 void *insertlock,
-					 unsigned char issue_irq_if_empty,
-					 u64 irq_handle,
-					 char oktowait,
-					 u8 *channel_id);
-
-/* uisqueue_get_cmdrsp gets the cmdrsp entry at the head of the queue
- * and copies it to the area pointed by cmdrsp param.
- * returns 0 if queue is empty, 1 otherwise
- */
-int
-
-uisqueue_get_cmdrsp(struct uisqueue_info *queueinfo, void *cmdrsp,
-		    unsigned int queue);
-
-#define MAX_NAME_SIZE_UISQUEUE 64
-
-struct extport_info {
-	u8 valid:1;
-	/* if 1, indicates this extport slot is occupied
-	 * if 0, indicates that extport slot is unoccupied */
-
-	u32 num_devs_using;
-	/* When extport is added, this is set to 0.  For exports
-	* located in NETWORK switches:
-	* Each time a VNIC, i.e., intport, is added to the switch this
-	* is used to assign a pref_pnic for the VNIC and when assigned
-	* to a VNIC this counter is incremented. When a VNIC is
-	* deleted, the extport corresponding to the VNIC's pref_pnic
-	* is located and its num_devs_using is decremented. For VNICs,
-	* num_devs_using is basically used to load-balance transmit
-	* traffic from VNICs.
-	*/
-
-	struct switch_info *swtch;
-	struct pci_id pci_id;
-	char name[MAX_NAME_SIZE_UISQUEUE];
-	union {
-		struct vhba_wwnn wwnn;
-		unsigned char macaddr[MAX_MACADDR_LEN];
-	};
-};
-
-struct device_info {
-	void __iomem *chanptr;
-	u64 channel_addr;
-	u64 channel_bytes;
-	uuid_le channel_uuid;
-	uuid_le instance_uuid;
-	struct irq_info intr;
-	struct switch_info *swtch;
-	char devid[30];		/* "vbus<busno>:dev<devno>" */
-	u16 polling;
-	struct semaphore interrupt_callback_lock;
-	u32 bus_no;
-	u32 dev_no;
-	int (*interrupt)(void *);
-	void *interrupt_context;
-	void *private_data;
-	struct list_head list_polling_device_channels;
-	unsigned long long moved_to_tail_cnt;
-	unsigned long long first_busy_cnt;
-	unsigned long long last_on_list_cnt;
-};
-
-enum switch_type {
-	RECOVERY_LAN = 1,
-	IB_LAN = 2
-};
-
-struct bus_info {
-	u32 bus_no, device_count;
-	struct device_info **device;
-	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 partition_name[99];
-	struct bus_info *next;
-	u8 local_vnic;		/* 1 if local vnic created internally
-				 * by IOVM; 0 otherwise... */
-};
-
-struct sn_list_entry {
-	struct uisscsi_dest pdest;	/* scsi bus, target, lun for
-					 * phys disk */
-	u8 sernum[MAX_SERIAL_NUM];	/* serial num of physical
-					 * disk.. The length is always
-					 * MAX_SERIAL_NUM, padded with
-					 * spaces */
-	struct sn_list_entry *next;
-};
-
-/*
- * IO messages sent to UisnicControlChanFunc & UissdControlChanFunc by
- * code that processes the ControlVm channel messages.
- */
-
-enum iopart_msg_type {
-	IOPART_ADD_VNIC,
-	IOPART_DEL_VNIC,
-	IOPART_DEL_ALL_VNICS,
-	IOPART_ADD_VHBA,
-	IOPART_ADD_VDISK,
-	IOPART_DEL_VHBA,
-	IOPART_DEL_VDISK,
-	IOPART_DEL_ALL_VDISKS_FOR_VHBA,
-	IOPART_DEL_ALL_VHBAS,
-	IOPART_ATTACH_PHBA,
-	IOPART_DETACH_PHBA,	/* 10 */
-	IOPART_ATTACH_PNIC,
-	IOPART_DETACH_PNIC,
-	IOPART_DETACH_VHBA,
-	IOPART_DETACH_VNIC,
-	IOPART_PAUSE_VDISK,
-	IOPART_RESUME_VDISK,
-	IOPART_ADD_DEVICE,	/* add generic device */
-	IOPART_DEL_DEVICE,	/* del generic device */
-};
-
-struct add_virt_iopart {
-	void *chanptr;		/* pointer to data channel */
-	u64 guest_handle;	/* used to convert guest physical
-				 * address to real physical address
-				 * for DMA, for ex. */
-	u64 recv_bus_irq_handle;	/* used to register to receive
-					 * bus level interrupts. */
-	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
-	* IO drivers to connect to interrupt.  sendInterruptHandle is
-	* used by IOPart drivers as parameter to
-	* Issue_VMCALL_IO_QUEUE_TRANSITION to interrupt thread in
-	* guest linux/windows IO drivers when data channel queue for
-	* vhba/vnic goes from EMPTY to NON-EMPTY. */
-	struct switch_info *swtch;	/* pointer to the virtual
-					 * switch to which the vnic is
-					 * connected */
-
-	u8 use_g2g_copy;	/* Used to determine if a virtual HBA
-				 * needs to use G2G copy. */
-	u8 filler[7];
-
-	u32 bus_no;
-	u32 dev_no;
-	char *params;
-	ulong params_bytes;
-
-};
-
-struct add_vdisk_iopart {
-	void *chanptr;		      /* pointer to data channel */
-	int implicit;
-	struct uisscsi_dest vdest;    /* scsi bus, target, lun for virt disk */
-	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 */
-};
-
-struct del_vdisk_iopart {
-	void *chanptr;		     /* pointer to data channel */
-	struct uisscsi_dest vdest;   /* scsi bus, target, lun for virt disk */
-};
-
-struct del_virt_iopart {
-	void *chanptr;		     /* pointer to data channel */
-};
-
-struct det_virt_iopart {	     /* detach internal port */
-	void *chanptr;		     /* pointer to data channel */
-	struct switch_info *swtch;
-};
-
-struct paures_vdisk_iopart {
-	void *chanptr;		     /* pointer to data channel */
-	struct uisscsi_dest vdest;   /* scsi bus, target, lun for virt disk */
-};
-
-struct add_switch_iopart {	     /* add switch */
-	struct switch_info *swtch;
-	char *params;
-	ulong params_bytes;
-};
-
-struct del_switch_iopart {	     /* destroy switch */
-	struct switch_info *swtch;
-};
-
-struct io_msgs {
-	enum iopart_msg_type msgtype;
-
-	/* additional params needed by some messages */
-	union {
-		struct add_virt_iopart add_vhba;
-		struct add_virt_iopart add_vnic;
-		struct add_vdisk_iopart add_vdisk;
-		struct del_virt_iopart del_vhba;
-		struct del_virt_iopart del_vnic;
-		struct det_virt_iopart det_vhba;
-		struct det_virt_iopart det_vnic;
-		struct del_vdisk_iopart del_vdisk;
-		struct del_virt_iopart del_all_vdisks_for_vhba;
-		struct add_virt_iopart add_device;
-		struct del_virt_iopart del_device;
-		struct det_virt_iopart det_intport;
-		struct add_switch_iopart add_switch;
-		struct del_switch_iopart del_switch;
-		struct extport_info *ext_port;	/* for attach or detach
-						 * pnic/generic delete all
-						 * vhbas/allvnics need no
-						 * parameters */
-		struct paures_vdisk_iopart paures_vdisk;
-	};
-};
-
-/*
-* Guest messages sent to VirtControlChanFunc by code that processes
-* the ControlVm channel messages.
-*/
-
-enum guestpart_msg_type {
-	GUEST_ADD_VBUS,
-	GUEST_ADD_VHBA,
-	GUEST_ADD_VNIC,
-	GUEST_DEL_VBUS,
-	GUEST_DEL_VHBA,
-	GUEST_DEL_VNIC,
-	GUEST_DEL_ALL_VHBAS,
-	GUEST_DEL_ALL_VNICS,
-	GUEST_DEL_ALL_VBUSES,	/* deletes all vhbas & vnics on all
-				 * buses and deletes all buses */
-	GUEST_PAUSE_VHBA,
-	GUEST_PAUSE_VNIC,
-	GUEST_RESUME_VHBA,
-	GUEST_RESUME_VNIC
-};
-
-struct add_vbus_guestpart {
-	void __iomem *chanptr;		/* pointer to data channel for bus -
-					 * NOT YET USED */
-	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 {
-	u32 bus_no;		/* bus number to be deleted */
-	/* once we start using the bus's channel, add can dump busNo
-	* into the channel header and then delete will need only one
-	* parameter, chanptr. */
-};
-
-struct add_virt_guestpart {
-	void __iomem *chanptr;		/* pointer to data channel */
-	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 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
-	 * monitor VMCALL that will cause an interrupt to be generated
-	 * for the other end.
-	 */
-};
-
-struct pause_virt_guestpart {
-	void __iomem *chanptr;		/* pointer to data channel */
-};
-
-struct resume_virt_guestpart {
-	void __iomem *chanptr;		/* pointer to data channel */
-};
-
-struct del_virt_guestpart {
-	void __iomem *chanptr;		/* pointer to data channel */
-};
-
-struct init_chipset_guestpart {
-	u32 bus_count;		/* indicates the max number of busses */
-	u32 switch_count;	/* indicates the max number of switches */
-};
-
-struct guest_msgs {
-	enum guestpart_msg_type msgtype;
-
-	/* additional params needed by messages */
-	union {
-		struct add_vbus_guestpart add_vbus;
-		struct add_virt_guestpart add_vhba;
-		struct add_virt_guestpart add_vnic;
-		struct pause_virt_guestpart pause_vhba;
-		struct pause_virt_guestpart pause_vnic;
-		struct resume_virt_guestpart resume_vhba;
-		struct resume_virt_guestpart resume_vnic;
-		struct del_vbus_guestpart del_vbus;
-		struct del_virt_guestpart del_vhba;
-		struct del_virt_guestpart del_vnic;
-		struct del_vbus_guestpart del_all_vhbas;
-		struct del_vbus_guestpart del_all_vnics;
-		/* del_all_vbuses needs no parameters */
-	};
-	struct init_chipset_guestpart init_chipset;
-
-};
-
-#endif				/* __UISQUEUE_H__ */
diff --git a/drivers/staging/unisys/include/uisthread.h b/drivers/staging/unisys/include/uisthread.h
deleted file mode 100644
index 52c3eb4ded2c..000000000000
--- a/drivers/staging/unisys/include/uisthread.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/* uisthread.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*****************************************************************************/
-/* Unisys thread utilities header                                            */
-/*****************************************************************************/
-
-#ifndef __UISTHREAD_H__
-#define __UISTHREAD_H__
-
-#include "linux/completion.h"
-
-struct uisthread_info {
-	struct task_struct *task;
-	int id;
-	struct completion has_stopped;
-};
-
-/* returns 0 for failure, 1 for success */
-int uisthread_start(
-	struct uisthread_info *thrinfo,
-	int (*threadfn)(void *),
-	void *thrcontext,
-	char *name);
-
-void uisthread_stop(struct uisthread_info *thrinfo);
-
-#endif /* __UISTHREAD_H__ */
diff --git a/drivers/staging/unisys/include/uisutils.h b/drivers/staging/unisys/include/uisutils.h
deleted file mode 100644
index c7d0ba8aafd8..000000000000
--- a/drivers/staging/unisys/include/uisutils.h
+++ /dev/null
@@ -1,299 +0,0 @@
-/* uisutils.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*
- * Unisys Virtual HBA utilities header
- */
-
-#ifndef __UISUTILS__H__
-#define __UISUTILS__H__
-#include <linux/string.h>
-#include <linux/io.h>
-#include <linux/sched.h>
-#include <linux/gfp.h>
-#include <linux/uuid.h>
-#include <linux/if_ether.h>
-
-#include "vmcallinterface.h"
-#include "channel.h"
-#include "uisthread.h"
-#include "uisqueue.h"
-#include "diagnostics/appos_subsystems.h"
-#include "vbusdeviceinfo.h"
-#include <linux/atomic.h>
-
-/* This is the MAGIC number stuffed by virthba in host->this_id. Used to
- * identify virtual hbas.
- */
-#define UIS_MAGIC_VHBA 707
-
-/* global function pointers that act as callback functions into
- * uisnicmod, uissdmod, and virtpcimod
- */
-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;
-
-struct req_handler_info {
-	uuid_le switch_uuid;
-	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);
-	char switch_type_name[99];
-	struct list_head list_link;	/* links into ReqHandlerInfo_list */
-};
-
-struct req_handler_info *req_handler_find(uuid_le switch_uuid);
-
-#define uislib_ioremap_cache(addr, size) \
-	dbg_ioremap_cache(addr, size, __FILE__, __LINE__)
-
-static inline void __iomem *
-dbg_ioremap_cache(u64 addr, unsigned long size, char *file, int line)
-{
-	void __iomem *new;
-
-	new = ioremap_cache(addr, size);
-	return new;
-}
-
-#define uislib_ioremap(addr, size) dbg_ioremap(addr, size, __FILE__, __LINE__)
-
-static inline void *
-dbg_ioremap(u64 addr, unsigned long size, char *file, int line)
-{
-	void *new;
-
-	new = ioremap(addr, size);
-	return new;
-}
-
-#define uislib_iounmap(addr) dbg_iounmap(addr, __FILE__, __LINE__)
-
-static inline void
-dbg_iounmap(void __iomem *addr, char *file, int line)
-{
-	iounmap(addr);
-}
-
-#define PROC_READ_BUFFER_SIZE 131072	/* size of the buffer to allocate to
-					 * hold all of /proc/XXX/info */
-int uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
-			     char *format, ...);
-
-int uisctrl_register_req_handler(int type, void *fptr,
-			struct ultra_vbus_deviceinfo *chipset_driver_info);
-
-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_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 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 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(enum controlvm_id id,
-		      struct controlvm_message_packet *event);
-
-/* structure used by vhba & vnic to keep track of queue & thread info */
-struct chaninfo {
-	struct uisqueue_info *queueinfo;
-	/* this specifies the queue structures for a channel */
-	/* ALLOCATED BY THE OTHER END - WE JUST GET A POINTER TO THE MEMORY */
-	spinlock_t insertlock;
-	/* currently used only in virtnic when sending data to uisnic */
-	/* to synchronize the inserts into the signal queue */
-	struct uisthread_info threadinfo;
-	/* this specifies the thread structures used by the thread that */
-	/* handles this channel */
-};
-
-/* this is the wait code for all the threads - it is used to get
-* something from a queue choices: wait_for_completion_interruptible,
-* _timeout, interruptible_timeout
-*/
-#define UIS_THREAD_WAIT_MSEC(x) { \
-	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); \
-}
-
-/* This is a hack until we fix IOVM to initialize the channel header
- * correctly at DEVICE_CREATE time, INSTEAD OF waiting until
- * DEVICE_CONFIGURE time.
- */
-static inline void
-wait_for_valid_guid(uuid_le __iomem *guid)
-{
-	uuid_le tmpguid;
-
-	while (1) {
-		memcpy_fromio((void *)&tmpguid,
-			      (void __iomem *)guid, sizeof(uuid_le));
-		if (uuid_le_cmp(tmpguid, NULL_UUID_LE) != 0)
-			break;
-		UIS_THREAD_WAIT_SEC(5);
-	}
-}
-
-static inline unsigned int
-issue_vmcall_io_controlvm_addr(u64 *control_addr, u32 *control_bytes)
-{
-	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.address;
-		*control_bytes = params.channel_bytes;
-	}
-	return result;
-}
-
-static inline unsigned int issue_vmcall_io_diag_addr(u64 *diag_channel_addr)
-{
-	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.address;
-	return result;
-}
-
-static inline unsigned int issue_vmcall_io_visorserial_addr(u64 *channel_addr)
-{
-	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.address;
-	return result;
-}
-
-static inline s64 issue_vmcall_query_guest_virtual_time_offset(void)
-{
-	u64 result = VMCALL_SUCCESS;
-	u64 physaddr = 0;
-
-	ISSUE_IO_VMCALL(VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET, physaddr,
-			result);
-	return result;
-}
-
-struct log_info_t {
-	unsigned long long last_cycles;
-	unsigned long long delta_sum[64];
-	unsigned long long delta_cnt[64];
-	unsigned long long max_delta[64];
-	unsigned long long min_delta[64];
-};
-
-static inline int issue_vmcall_update_physical_time(u64 adjustment)
-{
-	int result = VMCALL_SUCCESS;
-
-	ISSUE_IO_VMCALL(VMCALL_UPDATE_PHYSICAL_TIME, adjustment, result);
-	return result;
-}
-
-static inline unsigned int issue_vmcall_channel_mismatch(const char *chname,
-			      const char *item_name, u32 line_no,
-			      const char *path_n_fn)
-{
-	struct vmcall_channel_version_mismatch_params params;
-	int result = VMCALL_SUCCESS;
-	u64 physaddr;
-	char *last_slash = NULL;
-
-	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.file_name, last_slash, sizeof(params.file_name));
-	} else
-		strlcpy(params.file_name,
-			"Cannot determine source filename",
-			sizeof(params.file_name));
-
-	physaddr = virt_to_phys(&params);
-	ISSUE_IO_VMCALL(VMCALL_CHANNEL_VERSION_MISMATCH, 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__)
-void uislib_cache_free(struct kmem_cache *cur_pool, void *p, char *fn, int ln);
-#define UISCACHEFREE(cur_pool, p) \
-	uislib_cache_free(cur_pool, p, __FILE__, __LINE__)
-
-void uislib_enable_channel_interrupts(u32 bus_no, u32 dev_no,
-				      int (*interrupt)(void *),
-				      void *interrupt_context);
-void uislib_disable_channel_interrupts(u32 bus_no, u32 dev_no);
-void uislib_force_channel_interrupt(u32 bus_no, u32 dev_no);
-
-#endif /* __UISUTILS__H__ */
diff --git a/drivers/staging/unisys/include/vbushelper.h b/drivers/staging/unisys/include/vbushelper.h
index 84abe5f99f54..f272975b2920 100644
--- a/drivers/staging/unisys/include/vbushelper.h
+++ b/drivers/staging/unisys/include/vbushelper.h
@@ -18,8 +18,6 @@
 #ifndef __VBUSHELPER_H__
 #define __VBUSHELPER_H__
 
-#include "vbusdeviceinfo.h"
-
 /* TARGET_HOSTNAME specified as -DTARGET_HOSTNAME=\"thename\" on the
  * command line */
 
diff --git a/drivers/staging/unisys/common-spar/include/version.h b/drivers/staging/unisys/include/version.h
index 83d1da7a2f81..83d1da7a2f81 100644
--- a/drivers/staging/unisys/common-spar/include/version.h
+++ b/drivers/staging/unisys/include/version.h
diff --git a/drivers/staging/unisys/include/visorbus.h b/drivers/staging/unisys/include/visorbus.h
new file mode 100644
index 000000000000..e4a21e42e868
--- /dev/null
+++ b/drivers/staging/unisys/include/visorbus.h
@@ -0,0 +1,222 @@
+/* visorbus.h
+ *
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or (at
+ * your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ */
+
+/*
+ *  This header file is to be included by other kernel mode components that
+ *  implement a particular kind of visor_device.  Each of these other kernel
+ *  mode components is called a visor device driver.  Refer to visortemplate
+ *  for a minimal sample visor device driver.
+ *
+ *  There should be nothing in this file that is private to the visorbus
+ *  bus implementation itself.
+ *
+ */
+
+#ifndef __VISORBUS_H__
+#define __VISORBUS_H__
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/poll.h>
+#include <linux/kernel.h>
+#include <linux/uuid.h>
+
+#include "periodic_work.h"
+#include "channel.h"
+
+struct visor_driver;
+struct visor_device;
+extern struct bus_type visorbus_type;
+
+typedef void (*visorbus_state_complete_func) (struct visor_device *dev,
+					      int status);
+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. */
+};
+
+/** This struct describes a specific Supervisor channel, by providing its
+ *  GUID, name, and sizes.
+ */
+struct visor_channeltype_descriptor {
+	const uuid_le guid;
+	const char *name;
+};
+
+/** Information provided by each visor driver when it registers with the
+ *  visorbus driver.
+ */
+struct visor_driver {
+	const char *name;
+	const char *version;
+	const char *vertag;
+	const char *build_date;
+	const char *build_time;
+	struct module *owner;
+
+	/** Types of channels handled by this driver, ending with 0 GUID.
+	 *  Our specialized BUS.match() method knows about this list, and
+	 *  uses it to determine whether this driver will in fact handle a
+	 *  new device that it has detected.
+	 */
+	struct visor_channeltype_descriptor *channel_types;
+
+	/** Called when a new device comes online, by our probe() function
+	 *  specified by driver.probe() (triggered ultimately by some call
+	 *  to driver_register() / bus_add_driver() / driver_attach()).
+	 */
+	int (*probe)(struct visor_device *dev);
+
+	/** Called when a new device is removed, by our remove() function
+	 *  specified by driver.remove() (triggered ultimately by some call
+	 *  to device_release_driver()).
+	 */
+	void (*remove)(struct visor_device *dev);
+
+	/** Called periodically, whenever there is a possibility that
+	 *  "something interesting" may have happened to the channel state.
+	 */
+	void (*channel_interrupt)(struct visor_device *dev);
+
+	/** Called to initiate a change of the device's state.  If the return
+	 *  valu`e is < 0, there was an error and the state transition will NOT
+	 *  occur.  If the return value is >= 0, then the state transition was
+	 *  INITIATED successfully, and complete_func() will be called (or was
+	 *  just called) with the final status when either the state transition
+	 *  fails or completes successfully.
+	 */
+	int (*pause)(struct visor_device *dev,
+		     visorbus_state_complete_func complete_func);
+	int (*resume)(struct visor_device *dev,
+		      visorbus_state_complete_func complete_func);
+
+	/** These fields are for private use by the bus driver only. */
+	struct device_driver driver;
+	struct driver_attribute version_attr;
+};
+
+#define to_visor_driver(x) container_of(x, struct visor_driver, driver)
+
+/** A device type for things "plugged" into the visorbus bus */
+
+struct visor_device {
+	/** visor driver can use the visorchannel member with the functions
+	 *  defined in visorchannel.h to access the channel
+	 */
+	struct visorchannel *visorchannel;
+	uuid_le channel_type_guid;
+	u64 channel_bytes;
+
+	/** These fields are for private use by the bus driver only.
+	 *  A notable exception is that the visor driver can use
+	 *  visor_get_drvdata() and visor_set_drvdata() to retrieve or stash
+	 *  private visor driver specific data within the device member.
+	 */
+	struct device device;
+	struct list_head list_all;
+	struct periodic_work *periodic_work;
+	bool being_removed;
+	bool responded_to_device_create;
+	struct kobject kobjdevmajorminor; /* visorbus<x>/dev<y>/devmajorminor/*/
+	struct {
+		int major, minor;
+		void *attr;	/* private use by devmajorminor_attr.c you can
+				   * change this constant to whatever you
+				   * want; */
+	} devnodes[5];
+	/* the code will detect and behave appropriately) */
+	struct semaphore visordriver_callback_lock;
+	bool pausing;
+	bool resuming;
+	u32 chipset_bus_no;
+	u32 chipset_dev_no;
+	struct visorchipset_state state;
+	uuid_le type;
+	uuid_le inst;
+	u8 *name;
+	u8 *description;
+	struct controlvm_message_header *pending_msg_hdr;
+	void *vbus_hdr_info;
+	u32 switch_no;
+	u32 internal_port_no;
+	uuid_le partition_uuid;
+};
+
+#define to_visor_device(x) container_of(x, struct visor_device, device)
+
+#ifndef STANDALONE_CLIENT
+int visorbus_register_visor_driver(struct visor_driver *);
+void visorbus_unregister_visor_driver(struct visor_driver *);
+int visorbus_read_channel(struct visor_device *dev,
+			  unsigned long offset, void *dest,
+			  unsigned long nbytes);
+int visorbus_write_channel(struct visor_device *dev,
+			   unsigned long offset, void *src,
+			   unsigned long nbytes);
+int visorbus_clear_channel(struct visor_device *dev,
+			   unsigned long offset, u8 ch, unsigned long nbytes);
+int visorbus_registerdevnode(struct visor_device *dev,
+			     const char *name, int major, int minor);
+void visorbus_enable_channel_interrupts(struct visor_device *dev);
+void visorbus_disable_channel_interrupts(struct visor_device *dev);
+#endif
+
+/* Note that for visorchannel_create()
+ * <channel_bytes> and <guid> arguments may be 0 if we are a channel CLIENT.
+ * In this case, the values can simply be read from the channel header.
+ */
+struct visorchannel *visorchannel_create(u64 physaddr,
+					 unsigned long channel_bytes,
+					 gfp_t gfp, uuid_le guid);
+struct visorchannel *visorchannel_create_with_lock(u64 physaddr,
+						   unsigned long channel_bytes,
+						   gfp_t gfp, uuid_le guid);
+void visorchannel_destroy(struct visorchannel *channel);
+int visorchannel_read(struct visorchannel *channel, ulong offset,
+		      void *local, ulong nbytes);
+int visorchannel_write(struct visorchannel *channel, ulong offset,
+		       void *local, ulong nbytes);
+int visorchannel_clear(struct visorchannel *channel, ulong offset,
+		       u8 ch, ulong nbytes);
+bool visorchannel_signalremove(struct visorchannel *channel, u32 queue,
+			       void *msg);
+bool visorchannel_signalinsert(struct visorchannel *channel, u32 queue,
+			       void *msg);
+int visorchannel_signalqueue_slots_avail(struct visorchannel *channel,
+					 u32 queue);
+int visorchannel_signalqueue_max_slots(struct visorchannel *channel, u32 queue);
+u64 visorchannel_get_physaddr(struct visorchannel *channel);
+ulong visorchannel_get_nbytes(struct visorchannel *channel);
+char *visorchannel_id(struct visorchannel *channel, char *s);
+char *visorchannel_zoneid(struct visorchannel *channel, char *s);
+u64 visorchannel_get_clientpartition(struct visorchannel *channel);
+int visorchannel_set_clientpartition(struct visorchannel *channel,
+				     u64 partition_handle);
+uuid_le visorchannel_get_uuid(struct visorchannel *channel);
+char *visorchannel_uuid_id(uuid_le *guid, char *s);
+void visorchannel_debug(struct visorchannel *channel, int num_queues,
+			struct seq_file *seq, u32 off);
+void __iomem *visorchannel_get_header(struct visorchannel *channel);
+
+#define BUS_ROOT_DEVICE		UINT_MAX
+struct visor_device *visorbus_get_device_by_id(u32 bus_no, u32 dev_no,
+					       struct visor_device *from);
+#endif
diff --git a/drivers/staging/unisys/uislib/Kconfig b/drivers/staging/unisys/uislib/Kconfig
deleted file mode 100644
index c39a0a21ae5f..000000000000
--- a/drivers/staging/unisys/uislib/Kconfig
+++ /dev/null
@@ -1,10 +0,0 @@
-#
-# Unisys uislib configuration
-#
-
-config UNISYS_UISLIB
-	tristate "Unisys uislib driver"
-	select UNISYS_VISORCHIPSET
-	---help---
-	If you say Y here, you will enable the Unisys uislib driver.
-
diff --git a/drivers/staging/unisys/uislib/Makefile b/drivers/staging/unisys/uislib/Makefile
deleted file mode 100644
index 860f494f132f..000000000000
--- a/drivers/staging/unisys/uislib/Makefile
+++ /dev/null
@@ -1,12 +0,0 @@
-#
-# Makefile for Unisys uislib
-#
-
-obj-$(CONFIG_UNISYS_UISLIB)	+= visoruislib.o
-
-visoruislib-y := uislib.o uisqueue.o  uisthread.o  uisutils.o
-
-ccflags-y += -Idrivers/staging/unisys/include
-ccflags-y += -Idrivers/staging/unisys/visorchipset
-ccflags-y += -Idrivers/staging/unisys/common-spar/include
-ccflags-y += -Idrivers/staging/unisys/common-spar/include/channels
diff --git a/drivers/staging/unisys/uislib/uislib.c b/drivers/staging/unisys/uislib/uislib.c
deleted file mode 100644
index f93d0bb11b12..000000000000
--- a/drivers/staging/unisys/uislib/uislib.c
+++ /dev/null
@@ -1,1372 +0,0 @@
-/* uislib.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* @ALL_INSPECTED */
-#define EXPORT_SYMTAB
-#include <linux/kernel.h>
-#include <linux/highmem.h>
-#ifdef CONFIG_MODVERSIONS
-#include <config/modversions.h>
-#endif
-#include <linux/module.h>
-#include <linux/debugfs.h>
-
-#include <linux/types.h>
-#include <linux/uuid.h>
-
-#include <linux/version.h>
-#include "diagnostics/appos_subsystems.h"
-#include "uisutils.h"
-#include "vbuschannel.h"
-
-#include <linux/proc_fs.h>
-#include <linux/uaccess.h>	/* for copy_from_user */
-#include <linux/ctype.h>	/* for toupper */
-#include <linux/list.h>
-
-#include "sparstop.h"
-#include "visorchipset.h"
-#include "version.h"
-#include "guestlinuxdebug.h"
-
-#define SET_PROC_OWNER(x, y)
-
-#define POLLJIFFIES_NORMAL 1
-/* Choose whether or not you want to wakeup the request-polling thread
- * after an IO termination:
- * this is shorter than using __FILE__ (full path name) in
- * debug/info/error messages
- */
-#define CURRENT_FILE_PC UISLIB_PC_uislib_c
-#define __MYFILE__ "uislib.c"
-
-/* global function pointers that act as callback functions into virtpcimod */
-int (*virt_control_chan_func)(struct guest_msgs *);
-
-static int debug_buf_valid;
-static char *debug_buf;	/* Note this MUST be global,
-					 * because the contents must */
-static unsigned int chipset_inited;
-
-#define WAIT_ON_CALLBACK(handle)	\
-	do {			\
-		if (handle)		\
-			break;		\
-		UIS_THREAD_WAIT;	\
-	} while (1)
-
-static struct bus_info *bus_list;
-static rwlock_t bus_list_lock;
-static int bus_list_count;	/* number of buses in the list */
-static int max_bus_count;		/* maximum number of buses expected */
-static u64 phys_data_chan;
-static int platform_no;
-
-static struct uisthread_info incoming_ti;
-static BOOL incoming_started = FALSE;
-static LIST_HEAD(poll_dev_chan);
-static unsigned long long tot_moved_to_tail_cnt;
-static unsigned long long tot_wait_cnt;
-static unsigned long long tot_wakeup_cnt;
-static unsigned long long tot_schedule_cnt;
-static int en_smart_wakeup = 1;
-static DEFINE_SEMAPHORE(poll_dev_lock);	/* unlocked */
-static DECLARE_WAIT_QUEUE_HEAD(poll_dev_wake_q);
-static int poll_dev_start;
-
-#define CALLHOME_PROC_ENTRY_FN "callhome"
-#define CALLHOME_THROTTLED_PROC_ENTRY_FN "callhome_throttled"
-
-#define DIR_DEBUGFS_ENTRY "uislib"
-static struct dentry *dir_debugfs;
-
-#define PLATFORMNUMBER_DEBUGFS_ENTRY_FN "platform"
-static struct dentry *platformnumber_debugfs_read;
-
-#define CYCLES_BEFORE_WAIT_DEBUGFS_ENTRY_FN "cycles_before_wait"
-static struct dentry *cycles_before_wait_debugfs_read;
-
-#define SMART_WAKEUP_DEBUGFS_ENTRY_FN "smart_wakeup"
-static struct dentry *smart_wakeup_debugfs_entry;
-
-#define INFO_DEBUGFS_ENTRY_FN "info"
-static struct dentry *info_debugfs_entry;
-
-static unsigned long long cycles_before_wait, wait_cycles;
-
-/*****************************************************/
-/* local functions                                   */
-/*****************************************************/
-
-static ssize_t info_debugfs_read(struct file *file, char __user *buf,
-				 size_t len, loff_t *offset);
-static const struct file_operations debugfs_info_fops = {
-	.read = info_debugfs_read,
-};
-
-static void
-init_msg_header(struct controlvm_message *msg, u32 id, uint rsp, uint 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 *init_vbus_channel(u64 ch_addr, u32 ch_bytes)
-{
-	void __iomem *ch = uislib_ioremap_cache(ch_addr, ch_bytes);
-
-	if (!ch)
-		return NULL;
-
-	if (!SPAR_VBUS_CHANNEL_OK_CLIENT(ch)) {
-		uislib_iounmap(ch);
-		return NULL;
-	}
-	return ch;
-}
-
-static int
-create_bus(struct controlvm_message *msg, char *buf)
-{
-	u32 bus_no, dev_count;
-	struct bus_info *tmp, *bus;
-	size_t size;
-
-	if (max_bus_count == bus_list_count) {
-		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, max_bus_count,
-				 POSTCODE_SEVERITY_ERR);
-		return CONTROLVM_RESP_ERROR_MAX_BUSES;
-	}
-
-	bus_no = msg->cmd.create_bus.bus_no;
-	dev_count = msg->cmd.create_bus.dev_count;
-
-	POSTCODE_LINUX_4(BUS_CREATE_ENTRY_PC, bus_no, dev_count,
-			 POSTCODE_SEVERITY_INFO);
-
-	size =
-	    sizeof(struct bus_info) +
-	    (dev_count * sizeof(struct device_info *));
-	bus = kzalloc(size, GFP_ATOMIC);
-	if (!bus) {
-		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
-				 POSTCODE_SEVERITY_ERR);
-		return CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
-	}
-
-	/* Currently by default, the bus Number is the GuestHandle.
-	 * Configure Bus message can override this.
-	 */
-	if (msg->hdr.flags.test_message) {
-		/* This implies we're the IOVM so set guest handle to 0... */
-		bus->guest_handle = 0;
-		bus->bus_no = bus_no;
-		bus->local_vnic = 1;
-	} else {
-		bus->bus_no = bus_no;
-		bus->guest_handle = bus_no;
-	}
-	sprintf(bus->name, "%d", (int)bus->bus_no);
-	bus->device_count = dev_count;
-	bus->device =
-	    (struct device_info **)((char *)bus + sizeof(struct bus_info));
-	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(&bus_list_lock);
-	for (tmp = bus_list; tmp; tmp = tmp->next) {
-		if (tmp->bus_no == bus->bus_no)
-			break;
-	}
-	read_unlock(&bus_list_lock);
-	if (tmp) {
-		/* found a bus already in the list with same bus_no -
-		 * reject add
-		 */
-		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
-				 POSTCODE_SEVERITY_ERR);
-		kfree(bus);
-		return CONTROLVM_RESP_ERROR_ALREADY_DONE;
-	}
-	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) {
-		struct guest_msgs cmd;
-
-		cmd.msgtype = GUEST_ADD_VBUS;
-		cmd.add_vbus.bus_no = bus_no;
-		cmd.add_vbus.chanptr = bus->bus_channel;
-		cmd.add_vbus.dev_count = dev_count;
-		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) {
-			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
-					 POSTCODE_SEVERITY_ERR);
-			kfree(bus);
-			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-		}
-		if (!virt_control_chan_func(&cmd)) {
-			POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus->bus_no,
-					 POSTCODE_SEVERITY_ERR);
-			kfree(bus);
-			return
-			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-		}
-	}
-
-	/* add bus at the head of our list */
-	write_lock(&bus_list_lock);
-	if (!bus_list) {
-		bus_list = bus;
-	} else {
-		bus->next = bus_list;
-		bus_list = bus;
-	}
-	bus_list_count++;
-	write_unlock(&bus_list_lock);
-
-	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, bus->bus_no,
-			 POSTCODE_SEVERITY_INFO);
-	return CONTROLVM_RESP_SUCCESS;
-}
-
-static int
-destroy_bus(struct controlvm_message *msg, char *buf)
-{
-	int i;
-	struct bus_info *bus, *prev = NULL;
-	struct guest_msgs cmd;
-	u32 bus_no;
-
-	bus_no = msg->cmd.destroy_bus.bus_no;
-
-	read_lock(&bus_list_lock);
-
-	bus = bus_list;
-	while (bus) {
-		if (bus->bus_no == bus_no)
-			break;
-		prev = bus;
-		bus = bus->next;
-	}
-
-	if (!bus) {
-		read_unlock(&bus_list_lock);
-		return CONTROLVM_RESP_ERROR_ALREADY_DONE;
-	}
-
-	/* verify that this bus has no devices. */
-	for (i = 0; i < bus->device_count; i++) {
-		if (bus->device[i]) {
-			read_unlock(&bus_list_lock);
-			return CONTROLVM_RESP_ERROR_BUS_DEVICE_ATTACHED;
-		}
-	}
-	read_unlock(&bus_list_lock);
-
-	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 = bus_no;
-	if (!virt_control_chan_func)
-		return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-
-	if (!virt_control_chan_func(&cmd))
-		return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-
-	/* finally, remove the bus from the list */
-remove:
-	write_lock(&bus_list_lock);
-	if (prev)	/* not at head */
-		prev->next = bus->next;
-	else
-		bus_list = bus->next;
-	bus_list_count--;
-	write_unlock(&bus_list_lock);
-
-	if (bus->bus_channel) {
-		uislib_iounmap(bus->bus_channel);
-		bus->bus_channel = NULL;
-	}
-
-	kfree(bus);
-	return CONTROLVM_RESP_SUCCESS;
-}
-
-static int create_device(struct controlvm_message *msg, char *buf)
-{
-	struct device_info *dev;
-	struct bus_info *bus;
-	struct guest_msgs cmd;
-	u32 bus_no, dev_no;
-	int result = CONTROLVM_RESP_SUCCESS;
-	u64 min_size = MIN_IO_CHANNEL_SIZE;
-	struct req_handler_info *req_handler;
-
-	bus_no = msg->cmd.create_device.bus_no;
-	dev_no = msg->cmd.create_device.dev_no;
-
-	POSTCODE_LINUX_4(DEVICE_CREATE_ENTRY_PC, dev_no, bus_no,
-			 POSTCODE_SEVERITY_INFO);
-
-	dev = kzalloc(sizeof(*dev), GFP_ATOMIC);
-	if (!dev) {
-		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
-				 POSTCODE_SEVERITY_ERR);
-		return CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
-	}
-
-	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 = bus_no;
-	dev->dev_no = dev_no;
-	sema_init(&dev->interrupt_callback_lock, 1);	/* unlocked */
-	sprintf(dev->devid, "vbus%u:dev%u", (unsigned)bus_no, (unsigned)dev_no);
-	/* map the channel memory for the device. */
-	if (msg->hdr.flags.test_message) {
-		dev->chanptr = (void __iomem *)__va(dev->channel_addr);
-	} else {
-		req_handler = req_handler_find(dev->channel_uuid);
-		if (req_handler)
-			/* generic service handler registered for this
-			 * channel
-			 */
-			min_size = req_handler->min_channel_bytes;
-		if (min_size > msg->cmd.create_device.channel_bytes) {
-			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
-					 bus_no, POSTCODE_SEVERITY_ERR);
-			result = CONTROLVM_RESP_ERROR_CHANNEL_SIZE_TOO_SMALL;
-			goto cleanup;
-		}
-		dev->chanptr =
-		    uislib_ioremap_cache(dev->channel_addr,
-					 msg->cmd.create_device.channel_bytes);
-		if (!dev->chanptr) {
-			result = CONTROLVM_RESP_ERROR_IOREMAP_FAILED;
-			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
-					 bus_no, POSTCODE_SEVERITY_ERR);
-			goto cleanup;
-		}
-	}
-	dev->instance_uuid = msg->cmd.create_device.dev_inst_uuid;
-	dev->channel_bytes = msg->cmd.create_device.channel_bytes;
-
-	read_lock(&bus_list_lock);
-	for (bus = bus_list; bus; bus = bus->next) {
-		if (bus->bus_no != bus_no)
-			continue;
-		/* make sure the device number is valid */
-		if (dev_no >= bus->device_count) {
-			result = CONTROLVM_RESP_ERROR_MAX_DEVICES;
-			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
-					 bus_no, POSTCODE_SEVERITY_ERR);
-			read_unlock(&bus_list_lock);
-			goto cleanup;
-		}
-		/* make sure this device is not already set */
-		if (bus->device[dev_no]) {
-			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC,
-					 dev_no, bus_no,
-					 POSTCODE_SEVERITY_ERR);
-			result = CONTROLVM_RESP_ERROR_ALREADY_DONE;
-			read_unlock(&bus_list_lock);
-			goto cleanup;
-		}
-		read_unlock(&bus_list_lock);
-		/* the msg is bound for virtpci; send
-		 * guest_msgs struct to callback
-		 */
-		if (msg->hdr.flags.server) {
-			bus->device[dev_no] = dev;
-			POSTCODE_LINUX_4(DEVICE_CREATE_SUCCESS_PC, dev_no,
-					 bus_no, POSTCODE_SEVERITY_INFO);
-			return CONTROLVM_RESP_SUCCESS;
-		}
-		if (uuid_le_cmp(dev->channel_uuid,
-				spar_vhba_channel_protocol_uuid) == 0) {
-			wait_for_valid_guid(&((struct channel_header __iomem *)
-					    (dev->chanptr))->chtype);
-			if (!SPAR_VHBA_CHANNEL_OK_CLIENT(dev->chanptr)) {
-				POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC,
-						 dev_no, bus_no,
-						 POSTCODE_SEVERITY_ERR);
-				result = CONTROLVM_RESP_ERROR_CHANNEL_INVALID;
-				goto cleanup;
-			}
-			cmd.msgtype = GUEST_ADD_VHBA;
-			cmd.add_vhba.chanptr = dev->chanptr;
-			cmd.add_vhba.bus_no = bus_no;
-			cmd.add_vhba.device_no = dev_no;
-			cmd.add_vhba.instance_uuid = dev->instance_uuid;
-			cmd.add_vhba.intr = dev->intr;
-		} else if (uuid_le_cmp(dev->channel_uuid,
-				       spar_vnic_channel_protocol_uuid) == 0) {
-			wait_for_valid_guid(&((struct channel_header __iomem *)
-					    (dev->chanptr))->chtype);
-			if (!SPAR_VNIC_CHANNEL_OK_CLIENT(dev->chanptr)) {
-				POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC,
-						 dev_no, bus_no,
-						 POSTCODE_SEVERITY_ERR);
-				result = CONTROLVM_RESP_ERROR_CHANNEL_INVALID;
-				goto cleanup;
-			}
-			cmd.msgtype = GUEST_ADD_VNIC;
-			cmd.add_vnic.chanptr = dev->chanptr;
-			cmd.add_vnic.bus_no = bus_no;
-			cmd.add_vnic.device_no = dev_no;
-			cmd.add_vnic.instance_uuid = dev->instance_uuid;
-			cmd.add_vhba.intr = dev->intr;
-		} else {
-			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
-					 bus_no, POSTCODE_SEVERITY_ERR);
-			result = CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
-			goto cleanup;
-		}
-
-		if (!virt_control_chan_func) {
-			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
-					 bus_no, POSTCODE_SEVERITY_ERR);
-			result = CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-			goto cleanup;
-		}
-
-		if (!virt_control_chan_func(&cmd)) {
-			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no,
-					 bus_no, POSTCODE_SEVERITY_ERR);
-			result =
-			     CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-			goto cleanup;
-		}
-
-		bus->device[dev_no] = dev;
-		POSTCODE_LINUX_4(DEVICE_CREATE_SUCCESS_PC, dev_no,
-				 bus_no, POSTCODE_SEVERITY_INFO);
-		return CONTROLVM_RESP_SUCCESS;
-	}
-	read_unlock(&bus_list_lock);
-
-	POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
-			 POSTCODE_SEVERITY_ERR);
-	result = CONTROLVM_RESP_ERROR_BUS_INVALID;
-
-cleanup:
-	if (!msg->hdr.flags.test_message) {
-		uislib_iounmap(dev->chanptr);
-		dev->chanptr = NULL;
-	}
-
-	kfree(dev);
-	return result;
-}
-
-static int pause_device(struct controlvm_message *msg)
-{
-	u32 bus_no, dev_no;
-	struct bus_info *bus;
-	struct device_info *dev;
-	struct guest_msgs cmd;
-	int retval = CONTROLVM_RESP_SUCCESS;
-
-	bus_no = msg->cmd.device_change_state.bus_no;
-	dev_no = msg->cmd.device_change_state.dev_no;
-
-	read_lock(&bus_list_lock);
-	for (bus = bus_list; bus; bus = bus->next) {
-		if (bus->bus_no == bus_no) {
-			/* make sure the device number is valid */
-			if (dev_no >= bus->device_count) {
-				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
-			} else {
-				/* make sure this device exists */
-				dev = bus->device[dev_no];
-				if (!dev) {
-					retval =
-					  CONTROLVM_RESP_ERROR_ALREADY_DONE;
-				}
-			}
-			break;
-		}
-	}
-	if (!bus)
-		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
-
-	read_unlock(&bus_list_lock);
-	if (retval == CONTROLVM_RESP_SUCCESS) {
-		/* the msg is bound for virtpci; send
-		 * guest_msgs struct to callback
-		 */
-		if (uuid_le_cmp(dev->channel_uuid,
-				spar_vhba_channel_protocol_uuid) == 0) {
-			cmd.msgtype = GUEST_PAUSE_VHBA;
-			cmd.pause_vhba.chanptr = dev->chanptr;
-		} else if (uuid_le_cmp(dev->channel_uuid,
-				       spar_vnic_channel_protocol_uuid) == 0) {
-			cmd.msgtype = GUEST_PAUSE_VNIC;
-			cmd.pause_vnic.chanptr = dev->chanptr;
-		} else {
-			return CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
-		}
-		if (!virt_control_chan_func)
-			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-		if (!virt_control_chan_func(&cmd)) {
-			return
-			  CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-		}
-	}
-	return retval;
-}
-
-static int resume_device(struct controlvm_message *msg)
-{
-	u32 bus_no, dev_no;
-	struct bus_info *bus;
-	struct device_info *dev;
-	struct guest_msgs cmd;
-	int retval = CONTROLVM_RESP_SUCCESS;
-
-	bus_no = msg->cmd.device_change_state.bus_no;
-	dev_no = msg->cmd.device_change_state.dev_no;
-
-	read_lock(&bus_list_lock);
-	for (bus = bus_list; bus; bus = bus->next) {
-		if (bus->bus_no == bus_no) {
-			/* make sure the device number is valid */
-			if (dev_no >= bus->device_count) {
-				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
-			} else {
-				/* make sure this device exists */
-				dev = bus->device[dev_no];
-				if (!dev) {
-					retval =
-					  CONTROLVM_RESP_ERROR_ALREADY_DONE;
-				}
-			}
-			break;
-		}
-	}
-
-	if (!bus)
-		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
-
-	read_unlock(&bus_list_lock);
-	/* the msg is bound for virtpci; send
-	 * guest_msgs struct to callback
-	 */
-	if (retval == CONTROLVM_RESP_SUCCESS) {
-		if (uuid_le_cmp(dev->channel_uuid,
-				spar_vhba_channel_protocol_uuid) == 0) {
-			cmd.msgtype = GUEST_RESUME_VHBA;
-			cmd.resume_vhba.chanptr = dev->chanptr;
-		} else if (uuid_le_cmp(dev->channel_uuid,
-				       spar_vnic_channel_protocol_uuid) == 0) {
-			cmd.msgtype = GUEST_RESUME_VNIC;
-			cmd.resume_vnic.chanptr = dev->chanptr;
-		} else {
-			return CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
-		}
-		if (!virt_control_chan_func)
-			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-		if (!virt_control_chan_func(&cmd)) {
-			return
-			  CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-		}
-	}
-	return retval;
-}
-
-static int destroy_device(struct controlvm_message *msg, char *buf)
-{
-	u32 bus_no, dev_no;
-	struct bus_info *bus;
-	struct device_info *dev;
-	struct guest_msgs cmd;
-	int retval = CONTROLVM_RESP_SUCCESS;
-
-	bus_no = msg->cmd.destroy_device.bus_no;
-	dev_no = msg->cmd.destroy_device.bus_no;
-
-	read_lock(&bus_list_lock);
-	for (bus = bus_list; bus; bus = bus->next) {
-		if (bus->bus_no == bus_no) {
-			/* make sure the device number is valid */
-			if (dev_no >= bus->device_count) {
-				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
-			} else {
-				/* make sure this device exists */
-				dev = bus->device[dev_no];
-				if (!dev) {
-					retval =
-					     CONTROLVM_RESP_ERROR_ALREADY_DONE;
-				}
-			}
-			break;
-		}
-	}
-
-	if (!bus)
-		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
-	read_unlock(&bus_list_lock);
-	if (retval == CONTROLVM_RESP_SUCCESS) {
-		/* the msg is bound for virtpci; send
-		 * guest_msgs struct to callback
-		 */
-		if (uuid_le_cmp(dev->channel_uuid,
-				spar_vhba_channel_protocol_uuid) == 0) {
-			cmd.msgtype = GUEST_DEL_VHBA;
-			cmd.del_vhba.chanptr = dev->chanptr;
-		} else if (uuid_le_cmp(dev->channel_uuid,
-				       spar_vnic_channel_protocol_uuid) == 0) {
-			cmd.msgtype = GUEST_DEL_VNIC;
-			cmd.del_vnic.chanptr = dev->chanptr;
-		} else {
-			return
-			    CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
-		}
-		if (!virt_control_chan_func) {
-			return
-			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-		}
-		if (!virt_control_chan_func(&cmd)) {
-			return
-			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-		}
-/* you must disable channel interrupts BEFORE you unmap the channel,
- * because if you unmap first, there may still be some activity going
- * on which accesses the channel and you will get a "unable to handle
- * kernel paging request"
- */
-		if (dev->polling)
-			uislib_disable_channel_interrupts(bus_no, dev_no);
-		/* unmap the channel memory for the device. */
-		if (!msg->hdr.flags.test_message)
-			uislib_iounmap(dev->chanptr);
-		kfree(dev);
-		bus->device[dev_no] = NULL;
-	}
-	return retval;
-}
-
-static int
-init_chipset(struct controlvm_message *msg, char *buf)
-{
-	POSTCODE_LINUX_2(CHIPSET_INIT_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-
-	max_bus_count = msg->cmd.init_chipset.bus_count;
-	platform_no = msg->cmd.init_chipset.platform_number;
-	phys_data_chan = 0;
-
-	/* We need to make sure we have our functions registered
-	* before processing messages.  If we are a test vehicle 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 test_message is set, we will wait for the
-	* functions.
-	*/
-	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);
-
-	return CONTROLVM_RESP_SUCCESS;
-}
-
-static int delete_bus_glue(u32 bus_no)
-{
-	struct controlvm_message msg;
-
-	init_msg_header(&msg, CONTROLVM_BUS_DESTROY, 0, 0);
-	msg.cmd.destroy_bus.bus_no = bus_no;
-	if (destroy_bus(&msg, NULL) != CONTROLVM_RESP_SUCCESS)
-		return 0;
-	return 1;
-}
-
-static int delete_device_glue(u32 bus_no, u32 dev_no)
-{
-	struct controlvm_message msg;
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_DESTROY, 0, 0);
-	msg.cmd.destroy_device.bus_no = bus_no;
-	msg.cmd.destroy_device.dev_no = dev_no;
-	if (destroy_device(&msg, NULL) != CONTROLVM_RESP_SUCCESS)
-		return 0;
-	return 1;
-}
-
-int
-uislib_client_inject_add_bus(u32 bus_no, uuid_le inst_uuid,
-			     u64 channel_addr, ulong n_channel_bytes)
-{
-	struct controlvm_message msg;
-
-	/* step 0: init the chipset */
-	POSTCODE_LINUX_3(CHIPSET_INIT_ENTRY_PC, bus_no, POSTCODE_SEVERITY_INFO);
-
-	if (!chipset_inited) {
-		/* step: initialize the chipset */
-		init_msg_header(&msg, CONTROLVM_CHIPSET_INIT, 0, 0);
-		/* this change is needed so that console will come up
-		* OK even when the bus 0 create comes in late.  If the
-		* bus 0 create is the first create, then the add_vnic
-		* will work fine, but if the bus 0 create arrives
-		* after number 4, then the add_vnic will fail, and the
-		* ultraboot will fail.
-		*/
-		msg.cmd.init_chipset.bus_count = 23;
-		msg.cmd.init_chipset.switch_count = 0;
-		if (init_chipset(&msg, NULL) != CONTROLVM_RESP_SUCCESS)
-			return 0;
-		POSTCODE_LINUX_3(CHIPSET_INIT_EXIT_PC, bus_no,
-				 POSTCODE_SEVERITY_INFO);
-	}
-
-	/* step 1: create a bus */
-	POSTCODE_LINUX_3(BUS_CREATE_ENTRY_PC, bus_no,
-			 POSTCODE_SEVERITY_WARNING);
-	init_msg_header(&msg, CONTROLVM_BUS_CREATE, 0, 0);
-	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) {
-		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
-				 POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, bus_no, POSTCODE_SEVERITY_INFO);
-
-	return 1;
-}
-EXPORT_SYMBOL_GPL(uislib_client_inject_add_bus);
-
-int
-uislib_client_inject_del_bus(u32 bus_no)
-{
-	return delete_bus_glue(bus_no);
-}
-EXPORT_SYMBOL_GPL(uislib_client_inject_del_bus);
-
-int
-uislib_client_inject_pause_vhba(u32 bus_no, u32 dev_no)
-{
-	struct controlvm_message msg;
-	int rc;
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_CHANGESTATE, 0, 0);
-	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)
-		return rc;
-	return 0;
-}
-EXPORT_SYMBOL_GPL(uislib_client_inject_pause_vhba);
-
-int
-uislib_client_inject_resume_vhba(u32 bus_no, u32 dev_no)
-{
-	struct controlvm_message msg;
-	int rc;
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_CHANGESTATE, 0, 0);
-	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)
-		return rc;
-	return 0;
-}
-EXPORT_SYMBOL_GPL(uislib_client_inject_resume_vhba);
-
-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 inst_uuid,
-			      struct irq_info *intr)
-{
-	struct controlvm_message msg;
-
-	/* 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, dev_no, bus_no,
-			 POSTCODE_SEVERITY_INFO);
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_CREATE, 0, 0);
-	if (is_test_addr)
-		/* signify that the physical channel address does NOT
-		 * need to be ioremap()ed
-		 */
-		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.create_device.intr = *intr;
-	else
-		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) {
-		POSTCODE_LINUX_4(VHBA_CREATE_FAILURE_PC, chan_bytes,
-				 MIN_IO_CHANNEL_SIZE, POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-	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) {
-		POSTCODE_LINUX_4(VHBA_CREATE_FAILURE_PC, dev_no, bus_no,
-				 POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-	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 bus_no, u32 dev_no)
-{
-	return delete_device_glue(bus_no, dev_no);
-}
-EXPORT_SYMBOL_GPL(uislib_client_inject_del_vhba);
-
-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 inst_uuid,
-			      struct irq_info *intr)
-{
-	struct controlvm_message msg;
-
-	/* 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, dev_no, bus_no,
-			 POSTCODE_SEVERITY_INFO);
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_CREATE, 0, 0);
-	if (is_test_addr)
-		/* signify that the physical channel address does NOT
-		 * need to be ioremap()ed
-		 */
-		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.create_device.intr = *intr;
-	else
-		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) {
-		POSTCODE_LINUX_4(VNIC_CREATE_FAILURE_PC, chan_bytes,
-				 MIN_IO_CHANNEL_SIZE, POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-	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) {
-		POSTCODE_LINUX_4(VNIC_CREATE_FAILURE_PC, dev_no, bus_no,
-				 POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-
-	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 bus_no, u32 dev_no)
-{
-	struct controlvm_message msg;
-	int rc;
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_CHANGESTATE, 0, 0);
-	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)
-		return -1;
-	return 0;
-}
-EXPORT_SYMBOL_GPL(uislib_client_inject_pause_vnic);
-
-int
-uislib_client_inject_resume_vnic(u32 bus_no, u32 dev_no)
-{
-	struct controlvm_message msg;
-	int rc;
-
-	init_msg_header(&msg, CONTROLVM_DEVICE_CHANGESTATE, 0, 0);
-	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)
-		return -1;
-	return 0;
-}
-EXPORT_SYMBOL_GPL(uislib_client_inject_resume_vnic);
-
-int
-uislib_client_inject_del_vnic(u32 bus_no, u32 dev_no)
-{
-	return delete_device_glue(bus_no, dev_no);
-}
-EXPORT_SYMBOL_GPL(uislib_client_inject_del_vnic);
-
-void *
-uislib_cache_alloc(struct kmem_cache *cur_pool, char *fn, int ln)
-{
-	/* __GFP_NORETRY means "ok to fail", meaning kmalloc() can
-	* return NULL.  If you do NOT specify __GFP_NORETRY, Linux
-	* will go to extreme measures to get memory for you (like,
-	* invoke oom killer), which will probably cripple the system.
-	*/
-	void *p = kmem_cache_alloc(cur_pool, GFP_ATOMIC | __GFP_NORETRY);
-
-	if (!p)
-		return NULL;
-	return p;
-}
-EXPORT_SYMBOL_GPL(uislib_cache_alloc);
-
-void
-uislib_cache_free(struct kmem_cache *cur_pool, void *p, char *fn, int ln)
-{
-	if (!p)
-		return;
-	kmem_cache_free(cur_pool, p);
-}
-EXPORT_SYMBOL_GPL(uislib_cache_free);
-
-/*****************************************************/
-/* proc filesystem callback functions                */
-/*****************************************************/
-
-#define PLINE(...) uisutil_add_proc_line_ex(&tot, buff, \
-					       buff_len, __VA_ARGS__)
-
-static int
-info_debugfs_read_helper(char **buff, int *buff_len)
-{
-	int i, tot = 0;
-	struct bus_info *bus;
-
-	if (PLINE("\nBuses:\n") < 0)
-		goto err_done;
-
-	read_lock(&bus_list_lock);
-	for (bus = bus_list; bus; bus = bus->next) {
-		if (PLINE("    bus=0x%p, busNo=%d, deviceCount=%d\n",
-			  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->device_count; i++) {
-			if (bus->device[i]) {
-				if (PLINE("            busNo %d, device[%i]: 0x%p, chanptr=0x%p, swtch=0x%p\n",
-					  bus->bus_no, i, bus->device[i],
-					  bus->device[i]->chanptr,
-					  bus->device[i]->swtch) < 0)
-					goto err_done_unlock;
-
-				if (PLINE("            first_busy_cnt=%llu, moved_to_tail_cnt=%llu, last_on_list_cnt=%llu\n",
-					  bus->device[i]->first_busy_cnt,
-					  bus->device[i]->moved_to_tail_cnt,
-					  bus->device[i]->last_on_list_cnt) < 0)
-					goto err_done_unlock;
-			}
-		}
-	}
-	read_unlock(&bus_list_lock);
-
-	if (PLINE("UisUtils_Registered_Services: %d\n",
-		  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)
-			goto err_done;
-	if (PLINE("tot_wakeup_cnt %llu:tot_wait_cnt %llu:tot_schedule_cnt %llu\n",
-		  tot_wakeup_cnt, tot_wait_cnt, tot_schedule_cnt) < 0)
-			goto err_done;
-	if (PLINE("en_smart_wakeup %d\n", en_smart_wakeup) < 0)
-			goto err_done;
-	if (PLINE("tot_moved_to_tail_cnt %llu\n", tot_moved_to_tail_cnt) < 0)
-			goto err_done;
-
-	return tot;
-
-err_done_unlock:
-	read_unlock(&bus_list_lock);
-err_done:
-	return -1;
-}
-
-static ssize_t info_debugfs_read(struct file *file, char __user *buf,
-				 size_t len, loff_t *offset)
-{
-	char *temp;
-	int total_bytes = 0;
-	int remaining_bytes = PROC_READ_BUFFER_SIZE;
-
-/* *start = buf; */
-	if (!debug_buf) {
-		debug_buf = vmalloc(PROC_READ_BUFFER_SIZE);
-
-		if (!debug_buf)
-			return -ENOMEM;
-	}
-
-	temp = debug_buf;
-
-	if ((*offset == 0) || (!debug_buf_valid)) {
-		/* if the read fails, then -1 will be returned */
-		total_bytes = info_debugfs_read_helper(&temp, &remaining_bytes);
-		debug_buf_valid = 1;
-	} else {
-		total_bytes = strlen(debug_buf);
-	}
-
-	return simple_read_from_buffer(buf, len, offset,
-				       debug_buf, total_bytes);
-}
-
-static struct device_info *find_dev(u32 bus_no, u32 dev_no)
-{
-	struct bus_info *bus;
-	struct device_info *dev = NULL;
-
-	read_lock(&bus_list_lock);
-	for (bus = bus_list; bus; bus = bus->next) {
-		if (bus->bus_no == bus_no) {
-			/* make sure the device number is valid */
-			if (dev_no >= bus->device_count)
-				break;
-			dev = bus->device[dev_no];
-			break;
-		}
-	}
-	read_unlock(&bus_list_lock);
-	return dev;
-}
-
-/*  This thread calls the "interrupt" function for each device that has
- *  enabled such using uislib_enable_channel_interrupts().  The "interrupt"
- *  function typically reads and processes the devices's channel input
- *  queue.  This thread repeatedly does this, until the thread is told to stop
- *  (via uisthread_stop()).  Sleeping rules:
- *  - If we have called the "interrupt" function for all devices, and all of
- *    them have reported "nothing processed" (returned 0), then we will go to
- *    sleep for a maximum of POLLJIFFIES_NORMAL jiffies.
- *  - If anyone calls uislib_force_channel_interrupt(), the above jiffy
- *    sleep will be interrupted, and we will resume calling the "interrupt"
- *    function for all devices.
- *  - The list of devices is dynamically re-ordered in order to
- *    attempt to preserve fairness.  Whenever we spin thru the list of
- *    devices and call the dev->interrupt() function, if we find
- *    devices which report that there is still more work to do, the
- *    the first such device we find is moved to the end of the device
- *    list.  This ensures that extremely busy devices don't starve out
- *    less-busy ones.
- *
- */
-static int process_incoming(void *v)
-{
-	unsigned long long cur_cycles, old_cycles, idle_cycles, delta_cycles;
-	struct list_head *new_tail = NULL;
-	int i;
-
-	UIS_DAEMONIZE("dev_incoming");
-	for (i = 0; i < 16; i++) {
-		old_cycles = get_cycles();
-		wait_event_timeout(poll_dev_wake_q,
-				   0, POLLJIFFIES_NORMAL);
-		cur_cycles = get_cycles();
-		if (wait_cycles == 0) {
-			wait_cycles = (cur_cycles - old_cycles);
-		} else {
-			if (wait_cycles < (cur_cycles - old_cycles))
-				wait_cycles = (cur_cycles - old_cycles);
-		}
-	}
-	cycles_before_wait = wait_cycles;
-	idle_cycles = 0;
-	poll_dev_start = 0;
-	while (1) {
-		struct list_head *lelt, *tmp;
-		struct device_info *dev = NULL;
-
-		/* poll each channel for input */
-		down(&poll_dev_lock);
-		new_tail = NULL;
-		list_for_each_safe(lelt, tmp, &poll_dev_chan) {
-			int rc = 0;
-
-			dev = list_entry(lelt, struct device_info,
-					 list_polling_device_channels);
-			down(&dev->interrupt_callback_lock);
-			if (dev->interrupt)
-				rc = dev->interrupt(dev->interrupt_context);
-			else
-				continue;
-			up(&dev->interrupt_callback_lock);
-			if (rc) {
-				/* dev->interrupt returned, but there
-				* is still more work to do.
-				* Reschedule work to occur as soon as
-				* possible. */
-				idle_cycles = 0;
-				if (!new_tail) {
-					dev->first_busy_cnt++;
-					if (!
-					    (list_is_last
-					     (lelt,
-					      &poll_dev_chan))) {
-						new_tail = lelt;
-						dev->moved_to_tail_cnt++;
-					} else {
-						dev->last_on_list_cnt++;
-					}
-				}
-			}
-			if (kthread_should_stop())
-				break;
-		}
-		if (new_tail) {
-			tot_moved_to_tail_cnt++;
-			list_move_tail(new_tail, &poll_dev_chan);
-		}
-		up(&poll_dev_lock);
-		cur_cycles = get_cycles();
-		delta_cycles = cur_cycles - old_cycles;
-		old_cycles = cur_cycles;
-
-		/* At this point, we have scanned thru all of the
-		* channels, and at least one of the following is true:
-		* - there is no input waiting on any of the channels
-		* - we have received a signal to stop this thread
-		*/
-		if (kthread_should_stop())
-			break;
-		if (en_smart_wakeup == 0xFF)
-			break;
-		/* wait for POLLJIFFIES_NORMAL jiffies, or until
-		* someone wakes up poll_dev_wake_q,
-		* whichever comes first only do a wait when we have
-		* been idle for cycles_before_wait cycles.
-		*/
-		if (idle_cycles > cycles_before_wait) {
-			poll_dev_start = 0;
-			tot_wait_cnt++;
-			wait_event_timeout(poll_dev_wake_q,
-					   poll_dev_start,
-					   POLLJIFFIES_NORMAL);
-			poll_dev_start = 1;
-		} else {
-			tot_schedule_cnt++;
-			schedule();
-			idle_cycles = idle_cycles + delta_cycles;
-		}
-	}
-	complete_and_exit(&incoming_ti.has_stopped, 0);
-}
-
-static BOOL
-initialize_incoming_thread(void)
-{
-	if (incoming_started)
-		return TRUE;
-	if (!uisthread_start(&incoming_ti,
-			     &process_incoming, NULL, "dev_incoming")) {
-		return FALSE;
-	}
-	incoming_started = TRUE;
-	return TRUE;
-}
-
-/*  Add a new device/channel to the list being processed by
- *  process_incoming().
- *  <interrupt> - indicates the function to call periodically.
- *  <interrupt_context> - indicates the data to pass to the <interrupt>
- *                        function.
- */
-void
-uislib_enable_channel_interrupts(u32 bus_no, u32 dev_no,
-				 int (*interrupt)(void *),
-				 void *interrupt_context)
-{
-	struct device_info *dev;
-
-	dev = find_dev(bus_no, dev_no);
-	if (!dev)
-		return;
-
-	down(&poll_dev_lock);
-	initialize_incoming_thread();
-	dev->interrupt = interrupt;
-	dev->interrupt_context = interrupt_context;
-	dev->polling = TRUE;
-	list_add_tail(&dev->list_polling_device_channels,
-		      &poll_dev_chan);
-	up(&poll_dev_lock);
-}
-EXPORT_SYMBOL_GPL(uislib_enable_channel_interrupts);
-
-/*  Remove a device/channel from the list being processed by
- *  process_incoming().
- */
-void
-uislib_disable_channel_interrupts(u32 bus_no, u32 dev_no)
-{
-	struct device_info *dev;
-
-	dev = find_dev(bus_no, dev_no);
-	if (!dev)
-		return;
-	down(&poll_dev_lock);
-	list_del(&dev->list_polling_device_channels);
-	dev->polling = FALSE;
-	dev->interrupt = NULL;
-	up(&poll_dev_lock);
-}
-EXPORT_SYMBOL_GPL(uislib_disable_channel_interrupts);
-
-static void
-do_wakeup_polling_device_channels(struct work_struct *dummy)
-{
-	if (!poll_dev_start) {
-		poll_dev_start = 1;
-		wake_up(&poll_dev_wake_q);
-	}
-}
-
-static DECLARE_WORK(work_wakeup_polling_device_channels,
-		    do_wakeup_polling_device_channels);
-
-/*  Call this function when you want to send a hint to process_incoming() that
- *  your device might have more requests.
- */
-void
-uislib_force_channel_interrupt(u32 bus_no, u32 dev_no)
-{
-	if (en_smart_wakeup == 0)
-		return;
-	if (poll_dev_start)
-		return;
-	/* The point of using schedule_work() instead of just doing
-	 * the work inline is to force a slight delay before waking up
-	 * the process_incoming() thread.
-	 */
-	tot_wakeup_cnt++;
-	schedule_work(&work_wakeup_polling_device_channels);
-}
-EXPORT_SYMBOL_GPL(uislib_force_channel_interrupt);
-
-/*****************************************************/
-/* Module Init & Exit functions                      */
-/*****************************************************/
-
-static int __init
-uislib_mod_init(void)
-{
-	if (!unisys_spar_platform)
-		return -ENODEV;
-
-	/* initialize global pointers to NULL */
-	bus_list = NULL;
-	bus_list_count = 0;
-	max_bus_count = 0;
-	rwlock_init(&bus_list_lock);
-	virt_control_chan_func = NULL;
-
-	/* Issue VMCALL_GET_CONTROLVM_ADDR to get CtrlChanPhysAddr and
-	 * then map this physical address to a virtual address. */
-	POSTCODE_LINUX_2(DRIVER_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-
-	dir_debugfs = debugfs_create_dir(DIR_DEBUGFS_ENTRY, NULL);
-	if (dir_debugfs) {
-		info_debugfs_entry = debugfs_create_file(
-			INFO_DEBUGFS_ENTRY_FN, 0444, dir_debugfs, NULL,
-			&debugfs_info_fops);
-
-		platformnumber_debugfs_read = debugfs_create_u32(
-			PLATFORMNUMBER_DEBUGFS_ENTRY_FN, 0444, dir_debugfs,
-			&platform_no);
-
-		cycles_before_wait_debugfs_read = debugfs_create_u64(
-			CYCLES_BEFORE_WAIT_DEBUGFS_ENTRY_FN, 0666, dir_debugfs,
-			&cycles_before_wait);
-
-		smart_wakeup_debugfs_entry = debugfs_create_bool(
-			SMART_WAKEUP_DEBUGFS_ENTRY_FN, 0666, dir_debugfs,
-			&en_smart_wakeup);
-	}
-
-	POSTCODE_LINUX_3(DRIVER_EXIT_PC, 0, POSTCODE_SEVERITY_INFO);
-	return 0;
-}
-
-static void __exit
-uislib_mod_exit(void)
-{
-	if (debug_buf) {
-		vfree(debug_buf);
-		debug_buf = NULL;
-	}
-
-	debugfs_remove(info_debugfs_entry);
-	debugfs_remove(smart_wakeup_debugfs_entry);
-	debugfs_remove(cycles_before_wait_debugfs_read);
-	debugfs_remove(platformnumber_debugfs_read);
-	debugfs_remove(dir_debugfs);
-}
-
-module_init(uislib_mod_init);
-module_exit(uislib_mod_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Usha Srinivasan");
-MODULE_ALIAS("uislib");
-  /* this is extracted during depmod and kept in modules.dep */
diff --git a/drivers/staging/unisys/uislib/uisqueue.c b/drivers/staging/unisys/uislib/uisqueue.c
deleted file mode 100644
index d46dd7428a30..000000000000
--- a/drivers/staging/unisys/uislib/uisqueue.c
+++ /dev/null
@@ -1,322 +0,0 @@
-/* uisqueue.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* @ALL_INSPECTED */
-#include <linux/kernel.h>
-#include <linux/module.h>
-
-#include "uisutils.h"
-
-/* this is shorter than using __FILE__ (full path name) in
- * debug/info/error messages */
-#define CURRENT_FILE_PC UISLIB_PC_uisqueue_c
-#define __MYFILE__ "uisqueue.c"
-
-#define CHECK_CACHE_ALIGN 0
-
-/*****************************************************/
-/* Exported functions                                */
-/*****************************************************/
-
-/*
- * Routine Description:
- * Tries to insert the prebuilt signal pointed to by pSignal into the nth
- * Queue of the Channel pointed to by pChannel
- *
- * Parameters:
- * pChannel: (IN) points to the IO Channel
- * Queue: (IN) nth Queue of the IO Channel
- * pSignal: (IN) pointer to the signal
- *
- * Assumptions:
- * - pChannel, Queue and pSignal are valid.
- * - If insertion fails due to a full queue, the caller will determine the
- * retry policy (e.g. wait & try again, report an error, etc.).
- *
- * Return value:
- * 1 if the insertion succeeds, 0 if the queue was full.
- */
-unsigned char spar_signal_insert(struct channel_header __iomem *ch, u32 queue,
-				 void *sig)
-{
-	void __iomem *psignal;
-	unsigned int head, tail, nof;
-
-	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);
-
-	/* queue is full if (head + 1) % n equals tail */
-	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->max_slots);
-
-	/* copy signal to the head location from the area pointed to
-	 * by pSignal
-	 */
-	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);
-
-	writeq(readq(&pqhdr->num_sent) + 1, &pqhdr->num_sent);
-	return 1;
-}
-EXPORT_SYMBOL_GPL(spar_signal_insert);
-
-/*
- * Routine Description:
- * Removes one signal from Channel pChannel's nth Queue at the
- * time of the call and copies it into the memory pointed to by
- * pSignal.
- *
- * Parameters:
- * pChannel: (IN) points to the IO Channel
- * Queue: (IN) nth Queue of the IO Channel
- * pSignal: (IN) pointer to where the signals are to be copied
- *
- * Assumptions:
- * - pChannel and Queue are valid.
- * - pSignal points to a memory area large enough to hold queue's SignalSize
- *
- * Return value:
- * 1 if the removal succeeds, 0 if the queue was empty.
- */
-unsigned char
-spar_signal_remove(struct channel_header __iomem *ch, u32 queue, void *sig)
-{
-	void __iomem *psource;
-	unsigned int head, tail;
-	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);
-
-	/* queue is empty if the head index equals the tail index */
-	if (head == tail) {
-		writeq(readq(&pqhdr->num_empty) + 1, &pqhdr->num_empty);
-		return 0;
-	}
-
-	/* advance past the 'empty' front slot */
-	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->sig_base_offset) +
-		(tail * readl(&pqhdr->signal_size));
-	memcpy_fromio(sig, psource, readl(&pqhdr->signal_size));
-
-	mb(); /* channel synch */
-	writel(tail, &pqhdr->tail);
-
-	writeq(readq(&pqhdr->num_received) + 1,
-	       &pqhdr->num_received);
-	return 1;
-}
-EXPORT_SYMBOL_GPL(spar_signal_remove);
-
-/*
- * Routine Description:
- * Removes all signals present in Channel pChannel's nth Queue at the
- * time of the call and copies them into the memory pointed to by
- * pSignal.  Returns the # of signals copied as the value of the routine.
- *
- * Parameters:
- * pChannel: (IN) points to the IO Channel
- * Queue: (IN) nth Queue of the IO Channel
- * pSignal: (IN) pointer to where the signals are to be copied
- *
- * Assumptions:
- * - pChannel and Queue are valid.
- * - pSignal points to a memory area large enough to hold Queue's MaxSignals
- * # of signals, each of which is Queue's SignalSize.
- *
- * Return value:
- * # of signals copied.
- */
-unsigned int spar_signal_remove_all(struct channel_header *ch, u32 queue,
-				    void *sig)
-{
-	void *psource;
-	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;
-
-	/* queue is empty if the head index equals the tail index */
-	if (head == tail)
-		return 0;
-
-	while (head != tail) {
-		/* advance past the 'empty' front slot */
-		tail = (tail + 1) % pqhdr->max_slots;
-
-		/* copy signal from tail location to the area pointed
-		 * to by pSignal
-		 */
-		psource =
-		    (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;
-
-		count++;
-		pqhdr->num_received++;
-	}
-
-	return count;
-}
-
-/*
- * Routine Description:
- * Determine whether a signal queue is empty.
- *
- * Parameters:
- * pChannel: (IN) points to the IO Channel
- * Queue: (IN) nth Queue of the IO Channel
- *
- * Return value:
- * 1 if the signal queue is empty, 0 otherwise.
- */
-unsigned char spar_signalqueue_empty(struct channel_header __iomem *ch,
-				     u32 queue)
-{
-	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(spar_signalqueue_empty);
-
-unsigned long long
-uisqueue_interlocked_or(unsigned long long __iomem *tgt,
-			unsigned long long set)
-{
-	unsigned long long i;
-	unsigned long long j;
-
-	j = readq(tgt);
-	do {
-		i = j;
-		j = cmpxchg((__force unsigned long long *)tgt, i, i | set);
-
-	} while (i != j);
-
-	return j;
-}
-EXPORT_SYMBOL_GPL(uisqueue_interlocked_or);
-
-unsigned long long
-uisqueue_interlocked_and(unsigned long long __iomem *tgt,
-			 unsigned long long set)
-{
-	unsigned long long i;
-	unsigned long long j;
-
-	j = readq(tgt);
-	do {
-		i = j;
-		j = cmpxchg((__force unsigned long long *)tgt, i, i & set);
-
-	} while (i != j);
-
-	return j;
-}
-EXPORT_SYMBOL_GPL(uisqueue_interlocked_and);
-
-static u8
-do_locked_client_insert(struct uisqueue_info *queueinfo,
-			unsigned int whichqueue,
-			void *signal,
-			spinlock_t *lock,
-			u8 *channel_id)
-{
-	unsigned long flags;
-	u8 rc = 0;
-
-	spin_lock_irqsave(lock, flags);
-	if (!spar_channel_client_acquire_os(queueinfo->chan, channel_id))
-		goto unlock;
-	if (spar_signal_insert(queueinfo->chan, whichqueue, signal)) {
-		queueinfo->packets_sent++;
-		rc = 1;
-	}
-	spar_channel_client_release_os(queueinfo->chan, channel_id);
-unlock:
-	spin_unlock_irqrestore((spinlock_t *)lock, flags);
-	return rc;
-}
-
-int
-uisqueue_put_cmdrsp_with_lock_client(struct uisqueue_info *queueinfo,
-				     struct uiscmdrsp *cmdrsp,
-				     unsigned int whichqueue,
-				     void *insertlock,
-				     unsigned char issue_irq_if_empty,
-				     u64 irq_handle,
-				     char oktowait, u8 *channel_id)
-{
-	while (!do_locked_client_insert(queueinfo, whichqueue, cmdrsp,
-					(spinlock_t *)insertlock,
-					channel_id)) {
-		if (oktowait != OK_TO_WAIT)
-			return 0;	/* failed to queue */
-
-		/* try again */
-		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(msecs_to_jiffies(10));
-	}
-	return 1;
-}
-EXPORT_SYMBOL_GPL(uisqueue_put_cmdrsp_with_lock_client);
-
-/* uisqueue_get_cmdrsp gets the cmdrsp entry at the head of the queue
- * returns NULL if queue is empty */
-int
-uisqueue_get_cmdrsp(struct uisqueue_info *queueinfo,
-		    void *cmdrsp, unsigned int whichqueue)
-{
-	if (!spar_signal_remove(queueinfo->chan, whichqueue, cmdrsp))
-		return 0;
-
-	queueinfo->packets_received++;
-
-	return 1;		/* Success */
-}
-EXPORT_SYMBOL_GPL(uisqueue_get_cmdrsp);
diff --git a/drivers/staging/unisys/uislib/uisthread.c b/drivers/staging/unisys/uislib/uisthread.c
deleted file mode 100644
index d3c973b617ee..000000000000
--- a/drivers/staging/unisys/uislib/uisthread.c
+++ /dev/null
@@ -1,69 +0,0 @@
-/* uisthread.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* @ALL_INSPECTED */
-#include <asm/processor.h>
-#include <linux/signal.h>
-#include <linux/sched.h>
-#include <linux/kthread.h>
-#include "uisutils.h"
-#include "uisthread.h"
-
-/* this is shorter than using __FILE__ (full path name) in
- * debug/info/error messages
- */
-#define CURRENT_FILE_PC UISLIB_PC_uisthread_c
-#define __MYFILE__ "uisthread.c"
-
-/*****************************************************/
-/* Exported functions                                */
-/*****************************************************/
-
-/* returns 0 for failure, 1 for success */
-int
-uisthread_start(struct uisthread_info *thrinfo,
-		int (*threadfn)(void *), void *thrcontext, char *name)
-{
-	/* used to stop the thread */
-	init_completion(&thrinfo->has_stopped);
-	thrinfo->task = kthread_run(threadfn, thrcontext, name);
-	if (IS_ERR(thrinfo->task)) {
-		thrinfo->id = 0;
-		return 0;	/* failure */
-	}
-	thrinfo->id = thrinfo->task->pid;
-	return 1;
-}
-EXPORT_SYMBOL_GPL(uisthread_start);
-
-void
-uisthread_stop(struct uisthread_info *thrinfo)
-{
-	int stopped = 0;
-
-	if (thrinfo->id == 0)
-		return;		/* thread not running */
-
-	kthread_stop(thrinfo->task);
-	/* give up if the thread has NOT died in 1 minute */
-	if (wait_for_completion_timeout(&thrinfo->has_stopped, 60 * HZ))
-		stopped = 1;
-
-	if (stopped)
-		thrinfo->id = 0;
-}
-EXPORT_SYMBOL_GPL(uisthread_stop);
diff --git a/drivers/staging/unisys/uislib/uisutils.c b/drivers/staging/unisys/uislib/uisutils.c
deleted file mode 100644
index 26ab76526813..000000000000
--- a/drivers/staging/unisys/uislib/uisutils.c
+++ /dev/null
@@ -1,137 +0,0 @@
-/* uisutils.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#include <linux/string.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <linux/uuid.h>
-#include <linux/spinlock.h>
-#include <linux/list.h>
-#include "uisutils.h"
-#include "version.h"
-#include "vbushelper.h"
-#include <linux/skbuff.h>
-#ifdef CONFIG_HIGHMEM
-#include <linux/highmem.h>
-#endif
-
-/* this is shorter than using __FILE__ (full path name) in
- * debug/info/error messages
- */
-#define CURRENT_FILE_PC UISLIB_PC_uisutils_c
-#define __MYFILE__ "uisutils.c"
-
-/* exports */
-atomic_t uisutils_registered_services = ATOMIC_INIT(0);
-					/* num registrations via
-					 * uisctrl_register_req_handler() or
-					 * uisctrl_register_req_handler_ex() */
-
-/*****************************************************/
-/* Utility functions                                 */
-/*****************************************************/
-
-int
-uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
-			 char *format, ...)
-{
-	va_list args;
-	int len;
-
-	va_start(args, format);
-	len = vsnprintf(*buffer, *buffer_remaining, format, args);
-	va_end(args);
-	if (len >= *buffer_remaining) {
-		*buffer += *buffer_remaining;
-		*total += *buffer_remaining;
-		*buffer_remaining = 0;
-		return -1;
-	}
-	*buffer_remaining -= len;
-	*buffer += len;
-	*total += len;
-	return len;
-}
-EXPORT_SYMBOL_GPL(uisutil_add_proc_line_ex);
-
-int
-uisctrl_register_req_handler(int type, void *fptr,
-			     struct ultra_vbus_deviceinfo *chipset_driver_info)
-{
-	switch (type) {
-	case 2:
-		if (fptr) {
-			if (!virt_control_chan_func)
-				atomic_inc(&uisutils_registered_services);
-			virt_control_chan_func = fptr;
-		} else {
-			if (virt_control_chan_func)
-				atomic_dec(&uisutils_registered_services);
-			virt_control_chan_func = NULL;
-		}
-		break;
-
-	default:
-		return 0;
-	}
-	if (chipset_driver_info)
-		bus_device_info_init(chipset_driver_info, "chipset", "uislib",
-				     VERSION, NULL);
-
-	return 1;
-}
-EXPORT_SYMBOL_GPL(uisctrl_register_req_handler);
-
-/*
- * unsigned int uisutil_copy_fragsinfo_from_skb(unsigned char *calling_ctx,
- *					     void *skb_in,
- *					     unsigned int firstfraglen,
- *					     unsigned int frags_max,
- *					     struct phys_info frags[])
- *
- *	calling_ctx - input -   a string that is displayed to show
- *				who called * this func
- *	void *skb_in -  skb whose frag info we're copying type is hidden so we
- *			don't need to include skbbuff in uisutils.h which is
- *			included in non-networking code.
- *	unsigned int firstfraglen - input - length of first fragment in skb
- *	unsigned int frags_max - input - max len of frags array
- *	struct phys_info frags[] - output - frags array filled in on output
- *					    return value indicates number of
- *					    entries filled in frags
- */
-
-static LIST_HEAD(req_handler_info_list); /* list of struct req_handler_info */
-static DEFINE_SPINLOCK(req_handler_info_list_lock);
-
-struct req_handler_info *
-req_handler_find(uuid_le switch_uuid)
-{
-	struct list_head *lelt, *tmp;
-	struct req_handler_info *entry = NULL;
-
-	spin_lock(&req_handler_info_list_lock);
-	list_for_each_safe(lelt, tmp, &req_handler_info_list) {
-		entry = list_entry(lelt, struct req_handler_info, list_link);
-		if (uuid_le_cmp(entry->switch_uuid, switch_uuid) == 0) {
-			spin_unlock(&req_handler_info_list_lock);
-			return entry;
-		}
-	}
-	spin_unlock(&req_handler_info_list_lock);
-	return NULL;
-}
diff --git a/drivers/staging/unisys/virthba/Kconfig b/drivers/staging/unisys/virthba/Kconfig
deleted file mode 100644
index dfadfc49114a..000000000000
--- a/drivers/staging/unisys/virthba/Kconfig
+++ /dev/null
@@ -1,13 +0,0 @@
-#
-# Unisys virthba configuration
-#
-
-config UNISYS_VIRTHBA
-	tristate "Unisys virthba driver"
-	depends on SCSI
-	select UNISYS_VISORCHIPSET
-	select UNISYS_UISLIB
-	select UNISYS_VIRTPCI
-	---help---
-	If you say Y here, you will enable the Unisys virthba driver.
-
diff --git a/drivers/staging/unisys/virthba/Makefile b/drivers/staging/unisys/virthba/Makefile
deleted file mode 100644
index a4e403739183..000000000000
--- a/drivers/staging/unisys/virthba/Makefile
+++ /dev/null
@@ -1,12 +0,0 @@
-#
-# Makefile for Unisys virthba
-#
-
-obj-$(CONFIG_UNISYS_VIRTHBA)	+= virthba.o
-
-ccflags-y += -Idrivers/staging/unisys/include
-ccflags-y += -Idrivers/staging/unisys/uislib
-ccflags-y += -Idrivers/staging/unisys/visorchipset
-ccflags-y += -Idrivers/staging/unisys/virtpci
-ccflags-y += -Idrivers/staging/unisys/common-spar/include
-ccflags-y += -Idrivers/staging/unisys/common-spar/include/channels
diff --git a/drivers/staging/unisys/virthba/virthba.c b/drivers/staging/unisys/virthba/virthba.c
deleted file mode 100644
index d9001cca0f73..000000000000
--- a/drivers/staging/unisys/virthba/virthba.c
+++ /dev/null
@@ -1,1572 +0,0 @@
-/* virthba.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#define EXPORT_SYMTAB
-
-/* if you want to turn on some debugging of write device data or read
- * device data, define these two undefs.  You will probably want to
- * customize the code which is here since it was written assuming
- * reading and writing a specific data file df.64M.txt which is a
- * 64Megabyte file created by Art Nilson using a scritp I wrote called
- * cr_test_data.pl.  The data file consists of 256 byte lines of text
- * which start with an 8 digit sequence number, a colon, and then
- * letters after that */
-
-#include <linux/kernel.h>
-#ifdef CONFIG_MODVERSIONS
-#include <config/modversions.h>
-#endif
-
-#include "diagnostics/appos_subsystems.h"
-#include "uisutils.h"
-#include "uisqueue.h"
-#include "uisthread.h"
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/pci.h>
-#include <linux/spinlock.h>
-#include <linux/device.h>
-#include <linux/slab.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
-#include <scsi/scsi_cmnd.h>
-#include <scsi/scsi_device.h>
-#include <asm/param.h>
-#include <linux/debugfs.h>
-#include <linux/types.h>
-
-#include "virthba.h"
-#include "virtpci.h"
-#include "visorchipset.h"
-#include "version.h"
-#include "guestlinuxdebug.h"
-/* this is shorter than using __FILE__ (full path name) in
- * debug/info/error messages
- */
-#define CURRENT_FILE_PC VIRT_HBA_PC_virthba_c
-#define __MYFILE__ "virthba.c"
-
-/* NOTE:  L1_CACHE_BYTES >=128 */
-#define DEVICE_ATTRIBUTE struct device_attribute
-
- /* MAX_BUF = 6 lines x 10 MAXVHBA x 80 characters
- *         = 4800 bytes ~ 2^13 = 8192 bytes
- */
-#define MAX_BUF 8192
-
-/*****************************************************/
-/* Forward declarations                              */
-/*****************************************************/
-static int virthba_probe(struct virtpci_dev *dev,
-			 const struct pci_device_id *id);
-static void virthba_remove(struct virtpci_dev *dev);
-static int virthba_abort_handler(struct scsi_cmnd *scsicmd);
-static int virthba_bus_reset_handler(struct scsi_cmnd *scsicmd);
-static int virthba_device_reset_handler(struct scsi_cmnd *scsicmd);
-static int virthba_host_reset_handler(struct scsi_cmnd *scsicmd);
-static const char *virthba_get_info(struct Scsi_Host *shp);
-static int virthba_ioctl(struct scsi_device *dev, int cmd, void __user *arg);
-static int virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
-				     void (*virthba_cmnd_done)
-					   (struct scsi_cmnd *));
-
-static const struct x86_cpu_id unisys_spar_ids[] = {
-	{ X86_VENDOR_INTEL, 6, 62, X86_FEATURE_ANY },
-	{}
-};
-
-/* Autoload */
-MODULE_DEVICE_TABLE(x86cpu, unisys_spar_ids);
-
-#ifdef DEF_SCSI_QCMD
-static DEF_SCSI_QCMD(virthba_queue_command)
-#else
-#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);
-static int process_incoming_rsps(void *);
-static int virthba_serverup(struct virtpci_dev *virtpcidev);
-static int virthba_serverdown(struct virtpci_dev *virtpcidev, u32 state);
-static void do_disk_add_remove(struct work_struct *work);
-static void virthba_serverdown_complete(struct work_struct *work);
-static ssize_t info_debugfs_read(struct file *file, char __user *buf,
-				 size_t len, loff_t *offset);
-static ssize_t enable_ints_write(struct file *file,
-				 const char __user *buffer, size_t count,
-				 loff_t *ppos);
-
-/*****************************************************/
-/* Globals                                           */
-/*****************************************************/
-
-static int rsltq_wait_usecs = 4000;	/* Default 4ms */
-static unsigned int max_buff_len;
-
-/* Module options */
-static char *virthba_options = "NONE";
-
-static const struct pci_device_id virthba_id_table[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_UNISYS, PCI_DEVICE_ID_VIRTHBA)},
-	{0},
-};
-
-/* export virthba_id_table */
-MODULE_DEVICE_TABLE(pci, virthba_id_table);
-
-static struct workqueue_struct *virthba_serverdown_workqueue;
-
-static struct virtpci_driver virthba_driver = {
-	.name = "uisvirthba",
-	.version = VERSION,
-	.vertag = NULL,
-	.id_table = virthba_id_table,
-	.probe = virthba_probe,
-	.remove = virthba_remove,
-	.resume = virthba_serverup,
-	.suspend = virthba_serverdown
-};
-
-/* The Send and Recive Buffers of the IO Queue may both be full */
-#define MAX_PENDING_REQUESTS (MIN_NUMSIGNALS*2)
-#define INTERRUPT_VECTOR_MASK 0x3F
-
-struct scsipending {
-	char cmdtype;		/* Type of pointer that is being stored */
-	void *sent;		/* The Data being tracked */
-	/* struct scsi_cmnd *type for virthba_queue_command */
-	/* struct uiscmdrsp *type for management commands */
-};
-
-#define VIRTHBA_ERROR_COUNT 30
-#define IOS_ERROR_THRESHOLD 1000
-struct virtdisk_info {
-	u32 valid;
-	u32 channel, id, lun;	/* Disk Path */
-	atomic_t ios_threshold;
-	atomic_t error_count;
-	struct virtdisk_info *next;
-};
-
-/* Each Scsi_Host has a host_data area that contains this struct. */
-struct virthba_info {
-	struct Scsi_Host *scsihost;
-	struct virtpci_dev *virtpcidev;
-	struct list_head dev_info_list;
-	struct chaninfo chinfo;
-	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
-							     that have been */
-	/* forwarded to the IOVM and haven't returned yet */
-	unsigned int nextinsert;	/* Start search for next pending
-					   free slot here */
-	spinlock_t privlock;
-	bool serverdown;
-	bool serverchangingstate;
-	unsigned long long acquire_failed_cnt;
-	unsigned long long interrupts_rcvd;
-	unsigned long long interrupts_notme;
-	unsigned long long interrupts_disabled;
-	struct work_struct serverdown_completion;
-	u64 __iomem *flags_addr;
-	atomic_t interrupt_rcvd;
-	wait_queue_head_t rsp_queue;
-	struct virtdisk_info head;
-};
-
-/* Work Data for dar_work_queue */
-struct diskaddremove {
-	u8 add;			/* 0-remove, 1-add */
-	struct Scsi_Host *shost; /* Scsi Host for this virthba instance */
-	u32 channel, id, lun;	/* Disk Path */
-	struct diskaddremove *next;
-};
-
-#define virtpci_dev_to_virthba_virthba_get_info(d) \
-	container_of(d, struct virthba_info, virtpcidev)
-
-static DEVICE_ATTRIBUTE *virthba_shost_attrs[];
-static struct scsi_host_template virthba_driver_template = {
-	.name = "Unisys Virtual HBA",
-	.info = virthba_get_info,
-	.ioctl = virthba_ioctl,
-	.queuecommand = virthba_queue_command,
-	.eh_abort_handler = virthba_abort_handler,
-	.eh_device_reset_handler = virthba_device_reset_handler,
-	.eh_bus_reset_handler = virthba_bus_reset_handler,
-	.eh_host_reset_handler = virthba_host_reset_handler,
-	.shost_attrs = virthba_shost_attrs,
-
-#define VIRTHBA_MAX_CMNDS 128
-	.can_queue = VIRTHBA_MAX_CMNDS,
-	.sg_tablesize = 64,	/* largest number of address/length pairs */
-	.this_id = -1,
-	.slave_alloc = virthba_slave_alloc,
-	.slave_configure = virthba_slave_configure,
-	.slave_destroy = virthba_slave_destroy,
-	.use_clustering = ENABLE_CLUSTERING,
-};
-
-struct virthba_devices_open {
-	struct virthba_info *virthbainfo;
-};
-
-static const struct file_operations debugfs_info_fops = {
-	.read = info_debugfs_read,
-};
-
-static const struct file_operations debugfs_enable_ints_fops = {
-	.write = enable_ints_write,
-};
-
-/*****************************************************/
-/* Structs                                           */
-/*****************************************************/
-
-#define VIRTHBASOPENMAX 1
-/* array of open devices maintained by open() and close(); */
-static struct virthba_devices_open virthbas_open[VIRTHBASOPENMAX];
-static struct dentry *virthba_debugfs_dir;
-
-/*****************************************************/
-/* Local Functions				     */
-/*****************************************************/
-static int
-add_scsipending_entry(struct virthba_info *vhbainfo, char cmdtype, void *new)
-{
-	unsigned long flags;
-	int insert_location;
-
-	spin_lock_irqsave(&vhbainfo->privlock, flags);
-	insert_location = vhbainfo->nextinsert;
-	while (vhbainfo->pending[insert_location].sent) {
-		insert_location = (insert_location + 1) % MAX_PENDING_REQUESTS;
-		if (insert_location == (int)vhbainfo->nextinsert) {
-			spin_unlock_irqrestore(&vhbainfo->privlock, flags);
-			return -1;
-		}
-	}
-
-	vhbainfo->pending[insert_location].cmdtype = cmdtype;
-	vhbainfo->pending[insert_location].sent = new;
-	vhbainfo->nextinsert = (insert_location + 1) % MAX_PENDING_REQUESTS;
-	spin_unlock_irqrestore(&vhbainfo->privlock, flags);
-
-	return insert_location;
-}
-
-static unsigned int
-add_scsipending_entry_with_wait(struct virthba_info *vhbainfo, char cmdtype,
-				void *new)
-{
-	int insert_location = add_scsipending_entry(vhbainfo, cmdtype, new);
-
-	while (insert_location == -1) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(msecs_to_jiffies(10));
-		insert_location = add_scsipending_entry(vhbainfo, cmdtype, new);
-	}
-
-	return (unsigned int)insert_location;
-}
-
-static void *
-del_scsipending_entry(struct virthba_info *vhbainfo, uintptr_t del)
-{
-	unsigned long flags;
-	void *sent = NULL;
-
-	if (del < MAX_PENDING_REQUESTS) {
-		spin_lock_irqsave(&vhbainfo->privlock, flags);
-		sent = vhbainfo->pending[del].sent;
-
-		vhbainfo->pending[del].cmdtype = 0;
-		vhbainfo->pending[del].sent = NULL;
-		spin_unlock_irqrestore(&vhbainfo->privlock, flags);
-	}
-
-	return sent;
-}
-
-/* dar_work_queue (Disk Add/Remove) */
-static struct work_struct dar_work_queue;
-static struct diskaddremove *dar_work_queue_head;
-static spinlock_t dar_work_queue_lock;
-static unsigned short dar_work_queue_sched;
-#define QUEUE_DISKADDREMOVE(dar) { \
-	spin_lock_irqsave(&dar_work_queue_lock, flags); \
-	if (!dar_work_queue_head) { \
-		dar_work_queue_head = dar; \
-		dar->next = NULL; \
-	} \
-	else { \
-		dar->next = dar_work_queue_head; \
-		dar_work_queue_head = dar; \
-	} \
-	if (!dar_work_queue_sched) { \
-		schedule_work(&dar_work_queue); \
-		dar_work_queue_sched = 1; \
-	} \
-	spin_unlock_irqrestore(&dar_work_queue_lock, flags); \
-}
-
-static inline void
-send_disk_add_remove(struct diskaddremove *dar)
-{
-	struct scsi_device *sdev;
-	int error;
-
-	sdev = scsi_device_lookup(dar->shost, dar->channel, dar->id, dar->lun);
-	if (sdev) {
-		if (!(dar->add))
-			scsi_remove_device(sdev);
-	} else if (dar->add) {
-		error =
-		    scsi_add_device(dar->shost, dar->channel, dar->id,
-				    dar->lun);
-	}
-	kfree(dar);
-}
-
-/*****************************************************/
-/* dar_work_queue Handler Thread                     */
-/*****************************************************/
-static void
-do_disk_add_remove(struct work_struct *work)
-{
-	struct diskaddremove *dar;
-	struct diskaddremove *tmphead;
-	int i = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dar_work_queue_lock, flags);
-	tmphead = dar_work_queue_head;
-	dar_work_queue_head = NULL;
-	dar_work_queue_sched = 0;
-	spin_unlock_irqrestore(&dar_work_queue_lock, flags);
-	while (tmphead) {
-		dar = tmphead;
-		tmphead = dar->next;
-		send_disk_add_remove(dar);
-		i++;
-	}
-}
-
-/*****************************************************/
-/* Routine to add entry to dar_work_queue            */
-/*****************************************************/
-static void
-process_disk_notify(struct Scsi_Host *shost, struct uiscmdrsp *cmdrsp)
-{
-	struct diskaddremove *dar;
-	unsigned long flags;
-
-	dar = kzalloc(sizeof(*dar), GFP_ATOMIC);
-	if (dar) {
-		dar->add = cmdrsp->disknotify.add;
-		dar->shost = shost;
-		dar->channel = cmdrsp->disknotify.channel;
-		dar->id = cmdrsp->disknotify.id;
-		dar->lun = cmdrsp->disknotify.lun;
-		QUEUE_DISKADDREMOVE(dar);
-	}
-}
-
-/*****************************************************/
-/* Probe Remove Functions                            */
-/*****************************************************/
-static irqreturn_t
-virthba_isr(int irq, void *dev_id)
-{
-	struct virthba_info *virthbainfo = (struct virthba_info *)dev_id;
-	struct channel_header __iomem *channel_header;
-	struct signal_queue_header __iomem *pqhdr;
-	u64 mask;
-	unsigned long long rc1;
-
-	if (!virthbainfo)
-		return IRQ_NONE;
-	virthbainfo->interrupts_rcvd++;
-	channel_header = virthbainfo->chinfo.queueinfo->chan;
-	if (((readq(&channel_header->features)
-	      & ULTRA_IO_IOVM_IS_OK_WITH_DRIVER_DISABLING_INTS) != 0) &&
-	     ((readq(&channel_header->features) &
-		 ULTRA_IO_DRIVER_DISABLES_INTS) !=
-		0)) {
-		virthbainfo->interrupts_disabled++;
-		mask = ~ULTRA_CHANNEL_ENABLE_INTS;
-		rc1 = uisqueue_interlocked_and(virthbainfo->flags_addr, mask);
-	}
-	if (spar_signalqueue_empty(channel_header, IOCHAN_FROM_IOPART)) {
-		virthbainfo->interrupts_notme++;
-		return IRQ_NONE;
-	}
-	pqhdr = (struct signal_queue_header __iomem *)
-		((char __iomem *)channel_header +
-		 readq(&channel_header->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;
-}
-
-static int
-virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
-{
-	int error;
-	struct Scsi_Host *scsihost;
-	struct virthba_info *virthbainfo;
-	int rsp;
-	int i;
-	irq_handler_t handler = virthba_isr;
-	struct channel_header __iomem *channel_header;
-	struct signal_queue_header __iomem *pqhdr;
-	u64 mask;
-
-	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
-	 * instance of a scsi host adapter. This scsi_host_alloc
-	 * function allocates a new Scsi_Host struct & performs basic
-	 * initialization.  The host is not published to the scsi
-	 * midlayer until scsi_add_host is called.
-	 */
-
-	/* arg 2 passed in length of extra space we want allocated
-	 * with scsi_host struct for our own use scsi_host_alloc
-	 * assign host_no
-	 */
-	scsihost = scsi_host_alloc(&virthba_driver_template,
-				   sizeof(struct virthba_info));
-	if (!scsihost)
-		return -ENODEV;
-
-	scsihost->this_id = UIS_MAGIC_VHBA;
-	/* linux treats max-channel differently than max-id & max-lun.
-	 * In the latter cases, those two values result in 0 to max-1
-	 * (inclusive) being scanned. But in the case of channels, the
-	 * scan is 0 to max (inclusive); so we will subtract one from
-	 * the max-channel value.
-	 */
-	scsihost->max_channel = (unsigned)virtpcidev->scsi.max.max_channel;
-	scsihost->max_id = (unsigned)virtpcidev->scsi.max.max_id;
-	scsihost->max_lun = (unsigned)virtpcidev->scsi.max.max_lun;
-	scsihost->cmd_per_lun = (unsigned)virtpcidev->scsi.max.cmd_per_lun;
-	scsihost->max_sectors =
-	    (unsigned short)(virtpcidev->scsi.max.max_io_size >> 9);
-	scsihost->sg_tablesize =
-	    (unsigned short)(virtpcidev->scsi.max.max_io_size / PAGE_SIZE);
-	if (scsihost->sg_tablesize > MAX_PHYS_INFO)
-		scsihost->sg_tablesize = MAX_PHYS_INFO;
-
-	/* this creates "host%d" in sysfs.  If 2nd argument is NULL,
-	 * then this generic /sys/devices/platform/host?  device is
-	 * created and /sys/scsi_host/host? ->
-	 * /sys/devices/platform/host?  If 2nd argument is not NULL,
-	 * then this generic /sys/devices/<path>/host? is created and
-	 * host? points to that device instead.
-	 */
-	error = scsi_add_host(scsihost, &virtpcidev->generic_dev);
-	if (error) {
-		POSTCODE_LINUX_2(VHBA_PROBE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-		/* decr refcount on scsihost which was incremented by
-		 * scsi_add_host so the scsi_host gets deleted
-		 */
-		scsi_host_put(scsihost);
-		return -ENODEV;
-	}
-
-	virthbainfo = (struct virthba_info *)scsihost->hostdata;
-	memset(virthbainfo, 0, sizeof(struct virthba_info));
-	for (i = 0; i < VIRTHBASOPENMAX; i++) {
-		if (!virthbas_open[i].virthbainfo) {
-			virthbas_open[i].virthbainfo = virthbainfo;
-			break;
-		}
-	}
-	virthbainfo->interrupt_vector = -1;
-	virthbainfo->chinfo.queueinfo = &virtpcidev->queueinfo;
-	virthbainfo->virtpcidev = virtpcidev;
-	spin_lock_init(&virthbainfo->chinfo.insertlock);
-
-	init_waitqueue_head(&virthbainfo->rsp_queue);
-	spin_lock_init(&virthbainfo->privlock);
-	memset(&virthbainfo->pending, 0, sizeof(virthbainfo->pending));
-	virthbainfo->serverdown = false;
-	virthbainfo->serverchangingstate = false;
-
-	virthbainfo->intr = virtpcidev->intr;
-	/* save of host within virthba_info */
-	virthbainfo->scsihost = scsihost;
-
-	/* save of host within virtpci_dev */
-	virtpcidev->scsi.scsihost = scsihost;
-
-	/* Setup workqueue for serverdown messages */
-	INIT_WORK(&virthbainfo->serverdown_completion,
-		  virthba_serverdown_complete);
-
-	writeq(readq(&virthbainfo->chinfo.queueinfo->chan->features) |
-	       ULTRA_IO_CHANNEL_IS_POLLING,
-	       &virthbainfo->chinfo.queueinfo->chan->features);
-	/* start thread that will receive scsicmnd responses */
-
-	channel_header = virthbainfo->chinfo.queueinfo->chan;
-	pqhdr = (struct signal_queue_header __iomem *)
-		((char __iomem *)channel_header +
-		 readq(&channel_header->ch_space_offset)) + IOCHAN_FROM_IOPART;
-	virthbainfo->flags_addr = &pqhdr->features;
-
-	if (!uisthread_start(&virthbainfo->chinfo.threadinfo,
-			     process_incoming_rsps,
-			     virthbainfo, "vhba_incoming")) {
-		/* decr refcount on scsihost which was incremented by
-		 * scsi_add_host so the scsi_host gets deleted
-		 */
-		POSTCODE_LINUX_2(VHBA_PROBE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-		scsi_host_put(scsihost);
-		return -ENODEV;
-	}
-	virthbainfo->interrupt_vector =
-	    virthbainfo->intr.recv_irq_handle & INTERRUPT_VECTOR_MASK;
-	rsp = request_irq(virthbainfo->interrupt_vector, handler, IRQF_SHARED,
-			  scsihost->hostt->name, virthbainfo);
-	if (rsp != 0) {
-		virthbainfo->interrupt_vector = -1;
-		POSTCODE_LINUX_2(VHBA_PROBE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-	} else {
-		u64 __iomem *features_addr =
-		    &virthbainfo->chinfo.queueinfo->chan->features;
-		mask = ~(ULTRA_IO_CHANNEL_IS_POLLING |
-			 ULTRA_IO_DRIVER_DISABLES_INTS);
-		uisqueue_interlocked_and(features_addr, mask);
-		mask = ULTRA_IO_DRIVER_ENABLES_INTS;
-		uisqueue_interlocked_or(features_addr, mask);
-		rsltq_wait_usecs = 4000000;
-	}
-
-	scsi_scan_host(scsihost);
-
-	POSTCODE_LINUX_2(VHBA_PROBE_EXIT_PC, POSTCODE_SEVERITY_INFO);
-	return 0;
-}
-
-static void
-virthba_remove(struct virtpci_dev *virtpcidev)
-{
-	struct virthba_info *virthbainfo;
-	struct Scsi_Host *scsihost =
-	    (struct Scsi_Host *)virtpcidev->scsi.scsihost;
-
-	virthbainfo = (struct virthba_info *)scsihost->hostdata;
-	if (virthbainfo->interrupt_vector != -1)
-		free_irq(virthbainfo->interrupt_vector, virthbainfo);
-
-	scsi_remove_host(scsihost);
-
-	uisthread_stop(&virthbainfo->chinfo.threadinfo);
-
-	/* decr refcount on scsihost which was incremented by
-	 * scsi_add_host so the scsi_host gets deleted
-	 */
-	scsi_host_put(scsihost);
-}
-
-static int
-forward_vdiskmgmt_command(enum vdisk_mgmt_types vdiskcmdtype,
-			  struct Scsi_Host *scsihost,
-			  struct uisscsi_dest *vdest)
-{
-	struct uiscmdrsp *cmdrsp;
-	struct virthba_info *virthbainfo =
-	    (struct virthba_info *)scsihost->hostdata;
-	int notifyresult = 0xffff;
-	wait_queue_head_t notifyevent;
-
-	if (virthbainfo->serverdown || virthbainfo->serverchangingstate)
-		return FAILED;
-
-	cmdrsp = kzalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
-	if (!cmdrsp)
-		return FAILED;  /* reject */
-
-	init_waitqueue_head(&notifyevent);
-
-	/* issue VDISK_MGMT_CMD
-	 * set type to command - as opposed to task mgmt
-	 */
-	cmdrsp->cmdtype = CMD_VDISKMGMT_TYPE;
-	/* specify the event that has to be triggered when this cmd is
-	 * complete
-	 */
-	cmdrsp->vdiskmgmt.notify = (void *)&notifyevent;
-	cmdrsp->vdiskmgmt.notifyresult = (void *)&notifyresult;
-
-	/* save destination */
-	cmdrsp->vdiskmgmt.vdisktype = vdiskcmdtype;
-	cmdrsp->vdiskmgmt.vdest.channel = vdest->channel;
-	cmdrsp->vdiskmgmt.vdest.id = vdest->id;
-	cmdrsp->vdiskmgmt.vdest.lun = vdest->lun;
-	cmdrsp->vdiskmgmt.scsicmd =
-	    (void *)(uintptr_t)
-		add_scsipending_entry_with_wait(virthbainfo, CMD_VDISKMGMT_TYPE,
-						(void *)cmdrsp);
-
-	uisqueue_put_cmdrsp_with_lock_client(virthbainfo->chinfo.queueinfo,
-					     cmdrsp, IOCHAN_TO_IOPART,
-					     &virthbainfo->chinfo.insertlock,
-					     DONT_ISSUE_INTERRUPT, (u64)NULL,
-					     OK_TO_WAIT, "vhba");
-	wait_event(notifyevent, notifyresult != 0xffff);
-	kfree(cmdrsp);
-	return SUCCESS;
-}
-
-/*****************************************************/
-/* Scsi Host support functions                       */
-/*****************************************************/
-
-static int
-forward_taskmgmt_command(enum task_mgmt_types tasktype,
-			 struct scsi_device *scsidev)
-{
-	struct uiscmdrsp *cmdrsp;
-	struct virthba_info *virthbainfo =
-	    (struct virthba_info *)scsidev->host->hostdata;
-	int notifyresult = 0xffff;
-	wait_queue_head_t notifyevent;
-
-	if (virthbainfo->serverdown || virthbainfo->serverchangingstate)
-		return FAILED;
-
-	cmdrsp = kzalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
-	if (!cmdrsp)
-		return FAILED;	/* reject */
-
-	init_waitqueue_head(&notifyevent);
-
-	/* issue TASK_MGMT_ABORT_TASK */
-	/* set type to command - as opposed to task mgmt */
-	cmdrsp->cmdtype = CMD_SCSITASKMGMT_TYPE;
-	/* specify the event that has to be triggered when this */
-	/* cmd is complete */
-	cmdrsp->scsitaskmgmt.notify = (void *)&notifyevent;
-	cmdrsp->scsitaskmgmt.notifyresult = (void *)&notifyresult;
-
-	/* save destination */
-	cmdrsp->scsitaskmgmt.tasktype = tasktype;
-	cmdrsp->scsitaskmgmt.vdest.channel = scsidev->channel;
-	cmdrsp->scsitaskmgmt.vdest.id = scsidev->id;
-	cmdrsp->scsitaskmgmt.vdest.lun = scsidev->lun;
-	cmdrsp->scsitaskmgmt.scsicmd =
-	    (void *)(uintptr_t)
-		add_scsipending_entry_with_wait(virthbainfo,
-						CMD_SCSITASKMGMT_TYPE,
-						(void *)cmdrsp);
-
-	uisqueue_put_cmdrsp_with_lock_client(virthbainfo->chinfo.queueinfo,
-					     cmdrsp, IOCHAN_TO_IOPART,
-					     &virthbainfo->chinfo.insertlock,
-					     DONT_ISSUE_INTERRUPT, (u64)NULL,
-					     OK_TO_WAIT, "vhba");
-	wait_event(notifyevent, notifyresult != 0xffff);
-	kfree(cmdrsp);
-	return SUCCESS;
-}
-
-/* The abort handler returns SUCCESS if it has succeeded to make LLDD
- * and all related hardware forget about the scmd.
- */
-static int
-virthba_abort_handler(struct scsi_cmnd *scsicmd)
-{
-	/* issue TASK_MGMT_ABORT_TASK */
-	struct scsi_device *scsidev;
-	struct virtdisk_info *vdisk;
-
-	scsidev = scsicmd->device;
-	for (vdisk = &((struct virthba_info *)scsidev->host->hostdata)->head;
-	     vdisk->next; vdisk = vdisk->next) {
-		if ((scsidev->channel == vdisk->channel) &&
-		    (scsidev->id == vdisk->id) &&
-		    (scsidev->lun == vdisk->lun)) {
-			if (atomic_read(&vdisk->error_count) <
-			    VIRTHBA_ERROR_COUNT) {
-				atomic_inc(&vdisk->error_count);
-				POSTCODE_LINUX_2(VHBA_COMMAND_HANDLER_PC,
-						 POSTCODE_SEVERITY_INFO);
-			} else
-				atomic_set(&vdisk->ios_threshold,
-					   IOS_ERROR_THRESHOLD);
-		}
-	}
-	return forward_taskmgmt_command(TASK_MGMT_ABORT_TASK, scsicmd->device);
-}
-
-static int
-virthba_bus_reset_handler(struct scsi_cmnd *scsicmd)
-{
-	/* issue TASK_MGMT_TARGET_RESET for each target on the bus */
-	struct scsi_device *scsidev;
-	struct virtdisk_info *vdisk;
-
-	scsidev = scsicmd->device;
-	for (vdisk = &((struct virthba_info *)scsidev->host->hostdata)->head;
-	     vdisk->next; vdisk = vdisk->next) {
-		if ((scsidev->channel == vdisk->channel) &&
-		    (scsidev->id == vdisk->id) &&
-		    (scsidev->lun == vdisk->lun)) {
-			if (atomic_read(&vdisk->error_count) <
-			    VIRTHBA_ERROR_COUNT) {
-				atomic_inc(&vdisk->error_count);
-				POSTCODE_LINUX_2(VHBA_COMMAND_HANDLER_PC,
-						 POSTCODE_SEVERITY_INFO);
-			} else
-				atomic_set(&vdisk->ios_threshold,
-					   IOS_ERROR_THRESHOLD);
-		}
-	}
-	return forward_taskmgmt_command(TASK_MGMT_BUS_RESET, scsicmd->device);
-}
-
-static int
-virthba_device_reset_handler(struct scsi_cmnd *scsicmd)
-{
-	/* issue TASK_MGMT_LUN_RESET */
-	struct scsi_device *scsidev;
-	struct virtdisk_info *vdisk;
-
-	scsidev = scsicmd->device;
-	for (vdisk = &((struct virthba_info *)scsidev->host->hostdata)->head;
-	     vdisk->next; vdisk = vdisk->next) {
-		if ((scsidev->channel == vdisk->channel) &&
-		    (scsidev->id == vdisk->id) &&
-		    (scsidev->lun == vdisk->lun)) {
-			if (atomic_read(&vdisk->error_count) <
-			    VIRTHBA_ERROR_COUNT) {
-				atomic_inc(&vdisk->error_count);
-				POSTCODE_LINUX_2(VHBA_COMMAND_HANDLER_PC,
-						 POSTCODE_SEVERITY_INFO);
-			} else
-				atomic_set(&vdisk->ios_threshold,
-					   IOS_ERROR_THRESHOLD);
-		}
-	}
-	return forward_taskmgmt_command(TASK_MGMT_LUN_RESET, scsicmd->device);
-}
-
-static int
-virthba_host_reset_handler(struct scsi_cmnd *scsicmd)
-{
-	/* issue TASK_MGMT_TARGET_RESET for each target on each bus for host */
-	return SUCCESS;
-}
-
-static char virthba_get_info_str[256];
-
-static const char *
-virthba_get_info(struct Scsi_Host *shp)
-{
-	/* Return version string */
-	sprintf(virthba_get_info_str, "virthba, version %s\n", VIRTHBA_VERSION);
-	return virthba_get_info_str;
-}
-
-static int
-virthba_ioctl(struct scsi_device *dev, int cmd, void __user *arg)
-{
-	return -EINVAL;
-}
-
-/* This returns SCSI_MLQUEUE_DEVICE_BUSY if the signal queue to IOpart
- * is full.
- */
-static int
-virthba_queue_command_lck(struct scsi_cmnd *scsicmd,
-			  void (*virthba_cmnd_done)(struct scsi_cmnd *))
-{
-	struct scsi_device *scsidev = scsicmd->device;
-	int insert_location;
-	unsigned char op;
-	unsigned char *cdb = scsicmd->cmnd;
-	struct Scsi_Host *scsihost = scsidev->host;
-	struct uiscmdrsp *cmdrsp;
-	unsigned int i;
-	struct virthba_info *virthbainfo =
-	    (struct virthba_info *)scsihost->hostdata;
-	struct scatterlist *sg = NULL;
-	struct scatterlist *sgl = NULL;
-	int sg_failed = 0;
-
-	if (virthbainfo->serverdown || virthbainfo->serverchangingstate)
-		return SCSI_MLQUEUE_DEVICE_BUSY;
-	cmdrsp = kzalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
-	if (!cmdrsp)
-		return 1;	/* reject the command */
-
-	/* now saving everything we need from scsi_cmd into cmdrsp
-	 * before we queue cmdrsp set type to command - as opposed to
-	 * task mgmt
-	 */
-	cmdrsp->cmdtype = CMD_SCSI_TYPE;
-	/* save the pending insertion location.  Deletion from pending
-	 * will return the scsicmd pointer for completion
-	 */
-	insert_location =
-	    add_scsipending_entry(virthbainfo, CMD_SCSI_TYPE, (void *)scsicmd);
-	if (insert_location != -1) {
-		cmdrsp->scsi.scsicmd = (void *)(uintptr_t)insert_location;
-	} else {
-		kfree(cmdrsp);
-		return SCSI_MLQUEUE_DEVICE_BUSY;
-	}
-	/* save done function that we have call when cmd is complete */
-	scsicmd->scsi_done = virthba_cmnd_done;
-	/* save destination */
-	cmdrsp->scsi.vdest.channel = scsidev->channel;
-	cmdrsp->scsi.vdest.id = scsidev->id;
-	cmdrsp->scsi.vdest.lun = scsidev->lun;
-	/* save datadir */
-	cmdrsp->scsi.data_dir = scsicmd->sc_data_direction;
-	memcpy(cmdrsp->scsi.cmnd, cdb, MAX_CMND_SIZE);
-
-	cmdrsp->scsi.bufflen = scsi_bufflen(scsicmd);
-
-	/* keep track of the max buffer length so far. */
-	if (cmdrsp->scsi.bufflen > max_buff_len)
-		max_buff_len = cmdrsp->scsi.bufflen;
-
-	if (scsi_sg_count(scsicmd) > MAX_PHYS_INFO) {
-		del_scsipending_entry(virthbainfo, (uintptr_t)insert_location);
-		kfree(cmdrsp);
-		return 1;	/* reject the command */
-	}
-
-	/* This is what we USED to do when we assumed we were running */
-	/* uissd & virthba on the same Linux system. */
-	/* cmdrsp->scsi.buffer = scsicmd->request_buffer; */
-	/* The following code does NOT make that assumption. */
-	/* convert buffer to phys information */
-	if (scsi_sg_count(scsicmd) == 0) {
-		if (scsi_bufflen(scsicmd) > 0) {
-			BUG_ON(scsi_sg_count(scsicmd) == 0);
-		}
-	} else {
-		/* buffer is scatterlist - copy it out */
-		sgl = scsi_sglist(scsicmd);
-
-		for_each_sg(sgl, sg, scsi_sg_count(scsicmd), i) {
-			cmdrsp->scsi.gpi_list[i].address = sg_phys(sg);
-			cmdrsp->scsi.gpi_list[i].length = sg->length;
-		}
-
-		if (sg_failed) {
-			/* BUG(); ***** For now, let it fail in uissd
-			 * if it is a problem, as it might just
-			 * work
-			 */
-		}
-
-		cmdrsp->scsi.guest_phys_entries = scsi_sg_count(scsicmd);
-	}
-
-	op = cdb[0];
-	i = uisqueue_put_cmdrsp_with_lock_client(virthbainfo->chinfo.queueinfo,
-						 cmdrsp, IOCHAN_TO_IOPART,
-						 &virthbainfo->chinfo.
-						 insertlock,
-						 DONT_ISSUE_INTERRUPT,
-						 (u64)NULL, DONT_WAIT, "vhba");
-	if (i == 0) {
-		/* queue must be full - and we said don't wait - return busy */
-		kfree(cmdrsp);
-		del_scsipending_entry(virthbainfo, (uintptr_t)insert_location);
-		return SCSI_MLQUEUE_DEVICE_BUSY;
-	}
-
-	/* we're done with cmdrsp space - data from it has been copied
-	 * into channel - free it now.
-	 */
-	kfree(cmdrsp);
-	return 0;		/* non-zero implies host/device is busy */
-}
-
-static int
-virthba_slave_alloc(struct scsi_device *scsidev)
-{
-	/* this called by the midlayer before scan for new devices -
-	 * LLD can alloc any struct & do init if needed.
-	 */
-	struct virtdisk_info *vdisk;
-	struct virtdisk_info *tmpvdisk;
-	struct virthba_info *virthbainfo;
-	struct Scsi_Host *scsihost = (struct Scsi_Host *)scsidev->host;
-
-	virthbainfo = (struct virthba_info *)scsihost->hostdata;
-	if (!virthbainfo)
-		return 0;	/* even though we errored, treat as success */
-
-	for (vdisk = &virthbainfo->head; vdisk->next; vdisk = vdisk->next) {
-		if (vdisk->next->valid &&
-		    (vdisk->next->channel == scsidev->channel) &&
-		    (vdisk->next->id == scsidev->id) &&
-		    (vdisk->next->lun == scsidev->lun))
-			return 0;
-	}
-	tmpvdisk = kzalloc(sizeof(*tmpvdisk), GFP_ATOMIC);
-	if (!tmpvdisk)
-		return 0;
-
-	tmpvdisk->channel = scsidev->channel;
-	tmpvdisk->id = scsidev->id;
-	tmpvdisk->lun = scsidev->lun;
-	tmpvdisk->valid = 1;
-	vdisk->next = tmpvdisk;
-	return 0;		/* success */
-}
-
-static int
-virthba_slave_configure(struct scsi_device *scsidev)
-{
-	return 0;		/* success */
-}
-
-static void
-virthba_slave_destroy(struct scsi_device *scsidev)
-{
-	/* midlevel calls this after device has been quiesced and
-	 * before it is to be deleted.
-	 */
-	struct virtdisk_info *vdisk, *delvdisk;
-	struct virthba_info *virthbainfo;
-	struct Scsi_Host *scsihost = (struct Scsi_Host *)scsidev->host;
-
-	virthbainfo = (struct virthba_info *)scsihost->hostdata;
-	for (vdisk = &virthbainfo->head; vdisk->next; vdisk = vdisk->next) {
-		if (vdisk->next->valid &&
-		    (vdisk->next->channel == scsidev->channel) &&
-		    (vdisk->next->id == scsidev->id) &&
-		    (vdisk->next->lun == scsidev->lun)) {
-			delvdisk = vdisk->next;
-			vdisk->next = vdisk->next->next;
-			kfree(delvdisk);
-			return;
-		}
-	}
-}
-
-/*****************************************************/
-/* Scsi Cmnd support thread                          */
-/*****************************************************/
-
-static void
-do_scsi_linuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
-{
-	struct virtdisk_info *vdisk;
-	struct scsi_device *scsidev;
-	struct sense_data *sd;
-
-	scsidev = scsicmd->device;
-	memcpy(scsicmd->sense_buffer, cmdrsp->scsi.sensebuf, MAX_SENSE_SIZE);
-	sd = (struct sense_data *)scsicmd->sense_buffer;
-
-	/* Do not log errors for disk-not-present inquiries */
-	if ((cmdrsp->scsi.cmnd[0] == INQUIRY) &&
-	    (host_byte(cmdrsp->scsi.linuxstat) == DID_NO_CONNECT) &&
-	    (cmdrsp->scsi.addlstat == ADDL_SEL_TIMEOUT))
-		return;
-
-	/* Okay see what our error_count is here.... */
-	for (vdisk = &((struct virthba_info *)scsidev->host->hostdata)->head;
-	     vdisk->next; vdisk = vdisk->next) {
-		if ((scsidev->channel != vdisk->channel) ||
-		    (scsidev->id != vdisk->id) ||
-		    (scsidev->lun != vdisk->lun))
-			continue;
-
-		if (atomic_read(&vdisk->error_count) < VIRTHBA_ERROR_COUNT) {
-			atomic_inc(&vdisk->error_count);
-			atomic_set(&vdisk->ios_threshold, IOS_ERROR_THRESHOLD);
-		}
-	}
-}
-
-static void
-do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
-{
-	struct scsi_device *scsidev;
-	unsigned char buf[36];
-	struct scatterlist *sg;
-	unsigned int i;
-	char *thispage;
-	char *thispage_orig;
-	int bufind = 0;
-	struct virtdisk_info *vdisk;
-
-	scsidev = scsicmd->device;
-	if ((cmdrsp->scsi.cmnd[0] == INQUIRY) &&
-	    (cmdrsp->scsi.bufflen >= MIN_INQUIRY_RESULT_LEN)) {
-		if (cmdrsp->scsi.no_disk_result == 0)
-			return;
-
-		/* Linux scsi code is weird; it wants
-		 * a device at Lun 0 to issue report
-		 * luns, but we don't want a disk
-		 * there so we'll present a processor
-		 * there. */
-		SET_NO_DISK_INQUIRY_RESULT(buf, cmdrsp->scsi.bufflen,
-					   scsidev->lun,
-					   DEV_DISK_CAPABLE_NOT_PRESENT,
-					   DEV_NOT_CAPABLE);
-
-		if (scsi_sg_count(scsicmd) == 0) {
-			if (scsi_bufflen(scsicmd) > 0) {
-				BUG_ON(scsi_sg_count(scsicmd) ==
-				       0);
-			}
-			memcpy(scsi_sglist(scsicmd), buf,
-			       cmdrsp->scsi.bufflen);
-			return;
-		}
-
-		sg = scsi_sglist(scsicmd);
-		for (i = 0; i < scsi_sg_count(scsicmd); i++) {
-			thispage_orig = kmap_atomic(sg_page(sg + i));
-			thispage = (void *)((unsigned long)thispage_orig |
-					     sg[i].offset);
-			memcpy(thispage, buf + bufind, sg[i].length);
-			kunmap_atomic(thispage_orig);
-			bufind += sg[i].length;
-		}
-	} else {
-		vdisk = &((struct virthba_info *)scsidev->host->hostdata)->head;
-		for ( ; vdisk->next; vdisk = vdisk->next) {
-			if ((scsidev->channel != vdisk->channel) ||
-			    (scsidev->id != vdisk->id) ||
-			    (scsidev->lun != vdisk->lun))
-				continue;
-
-			if (atomic_read(&vdisk->ios_threshold) > 0) {
-				atomic_dec(&vdisk->ios_threshold);
-				if (atomic_read(&vdisk->ios_threshold) == 0) {
-					atomic_set(&vdisk->error_count, 0);
-				}
-			}
-		}
-	}
-}
-
-static void
-complete_scsi_command(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
-{
-	/* take what we need out of cmdrsp and complete the scsicmd */
-	scsicmd->result = cmdrsp->scsi.linuxstat;
-	if (cmdrsp->scsi.linuxstat)
-		do_scsi_linuxstat(cmdrsp, scsicmd);
-	else
-		do_scsi_nolinuxstat(cmdrsp, scsicmd);
-
-	if (scsicmd->scsi_done)
-		scsicmd->scsi_done(scsicmd);
-}
-
-static inline void
-complete_vdiskmgmt_command(struct uiscmdrsp *cmdrsp)
-{
-	/* copy the result of the taskmgmt and */
-	/* wake up the error handler that is waiting for this */
-	*(int *)cmdrsp->vdiskmgmt.notifyresult = cmdrsp->vdiskmgmt.result;
-	wake_up_all((wait_queue_head_t *)cmdrsp->vdiskmgmt.notify);
-}
-
-static inline void
-complete_taskmgmt_command(struct uiscmdrsp *cmdrsp)
-{
-	/* copy the result of the taskmgmt and */
-	/* wake up the error handler that is waiting for this */
-	*(int *)cmdrsp->scsitaskmgmt.notifyresult =
-	    cmdrsp->scsitaskmgmt.result;
-	wake_up_all((wait_queue_head_t *)cmdrsp->scsitaskmgmt.notify);
-}
-
-static void
-drain_queue(struct virthba_info *virthbainfo, struct chaninfo *dc,
-	    struct uiscmdrsp *cmdrsp)
-{
-	unsigned long flags;
-	int qrslt = 0;
-	struct scsi_cmnd *scsicmd;
-	struct Scsi_Host *shost = virthbainfo->scsihost;
-
-	while (1) {
-		spin_lock_irqsave(&virthbainfo->chinfo.insertlock, flags);
-		if (!spar_channel_client_acquire_os(dc->queueinfo->chan,
-						    "vhba")) {
-			spin_unlock_irqrestore(&virthbainfo->chinfo.insertlock,
-					       flags);
-			virthbainfo->acquire_failed_cnt++;
-			break;
-		}
-		qrslt = uisqueue_get_cmdrsp(dc->queueinfo, cmdrsp,
-					    IOCHAN_FROM_IOPART);
-		spar_channel_client_release_os(dc->queueinfo->chan, "vhba");
-		spin_unlock_irqrestore(&virthbainfo->chinfo.insertlock, flags);
-		if (qrslt == 0)
-			break;
-		if (cmdrsp->cmdtype == CMD_SCSI_TYPE) {
-			/* scsicmd location is returned by the
-			 * deletion
-			 */
-			scsicmd = del_scsipending_entry(virthbainfo,
-							(uintptr_t)
-							 cmdrsp->scsi.scsicmd);
-			if (!scsicmd)
-				break;
-			/* complete the orig cmd */
-			complete_scsi_command(cmdrsp, scsicmd);
-		} else if (cmdrsp->cmdtype == CMD_SCSITASKMGMT_TYPE) {
-			if (!del_scsipending_entry(virthbainfo,
-				   (uintptr_t)cmdrsp->scsitaskmgmt.scsicmd))
-				break;
-			complete_taskmgmt_command(cmdrsp);
-		} else if (cmdrsp->cmdtype == CMD_NOTIFYGUEST_TYPE) {
-			/* The vHba pointer has no meaning in
-			 * a Client/Guest Partition. Let's be
-			 * safe and set it to NULL now.  Do
-			 * not use it here! */
-			cmdrsp->disknotify.v_hba = NULL;
-			process_disk_notify(shost, cmdrsp);
-		} else if (cmdrsp->cmdtype == CMD_VDISKMGMT_TYPE) {
-			if (!del_scsipending_entry(virthbainfo,
-						   (uintptr_t)
-						    cmdrsp->vdiskmgmt.scsicmd))
-				break;
-			complete_vdiskmgmt_command(cmdrsp);
-		}
-		/* cmdrsp is now available for reuse */
-	}
-}
-
-/* main function for the thread that waits for scsi commands to arrive
- * in a specified queue
- */
-static int
-process_incoming_rsps(void *v)
-{
-	struct virthba_info *virthbainfo = v;
-	struct chaninfo *dc = &virthbainfo->chinfo;
-	struct uiscmdrsp *cmdrsp = NULL;
-	const int SZ = sizeof(struct uiscmdrsp);
-	u64 mask;
-	unsigned long long rc1;
-
-	UIS_DAEMONIZE("vhba_incoming");
-	/* alloc once and reuse */
-	cmdrsp = kmalloc(SZ, GFP_ATOMIC);
-	if (!cmdrsp) {
-		complete_and_exit(&dc->threadinfo.has_stopped, 0);
-		return 0;
-	}
-	mask = ULTRA_CHANNEL_ENABLE_INTS;
-	while (1) {
-		if (kthread_should_stop())
-			break;
-		wait_event_interruptible_timeout(virthbainfo->rsp_queue,
-			 (atomic_read(&virthbainfo->interrupt_rcvd) == 1),
-				      usecs_to_jiffies(rsltq_wait_usecs));
-		atomic_set(&virthbainfo->interrupt_rcvd, 0);
-		/* drain queue */
-		drain_queue(virthbainfo, dc, cmdrsp);
-		rc1 = uisqueue_interlocked_or(virthbainfo->flags_addr, mask);
-	}
-
-	kfree(cmdrsp);
-
-	complete_and_exit(&dc->threadinfo.has_stopped, 0);
-}
-
-/*****************************************************/
-/* Debugfs filesystem functions                      */
-/*****************************************************/
-
-static ssize_t info_debugfs_read(struct file *file,
-				 char __user *buf, size_t len, loff_t *offset)
-{
-	ssize_t bytes_read = 0;
-	int str_pos = 0;
-	u64 phys_flags_addr;
-	int i;
-	struct virthba_info *virthbainfo;
-	char *vbuf;
-
-	if (len > MAX_BUF)
-		len = MAX_BUF;
-	vbuf = kzalloc(len, GFP_KERNEL);
-	if (!vbuf)
-		return -ENOMEM;
-
-	for (i = 0; i < VIRTHBASOPENMAX; i++) {
-		if (!virthbas_open[i].virthbainfo)
-			continue;
-
-		virthbainfo = virthbas_open[i].virthbainfo;
-
-		str_pos += scnprintf(vbuf + str_pos,
-				len - str_pos, "max_buff_len:%u\n",
-				max_buff_len);
-
-		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
-				"\nvirthba result queue poll wait:%d usecs.\n",
-				rsltq_wait_usecs);
-		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
-				"\ninterrupts_rcvd = %llu, interrupts_disabled = %llu\n",
-				virthbainfo->interrupts_rcvd,
-				virthbainfo->interrupts_disabled);
-		str_pos += scnprintf(vbuf + str_pos,
-				len - str_pos, "\ninterrupts_notme = %llu,\n",
-				virthbainfo->interrupts_notme);
-		phys_flags_addr = virt_to_phys((__force  void *)
-					       virthbainfo->flags_addr);
-		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
-				"flags_addr = %p, phys_flags_addr=0x%016llx, FeatureFlags=%llu\n",
-				virthbainfo->flags_addr, phys_flags_addr,
-				(__le64)readq(virthbainfo->flags_addr));
-		str_pos += scnprintf(vbuf + str_pos,
-			len - str_pos, "acquire_failed_cnt:%llu\n",
-			virthbainfo->acquire_failed_cnt);
-		str_pos += scnprintf(vbuf + str_pos, len - str_pos, "\n");
-	}
-
-	bytes_read = simple_read_from_buffer(buf, len, offset, vbuf, str_pos);
-	kfree(vbuf);
-	return bytes_read;
-}
-
-static ssize_t enable_ints_write(struct file *file, const char __user *buffer,
-				 size_t count, loff_t *ppos)
-{
-	char buf[4];
-	int i, new_value;
-	struct virthba_info *virthbainfo;
-
-	u64 __iomem *features_addr;
-	u64 mask;
-
-	if (count >= ARRAY_SIZE(buf))
-		return -EINVAL;
-
-	buf[count] = '\0';
-	if (copy_from_user(buf, buffer, count))
-		return -EFAULT;
-
-	i = kstrtoint(buf, 10, &new_value);
-
-	if (i != 0)
-		return -EFAULT;
-
-	/* set all counts to new_value usually 0 */
-	for (i = 0; i < VIRTHBASOPENMAX; i++) {
-		if (virthbas_open[i].virthbainfo) {
-			virthbainfo = virthbas_open[i].virthbainfo;
-			features_addr =
-				&virthbainfo->chinfo.queueinfo->chan->features;
-			if (new_value == 1) {
-				mask = ~(ULTRA_IO_CHANNEL_IS_POLLING |
-					 ULTRA_IO_DRIVER_DISABLES_INTS);
-				uisqueue_interlocked_and(features_addr, mask);
-				mask = ULTRA_IO_DRIVER_ENABLES_INTS;
-				uisqueue_interlocked_or(features_addr, mask);
-				rsltq_wait_usecs = 4000000;
-			} else {
-				mask = ~(ULTRA_IO_DRIVER_ENABLES_INTS |
-					 ULTRA_IO_DRIVER_DISABLES_INTS);
-				uisqueue_interlocked_and(features_addr, mask);
-				mask = ULTRA_IO_CHANNEL_IS_POLLING;
-				uisqueue_interlocked_or(features_addr, mask);
-				rsltq_wait_usecs = 4000;
-			}
-		}
-	}
-	return count;
-}
-
-/* As per VirtpciFunc returns 1 for success and 0 for failure */
-static int
-virthba_serverup(struct virtpci_dev *virtpcidev)
-{
-	struct virthba_info *virthbainfo =
-	    (struct virthba_info *)((struct Scsi_Host *)virtpcidev->scsi.
-				     scsihost)->hostdata;
-
-	if (!virthbainfo->serverdown)
-		return 1;
-
-	if (virthbainfo->serverchangingstate)
-		return 0;
-
-	virthbainfo->serverchangingstate = true;
-	/* Must transition channel to ATTACHED state BEFORE we
-	 * can start using the device again
-	 */
-	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,
-			     process_incoming_rsps,
-			     virthbainfo, "vhba_incoming")) {
-		return 0;
-	}
-	virthbainfo->serverdown = false;
-	virthbainfo->serverchangingstate = false;
-
-	return 1;
-}
-
-static void
-virthba_serverdown_complete(struct work_struct *work)
-{
-	struct virthba_info *virthbainfo;
-	struct virtpci_dev *virtpcidev;
-	int i;
-	struct scsipending *pendingdel = NULL;
-	struct scsi_cmnd *scsicmd = NULL;
-	struct uiscmdrsp *cmdrsp;
-	unsigned long flags;
-
-	virthbainfo = container_of(work, struct virthba_info,
-				   serverdown_completion);
-
-	/* Stop Using the IOVM Response Queue (queue should be drained
-	 * by the end)
-	 */
-	uisthread_stop(&virthbainfo->chinfo.threadinfo);
-
-	/* Fail Commands that weren't completed */
-	spin_lock_irqsave(&virthbainfo->privlock, flags);
-	for (i = 0; i < MAX_PENDING_REQUESTS; i++) {
-		pendingdel = &virthbainfo->pending[i];
-		switch (pendingdel->cmdtype) {
-		case CMD_SCSI_TYPE:
-			scsicmd = (struct scsi_cmnd *)pendingdel->sent;
-			scsicmd->result = DID_RESET << 16;
-			if (scsicmd->scsi_done)
-				scsicmd->scsi_done(scsicmd);
-			break;
-		case CMD_SCSITASKMGMT_TYPE:
-			cmdrsp = (struct uiscmdrsp *)pendingdel->sent;
-			wake_up_all((wait_queue_head_t *)
-				    cmdrsp->scsitaskmgmt.notify);
-			*(int *)cmdrsp->scsitaskmgmt.notifyresult =
-				TASK_MGMT_FAILED;
-			break;
-		case CMD_VDISKMGMT_TYPE:
-			cmdrsp = (struct uiscmdrsp *)pendingdel->sent;
-			*(int *)cmdrsp->vdiskmgmt.notifyresult =
-			    VDISK_MGMT_FAILED;
-			wake_up_all((wait_queue_head_t *)
-				    cmdrsp->vdiskmgmt.notify);
-			break;
-		default:
-			break;
-		}
-		pendingdel->cmdtype = 0;
-		pendingdel->sent = NULL;
-	}
-	spin_unlock_irqrestore(&virthbainfo->privlock, flags);
-
-	virtpcidev = virthbainfo->virtpcidev;
-
-	virthbainfo->serverdown = true;
-	virthbainfo->serverchangingstate = false;
-	/* Return the ServerDown response to Command */
-	visorchipset_device_pause_response(virtpcidev->bus_no,
-					   virtpcidev->device_no, 0);
-}
-
-/* As per VirtpciFunc returns 1 for success and 0 for failure */
-static int
-virthba_serverdown(struct virtpci_dev *virtpcidev, u32 state)
-{
-	int stat = 1;
-
-	struct virthba_info *virthbainfo =
-	    (struct virthba_info *)((struct Scsi_Host *)virtpcidev->scsi.
-				     scsihost)->hostdata;
-
-	if (!virthbainfo->serverdown && !virthbainfo->serverchangingstate) {
-		virthbainfo->serverchangingstate = true;
-		queue_work(virthba_serverdown_workqueue,
-			   &virthbainfo->serverdown_completion);
-	} else if (virthbainfo->serverchangingstate) {
-		stat = 0;
-	}
-
-	return stat;
-}
-
-/*****************************************************/
-/* Module Init & Exit functions                      */
-/*****************************************************/
-
-static int __init
-virthba_parse_line(char *str)
-{
-	return 1;
-}
-
-static void __init
-virthba_parse_options(char *line)
-{
-	char *next = line;
-
-	POSTCODE_LINUX_2(VHBA_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-	if (!line || !*line)
-		return;
-	while ((line = next)) {
-		next = strchr(line, ' ');
-		if (next)
-			*next++ = 0;
-		virthba_parse_line(line);
-	}
-
-	POSTCODE_LINUX_2(VHBA_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
-}
-
-static int __init
-virthba_mod_init(void)
-{
-	int error;
-	int i;
-
-	if (!unisys_spar_platform)
-		return -ENODEV;
-
-	POSTCODE_LINUX_2(VHBA_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-	virthba_parse_options(virthba_options);
-
-	error = virtpci_register_driver(&virthba_driver);
-	if (error < 0) {
-		POSTCODE_LINUX_3(VHBA_CREATE_FAILURE_PC, error,
-				 POSTCODE_SEVERITY_ERR);
-	} else {
-		/* create the debugfs directories and entries */
-		virthba_debugfs_dir = debugfs_create_dir("virthba", NULL);
-		debugfs_create_file("info", S_IRUSR, virthba_debugfs_dir,
-				    NULL, &debugfs_info_fops);
-		debugfs_create_u32("rqwait_usecs", S_IRUSR | S_IWUSR,
-				   virthba_debugfs_dir, &rsltq_wait_usecs);
-		debugfs_create_file("enable_ints", S_IWUSR,
-				    virthba_debugfs_dir, NULL,
-				    &debugfs_enable_ints_fops);
-		/* Initialize dar_work_queue */
-		INIT_WORK(&dar_work_queue, do_disk_add_remove);
-		spin_lock_init(&dar_work_queue_lock);
-
-		/* clear out array */
-		for (i = 0; i < VIRTHBASOPENMAX; i++)
-			virthbas_open[i].virthbainfo = NULL;
-		/* Initialize the serverdown workqueue */
-		virthba_serverdown_workqueue =
-		    create_singlethread_workqueue("virthba_serverdown");
-		if (!virthba_serverdown_workqueue) {
-			POSTCODE_LINUX_2(VHBA_CREATE_FAILURE_PC,
-					 POSTCODE_SEVERITY_ERR);
-			error = -1;
-		}
-	}
-
-	POSTCODE_LINUX_2(VHBA_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
-	return error;
-}
-
-static ssize_t
-virthba_acquire_lun(struct device *cdev, struct device_attribute *attr,
-		    const char *buf, size_t count)
-{
-	struct uisscsi_dest vdest;
-	struct Scsi_Host *shost = class_to_shost(cdev);
-	int i;
-
-	i = sscanf(buf, "%d-%d-%d", &vdest.channel, &vdest.id, &vdest.lun);
-	if (i != 3)
-		return i;
-
-	return forward_vdiskmgmt_command(VDISK_MGMT_ACQUIRE, shost, &vdest);
-}
-
-static ssize_t
-virthba_release_lun(struct device *cdev, struct device_attribute *attr,
-		    const char *buf, size_t count)
-{
-	struct uisscsi_dest vdest;
-	struct Scsi_Host *shost = class_to_shost(cdev);
-	int i;
-
-	i = sscanf(buf, "%d-%d-%d", &vdest.channel, &vdest.id, &vdest.lun);
-	if (i != 3)
-		return i;
-
-	return forward_vdiskmgmt_command(VDISK_MGMT_RELEASE, shost, &vdest);
-}
-
-#define CLASS_DEVICE_ATTR(_name, _mode, _show, _store)      \
-	struct device_attribute class_device_attr_##_name =   \
-		__ATTR(_name, _mode, _show, _store)
-
-static CLASS_DEVICE_ATTR(acquire_lun, S_IWUSR, NULL, virthba_acquire_lun);
-static CLASS_DEVICE_ATTR(release_lun, S_IWUSR, NULL, virthba_release_lun);
-
-static DEVICE_ATTRIBUTE *virthba_shost_attrs[] = {
-	&class_device_attr_acquire_lun,
-	&class_device_attr_release_lun,
-	NULL
-};
-
-static void __exit
-virthba_mod_exit(void)
-{
-	virtpci_unregister_driver(&virthba_driver);
-	/* unregister is going to call virthba_remove */
-	/* destroy serverdown completion workqueue */
-	if (virthba_serverdown_workqueue) {
-		destroy_workqueue(virthba_serverdown_workqueue);
-		virthba_serverdown_workqueue = NULL;
-	}
-
-	debugfs_remove_recursive(virthba_debugfs_dir);
-}
-
-/* specify function to be run at module insertion time */
-module_init(virthba_mod_init);
-
-/* specify function to be run when module is removed */
-module_exit(virthba_mod_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Usha Srinivasan");
-MODULE_ALIAS("uisvirthba");
-	/* this is extracted during depmod and kept in modules.dep */
-/* module parameter */
-module_param(virthba_options, charp, S_IRUGO);
diff --git a/drivers/staging/unisys/virthba/virthba.h b/drivers/staging/unisys/virthba/virthba.h
deleted file mode 100644
index 59901668d4f4..000000000000
--- a/drivers/staging/unisys/virthba/virthba.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/* virthba.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*
- * 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/Kconfig b/drivers/staging/unisys/virtpci/Kconfig
deleted file mode 100644
index 6d19482ce11b..000000000000
--- a/drivers/staging/unisys/virtpci/Kconfig
+++ /dev/null
@@ -1,10 +0,0 @@
-#
-# Unisys virtpci configuration
-#
-
-config UNISYS_VIRTPCI
-	tristate "Unisys virtpci driver"
-	select UNISYS_UISLIB
-	---help---
-	If you say Y here, you will enable the Unisys virtpci driver.
-
diff --git a/drivers/staging/unisys/virtpci/Makefile b/drivers/staging/unisys/virtpci/Makefile
deleted file mode 100644
index a26c696219a5..000000000000
--- a/drivers/staging/unisys/virtpci/Makefile
+++ /dev/null
@@ -1,10 +0,0 @@
-#
-# Makefile for Unisys virtpci
-#
-
-obj-$(CONFIG_UNISYS_VIRTPCI)	+= virtpci.o
-
-ccflags-y += -Idrivers/staging/unisys/include
-ccflags-y += -Idrivers/staging/unisys/uislib
-ccflags-y += -Idrivers/staging/unisys/common-spar/include
-ccflags-y += -Idrivers/staging/unisys/common-spar/include/channels
diff --git a/drivers/staging/unisys/virtpci/virtpci.c b/drivers/staging/unisys/virtpci/virtpci.c
deleted file mode 100644
index d5ad01783c07..000000000000
--- a/drivers/staging/unisys/virtpci/virtpci.c
+++ /dev/null
@@ -1,1394 +0,0 @@
-/* virtpci.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#define EXPORT_SYMTAB
-
-#include <linux/kernel.h>
-#ifdef CONFIG_MODVERSIONS
-#include <config/modversions.h>
-#endif
-#include "diagnostics/appos_subsystems.h"
-#include "uisutils.h"
-#include "vbuschannel.h"
-#include "vbushelper.h"
-#include <linux/types.h>
-#include <linux/io.h>
-#include <linux/uuid.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/pci.h>
-#include <linux/device.h>
-#include <linux/list.h>
-#include <linux/slab.h>
-#include <linux/mod_devicetable.h>
-#include <linux/if_ether.h>
-#include <linux/version.h>
-#include <linux/debugfs.h>
-#include "version.h"
-#include "guestlinuxdebug.h"
-#include "timskmod.h"
-
-struct driver_private {
-	struct kobject kobj;
-	struct klist klist_devices;
-	struct klist_node knode_bus;
-	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
- * it ourselves, and a macro to make getting the field a bit simpler.
- */
-#ifndef BUS_ID_SIZE
-#define BUS_ID_SIZE 20
-#endif
-
-#define BUS_ID(x) dev_name(x)
-
-/* MAX_BUF = 4 busses x ( 32 devices/bus + 1 busline) x 80 characters
- *         = 10,560 bytes ~ 2^14 = 16,384 bytes
- */
-#define MAX_BUF 16384
-
-#include "virtpci.h"
-
-/* this is shorter than using __FILE__ (full path name) in
- * debug/info/error messages
- */
-#define CURRENT_FILE_PC VIRT_PCI_PC_virtpci_c
-#define __MYFILE__ "virtpci.c"
-
-#define VIRTPCI_VERSION "01.00"
-
-/*****************************************************/
-/* Forward declarations                              */
-/*****************************************************/
-
-static int delete_vbus_device(struct device *vbus, void *data);
-static int match_busid(struct device *dev, void *data);
-static void virtpci_bus_release(struct device *dev);
-static void virtpci_device_release(struct device *dev);
-static int virtpci_device_add(struct device *parentbus, int devtype,
-			      struct add_virt_guestpart *addparams,
-			      struct scsi_adap_info *scsi,
-			      struct net_adap_info *net);
-static int virtpci_device_del(struct device *parentbus, int devtype,
-			      struct vhba_wwnn *wwnn, unsigned char macaddr[]);
-static int virtpci_device_serverdown(struct device *parentbus, int devtype,
-				     struct vhba_wwnn *wwnn,
-				     unsigned char macaddr[]);
-static int virtpci_device_serverup(struct device *parentbus, int devtype,
-				   struct vhba_wwnn *wwnn,
-				   unsigned char macaddr[]);
-static ssize_t virtpci_driver_attr_show(struct kobject *kobj,
-					struct attribute *attr, char *buf);
-static ssize_t virtpci_driver_attr_store(struct kobject *kobj,
-					 struct attribute *attr,
-					 const char *buf, size_t count);
-static int virtpci_bus_match(struct device *dev, struct device_driver *drv);
-static int virtpci_uevent(struct device *dev, struct kobj_uevent_env *env);
-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);
-
-static const struct file_operations debugfs_info_fops = {
-	.read = info_debugfs_read,
-};
-
-/*****************************************************/
-/* Globals                                           */
-/*****************************************************/
-
-/* methods in bus_type struct allow the bus code to serve as an
- * intermediary between the device core and individual device core and
- * individual drivers
- */
-static struct bus_type virtpci_bus_type = {
-	.name = "uisvirtpci",
-	.match = virtpci_bus_match,
-	.uevent = virtpci_uevent,
-};
-
-static struct device virtpci_rootbus_device = {
-	.init_name = "vbusroot",	/* root bus */
-	.release = virtpci_bus_release
-};
-
-/* filled in with info about parent chipset driver when we register with it */
-static struct ultra_vbus_deviceinfo chipset_driver_info;
-
-static const struct sysfs_ops virtpci_driver_sysfs_ops = {
-	.show = virtpci_driver_attr_show,
-	.store = virtpci_driver_attr_store,
-};
-
-static struct kobj_type virtpci_driver_kobj_type = {
-	.sysfs_ops = &virtpci_driver_sysfs_ops,
-};
-
-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 struct ultra_vbus_deviceinfo bus_driver_info;
-
-/*****************************************************/
-/* debugfs entries                                   */
-/*****************************************************/
-/* dentry is used to create the debugfs entry directory
- * for virtpci
- */
-static struct dentry *virtpci_debugfs_dir;
-
-struct virtpci_busdev {
-	struct device virtpci_bus_device;
-};
-
-/*****************************************************/
-/* Local functions                                   */
-/*****************************************************/
-
-static inline
-int WAIT_FOR_IO_CHANNEL(struct spar_io_channel_protocol __iomem  *chanptr)
-{
-	int count = 120;
-
-	while (count > 0) {
-		if (SPAR_CHANNEL_SERVER_READY(&chanptr->channel_header))
-			return 1;
-		UIS_THREAD_WAIT_SEC(1);
-		count--;
-	}
-	return 0;
-}
-
-/* Write the contents of <info> to the ULTRA_VBUS_CHANNEL_PROTOCOL.ChpInfo. */
-static int write_vbus_chp_info(struct spar_vbus_channel_protocol *chan,
-			       struct ultra_vbus_deviceinfo *info)
-{
-	int off;
-
-	if (!chan)
-		return -1;
-
-	off = sizeof(struct channel_header) + chan->hdr_info.chp_info_offset;
-	if (chan->hdr_info.chp_info_offset == 0) {
-		return -1;
-	}
-	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_bus_info(struct spar_vbus_channel_protocol *chan,
-			       struct ultra_vbus_deviceinfo *info)
-{
-	int off;
-
-	if (!chan)
-		return -1;
-
-	off = sizeof(struct channel_header) + chan->hdr_info.bus_info_offset;
-	if (chan->hdr_info.bus_info_offset == 0)
-		return -1;
-	memcpy(((u8 *)(chan)) + off, info, sizeof(*info));
-	return 0;
-}
-
-/* Write the contents of <info> to the
- * ULTRA_VBUS_CHANNEL_PROTOCOL.DevInfo[<devix>].
- */
-static int
-write_vbus_dev_info(struct spar_vbus_channel_protocol *chan,
-		    struct ultra_vbus_deviceinfo *info, int devix)
-{
-	int off;
-
-	if (!chan)
-		return -1;
-
-	off =
-	    (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)
-		return -1;
-
-	memcpy(((u8 *)(chan)) + off, info, sizeof(*info));
-	return 0;
-}
-
-/* adds a vbus
- * returns 0 failure, 1 success,
- */
-static int add_vbus(struct add_vbus_guestpart *addparams)
-{
-	int ret;
-	struct device *vbus;
-
-	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->bus_no);
-	vbus->release = virtpci_bus_release;
-	vbus->parent = &virtpci_rootbus_device;	/* root bus is parent */
-	vbus->bus = &virtpci_bus_type;	/* bus type */
-	vbus->platform_data = (__force void *)addparams->chanptr;
-
-	/* register a virt bus device -
-	 * this bus shows up under /sys/devices with .name value
-	 * "virtpci%d" any devices added to this bus then show up under
-	 * /sys/devices/virtpci0
-	 */
-	ret = device_register(vbus);
-	if (ret) {
-		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-	write_vbus_chp_info(vbus->platform_data /* chanptr */,
-			    &chipset_driver_info);
-	write_vbus_bus_info(vbus->platform_data /* chanptr */,
-			    &bus_driver_info);
-	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
-	return 1;
-}
-
-/* for CHANSOCK wwwnn/max are AUTO-GENERATED; for normal channels,
- * wwnn/max are in the channel header.
- */
-#define GET_SCSIADAPINFO_FROM_CHANPTR(chanptr) {			\
-	memcpy_fromio(&scsi.wwnn,					\
-		      &((struct spar_io_channel_protocol __iomem *)	\
-			chanptr)->vhba.wwnn,				\
-		      sizeof(struct vhba_wwnn));			\
-	memcpy_fromio(&scsi.max,					\
-		      &((struct spar_io_channel_protocol __iomem *)	\
-			chanptr)->vhba.max,				\
-		      sizeof(struct vhba_config_max));			\
-	}
-
-/* adds a vhba
- * returns 0 failure, 1 success,
- */
-static int add_vhba(struct add_virt_guestpart *addparams)
-{
-	int i;
-	struct scsi_adap_info scsi;
-	struct device *vbus;
-	unsigned char busid[BUS_ID_SIZE];
-
-	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-	if (!WAIT_FOR_IO_CHANNEL
-	    ((struct spar_io_channel_protocol __iomem *)addparams->chanptr)) {
-		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-
-	GET_SCSIADAPINFO_FROM_CHANPTR(addparams->chanptr);
-
-	/* 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)
-		return 0;
-
-	i = virtpci_device_add(vbus, VIRTHBA_TYPE, addparams, &scsi, NULL);
-	if (i) {
-		POSTCODE_LINUX_3(VPCI_CREATE_EXIT_PC, i,
-				 POSTCODE_SEVERITY_INFO);
-	}
-	return i;
-}
-
-/* for CHANSOCK macaddr is AUTO-GENERATED; for normal channels,
- * macaddr is in the channel header.
- */
-#define GET_NETADAPINFO_FROM_CHANPTR(chanptr) {				\
-		memcpy_fromio(net.mac_addr,				\
-		       ((struct spar_io_channel_protocol __iomem *)	\
-		       chanptr)->vnic.macaddr,				\
-		       MAX_MACADDR_LEN);				\
-		net.num_rcv_bufs =					\
-			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));				\
-}
-
-/* adds a vnic
- * returns 0 failure, 1 success,
- */
-static int
-add_vnic(struct add_virt_guestpart *addparams)
-{
-	int i;
-	struct net_adap_info net;
-	struct device *vbus;
-	unsigned char busid[BUS_ID_SIZE];
-
-	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-	if (!WAIT_FOR_IO_CHANNEL
-	    ((struct spar_io_channel_protocol __iomem *)addparams->chanptr)) {
-		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-
-	GET_NETADAPINFO_FROM_CHANPTR(addparams->chanptr);
-
-	/* 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)
-		return 0;
-
-	i = virtpci_device_add(vbus, VIRTNIC_TYPE, addparams, NULL, &net);
-	if (i) {
-		POSTCODE_LINUX_3(VPCI_CREATE_EXIT_PC, i,
-				 POSTCODE_SEVERITY_INFO);
-		return 1;
-	}
-	return 0;
-}
-
-/* delete vbus
- * returns 0 failure, 1 success,
- */
-static int
-delete_vbus(struct del_vbus_guestpart *delparams)
-{
-	struct device *vbus;
-	unsigned char busid[BUS_ID_SIZE];
-
-	/* 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)
-		return 0;
-
-	/* ensure that bus has no devices? -- TBD */
-	return 1;
-}
-
-static int
-delete_vbus_device(struct device *vbus, void *data)
-{
-	struct device *dev = &virtpci_rootbus_device;
-
-	if ((data) && match_busid(vbus, (void *)BUS_ID(dev))) {
-		/* skip it - don't delete root bus */
-		return 0;	/* pretend no error */
-	}
-	device_unregister(vbus);
-	kfree(vbus);
-	return 0;		/* no error */
-}
-
-/* pause vhba
-* returns 0 failure, 1 success,
-*/
-static int pause_vhba(struct pause_virt_guestpart *pauseparams)
-{
-	int i;
-	struct scsi_adap_info scsi;
-
-	GET_SCSIADAPINFO_FROM_CHANPTR(pauseparams->chanptr);
-
-	i = virtpci_device_serverdown(NULL /*no parent bus */, VIRTHBA_TYPE,
-				      &scsi.wwnn, NULL);
-	return i;
-}
-
-/* pause vnic
- * returns 0 failure, 1 success,
- */
-static int pause_vnic(struct pause_virt_guestpart *pauseparams)
-{
-	int i;
-	struct net_adap_info net;
-
-	GET_NETADAPINFO_FROM_CHANPTR(pauseparams->chanptr);
-
-	i = virtpci_device_serverdown(NULL /*no parent bus */, VIRTNIC_TYPE,
-				      NULL, net.mac_addr);
-	return i;
-}
-
-/* resume vhba
- * returns 0 failure, 1 success,
- */
-static int resume_vhba(struct resume_virt_guestpart *resumeparams)
-{
-	int i;
-	struct scsi_adap_info scsi;
-
-	GET_SCSIADAPINFO_FROM_CHANPTR(resumeparams->chanptr);
-
-	i = virtpci_device_serverup(NULL /*no parent bus */, VIRTHBA_TYPE,
-				    &scsi.wwnn, NULL);
-	return i;
-}
-
-/* resume vnic
-* returns 0 failure, 1 success,
-*/
-static int
-resume_vnic(struct resume_virt_guestpart *resumeparams)
-{
-	int i;
-	struct net_adap_info net;
-
-	GET_NETADAPINFO_FROM_CHANPTR(resumeparams->chanptr);
-
-	i = virtpci_device_serverup(NULL /*no parent bus */, VIRTNIC_TYPE,
-				    NULL, net.mac_addr);
-	return i;
-}
-
-/* delete vhba
-* returns 0 failure, 1 success,
-*/
-static int delete_vhba(struct del_virt_guestpart *delparams)
-{
-	int i;
-	struct scsi_adap_info scsi;
-
-	GET_SCSIADAPINFO_FROM_CHANPTR(delparams->chanptr);
-
-	i = virtpci_device_del(NULL /*no parent bus */, VIRTHBA_TYPE,
-			       &scsi.wwnn, NULL);
-	if (i) {
-		return 1;
-	}
-	return 0;
-}
-
-/* deletes a vnic
- * returns 0 failure, 1 success,
- */
-static int delete_vnic(struct del_virt_guestpart *delparams)
-{
-	int i;
-	struct net_adap_info net;
-
-	GET_NETADAPINFO_FROM_CHANPTR(delparams->chanptr);
-
-	i = virtpci_device_del(NULL /*no parent bus */, VIRTNIC_TYPE, NULL,
-			       net.mac_addr);
-	return i;
-}
-
-#define DELETE_ONE_VPCIDEV(vpcidev) { \
-	device_unregister(&vpcidev->generic_dev); \
-	kfree(vpcidev); \
-}
-
-/* deletes all vhbas and vnics
- * returns 0 failure, 1 success,
- */
-static void delete_all(void)
-{
-	int count = 0;
-	unsigned long flags;
-	struct virtpci_dev *tmpvpcidev, *nextvpcidev;
-
-	/* delete the entire vhba/vnic list in one shot */
-	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) {
-		nextvpcidev = tmpvpcidev->next;
-		/* delete the vhba/vnic at tmpvpcidev */
-		DELETE_ONE_VPCIDEV(tmpvpcidev);
-		tmpvpcidev = nextvpcidev;
-		count++;
-	}
-
-	/* now delete each vbus */
-	bus_for_each_dev(&virtpci_bus_type, NULL, (void *)1,
-			 delete_vbus_device);
-}
-
-/* deletes all vnics or vhbas
- * returns 0 failure, 1 success,
- */
-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;
-
-	/* 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)
-		return 0;
-
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE))
-		return 0;
-
-	/* delete all vhbas/vnics */
-	i = virtpci_device_del(vbus, devtype, NULL, NULL);
-	return 1;
-}
-
-static int virtpci_ctrlchan_func(struct guest_msgs *msg)
-{
-	switch (msg->msgtype) {
-	case GUEST_ADD_VBUS:
-		return add_vbus(&msg->add_vbus);
-	case GUEST_ADD_VHBA:
-		return add_vhba(&msg->add_vhba);
-	case GUEST_ADD_VNIC:
-		return add_vnic(&msg->add_vnic);
-	case GUEST_DEL_VBUS:
-		return delete_vbus(&msg->del_vbus);
-	case GUEST_DEL_VHBA:
-		return delete_vhba(&msg->del_vhba);
-	case GUEST_DEL_VNIC:
-		return delete_vnic(&msg->del_vhba);
-	case GUEST_DEL_ALL_VHBAS:
-		return delete_all_virt(VIRTHBA_TYPE, &msg->del_all_vhbas);
-	case GUEST_DEL_ALL_VNICS:
-		return delete_all_virt(VIRTNIC_TYPE, &msg->del_all_vnics);
-	case GUEST_DEL_ALL_VBUSES:
-		delete_all();
-		return 1;
-	case GUEST_PAUSE_VHBA:
-		return pause_vhba(&msg->pause_vhba);
-	case GUEST_PAUSE_VNIC:
-		return pause_vnic(&msg->pause_vnic);
-	case GUEST_RESUME_VHBA:
-		return resume_vhba(&msg->resume_vhba);
-	case GUEST_RESUME_VNIC:
-		return resume_vnic(&msg->resume_vnic);
-	default:
-		return 0;
-	}
-}
-
-/* same as driver_helper in bus.c linux */
-static int match_busid(struct device *dev, void *data)
-{
-	const char *name = data;
-
-	if (strcmp(name, BUS_ID(dev)) == 0)
-		return 1;
-	return 0;
-}
-
-/*****************************************************/
-/*  Bus functions                                    */
-/*****************************************************/
-
-static const struct pci_device_id *
-virtpci_match_device(const struct pci_device_id *ids,
-		     const struct virtpci_dev *dev)
-{
-	while (ids->vendor || ids->subvendor || ids->class_mask) {
-		if ((ids->vendor == dev->vendor) &&
-		    (ids->device == dev->device))
-			return ids;
-
-		ids++;
-	}
-	return NULL;
-}
-
-/* NOTE: !!!!!!  This function is called when a new device is added
-* for this bus.  Or, it is called for existing devices when a new
-* driver is added for this bus.  It returns nonzero if a given device
-* can be handled by the given driver.
-*/
-static int virtpci_bus_match(struct device *dev, struct device_driver *drv)
-{
-	struct virtpci_dev *virtpcidev = device_to_virtpci_dev(dev);
-	struct virtpci_driver *virtpcidrv = driver_to_virtpci_driver(drv);
-	int match = 0;
-
-	/* check ids list for a match */
-	if (virtpci_match_device(virtpcidrv->id_table, virtpcidev))
-		match = 1;
-
-	return match;		/* 0 - no match; 1 - yes it matches */
-}
-
-static int virtpci_uevent(struct device *dev, struct kobj_uevent_env *env)
-{
-	/* add variables to the environment prior to the generation of
-	 * hotplug events to user space
-	 */
-	if (add_uevent_var(env, "VIRTPCI_VERSION=%s", VIRTPCI_VERSION))
-		return -ENOMEM;
-	return 0;
-}
-
-/* For a child device just created on a client bus, fill in
- * information about the driver that is controlling this device into
- * the appropriate slot within the vbus channel of the bus
- * instance.
- */
-static void fix_vbus_dev_info(struct device *dev, int dev_no, int dev_type,
-			      struct virtpci_driver *virtpcidrv)
-{
-	struct device *vbus;
-	void *chan;
-	struct ultra_vbus_deviceinfo dev_info;
-	const char *stype;
-
-	if (!dev)
-		return;
-	if (!virtpcidrv)
-		return;
-
-	vbus = dev->parent;
-	if (!vbus)
-		return;
-
-	chan = vbus->platform_data;
-	if (!chan)
-		return;
-
-	switch (dev_type) {
-	case PCI_DEVICE_ID_VIRTHBA:
-		stype = "vHBA";
-		break;
-	case PCI_DEVICE_ID_VIRTNIC:
-		stype = "vNIC";
-		break;
-	default:
-		stype = "unknown";
-		break;
-	}
-	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_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
-* and whether this driver can work with it.  It should return -ENODEV
-* in case of failure.
-*/
-static int virtpci_device_probe(struct device *dev)
-{
-	struct virtpci_dev *virtpcidev = device_to_virtpci_dev(dev);
-	struct virtpci_driver *virtpcidrv =
-	    driver_to_virtpci_driver(dev->driver);
-	const struct pci_device_id *id;
-	int error = 0;
-
-	POSTCODE_LINUX_2(VPCI_PROBE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-	/* static match and static probe vs dynamic match & dynamic
-	 * probe - do we care?.
-	 */
-	if (!virtpcidrv->id_table)
-		return -ENODEV;
-
-	id = virtpci_match_device(virtpcidrv->id_table, virtpcidev);
-	if (!id)
-		return -ENODEV;
-
-	/* increment reference count */
-	get_device(dev);
-
-	/* if virtpcidev is not already claimed & probe function is
-	 * valid, probe it
-	 */
-	if (!virtpcidev->mydriver && virtpcidrv->probe) {
-		/* call the probe function - virthba or virtnic probe
-		 * is what it should be
-		 */
-		error = virtpcidrv->probe(virtpcidev, id);
-		if (!error) {
-			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 {
-			put_device(dev);
-		}
-	}
-	POSTCODE_LINUX_2(VPCI_PROBE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-	return error;		/* -ENODEV for probe failure */
-}
-
-static int virtpci_device_remove(struct device *dev_)
-{
-	/* dev_ passed in is the HBA device which we called
-	* generic_dev in our virtpcidev struct
-	*/
-	struct virtpci_dev *virtpcidev = device_to_virtpci_dev(dev_);
-	struct virtpci_driver *virtpcidrv = virtpcidev->mydriver;
-
-	if (virtpcidrv) {
-		/* TEMP: assuming we have only one such driver for now */
-		if (virtpcidrv->remove)
-			virtpcidrv->remove(virtpcidev);
-		virtpcidev->mydriver = NULL;
-	}
-
-	put_device(dev_);
-	return 0;
-}
-
-/*****************************************************/
-/* Bus functions                                     */
-/*****************************************************/
-
-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,
-			      struct net_adap_info *net)
-{
-	struct virtpci_dev *virtpcidev = NULL;
-	struct virtpci_dev *tmpvpcidev = NULL, *prev;
-	unsigned long flags;
-	int ret;
-	struct spar_io_channel_protocol __iomem *io_chan = NULL;
-	struct device *dev;
-
-	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
-		POSTCODE_LINUX_3(VPCI_CREATE_FAILURE_PC, devtype,
-				 POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-
-	/* add a Virtual Device */
-	virtpcidev = kzalloc(sizeof(*virtpcidev), GFP_ATOMIC);
-	if (!virtpcidev) {
-		POSTCODE_LINUX_2(MALLOC_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-
-	/* initialize stuff unique to virtpci_dev struct */
-	virtpcidev->devtype = devtype;
-	if (devtype == VIRTHBA_TYPE) {
-		virtpcidev->device = PCI_DEVICE_ID_VIRTHBA;
-		virtpcidev->scsi = *scsi;
-	} else {
-		virtpcidev->device = PCI_DEVICE_ID_VIRTNIC;
-		virtpcidev->net = *net;
-	}
-	virtpcidev->vendor = PCI_VENDOR_ID_UNISYS;
-	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... */
-	io_chan = (struct spar_io_channel_protocol __iomem *)
-		virtpcidev->queueinfo.chan;
-
-	virtpcidev->intr = addparams->intr;
-
-	/* initialize stuff in the device portion of the struct */
-	virtpcidev->generic_dev.bus = &virtpci_bus_type;
-	virtpcidev->generic_dev.parent = parentbus;
-	virtpcidev->generic_dev.release = virtpci_device_release;
-
-	dev_set_name(&virtpcidev->generic_dev, "%x:%x",
-		     addparams->bus_no, addparams->device_no);
-
-	/* add the vhba/vnic to virtpci device list - but check for
-	 * duplicate wwnn/macaddr first
-	 */
-	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) &&
-			    (tmpvpcidev->scsi.wwnn.wwnn2 == scsi->wwnn.wwnn2)) {
-				/* duplicate - already have vpcidev
-				   with this wwnn */
-				break;
-			}
-		} else
-		    if (memcmp
-			(tmpvpcidev->net.mac_addr, net->mac_addr,
-			 MAX_MACADDR_LEN) == 0) {
-			/* duplicate - already have vnic with this wwnn */
-			break;
-		}
-	}
-	if (tmpvpcidev) {
-		/* found a vhba/vnic already in the list with same
-		 * wwnn or macaddr - reject add
-		 */
-		write_unlock_irqrestore(&vpcidev_list_lock, flags);
-		kfree(virtpcidev);
-		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-		return 0;
-	}
-
-	/* add it at the head */
-	if (!vpcidev_list_head) {
-		vpcidev_list_head = virtpcidev;
-	} else {
-		/* insert virtpcidev at the head of our linked list of
-		 * vpcidevs
-		 */
-		virtpcidev->next = vpcidev_list_head;
-		vpcidev_list_head = virtpcidev;
-	}
-
-	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().
-	 */
-	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
-	* cause a "scheduling while atomic".
-	*/
-	ret = device_register(&virtpcidev->generic_dev);
-	/* NOTE: THIS IS CALLING HOTPLUG virtpci_hotplug!!!
-	 * This call to device_register results in virtpci_bus_match
-	 * being called !!!!!  And, if match returns success, then
-	 * virtpcidev->generic_dev.driver is setup to core_driver,
-	 * i.e., virtpci and the probe function
-	 * virtpcidev->generic_dev.driver->probe is called which
-	 * results in virtpci_device_probe being called. And if
-	 * virtpci_device_probe is successful
-	 */
-	if (ret) {
-		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(&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
-					vpcidev_list_head = tmpvpcidev->next;
-				break;
-			}
-		}
-		write_unlock_irqrestore(&vpcidev_list_lock, flags);
-		kfree(virtpcidev);
-		return 0;
-	}
-
-	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
-	return 1;
-}
-
-static int virtpci_device_serverdown(struct device *parentbus,
-				     int devtype,
-				     struct vhba_wwnn *wwnn,
-				     unsigned char macaddr[])
-{
-	int pausethisone = 0;
-	bool found = false;
-	struct virtpci_dev *tmpvpcidev, *prevvpcidev;
-	struct virtpci_driver *vpcidriver;
-	unsigned long flags;
-	int rc = 0;
-
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE))
-		return 0;
-
-	/* find the vhba or vnic in virtpci device list */
-	write_lock_irqsave(&vpcidev_list_lock, flags);
-
-	for (tmpvpcidev = vpcidev_list_head, prevvpcidev = NULL;
-	     (tmpvpcidev && !found);
-	     prevvpcidev = tmpvpcidev, tmpvpcidev = tmpvpcidev->next) {
-		if (tmpvpcidev->devtype != devtype)
-			continue;
-
-		if (devtype == VIRTHBA_TYPE) {
-			pausethisone =
-			    ((tmpvpcidev->scsi.wwnn.wwnn1 == wwnn->wwnn1) &&
-			     (tmpvpcidev->scsi.wwnn.wwnn2 == wwnn->wwnn2));
-			/* devtype is vhba, we're pausing vhba whose
-			* wwnn matches the current device's wwnn
-			*/
-		} else {	/* VIRTNIC_TYPE */
-			pausethisone =
-			    memcmp(tmpvpcidev->net.mac_addr, macaddr,
-				   MAX_MACADDR_LEN) == 0;
-			/* devtype is vnic, we're pausing vnic whose
-			* macaddr matches the current device's macaddr */
-		}
-
-		if (!pausethisone)
-			continue;
-
-		found = true;
-		vpcidriver = tmpvpcidev->mydriver;
-		rc = vpcidriver->suspend(tmpvpcidev, 0);
-	}
-	write_unlock_irqrestore(&vpcidev_list_lock, flags);
-
-	if (!found)
-		return 0;
-
-	return rc;
-}
-
-static int virtpci_device_serverup(struct device *parentbus,
-				   int devtype,
-				   struct vhba_wwnn *wwnn,
-				   unsigned char macaddr[])
-{
-	int resumethisone = 0;
-	bool found = false;
-	struct virtpci_dev *tmpvpcidev, *prevvpcidev;
-	struct virtpci_driver *vpcidriver;
-	unsigned long flags;
-	int rc = 0;
-
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE))
-		return 0;
-
-
-	/* find the vhba or vnic in virtpci device list */
-	write_lock_irqsave(&vpcidev_list_lock, flags);
-
-	for (tmpvpcidev = vpcidev_list_head, prevvpcidev = NULL;
-	     (tmpvpcidev && !found);
-	     prevvpcidev = tmpvpcidev, tmpvpcidev = tmpvpcidev->next) {
-		if (tmpvpcidev->devtype != devtype)
-			continue;
-
-		if (devtype == VIRTHBA_TYPE) {
-			resumethisone =
-			    ((tmpvpcidev->scsi.wwnn.wwnn1 == wwnn->wwnn1) &&
-			     (tmpvpcidev->scsi.wwnn.wwnn2 == wwnn->wwnn2));
-			/* devtype is vhba, we're resuming vhba whose
-			* wwnn matches the current device's wwnn */
-		} else {	/* VIRTNIC_TYPE */
-			resumethisone =
-			    memcmp(tmpvpcidev->net.mac_addr, macaddr,
-				   MAX_MACADDR_LEN) == 0;
-			/* devtype is vnic, we're resuming vnic whose
-			* macaddr matches the current device's macaddr */
-		}
-
-		if (!resumethisone)
-			continue;
-
-		found = true;
-		vpcidriver = tmpvpcidev->mydriver;
-		/* This should be done at BUS resume time, but an
-		* existing problem prevents us from ever getting a bus
-		* resume...  This hack would fail to work should we
-		* ever have a bus that contains NO devices, since we
-		* would never even get here in that case.
-		*/
-		fix_vbus_dev_info(&tmpvpcidev->generic_dev,
-				  tmpvpcidev->device_no,
-				  tmpvpcidev->device, vpcidriver);
-		rc = vpcidriver->resume(tmpvpcidev);
-	}
-
-	write_unlock_irqrestore(&vpcidev_list_lock, flags);
-
-	if (!found)
-		return 0;
-
-	return rc;
-}
-
-static int virtpci_device_del(struct device *parentbus,
-			      int devtype, struct vhba_wwnn *wwnn,
-			      unsigned char macaddr[])
-{
-	int count = 0, all = 0, delthisone;
-	struct virtpci_dev *tmpvpcidev, *prevvpcidev, *dellist = NULL;
-	unsigned long flags;
-
-#define DEL_CONTINUE { \
-	prevvpcidev = tmpvpcidev;\
-	tmpvpcidev = tmpvpcidev->next;\
-	continue; \
-}
-
-	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE))
-		return 0;
-
-	/* see if we are to delete all - NOTE: all implies we have a
-	 * valid parentbus
-	 */
-	all = ((devtype == VIRTHBA_TYPE) && (!wwnn)) ||
-	    ((devtype == VIRTNIC_TYPE) && (!macaddr));
-
-	/* find all the vhba or vnic or both in virtpci device list
-	* keep list of ones we are deleting so we can call
-	* device_unregister after we release the lock; otherwise we
-	* encounter "schedule while atomic"
-	*/
-	write_lock_irqsave(&vpcidev_list_lock, flags);
-	for (tmpvpcidev = vpcidev_list_head, prevvpcidev = NULL; tmpvpcidev;) {
-		if (tmpvpcidev->devtype != devtype)
-			DEL_CONTINUE;
-
-		if (all) {
-			delthisone =
-			    (tmpvpcidev->generic_dev.parent == parentbus);
-			/* we're deleting all vhbas or vnics on the
-			 * specified parent bus
-			 */
-		} else if (devtype == VIRTHBA_TYPE) {
-			delthisone =
-			    ((tmpvpcidev->scsi.wwnn.wwnn1 == wwnn->wwnn1) &&
-			     (tmpvpcidev->scsi.wwnn.wwnn2 == wwnn->wwnn2));
-			/* devtype is vhba, we're deleting vhba whose
-			 * wwnn matches the current device's wwnn
-			 */
-		} else {	/* VIRTNIC_TYPE */
-			delthisone =
-			    memcmp(tmpvpcidev->net.mac_addr, macaddr,
-				   MAX_MACADDR_LEN) == 0;
-			/* devtype is vnic, we're deleting vnic whose
-			* macaddr matches the current device's macaddr
-			*/
-		}
-
-		if (!delthisone)
-			DEL_CONTINUE;
-
-		/* take vhba/vnic out of the list */
-		if (prevvpcidev)
-			/* not at head */
-			prevvpcidev->next = tmpvpcidev->next;
-		else
-			vpcidev_list_head = tmpvpcidev->next;
-
-		/* add it to our deletelist */
-		tmpvpcidev->next = dellist;
-		dellist = tmpvpcidev;
-
-		count++;
-		if (!all)
-			break;	/* done */
-		/* going to top of loop again - set tmpvpcidev to next
-		 * one we're to process
-		 */
-		if (prevvpcidev)
-			tmpvpcidev = prevvpcidev->next;
-		else
-			tmpvpcidev = vpcidev_list_head;
-	}
-	write_unlock_irqrestore(&vpcidev_list_lock, flags);
-
-	if (!all && (count == 0))
-		return 0;
-
-	/* now delete each one from delete list */
-	while (dellist) {
-		/* save next */
-		tmpvpcidev = dellist->next;
-		/* delete the vhba/vnic at dellist */
-		DELETE_ONE_VPCIDEV(dellist);
-		/* do next */
-		dellist = tmpvpcidev;
-	}
-
-	return count;
-}
-
-static void virtpci_device_release(struct device *dev_)
-{
-	/* this function is called when the last reference to the
-	 * device is removed
-	 */
-}
-
-/*****************************************************/
-/* Driver functions                                  */
-/*****************************************************/
-
-#define kobj_to_device_driver(obj) container_of(obj, struct device_driver, kobj)
-#define attribute_to_driver_attribute(obj) \
-	container_of(obj, struct driver_attribute, attr)
-
-static ssize_t virtpci_driver_attr_show(struct kobject *kobj,
-					struct attribute *attr,
-					char *buf)
-{
-	struct driver_attribute *dattr = attribute_to_driver_attribute(attr);
-	ssize_t ret = 0;
-
-	struct driver_private *dprivate = to_driver(kobj);
-	struct device_driver *driver = dprivate->driver;
-
-	if (dattr->show)
-		ret = dattr->show(driver, buf);
-
-	return ret;
-}
-
-static ssize_t virtpci_driver_attr_store(struct kobject *kobj,
-					 struct attribute *attr,
-					 const char *buf, size_t count)
-{
-	struct driver_attribute *dattr = attribute_to_driver_attribute(attr);
-	ssize_t ret = 0;
-
-	struct driver_private *dprivate = to_driver(kobj);
-	struct device_driver *driver = dprivate->driver;
-
-	if (dattr->store)
-		ret = dattr->store(driver, buf, count);
-
-	return ret;
-}
-
-/* register a new virtpci driver */
-int virtpci_register_driver(struct virtpci_driver *drv)
-{
-	int result = 0;
-
-	if (!drv->id_table)
-		return 1;
-	/* initialize core driver fields needed to call driver_register */
-	drv->core_driver.name = drv->name;	/* name of driver in sysfs */
-	drv->core_driver.bus = &virtpci_bus_type;	/* type of bus this
-							 * driver works with */
-	drv->core_driver.probe = virtpci_device_probe;	/* called to query the
-							 * existence of a
-							 * specific device and
-							 * whether this driver
-							 *can work with it */
-	drv->core_driver.remove = virtpci_device_remove; /* called when the
-							  * device is removed
-							  * from the system */
-	/* register with core */
-	result = driver_register(&drv->core_driver);
-	/* calls bus_add_driver which calls driver_attach and
-	 * module_add_driver
-	 */
-	if (result)
-		return result;	/* failed */
-
-	drv->core_driver.p->kobj.ktype = &virtpci_driver_kobj_type;
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(virtpci_register_driver);
-
-void virtpci_unregister_driver(struct virtpci_driver *drv)
-{
-	driver_unregister(&drv->core_driver);
-	/* driver_unregister calls bus_remove_driver
-	 * bus_remove_driver calls device_detach
-	 * device_detach calls device_release_driver for each of the
-	 * driver's devices
-	 * device_release driver calls drv->remove which is
-	 * virtpci_device_remove
-	 * virtpci_device_remove calls virthba_remove
-	 */
-}
-EXPORT_SYMBOL_GPL(virtpci_unregister_driver);
-
-/*****************************************************/
-/* debugfs filesystem functions                      */
-/*****************************************************/
-struct print_vbus_info {
-	int *str_pos;
-	char *buf;
-	size_t *len;
-};
-
-static int print_vbus(struct device *vbus, void *data)
-{
-	struct print_vbus_info *p = (struct print_vbus_info *)data;
-
-	*p->str_pos += scnprintf(p->buf + *p->str_pos, *p->len - *p->str_pos,
-				"bus_id:%s\n", dev_name(vbus));
-	return 0;
-}
-
-static ssize_t info_debugfs_read(struct file *file, char __user *buf,
-				 size_t len, loff_t *offset)
-{
-	ssize_t bytes_read = 0;
-	int str_pos = 0;
-	struct virtpci_dev *tmpvpcidev;
-	unsigned long flags;
-	struct print_vbus_info printparam;
-	char *vbuf;
-
-	if (len > MAX_BUF)
-		len = MAX_BUF;
-	vbuf = kzalloc(len, GFP_KERNEL);
-	if (!vbuf)
-		return -ENOMEM;
-
-	str_pos += scnprintf(vbuf + str_pos, len - str_pos,
-			" Virtual PCI Bus devices\n");
-	printparam.str_pos = &str_pos;
-	printparam.buf = vbuf;
-	printparam.len = &len;
-	bus_for_each_dev(&virtpci_bus_type, NULL, (void *)&printparam,
-			 print_vbus);
-
-	str_pos += scnprintf(vbuf + str_pos, len - str_pos,
-			"\n Virtual PCI devices\n");
-	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->bus_no,
-					tmpvpcidev->device_no,
-					tmpvpcidev->scsi.wwnn.wwnn1,
-					tmpvpcidev->scsi.wwnn.wwnn2,
-					tmpvpcidev->scsi.max.max_channel,
-					tmpvpcidev->scsi.max.max_id,
-					tmpvpcidev->scsi.max.max_lun,
-					tmpvpcidev->scsi.max.cmd_per_lun);
-		} else {
-			str_pos += scnprintf(vbuf + str_pos, len - str_pos,
-					"[%d:%d] VNic:%pM num_rcv_bufs:%d mtu:%d",
-					tmpvpcidev->bus_no,
-					tmpvpcidev->device_no,
-					tmpvpcidev->net.mac_addr,
-					tmpvpcidev->net.num_rcv_bufs,
-					tmpvpcidev->net.mtu);
-		}
-		str_pos += scnprintf(vbuf + str_pos,
-				len - str_pos, " chanptr:%p\n",
-				tmpvpcidev->queueinfo.chan);
-				tmpvpcidev = tmpvpcidev->next;
-	}
-	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);
-	kfree(vbuf);
-	return bytes_read;
-}
-
-/*****************************************************/
-/* Module Init & Exit functions                      */
-/*****************************************************/
-
-static int __init virtpci_mod_init(void)
-{
-	int ret;
-
-	if (!unisys_spar_platform)
-		return -ENODEV;
-
-	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
-
-	ret = bus_register(&virtpci_bus_type);
-	/* creates /sys/bus/uisvirtpci which contains devices &
-	 * drivers directory
-	 */
-	if (ret) {
-		POSTCODE_LINUX_3(VPCI_CREATE_FAILURE_PC, ret,
-				 POSTCODE_SEVERITY_ERR);
-		return ret;
-	}
-	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);
-	if (ret) {
-		bus_unregister(&virtpci_bus_type);
-		POSTCODE_LINUX_3(VPCI_CREATE_FAILURE_PC, ret,
-				 POSTCODE_SEVERITY_ERR);
-		return ret;
-	}
-
-	if (!uisctrl_register_req_handler(2, (void *)&virtpci_ctrlchan_func,
-					  &chipset_driver_info)) {
-		POSTCODE_LINUX_2(VPCI_CREATE_FAILURE_PC, POSTCODE_SEVERITY_ERR);
-		device_unregister(&virtpci_rootbus_device);
-		bus_unregister(&virtpci_bus_type);
-		return -1;
-	}
-
-	/* 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);
-	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
-	return 0;
-}
-
-static void __exit virtpci_mod_exit(void)
-{
-	/* unregister the callback function */
-	device_unregister(&virtpci_rootbus_device);
-	bus_unregister(&virtpci_bus_type);
-	debugfs_remove_recursive(virtpci_debugfs_dir);
-}
-
-module_init(virtpci_mod_init);
-module_exit(virtpci_mod_exit);
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Usha Srinivasan");
-MODULE_ALIAS("uisvirtpci");
-
diff --git a/drivers/staging/unisys/virtpci/virtpci.h b/drivers/staging/unisys/virtpci/virtpci.h
deleted file mode 100644
index 9d85f55e8169..000000000000
--- a/drivers/staging/unisys/virtpci/virtpci.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/* virtpci.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*
- * Unisys Virtual PCI driver header
- */
-
-#ifndef __VIRTPCI_H__
-#define __VIRTPCI_H__
-
-#include "uisqueue.h"
-#include <linux/version.h>
-#include <linux/uuid.h>
-
-#define PCI_DEVICE_ID_VIRTHBA 0xAA00
-#define PCI_DEVICE_ID_VIRTNIC 0xAB00
-
-struct scsi_adap_info {
-	void *scsihost;		/* scsi host if this device is a scsi hba */
-	struct vhba_wwnn wwnn;	/* the world wide node name of vhba */
-	struct vhba_config_max max;	/* various max specifications used
-					 * to config vhba */
-};
-
-struct net_adap_info {
-	struct net_device *netdev;	/* network device if this
-					 * device is a NIC */
-	u8 mac_addr[MAX_MACADDR_LEN];
-	int num_rcv_bufs;
-	unsigned mtu;
-	uuid_le zone_uuid;
-};
-
-enum virtpci_dev_type {
-	VIRTHBA_TYPE = 0,
-	VIRTNIC_TYPE = 1,
-	VIRTBUS_TYPE = 6,
-};
-
-struct virtpci_dev {
-	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 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;
-		struct net_adap_info net;
-	};
-
-	struct uisqueue_info queueinfo;	/* holds ptr to channel where cmds &
-					 * rsps are queued & retrieved */
-	struct virtpci_dev *next;	/* points to next virtpci device */
-};
-
-struct virtpci_driver {
-	struct list_head node;
-	const char *name;	/* the name of the driver in sysfs */
-	const char *version;
-	const char *vertag;
-	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 */
-	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 */
-	int (*resume)(struct virtpci_dev *dev);	/* Device woken up */
-	int (*enable_wake)(struct virtpci_dev *dev,
-			   u32 state, int enable);	/* Enable wake event */
-	struct device_driver core_driver;	/* VIRTPCI core fills this in */
-};
-
-#define	driver_to_virtpci_driver(in_drv) \
-	container_of(in_drv, struct virtpci_driver, core_driver)
-#define device_to_virtpci_dev(in_dev) \
-	container_of(in_dev, struct virtpci_dev, generic_dev)
-
-int virtpci_register_driver(struct virtpci_driver *);
-void virtpci_unregister_driver(struct virtpci_driver *);
-
-#endif /* __VIRTPCI_H__ */
diff --git a/drivers/staging/unisys/visorbus/Kconfig b/drivers/staging/unisys/visorbus/Kconfig
new file mode 100644
index 000000000000..9b299ac86015
--- /dev/null
+++ b/drivers/staging/unisys/visorbus/Kconfig
@@ -0,0 +1,9 @@
+#
+# Unisys visorbus configuration
+#
+
+config UNISYS_VISORBUS
+	tristate "Unisys visorbus driver"
+	depends on UNISYSSPAR
+	---help---
+	If you say Y here, you will enable the Unisys visorbus driver.
diff --git a/drivers/staging/unisys/visorbus/Makefile b/drivers/staging/unisys/visorbus/Makefile
new file mode 100644
index 000000000000..fa27ee5f336c
--- /dev/null
+++ b/drivers/staging/unisys/visorbus/Makefile
@@ -0,0 +1,13 @@
+#
+# Makefile for Unisys visorbus
+#
+
+obj-$(CONFIG_UNISYS_VISORBUS)	+= visorbus.o
+
+visorbus-y := visorbus_main.o
+visorbus-y += visorchannel.o
+visorbus-y += visorchipset.o
+visorbus-y += periodic_work.o
+
+ccflags-y += -Idrivers/staging/unisys/include
+ccflags-y += -Idrivers/staging/unisys/visorutil
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h b/drivers/staging/unisys/visorbus/controlvmchannel.h
index a66db7968d6c..a50d9cf4bed7 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
+++ b/drivers/staging/unisys/visorbus/controlvmchannel.h
@@ -18,59 +18,75 @@
 
 #include <linux/uuid.h>
 #include "channel.h"
-#include "controlframework.h"
-
-typedef u64 GUEST_PHYSICAL_ADDRESS;
-
-enum { INVALID_GUEST_FIRMWARE, SAMPLE_GUEST_FIRMWARE,
-	    TIANO32_GUEST_FIRMWARE, TIANO64_GUEST_FIRMWARE
-};
 
 /* {2B3C2D10-7EF5-4ad8-B966-3448B7386B3D} */
 #define SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID	\
 		UUID_LE(0x2b3c2d10, 0x7ef5, 0x4ad8, \
-				0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d)
-
-static const uuid_le spar_controlvm_channel_protocol_uuid =
-	SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID;
+			0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d)
 
 #define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE \
 	ULTRA_CHANNEL_PROTOCOL_SIGNATURE
-#define CONTROLVM_MESSAGE_MAX     64
+#define CONTROLVM_MESSAGE_MAX 64
 
 /* 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. */
+ * 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_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID  1
 
 #define SPAR_CONTROLVM_CHANNEL_OK_CLIENT(ch)           \
 	spar_check_channel_client(ch, \
-		spar_controlvm_channel_protocol_uuid, \
+		SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID, \
 		"controlvm", \
 		sizeof(struct spar_controlvm_channel_protocol), \
 		ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID, \
 		ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE)
 
-#define MY_DEVICE_INDEX 0
-#define MAX_MACDATA_LEN 8 /* number of bytes for MAC address in config packet */
 #define MAX_SERIAL_NUM	32
 
-#define DISK_ZERO_PUN_NUMBER	1  /* Target ID on the SCSI bus for LUN 0 */
-#define DISK_ZERO_LUN_NUMBER	3  /* Logical Unit Number */
-
-/* Defines for various channel queues... */
+/* Defines for various channel queues */
 #define CONTROLVM_QUEUE_REQUEST		0
 #define CONTROLVM_QUEUE_RESPONSE	1
-#define	CONTROLVM_QUEUE_EVENT		2
+#define CONTROLVM_QUEUE_EVENT		2
 #define CONTROLVM_QUEUE_ACK		3
 
-/* Max number of messages stored during IOVM creation to be reused
- * after crash */
+/* Max num of messages stored during IOVM creation to be reused after crash */
 #define CONTROLVM_CRASHMSG_MAX		2
 
-/** Ids for commands that may appear in either queue of a ControlVm channel.
+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
+				 * ShuttingDown */
+	u16 allocated:1;	/* Bit 4: memory (device/port number)
+				 * has been selected by Command */
+	u16 known:1;		/* Bit 5: has been introduced to the
+				 * service/guest partition */
+	u16 ready:1;		/* Bit 6: service/Guest partition has
+				 * responded to introduction */
+	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 */
+};
+
+static const struct spar_segment_state segment_state_running = {
+	1, 1, 1, 0, 1, 1, 1, 1
+};
+
+static const struct spar_segment_state segment_state_paused = {
+	1, 1, 1, 0, 1, 1, 1, 0
+};
+
+static const struct spar_segment_state segment_state_standby = {
+	1, 1, 0, 0, 1, 1, 1, 0
+};
+
+/* Ids for commands that may appear in either queue of a ControlVm channel.
  *
  *  Commands that are initiated by the command partition (CP), by an IO or
  *  console service partition (SP), or by a guest partition (GP)are:
@@ -84,60 +100,49 @@ static const uuid_le spar_controlvm_channel_protocol_uuid =
  */
 enum controlvm_id {
 	CONTROLVM_INVALID = 0,
-	/* SWITCH commands required Parameter: SwitchNumber  */
-	/* BUS commands required Parameter: BusNumber  */
-	CONTROLVM_BUS_CREATE = 0x101,	/* CP --> SP, GP */
-	CONTROLVM_BUS_DESTROY = 0x102,	/* CP --> SP, GP */
-	CONTROLVM_BUS_CONFIGURE = 0x104,	/* CP --> SP */
-	CONTROLVM_BUS_CHANGESTATE = 0x105,	/* CP --> SP, GP */
-	CONTROLVM_BUS_CHANGESTATE_EVENT = 0x106, /* SP, GP --> CP */
-/* DEVICE commands required Parameter: BusNumber, DeviceNumber  */
-
-	CONTROLVM_DEVICE_CREATE = 0x201,	/* CP --> SP, GP */
-	CONTROLVM_DEVICE_DESTROY = 0x202,	/* CP --> SP, GP */
-	CONTROLVM_DEVICE_CONFIGURE = 0x203,	/* CP --> SP */
-	CONTROLVM_DEVICE_CHANGESTATE = 0x204,	/* CP --> SP, GP */
-	CONTROLVM_DEVICE_CHANGESTATE_EVENT = 0x205, /* SP, GP --> CP */
-	CONTROLVM_DEVICE_RECONFIGURE = 0x206,	/* CP --> Boot */
-/* DISK commands required Parameter: BusNumber, DeviceNumber  */
-	CONTROLVM_DISK_CREATE = 0x221,	/* CP --> SP */
-	CONTROLVM_DISK_DESTROY = 0x222,	/* CP --> SP */
-	CONTROLVM_DISK_CONFIGURE = 0x223,	/* CP --> SP */
-	CONTROLVM_DISK_CHANGESTATE = 0x224,	/* CP --> SP */
+	/* SWITCH commands required Parameter: SwitchNumber */
+	/* BUS commands required Parameter: BusNumber */
+	CONTROLVM_BUS_CREATE = 0x101,			/* CP --> SP, GP */
+	CONTROLVM_BUS_DESTROY = 0x102,			/* CP --> SP, GP */
+	CONTROLVM_BUS_CONFIGURE = 0x104,		/* CP --> SP */
+	CONTROLVM_BUS_CHANGESTATE = 0x105,		/* CP --> SP, GP */
+	CONTROLVM_BUS_CHANGESTATE_EVENT = 0x106,	/* SP, GP --> CP */
+/* DEVICE commands required Parameter: BusNumber, DeviceNumber */
+
+	CONTROLVM_DEVICE_CREATE = 0x201,		/* CP --> SP, GP */
+	CONTROLVM_DEVICE_DESTROY = 0x202,		/* CP --> SP, GP */
+	CONTROLVM_DEVICE_CONFIGURE = 0x203,		/* CP --> SP */
+	CONTROLVM_DEVICE_CHANGESTATE = 0x204,		/* CP --> SP, GP */
+	CONTROLVM_DEVICE_CHANGESTATE_EVENT = 0x205,	/* SP, GP --> CP */
+	CONTROLVM_DEVICE_RECONFIGURE = 0x206,		/* CP --> Boot */
 /* CHIPSET commands */
-	CONTROLVM_CHIPSET_INIT = 0x301,	/* CP --> SP, GP */
-	CONTROLVM_CHIPSET_STOP = 0x302,	/* CP --> SP, GP */
-	CONTROLVM_CHIPSET_SHUTDOWN = 0x303,	/* CP --> SP */
-	CONTROLVM_CHIPSET_READY = 0x304,	/* CP --> SP */
-	CONTROLVM_CHIPSET_SELFTEST = 0x305,	/* CP --> SP */
+	CONTROLVM_CHIPSET_INIT = 0x301,			/* CP --> SP, GP */
+	CONTROLVM_CHIPSET_STOP = 0x302,			/* CP --> SP, GP */
+	CONTROLVM_CHIPSET_READY = 0x304,		/* CP --> SP */
+	CONTROLVM_CHIPSET_SELFTEST = 0x305,		/* CP --> SP */
 
 };
 
 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
-	  *   above) will receive the interrupt. Passed as a parameter
-	  *   to Issue_VMCALL_IO_QUEUE_TRANSITION, which generates the
-	  *   interrupt.  Currently this is used by IOPart-SP to wake
-	  *   up GP when Data Channel transitions from empty to
-	  *   non-empty.*/
-	u64 send_irq_handle;
-
-	 /**< specifies interrupt handle. It is used to retrieve the
+	u64 reserved1;
+
+	 /* 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
-	  *   interrupt.  Used by IOPart-GP only. */
+	  *   interrupt.  Used by IOPart-GP only.
+	  */
 	u64 recv_irq_handle;
 
-	 /**< specifies interrupt vector. It, interrupt pin, and shared are
+	 /* specifies interrupt vector. It, interrupt pin, and shared are
 	  *   used to connect to the corresponding interrupt.  Used by
-	  *   IOPart-GP only. */
+	  *   IOPart-GP only.
+	  */
 	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. */
+    /* 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 recv_irq_shared;
 	u8 reserved[3];	/* Natural alignment purposes */
 };
@@ -151,20 +156,19 @@ struct pci_id {
 };
 
 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 */
+	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 */
 };
 
 enum ultra_chipset_feature {
 	ULTRA_CHIPSET_FEATURE_REPLY = 0x00000001,
 	ULTRA_CHIPSET_FEATURE_PARA_HOTPLUG = 0x00000002,
-	ULTRA_CHIPSET_FEATURE_PCIVBUS = 0x00000004
 };
 
-/** This is the common structure that is at the beginning of every
+/* 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.
@@ -181,26 +185,26 @@ struct controlvm_message_header  {
 	u32 completion_status;	/* Error status code or result of
 				 * message completion */
 	struct  {
-		u32 failed:1;		   /**< =1 in a response to * signify
+		u32 failed:1;		   /* =1 in a response to * signify
 					    * failure */
-		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
+		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 test_message:1;	   /**< =1 for testing use only
+		u32 test_message:1;	   /* =1 for testing use only
 					    * (Control and Command ignore this
 					    * bit) */
-		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 writer_in_diag:1;	/**< =1 the DiagWriter is active in the
-					 * Diagnostic Partition*/
+		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 writer_in_diag:1;	   /* =1 the DiagWriter is active in the
+					    * Diagnostic Partition*/
 	} flags;
 	u32 reserved;		/* Natural alignment */
 	u64 message_handle;	/* Identifies the particular message instance,
@@ -216,8 +220,8 @@ struct controlvm_message_header  {
 };
 
 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 */
+	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 */
@@ -228,11 +232,10 @@ struct controlvm_packet_device_create  {
 };	/* for CONTROLVM_DEVICE_CREATE */
 
 struct controlvm_packet_device_configure  {
-	u32 bus_no;	      /**< bus # (0..n-1) from the msg
+	u32 bus_no;	      /* bus # (0..n-1) from the msg
 			       * receiver's perspective */
-
-	    /* Control uses header SegmentIndex field to access bus number... */
-	u32 dev_no;	      /**< bus-relative (0..n-1) device number */
+	/* Control uses header SegmentIndex field to access bus number... */
+	u32 dev_no;	      /* bus-relative (0..n-1) device number */
 } ;	/* for CONTROLVM_DEVICE_CONFIGURE */
 
 struct controlvm_message_device_create {
@@ -342,77 +345,48 @@ struct controlvm_message {
 	struct controlvm_message_packet cmd;
 };
 
-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 */
-};
-
-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;
-};
-
 struct spar_controlvm_channel_protocol {
-	 struct channel_header header;
-	 GUEST_PHYSICAL_ADDRESS gp_controlvm;	/* guest physical address of
-						 * this channel */
-	 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 gp_boot_romdisk;/* guest phys addr of (read
-						 * only) Boot ROM disk */
-	 GUEST_PHYSICAL_ADDRESS gp_boot_ramdisk;/* guest phys addr of writable
-						 * Boot RAM disk */
-	 GUEST_PHYSICAL_ADDRESS gp_acpi_table;	/* guest phys addr of acpi
-						 * table */
-	 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 gp_nvram;	/* guest phys addr of NVRAM
-						 * channel */
-	 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
+	struct channel_header header;
+	u64 gp_controlvm;	/* guest phys addr of this channel */
+	u64 gp_partition_tables;/* guest phys addr of partition tables */
+	u64 gp_diag_guest;	/* guest phys addr of diagnostic channel */
+	u64 gp_boot_romdisk;/* guest phys addr of (read* only) Boot ROM disk */
+	u64 gp_boot_ramdisk;/* guest phys addr of writable Boot RAM disk */
+	u64 gp_acpi_table;	/* guest phys addr of acpi table */
+	u64 gp_control_channel;/* guest phys addr of control channel */
+	u64 gp_diag_romdisk;/* guest phys addr of diagnostic ROM disk */
+	u64 gp_nvram;	/* guest phys addr of NVRAM channel */
+	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 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 gp_physical_smbios_table;/* guest phys addr of
-							  * SMBIOS table  */
-	 /* ULTRA_MAX_GUESTS_PER_SERVICE */
-	 struct guest_devices gp_obsolete_guest_devices[16];
-
-	 /* guest physical address of EFI firmware image base  */
-	 GUEST_PHYSICAL_ADDRESS virtual_guest_firmware_image_base;
-
-	 /* guest physical address of EFI firmware entry point  */
-	 GUEST_PHYSICAL_ADDRESS virtual_guest_firmware_entry_point;
-
-	 /* guest EFI firmware image size  */
-	 u64 virtual_guest_firmware_image_size;
-
-	 /* GPA = 1MB where EFI firmware image is copied to  */
-	 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 restore_action;	/* Restore Action field to restore the guest
+	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 */
+	u64 gp_smbios_table;		/* guest phys addr of SMBIOS tables */
+	u64 gp_physical_smbios_table;	/* guest phys addr of SMBIOS table  */
+	/* ULTRA_MAX_GUESTS_PER_SERVICE */
+	char gp_reserved[2688];
+
+	/* guest physical address of EFI firmware image base  */
+	u64 virtual_guest_firmware_image_base;
+
+	/* guest physical address of EFI firmware entry point  */
+	u64 virtual_guest_firmware_entry_point;
+
+	/* guest EFI firmware image size  */
+	u64 virtual_guest_firmware_image_size;
+
+	/* GPA = 1MB where EFI firmware image is copied to  */
+	u64 virtual_guest_firmware_boot_base;
+	u64 virtual_guest_image_base;
+	u64 virtual_guest_image_size;
+	u64 prototype_control_channel_offset;
+	u64 virtual_guest_partition_handle;
+
+	u16 restore_action;	/* Restore Action field to restore the guest
 				 * partition */
 	u16 dump_action;	/* For Windows guests it shows if the visordisk
 				 * is running in dump mode */
@@ -462,7 +436,7 @@ struct spar_controlvm_channel_protocol {
 	 struct controlvm_message saved_crash_msg[CONTROLVM_CRASHMSG_MAX];
 };
 
-/* Offsets for VM channel attributes... */
+/* Offsets for VM channel attributes */
 #define VM_CH_REQ_QUEUE_OFFSET \
 	offsetof(struct spar_controlvm_channel_protocol, request_queue)
 #define VM_CH_RESP_QUEUE_OFFSET \
diff --git a/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h b/drivers/staging/unisys/visorbus/controlvmcompletionstatus.h
index f74f5d8c2820..f74f5d8c2820 100644
--- a/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
+++ b/drivers/staging/unisys/visorbus/controlvmcompletionstatus.h
diff --git a/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h b/drivers/staging/unisys/visorbus/iovmcall_gnuc.h
index 57dd93e0cc83..57dd93e0cc83 100644
--- a/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
+++ b/drivers/staging/unisys/visorbus/iovmcall_gnuc.h
diff --git a/drivers/staging/unisys/visorutil/periodic_work.c b/drivers/staging/unisys/visorbus/periodic_work.c
index abbfb48894f3..5e56088cf855 100644
--- a/drivers/staging/unisys/visorutil/periodic_work.c
+++ b/drivers/staging/unisys/visorbus/periodic_work.c
@@ -18,8 +18,8 @@
 /*
  *  Helper functions to schedule periodic work in Linux kernel mode.
  */
+#include <linux/sched.h>
 
-#include "timskmod.h"
 #include "periodic_work.h"
 
 #define MYDRVNAME "periodic_work"
@@ -29,8 +29,8 @@ struct periodic_work {
 	struct delayed_work work;
 	void (*workfunc)(void *);
 	void *workfuncarg;
-	BOOL is_scheduled;
-	BOOL want_to_stop;
+	bool is_scheduled;
+	bool want_to_stop;
 	ulong jiffy_interval;
 	struct workqueue_struct *workqueue;
 	const char *devnam;
@@ -74,64 +74,64 @@ 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
+ *  If this function returns false, there was a failure and the
  *  periodic work is no longer scheduled
  */
-BOOL visor_periodic_work_nextperiod(struct periodic_work *pw)
+bool visor_periodic_work_nextperiod(struct periodic_work *pw)
 {
-	BOOL rc = FALSE;
+	bool rc = false;
 
 	write_lock(&pw->lock);
 	if (pw->want_to_stop) {
-		pw->is_scheduled = FALSE;
-		pw->want_to_stop = FALSE;
-		rc = TRUE;  /* yes, TRUE; see visor_periodic_work_stop() */
+		pw->is_scheduled = false;
+		pw->want_to_stop = false;
+		rc = true;  /* yes, true; see visor_periodic_work_stop() */
 		goto unlock;
 	} else if (queue_delayed_work(pw->workqueue, &pw->work,
 				      pw->jiffy_interval) < 0) {
-		pw->is_scheduled = FALSE;
-		rc = FALSE;
+		pw->is_scheduled = false;
+		rc = false;
 		goto unlock;
 	}
-	rc = TRUE;
+	rc = true;
 unlock:
 	write_unlock(&pw->lock);
 	return rc;
 }
 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
+/** 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).
  */
-BOOL visor_periodic_work_start(struct periodic_work *pw)
+bool visor_periodic_work_start(struct periodic_work *pw)
 {
-	BOOL rc = FALSE;
+	bool rc = false;
 
 	write_lock(&pw->lock);
 	if (pw->is_scheduled) {
-		rc = FALSE;
+		rc = false;
 		goto unlock;
 	}
 	if (pw->want_to_stop) {
-		rc = FALSE;
+		rc = false;
 		goto unlock;
 	}
 	INIT_DELAYED_WORK(&pw->work, &periodic_work_func);
 	if (queue_delayed_work(pw->workqueue, &pw->work,
 			       pw->jiffy_interval) < 0) {
-		rc = FALSE;
+		rc = false;
 		goto unlock;
 	}
-	pw->is_scheduled = TRUE;
-	rc = TRUE;
+	pw->is_scheduled = true;
+	rc = true;
 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
+/** This function returns true iff your call actually stopped the periodic
  *  work.
  *
  *  -- PAY ATTENTION... this is important --
@@ -165,20 +165,20 @@ EXPORT_SYMBOL_GPL(visor_periodic_work_start);
  *     this deadlock, you will get hung up in an infinite loop saying
  *     "waiting for delayed work...".
  */
-BOOL visor_periodic_work_stop(struct periodic_work *pw)
+bool visor_periodic_work_stop(struct periodic_work *pw)
 {
-	BOOL stopped_something = FALSE;
+	bool stopped_something = false;
 
 	write_lock(&pw->lock);
 	stopped_something = pw->is_scheduled && (!pw->want_to_stop);
 	while (pw->is_scheduled) {
-		pw->want_to_stop = TRUE;
+		pw->want_to_stop = true;
 		if (cancel_delayed_work(&pw->work)) {
 			/* We get here if the delayed work was pending as
 			 * delayed work, but was NOT run.
 			 */
 			WARN_ON(!pw->is_scheduled);
-			pw->is_scheduled = FALSE;
+			pw->is_scheduled = false;
 		} else {
 			/* If we get here, either the delayed work:
 			 * - was run, OR,
@@ -192,10 +192,10 @@ BOOL visor_periodic_work_stop(struct periodic_work *pw)
 		}
 		if (pw->is_scheduled) {
 			write_unlock(&pw->lock);
-			SLEEPJIFFIES(10);
+			schedule_timeout_interruptible(msecs_to_jiffies(10));
 			write_lock(&pw->lock);
 		} else {
-			pw->want_to_stop = FALSE;
+			pw->want_to_stop = false;
 		}
 	}
 	write_unlock(&pw->lock);
diff --git a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h b/drivers/staging/unisys/visorbus/vbuschannel.h
index 2c42ce16e0cf..5ed83a3f1428 100644
--- a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
+++ b/drivers/staging/unisys/visorbus/vbuschannel.h
@@ -54,7 +54,7 @@ static const uuid_le spar_vbus_channel_protocol_uuid =
 #define SPAR_VBUS_CHANNEL_OK_SERVER(actual_bytes)    \
 	(spar_check_channel_server(spar_vbus_channel_protocol_uuid,	\
 				   "vbus",				\
-				   sizeof(struct ultra_vbus_channel_protocol),\
+				   sizeof(struct spar_vbus_channel_protocol),\
 				   actual_bytes))
 
 #pragma pack(push, 1)		/* both GCC and VC now allow this pragma */
diff --git a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h b/drivers/staging/unisys/visorbus/vbusdeviceinfo.h
index 9b6d3e69355c..9b6d3e69355c 100644
--- a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
+++ b/drivers/staging/unisys/visorbus/vbusdeviceinfo.h
diff --git a/drivers/staging/unisys/visorbus/visorbus_main.c b/drivers/staging/unisys/visorbus/visorbus_main.c
new file mode 100644
index 000000000000..6db47196c189
--- /dev/null
+++ b/drivers/staging/unisys/visorbus/visorbus_main.c
@@ -0,0 +1,1518 @@
+/* visorbus_main.c
+ *
+ * Copyright � 2010 - 2013 UNISYS CORPORATION
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or (at
+ * your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ */
+
+#include <linux/uuid.h>
+
+#include "visorbus.h"
+#include "visorbus_private.h"
+#include "version.h"
+#include "periodic_work.h"
+#include "vbuschannel.h"
+#include "guestlinuxdebug.h"
+#include "vmcallinterface.h"
+
+#define MYDRVNAME "visorbus"
+
+/* module parameters */
+static int visorbus_debug;
+static int visorbus_forcematch;
+static int visorbus_forcenomatch;
+static int visorbus_debugref;
+#define SERIALLOOPBACKCHANADDR (100 * 1024 * 1024)
+
+#define CURRENT_FILE_PC VISOR_BUS_PC_visorbus_main_c
+#define POLLJIFFIES_TESTWORK         100
+#define POLLJIFFIES_NORMALCHANNEL     10
+
+static int visorbus_uevent(struct device *xdev, struct kobj_uevent_env *env);
+static int visorbus_match(struct device *xdev, struct device_driver *xdrv);
+static void fix_vbus_dev_info(struct visor_device *visordev);
+
+/*  BUS type attributes
+ *
+ *  define & implement display of bus attributes under
+ *  /sys/bus/visorbus.
+ *
+ */
+
+static ssize_t version_show(struct bus_type *bus, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", VERSION);
+}
+
+static BUS_ATTR_RO(version);
+
+static struct attribute *visorbus_bus_attrs[] = {
+	&bus_attr_version.attr,
+	NULL,
+};
+
+static const struct attribute_group visorbus_bus_group = {
+	.attrs = visorbus_bus_attrs,
+};
+
+static const struct attribute_group *visorbus_bus_groups[] = {
+	&visorbus_bus_group,
+	NULL,
+};
+
+/** This describes the TYPE of bus.
+ *  (Don't confuse this with an INSTANCE of the bus.)
+ */
+struct bus_type visorbus_type = {
+	.name = "visorbus",
+	.match = visorbus_match,
+	.uevent = visorbus_uevent,
+	.bus_groups = visorbus_bus_groups,
+};
+
+static struct delayed_work periodic_work;
+
+/* YES, we need 2 workqueues.
+ * The reason is, workitems on the test queue may need to cancel
+ * workitems on the other queue.  You will be in for trouble if you try to
+ * do this with workitems queued on the same workqueue.
+ */
+static struct workqueue_struct *periodic_test_workqueue;
+static struct workqueue_struct *periodic_dev_workqueue;
+static long long bus_count;	/** number of bus instances */
+					/** ever-increasing */
+
+static void chipset_bus_create(struct visor_device *bus_info);
+static void chipset_bus_destroy(struct visor_device *bus_info);
+static void chipset_device_create(struct visor_device *dev_info);
+static void chipset_device_destroy(struct visor_device *dev_info);
+static void chipset_device_pause(struct visor_device *dev_info);
+static void chipset_device_resume(struct visor_device *dev_info);
+
+/** These functions are implemented herein, and are called by the chipset
+ *  driver to notify us about specific events.
+ */
+static struct visorchipset_busdev_notifiers chipset_notifiers = {
+	.bus_create = chipset_bus_create,
+	.bus_destroy = chipset_bus_destroy,
+	.device_create = chipset_device_create,
+	.device_destroy = chipset_device_destroy,
+	.device_pause = chipset_device_pause,
+	.device_resume = chipset_device_resume,
+};
+
+/** These functions are implemented in the chipset driver, and we call them
+ *  herein when we want to acknowledge a specific event.
+ */
+static struct visorchipset_busdev_responders chipset_responders;
+
+/* filled in with info about parent chipset driver when we register with it */
+static struct ultra_vbus_deviceinfo chipset_driverinfo;
+/* filled in with info about this driver, wrt it servicing client busses */
+static struct ultra_vbus_deviceinfo clientbus_driverinfo;
+
+/** list of visor_device structs, linked via .list_all */
+static LIST_HEAD(list_all_bus_instances);
+/** list of visor_device structs, linked via .list_all */
+static LIST_HEAD(list_all_device_instances);
+
+static int
+visorbus_uevent(struct device *xdev, struct kobj_uevent_env *env)
+{
+	if (add_uevent_var(env, "VERSION=%s", VERSION))
+		return -ENOMEM;
+	return 0;
+}
+
+/* This is called automatically upon adding a visor_device (device_add), or
+ * adding a visor_driver (visorbus_register_visor_driver), and returns 1 iff the
+ * provided driver can control the specified device.
+ */
+static int
+visorbus_match(struct device *xdev, struct device_driver *xdrv)
+{
+	uuid_le channel_type;
+	int rc = 0;
+	int i;
+	struct visor_device *dev;
+	struct visor_driver *drv;
+
+	dev = to_visor_device(xdev);
+	drv = to_visor_driver(xdrv);
+	channel_type = visorchannel_get_uuid(dev->visorchannel);
+	if (visorbus_forcematch) {
+		rc = 1;
+		goto away;
+	}
+	if (visorbus_forcenomatch)
+		goto away;
+
+	if (!drv->channel_types)
+		goto away;
+	for (i = 0;
+	     (uuid_le_cmp(drv->channel_types[i].guid, NULL_UUID_LE) != 0) ||
+	     (drv->channel_types[i].name);
+	     i++)
+		if (uuid_le_cmp(drv->channel_types[i].guid,
+				channel_type) == 0) {
+			rc = i + 1;
+			goto away;
+		}
+away:
+	return rc;
+}
+
+/** This is called when device_unregister() is called for the bus device
+ *  instance, after all other tasks involved with destroying the device
+ *  are complete.
+ */
+static void
+visorbus_release_busdevice(struct device *xdev)
+{
+	struct visor_device *dev = dev_get_drvdata(xdev);
+
+	dev_set_drvdata(xdev, NULL);
+	kfree(dev);
+}
+
+/** This is called when device_unregister() is called for each child
+ *  device instance.
+ */
+static void
+visorbus_release_device(struct device *xdev)
+{
+	struct visor_device *dev = to_visor_device(xdev);
+
+	if (dev->periodic_work) {
+		visor_periodic_work_destroy(dev->periodic_work);
+		dev->periodic_work = NULL;
+	}
+	if (dev->visorchannel) {
+		visorchannel_destroy(dev->visorchannel);
+		dev->visorchannel = NULL;
+	}
+	kfree(dev);
+}
+
+/* Implement publishing of device node attributes under:
+ *
+ *     /sys/bus/visorbus<x>/dev<y>/devmajorminor
+ *
+ */
+
+#define to_devmajorminor_attr(_attr) \
+	container_of(_attr, struct devmajorminor_attribute, attr)
+#define to_visor_device_from_kobjdevmajorminor(obj) \
+	container_of(obj, struct visor_device, kobjdevmajorminor)
+
+struct devmajorminor_attribute {
+	struct attribute attr;
+	int slot;
+	 ssize_t (*show)(struct visor_device *, int slot, char *buf);
+	 ssize_t (*store)(struct visor_device *, int slot, const char *buf,
+			  size_t count);
+};
+
+static ssize_t DEVMAJORMINOR_ATTR(struct visor_device *dev, int slot, char *buf)
+{
+	int maxdevnodes = ARRAY_SIZE(dev->devnodes) / sizeof(dev->devnodes[0]);
+
+	if (slot < 0 || slot >= maxdevnodes)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%d:%d\n",
+			dev->devnodes[slot].major, dev->devnodes[slot].minor);
+}
+
+static ssize_t
+devmajorminor_attr_show(struct kobject *kobj, struct attribute *attr, char *buf)
+{
+	struct devmajorminor_attribute *devmajorminor_attr =
+	    to_devmajorminor_attr(attr);
+	struct visor_device *dev = to_visor_device_from_kobjdevmajorminor(kobj);
+	ssize_t ret = 0;
+
+	if (devmajorminor_attr->show)
+		ret = devmajorminor_attr->show(dev,
+					       devmajorminor_attr->slot, buf);
+	return ret;
+}
+
+static ssize_t
+devmajorminor_attr_store(struct kobject *kobj,
+			 struct attribute *attr, const char *buf, size_t count)
+{
+	struct devmajorminor_attribute *devmajorminor_attr =
+	    to_devmajorminor_attr(attr);
+	struct visor_device *dev = to_visor_device_from_kobjdevmajorminor(kobj);
+	ssize_t ret = 0;
+
+	if (devmajorminor_attr->store)
+		ret = devmajorminor_attr->store(dev,
+						devmajorminor_attr->slot,
+						buf, count);
+	return ret;
+}
+
+static int register_devmajorminor_attributes(struct visor_device *dev);
+
+static int
+devmajorminor_create_file(struct visor_device *dev, const char *name,
+			  int major, int minor)
+{
+	int maxdevnodes = ARRAY_SIZE(dev->devnodes) / sizeof(dev->devnodes[0]);
+	struct devmajorminor_attribute *myattr = NULL;
+	int x = -1, rc = 0, slot = -1;
+
+	register_devmajorminor_attributes(dev);
+	for (slot = 0; slot < maxdevnodes; slot++)
+		if (!dev->devnodes[slot].attr)
+			break;
+	if (slot == maxdevnodes) {
+		rc = -ENOMEM;
+		goto away;
+	}
+	myattr = kmalloc(sizeof(*myattr), GFP_KERNEL);
+	if (!myattr) {
+		rc = -ENOMEM;
+		goto away;
+	}
+	memset(myattr, 0, sizeof(struct devmajorminor_attribute));
+	myattr->show = DEVMAJORMINOR_ATTR;
+	myattr->store = NULL;
+	myattr->slot = slot;
+	myattr->attr.name = name;
+	myattr->attr.mode = S_IRUGO;
+	dev->devnodes[slot].attr = myattr;
+	dev->devnodes[slot].major = major;
+	dev->devnodes[slot].minor = minor;
+	x = sysfs_create_file(&dev->kobjdevmajorminor, &myattr->attr);
+	if (x < 0) {
+		rc = x;
+		goto away;
+	}
+	kobject_uevent(&dev->device.kobj, KOBJ_ONLINE);
+away:
+	if (rc < 0) {
+		kfree(myattr);
+		myattr = NULL;
+		dev->devnodes[slot].attr = NULL;
+	}
+	return rc;
+}
+
+static void
+devmajorminor_remove_file(struct visor_device *dev, int slot)
+{
+	int maxdevnodes = ARRAY_SIZE(dev->devnodes) / sizeof(dev->devnodes[0]);
+	struct devmajorminor_attribute *myattr = NULL;
+
+	if (slot < 0 || slot >= maxdevnodes)
+		return;
+	myattr = (struct devmajorminor_attribute *)(dev->devnodes[slot].attr);
+	if (!myattr)
+		return;
+	sysfs_remove_file(&dev->kobjdevmajorminor, &myattr->attr);
+	kobject_uevent(&dev->device.kobj, KOBJ_OFFLINE);
+	dev->devnodes[slot].attr = NULL;
+	kfree(myattr);
+}
+
+static void
+devmajorminor_remove_all_files(struct visor_device *dev)
+{
+	int i = 0;
+	int maxdevnodes = ARRAY_SIZE(dev->devnodes) / sizeof(dev->devnodes[0]);
+
+	for (i = 0; i < maxdevnodes; i++)
+		devmajorminor_remove_file(dev, i);
+}
+
+static const struct sysfs_ops devmajorminor_sysfs_ops = {
+	.show = devmajorminor_attr_show,
+	.store = devmajorminor_attr_store,
+};
+
+static struct kobj_type devmajorminor_kobj_type = {
+	.sysfs_ops = &devmajorminor_sysfs_ops
+};
+
+static int
+register_devmajorminor_attributes(struct visor_device *dev)
+{
+	int rc = 0, x = 0;
+
+	if (dev->kobjdevmajorminor.parent)
+		goto away;	/* already registered */
+	x = kobject_init_and_add(&dev->kobjdevmajorminor,
+				 &devmajorminor_kobj_type, &dev->device.kobj,
+				 "devmajorminor");
+	if (x < 0) {
+		rc = x;
+		goto away;
+	}
+
+	kobject_uevent(&dev->kobjdevmajorminor, KOBJ_ADD);
+
+away:
+	return rc;
+}
+
+static void
+unregister_devmajorminor_attributes(struct visor_device *dev)
+{
+	if (!dev->kobjdevmajorminor.parent)
+		return;		/* already unregistered */
+	devmajorminor_remove_all_files(dev);
+
+	kobject_del(&dev->kobjdevmajorminor);
+	kobject_put(&dev->kobjdevmajorminor);
+	dev->kobjdevmajorminor.parent = NULL;
+}
+
+/* begin implementation of specific channel attributes to appear under
+* /sys/bus/visorbus<x>/dev<y>/channel
+*/
+static ssize_t physaddr_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct visor_device *vdev = to_visor_device(dev);
+
+	if (!vdev->visorchannel)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "0x%Lx\n",
+			visorchannel_get_physaddr(vdev->visorchannel));
+}
+
+static ssize_t nbytes_show(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct visor_device *vdev = to_visor_device(dev);
+
+	if (!vdev->visorchannel)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "0x%lx\n",
+			visorchannel_get_nbytes(vdev->visorchannel));
+}
+
+static ssize_t clientpartition_show(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	struct visor_device *vdev = to_visor_device(dev);
+
+	if (!vdev->visorchannel)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "0x%Lx\n",
+			visorchannel_get_clientpartition(vdev->visorchannel));
+}
+
+static ssize_t typeguid_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct visor_device *vdev = to_visor_device(dev);
+	char s[99];
+
+	if (!vdev->visorchannel)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			visorchannel_id(vdev->visorchannel, s));
+}
+
+static ssize_t zoneguid_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct visor_device *vdev = to_visor_device(dev);
+	char s[99];
+
+	if (!vdev->visorchannel)
+		return 0;
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			visorchannel_zoneid(vdev->visorchannel, s));
+}
+
+static ssize_t typename_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct visor_device *vdev = to_visor_device(dev);
+	int i = 0;
+	struct bus_type *xbus = dev->bus;
+	struct device_driver *xdrv = dev->driver;
+	struct visor_driver *drv = NULL;
+
+	if (!vdev->visorchannel || !xbus || !xdrv)
+		return 0;
+	i = xbus->match(dev, xdrv);
+	if (!i)
+		return 0;
+	drv = to_visor_driver(xdrv);
+	return snprintf(buf, PAGE_SIZE, "%s\n", drv->channel_types[i - 1].name);
+}
+
+static DEVICE_ATTR_RO(physaddr);
+static DEVICE_ATTR_RO(nbytes);
+static DEVICE_ATTR_RO(clientpartition);
+static DEVICE_ATTR_RO(typeguid);
+static DEVICE_ATTR_RO(zoneguid);
+static DEVICE_ATTR_RO(typename);
+
+static struct attribute *channel_attrs[] = {
+		&dev_attr_physaddr.attr,
+		&dev_attr_nbytes.attr,
+		&dev_attr_clientpartition.attr,
+		&dev_attr_typeguid.attr,
+		&dev_attr_zoneguid.attr,
+		&dev_attr_typename.attr,
+};
+
+static struct attribute_group channel_attr_grp = {
+		.name = "channel",
+		.attrs = channel_attrs,
+};
+
+static const struct attribute_group *visorbus_dev_groups[] = {
+		&channel_attr_grp,
+		NULL
+};
+
+/* end implementation of specific channel attributes */
+
+/*  BUS instance attributes
+ *
+ *  define & implement display of bus attributes under
+ *  /sys/bus/visorbus/busses/visorbus<n>.
+ *
+ *  This is a bit hoaky because the kernel does not yet have the infrastructure
+ *  to separate bus INSTANCE attributes from bus TYPE attributes...
+ *  so we roll our own.  See businst.c / businst.h.
+ *
+ */
+
+static ssize_t partition_handle_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf) {
+	struct visor_device *vdev = to_visor_device(dev);
+	u64 handle = visorchannel_get_clientpartition(vdev->visorchannel);
+
+	return snprintf(buf, PAGE_SIZE, "0x%Lx\n", handle);
+}
+
+static ssize_t partition_guid_show(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf) {
+	struct visor_device *vdev = to_visor_device(dev);
+
+	return snprintf(buf, PAGE_SIZE, "{%pUb}\n", &vdev->partition_uuid);
+}
+
+static ssize_t partition_name_show(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf) {
+	struct visor_device *vdev = to_visor_device(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", vdev->name);
+}
+
+static ssize_t channel_addr_show(struct device *dev,
+				 struct device_attribute *attr,
+				 char *buf) {
+	struct visor_device *vdev = to_visor_device(dev);
+	u64 addr = visorchannel_get_physaddr(vdev->visorchannel);
+
+	return snprintf(buf, PAGE_SIZE, "0x%Lx\n", addr);
+}
+
+static ssize_t channel_bytes_show(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf) {
+	struct visor_device *vdev = to_visor_device(dev);
+	u64 nbytes = visorchannel_get_nbytes(vdev->visorchannel);
+
+	return snprintf(buf, PAGE_SIZE, "0x%Lx\n", nbytes);
+}
+
+static ssize_t channel_id_show(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf) {
+	struct visor_device *vdev = to_visor_device(dev);
+	int len = 0;
+
+	if (vdev->visorchannel) {
+		visorchannel_id(vdev->visorchannel, buf);
+		len = strlen(buf);
+		buf[len++] = '\n';
+	}
+	return len;
+}
+
+static ssize_t client_bus_info_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf) {
+	struct visor_device *vdev = to_visor_device(dev);
+	struct visorchannel *channel = vdev->visorchannel;
+
+	int i, x, remain = PAGE_SIZE;
+	unsigned long off;
+	char *p = buf;
+	u8 *partition_name;
+	struct ultra_vbus_deviceinfo dev_info;
+
+	partition_name = "";
+	if (channel) {
+		if (vdev->name)
+			partition_name = vdev->name;
+		x = snprintf(p, remain,
+			     "Client device / client driver info for %s partition (vbus #%d):\n",
+			     partition_name, vdev->chipset_dev_no);
+		p += x;
+		remain -= x;
+		x = visorchannel_read(channel,
+				      offsetof(struct
+					       spar_vbus_channel_protocol,
+					       chp_info),
+				      &dev_info, sizeof(dev_info));
+		if (x >= 0) {
+			x = vbuschannel_devinfo_to_string(&dev_info, p,
+							  remain, -1);
+			p += x;
+			remain -= x;
+		}
+		x = visorchannel_read(channel,
+				      offsetof(struct
+					       spar_vbus_channel_protocol,
+					       bus_info),
+				      &dev_info, sizeof(dev_info));
+		if (x >= 0) {
+			x = vbuschannel_devinfo_to_string(&dev_info, p,
+							  remain, -1);
+			p += x;
+			remain -= x;
+		}
+		off = offsetof(struct spar_vbus_channel_protocol, dev_info);
+		i = 0;
+		while (off + sizeof(dev_info) <=
+		       visorchannel_get_nbytes(channel)) {
+			x = visorchannel_read(channel,
+					      off, &dev_info, sizeof(dev_info));
+			if (x >= 0) {
+				x = vbuschannel_devinfo_to_string
+				    (&dev_info, p, remain, i);
+				p += x;
+				remain -= x;
+			}
+			off += sizeof(dev_info);
+			i++;
+		}
+	}
+	return PAGE_SIZE - remain;
+}
+
+static DEVICE_ATTR_RO(partition_handle);
+static DEVICE_ATTR_RO(partition_guid);
+static DEVICE_ATTR_RO(partition_name);
+static DEVICE_ATTR_RO(channel_addr);
+static DEVICE_ATTR_RO(channel_bytes);
+static DEVICE_ATTR_RO(channel_id);
+static DEVICE_ATTR_RO(client_bus_info);
+
+static struct attribute *dev_attrs[] = {
+		&dev_attr_partition_handle.attr,
+		&dev_attr_partition_guid.attr,
+		&dev_attr_partition_name.attr,
+		&dev_attr_channel_addr.attr,
+		&dev_attr_channel_bytes.attr,
+		&dev_attr_channel_id.attr,
+		&dev_attr_client_bus_info.attr,
+		NULL
+};
+
+static struct attribute_group dev_attr_grp = {
+		.attrs = dev_attrs,
+};
+
+static const struct attribute_group *visorbus_groups[] = {
+		&dev_attr_grp,
+		NULL
+};
+
+/*  DRIVER attributes
+ *
+ *  define & implement display of driver attributes under
+ *  /sys/bus/visorbus/drivers/<drivername>.
+ *
+ */
+
+static ssize_t
+DRIVER_ATTR_version(struct device_driver *xdrv, char *buf)
+{
+	struct visor_driver *drv = to_visor_driver(xdrv);
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", drv->version);
+}
+
+static int
+register_driver_attributes(struct visor_driver *drv)
+{
+	int rc;
+	struct driver_attribute version =
+	    __ATTR(version, S_IRUGO, DRIVER_ATTR_version, NULL);
+	drv->version_attr = version;
+	rc = driver_create_file(&drv->driver, &drv->version_attr);
+	return rc;
+}
+
+static void
+unregister_driver_attributes(struct visor_driver *drv)
+{
+	driver_remove_file(&drv->driver, &drv->version_attr);
+}
+
+static void
+dev_periodic_work(void *xdev)
+{
+	struct visor_device *dev = (struct visor_device *)xdev;
+	struct visor_driver *drv = to_visor_driver(dev->device.driver);
+
+	down(&dev->visordriver_callback_lock);
+	if (drv->channel_interrupt)
+		drv->channel_interrupt(dev);
+	up(&dev->visordriver_callback_lock);
+	if (!visor_periodic_work_nextperiod(dev->periodic_work))
+		put_device(&dev->device);
+}
+
+static void
+dev_start_periodic_work(struct visor_device *dev)
+{
+	if (dev->being_removed)
+		return;
+	/* now up by at least 2 */
+	get_device(&dev->device);
+	if (!visor_periodic_work_start(dev->periodic_work))
+		put_device(&dev->device);
+}
+
+static void
+dev_stop_periodic_work(struct visor_device *dev)
+{
+	if (visor_periodic_work_stop(dev->periodic_work))
+		put_device(&dev->device);
+}
+
+/** This is called automatically upon adding a visor_device (device_add), or
+ *  adding a visor_driver (visorbus_register_visor_driver), but only after
+ *  visorbus_match has returned 1 to indicate a successful match between
+ *  driver and device.
+ */
+static int
+visordriver_probe_device(struct device *xdev)
+{
+	int rc;
+	struct visor_driver *drv;
+	struct visor_device *dev;
+
+	drv = to_visor_driver(xdev->driver);
+	dev = to_visor_device(xdev);
+	down(&dev->visordriver_callback_lock);
+	dev->being_removed = false;
+	/*
+	 * ensure that the dev->being_removed flag is cleared before
+	 * we start the probe
+	 */
+	wmb();
+	get_device(&dev->device);
+	if (!drv->probe) {
+		up(&dev->visordriver_callback_lock);
+		rc = -1;
+		goto away;
+	}
+	rc = drv->probe(dev);
+	if (rc < 0)
+		goto away;
+
+	fix_vbus_dev_info(dev);
+	up(&dev->visordriver_callback_lock);
+	rc = 0;
+away:
+	if (rc != 0)
+		put_device(&dev->device);
+	return rc;
+}
+
+/** This is called when device_unregister() is called for each child device
+ *  instance, to notify the appropriate visorbus_driver that the device is
+ *  going away, and to decrease the reference count of the device.
+ */
+static int
+visordriver_remove_device(struct device *xdev)
+{
+	struct visor_device *dev;
+	struct visor_driver *drv;
+
+	dev = to_visor_device(xdev);
+	drv = to_visor_driver(xdev->driver);
+	down(&dev->visordriver_callback_lock);
+	dev->being_removed = true;
+	/*
+	 * ensure that the dev->being_removed flag is set before we start the
+	 * actual removal
+	 */
+	wmb();
+	if (drv) {
+		if (drv->remove)
+			drv->remove(dev);
+	}
+	up(&dev->visordriver_callback_lock);
+	dev_stop_periodic_work(dev);
+	devmajorminor_remove_all_files(dev);
+
+	put_device(&dev->device);
+
+	return 0;
+}
+
+/** A particular type of visor driver calls this function to register
+ *  the driver.  The caller MUST fill in the following fields within the
+ *  #drv structure:
+ *      name, version, owner, channel_types, probe, remove
+ *
+ *  Here's how the whole Linux bus / driver / device model works.
+ *
+ *  At system start-up, the visorbus kernel module is loaded, which registers
+ *  visorbus_type as a bus type, using bus_register().
+ *
+ *  All kernel modules that support particular device types on a
+ *  visorbus bus are loaded.  Each of these kernel modules calls
+ *  visorbus_register_visor_driver() in their init functions, passing a
+ *  visor_driver struct.  visorbus_register_visor_driver() in turn calls
+ *  register_driver(&visor_driver.driver).  This .driver member is
+ *  initialized with generic methods (like probe), whose sole responsibility
+ *  is to act as a broker for the real methods, which are within the
+ *  visor_driver struct.  (This is the way the subclass behavior is
+ *  implemented, since visor_driver is essentially a subclass of the
+ *  generic driver.)  Whenever a driver_register() happens, core bus code in
+ *  the kernel does (see device_attach() in drivers/base/dd.c):
+ *
+ *      for each dev associated with the bus (the bus that driver is on) that
+ *      does not yet have a driver
+ *          if bus.match(dev,newdriver) == yes_matched  ** .match specified
+ *                                                 ** during bus_register().
+ *              newdriver.probe(dev)  ** for visor drivers, this will call
+ *                    ** the generic driver.probe implemented in visorbus.c,
+ *                    ** which in turn calls the probe specified within the
+ *                    ** struct visor_driver (which was specified by the
+ *                    ** actual device driver as part of
+ *                    ** visorbus_register_visor_driver()).
+ *
+ *  The above dance also happens when a new device appears.
+ *  So the question is, how are devices created within the system?
+ *  Basically, just call device_add(dev).  See pci_bus_add_devices().
+ *  pci_scan_device() shows an example of how to build a device struct.  It
+ *  returns the newly-created struct to pci_scan_single_device(), who adds it
+ *  to the list of devices at PCIBUS.devices.  That list of devices is what
+ *  is traversed by pci_bus_add_devices().
+ *
+ */
+int visorbus_register_visor_driver(struct visor_driver *drv)
+{
+	int rc = 0;
+
+	drv->driver.name = drv->name;
+	drv->driver.bus = &visorbus_type;
+	drv->driver.probe = visordriver_probe_device;
+	drv->driver.remove = visordriver_remove_device;
+	drv->driver.owner = drv->owner;
+
+	/* driver_register does this:
+	 *   bus_add_driver(drv)
+	 *   ->if (drv.bus)  ** (bus_type) **
+	 *       driver_attach(drv)
+	 *         for each dev with bus type of drv.bus
+	 *           if (!dev.drv)  ** no driver assigned yet **
+	 *             if (bus.match(dev,drv))  [visorbus_match]
+	 *               dev.drv = drv
+	 *               if (!drv.probe(dev))   [visordriver_probe_device]
+	 *                 dev.drv = NULL
+	 */
+
+	rc = driver_register(&drv->driver);
+	if (rc < 0)
+		return rc;
+	rc = register_driver_attributes(drv);
+	return rc;
+}
+EXPORT_SYMBOL_GPL(visorbus_register_visor_driver);
+
+/** A particular type of visor driver calls this function to unregister
+ *  the driver, i.e., within its module_exit function.
+ */
+void
+visorbus_unregister_visor_driver(struct visor_driver *drv)
+{
+	unregister_driver_attributes(drv);
+	driver_unregister(&drv->driver);
+}
+EXPORT_SYMBOL_GPL(visorbus_unregister_visor_driver);
+
+int
+visorbus_read_channel(struct visor_device *dev, unsigned long offset,
+		      void *dest, unsigned long nbytes)
+{
+	return visorchannel_read(dev->visorchannel, offset, dest, nbytes);
+}
+EXPORT_SYMBOL_GPL(visorbus_read_channel);
+
+int
+visorbus_write_channel(struct visor_device *dev, unsigned long offset,
+		       void *src, unsigned long nbytes)
+{
+	return visorchannel_write(dev->visorchannel, offset, src, nbytes);
+}
+EXPORT_SYMBOL_GPL(visorbus_write_channel);
+
+int
+visorbus_clear_channel(struct visor_device *dev, unsigned long offset, u8 ch,
+		       unsigned long nbytes)
+{
+	return visorchannel_clear(dev->visorchannel, offset, ch, nbytes);
+}
+EXPORT_SYMBOL_GPL(visorbus_clear_channel);
+
+int
+visorbus_registerdevnode(struct visor_device *dev,
+			 const char *name, int major, int minor)
+{
+	return devmajorminor_create_file(dev, name, major, minor);
+}
+EXPORT_SYMBOL_GPL(visorbus_registerdevnode);
+
+/** We don't really have a real interrupt, so for now we just call the
+ *  interrupt function periodically...
+ */
+void
+visorbus_enable_channel_interrupts(struct visor_device *dev)
+{
+	dev_start_periodic_work(dev);
+}
+EXPORT_SYMBOL_GPL(visorbus_enable_channel_interrupts);
+
+void
+visorbus_disable_channel_interrupts(struct visor_device *dev)
+{
+	dev_stop_periodic_work(dev);
+}
+EXPORT_SYMBOL_GPL(visorbus_disable_channel_interrupts);
+
+/** This is how everything starts from the device end.
+ *  This function is called when a channel first appears via a ControlVM
+ *  message.  In response, this function allocates a visor_device to
+ *  correspond to the new channel, and attempts to connect it the appropriate
+ *  driver.  If the appropriate driver is found, the visor_driver.probe()
+ *  function for that driver will be called, and will be passed the new
+ *  visor_device that we just created.
+ *
+ *  It's ok if the appropriate driver is not yet loaded, because in that case
+ *  the new device struct will just stick around in the bus' list of devices.
+ *  When the appropriate driver calls visorbus_register_visor_driver(), the
+ *  visor_driver.probe() for the new driver will be called with the new
+ *  device.
+ */
+static int
+create_visor_device(struct visor_device *dev)
+{
+	int rc = -1;
+	u32 chipset_bus_no = dev->chipset_bus_no;
+	u32 chipset_dev_no = dev->chipset_dev_no;
+
+	POSTCODE_LINUX_4(DEVICE_CREATE_ENTRY_PC, chipset_dev_no, chipset_bus_no,
+			 POSTCODE_SEVERITY_INFO);
+
+	sema_init(&dev->visordriver_callback_lock, 1);	/* unlocked */
+	dev->device.bus = &visorbus_type;
+	dev->device.groups = visorbus_dev_groups;
+	device_initialize(&dev->device);
+	dev->device.release = visorbus_release_device;
+	/* keep a reference just for us (now 2) */
+	get_device(&dev->device);
+	dev->periodic_work =
+		visor_periodic_work_create(POLLJIFFIES_NORMALCHANNEL,
+					   periodic_dev_workqueue,
+					   dev_periodic_work,
+					   dev, dev_name(&dev->device));
+	if (!dev->periodic_work) {
+		POSTCODE_LINUX_3(DEVICE_CREATE_FAILURE_PC, chipset_dev_no,
+				 DIAG_SEVERITY_ERR);
+		goto away;
+	}
+
+	/* bus_id must be a unique name with respect to this bus TYPE
+	 * (NOT bus instance).  That's why we need to include the bus
+	 * number within the name.
+	 */
+	dev_set_name(&dev->device, "vbus%u:dev%u",
+		     chipset_bus_no, chipset_dev_no);
+
+	/*  device_add does this:
+	 *    bus_add_device(dev)
+	 *    ->device_attach(dev)
+	 *      ->for each driver drv registered on the bus that dev is on
+	 *          if (dev.drv)  **  device already has a driver **
+	 *            ** not sure we could ever get here... **
+	 *          else
+	 *            if (bus.match(dev,drv)) [visorbus_match]
+	 *              dev.drv = drv
+	 *              if (!drv.probe(dev))  [visordriver_probe_device]
+	 *                dev.drv = NULL
+	 *
+	 *  Note that device_add does NOT fail if no driver failed to
+	 *  claim the device.  The device will be linked onto
+	 *  bus_type.klist_devices regardless (use bus_for_each_dev).
+	 */
+	rc = device_add(&dev->device);
+	if (rc < 0) {
+		POSTCODE_LINUX_3(DEVICE_ADD_PC, chipset_bus_no,
+				 DIAG_SEVERITY_ERR);
+		goto away;
+	}
+
+	rc = register_devmajorminor_attributes(dev);
+	if (rc < 0) {
+		POSTCODE_LINUX_3(DEVICE_REGISTER_FAILURE_PC, chipset_dev_no,
+				 DIAG_SEVERITY_ERR);
+		goto away_register;
+	}
+
+	list_add_tail(&dev->list_all, &list_all_device_instances);
+	return 0;
+
+away_register:
+	device_unregister(&dev->device);
+away:
+	put_device(&dev->device);
+	return rc;
+}
+
+static void
+remove_visor_device(struct visor_device *dev)
+{
+	list_del(&dev->list_all);
+	unregister_devmajorminor_attributes(dev);
+	put_device(&dev->device);
+	device_unregister(&dev->device);
+}
+
+static int
+get_vbus_header_info(struct visorchannel *chan,
+		     struct spar_vbus_headerinfo *hdr_info)
+{
+	int rc = -1;
+
+	if (!SPAR_VBUS_CHANNEL_OK_CLIENT(visorchannel_get_header(chan)))
+		goto away;
+	if (visorchannel_read(chan, sizeof(struct channel_header), hdr_info,
+			      sizeof(*hdr_info)) < 0) {
+		goto away;
+	}
+	if (hdr_info->struct_bytes < sizeof(struct spar_vbus_headerinfo))
+		goto away;
+	if (hdr_info->device_info_struct_bytes <
+	    sizeof(struct ultra_vbus_deviceinfo)) {
+		goto away;
+	}
+	rc = 0;
+away:
+	return rc;
+}
+
+/* Write the contents of <info> to the struct
+ * spar_vbus_channel_protocol.chp_info. */
+
+static int
+write_vbus_chp_info(struct visorchannel *chan,
+		    struct spar_vbus_headerinfo *hdr_info,
+		    struct ultra_vbus_deviceinfo *info)
+{
+	int off = sizeof(struct channel_header) + hdr_info->chp_info_offset;
+
+	if (hdr_info->chp_info_offset == 0)
+			return -1;
+
+	if (visorchannel_write(chan, off, info, sizeof(*info)) < 0)
+			return -1;
+	return 0;
+}
+
+/* Write the contents of <info> to the struct
+ * spar_vbus_channel_protocol.bus_info. */
+
+static int
+write_vbus_bus_info(struct visorchannel *chan,
+		    struct spar_vbus_headerinfo *hdr_info,
+		    struct ultra_vbus_deviceinfo *info)
+{
+	int off = sizeof(struct channel_header) + hdr_info->bus_info_offset;
+
+	if (hdr_info->bus_info_offset == 0)
+			return -1;
+
+	if (visorchannel_write(chan, off, info, sizeof(*info)) < 0)
+			return -1;
+	return 0;
+}
+
+/* Write the contents of <info> to the
+ * struct spar_vbus_channel_protocol.dev_info[<devix>].
+ */
+static int
+write_vbus_dev_info(struct visorchannel *chan,
+		    struct spar_vbus_headerinfo *hdr_info,
+		    struct ultra_vbus_deviceinfo *info, int devix)
+{
+	int off =
+	    (sizeof(struct channel_header) + hdr_info->dev_info_offset) +
+	    (hdr_info->device_info_struct_bytes * devix);
+
+	if (hdr_info->dev_info_offset == 0)
+			return -1;
+
+	if (visorchannel_write(chan, off, info, sizeof(*info)) < 0)
+			return -1;
+	return 0;
+}
+
+/* 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
+ * instance.
+ */
+static void
+fix_vbus_dev_info(struct visor_device *visordev)
+{
+	int i;
+	struct visor_device *bdev;
+	struct visor_driver *visordrv;
+	int bus_no = visordev->chipset_bus_no;
+	int dev_no = visordev->chipset_dev_no;
+	struct ultra_vbus_deviceinfo dev_info;
+	const char *chan_type_name = NULL;
+	struct spar_vbus_headerinfo *hdr_info;
+
+	if (!visordev->device.driver)
+			return;
+
+	hdr_info = (struct spar_vbus_headerinfo *)visordev->vbus_hdr_info;
+	if (!hdr_info)
+		return;
+
+	bdev = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
+	if (!bdev)
+		return;
+
+	visordrv = to_visor_driver(visordev->device.driver);
+
+	/* Within the list of device types (by GUID) that the driver
+	 * says it supports, find out which one of those types matches
+	 * the type of this device, so that we can include the device
+	 * type name
+	 */
+	for (i = 0; visordrv->channel_types[i].name; i++) {
+		if (memcmp(&visordrv->channel_types[i].guid,
+			   &visordev->channel_type_guid,
+			   sizeof(visordrv->channel_types[i].guid)) == 0) {
+			chan_type_name = visordrv->channel_types[i].name;
+			break;
+		}
+	}
+
+	bus_device_info_init(&dev_info, chan_type_name,
+			     visordrv->name, visordrv->version,
+			     visordrv->vertag);
+	write_vbus_dev_info(bdev->visorchannel, hdr_info, &dev_info, dev_no);
+
+	/* Re-write bus+chipset info, because it is possible that this
+	* was previously written by our evil counterpart, virtpci.
+	*/
+	write_vbus_chp_info(bdev->visorchannel, hdr_info, &chipset_driverinfo);
+	write_vbus_bus_info(bdev->visorchannel, hdr_info,
+			    &clientbus_driverinfo);
+}
+
+/** Create a device instance for the visor bus itself.
+ */
+static int
+create_bus_instance(struct visor_device *dev)
+{
+	int rc;
+	int id = dev->chipset_bus_no;
+	struct spar_vbus_headerinfo *hdr_info;
+
+	POSTCODE_LINUX_2(BUS_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
+
+	hdr_info = kzalloc(sizeof(*hdr_info), GFP_KERNEL);
+	if (!hdr_info) {
+		rc = -1;
+		goto away;
+	}
+
+	dev_set_name(&dev->device, "visorbus%d", id);
+	dev->device.bus = &visorbus_type;
+	dev->device.groups = visorbus_groups;
+	dev->device.release = visorbus_release_busdevice;
+
+	if (device_register(&dev->device) < 0) {
+		POSTCODE_LINUX_3(DEVICE_CREATE_FAILURE_PC, id,
+				 POSTCODE_SEVERITY_ERR);
+		rc = -1;
+		goto away_mem;
+	}
+
+	if (get_vbus_header_info(dev->visorchannel, hdr_info) >= 0) {
+		dev->vbus_hdr_info = (void *)hdr_info;
+		write_vbus_chp_info(dev->visorchannel, hdr_info,
+				    &chipset_driverinfo);
+		write_vbus_bus_info(dev->visorchannel, hdr_info,
+				    &clientbus_driverinfo);
+	} else {
+		kfree(hdr_info);
+	}
+	bus_count++;
+	list_add_tail(&dev->list_all, &list_all_bus_instances);
+	dev_set_drvdata(&dev->device, dev);
+	return 0;
+
+away_mem:
+	kfree(hdr_info);
+away:
+	return rc;
+}
+
+/** Remove a device instance for the visor bus itself.
+ */
+static void
+remove_bus_instance(struct visor_device *dev)
+{
+	/* Note that this will result in the release method for
+	 * dev->dev being called, which will call
+	 * visorbus_release_busdevice().  This has something to do with
+	 * the put_device() done in device_unregister(), but I have never
+	 * successfully been able to trace thru the code to see where/how
+	 * release() gets called.  But I know it does.
+	 */
+	bus_count--;
+	if (dev->visorchannel) {
+		visorchannel_destroy(dev->visorchannel);
+		dev->visorchannel = NULL;
+	}
+	kfree(dev->vbus_hdr_info);
+	list_del(&dev->list_all);
+	device_unregister(&dev->device);
+}
+
+/** Create and register the one-and-only one instance of
+ *  the visor bus type (visorbus_type).
+ */
+static int
+create_bus_type(void)
+{
+	int rc = 0;
+
+	rc = bus_register(&visorbus_type);
+	return rc;
+}
+
+/** Remove the one-and-only one instance of the visor bus type (visorbus_type).
+ */
+static void
+remove_bus_type(void)
+{
+	bus_unregister(&visorbus_type);
+}
+
+/** Remove all child visor bus device instances.
+ */
+static void
+remove_all_visor_devices(void)
+{
+	struct list_head *listentry, *listtmp;
+
+	list_for_each_safe(listentry, listtmp, &list_all_device_instances) {
+		struct visor_device *dev = list_entry(listentry,
+						      struct visor_device,
+						      list_all);
+		remove_visor_device(dev);
+	}
+}
+
+static void
+chipset_bus_create(struct visor_device *dev)
+{
+	int rc;
+	u32 bus_no = dev->chipset_bus_no;
+
+	POSTCODE_LINUX_3(BUS_CREATE_ENTRY_PC, bus_no, POSTCODE_SEVERITY_INFO);
+	rc = create_bus_instance(dev);
+	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, bus_no, POSTCODE_SEVERITY_INFO);
+
+	if (rc < 0)
+		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
+				 POSTCODE_SEVERITY_ERR);
+	else
+		POSTCODE_LINUX_3(CHIPSET_INIT_SUCCESS_PC, bus_no,
+				 POSTCODE_SEVERITY_INFO);
+
+	if (chipset_responders.bus_create)
+		(*chipset_responders.bus_create) (dev, rc);
+}
+
+static void
+chipset_bus_destroy(struct visor_device *dev)
+{
+	remove_bus_instance(dev);
+	if (chipset_responders.bus_destroy)
+		(*chipset_responders.bus_destroy)(dev, 0);
+}
+
+static void
+chipset_device_create(struct visor_device *dev_info)
+{
+	int rc = -1;
+	u32 bus_no = dev_info->chipset_bus_no;
+	u32 dev_no = dev_info->chipset_dev_no;
+
+	POSTCODE_LINUX_4(DEVICE_CREATE_ENTRY_PC, dev_no, bus_no,
+			 POSTCODE_SEVERITY_INFO);
+
+	rc = create_visor_device(dev_info);
+	if (chipset_responders.device_create)
+		chipset_responders.device_create(dev_info, rc);
+
+	if (rc < 0)
+		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
+				 POSTCODE_SEVERITY_ERR);
+	else
+		POSTCODE_LINUX_4(DEVICE_CREATE_SUCCESS_PC, dev_no, bus_no,
+				 POSTCODE_SEVERITY_INFO);
+}
+
+static void
+chipset_device_destroy(struct visor_device *dev_info)
+{
+	remove_visor_device(dev_info);
+
+	if (chipset_responders.device_destroy)
+		(*chipset_responders.device_destroy) (dev_info, 0);
+}
+
+/* This is the callback function specified for a function driver, to
+ * be called when a pending "pause device" operation has been
+ * completed.
+ */
+static void
+pause_state_change_complete(struct visor_device *dev, int status)
+{
+	if (!dev->pausing)
+			return;
+
+	dev->pausing = false;
+	if (!chipset_responders.device_pause) /* this can never happen! */
+			return;
+
+	/* Notify the chipset driver that the pause is complete, which
+	* will presumably want to send some sort of response to the
+	* initiator. */
+	(*chipset_responders.device_pause) (dev, status);
+}
+
+/* This is the callback function specified for a function driver, to
+ * be called when a pending "resume device" operation has been
+ * completed.
+ */
+static void
+resume_state_change_complete(struct visor_device *dev, int status)
+{
+	if (!dev->resuming)
+			return;
+
+	dev->resuming = false;
+	if (!chipset_responders.device_resume) /* this can never happen! */
+			return;
+
+	/* Notify the chipset driver that the resume is complete,
+	 * which will presumably want to send some sort of response to
+	 * the initiator. */
+	(*chipset_responders.device_resume) (dev, status);
+}
+
+/* Tell the subordinate function driver for a specific device to pause
+ * or resume that device.  Result is returned asynchronously via a
+ * callback function.
+ */
+static void
+initiate_chipset_device_pause_resume(struct visor_device *dev, bool is_pause)
+{
+	int rc = -1, x;
+	struct visor_driver *drv = NULL;
+	void (*notify_func)(struct visor_device *dev, int response) = NULL;
+
+	if (is_pause)
+		notify_func = chipset_responders.device_pause;
+	else
+		notify_func = chipset_responders.device_resume;
+	if (!notify_func)
+			goto away;
+
+	drv = to_visor_driver(dev->device.driver);
+	if (!drv)
+			goto away;
+
+	if (dev->pausing || dev->resuming)
+			goto away;
+
+	/* Note that even though both drv->pause() and drv->resume
+	 * specify a callback function, it is NOT necessary for us to
+	 * increment our local module usage count.  Reason is, there
+	 * is already a linkage dependency between child function
+	 * drivers and visorbus, so it is already IMPOSSIBLE to unload
+	 * visorbus while child function drivers are still running.
+	 */
+	if (is_pause) {
+		if (!drv->pause)
+				goto away;
+
+		dev->pausing = true;
+		x = drv->pause(dev, pause_state_change_complete);
+	} else {
+		/* This should be done at BUS resume time, but an
+		 * existing problem prevents us from ever getting a bus
+		 * resume...  This hack would fail to work should we
+		 * ever have a bus that contains NO devices, since we
+		 * would never even get here in that case. */
+		fix_vbus_dev_info(dev);
+		if (!drv->resume)
+				goto away;
+
+		dev->resuming = true;
+		x = drv->resume(dev, resume_state_change_complete);
+	}
+	if (x < 0) {
+		if (is_pause)
+			dev->pausing = false;
+		else
+			dev->resuming = false;
+		goto away;
+	}
+	rc = 0;
+away:
+	if (rc < 0) {
+		if (notify_func)
+				(*notify_func)(dev, rc);
+	}
+}
+
+static void
+chipset_device_pause(struct visor_device *dev_info)
+{
+	initiate_chipset_device_pause_resume(dev_info, true);
+}
+
+static void
+chipset_device_resume(struct visor_device *dev_info)
+{
+	initiate_chipset_device_pause_resume(dev_info, false);
+}
+
+struct channel_size_info {
+	uuid_le guid;
+	unsigned long min_size;
+	unsigned long max_size;
+};
+
+int
+visorbus_init(void)
+{
+	int rc = 0;
+
+	POSTCODE_LINUX_3(DRIVER_ENTRY_PC, rc, POSTCODE_SEVERITY_INFO);
+	bus_device_info_init(&clientbus_driverinfo,
+			     "clientbus", "visorbus",
+			     VERSION, NULL);
+
+	rc = create_bus_type();
+	if (rc < 0) {
+		POSTCODE_LINUX_2(BUS_CREATE_ENTRY_PC, DIAG_SEVERITY_ERR);
+		goto away;
+	}
+
+	periodic_dev_workqueue = create_singlethread_workqueue("visorbus_dev");
+	if (!periodic_dev_workqueue) {
+		POSTCODE_LINUX_2(CREATE_WORKQUEUE_PC, DIAG_SEVERITY_ERR);
+		rc = -ENOMEM;
+		goto away;
+	}
+
+	/* This enables us to receive notifications when devices appear for
+	 * which this service partition is to be a server for.
+	 */
+	visorchipset_register_busdev(&chipset_notifiers,
+				     &chipset_responders,
+				     &chipset_driverinfo);
+
+	rc = 0;
+
+away:
+	if (rc)
+			POSTCODE_LINUX_3(CHIPSET_INIT_FAILURE_PC, rc,
+					 POSTCODE_SEVERITY_ERR);
+	return rc;
+}
+
+void
+visorbus_exit(void)
+{
+	struct list_head *listentry, *listtmp;
+
+	visorchipset_register_busdev(NULL, NULL, NULL);
+	remove_all_visor_devices();
+
+	flush_workqueue(periodic_dev_workqueue); /* better not be any work! */
+	destroy_workqueue(periodic_dev_workqueue);
+	periodic_dev_workqueue = NULL;
+
+	if (periodic_test_workqueue) {
+		cancel_delayed_work(&periodic_work);
+		flush_workqueue(periodic_test_workqueue);
+		destroy_workqueue(periodic_test_workqueue);
+		periodic_test_workqueue = NULL;
+	}
+
+	list_for_each_safe(listentry, listtmp, &list_all_bus_instances) {
+		struct visor_device *dev = list_entry(listentry,
+							      struct
+							      visor_device,
+							      list_all);
+		remove_bus_instance(dev);
+	}
+	remove_bus_type();
+}
+
+module_param_named(debug, visorbus_debug, int, S_IRUGO);
+MODULE_PARM_DESC(visorbus_debug, "1 to debug");
+
+module_param_named(forcematch, visorbus_forcematch, int, S_IRUGO);
+MODULE_PARM_DESC(visorbus_forcematch,
+		 "1 to force a successful dev <--> drv match");
+
+module_param_named(forcenomatch, visorbus_forcenomatch, int, S_IRUGO);
+MODULE_PARM_DESC(visorbus_forcenomatch,
+		 "1 to force an UNsuccessful dev <--> drv match");
+
+module_param_named(debugref, visorbus_debugref, int, S_IRUGO);
+MODULE_PARM_DESC(visorbus_debugref, "1 to debug reference counting");
diff --git a/drivers/staging/unisys/visorbus/visorbus_private.h b/drivers/staging/unisys/visorbus/visorbus_private.h
new file mode 100644
index 000000000000..2f12483e38ab
--- /dev/null
+++ b/drivers/staging/unisys/visorbus/visorbus_private.h
@@ -0,0 +1,69 @@
+/* visorchipset.h
+ *
+ * Copyright (C) 2010 - 2013 UNISYS CORPORATION
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or (at
+ * your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ */
+
+#ifndef __VISORCHIPSET_H__
+#define __VISORCHIPSET_H__
+
+#include <linux/uuid.h>
+
+#include "controlvmchannel.h"
+#include "vbusdeviceinfo.h"
+#include "vbushelper.h"
+
+/*  These functions will be called from within visorchipset when certain
+ *  events happen.  (The implementation of these functions is outside of
+ *  visorchipset.)
+ */
+struct visorchipset_busdev_notifiers {
+	void (*bus_create)(struct visor_device *bus_info);
+	void (*bus_destroy)(struct visor_device *bus_info);
+	void (*device_create)(struct visor_device *bus_info);
+	void (*device_destroy)(struct visor_device *bus_info);
+	void (*device_pause)(struct visor_device *bus_info);
+	void (*device_resume)(struct visor_device *bus_info);
+};
+
+/*  These functions live inside visorchipset, and will be called to indicate
+ *  responses to specific events (by code outside of visorchipset).
+ *  For now, the value for each response is simply either:
+ *       0 = it worked
+ *      -1 = it failed
+ */
+struct visorchipset_busdev_responders {
+	void (*bus_create)(struct visor_device *p, int response);
+	void (*bus_destroy)(struct visor_device *p, int response);
+	void (*device_create)(struct visor_device *p, int response);
+	void (*device_destroy)(struct visor_device *p, int response);
+	void (*device_pause)(struct visor_device *p, int response);
+	void (*device_resume)(struct visor_device *p, int response);
+};
+
+/** Register functions (in the bus driver) to get called by visorchipset
+ *  whenever a bus or device appears for which this guest is to be the
+ *  client for.  visorchipset will fill in <responders>, to indicate
+ *  functions the bus driver should call to indicate message responses.
+ */
+void
+visorchipset_register_busdev(
+			struct visorchipset_busdev_notifiers *notifiers,
+			struct visorchipset_busdev_responders *responders,
+			struct ultra_vbus_deviceinfo *driver_info);
+
+/* visorbus init and exit functions */
+int visorbus_init(void);
+void visorbus_exit(void);
+#endif
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c b/drivers/staging/unisys/visorbus/visorchannel.c
index 7a9a7242f75d..20b63496e9f2 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
+++ b/drivers/staging/unisys/visorbus/visorchannel.c
@@ -17,23 +17,31 @@
 
 /*
  *  This provides Supervisor channel communication primitives, which are
- *  independent of the mechanism used to access the channel data.  All channel
- *  data is accessed using the memregion abstraction.  (memregion has both
- *  a CM2 implementation and a direct memory implementation.)
+ *  independent of the mechanism used to access the channel data.
  */
 
-#include "globals.h"
-#include "visorchannel.h"
 #include <linux/uuid.h>
 
+#include "version.h"
+#include "visorbus.h"
+#include "controlvmchannel.h"
+
 #define MYDRVNAME "visorchannel"
 
+#define SPAR_CONSOLEVIDEO_CHANNEL_PROTOCOL_GUID \
+	UUID_LE(0x3cd6e705, 0xd6a2, 0x4aa5,           \
+		0xad, 0x5c, 0x7b, 0x8, 0x88, 0x9d, 0xff, 0xe2)
+static const uuid_le spar_video_guid = SPAR_CONSOLEVIDEO_CHANNEL_PROTOCOL_GUID;
+
 struct visorchannel {
-	struct memregion *memregion;	/* from visor_memregion_create() */
+	u64 physaddr;
+	ulong nbytes;
+	void __iomem *mapped;
+	bool requested;
 	struct channel_header chan_hdr;
 	uuid_le guid;
 	ulong size;
-	BOOL needs_lock;	/* channel creator knows if more than one
+	bool needs_lock;	/* channel creator knows if more than one
 				 * thread will be inserting or removing */
 	spinlock_t insert_lock; /* protect head writes in chan_hdr */
 	spinlock_t remove_lock;	/* protect tail writes in chan_hdr */
@@ -44,126 +52,133 @@ struct visorchannel {
 		struct signal_queue_header event_queue;
 		struct signal_queue_header ack_queue;
 	} safe_uis_queue;
+	uuid_le type;
+	uuid_le inst;
 };
 
 /* Creates the struct visorchannel abstraction for a data area in memory,
  * but does NOT modify this data area.
  */
 static struct visorchannel *
-visorchannel_create_guts(HOSTADDRESS physaddr, ulong channel_bytes,
-			 struct visorchannel *parent, ulong off, uuid_le guid,
-			 BOOL needs_lock)
+visorchannel_create_guts(u64 physaddr, unsigned long channel_bytes,
+			 gfp_t gfp, unsigned long off,
+			 uuid_le guid, bool needs_lock)
 {
-	struct visorchannel *p = NULL;
-	void *rc = NULL;
+	struct visorchannel *channel;
+	int err;
+	size_t size = sizeof(struct channel_header);
+
+	if (physaddr == 0)
+		return NULL;
 
-	p = kmalloc(sizeof(*p), GFP_KERNEL|__GFP_NORETRY);
-	if (!p) {
-		rc = NULL;
+	channel = kzalloc(sizeof(*channel), gfp);
+	if (!channel)
 		goto cleanup;
+
+	channel->needs_lock = needs_lock;
+	spin_lock_init(&channel->insert_lock);
+	spin_lock_init(&channel->remove_lock);
+
+	/* Video driver constains the efi framebuffer so it will get a
+	 * conflict resource when requesting its full mem region. Since
+	 * we are only using the efi framebuffer for video we can ignore
+	 * this. Remember that we haven't requested it so we don't try to
+	 * release later on.
+	 */
+	channel->requested = request_mem_region(physaddr, size, MYDRVNAME);
+	if (!channel->requested) {
+		if (uuid_le_cmp(guid, spar_video_guid)) {
+			/* Not the video channel we care about this */
+			goto cleanup;
+		}
 	}
-	p->memregion = NULL;
-	p->needs_lock = needs_lock;
-	spin_lock_init(&p->insert_lock);
-	spin_lock_init(&p->remove_lock);
-
-	/* prepare chan_hdr (abstraction to read/write channel memory) */
-	if (!parent)
-		p->memregion =
-		    visor_memregion_create(physaddr,
-					   sizeof(struct channel_header));
-	else
-		p->memregion =
-		    visor_memregion_create_overlapped(parent->memregion,
-				off, sizeof(struct channel_header));
-	if (!p->memregion) {
-		rc = NULL;
+
+	channel->mapped = ioremap_cache(physaddr, size);
+	if (!channel->mapped) {
+		release_mem_region(physaddr, size);
 		goto cleanup;
 	}
-	if (visor_memregion_read(p->memregion, 0, &p->chan_hdr,
-				 sizeof(struct channel_header)) < 0) {
-		rc = NULL;
+
+	channel->physaddr = physaddr;
+	channel->nbytes = size;
+
+	err = visorchannel_read(channel, 0, &channel->chan_hdr,
+				sizeof(struct channel_header));
+	if (err)
 		goto cleanup;
-	}
+
+	/* we had better be a CLIENT of this channel */
 	if (channel_bytes == 0)
-		/* we had better be a CLIENT of this channel */
-		channel_bytes = (ulong)p->chan_hdr.size;
+		channel_bytes = (ulong)channel->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.chtype;
-	if (visor_memregion_resize(p->memregion, channel_bytes) < 0) {
-		rc = NULL;
+		guid = channel->chan_hdr.chtype;
+
+	iounmap(channel->mapped);
+	if (channel->requested)
+		release_mem_region(channel->physaddr, channel->nbytes);
+	channel->mapped = NULL;
+	channel->requested = request_mem_region(channel->physaddr,
+						channel_bytes, MYDRVNAME);
+	if (!channel->requested) {
+		if (uuid_le_cmp(guid, spar_video_guid)) {
+			/* Different we care about this */
+			goto cleanup;
+		}
+	}
+
+	channel->mapped = ioremap_cache(channel->physaddr, channel_bytes);
+	if (!channel->mapped) {
+		release_mem_region(channel->physaddr, channel_bytes);
 		goto cleanup;
 	}
-	p->size = channel_bytes;
-	p->guid = guid;
 
-	rc = p;
-cleanup:
+	channel->nbytes = channel_bytes;
 
-	if (!rc) {
-		if (!p) {
-			visorchannel_destroy(p);
-			p = NULL;
-		}
-	}
-	return rc;
+	channel->size = channel_bytes;
+	channel->guid = guid;
+	return channel;
+
+cleanup:
+	visorchannel_destroy(channel);
+	return NULL;
 }
 
 struct visorchannel *
-visorchannel_create(HOSTADDRESS physaddr, ulong channel_bytes, uuid_le guid)
+visorchannel_create(u64 physaddr, unsigned long channel_bytes,
+		    gfp_t gfp, uuid_le guid)
 {
-	return visorchannel_create_guts(physaddr, channel_bytes, NULL, 0, guid,
-					FALSE);
+	return visorchannel_create_guts(physaddr, channel_bytes, gfp, 0, guid,
+					false);
 }
 EXPORT_SYMBOL_GPL(visorchannel_create);
 
 struct visorchannel *
-visorchannel_create_with_lock(HOSTADDRESS physaddr, ulong channel_bytes,
-			      uuid_le guid)
+visorchannel_create_with_lock(u64 physaddr, unsigned long channel_bytes,
+			      gfp_t gfp, uuid_le guid)
 {
-	return visorchannel_create_guts(physaddr, channel_bytes, NULL, 0, guid,
-					TRUE);
+	return visorchannel_create_guts(physaddr, channel_bytes, gfp, 0, guid,
+					true);
 }
 EXPORT_SYMBOL_GPL(visorchannel_create_with_lock);
 
-struct visorchannel *
-visorchannel_create_overlapped(ulong channel_bytes,
-			       struct visorchannel *parent, ulong off,
-			       uuid_le guid)
-{
-	return visorchannel_create_guts(0, channel_bytes, parent, off, guid,
-					FALSE);
-}
-EXPORT_SYMBOL_GPL(visorchannel_create_overlapped);
-
-struct visorchannel *
-visorchannel_create_overlapped_with_lock(ulong channel_bytes,
-					 struct visorchannel *parent, ulong off,
-					 uuid_le guid)
-{
-	return visorchannel_create_guts(0, channel_bytes, parent, off, guid,
-					TRUE);
-}
-EXPORT_SYMBOL_GPL(visorchannel_create_overlapped_with_lock);
-
 void
 visorchannel_destroy(struct visorchannel *channel)
 {
 	if (!channel)
 		return;
-	if (channel->memregion) {
-		visor_memregion_destroy(channel->memregion);
-		channel->memregion = NULL;
+	if (channel->mapped) {
+		iounmap(channel->mapped);
+		if (channel->requested)
+			release_mem_region(channel->physaddr, channel->nbytes);
 	}
 	kfree(channel);
 }
 EXPORT_SYMBOL_GPL(visorchannel_destroy);
 
-HOSTADDRESS
+u64
 visorchannel_get_physaddr(struct visorchannel *channel)
 {
-	return visor_memregion_get_physaddr(channel->memregion);
+	return channel->physaddr;
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_physaddr);
 
@@ -196,13 +211,22 @@ visorchannel_zoneid(struct visorchannel *channel, char *s)
 }
 EXPORT_SYMBOL_GPL(visorchannel_zoneid);
 
-HOSTADDRESS
+u64
 visorchannel_get_clientpartition(struct visorchannel *channel)
 {
 	return channel->chan_hdr.partition_handle;
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_clientpartition);
 
+int
+visorchannel_set_clientpartition(struct visorchannel *channel,
+				 u64 partition_handle)
+{
+	channel->chan_hdr.partition_handle = partition_handle;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(visorchannel_set_clientpartition);
+
 uuid_le
 visorchannel_get_uuid(struct visorchannel *channel)
 {
@@ -210,25 +234,16 @@ visorchannel_get_uuid(struct visorchannel *channel)
 }
 EXPORT_SYMBOL_GPL(visorchannel_get_uuid);
 
-struct memregion *
-visorchannel_get_memregion(struct visorchannel *channel)
-{
-	return channel->memregion;
-}
-EXPORT_SYMBOL_GPL(visorchannel_get_memregion);
-
 int
 visorchannel_read(struct visorchannel *channel, ulong offset,
 		  void *local, ulong nbytes)
 {
-	int rc = visor_memregion_read(channel->memregion, offset,
-				      local, nbytes);
-	if ((rc >= 0) && (offset == 0) &&
-	    (nbytes >= sizeof(struct channel_header))) {
-		memcpy(&channel->chan_hdr, local,
-		       sizeof(struct channel_header));
-	}
-	return rc;
+	if (offset + nbytes > channel->nbytes)
+		return -EIO;
+
+	memcpy_fromio(local, channel->mapped + offset, nbytes);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(visorchannel_read);
 
@@ -236,10 +251,20 @@ int
 visorchannel_write(struct visorchannel *channel, ulong offset,
 		   void *local, ulong nbytes)
 {
-	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);
+	size_t chdr_size = sizeof(struct channel_header);
+	size_t copy_size;
+
+	if (offset + nbytes > channel->nbytes)
+		return -EIO;
+
+	if (offset < chdr_size) {
+		copy_size = min(chdr_size - offset, nbytes);
+		memcpy(&channel->chan_hdr + offset, local, copy_size);
+	}
+
+	memcpy_toio(channel->mapped + offset, local, nbytes);
+
+	return 0;
 }
 EXPORT_SYMBOL_GPL(visorchannel_write);
 
@@ -247,38 +272,35 @@ int
 visorchannel_clear(struct visorchannel *channel, ulong offset, u8 ch,
 		   ulong nbytes)
 {
-	int rc = -1;
-	int bufsize = 65536;
+	int err;
+	int bufsize = PAGE_SIZE;
 	int written = 0;
-	u8 *buf = vmalloc(bufsize);
+	u8 *buf;
 
+	buf = (u8 *) __get_free_page(GFP_KERNEL);
 	if (!buf)
-		goto cleanup;
+		return -ENOMEM;
 
 	memset(buf, ch, bufsize);
+
 	while (nbytes > 0) {
-		ulong thisbytes = bufsize;
-		int x = -1;
+		int thisbytes = bufsize;
 
 		if (nbytes < thisbytes)
 			thisbytes = nbytes;
-		x = visor_memregion_write(channel->memregion, offset + written,
-					  buf, thisbytes);
-		if (x < 0) {
-			rc = x;
+		err = visorchannel_write(channel, offset + written,
+					 buf, thisbytes);
+		if (err)
 			goto cleanup;
-		}
+
 		written += thisbytes;
 		nbytes -= thisbytes;
 	}
-	rc = 0;
+	err = 0;
 
 cleanup:
-	if (buf) {
-		vfree(buf);
-		buf = NULL;
-	}
-	return rc;
+	free_page((unsigned long) buf);
+	return err;
 }
 EXPORT_SYMBOL_GPL(visorchannel_clear);
 
@@ -306,108 +328,77 @@ EXPORT_SYMBOL_GPL(visorchannel_get_header);
 /** Write the contents of a specific field within a SIGNAL_QUEUE_HEADER back
  *  into host memory
  */
-#define SIG_WRITE_FIELD(channel, queue, sig_hdr, FIELD)			\
-	(visor_memregion_write(channel->memregion,			\
-			       SIG_QUEUE_OFFSET(&channel->chan_hdr, queue)+ \
-			       offsetof(struct signal_queue_header, FIELD),\
-			       &((sig_hdr)->FIELD),			\
-			       sizeof((sig_hdr)->FIELD)) >= 0)
-
-static BOOL
+#define SIG_WRITE_FIELD(channel, queue, sig_hdr, FIELD)			 \
+	(visorchannel_write(channel,					 \
+			    SIG_QUEUE_OFFSET(&channel->chan_hdr, queue)+ \
+			    offsetof(struct signal_queue_header, FIELD), \
+			    &((sig_hdr)->FIELD),			 \
+			    sizeof((sig_hdr)->FIELD)) >= 0)
+
+static bool
 sig_read_header(struct visorchannel *channel, u32 queue,
 		struct signal_queue_header *sig_hdr)
 {
-	BOOL rc = FALSE;
+	int err;
 
 	if (channel->chan_hdr.ch_space_offset < sizeof(struct channel_header))
-		goto cleanup;
+		return false;
 
 	/* Read the appropriate SIGNAL_QUEUE_HEADER into local memory. */
+	err = visorchannel_read(channel,
+				SIG_QUEUE_OFFSET(&channel->chan_hdr, queue),
+				sig_hdr, sizeof(struct signal_queue_header));
+	if (err)
+		return false;
 
-	if (visor_memregion_read(channel->memregion,
-				 SIG_QUEUE_OFFSET(&channel->chan_hdr, queue),
-				 sig_hdr,
-				 sizeof(struct signal_queue_header)) < 0) {
-		goto cleanup;
-	}
-	rc = TRUE;
-cleanup:
-	return rc;
+	return true;
 }
 
-static BOOL
-sig_do_data(struct visorchannel *channel, u32 queue,
-	    struct signal_queue_header *sig_hdr, u32 slot, void *data,
-	    BOOL is_write)
+static inline bool
+sig_read_data(struct visorchannel *channel, u32 queue,
+	      struct signal_queue_header *sig_hdr, u32 slot, void *data)
 {
-	BOOL rc = FALSE;
+	int err;
 	int signal_data_offset = SIG_DATA_OFFSET(&channel->chan_hdr, queue,
 						 sig_hdr, slot);
-	if (is_write) {
-		if (visor_memregion_write(channel->memregion,
-					  signal_data_offset,
-					  data, sig_hdr->signal_size) < 0) {
-			goto cleanup;
-		}
-	} else {
-		if (visor_memregion_read(channel->memregion, signal_data_offset,
-					 data, sig_hdr->signal_size) < 0) {
-			goto cleanup;
-		}
-	}
-	rc = TRUE;
-cleanup:
-	return rc;
-}
 
-static inline BOOL
-sig_read_data(struct visorchannel *channel, u32 queue,
-	      struct signal_queue_header *sig_hdr, u32 slot, void *data)
-{
-	return sig_do_data(channel, queue, sig_hdr, slot, data, FALSE);
+	err = visorchannel_read(channel, signal_data_offset,
+				data, sig_hdr->signal_size);
+	if (err)
+		return false;
+
+	return true;
 }
 
-static inline BOOL
+static inline bool
 sig_write_data(struct visorchannel *channel, u32 queue,
 	       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(struct signal_queue_header *psafe_sqh,
-			struct signal_queue_header *punsafe_sqh,
-			u32 *phead, u32 *ptail)
-{
-	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;
+	int err;
+	int signal_data_offset = SIG_DATA_OFFSET(&channel->chan_hdr, queue,
+						 sig_hdr, slot);
 
-		/* Sync with client as necessary */
-		punsafe_sqh->head = *phead;
-		punsafe_sqh->tail = *ptail;
+	err = visorchannel_write(channel, signal_data_offset,
+				 data, sig_hdr->signal_size);
+	if (err)
+		return false;
 
-		return 0;
-	}
-	return 1;
-}				/* end safe_sig_queue_validate */
+	return true;
+}
 
-static BOOL
+static bool
 signalremove_inner(struct visorchannel *channel, u32 queue, void *msg)
 {
 	struct signal_queue_header sig_hdr;
 
 	if (!sig_read_header(channel, queue, &sig_hdr))
-		return FALSE;
+		return false;
 	if (sig_hdr.head == sig_hdr.tail)
-		return FALSE;	/* no signals to remove */
+		return false;	/* no signals to remove */
 
 	sig_hdr.tail = (sig_hdr.tail + 1) % sig_hdr.max_slots;
-	if (!sig_read_data(channel, queue, &sig_hdr, sig_hdr.tail, msg)) {
-		return FALSE;
-	}
+	if (!sig_read_data(channel, queue, &sig_hdr, sig_hdr.tail, msg))
+		return false;
 	sig_hdr.num_received++;
 
 	/* For each data field in SIGNAL_QUEUE_HEADER that was modified,
@@ -415,16 +406,16 @@ signalremove_inner(struct visorchannel *channel, u32 queue, void *msg)
 	 */
 	mb(); /* required for channel synch */
 	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, tail))
-		return FALSE;
+		return false;
 	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_received))
-		return FALSE;
-	return TRUE;
+		return false;
+	return true;
 }
 
-BOOL
+bool
 visorchannel_signalremove(struct visorchannel *channel, u32 queue, void *msg)
 {
-	BOOL rc;
+	bool rc;
 
 	if (channel->needs_lock) {
 		spin_lock(&channel->remove_lock);
@@ -438,29 +429,28 @@ visorchannel_signalremove(struct visorchannel *channel, u32 queue, void *msg)
 }
 EXPORT_SYMBOL_GPL(visorchannel_signalremove);
 
-static BOOL
+static bool
 signalinsert_inner(struct visorchannel *channel, u32 queue, void *msg)
 {
 	struct signal_queue_header sig_hdr;
 
 	if (!sig_read_header(channel, queue, &sig_hdr))
-		return FALSE;
+		return false;
 
 	sig_hdr.head = ((sig_hdr.head + 1) % sig_hdr.max_slots);
 	if (sig_hdr.head == sig_hdr.tail) {
 		sig_hdr.num_overflows++;
-		visor_memregion_write(channel->memregion,
-				      SIG_QUEUE_OFFSET(&channel->chan_hdr,
-						       queue) +
-				      offsetof(struct signal_queue_header,
-					       num_overflows),
-				      &(sig_hdr.num_overflows),
-				      sizeof(sig_hdr.num_overflows));
-		return FALSE;
+		visorchannel_write(channel,
+				   SIG_QUEUE_OFFSET(&channel->chan_hdr, queue) +
+				   offsetof(struct signal_queue_header,
+					    num_overflows),
+				   &(sig_hdr.num_overflows),
+				   sizeof(sig_hdr.num_overflows));
+		return false;
 	}
 
 	if (!sig_write_data(channel, queue, &sig_hdr, sig_hdr.head, msg))
-		return FALSE;
+		return false;
 
 	sig_hdr.num_sent++;
 
@@ -469,18 +459,17 @@ signalinsert_inner(struct visorchannel *channel, u32 queue, void *msg)
 	 */
 	mb(); /* required for channel synch */
 	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, head))
-		return FALSE;
-	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_sent)) {
-		return FALSE;
-	}
+		return false;
+	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_sent))
+		return false;
 
-	return TRUE;
+	return true;
 }
 
-BOOL
+bool
 visorchannel_signalinsert(struct visorchannel *channel, u32 queue, void *msg)
 {
-	BOOL rc;
+	bool rc;
 
 	if (channel->needs_lock) {
 		spin_lock(&channel->insert_lock);
@@ -552,9 +541,8 @@ void
 visorchannel_debug(struct visorchannel *channel, int num_queues,
 		   struct seq_file *seq, u32 off)
 {
-	HOSTADDRESS addr = 0;
+	u64 addr = 0;
 	ulong nbytes = 0, nbytes_region = 0;
-	struct memregion *memregion = NULL;
 	struct channel_header hdr;
 	struct channel_header *phdr = &hdr;
 	int i = 0;
@@ -562,12 +550,9 @@ visorchannel_debug(struct visorchannel *channel, int num_queues,
 
 	if (!channel)
 		return;
-	memregion = channel->memregion;
-	if (!memregion)
-		return;
 
-	addr = visor_memregion_get_physaddr(memregion);
-	nbytes_region = visor_memregion_get_nbytes(memregion);
+	addr = visorchannel_get_physaddr(channel);
+	nbytes_region = visorchannel_get_nbytes(channel);
 	errcode = visorchannel_read(channel, off,
 				    phdr, sizeof(struct channel_header));
 	if (errcode < 0) {
@@ -626,40 +611,3 @@ visorchannel_debug(struct visorchannel *channel, int num_queues,
 		   addr + off, nbytes);
 }
 EXPORT_SYMBOL_GPL(visorchannel_debug);
-
-void
-visorchannel_dump_section(struct visorchannel *chan, char *s,
-			  int off, int len, struct seq_file *seq)
-{
-	char *buf, *tbuf, *fmtbuf;
-	int fmtbufsize = 0;
-	int i;
-	int errcode = 0;
-
-	fmtbufsize = 100 * COVQ(len, 16);
-	buf = kmalloc(len, GFP_KERNEL|__GFP_NORETRY);
-	if (!buf)
-		return;
-	fmtbuf = kmalloc(fmtbufsize, GFP_KERNEL|__GFP_NORETRY);
-	if (!fmtbuf)
-		goto fmt_failed;
-
-	errcode = visorchannel_read(chan, off, buf, len);
-	if (errcode < 0)
-		goto read_failed;
-	seq_printf(seq, "channel %s:\n", s);
-	tbuf = buf;
-	while (len > 0) {
-		i = (len < 16) ? len : 16;
-		hex_dump_to_buffer(tbuf, i, 16, 1, fmtbuf, fmtbufsize, TRUE);
-		seq_printf(seq, "%s\n", fmtbuf);
-		tbuf += 16;
-		len -= 16;
-	}
-
-read_failed:
-	kfree(fmtbuf);
-fmt_failed:
-	kfree(buf);
-}
-EXPORT_SYMBOL_GPL(visorchannel_dump_section);
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_main.c b/drivers/staging/unisys/visorbus/visorchipset.c
index f2663d2c7530..bb8087e70127 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset_main.c
+++ b/drivers/staging/unisys/visorbus/visorchipset.c
@@ -15,27 +15,28 @@
  * details.
  */
 
-#include "globals.h"
-#include "visorchipset.h"
-#include "procobjecttree.h"
-#include "visorchannel.h"
-#include "periodic_work.h"
-#include "file.h"
-#include "parser.h"
-#include "uisutils.h"
-#include "controlvmcompletionstatus.h"
-#include "guestlinuxdebug.h"
-
+#include <linux/acpi.h>
+#include <linux/cdev.h>
+#include <linux/ctype.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
 #include <linux/nls.h>
 #include <linux/netdevice.h>
 #include <linux/platform_device.h>
 #include <linux/uuid.h>
+#include <linux/crash_dump.h>
+
+#include "channel_guid.h"
+#include "controlvmchannel.h"
+#include "controlvmcompletionstatus.h"
+#include "guestlinuxdebug.h"
+#include "periodic_work.h"
+#include "version.h"
+#include "visorbus.h"
+#include "visorbus_private.h"
+#include "vmcallinterface.h"
 
 #define CURRENT_FILE_PC VISOR_CHIPSET_PC_visorchipset_main_c
-#define TEST_VNIC_PHYSITF "eth0"	/* physical network itf for
-					 * vnic loopback test */
-#define TEST_VNIC_SWITCHNO 1
-#define TEST_VNIC_BUSNO 9
 
 #define MAX_NAME_SIZE 128
 #define MAX_IP_SIZE   50
@@ -43,82 +44,88 @@
 #define POLLJIFFIES_CONTROLVMCHANNEL_FAST   1
 #define POLLJIFFIES_CONTROLVMCHANNEL_SLOW 100
 
+#define MAX_CONTROLVM_PAYLOAD_BYTES (1024*128)
+
+#define VISORCHIPSET_MMAP_CONTROLCHANOFFSET	0x00000000
+
+
+#define UNISYS_SPAR_LEAF_ID 0x40000000
+
+/* The s-Par leaf ID returns "UnisysSpar64" encoded across ebx, ecx, edx */
+#define UNISYS_SPAR_ID_EBX 0x73696e55
+#define UNISYS_SPAR_ID_ECX 0x70537379
+#define UNISYS_SPAR_ID_EDX 0x34367261
+
+/*
+ * Module parameters
+ */
+static int visorchipset_major;
+static int visorchipset_visorbusregwait = 1;	/* default is on */
+static int visorchipset_holdchipsetready;
+static unsigned long controlvm_payload_bytes_buffered;
+
+static int
+visorchipset_open(struct inode *inode, struct file *file)
+{
+	unsigned minor_number = iminor(inode);
+
+	if (minor_number)
+		return -ENODEV;
+	file->private_data = NULL;
+	return 0;
+}
+
+static int
+visorchipset_release(struct inode *inode, struct file *file)
+{
+	return 0;
+}
+
 /* When the controlvm channel is idle for at least MIN_IDLE_SECONDS,
 * we switch to slow polling mode.  As soon as we get a controlvm
 * message, we switch back to fast polling mode.
 */
 #define MIN_IDLE_SECONDS 10
-static ulong poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
-static ulong most_recent_message_jiffies;	/* when we got our last
+static unsigned long poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
+static unsigned long most_recent_message_jiffies;	/* when we got our last
 						 * controlvm message */
-static inline char *
-NONULLSTR(char *s)
-{
-	if (s)
-		return s;
-	return "";
-}
-
-static int serverregistered;
-static int clientregistered;
+static int visorbusregistered;
 
 #define MAX_CHIPSET_EVENTS 2
 static u8 chipset_events[MAX_CHIPSET_EVENTS] = { 0, 0 };
 
+struct parser_context {
+	unsigned long allocbytes;
+	unsigned long param_bytes;
+	u8 *curr;
+	unsigned long bytes_remaining;
+	bool byte_stream;
+	char data[0];
+};
+
 static struct delayed_work periodic_controlvm_work;
 static struct workqueue_struct *periodic_controlvm_workqueue;
 static DEFINE_SEMAPHORE(notifier_lock);
 
-static struct controlvm_message_header g_diag_msg_hdr;
+static struct cdev file_cdev;
+static struct visorchannel **file_controlvm_channel;
 static struct controlvm_message_header g_chipset_msg_hdr;
-static struct controlvm_message_header g_del_dump_msg_hdr;
-static const uuid_le spar_diag_pool_channel_protocol_uuid =
-	SPAR_DIAG_POOL_CHANNEL_PROTOCOL_UUID;
-/* 0xffffff is an invalid Bus/Device number */
-static ulong g_diagpool_bus_no = 0xffffff;
-static ulong g_diagpool_dev_no = 0xffffff;
 static struct controlvm_message_packet g_devicechangestate_packet;
 
-/* Only VNIC and VHBA channels are sent to visorclientbus (aka
- * "visorhackbus")
- */
-#define FOR_VISORHACKBUS(channel_type_guid) \
-	(((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) \
-	(uuid_le_cmp(channel_type_guid,\
-		     spar_diag_pool_channel_protocol_uuid) == 0)
-
 static LIST_HEAD(bus_info_list);
 static LIST_HEAD(dev_info_list);
 
 static struct visorchannel *controlvm_channel;
 
 /* Manages the request payload in the controlvm channel */
-static struct controlvm_payload_info {
+struct visor_controlvm_payload_info {
 	u8 __iomem *ptr;	/* pointer to base address of payload pool */
 	u64 offset;		/* offset from beginning of controlvm
 				 * channel to beginning of payload * pool */
 	u32 bytes;		/* number of bytes in payload pool */
-} controlvm_payload_info;
+};
 
-/* Manages the info for a CONTROLVM_DUMP_CAPTURESTATE /
- * CONTROLVM_DUMP_GETTEXTDUMP / CONTROLVM_DUMP_COMPLETE conversation.
- */
-static struct livedump_info {
-	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;
-	atomic_t buffers_in_use;
-	ulong destination;
-} livedump_info;
+static struct visor_controlvm_payload_info controlvm_payload_info;
 
 /* The following globals are used to handle the scenario where we are unable to
  * offload the payload from a controlvm message due to memory requirements.  In
@@ -126,14 +133,7 @@ static struct livedump_info {
  * process it again the next time controlvm_periodic_work() runs.
  */
 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)
- * TRANSMIT_FILE PutFile payloads.
- */
-static struct kmem_cache *putfile_buffer_list_pool;
-static const char putfile_buffer_list_pool_name[] =
-	"controlvm_putfile_buffer_list_pool";
+static bool controlvm_pending_msg_valid;
 
 /* This identifies a data buffer that has been received via a controlvm messages
  * in a remote --> local CONTROLVM_TRANSMIT_FILE conversation.
@@ -203,8 +203,6 @@ struct putfile_request {
 	int completion_status;
 };
 
-static atomic_t visorchipset_cache_buffers_in_use = ATOMIC_INIT(0);
-
 struct parahotplug_request {
 	struct list_head list;
 	int id;
@@ -219,14 +217,16 @@ static void parahotplug_process_list(void);
 /* Manages the info for a CONTROLVM_DUMP_CAPTURESTATE /
  * CONTROLVM_REPORTEVENT.
  */
-static struct visorchipset_busdev_notifiers busdev_server_notifiers;
-static struct visorchipset_busdev_notifiers busdev_client_notifiers;
+static struct visorchipset_busdev_notifiers busdev_notifiers;
 
-static void bus_create_response(ulong bus_no, int response);
-static void bus_destroy_response(ulong bus_no, int response);
-static void device_create_response(ulong bus_no, ulong dev_no, int response);
-static void device_destroy_response(ulong bus_no, ulong dev_no, int response);
-static void device_resume_response(ulong bus_no, ulong dev_no, int response);
+static void bus_create_response(struct visor_device *p, int response);
+static void bus_destroy_response(struct visor_device *p, int response);
+static void device_create_response(struct visor_device *p, int response);
+static void device_destroy_response(struct visor_device *p, int response);
+static void device_resume_response(struct visor_device *p, int response);
+
+static void visorchipset_device_pause_response(struct visor_device *p,
+					       int response);
 
 static struct visorchipset_busdev_responders busdev_responders = {
 	.bus_create = bus_create_response,
@@ -331,11 +331,16 @@ static const struct attribute_group *visorchipset_dev_groups[] = {
 	NULL
 };
 
+static void visorchipset_dev_release(struct device *dev)
+{
+}
+
 /* /sys/devices/platform/visorchipset */
 static struct platform_device visorchipset_platform_device = {
 	.name = "visorchipset",
 	.id = -1,
 	.dev.groups = visorchipset_dev_groups,
+	.dev.release = visorchipset_dev_release,
 };
 
 /* Function prototypes */
@@ -348,6 +353,183 @@ static void controlvm_respond_physdev_changestate(
 		struct controlvm_message_header *msg_hdr, int response,
 		struct spar_segment_state state);
 
+
+static void parser_done(struct parser_context *ctx);
+
+static struct parser_context *
+parser_init_byte_stream(u64 addr, u32 bytes, bool local, bool *retry)
+{
+	int allocbytes = sizeof(struct parser_context) + bytes;
+	struct parser_context *rc = NULL;
+	struct parser_context *ctx = NULL;
+
+	if (retry)
+		*retry = false;
+
+	/*
+	 * alloc an 0 extra byte to ensure payload is
+	 * '\0'-terminated
+	 */
+	allocbytes++;
+	if ((controlvm_payload_bytes_buffered + bytes)
+	    > MAX_CONTROLVM_PAYLOAD_BYTES) {
+		if (retry)
+			*retry = true;
+		rc = NULL;
+		goto cleanup;
+	}
+	ctx = kzalloc(allocbytes, GFP_KERNEL|__GFP_NORETRY);
+	if (!ctx) {
+		if (retry)
+			*retry = true;
+		rc = NULL;
+		goto cleanup;
+	}
+
+	ctx->allocbytes = allocbytes;
+	ctx->param_bytes = bytes;
+	ctx->curr = NULL;
+	ctx->bytes_remaining = 0;
+	ctx->byte_stream = false;
+	if (local) {
+		void *p;
+
+		if (addr > virt_to_phys(high_memory - 1)) {
+			rc = NULL;
+			goto cleanup;
+		}
+		p = __va((unsigned long) (addr));
+		memcpy(ctx->data, p, bytes);
+	} else {
+		void __iomem *mapping;
+
+		if (!request_mem_region(addr, bytes, "visorchipset")) {
+			rc = NULL;
+			goto cleanup;
+		}
+
+		mapping = ioremap_cache(addr, bytes);
+		if (!mapping) {
+			release_mem_region(addr, bytes);
+			rc = NULL;
+			goto cleanup;
+		}
+		memcpy_fromio(ctx->data, mapping, bytes);
+		release_mem_region(addr, bytes);
+	}
+
+	ctx->byte_stream = true;
+	rc = ctx;
+cleanup:
+	if (rc) {
+		controlvm_payload_bytes_buffered += ctx->param_bytes;
+	} else {
+		if (ctx) {
+			parser_done(ctx);
+			ctx = NULL;
+		}
+	}
+	return rc;
+}
+
+static uuid_le
+parser_id_get(struct parser_context *ctx)
+{
+	struct spar_controlvm_parameters_header *phdr = NULL;
+
+	if (ctx == NULL)
+		return NULL_UUID_LE;
+	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
+	return phdr->id;
+}
+
+/** Describes the state from the perspective of which controlvm messages have
+ *  been received for a bus or device.
+ */
+
+enum PARSER_WHICH_STRING {
+	PARSERSTRING_INITIATOR,
+	PARSERSTRING_TARGET,
+	PARSERSTRING_CONNECTION,
+	PARSERSTRING_NAME, /* TODO: only PARSERSTRING_NAME is used ? */
+};
+
+static void
+parser_param_start(struct parser_context *ctx,
+		   enum PARSER_WHICH_STRING which_string)
+{
+	struct spar_controlvm_parameters_header *phdr = NULL;
+
+	if (ctx == NULL)
+		goto Away;
+	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
+	switch (which_string) {
+	case PARSERSTRING_INITIATOR:
+		ctx->curr = ctx->data + phdr->initiator_offset;
+		ctx->bytes_remaining = phdr->initiator_length;
+		break;
+	case PARSERSTRING_TARGET:
+		ctx->curr = ctx->data + phdr->target_offset;
+		ctx->bytes_remaining = phdr->target_length;
+		break;
+	case PARSERSTRING_CONNECTION:
+		ctx->curr = ctx->data + phdr->connection_offset;
+		ctx->bytes_remaining = phdr->connection_length;
+		break;
+	case PARSERSTRING_NAME:
+		ctx->curr = ctx->data + phdr->name_offset;
+		ctx->bytes_remaining = phdr->name_length;
+		break;
+	default:
+		break;
+	}
+
+Away:
+	return;
+}
+
+static void parser_done(struct parser_context *ctx)
+{
+	if (!ctx)
+		return;
+	controlvm_payload_bytes_buffered -= ctx->param_bytes;
+	kfree(ctx);
+}
+
+static void *
+parser_string_get(struct parser_context *ctx)
+{
+	u8 *pscan;
+	unsigned long nscan;
+	int value_length = -1;
+	void *value = NULL;
+	int i;
+
+	if (!ctx)
+		return NULL;
+	pscan = ctx->curr;
+	nscan = ctx->bytes_remaining;
+	if (nscan == 0)
+		return NULL;
+	if (!pscan)
+		return NULL;
+	for (i = 0, value_length = -1; i < nscan; i++)
+		if (pscan[i] == '\0') {
+			value_length = i;
+			break;
+		}
+	if (value_length < 0)	/* '\0' was not included in the length */
+		value_length = nscan;
+	value = kmalloc(value_length + 1, GFP_KERNEL|__GFP_NORETRY);
+	if (value == NULL)
+		return NULL;
+	if (value_length > 0)
+		memcpy(value, pscan, value_length);
+	((u8 *) (value))[value_length] = '\0';
+	return value;
+}
+
+
 static ssize_t toolaction_show(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
@@ -367,7 +549,7 @@ static ssize_t toolaction_store(struct device *dev,
 	u8 tool_action;
 	int ret;
 
-	if (kstrtou8(buf, 10, &tool_action) != 0)
+	if (kstrtou8(buf, 10, &tool_action))
 		return -EINVAL;
 
 	ret = visorchannel_write(controlvm_channel,
@@ -401,7 +583,7 @@ static ssize_t boottotool_store(struct device *dev,
 	int val, ret;
 	struct efi_spar_indication efi_spar_indication;
 
-	if (kstrtoint(buf, 10, &val) != 0)
+	if (kstrtoint(buf, 10, &val))
 		return -EINVAL;
 
 	efi_spar_indication.boot_to_tool = val;
@@ -433,7 +615,7 @@ static ssize_t error_store(struct device *dev, struct device_attribute *attr,
 	u32 error;
 	int ret;
 
-	if (kstrtou32(buf, 10, &error) != 0)
+	if (kstrtou32(buf, 10, &error))
 		return -EINVAL;
 
 	ret = visorchannel_write(controlvm_channel,
@@ -463,7 +645,7 @@ static ssize_t textid_store(struct device *dev, struct device_attribute *attr,
 	u32 text_id;
 	int ret;
 
-	if (kstrtou32(buf, 10, &text_id) != 0)
+	if (kstrtou32(buf, 10, &text_id))
 		return -EINVAL;
 
 	ret = visorchannel_write(controlvm_channel,
@@ -494,7 +676,7 @@ static ssize_t remaining_steps_store(struct device *dev,
 	u16 remaining_steps;
 	int ret;
 
-	if (kstrtou16(buf, 10, &remaining_steps) != 0)
+	if (kstrtou16(buf, 10, &remaining_steps))
 		return -EINVAL;
 
 	ret = visorchannel_write(controlvm_channel,
@@ -506,30 +688,44 @@ static ssize_t remaining_steps_store(struct device *dev,
 	return count;
 }
 
-static void
-bus_info_clear(void *v)
-{
-	struct visorchipset_bus_info *p = (struct visorchipset_bus_info *) (v);
+struct visor_busdev {
+	u32 bus_no;
+	u32 dev_no;
+};
 
-	kfree(p->name);
-	p->name = NULL;
+static int match_visorbus_dev_by_id(struct device *dev, void *data)
+{
+	struct visor_device *vdev = to_visor_device(dev);
+	struct visor_busdev *id = (struct visor_busdev *)data;
+	u32 bus_no = id->bus_no;
+	u32 dev_no = id->dev_no;
 
-	kfree(p->description);
-	p->description = NULL;
+	if ((vdev->chipset_bus_no == bus_no) &&
+	    (vdev->chipset_dev_no == dev_no))
+		return 1;
 
-	p->state.created = 0;
-	memset(p, 0, sizeof(struct visorchipset_bus_info));
+	return 0;
 }
-
-static void
-dev_info_clear(void *v)
+struct visor_device *visorbus_get_device_by_id(u32 bus_no, u32 dev_no,
+					       struct visor_device *from)
 {
-	struct visorchipset_device_info *p =
-			(struct visorchipset_device_info *)(v);
+	struct device *dev;
+	struct device *dev_start = NULL;
+	struct visor_device *vdev = NULL;
+	struct visor_busdev id = {
+			.bus_no = bus_no,
+			.dev_no = dev_no
+		};
 
-	p->state.created = 0;
-	memset(p, 0, sizeof(struct visorchipset_device_info));
+	if (from)
+		dev_start = &from->device;
+	dev = bus_find_device(&visorbus_type, dev_start, (void *)&id,
+			      match_visorbus_dev_by_id);
+	if (dev)
+		vdev = to_visor_device(dev);
+	return vdev;
 }
+EXPORT_SYMBOL(visorbus_get_device_by_id);
 
 static u8
 check_chipset_events(void)
@@ -552,19 +748,19 @@ clear_chipset_events(void)
 }
 
 void
-visorchipset_register_busdev_server(
+visorchipset_register_busdev(
 			struct visorchipset_busdev_notifiers *notifiers,
 			struct visorchipset_busdev_responders *responders,
 			struct ultra_vbus_deviceinfo *driver_info)
 {
 	down(&notifier_lock);
 	if (!notifiers) {
-		memset(&busdev_server_notifiers, 0,
-		       sizeof(busdev_server_notifiers));
-		serverregistered = 0;	/* clear flag */
+		memset(&busdev_notifiers, 0,
+		       sizeof(busdev_notifiers));
+		visorbusregistered = 0;	/* clear flag */
 	} else {
-		busdev_server_notifiers = *notifiers;
-		serverregistered = 1;	/* set flag */
+		busdev_notifiers = *notifiers;
+		visorbusregistered = 1;	/* set flag */
 	}
 	if (responders)
 		*responders = busdev_responders;
@@ -574,50 +770,7 @@ visorchipset_register_busdev_server(
 
 	up(&notifier_lock);
 }
-EXPORT_SYMBOL_GPL(visorchipset_register_busdev_server);
-
-void
-visorchipset_register_busdev_client(
-			struct visorchipset_busdev_notifiers *notifiers,
-			struct visorchipset_busdev_responders *responders,
-			struct ultra_vbus_deviceinfo *driver_info)
-{
-	down(&notifier_lock);
-	if (!notifiers) {
-		memset(&busdev_client_notifiers, 0,
-		       sizeof(busdev_client_notifiers));
-		clientregistered = 0;	/* clear flag */
-	} else {
-		busdev_client_notifiers = *notifiers;
-		clientregistered = 1;	/* set flag */
-	}
-	if (responders)
-		*responders = busdev_responders;
-	if (driver_info)
-		bus_device_info_init(driver_info, "chipset(bolts)",
-				     "visorchipset", VERSION, NULL);
-	up(&notifier_lock);
-}
-EXPORT_SYMBOL_GPL(visorchipset_register_busdev_client);
-
-static void
-cleanup_controlvm_structures(void)
-{
-	struct visorchipset_bus_info *bi, *tmp_bi;
-	struct visorchipset_device_info *di, *tmp_di;
-
-	list_for_each_entry_safe(bi, tmp_bi, &bus_info_list, entry) {
-		bus_info_clear(bi);
-		list_del(&bi->entry);
-		kfree(bi);
-	}
-
-	list_for_each_entry_safe(di, tmp_di, &dev_info_list, entry) {
-		dev_info_clear(di);
-		list_del(&di->entry);
-		kfree(di);
-	}
-}
+EXPORT_SYMBOL_GPL(visorchipset_register_busdev);
 
 static void
 chipset_init(struct controlvm_message *inmsg)
@@ -645,8 +798,6 @@ chipset_init(struct controlvm_message *inmsg)
 	features |= ULTRA_CHIPSET_FEATURE_REPLY;
 
 cleanup:
-	if (rc < 0)
-		cleanup_controlvm_structures();
 	if (inmsg->hdr.flags.response_expected)
 		controlvm_respond_chipset_init(&inmsg->hdr, rc, features);
 }
@@ -672,14 +823,6 @@ controlvm_respond(struct controlvm_message_header *msg_hdr, int response)
 	struct controlvm_message outmsg;
 
 	controlvm_init_response(&outmsg, msg_hdr, response);
-	/* For DiagPool channel DEVICE_CHANGESTATE, we need to send
-	* back the deviceChangeState structure in the packet. */
-	if (msg_hdr->id == CONTROLVM_DEVICE_CHANGESTATE &&
-	    g_devicechangestate_packet.device_change_state.bus_no ==
-	    g_diagpool_bus_no &&
-	    g_devicechangestate_packet.device_change_state.dev_no ==
-	    g_diagpool_dev_no)
-		outmsg.cmd = g_devicechangestate_packet;
 	if (outmsg.hdr.flags.test_message == 1)
 		return;
 
@@ -719,113 +862,40 @@ static void controlvm_respond_physdev_changestate(
 	}
 }
 
-void
-visorchipset_save_message(struct controlvm_message *msg,
-			  enum crash_obj_type type)
-{
-	u32 crash_msg_offset;
-	u16 crash_msg_count;
-
-	/* get saved message count */
-	if (visorchannel_read(controlvm_channel,
-			      offsetof(struct spar_controlvm_channel_protocol,
-				       saved_crash_message_count),
-			      &crash_msg_count, sizeof(u16)) < 0) {
-		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
-				 POSTCODE_SEVERITY_ERR);
-		return;
-	}
-
-	if (crash_msg_count != CONTROLVM_CRASHMSG_MAX) {
-		POSTCODE_LINUX_3(CRASH_DEV_COUNT_FAILURE_PC,
-				 crash_msg_count,
-				 POSTCODE_SEVERITY_ERR);
-		return;
-	}
-
-	/* get saved crash message offset */
-	if (visorchannel_read(controlvm_channel,
-			      offsetof(struct spar_controlvm_channel_protocol,
-				       saved_crash_message_offset),
-			      &crash_msg_offset, sizeof(u32)) < 0) {
-		POSTCODE_LINUX_2(CRASH_DEV_CTRL_RD_FAILURE_PC,
-				 POSTCODE_SEVERITY_ERR);
-		return;
-	}
-
-	if (type == CRASH_BUS) {
-		if (visorchannel_write(controlvm_channel,
-				       crash_msg_offset,
-				       msg,
-				       sizeof(struct controlvm_message)) < 0) {
-			POSTCODE_LINUX_2(SAVE_MSG_BUS_FAILURE_PC,
-					 POSTCODE_SEVERITY_ERR);
-			return;
-		}
-	} else {
-		if (visorchannel_write(controlvm_channel,
-				       crash_msg_offset +
-				       sizeof(struct controlvm_message), msg,
-				       sizeof(struct controlvm_message)) < 0) {
-			POSTCODE_LINUX_2(SAVE_MSG_DEV_FAILURE_PC,
-					 POSTCODE_SEVERITY_ERR);
-			return;
-		}
-	}
-}
-EXPORT_SYMBOL_GPL(visorchipset_save_message);
+enum crash_obj_type {
+	CRASH_DEV,
+	CRASH_BUS,
+};
 
 static void
-bus_responder(enum controlvm_id cmd_id, ulong bus_no, int response)
+bus_responder(enum controlvm_id cmd_id,
+	      struct controlvm_message_header *pending_msg_hdr,
+	      int response)
 {
-	struct visorchipset_bus_info *p = NULL;
-	BOOL need_clear = FALSE;
+	if (pending_msg_hdr == NULL)
+		return;		/* no controlvm response needed */
 
-	p = findbus(&bus_info_list, bus_no);
-	if (!p)
+	if (pending_msg_hdr->id != (u32)cmd_id)
 		return;
 
-	if (response < 0) {
-		if ((cmd_id == CONTROLVM_BUS_CREATE) &&
-		    (response != (-CONTROLVM_RESP_ERROR_ALREADY_DONE)))
-			/* undo the row we just created... */
-			delbusdevices(&dev_info_list, bus_no);
-	} else {
-		if (cmd_id == CONTROLVM_BUS_CREATE)
-			p->state.created = 1;
-		if (cmd_id == CONTROLVM_BUS_DESTROY)
-			need_clear = TRUE;
-	}
-
-	if (p->pending_msg_hdr.id == CONTROLVM_INVALID)
-		return;		/* no controlvm response needed */
-	if (p->pending_msg_hdr.id != (u32)cmd_id)
-		return;
-	controlvm_respond(&p->pending_msg_hdr, response);
-	p->pending_msg_hdr.id = CONTROLVM_INVALID;
-	if (need_clear) {
-		bus_info_clear(p);
-		delbusdevices(&dev_info_list, bus_no);
-	}
+	controlvm_respond(pending_msg_hdr, response);
 }
 
 static void
 device_changestate_responder(enum controlvm_id cmd_id,
-			     ulong bus_no, ulong dev_no, int response,
+			     struct visor_device *p, int response,
 			     struct spar_segment_state response_state)
 {
-	struct visorchipset_device_info *p = NULL;
 	struct controlvm_message outmsg;
+	u32 bus_no = p->chipset_bus_no;
+	u32 dev_no = p->chipset_dev_no;
 
-	p = finddevice(&dev_info_list, bus_no, dev_no);
-	if (!p)
-		return;
-	if (p->pending_msg_hdr.id == CONTROLVM_INVALID)
+	if (p->pending_msg_hdr == NULL)
 		return;		/* no controlvm response needed */
-	if (p->pending_msg_hdr.id != cmd_id)
+	if (p->pending_msg_hdr->id != cmd_id)
 		return;
 
-	controlvm_init_response(&outmsg, &p->pending_msg_hdr, response);
+	controlvm_init_response(&outmsg, p->pending_msg_hdr, response);
 
 	outmsg.cmd.device_change_state.bus_no = bus_no;
 	outmsg.cmd.device_change_state.dev_no = dev_no;
@@ -834,96 +904,74 @@ device_changestate_responder(enum controlvm_id cmd_id,
 	if (!visorchannel_signalinsert(controlvm_channel,
 				       CONTROLVM_QUEUE_REQUEST, &outmsg))
 		return;
-
-	p->pending_msg_hdr.id = CONTROLVM_INVALID;
 }
 
 static void
-device_responder(enum controlvm_id cmd_id, ulong bus_no, ulong dev_no,
+device_responder(enum controlvm_id cmd_id,
+		 struct controlvm_message_header *pending_msg_hdr,
 		 int response)
 {
-	struct visorchipset_device_info *p = NULL;
-	BOOL need_clear = FALSE;
-
-	p = finddevice(&dev_info_list, bus_no, dev_no);
-	if (!p)
-		return;
-	if (response >= 0) {
-		if (cmd_id == CONTROLVM_DEVICE_CREATE)
-			p->state.created = 1;
-		if (cmd_id == CONTROLVM_DEVICE_DESTROY)
-			need_clear = TRUE;
-	}
-
-	if (p->pending_msg_hdr.id == CONTROLVM_INVALID)
+	if (pending_msg_hdr == NULL)
 		return;		/* no controlvm response needed */
 
-	if (p->pending_msg_hdr.id != (u32)cmd_id)
+	if (pending_msg_hdr->id != (u32)cmd_id)
 		return;
 
-	controlvm_respond(&p->pending_msg_hdr, response);
-	p->pending_msg_hdr.id = CONTROLVM_INVALID;
-	if (need_clear)
-		dev_info_clear(p);
+	controlvm_respond(pending_msg_hdr, response);
 }
 
 static void
-bus_epilog(u32 bus_no,
+bus_epilog(struct visor_device *bus_info,
 	   u32 cmd, struct controlvm_message_header *msg_hdr,
-	   int response, BOOL need_response)
+	   int response, bool need_response)
 {
-	BOOL notified = FALSE;
+	bool notified = false;
+	struct controlvm_message_header *pmsg_hdr = NULL;
 
-	struct visorchipset_bus_info *bus_info = findbus(&bus_info_list,
-							 bus_no);
+	if (!bus_info) {
+		/* relying on a valid passed in response code */
+		/* be lazy and re-use msg_hdr for this failure, is this ok?? */
+		pmsg_hdr = msg_hdr;
+		goto away;
+	}
 
-	if (!bus_info)
-		return;
+	if (bus_info->pending_msg_hdr) {
+		/* only non-NULL if dev is still waiting on a response */
+		response = -CONTROLVM_RESP_ERROR_MESSAGE_ID_INVALID_FOR_CLIENT;
+		pmsg_hdr = bus_info->pending_msg_hdr;
+		goto away;
+	}
 
 	if (need_response) {
-		memcpy(&bus_info->pending_msg_hdr, msg_hdr,
+		pmsg_hdr = kzalloc(sizeof(*pmsg_hdr), GFP_KERNEL);
+		if (!pmsg_hdr) {
+			response = -CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
+			goto away;
+		}
+
+		memcpy(pmsg_hdr, msg_hdr,
 		       sizeof(struct controlvm_message_header));
-	} else {
-		bus_info->pending_msg_hdr.id = CONTROLVM_INVALID;
+		bus_info->pending_msg_hdr = pmsg_hdr;
 	}
 
 	down(&notifier_lock);
 	if (response == CONTROLVM_RESP_SUCCESS) {
 		switch (cmd) {
 		case CONTROLVM_BUS_CREATE:
-			/* We can't tell from the bus_create
-			* information which of our 2 bus flavors the
-			* devices on this bus will ultimately end up.
-			* FORTUNATELY, it turns out it is harmless to
-			* send the bus_create to both of them.  We can
-			* narrow things down a little bit, though,
-			* because we know: - BusDev_Server can handle
-			* either server or client devices
-			* - BusDev_Client can handle ONLY client
-			* devices */
-			if (busdev_server_notifiers.bus_create) {
-				(*busdev_server_notifiers.bus_create) (bus_no);
-				notified = TRUE;
-			}
-			if ((!bus_info->flags.server) /*client */ &&
-			    busdev_client_notifiers.bus_create) {
-				(*busdev_client_notifiers.bus_create) (bus_no);
-				notified = TRUE;
+			if (busdev_notifiers.bus_create) {
+				(*busdev_notifiers.bus_create) (bus_info);
+				notified = true;
 			}
 			break;
 		case CONTROLVM_BUS_DESTROY:
-			if (busdev_server_notifiers.bus_destroy) {
-				(*busdev_server_notifiers.bus_destroy) (bus_no);
-				notified = TRUE;
-			}
-			if ((!bus_info->flags.server) /*client */ &&
-			    busdev_client_notifiers.bus_destroy) {
-				(*busdev_client_notifiers.bus_destroy) (bus_no);
-				notified = TRUE;
+			if (busdev_notifiers.bus_destroy) {
+				(*busdev_notifiers.bus_destroy) (bus_info);
+				notified = true;
 			}
 			break;
 		}
 	}
+away:
 	if (notified)
 		/* The callback function just called above is responsible
 		 * for calling the appropriate visorchipset_busdev_responders
@@ -931,37 +979,51 @@ bus_epilog(u32 bus_no,
 		 */
 		;
 	else
-		bus_responder(cmd, bus_no, response);
+		/*
+		 * Do not kfree(pmsg_hdr) as this is the failure path.
+		 * The success path ('notified') will call the responder
+		 * directly and kfree() there.
+		 */
+		bus_responder(cmd, pmsg_hdr, response);
 	up(&notifier_lock);
 }
 
 static void
-device_epilog(u32 bus_no, u32 dev_no, struct spar_segment_state state, u32 cmd,
+device_epilog(struct visor_device *dev_info,
+	      struct spar_segment_state state, u32 cmd,
 	      struct controlvm_message_header *msg_hdr, int response,
-	      BOOL need_response, BOOL for_visorbus)
+	      bool need_response, bool for_visorbus)
 {
-	struct visorchipset_busdev_notifiers *notifiers = NULL;
-	BOOL notified = FALSE;
+	struct visorchipset_busdev_notifiers *notifiers;
+	bool notified = false;
+	struct controlvm_message_header *pmsg_hdr = NULL;
 
-	struct visorchipset_device_info *dev_info =
-		finddevice(&dev_info_list, bus_no, dev_no);
-	char *envp[] = {
-		"SPARSP_DIAGPOOL_PAUSED_STATE = 1",
-		NULL
-	};
+	notifiers = &busdev_notifiers;
 
-	if (!dev_info)
-		return;
+	if (!dev_info) {
+		/* relying on a valid passed in response code */
+		/* be lazy and re-use msg_hdr for this failure, is this ok?? */
+		pmsg_hdr = msg_hdr;
+		goto away;
+	}
+
+	if (dev_info->pending_msg_hdr) {
+		/* only non-NULL if dev is still waiting on a response */
+		response = -CONTROLVM_RESP_ERROR_MESSAGE_ID_INVALID_FOR_CLIENT;
+		pmsg_hdr = dev_info->pending_msg_hdr;
+		goto away;
+	}
 
-	if (for_visorbus)
-		notifiers = &busdev_server_notifiers;
-	else
-		notifiers = &busdev_client_notifiers;
 	if (need_response) {
-		memcpy(&dev_info->pending_msg_hdr, msg_hdr,
+		pmsg_hdr = kzalloc(sizeof(*pmsg_hdr), GFP_KERNEL);
+		if (!pmsg_hdr) {
+			response = -CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
+			goto away;
+		}
+
+		memcpy(pmsg_hdr, msg_hdr,
 		       sizeof(struct controlvm_message_header));
-	} else {
-		dev_info->pending_msg_hdr.id = CONTROLVM_INVALID;
+		dev_info->pending_msg_hdr = pmsg_hdr;
 	}
 
 	down(&notifier_lock);
@@ -969,8 +1031,8 @@ device_epilog(u32 bus_no, u32 dev_no, struct spar_segment_state state, u32 cmd,
 		switch (cmd) {
 		case CONTROLVM_DEVICE_CREATE:
 			if (notifiers->device_create) {
-				(*notifiers->device_create) (bus_no, dev_no);
-				notified = TRUE;
+				(*notifiers->device_create) (dev_info);
+				notified = true;
 			}
 			break;
 		case CONTROLVM_DEVICE_CHANGESTATE:
@@ -979,9 +1041,8 @@ device_epilog(u32 bus_no, u32 dev_no, struct spar_segment_state state, u32 cmd,
 			    state.operating ==
 				segment_state_running.operating) {
 				if (notifiers->device_resume) {
-					(*notifiers->device_resume) (bus_no,
-								     dev_no);
-					notified = TRUE;
+					(*notifiers->device_resume) (dev_info);
+					notified = true;
 				}
 			}
 			/* ServerNotReady / ServerLost / SegmentStateStandby */
@@ -992,35 +1053,20 @@ device_epilog(u32 bus_no, u32 dev_no, struct spar_segment_state state, u32 cmd,
 				 * where server is lost
 				 */
 				if (notifiers->device_pause) {
-					(*notifiers->device_pause) (bus_no,
-								    dev_no);
-					notified = TRUE;
-				}
-			} 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
-				 */
-				if (bus_no == g_diagpool_bus_no &&
-				    dev_no == g_diagpool_dev_no) {
-					/* this will trigger the
-					 * diag_shutdown.sh script in
-					 * the visorchipset hotplug */
-					kobject_uevent_env
-					    (&visorchipset_platform_device.dev.
-					     kobj, KOBJ_ONLINE, envp);
+					(*notifiers->device_pause) (dev_info);
+					notified = true;
 				}
 			}
 			break;
 		case CONTROLVM_DEVICE_DESTROY:
 			if (notifiers->device_destroy) {
-				(*notifiers->device_destroy) (bus_no, dev_no);
-				notified = TRUE;
+				(*notifiers->device_destroy) (dev_info);
+				notified = true;
 			}
 			break;
 		}
 	}
+away:
 	if (notified)
 		/* The callback function just called above is responsible
 		 * for calling the appropriate visorchipset_busdev_responders
@@ -1028,7 +1074,12 @@ device_epilog(u32 bus_no, u32 dev_no, struct spar_segment_state state, u32 cmd,
 		 */
 		;
 	else
-		device_responder(cmd, bus_no, dev_no, response);
+		/*
+		 * Do not kfree(pmsg_hdr) as this is the failure path.
+		 * The success path ('notified') will call the responder
+		 * directly and kfree() there.
+		 */
+		device_responder(cmd, pmsg_hdr, response);
 	up(&notifier_lock);
 }
 
@@ -1036,11 +1087,12 @@ static void
 bus_create(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong bus_no = cmd->create_bus.bus_no;
+	u32 bus_no = cmd->create_bus.bus_no;
 	int rc = CONTROLVM_RESP_SUCCESS;
-	struct visorchipset_bus_info *bus_info = NULL;
+	struct visor_device *bus_info;
+	struct visorchannel *visorchannel;
 
-	bus_info = findbus(&bus_info_list, bus_no);
+	bus_info = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
 	if (bus_info && (bus_info->state.created == 1)) {
 		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
@@ -1055,30 +1107,31 @@ bus_create(struct controlvm_message *inmsg)
 		goto cleanup;
 	}
 
-	INIT_LIST_HEAD(&bus_info->entry);
-	bus_info->bus_no = bus_no;
-	bus_info->dev_no = cmd->create_bus.dev_count;
+	INIT_LIST_HEAD(&bus_info->list_all);
+	bus_info->chipset_bus_no = bus_no;
+	bus_info->chipset_dev_no = BUS_ROOT_DEVICE;
 
 	POSTCODE_LINUX_3(BUS_CREATE_ENTRY_PC, bus_no, POSTCODE_SEVERITY_INFO);
 
-	if (inmsg->hdr.flags.test_message == 1)
-		bus_info->chan_info.addr_type = ADDRTYPE_LOCALTEST;
-	else
-		bus_info->chan_info.addr_type = ADDRTYPE_LOCALPHYSICAL;
-
-	bus_info->flags.server = inmsg->hdr.flags.server;
-	bus_info->chan_info.channel_addr = cmd->create_bus.channel_addr;
-	bus_info->chan_info.n_channel_bytes = cmd->create_bus.channel_bytes;
-	bus_info->chan_info.channel_type_uuid =
-			cmd->create_bus.bus_data_type_uuid;
-	bus_info->chan_info.channel_inst_uuid = cmd->create_bus.bus_inst_uuid;
+	visorchannel = visorchannel_create(cmd->create_bus.channel_addr,
+					   cmd->create_bus.channel_bytes,
+					   GFP_KERNEL,
+					   cmd->create_bus.bus_data_type_uuid);
 
-	list_add(&bus_info->entry, &bus_info_list);
+	if (!visorchannel) {
+		POSTCODE_LINUX_3(BUS_CREATE_FAILURE_PC, bus_no,
+				 POSTCODE_SEVERITY_ERR);
+		rc = -CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
+		kfree(bus_info);
+		bus_info = NULL;
+		goto cleanup;
+	}
+	bus_info->visorchannel = visorchannel;
 
 	POSTCODE_LINUX_3(BUS_CREATE_EXIT_PC, bus_no, POSTCODE_SEVERITY_INFO);
 
 cleanup:
-	bus_epilog(bus_no, CONTROLVM_BUS_CREATE, &inmsg->hdr,
+	bus_epilog(bus_info, CONTROLVM_BUS_CREATE, &inmsg->hdr,
 		   rc, inmsg->hdr.flags.response_expected == 1);
 }
 
@@ -1086,18 +1139,20 @@ static void
 bus_destroy(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong bus_no = cmd->destroy_bus.bus_no;
-	struct visorchipset_bus_info *bus_info;
+	u32 bus_no = cmd->destroy_bus.bus_no;
+	struct visor_device *bus_info;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	bus_info = findbus(&bus_info_list, bus_no);
+	bus_info = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
 	if (!bus_info)
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
 	else if (bus_info->state.created == 0)
 		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
 
-	bus_epilog(bus_no, CONTROLVM_BUS_DESTROY, &inmsg->hdr,
+	bus_epilog(bus_info, CONTROLVM_BUS_DESTROY, &inmsg->hdr,
 		   rc, inmsg->hdr.flags.response_expected == 1);
+
+	/* bus_info is freed as part of the busdevice_release function */
 }
 
 static void
@@ -1105,16 +1160,15 @@ bus_configure(struct controlvm_message *inmsg,
 	      struct parser_context *parser_ctx)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong bus_no = cmd->configure_bus.bus_no;
-	struct visorchipset_bus_info *bus_info = NULL;
+	u32 bus_no;
+	struct visor_device *bus_info;
 	int rc = CONTROLVM_RESP_SUCCESS;
-	char s[99];
 
 	bus_no = cmd->configure_bus.bus_no;
 	POSTCODE_LINUX_3(BUS_CONFIGURE_ENTRY_PC, bus_no,
 			 POSTCODE_SEVERITY_INFO);
 
-	bus_info = findbus(&bus_info_list, bus_no);
+	bus_info = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
 	if (!bus_info) {
 		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
@@ -1123,21 +1177,21 @@ bus_configure(struct controlvm_message *inmsg,
 		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
-	} else if (bus_info->pending_msg_hdr.id != CONTROLVM_INVALID) {
+	} else if (bus_info->pending_msg_hdr != NULL) {
 		POSTCODE_LINUX_3(BUS_CONFIGURE_FAILURE_PC, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_MESSAGE_ID_INVALID_FOR_CLIENT;
 	} else {
-		bus_info->partition_handle = cmd->configure_bus.guest_handle;
+		visorchannel_set_clientpartition(bus_info->visorchannel,
+				cmd->configure_bus.guest_handle);
 		bus_info->partition_uuid = parser_id_get(parser_ctx);
 		parser_param_start(parser_ctx, PARSERSTRING_NAME);
 		bus_info->name = parser_string_get(parser_ctx);
 
-		visorchannel_uuid_id(&bus_info->partition_uuid, s);
 		POSTCODE_LINUX_3(BUS_CONFIGURE_EXIT_PC, bus_no,
 				 POSTCODE_SEVERITY_INFO);
 	}
-	bus_epilog(bus_no, CONTROLVM_BUS_CONFIGURE, &inmsg->hdr,
+	bus_epilog(bus_info, CONTROLVM_BUS_CONFIGURE, &inmsg->hdr,
 		   rc, inmsg->hdr.flags.response_expected == 1);
 }
 
@@ -1145,32 +1199,36 @@ static void
 my_device_create(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong bus_no = cmd->create_device.bus_no;
-	ulong dev_no = cmd->create_device.dev_no;
-	struct visorchipset_device_info *dev_info = NULL;
-	struct visorchipset_bus_info *bus_info = NULL;
+	u32 bus_no = cmd->create_device.bus_no;
+	u32 dev_no = cmd->create_device.dev_no;
+	struct visor_device *dev_info = NULL;
+	struct visor_device *bus_info;
+	struct visorchannel *visorchannel;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	dev_info = finddevice(&dev_info_list, bus_no, dev_no);
-	if (dev_info && (dev_info->state.created == 1)) {
+	bus_info = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
+	if (!bus_info) {
 		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
-		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
+		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
 		goto cleanup;
 	}
-	bus_info = findbus(&bus_info_list, bus_no);
-	if (!bus_info) {
+
+	if (bus_info->state.created == 0) {
 		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
 		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
 		goto cleanup;
 	}
-	if (bus_info->state.created == 0) {
+
+	dev_info = visorbus_get_device_by_id(bus_no, dev_no, NULL);
+	if (dev_info && (dev_info->state.created == 1)) {
 		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
-		rc = -CONTROLVM_RESP_ERROR_BUS_INVALID;
+		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
 		goto cleanup;
 	}
+
 	dev_info = kzalloc(sizeof(*dev_info), GFP_KERNEL);
 	if (!dev_info) {
 		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
@@ -1179,49 +1237,50 @@ my_device_create(struct controlvm_message *inmsg)
 		goto cleanup;
 	}
 
-	INIT_LIST_HEAD(&dev_info->entry);
-	dev_info->bus_no = bus_no;
-	dev_info->dev_no = dev_no;
-	dev_info->dev_inst_uuid = cmd->create_device.dev_inst_uuid;
+	dev_info->chipset_bus_no = bus_no;
+	dev_info->chipset_dev_no = dev_no;
+	dev_info->inst = cmd->create_device.dev_inst_uuid;
+
+	/* not sure where the best place to set the 'parent' */
+	dev_info->device.parent = &bus_info->device;
+
 	POSTCODE_LINUX_4(DEVICE_CREATE_ENTRY_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_INFO);
 
-	if (inmsg->hdr.flags.test_message == 1)
-		dev_info->chan_info.addr_type = ADDRTYPE_LOCALTEST;
-	else
-		dev_info->chan_info.addr_type = ADDRTYPE_LOCALPHYSICAL;
-	dev_info->chan_info.channel_addr = cmd->create_device.channel_addr;
-	dev_info->chan_info.n_channel_bytes = cmd->create_device.channel_bytes;
-	dev_info->chan_info.channel_type_uuid =
-			cmd->create_device.data_type_uuid;
-	dev_info->chan_info.intr = cmd->create_device.intr;
-	list_add(&dev_info->entry, &dev_info_list);
+	visorchannel = visorchannel_create(cmd->create_device.channel_addr,
+					   cmd->create_device.channel_bytes,
+					   GFP_KERNEL,
+					   cmd->create_device.data_type_uuid);
+
+	if (!visorchannel) {
+		POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
+				 POSTCODE_SEVERITY_ERR);
+		rc = -CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
+		kfree(dev_info);
+		dev_info = NULL;
+		goto cleanup;
+	}
+	dev_info->visorchannel = visorchannel;
+	dev_info->channel_type_guid = cmd->create_device.data_type_uuid;
 	POSTCODE_LINUX_4(DEVICE_CREATE_EXIT_PC, dev_no, bus_no,
 			 POSTCODE_SEVERITY_INFO);
 cleanup:
-	/* get the bus and devNo for DiagPool channel */
-	if (dev_info &&
-	    is_diagpool_channel(dev_info->chan_info.channel_type_uuid)) {
-		g_diagpool_bus_no = bus_no;
-		g_diagpool_dev_no = dev_no;
-	}
-	device_epilog(bus_no, dev_no, segment_state_running,
+	device_epilog(dev_info, segment_state_running,
 		      CONTROLVM_DEVICE_CREATE, &inmsg->hdr, rc,
-		      inmsg->hdr.flags.response_expected == 1,
-		      FOR_VISORBUS(dev_info->chan_info.channel_type_uuid));
+		      inmsg->hdr.flags.response_expected == 1, 1);
 }
 
 static void
 my_device_changestate(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong bus_no = cmd->device_change_state.bus_no;
-	ulong dev_no = cmd->device_change_state.dev_no;
+	u32 bus_no = cmd->device_change_state.bus_no;
+	u32 dev_no = cmd->device_change_state.dev_no;
 	struct spar_segment_state state = cmd->device_change_state.state;
-	struct visorchipset_device_info *dev_info = NULL;
+	struct visor_device *dev_info;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	dev_info = finddevice(&dev_info_list, bus_no, dev_no);
+	dev_info = visorbus_get_device_by_id(bus_no, dev_no, NULL);
 	if (!dev_info) {
 		POSTCODE_LINUX_4(DEVICE_CHANGESTATE_FAILURE_PC, dev_no, bus_no,
 				 POSTCODE_SEVERITY_ERR);
@@ -1232,45 +1291,41 @@ my_device_changestate(struct controlvm_message *inmsg)
 		rc = -CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 	}
 	if ((rc >= CONTROLVM_RESP_SUCCESS) && dev_info)
-		device_epilog(bus_no, dev_no, state,
+		device_epilog(dev_info, state,
 			      CONTROLVM_DEVICE_CHANGESTATE, &inmsg->hdr, rc,
-			      inmsg->hdr.flags.response_expected == 1,
-			      FOR_VISORBUS(
-					dev_info->chan_info.channel_type_uuid));
+			      inmsg->hdr.flags.response_expected == 1, 1);
 }
 
 static void
 my_device_destroy(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
-	ulong bus_no = cmd->destroy_device.bus_no;
-	ulong dev_no = cmd->destroy_device.dev_no;
-	struct visorchipset_device_info *dev_info = NULL;
+	u32 bus_no = cmd->destroy_device.bus_no;
+	u32 dev_no = cmd->destroy_device.dev_no;
+	struct visor_device *dev_info;
 	int rc = CONTROLVM_RESP_SUCCESS;
 
-	dev_info = finddevice(&dev_info_list, bus_no, dev_no);
+	dev_info = visorbus_get_device_by_id(bus_no, dev_no, NULL);
 	if (!dev_info)
 		rc = -CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 	else if (dev_info->state.created == 0)
 		rc = -CONTROLVM_RESP_ERROR_ALREADY_DONE;
 
 	if ((rc >= CONTROLVM_RESP_SUCCESS) && dev_info)
-		device_epilog(bus_no, dev_no, segment_state_running,
+		device_epilog(dev_info, segment_state_running,
 			      CONTROLVM_DEVICE_DESTROY, &inmsg->hdr, rc,
-			      inmsg->hdr.flags.response_expected == 1,
-			      FOR_VISORBUS(
-					dev_info->chan_info.channel_type_uuid));
+			      inmsg->hdr.flags.response_expected == 1, 1);
 }
 
 /* When provided with the physical address of the controlvm channel
  * (phys_addr), the offset to the payload area we need to manage
  * (offset), and the size of this payload area (bytes), fills in the
- * controlvm_payload_info struct.  Returns TRUE for success or FALSE
+ * controlvm_payload_info struct.  Returns true for success or false
  * for failure.
  */
 static int
-initialize_controlvm_payload_info(HOSTADDRESS phys_addr, u64 offset, u32 bytes,
-				  struct controlvm_payload_info *info)
+initialize_controlvm_payload_info(u64 phys_addr, u64 offset, u32 bytes,
+				  struct visor_controlvm_payload_info *info)
 {
 	u8 __iomem *payload = NULL;
 	int rc = CONTROLVM_RESP_SUCCESS;
@@ -1279,7 +1334,7 @@ initialize_controlvm_payload_info(HOSTADDRESS phys_addr, u64 offset, u32 bytes,
 		rc = -CONTROLVM_RESP_ERROR_PAYLOAD_INVALID;
 		goto cleanup;
 	}
-	memset(info, 0, sizeof(struct controlvm_payload_info));
+	memset(info, 0, sizeof(struct visor_controlvm_payload_info));
 	if ((offset == 0) || (bytes == 0)) {
 		rc = -CONTROLVM_RESP_ERROR_PAYLOAD_INVALID;
 		goto cleanup;
@@ -1305,19 +1360,19 @@ cleanup:
 }
 
 static void
-destroy_controlvm_payload_info(struct controlvm_payload_info *info)
+destroy_controlvm_payload_info(struct visor_controlvm_payload_info *info)
 {
 	if (info->ptr) {
 		iounmap(info->ptr);
 		info->ptr = NULL;
 	}
-	memset(info, 0, sizeof(struct controlvm_payload_info));
+	memset(info, 0, sizeof(struct visor_controlvm_payload_info));
 }
 
 static void
 initialize_controlvm_payload(void)
 {
-	HOSTADDRESS phys_addr = visorchannel_get_physaddr(controlvm_channel);
+	u64 phys_addr = visorchannel_get_physaddr(controlvm_channel);
 	u64 payload_offset = 0;
 	u32 payload_bytes = 0;
 
@@ -1345,15 +1400,14 @@ initialize_controlvm_payload(void)
 /*  Send ACTION=online for DEVPATH=/sys/devices/platform/visorchipset.
  *  Returns CONTROLVM_RESP_xxx code.
  */
-int
+static int
 visorchipset_chipset_ready(void)
 {
 	kobject_uevent(&visorchipset_platform_device.dev.kobj, KOBJ_ONLINE);
 	return CONTROLVM_RESP_SUCCESS;
 }
-EXPORT_SYMBOL_GPL(visorchipset_chipset_ready);
 
-int
+static int
 visorchipset_chipset_selftest(void)
 {
 	char env_selftest[20];
@@ -1364,18 +1418,16 @@ visorchipset_chipset_selftest(void)
 			   envp);
 	return CONTROLVM_RESP_SUCCESS;
 }
-EXPORT_SYMBOL_GPL(visorchipset_chipset_selftest);
 
 /*  Send ACTION=offline for DEVPATH=/sys/devices/platform/visorchipset.
  *  Returns CONTROLVM_RESP_xxx code.
  */
-int
+static int
 visorchipset_chipset_notready(void)
 {
 	kobject_uevent(&visorchipset_platform_device.dev.kobj, KOBJ_OFFLINE);
 	return CONTROLVM_RESP_SUCCESS;
 }
-EXPORT_SYMBOL_GPL(visorchipset_chipset_notready);
 
 static void
 chipset_ready(struct controlvm_message_header *msg_hdr)
@@ -1419,17 +1471,17 @@ chipset_notready(struct controlvm_message_header *msg_hdr)
 /* This is your "one-stop" shop for grabbing the next message from the
  * CONTROLVM_QUEUE_EVENT queue in the controlvm channel.
  */
-static BOOL
+static bool
 read_controlvm_event(struct controlvm_message *msg)
 {
 	if (visorchannel_signalremove(controlvm_channel,
 				      CONTROLVM_QUEUE_EVENT, msg)) {
 		/* got a message */
 		if (msg->hdr.flags.test_message == 1)
-			return FALSE;
-		return TRUE;
+			return false;
+		return true;
 	}
-	return FALSE;
+	return false;
 }
 
 /*
@@ -1535,8 +1587,8 @@ parahotplug_request_kickoff(struct parahotplug_request *req)
 static void
 parahotplug_process_list(void)
 {
-	struct list_head *pos = NULL;
-	struct list_head *tmp = NULL;
+	struct list_head *pos;
+	struct list_head *tmp;
 
 	spin_lock(&parahotplug_request_list_lock);
 
@@ -1567,8 +1619,8 @@ parahotplug_process_list(void)
 static int
 parahotplug_request_complete(int id, u16 active)
 {
-	struct list_head *pos = NULL;
-	struct list_head *tmp = NULL;
+	struct list_head *pos;
+	struct list_head *tmp;
 
 	spin_lock(&parahotplug_request_list_lock);
 
@@ -1640,29 +1692,29 @@ parahotplug_process_message(struct controlvm_message *inmsg)
 
 /* Process a controlvm message.
  * Return result:
- *    FALSE - this function will return FALSE only in the case where the
+ *    false - this function will return false only in the case where the
  *            controlvm message was NOT processed, but processing must be
  *            retried before reading the next controlvm message; a
  *            scenario where this can occur is when we need to throttle
  *            the allocation of memory in which to copy out controlvm
  *            payload data
- *    TRUE  - processing of the controlvm message completed,
+ *    true  - processing of the controlvm message completed,
  *            either successfully or with an error.
  */
-static BOOL
-handle_command(struct controlvm_message inmsg, HOSTADDRESS channel_addr)
+static bool
+handle_command(struct controlvm_message inmsg, u64 channel_addr)
 {
 	struct controlvm_message_packet *cmd = &inmsg.cmd;
-	u64 parm_addr = 0;
-	u32 parm_bytes = 0;
+	u64 parm_addr;
+	u32 parm_bytes;
 	struct parser_context *parser_ctx = NULL;
-	bool local_addr = false;
+	bool local_addr;
 	struct controlvm_message ackmsg;
 
 	/* create parsing context if necessary */
 	local_addr = (inmsg.hdr.flags.test_message == 1);
 	if (channel_addr == 0)
-		return TRUE;
+		return true;
 	parm_addr = channel_addr + inmsg.hdr.payload_vm_offset;
 	parm_bytes = inmsg.hdr.payload_bytes;
 
@@ -1670,14 +1722,14 @@ handle_command(struct controlvm_message inmsg, HOSTADDRESS channel_addr)
 	 * within our OS-controlled memory.  We need to know that, because it
 	 * makes a difference in how we compute the virtual address.
 	 */
-	if (parm_addr != 0 && parm_bytes != 0) {
-		BOOL retry = FALSE;
+	if (parm_addr && parm_bytes) {
+		bool retry = false;
 
 		parser_ctx =
 		    parser_init_byte_stream(parm_addr, parm_bytes,
 					    local_addr, &retry);
 		if (!parser_ctx && retry)
-			return FALSE;
+			return false;
 	}
 
 	if (!local_addr) {
@@ -1711,7 +1763,6 @@ handle_command(struct controlvm_message inmsg, HOSTADDRESS channel_addr)
 			/* save the hdr and cmd structures for later use */
 			/* when sending back the response to Command */
 			my_device_changestate(&inmsg);
-			g_diag_msg_hdr = inmsg.hdr;
 			g_devicechangestate_packet = inmsg.cmd;
 			break;
 		}
@@ -1744,10 +1795,26 @@ handle_command(struct controlvm_message inmsg, HOSTADDRESS channel_addr)
 		parser_done(parser_ctx);
 		parser_ctx = NULL;
 	}
-	return TRUE;
+	return true;
 }
 
-static HOSTADDRESS controlvm_get_channel_address(void)
+static inline unsigned int
+issue_vmcall_io_controlvm_addr(u64 *control_addr, u32 *control_bytes)
+{
+	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.address;
+		*control_bytes = params.channel_bytes;
+	}
+	return result;
+}
+
+static u64 controlvm_get_channel_address(void)
 {
 	u64 addr = 0;
 	u32 size = 0;
@@ -1762,17 +1829,12 @@ static void
 controlvm_periodic_work(struct work_struct *work)
 {
 	struct controlvm_message inmsg;
-	BOOL got_command = FALSE;
-	BOOL handle_command_failed = FALSE;
+	bool got_command = false;
+	bool handle_command_failed = false;
 	static u64 poll_count;
 
 	/* make sure visorbus server is registered for controlvm callbacks */
-	if (visorchipset_serverregwait && !serverregistered)
-		goto cleanup;
-	/* make sure visorclientbus server is regsitered for controlvm
-	 * callbacks
-	 */
-	if (visorchipset_clientregwait && !clientregistered)
+	if (visorchipset_visorbusregwait && !visorbusregistered)
 		goto cleanup;
 
 	poll_count++;
@@ -1805,14 +1867,14 @@ controlvm_periodic_work(struct work_struct *work)
 			* rather than reading a new one
 			*/
 			inmsg = controlvm_pending_msg;
-			controlvm_pending_msg_valid = FALSE;
+			controlvm_pending_msg_valid = false;
 			got_command = true;
 		} else {
 			got_command = read_controlvm_event(&inmsg);
 		}
 	}
 
-	handle_command_failed = FALSE;
+	handle_command_failed = false;
 	while (got_command && (!handle_command_failed)) {
 		most_recent_message_jiffies = jiffies;
 		if (handle_command(inmsg,
@@ -1826,9 +1888,9 @@ controlvm_periodic_work(struct work_struct *work)
 			* controlvm msg so we will attempt to
 			* reprocess it on our next loop
 			*/
-			handle_command_failed = TRUE;
+			handle_command_failed = true;
 			controlvm_pending_msg = inmsg;
-			controlvm_pending_msg_valid = TRUE;
+			controlvm_pending_msg_valid = true;
 		}
 	}
 
@@ -1863,14 +1925,8 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	u32 local_crash_msg_offset;
 	u16 local_crash_msg_count;
 
-	/* make sure visorbus server is registered for controlvm callbacks */
-	if (visorchipset_serverregwait && !serverregistered)
-		goto cleanup;
-
-	/* make sure visorclientbus server is regsitered for controlvm
-	 * callbacks
-	 */
-	if (visorchipset_clientregwait && !clientregistered)
+	/* make sure visorbus is registered for controlvm callbacks */
+	if (visorchipset_visorbusregwait && !visorbusregistered)
 		goto cleanup;
 
 	POSTCODE_LINUX_2(CRASH_DEV_ENTRY_PC, POSTCODE_SEVERITY_INFO);
@@ -1931,7 +1987,7 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	}
 
 	/* reuse IOVM create bus message */
-	if (local_crash_bus_msg.cmd.create_bus.channel_addr != 0) {
+	if (local_crash_bus_msg.cmd.create_bus.channel_addr) {
 		bus_create(&local_crash_bus_msg);
 	} else {
 		POSTCODE_LINUX_2(CRASH_DEV_BUS_NULL_FAILURE_PC,
@@ -1940,7 +1996,7 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	}
 
 	/* reuse create device message for storage device */
-	if (local_crash_dev_msg.cmd.create_device.channel_addr != 0) {
+	if (local_crash_dev_msg.cmd.create_device.channel_addr) {
 		my_device_create(&local_crash_dev_msg);
 	} else {
 		POSTCODE_LINUX_2(CRASH_DEV_DEV_NULL_FAILURE_PC,
@@ -1959,135 +2015,71 @@ cleanup:
 }
 
 static void
-bus_create_response(ulong bus_no, int response)
+bus_create_response(struct visor_device *bus_info, int response)
 {
-	bus_responder(CONTROLVM_BUS_CREATE, bus_no, response);
-}
+	if (response >= 0)
+		bus_info->state.created = 1;
 
-static void
-bus_destroy_response(ulong bus_no, int response)
-{
-	bus_responder(CONTROLVM_BUS_DESTROY, bus_no, response);
-}
+	bus_responder(CONTROLVM_BUS_CREATE, bus_info->pending_msg_hdr,
+		      response);
 
-static void
-device_create_response(ulong bus_no, ulong dev_no, int response)
-{
-	device_responder(CONTROLVM_DEVICE_CREATE, bus_no, dev_no, response);
+	kfree(bus_info->pending_msg_hdr);
+	bus_info->pending_msg_hdr = NULL;
 }
 
 static void
-device_destroy_response(ulong bus_no, ulong dev_no, int response)
+bus_destroy_response(struct visor_device *bus_info, int response)
 {
-	device_responder(CONTROLVM_DEVICE_DESTROY, bus_no, dev_no, response);
-}
+	bus_responder(CONTROLVM_BUS_DESTROY, bus_info->pending_msg_hdr,
+		      response);
 
-void
-visorchipset_device_pause_response(ulong bus_no, ulong dev_no, int response)
-{
-	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
-				     bus_no, dev_no, response,
-				     segment_state_standby);
+	kfree(bus_info->pending_msg_hdr);
+	bus_info->pending_msg_hdr = NULL;
 }
-EXPORT_SYMBOL_GPL(visorchipset_device_pause_response);
 
 static void
-device_resume_response(ulong bus_no, ulong dev_no, int response)
-{
-	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
-				     bus_no, dev_no, response,
-				     segment_state_running);
-}
-
-BOOL
-visorchipset_get_bus_info(ulong bus_no, struct visorchipset_bus_info *bus_info)
-{
-	void *p = findbus(&bus_info_list, bus_no);
-
-	if (!p)
-		return FALSE;
-	memcpy(bus_info, p, sizeof(struct visorchipset_bus_info));
-	return TRUE;
-}
-EXPORT_SYMBOL_GPL(visorchipset_get_bus_info);
-
-BOOL
-visorchipset_set_bus_context(ulong bus_no, void *context)
+device_create_response(struct visor_device *dev_info, int response)
 {
-	struct visorchipset_bus_info *p = findbus(&bus_info_list, bus_no);
-
-	if (!p)
-		return FALSE;
-	p->bus_driver_context = context;
-	return TRUE;
-}
-EXPORT_SYMBOL_GPL(visorchipset_set_bus_context);
+	if (response >= 0)
+		dev_info->state.created = 1;
 
-BOOL
-visorchipset_get_device_info(ulong bus_no, ulong dev_no,
-			     struct visorchipset_device_info *dev_info)
-{
-	void *p = finddevice(&dev_info_list, bus_no, dev_no);
+	device_responder(CONTROLVM_DEVICE_CREATE, dev_info->pending_msg_hdr,
+			 response);
 
-	if (!p)
-		return FALSE;
-	memcpy(dev_info, p, sizeof(struct visorchipset_device_info));
-	return TRUE;
+	kfree(dev_info->pending_msg_hdr);
 }
-EXPORT_SYMBOL_GPL(visorchipset_get_device_info);
 
-BOOL
-visorchipset_set_device_context(ulong bus_no, ulong dev_no, void *context)
+static void
+device_destroy_response(struct visor_device *dev_info, int response)
 {
-	struct visorchipset_device_info *p =
-			finddevice(&dev_info_list, bus_no, dev_no);
+	device_responder(CONTROLVM_DEVICE_DESTROY, dev_info->pending_msg_hdr,
+			 response);
 
-	if (!p)
-		return FALSE;
-	p->bus_driver_context = context;
-	return TRUE;
+	kfree(dev_info->pending_msg_hdr);
+	dev_info->pending_msg_hdr = NULL;
 }
-EXPORT_SYMBOL_GPL(visorchipset_set_device_context);
 
-/* Generic wrapper function for allocating memory from a kmem_cache pool.
- */
-void *
-visorchipset_cache_alloc(struct kmem_cache *pool, BOOL ok_to_block,
-			 char *fn, int ln)
+static void
+visorchipset_device_pause_response(struct visor_device *dev_info,
+				   int response)
 {
-	gfp_t gfp;
-	void *p;
-
-	if (ok_to_block)
-		gfp = GFP_KERNEL;
-	else
-		gfp = GFP_ATOMIC;
-	/* __GFP_NORETRY means "ok to fail", meaning
-	 * kmem_cache_alloc() can return NULL, implying the caller CAN
-	 * cope with failure.  If you do NOT specify __GFP_NORETRY,
-	 * Linux will go to extreme measures to get memory for you
-	 * (like, invoke oom killer), which will probably cripple the
-	 * system.
-	 */
-	gfp |= __GFP_NORETRY;
-	p = kmem_cache_alloc(pool, gfp);
-	if (!p)
-		return NULL;
+	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
+				     dev_info, response,
+				     segment_state_standby);
 
-	atomic_inc(&visorchipset_cache_buffers_in_use);
-	return p;
+	kfree(dev_info->pending_msg_hdr);
+	dev_info->pending_msg_hdr = NULL;
 }
 
-/* Generic wrapper function for freeing memory from a kmem_cache pool.
- */
-void
-visorchipset_cache_free(struct kmem_cache *pool, void *p, char *fn, int ln)
+static void
+device_resume_response(struct visor_device *dev_info, int response)
 {
-	if (!p)
-		return;
+	device_changestate_responder(CONTROLVM_DEVICE_CHANGESTATE,
+				     dev_info, response,
+				     segment_state_running);
 
-	atomic_dec(&visorchipset_cache_buffers_in_use);
-	kmem_cache_free(pool, p);
+	kfree(dev_info->pending_msg_hdr);
+	dev_info->pending_msg_hdr = NULL;
 }
 
 static ssize_t chipsetready_store(struct device *dev,
@@ -2099,10 +2091,10 @@ static ssize_t chipsetready_store(struct device *dev,
 	if (sscanf(buf, "%63s", msgtype) != 1)
 		return -EINVAL;
 
-	if (strcmp(msgtype, "CALLHOMEDISK_MOUNTED") == 0) {
+	if (!strcmp(msgtype, "CALLHOMEDISK_MOUNTED")) {
 		chipset_events[0] = 1;
 		return count;
-	} else if (strcmp(msgtype, "MODULES_LOADED") == 0) {
+	} else if (!strcmp(msgtype, "MODULES_LOADED")) {
 		chipset_events[1] = 1;
 		return count;
 	}
@@ -2117,9 +2109,9 @@ static ssize_t devicedisabled_store(struct device *dev,
 				    struct device_attribute *attr,
 				    const char *buf, size_t count)
 {
-	uint id;
+	unsigned int id;
 
-	if (kstrtouint(buf, 10, &id) != 0)
+	if (kstrtouint(buf, 10, &id))
 		return -EINVAL;
 
 	parahotplug_request_complete(id, 0);
@@ -2134,52 +2126,158 @@ static ssize_t deviceenabled_store(struct device *dev,
 				   struct device_attribute *attr,
 				   const char *buf, size_t count)
 {
-	uint id;
+	unsigned int id;
 
-	if (kstrtouint(buf, 10, &id) != 0)
+	if (kstrtouint(buf, 10, &id))
 		return -EINVAL;
 
 	parahotplug_request_complete(id, 1);
 	return count;
 }
 
-static int __init
-visorchipset_init(void)
+static int
+visorchipset_mmap(struct file *file, struct vm_area_struct *vma)
 {
-	int rc = 0, x = 0;
-	HOSTADDRESS addr;
+	unsigned long physaddr = 0;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	u64 addr = 0;
 
-	if (!unisys_spar_platform)
-		return -ENODEV;
+	/* sv_enable_dfp(); */
+	if (offset & (PAGE_SIZE - 1))
+		return -ENXIO;	/* need aligned offsets */
 
-	memset(&busdev_server_notifiers, 0, sizeof(busdev_server_notifiers));
-	memset(&busdev_client_notifiers, 0, sizeof(busdev_client_notifiers));
-	memset(&controlvm_payload_info, 0, sizeof(controlvm_payload_info));
-	memset(&livedump_info, 0, sizeof(livedump_info));
-	atomic_set(&livedump_info.buffers_in_use, 0);
+	switch (offset) {
+	case VISORCHIPSET_MMAP_CONTROLCHANOFFSET:
+		vma->vm_flags |= VM_IO;
+		if (!*file_controlvm_channel)
+			return -ENXIO;
 
-	if (visorchipset_testvnic) {
-		POSTCODE_LINUX_3(CHIPSET_INIT_FAILURE_PC, x, DIAG_SEVERITY_ERR);
-		rc = x;
-		goto cleanup;
+		visorchannel_read(*file_controlvm_channel,
+			offsetof(struct spar_controlvm_channel_protocol,
+				 gp_control_channel),
+			&addr, sizeof(addr));
+		if (!addr)
+			return -ENXIO;
+
+		physaddr = (unsigned long)addr;
+		if (remap_pfn_range(vma, vma->vm_start,
+				    physaddr >> PAGE_SHIFT,
+				    vma->vm_end - vma->vm_start,
+				    /*pgprot_noncached */
+				    (vma->vm_page_prot))) {
+			return -EAGAIN;
+		}
+		break;
+	default:
+		return -ENXIO;
 	}
+	return 0;
+}
 
-	addr = controlvm_get_channel_address();
-	if (addr != 0) {
-		controlvm_channel =
-		    visorchannel_create_with_lock
-		    (addr,
-		     sizeof(struct spar_controlvm_channel_protocol),
-		     spar_controlvm_channel_protocol_uuid);
-		if (SPAR_CONTROLVM_CHANNEL_OK_CLIENT(
-				visorchannel_get_header(controlvm_channel))) {
-			initialize_controlvm_payload();
-		} else {
-			visorchannel_destroy(controlvm_channel);
-			controlvm_channel = NULL;
-			return -ENODEV;
+static inline s64 issue_vmcall_query_guest_virtual_time_offset(void)
+{
+	u64 result = VMCALL_SUCCESS;
+	u64 physaddr = 0;
+
+	ISSUE_IO_VMCALL(VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET, physaddr,
+			result);
+	return result;
+}
+
+static inline int issue_vmcall_update_physical_time(u64 adjustment)
+{
+	int result = VMCALL_SUCCESS;
+
+	ISSUE_IO_VMCALL(VMCALL_UPDATE_PHYSICAL_TIME, adjustment, result);
+	return result;
+}
+
+static long visorchipset_ioctl(struct file *file, unsigned int cmd,
+			       unsigned long arg)
+{
+	s64 adjustment;
+	s64 vrtc_offset;
+
+	switch (cmd) {
+	case VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET:
+		/* get the physical rtc offset */
+		vrtc_offset = issue_vmcall_query_guest_virtual_time_offset();
+		if (copy_to_user((void __user *)arg, &vrtc_offset,
+				 sizeof(vrtc_offset))) {
+			return -EFAULT;
+		}
+		return 0;
+	case VMCALL_UPDATE_PHYSICAL_TIME:
+		if (copy_from_user(&adjustment, (void __user *)arg,
+				   sizeof(adjustment))) {
+			return -EFAULT;
 		}
+		return issue_vmcall_update_physical_time(adjustment);
+	default:
+		return -EFAULT;
+	}
+}
+
+static const struct file_operations visorchipset_fops = {
+	.owner = THIS_MODULE,
+	.open = visorchipset_open,
+	.read = NULL,
+	.write = NULL,
+	.unlocked_ioctl = visorchipset_ioctl,
+	.release = visorchipset_release,
+	.mmap = visorchipset_mmap,
+};
+
+static int
+visorchipset_file_init(dev_t major_dev, struct visorchannel **controlvm_channel)
+{
+	int rc = 0;
+
+	file_controlvm_channel = controlvm_channel;
+	cdev_init(&file_cdev, &visorchipset_fops);
+	file_cdev.owner = THIS_MODULE;
+	if (MAJOR(major_dev) == 0) {
+		rc = alloc_chrdev_region(&major_dev, 0, 1, "visorchipset");
+		/* dynamic major device number registration required */
+		if (rc < 0)
+			return rc;
+	} else {
+		/* static major device number registration required */
+		rc = register_chrdev_region(major_dev, 1, "visorchipset");
+		if (rc < 0)
+			return rc;
+	}
+	rc = cdev_add(&file_cdev, MKDEV(MAJOR(major_dev), 0), 1);
+	if (rc < 0) {
+		unregister_chrdev_region(major_dev, 1);
+		return rc;
+	}
+	return 0;
+}
+
+static int
+visorchipset_init(struct acpi_device *acpi_device)
+{
+	int rc = 0;
+	u64 addr;
+	int tmp_sz = sizeof(struct spar_controlvm_channel_protocol);
+	uuid_le uuid = SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID;
+
+	addr = controlvm_get_channel_address();
+	if (!addr)
+		return -ENODEV;
+
+	memset(&busdev_notifiers, 0, sizeof(busdev_notifiers));
+	memset(&controlvm_payload_info, 0, sizeof(controlvm_payload_info));
+
+	controlvm_channel = visorchannel_create_with_lock(addr, tmp_sz,
+							  GFP_KERNEL, uuid);
+	if (SPAR_CONTROLVM_CHANNEL_OK_CLIENT(
+		    visorchannel_get_header(controlvm_channel))) {
+		initialize_controlvm_payload();
 	} else {
+		visorchannel_destroy(controlvm_channel);
+		controlvm_channel = NULL;
 		return -ENODEV;
 	}
 
@@ -2190,47 +2288,32 @@ visorchipset_init(void)
 		goto cleanup;
 	}
 
-	memset(&g_diag_msg_hdr, 0, sizeof(struct controlvm_message_header));
-
 	memset(&g_chipset_msg_hdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_del_dump_msg_hdr, 0, sizeof(struct controlvm_message_header));
-
-	putfile_buffer_list_pool =
-	    kmem_cache_create(putfile_buffer_list_pool_name,
-			      sizeof(struct putfile_buffer_entry),
-			      0, SLAB_HWCACHE_ALIGN, NULL);
-	if (!putfile_buffer_list_pool) {
-		POSTCODE_LINUX_2(CHIPSET_INIT_FAILURE_PC, DIAG_SEVERITY_ERR);
-		rc = -1;
+	/* if booting in a crash kernel */
+	if (is_kdump_kernel())
+		INIT_DELAYED_WORK(&periodic_controlvm_work,
+				  setup_crash_devices_work_queue);
+	else
+		INIT_DELAYED_WORK(&periodic_controlvm_work,
+				  controlvm_periodic_work);
+	periodic_controlvm_workqueue =
+	    create_singlethread_workqueue("visorchipset_controlvm");
+
+	if (!periodic_controlvm_workqueue) {
+		POSTCODE_LINUX_2(CREATE_WORKQUEUE_FAILED_PC,
+				 DIAG_SEVERITY_ERR);
+		rc = -ENOMEM;
 		goto cleanup;
 	}
-	if (!visorchipset_disable_controlvm) {
-		/* if booting in a crash kernel */
-		if (visorchipset_crash_kernel)
-			INIT_DELAYED_WORK(&periodic_controlvm_work,
-					  setup_crash_devices_work_queue);
-		else
-			INIT_DELAYED_WORK(&periodic_controlvm_work,
-					  controlvm_periodic_work);
-		periodic_controlvm_workqueue =
-		    create_singlethread_workqueue("visorchipset_controlvm");
-
-		if (!periodic_controlvm_workqueue) {
-			POSTCODE_LINUX_2(CREATE_WORKQUEUE_FAILED_PC,
-					 DIAG_SEVERITY_ERR);
-			rc = -ENOMEM;
-			goto cleanup;
-		}
-		most_recent_message_jiffies = jiffies;
-		poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
-		rc = queue_delayed_work(periodic_controlvm_workqueue,
-					&periodic_controlvm_work, poll_jiffies);
-		if (rc < 0) {
-			POSTCODE_LINUX_2(QUEUE_DELAYED_WORK_PC,
-					 DIAG_SEVERITY_ERR);
-			goto cleanup;
-		}
+	most_recent_message_jiffies = jiffies;
+	poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
+	rc = queue_delayed_work(periodic_controlvm_workqueue,
+				&periodic_controlvm_work, poll_jiffies);
+	if (rc < 0) {
+		POSTCODE_LINUX_2(QUEUE_DELAYED_WORK_PC,
+				 DIAG_SEVERITY_ERR);
+		goto cleanup;
 	}
 
 	visorchipset_platform_device.dev.devt = major_dev;
@@ -2240,7 +2323,8 @@ visorchipset_init(void)
 		goto cleanup;
 	}
 	POSTCODE_LINUX_2(CHIPSET_INIT_SUCCESS_PC, POSTCODE_SEVERITY_INFO);
-	rc = 0;
+
+	rc = visorbus_init();
 cleanup:
 	if (rc) {
 		POSTCODE_LINUX_3(CHIPSET_INIT_FAILURE_PC, rc,
@@ -2250,83 +2334,101 @@ cleanup:
 }
 
 static void
-visorchipset_exit(void)
+visorchipset_file_cleanup(dev_t major_dev)
 {
-	POSTCODE_LINUX_2(DRIVER_EXIT_PC, POSTCODE_SEVERITY_INFO);
+	if (file_cdev.ops)
+		cdev_del(&file_cdev);
+	file_cdev.ops = NULL;
+	unregister_chrdev_region(major_dev, 1);
+}
 
-	if (visorchipset_disable_controlvm) {
-		;
-	} else {
-		cancel_delayed_work(&periodic_controlvm_work);
-		flush_workqueue(periodic_controlvm_workqueue);
-		destroy_workqueue(periodic_controlvm_workqueue);
-		periodic_controlvm_workqueue = NULL;
-		destroy_controlvm_payload_info(&controlvm_payload_info);
-	}
-	if (putfile_buffer_list_pool) {
-		kmem_cache_destroy(putfile_buffer_list_pool);
-		putfile_buffer_list_pool = NULL;
-	}
+static int
+visorchipset_exit(struct acpi_device *acpi_device)
+{
+	POSTCODE_LINUX_2(DRIVER_EXIT_PC, POSTCODE_SEVERITY_INFO);
 
-	cleanup_controlvm_structures();
+	visorbus_exit();
 
-	memset(&g_diag_msg_hdr, 0, sizeof(struct controlvm_message_header));
+	cancel_delayed_work(&periodic_controlvm_work);
+	flush_workqueue(periodic_controlvm_workqueue);
+	destroy_workqueue(periodic_controlvm_workqueue);
+	periodic_controlvm_workqueue = NULL;
+	destroy_controlvm_payload_info(&controlvm_payload_info);
 
 	memset(&g_chipset_msg_hdr, 0, sizeof(struct controlvm_message_header));
 
-	memset(&g_del_dump_msg_hdr, 0, sizeof(struct controlvm_message_header));
-
 	visorchannel_destroy(controlvm_channel);
 
 	visorchipset_file_cleanup(visorchipset_platform_device.dev.devt);
+	platform_device_unregister(&visorchipset_platform_device);
 	POSTCODE_LINUX_2(DRIVER_EXIT_PC, POSTCODE_SEVERITY_INFO);
+
+	return 0;
 }
 
-module_param_named(testvnic, visorchipset_testvnic, int, S_IRUGO);
-MODULE_PARM_DESC(visorchipset_testvnic, "1 to test vnic, using dummy VNIC connected via a loopback to a physical ethernet");
-int visorchipset_testvnic = 0;
+static const struct acpi_device_id unisys_device_ids[] = {
+	{"PNP0A07", 0},
+	{"", 0},
+};
 
-module_param_named(testvnicclient, visorchipset_testvnicclient, int, S_IRUGO);
-MODULE_PARM_DESC(visorchipset_testvnicclient, "1 to test vnic, using real VNIC channel attached to a separate IOVM guest");
-int visorchipset_testvnicclient = 0;
+static struct acpi_driver unisys_acpi_driver = {
+	.name = "unisys_acpi",
+	.class = "unisys_acpi_class",
+	.owner = THIS_MODULE,
+	.ids = unisys_device_ids,
+	.ops = {
+		.add = visorchipset_init,
+		.remove = visorchipset_exit,
+		},
+};
+static __init uint32_t visorutil_spar_detect(void)
+{
+	unsigned int eax, ebx, ecx, edx;
 
-module_param_named(testmsg, visorchipset_testmsg, int, S_IRUGO);
-MODULE_PARM_DESC(visorchipset_testmsg,
-		 "1 to manufacture the chipset, bus, and switch messages");
-int visorchipset_testmsg = 0;
+	if (cpu_has_hypervisor) {
+		/* check the ID */
+		cpuid(UNISYS_SPAR_LEAF_ID, &eax, &ebx, &ecx, &edx);
+		return  (ebx == UNISYS_SPAR_ID_EBX) &&
+			(ecx == UNISYS_SPAR_ID_ECX) &&
+			(edx == UNISYS_SPAR_ID_EDX);
+	} else {
+		return 0;
+	}
+}
 
-module_param_named(major, visorchipset_major, int, S_IRUGO);
-MODULE_PARM_DESC(visorchipset_major, "major device number to use for the device node");
-int visorchipset_major = 0;
+static int init_unisys(void)
+{
+	int result;
+
+	if (!visorutil_spar_detect())
+		return -ENODEV;
+
+	result = acpi_bus_register_driver(&unisys_acpi_driver);
+	if (result)
+		return -ENODEV;
+
+	pr_info("Unisys Visorchipset Driver Loaded.\n");
+	return 0;
+};
+
+static void exit_unisys(void)
+{
+	acpi_bus_unregister_driver(&unisys_acpi_driver);
+}
 
-module_param_named(serverregwait, visorchipset_serverregwait, int, S_IRUGO);
-MODULE_PARM_DESC(visorchipset_serverreqwait,
+module_param_named(major, visorchipset_major, int, S_IRUGO);
+MODULE_PARM_DESC(visorchipset_major,
+		 "major device number to use for the device node");
+module_param_named(visorbusregwait, visorchipset_visorbusregwait, int, S_IRUGO);
+MODULE_PARM_DESC(visorchipset_visorbusreqwait,
 		 "1 to have the module wait for the visor bus to register");
-int visorchipset_serverregwait = 0;	/* default is off */
-module_param_named(clientregwait, visorchipset_clientregwait, int, S_IRUGO);
-MODULE_PARM_DESC(visorchipset_clientregwait, "1 to have the module wait for the visorclientbus to register");
-int visorchipset_clientregwait = 1;	/* default is on */
-module_param_named(testteardown, visorchipset_testteardown, int, S_IRUGO);
-MODULE_PARM_DESC(visorchipset_testteardown,
-		 "1 to test teardown of the chipset, bus, and switch");
-int visorchipset_testteardown = 0;	/* default is off */
-module_param_named(disable_controlvm, visorchipset_disable_controlvm, int,
-		   S_IRUGO);
-MODULE_PARM_DESC(visorchipset_disable_controlvm,
-		 "1 to disable polling of controlVm channel");
-int visorchipset_disable_controlvm = 0;	/* default is off */
-module_param_named(crash_kernel, visorchipset_crash_kernel, int, S_IRUGO);
-MODULE_PARM_DESC(visorchipset_crash_kernel,
-		 "1 means we are running in crash kernel");
-int visorchipset_crash_kernel = 0; /* default is running in non-crash kernel */
 module_param_named(holdchipsetready, visorchipset_holdchipsetready,
 		   int, S_IRUGO);
 MODULE_PARM_DESC(visorchipset_holdchipsetready,
 		 "1 to hold response to CHIPSET_READY");
-int visorchipset_holdchipsetready = 0; /* default is to send CHIPSET_READY
-				      * response immediately */
-module_init(visorchipset_init);
-module_exit(visorchipset_exit);
+
+module_init(init_unisys);
+module_exit(exit_unisys);
 
 MODULE_AUTHOR("Unisys");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/unisys/common-spar/include/vmcallinterface.h b/drivers/staging/unisys/visorbus/vmcallinterface.h
index 59a7459eb962..7a53df00726a 100644
--- a/drivers/staging/unisys/common-spar/include/vmcallinterface.h
+++ b/drivers/staging/unisys/visorbus/vmcallinterface.h
@@ -85,10 +85,8 @@ enum vmcall_monitor_interface_method_tuple { /* VMCALL identification tuples  */
     /* The following uses VMCALL_POST_CODE_LOGEVENT interface but is currently
      * not used much */
 #define ISSUE_IO_VMCALL_POSTCODE_SEVERITY(postcode, severity)		\
-do {									\
 	ISSUE_IO_EXTENDED_VMCALL(VMCALL_POST_CODE_LOGEVENT, severity,	\
-				 MDS_APPOS, postcode);			\
-} while (0)
+				 MDS_APPOS, postcode)
 #endif
 
 /* Structures for IO VMCALLs */
@@ -96,18 +94,6 @@ do {									\
 /* ///////////// BEGIN PRAGMA PACK PUSH 1 ///////////////////////// */
 /* ///////////// ONLY STRUCT TYPE SHOULD BE BELOW */
 #pragma pack(push, 1)
-struct phys_info {
-	u64 pi_pfn;
-	u16 pi_off;
-	u16 pi_len;
-};
-
-#pragma pack(pop)
-/* ///////////// END PRAGMA PACK PUSH 1 /////////////////////////// */
-
-/* ///////////// BEGIN PRAGMA PACK PUSH 1 ///////////////////////// */
-/* ///////////// ONLY STRUCT TYPE SHOULD BE BELOW */
-#pragma pack(push, 1)
 /* Parameters to VMCALL_IO_CONTROLVM_ADDR interface */
 struct vmcall_io_controlvm_addr_params {
 	    /* The Guest-relative physical address of the ControlVm channel.
diff --git a/drivers/staging/unisys/visorchannel/Kconfig b/drivers/staging/unisys/visorchannel/Kconfig
deleted file mode 100644
index 8d31bebf039a..000000000000
--- a/drivers/staging/unisys/visorchannel/Kconfig
+++ /dev/null
@@ -1,10 +0,0 @@
-#
-# Unisys visorchannel configuration
-#
-
-config UNISYS_VISORCHANNEL
-	tristate "Unisys visorchannel driver"
-	select UNISYS_VISORUTIL
-	---help---
-	If you say Y here, you will enable the Unisys visorchannel driver.
-
diff --git a/drivers/staging/unisys/visorchannel/Makefile b/drivers/staging/unisys/visorchannel/Makefile
deleted file mode 100644
index e079c96b1cdf..000000000000
--- a/drivers/staging/unisys/visorchannel/Makefile
+++ /dev/null
@@ -1,12 +0,0 @@
-#
-# Makefile for Unisys visorchannel
-#
-
-obj-$(CONFIG_UNISYS_VISORCHANNEL)	+= visorchannel.o
-
-visorchannel-y := visorchannel_main.o visorchannel_funcs.o
-
-ccflags-y += -Idrivers/staging/unisys/include
-ccflags-y += -Idrivers/staging/unisys/common-spar/include
-ccflags-y += -Idrivers/staging/unisys/common-spar/include/channels
-ccflags-y += -Idrivers/staging/unisys/visorutil
diff --git a/drivers/staging/unisys/visorchannel/globals.h b/drivers/staging/unisys/visorchannel/globals.h
deleted file mode 100644
index 0ed8e1d8033a..000000000000
--- a/drivers/staging/unisys/visorchannel/globals.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/* globals.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __VISORCHANNEL_GLOBALS_H__
-#define __VISORCHANNEL_GLOBALS_H__
-
-#include "timskmod.h"
-#include "memregion.h"
-#include "version.h"
-
-#define MYDRVNAME "visorchannel"
-
-#endif
diff --git a/drivers/staging/unisys/visorchannel/visorchannel.h b/drivers/staging/unisys/visorchannel/visorchannel.h
deleted file mode 100644
index 63f1b9760373..000000000000
--- a/drivers/staging/unisys/visorchannel/visorchannel.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/* visorchannel.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __VISORCHANNEL_H__
-#define __VISORCHANNEL_H__
-
-#include <linux/uuid.h>
-
-#include "memregion.h"
-#include "channel.h"
-#ifndef HOSTADDRESS
-#define HOSTADDRESS u64
-#endif
-#ifndef BOOL
-#define BOOL int
-#endif
-
-/* Note that for visorchannel_create() and visorchannel_create_overlapped(),
- * <channel_bytes> and <guid> arguments may be 0 if we are a channel CLIENT.
- * In this case, the values can simply be read from the channel header.
- */
-struct visorchannel *visorchannel_create(HOSTADDRESS physaddr,
-					 ulong channel_bytes, uuid_le guid);
-struct visorchannel *visorchannel_create_overlapped(ulong channel_bytes,
-						    struct visorchannel *parent,
-						    ulong off, uuid_le guid);
-struct visorchannel *visorchannel_create_with_lock(HOSTADDRESS physaddr,
-						   ulong channel_bytes,
-						   uuid_le guid);
-struct visorchannel *visorchannel_create_overlapped_with_lock(
-				ulong channel_bytes,
-				struct visorchannel *parent,
-				ulong off, uuid_le guid);
-void visorchannel_destroy(struct visorchannel *channel);
-int visorchannel_read(struct visorchannel *channel, ulong offset,
-		      void *local, ulong nbytes);
-int visorchannel_write(struct visorchannel *channel, ulong offset,
-		       void *local, ulong nbytes);
-int visorchannel_clear(struct visorchannel *channel, ulong offset,
-		       u8 ch, ulong nbytes);
-BOOL visorchannel_signalremove(struct visorchannel *channel, u32 queue,
-			       void *msg);
-BOOL visorchannel_signalinsert(struct visorchannel *channel, u32 queue,
-			       void *msg);
-int visorchannel_signalqueue_slots_avail(struct visorchannel *channel,
-					 u32 queue);
-int visorchannel_signalqueue_max_slots(struct visorchannel *channel, u32 queue);
-HOSTADDRESS visorchannel_get_physaddr(struct visorchannel *channel);
-ulong visorchannel_get_nbytes(struct visorchannel *channel);
-char *visorchannel_id(struct visorchannel *channel, char *s);
-char *visorchannel_zoneid(struct visorchannel *channel, char *s);
-u64 visorchannel_get_clientpartition(struct visorchannel *channel);
-uuid_le visorchannel_get_uuid(struct visorchannel *channel);
-struct memregion *visorchannel_get_memregion(struct visorchannel *channel);
-char *visorchannel_uuid_id(uuid_le *guid, char *s);
-void visorchannel_debug(struct visorchannel *channel, int num_queues,
-			struct seq_file *seq, u32 off);
-void visorchannel_dump_section(struct visorchannel *chan, char *s,
-			       int off, int len, struct seq_file *seq);
-void __iomem *visorchannel_get_header(struct visorchannel *channel);
-
-#endif
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_main.c b/drivers/staging/unisys/visorchannel/visorchannel_main.c
deleted file mode 100644
index 787d4774b199..000000000000
--- a/drivers/staging/unisys/visorchannel/visorchannel_main.c
+++ /dev/null
@@ -1,50 +0,0 @@
-/* visorchannel_main.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*
- *  This is a module "wrapper" around visorchannel_funcs.
- */
-
-#include "globals.h"
-#include "channel.h"
-#include "visorchannel.h"
-#include <linux/uuid.h>
-
-#define MYDRVNAME "visorchannel"
-
-static int __init
-visorchannel_init(void)
-{
-	if (!unisys_spar_platform)
-		return -ENODEV;
-
-	return 0;
-}
-
-static void
-visorchannel_exit(void)
-{
-}
-
-module_init(visorchannel_init);
-module_exit(visorchannel_exit);
-
-MODULE_AUTHOR("Unisys");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Supervisor channel driver for service partition: ver "
-		   VERSION);
-MODULE_VERSION(VERSION);
diff --git a/drivers/staging/unisys/visorchipset/Kconfig b/drivers/staging/unisys/visorchipset/Kconfig
deleted file mode 100644
index b03bfc5c3043..000000000000
--- a/drivers/staging/unisys/visorchipset/Kconfig
+++ /dev/null
@@ -1,11 +0,0 @@
-#
-# Unisys visorchipset configuration
-#
-
-config UNISYS_VISORCHIPSET
-	tristate "Unisys visorchipset driver"
-	select UNISYS_VISORUTIL
-	select UNISYS_VISORCHANNEL
-	---help---
-	If you say Y here, you will enable the Unisys visorchipset driver.
-
diff --git a/drivers/staging/unisys/visorchipset/Makefile b/drivers/staging/unisys/visorchipset/Makefile
deleted file mode 100644
index 12686906bef3..000000000000
--- a/drivers/staging/unisys/visorchipset/Makefile
+++ /dev/null
@@ -1,15 +0,0 @@
-#
-# Makefile for Unisys visorchipset
-#
-
-obj-$(CONFIG_UNISYS_VISORCHIPSET)	+= visorchipset.o
-
-visorchipset-y := visorchipset_main.o file.o parser.o
-
-ccflags-y += -Idrivers/staging/unisys/include
-ccflags-y += -Idrivers/staging/unisys/uislib
-ccflags-y += -Idrivers/staging/unisys/visorchannel
-ccflags-y += -Idrivers/staging/unisys/common-spar/include
-ccflags-y += -Idrivers/staging/unisys/common-spar/include/channels
-ccflags-y += -Idrivers/staging/unisys/visorutil
-ccflags-y += -Iinclude/generated
diff --git a/drivers/staging/unisys/visorchipset/file.c b/drivers/staging/unisys/visorchipset/file.c
deleted file mode 100644
index 203de0b5f607..000000000000
--- a/drivers/staging/unisys/visorchipset/file.c
+++ /dev/null
@@ -1,160 +0,0 @@
-/* file.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/* This contains the implementation that allows a usermode program to
- * communicate with the visorchipset driver using a device/file interface.
- */
-
-#include "globals.h"
-#include "visorchannel.h"
-#include <linux/mm.h>
-#include <linux/fs.h>
-#include "uisutils.h"
-#include "file.h"
-
-#define CURRENT_FILE_PC VISOR_CHIPSET_PC_file_c
-
-static struct cdev file_cdev;
-static struct visorchannel **file_controlvm_channel;
-
-void
-visorchipset_file_cleanup(dev_t major_dev)
-{
-	if (file_cdev.ops != NULL)
-		cdev_del(&file_cdev);
-	file_cdev.ops = NULL;
-	unregister_chrdev_region(major_dev, 1);
-}
-
-static int
-visorchipset_open(struct inode *inode, struct file *file)
-{
-	unsigned minor_number = iminor(inode);
-
-	if (minor_number != 0)
-		return -ENODEV;
-	file->private_data = NULL;
-	return 0;
-}
-
-static int
-visorchipset_release(struct inode *inode, struct file *file)
-{
-	return 0;
-}
-
-static int
-visorchipset_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	ulong physaddr = 0;
-	ulong offset = vma->vm_pgoff << PAGE_SHIFT;
-	GUEST_PHYSICAL_ADDRESS addr = 0;
-
-	/* sv_enable_dfp(); */
-	if (offset & (PAGE_SIZE - 1))
-		return -ENXIO;	/* need aligned offsets */
-
-	switch (offset) {
-	case VISORCHIPSET_MMAP_CONTROLCHANOFFSET:
-		vma->vm_flags |= VM_IO;
-		if (*file_controlvm_channel == NULL) {
-			return -ENXIO;
-		}
-		visorchannel_read(*file_controlvm_channel,
-			offsetof(struct spar_controlvm_channel_protocol,
-				 gp_control_channel),
-			&addr, sizeof(addr));
-		if (addr == 0) {
-			return -ENXIO;
-		}
-		physaddr = (ulong)addr;
-		if (remap_pfn_range(vma, vma->vm_start,
-				    physaddr >> PAGE_SHIFT,
-				    vma->vm_end - vma->vm_start,
-				    /*pgprot_noncached */
-				    (vma->vm_page_prot))) {
-			return -EAGAIN;
-		}
-		break;
-	default:
-		return -ENOSYS;
-	}
-	return 0;
-}
-
-static long visorchipset_ioctl(struct file *file, unsigned int cmd,
-				unsigned long arg)
-{
-	s64 adjustment;
-	s64 vrtc_offset;
-
-	switch (cmd) {
-	case VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET:
-		/* get the physical rtc offset */
-		vrtc_offset = issue_vmcall_query_guest_virtual_time_offset();
-		if (copy_to_user
-		    ((void __user *)arg, &vrtc_offset, sizeof(vrtc_offset))) {
-			return -EFAULT;
-		}
-		return SUCCESS;
-	case VMCALL_UPDATE_PHYSICAL_TIME:
-		if (copy_from_user
-		    (&adjustment, (void __user *)arg, sizeof(adjustment))) {
-			return -EFAULT;
-		}
-		return issue_vmcall_update_physical_time(adjustment);
-	default:
-		return -EFAULT;
-	}
-}
-
-static const struct file_operations visorchipset_fops = {
-	.owner = THIS_MODULE,
-	.open = visorchipset_open,
-	.read = NULL,
-	.write = NULL,
-	.unlocked_ioctl = visorchipset_ioctl,
-	.release = visorchipset_release,
-	.mmap = visorchipset_mmap,
-};
-
-int
-visorchipset_file_init(dev_t major_dev, struct visorchannel **controlvm_channel)
-{
-	int rc = 0;
-
-	file_controlvm_channel = controlvm_channel;
-	cdev_init(&file_cdev, &visorchipset_fops);
-	file_cdev.owner = THIS_MODULE;
-	if (MAJOR(major_dev) == 0) {
-		rc = alloc_chrdev_region(&major_dev, 0, 1, MYDRVNAME);
-		/* dynamic major device number registration required */
-		if (rc < 0)
-			return rc;
-	} else {
-		/* static major device number registration required */
-		rc = register_chrdev_region(major_dev, 1, MYDRVNAME);
-		if (rc < 0)
-			return rc;
-	}
-	rc = cdev_add(&file_cdev, MKDEV(MAJOR(major_dev), 0), 1);
-	if (rc < 0) {
-		unregister_chrdev_region(major_dev, 1);
-		return rc;
-	}
-	return 0;
-}
diff --git a/drivers/staging/unisys/visorchipset/file.h b/drivers/staging/unisys/visorchipset/file.h
deleted file mode 100644
index 51f7699b744b..000000000000
--- a/drivers/staging/unisys/visorchipset/file.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/* file.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __FILE_H__
-#define __FILE_H__
-
-#include "globals.h"
-
-int visorchipset_file_init(dev_t majorDev,
-			   struct visorchannel **pControlVm_channel);
-void visorchipset_file_cleanup(dev_t major_dev);
-
-#endif
diff --git a/drivers/staging/unisys/visorchipset/globals.h b/drivers/staging/unisys/visorchipset/globals.h
deleted file mode 100644
index f76e498a36b5..000000000000
--- a/drivers/staging/unisys/visorchipset/globals.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/* globals.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __VISORCHIPSET_GLOBALS_H__
-#define __VISORCHIPSET_GLOBALS_H__
-
-#include "diagnostics/appos_subsystems.h"
-#include "timskmod.h"
-#include "visorchipset.h"
-#include "visorchipset_umode.h"
-#include "version.h"
-
-#define MYDRVNAME "visorchipset"
-
-/* module parameters */
-
-extern int visorchipset_testvnic;
-extern int visorchipset_testvnicclient;
-extern int visorchipset_testmsg;
-extern int visorchipset_major;
-extern int visorchipset_serverregwait;
-extern int visorchipset_clientregwait;
-extern int visorchipset_testteardown;
-extern int visorchipset_disable_controlvm;
-extern int visorchipset_crash_kernel;
-extern int visorchipset_holdchipsetready;
-
-#endif
diff --git a/drivers/staging/unisys/visorchipset/parser.c b/drivers/staging/unisys/visorchipset/parser.c
deleted file mode 100644
index d8a2d6f5a75d..000000000000
--- a/drivers/staging/unisys/visorchipset/parser.c
+++ /dev/null
@@ -1,430 +0,0 @@
-/* parser.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#include "parser.h"
-#include "memregion.h"
-#include "controlvmchannel.h"
-#include <linux/ctype.h>
-#include <linux/mm.h>
-#include <linux/uuid.h>
-
-#define MYDRVNAME "visorchipset_parser"
-#define CURRENT_FILE_PC VISOR_CHIPSET_PC_parser_c
-
-/* We will refuse to allocate more than this many bytes to copy data from
- * incoming payloads.  This serves as a throttling mechanism.
- */
-#define MAX_CONTROLVM_PAYLOAD_BYTES (1024*128)
-static ulong controlvm_payload_bytes_buffered;
-
-struct parser_context {
-	ulong allocbytes;
-	ulong param_bytes;
-	u8 *curr;
-	ulong bytes_remaining;
-	BOOL byte_stream;
-	char data[0];
-};
-
-static struct parser_context *
-parser_init_guts(u64 addr, u32 bytes, BOOL local,
-		 BOOL standard_payload_header, BOOL *retry)
-{
-	int allocbytes = sizeof(struct parser_context) + bytes;
-	struct parser_context *rc = NULL;
-	struct parser_context *ctx = NULL;
-	struct memregion *rgn = NULL;
-	struct spar_controlvm_parameters_header *phdr = NULL;
-
-	if (retry)
-		*retry = FALSE;
-	if (!standard_payload_header)
-		/* alloc and 0 extra byte to ensure payload is
-		 * '\0'-terminated
-		 */
-		allocbytes++;
-	if ((controlvm_payload_bytes_buffered + bytes)
-	    > MAX_CONTROLVM_PAYLOAD_BYTES) {
-		if (retry)
-			*retry = TRUE;
-		rc = NULL;
-		goto cleanup;
-	}
-	ctx = kzalloc(allocbytes, GFP_KERNEL|__GFP_NORETRY);
-	if (!ctx) {
-		if (retry)
-			*retry = TRUE;
-		rc = NULL;
-		goto cleanup;
-	}
-
-	ctx->allocbytes = allocbytes;
-	ctx->param_bytes = bytes;
-	ctx->curr = NULL;
-	ctx->bytes_remaining = 0;
-	ctx->byte_stream = FALSE;
-	if (local) {
-		void *p;
-
-		if (addr > virt_to_phys(high_memory - 1)) {
-			rc = NULL;
-			goto cleanup;
-		}
-		p = __va((ulong) (addr));
-		memcpy(ctx->data, p, bytes);
-	} else {
-		rgn = visor_memregion_create(addr, bytes);
-		if (!rgn) {
-			rc = NULL;
-			goto cleanup;
-		}
-		if (visor_memregion_read(rgn, 0, ctx->data, bytes) < 0) {
-			rc = NULL;
-			goto cleanup;
-		}
-	}
-	if (!standard_payload_header) {
-		ctx->byte_stream = TRUE;
-		rc = ctx;
-		goto cleanup;
-	}
-	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
-	if (phdr->total_length != bytes) {
-		rc = NULL;
-		goto cleanup;
-	}
-	if (phdr->total_length < phdr->header_length) {
-		rc = NULL;
-		goto cleanup;
-	}
-	if (phdr->header_length <
-	    sizeof(struct spar_controlvm_parameters_header)) {
-		rc = NULL;
-		goto cleanup;
-	}
-
-	rc = ctx;
-cleanup:
-	if (rgn) {
-		visor_memregion_destroy(rgn);
-		rgn = NULL;
-	}
-	if (rc) {
-		controlvm_payload_bytes_buffered += ctx->param_bytes;
-	} else {
-		if (ctx) {
-			parser_done(ctx);
-			ctx = NULL;
-		}
-	}
-	return rc;
-}
-
-struct parser_context *
-parser_init(u64 addr, u32 bytes, BOOL local, BOOL *retry)
-{
-	return parser_init_guts(addr, bytes, local, TRUE, retry);
-}
-
-/* Call this instead of parser_init() if the payload area consists of just
- * 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.
- */
-struct parser_context *
-parser_init_byte_stream(u64 addr, u32 bytes, BOOL local, BOOL *retry)
-{
-	return parser_init_guts(addr, bytes, local, FALSE, retry);
-}
-
-/* Obtain '\0'-terminated copy of string in payload area.
- */
-char *
-parser_simpleString_get(struct parser_context *ctx)
-{
-	if (!ctx->byte_stream)
-		return NULL;
-	return ctx->data;	/* note this IS '\0'-terminated, because of
-				 * the num of bytes we alloc+clear in
-				 * parser_init_byteStream() */
-}
-
-/* Obtain a copy of the buffer in the payload area.
- */
-void *parser_byte_stream_get(struct parser_context *ctx, ulong *nbytes)
-{
-	if (!ctx->byte_stream)
-		return NULL;
-	if (nbytes)
-		*nbytes = ctx->param_bytes;
-	return (void *)ctx->data;
-}
-
-uuid_le
-parser_id_get(struct parser_context *ctx)
-{
-	struct spar_controlvm_parameters_header *phdr = NULL;
-
-	if (ctx == NULL)
-		return NULL_UUID_LE;
-	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
-	return phdr->id;
-}
-
-void
-parser_param_start(struct parser_context *ctx, PARSER_WHICH_STRING which_string)
-{
-	struct spar_controlvm_parameters_header *phdr = NULL;
-
-	if (ctx == NULL)
-		goto Away;
-	phdr = (struct spar_controlvm_parameters_header *)(ctx->data);
-	switch (which_string) {
-	case PARSERSTRING_INITIATOR:
-		ctx->curr = ctx->data + phdr->initiator_offset;
-		ctx->bytes_remaining = phdr->initiator_length;
-		break;
-	case PARSERSTRING_TARGET:
-		ctx->curr = ctx->data + phdr->target_offset;
-		ctx->bytes_remaining = phdr->target_length;
-		break;
-	case PARSERSTRING_CONNECTION:
-		ctx->curr = ctx->data + phdr->connection_offset;
-		ctx->bytes_remaining = phdr->connection_length;
-		break;
-	case PARSERSTRING_NAME:
-		ctx->curr = ctx->data + phdr->name_offset;
-		ctx->bytes_remaining = phdr->name_length;
-		break;
-	default:
-		break;
-	}
-
-Away:
-	return;
-}
-
-void
-parser_done(struct parser_context *ctx)
-{
-	if (!ctx)
-		return;
-	controlvm_payload_bytes_buffered -= ctx->param_bytes;
-	kfree(ctx);
-}
-
-/** Return length of string not counting trailing spaces. */
-static int
-string_length_no_trail(char *s, int len)
-{
-	int i = len - 1;
-
-	while (i >= 0) {
-		if (!isspace(s[i]))
-			return i + 1;
-		i--;
-	}
-	return 0;
-}
-
-/** Grab the next name and value out of the parameter buffer.
- *  The entire parameter buffer looks like this:
- *      <name>=<value>\0
- *      <name>=<value>\0
- *      ...
- *      \0
- *  If successful, the next <name> value is returned within the supplied
- *  <nam> buffer (the value is always upper-cased), and the corresponding
- *  <value> is returned within a kmalloc()ed buffer, whose pointer is
- *  provided as the return value of this function.
- *  (The total number of bytes allocated is strlen(<value>)+1.)
- *
- *  NULL is returned to indicate failure, which can occur for several reasons:
- *  - all <name>=<value> pairs have already been processed
- *  - bad parameter
- *  - parameter buffer ends prematurely (couldn't find an '=' or '\0' within
- *    the confines of the parameter buffer)
- *  - the <nam> buffer is not large enough to hold the <name> of the next
- *    parameter
- */
-void *
-parser_param_get(struct parser_context *ctx, char *nam, int namesize)
-{
-	u8 *pscan, *pnam = nam;
-	ulong nscan;
-	int value_length = -1, orig_value_length = -1;
-	void *value = NULL;
-	int i;
-	int closing_quote = 0;
-
-	if (!ctx)
-		return NULL;
-	pscan = ctx->curr;
-	nscan = ctx->bytes_remaining;
-	if (nscan == 0)
-		return NULL;
-	if (*pscan == '\0')
-		/*  This is the normal return point after you have processed
-		 *  all of the <name>=<value> pairs in a syntactically-valid
-		 *  parameter buffer.
-		 */
-		return NULL;
-
-	/* skip whitespace */
-	while (isspace(*pscan)) {
-		pscan++;
-		nscan--;
-		if (nscan == 0)
-			return NULL;
-	}
-
-	while (*pscan != ':') {
-		if (namesize <= 0)
-			return NULL;
-		*pnam = toupper(*pscan);
-		pnam++;
-		namesize--;
-		pscan++;
-		nscan--;
-		if (nscan == 0)
-			return NULL;
-	}
-	if (namesize <= 0)
-		return NULL;
-	*pnam = '\0';
-	nam[string_length_no_trail(nam, strlen(nam))] = '\0';
-
-	/* point to char immediately after ":" in "<name>:<value>" */
-	pscan++;
-	nscan--;
-	/* skip whitespace */
-	while (isspace(*pscan)) {
-		pscan++;
-		nscan--;
-		if (nscan == 0)
-			return NULL;
-	}
-	if (nscan == 0)
-		return NULL;
-	if (*pscan == '\'' || *pscan == '"') {
-		closing_quote = *pscan;
-		pscan++;
-		nscan--;
-		if (nscan == 0)
-			return NULL;
-	}
-
-	/* look for a separator character, terminator character, or
-	 * end of data
-	 */
-	for (i = 0, value_length = -1; i < nscan; i++) {
-		if (closing_quote) {
-			if (pscan[i] == '\0')
-				return NULL;
-			if (pscan[i] == closing_quote) {
-				value_length = i;
-				break;
-			}
-		} else
-		    if (pscan[i] == ',' || pscan[i] == ';'
-			|| pscan[i] == '\0') {
-			value_length = i;
-			break;
-		}
-	}
-	if (value_length < 0) {
-		if (closing_quote)
-			return NULL;
-		value_length = nscan;
-	}
-	orig_value_length = value_length;
-	if (closing_quote == 0)
-		value_length = string_length_no_trail(pscan, orig_value_length);
-	value = kmalloc(value_length + 1, GFP_KERNEL|__GFP_NORETRY);
-	if (value == NULL)
-		return NULL;
-	memcpy(value, pscan, value_length);
-	((u8 *) (value))[value_length] = '\0';
-
-	pscan += orig_value_length;
-	nscan -= orig_value_length;
-
-	/* skip past separator or closing quote */
-	if (nscan > 0) {
-		if (*pscan != '\0') {
-			pscan++;
-			nscan--;
-		}
-	}
-
-	if (closing_quote && (nscan > 0)) {
-		/* we still need to skip around the real separator if present */
-		/* first, skip whitespace */
-		while (isspace(*pscan)) {
-			pscan++;
-			nscan--;
-			if (nscan == 0)
-				break;
-		}
-		if (nscan > 0) {
-			if (*pscan == ',' || *pscan == ';') {
-				pscan++;
-				nscan--;
-			} else if (*pscan != '\0') {
-				kfree(value);
-				value = NULL;
-				return NULL;
-			}
-		}
-	}
-	ctx->curr = pscan;
-	ctx->bytes_remaining = nscan;
-	return value;
-}
-
-void *
-parser_string_get(struct parser_context *ctx)
-{
-	u8 *pscan;
-	ulong nscan;
-	int value_length = -1;
-	void *value = NULL;
-	int i;
-
-	if (!ctx)
-		return NULL;
-	pscan = ctx->curr;
-	nscan = ctx->bytes_remaining;
-	if (nscan == 0)
-		return NULL;
-	if (!pscan)
-		return NULL;
-	for (i = 0, value_length = -1; i < nscan; i++)
-		if (pscan[i] == '\0') {
-			value_length = i;
-			break;
-		}
-	if (value_length < 0)	/* '\0' was not included in the length */
-		value_length = nscan;
-	value = kmalloc(value_length + 1, GFP_KERNEL|__GFP_NORETRY);
-	if (value == NULL)
-		return NULL;
-	if (value_length > 0)
-		memcpy(value, pscan, value_length);
-	((u8 *) (value))[value_length] = '\0';
-	return value;
-}
diff --git a/drivers/staging/unisys/visorchipset/parser.h b/drivers/staging/unisys/visorchipset/parser.h
deleted file mode 100644
index 2b903f1beff2..000000000000
--- a/drivers/staging/unisys/visorchipset/parser.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/* parser.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __PARSER_H__
-#define __PARSER_H__
-
-#include <linux/uuid.h>
-
-#include "timskmod.h"
-#include "channel.h"
-
-typedef enum {
-	PARSERSTRING_INITIATOR,
-	PARSERSTRING_TARGET,
-	PARSERSTRING_CONNECTION,
-	PARSERSTRING_NAME,
-} PARSER_WHICH_STRING;
-
-struct parser_context *parser_init(u64 addr, u32 bytes, BOOL isLocal,
-				   BOOL *tryAgain);
-struct parser_context *parser_init_byte_stream(u64 addr, u32 bytes, BOOL local,
-				       BOOL *retry);
-void parser_param_start(struct parser_context *ctx,
-			PARSER_WHICH_STRING which_string);
-void *parser_param_get(struct parser_context *ctx, char *nam, int namesize);
-void *parser_string_get(struct parser_context *ctx);
-uuid_le parser_id_get(struct parser_context *ctx);
-char *parser_simpleString_get(struct parser_context *ctx);
-void *parser_byte_stream_get(struct parser_context *ctx, ulong *nbytes);
-void parser_done(struct parser_context *ctx);
-
-#endif
diff --git a/drivers/staging/unisys/visorchipset/visorchipset.h b/drivers/staging/unisys/visorchipset/visorchipset.h
deleted file mode 100644
index bd46df9ef45a..000000000000
--- a/drivers/staging/unisys/visorchipset/visorchipset.h
+++ /dev/null
@@ -1,236 +0,0 @@
-/* visorchipset.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __VISORCHIPSET_H__
-#define __VISORCHIPSET_H__
-
-#include <linux/uuid.h>
-
-#include "timskmod.h"
-#include "channel.h"
-#include "controlvmchannel.h"
-#include "parser.h"
-#include "procobjecttree.h"
-#include "vbusdeviceinfo.h"
-#include "vbushelper.h"
-
-/** Describes the state from the perspective of which controlvm messages have
- *  been received for a bus or device.
- */
-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. */
-};
-
-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,
-
-	/** address is guest physical, and withIN the confines of the
-	 *  physical memory controlled by the running OS.
-	 */
-	ADDRTYPE_LOCALTEST,
-};
-
-enum crash_obj_type {
-	CRASH_DEV,
-	CRASH_BUS,
-};
-
-/** Attributes for a particular Supervisor channel.
- */
-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;
-
-};
-
-/** 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().
- */
-struct visorchipset_device_info {
-	struct list_head entry;
-	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;
-
-};
-
-static inline struct visorchipset_device_info *finddevice(
-		struct list_head *list, u32 bus_no, u32 dev_no)
-{
-	struct visorchipset_device_info *p;
-
-	list_for_each_entry(p, list, entry) {
-		if (p->bus_no == bus_no && p->dev_no == dev_no)
-			return p;
-	}
-	return NULL;
-}
-
-static inline void delbusdevices(struct list_head *list, u32 bus_no)
-{
-	struct visorchipset_device_info *p, *tmp;
-
-	list_for_each_entry_safe(p, tmp, list, entry) {
-		if (p->bus_no == bus_no) {
-			list_del(&p->entry);
-			kfree(p);
-		}
-	}
-}
-
-/** Attributes for a particular Supervisor bus.
- *  (For a service partition acting as the server for buses/devices, there
- *  is a 1-to-1 relationship between busses and guest partitions.)
- *  Any visorchipset client can query these attributes using
- *  visorchipset_get_client_bus_info() or visorchipset_get_bus_info().
- */
-struct visorchipset_bus_info {
-	struct list_head entry;
-	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;
-	struct {
-		u32 server:1;
-		/* Add new fields above. */
-		/* Remaining bits in this 32-bit word are unused. */
-	} flags;
-	struct controlvm_message_header pending_msg_hdr;/* CONTROLVM MsgHdr */
-	/** For private use by the bus driver */
-	void *bus_driver_context;
-	u64 dev_no;
-
-};
-
-static inline struct visorchipset_bus_info *
-findbus(struct list_head *list, u32 bus_no)
-{
-	struct visorchipset_bus_info *p;
-
-	list_for_each_entry(p, list, entry) {
-		if (p->bus_no == bus_no)
-			return p;
-	}
-	return NULL;
-}
-
-/*  These functions will be called from within visorchipset when certain
- *  events happen.  (The implementation of these functions is outside of
- *  visorchipset.)
- */
-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).
- *  For now, the value for each response is simply either:
- *       0 = it worked
- *      -1 = it failed
- */
-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
- *  to be the server for.  visorchipset will fill in <responders>, to
- *  indicate functions the bus driver should call to indicate message
- *  responses.
- */
-void
-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
- *  to be the client for.  visorchipset will fill in <responders>, to
- *  indicate functions the bus driver should call to indicate message
- *  responses.
- */
-void
-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) (struct controlvm_message *msg,
-					       int status);
-
-void visorchipset_device_pause_response(ulong bus_no, ulong dev_no,
-					int response);
-
-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_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);
-
-#endif
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_umode.h b/drivers/staging/unisys/visorchipset/visorchipset_umode.h
deleted file mode 100644
index 6cf6eccb3f4a..000000000000
--- a/drivers/staging/unisys/visorchipset/visorchipset_umode.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/* visorchipset_umode.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/** @file *********************************************************************
- *
- *  This describes structures needed for the interface between the
- *  visorchipset driver and a user-mode component that opens the device.
- *
- ******************************************************************************
- */
-
-#ifndef __VISORCHIPSET_UMODE_H
-#define __VISORCHIPSET_UMODE_H
-
-/** The user-mode program can access the control channel buffer directly
- *  via this memory map.
- */
-#define VISORCHIPSET_MMAP_CONTROLCHANOFFSET    (0x00000000)
-#define VISORCHIPSET_MMAP_CONTROLCHANSIZE      (0x00400000)  /* 4MB */
-
-#endif /* __VISORCHIPSET_UMODE_H */
diff --git a/drivers/staging/unisys/visornic/Kconfig b/drivers/staging/unisys/visornic/Kconfig
new file mode 100644
index 000000000000..1676dc7072d5
--- /dev/null
+++ b/drivers/staging/unisys/visornic/Kconfig
@@ -0,0 +1,15 @@
+#
+# Unisys visornic configuration
+#
+
+config UNISYS_VISORNIC
+	tristate "Unisys visornic driver"
+	depends on UNISYSSPAR && UNISYS_VISORBUS && NET
+	---help---
+	The Unisys Visornic driver provides support for s-Par network
+	devices exposed on the s-Par visorbus. When a message is sent
+	to visorbus to create a network device, the probe function of
+	visornic is called to create the netdev device. Networking on
+	s-Par switches will not work if this driver is not selected.
+	If you say Y here, you will enable the Unisys visornic driver.
+
diff --git a/drivers/staging/unisys/visornic/Makefile b/drivers/staging/unisys/visornic/Makefile
new file mode 100644
index 000000000000..439e95e03300
--- /dev/null
+++ b/drivers/staging/unisys/visornic/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for Unisys channel
+#
+
+obj-$(CONFIG_UNISYS_VISORNIC)	+= visornic.o
+
+visornic-y := visornic_main.o
+
+ccflags-y += -Idrivers/staging/unisys/include
+
diff --git a/drivers/staging/unisys/visornic/visornic_main.c b/drivers/staging/unisys/visornic/visornic_main.c
new file mode 100644
index 000000000000..710074437737
--- /dev/null
+++ b/drivers/staging/unisys/visornic/visornic_main.c
@@ -0,0 +1,2140 @@
+/* Copyright (c) 2012 - 2015 UNISYS CORPORATION
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or (at
+ * your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
+ * NON INFRINGEMENT.  See the GNU General Public License for more
+ * details.
+ */
+
+/* This driver lives in a spar partition, and registers to ethernet io
+ * channels from the visorbus driver. It creates netdev devices and
+ * forwards transmit to the IO channel and accepts rcvs from the IO
+ * Partition via the IO channel.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/kthread.h>
+
+#include "visorbus.h"
+#include "iochannel.h"
+
+#define VISORNIC_INFINITE_RESPONSE_WAIT 0
+#define VISORNICSOPENMAX 32
+#define MAXDEVICES     16384
+
+/* MAX_BUF = 64 lines x 32 MAXVNIC x 80 characters
+ *         = 163840 bytes
+ */
+#define MAX_BUF 163840
+
+static spinlock_t dev_num_pool_lock;
+static void *dev_num_pool;	/**< pool to grab device numbers from */
+
+static int visornic_probe(struct visor_device *dev);
+static void visornic_remove(struct visor_device *dev);
+static int visornic_pause(struct visor_device *dev,
+			  visorbus_state_complete_func complete_func);
+static int visornic_resume(struct visor_device *dev,
+			   visorbus_state_complete_func complete_func);
+
+/* DEBUGFS declarations */
+static ssize_t info_debugfs_read(struct file *file, char __user *buf,
+				 size_t len, loff_t *offset);
+static ssize_t enable_ints_write(struct file *file, const char __user *buf,
+				 size_t len, loff_t *ppos);
+static struct dentry *visornic_debugfs_dir;
+static const struct file_operations debugfs_info_fops = {
+	.read = info_debugfs_read,
+};
+
+static const struct file_operations debugfs_enable_ints_fops = {
+	.write = enable_ints_write,
+};
+
+static struct workqueue_struct *visornic_serverdown_workqueue;
+static struct workqueue_struct *visornic_timeout_reset_workqueue;
+
+/* GUIDS for director channel type supported by this driver.  */
+static struct visor_channeltype_descriptor visornic_channel_types[] = {
+	/* Note that the only channel type we expect to be reported by the
+	 * bus driver is the SPAR_VNIC channel.
+	 */
+	{ SPAR_VNIC_CHANNEL_PROTOCOL_UUID, "ultravnic" },
+	{ NULL_UUID_LE, NULL }
+};
+
+/* This is used to tell the visor bus driver which types of visor devices
+ * we support, and what functions to call when a visor device that we support
+ * is attached or removed.
+ */
+static struct visor_driver visornic_driver = {
+	.name = "visornic",
+	.version = "1.0.0.0",
+	.vertag = NULL,
+	.owner = THIS_MODULE,
+	.channel_types = visornic_channel_types,
+	.probe = visornic_probe,
+	.remove = visornic_remove,
+	.pause = visornic_pause,
+	.resume = visornic_resume,
+	.channel_interrupt = NULL,
+};
+
+struct visor_thread_info {
+	struct task_struct *task;
+	struct completion has_stopped;
+	int id;
+};
+
+struct chanstat {
+	unsigned long got_rcv;
+	unsigned long got_enbdisack;
+	unsigned long got_xmit_done;
+	unsigned long xmit_fail;
+	unsigned long sent_enbdis;
+	unsigned long sent_promisc;
+	unsigned long sent_post;
+	unsigned long sent_xmit;
+	unsigned long reject_count;
+	unsigned long extra_rcvbufs_sent;
+};
+
+struct visornic_devdata {
+	int devnum;
+	int thread_wait_ms;
+	unsigned short enabled;		/* 0 disabled 1 enabled to receive */
+	unsigned short enab_dis_acked;	/* NET_RCV_ENABLE/DISABLE acked by
+					 * IOPART
+					 */
+	struct visor_device *dev;
+	char name[99];
+	struct list_head list_all;   /* < link within list_all_devices list */
+	struct kref kref;
+	struct net_device *netdev;
+	struct net_device_stats net_stats;
+	atomic_t interrupt_rcvd;
+	wait_queue_head_t rsp_queue;
+	struct sk_buff **rcvbuf;
+	u64 uniquenum; /* TODO figure out why not used */
+	unsigned short old_flags;	/* flags as they were prior to
+					 * set_multicast_list
+					 */
+	atomic_t usage;			/* count of users */
+	int num_rcv_bufs;		/* indicates how many rcv buffers
+					 * the vnic will post
+					 */
+	int num_rcv_bufs_could_not_alloc;
+	atomic_t num_rcvbuf_in_iovm;
+	unsigned long alloc_failed_in_if_needed_cnt;
+	unsigned long alloc_failed_in_repost_rtn_cnt;
+	int max_outstanding_net_xmits;   /* absolute max number of outstanding
+					  * xmits - should never hit this
+					  */
+	int upper_threshold_net_xmits;   /* high water mark for calling
+					  * netif_stop_queue()
+					  */
+	int lower_threshold_net_xmits;	 /* high water mark for calling
+					  * netif_wake_queue()
+					  */
+	struct sk_buff_head xmitbufhead; /* xmitbufhead is the head of the
+					  * xmit buffer list that have been
+					  * sent to the IOPART end
+					  */
+	struct work_struct serverdown_completion;
+	struct work_struct timeout_reset;
+	struct uiscmdrsp *cmdrsp_rcv;	 /* cmdrsp_rcv is used for
+					  * posting/unposting rcv buffers
+					  */
+	struct uiscmdrsp *xmit_cmdrsp;	 /* used to issue NET_XMIT - there is
+					  * never more that one xmit in
+					  * progress at a time
+					  */
+	bool server_down;		 /* IOPART is down */
+	bool server_change_state;	 /* Processing SERVER_CHANGESTATE msg */
+	struct dentry *eth_debugfs_dir;
+	struct visor_thread_info threadinfo;
+	u64 interrupts_rcvd;
+	u64 interrupts_notme;
+	u64 interrupts_disabled;
+	u64 busy_cnt;
+	spinlock_t priv_lock;  /* spinlock to access devdata structures */
+
+	/* flow control counter */
+	u64 flow_control_upper_hits;
+	u64 flow_control_lower_hits;
+
+	/* debug counters */
+	unsigned long n_rcv0;			/* # rcvs of 0 buffers */
+	unsigned long n_rcv1;			/* # rcvs of 1 buffers */
+	unsigned long n_rcv2;			/* # rcvs of 2 buffers */
+	unsigned long n_rcvx;			/* # rcvs of >2 buffers */
+	unsigned long found_repost_rcvbuf_cnt;	/* # times we called
+						 *   repost_rcvbuf_cnt
+						 */
+	unsigned long repost_found_skb_cnt;	/* # times found the skb */
+	unsigned long n_repost_deficit;		/* # times we couldn't find
+						 *   all of the rcv buffers
+						 */
+	unsigned long bad_rcv_buf;		/* # times we negleted to
+						 * free the rcv skb because
+						 * we didn't know where it
+						 * came from
+						 */
+	unsigned long n_rcv_packets_not_accepted;/* # bogs rcv packets */
+
+	int queuefullmsg_logged;
+	struct chanstat chstat;
+};
+
+/* array of open devices maintained by open() and close() */
+static struct net_device *num_visornic_open[VISORNICSOPENMAX];
+
+/* List of all visornic_devdata structs,
+ * linked via the list_all member
+ */
+static LIST_HEAD(list_all_devices);
+static DEFINE_SPINLOCK(lock_all_devices);
+
+/**
+ *	visor_copy_fragsinfo_from_skb(
+ *	@skb_in: skbuff that we are pulling the frags from
+ *	@firstfraglen: length of first fragment in skb
+ *	@frags_max: max len of frags array
+ *	@frags: frags array filled in on output
+ *
+ *	Copy the fragment list in the SKB to a phys_info
+ *	array that the IOPART understands.
+ *	Return value indicates number of entries filled in frags
+ *	Negative values indicate an error.
+ */
+static unsigned int
+visor_copy_fragsinfo_from_skb(struct sk_buff *skb, unsigned int firstfraglen,
+			      unsigned int frags_max,
+			      struct phys_info frags[])
+{
+	unsigned int count = 0, ii, size, offset = 0, numfrags;
+
+	numfrags = skb_shinfo(skb)->nr_frags;
+
+	while (firstfraglen) {
+		if (count == frags_max)
+			return -EINVAL;
+
+		frags[count].pi_pfn =
+			page_to_pfn(virt_to_page(skb->data + offset));
+		frags[count].pi_off =
+			(unsigned long)(skb->data + offset) & PI_PAGE_MASK;
+		size = min_t(unsigned int, firstfraglen,
+			     PI_PAGE_SIZE - frags[count].pi_off);
+
+		/* can take smallest of firstfraglen (what's left) OR
+		 * bytes left in the page
+		 */
+		frags[count].pi_len = size;
+		firstfraglen -= size;
+		offset += size;
+		count++;
+	}
+	if (numfrags) {
+		if ((count + numfrags) > frags_max)
+			return -EINVAL;
+
+		for (ii = 0; ii < numfrags; ii++) {
+			count = add_physinfo_entries(page_to_pfn(
+				skb_frag_page(&skb_shinfo(skb)->frags[ii])),
+					      skb_shinfo(skb)->frags[ii].
+					      page_offset,
+					      skb_shinfo(skb)->frags[ii].
+					      size, count, frags_max, frags);
+			if (!count)
+				return -EIO;
+		}
+	}
+	if (skb_shinfo(skb)->frag_list) {
+		struct sk_buff *skbinlist;
+		int c;
+
+		for (skbinlist = skb_shinfo(skb)->frag_list; skbinlist;
+		     skbinlist = skbinlist->next) {
+			c = visor_copy_fragsinfo_from_skb(skbinlist,
+							  skbinlist->len -
+							  skbinlist->data_len,
+							  frags_max - count,
+							  &frags[count]);
+			if (c < 0)
+				return c;
+			count += c;
+		}
+	}
+	return count;
+}
+
+/**
+ *	visort_thread_start - starts thread for the device
+ *	@thrinfo: The thread to start
+ *	@threadfn: Function the thread starts
+ *	@thrcontext: Context to pass to the thread, i.e. devdata
+ *	@name:	string describing name of thread
+ *
+ *	Starts a thread for the device, currently only thread is
+ *	process_incoming_rsps
+ *	Returns 0 on success;
+ */
+static int visor_thread_start(struct visor_thread_info *thrinfo,
+			      int (*threadfn)(void *),
+			      void *thrcontext, char *name)
+{
+	/* used to stop the thread */
+	init_completion(&thrinfo->has_stopped);
+	thrinfo->task = kthread_run(threadfn, thrcontext, name);
+	if (IS_ERR(thrinfo->task)) {
+		thrinfo->id = 0;
+		return -EINVAL;
+	}
+	thrinfo->id = thrinfo->task->pid;
+	return 0;
+}
+
+/**
+ *	visor_thread_stop - stop a thread for the device
+ *	@thrinfo: The thread to stop
+ *
+ *	Stop the thread and wait for completion for a minute
+ *	Returns void.
+ */
+static void visor_thread_stop(struct visor_thread_info *thrinfo)
+{
+	if (!thrinfo->id)
+		return;	/* thread not running */
+
+	kthread_stop(thrinfo->task);
+	/* give up if the thread has NOT died in 1 minute */
+	if (wait_for_completion_timeout(&thrinfo->has_stopped, 60 * HZ))
+		thrinfo->id = 0;
+}
+
+/* DebugFS code */
+static ssize_t info_debugfs_read(struct file *file, char __user *buf,
+				 size_t len, loff_t *offset)
+{
+	int i;
+	ssize_t bytes_read = 0;
+	int str_pos = 0;
+	struct visornic_devdata *devdata;
+	char *vbuf;
+
+	if (len > MAX_BUF)
+		len = MAX_BUF;
+	vbuf = kzalloc(len, GFP_KERNEL);
+	if (!vbuf)
+		return -ENOMEM;
+
+	/* for each vnic channel
+	 * dump out channel specific data
+	 */
+	for (i = 0; i < VISORNICSOPENMAX; i++) {
+		if (!num_visornic_open[i])
+			continue;
+
+		devdata = netdev_priv(num_visornic_open[i]);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     "Vnic i = %d\n", i);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     "netdev = %s (0x%p), MAC Addr %pM\n",
+				     num_visornic_open[i]->name,
+				     num_visornic_open[i],
+				     num_visornic_open[i]->dev_addr);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     "VisorNic Dev Info = 0x%p\n", devdata);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " num_rcv_bufs = %d\n",
+				     devdata->num_rcv_bufs);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " max_oustanding_next_xmits = %d\n",
+				    devdata->max_outstanding_net_xmits);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " upper_threshold_net_xmits = %d\n",
+				     devdata->upper_threshold_net_xmits);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " lower_threshold_net_xmits = %d\n",
+				     devdata->lower_threshold_net_xmits);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " queuefullmsg_logged = %d\n",
+				     devdata->queuefullmsg_logged);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.got_rcv = %lu\n",
+				     devdata->chstat.got_rcv);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.got_enbdisack = %lu\n",
+				     devdata->chstat.got_enbdisack);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.got_xmit_done = %lu\n",
+				     devdata->chstat.got_xmit_done);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.xmit_fail = %lu\n",
+				     devdata->chstat.xmit_fail);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.sent_enbdis = %lu\n",
+				     devdata->chstat.sent_enbdis);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.sent_promisc = %lu\n",
+				     devdata->chstat.sent_promisc);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.sent_post = %lu\n",
+				     devdata->chstat.sent_post);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.sent_xmit = %lu\n",
+				     devdata->chstat.sent_xmit);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.reject_count = %lu\n",
+				     devdata->chstat.reject_count);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " chstat.extra_rcvbufs_sent = %lu\n",
+				     devdata->chstat.extra_rcvbufs_sent);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " n_rcv0 = %lu\n", devdata->n_rcv0);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " n_rcv1 = %lu\n", devdata->n_rcv1);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " n_rcv2 = %lu\n", devdata->n_rcv2);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " n_rcvx = %lu\n", devdata->n_rcvx);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " num_rcvbuf_in_iovm = %d\n",
+				     atomic_read(&devdata->num_rcvbuf_in_iovm));
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " alloc_failed_in_if_needed_cnt = %lu\n",
+				     devdata->alloc_failed_in_if_needed_cnt);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " alloc_failed_in_repost_rtn_cnt = %lu\n",
+				     devdata->alloc_failed_in_repost_rtn_cnt);
+		/* str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+		 *		     " inner_loop_limit_reached_cnt = %lu\n",
+		 *		     devdata->inner_loop_limit_reached_cnt);
+		 */
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " found_repost_rcvbuf_cnt = %lu\n",
+				     devdata->found_repost_rcvbuf_cnt);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " repost_found_skb_cnt = %lu\n",
+				     devdata->repost_found_skb_cnt);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " n_repost_deficit = %lu\n",
+				     devdata->n_repost_deficit);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " bad_rcv_buf = %lu\n",
+				     devdata->bad_rcv_buf);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " n_rcv_packets_not_accepted = %lu\n",
+				     devdata->n_rcv_packets_not_accepted);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " interrupts_rcvd = %llu\n",
+				     devdata->interrupts_rcvd);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " interrupts_notme = %llu\n",
+				     devdata->interrupts_notme);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " interrupts_disabled = %llu\n",
+				     devdata->interrupts_disabled);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " busy_cnt = %llu\n",
+				     devdata->busy_cnt);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " flow_control_upper_hits = %llu\n",
+				     devdata->flow_control_upper_hits);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " flow_control_lower_hits = %llu\n",
+				     devdata->flow_control_lower_hits);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " thread_wait_ms = %d\n",
+				     devdata->thread_wait_ms);
+		str_pos += scnprintf(vbuf + str_pos, len - str_pos,
+				     " netif_queue = %s\n",
+				     netif_queue_stopped(devdata->netdev) ?
+				     "stopped" : "running");
+	}
+	bytes_read = simple_read_from_buffer(buf, len, offset, vbuf, str_pos);
+	kfree(vbuf);
+	return bytes_read;
+}
+
+static ssize_t enable_ints_write(struct file *file,
+				 const char __user *buffer,
+				 size_t count, loff_t *ppos)
+{
+	char buf[4];
+	int i, new_value;
+	struct visornic_devdata *devdata;
+
+	if (count >= ARRAY_SIZE(buf))
+		return -EINVAL;
+
+	buf[count] = '\0';
+	if (copy_from_user(buf, buffer, count))
+		return -EFAULT;
+
+	i = kstrtoint(buf, 10, &new_value);
+	if (i != 0)
+		return -EFAULT;
+
+	/* set all counts to new_value usually 0 */
+	for (i = 0; i < VISORNICSOPENMAX; i++) {
+		if (num_visornic_open[i]) {
+			devdata = netdev_priv(num_visornic_open[i]);
+			/* TODO update features bit in channel */
+		}
+	}
+
+	return count;
+}
+
+/**
+ *	visornic_serverdown_complete - IOPART went down, need to pause
+ *				       device
+ *	@work: Work queue it was scheduled on
+ *
+ *	The IO partition has gone down and we need to do some cleanup
+ *	for when it comes back. Treat the IO partition as the link
+ *	being down.
+ *	Returns void.
+ */
+static void
+visornic_serverdown_complete(struct work_struct *work)
+{
+	struct visornic_devdata *devdata;
+	struct net_device *netdev;
+	unsigned long flags;
+	int i = 0, count = 0;
+
+	devdata = container_of(work, struct visornic_devdata,
+			       serverdown_completion);
+	netdev = devdata->netdev;
+
+	/* Stop using datachan */
+	visor_thread_stop(&devdata->threadinfo);
+
+	/* Inform Linux that the link is down */
+	netif_carrier_off(netdev);
+	netif_stop_queue(netdev);
+
+	/* Free the skb for XMITs that haven't been serviced by the server
+	 * We shouldn't have to inform Linux about these IOs because they
+	 * are "lost in the ethernet"
+	 */
+	skb_queue_purge(&devdata->xmitbufhead);
+
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+	/* free rcv buffers */
+	for (i = 0; i < devdata->num_rcv_bufs; i++) {
+		if (devdata->rcvbuf[i]) {
+			kfree_skb(devdata->rcvbuf[i]);
+			devdata->rcvbuf[i] = NULL;
+			count++;
+		}
+	}
+	atomic_set(&devdata->num_rcvbuf_in_iovm, 0);
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+	devdata->server_down = true;
+	devdata->server_change_state = false;
+}
+
+/**
+ *	visornic_serverdown - Command has notified us that IOPARt is down
+ *	@devdata: device that is being managed by IOPART
+ *
+ *	Schedule the work needed to handle the server down request. Make
+ *	sure we haven't already handled the server change state event.
+ *	Returns 0 if we scheduled the work, -EINVAL on error.
+ */
+static int
+visornic_serverdown(struct visornic_devdata *devdata)
+{
+	if (!devdata->server_down && !devdata->server_change_state) {
+		devdata->server_change_state = true;
+		queue_work(visornic_serverdown_workqueue,
+			   &devdata->serverdown_completion);
+	} else if (devdata->server_change_state) {
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ *	alloc_rcv_buf	- alloc rcv buffer to be given to the IO Partition.
+ *	@netdev: network adapter the rcv bufs are attached too.
+ *
+ *	Create an sk_buff (rcv_buf) that will be passed to the IO Partition
+ *	so that it can write rcv data into our memory space.
+ *	Return pointer to sk_buff
+ */
+static struct sk_buff *
+alloc_rcv_buf(struct net_device *netdev)
+{
+	struct sk_buff *skb;
+
+	/* NOTE: the first fragment in each rcv buffer is pointed to by
+	 * rcvskb->data. For now all rcv buffers will be RCVPOST_BUF_SIZE
+	 * in length, so the firstfrag is large enough to hold 1514.
+	 */
+	skb = alloc_skb(RCVPOST_BUF_SIZE, GFP_ATOMIC);
+	if (!skb)
+		return NULL;
+	skb->dev = netdev;
+	skb->len = RCVPOST_BUF_SIZE;
+	/* current value of mtu doesn't come into play here; large
+	 * packets will just end up using multiple rcv buffers all of
+	 * same size
+	 */
+	skb->data_len = 0;      /* dev_alloc_skb already zeroes it out
+				 * for clarification.
+				 */
+	return skb;
+}
+
+/**
+ *	post_skb	- post a skb to the IO Partition.
+ *	@cmdrsp: cmdrsp packet to be send to the IO Partition
+ *	@devdata: visornic_devdata to post the skb too
+ *	@skb: skb to give to the IO partition
+ *
+ *	Send the skb to the IO Partition.
+ *	Returns void
+ */
+static inline void
+post_skb(struct uiscmdrsp *cmdrsp,
+	 struct visornic_devdata *devdata, struct sk_buff *skb)
+{
+	cmdrsp->net.buf = skb;
+	cmdrsp->net.rcvpost.frag.pi_pfn = page_to_pfn(virt_to_page(skb->data));
+	cmdrsp->net.rcvpost.frag.pi_off =
+		(unsigned long)skb->data & PI_PAGE_MASK;
+	cmdrsp->net.rcvpost.frag.pi_len = skb->len;
+	cmdrsp->net.rcvpost.unique_num = devdata->uniquenum;
+
+	if ((cmdrsp->net.rcvpost.frag.pi_off + skb->len) <= PI_PAGE_SIZE) {
+		cmdrsp->net.type = NET_RCV_POST;
+		cmdrsp->cmdtype = CMD_NET_TYPE;
+		visorchannel_signalinsert(devdata->dev->visorchannel,
+					  IOCHAN_TO_IOPART,
+					  cmdrsp);
+		atomic_inc(&devdata->num_rcvbuf_in_iovm);
+		devdata->chstat.sent_post++;
+	}
+}
+
+/**
+ *	send_enbdis	- send NET_RCV_ENBDIS to IO Partition
+ *	@netdev: netdevice we are enable/disable, used as context
+ *		 return value
+ *	@state: enable = 1/disable = 0
+ *	@devdata: visornic device we are enabling/disabling
+ *
+ *	Send the enable/disable message to the IO Partition.
+ *	Returns void
+ */
+static void
+send_enbdis(struct net_device *netdev, int state,
+	    struct visornic_devdata *devdata)
+{
+	devdata->cmdrsp_rcv->net.enbdis.enable = state;
+	devdata->cmdrsp_rcv->net.enbdis.context = netdev;
+	devdata->cmdrsp_rcv->net.type = NET_RCV_ENBDIS;
+	devdata->cmdrsp_rcv->cmdtype = CMD_NET_TYPE;
+	visorchannel_signalinsert(devdata->dev->visorchannel,
+				  IOCHAN_TO_IOPART,
+				  devdata->cmdrsp_rcv);
+	devdata->chstat.sent_enbdis++;
+}
+
+/**
+ *	visornic_disable_with_timeout - Disable network adapter
+ *	@netdev: netdevice to disale
+ *	@timeout: timeout to wait for disable
+ *
+ *	Disable the network adapter and inform the IO Partition that we
+ *	are disabled, reclaim memory from rcv bufs.
+ *	Returns 0 on success, negative for failure of IO Partition
+ *	responding.
+ *
+ */
+static int
+visornic_disable_with_timeout(struct net_device *netdev, const int timeout)
+{
+	struct visornic_devdata *devdata = netdev_priv(netdev);
+	int i;
+	unsigned long flags;
+	int wait = 0;
+
+	/* stop the transmit queue so nothing more can be transmitted */
+	netif_stop_queue(netdev);
+
+	/* send a msg telling the other end we are stopping incoming pkts */
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+	devdata->enabled = 0;
+	devdata->enab_dis_acked = 0; /* must wait for ack */
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+	/* send disable and wait for ack -- don't hold lock when sending
+	 * disable because if the queue is full, insert might sleep.
+	 */
+	send_enbdis(netdev, 0, devdata);
+
+	/* wait for ack to arrive before we try to free rcv buffers
+	 * NOTE: the other end automatically unposts the rcv buffers when
+	 * when it gets a disable.
+	 */
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+	while ((timeout == VISORNIC_INFINITE_RESPONSE_WAIT) ||
+	       (wait < timeout)) {
+		if (devdata->enab_dis_acked)
+			break;
+		if (devdata->server_down || devdata->server_change_state) {
+			spin_unlock_irqrestore(&devdata->priv_lock, flags);
+			return -EIO;
+		}
+		set_current_state(TASK_INTERRUPTIBLE);
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		wait += schedule_timeout(msecs_to_jiffies(10));
+		spin_lock_irqsave(&devdata->priv_lock, flags);
+	}
+
+	/* Wait for usage to go to 1 (no other users) before freeing
+	 * rcv buffers
+	 */
+	if (atomic_read(&devdata->usage) > 1) {
+		while (1) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			spin_unlock_irqrestore(&devdata->priv_lock, flags);
+			schedule_timeout(msecs_to_jiffies(10));
+			spin_lock_irqsave(&devdata->priv_lock, flags);
+			if (atomic_read(&devdata->usage))
+				break;
+		}
+	}
+
+	/* we've set enabled to 0, so we can give up the lock. */
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+	/* Free rcv buffers - other end has automatically unposed them on
+	 * disable
+	 */
+	for (i = 0; i < devdata->num_rcv_bufs; i++) {
+		if (devdata->rcvbuf[i]) {
+			kfree_skb(devdata->rcvbuf[i]);
+			devdata->rcvbuf[i] = NULL;
+		}
+	}
+
+	/* remove references from array */
+	for (i = 0; i < VISORNICSOPENMAX; i++)
+		if (num_visornic_open[i] == netdev) {
+			num_visornic_open[i] = NULL;
+			break;
+		}
+
+	return 0;
+}
+
+/**
+ *	init_rcv_bufs  -- initialize receive bufs and send them to the IO Part
+ *	@netdev: struct netdevice
+ *	@devdata: visornic_devdata
+ *
+ *	Allocate rcv buffers and post them to the IO Partition.
+ *	Return 0 for success, and negative for failure.
+ */
+static int
+init_rcv_bufs(struct net_device *netdev, struct visornic_devdata *devdata)
+{
+	int i, count;
+
+	/* allocate fixed number of receive buffers to post to uisnic
+	 * post receive buffers after we've allocated a required amount
+	 */
+	for (i = 0; i < devdata->num_rcv_bufs; i++) {
+		devdata->rcvbuf[i] = alloc_rcv_buf(netdev);
+		if (!devdata->rcvbuf[i])
+			break; /* if we failed to allocate one let us stop */
+	}
+	if (i == 0) /* couldn't even allocate one -- bail out */
+		return -ENOMEM;
+	count = i;
+
+	/* Ensure we can alloc 2/3rd of the requeested number of buffers.
+	 * 2/3 is an arbitrary choice; used also in ndis init.c
+	 */
+	if (count < ((2 * devdata->num_rcv_bufs) / 3)) {
+		/* free receive buffers we did alloc and then bail out */
+		for (i = 0; i < count; i++) {
+			kfree_skb(devdata->rcvbuf[i]);
+			devdata->rcvbuf[i] = NULL;
+		}
+		return -ENOMEM;
+	}
+
+	/* post receive buffers to receive incoming input - without holding
+	 * lock - we've not enabled nor started the queue so there shouldn't
+	 * be any rcv or xmit activity
+	 */
+	for (i = 0; i < count; i++)
+		post_skb(devdata->cmdrsp_rcv, devdata, devdata->rcvbuf[i]);
+
+	return 0;
+}
+
+/**
+ *	visornic_enable_with_timeout	- send enable to IO Part
+ *	@netdev: struct net_device
+ *	@timeout: Time to wait for the ACK from the enable
+ *
+ *	Sends enable to IOVM, inits, and posts receive buffers to IOVM
+ *	timeout is defined in msecs (timeout of 0 specifies infinite wait)
+ *	Return 0 for success, negavite for failure.
+ */
+static int
+visornic_enable_with_timeout(struct net_device *netdev, const int timeout)
+{
+	int i;
+	struct visornic_devdata *devdata = netdev_priv(netdev);
+	unsigned long flags;
+	int wait = 0;
+
+	/* NOTE: the other end automatically unposts the rcv buffers when it
+	 * gets a disable.
+	 */
+	i = init_rcv_bufs(netdev, devdata);
+	if (i < 0)
+		return i;
+
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+	devdata->enabled = 1;
+
+	/* now we're ready, let's send an ENB to uisnic but until we get
+	 * an ACK back from uisnic, we'll drop the packets
+	 */
+	devdata->n_rcv_packets_not_accepted = 0;
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+	/* send enable and wait for ack -- don't hold lock when sending enable
+	 * because if the queue is full, insert might sleep.
+	 */
+	send_enbdis(netdev, 1, devdata);
+
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+	while ((timeout == VISORNIC_INFINITE_RESPONSE_WAIT) ||
+	       (wait < timeout)) {
+		if (devdata->enab_dis_acked)
+			break;
+		if (devdata->server_down || devdata->server_change_state) {
+			spin_unlock_irqrestore(&devdata->priv_lock, flags);
+			return -EIO;
+		}
+		set_current_state(TASK_INTERRUPTIBLE);
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		wait += schedule_timeout(msecs_to_jiffies(10));
+		spin_lock_irqsave(&devdata->priv_lock, flags);
+	}
+
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+	if (!devdata->enab_dis_acked)
+		return -EIO;
+
+	/* find an open slot in the array to save off VisorNic references
+	 * for debug
+	 */
+	for (i = 0; i < VISORNICSOPENMAX; i++) {
+		if (!num_visornic_open[i]) {
+			num_visornic_open[i] = netdev;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ *	visornic_timeout_reset	- handle xmit timeout resets
+ *	@work	work item that scheduled the work
+ *
+ *	Transmit Timeouts are typically handled by resetting the
+ *	device for our virtual NIC we will send a Disable and Enable
+ *	to the IOVM. If it doesn't respond we will trigger a serverdown.
+ */
+static void
+visornic_timeout_reset(struct work_struct *work)
+{
+	struct visornic_devdata *devdata;
+	struct net_device *netdev;
+	int response = 0;
+
+	devdata = container_of(work, struct visornic_devdata, timeout_reset);
+	netdev = devdata->netdev;
+
+	netif_stop_queue(netdev);
+	response = visornic_disable_with_timeout(netdev, 100);
+	if (response)
+		goto call_serverdown;
+
+	response = visornic_enable_with_timeout(netdev, 100);
+	if (response)
+		goto call_serverdown;
+	netif_wake_queue(netdev);
+
+	return;
+
+call_serverdown:
+	visornic_serverdown(devdata);
+}
+
+/**
+ *	visornic_open - Enable the visornic device and mark the queue started
+ *	@netdev: netdevice to start
+ *
+ *      Enable the device and start the transmit queue.
+ *      Return 0 for success
+ */
+static int
+visornic_open(struct net_device *netdev)
+{
+	visornic_enable_with_timeout(netdev, VISORNIC_INFINITE_RESPONSE_WAIT);
+
+	/* start the interface's transmit queue, allowing it to accept
+	 * packets for transmission
+	 */
+	netif_start_queue(netdev);
+
+	return 0;
+}
+
+/**
+ *	visornic_close - Disables the visornic device and stops the queues
+ *	@netdev: netdevice to start
+ *
+ *      Disable the device and stop the transmit queue.
+ *      Return 0 for success
+ */
+static int
+visornic_close(struct net_device *netdev)
+{
+	netif_stop_queue(netdev);
+	visornic_disable_with_timeout(netdev, VISORNIC_INFINITE_RESPONSE_WAIT);
+
+	return 0;
+}
+
+/**
+ *	visornic_xmit - send a packet to the IO Partition
+ *	@skb: Packet to be sent
+ *	@netdev: net device the packet is being sent from
+ *
+ *	Convert the skb to a cmdrsp so the IO Partition can undersand it.
+ *	Send the XMIT command to the IO Partition for processing. This
+ *	function is protected from concurrent calls by a spinlock xmit_lock
+ *	in the net_device struct, but as soon as the function returns it
+ *	can be called again.
+ *	Returns NETDEV_TX_OK for success, NETDEV_TX_BUSY for error.
+ */
+static int
+visornic_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct visornic_devdata *devdata;
+	int len, firstfraglen, padlen;
+	struct uiscmdrsp *cmdrsp = NULL;
+	unsigned long flags;
+
+	devdata = netdev_priv(netdev);
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+
+	if (netif_queue_stopped(netdev) || devdata->server_down ||
+	    devdata->server_change_state) {
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		devdata->busy_cnt++;
+		return NETDEV_TX_BUSY;
+	}
+
+	/* sk_buff struct is used to host network data throughout all the
+	 * linux network subsystems
+	 */
+	len = skb->len;
+
+	/* skb->len is the FULL length of data (including fragmentary portion)
+	 * skb->data_len is the length of the fragment portion in frags
+	 * skb->len - skb->data_len is size of the 1st fragment in skb->data
+	 * calculate the length of the first fragment that skb->data is
+	 * pointing to
+	 */
+	firstfraglen = skb->len - skb->data_len;
+	if (firstfraglen < ETH_HEADER_SIZE) {
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		devdata->busy_cnt++;
+		return NETDEV_TX_BUSY;
+	}
+
+	if ((len < ETH_MIN_PACKET_SIZE) &&
+	    ((skb_end_pointer(skb) - skb->data) >= ETH_MIN_PACKET_SIZE)) {
+		/* pad the packet out to minimum size */
+		padlen = ETH_MIN_PACKET_SIZE - len;
+		memset(&skb->data[len], 0, padlen);
+		skb->tail += padlen;
+		skb->len += padlen;
+		len += padlen;
+		firstfraglen += padlen;
+	}
+
+	cmdrsp = devdata->xmit_cmdrsp;
+	/* clear cmdrsp */
+	memset(cmdrsp, 0, SIZEOF_CMDRSP);
+	cmdrsp->net.type = NET_XMIT;
+	cmdrsp->cmdtype = CMD_NET_TYPE;
+
+	/* save the pointer to skb -- we'll need it for completion */
+	cmdrsp->net.buf = skb;
+
+	if (((devdata->chstat.sent_xmit >= devdata->chstat.got_xmit_done) &&
+	     (devdata->chstat.sent_xmit - devdata->chstat.got_xmit_done >=
+	     devdata->max_outstanding_net_xmits)) ||
+	     ((devdata->chstat.sent_xmit < devdata->chstat.got_xmit_done) &&
+	     (ULONG_MAX - devdata->chstat.got_xmit_done +
+	      devdata->chstat.sent_xmit >=
+	      devdata->max_outstanding_net_xmits))) {
+		/* too many NET_XMITs queued over to IOVM - need to wait
+		 */
+		devdata->chstat.reject_count++;
+		if (!devdata->queuefullmsg_logged &&
+		    ((devdata->chstat.reject_count & 0x3ff) == 1))
+			devdata->queuefullmsg_logged = 1;
+		netif_stop_queue(netdev);
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		devdata->busy_cnt++;
+		return NETDEV_TX_BUSY;
+	}
+	if (devdata->queuefullmsg_logged)
+		devdata->queuefullmsg_logged = 0;
+
+	if (skb->ip_summed == CHECKSUM_UNNECESSARY) {
+		cmdrsp->net.xmt.lincsum.valid = 1;
+		cmdrsp->net.xmt.lincsum.protocol = skb->protocol;
+		if (skb_transport_header(skb) > skb->data) {
+			cmdrsp->net.xmt.lincsum.hrawoff =
+				skb_transport_header(skb) - skb->data;
+			cmdrsp->net.xmt.lincsum.hrawoff = 1;
+		}
+		if (skb_network_header(skb) > skb->data) {
+			cmdrsp->net.xmt.lincsum.nhrawoff =
+				skb_network_header(skb) - skb->data;
+			cmdrsp->net.xmt.lincsum.nhrawoffv = 1;
+		}
+		cmdrsp->net.xmt.lincsum.csum = skb->csum;
+	} else {
+		cmdrsp->net.xmt.lincsum.valid = 0;
+	}
+
+	/* save off the length of the entire data packet */
+	cmdrsp->net.xmt.len = len;
+
+	/* copy ethernet header from first frag into ocmdrsp
+	 * - everything else will be pass in frags & DMA'ed
+	 */
+	memcpy(cmdrsp->net.xmt.ethhdr, skb->data, ETH_HEADER_SIZE);
+	/* copy frags info - from skb->data we need to only provide access
+	 * beyond eth header
+	 */
+	cmdrsp->net.xmt.num_frags =
+		visor_copy_fragsinfo_from_skb(skb, firstfraglen,
+					      MAX_PHYS_INFO,
+					      cmdrsp->net.xmt.frags);
+	if (cmdrsp->net.xmt.num_frags == -1) {
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		devdata->busy_cnt++;
+		return NETDEV_TX_BUSY;
+	}
+
+	if (!visorchannel_signalinsert(devdata->dev->visorchannel,
+				       IOCHAN_TO_IOPART, cmdrsp)) {
+		netif_stop_queue(netdev);
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		devdata->busy_cnt++;
+		return NETDEV_TX_BUSY;
+	}
+
+	/* Track the skbs that have been sent to the IOVM for XMIT */
+	skb_queue_head(&devdata->xmitbufhead, skb);
+
+	/* set the last transmission start time
+	 * linux doc says: Do not forget to update netdev->trans_start to
+	 * jiffies after each new tx packet is given to the hardware.
+	 */
+	netdev->trans_start = jiffies;
+
+	/* update xmt stats */
+	devdata->net_stats.tx_packets++;
+	devdata->net_stats.tx_bytes += skb->len;
+	devdata->chstat.sent_xmit++;
+
+	/* check to see if we have hit the high watermark for
+	 * netif_stop_queue()
+	 */
+	if (((devdata->chstat.sent_xmit >= devdata->chstat.got_xmit_done) &&
+	     (devdata->chstat.sent_xmit - devdata->chstat.got_xmit_done >=
+	      devdata->upper_threshold_net_xmits)) ||
+	    ((devdata->chstat.sent_xmit < devdata->chstat.got_xmit_done) &&
+	     (ULONG_MAX - devdata->chstat.got_xmit_done +
+	      devdata->chstat.sent_xmit >=
+	      devdata->upper_threshold_net_xmits))) {
+		/* too many NET_XMITs queued over to IOVM - need to wait */
+		netif_stop_queue(netdev); /* calling stop queue - call
+					   * netif_wake_queue() after lower
+					   * threshold
+					   */
+		devdata->flow_control_upper_hits++;
+	}
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+	/* skb will be freed when we get back NET_XMIT_DONE */
+	return NETDEV_TX_OK;
+}
+
+/**
+ *	visornic_get_stats - returns net_stats of the visornic device
+ *	@netdev: netdevice
+ *
+ *	Returns the net_device_stats for the device
+ */
+static struct net_device_stats *
+visornic_get_stats(struct net_device *netdev)
+{
+	struct visornic_devdata *devdata = netdev_priv(netdev);
+
+	return &devdata->net_stats;
+}
+
+/**
+ *	visornic_ioctl - ioctl function for netdevice.
+ *	@netdev: netdevice
+ *	@ifr: ignored
+ *	@cmd: ignored
+ *
+ *	Currently not supported.
+ *	Returns EOPNOTSUPP
+ */
+static int
+visornic_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
+{
+	return -EOPNOTSUPP;
+}
+
+/**
+ *	visornic_change_mtu - changes mtu of device.
+ *	@netdev: netdevice
+ *	@new_mtu: value of new mtu
+ *
+ *	MTU cannot be changed by system, must be changed via
+ *	CONTROLVM message. All vnics and pnics in a switch have
+ *	to have the same MTU for everything to work.
+ *	Currently not supported.
+ *	Returns EINVAL
+ */
+static int
+visornic_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	return -EINVAL;
+}
+
+/**
+ *	visornic_set_multi - changes mtu of device.
+ *	@netdev: netdevice
+ *
+ *	Only flag we support currently is IFF_PROMISC
+ *	Returns void
+ */
+static void
+visornic_set_multi(struct net_device *netdev)
+{
+	struct uiscmdrsp *cmdrsp;
+	struct visornic_devdata *devdata = netdev_priv(netdev);
+
+	/* any filtering changes */
+	if (devdata->old_flags != netdev->flags) {
+		if ((netdev->flags & IFF_PROMISC) !=
+		    (devdata->old_flags & IFF_PROMISC)) {
+			cmdrsp = kmalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
+			if (!cmdrsp)
+				return;
+			cmdrsp->cmdtype = CMD_NET_TYPE;
+			cmdrsp->net.type = NET_RCV_PROMISC;
+			cmdrsp->net.enbdis.context = netdev;
+			cmdrsp->net.enbdis.enable =
+				(netdev->flags & IFF_PROMISC);
+			visorchannel_signalinsert(devdata->dev->visorchannel,
+						  IOCHAN_TO_IOPART,
+						  cmdrsp);
+			kfree(cmdrsp);
+		}
+		devdata->old_flags = netdev->flags;
+	}
+}
+
+/**
+ *	visornic_xmit_timeout - request to timeout the xmit
+ *	@netdev
+ *
+ *	Queue the work and return. Make sure we have not already
+ *	been informed the IO Partition is gone, if it is gone
+ *	we will already timeout the xmits.
+ */
+static void
+visornic_xmit_timeout(struct net_device *netdev)
+{
+	struct visornic_devdata *devdata = netdev_priv(netdev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+	/* Ensure that a ServerDown message hasn't been received */
+	if (!devdata->enabled ||
+	    (devdata->server_down && !devdata->server_change_state)) {
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+	queue_work(visornic_timeout_reset_workqueue, &devdata->timeout_reset);
+}
+
+/**
+ *	repost_return	- repost rcv bufs that have come back
+ *	@cmdrsp: io channel command struct to post
+ *	@devdata: visornic devdata for the device
+ *	@skb: skb
+ *	@netdev: netdevice
+ *
+ *	Repost rcv buffers that have been returned to us when
+ *	we are finished with them.
+ *	Returns 0 for success, -1 for error.
+ */
+static inline int
+repost_return(struct uiscmdrsp *cmdrsp, struct visornic_devdata *devdata,
+	      struct sk_buff *skb, struct net_device *netdev)
+{
+	struct net_pkt_rcv copy;
+	int i = 0, cc, numreposted;
+	int found_skb = 0;
+	int status = 0;
+
+	copy = cmdrsp->net.rcv;
+	switch (copy.numrcvbufs) {
+	case 0:
+		devdata->n_rcv0++;
+		break;
+	case 1:
+		devdata->n_rcv1++;
+		break;
+	case 2:
+		devdata->n_rcv2++;
+		break;
+	default:
+		devdata->n_rcvx++;
+		break;
+	}
+	for (cc = 0, numreposted = 0; cc < copy.numrcvbufs; cc++) {
+		for (i = 0; i < devdata->num_rcv_bufs; i++) {
+			if (devdata->rcvbuf[i] != copy.rcvbuf[cc])
+				continue;
+
+			if ((skb) && devdata->rcvbuf[i] == skb) {
+				devdata->found_repost_rcvbuf_cnt++;
+				found_skb = 1;
+				devdata->repost_found_skb_cnt++;
+			}
+			devdata->rcvbuf[i] = alloc_rcv_buf(netdev);
+			if (!devdata->rcvbuf[i]) {
+				devdata->num_rcv_bufs_could_not_alloc++;
+				devdata->alloc_failed_in_repost_rtn_cnt++;
+				status = -ENOMEM;
+				break;
+			}
+			post_skb(cmdrsp, devdata, devdata->rcvbuf[i]);
+			numreposted++;
+			break;
+		}
+	}
+	if (numreposted != copy.numrcvbufs) {
+		devdata->n_repost_deficit++;
+		status = -EINVAL;
+	}
+	if (skb) {
+		if (found_skb) {
+			kfree_skb(skb);
+		} else {
+			status = -EINVAL;
+			devdata->bad_rcv_buf++;
+		}
+	}
+	atomic_dec(&devdata->usage);
+	return status;
+}
+
+/**
+ *	visornic_rx - Handle receive packets coming back from IO Part
+ *	@cmdrsp: Receive packet returned from IO Part
+ *
+ *	Got a receive packet back from the IO Part, handle it and send
+ *	it up the stack.
+ *	Returns void
+ */
+static void
+visornic_rx(struct uiscmdrsp *cmdrsp)
+{
+	struct visornic_devdata *devdata;
+	struct sk_buff *skb, *prev, *curr;
+	struct net_device *netdev;
+	int cc, currsize, off, status;
+	struct ethhdr *eth;
+	unsigned long flags;
+#ifdef DEBUG
+	struct phys_info testfrags[MAX_PHYS_INFO];
+#endif
+
+	/* post new rcv buf to the other end using the cmdrsp we have at hand
+	 * post it without holding lock - but we'll use the signal lock to
+	 * synchronize the queue insert the cmdrsp that contains the net.rcv
+	 * is the one we are using to repost, so copy the info we need from it.
+	 */
+	skb = cmdrsp->net.buf;
+	netdev = skb->dev;
+
+	if (!netdev) {
+		/* We must have previously downed this network device and
+		 * this skb and device is no longer valid. This also means
+		 * the skb reference was removed from devdata->rcvbuf so no
+		 * need to search for it.
+		 * All we can do is free the skb and return.
+		 * Note: We crash if we try to log this here.
+		 */
+		kfree_skb(skb);
+		return;
+	}
+
+	devdata = netdev_priv(netdev);
+
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+	atomic_dec(&devdata->num_rcvbuf_in_iovm);
+
+	/* update rcv stats - call it with priv_lock held */
+	devdata->net_stats.rx_packets++;
+	devdata->net_stats.rx_bytes = skb->len;
+
+	atomic_inc(&devdata->usage);	/* don't want a close to happen before
+					 *  we're done here
+					 */
+
+	/* set length to how much was ACTUALLY received -
+	 * NOTE: rcv_done_len includes actual length of data rcvd
+	 * including ethhdr
+	 */
+	skb->len = cmdrsp->net.rcv.rcv_done_len;
+
+	/* test enabled while holding lock */
+	if (!(devdata->enabled && devdata->enab_dis_acked)) {
+		/* don't process it unless we're in enable mode and until
+		 * we've gotten an ACK saying the other end got our RCV enable
+		 */
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+		repost_return(cmdrsp, devdata, skb, netdev);
+		return;
+	}
+
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+	/* when skb was allocated, skb->dev, skb->data, skb->len and
+	 * skb->data_len were setup. AND, data has already put into the
+	 * skb (both first frag and in frags pages)
+	 * NOTE: firstfragslen is the amount of data in skb->data and that
+	 * which is not in nr_frags or frag_list. This is now simply
+	 * RCVPOST_BUF_SIZE. bump tail to show how much data is in
+	 * firstfrag & set data_len to show rest see if we have to chain
+	 * frag_list.
+	 */
+	if (skb->len > RCVPOST_BUF_SIZE) {	/* do PRECAUTIONARY check */
+		if (cmdrsp->net.rcv.numrcvbufs < 2) {
+			if (repost_return(cmdrsp, devdata, skb, netdev) < 0)
+				dev_err(&devdata->netdev->dev,
+					"repost_return failed");
+			return;
+		}
+		/* length rcvd is greater than firstfrag in this skb rcv buf  */
+		skb->tail += RCVPOST_BUF_SIZE;	/* amount in skb->data */
+		skb->data_len = skb->len - RCVPOST_BUF_SIZE;	/* amount that
+								   will be in
+								   frag_list */
+	} else {
+		/* data fits in this skb - no chaining - do
+		 * PRECAUTIONARY check
+		 */
+		if (cmdrsp->net.rcv.numrcvbufs != 1) {	/* should be 1 */
+			if (repost_return(cmdrsp, devdata, skb, netdev) < 0)
+				dev_err(&devdata->netdev->dev,
+					"repost_return failed");
+			return;
+		}
+		skb->tail += skb->len;
+		skb->data_len = 0;	/* nothing rcvd in frag_list */
+	}
+	off = skb_tail_pointer(skb) - skb->data;
+
+	/* amount we bumped tail by in the head skb
+	 * it is used to calculate the size of each chained skb below
+	 * it is also used to index into bufline to continue the copy
+	 * (for chansocktwopc)
+	 * if necessary chain the rcv skbs together.
+	 * NOTE: index 0 has the same as cmdrsp->net.rcv.skb; we need to
+	 * chain the rest to that one.
+	 * - do PRECAUTIONARY check
+	 */
+	if (cmdrsp->net.rcv.rcvbuf[0] != skb) {
+		if (repost_return(cmdrsp, devdata, skb, netdev) < 0)
+			dev_err(&devdata->netdev->dev, "repost_return failed");
+		return;
+	}
+
+	if (cmdrsp->net.rcv.numrcvbufs > 1) {
+		/* chain the various rcv buffers into the skb's frag_list. */
+		/* Note: off was initialized above  */
+		for (cc = 1, prev = NULL;
+		     cc < cmdrsp->net.rcv.numrcvbufs; cc++) {
+			curr = (struct sk_buff *)cmdrsp->net.rcv.rcvbuf[cc];
+			curr->next = NULL;
+			if (!prev)	/* start of list- set head */
+				skb_shinfo(skb)->frag_list = curr;
+			else
+				prev->next = curr;
+			prev = curr;
+
+			/* should we set skb->len and skb->data_len for each
+			 * buffer being chained??? can't hurt!
+			 */
+			currsize = min(skb->len - off,
+				       (unsigned int)RCVPOST_BUF_SIZE);
+			curr->len = currsize;
+			curr->tail += currsize;
+			curr->data_len = 0;
+			off += currsize;
+		}
+#ifdef DEBUG
+		/* assert skb->len == off */
+		if (skb->len != off) {
+			dev_err(&devdata->netdev->dev,
+				"%s something wrong; skb->len:%d != off:%d\n",
+				netdev->name, skb->len, off);
+		}
+		/* test code */
+		cc = util_copy_fragsinfo_from_skb("rcvchaintest", skb,
+						  RCVPOST_BUF_SIZE,
+						  MAX_PHYS_INFO, testfrags);
+		if (cc != cmdrsp->net.rcv.numrcvbufs) {
+			dev_err(&devdata->netdev->dev,
+				"**** %s Something wrong; rcvd chain length %d different from one we calculated %d\n",
+				netdev->name, cmdrsp->net.rcv.numrcvbufs, cc);
+		}
+		for (i = 0; i < cc; i++) {
+			dev_inf(&devdata->netdev->dev,
+				"test:RCVPOST_BUF_SIZE:%d[%d] pfn:%llu off:0x%x len:%d\n",
+				RCVPOST_BUF_SIZE, i, testfrags[i].pi_pfn,
+				testfrags[i].pi_off, testfrags[i].pi_len);
+		}
+#endif
+	}
+
+	/* set up packet's protocl type using ethernet header - this
+	 * sets up skb->pkt_type & it also PULLS out the eth header
+	 */
+	skb->protocol = eth_type_trans(skb, netdev);
+
+	eth = eth_hdr(skb);
+
+	skb->csum = 0;
+	skb->ip_summed = CHECKSUM_NONE;
+
+	do {
+		if (netdev->flags & IFF_PROMISC)
+			break;	/* accept all packets */
+		if (skb->pkt_type == PACKET_BROADCAST) {
+			if (netdev->flags & IFF_BROADCAST)
+				break;	/* accept all broadcast packets */
+		} else if (skb->pkt_type == PACKET_MULTICAST) {
+			if ((netdev->flags & IFF_MULTICAST) &&
+			    (netdev_mc_count(netdev))) {
+				struct netdev_hw_addr *ha;
+				int found_mc = 0;
+
+				/* only accept multicast packets that we can
+				 * find in our multicast address list
+				 */
+				netdev_for_each_mc_addr(ha, netdev) {
+					if (ether_addr_equal(eth->h_dest,
+							     ha->addr)) {
+						found_mc = 1;
+						break;
+					}
+				}
+				if (found_mc)
+					break;	/* accept packet, dest
+						   matches a multicast
+						   address */
+			}
+		} else if (skb->pkt_type == PACKET_HOST) {
+			break;	/* accept packet, h_dest must match vnic
+				   mac address */
+		} else if (skb->pkt_type == PACKET_OTHERHOST) {
+			/* something is not right */
+			dev_err(&devdata->netdev->dev,
+				"**** FAILED to deliver rcv packet to OS; name:%s Dest:%pM VNIC:%pM\n",
+				netdev->name, eth->h_dest, netdev->dev_addr);
+		}
+		/* drop packet - don't forward it up to OS */
+		devdata->n_rcv_packets_not_accepted++;
+		repost_return(cmdrsp, devdata, skb, netdev);
+		return;
+	} while (0);
+
+	status = netif_rx(skb);
+	/* netif_rx returns various values, but "in practice most drivers
+	 * ignore the return value
+	 */
+
+	skb = NULL;
+	/*
+	 * whether the packet got dropped or handled, the skb is freed by
+	 * kernel code, so we shouldn't free it. but we should repost a
+	 * new rcv buffer.
+	 */
+	repost_return(cmdrsp, devdata, skb, netdev);
+}
+
+/**
+ *	devdata_initialize	- Initialize devdata structure
+ *	@devdata: visornic_devdata structure to initialize
+ *	#dev: visorbus_deviced it belongs to
+ *
+ *	Setup initial values for the visornic based on channel and default
+ *	values.
+ *	Returns a pointer to the devdata if successful, else NULL
+ */
+static struct visornic_devdata *
+devdata_initialize(struct visornic_devdata *devdata, struct visor_device *dev)
+{
+	int devnum = -1;
+
+	if (!devdata)
+		return NULL;
+	memset(devdata, '\0', sizeof(struct visornic_devdata));
+	spin_lock(&dev_num_pool_lock);
+	devnum = find_first_zero_bit(dev_num_pool, MAXDEVICES);
+	set_bit(devnum, dev_num_pool);
+	spin_unlock(&dev_num_pool_lock);
+	if (devnum == MAXDEVICES)
+		devnum = -1;
+	if (devnum < 0) {
+		kfree(devdata);
+		return NULL;
+	}
+	devdata->devnum = devnum;
+	devdata->dev = dev;
+	strncpy(devdata->name, dev_name(&dev->device), sizeof(devdata->name));
+	kref_init(&devdata->kref);
+	spin_lock(&lock_all_devices);
+	list_add_tail(&devdata->list_all, &list_all_devices);
+	spin_unlock(&lock_all_devices);
+	return devdata;
+}
+
+/**
+ *	devdata_release	- Frees up a devdata
+ *	@mykref: kref to the devdata
+ *
+ *	Frees up a devdata.
+ *	Returns void
+ */
+static void devdata_release(struct kref *mykref)
+{
+	struct visornic_devdata *devdata =
+		container_of(mykref, struct visornic_devdata, kref);
+
+	spin_lock(&dev_num_pool_lock);
+	clear_bit(devdata->devnum, dev_num_pool);
+	spin_unlock(&dev_num_pool_lock);
+	spin_lock(&lock_all_devices);
+	list_del(&devdata->list_all);
+	spin_unlock(&lock_all_devices);
+	kfree(devdata);
+}
+
+static const struct net_device_ops visornic_dev_ops = {
+	.ndo_open = visornic_open,
+	.ndo_stop = visornic_close,
+	.ndo_start_xmit = visornic_xmit,
+	.ndo_get_stats = visornic_get_stats,
+	.ndo_do_ioctl = visornic_ioctl,
+	.ndo_change_mtu = visornic_change_mtu,
+	.ndo_tx_timeout = visornic_xmit_timeout,
+	.ndo_set_rx_mode = visornic_set_multi,
+};
+
+/**
+ *	send_rcv_posts_if_needed
+ *	@devdata: visornic device
+ *
+ *	Send receive buffers to the IO Partition.
+ *	Returns void
+ */
+static void
+send_rcv_posts_if_needed(struct visornic_devdata *devdata)
+{
+	int i;
+	struct net_device *netdev;
+	struct uiscmdrsp *cmdrsp = devdata->cmdrsp_rcv;
+	int cur_num_rcv_bufs_to_alloc, rcv_bufs_allocated;
+
+	/* don't do this until vnic is marked ready */
+	if (!(devdata->enabled && devdata->enab_dis_acked))
+		return;
+
+	netdev = devdata->netdev;
+	rcv_bufs_allocated = 0;
+	/* this code is trying to prevent getting stuck here forever,
+	 * but still retry it if you cant allocate them all this time.
+	 */
+	cur_num_rcv_bufs_to_alloc = devdata->num_rcv_bufs_could_not_alloc;
+	while (cur_num_rcv_bufs_to_alloc > 0) {
+		cur_num_rcv_bufs_to_alloc--;
+		for (i = 0; i < devdata->num_rcv_bufs; i++) {
+			if (devdata->rcvbuf[i])
+				continue;
+			devdata->rcvbuf[i] = alloc_rcv_buf(netdev);
+			if (!devdata->rcvbuf[i]) {
+				devdata->alloc_failed_in_if_needed_cnt++;
+				break;
+			}
+			rcv_bufs_allocated++;
+			post_skb(cmdrsp, devdata, devdata->rcvbuf[i]);
+			devdata->chstat.extra_rcvbufs_sent++;
+		}
+	}
+	devdata->num_rcv_bufs_could_not_alloc -= rcv_bufs_allocated;
+}
+
+/**
+ *	draing_queue	- drains the response queue
+ *	@cmdrsp: io channel command response message
+ *	@devdata: visornic device to drain
+ *
+ *	Drain the respones queue of any responses from the IO partition.
+ *	Process the responses as we get them.
+ *	Returns when response queue is empty or when the threadd stops.
+ */
+static void
+drain_queue(struct uiscmdrsp *cmdrsp, struct visornic_devdata *devdata)
+{
+	unsigned long flags;
+	struct net_device *netdev;
+
+	/* drain queue */
+	while (1) {
+		/* TODO: CLIENT ACQUIRE -- Don't really need this at the
+		 * moment */
+		if (!visorchannel_signalremove(devdata->dev->visorchannel,
+					       IOCHAN_FROM_IOPART,
+					       cmdrsp))
+			break; /* queue empty */
+
+		switch (cmdrsp->net.type) {
+		case NET_RCV:
+			devdata->chstat.got_rcv++;
+			/* process incoming packet */
+			visornic_rx(cmdrsp);
+			break;
+		case NET_XMIT_DONE:
+			spin_lock_irqsave(&devdata->priv_lock, flags);
+			devdata->chstat.got_xmit_done++;
+			if (cmdrsp->net.xmtdone.xmt_done_result)
+				devdata->chstat.xmit_fail++;
+			/* only call queue wake if we stopped it */
+			netdev = ((struct sk_buff *)cmdrsp->net.buf)->dev;
+			/* ASSERT netdev == vnicinfo->netdev; */
+			if ((netdev == devdata->netdev) &&
+			    netif_queue_stopped(netdev)) {
+				/* check to see if we have crossed
+				 * the lower watermark for
+				 * netif_wake_queue()
+				 */
+				if (((devdata->chstat.sent_xmit >=
+				    devdata->chstat.got_xmit_done) &&
+				    (devdata->chstat.sent_xmit -
+				    devdata->chstat.got_xmit_done <=
+				    devdata->lower_threshold_net_xmits)) ||
+				    ((devdata->chstat.sent_xmit <
+				    devdata->chstat.got_xmit_done) &&
+				    (ULONG_MAX - devdata->chstat.got_xmit_done
+				    + devdata->chstat.sent_xmit <=
+				    devdata->lower_threshold_net_xmits))) {
+					/* enough NET_XMITs completed
+					 * so can restart netif queue
+					 */
+					netif_wake_queue(netdev);
+					devdata->flow_control_lower_hits++;
+				}
+			}
+			skb_unlink(cmdrsp->net.buf, &devdata->xmitbufhead);
+			spin_unlock_irqrestore(&devdata->priv_lock, flags);
+			kfree_skb(cmdrsp->net.buf);
+			break;
+		case NET_RCV_ENBDIS_ACK:
+			devdata->chstat.got_enbdisack++;
+			netdev = (struct net_device *)
+			cmdrsp->net.enbdis.context;
+			spin_lock_irqsave(&devdata->priv_lock, flags);
+			devdata->enab_dis_acked = 1;
+			spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+			if (devdata->server_down &&
+			    devdata->server_change_state) {
+				/* Inform Linux that the link is up */
+				devdata->server_down = false;
+				devdata->server_change_state = false;
+				netif_wake_queue(netdev);
+				netif_carrier_on(netdev);
+			}
+			break;
+		case NET_CONNECT_STATUS:
+			netdev = devdata->netdev;
+			if (cmdrsp->net.enbdis.enable == 1) {
+				spin_lock_irqsave(&devdata->priv_lock, flags);
+				devdata->enabled = cmdrsp->net.enbdis.enable;
+				spin_unlock_irqrestore(&devdata->priv_lock,
+						       flags);
+				netif_wake_queue(netdev);
+				netif_carrier_on(netdev);
+			} else {
+				netif_stop_queue(netdev);
+				netif_carrier_off(netdev);
+				spin_lock_irqsave(&devdata->priv_lock, flags);
+				devdata->enabled = cmdrsp->net.enbdis.enable;
+				spin_unlock_irqrestore(&devdata->priv_lock,
+						       flags);
+			}
+			break;
+		default:
+			break;
+		}
+		/* cmdrsp is now available for reuse  */
+
+		if (kthread_should_stop())
+			break;
+	}
+}
+
+/**
+ *	process_incoming_rsps	- Checks the status of the response queue.
+ *	@v: void pointer to the visronic devdata
+ *
+ *	Main function of the vnic_incoming thread. Peridocially check the
+ *	response queue and drain it if needed.
+ *	Returns when thread has stopped.
+ */
+static int
+process_incoming_rsps(void *v)
+{
+	struct visornic_devdata *devdata = v;
+	struct uiscmdrsp *cmdrsp = NULL;
+	const int SZ = SIZEOF_CMDRSP;
+
+	cmdrsp = kmalloc(SZ, GFP_ATOMIC);
+	if (!cmdrsp)
+		complete_and_exit(&devdata->threadinfo.has_stopped, 0);
+
+	while (1) {
+		wait_event_interruptible_timeout(
+			devdata->rsp_queue, (atomic_read(
+					     &devdata->interrupt_rcvd) == 1),
+				msecs_to_jiffies(devdata->thread_wait_ms));
+
+		/* periodically check to see if there are any rcf bufs which
+		 * need to get sent to the IOSP. This can only happen if
+		 * we run out of memory when trying to allocate skbs.
+		 */
+		atomic_set(&devdata->interrupt_rcvd, 0);
+		send_rcv_posts_if_needed(devdata);
+		drain_queue(cmdrsp, devdata);
+		if (kthread_should_stop())
+			break;
+	}
+
+	kfree(cmdrsp);
+	complete_and_exit(&devdata->threadinfo.has_stopped, 0);
+}
+
+/**
+ *	visornic_probe	- probe function for visornic devices
+ *	@dev: The visor device discovered
+ *
+ *	Called when visorbus discovers a visornic device on its
+ *	bus. It creates a new visornic ethernet adapter.
+ *	Returns 0 or negative for error.
+ */
+static int visornic_probe(struct visor_device *dev)
+{
+	struct visornic_devdata *devdata = NULL;
+	struct net_device *netdev = NULL;
+	int err;
+	int channel_offset = 0;
+	u64 features;
+
+	netdev = alloc_etherdev(sizeof(struct visornic_devdata));
+	if (!netdev)
+		return -ENOMEM;
+
+	netdev->netdev_ops = &visornic_dev_ops;
+	netdev->watchdog_timeo = (5 * HZ);
+	netdev->dev.parent = &dev->device;
+
+	/* Get MAC adddress from channel and read it into the device. */
+	netdev->addr_len = ETH_ALEN;
+	channel_offset = offsetof(struct spar_io_channel_protocol,
+				  vnic.macaddr);
+	err = visorbus_read_channel(dev, channel_offset, netdev->dev_addr,
+				    ETH_ALEN);
+	if (err < 0)
+		goto cleanup_netdev;
+
+	devdata = devdata_initialize(netdev_priv(netdev), dev);
+	if (!devdata) {
+		err = -ENOMEM;
+		goto cleanup_netdev;
+	}
+
+	devdata->netdev = netdev;
+	init_waitqueue_head(&devdata->rsp_queue);
+	spin_lock_init(&devdata->priv_lock);
+	devdata->enabled = 0; /* not yet */
+	atomic_set(&devdata->usage, 1);
+
+	/* Setup rcv bufs */
+	channel_offset = offsetof(struct spar_io_channel_protocol,
+				  vnic.num_rcv_bufs);
+	err = visorbus_read_channel(dev, channel_offset,
+				    &devdata->num_rcv_bufs, 4);
+	if (err)
+		goto cleanup_netdev;
+
+	devdata->rcvbuf = kmalloc(sizeof(struct sk_buff *) *
+				  devdata->num_rcv_bufs, GFP_KERNEL);
+	if (!devdata->rcvbuf) {
+		err = -ENOMEM;
+		goto cleanup_rcvbuf;
+	}
+
+	/* set the net_xmit outstanding threshold */
+	/* always leave two slots open but you should have 3 at a minimum */
+	devdata->max_outstanding_net_xmits =
+		max(3, ((devdata->num_rcv_bufs / 3) - 2));
+	devdata->upper_threshold_net_xmits =
+		max(2, devdata->max_outstanding_net_xmits - 1);
+	devdata->lower_threshold_net_xmits =
+		max(1, devdata->max_outstanding_net_xmits / 2);
+
+	skb_queue_head_init(&devdata->xmitbufhead);
+
+	/* create a cmdrsp we can use to post and unpost rcv buffers */
+	devdata->cmdrsp_rcv = kmalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
+	if (!devdata->cmdrsp_rcv) {
+		err = -ENOMEM;
+		goto cleanup_cmdrsp_rcv;
+	}
+	devdata->xmit_cmdrsp = kmalloc(SIZEOF_CMDRSP, GFP_ATOMIC);
+	if (!devdata->xmit_cmdrsp) {
+		err = -ENOMEM;
+		goto cleanup_xmit_cmdrsp;
+	}
+	INIT_WORK(&devdata->serverdown_completion,
+		  visornic_serverdown_complete);
+	INIT_WORK(&devdata->timeout_reset, visornic_timeout_reset);
+	devdata->server_down = false;
+	devdata->server_change_state = false;
+
+	/*set the default mtu */
+	channel_offset = offsetof(struct spar_io_channel_protocol,
+				  vnic.mtu);
+	err = visorbus_read_channel(dev, channel_offset, &netdev->mtu, 4);
+	if (err)
+		goto cleanup_xmit_cmdrsp;
+
+	/* TODO: Setup Interrupt information */
+	/* Let's start our threads to get responses */
+	channel_offset = offsetof(struct spar_io_channel_protocol,
+				  channel_header.features);
+	err = visorbus_read_channel(dev, channel_offset, &features, 8);
+	if (err)
+		goto cleanup_xmit_cmdrsp;
+
+	features |= ULTRA_IO_CHANNEL_IS_POLLING;
+	err = visorbus_write_channel(dev, channel_offset, &features, 8);
+	if (err)
+		goto cleanup_xmit_cmdrsp;
+
+	devdata->thread_wait_ms = 2;
+	visor_thread_start(&devdata->threadinfo, process_incoming_rsps,
+			   devdata, "vnic_incoming");
+
+	err = register_netdev(netdev);
+	if (err)
+		goto cleanup_thread_stop;
+
+	/* create debgug/sysfs directories */
+	devdata->eth_debugfs_dir = debugfs_create_dir(netdev->name,
+						      visornic_debugfs_dir);
+	if (!devdata->eth_debugfs_dir) {
+		err = -ENOMEM;
+		goto cleanup_thread_stop;
+	}
+
+	return 0;
+
+cleanup_thread_stop:
+	visor_thread_stop(&devdata->threadinfo);
+
+cleanup_xmit_cmdrsp:
+	kfree(devdata->xmit_cmdrsp);
+
+cleanup_cmdrsp_rcv:
+	kfree(devdata->cmdrsp_rcv);
+
+cleanup_rcvbuf:
+	kfree(devdata->rcvbuf);
+
+cleanup_netdev:
+	free_netdev(netdev);
+	return err;
+}
+
+/**
+ *	host_side_disappeared	- IO part is gone.
+ *	@devdata: device object
+ *
+ *	IO partition servicing this device is gone, do cleanup
+ *	Returns void.
+ */
+static void host_side_disappeared(struct visornic_devdata *devdata)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&devdata->priv_lock, flags);
+	sprintf(devdata->name, "<dev#%d-history>", devdata->devnum);
+	devdata->dev = NULL;   /* indicate device destroyed */
+	spin_unlock_irqrestore(&devdata->priv_lock, flags);
+}
+
+/**
+ *	visornic_remove		- Called when visornic dev goes away
+ *	@dev: visornic device that is being removed
+ *
+ *	Called when DEVICE_DESTROY gets called to remove device.
+ *	Returns void
+ */
+static void visornic_remove(struct visor_device *dev)
+{
+	struct visornic_devdata *devdata = dev_get_drvdata(&dev->device);
+
+	if (!devdata)
+		return;
+	dev_set_drvdata(&dev->device, NULL);
+	host_side_disappeared(devdata);
+	kref_put(&devdata->kref, devdata_release);
+}
+
+/**
+ *	visornic_pause		- Called when IO Part disappears
+ *	@dev: visornic device that is being serviced
+ *	@complete_func: call when finished.
+ *
+ *	Called when the IO Partition has gone down. Need to free
+ *	up resources and wait for IO partition to come back. Mark
+ *	link as down and don't attempt any DMA. When we have freed
+ *	memory call the complete_func so that Command knows we are
+ *	done. If we don't call complete_func, IO part will never
+ *	come back.
+ *	Returns 0 for success.
+ */
+static int visornic_pause(struct visor_device *dev,
+			  visorbus_state_complete_func complete_func)
+{
+	struct visornic_devdata *devdata = dev_get_drvdata(&dev->device);
+
+	visornic_serverdown(devdata);
+	complete_func(dev, 0);
+	return 0;
+}
+
+/**
+ *	visornic_resume		- Called when IO part has recovered
+ *	@dev: visornic device that is being serviced
+ *	@compelte_func: call when finished
+ *
+ *	Called when the IO partition has recovered. Reestablish
+ *	connection to the IO part and set the link up. Okay to do
+ *	DMA again.
+ *	Returns 0 for success.
+ */
+static int visornic_resume(struct visor_device *dev,
+			   visorbus_state_complete_func complete_func)
+{
+	struct visornic_devdata *devdata;
+	struct net_device *netdev;
+	unsigned long flags;
+
+	devdata = dev_get_drvdata(&dev->device);
+	if (!devdata)
+		return -EINVAL;
+
+	netdev = devdata->netdev;
+
+	if (devdata->server_down && !devdata->server_change_state) {
+		devdata->server_change_state = true;
+		/* Must transition channel to ATTACHED state BEFORE
+		 * we can start using the device again.
+		 * TODO: State transitions
+		 */
+		visor_thread_start(&devdata->threadinfo, process_incoming_rsps,
+				   devdata, "vnic_incoming");
+		init_rcv_bufs(netdev, devdata);
+		spin_lock_irqsave(&devdata->priv_lock, flags);
+		devdata->enabled = 1;
+
+		/* Now we're ready, let's send an ENB to uisnic but until
+		 * we get an ACK back from uisnic, we'll drop the packets
+		 */
+		devdata->enab_dis_acked = 0;
+		spin_unlock_irqrestore(&devdata->priv_lock, flags);
+
+		/* send enable and wait for ack - don't hold lock when
+		 * sending enable because if the queue if sull, insert
+		 * might sleep.
+		 */
+		send_enbdis(netdev, 1, devdata);
+	} else if (devdata->server_change_state) {
+		return -EIO;
+	}
+
+	complete_func(dev, 0);
+	return 0;
+}
+
+/**
+ *	visornic_init	- Init function
+ *
+ *	Init function for the visornic driver. Do initial driver setup
+ *	and wait for devices.
+ *	Returns 0 for success, negative for error.
+ */
+static int visornic_init(void)
+{
+	struct dentry *ret;
+	int err = -ENOMEM;
+
+	/* create workqueue for serverdown completion */
+	visornic_serverdown_workqueue =
+		create_singlethread_workqueue("visornic_serverdown");
+	if (!visornic_serverdown_workqueue)
+		return -ENOMEM;
+
+	/* create workqueue for tx timeout reset */
+	visornic_timeout_reset_workqueue =
+		create_singlethread_workqueue("visornic_timeout_reset");
+	if (!visornic_timeout_reset_workqueue)
+		return -ENOMEM;
+
+	visornic_debugfs_dir = debugfs_create_dir("visornic", NULL);
+	if (!visornic_debugfs_dir)
+		return err;
+
+	ret = debugfs_create_file("info", S_IRUSR, visornic_debugfs_dir, NULL,
+				  &debugfs_info_fops);
+	if (!ret)
+		goto cleanup_debugfs;
+	ret = debugfs_create_file("enable_ints", S_IWUSR, visornic_debugfs_dir,
+				  NULL, &debugfs_enable_ints_fops);
+	if (!ret)
+		goto cleanup_debugfs;
+
+	/* create workqueue for serverdown completion */
+	visornic_serverdown_workqueue =
+		create_singlethread_workqueue("visornic_serverdown");
+	if (!visornic_serverdown_workqueue)
+		goto cleanup_debugfs;
+
+	/* create workqueue for tx timeout reset */
+	visornic_timeout_reset_workqueue =
+		create_singlethread_workqueue("visornic_timeout_reset");
+	if (!visornic_timeout_reset_workqueue)
+		goto cleanup_workqueue;
+
+	spin_lock_init(&dev_num_pool_lock);
+	dev_num_pool = kzalloc(BITS_TO_LONGS(MAXDEVICES), GFP_KERNEL);
+	if (!dev_num_pool)
+		goto cleanup_workqueue;
+
+	visorbus_register_visor_driver(&visornic_driver);
+	return 0;
+
+cleanup_workqueue:
+	flush_workqueue(visornic_serverdown_workqueue);
+	destroy_workqueue(visornic_serverdown_workqueue);
+	if (visornic_timeout_reset_workqueue) {
+		flush_workqueue(visornic_timeout_reset_workqueue);
+		destroy_workqueue(visornic_timeout_reset_workqueue);
+	}
+cleanup_debugfs:
+	debugfs_remove_recursive(visornic_debugfs_dir);
+
+	return err;
+}
+
+/**
+ *	visornic_cleanup	- driver exit routine
+ *
+ *	Unregister driver from the bus and free up memory.
+ */
+static void visornic_cleanup(void)
+{
+	if (visornic_serverdown_workqueue) {
+		flush_workqueue(visornic_serverdown_workqueue);
+		destroy_workqueue(visornic_serverdown_workqueue);
+	}
+	if (visornic_timeout_reset_workqueue) {
+		flush_workqueue(visornic_timeout_reset_workqueue);
+		destroy_workqueue(visornic_timeout_reset_workqueue);
+	}
+	debugfs_remove_recursive(visornic_debugfs_dir);
+
+	visorbus_unregister_visor_driver(&visornic_driver);
+	kfree(dev_num_pool);
+	dev_num_pool = NULL;
+}
+
+module_init(visornic_init);
+module_exit(visornic_cleanup);
+
+MODULE_AUTHOR("Unisys");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("sPAR nic driver for sparlinux: ver 1.0.0.0");
+MODULE_VERSION("1.0.0.0");
diff --git a/drivers/staging/unisys/visorutil/Kconfig b/drivers/staging/unisys/visorutil/Kconfig
deleted file mode 100644
index be9c2cf890cc..000000000000
--- a/drivers/staging/unisys/visorutil/Kconfig
+++ /dev/null
@@ -1,9 +0,0 @@
-#
-# Unisys timskmod configuration
-#
-
-config UNISYS_VISORUTIL
-	tristate "Unisys visorutil driver"
-	---help---
-	If you say Y here, you will enable the Unisys visorutil driver.
-
diff --git a/drivers/staging/unisys/visorutil/Makefile b/drivers/staging/unisys/visorutil/Makefile
deleted file mode 100644
index d9ab5a36e3bf..000000000000
--- a/drivers/staging/unisys/visorutil/Makefile
+++ /dev/null
@@ -1,9 +0,0 @@
-#
-# Makefile for Unisys timskmod
-#
-
-obj-$(CONFIG_UNISYS_VISORUTIL)	+= visorutil.o
-
-visorutil-y := charqueue.o  periodic_work.o  memregion_direct.o visorkmodutils.o
-
-ccflags-y += -Idrivers/staging/unisys/include
diff --git a/drivers/staging/unisys/visorutil/charqueue.c b/drivers/staging/unisys/visorutil/charqueue.c
deleted file mode 100644
index c91752a2d06b..000000000000
--- a/drivers/staging/unisys/visorutil/charqueue.c
+++ /dev/null
@@ -1,127 +0,0 @@
-/* charqueue.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*
- *  Simple character queue implementation for Linux kernel mode.
- */
-
-#include "charqueue.h"
-
-#define MYDRVNAME "charqueue"
-
-#define IS_EMPTY(charqueue) (charqueue->head == charqueue->tail)
-
-struct charqueue {
-	int alloc_size;
-	int nslots;
-	spinlock_t lock; /* read/write lock for this structure */
-	int head, tail;
-	unsigned char buf[0];
-};
-
-struct charqueue *visor_charqueue_create(ulong nslots)
-{
-	int alloc_size = sizeof(struct charqueue) + nslots + 1;
-	struct charqueue *cq;
-
-	cq = kmalloc(alloc_size, GFP_KERNEL|__GFP_NORETRY);
-	if (cq == NULL)
-		return NULL;
-	cq->alloc_size = alloc_size;
-	cq->nslots = nslots;
-	cq->head = 0;
-	cq->tail = 0;
-	spin_lock_init(&cq->lock);
-	return cq;
-}
-EXPORT_SYMBOL_GPL(visor_charqueue_create);
-
-void visor_charqueue_enqueue(struct charqueue *charqueue, unsigned char c)
-{
-	int alloc_slots = charqueue->nslots+1;  /* 1 slot is always empty */
-
-	spin_lock(&charqueue->lock);
-	charqueue->head = (charqueue->head+1) % alloc_slots;
-	if (charqueue->head == charqueue->tail)
-		/* overflow; overwrite the oldest entry */
-		charqueue->tail = (charqueue->tail+1) % alloc_slots;
-	charqueue->buf[charqueue->head] = c;
-	spin_unlock(&charqueue->lock);
-}
-EXPORT_SYMBOL_GPL(visor_charqueue_enqueue);
-
-BOOL visor_charqueue_is_empty(struct charqueue *charqueue)
-{
-	BOOL b;
-
-	spin_lock(&charqueue->lock);
-	b = IS_EMPTY(charqueue);
-	spin_unlock(&charqueue->lock);
-	return b;
-}
-EXPORT_SYMBOL_GPL(visor_charqueue_is_empty);
-
-static int charqueue_dequeue_1(struct charqueue *charqueue)
-{
-	int alloc_slots = charqueue->nslots + 1;  /* 1 slot is always empty */
-
-	if (IS_EMPTY(charqueue))
-		return -1;
-	charqueue->tail = (charqueue->tail+1) % alloc_slots;
-	return charqueue->buf[charqueue->tail];
-}
-
-int charqueue_dequeue(struct charqueue *charqueue)
-{
-	int rc;
-
-	spin_lock(&charqueue->lock);
-	rc = charqueue_dequeue_1(charqueue);
-	spin_unlock(&charqueue->lock);
-	return rc;
-}
-
-int visor_charqueue_dequeue_n(struct charqueue *charqueue, unsigned char *buf,
-			      int n)
-{
-	int rc, counter = 0, c;
-
-	spin_lock(&charqueue->lock);
-	for (;;) {
-		if (n <= 0)
-			break;  /* no more buffer space */
-		c = charqueue_dequeue_1(charqueue);
-		if (c < 0)
-			break;  /* no more input */
-		*buf = (unsigned char)(c);
-		buf++;
-		n--;
-		counter++;
-	}
-	rc = counter;
-	spin_unlock(&charqueue->lock);
-	return rc;
-}
-EXPORT_SYMBOL_GPL(visor_charqueue_dequeue_n);
-
-void visor_charqueue_destroy(struct charqueue *charqueue)
-{
-	if (charqueue == NULL)
-		return;
-	kfree(charqueue);
-}
-EXPORT_SYMBOL_GPL(visor_charqueue_destroy);
diff --git a/drivers/staging/unisys/visorutil/charqueue.h b/drivers/staging/unisys/visorutil/charqueue.h
deleted file mode 100644
index f46a776b935b..000000000000
--- a/drivers/staging/unisys/visorutil/charqueue.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/* charqueue.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __CHARQUEUE_H__
-#define __CHARQUEUE_H__
-
-#include "timskmod.h"
-
-/* struct charqueue is an opaque structure to users.
- * Fields are declared only in the implementation .c files.
- */
-struct 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/memregion.h b/drivers/staging/unisys/visorutil/memregion.h
deleted file mode 100644
index 0c3eebcf6d50..000000000000
--- a/drivers/staging/unisys/visorutil/memregion.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/* memregion.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __MEMREGION_H__
-#define __MEMREGION_H__
-
-#include "timskmod.h"
-
-/* struct memregion is an opaque structure to users.
- * Fields are declared only in the implementation .c files.
- */
-struct 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(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(struct memregion *memregion);
-
-#endif
diff --git a/drivers/staging/unisys/visorutil/memregion_direct.c b/drivers/staging/unisys/visorutil/memregion_direct.c
deleted file mode 100644
index eb7422fbe20f..000000000000
--- a/drivers/staging/unisys/visorutil/memregion_direct.c
+++ /dev/null
@@ -1,207 +0,0 @@
-/* memregion_direct.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-/*
- *  This is an implementation of memory regions that can be used to read/write
- *  channel memory (in main memory of the host system) from code running in
- *  a virtual partition.
- */
-#include "timskmod.h"
-#include "memregion.h"
-
-#define MYDRVNAME "memregion"
-
-struct memregion {
-	HOSTADDRESS physaddr;
-	ulong nbytes;
-	void __iomem *mapped;
-	BOOL requested;
-	BOOL overlapped;
-};
-
-static BOOL mapit(struct memregion *memregion);
-static void unmapit(struct memregion *memregion);
-
-struct memregion *
-visor_memregion_create(HOSTADDRESS physaddr, ulong nbytes)
-{
-	struct memregion *rc = NULL;
-	struct memregion *memregion;
-
-	memregion = kzalloc(sizeof(*memregion), GFP_KERNEL | __GFP_NORETRY);
-	if (memregion == NULL)
-		return NULL;
-
-	memregion->physaddr = physaddr;
-	memregion->nbytes = nbytes;
-	memregion->overlapped = FALSE;
-	if (!mapit(memregion)) {
-		rc = NULL;
-		goto cleanup;
-	}
-	rc = memregion;
-cleanup:
-	if (rc == NULL) {
-		visor_memregion_destroy(memregion);
-		memregion = NULL;
-	}
-	return rc;
-}
-EXPORT_SYMBOL_GPL(visor_memregion_create);
-
-struct memregion *
-visor_memregion_create_overlapped(struct memregion *parent, ulong offset,
-				  ulong nbytes)
-{
-	struct memregion *memregion = NULL;
-
-	if (parent == NULL)
-		return NULL;
-
-	if (parent->mapped == NULL)
-		return NULL;
-
-	if ((offset >= parent->nbytes) ||
-	    ((offset + nbytes) >= parent->nbytes))
-		return NULL;
-
-	memregion = kzalloc(sizeof(*memregion), GFP_KERNEL|__GFP_NORETRY);
-	if (memregion == NULL)
-		return NULL;
-
-	memregion->physaddr = parent->physaddr + offset;
-	memregion->nbytes = nbytes;
-	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(struct memregion *memregion)
-{
-	ulong physaddr = (ulong)(memregion->physaddr);
-	ulong nbytes = memregion->nbytes;
-
-	memregion->requested = FALSE;
-	if (request_mem_region(physaddr, nbytes, MYDRVNAME))
-		memregion->requested = TRUE;
-	memregion->mapped = ioremap_cache(physaddr, nbytes);
-	if (!memregion->mapped)
-		return FALSE;
-	return TRUE;
-}
-
-static void
-unmapit(struct memregion *memregion)
-{
-	if (memregion->mapped != NULL) {
-		iounmap(memregion->mapped);
-		memregion->mapped = NULL;
-	}
-	if (memregion->requested) {
-		release_mem_region((ulong)(memregion->physaddr),
-				   memregion->nbytes);
-		memregion->requested = FALSE;
-	}
-}
-
-HOSTADDRESS
-visor_memregion_get_physaddr(struct memregion *memregion)
-{
-	return memregion->physaddr;
-}
-EXPORT_SYMBOL_GPL(visor_memregion_get_physaddr);
-
-ulong
-visor_memregion_get_nbytes(struct memregion *memregion)
-{
-	return memregion->nbytes;
-}
-EXPORT_SYMBOL_GPL(visor_memregion_get_nbytes);
-
-void __iomem *
-visor_memregion_get_pointer(struct memregion *memregion)
-{
-	return memregion->mapped;
-}
-EXPORT_SYMBOL_GPL(visor_memregion_get_pointer);
-
-int
-visor_memregion_resize(struct memregion *memregion, ulong newsize)
-{
-	if (newsize == memregion->nbytes)
-		return 0;
-	if (memregion->overlapped)
-		/* no error check here - we no longer know the
-		 * parent's range!
-		 */
-		memregion->nbytes = newsize;
-	else {
-		unmapit(memregion);
-		memregion->nbytes = newsize;
-		if (!mapit(memregion))
-			return -1;
-	}
-	return 0;
-}
-EXPORT_SYMBOL_GPL(visor_memregion_resize);
-
-static int
-memregion_readwrite(BOOL is_write,
-		    struct memregion *memregion, ulong offset,
-		    void *local, ulong nbytes)
-{
-	if (offset + nbytes > memregion->nbytes)
-		return -EIO;
-
-	if (is_write)
-		memcpy_toio(memregion->mapped + offset, local, nbytes);
-	else
-		memcpy_fromio(local, memregion->mapped + offset, nbytes);
-
-	return 0;
-}
-
-int
-visor_memregion_read(struct memregion *memregion, ulong offset, void *dest,
-		     ulong nbytes)
-{
-	return memregion_readwrite(FALSE, memregion, offset, dest, nbytes);
-}
-EXPORT_SYMBOL_GPL(visor_memregion_read);
-
-int
-visor_memregion_write(struct memregion *memregion, ulong offset, void *src,
-		      ulong nbytes)
-{
-	return memregion_readwrite(TRUE, memregion, offset, src, nbytes);
-}
-EXPORT_SYMBOL_GPL(visor_memregion_write);
-
-void
-visor_memregion_destroy(struct memregion *memregion)
-{
-	if (memregion == NULL)
-		return;
-	if (!memregion->overlapped)
-		unmapit(memregion);
-	kfree(memregion);
-}
-EXPORT_SYMBOL_GPL(visor_memregion_destroy);
-
diff --git a/drivers/staging/unisys/visorutil/visorkmodutils.c b/drivers/staging/unisys/visorutil/visorkmodutils.c
deleted file mode 100644
index 62f0f7046e17..000000000000
--- a/drivers/staging/unisys/visorutil/visorkmodutils.c
+++ /dev/null
@@ -1,71 +0,0 @@
-/* timskmodutils.c
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#include "timskmod.h"
-
-#define MYDRVNAME "timskmodutils"
-
-/* s-Par uses the Intel processor's VT-X features to separate groups of
- * processors into partitions. The firmware sets the hypervisor bit and
- * reports an ID in the HV capabilities leaf so that the partition's OS
- * knows s-Par is present and managing the processors.
- */
-
-#define UNISYS_SPAR_LEAF_ID 0x40000000
-
-/* The s-Par leaf ID returns "UnisysSpar64" encoded across ebx, ecx, edx */
-#define UNISYS_SPAR_ID_EBX 0x73696e55
-#define UNISYS_SPAR_ID_ECX 0x70537379
-#define UNISYS_SPAR_ID_EDX 0x34367261
-
-int unisys_spar_platform;
-EXPORT_SYMBOL_GPL(unisys_spar_platform);
-
-static __init uint32_t visorutil_spar_detect(void)
-{
-	unsigned int eax, ebx, ecx, edx;
-
-	if (cpu_has_hypervisor) {
-		/* check the ID */
-		cpuid(UNISYS_SPAR_LEAF_ID, &eax, &ebx, &ecx, &edx);
-		return  (ebx == UNISYS_SPAR_ID_EBX) &&
-			(ecx == UNISYS_SPAR_ID_ECX) &&
-			(edx == UNISYS_SPAR_ID_EDX);
-	} else {
-		return 0;
-	}
-}
-
-static __init int visorutil_mod_init(void)
-{
-	if (visorutil_spar_detect()) {
-		unisys_spar_platform = TRUE;
-		return 0;
-	} else {
-		return -ENODEV;
-	}
-}
-
-static __exit void
-visorutil_mod_exit(void)
-{
-}
-
-module_init(visorutil_mod_init);
-module_exit(visorutil_mod_exit);
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index 19ba749bb122..9cca97af3044 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -99,23 +99,10 @@ struct image_desc {
 	struct mutex mutex;	/* Mutex for locking image */
 	struct device *device;	/* Sysfs device */
 	struct vme_resource *resource;	/* VME resource */
-	int users;		/* Number of current users */
 	int mmap_count;		/* Number of current mmap's */
 };
 static struct image_desc image[VME_DEVS];
 
-struct driver_stats {
-	unsigned long reads;
-	unsigned long writes;
-	unsigned long ioctls;
-	unsigned long irqs;
-	unsigned long berrs;
-	unsigned long dmaerrors;
-	unsigned long timeouts;
-	unsigned long external;
-};
-static struct driver_stats statistics;
-
 static struct cdev *vme_user_cdev;		/* Character device */
 static struct class *vme_user_sysfs_class;	/* Sysfs class */
 static struct vme_dev *vme_user_bridge;		/* Pointer to user device */
@@ -128,98 +115,11 @@ static const int type[VME_DEVS] = {	MASTER_MINOR,	MASTER_MINOR,
 					CONTROL_MINOR
 				};
 
-
-static int vme_user_open(struct inode *, struct file *);
-static int vme_user_release(struct inode *, struct file *);
-static ssize_t vme_user_read(struct file *, char __user *, size_t, loff_t *);
-static ssize_t vme_user_write(struct file *, const char __user *, size_t,
-	loff_t *);
-static loff_t vme_user_llseek(struct file *, loff_t, int);
-static long vme_user_unlocked_ioctl(struct file *, unsigned int, unsigned long);
-static int vme_user_mmap(struct file *file, struct vm_area_struct *vma);
-
-static void vme_user_vm_open(struct vm_area_struct *vma);
-static void vme_user_vm_close(struct vm_area_struct *vma);
-
-static int vme_user_match(struct vme_dev *);
-static int vme_user_probe(struct vme_dev *);
-static int vme_user_remove(struct vme_dev *);
-
-static const struct file_operations vme_user_fops = {
-	.open = vme_user_open,
-	.release = vme_user_release,
-	.read = vme_user_read,
-	.write = vme_user_write,
-	.llseek = vme_user_llseek,
-	.unlocked_ioctl = vme_user_unlocked_ioctl,
-	.compat_ioctl = vme_user_unlocked_ioctl,
-	.mmap = vme_user_mmap,
-};
-
 struct vme_user_vma_priv {
 	unsigned int minor;
 	atomic_t refcnt;
 };
 
-static const struct vm_operations_struct vme_user_vm_ops = {
-	.open = vme_user_vm_open,
-	.close = vme_user_vm_close,
-};
-
-
-/*
- * Reset all the statistic counters
- */
-static void reset_counters(void)
-{
-	statistics.reads = 0;
-	statistics.writes = 0;
-	statistics.ioctls = 0;
-	statistics.irqs = 0;
-	statistics.berrs = 0;
-	statistics.dmaerrors = 0;
-	statistics.timeouts = 0;
-}
-
-static int vme_user_open(struct inode *inode, struct file *file)
-{
-	int err;
-	unsigned int minor = MINOR(inode->i_rdev);
-
-	mutex_lock(&image[minor].mutex);
-	/* Allow device to be opened if a resource is needed and allocated. */
-	if (minor < CONTROL_MINOR && image[minor].resource == NULL) {
-		pr_err("No resources allocated for device\n");
-		err = -EINVAL;
-		goto err_res;
-	}
-
-	/* Increment user count */
-	image[minor].users++;
-
-	mutex_unlock(&image[minor].mutex);
-
-	return 0;
-
-err_res:
-	mutex_unlock(&image[minor].mutex);
-
-	return err;
-}
-
-static int vme_user_release(struct inode *inode, struct file *file)
-{
-	unsigned int minor = MINOR(inode->i_rdev);
-
-	mutex_lock(&image[minor].mutex);
-
-	/* Decrement user count */
-	image[minor].users--;
-
-	mutex_unlock(&image[minor].mutex);
-
-	return 0;
-}
 
 /*
  * We are going ot alloc a page during init per window for small transfers.
@@ -430,15 +330,17 @@ static loff_t vme_user_llseek(struct file *file, loff_t off, int whence)
 	size_t image_size;
 	loff_t res;
 
-	if (minor == CONTROL_MINOR)
-		return -EINVAL;
-
-	mutex_lock(&image[minor].mutex);
-	image_size = vme_get_size(image[minor].resource);
-	res = fixed_size_llseek(file, off, whence, image_size);
-	mutex_unlock(&image[minor].mutex);
+	switch (type[minor]) {
+	case MASTER_MINOR:
+	case SLAVE_MINOR:
+		mutex_lock(&image[minor].mutex);
+		image_size = vme_get_size(image[minor].resource);
+		res = fixed_size_llseek(file, off, whence, image_size);
+		mutex_unlock(&image[minor].mutex);
+		return res;
+	}
 
-	return res;
+	return -EINVAL;
 }
 
 /*
@@ -463,8 +365,6 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 	dma_addr_t pci_addr;
 	void __user *argp = (void __user *)arg;
 
-	statistics.ioctls++;
-
 	switch (type[minor]) {
 	case CONTROL_MINOR:
 		switch (cmd) {
@@ -608,6 +508,11 @@ static void vme_user_vm_close(struct vm_area_struct *vma)
 	kfree(vma_priv);
 }
 
+static const struct vm_operations_struct vme_user_vm_ops = {
+	.open = vme_user_vm_open,
+	.close = vme_user_vm_close,
+};
+
 static int vme_user_master_mmap(unsigned int minor, struct vm_area_struct *vma)
 {
 	int err;
@@ -649,78 +554,15 @@ static int vme_user_mmap(struct file *file, struct vm_area_struct *vma)
 	return -ENODEV;
 }
 
-
-/*
- * Unallocate a previously allocated buffer
- */
-static void buf_unalloc(int num)
-{
-	if (image[num].kern_buf) {
-#ifdef VME_DEBUG
-		pr_debug("UniverseII:Releasing buffer at %p\n",
-			 image[num].pci_buf);
-#endif
-
-		vme_free_consistent(image[num].resource, image[num].size_buf,
-			image[num].kern_buf, image[num].pci_buf);
-
-		image[num].kern_buf = NULL;
-		image[num].pci_buf = 0;
-		image[num].size_buf = 0;
-
-#ifdef VME_DEBUG
-	} else {
-		pr_debug("UniverseII: Buffer not allocated\n");
-#endif
-	}
-}
-
-static struct vme_driver vme_user_driver = {
-	.name = driver_name,
-	.match = vme_user_match,
-	.probe = vme_user_probe,
-	.remove = vme_user_remove,
+static const struct file_operations vme_user_fops = {
+	.read = vme_user_read,
+	.write = vme_user_write,
+	.llseek = vme_user_llseek,
+	.unlocked_ioctl = vme_user_unlocked_ioctl,
+	.compat_ioctl = vme_user_unlocked_ioctl,
+	.mmap = vme_user_mmap,
 };
 
-
-static int __init vme_user_init(void)
-{
-	int retval = 0;
-
-	pr_info("VME User Space Access Driver\n");
-
-	if (bus_num == 0) {
-		pr_err("No cards, skipping registration\n");
-		retval = -ENODEV;
-		goto err_nocard;
-	}
-
-	/* Let's start by supporting one bus, we can support more than one
-	 * in future revisions if that ever becomes necessary.
-	 */
-	if (bus_num > VME_USER_BUS_MAX) {
-		pr_err("Driver only able to handle %d buses\n",
-		       VME_USER_BUS_MAX);
-		bus_num = VME_USER_BUS_MAX;
-	}
-
-	/*
-	 * Here we just register the maximum number of devices we can and
-	 * leave vme_user_match() to allow only 1 to go through to probe().
-	 * This way, if we later want to allow multiple user access devices,
-	 * we just change the code in vme_user_match().
-	 */
-	retval = vme_register_driver(&vme_user_driver, VME_MAX_SLOTS);
-	if (retval != 0)
-		goto err_reg;
-
-	return retval;
-
-err_reg:
-err_nocard:
-	return retval;
-}
-
 static int vme_user_match(struct vme_dev *vdev)
 {
 	int i;
@@ -760,12 +602,8 @@ static int vme_user_probe(struct vme_dev *vdev)
 		mutex_init(&image[i].mutex);
 		image[i].device = NULL;
 		image[i].resource = NULL;
-		image[i].users = 0;
 	}
 
-	/* Initialise statistics counters */
-	reset_counters();
-
 	/* Assign major and minor numbers for the driver */
 	err = register_chrdev_region(MKDEV(VME_MAJOR, 0), VME_DEVS,
 		driver_name);
@@ -902,7 +740,8 @@ err_master:
 err_slave:
 	while (i > SLAVE_MINOR) {
 		i--;
-		buf_unalloc(i);
+		vme_free_consistent(image[i].resource, image[i].size_buf,
+				    image[i].kern_buf, image[i].pci_buf);
 		vme_slave_free(image[i].resource);
 	}
 err_class:
@@ -932,7 +771,8 @@ static int vme_user_remove(struct vme_dev *dev)
 
 	for (i = SLAVE_MINOR; i < (SLAVE_MAX + 1); i++) {
 		vme_slave_set(image[i].resource, 0, 0, 0, 0, VME_A32, 0);
-		buf_unalloc(i);
+		vme_free_consistent(image[i].resource, image[i].size_buf,
+				    image[i].kern_buf, image[i].pci_buf);
 		vme_slave_free(image[i].resource);
 	}
 
@@ -945,6 +785,51 @@ static int vme_user_remove(struct vme_dev *dev)
 	return 0;
 }
 
+static struct vme_driver vme_user_driver = {
+	.name = driver_name,
+	.match = vme_user_match,
+	.probe = vme_user_probe,
+	.remove = vme_user_remove,
+};
+
+static int __init vme_user_init(void)
+{
+	int retval = 0;
+
+	pr_info("VME User Space Access Driver\n");
+
+	if (bus_num == 0) {
+		pr_err("No cards, skipping registration\n");
+		retval = -ENODEV;
+		goto err_nocard;
+	}
+
+	/* Let's start by supporting one bus, we can support more than one
+	 * in future revisions if that ever becomes necessary.
+	 */
+	if (bus_num > VME_USER_BUS_MAX) {
+		pr_err("Driver only able to handle %d buses\n",
+		       VME_USER_BUS_MAX);
+		bus_num = VME_USER_BUS_MAX;
+	}
+
+	/*
+	 * Here we just register the maximum number of devices we can and
+	 * leave vme_user_match() to allow only 1 to go through to probe().
+	 * This way, if we later want to allow multiple user access devices,
+	 * we just change the code in vme_user_match().
+	 */
+	retval = vme_register_driver(&vme_user_driver, VME_MAX_SLOTS);
+	if (retval != 0)
+		goto err_reg;
+
+	return retval;
+
+err_reg:
+err_nocard:
+	return retval;
+}
+
 static void __exit vme_user_exit(void)
 {
 	vme_unregister_driver(&vme_user_driver);
diff --git a/drivers/staging/vt6655/Makefile b/drivers/staging/vt6655/Makefile
index 115b951bf0d9..d55c3baade53 100644
--- a/drivers/staging/vt6655/Makefile
+++ b/drivers/staging/vt6655/Makefile
@@ -11,7 +11,6 @@ vt6655_stage-y +=	device_main.o \
 	dpc.o \
 	power.o \
 	srom.o \
-	mib.o \
 	key.o \
 	rf.o
 
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
index 440537e47121..5cf1b337cba7 100644
--- a/drivers/staging/vt6655/device.h
+++ b/drivers/staging/vt6655/device.h
@@ -68,7 +68,6 @@
 
 #include "device_cfg.h"
 #include "card.h"
-#include "mib.h"
 #include "srom.h"
 #include "desc.h"
 #include "key.h"
@@ -239,7 +238,6 @@ struct vnt_private {
 	CHIP_TYPE                   chip_id;
 
 	void __iomem                *PortOffset;
-	unsigned long dwIsr;
 	u32                         memaddr;
 	u32                         ioaddr;
 	u32                         io_size;
@@ -286,11 +284,6 @@ struct vnt_private {
 	unsigned char abyCurrentNetAddr[ETH_ALEN]; __aligned(2)
 	bool bLinkPass;          /* link status: OK or fail */
 
-	/* Adapter statistics */
-	SStatCounter                scStatistic;
-	/* 802.11 counter */
-	SDot11Counters              s802_11Counter;
-
 	unsigned int	uCurrRSSI;
 	unsigned char byCurrSQ;
 
@@ -410,6 +403,10 @@ struct vnt_private {
 	unsigned char abyEEPROM[EEP_MAX_CONTEXT_SIZE]; /* unsigned long alignment */
 
 	unsigned short wBeaconInterval;
+
+	struct work_struct interrupt_work;
+
+	struct ieee80211_low_level_stats low_stats;
 };
 
 static inline PDEVICE_RD_INFO alloc_rd_info(void)
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index 0343ae386f03..ed040fbb7df8 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -32,7 +32,6 @@
  *   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_intr - interrupt handle function
  *   device_rx_srv - rx service function
  *   device_alloc_rx_buf - rx buffer pre-allocated function
  *   device_free_tx_buf - free tx buffer function
@@ -148,16 +147,6 @@ static void vt6655_init_info(struct pci_dev *pcid,
 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  irqreturn_t  device_intr(int irq,  void *dev_instance);
-
-#ifdef CONFIG_PM
-static int device_notify_reboot(struct notifier_block *, unsigned long event, void *ptr);
-static struct notifier_block device_notifier = {
-	.notifier_call = device_notify_reboot,
-	.next = NULL,
-	.priority = 0,
-};
-#endif
 
 static void device_init_rd0_ring(struct vnt_private *pDevice);
 static void device_init_rd1_ring(struct vnt_private *pDevice);
@@ -807,6 +796,10 @@ static int device_rx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 	     pRD = pRD->next) {
 		if (works++ > 15)
 			break;
+
+		if (!pRD->pRDInfo->skb)
+			break;
+
 		if (vnt_receive_frame(pDevice, pRD)) {
 			if (!device_alloc_rx_buf(pDevice, pRD)) {
 				dev_err(&pDevice->pcid->dev,
@@ -1053,129 +1046,135 @@ static void vnt_check_bb_vga(struct vnt_private *priv)
 	}
 }
 
-static  irqreturn_t  device_intr(int irq,  void *dev_instance)
+static void vnt_interrupt_process(struct vnt_private *priv)
 {
-	struct vnt_private *pDevice = dev_instance;
+	struct ieee80211_low_level_stats *low_stats = &priv->low_stats;
 	int             max_count = 0;
-	unsigned long dwMIBCounter = 0;
-	unsigned char byOrgPageSel = 0;
-	int             handled = 0;
+	u32 mib_counter;
+	u32 isr;
 	unsigned long flags;
 
-	MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
+	MACvReadISR(priv->PortOffset, &isr);
 
-	if (pDevice->dwIsr == 0)
-		return IRQ_RETVAL(handled);
+	if (isr == 0)
+		return;
 
-	if (pDevice->dwIsr == 0xffffffff) {
-		pr_debug("dwIsr = 0xffff\n");
-		return IRQ_RETVAL(handled);
+	if (isr == 0xffffffff) {
+		pr_debug("isr = 0xffff\n");
+		return;
 	}
 
-	handled = 1;
-	MACvIntDisable(pDevice->PortOffset);
+	MACvIntDisable(priv->PortOffset);
 
-	spin_lock_irqsave(&pDevice->lock, flags);
+	spin_lock_irqsave(&priv->lock, flags);
 
-	/* Make sure current page is 0 */
-	VNSvInPortB(pDevice->PortOffset + MAC_REG_PAGE1SEL, &byOrgPageSel);
-	if (byOrgPageSel == 1)
-		MACvSelectPage0(pDevice->PortOffset);
-	else
-		byOrgPageSel = 0;
+	/* Read low level stats */
+	MACvReadMIBCounter(priv->PortOffset, &mib_counter);
+
+	low_stats->dot11RTSSuccessCount += mib_counter & 0xff;
+	low_stats->dot11RTSFailureCount += (mib_counter >> 8) & 0xff;
+	low_stats->dot11ACKFailureCount += (mib_counter >> 16) & 0xff;
+	low_stats->dot11FCSErrorCount += (mib_counter >> 24) & 0xff;
 
-	MACvReadMIBCounter(pDevice->PortOffset, &dwMIBCounter);
 	/*
 	 * TBD....
 	 * Must do this after doing rx/tx, cause ISR bit is slow
 	 * than RD/TD write back
 	 * update ISR counter
 	 */
-	STAvUpdate802_11Counter(&pDevice->s802_11Counter, &pDevice->scStatistic, dwMIBCounter);
-	while (pDevice->dwIsr != 0) {
-		STAvUpdateIsrStatCounter(&pDevice->scStatistic, pDevice->dwIsr);
-		MACvWriteISR(pDevice->PortOffset, pDevice->dwIsr);
+	while (isr && priv->vif) {
+		MACvWriteISR(priv->PortOffset, isr);
 
-		if (pDevice->dwIsr & ISR_FETALERR) {
+		if (isr & ISR_FETALERR) {
 			pr_debug(" ISR_FETALERR\n");
-			VNSvOutPortB(pDevice->PortOffset + MAC_REG_SOFTPWRCTL, 0);
-			VNSvOutPortW(pDevice->PortOffset + MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPECTI);
-			device_error(pDevice, pDevice->dwIsr);
+			VNSvOutPortB(priv->PortOffset + MAC_REG_SOFTPWRCTL, 0);
+			VNSvOutPortW(priv->PortOffset +
+				     MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPECTI);
+			device_error(priv, isr);
 		}
 
-		if (pDevice->dwIsr & ISR_TBTT) {
-			if (pDevice->vif &&
-			    pDevice->op_mode != NL80211_IFTYPE_ADHOC)
-				vnt_check_bb_vga(pDevice);
+		if (isr & ISR_TBTT) {
+			if (priv->op_mode != NL80211_IFTYPE_ADHOC)
+				vnt_check_bb_vga(priv);
 
-			pDevice->bBeaconSent = false;
-			if (pDevice->bEnablePSMode)
-				PSbIsNextTBTTWakeUp((void *)pDevice);
+			priv->bBeaconSent = false;
+			if (priv->bEnablePSMode)
+				PSbIsNextTBTTWakeUp((void *)priv);
 
-			if ((pDevice->op_mode == NL80211_IFTYPE_AP ||
-			    pDevice->op_mode == NL80211_IFTYPE_ADHOC) &&
-			    pDevice->vif->bss_conf.enable_beacon) {
-				MACvOneShotTimer1MicroSec(pDevice->PortOffset,
-							  (pDevice->vif->bss_conf.beacon_int - MAKE_BEACON_RESERVED) << 10);
+			if ((priv->op_mode == NL80211_IFTYPE_AP ||
+			    priv->op_mode == NL80211_IFTYPE_ADHOC) &&
+			    priv->vif->bss_conf.enable_beacon) {
+				MACvOneShotTimer1MicroSec(priv->PortOffset,
+							  (priv->vif->bss_conf.beacon_int - MAKE_BEACON_RESERVED) << 10);
 			}
 
 			/* TODO: adhoc PS mode */
 
 		}
 
-		if (pDevice->dwIsr & ISR_BNTX) {
-			if (pDevice->op_mode == NL80211_IFTYPE_ADHOC) {
-				pDevice->bIsBeaconBufReadySet = false;
-				pDevice->cbBeaconBufReadySetCnt = 0;
+		if (isr & ISR_BNTX) {
+			if (priv->op_mode == NL80211_IFTYPE_ADHOC) {
+				priv->bIsBeaconBufReadySet = false;
+				priv->cbBeaconBufReadySetCnt = 0;
 			}
 
-			pDevice->bBeaconSent = true;
+			priv->bBeaconSent = true;
 		}
 
-		if (pDevice->dwIsr & ISR_RXDMA0)
-			max_count += device_rx_srv(pDevice, TYPE_RXDMA0);
+		if (isr & ISR_RXDMA0)
+			max_count += device_rx_srv(priv, TYPE_RXDMA0);
 
-		if (pDevice->dwIsr & ISR_RXDMA1)
-			max_count += device_rx_srv(pDevice, TYPE_RXDMA1);
+		if (isr & ISR_RXDMA1)
+			max_count += device_rx_srv(priv, TYPE_RXDMA1);
 
-		if (pDevice->dwIsr & ISR_TXDMA0)
-			max_count += device_tx_srv(pDevice, TYPE_TXDMA0);
+		if (isr & ISR_TXDMA0)
+			max_count += device_tx_srv(priv, TYPE_TXDMA0);
 
-		if (pDevice->dwIsr & ISR_AC0DMA)
-			max_count += device_tx_srv(pDevice, TYPE_AC0DMA);
+		if (isr & ISR_AC0DMA)
+			max_count += device_tx_srv(priv, TYPE_AC0DMA);
 
-		if (pDevice->dwIsr & ISR_SOFTTIMER1) {
-			if (pDevice->vif) {
-				if (pDevice->vif->bss_conf.enable_beacon)
-					vnt_beacon_make(pDevice, pDevice->vif);
-			}
+		if (isr & ISR_SOFTTIMER1) {
+			if (priv->vif->bss_conf.enable_beacon)
+				vnt_beacon_make(priv, priv->vif);
 		}
 
 		/* 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);
-		}
+		if (AVAIL_TD(priv, TYPE_TXDMA0) &&
+		    AVAIL_TD(priv, TYPE_AC0DMA) &&
+		    ieee80211_queue_stopped(priv->hw, 0))
+			ieee80211_wake_queues(priv->hw);
 
-		MACvReadISR(pDevice->PortOffset, &pDevice->dwIsr);
+		MACvReadISR(priv->PortOffset, &isr);
 
-		MACvReceive0(pDevice->PortOffset);
-		MACvReceive1(pDevice->PortOffset);
+		MACvReceive0(priv->PortOffset);
+		MACvReceive1(priv->PortOffset);
 
-		if (max_count > pDevice->sOpts.int_works)
+		if (max_count > priv->sOpts.int_works)
 			break;
 	}
 
-	if (byOrgPageSel == 1)
-		MACvSelectPage1(pDevice->PortOffset);
+	spin_unlock_irqrestore(&priv->lock, flags);
 
-	spin_unlock_irqrestore(&pDevice->lock, flags);
+	MACvIntEnable(priv->PortOffset, IMR_MASK_VALUE);
+}
 
-	MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
+static void vnt_interrupt_work(struct work_struct *work)
+{
+	struct vnt_private *priv =
+		container_of(work, struct vnt_private, interrupt_work);
 
-	return IRQ_RETVAL(handled);
+	if (priv->vif)
+		vnt_interrupt_process(priv);
+}
+
+static irqreturn_t vnt_interrupt(int irq,  void *arg)
+{
+	struct vnt_private *priv = arg;
+
+	if (priv->vif)
+		schedule_work(&priv->interrupt_work);
+
+	return IRQ_HANDLED;
 }
 
 static int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
@@ -1267,7 +1266,7 @@ static int vnt_start(struct ieee80211_hw *hw)
 	if (!device_init_rings(priv))
 		return -ENOMEM;
 
-	ret = request_irq(priv->pcid->irq, &device_intr,
+	ret = request_irq(priv->pcid->irq, &vnt_interrupt,
 			  IRQF_SHARED, "vt6655", priv);
 	if (ret) {
 		dev_dbg(&priv->pcid->dev, "failed to start irq\n");
@@ -1296,6 +1295,8 @@ static void vnt_stop(struct ieee80211_hw *hw)
 
 	ieee80211_stop_queues(hw);
 
+	cancel_work_sync(&priv->interrupt_work);
+
 	MACbShutdown(priv->PortOffset);
 	MACbSoftwareReset(priv->PortOffset);
 	CARDbRadioPowerOff(priv);
@@ -1524,21 +1525,12 @@ static void vnt_configure(struct ieee80211_hw *hw,
 	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;
+	*total_flags &= FIF_ALLMULTI | FIF_OTHER_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) {
 			unsigned long flags;
@@ -1609,6 +1601,16 @@ static int vnt_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	return 0;
 }
 
+static int vnt_get_stats(struct ieee80211_hw *hw,
+			 struct ieee80211_low_level_stats *stats)
+{
+	struct vnt_private *priv = hw->priv;
+
+	memcpy(stats, &priv->low_stats, sizeof(*stats));
+
+	return 0;
+}
+
 static u64 vnt_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	struct vnt_private *priv = hw->priv;
@@ -1646,6 +1648,7 @@ static const struct ieee80211_ops vnt_mac_ops = {
 	.prepare_multicast	= vnt_prepare_multicast,
 	.configure_filter	= vnt_configure,
 	.set_key		= vnt_set_key,
+	.get_stats		= vnt_get_stats,
 	.get_tsf		= vnt_get_tsf,
 	.set_tsf		= vnt_set_tsf,
 	.reset_tsf		= vnt_reset_tsf,
@@ -1771,6 +1774,8 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 		return -ENODEV;
 	}
 
+	INIT_WORK(&priv->interrupt_work, vnt_interrupt_work);
+
 	/* do reset */
 	if (!MACbSoftwareReset(priv->PortOffset)) {
 		dev_err(&pcid->dev, ": Failed to access MAC hardware..\n");
@@ -1802,10 +1807,10 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 		BIT(NL80211_IFTYPE_ADHOC) | BIT(NL80211_IFTYPE_AP);
 
-	priv->hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		IEEE80211_HW_SIGNAL_DBM |
-		IEEE80211_HW_TIMING_BEACON_ONLY;
+	ieee80211_hw_set(priv->hw, TIMING_BEACON_ONLY);
+	ieee80211_hw_set(priv->hw, SIGNAL_DBM);
+	ieee80211_hw_set(priv->hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(priv->hw, REPORTS_TX_ACK_STATUS);
 
 	priv->hw->max_signal = 100;
 
@@ -1864,47 +1869,4 @@ static struct pci_driver device_driver = {
 #endif
 };
 
-static int __init vt6655_init_module(void)
-{
-	int ret;
-
-	ret = pci_register_driver(&device_driver);
-#ifdef CONFIG_PM
-	if (ret >= 0)
-		register_reboot_notifier(&device_notifier);
-#endif
-
-	return ret;
-}
-
-static void __exit vt6655_cleanup_module(void)
-{
-#ifdef CONFIG_PM
-	unregister_reboot_notifier(&device_notifier);
-#endif
-	pci_unregister_driver(&device_driver);
-}
-
-module_init(vt6655_init_module);
-module_exit(vt6655_cleanup_module);
-
-#ifdef CONFIG_PM
-static int
-device_notify_reboot(struct notifier_block *nb, unsigned long event, void *p)
-{
-	struct pci_dev *pdev = NULL;
-
-	switch (event) {
-	case SYS_DOWN:
-	case SYS_HALT:
-	case SYS_POWER_OFF:
-		for_each_pci_dev(pdev) {
-			if (pci_dev_driver(pdev) == &device_driver) {
-				if (pci_get_drvdata(pdev))
-					vt6655_suspend(pdev, PMSG_HIBERNATE);
-			}
-		}
-	}
-	return NOTIFY_DONE;
-}
-#endif
+module_pci_driver(device_driver);
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
index 8048b3263360..aed530f022b8 100644
--- a/drivers/staging/vt6655/mac.c
+++ b/drivers/staging/vt6655/mac.c
@@ -70,7 +70,8 @@
  * Return Value: true if all test bits On; otherwise false
  *
  */
-bool MACbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byRegOfs, unsigned char byTestBits)
+bool MACbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byRegOfs,
+		     unsigned char byTestBits)
 {
 	unsigned char byData;
 
@@ -93,7 +94,8 @@ bool MACbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byRegOfs, unsigned ch
  * Return Value: true if all test bits Off; otherwise false
  *
  */
-bool MACbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byRegOfs, unsigned char byTestBits)
+bool MACbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byRegOfs,
+		      unsigned char byTestBits)
 {
 	unsigned char byData;
 
@@ -218,7 +220,8 @@ void MACvSaveContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf)
 
 	/* read page1 register */
 	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++)
-		VNSvInPortB((dwIoBase + ii), (pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
+		VNSvInPortB((dwIoBase + ii),
+			    (pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
 
 	MACvSelectPage0(dwIoBase);
 }
@@ -244,7 +247,8 @@ void MACvRestoreContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf)
 	MACvSelectPage1(dwIoBase);
 	/* restore page1 */
 	for (ii = 0; ii < MAC_MAX_CONTEXT_SIZE_PAGE1; ii++)
-		VNSvOutPortB((dwIoBase + ii), *(pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
+		VNSvOutPortB((dwIoBase + ii),
+			     *(pbyCxtBuf + MAC_MAX_CONTEXT_SIZE_PAGE0 + ii));
 
 	MACvSelectPage0(dwIoBase);
 
@@ -263,13 +267,18 @@ void MACvRestoreContext(void __iomem *dwIoBase, unsigned char *pbyCxtBuf)
 		VNSvOutPortB(dwIoBase + ii, *(pbyCxtBuf + ii));
 
 	/* restore CURR_RX_DESC_ADDR, CURR_TX_DESC_ADDR */
-	VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0, *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0));
-	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR));
-	VNSvOutPortD(dwIoBase + MAC_REG_BCNDMAPTR, *(unsigned long *)(pbyCxtBuf + MAC_REG_BCNDMAPTR));
-
-	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR0, *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0));
-
-	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR1, *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1));
+	VNSvOutPortD(dwIoBase + MAC_REG_TXDMAPTR0,
+		     *(unsigned long *)(pbyCxtBuf + MAC_REG_TXDMAPTR0));
+	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR,
+		     *(unsigned long *)(pbyCxtBuf + MAC_REG_AC0DMAPTR));
+	VNSvOutPortD(dwIoBase + MAC_REG_BCNDMAPTR,
+		     *(unsigned long *)(pbyCxtBuf + MAC_REG_BCNDMAPTR));
+
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR0,
+		     *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR0));
+
+	VNSvOutPortD(dwIoBase + MAC_REG_RXDMAPTR1,
+		     *(unsigned long *)(pbyCxtBuf + MAC_REG_RXDMAPTR1));
 }
 
 /*
@@ -641,7 +650,8 @@ void MACvSetCurrRx1DescAddr(void __iomem *dwIoBase, unsigned long dwCurrDescAddr
  * Return Value: none
  *
  */
-void MACvSetCurrTx0DescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAddr)
+void MACvSetCurrTx0DescAddrEx(void __iomem *dwIoBase,
+			      unsigned long dwCurrDescAddr)
 {
 	unsigned short ww;
 	unsigned char byData;
@@ -679,7 +689,8 @@ void MACvSetCurrTx0DescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAd
  *
  */
 /* TxDMA1 = AC0DMA */
-void MACvSetCurrAC0DescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAddr)
+void MACvSetCurrAC0DescAddrEx(void __iomem *dwIoBase,
+			      unsigned long dwCurrDescAddr)
 {
 	unsigned short ww;
 	unsigned char byData;
@@ -703,7 +714,8 @@ void MACvSetCurrAC0DescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAd
 		VNSvOutPortB(dwIoBase + MAC_REG_AC0DMACTL, DMACTL_RUN);
 }
 
-void MACvSetCurrTXDescAddr(int iTxType, void __iomem *dwIoBase, unsigned long dwCurrDescAddr)
+void MACvSetCurrTXDescAddr(int iTxType, void __iomem *dwIoBase,
+			   unsigned long dwCurrDescAddr)
 {
 	if (iTxType == TYPE_AC0DMA)
 		MACvSetCurrAC0DescAddrEx(dwIoBase, dwCurrDescAddr);
@@ -767,7 +779,8 @@ void MACvOneShotTimer1MicroSec(void __iomem *dwIoBase, unsigned int uDelayTime)
 	VNSvOutPortB(dwIoBase + MAC_REG_TMCTL1, (TMCTL_TMD | TMCTL_TE));
 }
 
-void MACvSetMISCFifo(void __iomem *dwIoBase, unsigned short wOffset, unsigned long dwData)
+void MACvSetMISCFifo(void __iomem *dwIoBase, unsigned short wOffset,
+		     unsigned long dwData)
 {
 	if (wOffset > 273)
 		return;
@@ -816,8 +829,10 @@ 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 MACvSetKeyEntry(void __iomem *dwIoBase, unsigned short wKeyCtl,
+		     unsigned int uEntryIdx, unsigned int uKeyIdx,
+		     unsigned char *pbyAddr, u32 *pdwKey,
+		     unsigned char byLocalID)
 {
 	unsigned short wOffset;
 	u32 dwData;
diff --git a/drivers/staging/vt6655/mib.c b/drivers/staging/vt6655/mib.c
deleted file mode 100644
index d55c762027ed..000000000000
--- a/drivers/staging/vt6655/mib.c
+++ /dev/null
@@ -1,139 +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: mib.c
- *
- * Purpose: Implement MIB Data Structure
- *
- * Author: Tevin Chen
- *
- * Date: May 21, 1996
- *
- * Functions:
- *      STAvUpdateIstStatCounter - Update ISR statistic counter
- *      STAvUpdate802_11Counter - Update 802.11 mib counter
- *
- * Revision History:
- *
- */
-
-#include "mac.h"
-#include "mib.h"
-
-/*---------------------  Static Classes  ----------------------------*/
-
-/*---------------------  Static Variables  --------------------------*/
-
-/*---------------------  Static Functions  --------------------------*/
-
-/*---------------------  Export Variables  --------------------------*/
-
-/*---------------------  Export Functions  --------------------------*/
-
-/*
- * Description: Update Isr Statistic Counter
- *
- * Parameters:
- *  In:
- *      pStatistic  - Pointer to Statistic Counter Data Structure
- *      wisr        - Interrupt status
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void STAvUpdateIsrStatCounter(PSStatCounter pStatistic, unsigned long dwIsr)
-{
-	/**********************/
-	/* ABNORMAL interrupt */
-	/**********************/
-	/* not any IMR bit invoke irq */
-
-	if (dwIsr == 0) {
-		pStatistic->ISRStat.dwIsrUnknown++;
-		return;
-	}
-
-/* Added by Kyle */
-	if (dwIsr & ISR_TXDMA0)               /* ISR, bit0 */
-		pStatistic->ISRStat.dwIsrTx0OK++;             /* TXDMA0 successful */
-
-	if (dwIsr & ISR_AC0DMA)               /* ISR, bit1 */
-		pStatistic->ISRStat.dwIsrAC0TxOK++;           /* AC0DMA successful */
-
-	if (dwIsr & ISR_BNTX)                 /* ISR, bit2 */
-		pStatistic->ISRStat.dwIsrBeaconTxOK++;        /* BeaconTx successful */
-
-	if (dwIsr & ISR_RXDMA0)               /* ISR, bit3 */
-		pStatistic->ISRStat.dwIsrRx0OK++;             /* Rx0 successful */
-
-	if (dwIsr & ISR_TBTT)                 /* ISR, bit4 */
-		pStatistic->ISRStat.dwIsrTBTTInt++;           /* TBTT successful */
-
-	if (dwIsr & ISR_SOFTTIMER)            /* ISR, bit6 */
-		pStatistic->ISRStat.dwIsrSTIMERInt++;
-
-	if (dwIsr & ISR_WATCHDOG)             /* ISR, bit7 */
-		pStatistic->ISRStat.dwIsrWatchDog++;
-
-	if (dwIsr & ISR_FETALERR)             /* ISR, bit8 */
-		pStatistic->ISRStat.dwIsrUnrecoverableError++;
-
-	if (dwIsr & ISR_SOFTINT)              /* ISR, bit9 */
-		pStatistic->ISRStat.dwIsrSoftInterrupt++;     /* software interrupt */
-
-	if (dwIsr & ISR_MIBNEARFULL)          /* ISR, bit10 */
-		pStatistic->ISRStat.dwIsrMIBNearfull++;
-
-	if (dwIsr & ISR_RXNOBUF)              /* ISR, bit11 */
-		pStatistic->ISRStat.dwIsrRxNoBuf++;           /* Rx No Buff */
-
-	if (dwIsr & ISR_RXDMA1)               /* ISR, bit12 */
-		pStatistic->ISRStat.dwIsrRx1OK++;             /* Rx1 successful */
-
-	if (dwIsr & ISR_SOFTTIMER1)           /* ISR, bit21 */
-		pStatistic->ISRStat.dwIsrSTIMER1Int++;
-}
-
-/*
- * Description: Update 802.11 mib counter
- *
- * Parameters:
- *  In:
- *      p802_11Counter  - Pointer to 802.11 mib counter
- *      pStatistic      - Pointer to Statistic Counter Data Structure
- *      dwCounter       - hardware counter for 802.11 mib
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void
-STAvUpdate802_11Counter(
-	PSDot11Counters         p802_11Counter,
-	PSStatCounter           pStatistic,
-	unsigned long dwCounter
-)
-{
-	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);
-}
diff --git a/drivers/staging/vt6655/mib.h b/drivers/staging/vt6655/mib.h
deleted file mode 100644
index 5cb59b8a1c7c..000000000000
--- a/drivers/staging/vt6655/mib.h
+++ /dev/null
@@ -1,82 +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: mib.h
- *
- * Purpose: Implement MIB Data Structure
- *
- * Author: Tevin Chen
- *
- * Date: May 21, 1996
- *
- */
-
-#ifndef __MIB_H__
-#define __MIB_H__
-
-#include "desc.h"
-
-//
-// 802.11 counter
-//
-
-typedef struct tagSDot11Counters {
-	unsigned long long   RTSSuccessCount;
-	unsigned long long   RTSFailureCount;
-	unsigned long long   ACKFailureCount;
-	unsigned long long   FCSErrorCount;
-} SDot11Counters, *PSDot11Counters;
-
-//
-// Custom counter
-//
-typedef struct tagSISRCounters {
-	unsigned long dwIsrTx0OK;
-	unsigned long dwIsrAC0TxOK;
-	unsigned long dwIsrBeaconTxOK;
-	unsigned long dwIsrRx0OK;
-	unsigned long dwIsrTBTTInt;
-	unsigned long dwIsrSTIMERInt;
-	unsigned long dwIsrWatchDog;
-	unsigned long dwIsrUnrecoverableError;
-	unsigned long dwIsrSoftInterrupt;
-	unsigned long dwIsrMIBNearfull;
-	unsigned long dwIsrRxNoBuf;
-
-	unsigned long dwIsrUnknown;
-
-	unsigned long dwIsrRx1OK;
-	unsigned long dwIsrSTIMER1Int;
-} SISRCounters, *PSISRCounters;
-
-//
-// statistic counter
-//
-typedef struct tagSStatCounter {
-	SISRCounters ISRStat;
-} SStatCounter, *PSStatCounter;
-
-void STAvUpdateIsrStatCounter(PSStatCounter pStatistic, unsigned long dwIsr);
-
-void STAvUpdate802_11Counter(
-	PSDot11Counters p802_11Counter,
-	PSStatCounter   pStatistic,
-	unsigned long dwCounter
-);
-
-#endif // __MIB_H__
diff --git a/drivers/staging/vt6655/power.h b/drivers/staging/vt6655/power.h
index 1083341b2a47..538e68507bb0 100644
--- a/drivers/staging/vt6655/power.h
+++ b/drivers/staging/vt6655/power.h
@@ -29,9 +29,9 @@
 #ifndef __POWER_H__
 #define __POWER_H__
 
-#define     C_PWBT                   1000      // micro sec. power up before TBTT
-#define     PS_FAST_INTERVAL         1         // Fast power saving listen interval
-#define     PS_MAX_INTERVAL          4         // MAX power saving listen interval
+#define C_PWBT                   1000    /* micro sec. power up before TBTT */
+#define PS_FAST_INTERVAL         1       /* Fast power saving listen interval */
+#define PS_MAX_INTERVAL          4       /* MAX power saving listen interval */
 
 void
 PSvDisablePowerSaving(
@@ -50,4 +50,4 @@ PSbIsNextTBTTWakeUp(
 	void *hDeviceContext
 );
 
-#endif //__POWER_H__
+#endif /* __POWER_H__ */
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index 74687761bd2e..534338c46619 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -656,7 +656,7 @@ s_vFillRTSHead(
 		uRTSFrameLen -= 4;
 	}
 
-	/* Note: So far RTSHead dosen't appear in ATIM & Beacom DMA, so we don't need to take them into account.
+	/* Note: So far RTSHead doesn't appear in ATIM & Beacom DMA, so we don't need to take them into account.
 	       Otherwise, we need to modify codes for them. */
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (byFBOption == AUTO_FB_NONE) {
@@ -1093,7 +1093,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {/* 802.11g packet */
 
 		if (byFBOption == AUTO_FB_NONE) {
-			if (bRTS == true) {/* RTS_need */
+			if (bRTS) {/* RTS_need */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts));
 				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) + cbMICHDR);
@@ -1115,7 +1115,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 			}
 		} else {
 			/* Auto Fall Back */
-			if (bRTS == true) {/* RTS_need */
+			if (bRTS) {/* RTS_need */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts));
 				pvRTS = (void *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) + cbMICHDR);
@@ -1138,7 +1138,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 	} else {/* 802.11a/b packet */
 
 		if (byFBOption == AUTO_FB_NONE) {
-			if (bRTS == true) {
+			if (bRTS) {
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
 				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
@@ -1158,7 +1158,7 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 			}
 		} else {
 			/* Auto Fall Back */
-			if (bRTS == true) { /* RTS_need */
+			if (bRTS) { /* RTS_need */
 				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
 				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index 67ff13f4f731..a215563de07d 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -389,7 +389,7 @@ void vnt_update_ifs(struct vnt_private *priv)
 			}
 		}
 
-		if (ofdm_rate == true)
+		if (ofdm_rate)
 			max_min = 4;
 		else
 			max_min = 5;
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
index f71d59fa3b21..635d931de409 100644
--- a/drivers/staging/vt6656/device.h
+++ b/drivers/staging/vt6656/device.h
@@ -364,7 +364,7 @@ struct vnt_private {
 	/* Power save */
 	u16 current_aid;
 
-	/* Beacon releated */
+	/* Beacon related */
 	u16 seq_counter;
 
 	enum vnt_cmd_state command_state;
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index ab3ab84cb0a7..f97323f19acf 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -522,7 +522,7 @@ static int vnt_start(struct ieee80211_hw *hw)
 
 	priv->rx_buf_sz = MAX_TOTAL_SIZE_WITH_ALL_HEADERS;
 
-	if (vnt_alloc_bufs(priv) == false) {
+	if (!vnt_alloc_bufs(priv)) {
 		dev_dbg(&priv->usb->dev, "vnt_alloc_bufs fail...\n");
 		return -ENOMEM;
 	}
@@ -785,8 +785,7 @@ static void vnt_configure(struct ieee80211_hw *hw,
 	u8 rx_mode = 0;
 	int rc;
 
-	*total_flags &= FIF_ALLMULTI | FIF_OTHER_BSS | FIF_PROMISC_IN_BSS |
-		FIF_BCN_PRBRESP_PROMISC;
+	*total_flags &= FIF_ALLMULTI | FIF_OTHER_BSS | FIF_BCN_PRBRESP_PROMISC;
 
 	rc = vnt_control_in(priv, MESSAGE_TYPE_READ, MAC_REG_RCR,
 		MESSAGE_REQUEST_MACREG, sizeof(u8), &rx_mode);
@@ -796,14 +795,6 @@ static void vnt_configure(struct ieee80211_hw *hw,
 
 	dev_dbg(&priv->usb->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)
@@ -987,10 +978,10 @@ vt6656_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 		BIT(NL80211_IFTYPE_ADHOC) | BIT(NL80211_IFTYPE_AP);
 
-	priv->hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-		IEEE80211_HW_SIGNAL_DBM |
-		IEEE80211_HW_TIMING_BEACON_ONLY;
+	ieee80211_hw_set(priv->hw, TIMING_BEACON_ONLY);
+	ieee80211_hw_set(priv->hw, SIGNAL_DBM);
+	ieee80211_hw_set(priv->hw, RX_INCLUDES_FCS);
+	ieee80211_hw_set(priv->hw, REPORTS_TX_ACK_STATUS);
 
 	priv->hw->max_signal = 100;
 
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index 5c589962a1e8..8116791f4f06 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -87,7 +87,7 @@ static struct vnt_usb_send_context
 			return NULL;
 
 		context = priv->tx_context[ii];
-		if (context->in_use == false) {
+		if (!context->in_use) {
 			context->in_use = true;
 			memset(context->data, 0,
 					MAX_TOTAL_SIZE_WITH_ALL_HEADERS);
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index 88bf518f23eb..a5912dde9b31 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -141,7 +141,7 @@ int vnt_start_interrupt_urb(struct vnt_private *priv)
 {
 	int status = STATUS_FAILURE;
 
-	if (priv->int_buf.in_use == true)
+	if (priv->int_buf.in_use)
 		return STATUS_FAILURE;
 
 	priv->int_buf.in_use = true;
diff --git a/drivers/staging/wilc1000/Kconfig b/drivers/staging/wilc1000/Kconfig
new file mode 100644
index 000000000000..062d9c5ca4bd
--- /dev/null
+++ b/drivers/staging/wilc1000/Kconfig
@@ -0,0 +1,59 @@
+config WILC1000
+	tristate "WILC1000 support (WiFi only)"
+	depends on BROKEN
+	depends on !S390
+	depends on CFG80211 && WEXT_CORE && INET
+	depends on MMC || SPI
+	---help---
+	  This module only support IEEE 802.11n WiFi.
+
+choice
+        prompt "Memory Allocation"
+        depends on WILC1000
+        default WILC1000_PREALLOCATE_AT_LOADING_DRIVER
+
+config WILC1000_PREALLOCATE_AT_LOADING_DRIVER
+	bool "Preallocate memory at loading driver"
+	---help---
+	  This choice supports static allocation of the memory
+	  for the receive buffer. The driver will allocate the RX buffer
+	  during initial time. The driver will also free the buffer
+	  by calling network device stop.
+
+config WILC1000_DYNAMICALLY_ALLOCATE_MEMROY
+        bool "Dynamically allocate memory in real time"
+        ---help---
+	  This choice supports dynamic allocation of the memory
+	  for the receive buffer. The driver will allocate the RX buffer
+	  when it is required.
+endchoice
+
+choice
+	prompt "Bus Type"
+	depends on WILC1000
+	default WILC1000_SDIO
+
+	config WILC1000_SDIO
+	bool "SDIO support"
+	depends on MMC
+	---help---
+	  This module adds support for the SDIO interface
+	  of adapters using WILC chipset. Select this if
+	  your platform is using the SDIO bus.
+
+	config WILC1000_SPI
+	depends on SPI
+	bool "SPI support"
+	---help---
+	  This module adds support for the SPI interface
+	  of adapters using WILC chipset. Select this if
+	  your platform is using the SPI bus.
+endchoice
+
+config WILC1000_HW_OOB_INTR
+	bool "Use out of band interrupt"
+	depends on WILC1000 && WILC1000_SDIO
+	default n
+	---help---
+	  If your platform don't recognize SDIO IRQ, connect chipset external IRQ pin
+	  and check this option. Or, Use this to get all interrupts including SDIO interrupts.
diff --git a/drivers/staging/wilc1000/Makefile b/drivers/staging/wilc1000/Makefile
new file mode 100644
index 000000000000..a78c4d529a58
--- /dev/null
+++ b/drivers/staging/wilc1000/Makefile
@@ -0,0 +1,34 @@
+obj-$(CONFIG_WILC1000) += wilc1000.o
+obj-$(CONFIG_WILC1000_PREALLOCATE_DURING_SYSTEM_BOOT) += wilc_exported_buf.o
+
+
+ccflags-$(CONFIG_WILC1000_SDIO) += -DWILC_SDIO -DCOMPLEMENT_BOOT
+ccflags-$(CONFIG_WILC1000_HW_OOB_INTR) += -DWILC_SDIO_IRQ_GPIO
+ccflags-$(CONFIG_WILC1000_SPI) += -DWILC_SPI
+
+ccflags-y += -DSTA_FIRMWARE=\"atmel/wilc1000_fw.bin\" \
+		-DAP_FIRMWARE=\"atmel/wilc1000_ap_fw.bin\" \
+		-DP2P_CONCURRENCY_FIRMWARE=\"atmel/wilc1000_p2p_fw.bin\"
+
+ccflags-y += -I$(src)/ -D__CHECK_ENDIAN__ -DWILC_ASIC_A0 \
+		-DPLL_WORKAROUND -DCONNECT_DIRECT  -DAGING_ALG \
+		-DWILC_PARSE_SCAN_IN_HOST -DDISABLE_PWRSAVE_AND_SCAN_DURING_IP \
+		-Wno-unused-function -DUSE_WIRELESS -DWILC_DEBUGFS
+#ccflags-y += -DTCP_ACK_FILTER
+
+ccflags-$(CONFIG_WILC1000_PREALLOCATE_DURING_SYSTEM_BOOT) += -DMEMORY_STATIC \
+								-DWILC_PREALLOC_AT_BOOT
+
+ccflags-$(CONFIG_WILC1000_PREALLOCATE_AT_LOADING_DRIVER) += -DMEMORY_STATIC \
+								-DWILC_PREALLOC_AT_INSMOD
+
+ccflags-$(CONFIG_WILC1000_DYNAMICALLY_ALLOCATE_MEMROY) += -DWILC_NORMAL_ALLOC
+
+
+wilc1000-objs := wilc_wfi_netdevice.o wilc_wfi_cfgoperations.o linux_wlan.o linux_mon.o \
+			wilc_memory.o wilc_msgqueue.o wilc_sleep.o wilc_strutils.o \
+			wilc_timer.o coreconfigurator.o host_interface.o \
+			fifo_buffer.o wilc_sdio.o wilc_spi.o wilc_wlan_cfg.o wilc_debugfs.o
+
+wilc1000-$(CONFIG_WILC1000_SDIO) += linux_wlan_sdio.o
+wilc1000-$(CONFIG_WILC1000_SPI) += linux_wlan_spi.o
diff --git a/drivers/staging/wilc1000/TODO b/drivers/staging/wilc1000/TODO
new file mode 100644
index 000000000000..95199d80a3e4
--- /dev/null
+++ b/drivers/staging/wilc1000/TODO
@@ -0,0 +1,14 @@
+TODO:
+- remove the defined feature as kernel versions
+- remove OS wrapper functions
+- remove custom debug and tracing functions
+- rework comments and function headers(also coding style)
+- replace all semaphores with mutexes or completions
+- make spi and sdio components coexist in one build
+- turn compile-time platform configuration (BEAGLE_BOARD,
+  PANDA_BOARD, PLAT_WMS8304, PLAT_RKXXXX, CUSTOMER_PLATFORM, ...)
+  into run-time options that are read from DT
+- support soft-ap and p2p mode
+- support resume/suspend function
+- replace SIOCDEVPRIVATE commands with generic API functions
+- use wext-core handling instead of private SIOCSIWPRIV implementation
diff --git a/drivers/staging/wilc1000/coreconfigsimulator.h b/drivers/staging/wilc1000/coreconfigsimulator.h
new file mode 100644
index 000000000000..5e01f8e4a41d
--- /dev/null
+++ b/drivers/staging/wilc1000/coreconfigsimulator.h
@@ -0,0 +1,17 @@
+
+/*!
+ *  @file	coreconfigsimulator.h
+ *  @brief
+ *  @author
+ *  @sa		coreconfigsimulator.c
+ *  @date	1 Mar 2012
+ *  @version	1.0
+ */
+
+#ifndef CORECONFIGSIMULATOR_H
+#define CORECONFIGSIMULATOR_H
+
+extern s32 CoreConfigSimulatorInit(void);
+extern s32 CoreConfigSimulatorDeInit(void);
+
+#endif
diff --git a/drivers/staging/wilc1000/coreconfigurator.c b/drivers/staging/wilc1000/coreconfigurator.c
new file mode 100644
index 000000000000..ed6ac45c0385
--- /dev/null
+++ b/drivers/staging/wilc1000/coreconfigurator.c
@@ -0,0 +1,2156 @@
+
+/*!
+ *  @file	coreconfigurator.c
+ *  @brief
+ *  @author
+ *  @sa		coreconfigurator.h
+ *  @date	1 Mar 2012
+ *  @version	1.0
+ */
+
+
+/*****************************************************************************/
+/* File Includes                                                             */
+/*****************************************************************************/
+#include "coreconfigurator.h"
+/*****************************************************************************/
+/* Constants                                                                 */
+/*****************************************************************************/
+#define INLINE static __inline
+#define PHY_802_11n
+#define MAX_CFG_PKTLEN     1450
+#define MSG_HEADER_LEN     4
+#define QUERY_MSG_TYPE     'Q'
+#define WRITE_MSG_TYPE     'W'
+#define RESP_MSG_TYPE      'R'
+#define WRITE_RESP_SUCCESS 1
+#define INVALID         255
+#define MAC_ADDR_LEN    6
+#define TAG_PARAM_OFFSET	(MAC_HDR_LEN + TIME_STAMP_LEN + \
+							BEACON_INTERVAL_LEN + CAP_INFO_LEN)
+
+/*****************************************************************************/
+/* Function Macros                                                           */
+/*****************************************************************************/
+
+
+/*****************************************************************************/
+/* Type Definitions                                                          */
+/*****************************************************************************/
+
+/* Basic Frame Type Codes (2-bit) */
+typedef enum {
+	FRAME_TYPE_CONTROL     = 0x04,
+	FRAME_TYPE_DATA        = 0x08,
+	FRAME_TYPE_MANAGEMENT  = 0x00,
+	FRAME_TYPE_RESERVED    = 0x0C,
+	FRAME_TYPE_FORCE_32BIT = 0xFFFFFFFF
+} tenuBasicFrmType;
+
+/* Frame Type and Subtype Codes (6-bit) */
+typedef enum {
+	ASSOC_REQ             = 0x00,
+	ASSOC_RSP             = 0x10,
+	REASSOC_REQ           = 0x20,
+	REASSOC_RSP           = 0x30,
+	PROBE_REQ             = 0x40,
+	PROBE_RSP             = 0x50,
+	BEACON                = 0x80,
+	ATIM                  = 0x90,
+	DISASOC               = 0xA0,
+	AUTH                  = 0xB0,
+	DEAUTH                = 0xC0,
+	ACTION                = 0xD0,
+	PS_POLL               = 0xA4,
+	RTS                   = 0xB4,
+	CTS                   = 0xC4,
+	ACK                   = 0xD4,
+	CFEND                 = 0xE4,
+	CFEND_ACK             = 0xF4,
+	DATA                  = 0x08,
+	DATA_ACK              = 0x18,
+	DATA_POLL             = 0x28,
+	DATA_POLL_ACK         = 0x38,
+	NULL_FRAME            = 0x48,
+	CFACK                 = 0x58,
+	CFPOLL                = 0x68,
+	CFPOLL_ACK            = 0x78,
+	QOS_DATA              = 0x88,
+	QOS_DATA_ACK          = 0x98,
+	QOS_DATA_POLL         = 0xA8,
+	QOS_DATA_POLL_ACK     = 0xB8,
+	QOS_NULL_FRAME        = 0xC8,
+	QOS_CFPOLL            = 0xE8,
+	QOS_CFPOLL_ACK        = 0xF8,
+	BLOCKACK_REQ          = 0x84,
+	BLOCKACK              = 0x94,
+	FRAME_SUBTYPE_FORCE_32BIT  = 0xFFFFFFFF
+} tenuFrmSubtype;
+
+/* Basic Frame Classes */
+typedef enum {
+	CLASS1_FRAME_TYPE      = 0x00,
+	CLASS2_FRAME_TYPE      = 0x01,
+	CLASS3_FRAME_TYPE      = 0x02,
+	FRAME_CLASS_FORCE_32BIT  = 0xFFFFFFFF
+} tenuFrameClass;
+
+/* Element ID  of various Information Elements */
+typedef enum {
+	ISSID               = 0,   /* Service Set Identifier         */
+	ISUPRATES           = 1,   /* Supported Rates                */
+	IFHPARMS            = 2,   /* FH parameter set               */
+	IDSPARMS            = 3,   /* DS parameter set               */
+	ICFPARMS            = 4,   /* CF parameter set               */
+	ITIM                = 5,   /* Traffic Information Map        */
+	IIBPARMS            = 6,   /* IBSS parameter set             */
+	ICOUNTRY            = 7,   /* Country element                */
+	IEDCAPARAMS         = 12,  /* EDCA parameter set             */
+	ITSPEC              = 13,  /* Traffic Specification          */
+	ITCLAS              = 14,  /* Traffic Classification         */
+	ISCHED              = 15,  /* Schedule                       */
+	ICTEXT              = 16,  /* Challenge Text                 */
+	IPOWERCONSTRAINT    = 32,  /* Power Constraint               */
+	IPOWERCAPABILITY    = 33,  /* Power Capability               */
+	ITPCREQUEST         = 34,  /* TPC Request                    */
+	ITPCREPORT          = 35,  /* TPC Report                     */
+	ISUPCHANNEL         = 36,  /* Supported channel list         */
+	ICHSWANNOUNC        = 37,  /* Channel Switch Announcement    */
+	IMEASUREMENTREQUEST = 38,  /* Measurement request            */
+	IMEASUREMENTREPORT  = 39,  /* Measurement report             */
+	IQUIET              = 40,  /* Quiet element Info             */
+	IIBSSDFS            = 41,  /* IBSS DFS                       */
+	IERPINFO            = 42,  /* ERP Information                */
+	ITSDELAY            = 43,  /* TS Delay                       */
+	ITCLASPROCESS       = 44,  /* TCLAS Processing               */
+	IHTCAP              = 45,  /* HT Capabilities                */
+	IQOSCAP             = 46,  /* QoS Capability                 */
+	IRSNELEMENT         = 48,  /* RSN Information Element        */
+	IEXSUPRATES         = 50,  /* Extended Supported Rates       */
+	IEXCHSWANNOUNC      = 60,  /* Extended Ch Switch Announcement*/
+	IHTOPERATION        = 61,  /* HT Information                 */
+	ISECCHOFF           = 62,  /* Secondary Channel Offeset      */
+	I2040COEX           = 72,  /* 20/40 Coexistence IE           */
+	I2040INTOLCHREPORT  = 73,  /* 20/40 Intolerant channel report*/
+	IOBSSSCAN           = 74,  /* OBSS Scan parameters           */
+	IEXTCAP             = 127, /* Extended capability            */
+	IWMM                = 221, /* WMM parameters                 */
+	IWPAELEMENT         = 221, /* WPA Information Element        */
+	INFOELEM_ID_FORCE_32BIT  = 0xFFFFFFFF
+} tenuInfoElemID;
+
+
+typedef struct {
+	char *pcRespBuffer;
+	s32 s32MaxRespBuffLen;
+	s32 s32BytesRead;
+	bool bRespRequired;
+} tstrConfigPktInfo;
+
+
+
+/*****************************************************************************/
+/* Extern Variable Declarations                                              */
+/*****************************************************************************/
+
+
+/*****************************************************************************/
+/* Extern Function Declarations                                              */
+/*****************************************************************************/
+extern s32 SendRawPacket(s8 *ps8Packet, s32 s32PacketLen);
+extern void NetworkInfoReceived(u8 *pu8Buffer, u32 u32Length);
+extern void GnrlAsyncInfoReceived(u8 *pu8Buffer, u32 u32Length);
+extern void host_int_ScanCompleteReceived(u8 *pu8Buffer, u32 u32Length);
+/*****************************************************************************/
+/* Global Variables                                                          */
+/*****************************************************************************/
+static struct semaphore SemHandleSendPkt;
+static struct semaphore SemHandlePktResp;
+
+static s8 *gps8ConfigPacket;
+
+static tstrConfigPktInfo gstrConfigPktInfo;
+
+static u8 g_seqno;
+
+static s16 g_wid_num          = -1;
+
+static u16 Res_Len;
+
+static u8 g_oper_mode    = SET_CFG;
+
+/* WID Switches */
+static tstrWID gastrWIDs[] = {
+	{WID_FIRMWARE_VERSION,          WID_STR},
+	{WID_PHY_VERSION,               WID_STR},
+	{WID_HARDWARE_VERSION,          WID_STR},
+	{WID_BSS_TYPE,                  WID_CHAR},
+	{WID_QOS_ENABLE,                WID_CHAR},
+	{WID_11I_MODE,                  WID_CHAR},
+	{WID_CURRENT_TX_RATE,           WID_CHAR},
+	{WID_LINKSPEED,                 WID_CHAR},
+	{WID_RTS_THRESHOLD,             WID_SHORT},
+	{WID_FRAG_THRESHOLD,            WID_SHORT},
+	{WID_SSID,                      WID_STR},
+	{WID_BSSID,                     WID_ADR},
+	{WID_BEACON_INTERVAL,           WID_SHORT},
+	{WID_POWER_MANAGEMENT,          WID_CHAR},
+	{WID_LISTEN_INTERVAL,           WID_CHAR},
+	{WID_DTIM_PERIOD,               WID_CHAR},
+	{WID_CURRENT_CHANNEL,           WID_CHAR},
+	{WID_TX_POWER_LEVEL_11A,        WID_CHAR},
+	{WID_TX_POWER_LEVEL_11B,        WID_CHAR},
+	{WID_PREAMBLE,                  WID_CHAR},
+	{WID_11G_OPERATING_MODE,        WID_CHAR},
+	{WID_MAC_ADDR,                  WID_ADR},
+	{WID_IP_ADDRESS,                WID_ADR},
+	{WID_ACK_POLICY,                WID_CHAR},
+	{WID_PHY_ACTIVE_REG,            WID_CHAR},
+	{WID_AUTH_TYPE,                 WID_CHAR},
+	{WID_REKEY_POLICY,              WID_CHAR},
+	{WID_REKEY_PERIOD,              WID_INT},
+	{WID_REKEY_PACKET_COUNT,        WID_INT},
+	{WID_11I_PSK,                   WID_STR},
+	{WID_1X_KEY,                    WID_STR},
+	{WID_1X_SERV_ADDR,              WID_IP},
+	{WID_SUPP_USERNAME,             WID_STR},
+	{WID_SUPP_PASSWORD,             WID_STR},
+	{WID_USER_CONTROL_ON_TX_POWER,  WID_CHAR},
+	{WID_MEMORY_ADDRESS,            WID_INT},
+	{WID_MEMORY_ACCESS_32BIT,       WID_INT},
+	{WID_MEMORY_ACCESS_16BIT,       WID_SHORT},
+	{WID_MEMORY_ACCESS_8BIT,        WID_CHAR},
+	{WID_SITE_SURVEY_RESULTS,       WID_STR},
+	{WID_PMKID_INFO,                WID_STR},
+	{WID_ASSOC_RES_INFO,            WID_STR},
+	{WID_MANUFACTURER,              WID_STR}, /* 4 Wids added for the CAPI tool*/
+	{WID_MODEL_NAME,                WID_STR},
+	{WID_MODEL_NUM,                 WID_STR},
+	{WID_DEVICE_NAME,               WID_STR},
+	{WID_SSID_PROBE_REQ,            WID_STR},
+
+#ifdef MAC_802_11N
+	{WID_11N_ENABLE,                WID_CHAR},
+	{WID_11N_CURRENT_TX_MCS,        WID_CHAR},
+	{WID_TX_POWER_LEVEL_11N,        WID_CHAR},
+	{WID_11N_OPERATING_MODE,        WID_CHAR},
+	{WID_11N_SMPS_MODE,             WID_CHAR},
+	{WID_11N_PROT_MECH,             WID_CHAR},
+	{WID_11N_ERP_PROT_TYPE,         WID_CHAR},
+	{WID_11N_HT_PROT_TYPE,          WID_CHAR},
+	{WID_11N_PHY_ACTIVE_REG_VAL,    WID_INT},
+	{WID_11N_PRINT_STATS,           WID_CHAR},
+	{WID_11N_AUTORATE_TABLE,        WID_BIN_DATA},
+	{WID_HOST_CONFIG_IF_TYPE,       WID_CHAR},
+	{WID_HOST_DATA_IF_TYPE,         WID_CHAR},
+	{WID_11N_SIG_QUAL_VAL,          WID_SHORT},
+	{WID_11N_IMMEDIATE_BA_ENABLED,  WID_CHAR},
+	{WID_11N_TXOP_PROT_DISABLE,     WID_CHAR},
+	{WID_11N_SHORT_GI_20MHZ_ENABLE, WID_CHAR},
+	{WID_SHORT_SLOT_ALLOWED,        WID_CHAR},
+	{WID_11W_ENABLE,                WID_CHAR},
+	{WID_11W_MGMT_PROT_REQ,         WID_CHAR},
+	{WID_2040_ENABLE,               WID_CHAR},
+	{WID_2040_COEXISTENCE,          WID_CHAR},
+	{WID_USER_SEC_CHANNEL_OFFSET,   WID_CHAR},
+	{WID_2040_CURR_CHANNEL_OFFSET,  WID_CHAR},
+	{WID_2040_40MHZ_INTOLERANT,     WID_CHAR},
+	{WID_HUT_RESTART,               WID_CHAR},
+	{WID_HUT_NUM_TX_PKTS,           WID_INT},
+	{WID_HUT_FRAME_LEN,             WID_SHORT},
+	{WID_HUT_TX_FORMAT,             WID_CHAR},
+	{WID_HUT_BANDWIDTH,             WID_CHAR},
+	{WID_HUT_OP_BAND,               WID_CHAR},
+	{WID_HUT_STBC,                  WID_CHAR},
+	{WID_HUT_ESS,                   WID_CHAR},
+	{WID_HUT_ANTSET,                WID_CHAR},
+	{WID_HUT_HT_OP_MODE,            WID_CHAR},
+	{WID_HUT_RIFS_MODE,             WID_CHAR},
+	{WID_HUT_SMOOTHING_REC,         WID_CHAR},
+	{WID_HUT_SOUNDING_PKT,          WID_CHAR},
+	{WID_HUT_HT_CODING,             WID_CHAR},
+	{WID_HUT_TEST_DIR,              WID_CHAR},
+	{WID_HUT_TXOP_LIMIT,            WID_SHORT},
+	{WID_HUT_DEST_ADDR,             WID_ADR},
+	{WID_HUT_TX_PATTERN,            WID_BIN_DATA},
+	{WID_HUT_TX_TIME_TAKEN,         WID_INT},
+	{WID_HUT_PHY_TEST_MODE,         WID_CHAR},
+	{WID_HUT_PHY_TEST_RATE_HI,      WID_CHAR},
+	{WID_HUT_PHY_TEST_RATE_LO,      WID_CHAR},
+	{WID_HUT_TX_TEST_TIME,          WID_INT},
+	{WID_HUT_LOG_INTERVAL,          WID_INT},
+	{WID_HUT_DISABLE_RXQ_REPLENISH, WID_CHAR},
+	{WID_HUT_TEST_ID,               WID_STR},
+	{WID_HUT_KEY_ORIGIN,            WID_CHAR},
+	{WID_HUT_BCST_PERCENT,          WID_CHAR},
+	{WID_HUT_GROUP_CIPHER_TYPE,     WID_CHAR},
+	{WID_HUT_STATS,                 WID_BIN_DATA},
+	{WID_HUT_TSF_TEST_MODE,         WID_CHAR},
+	{WID_HUT_SIG_QUAL_AVG,          WID_SHORT},
+	{WID_HUT_SIG_QUAL_AVG_CNT,      WID_SHORT},
+	{WID_HUT_TSSI_VALUE,            WID_CHAR},
+	{WID_HUT_MGMT_PERCENT,          WID_CHAR},
+	{WID_HUT_MGMT_BCST_PERCENT,     WID_CHAR},
+	{WID_HUT_MGMT_ALLOW_HT,         WID_CHAR},
+	{WID_HUT_UC_MGMT_TYPE,          WID_CHAR},
+	{WID_HUT_BC_MGMT_TYPE,          WID_CHAR},
+	{WID_HUT_UC_MGMT_FRAME_LEN,     WID_SHORT},
+	{WID_HUT_BC_MGMT_FRAME_LEN,     WID_SHORT},
+	{WID_HUT_11W_MFP_REQUIRED_TX,   WID_CHAR},
+	{WID_HUT_11W_MFP_PEER_CAPABLE,  WID_CHAR},
+	{WID_HUT_11W_TX_IGTK_ID,        WID_CHAR},
+	{WID_HUT_FC_TXOP_MOD,           WID_CHAR},
+	{WID_HUT_FC_PROT_TYPE,          WID_CHAR},
+	{WID_HUT_SEC_CCA_ASSERT,        WID_CHAR},
+#endif /* MAC_802_11N */
+};
+
+u16 g_num_total_switches = (sizeof(gastrWIDs) / sizeof(tstrWID));
+/*****************************************************************************/
+/* Static Function Declarations                                              */
+/*****************************************************************************/
+
+
+
+/*****************************************************************************/
+/* Functions                                                                 */
+/*****************************************************************************/
+INLINE u8 ascii_hex_to_dec(u8 num)
+{
+	if ((num >= '0') && (num <= '9'))
+		return (num - '0');
+	else if ((num >= 'A') && (num <= 'F'))
+		return (10 + (num - 'A'));
+	else if ((num >= 'a') && (num <= 'f'))
+		return (10 + (num - 'a'));
+
+	return INVALID;
+}
+
+INLINE u8 get_hex_char(u8 inp)
+{
+	u8 *d2htab = "0123456789ABCDEF";
+
+	return d2htab[inp & 0xF];
+}
+
+/* This function extracts the MAC address held in a string in standard format */
+/* into another buffer as integers.                                           */
+INLINE u16 extract_mac_addr(char *str, u8 *buff)
+{
+	*buff = 0;
+	while (*str != '\0') {
+		if ((*str == ':') || (*str == '-'))
+			*(++buff) = 0;
+		else
+			*buff = (*buff << 4) + ascii_hex_to_dec(*str);
+
+		str++;
+	}
+
+	return MAC_ADDR_LEN;
+}
+
+/* This function creates MAC address in standard format from a buffer of      */
+/* integers.                                                                  */
+INLINE void create_mac_addr(u8 *str, u8 *buff)
+{
+	u32 i = 0;
+	u32 j = 0;
+
+	for (i = 0; i < MAC_ADDR_LEN; i++) {
+		str[j++] = get_hex_char((u8)((buff[i] >> 4) & 0x0F));
+		str[j++] = get_hex_char((u8)(buff[i] & 0x0F));
+		str[j++] = ':';
+	}
+	str[--j] = '\0';
+}
+
+/* This function converts the IP address string in dotted decimal format to */
+/* unsigned integer. This functionality is similar to the library function  */
+/* inet_addr() but is reimplemented here since I could not confirm that     */
+/* inet_addr is platform independent.                                       */
+/* ips=>IP Address String in dotted decimal format                          */
+/* ipn=>Pointer to IP Address in integer format                             */
+INLINE u8 conv_ip_to_int(u8 *ips, u32 *ipn)
+{
+	u8 i   = 0;
+	u8 ipb = 0;
+	*ipn = 0;
+	/* Integer to string for each component */
+	while (ips[i] != '\0') {
+		if (ips[i] == '.') {
+			*ipn = ((*ipn) << 8) | ipb;
+			ipb = 0;
+		} else {
+			ipb = ipb * 10 + ascii_hex_to_dec(ips[i]);
+		}
+
+		i++;
+	}
+
+	/* The last byte of the IP address is read in here */
+	*ipn = ((*ipn) << 8) | ipb;
+
+	return 0;
+}
+
+/* This function converts the IP address from integer format to dotted    */
+/* decimal string format. Alternative to std library fn inet_ntoa().      */
+/* ips=>Buffer to hold IP Address String dotted decimal format (Min 17B)  */
+/* ipn=>IP Address in integer format                                      */
+INLINE u8 conv_int_to_ip(u8 *ips, u32 ipn)
+{
+	u8 i   = 0;
+	u8 ipb = 0;
+	u8 cnt = 0;
+	u8 ipbsize = 0;
+
+	for (cnt = 4; cnt > 0; cnt--) {
+		ipb = (ipn >> (8 * (cnt - 1))) & 0xFF;
+
+		if (ipb >= 100)
+			ipbsize = 2;
+		else if (ipb >= 10)
+			ipbsize = 1;
+		else
+			ipbsize = 0;
+
+		switch (ipbsize) {
+		case 2:
+			ips[i++] = get_hex_char(ipb / 100);
+			ipb %= 100;
+
+		case 1:
+			ips[i++] = get_hex_char(ipb / 10);
+			ipb %= 10;
+
+		default:
+			ips[i++] = get_hex_char(ipb);
+		}
+
+		if (cnt > 1)
+			ips[i++] = '.';
+	}
+
+	ips[i] = '\0';
+
+	return i;
+}
+
+INLINE tenuWIDtype get_wid_type(u32 wid_num)
+{
+	/* Check for iconfig specific WID types first */
+	if ((wid_num == WID_BSSID) ||
+	    (wid_num == WID_MAC_ADDR) ||
+	    (wid_num == WID_IP_ADDRESS) ||
+	    (wid_num == WID_HUT_DEST_ADDR)) {
+		return WID_ADR;
+	}
+
+	if ((WID_1X_SERV_ADDR == wid_num) ||
+	    (WID_STACK_IP_ADDR == wid_num) ||
+	    (WID_STACK_NETMASK_ADDR == wid_num)) {
+		return WID_IP;
+	}
+
+	/* Next check for standard WID types */
+	if (wid_num < 0x1000)
+		return WID_CHAR;
+	else if (wid_num < 0x2000)
+		return WID_SHORT;
+	else if (wid_num < 0x3000)
+		return WID_INT;
+	else if (wid_num < 0x4000)
+		return WID_STR;
+	else if (wid_num < 0x5000)
+		return WID_BIN_DATA;
+
+	return WID_UNDEF;
+}
+
+
+/* This function extracts the beacon period field from the beacon or probe   */
+/* response frame.                                                           */
+INLINE u16 get_beacon_period(u8 *data)
+{
+	u16 bcn_per = 0;
+
+	bcn_per  = data[0];
+	bcn_per |= (data[1] << 8);
+
+	return bcn_per;
+}
+
+INLINE u32 get_beacon_timestamp_lo(u8 *data)
+{
+	u32 time_stamp = 0;
+	u32 index    = MAC_HDR_LEN;
+
+	time_stamp |= data[index++];
+	time_stamp |= (data[index++] << 8);
+	time_stamp |= (data[index++] << 16);
+	time_stamp |= (data[index]   << 24);
+
+	return time_stamp;
+}
+
+INLINE u32 get_beacon_timestamp_hi(u8 *data)
+{
+	u32 time_stamp = 0;
+	u32 index    = (MAC_HDR_LEN + 4);
+
+	time_stamp |= data[index++];
+	time_stamp |= (data[index++] << 8);
+	time_stamp |= (data[index++] << 16);
+	time_stamp |= (data[index]   << 24);
+
+	return time_stamp;
+}
+
+/* This function extracts the 'frame type' bits from the MAC header of the   */
+/* input frame.                                                              */
+/* Returns the value in the LSB of the returned value.                       */
+INLINE tenuBasicFrmType get_type(u8 *header)
+{
+	return ((tenuBasicFrmType)(header[0] & 0x0C));
+}
+
+/* This function extracts the 'frame type and sub type' bits from the MAC    */
+/* header of the input frame.                                                */
+/* Returns the value in the LSB of the returned value.                       */
+INLINE tenuFrmSubtype get_sub_type(u8 *header)
+{
+	return ((tenuFrmSubtype)(header[0] & 0xFC));
+}
+
+/* This function extracts the 'to ds' bit from the MAC header of the input   */
+/* frame.                                                                    */
+/* Returns the value in the LSB of the returned value.                       */
+INLINE u8 get_to_ds(u8 *header)
+{
+	return (header[1] & 0x01);
+}
+
+/* This function extracts the 'from ds' bit from the MAC header of the input */
+/* frame.                                                                    */
+/* Returns the value in the LSB of the returned value.                       */
+INLINE u8 get_from_ds(u8 *header)
+{
+	return ((header[1] & 0x02) >> 1);
+}
+
+/* This function extracts the MAC Address in 'address1' field of the MAC     */
+/* header and updates the MAC Address in the allocated 'addr' variable.      */
+INLINE void get_address1(u8 *pu8msa, u8 *addr)
+{
+	WILC_memcpy(addr, pu8msa + 4, 6);
+}
+
+/* This function extracts the MAC Address in 'address2' field of the MAC     */
+/* header and updates the MAC Address in the allocated 'addr' variable.      */
+INLINE void get_address2(u8 *pu8msa, u8 *addr)
+{
+	WILC_memcpy(addr, pu8msa + 10, 6);
+}
+
+/* This function extracts the MAC Address in 'address3' field of the MAC     */
+/* header and updates the MAC Address in the allocated 'addr' variable.      */
+INLINE void get_address3(u8 *pu8msa, u8 *addr)
+{
+	WILC_memcpy(addr, pu8msa + 16, 6);
+}
+
+/* This function extracts the BSSID from the incoming WLAN packet based on   */
+/* the 'from ds' bit, and updates the MAC Address in the allocated 'addr'    */
+/* variable.                                                                 */
+INLINE void get_BSSID(u8 *data, u8 *bssid)
+{
+	if (get_from_ds(data) == 1)
+		get_address2(data, bssid);
+	else if (get_to_ds(data) == 1)
+		get_address1(data, bssid);
+	else
+		get_address3(data, bssid);
+}
+
+/* This function extracts the SSID from a beacon/probe response frame        */
+INLINE void get_ssid(u8 *data, u8 *ssid, u8 *p_ssid_len)
+{
+	u8 len = 0;
+	u8 i   = 0;
+	u8 j   = 0;
+
+	len = data[MAC_HDR_LEN + TIME_STAMP_LEN + BEACON_INTERVAL_LEN +
+		   CAP_INFO_LEN + 1];
+	j   = MAC_HDR_LEN + TIME_STAMP_LEN + BEACON_INTERVAL_LEN +
+		CAP_INFO_LEN + 2;
+
+	/* If the SSID length field is set wrongly to a value greater than the   */
+	/* allowed maximum SSID length limit, reset the length to 0              */
+	if (len >= MAX_SSID_LEN)
+		len = 0;
+
+	for (i = 0; i < len; i++, j++)
+		ssid[i] = data[j];
+
+	ssid[len] = '\0';
+
+	*p_ssid_len = len;
+}
+
+/* This function extracts the capability info field from the beacon or probe */
+/* response frame.                                                           */
+INLINE u16 get_cap_info(u8 *data)
+{
+	u16 cap_info = 0;
+	u16 index    = MAC_HDR_LEN;
+	tenuFrmSubtype st = BEACON;
+
+	st = get_sub_type(data);
+
+	/* Location of the Capability field is different for Beacon and */
+	/* Association frames.                                          */
+	if ((st == BEACON) || (st == PROBE_RSP))
+		index += TIME_STAMP_LEN + BEACON_INTERVAL_LEN;
+
+	cap_info  = data[index];
+	cap_info |= (data[index + 1] << 8);
+
+	return cap_info;
+}
+
+/* This function extracts the capability info field from the Association */
+/* response frame.                                                                       */
+INLINE u16 get_assoc_resp_cap_info(u8 *data)
+{
+	u16 cap_info = 0;
+
+	cap_info  = data[0];
+	cap_info |= (data[1] << 8);
+
+	return cap_info;
+}
+
+/* This funcion extracts the association status code from the incoming       */
+/* association response frame and returns association status code            */
+INLINE u16 get_asoc_status(u8 *data)
+{
+	u16 asoc_status = 0;
+
+	asoc_status = data[3];
+	asoc_status = (asoc_status << 8) | data[2];
+
+	return asoc_status;
+}
+
+/* This function extracts association ID from the incoming association       */
+/* response frame							                                     */
+INLINE u16 get_asoc_id(u8 *data)
+{
+	u16 asoc_id = 0;
+
+	asoc_id  = data[4];
+	asoc_id |= (data[5] << 8);
+
+	return asoc_id;
+}
+
+/**
+ *  @brief              initializes the Core Configurator
+ *  @details
+ *  @return     Error code indicating success/failure
+ *  @note
+ *  @author	mabubakr
+ *  @date		1 Mar 2012
+ *  @version		1.0
+ */
+
+s32 CoreConfiguratorInit(void)
+{
+	s32 s32Error = WILC_SUCCESS;
+	PRINT_D(CORECONFIG_DBG, "CoreConfiguratorInit()\n");
+
+	sema_init(&SemHandleSendPkt, 1);
+	sema_init(&SemHandlePktResp, 0);
+
+	gps8ConfigPacket = (s8 *)WILC_MALLOC(MAX_PACKET_BUFF_SIZE);
+	if (gps8ConfigPacket == NULL) {
+		PRINT_ER("failed in gps8ConfigPacket allocation\n");
+		s32Error = WILC_NO_MEM;
+		goto _fail_;
+	}
+
+	WILC_memset((void *)gps8ConfigPacket, 0, MAX_PACKET_BUFF_SIZE);
+
+	WILC_memset((void *)(&gstrConfigPktInfo), 0, sizeof(tstrConfigPktInfo));
+_fail_:
+	return s32Error;
+}
+
+u8 *get_tim_elm(u8 *pu8msa, u16 u16RxLen, u16 u16TagParamOffset)
+{
+	u16 u16index = 0;
+
+	/*************************************************************************/
+	/*                       Beacon Frame - Frame Body                       */
+	/* --------------------------------------------------------------------- */
+	/* |Timestamp |BeaconInt |CapInfo |SSID |SupRates |DSParSet |TIM elm   | */
+	/* --------------------------------------------------------------------- */
+	/* |8         |2         |2       |2-34 |3-10     |3        |4-256     | */
+	/* --------------------------------------------------------------------- */
+	/*                                                                       */
+	/*************************************************************************/
+
+	u16index = u16TagParamOffset;
+
+	/* Search for the TIM Element Field and return if the element is found */
+	while (u16index < (u16RxLen - FCS_LEN)) {
+		if (pu8msa[u16index] == ITIM) {
+			return &pu8msa[u16index];
+		} else {
+			u16index += (IE_HDR_LEN + pu8msa[u16index + 1]);
+		}
+	}
+
+	return 0;
+}
+
+/* This function gets the current channel information from
+ * the 802.11n beacon/probe response frame */
+u8 get_current_channel_802_11n(u8 *pu8msa, u16 u16RxLen)
+{
+	u16 index;
+
+	index = TAG_PARAM_OFFSET;
+	while (index < (u16RxLen - FCS_LEN)) {
+		if (pu8msa[index] == IDSPARMS)
+			return pu8msa[index + 2];
+		else
+			/* Increment index by length information and header */
+			index += pu8msa[index + 1] + IE_HDR_LEN;
+	}
+
+	/* Return current channel information from the MIB, if beacon/probe  */
+	/* response frame does not contain the DS parameter set IE           */
+	/* return (mget_CurrentChannel() + 1); */
+	return 0;  /* no MIB here */
+}
+
+u8 get_current_channel(u8 *pu8msa, u16 u16RxLen)
+{
+#ifdef PHY_802_11n
+#ifdef FIVE_GHZ_BAND
+	/* Get the current channel as its not set in */
+	/* 802.11a beacons/probe response            */
+	return (get_rf_channel() + 1);
+#else /* FIVE_GHZ_BAND */
+	/* Extract current channel information from */
+	/* the beacon/probe response frame          */
+	return get_current_channel_802_11n(pu8msa, u16RxLen);
+#endif /* FIVE_GHZ_BAND */
+#else
+	return 0;
+#endif /* PHY_802_11n */
+}
+
+/**
+ *  @brief                      parses the received 'N' message
+ *  @details
+ *  @param[in]  pu8MsgBuffer The message to be parsed
+ *  @param[out]         ppstrNetworkInfo pointer to pointer to the structure containing the parsed Network Info
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mabubakr
+ *  @date			1 Mar 2012
+ *  @version		1.0
+ */
+s32 ParseNetworkInfo(u8 *pu8MsgBuffer, tstrNetworkInfo **ppstrNetworkInfo)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrNetworkInfo *pstrNetworkInfo = NULL;
+	u8 u8MsgType = 0;
+	u8 u8MsgID = 0;
+	u16 u16MsgLen = 0;
+
+	u16 u16WidID = (u16)WID_NIL;
+	u16 u16WidLen  = 0;
+	u8  *pu8WidVal = 0;
+
+	u8MsgType = pu8MsgBuffer[0];
+
+	/* Check whether the received message type is 'N' */
+	if ('N' != u8MsgType) {
+		PRINT_ER("Received Message format incorrect.\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	/* Extract message ID */
+	u8MsgID = pu8MsgBuffer[1];
+
+	/* Extract message Length */
+	u16MsgLen = MAKE_WORD16(pu8MsgBuffer[2], pu8MsgBuffer[3]);
+
+	/* Extract WID ID */
+	u16WidID = MAKE_WORD16(pu8MsgBuffer[4], pu8MsgBuffer[5]);
+
+	/* Extract WID Length */
+	u16WidLen = MAKE_WORD16(pu8MsgBuffer[6], pu8MsgBuffer[7]);
+
+	/* Assign a pointer to the WID value */
+	pu8WidVal  = &pu8MsgBuffer[8];
+
+	/* parse the WID value of the WID "WID_NEWORK_INFO" */
+	{
+		u8  *pu8msa = 0;
+		u16 u16RxLen = 0;
+		u8 *pu8TimElm = 0;
+		u8 *pu8IEs = 0;
+		u16 u16IEsLen = 0;
+		u8 u8index = 0;
+		u32 u32Tsf_Lo;
+		u32 u32Tsf_Hi;
+
+		pstrNetworkInfo = (tstrNetworkInfo *)WILC_MALLOC(sizeof(tstrNetworkInfo));
+		WILC_memset((void *)(pstrNetworkInfo), 0, sizeof(tstrNetworkInfo));
+
+		pstrNetworkInfo->s8rssi = pu8WidVal[0];
+
+		/* Assign a pointer to msa "Mac Header Start Address" */
+		pu8msa = &pu8WidVal[1];
+
+		u16RxLen = u16WidLen - 1;
+
+		/* parse msa*/
+
+		/* Get the cap_info */
+		pstrNetworkInfo->u16CapInfo = get_cap_info(pu8msa);
+		#ifdef WILC_P2P
+		/* Get time-stamp [Low only 32 bit] */
+		pstrNetworkInfo->u32Tsf = get_beacon_timestamp_lo(pu8msa);
+		PRINT_D(CORECONFIG_DBG, "TSF :%x\n", pstrNetworkInfo->u32Tsf);
+		#endif
+
+		/* Get full time-stamp [Low and High 64 bit] */
+		u32Tsf_Lo = get_beacon_timestamp_lo(pu8msa);
+		u32Tsf_Hi = get_beacon_timestamp_hi(pu8msa);
+
+		pstrNetworkInfo->u64Tsf = u32Tsf_Lo | ((u64)u32Tsf_Hi << 32);
+
+		/* Get SSID */
+		get_ssid(pu8msa, pstrNetworkInfo->au8ssid, &(pstrNetworkInfo->u8SsidLen));
+
+		/* Get BSSID */
+		get_BSSID(pu8msa, pstrNetworkInfo->au8bssid);
+
+		/* Get the current channel */
+		pstrNetworkInfo->u8channel = get_current_channel(pu8msa, (u16RxLen + FCS_LEN));
+
+		/* Get beacon period */
+		u8index = (MAC_HDR_LEN + TIME_STAMP_LEN);
+
+		pstrNetworkInfo->u16BeaconPeriod = get_beacon_period(pu8msa + u8index);
+
+		u8index += BEACON_INTERVAL_LEN + CAP_INFO_LEN;
+
+		/* Get DTIM Period */
+		pu8TimElm = get_tim_elm(pu8msa, (u16RxLen + FCS_LEN), u8index);
+		if (pu8TimElm != 0) {
+			pstrNetworkInfo->u8DtimPeriod = pu8TimElm[3];
+		}
+		pu8IEs = &pu8msa[MAC_HDR_LEN + TIME_STAMP_LEN + BEACON_INTERVAL_LEN + CAP_INFO_LEN];
+		u16IEsLen = u16RxLen - (MAC_HDR_LEN + TIME_STAMP_LEN + BEACON_INTERVAL_LEN + CAP_INFO_LEN);
+
+		if (u16IEsLen > 0) {
+			pstrNetworkInfo->pu8IEs = (u8 *)WILC_MALLOC(u16IEsLen);
+			WILC_memset((void *)(pstrNetworkInfo->pu8IEs), 0, u16IEsLen);
+
+			WILC_memcpy(pstrNetworkInfo->pu8IEs, pu8IEs, u16IEsLen);
+		}
+		pstrNetworkInfo->u16IEsLen = u16IEsLen;
+
+	}
+
+	*ppstrNetworkInfo = pstrNetworkInfo;
+
+ERRORHANDLER:
+	return s32Error;
+}
+
+/**
+ *  @brief              Deallocates the parsed Network Info
+ *  @details
+ *  @param[in]  pstrNetworkInfo Network Info to be deallocated
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mabubakr
+ *  @date		1 Mar 2012
+ *  @version		1.0
+ */
+s32 DeallocateNetworkInfo(tstrNetworkInfo *pstrNetworkInfo)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	if (pstrNetworkInfo != NULL) {
+		if (pstrNetworkInfo->pu8IEs != NULL) {
+			WILC_FREE(pstrNetworkInfo->pu8IEs);
+			pstrNetworkInfo->pu8IEs = NULL;
+		} else {
+			s32Error = WILC_FAIL;
+		}
+
+		WILC_FREE(pstrNetworkInfo);
+		pstrNetworkInfo = NULL;
+
+	} else {
+		s32Error = WILC_FAIL;
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief                      parses the received Association Response frame
+ *  @details
+ *  @param[in]  pu8Buffer The Association Response frame to be parsed
+ *  @param[out]         ppstrConnectRespInfo pointer to pointer to the structure containing the parsed Association Response Info
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mabubakr
+ *  @date			2 Apr 2012
+ *  @version		1.0
+ */
+s32 ParseAssocRespInfo(u8 *pu8Buffer, u32 u32BufferLen,
+			       tstrConnectRespInfo **ppstrConnectRespInfo)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrConnectRespInfo *pstrConnectRespInfo = NULL;
+	u16 u16AssocRespLen = 0;
+	u8 *pu8IEs = 0;
+	u16 u16IEsLen = 0;
+
+	pstrConnectRespInfo = (tstrConnectRespInfo *)WILC_MALLOC(sizeof(tstrConnectRespInfo));
+	WILC_memset((void *)(pstrConnectRespInfo), 0, sizeof(tstrConnectRespInfo));
+
+	/* u16AssocRespLen = pu8Buffer[0]; */
+	u16AssocRespLen = (u16)u32BufferLen;
+
+	/* get the status code */
+	pstrConnectRespInfo->u16ConnectStatus = get_asoc_status(pu8Buffer);
+	if (pstrConnectRespInfo->u16ConnectStatus == SUCCESSFUL_STATUSCODE) {
+
+		/* get the capability */
+		pstrConnectRespInfo->u16capability = get_assoc_resp_cap_info(pu8Buffer);
+
+		/* get the Association ID */
+		pstrConnectRespInfo->u16AssocID = get_asoc_id(pu8Buffer);
+
+		/* get the Information Elements */
+		pu8IEs = &pu8Buffer[CAP_INFO_LEN + STATUS_CODE_LEN + AID_LEN];
+		u16IEsLen = u16AssocRespLen - (CAP_INFO_LEN + STATUS_CODE_LEN + AID_LEN);
+
+		pstrConnectRespInfo->pu8RespIEs = (u8 *)WILC_MALLOC(u16IEsLen);
+		WILC_memset((void *)(pstrConnectRespInfo->pu8RespIEs), 0, u16IEsLen);
+
+		WILC_memcpy(pstrConnectRespInfo->pu8RespIEs, pu8IEs, u16IEsLen);
+		pstrConnectRespInfo->u16RespIEsLen = u16IEsLen;
+	}
+
+	*ppstrConnectRespInfo = pstrConnectRespInfo;
+
+
+	return s32Error;
+}
+
+/**
+ *  @brief                      Deallocates the parsed Association Response Info
+ *  @details
+ *  @param[in]  pstrNetworkInfo Network Info to be deallocated
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mabubakr
+ *  @date			2 Apr 2012
+ *  @version		1.0
+ */
+s32 DeallocateAssocRespInfo(tstrConnectRespInfo *pstrConnectRespInfo)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	if (pstrConnectRespInfo != NULL) {
+		if (pstrConnectRespInfo->pu8RespIEs != NULL) {
+			WILC_FREE(pstrConnectRespInfo->pu8RespIEs);
+			pstrConnectRespInfo->pu8RespIEs = NULL;
+		} else {
+			s32Error = WILC_FAIL;
+		}
+
+		WILC_FREE(pstrConnectRespInfo);
+		pstrConnectRespInfo = NULL;
+
+	} else {
+		s32Error = WILC_FAIL;
+	}
+
+	return s32Error;
+}
+
+#ifndef CONNECT_DIRECT
+s32 ParseSurveyResults(u8 ppu8RcvdSiteSurveyResults[][MAX_SURVEY_RESULT_FRAG_SIZE],
+			       wid_site_survey_reslts_s **ppstrSurveyResults,
+			       u32 *pu32SurveyResultsCount)
+{
+	s32 s32Error = WILC_SUCCESS;
+	wid_site_survey_reslts_s *pstrSurveyResults = NULL;
+	u32 u32SurveyResultsCount = 0;
+	u32 u32SurveyBytesLength = 0;
+	u8 *pu8BufferPtr;
+	u32 u32RcvdSurveyResultsNum = 2;
+	u8 u8ReadSurveyResFragNum;
+	u32 i;
+	u32 j;
+
+	for (i = 0; i < u32RcvdSurveyResultsNum; i++) {
+		u32SurveyBytesLength = ppu8RcvdSiteSurveyResults[i][0];
+
+
+		for (j = 0; j < u32SurveyBytesLength; j += SURVEY_RESULT_LENGTH) {
+			u32SurveyResultsCount++;
+		}
+	}
+
+	pstrSurveyResults = (wid_site_survey_reslts_s *)WILC_MALLOC(u32SurveyResultsCount * sizeof(wid_site_survey_reslts_s));
+	if (pstrSurveyResults == NULL) {
+		u32SurveyResultsCount = 0;
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	WILC_memset((void *)(pstrSurveyResults), 0, u32SurveyResultsCount * sizeof(wid_site_survey_reslts_s));
+
+	u32SurveyResultsCount = 0;
+
+	for (i = 0; i < u32RcvdSurveyResultsNum; i++) {
+		pu8BufferPtr = ppu8RcvdSiteSurveyResults[i];
+
+		u32SurveyBytesLength = pu8BufferPtr[0];
+
+		/* TODO: mostafa: pu8BufferPtr[1] contains the fragment num */
+		u8ReadSurveyResFragNum = pu8BufferPtr[1];
+
+		pu8BufferPtr += 2;
+
+		for (j = 0; j < u32SurveyBytesLength; j += SURVEY_RESULT_LENGTH) {
+			WILC_memcpy(&pstrSurveyResults[u32SurveyResultsCount], pu8BufferPtr, SURVEY_RESULT_LENGTH);
+			pu8BufferPtr += SURVEY_RESULT_LENGTH;
+			u32SurveyResultsCount++;
+		}
+	}
+
+ERRORHANDLER:
+	*ppstrSurveyResults = pstrSurveyResults;
+	*pu32SurveyResultsCount = u32SurveyResultsCount;
+
+	return s32Error;
+}
+
+
+s32 DeallocateSurveyResults(wid_site_survey_reslts_s *pstrSurveyResults)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	if (pstrSurveyResults != NULL) {
+		WILC_FREE(pstrSurveyResults);
+	}
+
+	return s32Error;
+}
+#endif
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : ProcessCharWid                                         */
+/*                                                                           */
+/*  Description   : This function processes a WID of type WID_CHAR and       */
+/*                  updates the cfg packet with the supplied value.          */
+/*                                                                           */
+/*  Inputs        : 1) Pointer to WID cfg structure                          */
+/*                  2) Value to set                                          */
+/*                                                                           */
+/*  Globals       :                                                          */
+/*                                                                           */
+/*  Processing    :                                                          */
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : None                                                     */
+/*                                                                           */
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2008   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+void ProcessCharWid(char *pcPacket, s32 *ps32PktLen,
+		    tstrWID *pstrWID, s8 *ps8WidVal)
+{
+	u8 *pu8val = (u8 *)ps8WidVal;
+	u8 u8val = 0;
+	s32 s32PktLen = *ps32PktLen;
+	if (pstrWID == NULL) {
+		PRINT_WRN(CORECONFIG_DBG, "Can't set CHAR val 0x%x ,NULL structure\n", u8val);
+		return;
+	}
+
+	/* WID */
+	pcPacket[s32PktLen++] = (u8)(pstrWID->u16WIDid & 0xFF);
+	pcPacket[s32PktLen++] = (u8)(pstrWID->u16WIDid >> 8) & 0xFF;
+	if (g_oper_mode == SET_CFG) {
+		u8val = *pu8val;
+
+		/* Length */
+		pcPacket[s32PktLen++] = sizeof(u8);
+
+
+		/* Value */
+		pcPacket[s32PktLen++] = u8val;
+	}
+	*ps32PktLen = s32PktLen;
+}
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : ProcessShortWid                                        */
+/*                                                                           */
+/*  Description   : This function processes a WID of type WID_SHORT and      */
+/*                  updates the cfg packet with the supplied value.          */
+/*                                                                           */
+/*  Inputs        : 1) Pointer to WID cfg structure                          */
+/*                  2) Value to set                                          */
+/*                                                                           */
+/*  Globals       :                                                          */
+/*                                                                           */
+/*  Processing    :                                                          */
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : None                                                     */
+/*                                                                           */
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2008   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+void ProcessShortWid(char *pcPacket, s32 *ps32PktLen,
+		     tstrWID *pstrWID, s8 *ps8WidVal)
+{
+	u16 *pu16val = (u16 *)ps8WidVal;
+	u16 u16val = 0;
+	s32 s32PktLen = *ps32PktLen;
+	if (pstrWID == NULL) {
+		PRINT_WRN(CORECONFIG_DBG, "Can't set SHORT val 0x%x ,NULL structure\n", u16val);
+		return;
+	}
+
+	/* WID */
+	pcPacket[s32PktLen++] = (u8)(pstrWID->u16WIDid & 0xFF);
+	pcPacket[s32PktLen++] = (u8)((pstrWID->u16WIDid >> 8) & 0xFF);
+
+	if (g_oper_mode == SET_CFG) {
+		u16val = *pu16val;
+
+		/* Length */
+		pcPacket[s32PktLen++] = sizeof(u16);
+
+		/* Value */
+		pcPacket[s32PktLen++] = (u8)(u16val & 0xFF);
+		pcPacket[s32PktLen++] = (u8)((u16val >> 8) & 0xFF);
+	}
+	*ps32PktLen = s32PktLen;
+}
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : ProcessIntWid                                          */
+/*                                                                           */
+/*  Description   : This function processes a WID of type WID_INT and        */
+/*                  updates the cfg packet with the supplied value.          */
+/*                                                                           */
+/*  Inputs        : 1) Pointer to WID cfg structure                          */
+/*                  2) Value to set                                          */
+/*                                                                           */
+/*  Globals       :                                                          */
+/*                                                                           */
+/*  Processing    :                                                          */
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : None                                                     */
+/*                                                                           */
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2008   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+void ProcessIntWid(char *pcPacket, s32 *ps32PktLen,
+		   tstrWID *pstrWID, s8 *ps8WidVal)
+{
+	u32 *pu32val = (u32 *)ps8WidVal;
+	u32 u32val = 0;
+	s32 s32PktLen = *ps32PktLen;
+	if (pstrWID == NULL) {
+		PRINT_WRN(CORECONFIG_DBG, "Can't set INT val 0x%x , NULL structure\n", u32val);
+		return;
+	}
+
+	/* WID */
+	pcPacket[s32PktLen++] = (u8)(pstrWID->u16WIDid & 0xFF);
+	pcPacket[s32PktLen++] = (u8)((pstrWID->u16WIDid >> 8) & 0xFF);
+
+	if (g_oper_mode == SET_CFG) {
+		u32val = *pu32val;
+
+		/* Length */
+		pcPacket[s32PktLen++] = sizeof(u32);
+
+		/* Value */
+		pcPacket[s32PktLen++] = (u8)(u32val & 0xFF);
+		pcPacket[s32PktLen++] = (u8)((u32val >> 8) & 0xFF);
+		pcPacket[s32PktLen++] = (u8)((u32val >> 16) & 0xFF);
+		pcPacket[s32PktLen++] = (u8)((u32val >> 24) & 0xFF);
+	}
+	*ps32PktLen = s32PktLen;
+}
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : ProcessIPwid                                           */
+/*                                                                           */
+/*  Description   : This function processes a WID of type WID_IP and         */
+/*                  updates the cfg packet with the supplied value.          */
+/*                                                                           */
+/*  Inputs        : 1) Pointer to WID cfg structure                          */
+/*                  2) Value to set                                          */
+/*                                                                           */
+/*  Globals       :                                                          */
+/*                                                                           */
+/*                                                                           */
+/*  Processing    :                                                          */
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : None                                                     */
+/*                                                                           */
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2008   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+void ProcessIPwid(char *pcPacket, s32 *ps32PktLen,
+		  tstrWID *pstrWID, u8 *pu8ip)
+{
+	u32 u32val = 0;
+	s32 s32PktLen = *ps32PktLen;
+
+	if (pstrWID == NULL) {
+		PRINT_WRN(CORECONFIG_DBG, "Can't set IP Addr , NULL structure\n");
+		return;
+	}
+
+	/* WID */
+	pcPacket[s32PktLen++] = (u8)(pstrWID->u16WIDid & 0xFF);
+	pcPacket[s32PktLen++] = (u8)((pstrWID->u16WIDid >> 8) & 0xFF);
+
+	if (g_oper_mode == SET_CFG) {
+		/* Length */
+		pcPacket[s32PktLen++] = sizeof(u32);
+
+		/* Convert the IP Address String to Integer */
+		conv_ip_to_int(pu8ip, &u32val);
+
+		/* Value */
+		pcPacket[s32PktLen++] = (u8)(u32val & 0xFF);
+		pcPacket[s32PktLen++] = (u8)((u32val >> 8) & 0xFF);
+		pcPacket[s32PktLen++] = (u8)((u32val >> 16) & 0xFF);
+		pcPacket[s32PktLen++] = (u8)((u32val >> 24) & 0xFF);
+	}
+	*ps32PktLen = s32PktLen;
+}
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : ProcessStrWid                                          */
+/*                                                                           */
+/*  Description   : This function processes a WID of type WID_STR and        */
+/*                  updates the cfg packet with the supplied value.          */
+/*                                                                           */
+/*  Inputs        : 1) Pointer to WID cfg structure                          */
+/*                  2) Value to set                                          */
+/*                                                                           */
+/*  Globals       :                                                          */
+/*                                                                           */
+/*  Processing    :                                                          */
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : None                                                     */
+/*                                                                           */
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2008   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+void ProcessStrWid(char *pcPacket, s32 *ps32PktLen,
+		   tstrWID *pstrWID, u8 *pu8val, s32 s32ValueSize)
+{
+	u16 u16MsgLen = 0;
+	u16 idx    = 0;
+	s32 s32PktLen = *ps32PktLen;
+	if (pstrWID == NULL) {
+		PRINT_WRN(CORECONFIG_DBG, "Can't set STR val, NULL structure\n");
+		return;
+	}
+
+	/* WID */
+	pcPacket[s32PktLen++] = (u8)(pstrWID->u16WIDid & 0xFF);
+	pcPacket[s32PktLen++] = (u8)((pstrWID->u16WIDid >> 8) & 0xFF);
+
+	if (g_oper_mode == SET_CFG) {
+		/* Message Length */
+		/* u16MsgLen = WILC_strlen(pu8val); */
+		u16MsgLen = (u16)s32ValueSize;
+
+		/* Length */
+		pcPacket[s32PktLen++] = (u8)u16MsgLen;
+
+		/* Value */
+		for (idx = 0; idx < u16MsgLen; idx++)
+			pcPacket[s32PktLen++] = pu8val[idx];
+	}
+	*ps32PktLen = s32PktLen;
+}
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : ProcessAdrWid                                          */
+/*                                                                           */
+/*  Description   : This function processes a WID of type WID_ADR and        */
+/*                  updates the cfg packet with the supplied value.          */
+/*                                                                           */
+/*  Inputs        : 1) Pointer to WID cfg structure                          */
+/*                  2) Value to set                                          */
+/*                                                                           */
+/*  Globals       :                                                          */
+/*                                                                           */
+/*  Processing    :                                                          */
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : None                                                     */
+/*                                                                           */
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2008   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+void ProcessAdrWid(char *pcPacket, s32 *ps32PktLen,
+		   tstrWID *pstrWID, u8 *pu8val)
+{
+	u16 u16MsgLen = 0;
+	s32 s32PktLen = *ps32PktLen;
+
+	if (pstrWID == NULL) {
+		PRINT_WRN(CORECONFIG_DBG, "Can't set Addr WID, NULL structure\n");
+		return;
+	}
+
+	/* WID */
+	pcPacket[s32PktLen++] = (u8)(pstrWID->u16WIDid & 0xFF);
+	pcPacket[s32PktLen++] = (u8)((pstrWID->u16WIDid >> 8) & 0xFF);
+
+	if (g_oper_mode == SET_CFG) {
+		/* Message Length */
+		u16MsgLen = MAC_ADDR_LEN;
+
+		/* Length */
+		pcPacket[s32PktLen++] = (u8)u16MsgLen;
+
+		/* Value */
+		extract_mac_addr(pu8val, pcPacket + s32PktLen);
+		s32PktLen += u16MsgLen;
+	}
+	*ps32PktLen = s32PktLen;
+}
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : ProcessBinWid                                          */
+/*                                                                           */
+/*  Description   : This function processes a WID of type WID_BIN_DATA and        */
+/*                  updates the cfg packet with the supplied value.          */
+/*                                                                           */
+/*  Inputs        : 1) Pointer to WID cfg structure                          */
+/*                  2) Name of file containing the binary data in text mode  */
+/*                                                                           */
+/*  Globals       :                                                          */
+/*                                                                           */
+/*  Processing    : The binary data is expected to be supplied through a     */
+/*                  file in text mode. This file is expected to be in the    */
+/*                  finject format. It is parsed, converted to binary format */
+/*                  and copied into g_cfg_pkt for further processing. This   */
+/*                  is obviously a round-about way of processing involving   */
+/*                  multiple (re)conversions between bin & ascii formats.    */
+/*                  But it is done nevertheless to retain uniformity and for */
+/*                  ease of debugging.                                       */
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : None                                                     */
+/*                                                                           */
+
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2008   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+void ProcessBinWid(char *pcPacket, s32 *ps32PktLen,
+		   tstrWID *pstrWID, u8 *pu8val, s32 s32ValueSize)
+{
+	/* WILC_ERROR("processing Binary WIDs is not supported\n"); */
+
+	u16 u16MsgLen = 0;
+	u16 idx    = 0;
+	s32 s32PktLen = *ps32PktLen;
+	u8 u8checksum = 0;
+
+	if (pstrWID == NULL) {
+		PRINT_WRN(CORECONFIG_DBG, "Can't set BIN val, NULL structure\n");
+		return;
+	}
+
+	/* WID */
+	pcPacket[s32PktLen++] = (u8)(pstrWID->u16WIDid & 0xFF);
+	pcPacket[s32PktLen++] = (u8)((pstrWID->u16WIDid >> 8) & 0xFF);
+
+	if (g_oper_mode == SET_CFG) {
+		/* Message Length */
+		u16MsgLen = (u16)s32ValueSize;
+
+		/* Length */
+		/* pcPacket[s32PktLen++] = (u8)u16MsgLen; */
+		pcPacket[s32PktLen++] = (u8)(u16MsgLen  & 0xFF);
+		pcPacket[s32PktLen++] = (u8)((u16MsgLen >> 8) & 0xFF);
+
+		/* Value */
+		for (idx = 0; idx < u16MsgLen; idx++)
+			pcPacket[s32PktLen++] = pu8val[idx];
+
+		/* checksum */
+		for (idx = 0; idx < u16MsgLen; idx++)
+			u8checksum += pcPacket[MSG_HEADER_LEN + idx + 4];
+
+		pcPacket[s32PktLen++] = u8checksum;
+	}
+	*ps32PktLen = s32PktLen;
+}
+
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : further_process_response                                 */
+/*                                                                           */
+/*  Description   : This function parses the response frame got from the     */
+/*                  device.                                                  */
+/*                                                                           */
+/*  Inputs        : 1) The received response frame                           */
+/*                  2) WID                                                   */
+/*                  3) WID Length                                            */
+/*                  4) Output file handle                                    */
+/*                  5) Process Wid Number(i.e wid from --widn switch)        */
+/*                  6) Index the array in the Global Wid Structure.          */
+/*                                                                           */
+/*  Globals       : g_wid_num, gastrWIDs                                     */
+/*                                                                           */
+/*  Processing    : This function parses the response of the device depending*/
+/*                  WID type and writes it to the output file in Hex or      */
+/*                  decimal notation depending on the --getx or --get switch.*/
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : 0 on Success & -2 on Failure                             */
+/*                                                                           */
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2009   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+s32 further_process_response(u8 *resp,
+				     u16 u16WIDid,
+				     u16 cfg_len,
+				     bool process_wid_num,
+				     u32 cnt,
+				     tstrWID *pstrWIDresult)
+{
+	u32 retval = 0;
+	u32 idx = 0;
+	u8 cfg_chr  = 0;
+	u16 cfg_sht  = 0;
+	u32 cfg_int  = 0;
+	u8 cfg_str[256] = {0};
+	tenuWIDtype enuWIDtype = WID_UNDEF;
+
+	if (process_wid_num) {
+		enuWIDtype = get_wid_type(g_wid_num);
+	} else {
+		enuWIDtype = gastrWIDs[cnt].enuWIDtype;
+	}
+
+
+	switch (enuWIDtype) {
+	case WID_CHAR:
+		cfg_chr = resp[idx];
+		/*Set local copy of WID*/
+		*(pstrWIDresult->ps8WidVal) = cfg_chr;
+		break;
+
+	case WID_SHORT:
+	{
+		u16 *pu16val = (u16 *)(pstrWIDresult->ps8WidVal);
+		cfg_sht = MAKE_WORD16(resp[idx], resp[idx + 1]);
+		/*Set local copy of WID*/
+		/* pstrWIDresult->ps8WidVal = (s8*)(s32)cfg_sht; */
+		*pu16val = cfg_sht;
+		break;
+	}
+
+	case WID_INT:
+	{
+		u32 *pu32val = (u32 *)(pstrWIDresult->ps8WidVal);
+		cfg_int = MAKE_WORD32(
+				MAKE_WORD16(resp[idx], resp[idx + 1]),
+				MAKE_WORD16(resp[idx + 2], resp[idx + 3])
+				);
+		/*Set local copy of WID*/
+		/* pstrWIDresult->ps8WidVal = (s8*)cfg_int; */
+		*pu32val = cfg_int;
+		break;
+	}
+
+	case WID_STR:
+		WILC_memcpy(cfg_str, resp + idx, cfg_len);
+		/* cfg_str[cfg_len] = '\0'; //mostafa: no need currently for NULL termination */
+		if (process_wid_num) {
+			/*fprintf(out_file,"0x%4.4x = %s\n",g_wid_num,
+			 *                              cfg_str);*/
+		} else {
+			/*fprintf(out_file,"%s = %s\n",gastrWIDs[cnt].cfg_switch,
+			 *                           cfg_str);*/
+		}
+
+		if (pstrWIDresult->s32ValueSize >= cfg_len) {
+			WILC_memcpy(pstrWIDresult->ps8WidVal, cfg_str, cfg_len); /* mostafa: no need currently for the extra NULL byte */
+			pstrWIDresult->s32ValueSize = cfg_len;
+		} else {
+			PRINT_ER("allocated WID buffer length is smaller than the received WID Length\n");
+			retval = -2;
+		}
+
+		break;
+
+	case WID_ADR:
+		create_mac_addr(cfg_str, resp + idx);
+
+		WILC_strncpy(pstrWIDresult->ps8WidVal, cfg_str, WILC_strlen(cfg_str));
+		pstrWIDresult->ps8WidVal[WILC_strlen(cfg_str)] = '\0';
+		if (process_wid_num) {
+			/*fprintf(out_file,"0x%4.4x = %s\n",g_wid_num,
+			 *                              cfg_str);*/
+		} else {
+			/*fprintf(out_file,"%s = %s\n",gastrWIDs[cnt].cfg_switch,
+			 *                           cfg_str);*/
+		}
+		break;
+
+	case WID_IP:
+		cfg_int = MAKE_WORD32(
+				MAKE_WORD16(resp[idx], resp[idx + 1]),
+				MAKE_WORD16(resp[idx + 2], resp[idx + 3])
+				);
+		conv_int_to_ip(cfg_str, cfg_int);
+		if (process_wid_num) {
+			/*fprintf(out_file,"0x%4.4x = %s\n",g_wid_num,
+			 *                              cfg_str);*/
+		} else {
+			/*fprintf(out_file,"%s = %s\n",gastrWIDs[cnt].cfg_switch,
+			 *                           cfg_str);*/
+		}
+		break;
+
+	case WID_BIN_DATA:
+		if (pstrWIDresult->s32ValueSize >= cfg_len) {
+			WILC_memcpy(pstrWIDresult->ps8WidVal, resp + idx, cfg_len);
+			pstrWIDresult->s32ValueSize = cfg_len;
+		} else {
+			PRINT_ER("Allocated WID buffer length is smaller than the received WID Length Err(%d)\n", retval);
+			retval = -2;
+		}
+		break;
+
+	default:
+		PRINT_ER("ERROR: Check config database: Error(%d)\n", retval);
+		retval = -2;
+		break;
+	}
+
+	return retval;
+}
+
+/*****************************************************************************/
+/*                                                                           */
+/*  Function Name : ParseResponse                                           */
+/*                                                                           */
+/*  Description   : This function parses the command-line options and        */
+/*                  creates the config packets which can be sent to the WLAN */
+/*                  station.                                                 */
+/*                                                                           */
+/*  Inputs        : 1) The received response frame                           */
+/*                                                                           */
+/*  Globals       : g_opt_list, gastrWIDs	                                 */
+/*                                                                           */
+/*  Processing    : This function parses the options and creates different   */
+/*                  types of packets depending upon the WID-type             */
+/*                  corresponding to the option.                             */
+/*                                                                           */
+/*  Outputs       : None                                                     */
+/*                                                                           */
+/*  Returns       : 0 on Success & -1 on Failure                             */
+/*                                                                           */
+/*  Issues        : None                                                     */
+/*                                                                           */
+/*  Revision History:                                                        */
+/*                                                                           */
+/*         DD MM YYYY   Author(s)       Changes (Describe the changes made)  */
+/*         08 01 2008   Ittiam          Draft                                */
+/*                                                                           */
+/*****************************************************************************/
+
+s32 ParseResponse(u8 *resp, tstrWID *pstrWIDcfgResult)
+{
+	u16 u16RespLen = 0;
+	u16 u16WIDid  = 0;
+	u16 cfg_len  = 0;
+	tenuWIDtype enuWIDtype = WID_UNDEF;
+	bool num_wid_processed = false;
+	u32 cnt = 0;
+	u32 idx = 0;
+	u32 ResCnt = 0;
+	/* Check whether the received frame is a valid response */
+	if (RESP_MSG_TYPE != resp[0]) {
+		PRINT_INFO(CORECONFIG_DBG, "Received Message format incorrect.\n");
+		return -1;
+	}
+
+	/* Extract Response Length */
+	u16RespLen = MAKE_WORD16(resp[2], resp[3]);
+	Res_Len = u16RespLen;
+
+	for (idx = MSG_HEADER_LEN; idx < u16RespLen; ) {
+		u16WIDid = MAKE_WORD16(resp[idx], resp[idx + 1]);
+		cfg_len = resp[idx + 2];
+		/* Incase of Bin Type Wid, the length is given by two byte field      */
+		enuWIDtype = get_wid_type(u16WIDid);
+		if (WID_BIN_DATA == enuWIDtype) {
+			cfg_len |= ((u16)resp[idx + 3] << 8) & 0xFF00;
+			idx++;
+		}
+		idx += 3;
+		if ((u16WIDid == g_wid_num) && (!num_wid_processed)) {
+			num_wid_processed = true;
+
+			if (-2 == further_process_response(&resp[idx], u16WIDid, cfg_len, true, 0, &pstrWIDcfgResult[ResCnt])) {
+				return -2;
+			}
+			ResCnt++;
+		} else {
+			for (cnt = 0; cnt < g_num_total_switches; cnt++) {
+				if (gastrWIDs[cnt].u16WIDid == u16WIDid) {
+					if (-2 == further_process_response(&resp[idx], u16WIDid, cfg_len, false, cnt,
+									   &pstrWIDcfgResult[ResCnt])) {
+						return -2;
+					}
+					ResCnt++;
+				}
+			}
+		}
+		idx += cfg_len;
+		/* In case if BIN type Wid, The last byte of the Cfg packet is the    */
+		/* Checksum. The WID Length field does not accounts for the checksum. */
+		/* The Checksum is discarded.                                         */
+		if (WID_BIN_DATA == enuWIDtype) {
+			idx++;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ *  @brief              parses the write response [just detects its status: success or failure]
+ *  @details
+ *  @param[in]  pu8RespBuffer The Response to be parsed
+ *  @return     Error code indicating Write Operation status:
+ *                            WRITE_RESP_SUCCESS (1) => Write Success.
+ *                            WILC_FAIL (-100)               => Write Failure.
+ *  @note
+ *  @author		Ittiam
+ *  @date		11 Aug 2009
+ *  @version	1.0
+ */
+
+s32 ParseWriteResponse(u8 *pu8RespBuffer)
+{
+	s32 s32Error = WILC_FAIL;
+	u16 u16RespLen   = 0;
+	u16 u16WIDtype = (u16)WID_NIL;
+
+	/* Check whether the received frame is a valid response */
+	if (RESP_MSG_TYPE != pu8RespBuffer[0]) {
+		PRINT_ER("Received Message format incorrect.\n");
+		return WILC_FAIL;
+	}
+
+	/* Extract Response Length */
+	u16RespLen = MAKE_WORD16(pu8RespBuffer[2], pu8RespBuffer[3]);
+
+	u16WIDtype = MAKE_WORD16(pu8RespBuffer[4], pu8RespBuffer[5]);
+
+	/* Check for WID_STATUS ID and then check the length and status value */
+	if ((u16WIDtype == WID_STATUS) &&
+	    (pu8RespBuffer[6] == 1) &&
+	    (pu8RespBuffer[7] == WRITE_RESP_SUCCESS)) {
+		s32Error = WRITE_RESP_SUCCESS;
+		return s32Error;
+	}
+
+	/* If the length or status are not as expected return failure    */
+	s32Error = WILC_FAIL;
+	return s32Error;
+
+}
+
+/**
+ *  @brief                      creates the header of the Configuration Packet
+ *  @details
+ *  @param[in,out] pcpacket The Configuration Packet
+ *  @param[in,out] ps32PacketLength Length of the Configuration Packet
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		aismail
+ *  @date		18 Feb 2012
+ *  @version		1.0
+ */
+
+s32 CreatePacketHeader(char *pcpacket, s32 *ps32PacketLength)
+{
+	s32 s32Error = WILC_SUCCESS;
+	u16 u16MsgLen = (u16)(*ps32PacketLength);
+	u16 u16MsgInd = 0;
+
+	/* The format of the message is:                                         */
+	/* +-------------------------------------------------------------------+ */
+	/* | Message Type | Message ID |  Message Length |Message body         | */
+	/* +-------------------------------------------------------------------+ */
+	/* |     1 Byte   |   1 Byte   |     2 Bytes     | Message Length - 4  | */
+	/* +-------------------------------------------------------------------+ */
+
+	/* The format of a message body of a message type 'W' is:                */
+	/* +-------------------------------------------------------------------+ */
+	/* | WID0      | WID0 Length | WID0 Value  | ......................... | */
+	/* +-------------------------------------------------------------------+ */
+	/* | 2 Bytes   | 1 Byte      | WID0 Length | ......................... | */
+	/* +-------------------------------------------------------------------+ */
+
+
+
+	/* Message Type */
+	if (g_oper_mode == SET_CFG)
+		pcpacket[u16MsgInd++] = WRITE_MSG_TYPE;
+	else
+		pcpacket[u16MsgInd++] = QUERY_MSG_TYPE;
+
+	/* Sequence Number */
+	pcpacket[u16MsgInd++] = g_seqno++;
+
+	/* Message Length */
+	pcpacket[u16MsgInd++] = (u8)(u16MsgLen & 0xFF);
+	pcpacket[u16MsgInd++] = (u8)((u16MsgLen >> 8) & 0xFF);
+
+	*ps32PacketLength = u16MsgLen;
+
+	return s32Error;
+}
+
+/**
+ *  @brief              creates Configuration packet based on the Input WIDs
+ *  @details
+ *  @param[in]  pstrWIDs WIDs to be sent in the configuration packet
+ *  @param[in]  u32WIDsCount number of WIDs to be sent in the configuration packet
+ *  @param[out]         ps8packet The created Configuration Packet
+ *  @param[out]         ps32PacketLength Length of the created Configuration Packet
+ *  @return     Error code indicating success/failure
+ *  @note
+ *  @author
+ *  @date		1 Mar 2012
+ *  @version	1.0
+ */
+
+s32 CreateConfigPacket(s8 *ps8packet, s32 *ps32PacketLength,
+			       tstrWID *pstrWIDs, u32 u32WIDsCount)
+{
+	s32 s32Error = WILC_SUCCESS;
+	u32 u32idx = 0;
+	*ps32PacketLength = MSG_HEADER_LEN;
+	for (u32idx = 0; u32idx < u32WIDsCount; u32idx++) {
+		switch (pstrWIDs[u32idx].enuWIDtype) {
+		case WID_CHAR:
+			ProcessCharWid(ps8packet, ps32PacketLength, &pstrWIDs[u32idx],
+				       pstrWIDs[u32idx].ps8WidVal);
+			break;
+
+		case WID_SHORT:
+			ProcessShortWid(ps8packet, ps32PacketLength, &pstrWIDs[u32idx],
+					pstrWIDs[u32idx].ps8WidVal);
+			break;
+
+		case WID_INT:
+			ProcessIntWid(ps8packet, ps32PacketLength, &pstrWIDs[u32idx],
+				      pstrWIDs[u32idx].ps8WidVal);
+			break;
+
+		case WID_STR:
+			ProcessStrWid(ps8packet, ps32PacketLength, &pstrWIDs[u32idx],
+				      pstrWIDs[u32idx].ps8WidVal, pstrWIDs[u32idx].s32ValueSize);
+			break;
+
+		case WID_IP:
+			ProcessIPwid(ps8packet, ps32PacketLength, &pstrWIDs[u32idx],
+				     pstrWIDs[u32idx].ps8WidVal);
+			break;
+
+		case WID_BIN_DATA:
+			ProcessBinWid(ps8packet, ps32PacketLength, &pstrWIDs[u32idx],
+				      pstrWIDs[u32idx].ps8WidVal, pstrWIDs[u32idx].s32ValueSize);
+			break;
+
+		default:
+			PRINT_ER("ERROR: Check Config database\n");
+		}
+	}
+
+	CreatePacketHeader(ps8packet, ps32PacketLength);
+
+	return s32Error;
+}
+
+s32 ConfigWaitResponse(char *pcRespBuffer, s32 s32MaxRespBuffLen, s32 *ps32BytesRead,
+			       bool bRespRequired)
+{
+	s32 s32Error = WILC_SUCCESS;
+	/*bug 3878*/
+	/*removed to caller function*/
+	/*gstrConfigPktInfo.pcRespBuffer = pcRespBuffer;
+	 * gstrConfigPktInfo.s32MaxRespBuffLen = s32MaxRespBuffLen;
+	 * gstrConfigPktInfo.bRespRequired = bRespRequired;*/
+
+
+	if (gstrConfigPktInfo.bRespRequired) {
+		down(&SemHandlePktResp);
+
+		*ps32BytesRead = gstrConfigPktInfo.s32BytesRead;
+	}
+
+	WILC_memset((void *)(&gstrConfigPktInfo), 0, sizeof(tstrConfigPktInfo));
+
+	return s32Error;
+}
+
+/**
+ *  @brief              sends certain Configuration Packet based on the input WIDs pstrWIDs
+ *                      and retrieves the packet response pu8RxResp
+ *  @details
+ *  @param[in]  pstrWIDs WIDs to be sent in the configuration packet
+ *  @param[in]  u32WIDsCount number of WIDs to be sent in the configuration packet
+ *  @param[out]         pu8RxResp The received Packet Response
+ *  @param[out]         ps32RxRespLen Length of the received Packet Response
+ *  @return     Error code indicating success/failure
+ *  @note
+ *  @author	mabubakr
+ *  @date		1 Mar 2012
+ *  @version	1.0
+ */
+#ifdef SIMULATION
+s32 SendConfigPkt(u8 u8Mode, tstrWID *pstrWIDs,
+			  u32 u32WIDsCount, bool bRespRequired, u32 drvHandler)
+{
+	s32 s32Error = WILC_SUCCESS;
+	s32 err = WILC_SUCCESS;
+	s32 s32ConfigPacketLen = 0;
+	s32 s32RcvdRespLen = 0;
+
+	down(&SemHandleSendPkt);
+
+	/*set the packet mode*/
+	g_oper_mode = u8Mode;
+
+	WILC_memset((void *)gps8ConfigPacket, 0, MAX_PACKET_BUFF_SIZE);
+
+	if (CreateConfigPacket(gps8ConfigPacket, &s32ConfigPacketLen, pstrWIDs, u32WIDsCount) != WILC_SUCCESS) {
+		s32Error = WILC_FAIL;
+		goto End_ConfigPkt;
+	}
+	/*bug 3878*/
+	gstrConfigPktInfo.pcRespBuffer = gps8ConfigPacket;
+	gstrConfigPktInfo.s32MaxRespBuffLen = MAX_PACKET_BUFF_SIZE;
+	PRINT_INFO(CORECONFIG_DBG, "GLOBAL =bRespRequired =%d\n", bRespRequired);
+	gstrConfigPktInfo.bRespRequired = bRespRequired;
+
+	s32Error = SendRawPacket(gps8ConfigPacket, s32ConfigPacketLen);
+	if (s32Error != WILC_SUCCESS) {
+		goto End_ConfigPkt;
+	}
+
+	WILC_memset((void *)gps8ConfigPacket, 0, MAX_PACKET_BUFF_SIZE);
+
+	ConfigWaitResponse(gps8ConfigPacket, MAX_PACKET_BUFF_SIZE, &s32RcvdRespLen, bRespRequired);
+
+
+	if (bRespRequired)	{
+		/* If the operating Mode is GET, then we expect a response frame from */
+		/* the driver. Hence start listening to the port for response         */
+		if (g_oper_mode == GET_CFG) {
+			#if 1
+			err = ParseResponse(gps8ConfigPacket, pstrWIDs);
+			if (err != 0) {
+				s32Error = WILC_FAIL;
+				goto End_ConfigPkt;
+			} else {
+				s32Error = WILC_SUCCESS;
+			}
+			#endif
+		} else {
+			err = ParseWriteResponse(gps8ConfigPacket);
+			if (err != WRITE_RESP_SUCCESS) {
+				s32Error = WILC_FAIL;
+				goto End_ConfigPkt;
+			} else {
+				s32Error = WILC_SUCCESS;
+			}
+		}
+
+
+	}
+
+
+End_ConfigPkt:
+	up(&SemHandleSendPkt);
+
+	return s32Error;
+}
+#endif
+s32 ConfigProvideResponse(char *pcRespBuffer, s32 s32RespLen)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	if (gstrConfigPktInfo.bRespRequired) {
+		if (s32RespLen <= gstrConfigPktInfo.s32MaxRespBuffLen) {
+			WILC_memcpy(gstrConfigPktInfo.pcRespBuffer, pcRespBuffer, s32RespLen);
+			gstrConfigPktInfo.s32BytesRead = s32RespLen;
+		} else {
+			WILC_memcpy(gstrConfigPktInfo.pcRespBuffer, pcRespBuffer, gstrConfigPktInfo.s32MaxRespBuffLen);
+			gstrConfigPktInfo.s32BytesRead = gstrConfigPktInfo.s32MaxRespBuffLen;
+			PRINT_ER("BusProvideResponse() Response greater than the prepared Buffer Size\n");
+		}
+
+		up(&SemHandlePktResp);
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              writes the received packet pu8RxPacket in the global Rx FIFO buffer
+ *  @details
+ *  @param[in]  pu8RxPacket The received packet
+ *  @param[in]  s32RxPacketLen Length of the received packet
+ *  @return     Error code indicating success/failure
+ *  @note
+ *
+ *  @author	mabubakr
+ *  @date		1 Mar 2012
+ *  @version	1.0
+ */
+
+s32 ConfigPktReceived(u8 *pu8RxPacket, s32 s32RxPacketLen)
+{
+	s32 s32Error = WILC_SUCCESS;
+	u8 u8MsgType = 0;
+
+	u8MsgType = pu8RxPacket[0];
+
+	switch (u8MsgType) {
+	case 'R':
+		ConfigProvideResponse(pu8RxPacket, s32RxPacketLen);
+
+		break;
+
+	case 'N':
+		PRINT_INFO(CORECONFIG_DBG, "NetworkInfo packet received\n");
+		NetworkInfoReceived(pu8RxPacket, s32RxPacketLen);
+		break;
+
+	case 'I':
+		GnrlAsyncInfoReceived(pu8RxPacket, s32RxPacketLen);
+		break;
+
+	case 'S':
+		host_int_ScanCompleteReceived(pu8RxPacket, s32RxPacketLen);
+		break;
+
+	default:
+		PRINT_ER("ConfigPktReceived(): invalid received msg type at the Core Configurator\n");
+		break;
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              Deinitializes the Core Configurator
+ *  @details
+ *  @return     Error code indicating success/failure
+ *  @note
+ *  @author	mabubakr
+ *  @date		1 Mar 2012
+ *  @version	1.0
+ */
+
+s32 CoreConfiguratorDeInit(void)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	PRINT_D(CORECONFIG_DBG, "CoreConfiguratorDeInit()\n");
+
+	if (gps8ConfigPacket != NULL) {
+
+		WILC_FREE(gps8ConfigPacket);
+		gps8ConfigPacket = NULL;
+	}
+
+	return s32Error;
+}
+
+
+#ifndef SIMULATION
+/*Using the global handle of the driver*/
+extern wilc_wlan_oup_t *gpstrWlanOps;
+/**
+ *  @brief              sends certain Configuration Packet based on the input WIDs pstrWIDs
+ *  using driver config layer
+ *
+ *  @details
+ *  @param[in]  pstrWIDs WIDs to be sent in the configuration packet
+ *  @param[in]  u32WIDsCount number of WIDs to be sent in the configuration packet
+ *  @param[out]         pu8RxResp The received Packet Response
+ *  @param[out]         ps32RxRespLen Length of the received Packet Response
+ *  @return     Error code indicating success/failure
+ *  @note
+ *  @author	mabubakr
+ *  @date		1 Mar 2012
+ *  @version	1.0
+ */
+s32 SendConfigPkt(u8 u8Mode, tstrWID *pstrWIDs,
+			  u32 u32WIDsCount, bool bRespRequired, u32 drvHandler)
+{
+	s32 counter = 0, ret = 0;
+	if (gpstrWlanOps == NULL) {
+		PRINT_D(CORECONFIG_DBG, "Net Dev is still not initialized\n");
+		return 1;
+	} else {
+		PRINT_D(CORECONFIG_DBG, "Net Dev is initialized\n");
+	}
+	if (gpstrWlanOps->wlan_cfg_set == NULL ||
+	    gpstrWlanOps->wlan_cfg_get == NULL)	{
+		PRINT_D(CORECONFIG_DBG, "Set and Get is still not initialized\n");
+		return 1;
+	} else {
+		PRINT_D(CORECONFIG_DBG, "SET is initialized\n");
+	}
+	if (u8Mode == GET_CFG) {
+		for (counter = 0; counter < u32WIDsCount; counter++) {
+			PRINT_INFO(CORECONFIG_DBG, "Sending CFG packet [%d][%d]\n", !counter,
+				   (counter == u32WIDsCount - 1));
+			if (!gpstrWlanOps->wlan_cfg_get(!counter,
+							pstrWIDs[counter].u16WIDid,
+							(counter == u32WIDsCount - 1), drvHandler)) {
+				ret = -1;
+				printk("[Sendconfigpkt]Get Timed out\n");
+				break;
+			}
+		}
+		/**
+		 *      get the value
+		 **/
+		/* WILC_Sleep(1000); */
+		counter = 0;
+		for (counter = 0; counter < u32WIDsCount; counter++) {
+			pstrWIDs[counter].s32ValueSize = gpstrWlanOps->wlan_cfg_get_value(
+					pstrWIDs[counter].u16WIDid,
+					pstrWIDs[counter].ps8WidVal, pstrWIDs[counter].s32ValueSize);
+
+		}
+	} else if (u8Mode == SET_CFG) {
+		for (counter = 0; counter < u32WIDsCount; counter++) {
+			PRINT_D(CORECONFIG_DBG, "Sending config SET PACKET WID:%x\n", pstrWIDs[counter].u16WIDid);
+			if (!gpstrWlanOps->wlan_cfg_set(!counter,
+							pstrWIDs[counter].u16WIDid, pstrWIDs[counter].ps8WidVal,
+							pstrWIDs[counter].s32ValueSize,
+							(counter == u32WIDsCount - 1), drvHandler)) {
+				ret = -1;
+				printk("[Sendconfigpkt]Set Timed out\n");
+				break;
+			}
+		}
+	}
+
+	return ret;
+}
+#endif
diff --git a/drivers/staging/wilc1000/coreconfigurator.h b/drivers/staging/wilc1000/coreconfigurator.h
new file mode 100644
index 000000000000..9059c8df7ce5
--- /dev/null
+++ b/drivers/staging/wilc1000/coreconfigurator.h
@@ -0,0 +1,197 @@
+
+/*!
+ *  @file	coreconfigurator.h
+ *  @brief
+ *  @author
+ *  @sa		coreconfigurator.c
+ *  @date	1 Mar 2012
+ *  @version	1.0
+ */
+
+
+#ifndef CORECONFIGURATOR_H
+#define CORECONFIGURATOR_H
+
+#include "wilc_wlan_if.h"
+/*****************************************************************************/
+/* Constants                                                                 */
+/*****************************************************************************/
+/* Number of WID Options Supported */
+#define NUM_BASIC_SWITCHES      45
+#define NUM_FHSS_SWITCHES       0
+
+#define NUM_RSSI                5
+
+#ifdef MAC_802_11N
+#define NUM_11N_BASIC_SWITCHES  25
+#define NUM_11N_HUT_SWITCHES    47
+#else /* MAC_802_11N */
+#define NUM_11N_BASIC_SWITCHES  0
+#define NUM_11N_HUT_SWITCHES    0
+#endif /* MAC_802_11N */
+
+extern u16 g_num_total_switches;
+
+#define MAC_HDR_LEN             24          /* No Address4 - non-ESS         */
+#define MAX_SSID_LEN            33
+#define FCS_LEN                 4
+#define TIME_STAMP_LEN          8
+#define BEACON_INTERVAL_LEN     2
+#define CAP_INFO_LEN            2
+#define STATUS_CODE_LEN         2
+#define AID_LEN                 2
+#define IE_HDR_LEN              2
+
+
+/* Operating Mode: SET */
+#define SET_CFG              0
+/* Operating Mode: GET */
+#define GET_CFG              1
+
+#define MAX_PACKET_BUFF_SIZE 1596
+
+#define MAX_STRING_LEN               256
+#define MAX_SURVEY_RESULT_FRAG_SIZE  MAX_STRING_LEN
+#define SURVEY_RESULT_LENGTH         44
+#define MAX_ASSOC_RESP_FRAME_SIZE    MAX_STRING_LEN
+
+#define STATUS_MSG_LEN               12
+#define MAC_CONNECTED                1
+#define MAC_DISCONNECTED             0
+
+
+
+/*****************************************************************************/
+/* Function Macros                                                           */
+/*****************************************************************************/
+#define MAKE_WORD16(lsb, msb) ((((u16)(msb) << 8) & 0xFF00) | (lsb))
+#define MAKE_WORD32(lsw, msw) ((((u32)(msw) << 16) & 0xFFFF0000) | (lsw))
+
+
+/*****************************************************************************/
+/* Type Definitions                                                                                                                       */
+/*****************************************************************************/
+/* Status Codes for Authentication and Association Frames */
+typedef enum {
+	SUCCESSFUL_STATUSCODE    = 0,
+	UNSPEC_FAIL              = 1,
+	UNSUP_CAP                = 10,
+	REASOC_NO_ASOC           = 11,
+	FAIL_OTHER               = 12,
+	UNSUPT_ALG               = 13,
+	AUTH_SEQ_FAIL            = 14,
+	CHLNG_FAIL               = 15,
+	AUTH_TIMEOUT             = 16,
+	AP_FULL                  = 17,
+	UNSUP_RATE               = 18,
+	SHORT_PREAMBLE_UNSUP     = 19,
+	PBCC_UNSUP               = 20,
+	CHANNEL_AGIL_UNSUP       = 21,
+	SHORT_SLOT_UNSUP         = 25,
+	OFDM_DSSS_UNSUP          = 26,
+	CONNECT_STS_FORCE_16_BIT = 0xFFFF
+} tenuConnectSts;
+
+typedef struct {
+	u16 u16WIDid;
+	tenuWIDtype enuWIDtype;
+	s32 s32ValueSize;
+	s8      *ps8WidVal;
+
+} tstrWID;
+
+typedef struct {
+	u8 u8Full;
+	u8 u8Index;
+	s8 as8RSSI[NUM_RSSI];
+} tstrRSSI;
+/* This structure is used to support parsing of the received 'N' message */
+typedef struct {
+	s8 s8rssi;
+	u16 u16CapInfo;
+	u8 au8ssid[MAX_SSID_LEN];
+	u8 u8SsidLen;
+	u8 au8bssid[6];
+	u16 u16BeaconPeriod;
+	u8 u8DtimPeriod;
+	u8 u8channel;
+	unsigned long u32TimeRcvdInScanCached; /* of type unsigned long to be accepted by the linux kernel macro time_after() */
+	unsigned long u32TimeRcvdInScan;
+	bool bNewNetwork;
+#ifdef AGING_ALG
+	u8 u8Found;
+#endif
+#ifdef WILC_P2P
+	u32 u32Tsf; /* time-stamp [Low only 32 bit] */
+#endif
+	u8 *pu8IEs;
+	u16 u16IEsLen;
+	void *pJoinParams;
+	tstrRSSI strRssi;
+	u64 u64Tsf; /* time-stamp [Low and High 64 bit] */
+} tstrNetworkInfo;
+
+/* This structure is used to support parsing of the received Association Response frame */
+typedef struct {
+	u16 u16capability;
+	u16 u16ConnectStatus;
+	u16 u16AssocID;
+	u8 *pu8RespIEs;
+	u16 u16RespIEsLen;
+} tstrConnectRespInfo;
+
+
+typedef struct {
+	u8 au8bssid[6];
+	u8 *pu8ReqIEs;
+	size_t ReqIEsLen;
+	u8 *pu8RespIEs;
+	u16 u16RespIEsLen;
+	u16 u16ConnectStatus;
+} tstrConnectInfo;
+
+
+
+typedef struct {
+	u16 u16reason;
+	u8 *ie;
+	size_t ie_len;
+} tstrDisconnectNotifInfo;
+
+#ifndef CONNECT_DIRECT
+typedef struct wid_site_survey_reslts {
+	char SSID[MAX_SSID_LEN];
+	u8 BssType;
+	u8 Channel;
+	u8 SecurityStatus;
+	u8 BSSID[6];
+	char RxPower;
+	u8 Reserved;
+
+} wid_site_survey_reslts_s;
+#endif
+
+extern s32 CoreConfiguratorInit(void);
+extern s32 CoreConfiguratorDeInit(void);
+
+extern s32 SendConfigPkt(u8 u8Mode, tstrWID *pstrWIDs,
+				 u32 u32WIDsCount, bool bRespRequired, u32 drvHandler);
+extern s32 ParseNetworkInfo(u8 *pu8MsgBuffer, tstrNetworkInfo **ppstrNetworkInfo);
+extern s32 DeallocateNetworkInfo(tstrNetworkInfo *pstrNetworkInfo);
+
+extern s32 ParseAssocRespInfo(u8 *pu8Buffer, u32 u32BufferLen,
+				      tstrConnectRespInfo **ppstrConnectRespInfo);
+extern s32 DeallocateAssocRespInfo(tstrConnectRespInfo *pstrConnectRespInfo);
+
+#ifndef CONNECT_DIRECT
+extern s32 ParseSurveyResults(u8 ppu8RcvdSiteSurveyResults[][MAX_SURVEY_RESULT_FRAG_SIZE],
+				      wid_site_survey_reslts_s **ppstrSurveyResults, u32 *pu32SurveyResultsCount);
+extern s32 DeallocateSurveyResults(wid_site_survey_reslts_s *pstrSurveyResults);
+#endif
+
+extern s32 SendRawPacket(s8 *pspacket, s32 s32PacketLen);
+extern void NetworkInfoReceived(u8 *pu8Buffer, u32 u32Length);
+void GnrlAsyncInfoReceived(u8 *pu8Buffer, u32 u32Length);
+void host_int_ScanCompleteReceived(u8 *pu8Buffer, u32 u32Length);
+
+#endif
diff --git a/drivers/staging/wilc1000/fifo_buffer.c b/drivers/staging/wilc1000/fifo_buffer.c
new file mode 100644
index 000000000000..b6c07cfc43d2
--- /dev/null
+++ b/drivers/staging/wilc1000/fifo_buffer.c
@@ -0,0 +1,133 @@
+
+
+#include "fifo_buffer.h"
+
+
+
+u32 FIFO_InitBuffer(tHANDLE *hBuffer, u32 u32BufferLength)
+{
+	u32 u32Error = 0;
+	tstrFifoHandler *pstrFifoHandler = WILC_MALLOC (sizeof (tstrFifoHandler));
+	if (pstrFifoHandler) {
+		WILC_memset (pstrFifoHandler, 0, sizeof (tstrFifoHandler));
+		pstrFifoHandler->pu8Buffer = WILC_MALLOC (u32BufferLength);
+		if (pstrFifoHandler->pu8Buffer)	{
+			pstrFifoHandler->u32BufferLength = u32BufferLength;
+			WILC_memset (pstrFifoHandler->pu8Buffer, 0, u32BufferLength);
+			/* create semaphore */
+			sema_init(&pstrFifoHandler->SemBuffer, 1);
+			*hBuffer = pstrFifoHandler;
+		} else {
+			*hBuffer = NULL;
+			u32Error = 1;
+		}
+	} else {
+		u32Error = 1;
+	}
+	return u32Error;
+}
+u32 FIFO_DeInit(tHANDLE hFifo)
+{
+	u32 u32Error = 0;
+	tstrFifoHandler *pstrFifoHandler = (tstrFifoHandler *) hFifo;
+	if (pstrFifoHandler) {
+		if (pstrFifoHandler->pu8Buffer)	{
+			WILC_FREE (pstrFifoHandler->pu8Buffer);
+		} else {
+			u32Error = 1;
+		}
+
+		WILC_FREE (pstrFifoHandler);
+	} else {
+		u32Error = 1;
+	}
+	return u32Error;
+}
+
+u32 FIFO_ReadBytes(tHANDLE hFifo, u8 *pu8Buffer, u32 u32BytesToRead, u32 *pu32BytesRead)
+{
+	u32 u32Error = 0;
+	tstrFifoHandler *pstrFifoHandler = (tstrFifoHandler *) hFifo;
+	if (pstrFifoHandler && pu32BytesRead) {
+		if (pstrFifoHandler->u32TotalBytes) {
+			down(&pstrFifoHandler->SemBuffer);
+
+			if (u32BytesToRead > pstrFifoHandler->u32TotalBytes) {
+				*pu32BytesRead = pstrFifoHandler->u32TotalBytes;
+			} else {
+				*pu32BytesRead = u32BytesToRead;
+			}
+			if ((pstrFifoHandler->u32ReadOffset + u32BytesToRead) <= pstrFifoHandler->u32BufferLength) {
+				WILC_memcpy(pu8Buffer, pstrFifoHandler->pu8Buffer + pstrFifoHandler->u32ReadOffset,
+					    *pu32BytesRead);
+				/* update read offset and total bytes */
+				pstrFifoHandler->u32ReadOffset += u32BytesToRead;
+				pstrFifoHandler->u32TotalBytes -= u32BytesToRead;
+
+			} else {
+				u32 u32FirstPart =
+					pstrFifoHandler->u32BufferLength - pstrFifoHandler->u32ReadOffset;
+				WILC_memcpy(pu8Buffer, pstrFifoHandler->pu8Buffer + pstrFifoHandler->u32ReadOffset,
+					    u32FirstPart);
+				WILC_memcpy(pu8Buffer + u32FirstPart, pstrFifoHandler->pu8Buffer,
+					    u32BytesToRead - u32FirstPart);
+				/* update read offset and total bytes */
+				pstrFifoHandler->u32ReadOffset = u32BytesToRead - u32FirstPart;
+				pstrFifoHandler->u32TotalBytes -= u32BytesToRead;
+			}
+			up(&pstrFifoHandler->SemBuffer);
+		} else {
+			u32Error = 1;
+		}
+	} else {
+		u32Error = 1;
+	}
+	return u32Error;
+}
+
+u32 FIFO_WriteBytes(tHANDLE hFifo, u8 *pu8Buffer, u32 u32BytesToWrite, bool bForceOverWrite)
+{
+	u32 u32Error = 0;
+	tstrFifoHandler *pstrFifoHandler = (tstrFifoHandler *) hFifo;
+	if (pstrFifoHandler) {
+		if (u32BytesToWrite < pstrFifoHandler->u32BufferLength)	{
+			if ((pstrFifoHandler->u32TotalBytes + u32BytesToWrite) <= pstrFifoHandler->u32BufferLength ||
+			    bForceOverWrite) {
+				down(&pstrFifoHandler->SemBuffer);
+				if ((pstrFifoHandler->u32WriteOffset + u32BytesToWrite) <= pstrFifoHandler->u32BufferLength) {
+					WILC_memcpy(pstrFifoHandler->pu8Buffer + pstrFifoHandler->u32WriteOffset, pu8Buffer,
+						    u32BytesToWrite);
+					/* update read offset and total bytes */
+					pstrFifoHandler->u32WriteOffset += u32BytesToWrite;
+					pstrFifoHandler->u32TotalBytes  += u32BytesToWrite;
+
+				} else {
+					u32 u32FirstPart =
+						pstrFifoHandler->u32BufferLength - pstrFifoHandler->u32WriteOffset;
+					WILC_memcpy(pstrFifoHandler->pu8Buffer + pstrFifoHandler->u32WriteOffset, pu8Buffer,
+						    u32FirstPart);
+					WILC_memcpy(pstrFifoHandler->pu8Buffer, pu8Buffer + u32FirstPart,
+						    u32BytesToWrite - u32FirstPart);
+					/* update read offset and total bytes */
+					pstrFifoHandler->u32WriteOffset = u32BytesToWrite - u32FirstPart;
+					pstrFifoHandler->u32TotalBytes += u32BytesToWrite;
+				}
+				/* if data overwriten */
+				if (pstrFifoHandler->u32TotalBytes > pstrFifoHandler->u32BufferLength) {
+					/* adjust read offset to the oldest data available */
+					pstrFifoHandler->u32ReadOffset = pstrFifoHandler->u32WriteOffset;
+					/* data availabe is the buffer length */
+					pstrFifoHandler->u32TotalBytes = pstrFifoHandler->u32BufferLength;
+				}
+				up(&pstrFifoHandler->SemBuffer);
+			} else {
+				u32Error = 1;
+			}
+		} else {
+			u32Error = 1;
+		}
+	} else {
+		u32Error = 1;
+	}
+	return u32Error;
+}
diff --git a/drivers/staging/wilc1000/fifo_buffer.h b/drivers/staging/wilc1000/fifo_buffer.h
new file mode 100644
index 000000000000..7b76998e4238
--- /dev/null
+++ b/drivers/staging/wilc1000/fifo_buffer.h
@@ -0,0 +1,26 @@
+
+#include <linux/types.h>
+#include <linux/semaphore.h>
+#include "wilc_memory.h"
+#include "wilc_strutils.h"
+
+
+#define tHANDLE	void *
+
+typedef struct {
+	u8		*pu8Buffer;
+	u32 u32BufferLength;
+	u32 u32WriteOffset;
+	u32 u32ReadOffset;
+	u32 u32TotalBytes;
+	struct semaphore SemBuffer;
+} tstrFifoHandler;
+
+
+extern u32 FIFO_InitBuffer(tHANDLE *hBuffer,
+								   u32 u32BufferLength);
+extern u32 FIFO_DeInit(tHANDLE hFifo);
+extern u32 FIFO_ReadBytes(tHANDLE hFifo, u8 *pu8Buffer,
+				u32 u32BytesToRead, u32 *pu32BytesRead);
+extern u32 FIFO_WriteBytes(tHANDLE hFifo, u8 *pu8Buffer,
+				u32 u32BytesToWrite, bool bForceOverWrite);
diff --git a/drivers/staging/wilc1000/host_interface.c b/drivers/staging/wilc1000/host_interface.c
new file mode 100644
index 000000000000..6b10bbbe6ab2
--- /dev/null
+++ b/drivers/staging/wilc1000/host_interface.c
@@ -0,0 +1,7963 @@
+#include "host_interface.h"
+#include "coreconfigurator.h"
+
+extern s32 TransportInit(void);
+extern s32 TransportDeInit(void);
+extern u8 connecting;
+
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+extern WILC_TimerHandle hDuringIpTimer;
+#endif
+
+extern bool bEnablePS;
+/*BugID_5137*/
+extern u8 g_wilc_initialized;
+/*****************************************************************************/
+/*								Macros                                       */
+/*****************************************************************************/
+
+/* Message types of the Host IF Message Queue*/
+#define HOST_IF_MSG_SCAN                        0
+#define HOST_IF_MSG_CONNECT                     1
+#define HOST_IF_MSG_RCVD_GNRL_ASYNC_INFO        2
+#define HOST_IF_MSG_KEY                         3
+#define HOST_IF_MSG_RCVD_NTWRK_INFO             4
+#define HOST_IF_MSG_RCVD_SCAN_COMPLETE          5
+#define HOST_IF_MSG_CFG_PARAMS                  6
+#define HOST_IF_MSG_SET_CHANNEL                 7
+#define HOST_IF_MSG_DISCONNECT                  8
+#define HOST_IF_MSG_GET_RSSI                    9
+#define HOST_IF_MSG_GET_CHNL                    10
+#define HOST_IF_MSG_ADD_BEACON                  11
+#define HOST_IF_MSG_DEL_BEACON                  12
+#define HOST_IF_MSG_ADD_STATION                 13
+#define HOST_IF_MSG_DEL_STATION                 14
+#define HOST_IF_MSG_EDIT_STATION                15
+#define HOST_IF_MSG_SCAN_TIMER_FIRED            16
+#define HOST_IF_MSG_CONNECT_TIMER_FIRED         17
+#define HOST_IF_MSG_POWER_MGMT                  18
+#define HOST_IF_MSG_GET_INACTIVETIME            19
+#define HOST_IF_MSG_REMAIN_ON_CHAN              20
+#define HOST_IF_MSG_REGISTER_FRAME              21
+#define HOST_IF_MSG_LISTEN_TIMER_FIRED          22
+#define HOST_IF_MSG_GET_LINKSPEED               23
+#define HOST_IF_MSG_SET_WFIDRV_HANDLER          24
+#define HOST_IF_MSG_SET_MAC_ADDRESS             25
+#define HOST_IF_MSG_GET_MAC_ADDRESS             26
+#define HOST_IF_MSG_SET_OPERATION_MODE          27
+#define HOST_IF_MSG_SET_IPADDRESS               28
+#define HOST_IF_MSG_GET_IPADDRESS               29
+#define HOST_IF_MSG_FLUSH_CONNECT               30
+#define HOST_IF_MSG_GET_STATISTICS              31
+#define HOST_IF_MSG_SET_MULTICAST_FILTER        32
+#define HOST_IF_MSG_ADD_BA_SESSION              33
+#define HOST_IF_MSG_DEL_BA_SESSION              34
+#define HOST_IF_MSG_Q_IDLE                      35
+#define HOST_IF_MSG_DEL_ALL_STA                 36
+#define HOST_IF_MSG_DEL_ALL_RX_BA_SESSIONS      34
+#define HOST_IF_MSG_EXIT                        100
+
+#define HOST_IF_SCAN_TIMEOUT                    4000
+#define HOST_IF_CONNECT_TIMEOUT                 9500
+
+#define BA_SESSION_DEFAULT_BUFFER_SIZE          16
+#define BA_SESSION_DEFAULT_TIMEOUT              1000
+#define BLOCK_ACK_REQ_SIZE                      0x14
+/*****************************************************************************/
+/*								Type Definitions							 */
+/*****************************************************************************/
+
+/*!
+ *  @struct             tstrHostIFCfgParamAttr
+ *  @brief		Structure to hold Host IF CFG Params Attributes
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		02 April 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFCfgParamAttr {
+	tstrCfgParamVal pstrCfgParamVal;
+
+} tstrHostIFCfgParamAttr;
+
+/*!
+ *  @struct             tstrHostIFwpaAttr
+ *  @brief		Structure to hold Host IF Scan Attributes
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFwpaAttr {
+	u8 *pu8key;
+	const u8 *pu8macaddr;
+	u8 *pu8seq;
+	u8 u8seqlen;
+	u8 u8keyidx;
+	u8 u8Keylen;
+	u8 u8Ciphermode;
+} tstrHostIFwpaAttr;
+
+
+/*!
+ *  @struct             tstrHostIFwepAttr
+ *  @brief		Structure to hold Host IF Scan Attributes
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFwepAttr {
+	u8 *pu8WepKey;
+	u8 u8WepKeylen;
+	u8 u8Wepidx;
+	u8 u8mode;
+	AUTHTYPE_T tenuAuth_type;
+
+} tstrHostIFwepAttr;
+
+/*!
+ *  @struct             tuniHostIFkeyAttr
+ *  @brief		Structure to hold Host IF Scan Attributes
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef union _tuniHostIFkeyAttr {
+	tstrHostIFwepAttr strHostIFwepAttr;
+	tstrHostIFwpaAttr strHostIFwpaAttr;
+	tstrHostIFpmkidAttr strHostIFpmkidAttr;
+} tuniHostIFkeyAttr;
+
+/*!
+ *  @struct             tstrHostIFkeyAttr
+ *  @brief		Structure to hold Host IF Scan Attributes
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFkeyAttr {
+	tenuKeyType enuKeyType;
+	u8 u8KeyAction;
+	tuniHostIFkeyAttr uniHostIFkeyAttr;
+} tstrHostIFkeyAttr;
+
+
+
+
+/*!
+ *  @struct             tstrHostIFscanAttr
+ *  @brief		Structure to hold Host IF Scan Attributes
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mostafa Abu Bakr
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFscanAttr {
+	u8 u8ScanSource;
+	u8 u8ScanType;
+	u8 *pu8ChnlFreqList;
+	u8 u8ChnlListLen;
+	u8 *pu8IEs;
+	size_t IEsLen;
+	tWILCpfScanResult pfScanResult;
+	void *pvUserArg;
+	/*BugID_4189*/
+	tstrHiddenNetwork strHiddenNetwork;
+
+} tstrHostIFscanAttr;
+
+/*!
+ *  @struct             tstrHostIFconnectAttr
+ *  @brief		Structure to hold Host IF Connect Attributes
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mostafa Abu Bakr
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFconnectAttr {
+	u8 *pu8bssid;
+	u8 *pu8ssid;
+	size_t ssidLen;
+	u8 *pu8IEs;
+	size_t IEsLen;
+	u8 u8security;
+	tWILCpfConnectResult pfConnectResult;
+	void *pvUserArg;
+	AUTHTYPE_T tenuAuth_type;
+	u8 u8channel;
+	void *pJoinParams;
+} tstrHostIFconnectAttr;
+
+/*!
+ *  @struct             tstrRcvdGnrlAsyncInfo
+ *  @brief		Structure to hold Received General Asynchronous info
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mostafa Abu Bakr
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrRcvdGnrlAsyncInfo {
+	u8 *pu8Buffer;
+	u32 u32Length;
+} tstrRcvdGnrlAsyncInfo;
+
+/*!
+ *  @struct             tstrHostIFSetChan
+ *  @brief		Set Channel  message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFSetChan {
+	u8 u8SetChan;
+} tstrHostIFSetChan;
+
+/*!
+ *  @struct             tstrHostIFSetChan
+ *  @brief		Get Channel  message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		01 Jule 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFGetChan {
+	u8 u8GetChan;
+} tstrHostIFGetChan;
+
+/*bug3819: Add Scan acomplete notification to host*/
+/*!
+ *  @struct             tstrScanComplete
+ *  @brief			hold received Async. Scan Complete message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		zsalah
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+/*typedef struct _tstrScanComplete
+ * {
+ *      u8* pu8Buffer;
+ *      u32 u32Length;
+ * } tstrScanComplete;*/
+
+/*!
+ *  @struct             tstrHostIFSetBeacon
+ *  @brief		Set Beacon  message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		10 July 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFSetBeacon {
+	u32 u32Interval;                        /*!< Beacon Interval. Period between two successive beacons on air  */
+	u32 u32DTIMPeriod;              /*!< DTIM Period. Indicates how many Beacon frames
+											*                              (including the current frame) appear before the next DTIM		*/
+	u32 u32HeadLen;                         /*!< Length of the head buffer in bytes		*/
+	u8 *pu8Head;                    /*!< Pointer to the beacon's head buffer. Beacon's head	is the part
+											*              from the beacon's start till the TIM element, NOT including the TIM		*/
+	u32 u32TailLen;                         /*!< Length of the tail buffer in bytes	*/
+	u8 *pu8Tail;                    /*!< Pointer to the beacon's tail buffer. Beacon's tail	starts just
+											*                              after the TIM inormation element	*/
+} tstrHostIFSetBeacon;
+
+
+
+/*!
+ *  @struct             tstrHostIFDelBeacon
+ *  @brief		Del Beacon  message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		15 July 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFDelBeacon {
+	u8 u8dummy;
+} tstrHostIFDelBeacon;
+
+/*!
+ *  @struct             tstrHostIFSetMulti
+ *  @brief		set Multicast filter Address
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Abdelrahman Sobhy
+ *  @date		30 August 2013
+ *  @version		1.0 Description
+ */
+
+typedef struct {
+	bool bIsEnabled;
+	u32 u32count;
+} tstrHostIFSetMulti;
+
+/*!
+ *  @struct             tstrHostIFDelAllSta
+ *  @brief		Deauth station message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		09 April 2014
+ *  @version		1.0 Description
+ */
+
+typedef struct {
+	u8 au8Sta_DelAllSta[MAX_NUM_STA][ETH_ALEN];
+	u8 u8Num_AssocSta;
+} tstrHostIFDelAllSta;
+
+/*!
+ *  @struct             tstrHostIFDelSta
+ *  @brief		Delete station message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		15 July 2012
+ *  @version		1.0 Description
+ */
+
+typedef struct {
+	u8 au8MacAddr[ETH_ALEN];
+} tstrHostIFDelSta;
+
+/*!
+ *  @struct             tstrTimerCb
+ *  @brief		Timer callback message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mostafa Abu Bakr
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrTimerCb {
+	void *pvUsrArg;                 /*!< Private data passed at timer start */
+} tstrTimerCb;
+
+/*!
+ *  @struct     tstrHostIfPowerMgmtParam
+ *  @brief		Power management message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		24 November 2012
+ *  @version		1.0
+ */
+typedef struct {
+
+	bool bIsEnabled;
+	u32 u32Timeout;
+} tstrHostIfPowerMgmtParam;
+
+/*!
+ *  @struct             tstrHostIFSetIPAddr
+ *  @brief		set IP Address message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Abdelrahman Sobhy
+ *  @date		30 August 2013
+ *  @version		1.0 Description
+ */
+
+typedef struct {
+	u8 *au8IPAddr;
+	u8 idx;
+} tstrHostIFSetIPAddr;
+
+/*!
+ *  @struct     tstrHostIfStaInactiveT
+ *  @brief		Get station message body
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author	    Mai Daftedar
+ *  @date		16 April 2013
+ *  @version		1.0
+ */
+typedef struct {
+	u8 mac[6];
+
+} tstrHostIfStaInactiveT;
+/**/
+/*!
+ *  @union              tuniHostIFmsgBody
+ *  @brief		Message body for the Host Interface message_q
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mostafa Abu Bakr
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef union _tuniHostIFmsgBody {
+	tstrHostIFscanAttr strHostIFscanAttr;                           /*!< Host IF Scan Request Attributes message body */
+	tstrHostIFconnectAttr strHostIFconnectAttr;     /*!< Host IF Connect Request Attributes message body */
+	tstrRcvdNetworkInfo strRcvdNetworkInfo;                 /*!< Received Asynchronous Network Info message body */
+	tstrRcvdGnrlAsyncInfo strRcvdGnrlAsyncInfo;     /*!< Received General Asynchronous Info message body */
+	tstrHostIFkeyAttr strHostIFkeyAttr;                             /*!<>*/
+	tstrHostIFCfgParamAttr strHostIFCfgParamAttr;            /*! <CFG Parameter message Body> */
+	tstrHostIFSetChan strHostIFSetChan;
+	tstrHostIFGetChan strHostIFGetChan;
+	tstrHostIFSetBeacon strHostIFSetBeacon;                 /*!< Set beacon message body */
+	tstrHostIFDelBeacon strHostIFDelBeacon;                 /*!< Del beacon message body */
+	tstrWILC_AddStaParam strAddStaParam;                    /*!< Add station message body */
+	tstrHostIFDelSta strDelStaParam;                                /*!< Del Station message body */
+	tstrWILC_AddStaParam strEditStaParam;                           /*!< Edit station message body */
+	/* tstrScanComplete		strScanComplete;		/ *Received Async. Scan Complete message body* / */
+	tstrTimerCb strTimerCb;                                                 /*!< Timer callback message body */
+	tstrHostIfPowerMgmtParam strPowerMgmtparam;     /*!< Power Management message body */
+	tstrHostIfStaInactiveT strHostIfStaInactiveT;
+	tstrHostIFSetIPAddr strHostIfSetIP;
+	tstrHostIfSetDrvHandler strHostIfSetDrvHandler;
+	tstrHostIFSetMulti strHostIfSetMulti;
+	tstrHostIfSetOperationMode strHostIfSetOperationMode;
+	tstrHostIfSetMacAddress strHostIfSetMacAddress;
+	tstrHostIfGetMacAddress strHostIfGetMacAddress;
+	tstrHostIfBASessionInfo strHostIfBASessionInfo;
+	#ifdef WILC_P2P
+	tstrHostIfRemainOnChan strHostIfRemainOnChan;
+	tstrHostIfRegisterFrame strHostIfRegisterFrame;
+	#endif
+	char *pUserData;
+	tstrHostIFDelAllSta strHostIFDelAllSta;
+} tuniHostIFmsgBody;
+
+/*!
+ *  @struct             tstrHostIFmsg
+ *  @brief		Host Interface message
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mostafa Abu Bakr
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrHostIFmsg {
+	u16 u16MsgId;                                           /*!< Message ID */
+	tuniHostIFmsgBody uniHostIFmsgBody;             /*!< Message body */
+	void *drvHandler;
+} tstrHostIFmsg;
+
+#ifdef CONNECT_DIRECT
+typedef struct _tstrWidJoinReqExt {
+	char SSID[MAX_SSID_LEN];
+	u8 u8channel;
+	u8 BSSID[6];
+} tstrWidJoinReqExt;
+#endif
+
+/*Bug4218: Parsing Join Param*/
+#ifdef WILC_PARSE_SCAN_IN_HOST
+/*Struct containg joinParam of each AP*/
+typedef struct _tstrJoinBssParam {
+	BSSTYPE_T bss_type;
+	u8 dtim_period;
+	u16 beacon_period;
+	u16 cap_info;
+	u8 au8bssid[6];
+	char ssid[MAX_SSID_LEN];
+	u8 ssidLen;
+	u8 supp_rates[MAX_RATES_SUPPORTED + 1];
+	u8 ht_capable;
+	u8 wmm_cap;
+	u8 uapsd_cap;
+	bool rsn_found;
+	u8 rsn_grp_policy;
+	u8 mode_802_11i;
+	u8 rsn_pcip_policy[3];
+	u8 rsn_auth_policy[3];
+	u8 rsn_cap[2];
+	struct _tstrJoinParam *nextJoinBss;
+	#ifdef WILC_P2P
+	u32 tsf;
+	u8 u8NoaEnbaled;
+	u8 u8OppEnable;
+	u8 u8CtWindow;
+	u8 u8Count;
+	u8 u8Index;
+	u8 au8Duration[4];
+	u8 au8Interval[4];
+	u8 au8StartTime[4];
+	#endif
+} tstrJoinBssParam;
+/*Bug4218: Parsing Join Param*/
+/*a linked list table containing needed join parameters entries for each AP found in most recent scan*/
+typedef struct _tstrBssTable {
+	u8 u8noBssEntries;
+	tstrJoinBssParam *head;
+	tstrJoinBssParam *tail;
+} tstrBssTable;
+#endif /*WILC_PARSE_SCAN_IN_HOST*/
+
+typedef enum {
+	SCAN_TIMER = 0,
+	CONNECT_TIMER	= 1,
+	SCAN_CONNECT_TIMER_FORCE_32BIT = 0xFFFFFFFF
+} tenuScanConnTimer;
+
+/*****************************************************************************/
+/*																			 */
+/*							Global Variabls	                                                                 */
+/*																			 */
+/*****************************************************************************/
+
+
+tstrWILC_WFIDrv *terminated_handle = NULL;
+tstrWILC_WFIDrv *gWFiDrvHandle = NULL;
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+bool g_obtainingIP = false;
+#endif
+u8 P2P_LISTEN_STATE;
+static struct task_struct *HostIFthreadHandler;
+static WILC_MsgQueueHandle gMsgQHostIF;
+static struct semaphore hSemHostIFthrdEnd;
+
+struct semaphore hSemDeinitDrvHandle;
+static struct semaphore hWaitResponse;
+struct semaphore hSemHostIntDeinit;
+WILC_TimerHandle g_hPeriodicRSSI;
+
+
+
+u8 gau8MulticastMacAddrList[WILC_MULTICAST_TABLE_SIZE][ETH_ALEN];
+
+#ifndef CONNECT_DIRECT
+static u8 gapu8RcvdSurveyResults[2][MAX_SURVEY_RESULT_FRAG_SIZE];
+#endif
+
+static u8 gapu8RcvdAssocResp[MAX_ASSOC_RESP_FRAME_SIZE];
+
+bool gbScanWhileConnected = false;
+
+static s8 gs8Rssi;
+static s8 gs8lnkspd;
+static u8 gu8Chnl;
+static u8 gs8SetIP[2][4];
+static u8 gs8GetIP[2][4];
+#ifdef WILC_AP_EXTERNAL_MLME
+static u32 gu32InactiveTime;
+static u8 gu8DelBcn;
+#endif
+#ifndef SIMULATION
+static u32 gu32WidConnRstHack;
+#endif
+
+/*BugID_5137*/
+u8 *gu8FlushedJoinReq;
+u8 *gu8FlushedInfoElemAsoc;
+u8 gu8Flushed11iMode;
+u8 gu8FlushedAuthType;
+u32 gu32FlushedJoinReqSize;
+u32 gu32FlushedInfoElemAsocSize;
+u32 gu8FlushedJoinReqDrvHandler;
+#define REAL_JOIN_REQ 0
+#define FLUSHED_JOIN_REQ 1
+#define FLUSHED_BYTE_POS 79     /* Position the byte indicating flushing in the flushed request */
+
+/*Bug4218: Parsing Join Param*/
+#ifdef WILC_PARSE_SCAN_IN_HOST
+/*Bug4218: Parsing Join Param*/
+static void *host_int_ParseJoinBssParam(tstrNetworkInfo *ptstrNetworkInfo);
+#endif /*WILC_PARSE_SCAN_IN_HOST*/
+
+extern void chip_sleep_manually(u32 u32SleepTime);
+extern int linux_wlan_get_num_conn_ifcs(void);
+
+/**
+ *  @brief Handle_SetChannel
+ *  @details    Sending config packet to firmware to set channel
+ *  @param[in]   tstrHostIFSetChan* pstrHostIFSetChan
+ *  @return     Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_SetChannel(void *drvHandler, tstrHostIFSetChan *pstrHostIFSetChan)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID	strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_CURRENT_CHANNEL;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = (char *)&(pstrHostIFSetChan->u8SetChan);
+	strWID.s32ValueSize = sizeof(char);
+
+	PRINT_D(HOSTINF_DBG, "Setting channel\n");
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to set channel\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+/**
+ *  @brief Handle_SetWfiDrvHandler
+ *  @details    Sending config packet to firmware to set driver handler
+ *  @param[in]   void * drvHandler,tstrHostIfSetDrvHandler* pstrHostIfSetDrvHandler
+ *  @return     Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_SetWfiDrvHandler(tstrHostIfSetDrvHandler *pstrHostIfSetDrvHandler)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID	strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)((pstrHostIfSetDrvHandler->u32Address));
+
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_SET_DRV_HANDLER;
+	strWID.enuWIDtype = WID_INT;
+	strWID.ps8WidVal = (s8 *)&(pstrHostIfSetDrvHandler->u32Address);
+	strWID.s32ValueSize = sizeof(u32);
+
+	/*Sending Cfg*/
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+
+	if ((pstrHostIfSetDrvHandler->u32Address) == (u32)NULL) {
+		up(&hSemDeinitDrvHandle);
+	}
+
+
+	if (s32Error) {
+		PRINT_ER("Failed to set driver handler\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_SetWfiAPDrvHandler
+ *  @details    Sending config packet to firmware to set driver handler
+ *  @param[in]   void * drvHandler,tstrHostIfSetDrvHandler* pstrHostIfSetDrvHandler
+ *  @return     Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_SetOperationMode(void *drvHandler, tstrHostIfSetOperationMode *pstrHostIfSetOperationMode)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID	strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_SET_OPERATION_MODE;
+	strWID.enuWIDtype = WID_INT;
+	strWID.ps8WidVal = (s8 *)&(pstrHostIfSetOperationMode->u32Mode);
+	strWID.s32ValueSize = sizeof(u32);
+
+	/*Sending Cfg*/
+	PRINT_INFO(HOSTINF_DBG, "pstrWFIDrv= %p \n", pstrWFIDrv);
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+
+	if ((pstrHostIfSetOperationMode->u32Mode) == (u32)NULL) {
+		up(&hSemDeinitDrvHandle);
+	}
+
+
+	if (s32Error) {
+		PRINT_ER("Failed to set driver handler\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief host_int_set_IPAddress
+ *  @details       Setting IP address params in message queue
+ *  @param[in]    WILC_WFIDrvHandle hWFIDrv, u8* pu8IPAddr
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 Handle_set_IPAddress(void *drvHandler, u8 *pu8IPAddr, u8 idx)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	char firmwareIPAddress[4] = {0};
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	if (pu8IPAddr[0] < 192)
+		pu8IPAddr[0] = 0;
+
+	PRINT_INFO(HOSTINF_DBG, "Indx = %d, Handling set  IP = %d.%d.%d.%d \n", idx, pu8IPAddr[0], pu8IPAddr[1], pu8IPAddr[2], pu8IPAddr[3]);
+
+	WILC_memcpy(gs8SetIP[idx], pu8IPAddr, IP_ALEN);
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_IP_ADDRESS;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = (u8 *)pu8IPAddr;
+	strWID.s32ValueSize = IP_ALEN;
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+
+
+	host_int_get_ipaddress((WILC_WFIDrvHandle)drvHandler, firmwareIPAddress, idx);
+
+	if (s32Error) {
+		PRINT_D(HOSTINF_DBG, "Failed to set IP address\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	} else {
+		PRINT_INFO(HOSTINF_DBG, "IP address set\n");
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+
+/**
+ *  @brief Handle_get_IPAddress
+ *  @details       Setting IP address params in message queue
+ *  @param[in]    WILC_WFIDrvHandle hWFIDrv, u8* pu8IPAddr
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 Handle_get_IPAddress(void *drvHandler, u8 *pu8IPAddr, u8 idx)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_IP_ADDRESS;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = (u8 *)WILC_MALLOC(IP_ALEN);
+	strWID.s32ValueSize = IP_ALEN;
+
+	s32Error = SendConfigPkt(GET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+	PRINT_INFO(HOSTINF_DBG, "%d.%d.%d.%d\n", (u8)(strWID.ps8WidVal[0]), (u8)(strWID.ps8WidVal[1]), (u8)(strWID.ps8WidVal[2]), (u8)(strWID.ps8WidVal[3]));
+
+	WILC_memcpy(gs8GetIP[idx], strWID.ps8WidVal, IP_ALEN);
+
+	/*get the value by searching the local copy*/
+	WILC_FREE(strWID.ps8WidVal);
+
+	if (WILC_memcmp(gs8GetIP[idx], gs8SetIP[idx], IP_ALEN) != 0)
+		host_int_setup_ipaddress((WILC_WFIDrvHandle)pstrWFIDrv, gs8SetIP[idx], idx);
+
+	if (s32Error != WILC_SUCCESS) {
+		PRINT_ER("Failed to get IP address\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	} else {
+		PRINT_INFO(HOSTINF_DBG, "IP address retrieved:: u8IfIdx = %d \n", idx);
+		PRINT_INFO(HOSTINF_DBG, "%d.%d.%d.%d\n", gs8GetIP[idx][0], gs8GetIP[idx][1], gs8GetIP[idx][2], gs8GetIP[idx][3]);
+		PRINT_INFO(HOSTINF_DBG, "\n");
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+
+/*BugId_5077*/
+/**
+ *  @brief Handle_SetMacAddress
+ *  @details    Setting mac address
+ *  @param[in]   void * drvHandler,tstrHostIfSetDrvHandler* pstrHostIfSetDrvHandler
+ *  @return     Error code.
+ *  @author	Amr Abdel-Moghny
+ *  @date		November 2013
+ *  @version	7.0
+ */
+static s32 Handle_SetMacAddress(void *drvHandler, tstrHostIfSetMacAddress *pstrHostIfSetMacAddress)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID	strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	u8 *mac_buf = (u8 *)WILC_MALLOC(ETH_ALEN);
+	if (mac_buf == NULL) {
+		PRINT_ER("No buffer to send mac address\n");
+		return WILC_FAIL;
+	}
+	WILC_memcpy(mac_buf, pstrHostIfSetMacAddress->u8MacAddress, ETH_ALEN);
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_MAC_ADDR;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = mac_buf;
+	strWID.s32ValueSize = ETH_ALEN;
+	PRINT_D(GENERIC_DBG, "mac addr = :%x:%x:%x:%x:%x:%x\n", strWID.ps8WidVal[0], strWID.ps8WidVal[1], strWID.ps8WidVal[2], strWID.ps8WidVal[3], strWID.ps8WidVal[4], strWID.ps8WidVal[5]);
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to set mac address\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	WILC_FREE(mac_buf);
+	return s32Error;
+}
+
+
+/*BugID_5213*/
+/**
+ *  @brief Handle_GetMacAddress
+ *  @details    Getting mac address
+ *  @param[in]   void * drvHandler,tstrHostIfSetDrvHandler* pstrHostIfSetDrvHandler
+ *  @return     Error code.
+ *  @author	Amr Abdel-Moghny
+ *  @date		JAN 2013
+ *  @version	8.0
+ */
+static s32 Handle_GetMacAddress(void *drvHandler, tstrHostIfGetMacAddress *pstrHostIfGetMacAddress)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID	strWID;
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_MAC_ADDR;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = pstrHostIfGetMacAddress->u8MacAddress;
+	strWID.s32ValueSize = ETH_ALEN;
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(GET_CFG, &strWID, 1, false, (u32)drvHandler);
+	if (s32Error) {
+		PRINT_ER("Failed to get mac address\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	up(&hWaitResponse);
+
+	return s32Error;
+}
+
+
+/**
+ *  @brief Handle_CfgParam
+ *  @details    Sending config packet to firmware to set CFG params
+ *  @param[in]   tstrHostIFCfgParamAttr* strHostIFCfgParamAttr
+ *  @return     Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_CfgParam(void *drvHandler, tstrHostIFCfgParamAttr *strHostIFCfgParamAttr)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWIDList[32];
+	u8 u8WidCnt = 0;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+
+	down(&(pstrWFIDrv->gtOsCfgValuesSem));
+
+
+	PRINT_D(HOSTINF_DBG, "Setting CFG params\n");
+
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & BSS_TYPE) {
+		/*----------------------------------------------------------*/
+		/*Input Value:	INFRASTRUCTURE = 1,							*/
+		/*				INDEPENDENT= 2,								*/
+		/*				ANY_BSS= 3									*/
+		/*----------------------------------------------------------*/
+		/* validate input then copy>> need to check value 4 and 5 */
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.bss_type < 6) {
+			strWIDList[u8WidCnt].u16WIDid = WID_BSS_TYPE;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.bss_type;
+			strWIDList[u8WidCnt].enuWIDtype = WID_CHAR;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(char);
+			pstrWFIDrv->strCfgValues.bss_type = (u8)strHostIFCfgParamAttr->pstrCfgParamVal.bss_type;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & AUTH_TYPE) {
+		/*------------------------------------------------------*/
+		/*Input Values: OPEN_SYSTEM     = 0,					*/
+		/*				SHARED_KEY      = 1,					*/
+		/*				ANY             = 2						*/
+		/*------------------------------------------------------*/
+		/*validate Possible values*/
+		if ((strHostIFCfgParamAttr->pstrCfgParamVal.auth_type) == 1 || (strHostIFCfgParamAttr->pstrCfgParamVal.auth_type) == 2 || (strHostIFCfgParamAttr->pstrCfgParamVal.auth_type) == 5) {
+			strWIDList[u8WidCnt].u16WIDid = WID_AUTH_TYPE;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.auth_type;
+			strWIDList[u8WidCnt].enuWIDtype = WID_CHAR;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(char);
+			pstrWFIDrv->strCfgValues.auth_type = (u8)strHostIFCfgParamAttr->pstrCfgParamVal.auth_type;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & AUTHEN_TIMEOUT) {
+		/* range is 1 to 65535. */
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.auth_timeout > 0 && strHostIFCfgParamAttr->pstrCfgParamVal.auth_timeout < 65536) {
+			strWIDList[u8WidCnt].u16WIDid = WID_AUTH_TIMEOUT;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.auth_timeout;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.auth_timeout = strHostIFCfgParamAttr->pstrCfgParamVal.auth_timeout;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & POWER_MANAGEMENT) {
+		/*-----------------------------------------------------------*/
+		/*Input Values:	NO_POWERSAVE     = 0,						*/
+		/*				MIN_FAST_PS      = 1,						*/
+		/*				MAX_FAST_PS      = 2,						*/
+		/*				MIN_PSPOLL_PS    = 3,						*/
+		/*				MAX_PSPOLL_PS    = 4						*/
+		/*----------------------------------------------------------*/
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.power_mgmt_mode < 5) {
+			strWIDList[u8WidCnt].u16WIDid = WID_POWER_MANAGEMENT;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.power_mgmt_mode;
+			strWIDList[u8WidCnt].enuWIDtype = WID_CHAR;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(char);
+			pstrWFIDrv->strCfgValues.power_mgmt_mode = (u8)strHostIFCfgParamAttr->pstrCfgParamVal.power_mgmt_mode;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & RETRY_SHORT) {
+		/* range from 1 to 256 */
+		if ((strHostIFCfgParamAttr->pstrCfgParamVal.short_retry_limit > 0) && (strHostIFCfgParamAttr->pstrCfgParamVal.short_retry_limit < 256))	{
+			strWIDList[u8WidCnt].u16WIDid = WID_SHORT_RETRY_LIMIT;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.short_retry_limit;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.short_retry_limit = strHostIFCfgParamAttr->pstrCfgParamVal.short_retry_limit;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & RETRY_LONG) {
+		/* range from 1 to 256 */
+		if ((strHostIFCfgParamAttr->pstrCfgParamVal.long_retry_limit > 0) && (strHostIFCfgParamAttr->pstrCfgParamVal.long_retry_limit < 256)) {
+			strWIDList[u8WidCnt].u16WIDid = WID_LONG_RETRY_LIMIT;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.long_retry_limit;
+
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.long_retry_limit = strHostIFCfgParamAttr->pstrCfgParamVal.long_retry_limit;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & FRAG_THRESHOLD) {
+
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.frag_threshold > 255 && strHostIFCfgParamAttr->pstrCfgParamVal.frag_threshold < 7937) {
+			strWIDList[u8WidCnt].u16WIDid = WID_FRAG_THRESHOLD;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.frag_threshold;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.frag_threshold = strHostIFCfgParamAttr->pstrCfgParamVal.frag_threshold;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & RTS_THRESHOLD) {
+		/* range 256 to 65535 */
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.rts_threshold > 255 && strHostIFCfgParamAttr->pstrCfgParamVal.rts_threshold < 65536)	{
+			strWIDList[u8WidCnt].u16WIDid = WID_RTS_THRESHOLD;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.rts_threshold;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.rts_threshold = strHostIFCfgParamAttr->pstrCfgParamVal.rts_threshold;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & PREAMBLE) {
+		/*-----------------------------------------------------*/
+		/*Input Values: Short= 0,								*/
+		/*				Long= 1,                                */
+		/*				Auto= 2									*/
+		/*------------------------------------------------------*/
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.preamble_type < 3) {
+			strWIDList[u8WidCnt].u16WIDid = WID_PREAMBLE;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.preamble_type;
+			strWIDList[u8WidCnt].enuWIDtype = WID_CHAR;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(char);
+			pstrWFIDrv->strCfgValues.preamble_type = strHostIFCfgParamAttr->pstrCfgParamVal.preamble_type;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & SHORT_SLOT_ALLOWED) {
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.short_slot_allowed < 2) {
+			strWIDList[u8WidCnt].u16WIDid = WID_SHORT_SLOT_ALLOWED;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.short_slot_allowed;
+			strWIDList[u8WidCnt].enuWIDtype = WID_CHAR;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(char);
+			pstrWFIDrv->strCfgValues.short_slot_allowed = (u8)strHostIFCfgParamAttr->pstrCfgParamVal.short_slot_allowed;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & TXOP_PROT_DISABLE) {
+		/*Description:	used to Disable RTS-CTS protection for TXOP burst*/
+		/*transmission when the acknowledgement policy is No-Ack or Block-Ack	*/
+		/* this information is useful for external supplicant                                   */
+		/*Input Values: 1 for enable and 0 for disable.							*/
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.txop_prot_disabled < 2) {
+			strWIDList[u8WidCnt].u16WIDid = WID_11N_TXOP_PROT_DISABLE;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.txop_prot_disabled;
+			strWIDList[u8WidCnt].enuWIDtype = WID_CHAR;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(char);
+			pstrWFIDrv->strCfgValues.txop_prot_disabled = (u8)strHostIFCfgParamAttr->pstrCfgParamVal.txop_prot_disabled;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & BEACON_INTERVAL) {
+		/* range is 1 to 65535. */
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.beacon_interval > 0 && strHostIFCfgParamAttr->pstrCfgParamVal.beacon_interval < 65536) {
+			strWIDList[u8WidCnt].u16WIDid = WID_BEACON_INTERVAL;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.beacon_interval;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.beacon_interval = strHostIFCfgParamAttr->pstrCfgParamVal.beacon_interval;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & DTIM_PERIOD) {
+		/* range is 1 to 255. */
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.dtim_period > 0 && strHostIFCfgParamAttr->pstrCfgParamVal.dtim_period < 256) {
+			strWIDList[u8WidCnt].u16WIDid = WID_DTIM_PERIOD;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.dtim_period;
+			strWIDList[u8WidCnt].enuWIDtype = WID_CHAR;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(char);
+			pstrWFIDrv->strCfgValues.dtim_period = strHostIFCfgParamAttr->pstrCfgParamVal.dtim_period;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & SITE_SURVEY) {
+		/*----------------------------------------------------------------------*/
+		/*Input Values: SITE_SURVEY_1CH    = 0, i.e.: currently set channel		*/
+		/*				SITE_SURVEY_ALL_CH = 1,									*/
+		/*				SITE_SURVEY_OFF    = 2									*/
+		/*----------------------------------------------------------------------*/
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.site_survey_enabled < 3) {
+			strWIDList[u8WidCnt].u16WIDid = WID_SITE_SURVEY;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.site_survey_enabled;
+			strWIDList[u8WidCnt].enuWIDtype = WID_CHAR;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(char);
+			pstrWFIDrv->strCfgValues.site_survey_enabled = (u8)strHostIFCfgParamAttr->pstrCfgParamVal.site_survey_enabled;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & SITE_SURVEY_SCAN_TIME) {
+		/* range is 1 to 65535. */
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.site_survey_scan_time > 0 && strHostIFCfgParamAttr->pstrCfgParamVal.site_survey_scan_time < 65536) {
+			strWIDList[u8WidCnt].u16WIDid = WID_SITE_SURVEY_SCAN_TIME;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.site_survey_scan_time;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.site_survey_scan_time = strHostIFCfgParamAttr->pstrCfgParamVal.site_survey_scan_time;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & ACTIVE_SCANTIME) {
+		/* range is 1 to 65535. */
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.active_scan_time > 0 && strHostIFCfgParamAttr->pstrCfgParamVal.active_scan_time < 65536) {
+			strWIDList[u8WidCnt].u16WIDid = WID_ACTIVE_SCAN_TIME;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.active_scan_time;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.active_scan_time = strHostIFCfgParamAttr->pstrCfgParamVal.active_scan_time;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & PASSIVE_SCANTIME) {
+		/* range is 1 to 65535. */
+		if (strHostIFCfgParamAttr->pstrCfgParamVal.passive_scan_time > 0 && strHostIFCfgParamAttr->pstrCfgParamVal.passive_scan_time < 65536) {
+			strWIDList[u8WidCnt].u16WIDid = WID_PASSIVE_SCAN_TIME;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&strHostIFCfgParamAttr->pstrCfgParamVal.passive_scan_time;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.passive_scan_time = strHostIFCfgParamAttr->pstrCfgParamVal.passive_scan_time;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	if (strHostIFCfgParamAttr->pstrCfgParamVal.u32SetCfgFlag & CURRENT_TX_RATE) {
+		CURRENT_TX_RATE_T curr_tx_rate = strHostIFCfgParamAttr->pstrCfgParamVal.curr_tx_rate;
+		/*----------------------------------------------------------------------*/
+		/*Rates:		1   2   5.5   11   6  9  12  18  24  36  48   54  Auto	*/
+		/*InputValues:	1   2     3    4   5  6   7   8   9  10  11   12  0		*/
+		/*----------------------------------------------------------------------*/
+		/* validate rate */
+		if (curr_tx_rate == AUTORATE || curr_tx_rate == MBPS_1
+		    || curr_tx_rate == MBPS_2 || curr_tx_rate == MBPS_5_5
+		    || curr_tx_rate == MBPS_11 || curr_tx_rate == MBPS_6
+		    || curr_tx_rate == MBPS_9 || curr_tx_rate == MBPS_12
+		    || curr_tx_rate == MBPS_18 || curr_tx_rate == MBPS_24
+		    || curr_tx_rate == MBPS_36 || curr_tx_rate == MBPS_48 || curr_tx_rate == MBPS_54) {
+			strWIDList[u8WidCnt].u16WIDid = WID_CURRENT_TX_RATE;
+			strWIDList[u8WidCnt].ps8WidVal = (s8 *)&curr_tx_rate;
+			strWIDList[u8WidCnt].enuWIDtype = WID_SHORT;
+			strWIDList[u8WidCnt].s32ValueSize = sizeof(u16);
+			pstrWFIDrv->strCfgValues.curr_tx_rate = (u8)curr_tx_rate;
+		} else {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+		u8WidCnt++;
+	}
+	s32Error = SendConfigPkt(SET_CFG, strWIDList, u8WidCnt, false, (u32)pstrWFIDrv);
+
+	if (s32Error) {
+		PRINT_ER("Error in setting CFG params\n");
+
+	}
+	WILC_CATCH(s32Error)
+	{
+	}
+	up(&(pstrWFIDrv->gtOsCfgValuesSem));
+	return s32Error;
+}
+
+
+/**
+ *  @brief Handle_wait_msg_q_empty
+ *  @details       this should be the last msg and then the msg Q becomes idle
+ *  @param[in]    tstrHostIFscanAttr* pstrHostIFscanAttr
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_wait_msg_q_empty(void)
+{
+	s32 s32Error = WILC_SUCCESS;
+	g_wilc_initialized = 0;
+	up(&hWaitResponse);
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_Scan
+ *  @details       Sending config packet to firmware to set the scan params
+ *  @param[in]    tstrHostIFscanAttr* pstrHostIFscanAttr
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_Scan(void *drvHandler, tstrHostIFscanAttr *pstrHostIFscanAttr)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWIDList[5];
+	u32 u32WidsCount = 0;
+	u32 i;
+	u8 *pu8Buffer;
+	u8 valuesize = 0;
+	u8 *pu8HdnNtwrksWidVal = NULL;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *) drvHandler;
+
+	PRINT_D(HOSTINF_DBG, "Setting SCAN params\n");
+	PRINT_D(HOSTINF_DBG, "Scanning: In [%d] state \n", pstrWFIDrv->enuHostIFstate);
+
+	pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult = pstrHostIFscanAttr->pfScanResult;
+	pstrWFIDrv->strWILC_UsrScanReq.u32UserScanPvoid = pstrHostIFscanAttr->pvUserArg;
+
+	#ifdef WILC_P2P
+	#endif
+
+	if ((pstrWFIDrv->enuHostIFstate >= HOST_IF_SCANNING) && (pstrWFIDrv->enuHostIFstate < HOST_IF_CONNECTED)) {
+		/* here we either in HOST_IF_SCANNING, HOST_IF_WAITING_CONN_REQ or HOST_IF_WAITING_CONN_RESP */
+		PRINT_D(GENERIC_DBG, "Don't scan we are already in [%d] state\n", pstrWFIDrv->enuHostIFstate);
+		WILC_ERRORREPORT(s32Error, WILC_BUSY);
+	}
+
+	#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+	if (g_obtainingIP || connecting) {
+		PRINT_D(GENERIC_DBG, "[handle_scan]: Don't do obss scan until IP adresss is obtained\n");
+		WILC_ERRORREPORT(s32Error, WILC_BUSY);
+	}
+	#endif
+
+	PRINT_D(HOSTINF_DBG, "Setting SCAN params\n");
+
+
+	pstrWFIDrv->strWILC_UsrScanReq.u32RcvdChCount = 0;
+
+	/*BugID_4189*/
+	strWIDList[u32WidsCount].u16WIDid = (u16)WID_SSID_PROBE_REQ;
+	strWIDList[u32WidsCount].enuWIDtype = WID_STR;
+
+	for (i = 0; i < pstrHostIFscanAttr->strHiddenNetwork.u8ssidnum; i++) {
+		valuesize += ((pstrHostIFscanAttr->strHiddenNetwork.pstrHiddenNetworkInfo[i].u8ssidlen) + 1);
+	}
+	pu8HdnNtwrksWidVal = WILC_MALLOC(valuesize + 1);
+	strWIDList[u32WidsCount].ps8WidVal = pu8HdnNtwrksWidVal;
+	if (strWIDList[u32WidsCount].ps8WidVal != NULL) {
+		pu8Buffer = strWIDList[u32WidsCount].ps8WidVal;
+
+		*pu8Buffer++ = pstrHostIFscanAttr->strHiddenNetwork.u8ssidnum;
+
+		PRINT_D(HOSTINF_DBG, "In Handle_ProbeRequest number of ssid %d\n", pstrHostIFscanAttr->strHiddenNetwork.u8ssidnum);
+
+		for (i = 0; i < pstrHostIFscanAttr->strHiddenNetwork.u8ssidnum; i++) {
+			*pu8Buffer++ = pstrHostIFscanAttr->strHiddenNetwork.pstrHiddenNetworkInfo[i].u8ssidlen;
+			WILC_memcpy(pu8Buffer, pstrHostIFscanAttr->strHiddenNetwork.pstrHiddenNetworkInfo[i].pu8ssid, pstrHostIFscanAttr->strHiddenNetwork.pstrHiddenNetworkInfo[i].u8ssidlen);
+			pu8Buffer += pstrHostIFscanAttr->strHiddenNetwork.pstrHiddenNetworkInfo[i].u8ssidlen;
+		}
+
+
+
+		strWIDList[u32WidsCount].s32ValueSize =  (s32)(valuesize + 1);
+		u32WidsCount++;
+	}
+
+	/*filling cfg param array*/
+
+	/* if((pstrHostIFscanAttr->pu8IEs != NULL) && (pstrHostIFscanAttr->IEsLen != 0)) */
+	{
+		/* IEs to be inserted in Probe Request */
+		strWIDList[u32WidsCount].u16WIDid = WID_INFO_ELEMENT_PROBE;
+		strWIDList[u32WidsCount].enuWIDtype = WID_BIN_DATA;
+		strWIDList[u32WidsCount].ps8WidVal = pstrHostIFscanAttr->pu8IEs;
+		strWIDList[u32WidsCount].s32ValueSize = pstrHostIFscanAttr->IEsLen;
+		u32WidsCount++;
+	}
+
+	/*Scan Type*/
+	strWIDList[u32WidsCount].u16WIDid = WID_SCAN_TYPE;
+	strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrHostIFscanAttr->u8ScanType));
+	u32WidsCount++;
+
+	/*list of channels to be scanned*/
+	strWIDList[u32WidsCount].u16WIDid = WID_SCAN_CHANNEL_LIST;
+	strWIDList[u32WidsCount].enuWIDtype = WID_BIN_DATA;
+
+	/* Bug 4648: Convert channel numbers to start from 0 not 1. */
+	if (pstrHostIFscanAttr->pu8ChnlFreqList != NULL && pstrHostIFscanAttr->u8ChnlListLen > 0) {
+		int i;
+
+		for (i = 0; i < pstrHostIFscanAttr->u8ChnlListLen; i++)	{
+			if (pstrHostIFscanAttr->pu8ChnlFreqList[i] > 0) {
+				pstrHostIFscanAttr->pu8ChnlFreqList[i] = pstrHostIFscanAttr->pu8ChnlFreqList[i] - 1;
+			}
+		}
+	}
+
+	strWIDList[u32WidsCount].ps8WidVal = pstrHostIFscanAttr->pu8ChnlFreqList;
+	strWIDList[u32WidsCount].s32ValueSize = pstrHostIFscanAttr->u8ChnlListLen;
+	u32WidsCount++;
+
+	/*Scan Request*/
+	strWIDList[u32WidsCount].u16WIDid = WID_START_SCAN_REQ;
+	strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrHostIFscanAttr->u8ScanSource));
+	u32WidsCount++;
+
+	/*keep the state as is , no need to change it*/
+	/* gWFiDrvHandle->enuHostIFstate = HOST_IF_SCANNING; */
+
+	if (pstrWFIDrv->enuHostIFstate == HOST_IF_CONNECTED) {
+		gbScanWhileConnected = true;
+	} else if (pstrWFIDrv->enuHostIFstate == HOST_IF_IDLE)	  {
+		gbScanWhileConnected = false;
+	}
+
+	s32Error = SendConfigPkt(SET_CFG, strWIDList, u32WidsCount, false, (u32)pstrWFIDrv);
+
+	if (s32Error) {
+		PRINT_ER("Failed to send scan paramters config packet\n");
+		WILC_ERRORREPORT(s32Error, s32Error);
+	} else {
+		PRINT_D(HOSTINF_DBG, "Successfully sent SCAN params config packet\n");
+	}
+
+	WILC_CATCH(s32Error)
+	{
+		WILC_TimerStop(&(pstrWFIDrv->hScanTimer), NULL);
+		/*if there is an ongoing scan request*/
+		Handle_ScanDone(drvHandler, SCAN_EVENT_ABORTED);
+	}
+
+	/* Deallocate pstrHostIFscanAttr->u8ChnlListLen which was prevoisuly allocated by the sending thread */
+	if (pstrHostIFscanAttr->pu8ChnlFreqList != NULL) {
+		WILC_FREE(pstrHostIFscanAttr->pu8ChnlFreqList);
+		pstrHostIFscanAttr->pu8ChnlFreqList = NULL;
+	}
+
+	/* Deallocate pstrHostIFscanAttr->pu8IEs which was previously allocated by the sending thread */
+	if (pstrHostIFscanAttr->pu8IEs != NULL)	{
+		WILC_FREE(pstrHostIFscanAttr->pu8IEs);
+		pstrHostIFscanAttr->pu8IEs = NULL;
+	}
+	if (pstrHostIFscanAttr->strHiddenNetwork.pstrHiddenNetworkInfo != NULL)	{
+		WILC_FREE(pstrHostIFscanAttr->strHiddenNetwork.pstrHiddenNetworkInfo);
+		pstrHostIFscanAttr->strHiddenNetwork.pstrHiddenNetworkInfo = NULL;
+	}
+
+	/* Deallocate pstrHostIFscanAttr->u8ChnlListLen which was prevoisuly allocated by the sending thread */
+	if (pstrHostIFscanAttr->pu8ChnlFreqList != NULL) {
+		WILC_FREE(pstrHostIFscanAttr->pu8ChnlFreqList);
+		pstrHostIFscanAttr->pu8ChnlFreqList = NULL;
+	}
+
+	if (pu8HdnNtwrksWidVal != NULL)	{
+		WILC_FREE(pu8HdnNtwrksWidVal);
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_ScanDone
+ *  @details       Call scan notification callback function
+ *  @param[in]    NONE
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_ScanDone(void *drvHandler, tenuScanEvent enuEvent)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+
+	u8 u8abort_running_scan;
+	tstrWID strWID;
+
+
+	PRINT_D(HOSTINF_DBG, "in Handle_ScanDone()\n");
+
+	/*BugID_4978*/
+	/*Ask FW to abort the running scan, if any*/
+	if (enuEvent == SCAN_EVENT_ABORTED) {
+		PRINT_D(GENERIC_DBG, "Abort running scan\n");
+		u8abort_running_scan = 1;
+		strWID.u16WIDid	= (u16)WID_ABORT_RUNNING_SCAN;
+		strWID.enuWIDtype	= WID_CHAR;
+		strWID.ps8WidVal = (s8 *)&u8abort_running_scan;
+		strWID.s32ValueSize = sizeof(char);
+
+		/*Sending Cfg*/
+		s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+		if (s32Error != WILC_SUCCESS) {
+			PRINT_ER("Failed to set abort running scan\n");
+			WILC_ERRORREPORT(s32Error, WILC_FAIL);
+		}
+		WILC_CATCH(s32Error)
+		{
+		}
+	}
+
+	if (pstrWFIDrv == NULL)	{
+		PRINT_ER("Driver handler is NULL\n");
+		return s32Error;
+	}
+
+	/*if there is an ongoing scan request*/
+	if (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult) {
+		pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult(enuEvent, NULL,
+								pstrWFIDrv->strWILC_UsrScanReq.u32UserScanPvoid, NULL);
+		/*delete current scan request*/
+		pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult = NULL;
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_Connect
+ *  @details       Sending config packet to firmware to starting connection
+ *  @param[in]    tstrHostIFconnectAttr* pstrHostIFconnectAttr
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+u8 u8ConnectedSSID[6] = {0};
+static s32 Handle_Connect(void *drvHandler, tstrHostIFconnectAttr *pstrHostIFconnectAttr)
+{
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *) drvHandler;
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWIDList[8];
+	u32 u32WidsCount = 0, dummyval = 0;
+	/* char passphrase[] = "12345678"; */
+	#ifndef CONNECT_DIRECT
+	s32 s32Err = WILC_SUCCESS;
+	u32 i;
+	u8 u8bssDscListIndex;
+	wid_site_survey_reslts_s *pstrSurveyResults = NULL;
+	#else
+	u8 *pu8CurrByte = NULL;
+	/*Bug4218: Parsing Join Param*/
+	#ifdef WILC_PARSE_SCAN_IN_HOST
+	tstrJoinBssParam *ptstrJoinBssParam;
+	#endif /*WILC_PARSE_SCAN_IN_HOST*/
+
+	#endif
+
+	PRINT_D(GENERIC_DBG, "Handling connect request\n");
+
+	#ifndef CONNECT_DIRECT
+	WILC_memset(gapu8RcvdSurveyResults[0], 0, MAX_SURVEY_RESULT_FRAG_SIZE);
+	WILC_memset(gapu8RcvdSurveyResults[1], 0, MAX_SURVEY_RESULT_FRAG_SIZE);
+
+
+	PRINT_D(HOSTINF_DBG, "Getting site survey results\n");
+	s32Err = host_int_get_site_survey_results((WILC_WFIDrvHandle)pstrWFIDrv,
+						  gapu8RcvdSurveyResults,
+						  MAX_SURVEY_RESULT_FRAG_SIZE);
+	if (s32Err) {
+		PRINT_ER("Failed to get site survey results\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+
+	}
+	s32Err = ParseSurveyResults(gapu8RcvdSurveyResults, &pstrSurveyResults,
+				    &pstrWFIDrv->u32SurveyResultsCount);
+
+
+	if (s32Err == WILC_SUCCESS) {
+		/* use the parsed info in pstrSurveyResults, then deallocate it */
+		PRINT_D(HOSTINF_DBG, "Copying site survey results in global structure, then deallocate\n");
+		for (i = 0; i < pstrWFIDrv->u32SurveyResultsCount; i++)	{
+			WILC_memcpy(&pstrWFIDrv->astrSurveyResults[i], &pstrSurveyResults[i],
+				    sizeof(wid_site_survey_reslts_s));
+		}
+
+		DeallocateSurveyResults(pstrSurveyResults);
+	} else {
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+		PRINT_ER("ParseSurveyResults() Error(%d) \n", s32Err);
+	}
+
+
+	for (i = 0; i < pstrWFIDrv->u32SurveyResultsCount; i++)	{
+		if (WILC_memcmp(pstrWFIDrv->astrSurveyResults[i].SSID,
+				pstrHostIFconnectAttr->pu8ssid,
+				pstrHostIFconnectAttr->ssidLen) == 0) {
+			PRINT_INFO(HOSTINF_DBG, "Network with required SSID is found %s\n", pstrHostIFconnectAttr->pu8ssid);
+			if (pstrHostIFconnectAttr->pu8bssid == NULL) {
+				/* BSSID is not passed from the user, so decision of matching
+				 * is done by SSID only */
+				PRINT_INFO(HOSTINF_DBG, "BSSID is not passed from the user\n");
+				break;
+			} else {
+				/* BSSID is also passed from the user, so decision of matching
+				 * should consider also this passed BSSID */
+
+				if (WILC_memcmp(pstrWFIDrv->astrSurveyResults[i].BSSID,
+						pstrHostIFconnectAttr->pu8bssid,
+						6) == 0) {
+					PRINT_INFO(HOSTINF_DBG, "BSSID is passed from the user and matched\n");
+					break;
+				}
+			}
+		}
+	}
+
+	if (i < pstrWFIDrv->u32SurveyResultsCount) {
+		u8bssDscListIndex = i;
+
+		PRINT_INFO(HOSTINF_DBG, "Connecting to network of Bss Idx %d and SSID %s and channel %d \n",
+			   u8bssDscListIndex, pstrWFIDrv->astrSurveyResults[u8bssDscListIndex].SSID,
+			   pstrWFIDrv->astrSurveyResults[u8bssDscListIndex].Channel);
+
+		PRINT_INFO(HOSTINF_DBG, "Saving connection parameters in global structure\n");
+
+		if (pstrHostIFconnectAttr->pu8bssid != NULL) {
+			pstrWFIDrv->strWILC_UsrConnReq.pu8bssid = (u8 *)WILC_MALLOC(6);
+			WILC_memcpy(pstrWFIDrv->strWILC_UsrConnReq.pu8bssid, pstrHostIFconnectAttr->pu8bssid, 6);
+		}
+
+		pstrWFIDrv->strWILC_UsrConnReq.ssidLen = pstrHostIFconnectAttr->ssidLen;
+		if (pstrHostIFconnectAttr->pu8ssid != NULL) {
+			pstrWFIDrv->strWILC_UsrConnReq.pu8ssid = (u8 *)WILC_MALLOC(pstrHostIFconnectAttr->ssidLen + 1);
+			WILC_memcpy(pstrWFIDrv->strWILC_UsrConnReq.pu8ssid, pstrHostIFconnectAttr->pu8ssid,
+				    pstrHostIFconnectAttr->ssidLen);
+			pstrWFIDrv->strWILC_UsrConnReq.pu8ssid[pstrHostIFconnectAttr->ssidLen] = '\0';
+		}
+
+		pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen = pstrHostIFconnectAttr->IEsLen;
+		if (pstrHostIFconnectAttr->pu8IEs != NULL) {
+			pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs = (u8 *)WILC_MALLOC(pstrHostIFconnectAttr->IEsLen);
+			WILC_memcpy(pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs, pstrHostIFconnectAttr->pu8IEs,
+				    pstrHostIFconnectAttr->IEsLen);
+		}
+
+		pstrWFIDrv->strWILC_UsrConnReq.u8security = pstrHostIFconnectAttr->u8security;
+		pstrWFIDrv->strWILC_UsrConnReq.tenuAuth_type = pstrHostIFconnectAttr->tenuAuth_type;
+		pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult = pstrHostIFconnectAttr->pfConnectResult;
+		pstrWFIDrv->strWILC_UsrConnReq.u32UserConnectPvoid = pstrHostIFconnectAttr->pvUserArg;
+
+
+		/* if((gWFiDrvHandle->strWILC_UsrConnReq.pu8ConnReqIEs != NULL) && */
+		/* (gWFiDrvHandle->strWILC_UsrConnReq.ConnReqIEsLen != 0)) */
+		{
+			/* IEs to be inserted in Association Request */
+			strWIDList[u32WidsCount].u16WIDid = WID_INFO_ELEMENT_ASSOCIATE;
+			strWIDList[u32WidsCount].enuWIDtype = WID_BIN_DATA;
+			strWIDList[u32WidsCount].ps8WidVal = pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs;
+			strWIDList[u32WidsCount].s32ValueSize = pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen;
+			u32WidsCount++;
+		}
+		strWIDList[u32WidsCount].u16WIDid = (u16)WID_11I_MODE;
+		strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+		strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+		strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrWFIDrv->strWILC_UsrConnReq.u8security));
+		u32WidsCount++;
+
+		PRINT_INFO(HOSTINF_DBG, "Encrypt Mode = %x\n", pstrWFIDrv->strWILC_UsrConnReq.u8security);
+
+		strWIDList[u32WidsCount].u16WIDid = (u16)WID_AUTH_TYPE;
+		strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+		strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+		strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&pstrWFIDrv->strWILC_UsrConnReq.tenuAuth_type);
+		u32WidsCount++;
+
+		PRINT_INFO(HOSTINF_DBG, "Authentication Type = %x\n", pstrWFIDrv->strWILC_UsrConnReq.tenuAuth_type);
+		/*
+		 * strWIDList[u32WidsCount].u16WIDid = (u16)WID_11I_PSK;
+		 * strWIDList[u32WidsCount].enuWIDtype = WID_STR;
+		 * strWIDList[u32WidsCount].s32ValueSize = sizeof(passphrase);
+		 * strWIDList[u32WidsCount].ps8WidVal = (s8*)(passphrase);
+		 * u32WidsCount++;
+		 */
+
+		strWIDList[u32WidsCount].u16WIDid = (u16)WID_JOIN_REQ;
+		strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+		strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+		strWIDList[u32WidsCount].ps8WidVal = (s8 *)&u8bssDscListIndex;
+		u32WidsCount++;
+
+		#ifndef SIMULATION
+		/* A temporary workaround to avoid handling the misleading MAC_DISCONNECTED raised from the
+		 *   firmware at chip reset when processing the WIDs of the Connect Request.
+		 *   (This workaround should be removed in the future when the Chip reset of the Connect WIDs is disabled) */
+		/* ////////////////////// */
+		gu32WidConnRstHack = 0;
+		/* ////////////////////// */
+		#endif
+
+		s32Error = SendConfigPkt(SET_CFG, strWIDList, u32WidsCount, false, (u32)pstrWFIDrv);
+		if (s32Error) {
+			PRINT_ER("Handle_Connect()] failed to send config packet\n");
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+		} else {
+			pstrWFIDrv->enuHostIFstate = HOST_IF_WAITING_CONN_RESP;
+		}
+
+	} else {
+		PRINT_ER("Required BSSID not found\n");
+		WILC_ERRORREPORT(s32Error, WILC_NOT_FOUND);
+	}
+
+	#else
+
+	/* if we try to connect to an already connected AP then discard the request */
+
+	if (WILC_memcmp(pstrHostIFconnectAttr->pu8bssid, u8ConnectedSSID, ETH_ALEN) == 0) {
+
+		s32Error = WILC_SUCCESS;
+		PRINT_ER("Trying to connect to an already connected AP, Discard connect request\n");
+		return s32Error;
+	}
+
+	PRINT_INFO(HOSTINF_DBG, "Saving connection parameters in global structure\n");
+
+	/*Bug4218: Parsing Join Param*/
+	#ifdef WILC_PARSE_SCAN_IN_HOST
+	ptstrJoinBssParam = (tstrJoinBssParam *)pstrHostIFconnectAttr->pJoinParams;
+	if (ptstrJoinBssParam == NULL) {
+		PRINT_ER("Required BSSID not found\n");
+		WILC_ERRORREPORT(s32Error, WILC_NOT_FOUND);
+	}
+	#endif /*WILC_PARSE_SCAN_IN_HOST*/
+
+	if (pstrHostIFconnectAttr->pu8bssid != NULL) {
+		pstrWFIDrv->strWILC_UsrConnReq.pu8bssid = (u8 *)WILC_MALLOC(6);
+		WILC_memcpy(pstrWFIDrv->strWILC_UsrConnReq.pu8bssid, pstrHostIFconnectAttr->pu8bssid, 6);
+	}
+
+	pstrWFIDrv->strWILC_UsrConnReq.ssidLen = pstrHostIFconnectAttr->ssidLen;
+	if (pstrHostIFconnectAttr->pu8ssid != NULL) {
+		pstrWFIDrv->strWILC_UsrConnReq.pu8ssid = (u8 *)WILC_MALLOC(pstrHostIFconnectAttr->ssidLen + 1);
+		WILC_memcpy(pstrWFIDrv->strWILC_UsrConnReq.pu8ssid, pstrHostIFconnectAttr->pu8ssid,
+			    pstrHostIFconnectAttr->ssidLen);
+		pstrWFIDrv->strWILC_UsrConnReq.pu8ssid[pstrHostIFconnectAttr->ssidLen] = '\0';
+	}
+
+	pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen = pstrHostIFconnectAttr->IEsLen;
+	if (pstrHostIFconnectAttr->pu8IEs != NULL) {
+		pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs = (u8 *)WILC_MALLOC(pstrHostIFconnectAttr->IEsLen);
+		WILC_memcpy(pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs, pstrHostIFconnectAttr->pu8IEs,
+			    pstrHostIFconnectAttr->IEsLen);
+	}
+
+	pstrWFIDrv->strWILC_UsrConnReq.u8security = pstrHostIFconnectAttr->u8security;
+	pstrWFIDrv->strWILC_UsrConnReq.tenuAuth_type = pstrHostIFconnectAttr->tenuAuth_type;
+	pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult = pstrHostIFconnectAttr->pfConnectResult;
+	pstrWFIDrv->strWILC_UsrConnReq.u32UserConnectPvoid = pstrHostIFconnectAttr->pvUserArg;
+
+	strWIDList[u32WidsCount].u16WIDid = WID_SUCCESS_FRAME_COUNT;
+	strWIDList[u32WidsCount].enuWIDtype = WID_INT;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(u32);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(dummyval));
+	u32WidsCount++;
+
+	strWIDList[u32WidsCount].u16WIDid = WID_RECEIVED_FRAGMENT_COUNT;
+	strWIDList[u32WidsCount].enuWIDtype = WID_INT;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(u32);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(dummyval));
+	u32WidsCount++;
+
+	strWIDList[u32WidsCount].u16WIDid = WID_FAILED_COUNT;
+	strWIDList[u32WidsCount].enuWIDtype = WID_INT;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(u32);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(dummyval));
+	u32WidsCount++;
+
+	/* if((gWFiDrvHandle->strWILC_UsrConnReq.pu8ConnReqIEs != NULL) && */
+	/* (gWFiDrvHandle->strWILC_UsrConnReq.ConnReqIEsLen != 0)) */
+	{
+		/* IEs to be inserted in Association Request */
+		strWIDList[u32WidsCount].u16WIDid = WID_INFO_ELEMENT_ASSOCIATE;
+		strWIDList[u32WidsCount].enuWIDtype = WID_BIN_DATA;
+		strWIDList[u32WidsCount].ps8WidVal = pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs;
+		strWIDList[u32WidsCount].s32ValueSize = pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen;
+		u32WidsCount++;
+
+		/*BugID_5137*/
+		if (WILC_memcmp("DIRECT-", pstrHostIFconnectAttr->pu8ssid, 7)) {
+
+			gu32FlushedInfoElemAsocSize = pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen;
+			gu8FlushedInfoElemAsoc =  WILC_MALLOC(gu32FlushedInfoElemAsocSize);
+			memcpy(gu8FlushedInfoElemAsoc, pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs,
+			       gu32FlushedInfoElemAsocSize);
+		}
+	}
+	strWIDList[u32WidsCount].u16WIDid = (u16)WID_11I_MODE;
+	strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrWFIDrv->strWILC_UsrConnReq.u8security));
+	u32WidsCount++;
+
+	/*BugID_5137*/
+	if (WILC_memcmp("DIRECT-", pstrHostIFconnectAttr->pu8ssid, 7))
+		gu8Flushed11iMode = pstrWFIDrv->strWILC_UsrConnReq.u8security;
+
+	PRINT_INFO(HOSTINF_DBG, "Encrypt Mode = %x\n", pstrWFIDrv->strWILC_UsrConnReq.u8security);
+
+
+	strWIDList[u32WidsCount].u16WIDid = (u16)WID_AUTH_TYPE;
+	strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&pstrWFIDrv->strWILC_UsrConnReq.tenuAuth_type);
+	u32WidsCount++;
+
+	/*BugID_5137*/
+	if (WILC_memcmp("DIRECT-", pstrHostIFconnectAttr->pu8ssid, 7))
+		gu8FlushedAuthType = (u8)pstrWFIDrv->strWILC_UsrConnReq.tenuAuth_type;
+
+	PRINT_INFO(HOSTINF_DBG, "Authentication Type = %x\n", pstrWFIDrv->strWILC_UsrConnReq.tenuAuth_type);
+	/*
+	 * strWIDList[u32WidsCount].u16WIDid = (u16)WID_11I_PSK;
+	 * strWIDList[u32WidsCount].enuWIDtype = WID_STR;
+	 * strWIDList[u32WidsCount].s32ValueSize = sizeof(passphrase);
+	 * strWIDList[u32WidsCount].ps8WidVal = (s8*)(passphrase);
+	 * u32WidsCount++;
+	 */
+
+	PRINT_D(HOSTINF_DBG, "Connecting to network of SSID %s on channel %d\n",
+		pstrWFIDrv->strWILC_UsrConnReq.pu8ssid, pstrHostIFconnectAttr->u8channel);
+
+
+#ifndef WILC_PARSE_SCAN_IN_HOST
+	strWIDList[u32WidsCount].u16WIDid = (u16)WID_JOIN_REQ_EXTENDED;
+	strWIDList[u32WidsCount].enuWIDtype = WID_STR;
+	strWIDList[u32WidsCount].s32ValueSize = MAX_SSID_LEN + 7;
+	strWIDList[u32WidsCount].ps8WidVal = WILC_MALLOC(strWIDList[u32WidsCount].s32ValueSize);
+
+	if (strWIDList[u32WidsCount].ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWIDList[u32WidsCount].ps8WidVal;
+
+	if (pstrHostIFconnectAttr->pu8ssid != NULL) {
+		WILC_memcpy(pu8CurrByte, pstrHostIFconnectAttr->pu8ssid, pstrHostIFconnectAttr->ssidLen);
+		pu8CurrByte[pstrHostIFconnectAttr->ssidLen] = '\0';
+	}
+	pu8CurrByte += MAX_SSID_LEN;
+	if ((pstrHostIFconnectAttr->u8channel >= 1) && (pstrHostIFconnectAttr->u8channel <= 14)) {
+		*(pu8CurrByte++) = pstrHostIFconnectAttr->u8channel;
+	} else {
+		PRINT_ER("Channel out of range\n");
+		*(pu8CurrByte++) = 0xFF;
+	}
+	if (pstrHostIFconnectAttr->pu8bssid != NULL) {
+		WILC_memcpy(pu8CurrByte, pstrHostIFconnectAttr->pu8bssid, 6);
+	}
+	pu8CurrByte += 6;
+
+	/* keep the buffer at the start of the allocated pointer to use it with the free*/
+	pu8CurrByte = strWIDList[u32WidsCount].ps8WidVal;
+
+	#else
+
+	strWIDList[u32WidsCount].u16WIDid = (u16)WID_JOIN_REQ_EXTENDED;
+	strWIDList[u32WidsCount].enuWIDtype = WID_STR;
+
+	/*Sending NoA attributes during connection*/
+	strWIDList[u32WidsCount].s32ValueSize = 112; /* 79; */
+	strWIDList[u32WidsCount].ps8WidVal = WILC_MALLOC(strWIDList[u32WidsCount].s32ValueSize);
+
+	/*BugID_5137*/
+	if (WILC_memcmp("DIRECT-", pstrHostIFconnectAttr->pu8ssid, 7)) {
+		gu32FlushedJoinReqSize = strWIDList[u32WidsCount].s32ValueSize;
+		gu8FlushedJoinReq = WILC_MALLOC(gu32FlushedJoinReqSize);
+	}
+	if (strWIDList[u32WidsCount].ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWIDList[u32WidsCount].ps8WidVal;
+
+
+	if (pstrHostIFconnectAttr->pu8ssid != NULL) {
+		WILC_memcpy(pu8CurrByte, pstrHostIFconnectAttr->pu8ssid, pstrHostIFconnectAttr->ssidLen);
+		pu8CurrByte[pstrHostIFconnectAttr->ssidLen] = '\0';
+	}
+	pu8CurrByte += MAX_SSID_LEN;
+
+	/* BSS type*/
+	*(pu8CurrByte++) = INFRASTRUCTURE;
+	/* Channel*/
+	if ((pstrHostIFconnectAttr->u8channel >= 1) && (pstrHostIFconnectAttr->u8channel <= 14)) {
+		*(pu8CurrByte++) = pstrHostIFconnectAttr->u8channel;
+	} else {
+		PRINT_ER("Channel out of range\n");
+		*(pu8CurrByte++) = 0xFF;
+	}
+	/* Cap Info*/
+	*(pu8CurrByte++)  = (ptstrJoinBssParam->cap_info) & 0xFF;
+	*(pu8CurrByte++)  = ((ptstrJoinBssParam->cap_info) >> 8) & 0xFF;
+	PRINT_D(HOSTINF_DBG, "* Cap Info %0x*\n", (*(pu8CurrByte - 2) | ((*(pu8CurrByte - 1)) << 8)));
+
+	/* sa*/
+	if (pstrHostIFconnectAttr->pu8bssid != NULL) {
+		WILC_memcpy(pu8CurrByte, pstrHostIFconnectAttr->pu8bssid, 6);
+	}
+	pu8CurrByte += 6;
+
+	/* bssid*/
+	if (pstrHostIFconnectAttr->pu8bssid != NULL) {
+		WILC_memcpy(pu8CurrByte, pstrHostIFconnectAttr->pu8bssid, 6);
+	}
+	pu8CurrByte += 6;
+
+	/* Beacon Period*/
+	*(pu8CurrByte++)  = (ptstrJoinBssParam->beacon_period) & 0xFF;
+	*(pu8CurrByte++)  = ((ptstrJoinBssParam->beacon_period) >> 8) & 0xFF;
+	PRINT_D(HOSTINF_DBG, "* Beacon Period %d*\n", (*(pu8CurrByte - 2) | ((*(pu8CurrByte - 1)) << 8)));
+	/* DTIM Period*/
+	*(pu8CurrByte++)  =  ptstrJoinBssParam->dtim_period;
+	PRINT_D(HOSTINF_DBG, "* DTIM Period %d*\n", (*(pu8CurrByte - 1)));
+	/* Supported rates*/
+	WILC_memcpy(pu8CurrByte, ptstrJoinBssParam->supp_rates, MAX_RATES_SUPPORTED + 1);
+	pu8CurrByte += (MAX_RATES_SUPPORTED + 1);
+
+	/* wmm cap*/
+	*(pu8CurrByte++)  =  ptstrJoinBssParam->wmm_cap;
+	PRINT_D(HOSTINF_DBG, "* wmm cap%d*\n", (*(pu8CurrByte - 1)));
+	/* uapsd cap*/
+	*(pu8CurrByte++)  = ptstrJoinBssParam->uapsd_cap;
+
+	/* ht cap*/
+	*(pu8CurrByte++)  = ptstrJoinBssParam->ht_capable;
+	/* copy this information to the user request */
+	pstrWFIDrv->strWILC_UsrConnReq.IsHTCapable = ptstrJoinBssParam->ht_capable;
+
+	/* rsn found*/
+	*(pu8CurrByte++)  =  ptstrJoinBssParam->rsn_found;
+	PRINT_D(HOSTINF_DBG, "* rsn found %d*\n", *(pu8CurrByte - 1));
+	/* rsn group policy*/
+	*(pu8CurrByte++)  =  ptstrJoinBssParam->rsn_grp_policy;
+	PRINT_D(HOSTINF_DBG, "* rsn group policy %0x*\n", (*(pu8CurrByte - 1)));
+	/* mode_802_11i*/
+	*(pu8CurrByte++) =  ptstrJoinBssParam->mode_802_11i;
+	PRINT_D(HOSTINF_DBG, "* mode_802_11i %d*\n", (*(pu8CurrByte - 1)));
+	/* rsn pcip policy*/
+	WILC_memcpy(pu8CurrByte, ptstrJoinBssParam->rsn_pcip_policy, sizeof(ptstrJoinBssParam->rsn_pcip_policy));
+	pu8CurrByte += sizeof(ptstrJoinBssParam->rsn_pcip_policy);
+
+	/* rsn auth policy*/
+	WILC_memcpy(pu8CurrByte, ptstrJoinBssParam->rsn_auth_policy, sizeof(ptstrJoinBssParam->rsn_auth_policy));
+	pu8CurrByte += sizeof(ptstrJoinBssParam->rsn_auth_policy);
+
+	/* rsn auth policy*/
+	WILC_memcpy(pu8CurrByte, ptstrJoinBssParam->rsn_cap, sizeof(ptstrJoinBssParam->rsn_cap));
+	pu8CurrByte += sizeof(ptstrJoinBssParam->rsn_cap);
+
+	/*BugID_5137*/
+	*(pu8CurrByte++) = REAL_JOIN_REQ;
+
+		#ifdef WILC_P2P
+	*(pu8CurrByte++) = ptstrJoinBssParam->u8NoaEnbaled;
+	if (ptstrJoinBssParam->u8NoaEnbaled) {
+		PRINT_D(HOSTINF_DBG, "NOA present\n");
+
+		*(pu8CurrByte++) = (ptstrJoinBssParam->tsf) & 0xFF;
+		*(pu8CurrByte++) = ((ptstrJoinBssParam->tsf) >> 8) & 0xFF;
+		*(pu8CurrByte++) = ((ptstrJoinBssParam->tsf) >> 16) & 0xFF;
+		*(pu8CurrByte++) = ((ptstrJoinBssParam->tsf) >> 24) & 0xFF;
+
+		*(pu8CurrByte++) = ptstrJoinBssParam->u8Index;
+
+		*(pu8CurrByte++) = ptstrJoinBssParam->u8OppEnable;
+
+		if (ptstrJoinBssParam->u8OppEnable)
+			*(pu8CurrByte++) = ptstrJoinBssParam->u8CtWindow;
+
+		*(pu8CurrByte++) = ptstrJoinBssParam->u8Count;
+
+		WILC_memcpy(pu8CurrByte, ptstrJoinBssParam->au8Duration, sizeof(ptstrJoinBssParam->au8Duration));
+
+		pu8CurrByte += sizeof(ptstrJoinBssParam->au8Duration);
+
+		WILC_memcpy(pu8CurrByte, ptstrJoinBssParam->au8Interval, sizeof(ptstrJoinBssParam->au8Interval));
+
+		pu8CurrByte += sizeof(ptstrJoinBssParam->au8Interval);
+
+		WILC_memcpy(pu8CurrByte, ptstrJoinBssParam->au8StartTime, sizeof(ptstrJoinBssParam->au8StartTime));
+
+		pu8CurrByte += sizeof(ptstrJoinBssParam->au8StartTime);
+
+	} else
+		PRINT_D(HOSTINF_DBG, "NOA not present\n");
+	#endif
+
+
+	/* keep the buffer at the start of the allocated pointer to use it with the free*/
+	pu8CurrByte = strWIDList[u32WidsCount].ps8WidVal;
+
+
+	#endif /* #ifdef WILC_PARSE_SCAN_IN_HOST*/
+	u32WidsCount++;
+
+	#ifndef SIMULATION
+	/* A temporary workaround to avoid handling the misleading MAC_DISCONNECTED raised from the
+	 *   firmware at chip reset when processing the WIDs of the Connect Request.
+	 *   (This workaround should be removed in the future when the Chip reset of the Connect WIDs is disabled) */
+	/* ////////////////////// */
+	gu32WidConnRstHack = 0;
+	/* ////////////////////// */
+	#endif
+
+	/*BugID_5137*/
+	if (WILC_memcmp("DIRECT-", pstrHostIFconnectAttr->pu8ssid, 7)) {
+		memcpy(gu8FlushedJoinReq, pu8CurrByte, gu32FlushedJoinReqSize);
+		gu8FlushedJoinReqDrvHandler = (u32)pstrWFIDrv;
+	}
+
+	PRINT_D(GENERIC_DBG, "send HOST_IF_WAITING_CONN_RESP\n");
+
+	if (pstrHostIFconnectAttr->pu8bssid != NULL) {
+		WILC_memcpy(u8ConnectedSSID, pstrHostIFconnectAttr->pu8bssid, ETH_ALEN);
+
+		PRINT_D(GENERIC_DBG, "save Bssid = %x:%x:%x:%x:%x:%x\n", (pstrHostIFconnectAttr->pu8bssid[0]), (pstrHostIFconnectAttr->pu8bssid[1]), (pstrHostIFconnectAttr->pu8bssid[2]), (pstrHostIFconnectAttr->pu8bssid[3]), (pstrHostIFconnectAttr->pu8bssid[4]), (pstrHostIFconnectAttr->pu8bssid[5]));
+		PRINT_D(GENERIC_DBG, "save bssid = %x:%x:%x:%x:%x:%x\n", (u8ConnectedSSID[0]), (u8ConnectedSSID[1]), (u8ConnectedSSID[2]), (u8ConnectedSSID[3]), (u8ConnectedSSID[4]), (u8ConnectedSSID[5]));
+	}
+
+	s32Error = SendConfigPkt(SET_CFG, strWIDList, u32WidsCount, false, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Handle_Connect()] failed to send config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	} else {
+		PRINT_D(GENERIC_DBG, "set HOST_IF_WAITING_CONN_RESP\n");
+		pstrWFIDrv->enuHostIFstate = HOST_IF_WAITING_CONN_RESP;
+	}
+	#endif
+
+	WILC_CATCH(s32Error)
+	{
+		tstrConnectInfo strConnectInfo;
+
+		WILC_TimerStop(&(pstrWFIDrv->hConnectTimer), NULL);
+
+		PRINT_D(HOSTINF_DBG, "could not start connecting to the required network\n");
+
+		WILC_memset(&strConnectInfo, 0, sizeof(tstrConnectInfo));
+
+		if (pstrHostIFconnectAttr->pfConnectResult != NULL) {
+			if (pstrHostIFconnectAttr->pu8bssid != NULL) {
+				WILC_memcpy(strConnectInfo.au8bssid, pstrHostIFconnectAttr->pu8bssid, 6);
+			}
+
+			if (pstrHostIFconnectAttr->pu8IEs != NULL) {
+				strConnectInfo.ReqIEsLen = pstrHostIFconnectAttr->IEsLen;
+				strConnectInfo.pu8ReqIEs = (u8 *)WILC_MALLOC(pstrHostIFconnectAttr->IEsLen);
+				WILC_memcpy(strConnectInfo.pu8ReqIEs,
+					    pstrHostIFconnectAttr->pu8IEs,
+					    pstrHostIFconnectAttr->IEsLen);
+			}
+
+			pstrHostIFconnectAttr->pfConnectResult(CONN_DISCONN_EVENT_CONN_RESP,
+							       &strConnectInfo,
+							       MAC_DISCONNECTED,
+							       NULL,
+							       pstrHostIFconnectAttr->pvUserArg);
+			/*Change state to idle*/
+			pstrWFIDrv->enuHostIFstate = HOST_IF_IDLE;
+			/* Deallocation */
+			if (strConnectInfo.pu8ReqIEs != NULL) {
+				WILC_FREE(strConnectInfo.pu8ReqIEs);
+				strConnectInfo.pu8ReqIEs = NULL;
+			}
+
+		} else {
+			PRINT_ER("Connect callback function pointer is NULL \n");
+		}
+	}
+
+	PRINT_D(HOSTINF_DBG, "Deallocating connection parameters\n");
+	/* Deallocate pstrHostIFconnectAttr->pu8bssid which was prevoisuly allocated by the sending thread */
+	if (pstrHostIFconnectAttr->pu8bssid != NULL) {
+		WILC_FREE(pstrHostIFconnectAttr->pu8bssid);
+		pstrHostIFconnectAttr->pu8bssid = NULL;
+	}
+
+	/* Deallocate pstrHostIFconnectAttr->pu8ssid which was prevoisuly allocated by the sending thread */
+	if (pstrHostIFconnectAttr->pu8ssid != NULL) {
+		WILC_FREE(pstrHostIFconnectAttr->pu8ssid);
+		pstrHostIFconnectAttr->pu8ssid = NULL;
+	}
+
+	/* Deallocate pstrHostIFconnectAttr->pu8IEs which was prevoisuly allocated by the sending thread */
+	if (pstrHostIFconnectAttr->pu8IEs != NULL) {
+		WILC_FREE(pstrHostIFconnectAttr->pu8IEs);
+		pstrHostIFconnectAttr->pu8IEs = NULL;
+	}
+
+	if (pu8CurrByte != NULL) {
+		WILC_FREE(pu8CurrByte);
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief                      Handle_FlushConnect
+ *  @details            Sending config packet to firmware to flush an old connection
+ *                              after switching FW from station one to hybrid one
+ *  @param[in]          void * drvHandler
+ *  @return             Error code.
+ *  @author		Amr Abdel-Moghny
+ *  @date			19 DEC 2013
+ *  @version		8.0
+ */
+
+static s32 Handle_FlushConnect(void *drvHandler)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWIDList[5];
+	u32 u32WidsCount = 0;
+	u8 *pu8CurrByte = NULL;
+
+
+	/* IEs to be inserted in Association Request */
+	strWIDList[u32WidsCount].u16WIDid = WID_INFO_ELEMENT_ASSOCIATE;
+	strWIDList[u32WidsCount].enuWIDtype = WID_BIN_DATA;
+	strWIDList[u32WidsCount].ps8WidVal = gu8FlushedInfoElemAsoc;
+	strWIDList[u32WidsCount].s32ValueSize = gu32FlushedInfoElemAsocSize;
+	u32WidsCount++;
+
+	strWIDList[u32WidsCount].u16WIDid = (u16)WID_11I_MODE;
+	strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(gu8Flushed11iMode));
+	u32WidsCount++;
+
+
+
+	strWIDList[u32WidsCount].u16WIDid = (u16)WID_AUTH_TYPE;
+	strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&gu8FlushedAuthType);
+	u32WidsCount++;
+
+
+	#ifdef WILC_PARSE_SCAN_IN_HOST
+	strWIDList[u32WidsCount].u16WIDid = (u16)WID_JOIN_REQ_EXTENDED;
+	strWIDList[u32WidsCount].enuWIDtype = WID_STR;
+	strWIDList[u32WidsCount].s32ValueSize = gu32FlushedJoinReqSize;
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)gu8FlushedJoinReq;
+	pu8CurrByte = strWIDList[u32WidsCount].ps8WidVal;
+
+	pu8CurrByte += FLUSHED_BYTE_POS;
+	*(pu8CurrByte) = FLUSHED_JOIN_REQ;
+
+	u32WidsCount++;
+
+	#endif
+
+	s32Error = SendConfigPkt(SET_CFG, strWIDList, u32WidsCount, false, gu8FlushedJoinReqDrvHandler);
+	if (s32Error) {
+		PRINT_ER("Handle_Flush_Connect()] failed to send config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief                 Handle_ConnectTimeout
+ *  @details       Call connect notification callback function indicating connection failure
+ *  @param[in]    NONE
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_ConnectTimeout(void *drvHandler)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrConnectInfo strConnectInfo;
+	tstrWID strWID;
+	u16 u16DummyReasonCode = 0;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *) drvHandler;
+
+	if (pstrWFIDrv == NULL)	{
+		PRINT_ER("Driver handler is NULL\n");
+		return s32Error;
+	}
+
+	pstrWFIDrv->enuHostIFstate = HOST_IF_IDLE;
+
+	gbScanWhileConnected = false;
+
+
+	WILC_memset(&strConnectInfo, 0, sizeof(tstrConnectInfo));
+
+
+	/* First, we will notify the upper layer with the Connection failure {through the Connect Callback function},
+	 *   then we will notify our firmware also with the Connection failure {through sending to it Cfg packet carrying
+	 *   WID_DISCONNECT} */
+	if (pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult != NULL)	{
+		if (pstrWFIDrv->strWILC_UsrConnReq.pu8bssid != NULL) {
+			WILC_memcpy(strConnectInfo.au8bssid,
+				    pstrWFIDrv->strWILC_UsrConnReq.pu8bssid, 6);
+		}
+
+		if (pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs != NULL) {
+			strConnectInfo.ReqIEsLen = pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen;
+			strConnectInfo.pu8ReqIEs = (u8 *)WILC_MALLOC(pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen);
+			WILC_memcpy(strConnectInfo.pu8ReqIEs,
+				    pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs,
+				    pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen);
+		}
+
+		pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult(CONN_DISCONN_EVENT_CONN_RESP,
+								   &strConnectInfo,
+								   MAC_DISCONNECTED,
+								   NULL,
+								   pstrWFIDrv->strWILC_UsrConnReq.u32UserConnectPvoid);
+
+		/* Deallocation of strConnectInfo.pu8ReqIEs */
+		if (strConnectInfo.pu8ReqIEs != NULL) {
+			WILC_FREE(strConnectInfo.pu8ReqIEs);
+			strConnectInfo.pu8ReqIEs = NULL;
+		}
+	} else {
+		PRINT_ER("Connect callback function pointer is NULL \n");
+	}
+
+	/* Here we will notify our firmware also with the Connection failure {through sending to it Cfg packet carrying
+	 *   WID_DISCONNECT} */
+	strWID.u16WIDid = (u16)WID_DISCONNECT;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = (s8 *)&u16DummyReasonCode;
+	strWID.s32ValueSize = sizeof(char);
+
+	PRINT_D(HOSTINF_DBG, "Sending disconnect request\n");
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, false, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to send dissconect config packet\n");
+	}
+
+	/* Deallocation of the Saved Connect Request in the global Handle */
+	pstrWFIDrv->strWILC_UsrConnReq.ssidLen = 0;
+	if (pstrWFIDrv->strWILC_UsrConnReq.pu8ssid != NULL) {
+		WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8ssid);
+		pstrWFIDrv->strWILC_UsrConnReq.pu8ssid = NULL;
+	}
+
+	if (pstrWFIDrv->strWILC_UsrConnReq.pu8bssid != NULL) {
+		WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8bssid);
+		pstrWFIDrv->strWILC_UsrConnReq.pu8bssid = NULL;
+	}
+
+	pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen = 0;
+	if (pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs != NULL) {
+		WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs);
+		pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs = NULL;
+	}
+
+	WILC_memset(u8ConnectedSSID, 0, ETH_ALEN);
+	/*BugID_5213*/
+	/*Freeing flushed join request params on connect timeout*/
+	if (gu8FlushedJoinReq != NULL && gu8FlushedJoinReqDrvHandler == (u32)drvHandler) {
+		WILC_FREE(gu8FlushedJoinReq);
+		gu8FlushedJoinReq = NULL;
+	}
+	if (gu8FlushedInfoElemAsoc != NULL && gu8FlushedJoinReqDrvHandler == (u32)drvHandler) {
+		WILC_FREE(gu8FlushedInfoElemAsoc);
+		gu8FlushedInfoElemAsoc = NULL;
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_RcvdNtwrkInfo
+ *  @details       Handling received network information
+ *  @param[in]    tstrRcvdNetworkInfo* pstrRcvdNetworkInfo
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_RcvdNtwrkInfo(void *drvHandler, tstrRcvdNetworkInfo *pstrRcvdNetworkInfo)
+{
+	u32 i;
+	bool bNewNtwrkFound;
+
+
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrNetworkInfo *pstrNetworkInfo = NULL;
+	void *pJoinParams = NULL;
+
+	tstrWILC_WFIDrv *pstrWFIDrv  = (tstrWILC_WFIDrv *)drvHandler;
+
+
+
+	bNewNtwrkFound = true;
+	PRINT_INFO(HOSTINF_DBG, "Handling received network info\n");
+
+	/*if there is a an ongoing scan request*/
+	if (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult) {
+		PRINT_D(HOSTINF_DBG, "State: Scanning, parsing network information received\n");
+		ParseNetworkInfo(pstrRcvdNetworkInfo->pu8Buffer, &pstrNetworkInfo);
+		if ((pstrNetworkInfo == NULL)
+		    || (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult == NULL)) {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+
+		/* check whether this network is discovered before */
+		for (i = 0; i < pstrWFIDrv->strWILC_UsrScanReq.u32RcvdChCount; i++) {
+
+			if ((pstrWFIDrv->strWILC_UsrScanReq.astrFoundNetworkInfo[i].au8bssid != NULL) &&
+			    (pstrNetworkInfo->au8bssid != NULL)) {
+				if (WILC_memcmp(pstrWFIDrv->strWILC_UsrScanReq.astrFoundNetworkInfo[i].au8bssid,
+						pstrNetworkInfo->au8bssid, 6) == 0) {
+					if (pstrNetworkInfo->s8rssi <= pstrWFIDrv->strWILC_UsrScanReq.astrFoundNetworkInfo[i].s8rssi) {
+						/*we have already found this network with better rssi, so keep the old cached one and don't
+						 *  send anything to the upper layer */
+						PRINT_D(HOSTINF_DBG, "Network previously discovered\n");
+						goto done;
+					} else {
+						/* here the same already found network is found again but with a better rssi, so just update
+						 *   the rssi for this cached network and send this updated network to the upper layer but
+						 *   don't add a new record for it */
+						pstrWFIDrv->strWILC_UsrScanReq.astrFoundNetworkInfo[i].s8rssi = pstrNetworkInfo->s8rssi;
+						bNewNtwrkFound = false;
+						break;
+					}
+				}
+			}
+		}
+
+		if (bNewNtwrkFound == true) {
+			/* here it is confirmed that it is a new discovered network,
+			 * so add its record then call the User CallBack function */
+
+			PRINT_D(HOSTINF_DBG, "New network found\n");
+
+			if (pstrWFIDrv->strWILC_UsrScanReq.u32RcvdChCount < MAX_NUM_SCANNED_NETWORKS) {
+				pstrWFIDrv->strWILC_UsrScanReq.astrFoundNetworkInfo[pstrWFIDrv->strWILC_UsrScanReq.u32RcvdChCount].s8rssi = pstrNetworkInfo->s8rssi;
+
+				if ((pstrWFIDrv->strWILC_UsrScanReq.astrFoundNetworkInfo[pstrWFIDrv->strWILC_UsrScanReq.u32RcvdChCount].au8bssid != NULL)
+				    && (pstrNetworkInfo->au8bssid != NULL)) {
+					WILC_memcpy(pstrWFIDrv->strWILC_UsrScanReq.astrFoundNetworkInfo[pstrWFIDrv->strWILC_UsrScanReq.u32RcvdChCount].au8bssid,
+						    pstrNetworkInfo->au8bssid, 6);
+
+					pstrWFIDrv->strWILC_UsrScanReq.u32RcvdChCount++;
+
+					pstrNetworkInfo->bNewNetwork = true;
+					/*Bug4218: Parsing Join Param*/
+					/* add new BSS to JoinBssTable */
+				#ifdef WILC_PARSE_SCAN_IN_HOST
+					pJoinParams = host_int_ParseJoinBssParam(pstrNetworkInfo);
+				#endif /*WILC_PARSE_SCAN_IN_HOST*/
+
+					pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult(SCAN_EVENT_NETWORK_FOUND, pstrNetworkInfo,
+											pstrWFIDrv->strWILC_UsrScanReq.u32UserScanPvoid,
+											pJoinParams);
+
+
+				}
+			} else {
+				PRINT_WRN(HOSTINF_DBG, "Discovered networks exceeded max. limit \n");
+			}
+		} else {
+			pstrNetworkInfo->bNewNetwork = false;
+			/* just call the User CallBack function to send the same discovered network with its updated RSSI */
+			pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult(SCAN_EVENT_NETWORK_FOUND, pstrNetworkInfo,
+									pstrWFIDrv->strWILC_UsrScanReq.u32UserScanPvoid, NULL);
+		}
+	}
+
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+done:
+	/* Deallocate pstrRcvdNetworkInfo->pu8Buffer which was prevoisuly allocated by the sending thread */
+	if (pstrRcvdNetworkInfo->pu8Buffer != NULL) {
+		WILC_FREE(pstrRcvdNetworkInfo->pu8Buffer);
+		pstrRcvdNetworkInfo->pu8Buffer = NULL;
+	}
+
+	/*free structure allocated*/
+	if (pstrNetworkInfo != NULL) {
+		DeallocateNetworkInfo(pstrNetworkInfo);
+		pstrNetworkInfo = NULL;
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_RcvdGnrlAsyncInfo
+ *  @details       Handling received asynchrous general network information
+ *  @param[in]    tstrRcvdGnrlAsyncInfo* pstrRcvdGnrlAsyncInfo
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_RcvdGnrlAsyncInfo(void *drvHandler, tstrRcvdGnrlAsyncInfo *pstrRcvdGnrlAsyncInfo)
+{
+	/* TODO: mostafa: till now, this function just handles only the received mac status msg, */
+	/*				 which carries only 1 WID which have WID ID = WID_STATUS */
+	s32 s32Error = WILC_SUCCESS;
+	u8 u8MsgType = 0;
+	u8 u8MsgID = 0;
+	u16 u16MsgLen = 0;
+	u16 u16WidID = (u16)WID_NIL;
+	u8 u8WidLen  = 0;
+	u8 u8MacStatus;
+	u8 u8MacStatusReasonCode;
+	u8 u8MacStatusAdditionalInfo;
+	tstrConnectInfo strConnectInfo;
+	tstrDisconnectNotifInfo strDisconnectNotifInfo;
+	s32 s32Err = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *) drvHandler;
+	if (pstrWFIDrv == NULL)	{
+		PRINT_ER("Driver handler is NULL\n");
+	}
+	PRINT_D(GENERIC_DBG, "Current State = %d,Received state = %d\n", pstrWFIDrv->enuHostIFstate,
+		pstrRcvdGnrlAsyncInfo->pu8Buffer[7]);
+
+	if ((pstrWFIDrv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) ||
+	    (pstrWFIDrv->enuHostIFstate == HOST_IF_CONNECTED) ||
+	    pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult) {
+		if ((pstrRcvdGnrlAsyncInfo->pu8Buffer == NULL) ||
+		    (pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult == NULL)) {
+			WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+		}
+
+		u8MsgType = pstrRcvdGnrlAsyncInfo->pu8Buffer[0];
+
+		/* Check whether the received message type is 'I' */
+		if ('I' != u8MsgType) {
+			PRINT_ER("Received Message format incorrect.\n");
+			WILC_ERRORREPORT(s32Error, WILC_FAIL);
+		}
+
+		/* Extract message ID */
+		u8MsgID = pstrRcvdGnrlAsyncInfo->pu8Buffer[1];
+
+		/* Extract message Length */
+		u16MsgLen = MAKE_WORD16(pstrRcvdGnrlAsyncInfo->pu8Buffer[2], pstrRcvdGnrlAsyncInfo->pu8Buffer[3]);
+
+		/* Extract WID ID [expected to be = WID_STATUS] */
+		u16WidID = MAKE_WORD16(pstrRcvdGnrlAsyncInfo->pu8Buffer[4], pstrRcvdGnrlAsyncInfo->pu8Buffer[5]);
+
+		/* Extract WID Length [expected to be = 1] */
+		u8WidLen = pstrRcvdGnrlAsyncInfo->pu8Buffer[6];
+
+		/* get the WID value [expected to be one of two values: either MAC_CONNECTED = (1) or MAC_DISCONNECTED = (0)] */
+		u8MacStatus  = pstrRcvdGnrlAsyncInfo->pu8Buffer[7];
+		u8MacStatusReasonCode = pstrRcvdGnrlAsyncInfo->pu8Buffer[8];
+		u8MacStatusAdditionalInfo = pstrRcvdGnrlAsyncInfo->pu8Buffer[9];
+		PRINT_INFO(HOSTINF_DBG, "Recieved MAC status = %d with Reason = %d , Info = %d\n", u8MacStatus, u8MacStatusReasonCode, u8MacStatusAdditionalInfo);
+		if (pstrWFIDrv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) {
+			/* our station had sent Association Request frame, so here it will get the Association Response frame then parse it */
+			u32 u32RcvdAssocRespInfoLen;
+			tstrConnectRespInfo *pstrConnectRespInfo = NULL;
+
+			PRINT_D(HOSTINF_DBG, "Recieved MAC status = %d with Reason = %d , Code = %d\n", u8MacStatus, u8MacStatusReasonCode, u8MacStatusAdditionalInfo);
+
+			WILC_memset(&strConnectInfo, 0, sizeof(tstrConnectInfo));
+
+			if (u8MacStatus == MAC_CONNECTED) {
+				WILC_memset(gapu8RcvdAssocResp, 0, MAX_ASSOC_RESP_FRAME_SIZE);
+
+				host_int_get_assoc_res_info((WILC_WFIDrvHandle)pstrWFIDrv,
+							    gapu8RcvdAssocResp,
+							    MAX_ASSOC_RESP_FRAME_SIZE,
+							    &u32RcvdAssocRespInfoLen);
+
+				PRINT_INFO(HOSTINF_DBG, "Received association response with length = %d\n", u32RcvdAssocRespInfoLen);
+
+				if (u32RcvdAssocRespInfoLen != 0) {
+
+					PRINT_D(HOSTINF_DBG, "Parsing association response\n");
+					s32Err = ParseAssocRespInfo(gapu8RcvdAssocResp, u32RcvdAssocRespInfoLen,
+								    &pstrConnectRespInfo);
+					if (s32Err) {
+						PRINT_ER("ParseAssocRespInfo() returned error %d \n", s32Err);
+					} else {
+						/* use the necessary parsed Info from the Received Association Response */
+						strConnectInfo.u16ConnectStatus = pstrConnectRespInfo->u16ConnectStatus;
+
+						if (strConnectInfo.u16ConnectStatus == SUCCESSFUL_STATUSCODE) {
+							PRINT_INFO(HOSTINF_DBG, "Association response received : Successful connection status\n");
+							if (pstrConnectRespInfo->pu8RespIEs != NULL) {
+								strConnectInfo.u16RespIEsLen = pstrConnectRespInfo->u16RespIEsLen;
+
+
+								strConnectInfo.pu8RespIEs = (u8 *)WILC_MALLOC(pstrConnectRespInfo->u16RespIEsLen);
+								WILC_memcpy(strConnectInfo.pu8RespIEs, pstrConnectRespInfo->pu8RespIEs,
+									    pstrConnectRespInfo->u16RespIEsLen);
+							}
+						}
+
+						/* deallocate the Assoc. Resp. parsed structure as it is not needed anymore */
+						if (pstrConnectRespInfo != NULL) {
+							DeallocateAssocRespInfo(pstrConnectRespInfo);
+							pstrConnectRespInfo = NULL;
+						}
+					}
+				}
+			}
+
+			/* The station has just received mac status and it also received assoc. response which
+			 *   it was waiting for.
+			 *   So check first the matching between the received mac status and the received status code in Asoc Resp */
+			if ((u8MacStatus == MAC_CONNECTED) &&
+			    (strConnectInfo.u16ConnectStatus != SUCCESSFUL_STATUSCODE))	{
+				PRINT_ER("Received MAC status is MAC_CONNECTED while the received status code in Asoc Resp is not SUCCESSFUL_STATUSCODE \n");
+				WILC_memset(u8ConnectedSSID, 0, ETH_ALEN);
+
+			} else if (u8MacStatus == MAC_DISCONNECTED)    {
+				PRINT_ER("Received MAC status is MAC_DISCONNECTED\n");
+				WILC_memset(u8ConnectedSSID, 0, ETH_ALEN);
+			}
+
+			/* TODO: mostafa: correct BSSID should be retrieved from actual BSSID received from AP */
+			/*               through a structure of type tstrConnectRespInfo */
+			if (pstrWFIDrv->strWILC_UsrConnReq.pu8bssid != NULL) {
+				PRINT_D(HOSTINF_DBG, "Retrieving actual BSSID from AP\n");
+				WILC_memcpy(strConnectInfo.au8bssid, pstrWFIDrv->strWILC_UsrConnReq.pu8bssid, 6);
+
+				if ((u8MacStatus == MAC_CONNECTED) &&
+				    (strConnectInfo.u16ConnectStatus == SUCCESSFUL_STATUSCODE))	{
+					WILC_memcpy(pstrWFIDrv->au8AssociatedBSSID,
+						    pstrWFIDrv->strWILC_UsrConnReq.pu8bssid, ETH_ALEN);
+				}
+			}
+
+
+			if (pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs != NULL) {
+				strConnectInfo.ReqIEsLen = pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen;
+				strConnectInfo.pu8ReqIEs = (u8 *)WILC_MALLOC(pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen);
+				WILC_memcpy(strConnectInfo.pu8ReqIEs,
+					    pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs,
+					    pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen);
+			}
+
+
+			WILC_TimerStop(&(pstrWFIDrv->hConnectTimer), NULL);
+			pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult(CONN_DISCONN_EVENT_CONN_RESP,
+									   &strConnectInfo,
+									   u8MacStatus,
+									   NULL,
+									   pstrWFIDrv->strWILC_UsrConnReq.u32UserConnectPvoid);
+
+
+			/* if received mac status is MAC_CONNECTED and
+			 *  received status code in Asoc Resp is SUCCESSFUL_STATUSCODE, change state to CONNECTED
+			 *  else change state to IDLE */
+			if ((u8MacStatus == MAC_CONNECTED) &&
+			    (strConnectInfo.u16ConnectStatus == SUCCESSFUL_STATUSCODE))	{
+				#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+
+				host_int_set_power_mgmt((WILC_WFIDrvHandle)pstrWFIDrv, 0, 0);
+				#endif
+
+				PRINT_D(HOSTINF_DBG, "MAC status : CONNECTED and Connect Status : Successful\n");
+				pstrWFIDrv->enuHostIFstate = HOST_IF_CONNECTED;
+
+				#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+				PRINT_D(GENERIC_DBG, "Obtaining an IP, Disable Scan\n");
+				g_obtainingIP = true;
+				WILC_TimerStart(&hDuringIpTimer, 10000, NULL, NULL);
+				#endif
+
+				#ifdef WILC_PARSE_SCAN_IN_HOST
+				/* open a BA session if possible */
+				/* if(pstrWFIDrv->strWILC_UsrConnReq.IsHTCapable) */
+
+				#endif
+
+				/* host_int_addBASession(pstrWFIDrv->strWILC_UsrConnReq.pu8bssid,0, */
+				/* BA_SESSION_DEFAULT_BUFFER_SIZE,BA_SESSION_DEFAULT_TIMEOUT); */
+			} else {
+				PRINT_D(HOSTINF_DBG, "MAC status : %d and Connect Status : %d\n", u8MacStatus, strConnectInfo.u16ConnectStatus);
+				pstrWFIDrv->enuHostIFstate = HOST_IF_IDLE;
+				gbScanWhileConnected = false;
+			}
+
+			/* Deallocation */
+			if (strConnectInfo.pu8RespIEs != NULL) {
+				WILC_FREE(strConnectInfo.pu8RespIEs);
+				strConnectInfo.pu8RespIEs = NULL;
+			}
+
+			if (strConnectInfo.pu8ReqIEs != NULL) {
+				WILC_FREE(strConnectInfo.pu8ReqIEs);
+				strConnectInfo.pu8ReqIEs = NULL;
+			}
+
+
+			pstrWFIDrv->strWILC_UsrConnReq.ssidLen = 0;
+			if (pstrWFIDrv->strWILC_UsrConnReq.pu8ssid != NULL) {
+				WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8ssid);
+				pstrWFIDrv->strWILC_UsrConnReq.pu8ssid = NULL;
+			}
+
+			if (pstrWFIDrv->strWILC_UsrConnReq.pu8bssid != NULL) {
+				WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8bssid);
+				pstrWFIDrv->strWILC_UsrConnReq.pu8bssid = NULL;
+			}
+
+			pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen = 0;
+			if (pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs != NULL) {
+				WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs);
+				pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs = NULL;
+			}
+
+		} else if ((u8MacStatus == MAC_DISCONNECTED) &&
+			   (pstrWFIDrv->enuHostIFstate == HOST_IF_CONNECTED)) {
+			/* Disassociation or Deauthentication frame has been received */
+			PRINT_D(HOSTINF_DBG, "Received MAC_DISCONNECTED from the FW\n");
+
+			WILC_memset(&strDisconnectNotifInfo, 0, sizeof(tstrDisconnectNotifInfo));
+
+			if (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult) {
+				PRINT_D(HOSTINF_DBG, "\n\n<< Abort the running OBSS Scan >> \n\n");
+				WILC_TimerStop(&(pstrWFIDrv->hScanTimer), NULL);
+				Handle_ScanDone((void *)pstrWFIDrv, SCAN_EVENT_ABORTED);
+			}
+
+			strDisconnectNotifInfo.u16reason = 0;
+			strDisconnectNotifInfo.ie = NULL;
+			strDisconnectNotifInfo.ie_len = 0;
+
+			if (pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult != NULL)	{
+				#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+
+				g_obtainingIP = false;
+				host_int_set_power_mgmt((WILC_WFIDrvHandle)pstrWFIDrv, 0, 0);
+				#endif
+
+				pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult(CONN_DISCONN_EVENT_DISCONN_NOTIF,
+										   NULL,
+										   0,
+										   &strDisconnectNotifInfo,
+										   pstrWFIDrv->strWILC_UsrConnReq.u32UserConnectPvoid);
+
+			} else {
+				PRINT_ER("Connect result callback function is NULL \n");
+			}
+
+			WILC_memset(pstrWFIDrv->au8AssociatedBSSID, 0, ETH_ALEN);
+
+
+			/* Deallocation */
+
+			/* if Information Elements were retrieved from the Received deauth/disassoc frame, then they
+			 *  should be deallocated here */
+			/*
+			 * if(strDisconnectNotifInfo.ie != NULL)
+			 * {
+			 *      WILC_FREE(strDisconnectNotifInfo.ie);
+			 *      strDisconnectNotifInfo.ie = NULL;
+			 * }
+			 */
+
+			pstrWFIDrv->strWILC_UsrConnReq.ssidLen = 0;
+			if (pstrWFIDrv->strWILC_UsrConnReq.pu8ssid != NULL) {
+				WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8ssid);
+				pstrWFIDrv->strWILC_UsrConnReq.pu8ssid = NULL;
+			}
+
+			if (pstrWFIDrv->strWILC_UsrConnReq.pu8bssid != NULL) {
+				WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8bssid);
+				pstrWFIDrv->strWILC_UsrConnReq.pu8bssid = NULL;
+			}
+
+			pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen = 0;
+			if (pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs != NULL) {
+				WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs);
+				pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs = NULL;
+			}
+
+			/*BugID_5213*/
+			/*Freeing flushed join request params on receiving*/
+			/*MAC_DISCONNECTED while connected*/
+			if (gu8FlushedJoinReq != NULL && gu8FlushedJoinReqDrvHandler == (u32)drvHandler) {
+				WILC_FREE(gu8FlushedJoinReq);
+				gu8FlushedJoinReq = NULL;
+			}
+			if (gu8FlushedInfoElemAsoc != NULL && gu8FlushedJoinReqDrvHandler == (u32)drvHandler) {
+				WILC_FREE(gu8FlushedInfoElemAsoc);
+				gu8FlushedInfoElemAsoc = NULL;
+			}
+
+			pstrWFIDrv->enuHostIFstate = HOST_IF_IDLE;
+			gbScanWhileConnected = false;
+
+		} else if ((u8MacStatus == MAC_DISCONNECTED) &&
+			   (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult != NULL)) {
+			PRINT_D(HOSTINF_DBG, "Received MAC_DISCONNECTED from the FW while scanning\n");
+			PRINT_D(HOSTINF_DBG, "\n\n<< Abort the running Scan >> \n\n");
+			/*Abort the running scan*/
+			WILC_TimerStop(&(pstrWFIDrv->hScanTimer), NULL);
+			if (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult) {
+				Handle_ScanDone((void *)pstrWFIDrv, SCAN_EVENT_ABORTED);
+
+			}
+		}
+
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	/* Deallocate pstrRcvdGnrlAsyncInfo->pu8Buffer which was prevoisuly allocated by the sending thread */
+	if (pstrRcvdGnrlAsyncInfo->pu8Buffer != NULL) {
+		WILC_FREE(pstrRcvdGnrlAsyncInfo->pu8Buffer);
+		pstrRcvdGnrlAsyncInfo->pu8Buffer = NULL;
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_Key
+ *  @details       Sending config packet to firmware to set key
+ *  @param[in]    tstrHostIFkeyAttr* pstrHostIFkeyAttr
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static int Handle_Key(void *drvHandler, tstrHostIFkeyAttr *pstrHostIFkeyAttr)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	#ifdef WILC_AP_EXTERNAL_MLME
+	tstrWID strWIDList[5];
+	#endif
+	u8 i;
+	u8 *pu8keybuf;
+	s8 s8idxarray[1];
+	s8 ret = 0;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+
+	switch (pstrHostIFkeyAttr->enuKeyType) {
+
+
+	case WEP:
+
+#ifdef WILC_AP_EXTERNAL_MLME
+		if (pstrHostIFkeyAttr->u8KeyAction & ADDKEY_AP)	{
+
+			PRINT_D(HOSTINF_DBG, "Handling WEP key\n");
+			PRINT_D(GENERIC_DBG, "ID Hostint is %d\n", (pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx));
+			strWIDList[0].u16WIDid = (u16)WID_11I_MODE;
+			strWIDList[0].enuWIDtype = WID_CHAR;
+			strWIDList[0].s32ValueSize = sizeof(char);
+			strWIDList[0].ps8WidVal = (s8 *)(&(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8mode));
+
+			strWIDList[1].u16WIDid     = WID_AUTH_TYPE;
+			strWIDList[1].enuWIDtype  = WID_CHAR;
+			strWIDList[1].s32ValueSize = sizeof(char);
+			strWIDList[1].ps8WidVal = (s8 *)(&(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.tenuAuth_type));
+
+			strWIDList[2].u16WIDid	= (u16)WID_KEY_ID;
+			strWIDList[2].enuWIDtype	= WID_CHAR;
+
+			strWIDList[2].ps8WidVal	= (s8 *)(&(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx));
+			strWIDList[2].s32ValueSize = sizeof(char);
+
+
+			pu8keybuf = (u8 *)WILC_MALLOC(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen);
+
+
+			if (pu8keybuf == NULL) {
+				PRINT_ER("No buffer to send Key\n");
+				return -1;
+			}
+
+			WILC_memcpy(pu8keybuf, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.pu8WepKey,
+				    pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen);
+
+
+			WILC_FREE(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.pu8WepKey);
+
+			strWIDList[3].u16WIDid = (u16)WID_WEP_KEY_VALUE;
+			strWIDList[3].enuWIDtype = WID_STR;
+			strWIDList[3].s32ValueSize = pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen;
+			strWIDList[3].ps8WidVal = (s8 *)pu8keybuf;
+
+
+			s32Error = SendConfigPkt(SET_CFG, strWIDList, 4, true, (u32)pstrWFIDrv);
+			WILC_FREE(pu8keybuf);
+
+
+		}
+#endif
+
+		if (pstrHostIFkeyAttr->u8KeyAction & ADDKEY) {
+			PRINT_D(HOSTINF_DBG, "Handling WEP key\n");
+			pu8keybuf = (u8 *)WILC_MALLOC(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen + 2);
+			if (pu8keybuf == NULL) {
+				PRINT_ER("No buffer to send Key\n");
+				return -1;
+			}
+			pu8keybuf[0] = pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx;
+
+			WILC_memcpy(pu8keybuf + 1, &pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen, 1);
+
+			WILC_memcpy(pu8keybuf + 2, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.pu8WepKey,
+				    pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen);
+
+			WILC_FREE(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.pu8WepKey);
+
+			strWID.u16WIDid	= (u16)WID_ADD_WEP_KEY;
+			strWID.enuWIDtype	= WID_STR;
+			strWID.ps8WidVal	= (s8 *)pu8keybuf;
+			strWID.s32ValueSize = pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen + 2;
+
+			s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+			WILC_FREE(pu8keybuf);
+		} else if (pstrHostIFkeyAttr->u8KeyAction & REMOVEKEY)	  {
+
+			PRINT_D(HOSTINF_DBG, "Removing key\n");
+			strWID.u16WIDid	= (u16)WID_REMOVE_WEP_KEY;
+			strWID.enuWIDtype	= WID_STR;
+
+			s8idxarray[0] = (s8)pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx;
+			strWID.ps8WidVal = s8idxarray;
+			strWID.s32ValueSize = 1;
+
+			s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+		} else {
+			strWID.u16WIDid	= (u16)WID_KEY_ID;
+			strWID.enuWIDtype	= WID_CHAR;
+			strWID.ps8WidVal	= (s8 *)(&(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx));
+			strWID.s32ValueSize = sizeof(char);
+
+			PRINT_D(HOSTINF_DBG, "Setting default key index\n");
+
+			s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+		}
+		up(&(pstrWFIDrv->hSemTestKeyBlock));
+		break;
+
+	case WPARxGtk:
+			#ifdef WILC_AP_EXTERNAL_MLME
+		if (pstrHostIFkeyAttr->u8KeyAction & ADDKEY_AP)	{
+			pu8keybuf = (u8 *)WILC_MALLOC(RX_MIC_KEY_MSG_LEN);
+			if (pu8keybuf == NULL) {
+				PRINT_ER("No buffer to send RxGTK Key\n");
+				ret = -1;
+				goto _WPARxGtk_end_case_;
+			}
+
+			WILC_memset(pu8keybuf, 0, RX_MIC_KEY_MSG_LEN);
+
+
+			/*|----------------------------------------------------------------------------|
+			 * |Sta Address | Key RSC | KeyID | Key Length | Temporal Key	| Rx Michael Key |
+			 * |------------|---------|-------|------------|---------------|----------------|
+			 |	6 bytes	 | 8 byte  |1 byte |  1 byte	|   16 bytes	|	  8 bytes	 |*/
+
+
+
+			if (pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8seq != NULL)
+				WILC_memcpy(pu8keybuf + 6, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8seq, 8);
+
+
+			WILC_memcpy(pu8keybuf + 14, &pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8keyidx, 1);
+
+			WILC_memcpy(pu8keybuf + 15, &pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen, 1);
+
+			WILC_memcpy(pu8keybuf + 16, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8key,
+				    pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen);
+			/* pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Ciphermode =  0X51; */
+			strWIDList[0].u16WIDid = (u16)WID_11I_MODE;
+			strWIDList[0].enuWIDtype = WID_CHAR;
+			strWIDList[0].s32ValueSize = sizeof(char);
+			strWIDList[0].ps8WidVal = (s8 *)(&(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Ciphermode));
+
+			strWIDList[1].u16WIDid	= (u16)WID_ADD_RX_GTK;
+			strWIDList[1].enuWIDtype	= WID_STR;
+			strWIDList[1].ps8WidVal	= (s8 *)pu8keybuf;
+			strWIDList[1].s32ValueSize = RX_MIC_KEY_MSG_LEN;
+
+			s32Error = SendConfigPkt(SET_CFG, strWIDList, 2, true, (u32)pstrWFIDrv);
+
+			WILC_FREE(pu8keybuf);
+
+			/* ////////////////////////// */
+			up(&(pstrWFIDrv->hSemTestKeyBlock));
+			/* ///////////////////////// */
+		}
+
+			#endif
+		if (pstrHostIFkeyAttr->u8KeyAction & ADDKEY) {
+			PRINT_D(HOSTINF_DBG, "Handling group key(Rx) function\n");
+
+			pu8keybuf = (u8 *)WILC_MALLOC(RX_MIC_KEY_MSG_LEN);
+			if (pu8keybuf == NULL) {
+				PRINT_ER("No buffer to send RxGTK Key\n");
+				ret = -1;
+				goto _WPARxGtk_end_case_;
+			}
+
+			WILC_memset(pu8keybuf, 0, RX_MIC_KEY_MSG_LEN);
+
+
+			/*|----------------------------------------------------------------------------|
+			 * |Sta Address | Key RSC | KeyID | Key Length | Temporal Key	| Rx Michael Key |
+			 * |------------|---------|-------|------------|---------------|----------------|
+			 |	6 bytes	 | 8 byte  |1 byte |  1 byte	|   16 bytes	|	  8 bytes	 |*/
+
+			if (pstrWFIDrv->enuHostIFstate == HOST_IF_CONNECTED) {
+				WILC_memcpy(pu8keybuf, pstrWFIDrv->au8AssociatedBSSID, ETH_ALEN);
+			} else {
+				PRINT_ER("Couldn't handle WPARxGtk while enuHostIFstate is not HOST_IF_CONNECTED \n");
+			}
+
+			WILC_memcpy(pu8keybuf + 6, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8seq, 8);
+
+			WILC_memcpy(pu8keybuf + 14, &pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8keyidx, 1);
+
+			WILC_memcpy(pu8keybuf + 15, &pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen, 1);
+			WILC_memcpy(pu8keybuf + 16, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8key,
+				    pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen);
+
+			strWID.u16WIDid	= (u16)WID_ADD_RX_GTK;
+			strWID.enuWIDtype	= WID_STR;
+			strWID.ps8WidVal	= (s8 *)pu8keybuf;
+			strWID.s32ValueSize = RX_MIC_KEY_MSG_LEN;
+
+			s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+			WILC_FREE(pu8keybuf);
+
+			/* ////////////////////////// */
+			up(&(pstrWFIDrv->hSemTestKeyBlock));
+			/* ///////////////////////// */
+		}
+_WPARxGtk_end_case_:
+		WILC_FREE(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8key);
+		WILC_FREE(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8seq);
+		if (ret == -1)
+			return ret;
+
+		break;
+
+	case WPAPtk:
+		#ifdef WILC_AP_EXTERNAL_MLME
+		if (pstrHostIFkeyAttr->u8KeyAction & ADDKEY_AP)	{
+
+
+			pu8keybuf = (u8 *)WILC_MALLOC(PTK_KEY_MSG_LEN + 1);
+
+
+
+			if (pu8keybuf == NULL) {
+				PRINT_ER("No buffer to send PTK Key\n");
+				ret = -1;
+				goto _WPAPtk_end_case_;
+
+			}
+
+			/*|-----------------------------------------------------------------------------|
+			 * |Station address |   keyidx     |Key Length    |Temporal Key  | Rx Michael Key |Tx Michael Key |
+			 * |----------------|------------  |--------------|----------------|---------------|
+			 |	6 bytes    |	1 byte    |   1byte	 |   16 bytes	 |	  8 bytes	  |	   8 bytes	  |
+			 |-----------------------------------------------------------------------------|*/
+
+			WILC_memcpy(pu8keybuf, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8macaddr, 6);  /*1 bytes Key Length */
+
+			WILC_memcpy(pu8keybuf + 6, &pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8keyidx, 1);
+			WILC_memcpy(pu8keybuf + 7, &pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen, 1);
+			/*16 byte TK*/
+			WILC_memcpy(pu8keybuf + 8, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8key,
+				    pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen);
+
+
+			strWIDList[0].u16WIDid = (u16)WID_11I_MODE;
+			strWIDList[0].enuWIDtype = WID_CHAR;
+			strWIDList[0].s32ValueSize = sizeof(char);
+			strWIDList[0].ps8WidVal = (s8 *)(&(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Ciphermode));
+
+			strWIDList[1].u16WIDid	= (u16)WID_ADD_PTK;
+			strWIDList[1].enuWIDtype	= WID_STR;
+			strWIDList[1].ps8WidVal	= (s8 *)pu8keybuf;
+			strWIDList[1].s32ValueSize = PTK_KEY_MSG_LEN + 1;
+
+			s32Error = SendConfigPkt(SET_CFG, strWIDList, 2, true, (u32)pstrWFIDrv);
+			WILC_FREE(pu8keybuf);
+
+			/* ////////////////////////// */
+			up(&(pstrWFIDrv->hSemTestKeyBlock));
+			/* ///////////////////////// */
+		}
+		#endif
+		if (pstrHostIFkeyAttr->u8KeyAction & ADDKEY) {
+
+
+			pu8keybuf = (u8 *)WILC_MALLOC(PTK_KEY_MSG_LEN);
+
+
+
+			if (pu8keybuf == NULL) {
+				PRINT_ER("No buffer to send PTK Key\n");
+				ret = -1;
+				goto _WPAPtk_end_case_;
+
+			}
+
+			/*|-----------------------------------------------------------------------------|
+			 * |Station address | Key Length |	Temporal Key | Rx Michael Key |Tx Michael Key |
+			 * |----------------|------------|--------------|----------------|---------------|
+			 |	6 bytes		 |	1byte	  |   16 bytes	 |	  8 bytes	  |	   8 bytes	  |
+			 |-----------------------------------------------------------------------------|*/
+
+			WILC_memcpy(pu8keybuf, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8macaddr, 6);  /*1 bytes Key Length */
+
+			WILC_memcpy(pu8keybuf + 6, &pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen, 1);
+			/*16 byte TK*/
+			WILC_memcpy(pu8keybuf + 7, pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8key,
+				    pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen);
+
+
+			strWID.u16WIDid	= (u16)WID_ADD_PTK;
+			strWID.enuWIDtype	= WID_STR;
+			strWID.ps8WidVal	= (s8 *)pu8keybuf;
+			strWID.s32ValueSize = PTK_KEY_MSG_LEN;
+
+			s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+			WILC_FREE(pu8keybuf);
+
+			/* ////////////////////////// */
+			up(&(pstrWFIDrv->hSemTestKeyBlock));
+			/* ///////////////////////// */
+		}
+
+_WPAPtk_end_case_:
+		WILC_FREE(pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFwpaAttr.pu8key);
+		if (ret == -1)
+			return ret;
+
+		break;
+
+
+	case PMKSA:
+
+		PRINT_D(HOSTINF_DBG, "Handling PMKSA key\n");
+
+		pu8keybuf = (u8 *)WILC_MALLOC((pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFpmkidAttr.numpmkid * PMKSA_KEY_LEN) + 1);
+		if (pu8keybuf == NULL) {
+			PRINT_ER("No buffer to send PMKSA Key\n");
+			return -1;
+		}
+
+		pu8keybuf[0] = pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFpmkidAttr.numpmkid;
+
+		for (i = 0; i < pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFpmkidAttr.numpmkid; i++) {
+
+			WILC_memcpy(pu8keybuf + ((PMKSA_KEY_LEN * i) + 1), pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFpmkidAttr.pmkidlist[i].bssid, ETH_ALEN);
+			WILC_memcpy(pu8keybuf + ((PMKSA_KEY_LEN * i) + ETH_ALEN + 1), pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFpmkidAttr.pmkidlist[i].pmkid, PMKID_LEN);
+		}
+
+		strWID.u16WIDid	= (u16)WID_PMKID_INFO;
+		strWID.enuWIDtype = WID_STR;
+		strWID.ps8WidVal = (s8 *)pu8keybuf;
+		strWID.s32ValueSize = (pstrHostIFkeyAttr->uniHostIFkeyAttr.strHostIFpmkidAttr.numpmkid * PMKSA_KEY_LEN) + 1;
+
+		s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+		WILC_FREE(pu8keybuf);
+		break;
+	}
+
+	if (s32Error)
+		PRINT_ER("Failed to send key config packet\n");
+
+
+	return s32Error;
+}
+
+
+/**
+ *  @brief Handle_Disconnect
+ *  @details       Sending config packet to firmware to disconnect
+ *  @param[in]    NONE
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_Disconnect(void *drvHandler)
+{
+	tstrWID strWID;
+
+	s32 s32Error = WILC_SUCCESS;
+	u16 u16DummyReasonCode = 0;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+
+	strWID.u16WIDid = (u16)WID_DISCONNECT;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = (s8 *)&u16DummyReasonCode;
+	strWID.s32ValueSize = sizeof(char);
+
+
+
+	PRINT_D(HOSTINF_DBG, "Sending disconnect request\n");
+
+	#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+
+	g_obtainingIP = false;
+	host_int_set_power_mgmt((WILC_WFIDrvHandle)pstrWFIDrv, 0, 0);
+	#endif
+
+	WILC_memset(u8ConnectedSSID, 0, ETH_ALEN);
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, false, (u32)pstrWFIDrv);
+
+	if (s32Error) {
+		PRINT_ER("Failed to send dissconect config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	} else {
+		tstrDisconnectNotifInfo strDisconnectNotifInfo;
+
+		WILC_memset(&strDisconnectNotifInfo, 0, sizeof(tstrDisconnectNotifInfo));
+
+		strDisconnectNotifInfo.u16reason = 0;
+		strDisconnectNotifInfo.ie = NULL;
+		strDisconnectNotifInfo.ie_len = 0;
+
+		if (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult) {
+			WILC_TimerStop(&(pstrWFIDrv->hScanTimer), NULL);
+			pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult(SCAN_EVENT_ABORTED, NULL,
+									pstrWFIDrv->strWILC_UsrScanReq.u32UserScanPvoid, NULL);
+
+			pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult = NULL;
+		}
+
+		if (pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult != NULL)	{
+
+			/*BugID_5193*/
+			/*Stop connect timer, if connection in progress*/
+			if (pstrWFIDrv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) {
+				PRINT_D(HOSTINF_DBG, "Upper layer requested termination of connection\n");
+				WILC_TimerStop(&(pstrWFIDrv->hConnectTimer), NULL);
+			}
+
+			pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult(CONN_DISCONN_EVENT_DISCONN_NOTIF, NULL,
+									   0, &strDisconnectNotifInfo, pstrWFIDrv->strWILC_UsrConnReq.u32UserConnectPvoid);
+		} else {
+			PRINT_ER("strWILC_UsrConnReq.pfUserConnectResult = NULL \n");
+		}
+
+		gbScanWhileConnected = false;
+
+		pstrWFIDrv->enuHostIFstate = HOST_IF_IDLE;
+
+		WILC_memset(pstrWFIDrv->au8AssociatedBSSID, 0, ETH_ALEN);
+
+
+		/* Deallocation */
+		pstrWFIDrv->strWILC_UsrConnReq.ssidLen = 0;
+		if (pstrWFIDrv->strWILC_UsrConnReq.pu8ssid != NULL) {
+			WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8ssid);
+			pstrWFIDrv->strWILC_UsrConnReq.pu8ssid = NULL;
+		}
+
+		if (pstrWFIDrv->strWILC_UsrConnReq.pu8bssid != NULL) {
+			WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8bssid);
+			pstrWFIDrv->strWILC_UsrConnReq.pu8bssid = NULL;
+		}
+
+		pstrWFIDrv->strWILC_UsrConnReq.ConnReqIEsLen = 0;
+		if (pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs != NULL) {
+			WILC_FREE(pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs);
+			pstrWFIDrv->strWILC_UsrConnReq.pu8ConnReqIEs = NULL;
+		}
+
+
+		/*BugID_5137*/
+		if (gu8FlushedJoinReq != NULL && gu8FlushedJoinReqDrvHandler == (u32)drvHandler) {
+			WILC_FREE(gu8FlushedJoinReq);
+			gu8FlushedJoinReq = NULL;
+		}
+		if (gu8FlushedInfoElemAsoc != NULL && gu8FlushedJoinReqDrvHandler == (u32)drvHandler) {
+			WILC_FREE(gu8FlushedInfoElemAsoc);
+			gu8FlushedInfoElemAsoc = NULL;
+		}
+
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	/* ////////////////////////// */
+	up(&(pstrWFIDrv->hSemTestDisconnectBlock));
+	/* ///////////////////////// */
+
+}
+
+
+void resolve_disconnect_aberration(void *drvHandler)
+{
+	tstrWILC_WFIDrv *pstrWFIDrv;
+
+	pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	if (pstrWFIDrv  == NULL)
+		return;
+	if ((pstrWFIDrv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) || (pstrWFIDrv->enuHostIFstate == HOST_IF_CONNECTING)) {
+		PRINT_D(HOSTINF_DBG, "\n\n<< correcting Supplicant state machine >>\n\n");
+		host_int_disconnect((WILC_WFIDrvHandle)pstrWFIDrv, 1);
+	}
+}
+static s32 Switch_Log_Terminal(void *drvHandler)
+{
+
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	static char dummy = 9;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	strWID.u16WIDid = (u16)WID_LOGTerminal_Switch;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = &dummy;
+	strWID.s32ValueSize = sizeof(char);
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+
+	if (s32Error) {
+		PRINT_D(HOSTINF_DBG, "Failed to switch log terminal\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	} else {
+		PRINT_INFO(HOSTINF_DBG, "MAC address set :: \n");
+
+
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_GetChnl
+ *  @details       Sending config packet to get channel
+ *  @param[in]    NONE
+ *  @return         NONE
+ *
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_GetChnl(void *drvHandler)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID	strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	strWID.u16WIDid = (u16)WID_CURRENT_CHANNEL;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = (s8 *)&gu8Chnl;
+	strWID.s32ValueSize = sizeof(char);
+
+	PRINT_D(HOSTINF_DBG, "Getting channel value\n");
+
+	s32Error = SendConfigPkt(GET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	/*get the value by searching the local copy*/
+	if (s32Error) {
+		PRINT_ER("Failed to get channel number\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	up(&(pstrWFIDrv->hSemGetCHNL));
+
+	return s32Error;
+
+
+
+}
+
+
+/**
+ *  @brief Handle_GetRssi
+ *  @details       Sending config packet to get RSSI
+ *  @param[in]    NONE
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_GetRssi(void *drvHandler)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	strWID.u16WIDid = (u16)WID_RSSI;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = &gs8Rssi;
+	strWID.s32ValueSize = sizeof(char);
+
+	/*Sending Cfg*/
+	PRINT_D(HOSTINF_DBG, "Getting RSSI value\n");
+
+	s32Error = SendConfigPkt(GET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to get RSSI value\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	up(&(pstrWFIDrv->hSemGetRSSI));
+
+
+}
+
+
+static void Handle_GetLinkspeed(void *drvHandler)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	gs8lnkspd = 0;
+
+	strWID.u16WIDid = (u16)WID_LINKSPEED;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = &gs8lnkspd;
+	strWID.s32ValueSize = sizeof(char);
+	/*Sending Cfg*/
+	PRINT_D(HOSTINF_DBG, "Getting LINKSPEED value\n");
+
+	s32Error = SendConfigPkt(GET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to get LINKSPEED value\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	up(&(pstrWFIDrv->hSemGetLINKSPEED));
+
+
+}
+
+s32 Handle_GetStatistics(void *drvHandler, tstrStatistics *pstrStatistics)
+{
+	tstrWID strWIDList[5];
+	uint32_t u32WidsCount = 0, s32Error = 0;
+
+	strWIDList[u32WidsCount].u16WIDid = WID_LINKSPEED;
+	strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrStatistics->u8LinkSpeed));
+	u32WidsCount++;
+
+	strWIDList[u32WidsCount].u16WIDid = WID_RSSI;
+	strWIDList[u32WidsCount].enuWIDtype = WID_CHAR;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(char);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrStatistics->s8RSSI));
+	u32WidsCount++;
+
+	strWIDList[u32WidsCount].u16WIDid = WID_SUCCESS_FRAME_COUNT;
+	strWIDList[u32WidsCount].enuWIDtype = WID_INT;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(u32);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrStatistics->u32TxCount));
+	u32WidsCount++;
+
+	strWIDList[u32WidsCount].u16WIDid = WID_RECEIVED_FRAGMENT_COUNT;
+	strWIDList[u32WidsCount].enuWIDtype = WID_INT;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(u32);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrStatistics->u32RxCount));
+	u32WidsCount++;
+
+	strWIDList[u32WidsCount].u16WIDid = WID_FAILED_COUNT;
+	strWIDList[u32WidsCount].enuWIDtype = WID_INT;
+	strWIDList[u32WidsCount].s32ValueSize = sizeof(u32);
+	strWIDList[u32WidsCount].ps8WidVal = (s8 *)(&(pstrStatistics->u32TxFailureCount));
+	u32WidsCount++;
+
+	s32Error = SendConfigPkt(GET_CFG, strWIDList, u32WidsCount, false, (u32)drvHandler);
+
+	if (s32Error) {
+		PRINT_ER("Failed to send scan paramters config packet\n");
+		/* WILC_ERRORREPORT(s32Error, s32Error); */
+	}
+	up(&hWaitResponse);
+	return 0;
+
+}
+
+
+#ifdef WILC_AP_EXTERNAL_MLME
+
+
+/**
+ *  @brief Handle_Get_InActiveTime
+ *  @details       Sending config packet to set mac adddress for station and
+ *                 get inactive time
+ *  @param[in]    NONE
+ *  @return         NONE
+ *
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static s32 Handle_Get_InActiveTime(void *drvHandler, tstrHostIfStaInactiveT *strHostIfStaInactiveT)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	u8 *stamac;
+	tstrWID	strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+
+	strWID.u16WIDid = (u16)WID_SET_STA_MAC_INACTIVE_TIME;
+	strWID.enuWIDtype = WID_STR;
+	strWID.s32ValueSize = ETH_ALEN;
+	strWID.ps8WidVal = (u8 *)WILC_MALLOC(strWID.s32ValueSize);
+
+
+	stamac = strWID.ps8WidVal;
+	WILC_memcpy(stamac, strHostIfStaInactiveT->mac, ETH_ALEN);
+
+
+	PRINT_D(CFG80211_DBG, "SETING STA inactive time\n");
+
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	/*get the value by searching the local copy*/
+	if (s32Error) {
+		PRINT_ER("Failed to SET incative time\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+
+	strWID.u16WIDid = (u16)WID_GET_INACTIVE_TIME;
+	strWID.enuWIDtype = WID_INT;
+	strWID.ps8WidVal = (s8 *)&gu32InactiveTime;
+	strWID.s32ValueSize = sizeof(u32);
+
+
+	s32Error = SendConfigPkt(GET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	/*get the value by searching the local copy*/
+	if (s32Error) {
+		PRINT_ER("Failed to get incative time\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+
+	PRINT_D(CFG80211_DBG, "Getting inactive time : %d\n", gu32InactiveTime);
+
+	up(&(pstrWFIDrv->hSemInactiveTime));
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+
+	return s32Error;
+
+
+
+}
+
+
+/**
+ *  @brief Handle_AddBeacon
+ *  @details       Sending config packet to add beacon
+ *  @param[in]    tstrHostIFSetBeacon* pstrSetBeaconParam
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_AddBeacon(void *drvHandler, tstrHostIFSetBeacon *pstrSetBeaconParam)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	u8 *pu8CurrByte;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	PRINT_D(HOSTINF_DBG, "Adding BEACON\n");
+
+	strWID.u16WIDid = (u16)WID_ADD_BEACON;
+	strWID.enuWIDtype = WID_BIN;
+	strWID.s32ValueSize = pstrSetBeaconParam->u32HeadLen + pstrSetBeaconParam->u32TailLen + 16;
+	strWID.ps8WidVal = WILC_MALLOC(strWID.s32ValueSize);
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWID.ps8WidVal;
+	*pu8CurrByte++ = (pstrSetBeaconParam->u32Interval & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32Interval >> 8) & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32Interval >> 16) & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32Interval >> 24) & 0xFF);
+
+	*pu8CurrByte++ = (pstrSetBeaconParam->u32DTIMPeriod & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32DTIMPeriod >> 8) & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32DTIMPeriod >> 16) & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32DTIMPeriod >> 24) & 0xFF);
+
+	*pu8CurrByte++ = (pstrSetBeaconParam->u32HeadLen & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32HeadLen >> 8) & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32HeadLen >> 16) & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32HeadLen >> 24) & 0xFF);
+
+	memcpy(pu8CurrByte, pstrSetBeaconParam->pu8Head, pstrSetBeaconParam->u32HeadLen);
+	pu8CurrByte += pstrSetBeaconParam->u32HeadLen;
+
+	*pu8CurrByte++ = (pstrSetBeaconParam->u32TailLen & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32TailLen >> 8) & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32TailLen >> 16) & 0xFF);
+	*pu8CurrByte++ = ((pstrSetBeaconParam->u32TailLen >> 24) & 0xFF);
+
+	/* Bug 4599 : if tail length = 0 skip copying */
+	if (pstrSetBeaconParam->pu8Tail > 0)
+		memcpy(pu8CurrByte, pstrSetBeaconParam->pu8Tail, pstrSetBeaconParam->u32TailLen);
+	pu8CurrByte += pstrSetBeaconParam->u32TailLen;
+
+
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, false, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to send add beacon config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	WILC_FREE_IF_TRUE(strWID.ps8WidVal);
+	WILC_FREE_IF_TRUE(pstrSetBeaconParam->pu8Head);
+	WILC_FREE_IF_TRUE(pstrSetBeaconParam->pu8Tail);
+}
+
+
+/**
+ *  @brief Handle_AddBeacon
+ *  @details       Sending config packet to delete beacon
+ *  @param[in]   tstrHostIFDelBeacon* pstrDelBeacon
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_DelBeacon(void *drvHandler, tstrHostIFDelBeacon *pstrDelBeacon)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	u8 *pu8CurrByte;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	strWID.u16WIDid = (u16)WID_DEL_BEACON;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.s32ValueSize = sizeof(char);
+	strWID.ps8WidVal = &gu8DelBcn;
+
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWID.ps8WidVal;
+
+	PRINT_D(HOSTINF_DBG, "Deleting BEACON\n");
+	/* TODO: build del beacon message*/
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, false, (u32)pstrWFIDrv);
+	if (s32Error) {
+
+		PRINT_ER("Failed to send delete beacon config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+}
+
+
+/**
+ *  @brief WILC_HostIf_PackStaParam
+ *  @details       Handling packing of the station params in a buffer
+ *  @param[in]   u8* pu8Buffer, tstrWILC_AddStaParam* pstrStationParam
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static u32 WILC_HostIf_PackStaParam(u8 *pu8Buffer, tstrWILC_AddStaParam *pstrStationParam)
+{
+	u8 *pu8CurrByte;
+
+	pu8CurrByte = pu8Buffer;
+
+	PRINT_D(HOSTINF_DBG, "Packing STA params\n");
+	WILC_memcpy(pu8CurrByte, pstrStationParam->au8BSSID, ETH_ALEN);
+	pu8CurrByte +=  ETH_ALEN;
+
+	*pu8CurrByte++ = pstrStationParam->u16AssocID & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->u16AssocID >> 8) & 0xFF;
+
+	*pu8CurrByte++ = pstrStationParam->u8NumRates;
+	if (pstrStationParam->u8NumRates > 0) {
+		WILC_memcpy(pu8CurrByte, pstrStationParam->pu8Rates, pstrStationParam->u8NumRates);
+	}
+	pu8CurrByte += pstrStationParam->u8NumRates;
+
+	*pu8CurrByte++ = pstrStationParam->bIsHTSupported;
+	*pu8CurrByte++ = pstrStationParam->u16HTCapInfo & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->u16HTCapInfo >> 8) & 0xFF;
+
+	*pu8CurrByte++ = pstrStationParam->u8AmpduParams;
+	WILC_memcpy(pu8CurrByte, pstrStationParam->au8SuppMCsSet, WILC_SUPP_MCS_SET_SIZE);
+	pu8CurrByte += WILC_SUPP_MCS_SET_SIZE;
+
+	*pu8CurrByte++ = pstrStationParam->u16HTExtParams & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->u16HTExtParams >> 8) & 0xFF;
+
+	*pu8CurrByte++ = pstrStationParam->u32TxBeamformingCap & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->u32TxBeamformingCap >> 8) & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->u32TxBeamformingCap >> 16) & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->u32TxBeamformingCap >> 24) & 0xFF;
+
+	*pu8CurrByte++ = pstrStationParam->u8ASELCap;
+
+	*pu8CurrByte++ = pstrStationParam->u16FlagsMask & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->u16FlagsMask >> 8) & 0xFF;
+
+	*pu8CurrByte++ = pstrStationParam->u16FlagsSet & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->u16FlagsSet >> 8) & 0xFF;
+
+	return pu8CurrByte - pu8Buffer;
+}
+
+/**
+ *  @brief Handle_AddStation
+ *  @details       Sending config packet to add station
+ *  @param[in]   tstrWILC_AddStaParam* pstrStationParam
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_AddStation(void *drvHandler, tstrWILC_AddStaParam *pstrStationParam)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	u8 *pu8CurrByte;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	PRINT_D(HOSTINF_DBG, "Handling add station\n");
+	strWID.u16WIDid = (u16)WID_ADD_STA;
+	strWID.enuWIDtype = WID_BIN;
+	strWID.s32ValueSize = WILC_ADD_STA_LENGTH + pstrStationParam->u8NumRates;
+
+	strWID.ps8WidVal = WILC_MALLOC(strWID.s32ValueSize);
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWID.ps8WidVal;
+	pu8CurrByte += WILC_HostIf_PackStaParam(pu8CurrByte, pstrStationParam);
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, false, (u32)pstrWFIDrv);
+	if (s32Error != WILC_SUCCESS) {
+
+		PRINT_ER("Failed to send add station config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	WILC_FREE_IF_TRUE(pstrStationParam->pu8Rates);
+	WILC_FREE_IF_TRUE(strWID.ps8WidVal);
+}
+
+/**
+ *  @brief Handle_DelAllSta
+ *  @details        Sending config packet to delete station
+ *  @param[in]   tstrHostIFDelSta* pstrDelStaParam
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_DelAllSta(void *drvHandler, tstrHostIFDelAllSta *pstrDelAllStaParam)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	u8 *pu8CurrByte;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	u8 i;
+	u8 au8Zero_Buff[6] = {0};
+	strWID.u16WIDid = (u16)WID_DEL_ALL_STA;
+	strWID.enuWIDtype = WID_STR;
+	strWID.s32ValueSize = (pstrDelAllStaParam->u8Num_AssocSta * ETH_ALEN) + 1;
+
+	PRINT_D(HOSTINF_DBG, "Handling delete station \n");
+
+	strWID.ps8WidVal = WILC_MALLOC((pstrDelAllStaParam->u8Num_AssocSta * ETH_ALEN) + 1);
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWID.ps8WidVal;
+
+	*(pu8CurrByte++) = pstrDelAllStaParam->u8Num_AssocSta;
+
+	for (i = 0; i < MAX_NUM_STA; i++) {
+		if (memcmp(pstrDelAllStaParam->au8Sta_DelAllSta[i], au8Zero_Buff, ETH_ALEN))
+			WILC_memcpy(pu8CurrByte, pstrDelAllStaParam->au8Sta_DelAllSta[i], ETH_ALEN);
+		else
+			continue;
+
+		pu8CurrByte += ETH_ALEN;
+	}
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+
+		PRINT_ER("Failed to send add station config packe\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	WILC_FREE_IF_TRUE(strWID.ps8WidVal);
+
+	up(&hWaitResponse);
+}
+
+
+/**
+ *  @brief Handle_DelStation
+ *  @details        Sending config packet to delete station
+ *  @param[in]   tstrHostIFDelSta* pstrDelStaParam
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_DelStation(void *drvHandler, tstrHostIFDelSta *pstrDelStaParam)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	u8 *pu8CurrByte;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	strWID.u16WIDid = (u16)WID_REMOVE_STA;
+	strWID.enuWIDtype = WID_BIN;
+	strWID.s32ValueSize = ETH_ALEN;
+
+	PRINT_D(HOSTINF_DBG, "Handling delete station \n");
+
+	strWID.ps8WidVal = WILC_MALLOC(strWID.s32ValueSize);
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWID.ps8WidVal;
+
+	WILC_memcpy(pu8CurrByte, pstrDelStaParam->au8MacAddr, ETH_ALEN);
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, false, (u32)pstrWFIDrv);
+	if (s32Error) {
+
+		PRINT_ER("Failed to send add station config packe\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	WILC_FREE_IF_TRUE(strWID.ps8WidVal);
+}
+
+
+/**
+ *  @brief Handle_EditStation
+ *  @details        Sending config packet to edit station
+ *  @param[in]   tstrWILC_AddStaParam* pstrStationParam
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_EditStation(void *drvHandler, tstrWILC_AddStaParam *pstrStationParam)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	u8 *pu8CurrByte;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	strWID.u16WIDid = (u16)WID_EDIT_STA;
+	strWID.enuWIDtype = WID_BIN;
+	strWID.s32ValueSize = WILC_ADD_STA_LENGTH + pstrStationParam->u8NumRates;
+
+	PRINT_D(HOSTINF_DBG, "Handling edit station\n");
+	strWID.ps8WidVal = WILC_MALLOC(strWID.s32ValueSize);
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWID.ps8WidVal;
+	pu8CurrByte += WILC_HostIf_PackStaParam(pu8CurrByte, pstrStationParam);
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, false, (u32)pstrWFIDrv);
+	if (s32Error) {
+
+		PRINT_ER("Failed to send edit station config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	WILC_FREE_IF_TRUE(pstrStationParam->pu8Rates);
+	WILC_FREE_IF_TRUE(strWID.ps8WidVal);
+}
+#endif /*WILC_AP_EXTERNAL_MLME*/
+
+#ifdef WILC_P2P
+/**
+ *  @brief Handle_RemainOnChan
+ *  @details        Sending config packet to edit station
+ *  @param[in]   tstrWILC_AddStaParam* pstrStationParam
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static int Handle_RemainOnChan(void *drvHandler, tstrHostIfRemainOnChan *pstrHostIfRemainOnChan)
+{
+	s32 s32Error = WILC_SUCCESS;
+	u8 u8remain_on_chan_flag;
+	tstrWID strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *) drvHandler;
+
+	/*If it's a pendig remain-on-channel, don't overwrite gWFiDrvHandle values (since incoming msg is garbbage)*/
+	if (!pstrWFIDrv->u8RemainOnChan_pendingreq) {
+		pstrWFIDrv->strHostIfRemainOnChan.pVoid = pstrHostIfRemainOnChan->pVoid;
+		pstrWFIDrv->strHostIfRemainOnChan.pRemainOnChanExpired = pstrHostIfRemainOnChan->pRemainOnChanExpired;
+		pstrWFIDrv->strHostIfRemainOnChan.pRemainOnChanReady = pstrHostIfRemainOnChan->pRemainOnChanReady;
+		pstrWFIDrv->strHostIfRemainOnChan.u16Channel = pstrHostIfRemainOnChan->u16Channel;
+		pstrWFIDrv->strHostIfRemainOnChan.u32ListenSessionID = pstrHostIfRemainOnChan->u32ListenSessionID;
+	} else {
+		/*Set the channel to use it as a wid val*/
+		pstrHostIfRemainOnChan->u16Channel = pstrWFIDrv->strHostIfRemainOnChan.u16Channel;
+	}
+
+	if (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult != NULL) {
+		PRINT_INFO(GENERIC_DBG, "Required to remain on chan while scanning return\n");
+		pstrWFIDrv->u8RemainOnChan_pendingreq = 1;
+		WILC_ERRORREPORT(s32Error, WILC_BUSY);
+	}
+	if (pstrWFIDrv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) {
+		PRINT_INFO(GENERIC_DBG, "Required to remain on chan while connecting return\n");
+		WILC_ERRORREPORT(s32Error, WILC_BUSY);
+	}
+
+	#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+	if (g_obtainingIP || connecting) {
+		PRINT_D(GENERIC_DBG, "[handle_scan]: Don't do obss scan until IP adresss is obtained\n");
+		WILC_ERRORREPORT(s32Error, WILC_BUSY);
+	}
+	#endif
+
+	PRINT_D(HOSTINF_DBG, "Setting channel :%d\n", pstrHostIfRemainOnChan->u16Channel);
+
+	u8remain_on_chan_flag = true;
+	strWID.u16WIDid	= (u16)WID_REMAIN_ON_CHAN;
+	strWID.enuWIDtype	= WID_STR;
+	strWID.s32ValueSize = 2;
+	strWID.ps8WidVal = (s8 *)WILC_MALLOC(strWID.s32ValueSize);
+
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	strWID.ps8WidVal[0] = u8remain_on_chan_flag;
+	strWID.ps8WidVal[1] = (s8)pstrHostIfRemainOnChan->u16Channel;
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error != WILC_SUCCESS) {
+		PRINT_ER("Failed to set remain on channel\n");
+	}
+
+	WILC_CATCH(-1)
+	{
+		P2P_LISTEN_STATE = 1;
+		WILC_TimerStart(&(pstrWFIDrv->hRemainOnChannel), pstrHostIfRemainOnChan->u32duration, (void *)pstrWFIDrv, NULL);
+
+		/*Calling CFG ready_on_channel*/
+		if (pstrWFIDrv->strHostIfRemainOnChan.pRemainOnChanReady) {
+			pstrWFIDrv->strHostIfRemainOnChan.pRemainOnChanReady(pstrWFIDrv->strHostIfRemainOnChan.pVoid);
+		}
+
+		if (pstrWFIDrv->u8RemainOnChan_pendingreq)
+			pstrWFIDrv->u8RemainOnChan_pendingreq = 0;
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief Handle_RegisterFrame
+ *  @details
+ *  @param[in]
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static int Handle_RegisterFrame(void *drvHandler, tstrHostIfRegisterFrame *pstrHostIfRegisterFrame)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	u8 *pu8CurrByte;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	PRINT_D(HOSTINF_DBG, "Handling frame register Flag : %d FrameType: %d\n", pstrHostIfRegisterFrame->bReg, pstrHostIfRegisterFrame->u16FrameType);
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_REGISTER_FRAME;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = WILC_MALLOC(sizeof(u16) + 2);
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWID.ps8WidVal;
+
+	*pu8CurrByte++ = pstrHostIfRegisterFrame->bReg;
+	*pu8CurrByte++ = pstrHostIfRegisterFrame->u8Regid;
+	WILC_memcpy(pu8CurrByte, &(pstrHostIfRegisterFrame->u16FrameType), sizeof(u16));
+
+
+	strWID.s32ValueSize = sizeof(u16) + 2;
+
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to frame register config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	}
+
+
+	WILC_CATCH(s32Error)
+	{
+	}
+
+	return s32Error;
+
+}
+
+/**
+ *  @brief                      Handle_ListenStateExpired
+ *  @details            Handle of listen state expiration
+ *  @param[in]          NONE
+ *  @return             Error code.
+ *  @author
+ *  @date
+ *  @version		1.0
+ */
+#define FALSE_FRMWR_CHANNEL 100
+static u32 Handle_ListenStateExpired(void *drvHandler, tstrHostIfRemainOnChan *pstrHostIfRemainOnChan)
+{
+	u8 u8remain_on_chan_flag;
+	tstrWID strWID;
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *) drvHandler;
+
+	PRINT_D(HOSTINF_DBG, "CANCEL REMAIN ON CHAN\n");
+
+	/*BugID_5477*/
+	/*Make sure we are already in listen state*/
+	/*This is to handle duplicate expiry messages (listen timer fired and supplicant called cancel_remain_on_channel())*/
+	if (P2P_LISTEN_STATE) {
+		u8remain_on_chan_flag = false;
+		strWID.u16WIDid	= (u16)WID_REMAIN_ON_CHAN;
+		strWID.enuWIDtype	= WID_STR;
+		strWID.s32ValueSize = 2;
+		strWID.ps8WidVal = WILC_MALLOC(strWID.s32ValueSize);
+
+		if (strWID.ps8WidVal == NULL) {
+			PRINT_ER("Failed to allocate memory\n");
+		}
+
+		strWID.ps8WidVal[0] = u8remain_on_chan_flag;
+		strWID.ps8WidVal[1] = FALSE_FRMWR_CHANNEL;
+
+		/*Sending Cfg*/
+		s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+		if (s32Error != WILC_SUCCESS) {
+			PRINT_ER("Failed to set remain on channel\n");
+			goto _done_;
+		}
+
+		if (pstrWFIDrv->strHostIfRemainOnChan.pRemainOnChanExpired) {
+			pstrWFIDrv->strHostIfRemainOnChan.pRemainOnChanExpired(pstrWFIDrv->strHostIfRemainOnChan.pVoid
+									       , pstrHostIfRemainOnChan->u32ListenSessionID);
+		}
+		P2P_LISTEN_STATE = 0;
+	} else {
+		PRINT_D(GENERIC_DBG, "Not in listen state\n");
+		s32Error = WILC_FAIL;
+	}
+
+_done_:
+	return s32Error;
+}
+
+
+/**
+ *  @brief                      ListenTimerCB
+ *  @details            Callback function of remain-on-channel timer
+ *  @param[in]          NONE
+ *  @return             Error code.
+ *  @author
+ *  @date
+ *  @version		1.0
+ */
+static void ListenTimerCB(void *pvArg)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)pvArg;
+	/*Stopping remain-on-channel timer*/
+	WILC_TimerStop(&(pstrWFIDrv->hRemainOnChannel), NULL);
+
+	/* prepare the Timer Callback message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_LISTEN_TIMER_FIRED;
+	strHostIFmsg.drvHandler = pstrWFIDrv;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan.u32ListenSessionID = pstrWFIDrv->strHostIfRemainOnChan.u32ListenSessionID;
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+}
+#endif
+
+
+/**
+ *  @brief Handle_EditStation
+ *  @details        Sending config packet to edit station
+ *  @param[in]   tstrWILC_AddStaParam* pstrStationParam
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_PowerManagement(void *drvHandler, tstrHostIfPowerMgmtParam *strPowerMgmtParam)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	s8 s8PowerMode;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	strWID.u16WIDid = (u16)WID_POWER_MANAGEMENT;
+
+	if (strPowerMgmtParam->bIsEnabled == true)	{
+		s8PowerMode = MIN_FAST_PS;
+	} else {
+		s8PowerMode = NO_POWERSAVE;
+	}
+	PRINT_D(HOSTINF_DBG, "Handling power mgmt to %d\n", s8PowerMode);
+	strWID.ps8WidVal = &s8PowerMode;
+	strWID.s32ValueSize = sizeof(char);
+
+	PRINT_D(HOSTINF_DBG, "Handling Power Management\n");
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to send power management config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+}
+
+/**
+ *  @brief Handle_SetMulticastFilter
+ *  @details        Set Multicast filter in firmware
+ *  @param[in]   tstrHostIFSetMulti* strHostIfSetMulti
+ *  @return         NONE
+ *  @author		asobhy
+ *  @date
+ *  @version	1.0
+ */
+static void Handle_SetMulticastFilter(void *drvHandler, tstrHostIFSetMulti *strHostIfSetMulti)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	u8 *pu8CurrByte;
+
+	PRINT_D(HOSTINF_DBG, "Setup Multicast Filter\n");
+
+	strWID.u16WIDid = (u16)WID_SETUP_MULTICAST_FILTER;
+	strWID.enuWIDtype = WID_BIN;
+	strWID.s32ValueSize = sizeof(tstrHostIFSetMulti) + ((strHostIfSetMulti->u32count) * ETH_ALEN);
+	strWID.ps8WidVal = WILC_MALLOC(strWID.s32ValueSize);
+	if (strWID.ps8WidVal == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+
+	pu8CurrByte = strWID.ps8WidVal;
+	*pu8CurrByte++ = (strHostIfSetMulti->bIsEnabled & 0xFF);
+	*pu8CurrByte++ = ((strHostIfSetMulti->bIsEnabled >> 8) & 0xFF);
+	*pu8CurrByte++ = ((strHostIfSetMulti->bIsEnabled >> 16) & 0xFF);
+	*pu8CurrByte++ = ((strHostIfSetMulti->bIsEnabled >> 24) & 0xFF);
+
+	*pu8CurrByte++ = (strHostIfSetMulti->u32count & 0xFF);
+	*pu8CurrByte++ = ((strHostIfSetMulti->u32count >> 8) & 0xFF);
+	*pu8CurrByte++ = ((strHostIfSetMulti->u32count >> 16) & 0xFF);
+	*pu8CurrByte++ = ((strHostIfSetMulti->u32count >> 24) & 0xFF);
+
+	if ((strHostIfSetMulti->u32count) > 0)
+		memcpy(pu8CurrByte, gau8MulticastMacAddrList, ((strHostIfSetMulti->u32count) * ETH_ALEN));
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, false, (u32)drvHandler);
+	if (s32Error) {
+		PRINT_ER("Failed to send setup multicast config packet\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	WILC_FREE_IF_TRUE(strWID.ps8WidVal);
+
+}
+
+
+/*BugID_5222*/
+/**
+ *  @brief                      Handle_AddBASession
+ *  @details            Add block ack session
+ *  @param[in]          tstrHostIFSetMulti* strHostIfSetMulti
+ *  @return             NONE
+ *  @author		Amr Abdel-Moghny
+ *  @date			Feb. 2014
+ *  @version		9.0
+ */
+static s32 Handle_AddBASession(void *drvHandler, tstrHostIfBASessionInfo *strHostIfBASessionInfo)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	int AddbaTimeout = 100;
+	char *ptr = NULL;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	PRINT_D(HOSTINF_DBG, "Opening Block Ack session with\nBSSID = %.2x:%.2x:%.2x \nTID=%d \nBufferSize == %d \nSessionTimeOut = %d\n",
+		strHostIfBASessionInfo->au8Bssid[0],
+		strHostIfBASessionInfo->au8Bssid[1],
+		strHostIfBASessionInfo->au8Bssid[2],
+		strHostIfBASessionInfo->u16BufferSize,
+		strHostIfBASessionInfo->u16SessionTimeout,
+		strHostIfBASessionInfo->u8Ted);
+
+	strWID.u16WIDid = (u16)WID_11E_P_ACTION_REQ;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = (u8 *)WILC_MALLOC(BLOCK_ACK_REQ_SIZE);
+	strWID.s32ValueSize = BLOCK_ACK_REQ_SIZE;
+	ptr = strWID.ps8WidVal;
+	/* *ptr++ = 0x14; */
+	*ptr++ = 0x14;
+	*ptr++ = 0x3;
+	*ptr++ = 0x0;
+	WILC_memcpy(ptr, strHostIfBASessionInfo->au8Bssid, ETH_ALEN);
+	ptr += ETH_ALEN;
+	*ptr++ = strHostIfBASessionInfo->u8Ted;
+	/* BA Policy*/
+	*ptr++ = 1;
+	/* Buffer size*/
+	*ptr++ = (strHostIfBASessionInfo->u16BufferSize & 0xFF);
+	*ptr++ = ((strHostIfBASessionInfo->u16BufferSize >> 16) & 0xFF);
+	/* BA timeout*/
+	*ptr++ = (strHostIfBASessionInfo->u16SessionTimeout & 0xFF);
+	*ptr++ = ((strHostIfBASessionInfo->u16SessionTimeout >> 16) & 0xFF);
+	/* ADDBA timeout*/
+	*ptr++ = (AddbaTimeout & 0xFF);
+	*ptr++ = ((AddbaTimeout >> 16) & 0xFF);
+	/* Group Buffer Max Frames*/
+	*ptr++ = 8;
+	/* Group Buffer Timeout */
+	*ptr++ = 0;
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error)
+		PRINT_D(HOSTINF_DBG, "Couldn't open BA Session\n");
+
+
+	strWID.u16WIDid = (u16)WID_11E_P_ACTION_REQ;
+	strWID.enuWIDtype = WID_STR;
+	strWID.s32ValueSize = 15;
+	ptr = strWID.ps8WidVal;
+	/* *ptr++ = 0x14; */
+	*ptr++ = 15;
+	*ptr++ = 7;
+	*ptr++ = 0x2;
+	WILC_memcpy(ptr, strHostIfBASessionInfo->au8Bssid, ETH_ALEN);
+	ptr += ETH_ALEN;
+	/* TID*/
+	*ptr++ = strHostIfBASessionInfo->u8Ted;
+	/* Max Num MSDU */
+	*ptr++ = 8;
+	/* BA timeout*/
+	*ptr++ = (strHostIfBASessionInfo->u16BufferSize & 0xFF);
+	*ptr++ = ((strHostIfBASessionInfo->u16SessionTimeout >> 16) & 0xFF);
+	/*Ack-Policy */
+	*ptr++ = 3;
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+	if (strWID.ps8WidVal != NULL)
+		WILC_FREE(strWID.ps8WidVal);
+
+	return s32Error;
+
+}
+
+
+/*BugID_5222*/
+/**
+ *  @brief                      Handle_DelBASession
+ *  @details            Delete block ack session
+ *  @param[in]          tstrHostIFSetMulti* strHostIfSetMulti
+ *  @return             NONE
+ *  @author		Amr Abdel-Moghny
+ *  @date			Feb. 2013
+ *  @version		9.0
+ */
+static s32 Handle_DelBASession(void *drvHandler, tstrHostIfBASessionInfo *strHostIfBASessionInfo)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	char *ptr = NULL;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	PRINT_D(GENERIC_DBG, "Delete Block Ack session with\nBSSID = %.2x:%.2x:%.2x \nTID=%d\n",
+		strHostIfBASessionInfo->au8Bssid[0],
+		strHostIfBASessionInfo->au8Bssid[1],
+		strHostIfBASessionInfo->au8Bssid[2],
+		strHostIfBASessionInfo->u8Ted);
+
+	strWID.u16WIDid = (u16)WID_11E_P_ACTION_REQ;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = (u8 *)WILC_MALLOC(BLOCK_ACK_REQ_SIZE);
+	strWID.s32ValueSize = BLOCK_ACK_REQ_SIZE;
+	ptr = strWID.ps8WidVal;
+	/* *ptr++ = 0x14; */
+	*ptr++ = 0x14;
+	*ptr++ = 0x3;
+	*ptr++ = 0x2;
+	WILC_memcpy(ptr, strHostIfBASessionInfo->au8Bssid, ETH_ALEN);
+	ptr += ETH_ALEN;
+	*ptr++ = strHostIfBASessionInfo->u8Ted;
+	/* BA direction = recipent*/
+	*ptr++ = 0;
+	/* Delba Reason */
+	*ptr++ = 32; /* Unspecific QOS reason */
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error)
+		PRINT_D(HOSTINF_DBG, "Couldn't delete BA Session\n");
+
+
+	strWID.u16WIDid = (u16)WID_11E_P_ACTION_REQ;
+	strWID.enuWIDtype = WID_STR;
+	strWID.s32ValueSize = 15;
+	ptr = strWID.ps8WidVal;
+	/* *ptr++ = 0x14; */
+	*ptr++ = 15;
+	*ptr++ = 7;
+	*ptr++ = 0x3;
+	WILC_memcpy(ptr, strHostIfBASessionInfo->au8Bssid, ETH_ALEN);
+	ptr += ETH_ALEN;
+	/* TID*/
+	*ptr++ = strHostIfBASessionInfo->u8Ted;
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+
+	if (strWID.ps8WidVal != NULL)
+		WILC_FREE(strWID.ps8WidVal);
+
+	/*BugID_5222*/
+	up(&hWaitResponse);
+
+	return s32Error;
+
+}
+
+
+/**
+ *  @brief                      Handle_DelAllRxBASessions
+ *  @details            Delete all Rx BA sessions
+ *  @param[in]          tstrHostIFSetMulti* strHostIfSetMulti
+ *  @return             NONE
+ *  @author		Abdelrahman Sobhy
+ *  @date			Feb. 2013
+ *  @version		9.0
+ */
+static s32 Handle_DelAllRxBASessions(void *drvHandler, tstrHostIfBASessionInfo *strHostIfBASessionInfo)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	char *ptr = NULL;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+	PRINT_D(GENERIC_DBG, "Delete Block Ack session with\nBSSID = %.2x:%.2x:%.2x \nTID=%d\n",
+		strHostIfBASessionInfo->au8Bssid[0],
+		strHostIfBASessionInfo->au8Bssid[1],
+		strHostIfBASessionInfo->au8Bssid[2],
+		strHostIfBASessionInfo->u8Ted);
+
+	strWID.u16WIDid = (u16)WID_DEL_ALL_RX_BA;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = (u8 *)WILC_MALLOC(BLOCK_ACK_REQ_SIZE);
+	strWID.s32ValueSize = BLOCK_ACK_REQ_SIZE;
+	ptr = strWID.ps8WidVal;
+	*ptr++ = 0x14;
+	*ptr++ = 0x3;
+	*ptr++ = 0x2;
+	WILC_memcpy(ptr, strHostIfBASessionInfo->au8Bssid, ETH_ALEN);
+	ptr += ETH_ALEN;
+	*ptr++ = strHostIfBASessionInfo->u8Ted;
+	/* BA direction = recipent*/
+	*ptr++ = 0;
+	/* Delba Reason */
+	*ptr++ = 32; /* Unspecific QOS reason */
+
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error)
+		PRINT_D(HOSTINF_DBG, "Couldn't delete BA Session\n");
+
+
+	if (strWID.ps8WidVal != NULL)
+		WILC_FREE(strWID.ps8WidVal);
+
+	/*BugID_5222*/
+	up(&hWaitResponse);
+
+	return s32Error;
+
+}
+
+/**
+ *  @brief hostIFthread
+ *  @details        Main thread to handle message queue requests
+ *  @param[in]   void* pvArg
+ *  @return         NONE
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+static int hostIFthread(void *pvArg)
+{
+	u32 u32Ret;
+	tstrHostIFmsg strHostIFmsg;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	while (1) {
+		WILC_MsgQueueRecv(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), &u32Ret, NULL);
+		pstrWFIDrv = (tstrWILC_WFIDrv *)strHostIFmsg.drvHandler;
+		if (strHostIFmsg.u16MsgId == HOST_IF_MSG_EXIT) {
+			PRINT_D(GENERIC_DBG, "THREAD: Exiting HostIfThread\n");
+			break;
+		}
+
+
+		/*Re-Queue HIF message*/
+		if ((!g_wilc_initialized)) {
+			PRINT_D(GENERIC_DBG, "--WAIT--");
+			WILC_Sleep(200);
+			WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+			continue;
+		}
+
+		if (strHostIFmsg.u16MsgId == HOST_IF_MSG_CONNECT && pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult != NULL) {
+			PRINT_D(HOSTINF_DBG, "Requeue connect request till scan done received\n");
+			WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+			WILC_Sleep(2);
+			continue;
+		}
+
+		switch (strHostIFmsg.u16MsgId) {
+		case HOST_IF_MSG_Q_IDLE:
+			Handle_wait_msg_q_empty();
+			break;
+
+		case HOST_IF_MSG_SCAN:
+			Handle_Scan(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr);
+			break;
+
+		case HOST_IF_MSG_CONNECT:
+			Handle_Connect(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr);
+			break;
+
+		/*BugID_5137*/
+		case HOST_IF_MSG_FLUSH_CONNECT:
+			Handle_FlushConnect(strHostIFmsg.drvHandler);
+			break;
+
+		case HOST_IF_MSG_RCVD_NTWRK_INFO:
+			Handle_RcvdNtwrkInfo(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strRcvdNetworkInfo);
+			break;
+
+		case HOST_IF_MSG_RCVD_GNRL_ASYNC_INFO:
+			Handle_RcvdGnrlAsyncInfo(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strRcvdGnrlAsyncInfo);
+			break;
+
+		case HOST_IF_MSG_KEY:
+			Handle_Key(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr);
+			break;
+
+		case HOST_IF_MSG_CFG_PARAMS:
+
+			Handle_CfgParam(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIFCfgParamAttr);
+			break;
+
+		case HOST_IF_MSG_SET_CHANNEL:
+			Handle_SetChannel(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIFSetChan);
+			break;
+
+		case HOST_IF_MSG_DISCONNECT:
+			Handle_Disconnect(strHostIFmsg.drvHandler);
+			break;
+
+		case HOST_IF_MSG_RCVD_SCAN_COMPLETE:
+			WILC_TimerStop(&(pstrWFIDrv->hScanTimer), NULL);
+			PRINT_D(HOSTINF_DBG, "scan completed successfully\n");
+
+			/*BugID_5213*/
+			/*Allow chip sleep, only if both interfaces are not connected*/
+			if (!linux_wlan_get_num_conn_ifcs()) {
+				chip_sleep_manually(INFINITE_SLEEP_TIME);
+			}
+
+			Handle_ScanDone(strHostIFmsg.drvHandler, SCAN_EVENT_DONE);
+
+				#ifdef WILC_P2P
+			if (pstrWFIDrv->u8RemainOnChan_pendingreq)
+				Handle_RemainOnChan(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan);
+				#endif
+
+			break;
+
+		case HOST_IF_MSG_GET_RSSI:
+			Handle_GetRssi(strHostIFmsg.drvHandler);
+			break;
+
+		case HOST_IF_MSG_GET_LINKSPEED:
+			Handle_GetLinkspeed(strHostIFmsg.drvHandler);
+			break;
+
+		case HOST_IF_MSG_GET_STATISTICS:
+			Handle_GetStatistics(strHostIFmsg.drvHandler, (tstrStatistics *)strHostIFmsg.uniHostIFmsgBody.pUserData);
+			break;
+
+		case HOST_IF_MSG_GET_CHNL:
+			Handle_GetChnl(strHostIFmsg.drvHandler);
+			break;
+
+#ifdef WILC_AP_EXTERNAL_MLME
+		case HOST_IF_MSG_ADD_BEACON:
+			Handle_AddBeacon(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIFSetBeacon);
+			break;
+
+		case HOST_IF_MSG_DEL_BEACON:
+			Handle_DelBeacon(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIFDelBeacon);
+			break;
+
+		case HOST_IF_MSG_ADD_STATION:
+			Handle_AddStation(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strAddStaParam);
+			break;
+
+		case HOST_IF_MSG_DEL_STATION:
+			Handle_DelStation(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strDelStaParam);
+			break;
+
+		case HOST_IF_MSG_EDIT_STATION:
+			Handle_EditStation(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strEditStaParam);
+			break;
+
+		case HOST_IF_MSG_GET_INACTIVETIME:
+			Handle_Get_InActiveTime(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfStaInactiveT);
+			break;
+
+#endif /*WILC_AP_EXTERNAL_MLME*/
+		case HOST_IF_MSG_SCAN_TIMER_FIRED:
+			PRINT_D(HOSTINF_DBG, "Scan Timeout\n");
+
+			Handle_ScanDone(strHostIFmsg.drvHandler, SCAN_EVENT_ABORTED);
+			break;
+
+		case HOST_IF_MSG_CONNECT_TIMER_FIRED:
+			PRINT_D(HOSTINF_DBG, "Connect Timeout \n");
+			Handle_ConnectTimeout(strHostIFmsg.drvHandler);
+			break;
+
+		case HOST_IF_MSG_POWER_MGMT:
+			Handle_PowerManagement(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strPowerMgmtparam);
+			break;
+
+		case HOST_IF_MSG_SET_WFIDRV_HANDLER:
+			Handle_SetWfiDrvHandler(&strHostIFmsg.uniHostIFmsgBody.strHostIfSetDrvHandler);
+			break;
+
+		case HOST_IF_MSG_SET_OPERATION_MODE:
+			Handle_SetOperationMode(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfSetOperationMode);
+			break;
+
+		case HOST_IF_MSG_SET_IPADDRESS:
+			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_SET_IPADDRESS\n");
+			Handle_set_IPAddress(strHostIFmsg.drvHandler, strHostIFmsg.uniHostIFmsgBody.strHostIfSetIP.au8IPAddr, strHostIFmsg.uniHostIFmsgBody.strHostIfSetIP.idx);
+			break;
+
+		case HOST_IF_MSG_GET_IPADDRESS:
+			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_SET_IPADDRESS\n");
+			Handle_get_IPAddress(strHostIFmsg.drvHandler, strHostIFmsg.uniHostIFmsgBody.strHostIfSetIP.au8IPAddr, strHostIFmsg.uniHostIFmsgBody.strHostIfSetIP.idx);
+			break;
+
+		/*BugID_5077*/
+		case HOST_IF_MSG_SET_MAC_ADDRESS:
+			Handle_SetMacAddress(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfSetMacAddress);
+			break;
+
+		/*BugID_5213*/
+		case HOST_IF_MSG_GET_MAC_ADDRESS:
+			Handle_GetMacAddress(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfGetMacAddress);
+			break;
+
+#ifdef WILC_P2P
+		case HOST_IF_MSG_REMAIN_ON_CHAN:
+			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_REMAIN_ON_CHAN\n");
+			Handle_RemainOnChan(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan);
+			break;
+
+		case HOST_IF_MSG_REGISTER_FRAME:
+			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_REGISTER_FRAME\n");
+			Handle_RegisterFrame(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfRegisterFrame);
+			break;
+
+		case HOST_IF_MSG_LISTEN_TIMER_FIRED:
+			Handle_ListenStateExpired(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan);
+			break;
+
+			#endif
+		case HOST_IF_MSG_SET_MULTICAST_FILTER:
+			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_SET_MULTICAST_FILTER\n");
+			Handle_SetMulticastFilter(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfSetMulti);
+			break;
+
+		/*BugID_5222*/
+		case HOST_IF_MSG_ADD_BA_SESSION:
+			Handle_AddBASession(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfBASessionInfo);
+			break;
+
+		case HOST_IF_MSG_DEL_ALL_RX_BA_SESSIONS:
+			Handle_DelAllRxBASessions(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIfBASessionInfo);
+			break;
+
+		case HOST_IF_MSG_DEL_ALL_STA:
+			Handle_DelAllSta(strHostIFmsg.drvHandler, &strHostIFmsg.uniHostIFmsgBody.strHostIFDelAllSta);
+			break;
+
+		default:
+			PRINT_ER("[Host Interface] undefined Received Msg ID  \n");
+			break;
+		}
+	}
+
+	PRINT_D(HOSTINF_DBG, "Releasing thread exit semaphore\n");
+	up(&hSemHostIFthrdEnd);
+	return 0;
+}
+
+static void TimerCB_Scan(void *pvArg)
+{
+	tstrHostIFmsg strHostIFmsg;
+
+	/* prepare the Timer Callback message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.drvHandler = pvArg;
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_SCAN_TIMER_FIRED;
+
+	/* send the message */
+	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+}
+
+static void TimerCB_Connect(void *pvArg)
+{
+	tstrHostIFmsg strHostIFmsg;
+
+	/* prepare the Timer Callback message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.drvHandler = pvArg;
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_CONNECT_TIMER_FIRED;
+
+	/* send the message */
+	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+}
+
+
+/**
+ *  @brief              removes wpa/wpa2 keys
+ *  @details    only in BSS STA mode if External Supplicant support is enabled.
+ *                              removes all WPA/WPA2 station key entries from MAC hardware.
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  6 bytes of Station Adress in the station entry table
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+/* Check implementation in core adding 9 bytes to the input! */
+s32 host_int_remove_key(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8StaAddress)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	strWID.u16WIDid	= (u16)WID_REMOVE_KEY;
+	strWID.enuWIDtype	= WID_STR;
+	strWID.ps8WidVal	= (s8 *)pu8StaAddress;
+	strWID.s32ValueSize = 6;
+
+	return s32Error;
+
+}
+
+/**
+ *  @brief              removes WEP key
+ *  @details    valid only in BSS STA mode if External Supplicant support is enabled.
+ *                              remove a WEP key entry from MAC HW.
+ *                              The BSS Station automatically finds the index of the entry using its
+ *                              BSS ID and removes that entry from the MAC hardware.
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  6 bytes of Station Adress in the station entry table
+ *  @return             Error code indicating success/failure
+ *  @note               NO need for the STA add since it is not used for processing
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_remove_wep_key(WILC_WFIDrvHandle hWFIDrv, u8 u8keyIdx)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/* prepare the Remove Wep Key Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_KEY;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.enuKeyType = WEP;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = REMOVEKEY;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx = u8keyIdx;
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error)
+		PRINT_ER("Error in sending message queue : Request to remove WEP key \n");
+	down(&(pstrWFIDrv->hSemTestKeyBlock));
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief              sets WEP default key
+ *  @details    Sets the index of the WEP encryption key in use,
+ *                              in the key table
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  key index ( 0, 1, 2, 3)
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_WEPDefaultKeyID(WILC_WFIDrvHandle hWFIDrv, u8 u8Index)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/* prepare the Key Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_KEY;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.enuKeyType = WEP;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = DEFAULTKEY;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx = u8Index;
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error)
+		PRINT_ER("Error in sending message queue : Default key index\n");
+	down(&(pstrWFIDrv->hSemTestKeyBlock));
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              sets WEP deafault key
+ *  @details    valid only in BSS STA mode if External Supplicant support is enabled.
+ *                              sets WEP key entry into MAC hardware when it receives the
+ *                              corresponding request from NDIS.
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing WEP Key in the following format
+ *|---------------------------------------|
+ *|Key ID Value | Key Length |	Key		|
+ *|-------------|------------|------------|
+ |	1byte	  |		1byte  | Key Length	|
+ ||---------------------------------------|
+ |
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_add_wep_key_bss_sta(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8WepKey, u8 u8WepKeylen, u8 u8Keyidx)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+
+	}
+
+	/* prepare the Key Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_KEY;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.enuKeyType = WEP;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = ADDKEY;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.pu8WepKey = (u8 *)WILC_MALLOC(u8WepKeylen);
+
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwepAttr.pu8WepKey,
+		    pu8WepKey, u8WepKeylen);
+
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen = (u8WepKeylen);
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx = u8Keyidx;
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error)
+		PRINT_ER("Error in sending message queue :WEP Key\n");
+	down(&(pstrWFIDrv->hSemTestKeyBlock));
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+
+}
+
+#ifdef WILC_AP_EXTERNAL_MLME
+/**
+ *
+ *  @brief              host_int_add_wep_key_bss_ap
+ *  @details    valid only in BSS AP mode if External Supplicant support is enabled.
+ *                              sets WEP key entry into MAC hardware when it receives the
+ *
+ *                              corresponding request from NDIS.
+ *  @param[in,out] handle to the wifi driver
+ *
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mdaftedar
+ *  @date		28 FEB 2013
+ *  @version		1.0
+ */
+s32 host_int_add_wep_key_bss_ap(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8WepKey, u8 u8WepKeylen, u8 u8Keyidx, u8 u8mode, AUTHTYPE_T tenuAuth_type)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	u8 i;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+
+	}
+
+	/* prepare the Key Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	if (INFO) {
+		for (i = 0; i < u8WepKeylen; i++)
+			PRINT_INFO(HOSTAPD_DBG, "KEY is %x\n", pu8WepKey[i]);
+	}
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_KEY;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.enuKeyType = WEP;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = ADDKEY_AP;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.pu8WepKey = (u8 *)WILC_MALLOC((u8WepKeylen));
+
+
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwepAttr.pu8WepKey,
+		    pu8WepKey, (u8WepKeylen));
+
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.u8WepKeylen = (u8WepKeylen);
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.u8Wepidx = u8Keyidx;
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.u8mode = u8mode;
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwepAttr.tenuAuth_type = tenuAuth_type;
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+
+	if (s32Error)
+		PRINT_ER("Error in sending message queue :WEP Key\n");
+	down(&(pstrWFIDrv->hSemTestKeyBlock));
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+
+}
+#endif
+/**
+ *  @brief              adds ptk Key
+ *  @details
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing PTK Key in the following format
+ *|-----------------------------------------------------------------------------|
+ *|Station address | Key Length |	Temporal Key | Rx Michael Key |Tx Michael Key |
+ *|----------------|------------|--------------|----------------|---------------|
+ |	6 bytes		 |	1byte	  |   16 bytes	 |	  8 bytes	  |	   8 bytes	  |
+ ||-----------------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_add_ptk(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8Ptk, u8 u8PtkKeylen,
+			     const u8 *mac_addr, const u8 *pu8RxMic, const u8 *pu8TxMic, u8 mode, u8 u8Ciphermode, u8 u8Idx)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	u8 u8KeyLen = u8PtkKeylen;
+	u32 i;
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+	if (pu8RxMic != NULL) {
+		u8KeyLen += RX_MIC_KEY_LEN;
+	}
+	if (pu8TxMic != NULL) {
+		u8KeyLen += TX_MIC_KEY_LEN;
+	}
+
+	/* prepare the Key Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_KEY;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.enuKeyType = WPAPtk;
+	#ifdef WILC_AP_EXTERNAL_MLME
+	if (mode == AP_MODE) {
+		strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = ADDKEY_AP;
+		strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+		uniHostIFkeyAttr.strHostIFwpaAttr.u8keyidx = u8Idx;
+	}
+	#endif
+	if (mode == STATION_MODE)
+		strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = ADDKEY;
+
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwpaAttr.pu8key = (u8 *)WILC_MALLOC(u8PtkKeylen);
+
+
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwpaAttr.pu8key,
+		    pu8Ptk, u8PtkKeylen);
+
+	if (pu8RxMic != NULL) {
+
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwpaAttr.pu8key + 16,
+			    pu8RxMic, RX_MIC_KEY_LEN);
+		if (INFO) {
+			for (i = 0; i < RX_MIC_KEY_LEN; i++)
+				PRINT_INFO(CFG80211_DBG, "PairwiseRx[%d] = %x\n", i, pu8RxMic[i]);
+		}
+	}
+	if (pu8TxMic != NULL) {
+
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwpaAttr.pu8key + 24,
+			    pu8TxMic, TX_MIC_KEY_LEN);
+		if (INFO) {
+			for (i = 0; i < TX_MIC_KEY_LEN; i++)
+				PRINT_INFO(CFG80211_DBG, "PairwiseTx[%d] = %x\n", i, pu8TxMic[i]);
+		}
+	}
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen = u8KeyLen;
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwpaAttr.u8Ciphermode = u8Ciphermode;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwpaAttr.pu8macaddr = mac_addr;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+
+	if (s32Error)
+		PRINT_ER("Error in sending message queue:  PTK Key\n");
+
+	/* ////////////// */
+	down(&(pstrWFIDrv->hSemTestKeyBlock));
+	/* WILC_Sleep(100); */
+	/* /////// */
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              adds Rx GTk Key
+ *  @details
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  pu8RxGtk : contains temporal key | Rx Mic | Tx Mic
+ *                              u8GtkKeylen :The total key length
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_add_rx_gtk(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8RxGtk, u8 u8GtkKeylen,
+				u8 u8KeyIdx, u32 u32KeyRSClen, const u8 *KeyRSC,
+				const u8 *pu8RxMic, const u8 *pu8TxMic, u8 mode, u8 u8Ciphermode)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	u8 u8KeyLen = u8GtkKeylen;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+	/* prepare the Key Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	if (pu8RxMic != NULL) {
+		u8KeyLen += RX_MIC_KEY_LEN;
+	}
+	if (pu8TxMic != NULL) {
+		u8KeyLen += TX_MIC_KEY_LEN;
+	}
+	if (KeyRSC != NULL) {
+		strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+		uniHostIFkeyAttr.strHostIFwpaAttr.pu8seq = (u8 *)WILC_MALLOC(u32KeyRSClen);
+
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwpaAttr.pu8seq,
+			    KeyRSC, u32KeyRSClen);
+	}
+
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_KEY;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.enuKeyType = WPARxGtk;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+    #ifdef WILC_AP_EXTERNAL_MLME
+	if (mode == AP_MODE) {
+		strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = ADDKEY_AP;
+		strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwpaAttr.u8Ciphermode = u8Ciphermode;
+	}
+    #endif
+	if (mode == STATION_MODE)
+		strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = ADDKEY;
+
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwpaAttr.pu8key = (u8 *)WILC_MALLOC(u8KeyLen);
+
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwpaAttr.pu8key,
+		    pu8RxGtk, u8GtkKeylen);
+
+	if (pu8RxMic != NULL) {
+
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwpaAttr.pu8key + 16,
+			    pu8RxMic, RX_MIC_KEY_LEN);
+
+	}
+	if (pu8TxMic != NULL) {
+
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFwpaAttr.pu8key + 24,
+			    pu8TxMic, TX_MIC_KEY_LEN);
+
+	}
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwpaAttr.u8keyidx = u8KeyIdx;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwpaAttr.u8Keylen = u8KeyLen;
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.
+	uniHostIFkeyAttr.strHostIFwpaAttr.u8seqlen = u32KeyRSClen;
+
+
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error)
+		PRINT_ER("Error in sending message queue:  RX GTK\n");
+	/* ////////////// */
+	down(&(pstrWFIDrv->hSemTestKeyBlock));
+	/* WILC_Sleep(100); */
+	/* /////// */
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief              host_int_set_pmkid_info
+ *  @details    caches the pmkid valid only in BSS STA mode if External Supplicant
+ *                              support is enabled. This Function sets the PMKID in firmware
+ *                              when host drivr receives the corresponding request from NDIS.
+ *                              The firmware then includes theset PMKID in the appropriate
+ *                              management frames
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing PMKID Info in the following format
+ *|-----------------------------------------------------------------|
+ *|NumEntries |	BSSID[1] | PMKID[1] |  ...	| BSSID[K] | PMKID[K] |
+ *|-----------|------------|----------|-------|----------|----------|
+ |	   1	|		6	 |   16		|  ...	|	 6	   |	16	  |
+ ||-----------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_pmkid_info(WILC_WFIDrvHandle hWFIDrv, tstrHostIFpmkidAttr *pu8PmkidInfoArray)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	u32 i;
+
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/* prepare the Key Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_KEY;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.enuKeyType = PMKSA;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.u8KeyAction = ADDKEY;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	for (i = 0; i < pu8PmkidInfoArray->numpmkid; i++) {
+
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFpmkidAttr.pmkidlist[i].bssid, &pu8PmkidInfoArray->pmkidlist[i].bssid,
+			    ETH_ALEN);
+
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFkeyAttr.uniHostIFkeyAttr.strHostIFpmkidAttr.pmkidlist[i].pmkid, &pu8PmkidInfoArray->pmkidlist[i].pmkid,
+			    PMKID_LEN);
+	}
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error)
+		PRINT_ER(" Error in sending messagequeue: PMKID Info\n");
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              gets the cached the pmkid info
+ *  @details    valid only in BSS STA mode if External Supplicant
+ *                              support is enabled. This Function sets the PMKID in firmware
+ *                              when host drivr receives the corresponding request from NDIS.
+ *                              The firmware then includes theset PMKID in the appropriate
+ *                              management frames
+ *  @param[in,out] handle to the wifi driver,
+ *                                message containing PMKID Info in the following format
+ *|-----------------------------------------------------------------|
+ *|NumEntries |	BSSID[1] | PMKID[1] |  ...	| BSSID[K] | PMKID[K] |
+ *|-----------|------------|----------|-------|----------|----------|
+ |	   1	|		6	 |   16		|  ...	|	 6	   |	16	  |
+ ||-----------------------------------------------------------------|
+ *  @param[in]
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_pmkid_info(WILC_WFIDrvHandle hWFIDrv, u8 *pu8PmkidInfoArray,
+				    u32 u32PmkidInfoLen)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	strWID.u16WIDid	= (u16)WID_PMKID_INFO;
+	strWID.enuWIDtype	= WID_STR;
+	strWID.s32ValueSize = u32PmkidInfoLen;
+	strWID.ps8WidVal = pu8PmkidInfoArray;
+
+	return s32Error;
+}
+
+/**
+ *  @brief              sets the pass phrase
+ *  @details    AP/STA mode. This function gives the pass phrase used to
+ *                              generate the Pre-Shared Key when WPA/WPA2 is enabled
+ *                              The length of the field can vary from 8 to 64 bytes,
+ *                              the lower layer should get the
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]   String containing PSK
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_RSNAConfigPSKPassPhrase(WILC_WFIDrvHandle hWFIDrv, u8 *pu8PassPhrase,
+						 u8 u8Psklength)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	/* u8 u8Psklength = WILC_strlen(pu8PassPhrase); */
+	/*validating psk length*/
+	if ((u8Psklength > 7) && (u8Psklength < 65)) {
+		strWID.u16WIDid	= (u16)WID_11I_PSK;
+		strWID.enuWIDtype	= WID_STR;
+		strWID.ps8WidVal	= pu8PassPhrase;
+		strWID.s32ValueSize = u8Psklength;
+	}
+
+	return s32Error;
+}
+/**
+ *  @brief              host_int_get_MacAddress
+ *  @details	gets mac address
+ *  @param[in,out] handle to the wifi driver,
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mdaftedar
+ *  @date		19 April 2012
+ *  @version		1.0
+ */
+s32 host_int_get_MacAddress(WILC_WFIDrvHandle hWFIDrv, u8 *pu8MacAddress)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+
+
+	/* prepare the Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_GET_MAC_ADDRESS;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfGetMacAddress.u8MacAddress = pu8MacAddress;
+	strHostIFmsg.drvHandler = hWFIDrv;
+	/* send the message */
+	s32Error =	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Failed to send get mac address\n");
+		return WILC_FAIL;
+	}
+
+	down(&hWaitResponse);
+	return s32Error;
+}
+
+/**
+ *  @brief              host_int_set_MacAddress
+ *  @details	sets mac address
+ *  @param[in,out] handle to the wifi driver,
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mabubakr
+ *  @date		16 July 2012
+ *  @version		1.0
+ */
+s32 host_int_set_MacAddress(WILC_WFIDrvHandle hWFIDrv, u8 *pu8MacAddress)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+
+	PRINT_D(GENERIC_DBG, "mac addr = %x:%x:%x\n", pu8MacAddress[0], pu8MacAddress[1], pu8MacAddress[2]);
+
+	/* prepare setting mac address message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_SET_MAC_ADDRESS;
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIfSetMacAddress.u8MacAddress, pu8MacAddress, ETH_ALEN);
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Failed to send message queue: Set mac address\n");
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+
+}
+
+/**
+ *  @brief              host_int_get_RSNAConfigPSKPassPhrase
+ *  @details    gets the pass phrase:AP/STA mode. This function gets the pass phrase used to
+ *                              generate the Pre-Shared Key when WPA/WPA2 is enabled
+ *                              The length of the field can vary from 8 to 64 bytes,
+ *                              the lower layer should get the
+ *  @param[in,out] handle to the wifi driver,
+ *                                String containing PSK
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_RSNAConfigPSKPassPhrase(WILC_WFIDrvHandle hWFIDrv,
+						 u8 *pu8PassPhrase, u8 u8Psklength)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	strWID.u16WIDid	= (u16)WID_11I_PSK;
+	strWID.enuWIDtype	= WID_STR;
+	strWID.s32ValueSize = u8Psklength;
+	strWID.ps8WidVal	= pu8PassPhrase;
+
+	return s32Error;
+}
+
+/**
+ *  @brief                      host_int_get_site_survey_results
+ *  @details            gets the site survey results
+ *  @param[in,out] handle to the wifi driver,
+ *                                Message containing  site survey results in the
+ *                                following format
+ *|---------------------------------------------------|
+ | MsgLength | fragNo.	| MsgBodyLength	| MsgBody	|
+ ||-----------|-----------|---------------|-----------|
+ |	 1		|	  1		|		1		|	 1		|
+ | -----------------------------------------	 |  ----------------
+ |
+ ||---------------------------------------|
+ | Network1 | Netweork2 | ... | Network5 |
+ ||---------------------------------------|
+ |	44	   |	44	   | ... |	 44		|
+ | -------------------------- | ---------------------------------------
+ |
+ ||---------------------------------------------------------------------|
+ | SSID | BSS Type | Channel | Security Status| BSSID | RSSI |Reserved |
+ |
+ |
+ ||------|----------|---------|----------------|-------|------|---------|
+ |  33  |	 1	  |	  1		|		1		 |	  6	 |	 1	|	 1	  |
+ ||---------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+#ifndef CONNECT_DIRECT
+s32 host_int_get_site_survey_results(WILC_WFIDrvHandle hWFIDrv,
+					     u8 ppu8RcvdSiteSurveyResults[][MAX_SURVEY_RESULT_FRAG_SIZE],
+					     u32 u32MaxSiteSrvyFragLen)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID astrWIDList[2];
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+	astrWIDList[0].u16WIDid = (u16)WID_SITE_SURVEY_RESULTS;
+	astrWIDList[0].enuWIDtype = WID_STR;
+	astrWIDList[0].ps8WidVal = ppu8RcvdSiteSurveyResults[0];
+	astrWIDList[0].s32ValueSize = u32MaxSiteSrvyFragLen;
+
+	astrWIDList[1].u16WIDid = (u16)WID_SITE_SURVEY_RESULTS;
+	astrWIDList[1].enuWIDtype = WID_STR;
+	astrWIDList[1].ps8WidVal = ppu8RcvdSiteSurveyResults[1];
+	astrWIDList[1].s32ValueSize = u32MaxSiteSrvyFragLen;
+
+	s32Error = SendConfigPkt(GET_CFG, astrWIDList, 2, true, (u32)pstrWFIDrv);
+
+	/*get the value by searching the local copy*/
+	if (s32Error) {
+		PRINT_ER("Failed to send config packet to get survey results\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+#endif
+
+/**
+ *  @brief              sets a start scan request
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Scan Source one of the following values
+ *                              DEFAULT_SCAN        0
+ *                              USER_SCAN           BIT0
+ *                              OBSS_PERIODIC_SCAN  BIT1
+ *                              OBSS_ONETIME_SCAN   BIT2
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_start_scan_req(WILC_WFIDrvHandle hWFIDrv, u8 scanSource)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	strWID.u16WIDid = (u16)WID_START_SCAN_REQ;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = (s8 *)&scanSource;
+	strWID.s32ValueSize = sizeof(char);
+
+	return s32Error;
+}
+
+/**
+ *  @brief                      host_int_get_start_scan_req
+ *  @details            gets a start scan request
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Scan Source one of the following values
+ *                              DEFAULT_SCAN        0
+ *                              USER_SCAN           BIT0
+ *                              OBSS_PERIODIC_SCAN  BIT1
+ *                              OBSS_ONETIME_SCAN   BIT2
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_get_start_scan_req(WILC_WFIDrvHandle hWFIDrv, u8 *pu8ScanSource)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	strWID.u16WIDid = (u16)WID_START_SCAN_REQ;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = (s8 *)pu8ScanSource;
+	strWID.s32ValueSize = sizeof(char);
+
+	return s32Error;
+}
+
+/**
+ *  @brief                      host_int_set_join_req
+ *  @details            sets a join request
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Index of the bss descriptor
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_join_req(WILC_WFIDrvHandle hWFIDrv, u8 *pu8bssid,
+				  const u8 *pu8ssid, size_t ssidLen,
+				  const u8 *pu8IEs, size_t IEsLen,
+				  tWILCpfConnectResult pfConnectResult, void *pvUserArg,
+				  u8 u8security, AUTHTYPE_T tenuAuth_type,
+				  u8 u8channel,
+				  void *pJoinParams)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tenuScanConnTimer enuScanConnTimer;
+
+	if (pstrWFIDrv == NULL || pfConnectResult == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	if (hWFIDrv == NULL) {
+		PRINT_ER("Driver not initialized: gWFiDrvHandle = NULL\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	if (pJoinParams == NULL) {
+		PRINT_ER("Unable to Join - JoinParams is NULL\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+
+	}
+/*
+ *      if(gWFiDrvHandle->strWILC_UsrScanReq.u32RcvdChCount == 0)
+ *      {
+ *              PRINT_ER("No scan results exist: Scanning should be done\n");
+ *              WILC_ERRORREPORT(s32Error, WILC_FAIL);
+ *      }
+ */
+	/* prepare the Connect Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_CONNECT;
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.u8security = u8security;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.tenuAuth_type = tenuAuth_type;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.u8channel = u8channel;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pfConnectResult = pfConnectResult;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pvUserArg = pvUserArg;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pJoinParams = pJoinParams;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	if (pu8bssid != NULL) {
+		strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pu8bssid = (u8 *)WILC_MALLOC(6); /* will be deallocated by the receiving thread */
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pu8bssid,
+			    pu8bssid, 6);
+	}
+
+	if (pu8ssid != NULL) {
+		strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.ssidLen = ssidLen;
+		strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pu8ssid = (u8 *)WILC_MALLOC(ssidLen); /* will be deallocated by the receiving thread */
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pu8ssid,
+
+			    pu8ssid, ssidLen);
+	}
+
+	if (pu8IEs != NULL) {
+		strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.IEsLen = IEsLen;
+		strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pu8IEs = (u8 *)WILC_MALLOC(IEsLen); /* will be deallocated by the receiving thread */
+		WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFconnectAttr.pu8IEs,
+			    pu8IEs, IEsLen);
+	}
+	if (pstrWFIDrv->enuHostIFstate < HOST_IF_CONNECTING) {
+		pstrWFIDrv->enuHostIFstate = HOST_IF_CONNECTING;
+	} else
+		PRINT_D(GENERIC_DBG, "Don't set state to 'connecting' as state is %d\n", pstrWFIDrv->enuHostIFstate);
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Failed to send message queue: Set join request\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	enuScanConnTimer = CONNECT_TIMER;
+	WILC_TimerStart(&(pstrWFIDrv->hConnectTimer), HOST_IF_CONNECT_TIMEOUT, (void *) hWFIDrv, NULL);
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              Flush a join request parameters to FW, but actual connection
+ *  @details    The function is called in situation where WILC is connected to AP and
+ *                      required to switch to hybrid FW for P2P connection
+ *  @param[in] handle to the wifi driver,
+ *  @return     Error code indicating success/failure
+ *  @note
+ *  @author	Amr Abdel-Moghny
+ *  @date		19 DEC 2013
+ *  @version	8.0
+ */
+
+s32 host_int_flush_join_req(WILC_WFIDrvHandle hWFIDrv)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (!gu8FlushedJoinReq)	{
+		s32Error = WILC_FAIL;
+		return s32Error;
+	}
+
+
+	if (hWFIDrv  == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_FLUSH_CONNECT;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Failed to send message queue: Flush join request\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief                      host_int_disconnect
+ *  @details            disconnects from the currently associated network
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Reason Code of the Disconnection
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_disconnect(WILC_WFIDrvHandle hWFIDrv, u16 u16ReasonCode)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+	if (pstrWFIDrv == NULL) {
+		PRINT_ER("Driver not initialized: pstrWFIDrv = NULL \n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	if (pstrWFIDrv == NULL)	{
+		PRINT_ER("gWFiDrvHandle = NULL\n");
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	/* prepare the Disconnect Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_DISCONNECT;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error)
+		PRINT_ER("Failed to send message queue: disconnect\n");
+	/* ////////////// */
+	down(&(pstrWFIDrv->hSemTestDisconnectBlock));
+	/* /////// */
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              host_int_disconnect_station
+ *  @details     disconnects a sta
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Association Id of the station to be disconnected
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_disconnect_station(WILC_WFIDrvHandle hWFIDrv, u8 assoc_id)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	strWID.u16WIDid = (u16)WID_DISCONNECT;
+	strWID.enuWIDtype = WID_CHAR;
+	strWID.ps8WidVal = (s8 *)&assoc_id;
+	strWID.s32ValueSize = sizeof(char);
+
+	return s32Error;
+}
+
+/**
+ *  @brief                      host_int_get_assoc_req_info
+ *  @details            gets a Association request info
+ *  @param[in,out] handle to the wifi driver,
+ *                              Message containg assoc. req info in the following format
+ * ------------------------------------------------------------------------
+ |                        Management Frame Format                    |
+ ||-------------------------------------------------------------------|
+ ||Frame Control|Duration|DA|SA|BSSID|Sequence Control|Frame Body|FCS |
+ ||-------------|--------|--|--|-----|----------------|----------|----|
+ | 2           |2       |6 |6 |6    |		2       |0 - 2312  | 4  |
+ ||-------------------------------------------------------------------|
+ |                                                                   |
+ |             Association Request Frame - Frame Body                |
+ ||-------------------------------------------------------------------|
+ | Capability Information | Listen Interval | SSID | Supported Rates |
+ ||------------------------|-----------------|------|-----------------|
+ |			2            |		 2         | 2-34 |		3-10        |
+ | ---------------------------------------------------------------------
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_get_assoc_req_info(WILC_WFIDrvHandle hWFIDrv, u8 *pu8AssocReqInfo,
+					u32 u32AssocReqInfoLen)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	strWID.u16WIDid = (u16)WID_ASSOC_REQ_INFO;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = pu8AssocReqInfo;
+	strWID.s32ValueSize = u32AssocReqInfoLen;
+
+
+	return s32Error;
+}
+
+/**
+ *  @brief              gets a Association Response info
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                              Message containg assoc. resp info
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_assoc_res_info(WILC_WFIDrvHandle hWFIDrv, u8 *pu8AssocRespInfo,
+					u32 u32MaxAssocRespInfoLen, u32 *pu32RcvdAssocRespInfoLen)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+	if (pstrWFIDrv == NULL) {
+		PRINT_ER("Driver not initialized: pstrWFIDrv = NULL \n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	strWID.u16WIDid = (u16)WID_ASSOC_RES_INFO;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = pu8AssocRespInfo;
+	strWID.s32ValueSize = u32MaxAssocRespInfoLen;
+
+
+	/* Sending Configuration packet */
+	s32Error = SendConfigPkt(GET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Failed to send association response config packet\n");
+		*pu32RcvdAssocRespInfoLen = 0;
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	} else {
+		*pu32RcvdAssocRespInfoLen = strWID.s32ValueSize;
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief              gets a Association Response info
+ *  @details    Valid only in STA mode. This function gives the RSSI
+ *                              values observed in all the channels at the time of scanning.
+ *                              The length of the field is 1 greater that the total number of
+ *                              channels supported. Byte 0 contains the number of channels while
+ *                              each of Byte N contains	the observed RSSI value for the channel index N.
+ *  @param[in,out] handle to the wifi driver,
+ *                              array of scanned channels' RSSI
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_rx_power_level(WILC_WFIDrvHandle hWFIDrv, u8 *pu8RxPowerLevel,
+					u32 u32RxPowerLevelLen)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID strWID;
+	/* tstrWILC_WFIDrv * pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv; */
+
+	strWID.u16WIDid = (u16)WID_RX_POWER_LEVEL;
+	strWID.enuWIDtype = WID_STR;
+	strWID.ps8WidVal = pu8RxPowerLevel;
+	strWID.s32ValueSize = u32RxPowerLevelLen;
+
+
+	return s32Error;
+}
+
+/**
+ *  @brief              sets a channel
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Index of the channel to be set
+ *|-------------------------------------------------------------------|
+ |          CHANNEL1      CHANNEL2 ....		             CHANNEL14	|
+ |  Input:         1             2					            14	|
+ ||-------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_mac_chnl_num(WILC_WFIDrvHandle hWFIDrv, u8 u8ChNum)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/* prepare the set channel message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_SET_CHANNEL;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFSetChan.u8SetChan = u8ChNum;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+
+s32 host_int_wait_msg_queue_idle(void)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	tstrHostIFmsg strHostIFmsg;
+
+	/* prepare the set driver handler message */
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_Q_IDLE;
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	/* wait untill MSG Q is empty */
+	down(&hWaitResponse);
+
+	return s32Error;
+
+}
+
+s32 host_int_set_wfi_drv_handler(u32 u32address)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	tstrHostIFmsg strHostIFmsg;
+
+
+	/* prepare the set driver handler message */
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_SET_WFIDRV_HANDLER;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfSetDrvHandler.u32Address = u32address;
+	/* strHostIFmsg.drvHandler=hWFIDrv; */
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+
+
+s32 host_int_set_operation_mode(WILC_WFIDrvHandle hWFIDrv, u32 u32mode)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	tstrHostIFmsg strHostIFmsg;
+
+
+	/* prepare the set driver handler message */
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_SET_OPERATION_MODE;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfSetOperationMode.u32Mode = u32mode;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              gets the current channel index
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                              current channel index
+ *|-----------------------------------------------------------------------|
+ |          CHANNEL1      CHANNEL2 ....                     CHANNEL14	|
+ |  Input:         1             2                                 14	|
+ ||-----------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_host_chnl_num(WILC_WFIDrvHandle hWFIDrv, u8 *pu8ChNo)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		PRINT_ER("Driver not initialized: pstrWFIDrv = NULL \n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/* prepare the Get Channel Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_GET_CHNL;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/* send the message */
+	s32Error =	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error)
+		PRINT_ER("Failed to send get host channel param's message queue ");
+	down(&(pstrWFIDrv->hSemGetCHNL));
+	/* gu8Chnl = 11; */
+
+	*pu8ChNo = gu8Chnl;
+
+	WILC_CATCH(s32Error)
+	{
+	}
+
+	return s32Error;
+
+
+}
+
+
+/**
+ *  @brief                       host_int_test_set_int_wid
+ *  @details             Test function for setting wids
+ *  @param[in,out]   WILC_WFIDrvHandle hWFIDrv, u32 u32TestMemAddr
+ *  @return              Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_test_set_int_wid(WILC_WFIDrvHandle hWFIDrv, u32 u32TestMemAddr)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID	strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+
+	if (pstrWFIDrv == NULL) {
+		PRINT_ER("Driver not initialized: pstrWFIDrv = NULL \n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/*prepare configuration packet*/
+	strWID.u16WIDid = (u16)WID_MEMORY_ADDRESS;
+	strWID.enuWIDtype = WID_INT;
+	strWID.ps8WidVal = (char *)&u32TestMemAddr;
+	strWID.s32ValueSize = sizeof(u32);
+
+	/*Sending Cfg*/
+	s32Error = SendConfigPkt(SET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Test Function: Failed to set wid value\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	} else {
+		PRINT_D(HOSTINF_DBG, "Successfully set wid value\n");
+
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+}
+
+#ifdef WILC_AP_EXTERNAL_MLME
+/**
+ *  @brief              host_int_get_inactive_time
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                              current sta macaddress, inactive_time
+ *  @return
+ *  @note
+ *  @author
+ *  @date
+ *  @version		1.0
+ */
+s32 host_int_get_inactive_time(WILC_WFIDrvHandle hWFIDrv, const u8 *mac, u32 *pu32InactiveTime)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		PRINT_ER("Driver not initialized: pstrWFIDrv = NULL \n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIfStaInactiveT.mac,
+		    mac, ETH_ALEN);
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_GET_INACTIVETIME;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/* send the message */
+	s32Error =	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error)
+		PRINT_ER("Failed to send get host channel param's message queue ");
+
+	down(&(pstrWFIDrv->hSemInactiveTime));
+
+	*pu32InactiveTime = gu32InactiveTime;
+
+	WILC_CATCH(s32Error)
+	{
+	}
+
+	return s32Error;
+}
+#endif
+/**
+ *  @brief              host_int_test_get_int_wid
+ *  @details    Test function for getting wids
+ *  @param[in,out] WILC_WFIDrvHandle hWFIDrv, u32* pu32TestMemAddr
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_test_get_int_wid(WILC_WFIDrvHandle hWFIDrv, u32 *pu32TestMemAddr)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWID	strWID;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+
+	if (pstrWFIDrv == NULL) {
+		PRINT_ER("Driver not initialized: pstrWFIDrv = NULL \n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	strWID.u16WIDid = (u16)WID_MEMORY_ADDRESS;
+	strWID.enuWIDtype = WID_INT;
+	strWID.ps8WidVal = (s8 *)pu32TestMemAddr;
+	strWID.s32ValueSize = sizeof(u32);
+
+	s32Error = SendConfigPkt(GET_CFG, &strWID, 1, true, (u32)pstrWFIDrv);
+	/*get the value by searching the local copy*/
+	if (s32Error) {
+		PRINT_ER("Test Function: Failed to get wid value\n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_STATE);
+	} else {
+		PRINT_D(HOSTINF_DBG, "Successfully got wid value\n");
+
+	}
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+}
+
+
+/**
+ *  @brief              host_int_get_rssi
+ *  @details    gets the currently maintained RSSI value for the station.
+ *                              The received signal strength value in dB.
+ *                              The range of valid values is -128 to 0.
+ *  @param[in,out] handle to the wifi driver,
+ *                              rssi value in dB
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_rssi(WILC_WFIDrvHandle hWFIDrv, s8 *ps8Rssi)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+
+	/* prepare the Get RSSI Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_GET_RSSI;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/* send the message */
+	s32Error =	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Failed to send get host channel param's message queue ");
+		return WILC_FAIL;
+	}
+
+	down(&(pstrWFIDrv->hSemGetRSSI));
+
+
+	if (ps8Rssi == NULL) {
+		PRINT_ER("RSS pointer value is null");
+		return WILC_FAIL;
+	}
+
+
+	*ps8Rssi = gs8Rssi;
+
+
+	return s32Error;
+}
+
+s32 host_int_get_link_speed(WILC_WFIDrvHandle hWFIDrv, s8 *ps8lnkspd)
+{
+	tstrHostIFmsg strHostIFmsg;
+	s32 s32Error = WILC_SUCCESS;
+
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+
+
+	/* prepare the Get LINKSPEED Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_GET_LINKSPEED;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/* send the message */
+	s32Error =	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Failed to send GET_LINKSPEED to message queue ");
+		return WILC_FAIL;
+	}
+
+	down(&(pstrWFIDrv->hSemGetLINKSPEED));
+
+
+	if (ps8lnkspd == NULL) {
+		PRINT_ER("LINKSPEED pointer value is null");
+		return WILC_FAIL;
+	}
+
+
+	*ps8lnkspd = gs8lnkspd;
+
+
+	return s32Error;
+}
+
+s32 host_int_get_statistics(WILC_WFIDrvHandle hWFIDrv, tstrStatistics *pstrStatistics)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+
+
+	/* prepare the Get RSSI Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_GET_STATISTICS;
+	strHostIFmsg.uniHostIFmsgBody.pUserData = (char *)pstrStatistics;
+	strHostIFmsg.drvHandler = hWFIDrv;
+	/* send the message */
+	s32Error =	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Failed to send get host channel param's message queue ");
+		return WILC_FAIL;
+	}
+
+	down(&hWaitResponse);
+	return s32Error;
+}
+
+
+/**
+ *  @brief              host_int_scan
+ *  @details    scans a set of channels
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Scan source
+ *                              Scan Type	PASSIVE_SCAN = 0,
+ *                                                      ACTIVE_SCAN  = 1
+ *                              Channels Array
+ *                              Channels Array length
+ *                              Scan Callback function
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_scan(WILC_WFIDrvHandle hWFIDrv, u8 u8ScanSource,
+			  u8 u8ScanType, u8 *pu8ChnlFreqList,
+			  u8 u8ChnlListLen, const u8 *pu8IEs,
+			  size_t IEsLen, tWILCpfScanResult ScanResult,
+			  void *pvUserArg, tstrHiddenNetwork  *pstrHiddenNetwork)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tenuScanConnTimer enuScanConnTimer;
+
+	if (pstrWFIDrv == NULL || ScanResult == NULL)	{
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+
+	/* prepare the Scan Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_SCAN;
+
+	if (pstrHiddenNetwork != NULL) {
+		strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.strHiddenNetwork.pstrHiddenNetworkInfo = pstrHiddenNetwork->pstrHiddenNetworkInfo;
+		strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.strHiddenNetwork.u8ssidnum = pstrHiddenNetwork->u8ssidnum;
+
+	} else
+		PRINT_D(HOSTINF_DBG, "pstrHiddenNetwork IS EQUAL TO NULL\n");
+
+	strHostIFmsg.drvHandler = hWFIDrv;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.u8ScanSource = u8ScanSource;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.u8ScanType = u8ScanType;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.pfScanResult = ScanResult;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.pvUserArg = pvUserArg;
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.u8ChnlListLen = u8ChnlListLen;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.pu8ChnlFreqList = (u8 *)WILC_MALLOC(u8ChnlListLen);        /* will be deallocated by the receiving thread */
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.pu8ChnlFreqList,
+		    pu8ChnlFreqList, u8ChnlListLen);
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.IEsLen = IEsLen;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.pu8IEs = (u8 *)WILC_MALLOC(IEsLen);        /* will be deallocated by the receiving thread */
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strHostIFscanAttr.pu8IEs,
+		    pu8IEs, IEsLen);
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Error in sending message queue scanning parameters: Error(%d)\n", s32Error);
+		WILC_ERRORREPORT(s32Error, WILC_FAIL);
+	}
+
+	enuScanConnTimer = SCAN_TIMER;
+	PRINT_D(HOSTINF_DBG, ">> Starting the SCAN timer\n");
+	WILC_TimerStart(&(pstrWFIDrv->hScanTimer), HOST_IF_SCAN_TIMEOUT, (void *) hWFIDrv, NULL);
+
+
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+
+}
+/**
+ *  @brief                      hif_set_cfg
+ *  @details            sets configuration wids values
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	WID, WID value
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 hif_set_cfg(WILC_WFIDrvHandle hWFIDrv, tstrCfgParamVal *pstrCfgParamVal)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+	tstrHostIFmsg strHostIFmsg;
+
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+	/* prepare the WiphyParams Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_CFG_PARAMS;
+	strHostIFmsg.uniHostIFmsgBody.strHostIFCfgParamAttr.pstrCfgParamVal = *pstrCfgParamVal;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+
+	WILC_CATCH(s32Error)
+	{
+	}
+
+	return s32Error;
+
+}
+
+
+/**
+ *  @brief              hif_get_cfg
+ *  @details    gets configuration wids values
+ *  @param[in,out] handle to the wifi driver,
+ *                              WID value
+ *  @param[in]	WID,
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 hif_get_cfg(WILC_WFIDrvHandle hWFIDrv, u16 u16WID, u16 *pu16WID_Value)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+
+	down(&(pstrWFIDrv->gtOsCfgValuesSem));
+
+	if (pstrWFIDrv == NULL) {
+		PRINT_ER("Driver not initialized: pstrWFIDrv = NULL \n");
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+	PRINT_D(HOSTINF_DBG, "Getting configuration parameters\n");
+	switch (u16WID)	{
+
+	case WID_BSS_TYPE:
+		*pu16WID_Value = (u16)pstrWFIDrv->strCfgValues.bss_type;
+		break;
+
+	case WID_AUTH_TYPE:
+		*pu16WID_Value = (u16)pstrWFIDrv->strCfgValues.auth_type;
+		break;
+
+	case WID_AUTH_TIMEOUT:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.auth_timeout;
+		break;
+
+	case WID_POWER_MANAGEMENT:
+		*pu16WID_Value = (u16)pstrWFIDrv->strCfgValues.power_mgmt_mode;
+		break;
+
+	case WID_SHORT_RETRY_LIMIT:
+		*pu16WID_Value =       pstrWFIDrv->strCfgValues.short_retry_limit;
+		break;
+
+	case WID_LONG_RETRY_LIMIT:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.long_retry_limit;
+		break;
+
+	case WID_FRAG_THRESHOLD:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.frag_threshold;
+		break;
+
+	case WID_RTS_THRESHOLD:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.rts_threshold;
+		break;
+
+	case WID_PREAMBLE:
+		*pu16WID_Value = (u16)pstrWFIDrv->strCfgValues.preamble_type;
+		break;
+
+	case WID_SHORT_SLOT_ALLOWED:
+		*pu16WID_Value = (u16) pstrWFIDrv->strCfgValues.short_slot_allowed;
+		break;
+
+	case WID_11N_TXOP_PROT_DISABLE:
+		*pu16WID_Value = (u16)pstrWFIDrv->strCfgValues.txop_prot_disabled;
+		break;
+
+	case WID_BEACON_INTERVAL:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.beacon_interval;
+		break;
+
+	case WID_DTIM_PERIOD:
+		*pu16WID_Value = (u16)pstrWFIDrv->strCfgValues.dtim_period;
+		break;
+
+	case WID_SITE_SURVEY:
+		*pu16WID_Value = (u16)pstrWFIDrv->strCfgValues.site_survey_enabled;
+		break;
+
+	case WID_SITE_SURVEY_SCAN_TIME:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.site_survey_scan_time;
+		break;
+
+	case WID_ACTIVE_SCAN_TIME:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.active_scan_time;
+		break;
+
+	case WID_PASSIVE_SCAN_TIME:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.passive_scan_time;
+		break;
+
+	case WID_CURRENT_TX_RATE:
+		*pu16WID_Value = pstrWFIDrv->strCfgValues.curr_tx_rate;
+		break;
+
+	default:
+		break;
+	}
+
+	up(&(pstrWFIDrv->gtOsCfgValuesSem));
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+
+}
+
+/*****************************************************************************/
+/*							Notification Functions							 */
+/*****************************************************************************/
+/**
+ *  @brief              notifies host with join and leave requests
+ *  @details    This function prepares an Information frame having the
+ *                              information about a joining/leaving station.
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	6 byte Sta Adress
+ *                              Join or leave flag:
+ *                              Join = 1,
+ *                              Leave =0
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+void host_int_send_join_leave_info_to_host
+	(u16 assocId, u8 *stationAddr, bool joining)
+{
+}
+/**
+ *  @brief              notifies host with stations found in scan
+ *  @details    sends the beacon/probe response from scan
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Sta Address,
+ *                              Frame length,
+ *                              Rssi of the Station found
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+void GetPeriodicRSSI(void *pvArg)
+{
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)pvArg;
+	if (pstrWFIDrv == NULL)	{
+		PRINT_ER("Driver handler is NULL\n");
+		return;
+	}
+
+	if (pstrWFIDrv->enuHostIFstate == HOST_IF_CONNECTED) {
+		s32 s32Error = WILC_SUCCESS;
+		tstrHostIFmsg strHostIFmsg;
+
+		/* prepare the Get RSSI Message */
+		WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+		strHostIFmsg.u16MsgId = HOST_IF_MSG_GET_RSSI;
+		strHostIFmsg.drvHandler = pstrWFIDrv;
+
+		/* send the message */
+		s32Error =	WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+		if (s32Error) {
+			PRINT_ER("Failed to send get host channel param's message queue ");
+			return;
+		}
+	}
+	WILC_TimerStart(&(g_hPeriodicRSSI), 5000, (void *)pstrWFIDrv, NULL);
+}
+
+
+void host_int_send_network_info_to_host
+	(u8 *macStartAddress, u16 u16RxFrameLen, s8 s8Rssi)
+{
+}
+/**
+ *  @brief              host_int_init
+ *  @details    host interface initialization function
+ *  @param[in,out] handle to the wifi driver,
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+static u32 u32Intialized;
+static u32 msgQ_created;
+static u32 clients_count;
+
+s32 host_int_init(WILC_WFIDrvHandle *phWFIDrv)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+
+	/*if(u32Intialized == 1)
+	 * {
+	 *      PRINT_D(HOSTINF_DBG,"Host interface is previously initialized\n");
+	 * *phWFIDrv = (WILC_WFIDrvHandle)gWFiDrvHandle; //Will be adjusted later for P2P
+	 *      return 0;
+	 * }	*/
+	PRINT_D(HOSTINF_DBG, "Initializing host interface for client %d\n", clients_count + 1);
+
+	gbScanWhileConnected = false;
+
+	sema_init(&hWaitResponse, 0);
+
+
+
+	/*Allocate host interface private structure*/
+	pstrWFIDrv  = (tstrWILC_WFIDrv *)WILC_MALLOC(sizeof(tstrWILC_WFIDrv));
+	if (pstrWFIDrv == NULL) {
+		/* WILC_ERRORREPORT(s32Error,WILC_NO_MEM); */
+		s32Error = WILC_NO_MEM;
+		PRINT_ER("Failed to allocate memory\n");
+		goto _fail_timer_2;
+	}
+	WILC_memset(pstrWFIDrv, 0, sizeof(tstrWILC_WFIDrv));
+	/*return driver handle to user*/
+	*phWFIDrv = (WILC_WFIDrvHandle)pstrWFIDrv;
+	/*save into globl handle*/
+
+	#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+
+	g_obtainingIP = false;
+	#endif
+
+	PRINT_D(HOSTINF_DBG, "Global handle pointer value=%p\n", pstrWFIDrv);
+	/* /////////////////////////////////////// */
+	if (clients_count == 0)	{
+		sema_init(&hSemHostIFthrdEnd, 0);
+		sema_init(&hSemDeinitDrvHandle, 0);
+		/*BugID_5348*/
+		sema_init(&hSemHostIntDeinit, 1);
+	}
+
+	sema_init(&(pstrWFIDrv->hSemTestKeyBlock), 0);
+	sema_init(&(pstrWFIDrv->hSemTestDisconnectBlock), 0);
+	sema_init(&(pstrWFIDrv->hSemGetRSSI), 0);
+	sema_init(&(pstrWFIDrv->hSemGetLINKSPEED), 0);
+	sema_init(&(pstrWFIDrv->hSemGetCHNL), 0);
+	sema_init(&(pstrWFIDrv->hSemInactiveTime), 0);
+
+	/* /////////////////////////////////////// */
+
+
+
+	PRINT_D(HOSTINF_DBG, "INIT: CLIENT COUNT %d\n", clients_count);
+
+	if (clients_count == 0)	{
+
+		s32Error = WILC_MsgQueueCreate(&gMsgQHostIF, NULL);
+
+
+		if (s32Error < 0) {
+			PRINT_ER("Failed to creat MQ\n");
+			goto _fail_;
+		}
+		msgQ_created = 1;
+		HostIFthreadHandler = kthread_run(hostIFthread, NULL, "WILC_kthread");
+		if (IS_ERR(HostIFthreadHandler)) {
+			PRINT_ER("Failed to creat Thread\n");
+			s32Error = WILC_FAIL;
+			goto _fail_mq_;
+		}
+		s32Error = WILC_TimerCreate(&(g_hPeriodicRSSI), GetPeriodicRSSI, NULL);
+		if (s32Error < 0) {
+			PRINT_ER("Failed to creat Timer\n");
+			goto _fail_timer_1;
+		}
+		WILC_TimerStart(&(g_hPeriodicRSSI), 5000, (void *)pstrWFIDrv, NULL);
+
+	}
+
+
+	s32Error = WILC_TimerCreate(&(pstrWFIDrv->hScanTimer), TimerCB_Scan, NULL);
+	if (s32Error < 0) {
+		PRINT_ER("Failed to creat Timer\n");
+		goto _fail_thread_;
+	}
+
+	s32Error = WILC_TimerCreate(&(pstrWFIDrv->hConnectTimer), TimerCB_Connect, NULL);
+	if (s32Error < 0) {
+		PRINT_ER("Failed to creat Timer\n");
+		goto _fail_timer_1;
+	}
+
+
+	#ifdef WILC_P2P
+	/*Remain on channel timer*/
+	s32Error = WILC_TimerCreate(&(pstrWFIDrv->hRemainOnChannel), ListenTimerCB, NULL);
+	if (s32Error < 0) {
+		PRINT_ER("Failed to creat Remain-on-channel Timer\n");
+		goto _fail_timer_3;
+	}
+	#endif
+
+	sema_init(&(pstrWFIDrv->gtOsCfgValuesSem), 1);
+	down(&(pstrWFIDrv->gtOsCfgValuesSem));
+
+
+
+#ifdef SIMULATION
+	TransportInit();
+#endif
+
+	pstrWFIDrv->enuHostIFstate = HOST_IF_IDLE;
+	/* gWFiDrvHandle->bPendingConnRequest = false; */
+
+	/*Initialize CFG WIDS Defualt Values*/
+
+	pstrWFIDrv->strCfgValues.site_survey_enabled = SITE_SURVEY_OFF;
+	pstrWFIDrv->strCfgValues.scan_source = DEFAULT_SCAN;
+	pstrWFIDrv->strCfgValues.active_scan_time = ACTIVE_SCAN_TIME;
+	pstrWFIDrv->strCfgValues.passive_scan_time = PASSIVE_SCAN_TIME;
+	pstrWFIDrv->strCfgValues.curr_tx_rate = AUTORATE;
+
+
+	#ifdef WILC_P2P
+
+	pstrWFIDrv->u64P2p_MgmtTimeout = 0;
+
+	#endif
+
+	PRINT_INFO(HOSTINF_DBG, "Initialization values, Site survey value: %d\n Scan source: %d\n Active scan time: %d\n Passive scan time: %d\nCurrent tx Rate = %d\n",
+
+		   pstrWFIDrv->strCfgValues.site_survey_enabled, pstrWFIDrv->strCfgValues.scan_source,
+		   pstrWFIDrv->strCfgValues.active_scan_time, pstrWFIDrv->strCfgValues.passive_scan_time,
+		   pstrWFIDrv->strCfgValues.curr_tx_rate);
+
+
+	up(&(pstrWFIDrv->gtOsCfgValuesSem));
+
+	/*TODO Code to setup simulation to be removed later*/
+	/*Intialize configurator module*/
+	s32Error = CoreConfiguratorInit();
+	if (s32Error < 0) {
+		PRINT_ER("Failed to initialize core configurator\n");
+		goto _fail_mem_;
+	}
+
+#ifdef SIMULATION
+	/*Initialize Simulaor*/
+	CoreConfigSimulatorInit();
+#endif
+
+	u32Intialized = 1;
+	clients_count++; /* increase number of created entities */
+
+	return s32Error;
+
+
+_fail_mem_:
+	if (pstrWFIDrv != NULL)
+		WILC_FREE(pstrWFIDrv);
+#ifdef WILC_P2P
+_fail_timer_3:
+	WILC_TimerDestroy(&(pstrWFIDrv->hRemainOnChannel), NULL);
+#endif
+_fail_timer_2:
+	up(&(pstrWFIDrv->gtOsCfgValuesSem));
+	WILC_TimerDestroy(&(pstrWFIDrv->hConnectTimer), NULL);
+_fail_timer_1:
+	WILC_TimerDestroy(&(pstrWFIDrv->hScanTimer), NULL);
+_fail_thread_:
+	kthread_stop(HostIFthreadHandler);
+_fail_mq_:
+	WILC_MsgQueueDestroy(&gMsgQHostIF, NULL);
+_fail_:
+	return s32Error;
+
+
+}
+/**
+ *  @brief              host_int_deinit
+ *  @details    host interface initialization function
+ *  @param[in,out] handle to the wifi driver,
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_deinit(WILC_WFIDrvHandle hWFIDrv)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+
+
+	/*obtain driver handle*/
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	/*if(u32Intialized == 0)
+	 * {
+	 *      PRINT_ER("Host Interface is not initialized\n");
+	 *      return 0;
+	 * }*/
+
+	/*BugID_5348*/
+
+	if (pstrWFIDrv == NULL)	{
+		PRINT_ER("pstrWFIDrv = NULL\n");
+		return 0;
+	}
+
+	down(&hSemHostIntDeinit);
+
+	terminated_handle = pstrWFIDrv;
+	PRINT_D(HOSTINF_DBG, "De-initializing host interface for client %d\n", clients_count);
+
+	/*BugID_5348*/
+	/*Destroy all timers before acquiring hSemDeinitDrvHandle*/
+	/*to guarantee handling all messages befor proceeding*/
+	if (WILC_TimerDestroy(&(pstrWFIDrv->hScanTimer), NULL)) {
+		PRINT_D(HOSTINF_DBG, ">> Scan timer is active \n");
+		/* msleep(HOST_IF_SCAN_TIMEOUT+1000); */
+	}
+
+	if (WILC_TimerDestroy(&(pstrWFIDrv->hConnectTimer), NULL)) {
+		PRINT_D(HOSTINF_DBG, ">> Connect timer is active \n");
+		/* msleep(HOST_IF_CONNECT_TIMEOUT+1000); */
+	}
+
+
+	if (WILC_TimerDestroy(&(g_hPeriodicRSSI), NULL)) {
+		PRINT_D(HOSTINF_DBG, ">> Connect timer is active \n");
+		/* msleep(HOST_IF_CONNECT_TIMEOUT+1000); */
+	}
+
+	#ifdef WILC_P2P
+	/*Destroy Remain-onchannel Timer*/
+	WILC_TimerDestroy(&(pstrWFIDrv->hRemainOnChannel), NULL);
+	#endif
+
+	host_int_set_wfi_drv_handler((u32)NULL);
+	down(&hSemDeinitDrvHandle);
+
+
+	/*Calling the CFG80211 scan done function with the abort flag set to true*/
+	if (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult) {
+		pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult(SCAN_EVENT_ABORTED, NULL,
+								pstrWFIDrv->strWILC_UsrScanReq.u32UserScanPvoid, NULL);
+
+		pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult = NULL;
+	}
+	/*deinit configurator and simulator*/
+#ifdef SIMULATION
+	CoreConfigSimulatorDeInit();
+#endif
+	CoreConfiguratorDeInit();
+#ifdef SIMULATION
+	TransportDeInit();
+#endif
+
+	pstrWFIDrv->enuHostIFstate = HOST_IF_IDLE;
+
+	gbScanWhileConnected = false;
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	if (clients_count == 1)	{
+		if (WILC_TimerDestroy(&g_hPeriodicRSSI, NULL)) {
+			PRINT_D(HOSTINF_DBG, ">> Connect timer is active \n");
+			/* msleep(HOST_IF_CONNECT_TIMEOUT+1000); */
+		}
+		strHostIFmsg.u16MsgId = HOST_IF_MSG_EXIT;
+		strHostIFmsg.drvHandler = hWFIDrv;
+
+
+		s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+		if (s32Error != WILC_SUCCESS) {
+			PRINT_ER("Error in sending deinit's message queue message function: Error(%d)\n", s32Error);
+		}
+
+		down(&hSemHostIFthrdEnd);
+
+
+
+		WILC_MsgQueueDestroy(&gMsgQHostIF, NULL);
+		msgQ_created = 0;
+	}
+
+	down(&(pstrWFIDrv->gtOsCfgValuesSem));
+
+	/*Setting the gloabl driver handler with NULL*/
+	u32Intialized = 0;
+	/* gWFiDrvHandle = NULL; */
+	if (pstrWFIDrv != NULL) {
+		WILC_FREE(pstrWFIDrv);
+		/* pstrWFIDrv=NULL; */
+
+	}
+
+	clients_count--; /* Decrease number of created entities */
+	terminated_handle = NULL;
+	up(&hSemHostIntDeinit);
+	return s32Error;
+}
+
+
+/**
+ *  @brief              NetworkInfoReceived
+ *  @details    function to to be called when network info packet is received
+ *  @param[in]	pu8Buffer the received packet
+ *  @param[in]   u32Length  length of the received packet
+ *  @return             none
+ *  @note
+ *  @author
+ *  @date		1 Mar 2012
+ *  @version		1.0
+ */
+void NetworkInfoReceived(u8 *pu8Buffer, u32 u32Length)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+	u32 drvHandler;
+	tstrWILC_WFIDrv *pstrWFIDrv = NULL;
+
+	drvHandler = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
+	pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+
+
+
+	if (pstrWFIDrv == NULL || pstrWFIDrv == terminated_handle)	{
+		PRINT_ER("NetworkInfo received but driver not init[%p]\n", pstrWFIDrv);
+		return;
+	}
+
+	/* prepare the Asynchronous Network Info message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_RCVD_NTWRK_INFO;
+	strHostIFmsg.drvHandler = pstrWFIDrv;
+
+	strHostIFmsg.uniHostIFmsgBody.strRcvdNetworkInfo.u32Length = u32Length;
+	strHostIFmsg.uniHostIFmsgBody.strRcvdNetworkInfo.pu8Buffer = (u8 *)WILC_MALLOC(u32Length); /* will be deallocated by the receiving thread */
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strRcvdNetworkInfo.pu8Buffer,
+		    pu8Buffer, u32Length);
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Error in sending network info message queue message parameters: Error(%d)\n", s32Error);
+	}
+
+
+	return;
+}
+
+/**
+ *  @brief              GnrlAsyncInfoReceived
+ *  @details    function to be called when general Asynchronous info packet is received
+ *  @param[in]	pu8Buffer the received packet
+ *  @param[in]   u32Length  length of the received packet
+ *  @return             none
+ *  @note
+ *  @author
+ *  @date		15 Mar 2012
+ *  @version		1.0
+ */
+void GnrlAsyncInfoReceived(u8 *pu8Buffer, u32 u32Length)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+	u32 drvHandler;
+	tstrWILC_WFIDrv *pstrWFIDrv = NULL;
+
+	/*BugID_5348*/
+	down(&hSemHostIntDeinit);
+
+	drvHandler = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
+	pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+	PRINT_D(HOSTINF_DBG, "General asynchronous info packet received \n");
+
+
+	if (pstrWFIDrv == NULL || pstrWFIDrv == terminated_handle) {
+		PRINT_D(HOSTINF_DBG, "Wifi driver handler is equal to NULL\n");
+		/*BugID_5348*/
+		up(&hSemHostIntDeinit);
+		return;
+	}
+
+	if (pstrWFIDrv->strWILC_UsrConnReq.pfUserConnectResult == NULL) {
+		/* received mac status is not needed when there is no current Connect Request */
+		PRINT_ER("Received mac status is not needed when there is no current Connect Reques\n");
+		/*BugID_5348*/
+		up(&hSemHostIntDeinit);
+		return;
+	}
+
+	/* prepare the General Asynchronous Info message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_RCVD_GNRL_ASYNC_INFO;
+	strHostIFmsg.drvHandler = pstrWFIDrv;
+
+
+	strHostIFmsg.uniHostIFmsgBody.strRcvdGnrlAsyncInfo.u32Length = u32Length;
+	strHostIFmsg.uniHostIFmsgBody.strRcvdGnrlAsyncInfo.pu8Buffer = (u8 *)WILC_MALLOC(u32Length); /* will be deallocated by the receiving thread */
+	WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strRcvdGnrlAsyncInfo.pu8Buffer,
+		    pu8Buffer, u32Length);
+
+	/* send the message */
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		PRINT_ER("Error in sending message queue asynchronous message info: Error(%d)\n", s32Error);
+	}
+
+	/*BugID_5348*/
+	up(&hSemHostIntDeinit);
+	return;
+}
+
+/**
+ *  @brief host_int_ScanCompleteReceived
+ *  @details        Setting scan complete received notifcation in message queue
+ *  @param[in]     u8* pu8Buffer, u32 u32Length
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+void host_int_ScanCompleteReceived(u8 *pu8Buffer, u32 u32Length)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrHostIFmsg strHostIFmsg;
+	u32 drvHandler;
+	tstrWILC_WFIDrv *pstrWFIDrv = NULL;
+	drvHandler = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
+	pstrWFIDrv = (tstrWILC_WFIDrv *)drvHandler;
+
+
+	PRINT_D(GENERIC_DBG, "Scan notification received %p\n", pstrWFIDrv);
+
+	if (pstrWFIDrv == NULL || pstrWFIDrv == terminated_handle) {
+		return;
+	}
+
+	/*if there is an ongoing scan request*/
+	if (pstrWFIDrv->strWILC_UsrScanReq.pfUserScanResult) {
+		/* prepare theScan Done message */
+		WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+		strHostIFmsg.u16MsgId = HOST_IF_MSG_RCVD_SCAN_COMPLETE;
+		strHostIFmsg.drvHandler = pstrWFIDrv;
+
+
+		/* will be deallocated by the receiving thread */
+		/*no need to send message body*/
+
+		/*strHostIFmsg.uniHostIFmsgBody.strScanComplete.u32Length = u32Length;
+		 * strHostIFmsg.uniHostIFmsgBody.strScanComplete.pu8Buffer  = (u8*)WILC_MALLOC(u32Length);
+		 * WILC_memcpy(strHostIFmsg.uniHostIFmsgBody.strScanComplete.pu8Buffer,
+		 *                        pu8Buffer, u32Length); */
+
+		/* send the message */
+		s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+		if (s32Error) {
+			PRINT_ER("Error in sending message queue scan complete parameters: Error(%d)\n", s32Error);
+		}
+	}
+
+
+	return;
+
+}
+
+#ifdef WILC_P2P
+/**
+ *  @brief              host_int_remain_on_channel
+ *  @details
+ *  @param[in]          Handle to wifi driver
+ *                              Duration to remain on channel
+ *                              Channel to remain on
+ *                              Pointer to fn to be called on receive frames in listen state
+ *                              Pointer to remain-on-channel expired fn
+ *                              Priv
+ *  @return             Error code.
+ *  @author
+ *  @date
+ *  @version		1.0
+ */
+s32 host_int_remain_on_channel(WILC_WFIDrvHandle hWFIDrv, u32 u32SessionID, u32 u32duration, u16 chan, tWILCpfRemainOnChanExpired RemainOnChanExpired, tWILCpfRemainOnChanReady RemainOnChanReady, void *pvUserArg)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/* prepare the remainonchan Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_REMAIN_ON_CHAN;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan.u16Channel = chan;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan.pRemainOnChanExpired = RemainOnChanExpired;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan.pRemainOnChanReady = RemainOnChanReady;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan.pVoid = pvUserArg;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan.u32duration = u32duration;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan.u32ListenSessionID = u32SessionID;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief              host_int_ListenStateExpired
+ *  @details
+ *  @param[in]          Handle to wifi driver
+ *                              Duration to remain on channel
+ *                              Channel to remain on
+ *                              Pointer to fn to be called on receive frames in listen state
+ *                              Pointer to remain-on-channel expired fn
+ *                              Priv
+ *  @return             Error code.
+ *  @author
+ *  @date
+ *  @version		1.0
+ */
+s32 host_int_ListenStateExpired(WILC_WFIDrvHandle hWFIDrv, u32 u32SessionID)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/*Stopping remain-on-channel timer*/
+	WILC_TimerStop(&(pstrWFIDrv->hRemainOnChannel), NULL);
+
+	/* prepare the timer fire Message */
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_LISTEN_TIMER_FIRED;
+	strHostIFmsg.drvHandler = hWFIDrv;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRemainOnChan.u32ListenSessionID = u32SessionID;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief              host_int_frame_register
+ *  @details
+ *  @param[in]          Handle to wifi driver
+ *  @return             Error code.
+ *  @author
+ *  @date
+ *  @version		1.0*/
+s32 host_int_frame_register(WILC_WFIDrvHandle hWFIDrv, u16 u16FrameType, bool bReg)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_REGISTER_FRAME;
+	switch (u16FrameType) {
+	case ACTION:
+		PRINT_D(HOSTINF_DBG, "ACTION\n");
+		strHostIFmsg.uniHostIFmsgBody.strHostIfRegisterFrame.u8Regid = ACTION_FRM_IDX;
+		break;
+
+	case PROBE_REQ:
+		PRINT_D(HOSTINF_DBG, "PROBE REQ\n");
+		strHostIFmsg.uniHostIFmsgBody.strHostIfRegisterFrame.u8Regid = PROBE_REQ_IDX;
+		break;
+
+	default:
+		PRINT_D(HOSTINF_DBG, "Not valid frame type\n");
+		break;
+	}
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRegisterFrame.u16FrameType = u16FrameType;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfRegisterFrame.bReg = bReg;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+
+
+}
+#endif
+
+#ifdef WILC_AP_EXTERNAL_MLME
+/**
+ *  @brief host_int_add_beacon
+ *  @details       Setting add beacon params in message queue
+ *  @param[in]    WILC_WFIDrvHandle hWFIDrv, u32 u32Interval,
+ *                         u32 u32DTIMPeriod,u32 u32HeadLen, u8* pu8Head,
+ *                         u32 u32TailLen, u8* pu8Tail
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_add_beacon(WILC_WFIDrvHandle hWFIDrv, u32 u32Interval,
+				u32 u32DTIMPeriod,
+				u32 u32HeadLen, u8 *pu8Head,
+				u32 u32TailLen, u8 *pu8Tail)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrHostIFSetBeacon *pstrSetBeaconParam = &strHostIFmsg.uniHostIFmsgBody.strHostIFSetBeacon;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	PRINT_D(HOSTINF_DBG, "Setting adding beacon message queue params\n");
+
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_ADD_BEACON;
+	strHostIFmsg.drvHandler = hWFIDrv;
+	pstrSetBeaconParam->u32Interval = u32Interval;
+	pstrSetBeaconParam->u32DTIMPeriod = u32DTIMPeriod;
+	pstrSetBeaconParam->u32HeadLen = u32HeadLen;
+	pstrSetBeaconParam->pu8Head = (u8 *)WILC_MALLOC(u32HeadLen);
+	if (pstrSetBeaconParam->pu8Head == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+	}
+	WILC_memcpy(pstrSetBeaconParam->pu8Head, pu8Head, u32HeadLen);
+	pstrSetBeaconParam->u32TailLen = u32TailLen;
+
+	/* Bug 4599 : if tail length = 0 skip allocating & copying */
+	if (u32TailLen > 0) {
+		pstrSetBeaconParam->pu8Tail = (u8 *)WILC_MALLOC(u32TailLen);
+		if (pstrSetBeaconParam->pu8Tail == NULL) {
+			WILC_ERRORREPORT(s32Error, WILC_NO_MEM);
+		}
+		WILC_memcpy(pstrSetBeaconParam->pu8Tail, pu8Tail, u32TailLen);
+	} else {
+		pstrSetBeaconParam->pu8Tail = NULL;
+	}
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+		if (pstrSetBeaconParam->pu8Head != NULL) {
+			WILC_FREE(pstrSetBeaconParam->pu8Head);
+		}
+
+		if (pstrSetBeaconParam->pu8Tail != NULL) {
+			WILC_FREE(pstrSetBeaconParam->pu8Tail);
+		}
+	}
+
+	return s32Error;
+
+}
+
+
+/**
+ *  @brief host_int_del_beacon
+ *  @details       Setting add beacon params in message queue
+ *  @param[in]    WILC_WFIDrvHandle hWFIDrv
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_del_beacon(WILC_WFIDrvHandle hWFIDrv)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_DEL_BEACON;
+	strHostIFmsg.drvHandler = hWFIDrv;
+	PRINT_D(HOSTINF_DBG, "Setting deleting beacon message queue params\n");
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	WILC_ERRORCHECK(s32Error);
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+
+
+/**
+ *  @brief host_int_add_station
+ *  @details       Setting add station params in message queue
+ *  @param[in]    WILC_WFIDrvHandle hWFIDrv, tstrWILC_AddStaParam* pstrStaParams
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_add_station(WILC_WFIDrvHandle hWFIDrv, tstrWILC_AddStaParam *pstrStaParams)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrWILC_AddStaParam *pstrAddStationMsg = &strHostIFmsg.uniHostIFmsgBody.strAddStaParam;
+
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	PRINT_D(HOSTINF_DBG, "Setting adding station message queue params\n");
+
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_ADD_STATION;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	WILC_memcpy(pstrAddStationMsg, pstrStaParams, sizeof(tstrWILC_AddStaParam));
+	if (pstrAddStationMsg->u8NumRates > 0) {
+		u8 *rates = WILC_MALLOC(pstrAddStationMsg->u8NumRates);
+		WILC_NULLCHECK(s32Error, rates);
+
+		WILC_memcpy(rates, pstrStaParams->pu8Rates, pstrAddStationMsg->u8NumRates);
+		pstrAddStationMsg->pu8Rates = rates;
+	}
+
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief host_int_del_station
+ *  @details       Setting delete station params in message queue
+ *  @param[in]    WILC_WFIDrvHandle hWFIDrv, u8* pu8MacAddr
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_del_station(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8MacAddr)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrHostIFDelSta *pstrDelStationMsg = &strHostIFmsg.uniHostIFmsgBody.strDelStaParam;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	PRINT_D(HOSTINF_DBG, "Setting deleting station message queue params\n");
+
+
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_DEL_STATION;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/*BugID_4795: Handling situation of deleting all stations*/
+	if (pu8MacAddr == NULL)
+		WILC_memset(pstrDelStationMsg->au8MacAddr, 255, ETH_ALEN);
+	else
+		WILC_memcpy(pstrDelStationMsg->au8MacAddr, pu8MacAddr, ETH_ALEN);
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+/**
+ *  @brief      host_int_del_allstation
+ *  @details    Setting del station params in message queue
+ *  @param[in]  WILC_WFIDrvHandle hWFIDrv, u8 pu8MacAddr[][ETH_ALEN]s
+ *  @return        Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_del_allstation(WILC_WFIDrvHandle hWFIDrv, u8 pu8MacAddr[][ETH_ALEN])
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrHostIFDelAllSta *pstrDelAllStationMsg = &strHostIFmsg.uniHostIFmsgBody.strHostIFDelAllSta;
+	u8 au8Zero_Buff[ETH_ALEN] = {0};
+	u32 i;
+	u8 u8AssocNumb = 0;
+
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	PRINT_D(HOSTINF_DBG, "Setting deauthenticating station message queue params\n");
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_DEL_ALL_STA;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	/* Handling situation of deauthenticing all associated stations*/
+	for (i = 0; i < MAX_NUM_STA; i++) {
+		if (memcmp(pu8MacAddr[i], au8Zero_Buff, ETH_ALEN)) {
+			WILC_memcpy(pstrDelAllStationMsg->au8Sta_DelAllSta[i], pu8MacAddr[i], ETH_ALEN);
+			PRINT_D(CFG80211_DBG, "BSSID = %x%x%x%x%x%x\n", pstrDelAllStationMsg->au8Sta_DelAllSta[i][0], pstrDelAllStationMsg->au8Sta_DelAllSta[i][1], pstrDelAllStationMsg->au8Sta_DelAllSta[i][2], pstrDelAllStationMsg->au8Sta_DelAllSta[i][3], pstrDelAllStationMsg->au8Sta_DelAllSta[i][4],
+				pstrDelAllStationMsg->au8Sta_DelAllSta[i][5]);
+			u8AssocNumb++;
+		}
+	}
+	if (!u8AssocNumb) {
+		PRINT_D(CFG80211_DBG, "NO ASSOCIATED STAS\n");
+		return s32Error;
+	}
+
+	pstrDelAllStationMsg->u8Num_AssocSta = u8AssocNumb;
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+
+
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+	down(&hWaitResponse);
+
+	return s32Error;
+
+}
+
+/**
+ *  @brief host_int_edit_station
+ *  @details       Setting edit station params in message queue
+ *  @param[in]    WILC_WFIDrvHandle hWFIDrv, tstrWILC_AddStaParam* pstrStaParams
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_edit_station(WILC_WFIDrvHandle hWFIDrv, tstrWILC_AddStaParam *pstrStaParams)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrWILC_AddStaParam *pstrAddStationMsg = &strHostIFmsg.uniHostIFmsgBody.strAddStaParam;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	PRINT_D(HOSTINF_DBG, "Setting editing station message queue params\n");
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_EDIT_STATION;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	WILC_memcpy(pstrAddStationMsg, pstrStaParams, sizeof(tstrWILC_AddStaParam));
+	if (pstrAddStationMsg->u8NumRates > 0) {
+		u8 *rates = WILC_MALLOC(pstrAddStationMsg->u8NumRates);
+		WILC_NULLCHECK(s32Error, rates);
+		WILC_memcpy(rates, pstrStaParams->pu8Rates, pstrAddStationMsg->u8NumRates);
+		pstrAddStationMsg->pu8Rates = rates;
+	}
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+#endif /*WILC_AP_EXTERNAL_MLME*/
+uint32_t wilc_get_chipid(uint8_t);
+
+s32 host_int_set_power_mgmt(WILC_WFIDrvHandle hWFIDrv, bool bIsEnabled, u32 u32Timeout)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrHostIfPowerMgmtParam *pstrPowerMgmtParam = &strHostIFmsg.uniHostIFmsgBody.strPowerMgmtparam;
+
+	PRINT_INFO(HOSTINF_DBG, "\n\n>> Setting PS to %d << \n\n", bIsEnabled);
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	PRINT_D(HOSTINF_DBG, "Setting Power management message queue params\n");
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_POWER_MGMT;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	pstrPowerMgmtParam->bIsEnabled = bIsEnabled;
+	pstrPowerMgmtParam->u32Timeout = u32Timeout;
+
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+
+s32 host_int_setup_multicast_filter(WILC_WFIDrvHandle hWFIDrv, bool bIsEnabled, u32 u32count)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrHostIFSetMulti *pstrMulticastFilterParam = &strHostIFmsg.uniHostIFmsgBody.strHostIfSetMulti;
+
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	PRINT_D(HOSTINF_DBG, "Setting Multicast Filter params\n");
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_SET_MULTICAST_FILTER;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	pstrMulticastFilterParam->bIsEnabled = bIsEnabled;
+	pstrMulticastFilterParam->u32count = u32count;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+
+
+
+/*Bug4218: Parsing Join Param*/
+#ifdef WILC_PARSE_SCAN_IN_HOST
+
+/*Bug4218: Parsing Join Param*/
+/**
+ *  @brief              host_int_ParseJoinBssParam
+ *  @details            Parse Needed Join Parameters and save it in a new JoinBssParam entry
+ *  @param[in]          tstrNetworkInfo* ptstrNetworkInfo
+ *  @return
+ *  @author		zsalah
+ *  @date
+ *  @version		1.0**/
+static void *host_int_ParseJoinBssParam(tstrNetworkInfo *ptstrNetworkInfo)
+{
+	tstrJoinBssParam *pNewJoinBssParam = NULL;
+	u8 *pu8IEs;
+	u16 u16IEsLen;
+	u16 index = 0;
+	u8 suppRatesNo = 0;
+	u8 extSuppRatesNo;
+	u16 jumpOffset;
+	u8 pcipherCount;
+	u8 authCount;
+	u8 pcipherTotalCount = 0;
+	u8 authTotalCount = 0;
+	u8 i, j;
+
+	pu8IEs = ptstrNetworkInfo->pu8IEs;
+	u16IEsLen = ptstrNetworkInfo->u16IEsLen;
+
+	pNewJoinBssParam = WILC_MALLOC(sizeof(tstrJoinBssParam));
+	if (pNewJoinBssParam != NULL) {
+		WILC_memset(pNewJoinBssParam, 0, sizeof(tstrJoinBssParam));
+		pNewJoinBssParam->dtim_period = ptstrNetworkInfo->u8DtimPeriod;
+		pNewJoinBssParam->beacon_period = ptstrNetworkInfo->u16BeaconPeriod;
+		pNewJoinBssParam->cap_info = ptstrNetworkInfo->u16CapInfo;
+		WILC_memcpy(pNewJoinBssParam->au8bssid, ptstrNetworkInfo->au8bssid, 6);
+		/*for(i=0; i<6;i++)
+		 *      PRINT_D(HOSTINF_DBG,"%c",pNewJoinBssParam->au8bssid[i]);*/
+		WILC_memcpy((u8 *)pNewJoinBssParam->ssid, ptstrNetworkInfo->au8ssid, ptstrNetworkInfo->u8SsidLen + 1);
+		pNewJoinBssParam->ssidLen = ptstrNetworkInfo->u8SsidLen;
+		WILC_memset(pNewJoinBssParam->rsn_pcip_policy, 0xFF, 3);
+		WILC_memset(pNewJoinBssParam->rsn_auth_policy, 0xFF, 3);
+		/*for(i=0; i<pNewJoinBssParam->ssidLen;i++)
+		 *      PRINT_D(HOSTINF_DBG,"%c",pNewJoinBssParam->ssid[i]);*/
+
+		/* parse supported rates: */
+		while (index < u16IEsLen) {
+			/* supportedRates IE */
+			if (pu8IEs[index] == SUPP_RATES_IE) {
+				/* PRINT_D(HOSTINF_DBG, "Supported Rates\n"); */
+				suppRatesNo = pu8IEs[index + 1];
+				pNewJoinBssParam->supp_rates[0] = suppRatesNo;
+				index += 2; /* skipping ID and length bytes; */
+
+				for (i = 0; i < suppRatesNo; i++) {
+					pNewJoinBssParam->supp_rates[i + 1] = pu8IEs[index + i];
+					/* PRINT_D(HOSTINF_DBG,"%0x ",pNewJoinBssParam->supp_rates[i+1]); */
+				}
+				index += suppRatesNo;
+				continue;
+			}
+			/* Ext SupportedRates IE */
+			else if (pu8IEs[index] == EXT_SUPP_RATES_IE) {
+				/* PRINT_D(HOSTINF_DBG, "Extended Supported Rates\n"); */
+				/* checking if no of ext. supp and supp rates < max limit */
+				extSuppRatesNo = pu8IEs[index + 1];
+				if (extSuppRatesNo > (MAX_RATES_SUPPORTED - suppRatesNo))
+					pNewJoinBssParam->supp_rates[0] = MAX_RATES_SUPPORTED;
+				else
+					pNewJoinBssParam->supp_rates[0] += extSuppRatesNo;
+				index += 2;
+				/* pNewJoinBssParam.supp_rates[0] contains now old number not the ext. no */
+				for (i = 0; i < (pNewJoinBssParam->supp_rates[0] - suppRatesNo); i++) {
+					pNewJoinBssParam->supp_rates[suppRatesNo + i + 1] = pu8IEs[index + i];
+					/* PRINT_D(HOSTINF_DBG,"%0x ",pNewJoinBssParam->supp_rates[suppRatesNo+i+1]); */
+				}
+				index += extSuppRatesNo;
+				continue;
+			}
+			/* HT Cap. IE */
+			else if (pu8IEs[index] == HT_CAPABILITY_IE) {
+				/* if IE found set the flag */
+				pNewJoinBssParam->ht_capable = true;
+				index += pu8IEs[index + 1] + 2; /* ID,Length bytes and IE body */
+				/* PRINT_D(HOSTINF_DBG,"HT_CAPABALE\n"); */
+				continue;
+			} else if ((pu8IEs[index] == WMM_IE) && /* WMM Element ID */
+				   (pu8IEs[index + 2] == 0x00) && (pu8IEs[index + 3] == 0x50) &&
+				   (pu8IEs[index + 4] == 0xF2) && /* OUI */
+				   (pu8IEs[index + 5] == 0x02) && /* OUI Type     */
+				   ((pu8IEs[index + 6] == 0x00) || (pu8IEs[index + 6] == 0x01)) && /* OUI Sub Type */
+				   (pu8IEs[index + 7] == 0x01)) {
+				/* Presence of WMM Info/Param element indicates WMM capability */
+				pNewJoinBssParam->wmm_cap = true;
+
+				/* Check if Bit 7 is set indicating U-APSD capability */
+				if (pu8IEs[index + 8] & (1 << 7)) {
+					pNewJoinBssParam->uapsd_cap = true;
+				}
+				index += pu8IEs[index + 1] + 2;
+				continue;
+			}
+			#ifdef WILC_P2P
+			else if ((pu8IEs[index] == P2P_IE) && /* P2P Element ID */
+				 (pu8IEs[index + 2] == 0x50) && (pu8IEs[index + 3] == 0x6f) &&
+				 (pu8IEs[index + 4] == 0x9a) && /* OUI */
+				 (pu8IEs[index + 5] == 0x09) && (pu8IEs[index + 6] == 0x0c)) { /* OUI Type     */
+				u16 u16P2P_count;
+				pNewJoinBssParam->tsf = ptstrNetworkInfo->u32Tsf;
+				pNewJoinBssParam->u8NoaEnbaled = 1;
+				pNewJoinBssParam->u8Index = pu8IEs[index + 9];
+
+				/* Check if Bit 7 is set indicating Opss capability */
+				if (pu8IEs[index + 10] & (1 << 7)) {
+					pNewJoinBssParam->u8OppEnable = 1;
+					pNewJoinBssParam->u8CtWindow = pu8IEs[index + 10];
+				} else
+					pNewJoinBssParam->u8OppEnable = 0;
+				/* HOSTINF_DBG */
+				PRINT_D(GENERIC_DBG, "P2P Dump \n");
+				for (i = 0; i < pu8IEs[index + 7]; i++)
+					PRINT_D(GENERIC_DBG, " %x \n", pu8IEs[index + 9 + i]);
+
+				pNewJoinBssParam->u8Count = pu8IEs[index + 11];
+				u16P2P_count = index + 12;
+
+				WILC_memcpy(pNewJoinBssParam->au8Duration, pu8IEs + u16P2P_count, 4);
+				u16P2P_count += 4;
+
+				WILC_memcpy(pNewJoinBssParam->au8Interval, pu8IEs + u16P2P_count, 4);
+				u16P2P_count += 4;
+
+				WILC_memcpy(pNewJoinBssParam->au8StartTime, pu8IEs + u16P2P_count, 4);
+
+				index += pu8IEs[index + 1] + 2;
+				continue;
+
+			}
+			#endif
+			else if ((pu8IEs[index] == RSN_IE) ||
+				 ((pu8IEs[index] == WPA_IE) && (pu8IEs[index + 2] == 0x00) &&
+				  (pu8IEs[index + 3] == 0x50) && (pu8IEs[index + 4] == 0xF2) &&
+				  (pu8IEs[index + 5] == 0x01)))	{
+				u16 rsnIndex = index;
+				/*PRINT_D(HOSTINF_DBG,"RSN IE Length:%d\n",pu8IEs[rsnIndex+1]);
+				 * for(i=0; i<pu8IEs[rsnIndex+1]; i++)
+				 * {
+				 *      PRINT_D(HOSTINF_DBG,"%0x ",pu8IEs[rsnIndex+2+i]);
+				 * }*/
+				if (pu8IEs[rsnIndex] == RSN_IE)	{
+					pNewJoinBssParam->mode_802_11i = 2;
+					/* PRINT_D(HOSTINF_DBG,"\nRSN_IE\n"); */
+				} else { /* check if rsn was previously parsed */
+					if (pNewJoinBssParam->mode_802_11i == 0)
+						pNewJoinBssParam->mode_802_11i = 1;
+					/* PRINT_D(HOSTINF_DBG,"\nWPA_IE\n"); */
+					rsnIndex += 4;
+				}
+				rsnIndex += 7; /* skipping id, length, version(2B) and first 3 bytes of gcipher */
+				pNewJoinBssParam->rsn_grp_policy = pu8IEs[rsnIndex];
+				rsnIndex++;
+				/* PRINT_D(HOSTINF_DBG,"Group Policy: %0x \n",pNewJoinBssParam->rsn_grp_policy); */
+				/* initialize policies with invalid values */
+
+				jumpOffset = pu8IEs[rsnIndex] * 4; /* total no.of bytes of pcipher field (count*4) */
+
+				/*parsing pairwise cipher*/
+
+				/* saving 3 pcipher max. */
+				pcipherCount = (pu8IEs[rsnIndex] > 3) ? 3 : pu8IEs[rsnIndex];
+				rsnIndex += 2; /* jump 2 bytes of pcipher count */
+
+				/* PRINT_D(HOSTINF_DBG,"\npcipher:%d \n",pcipherCount); */
+				for (i = pcipherTotalCount, j = 0; i < pcipherCount + pcipherTotalCount && i < 3; i++, j++) {
+					/* each count corresponds to 4 bytes, only last byte is saved */
+					pNewJoinBssParam->rsn_pcip_policy[i] = pu8IEs[rsnIndex + ((j + 1) * 4) - 1];
+					/* PRINT_D(HOSTINF_DBG,"PAIR policy = [%0x,%0x]\n",pNewJoinBssParam->rsn_pcip_policy[i],i); */
+				}
+				pcipherTotalCount += pcipherCount;
+				rsnIndex += jumpOffset;
+
+				jumpOffset = pu8IEs[rsnIndex] * 4;
+
+				/*parsing AKM suite (auth_policy)*/
+				/* saving 3 auth policies max. */
+				authCount = (pu8IEs[rsnIndex] > 3) ? 3 : pu8IEs[rsnIndex];
+				rsnIndex += 2; /* jump 2 bytes of pcipher count */
+
+				for (i = authTotalCount, j = 0; i < authTotalCount + authCount; i++, j++) {
+					/* each count corresponds to 4 bytes, only last byte is saved */
+					pNewJoinBssParam->rsn_auth_policy[i] = pu8IEs[rsnIndex + ((j + 1) * 4) - 1];
+				}
+				authTotalCount += authCount;
+				rsnIndex += jumpOffset;
+				/*pasring rsn cap. only if rsn IE*/
+				if (pu8IEs[index] == RSN_IE) {
+					pNewJoinBssParam->rsn_cap[0] = pu8IEs[rsnIndex];
+					pNewJoinBssParam->rsn_cap[1] = pu8IEs[rsnIndex + 1];
+					rsnIndex += 2;
+				}
+				pNewJoinBssParam->rsn_found = true;
+				index += pu8IEs[index + 1] + 2; /* ID,Length bytes and IE body */
+				continue;
+			} else
+				index += pu8IEs[index + 1] + 2;  /* ID,Length bytes and IE body */
+
+		}
+
+
+	}
+
+	return (void *)pNewJoinBssParam;
+
+}
+
+void host_int_freeJoinParams(void *pJoinParams)
+{
+	if ((tstrJoinBssParam *)pJoinParams != NULL)
+		WILC_FREE((tstrJoinBssParam *)pJoinParams);
+	else
+		PRINT_ER("Unable to FREE null pointer\n");
+}
+#endif  /*WILC_PARSE_SCAN_IN_HOST*/
+
+
+/**
+ *  @brief              host_int_addBASession
+ *  @details            Open a block Ack session with the given parameters
+ *  @param[in]          tstrNetworkInfo* ptstrNetworkInfo
+ *  @return
+ *  @author		anoureldin
+ *  @date
+ *  @version		1.0**/
+
+static int host_int_addBASession(WILC_WFIDrvHandle hWFIDrv, char *pBSSID, char TID, short int BufferSize,
+				 short int SessionTimeout, void *drvHandler)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrHostIfBASessionInfo *pBASessionInfo = &strHostIFmsg.uniHostIFmsgBody.strHostIfBASessionInfo;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_ADD_BA_SESSION;
+
+	memcpy(pBASessionInfo->au8Bssid, pBSSID, ETH_ALEN);
+	pBASessionInfo->u8Ted = TID;
+	pBASessionInfo->u16BufferSize = BufferSize;
+	pBASessionInfo->u16SessionTimeout = SessionTimeout;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+}
+
+
+s32 host_int_delBASession(WILC_WFIDrvHandle hWFIDrv, char *pBSSID, char TID)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrHostIfBASessionInfo *pBASessionInfo = &strHostIFmsg.uniHostIFmsgBody.strHostIfBASessionInfo;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_DEL_BA_SESSION;
+
+	memcpy(pBASessionInfo->au8Bssid, pBSSID, ETH_ALEN);
+	pBASessionInfo->u8Ted = TID;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	/*BugID_5222*/
+	down(&hWaitResponse);
+
+	return s32Error;
+}
+
+s32 host_int_del_All_Rx_BASession(WILC_WFIDrvHandle hWFIDrv, char *pBSSID, char TID)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+	tstrHostIfBASessionInfo *pBASessionInfo = &strHostIFmsg.uniHostIFmsgBody.strHostIfBASessionInfo;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_DEL_ALL_RX_BA_SESSIONS;
+
+	memcpy(pBASessionInfo->au8Bssid, pBSSID, ETH_ALEN);
+	pBASessionInfo->u8Ted = TID;
+	strHostIFmsg.drvHandler = hWFIDrv;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	/*BugID_5222*/
+	down(&hWaitResponse);
+
+	return s32Error;
+}
+
+/**
+ *  @brief              host_int_setup_ipaddress
+ *  @details            setup IP in firmware
+ *  @param[in]          Handle to wifi driver
+ *  @return             Error code.
+ *  @author		Abdelrahman Sobhy
+ *  @date
+ *  @version		1.0*/
+s32 host_int_setup_ipaddress(WILC_WFIDrvHandle hWFIDrv, u8 *u16ipadd, u8 idx)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	/* TODO: Enable This feature on softap firmware */
+	return 0;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_SET_IPADDRESS;
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIfSetIP.au8IPAddr = u16ipadd;
+	strHostIFmsg.drvHandler = hWFIDrv;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfSetIP.idx = idx;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+
+
+}
+
+/**
+ *  @brief              host_int_get_ipaddress
+ *  @details            Get IP from firmware
+ *  @param[in]          Handle to wifi driver
+ *  @return             Error code.
+ *  @author		Abdelrahman Sobhy
+ *  @date
+ *  @version		1.0*/
+s32 host_int_get_ipaddress(WILC_WFIDrvHandle hWFIDrv, u8 *u16ipadd, u8 idx)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv = (tstrWILC_WFIDrv *)hWFIDrv;
+	tstrHostIFmsg strHostIFmsg;
+
+	if (pstrWFIDrv == NULL) {
+		WILC_ERRORREPORT(s32Error, WILC_INVALID_ARGUMENT);
+	}
+
+	WILC_memset(&strHostIFmsg, 0, sizeof(tstrHostIFmsg));
+
+	/* prepare the WiphyParams Message */
+	strHostIFmsg.u16MsgId = HOST_IF_MSG_GET_IPADDRESS;
+
+	strHostIFmsg.uniHostIFmsgBody.strHostIfSetIP.au8IPAddr = u16ipadd;
+	strHostIFmsg.drvHandler=hWFIDrv;
+	strHostIFmsg.uniHostIFmsgBody.strHostIfSetIP.idx= idx;
+
+	s32Error = WILC_MsgQueueSend(&gMsgQHostIF, &strHostIFmsg, sizeof(tstrHostIFmsg), NULL);
+	if (s32Error) {
+		WILC_ERRORREPORT(s32Error, s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+
+	}
+
+	return s32Error;
+
+
+}
diff --git a/drivers/staging/wilc1000/host_interface.h b/drivers/staging/wilc1000/host_interface.h
new file mode 100644
index 000000000000..38db740745cd
--- /dev/null
+++ b/drivers/staging/wilc1000/host_interface.h
@@ -0,0 +1,1281 @@
+/*!
+ *  @file	host_interface.h
+ *  @brief	File containg host interface APIs
+ *  @author	zsalah
+ *  @sa		host_interface.c
+ *  @date	8 March 2012
+ *  @version	1.0
+ */
+
+#ifndef HOST_INT_H
+#define HOST_INT_H
+
+#include "coreconfigurator.h"
+#include "coreconfigsimulator.h"
+/*****************************************************************************/
+/*								Macros                                       */
+/*****************************************************************************/
+#define FAIL		0x0000
+#define SUCCESS		0x0001
+
+#define IP_ALEN  4
+
+#define BIT2                    ((u32)(1 << 2))
+#define BIT1                    ((u32)(1 << 1))
+#define BIT0                    ((u32)(1 << 0))
+
+#define AP_MODE		0x01
+#define STATION_MODE	0x02
+#define GO_MODE	0x03
+#define CLIENT_MODE	0x04
+
+
+#define MAX_NUM_STA                 9
+#define ACTIVE_SCAN_TIME			10
+#define PASSIVE_SCAN_TIME			1200
+#define MIN_SCAN_TIME				10
+#define MAX_SCAN_TIME				1200
+#define DEFAULT_SCAN				0
+#define USER_SCAN					BIT0
+#define OBSS_PERIODIC_SCAN			BIT1
+#define OBSS_ONETIME_SCAN			BIT2
+#define GTK_RX_KEY_BUFF_LEN			24
+#define ADDKEY						0x1
+#define REMOVEKEY					0x2
+#define DEFAULTKEY					0x4
+#define ADDKEY_AP					0x8
+#define MAX_NUM_SCANNED_NETWORKS	100 /* 30		// rachel */
+#define MAX_NUM_SCANNED_NETWORKS_SHADOW	130
+#define MAX_NUM_PROBED_SSID            10  /*One more than the number of scanned ssids*/
+#define CHANNEL_SCAN_TIME			250 /* 250 */
+
+#define TX_MIC_KEY_LEN				8
+#define RX_MIC_KEY_LEN				8
+#define PTK_KEY_LEN					16
+
+#define TX_MIC_KEY_MSG_LEN			26
+#define RX_MIC_KEY_MSG_LEN			48
+#define PTK_KEY_MSG_LEN				39
+
+#define PMKSA_KEY_LEN				22
+#define ETH_ALEN  6
+#define PMKID_LEN					16
+#define WILC_MAX_NUM_PMKIDS  16
+#define WILC_SUPP_MCS_SET_SIZE	16
+#define WILC_ADD_STA_LENGTH	40 /* Not including the rates field cause it has variable length*/
+#define SCAN_EVENT_DONE_ABORTED
+/*****************************************************************************/
+/* Data Types                                                                */
+/*****************************************************************************/
+/* typedef unsigned char	uint8; */
+/* typedef signed char     int8; */
+/* typedef unsigned short	uint16; */
+/* typedef unsigned long   uint32; */
+/* typedef uint32   Bool; */
+
+typedef struct {
+	u16 cfg_wid;
+	WID_TYPE_T cfg_type;
+	s8     *pu8Para;
+} cfg_param_t;
+
+typedef struct _tstrStatistics {
+	u8 u8LinkSpeed;
+	s8 s8RSSI;
+	u32 u32TxCount;
+	u32 u32RxCount;
+	u32 u32TxFailureCount;
+
+} tstrStatistics;
+
+
+typedef enum {
+	HOST_IF_IDLE					= 0,
+	HOST_IF_SCANNING				= 1,
+	HOST_IF_CONNECTING				= 2,
+	HOST_IF_WAITING_CONN_RESP		= 3,
+	HOST_IF_CONNECTED				= 4,
+	HOST_IF_P2P_LISTEN				= 5,
+	HOST_IF_FORCE_32BIT			= 0xFFFFFFFF
+} tenuHostIFstate;
+
+typedef struct _tstrHostIFpmkid {
+	u8 bssid[ETH_ALEN];
+	u8 pmkid[PMKID_LEN];
+} tstrHostIFpmkid;
+
+typedef struct _tstrHostIFpmkidAttr {
+	u8 numpmkid;
+	tstrHostIFpmkid pmkidlist[WILC_MAX_NUM_PMKIDS];
+} tstrHostIFpmkidAttr;
+
+typedef enum {
+	AUTORATE	 = 0,
+	MBPS_1	     = 1,
+	MBPS_2	     = 2,
+	MBPS_5_5	     = 5,
+	MBPS_11	     = 11,
+	MBPS_6	     = 6,
+	MBPS_9	     = 9,
+	MBPS_12	     = 12,
+	MBPS_18	     = 18,
+	MBPS_24	     = 24,
+	MBPS_36	     = 36,
+	MBPS_48	     = 48,
+	MBPS_54	     = 54
+} CURRENT_TX_RATE_T;
+
+typedef struct {
+	u32 u32SetCfgFlag;
+	u8 ht_enable;
+	u8 bss_type;
+	u8 auth_type;
+	u16 auth_timeout;
+	u8 power_mgmt_mode;
+	u16 short_retry_limit;
+	u16 long_retry_limit;
+	u16 frag_threshold;
+	u16 rts_threshold;
+	u16 preamble_type;
+	u8 short_slot_allowed;
+	u8 txop_prot_disabled;
+	u16 beacon_interval;
+	u16 dtim_period;
+	SITE_SURVEY_T site_survey_enabled;
+	u16 site_survey_scan_time;
+	u8 scan_source;
+	u16 active_scan_time;
+	u16 passive_scan_time;
+	CURRENT_TX_RATE_T curr_tx_rate;
+
+} tstrCfgParamVal;
+
+typedef enum {
+	RETRY_SHORT		= 1 << 0,
+	RETRY_LONG		= 1 << 1,
+	FRAG_THRESHOLD	= 1 << 2,
+	RTS_THRESHOLD	= 1 << 3,
+	BSS_TYPE  = 1 << 4,
+	AUTH_TYPE = 1 << 5,
+	AUTHEN_TIMEOUT = 1 << 6,
+	POWER_MANAGEMENT = 1 << 7,
+	PREAMBLE = 1 << 8,
+	SHORT_SLOT_ALLOWED = 1 << 9,
+	TXOP_PROT_DISABLE = 1 << 10,
+	BEACON_INTERVAL = 1 << 11,
+	DTIM_PERIOD = 1 << 12,
+	SITE_SURVEY = 1 << 13,
+	SITE_SURVEY_SCAN_TIME = 1 << 14,
+	ACTIVE_SCANTIME = 1 << 15,
+	PASSIVE_SCANTIME = 1 << 16,
+	CURRENT_TX_RATE = 1 << 17,
+	HT_ENABLE = 1 << 18,
+} tenuCfgParam;
+
+typedef struct {
+	u8 au8bssid[6];
+	s8 s8rssi;
+} tstrFoundNetworkInfo;
+
+typedef enum {SCAN_EVENT_NETWORK_FOUND  = 0,
+	      SCAN_EVENT_DONE = 1,
+	      SCAN_EVENT_ABORTED = 2,
+	      SCAN_EVENT_FORCE_32BIT  = 0xFFFFFFFF} tenuScanEvent;
+
+typedef enum {
+	CONN_DISCONN_EVENT_CONN_RESP		= 0,
+	CONN_DISCONN_EVENT_DISCONN_NOTIF	= 1,
+	CONN_DISCONN_EVENT_FORCE_32BIT	 = 0xFFFFFFFF
+} tenuConnDisconnEvent;
+
+typedef enum {
+	WEP,
+	WPARxGtk,
+	/* WPATxGtk, */
+	WPAPtk,
+	PMKSA,
+} tenuKeyType;
+
+
+/*Scan callBack function definition*/
+typedef void (*tWILCpfScanResult)(tenuScanEvent, tstrNetworkInfo *, void *, void *);
+
+/*Connect callBack function definition*/
+typedef void (*tWILCpfConnectResult)(tenuConnDisconnEvent,
+				     tstrConnectInfo *,
+				     u8,
+				     tstrDisconnectNotifInfo *,
+				     void *);
+
+#ifdef WILC_P2P
+typedef void (*tWILCpfRemainOnChanExpired)(void *, u32);  /*Remain on channel expiration callback function*/
+typedef void (*tWILCpfRemainOnChanReady)(void *); /*Remain on channel callback function*/
+#endif
+
+/* typedef u32 WILC_WFIDrvHandle; */
+typedef struct {
+	s32 s32Dummy;
+} *WILC_WFIDrvHandle;
+
+/*!
+ *  @struct             tstrRcvdNetworkInfo
+ *  @brief		Structure to hold Received Asynchronous Network info
+ *  @details
+ *  @todo
+ *  @sa
+ *  @author		Mostafa Abu Bakr
+ *  @date		25 March 2012
+ *  @version		1.0
+ */
+typedef struct _tstrRcvdNetworkInfo {
+	u8 *pu8Buffer;
+	u32 u32Length;
+} tstrRcvdNetworkInfo;
+
+/*BugID_4156*/
+typedef struct _tstrHiddenNetworkInfo {
+	u8  *pu8ssid;
+	u8 u8ssidlen;
+
+} tstrHiddenNetworkInfo;
+
+typedef struct _tstrHiddenNetwork {
+	/* MAX_SSID_LEN */
+	tstrHiddenNetworkInfo *pstrHiddenNetworkInfo;
+	u8 u8ssidnum;
+
+} tstrHiddenNetwork;
+
+typedef struct {
+	/* Scan user call back function */
+	tWILCpfScanResult pfUserScanResult;
+
+	/* User specific parameter to be delivered through the Scan User Callback function */
+	void *u32UserScanPvoid;
+
+	u32 u32RcvdChCount;
+	tstrFoundNetworkInfo astrFoundNetworkInfo[MAX_NUM_SCANNED_NETWORKS];
+} tstrWILC_UsrScanReq;
+
+typedef struct {
+	u8 *pu8bssid;
+	u8 *pu8ssid;
+	u8 u8security;
+	AUTHTYPE_T tenuAuth_type;
+	size_t ssidLen;
+	u8 *pu8ConnReqIEs;
+	size_t ConnReqIEsLen;
+	/* Connect user call back function */
+	tWILCpfConnectResult pfUserConnectResult;
+	bool IsHTCapable;
+	/* User specific parameter to be delivered through the Connect User Callback function */
+	void *u32UserConnectPvoid;
+} tstrWILC_UsrConnReq;
+
+typedef struct {
+	u32 u32Address;
+} tstrHostIfSetDrvHandler;
+
+typedef struct {
+	u32 u32Mode;
+} tstrHostIfSetOperationMode;
+
+/*BugID_5077*/
+typedef struct {
+	u8 u8MacAddress[ETH_ALEN];
+} tstrHostIfSetMacAddress;
+
+/*BugID_5213*/
+typedef struct {
+	u8 *u8MacAddress;
+} tstrHostIfGetMacAddress;
+
+/*BugID_5222*/
+typedef struct {
+	u8 au8Bssid[ETH_ALEN];
+	u8 u8Ted;
+	u16 u16BufferSize;
+	u16 u16SessionTimeout;
+} tstrHostIfBASessionInfo;
+
+#ifdef WILC_P2P
+typedef struct {
+	u16 u16Channel;
+	u32 u32duration;
+	tWILCpfRemainOnChanExpired pRemainOnChanExpired;
+	tWILCpfRemainOnChanReady pRemainOnChanReady;
+	void *pVoid;
+	u32 u32ListenSessionID;
+} tstrHostIfRemainOnChan;
+
+typedef struct {
+
+	bool bReg;
+	u16 u16FrameType;
+	u8 u8Regid;
+
+
+} tstrHostIfRegisterFrame;
+
+
+#define   ACTION         0xD0
+#define   PROBE_REQ   0x40
+#define   PROBE_RESP  0x50
+#define   ACTION_FRM_IDX   0
+#define   PROBE_REQ_IDX     1
+
+
+enum p2p_listen_state {
+	P2P_IDLE,
+	P2P_LISTEN,
+	P2P_GRP_FORMATION
+};
+
+#endif
+typedef struct {
+	/* Scan user structure */
+	tstrWILC_UsrScanReq strWILC_UsrScanReq;
+
+	/* Connect User structure */
+	tstrWILC_UsrConnReq strWILC_UsrConnReq;
+
+	#ifdef WILC_P2P
+	/*Remain on channel struvture*/
+	tstrHostIfRemainOnChan strHostIfRemainOnChan;
+	u8 u8RemainOnChan_pendingreq;
+	u64 u64P2p_MgmtTimeout;
+	u8 u8P2PConnect;
+	#endif
+
+	tenuHostIFstate enuHostIFstate;
+
+	/* bool bPendingConnRequest; */
+
+	#ifndef CONNECT_DIRECT
+	u32 u32SurveyResultsCount;
+	wid_site_survey_reslts_s astrSurveyResults[MAX_NUM_SCANNED_NETWORKS];
+	#endif
+
+	u8 au8AssociatedBSSID[ETH_ALEN];
+	tstrCfgParamVal strCfgValues;
+/* semaphores */
+	struct semaphore gtOsCfgValuesSem;
+	struct semaphore hSemTestKeyBlock;
+
+	struct semaphore hSemTestDisconnectBlock;
+	struct semaphore hSemGetRSSI;
+	struct semaphore hSemGetLINKSPEED;
+	struct semaphore hSemGetCHNL;
+	struct semaphore hSemInactiveTime;
+/* timer handlers */
+	WILC_TimerHandle hScanTimer;
+	WILC_TimerHandle hConnectTimer;
+	#ifdef WILC_P2P
+	WILC_TimerHandle hRemainOnChannel;
+	#endif
+
+	bool IFC_UP;
+} tstrWILC_WFIDrv;
+
+/*!
+ *  @enum               tenuWILC_StaFlag
+ *  @brief			Used to decode the station flag set and mask in tstrWILC_AddStaParam
+ *  @details
+ *  @todo
+ *  @sa			tstrWILC_AddStaParam, enum nl80211_sta_flags
+ *  @author		Enumeraion's creator
+ *  @date			12 July 2012
+ *  @version		1.0 Description
+ */
+
+typedef enum {
+	WILC_STA_FLAG_INVALID = 0,
+	WILC_STA_FLAG_AUTHORIZED,                       /*!<  station is authorized (802.1X)*/
+	WILC_STA_FLAG_SHORT_PREAMBLE,   /*!< station is capable of receiving frames	with short barker preamble*/
+	WILC_STA_FLAG_WME,                              /*!< station is WME/QoS capable*/
+	WILC_STA_FLAG_MFP,                                      /*!< station uses management frame protection*/
+	WILC_STA_FLAG_AUTHENTICATED             /*!< station is authenticated*/
+} tenuWILC_StaFlag;
+
+typedef struct {
+	u8 au8BSSID[ETH_ALEN];
+	u16 u16AssocID;
+	u8 u8NumRates;
+	const u8 *pu8Rates;
+	bool bIsHTSupported;
+	u16 u16HTCapInfo;
+	u8 u8AmpduParams;
+	u8 au8SuppMCsSet[16];
+	u16 u16HTExtParams;
+	u32 u32TxBeamformingCap;
+	u8 u8ASELCap;
+	u16 u16FlagsMask;               /*<! Determines which of u16FlagsSet were changed>*/
+	u16 u16FlagsSet;                /*<! Decoded according to tenuWILC_StaFlag */
+} tstrWILC_AddStaParam;
+
+/* extern void CfgDisconnected(void* pUserVoid, u16 u16reason, u8 * ie, size_t ie_len); */
+
+/*****************************************************************************/
+/*																			 */
+/*							Host Interface API								 */
+/*																			 */
+/*****************************************************************************/
+
+/**
+ *  @brief              removes wpa/wpa2 keys
+ *  @details    only in BSS STA mode if External Supplicant support is enabled.
+ *                              removes all WPA/WPA2 station key entries from MAC hardware.
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  6 bytes of Station Adress in the station entry table
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_remove_key(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8StaAddress);
+/**
+ *  @brief              removes WEP key
+ *  @details    valid only in BSS STA mode if External Supplicant support is enabled.
+ *                              remove a WEP key entry from MAC HW.
+ *                              The BSS Station automatically finds the index of the entry using its
+ *                              BSS ID and removes that entry from the MAC hardware.
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  6 bytes of Station Adress in the station entry table
+ *  @return             Error code indicating success/failure
+ *  @note               NO need for the STA add since it is not used for processing
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_remove_wep_key(WILC_WFIDrvHandle hWFIDrv, u8 u8Index);
+/**
+ *  @brief              sets WEP deafault key
+ *  @details    Sets the index of the WEP encryption key in use,
+ *                              in the key table
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  key index ( 0, 1, 2, 3)
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_WEPDefaultKeyID(WILC_WFIDrvHandle hWFIDrv, u8 u8Index);
+
+/**
+ *  @brief              sets WEP deafault key
+ *  @details    valid only in BSS STA mode if External Supplicant support is enabled.
+ *                              sets WEP key entry into MAC hardware when it receives the
+ *                              corresponding request from NDIS.
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing WEP Key in the following format
+ *|---------------------------------------|
+ *|Key ID Value | Key Length |	Key		|
+ *|-------------|------------|------------|
+ |	1byte	  |		1byte  | Key Length	|
+ ||---------------------------------------|
+ |
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_add_wep_key_bss_sta(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8WepKey, u8 u8WepKeylen, u8 u8Keyidx);
+/**
+ *  @brief              host_int_add_wep_key_bss_ap
+ *  @details    valid only in AP mode if External Supplicant support is enabled.
+ *                              sets WEP key entry into MAC hardware when it receives the
+ *                              corresponding request from NDIS.
+ *  @param[in,out] handle to the wifi driver
+ *
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mdaftedar
+ *  @date		28 Feb 2013
+ *  @version		1.0
+ */
+s32 host_int_add_wep_key_bss_ap(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8WepKey, u8 u8WepKeylen, u8 u8Keyidx, u8 u8mode, AUTHTYPE_T tenuAuth_type);
+
+/**
+ *  @brief              adds ptk Key
+ *  @details
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing PTK Key in the following format
+ *|-------------------------------------------------------------------------|
+ *|Sta Adress | Key Length |	Temporal Key | Rx Michael Key |Tx Michael Key |
+ *|-----------|------------|---------------|----------------|---------------|
+ |	6 bytes |	1byte	 |   16 bytes	 |	  8 bytes	  |	   8 bytes	  |
+ ||-------------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_add_ptk(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8Ptk, u8 u8PtkKeylen,
+			     const u8 *mac_addr, const u8 *pu8RxMic, const u8 *pu8TxMic, u8 mode, u8 u8Ciphermode, u8 u8Idx);
+
+/**
+ *  @brief              host_int_get_inactive_time
+ *  @details
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing inactive time
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mdaftedar
+ *  @date		15 April 2013
+ *  @version		1.0
+ */
+s32 host_int_get_inactive_time(WILC_WFIDrvHandle hWFIDrv, const u8 *mac, u32 *pu32InactiveTime);
+
+/**
+ *  @brief              adds Rx GTk Key
+ *  @details
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing Rx GTK Key in the following format
+ *|----------------------------------------------------------------------------|
+ *|Sta Address | Key RSC | KeyID | Key Length | Temporal Key	| Rx Michael Key |
+ *|------------|---------|-------|------------|---------------|----------------|
+ |	6 bytes	 | 8 byte  |1 byte |  1 byte	|   16 bytes	|	  8 bytes	 |
+ ||----------------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_add_rx_gtk(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8RxGtk, u8 u8GtkKeylen,
+				u8 u8KeyIdx, u32 u32KeyRSClen, const u8 *KeyRSC,
+				const u8 *pu8RxMic, const u8 *pu8TxMic, u8 mode, u8 u8Ciphermode);
+
+
+/**
+ *  @brief              adds Tx GTk Key
+ *  @details
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing Tx GTK Key in the following format
+ *|----------------------------------------------------|
+ | KeyID | Key Length | Temporal Key	| Tx Michael Key |
+ ||-------|------------|--------------|----------------|
+ ||1 byte |  1 byte	 |   16 bytes	|	  8 bytes	 |
+ ||----------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_add_tx_gtk(WILC_WFIDrvHandle hWFIDrv, u8 u8KeyLen, u8 *pu8TxGtk, u8 u8KeyIdx);
+
+/**
+ *  @brief              caches the pmkid
+ *  @details    valid only in BSS STA mode if External Supplicant
+ *                              support is enabled. This Function sets the PMKID in firmware
+ *                              when host drivr receives the corresponding request from NDIS.
+ *                              The firmware then includes theset PMKID in the appropriate
+ *                              management frames
+ *  @param[in,out] handle to the wifi driver
+ *  @param[in]  message containing PMKID Info in the following format
+ *|-----------------------------------------------------------------|
+ *|NumEntries |	BSSID[1] | PMKID[1] |  ...	| BSSID[K] | PMKID[K] |
+ *|-----------|------------|----------|-------|----------|----------|
+ |	   1	|		6	 |   16		|  ...	|	 6	   |	16	  |
+ ||-----------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_set_pmkid_info(WILC_WFIDrvHandle hWFIDrv, tstrHostIFpmkidAttr *pu8PmkidInfoArray);
+/**
+ *  @brief              gets the cached the pmkid info
+ *  @details    valid only in BSS STA mode if External Supplicant
+ *                              support is enabled. This Function sets the PMKID in firmware
+ *                              when host drivr receives the corresponding request from NDIS.
+ *                              The firmware then includes theset PMKID in the appropriate
+ *                              management frames
+ *  @param[in,out] handle to the wifi driver,
+ *
+ *                                message containing PMKID Info in the following format
+ *|-----------------------------------------------------------------|
+ *|NumEntries |	BSSID[1] | PMKID[1] |  ...	| BSSID[K] | PMKID[K] |
+ *|-----------|------------|----------|-------|----------|----------|
+ |	   1	|		6	 |   16		|  ...	|	 6	   |	16	  |
+ ||-----------------------------------------------------------------|
+ *  @param[in]
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_get_pmkid_info(WILC_WFIDrvHandle hWFIDrv, u8 *pu8PmkidInfoArray,
+				    u32 u32PmkidInfoLen);
+
+/**
+ *  @brief              sets the pass phrase
+ *  @details    AP/STA mode. This function gives the pass phrase used to
+ *                              generate the Pre-Shared Key when WPA/WPA2 is enabled
+ *                              The length of the field can vary from 8 to 64 bytes,
+ *                              the lower layer should get the
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]   String containing PSK
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_RSNAConfigPSKPassPhrase(WILC_WFIDrvHandle hWFIDrv, u8 *pu8PassPhrase,
+						 u8 u8Psklength);
+/**
+ *  @brief              gets the pass phrase
+ *  @details    AP/STA mode. This function gets the pass phrase used to
+ *                              generate the Pre-Shared Key when WPA/WPA2 is enabled
+ *                              The length of the field can vary from 8 to 64 bytes,
+ *                              the lower layer should get the
+ *  @param[in,out] handle to the wifi driver,
+ *                                String containing PSK
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_RSNAConfigPSKPassPhrase(WILC_WFIDrvHandle hWFIDrv,
+						 u8 *pu8PassPhrase, u8 u8Psklength);
+
+/**
+ *  @brief              gets mac address
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mdaftedar
+ *  @date		19 April 2012
+ *  @version		1.0
+ */
+s32 host_int_get_MacAddress(WILC_WFIDrvHandle hWFIDrv, u8 *pu8MacAddress);
+
+/**
+ *  @brief              sets mac address
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		mabubakr
+ *  @date		16 July 2012
+ *  @version		1.0
+ */
+s32 host_int_set_MacAddress(WILC_WFIDrvHandle hWFIDrv, u8 *pu8MacAddress);
+
+/**
+ *  @brief              wait until msg q is empty
+ *  @details
+ *  @param[in,out]
+ *
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		asobhy
+ *  @date		19 march 2014
+ *  @version		1.0
+ */
+s32 host_int_wait_msg_queue_idle(void);
+
+/**
+ *  @brief              gets the site survey results
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                                Message containing  site survey results in the
+ *                                following formate
+ *|---------------------------------------------------|
+ | MsgLength | fragNo.	| MsgBodyLength	| MsgBody	|
+ ||-----------|-----------|---------------|-----------|
+ |	 1		|	  1		|		1		|	 1		|
+ | -----------------------------------------	 |  ----------------
+ |
+ ||---------------------------------------|
+ | Network1 | Netweork2 | ... | Network5 |
+ ||---------------------------------------|
+ |	44	   |	44	   | ... |	 44		|
+ | -------------------------- | ---------------------------------------
+ |
+ ||---------------------------------------------------------------------|
+ | SSID | BSS Type | Channel | Security Status| BSSID | RSSI |Reserved |
+ ||------|----------|---------|----------------|-------|------|---------|
+ |  33  |	 1	  |	  1		|		1		 |	  6	 |	 1	|	 1	  |
+ ||---------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+#ifndef CONNECT_DIRECT
+s32 host_int_get_site_survey_results(WILC_WFIDrvHandle hWFIDrv,
+					     u8 ppu8RcvdSiteSurveyResults[][MAX_SURVEY_RESULT_FRAG_SIZE],
+					     u32 u32MaxSiteSrvyFragLen);
+#endif
+
+/**
+ *  @brief              sets a start scan request
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Scan Source one of the following values
+ *                              DEFAULT_SCAN        0
+ *                              USER_SCAN           BIT0
+ *                              OBSS_PERIODIC_SCAN  BIT1
+ *                              OBSS_ONETIME_SCAN   BIT2
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_set_start_scan_req(WILC_WFIDrvHandle hWFIDrv, u8 scanSource);
+/**
+ *  @brief              gets scan source of the last scan
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                              Scan Source one of the following values
+ *                              DEFAULT_SCAN        0
+ *                              USER_SCAN           BIT0
+ *                              OBSS_PERIODIC_SCAN  BIT1
+ *                              OBSS_ONETIME_SCAN   BIT2
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_start_scan_req(WILC_WFIDrvHandle hWFIDrv, u8 *pu8ScanSource);
+
+/**
+ *  @brief              sets a join request
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Index of the bss descriptor
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_set_join_req(WILC_WFIDrvHandle hWFIDrv, u8 *pu8bssid,
+				  const u8 *pu8ssid, size_t ssidLen,
+				  const u8 *pu8IEs, size_t IEsLen,
+				  tWILCpfConnectResult pfConnectResult, void *pvUserArg,
+				  u8 u8security, AUTHTYPE_T tenuAuth_type,
+				  u8 u8channel,
+				  void *pJoinParams);
+
+/**
+ *  @brief              Flush a join request parameters to FW, but actual connection
+ *  @details    The function is called in situation where WILC is connected to AP and
+ *                      required to switch to hybrid FW for P2P connection
+ *  @param[in] handle to the wifi driver,
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		Amr Abdel-Moghny
+ *  @date		19 DEC 2013
+ *  @version		8.0
+ */
+
+s32 host_int_flush_join_req(WILC_WFIDrvHandle hWFIDrv);
+
+
+/**
+ *  @brief              disconnects from the currently associated network
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Reason Code of the Disconnection
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_disconnect(WILC_WFIDrvHandle hWFIDrv, u16 u16ReasonCode);
+
+/**
+ *  @brief              disconnects a sta
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Association Id of the station to be disconnected
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_disconnect_station(WILC_WFIDrvHandle hWFIDrv, u8 assoc_id);
+/**
+ *  @brief              gets a Association request info
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                              Message containg assoc. req info in the following format
+ * ------------------------------------------------------------------------
+ |                        Management Frame Format                    |
+ ||-------------------------------------------------------------------|
+ ||Frame Control|Duration|DA|SA|BSSID|Sequence Control|Frame Body|FCS |
+ ||-------------|--------|--|--|-----|----------------|----------|----|
+ | 2           |2       |6 |6 |6    |		2       |0 - 2312  | 4  |
+ ||-------------------------------------------------------------------|
+ |                                                                   |
+ |             Association Request Frame - Frame Body                |
+ ||-------------------------------------------------------------------|
+ | Capability Information | Listen Interval | SSID | Supported Rates |
+ ||------------------------|-----------------|------|-----------------|
+ |			2            |		 2         | 2-34 |		3-10        |
+ | ---------------------------------------------------------------------
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_get_assoc_req_info(WILC_WFIDrvHandle hWFIDrv, u8 *pu8AssocReqInfo,
+					u32 u32AssocReqInfoLen);
+/**
+ *  @brief              gets a Association Response info
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                              Message containg assoc. resp info
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+
+s32 host_int_get_assoc_res_info(WILC_WFIDrvHandle hWFIDrv, u8 *pu8AssocRespInfo,
+					u32 u32MaxAssocRespInfoLen, u32 *pu32RcvdAssocRespInfoLen);
+/**
+ *  @brief              gets a Association Response info
+ *  @details    Valid only in STA mode. This function gives the RSSI
+ *                              values observed in all the channels at the time of scanning.
+ *                              The length of the field is 1 greater that the total number of
+ *                              channels supported. Byte 0 contains the number of channels while
+ *                              each of Byte N contains	the observed RSSI value for the channel index N.
+ *  @param[in,out] handle to the wifi driver,
+ *                              array of scanned channels' RSSI
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_rx_power_level(WILC_WFIDrvHandle hWFIDrv, u8 *pu8RxPowerLevel,
+					u32 u32RxPowerLevelLen);
+
+/**
+ *  @brief              sets a channel
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Index of the channel to be set
+ *|-------------------------------------------------------------------|
+ |          CHANNEL1      CHANNEL2 ....		             CHANNEL14	|
+ |  Input:         1             2					            14	|
+ ||-------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_set_mac_chnl_num(WILC_WFIDrvHandle hWFIDrv, u8 u8ChNum);
+
+/**
+ *  @brief              gets the current channel index
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                              current channel index
+ *|-----------------------------------------------------------------------|
+ |          CHANNEL1      CHANNEL2 ....                     CHANNEL14	|
+ |  Input:         1             2                                 14	|
+ ||-----------------------------------------------------------------------|
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_host_chnl_num(WILC_WFIDrvHandle hWFIDrv, u8 *pu8ChNo);
+/**
+ *  @brief              gets the sta rssi
+ *  @details    gets the currently maintained RSSI value for the station.
+ *                              The received signal strength value in dB.
+ *                              The range of valid values is -128 to 0.
+ *  @param[in,out] handle to the wifi driver,
+ *                              rssi value in dB
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_get_rssi(WILC_WFIDrvHandle hWFIDrv, s8 *ps8Rssi);
+s32 host_int_get_link_speed(WILC_WFIDrvHandle hWFIDrv, s8 *ps8lnkspd);
+/**
+ *  @brief              scans a set of channels
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]		Scan source
+ *                              Scan Type	PASSIVE_SCAN = 0,
+ *                                                      ACTIVE_SCAN  = 1
+ *                              Channels Array
+ *                              Channels Array length
+ *                              Scan Callback function
+ *                              User Argument to be delivered back through the Scan Cllback function
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_scan(WILC_WFIDrvHandle hWFIDrv, u8 u8ScanSource,
+			  u8 u8ScanType, u8 *pu8ChnlFreqList,
+			  u8 u8ChnlListLen, const u8 *pu8IEs,
+			  size_t IEsLen, tWILCpfScanResult ScanResult,
+			  void *pvUserArg, tstrHiddenNetwork *pstrHiddenNetwork);
+/**
+ *  @brief              sets configuration wids values
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	WID, WID value
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 hif_set_cfg(WILC_WFIDrvHandle hWFIDrv, tstrCfgParamVal *pstrCfgParamVal);
+
+/**
+ *  @brief              gets configuration wids values
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *                              WID value
+ *  @param[in]	WID,
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 hif_get_cfg(WILC_WFIDrvHandle hWFIDrv, u16 u16WID, u16 *pu16WID_Value);
+/*****************************************************************************/
+/*							Notification Functions							 */
+/*****************************************************************************/
+/**
+ *  @brief              notifies host with join and leave requests
+ *  @details    This function prepares an Information frame having the
+ *                              information about a joining/leaving station.
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	6 byte Sta Adress
+ *                              Join or leave flag:
+ *                              Join = 1,
+ *                              Leave =0
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+void host_int_send_join_leave_info_to_host
+	(u16 assocId, u8 *stationAddr, bool joining);
+
+/**
+ *  @brief              notifies host with stations found in scan
+ *  @details    sends the beacon/probe response from scan
+ *  @param[in,out] handle to the wifi driver,
+ *  @param[in]	Sta Address,
+ *                              Frame length,
+ *                              Rssi of the Station found
+ *  @return             Error code indicating success/failure
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+void host_int_send_network_info_to_host
+	(u8 *macStartAddress, u16 u16RxFrameLen, s8 s8Rssi);
+
+/**
+ *  @brief              host interface initialization function
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_init(WILC_WFIDrvHandle *phWFIDrv);
+
+/**
+ *  @brief              host interface initialization function
+ *  @details
+ *  @param[in,out] handle to the wifi driver,
+ *  @note
+ *  @author		zsalah
+ *  @date		8 March 2012
+ *  @version		1.0
+ */
+s32 host_int_deinit(WILC_WFIDrvHandle hWFIDrv);
+
+
+/*!
+ *  @fn		s32 host_int_add_beacon(WILC_WFIDrvHandle hWFIDrv,u8 u8Index)
+ *  @brief		Sends a beacon to the firmware to be transmitted over the air
+ *  @details
+ *  @param[in,out]	hWFIDrv		handle to the wifi driver
+ *  @param[in]	u32Interval	Beacon Interval. Period between two successive beacons on air
+ *  @param[in]	u32DTIMPeriod DTIM Period. Indicates how many Beacon frames
+ *              (including the current frame) appear before the next DTIM
+ *  @param[in]	u32Headlen	Length of the head buffer in bytes
+ *  @param[in]	pu8Head		Pointer to the beacon's head buffer. Beacon's head
+ *		is the part from the beacon's start till the TIM element, NOT including the TIM
+ *  @param[in]	u32Taillen	Length of the tail buffer in bytes
+ *  @param[in]	pu8Tail		Pointer to the beacon's tail buffer. Beacon's tail
+ *		starts just after the TIM inormation element
+ *  @return	0 for Success, error otherwise
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		10 Julys 2012
+ *  @version		1.0 Description
+ *
+ */
+s32 host_int_add_beacon(WILC_WFIDrvHandle hWFIDrv, u32 u32Interval,
+				u32 u32DTIMPeriod,
+				u32 u32HeadLen, u8 *pu8Head,
+				u32 u32TailLen, u8 *pu8tail);
+
+
+/*!
+ *  @fn		s32 host_int_del_beacon(WILC_WFIDrvHandle hWFIDrv)
+ *  @brief		Removes the beacon and stops trawilctting it over the air
+ *  @details
+ *  @param[in,out]	hWFIDrv		handle to the wifi driver
+ *  @return	0 for Success, error otherwise
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		10 Julys 2012
+ *  @version		1.0 Description
+ */
+s32 host_int_del_beacon(WILC_WFIDrvHandle hWFIDrv);
+
+/*!
+ *  @fn		s32 host_int_add_station(WILC_WFIDrvHandle hWFIDrv, tstrWILC_AddStaParam strStaParams)
+ *  @brief		Notifies the firmware with a new associated stations
+ *  @details
+ *  @param[in,out]	hWFIDrv		handle to the wifi driver
+ *  @param[in]	pstrStaParams	Station's parameters
+ *  @return	0 for Success, error otherwise
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		12 July 2012
+ *  @version		1.0 Description
+ */
+s32 host_int_add_station(WILC_WFIDrvHandle hWFIDrv, tstrWILC_AddStaParam *pstrStaParams);
+
+/*!
+ *  @fn		s32 host_int_del_allstation(WILC_WFIDrvHandle hWFIDrv, const u8* pu8MacAddr)
+ *  @brief		Deauthenticates clients when group is terminating
+ *  @details
+ *  @param[in,out]	hWFIDrv		handle to the wifi driver
+ *  @param[in]	pu8MacAddr	Station's mac address
+ *  @return	0 for Success, error otherwise
+ *  @todo
+ *  @sa
+ *  @author		Mai Daftedar
+ *  @date		09 April 2014
+ *  @version		1.0 Description
+ */
+s32 host_int_del_allstation(WILC_WFIDrvHandle hWFIDrv, u8 pu8MacAddr[][ETH_ALEN]);
+
+/*!
+ *  @fn		s32 host_int_del_station(WILC_WFIDrvHandle hWFIDrv, u8* pu8MacAddr)
+ *  @brief		Notifies the firmware with a new deleted station
+ *  @details
+ *  @param[in,out]	hWFIDrv		handle to the wifi driver
+ *  @param[in]	pu8MacAddr	Station's mac address
+ *  @return	0 for Success, error otherwise
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		15 July 2012
+ *  @version		1.0 Description
+ */
+s32 host_int_del_station(WILC_WFIDrvHandle hWFIDrv, const u8 *pu8MacAddr);
+
+/*!
+ *  @fn		s32 host_int_edit_station(WILC_WFIDrvHandle hWFIDrv, tstrWILC_AddStaParam strStaParams)
+ *  @brief		Notifies the firmware with new parameters of an already associated station
+ *  @details
+ *  @param[in,out]	hWFIDrv		handle to the wifi driver
+ *  @param[in]	pstrStaParams	Station's parameters
+ *  @return	0 for Success, error otherwise
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		15 July 2012
+ *  @version		1.0 Description
+ */
+s32 host_int_edit_station(WILC_WFIDrvHandle hWFIDrv, tstrWILC_AddStaParam *pstrStaParams);
+
+/*!
+ *  @fn		s32 host_int_set_power_mgmt(WILC_WFIDrvHandle hWFIDrv, bool bIsEnabled, u32 u32Timeout)
+ *  @brief		Set the power management mode to enabled or disabled
+ *  @details
+ *  @param[in,out]	hWFIDrv		handle to the wifi driver
+ *  @param[in]	bIsEnabled	TRUE if enabled, FALSE otherwise
+ *  @param[in]	u32Timeout	A timeout value of -1 allows the driver to adjust
+ *							the dynamic ps timeout value
+ *  @return	0 for Success, error otherwise
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		24 November 2012
+ *  @version		1.0 Description
+ */
+s32 host_int_set_power_mgmt(WILC_WFIDrvHandle hWFIDrv, bool bIsEnabled, u32 u32Timeout);
+/*  @param[in,out]	hWFIDrv		handle to the wifi driver
+ *  @param[in]	bIsEnabled	TRUE if enabled, FALSE otherwise
+ *  @param[in]	u8count		count of mac address entries in the filter table
+ *
+ *  @return	0 for Success, error otherwise
+ *  @todo
+ *  @sa
+ *  @author		Adham Abozaeid
+ *  @date		24 November 2012
+ *  @version		1.0 Description
+ */
+s32 host_int_setup_multicast_filter(WILC_WFIDrvHandle hWFIDrv, bool bIsEnabled, u32 u32count);
+/**
+ *  @brief           host_int_setup_ipaddress
+ *  @details       set IP address on firmware
+ *  @param[in]
+ *  @return         Error code.
+ *  @author		Abdelrahman Sobhy
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_setup_ipaddress(WILC_WFIDrvHandle hWFIDrv, u8 *pu8IPAddr, u8 idx);
+
+
+/**
+ *  @brief           host_int_delBASession
+ *  @details       Delete single Rx BA session
+ *  @param[in]
+ *  @return         Error code.
+ *  @author		Abdelrahman Sobhy
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_delBASession(WILC_WFIDrvHandle hWFIDrv, char *pBSSID, char TID);
+
+/**
+ *  @brief           host_int_delBASession
+ *  @details       Delete all Rx BA session
+ *  @param[in]
+ *  @return         Error code.
+ *  @author		Abdelrahman Sobhy
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_del_All_Rx_BASession(WILC_WFIDrvHandle hWFIDrv, char *pBSSID, char TID);
+
+
+/**
+ *  @brief           host_int_get_ipaddress
+ *  @details       get IP address on firmware
+ *  @param[in]
+ *  @return         Error code.
+ *  @author		Abdelrahman Sobhy
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_get_ipaddress(WILC_WFIDrvHandle hWFIDrv, u8 *pu8IPAddr, u8 idx);
+
+#ifdef WILC_P2P
+/**
+ *  @brief           host_int_remain_on_channel
+ *  @details
+ *  @param[in]
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_remain_on_channel(WILC_WFIDrvHandle hWFIDrv, u32 u32SessionID, u32 u32duration, u16 chan, tWILCpfRemainOnChanExpired RemainOnChanExpired, tWILCpfRemainOnChanReady RemainOnChanReady, void *pvUserArg);
+
+/**
+ *  @brief              host_int_ListenStateExpired
+ *  @details
+ *  @param[in]          Handle to wifi driver
+ *                              Duration to remain on channel
+ *                              Channel to remain on
+ *                              Pointer to fn to be called on receive frames in listen state
+ *                              Pointer to remain-on-channel expired fn
+ *                              Priv
+ *  @return             Error code.
+ *  @author
+ *  @date
+ *  @version		1.0
+ */
+s32 host_int_ListenStateExpired(WILC_WFIDrvHandle hWFIDrv, u32 u32SessionID);
+
+/**
+ *  @brief           host_int_frame_register
+ *  @details
+ *  @param[in]
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_frame_register(WILC_WFIDrvHandle hWFIDrv, u16 u16FrameType, bool bReg);
+#endif
+/**
+ *  @brief           host_int_set_wfi_drv_handler
+ *  @details
+ *  @param[in]
+ *  @return         Error code.
+ *  @author
+ *  @date
+ *  @version	1.0
+ */
+s32 host_int_set_wfi_drv_handler(u32 u32address);
+s32 host_int_set_operation_mode(WILC_WFIDrvHandle hWFIDrv, u32 u32mode);
+
+static s32 Handle_ScanDone(void *drvHandler, tenuScanEvent enuEvent);
+
+static int host_int_addBASession(WILC_WFIDrvHandle hWFIDrv, char *pBSSID, char TID, short int BufferSize,
+				 short int SessionTimeout, void *drvHandler);
+
+
+void host_int_freeJoinParams(void *pJoinParams);
+
+s32 host_int_get_statistics(WILC_WFIDrvHandle hWFIDrv, tstrStatistics *pstrStatistics);
+
+/*****************************************************************************/
+/*																			 */
+/*									EOF										 */
+/*																			 */
+/*****************************************************************************/
+#endif
diff --git a/drivers/staging/wilc1000/linux_mon.c b/drivers/staging/wilc1000/linux_mon.c
new file mode 100644
index 000000000000..f5296f53a3d2
--- /dev/null
+++ b/drivers/staging/wilc1000/linux_mon.c
@@ -0,0 +1,598 @@
+/*!
+ *  @file	linux_mon.c
+ *  @brief	File Operations OS wrapper functionality
+ *  @author	mdaftedar
+ *  @sa		wilc_wfi_netdevice.h
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+
+#ifndef SIMULATION
+#include "wilc_wfi_cfgoperations.h"
+#include "linux_wlan_common.h"
+#include "wilc_wlan_if.h"
+#include "wilc_wlan.h"
+#endif
+#ifdef WILC_FULLY_HOSTING_AP
+#include "wilc_host_ap.h"
+#endif
+#ifdef WILC_AP_EXTERNAL_MLME
+#ifdef SIMULATION
+#include "wilc_wfi_cfgoperations.h"
+#endif
+
+struct wilc_wfi_radiotap_hdr {
+	struct ieee80211_radiotap_header hdr;
+	u8 rate;
+	/* u32 channel; */
+} __attribute__((packed));
+
+struct wilc_wfi_radiotap_cb_hdr {
+	struct ieee80211_radiotap_header hdr;
+	u8 rate;
+	u8 dump;
+	u16 tx_flags;
+	/* u32 channel; */
+} __attribute__((packed));
+
+extern linux_wlan_t *g_linux_wlan;
+
+static struct net_device *wilc_wfi_mon; /* global monitor netdev */
+
+#ifdef SIMULATION
+extern int WILC_WFI_Tx(struct sk_buff *skb, struct net_device *dev);
+#elif USE_WIRELESS
+extern int  mac_xmit(struct sk_buff *skb, struct net_device *dev);
+#endif
+
+
+u8 srcAdd[6];
+u8 bssid[6];
+u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+/**
+ *  @brief      WILC_WFI_monitor_rx
+ *  @details
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	12 JUL 2012
+ *  @version	1.0
+ */
+
+#define IEEE80211_RADIOTAP_F_TX_RTS	0x0004  /* used rts/cts handshake */
+#define IEEE80211_RADIOTAP_F_TX_FAIL	0x0001  /* failed due to excessive*/
+#define IS_MANAGMEMENT				0x100
+#define IS_MANAGMEMENT_CALLBACK			0x080
+#define IS_MGMT_STATUS_SUCCES			0x040
+#define GET_PKT_OFFSET(a) (((a) >> 22) & 0x1ff)
+
+void WILC_WFI_monitor_rx(uint8_t *buff, uint32_t size)
+{
+	uint32_t header, pkt_offset;
+	struct sk_buff *skb = NULL;
+	struct wilc_wfi_radiotap_hdr *hdr;
+	struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
+
+	PRINT_INFO(HOSTAPD_DBG, "In monitor interface receive function\n");
+
+	/*   struct WILC_WFI_priv *priv = netdev_priv(dev); */
+
+	/*   priv = wiphy_priv(priv->dev->ieee80211_ptr->wiphy); */
+
+	/* Bug 4601 */
+	if (wilc_wfi_mon == NULL)
+		return;
+
+	if (!netif_running(wilc_wfi_mon)) {
+		PRINT_INFO(HOSTAPD_DBG, "Monitor interface already RUNNING\n");
+		return;
+	}
+
+	/* Get WILC header */
+	memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET);
+
+	/* The packet offset field conain info about what type of managment frame */
+	/* we are dealing with and ack status */
+	pkt_offset = GET_PKT_OFFSET(header);
+
+	if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
+
+		/* hostapd callback mgmt frame */
+
+		skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_cb_hdr));
+		if (skb == NULL) {
+			PRINT_INFO(HOSTAPD_DBG, "Monitor if : No memory to allocate skb");
+			return;
+		}
+
+		memcpy(skb_put(skb, size), buff, size);
+
+		cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *) skb_push(skb, sizeof(*cb_hdr));
+		memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
+
+		cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
+
+		cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));
+
+		cb_hdr->hdr.it_present = cpu_to_le32(
+				(1 << IEEE80211_RADIOTAP_RATE) |
+				(1 << IEEE80211_RADIOTAP_TX_FLAGS));
+
+		cb_hdr->rate = 5; /* txrate->bitrate / 5; */
+
+		if (pkt_offset & IS_MGMT_STATUS_SUCCES)	{
+			/* success */
+			cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS;
+		} else {
+			cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL;
+		}
+
+	} else {
+
+		skb = dev_alloc_skb(size + sizeof(struct wilc_wfi_radiotap_hdr));
+
+		if (skb == NULL) {
+			PRINT_INFO(HOSTAPD_DBG, "Monitor if : No memory to allocate skb");
+			return;
+		}
+
+		/* skb = skb_copy_expand(tx_skb, sizeof(*hdr), 0, GFP_ATOMIC); */
+		/* if (skb == NULL) */
+		/*      return; */
+
+		memcpy(skb_put(skb, size), buff, size);
+		hdr = (struct wilc_wfi_radiotap_hdr *) skb_push(skb, sizeof(*hdr));
+		memset(hdr, 0, sizeof(struct wilc_wfi_radiotap_hdr));
+		hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
+		/* hdr->hdr.it_pad = 0; */
+		hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_hdr));
+		PRINT_INFO(HOSTAPD_DBG, "Radiotap len %d\n", hdr->hdr.it_len);
+		hdr->hdr.it_present = cpu_to_le32
+				(1 << IEEE80211_RADIOTAP_RATE);                   /* | */
+		/* (1 << IEEE80211_RADIOTAP_CHANNEL)); */
+		PRINT_INFO(HOSTAPD_DBG, "Presentflags %d\n", hdr->hdr.it_present);
+		hdr->rate = 5; /* txrate->bitrate / 5; */
+
+	}
+
+/*	if(INFO || if(skb->data[9] == 0x00 || skb->data[9] == 0xb0))
+ *      {
+ *              for(i=0;i<skb->len;i++)
+ *                      PRINT_INFO(HOSTAPD_DBG,"Mon RxData[%d] = %02x\n",i,skb->data[i]);
+ *      }*/
+
+
+	skb->dev = wilc_wfi_mon;
+	skb_set_mac_header(skb, 0);
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	skb->pkt_type = PACKET_OTHERHOST;
+	skb->protocol = htons(ETH_P_802_2);
+	memset(skb->cb, 0, sizeof(skb->cb));
+
+	netif_rx(skb);
+
+
+}
+
+struct tx_complete_mon_data {
+	int size;
+	void *buff;
+};
+
+static void mgmt_tx_complete(void *priv, int status)
+{
+
+	/* struct sk_buff *skb2; */
+	/* struct wilc_wfi_radiotap_cb_hdr *cb_hdr; */
+
+	struct tx_complete_mon_data *pv_data = (struct tx_complete_mon_data *)priv;
+	u8 *buf =  pv_data->buff;
+
+
+
+	if (status == 1) {
+		if (INFO || buf[0] == 0x10 || buf[0] == 0xb0)
+			PRINT_INFO(HOSTAPD_DBG, "Packet sent successfully - Size = %d - Address = %p.\n", pv_data->size, pv_data->buff);
+	} else {
+		PRINT_INFO(HOSTAPD_DBG, "Couldn't send packet - Size = %d - Address = %p.\n", pv_data->size, pv_data->buff);
+	}
+
+
+/*			//(skb->data[9] == 0x00 || skb->data[9] == 0xb0 || skb->data[9] == 0x40 ||  skb->data[9] == 0xd0 )
+ *      {
+ *              skb2 = dev_alloc_skb(pv_data->size+sizeof(struct wilc_wfi_radiotap_cb_hdr));
+ *
+ *              memcpy(skb_put(skb2,pv_data->size),pv_data->buff, pv_data->size);
+ *
+ *              cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *) skb_push(skb2, sizeof(*cb_hdr));
+ *              memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
+ *
+ *               cb_hdr->hdr.it_version = 0;//PKTHDR_RADIOTAP_VERSION;
+ *
+ *              cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));
+ *
+ *       cb_hdr->hdr.it_present = cpu_to_le32(
+ *                                        (1 << IEEE80211_RADIOTAP_RATE) |
+ *                                       (1 << IEEE80211_RADIOTAP_TX_FLAGS));
+ *
+ *              cb_hdr->rate = 5;//txrate->bitrate / 5;
+ *              cb_hdr->tx_flags = 0x0004;
+ *
+ *              skb2->dev = wilc_wfi_mon;
+ *              skb_set_mac_header(skb2, 0);
+ *              skb2->ip_summed = CHECKSUM_UNNECESSARY;
+ *              skb2->pkt_type = PACKET_OTHERHOST;
+ *              skb2->protocol = htons(ETH_P_802_2);
+ *              memset(skb2->cb, 0, sizeof(skb2->cb));
+ *
+ *              netif_rx(skb2);
+ *      }*/
+
+	/* incase of fully hosting mode, the freeing will be done in response to the cfg packet */
+	#ifndef WILC_FULLY_HOSTING_AP
+	kfree(pv_data->buff);
+
+	kfree(pv_data);
+	#endif
+}
+static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
+{
+	linux_wlan_t *nic;
+	struct tx_complete_mon_data *mgmt_tx = NULL;
+
+	if (dev == NULL) {
+		PRINT_D(HOSTAPD_DBG, "ERROR: dev == NULL\n");
+		return WILC_FAIL;
+	}
+	nic = netdev_priv(dev);
+
+	netif_stop_queue(dev);
+	mgmt_tx = kmalloc(sizeof(struct tx_complete_mon_data), GFP_ATOMIC);
+	if (mgmt_tx == NULL) {
+		PRINT_ER("Failed to allocate memory for mgmt_tx structure\n");
+		return WILC_FAIL;
+	}
+
+	#ifdef WILC_FULLY_HOSTING_AP
+	/* add space for the pointer to tx_complete_mon_data */
+	len += sizeof(struct tx_complete_mon_data *);
+	#endif
+
+	mgmt_tx->buff = kmalloc(len, GFP_ATOMIC);
+	if (mgmt_tx->buff == NULL) {
+		PRINT_ER("Failed to allocate memory for mgmt_tx buff\n");
+		return WILC_FAIL;
+
+	}
+
+	mgmt_tx->size = len;
+
+	#ifndef WILC_FULLY_HOSTING_AP
+	memcpy(mgmt_tx->buff, buf, len);
+	#else
+	memcpy(mgmt_tx->buff, buf, len - sizeof(struct tx_complete_mon_data *));
+	memcpy((mgmt_tx->buff) + (len - sizeof(struct tx_complete_mon_data *)), &mgmt_tx, sizeof(struct tx_complete_mon_data *));
+
+	/* filter data frames to handle it's PS */
+	if (filter_monitor_data_frames((mgmt_tx->buff), len) == true) {
+		return;
+	}
+
+	#endif /* WILC_FULLY_HOSTING_AP */
+
+	g_linux_wlan->oup.wlan_add_mgmt_to_tx_que(mgmt_tx, mgmt_tx->buff, mgmt_tx->size, mgmt_tx_complete);
+
+	netif_wake_queue(dev);
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_mon_xmit
+ *  @details
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	12 JUL 2012
+ *  @version	1.0
+ */
+static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb,
+				     struct net_device *dev)
+{
+	struct ieee80211_radiotap_header *rtap_hdr;
+	u32 rtap_len, i, ret = 0;
+	struct WILC_WFI_mon_priv  *mon_priv;
+
+	struct sk_buff *skb2;
+	struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
+
+	/* Bug 4601 */
+	if (wilc_wfi_mon == NULL)
+		return WILC_FAIL;
+
+	/* if(skb->data[3] == 0x10 || skb->data[3] == 0xb0) */
+
+	mon_priv = netdev_priv(wilc_wfi_mon);
+
+	if (mon_priv == NULL) {
+		PRINT_ER("Monitor interface private structure is NULL\n");
+		return WILC_FAIL;
+	}
+
+	rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
+
+	rtap_len = ieee80211_get_radiotap_len(skb->data);
+	if (skb->len < rtap_len) {
+		PRINT_ER("Error in radiotap header\n");
+		return -1;
+	}
+	/* skip the radiotap header */
+	PRINT_INFO(HOSTAPD_DBG, "Radiotap len: %d\n", rtap_len);
+
+	if (INFO) {
+		for (i = 0; i < rtap_len; i++)
+			PRINT_INFO(HOSTAPD_DBG, "Radiotap_hdr[%d] %02x\n", i, skb->data[i]);
+	}
+	/* Skip the ratio tap header */
+	skb_pull(skb, rtap_len);
+
+	if (skb->data[0] == 0xc0)
+		PRINT_INFO(HOSTAPD_DBG, "%x:%x:%x:%x:%x%x\n", skb->data[4], skb->data[5], skb->data[6], skb->data[7], skb->data[8], skb->data[9]);
+
+	if (skb->data[0] == 0xc0 && (!(memcmp(broadcast, &skb->data[4], 6)))) {
+		skb2 = dev_alloc_skb(skb->len + sizeof(struct wilc_wfi_radiotap_cb_hdr));
+
+		memcpy(skb_put(skb2, skb->len), skb->data, skb->len);
+
+		cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *) skb_push(skb2, sizeof(*cb_hdr));
+		memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
+
+		cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
+
+		cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));
+
+		cb_hdr->hdr.it_present = cpu_to_le32(
+				(1 << IEEE80211_RADIOTAP_RATE) |
+				(1 << IEEE80211_RADIOTAP_TX_FLAGS));
+
+		cb_hdr->rate = 5; /* txrate->bitrate / 5; */
+		cb_hdr->tx_flags = 0x0004;
+
+		skb2->dev = wilc_wfi_mon;
+		skb_set_mac_header(skb2, 0);
+		skb2->ip_summed = CHECKSUM_UNNECESSARY;
+		skb2->pkt_type = PACKET_OTHERHOST;
+		skb2->protocol = htons(ETH_P_802_2);
+		memset(skb2->cb, 0, sizeof(skb2->cb));
+
+		netif_rx(skb2);
+
+		return 0;
+	}
+	skb->dev = mon_priv->real_ndev;
+
+	PRINT_INFO(HOSTAPD_DBG, "Skipping the radiotap header\n");
+
+
+
+	/* actual deliver of data is device-specific, and not shown here */
+	PRINT_INFO(HOSTAPD_DBG, "SKB netdevice name = %s\n", skb->dev->name);
+	PRINT_INFO(HOSTAPD_DBG, "MONITOR real dev name = %s\n", mon_priv->real_ndev->name);
+
+	#ifdef SIMULATION
+	ret = WILC_WFI_Tx(skb, mon_priv->real_ndev);
+	#elif USE_WIRELESS
+	/* Identify if Ethernet or MAC header (data or mgmt) */
+	memcpy(srcAdd, &skb->data[10], 6);
+	memcpy(bssid, &skb->data[16], 6);
+	/* if source address and bssid fields are equal>>Mac header */
+	/*send it to mgmt frames handler */
+	if (!(memcmp(srcAdd, bssid, 6))) {
+		mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
+		dev_kfree_skb(skb);
+	} else
+		ret = mac_xmit(skb, mon_priv->real_ndev);
+	#endif
+
+	/* return NETDEV_TX_OK; */
+	return ret;
+}
+
+static const struct net_device_ops wilc_wfi_netdev_ops = {
+	.ndo_start_xmit         = WILC_WFI_mon_xmit,
+
+};
+
+#ifdef WILC_FULLY_HOSTING_AP
+/*
+ *  @brief                      WILC_mgm_HOSTAPD_ACK
+ *  @details            report the status of transmitted mgmt frames to HOSTAPD
+ *  @param[in]          priv : pointer to tx_complete_mon_data struct
+ *				bStatus : status of transmission
+ *  @author		Abd Al-Rahman Diab
+ *  @date			9 May 2013
+ *  @version		1.0
+ */
+void WILC_mgm_HOSTAPD_ACK(void *priv, bool bStatus)
+{
+	struct sk_buff *skb;
+	struct wilc_wfi_radiotap_cb_hdr *cb_hdr;
+
+	struct tx_complete_mon_data *pv_data = (struct tx_complete_mon_data *)priv;
+	u8 *buf =  pv_data->buff;
+
+	/* len of the original frame without the added pointer at the tail */
+	u16 u16len = (pv_data->size) - sizeof(struct tx_complete_mon_data *);
+
+
+	/*if(bStatus == 1){
+	 *      if(INFO || buf[0] == 0x10 || buf[0] == 0xb0)
+	 *      PRINT_D(HOSTAPD_DBG,"Packet sent successfully - Size = %d - Address = %p.\n",u16len,pv_data->buff);
+	 * }else{
+	 *              PRINT_D(HOSTAPD_DBG,"Couldn't send packet - Size = %d - Address = %p.\n",u16len,pv_data->buff);
+	 *      }
+	 */
+
+	/* (skb->data[9] == 0x00 || skb->data[9] == 0xb0 || skb->data[9] == 0x40 ||  skb->data[9] == 0xd0 ) */
+	{
+		skb = dev_alloc_skb(u16len + sizeof(struct wilc_wfi_radiotap_cb_hdr));
+
+		memcpy(skb_put(skb, u16len), pv_data->buff, u16len);
+
+		cb_hdr = (struct wilc_wfi_radiotap_cb_hdr *) skb_push(skb, sizeof(*cb_hdr));
+		memset(cb_hdr, 0, sizeof(struct wilc_wfi_radiotap_cb_hdr));
+
+		cb_hdr->hdr.it_version = 0; /* PKTHDR_RADIOTAP_VERSION; */
+
+		cb_hdr->hdr.it_len = cpu_to_le16(sizeof(struct wilc_wfi_radiotap_cb_hdr));
+
+		cb_hdr->hdr.it_present = cpu_to_le32(
+				(1 << IEEE80211_RADIOTAP_RATE) |
+				(1 << IEEE80211_RADIOTAP_TX_FLAGS));
+
+		cb_hdr->rate = 5; /* txrate->bitrate / 5; */
+
+
+		if (bStatus) {
+			/* success */
+			cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_RTS;
+		} else {
+			cb_hdr->tx_flags = IEEE80211_RADIOTAP_F_TX_FAIL;
+		}
+
+		skb->dev = wilc_wfi_mon;
+		skb_set_mac_header(skb, 0);
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		skb->pkt_type = PACKET_OTHERHOST;
+		skb->protocol = htons(ETH_P_802_2);
+		memset(skb->cb, 0, sizeof(skb->cb));
+
+		netif_rx(skb);
+	}
+
+	/* incase of fully hosting mode, the freeing will be done in response to the cfg packet */
+	kfree(pv_data->buff);
+
+	kfree(pv_data);
+
+}
+#endif /* WILC_FULLY_HOSTING_AP */
+
+/**
+ *  @brief      WILC_WFI_mon_setup
+ *  @details
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	12 JUL 2012
+ *  @version	1.0
+ */
+static void WILC_WFI_mon_setup(struct net_device *dev)
+{
+
+	dev->netdev_ops = &wilc_wfi_netdev_ops;
+	/* dev->destructor = free_netdev; */
+	PRINT_INFO(CORECONFIG_DBG, "In Ethernet setup function\n");
+	ether_setup(dev);
+	dev->tx_queue_len = 0;
+	dev->type = ARPHRD_IEEE80211_RADIOTAP;
+	memset(dev->dev_addr, 0, ETH_ALEN);
+
+	#ifdef USE_WIRELESS
+	{
+		/* u8 * mac_add; */
+		unsigned char mac_add[] = {0x00, 0x50, 0xc2, 0x5e, 0x10, 0x8f};
+		/* priv = wiphy_priv(priv->dev->ieee80211_ptr->wiphy); */
+		/* mac_add = (u8*)WILC_MALLOC(ETH_ALEN); */
+		/* status = host_int_get_MacAddress(priv->hWILCWFIDrv,mac_add); */
+		/* mac_add[ETH_ALEN-1]+=1; */
+		memcpy(dev->dev_addr, mac_add, ETH_ALEN);
+	}
+	#else
+	dev->dev_addr[0] = 0x12;
+	#endif
+
+}
+
+/**
+ *  @brief      WILC_WFI_init_mon_interface
+ *  @details
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	12 JUL 2012
+ *  @version	1.0
+ */
+struct net_device *WILC_WFI_init_mon_interface(const char *name, struct net_device *real_dev)
+{
+
+
+	u32 ret = WILC_SUCCESS;
+	struct WILC_WFI_mon_priv *priv;
+
+	/*If monitor interface is already initialized, return it*/
+	if (wilc_wfi_mon) {
+		return wilc_wfi_mon;
+	}
+
+	wilc_wfi_mon = alloc_etherdev(sizeof(struct WILC_WFI_mon_priv));
+	if (!wilc_wfi_mon) {
+		PRINT_ER("failed to allocate memory\n");
+		return NULL;
+
+	}
+
+	wilc_wfi_mon->type = ARPHRD_IEEE80211_RADIOTAP;
+	strncpy(wilc_wfi_mon->name, name, IFNAMSIZ);
+	wilc_wfi_mon->name[IFNAMSIZ - 1] = 0;
+	wilc_wfi_mon->netdev_ops = &wilc_wfi_netdev_ops;
+
+	ret = register_netdevice(wilc_wfi_mon);
+	if (ret) {
+		PRINT_ER(" register_netdevice failed (%d)\n", ret);
+		return NULL;
+	}
+	priv = netdev_priv(wilc_wfi_mon);
+	if (priv == NULL) {
+		PRINT_ER("private structure is NULL\n");
+		return NULL;
+	}
+
+	priv->real_ndev = real_dev;
+
+	return wilc_wfi_mon;
+}
+
+/**
+ *  @brief      WILC_WFI_deinit_mon_interface
+ *  @details
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	12 JUL 2012
+ *  @version	1.0
+ */
+int WILC_WFI_deinit_mon_interface()
+{
+	bool rollback_lock = false;
+
+	if (wilc_wfi_mon != NULL) {
+		PRINT_D(HOSTAPD_DBG, "In Deinit monitor interface\n");
+		PRINT_D(HOSTAPD_DBG, "RTNL is being locked\n");
+		if (rtnl_is_locked()) {
+			rtnl_unlock();
+			rollback_lock = true;
+		}
+		PRINT_D(HOSTAPD_DBG, "Unregister netdev\n");
+		unregister_netdev(wilc_wfi_mon);
+		/* free_netdev(wilc_wfi_mon); */
+
+		if (rollback_lock) {
+			rtnl_lock();
+			rollback_lock = false;
+		}
+		wilc_wfi_mon = NULL;
+	}
+	return WILC_SUCCESS;
+
+}
+#endif /* WILC_AP_EXTERNAL_MLME */
diff --git a/drivers/staging/wilc1000/linux_wlan.c b/drivers/staging/wilc1000/linux_wlan.c
new file mode 100644
index 000000000000..b352c504a77d
--- /dev/null
+++ b/drivers/staging/wilc1000/linux_wlan.c
@@ -0,0 +1,2783 @@
+#ifndef SIMULATION
+#include "wilc_wfi_cfgoperations.h"
+#include "linux_wlan_common.h"
+#include "wilc_wlan_if.h"
+#include "wilc_wlan.h"
+#ifdef USE_WIRELESS
+#include "wilc_wfi_cfgoperations.h"
+#endif
+
+#include "linux_wlan_common.h"
+
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+
+#include <linux/kthread.h>
+#include <linux/firmware.h>
+#include <linux/delay.h>
+
+#include <linux/init.h>
+#include <linux/netdevice.h>
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+#include <linux/inetdevice.h>
+#endif
+#include <linux/etherdevice.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/skbuff.h>
+
+#include <linux/version.h>
+#include <linux/semaphore.h>
+
+#ifdef WILC_SDIO
+#include "linux_wlan_sdio.h"
+#else
+#include "linux_wlan_spi.h"
+#endif
+
+#ifdef WILC_FULLY_HOSTING_AP
+#include "wilc_host_ap.h"
+#endif
+
+#ifdef STATIC_MACADDRESS /* brandy_0724 [[ */
+#include <linux/vmalloc.h>
+#include <linux/fs.h>
+struct task_struct *wilc_mac_thread;
+unsigned char mac_add[] = {0x00, 0x80, 0xC2, 0x5E, 0xa2, 0xb2};
+#endif /* brandy_0724 ]] */
+
+#if defined(CUSTOMER_PLATFORM)
+/*
+ TODO : Write power control functions as customer platform.
+ */
+#else
+
+ #define _linux_wlan_device_power_on()		{}
+ #define _linux_wlan_device_power_off()		{}
+
+ #define _linux_wlan_device_detection()		{}
+ #define _linux_wlan_device_removal()		{}
+#endif
+
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+extern bool g_obtainingIP;
+#endif
+extern u16 Set_machw_change_vir_if(bool bValue);
+extern void resolve_disconnect_aberration(void *drvHandler);
+extern u8 gau8MulticastMacAddrList[WILC_MULTICAST_TABLE_SIZE][ETH_ALEN];
+void wilc1000_wlan_deinit(linux_wlan_t *nic);
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+extern WILC_TimerHandle hDuringIpTimer;
+#endif
+
+static int linux_wlan_device_power(int on_off)
+{
+	PRINT_D(INIT_DBG, "linux_wlan_device_power.. (%d)\n", on_off);
+
+	if (on_off) {
+		_linux_wlan_device_power_on();
+	} else {
+		_linux_wlan_device_power_off();
+	}
+
+	return 0;
+}
+
+static int linux_wlan_device_detection(int on_off)
+{
+	PRINT_D(INIT_DBG, "linux_wlan_device_detection.. (%d)\n", on_off);
+
+#ifdef WILC_SDIO
+	if (on_off) {
+		_linux_wlan_device_detection();
+	} else {
+		_linux_wlan_device_removal();
+	}
+#endif
+
+	return 0;
+}
+
+
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+static int dev_state_ev_handler(struct notifier_block *this, unsigned long event, void *ptr);
+
+static struct notifier_block g_dev_notifier = {
+	.notifier_call = dev_state_ev_handler
+};
+#endif
+
+#define wilc_wlan_deinit(nic)	{ if (&g_linux_wlan->oup != NULL)	 \
+		if (g_linux_wlan->oup.wlan_cleanup != NULL) \
+			g_linux_wlan->oup.wlan_cleanup(); }
+
+
+#ifndef STA_FIRMWARE
+#define STA_FIRMWARE	"wifi_firmware.bin"
+#endif
+
+#ifndef AP_FIRMWARE
+#define AP_FIRMWARE		"wifi_firmware_ap.bin"
+#endif
+
+#ifndef P2P_CONCURRENCY_FIRMWARE
+#define P2P_CONCURRENCY_FIRMWARE	"wifi_firmware_p2p_concurrency.bin"
+#endif
+
+
+
+typedef struct android_wifi_priv_cmd {
+	char *buf;
+	int used_len;
+	int total_len;
+} android_wifi_priv_cmd;
+
+
+#define IRQ_WAIT	1
+#define IRQ_NO_WAIT	0
+/*
+ *      to sync between mac_close and module exit.
+ *      don't initialize or de-initialize from init/deinitlocks
+ *      to be initialized from module wilc_netdev_init and
+ *      deinitialized from mdoule_exit
+ */
+static struct semaphore close_exit_sync;
+unsigned int int_rcvdU;
+unsigned int int_rcvdB;
+unsigned int int_clrd;
+
+static int wlan_deinit_locks(linux_wlan_t *nic);
+static void wlan_deinitialize_threads(linux_wlan_t *nic);
+static void linux_wlan_lock(void *vp);
+void linux_wlan_unlock(void *vp);
+extern void WILC_WFI_monitor_rx(uint8_t *buff, uint32_t size);
+extern void WILC_WFI_p2p_rx(struct net_device *dev, uint8_t *buff, uint32_t size);
+
+
+static void *internal_alloc(uint32_t size, uint32_t flag);
+static void linux_wlan_tx_complete(void *priv, int status);
+void frmw_to_linux(uint8_t *buff, uint32_t size, uint32_t pkt_offset);
+static int  mac_init_fn(struct net_device *ndev);
+int  mac_xmit(struct sk_buff *skb, struct net_device *dev);
+int  mac_open(struct net_device *ndev);
+int  mac_close(struct net_device *ndev);
+static struct net_device_stats *mac_stats(struct net_device *dev);
+static int  mac_ioctl(struct net_device *ndev, struct ifreq *req, int cmd);
+static void wilc_set_multicast_list(struct net_device *dev);
+
+
+
+/*
+ * for now - in frmw_to_linux there should be private data to be passed to it
+ * and this data should be pointer to net device
+ */
+linux_wlan_t *g_linux_wlan;
+wilc_wlan_oup_t *gpstrWlanOps;
+bool bEnablePS = true;
+
+static const struct net_device_ops wilc_netdev_ops = {
+	.ndo_init = mac_init_fn,
+	.ndo_open = mac_open,
+	.ndo_stop = mac_close,
+	.ndo_start_xmit = mac_xmit,
+	.ndo_do_ioctl = mac_ioctl,
+	.ndo_get_stats = mac_stats,
+	.ndo_set_rx_mode  = wilc_set_multicast_list,
+
+};
+
+#ifdef DEBUG_MODE
+
+extern volatile int timeNo;
+
+#define DEGUG_BUFFER_LENGTH 1000
+volatile int WatchDogdebuggerCounter;
+char DebugBuffer[DEGUG_BUFFER_LENGTH + 20] = {0};
+static char *ps8current = DebugBuffer;
+
+
+
+void printk_later(const char *format, ...)
+{
+	va_list args;
+	va_start (args, format);
+	ps8current += vsprintf (ps8current, format, args);
+	va_end (args);
+	if ((ps8current - DebugBuffer) > DEGUG_BUFFER_LENGTH) {
+		ps8current = DebugBuffer;
+	}
+
+}
+
+
+void dump_logs()
+{
+	if (DebugBuffer[0]) {
+		DebugBuffer[DEGUG_BUFFER_LENGTH] = 0;
+		PRINT_INFO(GENERIC_DBG, "early printed\n");
+		PRINT_D(GENERIC_DBG, ps8current + 1);
+		ps8current[1] = 0;
+		PRINT_INFO(GENERIC_DBG, "latest printed\n");
+		PRINT_D(GENERIC_DBG, DebugBuffer);
+		DebugBuffer[0] = 0;
+		ps8current = DebugBuffer;
+	}
+}
+
+void Reset_WatchDogdebugger()
+{
+	WatchDogdebuggerCounter = 0;
+}
+
+static int DebuggingThreadTask(void *vp)
+{
+	while (1) {
+		while (!WatchDogdebuggerCounter) {
+			PRINT_D(GENERIC_DBG, "Debug Thread Running %d\n", timeNo);
+			WatchDogdebuggerCounter = 1;
+			msleep(10000);
+		}
+		dump_logs();
+		WatchDogdebuggerCounter = 0;
+	}
+}
+
+
+#endif
+
+
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+static int dev_state_ev_handler(struct notifier_block *this, unsigned long event, void *ptr)
+{
+	struct in_ifaddr *dev_iface = (struct in_ifaddr *)ptr;
+	struct WILC_WFI_priv *priv;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+	struct net_device *dev;
+	u8 *pIP_Add_buff;
+	perInterface_wlan_t *nic;
+	u8 null_ip[4] = {0};
+	char wlan_dev_name[5] = "wlan0";
+
+	if (dev_iface == NULL || dev_iface->ifa_dev == NULL || dev_iface->ifa_dev->dev == NULL)	{
+		PRINT_D(GENERIC_DBG, "dev_iface = NULL\n");
+		return NOTIFY_DONE;
+	}
+
+	if ((memcmp(dev_iface->ifa_label, "wlan0", 5)) && (memcmp(dev_iface->ifa_label, "p2p0", 4))) {
+		PRINT_D(GENERIC_DBG, "Interface is neither WLAN0 nor P2P0\n");
+		return NOTIFY_DONE;
+	}
+
+	dev  = (struct net_device *)dev_iface->ifa_dev->dev;
+	if (dev->ieee80211_ptr == NULL || dev->ieee80211_ptr->wiphy == NULL) {
+		PRINT_D(GENERIC_DBG, "No Wireless registerd\n");
+		return NOTIFY_DONE;
+	}
+	priv = wiphy_priv(dev->ieee80211_ptr->wiphy);
+	if (priv == NULL) {
+		PRINT_D(GENERIC_DBG, "No Wireless Priv\n");
+		return NOTIFY_DONE;
+	}
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+	nic = netdev_priv(dev);
+	if (nic == NULL || pstrWFIDrv == NULL) {
+		PRINT_D(GENERIC_DBG, "No Wireless Priv\n");
+		return NOTIFY_DONE;
+	}
+
+	PRINT_INFO(GENERIC_DBG, "dev_state_ev_handler +++\n"); /* tony */
+
+	switch (event) {
+	case NETDEV_UP:
+		PRINT_D(GENERIC_DBG, "dev_state_ev_handler event=NETDEV_UP %p\n", dev);       /* tony */
+
+		PRINT_INFO(GENERIC_DBG, "\n ============== IP Address Obtained ===============\n\n");
+
+
+		/*If we are in station mode or client mode*/
+		if (nic->iftype == STATION_MODE || nic->iftype == CLIENT_MODE) {
+			pstrWFIDrv->IFC_UP = 1;
+			g_obtainingIP = false;
+			WILC_TimerStop(&hDuringIpTimer, NULL);
+			PRINT_D(GENERIC_DBG, "IP obtained , enable scan\n");
+		}
+
+
+
+		if (bEnablePS)
+			host_int_set_power_mgmt((WILC_WFIDrvHandle)pstrWFIDrv, 1, 0);
+
+		PRINT_D(GENERIC_DBG, "[%s] Up IP\n", dev_iface->ifa_label);
+
+		pIP_Add_buff = (char *) (&(dev_iface->ifa_address));
+		PRINT_D(GENERIC_DBG, "IP add=%d:%d:%d:%d \n", pIP_Add_buff[0], pIP_Add_buff[1], pIP_Add_buff[2], pIP_Add_buff[3]);
+		host_int_setup_ipaddress((WILC_WFIDrvHandle)pstrWFIDrv, pIP_Add_buff, nic->u8IfIdx);
+
+		break;
+
+	case NETDEV_DOWN:
+		PRINT_D(GENERIC_DBG, "dev_state_ev_handler event=NETDEV_DOWN %p\n", dev);               /* tony */
+
+		PRINT_INFO(GENERIC_DBG, "\n ============== IP Address Released ===============\n\n");
+		if (nic->iftype == STATION_MODE || nic->iftype == CLIENT_MODE) {
+			pstrWFIDrv->IFC_UP = 0;
+			g_obtainingIP = false;
+		}
+
+		if (memcmp(dev_iface->ifa_label, wlan_dev_name, 5) == 0)
+			host_int_set_power_mgmt((WILC_WFIDrvHandle)pstrWFIDrv, 0, 0);
+
+		resolve_disconnect_aberration(pstrWFIDrv);
+
+
+		PRINT_D(GENERIC_DBG, "[%s] Down IP\n", dev_iface->ifa_label);
+
+		pIP_Add_buff = null_ip;
+		PRINT_D(GENERIC_DBG, "IP add=%d:%d:%d:%d \n", pIP_Add_buff[0], pIP_Add_buff[1], pIP_Add_buff[2], pIP_Add_buff[3]);
+
+		host_int_setup_ipaddress((WILC_WFIDrvHandle)pstrWFIDrv, pIP_Add_buff, nic->u8IfIdx);
+
+		break;
+
+	default:
+		PRINT_INFO(GENERIC_DBG, "dev_state_ev_handler event=default\n");        /* tony */
+		PRINT_INFO(GENERIC_DBG, "[%s] unknown dev event: %lu\n", dev_iface->ifa_label, event);
+
+		break;
+	}
+
+	return NOTIFY_DONE;
+
+}
+#endif
+
+/*
+ *	Interrupt initialization and handling functions
+ */
+
+void linux_wlan_enable_irq(void)
+{
+
+#if (RX_BH_TYPE != RX_BH_THREADED_IRQ)
+#if (defined WILC_SPI) || (defined WILC_SDIO_IRQ_GPIO)
+	PRINT_D(INT_DBG, "Enabling IRQ ...\n");
+	enable_irq(g_linux_wlan->dev_irq_num);
+#endif
+#endif
+}
+
+void linux_wlan_disable_irq(int wait)
+{
+#if (defined WILC_SPI) || (defined WILC_SDIO_IRQ_GPIO)
+	if (wait) {
+		PRINT_D(INT_DBG, "Disabling IRQ ...\n");
+		disable_irq(g_linux_wlan->dev_irq_num);
+	} else {
+		PRINT_D(INT_DBG, "Disabling IRQ ...\n");
+		disable_irq_nosync(g_linux_wlan->dev_irq_num);
+	}
+#endif
+}
+
+#if (defined WILC_SPI) || (defined WILC_SDIO_IRQ_GPIO)
+static irqreturn_t isr_uh_routine(int irq, void *user_data)
+{
+
+
+	int_rcvdU++;
+#if (RX_BH_TYPE != RX_BH_THREADED_IRQ)
+	linux_wlan_disable_irq(IRQ_NO_WAIT);
+#endif
+	PRINT_D(INT_DBG, "Interrupt received UH\n");
+
+	/*While mac is closing cacncel the handling of any interrupts received*/
+	if (g_linux_wlan->close) {
+		PRINT_ER("Driver is CLOSING: Can't handle UH interrupt\n");
+	#if (RX_BH_TYPE == RX_BH_THREADED_IRQ)
+		return IRQ_HANDLED;
+	#else
+		return IRQ_NONE;
+	#endif
+
+	}
+#if (RX_BH_TYPE == RX_BH_WORK_QUEUE)
+	schedule_work(&g_linux_wlan->rx_work_queue);
+	return IRQ_HANDLED;
+#elif (RX_BH_TYPE == RX_BH_KTHREAD)
+	linux_wlan_unlock(&g_linux_wlan->rx_sem);
+	return IRQ_HANDLED;
+#elif (RX_BH_TYPE == RX_BH_THREADED_IRQ)
+	return IRQ_WAKE_THREAD;
+#endif
+
+}
+#endif
+
+#if (RX_BH_TYPE == RX_BH_WORK_QUEUE || RX_BH_TYPE == RX_BH_THREADED_IRQ)
+
+#if (RX_BH_TYPE == RX_BH_THREADED_IRQ)
+irqreturn_t isr_bh_routine(int irq, void *userdata)
+{
+	linux_wlan_t *nic;
+	nic = (linux_wlan_t *)userdata;
+#else
+static void isr_bh_routine(struct work_struct *work)
+{
+	perInterface_wlan_t *nic;
+	nic = (perInterface_wlan_t *)container_of(work, linux_wlan_t, rx_work_queue);
+#endif
+
+	/*While mac is closing cacncel the handling of any interrupts received*/
+	if (g_linux_wlan->close) {
+		PRINT_ER("Driver is CLOSING: Can't handle BH interrupt\n");
+	#if (RX_BH_TYPE == RX_BH_THREADED_IRQ)
+		return IRQ_HANDLED;
+	#else
+		return;
+	#endif
+
+
+
+	}
+
+	int_rcvdB++;
+	PRINT_D(INT_DBG, "Interrupt received BH\n");
+	if (g_linux_wlan->oup.wlan_handle_rx_isr != 0) {
+		g_linux_wlan->oup.wlan_handle_rx_isr();
+	} else {
+		PRINT_ER("wlan_handle_rx_isr() hasn't been initialized\n");
+	}
+
+
+#if (RX_BH_TYPE == RX_BH_THREADED_IRQ)
+	return IRQ_HANDLED;
+#endif
+}
+#elif (RX_BH_TYPE == RX_BH_KTHREAD)
+static int isr_bh_routine(void *vp)
+{
+	linux_wlan_t *nic;
+
+	nic = (linux_wlan_t *)vp;
+
+	while (1) {
+		linux_wlan_lock(&nic->rx_sem);
+		if (g_linux_wlan->close) {
+
+			while (!kthread_should_stop())
+				schedule();
+
+			break;
+		}
+		int_rcvdB++;
+		PRINT_D(INT_DBG, "Interrupt received BH\n");
+		if (g_linux_wlan->oup.wlan_handle_rx_isr != 0) {
+			g_linux_wlan->oup.wlan_handle_rx_isr();
+		} else {
+			PRINT_ER("wlan_handle_rx_isr() hasn't been initialized\n");
+		}
+	}
+
+	return 0;
+}
+#endif
+
+
+#if (defined WILC_SPI) || (defined WILC_SDIO_IRQ_GPIO)
+static int init_irq(linux_wlan_t *p_nic)
+{
+	int ret = 0;
+	linux_wlan_t *nic = p_nic;
+
+	/*initialize GPIO and register IRQ num*/
+	/*GPIO request*/
+	if ((gpio_request(GPIO_NUM, "WILC_INTR") == 0) &&
+	    (gpio_direction_input(GPIO_NUM) == 0)) {
+#if defined(CUSTOMER_PLATFORM)
+/*
+ TODO : save the registerd irq number to the private wilc context in kernel.
+ *
+ * ex) nic->dev_irq_num = gpio_to_irq(GPIO_NUM);
+ */
+#elif defined (NM73131_0_BOARD)
+		nic->dev_irq_num = IRQ_WILC1000;
+#elif defined (PANDA_BOARD)
+		gpio_export(GPIO_NUM, 1);
+		nic->dev_irq_num = OMAP_GPIO_IRQ(GPIO_NUM);
+		irq_set_irq_type(nic->dev_irq_num, IRQ_TYPE_LEVEL_LOW);
+#else
+		nic->dev_irq_num = gpio_to_irq(GPIO_NUM);
+#endif
+	} else {
+		ret = -1;
+		PRINT_ER("could not obtain gpio for WILC_INTR\n");
+	}
+
+
+#if (RX_BH_TYPE == RX_BH_THREADED_IRQ)
+	if ((ret != -1) && (request_threaded_irq(nic->dev_irq_num, isr_uh_routine, isr_bh_routine,
+						  IRQF_TRIGGER_LOW | IRQF_ONESHOT,               /*Without IRQF_ONESHOT the uh will remain kicked in and dont gave a chance to bh*/
+						  "WILC_IRQ", nic)) < 0) {
+
+#else
+	/*Request IRQ*/
+	if ((ret != -1) && (request_irq(nic->dev_irq_num, isr_uh_routine,
+					IRQF_TRIGGER_LOW, "WILC_IRQ", nic) < 0)) {
+
+#endif
+		PRINT_ER("Failed to request IRQ for GPIO: %d\n", GPIO_NUM);
+		ret = -1;
+	} else {
+
+		PRINT_D(INIT_DBG, "IRQ request succeeded IRQ-NUM= %d on GPIO: %d\n",
+			nic->dev_irq_num, GPIO_NUM);
+	}
+
+	return ret;
+}
+#endif
+
+static void deinit_irq(linux_wlan_t *nic)
+{
+#if (defined WILC_SPI) || (defined WILC_SDIO_IRQ_GPIO)
+	/* Deintialize IRQ */
+	if (&nic->dev_irq_num != 0) {
+		free_irq(nic->dev_irq_num, g_linux_wlan);
+
+		gpio_free(GPIO_NUM);
+	}
+#endif
+}
+
+
+/*
+ *      OS functions
+ */
+static void linux_wlan_msleep(uint32_t msc)
+{
+	if (msc <= 4000000) {
+		u32 u32Temp = msc * 1000;
+		usleep_range(u32Temp, u32Temp);
+	} else {
+		msleep(msc);
+	}
+}
+
+static void linux_wlan_atomic_msleep(uint32_t msc)
+{
+	mdelay(msc);
+}
+static void linux_wlan_dbg(uint8_t *buff)
+{
+	PRINT_D(INIT_DBG, "%d\n", *buff);
+}
+
+static void *linux_wlan_malloc_atomic(uint32_t sz)
+{
+	char *pntr = NULL;
+	pntr = kmalloc(sz, GFP_ATOMIC);
+	PRINT_D(MEM_DBG, "Allocating %d bytes at address %p\n", sz, pntr);
+	return (void *)pntr;
+
+}
+static void *linux_wlan_malloc(uint32_t sz)
+{
+	char *pntr = NULL;
+	pntr = kmalloc(sz, GFP_KERNEL);
+	PRINT_D(MEM_DBG, "Allocating %d bytes at address %p\n", sz, pntr);
+	return (void *)pntr;
+}
+
+void linux_wlan_free(void *vp)
+{
+	if (vp != NULL) {
+		PRINT_D(MEM_DBG, "Freeing %p\n", vp);
+		kfree(vp);
+	}
+}
+
+
+static void *internal_alloc(uint32_t size, uint32_t flag)
+{
+	char *pntr = NULL;
+	pntr = kmalloc(size, flag);
+	PRINT_D(MEM_DBG, "Allocating %d bytes at address %p\n", size, pntr);
+	return (void *)pntr;
+}
+
+
+static void linux_wlan_init_lock(char *lockName, void *plock, int count)
+{
+	sema_init((struct semaphore *)plock, count);
+	PRINT_D(LOCK_DBG, "Initializing [%s][%p]\n", lockName, plock);
+
+}
+
+static void linux_wlan_deinit_lock(void *plock)
+{
+	/* mutex_destroy((struct mutex*)plock); */
+}
+
+static void linux_wlan_lock(void *vp)
+{
+	PRINT_D(LOCK_DBG, "Locking %p\n", vp);
+	if (vp != NULL) {
+		while (down_interruptible((struct semaphore *) vp))
+			;
+	} else {
+		PRINT_ER("Failed, mutex is NULL\n");
+	}
+}
+
+static int linux_wlan_lock_timeout(void *vp, u32 timeout)
+{
+	int error = -1;
+	PRINT_D(LOCK_DBG, "Locking %p\n", vp);
+	if (vp != NULL)	{
+		error = down_timeout((struct semaphore *)vp, msecs_to_jiffies(timeout));
+	} else {
+		PRINT_ER("Failed, mutex is NULL\n");
+	}
+	return error;
+}
+
+void linux_wlan_unlock(void *vp)
+{
+	PRINT_D(LOCK_DBG, "Unlocking %p\n", vp);
+	if (vp != NULL)	{
+		up((struct semaphore *)vp);
+	} else {
+		PRINT_ER("Failed, mutex is NULL\n");
+	}
+}
+
+
+static void linux_wlan_init_mutex(char *lockName, void *plock, int count)
+{
+	mutex_init((struct mutex *)plock);
+	PRINT_D(LOCK_DBG, "Initializing mutex [%s][%p]\n", lockName, plock);
+
+}
+
+static void linux_wlan_deinit_mutex(void *plock)
+{
+	mutex_destroy((struct mutex *)plock);
+}
+
+static void linux_wlan_lock_mutex(void *vp)
+{
+	PRINT_D(LOCK_DBG, "Locking mutex %p\n", vp);
+	if (vp != NULL)	{
+		/*
+		 *      if(mutex_is_locked((struct mutex*)vp))
+		 *      {
+		 *              //PRINT_ER("Mutex already locked - %p \n",vp);
+		 *      }
+		 */
+		mutex_lock((struct mutex *)vp);
+
+	} else {
+		PRINT_ER("Failed, mutex is NULL\n");
+	}
+}
+
+static void linux_wlan_unlock_mutex(void *vp)
+{
+	PRINT_D(LOCK_DBG, "Unlocking mutex %p\n", vp);
+	if (vp != NULL) {
+
+		if (mutex_is_locked((struct mutex *)vp)) {
+			mutex_unlock((struct mutex *)vp);
+		} else {
+			/* PRINT_ER("Mutex already unlocked  - %p\n",vp); */
+		}
+
+	} else {
+		PRINT_ER("Failed, mutex is NULL\n");
+	}
+}
+
+
+/*Added by Amr - BugID_4720*/
+static void linux_wlan_init_spin_lock(char *lockName, void *plock, int count)
+{
+	spin_lock_init((spinlock_t *)plock);
+	PRINT_D(SPIN_DEBUG, "Initializing mutex [%s][%p]\n", lockName, plock);
+
+}
+
+static void linux_wlan_deinit_spin_lock(void *plock)
+{
+
+}
+static void linux_wlan_spin_lock(void *vp, unsigned long *flags)
+{
+	unsigned long lflags;
+	PRINT_D(SPIN_DEBUG, "Lock spin %p\n", vp);
+	if (vp != NULL) {
+		spin_lock_irqsave((spinlock_t *)vp, lflags);
+		*flags = lflags;
+	} else {
+		PRINT_ER("Failed, spin lock is NULL\n");
+	}
+}
+static void linux_wlan_spin_unlock(void *vp, unsigned long *flags)
+{
+	unsigned long lflags = *flags;
+	PRINT_D(SPIN_DEBUG, "Unlock spin %p\n", vp);
+	if (vp != NULL) {
+		spin_unlock_irqrestore((spinlock_t *)vp, lflags);
+		*flags = lflags;
+	} else {
+		PRINT_ER("Failed, spin lock is NULL\n");
+	}
+}
+
+static void linux_wlan_mac_indicate(int flag)
+{
+	/*I have to do it that way becuase there is no mean to encapsulate device pointer
+	 * as a parameter
+	 */
+	linux_wlan_t *pd = g_linux_wlan;
+	int status;
+
+	if (flag == WILC_MAC_INDICATE_STATUS) {
+		pd->oup.wlan_cfg_get_value(WID_STATUS, (unsigned char *)&status, 4);
+		if (pd->mac_status == WILC_MAC_STATUS_INIT) {
+			pd->mac_status = status;
+			linux_wlan_unlock(&pd->sync_event);
+		} else {
+			pd->mac_status = status;
+		}
+
+		if (pd->mac_status == WILC_MAC_STATUS_CONNECT) {        /* Connect */
+		}
+
+	} else if (flag == WILC_MAC_INDICATE_SCAN) {
+		PRINT_D(GENERIC_DBG, "Scanning ...\n");
+
+	}
+
+}
+
+struct net_device *GetIfHandler(uint8_t *pMacHeader)
+{
+	uint8_t *Bssid, *Bssid1;
+	int i = 0;
+
+	Bssid  = pMacHeader + 10;
+	Bssid1 = pMacHeader + 4;
+
+	for (i = 0; i < g_linux_wlan->u8NoIfcs; i++) {
+		if (!memcmp(Bssid1, g_linux_wlan->strInterfaceInfo[i].aBSSID, ETH_ALEN) ||
+		    !memcmp(Bssid, g_linux_wlan->strInterfaceInfo[i].aBSSID, ETH_ALEN))	{
+			return g_linux_wlan->strInterfaceInfo[i].wilc_netdev;
+		}
+	}
+	PRINT_INFO(INIT_DBG, "Invalide handle\n");
+	for (i = 0; i < 25; i++) {
+		PRINT_D(INIT_DBG, "%02x ", pMacHeader[i]);
+	}
+	Bssid  = pMacHeader + 18;
+	Bssid1 = pMacHeader + 12;
+	for (i = 0; i < g_linux_wlan->u8NoIfcs; i++) {
+		if (!memcmp(Bssid1, g_linux_wlan->strInterfaceInfo[i].aBSSID, ETH_ALEN) ||
+		    !memcmp(Bssid, g_linux_wlan->strInterfaceInfo[i].aBSSID, ETH_ALEN))	{
+			PRINT_D(INIT_DBG, "Ctx [%p]\n", g_linux_wlan->strInterfaceInfo[i].wilc_netdev);
+			return g_linux_wlan->strInterfaceInfo[i].wilc_netdev;
+		}
+	}
+	PRINT_INFO(INIT_DBG, "\n");
+	return NULL;
+}
+
+int linux_wlan_set_bssid(struct net_device *wilc_netdev, uint8_t *pBSSID)
+{
+	int i = 0;
+	int ret = -1;
+
+	PRINT_D(INIT_DBG, "set bssid on[%p]\n", wilc_netdev);
+	for (i = 0; i < g_linux_wlan->u8NoIfcs; i++) {
+		if (g_linux_wlan->strInterfaceInfo[i].wilc_netdev == wilc_netdev) {
+			PRINT_D(INIT_DBG, "set bssid [%x][%x][%x]\n", pBSSID[0], pBSSID[1], pBSSID[2]);
+			memcpy(g_linux_wlan->strInterfaceInfo[i].aBSSID, pBSSID, 6);
+			ret = 0;
+			break;
+		}
+	}
+	return ret;
+}
+
+/*BugID_5213*/
+/*Function to get number of connected interfaces*/
+int linux_wlan_get_num_conn_ifcs(void)
+{
+	uint8_t i = 0;
+	uint8_t null_bssid[6] = {0};
+	uint8_t ret_val = 0;
+
+	for (i = 0; i < g_linux_wlan->u8NoIfcs; i++) {
+		if (memcmp(g_linux_wlan->strInterfaceInfo[i].aBSSID, null_bssid, 6)) {
+			ret_val++;
+		}
+	}
+	return ret_val;
+}
+
+static int linux_wlan_rxq_task(void *vp)
+{
+
+	/* inform wilc1000_wlan_init that RXQ task is started. */
+	linux_wlan_unlock(&g_linux_wlan->rxq_thread_started);
+	while (1) {
+		linux_wlan_lock(&g_linux_wlan->rxq_event);
+		/* wait_for_completion(&g_linux_wlan->rxq_event); */
+
+		if (g_linux_wlan->close) {
+			/*Unlock the mutex in the mac_close function to indicate the exiting of the RX thread */
+			linux_wlan_unlock(&g_linux_wlan->rxq_thread_started);
+
+			while (!kthread_should_stop())
+				schedule();
+
+			PRINT_D(RX_DBG, " RX thread stopped\n");
+			break;
+		}
+		PRINT_D(RX_DBG, "Calling wlan_handle_rx_que()\n");
+
+		g_linux_wlan->oup.wlan_handle_rx_que();
+	}
+	return 0;
+}
+
+#define USE_TX_BACKOFF_DELAY_IF_NO_BUFFERS
+
+static int linux_wlan_txq_task(void *vp)
+{
+	int ret, txq_count;
+
+#if defined USE_TX_BACKOFF_DELAY_IF_NO_BUFFERS
+#define TX_BACKOFF_WEIGHT_INCR_STEP (1)
+#define TX_BACKOFF_WEIGHT_DECR_STEP (1)
+#define TX_BACKOFF_WEIGHT_MAX (7)
+#define TX_BACKOFF_WEIGHT_MIN (0)
+#define TX_BACKOFF_WEIGHT_UNIT_MS (10)
+	int backoff_weight = TX_BACKOFF_WEIGHT_MIN;
+	signed long timeout;
+#endif
+
+	/* inform wilc1000_wlan_init that TXQ task is started. */
+	linux_wlan_unlock(&g_linux_wlan->txq_thread_started);
+	while (1) {
+
+		PRINT_D(TX_DBG, "txq_task Taking a nap :)\n");
+		linux_wlan_lock(&g_linux_wlan->txq_event);
+		/* wait_for_completion(&pd->txq_event); */
+		PRINT_D(TX_DBG, "txq_task Who waked me up :$\n");
+
+		if (g_linux_wlan->close) {
+			/*Unlock the mutex in the mac_close function to indicate the exiting of the TX thread */
+			linux_wlan_unlock(&g_linux_wlan->txq_thread_started);
+
+			while (!kthread_should_stop())
+				schedule();
+
+			PRINT_D(TX_DBG, "TX thread stopped\n");
+			break;
+		}
+		PRINT_D(TX_DBG, "txq_task handle the sending packet and let me go to sleep.\n");
+#if !defined USE_TX_BACKOFF_DELAY_IF_NO_BUFFERS
+		g_linux_wlan->oup.wlan_handle_tx_que();
+#else
+		do {
+			ret = g_linux_wlan->oup.wlan_handle_tx_que(&txq_count);
+			if (txq_count < FLOW_CONTROL_LOWER_THRESHOLD /* && netif_queue_stopped(pd->wilc_netdev)*/) {
+				PRINT_D(TX_DBG, "Waking up queue\n");
+				/* netif_wake_queue(pd->wilc_netdev); */
+				if (netif_queue_stopped(g_linux_wlan->strInterfaceInfo[0].wilc_netdev))
+					netif_wake_queue(g_linux_wlan->strInterfaceInfo[0].wilc_netdev);
+				if (netif_queue_stopped(g_linux_wlan->strInterfaceInfo[1].wilc_netdev))
+					netif_wake_queue(g_linux_wlan->strInterfaceInfo[1].wilc_netdev);
+			}
+
+			if (ret == WILC_TX_ERR_NO_BUF) { /* failed to allocate buffers in chip. */
+				timeout = msecs_to_jiffies(TX_BACKOFF_WEIGHT_UNIT_MS << backoff_weight);
+				do {
+					/* Back off from sending packets for some time. */
+					/* schedule_timeout will allow RX task to run and free buffers.*/
+					/* set_current_state(TASK_UNINTERRUPTIBLE); */
+					/* timeout = schedule_timeout(timeout); */
+					msleep(TX_BACKOFF_WEIGHT_UNIT_MS << backoff_weight);
+				} while (/*timeout*/ 0);
+				backoff_weight += TX_BACKOFF_WEIGHT_INCR_STEP;
+				if (backoff_weight > TX_BACKOFF_WEIGHT_MAX) {
+					backoff_weight = TX_BACKOFF_WEIGHT_MAX;
+				}
+			} else {
+				if (backoff_weight > TX_BACKOFF_WEIGHT_MIN) {
+					backoff_weight -= TX_BACKOFF_WEIGHT_DECR_STEP;
+					if (backoff_weight < TX_BACKOFF_WEIGHT_MIN) {
+						backoff_weight = TX_BACKOFF_WEIGHT_MIN;
+					}
+				}
+			}
+			/*TODO: drop packets after a certain time/number of retry count. */
+		} while (ret == WILC_TX_ERR_NO_BUF && !g_linux_wlan->close); /* retry sending packets if no more buffers in chip. */
+#endif
+	}
+	return 0;
+}
+
+static void linux_wlan_rx_complete(void)
+{
+	PRINT_D(RX_DBG, "RX completed\n");
+}
+
+int linux_wlan_get_firmware(perInterface_wlan_t *p_nic)
+{
+
+	perInterface_wlan_t *nic = p_nic;
+	int ret = 0;
+	const struct firmware *wilc_firmware;
+	char *firmware;
+
+
+	if (nic->iftype == AP_MODE)
+		firmware = AP_FIRMWARE;
+	else if (nic->iftype == STATION_MODE)
+		firmware = STA_FIRMWARE;
+
+	/*BugID_5137*/
+	else {
+		PRINT_D(INIT_DBG, "Get P2P_CONCURRENCY_FIRMWARE\n");
+		firmware = P2P_CONCURRENCY_FIRMWARE;
+	}
+
+
+
+	if (nic == NULL) {
+		PRINT_ER("NIC is NULL\n");
+		goto _fail_;
+	}
+
+	if (&nic->wilc_netdev->dev == NULL) {
+		PRINT_ER("&nic->wilc_netdev->dev  is NULL\n");
+		goto _fail_;
+	}
+
+
+	/*	the firmare should be located in /lib/firmware in
+	 *      root file system with the name specified above */
+
+#ifdef WILC_SDIO
+	if (request_firmware(&wilc_firmware, firmware, &g_linux_wlan->wilc_sdio_func->dev) != 0) {
+		PRINT_ER("%s - firmare not available\n", firmware);
+		ret = -1;
+		goto _fail_;
+	}
+#else
+	if (request_firmware(&wilc_firmware, firmware, &g_linux_wlan->wilc_spidev->dev) != 0) {
+		PRINT_ER("%s - firmare not available\n", firmware);
+		ret = -1;
+		goto _fail_;
+	}
+#endif
+	g_linux_wlan->wilc_firmware = wilc_firmware; /* Bug 4703 */
+
+_fail_:
+
+	return ret;
+
+}
+
+#ifdef COMPLEMENT_BOOT
+int repeat_power_cycle(perInterface_wlan_t *nic);
+#endif
+
+static int linux_wlan_start_firmware(perInterface_wlan_t *nic)
+{
+
+	int ret = 0;
+	/* start firmware */
+	PRINT_D(INIT_DBG, "Starting Firmware ...\n");
+	ret = g_linux_wlan->oup.wlan_start();
+	if (ret < 0) {
+		PRINT_ER("Failed to start Firmware\n");
+		goto _fail_;
+	}
+
+	/* wait for mac ready */
+	PRINT_D(INIT_DBG, "Waiting for Firmware to get ready ...\n");
+	ret = linux_wlan_lock_timeout(&g_linux_wlan->sync_event, 5000);
+	if (ret) {
+#ifdef COMPLEMENT_BOOT
+		static int timeout = 5;
+
+		if (timeout--) {
+			PRINT_D(INIT_DBG, "repeat power cycle[%d]", timeout);
+			ret = repeat_power_cycle(nic);
+		} else {
+			timeout = 5;
+			ret = -1;
+			goto _fail_;
+		}
+#endif
+		PRINT_D(INIT_DBG, "Firmware start timed out");
+		goto _fail_;
+	}
+	/*
+	 *      TODO: Driver shouoldn't wait forever for firmware to get started -
+	 *      in case of timeout this should be handled properly
+	 */
+	PRINT_D(INIT_DBG, "Firmware successfully started\n");
+
+_fail_:
+	return ret;
+}
+static int linux_wlan_firmware_download(linux_wlan_t *p_nic)
+{
+
+	int ret = 0;
+
+	if (g_linux_wlan->wilc_firmware == NULL) {
+		PRINT_ER("Firmware buffer is NULL\n");
+		ret = -ENOBUFS;
+		goto _FAIL_;
+	}
+	/**
+	 *      do the firmware download
+	 **/
+	PRINT_D(INIT_DBG, "Downloading Firmware ...\n");
+	ret = g_linux_wlan->oup.wlan_firmware_download(g_linux_wlan->wilc_firmware->data, g_linux_wlan->wilc_firmware->size);
+	if (ret < 0) {
+		goto _FAIL_;
+	}
+
+	/* Freeing FW buffer */
+	PRINT_D(INIT_DBG, "Freeing FW buffer ...\n");
+	PRINT_D(INIT_DBG, "Releasing firmware\n");
+	release_firmware(g_linux_wlan->wilc_firmware);
+	g_linux_wlan->wilc_firmware = NULL;
+
+	PRINT_D(INIT_DBG, "Download Succeeded \n");
+
+_FAIL_:
+	return ret;
+}
+
+
+/* startup configuration - could be changed later using iconfig*/
+static int linux_wlan_init_test_config(struct net_device *dev, linux_wlan_t *p_nic)
+{
+
+	unsigned char c_val[64];
+	#ifndef STATIC_MACADDRESS
+	unsigned char mac_add[] = {0x00, 0x80, 0xC2, 0x5E, 0xa2, 0xff};
+	#endif
+
+	/*BugID_5077*/
+	struct WILC_WFI_priv *priv;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+
+	PRINT_D(TX_DBG, "Start configuring Firmware\n");
+	#ifndef STATIC_MACADDRESS
+	get_random_bytes(&mac_add[5], 1);
+	get_random_bytes(&mac_add[4], 1);
+	#endif
+	priv = wiphy_priv(dev->ieee80211_ptr->wiphy);
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+	PRINT_D(INIT_DBG, "Host = %p\n", pstrWFIDrv);
+
+	PRINT_D(INIT_DBG, "MAC address is : %02x-%02x-%02x-%02x-%02x-%02x\n", mac_add[0], mac_add[1], mac_add[2], mac_add[3], mac_add[4], mac_add[5]);
+	wilc_get_chipid(0);
+
+
+	if (g_linux_wlan->oup.wlan_cfg_set == NULL) {
+		PRINT_D(INIT_DBG, "Null p[ointer\n");
+		goto _fail_;
+	}
+
+	*(int *)c_val = (u32)pstrWFIDrv;
+
+	if (!g_linux_wlan->oup.wlan_cfg_set(1, WID_SET_DRV_HANDLER, c_val, 4, 0, 0))
+		goto _fail_;
+
+	/*to tell fw that we are going to use PC test - WILC specific*/
+	c_val[0] = 0;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_PC_TEST_MODE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = INFRASTRUCTURE;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_BSS_TYPE, c_val, 1, 0, 0))
+		goto _fail_;
+
+
+	/* c_val[0] = RATE_AUTO; / * bug 4275: Enable autorate and limit it to 24Mbps * / */
+	c_val[0] = RATE_AUTO;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_CURRENT_TX_RATE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = G_MIXED_11B_2_MODE;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11G_OPERATING_MODE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 1;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_CURRENT_CHANNEL, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = G_SHORT_PREAMBLE;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_PREAMBLE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = AUTO_PROT;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_PROT_MECH, c_val, 1, 0, 0))
+		goto _fail_;
+
+#ifdef SWITCH_LOG_TERMINAL
+	c_val[0] = AUTO_PROT;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_LOGTerminal_Switch, c_val, 1, 0, 0))
+		goto _fail_;
+#endif
+
+	c_val[0] = ACTIVE_SCAN;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_SCAN_TYPE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = SITE_SURVEY_OFF;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_SITE_SURVEY, c_val, 1, 0, 0))
+		goto _fail_;
+
+	*((int *)c_val) = 0xffff; /* Never use RTS-CTS */
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_RTS_THRESHOLD, c_val, 2, 0, 0))
+		goto _fail_;
+
+	*((int *)c_val) = 2346;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_FRAG_THRESHOLD, c_val, 2, 0, 0))
+		goto _fail_;
+
+	/*  SSID                                                                 */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :   String with length less than 32 bytes              */
+	/*  Values to set :   Any string with length less than 32 bytes          */
+	/*                    ( In BSS Station Set SSID to "" (null string)      */
+	/*                      to enable Broadcast SSID suppport )              */
+	/*  --------------------------------------------------------------       */
+#ifndef USE_WIRELESS
+	strcpy(c_val, "nwifi");
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_SSID, c_val, (strlen(c_val) + 1), 0, 0))
+		goto _fail_;
+#endif
+
+	c_val[0] = 0;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_BCAST_SSID, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 1;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_QOS_ENABLE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = NO_POWERSAVE;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_POWER_MANAGEMENT, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = NO_ENCRYPT; /* NO_ENCRYPT, 0x79 */
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11I_MODE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = OPEN_SYSTEM;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_AUTH_TYPE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	/*  WEP/802 11I Configuration                                            */
+	/*  ------------------------------------------------------------------   */
+	/*  Configuration : WEP Key                                              */
+	/*  Values (0x)   : 5 byte for WEP40 and 13 bytes for WEP104             */
+	/*                  In case more than 5 bytes are passed on for WEP 40   */
+	/*                  only first 5 bytes will be used as the key           */
+	/*  ------------------------------------------------------------------   */
+
+	strcpy(c_val, "123456790abcdef1234567890");
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_WEP_KEY_VALUE, c_val, (strlen(c_val) + 1), 0, 0))
+		goto _fail_;
+
+	/*  WEP/802 11I Configuration                                            */
+	/*  ------------------------------------------------------------------   */
+	/*  Configuration : AES/TKIP WPA/RSNA Pre-Shared Key                     */
+	/*  Values to set : Any string with length greater than equal to 8 bytes */
+	/*                  and less than 64 bytes                               */
+	/*  ------------------------------------------------------------------   */
+	strcpy(c_val, "12345678");
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11I_PSK, c_val, (strlen(c_val)), 0, 0))
+		goto _fail_;
+
+	/*  IEEE802.1X Key Configuration                                         */
+	/*  ------------------------------------------------------------------   */
+	/*  Configuration : Radius Server Access Secret Key                      */
+	/*  Values to set : Any string with length greater than equal to 8 bytes */
+	/*                  and less than 65 bytes                               */
+	/*  ------------------------------------------------------------------   */
+	strcpy(c_val, "password");
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_1X_KEY, c_val, (strlen(c_val) + 1), 0, 0))
+		goto _fail_;
+
+	/*   IEEE802.1X Server Address Configuration                             */
+	/*  ------------------------------------------------------------------   */
+	/*  Configuration : Radius Server IP Address                             */
+	/*  Values to set : Any valid IP Address                                 */
+	/*  ------------------------------------------------------------------   */
+	c_val[0] = 192;
+	c_val[1] = 168;
+	c_val[2] = 1;
+	c_val[3] = 112;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_1X_SERV_ADDR, c_val, 4, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 3;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_LISTEN_INTERVAL, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 3;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_DTIM_PERIOD, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = NORMAL_ACK;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_ACK_POLICY, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 0;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_USER_CONTROL_ON_TX_POWER, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 48;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_TX_POWER_LEVEL_11A, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 28;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_TX_POWER_LEVEL_11B, c_val, 1, 0, 0))
+		goto _fail_;
+
+	/*  Beacon Interval                                                      */
+	/*  -------------------------------------------------------------------- */
+	/*  Configuration : Sets the beacon interval value                       */
+	/*  Values to set : Any 16-bit value                                     */
+	/*  -------------------------------------------------------------------- */
+
+	*((int *)c_val) = 100;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_BEACON_INTERVAL, c_val, 2, 0, 0))
+		goto _fail_;
+
+	c_val[0] = REKEY_DISABLE;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_REKEY_POLICY, c_val, 1, 0, 0))
+		goto _fail_;
+
+	/*  Rekey Time (s) (Used only when the Rekey policy is 2 or 4)           */
+	/*  -------------------------------------------------------------------- */
+	/*  Configuration : Sets the Rekey Time (s)                              */
+	/*  Values to set : 32-bit value                                         */
+	/*  -------------------------------------------------------------------- */
+	*((int *)c_val) = 84600;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_REKEY_PERIOD, c_val, 4, 0, 0))
+		goto _fail_;
+
+	/*  Rekey Packet Count (in 1000s; used when Rekey Policy is 3)           */
+	/*  -------------------------------------------------------------------- */
+	/*  Configuration : Sets Rekey Group Packet count                        */
+	/*  Values to set : 32-bit Value                                         */
+	/*  -------------------------------------------------------------------- */
+	*((int *)c_val) = 500;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_REKEY_PACKET_COUNT, c_val, 4, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 1;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_SHORT_SLOT_ALLOWED, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = G_SELF_CTS_PROT;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_ERP_PROT_TYPE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 1;  /* Enable N */
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_ENABLE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = HT_MIXED_MODE;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_OPERATING_MODE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 1;   /* TXOP Prot disable in N mode: No RTS-CTS on TX A-MPDUs to save air-time. */
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_TXOP_PROT_DISABLE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	memcpy(c_val, mac_add, 6);
+
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_MAC_ADDR, c_val, 6, 0, 0))
+		goto _fail_;
+
+	/**
+	 *      AP only
+	 **/
+	c_val[0] = DETECT_PROTECT_REPORT;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_OBSS_NONHT_DETECTION, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = RTS_CTS_NONHT_PROT;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_HT_PROT_TYPE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 0;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_RIFS_PROT_ENABLE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = MIMO_MODE;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_SMPS_MODE, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 7;
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_CURRENT_TX_MCS, c_val, 1, 0, 0))
+		goto _fail_;
+
+	c_val[0] = 1; /* Enable N with immediate block ack. */
+	if (!g_linux_wlan->oup.wlan_cfg_set(0, WID_11N_IMMEDIATE_BA_ENABLED, c_val, 1, 1, (u32)pstrWFIDrv))
+		goto _fail_;
+
+	return 0;
+
+_fail_:
+	return -1;
+}
+
+
+/**************************/
+void wilc1000_wlan_deinit(linux_wlan_t *nic)
+{
+
+	if (g_linux_wlan->wilc1000_initialized)	{
+
+		printk("Deinitializing wilc1000  ...\n");
+
+		if (nic == NULL) {
+			PRINT_ER("nic is NULL\n");
+			return;
+		}
+
+#if defined(PLAT_ALLWINNER_A20) || defined(PLAT_ALLWINNER_A23) || defined(PLAT_ALLWINNER_A31)
+		/* johnny : remove */
+		PRINT_D(INIT_DBG, "skip wilc_bus_set_default_speed\n");
+#else
+		wilc_bus_set_default_speed();
+#endif
+
+		PRINT_D(INIT_DBG, "Disabling IRQ\n");
+		#if (!defined WILC_SDIO) || (defined WILC_SDIO_IRQ_GPIO)
+		linux_wlan_disable_irq(IRQ_WAIT);
+		#else
+		  #if defined(PLAT_ALLWINNER_A20) || defined(PLAT_ALLWINNER_A23) || defined(PLAT_ALLWINNER_A31)
+
+		  #else
+		linux_wlan_lock_mutex((void *)&g_linux_wlan->hif_cs);
+		disable_sdio_interrupt();
+		linux_wlan_unlock_mutex((void *)&g_linux_wlan->hif_cs);
+		  #endif
+		#endif
+
+
+		/* not sure if the following unlocks are needed or not*/
+		if (&g_linux_wlan->rxq_event != NULL) {
+			linux_wlan_unlock(&g_linux_wlan->rxq_event);
+		}
+
+		if (&g_linux_wlan->txq_event != NULL) {
+			linux_wlan_unlock(&g_linux_wlan->txq_event);
+		}
+
+
+	#if (RX_BH_TYPE == RX_BH_WORK_QUEUE)
+		/*Removing the work struct from the linux kernel workqueue*/
+		if (&g_linux_wlan->rx_work_queue != NULL)
+			flush_work(&g_linux_wlan->rx_work_queue);
+
+	#elif (RX_BH_TYPE == RX_BH_KTHREAD)
+		/* if(&nic->rx_sem != NULL) */
+		/* linux_wlan_unlock(&nic->rx_sem); */
+	#endif
+
+		PRINT_D(INIT_DBG, "Deinitializing Threads\n");
+		wlan_deinitialize_threads(nic);
+
+		PRINT_D(INIT_DBG, "Deinitializing IRQ\n");
+		deinit_irq(g_linux_wlan);
+
+
+		if (&g_linux_wlan->oup != NULL) {
+			if (g_linux_wlan->oup.wlan_stop != NULL)
+				g_linux_wlan->oup.wlan_stop();
+		}
+
+		PRINT_D(INIT_DBG, "Deinitializing WILC Wlan\n");
+		wilc_wlan_deinit(nic);
+#if (defined WILC_SDIO) && (!defined WILC_SDIO_IRQ_GPIO)
+  #if defined(PLAT_ALLWINNER_A20) || defined(PLAT_ALLWINNER_A23) || defined(PLAT_ALLWINNER_A31)
+		PRINT_D(INIT_DBG, "Disabling IRQ 2\n");
+
+		linux_wlan_lock_mutex((void *)&g_linux_wlan->hif_cs);
+		disable_sdio_interrupt();
+		linux_wlan_unlock_mutex((void *)&g_linux_wlan->hif_cs);
+  #endif
+#endif
+
+		/*De-Initialize locks*/
+		PRINT_D(INIT_DBG, "Deinitializing Locks\n");
+		wlan_deinit_locks(g_linux_wlan);
+
+		/* announce that wilc1000 is not initialized */
+		g_linux_wlan->wilc1000_initialized = 0;
+
+		PRINT_D(INIT_DBG, "wilc1000 deinitialization Done\n");
+
+	} else {
+		PRINT_D(INIT_DBG, "wilc1000 is not initialized\n");
+	}
+	return;
+}
+
+int wlan_init_locks(linux_wlan_t *p_nic)
+{
+
+	PRINT_D(INIT_DBG, "Initializing Locks ...\n");
+
+	/*initialize mutexes*/
+	linux_wlan_init_mutex("hif_lock/hif_cs", &g_linux_wlan->hif_cs, 1);
+	linux_wlan_init_mutex("rxq_lock/rxq_cs", &g_linux_wlan->rxq_cs, 1);
+	linux_wlan_init_mutex("txq_lock/txq_cs", &g_linux_wlan->txq_cs, 1);
+
+	/*Added by Amr - BugID_4720*/
+	linux_wlan_init_spin_lock("txq_spin_lock/txq_cs", &g_linux_wlan->txq_spinlock, 1);
+
+	/*Added by Amr - BugID_4720*/
+	linux_wlan_init_lock("txq_add_to_head_lock/txq_cs", &g_linux_wlan->txq_add_to_head_cs, 1);
+
+	linux_wlan_init_lock("txq_wait/txq_event", &g_linux_wlan->txq_event, 0);
+	linux_wlan_init_lock("rxq_wait/rxq_event", &g_linux_wlan->rxq_event, 0);
+
+	linux_wlan_init_lock("cfg_wait/cfg_event", &g_linux_wlan->cfg_event, 0);
+	linux_wlan_init_lock("sync_event", &g_linux_wlan->sync_event, 0);
+
+	linux_wlan_init_lock("rxq_lock/rxq_started", &g_linux_wlan->rxq_thread_started, 0);
+	linux_wlan_init_lock("rxq_lock/txq_started", &g_linux_wlan->txq_thread_started, 0);
+
+	#if (RX_BH_TYPE == RX_BH_KTHREAD)
+	linux_wlan_init_lock("BH_SEM", &g_linux_wlan->rx_sem, 0);
+	#endif
+
+	return 0;
+}
+
+static int wlan_deinit_locks(linux_wlan_t *nic)
+{
+	PRINT_D(INIT_DBG, "De-Initializing Locks\n");
+
+	if (&g_linux_wlan->hif_cs != NULL)
+		linux_wlan_deinit_mutex(&g_linux_wlan->hif_cs);
+
+	if (&g_linux_wlan->rxq_cs != NULL)
+		linux_wlan_deinit_mutex(&g_linux_wlan->rxq_cs);
+
+	if (&g_linux_wlan->txq_cs != NULL)
+		linux_wlan_deinit_mutex(&g_linux_wlan->txq_cs);
+
+	/*Added by Amr - BugID_4720*/
+	if (&g_linux_wlan->txq_spinlock != NULL)
+		linux_wlan_deinit_spin_lock(&g_linux_wlan->txq_spinlock);
+
+	if (&g_linux_wlan->rxq_event != NULL)
+		linux_wlan_deinit_lock(&g_linux_wlan->rxq_event);
+
+	if (&g_linux_wlan->txq_event != NULL)
+		linux_wlan_deinit_lock(&g_linux_wlan->txq_event);
+
+	/*Added by Amr - BugID_4720*/
+	if (&g_linux_wlan->txq_add_to_head_cs != NULL)
+		linux_wlan_deinit_lock(&g_linux_wlan->txq_add_to_head_cs);
+
+	if (&g_linux_wlan->rxq_thread_started != NULL)
+		linux_wlan_deinit_lock(&g_linux_wlan->rxq_thread_started);
+
+	if (&g_linux_wlan->txq_thread_started != NULL)
+		linux_wlan_deinit_lock(&g_linux_wlan->txq_thread_started);
+
+	if (&g_linux_wlan->cfg_event != NULL)
+		linux_wlan_deinit_lock(&g_linux_wlan->cfg_event);
+
+	if (&g_linux_wlan->sync_event != NULL)
+		linux_wlan_deinit_lock(&g_linux_wlan->sync_event);
+
+	return 0;
+}
+void linux_to_wlan(wilc_wlan_inp_t *nwi, linux_wlan_t *nic)
+{
+
+	PRINT_D(INIT_DBG, "Linux to Wlan services ...\n");
+
+	nwi->os_context.hif_critical_section = (void *)&g_linux_wlan->hif_cs;
+	nwi->os_context.os_private = (void *)nic;
+	nwi->os_context.tx_buffer_size = LINUX_TX_SIZE;
+	nwi->os_context.txq_critical_section = (void *)&g_linux_wlan->txq_cs;
+
+	/*Added by Amr - BugID_4720*/
+	nwi->os_context.txq_add_to_head_critical_section = (void *)&g_linux_wlan->txq_add_to_head_cs;
+
+	/*Added by Amr - BugID_4720*/
+	nwi->os_context.txq_spin_lock = (void *)&g_linux_wlan->txq_spinlock;
+
+	nwi->os_context.txq_wait_event = (void *)&g_linux_wlan->txq_event;
+
+#if defined (MEMORY_STATIC)
+	nwi->os_context.rx_buffer_size = LINUX_RX_SIZE;
+#endif
+	nwi->os_context.rxq_critical_section = (void *)&g_linux_wlan->rxq_cs;
+	nwi->os_context.rxq_wait_event = (void *)&g_linux_wlan->rxq_event;
+	nwi->os_context.cfg_wait_event = (void *)&g_linux_wlan->cfg_event;
+
+	nwi->os_func.os_sleep = linux_wlan_msleep;
+	nwi->os_func.os_atomic_sleep = linux_wlan_atomic_msleep;
+	nwi->os_func.os_debug = linux_wlan_dbg;
+	nwi->os_func.os_malloc = linux_wlan_malloc;
+	nwi->os_func.os_malloc_atomic = linux_wlan_malloc_atomic;
+	nwi->os_func.os_free = linux_wlan_free;
+	nwi->os_func.os_lock = linux_wlan_lock;
+	nwi->os_func.os_unlock = linux_wlan_unlock;
+	nwi->os_func.os_wait = linux_wlan_lock_timeout;
+	nwi->os_func.os_signal = linux_wlan_unlock;
+	nwi->os_func.os_enter_cs = linux_wlan_lock_mutex;
+	nwi->os_func.os_leave_cs = linux_wlan_unlock_mutex;
+
+	/*Added by Amr - BugID_4720*/
+	nwi->os_func.os_spin_lock = linux_wlan_spin_lock;
+	nwi->os_func.os_spin_unlock = linux_wlan_spin_unlock;
+
+#ifdef WILC_SDIO
+	nwi->io_func.io_type = HIF_SDIO;
+	nwi->io_func.io_init = linux_sdio_init;
+	nwi->io_func.io_deinit = linux_sdio_deinit;
+	nwi->io_func.u.sdio.sdio_cmd52 = linux_sdio_cmd52;
+	nwi->io_func.u.sdio.sdio_cmd53 = linux_sdio_cmd53;
+	nwi->io_func.u.sdio.sdio_set_max_speed = linux_sdio_set_max_speed;
+	nwi->io_func.u.sdio.sdio_set_default_speed = linux_sdio_set_default_speed;
+#else
+	nwi->io_func.io_type = HIF_SPI;
+	nwi->io_func.io_init = linux_spi_init;
+	nwi->io_func.io_deinit = linux_spi_deinit;
+	nwi->io_func.u.spi.spi_tx = linux_spi_write;
+	nwi->io_func.u.spi.spi_rx = linux_spi_read;
+	nwi->io_func.u.spi.spi_trx = linux_spi_write_read;
+	nwi->io_func.u.spi.spi_max_speed = linux_spi_set_max_speed;
+#endif
+
+	/*for now - to be revised*/
+	#ifdef WILC_FULLY_HOSTING_AP
+	/* incase of Fully hosted AP, all non cfg pkts are processed here*/
+	nwi->net_func.rx_indicate = WILC_Process_rx_frame;
+	#else
+	nwi->net_func.rx_indicate = frmw_to_linux;
+	#endif
+	nwi->net_func.rx_complete = linux_wlan_rx_complete;
+	nwi->indicate_func.mac_indicate = linux_wlan_mac_indicate;
+}
+
+int wlan_initialize_threads(perInterface_wlan_t *nic)
+{
+
+	int ret = 0;
+	PRINT_D(INIT_DBG, "Initializing Threads ...\n");
+
+#if (RX_BH_TYPE == RX_BH_WORK_QUEUE)
+	/*Initialize rx work queue task*/
+	INIT_WORK(&g_linux_wlan->rx_work_queue, isr_bh_routine);
+#elif (RX_BH_TYPE == RX_BH_KTHREAD)
+	PRINT_D(INIT_DBG, "Creating kthread for Rxq BH\n");
+	g_linux_wlan->rx_bh_thread = kthread_run(isr_bh_routine, (void *)g_linux_wlan, "K_RXQ_BH");
+	if (g_linux_wlan->rx_bh_thread == 0) {
+		PRINT_ER("couldn't create RX BH thread\n");
+		ret = -ENOBUFS;
+		goto _fail_;
+	}
+#endif
+
+#ifndef TCP_ENHANCEMENTS
+	/* create rx task */
+	PRINT_D(INIT_DBG, "Creating kthread for reception\n");
+	g_linux_wlan->rxq_thread = kthread_run(linux_wlan_rxq_task, (void *)g_linux_wlan, "K_RXQ_TASK");
+	if (g_linux_wlan->rxq_thread == 0) {
+		PRINT_ER("couldn't create RXQ thread\n");
+		ret = -ENOBUFS;
+		goto _fail_1;
+	}
+
+	/* wait for RXQ task to start. */
+	linux_wlan_lock(&g_linux_wlan->rxq_thread_started);
+
+#endif
+
+	/* create tx task */
+	PRINT_D(INIT_DBG, "Creating kthread for transmission\n");
+	g_linux_wlan->txq_thread = kthread_run(linux_wlan_txq_task, (void *)g_linux_wlan, "K_TXQ_TASK");
+	if (g_linux_wlan->txq_thread == 0) {
+		PRINT_ER("couldn't create TXQ thread\n");
+		ret = -ENOBUFS;
+		goto _fail_2;
+	}
+#ifdef DEBUG_MODE
+	PRINT_D(INIT_DBG, "Creating kthread for Debugging\n");
+	g_linux_wlan->txq_thread = kthread_run(DebuggingThreadTask, (void *)g_linux_wlan, "DebugThread");
+	if (g_linux_wlan->txq_thread == 0) {
+		PRINT_ER("couldn't create TXQ thread\n");
+		ret = -ENOBUFS;
+		goto _fail_2;
+	}
+#endif
+	/* wait for TXQ task to start. */
+	linux_wlan_lock(&g_linux_wlan->txq_thread_started);
+
+	return 0;
+
+_fail_2:
+	/*De-Initialize 2nd thread*/
+	g_linux_wlan->close = 1;
+	linux_wlan_unlock(&g_linux_wlan->rxq_event);
+	kthread_stop(g_linux_wlan->rxq_thread);
+
+#ifndef TCP_ENHANCEMENTS
+_fail_1:
+#endif
+	#if (RX_BH_TYPE == RX_BH_KTHREAD)
+	/*De-Initialize 1st thread*/
+	g_linux_wlan->close = 1;
+	linux_wlan_unlock(&g_linux_wlan->rx_sem);
+	kthread_stop(g_linux_wlan->rx_bh_thread);
+_fail_:
+	#endif
+	g_linux_wlan->close = 0;
+	return ret;
+}
+
+static void wlan_deinitialize_threads(linux_wlan_t *nic)
+{
+
+	g_linux_wlan->close = 1;
+	PRINT_D(INIT_DBG, "Deinitializing Threads\n");
+	if (&g_linux_wlan->rxq_event != NULL)
+		linux_wlan_unlock(&g_linux_wlan->rxq_event);
+
+
+	if (g_linux_wlan->rxq_thread != NULL) {
+		kthread_stop(g_linux_wlan->rxq_thread);
+		g_linux_wlan->rxq_thread = NULL;
+	}
+
+
+	if (&g_linux_wlan->txq_event != NULL)
+		linux_wlan_unlock(&g_linux_wlan->txq_event);
+
+
+	if (g_linux_wlan->txq_thread != NULL) {
+		kthread_stop(g_linux_wlan->txq_thread);
+		g_linux_wlan->txq_thread = NULL;
+	}
+
+	#if (RX_BH_TYPE == RX_BH_KTHREAD)
+	if (&g_linux_wlan->rx_sem != NULL)
+		linux_wlan_unlock(&g_linux_wlan->rx_sem);
+
+	if (g_linux_wlan->rx_bh_thread != NULL) {
+		kthread_stop(g_linux_wlan->rx_bh_thread);
+		g_linux_wlan->rx_bh_thread = NULL;
+	}
+	#endif
+}
+
+#ifdef STATIC_MACADDRESS
+const char *path_string[] = {
+	"/etc/wlan",
+	"/data/wlan",
+};
+
+static int linux_wlan_read_mac_addr(void *vp)
+{
+	int ret = 0;
+	struct file *fp = (struct file *)-ENOENT;
+	mm_segment_t old_fs;
+	loff_t pos = 0;
+	int index;
+	int array_size = ARRAY_SIZE(path_string);
+
+	/* change to KERNEL_DS address limit */
+	old_fs = get_fs();
+	set_fs(KERNEL_DS);
+
+	for (index = 0; index < array_size; index++) {
+		fp = filp_open(path_string[index], O_WRONLY, 0640);
+		if (!fp) {
+			ret = -1;
+			goto exit;
+		}
+
+		/*No such file or directory */
+		if (IS_ERR(fp) || !fp->f_op) {
+			get_random_bytes(&mac_add[3], 3);
+			/* open file to write */
+			fp = filp_open(path_string[index], O_WRONLY | O_CREAT, 0640);
+
+			if (!fp || IS_ERR(fp)) {
+				ret = -1;
+				continue;
+			} else {
+				/* write buf to file */
+				fp->f_op->write(fp, mac_add, 6, &pos);
+				break;
+			}
+		} else {
+			/* read file to buf */
+			fp->f_op->read(fp, mac_add, 6, &pos);
+			break;
+		}
+	}
+
+	if (index == array_size) {
+		PRINT_ER("random MAC\n");
+	}
+
+exit:
+	if (fp && !IS_ERR(fp)) {
+		filp_close(fp, NULL);
+	}
+
+	set_fs(old_fs);
+
+	return ret;
+}
+#endif
+
+#ifdef COMPLEMENT_BOOT
+
+extern volatile int probe;
+extern uint8_t core_11b_ready(void);
+
+#define READY_CHECK_THRESHOLD		30
+extern void wilc_wlan_global_reset(void);
+uint8_t wilc1000_prepare_11b_core(wilc_wlan_inp_t *nwi,	wilc_wlan_oup_t *nwo, linux_wlan_t *nic)
+{
+	uint8_t trials = 0;
+	while ((core_11b_ready() && (READY_CHECK_THRESHOLD > (trials++)))) {
+		PRINT_D(INIT_DBG, "11b core not ready yet: %u\n", trials);
+		wilc_wlan_deinit(nic);
+		wilc_wlan_global_reset();
+		sdio_unregister_driver(&wilc_bus);
+
+		linux_wlan_device_detection(0);
+
+		mdelay(100);
+
+		linux_wlan_device_detection(1);
+
+		sdio_register_driver(&wilc_bus);
+
+		while (!probe) {
+			msleep(100);
+		}
+		probe = 0;
+		g_linux_wlan->wilc_sdio_func = local_sdio_func;
+		linux_to_wlan(nwi, nic);
+		wilc_wlan_init(nwi, nwo);
+	}
+
+	if (READY_CHECK_THRESHOLD <= trials)
+		return 1;
+	else
+		return 0;
+
+}
+
+int repeat_power_cycle(perInterface_wlan_t *nic)
+{
+	int ret = 0;
+	wilc_wlan_inp_t nwi;
+	wilc_wlan_oup_t nwo;
+	sdio_unregister_driver(&wilc_bus);
+
+	linux_wlan_device_detection(0);
+	linux_wlan_device_power(0);
+	msleep(100);
+	linux_wlan_device_power(1);
+	msleep(80);
+	linux_wlan_device_detection(1);
+	msleep(20);
+
+	sdio_register_driver(&wilc_bus);
+
+	/* msleep(1000); */
+	while (!probe) {
+		msleep(100);
+	}
+	probe = 0;
+	g_linux_wlan->wilc_sdio_func = local_sdio_func;
+	linux_to_wlan(&nwi, g_linux_wlan);
+	ret = wilc_wlan_init(&nwi, &nwo);
+
+	g_linux_wlan->mac_status = WILC_MAC_STATUS_INIT;
+	#if (defined WILC_SDIO) && (!defined WILC_SDIO_IRQ_GPIO)
+	enable_sdio_interrupt();
+	#endif
+
+	if (linux_wlan_get_firmware(nic)) {
+		PRINT_ER("Can't get firmware \n");
+		ret = -1;
+		goto __fail__;
+	}
+
+	/*Download firmware*/
+	ret = linux_wlan_firmware_download(g_linux_wlan);
+	if (ret < 0) {
+		PRINT_ER("Failed to download firmware\n");
+		goto __fail__;
+	}
+	/* Start firmware*/
+	ret = linux_wlan_start_firmware(nic);
+	if (ret < 0) {
+		PRINT_ER("Failed to start firmware\n");
+	}
+__fail__:
+	return ret;
+}
+#endif
+
+int wilc1000_wlan_init(struct net_device *dev, perInterface_wlan_t *p_nic)
+{
+	wilc_wlan_inp_t nwi;
+	wilc_wlan_oup_t nwo;
+	perInterface_wlan_t *nic = p_nic;
+	int ret = 0;
+
+	if (!g_linux_wlan->wilc1000_initialized) {
+		g_linux_wlan->mac_status = WILC_MAC_STATUS_INIT;
+		g_linux_wlan->close = 0;
+		g_linux_wlan->wilc1000_initialized = 0;
+
+		wlan_init_locks(g_linux_wlan);
+
+#ifdef STATIC_MACADDRESS
+		wilc_mac_thread = kthread_run(linux_wlan_read_mac_addr, NULL, "wilc_mac_thread");
+		if (wilc_mac_thread < 0) {
+			PRINT_ER("couldn't create Mac addr thread\n");
+		}
+#endif
+
+		linux_to_wlan(&nwi, g_linux_wlan);
+
+		ret = wilc_wlan_init(&nwi, &nwo);
+		if (ret < 0) {
+			PRINT_ER("Initializing WILC_Wlan FAILED\n");
+			ret = -EIO;
+			goto _fail_locks_;
+		}
+		memcpy(&g_linux_wlan->oup, &nwo, sizeof(wilc_wlan_oup_t));
+
+		/*Save the oup structre into global pointer*/
+		gpstrWlanOps = &g_linux_wlan->oup;
+
+
+		ret = wlan_initialize_threads(nic);
+		if (ret < 0) {
+			PRINT_ER("Initializing Threads FAILED\n");
+			ret = -EIO;
+			goto _fail_wilc_wlan_;
+		}
+
+#if (defined WILC_SDIO) && (defined COMPLEMENT_BOOT)
+		if (wilc1000_prepare_11b_core(&nwi, &nwo, g_linux_wlan)) {
+			PRINT_ER("11b Core is not ready\n");
+			ret = -EIO;
+			goto _fail_threads_;
+		}
+#endif
+
+#if (!defined WILC_SDIO) || (defined WILC_SDIO_IRQ_GPIO)
+		if (init_irq(g_linux_wlan)) {
+			PRINT_ER("couldn't initialize IRQ\n");
+			ret = -EIO;
+			goto _fail_threads_;
+		}
+#endif
+
+#if (defined WILC_SDIO) && (!defined WILC_SDIO_IRQ_GPIO)
+		if (enable_sdio_interrupt()) {
+			PRINT_ER("couldn't initialize IRQ\n");
+			ret = -EIO;
+			goto _fail_irq_init_;
+		}
+#endif
+
+		if (linux_wlan_get_firmware(nic)) {
+			PRINT_ER("Can't get firmware \n");
+			ret = -EIO;
+			goto _fail_irq_enable_;
+		}
+
+
+		/*Download firmware*/
+		ret = linux_wlan_firmware_download(g_linux_wlan);
+		if (ret < 0) {
+			PRINT_ER("Failed to download firmware\n");
+			ret = -EIO;
+			goto _fail_irq_enable_;
+		}
+
+		/* Start firmware*/
+		ret = linux_wlan_start_firmware(nic);
+		if (ret < 0) {
+			PRINT_ER("Failed to start firmware\n");
+			ret = -EIO;
+			goto _fail_irq_enable_;
+		}
+
+		wilc_bus_set_max_speed();
+
+		if (g_linux_wlan->oup.wlan_cfg_get(1, WID_FIRMWARE_VERSION, 1, 0)) {
+			int size;
+			char Firmware_ver[20];
+			size = g_linux_wlan->oup.wlan_cfg_get_value(
+					WID_FIRMWARE_VERSION,
+					Firmware_ver, sizeof(Firmware_ver));
+			Firmware_ver[size] = '\0';
+			PRINT_D(INIT_DBG, "***** Firmware Ver = %s  *******\n", Firmware_ver);
+		}
+		/* Initialize firmware with default configuration */
+		ret = linux_wlan_init_test_config(dev, g_linux_wlan);
+
+		if (ret < 0) {
+			PRINT_ER("Failed to configure firmware\n");
+			ret = -EIO;
+			goto _fail_fw_start_;
+		}
+
+		g_linux_wlan->wilc1000_initialized = 1;
+		return 0; /*success*/
+
+
+_fail_fw_start_:
+		if (&g_linux_wlan->oup != NULL) {
+			if (g_linux_wlan->oup.wlan_stop != NULL)
+				g_linux_wlan->oup.wlan_stop();
+		}
+
+_fail_irq_enable_:
+#if (defined WILC_SDIO) && (!defined WILC_SDIO_IRQ_GPIO)
+		disable_sdio_interrupt();
+_fail_irq_init_:
+#endif
+#if (!defined WILC_SDIO) || (defined WILC_SDIO_IRQ_GPIO)
+		deinit_irq(g_linux_wlan);
+
+#endif
+_fail_threads_:
+		wlan_deinitialize_threads(g_linux_wlan);
+_fail_wilc_wlan_:
+		wilc_wlan_deinit(g_linux_wlan);
+_fail_locks_:
+		wlan_deinit_locks(g_linux_wlan);
+		PRINT_ER("WLAN Iinitialization FAILED\n");
+	} else {
+		PRINT_D(INIT_DBG, "wilc1000 already initialized\n");
+	}
+	return ret;
+}
+
+
+/*
+ *      - this function will be called automatically by OS when module inserted.
+ */
+
+#if !defined (NM73131_0_BOARD)
+int mac_init_fn(struct net_device *ndev)
+{
+
+	/*Why we do this !!!*/
+	netif_start_queue(ndev); /* ma */
+	netif_stop_queue(ndev); /* ma */
+
+	return 0;
+}
+#else
+int mac_init_fn(struct net_device *ndev)
+{
+
+	unsigned char mac_add[] = {0x00, 0x50, 0xc2, 0x5e, 0x10, 0x00};
+	/* TODO: get MAC address whenever the source is EPROM - hardcoded and copy it to ndev*/
+	memcpy(ndev->dev_addr, mac_add, 6);
+
+	if (!is_valid_ether_addr(ndev->dev_addr)) {
+		PRINT_ER("Error: Wrong MAC address\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+#endif
+
+
+void    WILC_WFI_frame_register(struct wiphy *wiphy, struct net_device *dev,
+				u16 frame_type, bool reg);
+
+/* This fn is called, when this device is setup using ifconfig */
+#if !defined (NM73131_0_BOARD)
+int mac_open(struct net_device *ndev)
+{
+	perInterface_wlan_t *nic;
+
+	/*BugID_5213*/
+	/*No need for setting mac address here anymore,*/
+	/*Just set it in init_test_config()*/
+	unsigned char mac_add[ETH_ALEN] = {0};
+	int ret = 0;
+	int i = 0;
+	struct WILC_WFI_priv *priv;
+
+	nic = netdev_priv(ndev);
+	priv = wiphy_priv(nic->wilc_netdev->ieee80211_ptr->wiphy);
+	PRINT_D(INIT_DBG, "MAC OPEN[%p]\n", ndev);
+
+	#ifdef USE_WIRELESS
+	ret = WILC_WFI_InitHostInt(ndev);
+	if (ret < 0) {
+		PRINT_ER("Failed to initialize host interface\n");
+
+		return ret;
+	}
+	#endif
+
+	/*initialize platform*/
+	PRINT_D(INIT_DBG, "*** re-init ***\n");
+	ret = wilc1000_wlan_init(ndev, nic);
+	if (ret < 0) {
+		PRINT_ER("Failed to initialize wilc1000\n");
+		WILC_WFI_DeInitHostInt(ndev);
+		return ret;
+	}
+
+	Set_machw_change_vir_if(false);
+
+	host_int_get_MacAddress(priv->hWILCWFIDrv, mac_add);
+	PRINT_D(INIT_DBG, "Mac address: %x:%x:%x:%x:%x:%x\n", mac_add[0], mac_add[1], mac_add[2],
+		mac_add[3], mac_add[4], mac_add[5]);
+
+	/* loop through the NUM of supported devices and set the MAC address */
+	for (i = 0; i < g_linux_wlan->u8NoIfcs; i++) {
+		if (ndev == g_linux_wlan->strInterfaceInfo[i].wilc_netdev) {
+			memcpy(g_linux_wlan->strInterfaceInfo[i].aSrcAddress, mac_add, ETH_ALEN);
+			g_linux_wlan->strInterfaceInfo[i].drvHandler = (u32)priv->hWILCWFIDrv;
+			break;
+		}
+	}
+
+	/* TODO: get MAC address whenever the source is EPROM - hardcoded and copy it to ndev*/
+	memcpy(ndev->dev_addr, g_linux_wlan->strInterfaceInfo[i].aSrcAddress, ETH_ALEN);
+
+	if (!is_valid_ether_addr(ndev->dev_addr)) {
+		PRINT_ER("Error: Wrong MAC address\n");
+		ret = -EINVAL;
+		goto _err_;
+	}
+
+
+	WILC_WFI_frame_register(nic->wilc_netdev->ieee80211_ptr->wiphy, nic->wilc_netdev,
+				nic->g_struct_frame_reg[0].frame_type, nic->g_struct_frame_reg[0].reg);
+	WILC_WFI_frame_register(nic->wilc_netdev->ieee80211_ptr->wiphy, nic->wilc_netdev,
+				nic->g_struct_frame_reg[1].frame_type, nic->g_struct_frame_reg[1].reg);
+	netif_wake_queue(ndev);
+	g_linux_wlan->open_ifcs++;
+	nic->mac_opened = 1;
+	return 0;
+
+_err_:
+	WILC_WFI_DeInitHostInt(ndev);
+	wilc1000_wlan_deinit(g_linux_wlan);
+	return ret;
+}
+#else
+int mac_open(struct net_device *ndev)
+{
+
+	linux_wlan_t *nic;
+	nic = netdev_priv(ndev);
+
+	/*initialize platform*/
+	if (wilc1000_wlan_init(nic)) {
+		PRINT_ER("Failed to initialize platform\n");
+		return 1;
+	}
+	/* Start the network interface queue for this device */
+	PRINT_D(INIT_DBG, "Starting netifQ\n");
+	netif_start_queue(ndev);
+/*	linux_wlan_lock(&close_exit_sync); */
+	return 0;
+}
+#endif
+
+struct net_device_stats *mac_stats(struct net_device *dev)
+{
+	perInterface_wlan_t *nic = netdev_priv(dev);
+
+
+	return &nic->netstats;
+}
+
+/* Setup the multicast filter */
+static void wilc_set_multicast_list(struct net_device *dev)
+{
+
+	struct netdev_hw_addr *ha;
+	struct WILC_WFI_priv *priv;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+	int i = 0;
+	priv = wiphy_priv(dev->ieee80211_ptr->wiphy);
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+
+
+	if (!dev)
+		return;
+
+	PRINT_D(INIT_DBG, "Setting Multicast List with count = %d. \n", dev->mc.count);
+
+	if (dev->flags & IFF_PROMISC) {
+		/* Normally, we should configure the chip to retrive all packets
+		 * but we don't wanna support this right now */
+		/* TODO: add promiscuous mode support */
+		PRINT_D(INIT_DBG, "Set promiscuous mode ON, retrive all packets \n");
+		return;
+	}
+
+	/* If there's more addresses than we handle, get all multicast
+	 * packets and sort them out in software. */
+	if ((dev->flags & IFF_ALLMULTI) || (dev->mc.count) > WILC_MULTICAST_TABLE_SIZE) {
+		PRINT_D(INIT_DBG, "Disable multicast filter, retrive all multicast packets\n");
+		/* get all multicast packets */
+		host_int_setup_multicast_filter((WILC_WFIDrvHandle)pstrWFIDrv, false, 0);
+		return;
+	}
+
+	/* No multicast?  Just get our own stuff */
+	if ((dev->mc.count) == 0) {
+		PRINT_D(INIT_DBG, "Enable multicast filter, retrive directed packets only.\n");
+		host_int_setup_multicast_filter((WILC_WFIDrvHandle)pstrWFIDrv, true, 0);
+		return;
+	}
+
+	/* Store all of the multicast addresses in the hardware filter */
+	netdev_for_each_mc_addr(ha, dev)
+	{
+		WILC_memcpy(gau8MulticastMacAddrList[i], ha->addr, ETH_ALEN);
+		PRINT_D(INIT_DBG, "Entry[%d]: %x:%x:%x:%x:%x:%x\n", i,
+			gau8MulticastMacAddrList[i][0], gau8MulticastMacAddrList[i][1], gau8MulticastMacAddrList[i][2], gau8MulticastMacAddrList[i][3], gau8MulticastMacAddrList[i][4], gau8MulticastMacAddrList[i][5]);
+		i++;
+	}
+
+	host_int_setup_multicast_filter((WILC_WFIDrvHandle)pstrWFIDrv, true, (dev->mc.count));
+
+	return;
+
+}
+
+static void linux_wlan_tx_complete(void *priv, int status)
+{
+
+	struct tx_complete_data *pv_data = (struct tx_complete_data *)priv;
+	if (status == 1) {
+		PRINT_D(TX_DBG, "Packet sent successfully - Size = %d - Address = %p - SKB = %p\n", pv_data->size, pv_data->buff, pv_data->skb);
+	} else {
+		PRINT_D(TX_DBG, "Couldn't send packet - Size = %d - Address = %p - SKB = %p\n", pv_data->size, pv_data->buff, pv_data->skb);
+	}
+	/* Free the SK Buffer, its work is done */
+	dev_kfree_skb(pv_data->skb);
+	linux_wlan_free(pv_data);
+}
+
+int mac_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+	perInterface_wlan_t *nic;
+	struct tx_complete_data *tx_data = NULL;
+	int QueueCount;
+	char *pu8UdpBuffer;
+	struct iphdr *ih;
+	struct ethhdr *eth_h;
+	nic = netdev_priv(ndev);
+
+	PRINT_D(INT_DBG, "\n========\n IntUH: %d - IntBH: %d - IntCld: %d \n========\n", int_rcvdU, int_rcvdB, int_clrd);
+	PRINT_D(TX_DBG, "Sending packet just received from TCP/IP\n");
+
+	/* Stop the network interface queue */
+	if (skb->dev != ndev) {
+		PRINT_ER("Packet not destined to this device\n");
+		return 0;
+	}
+
+	tx_data = (struct tx_complete_data *)internal_alloc(sizeof(struct tx_complete_data), GFP_ATOMIC);
+	if (tx_data == NULL) {
+		PRINT_ER("Failed to allocate memory for tx_data structure\n");
+		dev_kfree_skb(skb);
+		netif_wake_queue(ndev);
+		return 0;
+	}
+
+	tx_data->buff = skb->data;
+	tx_data->size = skb->len;
+	tx_data->skb  = skb;
+
+	eth_h = (struct ethhdr *)(skb->data);
+	if (eth_h->h_proto == 0x8e88) {
+		PRINT_D(INIT_DBG, "EAPOL transmitted\n");
+	}
+
+	/*get source and dest ip addresses*/
+	ih = (struct iphdr *)(skb->data + sizeof(struct ethhdr));
+
+	pu8UdpBuffer = (char *)ih + sizeof(struct iphdr);
+	if ((pu8UdpBuffer[1] == 68 && pu8UdpBuffer[3] == 67) || (pu8UdpBuffer[1] == 67 && pu8UdpBuffer[3] == 68)) {
+		PRINT_D(GENERIC_DBG, "DHCP Message transmitted, type:%x %x %x\n", pu8UdpBuffer[248], pu8UdpBuffer[249], pu8UdpBuffer[250]);
+
+	}
+	PRINT_D(TX_DBG, "Sending packet - Size = %d - Address = %p - SKB = %p\n", tx_data->size, tx_data->buff, tx_data->skb);
+
+	/* Send packet to MAC HW - for now the tx_complete function will be just status
+	 * indicator. still not sure if I need to suspend host transmission till the tx_complete
+	 * function called or not?
+	 * allocated buffer will be freed in tx_complete function.
+	 */
+	PRINT_D(TX_DBG, "Adding tx packet to TX Queue\n");
+	nic->netstats.tx_packets++;
+	nic->netstats.tx_bytes += tx_data->size;
+	tx_data->pBssid = g_linux_wlan->strInterfaceInfo[nic->u8IfIdx].aBSSID;
+	#ifndef WILC_FULLY_HOSTING_AP
+	QueueCount = g_linux_wlan->oup.wlan_add_to_tx_que((void *)tx_data,
+							  tx_data->buff,
+							  tx_data->size,
+							  linux_wlan_tx_complete);
+	#else
+	QueueCount = WILC_Xmit_data((void *)tx_data, HOST_TO_WLAN);
+	#endif /* WILC_FULLY_HOSTING_AP */
+
+
+	if (QueueCount > FLOW_CONTROL_UPPER_THRESHOLD) {
+		netif_stop_queue(g_linux_wlan->strInterfaceInfo[0].wilc_netdev);
+		netif_stop_queue(g_linux_wlan->strInterfaceInfo[1].wilc_netdev);
+	}
+
+	return 0;
+}
+
+
+int mac_close(struct net_device *ndev)
+{
+	struct WILC_WFI_priv *priv;
+	perInterface_wlan_t *nic;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+
+	nic = netdev_priv(ndev);
+
+	if ((nic == NULL) || (nic->wilc_netdev == NULL) || (nic->wilc_netdev->ieee80211_ptr == NULL) || (nic->wilc_netdev->ieee80211_ptr->wiphy == NULL)) {
+		PRINT_ER("nic = NULL\n");
+		return 0;
+	}
+
+	priv = wiphy_priv(nic->wilc_netdev->ieee80211_ptr->wiphy);
+
+	if (priv == NULL) {
+		PRINT_ER("priv = NULL\n");
+		return 0;
+	}
+
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+
+
+
+	PRINT_D(GENERIC_DBG, "Mac close\n");
+
+	if (g_linux_wlan == NULL) {
+		PRINT_ER("g_linux_wlan = NULL\n");
+		return 0;
+	}
+
+	if (pstrWFIDrv == NULL)	{
+		PRINT_ER("pstrWFIDrv = NULL\n");
+		return 0;
+	}
+
+	if ((g_linux_wlan->open_ifcs) > 0) {
+		g_linux_wlan->open_ifcs--;
+	} else {
+		PRINT_ER("ERROR: MAC close called while number of opened interfaces is zero\n");
+		return 0;
+	}
+
+	if (nic->wilc_netdev != NULL) {
+		/* Stop the network interface queue */
+		netif_stop_queue(nic->wilc_netdev);
+
+		#ifdef USE_WIRELESS
+		WILC_WFI_DeInitHostInt(nic->wilc_netdev);
+		#endif
+	}
+
+	if (g_linux_wlan->open_ifcs == 0) {
+		PRINT_D(GENERIC_DBG, "Deinitializing wilc1000\n");
+		g_linux_wlan->close = 1;
+		wilc1000_wlan_deinit(g_linux_wlan);
+		#ifdef USE_WIRELESS
+		#ifdef WILC_AP_EXTERNAL_MLME
+		WILC_WFI_deinit_mon_interface();
+		#endif
+		#endif
+	}
+
+	linux_wlan_unlock(&close_exit_sync);
+	nic->mac_opened = 0;
+
+	return 0;
+}
+
+
+int mac_ioctl(struct net_device *ndev, struct ifreq *req, int cmd)
+{
+
+	u8 *buff = NULL;
+	s8 rssi;
+	u32 size = 0, length = 0;
+	perInterface_wlan_t *nic;
+	struct WILC_WFI_priv *priv;
+	s32 s32Error = WILC_SUCCESS;
+
+
+
+	/* struct iwreq *wrq = (struct iwreq *) req;	// tony moved to case SIOCSIWPRIV */
+	#ifdef USE_WIRELESS
+	nic = netdev_priv(ndev);
+
+	if (!g_linux_wlan->wilc1000_initialized)
+		return 0;
+
+	#endif
+
+	switch (cmd) {
+
+	/* ]] 2013-06-24 */
+	case SIOCSIWPRIV:
+	{
+		struct iwreq *wrq = (struct iwreq *) req;               /* added by tony */
+
+		size = wrq->u.data.length;
+
+		if (size && wrq->u.data.pointer) {
+
+			buff = memdup_user(wrq->u.data.pointer, wrq->u.data.length);
+			if (IS_ERR(buff)) {
+				s32Error = PTR_ERR(buff);
+				goto done;
+			}
+
+			if (strncasecmp(buff, "RSSI", length) == 0) {
+
+					#ifdef USE_WIRELESS
+				priv = wiphy_priv(nic->wilc_netdev->ieee80211_ptr->wiphy);
+				s32Error = host_int_get_rssi(priv->hWILCWFIDrv, &(rssi));
+				if (s32Error)
+					PRINT_ER("Failed to send get rssi param's message queue ");
+					#endif
+				PRINT_INFO(GENERIC_DBG, "RSSI :%d\n", rssi);
+
+				/*Rounding up the rssi negative value*/
+				rssi += 5;
+
+				snprintf(buff, size, "rssi %d", rssi);
+
+				if (copy_to_user(wrq->u.data.pointer, buff, size)) {
+					PRINT_ER("%s: failed to copy data to user buffer\n", __func__);
+					s32Error = -EFAULT;
+					goto done;
+				}
+			}
+		}
+	}
+	break;
+
+	default:
+	{
+		PRINT_INFO(GENERIC_DBG, "Command - %d - has been received\n", cmd);
+		s32Error = -EOPNOTSUPP;
+		goto done;
+	}
+	}
+
+done:
+
+	if (buff != NULL) {
+		kfree(buff);
+	}
+
+	return s32Error;
+}
+
+void frmw_to_linux(uint8_t *buff, uint32_t size, uint32_t pkt_offset)
+{
+
+	unsigned int frame_len = 0;
+	int stats;
+	unsigned char *buff_to_send = NULL;
+	struct sk_buff *skb;
+#ifndef TCP_ENHANCEMENTS
+	char *pu8UdpBuffer;
+	struct iphdr *ih;
+#endif
+	struct net_device *wilc_netdev;
+	perInterface_wlan_t *nic;
+
+	wilc_netdev = GetIfHandler(buff);
+	if (wilc_netdev == NULL)
+		return;
+
+	buff += pkt_offset;
+	nic = netdev_priv(wilc_netdev);
+
+	if (size > 0) {
+
+		frame_len = size;
+		buff_to_send = buff;
+
+
+		/* Need to send the packet up to the host, allocate a skb buffer */
+		skb = dev_alloc_skb(frame_len);
+		if (skb == NULL) {
+			PRINT_ER("Low memory - packet droped\n");
+			return;
+		}
+
+		skb_reserve(skb, (unsigned int)skb->data & 0x3);
+
+		if (g_linux_wlan == NULL || wilc_netdev == NULL) {
+			PRINT_ER("wilc_netdev in g_linux_wlan is NULL");
+		}
+		skb->dev = wilc_netdev;
+
+		if (skb->dev == NULL) {
+			PRINT_ER("skb->dev is NULL\n");
+		}
+
+		/*
+		 * for(i=0;i<40;i++)
+		 * {
+		 *      if(i<frame_len)
+		 *              WILC_PRINTF("buff_to_send[%d]=%2x\n",i,buff_to_send[i]);
+		 *
+		 * }*/
+
+		/* skb_put(skb, frame_len); */
+		memcpy(skb_put(skb, frame_len), buff_to_send, frame_len);
+
+		/* WILC_PRINTF("After MEM_CPY\n"); */
+
+		/* nic = netdev_priv(wilc_netdev); */
+
+#ifdef USE_WIRELESS
+		/*	if(nic->monitor_flag)
+		 *      {
+		 *              WILC_WFI_monitor_rx(nic->wilc_netdev,skb);
+		 *              return;
+		 *      }*/
+#endif
+		skb->protocol = eth_type_trans(skb, wilc_netdev);
+			#ifndef TCP_ENHANCEMENTS
+		/*get source and dest ip addresses*/
+		ih = (struct iphdr *)(skb->data + sizeof(struct ethhdr));
+
+		pu8UdpBuffer = (char *)ih + sizeof(struct iphdr);
+		if (buff_to_send[35] == 67 && buff_to_send[37] == 68) {
+			PRINT_D(RX_DBG, "DHCP Message received\n");
+		}
+		if (buff_to_send[12] == 0x88 && buff_to_send[13] == 0x8e)
+			PRINT_D(GENERIC_DBG, "eapol received\n");
+			#endif
+		/* Send the packet to the stack by giving it to the bridge */
+		nic->netstats.rx_packets++;
+		nic->netstats.rx_bytes += frame_len;
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		stats = netif_rx(skb);
+		PRINT_D(RX_DBG, "netif_rx ret value is: %d\n", stats);
+	}
+		#ifndef TCP_ENHANCEMENTS
+	else {
+		PRINT_ER("Discard sending packet with len = %d\n", size);
+	}
+		#endif
+}
+
+void WILC_WFI_mgmt_rx(uint8_t *buff, uint32_t size)
+{
+	int i = 0;
+	perInterface_wlan_t *nic;
+
+	/*BugID_5450*/
+	/*Pass the frame on the monitor interface, if any.*/
+	/*Otherwise, pass it on p2p0 netdev, if registered on it*/
+	for (i = 0; i < g_linux_wlan->u8NoIfcs; i++) {
+		nic = netdev_priv(g_linux_wlan->strInterfaceInfo[i].wilc_netdev);
+		if (nic->monitor_flag) {
+			WILC_WFI_monitor_rx(buff, size);
+			return;
+		}
+	}
+
+	#ifdef WILC_P2P
+	nic = netdev_priv(g_linux_wlan->strInterfaceInfo[1].wilc_netdev); /* p2p0 */
+	if ((buff[0] == nic->g_struct_frame_reg[0].frame_type && nic->g_struct_frame_reg[0].reg) ||
+	    (buff[0] == nic->g_struct_frame_reg[1].frame_type && nic->g_struct_frame_reg[1].reg)) {
+		WILC_WFI_p2p_rx(g_linux_wlan->strInterfaceInfo[1].wilc_netdev, buff, size);
+	}
+	#endif
+}
+
+int wilc_netdev_init(void)
+{
+
+	int i;
+	perInterface_wlan_t *nic;
+	struct net_device *ndev;
+
+	linux_wlan_init_lock("close_exit_sync", &close_exit_sync, 0);
+
+	/*create the common structure*/
+	g_linux_wlan = (linux_wlan_t *)WILC_MALLOC(sizeof(linux_wlan_t));
+	memset(g_linux_wlan, 0, sizeof(linux_wlan_t));
+
+	/*Reset interrupt count debug*/
+	int_rcvdU = 0;
+	int_rcvdB = 0;
+	int_clrd = 0;
+	#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+	register_inetaddr_notifier(&g_dev_notifier);
+	#endif
+
+	for (i = 0; i < NUM_CONCURRENT_IFC; i++) {
+		/*allocate first ethernet device with perinterface_wlan_t as its private data*/
+		ndev = alloc_etherdev(sizeof(perInterface_wlan_t));
+		if (!ndev) {
+			PRINT_ER("Failed to allocate ethernet dev\n");
+			return -1;
+		}
+
+		nic = netdev_priv(ndev);
+		memset(nic, 0, sizeof(perInterface_wlan_t));
+
+		/*Name the Devices*/
+		if (i == 0) {
+		#if defined(NM73131)    /* tony, 2012-09-20 */
+			strcpy(ndev->name, "wilc_eth%d");
+		#elif defined(PLAT_CLM9722)                     /* rachel */
+			strcpy(ndev->name, "eth%d");
+		#else /* PANDA_BOARD, PLAT_ALLWINNER_A10, PLAT_ALLWINNER_A20, PLAT_ALLWINNER_A31, PLAT_AML8726_M3 or PLAT_WMS8304 */
+			strcpy(ndev->name, "wlan%d");
+		#endif
+		} else
+			strcpy(ndev->name, "p2p%d");
+
+		nic->u8IfIdx = g_linux_wlan->u8NoIfcs;
+		nic->wilc_netdev = ndev;
+		g_linux_wlan->strInterfaceInfo[g_linux_wlan->u8NoIfcs].wilc_netdev = ndev;
+		g_linux_wlan->u8NoIfcs++;
+		ndev->netdev_ops = &wilc_netdev_ops;
+
+		#ifdef USE_WIRELESS
+		{
+			struct wireless_dev *wdev;
+			/*Register WiFi*/
+			wdev = WILC_WFI_WiphyRegister(ndev);
+
+			#ifdef WILC_SDIO
+			/* set netdev, tony */
+			SET_NETDEV_DEV(ndev, &local_sdio_func->dev);
+			#endif
+
+			if (wdev == NULL) {
+				PRINT_ER("Can't register WILC Wiphy\n");
+				return -1;
+			}
+
+			/*linking the wireless_dev structure with the netdevice*/
+			nic->wilc_netdev->ieee80211_ptr = wdev;
+			nic->wilc_netdev->ml_priv = nic;
+			wdev->netdev = nic->wilc_netdev;
+			nic->netstats.rx_packets = 0;
+			nic->netstats.tx_packets = 0;
+			nic->netstats.rx_bytes = 0;
+			nic->netstats.tx_bytes = 0;
+
+		}
+		#endif
+
+
+		if (register_netdev(ndev)) {
+			PRINT_ER("Device couldn't be registered - %s\n", ndev->name);
+			return -1; /* ERROR */
+		}
+
+		nic->iftype = STATION_MODE;
+		nic->mac_opened = 0;
+
+	}
+
+	#ifndef WILC_SDIO
+	if (!linux_spi_init(&g_linux_wlan->wilc_spidev)) {
+		PRINT_ER("Can't initialize SPI \n");
+		return -1; /* ERROR */
+	}
+	g_linux_wlan->wilc_spidev = wilc_spi_dev;
+	#else
+	g_linux_wlan->wilc_sdio_func = local_sdio_func;
+	#endif
+
+	return 0;
+}
+
+
+/*The 1st function called after module inserted*/
+static int __init init_wilc_driver(void)
+{
+
+
+#if defined (WILC_DEBUGFS)
+	if (wilc_debugfs_init() < 0) {
+		PRINT_D(GENERIC_DBG, "fail to create debugfs for wilc driver\n");
+		return -1;
+	}
+#endif
+
+	printk("IN INIT FUNCTION\n");
+	printk("*** WILC1000 driver VERSION=[10.2] FW_VER=[10.2] ***\n");
+
+	linux_wlan_device_power(1);
+	msleep(100);
+	linux_wlan_device_detection(1);
+
+#ifdef WILC_SDIO
+	{
+		int ret;
+
+		ret = sdio_register_driver(&wilc_bus);
+		if (ret < 0) {
+			PRINT_D(INIT_DBG, "init_wilc_driver: Failed register sdio driver\n");
+		}
+
+		return ret;
+	}
+#else
+	PRINT_D(INIT_DBG, "Initializing netdev\n");
+	if (wilc_netdev_init()) {
+		PRINT_ER("Couldn't initialize netdev\n");
+	}
+	return 0;
+#endif
+}
+late_initcall(init_wilc_driver);
+
+static void __exit exit_wilc_driver(void)
+{
+	int i = 0;
+	perInterface_wlan_t *nic[NUM_CONCURRENT_IFC] = {NULL,};
+	#define CLOSE_TIMEOUT (12 * 1000)
+
+	if ((g_linux_wlan != NULL) && (((g_linux_wlan->strInterfaceInfo[0].wilc_netdev) != NULL)
+				       || ((g_linux_wlan->strInterfaceInfo[1].wilc_netdev) != NULL))) {
+	#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+		unregister_inetaddr_notifier(&g_dev_notifier);
+	#endif
+
+		for (i = 0; i < NUM_CONCURRENT_IFC; i++) {
+			nic[i] = netdev_priv(g_linux_wlan->strInterfaceInfo[i].wilc_netdev);
+		}
+	}
+
+
+	if ((g_linux_wlan != NULL) && g_linux_wlan->wilc_firmware != NULL) {
+		release_firmware(g_linux_wlan->wilc_firmware);
+		g_linux_wlan->wilc_firmware = NULL;
+	}
+
+
+	if ((g_linux_wlan != NULL) && (((g_linux_wlan->strInterfaceInfo[0].wilc_netdev) != NULL)
+				       || ((g_linux_wlan->strInterfaceInfo[1].wilc_netdev) != NULL))) {
+		PRINT_D(INIT_DBG, "Waiting for mac_close ....\n");
+
+		if (linux_wlan_lock_timeout(&close_exit_sync, CLOSE_TIMEOUT) < 0)
+			PRINT_D(INIT_DBG, "Closed TimedOUT\n");
+		else
+			PRINT_D(INIT_DBG, "mac_closed\n");
+
+
+		for (i = 0; i < NUM_CONCURRENT_IFC; i++) {
+			/* close all opened interfaces */
+			if (g_linux_wlan->strInterfaceInfo[i].wilc_netdev != NULL) {
+				if (nic[i]->mac_opened)	{
+					mac_close(g_linux_wlan->strInterfaceInfo[i].wilc_netdev);
+				}
+			}
+		}
+		for (i = 0; i < NUM_CONCURRENT_IFC; i++) {
+			PRINT_D(INIT_DBG, "Unregistering netdev %p \n", g_linux_wlan->strInterfaceInfo[i].wilc_netdev);
+			unregister_netdev(g_linux_wlan->strInterfaceInfo[i].wilc_netdev);
+			#ifdef USE_WIRELESS
+			PRINT_D(INIT_DBG, "Freeing Wiphy...\n");
+			WILC_WFI_WiphyFree(g_linux_wlan->strInterfaceInfo[i].wilc_netdev);
+			#endif
+			PRINT_D(INIT_DBG, "Freeing netdev...\n");
+			free_netdev(g_linux_wlan->strInterfaceInfo[i].wilc_netdev);
+		}
+	}
+
+
+#ifdef USE_WIRELESS
+#ifdef WILC_AP_EXTERNAL_MLME
+	/* Bug 4600 : WILC_WFI_deinit_mon_interface was already called at mac_close */
+	/* WILC_WFI_deinit_mon_interface(); */
+#endif
+#endif
+
+	/* if(g_linux_wlan->open_ifcs==0) */
+	{
+	#ifndef WILC_SDIO
+		PRINT_D(INIT_DBG, "SPI unregsiter...\n");
+		spi_unregister_driver(&wilc_bus);
+	#else
+		PRINT_D(INIT_DBG, "SDIO unregsiter...\n");
+		sdio_unregister_driver(&wilc_bus);
+	#endif
+
+		linux_wlan_deinit_lock(&close_exit_sync);
+		if (g_linux_wlan != NULL) {
+			WILC_FREE(g_linux_wlan);
+			g_linux_wlan = NULL;
+		}
+		printk("Module_exit Done.\n");
+
+#if defined (WILC_DEBUGFS)
+		wilc_debugfs_remove();
+#endif
+
+		linux_wlan_device_detection(0);
+		linux_wlan_device_power(0);
+	}
+}
+module_exit(exit_wilc_driver);
+
+MODULE_LICENSE("GPL");
+#endif
diff --git a/drivers/staging/wilc1000/linux_wlan_common.h b/drivers/staging/wilc1000/linux_wlan_common.h
new file mode 100644
index 000000000000..2476bfda1b46
--- /dev/null
+++ b/drivers/staging/wilc1000/linux_wlan_common.h
@@ -0,0 +1,182 @@
+#ifndef LINUX_WLAN_COMMON_H
+#define LINUX_WLAN_COMMON_H
+
+enum debug_region {
+	Generic_debug = 0,
+	Hostapd_debug,
+	Hostinf_debug,
+	CFG80211_debug,
+	Coreconfig_debug,
+	Interrupt_debug,
+	TX_debug,
+	RX_debug,
+	Lock_debug,
+	Tcp_enhance,
+	/*Added by amr - BugID_4720*/
+	Spin_debug,
+
+	Init_debug,
+	Bus_debug,
+	Mem_debug,
+	Firmware_debug,
+	COMP = 0xFFFFFFFF,
+};
+
+#define GENERIC_DBG             (1 << Generic_debug)
+#define HOSTAPD_DBG             (1 << Hostapd_debug)
+#define HOSTINF_DBG             (1 << Hostinf_debug)
+#define CORECONFIG_DBG          (1 << Coreconfig_debug)
+#define CFG80211_DBG            (1 << CFG80211_debug)
+#define INT_DBG                 (1 << Interrupt_debug)
+#define TX_DBG                  (1 << TX_debug)
+#define RX_DBG                  (1 << RX_debug)
+#define LOCK_DBG                (1 << Lock_debug)
+#define TCP_ENH                 (1 << Tcp_enhance)
+#define SPIN_DEBUG              (1 << Spin_debug)
+#define INIT_DBG                (1 << Init_debug)
+#define BUS_DBG                 (1 << Bus_debug)
+#define MEM_DBG                 (1 << Mem_debug)
+#define FIRM_DBG                (1 << Firmware_debug)
+
+#if defined (WILC_DEBUGFS)
+extern int wilc_debugfs_init(void);
+extern void wilc_debugfs_remove(void);
+
+extern atomic_t REGION;
+extern atomic_t DEBUG_LEVEL;
+
+#define DEBUG           (1 << 0)
+#define INFO            (1 << 1)
+#define WRN             (1 << 2)
+#define ERR             (1 << 3)
+
+#define PRINT_D(region, ...)						\
+	do {								\
+		if ((atomic_read(&DEBUG_LEVEL) & DEBUG) &&		\
+		   ((atomic_read(&REGION)) & (region))) {		\
+			printk("DBG [%s: %d]", __func__, __LINE__);	\
+			printk(__VA_ARGS__);				\
+		}							\
+	} while (0)
+
+#define PRINT_INFO(region, ...)						\
+	do {								\
+		if ((atomic_read(&DEBUG_LEVEL) & INFO) &&		\
+		   ((atomic_read(&REGION)) & (region))) {		\
+			printk("INFO [%s]", __func__);			\
+			printk(__VA_ARGS__);				\
+		}							\
+	} while (0)
+
+#define PRINT_WRN(region, ...)						\
+	do {								\
+		if ((atomic_read(&DEBUG_LEVEL) & WRN) &&		\
+		   ((atomic_read(&REGION)) & (region))) {		\
+			printk("WRN [%s: %d]", __func__, __LINE__);	\
+			printk(__VA_ARGS__);				\
+		}							\
+	} while (0)
+
+#define PRINT_ER(...)							\
+	do {								\
+		if ((atomic_read(&DEBUG_LEVEL) & ERR)) {		\
+			printk("ERR [%s: %d]", __func__, __LINE__);	\
+			printk(__VA_ARGS__);				\
+		}							\
+	} while (0)
+
+#else
+
+#define REGION  (INIT_DBG | GENERIC_DBG | CFG80211_DBG | FIRM_DBG | HOSTAPD_DBG)
+
+#define DEBUG       1
+#define INFO        0
+#define WRN         0
+
+#define PRINT_D(region, ...)						\
+	do {								\
+		if (DEBUG == 1 && ((REGION)&(region))) {		\
+			printk("DBG [%s: %d]", __func__, __LINE__);	\
+			printk(__VA_ARGS__);				\
+		}							\
+	} while (0)
+
+#define PRINT_INFO(region, ...)						\
+	do {								\
+		if (INFO == 1 && ((REGION)&(region))) {			\
+			printk("INFO [%s]", __func__);			\
+			printk(__VA_ARGS__);				\
+		}							\
+	} while (0)
+
+#define PRINT_WRN(region, ...)						\
+	do {								\
+		if (WRN == 1 && ((REGION)&(region))) {			\
+			printk("WRN [%s: %d]", __func__, __LINE__);	\
+			printk(__VA_ARGS__);				\
+		}							\
+	} while (0)
+
+#define PRINT_ER(...)							\
+	do {								\
+		printk("ERR [%s: %d]", __func__, __LINE__);		\
+		printk(__VA_ARGS__);					\
+	} while (0)
+#endif
+
+#define FN_IN   /* PRINT_D(">>> \n") */
+#define FN_OUT  /* PRINT_D("<<<\n") */
+
+#ifdef MEMORY_STATIC
+#define LINUX_RX_SIZE	(96 * 1024)
+#endif
+#define LINUX_TX_SIZE	(64 * 1024)
+
+
+#define WILC_MULTICAST_TABLE_SIZE	8
+
+#if defined (NM73131_0_BOARD)
+
+#define MODALIAS "wilc_spi"
+#define GPIO_NUM	IRQ_WILC1000_GPIO
+
+#elif defined (BEAGLE_BOARD)
+	#define SPI_CHANNEL	4
+
+	#if SPI_CHANNEL == 4
+		#define MODALIAS	"wilc_spi4"
+		#define GPIO_NUM	162
+	#else
+		#define MODALIAS	"wilc_spi3"
+		#define GPIO_NUM	133
+	#endif
+#elif defined(PANDA_BOARD)
+	#define MODALIAS	"WILC_SPI"
+	#define GPIO_NUM	139
+#elif defined(PLAT_WMS8304)             /* rachel */
+	#define MODALIAS	"wilc_spi"
+	#define GPIO_NUM	139
+#elif defined (PLAT_RKXXXX)
+ #define MODALIAS	"WILC_IRQ"
+ #define GPIO_NUM	RK30_PIN3_PD2 /* RK30_PIN3_PA1 */
+/* RK30_PIN3_PD2 */
+/* RK2928_PIN1_PA7 */
+
+#elif defined(CUSTOMER_PLATFORM)
+/*
+ TODO : specify MODALIAS name and GPIO number. This is certainly necessary for SPI interface.
+ *
+ * ex)
+ * #define MODALIAS  "WILC_SPI"
+ * #define GPIO_NUM  139
+ */
+
+#else
+/* base on SAMA5D3_Xplained Board */
+	#define MODALIAS	"WILC_SPI"
+	#define GPIO_NUM	0x44
+#endif
+
+
+void linux_wlan_enable_irq(void);
+#endif
diff --git a/drivers/staging/wilc1000/linux_wlan_sdio.c b/drivers/staging/wilc1000/linux_wlan_sdio.c
new file mode 100644
index 000000000000..858e3a191bce
--- /dev/null
+++ b/drivers/staging/wilc1000/linux_wlan_sdio.c
@@ -0,0 +1,249 @@
+#include "wilc_wfi_netdevice.h"
+
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio_ids.h>
+#include <linux/mmc/sdio.h>
+#include <linux/mmc/host.h>
+
+
+
+#if defined (NM73131_0_BOARD)
+#define SDIO_MODALIAS "wilc_sdio"
+#else
+#define SDIO_MODALIAS "wilc1000_sdio"
+#endif
+
+#if defined (NM73131_0_BOARD)
+ #define MAX_SPEED 50000000
+#elif defined(CUSTOMER_PLATFORM)
+/* TODO : User have to stable bus clock as user's environment. */
+ #ifdef MAX_BUS_SPEED
+ #define MAX_SPEED MAX_BUS_SPEED
+ #else
+ #define MAX_SPEED 50000000
+ #endif
+#else
+ #define MAX_SPEED (6 * 1000000) /* Max 50M */
+#endif
+
+
+struct sdio_func *local_sdio_func;
+extern linux_wlan_t *g_linux_wlan;
+extern int wilc_netdev_init(void);
+extern int sdio_clear_int(void);
+extern void wilc_handle_isr(void);
+
+static unsigned int sdio_default_speed;
+
+#define SDIO_VENDOR_ID_WILC 0x0296
+#define SDIO_DEVICE_ID_WILC 0x5347
+
+static const struct sdio_device_id wilc_sdio_ids[] = {
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_WILC, SDIO_DEVICE_ID_WILC) },
+};
+
+
+static void wilc_sdio_interrupt(struct sdio_func *func)
+{
+#ifndef WILC_SDIO_IRQ_GPIO
+	sdio_release_host(func);
+	wilc_handle_isr();
+	sdio_claim_host(func);
+#endif
+}
+
+
+int linux_sdio_cmd52(sdio_cmd52_t *cmd)
+{
+	struct sdio_func *func = g_linux_wlan->wilc_sdio_func;
+	int ret;
+	u8 data;
+
+	sdio_claim_host(func);
+
+	func->num = cmd->function;
+	if (cmd->read_write) {  /* write */
+		if (cmd->raw) {
+			sdio_writeb(func, cmd->data, cmd->address, &ret);
+			data = sdio_readb(func, cmd->address, &ret);
+			cmd->data = data;
+		} else {
+			sdio_writeb(func, cmd->data, cmd->address, &ret);
+		}
+	} else {        /* read */
+		data = sdio_readb(func, cmd->address, &ret);
+		cmd->data = data;
+	}
+
+	sdio_release_host(func);
+
+	if (ret < 0) {
+		PRINT_ER("wilc_sdio_cmd52..failed, err(%d)\n", ret);
+		return 0;
+	}
+	return 1;
+}
+
+
+int linux_sdio_cmd53(sdio_cmd53_t *cmd)
+{
+	struct sdio_func *func = g_linux_wlan->wilc_sdio_func;
+	int size, ret;
+
+	sdio_claim_host(func);
+
+	func->num = cmd->function;
+	func->cur_blksize = cmd->block_size;
+	if (cmd->block_mode)
+		size = cmd->count * cmd->block_size;
+	else
+		size = cmd->count;
+
+	if (cmd->read_write) {  /* write */
+		ret = sdio_memcpy_toio(func, cmd->address, (void *)cmd->buffer, size);
+	} else {        /* read */
+		ret = sdio_memcpy_fromio(func, (void *)cmd->buffer, cmd->address,  size);
+	}
+
+	sdio_release_host(func);
+
+
+	if (ret < 0) {
+		PRINT_ER("wilc_sdio_cmd53..failed, err(%d)\n", ret);
+		return 0;
+	}
+
+	return 1;
+}
+
+volatile int probe; /* COMPLEMENT_BOOT */
+static int linux_sdio_probe(struct sdio_func *func, const struct sdio_device_id *id)
+{
+	PRINT_D(INIT_DBG, "probe function\n");
+
+#ifdef COMPLEMENT_BOOT
+	if (local_sdio_func != NULL) {
+		local_sdio_func = func;
+		probe = 1;
+		PRINT_D(INIT_DBG, "local_sdio_func isn't NULL\n");
+		return 0;
+	}
+#endif
+	PRINT_D(INIT_DBG, "Initializing netdev\n");
+	local_sdio_func = func;
+	if (wilc_netdev_init()) {
+		PRINT_ER("Couldn't initialize netdev\n");
+		return -1;
+	}
+
+	printk("Driver Initializing success\n");
+	return 0;
+}
+
+static void linux_sdio_remove(struct sdio_func *func)
+{
+	/**
+	 *      TODO
+	 **/
+
+}
+
+struct sdio_driver wilc_bus = {
+	.name		= SDIO_MODALIAS,
+	.id_table	= wilc_sdio_ids,
+	.probe		= linux_sdio_probe,
+	.remove		= linux_sdio_remove,
+};
+
+int enable_sdio_interrupt(void)
+{
+	int ret = 0;
+#ifndef WILC_SDIO_IRQ_GPIO
+
+	sdio_claim_host(local_sdio_func);
+	ret = sdio_claim_irq(local_sdio_func, wilc_sdio_interrupt);
+	sdio_release_host(local_sdio_func);
+
+	if (ret < 0) {
+		PRINT_ER("can't claim sdio_irq, err(%d)\n", ret);
+		ret = -EIO;
+	}
+#endif
+	return ret;
+}
+
+void disable_sdio_interrupt(void)
+{
+
+#ifndef WILC_SDIO_IRQ_GPIO
+	int ret;
+
+	PRINT_D(INIT_DBG, "disable_sdio_interrupt IN\n");
+
+	sdio_claim_host(local_sdio_func);
+	ret = sdio_release_irq(local_sdio_func);
+	if (ret < 0) {
+		PRINT_ER("can't release sdio_irq, err(%d)\n", ret);
+	}
+	sdio_release_host(local_sdio_func);
+
+	PRINT_D(INIT_DBG, "disable_sdio_interrupt OUT\n");
+#endif
+}
+
+static int linux_sdio_set_speed(int speed)
+{
+	struct mmc_ios ios;
+	sdio_claim_host(local_sdio_func);
+
+	memcpy((void *)&ios, (void *)&local_sdio_func->card->host->ios, sizeof(struct mmc_ios));
+	local_sdio_func->card->host->ios.clock = speed;
+	ios.clock = speed;
+	local_sdio_func->card->host->ops->set_ios(local_sdio_func->card->host, &ios);
+	sdio_release_host(local_sdio_func);
+	PRINT_INFO(INIT_DBG, "@@@@@@@@@@@@ change SDIO speed to %d @@@@@@@@@\n", speed);
+
+	return 1;
+}
+
+static int linux_sdio_get_speed(void)
+{
+	return local_sdio_func->card->host->ios.clock;
+}
+
+int linux_sdio_init(void *pv)
+{
+
+	/**
+	 *      TODO :
+	 **/
+
+
+	sdio_default_speed = linux_sdio_get_speed();
+	return 1;
+}
+
+void linux_sdio_deinit(void *pv)
+{
+
+	/**
+	 *      TODO :
+	 **/
+
+
+	sdio_unregister_driver(&wilc_bus);
+}
+
+int linux_sdio_set_max_speed(void)
+{
+	return linux_sdio_set_speed(MAX_SPEED);
+}
+
+int linux_sdio_set_default_speed(void)
+{
+	return linux_sdio_set_speed(sdio_default_speed);
+}
+
+
+
diff --git a/drivers/staging/wilc1000/linux_wlan_sdio.h b/drivers/staging/wilc1000/linux_wlan_sdio.h
new file mode 100644
index 000000000000..4b515f5108e7
--- /dev/null
+++ b/drivers/staging/wilc1000/linux_wlan_sdio.h
@@ -0,0 +1,14 @@
+extern struct sdio_func *local_sdio_func;
+extern struct sdio_driver wilc_bus;
+
+#include <linux/mmc/sdio_func.h>
+
+int linux_sdio_init(void *);
+void linux_sdio_deinit(void *);
+int linux_sdio_cmd52(sdio_cmd52_t *cmd);
+int linux_sdio_cmd53(sdio_cmd53_t *cmd);
+int enable_sdio_interrupt(void);
+void disable_sdio_interrupt(void);
+int linux_sdio_set_max_speed(void);
+int linux_sdio_set_default_speed(void);
+
diff --git a/drivers/staging/wilc1000/linux_wlan_spi.c b/drivers/staging/wilc1000/linux_wlan_spi.c
new file mode 100644
index 000000000000..236669cfcd3a
--- /dev/null
+++ b/drivers/staging/wilc1000/linux_wlan_spi.c
@@ -0,0 +1,479 @@
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/cdev.h>
+#include <asm/uaccess.h>
+#include <linux/device.h>
+#include <linux/spi/spi.h>
+
+#include "linux_wlan_common.h"
+
+#define USE_SPI_DMA     0       /* johnny add */
+
+#ifdef WILC_ASIC_A0
+ #if defined(PLAT_PANDA_ES_OMAP4460)
+  #define MIN_SPEED 12000000
+  #define MAX_SPEED 24000000
+ #elif defined(PLAT_WMS8304)
+  #define MIN_SPEED 12000000
+  #define MAX_SPEED 24000000 /* 4000000 */
+ #elif defined(CUSTOMER_PLATFORM)
+/*
+  TODO : define Clock speed under 48M.
+ *
+ * ex)
+ * #define MIN_SPEED 24000000
+ * #define MAX_SPEED 48000000
+ */
+ #else
+  #define MIN_SPEED 24000000
+  #define MAX_SPEED 48000000
+ #endif
+#else /* WILC_ASIC_A0 */
+/* Limit clk to 6MHz on FPGA. */
+ #define MIN_SPEED 6000000
+ #define MAX_SPEED 6000000
+#endif /* WILC_ASIC_A0 */
+
+static uint32_t SPEED = MIN_SPEED;
+
+struct spi_device *wilc_spi_dev;
+void linux_spi_deinit(void *vp);
+
+static int __init wilc_bus_probe(struct spi_device *spi)
+{
+
+	PRINT_D(BUS_DBG, "spiModalias: %s\n", spi->modalias);
+	PRINT_D(BUS_DBG, "spiMax-Speed: %d\n", spi->max_speed_hz);
+	wilc_spi_dev = spi;
+
+	printk("Driver Initializing success\n");
+	return 0;
+}
+
+static int __exit wilc_bus_remove(struct spi_device *spi)
+{
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id wilc1000_of_match[] = {
+	{ .compatible = "atmel,wilc_spi", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, wilc1000_of_match);
+#endif
+
+struct spi_driver wilc_bus __refdata = {
+	.driver = {
+		.name = MODALIAS,
+#ifdef CONFIG_OF
+		.of_match_table = wilc1000_of_match,
+#endif
+	},
+	.probe =  wilc_bus_probe,
+	.remove = __exit_p(wilc_bus_remove),
+};
+
+
+void linux_spi_deinit(void *vp)
+{
+
+	spi_unregister_driver(&wilc_bus);
+
+	SPEED = MIN_SPEED;
+	PRINT_ER("@@@@@@@@@@@@ restore SPI speed to %d @@@@@@@@@\n", SPEED);
+
+}
+
+
+
+int linux_spi_init(void *vp)
+{
+	int ret = 1;
+	static int called;
+
+
+	if (called == 0) {
+		called++;
+		ret = spi_register_driver(&wilc_bus);
+	}
+
+	/* change return value to match WILC interface */
+	(ret < 0) ? (ret = 0) : (ret = 1);
+
+	return ret;
+}
+
+#if defined(PLAT_WMS8304)
+#define TXRX_PHASE_SIZE (4096)
+#endif
+
+#if defined (NM73131_0_BOARD)
+
+int linux_spi_write(uint8_t *b, uint32_t len)
+{
+
+	int ret;
+
+	if (len > 0 && b != NULL) {
+		struct spi_message msg;
+		PRINT_D(BUS_DBG, "Request writing %d bytes\n", len);
+		struct spi_transfer tr = {
+			.tx_buf = b,
+			.len = len,
+			.speed_hz = SPEED,
+			.delay_usecs = 0,
+		};
+
+		spi_message_init(&msg);
+		spi_message_add_tail(&tr, &msg);
+		ret = spi_sync(wilc_spi_dev, &msg);
+		if (ret < 0) {
+			PRINT_ER("SPI transaction failed\n");
+		}
+
+	} else {
+		PRINT_ER("can't write data with the following length: %d\n", len);
+		PRINT_ER("FAILED due to NULL buffer or ZERO length check the following length: %d\n", len);
+		ret = -1;
+	}
+
+	/* change return value to match WILC interface */
+	(ret < 0) ? (ret = 0) : (ret = 1);
+
+
+	return ret;
+}
+
+#elif defined(TXRX_PHASE_SIZE)
+
+int linux_spi_write(uint8_t *b, uint32_t len)
+{
+	int ret;
+	if (len > 0 && b != NULL) {
+		int i = 0;
+		int blk = len / TXRX_PHASE_SIZE;
+		int remainder = len % TXRX_PHASE_SIZE;
+
+		char *r_buffer = kzalloc(TXRX_PHASE_SIZE, GFP_KERNEL);
+		if (!r_buffer) {
+			PRINT_ER("Failed to allocate memory for r_buffer\n");
+		}
+
+		if (blk) {
+			while (i < blk)	{
+				struct spi_message msg;
+				struct spi_transfer tr = {
+					.tx_buf = b + (i * TXRX_PHASE_SIZE),
+					.len = TXRX_PHASE_SIZE,
+					.speed_hz = SPEED,
+					.bits_per_word = 8,
+					.delay_usecs = 0,
+				};
+
+				tr.rx_buf = r_buffer;
+
+				memset(&msg, 0, sizeof(msg));
+				spi_message_init(&msg);
+				msg.spi = wilc_spi_dev;
+				msg.is_dma_mapped = USE_SPI_DMA;
+
+				spi_message_add_tail(&tr, &msg);
+				ret = spi_sync(wilc_spi_dev, &msg);
+				if (ret < 0) {
+					PRINT_ER("SPI transaction failed\n");
+				}
+				i++;
+
+			}
+		}
+		if (remainder) {
+			struct spi_message msg;
+			struct spi_transfer tr = {
+				.tx_buf = b + (blk * TXRX_PHASE_SIZE),
+				.len = remainder,
+				.speed_hz = SPEED,
+				.bits_per_word = 8,
+				.delay_usecs = 0,
+			};
+			tr.rx_buf = r_buffer;
+
+			memset(&msg, 0, sizeof(msg));
+			spi_message_init(&msg);
+			msg.spi = wilc_spi_dev;
+			msg.is_dma_mapped = USE_SPI_DMA;                                /* rachel */
+
+			spi_message_add_tail(&tr, &msg);
+			ret = spi_sync(wilc_spi_dev, &msg);
+			if (ret < 0) {
+				PRINT_ER("SPI transaction failed\n");
+			}
+		}
+		kfree(r_buffer);
+	} else {
+		PRINT_ER("can't write data with the following length: %d\n", len);
+		PRINT_ER("FAILED due to NULL buffer or ZERO length check the following length: %d\n", len);
+		ret = -1;
+	}
+
+	/* change return value to match WILC interface */
+	(ret < 0) ? (ret = 0) : (ret = 1);
+
+	return ret;
+
+}
+
+#else
+int linux_spi_write(uint8_t *b, uint32_t len)
+{
+
+	int ret;
+	struct spi_message msg;
+
+	if (len > 0 && b != NULL) {
+		struct spi_transfer tr = {
+			.tx_buf = b,
+			.len = len,
+			.speed_hz = SPEED,
+			.delay_usecs = 0,
+		};
+		char *r_buffer = kzalloc(len, GFP_KERNEL);
+		if (!r_buffer) {
+			PRINT_ER("Failed to allocate memory for r_buffer\n");
+		}
+		tr.rx_buf = r_buffer;
+		PRINT_D(BUS_DBG, "Request writing %d bytes\n", len);
+
+		memset(&msg, 0, sizeof(msg));
+		spi_message_init(&msg);
+/* [[johnny add */
+		msg.spi = wilc_spi_dev;
+		msg.is_dma_mapped = USE_SPI_DMA;
+/* ]] */
+		spi_message_add_tail(&tr, &msg);
+
+		ret = spi_sync(wilc_spi_dev, &msg);
+		if (ret < 0) {
+			PRINT_ER("SPI transaction failed\n");
+		}
+
+		kfree(r_buffer);
+	} else {
+		PRINT_ER("can't write data with the following length: %d\n", len);
+		PRINT_ER("FAILED due to NULL buffer or ZERO length check the following length: %d\n", len);
+		ret = -1;
+	}
+
+	/* change return value to match WILC interface */
+	(ret < 0) ? (ret = 0) : (ret = 1);
+
+
+	return ret;
+}
+
+#endif
+
+#if defined (NM73131_0_BOARD)
+
+int linux_spi_read(unsigned char *rb, unsigned long rlen)
+{
+
+	int ret;
+
+	if (rlen > 0) {
+		struct spi_message msg;
+		struct spi_transfer tr = {
+			.rx_buf = rb,
+			.len = rlen,
+			.speed_hz = SPEED,
+			.delay_usecs = 0,
+
+		};
+
+		spi_message_init(&msg);
+		spi_message_add_tail(&tr, &msg);
+		ret = spi_sync(wilc_spi_dev, &msg);
+		if (ret < 0) {
+			PRINT_ER("SPI transaction failed\n");
+		}
+	} else {
+		PRINT_ER("can't read data with the following length: %ld\n", rlen);
+		ret = -1;
+	}
+	/* change return value to match WILC interface */
+	(ret < 0) ? (ret = 0) : (ret = 1);
+
+	return ret;
+}
+
+#elif defined(TXRX_PHASE_SIZE)
+
+int linux_spi_read(unsigned char *rb, unsigned long rlen)
+{
+	int ret;
+
+	if (rlen > 0) {
+		int i = 0;
+
+		int blk = rlen / TXRX_PHASE_SIZE;
+		int remainder = rlen % TXRX_PHASE_SIZE;
+
+		char *t_buffer = kzalloc(TXRX_PHASE_SIZE, GFP_KERNEL);
+		if (!t_buffer) {
+			PRINT_ER("Failed to allocate memory for t_buffer\n");
+		}
+
+		if (blk) {
+			while (i < blk)	{
+				struct spi_message msg;
+				struct spi_transfer tr = {
+					.rx_buf = rb + (i * TXRX_PHASE_SIZE),
+					.len = TXRX_PHASE_SIZE,
+					.speed_hz = SPEED,
+					.bits_per_word = 8,
+					.delay_usecs = 0,
+				};
+				tr.tx_buf = t_buffer;
+
+				memset(&msg, 0, sizeof(msg));
+				spi_message_init(&msg);
+				msg.spi = wilc_spi_dev;
+				msg.is_dma_mapped = USE_SPI_DMA;
+
+				spi_message_add_tail(&tr, &msg);
+				ret = spi_sync(wilc_spi_dev, &msg);
+				if (ret < 0) {
+					PRINT_ER("SPI transaction failed\n");
+				}
+				i++;
+			}
+		}
+		if (remainder) {
+			struct spi_message msg;
+			struct spi_transfer tr = {
+				.rx_buf = rb + (blk * TXRX_PHASE_SIZE),
+				.len = remainder,
+				.speed_hz = SPEED,
+				.bits_per_word = 8,
+				.delay_usecs = 0,
+			};
+			tr.tx_buf = t_buffer;
+
+			memset(&msg, 0, sizeof(msg));
+			spi_message_init(&msg);
+			msg.spi = wilc_spi_dev;
+			msg.is_dma_mapped = USE_SPI_DMA;                                /* rachel */
+
+			spi_message_add_tail(&tr, &msg);
+			ret = spi_sync(wilc_spi_dev, &msg);
+			if (ret < 0) {
+				PRINT_ER("SPI transaction failed\n");
+			}
+		}
+
+		kfree(t_buffer);
+	} else {
+		PRINT_ER("can't read data with the following length: %ld\n", rlen);
+		ret = -1;
+	}
+	/* change return value to match WILC interface */
+	(ret < 0) ? (ret = 0) : (ret = 1);
+
+	return ret;
+}
+
+#else
+int linux_spi_read(unsigned char *rb, unsigned long rlen)
+{
+
+	int ret;
+
+	if (rlen > 0) {
+		struct spi_message msg;
+		struct spi_transfer tr = {
+			.rx_buf = rb,
+			.len = rlen,
+			.speed_hz = SPEED,
+			.delay_usecs = 0,
+
+		};
+		char *t_buffer = kzalloc(rlen, GFP_KERNEL);
+		if (!t_buffer) {
+			PRINT_ER("Failed to allocate memory for t_buffer\n");
+		}
+		tr.tx_buf = t_buffer;
+
+		memset(&msg, 0, sizeof(msg));
+		spi_message_init(&msg);
+/* [[ johnny add */
+		msg.spi = wilc_spi_dev;
+		msg.is_dma_mapped = USE_SPI_DMA;
+/* ]] */
+		spi_message_add_tail(&tr, &msg);
+
+		ret = spi_sync(wilc_spi_dev, &msg);
+		if (ret < 0) {
+			PRINT_ER("SPI transaction failed\n");
+		}
+		kfree(t_buffer);
+	} else {
+		PRINT_ER("can't read data with the following length: %ld\n", rlen);
+		ret = -1;
+	}
+	/* change return value to match WILC interface */
+	(ret < 0) ? (ret = 0) : (ret = 1);
+
+	return ret;
+}
+
+#endif
+
+int linux_spi_write_read(unsigned char *wb, unsigned char *rb, unsigned int rlen)
+{
+
+	int ret;
+
+	if (rlen > 0) {
+		struct spi_message msg;
+		struct spi_transfer tr = {
+			.rx_buf = rb,
+			.tx_buf = wb,
+			.len = rlen,
+			.speed_hz = SPEED,
+			.bits_per_word = 8,
+			.delay_usecs = 0,
+
+		};
+
+		memset(&msg, 0, sizeof(msg));
+		spi_message_init(&msg);
+		msg.spi = wilc_spi_dev;
+		msg.is_dma_mapped = USE_SPI_DMA;
+
+		spi_message_add_tail(&tr, &msg);
+		ret = spi_sync(wilc_spi_dev, &msg);
+		if (ret < 0) {
+			PRINT_ER("SPI transaction failed\n");
+		}
+	} else {
+		PRINT_ER("can't read data with the following length: %d\n", rlen);
+		ret = -1;
+	}
+	/* change return value to match WILC interface */
+	(ret < 0) ? (ret = 0) : (ret = 1);
+
+	return ret;
+}
+
+int linux_spi_set_max_speed(void)
+{
+	SPEED = MAX_SPEED;
+
+	PRINT_INFO(BUS_DBG, "@@@@@@@@@@@@ change SPI speed to %d @@@@@@@@@\n", SPEED);
+	return 1;
+}
diff --git a/drivers/staging/wilc1000/linux_wlan_spi.h b/drivers/staging/wilc1000/linux_wlan_spi.h
new file mode 100644
index 000000000000..0ecad477de1c
--- /dev/null
+++ b/drivers/staging/wilc1000/linux_wlan_spi.h
@@ -0,0 +1,14 @@
+#ifndef LINUX_WLAN_SPI_H
+#define LINUX_WLAN_SPI_H
+
+#include <linux/spi/spi.h>
+extern struct spi_device *wilc_spi_dev;
+extern struct spi_driver wilc_bus;
+
+int linux_spi_init(void *vp);
+void linux_spi_deinit(void *vp);
+int linux_spi_write(uint8_t *b, uint32_t len);
+int linux_spi_read(uint8_t *rb, uint32_t rlen);
+int linux_spi_write_read(unsigned char *wb, unsigned char *rb, unsigned int rlen);
+int linux_spi_set_max_speed(void);
+#endif
diff --git a/drivers/staging/wilc1000/wilc_debugfs.c b/drivers/staging/wilc1000/wilc_debugfs.c
new file mode 100644
index 000000000000..c328208cda29
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_debugfs.c
@@ -0,0 +1,191 @@
+/*
+ * NewportMedia WiFi chipset driver test tools - wilc-debug
+ * Copyright (c) 2012 NewportMedia Inc.
+ * Author: SSW <sswd@wilcsemic.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.
+ *
+ */
+
+#if defined(WILC_DEBUGFS)
+#include <linux/module.h>
+#include <linux/debugfs.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+
+#include "wilc_wlan_if.h"
+
+
+static struct dentry *wilc_dir;
+
+/*
+ * --------------------------------------------------------------------------------
+ */
+
+#define DBG_REGION_ALL	(GENERIC_DBG | HOSTAPD_DBG | HOSTINF_DBG | CORECONFIG_DBG | CFG80211_DBG | INT_DBG | TX_DBG | RX_DBG | LOCK_DBG | INIT_DBG | BUS_DBG | MEM_DBG)
+#define DBG_LEVEL_ALL	(DEBUG | INFO | WRN | ERR)
+atomic_t REGION = ATOMIC_INIT(INIT_DBG | GENERIC_DBG | CFG80211_DBG | FIRM_DBG | HOSTAPD_DBG);
+atomic_t DEBUG_LEVEL = ATOMIC_INIT(ERR);
+
+/*
+ * --------------------------------------------------------------------------------
+ */
+
+
+static ssize_t wilc_debug_level_read(struct file *file, char __user *userbuf, size_t count, loff_t *ppos)
+{
+	char buf[128];
+	int res = 0;
+
+	/* only allow read from start */
+	if (*ppos > 0)
+		return 0;
+
+	res = scnprintf(buf, sizeof(buf), "Debug Level: %x\n", atomic_read(&DEBUG_LEVEL));
+
+	return simple_read_from_buffer(userbuf, count, ppos, buf, res);
+}
+
+static ssize_t wilc_debug_level_write(struct file *filp, const char *buf, size_t count, loff_t *ppos)
+{
+	char buffer[128] = {};
+	int flag = 0;
+
+	if (count > sizeof(buffer))
+		return -EINVAL;
+
+	if (copy_from_user(buffer, buf, count)) {
+		return -EFAULT;
+	}
+
+	flag = buffer[0] - '0';
+
+	if (flag > 0) {
+		flag = DEBUG | ERR;
+	} else if (flag < 0) {
+		flag = 100;
+	}
+
+	if (flag > DBG_LEVEL_ALL) {
+		printk("%s, value (0x%08x) is out of range, stay previous flag (0x%08x)\n", __func__, flag, atomic_read(&DEBUG_LEVEL));
+		return -EFAULT;
+	}
+
+	atomic_set(&DEBUG_LEVEL, (int)flag);
+
+	if (flag == 0) {
+		printk("Debug-level disabled\n");
+	} else {
+		printk("Debug-level enabled\n");
+	}
+	return count;
+}
+
+static ssize_t wilc_debug_region_read(struct file *file, char __user *userbuf, size_t count, loff_t *ppos)
+{
+	char buf[128];
+	int res = 0;
+
+	/* only allow read from start */
+	if (*ppos > 0)
+		return 0;
+
+	res = scnprintf(buf, sizeof(buf), "Debug region: %x\n", atomic_read(&REGION));
+
+	return simple_read_from_buffer(userbuf, count, ppos, buf, res);
+}
+
+static ssize_t wilc_debug_region_write(struct file *filp, const char *buf, size_t count, loff_t *ppos)
+{
+	char buffer[128] = {};
+	int flag;
+
+	if (count > sizeof(buffer))
+		return -EINVAL;
+
+	if (copy_from_user(buffer, buf, count)) {
+		return -EFAULT;
+	}
+
+	flag = buffer[0] - '0';
+
+	if (flag > DBG_REGION_ALL) {
+		printk("%s, value (0x%08x) is out of range, stay previous flag (0x%08x)\n", __func__, flag, atomic_read(&REGION));
+		return -EFAULT;
+	}
+
+	atomic_set(&REGION, (int)flag);
+	printk("new debug-region is %x\n", atomic_read(&REGION));
+
+	return count;
+}
+
+/*
+ * --------------------------------------------------------------------------------
+ */
+
+#define FOPS(_open, _read, _write, _poll) { \
+		.owner	= THIS_MODULE, \
+		.open	= (_open), \
+		.read	= (_read), \
+		.write	= (_write), \
+		.poll		= (_poll), \
+}
+
+struct wilc_debugfs_info_t {
+	const char *name;
+	int perm;
+	unsigned int data;
+	struct file_operations fops;
+};
+
+static struct wilc_debugfs_info_t debugfs_info[] = {
+	{ "wilc_debug_level",	0666,	(DEBUG | ERR), FOPS(NULL, wilc_debug_level_read, wilc_debug_level_write, NULL), },
+	{ "wilc_debug_region",	0666,	(INIT_DBG | GENERIC_DBG | CFG80211_DBG), FOPS(NULL, wilc_debug_region_read, wilc_debug_region_write, NULL), },
+};
+
+int wilc_debugfs_init(void)
+{
+	int i;
+
+	struct dentry *debugfs_files;
+	struct wilc_debugfs_info_t *info;
+
+	wilc_dir = debugfs_create_dir("wilc_wifi", NULL);
+	if (wilc_dir ==  ERR_PTR(-ENODEV)) {
+		/* it's not error. the debugfs is just not being enabled. */
+		printk("ERR, kernel has built without debugfs support\n");
+		return 0;
+	}
+
+	if (!wilc_dir) {
+		printk("ERR, debugfs create dir\n");
+		return -1;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(debugfs_info); i++) {
+		info = &debugfs_info[i];
+		debugfs_files = debugfs_create_file(info->name,
+						    info->perm,
+						    wilc_dir,
+						    &info->data,
+						    &info->fops);
+
+		if (!debugfs_files) {
+			printk("ERR fail to create the debugfs file, %s\n", info->name);
+			debugfs_remove_recursive(wilc_dir);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+void wilc_debugfs_remove(void)
+{
+	debugfs_remove_recursive(wilc_dir);
+}
+
+#endif
+
diff --git a/drivers/staging/wilc1000/wilc_errorsupport.h b/drivers/staging/wilc1000/wilc_errorsupport.h
new file mode 100644
index 000000000000..b9517dc7f661
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_errorsupport.h
@@ -0,0 +1,67 @@
+#ifndef __WILC_ERRORSUPPORT_H__
+#define __WILC_ERRORSUPPORT_H__
+
+/*!
+ *  @file		wilc_errorsupport.h
+ *  @brief		Error reporting and handling support
+ *  @author		syounan
+ *  @sa			wilc_oswrapper.h top level OS wrapper file
+ *  @date		10 Aug 2010
+ *  @version		1.0
+ */
+
+#include "linux_wlan_common.h"
+
+/* Psitive Numbers to indicate sucess with special status */
+#define WILC_ALREADY_EXSIT	(+100)    /** The requested object already exists */
+
+/* Generic success will return 0 */
+#define WILC_SUCCESS	0       /** Generic success */
+
+/* Negative numbers to indicate failures */
+#define WILC_FAIL			-100 /** Generic Fail */
+#define WILC_BUSY			-101 /** Busy with another operation*/
+#define WILC_INVALID_ARGUMENT		-102 /** A given argument is invalid*/
+#define WILC_INVALID_STATE		-103 /** An API request would violate the Driver state machine (i.e. to start PID while not camped)*/
+#define WILC_BUFFER_OVERFLOW		-104 /** In copy operations if the copied data is larger than the allocated buffer*/
+#define WILC_NULL_PTR			-105 /** null pointer is passed or used */
+#define WILC_EMPTY			-107
+#define WILC_FULL			-108
+#define WILC_TIMEOUT			-109
+#define WILC_CANCELED			-110 /** The required operation have been canceled by the user*/
+#define WILC_INVALID_FILE		-112 /** The Loaded file is corruped or having an invalid format */
+#define WILC_NOT_FOUND			-113 /** Cant find the file to load */
+#define WILC_NO_MEM			-114
+#define WILC_UNSUPPORTED_VERSION	-115
+#define WILC_FILE_EOF			-116
+
+
+/* Error type */
+typedef s32 WILC_ErrNo;
+
+#define WILC_IS_ERR(__status__) (__status__ < WILC_SUCCESS)
+
+#define WILC_ERRORCHECK(__status__) do { \
+		if (WILC_IS_ERR(__status__)) { \
+			PRINT_ER("PRINT_ER(%d)\n", __status__);	\
+			goto ERRORHANDLER; \
+		} \
+} while (0)
+
+#define WILC_ERRORREPORT(__status__, __err__) do { \
+		PRINT_ER("PRINT_ER(%d)\n", __err__); \
+		__status__ = __err__; \
+		goto ERRORHANDLER; \
+} while (0)
+
+#define  WILC_NULLCHECK(__status__, __ptr__)	do { \
+		if (__ptr__ == NULL) { \
+			WILC_ERRORREPORT(__status__, WILC_NULL_PTR); \
+		} \
+} while (0)
+
+#define WILC_CATCH(__status__) \
+ERRORHANDLER: \
+	if (WILC_IS_ERR(__status__)) \
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_exported_buf.c b/drivers/staging/wilc1000/wilc_exported_buf.c
new file mode 100644
index 000000000000..529457816f65
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_exported_buf.c
@@ -0,0 +1,76 @@
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+#define LINUX_RX_SIZE	(96 * 1024)
+#define LINUX_TX_SIZE	(64 * 1024)
+#define WILC1000_FW_SIZE (4 * 1024)
+
+#define DECLARE_WILC_BUFFER(name)	\
+	void *exported_ ## name = NULL;
+
+#define MALLOC_WILC_BUFFER(name, size)	\
+	exported_ ## name = kmalloc(size, GFP_KERNEL);	  \
+	if (!exported_ ## name) {   \
+		printk("fail to alloc: %s memory\n", exported_ ## name);  \
+		return -ENOBUFS;	\
+	}
+
+#define FREE_WILC_BUFFER(name)	\
+	kfree(exported_ ## name);
+
+/*
+ * Add necessary buffer pointers
+ */
+DECLARE_WILC_BUFFER(g_tx_buf)
+DECLARE_WILC_BUFFER(g_rx_buf)
+DECLARE_WILC_BUFFER(g_fw_buf)
+
+void *get_tx_buffer(void)
+{
+	return exported_g_tx_buf;
+}
+EXPORT_SYMBOL(get_tx_buffer);
+
+void *get_rx_buffer(void)
+{
+	return exported_g_rx_buf;
+}
+EXPORT_SYMBOL(get_rx_buffer);
+
+void *get_fw_buffer(void)
+{
+	return exported_g_fw_buf;
+}
+EXPORT_SYMBOL(get_fw_buffer);
+
+static int __init wilc_module_init(void)
+{
+	printk("wilc_module_init\n");
+	/*
+	 * alloc necessary memory
+	 */
+	MALLOC_WILC_BUFFER(g_tx_buf, LINUX_TX_SIZE)
+	MALLOC_WILC_BUFFER(g_rx_buf, LINUX_RX_SIZE)
+	MALLOC_WILC_BUFFER(g_fw_buf, WILC1000_FW_SIZE)
+
+	return 0;
+}
+
+static void __exit wilc_module_deinit(void)
+{
+	printk("wilc_module_deinit\n");
+	FREE_WILC_BUFFER(g_tx_buf)
+	FREE_WILC_BUFFER(g_rx_buf)
+	FREE_WILC_BUFFER(g_fw_buf)
+
+	return;
+}
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Tony Cho");
+MODULE_DESCRIPTION("WILC1xxx Memory Manager");
+pure_initcall(wilc_module_init);
+module_exit(wilc_module_deinit);
\ No newline at end of file
diff --git a/drivers/staging/wilc1000/wilc_log.h b/drivers/staging/wilc1000/wilc_log.h
new file mode 100644
index 000000000000..2269ebdec129
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_log.h
@@ -0,0 +1,47 @@
+#ifndef __WILC_LOG_H__
+#define __WILC_LOG_H__
+
+/* Errors will always get printed */
+#define WILC_ERROR(...) do {  WILC_PRINTF("(ERR)(%s:%d) ", __WILC_FUNCTION__, __WILC_LINE__); \
+			      WILC_PRINTF(__VA_ARGS__); \
+				} while (0)
+
+/* Wraning only printed if verbosity is 1 or more */
+#if (WILC_LOG_VERBOSITY_LEVEL > 0)
+#define WILC_WARN(...) do { WILC_PRINTF("(WRN)"); \
+			    WILC_PRINTF(__VA_ARGS__); \
+				} while (0)
+#else
+#define WILC_WARN(...) (0)
+#endif
+
+/* Info only printed if verbosity is 2 or more */
+#if (WILC_LOG_VERBOSITY_LEVEL > 1)
+#define WILC_INFO(...) do {  WILC_PRINTF("(INF)"); \
+			     WILC_PRINTF(__VA_ARGS__); \
+				} while (0)
+#else
+#define WILC_INFO(...) (0)
+#endif
+
+/* Debug is only printed if verbosity is 3 or more */
+#if (WILC_LOG_VERBOSITY_LEVEL > 2)
+#define WILC_DBG(...) do { WILC_PRINTF("(DBG)(%s:%d) ", __WILC_FUNCTION__, __WILC_LINE__); \
+			   WILC_PRINTF(__VA_ARGS__); \
+			} while (0)
+
+#else
+#define WILC_DBG(...) (0)
+#endif
+
+/* Function In/Out is only printed if verbosity is 4 or more */
+#if (WILC_LOG_VERBOSITY_LEVEL > 3)
+#define WILC_FN_IN do { WILC_PRINTF("(FIN) (%s:%d) \n", __WILC_FUNCTION__, __WILC_LINE__);  } while (0)
+#define WILC_FN_OUT(ret) do { WILC_PRINTF("(FOUT) (%s:%d) %d.\n", __WILC_FUNCTION__, __WILC_LINE__, (ret));  } while (0)
+#else
+#define WILC_FN_IN (0)
+#define WILC_FN_OUT(ret) (0)
+#endif
+
+
+#endif
\ No newline at end of file
diff --git a/drivers/staging/wilc1000/wilc_memory.c b/drivers/staging/wilc1000/wilc_memory.c
new file mode 100644
index 000000000000..c70707fefb66
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_memory.c
@@ -0,0 +1,58 @@
+
+#include "wilc_memory.h"
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+void *WILC_MemoryAlloc(u32 u32Size, tstrWILC_MemoryAttrs *strAttrs,
+		       char *pcFileName, u32 u32LineNo)
+{
+	if (u32Size > 0) {
+		return kmalloc(u32Size, GFP_ATOMIC);
+	} else {
+		return NULL;
+	}
+}
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+void *WILC_MemoryCalloc(u32 u32Size, tstrWILC_MemoryAttrs *strAttrs,
+			char *pcFileName, u32 u32LineNo)
+{
+	return kcalloc(u32Size, 1, GFP_KERNEL);
+}
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+void *WILC_MemoryRealloc(void *pvOldBlock, u32 u32NewSize,
+			 tstrWILC_MemoryAttrs *strAttrs, char *pcFileName, u32 u32LineNo)
+{
+	if (u32NewSize == 0) {
+		kfree(pvOldBlock);
+		return NULL;
+	} else if (pvOldBlock == NULL)	 {
+		return kmalloc(u32NewSize, GFP_KERNEL);
+	} else {
+		return krealloc(pvOldBlock, u32NewSize, GFP_KERNEL);
+	}
+
+}
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+void WILC_MemoryFree(const void *pvBlock, tstrWILC_MemoryAttrs *strAttrs,
+		     char *pcFileName, u32 u32LineNo)
+{
+	kfree(pvBlock);
+}
diff --git a/drivers/staging/wilc1000/wilc_memory.h b/drivers/staging/wilc1000/wilc_memory.h
new file mode 100644
index 000000000000..372d7053e873
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_memory.h
@@ -0,0 +1,239 @@
+#ifndef __WILC_MEMORY_H__
+#define __WILC_MEMORY_H__
+
+/*!
+ *  @file	wilc_memory.h
+ *  @brief	Memory OS wrapper functionality
+ *  @author	syounan
+ *  @sa		wilc_oswrapper.h top level OS wrapper file
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+
+#include <linux/types.h>
+#include <linux/slab.h>
+
+/*!
+ *  @struct             tstrWILC_MemoryAttrs
+ *  @brief		Memory API options
+ *  @author		syounan
+ *  @date		16 Aug 2010
+ *  @version		1.0
+ */
+typedef struct {
+} tstrWILC_MemoryAttrs;
+
+/*!
+ *  @brief	Allocates a given size of bytes
+ *  @param[in]	u32Size size of memory in bytes to be allocated
+ *  @param[in]	strAttrs Optional attributes, NULL for default
+ *              if not NULL, pAllocationPool should point to the pool to use for
+ *              this allocation. if NULL memory will be allocated directly from
+ *              the system
+ *  @param[in]	pcFileName file name of the calling code for debugging
+ *  @param[in]	u32LineNo line number of the calling code for debugging
+ *  @return	The new allocated block, NULL if allocation fails
+ *  @note	It is recommended to use of of the wrapper macros instead of
+ *              calling this function directly
+ *  @sa		sttrWILC_MemoryAttrs
+ *  @sa		WILC_MALLOC
+ *  @sa		WILC_MALLOC_EX
+ *  @sa		WILC_NEW
+ *  @sa		WILC_NEW_EX
+ *  @author	syounan
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+void *WILC_MemoryAlloc(u32 u32Size, tstrWILC_MemoryAttrs *strAttrs,
+		       char *pcFileName, u32 u32LineNo);
+
+/*!
+ *  @brief	Allocates a given size of bytes and zero filling it
+ *  @param[in]	u32Size size of memory in bytes to be allocated
+ *  @param[in]	strAttrs Optional attributes, NULL for default
+ *              if not NULL, pAllocationPool should point to the pool to use for
+ *              this allocation. if NULL memory will be allocated directly from
+ *              the system
+ *  @param[in]	pcFileName file name of the calling code for debugging
+ *  @param[in]	u32LineNo line number of the calling code for debugging
+ *  @return	The new allocated block, NULL if allocation fails
+ *  @note	It is recommended to use of of the wrapper macros instead of
+ *              calling this function directly
+ *  @sa		sttrWILC_MemoryAttrs
+ *  @sa		WILC_CALLOC
+ *  @sa		WILC_CALLOC_EX
+ *  @sa		WILC_NEW_0
+ *  @sa		WILC_NEW_0_EX
+ *  @author	syounan
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+void *WILC_MemoryCalloc(u32 u32Size, tstrWILC_MemoryAttrs *strAttrs,
+			char *pcFileName, u32 u32LineNo);
+
+/*!
+ *  @brief	Reallocates a given block to a new size
+ *  @param[in]	pvOldBlock the old memory block, if NULL then this function
+ *              behaves as a new allocation function
+ *  @param[in]	u32NewSize size of the new memory block in bytes, if zero then
+ *              this function behaves as a free function
+ *  @param[in]	strAttrs Optional attributes, NULL for default
+ *              if pAllocationPool!=NULL and pvOldBlock==NULL, pAllocationPool
+ *              should point to the pool to use for this allocation.
+ *              if pAllocationPool==NULL and pvOldBlock==NULL memory will be
+ *              allocated directly from	the system
+ *              if and pvOldBlock!=NULL, pAllocationPool will not be inspected
+ *              and reallocation is done from the same pool as the original block
+ *  @param[in]	pcFileName file name of the calling code for debugging
+ *  @param[in]	u32LineNo line number of the calling code for debugging
+ *  @return	The new allocated block, possibly same as pvOldBlock
+ *  @note	It is recommended to use of of the wrapper macros instead of
+ *              calling this function directly
+ *  @sa		sttrWILC_MemoryAttrs
+ *  @sa		WILC_REALLOC
+ *  @sa		WILC_REALLOC_EX
+ *  @author	syounan
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+void *WILC_MemoryRealloc(void *pvOldBlock, u32 u32NewSize,
+			 tstrWILC_MemoryAttrs *strAttrs, char *pcFileName, u32 u32LineNo);
+
+/*!
+ *  @brief	Frees given block
+ *  @param[in]	pvBlock the memory block to be freed
+ *  @param[in]	strAttrs Optional attributes, NULL for default
+ *  @param[in]	pcFileName file name of the calling code for debugging
+ *  @param[in]	u32LineNo line number of the calling code for debugging
+ *  @note	It is recommended to use of of the wrapper macros instead of
+ *              calling this function directly
+ *  @sa		sttrWILC_MemoryAttrs
+ *  @sa		WILC_FREE
+ *  @sa		WILC_FREE_EX
+ *  @sa		WILC_FREE_SET_NULL
+ *  @sa		WILC_FREE_IF_TRUE
+ *  @author	syounan
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+void WILC_MemoryFree(const void *pvBlock, tstrWILC_MemoryAttrs *strAttrs,
+			char *pcFileName, u32 u32LineNo);
+
+/*!
+ * @brief	standrad malloc wrapper with custom attributes
+ */
+	#define WILC_MALLOC_EX(__size__, __attrs__) \
+	(WILC_MemoryAlloc( \
+		 (__size__), __attrs__, NULL, 0))
+
+/*!
+ * @brief	standrad calloc wrapper with custom attributes
+ */
+	#define WILC_CALLOC_EX(__size__, __attrs__) \
+	(WILC_MemoryCalloc( \
+		 (__size__), __attrs__, NULL, 0))
+
+/*!
+ * @brief	standrad realloc wrapper with custom attributes
+ */
+	#define WILC_REALLOC_EX(__ptr__, __new_size__, __attrs__) \
+	(WILC_MemoryRealloc( \
+		 (__ptr__), (__new_size__), __attrs__, NULL, 0))
+/*!
+ * @brief	standrad free wrapper with custom attributes
+ */
+	#define WILC_FREE_EX(__ptr__, __attrs__) \
+	(WILC_MemoryFree( \
+		 (__ptr__), __attrs__, NULL, 0))
+
+/*!
+ * @brief	Allocates a block (with custom attributes) of given type and number of
+ * elements
+ */
+#define WILC_NEW_EX(__struct_type__, __n_structs__, __attrs__) \
+	((__struct_type__ *)WILC_MALLOC_EX( \
+		 sizeof(__struct_type__) * (u32)(__n_structs__), __attrs__))
+
+/*!
+ * @brief	Allocates a block (with custom attributes) of given type and number of
+ * elements and Zero-fills it
+ */
+#define WILC_NEW_0_EX(__struct_type__, __n_structs__, __attrs__) \
+	((__struct_type__ *)WILC_CALLOC_EX( \
+		 sizeof(__struct_type__) * (u32)(__n_structs__), __attrs__))
+
+/*!
+ * @brief	Frees a block (with custom attributes), also setting the original pointer
+ * to NULL
+ */
+#define WILC_FREE_SET_NULL_EX(__ptr__, __attrs__) do { \
+		if (__ptr__ != NULL) { \
+			WILC_FREE_EX(__ptr__, __attrs__); \
+			__ptr__ = NULL; \
+		} \
+} while (0)
+
+/*!
+ * @brief	Frees a block (with custom attributes) if the pointer expression evaluates
+ * to true
+ */
+#define WILC_FREE_IF_TRUE_EX(__ptr__, __attrs__) do { \
+		if (__ptr__ != NULL) { \
+			WILC_FREE_EX(__ptr__, __attrs__); \
+		} \
+} while (0)
+
+/*!
+ * @brief	standrad malloc wrapper with default attributes
+ */
+#define WILC_MALLOC(__size__) \
+	WILC_MALLOC_EX(__size__, NULL)
+
+/*!
+ * @brief	standrad calloc wrapper with default attributes
+ */
+#define WILC_CALLOC(__size__) \
+	WILC_CALLOC_EX(__size__, NULL)
+
+/*!
+ * @brief	standrad realloc wrapper with default attributes
+ */
+#define WILC_REALLOC(__ptr__, __new_size__) \
+	WILC_REALLOC_EX(__ptr__, __new_size__, NULL)
+
+/*!
+ * @brief	standrad free wrapper with default attributes
+ */
+#define WILC_FREE(__ptr__) \
+	WILC_FREE_EX(__ptr__, NULL)
+
+/*!
+ * @brief	Allocates a block (with default attributes) of given type and number of
+ * elements
+ */
+#define WILC_NEW(__struct_type__, __n_structs__) \
+	WILC_NEW_EX(__struct_type__, __n_structs__, NULL)
+
+/*!
+ * @brief	Allocates a block (with default attributes) of given type and number of
+ * elements and Zero-fills it
+ */
+#define WILC_NEW_0(__struct_type__, __n_structs__) \
+	WILC_NEW_O_EX(__struct_type__, __n_structs__, NULL)
+
+/*!
+ * @brief	Frees a block (with default attributes), also setting the original pointer
+ * to NULL
+ */
+#define WILC_FREE_SET_NULL(__ptr__) \
+	WILC_FREE_SET_NULL_EX(__ptr__, NULL)
+
+/*!
+ * @brief	Frees a block (with default attributes) if the pointer expression evaluates
+ * to true
+ */
+#define WILC_FREE_IF_TRUE(__ptr__) \
+	WILC_FREE_IF_TRUE_EX(__ptr__, NULL)
+
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_msgqueue.c b/drivers/staging/wilc1000/wilc_msgqueue.c
new file mode 100644
index 000000000000..16bcef4b5c00
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_msgqueue.c
@@ -0,0 +1,190 @@
+
+#include "wilc_msgqueue.h"
+#include <linux/spinlock.h>
+
+/*!
+ *  @author		syounan
+ *  @date		1 Sep 2010
+ *  @note		copied from FLO glue implementatuion
+ *  @version		1.0
+ */
+WILC_ErrNo WILC_MsgQueueCreate(WILC_MsgQueueHandle *pHandle,
+			       tstrWILC_MsgQueueAttrs *pstrAttrs)
+{
+	spin_lock_init(&pHandle->strCriticalSection);
+	sema_init(&pHandle->hSem, 0);
+	pHandle->pstrMessageList = NULL;
+	pHandle->u32ReceiversCount = 0;
+	pHandle->bExiting = false;
+	return WILC_SUCCESS;
+}
+
+/*!
+ *  @author		syounan
+ *  @date		1 Sep 2010
+ *  @note		copied from FLO glue implementatuion
+ *  @version		1.0
+ */
+WILC_ErrNo WILC_MsgQueueDestroy(WILC_MsgQueueHandle *pHandle,
+				tstrWILC_MsgQueueAttrs *pstrAttrs)
+{
+
+	pHandle->bExiting = true;
+
+	/* Release any waiting receiver thread. */
+	while (pHandle->u32ReceiversCount > 0) {
+		up(&(pHandle->hSem));
+		pHandle->u32ReceiversCount--;
+	}
+
+	while (pHandle->pstrMessageList != NULL) {
+		Message *pstrMessge = pHandle->pstrMessageList->pstrNext;
+		WILC_FREE(pHandle->pstrMessageList);
+		pHandle->pstrMessageList = pstrMessge;
+	}
+
+	return WILC_SUCCESS;
+}
+
+/*!
+ *  @author		syounan
+ *  @date		1 Sep 2010
+ *  @note		copied from FLO glue implementatuion
+ *  @version		1.0
+ */
+WILC_ErrNo WILC_MsgQueueSend(WILC_MsgQueueHandle *pHandle,
+			     const void *pvSendBuffer, u32 u32SendBufferSize,
+			     tstrWILC_MsgQueueAttrs *pstrAttrs)
+{
+	WILC_ErrNo s32RetStatus = WILC_SUCCESS;
+	unsigned long flags;
+	Message *pstrMessage = NULL;
+
+	if ((pHandle == NULL) || (u32SendBufferSize == 0) || (pvSendBuffer == NULL)) {
+		WILC_ERRORREPORT(s32RetStatus, WILC_INVALID_ARGUMENT);
+	}
+
+	if (pHandle->bExiting == true) {
+		WILC_ERRORREPORT(s32RetStatus, WILC_FAIL);
+	}
+
+	spin_lock_irqsave(&pHandle->strCriticalSection, flags);
+
+	/* construct a new message */
+	pstrMessage = WILC_NEW(Message, 1);
+	WILC_NULLCHECK(s32RetStatus, pstrMessage);
+	pstrMessage->u32Length = u32SendBufferSize;
+	pstrMessage->pstrNext = NULL;
+	pstrMessage->pvBuffer = WILC_MALLOC(u32SendBufferSize);
+	WILC_NULLCHECK(s32RetStatus, pstrMessage->pvBuffer);
+	WILC_memcpy(pstrMessage->pvBuffer, pvSendBuffer, u32SendBufferSize);
+
+
+	/* add it to the message queue */
+	if (pHandle->pstrMessageList == NULL) {
+		pHandle->pstrMessageList  = pstrMessage;
+	} else {
+		Message *pstrTailMsg = pHandle->pstrMessageList;
+		while (pstrTailMsg->pstrNext != NULL) {
+			pstrTailMsg = pstrTailMsg->pstrNext;
+		}
+		pstrTailMsg->pstrNext = pstrMessage;
+	}
+
+	spin_unlock_irqrestore(&pHandle->strCriticalSection, flags);
+
+	up(&pHandle->hSem);
+
+	WILC_CATCH(s32RetStatus)
+	{
+		/* error occured, free any allocations */
+		if (pstrMessage != NULL) {
+			if (pstrMessage->pvBuffer != NULL) {
+				WILC_FREE(pstrMessage->pvBuffer);
+			}
+			WILC_FREE(pstrMessage);
+		}
+	}
+
+	return s32RetStatus;
+}
+
+
+
+/*!
+ *  @author		syounan
+ *  @date		1 Sep 2010
+ *  @note		copied from FLO glue implementatuion
+ *  @version		1.0
+ */
+WILC_ErrNo WILC_MsgQueueRecv(WILC_MsgQueueHandle *pHandle,
+			     void *pvRecvBuffer, u32 u32RecvBufferSize,
+			     u32 *pu32ReceivedLength,
+			     tstrWILC_MsgQueueAttrs *pstrAttrs)
+{
+
+	Message *pstrMessage;
+	WILC_ErrNo s32RetStatus = WILC_SUCCESS;
+	unsigned long flags;
+	if ((pHandle == NULL) || (u32RecvBufferSize == 0)
+	    || (pvRecvBuffer == NULL) || (pu32ReceivedLength == NULL)) {
+		WILC_ERRORREPORT(s32RetStatus, WILC_INVALID_ARGUMENT);
+	}
+
+	if (pHandle->bExiting == true) {
+		WILC_ERRORREPORT(s32RetStatus, WILC_FAIL);
+	}
+
+	spin_lock_irqsave(&pHandle->strCriticalSection, flags);
+	pHandle->u32ReceiversCount++;
+	spin_unlock_irqrestore(&pHandle->strCriticalSection, flags);
+
+	down(&(pHandle->hSem));
+
+	if (s32RetStatus == WILC_TIMEOUT) {
+		/* timed out, just exit without consumeing the message */
+		spin_lock_irqsave(&pHandle->strCriticalSection, flags);
+		pHandle->u32ReceiversCount--;
+		spin_unlock_irqrestore(&pHandle->strCriticalSection, flags);
+	} else {
+		/* other non-timeout scenarios */
+		WILC_ERRORCHECK(s32RetStatus);
+
+		if (pHandle->bExiting) {
+			WILC_ERRORREPORT(s32RetStatus, WILC_FAIL);
+		}
+
+		spin_lock_irqsave(&pHandle->strCriticalSection, flags);
+
+		pstrMessage = pHandle->pstrMessageList;
+		if (pstrMessage == NULL) {
+			spin_unlock_irqrestore(&pHandle->strCriticalSection, flags);
+			WILC_ERRORREPORT(s32RetStatus, WILC_FAIL);
+		}
+		/* check buffer size */
+		if (u32RecvBufferSize < pstrMessage->u32Length)	{
+			spin_unlock_irqrestore(&pHandle->strCriticalSection, flags);
+			up(&pHandle->hSem);
+			WILC_ERRORREPORT(s32RetStatus, WILC_BUFFER_OVERFLOW);
+		}
+
+		/* consume the message */
+		pHandle->u32ReceiversCount--;
+		WILC_memcpy(pvRecvBuffer, pstrMessage->pvBuffer, pstrMessage->u32Length);
+		*pu32ReceivedLength = pstrMessage->u32Length;
+
+		pHandle->pstrMessageList = pstrMessage->pstrNext;
+
+		WILC_FREE(pstrMessage->pvBuffer);
+		WILC_FREE(pstrMessage);
+
+		spin_unlock_irqrestore(&pHandle->strCriticalSection, flags);
+
+	}
+
+	WILC_CATCH(s32RetStatus)
+	{
+	}
+
+	return s32RetStatus;
+}
diff --git a/drivers/staging/wilc1000/wilc_msgqueue.h b/drivers/staging/wilc1000/wilc_msgqueue.h
new file mode 100644
index 000000000000..35b10019eebd
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_msgqueue.h
@@ -0,0 +1,108 @@
+#ifndef __WILC_MSG_QUEUE_H__
+#define __WILC_MSG_QUEUE_H__
+
+/*!
+ *  @file	wilc_msgqueue.h
+ *  @brief	Message Queue OS wrapper functionality
+ *  @author	syounan
+ *  @sa		wilc_oswrapper.h top level OS wrapper file
+ *  @date	30 Aug 2010
+ *  @version	1.0
+ */
+
+#include "wilc_platform.h"
+#include "wilc_errorsupport.h"
+#include "wilc_memory.h"
+#include "wilc_strutils.h"
+
+/*!
+ *  @struct             tstrWILC_MsgQueueAttrs
+ *  @brief		Message Queue API options
+ *  @author		syounan
+ *  @date		30 Aug 2010
+ *  @version		1.0
+ */
+typedef struct {
+	/* a dummy member to avoid compiler errors*/
+	u8 dummy;
+
+} tstrWILC_MsgQueueAttrs;
+
+/*!
+ *  @brief		Creates a new Message queue
+ *  @details		Creates a new Message queue, if the feature
+ *                              CONFIG_WILC_MSG_QUEUE_IPC_NAME is enabled and pstrAttrs->pcName
+ *                              is not Null, then this message queue can be used for IPC with
+ *                              any other message queue having the same name in the system
+ *  @param[in,out]	pHandle handle to the message queue object
+ *  @param[in]	pstrAttrs Optional attributes, NULL for default
+ *  @return		Error code indicating sucess/failure
+ *  @sa			tstrWILC_MsgQueueAttrs
+ *  @author		syounan
+ *  @date		30 Aug 2010
+ *  @version		1.0
+ */
+WILC_ErrNo WILC_MsgQueueCreate(WILC_MsgQueueHandle *pHandle,
+			       tstrWILC_MsgQueueAttrs *pstrAttrs);
+
+
+/*!
+ *  @brief		Sends a message
+ *  @details		Sends a message, this API will block unil the message is
+ *                              actually sent or until it is timedout (as long as the feature
+ *                              CONFIG_WILC_MSG_QUEUE_TIMEOUT is enabled and pstrAttrs->u32Timeout
+ *                              is not set to WILC_OS_INFINITY), zero timeout is a valid value
+ *  @param[in]	pHandle handle to the message queue object
+ *  @param[in]	pvSendBuffer pointer to the data to send
+ *  @param[in]	u32SendBufferSize the size of the data to send
+ *  @param[in]	pstrAttrs Optional attributes, NULL for default
+ *  @return		Error code indicating sucess/failure
+ *  @sa			tstrWILC_MsgQueueAttrs
+ *  @author		syounan
+ *  @date		30 Aug 2010
+ *  @version		1.0
+ */
+WILC_ErrNo WILC_MsgQueueSend(WILC_MsgQueueHandle *pHandle,
+			     const void *pvSendBuffer, u32 u32SendBufferSize,
+			     tstrWILC_MsgQueueAttrs *pstrAttrs);
+
+
+/*!
+ *  @brief		Receives a message
+ *  @details		Receives a message, this API will block unil a message is
+ *                              received or until it is timedout (as long as the feature
+ *                              CONFIG_WILC_MSG_QUEUE_TIMEOUT is enabled and pstrAttrs->u32Timeout
+ *                              is not set to WILC_OS_INFINITY), zero timeout is a valid value
+ *  @param[in]	pHandle handle to the message queue object
+ *  @param[out]	pvRecvBuffer pointer to a buffer to fill with the received message
+ *  @param[in]	u32RecvBufferSize the size of the receive buffer
+ *  @param[out]	pu32ReceivedLength the length of received data
+ *  @param[in]	pstrAttrs Optional attributes, NULL for default
+ *  @return		Error code indicating sucess/failure
+ *  @sa			tstrWILC_MsgQueueAttrs
+ *  @author		syounan
+ *  @date		30 Aug 2010
+ *  @version		1.0
+ */
+WILC_ErrNo WILC_MsgQueueRecv(WILC_MsgQueueHandle *pHandle,
+			     void *pvRecvBuffer, u32 u32RecvBufferSize,
+			     u32 *pu32ReceivedLength,
+			     tstrWILC_MsgQueueAttrs *pstrAttrs);
+
+
+/*!
+ *  @brief		Destroys an existing  Message queue
+ *  @param[in]	pHandle handle to the message queue object
+ *  @param[in]	pstrAttrs Optional attributes, NULL for default
+ *  @return		Error code indicating sucess/failure
+ *  @sa			tstrWILC_MsgQueueAttrs
+ *  @author		syounan
+ *  @date		30 Aug 2010
+ *  @version		1.0
+ */
+WILC_ErrNo WILC_MsgQueueDestroy(WILC_MsgQueueHandle *pHandle,
+				tstrWILC_MsgQueueAttrs *pstrAttrs);
+
+
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_osconfig.h b/drivers/staging/wilc1000/wilc_osconfig.h
new file mode 100644
index 000000000000..f9c25140393e
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_osconfig.h
@@ -0,0 +1,9 @@
+/* Logs options */
+#define WILC_LOGS_NOTHING          0
+#define WILC_LOGS_WARN             1
+#define WILC_LOGS_WARN_INFO        2
+#define WILC_LOGS_WARN_INFO_DBG    3
+#define WILC_LOGS_WARN_INFO_DBG_FN 4
+#define WILC_LOGS_ALL              5
+
+#define WILC_LOG_VERBOSITY_LEVEL WILC_LOGS_ALL
diff --git a/drivers/staging/wilc1000/wilc_oswrapper.h b/drivers/staging/wilc1000/wilc_oswrapper.h
new file mode 100644
index 000000000000..e97aa96006e0
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_oswrapper.h
@@ -0,0 +1,41 @@
+#ifndef __WILC_OSWRAPPER_H__
+#define __WILC_OSWRAPPER_H__
+
+/*!
+ *  @file	wilc_oswrapper.h
+ *  @brief	Top level OS Wrapper, include this file and it will include all
+ *              other files as necessary
+ *  @author	syounan
+ *  @date	10 Aug 2010
+ *  @version	1.0
+ */
+
+/* OS Wrapper interface version */
+#define WILC_OSW_INTERFACE_VER 2
+
+/* Os Configuration File */
+#include "wilc_osconfig.h"
+#include "wilc_platform.h"
+
+/* Logging Functions */
+#include "wilc_log.h"
+
+/* Error reporting and handling support */
+#include "wilc_errorsupport.h"
+
+/* Sleep support */
+#include "wilc_sleep.h"
+
+/* Timer support */
+#include "wilc_timer.h"
+
+/* Memory support */
+#include "wilc_memory.h"
+
+/* String Utilities */
+#include "wilc_strutils.h"
+
+/* Message Queue */
+#include "wilc_msgqueue.h"
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_platform.h b/drivers/staging/wilc1000/wilc_platform.h
new file mode 100644
index 000000000000..d03532cc3af2
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_platform.h
@@ -0,0 +1,52 @@
+#ifndef __WILC_platfrom_H__
+#define __WILC_platfrom_H__
+
+#include <linux/kthread.h>
+#include <linux/semaphore.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/types.h>
+#include <linux/stat.h>
+#include <linux/time.h>
+#include <linux/version.h>
+#include "linux/string.h"
+/******************************************************************
+ *      OS specific types
+ *******************************************************************/
+
+typedef struct timer_list WILC_TimerHandle;
+
+
+
+/* Message Queue type is a structure */
+typedef struct __Message_struct {
+	void *pvBuffer;
+	u32 u32Length;
+	struct __Message_struct *pstrNext;
+} Message;
+
+typedef struct __MessageQueue_struct {
+	struct semaphore hSem;
+	spinlock_t strCriticalSection;
+	bool bExiting;
+	u32 u32ReceiversCount;
+	Message *pstrMessageList;
+} WILC_MsgQueueHandle;
+
+
+
+/*Time represented in 64 bit format*/
+typedef time_t WILC_Time;
+
+
+/*******************************************************************
+ *      others
+ ********************************************************************/
+
+/* Generic printf function */
+#define __WILC_FILE__		__FILE__
+#define __WILC_FUNCTION__	__func__
+#define __WILC_LINE__		__LINE__
+#endif
diff --git a/drivers/staging/wilc1000/wilc_sdio.c b/drivers/staging/wilc1000/wilc_sdio.c
new file mode 100644
index 000000000000..897e47e317ff
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_sdio.c
@@ -0,0 +1,1254 @@
+/* ////////////////////////////////////////////////////////////////////////// */
+/*  */
+/* Copyright (c) Atmel Corporation.  All rights reserved. */
+/*  */
+/* Module Name:  wilc_sdio.c */
+/*  */
+/*  */
+/* //////////////////////////////////////////////////////////////////////////// */
+
+#include "wilc_wlan_if.h"
+#include "wilc_wlan.h"
+
+
+#ifdef WILC1000_SINGLE_TRANSFER
+#define WILC_SDIO_BLOCK_SIZE 256
+#else
+ #if defined(PLAT_AML8726_M3) /* johnny */
+	#define WILC_SDIO_BLOCK_SIZE 512
+	#define MAX_SEG_SIZE (1 << 12) /* 4096 */
+ #else
+	#define WILC_SDIO_BLOCK_SIZE 512
+ #endif
+#endif
+
+typedef struct {
+	void *os_context;
+	wilc_wlan_os_func_t os_func;
+	uint32_t block_size;
+	int (*sdio_cmd52)(sdio_cmd52_t *);
+	int (*sdio_cmd53)(sdio_cmd53_t *);
+	int (*sdio_set_max_speed)(void);
+	int (*sdio_set_default_speed)(void);
+	wilc_debug_func dPrint;
+	int nint;
+#define MAX_NUN_INT_THRPT_ENH2 (5) /* Max num interrupts allowed in registers 0xf7, 0xf8 */
+	int has_thrpt_enh3;
+} wilc_sdio_t;
+
+static wilc_sdio_t g_sdio;
+
+#ifdef WILC_SDIO_IRQ_GPIO
+static int sdio_write_reg(uint32_t addr, uint32_t data);
+static int sdio_read_reg(uint32_t addr, uint32_t *data);
+#endif
+extern unsigned int int_clrd;
+
+/********************************************
+ *
+ *      Function 0
+ *
+ ********************************************/
+
+static int sdio_set_func0_csa_address(uint32_t adr)
+{
+	sdio_cmd52_t cmd;
+
+	/**
+	 *      Review: BIG ENDIAN
+	 **/
+	cmd.read_write = 1;
+	cmd.function = 0;
+	cmd.raw = 0;
+	cmd.address = 0x10c;
+	cmd.data = (uint8_t)adr;
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10c data...\n");
+		goto _fail_;
+	}
+
+	cmd.address = 0x10d;
+	cmd.data = (uint8_t)(adr >> 8);
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10d data...\n");
+		goto _fail_;
+	}
+
+	cmd.address = 0x10e;
+	cmd.data = (uint8_t)(adr >> 16);
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10e data...\n");
+		goto _fail_;
+	}
+
+	return 1;
+_fail_:
+	return 0;
+}
+
+static int sdio_set_func0_csa_address_byte0(uint32_t adr)
+{
+	sdio_cmd52_t cmd;
+
+
+	/**
+	 *      Review: BIG ENDIAN
+	 **/
+	cmd.read_write = 1;
+	cmd.function = 0;
+	cmd.raw = 0;
+	cmd.address = 0x10c;
+	cmd.data = (uint8_t)adr;
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10c data...\n");
+		goto _fail_;
+	}
+
+	return 1;
+_fail_:
+	return 0;
+}
+static int sdio_set_func0_block_size(uint32_t block_size)
+{
+	sdio_cmd52_t cmd;
+
+	cmd.read_write = 1;
+	cmd.function = 0;
+	cmd.raw = 0;
+	cmd.address = 0x10;
+	cmd.data = (uint8_t)block_size;
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10 data...\n");
+		goto _fail_;
+	}
+
+	cmd.address = 0x11;
+	cmd.data = (uint8_t)(block_size >> 8);
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x11 data...\n");
+		goto _fail_;
+	}
+
+	return 1;
+_fail_:
+	return 0;
+}
+
+/********************************************
+ *
+ *      Function 1
+ *
+ ********************************************/
+
+static int sdio_set_func1_block_size(uint32_t block_size)
+{
+	sdio_cmd52_t cmd;
+
+	cmd.read_write = 1;
+	cmd.function = 0;
+	cmd.raw = 0;
+	cmd.address = 0x110;
+	cmd.data = (uint8_t)block_size;
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x110 data...\n");
+		goto _fail_;
+	}
+	cmd.address = 0x111;
+	cmd.data = (uint8_t)(block_size >> 8);
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x111 data...\n");
+		goto _fail_;
+	}
+
+	return 1;
+_fail_:
+	return 0;
+}
+
+static int sdio_clear_int(void)
+{
+#ifndef WILC_SDIO_IRQ_GPIO
+	/* uint32_t sts; */
+	sdio_cmd52_t cmd;
+	cmd.read_write = 0;
+	cmd.function = 1;
+	cmd.raw = 0;
+	cmd.address = 0x4;
+	cmd.data = 0;
+	g_sdio.sdio_cmd52(&cmd);
+	int_clrd++;
+
+	return cmd.data;
+#else
+	uint32_t reg;
+	if (!sdio_read_reg(WILC_HOST_RX_CTRL_0, &reg)) {
+		g_sdio.dPrint(N_ERR, "[wilc spi]: Failed read reg (%08x)...\n", WILC_HOST_RX_CTRL_0);
+		return 0;
+	}
+	reg &= ~0x1;
+	sdio_write_reg(WILC_HOST_RX_CTRL_0, reg);
+	int_clrd++;
+	return 1;
+#endif
+
+}
+
+uint32_t sdio_xfer_cnt(void)
+{
+	uint32_t cnt = 0;
+	sdio_cmd52_t cmd;
+	cmd.read_write = 0;
+	cmd.function = 1;
+	cmd.raw = 0;
+	cmd.address = 0x1C;
+	cmd.data = 0;
+	g_sdio.sdio_cmd52(&cmd);
+	cnt = cmd.data;
+
+	cmd.read_write = 0;
+	cmd.function = 1;
+	cmd.raw = 0;
+	cmd.address = 0x1D;
+	cmd.data = 0;
+	g_sdio.sdio_cmd52(&cmd);
+	cnt |= (cmd.data << 8);
+
+	cmd.read_write = 0;
+	cmd.function = 1;
+	cmd.raw = 0;
+	cmd.address = 0x1E;
+	cmd.data = 0;
+	g_sdio.sdio_cmd52(&cmd);
+	cnt |= (cmd.data << 16);
+
+	return cnt;
+
+
+}
+
+/********************************************
+ *
+ *      Sdio interfaces
+ *
+ ********************************************/
+int sdio_check_bs(void)
+{
+	sdio_cmd52_t cmd;
+
+	/**
+	 *      poll until BS is 0
+	 **/
+	cmd.read_write = 0;
+	cmd.function = 0;
+	cmd.raw = 0;
+	cmd.address = 0xc;
+	cmd.data = 0;
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, get BS register...\n");
+		goto _fail_;
+	}
+
+	return 1;
+
+_fail_:
+
+	return 0;
+}
+
+static int sdio_write_reg(uint32_t addr, uint32_t data)
+{
+#ifdef BIG_ENDIAN
+	data = BYTE_SWAP(data);
+#endif
+
+	if ((addr >= 0xf0) && (addr <= 0xff)) {
+		sdio_cmd52_t cmd;
+		cmd.read_write = 1;
+		cmd.function = 0;
+		cmd.raw = 0;
+		cmd.address = addr;
+		cmd.data = data;
+		if (!g_sdio.sdio_cmd52(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd 52, read reg (%08x) ...\n", addr);
+			goto _fail_;
+		}
+	} else {
+		sdio_cmd53_t cmd;
+
+		/**
+		 *      set the AHB address
+		 **/
+		if (!sdio_set_func0_csa_address(addr))
+			goto _fail_;
+
+		cmd.read_write = 1;
+		cmd.function = 0;
+		cmd.address = 0x10f;
+		cmd.block_mode = 0;
+		cmd.increment = 1;
+		cmd.count = 4;
+		cmd.buffer = (uint8_t *)&data;
+		cmd.block_size = g_sdio.block_size; /* johnny : prevent it from setting unexpected value */
+
+		if (!g_sdio.sdio_cmd53(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53, write reg (%08x)...\n", addr);
+			goto _fail_;
+		}
+	}
+
+	return 1;
+
+_fail_:
+
+	return 0;
+}
+
+static int sdio_write(uint32_t addr, uint8_t *buf, uint32_t size)
+{
+	uint32_t block_size = g_sdio.block_size;
+	sdio_cmd53_t cmd;
+	int nblk, nleft;
+
+	cmd.read_write = 1;
+	if (addr > 0) {
+		/**
+		 *      has to be word aligned...
+		 **/
+		if (size & 0x3) {
+			size += 4;
+			size &= ~0x3;
+		}
+
+		/**
+		 *      func 0 access
+		 **/
+		cmd.function = 0;
+		cmd.address = 0x10f;
+	} else {
+#ifdef WILC1000_SINGLE_TRANSFER
+		/**
+		 *      has to be block aligned...
+		 **/
+		nleft = size % block_size;
+		if (nleft > 0) {
+			size += block_size;
+			size &= ~(block_size - 1);
+		}
+#else
+		/**
+		 *      has to be word aligned...
+		 **/
+		if (size & 0x3) {
+			size += 4;
+			size &= ~0x3;
+		}
+#endif
+
+		/**
+		 *      func 1 access
+		 **/
+		cmd.function = 1;
+		cmd.address = 0;
+	}
+
+	nblk = size / block_size;
+	nleft = size % block_size;
+
+	if (nblk > 0) {
+
+#if defined(PLAT_AML8726_M3_BACKUP) /* johnny */
+		int i;
+
+		for (i = 0; i < nblk; i++) {
+			cmd.block_mode = 0; /* 1; */
+			cmd.increment = 1;
+			cmd.count = block_size; /* nblk; */
+			cmd.buffer = buf;
+			cmd.block_size = block_size;
+			if (addr > 0) {
+				if (!sdio_set_func0_csa_address(addr))
+					goto _fail_;
+			}
+			if (!g_sdio.sdio_cmd53(&cmd)) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block send...\n", addr);
+				goto _fail_;
+			}
+
+			if (addr > 0)
+				addr += block_size;     /* addr += nblk*block_size; */
+
+			buf += block_size;              /* buf += nblk*block_size; */
+		}
+
+#elif defined(PLAT_AML8726_M3) /* johnny */
+
+		int i;
+		int rest;
+		int seg_cnt;
+
+		seg_cnt = (nblk * block_size) / MAX_SEG_SIZE;
+		rest = (nblk * block_size) & (MAX_SEG_SIZE - 1);
+
+		for (i = 0; i < seg_cnt; i++) {
+			cmd.block_mode = 1;
+			cmd.increment = 1;
+			cmd.count = MAX_SEG_SIZE / block_size;
+			cmd.buffer = buf;
+			cmd.block_size = block_size;
+
+			if (addr > 0) {
+				if (!sdio_set_func0_csa_address(addr))
+					goto _fail_;
+			}
+			if (!g_sdio.sdio_cmd53(&cmd)) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block send...\n", addr);
+				goto _fail_;
+			}
+
+			if (addr > 0)
+				addr += MAX_SEG_SIZE;
+
+			buf += MAX_SEG_SIZE;
+
+		}
+
+
+		if (rest > 0) {
+			cmd.block_mode = 1;
+			cmd.increment = 1;
+			cmd.count = rest / block_size;
+			cmd.buffer = buf;
+			cmd.block_size = block_size; /* johnny : prevent it from setting unexpected value */
+
+			if (addr > 0) {
+				if (!sdio_set_func0_csa_address(addr))
+					goto _fail_;
+			}
+			if (!g_sdio.sdio_cmd53(&cmd)) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], bytes send...\n", addr);
+				goto _fail_;
+			}
+
+			if (addr > 0)
+				addr += rest;
+
+			buf += rest;
+
+		}
+
+#else
+
+		cmd.block_mode = 1;
+		cmd.increment = 1;
+		cmd.count = nblk;
+		cmd.buffer = buf;
+		cmd.block_size = block_size;
+		if (addr > 0) {
+			if (!sdio_set_func0_csa_address(addr))
+				goto _fail_;
+		}
+		if (!g_sdio.sdio_cmd53(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block send...\n", addr);
+			goto _fail_;
+		}
+		if (addr > 0)
+			addr += nblk * block_size;
+		buf += nblk * block_size;
+
+#endif /* platform */
+	}
+
+
+	if (nleft > 0) {
+		cmd.block_mode = 0;
+		cmd.increment = 1;
+		cmd.count = nleft;
+		cmd.buffer = buf;
+
+		cmd.block_size = block_size; /* johnny : prevent it from setting unexpected value */
+
+		if (addr > 0) {
+			if (!sdio_set_func0_csa_address(addr))
+				goto _fail_;
+		}
+		if (!g_sdio.sdio_cmd53(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], bytes send...\n", addr);
+			goto _fail_;
+		}
+	}
+
+	return 1;
+
+_fail_:
+
+	return 0;
+}
+
+static int sdio_read_reg(uint32_t addr, uint32_t *data)
+{
+	if ((addr >= 0xf0) && (addr <= 0xff)) {
+		sdio_cmd52_t cmd;
+		cmd.read_write = 0;
+		cmd.function = 0;
+		cmd.raw = 0;
+		cmd.address = addr;
+		if (!g_sdio.sdio_cmd52(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd 52, read reg (%08x) ...\n", addr);
+			goto _fail_;
+		}
+		*data = cmd.data;
+	} else {
+		sdio_cmd53_t cmd;
+
+		if (!sdio_set_func0_csa_address(addr))
+			goto _fail_;
+
+		cmd.read_write = 0;
+		cmd.function = 0;
+		cmd.address = 0x10f;
+		cmd.block_mode = 0;
+		cmd.increment = 1;
+		cmd.count = 4;
+		cmd.buffer = (uint8_t *)data;
+
+		cmd.block_size = g_sdio.block_size; /* johnny : prevent it from setting unexpected value */
+
+		if (!g_sdio.sdio_cmd53(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53, read reg (%08x)...\n", addr);
+			goto _fail_;
+		}
+	}
+
+#ifdef BIG_ENDIAN
+	*data = BYTE_SWAP(*data);
+#endif
+
+	return 1;
+
+_fail_:
+
+	return 0;
+}
+
+static int sdio_read(uint32_t addr, uint8_t *buf, uint32_t size)
+{
+	uint32_t block_size = g_sdio.block_size;
+	sdio_cmd53_t cmd;
+	int nblk, nleft;
+
+	cmd.read_write = 0;
+	if (addr > 0) {
+		/**
+		 *      has to be word aligned...
+		 **/
+		if (size & 0x3) {
+			size += 4;
+			size &= ~0x3;
+		}
+
+		/**
+		 *      func 0 access
+		 **/
+		cmd.function = 0;
+		cmd.address = 0x10f;
+	} else {
+#ifdef WILC1000_SINGLE_TRANSFER
+		/**
+		 *      has to be block aligned...
+		 **/
+		nleft = size % block_size;
+		if (nleft > 0) {
+			size += block_size;
+			size &= ~(block_size - 1);
+		}
+#else
+		/**
+		 *      has to be word aligned...
+		 **/
+		if (size & 0x3) {
+			size += 4;
+			size &= ~0x3;
+		}
+#endif
+
+		/**
+		 *      func 1 access
+		 **/
+		cmd.function = 1;
+		cmd.address = 0;
+	}
+
+	nblk = size / block_size;
+	nleft = size % block_size;
+
+	if (nblk > 0) {
+
+#if defined(PLAT_AML8726_M3_BACKUP) /* johnny */
+
+		int i;
+
+		for (i = 0; i < nblk; i++) {
+			cmd.block_mode = 0; /* 1; */
+			cmd.increment = 1;
+			cmd.count = block_size; /* nblk; */
+			cmd.buffer = buf;
+			cmd.block_size = block_size;
+			if (addr > 0) {
+				if (!sdio_set_func0_csa_address(addr))
+					goto _fail_;
+			}
+			if (!g_sdio.sdio_cmd53(&cmd)) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block read...\n", addr);
+				goto _fail_;
+			}
+			if (addr > 0)
+				addr += block_size;             /* addr += nblk*block_size; */
+			buf += block_size;              /* buf += nblk*block_size; */
+		}
+
+#elif defined(PLAT_AML8726_M3) /* johnny */
+
+		int i;
+		int rest;
+		int seg_cnt;
+
+		seg_cnt = (nblk * block_size) / MAX_SEG_SIZE;
+		rest = (nblk * block_size) & (MAX_SEG_SIZE - 1);
+
+		for (i = 0; i < seg_cnt; i++) {
+			cmd.block_mode = 1;
+			cmd.increment = 1;
+			cmd.count = MAX_SEG_SIZE / block_size;
+			cmd.buffer = buf;
+			cmd.block_size = block_size;
+
+
+			if (addr > 0) {
+				if (!sdio_set_func0_csa_address(addr))
+					goto _fail_;
+			}
+			if (!g_sdio.sdio_cmd53(&cmd)) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block read...\n", addr);
+				goto _fail_;
+			}
+
+			if (addr > 0)
+				addr += MAX_SEG_SIZE;
+
+			buf += MAX_SEG_SIZE;
+
+		}
+
+
+		if (rest > 0) {
+			cmd.block_mode = 1;
+			cmd.increment = 1;
+			cmd.count = rest / block_size;
+			cmd.buffer = buf;
+			cmd.block_size = block_size; /* johnny : prevent it from setting unexpected value */
+
+			if (addr > 0) {
+				if (!sdio_set_func0_csa_address(addr))
+					goto _fail_;
+			}
+			if (!g_sdio.sdio_cmd53(&cmd)) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block read...\n", addr);
+				goto _fail_;
+			}
+
+			if (addr > 0)
+				addr += rest;
+
+			buf += rest;
+
+		}
+
+#else
+
+		cmd.block_mode = 1;
+		cmd.increment = 1;
+		cmd.count = nblk;
+		cmd.buffer = buf;
+		cmd.block_size = block_size;
+		if (addr > 0) {
+			if (!sdio_set_func0_csa_address(addr))
+				goto _fail_;
+		}
+		if (!g_sdio.sdio_cmd53(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block read...\n", addr);
+			goto _fail_;
+		}
+		if (addr > 0)
+			addr += nblk * block_size;
+		buf += nblk * block_size;
+
+#endif /* platform */
+	}       /* if (nblk > 0) */
+
+	if (nleft > 0) {
+		cmd.block_mode = 0;
+		cmd.increment = 1;
+		cmd.count = nleft;
+		cmd.buffer = buf;
+
+		cmd.block_size = block_size; /* johnny : prevent it from setting unexpected value */
+
+		if (addr > 0) {
+			if (!sdio_set_func0_csa_address(addr))
+				goto _fail_;
+		}
+		if (!g_sdio.sdio_cmd53(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], bytes read...\n", addr);
+			goto _fail_;
+		}
+	}
+
+	return 1;
+
+_fail_:
+
+	return 0;
+}
+
+/********************************************
+ *
+ *      Bus interfaces
+ *
+ ********************************************/
+
+static int sdio_deinit(void *pv)
+{
+	return 1;
+}
+
+static int sdio_sync(void)
+{
+	uint32_t reg;
+
+	/**
+	 *      Disable power sequencer
+	 **/
+	if (!sdio_read_reg(WILC_MISC, &reg)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read misc reg...\n");
+		return 0;
+	}
+
+	reg &= ~(1 << 8);
+	if (!sdio_write_reg(WILC_MISC, reg)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write misc reg...\n");
+		return 0;
+	}
+
+#ifdef WILC_SDIO_IRQ_GPIO
+	{
+		uint32_t reg;
+		int ret;
+
+		/**
+		 *      interrupt pin mux select
+		 **/
+		ret = sdio_read_reg(WILC_PIN_MUX_0, &reg);
+		if (!ret) {
+			g_sdio.dPrint(N_ERR, "[wilc spi]: Failed read reg (%08x)...\n", WILC_PIN_MUX_0);
+			return 0;
+		}
+		reg |= (1 << 8);
+		ret = sdio_write_reg(WILC_PIN_MUX_0, reg);
+		if (!ret) {
+			g_sdio.dPrint(N_ERR, "[wilc spi]: Failed write reg (%08x)...\n", WILC_PIN_MUX_0);
+			return 0;
+		}
+
+		/**
+		 *      interrupt enable
+		 **/
+		ret = sdio_read_reg(WILC_INTR_ENABLE, &reg);
+		if (!ret) {
+			g_sdio.dPrint(N_ERR, "[wilc spi]: Failed read reg (%08x)...\n", WILC_INTR_ENABLE);
+			return 0;
+		}
+		reg |= (1 << 16);
+		ret = sdio_write_reg(WILC_INTR_ENABLE, reg);
+		if (!ret) {
+			g_sdio.dPrint(N_ERR, "[wilc spi]: Failed write reg (%08x)...\n", WILC_INTR_ENABLE);
+			return 0;
+		}
+	}
+#endif
+
+	return 1;
+}
+
+static int sdio_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
+{
+	sdio_cmd52_t cmd;
+	int loop;
+	uint32_t chipid;
+	memset(&g_sdio, 0, sizeof(wilc_sdio_t));
+
+	g_sdio.dPrint = func;
+	g_sdio.os_context = inp->os_context.os_private;
+	memcpy((void *)&g_sdio.os_func, (void *)&inp->os_func, sizeof(wilc_wlan_os_func_t));
+
+	if (inp->io_func.io_init) {
+		if (!inp->io_func.io_init(g_sdio.os_context)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed io init bus...\n");
+			return 0;
+		}
+	} else {
+		return 0;
+	}
+
+	g_sdio.sdio_cmd52	= inp->io_func.u.sdio.sdio_cmd52;
+	g_sdio.sdio_cmd53	= inp->io_func.u.sdio.sdio_cmd53;
+	g_sdio.sdio_set_max_speed	= inp->io_func.u.sdio.sdio_set_max_speed;
+	g_sdio.sdio_set_default_speed	= inp->io_func.u.sdio.sdio_set_default_speed;
+
+	/**
+	 *      function 0 csa enable
+	 **/
+	cmd.read_write = 1;
+	cmd.function = 0;
+	cmd.raw = 1;
+	cmd.address = 0x100;
+	cmd.data = 0x80;
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, enable csa...\n");
+		goto _fail_;
+	}
+
+	/**
+	 *      function 0 block size
+	 **/
+	if (!sdio_set_func0_block_size(WILC_SDIO_BLOCK_SIZE)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, set func 0 block size...\n");
+		goto _fail_;
+	}
+	g_sdio.block_size = WILC_SDIO_BLOCK_SIZE;
+
+	/**
+	 *      enable func1 IO
+	 **/
+	cmd.read_write = 1;
+	cmd.function = 0;
+	cmd.raw = 1;
+	cmd.address = 0x2;
+	cmd.data = 0x2;
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio] Fail cmd 52, set IOE register...\n");
+		goto _fail_;
+	}
+
+	/**
+	 *      make sure func 1 is up
+	 **/
+	cmd.read_write = 0;
+	cmd.function = 0;
+	cmd.raw = 0;
+	cmd.address = 0x3;
+	loop = 3;
+	do {
+		cmd.data = 0;
+		if (!g_sdio.sdio_cmd52(&cmd)) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, get IOR register...\n");
+			goto _fail_;
+		}
+		if (cmd.data == 0x2)
+			break;
+	} while (loop--);
+
+	if (loop <= 0) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail func 1 is not ready...\n");
+		goto _fail_;
+	}
+
+	/**
+	 *      func 1 is ready, set func 1 block size
+	 **/
+	if (!sdio_set_func1_block_size(WILC_SDIO_BLOCK_SIZE)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail set func 1 block size...\n");
+		goto _fail_;
+	}
+
+	/**
+	 *      func 1 interrupt enable
+	 **/
+	cmd.read_write = 1;
+	cmd.function = 0;
+	cmd.raw = 1;
+	cmd.address = 0x4;
+	cmd.data = 0x3;
+	if (!g_sdio.sdio_cmd52(&cmd)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, set IEN register...\n");
+		goto _fail_;
+	}
+
+	/**
+	 *      make sure can read back chip id correctly
+	 **/
+	if (!sdio_read_reg(0x1000, &chipid)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd read chip id...\n");
+		goto _fail_;
+	}
+	g_sdio.dPrint(N_ERR, "[wilc sdio]: chipid (%08x)\n", chipid);
+	if ((chipid & 0xfff) > 0x2a0) {
+		g_sdio.has_thrpt_enh3 = 1;
+	} else {
+		g_sdio.has_thrpt_enh3 = 0;
+	}
+	g_sdio.dPrint(N_ERR, "[wilc sdio]: has_thrpt_enh3 = %d...\n", g_sdio.has_thrpt_enh3);
+
+
+	return 1;
+
+_fail_:
+
+	return 0;
+}
+
+static void sdio_set_max_speed(void)
+{
+	g_sdio.sdio_set_max_speed();
+}
+
+static void sdio_set_default_speed(void)
+{
+	g_sdio.sdio_set_default_speed();
+}
+
+static int sdio_read_size(uint32_t *size)
+{
+
+	uint32_t tmp;
+	sdio_cmd52_t cmd;
+
+	/**
+	 *      Read DMA count in words
+	 **/
+	{
+		cmd.read_write = 0;
+		cmd.function = 0;
+		cmd.raw = 0;
+		cmd.address = 0xf2;
+		cmd.data = 0;
+		g_sdio.sdio_cmd52(&cmd);
+		tmp = cmd.data;
+
+		/* cmd.read_write = 0; */
+		/* cmd.function = 0; */
+		/* cmd.raw = 0; */
+		cmd.address = 0xf3;
+		cmd.data = 0;
+		g_sdio.sdio_cmd52(&cmd);
+		tmp |= (cmd.data << 8);
+	}
+
+	*size = tmp;
+	return 1;
+}
+
+static int sdio_read_int(uint32_t *int_status)
+{
+
+	uint32_t tmp;
+	sdio_cmd52_t cmd;
+
+	sdio_read_size(&tmp);
+
+	/**
+	 *      Read IRQ flags
+	 **/
+#ifndef WILC_SDIO_IRQ_GPIO
+	/* cmd.read_write = 0; */
+	cmd.function = 1;
+	/* cmd.raw = 0; */
+	cmd.address = 0x04;
+	cmd.data = 0;
+	g_sdio.sdio_cmd52(&cmd);
+
+	if (cmd.data & (1 << 0)) {
+		tmp |= INT_0;
+	}
+	if (cmd.data & (1 << 2)) {
+		tmp |= INT_1;
+	}
+	if (cmd.data & (1 << 3)) {
+		tmp |= INT_2;
+	}
+	if (cmd.data & (1 << 4)) {
+		tmp |= INT_3;
+	}
+	if (cmd.data & (1 << 5)) {
+		tmp |= INT_4;
+	}
+	if (cmd.data & (1 << 6)) {
+		tmp |= INT_5;
+	}
+	{
+		int i;
+		for (i = g_sdio.nint; i < MAX_NUM_INT; i++) {
+			if ((tmp >> (IRG_FLAGS_OFFSET + i)) & 0x1) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Unexpected interrupt (1) : tmp=%x, data=%x\n", tmp, cmd.data);
+				break;
+			}
+		}
+	}
+#else
+	{
+		uint32_t irq_flags;
+
+		cmd.read_write = 0;
+		cmd.function = 0;
+		cmd.raw = 0;
+		cmd.address = 0xf7;
+		cmd.data = 0;
+		g_sdio.sdio_cmd52(&cmd);
+		irq_flags = cmd.data & 0x1f;
+		tmp |= ((irq_flags >> 0) << IRG_FLAGS_OFFSET);
+	}
+
+#endif
+
+	*int_status = tmp;
+
+	return 1;
+}
+
+static int sdio_clear_int_ext(uint32_t val)
+{
+	int ret;
+
+	if (g_sdio.has_thrpt_enh3) {
+		uint32_t reg;
+
+#ifdef WILC_SDIO_IRQ_GPIO
+		{
+			uint32_t flags;
+			flags = val & ((1 << MAX_NUN_INT_THRPT_ENH2) - 1);
+			reg = flags;
+		}
+#else
+		reg = 0;
+#endif
+		/* select VMM table 0 */
+		if ((val & SEL_VMM_TBL0) == SEL_VMM_TBL0)
+			reg |= (1 << 5);
+		/* select VMM table 1 */
+		if ((val & SEL_VMM_TBL1) == SEL_VMM_TBL1)
+			reg |= (1 << 6);
+		/* enable VMM */
+		if ((val & EN_VMM) == EN_VMM)
+			reg |= (1 << 7);
+		if (reg) {
+			sdio_cmd52_t cmd;
+			cmd.read_write = 1;
+			cmd.function = 0;
+			cmd.raw = 0;
+			cmd.address = 0xf8;
+			cmd.data = reg;
+
+			ret = g_sdio.sdio_cmd52(&cmd);
+			if (!ret) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0xf8 data (%d) ...\n", __LINE__);
+				goto _fail_;
+			}
+
+		}
+	} else {
+#ifdef WILC_SDIO_IRQ_GPIO
+		{
+			/* see below. has_thrpt_enh2 uses register 0xf8 to clear interrupts. */
+			/* Cannot clear multiple interrupts. Must clear each interrupt individually */
+			uint32_t flags;
+			flags = val & ((1 << MAX_NUM_INT) - 1);
+			if (flags) {
+				int i;
+
+				ret = 1;
+				for (i = 0; i < g_sdio.nint; i++) {
+					if (flags & 1) {
+						sdio_cmd52_t cmd;
+						cmd.read_write = 1;
+						cmd.function = 0;
+						cmd.raw = 0;
+						cmd.address = 0xf8;
+						cmd.data = (1 << i);
+
+						ret = g_sdio.sdio_cmd52(&cmd);
+						if (!ret) {
+							g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0xf8 data (%d) ...\n", __LINE__);
+							goto _fail_;
+						}
+
+					}
+					if (!ret)
+						break;
+					flags >>= 1;
+				}
+				if (!ret) {
+					goto _fail_;
+				}
+				for (i = g_sdio.nint; i < MAX_NUM_INT; i++) {
+					if (flags & 1)
+						g_sdio.dPrint(N_ERR, "[wilc sdio]: Unexpected interrupt cleared %d...\n", i);
+					flags >>= 1;
+				}
+			}
+		}
+#endif /* WILC_SDIO_IRQ_GPIO */
+
+
+		{
+			uint32_t vmm_ctl;
+
+			vmm_ctl = 0;
+			/* select VMM table 0 */
+			if ((val & SEL_VMM_TBL0) == SEL_VMM_TBL0)
+				vmm_ctl |= (1 << 0);
+			/* select VMM table 1 */
+			if ((val & SEL_VMM_TBL1) == SEL_VMM_TBL1)
+				vmm_ctl |= (1 << 1);
+			/* enable VMM */
+			if ((val & EN_VMM) == EN_VMM)
+				vmm_ctl |= (1 << 2);
+
+			if (vmm_ctl) {
+				sdio_cmd52_t cmd;
+
+				cmd.read_write = 1;
+				cmd.function = 0;
+				cmd.raw = 0;
+				cmd.address = 0xf6;
+				cmd.data = vmm_ctl;
+				ret = g_sdio.sdio_cmd52(&cmd);
+				if (!ret) {
+					g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0xf6 data (%d) ...\n", __LINE__);
+					goto _fail_;
+				}
+			}
+		}
+	}
+
+	return 1;
+_fail_:
+	return 0;
+}
+
+static int sdio_sync_ext(int nint /*  how mant interrupts to enable. */)
+{
+	uint32_t reg;
+
+
+	if (nint > MAX_NUM_INT) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Too many interupts (%d)...\n", nint);
+		return 0;
+	}
+	if (nint > MAX_NUN_INT_THRPT_ENH2) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Error: Cannot support more than 5 interrupts when has_thrpt_enh2=1.\n");
+		return 0;
+	}
+
+
+	g_sdio.nint = nint;
+
+	/**
+	 *      Disable power sequencer
+	 **/
+	if (!sdio_read_reg(WILC_MISC, &reg)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read misc reg...\n");
+		return 0;
+	}
+
+	reg &= ~(1 << 8);
+	if (!sdio_write_reg(WILC_MISC, reg)) {
+		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write misc reg...\n");
+		return 0;
+	}
+
+#ifdef WILC_SDIO_IRQ_GPIO
+	{
+		uint32_t reg;
+		int ret, i;
+
+
+		/**
+		 *      interrupt pin mux select
+		 **/
+		ret = sdio_read_reg(WILC_PIN_MUX_0, &reg);
+		if (!ret) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read reg (%08x)...\n", WILC_PIN_MUX_0);
+			return 0;
+		}
+		reg |= (1 << 8);
+		ret = sdio_write_reg(WILC_PIN_MUX_0, reg);
+		if (!ret) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write reg (%08x)...\n", WILC_PIN_MUX_0);
+			return 0;
+		}
+
+		/**
+		 *      interrupt enable
+		 **/
+		ret = sdio_read_reg(WILC_INTR_ENABLE, &reg);
+		if (!ret) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read reg (%08x)...\n", WILC_INTR_ENABLE);
+			return 0;
+		}
+
+		for (i = 0; (i < 5) && (nint > 0); i++, nint--) {
+			reg |= (1 << (27 + i));
+		}
+		ret = sdio_write_reg(WILC_INTR_ENABLE, reg);
+		if (!ret) {
+			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write reg (%08x)...\n", WILC_INTR_ENABLE);
+			return 0;
+		}
+		if (nint) {
+			ret = sdio_read_reg(WILC_INTR2_ENABLE, &reg);
+			if (!ret) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read reg (%08x)...\n", WILC_INTR2_ENABLE);
+				return 0;
+			}
+
+			for (i = 0; (i < 3) && (nint > 0); i++, nint--) {
+				reg |= (1 << i);
+			}
+
+			ret = sdio_read_reg(WILC_INTR2_ENABLE, &reg);
+			if (!ret) {
+				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write reg (%08x)...\n", WILC_INTR2_ENABLE);
+				return 0;
+			}
+		}
+	}
+#endif /* WILC_SDIO_IRQ_GPIO */
+	return 1;
+}
+
+
+/********************************************
+ *
+ *      Global sdio HIF function table
+ *
+ ********************************************/
+
+wilc_hif_func_t hif_sdio = {
+	sdio_init,
+	sdio_deinit,
+	sdio_read_reg,
+	sdio_write_reg,
+	sdio_read,
+	sdio_write,
+	sdio_sync,
+	sdio_clear_int,
+	sdio_read_int,
+	sdio_clear_int_ext,
+	sdio_read_size,
+	sdio_write,
+	sdio_read,
+	sdio_sync_ext,
+
+	sdio_set_max_speed,
+	sdio_set_default_speed,
+};
+
diff --git a/drivers/staging/wilc1000/wilc_sleep.c b/drivers/staging/wilc1000/wilc_sleep.c
new file mode 100644
index 000000000000..adab3cac64f9
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_sleep.c
@@ -0,0 +1,18 @@
+
+#include "wilc_sleep.h"
+
+/*
+ *  @author	mdaftedar
+ *  @date	10 Aug 2010
+ *  @version	1.0
+ */
+void WILC_Sleep(u32 u32TimeMilliSec)
+{
+	if (u32TimeMilliSec <= 4000000)	{
+		u32 u32Temp = u32TimeMilliSec * 1000;
+		usleep_range(u32Temp, u32Temp);
+	} else {
+		msleep(u32TimeMilliSec);
+	}
+
+}
diff --git a/drivers/staging/wilc1000/wilc_sleep.h b/drivers/staging/wilc1000/wilc_sleep.h
new file mode 100644
index 000000000000..cf9047f707a7
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_sleep.h
@@ -0,0 +1,20 @@
+#ifndef __WILC_SLEEP_H__
+#define __WILC_SLEEP_H__
+
+#include <linux/types.h>
+#include <linux/delay.h>
+
+/*!
+ *  @brief	forces the current thread to sleep until the given time has elapsed
+ *  @param[in]	u32TimeMilliSec Time to sleep in Milli seconds
+ *  @sa		WILC_SleepMicrosec
+ *  @author	syounan
+ *  @date	10 Aug 2010
+ *  @version	1.0
+ *  @note	This function offers a relatively innacurate and low resolution
+ *              sleep, for accurate high resolution sleep use u32TimeMicoSec
+ */
+/* TODO: remove and open-code in callers */
+void WILC_Sleep(u32 u32TimeMilliSec);
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_spi.c b/drivers/staging/wilc1000/wilc_spi.c
new file mode 100644
index 000000000000..abea5df656d7
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_spi.c
@@ -0,0 +1,1406 @@
+/* ////////////////////////////////////////////////////////////////////////// */
+/*  */
+/* Copyright (c) Atmel Corporation.  All rights reserved. */
+/*  */
+/* Module Name:  wilc_spi.c */
+/*  */
+/*  */
+/* //////////////////////////////////////////////////////////////////////////// */
+
+#include "wilc_wlan_if.h"
+#include "wilc_wlan.h"
+
+extern unsigned int int_clrd;
+
+/*
+ * #include <linux/kernel.h>
+ * #include <linux/string.h>
+ */
+typedef struct {
+	void *os_context;
+	int (*spi_tx)(uint8_t *, uint32_t);
+	int (*spi_rx)(uint8_t *, uint32_t);
+	int (*spi_trx)(uint8_t *, uint8_t *, uint32_t);
+	int (*spi_max_speed)(void);
+	wilc_debug_func dPrint;
+	int crc_off;
+	int nint;
+	int has_thrpt_enh;
+} wilc_spi_t;
+
+static wilc_spi_t g_spi;
+
+static int spi_read(uint32_t, uint8_t *, uint32_t);
+static int spi_write(uint32_t, uint8_t *, uint32_t);
+
+/********************************************
+ *
+ *      Crc7
+ *
+ ********************************************/
+
+static const uint8_t crc7_syndrome_table[256] = {
+	0x00, 0x09, 0x12, 0x1b, 0x24, 0x2d, 0x36, 0x3f,
+	0x48, 0x41, 0x5a, 0x53, 0x6c, 0x65, 0x7e, 0x77,
+	0x19, 0x10, 0x0b, 0x02, 0x3d, 0x34, 0x2f, 0x26,
+	0x51, 0x58, 0x43, 0x4a, 0x75, 0x7c, 0x67, 0x6e,
+	0x32, 0x3b, 0x20, 0x29, 0x16, 0x1f, 0x04, 0x0d,
+	0x7a, 0x73, 0x68, 0x61, 0x5e, 0x57, 0x4c, 0x45,
+	0x2b, 0x22, 0x39, 0x30, 0x0f, 0x06, 0x1d, 0x14,
+	0x63, 0x6a, 0x71, 0x78, 0x47, 0x4e, 0x55, 0x5c,
+	0x64, 0x6d, 0x76, 0x7f, 0x40, 0x49, 0x52, 0x5b,
+	0x2c, 0x25, 0x3e, 0x37, 0x08, 0x01, 0x1a, 0x13,
+	0x7d, 0x74, 0x6f, 0x66, 0x59, 0x50, 0x4b, 0x42,
+	0x35, 0x3c, 0x27, 0x2e, 0x11, 0x18, 0x03, 0x0a,
+	0x56, 0x5f, 0x44, 0x4d, 0x72, 0x7b, 0x60, 0x69,
+	0x1e, 0x17, 0x0c, 0x05, 0x3a, 0x33, 0x28, 0x21,
+	0x4f, 0x46, 0x5d, 0x54, 0x6b, 0x62, 0x79, 0x70,
+	0x07, 0x0e, 0x15, 0x1c, 0x23, 0x2a, 0x31, 0x38,
+	0x41, 0x48, 0x53, 0x5a, 0x65, 0x6c, 0x77, 0x7e,
+	0x09, 0x00, 0x1b, 0x12, 0x2d, 0x24, 0x3f, 0x36,
+	0x58, 0x51, 0x4a, 0x43, 0x7c, 0x75, 0x6e, 0x67,
+	0x10, 0x19, 0x02, 0x0b, 0x34, 0x3d, 0x26, 0x2f,
+	0x73, 0x7a, 0x61, 0x68, 0x57, 0x5e, 0x45, 0x4c,
+	0x3b, 0x32, 0x29, 0x20, 0x1f, 0x16, 0x0d, 0x04,
+	0x6a, 0x63, 0x78, 0x71, 0x4e, 0x47, 0x5c, 0x55,
+	0x22, 0x2b, 0x30, 0x39, 0x06, 0x0f, 0x14, 0x1d,
+	0x25, 0x2c, 0x37, 0x3e, 0x01, 0x08, 0x13, 0x1a,
+	0x6d, 0x64, 0x7f, 0x76, 0x49, 0x40, 0x5b, 0x52,
+	0x3c, 0x35, 0x2e, 0x27, 0x18, 0x11, 0x0a, 0x03,
+	0x74, 0x7d, 0x66, 0x6f, 0x50, 0x59, 0x42, 0x4b,
+	0x17, 0x1e, 0x05, 0x0c, 0x33, 0x3a, 0x21, 0x28,
+	0x5f, 0x56, 0x4d, 0x44, 0x7b, 0x72, 0x69, 0x60,
+	0x0e, 0x07, 0x1c, 0x15, 0x2a, 0x23, 0x38, 0x31,
+	0x46, 0x4f, 0x54, 0x5d, 0x62, 0x6b, 0x70, 0x79
+};
+
+static uint8_t crc7_byte(uint8_t crc, uint8_t data)
+{
+	return crc7_syndrome_table[(crc << 1) ^ data];
+}
+
+static uint8_t crc7(uint8_t crc, const uint8_t *buffer, uint32_t len)
+{
+	while (len--)
+		crc = crc7_byte(crc, *buffer++);
+	return crc;
+}
+
+/********************************************
+ *
+ *      Spi protocol Function
+ *
+ ********************************************/
+
+#define CMD_DMA_WRITE				0xc1
+#define CMD_DMA_READ				0xc2
+#define CMD_INTERNAL_WRITE		0xc3
+#define CMD_INTERNAL_READ		0xc4
+#define CMD_TERMINATE				0xc5
+#define CMD_REPEAT					0xc6
+#define CMD_DMA_EXT_WRITE		0xc7
+#define CMD_DMA_EXT_READ		0xc8
+#define CMD_SINGLE_WRITE			0xc9
+#define CMD_SINGLE_READ			0xca
+#define CMD_RESET						0xcf
+
+#define N_OK								1
+#define N_FAIL								0
+#define N_RESET							-1
+#define N_RETRY							-2
+
+#define DATA_PKT_SZ_256				256
+#define DATA_PKT_SZ_512			512
+#define DATA_PKT_SZ_1K				1024
+#define DATA_PKT_SZ_4K				(4 * 1024)
+#define DATA_PKT_SZ_8K				(8 * 1024)
+#define DATA_PKT_SZ					DATA_PKT_SZ_8K
+
+static int spi_cmd(uint8_t cmd, uint32_t adr, uint32_t data, uint32_t sz, uint8_t clockless)
+{
+	uint8_t bc[9];
+	int len = 5;
+	int result = N_OK;
+
+	bc[0] = cmd;
+	switch (cmd) {
+	case CMD_SINGLE_READ:                           /* single word (4 bytes) read */
+		bc[1] = (uint8_t)(adr >> 16);
+		bc[2] = (uint8_t)(adr >> 8);
+		bc[3] = (uint8_t)adr;
+		len = 5;
+		break;
+
+	case CMD_INTERNAL_READ:                 /* internal register read */
+		bc[1] = (uint8_t)(adr >> 8);
+		if (clockless)
+			bc[1] |= (1 << 7);
+		bc[2] = (uint8_t)adr;
+		bc[3] = 0x00;
+		len = 5;
+		break;
+
+	case CMD_TERMINATE:                                     /* termination */
+		bc[1] = 0x00;
+		bc[2] = 0x00;
+		bc[3] = 0x00;
+		len = 5;
+		break;
+
+	case CMD_REPEAT:                                                /* repeat */
+		bc[1] = 0x00;
+		bc[2] = 0x00;
+		bc[3] = 0x00;
+		len = 5;
+		break;
+
+	case CMD_RESET:                                                 /* reset */
+		bc[1] = 0xff;
+		bc[2] = 0xff;
+		bc[3] = 0xff;
+		len = 5;
+		break;
+
+	case CMD_DMA_WRITE:                                     /* dma write */
+	case CMD_DMA_READ:                                      /* dma read */
+		bc[1] = (uint8_t)(adr >> 16);
+		bc[2] = (uint8_t)(adr >> 8);
+		bc[3] = (uint8_t)adr;
+		bc[4] = (uint8_t)(sz >> 8);
+		bc[5] = (uint8_t)(sz);
+		len = 7;
+		break;
+
+	case CMD_DMA_EXT_WRITE:         /* dma extended write */
+	case CMD_DMA_EXT_READ:                  /* dma extended read */
+		bc[1] = (uint8_t)(adr >> 16);
+		bc[2] = (uint8_t)(adr >> 8);
+		bc[3] = (uint8_t)adr;
+		bc[4] = (uint8_t)(sz >> 16);
+		bc[5] = (uint8_t)(sz >> 8);
+		bc[6] = (uint8_t)(sz);
+		len = 8;
+		break;
+
+	case CMD_INTERNAL_WRITE:                /* internal register write */
+		bc[1] = (uint8_t)(adr >> 8);
+		if (clockless)
+			bc[1] |= (1 << 7);
+		bc[2] = (uint8_t)(adr);
+		bc[3] = (uint8_t)(data >> 24);
+		bc[4] = (uint8_t)(data >> 16);
+		bc[5] = (uint8_t)(data >> 8);
+		bc[6] = (uint8_t)(data);
+		len = 8;
+		break;
+
+	case CMD_SINGLE_WRITE:                  /* single word write */
+		bc[1] = (uint8_t)(adr >> 16);
+		bc[2] = (uint8_t)(adr >> 8);
+		bc[3] = (uint8_t)(adr);
+		bc[4] = (uint8_t)(data >> 24);
+		bc[5] = (uint8_t)(data >> 16);
+		bc[6] = (uint8_t)(data >> 8);
+		bc[7] = (uint8_t)(data);
+		len = 9;
+		break;
+
+	default:
+		result = N_FAIL;
+		break;
+	}
+
+	if (result) {
+		if (!g_spi.crc_off)
+			bc[len - 1] = (crc7(0x7f, (const uint8_t *)&bc[0], len - 1)) << 1;
+		else
+			len -= 1;
+
+		if (!g_spi.spi_tx(bc, len)) {
+			PRINT_ER("[wilc spi]: Failed cmd write, bus error...\n");
+			result = N_FAIL;
+		}
+	}
+
+	return result;
+}
+
+static int spi_cmd_rsp(uint8_t cmd)
+{
+	uint8_t rsp;
+	int result = N_OK;
+
+	/**
+	 *      Command/Control response
+	 **/
+	if ((cmd == CMD_RESET) ||
+	    (cmd == CMD_TERMINATE) ||
+	    (cmd == CMD_REPEAT)) {
+		if (!g_spi.spi_rx(&rsp, 1)) {
+			result = N_FAIL;
+			goto _fail_;
+		}
+	}
+
+	if (!g_spi.spi_rx(&rsp, 1)) {
+		PRINT_ER("[wilc spi]: Failed cmd response read, bus error...\n");
+		result = N_FAIL;
+		goto _fail_;
+	}
+
+	if (rsp != cmd) {
+		PRINT_ER("[wilc spi]: Failed cmd response, cmd (%02x), resp (%02x)\n", cmd, rsp);
+		result = N_FAIL;
+		goto _fail_;
+	}
+
+	/**
+	 *      State response
+	 **/
+	if (!g_spi.spi_rx(&rsp, 1)) {
+		PRINT_ER("[wilc spi]: Failed cmd state read, bus error...\n");
+		result = N_FAIL;
+		goto _fail_;
+	}
+
+	if (rsp != 0x00) {
+		PRINT_ER("[wilc spi]: Failed cmd state response state (%02x)\n", rsp);
+		result = N_FAIL;
+	}
+
+_fail_:
+
+	return result;
+}
+
+static int spi_cmd_complete(uint8_t cmd, uint32_t adr, uint8_t *b, uint32_t sz, uint8_t clockless)
+{
+	uint8_t wb[32], rb[32];
+	uint8_t wix, rix;
+	uint32_t len2;
+	uint8_t rsp;
+	int len = 0;
+	int result = N_OK;
+
+	wb[0] = cmd;
+	switch (cmd) {
+	case CMD_SINGLE_READ:                           /* single word (4 bytes) read */
+		wb[1] = (uint8_t)(adr >> 16);
+		wb[2] = (uint8_t)(adr >> 8);
+		wb[3] = (uint8_t)adr;
+		len = 5;
+		break;
+
+	case CMD_INTERNAL_READ:                 /* internal register read */
+		wb[1] = (uint8_t)(adr >> 8);
+		if (clockless == 1)
+			wb[1] |= (1 << 7);
+		wb[2] = (uint8_t)adr;
+		wb[3] = 0x00;
+		len = 5;
+		break;
+
+	case CMD_TERMINATE:                                     /* termination */
+		wb[1] = 0x00;
+		wb[2] = 0x00;
+		wb[3] = 0x00;
+		len = 5;
+		break;
+
+	case CMD_REPEAT:                                                /* repeat */
+		wb[1] = 0x00;
+		wb[2] = 0x00;
+		wb[3] = 0x00;
+		len = 5;
+		break;
+
+	case CMD_RESET:                                                 /* reset */
+		wb[1] = 0xff;
+		wb[2] = 0xff;
+		wb[3] = 0xff;
+		len = 5;
+		break;
+
+	case CMD_DMA_WRITE:                                     /* dma write */
+	case CMD_DMA_READ:                                      /* dma read */
+		wb[1] = (uint8_t)(adr >> 16);
+		wb[2] = (uint8_t)(adr >> 8);
+		wb[3] = (uint8_t)adr;
+		wb[4] = (uint8_t)(sz >> 8);
+		wb[5] = (uint8_t)(sz);
+		len = 7;
+		break;
+
+	case CMD_DMA_EXT_WRITE:         /* dma extended write */
+	case CMD_DMA_EXT_READ:                  /* dma extended read */
+		wb[1] = (uint8_t)(adr >> 16);
+		wb[2] = (uint8_t)(adr >> 8);
+		wb[3] = (uint8_t)adr;
+		wb[4] = (uint8_t)(sz >> 16);
+		wb[5] = (uint8_t)(sz >> 8);
+		wb[6] = (uint8_t)(sz);
+		len = 8;
+		break;
+
+	case CMD_INTERNAL_WRITE:                /* internal register write */
+		wb[1] = (uint8_t)(adr >> 8);
+		if (clockless == 1)
+			wb[1] |= (1 << 7);
+		wb[2] = (uint8_t)(adr);
+		wb[3] = b[3];
+		wb[4] = b[2];
+		wb[5] = b[1];
+		wb[6] = b[0];
+		len = 8;
+		break;
+
+	case CMD_SINGLE_WRITE:                  /* single word write */
+		wb[1] = (uint8_t)(adr >> 16);
+		wb[2] = (uint8_t)(adr >> 8);
+		wb[3] = (uint8_t)(adr);
+		wb[4] = b[3];
+		wb[5] = b[2];
+		wb[6] = b[1];
+		wb[7] = b[0];
+		len = 9;
+		break;
+
+	default:
+		result = N_FAIL;
+		break;
+	}
+
+	if (result != N_OK) {
+		return result;
+	}
+
+	if (!g_spi.crc_off) {
+		wb[len - 1] = (crc7(0x7f, (const uint8_t *)&wb[0], len - 1)) << 1;
+	} else {
+		len -= 1;
+	}
+
+#define NUM_SKIP_BYTES (1)
+#define NUM_RSP_BYTES (2)
+#define NUM_DATA_HDR_BYTES (1)
+#define NUM_DATA_BYTES (4)
+#define NUM_CRC_BYTES (2)
+#define NUM_DUMMY_BYTES (3)
+	if ((cmd == CMD_RESET) ||
+	    (cmd == CMD_TERMINATE) ||
+	    (cmd == CMD_REPEAT)) {
+		len2 = len + (NUM_SKIP_BYTES + NUM_RSP_BYTES + NUM_DUMMY_BYTES);
+	} else if ((cmd == CMD_INTERNAL_READ) || (cmd == CMD_SINGLE_READ)) {
+		if (!g_spi.crc_off) {
+			len2 = len + (NUM_RSP_BYTES + NUM_DATA_HDR_BYTES + NUM_DATA_BYTES
+				      + NUM_CRC_BYTES + NUM_DUMMY_BYTES);
+		} else {
+			len2 = len + (NUM_RSP_BYTES + NUM_DATA_HDR_BYTES + NUM_DATA_BYTES
+				      + NUM_DUMMY_BYTES);
+		}
+	} else {
+		len2 = len + (NUM_RSP_BYTES + NUM_DUMMY_BYTES);
+	}
+#undef NUM_DUMMY_BYTES
+
+	if (len2 > (sizeof(wb) / sizeof(wb[0]))) {
+		PRINT_ER("[wilc spi]: spi buffer size too small (%d) (%zu)\n",
+			 len2, (sizeof(wb) / sizeof(wb[0])));
+		result = N_FAIL;
+		return result;
+	}
+	/* zero spi write buffers. */
+	for (wix = len; wix < len2; wix++) {
+		wb[wix] = 0;
+	}
+	rix = len;
+
+	if (!g_spi.spi_trx(wb, rb, len2)) {
+		PRINT_ER("[wilc spi]: Failed cmd write, bus error...\n");
+		result = N_FAIL;
+		return result;
+	}
+
+	/**
+	 * Command/Control response
+	 **/
+	if ((cmd == CMD_RESET) ||
+	    (cmd == CMD_TERMINATE) ||
+	    (cmd == CMD_REPEAT)) {
+		rix++;         /* skip 1 byte */
+	}
+
+	/* do { */
+	rsp = rb[rix++];
+	/*	if(rsp == cmd) break; */
+	/* } while(&rptr[1] <= &rb[len2]); */
+
+	if (rsp != cmd) {
+		PRINT_ER("[wilc spi]: Failed cmd response, cmd (%02x)"
+			 ", resp (%02x)\n", cmd, rsp);
+		result = N_FAIL;
+		return result;
+	}
+
+	/**
+	 * State response
+	 **/
+	rsp = rb[rix++];
+	if (rsp != 0x00) {
+		PRINT_ER("[wilc spi]: Failed cmd state response "
+			 "state (%02x)\n", rsp);
+		result = N_FAIL;
+		return result;
+	}
+
+	if ((cmd == CMD_INTERNAL_READ) || (cmd == CMD_SINGLE_READ)
+	    || (cmd == CMD_DMA_READ) || (cmd == CMD_DMA_EXT_READ)) {
+		int retry;
+		/* uint16_t crc1, crc2; */
+		uint8_t crc[2];
+		/**
+		 * Data Respnose header
+		 **/
+		retry = 100;
+		do {
+			/* ensure there is room in buffer later to read data and crc */
+			if (rix < len2) {
+				rsp = rb[rix++];
+			} else {
+				retry = 0;
+				break;
+			}
+			if (((rsp >> 4) & 0xf) == 0xf)
+				break;
+		} while (retry--);
+
+		if (retry <= 0) {
+			PRINT_ER("[wilc spi]: Error, data read "
+				 "response (%02x)\n", rsp);
+			result = N_RESET;
+			return result;
+		}
+
+		if ((cmd == CMD_INTERNAL_READ) || (cmd == CMD_SINGLE_READ)) {
+			/**
+			 * Read bytes
+			 **/
+			if ((rix + 3) < len2) {
+				b[0] = rb[rix++];
+				b[1] = rb[rix++];
+				b[2] = rb[rix++];
+				b[3] = rb[rix++];
+			} else {
+				PRINT_ER("[wilc spi]: buffer overrun when reading data.\n");
+				result = N_FAIL;
+				return result;
+			}
+
+			if (!g_spi.crc_off) {
+				/**
+				 * Read Crc
+				 **/
+				if ((rix + 1) < len2) {
+					crc[0] = rb[rix++];
+					crc[1] = rb[rix++];
+				} else {
+					PRINT_ER("[wilc spi]: buffer overrun when reading crc.\n");
+					result = N_FAIL;
+					return result;
+				}
+			}
+		} else if ((cmd == CMD_DMA_READ) || (cmd == CMD_DMA_EXT_READ)) {
+			int ix;
+
+			/* some data may be read in response to dummy bytes. */
+			for (ix = 0; (rix < len2) && (ix < sz); ) {
+				b[ix++] = rb[rix++];
+			}
+
+			sz -= ix;
+
+			if (sz > 0) {
+				int nbytes;
+
+				if (sz <= (DATA_PKT_SZ - ix)) {
+					nbytes = sz;
+				} else {
+					nbytes = DATA_PKT_SZ - ix;
+				}
+
+				/**
+				 * Read bytes
+				 **/
+				if (!g_spi.spi_rx(&b[ix], nbytes)) {
+					PRINT_ER("[wilc spi]: Failed data block read, bus error...\n");
+					result = N_FAIL;
+					goto _error_;
+				}
+
+				/**
+				 * Read Crc
+				 **/
+				if (!g_spi.crc_off) {
+					if (!g_spi.spi_rx(crc, 2)) {
+						PRINT_ER("[wilc spi]: Failed data block crc read, bus error...\n");
+						result = N_FAIL;
+						goto _error_;
+					}
+				}
+
+
+				ix += nbytes;
+				sz -= nbytes;
+			}
+
+			/*  if any data in left unread, then read the rest using normal DMA code.*/
+			while (sz > 0) {
+				int nbytes;
+
+				if (sz <= DATA_PKT_SZ) {
+					nbytes = sz;
+				} else {
+					nbytes = DATA_PKT_SZ;
+				}
+
+				/**
+				 * read data response only on the next DMA cycles not
+				 * the first DMA since data response header is already
+				 * handled above for the first DMA.
+				 **/
+				/**
+				 * Data Respnose header
+				 **/
+				retry = 10;
+				do {
+					if (!g_spi.spi_rx(&rsp, 1)) {
+						PRINT_ER("[wilc spi]: Failed data response read, bus error...\n");
+						result = N_FAIL;
+						break;
+					}
+					if (((rsp >> 4) & 0xf) == 0xf)
+						break;
+				} while (retry--);
+
+				if (result == N_FAIL)
+					break;
+
+
+				/**
+				 * Read bytes
+				 **/
+				if (!g_spi.spi_rx(&b[ix], nbytes)) {
+					PRINT_ER("[wilc spi]: Failed data block read, bus error...\n");
+					result = N_FAIL;
+					break;
+				}
+
+				/**
+				 * Read Crc
+				 **/
+				if (!g_spi.crc_off) {
+					if (!g_spi.spi_rx(crc, 2)) {
+						PRINT_ER("[wilc spi]: Failed data block crc read, bus error...\n");
+						result = N_FAIL;
+						break;
+					}
+				}
+
+				ix += nbytes;
+				sz -= nbytes;
+			}
+		}
+	}
+_error_:
+	return result;
+}
+
+static int spi_data_read(uint8_t *b, uint32_t sz)
+{
+	int retry, ix, nbytes;
+	int result = N_OK;
+	uint8_t crc[2];
+	uint8_t rsp;
+
+	/**
+	 *      Data
+	 **/
+	ix = 0;
+	do {
+		if (sz <= DATA_PKT_SZ)
+			nbytes = sz;
+		else
+			nbytes = DATA_PKT_SZ;
+
+		/**
+		 *      Data Respnose header
+		 **/
+		retry = 10;
+		do {
+			if (!g_spi.spi_rx(&rsp, 1)) {
+				PRINT_ER("[wilc spi]: Failed data response read, bus error...\n");
+				result = N_FAIL;
+				break;
+			}
+			if (((rsp >> 4) & 0xf) == 0xf)
+				break;
+		} while (retry--);
+
+		if (result == N_FAIL)
+			break;
+
+		if (retry <= 0) {
+			PRINT_ER("[wilc spi]: Failed data response read...(%02x)\n", rsp);
+			result = N_FAIL;
+			break;
+		}
+
+		/**
+		 *      Read bytes
+		 **/
+		if (!g_spi.spi_rx(&b[ix], nbytes)) {
+			PRINT_ER("[wilc spi]: Failed data block read, bus error...\n");
+			result = N_FAIL;
+			break;
+		}
+
+		/**
+		 *      Read Crc
+		 **/
+		if (!g_spi.crc_off) {
+			if (!g_spi.spi_rx(crc, 2)) {
+				PRINT_ER("[wilc spi]: Failed data block crc read, bus error...\n");
+				result = N_FAIL;
+				break;
+			}
+		}
+
+		ix += nbytes;
+		sz -= nbytes;
+
+	} while (sz);
+
+	return result;
+}
+
+static int spi_data_write(uint8_t *b, uint32_t sz)
+{
+	int ix, nbytes;
+	int result = 1;
+	uint8_t cmd, order, crc[2] = {0};
+	/* uint8_t rsp; */
+
+	/**
+	 *      Data
+	 **/
+	ix = 0;
+	do {
+		if (sz <= DATA_PKT_SZ)
+			nbytes = sz;
+		else
+			nbytes = DATA_PKT_SZ;
+
+		/**
+		 *      Write command
+		 **/
+		cmd = 0xf0;
+		if (ix == 0) {
+			if (sz <= DATA_PKT_SZ)
+
+				order = 0x3;
+			else
+				order = 0x1;
+		} else {
+			if (sz <= DATA_PKT_SZ)
+				order = 0x3;
+			else
+				order = 0x2;
+		}
+		cmd |= order;
+		if (!g_spi.spi_tx(&cmd, 1)) {
+			PRINT_ER("[wilc spi]: Failed data block cmd write, bus error...\n");
+			result = N_FAIL;
+			break;
+		}
+
+		/**
+		 *      Write data
+		 **/
+		if (!g_spi.spi_tx(&b[ix], nbytes)) {
+			PRINT_ER("[wilc spi]: Failed data block write, bus error...\n");
+			result = N_FAIL;
+			break;
+		}
+
+		/**
+		 *      Write Crc
+		 **/
+		if (!g_spi.crc_off) {
+			if (!g_spi.spi_tx(crc, 2)) {
+				PRINT_ER("[wilc spi]: Failed data block crc write, bus error...\n");
+				result = N_FAIL;
+				break;
+			}
+		}
+
+		/**
+		 *      No need to wait for response
+		 **/
+		ix += nbytes;
+		sz -= nbytes;
+	} while (sz);
+
+
+	return result;
+}
+
+/********************************************
+ *
+ *      Spi Internal Read/Write Function
+ *
+ ********************************************/
+
+static int spi_internal_write(uint32_t adr, uint32_t dat)
+{
+	int result;
+
+#if defined USE_OLD_SPI_SW
+	/**
+	 *      Command
+	 **/
+	result = spi_cmd(CMD_INTERNAL_WRITE, adr, dat, 4, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed internal write cmd...\n");
+		return 0;
+	}
+
+	result = spi_cmd_rsp(CMD_INTERNAL_WRITE, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed internal write cmd response...\n");
+	}
+#else
+
+#ifdef BIG_ENDIAN
+	dat = BYTE_SWAP(dat);
+#endif
+	result = spi_cmd_complete(CMD_INTERNAL_WRITE, adr, (uint8_t *)&dat, 4, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed internal write cmd...\n");
+	}
+
+#endif
+	return result;
+}
+
+static int spi_internal_read(uint32_t adr, uint32_t *data)
+{
+	int result;
+
+#if defined USE_OLD_SPI_SW
+	result = spi_cmd(CMD_INTERNAL_READ, adr, 0, 4, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed internal read cmd...\n");
+		return 0;
+	}
+
+	result = spi_cmd_rsp(CMD_INTERNAL_READ, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed internal read cmd response...\n");
+		return 0;
+	}
+
+	/**
+	 *      Data
+	 **/
+	result = spi_data_read((uint8_t *)data, 4);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed internal read data...\n");
+		return 0;
+	}
+#else
+	result = spi_cmd_complete(CMD_INTERNAL_READ, adr, (uint8_t *)data, 4, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed internal read cmd...\n");
+		return 0;
+	}
+#endif
+
+
+#ifdef BIG_ENDIAN
+	*data = BYTE_SWAP(*data);
+#endif
+
+	return 1;
+}
+
+/********************************************
+ *
+ *      Spi interfaces
+ *
+ ********************************************/
+
+static int spi_write_reg(uint32_t addr, uint32_t data)
+{
+	int result = N_OK;
+	uint8_t cmd = CMD_SINGLE_WRITE;
+	uint8_t clockless = 0;
+
+
+#if defined USE_OLD_SPI_SW
+	{
+		result = spi_cmd(cmd, addr, data, 4, 0);
+		if (result != N_OK) {
+			PRINT_ER("[wilc spi]: Failed cmd, write reg (%08x)...\n", addr);
+			return 0;
+		}
+
+		result = spi_cmd_rsp(cmd, 0);
+		if (result != N_OK) {
+			PRINT_ER("[wilc spi]: Failed cmd response, write reg (%08x)...\n", addr);
+			return 0;
+		}
+
+		return 1;
+	}
+#else
+#ifdef BIG_ENDIAN
+	data = BYTE_SWAP(data);
+#endif
+	if (addr < 0x30) {
+		/* Clockless register*/
+		cmd = CMD_INTERNAL_WRITE;
+		clockless = 1;
+	}
+
+	result = spi_cmd_complete(cmd, addr, (uint8_t *)&data, 4, clockless);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd, write reg (%08x)...\n", addr);
+	}
+
+	return result;
+#endif
+
+}
+
+static int spi_write(uint32_t addr, uint8_t *buf, uint32_t size)
+{
+	int result;
+	uint8_t cmd = CMD_DMA_EXT_WRITE;
+
+	/**
+	 *      has to be greated than 4
+	 **/
+	if (size <= 4)
+		return 0;
+
+#if defined USE_OLD_SPI_SW
+	/**
+	 *      Command
+	 **/
+	result = spi_cmd(cmd, addr, 0, size, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd, write block (%08x)...\n", addr);
+		return 0;
+	}
+
+	result = spi_cmd_rsp(cmd, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi ]: Failed cmd response, write block (%08x)...\n", addr);
+		return 0;
+	}
+#else
+	result = spi_cmd_complete(cmd, addr, NULL, size, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd, write block (%08x)...\n", addr);
+		return 0;
+	}
+#endif
+
+	/**
+	 *      Data
+	 **/
+	result = spi_data_write(buf, size);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed block data write...\n");
+	}
+
+	return 1;
+}
+
+static int spi_read_reg(uint32_t addr, uint32_t *data)
+{
+	int result = N_OK;
+	uint8_t cmd = CMD_SINGLE_READ;
+	uint8_t clockless = 0;
+
+#if defined USE_OLD_SPI_SW
+	result = spi_cmd(cmd, addr, 0, 4, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd, read reg (%08x)...\n", addr);
+		return 0;
+	}
+	result = spi_cmd_rsp(cmd, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd response, read reg (%08x)...\n", addr);
+		return 0;
+	}
+
+	result = spi_data_read((uint8_t *)data, 4);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed data read...\n");
+		return 0;
+	}
+#else
+	if (addr < 0x30) {
+		/* PRINT_ER("***** read addr %d\n\n", addr); */
+		/* Clockless register*/
+		cmd = CMD_INTERNAL_READ;
+		clockless = 1;
+	}
+
+	result = spi_cmd_complete(cmd, addr, (uint8_t *)data, 4, clockless);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd, read reg (%08x)...\n", addr);
+		return 0;
+	}
+#endif
+
+
+#ifdef BIG_ENDIAN
+	*data = BYTE_SWAP(*data);
+#endif
+
+	return 1;
+}
+
+static int spi_read(uint32_t addr, uint8_t *buf, uint32_t size)
+{
+	uint8_t cmd = CMD_DMA_EXT_READ;
+	int result;
+
+	if (size <= 4)
+		return 0;
+
+#if defined USE_OLD_SPI_SW
+	/**
+	 *      Command
+	 **/
+	result = spi_cmd(cmd, addr, 0, size, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd, read block (%08x)...\n", addr);
+		return 0;
+	}
+
+	result = spi_cmd_rsp(cmd, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd response, read block (%08x)...\n", addr);
+		return 0;
+	}
+
+	/**
+	 *      Data
+	 **/
+	result = spi_data_read(buf, size);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed block data read...\n");
+		return 0;
+	}
+#else
+	result = spi_cmd_complete(cmd, addr, buf, size, 0);
+	if (result != N_OK) {
+		PRINT_ER("[wilc spi]: Failed cmd, read block (%08x)...\n", addr);
+		return 0;
+	}
+#endif
+
+
+	return 1;
+}
+
+/********************************************
+ *
+ *      Bus interfaces
+ *
+ ********************************************/
+
+static int spi_clear_int(void)
+{
+	uint32_t reg;
+	if (!spi_read_reg(WILC_HOST_RX_CTRL_0, &reg)) {
+		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_HOST_RX_CTRL_0);
+		return 0;
+	}
+	reg &= ~0x1;
+	spi_write_reg(WILC_HOST_RX_CTRL_0, reg);
+	int_clrd++;
+	return 1;
+}
+
+static int spi_deinit(void *pv)
+{
+	/**
+	 *      TODO:
+	 **/
+	return 1;
+}
+
+static int spi_sync(void)
+{
+	uint32_t reg;
+	int ret;
+
+	/**
+	 *      interrupt pin mux select
+	 **/
+	ret = spi_read_reg(WILC_PIN_MUX_0, &reg);
+	if (!ret) {
+		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_PIN_MUX_0);
+		return 0;
+	}
+	reg |= (1 << 8);
+	ret = spi_write_reg(WILC_PIN_MUX_0, reg);
+	if (!ret) {
+		PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_PIN_MUX_0);
+		return 0;
+	}
+
+	/**
+	 *      interrupt enable
+	 **/
+	ret = spi_read_reg(WILC_INTR_ENABLE, &reg);
+	if (!ret) {
+		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_INTR_ENABLE);
+		return 0;
+	}
+	reg |= (1 << 16);
+	ret = spi_write_reg(WILC_INTR_ENABLE, reg);
+	if (!ret) {
+		PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_INTR_ENABLE);
+		return 0;
+	}
+
+	return 1;
+}
+
+static int spi_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
+{
+	uint32_t reg;
+	uint32_t chipid;
+
+	static int isinit;
+
+	if (isinit) {
+
+		if (!spi_read_reg(0x1000, &chipid)) {
+			PRINT_ER("[wilc spi]: Fail cmd read chip id...\n");
+			return 0;
+		}
+		return 1;
+	}
+
+	memset(&g_spi, 0, sizeof(wilc_spi_t));
+
+	g_spi.dPrint = func;
+	g_spi.os_context = inp->os_context.os_private;
+	if (inp->io_func.io_init) {
+		if (!inp->io_func.io_init(g_spi.os_context)) {
+			PRINT_ER("[wilc spi]: Failed io init bus...\n");
+			return 0;
+		}
+	} else {
+		return 0;
+	}
+	g_spi.spi_tx = inp->io_func.u.spi.spi_tx;
+	g_spi.spi_rx = inp->io_func.u.spi.spi_rx;
+	g_spi.spi_trx = inp->io_func.u.spi.spi_trx;
+	g_spi.spi_max_speed = inp->io_func.u.spi.spi_max_speed;
+
+	/**
+	 *      configure protocol
+	 **/
+	g_spi.crc_off = 0;
+
+	/* TODO: We can remove the CRC trials if there is a definite way to reset */
+	/* the SPI to it's initial value. */
+	if (!spi_internal_read(WILC_SPI_PROTOCOL_OFFSET, &reg)) {
+		/* Read failed. Try with CRC off. This might happen when module
+		 * is removed but chip isn't reset*/
+		g_spi.crc_off = 1;
+		PRINT_ER("[wilc spi]: Failed internal read protocol with CRC on, retyring with CRC off...\n");
+		if (!spi_internal_read(WILC_SPI_PROTOCOL_OFFSET, &reg)) {
+			/* Reaad failed with both CRC on and off, something went bad */
+			PRINT_ER("[wilc spi]: Failed internal read protocol...\n");
+			return 0;
+		}
+	}
+	if (g_spi.crc_off == 0)	{
+		reg &= ~0xc;    /* disable crc checking */
+		reg &= ~0x70;
+		reg |= (0x5 << 4);
+		if (!spi_internal_write(WILC_SPI_PROTOCOL_OFFSET, reg)) {
+			PRINT_ER("[wilc spi %d]: Failed internal write protocol reg...\n", __LINE__);
+			return 0;
+		}
+		g_spi.crc_off = 1;
+	}
+
+
+	/**
+	 *      make sure can read back chip id correctly
+	 **/
+	if (!spi_read_reg(0x1000, &chipid)) {
+		PRINT_ER("[wilc spi]: Fail cmd read chip id...\n");
+		return 0;
+	}
+	/* PRINT_ER("[wilc spi]: chipid (%08x)\n", chipid); */
+
+	g_spi.has_thrpt_enh = 1;
+
+	isinit = 1;
+
+	return 1;
+}
+
+static void spi_max_bus_speed(void)
+{
+	g_spi.spi_max_speed();
+}
+
+static void spi_default_bus_speed(void)
+{
+}
+
+static int spi_read_size(uint32_t *size)
+{
+	int ret;
+	if (g_spi.has_thrpt_enh) {
+		ret = spi_internal_read(0xe840 - WILC_SPI_REG_BASE, size);
+		*size = *size  & IRQ_DMA_WD_CNT_MASK;
+	} else {
+		uint32_t tmp;
+		uint32_t byte_cnt;
+
+		ret = spi_read_reg(WILC_VMM_TO_HOST_SIZE, &byte_cnt);
+		if (!ret) {
+			PRINT_ER("[wilc spi]: Failed read WILC_VMM_TO_HOST_SIZE ...\n");
+			goto _fail_;
+		}
+		tmp = (byte_cnt >> 2) & IRQ_DMA_WD_CNT_MASK;
+		*size = tmp;
+	}
+
+
+
+_fail_:
+	return ret;
+}
+
+
+
+static int spi_read_int(uint32_t *int_status)
+{
+	int ret;
+	if (g_spi.has_thrpt_enh) {
+		ret = spi_internal_read(0xe840 - WILC_SPI_REG_BASE, int_status);
+	} else {
+		uint32_t tmp;
+		uint32_t byte_cnt;
+
+		ret = spi_read_reg(WILC_VMM_TO_HOST_SIZE, &byte_cnt);
+		if (!ret) {
+			PRINT_ER("[wilc spi]: Failed read WILC_VMM_TO_HOST_SIZE ...\n");
+			goto _fail_;
+		}
+		tmp = (byte_cnt >> 2) & IRQ_DMA_WD_CNT_MASK;
+
+		{
+			int happended, j;
+
+			j = 0;
+			do {
+				uint32_t irq_flags;
+
+				happended = 0;
+
+				spi_read_reg(0x1a90, &irq_flags);
+				tmp |= ((irq_flags >> 27) << IRG_FLAGS_OFFSET);
+
+				if (g_spi.nint > 5) {
+					spi_read_reg(0x1a94, &irq_flags);
+					tmp |= (((irq_flags >> 0) & 0x7) << (IRG_FLAGS_OFFSET + 5));
+				}
+
+				{
+					uint32_t unkmown_mask;
+
+					unkmown_mask = ~((1ul << g_spi.nint) - 1);
+
+					if ((tmp >> IRG_FLAGS_OFFSET) & unkmown_mask) {
+						PRINT_ER("[wilc spi]: Unexpected interrupt (2): j=%d, tmp=%x, mask=%x\n", j, tmp, unkmown_mask);
+						happended = 1;
+					}
+				}
+				j++;
+			} while (happended);
+		}
+
+		*int_status = tmp;
+
+	}
+
+_fail_:
+	return ret;
+}
+
+static int spi_clear_int_ext(uint32_t val)
+{
+	int ret;
+
+	if (g_spi.has_thrpt_enh) {
+		ret = spi_internal_write(0xe844 - WILC_SPI_REG_BASE, val);
+	} else {
+		uint32_t flags;
+		flags = val & ((1 << MAX_NUM_INT) - 1);
+		if (flags) {
+			int i;
+
+			ret = 1;
+			for (i = 0; i < g_spi.nint; i++) {
+				/* No matter what you write 1 or 0, it will clear interrupt. */
+				if (flags & 1)
+					ret = spi_write_reg(0x10c8 + i * 4, 1);
+				if (!ret)
+					break;
+				flags >>= 1;
+			}
+			if (!ret) {
+				PRINT_ER("[wilc spi]: Failed spi_write_reg, set reg %x ...\n", 0x10c8 + i * 4);
+				goto _fail_;
+			}
+			for (i = g_spi.nint; i < MAX_NUM_INT; i++) {
+				if (flags & 1)
+					PRINT_ER("[wilc spi]: Unexpected interrupt cleared %d...\n", i);
+				flags >>= 1;
+			}
+		}
+
+		{
+			uint32_t tbl_ctl;
+
+			tbl_ctl = 0;
+			/* select VMM table 0 */
+			if ((val & SEL_VMM_TBL0) == SEL_VMM_TBL0)
+				tbl_ctl |= (1 << 0);
+			/* select VMM table 1 */
+			if ((val & SEL_VMM_TBL1) == SEL_VMM_TBL1)
+				tbl_ctl |= (1 << 1);
+
+			ret = spi_write_reg(WILC_VMM_TBL_CTL, tbl_ctl);
+			if (!ret) {
+				PRINT_ER("[wilc spi]: fail write reg vmm_tbl_ctl...\n");
+				goto _fail_;
+			}
+
+			if ((val & EN_VMM) == EN_VMM) {
+				/**
+				 *      enable vmm transfer.
+				 **/
+				ret = spi_write_reg(WILC_VMM_CORE_CTL, 1);
+				if (!ret) {
+					PRINT_ER("[wilc spi]: fail write reg vmm_core_ctl...\n");
+					goto _fail_;
+				}
+			}
+		}
+	}
+_fail_:
+	return ret;
+}
+
+static int spi_sync_ext(int nint /*  how mant interrupts to enable. */)
+{
+	uint32_t reg;
+	int ret, i;
+
+	if (nint > MAX_NUM_INT) {
+		PRINT_ER("[wilc spi]: Too many interupts (%d)...\n", nint);
+		return 0;
+	}
+
+	g_spi.nint = nint;
+
+	/**
+	 *      interrupt pin mux select
+	 **/
+	ret = spi_read_reg(WILC_PIN_MUX_0, &reg);
+	if (!ret) {
+		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_PIN_MUX_0);
+		return 0;
+	}
+	reg |= (1 << 8);
+	ret = spi_write_reg(WILC_PIN_MUX_0, reg);
+	if (!ret) {
+		PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_PIN_MUX_0);
+		return 0;
+	}
+
+	/**
+	 *      interrupt enable
+	 **/
+	ret = spi_read_reg(WILC_INTR_ENABLE, &reg);
+	if (!ret) {
+		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_INTR_ENABLE);
+		return 0;
+	}
+
+	for (i = 0; (i < 5) && (nint > 0); i++, nint--) {
+		reg |= (1 << (27 + i));
+	}
+	ret = spi_write_reg(WILC_INTR_ENABLE, reg);
+	if (!ret) {
+		PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_INTR_ENABLE);
+		return 0;
+	}
+	if (nint) {
+		ret = spi_read_reg(WILC_INTR2_ENABLE, &reg);
+		if (!ret) {
+			PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_INTR2_ENABLE);
+			return 0;
+		}
+
+		for (i = 0; (i < 3) && (nint > 0); i++, nint--) {
+			reg |= (1 << i);
+		}
+
+		ret = spi_read_reg(WILC_INTR2_ENABLE, &reg);
+		if (!ret) {
+			PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_INTR2_ENABLE);
+			return 0;
+		}
+	}
+
+	return 1;
+}
+/********************************************
+ *
+ *      Global spi HIF function table
+ *
+ ********************************************/
+wilc_hif_func_t hif_spi = {
+	spi_init,
+	spi_deinit,
+	spi_read_reg,
+	spi_write_reg,
+	spi_read,
+	spi_write,
+	spi_sync,
+	spi_clear_int,
+	spi_read_int,
+	spi_clear_int_ext,
+	spi_read_size,
+	spi_write,
+	spi_read,
+	spi_sync_ext,
+	spi_max_bus_speed,
+	spi_default_bus_speed,
+};
diff --git a/drivers/staging/wilc1000/wilc_strutils.c b/drivers/staging/wilc1000/wilc_strutils.c
new file mode 100644
index 000000000000..e0145953ceef
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_strutils.c
@@ -0,0 +1,80 @@
+
+#define _CRT_SECURE_NO_DEPRECATE
+
+#include "wilc_strutils.h"
+
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+s32 WILC_memcmp(const void *pvArg1, const void *pvArg2, u32 u32Count)
+{
+	return memcmp(pvArg1, pvArg2, u32Count);
+}
+
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+void WILC_memcpy_INTERNAL(void *pvTarget, const void *pvSource, u32 u32Count)
+{
+	memcpy(pvTarget, pvSource, u32Count);
+}
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+void *WILC_memset(void *pvTarget, u8 u8SetValue, u32 u32Count)
+{
+	return memset(pvTarget, u8SetValue, u32Count);
+}
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+char *WILC_strncpy(char *pcTarget, const char *pcSource,
+			u32 u32Count)
+{
+	return strncpy(pcTarget, pcSource, u32Count);
+}
+
+s32 WILC_strncmp(const char *pcStr1, const char *pcStr2,
+			 u32 u32Count)
+{
+	s32 s32Result;
+
+	if (pcStr1 == NULL && pcStr2 == NULL)	{
+		s32Result = 0;
+	} else if (pcStr1 == NULL)	   {
+		s32Result = -1;
+	} else if (pcStr2 == NULL)	   {
+		s32Result = 1;
+	} else {
+		s32Result = strncmp(pcStr1, pcStr2, u32Count);
+		if (s32Result < 0) {
+			s32Result = -1;
+		} else if (s32Result > 0)    {
+			s32Result = 1;
+		}
+	}
+
+	return s32Result;
+}
+
+/*!
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+u32 WILC_strlen(const char *pcStr)
+{
+	return (u32)strlen(pcStr);
+}
diff --git a/drivers/staging/wilc1000/wilc_strutils.h b/drivers/staging/wilc1000/wilc_strutils.h
new file mode 100644
index 000000000000..d1445575a25e
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_strutils.h
@@ -0,0 +1,134 @@
+#ifndef __WILC_STRUTILS_H__
+#define __WILC_STRUTILS_H__
+
+/*!
+ *  @file	wilc_strutils.h
+ *  @brief	Basic string utilities
+ *  @author	syounan
+ *  @sa		wilc_oswrapper.h top level OS wrapper file
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+
+#include <linux/types.h>
+#include <linux/string.h>
+#include "wilc_errorsupport.h"
+
+/*!
+ *  @brief	Compares two memory buffers
+ *  @param[in]	pvArg1 pointer to the first memory location
+ *  @param[in]	pvArg2 pointer to the second memory location
+ *  @param[in]	u32Count the size of the memory buffers
+ *  @return	0 if the 2 buffers are equal, 1 if pvArg1 is bigger than pvArg2,
+ *              -1 if pvArg1 smaller than pvArg2
+ *  @note	this function repeats the functionality of standard memcmp
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+s32 WILC_memcmp(const void *pvArg1, const void *pvArg2, u32 u32Count);
+
+/*!
+ *  @brief	Internal implementation for memory copy
+ *  @param[in]	pvTarget the target buffer to which the data is copied into
+ *  @param[in]	pvSource pointer to the second memory location
+ *  @param[in]	u32Count the size of the data to copy
+ *  @note	this function should not be used directly, use WILC_memcpy instead
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+void WILC_memcpy_INTERNAL(void *pvTarget, const void *pvSource, u32 u32Count);
+
+/*!
+ *  @brief	Copies the contents of a memory buffer into another
+ *  @param[in]	pvTarget the target buffer to which the data is copied into
+ *  @param[in]	pvSource pointer to the second memory location
+ *  @param[in]	u32Count the size of the data to copy
+ *  @return	WILC_SUCCESS if copy is successfully handeled
+ *              WILC_FAIL if copy failed
+ *  @note	this function repeats the functionality of standard memcpy,
+ *              however memcpy is undefined if the two buffers overlap but this
+ *              implementation will check for overlap and report error
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+static WILC_ErrNo WILC_memcpy(void *pvTarget, const void *pvSource, u32 u32Count)
+{
+	if (
+		(((u8 *)pvTarget <= (u8 *)pvSource)
+		 && (((u8 *)pvTarget + u32Count) > (u8 *)pvSource))
+
+		|| (((u8 *)pvSource <= (u8 *)pvTarget)
+		    && (((u8 *)pvSource + u32Count) > (u8 *)pvTarget))
+		) {
+		/* ovelapped memory, return Error */
+		return WILC_FAIL;
+	} else {
+		WILC_memcpy_INTERNAL(pvTarget, pvSource, u32Count);
+		return WILC_SUCCESS;
+	}
+}
+
+/*!
+ *  @brief	Sets the contents of a memory buffer with the given value
+ *  @param[in]	pvTarget the target buffer which contsnts will be set
+ *  @param[in]	u8SetValue the value to be used
+ *  @param[in]	u32Count the size of the memory buffer
+ *  @return	value of pvTarget
+ *  @note	this function repeats the functionality of standard memset
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+void *WILC_memset(void *pvTarget, u8 u8SetValue, u32 u32Count);
+
+/*!
+ *  @brief	copies the contents of source string into the target string
+ *  @param[in]	pcTarget the target string buffer
+ *  @param[in]	pcSource the source string the will be copied
+ *  @param[in]	u32Count copying will proceed until a null character in pcSource
+ *              is encountered or u32Count of bytes copied
+ *  @return	value of pcTarget
+ *  @note	this function repeats the functionality of standard strncpy
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+char *WILC_strncpy(char *pcTarget, const char *pcSource,
+			u32 u32Count);
+
+/*!
+ *  @brief	Compares two strings up to u32Count characters
+ *  @details	Compares 2 strings reporting which is bigger, NULL is considered
+ *              the smallest string, then a zero length string then all other
+ *              strings depending on thier ascii characters order with small case
+ *              converted to uppder case
+ *  @param[in]	pcStr1 the first string, NULL is valid and considered smaller
+ *              than any other non-NULL string (incliding zero lenght strings)
+ *  @param[in]	pcStr2 the second string, NULL is valid and considered smaller
+ *              than any other non-NULL string (incliding zero lenght strings)
+ *  @param[in]	u32Count copying will proceed until a null character in pcStr1 or
+ *              pcStr2 is encountered or u32Count of bytes copied
+ *  @return	0 if the 2 strings are equal, 1 if pcStr1 is bigger than pcStr2,
+ *              -1 if pcStr1 smaller than pcStr2
+ *  @author	aabozaeid
+ *  @date	7 Dec 2010
+ *  @version	1.0
+ */
+s32 WILC_strncmp(const char *pcStr1, const char *pcStr2,
+			 u32 u32Count);
+
+/*!
+ *  @brief	gets the length of a string
+ *  @param[in]	pcStr the string
+ *  @return	the length
+ *  @note	this function repeats the functionality of standard strlen
+ *  @author	syounan
+ *  @date	18 Aug 2010
+ *  @version	1.0
+ */
+u32 WILC_strlen(const char *pcStr);
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_timer.c b/drivers/staging/wilc1000/wilc_timer.c
new file mode 100644
index 000000000000..dc71157f9c3e
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_timer.c
@@ -0,0 +1,45 @@
+
+#include "wilc_timer.h"
+
+WILC_ErrNo WILC_TimerCreate(WILC_TimerHandle *pHandle,
+	tpfWILC_TimerFunction pfCallback, tstrWILC_TimerAttrs *pstrAttrs)
+{
+	WILC_ErrNo s32RetStatus = WILC_SUCCESS;
+	setup_timer(pHandle, (void(*)(unsigned long))pfCallback, 0);
+
+	return s32RetStatus;
+}
+
+WILC_ErrNo WILC_TimerDestroy(WILC_TimerHandle *pHandle,
+	tstrWILC_TimerAttrs *pstrAttrs)
+{
+	WILC_ErrNo s32RetStatus = WILC_FAIL;
+	if (pHandle != NULL) {
+		s32RetStatus = del_timer_sync(pHandle);
+		pHandle = NULL;
+	}
+
+	return s32RetStatus;
+}
+
+
+WILC_ErrNo WILC_TimerStart(WILC_TimerHandle *pHandle, u32 u32Timeout,
+	void *pvArg, tstrWILC_TimerAttrs *pstrAttrs)
+{
+	WILC_ErrNo s32RetStatus = WILC_FAIL;
+	if (pHandle != NULL) {
+		pHandle->data = (unsigned long)pvArg;
+		s32RetStatus = mod_timer(pHandle, (jiffies + msecs_to_jiffies(u32Timeout)));
+	}
+	return s32RetStatus;
+}
+
+WILC_ErrNo WILC_TimerStop(WILC_TimerHandle *pHandle,
+	tstrWILC_TimerAttrs *pstrAttrs)
+{
+	WILC_ErrNo s32RetStatus = WILC_FAIL;
+	if (pHandle != NULL)
+		s32RetStatus = del_timer(pHandle);
+
+	return s32RetStatus;
+}
diff --git a/drivers/staging/wilc1000/wilc_timer.h b/drivers/staging/wilc1000/wilc_timer.h
new file mode 100644
index 000000000000..931269db3194
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_timer.h
@@ -0,0 +1,129 @@
+#ifndef __WILC_TIMER_H__
+#define __WILC_TIMER_H__
+
+/*!
+ *  @file	wilc_timer.h
+ *  @brief	Timer (One Shot and Periodic) OS wrapper functionality
+ *  @author	syounan
+ *  @sa		wilc_oswrapper.h top level OS wrapper file
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+
+#include "wilc_platform.h"
+#include "wilc_errorsupport.h"
+
+typedef void (*tpfWILC_TimerFunction)(void *);
+
+/*!
+ *  @struct             tstrWILC_TimerAttrs
+ *  @brief		Timer API options
+ *  @author		syounan
+ *  @date		16 Aug 2010
+ *  @version		1.0
+ */
+typedef struct {
+	/* a dummy member to avoid compiler errors*/
+	u8 dummy;
+} tstrWILC_TimerAttrs;
+
+/*!
+ *  @brief	Creates a new timer
+ *  @details	Timers are a useful utility to execute some callback function
+ *              in the future.
+ *              A timer object has 3 states : IDLE, PENDING and EXECUTING
+ *              IDLE : initial timer state after creation, no execution for the
+ *              callback function is planned
+ *              PENDING : a request to execute the callback function is made
+ *              using WILC_TimerStart.
+ *              EXECUTING : the timer has expired and its callback is now
+ *              executing, when execution is done the timer returns to PENDING
+ *              if the feature CONFIG_WILC_TIMER_PERIODIC is enabled and
+ *              the flag tstrWILC_TimerAttrs.bPeriodicTimer is set. otherwise the
+ *              timer will return to IDLE
+ *  @param[out]	pHandle handle to the newly created timer object
+ *  @param[in]	pfEntry pointer to the callback function to be called when the
+ *              timer expires
+ *              the underlaying OS may put many restrictions on what can be
+ *              called inside a timer's callback, as a general rule no blocking
+ *              operations (IO or semaphore Acquision) should be perfomred
+ *              It is recommended that the callback will be as short as possible
+ *              and only flags other threads to do the actual work
+ *              also it should be noted that the underlaying OS maynot give any
+ *              guarentees on which contect this callback will execute in
+ *  @param[in]	pstrAttrs Optional attributes, NULL for default
+ *  @return	Error code indicating sucess/failure
+ *  @sa		WILC_TimerAttrs
+ *  @author	syounan
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+WILC_ErrNo WILC_TimerCreate(WILC_TimerHandle *pHandle,
+			    tpfWILC_TimerFunction pfCallback, tstrWILC_TimerAttrs *pstrAttrs);
+
+
+/*!
+ *  @brief	Destroys a given timer
+ *  @details	This will destroy a given timer freeing any resources used by it
+ *              if the timer was PENDING Then must be cancelled as well(i.e.
+ *              goes to	IDLE, same effect as calling WILC_TimerCancel first)
+ *              if the timer was EXECUTING then the callback will be allowed to
+ *              finish first then all resources are freed
+ *  @param[in]	pHandle handle to the timer object
+ *  @param[in]	pstrAttrs Optional attributes, NULL for default
+ *  @return	Error code indicating sucess/failure
+ *  @sa		WILC_TimerAttrs
+ *  @author	syounan
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+WILC_ErrNo WILC_TimerDestroy(WILC_TimerHandle *pHandle,
+			     tstrWILC_TimerAttrs *pstrAttrs);
+
+/*!
+ *  @brief	Starts a given timer
+ *  @details	This function will move the timer to the PENDING state until the
+ *              given time expires (in msec) then the callback function will be
+ *              executed (timer in EXECUTING state) after execution is dene the
+ *              timer either goes to IDLE (if bPeriodicTimer==false) or
+ *              PENDING with same timeout value (if bPeriodicTimer==true)
+ *  @param[in]	pHandle handle to the timer object
+ *  @param[in]	u32Timeout timeout value in msec after witch the callback
+ *              function will be executed. Timeout value of 0 is not allowed for
+ *              periodic timers
+ *  @param[in]	pstrAttrs Optional attributes, NULL for default,
+ *              set bPeriodicTimer to run this timer as a periodic timer
+ *  @return	Error code indicating sucess/failure
+ *  @sa		WILC_TimerAttrs
+ *  @author	syounan
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+WILC_ErrNo WILC_TimerStart(WILC_TimerHandle *pHandle, u32 u32Timeout, void *pvArg,
+			   tstrWILC_TimerAttrs *pstrAttrs);
+
+
+/*!
+ *  @brief	Stops a given timer
+ *  @details	This function will move the timer to the IDLE state cancelling
+ *              any sheduled callback execution.
+ *              if this function is called on a timer already in the IDLE state
+ *              it will have no effect.
+ *              if this function is called on a timer in EXECUTING state
+ *              (callback has already started) it will wait until executing is
+ *              done then move the timer to the IDLE state (which is trivial
+ *              work if the timer is non periodic)
+ *  @param[in]	pHandle handle to the timer object
+ *  @param[in]	pstrAttrs Optional attributes, NULL for default,
+ *  @return	Error code indicating sucess/failure
+ *  @sa		WILC_TimerAttrs
+ *  @author	syounan
+ *  @date	16 Aug 2010
+ *  @version	1.0
+ */
+WILC_ErrNo WILC_TimerStop(WILC_TimerHandle *pHandle,
+			  tstrWILC_TimerAttrs *pstrAttrs);
+
+
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_type.h b/drivers/staging/wilc1000/wilc_type.h
new file mode 100644
index 000000000000..5f36e7f92cd1
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_type.h
@@ -0,0 +1,34 @@
+/* ////////////////////////////////////////////////////////////////////////// */
+/*  */
+/* Copyright (c) Atmel Corporation.  All rights reserved. */
+/*  */
+/* Module Name:  wilc_type.h */
+/*  */
+/*  */
+/* //////////////////////////////////////////////////////////////////////////// */
+#ifndef WILC_TYPE_H
+#define WILC_TYPE_H
+
+/********************************************
+ *
+ *      Type Defines
+ *
+ ********************************************/
+#ifdef WIN32
+typedef char int8_t;
+typedef short int16_t;
+typedef long int32_t;
+typedef unsigned char uint8_t;
+typedef unsigned short uint16_t;
+typedef unsigned long uint32_t;
+#else
+#ifdef _linux_
+/*typedef unsigned char         uint8_t;
+ * typedef unsigned short       uint16_t;
+ * typedef unsigned long        uint32_t;*/
+#include <stdint.h>
+#else
+#include "wilc_oswrapper.h"
+#endif
+#endif
+#endif
diff --git a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
new file mode 100644
index 000000000000..92064db9eb05
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
@@ -0,0 +1,3957 @@
+/*!
+ *  @file	wilc_wfi_cfgopertaions.c
+ *  @brief	CFG80211 Function Implementation functionality
+ *  @author	aabouzaeid
+ *                      mabubakr
+ *                      mdaftedar
+ *                      zsalah
+ *  @sa		wilc_wfi_cfgopertaions.h top level OS wrapper file
+ *  @date	31 Aug 2010
+ *  @version	1.0
+ */
+
+#include "wilc_wfi_cfgoperations.h"
+#include "wilc_wlan.c"
+#ifdef WILC_SDIO
+#include "linux_wlan_sdio.h"    /* tony : for set_wiphy_dev() */
+#endif
+
+
+#define IS_MANAGMEMENT				0x100
+#define IS_MANAGMEMENT_CALLBACK			0x080
+#define IS_MGMT_STATUS_SUCCES			0x040
+#define GET_PKT_OFFSET(a) (((a) >> 22) & 0x1ff)
+
+extern void linux_wlan_free(void *vp);
+extern int linux_wlan_get_firmware(perInterface_wlan_t *p_nic);
+extern void linux_wlan_unlock(void *vp);
+extern u16 Set_machw_change_vir_if(bool bValue);
+
+extern int mac_open(struct net_device *ndev);
+extern int mac_close(struct net_device *ndev);
+
+tstrNetworkInfo astrLastScannedNtwrksShadow[MAX_NUM_SCANNED_NETWORKS_SHADOW];
+u32 u32LastScannedNtwrksCountShadow;
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+WILC_TimerHandle hDuringIpTimer;
+#endif
+WILC_TimerHandle hAgingTimer;
+static u8 op_ifcs;
+extern u8 u8ConnectedSSID[6];
+
+/*BugID_5137*/
+u8 g_wilc_initialized = 1;
+extern linux_wlan_t *g_linux_wlan;
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+extern bool g_obtainingIP;
+#endif
+
+#define CHAN2G(_channel, _freq, _flags) {	 \
+		.band             = IEEE80211_BAND_2GHZ, \
+		.center_freq      = (_freq),		 \
+		.hw_value         = (_channel),		 \
+		.flags            = (_flags),		 \
+		.max_antenna_gain = 0,			 \
+		.max_power        = 30,			 \
+}
+
+/*Frequency range for channels*/
+static struct ieee80211_channel WILC_WFI_2ghz_channels[] = {
+	CHAN2G(1,  2412, 0),
+	CHAN2G(2,  2417, 0),
+	CHAN2G(3,  2422, 0),
+	CHAN2G(4,  2427, 0),
+	CHAN2G(5,  2432, 0),
+	CHAN2G(6,  2437, 0),
+	CHAN2G(7,  2442, 0),
+	CHAN2G(8,  2447, 0),
+	CHAN2G(9,  2452, 0),
+	CHAN2G(10, 2457, 0),
+	CHAN2G(11, 2462, 0),
+	CHAN2G(12, 2467, 0),
+	CHAN2G(13, 2472, 0),
+	CHAN2G(14, 2484, 0),
+};
+
+#define RATETAB_ENT(_rate, _hw_value, _flags) {	\
+		.bitrate  = (_rate),			\
+		.hw_value = (_hw_value),		\
+		.flags    = (_flags),			\
+}
+
+
+/* Table 6 in section 3.2.1.1 */
+static struct ieee80211_rate WILC_WFI_rates[] = {
+	RATETAB_ENT(10,  0,  0),
+	RATETAB_ENT(20,  1,  0),
+	RATETAB_ENT(55,  2,  0),
+	RATETAB_ENT(110, 3,  0),
+	RATETAB_ENT(60,  9,  0),
+	RATETAB_ENT(90,  6,  0),
+	RATETAB_ENT(120, 7,  0),
+	RATETAB_ENT(180, 8,  0),
+	RATETAB_ENT(240, 9,  0),
+	RATETAB_ENT(360, 10, 0),
+	RATETAB_ENT(480, 11, 0),
+	RATETAB_ENT(540, 12, 0),
+};
+
+#ifdef WILC_P2P
+struct p2p_mgmt_data {
+	int size;
+	u8 *buff;
+};
+
+/*Global variable used to state the current  connected STA channel*/
+u8 u8WLANChannel = INVALID_CHANNEL;
+
+/*BugID_5442*/
+u8 u8CurrChannel;
+
+u8 u8P2P_oui[] = {0x50, 0x6f, 0x9A, 0x09};
+u8 u8P2Plocalrandom = 0x01;
+u8 u8P2Precvrandom = 0x00;
+u8 u8P2P_vendorspec[] = {0xdd, 0x05, 0x00, 0x08, 0x40, 0x03};
+bool bWilc_ie = false;
+#endif
+
+static struct ieee80211_supported_band WILC_WFI_band_2ghz = {
+	.channels = WILC_WFI_2ghz_channels,
+	.n_channels = ARRAY_SIZE(WILC_WFI_2ghz_channels),
+	.bitrates = WILC_WFI_rates,
+	.n_bitrates = ARRAY_SIZE(WILC_WFI_rates),
+};
+
+
+/*BugID_5137*/
+struct add_key_params {
+	u8 key_idx;
+	bool pairwise;
+	u8 *mac_addr;
+};
+struct add_key_params g_add_gtk_key_params;
+struct wilc_wfi_key g_key_gtk_params;
+struct add_key_params g_add_ptk_key_params;
+struct wilc_wfi_key g_key_ptk_params;
+struct wilc_wfi_wep_key g_key_wep_params;
+u8 g_flushing_in_progress;
+bool g_ptk_keys_saved = false;
+bool g_gtk_keys_saved = false;
+bool g_wep_keys_saved = false;
+
+#define AGING_TIME	(9 * 1000)
+#define duringIP_TIME 15000
+
+void clear_shadow_scan(void *pUserVoid)
+{
+	struct WILC_WFI_priv *priv;
+	int i;
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+	if (op_ifcs == 0) {
+		WILC_TimerDestroy(&hAgingTimer, NULL);
+		PRINT_INFO(CORECONFIG_DBG, "destroy aging timer\n");
+
+		for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
+			if (astrLastScannedNtwrksShadow[u32LastScannedNtwrksCountShadow].pu8IEs != NULL) {
+				WILC_FREE(astrLastScannedNtwrksShadow[i].pu8IEs);
+				astrLastScannedNtwrksShadow[u32LastScannedNtwrksCountShadow].pu8IEs = NULL;
+			}
+
+			host_int_freeJoinParams(astrLastScannedNtwrksShadow[i].pJoinParams);
+			astrLastScannedNtwrksShadow[i].pJoinParams = NULL;
+		}
+		u32LastScannedNtwrksCountShadow = 0;
+	}
+
+}
+
+uint32_t get_rssi_avg(tstrNetworkInfo *pstrNetworkInfo)
+{
+	uint8_t i;
+	int rssi_v = 0;
+	uint8_t num_rssi = (pstrNetworkInfo->strRssi.u8Full) ? NUM_RSSI : (pstrNetworkInfo->strRssi.u8Index);
+
+	for (i = 0; i < num_rssi; i++)
+		rssi_v += pstrNetworkInfo->strRssi.as8RSSI[i];
+
+	rssi_v /= num_rssi;
+	return rssi_v;
+}
+
+void refresh_scan(void *pUserVoid, uint8_t all, bool bDirectScan)
+{
+	struct WILC_WFI_priv *priv;
+	struct wiphy *wiphy;
+	struct cfg80211_bss *bss = NULL;
+	int i;
+	int rssi = 0;
+
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+	wiphy = priv->dev->ieee80211_ptr->wiphy;
+
+	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
+		tstrNetworkInfo *pstrNetworkInfo;
+		pstrNetworkInfo = &(astrLastScannedNtwrksShadow[i]);
+
+
+		if ((!pstrNetworkInfo->u8Found) || all) {
+			s32 s32Freq;
+			struct ieee80211_channel *channel;
+
+			if (pstrNetworkInfo != NULL) {
+
+				s32Freq = ieee80211_channel_to_frequency((s32)pstrNetworkInfo->u8channel, IEEE80211_BAND_2GHZ);
+				channel = ieee80211_get_channel(wiphy, s32Freq);
+
+				rssi = get_rssi_avg(pstrNetworkInfo);
+				if (WILC_memcmp("DIRECT-", pstrNetworkInfo->au8ssid, 7) || bDirectScan)	{
+					bss = cfg80211_inform_bss(wiphy, channel, CFG80211_BSS_FTYPE_UNKNOWN, pstrNetworkInfo->au8bssid, pstrNetworkInfo->u64Tsf, pstrNetworkInfo->u16CapInfo,
+								  pstrNetworkInfo->u16BeaconPeriod, (const u8 *)pstrNetworkInfo->pu8IEs,
+								  (size_t)pstrNetworkInfo->u16IEsLen, (((s32)rssi) * 100), GFP_KERNEL);
+					cfg80211_put_bss(wiphy, bss);
+				}
+			}
+
+		}
+	}
+
+}
+
+void reset_shadow_found(void *pUserVoid)
+{
+	struct WILC_WFI_priv *priv;
+	int i;
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
+		astrLastScannedNtwrksShadow[i].u8Found = 0;
+
+	}
+}
+
+void update_scan_time(void *pUserVoid)
+{
+	struct WILC_WFI_priv *priv;
+	int i;
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
+		astrLastScannedNtwrksShadow[i].u32TimeRcvdInScan = jiffies;
+	}
+}
+
+void remove_network_from_shadow(void *pUserVoid)
+{
+	struct WILC_WFI_priv *priv;
+	unsigned long now = jiffies;
+	int i, j;
+
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+
+	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
+		if (time_after(now, astrLastScannedNtwrksShadow[i].u32TimeRcvdInScan + (unsigned long)(SCAN_RESULT_EXPIRE))) {
+			PRINT_D(CFG80211_DBG, "Network expired in ScanShadow: %s \n", astrLastScannedNtwrksShadow[i].au8ssid);
+
+			if (astrLastScannedNtwrksShadow[i].pu8IEs != NULL) {
+				WILC_FREE(astrLastScannedNtwrksShadow[i].pu8IEs);
+				astrLastScannedNtwrksShadow[i].pu8IEs = NULL;
+			}
+
+			host_int_freeJoinParams(astrLastScannedNtwrksShadow[i].pJoinParams);
+
+			for (j = i; (j < u32LastScannedNtwrksCountShadow - 1); j++) {
+				astrLastScannedNtwrksShadow[j] = astrLastScannedNtwrksShadow[j + 1];
+			}
+			u32LastScannedNtwrksCountShadow--;
+		}
+	}
+
+	PRINT_D(CFG80211_DBG, "Number of cached networks: %d\n", u32LastScannedNtwrksCountShadow);
+	if (u32LastScannedNtwrksCountShadow != 0)
+		WILC_TimerStart(&(hAgingTimer), AGING_TIME, pUserVoid, NULL);
+	else
+		PRINT_D(CFG80211_DBG, "No need to restart Aging timer\n");
+}
+
+#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+void clear_duringIP(void *pUserVoid)
+{
+	PRINT_D(GENERIC_DBG, "GO:IP Obtained , enable scan\n");
+	g_obtainingIP = false;
+}
+#endif
+
+int8_t is_network_in_shadow(tstrNetworkInfo *pstrNetworkInfo, void *pUserVoid)
+{
+	struct WILC_WFI_priv *priv;
+	int8_t state = -1;
+	int i;
+
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+	if (u32LastScannedNtwrksCountShadow == 0) {
+		PRINT_D(CFG80211_DBG, "Starting Aging timer\n");
+		WILC_TimerStart(&(hAgingTimer), AGING_TIME, pUserVoid, NULL);
+		state = -1;
+	} else {
+		/* Linear search for now */
+		for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
+			if (WILC_memcmp(astrLastScannedNtwrksShadow[i].au8bssid,
+					pstrNetworkInfo->au8bssid, 6) == 0) {
+				state = i;
+				break;
+			}
+		}
+	}
+	return state;
+}
+
+void add_network_to_shadow(tstrNetworkInfo *pstrNetworkInfo, void *pUserVoid, void *pJoinParams)
+{
+	struct WILC_WFI_priv *priv;
+	int8_t ap_found = is_network_in_shadow(pstrNetworkInfo, pUserVoid);
+	uint32_t ap_index = 0;
+	uint8_t rssi_index = 0;
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+
+	if (u32LastScannedNtwrksCountShadow >= MAX_NUM_SCANNED_NETWORKS_SHADOW) {
+		PRINT_D(CFG80211_DBG, "Shadow network reached its maximum limit\n");
+		return;
+	}
+	if (ap_found == -1) {
+		ap_index = u32LastScannedNtwrksCountShadow;
+		u32LastScannedNtwrksCountShadow++;
+
+	} else {
+		ap_index = ap_found;
+	}
+	rssi_index = astrLastScannedNtwrksShadow[ap_index].strRssi.u8Index;
+	astrLastScannedNtwrksShadow[ap_index].strRssi.as8RSSI[rssi_index++] = pstrNetworkInfo->s8rssi;
+	if (rssi_index == NUM_RSSI) {
+		rssi_index = 0;
+		astrLastScannedNtwrksShadow[ap_index].strRssi.u8Full = 1;
+	}
+	astrLastScannedNtwrksShadow[ap_index].strRssi.u8Index = rssi_index;
+
+	astrLastScannedNtwrksShadow[ap_index].s8rssi = pstrNetworkInfo->s8rssi;
+	astrLastScannedNtwrksShadow[ap_index].u16CapInfo = pstrNetworkInfo->u16CapInfo;
+
+	astrLastScannedNtwrksShadow[ap_index].u8SsidLen = pstrNetworkInfo->u8SsidLen;
+	WILC_memcpy(astrLastScannedNtwrksShadow[ap_index].au8ssid,
+		    pstrNetworkInfo->au8ssid, pstrNetworkInfo->u8SsidLen);
+
+	WILC_memcpy(astrLastScannedNtwrksShadow[ap_index].au8bssid,
+		    pstrNetworkInfo->au8bssid, ETH_ALEN);
+
+	astrLastScannedNtwrksShadow[ap_index].u16BeaconPeriod = pstrNetworkInfo->u16BeaconPeriod;
+	astrLastScannedNtwrksShadow[ap_index].u8DtimPeriod = pstrNetworkInfo->u8DtimPeriod;
+	astrLastScannedNtwrksShadow[ap_index].u8channel = pstrNetworkInfo->u8channel;
+
+	astrLastScannedNtwrksShadow[ap_index].u16IEsLen = pstrNetworkInfo->u16IEsLen;
+	astrLastScannedNtwrksShadow[ap_index].u64Tsf = pstrNetworkInfo->u64Tsf;
+	if (ap_found != -1)
+		WILC_FREE(astrLastScannedNtwrksShadow[ap_index].pu8IEs);
+	astrLastScannedNtwrksShadow[ap_index].pu8IEs =
+		(u8 *)WILC_MALLOC(pstrNetworkInfo->u16IEsLen);        /* will be deallocated by the WILC_WFI_CfgScan() function */
+	WILC_memcpy(astrLastScannedNtwrksShadow[ap_index].pu8IEs,
+		    pstrNetworkInfo->pu8IEs, pstrNetworkInfo->u16IEsLen);
+
+	astrLastScannedNtwrksShadow[ap_index].u32TimeRcvdInScan = jiffies;
+	astrLastScannedNtwrksShadow[ap_index].u32TimeRcvdInScanCached = jiffies;
+	astrLastScannedNtwrksShadow[ap_index].u8Found = 1;
+	if (ap_found != -1)
+		host_int_freeJoinParams(astrLastScannedNtwrksShadow[ap_index].pJoinParams);
+	astrLastScannedNtwrksShadow[ap_index].pJoinParams = pJoinParams;
+
+}
+
+
+/**
+ *  @brief      CfgScanResult
+ *  @details  Callback function which returns the scan results found
+ *
+ *  @param[in] tenuScanEvent enuScanEvent: enum, indicating the scan event triggered, whether that is
+ *                        SCAN_EVENT_NETWORK_FOUND or SCAN_EVENT_DONE
+ *                        tstrNetworkInfo* pstrNetworkInfo: structure holding the scan results information
+ *                        void* pUserVoid: Private structure associated with the wireless interface
+ *  @return     NONE
+ *  @author	mabubakr
+ *  @date
+ *  @version	1.0
+ */
+static void CfgScanResult(tenuScanEvent enuScanEvent, tstrNetworkInfo *pstrNetworkInfo, void *pUserVoid, void *pJoinParams)
+{
+	struct WILC_WFI_priv *priv;
+	struct wiphy *wiphy;
+	s32 s32Freq;
+	struct ieee80211_channel *channel;
+	s32 s32Error = WILC_SUCCESS;
+	struct cfg80211_bss *bss = NULL;
+
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+	if (priv->bCfgScanning == true) {
+		if (enuScanEvent == SCAN_EVENT_NETWORK_FOUND) {
+			wiphy = priv->dev->ieee80211_ptr->wiphy;
+			WILC_NULLCHECK(s32Error, wiphy);
+			if (wiphy->signal_type == CFG80211_SIGNAL_TYPE_UNSPEC
+			    &&
+			    ((((s32)pstrNetworkInfo->s8rssi) * 100) < 0
+			     ||
+			     (((s32)pstrNetworkInfo->s8rssi) * 100) > 100)
+			    ) {
+				WILC_ERRORREPORT(s32Error, WILC_FAIL);
+			}
+
+			if (pstrNetworkInfo != NULL) {
+				s32Freq = ieee80211_channel_to_frequency((s32)pstrNetworkInfo->u8channel, IEEE80211_BAND_2GHZ);
+				channel = ieee80211_get_channel(wiphy, s32Freq);
+
+				WILC_NULLCHECK(s32Error, channel);
+
+				PRINT_INFO(CFG80211_DBG, "Network Info:: CHANNEL Frequency: %d, RSSI: %d, CapabilityInfo: %d,"
+					   "BeaconPeriod: %d \n", channel->center_freq, (((s32)pstrNetworkInfo->s8rssi) * 100),
+					   pstrNetworkInfo->u16CapInfo, pstrNetworkInfo->u16BeaconPeriod);
+
+				if (pstrNetworkInfo->bNewNetwork == true) {
+					if (priv->u32RcvdChCount < MAX_NUM_SCANNED_NETWORKS) { /* TODO: mostafa: to be replaced by */
+						/*               max_scan_ssids */
+						PRINT_D(CFG80211_DBG, "Network %s found\n", pstrNetworkInfo->au8ssid);
+
+
+						priv->u32RcvdChCount++;
+
+
+
+						if (pJoinParams == NULL) {
+							PRINT_INFO(CORECONFIG_DBG, ">> Something really bad happened\n");
+						}
+						add_network_to_shadow(pstrNetworkInfo, priv, pJoinParams);
+
+						/*P2P peers are sent to WPA supplicant and added to shadow table*/
+
+						if (!(WILC_memcmp("DIRECT-", pstrNetworkInfo->au8ssid, 7))) {
+							bss = cfg80211_inform_bss(wiphy, channel, CFG80211_BSS_FTYPE_UNKNOWN,  pstrNetworkInfo->au8bssid, pstrNetworkInfo->u64Tsf, pstrNetworkInfo->u16CapInfo,
+										  pstrNetworkInfo->u16BeaconPeriod, (const u8 *)pstrNetworkInfo->pu8IEs,
+										  (size_t)pstrNetworkInfo->u16IEsLen, (((s32)pstrNetworkInfo->s8rssi) * 100), GFP_KERNEL);
+							cfg80211_put_bss(wiphy, bss);
+						}
+
+
+					} else {
+						PRINT_ER("Discovered networks exceeded the max limit\n");
+					}
+				} else {
+					u32 i;
+					/* So this network is discovered before, we'll just update its RSSI */
+					for (i = 0; i < priv->u32RcvdChCount; i++) {
+						if (WILC_memcmp(astrLastScannedNtwrksShadow[i].au8bssid, pstrNetworkInfo->au8bssid, 6) == 0) {
+							PRINT_D(CFG80211_DBG, "Update RSSI of %s \n", astrLastScannedNtwrksShadow[i].au8ssid);
+
+							astrLastScannedNtwrksShadow[i].s8rssi = pstrNetworkInfo->s8rssi;
+							astrLastScannedNtwrksShadow[i].u32TimeRcvdInScan = jiffies;
+							break;
+						}
+					}
+				}
+			}
+		} else if (enuScanEvent == SCAN_EVENT_DONE)    {
+			PRINT_D(CFG80211_DBG, "Scan Done[%p] \n", priv->dev);
+			PRINT_D(CFG80211_DBG, "Refreshing Scan ... \n");
+			refresh_scan(priv, 1, false);
+
+			if (priv->u32RcvdChCount > 0) {
+				PRINT_D(CFG80211_DBG, "%d Network(s) found \n", priv->u32RcvdChCount);
+			} else {
+				PRINT_D(CFG80211_DBG, "No networks found \n");
+			}
+
+			down(&(priv->hSemScanReq));
+
+			if (priv->pstrScanReq != NULL) {
+				cfg80211_scan_done(priv->pstrScanReq, false);
+				priv->u32RcvdChCount = 0;
+				priv->bCfgScanning = false;
+				priv->pstrScanReq = NULL;
+			}
+			up(&(priv->hSemScanReq));
+
+		}
+		/*Aborting any scan operation during mac close*/
+		else if (enuScanEvent == SCAN_EVENT_ABORTED) {
+			down(&(priv->hSemScanReq));
+
+			PRINT_D(CFG80211_DBG, "Scan Aborted \n");
+			if (priv->pstrScanReq != NULL) {
+
+				update_scan_time(priv);
+				refresh_scan(priv, 1, false);
+
+				cfg80211_scan_done(priv->pstrScanReq, false);
+				priv->bCfgScanning = false;
+				priv->pstrScanReq = NULL;
+			}
+			up(&(priv->hSemScanReq));
+		}
+	}
+
+
+	WILC_CATCH(s32Error)
+	{
+	}
+}
+
+
+/**
+ *  @brief      WILC_WFI_Set_PMKSA
+ *  @details  Check if pmksa is cached and set it.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_Set_PMKSA(u8 *bssid, struct WILC_WFI_priv *priv)
+{
+	u32 i;
+	s32 s32Error = WILC_SUCCESS;
+
+
+	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
+
+		if (!WILC_memcmp(bssid, priv->pmkid_list.pmkidlist[i].bssid,
+				 ETH_ALEN)) {
+			PRINT_D(CFG80211_DBG, "PMKID successful comparison");
+
+			/*If bssid is found, set the values*/
+			s32Error = host_int_set_pmkid_info(priv->hWILCWFIDrv, &priv->pmkid_list);
+
+			if (s32Error != WILC_SUCCESS)
+				PRINT_ER("Error in pmkid\n");
+
+			break;
+		}
+	}
+
+	return s32Error;
+
+
+}
+int linux_wlan_set_bssid(struct net_device *wilc_netdev, uint8_t *pBSSID);
+
+
+/**
+ *  @brief      CfgConnectResult
+ *  @details
+ *  @param[in] tenuConnDisconnEvent enuConnDisconnEvent: Type of connection response either
+ *                        connection response or disconnection notification.
+ *                        tstrConnectInfo* pstrConnectInfo: COnnection information.
+ *                        u8 u8MacStatus: Mac Status from firmware
+ *                        tstrDisconnectNotifInfo* pstrDisconnectNotifInfo: Disconnection Notification
+ *                        void* pUserVoid: Private data associated with wireless interface
+ *  @return     NONE
+ *  @author	mabubakr
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int connecting;
+
+static void CfgConnectResult(tenuConnDisconnEvent enuConnDisconnEvent,
+			     tstrConnectInfo *pstrConnectInfo,
+			     u8 u8MacStatus,
+			     tstrDisconnectNotifInfo *pstrDisconnectNotifInfo,
+			     void *pUserVoid)
+{
+	struct WILC_WFI_priv *priv;
+	struct net_device *dev;
+	#ifdef WILC_P2P
+	tstrWILC_WFIDrv *pstrWFIDrv;
+	#endif
+	u8 NullBssid[ETH_ALEN] = {0};
+	connecting = 0;
+
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+	dev = priv->dev;
+	#ifdef WILC_P2P
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+	#endif
+
+	if (enuConnDisconnEvent == CONN_DISCONN_EVENT_CONN_RESP) {
+		/*Initialization*/
+		u16 u16ConnectStatus = WLAN_STATUS_SUCCESS;
+
+		u16ConnectStatus = pstrConnectInfo->u16ConnectStatus;
+
+		PRINT_D(CFG80211_DBG, " Connection response received = %d\n", u8MacStatus);
+
+		if ((u8MacStatus == MAC_DISCONNECTED) &&
+		    (pstrConnectInfo->u16ConnectStatus == SUCCESSFUL_STATUSCODE)) {
+			/* The case here is that our station was waiting for association response frame and has just received it containing status code
+			 *  = SUCCESSFUL_STATUSCODE, while mac status is MAC_DISCONNECTED (which means something wrong happened) */
+			u16ConnectStatus = WLAN_STATUS_UNSPECIFIED_FAILURE;
+			linux_wlan_set_bssid(priv->dev, NullBssid);
+			WILC_memset(u8ConnectedSSID, 0, ETH_ALEN);
+
+			/*BugID_5457*/
+			/*Invalidate u8WLANChannel value on wlan0 disconnect*/
+			#ifdef WILC_P2P
+			if (!pstrWFIDrv->u8P2PConnect)
+				u8WLANChannel = INVALID_CHANNEL;
+			#endif
+
+			PRINT_ER("Unspecified failure: Connection status %d : MAC status = %d \n", u16ConnectStatus, u8MacStatus);
+		}
+
+		if (u16ConnectStatus == WLAN_STATUS_SUCCESS) {
+			bool bNeedScanRefresh = false;
+			u32 i;
+
+			PRINT_INFO(CFG80211_DBG, "Connection Successful:: BSSID: %x%x%x%x%x%x\n", pstrConnectInfo->au8bssid[0],
+				   pstrConnectInfo->au8bssid[1], pstrConnectInfo->au8bssid[2], pstrConnectInfo->au8bssid[3], pstrConnectInfo->au8bssid[4], pstrConnectInfo->au8bssid[5]);
+			WILC_memcpy(priv->au8AssociatedBss, pstrConnectInfo->au8bssid, ETH_ALEN);
+
+			/* BugID_4209: if this network has expired in the scan results in the above nl80211 layer, refresh them here by calling
+			 *  cfg80211_inform_bss() with the last Scan results before calling cfg80211_connect_result() to avoid
+			 *  Linux kernel warning generated at the nl80211 layer */
+
+			for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
+				if (WILC_memcmp(astrLastScannedNtwrksShadow[i].au8bssid,
+						pstrConnectInfo->au8bssid, ETH_ALEN) == 0) {
+					unsigned long now = jiffies;
+
+					if (time_after(now,
+						       astrLastScannedNtwrksShadow[i].u32TimeRcvdInScanCached + (unsigned long)(nl80211_SCAN_RESULT_EXPIRE - (1 * HZ)))) {
+						bNeedScanRefresh = true;
+					}
+
+					break;
+				}
+			}
+
+			if (bNeedScanRefresh) {
+				/*BugID_5418*/
+				/*Also, refrsh DIRECT- results if */
+				refresh_scan(priv, 1, true);
+
+			}
+
+		}
+
+
+		PRINT_D(CFG80211_DBG, "Association request info elements length = %zu\n", pstrConnectInfo->ReqIEsLen);
+
+		PRINT_D(CFG80211_DBG, "Association response info elements length = %d\n", pstrConnectInfo->u16RespIEsLen);
+
+		cfg80211_connect_result(dev, pstrConnectInfo->au8bssid,
+					pstrConnectInfo->pu8ReqIEs, pstrConnectInfo->ReqIEsLen,
+					pstrConnectInfo->pu8RespIEs, pstrConnectInfo->u16RespIEsLen,
+					u16ConnectStatus, GFP_KERNEL);                         /* TODO: mostafa: u16ConnectStatus to */
+		/* be replaced by pstrConnectInfo->u16ConnectStatus */
+	} else if (enuConnDisconnEvent == CONN_DISCONN_EVENT_DISCONN_NOTIF)    {
+		#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+		g_obtainingIP = false;
+		#endif
+		PRINT_ER("Received MAC_DISCONNECTED from firmware with reason %d on dev [%p]\n",
+			 pstrDisconnectNotifInfo->u16reason, priv->dev);
+		u8P2Plocalrandom = 0x01;
+		u8P2Precvrandom = 0x00;
+		bWilc_ie = false;
+		WILC_memset(priv->au8AssociatedBss, 0, ETH_ALEN);
+		linux_wlan_set_bssid(priv->dev, NullBssid);
+		WILC_memset(u8ConnectedSSID, 0, ETH_ALEN);
+
+		/*BugID_5457*/
+		/*Invalidate u8WLANChannel value on wlan0 disconnect*/
+		#ifdef WILC_P2P
+		if (!pstrWFIDrv->u8P2PConnect)
+			u8WLANChannel = INVALID_CHANNEL;
+		#endif
+		/*BugID_5315*/
+		/*Incase "P2P CLIENT Connected" send deauthentication reason by 3 to force the WPA_SUPPLICANT to directly change
+		 *      virtual interface to station*/
+		if ((pstrWFIDrv->IFC_UP) && (dev == g_linux_wlan->strInterfaceInfo[1].wilc_netdev)) {
+			pstrDisconnectNotifInfo->u16reason = 3;
+		}
+		/*BugID_5315*/
+		/*Incase "P2P CLIENT during connection(not connected)" send deauthentication reason by 1 to force the WPA_SUPPLICANT
+		 *      to scan again and retry the connection*/
+		else if ((!pstrWFIDrv->IFC_UP) && (dev == g_linux_wlan->strInterfaceInfo[1].wilc_netdev)) {
+			pstrDisconnectNotifInfo->u16reason = 1;
+		}
+		cfg80211_disconnected(dev, pstrDisconnectNotifInfo->u16reason, pstrDisconnectNotifInfo->ie,
+				      pstrDisconnectNotifInfo->ie_len, GFP_KERNEL);
+
+	}
+
+}
+
+
+/**
+ *  @brief      WILC_WFI_CfgSetChannel
+ *  @details    Set channel for a given wireless interface. Some devices
+ *                      may support multi-channel operation (by channel hopping) so cfg80211
+ *                      doesn't verify much. Note, however, that the passed netdev may be
+ *                      %NULL as well if the user requested changing the channel for the
+ *                      device itself, or for a monitor interface.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_CfgSetChannel(struct wiphy *wiphy,
+				  struct cfg80211_chan_def *chandef)
+{
+
+	u32 channelnum = 0;
+	struct WILC_WFI_priv *priv;
+	s32 s32Error = WILC_SUCCESS;
+	priv = wiphy_priv(wiphy);
+
+	channelnum = ieee80211_frequency_to_channel(chandef->chan->center_freq);
+	PRINT_D(CFG80211_DBG, "Setting channel %d with frequency %d\n", channelnum, chandef->chan->center_freq);
+
+	u8CurrChannel = channelnum;
+	s32Error   = host_int_set_mac_chnl_num(priv->hWILCWFIDrv, channelnum);
+
+	if (s32Error != WILC_SUCCESS)
+		PRINT_ER("Error in setting channel %d\n", channelnum);
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_CfgScan
+ *  @details    Request to do a scan. If returning zero, the scan request is given
+ *                      the driver, and will be valid until passed to cfg80211_scan_done().
+ *                      For scan results, call cfg80211_inform_bss(); you can call this outside
+ *                      the scan/scan_done bracket too.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mabubakr
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+
+/*
+ *	kernel version 3.8.8 supported
+ *	tony, sswd, WILC-KR, 2013-10-29
+ */
+static int WILC_WFI_CfgScan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
+{
+	struct WILC_WFI_priv *priv;
+	u32 i;
+	s32 s32Error = WILC_SUCCESS;
+	u8 au8ScanChanList[MAX_NUM_SCANNED_NETWORKS];
+	tstrHiddenNetwork strHiddenNetwork;
+
+	priv = wiphy_priv(wiphy);
+
+	priv->pstrScanReq = request;
+
+	priv->u32RcvdChCount = 0;
+
+	host_int_set_wfi_drv_handler((u32)priv->hWILCWFIDrv);
+
+
+	reset_shadow_found(priv);
+
+	priv->bCfgScanning = true;
+	if (request->n_channels <= MAX_NUM_SCANNED_NETWORKS) { /* TODO: mostafa: to be replaced by */
+		/*               max_scan_ssids */
+		for (i = 0; i < request->n_channels; i++) {
+			au8ScanChanList[i] = (u8)ieee80211_frequency_to_channel(request->channels[i]->center_freq);
+			PRINT_INFO(CFG80211_DBG, "ScanChannel List[%d] = %d,", i, au8ScanChanList[i]);
+		}
+
+		PRINT_D(CFG80211_DBG, "Requested num of scan channel %d\n", request->n_channels);
+		PRINT_D(CFG80211_DBG, "Scan Request IE len =  %zu\n", request->ie_len);
+
+		PRINT_D(CFG80211_DBG, "Number of SSIDs %d\n", request->n_ssids);
+
+		if (request->n_ssids >= 1) {
+
+
+			strHiddenNetwork.pstrHiddenNetworkInfo = WILC_MALLOC(request->n_ssids * sizeof(tstrHiddenNetwork));
+			strHiddenNetwork.u8ssidnum = request->n_ssids;
+
+
+			/*BugID_4156*/
+			for (i = 0; i < request->n_ssids; i++) {
+
+				if (request->ssids[i].ssid != NULL && request->ssids[i].ssid_len != 0) {
+					strHiddenNetwork.pstrHiddenNetworkInfo[i].pu8ssid = WILC_MALLOC(request->ssids[i].ssid_len);
+					WILC_memcpy(strHiddenNetwork.pstrHiddenNetworkInfo[i].pu8ssid, request->ssids[i].ssid, request->ssids[i].ssid_len);
+					strHiddenNetwork.pstrHiddenNetworkInfo[i].u8ssidlen = request->ssids[i].ssid_len;
+				} else {
+					PRINT_D(CFG80211_DBG, "Received one NULL SSID \n");
+					strHiddenNetwork.u8ssidnum -= 1;
+				}
+			}
+			PRINT_D(CFG80211_DBG, "Trigger Scan Request \n");
+			s32Error = host_int_scan(priv->hWILCWFIDrv, USER_SCAN, ACTIVE_SCAN,
+						 au8ScanChanList, request->n_channels,
+						 (const u8 *)request->ie, request->ie_len,
+						 CfgScanResult, (void *)priv, &strHiddenNetwork);
+		} else {
+			PRINT_D(CFG80211_DBG, "Trigger Scan Request \n");
+			s32Error = host_int_scan(priv->hWILCWFIDrv, USER_SCAN, ACTIVE_SCAN,
+						 au8ScanChanList, request->n_channels,
+						 (const u8 *)request->ie, request->ie_len,
+						 CfgScanResult, (void *)priv, NULL);
+		}
+
+	} else {
+		PRINT_ER("Requested num of scanned channels is greater than the max, supported"
+			 " channels \n");
+	}
+
+	if (s32Error != WILC_SUCCESS) {
+		s32Error = -EBUSY;
+		PRINT_WRN(CFG80211_DBG, "Device is busy: Error(%d)\n", s32Error);
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_CfgConnect
+ *  @details    Connect to the ESS with the specified parameters. When connected,
+ *                      call cfg80211_connect_result() with status code %WLAN_STATUS_SUCCESS.
+ *                      If the connection fails for some reason, call cfg80211_connect_result()
+ *                      with the status from the AP.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mabubakr
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_CfgConnect(struct wiphy *wiphy, struct net_device *dev,
+			       struct cfg80211_connect_params *sme)
+{
+	s32 s32Error = WILC_SUCCESS;
+	u32 i;
+	u8 u8security = NO_ENCRYPT;
+	AUTHTYPE_T tenuAuth_type = ANY;
+	char *pcgroup_encrypt_val = NULL;
+	char *pccipher_group = NULL;
+	char *pcwpa_version = NULL;
+
+	struct WILC_WFI_priv *priv;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+	tstrNetworkInfo *pstrNetworkInfo = NULL;
+
+
+	connecting = 1;
+	priv = wiphy_priv(wiphy);
+	pstrWFIDrv = (tstrWILC_WFIDrv *)(priv->hWILCWFIDrv);
+
+	host_int_set_wfi_drv_handler((u32)priv->hWILCWFIDrv);
+
+	PRINT_D(CFG80211_DBG, "Connecting to SSID [%s] on netdev [%p] host if [%p]\n", sme->ssid, dev, priv->hWILCWFIDrv);
+	#ifdef WILC_P2P
+	if (!(WILC_strncmp(sme->ssid, "DIRECT-", 7))) {
+		PRINT_D(CFG80211_DBG, "Connected to Direct network,OBSS disabled\n");
+		pstrWFIDrv->u8P2PConnect = 1;
+	} else
+		pstrWFIDrv->u8P2PConnect = 0;
+	#endif
+	PRINT_INFO(CFG80211_DBG, "Required SSID = %s\n , AuthType = %d \n", sme->ssid, sme->auth_type);
+
+	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
+		if ((sme->ssid_len == astrLastScannedNtwrksShadow[i].u8SsidLen) &&
+		    WILC_memcmp(astrLastScannedNtwrksShadow[i].au8ssid,
+				sme->ssid,
+				sme->ssid_len) == 0) {
+			PRINT_INFO(CFG80211_DBG, "Network with required SSID is found %s\n", sme->ssid);
+			if (sme->bssid == NULL)	{
+				/* BSSID is not passed from the user, so decision of matching
+				 * is done by SSID only */
+				PRINT_INFO(CFG80211_DBG, "BSSID is not passed from the user\n");
+				break;
+			} else {
+				/* BSSID is also passed from the user, so decision of matching
+				 * should consider also this passed BSSID */
+				if (WILC_memcmp(astrLastScannedNtwrksShadow[i].au8bssid,
+						sme->bssid,
+						ETH_ALEN) == 0)	{
+					PRINT_INFO(CFG80211_DBG, "BSSID is passed from the user and matched\n");
+					break;
+				}
+			}
+		}
+	}
+
+	if (i < u32LastScannedNtwrksCountShadow) {
+		PRINT_D(CFG80211_DBG, "Required bss is in scan results\n");
+
+		pstrNetworkInfo = &(astrLastScannedNtwrksShadow[i]);
+
+		PRINT_INFO(CFG80211_DBG, "network BSSID to be associated: %x%x%x%x%x%x\n",
+			   pstrNetworkInfo->au8bssid[0], pstrNetworkInfo->au8bssid[1],
+			   pstrNetworkInfo->au8bssid[2], pstrNetworkInfo->au8bssid[3],
+			   pstrNetworkInfo->au8bssid[4], pstrNetworkInfo->au8bssid[5]);
+	} else {
+		s32Error = -ENOENT;
+		if (u32LastScannedNtwrksCountShadow == 0)
+			PRINT_D(CFG80211_DBG, "No Scan results yet\n");
+		else
+			PRINT_D(CFG80211_DBG, "Required bss not in scan results: Error(%d)\n", s32Error);
+
+		goto done;
+	}
+
+	priv->WILC_WFI_wep_default = 0;
+	WILC_memset(priv->WILC_WFI_wep_key, 0, sizeof(priv->WILC_WFI_wep_key));
+	WILC_memset(priv->WILC_WFI_wep_key_len, 0, sizeof(priv->WILC_WFI_wep_key_len));
+
+	PRINT_INFO(CFG80211_DBG, "sme->crypto.wpa_versions=%x\n", sme->crypto.wpa_versions);
+	PRINT_INFO(CFG80211_DBG, "sme->crypto.cipher_group=%x\n", sme->crypto.cipher_group);
+
+	PRINT_INFO(CFG80211_DBG, "sme->crypto.n_ciphers_pairwise=%d\n", sme->crypto.n_ciphers_pairwise);
+
+	if (INFO) {
+		for (i = 0; i < sme->crypto.n_ciphers_pairwise; i++)
+			PRINT_D(CORECONFIG_DBG, "sme->crypto.ciphers_pairwise[%d]=%x\n", i, sme->crypto.ciphers_pairwise[i]);
+	}
+
+	if (sme->crypto.cipher_group != NO_ENCRYPT) {
+		/* To determine the u8security value, first we check the group cipher suite then {in case of WPA or WPA2}
+		 *  we will add to it the pairwise cipher suite(s) */
+		pcwpa_version = "Default";
+		PRINT_D(CORECONFIG_DBG, ">> sme->crypto.wpa_versions: %x\n", sme->crypto.wpa_versions);
+		if (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_WEP40) {
+			u8security = ENCRYPT_ENABLED | WEP;
+			pcgroup_encrypt_val = "WEP40";
+			pccipher_group = "WLAN_CIPHER_SUITE_WEP40";
+			PRINT_INFO(CFG80211_DBG, "WEP Default Key Idx = %d\n", sme->key_idx);
+
+			if (INFO) {
+				for (i = 0; i < sme->key_len; i++)
+					PRINT_D(CORECONFIG_DBG, "WEP Key Value[%d] = %d\n", i, sme->key[i]);
+			}
+			priv->WILC_WFI_wep_default = sme->key_idx;
+			priv->WILC_WFI_wep_key_len[sme->key_idx] = sme->key_len;
+			WILC_memcpy(priv->WILC_WFI_wep_key[sme->key_idx], sme->key, sme->key_len);
+
+			/*BugID_5137*/
+			g_key_wep_params.key_len = sme->key_len;
+			g_key_wep_params.key = WILC_MALLOC(sme->key_len);
+			memcpy(g_key_wep_params.key, sme->key, sme->key_len);
+			g_key_wep_params.key_idx = sme->key_idx;
+			g_wep_keys_saved = true;
+
+			host_int_set_WEPDefaultKeyID(priv->hWILCWFIDrv, sme->key_idx);
+			host_int_add_wep_key_bss_sta(priv->hWILCWFIDrv, sme->key, sme->key_len, sme->key_idx);
+		} else if (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_WEP104)   {
+			u8security = ENCRYPT_ENABLED | WEP | WEP_EXTENDED;
+			pcgroup_encrypt_val = "WEP104";
+			pccipher_group = "WLAN_CIPHER_SUITE_WEP104";
+
+			priv->WILC_WFI_wep_default = sme->key_idx;
+			priv->WILC_WFI_wep_key_len[sme->key_idx] = sme->key_len;
+			WILC_memcpy(priv->WILC_WFI_wep_key[sme->key_idx], sme->key, sme->key_len);
+
+			/*BugID_5137*/
+			g_key_wep_params.key_len = sme->key_len;
+			g_key_wep_params.key = WILC_MALLOC(sme->key_len);
+			memcpy(g_key_wep_params.key, sme->key, sme->key_len);
+			g_key_wep_params.key_idx = sme->key_idx;
+			g_wep_keys_saved = true;
+
+			host_int_set_WEPDefaultKeyID(priv->hWILCWFIDrv, sme->key_idx);
+			host_int_add_wep_key_bss_sta(priv->hWILCWFIDrv, sme->key, sme->key_len, sme->key_idx);
+		} else if (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)   {
+			if (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_TKIP)	{
+				u8security = ENCRYPT_ENABLED | WPA2 | TKIP;
+				pcgroup_encrypt_val = "WPA2_TKIP";
+				pccipher_group = "TKIP";
+			} else {     /* TODO: mostafa: here we assume that any other encryption type is AES */
+				     /* tenuSecurity_t = WPA2_AES; */
+				u8security = ENCRYPT_ENABLED | WPA2 | AES;
+				pcgroup_encrypt_val = "WPA2_AES";
+				pccipher_group = "AES";
+			}
+			pcwpa_version = "WPA_VERSION_2";
+		} else if (sme->crypto.wpa_versions & NL80211_WPA_VERSION_1)   {
+			if (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_TKIP)	{
+				u8security = ENCRYPT_ENABLED | WPA | TKIP;
+				pcgroup_encrypt_val = "WPA_TKIP";
+				pccipher_group = "TKIP";
+			} else {     /* TODO: mostafa: here we assume that any other encryption type is AES */
+				     /* tenuSecurity_t = WPA_AES; */
+				u8security = ENCRYPT_ENABLED | WPA | AES;
+				pcgroup_encrypt_val = "WPA_AES";
+				pccipher_group = "AES";
+
+			}
+			pcwpa_version = "WPA_VERSION_1";
+
+		} else {
+			s32Error = -ENOTSUPP;
+			PRINT_ER("Not supported cipher: Error(%d)\n", s32Error);
+
+			goto done;
+		}
+
+	}
+
+	/* After we set the u8security value from checking the group cipher suite, {in case of WPA or WPA2} we will
+	 *   add to it the pairwise cipher suite(s) */
+	if ((sme->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+	    || (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)) {
+		for (i = 0; i < sme->crypto.n_ciphers_pairwise; i++) {
+			if (sme->crypto.ciphers_pairwise[i] == WLAN_CIPHER_SUITE_TKIP) {
+				u8security = u8security | TKIP;
+			} else {     /* TODO: mostafa: here we assume that any other encryption type is AES */
+				u8security = u8security | AES;
+			}
+		}
+	}
+
+	PRINT_D(CFG80211_DBG, "Adding key with cipher group = %x\n", sme->crypto.cipher_group);
+
+	PRINT_D(CFG80211_DBG, "Authentication Type = %d\n", sme->auth_type);
+	switch (sme->auth_type)	{
+	case NL80211_AUTHTYPE_OPEN_SYSTEM:
+		PRINT_D(CFG80211_DBG, "In OPEN SYSTEM\n");
+		tenuAuth_type = OPEN_SYSTEM;
+		break;
+
+	case NL80211_AUTHTYPE_SHARED_KEY:
+		tenuAuth_type = SHARED_KEY;
+		PRINT_D(CFG80211_DBG, "In SHARED KEY\n");
+		break;
+
+	default:
+		PRINT_D(CFG80211_DBG, "Automatic Authentation type = %d\n", sme->auth_type);
+	}
+
+
+	/* ai: key_mgmt: enterprise case */
+	if (sme->crypto.n_akm_suites) {
+		switch (sme->crypto.akm_suites[0]) {
+		case WLAN_AKM_SUITE_8021X:
+			tenuAuth_type = IEEE8021;
+			break;
+
+		default:
+			break;
+		}
+	}
+
+
+	PRINT_INFO(CFG80211_DBG, "Required Channel = %d\n", pstrNetworkInfo->u8channel);
+
+	PRINT_INFO(CFG80211_DBG, "Group encryption value = %s\n Cipher Group = %s\n WPA version = %s\n",
+		   pcgroup_encrypt_val, pccipher_group, pcwpa_version);
+
+	/*BugID_5442*/
+	u8CurrChannel = pstrNetworkInfo->u8channel;
+
+	if (!pstrWFIDrv->u8P2PConnect) {
+		u8WLANChannel = pstrNetworkInfo->u8channel;
+	}
+
+	linux_wlan_set_bssid(dev, pstrNetworkInfo->au8bssid);
+
+	s32Error = host_int_set_join_req(priv->hWILCWFIDrv, pstrNetworkInfo->au8bssid, sme->ssid,
+					 sme->ssid_len, sme->ie, sme->ie_len,
+					 CfgConnectResult, (void *)priv, u8security,
+					 tenuAuth_type, pstrNetworkInfo->u8channel,
+					 pstrNetworkInfo->pJoinParams);
+	if (s32Error != WILC_SUCCESS) {
+		PRINT_ER("host_int_set_join_req(): Error(%d) \n", s32Error);
+		s32Error = -ENOENT;
+		goto done;
+	}
+
+done:
+
+	return s32Error;
+}
+
+
+/**
+ *  @brief      WILC_WFI_disconnect
+ *  @details    Disconnect from the BSS/ESS.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_disconnect(struct wiphy *wiphy, struct net_device *dev, u16 reason_code)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	#ifdef WILC_P2P
+	tstrWILC_WFIDrv *pstrWFIDrv;
+	#endif
+	uint8_t NullBssid[ETH_ALEN] = {0};
+	connecting = 0;
+	priv = wiphy_priv(wiphy);
+
+	/*BugID_5457*/
+	/*Invalidate u8WLANChannel value on wlan0 disconnect*/
+	#ifdef WILC_P2P
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+	if (!pstrWFIDrv->u8P2PConnect)
+		u8WLANChannel = INVALID_CHANNEL;
+	#endif
+	linux_wlan_set_bssid(priv->dev, NullBssid);
+
+	PRINT_D(CFG80211_DBG, "Disconnecting with reason code(%d)\n", reason_code);
+
+	u8P2Plocalrandom = 0x01;
+	u8P2Precvrandom = 0x00;
+	bWilc_ie = false;
+	#ifdef WILC_P2P
+	pstrWFIDrv->u64P2p_MgmtTimeout = 0;
+	#endif
+
+	s32Error = host_int_disconnect(priv->hWILCWFIDrv, reason_code);
+	if (s32Error != WILC_SUCCESS) {
+		PRINT_ER("Error in disconnecting: Error(%d)\n", s32Error);
+		s32Error = -EINVAL;
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_add_key
+ *  @details    Add a key with the given parameters. @mac_addr will be %NULL
+ *                      when adding a group key.
+ *  @param[in] key : key buffer; TKIP: 16-byte temporal key, 8-byte Tx Mic key, 8-byte Rx Mic Key
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
+			    bool pairwise,
+			    const u8 *mac_addr, struct key_params *params)
+
+{
+	s32 s32Error = WILC_SUCCESS, KeyLen = params->key_len;
+	u32 i;
+	struct WILC_WFI_priv *priv;
+	const u8 *pu8RxMic = NULL;
+	const u8 *pu8TxMic = NULL;
+	u8 u8mode = NO_ENCRYPT;
+	#ifdef WILC_AP_EXTERNAL_MLME
+	u8 u8gmode = NO_ENCRYPT;
+	u8 u8pmode = NO_ENCRYPT;
+	AUTHTYPE_T tenuAuth_type = ANY;
+	#endif
+
+	priv = wiphy_priv(wiphy);
+
+	PRINT_D(CFG80211_DBG, "Adding key with cipher suite = %x\n", params->cipher);
+
+	/*BugID_5137*/
+	PRINT_D(CFG80211_DBG, "%p %p %d\n", wiphy, netdev, key_index);
+
+	PRINT_D(CFG80211_DBG, "key %x %x %x\n", params->key[0],
+		params->key[1],
+		params->key[2]);
+
+
+	switch (params->cipher)	{
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+				#ifdef WILC_AP_EXTERNAL_MLME
+		if (priv->wdev->iftype == NL80211_IFTYPE_AP) {
+
+			priv->WILC_WFI_wep_default = key_index;
+			priv->WILC_WFI_wep_key_len[key_index] = params->key_len;
+			WILC_memcpy(priv->WILC_WFI_wep_key[key_index], params->key, params->key_len);
+
+			PRINT_D(CFG80211_DBG, "Adding AP WEP Default key Idx = %d\n", key_index);
+			PRINT_D(CFG80211_DBG, "Adding AP WEP Key len= %d\n", params->key_len);
+
+			for (i = 0; i < params->key_len; i++)
+				PRINT_D(CFG80211_DBG, "WEP AP key val[%d] = %x\n", i, params->key[i]);
+
+			tenuAuth_type = OPEN_SYSTEM;
+
+			if (params->cipher == WLAN_CIPHER_SUITE_WEP40)
+				u8mode = ENCRYPT_ENABLED | WEP;
+			else
+				u8mode = ENCRYPT_ENABLED | WEP | WEP_EXTENDED;
+
+			host_int_add_wep_key_bss_ap(priv->hWILCWFIDrv, params->key, params->key_len, key_index, u8mode, tenuAuth_type);
+			break;
+		}
+				#endif
+		if (WILC_memcmp(params->key, priv->WILC_WFI_wep_key[key_index], params->key_len)) {
+			priv->WILC_WFI_wep_default = key_index;
+			priv->WILC_WFI_wep_key_len[key_index] = params->key_len;
+			WILC_memcpy(priv->WILC_WFI_wep_key[key_index], params->key, params->key_len);
+
+			PRINT_D(CFG80211_DBG, "Adding WEP Default key Idx = %d\n", key_index);
+			PRINT_D(CFG80211_DBG, "Adding WEP Key length = %d\n", params->key_len);
+			if (INFO) {
+				for (i = 0; i < params->key_len; i++)
+					PRINT_INFO(CFG80211_DBG, "WEP key value[%d] = %d\n", i, params->key[i]);
+			}
+			host_int_add_wep_key_bss_sta(priv->hWILCWFIDrv, params->key, params->key_len, key_index);
+		}
+
+		break;
+
+	case WLAN_CIPHER_SUITE_TKIP:
+	case WLAN_CIPHER_SUITE_CCMP:
+				#ifdef WILC_AP_EXTERNAL_MLME
+		if (priv->wdev->iftype == NL80211_IFTYPE_AP || priv->wdev->iftype == NL80211_IFTYPE_P2P_GO) {
+
+			if (priv->wilc_gtk[key_index] == NULL) {
+				priv->wilc_gtk[key_index] = (struct wilc_wfi_key *)WILC_MALLOC(sizeof(struct wilc_wfi_key));
+				priv->wilc_gtk[key_index]->key = NULL;
+				priv->wilc_gtk[key_index]->seq = NULL;
+
+			}
+			if (priv->wilc_ptk[key_index] == NULL) {
+				priv->wilc_ptk[key_index] = (struct wilc_wfi_key *)WILC_MALLOC(sizeof(struct wilc_wfi_key));
+				priv->wilc_ptk[key_index]->key = NULL;
+				priv->wilc_ptk[key_index]->seq = NULL;
+			}
+
+
+
+			if (!pairwise)
+			{
+				if (params->cipher == WLAN_CIPHER_SUITE_TKIP)
+					u8gmode = ENCRYPT_ENABLED | WPA | TKIP;
+				else
+					u8gmode = ENCRYPT_ENABLED | WPA2 | AES;
+
+				priv->wilc_groupkey = u8gmode;
+
+				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
+
+					pu8TxMic = params->key + 24;
+					pu8RxMic = params->key + 16;
+					KeyLen = params->key_len - 16;
+				}
+				/* if there has been previous allocation for the same index through its key, free that memory and allocate again*/
+				if (priv->wilc_gtk[key_index]->key)
+					WILC_FREE(priv->wilc_gtk[key_index]->key);
+
+				priv->wilc_gtk[key_index]->key = (u8 *)WILC_MALLOC(params->key_len);
+				WILC_memcpy(priv->wilc_gtk[key_index]->key, params->key, params->key_len);
+
+				/* if there has been previous allocation for the same index through its seq, free that memory and allocate again*/
+				if (priv->wilc_gtk[key_index]->seq)
+					WILC_FREE(priv->wilc_gtk[key_index]->seq);
+
+				if ((params->seq_len) > 0) {
+					priv->wilc_gtk[key_index]->seq = (u8 *)WILC_MALLOC(params->seq_len);
+					WILC_memcpy(priv->wilc_gtk[key_index]->seq, params->seq, params->seq_len);
+				}
+
+				priv->wilc_gtk[key_index]->cipher = params->cipher;
+				priv->wilc_gtk[key_index]->key_len = params->key_len;
+				priv->wilc_gtk[key_index]->seq_len = params->seq_len;
+
+				if (INFO) {
+					for (i = 0; i < params->key_len; i++)
+						PRINT_INFO(CFG80211_DBG, "Adding group key value[%d] = %x\n", i, params->key[i]);
+					for (i = 0; i < params->seq_len; i++)
+						PRINT_INFO(CFG80211_DBG, "Adding group seq value[%d] = %x\n", i, params->seq[i]);
+				}
+
+
+				host_int_add_rx_gtk(priv->hWILCWFIDrv, params->key, KeyLen,
+						    key_index, params->seq_len, params->seq, pu8RxMic, pu8TxMic, AP_MODE, u8gmode);
+
+			} else {
+				PRINT_INFO(CFG80211_DBG, "STA Address: %x%x%x%x%x\n", mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4]);
+
+				if (params->cipher == WLAN_CIPHER_SUITE_TKIP)
+					u8pmode = ENCRYPT_ENABLED | WPA | TKIP;
+				else
+					u8pmode = priv->wilc_groupkey | AES;
+
+
+				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
+
+					pu8TxMic = params->key + 24;
+					pu8RxMic = params->key + 16;
+					KeyLen = params->key_len - 16;
+				}
+
+				if (priv->wilc_ptk[key_index]->key)
+					WILC_FREE(priv->wilc_ptk[key_index]->key);
+
+				priv->wilc_ptk[key_index]->key = (u8 *)WILC_MALLOC(params->key_len);
+
+				if (priv->wilc_ptk[key_index]->seq)
+					WILC_FREE(priv->wilc_ptk[key_index]->seq);
+
+				if ((params->seq_len) > 0)
+					priv->wilc_ptk[key_index]->seq = (u8 *)WILC_MALLOC(params->seq_len);
+
+				if (INFO) {
+					for (i = 0; i < params->key_len; i++)
+						PRINT_INFO(CFG80211_DBG, "Adding pairwise key value[%d] = %x\n", i, params->key[i]);
+
+					for (i = 0; i < params->seq_len; i++)
+						PRINT_INFO(CFG80211_DBG, "Adding group seq value[%d] = %x\n", i, params->seq[i]);
+				}
+
+				WILC_memcpy(priv->wilc_ptk[key_index]->key, params->key, params->key_len);
+
+				if ((params->seq_len) > 0)
+					WILC_memcpy(priv->wilc_ptk[key_index]->seq, params->seq, params->seq_len);
+
+				priv->wilc_ptk[key_index]->cipher = params->cipher;
+				priv->wilc_ptk[key_index]->key_len = params->key_len;
+				priv->wilc_ptk[key_index]->seq_len = params->seq_len;
+
+				host_int_add_ptk(priv->hWILCWFIDrv, params->key, KeyLen, mac_addr,
+						 pu8RxMic, pu8TxMic, AP_MODE, u8pmode, key_index);
+			}
+			break;
+		}
+				#endif
+
+		{
+			u8mode = 0;
+			if (!pairwise)
+			{
+				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
+					/* swap the tx mic by rx mic */
+					pu8RxMic = params->key + 24;
+					pu8TxMic = params->key + 16;
+					KeyLen = params->key_len - 16;
+				}
+
+				/*BugID_5137*/
+				/*save keys only on interface 0 (wifi interface)*/
+				if (!g_gtk_keys_saved && netdev == g_linux_wlan->strInterfaceInfo[0].wilc_netdev) {
+					g_add_gtk_key_params.key_idx = key_index;
+					g_add_gtk_key_params.pairwise = pairwise;
+					if (!mac_addr) {
+						g_add_gtk_key_params.mac_addr = NULL;
+					} else {
+						g_add_gtk_key_params.mac_addr = WILC_MALLOC(ETH_ALEN);
+						memcpy(g_add_gtk_key_params.mac_addr, mac_addr, ETH_ALEN);
+					}
+					g_key_gtk_params.key_len = params->key_len;
+					g_key_gtk_params.seq_len = params->seq_len;
+					g_key_gtk_params.key =  WILC_MALLOC(params->key_len);
+					memcpy(g_key_gtk_params.key, params->key, params->key_len);
+					if (params->seq_len > 0) {
+						g_key_gtk_params.seq =  WILC_MALLOC(params->seq_len);
+						memcpy(g_key_gtk_params.seq, params->seq, params->seq_len);
+					}
+					g_key_gtk_params.cipher = params->cipher;
+
+					PRINT_D(CFG80211_DBG, "key %x %x %x\n", g_key_gtk_params.key[0],
+						g_key_gtk_params.key[1],
+						g_key_gtk_params.key[2]);
+					g_gtk_keys_saved = true;
+				}
+
+				host_int_add_rx_gtk(priv->hWILCWFIDrv, params->key, KeyLen,
+						    key_index, params->seq_len, params->seq, pu8RxMic, pu8TxMic, STATION_MODE, u8mode);
+			} else {
+				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
+					/* swap the tx mic by rx mic */
+					pu8RxMic = params->key + 24;
+					pu8TxMic = params->key + 16;
+					KeyLen = params->key_len - 16;
+				}
+
+				/*BugID_5137*/
+				/*save keys only on interface 0 (wifi interface)*/
+				if (!g_ptk_keys_saved && netdev == g_linux_wlan->strInterfaceInfo[0].wilc_netdev) {
+					g_add_ptk_key_params.key_idx = key_index;
+					g_add_ptk_key_params.pairwise = pairwise;
+					if (!mac_addr) {
+						g_add_ptk_key_params.mac_addr = NULL;
+					} else {
+						g_add_ptk_key_params.mac_addr = WILC_MALLOC(ETH_ALEN);
+						memcpy(g_add_ptk_key_params.mac_addr, mac_addr, ETH_ALEN);
+					}
+					g_key_ptk_params.key_len = params->key_len;
+					g_key_ptk_params.seq_len = params->seq_len;
+					g_key_ptk_params.key =  WILC_MALLOC(params->key_len);
+					memcpy(g_key_ptk_params.key, params->key, params->key_len);
+					if (params->seq_len > 0) {
+						g_key_ptk_params.seq =  WILC_MALLOC(params->seq_len);
+						memcpy(g_key_ptk_params.seq, params->seq, params->seq_len);
+					}
+					g_key_ptk_params.cipher = params->cipher;
+
+					PRINT_D(CFG80211_DBG, "key %x %x %x\n", g_key_ptk_params.key[0],
+						g_key_ptk_params.key[1],
+						g_key_ptk_params.key[2]);
+					g_ptk_keys_saved = true;
+				}
+
+				host_int_add_ptk(priv->hWILCWFIDrv, params->key, KeyLen, mac_addr,
+						 pu8RxMic, pu8TxMic, STATION_MODE, u8mode, key_index);
+				PRINT_D(CFG80211_DBG, "Adding pairwise key\n");
+				if (INFO) {
+					for (i = 0; i < params->key_len; i++)
+						PRINT_INFO(CFG80211_DBG, "Adding pairwise key value[%d] = %d\n", i, params->key[i]);
+				}
+			}
+		}
+		break;
+
+	default:
+		PRINT_ER("Not supported cipher: Error(%d)\n", s32Error);
+		s32Error = -ENOTSUPP;
+
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_del_key
+ *  @details    Remove a key given the @mac_addr (%NULL for a group key)
+ *                      and @key_index, return -ENOENT if the key doesn't exist.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_del_key(struct wiphy *wiphy, struct net_device *netdev,
+			    u8 key_index,
+			    bool pairwise,
+			    const u8 *mac_addr)
+{
+	struct WILC_WFI_priv *priv;
+	s32 s32Error = WILC_SUCCESS;
+
+	priv = wiphy_priv(wiphy);
+
+	/*BugID_5137*/
+	/*delete saved keys, if any*/
+	if (netdev == g_linux_wlan->strInterfaceInfo[0].wilc_netdev) {
+		g_ptk_keys_saved = false;
+		g_gtk_keys_saved = false;
+		g_wep_keys_saved = false;
+
+		/*Delete saved WEP keys params, if any*/
+		if (g_key_wep_params.key != NULL) {
+			WILC_FREE(g_key_wep_params.key);
+			g_key_wep_params.key = NULL;
+		}
+
+		/*freeing memory allocated by "wilc_gtk" and "wilc_ptk" in "WILC_WIFI_ADD_KEY"*/
+
+	#ifdef WILC_AP_EXTERNAL_MLME
+		if ((priv->wilc_gtk[key_index]) != NULL) {
+
+			if (priv->wilc_gtk[key_index]->key != NULL) {
+
+				WILC_FREE(priv->wilc_gtk[key_index]->key);
+				priv->wilc_gtk[key_index]->key = NULL;
+			}
+			if (priv->wilc_gtk[key_index]->seq) {
+
+				WILC_FREE(priv->wilc_gtk[key_index]->seq);
+				priv->wilc_gtk[key_index]->seq = NULL;
+			}
+
+			WILC_FREE(priv->wilc_gtk[key_index]);
+			priv->wilc_gtk[key_index] = NULL;
+
+		}
+
+		if ((priv->wilc_ptk[key_index]) != NULL) {
+
+			if (priv->wilc_ptk[key_index]->key) {
+
+				WILC_FREE(priv->wilc_ptk[key_index]->key);
+				priv->wilc_ptk[key_index]->key = NULL;
+			}
+			if (priv->wilc_ptk[key_index]->seq) {
+
+				WILC_FREE(priv->wilc_ptk[key_index]->seq);
+				priv->wilc_ptk[key_index]->seq = NULL;
+			}
+			WILC_FREE(priv->wilc_ptk[key_index]);
+			priv->wilc_ptk[key_index] = NULL;
+		}
+	#endif
+
+		/*Delete saved PTK and GTK keys params, if any*/
+		if (g_key_ptk_params.key != NULL) {
+			WILC_FREE(g_key_ptk_params.key);
+			g_key_ptk_params.key = NULL;
+		}
+		if (g_key_ptk_params.seq != NULL) {
+			WILC_FREE(g_key_ptk_params.seq);
+			g_key_ptk_params.seq = NULL;
+		}
+
+		if (g_key_gtk_params.key != NULL) {
+			WILC_FREE(g_key_gtk_params.key);
+			g_key_gtk_params.key = NULL;
+		}
+		if (g_key_gtk_params.seq != NULL) {
+			WILC_FREE(g_key_gtk_params.seq);
+			g_key_gtk_params.seq = NULL;
+		}
+
+		/*Reset WILC_CHANGING_VIR_IF register to allow adding futrue keys to CE H/W*/
+		Set_machw_change_vir_if(false);
+	}
+
+	if (key_index >= 0 && key_index <= 3) {
+		WILC_memset(priv->WILC_WFI_wep_key[key_index], 0, priv->WILC_WFI_wep_key_len[key_index]);
+		priv->WILC_WFI_wep_key_len[key_index] = 0;
+
+		PRINT_D(CFG80211_DBG, "Removing WEP key with index = %d\n", key_index);
+		host_int_remove_wep_key(priv->hWILCWFIDrv, key_index);
+	} else {
+		PRINT_D(CFG80211_DBG, "Removing all installed keys\n");
+		host_int_remove_key(priv->hWILCWFIDrv, mac_addr);
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_get_key
+ *  @details    Get information about the key with the given parameters.
+ *                      @mac_addr will be %NULL when requesting information for a group
+ *                      key. All pointers given to the @callback function need not be valid
+ *                      after it returns. This function should return an error if it is
+ *                      not possible to retrieve the key, -ENOENT if it doesn't exist.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_get_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
+			    bool pairwise,
+			    const u8 *mac_addr, void *cookie, void (*callback)(void *cookie, struct key_params *))
+{
+
+	s32 s32Error = WILC_SUCCESS;
+
+	struct WILC_WFI_priv *priv;
+	struct  key_params key_params;
+	u32 i;
+	priv = wiphy_priv(wiphy);
+
+
+	if (!pairwise)
+	{
+		PRINT_D(CFG80211_DBG, "Getting group key idx: %x\n", key_index);
+
+		key_params.key = priv->wilc_gtk[key_index]->key;
+		key_params.cipher = priv->wilc_gtk[key_index]->cipher;
+		key_params.key_len = priv->wilc_gtk[key_index]->key_len;
+		key_params.seq = priv->wilc_gtk[key_index]->seq;
+		key_params.seq_len = priv->wilc_gtk[key_index]->seq_len;
+		if (INFO) {
+			for (i = 0; i < key_params.key_len; i++)
+				PRINT_INFO(CFG80211_DBG, "Retrieved key value %x\n", key_params.key[i]);
+		}
+	} else {
+		PRINT_D(CFG80211_DBG, "Getting pairwise  key\n");
+
+		key_params.key = priv->wilc_ptk[key_index]->key;
+		key_params.cipher = priv->wilc_ptk[key_index]->cipher;
+		key_params.key_len = priv->wilc_ptk[key_index]->key_len;
+		key_params.seq = priv->wilc_ptk[key_index]->seq;
+		key_params.seq_len = priv->wilc_ptk[key_index]->seq_len;
+	}
+
+	callback(cookie, &key_params);
+
+	return s32Error;        /* priv->wilc_gtk->key_len ?0 : -ENOENT; */
+}
+
+/**
+ *  @brief      WILC_WFI_set_default_key
+ *  @details    Set the default management frame key on an interface
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_set_default_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
+				    bool unicast, bool multicast)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+
+
+	priv = wiphy_priv(wiphy);
+
+	PRINT_D(CFG80211_DBG, "Setting default key with idx = %d \n", key_index);
+
+	if (key_index != priv->WILC_WFI_wep_default) {
+
+		host_int_set_WEPDefaultKeyID(priv->hWILCWFIDrv, key_index);
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_dump_survey
+ *  @details    Get site survey information
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_dump_survey(struct wiphy *wiphy, struct net_device *netdev,
+				int idx, struct survey_info *info)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+
+	if (idx != 0) {
+		s32Error = -ENOENT;
+		PRINT_ER("Error Idx value doesn't equal zero: Error(%d)\n", s32Error);
+
+	}
+
+	return s32Error;
+}
+
+
+/**
+ *  @brief      WILC_WFI_get_station
+ *  @details    Get station information for the station identified by @mac
+ *  @param[in]   NONE
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+
+extern uint32_t Statisitcs_totalAcks, Statisitcs_DroppedAcks;
+static int WILC_WFI_get_station(struct wiphy *wiphy, struct net_device *dev,
+				const u8 *mac, struct station_info *sinfo)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	perInterface_wlan_t *nic;
+	#ifdef WILC_AP_EXTERNAL_MLME
+	u32 i = 0;
+	u32 associatedsta = 0;
+	u32 inactive_time = 0;
+	#endif
+	priv = wiphy_priv(wiphy);
+	nic = netdev_priv(dev);
+
+	#ifdef WILC_AP_EXTERNAL_MLME
+	if (nic->iftype == AP_MODE || nic->iftype == GO_MODE) {
+		PRINT_D(HOSTAPD_DBG, "Getting station parameters\n");
+
+		PRINT_INFO(HOSTAPD_DBG, ": %x%x%x%x%x\n", mac[0], mac[1], mac[2], mac[3], mac[4]);
+
+		for (i = 0; i < NUM_STA_ASSOCIATED; i++) {
+
+			if (!(memcmp(mac, priv->assoc_stainfo.au8Sta_AssociatedBss[i], ETH_ALEN))) {
+				associatedsta = i;
+				break;
+			}
+
+		}
+
+		if (associatedsta == -1) {
+			s32Error = -ENOENT;
+			PRINT_ER("Station required is not associated : Error(%d)\n", s32Error);
+
+			return s32Error;
+		}
+
+		sinfo->filled |= BIT(NL80211_STA_INFO_INACTIVE_TIME);
+
+		host_int_get_inactive_time(priv->hWILCWFIDrv, mac, &(inactive_time));
+		sinfo->inactive_time = 1000 * inactive_time;
+		PRINT_D(CFG80211_DBG, "Inactive time %d\n", sinfo->inactive_time);
+
+	}
+	#endif
+
+	if (nic->iftype == STATION_MODE) {
+		tstrStatistics strStatistics;
+		host_int_get_statistics(priv->hWILCWFIDrv, &strStatistics);
+
+		/*
+		 * tony: 2013-11-13
+		 * tx_failed introduced more than
+		 * kernel version 3.0.0
+		 */
+		sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL) |
+						BIT( NL80211_STA_INFO_RX_PACKETS) |
+						BIT(NL80211_STA_INFO_TX_PACKETS) |
+						BIT(NL80211_STA_INFO_TX_FAILED) |
+						BIT(NL80211_STA_INFO_TX_BITRATE);
+
+		sinfo->signal		=  strStatistics.s8RSSI;
+		sinfo->rx_packets   =  strStatistics.u32RxCount;
+		sinfo->tx_packets   =  strStatistics.u32TxCount + strStatistics.u32TxFailureCount;
+		sinfo->tx_failed	=  strStatistics.u32TxFailureCount;
+		sinfo->txrate.legacy = strStatistics.u8LinkSpeed * 10;
+
+#ifdef TCP_ENHANCEMENTS
+		if ((strStatistics.u8LinkSpeed > TCP_ACK_FILTER_LINK_SPEED_THRESH) && (strStatistics.u8LinkSpeed != DEFAULT_LINK_SPEED)) {
+			Enable_TCP_ACK_Filter(true);
+		} else if (strStatistics.u8LinkSpeed != DEFAULT_LINK_SPEED)   {
+			Enable_TCP_ACK_Filter(false);
+		}
+#endif
+
+		PRINT_D(CORECONFIG_DBG, "*** stats[%d][%d][%d][%d][%d]\n", sinfo->signal, sinfo->rx_packets, sinfo->tx_packets,
+			sinfo->tx_failed, sinfo->txrate.legacy);
+	}
+	return s32Error;
+}
+
+
+/**
+ *  @brief      WILC_WFI_change_bss
+ *  @details    Modify parameters for a given BSS.
+ *  @param[in]
+ *   -use_cts_prot: Whether to use CTS protection
+ *	    (0 = no, 1 = yes, -1 = do not change)
+ *  -use_short_preamble: Whether the use of short preambles is allowed
+ *	    (0 = no, 1 = yes, -1 = do not change)
+ *  -use_short_slot_time: Whether the use of short slot time is allowed
+ *	    (0 = no, 1 = yes, -1 = do not change)
+ *  -basic_rates: basic rates in IEEE 802.11 format
+ *	    (or NULL for no change)
+ *  -basic_rates_len: number of basic rates
+ *  -ap_isolate: do not forward packets between connected stations
+ *  -ht_opmode: HT Operation mode
+ *         (u16 = opmode, -1 = do not change)
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int  WILC_WFI_change_bss(struct wiphy *wiphy, struct net_device *dev,
+				struct bss_parameters *params)
+{
+	PRINT_D(CFG80211_DBG, "Changing Bss parametrs\n");
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_auth
+ *  @details    Request to authenticate with the specified peer
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_auth(struct wiphy *wiphy, struct net_device *dev,
+			 struct cfg80211_auth_request *req)
+{
+	PRINT_D(CFG80211_DBG, "In Authentication Function\n");
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_assoc
+ *  @details    Request to (re)associate with the specified peer
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_assoc(struct wiphy *wiphy, struct net_device *dev,
+			  struct cfg80211_assoc_request *req)
+{
+	PRINT_D(CFG80211_DBG, "In Association Function\n");
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_deauth
+ *  @details    Request to deauthenticate from the specified peer
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int  WILC_WFI_deauth(struct wiphy *wiphy, struct net_device *dev,
+			    struct cfg80211_deauth_request *req, void *cookie)
+{
+	PRINT_D(CFG80211_DBG, "In De-authentication Function\n");
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_disassoc
+ *  @details    Request to disassociate from the specified peer
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int  WILC_WFI_disassoc(struct wiphy *wiphy, struct net_device *dev,
+			      struct cfg80211_disassoc_request *req, void *cookie)
+{
+	PRINT_D(CFG80211_DBG, "In Disassociation Function\n");
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_set_wiphy_params
+ *  @details    Notify that wiphy parameters have changed;
+ *  @param[in]   Changed bitfield (see &enum wiphy_params_flags) describes which values
+ *                      have changed.
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_set_wiphy_params(struct wiphy *wiphy, u32 changed)
+{
+	s32 s32Error = WILC_SUCCESS;
+	tstrCfgParamVal pstrCfgParamVal;
+	struct WILC_WFI_priv *priv;
+
+	priv = wiphy_priv(wiphy);
+
+	pstrCfgParamVal.u32SetCfgFlag = 0;
+	PRINT_D(CFG80211_DBG, "Setting Wiphy params \n");
+
+	if (changed & WIPHY_PARAM_RETRY_SHORT) {
+		PRINT_D(CFG80211_DBG, "Setting WIPHY_PARAM_RETRY_SHORT %d\n",
+			priv->dev->ieee80211_ptr->wiphy->retry_short);
+		pstrCfgParamVal.u32SetCfgFlag  |= RETRY_SHORT;
+		pstrCfgParamVal.short_retry_limit = priv->dev->ieee80211_ptr->wiphy->retry_short;
+	}
+	if (changed & WIPHY_PARAM_RETRY_LONG) {
+
+		PRINT_D(CFG80211_DBG, "Setting WIPHY_PARAM_RETRY_LONG %d\n", priv->dev->ieee80211_ptr->wiphy->retry_long);
+		pstrCfgParamVal.u32SetCfgFlag |= RETRY_LONG;
+		pstrCfgParamVal.long_retry_limit = priv->dev->ieee80211_ptr->wiphy->retry_long;
+
+	}
+	if (changed & WIPHY_PARAM_FRAG_THRESHOLD) {
+		PRINT_D(CFG80211_DBG, "Setting WIPHY_PARAM_FRAG_THRESHOLD %d\n", priv->dev->ieee80211_ptr->wiphy->frag_threshold);
+		pstrCfgParamVal.u32SetCfgFlag |= FRAG_THRESHOLD;
+		pstrCfgParamVal.frag_threshold = priv->dev->ieee80211_ptr->wiphy->frag_threshold;
+
+	}
+
+	if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
+		PRINT_D(CFG80211_DBG, "Setting WIPHY_PARAM_RTS_THRESHOLD %d\n", priv->dev->ieee80211_ptr->wiphy->rts_threshold);
+
+		pstrCfgParamVal.u32SetCfgFlag |= RTS_THRESHOLD;
+		pstrCfgParamVal.rts_threshold = priv->dev->ieee80211_ptr->wiphy->rts_threshold;
+
+	}
+
+	PRINT_D(CFG80211_DBG, "Setting CFG params in the host interface\n");
+	s32Error = hif_set_cfg(priv->hWILCWFIDrv, &pstrCfgParamVal);
+	if (s32Error)
+		PRINT_ER("Error in setting WIPHY PARAMS\n");
+
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_set_bitrate_mask
+ *  @details    set the bitrate mask configuration
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_set_bitrate_mask(struct wiphy *wiphy,
+				     struct net_device *dev, const u8 *peer,
+				     const struct cfg80211_bitrate_mask *mask)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	PRINT_D(CFG80211_DBG, "Setting Bitrate mask function\n");
+	return s32Error;
+
+}
+
+/**
+ *  @brief      WILC_WFI_set_pmksa
+ *  @details    Cache a PMKID for a BSSID. This is mostly useful for fullmac
+ *                      devices running firmwares capable of generating the (re) association
+ *                      RSN IE. It allows for faster roaming between WPA2 BSSIDs.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_set_pmksa(struct wiphy *wiphy, struct net_device *netdev,
+			      struct cfg80211_pmksa *pmksa)
+{
+	u32 i;
+	s32 s32Error = WILC_SUCCESS;
+	u8 flag = 0;
+
+	struct WILC_WFI_priv *priv = wiphy_priv(wiphy);
+
+	PRINT_D(CFG80211_DBG, "Setting PMKSA\n");
+
+
+	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
+		if (!WILC_memcmp(pmksa->bssid, priv->pmkid_list.pmkidlist[i].bssid,
+				 ETH_ALEN)) {
+			/*If bssid already exists and pmkid value needs to reset*/
+			flag = PMKID_FOUND;
+			PRINT_D(CFG80211_DBG, "PMKID already exists\n");
+			break;
+		}
+	}
+	if (i < WILC_MAX_NUM_PMKIDS) {
+		PRINT_D(CFG80211_DBG, "Setting PMKID in private structure\n");
+		WILC_memcpy(priv->pmkid_list.pmkidlist[i].bssid, pmksa->bssid,
+			    ETH_ALEN);
+		WILC_memcpy(priv->pmkid_list.pmkidlist[i].pmkid, pmksa->pmkid,
+			    PMKID_LEN);
+		if (!(flag == PMKID_FOUND))
+			priv->pmkid_list.numpmkid++;
+	} else {
+		PRINT_ER("Invalid PMKID index\n");
+		s32Error = -EINVAL;
+	}
+
+	if (!s32Error) {
+		PRINT_D(CFG80211_DBG, "Setting pmkid in the host interface\n");
+		s32Error = host_int_set_pmkid_info(priv->hWILCWFIDrv, &priv->pmkid_list);
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_del_pmksa
+ *  @details    Delete a cached PMKID.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_del_pmksa(struct wiphy *wiphy, struct net_device *netdev,
+			      struct cfg80211_pmksa *pmksa)
+{
+
+	u32 i;
+	u8 flag = 0;
+	s32 s32Error = WILC_SUCCESS;
+
+	struct WILC_WFI_priv *priv = wiphy_priv(wiphy);
+
+	PRINT_D(CFG80211_DBG, "Deleting PMKSA keys\n");
+
+	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
+		if (!WILC_memcmp(pmksa->bssid, priv->pmkid_list.pmkidlist[i].bssid,
+				 ETH_ALEN)) {
+			/*If bssid is found, reset the values*/
+			PRINT_D(CFG80211_DBG, "Reseting PMKID values\n");
+			WILC_memset(&priv->pmkid_list.pmkidlist[i], 0, sizeof(tstrHostIFpmkid));
+			flag = PMKID_FOUND;
+			break;
+		}
+	}
+
+	if (i < priv->pmkid_list.numpmkid && priv->pmkid_list.numpmkid > 0) {
+		for (; i < (priv->pmkid_list.numpmkid - 1); i++) {
+			WILC_memcpy(priv->pmkid_list.pmkidlist[i].bssid,
+				    priv->pmkid_list.pmkidlist[i + 1].bssid,
+				    ETH_ALEN);
+			WILC_memcpy(priv->pmkid_list.pmkidlist[i].pmkid,
+				    priv->pmkid_list.pmkidlist[i].pmkid,
+				    PMKID_LEN);
+		}
+		priv->pmkid_list.numpmkid--;
+	} else {
+		s32Error = -EINVAL;
+	}
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_flush_pmksa
+ *  @details    Flush all cached PMKIDs.
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int  WILC_WFI_flush_pmksa(struct wiphy *wiphy, struct net_device *netdev)
+{
+	struct WILC_WFI_priv *priv = wiphy_priv(wiphy);
+
+	PRINT_D(CFG80211_DBG,  "Flushing  PMKID key values\n");
+
+	/*Get cashed Pmkids and set all with zeros*/
+	WILC_memset(&priv->pmkid_list, 0, sizeof(tstrHostIFpmkidAttr));
+
+	return 0;
+}
+
+#ifdef WILC_P2P
+
+/**
+ *  @brief      WILC_WFI_CfgParseRxAction
+ *  @details Function parses the received  frames and modifies the following attributes:
+ *                -GO Intent
+ *		    -Channel list
+ *		    -Operating Channel
+ *
+ *  @param[in] u8* Buffer, u32 length
+ *  @return     NONE.
+ *  @author	mdaftedar
+ *  @date	12 DEC 2012
+ *  @version
+ */
+
+void WILC_WFI_CfgParseRxAction(u8 *buf, u32 len)
+{
+	u32 index = 0;
+	u32 i = 0, j = 0;
+
+	/*BugID_5460*/
+	#ifdef USE_SUPPLICANT_GO_INTENT
+	u8 intent;
+	u8 tie_breaker;
+	bool is_wilc_go = true;
+	#endif
+	u8 op_channel_attr_index = 0;
+	u8 channel_list_attr_index = 0;
+
+	while (index < len) {
+		if (buf[index] == GO_INTENT_ATTR_ID) {
+			#ifdef USE_SUPPLICANT_GO_INTENT
+			/*BugID_5460*/
+			/*Case 1: If we are going to be p2p client, no need to modify channels attributes*/
+			/*In negotiation frames, go intent attr value determines who will be GO*/
+			intent = GET_GO_INTENT(buf[index + 3]);
+			tie_breaker = GET_TIE_BREAKER(buf[index + 3]);
+			if (intent > SUPPLICANT_GO_INTENT
+			    || (intent == SUPPLICANT_GO_INTENT && tie_breaker == 1)) {
+				PRINT_D(GENERIC_DBG, "WILC will be client (intent %d tie breaker %d)\n", intent, tie_breaker);
+				is_wilc_go = false;
+			} else {
+				PRINT_D(GENERIC_DBG, "WILC will be GO (intent %d tie breaker %d)\n", intent, tie_breaker);
+				is_wilc_go = true;
+			}
+
+			#else   /* USE_SUPPLICANT_GO_INTENT */
+			#ifdef FORCE_P2P_CLIENT
+			buf[index + 3] = (buf[index + 3]  & 0x01) | (0x0f << 1);
+			#else
+			buf[index + 3] = (buf[index + 3]  & 0x01) | (0x00 << 1);
+			#endif
+			#endif  /* USE_SUPPLICANT_GO_INTENT */
+		}
+
+		#ifdef USE_SUPPLICANT_GO_INTENT
+		/*Case 2: If group bssid attribute is present, no need to modify channels attributes*/
+		/*In invitation req and rsp, group bssid attr presence determines who will be GO*/
+		if (buf[index] == GROUP_BSSID_ATTR_ID) {
+			PRINT_D(GENERIC_DBG, "Group BSSID: %2x:%2x:%2x\n", buf[index + 3]
+				, buf[index + 4]
+				, buf[index + 5]);
+			is_wilc_go = false;
+		}
+		#endif  /* USE_SUPPLICANT_GO_INTENT */
+
+		if (buf[index] ==  CHANLIST_ATTR_ID) {
+			channel_list_attr_index = index;
+		} else if (buf[index] ==  OPERCHAN_ATTR_ID)   {
+			op_channel_attr_index = index;
+		}
+		index += buf[index + 1] + 3; /* ID,Length byte */
+	}
+
+	#ifdef USE_SUPPLICANT_GO_INTENT
+	if (u8WLANChannel != INVALID_CHANNEL && is_wilc_go)
+	#else
+	if (u8WLANChannel != INVALID_CHANNEL)
+	#endif
+	{
+		/*Modify channel list attribute*/
+		if (channel_list_attr_index) {
+			PRINT_D(GENERIC_DBG, "Modify channel list attribute\n");
+			for (i = channel_list_attr_index + 3; i < ((channel_list_attr_index + 3) + buf[channel_list_attr_index + 1]); i++) {
+				if (buf[i] == 0x51) {
+					for (j = i + 2; j < ((i + 2) + buf[i + 1]); j++) {
+						buf[j] = u8WLANChannel;
+					}
+					break;
+				}
+			}
+		}
+		/*Modify operating channel attribute*/
+		if (op_channel_attr_index) {
+			PRINT_D(GENERIC_DBG, "Modify operating channel attribute\n");
+			buf[op_channel_attr_index + 6] = 0x51;
+			buf[op_channel_attr_index + 7] = u8WLANChannel;
+		}
+	}
+}
+
+/**
+ *  @brief      WILC_WFI_CfgParseTxAction
+ *  @details Function parses the transmitted  action frames and modifies the
+ *               GO Intent attribute
+ *  @param[in] u8* Buffer, u32 length, bool bOperChan, u8 iftype
+ *  @return     NONE.
+ *  @author	mdaftedar
+ *  @date	12 DEC 2012
+ *  @version
+ */
+void WILC_WFI_CfgParseTxAction(u8 *buf, u32 len, bool bOperChan, u8 iftype)
+{
+	u32 index = 0;
+	u32 i = 0, j = 0;
+
+	u8 op_channel_attr_index = 0;
+	u8 channel_list_attr_index = 0;
+	#ifdef USE_SUPPLICANT_GO_INTENT
+	bool is_wilc_go = false;
+
+	/*BugID_5460*/
+	/*Case 1: If we are already p2p client, no need to modify channels attributes*/
+	/*This to handle the case of inviting a p2p peer to join an existing group which we are a member in*/
+	if (iftype == CLIENT_MODE)
+		return;
+	#endif
+
+	while (index < len) {
+		#ifdef USE_SUPPLICANT_GO_INTENT
+		/*Case 2: If group bssid attribute is present, no need to modify channels attributes*/
+		/*In invitation req and rsp, group bssid attr presence determines who will be GO*/
+		/*Note: If we are already p2p client, group bssid attr may also be present (handled in Case 1)*/
+		if (buf[index] == GROUP_BSSID_ATTR_ID) {
+			PRINT_D(GENERIC_DBG, "Group BSSID: %2x:%2x:%2x\n", buf[index + 3]
+				, buf[index + 4]
+				, buf[index + 5]);
+			is_wilc_go = true;
+		}
+
+		#else   /* USE_SUPPLICANT_GO_INTENT */
+		if (buf[index] == GO_INTENT_ATTR_ID) {
+			#ifdef FORCE_P2P_CLIENT
+			buf[index + 3] = (buf[index + 3]  & 0x01) | (0x00 << 1);
+			#else
+			buf[index + 3] = (buf[index + 3]  & 0x01) | (0x0f << 1);
+			#endif
+
+			break;
+		}
+		#endif
+
+		if (buf[index] ==  CHANLIST_ATTR_ID) {
+			channel_list_attr_index = index;
+		} else if (buf[index] ==  OPERCHAN_ATTR_ID)   {
+			op_channel_attr_index = index;
+		}
+		index += buf[index + 1] + 3; /* ID,Length byte */
+	}
+
+	#ifdef USE_SUPPLICANT_GO_INTENT
+	/*No need to check bOperChan since only transmitted invitation frames are parsed*/
+	if (u8WLANChannel != INVALID_CHANNEL && is_wilc_go)
+	#else
+	if (u8WLANChannel != INVALID_CHANNEL && bOperChan)
+	#endif
+	{
+		/*Modify channel list attribute*/
+		if (channel_list_attr_index) {
+			PRINT_D(GENERIC_DBG, "Modify channel list attribute\n");
+			for (i = channel_list_attr_index + 3; i < ((channel_list_attr_index + 3) + buf[channel_list_attr_index + 1]); i++) {
+				if (buf[i] == 0x51) {
+					for (j = i + 2; j < ((i + 2) + buf[i + 1]); j++) {
+						buf[j] = u8WLANChannel;
+					}
+					break;
+				}
+			}
+		}
+		/*Modify operating channel attribute*/
+		if (op_channel_attr_index) {
+			PRINT_D(GENERIC_DBG, "Modify operating channel attribute\n");
+			buf[op_channel_attr_index + 6] = 0x51;
+			buf[op_channel_attr_index + 7] = u8WLANChannel;
+		}
+	}
+}
+
+/*  @brief                       WILC_WFI_p2p_rx
+ *  @details
+ *  @param[in]
+ *
+ *  @return             None
+ *  @author		Mai Daftedar
+ *  @date			2 JUN 2013
+ *  @version		1.0
+ */
+
+void WILC_WFI_p2p_rx (struct net_device *dev, uint8_t *buff, uint32_t size)
+{
+
+	struct WILC_WFI_priv *priv;
+	u32 header, pkt_offset;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+	u32 i = 0;
+	s32 s32Freq;
+	priv = wiphy_priv(dev->ieee80211_ptr->wiphy);
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+
+	/* Get WILC header */
+	WILC_memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET);
+
+	/* The packet offset field conain info about what type of managment frame */
+	/* we are dealing with and ack status */
+	pkt_offset = GET_PKT_OFFSET(header);
+
+	if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
+		if (buff[FRAME_TYPE_ID] == IEEE80211_STYPE_PROBE_RESP) {
+			PRINT_D(GENERIC_DBG, "Probe response ACK\n");
+			cfg80211_mgmt_tx_status(priv->wdev, priv->u64tx_cookie, buff, size, true, GFP_KERNEL);
+			return;
+		} else {
+			if (pkt_offset & IS_MGMT_STATUS_SUCCES)	{
+				PRINT_D(GENERIC_DBG, "Success Ack - Action frame category: %x Action Subtype: %d Dialog T: %x OR %x\n", buff[ACTION_CAT_ID], buff[ACTION_SUBTYPE_ID],
+					buff[ACTION_SUBTYPE_ID + 1], buff[P2P_PUB_ACTION_SUBTYPE + 1]);
+				cfg80211_mgmt_tx_status(priv->wdev, priv->u64tx_cookie, buff, size, true, GFP_KERNEL);
+			} else {
+				PRINT_D(GENERIC_DBG, "Fail Ack - Action frame category: %x Action Subtype: %d Dialog T: %x OR %x\n", buff[ACTION_CAT_ID], buff[ACTION_SUBTYPE_ID],
+					buff[ACTION_SUBTYPE_ID + 1], buff[P2P_PUB_ACTION_SUBTYPE + 1]);
+				cfg80211_mgmt_tx_status(priv->wdev, priv->u64tx_cookie, buff, size, false, GFP_KERNEL);
+			}
+			return;
+		}
+	} else {
+
+		PRINT_D(GENERIC_DBG, "Rx Frame Type:%x\n", buff[FRAME_TYPE_ID]);
+
+		/*BugID_5442*/
+		/*Upper layer is informed that the frame is received on this freq*/
+		s32Freq = ieee80211_channel_to_frequency(u8CurrChannel, IEEE80211_BAND_2GHZ);
+
+		if (ieee80211_is_action(buff[FRAME_TYPE_ID])) {
+			PRINT_D(GENERIC_DBG, "Rx Action Frame Type: %x %x\n", buff[ACTION_SUBTYPE_ID], buff[P2P_PUB_ACTION_SUBTYPE]);
+
+			if (priv->bCfgScanning == true && time_after_eq(jiffies, (unsigned long)pstrWFIDrv->u64P2p_MgmtTimeout)) {
+				PRINT_D(GENERIC_DBG, "Receiving action frames from wrong channels\n");
+				return;
+			}
+			if (buff[ACTION_CAT_ID] == PUB_ACTION_ATTR_ID) {
+
+				switch (buff[ACTION_SUBTYPE_ID]) {
+				case GAS_INTIAL_REQ:
+					PRINT_D(GENERIC_DBG, "GAS INITIAL REQ %x\n", buff[ACTION_SUBTYPE_ID]);
+					break;
+
+				case GAS_INTIAL_RSP:
+					PRINT_D(GENERIC_DBG, "GAS INITIAL RSP %x\n", buff[ACTION_SUBTYPE_ID]);
+					break;
+
+				case PUBLIC_ACT_VENDORSPEC:
+					/*Now we have a public action vendor specific action frame, check if its a p2p public action frame
+					 * based on the standard its should have the p2p_oui attribute with the following values 50 6f 9A 09*/
+					if (!WILC_memcmp(u8P2P_oui, &buff[ACTION_SUBTYPE_ID + 1], 4)) {
+						if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP))	{
+							if (!bWilc_ie) {
+								for (i = P2P_PUB_ACTION_SUBTYPE; i < size; i++)	{
+									if (!WILC_memcmp(u8P2P_vendorspec, &buff[i], 6)) {
+										u8P2Precvrandom = buff[i + 6];
+										bWilc_ie = true;
+										PRINT_D(GENERIC_DBG, "WILC Vendor specific IE:%02x\n", u8P2Precvrandom);
+										break;
+									}
+								}
+							}
+						}
+						if (u8P2Plocalrandom > u8P2Precvrandom)	{
+							if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP
+							      || buff[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)) {
+								for (i = P2P_PUB_ACTION_SUBTYPE + 2; i < size; i++) {
+									if (buff[i] == P2PELEM_ATTR_ID && !(WILC_memcmp(u8P2P_oui, &buff[i + 2], 4))) {
+										WILC_WFI_CfgParseRxAction(&buff[i + 6], size - (i + 6));
+										break;
+									}
+								}
+							}
+						} else
+							PRINT_D(GENERIC_DBG, "PEER WILL BE GO LocaRand=%02x RecvRand %02x\n", u8P2Plocalrandom, u8P2Precvrandom);
+					}
+
+
+					if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP) && (bWilc_ie))	{
+						PRINT_D(GENERIC_DBG, "Sending P2P to host without extra elemnt\n");
+						/* extra attribute for sig_dbm: signal strength in mBm, or 0 if unknown */
+						cfg80211_rx_mgmt(priv->wdev, s32Freq, 0, buff, size - 7, 0);
+						return;
+					}
+					break;
+
+				default:
+					PRINT_D(GENERIC_DBG, "NOT HANDLED PUBLIC ACTION FRAME TYPE:%x\n", buff[ACTION_SUBTYPE_ID]);
+					break;
+				}
+			}
+		}
+
+		cfg80211_rx_mgmt(priv->wdev, s32Freq, 0, buff, size - 7, 0);
+	}
+}
+
+/**
+ *  @brief                      WILC_WFI_mgmt_tx_complete
+ *  @details            Returns result of writing mgmt frame to VMM (Tx buffers are freed here)
+ *  @param[in]          priv
+ *                              transmitting status
+ *  @return             None
+ *  @author		Amr Abdelmoghny
+ *  @date			20 MAY 2013
+ *  @version		1.0
+ */
+static void WILC_WFI_mgmt_tx_complete(void *priv, int status)
+{
+	struct p2p_mgmt_data *pv_data = (struct p2p_mgmt_data *)priv;
+
+
+	kfree(pv_data->buff);
+	kfree(pv_data);
+}
+
+/**
+ * @brief               WILC_WFI_RemainOnChannelReady
+ *  @details    Callback function, called from handle_remain_on_channel on being ready on channel
+ *  @param
+ *  @return     none
+ *  @author	Amr abdelmoghny
+ *  @date		9 JUNE 2013
+ *  @version
+ */
+
+static void WILC_WFI_RemainOnChannelReady(void *pUserVoid)
+{
+	struct WILC_WFI_priv *priv;
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+
+	PRINT_D(HOSTINF_DBG, "Remain on channel ready \n");
+
+	priv->bInP2PlistenState = true;
+
+	cfg80211_ready_on_channel(priv->wdev,
+				  priv->strRemainOnChanParams.u64ListenCookie,
+				  priv->strRemainOnChanParams.pstrListenChan,
+				  priv->strRemainOnChanParams.u32ListenDuration,
+				  GFP_KERNEL);
+}
+
+/**
+ * @brief               WILC_WFI_RemainOnChannelExpired
+ *  @details    Callback function, called on expiration of remain-on-channel duration
+ *  @param
+ *  @return     none
+ *  @author	Amr abdelmoghny
+ *  @date		15 MAY 2013
+ *  @version
+ */
+
+static void WILC_WFI_RemainOnChannelExpired(void *pUserVoid, u32 u32SessionID)
+{
+	struct WILC_WFI_priv *priv;
+	priv = (struct WILC_WFI_priv *)pUserVoid;
+
+	/*BugID_5477*/
+	if (u32SessionID == priv->strRemainOnChanParams.u32ListenSessionID) {
+		PRINT_D(GENERIC_DBG, "Remain on channel expired \n");
+
+		priv->bInP2PlistenState = false;
+
+		/*Inform wpas of remain-on-channel expiration*/
+		cfg80211_remain_on_channel_expired(priv->wdev,
+						   priv->strRemainOnChanParams.u64ListenCookie,
+						   priv->strRemainOnChanParams.pstrListenChan,
+						   GFP_KERNEL);
+	} else {
+		PRINT_D(GENERIC_DBG, "Received ID 0x%x Expected ID 0x%x (No match)\n", u32SessionID
+			, priv->strRemainOnChanParams.u32ListenSessionID);
+	}
+}
+
+
+/**
+ *  @brief      WILC_WFI_remain_on_channel
+ *  @details    Request the driver to remain awake on the specified
+ *                      channel for the specified duration to complete an off-channel
+ *                      operation (e.g., public action frame exchange). When the driver is
+ *                      ready on the requested channel, it must indicate this with an event
+ *                      notification by calling cfg80211_ready_on_channel().
+ *  @param[in]
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int  WILC_WFI_remain_on_channel(struct wiphy *wiphy,
+				       struct wireless_dev *wdev,
+				       struct ieee80211_channel *chan,
+				       unsigned int duration, u64 *cookie)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	priv = wiphy_priv(wiphy);
+
+	PRINT_D(GENERIC_DBG, "Remaining on channel %d\n", chan->hw_value);
+
+	/*BugID_4800: if in AP mode, return.*/
+	/*This check is to handle the situation when user*/
+	/*requests "create group" during a running scan*/
+
+	if (wdev->iftype == NL80211_IFTYPE_AP) {
+		PRINT_D(GENERIC_DBG, "Required remain-on-channel while in AP mode");
+		return s32Error;
+	}
+
+	u8CurrChannel = chan->hw_value;
+
+	/*Setting params needed by WILC_WFI_RemainOnChannelExpired()*/
+	priv->strRemainOnChanParams.pstrListenChan = chan;
+	priv->strRemainOnChanParams.u64ListenCookie = *cookie;
+	priv->strRemainOnChanParams.u32ListenDuration = duration;
+	priv->strRemainOnChanParams.u32ListenSessionID++;
+
+	s32Error = host_int_remain_on_channel(priv->hWILCWFIDrv
+					      , priv->strRemainOnChanParams.u32ListenSessionID
+					      , duration
+					      , chan->hw_value
+					      , WILC_WFI_RemainOnChannelExpired
+					      , WILC_WFI_RemainOnChannelReady
+					      , (void *)priv);
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_cancel_remain_on_channel
+ *  @details    Cancel an on-going remain-on-channel operation.
+ *                      This allows the operation to be terminated prior to timeout based on
+ *                      the duration value.
+ *  @param[in]   struct wiphy *wiphy,
+ *  @param[in]  struct net_device *dev
+ *  @param[in]  u64 cookie,
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int   WILC_WFI_cancel_remain_on_channel(struct wiphy *wiphy,
+					       struct wireless_dev *wdev,
+					       u64 cookie)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	priv = wiphy_priv(wiphy);
+
+	PRINT_D(CFG80211_DBG, "Cancel remain on channel\n");
+
+	s32Error = host_int_ListenStateExpired(priv->hWILCWFIDrv, priv->strRemainOnChanParams.u32ListenSessionID);
+	return s32Error;
+}
+/**
+ *  @brief       WILC_WFI_add_wilcvendorspec
+ *  @details    Adding WILC information elemet to allow two WILC devices to
+ *                              identify each other and connect
+ *  @param[in]   u8 * buf
+ *  @return     void
+ *  @author	mdaftedar
+ *  @date	01 JAN 2014
+ *  @version	1.0
+ */
+void WILC_WFI_add_wilcvendorspec(u8 *buff)
+{
+	WILC_memcpy(buff, u8P2P_vendorspec, sizeof(u8P2P_vendorspec));
+}
+/**
+ *  @brief      WILC_WFI_mgmt_tx_frame
+ *  @details
+ *
+ *  @param[in]
+ *  @return     NONE.
+ *  @author	mdaftedar
+ *  @date	01 JUL 2012
+ *  @version
+ */
+extern linux_wlan_t *g_linux_wlan;
+extern bool bEnablePS;
+int WILC_WFI_mgmt_tx(struct wiphy *wiphy,
+			struct wireless_dev *wdev,
+			struct cfg80211_mgmt_tx_params *params,
+			u64 *cookie)
+{
+	struct ieee80211_channel *chan = params->chan;
+	unsigned int wait = params->wait;
+	const u8 *buf = params->buf;
+	size_t len = params->len;
+	const struct ieee80211_mgmt *mgmt;
+	struct p2p_mgmt_data *mgmt_tx;
+	struct WILC_WFI_priv *priv;
+	s32 s32Error = WILC_SUCCESS;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+	u32 i;
+	perInterface_wlan_t *nic;
+	u32 buf_len = len + sizeof(u8P2P_vendorspec) + sizeof(u8P2Plocalrandom);
+
+	nic = netdev_priv(wdev->netdev);
+	priv = wiphy_priv(wiphy);
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+
+	*cookie = (unsigned long)buf;
+	priv->u64tx_cookie = *cookie;
+	mgmt = (const struct ieee80211_mgmt *) buf;
+
+	if (ieee80211_is_mgmt(mgmt->frame_control)) {
+
+		/*mgmt frame allocation*/
+		mgmt_tx = (struct p2p_mgmt_data *)WILC_MALLOC(sizeof(struct p2p_mgmt_data));
+		if (mgmt_tx == NULL) {
+			PRINT_ER("Failed to allocate memory for mgmt_tx structure\n");
+			return WILC_FAIL;
+		}
+		mgmt_tx->buff = (char *)WILC_MALLOC(buf_len);
+		if (mgmt_tx->buff == NULL) {
+			PRINT_ER("Failed to allocate memory for mgmt_tx buff\n");
+			return WILC_FAIL;
+		}
+		WILC_memcpy(mgmt_tx->buff, buf, len);
+		mgmt_tx->size = len;
+
+
+		if (ieee80211_is_probe_resp(mgmt->frame_control)) {
+			PRINT_D(GENERIC_DBG, "TX: Probe Response\n");
+			PRINT_D(GENERIC_DBG, "Setting channel: %d\n", chan->hw_value);
+			host_int_set_mac_chnl_num(priv->hWILCWFIDrv, chan->hw_value);
+			/*Save the current channel after we tune to it*/
+			u8CurrChannel = chan->hw_value;
+		} else if (ieee80211_is_action(mgmt->frame_control))   {
+			PRINT_D(GENERIC_DBG, "ACTION FRAME:%x\n", (u16)mgmt->frame_control);
+
+
+			/*BugID_4847*/
+			if (buf[ACTION_CAT_ID] == PUB_ACTION_ATTR_ID) {
+				/*BugID_4847*/
+				/*Only set the channel, if not a negotiation confirmation frame
+				 * (If Negotiation confirmation frame, force it
+				 * to be transmitted on the same negotiation channel)*/
+
+				if (buf[ACTION_SUBTYPE_ID] != PUBLIC_ACT_VENDORSPEC ||
+				    buf[P2P_PUB_ACTION_SUBTYPE] != GO_NEG_CONF)	{
+					PRINT_D(GENERIC_DBG, "Setting channel: %d\n", chan->hw_value);
+					host_int_set_mac_chnl_num(priv->hWILCWFIDrv, chan->hw_value);
+					/*Save the current channel after we tune to it*/
+					u8CurrChannel = chan->hw_value;
+				}
+				switch (buf[ACTION_SUBTYPE_ID])	{
+				case GAS_INTIAL_REQ:
+				{
+					PRINT_D(GENERIC_DBG, "GAS INITIAL REQ %x\n", buf[ACTION_SUBTYPE_ID]);
+					break;
+				}
+
+				case GAS_INTIAL_RSP:
+				{
+					PRINT_D(GENERIC_DBG, "GAS INITIAL RSP %x\n", buf[ACTION_SUBTYPE_ID]);
+					break;
+				}
+
+				case PUBLIC_ACT_VENDORSPEC:
+				{
+					/*Now we have a public action vendor specific action frame, check if its a p2p public action frame
+					 * based on the standard its should have the p2p_oui attribute with the following values 50 6f 9A 09*/
+					if (!WILC_memcmp(u8P2P_oui, &buf[ACTION_SUBTYPE_ID + 1], 4)) {
+						/*For the connection of two WILC's connection generate a rand number to determine who will be a GO*/
+						if ((buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP)) {
+							if (u8P2Plocalrandom == 1 && u8P2Precvrandom < u8P2Plocalrandom) {
+								get_random_bytes(&u8P2Plocalrandom, 1);
+								/*Increment the number to prevent if its 0*/
+								u8P2Plocalrandom++;
+							}
+						}
+
+						if ((buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP
+						      || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)) {
+							if (u8P2Plocalrandom > u8P2Precvrandom)	{
+								PRINT_D(GENERIC_DBG, "LOCAL WILL BE GO LocaRand=%02x RecvRand %02x\n", u8P2Plocalrandom, u8P2Precvrandom);
+
+								/*Search for the p2p information information element , after the Public action subtype theres a byte for teh dialog token, skip that*/
+								for (i = P2P_PUB_ACTION_SUBTYPE + 2; i < len; i++) {
+									if (buf[i] == P2PELEM_ATTR_ID && !(WILC_memcmp(u8P2P_oui, &buf[i + 2], 4))) {
+										if (buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)
+											WILC_WFI_CfgParseTxAction(&mgmt_tx->buff[i + 6], len - (i + 6), true, nic->iftype);
+
+										/*BugID_5460*/
+										/*If using supplicant go intent, no need at all*/
+										/*to parse transmitted negotiation frames*/
+											#ifndef USE_SUPPLICANT_GO_INTENT
+										else
+											WILC_WFI_CfgParseTxAction(&mgmt_tx->buff[i + 6], len - (i + 6), false, nic->iftype);
+											#endif
+										break;
+									}
+								}
+
+								if (buf[P2P_PUB_ACTION_SUBTYPE] != P2P_INV_REQ && buf[P2P_PUB_ACTION_SUBTYPE] != P2P_INV_RSP) {
+									WILC_WFI_add_wilcvendorspec(&mgmt_tx->buff[len]);
+									mgmt_tx->buff[len + sizeof(u8P2P_vendorspec)] = u8P2Plocalrandom;
+									mgmt_tx->size = buf_len;
+								}
+							} else
+								PRINT_D(GENERIC_DBG, "PEER WILL BE GO LocaRand=%02x RecvRand %02x\n", u8P2Plocalrandom, u8P2Precvrandom);
+						}
+
+					} else {
+						PRINT_D(GENERIC_DBG, "Not a P2P public action frame\n");
+					}
+
+					break;
+				}
+
+				default:
+				{
+					PRINT_D(GENERIC_DBG, "NOT HANDLED PUBLIC ACTION FRAME TYPE:%x\n", buf[ACTION_SUBTYPE_ID]);
+					break;
+				}
+				}
+
+			}
+
+			PRINT_D(GENERIC_DBG, "TX: ACTION FRAME Type:%x : Chan:%d\n", buf[ACTION_SUBTYPE_ID], chan->hw_value);
+			pstrWFIDrv->u64P2p_MgmtTimeout = (jiffies + msecs_to_jiffies(wait));
+
+			PRINT_D(GENERIC_DBG, "Current Jiffies: %lu Timeout:%llu\n", jiffies, pstrWFIDrv->u64P2p_MgmtTimeout);
+
+		}
+
+		g_linux_wlan->oup.wlan_add_mgmt_to_tx_que(mgmt_tx, mgmt_tx->buff, mgmt_tx->size, WILC_WFI_mgmt_tx_complete);
+	} else {
+		PRINT_D(GENERIC_DBG, "This function transmits only management frames\n");
+	}
+	return s32Error;
+}
+
+int   WILC_WFI_mgmt_tx_cancel_wait(struct wiphy *wiphy,
+				   struct wireless_dev *wdev,
+				   u64 cookie)
+{
+	struct WILC_WFI_priv *priv;
+	tstrWILC_WFIDrv *pstrWFIDrv;
+	priv = wiphy_priv(wiphy);
+	pstrWFIDrv = (tstrWILC_WFIDrv *)priv->hWILCWFIDrv;
+
+
+	PRINT_D(GENERIC_DBG, "Tx Cancel wait :%lu\n", jiffies);
+	pstrWFIDrv->u64P2p_MgmtTimeout = jiffies;
+
+	if (priv->bInP2PlistenState == false) {
+		/* Bug 5504: This is just to avoid connection failure when getting stuck when the supplicant
+		 *                      considers the driver falsely that it is in Listen state */
+		cfg80211_remain_on_channel_expired(priv->wdev,
+						   priv->strRemainOnChanParams.u64ListenCookie,
+						   priv->strRemainOnChanParams.pstrListenChan,
+						   GFP_KERNEL);
+	}
+
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_frame_register
+ *  @details Notify driver that a management frame type was
+ *              registered. Note that this callback may not sleep, and cannot run
+ *                      concurrently with itself.
+ *  @param[in]
+ *  @return     NONE.
+ *  @author	mdaftedar
+ *  @date	01 JUL 2012
+ *  @version
+ */
+void    WILC_WFI_frame_register(struct wiphy *wiphy,
+				struct wireless_dev *wdev,
+				u16 frame_type, bool reg)
+{
+
+	struct WILC_WFI_priv *priv;
+	perInterface_wlan_t *nic;
+
+
+	priv = wiphy_priv(wiphy);
+	nic = netdev_priv(priv->wdev->netdev);
+
+
+
+	/*BugID_5137*/
+	if (!frame_type)
+		return;
+
+	PRINT_D(GENERIC_DBG, "Frame registering Frame Type: %x: Boolean: %d\n", frame_type, reg);
+	switch (frame_type) {
+	case PROBE_REQ:
+	{
+		nic->g_struct_frame_reg[0].frame_type = frame_type;
+		nic->g_struct_frame_reg[0].reg = reg;
+	}
+	break;
+
+	case ACTION:
+	{
+		nic->g_struct_frame_reg[1].frame_type = frame_type;
+		nic->g_struct_frame_reg[1].reg = reg;
+	}
+	break;
+
+	default:
+	{
+		break;
+	}
+
+	}
+	/*If mac is closed, then return*/
+	if (!g_linux_wlan->wilc1000_initialized) {
+		PRINT_D(GENERIC_DBG, "Return since mac is closed\n");
+		return;
+	}
+	host_int_frame_register(priv->hWILCWFIDrv, frame_type, reg);
+
+
+}
+#endif /*WILC_P2P*/
+
+/**
+ *  @brief      WILC_WFI_set_cqm_rssi_config
+ *  @details    Configure connection quality monitor RSSI threshold.
+ *  @param[in]   struct wiphy *wiphy:
+ *  @param[in]	struct net_device *dev:
+ *  @param[in]          s32 rssi_thold:
+ *  @param[in]	u32 rssi_hyst:
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int    WILC_WFI_set_cqm_rssi_config(struct wiphy *wiphy,
+					   struct net_device *dev,  s32 rssi_thold, u32 rssi_hyst)
+{
+	PRINT_D(CFG80211_DBG, "Setting CQM RSSi Function\n");
+	return 0;
+
+}
+/**
+ *  @brief      WILC_WFI_dump_station
+ *  @details    Configure connection quality monitor RSSI threshold.
+ *  @param[in]   struct wiphy *wiphy:
+ *  @param[in]	struct net_device *dev
+ *  @param[in]          int idx
+ *  @param[in]	u8 *mac
+ *  @param[in]	struct station_info *sinfo
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_dump_station(struct wiphy *wiphy, struct net_device *dev,
+				 int idx, u8 *mac, struct station_info *sinfo)
+{
+	struct WILC_WFI_priv *priv;
+	PRINT_D(CFG80211_DBG, "Dumping station information\n");
+
+	if (idx != 0)
+		return -ENOENT;
+
+	priv = wiphy_priv(wiphy);
+
+	sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL);
+
+	host_int_get_rssi(priv->hWILCWFIDrv, &(sinfo->signal));
+
+	return 0;
+
+}
+
+
+/**
+ *  @brief      WILC_WFI_set_power_mgmt
+ *  @details
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 JUL 2012
+ *  @version	1.0WILC_WFI_set_cqmWILC_WFI_set_cqm_rssi_configWILC_WFI_set_cqm_rssi_configWILC_WFI_set_cqm_rssi_configWILC_WFI_set_cqm_rssi_config_rssi_config
+ */
+int WILC_WFI_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
+			    bool enabled, int timeout)
+{
+	struct WILC_WFI_priv *priv;
+	PRINT_D(CFG80211_DBG, " Power save Enabled= %d , TimeOut = %d\n", enabled, timeout);
+
+	if (wiphy == NULL)
+		return -ENOENT;
+
+	priv = wiphy_priv(wiphy);
+	if (priv->hWILCWFIDrv == NULL) {
+		PRINT_ER("Driver is NULL\n");
+		return -EIO;
+	}
+
+	if (bEnablePS)
+		host_int_set_power_mgmt(priv->hWILCWFIDrv, enabled, timeout);
+
+
+	return WILC_SUCCESS;
+
+}
+#ifdef WILC_AP_EXTERNAL_MLME
+/**
+ *  @brief      WILC_WFI_change_virt_intf
+ *  @details    Change type/configuration of virtual interface,
+ *                      keep the struct wireless_dev's iftype updated.
+ *  @param[in]   NONE
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void wilc1000_wlan_deinit(linux_wlan_t *nic);
+int wilc1000_wlan_init(struct net_device *dev, perInterface_wlan_t *p_nic);
+
+static int WILC_WFI_change_virt_intf(struct wiphy *wiphy, struct net_device *dev,
+				     enum nl80211_iftype type, u32 *flags, struct vif_params *params)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	perInterface_wlan_t *nic;
+	u8 interface_type;
+	u16 TID = 0;
+	#ifdef WILC_P2P
+	u8 i;
+	#endif
+
+	nic = netdev_priv(dev);
+	priv = wiphy_priv(wiphy);
+
+	PRINT_D(HOSTAPD_DBG, "In Change virtual interface function\n");
+	PRINT_D(HOSTAPD_DBG, "Wireless interface name =%s\n", dev->name);
+	u8P2Plocalrandom = 0x01;
+	u8P2Precvrandom = 0x00;
+
+	bWilc_ie = false;
+
+	#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+	g_obtainingIP = false;
+	WILC_TimerStop(&hDuringIpTimer, NULL);
+	PRINT_D(GENERIC_DBG, "Changing virtual interface, enable scan\n");
+	#endif
+	/*BugID_5137*/
+	/*Set WILC_CHANGING_VIR_IF register to disallow adding futrue keys to CE H/W*/
+	if (g_ptk_keys_saved && g_gtk_keys_saved) {
+		Set_machw_change_vir_if(true);
+	}
+
+	switch (type) {
+	case NL80211_IFTYPE_STATION:
+		connecting = 0;
+		PRINT_D(HOSTAPD_DBG, "Interface type = NL80211_IFTYPE_STATION\n");
+
+		/* send delba over wlan interface */
+
+
+		dev->ieee80211_ptr->iftype = type;
+		priv->wdev->iftype = type;
+		nic->monitor_flag = 0;
+		nic->iftype = STATION_MODE;
+
+		/*Remove the enteries of the previously connected clients*/
+		memset(priv->assoc_stainfo.au8Sta_AssociatedBss, 0, MAX_NUM_STA * ETH_ALEN);
+		#ifndef SIMULATION
+		#ifdef WILC_P2P
+		interface_type = nic->iftype;
+		nic->iftype = STATION_MODE;
+
+		if (g_linux_wlan->wilc1000_initialized)	{
+			host_int_del_All_Rx_BASession(priv->hWILCWFIDrv, g_linux_wlan->strInterfaceInfo[0].aBSSID, TID);
+			/* ensure that the message Q is empty */
+			host_int_wait_msg_queue_idle();
+
+			/*BugID_5213*/
+			/*Eliminate host interface blocking state*/
+			linux_wlan_unlock((void *)&g_linux_wlan->cfg_event);
+
+			wilc1000_wlan_deinit(g_linux_wlan);
+			wilc1000_wlan_init(dev, nic);
+			g_wilc_initialized = 1;
+			nic->iftype = interface_type;
+
+			/*Setting interface 1 drv handler and mac address in newly downloaded FW*/
+			host_int_set_wfi_drv_handler(g_linux_wlan->strInterfaceInfo[0].drvHandler);
+			host_int_set_MacAddress((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+						g_linux_wlan->strInterfaceInfo[0].aSrcAddress);
+			host_int_set_operation_mode(priv->hWILCWFIDrv, STATION_MODE);
+
+			/*Add saved WEP keys, if any*/
+			if (g_wep_keys_saved) {
+				host_int_set_WEPDefaultKeyID((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+							     g_key_wep_params.key_idx);
+				host_int_add_wep_key_bss_sta((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+							     g_key_wep_params.key,
+							     g_key_wep_params.key_len,
+							     g_key_wep_params.key_idx);
+			}
+
+			/*No matter the driver handler passed here, it will be overwriiten*/
+			/*in Handle_FlushConnect() with gu8FlushedJoinReqDrvHandler*/
+			host_int_flush_join_req(priv->hWILCWFIDrv);
+
+			/*Add saved PTK and GTK keys, if any*/
+			if (g_ptk_keys_saved && g_gtk_keys_saved) {
+				PRINT_D(CFG80211_DBG, "ptk %x %x %x\n", g_key_ptk_params.key[0],
+					g_key_ptk_params.key[1],
+					g_key_ptk_params.key[2]);
+				PRINT_D(CFG80211_DBG, "gtk %x %x %x\n", g_key_gtk_params.key[0],
+					g_key_gtk_params.key[1],
+					g_key_gtk_params.key[2]);
+				WILC_WFI_add_key(g_linux_wlan->strInterfaceInfo[0].wilc_netdev->ieee80211_ptr->wiphy,
+						 g_linux_wlan->strInterfaceInfo[0].wilc_netdev,
+						 g_add_ptk_key_params.key_idx,
+						 g_add_ptk_key_params.pairwise,
+						 g_add_ptk_key_params.mac_addr,
+						 (struct key_params *)(&g_key_ptk_params));
+
+				WILC_WFI_add_key(g_linux_wlan->strInterfaceInfo[0].wilc_netdev->ieee80211_ptr->wiphy,
+						 g_linux_wlan->strInterfaceInfo[0].wilc_netdev,
+						 g_add_gtk_key_params.key_idx,
+						 g_add_gtk_key_params.pairwise,
+						 g_add_gtk_key_params.mac_addr,
+						 (struct key_params *)(&g_key_gtk_params));
+			}
+
+			/*BugID_4847: registered frames in firmware are now*/
+			/*lost due to mac close. So re-register those frames*/
+			if (g_linux_wlan->wilc1000_initialized)	{
+				for (i = 0; i < num_reg_frame; i++) {
+					PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", nic->g_struct_frame_reg[i].frame_type,
+						nic->g_struct_frame_reg[i].reg);
+					host_int_frame_register(priv->hWILCWFIDrv,
+								nic->g_struct_frame_reg[i].frame_type,
+								nic->g_struct_frame_reg[i].reg);
+				}
+			}
+
+			bEnablePS = true;
+			host_int_set_power_mgmt(priv->hWILCWFIDrv, 1, 0);
+		}
+		#endif
+		#endif
+		break;
+
+	case NL80211_IFTYPE_P2P_CLIENT:
+		bEnablePS = false;
+		host_int_set_power_mgmt(priv->hWILCWFIDrv, 0, 0);
+		connecting = 0;
+		PRINT_D(HOSTAPD_DBG, "Interface type = NL80211_IFTYPE_P2P_CLIENT\n");
+
+		host_int_del_All_Rx_BASession(priv->hWILCWFIDrv, g_linux_wlan->strInterfaceInfo[0].aBSSID, TID);
+
+		dev->ieee80211_ptr->iftype = type;
+		priv->wdev->iftype = type;
+		nic->monitor_flag = 0;
+
+		#ifndef SIMULATION
+		#ifdef WILC_P2P
+
+		PRINT_D(HOSTAPD_DBG, "Downloading P2P_CONCURRENCY_FIRMWARE\n");
+		nic->iftype = CLIENT_MODE;
+
+
+		if (g_linux_wlan->wilc1000_initialized)	{
+			/* ensure that the message Q is empty */
+			host_int_wait_msg_queue_idle();
+
+			wilc1000_wlan_deinit(g_linux_wlan);
+			wilc1000_wlan_init(dev, nic);
+			g_wilc_initialized = 1;
+
+			host_int_set_wfi_drv_handler(g_linux_wlan->strInterfaceInfo[0].drvHandler);
+			host_int_set_MacAddress((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+						g_linux_wlan->strInterfaceInfo[0].aSrcAddress);
+			host_int_set_operation_mode(priv->hWILCWFIDrv, STATION_MODE);
+
+			/*Add saved WEP keys, if any*/
+			if (g_wep_keys_saved) {
+				host_int_set_WEPDefaultKeyID((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+							     g_key_wep_params.key_idx);
+				host_int_add_wep_key_bss_sta((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+							     g_key_wep_params.key,
+							     g_key_wep_params.key_len,
+							     g_key_wep_params.key_idx);
+			}
+
+			/*No matter the driver handler passed here, it will be overwriiten*/
+			/*in Handle_FlushConnect() with gu8FlushedJoinReqDrvHandler*/
+			host_int_flush_join_req(priv->hWILCWFIDrv);
+
+			/*Add saved PTK and GTK keys, if any*/
+			if (g_ptk_keys_saved && g_gtk_keys_saved) {
+				PRINT_D(CFG80211_DBG, "ptk %x %x %x\n", g_key_ptk_params.key[0],
+					g_key_ptk_params.key[1],
+					g_key_ptk_params.key[2]);
+				PRINT_D(CFG80211_DBG, "gtk %x %x %x\n", g_key_gtk_params.key[0],
+					g_key_gtk_params.key[1],
+					g_key_gtk_params.key[2]);
+				WILC_WFI_add_key(g_linux_wlan->strInterfaceInfo[0].wilc_netdev->ieee80211_ptr->wiphy,
+						 g_linux_wlan->strInterfaceInfo[0].wilc_netdev,
+						 g_add_ptk_key_params.key_idx,
+						 g_add_ptk_key_params.pairwise,
+						 g_add_ptk_key_params.mac_addr,
+						 (struct key_params *)(&g_key_ptk_params));
+
+				WILC_WFI_add_key(g_linux_wlan->strInterfaceInfo[0].wilc_netdev->ieee80211_ptr->wiphy,
+						 g_linux_wlan->strInterfaceInfo[0].wilc_netdev,
+						 g_add_gtk_key_params.key_idx,
+						 g_add_gtk_key_params.pairwise,
+						 g_add_gtk_key_params.mac_addr,
+						 (struct key_params *)(&g_key_gtk_params));
+			}
+
+			/*Refresh scan, to refresh the scan results to the wpa_supplicant. Set MachHw to false to enable further key installments*/
+			refresh_scan(priv, 1, true);
+			Set_machw_change_vir_if(false);
+
+			/*BugID_4847: registered frames in firmware are now lost
+			 *  due to mac close. So re-register those frames */
+			if (g_linux_wlan->wilc1000_initialized)	{
+				for (i = 0; i < num_reg_frame; i++) {
+					PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", nic->g_struct_frame_reg[i].frame_type,
+						nic->g_struct_frame_reg[i].reg);
+					host_int_frame_register(priv->hWILCWFIDrv,
+								nic->g_struct_frame_reg[i].frame_type,
+								nic->g_struct_frame_reg[i].reg);
+				}
+			}
+		}
+		#endif
+		#endif
+		break;
+
+	case NL80211_IFTYPE_AP:
+		bEnablePS = false;
+		PRINT_D(HOSTAPD_DBG, "Interface type = NL80211_IFTYPE_AP %d\n", type);
+		dev->ieee80211_ptr->iftype = type;
+		priv->wdev->iftype = type;
+		nic->iftype = AP_MODE;
+		PRINT_D(CORECONFIG_DBG, "priv->hWILCWFIDrv[%p]\n", priv->hWILCWFIDrv);
+
+		#ifndef SIMULATION
+		PRINT_D(HOSTAPD_DBG, "Downloading AP firmware\n");
+		linux_wlan_get_firmware(nic);
+		#ifdef WILC_P2P
+		/*If wilc is running, then close-open to actually get new firmware running (serves P2P)*/
+		if (g_linux_wlan->wilc1000_initialized)	{
+			nic->iftype = AP_MODE;
+			g_linux_wlan->wilc1000_initialized = 1;
+			mac_close(dev);
+			mac_open(dev);
+
+			/*BugID_4847: registered frames in firmware are now lost
+			 * due to mac close. So re-register those frames */
+			for (i = 0; i < num_reg_frame; i++) {
+				PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", nic->g_struct_frame_reg[i].frame_type,
+					nic->g_struct_frame_reg[i].reg);
+				host_int_frame_register(priv->hWILCWFIDrv,
+							nic->g_struct_frame_reg[i].frame_type,
+							nic->g_struct_frame_reg[i].reg);
+			}
+		}
+		#endif
+		#endif
+		break;
+
+	case NL80211_IFTYPE_P2P_GO:
+		PRINT_D(GENERIC_DBG, "start duringIP timer\n");
+
+		#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+		g_obtainingIP = true;
+		WILC_TimerStart(&hDuringIpTimer, duringIP_TIME, NULL, NULL);
+		#endif
+		host_int_set_power_mgmt(priv->hWILCWFIDrv, 0, 0);
+		/*BugID_5222*/
+		/*Delete block ack has to be the latest config packet*/
+		/*sent before downloading new FW. This is because it blocks on*/
+		/*hWaitResponse semaphore, which allows previous config*/
+		/*packets to actually take action on old FW*/
+		host_int_del_All_Rx_BASession(priv->hWILCWFIDrv, g_linux_wlan->strInterfaceInfo[0].aBSSID, TID);
+		bEnablePS = false;
+		PRINT_D(HOSTAPD_DBG, "Interface type = NL80211_IFTYPE_GO\n");
+		dev->ieee80211_ptr->iftype = type;
+		priv->wdev->iftype = type;
+
+		PRINT_D(CORECONFIG_DBG, "priv->hWILCWFIDrv[%p]\n", priv->hWILCWFIDrv);
+
+		#ifndef SIMULATION
+		#ifdef WILC_P2P
+		PRINT_D(HOSTAPD_DBG, "Downloading P2P_CONCURRENCY_FIRMWARE\n");
+
+
+		#if 1
+		nic->iftype = GO_MODE;
+
+		/* ensure that the message Q is empty */
+		host_int_wait_msg_queue_idle();
+		wilc1000_wlan_deinit(g_linux_wlan);
+		wilc1000_wlan_init(dev, nic);
+		g_wilc_initialized = 1;
+
+
+		/*Setting interface 1 drv handler and mac address in newly downloaded FW*/
+		host_int_set_wfi_drv_handler(g_linux_wlan->strInterfaceInfo[0].drvHandler);
+		host_int_set_MacAddress((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+					g_linux_wlan->strInterfaceInfo[0].aSrcAddress);
+		host_int_set_operation_mode(priv->hWILCWFIDrv, AP_MODE);
+
+		/*Add saved WEP keys, if any*/
+		if (g_wep_keys_saved) {
+			host_int_set_WEPDefaultKeyID((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+						     g_key_wep_params.key_idx);
+			host_int_add_wep_key_bss_sta((WILC_WFIDrvHandle)(g_linux_wlan->strInterfaceInfo[0].drvHandler),
+						     g_key_wep_params.key,
+						     g_key_wep_params.key_len,
+						     g_key_wep_params.key_idx);
+		}
+
+		/*No matter the driver handler passed here, it will be overwriiten*/
+		/*in Handle_FlushConnect() with gu8FlushedJoinReqDrvHandler*/
+		host_int_flush_join_req(priv->hWILCWFIDrv);
+
+		/*Add saved PTK and GTK keys, if any*/
+		if (g_ptk_keys_saved && g_gtk_keys_saved) {
+			PRINT_D(CFG80211_DBG, "ptk %x %x %x cipher %x\n", g_key_ptk_params.key[0],
+				g_key_ptk_params.key[1],
+				g_key_ptk_params.key[2],
+				g_key_ptk_params.cipher);
+			PRINT_D(CFG80211_DBG, "gtk %x %x %x cipher %x\n", g_key_gtk_params.key[0],
+				g_key_gtk_params.key[1],
+				g_key_gtk_params.key[2],
+				g_key_gtk_params.cipher);
+			#if 1
+			WILC_WFI_add_key(g_linux_wlan->strInterfaceInfo[0].wilc_netdev->ieee80211_ptr->wiphy,
+					 g_linux_wlan->strInterfaceInfo[0].wilc_netdev,
+					 g_add_ptk_key_params.key_idx,
+					 g_add_ptk_key_params.pairwise,
+					 g_add_ptk_key_params.mac_addr,
+					 (struct key_params *)(&g_key_ptk_params));
+
+			WILC_WFI_add_key(g_linux_wlan->strInterfaceInfo[0].wilc_netdev->ieee80211_ptr->wiphy,
+					 g_linux_wlan->strInterfaceInfo[0].wilc_netdev,
+					 g_add_gtk_key_params.key_idx,
+					 g_add_gtk_key_params.pairwise,
+					 g_add_gtk_key_params.mac_addr,
+					 (struct key_params *)(&g_key_gtk_params));
+			#endif
+		}
+		#endif
+
+		/*BugID_4847: registered frames in firmware are now*/
+		/*lost due to mac close. So re-register those frames*/
+		if (g_linux_wlan->wilc1000_initialized)	{
+			for (i = 0; i < num_reg_frame; i++) {
+				PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", nic->g_struct_frame_reg[i].frame_type,
+					nic->g_struct_frame_reg[i].reg);
+				host_int_frame_register(priv->hWILCWFIDrv,
+							nic->g_struct_frame_reg[i].frame_type,
+							nic->g_struct_frame_reg[i].reg);
+			}
+		}
+		#endif
+		#endif
+		break;
+
+	default:
+		PRINT_ER("Unknown interface type= %d\n", type);
+		s32Error = -EINVAL;
+		return s32Error;
+		break;
+	}
+
+	return s32Error;
+}
+
+/* (austin.2013-07-23)
+ *
+ *      To support revised cfg80211_ops
+ *
+ *              add_beacon --> start_ap
+ *              set_beacon --> change_beacon
+ *              del_beacon --> stop_ap
+ *
+ *              beacon_parameters  -->	cfg80211_ap_settings
+ *                                                              cfg80211_beacon_data
+ *
+ *      applicable for linux kernel 3.4+
+ */
+
+/**
+ *  @brief      WILC_WFI_start_ap
+ *  @details    Add a beacon with given parameters, @head, @interval
+ *                      and @dtim_period will be valid, @tail is optional.
+ *  @param[in]   wiphy
+ *  @param[in]   dev	The net device structure
+ *  @param[in]   settings	cfg80211_ap_settings parameters for the beacon to be added
+ *  @return     int : Return 0 on Success.
+ *  @author	austin
+ *  @date	23 JUL 2013
+ *  @version	1.0
+ */
+static int WILC_WFI_start_ap(struct wiphy *wiphy, struct net_device *dev,
+			     struct cfg80211_ap_settings *settings)
+{
+	struct cfg80211_beacon_data *beacon = &(settings->beacon);
+	struct WILC_WFI_priv *priv;
+	s32 s32Error = WILC_SUCCESS;
+
+	priv = wiphy_priv(wiphy);
+	PRINT_D(HOSTAPD_DBG, "Starting ap\n");
+
+	PRINT_D(HOSTAPD_DBG, "Interval = %d \n DTIM period = %d\n Head length = %zu Tail length = %zu\n",
+		settings->beacon_interval, settings->dtim_period, beacon->head_len, beacon->tail_len);
+
+	s32Error = WILC_WFI_CfgSetChannel(wiphy, &settings->chandef);
+
+	if (s32Error != WILC_SUCCESS)
+		PRINT_ER("Error in setting channel\n");
+
+	linux_wlan_set_bssid(dev, g_linux_wlan->strInterfaceInfo[0].aSrcAddress);
+
+	#ifndef WILC_FULLY_HOSTING_AP
+	s32Error = host_int_add_beacon(priv->hWILCWFIDrv,
+					settings->beacon_interval,
+					settings->dtim_period,
+					beacon->head_len, (u8 *)beacon->head,
+					beacon->tail_len, (u8 *)beacon->tail);
+	#else
+	s32Error = host_add_beacon(priv->hWILCWFIDrv,
+					settings->beacon_interval,
+					settings->dtim_period,
+					beacon->head_len, (u8 *)beacon->head,
+					beacon->tail_len, (u8 *)beacon->tail);
+	#endif
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_change_beacon
+ *  @details    Add a beacon with given parameters, @head, @interval
+ *                      and @dtim_period will be valid, @tail is optional.
+ *  @param[in]   wiphy
+ *  @param[in]   dev	The net device structure
+ *  @param[in]   beacon	cfg80211_beacon_data for the beacon to be changed
+ *  @return     int : Return 0 on Success.
+ *  @author	austin
+ *  @date	23 JUL 2013
+ *  @version	1.0
+ */
+static int  WILC_WFI_change_beacon(struct wiphy *wiphy, struct net_device *dev,
+				   struct cfg80211_beacon_data *beacon)
+{
+	struct WILC_WFI_priv *priv;
+	s32 s32Error = WILC_SUCCESS;
+
+	priv = wiphy_priv(wiphy);
+	PRINT_D(HOSTAPD_DBG, "Setting beacon\n");
+
+
+#ifndef WILC_FULLY_HOSTING_AP
+	s32Error = host_int_add_beacon(priv->hWILCWFIDrv,
+					0,
+					0,
+					beacon->head_len, (u8 *)beacon->head,
+					beacon->tail_len, (u8 *)beacon->tail);
+#else
+	s32Error = host_add_beacon(priv->hWILCWFIDrv,
+					0,
+					0,
+					beacon->head_len, (u8 *)beacon->head,
+					beacon->tail_len, (u8 *)beacon->tail);
+#endif
+
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_stop_ap
+ *  @details    Remove beacon configuration and stop sending the beacon.
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	austin
+ *  @date	23 JUL 2013
+ *  @version	1.0
+ */
+static int  WILC_WFI_stop_ap(struct wiphy *wiphy, struct net_device *dev)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	u8 NullBssid[ETH_ALEN] = {0};
+
+
+	WILC_NULLCHECK(s32Error, wiphy);
+
+	priv = wiphy_priv(wiphy);
+
+	PRINT_D(HOSTAPD_DBG, "Deleting beacon\n");
+
+	/*BugID_5188*/
+	linux_wlan_set_bssid(dev, NullBssid);
+
+	#ifndef WILC_FULLY_HOSTING_AP
+	s32Error = host_int_del_beacon(priv->hWILCWFIDrv);
+	#else
+	s32Error = host_del_beacon(priv->hWILCWFIDrv);
+	#endif
+
+	WILC_ERRORCHECK(s32Error);
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_add_station
+ *  @details    Add a new station.
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int  WILC_WFI_add_station(struct wiphy *wiphy, struct net_device *dev,
+				 const u8 *mac, struct station_parameters *params)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	tstrWILC_AddStaParam strStaParams = {{0}};
+	perInterface_wlan_t *nic;
+
+
+	WILC_NULLCHECK(s32Error, wiphy);
+
+	priv = wiphy_priv(wiphy);
+	nic = netdev_priv(dev);
+
+	if (nic->iftype == AP_MODE || nic->iftype == GO_MODE) {
+		#ifndef WILC_FULLY_HOSTING_AP
+
+		WILC_memcpy(strStaParams.au8BSSID, mac, ETH_ALEN);
+		WILC_memcpy(priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid], mac, ETH_ALEN);
+		strStaParams.u16AssocID = params->aid;
+		strStaParams.u8NumRates = params->supported_rates_len;
+		strStaParams.pu8Rates = params->supported_rates;
+
+		PRINT_D(CFG80211_DBG, "Adding station parameters %d\n", params->aid);
+
+		PRINT_D(CFG80211_DBG, "BSSID = %x%x%x%x%x%x\n", priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][0], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][1], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][2], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][3], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][4],
+			priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][5]);
+		PRINT_D(HOSTAPD_DBG, "ASSOC ID = %d\n", strStaParams.u16AssocID);
+		PRINT_D(HOSTAPD_DBG, "Number of supported rates = %d\n", strStaParams.u8NumRates);
+
+		if (params->ht_capa == NULL) {
+			strStaParams.bIsHTSupported = false;
+		} else {
+			strStaParams.bIsHTSupported = true;
+			strStaParams.u16HTCapInfo = params->ht_capa->cap_info;
+			strStaParams.u8AmpduParams = params->ht_capa->ampdu_params_info;
+			WILC_memcpy(strStaParams.au8SuppMCsSet, &params->ht_capa->mcs, WILC_SUPP_MCS_SET_SIZE);
+			strStaParams.u16HTExtParams = params->ht_capa->extended_ht_cap_info;
+			strStaParams.u32TxBeamformingCap = params->ht_capa->tx_BF_cap_info;
+			strStaParams.u8ASELCap = params->ht_capa->antenna_selection_info;
+		}
+
+		strStaParams.u16FlagsMask = params->sta_flags_mask;
+		strStaParams.u16FlagsSet = params->sta_flags_set;
+
+		PRINT_D(HOSTAPD_DBG, "IS HT supported = %d\n", strStaParams.bIsHTSupported);
+		PRINT_D(HOSTAPD_DBG, "Capability Info = %d\n", strStaParams.u16HTCapInfo);
+		PRINT_D(HOSTAPD_DBG, "AMPDU Params = %d\n", strStaParams.u8AmpduParams);
+		PRINT_D(HOSTAPD_DBG, "HT Extended params = %d\n", strStaParams.u16HTExtParams);
+		PRINT_D(HOSTAPD_DBG, "Tx Beamforming Cap = %d\n", strStaParams.u32TxBeamformingCap);
+		PRINT_D(HOSTAPD_DBG, "Antenna selection info = %d\n", strStaParams.u8ASELCap);
+		PRINT_D(HOSTAPD_DBG, "Flag Mask = %d\n", strStaParams.u16FlagsMask);
+		PRINT_D(HOSTAPD_DBG, "Flag Set = %d\n", strStaParams.u16FlagsSet);
+
+		s32Error = host_int_add_station(priv->hWILCWFIDrv, &strStaParams);
+		WILC_ERRORCHECK(s32Error);
+
+		#else
+		PRINT_D(CFG80211_DBG, "Adding station parameters %d\n", params->aid);
+		WILC_memcpy(priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid], mac, ETH_ALEN);
+
+		PRINT_D(CFG80211_DBG, "BSSID = %x%x%x%x%x%x\n", priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][0], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][1], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][2], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][3], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][4],
+			priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][5]);
+
+		WILC_AP_AddSta(mac, params);
+		WILC_ERRORCHECK(s32Error);
+		#endif /* WILC_FULLY_HOSTING_AP */
+
+	}
+
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_del_station
+ *  @details    Remove a station; @mac may be NULL to remove all stations.
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_del_station(struct wiphy *wiphy, struct net_device *dev,
+				struct station_del_parameters *params)
+{
+	const u8 *mac = params->mac;
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	perInterface_wlan_t *nic;
+	WILC_NULLCHECK(s32Error, wiphy);
+
+	priv = wiphy_priv(wiphy);
+	nic = netdev_priv(dev);
+
+	if (nic->iftype == AP_MODE || nic->iftype == GO_MODE) {
+		PRINT_D(HOSTAPD_DBG, "Deleting station\n");
+
+
+		if (mac == NULL) {
+			PRINT_D(HOSTAPD_DBG, "All associated stations \n");
+			s32Error = host_int_del_allstation(priv->hWILCWFIDrv, priv->assoc_stainfo.au8Sta_AssociatedBss);
+		} else {
+			PRINT_D(HOSTAPD_DBG, "With mac address: %x%x%x%x%x%x\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+		}
+
+		#ifndef WILC_FULLY_HOSTING_AP
+		s32Error = host_int_del_station(priv->hWILCWFIDrv, mac);
+		#else
+		WILC_AP_RemoveSta(mac);
+		#endif /* WILC_FULLY_HOSTING_AP */
+
+		WILC_ERRORCHECK(s32Error);
+	}
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_change_station
+ *  @details    Modify a given station.
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_change_station(struct wiphy *wiphy, struct net_device *dev,
+				   const u8 *mac, struct station_parameters *params)
+{
+	s32 s32Error = WILC_SUCCESS;
+	struct WILC_WFI_priv *priv;
+	tstrWILC_AddStaParam strStaParams = {{0}};
+	perInterface_wlan_t *nic;
+
+
+	PRINT_D(HOSTAPD_DBG, "Change station paramters\n");
+
+	WILC_NULLCHECK(s32Error, wiphy);
+
+	priv = wiphy_priv(wiphy);
+	nic = netdev_priv(dev);
+
+	if (nic->iftype == AP_MODE || nic->iftype == GO_MODE) {
+		#ifndef WILC_FULLY_HOSTING_AP
+
+		WILC_memcpy(strStaParams.au8BSSID, mac, ETH_ALEN);
+		strStaParams.u16AssocID = params->aid;
+		strStaParams.u8NumRates = params->supported_rates_len;
+		strStaParams.pu8Rates = params->supported_rates;
+
+		PRINT_D(HOSTAPD_DBG, "BSSID = %x%x%x%x%x%x\n", strStaParams.au8BSSID[0], strStaParams.au8BSSID[1], strStaParams.au8BSSID[2], strStaParams.au8BSSID[3], strStaParams.au8BSSID[4],
+			strStaParams.au8BSSID[5]);
+		PRINT_D(HOSTAPD_DBG, "ASSOC ID = %d\n", strStaParams.u16AssocID);
+		PRINT_D(HOSTAPD_DBG, "Number of supported rates = %d\n", strStaParams.u8NumRates);
+
+		if (params->ht_capa == NULL) {
+			strStaParams.bIsHTSupported = false;
+		} else {
+			strStaParams.bIsHTSupported = true;
+			strStaParams.u16HTCapInfo = params->ht_capa->cap_info;
+			strStaParams.u8AmpduParams = params->ht_capa->ampdu_params_info;
+			WILC_memcpy(strStaParams.au8SuppMCsSet, &params->ht_capa->mcs, WILC_SUPP_MCS_SET_SIZE);
+			strStaParams.u16HTExtParams = params->ht_capa->extended_ht_cap_info;
+			strStaParams.u32TxBeamformingCap = params->ht_capa->tx_BF_cap_info;
+			strStaParams.u8ASELCap = params->ht_capa->antenna_selection_info;
+
+		}
+
+		strStaParams.u16FlagsMask = params->sta_flags_mask;
+		strStaParams.u16FlagsSet = params->sta_flags_set;
+
+		PRINT_D(HOSTAPD_DBG, "IS HT supported = %d\n", strStaParams.bIsHTSupported);
+		PRINT_D(HOSTAPD_DBG, "Capability Info = %d\n", strStaParams.u16HTCapInfo);
+		PRINT_D(HOSTAPD_DBG, "AMPDU Params = %d\n", strStaParams.u8AmpduParams);
+		PRINT_D(HOSTAPD_DBG, "HT Extended params = %d\n", strStaParams.u16HTExtParams);
+		PRINT_D(HOSTAPD_DBG, "Tx Beamforming Cap = %d\n", strStaParams.u32TxBeamformingCap);
+		PRINT_D(HOSTAPD_DBG, "Antenna selection info = %d\n", strStaParams.u8ASELCap);
+		PRINT_D(HOSTAPD_DBG, "Flag Mask = %d\n", strStaParams.u16FlagsMask);
+		PRINT_D(HOSTAPD_DBG, "Flag Set = %d\n", strStaParams.u16FlagsSet);
+
+		s32Error = host_int_edit_station(priv->hWILCWFIDrv, &strStaParams);
+		WILC_ERRORCHECK(s32Error);
+
+		#else
+		WILC_AP_EditSta(mac, params);
+		WILC_ERRORCHECK(s32Error);
+		#endif /* WILC_FULLY_HOSTING_AP */
+
+	}
+	WILC_CATCH(s32Error)
+	{
+	}
+	return s32Error;
+}
+
+
+/**
+ *  @brief      WILC_WFI_add_virt_intf
+ *  @details
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 JUL 2012
+ *  @version	1.0
+ */
+struct wireless_dev *WILC_WFI_add_virt_intf(struct wiphy *wiphy, const char *name,
+						unsigned char name_assign_type,
+						enum nl80211_iftype type, u32 *flags,
+						struct vif_params *params)
+{
+	perInterface_wlan_t *nic;
+	struct WILC_WFI_priv *priv;
+	struct net_device *new_ifc = NULL;
+	priv = wiphy_priv(wiphy);
+
+
+
+	PRINT_D(HOSTAPD_DBG, "Adding monitor interface[%p]\n", priv->wdev->netdev);
+
+	nic = netdev_priv(priv->wdev->netdev);
+
+
+	if (type == NL80211_IFTYPE_MONITOR) {
+		PRINT_D(HOSTAPD_DBG, "Monitor interface mode: Initializing mon interface virtual device driver\n");
+		PRINT_D(HOSTAPD_DBG, "Adding monitor interface[%p]\n", nic->wilc_netdev);
+		new_ifc = WILC_WFI_init_mon_interface(name, nic->wilc_netdev);
+		if (new_ifc != NULL) {
+			PRINT_D(HOSTAPD_DBG, "Setting monitor flag in private structure\n");
+			#ifdef SIMULATION
+			priv = netdev_priv(priv->wdev->netdev);
+			priv->monitor_flag = 1;
+			#else
+			nic = netdev_priv(priv->wdev->netdev);
+			nic->monitor_flag = 1;
+			#endif
+		} else
+			PRINT_ER("Error in initializing monitor interface\n ");
+	}
+	return priv->wdev;
+}
+
+/**
+ *  @brief      WILC_WFI_del_virt_intf
+ *  @details
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 JUL 2012
+ *  @version	1.0
+ */
+int WILC_WFI_del_virt_intf(struct wiphy *wiphy, struct wireless_dev *wdev)      /* tony for v3.8 support */
+{
+	PRINT_D(HOSTAPD_DBG, "Deleting virtual interface\n");
+	return WILC_SUCCESS;
+}
+
+
+
+#endif /*WILC_AP_EXTERNAL_MLME*/
+static struct cfg80211_ops WILC_WFI_cfg80211_ops = {
+
+	.set_monitor_channel = WILC_WFI_CfgSetChannel,
+	.scan = WILC_WFI_CfgScan,
+	.connect = WILC_WFI_CfgConnect,
+	.disconnect = WILC_WFI_disconnect,
+	.add_key = WILC_WFI_add_key,
+	.del_key = WILC_WFI_del_key,
+	.get_key = WILC_WFI_get_key,
+	.set_default_key = WILC_WFI_set_default_key,
+	#ifdef WILC_AP_EXTERNAL_MLME
+	.add_virtual_intf = WILC_WFI_add_virt_intf,
+	.del_virtual_intf = WILC_WFI_del_virt_intf,
+	.change_virtual_intf = WILC_WFI_change_virt_intf,
+
+	.start_ap = WILC_WFI_start_ap,
+	.change_beacon = WILC_WFI_change_beacon,
+	.stop_ap = WILC_WFI_stop_ap,
+	.add_station = WILC_WFI_add_station,
+	.del_station = WILC_WFI_del_station,
+	.change_station = WILC_WFI_change_station,
+	#endif /* WILC_AP_EXTERNAL_MLME*/
+	#ifndef WILC_FULLY_HOSTING_AP
+	.get_station = WILC_WFI_get_station,
+	#endif
+	.dump_station = WILC_WFI_dump_station,
+	.change_bss = WILC_WFI_change_bss,
+	.set_wiphy_params = WILC_WFI_set_wiphy_params,
+
+	.set_pmksa = WILC_WFI_set_pmksa,
+	.del_pmksa = WILC_WFI_del_pmksa,
+	.flush_pmksa = WILC_WFI_flush_pmksa,
+#ifdef WILC_P2P
+	.remain_on_channel = WILC_WFI_remain_on_channel,
+	.cancel_remain_on_channel = WILC_WFI_cancel_remain_on_channel,
+	.mgmt_tx_cancel_wait = WILC_WFI_mgmt_tx_cancel_wait,
+	.mgmt_tx = WILC_WFI_mgmt_tx,
+	.mgmt_frame_register = WILC_WFI_frame_register,
+	.set_power_mgmt = WILC_WFI_set_power_mgmt,
+	.set_cqm_rssi_config = WILC_WFI_set_cqm_rssi_config,
+#endif
+
+};
+
+
+
+
+
+/**
+ *  @brief      WILC_WFI_update_stats
+ *  @details    Modify parameters for a given BSS.
+ *  @param[in]
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0WILC_WFI_set_cqmWILC_WFI_set_cqm_rssi_configWILC_WFI_set_cqm_rssi_configWILC_WFI_set_cqm_rssi_configWILC_WFI_set_cqm_rssi_config_rssi_config
+ */
+int WILC_WFI_update_stats(struct wiphy *wiphy, u32 pktlen, u8 changed)
+{
+
+	struct WILC_WFI_priv *priv;
+
+	priv = wiphy_priv(wiphy);
+#if 1
+	switch (changed) {
+
+	case WILC_WFI_RX_PKT:
+	{
+		priv->netstats.rx_packets++;
+		priv->netstats.rx_bytes += pktlen;
+		priv->netstats.rx_time = get_jiffies_64();
+	}
+	break;
+
+	case WILC_WFI_TX_PKT:
+	{
+		priv->netstats.tx_packets++;
+		priv->netstats.tx_bytes += pktlen;
+		priv->netstats.tx_time = get_jiffies_64();
+
+	}
+	break;
+
+	default:
+		break;
+	}
+#endif
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_CfgAlloc
+ *  @details    Allocation of the wireless device structure and assigning it
+ *		to the cfg80211 operations structure.
+ *  @param[in]   NONE
+ *  @return     wireless_dev : Returns pointer to wireless_dev structure.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+struct wireless_dev *WILC_WFI_CfgAlloc(void)
+{
+
+	struct wireless_dev *wdev;
+
+
+	PRINT_D(CFG80211_DBG, "Allocating wireless device\n");
+	/*Allocating the wireless device structure*/
+	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+	if (!wdev) {
+		PRINT_ER("Cannot allocate wireless device\n");
+		goto _fail_;
+	}
+
+	/*Creating a new wiphy, linking wireless structure with the wiphy structure*/
+	wdev->wiphy = wiphy_new(&WILC_WFI_cfg80211_ops, sizeof(struct WILC_WFI_priv));
+	if (!wdev->wiphy) {
+		PRINT_ER("Cannot allocate wiphy\n");
+		goto _fail_mem_;
+
+	}
+
+	#ifdef WILC_AP_EXTERNAL_MLME
+	/* enable 802.11n HT */
+	WILC_WFI_band_2ghz.ht_cap.ht_supported = 1;
+	WILC_WFI_band_2ghz.ht_cap.cap |= (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
+	WILC_WFI_band_2ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+	WILC_WFI_band_2ghz.ht_cap.ampdu_factor = IEEE80211_HT_MAX_AMPDU_8K;
+	WILC_WFI_band_2ghz.ht_cap.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE;
+	#endif
+
+	/*wiphy bands*/
+	wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &WILC_WFI_band_2ghz;
+
+	return wdev;
+
+_fail_mem_:
+	kfree(wdev);
+_fail_:
+	return NULL;
+
+}
+/**
+ *  @brief      WILC_WFI_WiphyRegister
+ *  @details    Registering of the wiphy structure and interface modes
+ *  @param[in]   NONE
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+struct wireless_dev *WILC_WFI_WiphyRegister(struct net_device *net)
+{
+	struct WILC_WFI_priv *priv;
+	struct wireless_dev *wdev;
+	s32 s32Error = WILC_SUCCESS;
+
+	PRINT_D(CFG80211_DBG, "Registering wifi device\n");
+
+	wdev = WILC_WFI_CfgAlloc();
+	if (wdev == NULL) {
+		PRINT_ER("CfgAlloc Failed\n");
+		return NULL;
+	}
+
+
+	/*Return hardware description structure (wiphy)'s priv*/
+	priv = wdev_priv(wdev);
+	sema_init(&(priv->SemHandleUpdateStats), 1);
+
+	/*Link the wiphy with wireless structure*/
+	priv->wdev = wdev;
+
+	/*Maximum number of probed ssid to be added by user for the scan request*/
+	wdev->wiphy->max_scan_ssids = MAX_NUM_PROBED_SSID;
+	/*Maximum number of pmkids to be cashed*/
+	wdev->wiphy->max_num_pmkids = WILC_MAX_NUM_PMKIDS;
+	PRINT_INFO(CFG80211_DBG, "Max number of PMKIDs = %d\n", wdev->wiphy->max_num_pmkids);
+
+	wdev->wiphy->max_scan_ie_len = 1000;
+
+	/*signal strength in mBm (100*dBm) */
+	wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+
+	/*Set the availaible cipher suites*/
+	wdev->wiphy->cipher_suites = cipher_suites;
+	wdev->wiphy->n_cipher_suites = ARRAY_SIZE(cipher_suites);
+	/*Setting default managment types: for register action frame:  */
+	wdev->wiphy->mgmt_stypes = wilc_wfi_cfg80211_mgmt_types;
+
+#ifdef WILC_P2P
+	wdev->wiphy->max_remain_on_channel_duration = 500;
+	/*Setting the wiphy interfcae mode and type before registering the wiphy*/
+	wdev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) | BIT(NL80211_IFTYPE_AP) | BIT(NL80211_IFTYPE_MONITOR) | BIT(NL80211_IFTYPE_P2P_GO) |
+		BIT(NL80211_IFTYPE_P2P_CLIENT);
+	wdev->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+#else
+	wdev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) | BIT(NL80211_IFTYPE_AP) | BIT(NL80211_IFTYPE_MONITOR);
+#endif
+	wdev->iftype = NL80211_IFTYPE_STATION;
+
+
+
+	PRINT_INFO(CFG80211_DBG, "Max scan ids = %d,Max scan IE len = %d,Signal Type = %d,Interface Modes = %d,Interface Type = %d\n",
+		   wdev->wiphy->max_scan_ssids, wdev->wiphy->max_scan_ie_len, wdev->wiphy->signal_type,
+		   wdev->wiphy->interface_modes, wdev->iftype);
+
+	#ifdef WILC_SDIO
+	set_wiphy_dev(wdev->wiphy, &local_sdio_func->dev); /* tony */
+	#endif
+
+	/*Register wiphy structure*/
+	s32Error = wiphy_register(wdev->wiphy);
+	if (s32Error) {
+		PRINT_ER("Cannot register wiphy device\n");
+		/*should define what action to be taken in such failure*/
+	} else {
+		PRINT_D(CFG80211_DBG, "Successful Registering\n");
+	}
+
+	priv->dev = net;
+	return wdev;
+
+
+}
+/**
+ *  @brief      WILC_WFI_WiphyFree
+ *  @details    Freeing allocation of the wireless device structure
+ *  @param[in]   NONE
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_InitHostInt(struct net_device *net)
+{
+
+	s32 s32Error = WILC_SUCCESS;
+
+	struct WILC_WFI_priv *priv;
+
+	PRINT_D(INIT_DBG, "Host[%p][%p]\n", net, net->ieee80211_ptr);
+	priv = wdev_priv(net->ieee80211_ptr);
+	if (op_ifcs == 0) {
+		s32Error = WILC_TimerCreate(&(hAgingTimer), remove_network_from_shadow, NULL);
+		#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+		s32Error = WILC_TimerCreate(&(hDuringIpTimer), clear_duringIP, NULL);
+		#endif
+	}
+	op_ifcs++;
+	if (s32Error < 0) {
+		PRINT_ER("Failed to creat refresh Timer\n");
+		return s32Error;
+	}
+
+	priv->gbAutoRateAdjusted = false;
+
+	priv->bInP2PlistenState = false;
+
+	sema_init(&(priv->hSemScanReq), 1);
+	s32Error = host_int_init(&priv->hWILCWFIDrv);
+	if (s32Error) {
+		PRINT_ER("Error while initializing hostinterface\n");
+	}
+	return s32Error;
+}
+
+/**
+ *  @brief      WILC_WFI_WiphyFree
+ *  @details    Freeing allocation of the wireless device structure
+ *  @param[in]   NONE
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_DeInitHostInt(struct net_device *net)
+{
+	s32 s32Error = WILC_SUCCESS;
+
+	struct WILC_WFI_priv *priv;
+	priv = wdev_priv(net->ieee80211_ptr);
+
+	priv->gbAutoRateAdjusted = false;
+
+	priv->bInP2PlistenState = false;
+
+	op_ifcs--;
+
+	s32Error = host_int_deinit(priv->hWILCWFIDrv);
+
+	/* Clear the Shadow scan */
+	clear_shadow_scan(priv);
+	#ifdef DISABLE_PWRSAVE_AND_SCAN_DURING_IP
+	if (op_ifcs == 0) {
+		PRINT_D(CORECONFIG_DBG, "destroy during ip\n");
+		WILC_TimerDestroy(&hDuringIpTimer, NULL);
+	}
+	#endif
+
+	if (s32Error) {
+		PRINT_ER("Error while deintializing host interface\n");
+	}
+	return s32Error;
+}
+
+
+/**
+ *  @brief      WILC_WFI_WiphyFree
+ *  @details    Freeing allocation of the wireless device structure
+ *  @param[in]   NONE
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_WiphyFree(struct net_device *net)
+{
+
+	PRINT_D(CFG80211_DBG, "Unregistering wiphy\n");
+
+	if (net == NULL) {
+		PRINT_D(INIT_DBG, "net_device is NULL\n");
+		return;
+	}
+
+	if (net->ieee80211_ptr == NULL) {
+		PRINT_D(INIT_DBG, "ieee80211_ptr is NULL\n");
+		return;
+	}
+
+	if (net->ieee80211_ptr->wiphy == NULL) {
+		PRINT_D(INIT_DBG, "wiphy is NULL\n");
+		return;
+	}
+
+	wiphy_unregister(net->ieee80211_ptr->wiphy);
+
+	PRINT_D(INIT_DBG, "Freeing wiphy\n");
+	wiphy_free(net->ieee80211_ptr->wiphy);
+	kfree(net->ieee80211_ptr);
+
+}
diff --git a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.h b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.h
new file mode 100644
index 000000000000..c25350cb58c8
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.h
@@ -0,0 +1,129 @@
+/*!
+ *  @file	wilc_wfi_cfgoperations.h
+ *  @brief	Definitions for the network module
+ *  @author	syounan
+ *  @sa		wilc_oswrapper.h top level OS wrapper file
+ *  @date	31 Aug 2010
+ *  @version	1.0
+ */
+#ifndef NM_WFI_CFGOPERATIONS
+#define NM_WFI_CFGOPERATIONS
+#include "wilc_wfi_netdevice.h"
+
+#ifdef WILC_FULLY_HOSTING_AP
+#include "wilc_host_ap.h"
+#endif
+
+
+/* The following macros describe the bitfield map used by the firmware to determine its 11i mode */
+#define NO_ENCRYPT			0
+#define ENCRYPT_ENABLED	(1 << 0)
+#define WEP					(1 << 1)
+#define WEP_EXTENDED		(1 << 2)
+#define WPA					(1 << 3)
+#define WPA2				(1 << 4)
+#define AES					(1 << 5)
+#define TKIP					(1 << 6)
+
+#ifdef WILC_P2P
+/* #define	USE_SUPPLICANT_GO_INTENT */
+
+/*Public action frame index IDs*/
+#define		FRAME_TYPE_ID					0
+#define		ACTION_CAT_ID					24
+#define		ACTION_SUBTYPE_ID				25
+#define		P2P_PUB_ACTION_SUBTYPE          30
+
+/*Public action frame Attribute IDs*/
+#define		ACTION_FRAME					0xd0
+#define		GO_INTENT_ATTR_ID			0x04
+#define		CHANLIST_ATTR_ID		0x0b
+#define		OPERCHAN_ATTR_ID                0x11
+#ifdef	USE_SUPPLICANT_GO_INTENT
+#define	GROUP_BSSID_ATTR_ID			0x07
+#endif
+#define		PUB_ACTION_ATTR_ID			0x04
+#define		P2PELEM_ATTR_ID                     0xdd
+
+/*Public action subtype values*/
+#define		GO_NEG_REQ					0x00
+#define		GO_NEG_RSP					0x01
+#define		GO_NEG_CONF					0x02
+#define		P2P_INV_REQ                         0x03
+#define		P2P_INV_RSP				0x04
+#define		PUBLIC_ACT_VENDORSPEC		0x09
+#define		GAS_INTIAL_REQ					0x0a
+#define		GAS_INTIAL_RSP					0x0b
+
+#define		INVALID_CHANNEL					0
+#ifdef	USE_SUPPLICANT_GO_INTENT
+#define		SUPPLICANT_GO_INTENT			6
+#define		GET_GO_INTENT(a)				(((a) >> 1) & 0x0f)
+#define		GET_TIE_BREAKER(a)			(((a)) & 0x01)
+#else
+/* #define FORCE_P2P_CLIENT */
+#endif
+#endif
+
+#define nl80211_SCAN_RESULT_EXPIRE	(3 * HZ)
+#define SCAN_RESULT_EXPIRE				(40 * HZ)
+
+static const u32 cipher_suites[] = {
+	WLAN_CIPHER_SUITE_WEP40,
+	WLAN_CIPHER_SUITE_WEP104,
+	WLAN_CIPHER_SUITE_TKIP,
+	WLAN_CIPHER_SUITE_CCMP,
+	WLAN_CIPHER_SUITE_AES_CMAC,
+};
+
+static const struct ieee80211_txrx_stypes
+	wilc_wfi_cfg80211_mgmt_types[NUM_NL80211_IFTYPES] = {
+	[NL80211_IFTYPE_STATION] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+			BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
+	},
+	[NL80211_IFTYPE_AP] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+			BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+			BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+			BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+			BIT(IEEE80211_STYPE_AUTH >> 4) |
+			BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+			BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_P2P_CLIENT] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+			BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+			BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+			BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+			BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+			BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+			BIT(IEEE80211_STYPE_AUTH >> 4) |
+			BIT(IEEE80211_STYPE_DEAUTH >> 4)
+	}
+};
+
+/* Time to stay on the channel */
+#define WILC_WFI_DWELL_PASSIVE 100
+#define WILC_WFI_DWELL_ACTIVE  40
+
+struct wireless_dev *WILC_WFI_CfgAlloc(void);
+struct wireless_dev *WILC_WFI_WiphyRegister(struct net_device *net);
+void WILC_WFI_WiphyFree(struct net_device *net);
+int WILC_WFI_update_stats(struct wiphy *wiphy, u32 pktlen, u8 changed);
+int WILC_WFI_DeInitHostInt(struct net_device *net);
+int WILC_WFI_InitHostInt(struct net_device *net);
+void WILC_WFI_monitor_rx(uint8_t *buff, uint32_t size);
+int WILC_WFI_deinit_mon_interface(void);
+struct net_device *WILC_WFI_init_mon_interface(const char *name, struct net_device *real_dev);
+
+#ifdef TCP_ENHANCEMENTS
+#define TCP_ACK_FILTER_LINK_SPEED_THRESH 54
+#define DEFAULT_LINK_SPEED 72
+extern void Enable_TCP_ACK_Filter(bool value);
+#endif
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_wfi_netdevice.c b/drivers/staging/wilc1000/wilc_wfi_netdevice.c
new file mode 100644
index 000000000000..ab66ce4bd790
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wfi_netdevice.c
@@ -0,0 +1,951 @@
+/*!
+ *  @file	wilc_wfi_netdevice.c
+ *  @brief	File Operations OS wrapper functionality
+ *  @author	mdaftedar
+ *  @sa		wilc_wfi_netdevice.h
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+
+#ifdef SIMULATION
+
+#include "wilc_wfi_cfgoperations.h"
+#include "host_interface.h"
+
+
+MODULE_AUTHOR("Mai Daftedar");
+MODULE_LICENSE("Dual BSD/GPL");
+
+
+struct net_device *WILC_WFI_devs[2];
+
+/*
+ * Transmitter lockup simulation, normally disabled.
+ */
+static int lockup;
+module_param(lockup, int, 0);
+
+static int timeout = WILC_WFI_TIMEOUT;
+module_param(timeout, int, 0);
+
+/*
+ * Do we run in NAPI mode?
+ */
+static int use_napi ;
+module_param(use_napi, int, 0);
+
+
+/*
+ * A structure representing an in-flight packet.
+ */
+struct WILC_WFI_packet {
+	struct WILC_WFI_packet *next;
+	struct net_device *dev;
+	int datalen;
+	u8 data[ETH_DATA_LEN];
+};
+
+
+
+int pool_size = 8;
+module_param(pool_size, int, 0);
+
+
+static void WILC_WFI_TxTimeout(struct net_device *dev);
+static void (*WILC_WFI_Interrupt)(int, void *, struct pt_regs *);
+
+/**
+ *  @brief      WILC_WFI_SetupPool
+ *  @details    Set up a device's packet pool.
+ *  @param[in]  struct net_device *dev : Network Device Pointer
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_SetupPool(struct net_device *dev)
+{
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+	int i;
+	struct WILC_WFI_packet *pkt;
+
+	priv->ppool = NULL;
+	for (i = 0; i < pool_size; i++) {
+		pkt = kmalloc (sizeof (struct WILC_WFI_packet), GFP_KERNEL);
+		if (pkt == NULL) {
+			PRINT_D(RX_DBG, "Ran out of memory allocating packet pool\n");
+			return;
+		}
+		pkt->dev = dev;
+		pkt->next = priv->ppool;
+		priv->ppool = pkt;
+	}
+}
+
+/**
+ *  @brief      WILC_WFI_TearDownPool
+ *  @details    Internal cleanup function that's called after the network device
+ *              driver is unregistered
+ *  @param[in]  struct net_device *dev : Network Device Driver
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_TearDownPool(struct net_device *dev)
+{
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+	struct WILC_WFI_packet *pkt;
+
+	while ((pkt = priv->ppool)) {
+		priv->ppool = pkt->next;
+		kfree (pkt);
+		/* FIXME - in-flight packets ? */
+	}
+}
+
+/**
+ *  @brief      WILC_WFI_GetTxBuffer
+ *  @details    Buffer/pool management
+ *  @param[in]  net_device *dev : Network Device Driver Structure
+ *  @return     struct WILC_WFI_packet
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+struct WILC_WFI_packet *WILC_WFI_GetTxBuffer(struct net_device *dev)
+{
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+	unsigned long flags;
+	struct WILC_WFI_packet *pkt;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	pkt = priv->ppool;
+	priv->ppool = pkt->next;
+	if (priv->ppool == NULL) {
+		PRINT_INFO(RX_DBG, "Pool empty\n");
+		netif_stop_queue(dev);
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return pkt;
+}
+/**
+ *  @brief      WILC_WFI_ReleaseBuffer
+ *  @details    Buffer/pool management
+ *  @param[in]  WILC_WFI_packet *pkt : Structure holding in-flight packet
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_ReleaseBuffer(struct WILC_WFI_packet *pkt)
+{
+	unsigned long flags;
+	struct WILC_WFI_priv *priv = netdev_priv(pkt->dev);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	pkt->next = priv->ppool;
+	priv->ppool = pkt;
+	spin_unlock_irqrestore(&priv->lock, flags);
+	if (netif_queue_stopped(pkt->dev) && pkt->next == NULL)
+		netif_wake_queue(pkt->dev);
+}
+
+/**
+ *  @brief      WILC_WFI_EnqueueBuf
+ *  @details    Enqueuing packets in an RX buffer queue
+ *  @param[in]  WILC_WFI_packet *pkt : Structure holding in-flight packet
+ *  @param[in]   net_device *dev : Network Device Driver Structure
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_EnqueueBuf(struct net_device *dev, struct WILC_WFI_packet *pkt)
+{
+	unsigned long flags;
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	pkt->next = priv->rx_queue;   /* FIXME - misorders packets */
+	priv->rx_queue = pkt;
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+/**
+ *  @brief      WILC_WFI_DequeueBuf
+ *  @details    Dequeuing packets from the RX buffer queue
+ *  @param[in]   net_device *dev : Network Device Driver Structure
+ *  @return     WILC_WFI_packet *pkt : Structure holding in-flight pac
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+struct WILC_WFI_packet *WILC_WFI_DequeueBuf(struct net_device *dev)
+{
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+	struct WILC_WFI_packet *pkt;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	pkt = priv->rx_queue;
+	if (pkt != NULL)
+		priv->rx_queue = pkt->next;
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return pkt;
+}
+/**
+ *  @brief      WILC_WFI_RxInts
+ *  @details    Enable and disable receive interrupts.
+ *  @param[in]   net_device *dev : Network Device Driver Structure
+ *  @param[in]	enable : Enable/Disable flag
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static void WILC_WFI_RxInts(struct net_device *dev, int enable)
+{
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+	priv->rx_int_enabled = enable;
+}
+
+/**
+ *  @brief      WILC_WFI_Open
+ *  @details    Open Network Device Driver, called when the network
+ *              interface is opened. It starts the interface's transmit queue.
+ *  @param[in]   net_device *dev : Network Device Driver Structure
+ *  @param[in]	enable : Enable/Disable flag
+ *  @return     int : Returns 0 upon success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_Open(struct net_device *dev)
+{
+	/* request_region(), request_irq(), ....  (like fops->open) */
+	/*
+	 * Assign the hardware address of the board: use "\0SNULx", where
+	 * x is 0 or 1. The first byte is '\0' to avoid being a multicast
+	 * address (the first byte of multicast addrs is odd).
+	 */
+	memcpy(dev->dev_addr, "\0WLAN0", ETH_ALEN);
+	if (dev == WILC_WFI_devs[1])
+		dev->dev_addr[ETH_ALEN - 1]++;  /* \0SNUL1 */
+
+	WILC_WFI_InitHostInt(dev);
+	netif_start_queue(dev);
+	return 0;
+}
+/**
+ *  @brief      WILC_WFI_Release
+ *  @details    Release Network Device Driver, called when the network
+ *              interface is stopped or brought down. This function marks
+ *              the network driver as not being able to transmit
+ *  @param[in]   net_device *dev : Network Device Driver Structure
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_Release(struct net_device *dev)
+{
+	/* release ports, irq and such -- like fops->close */
+
+	netif_stop_queue(dev); /* can't transmit any more */
+
+	return 0;
+}
+/**
+ *  @brief      WILC_WFI_Config
+ *  @details    Configuration changes (passed on by ifconfig)
+ *  @param[in]   net_device *dev : Network Device Driver Structure
+ *  @param[in]	struct ifmap *map : Contains the ioctl implementation for the
+ *              network driver.
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_Config(struct net_device *dev, struct ifmap *map)
+{
+	if (dev->flags & IFF_UP) /* can't act on a running interface */
+		return -EBUSY;
+
+	/* Don't allow changing the I/O address */
+	if (map->base_addr != dev->base_addr) {
+		PRINT_D(RX_DBG, KERN_WARNING "WILC_WFI: Can't change I/O address\n");
+		return -EOPNOTSUPP;
+	}
+
+	/* Allow changing the IRQ */
+	if (map->irq != dev->irq) {
+		dev->irq = map->irq;
+		/* request_irq() is delayed to open-time */
+	}
+
+	/* ignore other fields */
+	return 0;
+}
+/**
+ *  @brief      WILC_WFI_Rx
+ *  @details    Receive a packet: retrieve, encapsulate and pass over to upper
+ *              levels
+ *  @param[in]   net_device *dev : Network Device Driver Structure
+ *  @param[in]	WILC_WFI_packet :
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_Rx(struct net_device *dev, struct WILC_WFI_packet *pkt)
+{
+	int i;
+	struct sk_buff *skb;
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+	s8 rssi;
+	/*
+	 * The packet has been retrieved from the transmission
+	 * medium. Build an skb around it, so upper layers can handle it
+	 */
+
+
+	skb = dev_alloc_skb(pkt->datalen + 2);
+	if (!skb) {
+		if (printk_ratelimit())
+			PRINT_D(RX_DBG, "WILC_WFI rx: low on mem - packet dropped\n");
+		priv->stats.rx_dropped++;
+		goto out;
+	}
+	skb_reserve(skb, 2);  /* align IP on 16B boundary */
+	memcpy(skb_put(skb, pkt->datalen), pkt->data, pkt->datalen);
+
+	if (priv->monitor_flag) {
+		PRINT_INFO(RX_DBG, "In monitor device name %s\n", dev->name);
+		priv = wiphy_priv(priv->dev->ieee80211_ptr->wiphy);
+		PRINT_D(RX_DBG, "VALUE PASSED IN OF HRWD %p\n", priv->hWILCWFIDrv);
+		/* host_int_get_rssi(priv->hWILCWFIDrv, &(rssi)); */
+		if (INFO) {
+			for (i = 14; i < skb->len; i++)
+				PRINT_INFO(RX_DBG, "RXdata[%d] %02x\n", i, skb->data[i]);
+		}
+		WILC_WFI_monitor_rx(dev, skb);
+		return;
+	}
+out:
+	return;
+}
+
+/**
+ *  @brief      WILC_WFI_Poll
+ *  @details    The poll implementation
+ *  @param[in]   struct napi_struct *napi :
+ *  @param[in]	int budget :
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static int WILC_WFI_Poll(struct napi_struct *napi, int budget)
+{
+	int npackets = 0;
+	struct sk_buff *skb;
+	struct WILC_WFI_priv  *priv = container_of(napi, struct WILC_WFI_priv, napi);
+	struct net_device  *dev =  priv->dev;
+	struct WILC_WFI_packet *pkt;
+
+	while (npackets < budget && priv->rx_queue) {
+		pkt = WILC_WFI_DequeueBuf(dev);
+		skb = dev_alloc_skb(pkt->datalen + 2);
+		if (!skb) {
+			if (printk_ratelimit())
+				PRINT_D(RX_DBG, "WILC_WFI: packet dropped\n");
+			priv->stats.rx_dropped++;
+			WILC_WFI_ReleaseBuffer(pkt);
+			continue;
+		}
+		skb_reserve(skb, 2);  /* align IP on 16B boundary */
+		memcpy(skb_put(skb, pkt->datalen), pkt->data, pkt->datalen);
+		skb->dev = dev;
+		skb->protocol = eth_type_trans(skb, dev);
+		skb->ip_summed = CHECKSUM_UNNECESSARY;  /* don't check it */
+		netif_receive_skb(skb);
+		/* Maintain stats */
+		npackets++;
+		WILC_WFI_update_stats(priv->dev->ieee80211_ptr->wiphy, pkt->datalen, WILC_WFI_RX_PKT);
+		WILC_WFI_ReleaseBuffer(pkt);
+	}
+	/* If we processed all packets, we're done; tell the kernel and re-enable ints */
+	if (npackets < budget) {
+		napi_complete(napi);
+		WILC_WFI_RxInts(dev, 1);
+	}
+	return npackets;
+}
+
+/**
+ *  @brief      WILC_WFI_Poll
+ *  @details    The typical interrupt entry point
+ *  @param[in]   struct napi_struct *napi :
+ *  @param[in]	int budget :
+ *  @return     int : Return 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static void WILC_WFI_RegularInterrupt(int irq, void *dev_id, struct pt_regs *regs)
+{
+	int statusword;
+	struct WILC_WFI_priv *priv;
+	struct WILC_WFI_packet *pkt = NULL;
+	/*
+	 * As usual, check the "device" pointer to be sure it is
+	 * really interrupting.
+	 * Then assign "struct device *dev"
+	 */
+	struct net_device *dev = (struct net_device *)dev_id;
+	/* ... and check with hw if it's really ours */
+
+	/* paranoid */
+	if (!dev)
+		return;
+
+	/* Lock the device */
+	priv = netdev_priv(dev);
+	spin_lock(&priv->lock);
+
+	/* retrieve statusword: real netdevices use I/O instructions */
+	statusword = priv->status;
+	priv->status = 0;
+	if (statusword & WILC_WFI_RX_INTR) {
+		/* send it to WILC_WFI_rx for handling */
+		pkt = priv->rx_queue;
+		if (pkt) {
+			priv->rx_queue = pkt->next;
+			WILC_WFI_Rx(dev, pkt);
+		}
+	}
+	if (statusword & WILC_WFI_TX_INTR) {
+		/* a transmission is over: free the skb */
+		WILC_WFI_update_stats(priv->dev->ieee80211_ptr->wiphy, priv->tx_packetlen, WILC_WFI_TX_PKT);
+		dev_kfree_skb(priv->skb);
+	}
+
+	/* Unlock the device and we are done */
+	spin_unlock(&priv->lock);
+	if (pkt)
+		WILC_WFI_ReleaseBuffer(pkt);  /* Do this outside the lock! */
+	return;
+}
+/**
+ *  @brief      WILC_WFI_NapiInterrupt
+ *  @details    A NAPI interrupt handler
+ *  @param[in]   irq:
+ *  @param[in]	dev_id:
+ *  @param[in]	pt_regs:
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+static void WILC_WFI_NapiInterrupt(int irq, void *dev_id, struct pt_regs *regs)
+{
+	int statusword;
+	struct WILC_WFI_priv *priv;
+
+	/*
+	 * As usual, check the "device" pointer for shared handlers.
+	 * Then assign "struct device *dev"
+	 */
+	struct net_device *dev = (struct net_device *)dev_id;
+	/* ... and check with hw if it's really ours */
+
+	/* paranoid */
+	if (!dev)
+		return;
+
+	/* Lock the device */
+	priv = netdev_priv(dev);
+	spin_lock(&priv->lock);
+
+	/* retrieve statusword: real netdevices use I/O instructions */
+	statusword = priv->status;
+	priv->status = 0;
+	if (statusword & WILC_WFI_RX_INTR) {
+		WILC_WFI_RxInts(dev, 0);   /* Disable further interrupts */
+		napi_schedule(&priv->napi);
+	}
+	if (statusword & WILC_WFI_TX_INTR) {
+		/* a transmission is over: free the skb */
+
+		WILC_WFI_update_stats(priv->dev->ieee80211_ptr->wiphy, priv->tx_packetlen, WILC_WFI_TX_PKT);
+		dev_kfree_skb(priv->skb);
+	}
+
+	/* Unlock the device and we are done */
+	spin_unlock(&priv->lock);
+	return;
+}
+
+/**
+ *  @brief      MI_WFI_HwTx
+ *  @details    Transmit a packet (low level interface)
+ *  @param[in]   buf:
+ *  @param[in]	len:
+ *  @param[in]	net_device *dev:
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_HwTx(char *buf, int len, struct net_device *dev)
+{
+	/*
+	 * This function deals with hw details. This interface loops
+	 * back the packet to the other WILC_WFI interface (if any).
+	 * In other words, this function implements the WILC_WFI behaviour,
+	 * while all other procedures are rather device-independent
+	 */
+	struct iphdr *ih;
+	struct net_device *dest;
+	struct WILC_WFI_priv *priv;
+	u32 *saddr, *daddr;
+	struct WILC_WFI_packet *tx_buffer;
+
+
+	/* I am paranoid. Ain't I? */
+	if (len < sizeof(struct ethhdr) + sizeof(struct iphdr)) {
+		PRINT_D(RX_DBG, "WILC_WFI: Hmm... packet too short (%i octets)\n",
+			len);
+		return;
+	}
+
+	if (0) {  /* enable this conditional to look at the data */
+		int i;
+		PRINT_D(RX_DBG, "len is %i", len);
+		for (i = 14; i < len; i++)
+			PRINT_D(RX_DBG, "TXdata[%d] %02x\n", i, buf[i] & 0xff);
+		/*   PRINT_D(RX_DBG, "\n"); */
+	}
+	/*
+	 * Ethhdr is 14 bytes, but the kernel arranges for iphdr
+	 * to be aligned (i.e., ethhdr is unaligned)
+	 */
+	ih = (struct iphdr *)(buf + sizeof(struct ethhdr));
+	saddr = &ih->saddr;
+	daddr = &ih->daddr;
+
+	((u8 *)saddr)[2] ^= 1;  /* change the third octet (class C) */
+	((u8 *)daddr)[2] ^= 1;
+
+	ih->check = 0;          /* and rebuild the checksum (ip needs it) */
+	ih->check = ip_fast_csum((unsigned char *)ih, ih->ihl);
+
+
+	if (dev == WILC_WFI_devs[0])
+		PRINT_D(RX_DBG, "%08x:%05i --> %08x:%05i\n",
+			ntohl(ih->saddr), ntohs(((struct tcphdr *)(ih + 1))->source),
+			ntohl(ih->daddr), ntohs(((struct tcphdr *)(ih + 1))->dest));
+	else
+		PRINT_D(RX_DBG, "%08x:%05i <-- %08x:%05i\n",
+			ntohl(ih->daddr), ntohs(((struct tcphdr *)(ih + 1))->dest),
+			ntohl(ih->saddr), ntohs(((struct tcphdr *)(ih + 1))->source));
+
+	/*
+	 * Ok, now the packet is ready for transmission: first simulate a
+	 * receive interrupt on the twin device, then  a
+	 * transmission-done on the transmitting device
+	 */
+	dest = WILC_WFI_devs[dev == WILC_WFI_devs[0] ? 1 : 0];
+	priv = netdev_priv(dest);
+
+	tx_buffer = WILC_WFI_GetTxBuffer(dev);
+	tx_buffer->datalen = len;
+	memcpy(tx_buffer->data, buf, len);
+	WILC_WFI_EnqueueBuf(dest, tx_buffer);
+	if (priv->rx_int_enabled) {
+		priv->status |= WILC_WFI_RX_INTR;
+		WILC_WFI_Interrupt(0, dest, NULL);
+	}
+
+	priv = netdev_priv(dev);
+	priv->tx_packetlen = len;
+	priv->tx_packetdata = buf;
+	priv->status |= WILC_WFI_TX_INTR;
+	if (lockup && ((priv->stats.tx_packets + 1) % lockup) == 0) {
+		/* Simulate a dropped transmit interrupt */
+		netif_stop_queue(dev);
+		PRINT_D(RX_DBG, "Simulate lockup at %ld, txp %ld\n", jiffies,
+			(unsigned long) priv->stats.tx_packets);
+	} else
+		WILC_WFI_Interrupt(0, dev, NULL);
+
+}
+
+/**
+ *  @brief      WILC_WFI_Tx
+ *  @details    Transmit a packet (called by the kernel)
+ *  @param[in]   sk_buff *skb:
+ *  @param[in]	net_device *dev:
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_Tx(struct sk_buff *skb, struct net_device *dev)
+{
+	int len;
+	char *data, shortpkt[ETH_ZLEN];
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+
+	/* priv = wiphy_priv(priv->dev->ieee80211_ptr->wiphy); */
+
+	/*  if(priv->monitor_flag) */
+	/*	 mac80211_hwsim_monitor_rx(skb); */
+
+
+	data = skb->data;
+	len = skb->len;
+
+	if (len < ETH_ZLEN) {
+		memset(shortpkt, 0, ETH_ZLEN);
+		memcpy(shortpkt, skb->data, skb->len);
+		len = ETH_ZLEN;
+		data = shortpkt;
+	}
+	dev->trans_start = jiffies;  /* save the timestamp */
+
+	/* Remember the skb, so we can free it at interrupt time */
+	priv->skb = skb;
+
+	/* actual deliver of data is device-specific, and not shown here */
+	WILC_WFI_HwTx(data, len, dev);
+
+	return 0;  /* Our simple device can not fail */
+}
+
+/**
+ *  @brief      WILC_WFI_TxTimeout
+ *  @details    Deal with a transmit timeout.
+ *  @param[in]	net_device *dev:
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_TxTimeout(struct net_device *dev)
+{
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+
+	PRINT_D(RX_DBG, "Transmit timeout at %ld, latency %ld\n", jiffies,
+		jiffies - dev->trans_start);
+	/* Simulate a transmission interrupt to get things moving */
+	priv->status = WILC_WFI_TX_INTR;
+	WILC_WFI_Interrupt(0, dev, NULL);
+	priv->stats.tx_errors++;
+	netif_wake_queue(dev);
+	return;
+}
+
+/**
+ *  @brief      WILC_WFI_Ioctl
+ *  @details    Ioctl commands
+ *  @param[in]	net_device *dev:
+ *  @param[in]	ifreq *rq
+ *  @param[in]	cmd:
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_Ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+{
+	PRINT_D(RX_DBG, "ioctl\n");
+	return 0;
+}
+
+/**
+ *  @brief      WILC_WFI_Stat
+ *  @details    Return statistics to the caller
+ *  @param[in]	net_device *dev:
+ *  @return     WILC_WFI_Stats : Return net_device_stats stucture with the
+ *              network device driver private data contents.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+struct net_device_stats *WILC_WFI_Stats(struct net_device *dev)
+{
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+	return &priv->stats;
+}
+
+/**
+ *  @brief      WILC_WFI_RebuildHeader
+ *  @details    This function is called to fill up an eth header, since arp is not
+ *              available on the interface
+ *  @param[in]	sk_buff *skb:
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_RebuildHeader(struct sk_buff *skb)
+{
+	struct ethhdr *eth = (struct ethhdr *) skb->data;
+	struct net_device *dev = skb->dev;
+
+	memcpy(eth->h_source, dev->dev_addr, dev->addr_len);
+	memcpy(eth->h_dest, dev->dev_addr, dev->addr_len);
+	eth->h_dest[ETH_ALEN - 1]   ^= 0x01;  /* dest is us xor 1 */
+	return 0;
+}
+/**
+ *  @brief      WILC_WFI_RebuildHeader
+ *  @details    This function is called to fill up an eth header, since arp is not
+ *              available on the interface
+ *  @param[in]	sk_buff *skb:
+ *  @param[in]	struct net_device *dev:
+ *  @param[in]   unsigned short type:
+ *  @param[in]   const void *saddr,
+ *  @param[in]   const void *daddr:
+ *  @param[in]          unsigned int len
+ *  @return     int : Return 0 on Success
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_Header(struct sk_buff *skb, struct net_device *dev,
+		    unsigned short type, const void *daddr, const void *saddr,
+		    unsigned int len)
+{
+	struct ethhdr *eth = (struct ethhdr *)skb_push(skb, ETH_HLEN);
+
+	eth->h_proto = htons(type);
+	memcpy(eth->h_source, saddr ? saddr : dev->dev_addr, dev->addr_len);
+	memcpy(eth->h_dest,   daddr ? daddr : dev->dev_addr, dev->addr_len);
+	eth->h_dest[ETH_ALEN - 1]   ^= 0x01;  /* dest is us xor 1 */
+	return dev->hard_header_len;
+}
+
+/**
+ *  @brief      WILC_WFI_ChangeMtu
+ *  @details    The "change_mtu" method is usually not needed.
+ *              If you need it, it must be like this.
+ *  @param[in]	net_device *dev : Network Device Driver Structure
+ *  @param[in]	new_mtu :
+ *  @return     int : Returns 0 on Success.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_ChangeMtu(struct net_device *dev, int new_mtu)
+{
+	unsigned long flags;
+	struct WILC_WFI_priv *priv = netdev_priv(dev);
+	spinlock_t *lock = &priv->lock;
+
+	/* check ranges */
+	if ((new_mtu < 68) || (new_mtu > 1500))
+		return -EINVAL;
+	/*
+	 * Do anything you need, and the accept the value
+	 */
+	spin_lock_irqsave(lock, flags);
+	dev->mtu = new_mtu;
+	spin_unlock_irqrestore(lock, flags);
+	return 0;  /* success */
+}
+
+static const struct header_ops WILC_WFI_header_ops = {
+	.create  = WILC_WFI_Header,
+	.rebuild = WILC_WFI_RebuildHeader,
+	.cache   = NULL,   /* disable caching */
+};
+
+
+static const struct net_device_ops WILC_WFI_netdev_ops = {
+	.ndo_open = WILC_WFI_Open,
+	.ndo_stop = WILC_WFI_Release,
+	.ndo_set_config = WILC_WFI_Config,
+	.ndo_start_xmit = WILC_WFI_Tx,
+	.ndo_do_ioctl = WILC_WFI_Ioctl,
+	.ndo_get_stats = WILC_WFI_Stats,
+	.ndo_change_mtu = WILC_WFI_ChangeMtu,
+	.ndo_tx_timeout = WILC_WFI_TxTimeout,
+};
+
+/**
+ *  @brief      WILC_WFI_Init
+ *  @details    The init function (sometimes called probe).
+ *              It is invoked by register_netdev()
+ *  @param[in]	net_device *dev:
+ *  @return     NONE
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+void WILC_WFI_Init(struct net_device *dev)
+{
+	struct WILC_WFI_priv *priv;
+
+
+	/*
+	 * Then, assign other fields in dev, using ether_setup() and some
+	 * hand assignments
+	 */
+	ether_setup(dev);  /* assign some of the fields */
+	/* 1- Allocate space */
+
+	dev->netdev_ops      = &WILC_WFI_netdev_ops;
+	dev->header_ops      = &WILC_WFI_header_ops;
+	dev->watchdog_timeo = timeout;
+	/* keep the default flags, just add NOARP */
+	dev->flags           |= IFF_NOARP;
+	dev->features        |= NETIF_F_NO_CSUM;
+	/*
+	 * Then, initialize the priv field. This encloses the statistics
+	 * and a few private fields.
+	 */
+	priv = netdev_priv(dev);
+	memset(priv, 0, sizeof(struct WILC_WFI_priv));
+	priv->dev = dev;
+	netif_napi_add(dev, &priv->napi, WILC_WFI_Poll, 2);
+	/* The last parameter above is the NAPI "weight". */
+	spin_lock_init(&priv->lock);
+	WILC_WFI_RxInts(dev, 1);           /* enable receive interrupts */
+	WILC_WFI_SetupPool(dev);
+}
+
+/**
+ *  @brief      WILC_WFI_Stat
+ *  @details    Return statistics to the caller
+ *  @param[in]	net_device *dev:
+ *  @return     WILC_WFI_Stats : Return net_device_stats stucture with the
+ *              network device driver private data contents.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+
+void WILC_WFI_Cleanup(void)
+{
+	int i;
+	struct WILC_WFI_priv *priv[2];
+
+	/*if(hwsim_mon!=NULL)
+	 * {
+	 *      PRINT_D(RX_DBG, "Freeing monitor interface\n");
+	 *      unregister_netdev(hwsim_mon);
+	 *      free_netdev(hwsim_mon);
+	 * }*/
+	for (i = 0; i < 2; i++) {
+		priv[i] = netdev_priv(WILC_WFI_devs[i]);
+
+		if (WILC_WFI_devs[i]) {
+			PRINT_D(RX_DBG, "Unregistering\n");
+			unregister_netdev(WILC_WFI_devs[i]);
+			WILC_WFI_TearDownPool(WILC_WFI_devs[i]);
+			free_netdev(WILC_WFI_devs[i]);
+			PRINT_D(RX_DBG, "[NETDEV]Stopping interface\n");
+			WILC_WFI_DeInitHostInt(WILC_WFI_devs[i]);
+			WILC_WFI_WiphyFree(WILC_WFI_devs[i]);
+		}
+
+	}
+	/* unregister_netdev(hwsim_mon); */
+	WILC_WFI_deinit_mon_interface();
+	return;
+}
+
+
+void StartConfigSim(void);
+
+
+
+
+
+
+
+/**
+ *  @brief      WILC_WFI_Stat
+ *  @details    Return statistics to the caller
+ *  @param[in]	net_device *dev:
+ *  @return     WILC_WFI_Stats : Return net_device_stats stucture with the
+ *              network device driver private data contents.
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+int WILC_WFI_InitModule(void)
+{
+
+	int result, i, ret = -ENOMEM;
+	struct WILC_WFI_priv *priv[2], *netpriv;
+	struct wireless_dev *wdev;
+	WILC_WFI_Interrupt = use_napi ? WILC_WFI_NapiInterrupt : WILC_WFI_RegularInterrupt;
+	char buf[IFNAMSIZ];
+
+	for (i = 0; i < 2; i++)	{
+
+		/* Allocate the net devices */
+		WILC_WFI_devs[i] = alloc_netdev(sizeof(struct WILC_WFI_priv), "wlan%d",
+						WILC_WFI_Init);
+		if (WILC_WFI_devs[i] == NULL)
+			goto out;
+		/* priv[i] = netdev_priv(WILC_WFI_devs[i]); */
+
+		wdev = WILC_WFI_WiphyRegister(WILC_WFI_devs[i]);
+		WILC_WFI_devs[i]->ieee80211_ptr = wdev;
+		netpriv = netdev_priv(WILC_WFI_devs[i]);
+		netpriv->dev->ieee80211_ptr = wdev;
+		netpriv->dev->ml_priv = netpriv;
+		wdev->netdev = netpriv->dev;
+
+		/*Registering the net device*/
+		result = register_netdev(WILC_WFI_devs[i]);
+		if (result)
+			PRINT_D(RX_DBG, "WILC_WFI: error %i registering device \"%s\"\n",
+				result, WILC_WFI_devs[i]->name);
+		else
+			ret = 0;
+	}
+
+
+	/*init atmel driver */
+	priv[0] = netdev_priv(WILC_WFI_devs[0]);
+	priv[1] = netdev_priv(WILC_WFI_devs[1]);
+
+	if (priv[1]->dev->ieee80211_ptr->wiphy->interface_modes && BIT(NL80211_IFTYPE_MONITOR))	{
+		/* snprintf(buf, IFNAMSIZ, "mon.%s",  priv[1]->dev->name); */
+		/*	WILC_WFI_init_mon_interface(); */
+		/*	priv[1]->monitor_flag = 1; */
+
+	}
+	priv[0]->bCfgScanning = false;
+	priv[0]->u32RcvdChCount = 0;
+
+	WILC_memset(priv[0]->au8AssociatedBss, 0xFF, ETH_ALEN);
+
+
+	/* ret = host_int_init(&priv[0]->hWILCWFIDrv); */
+	/*copy handle to the other driver*/
+	/* priv[1]->hWILCWFIDrv = priv[0]->hWILCWFIDrv; */
+	if (ret) {
+		PRINT_ER("Error Init Driver\n");
+	}
+
+
+out:
+	if (ret)
+		WILC_WFI_Cleanup();
+	return ret;
+
+
+}
+
+
+module_init(WILC_WFI_InitModule);
+module_exit(WILC_WFI_Cleanup);
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_wfi_netdevice.h b/drivers/staging/wilc1000/wilc_wfi_netdevice.h
new file mode 100644
index 000000000000..d413fa3861c0
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wfi_netdevice.h
@@ -0,0 +1,254 @@
+/*!
+ *  @file	wilc_wfi_netdevice.h
+ *  @brief	Definitions for the network module
+ *  @author	mdaftedar
+ *  @date	01 MAR 2012
+ *  @version	1.0
+ */
+#ifndef WILC_WFI_NETDEVICE
+#define WILC_WFI_NETDEVICE
+
+/* These are the flags in the statusword */
+#define WILC_WFI_RX_INTR 0x0001
+#define WILC_WFI_TX_INTR 0x0002
+
+/* Default timeout period */
+#define WILC_WFI_TIMEOUT 5   /* In jiffies */
+#define WILC_MAX_NUM_PMKIDS  16
+#define PMKID_LEN  16
+#define PMKID_FOUND 1
+ #define NUM_STA_ASSOCIATED 8
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/moduleparam.h>
+#include <linux/sched.h>
+#include <linux/kernel.h>
+#include <linux/slab.h> /* kmalloc() */
+#include <linux/errno.h>  /* error codes */
+#include <linux/types.h>  /* size_t */
+#include <linux/interrupt.h> /* mark_bh */
+#include <linux/time.h>
+#include <linux/in.h>
+#include <linux/netdevice.h>   /* struct device, and other headers */
+#include <linux/etherdevice.h> /* eth_type_trans */
+#include <linux/ip.h>          /* struct iphdr */
+#include <linux/tcp.h>         /* struct tcphdr */
+#include <linux/skbuff.h>
+#include <linux/ieee80211.h>
+#include <net/cfg80211.h>
+#include <linux/ieee80211.h>
+#include <net/cfg80211.h>
+#include <net/ieee80211_radiotap.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <asm/checksum.h>
+#include "host_interface.h"
+#include "wilc_wlan.h"
+#include <linux/wireless.h>     /* tony, 2013-06-12 */
+
+#define FLOW_CONTROL_LOWER_THRESHOLD	128
+#define FLOW_CONTROL_UPPER_THRESHOLD	256
+
+/*iftype*/
+enum stats_flags {
+	WILC_WFI_RX_PKT = 1 << 0,
+	WILC_WFI_TX_PKT = 1 << 1,
+};
+
+struct WILC_WFI_stats {
+	unsigned long rx_packets;
+	unsigned long tx_packets;
+	unsigned long rx_bytes;
+	unsigned long tx_bytes;
+	u64 rx_time;
+	u64 tx_time;
+
+};
+
+/*
+ * This structure is private to each device. It is used to pass
+ * packets in and out, so there is place for a packet
+ */
+
+#define RX_BH_KTHREAD 0
+#define RX_BH_WORK_QUEUE 1
+#define RX_BH_THREADED_IRQ 2
+#define num_reg_frame 2
+/*
+ * If you use RX_BH_WORK_QUEUE on LPC3131: You may lose the first interrupt on
+ * LPC3131 which is important to get the MAC start status when you are blocked inside
+ * linux_wlan_firmware_download() which blocks mac_open().
+ */
+#if defined (NM73131_0_BOARD)
+ #define RX_BH_TYPE  RX_BH_KTHREAD
+#elif defined (PANDA_BOARD)
+ #define RX_BH_TYPE  RX_BH_THREADED_IRQ
+#else
+ #define RX_BH_TYPE  RX_BH_KTHREAD
+#endif
+
+struct wilc_wfi_key {
+	u8 *key;
+	u8 *seq;
+	int key_len;
+	int seq_len;
+	u32 cipher;
+};
+struct wilc_wfi_wep_key {
+	u8 *key;
+	u8 key_len;
+	u8 key_idx;
+};
+
+struct sta_info {
+	u8 au8Sta_AssociatedBss[MAX_NUM_STA][ETH_ALEN];
+};
+
+#ifdef WILC_P2P
+/*Parameters needed for host interface for  remaining on channel*/
+struct wilc_wfi_p2pListenParams {
+	struct ieee80211_channel *pstrListenChan;
+	enum nl80211_channel_type tenuChannelType;
+	u32 u32ListenDuration;
+	u64 u64ListenCookie;
+	u32 u32ListenSessionID;
+};
+
+#endif  /*WILC_P2P*/
+
+struct WILC_WFI_priv {
+	struct wireless_dev *wdev;
+	struct cfg80211_scan_request *pstrScanReq;
+
+	#ifdef WILC_P2P
+	struct wilc_wfi_p2pListenParams strRemainOnChanParams;
+	u64 u64tx_cookie;
+	#endif
+
+	bool bCfgScanning;
+	u32 u32RcvdChCount;
+
+	u8 au8AssociatedBss[ETH_ALEN];
+	struct sta_info assoc_stainfo;
+	struct net_device_stats stats;
+	u8 monitor_flag;
+	int status;
+	struct WILC_WFI_packet *ppool;
+	struct WILC_WFI_packet *rx_queue; /* List of incoming packets */
+	int rx_int_enabled;
+	int tx_packetlen;
+	u8 *tx_packetdata;
+	struct sk_buff *skb;
+	spinlock_t lock;
+	struct net_device *dev;
+	struct napi_struct napi;
+	WILC_WFIDrvHandle hWILCWFIDrv;
+	WILC_WFIDrvHandle hWILCWFIDrv_2;
+	tstrHostIFpmkidAttr pmkid_list;
+	struct WILC_WFI_stats netstats;
+	u8 WILC_WFI_wep_default;
+	u8 WILC_WFI_wep_key[4][WLAN_KEY_LEN_WEP104];
+	u8 WILC_WFI_wep_key_len[4];
+	struct net_device *real_ndev;   /* The real interface that the monitor is on */
+	struct wilc_wfi_key *wilc_gtk[MAX_NUM_STA];
+	struct wilc_wfi_key *wilc_ptk[MAX_NUM_STA];
+	u8 wilc_groupkey;
+	/* semaphores */
+	struct semaphore SemHandleUpdateStats;
+	struct semaphore hSemScanReq;
+	/*  */
+	bool gbAutoRateAdjusted;
+
+	bool bInP2PlistenState;
+
+};
+
+typedef struct {
+	u16 frame_type;
+	bool reg;
+
+} struct_frame_reg;
+
+#define NUM_CONCURRENT_IFC 2
+typedef struct {
+	uint8_t aSrcAddress[ETH_ALEN];
+	uint8_t aBSSID[ETH_ALEN];
+	uint32_t drvHandler;
+	struct net_device *wilc_netdev;
+} tstrInterfaceInfo;
+typedef struct {
+	int mac_status;
+	int wilc1000_initialized;
+	#if (!defined WILC_SDIO) || (defined WILC_SDIO_IRQ_GPIO)
+	unsigned short dev_irq_num;
+	#endif
+	wilc_wlan_oup_t oup;
+	int close;
+	uint8_t u8NoIfcs;
+	tstrInterfaceInfo strInterfaceInfo[NUM_CONCURRENT_IFC];
+	uint8_t open_ifcs;
+	struct mutex txq_cs;
+
+	/*Added by Amr - BugID_4720*/
+	struct mutex txq_add_to_head_cs;
+	spinlock_t txq_spinlock;
+
+	struct mutex rxq_cs;
+	struct mutex hif_cs;
+
+	/* struct mutex txq_event; */
+	struct semaphore rxq_event;
+	struct semaphore cfg_event;
+	struct semaphore sync_event;
+
+	struct semaphore txq_event;
+	/* struct completion txq_event; */
+
+#if (RX_BH_TYPE == RX_BH_WORK_QUEUE)
+	struct work_struct rx_work_queue;
+#elif (RX_BH_TYPE == RX_BH_KTHREAD)
+	struct task_struct *rx_bh_thread;
+	struct semaphore rx_sem;
+#endif
+	struct semaphore rxq_thread_started;
+	struct semaphore txq_thread_started;
+
+	struct task_struct *rxq_thread;
+	struct task_struct *txq_thread;
+
+	unsigned char eth_src_address[NUM_CONCURRENT_IFC][6];
+	/* unsigned char eth_dst_address[6]; */
+
+	const struct firmware *wilc_firmware; /* Bug 4703 */
+
+	struct net_device *real_ndev;
+#ifdef WILC_SDIO
+	int already_claim;
+	struct sdio_func *wilc_sdio_func;
+#else
+	struct spi_device *wilc_spidev;
+#endif
+
+} linux_wlan_t;
+
+typedef struct {
+	uint8_t u8IfIdx;
+	u8 iftype;
+	int monitor_flag;
+	int mac_opened;
+	#ifdef WILC_P2P
+	struct_frame_reg g_struct_frame_reg[num_reg_frame];
+	#endif
+	struct net_device *wilc_netdev;
+	struct net_device_stats netstats;
+
+} perInterface_wlan_t;
+
+struct WILC_WFI_mon_priv {
+	struct net_device *real_ndev;
+};
+
+extern struct net_device *WILC_WFI_devs[];
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_wlan.c b/drivers/staging/wilc1000/wilc_wlan.c
new file mode 100644
index 000000000000..3af91f770485
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wlan.c
@@ -0,0 +1,2332 @@
+/* ////////////////////////////////////////////////////////////////////////// */
+/*  */
+/* Copyright (c) Atmel Corporation.  All rights reserved. */
+/*  */
+/* Module Name:  wilc_wlan.c */
+/*  */
+/*  */
+/* //////////////////////////////////////////////////////////////////////////// */
+
+#include "wilc_wlan_if.h"
+#include "wilc_wlan.h"
+#define INLINE static __inline
+
+/********************************************
+ *
+ *      Global
+ *
+ ********************************************/
+extern unsigned int int_clrd;
+extern wilc_hif_func_t hif_sdio;
+extern wilc_hif_func_t hif_spi;
+extern wilc_cfg_func_t mac_cfg;
+#if defined(PLAT_RK3026_TCHIP)
+extern u8 g_wilc_initialized; /* AMR : 0422 RK3026 Crash issue */
+#endif
+extern void WILC_WFI_mgmt_rx(uint8_t *buff, uint32_t size);
+extern void frmw_to_linux(uint8_t *buff, uint32_t size);
+int sdio_xfer_cnt(void);
+uint32_t wilc_get_chipid(uint8_t update);
+u16 Set_machw_change_vir_if(bool bValue);
+
+
+
+typedef struct {
+	int quit;
+
+	/**
+	 *      input interface functions
+	 **/
+	wilc_wlan_os_func_t os_func;
+	wilc_wlan_io_func_t io_func;
+	wilc_wlan_net_func_t net_func;
+	wilc_wlan_indicate_func_t indicate_func;
+
+	/**
+	 *      host interface functions
+	 **/
+	wilc_hif_func_t hif_func;
+	void *hif_lock;
+
+	/**
+	 *      configuration interface functions
+	 **/
+	wilc_cfg_func_t cif_func;
+	int cfg_frame_in_use;
+	wilc_cfg_frame_t cfg_frame;
+	uint32_t cfg_frame_offset;
+	int cfg_seq_no;
+	void *cfg_wait;
+
+	/**
+	 *      RX buffer
+	 **/
+	#ifdef MEMORY_STATIC
+	uint32_t rx_buffer_size;
+	uint8_t *rx_buffer;
+	uint32_t rx_buffer_offset;
+	#endif
+	/**
+	 *      TX buffer
+	 **/
+	uint32_t tx_buffer_size;
+	uint8_t *tx_buffer;
+	uint32_t tx_buffer_offset;
+
+	/**
+	 *      TX queue
+	 **/
+	void *txq_lock;
+
+	/*Added by Amr - BugID_4720*/
+	void *txq_add_to_head_lock;
+	void *txq_spinlock;
+	unsigned long txq_spinlock_flags;
+
+	struct txq_entry_t *txq_head;
+	struct txq_entry_t *txq_tail;
+	int txq_entries;
+	void *txq_wait;
+	int txq_exit;
+
+	/**
+	 *      RX queue
+	 **/
+	void *rxq_lock;
+	struct rxq_entry_t *rxq_head;
+	struct rxq_entry_t *rxq_tail;
+	int rxq_entries;
+	void *rxq_wait;
+	int rxq_exit;
+
+
+} wilc_wlan_dev_t;
+
+static wilc_wlan_dev_t g_wlan;
+
+INLINE void chip_allow_sleep(void);
+INLINE void chip_wakeup(void);
+/********************************************
+ *
+ *      Debug
+ *
+ ********************************************/
+
+static uint32_t dbgflag = N_INIT | N_ERR | N_INTR | N_TXQ | N_RXQ;
+
+static void wilc_debug(uint32_t flag, char *fmt, ...)
+{
+	char buf[256];
+	va_list args;
+	int len;
+
+	if (flag & dbgflag) {
+		va_start(args, fmt);
+		len = vsprintf(buf, fmt, args);
+		va_end(args);
+
+		if (g_wlan.os_func.os_debug)
+			g_wlan.os_func.os_debug(buf);
+	}
+
+	return;
+}
+
+static CHIP_PS_STATE_T genuChipPSstate = CHIP_WAKEDUP;
+
+/*BugID_5213*/
+/*acquire_bus() and release_bus() are made INLINE functions*/
+/*as a temporary workaround to fix a problem of receiving*/
+/*unknown interrupt from FW*/
+INLINE void acquire_bus(BUS_ACQUIRE_T acquire)
+{
+
+	g_wlan.os_func.os_enter_cs(g_wlan.hif_lock);
+	#ifndef WILC_OPTIMIZE_SLEEP_INT
+	if (genuChipPSstate != CHIP_WAKEDUP)
+	#endif
+	{
+		if (acquire == ACQUIRE_AND_WAKEUP)
+			chip_wakeup();
+	}
+
+}
+INLINE void release_bus(BUS_RELEASE_T release)
+{
+	#ifdef WILC_OPTIMIZE_SLEEP_INT
+	if (release == RELEASE_ALLOW_SLEEP)
+		chip_allow_sleep();
+	#endif
+	g_wlan.os_func.os_leave_cs(g_wlan.hif_lock);
+}
+/********************************************
+ *
+ *      Queue
+ *
+ ********************************************/
+
+static void wilc_wlan_txq_remove(struct txq_entry_t *tqe)
+{
+
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	/* unsigned long flags; */
+	if (tqe == p->txq_head)	{
+
+		p->txq_head = tqe->next;
+		if (p->txq_head)
+			p->txq_head->prev = NULL;
+
+
+	} else if (tqe == p->txq_tail)	    {
+		p->txq_tail = (tqe->prev);
+		if (p->txq_tail)
+			p->txq_tail->next = NULL;
+	} else {
+		tqe->prev->next = tqe->next;
+		tqe->next->prev = tqe->prev;
+	}
+	p->txq_entries -= 1;
+
+}
+
+static struct txq_entry_t *wilc_wlan_txq_remove_from_head(void)
+{
+	struct txq_entry_t *tqe;
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	unsigned long flags;
+	p->os_func.os_spin_lock(p->txq_spinlock, &flags);
+	if (p->txq_head) {
+		tqe = p->txq_head;
+		p->txq_head = tqe->next;
+		if (p->txq_head) {
+			p->txq_head->prev = NULL;
+		}
+		p->txq_entries -= 1;
+
+		/*Added by Amr - BugID_4720*/
+
+
+
+	} else {
+		tqe = NULL;
+	}
+	p->os_func.os_spin_unlock(p->txq_spinlock, &flags);
+	return tqe;
+}
+
+static void wilc_wlan_txq_add_to_tail(struct txq_entry_t *tqe)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	unsigned long flags;
+	/*Added by Amr - BugID_4720*/
+	p->os_func.os_spin_lock(p->txq_spinlock, &flags);
+
+	if (p->txq_head == NULL) {
+		tqe->next = NULL;
+		tqe->prev = NULL;
+		p->txq_head = tqe;
+		p->txq_tail = tqe;
+	} else {
+		tqe->next = NULL;
+		tqe->prev = p->txq_tail;
+		p->txq_tail->next = tqe;
+		p->txq_tail = tqe;
+	}
+	p->txq_entries += 1;
+	PRINT_D(TX_DBG, "Number of entries in TxQ = %d\n", p->txq_entries);
+
+	/*Added by Amr - BugID_4720*/
+	p->os_func.os_spin_unlock(p->txq_spinlock, &flags);
+
+	/**
+	 *      wake up TX queue
+	 **/
+	PRINT_D(TX_DBG, "Wake the txq_handling\n");
+
+	p->os_func.os_signal(p->txq_wait);
+
+
+}
+
+static int wilc_wlan_txq_add_to_head(struct txq_entry_t *tqe)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	unsigned long flags;
+	/*Added by Amr - BugID_4720*/
+	if (p->os_func.os_wait(p->txq_add_to_head_lock, CFG_PKTS_TIMEOUT))
+		return -1;
+
+	p->os_func.os_spin_lock(p->txq_spinlock, &flags);
+
+	if (p->txq_head == NULL) {
+		tqe->next = NULL;
+		tqe->prev = NULL;
+		p->txq_head = tqe;
+		p->txq_tail = tqe;
+	} else {
+		tqe->next = p->txq_head;
+		tqe->prev = NULL;
+		p->txq_head->prev = tqe;
+		p->txq_head = tqe;
+	}
+	p->txq_entries += 1;
+	PRINT_D(TX_DBG, "Number of entries in TxQ = %d\n", p->txq_entries);
+
+	/*Added by Amr - BugID_4720*/
+	p->os_func.os_spin_unlock(p->txq_spinlock, &flags);
+	p->os_func.os_signal(p->txq_add_to_head_lock);
+
+
+	/**
+	 *      wake up TX queue
+	 **/
+	p->os_func.os_signal(p->txq_wait);
+	PRINT_D(TX_DBG, "Wake up the txq_handler\n");
+
+	/*Added by Amr - BugID_4720*/
+	return 0;
+
+}
+
+uint32_t Statisitcs_totalAcks = 0, Statisitcs_DroppedAcks = 0;
+
+#ifdef	TCP_ACK_FILTER
+struct Ack_session_info;
+typedef struct Ack_session_info {
+	uint32_t Ack_seq_num;
+	uint32_t Bigger_Ack_num;
+	uint16_t src_port;
+	uint16_t dst_port;
+	uint16_t status;
+} Ack_session_info_t;
+
+typedef struct {
+	uint32_t ack_num;
+	uint32_t Session_index;
+	struct txq_entry_t  *txqe;
+} Pending_Acks_info_t /*Ack_info_t*/;
+
+
+
+
+struct Ack_session_info *Free_head;
+struct Ack_session_info *Alloc_head;
+
+#define TCP_FIN_MASK		(1 << 0)
+#define TCP_SYN_MASK		(1 << 1)
+#define TCP_Ack_MASK		(1 << 4)
+#define NOT_TCP_ACK			(-1)
+
+#define MAX_TCP_SESSION		25
+#define MAX_PENDING_ACKS		256
+Ack_session_info_t Acks_keep_track_info[2 * MAX_TCP_SESSION];
+Pending_Acks_info_t Pending_Acks_info[MAX_PENDING_ACKS];
+
+uint32_t PendingAcks_arrBase;
+uint32_t Opened_TCP_session;
+uint32_t Pending_Acks;
+
+
+
+static __inline int Init_TCP_tracking(void)
+{
+
+	return 0;
+
+}
+static __inline int add_TCP_track_session(uint32_t src_prt, uint32_t dst_prt, uint32_t seq)
+{
+	Acks_keep_track_info[Opened_TCP_session].Ack_seq_num = seq;
+	Acks_keep_track_info[Opened_TCP_session].Bigger_Ack_num = 0;
+	Acks_keep_track_info[Opened_TCP_session].src_port = src_prt;
+	Acks_keep_track_info[Opened_TCP_session].dst_port = dst_prt;
+	Opened_TCP_session++;
+
+	PRINT_D(TCP_ENH, "TCP Session %d to Ack %d\n", Opened_TCP_session, seq);
+	return 0;
+}
+
+static __inline int Update_TCP_track_session(uint32_t index, uint32_t Ack)
+{
+
+	if (Ack > Acks_keep_track_info[index].Bigger_Ack_num) {
+		Acks_keep_track_info[index].Bigger_Ack_num = Ack;
+	}
+	return 0;
+
+}
+static __inline int add_TCP_Pending_Ack(uint32_t Ack, uint32_t Session_index, struct txq_entry_t  *txqe)
+{
+	Statisitcs_totalAcks++;
+	if (Pending_Acks < MAX_PENDING_ACKS) {
+		Pending_Acks_info[PendingAcks_arrBase + Pending_Acks].ack_num = Ack;
+		Pending_Acks_info[PendingAcks_arrBase + Pending_Acks].txqe = txqe;
+		Pending_Acks_info[PendingAcks_arrBase + Pending_Acks].Session_index = Session_index;
+		txqe->tcp_PendingAck_index = PendingAcks_arrBase + Pending_Acks;
+		Pending_Acks++;
+
+	} else {
+
+	}
+	return 0;
+}
+static __inline int remove_TCP_related(void)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	unsigned long flags;
+	p->os_func.os_spin_lock(p->txq_spinlock, &flags);
+
+	p->os_func.os_spin_unlock(p->txq_spinlock, &flags);
+	return 0;
+}
+
+static __inline int tcp_process(struct txq_entry_t *tqe)
+{
+	int ret;
+	uint8_t *eth_hdr_ptr;
+	uint8_t *buffer = tqe->buffer;
+	unsigned short h_proto;
+	int i;
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	unsigned long flags;
+	p->os_func.os_spin_lock(p->txq_spinlock, &flags);
+
+	eth_hdr_ptr = &buffer[0];
+	h_proto = ntohs(*((unsigned short *)&eth_hdr_ptr[12]));
+	if (h_proto == 0x0800) { /* IP */
+		uint8_t *ip_hdr_ptr;
+		uint8_t protocol;
+
+		ip_hdr_ptr = &buffer[ETHERNET_HDR_LEN];
+		protocol = ip_hdr_ptr[9];
+
+
+		if (protocol == 0x06) {
+			uint8_t *tcp_hdr_ptr;
+			uint32_t IHL, Total_Length, Data_offset;
+			tcp_hdr_ptr = &ip_hdr_ptr[IP_HDR_LEN];
+			IHL = (ip_hdr_ptr[0] & 0xf) << 2;
+			Total_Length = (((uint32_t)ip_hdr_ptr[2]) << 8) + ((uint32_t)ip_hdr_ptr[3]);
+			Data_offset = (((uint32_t)tcp_hdr_ptr[12] & 0xf0) >> 2);
+			if (Total_Length == (IHL + Data_offset)) { /*we want to recognize the clear Acks(packet only carry Ack infos not with data) so data size must be equal zero*/
+				uint32_t seq_no, Ack_no;
+				seq_no	= (((uint32_t)tcp_hdr_ptr[4]) << 24) + (((uint32_t)tcp_hdr_ptr[5]) << 16) + (((uint32_t)tcp_hdr_ptr[6]) << 8) + ((uint32_t)tcp_hdr_ptr[7]);
+
+				Ack_no	= (((uint32_t)tcp_hdr_ptr[8]) << 24) + (((uint32_t)tcp_hdr_ptr[9]) << 16) + (((uint32_t)tcp_hdr_ptr[10]) << 8) + ((uint32_t)tcp_hdr_ptr[11]);
+
+
+				for (i = 0; i < Opened_TCP_session; i++) {
+					if (Acks_keep_track_info[i].Ack_seq_num == seq_no) {
+						Update_TCP_track_session(i, Ack_no);
+						break;
+					}
+				}
+				if (i == Opened_TCP_session) {
+					add_TCP_track_session(0, 0, seq_no);
+				}
+				add_TCP_Pending_Ack(Ack_no, i, tqe);
+
+
+			}
+
+		} else {
+			ret = 0;
+		}
+	} else {
+		ret = 0;
+	}
+	p->os_func.os_spin_unlock(p->txq_spinlock, &flags);
+	return ret;
+}
+
+
+static int wilc_wlan_txq_filter_dup_tcp_ack(void)
+{
+
+	uint32_t i = 0;
+	uint32_t Dropped = 0;
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+
+	p->os_func.os_spin_lock(p->txq_spinlock, &p->txq_spinlock_flags);
+	for (i = PendingAcks_arrBase; i < (PendingAcks_arrBase + Pending_Acks); i++) {
+		if (Pending_Acks_info[i].ack_num < Acks_keep_track_info[Pending_Acks_info[i].Session_index].Bigger_Ack_num) {
+			struct txq_entry_t *tqe;
+			PRINT_D(TCP_ENH, "DROP ACK: %u \n", Pending_Acks_info[i].ack_num);
+			tqe = Pending_Acks_info[i].txqe;
+			if (tqe) {
+				wilc_wlan_txq_remove(tqe);
+				Statisitcs_DroppedAcks++;
+				tqe->status = 1;                                /* mark the packet send */
+				if (tqe->tx_complete_func)
+					tqe->tx_complete_func(tqe->priv, tqe->status);
+				p->os_func.os_free(tqe);
+				Dropped++;
+			}
+		}
+	}
+	Pending_Acks = 0;
+	Opened_TCP_session = 0;
+
+	if (PendingAcks_arrBase == 0) {
+		PendingAcks_arrBase = MAX_TCP_SESSION;
+	} else {
+		PendingAcks_arrBase = 0;
+	}
+
+
+	p->os_func.os_spin_unlock(p->txq_spinlock, &p->txq_spinlock_flags);
+
+	while (Dropped > 0) {
+		/*consume the semaphore count of the removed packet*/
+		p->os_func.os_wait(p->txq_wait, 1);
+		Dropped--;
+	}
+
+	return 1;
+}
+#endif
+
+#ifdef TCP_ENHANCEMENTS
+bool EnableTCPAckFilter = false;
+
+void Enable_TCP_ACK_Filter(bool value)
+{
+	EnableTCPAckFilter = value;
+}
+
+bool is_TCP_ACK_Filter_Enabled(void)
+{
+	return EnableTCPAckFilter;
+}
+#endif
+
+static int wilc_wlan_txq_add_cfg_pkt(uint8_t *buffer, uint32_t buffer_size)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	struct txq_entry_t *tqe;
+
+	PRINT_D(TX_DBG, "Adding config packet ...\n");
+	if (p->quit) {
+		PRINT_D(TX_DBG, "Return due to clear function\n");
+		p->os_func.os_signal(p->cfg_wait);
+		return 0;
+	}
+
+	tqe = (struct txq_entry_t *)p->os_func.os_malloc_atomic(sizeof(struct txq_entry_t));
+	if (tqe == NULL) {
+		PRINT_ER("Failed to allocate memory\n");
+		return 0;
+	}
+
+	tqe->type = WILC_CFG_PKT;
+	tqe->buffer = buffer;
+	tqe->buffer_size = buffer_size;
+	tqe->tx_complete_func = NULL;
+	tqe->priv = NULL;
+#ifdef TCP_ACK_FILTER
+	tqe->tcp_PendingAck_index = NOT_TCP_ACK;
+#endif
+	/**
+	 *      Configuration packet always at the front
+	 **/
+	PRINT_D(TX_DBG, "Adding the config packet at the Queue tail\n");
+
+	/*Edited by Amr - BugID_4720*/
+	if (wilc_wlan_txq_add_to_head(tqe))
+		return 0;
+	return 1;
+}
+
+static int wilc_wlan_txq_add_net_pkt(void *priv, uint8_t *buffer, uint32_t buffer_size, wilc_tx_complete_func_t func)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	struct txq_entry_t *tqe;
+
+	if (p->quit)
+		return 0;
+
+	tqe = (struct txq_entry_t *)p->os_func.os_malloc_atomic(sizeof(struct txq_entry_t));
+
+	if (tqe == NULL)
+		return 0;
+	tqe->type = WILC_NET_PKT;
+	tqe->buffer = buffer;
+	tqe->buffer_size = buffer_size;
+	tqe->tx_complete_func = func;
+	tqe->priv = priv;
+
+	PRINT_D(TX_DBG, "Adding mgmt packet at the Queue tail\n");
+#ifdef TCP_ACK_FILTER
+	tqe->tcp_PendingAck_index = NOT_TCP_ACK;
+#ifdef TCP_ENHANCEMENTS
+	if (is_TCP_ACK_Filter_Enabled())
+#endif
+	tcp_process(tqe);
+#endif
+	wilc_wlan_txq_add_to_tail(tqe);
+	/*return number of itemes in the queue*/
+	return p->txq_entries;
+}
+/*Bug3959: transmitting mgmt frames received from host*/
+#if defined(WILC_AP_EXTERNAL_MLME) || defined(WILC_P2P)
+int wilc_wlan_txq_add_mgmt_pkt(void *priv, uint8_t *buffer, uint32_t buffer_size, wilc_tx_complete_func_t func)
+{
+
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	struct txq_entry_t *tqe;
+
+	if (p->quit)
+		return 0;
+
+	tqe = (struct txq_entry_t *)p->os_func.os_malloc_atomic(sizeof(struct txq_entry_t));
+
+	if (tqe == NULL)
+		return 0;
+	tqe->type = WILC_MGMT_PKT;
+	tqe->buffer = buffer;
+	tqe->buffer_size = buffer_size;
+	tqe->tx_complete_func = func;
+	tqe->priv = priv;
+#ifdef TCP_ACK_FILTER
+	tqe->tcp_PendingAck_index = NOT_TCP_ACK;
+#endif
+	PRINT_D(TX_DBG, "Adding Network packet at the Queue tail\n");
+	wilc_wlan_txq_add_to_tail(tqe);
+	return 1;
+}
+
+#ifdef WILC_FULLY_HOSTING_AP
+int wilc_FH_wlan_txq_add_net_pkt(void *priv, uint8_t *buffer, uint32_t buffer_size, wilc_tx_complete_func_t func)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	struct txq_entry_t *tqe;
+
+	if (p->quit)
+		return 0;
+
+	tqe = (struct txq_entry_t *)p->os_func.os_malloc_atomic(sizeof(struct txq_entry_t));
+
+	if (tqe == NULL)
+		return 0;
+	tqe->type = WILC_FH_DATA_PKT;
+	tqe->buffer = buffer;
+	tqe->buffer_size = buffer_size;
+	tqe->tx_complete_func = func;
+	tqe->priv = priv;
+	PRINT_D(TX_DBG, "Adding mgmt packet at the Queue tail\n");
+	wilc_wlan_txq_add_to_tail(tqe);
+	/*return number of itemes in the queue*/
+	return p->txq_entries;
+}
+#endif  /* WILC_FULLY_HOSTING_AP*/
+#endif /*WILC_AP_EXTERNAL_MLME*/
+static struct txq_entry_t *wilc_wlan_txq_get_first(void)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	struct txq_entry_t *tqe;
+	unsigned long flags;
+
+	/*Added by Amr - BugID_4720*/
+	p->os_func.os_spin_lock(p->txq_spinlock, &flags);
+
+	tqe = p->txq_head;
+
+	/*Added by Amr - BugID_4720*/
+	p->os_func.os_spin_unlock(p->txq_spinlock, &flags);
+
+
+	return tqe;
+}
+
+static struct txq_entry_t *wilc_wlan_txq_get_next(struct txq_entry_t *tqe)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	unsigned long flags;
+	/*Added by Amr - BugID_4720*/
+	p->os_func.os_spin_lock(p->txq_spinlock, &flags);
+
+	tqe = tqe->next;
+	/*Added by Amr - BugID_4720*/
+	p->os_func.os_spin_unlock(p->txq_spinlock, &flags);
+
+
+	return tqe;
+}
+
+static int wilc_wlan_rxq_add(struct rxq_entry_t *rqe)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+
+	if (p->quit)
+		return 0;
+
+	p->os_func.os_enter_cs(p->rxq_lock);
+	if (p->rxq_head == NULL) {
+		PRINT_D(RX_DBG, "Add to Queue head\n");
+		rqe->next = NULL;
+		p->rxq_head = rqe;
+		p->rxq_tail = rqe;
+	} else {
+		PRINT_D(RX_DBG, "Add to Queue tail\n");
+		p->rxq_tail->next = rqe;
+		rqe->next = NULL;
+		p->rxq_tail = rqe;
+	}
+	p->rxq_entries += 1;
+	PRINT_D(RX_DBG, "Number of queue entries: %d\n", p->rxq_entries);
+	p->os_func.os_leave_cs(p->rxq_lock);
+	return p->rxq_entries;
+}
+
+static struct rxq_entry_t *wilc_wlan_rxq_remove(void)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+
+	PRINT_D(RX_DBG, "Getting rxQ element\n");
+	if (p->rxq_head) {
+		struct rxq_entry_t *rqe;
+
+		p->os_func.os_enter_cs(p->rxq_lock);
+		rqe = p->rxq_head;
+		p->rxq_head = p->rxq_head->next;
+		p->rxq_entries -= 1;
+		PRINT_D(RX_DBG, "RXQ entries decreased\n");
+		p->os_func.os_leave_cs(p->rxq_lock);
+		return rqe;
+	}
+	PRINT_D(RX_DBG, "Nothing to get from Q\n");
+	return NULL;
+}
+
+
+/********************************************
+ *
+ *      Power Save handle functions
+ *
+ ********************************************/
+
+
+
+#ifdef WILC_OPTIMIZE_SLEEP_INT
+
+INLINE void chip_allow_sleep(void)
+{
+	uint32_t reg = 0;
+
+	/* Clear bit 1 */
+	g_wlan.hif_func.hif_read_reg(0xf0, &reg);
+
+	g_wlan.hif_func.hif_write_reg(0xf0, reg & ~(1 << 0));
+}
+
+INLINE void chip_wakeup(void)
+{
+	uint32_t reg, clk_status_reg, trials = 0;
+	uint32_t sleep_time;
+
+	if ((g_wlan.io_func.io_type & 0x1) == HIF_SPI) {
+		do {
+			g_wlan.hif_func.hif_read_reg(1, &reg);
+			/* Set bit 1 */
+			g_wlan.hif_func.hif_write_reg(1, reg | (1 << 1));
+
+			/* Clear bit 1*/
+			g_wlan.hif_func.hif_write_reg(1, reg & ~(1 << 1));
+
+			do {
+				/* Wait for the chip to stabilize*/
+				WILC_Sleep(2);
+				/* Make sure chip is awake. This is an extra step that can be removed */
+				/* later to avoid the bus access overhead */
+				if ((wilc_get_chipid(true) == 0)) {
+					wilc_debug(N_ERR, "Couldn't read chip id. Wake up failed\n");
+				}
+			} while ((wilc_get_chipid(true) == 0) && ((++trials % 3) == 0));
+
+		} while (wilc_get_chipid(true) == 0);
+	} else if ((g_wlan.io_func.io_type & 0x1) == HIF_SDIO)	 {
+		g_wlan.hif_func.hif_read_reg(0xf0, &reg);
+		do {
+			/* Set bit 1 */
+			g_wlan.hif_func.hif_write_reg(0xf0, reg | (1 << 0));
+
+			/* Check the clock status */
+			g_wlan.hif_func.hif_read_reg(0xf1, &clk_status_reg);
+
+			/* in case of clocks off, wait 2ms, and check it again. */
+			/* if still off, wait for another 2ms, for a total wait of 6ms. */
+			/* If still off, redo the wake up sequence */
+			while (((clk_status_reg & 0x1) == 0) && (((++trials) % 3) == 0)) {
+				/* Wait for the chip to stabilize*/
+				WILC_Sleep(2);
+
+				/* Make sure chip is awake. This is an extra step that can be removed */
+				/* later to avoid the bus access overhead */
+				g_wlan.hif_func.hif_read_reg(0xf1, &clk_status_reg);
+
+				if ((clk_status_reg & 0x1) == 0) {
+					wilc_debug(N_ERR, "clocks still OFF. Wake up failed\n");
+				}
+			}
+			/* in case of failure, Reset the wakeup bit to introduce a new edge on the next loop */
+			if ((clk_status_reg & 0x1) == 0) {
+				/* Reset bit 0 */
+				g_wlan.hif_func.hif_write_reg(0xf0, reg & (~(1 << 0)));
+			}
+		} while ((clk_status_reg & 0x1) == 0);
+	}
+
+
+	if (genuChipPSstate == CHIP_SLEEPING_MANUAL) {
+		g_wlan.hif_func.hif_read_reg(0x1C0C, &reg);
+		reg &= ~(1 << 0);
+		g_wlan.hif_func.hif_write_reg(0x1C0C, reg);
+
+		if (wilc_get_chipid(false) >= 0x1002b0) {
+			/* Enable PALDO back right after wakeup */
+			uint32_t val32;
+			g_wlan.hif_func.hif_read_reg(0x1e1c, &val32);
+			val32 |= (1 << 6);
+			g_wlan.hif_func.hif_write_reg(0x1e1c, val32);
+
+			g_wlan.hif_func.hif_read_reg(0x1e9c, &val32);
+			val32 |= (1 << 6);
+			g_wlan.hif_func.hif_write_reg(0x1e9c, val32);
+		}
+	}
+	genuChipPSstate = CHIP_WAKEDUP;
+}
+#else
+INLINE void chip_wakeup(void)
+{
+	uint32_t reg, trials = 0;
+	do {
+		if ((g_wlan.io_func.io_type & 0x1) == HIF_SPI) {
+			g_wlan.hif_func.hif_read_reg(1, &reg);
+			/* Make sure bit 1 is 0 before we start. */
+			g_wlan.hif_func.hif_write_reg(1, reg & ~(1 << 1));
+			/* Set bit 1 */
+			g_wlan.hif_func.hif_write_reg(1, reg | (1 << 1));
+			/* Clear bit 1*/
+			g_wlan.hif_func.hif_write_reg(1, reg  & ~(1 << 1));
+		} else if ((g_wlan.io_func.io_type & 0x1) == HIF_SDIO)	 {
+			/* Make sure bit 0 is 0 before we start. */
+			g_wlan.hif_func.hif_read_reg(0xf0, &reg);
+			g_wlan.hif_func.hif_write_reg(0xf0, reg & ~(1 << 0));
+			/* Set bit 1 */
+			g_wlan.hif_func.hif_write_reg(0xf0, reg | (1 << 0));
+			/* Clear bit 1 */
+			g_wlan.hif_func.hif_write_reg(0xf0, reg  & ~(1 << 0));
+		}
+
+		do {
+			/* Wait for the chip to stabilize*/
+			mdelay(3);
+
+			/* Make sure chip is awake. This is an extra step that can be removed */
+			/* later to avoid the bus access overhead */
+			if ((wilc_get_chipid(true) == 0)) {
+				wilc_debug(N_ERR, "Couldn't read chip id. Wake up failed\n");
+			}
+		} while ((wilc_get_chipid(true) == 0) && ((++trials % 3) == 0));
+
+	} while (wilc_get_chipid(true) == 0);
+
+	if (genuChipPSstate == CHIP_SLEEPING_MANUAL) {
+		g_wlan.hif_func.hif_read_reg(0x1C0C, &reg);
+		reg &= ~(1 << 0);
+		g_wlan.hif_func.hif_write_reg(0x1C0C, reg);
+
+		if (wilc_get_chipid(false) >= 0x1002b0) {
+			/* Enable PALDO back right after wakeup */
+			uint32_t val32;
+			g_wlan.hif_func.hif_read_reg(0x1e1c, &val32);
+			val32 |= (1 << 6);
+			g_wlan.hif_func.hif_write_reg(0x1e1c, val32);
+
+			g_wlan.hif_func.hif_read_reg(0x1e9c, &val32);
+			val32 |= (1 << 6);
+			g_wlan.hif_func.hif_write_reg(0x1e9c, val32);
+		}
+	}
+	genuChipPSstate = CHIP_WAKEDUP;
+}
+#endif
+void chip_sleep_manually(u32 u32SleepTime)
+{
+	if (genuChipPSstate != CHIP_WAKEDUP) {
+		/* chip is already sleeping. Do nothing */
+		return;
+	}
+	acquire_bus(ACQUIRE_ONLY);
+
+#ifdef WILC_OPTIMIZE_SLEEP_INT
+	chip_allow_sleep();
+#endif
+
+	/* Trigger the manual sleep interrupt */
+	g_wlan.hif_func.hif_write_reg(0x10a8, 1);
+
+	genuChipPSstate = CHIP_SLEEPING_MANUAL;
+	release_bus(RELEASE_ONLY);
+
+}
+
+
+/********************************************
+ *
+ *      Tx, Rx queue handle functions
+ *
+ ********************************************/
+static int wilc_wlan_handle_txq(uint32_t *pu32TxqCount)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	int i, entries = 0;
+	uint32_t sum;
+	uint32_t reg;
+	uint8_t *txb = p->tx_buffer;
+	uint32_t offset = 0;
+	int vmm_sz = 0;
+	struct txq_entry_t *tqe;
+	int ret = 0;
+	int counter;
+	int timeout;
+	uint32_t vmm_table[WILC_VMM_TBL_SIZE];
+	p->txq_exit = 0;
+	do {
+		if (p->quit)
+			break;
+
+		/*Added by Amr - BugID_4720*/
+		p->os_func.os_wait(p->txq_add_to_head_lock, CFG_PKTS_TIMEOUT);
+#ifdef	TCP_ACK_FILTER
+		wilc_wlan_txq_filter_dup_tcp_ack();
+#endif
+		/**
+		 *      build the vmm list
+		 **/
+		PRINT_D(TX_DBG, "Getting the head of the TxQ\n");
+		tqe = wilc_wlan_txq_get_first();
+		i = 0;
+		sum = 0;
+		do {
+			/* if ((tqe != NULL) && (i < (8)) && */
+			/* if ((tqe != NULL) && (i < (WILC_VMM_TBL_SIZE-1)) && */
+			if ((tqe != NULL) && (i < (WILC_VMM_TBL_SIZE - 1)) /* reserve last entry to 0 */) {
+
+				if (tqe->type == WILC_CFG_PKT) {
+					vmm_sz = ETH_CONFIG_PKT_HDR_OFFSET;
+				}
+				/*Bug3959: transmitting mgmt frames received from host*/
+				/*vmm_sz will only be equal to tqe->buffer_size + 4 bytes (HOST_HDR_OFFSET)*/
+				/* in other cases WILC_MGMT_PKT and WILC_DATA_PKT_MAC_HDR*/
+				else if (tqe->type == WILC_NET_PKT) {
+					vmm_sz = ETH_ETHERNET_HDR_OFFSET;
+				}
+#ifdef WILC_FULLY_HOSTING_AP
+				else if (tqe->type == WILC_FH_DATA_PKT)	{
+					vmm_sz = FH_TX_HOST_HDR_OFFSET;
+				}
+#endif
+#ifdef WILC_AP_EXTERNAL_MLME
+				else {
+					vmm_sz = HOST_HDR_OFFSET;
+				}
+#endif
+				vmm_sz += tqe->buffer_size;
+				PRINT_D(TX_DBG, "VMM Size before alignment = %d\n", vmm_sz);
+				if (vmm_sz & 0x3) {                                                                                                     /* has to be word aligned */
+					vmm_sz = (vmm_sz + 4) & ~0x3;
+				}
+				if ((sum + vmm_sz) > p->tx_buffer_size) {
+					break;
+				}
+				PRINT_D(TX_DBG, "VMM Size AFTER alignment = %d\n", vmm_sz);
+				vmm_table[i] = vmm_sz / 4;                                                                                /* table take the word size */
+				PRINT_D(TX_DBG, "VMMTable entry size = %d\n", vmm_table[i]);
+
+				if (tqe->type == WILC_CFG_PKT) {
+					vmm_table[i] |= (1 << 10);
+					PRINT_D(TX_DBG, "VMMTable entry changed for CFG packet = %d\n", vmm_table[i]);
+				}
+#ifdef BIG_ENDIAN
+				vmm_table[i] = BYTE_SWAP(vmm_table[i]);
+#endif
+
+				i++;
+				sum += vmm_sz;
+				PRINT_D(TX_DBG, "sum = %d\n", sum);
+				tqe = wilc_wlan_txq_get_next(tqe);
+			} else {
+				break;
+			}
+		} while (1);
+
+		if (i == 0) {           /* nothing in the queue */
+			PRINT_D(TX_DBG, "Nothing in TX-Q\n");
+			break;
+		} else {
+			PRINT_D(TX_DBG, "Mark the last entry in VMM table - number of previous entries = %d\n", i);
+			vmm_table[i] = 0x0;     /* mark the last element to 0 */
+		}
+		acquire_bus(ACQUIRE_AND_WAKEUP);
+		counter = 0;
+		do {
+
+			ret = p->hif_func.hif_read_reg(WILC_HOST_TX_CTRL, &reg);
+			if (!ret) {
+				wilc_debug(N_ERR, "[wilc txq]: fail can't read reg vmm_tbl_entry..\n");
+				break;
+			}
+
+			if ((reg & 0x1) == 0) {
+				/**
+				 *      write to vmm table
+				 **/
+				PRINT_D(TX_DBG, "Writing VMM table ... with Size = %d\n", ((i + 1) * 4));
+				break;
+			} else {
+				counter++;
+				if (counter > 200) {
+					counter = 0;
+					PRINT_D(TX_DBG, "Looping in tx ctrl , forcce quit\n");
+					ret = p->hif_func.hif_write_reg(WILC_HOST_TX_CTRL, 0);
+					break;
+				}
+				/**
+				 *      wait for vmm table is ready
+				 **/
+				PRINT_WRN(GENERIC_DBG, "[wilc txq]: warn, vmm table not clear yet, wait... \n");
+				release_bus(RELEASE_ALLOW_SLEEP);
+				p->os_func.os_sleep(3); /* wait 3 ms */
+				acquire_bus(ACQUIRE_AND_WAKEUP);
+			}
+		} while (!p->quit);
+
+		if (!ret) {
+			goto _end_;
+		}
+
+		timeout = 200;
+		do {
+
+			/**
+			 * write to vmm table
+			 **/
+			ret = p->hif_func.hif_block_tx(WILC_VMM_TBL_RX_SHADOW_BASE, (uint8_t *)vmm_table, ((i + 1) * 4)); /* Bug 4477 fix */
+			if (!ret) {
+				wilc_debug(N_ERR, "ERR block TX of VMM table.\n");
+				break;
+			}
+
+
+			/**
+			 * interrupt firmware
+			 **/
+			ret = p->hif_func.hif_write_reg(WILC_HOST_VMM_CTL, 0x2);
+			if (!ret) {
+				wilc_debug(N_ERR, "[wilc txq]: fail can't write reg host_vmm_ctl..\n");
+				break;
+			}
+
+			/**
+			 *      wait for confirm...
+			 **/
+
+			do {
+				ret = p->hif_func.hif_read_reg(WILC_HOST_VMM_CTL, &reg);
+				if (!ret) {
+					wilc_debug(N_ERR, "[wilc txq]: fail can't read reg host_vmm_ctl..\n");
+					break;
+				}
+				if ((reg >> 2) & 0x1) {
+					/**
+					 *      Get the entries
+					 **/
+					entries = ((reg >> 3) & 0x3f);
+					/* entries = ((reg>>3)&0x2f); */
+					break;
+				} else {
+					release_bus(RELEASE_ALLOW_SLEEP);
+					p->os_func.os_sleep(3); /* wait 3 ms */
+					acquire_bus(ACQUIRE_AND_WAKEUP);
+					PRINT_WRN(GENERIC_DBG, "Can't get VMM entery - reg = %2x\n", reg);
+				}
+			} while (--timeout);
+			if (timeout <= 0) {
+				ret = p->hif_func.hif_write_reg(WILC_HOST_VMM_CTL, 0x0);
+				break;
+			}
+
+			if (!ret) {
+				break;
+			}
+
+			if (entries == 0) {
+				PRINT_WRN(GENERIC_DBG, "[wilc txq]: no more buffer in the chip (reg: %08x), retry later [[ %d, %x ]] \n", reg, i, vmm_table[i - 1]);
+
+				/* undo the transaction. */
+				ret = p->hif_func.hif_read_reg(WILC_HOST_TX_CTRL, &reg);
+				if (!ret) {
+					wilc_debug(N_ERR, "[wilc txq]: fail can't read reg WILC_HOST_TX_CTRL..\n");
+					break;
+				}
+				reg &= ~(1ul << 0);
+				ret = p->hif_func.hif_write_reg(WILC_HOST_TX_CTRL, reg);
+				if (!ret) {
+					wilc_debug(N_ERR, "[wilc txq]: fail can't write reg WILC_HOST_TX_CTRL..\n");
+					break;
+				}
+				break;
+			} else {
+				break;
+			}
+		} while (1);
+
+		if (!ret) {
+			goto _end_;
+		}
+		if (entries == 0) {
+			ret = WILC_TX_ERR_NO_BUF;
+			goto _end_;
+		}
+
+		/* since copying data into txb takes some time, then
+		 * allow the bus lock to be released let the RX task go. */
+		release_bus(RELEASE_ALLOW_SLEEP);
+
+		/**
+		 *      Copy data to the TX buffer
+		 **/
+		offset = 0;
+		i = 0;
+		do {
+			tqe = wilc_wlan_txq_remove_from_head();
+			if (tqe != NULL && (vmm_table[i] != 0)) {
+				uint32_t header, buffer_offset;
+
+#ifdef BIG_ENDIAN
+				vmm_table[i] = BYTE_SWAP(vmm_table[i]);
+#endif
+				vmm_sz = (vmm_table[i] & 0x3ff);        /* in word unit */
+				vmm_sz *= 4;
+				header = (tqe->type << 31) | (tqe->buffer_size << 15) | vmm_sz;
+				/*Bug3959: transmitting mgmt frames received from host*/
+				/*setting bit 30 in the host header to indicate mgmt frame*/
+#ifdef WILC_AP_EXTERNAL_MLME
+				if (tqe->type == WILC_MGMT_PKT)	{
+					header |= (1 << 30);
+				} else {
+					header &= ~(1 << 30);
+				}
+#endif
+
+#ifdef BIG_ENDIAN
+				header = BYTE_SWAP(header);
+#endif
+				memcpy(&txb[offset], &header, 4);
+				if (tqe->type == WILC_CFG_PKT) {
+					buffer_offset = ETH_CONFIG_PKT_HDR_OFFSET;
+				}
+				/*Bug3959: transmitting mgmt frames received from host*/
+				/*buffer offset = HOST_HDR_OFFSET in other cases: WILC_MGMT_PKT*/
+				/* and WILC_DATA_PKT_MAC_HDR*/
+				else if (tqe->type == WILC_NET_PKT) {
+					char *pBSSID = ((struct tx_complete_data *)(tqe->priv))->pBssid;
+					buffer_offset = ETH_ETHERNET_HDR_OFFSET;
+					/* copy the bssid at the sart of the buffer */
+					memcpy(&txb[offset + 4], pBSSID, 6);
+				}
+#ifdef WILC_FULLY_HOSTING_AP
+				else if (tqe->type == WILC_FH_DATA_PKT)	{
+					buffer_offset = FH_TX_HOST_HDR_OFFSET;
+				}
+#endif
+				else {
+					buffer_offset = HOST_HDR_OFFSET;
+				}
+
+				memcpy(&txb[offset + buffer_offset], tqe->buffer, tqe->buffer_size);
+				offset += vmm_sz;
+				i++;
+				tqe->status = 1;                                /* mark the packet send */
+				if (tqe->tx_complete_func)
+					tqe->tx_complete_func(tqe->priv, tqe->status);
+				#ifdef TCP_ACK_FILTER
+				if (tqe->tcp_PendingAck_index != NOT_TCP_ACK) {
+					Pending_Acks_info[tqe->tcp_PendingAck_index].txqe = NULL;
+				}
+				#endif
+				p->os_func.os_free(tqe);
+			} else {
+				break;
+			}
+		} while (--entries);
+
+		/**
+		 *      lock the bus
+		 **/
+		acquire_bus(ACQUIRE_AND_WAKEUP);
+
+		ret = p->hif_func.hif_clear_int_ext(ENABLE_TX_VMM);
+		if (!ret) {
+			wilc_debug(N_ERR, "[wilc txq]: fail can't start tx VMM ...\n");
+			goto _end_;
+		}
+
+		/**
+		 *      transfer
+		 **/
+		ret = p->hif_func.hif_block_tx_ext(0, txb, offset);
+		if (!ret) {
+			wilc_debug(N_ERR, "[wilc txq]: fail can't block tx ext...\n");
+			goto _end_;
+		}
+
+_end_:
+
+		release_bus(RELEASE_ALLOW_SLEEP);
+		if (ret != 1)
+			break;
+	} while (0);
+	/*Added by Amr - BugID_4720*/
+	p->os_func.os_signal(p->txq_add_to_head_lock);
+
+	p->txq_exit = 1;
+	PRINT_D(TX_DBG, "THREAD: Exiting txq\n");
+	/* return tx[]q count */
+	*pu32TxqCount = p->txq_entries;
+	return ret;
+}
+
+static void wilc_wlan_handle_rxq(void)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	int offset = 0, size, has_packet = 0;
+	uint8_t *buffer;
+	struct rxq_entry_t *rqe;
+
+	p->rxq_exit = 0;
+
+
+
+
+	do {
+		if (p->quit) {
+			PRINT_D(RX_DBG, "exit 1st do-while due to Clean_UP function \n");
+			p->os_func.os_signal(p->cfg_wait);
+			break;
+		}
+		rqe = wilc_wlan_rxq_remove();
+		if (rqe == NULL) {
+			PRINT_D(RX_DBG, "nothing in the queue - exit 1st do-while\n");
+			break;
+		}
+		buffer = rqe->buffer;
+		size = rqe->buffer_size;
+		PRINT_D(RX_DBG, "rxQ entery Size = %d - Address = %p\n", size, buffer);
+		offset = 0;
+
+
+
+		do {
+			uint32_t header;
+			uint32_t pkt_len, pkt_offset, tp_len;
+			int is_cfg_packet;
+			PRINT_D(RX_DBG, "In the 2nd do-while\n");
+			memcpy(&header, &buffer[offset], 4);
+#ifdef BIG_ENDIAN
+			header = BYTE_SWAP(header);
+#endif
+			PRINT_D(RX_DBG, "Header = %04x - Offset = %d\n", header, offset);
+
+
+
+			is_cfg_packet = (header >> 31) & 0x1;
+			pkt_offset = (header >> 22) & 0x1ff;
+			tp_len = (header >> 11) & 0x7ff;
+			pkt_len = header & 0x7ff;
+
+			if (pkt_len == 0 || tp_len == 0) {
+				wilc_debug(N_RXQ, "[wilc rxq]: data corrupt, packet len or tp_len is 0 [%d][%d]\n", pkt_len, tp_len);
+				break;
+			}
+
+/*bug 3887: [AP] Allow Management frames to be passed to the host*/
+			#if defined(WILC_AP_EXTERNAL_MLME) || defined(WILC_P2P)
+			#define IS_MANAGMEMENT				0x100
+			#define IS_MANAGMEMENT_CALLBACK			0x080
+			#define IS_MGMT_STATUS_SUCCES			0x040
+
+
+			if (pkt_offset & IS_MANAGMEMENT) {
+				/* reset mgmt indicator bit, to use pkt_offeset in furthur calculations */
+				pkt_offset &= ~(IS_MANAGMEMENT | IS_MANAGMEMENT_CALLBACK | IS_MGMT_STATUS_SUCCES);
+
+#ifdef USE_WIRELESS
+				WILC_WFI_mgmt_rx(&buffer[offset + HOST_HDR_OFFSET], pkt_len);
+
+#endif
+
+			}
+			/* BUG4530 fix */
+			else
+			#endif
+			{
+
+				if (!is_cfg_packet) {
+
+					if (p->net_func.rx_indicate) {
+						if (pkt_len > 0) {
+							p->net_func.rx_indicate(&buffer[offset], pkt_len, pkt_offset);
+							has_packet = 1;
+						}
+					}
+				} else {
+					wilc_cfg_rsp_t rsp;
+
+
+
+					p->cif_func.rx_indicate(&buffer[pkt_offset + offset], pkt_len, &rsp);
+					if (rsp.type == WILC_CFG_RSP) {
+						/**
+						 *      wake up the waiting task...
+						 **/
+						PRINT_D(RX_DBG, "p->cfg_seq_no = %d - rsp.seq_no = %d\n", p->cfg_seq_no, rsp.seq_no);
+						if (p->cfg_seq_no == rsp.seq_no) {
+							p->os_func.os_signal(p->cfg_wait);
+						}
+					} else if (rsp.type == WILC_CFG_RSP_STATUS) {
+						/**
+						 *      Call back to indicate status...
+						 **/
+						if (p->indicate_func.mac_indicate) {
+							p->indicate_func.mac_indicate(WILC_MAC_INDICATE_STATUS);
+						}
+
+					} else if (rsp.type == WILC_CFG_RSP_SCAN) {
+						if (p->indicate_func.mac_indicate)
+							p->indicate_func.mac_indicate(WILC_MAC_INDICATE_SCAN);
+					}
+				}
+			}
+			offset += tp_len;
+			if (offset >= size)
+				break;
+		} while (1);
+
+
+#ifndef MEMORY_STATIC
+		if (buffer != NULL)
+			p->os_func.os_free((void *)buffer);
+#endif
+		if (rqe != NULL)
+			p->os_func.os_free((void *)rqe);
+
+		if (has_packet) {
+			if (p->net_func.rx_complete)
+				p->net_func.rx_complete();
+		}
+	} while (1);
+
+	p->rxq_exit = 1;
+	PRINT_D(RX_DBG, "THREAD: Exiting RX thread \n");
+	return;
+}
+
+/********************************************
+ *
+ *      Fast DMA Isr
+ *
+ ********************************************/
+static void wilc_unknown_isr_ext(void)
+{
+	g_wlan.hif_func.hif_clear_int_ext(0);
+}
+static void wilc_pllupdate_isr_ext(uint32_t int_stats)
+{
+
+	int trials = 10;
+
+	g_wlan.hif_func.hif_clear_int_ext(PLL_INT_CLR);
+
+	/* Waiting for PLL */
+	g_wlan.os_func.os_atomic_sleep(WILC_PLL_TO);
+
+	/* poll till read a valid data */
+	while (!(ISWILC1000(wilc_get_chipid(true)) && --trials)) {
+		PRINT_D(TX_DBG, "PLL update retrying\n");
+		g_wlan.os_func.os_atomic_sleep(1);
+	}
+}
+
+static void wilc_sleeptimer_isr_ext(uint32_t int_stats1)
+{
+	g_wlan.hif_func.hif_clear_int_ext(SLEEP_INT_CLR);
+#ifndef WILC_OPTIMIZE_SLEEP_INT
+	genuChipPSstate = CHIP_SLEEPING_AUTO;
+#endif
+}
+
+static void wilc_wlan_handle_isr_ext(uint32_t int_status)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+#ifdef MEMORY_STATIC
+	uint32_t offset = p->rx_buffer_offset;
+#endif
+	uint8_t *buffer = NULL;
+	uint32_t size;
+	uint32_t retries = 0;
+	int ret = 0;
+	struct rxq_entry_t *rqe;
+
+
+	/**
+	 *      Get the rx size
+	 **/
+
+	size = ((int_status & 0x7fff) << 2);
+
+	while (!size && retries < 10) {
+		uint32_t time = 0;
+		/*looping more secure*/
+		/*zero size make a crashe because the dma will not happen and that will block the firmware*/
+		wilc_debug(N_ERR, "RX Size equal zero ... Trying to read it again for %d time\n", time++);
+		p->hif_func.hif_read_size(&size);
+		size = ((size & 0x7fff) << 2);
+		retries++;
+
+	}
+
+	if (size > 0) {
+#ifdef MEMORY_STATIC
+		if (p->rx_buffer_size - offset < size)
+			offset = 0;
+
+		if (p->rx_buffer)
+			buffer = &p->rx_buffer[offset];
+		else {
+			wilc_debug(N_ERR, "[wilc isr]: fail Rx Buffer is NULL...drop the packets (%d)\n", size);
+			goto _end_;
+		}
+
+#else
+		buffer = p->os_func.os_malloc(size);
+		if (buffer == NULL) {
+			wilc_debug(N_ERR, "[wilc isr]: fail alloc host memory...drop the packets (%d)\n", size);
+			WILC_Sleep(100);
+			goto _end_;
+		}
+#endif
+
+		/**
+		 *      clear the chip's interrupt	 after getting size some register getting corrupted after clear the interrupt
+		 **/
+		p->hif_func.hif_clear_int_ext(DATA_INT_CLR | ENABLE_RX_VMM);
+
+
+		/**
+		 * start transfer
+		 **/
+		ret = p->hif_func.hif_block_rx_ext(0, buffer, size);
+
+		if (!ret) {
+			wilc_debug(N_ERR, "[wilc isr]: fail block rx...\n");
+			goto _end_;
+		}
+_end_:
+
+
+		if (ret) {
+#ifdef MEMORY_STATIC
+			offset += size;
+			p->rx_buffer_offset = offset;
+#endif
+			/**
+			 *      add to rx queue
+			 **/
+			rqe = (struct rxq_entry_t *)p->os_func.os_malloc(sizeof(struct rxq_entry_t));
+			if (rqe != NULL) {
+				rqe->buffer = buffer;
+				rqe->buffer_size = size;
+				PRINT_D(RX_DBG, "rxq entery Size= %d - Address = %p\n", rqe->buffer_size, rqe->buffer);
+				wilc_wlan_rxq_add(rqe);
+				p->os_func.os_signal(p->rxq_wait);
+			}
+		} else {
+#ifndef MEMORY_STATIC
+			if (buffer != NULL)
+				p->os_func.os_free(buffer);
+#endif
+		}
+	}
+#ifdef TCP_ENHANCEMENTS
+	wilc_wlan_handle_rxq();
+#endif
+}
+
+void wilc_handle_isr(void)
+{
+	uint32_t int_status;
+
+	acquire_bus(ACQUIRE_AND_WAKEUP);
+	g_wlan.hif_func.hif_read_int(&int_status);
+
+	if (int_status & PLL_INT_EXT) {
+		wilc_pllupdate_isr_ext(int_status);
+	}
+	if (int_status & DATA_INT_EXT) {
+		wilc_wlan_handle_isr_ext(int_status);
+	#ifndef WILC_OPTIMIZE_SLEEP_INT
+		/* Chip is up and talking*/
+		genuChipPSstate = CHIP_WAKEDUP;
+	#endif
+	}
+	if (int_status & SLEEP_INT_EXT) {
+		wilc_sleeptimer_isr_ext(int_status);
+	}
+
+	if (!(int_status & (ALL_INT_EXT))) {
+#ifdef WILC_SDIO
+		PRINT_D(TX_DBG, ">> UNKNOWN_INTERRUPT - 0x%08x\n", int_status);
+#endif
+		wilc_unknown_isr_ext();
+	}
+#if ((!defined WILC_SDIO) || (defined WILC_SDIO_IRQ_GPIO))
+	linux_wlan_enable_irq();
+#endif
+	release_bus(RELEASE_ALLOW_SLEEP);
+}
+
+/********************************************
+ *
+ *      Firmware download
+ *
+ ********************************************/
+static int wilc_wlan_firmware_download(const uint8_t *buffer, uint32_t buffer_size)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	uint32_t offset;
+	uint32_t addr, size, size2, blksz;
+	uint8_t *dma_buffer;
+	int ret = 0;
+
+	blksz = (1ul << 12); /* Bug 4703: 4KB Good enough size for most platforms = PAGE_SIZE. */
+	/* Allocate a DMA coherent  buffer. */
+
+#if (defined WILC_PREALLOC_AT_BOOT)
+	{
+		extern void *get_fw_buffer(void);
+		dma_buffer = (uint8_t *)get_fw_buffer();
+		PRINT_D(TX_DBG, "fw_buffer = 0x%x\n", dma_buffer);
+	}
+#else
+	dma_buffer = (uint8_t *)g_wlan.os_func.os_malloc(blksz);
+#endif
+	if (dma_buffer == NULL) {
+		/*EIO	5*/
+		ret = -5;
+		PRINT_ER("Can't allocate buffer for firmware download IO error\n ");
+		goto _fail_1;
+	}
+
+	PRINT_D(INIT_DBG, "Downloading firmware size = %d ...\n", buffer_size);
+	/**
+	 *      load the firmware
+	 **/
+	offset = 0;
+	do {
+		memcpy(&addr, &buffer[offset], 4);
+		memcpy(&size, &buffer[offset + 4], 4);
+#ifdef BIG_ENDIAN
+		addr = BYTE_SWAP(addr);
+		size = BYTE_SWAP(size);
+#endif
+		acquire_bus(ACQUIRE_ONLY);
+		offset += 8;
+		while (((int)size) && (offset < buffer_size)) {
+			if (size <= blksz) {
+				size2 = size;
+			} else {
+				size2 = blksz;
+			}
+			/* Copy firmware into a DMA coherent buffer */
+			memcpy(dma_buffer, &buffer[offset], size2);
+			ret = p->hif_func.hif_block_tx(addr, dma_buffer, size2);
+			if (!ret)
+				break;
+
+			addr += size2;
+			offset += size2;
+			size -= size2;
+		}
+		release_bus(RELEASE_ONLY);
+
+		if (!ret) {
+			/*EIO	5*/
+			ret = -5;
+			PRINT_ER("Can't download firmware IO error\n ");
+			goto _fail_;
+		}
+		PRINT_D(INIT_DBG, "Offset = %d\n", offset);
+	} while (offset < buffer_size);
+
+_fail_:
+
+#if (defined WILC_PREALLOC_AT_BOOT)
+
+#else
+	if (dma_buffer)
+		g_wlan.os_func.os_free(dma_buffer);
+#endif
+
+_fail_1:
+
+	return (ret < 0) ? ret : 0;
+}
+
+/********************************************
+ *
+ *      Common
+ *
+ ********************************************/
+static int wilc_wlan_start(void)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	uint32_t reg = 0;
+	int ret;
+	uint32_t chipid;
+
+	/**
+	 *      Set the host interface
+	 **/
+#ifdef OLD_FPGA_BITFILE
+	acquire_bus(ACQUIRE_ONLY);
+	ret = p->hif_func.hif_read_reg(WILC_VMM_CORE_CTL, &reg);
+	if (!ret) {
+		wilc_debug(N_ERR, "[wilc start]: fail read reg vmm_core_ctl...\n");
+		release_bus(RELEASE_ALLOW_SLEEP);
+		return ret;
+	}
+	reg |= (p->io_func.io_type << 2);
+	ret = p->hif_func.hif_write_reg(WILC_VMM_CORE_CTL, reg);
+	if (!ret) {
+		wilc_debug(N_ERR, "[wilc start]: fail write reg vmm_core_ctl...\n");
+		release_bus(RELEASE_ONLY);
+		return ret;
+	}
+#else
+	if (p->io_func.io_type == HIF_SDIO) {
+		reg = 0;
+		reg |= (1 << 3); /* bug 4456 and 4557 */
+	} else if (p->io_func.io_type == HIF_SPI) {
+		reg = 1;
+	}
+	acquire_bus(ACQUIRE_ONLY);
+	ret = p->hif_func.hif_write_reg(WILC_VMM_CORE_CFG, reg);
+	if (!ret) {
+		wilc_debug(N_ERR, "[wilc start]: fail write reg vmm_core_cfg...\n");
+		release_bus(RELEASE_ONLY);
+		/* EIO  5*/
+		ret = -5;
+		return ret;
+	}
+	reg = 0;
+#ifdef WILC_SDIO_IRQ_GPIO
+	reg |= WILC_HAVE_SDIO_IRQ_GPIO;
+#endif
+
+#ifdef WILC_DISABLE_PMU
+#else
+	reg |= WILC_HAVE_USE_PMU;
+#endif
+
+#ifdef WILC_SLEEP_CLK_SRC_XO
+	reg |= WILC_HAVE_SLEEP_CLK_SRC_XO;
+#elif defined WILC_SLEEP_CLK_SRC_RTC
+	reg |= WILC_HAVE_SLEEP_CLK_SRC_RTC;
+#endif
+
+#ifdef WILC_EXT_PA_INV_TX_RX
+	reg |= WILC_HAVE_EXT_PA_INV_TX_RX;
+#endif
+
+	reg |= WILC_HAVE_LEGACY_RF_SETTINGS;
+
+
+/*BugID_5257*/
+/*Set oscillator frequency*/
+#ifdef XTAL_24
+	reg |= WILC_HAVE_XTAL_24;
+#endif
+
+/*BugID_5271*/
+/*Enable/Disable GPIO configuration for FW logs*/
+#ifdef DISABLE_WILC_UART
+	reg |= WILC_HAVE_DISABLE_WILC_UART;
+#endif
+
+	ret = p->hif_func.hif_write_reg(WILC_GP_REG_1, reg);
+	if (!ret) {
+		wilc_debug(N_ERR, "[wilc start]: fail write WILC_GP_REG_1 ...\n");
+		release_bus(RELEASE_ONLY);
+		/* EIO  5*/
+		ret = -5;
+		return ret;
+	}
+#endif
+
+
+	/**
+	 *      Bus related
+	 **/
+	p->hif_func.hif_sync_ext(NUM_INT_EXT);
+
+	ret = p->hif_func.hif_read_reg(0x1000, &chipid);
+	if (!ret) {
+		wilc_debug(N_ERR, "[wilc start]: fail read reg 0x1000 ...\n");
+		release_bus(RELEASE_ONLY);
+		/* EIO  5*/
+		ret = -5;
+		return ret;
+	}
+
+	/**
+	 *      Go...
+	 **/
+
+
+	p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+	if ((reg & (1ul << 10)) == (1ul << 10)) {
+		reg &= ~(1ul << 10);
+		p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
+		p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+	}
+
+	reg |= (1ul << 10);
+	ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
+	p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+	release_bus(RELEASE_ONLY);
+
+	return (ret < 0) ? ret : 0;
+}
+
+void wilc_wlan_global_reset(void)
+{
+
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	acquire_bus(ACQUIRE_AND_WAKEUP);
+	p->hif_func.hif_write_reg(WILC_GLB_RESET_0, 0x0);
+	release_bus(RELEASE_ONLY);
+}
+static int wilc_wlan_stop(void)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	uint32_t reg = 0;
+	int ret;
+	uint8_t timeout = 10;
+	/**
+	 *      TODO: stop the firmware, need a re-download
+	 **/
+	acquire_bus(ACQUIRE_AND_WAKEUP);
+
+	ret = p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+	if (!ret) {
+		PRINT_ER("Error while reading reg\n");
+		release_bus(RELEASE_ALLOW_SLEEP);
+		return ret;
+	}
+
+	reg &= ~(1 << 10);
+
+
+	ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
+	if (!ret) {
+		PRINT_ER("Error while writing reg\n");
+		release_bus(RELEASE_ALLOW_SLEEP);
+		return ret;
+	}
+
+
+
+	do {
+		ret = p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+		if (!ret) {
+			PRINT_ER("Error while reading reg\n");
+			release_bus(RELEASE_ALLOW_SLEEP);
+			return ret;
+		}
+		PRINT_D(GENERIC_DBG, "Read RESET Reg %x : Retry%d\n", reg, timeout);
+		/*Workaround to ensure that the chip is actually reset*/
+		if ((reg & (1 << 10))) {
+			PRINT_D(GENERIC_DBG, "Bit 10 not reset : Retry %d\n", timeout);
+			reg &= ~(1 << 10);
+			ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
+			timeout--;
+		} else {
+			PRINT_D(GENERIC_DBG, "Bit 10 reset after : Retry %d\n", timeout);
+			ret = p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+			if (!ret) {
+				PRINT_ER("Error while reading reg\n");
+				release_bus(RELEASE_ALLOW_SLEEP);
+				return ret;
+			}
+			PRINT_D(GENERIC_DBG, "Read RESET Reg %x : Retry%d\n", reg, timeout);
+			break;
+		}
+
+	} while (timeout);
+#if 1
+/******************************************************************************/
+/* This was add at Bug 4595 to reset the chip while maintaining the bus state */
+/******************************************************************************/
+	reg = ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 8) | (1 << 9) | (1 << 26) | (1 << 29) | (1 << 30) | (1 << 31)); /**/
+	/**/
+	ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);                                 /**/
+	reg = ~(1 << 10);                                                                                               /**/
+	/**/
+	ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);                                 /**/
+/******************************************************************************/
+#endif
+
+	release_bus(RELEASE_ALLOW_SLEEP);
+
+	return ret;
+}
+
+static void wilc_wlan_cleanup(void)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	struct txq_entry_t *tqe;
+	struct rxq_entry_t *rqe;
+	uint32_t reg = 0;
+	int ret;
+
+	p->quit = 1;
+	do {
+		tqe = wilc_wlan_txq_remove_from_head();
+		if (tqe == NULL)
+			break;
+		if (tqe->tx_complete_func)
+			tqe->tx_complete_func(tqe->priv, 0);
+		p->os_func.os_free((void *)tqe);
+	} while (1);
+
+	do {
+		rqe = wilc_wlan_rxq_remove();
+		if (rqe == NULL)
+			break;
+#ifdef MEMORY_DYNAMIC
+		p->os_func.os_free((void *)tqe->buffer);
+#endif
+		p->os_func.os_free((void *)rqe);
+	} while (1);
+
+	/**
+	 *      clean up buffer
+	 **/
+
+#if (defined WILC_PREALLOC_AT_BOOT)
+
+#else
+	#ifdef MEMORY_STATIC
+	if (p->rx_buffer) {
+		p->os_func.os_free(p->rx_buffer);
+		p->rx_buffer = NULL;
+	}
+	#endif
+	if (p->tx_buffer) {
+		p->os_func.os_free(p->tx_buffer);
+		p->tx_buffer = NULL;
+	}
+#endif
+
+	acquire_bus(ACQUIRE_AND_WAKEUP);
+
+
+	ret = p->hif_func.hif_read_reg(WILC_GP_REG_0, &reg);
+	if (!ret) {
+		PRINT_ER("Error while reading reg\n");
+		release_bus(RELEASE_ALLOW_SLEEP);
+	}
+	PRINT_ER("Writing ABORT reg\n");
+	ret = p->hif_func.hif_write_reg(WILC_GP_REG_0, (reg | ABORT_INT));
+	if (!ret) {
+		PRINT_ER("Error while writing reg\n");
+		release_bus(RELEASE_ALLOW_SLEEP);
+	}
+	release_bus(RELEASE_ALLOW_SLEEP);
+	/**
+	 *      io clean up
+	 **/
+	p->hif_func.hif_deinit(NULL);
+
+}
+
+static int wilc_wlan_cfg_commit(int type, uint32_t drvHandler)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	wilc_cfg_frame_t *cfg = &p->cfg_frame;
+	int total_len = p->cfg_frame_offset + 4 + DRIVER_HANDLER_SIZE;
+	int seq_no = p->cfg_seq_no % 256;
+	int driver_handler = (u32)drvHandler;
+
+
+	/**
+	 *      Set up header
+	 **/
+	if (type == WILC_CFG_SET) {             /* Set */
+		cfg->wid_header[0] = 'W';
+	} else {                                        /* Query */
+		cfg->wid_header[0] = 'Q';
+	}
+	cfg->wid_header[1] = seq_no;    /* sequence number */
+	cfg->wid_header[2] = (uint8_t)total_len;
+	cfg->wid_header[3] = (uint8_t)(total_len >> 8);
+	cfg->wid_header[4] = (uint8_t)driver_handler;
+	cfg->wid_header[5] = (uint8_t)(driver_handler >> 8);
+	cfg->wid_header[6] = (uint8_t)(driver_handler >> 16);
+	cfg->wid_header[7] = (uint8_t)(driver_handler >> 24);
+	p->cfg_seq_no = seq_no;
+
+	/**
+	 *      Add to TX queue
+	 **/
+
+	/*Edited by Amr - BugID_4720*/
+	if (!wilc_wlan_txq_add_cfg_pkt(&cfg->wid_header[0], total_len))
+		return -1;
+
+	return 0;
+}
+
+static int wilc_wlan_cfg_set(int start, uint32_t wid, uint8_t *buffer, uint32_t buffer_size, int commit, uint32_t drvHandler)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	uint32_t offset;
+	int ret_size;
+
+
+	if (p->cfg_frame_in_use)
+		return 0;
+
+	if (start)
+		p->cfg_frame_offset = 0;
+
+	offset = p->cfg_frame_offset;
+	ret_size = p->cif_func.cfg_wid_set(p->cfg_frame.frame, offset, (uint16_t)wid, buffer, buffer_size);
+	offset += ret_size;
+	p->cfg_frame_offset = offset;
+
+	if (commit) {
+		PRINT_D(TX_DBG, "[WILC]PACKET Commit with sequence number %d\n", p->cfg_seq_no);
+		PRINT_D(RX_DBG, "Processing cfg_set()\n");
+		p->cfg_frame_in_use = 1;
+
+		/*Edited by Amr - BugID_4720*/
+		if (wilc_wlan_cfg_commit(WILC_CFG_SET, drvHandler))
+			ret_size = 0;   /* BugID_5213 */
+
+		if (p->os_func.os_wait(p->cfg_wait, CFG_PKTS_TIMEOUT)) {
+			PRINT_D(TX_DBG, "Set Timed Out\n");
+			ret_size = 0;
+		}
+		p->cfg_frame_in_use = 0;
+		p->cfg_frame_offset = 0;
+		p->cfg_seq_no += 1;
+
+	}
+
+	return ret_size;
+}
+static int wilc_wlan_cfg_get(int start, uint32_t wid, int commit, uint32_t drvHandler)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	uint32_t offset;
+	int ret_size;
+
+
+	if (p->cfg_frame_in_use)
+		return 0;
+
+	if (start)
+		p->cfg_frame_offset = 0;
+
+	offset = p->cfg_frame_offset;
+	ret_size = p->cif_func.cfg_wid_get(p->cfg_frame.frame, offset, (uint16_t)wid);
+	offset += ret_size;
+	p->cfg_frame_offset = offset;
+
+	if (commit) {
+		p->cfg_frame_in_use = 1;
+
+		/*Edited by Amr - BugID_4720*/
+		if (wilc_wlan_cfg_commit(WILC_CFG_QUERY, drvHandler))
+			ret_size = 0;   /* BugID_5213 */
+
+
+		if (p->os_func.os_wait(p->cfg_wait, CFG_PKTS_TIMEOUT)) {
+			PRINT_D(TX_DBG, "Get Timed Out\n");
+			ret_size = 0;
+		}
+		PRINT_D(GENERIC_DBG, "[WILC]Get Response received\n");
+		p->cfg_frame_in_use = 0;
+		p->cfg_frame_offset = 0;
+		p->cfg_seq_no += 1;
+	}
+
+	return ret_size;
+}
+
+static int wilc_wlan_cfg_get_val(uint32_t wid, uint8_t *buffer, uint32_t buffer_size)
+{
+	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
+	int ret;
+
+	ret = p->cif_func.cfg_wid_get_val((uint16_t)wid, buffer, buffer_size);
+
+	return ret;
+}
+
+void wilc_bus_set_max_speed(void)
+{
+
+	/* Increase bus speed to max possible.  */
+	g_wlan.hif_func.hif_set_max_bus_speed();
+}
+
+void wilc_bus_set_default_speed(void)
+{
+
+	/* Restore bus speed to default.  */
+	g_wlan.hif_func.hif_set_default_bus_speed();
+}
+uint32_t init_chip(void)
+{
+	uint32_t chipid;
+	uint32_t reg, ret = 0;
+
+#if defined(PLAT_RK3026_TCHIP)
+	acquire_bus(ACQUIRE_AND_WAKEUP); /* AMR : 0422 RK3026 Crash issue */
+#else
+	acquire_bus(ACQUIRE_ONLY);
+#endif
+
+	chipid = wilc_get_chipid(true);
+
+
+
+	if ((chipid & 0xfff) != 0xa0) {
+		/**
+		 * Avoid booting from boot ROM. Make sure that Drive IRQN [SDIO platform]
+		 * or SD_DAT3 [SPI platform] to ?1?
+		 **/
+		/* Set cortus reset register to register control. */
+		ret = g_wlan.hif_func.hif_read_reg(0x1118, &reg);
+		if (!ret) {
+			wilc_debug(N_ERR, "[wilc start]: fail read reg 0x1118 ...\n");
+			return ret;
+		}
+		reg |= (1 << 0);
+		ret = g_wlan.hif_func.hif_write_reg(0x1118, reg);
+		if (!ret) {
+			wilc_debug(N_ERR, "[wilc start]: fail write reg 0x1118 ...\n");
+			return ret;
+		}
+		/**
+		 * Write branch intruction to IRAM (0x71 trap) at location 0xFFFF0000
+		 * (Cortus map) or C0000 (AHB map).
+		 **/
+		ret = g_wlan.hif_func.hif_write_reg(0xc0000, 0x71);
+		if (!ret) {
+			wilc_debug(N_ERR, "[wilc start]: fail write reg 0xc0000 ...\n");
+			return ret;
+		}
+	}
+
+	release_bus(RELEASE_ONLY);
+
+	return ret;
+
+}
+
+uint32_t wilc_get_chipid(uint8_t update)
+{
+	static uint32_t chipid;
+	/* SDIO can't read into global variables */
+	/* Use this variable as a temp, then copy to the global */
+	uint32_t tempchipid = 0;
+	uint32_t rfrevid;
+
+	if (chipid == 0 || update != 0) {
+		g_wlan.hif_func.hif_read_reg(0x1000, &tempchipid);
+		g_wlan.hif_func.hif_read_reg(0x13f4, &rfrevid);
+		if (!ISWILC1000(tempchipid)) {
+			chipid = 0;
+			goto _fail_;
+		}
+		if (tempchipid == 0x1002a0) {
+			if (rfrevid == 0x1) { /* 1002A0 */
+			} else { /* if (rfrevid == 0x2) */   /* 1002A1 */
+				tempchipid = 0x1002a1;
+			}
+		} else if (tempchipid == 0x1002b0) {
+			if (rfrevid == 3) { /* 1002B0 */
+			} else if (rfrevid == 4) { /* 1002B1 */
+				tempchipid = 0x1002b1;
+			} else { /* if(rfrevid == 5) */   /* 1002B2 */
+				tempchipid = 0x1002b2;
+			}
+		} else {
+		}
+
+		chipid = tempchipid;
+	}
+_fail_:
+	return chipid;
+}
+
+#ifdef COMPLEMENT_BOOT
+uint8_t core_11b_ready(void)
+{
+	uint32_t reg_val;
+
+	acquire_bus(ACQUIRE_ONLY);
+	g_wlan.hif_func.hif_write_reg(0x16082c, 1);
+	g_wlan.hif_func.hif_write_reg(0x161600, 0x90);
+	g_wlan.hif_func.hif_read_reg(0x161600, &reg_val);
+	release_bus(RELEASE_ONLY);
+
+	if (reg_val == 0x90)
+		return 0;
+	else
+		return 1;
+}
+#endif
+
+int wilc_wlan_init(wilc_wlan_inp_t *inp, wilc_wlan_oup_t *oup)
+{
+
+	int ret = 0;
+
+	PRINT_D(INIT_DBG, "Initializing WILC_Wlan ...\n");
+
+	memset((void *)&g_wlan, 0, sizeof(wilc_wlan_dev_t));
+
+	/**
+	 *      store the input
+	 **/
+	memcpy((void *)&g_wlan.os_func, (void *)&inp->os_func, sizeof(wilc_wlan_os_func_t));
+	memcpy((void *)&g_wlan.io_func, (void *)&inp->io_func, sizeof(wilc_wlan_io_func_t));
+	memcpy((void *)&g_wlan.net_func, (void *)&inp->net_func, sizeof(wilc_wlan_net_func_t));
+	memcpy((void *)&g_wlan.indicate_func, (void *)&inp->indicate_func, sizeof(wilc_wlan_net_func_t));
+	g_wlan.hif_lock = inp->os_context.hif_critical_section;
+	g_wlan.txq_lock = inp->os_context.txq_critical_section;
+
+	/*Added by Amr - BugID_4720*/
+	g_wlan.txq_add_to_head_lock = inp->os_context.txq_add_to_head_critical_section;
+
+	/*Added by Amr - BugID_4720*/
+	g_wlan.txq_spinlock = inp->os_context.txq_spin_lock;
+
+	g_wlan.rxq_lock = inp->os_context.rxq_critical_section;
+	g_wlan.txq_wait = inp->os_context.txq_wait_event;
+	g_wlan.rxq_wait = inp->os_context.rxq_wait_event;
+	g_wlan.cfg_wait = inp->os_context.cfg_wait_event;
+	g_wlan.tx_buffer_size = inp->os_context.tx_buffer_size;
+#if defined (MEMORY_STATIC)
+	g_wlan.rx_buffer_size = inp->os_context.rx_buffer_size;
+#endif
+	/***
+	 *      host interface init
+	 **/
+#if defined(PLAT_RK3026_TCHIP) /* AMR : 0422 RK3026 Crash issue */
+	if (!g_wilc_initialized) {
+		custom_lock_bus(g_mac_open);
+		custom_wakeup(g_mac_open);
+	}
+#endif
+
+	if ((inp->io_func.io_type & 0x1) == HIF_SDIO) {
+		if (!hif_sdio.hif_init(inp, wilc_debug)) {
+			/* EIO	5 */
+			ret = -5;
+			goto _fail_;
+		}
+		memcpy((void *)&g_wlan.hif_func, &hif_sdio, sizeof(wilc_hif_func_t));
+	} else {
+		if ((inp->io_func.io_type & 0x1) == HIF_SPI) {
+			/**
+			 *      TODO:
+			 **/
+			if (!hif_spi.hif_init(inp, wilc_debug)) {
+				/* EIO	5 */
+				ret = -5;
+				goto _fail_;
+			}
+			memcpy((void *)&g_wlan.hif_func, &hif_spi, sizeof(wilc_hif_func_t));
+		} else {
+			/* EIO	5 */
+			ret = -5;
+			goto _fail_;
+		}
+	}
+
+	/***
+	 *      mac interface init
+	 **/
+	if (!mac_cfg.cfg_init(wilc_debug)) {
+		/* ENOBUFS	105 */
+		ret = -105;
+		goto _fail_;
+	}
+	memcpy((void *)&g_wlan.cif_func, &mac_cfg, sizeof(wilc_cfg_func_t));
+
+
+	/**
+	 *      alloc tx, rx buffer
+	 **/
+#if (defined WILC_PREALLOC_AT_BOOT)
+	extern void *get_tx_buffer(void);
+	extern void *get_rx_buffer(void);
+
+	PRINT_D(TX_DBG, "malloc before, g_wlan.tx_buffer = 0x%x, g_wlan.rx_buffer = 0x%x\n", g_wlan.tx_buffer, g_wlan.rx_buffer);
+#endif
+
+
+
+	if (g_wlan.tx_buffer == NULL)
+#if (defined WILC_PREALLOC_AT_BOOT)
+		g_wlan.tx_buffer = (uint8_t *)get_tx_buffer();
+#else
+		g_wlan.tx_buffer = (uint8_t *)g_wlan.os_func.os_malloc(g_wlan.tx_buffer_size);
+#endif
+	PRINT_D(TX_DBG, "g_wlan.tx_buffer = %p\n", g_wlan.tx_buffer);
+
+	if (g_wlan.tx_buffer == NULL) {
+		/* ENOBUFS	105 */
+		ret = -105;
+		PRINT_ER("Can't allocate Tx Buffer");
+		goto _fail_;
+	}
+
+/* rx_buffer is not used unless we activate USE_MEM STATIC which is not applicable, allocating such memory is useless*/
+#if defined (MEMORY_STATIC)
+	if (g_wlan.rx_buffer == NULL)
+  #if (defined WILC_PREALLOC_AT_BOOT)
+		g_wlan.rx_buffer = (uint8_t *)get_rx_buffer();
+  #else
+		g_wlan.rx_buffer = (uint8_t *)g_wlan.os_func.os_malloc(g_wlan.rx_buffer_size);
+  #endif
+	PRINT_D(TX_DBG, "g_wlan.rx_buffer =%p\n", g_wlan.rx_buffer);
+	if (g_wlan.rx_buffer == NULL) {
+		/* ENOBUFS	105 */
+		ret = -105;
+		PRINT_ER("Can't allocate Rx Buffer");
+		goto _fail_;
+	}
+#endif
+
+	/**
+	 *      export functions
+	 **/
+	oup->wlan_firmware_download = wilc_wlan_firmware_download;
+	oup->wlan_start = wilc_wlan_start;
+	oup->wlan_stop = wilc_wlan_stop;
+	oup->wlan_add_to_tx_que = wilc_wlan_txq_add_net_pkt;
+	oup->wlan_handle_tx_que = wilc_wlan_handle_txq;
+	oup->wlan_handle_rx_que = wilc_wlan_handle_rxq;
+	oup->wlan_handle_rx_isr = wilc_handle_isr;
+	oup->wlan_cleanup = wilc_wlan_cleanup;
+	oup->wlan_cfg_set = wilc_wlan_cfg_set;
+	oup->wlan_cfg_get = wilc_wlan_cfg_get;
+	oup->wlan_cfg_get_value = wilc_wlan_cfg_get_val;
+
+	/*Bug3959: transmitting mgmt frames received from host*/
+	#if defined(WILC_AP_EXTERNAL_MLME) || defined(WILC_P2P)
+	oup->wlan_add_mgmt_to_tx_que = wilc_wlan_txq_add_mgmt_pkt;
+
+	#ifdef WILC_FULLY_HOSTING_AP
+	oup->wlan_add_data_to_tx_que = wilc_FH_wlan_txq_add_net_pkt;
+	#endif
+	#endif
+
+	if (!init_chip()) {
+		/* EIO	5 */
+		ret = -5;
+		goto _fail_;
+	}
+#ifdef	TCP_ACK_FILTER
+	Init_TCP_tracking();
+#endif
+
+#if defined(PLAT_RK3026_TCHIP) /* AMR : 0422 RK3026 Crash issue */
+	if (!g_wilc_initialized)
+		custom_unlock_bus(g_mac_open);
+#endif
+
+	return 1;
+
+_fail_:
+
+#if (defined WILC_PREALLOC_AT_BOOT)
+
+#else
+  #ifdef MEMORY_STATIC
+	if (g_wlan.rx_buffer) {
+		g_wlan.os_func.os_free(g_wlan.rx_buffer);
+		g_wlan.rx_buffer = NULL;
+	}
+  #endif
+	if (g_wlan.tx_buffer) {
+		g_wlan.os_func.os_free(g_wlan.tx_buffer);
+		g_wlan.tx_buffer = NULL;
+	}
+#endif
+
+#if defined(PLAT_RK3026_TCHIP) /* AMR : 0422 RK3026 Crash issue */
+	if (!g_wilc_initialized)
+		custom_unlock_bus(g_mac_open);
+#endif
+
+	return ret;
+
+}
+
+#define BIT31 (1 << 31)
+u16 Set_machw_change_vir_if(bool bValue)
+{
+	u16 ret;
+	u32 reg;
+
+	/*Reset WILC_CHANGING_VIR_IF register to allow adding futrue keys to CE H/W*/
+	(&g_wlan)->os_func.os_enter_cs((&g_wlan)->hif_lock);
+	ret = (&g_wlan)->hif_func.hif_read_reg(WILC_CHANGING_VIR_IF, &reg);
+	if (!ret) {
+		PRINT_ER("Error while Reading reg WILC_CHANGING_VIR_IF\n");
+	}
+
+	if (bValue) {
+		reg |= (BIT31);
+	} else {
+		reg &= ~(BIT31);
+	}
+
+	ret = (&g_wlan)->hif_func.hif_write_reg(WILC_CHANGING_VIR_IF, reg);
+
+	if (!ret) {
+		PRINT_ER("Error while writing reg WILC_CHANGING_VIR_IF\n");
+	}
+	(&g_wlan)->os_func.os_leave_cs((&g_wlan)->hif_lock);
+
+	return ret;
+}
+
+#ifdef WILC_FULLY_HOSTING_AP
+wilc_wlan_dev_t *Get_wlan_context(u16 *pu16size)
+{
+	*pu16size = sizeof(wilc_wlan_dev_t);
+	return &g_wlan;
+}
+#endif
+
diff --git a/drivers/staging/wilc1000/wilc_wlan.h b/drivers/staging/wilc1000/wilc_wlan.h
new file mode 100644
index 000000000000..0ba7ec69e2b4
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wlan.h
@@ -0,0 +1,321 @@
+#ifndef WILC_WLAN_H
+#define WILC_WLAN_H
+
+#include "wilc_type.h"
+
+
+#define ISWILC1000(id)   (((id & 0xfffff000) == 0x100000) ? 1 : 0)
+
+
+/********************************************
+ *
+ *      Mac eth header length
+ *
+ ********************************************/
+#define DRIVER_HANDLER_SIZE 4
+#define MAX_MAC_HDR_LEN         26 /* QOS_MAC_HDR_LEN */
+#define SUB_MSDU_HEADER_LENGTH  14
+#define SNAP_HDR_LEN            8
+#define ETHERNET_HDR_LEN          14
+#define WORD_ALIGNMENT_PAD        0
+
+#define ETH_ETHERNET_HDR_OFFSET   (MAX_MAC_HDR_LEN + SUB_MSDU_HEADER_LENGTH + \
+				   SNAP_HDR_LEN - ETHERNET_HDR_LEN + WORD_ALIGNMENT_PAD)
+
+/*Bug3959: transmitting mgmt frames received from host*/
+#define HOST_HDR_OFFSET		4
+#define ETHERNET_HDR_LEN          14
+#define IP_HDR_LEN                20
+#define IP_HDR_OFFSET             ETHERNET_HDR_LEN
+#define UDP_HDR_OFFSET            (IP_HDR_LEN + IP_HDR_OFFSET)
+#define UDP_HDR_LEN               8
+#define UDP_DATA_OFFSET           (UDP_HDR_OFFSET + UDP_HDR_LEN)
+#define ETH_CONFIG_PKT_HDR_LEN    UDP_DATA_OFFSET
+
+#define ETH_CONFIG_PKT_HDR_OFFSET (ETH_ETHERNET_HDR_OFFSET + \
+				   ETH_CONFIG_PKT_HDR_LEN)
+#define   ACTION         0xD0
+#define   PROBE_REQ   0x40
+#ifdef WILC_FULLY_HOSTING_AP
+#define	FH_TX_HOST_HDR_OFFSET	24
+#endif
+
+/********************************************
+ *
+ *      Endian Conversion
+ *
+ ********************************************/
+
+#define BYTE_SWAP(val) ((((val) & 0x000000FF) << 24) + \
+			(((val) & 0x0000FF00) << 8)  + \
+			(((val) & 0x00FF0000) >> 8)   +	\
+			(((val) & 0xFF000000) >> 24))
+
+/********************************************
+ *
+ *      Register Defines
+ *
+ ********************************************/
+#define WILC_PERIPH_REG_BASE 0x1000
+/*BugID_5137*/
+#define WILC_CHANGING_VIR_IF                     (0x108c)
+#define WILC_CHIPID	(WILC_PERIPH_REG_BASE)
+#define WILC_GLB_RESET_0 (WILC_PERIPH_REG_BASE + 0x400)
+#define WILC_PIN_MUX_0 (WILC_PERIPH_REG_BASE + 0x408)
+#define WILC_HOST_TX_CTRL (WILC_PERIPH_REG_BASE + 0x6c)
+#define WILC_HOST_RX_CTRL_0 (WILC_PERIPH_REG_BASE + 0x70)
+#define WILC_HOST_RX_CTRL_1 (WILC_PERIPH_REG_BASE + 0x74)
+#define WILC_HOST_VMM_CTL	(WILC_PERIPH_REG_BASE + 0x78)
+#define WILC_HOST_RX_CTRL	(WILC_PERIPH_REG_BASE + 0x80)
+#define WILC_HOST_RX_EXTRA_SIZE	(WILC_PERIPH_REG_BASE + 0x84)
+#define WILC_HOST_TX_CTRL_1	(WILC_PERIPH_REG_BASE + 0x88)
+#define WILC_MISC	(WILC_PERIPH_REG_BASE + 0x428)
+#define WILC_INTR_REG_BASE (WILC_PERIPH_REG_BASE + 0xa00)
+#define WILC_INTR_ENABLE (WILC_INTR_REG_BASE)
+#define WILC_INTR2_ENABLE (WILC_INTR_REG_BASE + 4)
+
+#define WILC_INTR_POLARITY (WILC_INTR_REG_BASE + 0x10)
+#define WILC_INTR_TYPE (WILC_INTR_REG_BASE + 0x20)
+#define WILC_INTR_CLEAR (WILC_INTR_REG_BASE + 0x30)
+#define WILC_INTR_STATUS (WILC_INTR_REG_BASE + 0x40)
+
+#define WILC_VMM_TBL_SIZE 64
+#define WILC_VMM_TX_TBL_BASE (0x150400)
+#define WILC_VMM_RX_TBL_BASE (0x150500)
+
+#define WILC_VMM_BASE 0x150000
+#define WILC_VMM_CORE_CTL (WILC_VMM_BASE)
+#define WILC_VMM_TBL_CTL (WILC_VMM_BASE + 0x4)
+#define WILC_VMM_TBL_ENTRY (WILC_VMM_BASE + 0x8)
+#define WILC_VMM_TBL0_SIZE (WILC_VMM_BASE + 0xc)
+#define WILC_VMM_TO_HOST_SIZE (WILC_VMM_BASE + 0x10)
+#define WILC_VMM_CORE_CFG (WILC_VMM_BASE + 0x14)
+#define WILC_VMM_TBL_ACTIVE (WILC_VMM_BASE + 040)
+#define WILC_VMM_TBL_STATUS (WILC_VMM_BASE + 0x44)
+
+#define WILC_SPI_REG_BASE 0xe800
+#define WILC_SPI_CTL (WILC_SPI_REG_BASE)
+#define WILC_SPI_MASTER_DMA_ADDR (WILC_SPI_REG_BASE + 0x4)
+#define WILC_SPI_MASTER_DMA_COUNT (WILC_SPI_REG_BASE + 0x8)
+#define WILC_SPI_SLAVE_DMA_ADDR (WILC_SPI_REG_BASE + 0xc)
+#define WILC_SPI_SLAVE_DMA_COUNT (WILC_SPI_REG_BASE + 0x10)
+#define WILC_SPI_TX_MODE (WILC_SPI_REG_BASE + 0x20)
+#define WILC_SPI_PROTOCOL_CONFIG (WILC_SPI_REG_BASE + 0x24)
+#define WILC_SPI_INTR_CTL (WILC_SPI_REG_BASE + 0x2c)
+
+#define WILC_SPI_PROTOCOL_OFFSET (WILC_SPI_PROTOCOL_CONFIG - WILC_SPI_REG_BASE)
+
+#define WILC_AHB_DATA_MEM_BASE 0x30000
+#define WILC_AHB_SHARE_MEM_BASE 0xd0000
+
+#define WILC_VMM_TBL_RX_SHADOW_BASE WILC_AHB_SHARE_MEM_BASE /* Bug 4477 fix */
+#define WILC_VMM_TBL_RX_SHADOW_SIZE (256) /* Bug 4477 fix */
+
+#define WILC_GP_REG_0   0x149c
+#define WILC_GP_REG_1   0x14a0
+
+#define rHAVE_SDIO_IRQ_GPIO_BIT      (0)
+#define rHAVE_USE_PMU_BIT            (1)
+#define rHAVE_SLEEP_CLK_SRC_RTC_BIT  (2)
+#define rHAVE_SLEEP_CLK_SRC_XO_BIT   (3)
+#define rHAVE_EXT_PA_INV_TX_RX_BIT   (4)
+#define rHAVE_LEGACY_RF_SETTINGS_BIT (5)
+#define rHAVE_XTAL_24_BIT            (6)
+#define rHAVE_DISABLE_WILC_UART_BIT   (7)
+
+
+#define WILC_HAVE_SDIO_IRQ_GPIO       (1 << rHAVE_SDIO_IRQ_GPIO_BIT)
+#define WILC_HAVE_USE_PMU             (1 << rHAVE_USE_PMU_BIT)
+#define WILC_HAVE_SLEEP_CLK_SRC_RTC   (1 << rHAVE_SLEEP_CLK_SRC_RTC_BIT)
+#define WILC_HAVE_SLEEP_CLK_SRC_XO    (1 << rHAVE_SLEEP_CLK_SRC_XO_BIT)
+#define WILC_HAVE_EXT_PA_INV_TX_RX    (1 << rHAVE_EXT_PA_INV_TX_RX_BIT)
+#define WILC_HAVE_LEGACY_RF_SETTINGS  (1 << rHAVE_LEGACY_RF_SETTINGS_BIT)
+#define WILC_HAVE_XTAL_24             (1 << rHAVE_XTAL_24_BIT)
+#define WILC_HAVE_DISABLE_WILC_UART    (1 << rHAVE_DISABLE_WILC_UART_BIT)
+
+
+/********************************************
+ *
+ *      Wlan Defines
+ *
+ ********************************************/
+#define WILC_CFG_PKT	1
+#define WILC_NET_PKT 0
+/*Bug3959: transmitting mgmt frames received from host*/
+#ifdef WILC_AP_EXTERNAL_MLME
+#define WILC_MGMT_PKT 2
+
+#ifdef WILC_FULLY_HOSTING_AP
+#define WILC_FH_DATA_PKT 4
+#endif
+
+#endif /*WILC_AP_EXTERNAL_MLME*/
+#define WILC_CFG_SET 1
+#define WILC_CFG_QUERY 0
+
+#define WILC_CFG_RSP	1
+#define WILC_CFG_RSP_STATUS 2
+#define WILC_CFG_RSP_SCAN 3
+
+#ifdef WILC_SDIO
+#define WILC_PLL_TO	4
+#else
+#define WILC_PLL_TO	2
+#endif
+
+
+#define ABORT_INT   (1 << 31)
+
+/*******************************************/
+/*        E0 and later Interrupt flags.    */
+/*******************************************/
+/*******************************************/
+/*        E0 and later Interrupt flags.    */
+/*           IRQ Status word               */
+/* 15:0 = DMA count in words.              */
+/* 16: INT0 flag                           */
+/* 17: INT1 flag                           */
+/* 18: INT2 flag                           */
+/* 19: INT3 flag                           */
+/* 20: INT4 flag                           */
+/* 21: INT5 flag                           */
+/*******************************************/
+#define                 IRG_FLAGS_OFFSET 16
+#define IRQ_DMA_WD_CNT_MASK ((1ul << IRG_FLAGS_OFFSET) - 1)
+#define INT_0           (1 << (IRG_FLAGS_OFFSET))
+#define INT_1           (1 << (IRG_FLAGS_OFFSET + 1))
+#define INT_2           (1 << (IRG_FLAGS_OFFSET + 2))
+#define INT_3           (1 << (IRG_FLAGS_OFFSET + 3))
+#define INT_4           (1 << (IRG_FLAGS_OFFSET + 4))
+#define INT_5           (1 << (IRG_FLAGS_OFFSET + 5))
+#define MAX_NUM_INT     (6)
+
+/*******************************************/
+/*        E0 and later Interrupt flags.    */
+/*           IRQ Clear word                */
+/* 0: Clear INT0                           */
+/* 1: Clear INT1                           */
+/* 2: Clear INT2                           */
+/* 3: Clear INT3                           */
+/* 4: Clear INT4                           */
+/* 5: Clear INT5                           */
+/* 6: Select VMM table 1                   */
+/* 7: Select VMM table 2                   */
+/* 8: Enable VMM                           */
+/*******************************************/
+#define CLR_INT0             (1 << 0)
+#define CLR_INT1             (1 << 1)
+#define CLR_INT2             (1 << 2)
+#define CLR_INT3             (1 << 3)
+#define CLR_INT4             (1 << 4)
+#define CLR_INT5             (1 << 5)
+#define SEL_VMM_TBL0         (1 << 6)
+#define SEL_VMM_TBL1         (1 << 7)
+#define EN_VMM               (1 << 8)
+
+#define DATA_INT_EXT	INT_0
+#define PLL_INT_EXT         INT_1
+#define SLEEP_INT_EXT	INT_2
+#define ALL_INT_EXT     (DATA_INT_EXT | PLL_INT_EXT | SLEEP_INT_EXT)
+#define NUM_INT_EXT     (3)
+
+#define DATA_INT_CLR	CLR_INT0
+#define PLL_INT_CLR         CLR_INT1
+#define SLEEP_INT_CLR	CLR_INT2
+
+#define ENABLE_RX_VMM   (SEL_VMM_TBL1 | EN_VMM)
+#define ENABLE_TX_VMM   (SEL_VMM_TBL0 | EN_VMM)
+
+
+/*time for expiring the semaphores of cfg packets*/
+#define CFG_PKTS_TIMEOUT	2000
+/********************************************
+ *
+ *      Debug Type
+ *
+ ********************************************/
+typedef void (*wilc_debug_func)(uint32_t, char *, ...);
+
+/********************************************
+ *
+ *      Tx/Rx Queue Structure
+ *
+ ********************************************/
+
+struct txq_entry_t {
+	struct txq_entry_t *next;
+	struct txq_entry_t *prev;
+	int type;
+	int tcp_PendingAck_index;
+	uint8_t *buffer;
+	int buffer_size;
+	void *priv;
+	int status;
+	void (*tx_complete_func)(void *, int);
+};
+
+struct rxq_entry_t {
+	struct rxq_entry_t *next;
+	uint8_t *buffer;
+	int buffer_size;
+};
+
+/********************************************
+ *
+ *      Host IF Structure
+ *
+ ********************************************/
+
+typedef struct {
+	int (*hif_init)(wilc_wlan_inp_t *, wilc_debug_func);
+	int (*hif_deinit)(void *);
+	int (*hif_read_reg)(uint32_t, uint32_t *);
+	int (*hif_write_reg)(uint32_t, uint32_t);
+	int (*hif_block_rx)(uint32_t, uint8_t *, uint32_t);
+	int (*hif_block_tx)(uint32_t, uint8_t *, uint32_t);
+	int (*hif_sync)(void);
+	int (*hif_clear_int)(void);
+	int (*hif_read_int)(uint32_t *);
+	int (*hif_clear_int_ext)(uint32_t);
+	int (*hif_read_size)(uint32_t *);
+	int (*hif_block_tx_ext)(uint32_t, uint8_t *, uint32_t);
+	int (*hif_block_rx_ext)(uint32_t, uint8_t *, uint32_t);
+	int (*hif_sync_ext)(int);
+	void (*hif_set_max_bus_speed)(void);
+	void (*hif_set_default_bus_speed)(void);
+} wilc_hif_func_t;
+
+/********************************************
+ *
+ *      Configuration Structure
+ *
+ ********************************************/
+
+#define MAX_CFG_FRAME_SIZE 1468
+
+typedef struct {
+	uint8_t ether_header[14];
+	uint8_t ip_header[20];
+	uint8_t udp_header[8];
+	uint8_t wid_header[8];
+	uint8_t frame[MAX_CFG_FRAME_SIZE];
+} wilc_cfg_frame_t;
+
+typedef struct {
+	int (*wlan_tx)(uint8_t *, uint32_t, wilc_tx_complete_func_t);
+} wilc_wlan_cfg_func_t;
+
+typedef struct {
+	int type;
+	uint32_t seq_no;
+} wilc_cfg_rsp_t;
+
+typedef struct {
+	int (*cfg_wid_set)(uint8_t *, uint32_t, uint16_t, uint8_t *, int);
+	int (*cfg_wid_get)(uint8_t *, uint32_t, uint16_t);
+	int (*cfg_wid_get_val)(uint16_t, uint8_t *, uint32_t);
+	int (*rx_indicate)(uint8_t *, int, wilc_cfg_rsp_t *);
+	int (*cfg_init)(wilc_debug_func);
+} wilc_cfg_func_t;
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_wlan_cfg.c b/drivers/staging/wilc1000/wilc_wlan_cfg.c
new file mode 100644
index 000000000000..3cffe55b3a93
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wlan_cfg.c
@@ -0,0 +1,617 @@
+/* ////////////////////////////////////////////////////////////////////////// */
+/*  */
+/* Copyright (c) Atmel Corporation.  All rights reserved. */
+/*  */
+/* Module Name:  wilc_wlan_cfg.c */
+/*  */
+/*  */
+/* ///////////////////////////////////////////////////////////////////////// */
+
+#include "wilc_wlan_if.h"
+#include "wilc_wlan.h"
+#include "wilc_wlan_cfg.h"
+#include "coreconfigurator.h"
+
+#ifdef WILC_FULLY_HOSTING_AP
+#include "wilc_host_ap.h"
+void WILC_mgm_HOSTAPD_ACK(void *priv, bool bStatus);
+#endif
+
+/********************************************
+ *
+ *      Global Data
+ *
+ ********************************************/
+
+typedef struct {
+	wilc_debug_func dPrint;
+
+	int mac_status;
+	uint8_t mac_address[7];
+	uint8_t ip_address[5];
+	uint8_t bssid[7];
+	uint8_t ssid[34];
+	uint8_t firmware_version[129];
+	uint8_t supp_rate[24];
+	uint8_t wep_key[28];
+	uint8_t i_psk[66];
+	uint8_t hardwareProductVersion[33];
+	uint8_t phyversion[17];
+	uint8_t supp_username[21];
+	uint8_t supp_password[64];
+	uint8_t assoc_req[256];
+	uint8_t assoc_rsp[256];
+	uint8_t firmware_info[8];
+	uint8_t scan_result[256];
+	uint8_t scan_result1[256];
+} wilc_mac_cfg_t;
+
+static wilc_mac_cfg_t g_mac;
+
+static wilc_cfg_byte_t g_cfg_byte[] = {
+	{WID_BSS_TYPE, 0},
+	{WID_CURRENT_TX_RATE, 0},
+	{WID_CURRENT_CHANNEL, 0},
+	{WID_PREAMBLE, 0},
+	{WID_11G_OPERATING_MODE, 0},
+	{WID_STATUS, 0},
+	{WID_SCAN_TYPE, 0},
+	{WID_KEY_ID, 0},
+	{WID_QOS_ENABLE, 0},
+	{WID_POWER_MANAGEMENT, 0},
+	{WID_11I_MODE, 0},
+	{WID_AUTH_TYPE, 0},
+	{WID_SITE_SURVEY, 0},
+	{WID_LISTEN_INTERVAL, 0},
+	{WID_DTIM_PERIOD, 0},
+	{WID_ACK_POLICY, 0},
+	{WID_BCAST_SSID, 0},
+	{WID_REKEY_POLICY, 0},
+	{WID_SHORT_SLOT_ALLOWED, 0},
+	{WID_START_SCAN_REQ, 0},
+	{WID_RSSI, 0},
+	{WID_LINKSPEED, 0},
+	{WID_AUTO_RX_SENSITIVITY, 0},
+	{WID_DATAFLOW_CONTROL, 0},
+	{WID_SCAN_FILTER, 0},
+	{WID_11N_PROT_MECH, 0},
+	{WID_11N_ERP_PROT_TYPE, 0},
+	{WID_11N_ENABLE, 0},
+	{WID_11N_OPERATING_MODE, 0},
+	{WID_11N_OBSS_NONHT_DETECTION, 0},
+	{WID_11N_HT_PROT_TYPE, 0},
+	{WID_11N_RIFS_PROT_ENABLE, 0},
+	{WID_11N_SMPS_MODE, 0},
+	{WID_11N_CURRENT_TX_MCS, 0},
+	{WID_11N_SHORT_GI_ENABLE, 0},
+	{WID_RIFS_MODE, 0},
+	{WID_TX_ABORT_CONFIG, 0},
+	{WID_11N_IMMEDIATE_BA_ENABLED, 0},
+	{WID_11N_TXOP_PROT_DISABLE, 0},
+	{WID_NIL, 0}
+};
+
+static wilc_cfg_hword_t g_cfg_hword[] = {
+	{WID_LINK_LOSS_THRESHOLD, 0},
+	{WID_RTS_THRESHOLD, 0},
+	{WID_FRAG_THRESHOLD, 0},
+	{WID_SHORT_RETRY_LIMIT, 0},
+	{WID_LONG_RETRY_LIMIT, 0},
+	{WID_BEACON_INTERVAL, 0},
+	{WID_RX_SENSE, 0},
+	{WID_ACTIVE_SCAN_TIME, 0},
+	{WID_PASSIVE_SCAN_TIME, 0},
+	{WID_SITE_SURVEY_SCAN_TIME, 0},
+	{WID_JOIN_START_TIMEOUT, 0},
+	{WID_AUTH_TIMEOUT, 0},
+	{WID_ASOC_TIMEOUT, 0},
+	{WID_11I_PROTOCOL_TIMEOUT, 0},
+	{WID_EAPOL_RESPONSE_TIMEOUT, 0},
+	{WID_11N_SIG_QUAL_VAL, 0},
+	{WID_CCA_THRESHOLD, 0},
+	{WID_NIL, 0}
+};
+
+static wilc_cfg_word_t g_cfg_word[] = {
+	{WID_FAILED_COUNT, 0},
+	{WID_RETRY_COUNT, 0},
+	{WID_MULTIPLE_RETRY_COUNT, 0},
+	{WID_FRAME_DUPLICATE_COUNT, 0},
+	{WID_ACK_FAILURE_COUNT, 0},
+	{WID_RECEIVED_FRAGMENT_COUNT, 0},
+	{WID_MCAST_RECEIVED_FRAME_COUNT, 0},
+	{WID_FCS_ERROR_COUNT, 0},
+	{WID_SUCCESS_FRAME_COUNT, 0},
+	{WID_TX_FRAGMENT_COUNT, 0},
+	{WID_TX_MULTICAST_FRAME_COUNT, 0},
+	{WID_RTS_SUCCESS_COUNT, 0},
+	{WID_RTS_FAILURE_COUNT, 0},
+	{WID_WEP_UNDECRYPTABLE_COUNT, 0},
+	{WID_REKEY_PERIOD, 0},
+	{WID_REKEY_PACKET_COUNT, 0},
+	{WID_HW_RX_COUNT, 0},
+	{WID_GET_INACTIVE_TIME, 0},
+	{WID_NIL, 0}
+
+};
+
+static wilc_cfg_str_t g_cfg_str[] = {
+	{WID_SSID, g_mac.ssid},	/* 33 + 1 bytes */
+	{WID_FIRMWARE_VERSION, g_mac.firmware_version},
+	{WID_OPERATIONAL_RATE_SET, g_mac.supp_rate},
+	{WID_BSSID, g_mac.bssid},	/* 6 bytes */
+	{WID_WEP_KEY_VALUE, g_mac.wep_key},	/* 27 bytes */
+	{WID_11I_PSK, g_mac.i_psk},	/* 65 bytes */
+	/* {WID_11E_P_ACTION_REQ, g_mac.action_req}, */
+	{WID_HARDWARE_VERSION, g_mac.hardwareProductVersion},
+	{WID_MAC_ADDR, g_mac.mac_address},
+	{WID_PHY_VERSION, g_mac.phyversion},
+	{WID_SUPP_USERNAME, g_mac.supp_username},
+	{WID_SUPP_PASSWORD, g_mac.supp_password},
+	{WID_SITE_SURVEY_RESULTS, g_mac.scan_result},
+	{WID_SITE_SURVEY_RESULTS, g_mac.scan_result1},
+	/* {WID_RX_POWER_LEVEL, g_mac.channel_rssi}, */
+	{WID_ASSOC_REQ_INFO, g_mac.assoc_req},
+	{WID_ASSOC_RES_INFO, g_mac.assoc_rsp},
+	/* {WID_11N_P_ACTION_REQ, g_mac.action_req}, */
+	{WID_FIRMWARE_INFO, g_mac.firmware_version},
+	{WID_IP_ADDRESS, g_mac.ip_address},
+	{WID_NIL, NULL}
+};
+
+/********************************************
+ *
+ *      Configuration Functions
+ *
+ ********************************************/
+
+static int wilc_wlan_cfg_set_byte(uint8_t *frame, uint32_t offset, uint16_t id, uint8_t val8)
+{
+	uint8_t *buf;
+
+	if ((offset + 4) >= MAX_CFG_FRAME_SIZE)
+		return 0;
+
+	buf = &frame[offset];
+
+	buf[0] = (uint8_t)id;
+	buf[1] = (uint8_t)(id >> 8);
+	buf[2] = 1;
+	buf[3] = val8;
+	return 4;
+}
+
+static int wilc_wlan_cfg_set_hword(uint8_t *frame, uint32_t offset, uint16_t id, uint16_t val16)
+{
+	uint8_t *buf;
+
+	if ((offset + 5) >= MAX_CFG_FRAME_SIZE)
+		return 0;
+
+	buf = &frame[offset];
+
+	buf[0] = (uint8_t)id;
+	buf[1] = (uint8_t)(id >> 8);
+	buf[2] = 2;
+	buf[3] = (uint8_t)val16;
+	buf[4] = (uint8_t)(val16 >> 8);
+
+	return 5;
+}
+
+static int wilc_wlan_cfg_set_word(uint8_t *frame, uint32_t offset, uint16_t id, uint32_t val32)
+{
+	uint8_t *buf;
+
+	if ((offset + 7) >= MAX_CFG_FRAME_SIZE)
+		return 0;
+
+	buf = &frame[offset];
+
+	buf[0] = (uint8_t)id;
+	buf[1] = (uint8_t)(id >> 8);
+	buf[2] = 4;
+	buf[3] = (uint8_t)val32;
+	buf[4] = (uint8_t)(val32 >> 8);
+	buf[5] = (uint8_t)(val32 >> 16);
+	buf[6] = (uint8_t)(val32 >> 24);
+
+	return 7;
+}
+
+static int wilc_wlan_cfg_set_str(uint8_t *frame, uint32_t offset, uint16_t id, uint8_t *str, uint32_t size)
+{
+	uint8_t *buf;
+
+	if ((offset + size + 3) >= MAX_CFG_FRAME_SIZE)
+		return 0;
+
+	buf = &frame[offset];
+
+	buf[0] = (uint8_t)id;
+	buf[1] = (uint8_t)(id >> 8);
+	buf[2] = (uint8_t)size;
+
+	if ((str != NULL) && (size != 0))
+		memcpy(&buf[3], str, size);
+
+	return (size + 3);
+}
+
+static int wilc_wlan_cfg_set_bin(uint8_t *frame, uint32_t offset, uint16_t id, uint8_t *b, uint32_t size)
+{
+	uint8_t *buf;
+	uint32_t i;
+	uint8_t checksum = 0;
+
+	if ((offset + size + 5) >= MAX_CFG_FRAME_SIZE)
+		return 0;
+
+	buf = &frame[offset];
+	buf[0] = (uint8_t)id;
+	buf[1] = (uint8_t)(id >> 8);
+	buf[2] = (uint8_t)size;
+	buf[3] = (uint8_t)(size >> 8);
+
+	if ((b != NULL) && (size != 0)) {
+		memcpy(&buf[4], b, size);
+		for (i = 0; i < size; i++) {
+			checksum += buf[i + 4];
+		}
+	}
+
+	buf[size + 4] = checksum;
+
+	return (size + 5);
+}
+
+/********************************************
+ *
+ *      Configuration Response Functions
+ *
+ ********************************************/
+
+static void wilc_wlan_parse_response_frame(uint8_t *info, int size)
+{
+	uint32_t wid, len = 0, i = 0;
+	static int seq;
+
+	while (size > 0) {
+		i = 0;
+		wid = info[0] | (info[1] << 8);
+#ifdef BIG_ENDIAN
+		wid = BYTE_SWAP(wid);
+#endif
+		PRINT_INFO(GENERIC_DBG, "Processing response for %d seq %d\n", wid, seq++);
+		switch ((wid >> 12) & 0x7) {
+		case WID_CHAR:
+			do {
+				if (g_cfg_byte[i].id == WID_NIL)
+					break;
+
+				if (g_cfg_byte[i].id == wid) {
+					g_cfg_byte[i].val = info[3];
+					break;
+				}
+				i++;
+			} while (1);
+			len = 2;
+			break;
+
+		case WID_SHORT:
+			do {
+				if (g_cfg_hword[i].id == WID_NIL)
+					break;
+
+				if (g_cfg_hword[i].id == wid) {
+#ifdef BIG_ENDIAN
+					g_cfg_hword[i].val = (info[3] << 8) | (info[4]);
+#else
+					g_cfg_hword[i].val = info[3] | (info[4] << 8);
+#endif
+					break;
+				}
+				i++;
+			} while (1);
+			len = 3;
+			break;
+
+		case WID_INT:
+			do {
+				if (g_cfg_word[i].id == WID_NIL)
+					break;
+
+				if (g_cfg_word[i].id == wid) {
+#ifdef BIG_ENDIAN
+					g_cfg_word[i].val = (info[3] << 24) | (info[4] << 16) | (info[5] << 8) | (info[6]);
+#else
+					g_cfg_word[i].val = info[3] | (info[4] << 8) | (info[5] << 16) | (info[6] << 24);
+#endif
+					break;
+				}
+				i++;
+			} while (1);
+			len = 5;
+			break;
+
+		case WID_STR:
+			do {
+				if (g_cfg_str[i].id == WID_NIL)
+					break;
+
+				if (g_cfg_str[i].id == wid) {
+					if (wid == WID_SITE_SURVEY_RESULTS) {
+						static int toggle;
+						PRINT_INFO(GENERIC_DBG, "Site survey results received[%d]\n",
+							   size);
+
+						PRINT_INFO(GENERIC_DBG, "Site survey results value[%d]toggle[%d]\n", size, toggle);
+						i += toggle;
+						toggle ^= 1;
+					}
+					memcpy(g_cfg_str[i].str, &info[2], (info[2] + 1));
+					break;
+				}
+				i++;
+			} while (1);
+			len = 1 + info[2];
+			break;
+
+		default:
+			break;
+		}
+		size -= (2 + len);
+		info += (2 + len);
+	}
+
+	return;
+}
+
+static int wilc_wlan_parse_info_frame(uint8_t *info, int size)
+{
+	wilc_mac_cfg_t *pd = (wilc_mac_cfg_t *)&g_mac;
+	uint32_t wid, len;
+	int type = WILC_CFG_RSP_STATUS;
+
+	wid = info[0] | (info[1] << 8);
+
+	len = info[2];
+	PRINT_INFO(GENERIC_DBG, "Status Len = %d Id= %d\n", len, wid);
+	if ((len == 1) && (wid == WID_STATUS)) {
+		pd->mac_status = info[3];
+		type = WILC_CFG_RSP_STATUS;
+	}
+
+	return type;
+}
+
+/********************************************
+ *
+ *      Configuration Exported Functions
+ *
+ ********************************************/
+
+static int wilc_wlan_cfg_set_wid(uint8_t *frame, uint32_t offset, uint16_t id, uint8_t *buf, int size)
+{
+	uint8_t type = (id >> 12) & 0xf;
+	int ret = 0;
+
+	if (type == 0) {                                        /* byte command */
+		if (size >= 1)
+			ret = wilc_wlan_cfg_set_byte(frame, offset, id, *buf);
+	} else if (type == 1) {                 /* half word command */
+		if (size >= 2)
+			ret = wilc_wlan_cfg_set_hword(frame, offset, id, *((uint16_t *)buf));
+	} else if (type == 2) {                 /* word command */
+		if (size >= 4)
+			ret = wilc_wlan_cfg_set_word(frame, offset, id, *((uint32_t *)buf));
+	} else if (type == 3) {                 /* string command */
+		ret = wilc_wlan_cfg_set_str(frame, offset, id, buf, size);
+	} else if (type == 4) {                 /* binary command */
+		ret = wilc_wlan_cfg_set_bin(frame, offset, id, buf, size);
+	} else {
+		g_mac.dPrint(N_ERR, "illegal id\n");
+	}
+
+	return ret;
+}
+
+static int wilc_wlan_cfg_get_wid(uint8_t *frame, uint32_t offset, uint16_t id)
+{
+	uint8_t *buf;
+
+	if ((offset + 2) >= MAX_CFG_FRAME_SIZE)
+		return 0;
+
+	buf = &frame[offset];
+
+	buf[0] = (uint8_t)id;
+	buf[1] = (uint8_t)(id >> 8);
+
+	return 2;
+}
+
+static int wilc_wlan_cfg_get_wid_value(uint16_t wid, uint8_t *buffer, uint32_t buffer_size)
+{
+	uint32_t type = (wid >> 12) & 0xf;
+	int i, ret = 0;
+
+	if (wid == WID_STATUS) {
+		*((uint32_t *)buffer) = g_mac.mac_status;
+		return 4;
+	}
+
+	i = 0;
+	if (type == 0) {                                        /* byte command */
+		do {
+			if (g_cfg_byte[i].id == WID_NIL)
+				break;
+
+			if (g_cfg_byte[i].id == wid) {
+				memcpy(buffer,  &g_cfg_byte[i].val, 1);
+				ret = 1;
+				break;
+			}
+			i++;
+		} while (1);
+	} else if (type == 1) {                 /* half word command */
+		do {
+			if (g_cfg_hword[i].id == WID_NIL)
+				break;
+
+			if (g_cfg_hword[i].id == wid) {
+				memcpy(buffer,  &g_cfg_hword[i].val, 2);
+				ret = 2;
+				break;
+			}
+			i++;
+		} while (1);
+	} else if (type == 2) {                 /* word command */
+		do {
+			if (g_cfg_word[i].id == WID_NIL)
+				break;
+
+			if (g_cfg_word[i].id == wid) {
+				memcpy(buffer,  &g_cfg_word[i].val, 4);
+				ret = 4;
+				break;
+			}
+			i++;
+		} while (1);
+	} else if (type == 3) {                 /* string command */
+		do {
+			if (g_cfg_str[i].id == WID_NIL)
+				break;
+
+			if (g_cfg_str[i].id == wid) {
+				uint32_t size =  g_cfg_str[i].str[0];
+				if (buffer_size >= size) {
+					if (g_cfg_str[i].id == WID_SITE_SURVEY_RESULTS)	{
+						static int toggle;
+						PRINT_INFO(GENERIC_DBG, "Site survey results value[%d]\n",
+							   size);
+						i += toggle;
+						toggle ^= 1;
+
+					}
+					memcpy(buffer,  &g_cfg_str[i].str[1], size);
+					ret = size;
+				}
+				break;
+			}
+			i++;
+		} while (1);
+	} else {
+		g_mac.dPrint(N_ERR, "[CFG]: illegal type (%08x)\n", wid);
+	}
+
+	return ret;
+}
+
+static int wilc_wlan_cfg_indicate_rx(uint8_t *frame, int size, wilc_cfg_rsp_t *rsp)
+{
+	int ret = 1;
+	uint8_t msg_type;
+	uint8_t msg_id;
+	uint16_t msg_len;
+	#ifdef WILC_FULLY_HOSTING_AP
+	u32 *ptru32Frame;
+	bool bStatus = frame[2];
+
+	#ifdef BIG_ENDIAN
+	ptru32Frame = (frame[4] << 24) | (frame[5] << 16) | (frame[6] << 8) | frame[7];
+	#else
+	ptru32Frame = (frame[7] << 24) | (frame[6] << 16) | (frame[5] << 8) | frame[4];
+	#endif  /* BIG_ENDIAN */
+
+	#endif  /* WILC_FULLY_HOSTING_AP */
+
+	msg_type = frame[0];
+	msg_id = frame[1];      /* seq no */
+#ifdef BIG_ENDIAN
+	msg_len = (frame[2] << 8) | frame[3];
+#else
+	msg_len = (frame[3] << 8) | frame[2];
+#endif
+	frame += 4;
+	size -= 4;
+
+	/**
+	 *      The  valid types of response messages are 'R' (Response), 'I' (Information), and 'N' (Network Information)
+	 **/
+
+	switch (msg_type) {
+	case 'R':
+		wilc_wlan_parse_response_frame(frame, size);
+		rsp->type = WILC_CFG_RSP;
+		rsp->seq_no = msg_id;
+		break;
+
+	case 'I':
+		rsp->type = wilc_wlan_parse_info_frame(frame, size);
+		rsp->seq_no = msg_id;
+		/*call host interface info parse as well*/
+		PRINT_INFO(RX_DBG, "Info message received\n");
+		GnrlAsyncInfoReceived(frame - 4, size + 4);
+		break;
+
+	case 'L':
+#ifndef SWITCH_LOG_TERMINAL
+		PRINT_ER("Unexpected firmware log message received \n");
+#else
+		PRINT_D(FIRM_DBG, "\nFIRMWARE LOGS :\n<<\n%s\n>>\n", frame);
+		break;
+
+#endif
+#if 1
+	case 'N':
+		NetworkInfoReceived(frame - 4, size + 4);
+		rsp->type = 0;
+		break;
+
+#endif
+/*bug3819:*/
+	case 'S':
+		PRINT_INFO(RX_DBG, "Scan Notification Received \n");
+		host_int_ScanCompleteReceived(frame - 4, size + 4);
+		break;
+
+#ifdef WILC_FULLY_HOSTING_AP
+	case 'T':
+		PRINT_INFO(RX_DBG, "TBTT Notification Received \n");
+		process_tbtt_isr();
+		break;
+
+	case 'A':
+		PRINT_INFO(RX_DBG, "HOSTAPD ACK Notification Received \n");
+		WILC_mgm_HOSTAPD_ACK(ptru32Frame, bStatus);
+		break;
+#endif
+
+	default:
+		PRINT_INFO(RX_DBG, "Receive unknown message type[%d-%d-%d-%d-%d-%d-%d-%d]\n",
+			   frame[0], frame[1], frame[2], frame[3], frame[4],
+			   frame[5], frame[6], frame[7]);
+		rsp->type = 0;
+		rsp->seq_no = msg_id;
+		ret = 0;
+		break;
+	}
+
+	return ret;
+}
+
+static int wilc_wlan_cfg_init(wilc_debug_func func)
+{
+	memset((void *)&g_mac, 0, sizeof(wilc_mac_cfg_t));
+	g_mac.dPrint = func;
+	return 1;
+}
+
+wilc_cfg_func_t mac_cfg = {
+	wilc_wlan_cfg_set_wid,
+	wilc_wlan_cfg_get_wid,
+	wilc_wlan_cfg_get_wid_value,
+	wilc_wlan_cfg_indicate_rx,
+	wilc_wlan_cfg_init,
+};
diff --git a/drivers/staging/wilc1000/wilc_wlan_cfg.h b/drivers/staging/wilc1000/wilc_wlan_cfg.h
new file mode 100644
index 000000000000..8906611b2930
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wlan_cfg.h
@@ -0,0 +1,33 @@
+/* ////////////////////////////////////////////////////////////////////////// */
+/*  */
+/* Copyright (c) Atmel Corporation.  All rights reserved. */
+/*  */
+/* Module Name:  wilc_wlan_cfg.h */
+/*  */
+/*  */
+/* ///////////////////////////////////////////////////////////////////////// */
+
+#ifndef WILC_WLAN_CFG_H
+#define WILC_WLAN_CFG_H
+
+typedef struct {
+	uint16_t id;
+	uint16_t val;
+} wilc_cfg_byte_t;
+
+typedef struct {
+	uint16_t id;
+	uint16_t val;
+} wilc_cfg_hword_t;
+
+typedef struct {
+	uint32_t id;
+	uint32_t val;
+} wilc_cfg_word_t;
+
+typedef struct {
+	uint32_t id;
+	uint8_t *str;
+} wilc_cfg_str_t;
+
+#endif
diff --git a/drivers/staging/wilc1000/wilc_wlan_if.h b/drivers/staging/wilc1000/wilc_wlan_if.h
new file mode 100644
index 000000000000..8ed51e385118
--- /dev/null
+++ b/drivers/staging/wilc1000/wilc_wlan_if.h
@@ -0,0 +1,969 @@
+/* ////////////////////////////////////////////////////////////////////////// */
+/*  */
+/* Copyright (c) Atmel Corporation.  All rights reserved. */
+/*  */
+/* Module Name:  wilc_wlan_if.h */
+/*  */
+/*  */
+/* ///////////////////////////////////////////////////////////////////////// */
+
+
+#ifndef WILC_WLAN_IF_H
+#define WILC_WLAN_IF_H
+
+/*bug 3887: [AP] Allow Management frames to be passed to the host*/
+#define WILC_AP_EXTERNAL_MLME
+#define WILC_P2P
+#define TCP_ENHANCEMENTS
+/* #define MEMORY_STATIC */
+/* #define WILC_FULLY_HOSTING_AP */
+/* #define USE_OLD_SPI_SW */
+
+
+#include "wilc_type.h"
+#include "linux_wlan_common.h"
+
+
+/********************************************
+ *
+ *      Debug Flags
+ *
+ ********************************************/
+
+#define N_INIT		0x00000001
+#define N_ERR		0x00000002
+#define N_TXQ		0x00000004
+#define N_INTR		0x00000008
+#define N_RXQ		0x00000010
+
+/********************************************
+ *
+ *      Host Interface Defines
+ *
+ ********************************************/
+
+#define HIF_SDIO           (0)
+#define HIF_SPI            (1 << 0)
+#define HIF_SDIO_GPIO_IRQ  (1 << 2)
+
+
+/********************************************
+ *
+ *      Tx/Rx Buffer Size Defines
+ *
+ ********************************************/
+
+#define CE_TX_BUFFER_SIZE (64 * 1024)
+#define CE_RX_BUFFER_SIZE (384 * 1024)
+
+/********************************************
+ *
+ *      Wlan Interface Defines
+ *
+ ********************************************/
+
+typedef struct {
+	uint32_t read_write: 1;
+	uint32_t function: 3;
+	uint32_t raw: 1;
+	uint32_t address: 17;
+	uint32_t data: 8;
+} sdio_cmd52_t;
+
+typedef struct {
+	/* struct { */
+	uint32_t read_write: 1;
+	uint32_t function: 3;
+	uint32_t block_mode: 1;
+	uint32_t increment: 1;
+	uint32_t address: 17;
+	uint32_t count: 9;
+	/* } bit; */
+	uint8_t *buffer;
+	uint32_t block_size;
+} sdio_cmd53_t;
+
+typedef struct {
+	void (*os_sleep)(uint32_t);
+	void (*os_atomic_sleep)(uint32_t);
+	void (*os_debug)(uint8_t *);
+	void *(*os_malloc)(uint32_t);
+	void *(*os_malloc_atomic)(uint32_t);
+	void (*os_free)(void *);
+	void (*os_lock)(void *);
+	void (*os_unlock)(void *);
+	int (*os_wait)(void *, u32);
+	void (*os_signal)(void *);
+	void (*os_enter_cs)(void *);
+	void (*os_leave_cs)(void *);
+
+	/*Added by Amr - BugID_4720*/
+	void (*os_spin_lock)(void *, unsigned long *);
+	void (*os_spin_unlock)(void *, unsigned long *);
+
+} wilc_wlan_os_func_t;
+
+typedef struct {
+	int io_type;
+	int (*io_init)(void *);
+	void (*io_deinit)(void *);
+	union {
+		struct {
+			int (*sdio_cmd52)(sdio_cmd52_t *);
+			int (*sdio_cmd53)(sdio_cmd53_t *);
+			int (*sdio_set_max_speed)(void);
+			int (*sdio_set_default_speed)(void);
+		} sdio;
+		struct {
+			int (*spi_max_speed)(void);
+			int (*spi_tx)(uint8_t *, uint32_t);
+			int (*spi_rx)(uint8_t *, uint32_t);
+			int (*spi_trx)(uint8_t *, uint8_t *, uint32_t);
+		} spi;
+	} u;
+} wilc_wlan_io_func_t;
+
+typedef struct {
+	void (*rx_indicate)(uint8_t *, uint32_t, uint32_t);
+	void (*rx_complete)(void);
+} wilc_wlan_net_func_t;
+
+typedef struct {
+	void (*mac_indicate)(int);
+} wilc_wlan_indicate_func_t;
+#define WILC_MAC_INDICATE_STATUS		0x1
+#define WILC_MAC_STATUS_INIT	-1
+#define WILC_MAC_STATUS_READY 0
+#define WILC_MAC_STATUS_CONNECT 1
+
+#define WILC_MAC_INDICATE_SCAN		0x2
+
+typedef struct {
+	void *os_private;
+
+	void *hif_critical_section;
+
+	uint32_t tx_buffer_size;
+	void *txq_critical_section;
+
+	/*Added by Amr - BugID_4720*/
+	void *txq_add_to_head_critical_section;
+	void *txq_spin_lock;
+
+	void *txq_wait_event;
+
+#if defined(MEMORY_STATIC)
+	uint32_t rx_buffer_size;
+#endif
+	void *rxq_critical_section;
+	void *rxq_wait_event;
+
+	void *cfg_wait_event;
+} wilc_wlan_os_context_t;
+
+typedef struct {
+	wilc_wlan_os_context_t os_context;
+	wilc_wlan_os_func_t os_func;
+	wilc_wlan_io_func_t io_func;
+	wilc_wlan_net_func_t net_func;
+	wilc_wlan_indicate_func_t indicate_func;
+} wilc_wlan_inp_t;
+
+struct tx_complete_data {
+	#ifdef WILC_FULLY_HOSTING_AP
+	struct tx_complete_data *next;
+	#endif
+	int size;
+	void *buff;
+	uint8_t *pBssid;
+	struct sk_buff *skb;
+};
+
+
+typedef void (*wilc_tx_complete_func_t)(void *, int);
+
+#define WILC_TX_ERR_NO_BUF (-2)
+
+typedef struct {
+	int (*wlan_firmware_download)(const uint8_t *, uint32_t);
+	int (*wlan_start)(void);
+	int (*wlan_stop)(void);
+	int (*wlan_add_to_tx_que)(void *, uint8_t *, uint32_t, wilc_tx_complete_func_t);
+	int (*wlan_handle_tx_que)(uint32_t *);
+	void (*wlan_handle_rx_que)(void);
+	void (*wlan_handle_rx_isr)(void);
+	void (*wlan_cleanup)(void);
+	int (*wlan_cfg_set)(int, uint32_t, uint8_t *, uint32_t, int, uint32_t);
+	int (*wlan_cfg_get)(int, uint32_t, int, uint32_t);
+	int (*wlan_cfg_get_value)(uint32_t, uint8_t *, uint32_t);
+	/*Bug3959: transmitting mgmt frames received from host*/
+	#if defined(WILC_AP_EXTERNAL_MLME) || defined(WILC_P2P)
+	int (*wlan_add_mgmt_to_tx_que)(void *, uint8_t *, uint32_t, wilc_tx_complete_func_t);
+
+	#ifdef WILC_FULLY_HOSTING_AP
+	int (*wlan_add_data_to_tx_que)(void *, uint8_t *, uint32_t, wilc_tx_complete_func_t);
+	#endif
+
+	#endif
+} wilc_wlan_oup_t;
+
+/********************************************
+ *
+ *      Wlan Configuration ID
+ *
+ ********************************************/
+
+#define MAX_SSID_LEN            33
+#define MAX_RATES_SUPPORTED     12
+
+#define INFINITE_SLEEP_TIME		((u32)0xFFFFFFFF)
+
+#ifdef WILC_PARSE_SCAN_IN_HOST
+typedef enum {
+	SUPP_RATES_IE = 1,
+	EXT_SUPP_RATES_IE = 50,
+	HT_CAPABILITY_IE = 45,
+	RSN_IE = 48,
+	WPA_IE = 221,
+	WMM_IE = 221,
+	#ifdef WILC_P2P
+	P2P_IE = 221,
+	#endif
+} BEACON_IE;
+#endif
+typedef enum {
+	INFRASTRUCTURE = 0,
+	INDEPENDENT,
+	AP,
+} BSSTYPE_T;
+
+typedef enum {
+	RATE_AUTO = 0,
+	RATE_1MB = 1,
+	RATE_2MB = 2,
+	RATE_5MB = 5,
+	RATE_6MB = 6,
+	RATE_9MB = 9,
+	RATE_11MB = 11,
+	RATE_12MB = 12,
+	RATE_18MB = 18,
+	RATE_24MB = 24,
+	RATE_26MB = 36,
+	RATE_48MB = 48,
+	RATE_54MB = 54
+} TX_RATE_T;
+
+typedef enum {
+	B_ONLY_MODE = 0,                                /* basic rate: 1, 2 Mbps, otherwise: 5, 11 Mbps */
+	G_ONLY_MODE,                                    /* basic rate: 6, 12, 24 Mbps, otherwise: 9, 18, 36, 48, 54 Mbps */
+	G_MIXED_11B_1_MODE,             /* basic rate: 1, 2, 5.5, 11 Mbps, otherwise: all on */
+	G_MIXED_11B_2_MODE,             /* basic rate: 1, 2, 5, 11, 6, 12, 24 Mbps, otherwise: all on */
+} G_OPERATING_MODE_T;
+
+typedef enum {
+	G_SHORT_PREAMBLE = 0,   /* Short Preamble          */
+	G_LONG_PREAMBLE  = 1,           /* Long Preamble           */
+	G_AUTO_PREAMBLE  = 2,           /* Auto Preamble Selection */
+} G_PREAMBLE_T;
+
+#define MAC_CONNECTED    1
+#define MAC_DISCONNECTED 0
+
+/*bug3819: */
+#define SCAN_DONE		TRUE
+typedef enum {
+	PASSIVE_SCAN = 0,
+	ACTIVE_SCAN  = 1,
+} SCANTYPE_T;
+
+typedef enum {
+	NO_POWERSAVE     = 0,
+	MIN_FAST_PS      = 1,
+	MAX_FAST_PS      = 2,
+	MIN_PSPOLL_PS    = 3,
+	MAX_PSPOLL_PS    = 4
+} USER_PS_MODE_T;
+
+typedef enum {
+	CHIP_WAKEDUP			= 0,
+	CHIP_SLEEPING_AUTO      = 1,
+	CHIP_SLEEPING_MANUAL  = 2
+} CHIP_PS_STATE_T;
+
+typedef enum {
+	ACQUIRE_ONLY                             = 0,
+	ACQUIRE_AND_WAKEUP	= 1,
+} BUS_ACQUIRE_T;
+
+typedef enum {
+	RELEASE_ONLY				= 0,
+	RELEASE_ALLOW_SLEEP		= 1,
+} BUS_RELEASE_T;
+
+typedef enum {
+	NO_SECURITY = 0,
+	WEP_40 = 0x3,
+	WEP_104 = 0x7,
+	WPA_AES = 0x29,
+	WPA_TKIP = 0x49,
+	WPA_AES_TKIP = 0x69,            /* Aes or Tkip */
+	WPA2_AES = 0x31,
+	WPA2_TKIP = 0x51,
+	WPA2_AES_TKIP = 0x71,   /* Aes or Tkip */
+} SECURITY_T;
+
+typedef enum {
+	OPEN_SYSTEM     = 1,
+	SHARED_KEY      = 2,
+	ANY				= 3,
+	IEEE8021 = 5
+} AUTHTYPE_T;
+
+typedef enum {
+	SITE_SURVEY_1CH    = 0,
+	SITE_SURVEY_ALL_CH = 1,
+	SITE_SURVEY_OFF    = 2
+} SITE_SURVEY_T;
+
+typedef enum {
+	NORMAL_ACK = 0,
+	NO_ACK,
+} ACK_POLICY_T;
+
+typedef enum {
+	DONT_RESET = 0,
+	DO_RESET   = 1,
+	NO_REQUEST = 2,
+} RESET_REQ_T;
+
+typedef enum {
+	REKEY_DISABLE = 1,
+	REKEY_TIME_BASE,
+	REKEY_PKT_BASE,
+	REKEY_TIME_PKT_BASE
+} RSNA_REKEY_POLICY_T;
+
+typedef enum {
+	FILTER_NO       = 0x00,
+	FILTER_AP_ONLY  = 0x01,
+	FILTER_STA_ONLY = 0x02
+} SCAN_CLASS_FITLER_T;
+
+typedef enum {
+	PRI_HIGH_RSSI    = 0x00,
+	PRI_LOW_RSSI     = 0x04,
+	PRI_DETECT       = 0x08
+} SCAN_PRI_T;
+
+typedef enum {
+	CH_FILTER_OFF    = 0x00,
+	CH_FILTER_ON     = 0x10
+} CH_FILTER_T;
+
+typedef enum {
+	AUTO_PROT = 0,  /* Auto */
+	NO_PROT,                        /* Do not use any protection       */
+	ERP_PROT,                       /* Protect all ERP frame exchanges */
+	HT_PROT,                        /* Protect all HT frame exchanges  */
+	GF_PROT,                        /* Protect all GF frame exchanges  */
+} N_PROTECTION_MODE_T;
+
+typedef enum {
+	G_SELF_CTS_PROT,
+	G_RTS_CTS_PROT,
+} G_PROTECTION_MODE_T;
+
+typedef enum {
+	HT_MIXED_MODE = 1,
+	HT_ONLY_20MHZ_MODE,
+	HT_ONLY_20_40MHZ_MODE,
+} N_OPERATING_MODE_T;
+
+typedef enum {
+	NO_DETECT             = 0,
+	DETECT_ONLY           = 1,
+	DETECT_PROTECT        = 2,
+	DETECT_PROTECT_REPORT = 3,
+} N_OBSS_DETECTION_T;
+
+typedef enum {
+	RTS_CTS_NONHT_PROT = 0,                 /* RTS-CTS at non-HT rate      */
+	FIRST_FRAME_NONHT_PROT,         /* First frame at non-HT rate  */
+	LSIG_TXOP_PROT,                                 /* LSIG TXOP Protection        */
+	FIRST_FRAME_MIXED_PROT,         /* First frame at Mixed format */
+} N_PROTECTION_TYPE_T;
+
+typedef enum {
+	STATIC_MODE   = 1,
+	DYNAMIC_MODE  = 2,
+	MIMO_MODE     = 3,              /* power save disable */
+} N_SMPS_MODE_T;
+
+typedef enum {
+	DISABLE_SELF_CTS,
+	ENABLE_SELF_CTS,
+	DISABLE_TX_ABORT,
+	ENABLE_TX_ABORT,
+	HW_TRIGGER_ABORT,
+	SW_TRIGGER_ABORT,
+} TX_ABORT_OPTION_T;
+
+typedef enum {
+	WID_CHAR     = 0,
+	WID_SHORT    = 1,
+	WID_INT      = 2,
+	WID_STR      = 3,
+	WID_BIN_DATA = 4,
+	WID_BIN   = 5,
+	WID_IP    = 6,
+	WID_ADR   = 7,
+	WID_UNDEF = 8,
+	WID_TYPE_FORCE_32BIT  = 0xFFFFFFFF
+
+} WID_TYPE_T, tenuWIDtype;
+
+typedef enum {
+	WID_NIL                            = 0xffff,
+
+
+	/*  BSS Type                                                                                                                                                                            */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration :  Infrastructure    Independent   Access Point                                                                               */
+	/*  Values to set :         0               1            2                                                                                                                      */
+	/*  --------------------------------------------------------------      */
+	WID_BSS_TYPE						= 0x0000,
+
+	/*  Transmit Rate                                                                                                                                                                       */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration :  1  2  5.5  11  6  9  12  18  24  36  48  54                                                                                */
+	/*  Values to set :  1  2  5  11  6  9  12  18  24  36  48  54                                                                                  */
+	/*  --------------------------------------------------------------      */
+	WID_CURRENT_TX_RATE			= 0x0001,
+
+	/*  Channel                                                                                                                                                                                                     */
+	/*  -------------------------------------------------------------------         */
+	/*  Configuration(g) :  1  2  3  4  5  6  7  8   9   10  11  12  13  14                                                                                         */
+	/*  Values to set    :  1  2  3  4  5  6  7  8   9   10  11  12  13  14                                                                                         */
+	/*  --------------------------------------------------------------------        */
+	WID_CURRENT_CHANNEL			= 0x0002,
+
+	/*  Preamble                                                                                                                                                                            */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration :    short    long   Auto                                                                                                                             */
+	/*  Values to set :       0         1         2                                                                                                                                 */
+	/*  --------------------------------------------------------------      */
+	WID_PREAMBLE						= 0x0003,
+
+	/*  11g operating mode (ignored if 11g not present)                                                                                                     */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration :   HighPerf  Compat(RSet #1) Compat(RSet #2)                                                                 */
+	/*  Values to set :          1               2               3                                                                                                                  */
+	/*  --------------------------------------------------------------      */
+	WID_11G_OPERATING_MODE            = 0x0004,
+
+	/*  Mac status (response only)                                                                                                                                                                  */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration :   disconnect  connect                                                                                                                                               */
+	/*  Values to get :          0               1                                                                                                                                          */
+	/*  --------------------------------------------------------------      */
+	WID_STATUS						= 0x0005,
+
+	/*  Scan type                                                                                                                                                                           */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration :   Passive Scanning   Active Scanning                                                                                        */
+	/*  Values to set :                  0                 1                                                                                                                                */
+	/*  --------------------------------------------------------------      */
+	WID_SCAN_TYPE                      = 0x0007,
+
+	/*  Key Id (WEP default key Id)                                                                                                                                                 */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration :   Any value between 0 to 3                                                                                                                  */
+	/*  Values to set :	Same value. Default is 0                                                                                                                                */
+	/*  --------------------------------------------------------------      */
+	WID_KEY_ID                         = 0x0009,
+
+	/*  QoS Enable                                                                                                                                                                          */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration :   QoS Disable   WMM Enable                                                                                                          */
+	/*  Values to set :   0             1                                                                                                                                                   */
+	/*  --------------------------------------------------------------      */
+	WID_QOS_ENABLE                     = 0x000A,
+
+	/*  Power Management                                                                                                                                                                    */
+	/*  ------------------------------------------------------------------  */
+	/*  Configuration :   NO_POWERSAVE   MIN_POWERSAVE   MAX_POWERSAVE                                              */
+	/*  Values to set :   0              1               2                                                                                                                                  */
+	/*  ------------------------------------------------------------------   */
+	WID_POWER_MANAGEMENT               = 0x000B,
+
+	/*  WEP/802 11I Configuration                                            */
+	/*  ------------------------------------------------------------------  */
+	/*  Configuration : Disable WP40 WP104 WPA-AES WPA-TKIP RSN-AES RSN-TKIP                                */
+	/*  Values (0x)   :   00                03    07        29                      49                      31                      51                                      */
+	/*                                                                                                                                                                                                              */
+	/*  Configuration : WPA-AES+TKIP RSN-AES+TKIP                                                                                                                   */
+	/*  Values (0x)   :      69                             71                                                                                                                              */
+	/*  ------------------------------------------------------------------   */
+	WID_11I_MODE                       = 0x000C,
+
+	/*  WEP Configuration: Used in BSS STA mode only when WEP is enabled     */
+	/*  ------------------------------------------------------------------   */
+	/*  Configuration : Open System  Shared Key  Any Type  |   802.1x Auth   */
+	/*  Values (0x)   :    01             02         03    |      BIT2       */
+	/*  ------------------------------------------------------------------   */
+	WID_AUTH_TYPE                      = 0x000D,
+
+	/*  Site Survey Type                                                                                                                                                                    */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration       :  Values to set                                                                                                                                */
+	/*  Survey 1 Channel    :  0                                                                                                                                                    */
+	/*  survey all Channels :  1                                                                                                                                                    */
+	/*  Disable Site Survey :  2                                                                                                                                                    */
+	/*  --------------------------------------------------------------      */
+	WID_SITE_SURVEY                    = 0x000E,
+
+	/*  Listen Interval                                                      */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :   Any value between 1 to 255                         */
+	/*  Values to set :   Same value. Default is 3                           */
+	/*  --------------------------------------------------------------       */
+	WID_LISTEN_INTERVAL                = 0x000F,
+
+	/*  DTIM Period                                                          */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :   Any value between 1 to 255                         */
+	/*  Values to set :   Same value. Default is 3                           */
+	/*  --------------------------------------------------------------       */
+	WID_DTIM_PERIOD                    = 0x0010,
+
+	/*  ACK Policy                                                           */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :   Normal Ack            No Ack                       */
+	/*  Values to set :       0                   1                          */
+	/*  --------------------------------------------------------------       */
+	WID_ACK_POLICY                     = 0x0011,
+
+	/*  Reset MAC (Set only)                                                           */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :   Don't Reset	Reset	No Request                       */
+	/*  Values to set :       0                   1			2                          */
+	/*  --------------------------------------------------------------       */
+	WID_RESET                          = 0x0012,
+
+	/*  Broadcast SSID Option: Setting this will adhere to "" SSID element   */
+	/*  ------------------------------------------------------------------   */
+	/*  Configuration :   Enable             Disable                         */
+	/*  Values to set :   1                  0                               */
+	/*  ------------------------------------------------------------------   */
+	WID_BCAST_SSID                     = 0x0015,
+
+	/*  Disconnect (Station)                                                                                                                                                                                                                */
+	/*  ------------------------------------------------------------------  */
+	/*  Configuration :   Association ID                                                                                                                                                    */
+	/*  Values to set :   Association ID                                                                                                                                                    */
+	/*  ------------------------------------------------------------------  */
+	WID_DISCONNECT                     = 0x0016,
+
+	/*  11a Tx Power Level                                                   */
+	/*  -------------------------------------------------------------------- */
+	/*  Configuration : Sets TX Power (Higher the value greater the power)   */
+	/*  Values to set : Any value between 0 and 63 (inclusive; Default is 48)*/
+	/*  -------------------------------------------------------------------- */
+	WID_TX_POWER_LEVEL_11A             = 0x0018,
+
+	/*  Group Key Update Policy Selection                                    */
+	/*  -------------------------------------------------------------------- */
+	/*  Configuration : Disabled  timeBased  packetBased   timePacketBased   */
+	/*  Values to set :   1            2          3               4          */
+	/*  -------------------------------------------------------------------- */
+	WID_REKEY_POLICY                   = 0x0019,
+
+	/*  Allow Short Slot                                                     */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration : Disallow Short Slot      Allow Short Slot            */
+	/*              (Enable Only Long Slot) (Enable Short Slot if applicable)*/
+	/*  Values to set :    0         1                                       */
+	/*  --------------------------------------------------------------       */
+	WID_SHORT_SLOT_ALLOWED             = 0x001A,
+
+	WID_PHY_ACTIVE_REG                 = 0x001B,
+
+	/*  11b Tx Power Level                                                   */
+	/*  -------------------------------------------------------------------- */
+	/*  Configuration : Sets TX Power (Higher the value greater the power)   */
+	/*  Values to set : Any value between 0 and 63 (inclusive; Default is 48)*/
+	/*  -------------------------------------------------------------------- */
+	WID_TX_POWER_LEVEL_11B             = 0x001D,
+
+	/*  Scan Request                                                                                                                                                                                        */
+	/*  --------------------------------------------------------------------        */
+	/*  Configuration : Request default scan                                                                                                                                                                        */
+	/*  Values to set : 0																													*/
+	/*  -------------------------------------------------------------------- */
+	WID_START_SCAN_REQ                 = 0x001E,
+
+	/*  Rssi (get only)                                                                                                                                                                                     */
+	/*  --------------------------------------------------------------------        */
+	/*  Configuration :                                                                                                                                                                     */
+	/*  Values to get : Rssi value																													*/
+	/*  -------------------------------------------------------------------- */
+	WID_RSSI                           = 0x001F,
+
+	/*  Join Request                                                                                                                                                                                        */
+	/*  --------------------------------------------------------------------        */
+	/*  Configuration : Request to join                                                                                                                                                                     */
+	/*  Values to set : index of scan result																					*/
+	/*  -------------------------------------------------------------------- */
+	WID_JOIN_REQ                       = 0x0020,
+
+	WID_LINKSPEED								= 0x0026,
+
+	/*  Enable User Control of TX Power                                      */
+	/*  -------------------------------------------------------------------- */
+	/*  Configuration : Disable                  Enable                      */
+	/*  Values to set :    0                       1                         */
+	/*  -------------------------------------------------------------------- */
+	WID_USER_CONTROL_ON_TX_POWER       = 0x0027,
+
+	WID_MEMORY_ACCESS_8BIT             = 0x0029,
+
+	/*  Enable Auto RX Sensitivity feature                                                                                                                                                          */
+	/*  --------------------------------------------------------------------        */
+	/*  Configuration : Disable                  Enable                                                                                                                                     */
+	/*  Values to set :    0                       1                                                                                                                                                        */
+	/*  --------------------------------------------------------------------        */
+	WID_AUTO_RX_SENSITIVITY            = 0x0032,
+
+	/*  Receive Buffer Based Ack                                                                                                                                                                            */
+	/*  --------------------------------------------------------------------        */
+	/*  Configuration : Disable                  Enable                                                                                                                                     */
+	/*  Values to set :    0                       1                                                                                                                                                        */
+	/*  --------------------------------------------------------------------        */
+	WID_DATAFLOW_CONTROL               = 0x0033,
+
+	/*  Scan Filter                                                                                                                                                                                 */
+	/*  --------------------------------------------------------------------        */
+	/*  Configuration : Class		No filter       AP only			Station Only                                                                            */
+	/*  Values to set :                                     0                     1                     2                                                                                           */
+	/*  Configuration : Priority    High Rssi       Low Rssi		Detect													*/
+	/*  Values to set :                                     0                  0x4                  0x08                                                                                    */
+	/*  Configuration : Channel     filter off              filter on																	*/
+	/*  Values to set :                                     0                  0x10                                                                                                                 */
+	/*  --------------------------------------------------------------------        */
+	WID_SCAN_FILTER                    = 0x0036,
+
+	/*  Link Loss Threshold (measure in the beacon period)                                                                          */
+	/*  --------------------------------------------------------------------        */
+	/*  Configuration : Any value between 10 and 254 (Set to 255 to disable it)								*/
+	/*  Values to set : Same value. Default is 10																				*/
+	/*  --------------------------------------------------------------------        */
+	WID_LINK_LOSS_THRESHOLD            = 0x0037,
+
+	/*BugID_4978*/
+	WID_ABORT_RUNNING_SCAN = 0x003E,
+
+	/* NMAC Character WID list */
+	WID_WPS_START                      = 0x0043,
+
+	/*  Protection mode for MAC                                              */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  Auto  No protection  ERP    HT    GF                */
+	/*  Values to set :  0     1              2      3     4                 */
+	/*  --------------------------------------------------------------       */
+	WID_11N_PROT_MECH                  = 0x0080,
+
+	/*  ERP Protection type for MAC                                          */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  Self-CTS   RTS-CTS                                  */
+	/*  Values to set :  0          1                                        */
+	/*  --------------------------------------------------------------       */
+	WID_11N_ERP_PROT_TYPE              = 0x0081,
+
+	/*  HT Option Enable                                                     */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :   HT Enable          HT Disable                       */
+	/*  Values to set :   1                  0                               */
+	/*  --------------------------------------------------------------       */
+	WID_11N_ENABLE                     = 0x0082,
+
+	/*  11n Operating mode (Note that 11g operating mode will also be        */
+	/*  used in addition to this, if this is set to HT Mixed mode)           */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  HT Mixed  HT Only-20MHz   HT Only-20/40MHz          */
+	/*  Values to set :     1         2               3                         */
+	/*  --------------------------------------------------------------       */
+	WID_11N_OPERATING_MODE             = 0x0083,
+
+	/*  11n OBSS non-HT STA Detection flag                                   */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  Do not detect                                       */
+	/*  Values to set :  0                                                   */
+	/*  Configuration :  Detect, do not protect or report                    */
+	/*  Values to set :  1                                                   */
+	/*  Configuration :  Detect, protect and do not report                   */
+	/*  Values to set :  2                                                   */
+	/*  Configuration :  Detect, protect and report to other BSS             */
+	/*  Values to set :  3                                                   */
+	/*  --------------------------------------------------------------       */
+	WID_11N_OBSS_NONHT_DETECTION       = 0x0084,
+
+	/*  11n HT Protection Type                                               */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  RTS-CTS   First Frame Exchange at non-HT-rate       */
+	/*  Values to set :  0         1                                         */
+	/*  Configuration :  LSIG TXOP First Frame Exchange in Mixed Fmt         */
+	/*  Values to set :  2         3                                         */
+	/*  --------------------------------------------------------------       */
+	WID_11N_HT_PROT_TYPE               = 0x0085,
+
+	/*  11n RIFS Protection Enable Flag                                      */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  Disable    Enable                                   */
+	/*  Values to set :  0          1                                        */
+	/*  --------------------------------------------------------------       */
+	WID_11N_RIFS_PROT_ENABLE           = 0x0086,
+
+	/*  SMPS Mode                                                            */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  Static   Dynamic   MIMO (Power Save Disabled)       */
+	/*  Values to set :  1        2         3                                */
+	/*  --------------------------------------------------------------       */
+	WID_11N_SMPS_MODE                  = 0x0087,
+
+	/*  Current transmit MCS                                                 */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  MCS Index for data rate                                                                                                                    */
+	/*  Values to set :  0 to 7                                                                                                                                                     */
+	/*  --------------------------------------------------------------       */
+	WID_11N_CURRENT_TX_MCS             = 0x0088,
+
+	WID_11N_PRINT_STATS                = 0x0089,
+
+	/*  11n Short GI Enable Flag                                                                                                                                                    */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  Disable    Enable                                                                                                                                  */
+	/*  Values to set :  0          1                                                                                                                                                       */
+	/*  --------------------------------------------------------------       */
+	WID_11N_SHORT_GI_ENABLE            = 0x008D,
+
+	/*  11n RIFS Enable Flag                                                                                                                                                */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  Disable    Enable                                                                                                                                  */
+	/*  Values to set :  0          1                                                                                                                                                       */
+	/*  --------------------------------------------------------------       */
+	WID_RIFS_MODE                      = 0x0094,
+
+	/*  TX Abort Feature                                                                                                                                                    */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :  Disable Self CTS    Enable Self CTS                                                                                                        */
+	/*  Values to set :             0                                       1                                                                                                                               */
+	/*  Configuration :  Disable TX Abort    Enable TX Abort                                                                                                        */
+	/*  Values to set :             2                                       3                                                                                                                               */
+	/*  Configuration :  Enable HW TX Abort Enable SW TX Abort                                                                                              */
+	/*  Values to set :             4                                       5                                                                                                                               */
+	/*  --------------------------------------------------------------       */
+	WID_TX_ABORT_CONFIG                = 0x00A1,
+
+	WID_REG_TSSI_11B_VALUE             = 0x00A6,
+	WID_REG_TSSI_11G_VALUE             = 0x00A7,
+	WID_REG_TSSI_11N_VALUE             = 0x00A8,
+	WID_TX_CALIBRATION                 = 0x00A9,
+	WID_DSCR_TSSI_11B_VALUE            = 0x00AA,
+	WID_DSCR_TSSI_11G_VALUE            = 0x00AB,
+	WID_DSCR_TSSI_11N_VALUE            = 0x00AC,
+
+	/*  Immediate Block-Ack Support                                          */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration : Disable                  Enable                      */
+	/*  Values to set :    0                       1                         */
+	/*  --------------------------------------------------------------       */
+	WID_11N_IMMEDIATE_BA_ENABLED       = 0x00AF,
+
+	/*  TXOP Disable Flag                                                                                                                                                                   */
+	/*  --------------------------------------------------------------      */
+	/*  Configuration : Disable                  Enable                                                                                                                     */
+	/*  Values to set :    1                        0                                                                                                                               */
+	/*  --------------------------------------------------------------      */
+	WID_11N_TXOP_PROT_DISABLE          = 0x00B0,
+
+
+	WID_TX_POWER_LEVEL_11N             = 0x00B1,
+
+	/* Custom Character WID list */
+	WID_PC_TEST_MODE          = 0x00C8,
+	/*bug3819: */
+	/* SCAN Complete notification WID*/
+	WID_SCAN_COMPLETE		= 0x00C9,
+
+#ifdef WILC_AP_EXTERNAL_MLME
+	WID_DEL_BEACON					= 0x00CA,
+#endif
+
+	WID_LOGTerminal_Switch					= 0x00CD,
+	/* EMAC Short WID list */
+	/*  RTS Threshold                                                        */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :   Any value between 256 to 2347                      */
+	/*  Values to set :   Same value. Default is 2347                        */
+	/*  --------------------------------------------------------------       */
+	WID_RTS_THRESHOLD                  = 0x1000,
+
+	/*  Fragmentation Threshold                                              */
+	/*  --------------------------------------------------------------       */
+	/*  Configuration :   Any value between 256 to 2346                      */
+	/*  Values to set :   Same value. Default is 2346                        */
+	/*  --------------------------------------------------------------       */
+	WID_FRAG_THRESHOLD                 = 0x1001,
+
+	WID_SHORT_RETRY_LIMIT              = 0x1002,
+	WID_LONG_RETRY_LIMIT               = 0x1003,
+	WID_BEACON_INTERVAL                = 0x1006,
+	WID_MEMORY_ACCESS_16BIT            = 0x1008,
+	WID_RX_SENSE                       = 0x100B,
+	WID_ACTIVE_SCAN_TIME               = 0x100C,
+	WID_PASSIVE_SCAN_TIME              = 0x100D,
+
+	WID_SITE_SURVEY_SCAN_TIME          = 0x100E,
+	WID_JOIN_START_TIMEOUT             = 0x100F,
+	WID_AUTH_TIMEOUT                   = 0x1010,
+	WID_ASOC_TIMEOUT                   = 0x1011,
+	WID_11I_PROTOCOL_TIMEOUT           = 0x1012,
+	WID_EAPOL_RESPONSE_TIMEOUT         = 0x1013,
+
+	/* NMAC Short WID list */
+	WID_11N_SIG_QUAL_VAL               = 0x1085,
+	WID_CCA_THRESHOLD                  = 0x1087,
+
+	/* Custom Short WID list */
+
+	/* EMAC Integer WID list */
+	WID_FAILED_COUNT                   = 0x2000,
+	WID_RETRY_COUNT                    = 0x2001,
+	WID_MULTIPLE_RETRY_COUNT           = 0x2002,
+	WID_FRAME_DUPLICATE_COUNT          = 0x2003,
+	WID_ACK_FAILURE_COUNT              = 0x2004,
+	WID_RECEIVED_FRAGMENT_COUNT        = 0x2005,
+	WID_MCAST_RECEIVED_FRAME_COUNT     = 0x2006,
+	WID_FCS_ERROR_COUNT                = 0x2007,
+	WID_SUCCESS_FRAME_COUNT            = 0x2008,
+	WID_HUT_TX_COUNT                   = 0x200A,
+	WID_TX_FRAGMENT_COUNT              = 0x200B,
+	WID_TX_MULTICAST_FRAME_COUNT       = 0x200C,
+	WID_RTS_SUCCESS_COUNT              = 0x200D,
+	WID_RTS_FAILURE_COUNT              = 0x200E,
+	WID_WEP_UNDECRYPTABLE_COUNT        = 0x200F,
+	WID_REKEY_PERIOD                   = 0x2010,
+	WID_REKEY_PACKET_COUNT             = 0x2011,
+	WID_1X_SERV_ADDR                   = 0x2012,
+	WID_STACK_IP_ADDR                  = 0x2013,
+	WID_STACK_NETMASK_ADDR             = 0x2014,
+	WID_HW_RX_COUNT                    = 0x2015,
+	WID_MEMORY_ADDRESS                 = 0x201E,
+	WID_MEMORY_ACCESS_32BIT            = 0x201F,
+	WID_RF_REG_VAL                     = 0x2021,
+
+
+	/* NMAC Integer WID list */
+	WID_11N_PHY_ACTIVE_REG_VAL         = 0x2080,
+
+	/* Custom Integer WID list */
+	WID_GET_INACTIVE_TIME     = 0x2084,
+	WID_SET_DRV_HANDLER =		 0X2085,
+	WID_SET_OPERATION_MODE =	 0X2086,
+	/* EMAC String WID list */
+	WID_SSID                           = 0x3000,
+	WID_FIRMWARE_VERSION               = 0x3001,
+	WID_OPERATIONAL_RATE_SET           = 0x3002,
+	WID_BSSID                          = 0x3003,
+	WID_WEP_KEY_VALUE                  = 0x3004,
+	WID_11I_PSK                        = 0x3008,
+	WID_11E_P_ACTION_REQ               = 0x3009,
+	WID_1X_KEY                         = 0x300A,
+	WID_HARDWARE_VERSION               = 0x300B,
+	WID_MAC_ADDR                       = 0x300C,
+	WID_HUT_DEST_ADDR                  = 0x300D,
+	WID_PHY_VERSION                    = 0x300F,
+	WID_SUPP_USERNAME                  = 0x3010,
+	WID_SUPP_PASSWORD                  = 0x3011,
+	WID_SITE_SURVEY_RESULTS            = 0x3012,
+	WID_RX_POWER_LEVEL                 = 0x3013,
+	WID_DEL_ALL_RX_BA				= 0x3014,
+	WID_SET_STA_MAC_INACTIVE_TIME   = 0x3017,
+	WID_ADD_WEP_KEY                    = 0x3019,
+	WID_REMOVE_WEP_KEY                 = 0x301A,
+	WID_ADD_PTK                        = 0x301B,
+	WID_ADD_RX_GTK                     = 0x301C,
+	WID_ADD_TX_GTK                     = 0x301D,
+	WID_REMOVE_KEY                     = 0x301E,
+	WID_ASSOC_REQ_INFO                 = 0x301F,
+	WID_ASSOC_RES_INFO                 = 0x3020,
+	WID_MANUFACTURER                   = 0x3026, /*Added for CAPI tool */
+	WID_MODEL_NAME                                     = 0x3027, /*Added for CAPI tool */
+	WID_MODEL_NUM                      = 0x3028, /*Added for CAPI tool */
+	WID_DEVICE_NAME                                     = 0x3029, /*Added for CAPI tool */
+
+	/* NMAC String WID list */
+	WID_11N_P_ACTION_REQ               = 0x3080,
+	WID_HUT_TEST_ID                    = 0x3081,
+	WID_PMKID_INFO                     = 0x3082,
+	WID_FIRMWARE_INFO                  = 0x3083,
+	#ifdef WILC_P2P
+	WID_REGISTER_FRAME                = 0x3084,
+	#endif
+	WID_DEL_ALL_STA          = 0x3085,
+	 #ifdef WILC_P2P
+	WID_REMAIN_ON_CHAN  = 0x3996,
+	#endif
+	/*BugID_4156*/
+	WID_SSID_PROBE_REQ = 0x3997,
+	/*BugID_4124 WID to trigger modified Join Request using SSID and BSSID instead of bssListIdx (used by WID_JOIN_REQ)*/
+	WID_JOIN_REQ_EXTENDED		 = 0x3998,
+
+	/* BugID 4951: WID toset IP address in firmware */
+	WID_IP_ADDRESS					= 0x3999,
+
+
+
+	/* Custom String WID list */
+
+	/* EMAC Binary WID list */
+	WID_UAPSD_CONFIG                   = 0x4001,
+	WID_UAPSD_STATUS                   = 0x4002,
+	WID_WMM_AP_AC_PARAMS               = 0x4003,
+	WID_WMM_STA_AC_PARAMS              = 0x4004,
+	WID_NETWORK_INFO                   = 0x4005,
+	WID_STA_JOIN_INFO                  = 0x4006,
+	WID_CONNECTED_STA_LIST             = 0x4007,
+
+	/* NMAC Binary WID list */
+	WID_11N_AUTORATE_TABLE             = 0x4080,
+
+
+	/*Added here by Amr - BugID 4134*/
+	WID_SCAN_CHANNEL_LIST                      = 0x4084,
+
+	/*BugID_3746 WID to add IE to be added in next probe request*/
+	WID_INFO_ELEMENT_PROBE	 = 0x4085,
+	/*BugID_3746 WID to add IE to be added in next associate request*/
+	WID_INFO_ELEMENT_ASSOCIATE	 = 0x4086,
+	WID_ADD_STA					 = 0X4087,
+	WID_REMOVE_STA				 = 0X4088,
+	WID_EDIT_STA					 = 0X4089,
+	WID_ADD_BEACON				= 0x408a,
+
+	/* BugID 5108 */
+	WID_SETUP_MULTICAST_FILTER	= 0x408b,
+
+	/* Miscellaneous WIDs */
+	WID_ALL                            = 0x7FFE,
+	WID_MAX                            = 0xFFFF
+} WID_T;
+
+int wilc_wlan_init(wilc_wlan_inp_t *inp, wilc_wlan_oup_t *oup);
+
+void wilc_bus_set_max_speed(void);
+void wilc_bus_set_default_speed(void);
+uint32_t wilc_get_chipid(uint8_t update);
+
+
+#endif
diff --git a/drivers/staging/wlan-ng/cfg80211.c b/drivers/staging/wlan-ng/cfg80211.c
index 7c87aecf4744..342e2b30c48f 100644
--- a/drivers/staging/wlan-ng/cfg80211.c
+++ b/drivers/staging/wlan-ng/cfg80211.c
@@ -722,7 +722,7 @@ void prism2_connect_result(wlandevice_t *wlandev, u8 failed)
 void prism2_disconnected(wlandevice_t *wlandev)
 {
 	cfg80211_disconnected(wlandev->netdev, 0, NULL,
-		0, GFP_KERNEL);
+		0, false, GFP_KERNEL);
 }
 
 void prism2_roamed(wlandevice_t *wlandev)
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index bd69e8cf200f..49f2ef88929d 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -129,7 +129,7 @@ int skb_ether_to_p80211(wlandevice_t *wlandev, u32 ethconv,
 	} else {
 		/* step 1: classify ether frame, DIX or 802.3? */
 		proto = ntohs(e_hdr.type);
-		if (proto <= 1500) {
+		if (proto <= ETH_DATA_LEN) {
 			pr_debug("802.3 len: %d\n", skb->len);
 			/* codes <= 1500 reserved for 802.3 lengths */
 			/* it's 802.3, pass ether payload unchanged,  */
@@ -207,6 +207,8 @@ int skb_ether_to_p80211(wlandevice_t *wlandev, u32 ethconv,
 		/* XXXX need to pick keynum other than default? */
 
 		p80211_wep->data = kmalloc(skb->len, GFP_ATOMIC);
+		if (!p80211_wep->data)
+			return -ENOMEM;
 		foo = wep_encrypt(wlandev, skb->data, p80211_wep->data,
 				  skb->len,
 				  (wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK),
@@ -529,7 +531,7 @@ int p80211_stt_findproto(u16 proto)
 	   Need to do some testing to confirm.
 	 */
 
-	if (proto == 0x80f3)	/* APPLETALK */
+	if (proto == ETH_P_AARP)	/* APPLETALK */
 		return 1;
 
 	return 0;
diff --git a/drivers/staging/wlan-ng/p80211wep.c b/drivers/staging/wlan-ng/p80211wep.c
index c4fabadf5d74..c363456d93a3 100644
--- a/drivers/staging/wlan-ng/p80211wep.c
+++ b/drivers/staging/wlan-ng/p80211wep.c
@@ -53,7 +53,6 @@
 #include <linux/random.h>
 #include <linux/kernel.h>
 
-/* #define WEP_DEBUG	*/
 
 #include "p80211hdr.h"
 #include "p80211types.h"
@@ -133,10 +132,6 @@ int wep_change_key(wlandevice_t *wlandev, int keynum, u8 *key, int keylen)
 	if (keynum >= NUM_WEPKEYS)
 		return -1;
 
-#ifdef WEP_DEBUG
-	pr_debug("WEP key %d len %d = %*phC\n", keynum, keylen,
-			  8, key);
-#endif
 
 	wlandev->wep_keylens[keynum] = keylen;
 	memcpy(wlandev->wep_keys[keynum], key, keylen);
@@ -181,10 +176,6 @@ int wep_decrypt(wlandevice_t *wlandev, u8 *buf, u32 len, int key_override,
 
 	keylen += 3;		/* add in IV bytes */
 
-#ifdef WEP_DEBUG
-	pr_debug("D %d: %*ph (%d %d) %*phC\n", len, 3, key,
-			  keyidx, keylen, 5, key + 3);
-#endif
 
 	/* set up the RC4 state */
 	for (i = 0; i < 256; i++)
@@ -258,11 +249,6 @@ int wep_encrypt(wlandevice_t *wlandev, u8 *buf, u8 *dst, u32 len, int keynum,
 
 	keylen += 3;		/* add in IV bytes */
 
-#ifdef WEP_DEBUG
-	pr_debug("E %d (%d/%d %d) %*ph %*phC\n", len,
-			  iv[3], keynum, keylen, 3, key, 5, key + 3);
-#endif
-
 	/* set up the RC4 state */
 	for (i = 0; i < 256; i++)
 		s[i] = i;
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index 9408644cc8b8..fe36613589ae 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -708,7 +708,10 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 			continue;
 		}
 
-		/* Validate plug address against chunk data and identify chunk */
+		/*
+		 * Validate plug address against
+		 * chunk data and identify chunk
+		 */
 		for (c = 0; c < nfchunks; c++) {
 			cstart = fchunk[c].addr;
 			cend = fchunk[c].addr + fchunk[c].len;
@@ -923,7 +926,8 @@ static int read_fwfile(const struct ihex_binrec *record)
 				      rcnt,
 				      s3info[ns3info].len,
 				      s3info[ns3info].type);
-			if (((s3info[ns3info].len - 1) * sizeof(u16)) > sizeof(s3info[ns3info].info)) {
+			if (((s3info[ns3info].len - 1) * sizeof(u16)) >
+			   sizeof(s3info[ns3info].info)) {
 				pr_err("S3 inforec length too long - aborting\n");
 				return 1;
 			}
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index ddb294e7044f..0329c521d17c 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -428,7 +428,8 @@ u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate)
 			result = hfa384x_drvr_start(hw);
 			if (result) {
 				netdev_err(wlandev->netdev,
-				       "hfa384x_drvr_start() failed,result=%d\n", (int)result);
+					   "hfa384x_drvr_start() failed,result=%d\n",
+					   (int)result);
 				result =
 				 P80211ENUM_resultcode_implementation_failure;
 				wlandev->msdstate = WLAN_MSD_HWPRESENT;
@@ -471,7 +472,8 @@ u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate)
 			result = hfa384x_drvr_start(hw);
 			if (result) {
 				netdev_err(wlandev->netdev,
-				       "hfa384x_drvr_start() failed,result=%d\n", (int)result);
+					   "hfa384x_drvr_start() failed,result=%d\n",
+					   (int)result);
 				result =
 				  P80211ENUM_resultcode_implementation_failure;
 				wlandev->msdstate = WLAN_MSD_HWPRESENT;
@@ -481,7 +483,8 @@ u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate)
 			result = prism2sta_getcardinfo(wlandev);
 			if (result) {
 				netdev_err(wlandev->netdev,
-				       "prism2sta_getcardinfo() failed,result=%d\n", (int)result);
+					   "prism2sta_getcardinfo() failed,result=%d\n",
+					   (int)result);
 				result =
 				  P80211ENUM_resultcode_implementation_failure;
 				hfa384x_drvr_stop(hw);
@@ -491,7 +494,8 @@ u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate)
 			result = prism2sta_globalsetup(wlandev);
 			if (result) {
 				netdev_err(wlandev->netdev,
-				       "prism2sta_globalsetup() failed,result=%d\n", (int)result);
+					   "prism2sta_globalsetup() failed,result=%d\n",
+					   (int)result);
 				result =
 				  P80211ENUM_resultcode_implementation_failure;
 				hfa384x_drvr_stop(hw);
@@ -1244,9 +1248,9 @@ void prism2sta_processing_defer(struct work_struct *data)
 				     HFA384x_RID_CURRENTSSID, result);
 				return;
 			}
-			prism2mgmt_bytestr2pstr((struct hfa384x_bytestr *) &ssid,
-						(p80211pstrd_t *) &
-						wlandev->ssid);
+			prism2mgmt_bytestr2pstr(
+					(struct hfa384x_bytestr *) &ssid,
+					(p80211pstrd_t *) &wlandev->ssid);
 
 			/* Collect the port status */
 			result = hfa384x_drvr_getconfig16(hw,
@@ -1658,8 +1662,9 @@ static void prism2sta_inf_authreq_defer(wlandevice_t *wlandev,
 			if (hw->authlist.cnt >= WLAN_AUTH_MAX) {
 				rec.status = P80211ENUM_status_ap_full;
 			} else {
-				ether_addr_copy(hw->authlist.addr[hw->authlist.cnt],
-						rec.address);
+				ether_addr_copy(
+					hw->authlist.addr[hw->authlist.cnt],
+					rec.address);
 				hw->authlist.cnt++;
 				added = 1;
 			}
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 74e88200726c..943d463cf193 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -8,10 +8,7 @@
 
 #include <linux/sizes.h>
 #include <linux/module.h>
-
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
+#include <linux/pci.h>
 
 #include "XGI_main.h"
 #include "vb_init.h"
@@ -1770,7 +1767,7 @@ static int xgifb_probe(struct pci_dev *pdev,
 	}
 
 	xgifb_info->video_vbase = hw_info->pjVideoMemoryAddress =
-	ioremap(xgifb_info->video_base, xgifb_info->video_size);
+		ioremap_wc(xgifb_info->video_base, xgifb_info->video_size);
 	xgifb_info->mmio_vbase = ioremap(xgifb_info->mmio_base,
 					    xgifb_info->mmio_size);
 
@@ -2014,12 +2011,8 @@ static int xgifb_probe(struct pci_dev *pdev,
 
 	fb_alloc_cmap(&fb_info->cmap, 256, 0);
 
-#ifdef CONFIG_MTRR
-	xgifb_info->mtrr = mtrr_add(xgifb_info->video_base,
-		xgifb_info->video_size, MTRR_TYPE_WRCOMB, 1);
-	if (xgifb_info->mtrr >= 0)
-		dev_info(&pdev->dev, "Added MTRR\n");
-#endif
+	xgifb_info->mtrr = arch_phys_wc_add(xgifb_info->video_base,
+					    xgifb_info->video_size);
 
 	if (register_framebuffer(fb_info) < 0) {
 		ret = -EINVAL;
@@ -2031,11 +2024,7 @@ static int xgifb_probe(struct pci_dev *pdev,
 	return 0;
 
 error_mtrr:
-#ifdef CONFIG_MTRR
-	if (xgifb_info->mtrr >= 0)
-		mtrr_del(xgifb_info->mtrr, xgifb_info->video_base,
-			xgifb_info->video_size);
-#endif /* CONFIG_MTRR */
+	arch_phys_wc_del(xgifb_info->mtrr);
 error_1:
 	iounmap(xgifb_info->mmio_vbase);
 	iounmap(xgifb_info->video_vbase);
@@ -2059,11 +2048,7 @@ static void xgifb_remove(struct pci_dev *pdev)
 	struct fb_info *fb_info = xgifb_info->fb_info;
 
 	unregister_framebuffer(fb_info);
-#ifdef CONFIG_MTRR
-	if (xgifb_info->mtrr >= 0)
-		mtrr_del(xgifb_info->mtrr, xgifb_info->video_base,
-			xgifb_info->video_size);
-#endif /* CONFIG_MTRR */
+	arch_phys_wc_del(xgifb_info->mtrr);
 	iounmap(xgifb_info->mmio_vbase);
 	iounmap(xgifb_info->video_vbase);
 	release_mem_region(xgifb_info->mmio_base, xgifb_info->mmio_size);
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index a47395e92d20..c886dd2892a4 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -940,7 +940,7 @@ static void XGI_SetCRT1FIFO(struct xgi_hw_device_info *HwDeviceExtension,
 
 	data = xgifb_reg_get(pVBInfo->P3c4, 0x3D);
 	data &= 0xfe;
-	xgifb_reg_set(pVBInfo->P3c4, 0x3D, data); /* diable auto-threshold */
+	xgifb_reg_set(pVBInfo->P3c4, 0x3D, data); /* disable auto-threshold */
 
 	xgifb_reg_set(pVBInfo->P3c4, 0x08, 0x34);
 	data = xgifb_reg_get(pVBInfo->P3c4, 0x09);
@@ -1081,24 +1081,17 @@ static void XGI_WriteDAC(unsigned short dl,
 			 unsigned short dh,
 			 struct vb_device_info *pVBInfo)
 {
-	unsigned short temp, bh, bl;
+	unsigned short bh, bl;
 
 	bh = ah;
 	bl = al;
 
 	if (dl != 0) {
-		temp = bh;
-		bh = dh;
-		dh = temp;
-		if (dl == 1) {
-			temp = bl;
-			bl = dh;
-			dh = temp;
-		} else {
-			temp = bl;
-			bl = bh;
-			bh = temp;
-		}
+		swap(bh, dh);
+		if (dl == 1)
+			swap(bl, dh);
+		else
+			swap(bl, bh);
 	}
 	outb((unsigned short) dh, pVBInfo->P3c9);
 	outb((unsigned short) bh, pVBInfo->P3c9);
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 74e6114ff18f..4e68b62193ed 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -21,14 +21,14 @@
 #include <linux/crypto.h>
 #include <linux/completion.h>
 #include <linux/module.h>
+#include <linux/vmalloc.h>
 #include <linux/idr.h>
 #include <asm/unaligned.h>
-#include <scsi/scsi_device.h>
+#include <scsi/scsi_proto.h>
 #include <scsi/iscsi_proto.h>
 #include <scsi/scsi_tcq.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 
 #include <target/iscsi/iscsi_target_core.h>
 #include "iscsi_target_parameters.h"
@@ -715,7 +715,7 @@ static int iscsit_add_reject_from_cmd(
 	 */
 	if (cmd->se_cmd.se_tfo != NULL) {
 		pr_debug("iscsi reject: calling target_put_sess_cmd >>>>>>\n");
-		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+		target_put_sess_cmd(&cmd->se_cmd);
 	}
 	return -1;
 }
@@ -1001,13 +1001,15 @@ int iscsit_setup_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 		hdr->cmdsn, be32_to_cpu(hdr->data_length), payload_length,
 		conn->cid);
 
-	target_get_sess_cmd(conn->sess->se_sess, &cmd->se_cmd, true);
+	target_get_sess_cmd(&cmd->se_cmd, true);
 
 	cmd->sense_reason = transport_lookup_cmd_lun(&cmd->se_cmd,
 						     scsilun_to_int(&hdr->lun));
 	if (cmd->sense_reason)
 		goto attach_cmd;
 
+	/* only used for printks or comparing with ->ref_task_tag */
+	cmd->se_cmd.tag = (__force u32)cmd->init_task_tag;
 	cmd->sense_reason = target_setup_cmd_from_cdb(&cmd->se_cmd, hdr->cdb);
 	if (cmd->sense_reason) {
 		if (cmd->sense_reason == TCM_OUT_OF_RESOURCES) {
@@ -1067,7 +1069,7 @@ int iscsit_process_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 		if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER)
 			return -1;
 		else if (cmdsn_ret == CMDSN_LOWER_THAN_EXP) {
-			target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+			target_put_sess_cmd(&cmd->se_cmd);
 			return 0;
 		}
 	}
@@ -1083,7 +1085,7 @@ int iscsit_process_scsi_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 		if (!cmd->sense_reason)
 			return 0;
 
-		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+		target_put_sess_cmd(&cmd->se_cmd);
 		return 0;
 	}
 
@@ -1114,7 +1116,6 @@ static int
 iscsit_get_immediate_data(struct iscsi_cmd *cmd, struct iscsi_scsi_req *hdr,
 			  bool dump_payload)
 {
-	struct iscsi_conn *conn = cmd->conn;
 	int cmdsn_ret = 0, immed_ret = IMMEDIATE_DATA_NORMAL_OPERATION;
 	/*
 	 * Special case for Unsupported SAM WRITE Opcodes and ImmediateData=Yes.
@@ -1141,7 +1142,7 @@ after_immediate_data:
 
 			rc = iscsit_dump_data_payload(cmd->conn,
 						      cmd->first_burst_len, 1);
-			target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+			target_put_sess_cmd(&cmd->se_cmd);
 			return rc;
 		} else if (cmd->unsolicited_data)
 			iscsit_set_unsoliticed_dataout(cmd);
@@ -1810,7 +1811,7 @@ iscsit_handle_task_mgt_cmd(struct iscsi_conn *conn, struct iscsi_cmd *cmd,
 				      conn->sess->se_sess, 0, DMA_NONE,
 				      TCM_SIMPLE_TAG, cmd->sense_buffer + 2);
 
-		target_get_sess_cmd(conn->sess->se_sess, &cmd->se_cmd, true);
+		target_get_sess_cmd(&cmd->se_cmd, true);
 		sess_ref = true;
 
 		switch (function) {
@@ -1952,7 +1953,7 @@ attach:
 	 */
 	if (sess_ref) {
 		pr_debug("Handle TMR, using sess_ref=true check\n");
-		target_put_sess_cmd(conn->sess->se_sess, &cmd->se_cmd);
+		target_put_sess_cmd(&cmd->se_cmd);
 	}
 
 	iscsit_add_cmd_to_response_queue(cmd, conn, cmd->i_state);
@@ -2736,11 +2737,7 @@ static int iscsit_send_datain(struct iscsi_cmd *cmd, struct iscsi_conn *conn)
 	cmd->iov_data_count = iov_count;
 	cmd->tx_size = tx_size;
 
-	/* sendpage is preferred but can't insert markers */
-	if (!conn->conn_ops->IFMarker)
-		ret = iscsit_fe_sendpage_sg(cmd, conn);
-	else
-		ret = iscsit_send_tx_data(cmd, conn, 0);
+	ret = iscsit_fe_sendpage_sg(cmd, conn);
 
 	iscsit_unmap_iovec(cmd);
 
@@ -4072,17 +4069,9 @@ static int iscsi_target_rx_opcode(struct iscsi_conn *conn, unsigned char *buf)
 			" opcode while ERL=0, closing iSCSI connection.\n");
 			return -1;
 		}
-		if (!conn->conn_ops->OFMarker) {
-			pr_err("Unable to recover from unknown"
-			" opcode while OFMarker=No, closing iSCSI"
-				" connection.\n");
-			return -1;
-		}
-		if (iscsit_recover_from_unknown_opcode(conn) < 0) {
-			pr_err("Unable to recover from unknown"
-				" opcode, closing iSCSI connection.\n");
-			return -1;
-		}
+		pr_err("Unable to recover from unknown opcode while OFMarker=No,"
+		       " closing iSCSI connection.\n");
+		ret = -1;
 		break;
 	}
 
diff --git a/drivers/target/iscsi/iscsi_target_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index 469fce44ebad..c1898c84b3d2 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -24,7 +24,6 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 #include <target/iscsi/iscsi_transport.h>
 
@@ -860,57 +859,19 @@ static struct configfs_attribute *lio_target_initiator_attrs[] = {
 	NULL,
 };
 
-static struct se_node_acl *lio_tpg_alloc_fabric_acl(
-	struct se_portal_group *se_tpg)
+static int lio_target_init_nodeacl(struct se_node_acl *se_nacl,
+		const char *name)
 {
-	struct iscsi_node_acl *acl;
-
-	acl = kzalloc(sizeof(struct iscsi_node_acl), GFP_KERNEL);
-	if (!acl) {
-		pr_err("Unable to allocate memory for struct iscsi_node_acl\n");
-		return NULL;
-	}
-
-	return &acl->se_node_acl;
-}
-
-static struct se_node_acl *lio_target_make_nodeacl(
-	struct se_portal_group *se_tpg,
-	struct config_group *group,
-	const char *name)
-{
-	struct config_group *stats_cg;
-	struct iscsi_node_acl *acl;
-	struct se_node_acl *se_nacl_new, *se_nacl;
-	struct iscsi_portal_group *tpg = container_of(se_tpg,
-			struct iscsi_portal_group, tpg_se_tpg);
-	u32 cmdsn_depth;
-
-	se_nacl_new = lio_tpg_alloc_fabric_acl(se_tpg);
-	if (!se_nacl_new)
-		return ERR_PTR(-ENOMEM);
-
-	cmdsn_depth = tpg->tpg_attrib.default_cmdsn_depth;
-	/*
-	 * se_nacl_new may be released by core_tpg_add_initiator_node_acl()
-	 * when converting a NdoeACL from demo mode -> explict
-	 */
-	se_nacl = core_tpg_add_initiator_node_acl(se_tpg, se_nacl_new,
-				name, cmdsn_depth);
-	if (IS_ERR(se_nacl))
-		return se_nacl;
-
-	acl = container_of(se_nacl, struct iscsi_node_acl, se_node_acl);
-	stats_cg = &se_nacl->acl_fabric_stat_group;
+	struct iscsi_node_acl *acl =
+		container_of(se_nacl, struct iscsi_node_acl, se_node_acl);
+	struct config_group *stats_cg = &se_nacl->acl_fabric_stat_group;
 
 	stats_cg->default_groups = kmalloc(sizeof(struct config_group *) * 2,
 				GFP_KERNEL);
 	if (!stats_cg->default_groups) {
 		pr_err("Unable to allocate memory for"
 				" stats_cg->default_groups\n");
-		core_tpg_del_initiator_node_acl(se_tpg, se_nacl, 1);
-		kfree(acl);
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 	}
 
 	stats_cg->default_groups[0] = &acl->node_stat_grps.iscsi_sess_stats_group;
@@ -918,13 +879,11 @@ static struct se_node_acl *lio_target_make_nodeacl(
 	config_group_init_type_name(&acl->node_stat_grps.iscsi_sess_stats_group,
 			"iscsi_sess_stats", &iscsi_stat_sess_cit);
 
-	return se_nacl;
+	return 0;
 }
 
-static void lio_target_drop_nodeacl(
-	struct se_node_acl *se_nacl)
+static void lio_target_cleanup_nodeacl( struct se_node_acl *se_nacl)
 {
-	struct se_portal_group *se_tpg = se_nacl->se_tpg;
 	struct iscsi_node_acl *acl = container_of(se_nacl,
 			struct iscsi_node_acl, se_node_acl);
 	struct config_item *df_item;
@@ -938,9 +897,6 @@ static void lio_target_drop_nodeacl(
 		config_item_put(df_item);
 	}
 	kfree(stats_cg->default_groups);
-
-	core_tpg_del_initiator_node_acl(se_tpg, se_nacl, 1);
-	kfree(acl);
 }
 
 /* End items for lio_target_acl_cit */
@@ -1463,8 +1419,7 @@ static struct se_portal_group *lio_target_tiqn_addtpg(
 	if (!tpg)
 		return NULL;
 
-	ret = core_tpg_register(&iscsi_ops, wwn, &tpg->tpg_se_tpg,
-				tpg, TRANSPORT_TPG_TYPE_NORMAL);
+	ret = core_tpg_register(wwn, &tpg->tpg_se_tpg, SCSI_PROTOCOL_ISCSI);
 	if (ret < 0)
 		return NULL;
 
@@ -1735,14 +1690,6 @@ static char *iscsi_get_fabric_name(void)
 	return "iSCSI";
 }
 
-static u32 iscsi_get_task_tag(struct se_cmd *se_cmd)
-{
-	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
-
-	/* only used for printks or comparism with ->ref_task_tag */
-	return (__force u32)cmd->init_task_tag;
-}
-
 static int iscsi_get_cmd_state(struct se_cmd *se_cmd)
 {
 	struct iscsi_cmd *cmd = container_of(se_cmd, struct iscsi_cmd, se_cmd);
@@ -1832,78 +1779,58 @@ static void lio_aborted_task(struct se_cmd *se_cmd)
 	cmd->conn->conn_transport->iscsit_aborted_task(cmd->conn, cmd);
 }
 
-static char *lio_tpg_get_endpoint_wwn(struct se_portal_group *se_tpg)
+static inline struct iscsi_portal_group *iscsi_tpg(struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
+	return container_of(se_tpg, struct iscsi_portal_group, tpg_se_tpg);
+}
 
-	return &tpg->tpg_tiqn->tiqn[0];
+static char *lio_tpg_get_endpoint_wwn(struct se_portal_group *se_tpg)
+{
+	return iscsi_tpg(se_tpg)->tpg_tiqn->tiqn;
 }
 
 static u16 lio_tpg_get_tag(struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->tpgt;
+	return iscsi_tpg(se_tpg)->tpgt;
 }
 
 static u32 lio_tpg_get_default_depth(struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->tpg_attrib.default_cmdsn_depth;
+	return iscsi_tpg(se_tpg)->tpg_attrib.default_cmdsn_depth;
 }
 
 static int lio_tpg_check_demo_mode(struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->tpg_attrib.generate_node_acls;
+	return iscsi_tpg(se_tpg)->tpg_attrib.generate_node_acls;
 }
 
 static int lio_tpg_check_demo_mode_cache(struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->tpg_attrib.cache_dynamic_acls;
+	return iscsi_tpg(se_tpg)->tpg_attrib.cache_dynamic_acls;
 }
 
 static int lio_tpg_check_demo_mode_write_protect(
 	struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->tpg_attrib.demo_mode_write_protect;
+	return iscsi_tpg(se_tpg)->tpg_attrib.demo_mode_write_protect;
 }
 
 static int lio_tpg_check_prod_mode_write_protect(
 	struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->tpg_attrib.prod_mode_write_protect;
+	return iscsi_tpg(se_tpg)->tpg_attrib.prod_mode_write_protect;
 }
 
 static int lio_tpg_check_prot_fabric_only(
 	struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
 	/*
 	 * Only report fabric_prot_type if t10_pi has also been enabled
 	 * for incoming ib_isert sessions.
 	 */
-	if (!tpg->tpg_attrib.t10_pi)
+	if (!iscsi_tpg(se_tpg)->tpg_attrib.t10_pi)
 		return 0;
-
-	return tpg->tpg_attrib.fabric_prot_type;
-}
-
-static void lio_tpg_release_fabric_acl(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_acl)
-{
-	struct iscsi_node_acl *acl = container_of(se_acl,
-				struct iscsi_node_acl, se_node_acl);
-	kfree(acl);
+	return iscsi_tpg(se_tpg)->tpg_attrib.fabric_prot_type;
 }
 
 /*
@@ -1948,9 +1875,7 @@ static void lio_tpg_close_session(struct se_session *se_sess)
 
 static u32 lio_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
-	struct iscsi_portal_group *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->tpg_tiqn->tiqn_index;
+	return iscsi_tpg(se_tpg)->tpg_tiqn->tiqn_index;
 }
 
 static void lio_set_default_node_attributes(struct se_node_acl *se_acl)
@@ -1967,7 +1892,7 @@ static void lio_set_default_node_attributes(struct se_node_acl *se_acl)
 
 static int lio_check_stop_free(struct se_cmd *se_cmd)
 {
-	return target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+	return target_put_sess_cmd(se_cmd);
 }
 
 static void lio_release_cmd(struct se_cmd *se_cmd)
@@ -1981,14 +1906,11 @@ static void lio_release_cmd(struct se_cmd *se_cmd)
 const struct target_core_fabric_ops iscsi_ops = {
 	.module				= THIS_MODULE,
 	.name				= "iscsi",
+	.node_acl_size			= sizeof(struct iscsi_node_acl),
 	.get_fabric_name		= iscsi_get_fabric_name,
-	.get_fabric_proto_ident		= iscsi_get_fabric_proto_ident,
 	.tpg_get_wwn			= lio_tpg_get_endpoint_wwn,
 	.tpg_get_tag			= lio_tpg_get_tag,
 	.tpg_get_default_depth		= lio_tpg_get_default_depth,
-	.tpg_get_pr_transport_id	= iscsi_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= iscsi_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= iscsi_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= lio_tpg_check_demo_mode,
 	.tpg_check_demo_mode_cache	= lio_tpg_check_demo_mode_cache,
 	.tpg_check_demo_mode_write_protect =
@@ -1996,8 +1918,6 @@ const struct target_core_fabric_ops iscsi_ops = {
 	.tpg_check_prod_mode_write_protect =
 			lio_tpg_check_prod_mode_write_protect,
 	.tpg_check_prot_fabric_only	= &lio_tpg_check_prot_fabric_only,
-	.tpg_alloc_fabric_acl		= lio_tpg_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= lio_tpg_release_fabric_acl,
 	.tpg_get_inst_index		= lio_tpg_get_inst_index,
 	.check_stop_free		= lio_check_stop_free,
 	.release_cmd			= lio_release_cmd,
@@ -2008,7 +1928,6 @@ const struct target_core_fabric_ops iscsi_ops = {
 	.write_pending			= lio_write_pending,
 	.write_pending_status		= lio_write_pending_status,
 	.set_default_node_attributes	= lio_set_default_node_attributes,
-	.get_task_tag			= iscsi_get_task_tag,
 	.get_cmd_state			= iscsi_get_cmd_state,
 	.queue_data_in			= lio_queue_data_in,
 	.queue_status			= lio_queue_status,
@@ -2020,8 +1939,8 @@ const struct target_core_fabric_ops iscsi_ops = {
 	.fabric_drop_tpg		= lio_target_tiqn_deltpg,
 	.fabric_make_np			= lio_target_call_addnptotpg,
 	.fabric_drop_np			= lio_target_call_delnpfromtpg,
-	.fabric_make_nodeacl		= lio_target_make_nodeacl,
-	.fabric_drop_nodeacl		= lio_target_drop_nodeacl,
+	.fabric_init_nodeacl		= lio_target_init_nodeacl,
+	.fabric_cleanup_nodeacl		= lio_target_cleanup_nodeacl,
 
 	.tfc_discovery_attrs		= lio_target_discovery_auth_attrs,
 	.tfc_wwn_attrs			= lio_target_wwn_attrs,
diff --git a/drivers/target/iscsi/iscsi_target_device.c b/drivers/target/iscsi/iscsi_target_device.c
index 34c3cd1b05ce..5fabcd3d623f 100644
--- a/drivers/target/iscsi/iscsi_target_device.c
+++ b/drivers/target/iscsi/iscsi_target_device.c
@@ -17,7 +17,6 @@
  * GNU General Public License for more details.
  ******************************************************************************/
 
-#include <scsi/scsi_device.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 
diff --git a/drivers/target/iscsi/iscsi_target_erl0.c b/drivers/target/iscsi/iscsi_target_erl0.c
index 959a14c9dd5d..210f6e4830e3 100644
--- a/drivers/target/iscsi/iscsi_target_erl0.c
+++ b/drivers/target/iscsi/iscsi_target_erl0.c
@@ -956,56 +956,3 @@ void iscsit_take_action_for_connection_exit(struct iscsi_conn *conn)
 
 	iscsit_handle_connection_cleanup(conn);
 }
-
-/*
- *	This is the simple function that makes the magic of
- *	sync and steering happen in the follow paradoxical order:
- *
- *	0) Receive conn->of_marker (bytes left until next OFMarker)
- *	   bytes into an offload buffer.  When we pass the exact number
- *	   of bytes in conn->of_marker, iscsit_dump_data_payload() and hence
- *	   rx_data() will automatically receive the identical u32 marker
- *	   values and store it in conn->of_marker_offset;
- *	1) Now conn->of_marker_offset will contain the offset to the start
- *	   of the next iSCSI PDU.  Dump these remaining bytes into another
- *	   offload buffer.
- *	2) We are done!
- *	   Next byte in the TCP stream will contain the next iSCSI PDU!
- *	   Cool Huh?!
- */
-int iscsit_recover_from_unknown_opcode(struct iscsi_conn *conn)
-{
-	/*
-	 * Make sure the remaining bytes to next maker is a sane value.
-	 */
-	if (conn->of_marker > (conn->conn_ops->OFMarkInt * 4)) {
-		pr_err("Remaining bytes to OFMarker: %u exceeds"
-			" OFMarkInt bytes: %u.\n", conn->of_marker,
-				conn->conn_ops->OFMarkInt * 4);
-		return -1;
-	}
-
-	pr_debug("Advancing %u bytes in TCP stream to get to the"
-			" next OFMarker.\n", conn->of_marker);
-
-	if (iscsit_dump_data_payload(conn, conn->of_marker, 0) < 0)
-		return -1;
-
-	/*
-	 * Make sure the offset marker we retrived is a valid value.
-	 */
-	if (conn->of_marker_offset > (ISCSI_HDR_LEN + (ISCSI_CRC_LEN * 2) +
-	    conn->conn_ops->MaxRecvDataSegmentLength)) {
-		pr_err("OfMarker offset value: %u exceeds limit.\n",
-			conn->of_marker_offset);
-		return -1;
-	}
-
-	pr_debug("Discarding %u bytes of TCP stream to get to the"
-			" next iSCSI Opcode.\n", conn->of_marker_offset);
-
-	if (iscsit_dump_data_payload(conn, conn->of_marker_offset, 0) < 0)
-		return -1;
-
-	return 0;
-}
diff --git a/drivers/target/iscsi/iscsi_target_erl0.h b/drivers/target/iscsi/iscsi_target_erl0.h
index 21acc9a06376..a9e2f9497fb2 100644
--- a/drivers/target/iscsi/iscsi_target_erl0.h
+++ b/drivers/target/iscsi/iscsi_target_erl0.h
@@ -10,6 +10,5 @@ extern void iscsit_connection_reinstatement_rcfr(struct iscsi_conn *);
 extern void iscsit_cause_connection_reinstatement(struct iscsi_conn *, int);
 extern void iscsit_fall_back_to_erl0(struct iscsi_session *);
 extern void iscsit_take_action_for_connection_exit(struct iscsi_conn *);
-extern int iscsit_recover_from_unknown_opcode(struct iscsi_conn *);
 
 #endif   /*** ISCSI_TARGET_ERL0_H ***/
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 70d799dfab03..3d0fe4ff5590 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -410,8 +410,6 @@ static int iscsi_login_zero_tsih_s2(
 	if (iscsi_change_param_sprintf(conn, "ErrorRecoveryLevel=%d", na->default_erl))
 		return -1;
 
-	if (iscsi_login_disable_FIM_keys(conn->param_list, conn) < 0)
-		return -1;
 	/*
 	 * Set RDMAExtensions=Yes by default for iSER enabled network portals
 	 */
@@ -477,59 +475,6 @@ check_prot:
 	return 0;
 }
 
-/*
- * Remove PSTATE_NEGOTIATE for the four FIM related keys.
- * The Initiator node will be able to enable FIM by proposing them itself.
- */
-int iscsi_login_disable_FIM_keys(
-	struct iscsi_param_list *param_list,
-	struct iscsi_conn *conn)
-{
-	struct iscsi_param *param;
-
-	param = iscsi_find_param_from_key("OFMarker", param_list);
-	if (!param) {
-		pr_err("iscsi_find_param_from_key() for"
-				" OFMarker failed\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		return -1;
-	}
-	param->state &= ~PSTATE_NEGOTIATE;
-
-	param = iscsi_find_param_from_key("OFMarkInt", param_list);
-	if (!param) {
-		pr_err("iscsi_find_param_from_key() for"
-				" IFMarker failed\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		return -1;
-	}
-	param->state &= ~PSTATE_NEGOTIATE;
-
-	param = iscsi_find_param_from_key("IFMarker", param_list);
-	if (!param) {
-		pr_err("iscsi_find_param_from_key() for"
-				" IFMarker failed\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		return -1;
-	}
-	param->state &= ~PSTATE_NEGOTIATE;
-
-	param = iscsi_find_param_from_key("IFMarkInt", param_list);
-	if (!param) {
-		pr_err("iscsi_find_param_from_key() for"
-				" IFMarker failed\n");
-		iscsit_tx_login_rsp(conn, ISCSI_STATUS_CLS_TARGET_ERR,
-				ISCSI_LOGIN_STATUS_NO_RESOURCES);
-		return -1;
-	}
-	param->state &= ~PSTATE_NEGOTIATE;
-
-	return 0;
-}
-
 static int iscsi_login_non_zero_tsih_s1(
 	struct iscsi_conn *conn,
 	unsigned char *buf)
@@ -616,7 +561,7 @@ static int iscsi_login_non_zero_tsih_s2(
 	if (iscsi_change_param_sprintf(conn, "TargetPortalGroupTag=%hu", sess->tpg->tpgt))
 		return -1;
 
-	return iscsi_login_disable_FIM_keys(conn->param_list, conn);
+	return 0;
 }
 
 int iscsi_login_post_auth_non_zero_tsih(
@@ -765,7 +710,6 @@ int iscsi_post_login_handler(
 	conn->conn_state = TARG_CONN_STATE_LOGGED_IN;
 
 	iscsi_set_connection_parameters(conn->conn_ops, conn->param_list);
-	iscsit_set_sync_and_steering_values(conn);
 	/*
 	 * SCSI Initiator -> SCSI Target Port Mapping
 	 */
diff --git a/drivers/target/iscsi/iscsi_target_login.h b/drivers/target/iscsi/iscsi_target_login.h
index 29d098324b7f..1c7358081533 100644
--- a/drivers/target/iscsi/iscsi_target_login.h
+++ b/drivers/target/iscsi/iscsi_target_login.h
@@ -16,6 +16,5 @@ extern int iscsi_post_login_handler(struct iscsi_np *, struct iscsi_conn *, u8);
 extern void iscsi_target_login_sess_out(struct iscsi_conn *, struct iscsi_np *,
 				bool, bool);
 extern int iscsi_target_login_thread(void *);
-extern int iscsi_login_disable_FIM_keys(struct iscsi_param_list *, struct iscsi_conn *);
 
 #endif   /*** ISCSI_TARGET_LOGIN_H ***/
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index d4f9e9645697..e8a52f7d6204 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -34,13 +34,6 @@ int iscsi_login_rx_data(
 	iov.iov_len	= length;
 	iov.iov_base	= buf;
 
-	/*
-	 * Initial Marker-less Interval.
-	 * Add the values regardless of IFMarker/OFMarker, considering
-	 * it may not be negoitated yet.
-	 */
-	conn->of_marker += length;
-
 	rx_got = rx_data(conn, &iov, 1, length);
 	if (rx_got != length) {
 		pr_err("rx_data returned %d, expecting %d.\n",
@@ -72,13 +65,6 @@ int iscsi_login_tx_data(
 		iov_cnt++;
 	}
 
-	/*
-	 * Initial Marker-less Interval.
-	 * Add the values regardless of IFMarker/OFMarker, considering
-	 * it may not be negoitated yet.
-	 */
-	conn->if_marker += length;
-
 	tx_sent = tx_data(conn, &iov[0], iov_cnt, length);
 	if (tx_sent != length) {
 		pr_err("tx_data returned %d, expecting %d.\n",
@@ -97,12 +83,6 @@ void iscsi_dump_conn_ops(struct iscsi_conn_ops *conn_ops)
 				"CRC32C" : "None");
 	pr_debug("MaxRecvDataSegmentLength: %u\n",
 				conn_ops->MaxRecvDataSegmentLength);
-	pr_debug("OFMarker: %s\n", (conn_ops->OFMarker) ? "Yes" : "No");
-	pr_debug("IFMarker: %s\n", (conn_ops->IFMarker) ? "Yes" : "No");
-	if (conn_ops->OFMarker)
-		pr_debug("OFMarkInt: %u\n", conn_ops->OFMarkInt);
-	if (conn_ops->IFMarker)
-		pr_debug("IFMarkInt: %u\n", conn_ops->IFMarkInt);
 }
 
 void iscsi_dump_sess_ops(struct iscsi_sess_ops *sess_ops)
@@ -194,10 +174,6 @@ static struct iscsi_param *iscsi_set_default_param(struct iscsi_param_list *para
 	case TYPERANGE_DIGEST:
 		param->type = TYPE_VALUE_LIST | TYPE_STRING;
 		break;
-	case TYPERANGE_MARKINT:
-		param->type = TYPE_NUMBER_RANGE;
-		param->type_range |= TYPERANGE_1_TO_65535;
-		break;
 	case TYPERANGE_ISCSINAME:
 	case TYPERANGE_SESSIONTYPE:
 	case TYPERANGE_TARGETADDRESS:
@@ -422,13 +398,13 @@ int iscsi_create_default_params(struct iscsi_param_list **param_list_ptr)
 
 	param = iscsi_set_default_param(pl, IFMARKINT, INITIAL_IFMARKINT,
 			PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
-			TYPERANGE_MARKINT, USE_INITIAL_ONLY);
+			TYPERANGE_UTF8, USE_INITIAL_ONLY);
 	if (!param)
 		goto out;
 
 	param = iscsi_set_default_param(pl, OFMARKINT, INITIAL_OFMARKINT,
 			PHASE_OPERATIONAL, SCOPE_CONNECTION_ONLY, SENDER_BOTH,
-			TYPERANGE_MARKINT, USE_INITIAL_ONLY);
+			TYPERANGE_UTF8, USE_INITIAL_ONLY);
 	if (!param)
 		goto out;
 	/*
@@ -524,9 +500,9 @@ int iscsi_set_keys_to_negotiate(
 		} else if (!strcmp(param->name, OFMARKER)) {
 			SET_PSTATE_NEGOTIATE(param);
 		} else if (!strcmp(param->name, IFMARKINT)) {
-			SET_PSTATE_NEGOTIATE(param);
+			SET_PSTATE_REJECT(param);
 		} else if (!strcmp(param->name, OFMARKINT)) {
-			SET_PSTATE_NEGOTIATE(param);
+			SET_PSTATE_REJECT(param);
 		} else if (!strcmp(param->name, RDMAEXTENSIONS)) {
 			if (iser)
 				SET_PSTATE_NEGOTIATE(param);
@@ -906,91 +882,6 @@ static int iscsi_check_numerical_value(struct iscsi_param *param, char *value_pt
 	return 0;
 }
 
-static int iscsi_check_numerical_range_value(struct iscsi_param *param, char *value)
-{
-	char *left_val_ptr = NULL, *right_val_ptr = NULL;
-	char *tilde_ptr = NULL;
-	u32 left_val, right_val, local_left_val;
-
-	if (strcmp(param->name, IFMARKINT) &&
-	    strcmp(param->name, OFMARKINT)) {
-		pr_err("Only parameters \"%s\" or \"%s\" may contain a"
-		       " numerical range value.\n", IFMARKINT, OFMARKINT);
-		return -1;
-	}
-
-	if (IS_PSTATE_PROPOSER(param))
-		return 0;
-
-	tilde_ptr = strchr(value, '~');
-	if (!tilde_ptr) {
-		pr_err("Unable to locate numerical range indicator"
-			" \"~\" for \"%s\".\n", param->name);
-		return -1;
-	}
-	*tilde_ptr = '\0';
-
-	left_val_ptr = value;
-	right_val_ptr = value + strlen(left_val_ptr) + 1;
-
-	if (iscsi_check_numerical_value(param, left_val_ptr) < 0)
-		return -1;
-	if (iscsi_check_numerical_value(param, right_val_ptr) < 0)
-		return -1;
-
-	left_val = simple_strtoul(left_val_ptr, NULL, 0);
-	right_val = simple_strtoul(right_val_ptr, NULL, 0);
-	*tilde_ptr = '~';
-
-	if (right_val < left_val) {
-		pr_err("Numerical range for parameter \"%s\" contains"
-			" a right value which is less than the left.\n",
-				param->name);
-		return -1;
-	}
-
-	/*
-	 * For now,  enforce reasonable defaults for [I,O]FMarkInt.
-	 */
-	tilde_ptr = strchr(param->value, '~');
-	if (!tilde_ptr) {
-		pr_err("Unable to locate numerical range indicator"
-			" \"~\" for \"%s\".\n", param->name);
-		return -1;
-	}
-	*tilde_ptr = '\0';
-
-	left_val_ptr = param->value;
-	right_val_ptr = param->value + strlen(left_val_ptr) + 1;
-
-	local_left_val = simple_strtoul(left_val_ptr, NULL, 0);
-	*tilde_ptr = '~';
-
-	if (param->set_param) {
-		if ((left_val < local_left_val) ||
-		    (right_val < local_left_val)) {
-			pr_err("Passed value range \"%u~%u\" is below"
-				" minimum left value \"%u\" for key \"%s\","
-				" rejecting.\n", left_val, right_val,
-				local_left_val, param->name);
-			return -1;
-		}
-	} else {
-		if ((left_val < local_left_val) &&
-		    (right_val < local_left_val)) {
-			pr_err("Received value range \"%u~%u\" is"
-				" below minimum left value \"%u\" for key"
-				" \"%s\", rejecting.\n", left_val, right_val,
-				local_left_val, param->name);
-			SET_PSTATE_REJECT(param);
-			if (iscsi_update_param_value(param, REJECT) < 0)
-				return -1;
-		}
-	}
-
-	return 0;
-}
-
 static int iscsi_check_string_or_list_value(struct iscsi_param *param, char *value)
 {
 	if (IS_PSTATE_PROPOSER(param))
@@ -1027,33 +918,6 @@ static int iscsi_check_string_or_list_value(struct iscsi_param *param, char *val
 	return 0;
 }
 
-/*
- *	This function is used to pick a value range number,  currently just
- *	returns the lesser of both right values.
- */
-static char *iscsi_get_value_from_number_range(
-	struct iscsi_param *param,
-	char *value)
-{
-	char *end_ptr, *tilde_ptr1 = NULL, *tilde_ptr2 = NULL;
-	u32 acceptor_right_value, proposer_right_value;
-
-	tilde_ptr1 = strchr(value, '~');
-	if (!tilde_ptr1)
-		return NULL;
-	*tilde_ptr1++ = '\0';
-	proposer_right_value = simple_strtoul(tilde_ptr1, &end_ptr, 0);
-
-	tilde_ptr2 = strchr(param->value, '~');
-	if (!tilde_ptr2)
-		return NULL;
-	*tilde_ptr2++ = '\0';
-	acceptor_right_value = simple_strtoul(tilde_ptr2, &end_ptr, 0);
-
-	return (acceptor_right_value >= proposer_right_value) ?
-		tilde_ptr1 : tilde_ptr2;
-}
-
 static char *iscsi_check_valuelist_for_support(
 	struct iscsi_param *param,
 	char *value)
@@ -1103,7 +967,7 @@ static int iscsi_check_acceptor_state(struct iscsi_param *param, char *value,
 				struct iscsi_conn *conn)
 {
 	u8 acceptor_boolean_value = 0, proposer_boolean_value = 0;
-	char *negoitated_value = NULL;
+	char *negotiated_value = NULL;
 
 	if (IS_PSTATE_ACCEPTOR(param)) {
 		pr_err("Received key \"%s\" twice, protocol error.\n",
@@ -1203,24 +1067,16 @@ static int iscsi_check_acceptor_state(struct iscsi_param *param, char *value,
 			pr_debug("Updated %s to target MXDSL value: %s\n",
 					param->name, param->value);
 		}
-
-	} else if (IS_TYPE_NUMBER_RANGE(param)) {
-		negoitated_value = iscsi_get_value_from_number_range(
-					param, value);
-		if (!negoitated_value)
-			return -1;
-		if (iscsi_update_param_value(param, negoitated_value) < 0)
-			return -1;
 	} else if (IS_TYPE_VALUE_LIST(param)) {
-		negoitated_value = iscsi_check_valuelist_for_support(
+		negotiated_value = iscsi_check_valuelist_for_support(
 					param, value);
-		if (!negoitated_value) {
+		if (!negotiated_value) {
 			pr_err("Proposer's value list \"%s\" contains"
 				" no valid values from Acceptor's value list"
 				" \"%s\".\n", value, param->value);
 			return -1;
 		}
-		if (iscsi_update_param_value(param, negoitated_value) < 0)
+		if (iscsi_update_param_value(param, negotiated_value) < 0)
 			return -1;
 	} else if (IS_PHASE_DECLARATIVE(param)) {
 		if (iscsi_update_param_value(param, value) < 0)
@@ -1239,47 +1095,7 @@ static int iscsi_check_proposer_state(struct iscsi_param *param, char *value)
 		return -1;
 	}
 
-	if (IS_TYPE_NUMBER_RANGE(param)) {
-		u32 left_val = 0, right_val = 0, recieved_value = 0;
-		char *left_val_ptr = NULL, *right_val_ptr = NULL;
-		char *tilde_ptr = NULL;
-
-		if (!strcmp(value, IRRELEVANT) || !strcmp(value, REJECT)) {
-			if (iscsi_update_param_value(param, value) < 0)
-				return -1;
-			return 0;
-		}
-
-		tilde_ptr = strchr(value, '~');
-		if (tilde_ptr) {
-			pr_err("Illegal \"~\" in response for \"%s\".\n",
-					param->name);
-			return -1;
-		}
-		tilde_ptr = strchr(param->value, '~');
-		if (!tilde_ptr) {
-			pr_err("Unable to locate numerical range"
-				" indicator \"~\" for \"%s\".\n", param->name);
-			return -1;
-		}
-		*tilde_ptr = '\0';
-
-		left_val_ptr = param->value;
-		right_val_ptr = param->value + strlen(left_val_ptr) + 1;
-		left_val = simple_strtoul(left_val_ptr, NULL, 0);
-		right_val = simple_strtoul(right_val_ptr, NULL, 0);
-		recieved_value = simple_strtoul(value, NULL, 0);
-
-		*tilde_ptr = '~';
-
-		if ((recieved_value < left_val) ||
-		    (recieved_value > right_val)) {
-			pr_err("Illegal response \"%s=%u\", value must"
-				" be between %u and %u.\n", param->name,
-				recieved_value, left_val, right_val);
-			return -1;
-		}
-	} else if (IS_TYPE_VALUE_LIST(param)) {
+	if (IS_TYPE_VALUE_LIST(param)) {
 		char *comma_ptr = NULL, *tmp_ptr = NULL;
 
 		comma_ptr = strchr(value, ',');
@@ -1361,9 +1177,6 @@ static int iscsi_check_value(struct iscsi_param *param, char *value)
 		} else if (IS_TYPE_NUMBER(param)) {
 			if (iscsi_check_numerical_value(param, value) < 0)
 				return -1;
-		} else if (IS_TYPE_NUMBER_RANGE(param)) {
-			if (iscsi_check_numerical_range_value(param, value) < 0)
-				return -1;
 		} else if (IS_TYPE_STRING(param) || IS_TYPE_VALUE_LIST(param)) {
 			if (iscsi_check_string_or_list_value(param, value) < 0)
 				return -1;
@@ -1483,8 +1296,6 @@ static int iscsi_enforce_integrity_rules(
 	char *tmpptr;
 	u8 DataSequenceInOrder = 0;
 	u8 ErrorRecoveryLevel = 0, SessionType = 0;
-	u8 IFMarker = 0, OFMarker = 0;
-	u8 IFMarkInt_Reject = 1, OFMarkInt_Reject = 1;
 	u32 FirstBurstLength = 0, MaxBurstLength = 0;
 	struct iscsi_param *param = NULL;
 
@@ -1503,28 +1314,12 @@ static int iscsi_enforce_integrity_rules(
 		if (!strcmp(param->name, MAXBURSTLENGTH))
 			MaxBurstLength = simple_strtoul(param->value,
 					&tmpptr, 0);
-		if (!strcmp(param->name, IFMARKER))
-			if (!strcmp(param->value, YES))
-				IFMarker = 1;
-		if (!strcmp(param->name, OFMARKER))
-			if (!strcmp(param->value, YES))
-				OFMarker = 1;
-		if (!strcmp(param->name, IFMARKINT))
-			if (!strcmp(param->value, REJECT))
-				IFMarkInt_Reject = 1;
-		if (!strcmp(param->name, OFMARKINT))
-			if (!strcmp(param->value, REJECT))
-				OFMarkInt_Reject = 1;
 	}
 
 	list_for_each_entry(param, &param_list->param_list, p_list) {
 		if (!(param->phase & phase))
 			continue;
-		if (!SessionType && (!IS_PSTATE_ACCEPTOR(param) &&
-		     (strcmp(param->name, IFMARKER) &&
-		      strcmp(param->name, OFMARKER) &&
-		      strcmp(param->name, IFMARKINT) &&
-		      strcmp(param->name, OFMARKINT))))
+		if (!SessionType && !IS_PSTATE_ACCEPTOR(param))
 			continue;
 		if (!strcmp(param->name, MAXOUTSTANDINGR2T) &&
 		    DataSequenceInOrder && (ErrorRecoveryLevel > 0)) {
@@ -1556,38 +1351,6 @@ static int iscsi_enforce_integrity_rules(
 					param->name, param->value);
 			}
 		}
-		if (!strcmp(param->name, IFMARKER) && IFMarkInt_Reject) {
-			if (iscsi_update_param_value(param, NO) < 0)
-				return -1;
-			IFMarker = 0;
-			pr_debug("Reset \"%s\" to \"%s\".\n",
-					param->name, param->value);
-		}
-		if (!strcmp(param->name, OFMARKER) && OFMarkInt_Reject) {
-			if (iscsi_update_param_value(param, NO) < 0)
-				return -1;
-			OFMarker = 0;
-			pr_debug("Reset \"%s\" to \"%s\".\n",
-					 param->name, param->value);
-		}
-		if (!strcmp(param->name, IFMARKINT) && !IFMarker) {
-			if (!strcmp(param->value, REJECT))
-				continue;
-			param->state &= ~PSTATE_NEGOTIATE;
-			if (iscsi_update_param_value(param, IRRELEVANT) < 0)
-				return -1;
-			pr_debug("Reset \"%s\" to \"%s\".\n",
-					param->name, param->value);
-		}
-		if (!strcmp(param->name, OFMARKINT) && !OFMarker) {
-			if (!strcmp(param->value, REJECT))
-				continue;
-			param->state &= ~PSTATE_NEGOTIATE;
-			if (iscsi_update_param_value(param, IRRELEVANT) < 0)
-				return -1;
-			pr_debug("Reset \"%s\" to \"%s\".\n",
-					param->name, param->value);
-		}
 	}
 
 	return 0;
@@ -1824,24 +1587,6 @@ void iscsi_set_connection_parameters(
 			 */
 			pr_debug("MaxRecvDataSegmentLength:     %u\n",
 				ops->MaxRecvDataSegmentLength);
-		} else if (!strcmp(param->name, OFMARKER)) {
-			ops->OFMarker = !strcmp(param->value, YES);
-			pr_debug("OFMarker:                     %s\n",
-				param->value);
-		} else if (!strcmp(param->name, IFMARKER)) {
-			ops->IFMarker = !strcmp(param->value, YES);
-			pr_debug("IFMarker:                     %s\n",
-				param->value);
-		} else if (!strcmp(param->name, OFMARKINT)) {
-			ops->OFMarkInt =
-				simple_strtoul(param->value, &tmpptr, 0);
-			pr_debug("OFMarkInt:                    %s\n",
-				param->value);
-		} else if (!strcmp(param->name, IFMARKINT)) {
-			ops->IFMarkInt =
-				simple_strtoul(param->value, &tmpptr, 0);
-			pr_debug("IFMarkInt:                    %s\n",
-				param->value);
 		} else if (!strcmp(param->name, INITIATORRECVDATASEGMENTLENGTH)) {
 			ops->InitiatorRecvDataSegmentLength =
 				simple_strtoul(param->value, &tmpptr, 0);
diff --git a/drivers/target/iscsi/iscsi_target_parameters.h b/drivers/target/iscsi/iscsi_target_parameters.h
index a47046a752aa..a0751e3f0813 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.h
+++ b/drivers/target/iscsi/iscsi_target_parameters.h
@@ -138,8 +138,8 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define INITIAL_SESSIONTYPE			NORMAL
 #define INITIAL_IFMARKER			NO
 #define INITIAL_OFMARKER			NO
-#define INITIAL_IFMARKINT			"2048~65535"
-#define INITIAL_OFMARKINT			"2048~65535"
+#define INITIAL_IFMARKINT			REJECT
+#define INITIAL_OFMARKINT			REJECT
 
 /*
  * Initial values for iSER parameters following RFC-5046 Section 6
@@ -239,10 +239,9 @@ extern void iscsi_set_session_parameters(struct iscsi_sess_ops *,
 #define TYPERANGE_AUTH			0x0200
 #define TYPERANGE_DIGEST		0x0400
 #define TYPERANGE_ISCSINAME		0x0800
-#define TYPERANGE_MARKINT		0x1000
-#define TYPERANGE_SESSIONTYPE		0x2000
-#define TYPERANGE_TARGETADDRESS		0x4000
-#define TYPERANGE_UTF8			0x8000
+#define TYPERANGE_SESSIONTYPE		0x1000
+#define TYPERANGE_TARGETADDRESS		0x2000
+#define TYPERANGE_UTF8			0x4000
 
 #define IS_TYPERANGE_0_TO_2(p)		((p)->type_range & TYPERANGE_0_TO_2)
 #define IS_TYPERANGE_0_TO_3600(p)	((p)->type_range & TYPERANGE_0_TO_3600)
diff --git a/drivers/target/iscsi/iscsi_target_tmr.c b/drivers/target/iscsi/iscsi_target_tmr.c
index b0224a77e26d..cf59c397007b 100644
--- a/drivers/target/iscsi/iscsi_target_tmr.c
+++ b/drivers/target/iscsi/iscsi_target_tmr.c
@@ -17,7 +17,7 @@
  ******************************************************************************/
 
 #include <asm/unaligned.h>
-#include <scsi/scsi_device.h>
+#include <scsi/scsi_proto.h>
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
@@ -120,7 +120,7 @@ u8 iscsit_tmr_task_reassign(
 	struct iscsi_tmr_req *tmr_req = cmd->tmr_req;
 	struct se_tmr_req *se_tmr = cmd->se_cmd.se_tmr_req;
 	struct iscsi_tm *hdr = (struct iscsi_tm *) buf;
-	int ret, ref_lun;
+	u64 ret, ref_lun;
 
 	pr_debug("Got TASK_REASSIGN TMR ITT: 0x%08x,"
 		" RefTaskTag: 0x%08x, ExpDataSN: 0x%08x, CID: %hu\n",
@@ -164,7 +164,7 @@ u8 iscsit_tmr_task_reassign(
 	ref_lun = scsilun_to_int(&hdr->lun);
 	if (ref_lun != ref_cmd->se_cmd.orig_fe_lun) {
 		pr_err("Unable to perform connection recovery for"
-			" differing ref_lun: %d ref_cmd orig_fe_lun: %u\n",
+			" differing ref_lun: %llu ref_cmd orig_fe_lun: %llu\n",
 			ref_lun, ref_cmd->se_cmd.orig_fe_lun);
 		return ISCSI_TMF_RSP_REJECTED;
 	}
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index 5e3295fe404d..968068ffcb1c 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -18,7 +18,6 @@
 
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 
 #include <target/iscsi/iscsi_target_core.h>
 #include "iscsi_target_erl0.h"
@@ -67,9 +66,12 @@ int iscsit_load_discovery_tpg(void)
 		pr_err("Unable to allocate struct iscsi_portal_group\n");
 		return -1;
 	}
-
-	ret = core_tpg_register(&iscsi_ops, NULL, &tpg->tpg_se_tpg,
-				tpg, TRANSPORT_TPG_TYPE_DISCOVERY);
+	/*
+	 * Save iscsi_ops pointer for special case discovery TPG that
+	 * doesn't exist as se_wwn->wwn_group within configfs.
+	 */
+	tpg->tpg_se_tpg.se_tpg_tfo = &iscsi_ops;
+	ret = core_tpg_register(NULL, &tpg->tpg_se_tpg, -1);
 	if (ret < 0) {
 		kfree(tpg);
 		return -1;
@@ -280,8 +282,6 @@ int iscsit_tpg_del_portal_group(
 		return -EPERM;
 	}
 
-	core_tpg_clear_object_luns(&tpg->tpg_se_tpg);
-
 	if (tpg->param_list) {
 		iscsi_release_param_list(tpg->param_list);
 		tpg->param_list = NULL;
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index b18edda3e8af..a2bff0702eb2 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -22,7 +22,6 @@
 #include <scsi/iscsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 #include <target/iscsi/iscsi_transport.h>
 
 #include <target/iscsi/iscsi_target_core.h>
@@ -746,7 +745,7 @@ void iscsit_free_cmd(struct iscsi_cmd *cmd, bool shutdown)
 		rc = transport_generic_free_cmd(&cmd->se_cmd, shutdown);
 		if (!rc && shutdown && se_cmd && se_cmd->se_sess) {
 			__iscsit_free_cmd(cmd, true, shutdown);
-			target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+			target_put_sess_cmd(se_cmd);
 		}
 		break;
 	case ISCSI_OP_REJECT:
@@ -762,7 +761,7 @@ void iscsit_free_cmd(struct iscsi_cmd *cmd, bool shutdown)
 			rc = transport_generic_free_cmd(&cmd->se_cmd, shutdown);
 			if (!rc && shutdown && se_cmd->se_sess) {
 				__iscsit_free_cmd(cmd, true, shutdown);
-				target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+				target_put_sess_cmd(se_cmd);
 			}
 			break;
 		}
@@ -809,54 +808,6 @@ void iscsit_inc_session_usage_count(struct iscsi_session *sess)
 	spin_unlock_bh(&sess->session_usage_lock);
 }
 
-/*
- *	Setup conn->if_marker and conn->of_marker values based upon
- *	the initial marker-less interval. (see iSCSI v19 A.2)
- */
-int iscsit_set_sync_and_steering_values(struct iscsi_conn *conn)
-{
-	int login_ifmarker_count = 0, login_ofmarker_count = 0, next_marker = 0;
-	/*
-	 * IFMarkInt and OFMarkInt are negotiated as 32-bit words.
-	 */
-	u32 IFMarkInt = (conn->conn_ops->IFMarkInt * 4);
-	u32 OFMarkInt = (conn->conn_ops->OFMarkInt * 4);
-
-	if (conn->conn_ops->OFMarker) {
-		/*
-		 * Account for the first Login Command received not
-		 * via iscsi_recv_msg().
-		 */
-		conn->of_marker += ISCSI_HDR_LEN;
-		if (conn->of_marker <= OFMarkInt) {
-			conn->of_marker = (OFMarkInt - conn->of_marker);
-		} else {
-			login_ofmarker_count = (conn->of_marker / OFMarkInt);
-			next_marker = (OFMarkInt * (login_ofmarker_count + 1)) +
-					(login_ofmarker_count * MARKER_SIZE);
-			conn->of_marker = (next_marker - conn->of_marker);
-		}
-		conn->of_marker_offset = 0;
-		pr_debug("Setting OFMarker value to %u based on Initial"
-			" Markerless Interval.\n", conn->of_marker);
-	}
-
-	if (conn->conn_ops->IFMarker) {
-		if (conn->if_marker <= IFMarkInt) {
-			conn->if_marker = (IFMarkInt - conn->if_marker);
-		} else {
-			login_ifmarker_count = (conn->if_marker / IFMarkInt);
-			next_marker = (IFMarkInt * (login_ifmarker_count + 1)) +
-					(login_ifmarker_count * MARKER_SIZE);
-			conn->if_marker = (next_marker - conn->if_marker);
-		}
-		pr_debug("Setting IFMarker value to %u based on Initial"
-			" Markerless Interval.\n", conn->if_marker);
-	}
-
-	return 0;
-}
-
 struct iscsi_conn *iscsit_get_conn_from_cid(struct iscsi_session *sess, u16 cid)
 {
 	struct iscsi_conn *conn;
diff --git a/drivers/target/iscsi/iscsi_target_util.h b/drivers/target/iscsi/iscsi_target_util.h
index 1ab754a671ff..995f1cb29d0e 100644
--- a/drivers/target/iscsi/iscsi_target_util.h
+++ b/drivers/target/iscsi/iscsi_target_util.h
@@ -34,7 +34,6 @@ extern void iscsit_free_cmd(struct iscsi_cmd *, bool);
 extern int iscsit_check_session_usage_count(struct iscsi_session *);
 extern void iscsit_dec_session_usage_count(struct iscsi_session *);
 extern void iscsit_inc_session_usage_count(struct iscsi_session *);
-extern int iscsit_set_sync_and_steering_values(struct iscsi_conn *);
 extern struct iscsi_conn *iscsit_get_conn_from_cid(struct iscsi_session *, u16);
 extern struct iscsi_conn *iscsit_get_conn_from_cid_rcfr(struct iscsi_session *, u16);
 extern void iscsit_check_conn_usage_count(struct iscsi_conn *);
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 51f0c895c6a5..a556bdebd775 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -35,14 +35,11 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 
 #include "tcm_loop.h"
 
 #define to_tcm_loop_hba(hba)	container_of(hba, struct tcm_loop_hba, dev)
 
-static const struct target_core_fabric_ops loop_ops;
-
 static struct workqueue_struct *tcm_loop_workqueue;
 static struct kmem_cache *tcm_loop_cmd_cache;
 
@@ -165,6 +162,7 @@ static void tcm_loop_submission_work(struct work_struct *work)
 		transfer_length = scsi_bufflen(sc);
 	}
 
+	se_cmd->tag = tl_cmd->sc_cmd_tag;
 	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, TCM_SIMPLE_TAG,
@@ -217,7 +215,7 @@ 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,
-			      int lun, int task, enum tcm_tmreq_table tmr)
+			      u64 lun, int task, enum tcm_tmreq_table tmr)
 {
 	struct se_cmd *se_cmd = NULL;
 	struct se_session *se_sess;
@@ -409,7 +407,7 @@ static int tcm_loop_driver_probe(struct device *dev)
 	sh->max_id = 2;
 	sh->max_lun = 0;
 	sh->max_channel = 0;
-	sh->max_cmd_len = TL_SCSI_MAX_CMD_LEN;
+	sh->max_cmd_len = SCSI_MAX_VARLEN_CDB_SIZE;
 
 	host_prot = SHOST_DIF_TYPE1_PROTECTION | SHOST_DIF_TYPE2_PROTECTION |
 		    SHOST_DIF_TYPE3_PROTECTION | SHOST_DIX_TYPE1_PROTECTION |
@@ -520,147 +518,26 @@ static char *tcm_loop_get_fabric_name(void)
 	return "loopback";
 }
 
-static u8 tcm_loop_get_fabric_proto_ident(struct se_portal_group *se_tpg)
+static inline struct tcm_loop_tpg *tl_tpg(struct se_portal_group *se_tpg)
 {
-	struct tcm_loop_tpg *tl_tpg = se_tpg->se_tpg_fabric_ptr;
-	struct tcm_loop_hba *tl_hba = tl_tpg->tl_hba;
-	/*
-	 * tl_proto_id is set at tcm_loop_configfs.c:tcm_loop_make_scsi_hba()
-	 * time based on the protocol dependent prefix of the passed configfs group.
-	 *
-	 * Based upon tl_proto_id, TCM_Loop emulates the requested fabric
-	 * ProtocolID using target_core_fabric_lib.c symbols.
-	 */
-	switch (tl_hba->tl_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_fabric_proto_ident(se_tpg);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_fabric_proto_ident(se_tpg);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_fabric_proto_ident(se_tpg);
-	default:
-		pr_err("Unknown tl_proto_id: 0x%02x, using"
-			" SAS emulation\n", tl_hba->tl_proto_id);
-		break;
-	}
-
-	return sas_get_fabric_proto_ident(se_tpg);
+	return container_of(se_tpg, struct tcm_loop_tpg, tl_se_tpg);
 }
 
 static char *tcm_loop_get_endpoint_wwn(struct se_portal_group *se_tpg)
 {
-	struct tcm_loop_tpg *tl_tpg = se_tpg->se_tpg_fabric_ptr;
 	/*
 	 * Return the passed NAA identifier for the SAS Target Port
 	 */
-	return &tl_tpg->tl_hba->tl_wwn_address[0];
+	return &tl_tpg(se_tpg)->tl_hba->tl_wwn_address[0];
 }
 
 static u16 tcm_loop_get_tag(struct se_portal_group *se_tpg)
 {
-	struct tcm_loop_tpg *tl_tpg = se_tpg->se_tpg_fabric_ptr;
 	/*
 	 * This Tag is used when forming SCSI Name identifier in EVPD=1 0x83
 	 * to represent the SCSI Target Port.
 	 */
-	return tl_tpg->tl_tpgt;
-}
-
-static u32 tcm_loop_get_default_depth(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static u32 tcm_loop_get_pr_transport_id(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code,
-	unsigned char *buf)
-{
-	struct tcm_loop_tpg *tl_tpg = se_tpg->se_tpg_fabric_ptr;
-	struct tcm_loop_hba *tl_hba = tl_tpg->tl_hba;
-
-	switch (tl_hba->tl_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	default:
-		pr_err("Unknown tl_proto_id: 0x%02x, using"
-			" SAS emulation\n", tl_hba->tl_proto_id);
-		break;
-	}
-
-	return sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-			format_code, buf);
-}
-
-static u32 tcm_loop_get_pr_transport_id_len(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code)
-{
-	struct tcm_loop_tpg *tl_tpg = se_tpg->se_tpg_fabric_ptr;
-	struct tcm_loop_hba *tl_hba = tl_tpg->tl_hba;
-
-	switch (tl_hba->tl_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	default:
-		pr_err("Unknown tl_proto_id: 0x%02x, using"
-			" SAS emulation\n", tl_hba->tl_proto_id);
-		break;
-	}
-
-	return sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-			format_code);
-}
-
-/*
- * Used for handling SCSI fabric dependent TransportIDs in SPC-3 and above
- * Persistent Reservation SPEC_I_PT=1 and PROUT REGISTER_AND_MOVE operations.
- */
-static char *tcm_loop_parse_pr_out_transport_id(
-	struct se_portal_group *se_tpg,
-	const char *buf,
-	u32 *out_tid_len,
-	char **port_nexus_ptr)
-{
-	struct tcm_loop_tpg *tl_tpg = se_tpg->se_tpg_fabric_ptr;
-	struct tcm_loop_hba *tl_hba = tl_tpg->tl_hba;
-
-	switch (tl_hba->tl_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	case SCSI_PROTOCOL_FCP:
-		return fc_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	default:
-		pr_err("Unknown tl_proto_id: 0x%02x, using"
-			" SAS emulation\n", tl_hba->tl_proto_id);
-		break;
-	}
-
-	return sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-			port_nexus_ptr);
+	return tl_tpg(se_tpg)->tl_tpgt;
 }
 
 /*
@@ -703,30 +580,6 @@ static int tcm_loop_check_prot_fabric_only(struct se_portal_group *se_tpg)
 	return tl_tpg->tl_fabric_prot_type;
 }
 
-static struct se_node_acl *tcm_loop_tpg_alloc_fabric_acl(
-	struct se_portal_group *se_tpg)
-{
-	struct tcm_loop_nacl *tl_nacl;
-
-	tl_nacl = kzalloc(sizeof(struct tcm_loop_nacl), GFP_KERNEL);
-	if (!tl_nacl) {
-		pr_err("Unable to allocate struct tcm_loop_nacl\n");
-		return NULL;
-	}
-
-	return &tl_nacl->se_node_acl;
-}
-
-static void tcm_loop_tpg_release_fabric_acl(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl)
-{
-	struct tcm_loop_nacl *tl_nacl = container_of(se_nacl,
-				struct tcm_loop_nacl, se_node_acl);
-
-	kfree(tl_nacl);
-}
-
 static u32 tcm_loop_get_inst_index(struct se_portal_group *se_tpg)
 {
 	return 1;
@@ -742,14 +595,6 @@ static void tcm_loop_set_default_node_attributes(struct se_node_acl *se_acl)
 	return;
 }
 
-static u32 tcm_loop_get_task_tag(struct se_cmd *se_cmd)
-{
-	struct tcm_loop_cmd *tl_cmd = container_of(se_cmd,
-			struct tcm_loop_cmd, tl_se_cmd);
-
-	return tl_cmd->sc_cmd_tag;
-}
-
 static int tcm_loop_get_cmd_state(struct se_cmd *se_cmd)
 {
 	struct tcm_loop_cmd *tl_cmd = container_of(se_cmd,
@@ -902,7 +747,7 @@ static void tcm_loop_port_unlink(
 				se_lun->unpacked_lun);
 	if (!sd) {
 		pr_err("Unable to locate struct scsi_device for %d:%d:"
-			"%d\n", 0, tl_tpg->tl_tpgt, se_lun->unpacked_lun);
+			"%llu\n", 0, tl_tpg->tl_tpgt, se_lun->unpacked_lun);
 		return;
 	}
 	/*
@@ -1234,8 +1079,7 @@ static struct se_portal_group *tcm_loop_make_naa_tpg(
 	/*
 	 * Register the tl_tpg as a emulated SAS TCM Target Endpoint
 	 */
-	ret = core_tpg_register(&loop_ops, wwn, &tl_tpg->tl_se_tpg, tl_tpg,
-			TRANSPORT_TPG_TYPE_NORMAL);
+	ret = core_tpg_register(wwn, &tl_tpg->tl_se_tpg, tl_hba->tl_proto_id);
 	if (ret < 0)
 		return ERR_PTR(-ENOMEM);
 
@@ -1386,13 +1230,8 @@ static const struct target_core_fabric_ops loop_ops = {
 	.module				= THIS_MODULE,
 	.name				= "loopback",
 	.get_fabric_name		= tcm_loop_get_fabric_name,
-	.get_fabric_proto_ident		= tcm_loop_get_fabric_proto_ident,
 	.tpg_get_wwn			= tcm_loop_get_endpoint_wwn,
 	.tpg_get_tag			= tcm_loop_get_tag,
-	.tpg_get_default_depth		= tcm_loop_get_default_depth,
-	.tpg_get_pr_transport_id	= tcm_loop_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= tcm_loop_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= tcm_loop_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= tcm_loop_check_demo_mode,
 	.tpg_check_demo_mode_cache	= tcm_loop_check_demo_mode_cache,
 	.tpg_check_demo_mode_write_protect =
@@ -1400,8 +1239,6 @@ static const struct target_core_fabric_ops loop_ops = {
 	.tpg_check_prod_mode_write_protect =
 				tcm_loop_check_prod_mode_write_protect,
 	.tpg_check_prot_fabric_only	= tcm_loop_check_prot_fabric_only,
-	.tpg_alloc_fabric_acl		= tcm_loop_tpg_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= tcm_loop_tpg_release_fabric_acl,
 	.tpg_get_inst_index		= tcm_loop_get_inst_index,
 	.check_stop_free		= tcm_loop_check_stop_free,
 	.release_cmd			= tcm_loop_release_cmd,
@@ -1411,7 +1248,6 @@ static const struct target_core_fabric_ops loop_ops = {
 	.write_pending			= tcm_loop_write_pending,
 	.write_pending_status		= tcm_loop_write_pending_status,
 	.set_default_node_attributes	= tcm_loop_set_default_node_attributes,
-	.get_task_tag			= tcm_loop_get_task_tag,
 	.get_cmd_state			= tcm_loop_get_cmd_state,
 	.queue_data_in			= tcm_loop_queue_data_in,
 	.queue_status			= tcm_loop_queue_status,
diff --git a/drivers/target/loopback/tcm_loop.h b/drivers/target/loopback/tcm_loop.h
index 1e72ff77cac9..4346462094a1 100644
--- a/drivers/target/loopback/tcm_loop.h
+++ b/drivers/target/loopback/tcm_loop.h
@@ -2,11 +2,6 @@
 #define TL_WWN_ADDR_LEN			256
 #define TL_TPGS_PER_HBA			32
 
-/*
- * Used in tcm_loop_driver_probe() for struct Scsi_Host->max_cmd_len
- */
-#define TL_SCSI_MAX_CMD_LEN		32
-
 struct tcm_loop_cmd {
 	/* State of Linux/SCSI CDB+Data descriptor */
 	u32 sc_cmd_state;
@@ -33,10 +28,6 @@ struct tcm_loop_nexus {
 	struct se_session *se_sess;
 };
 
-struct tcm_loop_nacl {
-	struct se_node_acl se_node_acl;
-};
-
 #define TCM_TRANSPORT_ONLINE 0
 #define TCM_TRANSPORT_OFFLINE 1
 
diff --git a/drivers/target/sbp/sbp_target.c b/drivers/target/sbp/sbp_target.c
index 18b0f9703ff2..0edf320fb685 100644
--- a/drivers/target/sbp/sbp_target.c
+++ b/drivers/target/sbp/sbp_target.c
@@ -30,13 +30,12 @@
 #include <linux/ctype.h>
 #include <linux/firewire.h>
 #include <linux/firewire-constants.h>
-#include <scsi/scsi.h>
+#include <scsi/scsi_proto.h>
 #include <scsi/scsi_tcq.h>
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 #include <asm/unaligned.h>
 
@@ -109,13 +108,13 @@ static struct sbp_session *sbp_session_find_by_guid(
 }
 
 static struct sbp_login_descriptor *sbp_login_find_by_lun(
-		struct sbp_session *session, struct se_lun *lun)
+		struct sbp_session *session, u32 unpacked_lun)
 {
 	struct sbp_login_descriptor *login, *found = NULL;
 
 	spin_lock_bh(&session->lock);
 	list_for_each_entry(login, &session->login_list, link) {
-		if (login->lun == lun)
+		if (login->login_lun == unpacked_lun)
 			found = login;
 	}
 	spin_unlock_bh(&session->lock);
@@ -125,7 +124,7 @@ static struct sbp_login_descriptor *sbp_login_find_by_lun(
 
 static int sbp_login_count_all_by_lun(
 		struct sbp_tpg *tpg,
-		struct se_lun *lun,
+		u32 unpacked_lun,
 		int exclusive)
 {
 	struct se_session *se_sess;
@@ -139,7 +138,7 @@ static int sbp_login_count_all_by_lun(
 
 		spin_lock_bh(&sess->lock);
 		list_for_each_entry(login, &sess->login_list, link) {
-			if (login->lun != lun)
+			if (login->login_lun != unpacked_lun)
 				continue;
 
 			if (!exclusive || login->exclusive)
@@ -175,23 +174,23 @@ static struct sbp_login_descriptor *sbp_login_find_by_id(
 	return found;
 }
 
-static struct se_lun *sbp_get_lun_from_tpg(struct sbp_tpg *tpg, int lun)
+static u32 sbp_get_lun_from_tpg(struct sbp_tpg *tpg, u32 login_lun, int *err)
 {
 	struct se_portal_group *se_tpg = &tpg->se_tpg;
 	struct se_lun *se_lun;
 
-	if (lun >= TRANSPORT_MAX_LUNS_PER_TPG)
-		return ERR_PTR(-EINVAL);
-
-	spin_lock(&se_tpg->tpg_lun_lock);
-	se_lun = se_tpg->tpg_lun_list[lun];
-
-	if (se_lun->lun_status != TRANSPORT_LUN_STATUS_ACTIVE)
-		se_lun = ERR_PTR(-ENODEV);
-
-	spin_unlock(&se_tpg->tpg_lun_lock);
+	rcu_read_lock();
+	hlist_for_each_entry_rcu(se_lun, &se_tpg->tpg_lun_hlist, link) {
+		if (se_lun->unpacked_lun == login_lun) {
+			rcu_read_unlock();
+			*err = 0;
+			return login_lun;
+		}
+	}
+	rcu_read_unlock();
 
-	return se_lun;
+	*err = -ENODEV;
+	return login_lun;
 }
 
 static struct sbp_session *sbp_session_create(
@@ -295,17 +294,16 @@ static void sbp_management_request_login(
 {
 	struct sbp_tport *tport = agent->tport;
 	struct sbp_tpg *tpg = tport->tpg;
-	struct se_lun *se_lun;
-	int ret;
-	u64 guid;
 	struct sbp_session *sess;
 	struct sbp_login_descriptor *login;
 	struct sbp_login_response_block *response;
-	int login_response_len;
+	u64 guid;
+	u32 unpacked_lun;
+	int login_response_len, ret;
 
-	se_lun = sbp_get_lun_from_tpg(tpg,
-			LOGIN_ORB_LUN(be32_to_cpu(req->orb.misc)));
-	if (IS_ERR(se_lun)) {
+	unpacked_lun = sbp_get_lun_from_tpg(tpg,
+			LOGIN_ORB_LUN(be32_to_cpu(req->orb.misc)), &ret);
+	if (ret) {
 		pr_notice("login to unknown LUN: %d\n",
 			LOGIN_ORB_LUN(be32_to_cpu(req->orb.misc)));
 
@@ -326,11 +324,11 @@ static void sbp_management_request_login(
 	}
 
 	pr_notice("mgt_agent LOGIN to LUN %d from %016llx\n",
-		se_lun->unpacked_lun, guid);
+		unpacked_lun, guid);
 
 	sess = sbp_session_find_by_guid(tpg, guid);
 	if (sess) {
-		login = sbp_login_find_by_lun(sess, se_lun);
+		login = sbp_login_find_by_lun(sess, unpacked_lun);
 		if (login) {
 			pr_notice("initiator already logged-in\n");
 
@@ -358,7 +356,7 @@ static void sbp_management_request_login(
 	 * reject with access_denied if any logins present
 	 */
 	if (LOGIN_ORB_EXCLUSIVE(be32_to_cpu(req->orb.misc)) &&
-			sbp_login_count_all_by_lun(tpg, se_lun, 0)) {
+			sbp_login_count_all_by_lun(tpg, unpacked_lun, 0)) {
 		pr_warn("refusing exclusive login with other active logins\n");
 
 		req->status.status = cpu_to_be32(
@@ -371,7 +369,7 @@ static void sbp_management_request_login(
 	 * check exclusive bit in any existing login descriptor
 	 * reject with access_denied if any exclusive logins present
 	 */
-	if (sbp_login_count_all_by_lun(tpg, se_lun, 1)) {
+	if (sbp_login_count_all_by_lun(tpg, unpacked_lun, 1)) {
 		pr_warn("refusing login while another exclusive login present\n");
 
 		req->status.status = cpu_to_be32(
@@ -384,7 +382,7 @@ static void sbp_management_request_login(
 	 * check we haven't exceeded the number of allowed logins
 	 * reject with resources_unavailable if we have
 	 */
-	if (sbp_login_count_all_by_lun(tpg, se_lun, 0) >=
+	if (sbp_login_count_all_by_lun(tpg, unpacked_lun, 0) >=
 			tport->max_logins_per_lun) {
 		pr_warn("max number of logins reached\n");
 
@@ -440,7 +438,7 @@ static void sbp_management_request_login(
 	}
 
 	login->sess = sess;
-	login->lun = se_lun;
+	login->login_lun = unpacked_lun;
 	login->status_fifo_addr = sbp2_pointer_to_addr(&req->orb.status_fifo);
 	login->exclusive = LOGIN_ORB_EXCLUSIVE(be32_to_cpu(req->orb.misc));
 	login->login_id = atomic_inc_return(&login_id);
@@ -602,7 +600,7 @@ static void sbp_management_request_logout(
 	}
 
 	pr_info("mgt_agent LOGOUT from LUN %d session %d\n",
-		login->lun->unpacked_lun, login->login_id);
+		login->login_lun, login->login_id);
 
 	if (req->node_addr != login->sess->node_id) {
 		pr_warn("logout from different node ID\n");
@@ -1228,12 +1226,14 @@ static void sbp_handle_command(struct sbp_target_request *req)
 		goto err;
 	}
 
-	unpacked_lun = req->login->lun->unpacked_lun;
+	unpacked_lun = req->login->login_lun;
 	sbp_calc_data_length_direction(req, &data_length, &data_dir);
 
 	pr_debug("sbp_handle_command ORB:0x%llx unpacked_lun:%d data_len:%d data_dir:%d\n",
 			req->orb_pointer, unpacked_lun, data_length, data_dir);
 
+	/* only used for printk until we do TMRs */
+	req->se_cmd.tag = req->orb_pointer;
 	if (target_submit_cmd(&req->se_cmd, sess->se_sess, req->cmd_buf,
 			      req->sense_buf, unpacked_lun, data_length,
 			      TCM_SIMPLE_TAG, data_dir, 0))
@@ -1707,33 +1707,6 @@ static u16 sbp_get_tag(struct se_portal_group *se_tpg)
 	return tpg->tport_tpgt;
 }
 
-static u32 sbp_get_default_depth(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static struct se_node_acl *sbp_alloc_fabric_acl(struct se_portal_group *se_tpg)
-{
-	struct sbp_nacl *nacl;
-
-	nacl = kzalloc(sizeof(struct sbp_nacl), GFP_KERNEL);
-	if (!nacl) {
-		pr_err("Unable to allocate struct sbp_nacl\n");
-		return NULL;
-	}
-
-	return &nacl->se_node_acl;
-}
-
-static void sbp_release_fabric_acl(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl)
-{
-	struct sbp_nacl *nacl =
-		container_of(se_nacl, struct sbp_nacl, se_node_acl);
-	kfree(nacl);
-}
-
 static u32 sbp_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
 	return 1;
@@ -1795,15 +1768,6 @@ static void sbp_set_default_node_attrs(struct se_node_acl *nacl)
 	return;
 }
 
-static u32 sbp_get_task_tag(struct se_cmd *se_cmd)
-{
-	struct sbp_target_request *req = container_of(se_cmd,
-			struct sbp_target_request, se_cmd);
-
-	/* only used for printk until we do TMRs */
-	return (u32)req->orb_pointer;
-}
-
 static int sbp_get_cmd_state(struct se_cmd *se_cmd)
 {
 	return 0;
@@ -1859,106 +1823,23 @@ static int sbp_check_stop_free(struct se_cmd *se_cmd)
 	return 1;
 }
 
-/*
- * Handlers for Serial Bus Protocol 2/3 (SBP-2 / SBP-3)
- */
-static u8 sbp_get_fabric_proto_ident(struct se_portal_group *se_tpg)
-{
-	/*
-	 * Return a IEEE 1394 SCSI Protocol identifier for loopback operations
-	 * This is defined in section 7.5.1 Table 362 in spc4r17
-	 */
-	return SCSI_PROTOCOL_SBP;
-}
-
-static u32 sbp_get_pr_transport_id(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code,
-	unsigned char *buf)
-{
-	int ret;
-
-	/*
-	 * Set PROTOCOL IDENTIFIER to 3h for SBP
-	 */
-	buf[0] = SCSI_PROTOCOL_SBP;
-	/*
-	 * From spc4r17, 7.5.4.4 TransportID for initiator ports using SCSI
-	 * over IEEE 1394
-	 */
-	ret = hex2bin(&buf[8], se_nacl->initiatorname, 8);
-	if (ret < 0)
-		pr_debug("sbp transport_id: invalid hex string\n");
-
-	/*
-	 * The IEEE 1394 Transport ID is a hardcoded 24-byte length
-	 */
-	return 24;
-}
-
-static u32 sbp_get_pr_transport_id_len(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code)
-{
-	*format_code = 0;
-	/*
-	 * From spc4r17, 7.5.4.4 TransportID for initiator ports using SCSI
-	 * over IEEE 1394
-	 *
-	 * The SBP Transport ID is a hardcoded 24-byte length
-	 */
-	return 24;
-}
-
-/*
- * Used for handling SCSI fabric dependent TransportIDs in SPC-3 and above
- * Persistent Reservation SPEC_I_PT=1 and PROUT REGISTER_AND_MOVE operations.
- */
-static char *sbp_parse_pr_out_transport_id(
-	struct se_portal_group *se_tpg,
-	const char *buf,
-	u32 *out_tid_len,
-	char **port_nexus_ptr)
-{
-	/*
-	 * Assume the FORMAT CODE 00b from spc4r17, 7.5.4.4 TransportID
-	 * for initiator ports using SCSI over SBP Serial SCSI Protocol
-	 *
-	 * The TransportID for a IEEE 1394 Initiator Port is of fixed size of
-	 * 24 bytes, and IEEE 1394 does not contain a I_T nexus identifier,
-	 * so we return the **port_nexus_ptr set to NULL.
-	 */
-	*port_nexus_ptr = NULL;
-	*out_tid_len = 24;
-
-	return (char *)&buf[8];
-}
-
 static int sbp_count_se_tpg_luns(struct se_portal_group *tpg)
 {
-	int i, count = 0;
-
-	spin_lock(&tpg->tpg_lun_lock);
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		struct se_lun *se_lun = tpg->tpg_lun_list[i];
-
-		if (se_lun->lun_status == TRANSPORT_LUN_STATUS_FREE)
-			continue;
+	struct se_lun *lun;
+	int count = 0;
 
+	rcu_read_lock();
+	hlist_for_each_entry_rcu(lun, &tpg->tpg_lun_hlist, link)
 		count++;
-	}
-	spin_unlock(&tpg->tpg_lun_lock);
+	rcu_read_unlock();
 
 	return count;
 }
 
 static int sbp_update_unit_directory(struct sbp_tport *tport)
 {
-	int num_luns, num_entries, idx = 0, mgt_agt_addr, ret, i;
+	struct se_lun *lun;
+	int num_luns, num_entries, idx = 0, mgt_agt_addr, ret;
 	u32 *data;
 
 	if (tport->unit_directory.data) {
@@ -2020,28 +1901,23 @@ static int sbp_update_unit_directory(struct sbp_tport *tport)
 	/* unit unique ID (leaf is just after LUNs) */
 	data[idx++] = 0x8d000000 | (num_luns + 1);
 
-	spin_lock(&tport->tpg->se_tpg.tpg_lun_lock);
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		struct se_lun *se_lun = tport->tpg->se_tpg.tpg_lun_list[i];
+	rcu_read_lock();
+	hlist_for_each_entry_rcu(lun, &tport->tpg->se_tpg.tpg_lun_hlist, link) {
 		struct se_device *dev;
 		int type;
-
-		if (se_lun->lun_status == TRANSPORT_LUN_STATUS_FREE)
-			continue;
-
-		spin_unlock(&tport->tpg->se_tpg.tpg_lun_lock);
-
-		dev = se_lun->lun_se_dev;
+		/*
+		 * rcu_dereference_raw protected by se_lun->lun_group symlink
+		 * reference to se_device->dev_group.
+		 */
+		dev = rcu_dereference_raw(lun->lun_se_dev);
 		type = dev->transport->get_device_type(dev);
 
 		/* logical_unit_number */
 		data[idx++] = 0x14000000 |
 			((type << 16) & 0x1f0000) |
-			(se_lun->unpacked_lun & 0xffff);
-
-		spin_lock(&tport->tpg->se_tpg.tpg_lun_lock);
+			(lun->unpacked_lun & 0xffff);
 	}
-	spin_unlock(&tport->tpg->se_tpg.tpg_lun_lock);
+	rcu_read_unlock();
 
 	/* unit unique ID leaf */
 	data[idx++] = 2 << 16;
@@ -2100,48 +1976,13 @@ static ssize_t sbp_format_wwn(char *buf, size_t len, u64 wwn)
 	return snprintf(buf, len, "%016llx", wwn);
 }
 
-static struct se_node_acl *sbp_make_nodeacl(
-		struct se_portal_group *se_tpg,
-		struct config_group *group,
-		const char *name)
+static int sbp_init_nodeacl(struct se_node_acl *se_nacl, const char *name)
 {
-	struct se_node_acl *se_nacl, *se_nacl_new;
-	struct sbp_nacl *nacl;
 	u64 guid = 0;
-	u32 nexus_depth = 1;
 
 	if (sbp_parse_wwn(name, &guid) < 0)
-		return ERR_PTR(-EINVAL);
-
-	se_nacl_new = sbp_alloc_fabric_acl(se_tpg);
-	if (!se_nacl_new)
-		return ERR_PTR(-ENOMEM);
-
-	/*
-	 * se_nacl_new may be released by core_tpg_add_initiator_node_acl()
-	 * when converting a NodeACL from demo mode -> explict
-	 */
-	se_nacl = core_tpg_add_initiator_node_acl(se_tpg, se_nacl_new,
-			name, nexus_depth);
-	if (IS_ERR(se_nacl)) {
-		sbp_release_fabric_acl(se_tpg, se_nacl_new);
-		return se_nacl;
-	}
-
-	nacl = container_of(se_nacl, struct sbp_nacl, se_node_acl);
-	nacl->guid = guid;
-	sbp_format_wwn(nacl->iport_name, SBP_NAMELEN, guid);
-
-	return se_nacl;
-}
-
-static void sbp_drop_nodeacl(struct se_node_acl *se_acl)
-{
-	struct sbp_nacl *nacl =
-		container_of(se_acl, struct sbp_nacl, se_node_acl);
-
-	core_tpg_del_initiator_node_acl(se_acl->se_tpg, se_acl, 1);
-	kfree(nacl);
+		return -EINVAL;
+	return 0;
 }
 
 static int sbp_post_link_lun(
@@ -2214,8 +2055,7 @@ static struct se_portal_group *sbp_make_tpg(
 		goto out_free_tpg;
 	}
 
-	ret = core_tpg_register(&sbp_ops, wwn, &tpg->se_tpg, tpg,
-			TRANSPORT_TPG_TYPE_NORMAL);
+	ret = core_tpg_register(wwn, &tpg->se_tpg, SCSI_PROTOCOL_SBP);
 	if (ret < 0)
 		goto out_unreg_mgt_agt;
 
@@ -2505,19 +2345,12 @@ static const struct target_core_fabric_ops sbp_ops = {
 	.module				= THIS_MODULE,
 	.name				= "sbp",
 	.get_fabric_name		= sbp_get_fabric_name,
-	.get_fabric_proto_ident		= sbp_get_fabric_proto_ident,
 	.tpg_get_wwn			= sbp_get_fabric_wwn,
 	.tpg_get_tag			= sbp_get_tag,
-	.tpg_get_default_depth		= sbp_get_default_depth,
-	.tpg_get_pr_transport_id	= sbp_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= sbp_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= sbp_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= sbp_check_true,
 	.tpg_check_demo_mode_cache	= sbp_check_true,
 	.tpg_check_demo_mode_write_protect = sbp_check_false,
 	.tpg_check_prod_mode_write_protect = sbp_check_false,
-	.tpg_alloc_fabric_acl		= sbp_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= sbp_release_fabric_acl,
 	.tpg_get_inst_index		= sbp_tpg_get_inst_index,
 	.release_cmd			= sbp_release_cmd,
 	.shutdown_session		= sbp_shutdown_session,
@@ -2526,7 +2359,6 @@ static const struct target_core_fabric_ops sbp_ops = {
 	.write_pending			= sbp_write_pending,
 	.write_pending_status		= sbp_write_pending_status,
 	.set_default_node_attributes	= sbp_set_default_node_attrs,
-	.get_task_tag			= sbp_get_task_tag,
 	.get_cmd_state			= sbp_get_cmd_state,
 	.queue_data_in			= sbp_queue_data_in,
 	.queue_status			= sbp_queue_status,
@@ -2542,8 +2374,7 @@ static const struct target_core_fabric_ops sbp_ops = {
 	.fabric_pre_unlink		= sbp_pre_unlink_lun,
 	.fabric_make_np			= NULL,
 	.fabric_drop_np			= NULL,
-	.fabric_make_nodeacl		= sbp_make_nodeacl,
-	.fabric_drop_nodeacl		= sbp_drop_nodeacl,
+	.fabric_init_nodeacl		= sbp_init_nodeacl,
 
 	.tfc_wwn_attrs			= sbp_wwn_attrs,
 	.tfc_tpg_base_attrs		= sbp_tpg_base_attrs,
diff --git a/drivers/target/sbp/sbp_target.h b/drivers/target/sbp/sbp_target.h
index 6d0d74a2c545..73bcb1208832 100644
--- a/drivers/target/sbp/sbp_target.h
+++ b/drivers/target/sbp/sbp_target.h
@@ -125,7 +125,7 @@ struct sbp_login_descriptor {
 	struct sbp_session *sess;
 	struct list_head link;
 
-	struct se_lun *lun;
+	u32 login_lun;
 
 	u64 status_fifo_addr;
 	int exclusive;
@@ -151,15 +151,6 @@ struct sbp_session {
 	u64 reconnect_expires;
 };
 
-struct sbp_nacl {
-	/* Initiator EUI-64 */
-	u64 guid;
-	/* ASCII formatted GUID for SBP Initiator port */
-	char iport_name[SBP_NAMELEN];
-	/* Returned by sbp_make_nodeacl() */
-	struct se_node_acl se_node_acl;
-};
-
 struct sbp_tpg {
 	/* Target portal group tag for TCM */
 	u16 tport_tpgt;
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index 4f8d4d459aa4..49aba4a31747 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -28,14 +28,12 @@
 #include <linux/configfs.h>
 #include <linux/export.h>
 #include <linux/file.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_proto.h>
 #include <asm/unaligned.h>
 
 #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"
 #include "target_core_alua.h"
@@ -44,11 +42,13 @@
 static sense_reason_t core_alua_check_transition(int state, int valid,
 						 int *primary);
 static int core_alua_set_tg_pt_secondary_state(
-		struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem,
-		struct se_port *port, int explicit, int offline);
+		struct se_lun *lun, int explicit, int offline);
 
 static char *core_alua_dump_state(int state);
 
+static void __target_attach_tg_pt_gp(struct se_lun *lun,
+		struct t10_alua_tg_pt_gp *tg_pt_gp);
+
 static u16 alua_lu_gps_counter;
 static u32 alua_lu_gps_count;
 
@@ -146,9 +146,8 @@ sense_reason_t
 target_emulate_report_target_port_groups(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
-	struct se_port *port;
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
+	struct se_lun *lun;
 	unsigned char *buf;
 	u32 rd_len = 0, off;
 	int ext_hdr = (cmd->t_task_cdb[1] & 0x20);
@@ -223,9 +222,8 @@ target_emulate_report_target_port_groups(struct se_cmd *cmd)
 		rd_len += 8;
 
 		spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-		list_for_each_entry(tg_pt_gp_mem, &tg_pt_gp->tg_pt_gp_mem_list,
-				tg_pt_gp_mem_list) {
-			port = tg_pt_gp_mem->tg_pt;
+		list_for_each_entry(lun, &tg_pt_gp->tg_pt_gp_lun_list,
+				lun_tg_pt_gp_link) {
 			/*
 			 * Start Target Port descriptor format
 			 *
@@ -235,8 +233,8 @@ target_emulate_report_target_port_groups(struct se_cmd *cmd)
 			/*
 			 * Set RELATIVE TARGET PORT IDENTIFIER
 			 */
-			buf[off++] = ((port->sep_rtpi >> 8) & 0xff);
-			buf[off++] = (port->sep_rtpi & 0xff);
+			buf[off++] = ((lun->lun_rtpi >> 8) & 0xff);
+			buf[off++] = (lun->lun_rtpi & 0xff);
 			rd_len += 4;
 		}
 		spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
@@ -260,15 +258,11 @@ target_emulate_report_target_port_groups(struct se_cmd *cmd)
 		 * this CDB was received upon to determine this value individually
 		 * for ALUA target port group.
 		 */
-		port = cmd->se_lun->lun_sep;
-		tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
-		if (tg_pt_gp_mem) {
-			spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-			tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
-			if (tg_pt_gp)
-				buf[5] = tg_pt_gp->tg_pt_gp_implicit_trans_secs;
-			spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-		}
+		spin_lock(&cmd->se_lun->lun_tg_pt_gp_lock);
+		tg_pt_gp = cmd->se_lun->lun_tg_pt_gp;
+		if (tg_pt_gp)
+			buf[5] = tg_pt_gp->tg_pt_gp_implicit_trans_secs;
+		spin_unlock(&cmd->se_lun->lun_tg_pt_gp_lock);
 	}
 	transport_kunmap_data_sg(cmd);
 
@@ -285,10 +279,9 @@ sense_reason_t
 target_emulate_set_target_port_groups(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
-	struct se_port *port, *l_port = cmd->se_lun->lun_sep;
+	struct se_lun *l_lun = cmd->se_lun;
 	struct se_node_acl *nacl = cmd->se_sess->se_node_acl;
 	struct t10_alua_tg_pt_gp *tg_pt_gp = NULL, *l_tg_pt_gp;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem, *l_tg_pt_gp_mem;
 	unsigned char *buf;
 	unsigned char *ptr;
 	sense_reason_t rc = TCM_NO_SENSE;
@@ -296,9 +289,6 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 	int alua_access_state, primary = 0, valid_states;
 	u16 tg_pt_id, rtpi;
 
-	if (!l_port)
-		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-
 	if (cmd->data_length < 4) {
 		pr_warn("SET TARGET PORT GROUPS parameter list length %u too"
 			" small\n", cmd->data_length);
@@ -313,29 +303,24 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 	 * Determine if explicit ALUA via SET_TARGET_PORT_GROUPS is allowed
 	 * for the local tg_pt_gp.
 	 */
-	l_tg_pt_gp_mem = l_port->sep_alua_tg_pt_gp_mem;
-	if (!l_tg_pt_gp_mem) {
-		pr_err("Unable to access l_port->sep_alua_tg_pt_gp_mem\n");
-		rc = TCM_UNSUPPORTED_SCSI_OPCODE;
-		goto out;
-	}
-	spin_lock(&l_tg_pt_gp_mem->tg_pt_gp_mem_lock);
-	l_tg_pt_gp = l_tg_pt_gp_mem->tg_pt_gp;
+	spin_lock(&l_lun->lun_tg_pt_gp_lock);
+	l_tg_pt_gp = l_lun->lun_tg_pt_gp;
 	if (!l_tg_pt_gp) {
-		spin_unlock(&l_tg_pt_gp_mem->tg_pt_gp_mem_lock);
-		pr_err("Unable to access *l_tg_pt_gp_mem->tg_pt_gp\n");
+		spin_unlock(&l_lun->lun_tg_pt_gp_lock);
+		pr_err("Unable to access l_lun->tg_pt_gp\n");
 		rc = TCM_UNSUPPORTED_SCSI_OPCODE;
 		goto out;
 	}
-	spin_unlock(&l_tg_pt_gp_mem->tg_pt_gp_mem_lock);
 
 	if (!(l_tg_pt_gp->tg_pt_gp_alua_access_type & TPGS_EXPLICIT_ALUA)) {
+		spin_unlock(&l_lun->lun_tg_pt_gp_lock);
 		pr_debug("Unable to process SET_TARGET_PORT_GROUPS"
 				" while TPGS_EXPLICIT_ALUA is disabled\n");
 		rc = TCM_UNSUPPORTED_SCSI_OPCODE;
 		goto out;
 	}
 	valid_states = l_tg_pt_gp->tg_pt_gp_alua_supported_states;
+	spin_unlock(&l_lun->lun_tg_pt_gp_lock);
 
 	ptr = &buf[4]; /* Skip over RESERVED area in header */
 
@@ -397,7 +382,7 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 				spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
 				if (!core_alua_do_port_transition(tg_pt_gp,
-						dev, l_port, nacl,
+						dev, l_lun, nacl,
 						alua_access_state, 1))
 					found = true;
 
@@ -407,6 +392,8 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 			}
 			spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 		} else {
+			struct se_lun *lun;
+
 			/*
 			 * Extract the RELATIVE TARGET PORT IDENTIFIER to identify
 			 * the Target Port in question for the the incoming
@@ -418,17 +405,16 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 			 * for the struct se_device storage object.
 			 */
 			spin_lock(&dev->se_port_lock);
-			list_for_each_entry(port, &dev->dev_sep_list,
-							sep_list) {
-				if (port->sep_rtpi != rtpi)
+			list_for_each_entry(lun, &dev->dev_sep_list,
+							lun_dev_link) {
+				if (lun->lun_rtpi != rtpi)
 					continue;
 
-				tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
-
+				// XXX: racy unlock
 				spin_unlock(&dev->se_port_lock);
 
 				if (!core_alua_set_tg_pt_secondary_state(
-						tg_pt_gp_mem, port, 1, 1))
+						lun, 1, 1))
 					found = true;
 
 				spin_lock(&dev->se_port_lock);
@@ -697,9 +683,7 @@ target_alua_state_check(struct se_cmd *cmd)
 	struct se_device *dev = cmd->se_dev;
 	unsigned char *cdb = cmd->t_task_cdb;
 	struct se_lun *lun = cmd->se_lun;
-	struct se_port *port = lun->lun_sep;
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
 	int out_alua_state, nonop_delay_msecs;
 
 	if (dev->se_hba->hba_flags & HBA_FLAGS_INTERNAL_USE)
@@ -707,33 +691,27 @@ target_alua_state_check(struct se_cmd *cmd)
 	if (dev->transport->transport_flags & TRANSPORT_FLAG_PASSTHROUGH)
 		return 0;
 
-	if (!port)
-		return 0;
 	/*
 	 * First, check for a struct se_port specific secondary ALUA target port
 	 * access state: OFFLINE
 	 */
-	if (atomic_read(&port->sep_tg_pt_secondary_offline)) {
+	if (atomic_read(&lun->lun_tg_pt_secondary_offline)) {
 		pr_debug("ALUA: Got secondary offline status for local"
 				" target port\n");
 		set_ascq(cmd, ASCQ_04H_ALUA_OFFLINE);
 		return TCM_CHECK_CONDITION_NOT_READY;
 	}
-	 /*
-	 * Second, obtain the struct t10_alua_tg_pt_gp_member pointer to the
-	 * ALUA target port group, to obtain current ALUA access state.
-	 * Otherwise look for the underlying struct se_device association with
-	 * a ALUA logical unit group.
-	 */
-	tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
-	if (!tg_pt_gp_mem)
+
+	if (!lun->lun_tg_pt_gp)
 		return 0;
 
-	spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-	tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
+	spin_lock(&lun->lun_tg_pt_gp_lock);
+	tg_pt_gp = lun->lun_tg_pt_gp;
 	out_alua_state = atomic_read(&tg_pt_gp->tg_pt_gp_alua_access_state);
 	nonop_delay_msecs = tg_pt_gp->tg_pt_gp_nonop_delay_msecs;
-	spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+
+	// XXX: keeps using tg_pt_gp witout reference after unlock
+	spin_unlock(&lun->lun_tg_pt_gp_lock);
 	/*
 	 * Process ALUA_ACCESS_STATE_ACTIVE_OPTIMIZED in a separate conditional
 	 * statement so the compiler knows explicitly to check this case first.
@@ -765,7 +743,7 @@ target_alua_state_check(struct se_cmd *cmd)
 		break;
 	/*
 	 * OFFLINE is a secondary ALUA target port group access state, that is
-	 * handled above with struct se_port->sep_tg_pt_secondary_offline=1
+	 * handled above with struct se_lun->lun_tg_pt_secondary_offline=1
 	 */
 	case ALUA_ACCESS_STATE_OFFLINE:
 	default:
@@ -907,10 +885,6 @@ int core_alua_check_nonop_delay(
 }
 EXPORT_SYMBOL(core_alua_check_nonop_delay);
 
-/*
- * Called with tg_pt_gp->tg_pt_gp_md_mutex or tg_pt_gp_mem->sep_tg_pt_md_mutex
- *
- */
 static int core_alua_write_tpg_metadata(
 	const char *path,
 	unsigned char *md_buf,
@@ -966,22 +940,15 @@ static int core_alua_update_tpg_primary_metadata(
 	return rc;
 }
 
-static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
+static void core_alua_queue_state_change_ua(struct t10_alua_tg_pt_gp *tg_pt_gp)
 {
-	struct t10_alua_tg_pt_gp *tg_pt_gp = container_of(work,
-		struct t10_alua_tg_pt_gp, tg_pt_gp_transition_work.work);
-	struct se_device *dev = tg_pt_gp->tg_pt_gp_dev;
 	struct se_dev_entry *se_deve;
+	struct se_lun *lun;
 	struct se_lun_acl *lacl;
-	struct se_port *port;
-	struct t10_alua_tg_pt_gp_member *mem;
-	bool explicit = (tg_pt_gp->tg_pt_gp_alua_access_status ==
-			 ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG);
 
 	spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-	list_for_each_entry(mem, &tg_pt_gp->tg_pt_gp_mem_list,
-				tg_pt_gp_mem_list) {
-		port = mem->tg_pt;
+	list_for_each_entry(lun, &tg_pt_gp->tg_pt_gp_lun_list,
+				lun_tg_pt_gp_link) {
 		/*
 		 * After an implicit target port asymmetric access state
 		 * change, a device server shall establish a unit attention
@@ -996,38 +963,58 @@ static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
 		 * every I_T nexus other than the I_T nexus on which the SET
 		 * TARGET PORT GROUPS command
 		 */
-		atomic_inc_mb(&mem->tg_pt_gp_mem_ref_cnt);
+		if (!percpu_ref_tryget_live(&lun->lun_ref))
+			continue;
 		spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
 
-		spin_lock_bh(&port->sep_alua_lock);
-		list_for_each_entry(se_deve, &port->sep_alua_list,
-					alua_port_list) {
-			lacl = se_deve->se_lun_acl;
+		spin_lock(&lun->lun_deve_lock);
+		list_for_each_entry(se_deve, &lun->lun_deve_list, lun_link) {
+			lacl = rcu_dereference_check(se_deve->se_lun_acl,
+					lockdep_is_held(&lun->lun_deve_lock));
+
 			/*
-			 * se_deve->se_lun_acl pointer may be NULL for a
-			 * entry created without explicit Node+MappedLUN ACLs
+			 * spc4r37 p.242:
+			 * After an explicit target port asymmetric access
+			 * state change, a device server shall establish a
+			 * unit attention condition with the additional sense
+			 * code set to ASYMMETRIC ACCESS STATE CHANGED for
+			 * the initiator port associated with every I_T nexus
+			 * other than the I_T nexus on which the SET TARGET
+			 * PORT GROUPS command was received.
 			 */
-			if (!lacl)
-				continue;
-
 			if ((tg_pt_gp->tg_pt_gp_alua_access_status ==
 			     ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG) &&
-			   (tg_pt_gp->tg_pt_gp_alua_nacl != NULL) &&
-			    (tg_pt_gp->tg_pt_gp_alua_nacl == lacl->se_lun_nacl) &&
-			   (tg_pt_gp->tg_pt_gp_alua_port != NULL) &&
-			    (tg_pt_gp->tg_pt_gp_alua_port == port))
+			   (tg_pt_gp->tg_pt_gp_alua_lun != NULL) &&
+			    (tg_pt_gp->tg_pt_gp_alua_lun == lun))
 				continue;
 
-			core_scsi3_ua_allocate(lacl->se_lun_nacl,
-				se_deve->mapped_lun, 0x2A,
+			/*
+			 * se_deve->se_lun_acl pointer may be NULL for a
+			 * entry created without explicit Node+MappedLUN ACLs
+			 */
+			if (lacl && (tg_pt_gp->tg_pt_gp_alua_nacl != NULL) &&
+			    (tg_pt_gp->tg_pt_gp_alua_nacl == lacl->se_lun_nacl))
+				continue;
+
+			core_scsi3_ua_allocate(se_deve, 0x2A,
 				ASCQ_2AH_ASYMMETRIC_ACCESS_STATE_CHANGED);
 		}
-		spin_unlock_bh(&port->sep_alua_lock);
+		spin_unlock(&lun->lun_deve_lock);
 
 		spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-		atomic_dec_mb(&mem->tg_pt_gp_mem_ref_cnt);
+		percpu_ref_put(&lun->lun_ref);
 	}
 	spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
+}
+
+static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
+{
+	struct t10_alua_tg_pt_gp *tg_pt_gp = container_of(work,
+		struct t10_alua_tg_pt_gp, tg_pt_gp_transition_work.work);
+	struct se_device *dev = tg_pt_gp->tg_pt_gp_dev;
+	bool explicit = (tg_pt_gp->tg_pt_gp_alua_access_status ==
+			 ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG);
+
 	/*
 	 * Update the ALUA metadata buf that has been allocated in
 	 * core_alua_do_port_transition(), this metadata will be written
@@ -1057,6 +1044,9 @@ static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
 		tg_pt_gp->tg_pt_gp_id,
 		core_alua_dump_state(tg_pt_gp->tg_pt_gp_alua_previous_state),
 		core_alua_dump_state(tg_pt_gp->tg_pt_gp_alua_pending_state));
+
+	core_alua_queue_state_change_ua(tg_pt_gp);
+
 	spin_lock(&dev->t10_alua.tg_pt_gps_lock);
 	atomic_dec(&tg_pt_gp->tg_pt_gp_ref_cnt);
 	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
@@ -1109,6 +1099,8 @@ static int core_alua_do_transition_tg_pt(
 				ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG :
 				ALUA_STATUS_ALTERED_BY_IMPLICIT_ALUA;
 
+	core_alua_queue_state_change_ua(tg_pt_gp);
+
 	/*
 	 * Check for the optional ALUA primary state transition delay
 	 */
@@ -1143,7 +1135,7 @@ static int core_alua_do_transition_tg_pt(
 int core_alua_do_port_transition(
 	struct t10_alua_tg_pt_gp *l_tg_pt_gp,
 	struct se_device *l_dev,
-	struct se_port *l_port,
+	struct se_lun *l_lun,
 	struct se_node_acl *l_nacl,
 	int new_state,
 	int explicit)
@@ -1173,7 +1165,7 @@ int core_alua_do_port_transition(
 		 * core_alua_do_transition_tg_pt() will always return
 		 * success.
 		 */
-		l_tg_pt_gp->tg_pt_gp_alua_port = l_port;
+		l_tg_pt_gp->tg_pt_gp_alua_lun = l_lun;
 		l_tg_pt_gp->tg_pt_gp_alua_nacl = l_nacl;
 		rc = core_alua_do_transition_tg_pt(l_tg_pt_gp,
 						   new_state, explicit);
@@ -1212,10 +1204,10 @@ int core_alua_do_port_transition(
 				continue;
 
 			if (l_tg_pt_gp == tg_pt_gp) {
-				tg_pt_gp->tg_pt_gp_alua_port = l_port;
+				tg_pt_gp->tg_pt_gp_alua_lun = l_lun;
 				tg_pt_gp->tg_pt_gp_alua_nacl = l_nacl;
 			} else {
-				tg_pt_gp->tg_pt_gp_alua_port = NULL;
+				tg_pt_gp->tg_pt_gp_alua_lun = NULL;
 				tg_pt_gp->tg_pt_gp_alua_nacl = NULL;
 			}
 			atomic_inc_mb(&tg_pt_gp->tg_pt_gp_ref_cnt);
@@ -1252,22 +1244,20 @@ int core_alua_do_port_transition(
 	return rc;
 }
 
-/*
- * Called with tg_pt_gp_mem->sep_tg_pt_md_mutex held
- */
-static int core_alua_update_tpg_secondary_metadata(
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem,
-	struct se_port *port)
+static int core_alua_update_tpg_secondary_metadata(struct se_lun *lun)
 {
+	struct se_portal_group *se_tpg = lun->lun_tpg;
 	unsigned char *md_buf;
-	struct se_portal_group *se_tpg = port->sep_tpg;
 	char path[ALUA_METADATA_PATH_LEN], wwn[ALUA_SECONDARY_METADATA_WWN_LEN];
 	int len, rc;
 
+	mutex_lock(&lun->lun_tg_pt_md_mutex);
+
 	md_buf = kzalloc(ALUA_MD_BUF_LEN, GFP_KERNEL);
 	if (!md_buf) {
 		pr_err("Unable to allocate buf for ALUA metadata\n");
-		return -ENOMEM;
+		rc = -ENOMEM;
+		goto out_unlock;
 	}
 
 	memset(path, 0, ALUA_METADATA_PATH_LEN);
@@ -1282,32 +1272,33 @@ static int core_alua_update_tpg_secondary_metadata(
 
 	len = snprintf(md_buf, ALUA_MD_BUF_LEN, "alua_tg_pt_offline=%d\n"
 			"alua_tg_pt_status=0x%02x\n",
-			atomic_read(&port->sep_tg_pt_secondary_offline),
-			port->sep_tg_pt_secondary_stat);
+			atomic_read(&lun->lun_tg_pt_secondary_offline),
+			lun->lun_tg_pt_secondary_stat);
 
-	snprintf(path, ALUA_METADATA_PATH_LEN, "/var/target/alua/%s/%s/lun_%u",
+	snprintf(path, ALUA_METADATA_PATH_LEN, "/var/target/alua/%s/%s/lun_%llu",
 			se_tpg->se_tpg_tfo->get_fabric_name(), wwn,
-			port->sep_lun->unpacked_lun);
+			lun->unpacked_lun);
 
 	rc = core_alua_write_tpg_metadata(path, md_buf, len);
 	kfree(md_buf);
 
+out_unlock:
+	mutex_unlock(&lun->lun_tg_pt_md_mutex);
 	return rc;
 }
 
 static int core_alua_set_tg_pt_secondary_state(
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem,
-	struct se_port *port,
+	struct se_lun *lun,
 	int explicit,
 	int offline)
 {
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
 	int trans_delay_msecs;
 
-	spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-	tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
+	spin_lock(&lun->lun_tg_pt_gp_lock);
+	tg_pt_gp = lun->lun_tg_pt_gp;
 	if (!tg_pt_gp) {
-		spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+		spin_unlock(&lun->lun_tg_pt_gp_lock);
 		pr_err("Unable to complete secondary state"
 				" transition\n");
 		return -EINVAL;
@@ -1315,14 +1306,14 @@ static int core_alua_set_tg_pt_secondary_state(
 	trans_delay_msecs = tg_pt_gp->tg_pt_gp_trans_delay_msecs;
 	/*
 	 * Set the secondary ALUA target port access state to OFFLINE
-	 * or release the previously secondary state for struct se_port
+	 * or release the previously secondary state for struct se_lun
 	 */
 	if (offline)
-		atomic_set(&port->sep_tg_pt_secondary_offline, 1);
+		atomic_set(&lun->lun_tg_pt_secondary_offline, 1);
 	else
-		atomic_set(&port->sep_tg_pt_secondary_offline, 0);
+		atomic_set(&lun->lun_tg_pt_secondary_offline, 0);
 
-	port->sep_tg_pt_secondary_stat = (explicit) ?
+	lun->lun_tg_pt_secondary_stat = (explicit) ?
 			ALUA_STATUS_ALTERED_BY_EXPLICIT_STPG :
 			ALUA_STATUS_ALTERED_BY_IMPLICIT_ALUA;
 
@@ -1331,7 +1322,7 @@ static int core_alua_set_tg_pt_secondary_state(
 		"implicit", config_item_name(&tg_pt_gp->tg_pt_gp_group.cg_item),
 		tg_pt_gp->tg_pt_gp_id, (offline) ? "OFFLINE" : "ONLINE");
 
-	spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+	spin_unlock(&lun->lun_tg_pt_gp_lock);
 	/*
 	 * Do the optional transition delay after we set the secondary
 	 * ALUA access state.
@@ -1342,11 +1333,8 @@ static int core_alua_set_tg_pt_secondary_state(
 	 * See if we need to update the ALUA fabric port metadata for
 	 * secondary state and status
 	 */
-	if (port->sep_tg_pt_secondary_write_md) {
-		mutex_lock(&port->sep_tg_pt_md_mutex);
-		core_alua_update_tpg_secondary_metadata(tg_pt_gp_mem, port);
-		mutex_unlock(&port->sep_tg_pt_md_mutex);
-	}
+	if (lun->lun_tg_pt_secondary_write_md)
+		core_alua_update_tpg_secondary_metadata(lun);
 
 	return 0;
 }
@@ -1700,7 +1688,7 @@ struct t10_alua_tg_pt_gp *core_alua_allocate_tg_pt_gp(struct se_device *dev,
 		return NULL;
 	}
 	INIT_LIST_HEAD(&tg_pt_gp->tg_pt_gp_list);
-	INIT_LIST_HEAD(&tg_pt_gp->tg_pt_gp_mem_list);
+	INIT_LIST_HEAD(&tg_pt_gp->tg_pt_gp_lun_list);
 	mutex_init(&tg_pt_gp->tg_pt_gp_md_mutex);
 	spin_lock_init(&tg_pt_gp->tg_pt_gp_lock);
 	atomic_set(&tg_pt_gp->tg_pt_gp_ref_cnt, 0);
@@ -1794,32 +1782,11 @@ again:
 	return 0;
 }
 
-struct t10_alua_tg_pt_gp_member *core_alua_allocate_tg_pt_gp_mem(
-	struct se_port *port)
-{
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
-
-	tg_pt_gp_mem = kmem_cache_zalloc(t10_alua_tg_pt_gp_mem_cache,
-				GFP_KERNEL);
-	if (!tg_pt_gp_mem) {
-		pr_err("Unable to allocate struct t10_alua_tg_pt_gp_member\n");
-		return ERR_PTR(-ENOMEM);
-	}
-	INIT_LIST_HEAD(&tg_pt_gp_mem->tg_pt_gp_mem_list);
-	spin_lock_init(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-	atomic_set(&tg_pt_gp_mem->tg_pt_gp_mem_ref_cnt, 0);
-
-	tg_pt_gp_mem->tg_pt = port;
-	port->sep_alua_tg_pt_gp_mem = tg_pt_gp_mem;
-
-	return tg_pt_gp_mem;
-}
-
 void core_alua_free_tg_pt_gp(
 	struct t10_alua_tg_pt_gp *tg_pt_gp)
 {
 	struct se_device *dev = tg_pt_gp->tg_pt_gp_dev;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem, *tg_pt_gp_mem_tmp;
+	struct se_lun *lun, *next;
 
 	/*
 	 * Once we have reached this point, config_item_put() has already
@@ -1850,30 +1817,24 @@ void core_alua_free_tg_pt_gp(
 	 * struct se_port.
 	 */
 	spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-	list_for_each_entry_safe(tg_pt_gp_mem, tg_pt_gp_mem_tmp,
-			&tg_pt_gp->tg_pt_gp_mem_list, tg_pt_gp_mem_list) {
-		if (tg_pt_gp_mem->tg_pt_gp_assoc) {
-			list_del(&tg_pt_gp_mem->tg_pt_gp_mem_list);
-			tg_pt_gp->tg_pt_gp_members--;
-			tg_pt_gp_mem->tg_pt_gp_assoc = 0;
-		}
+	list_for_each_entry_safe(lun, next,
+			&tg_pt_gp->tg_pt_gp_lun_list, lun_tg_pt_gp_link) {
+		list_del_init(&lun->lun_tg_pt_gp_link);
+		tg_pt_gp->tg_pt_gp_members--;
+
 		spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
 		/*
-		 * tg_pt_gp_mem is associated with a single
-		 * se_port->sep_alua_tg_pt_gp_mem, and is released via
-		 * core_alua_free_tg_pt_gp_mem().
-		 *
 		 * If the passed tg_pt_gp does NOT match the default_tg_pt_gp,
 		 * assume we want to re-associate a given tg_pt_gp_mem with
 		 * default_tg_pt_gp.
 		 */
-		spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+		spin_lock(&lun->lun_tg_pt_gp_lock);
 		if (tg_pt_gp != dev->t10_alua.default_tg_pt_gp) {
-			__core_alua_attach_tg_pt_gp_mem(tg_pt_gp_mem,
+			__target_attach_tg_pt_gp(lun,
 					dev->t10_alua.default_tg_pt_gp);
 		} else
-			tg_pt_gp_mem->tg_pt_gp = NULL;
-		spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+			lun->lun_tg_pt_gp = NULL;
+		spin_unlock(&lun->lun_tg_pt_gp_lock);
 
 		spin_lock(&tg_pt_gp->tg_pt_gp_lock);
 	}
@@ -1882,35 +1843,6 @@ void core_alua_free_tg_pt_gp(
 	kmem_cache_free(t10_alua_tg_pt_gp_cache, tg_pt_gp);
 }
 
-void core_alua_free_tg_pt_gp_mem(struct se_port *port)
-{
-	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
-
-	tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
-	if (!tg_pt_gp_mem)
-		return;
-
-	while (atomic_read(&tg_pt_gp_mem->tg_pt_gp_mem_ref_cnt))
-		cpu_relax();
-
-	spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-	tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
-	if (tg_pt_gp) {
-		spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-		if (tg_pt_gp_mem->tg_pt_gp_assoc) {
-			list_del(&tg_pt_gp_mem->tg_pt_gp_mem_list);
-			tg_pt_gp->tg_pt_gp_members--;
-			tg_pt_gp_mem->tg_pt_gp_assoc = 0;
-		}
-		spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
-		tg_pt_gp_mem->tg_pt_gp = NULL;
-	}
-	spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-
-	kmem_cache_free(t10_alua_tg_pt_gp_mem_cache, tg_pt_gp_mem);
-}
-
 static struct t10_alua_tg_pt_gp *core_alua_get_tg_pt_gp_by_name(
 		struct se_device *dev, const char *name)
 {
@@ -1944,50 +1876,65 @@ static void core_alua_put_tg_pt_gp_from_name(
 	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 }
 
-/*
- * Called with struct t10_alua_tg_pt_gp_member->tg_pt_gp_mem_lock held
- */
-void __core_alua_attach_tg_pt_gp_mem(
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem,
-	struct t10_alua_tg_pt_gp *tg_pt_gp)
+static void __target_attach_tg_pt_gp(struct se_lun *lun,
+		struct t10_alua_tg_pt_gp *tg_pt_gp)
 {
+	struct se_dev_entry *se_deve;
+
+	assert_spin_locked(&lun->lun_tg_pt_gp_lock);
+
 	spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-	tg_pt_gp_mem->tg_pt_gp = tg_pt_gp;
-	tg_pt_gp_mem->tg_pt_gp_assoc = 1;
-	list_add_tail(&tg_pt_gp_mem->tg_pt_gp_mem_list,
-			&tg_pt_gp->tg_pt_gp_mem_list);
+	lun->lun_tg_pt_gp = tg_pt_gp;
+	list_add_tail(&lun->lun_tg_pt_gp_link, &tg_pt_gp->tg_pt_gp_lun_list);
 	tg_pt_gp->tg_pt_gp_members++;
+	spin_lock(&lun->lun_deve_lock);
+	list_for_each_entry(se_deve, &lun->lun_deve_list, lun_link)
+		core_scsi3_ua_allocate(se_deve, 0x3f,
+				       ASCQ_3FH_INQUIRY_DATA_HAS_CHANGED);
+	spin_unlock(&lun->lun_deve_lock);
 	spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
 }
 
-/*
- * Called with struct t10_alua_tg_pt_gp_member->tg_pt_gp_mem_lock held
- */
-static void __core_alua_drop_tg_pt_gp_mem(
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem,
-	struct t10_alua_tg_pt_gp *tg_pt_gp)
+void target_attach_tg_pt_gp(struct se_lun *lun,
+		struct t10_alua_tg_pt_gp *tg_pt_gp)
 {
+	spin_lock(&lun->lun_tg_pt_gp_lock);
+	__target_attach_tg_pt_gp(lun, tg_pt_gp);
+	spin_unlock(&lun->lun_tg_pt_gp_lock);
+}
+
+static void __target_detach_tg_pt_gp(struct se_lun *lun,
+		struct t10_alua_tg_pt_gp *tg_pt_gp)
+{
+	assert_spin_locked(&lun->lun_tg_pt_gp_lock);
+
 	spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-	list_del(&tg_pt_gp_mem->tg_pt_gp_mem_list);
-	tg_pt_gp_mem->tg_pt_gp = NULL;
-	tg_pt_gp_mem->tg_pt_gp_assoc = 0;
+	list_del_init(&lun->lun_tg_pt_gp_link);
 	tg_pt_gp->tg_pt_gp_members--;
 	spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
+
+	lun->lun_tg_pt_gp = NULL;
 }
 
-ssize_t core_alua_show_tg_pt_gp_info(struct se_port *port, char *page)
+void target_detach_tg_pt_gp(struct se_lun *lun)
+{
+	struct t10_alua_tg_pt_gp *tg_pt_gp;
+
+	spin_lock(&lun->lun_tg_pt_gp_lock);
+	tg_pt_gp = lun->lun_tg_pt_gp;
+	if (tg_pt_gp)
+		__target_detach_tg_pt_gp(lun, tg_pt_gp);
+	spin_unlock(&lun->lun_tg_pt_gp_lock);
+}
+
+ssize_t core_alua_show_tg_pt_gp_info(struct se_lun *lun, char *page)
 {
 	struct config_item *tg_pt_ci;
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
 	ssize_t len = 0;
 
-	tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
-	if (!tg_pt_gp_mem)
-		return len;
-
-	spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-	tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
+	spin_lock(&lun->lun_tg_pt_gp_lock);
+	tg_pt_gp = lun->lun_tg_pt_gp;
 	if (tg_pt_gp) {
 		tg_pt_ci = &tg_pt_gp->tg_pt_gp_group.cg_item;
 		len += sprintf(page, "TG Port Alias: %s\nTG Port Group ID:"
@@ -1999,34 +1946,33 @@ ssize_t core_alua_show_tg_pt_gp_info(struct se_port *port, char *page)
 					&tg_pt_gp->tg_pt_gp_alua_access_state)),
 			core_alua_dump_status(
 				tg_pt_gp->tg_pt_gp_alua_access_status),
-			(atomic_read(&port->sep_tg_pt_secondary_offline)) ?
+			atomic_read(&lun->lun_tg_pt_secondary_offline) ?
 			"Offline" : "None",
-			core_alua_dump_status(port->sep_tg_pt_secondary_stat));
+			core_alua_dump_status(lun->lun_tg_pt_secondary_stat));
 	}
-	spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+	spin_unlock(&lun->lun_tg_pt_gp_lock);
 
 	return len;
 }
 
 ssize_t core_alua_store_tg_pt_gp_info(
-	struct se_port *port,
+	struct se_lun *lun,
 	const char *page,
 	size_t count)
 {
-	struct se_portal_group *tpg;
-	struct se_lun *lun;
-	struct se_device *dev = port->sep_lun->lun_se_dev;
+	struct se_portal_group *tpg = lun->lun_tpg;
+	/*
+	 * rcu_dereference_raw protected by se_lun->lun_group symlink
+	 * reference to se_device->dev_group.
+	 */
+	struct se_device *dev = rcu_dereference_raw(lun->lun_se_dev);
 	struct t10_alua_tg_pt_gp *tg_pt_gp = NULL, *tg_pt_gp_new = NULL;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
 	unsigned char buf[TG_PT_GROUP_NAME_BUF];
 	int move = 0;
 
-	tpg = port->sep_tpg;
-	lun = port->sep_lun;
-
-	tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
-	if (!tg_pt_gp_mem)
-		return 0;
+	if (dev->transport->transport_flags & TRANSPORT_FLAG_PASSTHROUGH ||
+	    (dev->se_hba->hba_flags & HBA_FLAGS_INTERNAL_USE))
+		return -ENODEV;
 
 	if (count > TG_PT_GROUP_NAME_BUF) {
 		pr_err("ALUA Target Port Group alias too large!\n");
@@ -2050,8 +1996,8 @@ ssize_t core_alua_store_tg_pt_gp_info(
 			return -ENODEV;
 	}
 
-	spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-	tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
+	spin_lock(&lun->lun_tg_pt_gp_lock);
+	tg_pt_gp = lun->lun_tg_pt_gp;
 	if (tg_pt_gp) {
 		/*
 		 * Clearing an existing tg_pt_gp association, and replacing
@@ -2069,24 +2015,19 @@ ssize_t core_alua_store_tg_pt_gp_info(
 					&tg_pt_gp->tg_pt_gp_group.cg_item),
 				tg_pt_gp->tg_pt_gp_id);
 
-			__core_alua_drop_tg_pt_gp_mem(tg_pt_gp_mem, tg_pt_gp);
-			__core_alua_attach_tg_pt_gp_mem(tg_pt_gp_mem,
+			__target_detach_tg_pt_gp(lun, tg_pt_gp);
+			__target_attach_tg_pt_gp(lun,
 					dev->t10_alua.default_tg_pt_gp);
-			spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+			spin_unlock(&lun->lun_tg_pt_gp_lock);
 
 			return count;
 		}
-		/*
-		 * Removing existing association of tg_pt_gp_mem with tg_pt_gp
-		 */
-		__core_alua_drop_tg_pt_gp_mem(tg_pt_gp_mem, tg_pt_gp);
+		__target_detach_tg_pt_gp(lun, tg_pt_gp);
 		move = 1;
 	}
-	/*
-	 * Associate tg_pt_gp_mem with tg_pt_gp_new.
-	 */
-	__core_alua_attach_tg_pt_gp_mem(tg_pt_gp_mem, tg_pt_gp_new);
-	spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+
+	__target_attach_tg_pt_gp(lun, tg_pt_gp_new);
+	spin_unlock(&lun->lun_tg_pt_gp_lock);
 	pr_debug("Target_Core_ConfigFS: %s %s/tpgt_%hu/%s to ALUA"
 		" Target Port Group: alua/%s, ID: %hu\n", (move) ?
 		"Moving" : "Adding", tpg->se_tpg_tfo->tpg_get_wwn(tpg),
@@ -2269,11 +2210,8 @@ ssize_t core_alua_store_preferred_bit(
 
 ssize_t core_alua_show_offline_bit(struct se_lun *lun, char *page)
 {
-	if (!lun->lun_sep)
-		return -ENODEV;
-
 	return sprintf(page, "%d\n",
-		atomic_read(&lun->lun_sep->sep_tg_pt_secondary_offline));
+		atomic_read(&lun->lun_tg_pt_secondary_offline));
 }
 
 ssize_t core_alua_store_offline_bit(
@@ -2281,11 +2219,16 @@ ssize_t core_alua_store_offline_bit(
 	const char *page,
 	size_t count)
 {
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
+	/*
+	 * rcu_dereference_raw protected by se_lun->lun_group symlink
+	 * reference to se_device->dev_group.
+	 */
+	struct se_device *dev = rcu_dereference_raw(lun->lun_se_dev);
 	unsigned long tmp;
 	int ret;
 
-	if (!lun->lun_sep)
+	if (dev->transport->transport_flags & TRANSPORT_FLAG_PASSTHROUGH ||
+	    (dev->se_hba->hba_flags & HBA_FLAGS_INTERNAL_USE))
 		return -ENODEV;
 
 	ret = kstrtoul(page, 0, &tmp);
@@ -2298,14 +2241,8 @@ ssize_t core_alua_store_offline_bit(
 				tmp);
 		return -EINVAL;
 	}
-	tg_pt_gp_mem = lun->lun_sep->sep_alua_tg_pt_gp_mem;
-	if (!tg_pt_gp_mem) {
-		pr_err("Unable to locate *tg_pt_gp_mem\n");
-		return -EINVAL;
-	}
 
-	ret = core_alua_set_tg_pt_secondary_state(tg_pt_gp_mem,
-			lun->lun_sep, 0, (int)tmp);
+	ret = core_alua_set_tg_pt_secondary_state(lun, 0, (int)tmp);
 	if (ret < 0)
 		return -EINVAL;
 
@@ -2316,7 +2253,7 @@ ssize_t core_alua_show_secondary_status(
 	struct se_lun *lun,
 	char *page)
 {
-	return sprintf(page, "%d\n", lun->lun_sep->sep_tg_pt_secondary_stat);
+	return sprintf(page, "%d\n", lun->lun_tg_pt_secondary_stat);
 }
 
 ssize_t core_alua_store_secondary_status(
@@ -2339,7 +2276,7 @@ ssize_t core_alua_store_secondary_status(
 				tmp);
 		return -EINVAL;
 	}
-	lun->lun_sep->sep_tg_pt_secondary_stat = (int)tmp;
+	lun->lun_tg_pt_secondary_stat = (int)tmp;
 
 	return count;
 }
@@ -2348,8 +2285,7 @@ ssize_t core_alua_show_secondary_write_metadata(
 	struct se_lun *lun,
 	char *page)
 {
-	return sprintf(page, "%d\n",
-			lun->lun_sep->sep_tg_pt_secondary_write_md);
+	return sprintf(page, "%d\n", lun->lun_tg_pt_secondary_write_md);
 }
 
 ssize_t core_alua_store_secondary_write_metadata(
@@ -2370,7 +2306,7 @@ ssize_t core_alua_store_secondary_write_metadata(
 				" %lu\n", tmp);
 		return -EINVAL;
 	}
-	lun->lun_sep->sep_tg_pt_secondary_write_md = (int)tmp;
+	lun->lun_tg_pt_secondary_write_md = (int)tmp;
 
 	return count;
 }
diff --git a/drivers/target/target_core_alua.h b/drivers/target/target_core_alua.h
index 0a7d65e80404..9b250f9b33bf 100644
--- a/drivers/target/target_core_alua.h
+++ b/drivers/target/target_core_alua.h
@@ -85,7 +85,6 @@
 extern struct kmem_cache *t10_alua_lu_gp_cache;
 extern struct kmem_cache *t10_alua_lu_gp_mem_cache;
 extern struct kmem_cache *t10_alua_tg_pt_gp_cache;
-extern struct kmem_cache *t10_alua_tg_pt_gp_mem_cache;
 extern struct kmem_cache *t10_alua_lba_map_cache;
 extern struct kmem_cache *t10_alua_lba_map_mem_cache;
 
@@ -94,7 +93,7 @@ extern sense_reason_t target_emulate_set_target_port_groups(struct se_cmd *);
 extern sense_reason_t target_emulate_report_referrals(struct se_cmd *);
 extern int core_alua_check_nonop_delay(struct se_cmd *);
 extern int core_alua_do_port_transition(struct t10_alua_tg_pt_gp *,
-				struct se_device *, struct se_port *,
+				struct se_device *, struct se_lun *,
 				struct se_node_acl *, int, int);
 extern char *core_alua_dump_status(int);
 extern struct t10_alua_lba_map *core_alua_allocate_lba_map(
@@ -117,14 +116,11 @@ extern void core_alua_drop_lu_gp_dev(struct se_device *);
 extern struct t10_alua_tg_pt_gp *core_alua_allocate_tg_pt_gp(
 			struct se_device *, const char *, int);
 extern int core_alua_set_tg_pt_gp_id(struct t10_alua_tg_pt_gp *, u16);
-extern struct t10_alua_tg_pt_gp_member *core_alua_allocate_tg_pt_gp_mem(
-					struct se_port *);
 extern void core_alua_free_tg_pt_gp(struct t10_alua_tg_pt_gp *);
-extern void core_alua_free_tg_pt_gp_mem(struct se_port *);
-extern void __core_alua_attach_tg_pt_gp_mem(struct t10_alua_tg_pt_gp_member *,
-					struct t10_alua_tg_pt_gp *);
-extern ssize_t core_alua_show_tg_pt_gp_info(struct se_port *, char *);
-extern ssize_t core_alua_store_tg_pt_gp_info(struct se_port *, const char *,
+extern void target_detach_tg_pt_gp(struct se_lun *);
+extern void target_attach_tg_pt_gp(struct se_lun *, struct t10_alua_tg_pt_gp *);
+extern ssize_t core_alua_show_tg_pt_gp_info(struct se_lun *, char *);
+extern ssize_t core_alua_store_tg_pt_gp_info(struct se_lun *, const char *,
 						size_t);
 extern ssize_t core_alua_show_access_type(struct t10_alua_tg_pt_gp *, char *);
 extern ssize_t core_alua_store_access_type(struct t10_alua_tg_pt_gp *,
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index e7b0430a0575..0b0de3647478 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -41,7 +41,6 @@
 #include <target/target_core_backend.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 
 #include "target_core_internal.h"
@@ -51,15 +50,26 @@
 #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) \
+static void target_core_setup_##_name##_cit(struct target_backend *tb)	\
 {									\
-	struct target_backend_cits *tbc = &sa->tb_cits;			\
-	struct config_item_type *cit = &tbc->tb_##_name##_cit;		\
+	struct config_item_type *cit = &tb->tb_##_name##_cit;		\
 									\
 	cit->ct_item_ops = _item_ops;					\
 	cit->ct_group_ops = _group_ops;					\
 	cit->ct_attrs = _attrs;						\
-	cit->ct_owner = sa->owner;					\
+	cit->ct_owner = tb->ops->owner;					\
+	pr_debug("Setup generic %s\n", __stringify(_name));		\
+}
+
+#define TB_CIT_SETUP_DRV(_name, _item_ops, _group_ops)			\
+static void target_core_setup_##_name##_cit(struct target_backend *tb)	\
+{									\
+	struct config_item_type *cit = &tb->tb_##_name##_cit;		\
+									\
+	cit->ct_item_ops = _item_ops;					\
+	cit->ct_group_ops = _group_ops;					\
+	cit->ct_attrs = tb->ops->tb_##_name##_attrs;			\
+	cit->ct_owner = tb->ops->owner;					\
 	pr_debug("Setup generic %s\n", __stringify(_name));		\
 }
 
@@ -92,7 +102,7 @@ static ssize_t target_core_attr_show(struct config_item *item,
 				      char *page)
 {
 	return sprintf(page, "Target Engine Core ConfigFS Infrastructure %s"
-		" on %s/%s on "UTS_RELEASE"\n", TARGET_CORE_CONFIGFS_VERSION,
+		" on %s/%s on "UTS_RELEASE"\n", TARGET_CORE_VERSION,
 		utsname()->sysname, utsname()->machine);
 }
 
@@ -116,7 +126,7 @@ static struct target_fabric_configfs *target_core_get_fabric(
 
 	mutex_lock(&g_tf_lock);
 	list_for_each_entry(tf, &g_tf_list, tf_list) {
-		if (!strcmp(tf->tf_name, name)) {
+		if (!strcmp(tf->tf_ops->name, name)) {
 			atomic_inc(&tf->tf_access_cnt);
 			mutex_unlock(&g_tf_lock);
 			return tf;
@@ -193,29 +203,24 @@ static struct config_group *target_core_register_fabric(
 		return ERR_PTR(-EINVAL);
 	}
 	pr_debug("Target_Core_ConfigFS: REGISTER -> Located fabric:"
-			" %s\n", tf->tf_name);
+			" %s\n", tf->tf_ops->name);
 	/*
 	 * On a successful target_core_get_fabric() look, the returned
 	 * struct target_fabric_configfs *tf will contain a usage reference.
 	 */
 	pr_debug("Target_Core_ConfigFS: REGISTER tfc_wwn_cit -> %p\n",
-			&tf->tf_cit_tmpl.tfc_wwn_cit);
+			&tf->tf_wwn_cit);
 
 	tf->tf_group.default_groups = tf->tf_default_groups;
 	tf->tf_group.default_groups[0] = &tf->tf_disc_group;
 	tf->tf_group.default_groups[1] = NULL;
 
-	config_group_init_type_name(&tf->tf_group, name,
-			&tf->tf_cit_tmpl.tfc_wwn_cit);
+	config_group_init_type_name(&tf->tf_group, name, &tf->tf_wwn_cit);
 	config_group_init_type_name(&tf->tf_disc_group, "discovery_auth",
-			&tf->tf_cit_tmpl.tfc_discovery_cit);
+			&tf->tf_discovery_cit);
 
 	pr_debug("Target_Core_ConfigFS: REGISTER -> Allocated Fabric:"
 			" %s\n", tf->tf_group.cg_item.ci_name);
-	tf->tf_fabric = &tf->tf_group.cg_item;
-	pr_debug("Target_Core_ConfigFS: REGISTER -> Set tf->tf_fabric"
-			" for %s\n", name);
-
 	return &tf->tf_group;
 }
 
@@ -236,13 +241,9 @@ static void target_core_deregister_fabric(
 		" tf list\n", config_item_name(item));
 
 	pr_debug("Target_Core_ConfigFS: DEREGISTER -> located fabric:"
-			" %s\n", tf->tf_name);
+			" %s\n", tf->tf_ops->name);
 	atomic_dec(&tf->tf_access_cnt);
 
-	pr_debug("Target_Core_ConfigFS: DEREGISTER -> Releasing"
-			" tf->tf_fabric for %s\n", tf->tf_name);
-	tf->tf_fabric = NULL;
-
 	pr_debug("Target_Core_ConfigFS: DEREGISTER -> Releasing ci"
 			" %s\n", config_item_name(item));
 
@@ -318,10 +319,6 @@ static int target_fabric_tf_ops_check(const struct target_core_fabric_ops *tfo)
 		pr_err("Missing tfo->get_fabric_name()\n");
 		return -EINVAL;
 	}
-	if (!tfo->get_fabric_proto_ident) {
-		pr_err("Missing tfo->get_fabric_proto_ident()\n");
-		return -EINVAL;
-	}
 	if (!tfo->tpg_get_wwn) {
 		pr_err("Missing tfo->tpg_get_wwn()\n");
 		return -EINVAL;
@@ -330,18 +327,6 @@ static int target_fabric_tf_ops_check(const struct target_core_fabric_ops *tfo)
 		pr_err("Missing tfo->tpg_get_tag()\n");
 		return -EINVAL;
 	}
-	if (!tfo->tpg_get_default_depth) {
-		pr_err("Missing tfo->tpg_get_default_depth()\n");
-		return -EINVAL;
-	}
-	if (!tfo->tpg_get_pr_transport_id) {
-		pr_err("Missing tfo->tpg_get_pr_transport_id()\n");
-		return -EINVAL;
-	}
-	if (!tfo->tpg_get_pr_transport_id_len) {
-		pr_err("Missing tfo->tpg_get_pr_transport_id_len()\n");
-		return -EINVAL;
-	}
 	if (!tfo->tpg_check_demo_mode) {
 		pr_err("Missing tfo->tpg_check_demo_mode()\n");
 		return -EINVAL;
@@ -358,14 +343,6 @@ static int target_fabric_tf_ops_check(const struct target_core_fabric_ops *tfo)
 		pr_err("Missing tfo->tpg_check_prod_mode_write_protect()\n");
 		return -EINVAL;
 	}
-	if (!tfo->tpg_alloc_fabric_acl) {
-		pr_err("Missing tfo->tpg_alloc_fabric_acl()\n");
-		return -EINVAL;
-	}
-	if (!tfo->tpg_release_fabric_acl) {
-		pr_err("Missing tfo->tpg_release_fabric_acl()\n");
-		return -EINVAL;
-	}
 	if (!tfo->tpg_get_inst_index) {
 		pr_err("Missing tfo->tpg_get_inst_index()\n");
 		return -EINVAL;
@@ -398,10 +375,6 @@ static int target_fabric_tf_ops_check(const struct target_core_fabric_ops *tfo)
 		pr_err("Missing tfo->set_default_node_attributes()\n");
 		return -EINVAL;
 	}
-	if (!tfo->get_task_tag) {
-		pr_err("Missing tfo->get_task_tag()\n");
-		return -EINVAL;
-	}
 	if (!tfo->get_cmd_state) {
 		pr_err("Missing tfo->get_cmd_state()\n");
 		return -EINVAL;
@@ -464,15 +437,7 @@ int target_register_template(const struct target_core_fabric_ops *fo)
 
 	INIT_LIST_HEAD(&tf->tf_list);
 	atomic_set(&tf->tf_access_cnt, 0);
-
-	/*
-	 * Setup the default generic struct config_item_type's (cits) in
-	 * struct target_fabric_configfs->tf_cit_tmpl
-	 */
-	tf->tf_module = fo->module;
-	snprintf(tf->tf_name, TARGET_FABRIC_NAME_SIZE, "%s", fo->name);
-
-	tf->tf_ops = *fo;
+	tf->tf_ops = fo;
 	target_fabric_setup_cits(tf);
 
 	mutex_lock(&g_tf_lock);
@@ -489,7 +454,7 @@ void target_unregister_template(const struct target_core_fabric_ops *fo)
 
 	mutex_lock(&g_tf_lock);
 	list_for_each_entry(t, &g_tf_list, tf_list) {
-		if (!strcmp(t->tf_name, fo->name)) {
+		if (!strcmp(t->tf_ops->name, fo->name)) {
 			BUG_ON(atomic_read(&t->tf_access_cnt));
 			list_del(&t->tf_list);
 			kfree(t);
@@ -505,16 +470,605 @@ EXPORT_SYMBOL(target_unregister_template);
 //############################################################################*/
 
 /* Start functions for struct config_item_type tb_dev_attrib_cit */
+#define DEF_TB_DEV_ATTRIB_SHOW(_name)					\
+static ssize_t show_##_name(struct se_dev_attrib *da, char *page)	\
+{									\
+	return snprintf(page, PAGE_SIZE, "%u\n", da->_name);		\
+}
+
+DEF_TB_DEV_ATTRIB_SHOW(emulate_model_alias);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_dpo);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_fua_write);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_fua_read);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_write_cache);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_ua_intlck_ctrl);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_tas);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_tpu);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_tpws);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_caw);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_3pc);
+DEF_TB_DEV_ATTRIB_SHOW(pi_prot_type);
+DEF_TB_DEV_ATTRIB_SHOW(hw_pi_prot_type);
+DEF_TB_DEV_ATTRIB_SHOW(pi_prot_format);
+DEF_TB_DEV_ATTRIB_SHOW(enforce_pr_isids);
+DEF_TB_DEV_ATTRIB_SHOW(is_nonrot);
+DEF_TB_DEV_ATTRIB_SHOW(emulate_rest_reord);
+DEF_TB_DEV_ATTRIB_SHOW(force_pr_aptpl);
+DEF_TB_DEV_ATTRIB_SHOW(hw_block_size);
+DEF_TB_DEV_ATTRIB_SHOW(block_size);
+DEF_TB_DEV_ATTRIB_SHOW(hw_max_sectors);
+DEF_TB_DEV_ATTRIB_SHOW(optimal_sectors);
+DEF_TB_DEV_ATTRIB_SHOW(hw_queue_depth);
+DEF_TB_DEV_ATTRIB_SHOW(queue_depth);
+DEF_TB_DEV_ATTRIB_SHOW(max_unmap_lba_count);
+DEF_TB_DEV_ATTRIB_SHOW(max_unmap_block_desc_count);
+DEF_TB_DEV_ATTRIB_SHOW(unmap_granularity);
+DEF_TB_DEV_ATTRIB_SHOW(unmap_granularity_alignment);
+DEF_TB_DEV_ATTRIB_SHOW(max_write_same_len);
+
+#define DEF_TB_DEV_ATTRIB_STORE_U32(_name)				\
+static ssize_t store_##_name(struct se_dev_attrib *da, const char *page,\
+		size_t count)						\
+{									\
+	u32 val;							\
+	int ret;							\
+									\
+	ret = kstrtou32(page, 0, &val);					\
+	if (ret < 0)							\
+		return ret;						\
+	da->_name = val;						\
+	return count;							\
+}
+
+DEF_TB_DEV_ATTRIB_STORE_U32(max_unmap_lba_count);
+DEF_TB_DEV_ATTRIB_STORE_U32(max_unmap_block_desc_count);
+DEF_TB_DEV_ATTRIB_STORE_U32(unmap_granularity);
+DEF_TB_DEV_ATTRIB_STORE_U32(unmap_granularity_alignment);
+DEF_TB_DEV_ATTRIB_STORE_U32(max_write_same_len);
+
+#define DEF_TB_DEV_ATTRIB_STORE_BOOL(_name)				\
+static ssize_t store_##_name(struct se_dev_attrib *da, const char *page,\
+		size_t count)						\
+{									\
+	bool flag;							\
+	int ret;							\
+									\
+	ret = strtobool(page, &flag);					\
+	if (ret < 0)							\
+		return ret;						\
+	da->_name = flag;						\
+	return count;							\
+}
+
+DEF_TB_DEV_ATTRIB_STORE_BOOL(emulate_fua_write);
+DEF_TB_DEV_ATTRIB_STORE_BOOL(emulate_caw);
+DEF_TB_DEV_ATTRIB_STORE_BOOL(emulate_3pc);
+DEF_TB_DEV_ATTRIB_STORE_BOOL(enforce_pr_isids);
+DEF_TB_DEV_ATTRIB_STORE_BOOL(is_nonrot);
+
+#define DEF_TB_DEV_ATTRIB_STORE_STUB(_name)				\
+static ssize_t store_##_name(struct se_dev_attrib *da, const char *page,\
+		size_t count)						\
+{									\
+	printk_once(KERN_WARNING					\
+		"ignoring deprecated ##_name## attribute\n");	\
+	return count;							\
+}
+
+DEF_TB_DEV_ATTRIB_STORE_STUB(emulate_dpo);
+DEF_TB_DEV_ATTRIB_STORE_STUB(emulate_fua_read);
+
+static void dev_set_t10_wwn_model_alias(struct se_device *dev)
+{
+	const char *configname;
+
+	configname = config_item_name(&dev->dev_group.cg_item);
+	if (strlen(configname) >= 16) {
+		pr_warn("dev[%p]: Backstore name '%s' is too long for "
+			"INQUIRY_MODEL, truncating to 16 bytes\n", dev,
+			configname);
+	}
+	snprintf(&dev->t10_wwn.model[0], 16, "%s", configname);
+}
+
+static ssize_t store_emulate_model_alias(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	struct se_device *dev = da->da_dev;
+	bool flag;
+	int ret;
+
+	if (dev->export_count) {
+		pr_err("dev[%p]: Unable to change model alias"
+			" while export_count is %d\n",
+			dev, dev->export_count);
+		return -EINVAL;
+	}
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+
+	if (flag) {
+		dev_set_t10_wwn_model_alias(dev);
+	} else {
+		strncpy(&dev->t10_wwn.model[0],
+			dev->transport->inquiry_prod, 16);
+	}
+	da->emulate_model_alias = flag;
+	return count;
+}
+
+static ssize_t store_emulate_write_cache(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	bool flag;
+	int ret;
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+
+	if (flag && da->da_dev->transport->get_write_cache) {
+		pr_err("emulate_write_cache not supported for this device\n");
+		return -EINVAL;
+	}
+
+	da->emulate_write_cache = flag;
+	pr_debug("dev[%p]: SE Device WRITE_CACHE_EMULATION flag: %d\n",
+			da->da_dev, flag);
+	return count;
+}
+
+static ssize_t store_emulate_ua_intlck_ctrl(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	u32 val;
+	int ret;
+
+	ret = kstrtou32(page, 0, &val);
+	if (ret < 0)
+		return ret;
+
+	if (val != 0 && val != 1 && val != 2) {
+		pr_err("Illegal value %d\n", val);
+		return -EINVAL;
+	}
+
+	if (da->da_dev->export_count) {
+		pr_err("dev[%p]: Unable to change SE Device"
+			" UA_INTRLCK_CTRL while export_count is %d\n",
+			da->da_dev, da->da_dev->export_count);
+		return -EINVAL;
+	}
+	da->emulate_ua_intlck_ctrl = val;
+	pr_debug("dev[%p]: SE Device UA_INTRLCK_CTRL flag: %d\n",
+		da->da_dev, val);
+	return count;
+}
+
+static ssize_t store_emulate_tas(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	bool flag;
+	int ret;
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+
+	if (da->da_dev->export_count) {
+		pr_err("dev[%p]: Unable to change SE Device TAS while"
+			" export_count is %d\n",
+			da->da_dev, da->da_dev->export_count);
+		return -EINVAL;
+	}
+	da->emulate_tas = flag;
+	pr_debug("dev[%p]: SE Device TASK_ABORTED status bit: %s\n",
+		da->da_dev, flag ? "Enabled" : "Disabled");
+
+	return count;
+}
+
+static ssize_t store_emulate_tpu(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	bool flag;
+	int ret;
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * We expect this value to be non-zero when generic Block Layer
+	 * Discard supported is detected iblock_create_virtdevice().
+	 */
+	if (flag && !da->max_unmap_block_desc_count) {
+		pr_err("Generic Block Discard not supported\n");
+		return -ENOSYS;
+	}
+
+	da->emulate_tpu = flag;
+	pr_debug("dev[%p]: SE Device Thin Provisioning UNMAP bit: %d\n",
+		da->da_dev, flag);
+	return count;
+}
+
+static ssize_t store_emulate_tpws(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	bool flag;
+	int ret;
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * We expect this value to be non-zero when generic Block Layer
+	 * Discard supported is detected iblock_create_virtdevice().
+	 */
+	if (flag && !da->max_unmap_block_desc_count) {
+		pr_err("Generic Block Discard not supported\n");
+		return -ENOSYS;
+	}
+
+	da->emulate_tpws = flag;
+	pr_debug("dev[%p]: SE Device Thin Provisioning WRITE_SAME: %d\n",
+				da->da_dev, flag);
+	return count;
+}
+
+static ssize_t store_pi_prot_type(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	int old_prot = da->pi_prot_type, ret;
+	struct se_device *dev = da->da_dev;
+	u32 flag;
+
+	ret = kstrtou32(page, 0, &flag);
+	if (ret < 0)
+		return ret;
+
+	if (flag != 0 && flag != 1 && flag != 2 && flag != 3) {
+		pr_err("Illegal value %d for pi_prot_type\n", flag);
+		return -EINVAL;
+	}
+	if (flag == 2) {
+		pr_err("DIF TYPE2 protection currently not supported\n");
+		return -ENOSYS;
+	}
+	if (da->hw_pi_prot_type) {
+		pr_warn("DIF protection enabled on underlying hardware,"
+			" ignoring\n");
+		return count;
+	}
+	if (!dev->transport->init_prot || !dev->transport->free_prot) {
+		/* 0 is only allowed value for non-supporting backends */
+		if (flag == 0)
+			return 0;
+
+		pr_err("DIF protection not supported by backend: %s\n",
+		       dev->transport->name);
+		return -ENOSYS;
+	}
+	if (!(dev->dev_flags & DF_CONFIGURED)) {
+		pr_err("DIF protection requires device to be configured\n");
+		return -ENODEV;
+	}
+	if (dev->export_count) {
+		pr_err("dev[%p]: Unable to change SE Device PROT type while"
+		       " export_count is %d\n", dev, dev->export_count);
+		return -EINVAL;
+	}
+
+	da->pi_prot_type = flag;
+
+	if (flag && !old_prot) {
+		ret = dev->transport->init_prot(dev);
+		if (ret) {
+			da->pi_prot_type = old_prot;
+			return ret;
+		}
+
+	} else if (!flag && old_prot) {
+		dev->transport->free_prot(dev);
+	}
+
+	pr_debug("dev[%p]: SE Device Protection Type: %d\n", dev, flag);
+	return count;
+}
+
+static ssize_t store_pi_prot_format(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	struct se_device *dev = da->da_dev;
+	bool flag;
+	int ret;
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+
+	if (!flag)
+		return count;
+
+	if (!dev->transport->format_prot) {
+		pr_err("DIF protection format not supported by backend %s\n",
+		       dev->transport->name);
+		return -ENOSYS;
+	}
+	if (!(dev->dev_flags & DF_CONFIGURED)) {
+		pr_err("DIF protection format requires device to be configured\n");
+		return -ENODEV;
+	}
+	if (dev->export_count) {
+		pr_err("dev[%p]: Unable to format SE Device PROT type while"
+		       " export_count is %d\n", dev, dev->export_count);
+		return -EINVAL;
+	}
+
+	ret = dev->transport->format_prot(dev);
+	if (ret)
+		return ret;
+
+	pr_debug("dev[%p]: SE Device Protection Format complete\n", dev);
+	return count;
+}
+
+static ssize_t store_force_pr_aptpl(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	bool flag;
+	int ret;
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+	if (da->da_dev->export_count) {
+		pr_err("dev[%p]: Unable to set force_pr_aptpl while"
+		       " export_count is %d\n",
+		       da->da_dev, da->da_dev->export_count);
+		return -EINVAL;
+	}
+
+	da->force_pr_aptpl = flag;
+	pr_debug("dev[%p]: SE Device force_pr_aptpl: %d\n", da->da_dev, flag);
+	return count;
+}
+
+static ssize_t store_emulate_rest_reord(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	bool flag;
+	int ret;
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+
+	if (flag != 0) {
+		printk(KERN_ERR "dev[%p]: SE Device emulation of restricted"
+			" reordering not implemented\n", da->da_dev);
+		return -ENOSYS;
+	}
+	da->emulate_rest_reord = flag;
+	pr_debug("dev[%p]: SE Device emulate_rest_reord: %d\n",
+		da->da_dev, flag);
+	return count;
+}
+
+/*
+ * Note, this can only be called on unexported SE Device Object.
+ */
+static ssize_t store_queue_depth(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	struct se_device *dev = da->da_dev;
+	u32 val;
+	int ret;
+
+	ret = kstrtou32(page, 0, &val);
+	if (ret < 0)
+		return ret;
+
+	if (dev->export_count) {
+		pr_err("dev[%p]: Unable to change SE Device TCQ while"
+			" export_count is %d\n",
+			dev, dev->export_count);
+		return -EINVAL;
+	}
+	if (!val) {
+		pr_err("dev[%p]: Illegal ZERO value for queue_depth\n", dev);
+		return -EINVAL;
+	}
+
+	if (val > dev->dev_attrib.queue_depth) {
+		if (val > dev->dev_attrib.hw_queue_depth) {
+			pr_err("dev[%p]: Passed queue_depth:"
+				" %u exceeds TCM/SE_Device MAX"
+				" TCQ: %u\n", dev, val,
+				dev->dev_attrib.hw_queue_depth);
+			return -EINVAL;
+		}
+	}
+	da->queue_depth = dev->queue_depth = val;
+	pr_debug("dev[%p]: SE Device TCQ Depth changed to: %u\n", dev, val);
+	return count;
+}
+
+static ssize_t store_optimal_sectors(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	u32 val;
+	int ret;
+
+	ret = kstrtou32(page, 0, &val);
+	if (ret < 0)
+		return ret;
+
+	if (da->da_dev->export_count) {
+		pr_err("dev[%p]: Unable to change SE Device"
+			" optimal_sectors while export_count is %d\n",
+			da->da_dev, da->da_dev->export_count);
+		return -EINVAL;
+	}
+	if (val > da->hw_max_sectors) {
+		pr_err("dev[%p]: Passed optimal_sectors %u cannot be"
+			" greater than hw_max_sectors: %u\n",
+			da->da_dev, val, da->hw_max_sectors);
+		return -EINVAL;
+	}
+
+	da->optimal_sectors = val;
+	pr_debug("dev[%p]: SE Device optimal_sectors changed to %u\n",
+			da->da_dev, val);
+	return count;
+}
+
+static ssize_t store_block_size(struct se_dev_attrib *da,
+		const char *page, size_t count)
+{
+	u32 val;
+	int ret;
+
+	ret = kstrtou32(page, 0, &val);
+	if (ret < 0)
+		return ret;
+
+	if (da->da_dev->export_count) {
+		pr_err("dev[%p]: Unable to change SE Device block_size"
+			" while export_count is %d\n",
+			da->da_dev, da->da_dev->export_count);
+		return -EINVAL;
+	}
+
+	if (val != 512 && val != 1024 && val != 2048 && val != 4096) {
+		pr_err("dev[%p]: Illegal value for block_device: %u"
+			" for SE device, must be 512, 1024, 2048 or 4096\n",
+			da->da_dev, val);
+		return -EINVAL;
+	}
+
+	da->block_size = val;
+	if (da->max_bytes_per_io)
+		da->hw_max_sectors = da->max_bytes_per_io / val;
+
+	pr_debug("dev[%p]: SE Device block_size changed to %u\n",
+			da->da_dev, val);
+	return count;
+}
+
+CONFIGFS_EATTR_STRUCT(target_backend_dev_attrib, se_dev_attrib);
+#define TB_DEV_ATTR(_backend, _name, _mode)				\
+static struct target_backend_dev_attrib_attribute _backend##_dev_attrib_##_name = \
+	__CONFIGFS_EATTR(_name, _mode,					\
+	show_##_name,							\
+	store_##_name);
+
+#define TB_DEV_ATTR_RO(_backend, _name)					\
+static struct target_backend_dev_attrib_attribute _backend##_dev_attrib_##_name = \
+	__CONFIGFS_EATTR_RO(_name,					\
+	show_##_name);
+
+TB_DEV_ATTR(target_core, emulate_model_alias, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_dpo, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_fua_write, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_fua_read, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_write_cache, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_ua_intlck_ctrl, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_tas, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_tpu, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_tpws, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_caw, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_3pc, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, pi_prot_type, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR_RO(target_core, hw_pi_prot_type);
+TB_DEV_ATTR(target_core, pi_prot_format, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, enforce_pr_isids, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, is_nonrot, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, emulate_rest_reord, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, force_pr_aptpl, S_IRUGO | S_IWUSR)
+TB_DEV_ATTR_RO(target_core, hw_block_size);
+TB_DEV_ATTR(target_core, block_size, S_IRUGO | S_IWUSR)
+TB_DEV_ATTR_RO(target_core, hw_max_sectors);
+TB_DEV_ATTR(target_core, optimal_sectors, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR_RO(target_core, hw_queue_depth);
+TB_DEV_ATTR(target_core, queue_depth, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, max_unmap_lba_count, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, max_unmap_block_desc_count, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, unmap_granularity, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, unmap_granularity_alignment, S_IRUGO | S_IWUSR);
+TB_DEV_ATTR(target_core, max_write_same_len, S_IRUGO | S_IWUSR);
 
 CONFIGFS_EATTR_STRUCT(target_core_dev_attrib, se_dev_attrib);
 CONFIGFS_EATTR_OPS(target_core_dev_attrib, se_dev_attrib, da_group);
 
+/*
+ * dev_attrib attributes for devices using the target core SBC/SPC
+ * interpreter.  Any backend using spc_parse_cdb should be using
+ * these.
+ */
+struct configfs_attribute *sbc_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_is_nonrot.attr,
+	&target_core_dev_attrib_emulate_rest_reord.attr,
+	&target_core_dev_attrib_force_pr_aptpl.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_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,
+};
+EXPORT_SYMBOL(sbc_attrib_attrs);
+
+TB_DEV_ATTR_RO(target_pt, hw_pi_prot_type);
+TB_DEV_ATTR_RO(target_pt, hw_block_size);
+TB_DEV_ATTR_RO(target_pt, hw_max_sectors);
+TB_DEV_ATTR_RO(target_pt, hw_queue_depth);
+
+/*
+ * Minimal dev_attrib attributes for devices passing through CDBs.
+ * In this case we only provide a few read-only attributes for
+ * backwards compatibility.
+ */
+struct configfs_attribute *passthrough_attrib_attrs[] = {
+	&target_pt_dev_attrib_hw_pi_prot_type.attr,
+	&target_pt_dev_attrib_hw_block_size.attr,
+	&target_pt_dev_attrib_hw_max_sectors.attr,
+	&target_pt_dev_attrib_hw_queue_depth.attr,
+	NULL,
+};
+EXPORT_SYMBOL(passthrough_attrib_attrs);
+
 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,
 };
 
-TB_CIT_SETUP(dev_attrib, &target_core_dev_attrib_ops, NULL, NULL);
+TB_CIT_SETUP_DRV(dev_attrib, &target_core_dev_attrib_ops, NULL);
 
 /* End functions for struct config_item_type tb_dev_attrib_cit */
 
@@ -862,7 +1416,6 @@ static ssize_t target_core_dev_pr_show_attr_res_pr_holder_tg_port(
 		struct se_device *dev, char *page)
 {
 	struct se_node_acl *se_nacl;
-	struct se_lun *lun;
 	struct se_portal_group *se_tpg;
 	struct t10_pr_registration *pr_reg;
 	const struct target_core_fabric_ops *tfo;
@@ -877,7 +1430,6 @@ static ssize_t target_core_dev_pr_show_attr_res_pr_holder_tg_port(
 
 	se_nacl = pr_reg->pr_reg_nacl;
 	se_tpg = se_nacl->se_tpg;
-	lun = pr_reg->pr_reg_tg_pt_lun;
 	tfo = se_tpg->se_tpg_tfo;
 
 	len += sprintf(page+len, "SPC-3 Reservation: %s"
@@ -885,9 +1437,9 @@ static ssize_t target_core_dev_pr_show_attr_res_pr_holder_tg_port(
 		tfo->tpg_get_wwn(se_tpg));
 	len += sprintf(page+len, "SPC-3 Reservation: Relative Port"
 		" Identifier Tag: %hu %s Portal Group Tag: %hu"
-		" %s Logical Unit: %u\n", lun->lun_sep->sep_rtpi,
+		" %s Logical Unit: %llu\n", pr_reg->tg_pt_sep_rtpi,
 		tfo->get_fabric_name(), tfo->tpg_get_tag(se_tpg),
-		tfo->get_fabric_name(), lun->unpacked_lun);
+		tfo->get_fabric_name(), pr_reg->pr_aptpl_target_lun);
 
 out_unlock:
 	spin_unlock(&dev->dev_reservation_lock);
@@ -1012,12 +1564,12 @@ static match_table_t tokens = {
 	{Opt_res_type, "res_type=%d"},
 	{Opt_res_scope, "res_scope=%d"},
 	{Opt_res_all_tg_pt, "res_all_tg_pt=%d"},
-	{Opt_mapped_lun, "mapped_lun=%d"},
+	{Opt_mapped_lun, "mapped_lun=%lld"},
 	{Opt_target_fabric, "target_fabric=%s"},
 	{Opt_target_node, "target_node=%s"},
 	{Opt_tpgt, "tpgt=%d"},
 	{Opt_port_rtpi, "port_rtpi=%d"},
-	{Opt_target_lun, "target_lun=%d"},
+	{Opt_target_lun, "target_lun=%lld"},
 	{Opt_err, NULL}
 };
 
@@ -1032,10 +1584,10 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 	substring_t args[MAX_OPT_ARGS];
 	unsigned long long tmp_ll;
 	u64 sa_res_key = 0;
-	u32 mapped_lun = 0, target_lun = 0;
+	u64 mapped_lun = 0, target_lun = 0;
 	int ret = -1, res_holder = 0, all_tg_pt = 0, arg, token;
-	u16 port_rpti = 0, tpgt = 0;
-	u8 type = 0, scope;
+	u16 tpgt = 0;
+	u8 type = 0;
 
 	if (dev->transport->transport_flags & TRANSPORT_FLAG_PASSTHROUGH)
 		return 0;
@@ -1115,7 +1667,6 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 			break;
 		case Opt_res_scope:
 			match_int(args, &arg);
-			scope = (u8)arg;
 			break;
 		case Opt_res_all_tg_pt:
 			match_int(args, &arg);
@@ -1123,7 +1674,7 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 			break;
 		case Opt_mapped_lun:
 			match_int(args, &arg);
-			mapped_lun = (u32)arg;
+			mapped_lun = (u64)arg;
 			break;
 		/*
 		 * PR APTPL Metadata for Target Port
@@ -1155,11 +1706,10 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 			break;
 		case Opt_port_rtpi:
 			match_int(args, &arg);
-			port_rpti = (u16)arg;
 			break;
 		case Opt_target_lun:
 			match_int(args, &arg);
-			target_lun = (u32)arg;
+			target_lun = (u64)arg;
 			break;
 		default:
 			break;
@@ -1223,13 +1773,13 @@ TB_CIT_SETUP(dev_pr, &target_core_dev_pr_ops, NULL, target_core_dev_pr_attrs);
 static ssize_t target_core_show_dev_info(void *p, char *page)
 {
 	struct se_device *dev = p;
-	struct se_subsystem_api *t = dev->transport;
 	int bl = 0;
 	ssize_t read_bytes = 0;
 
 	transport_dump_dev_state(dev, page, &bl);
 	read_bytes += bl;
-	read_bytes += t->show_configfs_dev_params(dev, page+read_bytes);
+	read_bytes += dev->transport->show_configfs_dev_params(dev,
+			page+read_bytes);
 	return read_bytes;
 }
 
@@ -1247,9 +1797,8 @@ static ssize_t target_core_store_dev_control(
 	size_t count)
 {
 	struct se_device *dev = p;
-	struct se_subsystem_api *t = dev->transport;
 
-	return t->set_configfs_dev_params(dev, page, count);
+	return dev->transport->set_configfs_dev_params(dev, page, count);
 }
 
 static struct target_core_configfs_attribute target_core_attr_dev_control = {
@@ -2339,21 +2888,16 @@ static ssize_t target_core_alua_tg_pt_gp_show_attr_members(
 	struct t10_alua_tg_pt_gp *tg_pt_gp,
 	char *page)
 {
-	struct se_port *port;
-	struct se_portal_group *tpg;
 	struct se_lun *lun;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
 	ssize_t len = 0, cur_len;
 	unsigned char buf[TG_PT_GROUP_NAME_BUF];
 
 	memset(buf, 0, TG_PT_GROUP_NAME_BUF);
 
 	spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-	list_for_each_entry(tg_pt_gp_mem, &tg_pt_gp->tg_pt_gp_mem_list,
-			tg_pt_gp_mem_list) {
-		port = tg_pt_gp_mem->tg_pt;
-		tpg = port->sep_tpg;
-		lun = port->sep_lun;
+	list_for_each_entry(lun, &tg_pt_gp->tg_pt_gp_lun_list,
+			lun_tg_pt_gp_link) {
+		struct se_portal_group *tpg = lun->lun_tpg;
 
 		cur_len = snprintf(buf, TG_PT_GROUP_NAME_BUF, "%s/%s/tpgt_%hu"
 			"/%s\n", tpg->se_tpg_tfo->get_fabric_name(),
@@ -2526,9 +3070,9 @@ static struct config_group *target_core_make_subdev(
 	const char *name)
 {
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	struct se_subsystem_api *t;
 	struct config_item *hba_ci = &group->cg_item;
 	struct se_hba *hba = item_to_hba(hba_ci);
+	struct target_backend *tb = hba->backend;
 	struct se_device *dev;
 	struct config_group *dev_cg = NULL, *tg_pt_gp_cg = NULL;
 	struct config_group *dev_stat_grp = NULL;
@@ -2537,10 +3081,6 @@ static struct config_group *target_core_make_subdev(
 	ret = mutex_lock_interruptible(&hba->hba_access_mutex);
 	if (ret)
 		return ERR_PTR(ret);
-	/*
-	 * Locate the struct se_subsystem_api from parent's struct se_hba.
-	 */
-	t = hba->transport;
 
 	dev = target_alloc_device(hba, name);
 	if (!dev)
@@ -2553,17 +3093,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, &t->tb_cits.tb_dev_cit);
+	config_group_init_type_name(dev_cg, name, &tb->tb_dev_cit);
 	config_group_init_type_name(&dev->dev_attrib.da_group, "attrib",
-			&t->tb_cits.tb_dev_attrib_cit);
+			&tb->tb_dev_attrib_cit);
 	config_group_init_type_name(&dev->dev_pr_group, "pr",
-			&t->tb_cits.tb_dev_pr_cit);
+			&tb->tb_dev_pr_cit);
 	config_group_init_type_name(&dev->t10_wwn.t10_wwn_group, "wwn",
-			&t->tb_cits.tb_dev_wwn_cit);
+			&tb->tb_dev_wwn_cit);
 	config_group_init_type_name(&dev->t10_alua.alua_tg_pt_gps_group,
-			"alua", &t->tb_cits.tb_dev_alua_tg_pt_gps_cit);
+			"alua", &tb->tb_dev_alua_tg_pt_gps_cit);
 	config_group_init_type_name(&dev->dev_stat_grps.stat_group,
-			"statistics", &t->tb_cits.tb_dev_stat_cit);
+			"statistics", &tb->tb_dev_stat_cit);
 
 	dev_cg->default_groups[0] = &dev->dev_attrib.da_group;
 	dev_cg->default_groups[1] = &dev->dev_pr_group;
@@ -2693,8 +3233,8 @@ static ssize_t target_core_hba_show_attr_hba_info(
 	char *page)
 {
 	return sprintf(page, "HBA Index: %d plugin: %s version: %s\n",
-			hba->hba_id, hba->transport->name,
-			TARGET_CORE_CONFIGFS_VERSION);
+			hba->hba_id, hba->backend->ops->name,
+			TARGET_CORE_VERSION);
 }
 
 SE_HBA_ATTR_RO(hba_info);
@@ -2713,11 +3253,10 @@ static ssize_t target_core_hba_show_attr_hba_mode(struct se_hba *hba,
 static ssize_t target_core_hba_store_attr_hba_mode(struct se_hba *hba,
 				const char *page, size_t count)
 {
-	struct se_subsystem_api *transport = hba->transport;
 	unsigned long mode_flag;
 	int ret;
 
-	if (transport->pmode_enable_hba == NULL)
+	if (hba->backend->ops->pmode_enable_hba == NULL)
 		return -EINVAL;
 
 	ret = kstrtoul(page, 0, &mode_flag);
@@ -2731,7 +3270,7 @@ static ssize_t target_core_hba_store_attr_hba_mode(struct se_hba *hba,
 		return -EINVAL;
 	}
 
-	ret = transport->pmode_enable_hba(hba, mode_flag);
+	ret = hba->backend->ops->pmode_enable_hba(hba, mode_flag);
 	if (ret < 0)
 		return -EINVAL;
 	if (ret > 0)
@@ -2857,16 +3396,15 @@ 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)
+void target_setup_backend_cits(struct target_backend *tb)
 {
-	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);
+	target_core_setup_dev_cit(tb);
+	target_core_setup_dev_attrib_cit(tb);
+	target_core_setup_dev_pr_cit(tb);
+	target_core_setup_dev_wwn_cit(tb);
+	target_core_setup_dev_alua_tg_pt_gps_cit(tb);
+	target_core_setup_dev_stat_cit(tb);
 }
-EXPORT_SYMBOL(target_core_setup_sub_cits);
 
 static int __init target_core_init_configfs(void)
 {
@@ -2968,7 +3506,7 @@ static int __init target_core_init_configfs(void)
 		goto out_global;
 	}
 	pr_debug("TARGET_CORE[0]: Initialized ConfigFS Fabric"
-		" Infrastructure: "TARGET_CORE_CONFIGFS_VERSION" on %s/%s"
+		" Infrastructure: "TARGET_CORE_VERSION" on %s/%s"
 		" on "UTS_RELEASE"\n", utsname()->sysname, utsname()->machine);
 	/*
 	 * Register built-in RAMDISK subsystem logic for virtual LUN 0
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index ce5f768181ff..09e682b1c549 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -36,8 +36,8 @@
 #include <asm/unaligned.h>
 #include <net/sock.h>
 #include <net/tcp.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_device.h>
+#include <scsi/scsi_common.h>
+#include <scsi/scsi_proto.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
@@ -56,40 +56,37 @@ static struct se_hba *lun0_hba;
 struct se_device *g_lun0_dev;
 
 sense_reason_t
-transport_lookup_cmd_lun(struct se_cmd *se_cmd, u32 unpacked_lun)
+transport_lookup_cmd_lun(struct se_cmd *se_cmd, u64 unpacked_lun)
 {
 	struct se_lun *se_lun = NULL;
 	struct se_session *se_sess = se_cmd->se_sess;
-	struct se_device *dev;
-	unsigned long flags;
-
-	if (unpacked_lun >= TRANSPORT_MAX_LUNS_PER_TPG)
-		return TCM_NON_EXISTENT_LUN;
-
-	spin_lock_irqsave(&se_sess->se_node_acl->device_list_lock, flags);
-	se_cmd->se_deve = se_sess->se_node_acl->device_list[unpacked_lun];
-	if (se_cmd->se_deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS) {
-		struct se_dev_entry *deve = se_cmd->se_deve;
+	struct se_node_acl *nacl = se_sess->se_node_acl;
+	struct se_dev_entry *deve;
 
-		deve->total_cmds++;
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, unpacked_lun);
+	if (deve) {
+		atomic_long_inc(&deve->total_cmds);
 
 		if ((se_cmd->data_direction == DMA_TO_DEVICE) &&
 		    (deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY)) {
 			pr_err("TARGET_CORE[%s]: Detected WRITE_PROTECTED LUN"
-				" Access for 0x%08x\n",
+				" Access for 0x%08llx\n",
 				se_cmd->se_tfo->get_fabric_name(),
 				unpacked_lun);
-			spin_unlock_irqrestore(&se_sess->se_node_acl->device_list_lock, flags);
+			rcu_read_unlock();
 			return TCM_WRITE_PROTECTED;
 		}
 
 		if (se_cmd->data_direction == DMA_TO_DEVICE)
-			deve->write_bytes += se_cmd->data_length;
+			atomic_long_add(se_cmd->data_length,
+					&deve->write_bytes);
 		else if (se_cmd->data_direction == DMA_FROM_DEVICE)
-			deve->read_bytes += se_cmd->data_length;
+			atomic_long_add(se_cmd->data_length,
+					&deve->read_bytes);
 
-		se_lun = deve->se_lun;
-		se_cmd->se_lun = deve->se_lun;
+		se_lun = rcu_dereference(deve->se_lun);
+		se_cmd->se_lun = rcu_dereference(deve->se_lun);
 		se_cmd->pr_res_key = deve->pr_res_key;
 		se_cmd->orig_fe_lun = unpacked_lun;
 		se_cmd->se_cmd_flags |= SCF_SE_LUN_CMD;
@@ -97,7 +94,7 @@ transport_lookup_cmd_lun(struct se_cmd *se_cmd, u32 unpacked_lun)
 		percpu_ref_get(&se_lun->lun_ref);
 		se_cmd->lun_ref_active = true;
 	}
-	spin_unlock_irqrestore(&se_sess->se_node_acl->device_list_lock, flags);
+	rcu_read_unlock();
 
 	if (!se_lun) {
 		/*
@@ -107,7 +104,7 @@ transport_lookup_cmd_lun(struct se_cmd *se_cmd, u32 unpacked_lun)
 		 */
 		if (unpacked_lun != 0) {
 			pr_err("TARGET_CORE[%s]: Detected NON_EXISTENT_LUN"
-				" Access for 0x%08x\n",
+				" Access for 0x%08llx\n",
 				se_cmd->se_tfo->get_fabric_name(),
 				unpacked_lun);
 			return TCM_NON_EXISTENT_LUN;
@@ -119,64 +116,66 @@ transport_lookup_cmd_lun(struct se_cmd *se_cmd, u32 unpacked_lun)
 		    (se_cmd->data_direction != DMA_NONE))
 			return TCM_WRITE_PROTECTED;
 
-		se_lun = &se_sess->se_tpg->tpg_virt_lun0;
-		se_cmd->se_lun = &se_sess->se_tpg->tpg_virt_lun0;
+		se_lun = se_sess->se_tpg->tpg_virt_lun0;
+		se_cmd->se_lun = se_sess->se_tpg->tpg_virt_lun0;
 		se_cmd->orig_fe_lun = 0;
 		se_cmd->se_cmd_flags |= SCF_SE_LUN_CMD;
 
 		percpu_ref_get(&se_lun->lun_ref);
 		se_cmd->lun_ref_active = true;
 	}
+	/*
+	 * RCU reference protected by percpu se_lun->lun_ref taken above that
+	 * must drop to zero (including initial reference) before this se_lun
+	 * pointer can be kfree_rcu() by the final se_lun->lun_group put via
+	 * target_core_fabric_configfs.c:target_fabric_port_release
+	 */
+	se_cmd->se_dev = rcu_dereference_raw(se_lun->lun_se_dev);
+	atomic_long_inc(&se_cmd->se_dev->num_cmds);
 
-	/* Directly associate cmd with se_dev */
-	se_cmd->se_dev = se_lun->lun_se_dev;
-
-	dev = se_lun->lun_se_dev;
-	atomic_long_inc(&dev->num_cmds);
 	if (se_cmd->data_direction == DMA_TO_DEVICE)
-		atomic_long_add(se_cmd->data_length, &dev->write_bytes);
+		atomic_long_add(se_cmd->data_length,
+				&se_cmd->se_dev->write_bytes);
 	else if (se_cmd->data_direction == DMA_FROM_DEVICE)
-		atomic_long_add(se_cmd->data_length, &dev->read_bytes);
+		atomic_long_add(se_cmd->data_length,
+				&se_cmd->se_dev->read_bytes);
 
 	return 0;
 }
 EXPORT_SYMBOL(transport_lookup_cmd_lun);
 
-int transport_lookup_tmr_lun(struct se_cmd *se_cmd, u32 unpacked_lun)
+int transport_lookup_tmr_lun(struct se_cmd *se_cmd, u64 unpacked_lun)
 {
 	struct se_dev_entry *deve;
 	struct se_lun *se_lun = NULL;
 	struct se_session *se_sess = se_cmd->se_sess;
+	struct se_node_acl *nacl = se_sess->se_node_acl;
 	struct se_tmr_req *se_tmr = se_cmd->se_tmr_req;
 	unsigned long flags;
 
-	if (unpacked_lun >= TRANSPORT_MAX_LUNS_PER_TPG)
-		return -ENODEV;
-
-	spin_lock_irqsave(&se_sess->se_node_acl->device_list_lock, flags);
-	se_cmd->se_deve = se_sess->se_node_acl->device_list[unpacked_lun];
-	deve = se_cmd->se_deve;
-
-	if (deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS) {
-		se_tmr->tmr_lun = deve->se_lun;
-		se_cmd->se_lun = deve->se_lun;
-		se_lun = deve->se_lun;
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, unpacked_lun);
+	if (deve) {
+		se_tmr->tmr_lun = rcu_dereference(deve->se_lun);
+		se_cmd->se_lun = rcu_dereference(deve->se_lun);
+		se_lun = rcu_dereference(deve->se_lun);
 		se_cmd->pr_res_key = deve->pr_res_key;
 		se_cmd->orig_fe_lun = unpacked_lun;
 	}
-	spin_unlock_irqrestore(&se_sess->se_node_acl->device_list_lock, flags);
+	rcu_read_unlock();
 
 	if (!se_lun) {
 		pr_debug("TARGET_CORE[%s]: Detected NON_EXISTENT_LUN"
-			" Access for 0x%08x\n",
+			" Access for 0x%08llx\n",
 			se_cmd->se_tfo->get_fabric_name(),
 			unpacked_lun);
 		return -ENODEV;
 	}
-
-	/* Directly associate cmd with se_dev */
-	se_cmd->se_dev = se_lun->lun_se_dev;
-	se_tmr->tmr_dev = se_lun->lun_se_dev;
+	/*
+	 * XXX: Add percpu se_lun->lun_ref reference count for TMR
+	 */
+	se_cmd->se_dev = rcu_dereference_raw(se_lun->lun_se_dev);
+	se_tmr->tmr_dev = rcu_dereference_raw(se_lun->lun_se_dev);
 
 	spin_lock_irqsave(&se_tmr->tmr_dev->se_tmr_lock, flags);
 	list_add_tail(&se_tmr->tmr_list, &se_tmr->tmr_dev->dev_tmr_list);
@@ -186,9 +185,24 @@ int transport_lookup_tmr_lun(struct se_cmd *se_cmd, u32 unpacked_lun)
 }
 EXPORT_SYMBOL(transport_lookup_tmr_lun);
 
+bool target_lun_is_rdonly(struct se_cmd *cmd)
+{
+	struct se_session *se_sess = cmd->se_sess;
+	struct se_dev_entry *deve;
+	bool ret;
+
+	rcu_read_lock();
+	deve = target_nacl_find_deve(se_sess->se_node_acl, cmd->orig_fe_lun);
+	ret = (deve && deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY);
+	rcu_read_unlock();
+
+	return ret;
+}
+EXPORT_SYMBOL(target_lun_is_rdonly);
+
 /*
  * This function is called from core_scsi3_emulate_pro_register_and_move()
- * and core_scsi3_decode_spec_i_port(), and will increment &deve->pr_ref_count
+ * and core_scsi3_decode_spec_i_port(), and will increment &deve->pr_kref
  * when a matching rtpi is found.
  */
 struct se_dev_entry *core_get_se_deve_from_rtpi(
@@ -197,231 +211,238 @@ struct se_dev_entry *core_get_se_deve_from_rtpi(
 {
 	struct se_dev_entry *deve;
 	struct se_lun *lun;
-	struct se_port *port;
 	struct se_portal_group *tpg = nacl->se_tpg;
-	u32 i;
-
-	spin_lock_irq(&nacl->device_list_lock);
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		deve = nacl->device_list[i];
 
-		if (!(deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS))
-			continue;
-
-		lun = deve->se_lun;
+	rcu_read_lock();
+	hlist_for_each_entry_rcu(deve, &nacl->lun_entry_hlist, link) {
+		lun = rcu_dereference(deve->se_lun);
 		if (!lun) {
 			pr_err("%s device entries device pointer is"
 				" NULL, but Initiator has access.\n",
 				tpg->se_tpg_tfo->get_fabric_name());
 			continue;
 		}
-		port = lun->lun_sep;
-		if (!port) {
-			pr_err("%s device entries device pointer is"
-				" NULL, but Initiator has access.\n",
-				tpg->se_tpg_tfo->get_fabric_name());
-			continue;
-		}
-		if (port->sep_rtpi != rtpi)
+		if (lun->lun_rtpi != rtpi)
 			continue;
 
-		atomic_inc_mb(&deve->pr_ref_count);
-		spin_unlock_irq(&nacl->device_list_lock);
+		kref_get(&deve->pr_kref);
+		rcu_read_unlock();
 
 		return deve;
 	}
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 
 	return NULL;
 }
 
-int core_free_device_list_for_node(
+void core_free_device_list_for_node(
 	struct se_node_acl *nacl,
 	struct se_portal_group *tpg)
 {
 	struct se_dev_entry *deve;
-	struct se_lun *lun;
-	u32 i;
-
-	if (!nacl->device_list)
-		return 0;
-
-	spin_lock_irq(&nacl->device_list_lock);
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		deve = nacl->device_list[i];
-
-		if (!(deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS))
-			continue;
-
-		if (!deve->se_lun) {
-			pr_err("%s device entries device pointer is"
-				" NULL, but Initiator has access.\n",
-				tpg->se_tpg_tfo->get_fabric_name());
-			continue;
-		}
-		lun = deve->se_lun;
 
-		spin_unlock_irq(&nacl->device_list_lock);
-		core_disable_device_list_for_node(lun, NULL, deve->mapped_lun,
-			TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg);
-		spin_lock_irq(&nacl->device_list_lock);
+	mutex_lock(&nacl->lun_entry_mutex);
+	hlist_for_each_entry_rcu(deve, &nacl->lun_entry_hlist, link) {
+		struct se_lun *lun = rcu_dereference_check(deve->se_lun,
+					lockdep_is_held(&nacl->lun_entry_mutex));
+		core_disable_device_list_for_node(lun, deve, nacl, tpg);
 	}
-	spin_unlock_irq(&nacl->device_list_lock);
-
-	array_free(nacl->device_list, TRANSPORT_MAX_LUNS_PER_TPG);
-	nacl->device_list = NULL;
-
-	return 0;
+	mutex_unlock(&nacl->lun_entry_mutex);
 }
 
 void core_update_device_list_access(
-	u32 mapped_lun,
+	u64 mapped_lun,
 	u32 lun_access,
 	struct se_node_acl *nacl)
 {
 	struct se_dev_entry *deve;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[mapped_lun];
-	if (lun_access & TRANSPORT_LUNFLAGS_READ_WRITE) {
-		deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_ONLY;
-		deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_WRITE;
-	} else {
-		deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_WRITE;
-		deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_ONLY;
+	mutex_lock(&nacl->lun_entry_mutex);
+	deve = target_nacl_find_deve(nacl, mapped_lun);
+	if (deve) {
+		if (lun_access & TRANSPORT_LUNFLAGS_READ_WRITE) {
+			deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_ONLY;
+			deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_WRITE;
+		} else {
+			deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_WRITE;
+			deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_ONLY;
+		}
 	}
-	spin_unlock_irq(&nacl->device_list_lock);
+	mutex_unlock(&nacl->lun_entry_mutex);
 }
 
-/*      core_enable_device_list_for_node():
- *
- *
+/*
+ * Called with rcu_read_lock or nacl->device_list_lock held.
  */
+struct se_dev_entry *target_nacl_find_deve(struct se_node_acl *nacl, u64 mapped_lun)
+{
+	struct se_dev_entry *deve;
+
+	hlist_for_each_entry_rcu(deve, &nacl->lun_entry_hlist, link)
+		if (deve->mapped_lun == mapped_lun)
+			return deve;
+
+	return NULL;
+}
+EXPORT_SYMBOL(target_nacl_find_deve);
+
+void target_pr_kref_release(struct kref *kref)
+{
+	struct se_dev_entry *deve = container_of(kref, struct se_dev_entry,
+						 pr_kref);
+	complete(&deve->pr_comp);
+}
+
+static void
+target_luns_data_has_changed(struct se_node_acl *nacl, struct se_dev_entry *new,
+			     bool skip_new)
+{
+	struct se_dev_entry *tmp;
+
+	rcu_read_lock();
+	hlist_for_each_entry_rcu(tmp, &nacl->lun_entry_hlist, link) {
+		if (skip_new && tmp == new)
+			continue;
+		core_scsi3_ua_allocate(tmp, 0x3F,
+				       ASCQ_3FH_REPORTED_LUNS_DATA_HAS_CHANGED);
+	}
+	rcu_read_unlock();
+}
+
 int core_enable_device_list_for_node(
 	struct se_lun *lun,
 	struct se_lun_acl *lun_acl,
-	u32 mapped_lun,
+	u64 mapped_lun,
 	u32 lun_access,
 	struct se_node_acl *nacl,
 	struct se_portal_group *tpg)
 {
-	struct se_port *port = lun->lun_sep;
-	struct se_dev_entry *deve;
-
-	spin_lock_irq(&nacl->device_list_lock);
-
-	deve = nacl->device_list[mapped_lun];
-
-	/*
-	 * Check if the call is handling demo mode -> explicit LUN ACL
-	 * transition.  This transition must be for the same struct se_lun
-	 * + mapped_lun that was setup in demo mode..
-	 */
-	if (deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS) {
-		if (deve->se_lun_acl != NULL) {
-			pr_err("struct se_dev_entry->se_lun_acl"
-			       " already set for demo mode -> explicit"
-			       " LUN ACL transition\n");
-			spin_unlock_irq(&nacl->device_list_lock);
+	struct se_dev_entry *orig, *new;
+
+	new = kzalloc(sizeof(*new), GFP_KERNEL);
+	if (!new) {
+		pr_err("Unable to allocate se_dev_entry memory\n");
+		return -ENOMEM;
+	}
+
+	atomic_set(&new->ua_count, 0);
+	spin_lock_init(&new->ua_lock);
+	INIT_LIST_HEAD(&new->ua_list);
+	INIT_LIST_HEAD(&new->lun_link);
+
+	new->mapped_lun = mapped_lun;
+	kref_init(&new->pr_kref);
+	init_completion(&new->pr_comp);
+
+	if (lun_access & TRANSPORT_LUNFLAGS_READ_WRITE)
+		new->lun_flags |= TRANSPORT_LUNFLAGS_READ_WRITE;
+	else
+		new->lun_flags |= TRANSPORT_LUNFLAGS_READ_ONLY;
+
+	new->creation_time = get_jiffies_64();
+	new->attach_count++;
+
+	mutex_lock(&nacl->lun_entry_mutex);
+	orig = target_nacl_find_deve(nacl, mapped_lun);
+	if (orig && orig->se_lun) {
+		struct se_lun *orig_lun = rcu_dereference_check(orig->se_lun,
+					lockdep_is_held(&nacl->lun_entry_mutex));
+
+		if (orig_lun != lun) {
+			pr_err("Existing orig->se_lun doesn't match new lun"
+			       " for dynamic -> explicit NodeACL conversion:"
+				" %s\n", nacl->initiatorname);
+			mutex_unlock(&nacl->lun_entry_mutex);
+			kfree(new);
 			return -EINVAL;
 		}
-		if (deve->se_lun != lun) {
-			pr_err("struct se_dev_entry->se_lun does"
-			       " match passed struct se_lun for demo mode"
-			       " -> explicit LUN ACL transition\n");
-			spin_unlock_irq(&nacl->device_list_lock);
-			return -EINVAL;
-		}
-		deve->se_lun_acl = lun_acl;
+		BUG_ON(orig->se_lun_acl != NULL);
 
-		if (lun_access & TRANSPORT_LUNFLAGS_READ_WRITE) {
-			deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_ONLY;
-			deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_WRITE;
-		} else {
-			deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_WRITE;
-			deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_ONLY;
-		}
+		rcu_assign_pointer(new->se_lun, lun);
+		rcu_assign_pointer(new->se_lun_acl, lun_acl);
+		hlist_del_rcu(&orig->link);
+		hlist_add_head_rcu(&new->link, &nacl->lun_entry_hlist);
+		mutex_unlock(&nacl->lun_entry_mutex);
 
-		spin_unlock_irq(&nacl->device_list_lock);
-		return 0;
-	}
+		spin_lock(&lun->lun_deve_lock);
+		list_del(&orig->lun_link);
+		list_add_tail(&new->lun_link, &lun->lun_deve_list);
+		spin_unlock(&lun->lun_deve_lock);
+
+		kref_put(&orig->pr_kref, target_pr_kref_release);
+		wait_for_completion(&orig->pr_comp);
 
-	deve->se_lun = lun;
-	deve->se_lun_acl = lun_acl;
-	deve->mapped_lun = mapped_lun;
-	deve->lun_flags |= TRANSPORT_LUNFLAGS_INITIATOR_ACCESS;
-
-	if (lun_access & TRANSPORT_LUNFLAGS_READ_WRITE) {
-		deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_ONLY;
-		deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_WRITE;
-	} else {
-		deve->lun_flags &= ~TRANSPORT_LUNFLAGS_READ_WRITE;
-		deve->lun_flags |= TRANSPORT_LUNFLAGS_READ_ONLY;
+		target_luns_data_has_changed(nacl, new, true);
+		kfree_rcu(orig, rcu_head);
+		return 0;
 	}
 
-	deve->creation_time = get_jiffies_64();
-	deve->attach_count++;
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_assign_pointer(new->se_lun, lun);
+	rcu_assign_pointer(new->se_lun_acl, lun_acl);
+	hlist_add_head_rcu(&new->link, &nacl->lun_entry_hlist);
+	mutex_unlock(&nacl->lun_entry_mutex);
 
-	spin_lock_bh(&port->sep_alua_lock);
-	list_add_tail(&deve->alua_port_list, &port->sep_alua_list);
-	spin_unlock_bh(&port->sep_alua_lock);
+	spin_lock(&lun->lun_deve_lock);
+	list_add_tail(&new->lun_link, &lun->lun_deve_list);
+	spin_unlock(&lun->lun_deve_lock);
 
+	target_luns_data_has_changed(nacl, new, true);
 	return 0;
 }
 
-/*      core_disable_device_list_for_node():
- *
- *
+/*
+ *	Called with se_node_acl->lun_entry_mutex held.
  */
-int core_disable_device_list_for_node(
+void core_disable_device_list_for_node(
 	struct se_lun *lun,
-	struct se_lun_acl *lun_acl,
-	u32 mapped_lun,
-	u32 lun_access,
+	struct se_dev_entry *orig,
 	struct se_node_acl *nacl,
 	struct se_portal_group *tpg)
 {
-	struct se_port *port = lun->lun_sep;
-	struct se_dev_entry *deve = nacl->device_list[mapped_lun];
-
+	/*
+	 * rcu_dereference_raw protected by se_lun->lun_group symlink
+	 * reference to se_device->dev_group.
+	 */
+	struct se_device *dev = rcu_dereference_raw(lun->lun_se_dev);
 	/*
 	 * If the MappedLUN entry is being disabled, the entry in
-	 * port->sep_alua_list must be removed now before clearing the
+	 * lun->lun_deve_list must be removed now before clearing the
 	 * struct se_dev_entry pointers below as logic in
 	 * core_alua_do_transition_tg_pt() depends on these being present.
 	 *
 	 * deve->se_lun_acl will be NULL for demo-mode created LUNs
 	 * that have not been explicitly converted to MappedLUNs ->
-	 * struct se_lun_acl, but we remove deve->alua_port_list from
-	 * port->sep_alua_list. This also means that active UAs and
+	 * struct se_lun_acl, but we remove deve->lun_link from
+	 * lun->lun_deve_list. This also means that active UAs and
 	 * NodeACL context specific PR metadata for demo-mode
 	 * MappedLUN *deve will be released below..
 	 */
-	spin_lock_bh(&port->sep_alua_lock);
-	list_del(&deve->alua_port_list);
-	spin_unlock_bh(&port->sep_alua_lock);
+	spin_lock(&lun->lun_deve_lock);
+	list_del(&orig->lun_link);
+	spin_unlock(&lun->lun_deve_lock);
 	/*
-	 * Wait for any in process SPEC_I_PT=1 or REGISTER_AND_MOVE
-	 * PR operation to complete.
+	 * Disable struct se_dev_entry LUN ACL mapping
 	 */
-	while (atomic_read(&deve->pr_ref_count) != 0)
-		cpu_relax();
-
-	spin_lock_irq(&nacl->device_list_lock);
+	core_scsi3_ua_release_all(orig);
+
+	hlist_del_rcu(&orig->link);
+	clear_bit(DEF_PR_REG_ACTIVE, &orig->deve_flags);
+	rcu_assign_pointer(orig->se_lun, NULL);
+	rcu_assign_pointer(orig->se_lun_acl, NULL);
+	orig->lun_flags = 0;
+	orig->creation_time = 0;
+	orig->attach_count--;
 	/*
-	 * Disable struct se_dev_entry LUN ACL mapping
+	 * Before firing off RCU callback, wait for any in process SPEC_I_PT=1
+	 * or REGISTER_AND_MOVE PR operation to complete.
 	 */
-	core_scsi3_ua_release_all(deve);
-	deve->se_lun = NULL;
-	deve->se_lun_acl = NULL;
-	deve->lun_flags = 0;
-	deve->creation_time = 0;
-	deve->attach_count--;
-	spin_unlock_irq(&nacl->device_list_lock);
-
-	core_scsi3_free_pr_reg_from_nacl(lun->lun_se_dev, nacl);
-	return 0;
+	kref_put(&orig->pr_kref, target_pr_kref_release);
+	wait_for_completion(&orig->pr_comp);
+
+	kfree_rcu(orig, rcu_head);
+
+	core_scsi3_free_pr_reg_from_nacl(dev, nacl);
+	target_luns_data_has_changed(nacl, NULL, false);
 }
 
 /*      core_clear_lun_from_tpg():
@@ -432,53 +453,35 @@ void core_clear_lun_from_tpg(struct se_lun *lun, struct se_portal_group *tpg)
 {
 	struct se_node_acl *nacl;
 	struct se_dev_entry *deve;
-	u32 i;
 
-	spin_lock_irq(&tpg->acl_node_lock);
+	mutex_lock(&tpg->acl_node_mutex);
 	list_for_each_entry(nacl, &tpg->acl_node_list, acl_list) {
-		spin_unlock_irq(&tpg->acl_node_lock);
 
-		spin_lock_irq(&nacl->device_list_lock);
-		for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-			deve = nacl->device_list[i];
-			if (lun != deve->se_lun)
-				continue;
-			spin_unlock_irq(&nacl->device_list_lock);
+		mutex_lock(&nacl->lun_entry_mutex);
+		hlist_for_each_entry_rcu(deve, &nacl->lun_entry_hlist, link) {
+			struct se_lun *tmp_lun = rcu_dereference_check(deve->se_lun,
+					lockdep_is_held(&nacl->lun_entry_mutex));
 
-			core_disable_device_list_for_node(lun, NULL,
-				deve->mapped_lun, TRANSPORT_LUNFLAGS_NO_ACCESS,
-				nacl, tpg);
+			if (lun != tmp_lun)
+				continue;
 
-			spin_lock_irq(&nacl->device_list_lock);
+			core_disable_device_list_for_node(lun, deve, nacl, tpg);
 		}
-		spin_unlock_irq(&nacl->device_list_lock);
-
-		spin_lock_irq(&tpg->acl_node_lock);
+		mutex_unlock(&nacl->lun_entry_mutex);
 	}
-	spin_unlock_irq(&tpg->acl_node_lock);
+	mutex_unlock(&tpg->acl_node_mutex);
 }
 
-static struct se_port *core_alloc_port(struct se_device *dev)
+int core_alloc_rtpi(struct se_lun *lun, struct se_device *dev)
 {
-	struct se_port *port, *port_tmp;
-
-	port = kzalloc(sizeof(struct se_port), GFP_KERNEL);
-	if (!port) {
-		pr_err("Unable to allocate struct se_port\n");
-		return ERR_PTR(-ENOMEM);
-	}
-	INIT_LIST_HEAD(&port->sep_alua_list);
-	INIT_LIST_HEAD(&port->sep_list);
-	atomic_set(&port->sep_tg_pt_secondary_offline, 0);
-	spin_lock_init(&port->sep_alua_lock);
-	mutex_init(&port->sep_tg_pt_md_mutex);
+	struct se_lun *tmp;
 
 	spin_lock(&dev->se_port_lock);
-	if (dev->dev_port_count == 0x0000ffff) {
+	if (dev->export_count == 0x0000ffff) {
 		pr_warn("Reached dev->dev_port_count =="
 				" 0x0000ffff\n");
 		spin_unlock(&dev->se_port_lock);
-		return ERR_PTR(-ENOSPC);
+		return -ENOSPC;
 	}
 again:
 	/*
@@ -493,133 +496,23 @@ again:
 	 * 2h        Relative port 2, historically known as port B
 	 * 3h to FFFFh    Relative port 3 through 65 535
 	 */
-	port->sep_rtpi = dev->dev_rpti_counter++;
-	if (!port->sep_rtpi)
+	lun->lun_rtpi = dev->dev_rpti_counter++;
+	if (!lun->lun_rtpi)
 		goto again;
 
-	list_for_each_entry(port_tmp, &dev->dev_sep_list, sep_list) {
+	list_for_each_entry(tmp, &dev->dev_sep_list, lun_dev_link) {
 		/*
 		 * Make sure RELATIVE TARGET PORT IDENTIFIER is unique
 		 * for 16-bit wrap..
 		 */
-		if (port->sep_rtpi == port_tmp->sep_rtpi)
+		if (lun->lun_rtpi == tmp->lun_rtpi)
 			goto again;
 	}
 	spin_unlock(&dev->se_port_lock);
 
-	return port;
-}
-
-static void core_export_port(
-	struct se_device *dev,
-	struct se_portal_group *tpg,
-	struct se_port *port,
-	struct se_lun *lun)
-{
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem = NULL;
-
-	spin_lock(&dev->se_port_lock);
-	spin_lock(&lun->lun_sep_lock);
-	port->sep_tpg = tpg;
-	port->sep_lun = lun;
-	lun->lun_sep = port;
-	spin_unlock(&lun->lun_sep_lock);
-
-	list_add_tail(&port->sep_list, &dev->dev_sep_list);
-	spin_unlock(&dev->se_port_lock);
-
-	if (!(dev->transport->transport_flags & TRANSPORT_FLAG_PASSTHROUGH) &&
-	    !(dev->se_hba->hba_flags & HBA_FLAGS_INTERNAL_USE)) {
-		tg_pt_gp_mem = core_alua_allocate_tg_pt_gp_mem(port);
-		if (IS_ERR(tg_pt_gp_mem) || !tg_pt_gp_mem) {
-			pr_err("Unable to allocate t10_alua_tg_pt"
-					"_gp_member_t\n");
-			return;
-		}
-		spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-		__core_alua_attach_tg_pt_gp_mem(tg_pt_gp_mem,
-			dev->t10_alua.default_tg_pt_gp);
-		spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-		pr_debug("%s/%s: Adding to default ALUA Target Port"
-			" Group: alua/default_tg_pt_gp\n",
-			dev->transport->name, tpg->se_tpg_tfo->get_fabric_name());
-	}
-
-	dev->dev_port_count++;
-	port->sep_index = port->sep_rtpi; /* RELATIVE TARGET PORT IDENTIFIER */
-}
-
-/*
- *	Called with struct se_device->se_port_lock spinlock held.
- */
-static void core_release_port(struct se_device *dev, struct se_port *port)
-	__releases(&dev->se_port_lock) __acquires(&dev->se_port_lock)
-{
-	/*
-	 * Wait for any port reference for PR ALL_TG_PT=1 operation
-	 * to complete in __core_scsi3_alloc_registration()
-	 */
-	spin_unlock(&dev->se_port_lock);
-	if (atomic_read(&port->sep_tg_pt_ref_cnt))
-		cpu_relax();
-	spin_lock(&dev->se_port_lock);
-
-	core_alua_free_tg_pt_gp_mem(port);
-
-	list_del(&port->sep_list);
-	dev->dev_port_count--;
-	kfree(port);
-}
-
-int core_dev_export(
-	struct se_device *dev,
-	struct se_portal_group *tpg,
-	struct se_lun *lun)
-{
-	struct se_hba *hba = dev->se_hba;
-	struct se_port *port;
-
-	port = core_alloc_port(dev);
-	if (IS_ERR(port))
-		return PTR_ERR(port);
-
-	lun->lun_se_dev = dev;
-
-	spin_lock(&hba->device_lock);
-	dev->export_count++;
-	spin_unlock(&hba->device_lock);
-
-	core_export_port(dev, tpg, port, lun);
 	return 0;
 }
 
-void core_dev_unexport(
-	struct se_device *dev,
-	struct se_portal_group *tpg,
-	struct se_lun *lun)
-{
-	struct se_hba *hba = dev->se_hba;
-	struct se_port *port = lun->lun_sep;
-
-	spin_lock(&lun->lun_sep_lock);
-	if (lun->lun_se_dev == NULL) {
-		spin_unlock(&lun->lun_sep_lock);
-		return;
-	}
-	spin_unlock(&lun->lun_sep_lock);
-
-	spin_lock(&dev->se_port_lock);
-	core_release_port(dev, port);
-	spin_unlock(&dev->se_port_lock);
-
-	spin_lock(&hba->device_lock);
-	dev->export_count--;
-	spin_unlock(&hba->device_lock);
-
-	lun->lun_sep = NULL;
-	lun->lun_se_dev = NULL;
-}
-
 static void se_release_vpd_for_dev(struct se_device *dev)
 {
 	struct t10_vpd *vpd, *vpd_tmp;
@@ -651,556 +544,19 @@ static u32 se_dev_align_max_sectors(u32 max_sectors, u32 block_size)
 	return aligned_max_sectors;
 }
 
-bool se_dev_check_wce(struct se_device *dev)
-{
-	bool wce = false;
-
-	if (dev->transport->get_write_cache)
-		wce = dev->transport->get_write_cache(dev);
-	else if (dev->dev_attrib.emulate_write_cache > 0)
-		wce = true;
-
-	return wce;
-}
-
-int se_dev_set_max_unmap_lba_count(
-	struct se_device *dev,
-	u32 max_unmap_lba_count)
-{
-	dev->dev_attrib.max_unmap_lba_count = max_unmap_lba_count;
-	pr_debug("dev[%p]: Set max_unmap_lba_count: %u\n",
-			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,
-	u32 max_unmap_block_desc_count)
-{
-	dev->dev_attrib.max_unmap_block_desc_count =
-		max_unmap_block_desc_count;
-	pr_debug("dev[%p]: Set max_unmap_block_desc_count: %u\n",
-			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,
-	u32 unmap_granularity)
-{
-	dev->dev_attrib.unmap_granularity = unmap_granularity;
-	pr_debug("dev[%p]: Set unmap_granularity: %u\n",
-			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,
-	u32 unmap_granularity_alignment)
-{
-	dev->dev_attrib.unmap_granularity_alignment = unmap_granularity_alignment;
-	pr_debug("dev[%p]: Set unmap_granularity_alignment: %u\n",
-			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,
-	u32 max_write_same_len)
-{
-	dev->dev_attrib.max_write_same_len = max_write_same_len;
-	pr_debug("dev[%p]: Set max_write_same_len: %u\n",
-			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)
-{
-	const char *configname;
-
-	configname = config_item_name(&dev->dev_group.cg_item);
-	if (strlen(configname) >= 16) {
-		pr_warn("dev[%p]: Backstore name '%s' is too long for "
-			"INQUIRY_MODEL, truncating to 16 bytes\n", dev,
-			configname);
-	}
-	snprintf(&dev->t10_wwn.model[0], 16, "%s", configname);
-}
-
-int se_dev_set_emulate_model_alias(struct se_device *dev, int flag)
-{
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to change model alias"
-			" while export_count is %d\n",
-			dev, dev->export_count);
-			return -EINVAL;
-	}
-
-	if (flag != 0 && flag != 1) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-
-	if (flag) {
-		dev_set_t10_wwn_model_alias(dev);
-	} else {
-		strncpy(&dev->t10_wwn.model[0],
-			dev->transport->inquiry_prod, 16);
-	}
-	dev->dev_attrib.emulate_model_alias = flag;
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_model_alias);
-
-int se_dev_set_emulate_dpo(struct se_device *dev, int flag)
-{
-	if (flag != 0 && flag != 1) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-
-	if (flag) {
-		pr_err("dpo_emulated not supported\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_dpo);
-
-int se_dev_set_emulate_fua_write(struct se_device *dev, int flag)
-{
-	if (flag != 0 && flag != 1) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	if (flag &&
-	    dev->transport->get_write_cache) {
-		pr_warn("emulate_fua_write not supported for this device, ignoring\n");
-		return 0;
-	}
-	if (dev->export_count) {
-		pr_err("emulate_fua_write cannot be changed with active"
-		       " exports: %d\n", dev->export_count);
-		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)
-{
-	if (flag != 0 && flag != 1) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-
-	if (flag) {
-		pr_err("ua read emulated not supported\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_fua_read);
-
-int se_dev_set_emulate_write_cache(struct se_device *dev, int flag)
-{
-	if (flag != 0 && flag != 1) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	if (flag &&
-	    dev->transport->get_write_cache) {
-		pr_err("emulate_write_cache not supported for this device\n");
-		return -EINVAL;
-	}
-	if (dev->export_count) {
-		pr_err("emulate_write_cache cannot be changed with active"
-		       " exports: %d\n", dev->export_count);
-		return -EINVAL;
-	}
-	dev->dev_attrib.emulate_write_cache = flag;
-	pr_debug("dev[%p]: SE Device WRITE_CACHE_EMULATION flag: %d\n",
-			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)
-{
-	if ((flag != 0) && (flag != 1) && (flag != 2)) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to change SE Device"
-			" UA_INTRLCK_CTRL while export_count is %d\n",
-			dev, dev->export_count);
-		return -EINVAL;
-	}
-	dev->dev_attrib.emulate_ua_intlck_ctrl = flag;
-	pr_debug("dev[%p]: SE Device UA_INTRLCK_CTRL flag: %d\n",
-		dev, dev->dev_attrib.emulate_ua_intlck_ctrl);
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_ua_intlck_ctrl);
-
-int se_dev_set_emulate_tas(struct se_device *dev, int flag)
-{
-	if ((flag != 0) && (flag != 1)) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to change SE Device TAS while"
-			" export_count is %d\n",
-			dev, dev->export_count);
-		return -EINVAL;
-	}
-	dev->dev_attrib.emulate_tas = flag;
-	pr_debug("dev[%p]: SE Device TASK_ABORTED status bit: %s\n",
-		dev, (dev->dev_attrib.emulate_tas) ? "Enabled" : "Disabled");
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_tas);
-
-int se_dev_set_emulate_tpu(struct se_device *dev, int flag)
-{
-	if ((flag != 0) && (flag != 1)) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	/*
-	 * We expect this value to be non-zero when generic Block Layer
-	 * Discard supported is detected iblock_create_virtdevice().
-	 */
-	if (flag && !dev->dev_attrib.max_unmap_block_desc_count) {
-		pr_err("Generic Block Discard not supported\n");
-		return -ENOSYS;
-	}
-
-	dev->dev_attrib.emulate_tpu = flag;
-	pr_debug("dev[%p]: SE Device Thin Provisioning UNMAP bit: %d\n",
-				dev, flag);
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_tpu);
-
-int se_dev_set_emulate_tpws(struct se_device *dev, int flag)
-{
-	if ((flag != 0) && (flag != 1)) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	/*
-	 * We expect this value to be non-zero when generic Block Layer
-	 * Discard supported is detected iblock_create_virtdevice().
-	 */
-	if (flag && !dev->dev_attrib.max_unmap_block_desc_count) {
-		pr_err("Generic Block Discard not supported\n");
-		return -ENOSYS;
-	}
-
-	dev->dev_attrib.emulate_tpws = flag;
-	pr_debug("dev[%p]: SE Device Thin Provisioning WRITE_SAME: %d\n",
-				dev, flag);
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_tpws);
-
-int se_dev_set_emulate_caw(struct se_device *dev, int flag)
-{
-	if (flag != 0 && flag != 1) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	dev->dev_attrib.emulate_caw = flag;
-	pr_debug("dev[%p]: SE Device CompareAndWrite (AtomicTestandSet): %d\n",
-		 dev, flag);
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_caw);
-
-int se_dev_set_emulate_3pc(struct se_device *dev, int flag)
-{
-	if (flag != 0 && flag != 1) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	dev->dev_attrib.emulate_3pc = flag;
-	pr_debug("dev[%p]: SE Device 3rd Party Copy (EXTENDED_COPY): %d\n",
-		dev, flag);
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_emulate_3pc);
-
-int se_dev_set_pi_prot_type(struct se_device *dev, int flag)
-{
-	int rc, old_prot = dev->dev_attrib.pi_prot_type;
-
-	if (flag != 0 && flag != 1 && flag != 2 && flag != 3) {
-		pr_err("Illegal value %d for pi_prot_type\n", flag);
-		return -EINVAL;
-	}
-	if (flag == 2) {
-		pr_err("DIF TYPE2 protection currently not supported\n");
-		return -ENOSYS;
-	}
-	if (dev->dev_attrib.hw_pi_prot_type) {
-		pr_warn("DIF protection enabled on underlying hardware,"
-			" ignoring\n");
-		return 0;
-	}
-	if (!dev->transport->init_prot || !dev->transport->free_prot) {
-		/* 0 is only allowed value for non-supporting backends */
-		if (flag == 0)
-			return 0;
-
-		pr_err("DIF protection not supported by backend: %s\n",
-		       dev->transport->name);
-		return -ENOSYS;
-	}
-	if (!(dev->dev_flags & DF_CONFIGURED)) {
-		pr_err("DIF protection requires device to be configured\n");
-		return -ENODEV;
-	}
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to change SE Device PROT type while"
-		       " export_count is %d\n", dev, dev->export_count);
-		return -EINVAL;
-	}
-
-	dev->dev_attrib.pi_prot_type = flag;
-
-	if (flag && !old_prot) {
-		rc = dev->transport->init_prot(dev);
-		if (rc) {
-			dev->dev_attrib.pi_prot_type = old_prot;
-			return rc;
-		}
-
-	} else if (!flag && old_prot) {
-		dev->transport->free_prot(dev);
-	}
-	pr_debug("dev[%p]: SE Device Protection Type: %d\n", dev, flag);
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_pi_prot_type);
-
-int se_dev_set_pi_prot_format(struct se_device *dev, int flag)
-{
-	int rc;
-
-	if (!flag)
-		return 0;
-
-	if (flag != 1) {
-		pr_err("Illegal value %d for pi_prot_format\n", flag);
-		return -EINVAL;
-	}
-	if (!dev->transport->format_prot) {
-		pr_err("DIF protection format not supported by backend %s\n",
-		       dev->transport->name);
-		return -ENOSYS;
-	}
-	if (!(dev->dev_flags & DF_CONFIGURED)) {
-		pr_err("DIF protection format requires device to be configured\n");
-		return -ENODEV;
-	}
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to format SE Device PROT type while"
-		       " export_count is %d\n", dev, dev->export_count);
-		return -EINVAL;
-	}
-
-	rc = dev->transport->format_prot(dev);
-	if (rc)
-		return rc;
-
-	pr_debug("dev[%p]: SE Device Protection Format complete\n", dev);
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_pi_prot_format);
-
-int se_dev_set_enforce_pr_isids(struct se_device *dev, int flag)
-{
-	if ((flag != 0) && (flag != 1)) {
-		pr_err("Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	dev->dev_attrib.enforce_pr_isids = flag;
-	pr_debug("dev[%p]: SE Device enforce_pr_isids bit: %s\n", dev,
-		(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)
-{
-	if ((flag != 0) && (flag != 1)) {
-		printk(KERN_ERR "Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to set force_pr_aptpl while"
-		       " export_count is %d\n", dev, dev->export_count);
-		return -EINVAL;
-	}
-
-	dev->dev_attrib.force_pr_aptpl = 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)
-{
-	if ((flag != 0) && (flag != 1)) {
-		printk(KERN_ERR "Illegal value %d\n", flag);
-		return -EINVAL;
-	}
-	dev->dev_attrib.is_nonrot = flag;
-	pr_debug("dev[%p]: SE Device is_nonrot bit: %d\n",
-	       dev, flag);
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_is_nonrot);
-
-int se_dev_set_emulate_rest_reord(struct se_device *dev, int flag)
-{
-	if (flag != 0) {
-		printk(KERN_ERR "dev[%p]: SE Device emulatation of restricted"
-			" reordering not implemented\n", dev);
-		return -ENOSYS;
-	}
-	dev->dev_attrib.emulate_rest_reord = 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.
- */
-int se_dev_set_queue_depth(struct se_device *dev, u32 queue_depth)
-{
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to change SE Device TCQ while"
-			" export_count is %d\n",
-			dev, dev->export_count);
-		return -EINVAL;
-	}
-	if (!queue_depth) {
-		pr_err("dev[%p]: Illegal ZERO value for queue"
-			"_depth\n", dev);
-		return -EINVAL;
-	}
-
-	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_optimal_sectors(struct se_device *dev, u32 optimal_sectors)
-{
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to change SE Device"
-			" optimal_sectors while export_count is %d\n",
-			dev, dev->export_count);
-		return -EINVAL;
-	}
-	if (optimal_sectors > dev->dev_attrib.hw_max_sectors) {
-		pr_err("dev[%p]: Passed optimal_sectors %u cannot be"
-			" greater than hw_max_sectors: %u\n", dev,
-			optimal_sectors, dev->dev_attrib.hw_max_sectors);
-		return -EINVAL;
-	}
-
-	dev->dev_attrib.optimal_sectors = optimal_sectors;
-	pr_debug("dev[%p]: SE Device optimal_sectors changed to %u\n",
-			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)
-{
-	if (dev->export_count) {
-		pr_err("dev[%p]: Unable to change SE Device block_size"
-			" while export_count is %d\n",
-			dev, dev->export_count);
-		return -EINVAL;
-	}
-
-	if ((block_size != 512) &&
-	    (block_size != 1024) &&
-	    (block_size != 2048) &&
-	    (block_size != 4096)) {
-		pr_err("dev[%p]: Illegal value for block_device: %u"
-			" for SE device, must be 512, 1024, 2048 or 4096\n",
-			dev, block_size);
-		return -EINVAL;
-	}
-
-	dev->dev_attrib.block_size = block_size;
-	pr_debug("dev[%p]: SE Device block_size changed to %u\n",
-			dev, block_size);
-
-	if (dev->dev_attrib.max_bytes_per_io)
-		dev->dev_attrib.hw_max_sectors =
-			dev->dev_attrib.max_bytes_per_io / block_size;
-
-	return 0;
-}
-EXPORT_SYMBOL(se_dev_set_block_size);
-
-struct se_lun *core_dev_add_lun(
+int core_dev_add_lun(
 	struct se_portal_group *tpg,
 	struct se_device *dev,
-	u32 unpacked_lun)
+	struct se_lun *lun)
 {
-	struct se_lun *lun;
 	int rc;
 
-	lun = core_tpg_alloc_lun(tpg, unpacked_lun);
-	if (IS_ERR(lun))
-		return lun;
-
 	rc = core_tpg_add_lun(tpg, lun,
 				TRANSPORT_LUNFLAGS_READ_WRITE, dev);
 	if (rc < 0)
-		return ERR_PTR(rc);
+		return rc;
 
-	pr_debug("%s_TPG[%u]_LUN[%u] - Activated %s Logical Unit from"
+	pr_debug("%s_TPG[%u]_LUN[%llu] - Activated %s Logical Unit from"
 		" CORE HBA: %u\n", tpg->se_tpg_tfo->get_fabric_name(),
 		tpg->se_tpg_tfo->tpg_get_tag(tpg), lun->unpacked_lun,
 		tpg->se_tpg_tfo->get_fabric_name(), dev->se_hba->hba_id);
@@ -1210,20 +566,19 @@ struct se_lun *core_dev_add_lun(
 	 */
 	if (tpg->se_tpg_tfo->tpg_check_demo_mode(tpg)) {
 		struct se_node_acl *acl;
-		spin_lock_irq(&tpg->acl_node_lock);
+
+		mutex_lock(&tpg->acl_node_mutex);
 		list_for_each_entry(acl, &tpg->acl_node_list, acl_list) {
 			if (acl->dynamic_node_acl &&
 			    (!tpg->se_tpg_tfo->tpg_check_demo_mode_login_only ||
 			     !tpg->se_tpg_tfo->tpg_check_demo_mode_login_only(tpg))) {
-				spin_unlock_irq(&tpg->acl_node_lock);
-				core_tpg_add_node_to_devs(acl, tpg);
-				spin_lock_irq(&tpg->acl_node_lock);
+				core_tpg_add_node_to_devs(acl, tpg, lun);
 			}
 		}
-		spin_unlock_irq(&tpg->acl_node_lock);
+		mutex_unlock(&tpg->acl_node_mutex);
 	}
 
-	return lun;
+	return 0;
 }
 
 /*      core_dev_del_lun():
@@ -1234,7 +589,7 @@ void core_dev_del_lun(
 	struct se_portal_group *tpg,
 	struct se_lun *lun)
 {
-	pr_debug("%s_TPG[%u]_LUN[%u] - Deactivating %s Logical Unit from"
+	pr_debug("%s_TPG[%u]_LUN[%llu] - Deactivating %s Logical Unit from"
 		" device object\n", tpg->se_tpg_tfo->get_fabric_name(),
 		tpg->se_tpg_tfo->tpg_get_tag(tpg), lun->unpacked_lun,
 		tpg->se_tpg_tfo->get_fabric_name());
@@ -1242,72 +597,10 @@ void core_dev_del_lun(
 	core_tpg_remove_lun(tpg, lun);
 }
 
-struct se_lun *core_get_lun_from_tpg(struct se_portal_group *tpg, u32 unpacked_lun)
-{
-	struct se_lun *lun;
-
-	spin_lock(&tpg->tpg_lun_lock);
-	if (unpacked_lun > (TRANSPORT_MAX_LUNS_PER_TPG-1)) {
-		pr_err("%s LUN: %u exceeds TRANSPORT_MAX_LUNS"
-			"_PER_TPG-1: %u for Target Portal Group: %hu\n",
-			tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
-			TRANSPORT_MAX_LUNS_PER_TPG-1,
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		spin_unlock(&tpg->tpg_lun_lock);
-		return NULL;
-	}
-	lun = tpg->tpg_lun_list[unpacked_lun];
-
-	if (lun->lun_status != TRANSPORT_LUN_STATUS_FREE) {
-		pr_err("%s Logical Unit Number: %u is not free on"
-			" Target Portal Group: %hu, ignoring request.\n",
-			tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		spin_unlock(&tpg->tpg_lun_lock);
-		return NULL;
-	}
-	spin_unlock(&tpg->tpg_lun_lock);
-
-	return lun;
-}
-
-/*      core_dev_get_lun():
- *
- *
- */
-static struct se_lun *core_dev_get_lun(struct se_portal_group *tpg, u32 unpacked_lun)
-{
-	struct se_lun *lun;
-
-	spin_lock(&tpg->tpg_lun_lock);
-	if (unpacked_lun > (TRANSPORT_MAX_LUNS_PER_TPG-1)) {
-		pr_err("%s LUN: %u exceeds TRANSPORT_MAX_LUNS_PER"
-			"_TPG-1: %u for Target Portal Group: %hu\n",
-			tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
-			TRANSPORT_MAX_LUNS_PER_TPG-1,
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		spin_unlock(&tpg->tpg_lun_lock);
-		return NULL;
-	}
-	lun = tpg->tpg_lun_list[unpacked_lun];
-
-	if (lun->lun_status != TRANSPORT_LUN_STATUS_ACTIVE) {
-		pr_err("%s Logical Unit Number: %u is not active on"
-			" Target Portal Group: %hu, ignoring request.\n",
-			tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		spin_unlock(&tpg->tpg_lun_lock);
-		return NULL;
-	}
-	spin_unlock(&tpg->tpg_lun_lock);
-
-	return lun;
-}
-
 struct se_lun_acl *core_dev_init_initiator_node_lun_acl(
 	struct se_portal_group *tpg,
 	struct se_node_acl *nacl,
-	u32 mapped_lun,
+	u64 mapped_lun,
 	int *ret)
 {
 	struct se_lun_acl *lacl;
@@ -1325,7 +618,6 @@ struct se_lun_acl *core_dev_init_initiator_node_lun_acl(
 		return NULL;
 	}
 
-	INIT_LIST_HEAD(&lacl->lacl_list);
 	lacl->mapped_lun = mapped_lun;
 	lacl->se_lun_nacl = nacl;
 	snprintf(lacl->initiatorname, TRANSPORT_IQN_LEN, "%s",
@@ -1337,22 +629,16 @@ struct se_lun_acl *core_dev_init_initiator_node_lun_acl(
 int core_dev_add_initiator_node_lun_acl(
 	struct se_portal_group *tpg,
 	struct se_lun_acl *lacl,
-	u32 unpacked_lun,
+	struct se_lun *lun,
 	u32 lun_access)
 {
-	struct se_lun *lun;
-	struct se_node_acl *nacl;
-
-	lun = core_dev_get_lun(tpg, unpacked_lun);
-	if (!lun) {
-		pr_err("%s Logical Unit Number: %u is not active on"
-			" Target Portal Group: %hu, ignoring request.\n",
-			tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		return -EINVAL;
-	}
+	struct se_node_acl *nacl = lacl->se_lun_nacl;
+	/*
+	 * rcu_dereference_raw protected by se_lun->lun_group symlink
+	 * reference to se_device->dev_group.
+	 */
+	struct se_device *dev = rcu_dereference_raw(lun->lun_se_dev);
 
-	nacl = lacl->se_lun_nacl;
 	if (!nacl)
 		return -EINVAL;
 
@@ -1366,52 +652,40 @@ int core_dev_add_initiator_node_lun_acl(
 			lun_access, nacl, tpg) < 0)
 		return -EINVAL;
 
-	spin_lock(&lun->lun_acl_lock);
-	list_add_tail(&lacl->lacl_list, &lun->lun_acl_list);
-	atomic_inc_mb(&lun->lun_acl_count);
-	spin_unlock(&lun->lun_acl_lock);
-
-	pr_debug("%s_TPG[%hu]_LUN[%u->%u] - Added %s ACL for "
+	pr_debug("%s_TPG[%hu]_LUN[%llu->%llu] - Added %s ACL for "
 		" InitiatorNode: %s\n", tpg->se_tpg_tfo->get_fabric_name(),
-		tpg->se_tpg_tfo->tpg_get_tag(tpg), unpacked_lun, lacl->mapped_lun,
+		tpg->se_tpg_tfo->tpg_get_tag(tpg), lun->unpacked_lun, lacl->mapped_lun,
 		(lun_access & TRANSPORT_LUNFLAGS_READ_WRITE) ? "RW" : "RO",
 		lacl->initiatorname);
 	/*
 	 * Check to see if there are any existing persistent reservation APTPL
 	 * pre-registrations that need to be enabled for this LUN ACL..
 	 */
-	core_scsi3_check_aptpl_registration(lun->lun_se_dev, tpg, lun, nacl,
+	core_scsi3_check_aptpl_registration(dev, tpg, lun, nacl,
 					    lacl->mapped_lun);
 	return 0;
 }
 
-/*      core_dev_del_initiator_node_lun_acl():
- *
- *
- */
 int core_dev_del_initiator_node_lun_acl(
-	struct se_portal_group *tpg,
 	struct se_lun *lun,
 	struct se_lun_acl *lacl)
 {
+	struct se_portal_group *tpg = lun->lun_tpg;
 	struct se_node_acl *nacl;
+	struct se_dev_entry *deve;
 
 	nacl = lacl->se_lun_nacl;
 	if (!nacl)
 		return -EINVAL;
 
-	spin_lock(&lun->lun_acl_lock);
-	list_del(&lacl->lacl_list);
-	atomic_dec_mb(&lun->lun_acl_count);
-	spin_unlock(&lun->lun_acl_lock);
-
-	core_disable_device_list_for_node(lun, NULL, lacl->mapped_lun,
-		TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg);
-
-	lacl->se_lun = NULL;
+	mutex_lock(&nacl->lun_entry_mutex);
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (deve)
+		core_disable_device_list_for_node(lun, deve, nacl, tpg);
+	mutex_unlock(&nacl->lun_entry_mutex);
 
-	pr_debug("%s_TPG[%hu]_LUN[%u] - Removed ACL for"
-		" InitiatorNode: %s Mapped LUN: %u\n",
+	pr_debug("%s_TPG[%hu]_LUN[%llu] - Removed ACL for"
+		" InitiatorNode: %s Mapped LUN: %llu\n",
 		tpg->se_tpg_tfo->get_fabric_name(),
 		tpg->se_tpg_tfo->tpg_get_tag(tpg), lun->unpacked_lun,
 		lacl->initiatorname, lacl->mapped_lun);
@@ -1424,7 +698,7 @@ void core_dev_free_initiator_node_lun_acl(
 	struct se_lun_acl *lacl)
 {
 	pr_debug("%s_TPG[%hu] - Freeing ACL for %s InitiatorNode: %s"
-		" Mapped LUN: %u\n", tpg->se_tpg_tfo->get_fabric_name(),
+		" Mapped LUN: %llu\n", tpg->se_tpg_tfo->get_fabric_name(),
 		tpg->se_tpg_tfo->tpg_get_tag(tpg),
 		tpg->se_tpg_tfo->get_fabric_name(),
 		lacl->initiatorname, lacl->mapped_lun);
@@ -1473,14 +747,15 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 	struct se_device *dev;
 	struct se_lun *xcopy_lun;
 
-	dev = hba->transport->alloc_device(hba, name);
+	dev = hba->backend->ops->alloc_device(hba, name);
 	if (!dev)
 		return NULL;
 
 	dev->dev_link_magic = SE_DEV_LINK_MAGIC;
 	dev->se_hba = hba;
-	dev->transport = hba->transport;
+	dev->transport = hba->backend->ops;
 	dev->prot_length = sizeof(struct se_dif_v1_tuple);
+	dev->hba_index = hba->hba_index;
 
 	INIT_LIST_HEAD(&dev->dev_list);
 	INIT_LIST_HEAD(&dev->dev_sep_list);
@@ -1513,9 +788,9 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 
 	dev->dev_attrib.da_dev = dev;
 	dev->dev_attrib.emulate_model_alias = DA_EMULATE_MODEL_ALIAS;
-	dev->dev_attrib.emulate_dpo = DA_EMULATE_DPO;
-	dev->dev_attrib.emulate_fua_write = DA_EMULATE_FUA_WRITE;
-	dev->dev_attrib.emulate_fua_read = DA_EMULATE_FUA_READ;
+	dev->dev_attrib.emulate_dpo = 1;
+	dev->dev_attrib.emulate_fua_write = 1;
+	dev->dev_attrib.emulate_fua_read = 1;
 	dev->dev_attrib.emulate_write_cache = DA_EMULATE_WRITE_CACHE;
 	dev->dev_attrib.emulate_ua_intlck_ctrl = DA_EMULATE_UA_INTLLCK_CTRL;
 	dev->dev_attrib.emulate_tas = DA_EMULATE_TAS;
@@ -1537,12 +812,12 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 	dev->dev_attrib.max_write_same_len = DA_MAX_WRITE_SAME_LEN;
 
 	xcopy_lun = &dev->xcopy_lun;
-	xcopy_lun->lun_se_dev = dev;
-	init_completion(&xcopy_lun->lun_shutdown_comp);
-	INIT_LIST_HEAD(&xcopy_lun->lun_acl_list);
-	spin_lock_init(&xcopy_lun->lun_acl_lock);
-	spin_lock_init(&xcopy_lun->lun_sep_lock);
+	rcu_assign_pointer(xcopy_lun->lun_se_dev, dev);
 	init_completion(&xcopy_lun->lun_ref_comp);
+	INIT_LIST_HEAD(&xcopy_lun->lun_deve_list);
+	INIT_LIST_HEAD(&xcopy_lun->lun_dev_link);
+	mutex_init(&xcopy_lun->lun_tg_pt_md_mutex);
+	xcopy_lun->lun_tpg = &xcopy_pt_tpg;
 
 	return dev;
 }
@@ -1679,7 +954,7 @@ int core_dev_setup_virtual_lun0(void)
 		goto out_free_hba;
 	}
 
-	hba->transport->set_configfs_dev_params(dev, buf, sizeof(buf));
+	hba->backend->ops->set_configfs_dev_params(dev, buf, sizeof(buf));
 
 	ret = target_configure_device(dev);
 	if (ret)
diff --git a/drivers/target/target_core_fabric_configfs.c b/drivers/target/target_core_fabric_configfs.c
index 1f7886bb16bf..48a36989c1a6 100644
--- a/drivers/target/target_core_fabric_configfs.c
+++ b/drivers/target/target_core_fabric_configfs.c
@@ -36,7 +36,6 @@
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 
 #include "target_core_internal.h"
@@ -46,27 +45,25 @@
 #define TF_CIT_SETUP(_name, _item_ops, _group_ops, _attrs)		\
 static void target_fabric_setup_##_name##_cit(struct target_fabric_configfs *tf) \
 {									\
-	struct target_fabric_configfs_template *tfc = &tf->tf_cit_tmpl;	\
-	struct config_item_type *cit = &tfc->tfc_##_name##_cit;		\
+	struct config_item_type *cit = &tf->tf_##_name##_cit;		\
 									\
 	cit->ct_item_ops = _item_ops;					\
 	cit->ct_group_ops = _group_ops;					\
 	cit->ct_attrs = _attrs;						\
-	cit->ct_owner = tf->tf_module;					\
+	cit->ct_owner = tf->tf_ops->module;				\
 	pr_debug("Setup generic %s\n", __stringify(_name));		\
 }
 
 #define TF_CIT_SETUP_DRV(_name, _item_ops, _group_ops)		\
 static void target_fabric_setup_##_name##_cit(struct target_fabric_configfs *tf) \
 {									\
-	struct target_fabric_configfs_template *tfc = &tf->tf_cit_tmpl;	\
-	struct config_item_type *cit = &tfc->tfc_##_name##_cit;		\
-	struct configfs_attribute **attrs = tf->tf_ops.tfc_##_name##_attrs; \
+	struct config_item_type *cit = &tf->tf_##_name##_cit;		\
+	struct configfs_attribute **attrs = tf->tf_ops->tfc_##_name##_attrs; \
 									\
 	cit->ct_item_ops = _item_ops;					\
 	cit->ct_group_ops = _group_ops;					\
 	cit->ct_attrs = attrs;						\
-	cit->ct_owner = tf->tf_module;					\
+	cit->ct_owner = tf->tf_ops->module;				\
 	pr_debug("Setup generic %s\n", __stringify(_name));		\
 }
 
@@ -83,7 +80,7 @@ static int target_fabric_mappedlun_link(
 			struct se_lun_acl, se_lun_group);
 	struct se_portal_group *se_tpg;
 	struct config_item *nacl_ci, *tpg_ci, *tpg_ci_s, *wwn_ci, *wwn_ci_s;
-	int ret = 0, lun_access;
+	int lun_access;
 
 	if (lun->lun_link_magic != SE_LUN_LINK_MAGIC) {
 		pr_err("Bad lun->lun_link_magic, not a valid lun_ci pointer:"
@@ -93,12 +90,11 @@ static int target_fabric_mappedlun_link(
 	/*
 	 * Ensure that the source port exists
 	 */
-	if (!lun->lun_sep || !lun->lun_sep->sep_tpg) {
-		pr_err("Source se_lun->lun_sep or lun->lun_sep->sep"
-				"_tpg does not exist\n");
+	if (!lun->lun_se_dev) {
+		pr_err("Source se_lun->lun_se_dev does not exist\n");
 		return -EINVAL;
 	}
-	se_tpg = lun->lun_sep->sep_tpg;
+	se_tpg = lun->lun_tpg;
 
 	nacl_ci = &lun_acl_ci->ci_parent->ci_group->cg_item;
 	tpg_ci = &nacl_ci->ci_group->cg_item;
@@ -125,49 +121,35 @@ static int target_fabric_mappedlun_link(
 	 * which be will write protected (READ-ONLY) when
 	 * tpg_1/attrib/demo_mode_write_protect=1
 	 */
-	spin_lock_irq(&lacl->se_lun_nacl->device_list_lock);
-	deve = lacl->se_lun_nacl->device_list[lacl->mapped_lun];
-	if (deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS)
+	rcu_read_lock();
+	deve = target_nacl_find_deve(lacl->se_lun_nacl, lacl->mapped_lun);
+	if (deve)
 		lun_access = deve->lun_flags;
 	else
 		lun_access =
 			(se_tpg->se_tpg_tfo->tpg_check_prod_mode_write_protect(
 				se_tpg)) ? TRANSPORT_LUNFLAGS_READ_ONLY :
 					   TRANSPORT_LUNFLAGS_READ_WRITE;
-	spin_unlock_irq(&lacl->se_lun_nacl->device_list_lock);
+	rcu_read_unlock();
 	/*
 	 * Determine the actual mapped LUN value user wants..
 	 *
 	 * This value is what the SCSI Initiator actually sees the
-	 * iscsi/$IQN/$TPGT/lun/lun_* as on their SCSI Initiator Ports.
+	 * $FABRIC/$WWPN/$TPGT/lun/lun_* as on their SCSI Initiator Ports.
 	 */
-	ret = core_dev_add_initiator_node_lun_acl(se_tpg, lacl,
-			lun->unpacked_lun, lun_access);
-
-	return (ret < 0) ? -EINVAL : 0;
+	return core_dev_add_initiator_node_lun_acl(se_tpg, lacl, lun, lun_access);
 }
 
 static int target_fabric_mappedlun_unlink(
 	struct config_item *lun_acl_ci,
 	struct config_item *lun_ci)
 {
-	struct se_lun *lun;
 	struct se_lun_acl *lacl = container_of(to_config_group(lun_acl_ci),
 			struct se_lun_acl, se_lun_group);
-	struct se_node_acl *nacl = lacl->se_lun_nacl;
-	struct se_dev_entry *deve = nacl->device_list[lacl->mapped_lun];
-	struct se_portal_group *se_tpg;
-	/*
-	 * Determine if the underlying MappedLUN has already been released..
-	 */
-	if (!deve->se_lun)
-		return 0;
-
-	lun = container_of(to_config_group(lun_ci), struct se_lun, lun_group);
-	se_tpg = lun->lun_sep->sep_tpg;
+	struct se_lun *lun = container_of(to_config_group(lun_ci),
+			struct se_lun, lun_group);
 
-	core_dev_del_initiator_node_lun_acl(se_tpg, lun, lacl);
-	return 0;
+	return core_dev_del_initiator_node_lun_acl(lun, lacl);
 }
 
 CONFIGFS_EATTR_STRUCT(target_fabric_mappedlun, se_lun_acl);
@@ -183,14 +165,15 @@ static ssize_t target_fabric_mappedlun_show_write_protect(
 {
 	struct se_node_acl *se_nacl = lacl->se_lun_nacl;
 	struct se_dev_entry *deve;
-	ssize_t len;
+	ssize_t len = 0;
 
-	spin_lock_irq(&se_nacl->device_list_lock);
-	deve = se_nacl->device_list[lacl->mapped_lun];
-	len = sprintf(page, "%d\n",
-			(deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY) ?
-			1 : 0);
-	spin_unlock_irq(&se_nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(se_nacl, lacl->mapped_lun);
+	if (deve) {
+		len = sprintf(page, "%d\n",
+			(deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY) ? 1 : 0);
+	}
+	rcu_read_unlock();
 
 	return len;
 }
@@ -218,7 +201,7 @@ static ssize_t target_fabric_mappedlun_store_write_protect(
 			lacl->se_lun_nacl);
 
 	pr_debug("%s_ConfigFS: Changed Initiator ACL: %s"
-		" Mapped LUN: %u Write Protect bit to %s\n",
+		" Mapped LUN: %llu Write Protect bit to %s\n",
 		se_tpg->se_tpg_tfo->get_fabric_name(),
 		lacl->initiatorname, lacl->mapped_lun, (op) ? "ON" : "OFF");
 
@@ -338,7 +321,7 @@ static struct config_group *target_fabric_make_mappedlun(
 	struct config_item *acl_ci;
 	struct config_group *lacl_cg = NULL, *ml_stat_grp = NULL;
 	char *buf;
-	unsigned long mapped_lun;
+	unsigned long long mapped_lun;
 	int ret = 0;
 
 	acl_ci = &group->cg_item;
@@ -366,21 +349,9 @@ static struct config_group *target_fabric_make_mappedlun(
 	 * Determine the Mapped LUN value.  This is what the SCSI Initiator
 	 * Port will actually see.
 	 */
-	ret = kstrtoul(buf + 4, 0, &mapped_lun);
+	ret = kstrtoull(buf + 4, 0, &mapped_lun);
 	if (ret)
 		goto out;
-	if (mapped_lun > UINT_MAX) {
-		ret = -EINVAL;
-		goto out;
-	}
-	if (mapped_lun > (TRANSPORT_MAX_LUNS_PER_TPG-1)) {
-		pr_err("Mapped LUN: %lu exceeds TRANSPORT_MAX_LUNS_PER_TPG"
-			"-1: %u for Target Portal Group: %u\n", mapped_lun,
-			TRANSPORT_MAX_LUNS_PER_TPG-1,
-			se_tpg->se_tpg_tfo->tpg_get_tag(se_tpg));
-		ret = -EINVAL;
-		goto out;
-	}
 
 	lacl = core_dev_init_initiator_node_lun_acl(se_tpg, se_nacl,
 			mapped_lun, &ret);
@@ -399,9 +370,9 @@ static struct config_group *target_fabric_make_mappedlun(
 	}
 
 	config_group_init_type_name(&lacl->se_lun_group, name,
-			&tf->tf_cit_tmpl.tfc_tpg_mappedlun_cit);
+			&tf->tf_tpg_mappedlun_cit);
 	config_group_init_type_name(&lacl->ml_stat_grps.stat_group,
-			"statistics", &tf->tf_cit_tmpl.tfc_tpg_mappedlun_stat_cit);
+			"statistics", &tf->tf_tpg_mappedlun_stat_cit);
 	lacl_cg->default_groups[0] = &lacl->ml_stat_grps.stat_group;
 	lacl_cg->default_groups[1] = NULL;
 
@@ -458,10 +429,11 @@ static void target_fabric_nacl_base_release(struct config_item *item)
 {
 	struct se_node_acl *se_nacl = container_of(to_config_group(item),
 			struct se_node_acl, acl_group);
-	struct se_portal_group *se_tpg = se_nacl->se_tpg;
-	struct target_fabric_configfs *tf = se_tpg->se_tpg_wwn->wwn_tf;
+	struct target_fabric_configfs *tf = se_nacl->se_tpg->se_tpg_wwn->wwn_tf;
 
-	tf->tf_ops.fabric_drop_nodeacl(se_nacl);
+	if (tf->tf_ops->fabric_cleanup_nodeacl)
+		tf->tf_ops->fabric_cleanup_nodeacl(se_nacl);
+	core_tpg_del_initiator_node_acl(se_nacl);
 }
 
 static struct configfs_item_operations target_fabric_nacl_base_item_ops = {
@@ -501,15 +473,18 @@ static struct config_group *target_fabric_make_nodeacl(
 	struct se_node_acl *se_nacl;
 	struct config_group *nacl_cg;
 
-	if (!tf->tf_ops.fabric_make_nodeacl) {
-		pr_err("tf->tf_ops.fabric_make_nodeacl is NULL\n");
-		return ERR_PTR(-ENOSYS);
-	}
-
-	se_nacl = tf->tf_ops.fabric_make_nodeacl(se_tpg, group, name);
+	se_nacl = core_tpg_add_initiator_node_acl(se_tpg, name);
 	if (IS_ERR(se_nacl))
 		return ERR_CAST(se_nacl);
 
+	if (tf->tf_ops->fabric_init_nodeacl) {
+		int ret = tf->tf_ops->fabric_init_nodeacl(se_nacl, name);
+		if (ret) {
+			core_tpg_del_initiator_node_acl(se_nacl);
+			return ERR_PTR(ret);
+		}
+	}
+
 	nacl_cg = &se_nacl->acl_group;
 	nacl_cg->default_groups = se_nacl->acl_default_groups;
 	nacl_cg->default_groups[0] = &se_nacl->acl_attrib_group;
@@ -519,16 +494,15 @@ static struct config_group *target_fabric_make_nodeacl(
 	nacl_cg->default_groups[4] = NULL;
 
 	config_group_init_type_name(&se_nacl->acl_group, name,
-			&tf->tf_cit_tmpl.tfc_tpg_nacl_base_cit);
+			&tf->tf_tpg_nacl_base_cit);
 	config_group_init_type_name(&se_nacl->acl_attrib_group, "attrib",
-			&tf->tf_cit_tmpl.tfc_tpg_nacl_attrib_cit);
+			&tf->tf_tpg_nacl_attrib_cit);
 	config_group_init_type_name(&se_nacl->acl_auth_group, "auth",
-			&tf->tf_cit_tmpl.tfc_tpg_nacl_auth_cit);
+			&tf->tf_tpg_nacl_auth_cit);
 	config_group_init_type_name(&se_nacl->acl_param_group, "param",
-			&tf->tf_cit_tmpl.tfc_tpg_nacl_param_cit);
+			&tf->tf_tpg_nacl_param_cit);
 	config_group_init_type_name(&se_nacl->acl_fabric_stat_group,
-			"fabric_statistics",
-			&tf->tf_cit_tmpl.tfc_tpg_nacl_stat_cit);
+			"fabric_statistics", &tf->tf_tpg_nacl_stat_cit);
 
 	return &se_nacl->acl_group;
 }
@@ -575,7 +549,7 @@ static void target_fabric_np_base_release(struct config_item *item)
 	struct se_portal_group *se_tpg = se_tpg_np->tpg_np_parent;
 	struct target_fabric_configfs *tf = se_tpg->se_tpg_wwn->wwn_tf;
 
-	tf->tf_ops.fabric_drop_np(se_tpg_np);
+	tf->tf_ops->fabric_drop_np(se_tpg_np);
 }
 
 static struct configfs_item_operations target_fabric_np_base_item_ops = {
@@ -599,18 +573,18 @@ static struct config_group *target_fabric_make_np(
 	struct target_fabric_configfs *tf = se_tpg->se_tpg_wwn->wwn_tf;
 	struct se_tpg_np *se_tpg_np;
 
-	if (!tf->tf_ops.fabric_make_np) {
+	if (!tf->tf_ops->fabric_make_np) {
 		pr_err("tf->tf_ops.fabric_make_np is NULL\n");
 		return ERR_PTR(-ENOSYS);
 	}
 
-	se_tpg_np = tf->tf_ops.fabric_make_np(se_tpg, group, name);
+	se_tpg_np = tf->tf_ops->fabric_make_np(se_tpg, group, name);
 	if (!se_tpg_np || IS_ERR(se_tpg_np))
 		return ERR_PTR(-EINVAL);
 
 	se_tpg_np->tpg_np_parent = se_tpg;
 	config_group_init_type_name(&se_tpg_np->tpg_np_group, name,
-			&tf->tf_cit_tmpl.tfc_tpg_np_base_cit);
+			&tf->tf_tpg_np_base_cit);
 
 	return &se_tpg_np->tpg_np_group;
 }
@@ -654,10 +628,10 @@ static ssize_t target_fabric_port_show_attr_alua_tg_pt_gp(
 	struct se_lun *lun,
 	char *page)
 {
-	if (!lun || !lun->lun_sep)
+	if (!lun || !lun->lun_se_dev)
 		return -ENODEV;
 
-	return core_alua_show_tg_pt_gp_info(lun->lun_sep, page);
+	return core_alua_show_tg_pt_gp_info(lun, page);
 }
 
 static ssize_t target_fabric_port_store_attr_alua_tg_pt_gp(
@@ -665,10 +639,10 @@ static ssize_t target_fabric_port_store_attr_alua_tg_pt_gp(
 	const char *page,
 	size_t count)
 {
-	if (!lun || !lun->lun_sep)
+	if (!lun || !lun->lun_se_dev)
 		return -ENODEV;
 
-	return core_alua_store_tg_pt_gp_info(lun->lun_sep, page, count);
+	return core_alua_store_tg_pt_gp_info(lun, page, count);
 }
 
 TCM_PORT_ATTR(alua_tg_pt_gp, S_IRUGO | S_IWUSR);
@@ -680,7 +654,7 @@ static ssize_t target_fabric_port_show_attr_alua_tg_pt_offline(
 	struct se_lun *lun,
 	char *page)
 {
-	if (!lun || !lun->lun_sep)
+	if (!lun || !lun->lun_se_dev)
 		return -ENODEV;
 
 	return core_alua_show_offline_bit(lun, page);
@@ -691,7 +665,7 @@ static ssize_t target_fabric_port_store_attr_alua_tg_pt_offline(
 	const char *page,
 	size_t count)
 {
-	if (!lun || !lun->lun_sep)
+	if (!lun || !lun->lun_se_dev)
 		return -ENODEV;
 
 	return core_alua_store_offline_bit(lun, page, count);
@@ -706,7 +680,7 @@ static ssize_t target_fabric_port_show_attr_alua_tg_pt_status(
 	struct se_lun *lun,
 	char *page)
 {
-	if (!lun || !lun->lun_sep)
+	if (!lun || !lun->lun_se_dev)
 		return -ENODEV;
 
 	return core_alua_show_secondary_status(lun, page);
@@ -717,7 +691,7 @@ static ssize_t target_fabric_port_store_attr_alua_tg_pt_status(
 	const char *page,
 	size_t count)
 {
-	if (!lun || !lun->lun_sep)
+	if (!lun || !lun->lun_se_dev)
 		return -ENODEV;
 
 	return core_alua_store_secondary_status(lun, page, count);
@@ -732,7 +706,7 @@ static ssize_t target_fabric_port_show_attr_alua_tg_pt_write_md(
 	struct se_lun *lun,
 	char *page)
 {
-	if (!lun || !lun->lun_sep)
+	if (!lun || !lun->lun_se_dev)
 		return -ENODEV;
 
 	return core_alua_show_secondary_write_metadata(lun, page);
@@ -743,7 +717,7 @@ static ssize_t target_fabric_port_store_attr_alua_tg_pt_write_md(
 	const char *page,
 	size_t count)
 {
-	if (!lun || !lun->lun_sep)
+	if (!lun || !lun->lun_se_dev)
 		return -ENODEV;
 
 	return core_alua_store_secondary_write_metadata(lun, page, count);
@@ -769,7 +743,6 @@ static int target_fabric_port_link(
 	struct config_item *tpg_ci;
 	struct se_lun *lun = container_of(to_config_group(lun_ci),
 				struct se_lun, lun_group);
-	struct se_lun *lun_p;
 	struct se_portal_group *se_tpg;
 	struct se_device *dev =
 		container_of(to_config_group(se_dev_ci), struct se_device, dev_group);
@@ -797,20 +770,19 @@ static int target_fabric_port_link(
 		return -EEXIST;
 	}
 
-	lun_p = core_dev_add_lun(se_tpg, dev, lun->unpacked_lun);
-	if (IS_ERR(lun_p)) {
-		pr_err("core_dev_add_lun() failed\n");
-		ret = PTR_ERR(lun_p);
+	ret = core_dev_add_lun(se_tpg, dev, lun);
+	if (ret) {
+		pr_err("core_dev_add_lun() failed: %d\n", ret);
 		goto out;
 	}
 
-	if (tf->tf_ops.fabric_post_link) {
+	if (tf->tf_ops->fabric_post_link) {
 		/*
 		 * Call the optional fabric_post_link() to allow a
 		 * fabric module to setup any additional state once
 		 * core_dev_add_lun() has been called..
 		 */
-		tf->tf_ops.fabric_post_link(se_tpg, lun);
+		tf->tf_ops->fabric_post_link(se_tpg, lun);
 	}
 
 	return 0;
@@ -824,25 +796,34 @@ static int target_fabric_port_unlink(
 {
 	struct se_lun *lun = container_of(to_config_group(lun_ci),
 				struct se_lun, lun_group);
-	struct se_portal_group *se_tpg = lun->lun_sep->sep_tpg;
+	struct se_portal_group *se_tpg = lun->lun_tpg;
 	struct target_fabric_configfs *tf = se_tpg->se_tpg_wwn->wwn_tf;
 
-	if (tf->tf_ops.fabric_pre_unlink) {
+	if (tf->tf_ops->fabric_pre_unlink) {
 		/*
 		 * Call the optional fabric_pre_unlink() to allow a
 		 * fabric module to release any additional stat before
 		 * core_dev_del_lun() is called.
 		*/
-		tf->tf_ops.fabric_pre_unlink(se_tpg, lun);
+		tf->tf_ops->fabric_pre_unlink(se_tpg, lun);
 	}
 
 	core_dev_del_lun(se_tpg, lun);
 	return 0;
 }
 
+static void target_fabric_port_release(struct config_item *item)
+{
+	struct se_lun *lun = container_of(to_config_group(item),
+					  struct se_lun, lun_group);
+
+	kfree_rcu(lun, rcu_head);
+}
+
 static struct configfs_item_operations target_fabric_port_item_ops = {
 	.show_attribute		= target_fabric_port_attr_show,
 	.store_attribute	= target_fabric_port_attr_store,
+	.release		= target_fabric_port_release,
 	.allow_link		= target_fabric_port_link,
 	.drop_link		= target_fabric_port_unlink,
 };
@@ -887,7 +868,7 @@ static struct config_group *target_fabric_make_lun(
 			struct se_portal_group, tpg_lun_group);
 	struct target_fabric_configfs *tf = se_tpg->se_tpg_wwn->wwn_tf;
 	struct config_group *lun_cg = NULL, *port_stat_grp = NULL;
-	unsigned long unpacked_lun;
+	unsigned long long unpacked_lun;
 	int errno;
 
 	if (strstr(name, "lun_") != name) {
@@ -895,28 +876,27 @@ static struct config_group *target_fabric_make_lun(
 				" \"lun_$LUN_NUMBER\"\n");
 		return ERR_PTR(-EINVAL);
 	}
-	errno = kstrtoul(name + 4, 0, &unpacked_lun);
+	errno = kstrtoull(name + 4, 0, &unpacked_lun);
 	if (errno)
 		return ERR_PTR(errno);
-	if (unpacked_lun > UINT_MAX)
-		return ERR_PTR(-EINVAL);
 
-	lun = core_get_lun_from_tpg(se_tpg, unpacked_lun);
-	if (!lun)
-		return ERR_PTR(-EINVAL);
+	lun = core_tpg_alloc_lun(se_tpg, unpacked_lun);
+	if (IS_ERR(lun))
+		return ERR_CAST(lun);
 
 	lun_cg = &lun->lun_group;
 	lun_cg->default_groups = kmalloc(sizeof(struct config_group *) * 2,
 				GFP_KERNEL);
 	if (!lun_cg->default_groups) {
 		pr_err("Unable to allocate lun_cg->default_groups\n");
+		kfree(lun);
 		return ERR_PTR(-ENOMEM);
 	}
 
 	config_group_init_type_name(&lun->lun_group, name,
-			&tf->tf_cit_tmpl.tfc_tpg_port_cit);
+			&tf->tf_tpg_port_cit);
 	config_group_init_type_name(&lun->port_stat_grps.stat_group,
-			"statistics", &tf->tf_cit_tmpl.tfc_tpg_port_stat_cit);
+			"statistics", &tf->tf_tpg_port_stat_cit);
 	lun_cg->default_groups[0] = &lun->port_stat_grps.stat_group;
 	lun_cg->default_groups[1] = NULL;
 
@@ -926,6 +906,7 @@ static struct config_group *target_fabric_make_lun(
 	if (!port_stat_grp->default_groups) {
 		pr_err("Unable to allocate port_stat_grp->default_groups\n");
 		kfree(lun_cg->default_groups);
+		kfree(lun);
 		return ERR_PTR(-ENOMEM);
 	}
 	target_stat_setup_port_default_groups(lun);
@@ -1023,7 +1004,7 @@ static void target_fabric_tpg_release(struct config_item *item)
 	struct se_wwn *wwn = se_tpg->se_tpg_wwn;
 	struct target_fabric_configfs *tf = wwn->wwn_tf;
 
-	tf->tf_ops.fabric_drop_tpg(se_tpg);
+	tf->tf_ops->fabric_drop_tpg(se_tpg);
 }
 
 static struct configfs_item_operations target_fabric_tpg_base_item_ops = {
@@ -1046,12 +1027,12 @@ static struct config_group *target_fabric_make_tpg(
 	struct target_fabric_configfs *tf = wwn->wwn_tf;
 	struct se_portal_group *se_tpg;
 
-	if (!tf->tf_ops.fabric_make_tpg) {
-		pr_err("tf->tf_ops.fabric_make_tpg is NULL\n");
+	if (!tf->tf_ops->fabric_make_tpg) {
+		pr_err("tf->tf_ops->fabric_make_tpg is NULL\n");
 		return ERR_PTR(-ENOSYS);
 	}
 
-	se_tpg = tf->tf_ops.fabric_make_tpg(wwn, group, name);
+	se_tpg = tf->tf_ops->fabric_make_tpg(wwn, group, name);
 	if (!se_tpg || IS_ERR(se_tpg))
 		return ERR_PTR(-EINVAL);
 	/*
@@ -1067,19 +1048,19 @@ static struct config_group *target_fabric_make_tpg(
 	se_tpg->tpg_group.default_groups[6] = NULL;
 
 	config_group_init_type_name(&se_tpg->tpg_group, name,
-			&tf->tf_cit_tmpl.tfc_tpg_base_cit);
+			&tf->tf_tpg_base_cit);
 	config_group_init_type_name(&se_tpg->tpg_lun_group, "lun",
-			&tf->tf_cit_tmpl.tfc_tpg_lun_cit);
+			&tf->tf_tpg_lun_cit);
 	config_group_init_type_name(&se_tpg->tpg_np_group, "np",
-			&tf->tf_cit_tmpl.tfc_tpg_np_cit);
+			&tf->tf_tpg_np_cit);
 	config_group_init_type_name(&se_tpg->tpg_acl_group, "acls",
-			&tf->tf_cit_tmpl.tfc_tpg_nacl_cit);
+			&tf->tf_tpg_nacl_cit);
 	config_group_init_type_name(&se_tpg->tpg_attrib_group, "attrib",
-			&tf->tf_cit_tmpl.tfc_tpg_attrib_cit);
+			&tf->tf_tpg_attrib_cit);
 	config_group_init_type_name(&se_tpg->tpg_auth_group, "auth",
-			&tf->tf_cit_tmpl.tfc_tpg_auth_cit);
+			&tf->tf_tpg_auth_cit);
 	config_group_init_type_name(&se_tpg->tpg_param_group, "param",
-			&tf->tf_cit_tmpl.tfc_tpg_param_cit);
+			&tf->tf_tpg_param_cit);
 
 	return &se_tpg->tpg_group;
 }
@@ -1112,7 +1093,7 @@ static void target_fabric_release_wwn(struct config_item *item)
 				struct se_wwn, wwn_group);
 	struct target_fabric_configfs *tf = wwn->wwn_tf;
 
-	tf->tf_ops.fabric_drop_wwn(wwn);
+	tf->tf_ops->fabric_drop_wwn(wwn);
 }
 
 static struct configfs_item_operations target_fabric_tpg_item_ops = {
@@ -1148,12 +1129,12 @@ static struct config_group *target_fabric_make_wwn(
 				struct target_fabric_configfs, tf_group);
 	struct se_wwn *wwn;
 
-	if (!tf->tf_ops.fabric_make_wwn) {
+	if (!tf->tf_ops->fabric_make_wwn) {
 		pr_err("tf->tf_ops.fabric_make_wwn is NULL\n");
 		return ERR_PTR(-ENOSYS);
 	}
 
-	wwn = tf->tf_ops.fabric_make_wwn(tf, group, name);
+	wwn = tf->tf_ops->fabric_make_wwn(tf, group, name);
 	if (!wwn || IS_ERR(wwn))
 		return ERR_PTR(-EINVAL);
 
@@ -1165,10 +1146,9 @@ static struct config_group *target_fabric_make_wwn(
 	wwn->wwn_group.default_groups[0] = &wwn->fabric_stat_group;
 	wwn->wwn_group.default_groups[1] = NULL;
 
-	config_group_init_type_name(&wwn->wwn_group, name,
-			&tf->tf_cit_tmpl.tfc_tpg_cit);
+	config_group_init_type_name(&wwn->wwn_group, name, &tf->tf_tpg_cit);
 	config_group_init_type_name(&wwn->fabric_stat_group, "fabric_statistics",
-			&tf->tf_cit_tmpl.tfc_wwn_fabric_stats_cit);
+			&tf->tf_wwn_fabric_stats_cit);
 
 	return &wwn->wwn_group;
 }
diff --git a/drivers/target/target_core_fabric_lib.c b/drivers/target/target_core_fabric_lib.c
index 35bfe77160d8..cb6497ce4b61 100644
--- a/drivers/target/target_core_fabric_lib.c
+++ b/drivers/target/target_core_fabric_lib.c
@@ -24,134 +24,45 @@
  *
  ******************************************************************************/
 
+/*
+ * See SPC4, section 7.5 "Protocol specific parameters" for details
+ * on the formats implemented in this file.
+ */
+
 #include <linux/kernel.h>
 #include <linux/string.h>
 #include <linux/ctype.h>
 #include <linux/spinlock.h>
 #include <linux/export.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
+
+#include <scsi/scsi_proto.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 
 #include "target_core_internal.h"
 #include "target_core_pr.h"
 
-/*
- * Handlers for Serial Attached SCSI (SAS)
- */
-u8 sas_get_fabric_proto_ident(struct se_portal_group *se_tpg)
-{
-	/*
-	 * Return a SAS Serial SCSI Protocol identifier for loopback operations
-	 * This is defined in  section 7.5.1 Table 362 in spc4r17
-	 */
-	return 0x6;
-}
-EXPORT_SYMBOL(sas_get_fabric_proto_ident);
 
-u32 sas_get_pr_transport_id(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
+static int sas_get_pr_transport_id(
+	struct se_node_acl *nacl,
 	int *format_code,
 	unsigned char *buf)
 {
-	unsigned char *ptr;
 	int ret;
 
-	/*
-	 * Set PROTOCOL IDENTIFIER to 6h for SAS
-	 */
-	buf[0] = 0x06;
-	/*
-	 * From spc4r17, 7.5.4.7 TransportID for initiator ports using SCSI
-	 * over SAS Serial SCSI Protocol
-	 */
-	ptr = &se_nacl->initiatorname[4]; /* Skip over 'naa. prefix */
-
-	ret = hex2bin(&buf[4], ptr, 8);
-	if (ret < 0)
-		pr_debug("sas transport_id: invalid hex string\n");
+	/* Skip over 'naa. prefix */
+	ret = hex2bin(&buf[4], &nacl->initiatorname[4], 8);
+	if (ret) {
+		pr_debug("%s: invalid hex string\n", __func__);
+		return ret;
+	}
 
-	/*
-	 * The SAS Transport ID is a hardcoded 24-byte length
-	 */
 	return 24;
 }
-EXPORT_SYMBOL(sas_get_pr_transport_id);
 
-u32 sas_get_pr_transport_id_len(
-	struct se_portal_group *se_tpg,
+static int fc_get_pr_transport_id(
 	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code)
-{
-	*format_code = 0;
-	/*
-	 * From spc4r17, 7.5.4.7 TransportID for initiator ports using SCSI
-	 * over SAS Serial SCSI Protocol
-	 *
-	 * The SAS Transport ID is a hardcoded 24-byte length
-	 */
-	return 24;
-}
-EXPORT_SYMBOL(sas_get_pr_transport_id_len);
-
-/*
- * Used for handling SCSI fabric dependent TransportIDs in SPC-3 and above
- * Persistent Reservation SPEC_I_PT=1 and PROUT REGISTER_AND_MOVE operations.
- */
-char *sas_parse_pr_out_transport_id(
-	struct se_portal_group *se_tpg,
-	const char *buf,
-	u32 *out_tid_len,
-	char **port_nexus_ptr)
-{
-	/*
-	 * Assume the FORMAT CODE 00b from spc4r17, 7.5.4.7 TransportID
-	 * for initiator ports using SCSI over SAS Serial SCSI Protocol
-	 *
-	 * The TransportID for a SAS Initiator Port is of fixed size of
-	 * 24 bytes, and SAS does not contain a I_T nexus identifier,
-	 * so we return the **port_nexus_ptr set to NULL.
-	 */
-	*port_nexus_ptr = NULL;
-	*out_tid_len = 24;
-
-	return (char *)&buf[4];
-}
-EXPORT_SYMBOL(sas_parse_pr_out_transport_id);
-
-/*
- * Handlers for Fibre Channel Protocol (FCP)
- */
-u8 fc_get_fabric_proto_ident(struct se_portal_group *se_tpg)
-{
-	return 0x0;	/* 0 = fcp-2 per SPC4 section 7.5.1 */
-}
-EXPORT_SYMBOL(fc_get_fabric_proto_ident);
-
-u32 fc_get_pr_transport_id_len(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code)
-{
-	*format_code = 0;
-	/*
-	 * The FC Transport ID is a hardcoded 24-byte length
-	 */
-	return 24;
-}
-EXPORT_SYMBOL(fc_get_pr_transport_id_len);
-
-u32 fc_get_pr_transport_id(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
 	int *format_code,
 	unsigned char *buf)
 {
@@ -160,24 +71,20 @@ u32 fc_get_pr_transport_id(
 	u32 off = 8;
 
 	/*
-	 * PROTOCOL IDENTIFIER is 0h for FCP-2
-	 *
-	 * From spc4r17, 7.5.4.2 TransportID for initiator ports using
-	 * SCSI over Fibre Channel
-	 *
 	 * We convert the ASCII formatted N Port name into a binary
 	 * encoded TransportID.
 	 */
 	ptr = &se_nacl->initiatorname[0];
-
 	for (i = 0; i < 24; ) {
 		if (!strncmp(&ptr[i], ":", 1)) {
 			i++;
 			continue;
 		}
 		ret = hex2bin(&buf[off++], &ptr[i], 1);
-		if (ret < 0)
-			pr_debug("fc transport_id: invalid hex string\n");
+		if (ret < 0) {
+			pr_debug("%s: invalid hex string\n", __func__);
+			return ret;
+		}
 		i += 2;
 	}
 	/*
@@ -185,42 +92,52 @@ u32 fc_get_pr_transport_id(
 	 */
 	return 24;
 }
-EXPORT_SYMBOL(fc_get_pr_transport_id);
 
-char *fc_parse_pr_out_transport_id(
-	struct se_portal_group *se_tpg,
-	const char *buf,
-	u32 *out_tid_len,
-	char **port_nexus_ptr)
+static int sbp_get_pr_transport_id(
+	struct se_node_acl *nacl,
+	int *format_code,
+	unsigned char *buf)
 {
-	/*
-	 * The TransportID for a FC N Port is of fixed size of
-	 * 24 bytes, and FC does not contain a I_T nexus identifier,
-	 * so we return the **port_nexus_ptr set to NULL.
-	 */
-	*port_nexus_ptr = NULL;
-	*out_tid_len = 24;
+	int ret;
 
-	 return (char *)&buf[8];
-}
-EXPORT_SYMBOL(fc_parse_pr_out_transport_id);
+	ret = hex2bin(&buf[8], nacl->initiatorname, 8);
+	if (ret) {
+		pr_debug("%s: invalid hex string\n", __func__);
+		return ret;
+	}
 
-/*
- * Handlers for Internet Small Computer Systems Interface (iSCSI)
- */
+	return 24;
+}
 
-u8 iscsi_get_fabric_proto_ident(struct se_portal_group *se_tpg)
+static int srp_get_pr_transport_id(
+	struct se_node_acl *nacl,
+	int *format_code,
+	unsigned char *buf)
 {
-	/*
-	 * This value is defined for "Internet SCSI (iSCSI)"
-	 * in spc4r17 section 7.5.1 Table 362
-	 */
-	return 0x5;
+	const char *p;
+	unsigned len, count, leading_zero_bytes;
+	int rc;
+
+	p = nacl->initiatorname;
+	if (strncasecmp(p, "0x", 2) == 0)
+		p += 2;
+	len = strlen(p);
+	if (len % 2)
+		return -EINVAL;
+
+	count = min(len / 2, 16U);
+	leading_zero_bytes = 16 - count;
+	memset(buf + 8, 0, leading_zero_bytes);
+	rc = hex2bin(buf + 8 + leading_zero_bytes, p, count);
+	if (rc < 0) {
+		pr_debug("hex2bin failed for %s: %d\n", __func__, rc);
+		return rc;
+	}
+
+	return 24;
 }
-EXPORT_SYMBOL(iscsi_get_fabric_proto_ident);
 
-u32 iscsi_get_pr_transport_id(
-	struct se_portal_group *se_tpg,
+static int iscsi_get_pr_transport_id(
 	struct se_node_acl *se_nacl,
 	struct t10_pr_registration *pr_reg,
 	int *format_code,
@@ -231,10 +148,6 @@ u32 iscsi_get_pr_transport_id(
 
 	spin_lock_irq(&se_nacl->nacl_sess_lock);
 	/*
-	 * Set PROTOCOL IDENTIFIER to 5h for iSCSI
-	*/
-	buf[0] = 0x05;
-	/*
 	 * From spc4r17 Section 7.5.4.6: TransportID for initiator
 	 * ports using SCSI over iSCSI.
 	 *
@@ -313,10 +226,8 @@ u32 iscsi_get_pr_transport_id(
 
 	return len;
 }
-EXPORT_SYMBOL(iscsi_get_pr_transport_id);
 
-u32 iscsi_get_pr_transport_id_len(
-	struct se_portal_group *se_tpg,
+static int iscsi_get_pr_transport_id_len(
 	struct se_node_acl *se_nacl,
 	struct t10_pr_registration *pr_reg,
 	int *format_code)
@@ -359,9 +270,8 @@ u32 iscsi_get_pr_transport_id_len(
 
 	return len;
 }
-EXPORT_SYMBOL(iscsi_get_pr_transport_id_len);
 
-char *iscsi_parse_pr_out_transport_id(
+static char *iscsi_parse_pr_out_transport_id(
 	struct se_portal_group *se_tpg,
 	const char *buf,
 	u32 *out_tid_len,
@@ -448,4 +358,79 @@ char *iscsi_parse_pr_out_transport_id(
 
 	return (char *)&buf[4];
 }
-EXPORT_SYMBOL(iscsi_parse_pr_out_transport_id);
+
+int target_get_pr_transport_id_len(struct se_node_acl *nacl,
+		struct t10_pr_registration *pr_reg, int *format_code)
+{
+	switch (nacl->se_tpg->proto_id) {
+	case SCSI_PROTOCOL_FCP:
+	case SCSI_PROTOCOL_SBP:
+	case SCSI_PROTOCOL_SRP:
+	case SCSI_PROTOCOL_SAS:
+		break;
+	case SCSI_PROTOCOL_ISCSI:
+		return iscsi_get_pr_transport_id_len(nacl, pr_reg, format_code);
+	default:
+		pr_err("Unknown proto_id: 0x%02x\n", nacl->se_tpg->proto_id);
+		return -EINVAL;
+	}
+
+	/*
+	 * Most transports use a fixed length 24 byte identifier.
+	 */
+	*format_code = 0;
+	return 24;
+}
+
+int target_get_pr_transport_id(struct se_node_acl *nacl,
+		struct t10_pr_registration *pr_reg, int *format_code,
+		unsigned char *buf)
+{
+	switch (nacl->se_tpg->proto_id) {
+	case SCSI_PROTOCOL_SAS:
+		return sas_get_pr_transport_id(nacl, format_code, buf);
+	case SCSI_PROTOCOL_SBP:
+		return sbp_get_pr_transport_id(nacl, format_code, buf);
+	case SCSI_PROTOCOL_SRP:
+		return srp_get_pr_transport_id(nacl, format_code, buf);
+	case SCSI_PROTOCOL_FCP:
+		return fc_get_pr_transport_id(nacl, format_code, buf);
+	case SCSI_PROTOCOL_ISCSI:
+		return iscsi_get_pr_transport_id(nacl, pr_reg, format_code,
+				buf);
+	default:
+		pr_err("Unknown proto_id: 0x%02x\n", nacl->se_tpg->proto_id);
+		return -EINVAL;
+	}
+}
+
+const char *target_parse_pr_out_transport_id(struct se_portal_group *tpg,
+		const char *buf, u32 *out_tid_len, char **port_nexus_ptr)
+{
+	u32 offset;
+
+	switch (tpg->proto_id) {
+	case SCSI_PROTOCOL_SAS:
+		/*
+		 * Assume the FORMAT CODE 00b from spc4r17, 7.5.4.7 TransportID
+		 * for initiator ports using SCSI over SAS Serial SCSI Protocol.
+		 */
+		offset = 4;
+		break;
+	case SCSI_PROTOCOL_SBP:
+	case SCSI_PROTOCOL_SRP:
+	case SCSI_PROTOCOL_FCP:
+		offset = 8;
+		break;
+	case SCSI_PROTOCOL_ISCSI:
+		return iscsi_parse_pr_out_transport_id(tpg, buf, out_tid_len,
+					port_nexus_ptr);
+	default:
+		pr_err("Unknown proto_id: 0x%02x\n", tpg->proto_id);
+		return NULL;
+	}
+
+	*port_nexus_ptr = NULL;
+	*out_tid_len = 24;
+	return buf + offset;
+}
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 3f27bfd816d8..e3195700211a 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -30,14 +30,13 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/module.h>
+#include <linux/vmalloc.h>
 #include <linux/falloc.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
+#include <scsi/scsi_proto.h>
 #include <asm/unaligned.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
-#include <target/target_core_backend_configfs.h>
 
 #include "target_core_file.h"
 
@@ -46,10 +45,6 @@ static inline struct fd_dev *FD_DEV(struct se_device *dev)
 	return container_of(dev, struct fd_dev, dev);
 }
 
-/*	fd_attach_hba(): (Part of se_subsystem_api_t template)
- *
- *
- */
 static int fd_attach_hba(struct se_hba *hba, u32 host_id)
 {
 	struct fd_host *fd_host;
@@ -66,7 +61,7 @@ static int fd_attach_hba(struct se_hba *hba, u32 host_id)
 
 	pr_debug("CORE_HBA[%d] - TCM FILEIO HBA Driver %s on Generic"
 		" Target Core Stack %s\n", hba->hba_id, FD_VERSION,
-		TARGET_CORE_MOD_VERSION);
+		TARGET_CORE_VERSION);
 	pr_debug("CORE_HBA[%d] - Attached FILEIO HBA: %u to Generic\n",
 		hba->hba_id, fd_host->fd_host_id);
 
@@ -246,87 +241,34 @@ fail:
 	return ret;
 }
 
-static void fd_free_device(struct se_device *dev)
+static void fd_dev_call_rcu(struct rcu_head *p)
 {
+	struct se_device *dev = container_of(p, struct se_device, rcu_head);
 	struct fd_dev *fd_dev = FD_DEV(dev);
 
-	if (fd_dev->fd_file) {
-		filp_close(fd_dev->fd_file, NULL);
-		fd_dev->fd_file = NULL;
-	}
-
 	kfree(fd_dev);
 }
 
-static int fd_do_prot_rw(struct se_cmd *cmd, struct fd_prot *fd_prot,
-			 int is_write)
+static void fd_free_device(struct se_device *dev)
 {
-	struct se_device *se_dev = cmd->se_dev;
-	struct fd_dev *dev = FD_DEV(se_dev);
-	struct file *prot_fd = dev->fd_prot_file;
-	loff_t pos = (cmd->t_task_lba * se_dev->prot_length);
-	unsigned char *buf;
-	u32 prot_size;
-	int rc, ret = 1;
-
-	prot_size = (cmd->data_length / se_dev->dev_attrib.block_size) *
-		     se_dev->prot_length;
-
-	if (!is_write) {
-		fd_prot->prot_buf = kzalloc(prot_size, GFP_KERNEL);
-		if (!fd_prot->prot_buf) {
-			pr_err("Unable to allocate fd_prot->prot_buf\n");
-			return -ENOMEM;
-		}
-		buf = fd_prot->prot_buf;
-
-		fd_prot->prot_sg_nents = 1;
-		fd_prot->prot_sg = kzalloc(sizeof(struct scatterlist),
-					   GFP_KERNEL);
-		if (!fd_prot->prot_sg) {
-			pr_err("Unable to allocate fd_prot->prot_sg\n");
-			kfree(fd_prot->prot_buf);
-			return -ENOMEM;
-		}
-		sg_init_table(fd_prot->prot_sg, fd_prot->prot_sg_nents);
-		sg_set_buf(fd_prot->prot_sg, buf, prot_size);
-	}
-
-	if (is_write) {
-		rc = kernel_write(prot_fd, fd_prot->prot_buf, prot_size, pos);
-		if (rc < 0 || prot_size != rc) {
-			pr_err("kernel_write() for fd_do_prot_rw failed:"
-			       " %d\n", rc);
-			ret = -EINVAL;
-		}
-	} else {
-		rc = kernel_read(prot_fd, pos, fd_prot->prot_buf, prot_size);
-		if (rc < 0) {
-			pr_err("kernel_read() for fd_do_prot_rw failed:"
-			       " %d\n", rc);
-			ret = -EINVAL;
-		}
-	}
+	struct fd_dev *fd_dev = FD_DEV(dev);
 
-	if (is_write || ret < 0) {
-		kfree(fd_prot->prot_sg);
-		kfree(fd_prot->prot_buf);
+	if (fd_dev->fd_file) {
+		filp_close(fd_dev->fd_file, NULL);
+		fd_dev->fd_file = NULL;
 	}
-
-	return ret;
+	call_rcu(&dev->rcu_head, fd_dev_call_rcu);
 }
 
-static int fd_do_rw(struct se_cmd *cmd, struct scatterlist *sgl,
-		u32 sgl_nents, int is_write)
+static int fd_do_rw(struct se_cmd *cmd, struct file *fd,
+		    u32 block_size, struct scatterlist *sgl,
+		    u32 sgl_nents, u32 data_length, int is_write)
 {
-	struct se_device *se_dev = cmd->se_dev;
-	struct fd_dev *dev = FD_DEV(se_dev);
-	struct file *fd = dev->fd_file;
 	struct scatterlist *sg;
 	struct iov_iter iter;
 	struct bio_vec *bvec;
 	ssize_t len = 0;
-	loff_t pos = (cmd->t_task_lba * se_dev->dev_attrib.block_size);
+	loff_t pos = (cmd->t_task_lba * block_size);
 	int ret = 0, i;
 
 	bvec = kcalloc(sgl_nents, sizeof(struct bio_vec), GFP_KERNEL);
@@ -352,7 +294,7 @@ static int fd_do_rw(struct se_cmd *cmd, struct scatterlist *sgl,
 	kfree(bvec);
 
 	if (is_write) {
-		if (ret < 0 || ret != cmd->data_length) {
+		if (ret < 0 || ret != data_length) {
 			pr_err("%s() write returned %d\n", __func__, ret);
 			return (ret < 0 ? ret : -EINVAL);
 		}
@@ -363,10 +305,10 @@ static int fd_do_rw(struct se_cmd *cmd, struct scatterlist *sgl,
 		 * block_device.
 		 */
 		if (S_ISBLK(file_inode(fd)->i_mode)) {
-			if (ret < 0 || ret != cmd->data_length) {
+			if (ret < 0 || ret != data_length) {
 				pr_err("%s() returned %d, expecting %u for "
 						"S_ISBLK\n", __func__, ret,
-						cmd->data_length);
+						data_length);
 				return (ret < 0 ? ret : -EINVAL);
 			}
 		} else {
@@ -533,9 +475,9 @@ fd_do_prot_unmap(struct se_cmd *cmd, sector_t lba, sector_t nolb)
 }
 
 static sense_reason_t
-fd_do_unmap(struct se_cmd *cmd, void *priv, sector_t lba, sector_t nolb)
+fd_execute_unmap(struct se_cmd *cmd, sector_t lba, sector_t nolb)
 {
-	struct file *file = priv;
+	struct file *file = FD_DEV(cmd->se_dev)->fd_file;
 	struct inode *inode = file->f_mapping->host;
 	int ret;
 
@@ -577,42 +519,13 @@ fd_do_unmap(struct se_cmd *cmd, void *priv, sector_t lba, sector_t nolb)
 }
 
 static sense_reason_t
-fd_execute_write_same_unmap(struct se_cmd *cmd)
-{
-	struct se_device *se_dev = cmd->se_dev;
-	struct fd_dev *fd_dev = FD_DEV(se_dev);
-	struct file *file = fd_dev->fd_file;
-	sector_t lba = cmd->t_task_lba;
-	sector_t nolb = sbc_get_write_same_sectors(cmd);
-	sense_reason_t ret;
-
-	if (!nolb) {
-		target_complete_cmd(cmd, SAM_STAT_GOOD);
-		return 0;
-	}
-
-	ret = fd_do_unmap(cmd, file, lba, nolb);
-	if (ret)
-		return ret;
-
-	target_complete_cmd(cmd, GOOD);
-	return 0;
-}
-
-static sense_reason_t
-fd_execute_unmap(struct se_cmd *cmd)
-{
-	struct file *file = FD_DEV(cmd->se_dev)->fd_file;
-
-	return sbc_execute_unmap(cmd, fd_do_unmap, file);
-}
-
-static sense_reason_t
 fd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 	      enum dma_data_direction data_direction)
 {
 	struct se_device *dev = cmd->se_dev;
-	struct fd_prot fd_prot;
+	struct fd_dev *fd_dev = FD_DEV(dev);
+	struct file *file = fd_dev->fd_file;
+	struct file *pfile = fd_dev->fd_prot_file;
 	sense_reason_t rc;
 	int ret = 0;
 	/*
@@ -630,58 +543,45 @@ fd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 	 * physical memory addresses to struct iovec virtual memory.
 	 */
 	if (data_direction == DMA_FROM_DEVICE) {
-		memset(&fd_prot, 0, sizeof(struct fd_prot));
-
 		if (cmd->prot_type && dev->dev_attrib.pi_prot_type) {
-			ret = fd_do_prot_rw(cmd, &fd_prot, false);
+			ret = fd_do_rw(cmd, pfile, dev->prot_length,
+				       cmd->t_prot_sg, cmd->t_prot_nents,
+				       cmd->prot_length, 0);
 			if (ret < 0)
 				return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		}
 
-		ret = fd_do_rw(cmd, sgl, sgl_nents, 0);
+		ret = fd_do_rw(cmd, file, dev->dev_attrib.block_size,
+			       sgl, sgl_nents, cmd->data_length, 0);
 
 		if (ret > 0 && cmd->prot_type && dev->dev_attrib.pi_prot_type) {
-			u32 sectors = cmd->data_length / dev->dev_attrib.block_size;
+			u32 sectors = cmd->data_length >>
+					ilog2(dev->dev_attrib.block_size);
 
-			rc = sbc_dif_verify_read(cmd, cmd->t_task_lba, sectors,
-						 0, fd_prot.prot_sg, 0);
-			if (rc) {
-				kfree(fd_prot.prot_sg);
-				kfree(fd_prot.prot_buf);
+			rc = sbc_dif_verify(cmd, cmd->t_task_lba, sectors,
+					    0, cmd->t_prot_sg, 0);
+			if (rc)
 				return rc;
-			}
-			kfree(fd_prot.prot_sg);
-			kfree(fd_prot.prot_buf);
 		}
 	} else {
-		memset(&fd_prot, 0, sizeof(struct fd_prot));
-
 		if (cmd->prot_type && dev->dev_attrib.pi_prot_type) {
-			u32 sectors = cmd->data_length / dev->dev_attrib.block_size;
+			u32 sectors = cmd->data_length >>
+					ilog2(dev->dev_attrib.block_size);
 
-			ret = fd_do_prot_rw(cmd, &fd_prot, false);
-			if (ret < 0)
-				return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-
-			rc = sbc_dif_verify_write(cmd, cmd->t_task_lba, sectors,
-						  0, fd_prot.prot_sg, 0);
-			if (rc) {
-				kfree(fd_prot.prot_sg);
-				kfree(fd_prot.prot_buf);
+			rc = sbc_dif_verify(cmd, cmd->t_task_lba, sectors,
+					    0, cmd->t_prot_sg, 0);
+			if (rc)
 				return rc;
-			}
 		}
 
-		ret = fd_do_rw(cmd, sgl, sgl_nents, 1);
+		ret = fd_do_rw(cmd, file, dev->dev_attrib.block_size,
+			       sgl, sgl_nents, cmd->data_length, 1);
 		/*
 		 * Perform implicit vfs_fsync_range() for fd_do_writev() ops
 		 * for SCSI WRITEs with Forced Unit Access (FUA) set.
 		 * Allow this to happen independent of WCE=0 setting.
 		 */
-		if (ret > 0 &&
-		    dev->dev_attrib.emulate_fua_write > 0 &&
-		    (cmd->se_cmd_flags & SCF_FUA)) {
-			struct fd_dev *fd_dev = FD_DEV(dev);
+		if (ret > 0 && (cmd->se_cmd_flags & SCF_FUA)) {
 			loff_t start = cmd->t_task_lba *
 				dev->dev_attrib.block_size;
 			loff_t end;
@@ -695,17 +595,16 @@ fd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 		}
 
 		if (ret > 0 && cmd->prot_type && dev->dev_attrib.pi_prot_type) {
-			ret = fd_do_prot_rw(cmd, &fd_prot, true);
+			ret = fd_do_rw(cmd, pfile, dev->prot_length,
+				       cmd->t_prot_sg, cmd->t_prot_nents,
+				       cmd->prot_length, 1);
 			if (ret < 0)
 				return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		}
 	}
 
-	if (ret < 0) {
-		kfree(fd_prot.prot_sg);
-		kfree(fd_prot.prot_buf);
+	if (ret < 0)
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-	}
 
 	if (ret)
 		target_complete_cmd(cmd, SAM_STAT_GOOD);
@@ -908,7 +807,6 @@ static struct sbc_ops fd_sbc_ops = {
 	.execute_rw		= fd_execute_rw,
 	.execute_sync_cache	= fd_execute_sync_cache,
 	.execute_write_same	= fd_execute_write_same,
-	.execute_write_same_unmap = fd_execute_write_same_unmap,
 	.execute_unmap		= fd_execute_unmap,
 };
 
@@ -918,42 +816,7 @@ 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_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 = {
+static const struct target_backend_ops fileio_ops = {
 	.name			= "fileio",
 	.inquiry_prod		= "FILEIO",
 	.inquiry_rev		= FD_VERSION,
@@ -971,21 +834,17 @@ static struct se_subsystem_api fileio_template = {
 	.init_prot		= fd_init_prot,
 	.format_prot		= fd_format_prot,
 	.free_prot		= fd_free_prot,
+	.tb_dev_attrib_attrs	= sbc_attrib_attrs,
 };
 
 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);
+	return transport_backend_register(&fileio_ops);
 }
 
 static void __exit fileio_module_exit(void)
 {
-	transport_subsystem_release(&fileio_template);
+	target_backend_unregister(&fileio_ops);
 }
 
 MODULE_DESCRIPTION("TCM FILEIO subsystem plugin");
diff --git a/drivers/target/target_core_file.h b/drivers/target/target_core_file.h
index 182cbb295039..068966fce308 100644
--- a/drivers/target/target_core_file.h
+++ b/drivers/target/target_core_file.h
@@ -21,12 +21,6 @@
 #define FDBD_HAS_BUFFERED_IO_WCE 0x04
 #define FDBD_FORMAT_UNIT_SIZE	2048
 
-struct fd_prot {
-	unsigned char	*prot_buf;
-	struct scatterlist *prot_sg;
-	u32 prot_sg_nents;
-};
-
 struct fd_dev {
 	struct se_device dev;
 
diff --git a/drivers/target/target_core_hba.c b/drivers/target/target_core_hba.c
index ff95f95dcd13..62ea4e8e70a8 100644
--- a/drivers/target/target_core_hba.c
+++ b/drivers/target/target_core_hba.c
@@ -36,67 +36,78 @@
 #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"
 
-static LIST_HEAD(subsystem_list);
-static DEFINE_MUTEX(subsystem_mutex);
+static LIST_HEAD(backend_list);
+static DEFINE_MUTEX(backend_mutex);
 
 static u32 hba_id_counter;
 
 static DEFINE_SPINLOCK(hba_lock);
 static LIST_HEAD(hba_list);
 
-int transport_subsystem_register(struct se_subsystem_api *sub_api)
-{
-	struct se_subsystem_api *s;
-
-	INIT_LIST_HEAD(&sub_api->sub_api_list);
 
-	mutex_lock(&subsystem_mutex);
-	list_for_each_entry(s, &subsystem_list, sub_api_list) {
-		if (!strcmp(s->name, sub_api->name)) {
-			pr_err("%p is already registered with"
-				" duplicate name %s, unable to process"
-				" request\n", s, s->name);
-			mutex_unlock(&subsystem_mutex);
+int transport_backend_register(const struct target_backend_ops *ops)
+{
+	struct target_backend *tb, *old;
+
+	tb = kzalloc(sizeof(*tb), GFP_KERNEL);
+	if (!tb)
+		return -ENOMEM;
+	tb->ops = ops;
+
+	mutex_lock(&backend_mutex);
+	list_for_each_entry(old, &backend_list, list) {
+		if (!strcmp(old->ops->name, ops->name)) {
+			pr_err("backend %s already registered.\n", ops->name);
+			mutex_unlock(&backend_mutex);
+			kfree(tb);
 			return -EEXIST;
 		}
 	}
-	list_add_tail(&sub_api->sub_api_list, &subsystem_list);
-	mutex_unlock(&subsystem_mutex);
+	target_setup_backend_cits(tb);
+	list_add_tail(&tb->list, &backend_list);
+	mutex_unlock(&backend_mutex);
 
-	pr_debug("TCM: Registered subsystem plugin: %s struct module:"
-			" %p\n", sub_api->name, sub_api->owner);
+	pr_debug("TCM: Registered subsystem plugin: %s struct module: %p\n",
+			ops->name, ops->owner);
 	return 0;
 }
-EXPORT_SYMBOL(transport_subsystem_register);
+EXPORT_SYMBOL(transport_backend_register);
 
-void transport_subsystem_release(struct se_subsystem_api *sub_api)
+void target_backend_unregister(const struct target_backend_ops *ops)
 {
-	mutex_lock(&subsystem_mutex);
-	list_del(&sub_api->sub_api_list);
-	mutex_unlock(&subsystem_mutex);
+	struct target_backend *tb;
+
+	mutex_lock(&backend_mutex);
+	list_for_each_entry(tb, &backend_list, list) {
+		if (tb->ops == ops) {
+			list_del(&tb->list);
+			kfree(tb);
+			break;
+		}
+	}
+	mutex_unlock(&backend_mutex);
 }
-EXPORT_SYMBOL(transport_subsystem_release);
+EXPORT_SYMBOL(target_backend_unregister);
 
-static struct se_subsystem_api *core_get_backend(const char *sub_name)
+static struct target_backend *core_get_backend(const char *name)
 {
-	struct se_subsystem_api *s;
+	struct target_backend *tb;
 
-	mutex_lock(&subsystem_mutex);
-	list_for_each_entry(s, &subsystem_list, sub_api_list) {
-		if (!strcmp(s->name, sub_name))
+	mutex_lock(&backend_mutex);
+	list_for_each_entry(tb, &backend_list, list) {
+		if (!strcmp(tb->ops->name, name))
 			goto found;
 	}
-	mutex_unlock(&subsystem_mutex);
+	mutex_unlock(&backend_mutex);
 	return NULL;
 found:
-	if (s->owner && !try_module_get(s->owner))
-		s = NULL;
-	mutex_unlock(&subsystem_mutex);
-	return s;
+	if (tb->ops->owner && !try_module_get(tb->ops->owner))
+		tb = NULL;
+	mutex_unlock(&backend_mutex);
+	return tb;
 }
 
 struct se_hba *
@@ -117,13 +128,13 @@ core_alloc_hba(const char *plugin_name, u32 plugin_dep_id, u32 hba_flags)
 	hba->hba_index = scsi_get_new_index(SCSI_INST_INDEX);
 	hba->hba_flags |= hba_flags;
 
-	hba->transport = core_get_backend(plugin_name);
-	if (!hba->transport) {
+	hba->backend = core_get_backend(plugin_name);
+	if (!hba->backend) {
 		ret = -EINVAL;
 		goto out_free_hba;
 	}
 
-	ret = hba->transport->attach_hba(hba, plugin_dep_id);
+	ret = hba->backend->ops->attach_hba(hba, plugin_dep_id);
 	if (ret < 0)
 		goto out_module_put;
 
@@ -138,8 +149,8 @@ core_alloc_hba(const char *plugin_name, u32 plugin_dep_id, u32 hba_flags)
 	return hba;
 
 out_module_put:
-	module_put(hba->transport->owner);
-	hba->transport = NULL;
+	module_put(hba->backend->ops->owner);
+	hba->backend = NULL;
 out_free_hba:
 	kfree(hba);
 	return ERR_PTR(ret);
@@ -150,7 +161,7 @@ core_delete_hba(struct se_hba *hba)
 {
 	WARN_ON(hba->dev_count);
 
-	hba->transport->detach_hba(hba);
+	hba->backend->ops->detach_hba(hba);
 
 	spin_lock(&hba_lock);
 	list_del(&hba->hba_node);
@@ -159,9 +170,9 @@ core_delete_hba(struct se_hba *hba)
 	pr_debug("CORE_HBA[%d] - Detached HBA from Generic Target"
 			" Core\n", hba->hba_id);
 
-	module_put(hba->transport->owner);
+	module_put(hba->backend->ops->owner);
 
-	hba->transport = NULL;
+	hba->backend = NULL;
 	kfree(hba);
 	return 0;
 }
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index 8c965683789f..6d88d24e6cce 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -35,13 +35,11 @@
 #include <linux/genhd.h>
 #include <linux/file.h>
 #include <linux/module.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
+#include <scsi/scsi_proto.h>
 #include <asm/unaligned.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
-#include <target/target_core_backend_configfs.h>
 
 #include "target_core_iblock.h"
 
@@ -54,17 +52,11 @@ static inline struct iblock_dev *IBLOCK_DEV(struct se_device *dev)
 }
 
 
-static struct se_subsystem_api iblock_template;
-
-/*	iblock_attach_hba(): (Part of se_subsystem_api_t template)
- *
- *
- */
 static int iblock_attach_hba(struct se_hba *hba, u32 host_id)
 {
 	pr_debug("CORE_HBA[%d] - TCM iBlock HBA Driver %s on"
 		" Generic Target Core Stack %s\n", hba->hba_id,
-		IBLOCK_VERSION, TARGET_CORE_MOD_VERSION);
+		IBLOCK_VERSION, TARGET_CORE_VERSION);
 	return 0;
 }
 
@@ -198,6 +190,14 @@ out:
 	return ret;
 }
 
+static void iblock_dev_call_rcu(struct rcu_head *p)
+{
+	struct se_device *dev = container_of(p, struct se_device, rcu_head);
+	struct iblock_dev *ib_dev = IBLOCK_DEV(dev);
+
+	kfree(ib_dev);
+}
+
 static void iblock_free_device(struct se_device *dev)
 {
 	struct iblock_dev *ib_dev = IBLOCK_DEV(dev);
@@ -207,7 +207,7 @@ static void iblock_free_device(struct se_device *dev)
 	if (ib_dev->ibd_bio_set != NULL)
 		bioset_free(ib_dev->ibd_bio_set);
 
-	kfree(ib_dev);
+	call_rcu(&dev->rcu_head, iblock_dev_call_rcu);
 }
 
 static unsigned long long iblock_emulate_read_cap_with_block_size(
@@ -415,10 +415,9 @@ iblock_execute_sync_cache(struct se_cmd *cmd)
 }
 
 static sense_reason_t
-iblock_do_unmap(struct se_cmd *cmd, void *priv,
-		sector_t lba, sector_t nolb)
+iblock_execute_unmap(struct se_cmd *cmd, sector_t lba, sector_t nolb)
 {
-	struct block_device *bdev = priv;
+	struct block_device *bdev = IBLOCK_DEV(cmd->se_dev)->ibd_bd;
 	int ret;
 
 	ret = blkdev_issue_discard(bdev, lba, nolb, GFP_KERNEL, 0);
@@ -431,30 +430,6 @@ iblock_do_unmap(struct se_cmd *cmd, void *priv,
 }
 
 static sense_reason_t
-iblock_execute_unmap(struct se_cmd *cmd)
-{
-	struct block_device *bdev = IBLOCK_DEV(cmd->se_dev)->ibd_bd;
-
-	return sbc_execute_unmap(cmd, iblock_do_unmap, bdev);
-}
-
-static sense_reason_t
-iblock_execute_write_same_unmap(struct se_cmd *cmd)
-{
-	struct block_device *bdev = IBLOCK_DEV(cmd->se_dev)->ibd_bd;
-	sector_t lba = cmd->t_task_lba;
-	sector_t nolb = sbc_get_write_same_sectors(cmd);
-	sense_reason_t ret;
-
-	ret = iblock_do_unmap(cmd, bdev, lba, nolb);
-	if (ret)
-		return ret;
-
-	target_complete_cmd(cmd, GOOD);
-	return 0;
-}
-
-static sense_reason_t
 iblock_execute_write_same(struct se_cmd *cmd)
 {
 	struct iblock_req *ibr;
@@ -845,7 +820,6 @@ static struct sbc_ops iblock_sbc_ops = {
 	.execute_rw		= iblock_execute_rw,
 	.execute_sync_cache	= iblock_execute_sync_cache,
 	.execute_write_same	= iblock_execute_write_same,
-	.execute_write_same_unmap = iblock_execute_write_same_unmap,
 	.execute_unmap		= iblock_execute_unmap,
 };
 
@@ -864,42 +838,7 @@ 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_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 = {
+static const struct target_backend_ops iblock_ops = {
 	.name			= "iblock",
 	.inquiry_prod		= "IBLOCK",
 	.inquiry_rev		= IBLOCK_VERSION,
@@ -919,21 +858,17 @@ static struct se_subsystem_api iblock_template = {
 	.get_io_min		= iblock_get_io_min,
 	.get_io_opt		= iblock_get_io_opt,
 	.get_write_cache	= iblock_get_write_cache,
+	.tb_dev_attrib_attrs	= sbc_attrib_attrs,
 };
 
 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);
+	return transport_backend_register(&iblock_ops);
 }
 
 static void __exit iblock_module_exit(void)
 {
-	transport_subsystem_release(&iblock_template);
+	target_backend_unregister(&iblock_ops);
 }
 
 MODULE_DESCRIPTION("TCM IBLOCK subsystem plugin");
diff --git a/drivers/target/target_core_internal.h b/drivers/target/target_core_internal.h
index 68bd7f5d9f73..99c24acfe676 100644
--- a/drivers/target/target_core_internal.h
+++ b/drivers/target/target_core_internal.h
@@ -1,6 +1,53 @@
 #ifndef TARGET_CORE_INTERNAL_H
 #define TARGET_CORE_INTERNAL_H
 
+#define TARGET_CORE_NAME_MAX_LEN	64
+#define TARGET_FABRIC_NAME_SIZE		32
+
+struct target_backend {
+	struct list_head list;
+
+	const struct target_backend_ops *ops;
+
+	struct config_item_type tb_dev_cit;
+	struct config_item_type tb_dev_attrib_cit;
+	struct config_item_type tb_dev_pr_cit;
+	struct config_item_type tb_dev_wwn_cit;
+	struct config_item_type tb_dev_alua_tg_pt_gps_cit;
+	struct config_item_type tb_dev_stat_cit;
+};
+
+struct target_fabric_configfs {
+	atomic_t		tf_access_cnt;
+	struct list_head	tf_list;
+	struct config_group	tf_group;
+	struct config_group	tf_disc_group;
+	struct config_group	*tf_default_groups[2];
+	const struct target_core_fabric_ops *tf_ops;
+
+	struct config_item_type tf_discovery_cit;
+	struct config_item_type	tf_wwn_cit;
+	struct config_item_type tf_wwn_fabric_stats_cit;
+	struct config_item_type tf_tpg_cit;
+	struct config_item_type tf_tpg_base_cit;
+	struct config_item_type tf_tpg_lun_cit;
+	struct config_item_type tf_tpg_port_cit;
+	struct config_item_type tf_tpg_port_stat_cit;
+	struct config_item_type tf_tpg_np_cit;
+	struct config_item_type tf_tpg_np_base_cit;
+	struct config_item_type tf_tpg_attrib_cit;
+	struct config_item_type tf_tpg_auth_cit;
+	struct config_item_type tf_tpg_param_cit;
+	struct config_item_type tf_tpg_nacl_cit;
+	struct config_item_type tf_tpg_nacl_base_cit;
+	struct config_item_type tf_tpg_nacl_attrib_cit;
+	struct config_item_type tf_tpg_nacl_auth_cit;
+	struct config_item_type tf_tpg_nacl_param_cit;
+	struct config_item_type tf_tpg_nacl_stat_cit;
+	struct config_item_type tf_tpg_mappedlun_cit;
+	struct config_item_type tf_tpg_mappedlun_stat_cit;
+};
+
 /* target_core_alua.c */
 extern struct t10_alua_lu_gp *default_lu_gp;
 
@@ -8,28 +55,27 @@ extern struct t10_alua_lu_gp *default_lu_gp;
 extern struct mutex g_device_mutex;
 extern struct list_head g_device_list;
 
+int	core_alloc_rtpi(struct se_lun *lun, struct se_device *dev);
 struct se_dev_entry *core_get_se_deve_from_rtpi(struct se_node_acl *, u16);
-int	core_free_device_list_for_node(struct se_node_acl *,
+void	target_pr_kref_release(struct kref *);
+void	core_free_device_list_for_node(struct se_node_acl *,
 		struct se_portal_group *);
-void	core_update_device_list_access(u32, u32, struct se_node_acl *);
+void	core_update_device_list_access(u64, u32, struct se_node_acl *);
+struct se_dev_entry *target_nacl_find_deve(struct se_node_acl *, u64);
 int	core_enable_device_list_for_node(struct se_lun *, struct se_lun_acl *,
-		u32, u32, struct se_node_acl *, struct se_portal_group *);
-int	core_disable_device_list_for_node(struct se_lun *, struct se_lun_acl *,
-		u32, u32, struct se_node_acl *, struct se_portal_group *);
+		u64, u32, struct se_node_acl *, struct se_portal_group *);
+void	core_disable_device_list_for_node(struct se_lun *, struct se_dev_entry *,
+		struct se_node_acl *, struct se_portal_group *);
 void	core_clear_lun_from_tpg(struct se_lun *, struct se_portal_group *);
-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 *);
-struct se_lun *core_dev_add_lun(struct se_portal_group *, struct se_device *, u32);
+int	core_dev_add_lun(struct se_portal_group *, struct se_device *,
+		struct se_lun *lun);
 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);
 struct se_lun_acl *core_dev_init_initiator_node_lun_acl(struct se_portal_group *,
-		struct se_node_acl *, u32, int *);
+		struct se_node_acl *, u64, int *);
 int	core_dev_add_initiator_node_lun_acl(struct se_portal_group *,
-		struct se_lun_acl *, u32, u32);
-int	core_dev_del_initiator_node_lun_acl(struct se_portal_group *,
-		struct se_lun *, struct se_lun_acl *);
+		struct se_lun_acl *, struct se_lun *lun, u32);
+int	core_dev_del_initiator_node_lun_acl(struct se_lun *,
+		struct se_lun_acl *);
 void	core_dev_free_initiator_node_lun_acl(struct se_portal_group *,
 		struct se_lun_acl *lacl);
 int	core_dev_setup_virtual_lun0(void);
@@ -38,6 +84,18 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name);
 int	target_configure_device(struct se_device *dev);
 void	target_free_device(struct se_device *);
 
+/* target_core_configfs.c */
+void	target_setup_backend_cits(struct target_backend *);
+
+/* target_core_fabric_lib.c */
+int	target_get_pr_transport_id_len(struct se_node_acl *nacl,
+		struct t10_pr_registration *pr_reg, int *format_code);
+int	target_get_pr_transport_id(struct se_node_acl *nacl,
+		struct t10_pr_registration *pr_reg, int *format_code,
+		unsigned char *buf);
+const char *target_parse_pr_out_transport_id(struct se_portal_group *tpg,
+		const char *buf, u32 *out_tid_len, char **port_nexus_ptr);
+
 /* target_core_hba.c */
 struct se_hba *core_alloc_hba(const char *, u32, u32);
 int	core_delete_hba(struct se_hba *);
@@ -53,12 +111,16 @@ extern struct se_device *g_lun0_dev;
 
 struct se_node_acl *__core_tpg_get_initiator_node_acl(struct se_portal_group *tpg,
 		const char *);
-void	core_tpg_add_node_to_devs(struct se_node_acl *, struct se_portal_group *);
+void	core_tpg_add_node_to_devs(struct se_node_acl *, struct se_portal_group *,
+				  struct se_lun *);
 void	core_tpg_wait_for_nacl_pr_ref(struct se_node_acl *);
-struct se_lun *core_tpg_alloc_lun(struct se_portal_group *, u32);
+struct se_lun *core_tpg_alloc_lun(struct se_portal_group *, u64);
 int	core_tpg_add_lun(struct se_portal_group *, struct se_lun *,
 		u32, struct se_device *);
 void core_tpg_remove_lun(struct se_portal_group *, struct se_lun *);
+struct se_node_acl *core_tpg_add_initiator_node_acl(struct se_portal_group *tpg,
+		const char *initiatorname);
+void core_tpg_del_initiator_node_acl(struct se_node_acl *acl);
 
 /* target_core_transport.c */
 extern struct kmem_cache *se_tmr_req_cache;
@@ -77,14 +139,19 @@ int	transport_dump_vpd_assoc(struct t10_vpd *, unsigned char *, int);
 int	transport_dump_vpd_ident_type(struct t10_vpd *, unsigned char *, int);
 int	transport_dump_vpd_ident(struct t10_vpd *, unsigned char *, int);
 bool	target_stop_cmd(struct se_cmd *cmd, unsigned long *flags);
-int	transport_clear_lun_ref(struct se_lun *);
+void	transport_clear_lun_ref(struct se_lun *);
 void	transport_send_task_abort(struct se_cmd *);
 sense_reason_t	target_cmd_size_check(struct se_cmd *cmd, unsigned int size);
 void	target_qf_do_work(struct work_struct *work);
+bool	target_check_wce(struct se_device *dev);
+bool	target_check_fua(struct se_device *dev);
 
 /* target_core_stat.c */
 void	target_stat_setup_dev_default_groups(struct se_device *);
 void	target_stat_setup_port_default_groups(struct se_lun *);
 void	target_stat_setup_mappedlun_default_groups(struct se_lun_acl *);
 
+/* target_core_xcopy.c */
+extern struct se_portal_group xcopy_pt_tpg;
+
 #endif /* TARGET_CORE_INTERNAL_H */
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index a15411c79ae9..0fdbe43b7dad 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -27,15 +27,14 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/list.h>
+#include <linux/vmalloc.h>
 #include <linux/file.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_proto.h>
 #include <asm/unaligned.h>
 
 #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"
 #include "target_core_pr.h"
@@ -45,7 +44,6 @@
  * Used for Specify Initiator Ports Capable Bit (SPEC_I_PT)
  */
 struct pr_transport_id_holder {
-	int dest_local_nexus;
 	struct t10_pr_registration *dest_pr_reg;
 	struct se_portal_group *dest_tpg;
 	struct se_node_acl *dest_node_acl;
@@ -231,9 +229,10 @@ target_scsi2_reservation_release(struct se_cmd *cmd)
 		dev->dev_reservation_flags &= ~DRF_SPC2_RESERVATIONS_WITH_ISID;
 	}
 	tpg = sess->se_tpg;
-	pr_debug("SCSI-2 Released reservation for %s LUN: %u ->"
-		" MAPPED LUN: %u for %s\n", tpg->se_tpg_tfo->get_fabric_name(),
-		cmd->se_lun->unpacked_lun, cmd->se_deve->mapped_lun,
+	pr_debug("SCSI-2 Released reservation for %s LUN: %llu ->"
+		" MAPPED LUN: %llu for %s\n",
+		tpg->se_tpg_tfo->get_fabric_name(),
+		cmd->se_lun->unpacked_lun, cmd->orig_fe_lun,
 		sess->se_node_acl->initiatorname);
 
 out_unlock:
@@ -277,12 +276,12 @@ target_scsi2_reservation_reserve(struct se_cmd *cmd)
 	   (dev->dev_reserved_node_acl != sess->se_node_acl)) {
 		pr_err("SCSI-2 RESERVATION CONFLIFT for %s fabric\n",
 			tpg->se_tpg_tfo->get_fabric_name());
-		pr_err("Original reserver LUN: %u %s\n",
+		pr_err("Original reserver LUN: %llu %s\n",
 			cmd->se_lun->unpacked_lun,
 			dev->dev_reserved_node_acl->initiatorname);
-		pr_err("Current attempt - LUN: %u -> MAPPED LUN: %u"
+		pr_err("Current attempt - LUN: %llu -> MAPPED LUN: %llu"
 			" from %s \n", cmd->se_lun->unpacked_lun,
-			cmd->se_deve->mapped_lun,
+			cmd->orig_fe_lun,
 			sess->se_node_acl->initiatorname);
 		ret = TCM_RESERVATION_CONFLICT;
 		goto out_unlock;
@@ -294,9 +293,9 @@ target_scsi2_reservation_reserve(struct se_cmd *cmd)
 		dev->dev_res_bin_isid = sess->sess_bin_isid;
 		dev->dev_reservation_flags |= DRF_SPC2_RESERVATIONS_WITH_ISID;
 	}
-	pr_debug("SCSI-2 Reserved %s LUN: %u -> MAPPED LUN: %u"
+	pr_debug("SCSI-2 Reserved %s LUN: %llu -> MAPPED LUN: %llu"
 		" for %s\n", tpg->se_tpg_tfo->get_fabric_name(),
-		cmd->se_lun->unpacked_lun, cmd->se_deve->mapped_lun,
+		cmd->se_lun->unpacked_lun, cmd->orig_fe_lun,
 		sess->se_node_acl->initiatorname);
 
 out_unlock:
@@ -314,28 +313,31 @@ out:
  * This function is called by those initiator ports who are *NOT*
  * the active PR reservation holder when a reservation is present.
  */
-static int core_scsi3_pr_seq_non_holder(
-	struct se_cmd *cmd,
-	u32 pr_reg_type)
+static int core_scsi3_pr_seq_non_holder(struct se_cmd *cmd, u32 pr_reg_type,
+					bool isid_mismatch)
 {
 	unsigned char *cdb = cmd->t_task_cdb;
-	struct se_dev_entry *se_deve;
 	struct se_session *se_sess = cmd->se_sess;
-	int other_cdb = 0, ignore_reg;
+	struct se_node_acl *nacl = se_sess->se_node_acl;
+	int other_cdb = 0;
 	int registered_nexus = 0, ret = 1; /* Conflict by default */
 	int all_reg = 0, reg_only = 0; /* ALL_REG, REG_ONLY */
 	int we = 0; /* Write Exclusive */
 	int legacy = 0; /* Act like a legacy device and return
 			 * RESERVATION CONFLICT on some CDBs */
 
-	se_deve = se_sess->se_node_acl->device_list[cmd->orig_fe_lun];
-	/*
-	 * Determine if the registration should be ignored due to
-	 * non-matching ISIDs in target_scsi3_pr_reservation_check().
-	 */
-	ignore_reg = (pr_reg_type & 0x80000000);
-	if (ignore_reg)
-		pr_reg_type &= ~0x80000000;
+	if (isid_mismatch) {
+		registered_nexus = 0;
+	} else {
+		struct se_dev_entry *se_deve;
+
+		rcu_read_lock();
+		se_deve = target_nacl_find_deve(nacl, cmd->orig_fe_lun);
+		if (se_deve)
+			registered_nexus = test_bit(DEF_PR_REG_ACTIVE,
+						    &se_deve->deve_flags);
+		rcu_read_unlock();
+	}
 
 	switch (pr_reg_type) {
 	case PR_TYPE_WRITE_EXCLUSIVE:
@@ -345,8 +347,6 @@ static int core_scsi3_pr_seq_non_holder(
 		 * Some commands are only allowed for the persistent reservation
 		 * holder.
 		 */
-		if ((se_deve->def_pr_registered) && !(ignore_reg))
-			registered_nexus = 1;
 		break;
 	case PR_TYPE_WRITE_EXCLUSIVE_REGONLY:
 		we = 1;
@@ -355,8 +355,6 @@ static int core_scsi3_pr_seq_non_holder(
 		 * Some commands are only allowed for registered I_T Nexuses.
 		 */
 		reg_only = 1;
-		if ((se_deve->def_pr_registered) && !(ignore_reg))
-			registered_nexus = 1;
 		break;
 	case PR_TYPE_WRITE_EXCLUSIVE_ALLREG:
 		we = 1;
@@ -365,8 +363,6 @@ static int core_scsi3_pr_seq_non_holder(
 		 * Each registered I_T Nexus is a reservation holder.
 		 */
 		all_reg = 1;
-		if ((se_deve->def_pr_registered) && !(ignore_reg))
-			registered_nexus = 1;
 		break;
 	default:
 		return -EINVAL;
@@ -572,6 +568,7 @@ target_scsi3_pr_reservation_check(struct se_cmd *cmd)
 	struct se_device *dev = cmd->se_dev;
 	struct se_session *sess = cmd->se_sess;
 	u32 pr_reg_type;
+	bool isid_mismatch = false;
 
 	if (!dev->dev_pr_res_holder)
 		return 0;
@@ -584,7 +581,7 @@ target_scsi3_pr_reservation_check(struct se_cmd *cmd)
 	if (dev->dev_pr_res_holder->isid_present_at_reg) {
 		if (dev->dev_pr_res_holder->pr_reg_bin_isid !=
 		    sess->sess_bin_isid) {
-			pr_reg_type |= 0x80000000;
+			isid_mismatch = true;
 			goto check_nonholder;
 		}
 	}
@@ -592,7 +589,7 @@ target_scsi3_pr_reservation_check(struct se_cmd *cmd)
 	return 0;
 
 check_nonholder:
-	if (core_scsi3_pr_seq_non_holder(cmd, pr_reg_type))
+	if (core_scsi3_pr_seq_non_holder(cmd, pr_reg_type, isid_mismatch))
 		return TCM_RESERVATION_CONFLICT;
 	return 0;
 }
@@ -620,7 +617,9 @@ static u32 core_scsi3_pr_generation(struct se_device *dev)
 static struct t10_pr_registration *__core_scsi3_do_alloc_registration(
 	struct se_device *dev,
 	struct se_node_acl *nacl,
+	struct se_lun *lun,
 	struct se_dev_entry *deve,
+	u64 mapped_lun,
 	unsigned char *isid,
 	u64 sa_res_key,
 	int all_tg_pt,
@@ -642,12 +641,12 @@ static struct t10_pr_registration *__core_scsi3_do_alloc_registration(
 	atomic_set(&pr_reg->pr_res_holders, 0);
 	pr_reg->pr_reg_nacl = nacl;
 	pr_reg->pr_reg_deve = deve;
-	pr_reg->pr_res_mapped_lun = deve->mapped_lun;
-	pr_reg->pr_aptpl_target_lun = deve->se_lun->unpacked_lun;
+	pr_reg->pr_res_mapped_lun = mapped_lun;
+	pr_reg->pr_aptpl_target_lun = lun->unpacked_lun;
+	pr_reg->tg_pt_sep_rtpi = lun->lun_rtpi;
 	pr_reg->pr_res_key = sa_res_key;
 	pr_reg->pr_reg_all_tg_pt = all_tg_pt;
 	pr_reg->pr_reg_aptpl = aptpl;
-	pr_reg->pr_reg_tg_pt_lun = deve->se_lun;
 	/*
 	 * If an ISID value for this SCSI Initiator Port exists,
 	 * save it to the registration now.
@@ -671,7 +670,9 @@ static void core_scsi3_lunacl_undepend_item(struct se_dev_entry *);
 static struct t10_pr_registration *__core_scsi3_alloc_registration(
 	struct se_device *dev,
 	struct se_node_acl *nacl,
+	struct se_lun *lun,
 	struct se_dev_entry *deve,
+	u64 mapped_lun,
 	unsigned char *isid,
 	u64 sa_res_key,
 	int all_tg_pt,
@@ -679,7 +680,8 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 {
 	struct se_dev_entry *deve_tmp;
 	struct se_node_acl *nacl_tmp;
-	struct se_port *port, *port_tmp;
+	struct se_lun_acl *lacl_tmp;
+	struct se_lun *lun_tmp, *next, *dest_lun;
 	const struct target_core_fabric_ops *tfo = nacl->se_tpg->se_tpg_tfo;
 	struct t10_pr_registration *pr_reg, *pr_reg_atp, *pr_reg_tmp, *pr_reg_tmp_safe;
 	int ret;
@@ -687,8 +689,9 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 	 * Create a registration for the I_T Nexus upon which the
 	 * PROUT REGISTER was received.
 	 */
-	pr_reg = __core_scsi3_do_alloc_registration(dev, nacl, deve, isid,
-			sa_res_key, all_tg_pt, aptpl);
+	pr_reg = __core_scsi3_do_alloc_registration(dev, nacl, lun, deve, mapped_lun,
+						    isid, sa_res_key, all_tg_pt,
+						    aptpl);
 	if (!pr_reg)
 		return NULL;
 	/*
@@ -701,13 +704,13 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 	 * for ALL_TG_PT=1
 	 */
 	spin_lock(&dev->se_port_lock);
-	list_for_each_entry_safe(port, port_tmp, &dev->dev_sep_list, sep_list) {
-		atomic_inc_mb(&port->sep_tg_pt_ref_cnt);
+	list_for_each_entry_safe(lun_tmp, next, &dev->dev_sep_list, lun_dev_link) {
+		if (!percpu_ref_tryget_live(&lun_tmp->lun_ref))
+			continue;
 		spin_unlock(&dev->se_port_lock);
 
-		spin_lock_bh(&port->sep_alua_lock);
-		list_for_each_entry(deve_tmp, &port->sep_alua_list,
-					alua_port_list) {
+		spin_lock(&lun_tmp->lun_deve_lock);
+		list_for_each_entry(deve_tmp, &lun_tmp->lun_deve_list, lun_link) {
 			/*
 			 * This pointer will be NULL for demo mode MappedLUNs
 			 * that have not been make explicit via a ConfigFS
@@ -716,7 +719,9 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 			if (!deve_tmp->se_lun_acl)
 				continue;
 
-			nacl_tmp = deve_tmp->se_lun_acl->se_lun_nacl;
+			lacl_tmp = rcu_dereference_check(deve_tmp->se_lun_acl,
+						lockdep_is_held(&lun_tmp->lun_deve_lock));
+			nacl_tmp = lacl_tmp->se_lun_nacl;
 			/*
 			 * Skip the matching struct se_node_acl that is allocated
 			 * above..
@@ -736,8 +741,8 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 			if (strcmp(nacl->initiatorname, nacl_tmp->initiatorname))
 				continue;
 
-			atomic_inc_mb(&deve_tmp->pr_ref_count);
-			spin_unlock_bh(&port->sep_alua_lock);
+			kref_get(&deve_tmp->pr_kref);
+			spin_unlock(&lun_tmp->lun_deve_lock);
 			/*
 			 * Grab a configfs group dependency that is released
 			 * for the exception path at label out: below, or upon
@@ -748,8 +753,8 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 			if (ret < 0) {
 				pr_err("core_scsi3_lunacl_depend"
 						"_item() failed\n");
-				atomic_dec_mb(&port->sep_tg_pt_ref_cnt);
-				atomic_dec_mb(&deve_tmp->pr_ref_count);
+				percpu_ref_put(&lun_tmp->lun_ref);
+				kref_put(&deve_tmp->pr_kref, target_pr_kref_release);
 				goto out;
 			}
 			/*
@@ -759,24 +764,27 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 			 * the original *pr_reg is processed in
 			 * __core_scsi3_add_registration()
 			 */
+			dest_lun = rcu_dereference_check(deve_tmp->se_lun,
+				atomic_read(&deve_tmp->pr_kref.refcount) != 0);
+
 			pr_reg_atp = __core_scsi3_do_alloc_registration(dev,
-						nacl_tmp, deve_tmp, NULL,
+						nacl_tmp, dest_lun, deve_tmp,
+						deve_tmp->mapped_lun, NULL,
 						sa_res_key, all_tg_pt, aptpl);
 			if (!pr_reg_atp) {
-				atomic_dec_mb(&port->sep_tg_pt_ref_cnt);
-				atomic_dec_mb(&deve_tmp->pr_ref_count);
+				percpu_ref_put(&lun_tmp->lun_ref);
 				core_scsi3_lunacl_undepend_item(deve_tmp);
 				goto out;
 			}
 
 			list_add_tail(&pr_reg_atp->pr_reg_atp_mem_list,
 				      &pr_reg->pr_reg_atp_list);
-			spin_lock_bh(&port->sep_alua_lock);
+			spin_lock(&lun_tmp->lun_deve_lock);
 		}
-		spin_unlock_bh(&port->sep_alua_lock);
+		spin_unlock(&lun_tmp->lun_deve_lock);
 
 		spin_lock(&dev->se_port_lock);
-		atomic_dec_mb(&port->sep_tg_pt_ref_cnt);
+		percpu_ref_put(&lun_tmp->lun_ref);
 	}
 	spin_unlock(&dev->se_port_lock);
 
@@ -797,10 +805,10 @@ int core_scsi3_alloc_aptpl_registration(
 	u64 sa_res_key,
 	unsigned char *i_port,
 	unsigned char *isid,
-	u32 mapped_lun,
+	u64 mapped_lun,
 	unsigned char *t_port,
 	u16 tpgt,
-	u32 target_lun,
+	u64 target_lun,
 	int res_holder,
 	int all_tg_pt,
 	u8 type)
@@ -831,7 +839,6 @@ int core_scsi3_alloc_aptpl_registration(
 	pr_reg->pr_res_key = sa_res_key;
 	pr_reg->pr_reg_all_tg_pt = all_tg_pt;
 	pr_reg->pr_reg_aptpl = 1;
-	pr_reg->pr_reg_tg_pt_lun = NULL;
 	pr_reg->pr_res_scope = 0; /* Always LUN_SCOPE */
 	pr_reg->pr_res_type = type;
 	/*
@@ -895,9 +902,9 @@ static int __core_scsi3_check_aptpl_registration(
 	struct se_device *dev,
 	struct se_portal_group *tpg,
 	struct se_lun *lun,
-	u32 target_lun,
+	u64 target_lun,
 	struct se_node_acl *nacl,
-	struct se_dev_entry *deve)
+	u64 mapped_lun)
 {
 	struct t10_pr_registration *pr_reg, *pr_reg_tmp;
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
@@ -925,14 +932,13 @@ static int __core_scsi3_check_aptpl_registration(
 				pr_reg_aptpl_list) {
 
 		if (!strcmp(pr_reg->pr_iport, i_port) &&
-		     (pr_reg->pr_res_mapped_lun == deve->mapped_lun) &&
+		     (pr_reg->pr_res_mapped_lun == mapped_lun) &&
 		    !(strcmp(pr_reg->pr_tport, t_port)) &&
 		     (pr_reg->pr_reg_tpgt == tpgt) &&
 		     (pr_reg->pr_aptpl_target_lun == target_lun)) {
 
 			pr_reg->pr_reg_nacl = nacl;
-			pr_reg->pr_reg_deve = deve;
-			pr_reg->pr_reg_tg_pt_lun = lun;
+			pr_reg->tg_pt_sep_rtpi = lun->lun_rtpi;
 
 			list_del(&pr_reg->pr_reg_aptpl_list);
 			spin_unlock(&pr_tmpl->aptpl_reg_lock);
@@ -967,15 +973,14 @@ int core_scsi3_check_aptpl_registration(
 	struct se_portal_group *tpg,
 	struct se_lun *lun,
 	struct se_node_acl *nacl,
-	u32 mapped_lun)
+	u64 mapped_lun)
 {
-	struct se_dev_entry *deve = nacl->device_list[mapped_lun];
-
 	if (dev->dev_reservation_flags & DRF_SPC2_RESERVATIONS)
 		return 0;
 
 	return __core_scsi3_check_aptpl_registration(dev, tpg, lun,
-				lun->unpacked_lun, nacl, deve);
+						     lun->unpacked_lun, nacl,
+						     mapped_lun);
 }
 
 static void __core_scsi3_dump_registration(
@@ -1009,10 +1014,6 @@ static void __core_scsi3_dump_registration(
 		pr_reg->pr_reg_aptpl);
 }
 
-/*
- * this function can be called with struct se_device->dev_reservation_lock
- * when register_move = 1
- */
 static void __core_scsi3_add_registration(
 	struct se_device *dev,
 	struct se_node_acl *nacl,
@@ -1023,6 +1024,7 @@ static void __core_scsi3_add_registration(
 	const struct target_core_fabric_ops *tfo = nacl->se_tpg->se_tpg_tfo;
 	struct t10_pr_registration *pr_reg_tmp, *pr_reg_tmp_safe;
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
+	struct se_dev_entry *deve;
 
 	/*
 	 * Increment PRgeneration counter for struct se_device upon a successful
@@ -1039,10 +1041,16 @@ static void __core_scsi3_add_registration(
 
 	spin_lock(&pr_tmpl->registration_lock);
 	list_add_tail(&pr_reg->pr_reg_list, &pr_tmpl->registration_list);
-	pr_reg->pr_reg_deve->def_pr_registered = 1;
 
 	__core_scsi3_dump_registration(tfo, dev, nacl, pr_reg, register_type);
 	spin_unlock(&pr_tmpl->registration_lock);
+
+	rcu_read_lock();
+	deve = pr_reg->pr_reg_deve;
+	if (deve)
+		set_bit(DEF_PR_REG_ACTIVE, &deve->deve_flags);
+	rcu_read_unlock();
+
 	/*
 	 * Skip extra processing for ALL_TG_PT=0 or REGISTER_AND_MOVE.
 	 */
@@ -1054,6 +1062,8 @@ static void __core_scsi3_add_registration(
 	 */
 	list_for_each_entry_safe(pr_reg_tmp, pr_reg_tmp_safe,
 			&pr_reg->pr_reg_atp_list, pr_reg_atp_mem_list) {
+		struct se_node_acl *nacl_tmp = pr_reg_tmp->pr_reg_nacl;
+
 		list_del(&pr_reg_tmp->pr_reg_atp_mem_list);
 
 		pr_reg_tmp->pr_res_generation = core_scsi3_pr_generation(dev);
@@ -1061,12 +1071,17 @@ static void __core_scsi3_add_registration(
 		spin_lock(&pr_tmpl->registration_lock);
 		list_add_tail(&pr_reg_tmp->pr_reg_list,
 			      &pr_tmpl->registration_list);
-		pr_reg_tmp->pr_reg_deve->def_pr_registered = 1;
 
-		__core_scsi3_dump_registration(tfo, dev,
-				pr_reg_tmp->pr_reg_nacl, pr_reg_tmp,
-				register_type);
+		__core_scsi3_dump_registration(tfo, dev, nacl_tmp, pr_reg_tmp,
+					       register_type);
 		spin_unlock(&pr_tmpl->registration_lock);
+
+		rcu_read_lock();
+		deve = pr_reg_tmp->pr_reg_deve;
+		if (deve)
+			set_bit(DEF_PR_REG_ACTIVE, &deve->deve_flags);
+		rcu_read_unlock();
+
 		/*
 		 * Drop configfs group dependency reference from
 		 * __core_scsi3_alloc_registration()
@@ -1078,7 +1093,9 @@ static void __core_scsi3_add_registration(
 static int core_scsi3_alloc_registration(
 	struct se_device *dev,
 	struct se_node_acl *nacl,
+	struct se_lun *lun,
 	struct se_dev_entry *deve,
+	u64 mapped_lun,
 	unsigned char *isid,
 	u64 sa_res_key,
 	int all_tg_pt,
@@ -1088,8 +1105,9 @@ static int core_scsi3_alloc_registration(
 {
 	struct t10_pr_registration *pr_reg;
 
-	pr_reg = __core_scsi3_alloc_registration(dev, nacl, deve, isid,
-			sa_res_key, all_tg_pt, aptpl);
+	pr_reg = __core_scsi3_alloc_registration(dev, nacl, lun, deve, mapped_lun,
+						 isid, sa_res_key, all_tg_pt,
+						 aptpl);
 	if (!pr_reg)
 		return -EPERM;
 
@@ -1242,13 +1260,13 @@ static void __core_scsi3_free_registration(
 	const struct target_core_fabric_ops *tfo =
 			pr_reg->pr_reg_nacl->se_tpg->se_tpg_tfo;
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
+	struct se_node_acl *nacl = pr_reg->pr_reg_nacl;
+	struct se_dev_entry *deve;
 	char i_buf[PR_REG_ISID_ID_LEN];
 
 	memset(i_buf, 0, PR_REG_ISID_ID_LEN);
 	core_pr_dump_initiator_port(pr_reg, i_buf, PR_REG_ISID_ID_LEN);
 
-	pr_reg->pr_reg_deve->def_pr_registered = 0;
-	pr_reg->pr_reg_deve->pr_res_key = 0;
 	if (!list_empty(&pr_reg->pr_reg_list))
 		list_del(&pr_reg->pr_reg_list);
 	/*
@@ -1257,6 +1275,8 @@ static void __core_scsi3_free_registration(
 	 */
 	if (dec_holders)
 		core_scsi3_put_pr_reg(pr_reg);
+
+	spin_unlock(&pr_tmpl->registration_lock);
 	/*
 	 * Wait until all reference from any other I_T nexuses for this
 	 * *pr_reg have been released.  Because list_del() is called above,
@@ -1264,13 +1284,18 @@ static void __core_scsi3_free_registration(
 	 * count back to zero, and we release *pr_reg.
 	 */
 	while (atomic_read(&pr_reg->pr_res_holders) != 0) {
-		spin_unlock(&pr_tmpl->registration_lock);
 		pr_debug("SPC-3 PR [%s] waiting for pr_res_holders\n",
 				tfo->get_fabric_name());
 		cpu_relax();
-		spin_lock(&pr_tmpl->registration_lock);
 	}
 
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, pr_reg->pr_res_mapped_lun);
+	if (deve)
+		clear_bit(DEF_PR_REG_ACTIVE, &deve->deve_flags);
+	rcu_read_unlock();
+
+	spin_lock(&pr_tmpl->registration_lock);
 	pr_debug("SPC-3 PR [%s] Service Action: UNREGISTER Initiator"
 		" Node: %s%s\n", tfo->get_fabric_name(),
 		pr_reg->pr_reg_nacl->initiatorname,
@@ -1392,12 +1417,14 @@ static void core_scsi3_nodeacl_undepend_item(struct se_node_acl *nacl)
 
 static int core_scsi3_lunacl_depend_item(struct se_dev_entry *se_deve)
 {
-	struct se_lun_acl *lun_acl = se_deve->se_lun_acl;
+	struct se_lun_acl *lun_acl;
 	struct se_node_acl *nacl;
 	struct se_portal_group *tpg;
 	/*
 	 * For nacl->dynamic_node_acl=1
 	 */
+	lun_acl = rcu_dereference_check(se_deve->se_lun_acl,
+				atomic_read(&se_deve->pr_kref.refcount) != 0);
 	if (!lun_acl)
 		return 0;
 
@@ -1409,21 +1436,23 @@ static int core_scsi3_lunacl_depend_item(struct se_dev_entry *se_deve)
 
 static void core_scsi3_lunacl_undepend_item(struct se_dev_entry *se_deve)
 {
-	struct se_lun_acl *lun_acl = se_deve->se_lun_acl;
+	struct se_lun_acl *lun_acl;
 	struct se_node_acl *nacl;
 	struct se_portal_group *tpg;
 	/*
 	 * For nacl->dynamic_node_acl=1
 	 */
+	lun_acl = rcu_dereference_check(se_deve->se_lun_acl,
+				atomic_read(&se_deve->pr_kref.refcount) != 0);
 	if (!lun_acl) {
-		atomic_dec_mb(&se_deve->pr_ref_count);
+		kref_put(&se_deve->pr_kref, target_pr_kref_release);
 		return;
 	}
 	nacl = lun_acl->se_lun_nacl;
 	tpg = nacl->se_tpg;
 
 	target_undepend_item(&lun_acl->se_lun_group.cg_item);
-	atomic_dec_mb(&se_deve->pr_ref_count);
+	kref_put(&se_deve->pr_kref, target_pr_kref_release);
 }
 
 static sense_reason_t
@@ -1436,30 +1465,25 @@ core_scsi3_decode_spec_i_port(
 	int aptpl)
 {
 	struct se_device *dev = cmd->se_dev;
-	struct se_port *tmp_port;
 	struct se_portal_group *dest_tpg = NULL, *tmp_tpg;
 	struct se_session *se_sess = cmd->se_sess;
 	struct se_node_acl *dest_node_acl = NULL;
-	struct se_dev_entry *dest_se_deve = NULL, *local_se_deve;
+	struct se_dev_entry *dest_se_deve = NULL;
 	struct t10_pr_registration *dest_pr_reg, *local_pr_reg, *pr_reg_e;
 	struct t10_pr_registration *pr_reg_tmp, *pr_reg_tmp_safe;
 	LIST_HEAD(tid_dest_list);
 	struct pr_transport_id_holder *tidh_new, *tidh, *tidh_tmp;
-	const struct target_core_fabric_ops *tmp_tf_ops;
-	unsigned char *buf;
-	unsigned char *ptr, *i_str = NULL, proto_ident, tmp_proto_ident;
+	unsigned char *buf, *ptr, proto_ident;
+	const unsigned char *i_str;
 	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;
 
-	local_se_deve = se_sess->se_node_acl->device_list[cmd->orig_fe_lun];
 	/*
 	 * Allocate a struct pr_transport_id_holder and setup the
-	 * local_node_acl and local_se_deve pointers and add to
-	 * struct list_head tid_dest_list for add registration
-	 * processing in the loop of tid_dest_list below.
+	 * local_node_acl pointer and add to struct list_head tid_dest_list
+	 * for add registration processing in the loop of tid_dest_list below.
 	 */
 	tidh_new = kzalloc(sizeof(struct pr_transport_id_holder), GFP_KERNEL);
 	if (!tidh_new) {
@@ -1469,10 +1493,10 @@ core_scsi3_decode_spec_i_port(
 	INIT_LIST_HEAD(&tidh_new->dest_list);
 	tidh_new->dest_tpg = tpg;
 	tidh_new->dest_node_acl = se_sess->se_node_acl;
-	tidh_new->dest_se_deve = local_se_deve;
 
 	local_pr_reg = __core_scsi3_alloc_registration(cmd->se_dev,
-				se_sess->se_node_acl, local_se_deve, l_isid,
+				se_sess->se_node_acl, cmd->se_lun,
+				NULL, cmd->orig_fe_lun, l_isid,
 				sa_res_key, all_tg_pt, aptpl);
 	if (!local_pr_reg) {
 		kfree(tidh_new);
@@ -1481,10 +1505,10 @@ core_scsi3_decode_spec_i_port(
 	tidh_new->dest_pr_reg = local_pr_reg;
 	/*
 	 * The local I_T nexus does not hold any configfs dependances,
-	 * so we set tid_h->dest_local_nexus=1 to prevent the
+	 * so we set tidh_new->dest_se_deve to NULL to prevent the
 	 * configfs_undepend_item() calls in the tid_dest_list loops below.
 	 */
-	tidh_new->dest_local_nexus = 1;
+	tidh_new->dest_se_deve = NULL;
 	list_add_tail(&tidh_new->dest_list, &tid_dest_list);
 
 	if (cmd->data_length < 28) {
@@ -1525,32 +1549,25 @@ core_scsi3_decode_spec_i_port(
 	ptr = &buf[28];
 
 	while (tpdl > 0) {
+		struct se_lun *dest_lun, *tmp_lun;
+
 		proto_ident = (ptr[0] & 0x0f);
 		dest_tpg = NULL;
 
 		spin_lock(&dev->se_port_lock);
-		list_for_each_entry(tmp_port, &dev->dev_sep_list, sep_list) {
-			tmp_tpg = tmp_port->sep_tpg;
-			if (!tmp_tpg)
-				continue;
-			tmp_tf_ops = tmp_tpg->se_tpg_tfo;
-			if (!tmp_tf_ops)
-				continue;
-			if (!tmp_tf_ops->get_fabric_proto_ident ||
-			    !tmp_tf_ops->tpg_parse_pr_out_transport_id)
-				continue;
+		list_for_each_entry(tmp_lun, &dev->dev_sep_list, lun_dev_link) {
+			tmp_tpg = tmp_lun->lun_tpg;
+
 			/*
 			 * Look for the matching proto_ident provided by
 			 * the received TransportID
 			 */
-			tmp_proto_ident = tmp_tf_ops->get_fabric_proto_ident(tmp_tpg);
-			if (tmp_proto_ident != proto_ident)
+			if (tmp_tpg->proto_id != proto_ident)
 				continue;
-			dest_rtpi = tmp_port->sep_rtpi;
+			dest_rtpi = tmp_lun->lun_rtpi;
 
-			i_str = tmp_tf_ops->tpg_parse_pr_out_transport_id(
-					tmp_tpg, (const char *)ptr, &tid_len,
-					&iport_ptr);
+			i_str = target_parse_pr_out_transport_id(tmp_tpg,
+					(const char *)ptr, &tid_len, &iport_ptr);
 			if (!i_str)
 				continue;
 
@@ -1569,12 +1586,12 @@ core_scsi3_decode_spec_i_port(
 			 * from the decoded fabric module specific TransportID
 			 * at *i_str.
 			 */
-			spin_lock_irq(&tmp_tpg->acl_node_lock);
+			mutex_lock(&tmp_tpg->acl_node_mutex);
 			dest_node_acl = __core_tpg_get_initiator_node_acl(
 						tmp_tpg, i_str);
 			if (dest_node_acl)
 				atomic_inc_mb(&dest_node_acl->acl_pr_ref_count);
-			spin_unlock_irq(&tmp_tpg->acl_node_lock);
+			mutex_unlock(&tmp_tpg->acl_node_mutex);
 
 			if (!dest_node_acl) {
 				core_scsi3_tpg_undepend_item(tmp_tpg);
@@ -1644,7 +1661,7 @@ core_scsi3_decode_spec_i_port(
 		if (core_scsi3_lunacl_depend_item(dest_se_deve)) {
 			pr_err("core_scsi3_lunacl_depend_item()"
 					" failed\n");
-			atomic_dec_mb(&dest_se_deve->pr_ref_count);
+			kref_put(&dest_se_deve->pr_kref, target_pr_kref_release);
 			core_scsi3_nodeacl_undepend_item(dest_node_acl);
 			core_scsi3_tpg_undepend_item(dest_tpg);
 			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
@@ -1652,7 +1669,7 @@ core_scsi3_decode_spec_i_port(
 		}
 
 		pr_debug("SPC-3 PR SPEC_I_PT: Located %s Node: %s"
-			" dest_se_deve mapped_lun: %u\n",
+			" dest_se_deve mapped_lun: %llu\n",
 			dest_tpg->se_tpg_tfo->get_fabric_name(),
 			dest_node_acl->initiatorname, dest_se_deve->mapped_lun);
 
@@ -1708,9 +1725,13 @@ core_scsi3_decode_spec_i_port(
 		 * and then call __core_scsi3_add_registration() in the
 		 * 2nd loop which will never fail.
 		 */
+		dest_lun = rcu_dereference_check(dest_se_deve->se_lun,
+				atomic_read(&dest_se_deve->pr_kref.refcount) != 0);
+
 		dest_pr_reg = __core_scsi3_alloc_registration(cmd->se_dev,
-				dest_node_acl, dest_se_deve, iport_ptr,
-				sa_res_key, all_tg_pt, aptpl);
+					dest_node_acl, dest_lun, dest_se_deve,
+					dest_se_deve->mapped_lun, iport_ptr,
+					sa_res_key, all_tg_pt, aptpl);
 		if (!dest_pr_reg) {
 			core_scsi3_lunacl_undepend_item(dest_se_deve);
 			core_scsi3_nodeacl_undepend_item(dest_node_acl);
@@ -1748,7 +1769,6 @@ core_scsi3_decode_spec_i_port(
 		dest_node_acl = tidh->dest_node_acl;
 		dest_se_deve = tidh->dest_se_deve;
 		dest_pr_reg = tidh->dest_pr_reg;
-		dest_local_nexus = tidh->dest_local_nexus;
 
 		list_del(&tidh->dest_list);
 		kfree(tidh);
@@ -1761,10 +1781,11 @@ core_scsi3_decode_spec_i_port(
 
 		pr_debug("SPC-3 PR [%s] SPEC_I_PT: Successfully"
 			" registered Transport ID for Node: %s%s Mapped LUN:"
-			" %u\n", dest_tpg->se_tpg_tfo->get_fabric_name(),
-			dest_node_acl->initiatorname, i_buf, dest_se_deve->mapped_lun);
+			" %llu\n", dest_tpg->se_tpg_tfo->get_fabric_name(),
+			dest_node_acl->initiatorname, i_buf, (dest_se_deve) ?
+			dest_se_deve->mapped_lun : 0);
 
-		if (dest_local_nexus)
+		if (!dest_se_deve)
 			continue;
 
 		core_scsi3_lunacl_undepend_item(dest_se_deve);
@@ -1785,7 +1806,6 @@ out:
 		dest_node_acl = tidh->dest_node_acl;
 		dest_se_deve = tidh->dest_se_deve;
 		dest_pr_reg = tidh->dest_pr_reg;
-		dest_local_nexus = tidh->dest_local_nexus;
 
 		list_del(&tidh->dest_list);
 		kfree(tidh);
@@ -1803,7 +1823,7 @@ out:
 
 		kmem_cache_free(t10_pr_reg_cache, dest_pr_reg);
 
-		if (dest_local_nexus)
+		if (!dest_se_deve)
 			continue;
 
 		core_scsi3_lunacl_undepend_item(dest_se_deve);
@@ -1818,7 +1838,6 @@ static int core_scsi3_update_aptpl_buf(
 	unsigned char *buf,
 	u32 pr_aptpl_buf_len)
 {
-	struct se_lun *lun;
 	struct se_portal_group *tpg;
 	struct t10_pr_registration *pr_reg;
 	unsigned char tmp[512], isid_buf[32];
@@ -1837,7 +1856,6 @@ static int core_scsi3_update_aptpl_buf(
 		tmp[0] = '\0';
 		isid_buf[0] = '\0';
 		tpg = pr_reg->pr_reg_nacl->se_tpg;
-		lun = pr_reg->pr_reg_tg_pt_lun;
 		/*
 		 * Write out any ISID value to APTPL metadata that was included
 		 * in the original registration.
@@ -1856,7 +1874,7 @@ static int core_scsi3_update_aptpl_buf(
 				"sa_res_key=%llu\n"
 				"res_holder=1\nres_type=%02x\n"
 				"res_scope=%02x\nres_all_tg_pt=%d\n"
-				"mapped_lun=%u\n", reg_count,
+				"mapped_lun=%llu\n", reg_count,
 				tpg->se_tpg_tfo->get_fabric_name(),
 				pr_reg->pr_reg_nacl->initiatorname, isid_buf,
 				pr_reg->pr_res_key, pr_reg->pr_res_type,
@@ -1866,7 +1884,7 @@ static int core_scsi3_update_aptpl_buf(
 			snprintf(tmp, 512, "PR_REG_START: %d\n"
 				"initiator_fabric=%s\ninitiator_node=%s\n%s"
 				"sa_res_key=%llu\nres_holder=0\n"
-				"res_all_tg_pt=%d\nmapped_lun=%u\n",
+				"res_all_tg_pt=%d\nmapped_lun=%llu\n",
 				reg_count, tpg->se_tpg_tfo->get_fabric_name(),
 				pr_reg->pr_reg_nacl->initiatorname, isid_buf,
 				pr_reg->pr_res_key, pr_reg->pr_reg_all_tg_pt,
@@ -1885,11 +1903,12 @@ static int core_scsi3_update_aptpl_buf(
 		 * Include information about the associated SCSI target port.
 		 */
 		snprintf(tmp, 512, "target_fabric=%s\ntarget_node=%s\n"
-			"tpgt=%hu\nport_rtpi=%hu\ntarget_lun=%u\nPR_REG_END:"
+			"tpgt=%hu\nport_rtpi=%hu\ntarget_lun=%llu\nPR_REG_END:"
 			" %d\n", tpg->se_tpg_tfo->get_fabric_name(),
 			tpg->se_tpg_tfo->tpg_get_wwn(tpg),
 			tpg->se_tpg_tfo->tpg_get_tag(tpg),
-			lun->lun_sep->sep_rtpi, lun->unpacked_lun, reg_count);
+			pr_reg->tg_pt_sep_rtpi, pr_reg->pr_aptpl_target_lun,
+			reg_count);
 
 		if ((len + strlen(tmp) >= pr_aptpl_buf_len)) {
 			pr_err("Unable to update renaming APTPL metadata,"
@@ -2000,7 +2019,6 @@ core_scsi3_emulate_pro_register(struct se_cmd *cmd, u64 res_key, u64 sa_res_key,
 {
 	struct se_session *se_sess = cmd->se_sess;
 	struct se_device *dev = cmd->se_dev;
-	struct se_dev_entry *se_deve;
 	struct se_lun *se_lun = cmd->se_lun;
 	struct se_portal_group *se_tpg;
 	struct t10_pr_registration *pr_reg, *pr_reg_p, *pr_reg_tmp;
@@ -2014,7 +2032,6 @@ core_scsi3_emulate_pro_register(struct se_cmd *cmd, u64 res_key, u64 sa_res_key,
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 	}
 	se_tpg = se_sess->se_tpg;
-	se_deve = se_sess->se_node_acl->device_list[cmd->orig_fe_lun];
 
 	if (se_tpg->se_tpg_tfo->sess_get_initiator_sid) {
 		memset(&isid_buf[0], 0, PR_REG_ISID_LEN);
@@ -2045,7 +2062,8 @@ core_scsi3_emulate_pro_register(struct se_cmd *cmd, u64 res_key, u64 sa_res_key,
 			 * Logical Unit of the SCSI device server.
 			 */
 			if (core_scsi3_alloc_registration(cmd->se_dev,
-					se_sess->se_node_acl, se_deve, isid_ptr,
+					se_sess->se_node_acl, cmd->se_lun,
+					NULL, cmd->orig_fe_lun, isid_ptr,
 					sa_res_key, all_tg_pt, aptpl,
 					register_type, 0)) {
 				pr_err("Unable to allocate"
@@ -2066,7 +2084,6 @@ core_scsi3_emulate_pro_register(struct se_cmd *cmd, u64 res_key, u64 sa_res_key,
 			if (ret != 0)
 				return ret;
 		}
-
 		return core_scsi3_update_and_write_aptpl(dev, aptpl);
 	}
 
@@ -2180,7 +2197,7 @@ core_scsi3_emulate_pro_register(struct se_cmd *cmd, u64 res_key, u64 sa_res_key,
 					&pr_tmpl->registration_list,
 					pr_reg_list) {
 
-				core_scsi3_ua_allocate(
+				target_ua_allocate_lun(
 					pr_reg_p->pr_reg_nacl,
 					pr_reg_p->pr_res_mapped_lun,
 					0x2A,
@@ -2607,7 +2624,7 @@ core_scsi3_emulate_pro_release(struct se_cmd *cmd, int type, int scope,
 		if (pr_reg_p == pr_reg)
 			continue;
 
-		core_scsi3_ua_allocate(pr_reg_p->pr_reg_nacl,
+		target_ua_allocate_lun(pr_reg_p->pr_reg_nacl,
 				pr_reg_p->pr_res_mapped_lun,
 				0x2A, ASCQ_2AH_RESERVATIONS_RELEASED);
 	}
@@ -2630,7 +2647,7 @@ core_scsi3_emulate_pro_clear(struct se_cmd *cmd, u64 res_key)
 	struct se_session *se_sess = cmd->se_sess;
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
 	struct t10_pr_registration *pr_reg, *pr_reg_tmp, *pr_reg_n, *pr_res_holder;
-	u32 pr_res_mapped_lun = 0;
+	u64 pr_res_mapped_lun = 0;
 	int calling_it_nexus = 0;
 	/*
 	 * Locate the existing *pr_reg via struct se_node_acl pointers
@@ -2692,7 +2709,7 @@ core_scsi3_emulate_pro_clear(struct se_cmd *cmd, u64 res_key)
 		 *    additional sense code set to RESERVATIONS PREEMPTED.
 		 */
 		if (!calling_it_nexus)
-			core_scsi3_ua_allocate(pr_reg_nacl, pr_res_mapped_lun,
+			target_ua_allocate_lun(pr_reg_nacl, pr_res_mapped_lun,
 				0x2A, ASCQ_2AH_RESERVATIONS_PREEMPTED);
 	}
 	spin_unlock(&pr_tmpl->registration_lock);
@@ -2786,7 +2803,7 @@ core_scsi3_pro_preempt(struct se_cmd *cmd, int type, int scope, u64 res_key,
 	LIST_HEAD(preempt_and_abort_list);
 	struct t10_pr_registration *pr_reg, *pr_reg_tmp, *pr_reg_n, *pr_res_holder;
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
-	u32 pr_res_mapped_lun = 0;
+	u64 pr_res_mapped_lun = 0;
 	int all_reg = 0, calling_it_nexus = 0;
 	bool sa_res_key_unmatched = sa_res_key != 0;
 	int prh_type = 0, prh_scope = 0;
@@ -2901,7 +2918,7 @@ core_scsi3_pro_preempt(struct se_cmd *cmd, int type, int scope, u64 res_key,
 						NULL, 0);
 			}
 			if (!calling_it_nexus)
-				core_scsi3_ua_allocate(pr_reg_nacl,
+				target_ua_allocate_lun(pr_reg_nacl,
 					pr_res_mapped_lun, 0x2A,
 					ASCQ_2AH_REGISTRATIONS_PREEMPTED);
 		}
@@ -3007,7 +3024,7 @@ core_scsi3_pro_preempt(struct se_cmd *cmd, int type, int scope, u64 res_key,
 		 *    persistent reservation and/or registration, with the
 		 *    additional sense code set to REGISTRATIONS PREEMPTED;
 		 */
-		core_scsi3_ua_allocate(pr_reg_nacl, pr_res_mapped_lun, 0x2A,
+		target_ua_allocate_lun(pr_reg_nacl, pr_res_mapped_lun, 0x2A,
 				ASCQ_2AH_REGISTRATIONS_PREEMPTED);
 	}
 	spin_unlock(&pr_tmpl->registration_lock);
@@ -3040,7 +3057,7 @@ core_scsi3_pro_preempt(struct se_cmd *cmd, int type, int scope, u64 res_key,
 			if (calling_it_nexus)
 				continue;
 
-			core_scsi3_ua_allocate(pr_reg->pr_reg_nacl,
+			target_ua_allocate_lun(pr_reg->pr_reg_nacl,
 					pr_reg->pr_res_mapped_lun, 0x2A,
 					ASCQ_2AH_RESERVATIONS_RELEASED);
 		}
@@ -3099,15 +3116,14 @@ core_scsi3_emulate_pro_register_and_move(struct se_cmd *cmd, u64 res_key,
 	struct se_session *se_sess = cmd->se_sess;
 	struct se_device *dev = cmd->se_dev;
 	struct se_dev_entry *dest_se_deve = NULL;
-	struct se_lun *se_lun = cmd->se_lun;
+	struct se_lun *se_lun = cmd->se_lun, *tmp_lun;
 	struct se_node_acl *pr_res_nacl, *pr_reg_nacl, *dest_node_acl = NULL;
-	struct se_port *se_port;
 	struct se_portal_group *se_tpg, *dest_se_tpg = NULL;
 	const struct target_core_fabric_ops *dest_tf_ops = NULL, *tf_ops;
 	struct t10_pr_registration *pr_reg, *pr_res_holder, *dest_pr_reg;
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
 	unsigned char *buf;
-	unsigned char *initiator_str;
+	const unsigned char *initiator_str;
 	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;
@@ -3186,12 +3202,10 @@ core_scsi3_emulate_pro_register_and_move(struct se_cmd *cmd, u64 res_key,
 	}
 
 	spin_lock(&dev->se_port_lock);
-	list_for_each_entry(se_port, &dev->dev_sep_list, sep_list) {
-		if (se_port->sep_rtpi != rtpi)
-			continue;
-		dest_se_tpg = se_port->sep_tpg;
-		if (!dest_se_tpg)
+	list_for_each_entry(tmp_lun, &dev->dev_sep_list, lun_dev_link) {
+		if (tmp_lun->lun_rtpi != rtpi)
 			continue;
+		dest_se_tpg = tmp_lun->lun_tpg;
 		dest_tf_ops = dest_se_tpg->se_tpg_tfo;
 		if (!dest_tf_ops)
 			continue;
@@ -3230,23 +3244,16 @@ core_scsi3_emulate_pro_register_and_move(struct se_cmd *cmd, u64 res_key,
 	pr_debug("SPC-3 PR REGISTER_AND_MOVE: Extracted Protocol Identifier:"
 			" 0x%02x\n", proto_ident);
 
-	if (proto_ident != dest_tf_ops->get_fabric_proto_ident(dest_se_tpg)) {
+	if (proto_ident != dest_se_tpg->proto_id) {
 		pr_err("SPC-3 PR REGISTER_AND_MOVE: Received"
 			" proto_ident: 0x%02x does not match ident: 0x%02x"
 			" from fabric: %s\n", proto_ident,
-			dest_tf_ops->get_fabric_proto_ident(dest_se_tpg),
+			dest_se_tpg->proto_id,
 			dest_tf_ops->get_fabric_name());
 		ret = TCM_INVALID_PARAMETER_LIST;
 		goto out;
 	}
-	if (dest_tf_ops->tpg_parse_pr_out_transport_id == NULL) {
-		pr_err("SPC-3 PR REGISTER_AND_MOVE: Fabric does not"
-			" containg a valid tpg_parse_pr_out_transport_id"
-			" function pointer\n");
-		ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-		goto out;
-	}
-	initiator_str = dest_tf_ops->tpg_parse_pr_out_transport_id(dest_se_tpg,
+	initiator_str = target_parse_pr_out_transport_id(dest_se_tpg,
 			(const char *)&buf[24], &tmp_tid_len, &iport_ptr);
 	if (!initiator_str) {
 		pr_err("SPC-3 PR REGISTER_AND_MOVE: Unable to locate"
@@ -3295,12 +3302,12 @@ after_iport_check:
 	/*
 	 * Locate the destination struct se_node_acl from the received Transport ID
 	 */
-	spin_lock_irq(&dest_se_tpg->acl_node_lock);
+	mutex_lock(&dest_se_tpg->acl_node_mutex);
 	dest_node_acl = __core_tpg_get_initiator_node_acl(dest_se_tpg,
 				initiator_str);
 	if (dest_node_acl)
 		atomic_inc_mb(&dest_node_acl->acl_pr_ref_count);
-	spin_unlock_irq(&dest_se_tpg->acl_node_lock);
+	mutex_unlock(&dest_se_tpg->acl_node_mutex);
 
 	if (!dest_node_acl) {
 		pr_err("Unable to locate %s dest_node_acl for"
@@ -3337,14 +3344,14 @@ after_iport_check:
 
 	if (core_scsi3_lunacl_depend_item(dest_se_deve)) {
 		pr_err("core_scsi3_lunacl_depend_item() failed\n");
-		atomic_dec_mb(&dest_se_deve->pr_ref_count);
+		kref_put(&dest_se_deve->pr_kref, target_pr_kref_release);
 		dest_se_deve = NULL;
 		ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		goto out;
 	}
 
 	pr_debug("SPC-3 PR REGISTER_AND_MOVE: Located %s node %s LUN"
-		" ACL for dest_se_deve->mapped_lun: %u\n",
+		" ACL for dest_se_deve->mapped_lun: %llu\n",
 		dest_tf_ops->get_fabric_name(), dest_node_acl->initiatorname,
 		dest_se_deve->mapped_lun);
 
@@ -3421,13 +3428,17 @@ after_iport_check:
 	dest_pr_reg = __core_scsi3_locate_pr_reg(dev, dest_node_acl,
 					iport_ptr);
 	if (!dest_pr_reg) {
-		if (core_scsi3_alloc_registration(cmd->se_dev,
-				dest_node_acl, dest_se_deve, iport_ptr,
-				sa_res_key, 0, aptpl, 2, 1)) {
-			spin_unlock(&dev->dev_reservation_lock);
+		struct se_lun *dest_lun = rcu_dereference_check(dest_se_deve->se_lun,
+				atomic_read(&dest_se_deve->pr_kref.refcount) != 0);
+
+		spin_unlock(&dev->dev_reservation_lock);
+		if (core_scsi3_alloc_registration(cmd->se_dev, dest_node_acl,
+					dest_lun, dest_se_deve, dest_se_deve->mapped_lun,
+					iport_ptr, sa_res_key, 0, aptpl, 2, 1)) {
 			ret = TCM_INVALID_PARAMETER_LIST;
 			goto out;
 		}
+		spin_lock(&dev->dev_reservation_lock);
 		dest_pr_reg = __core_scsi3_locate_pr_reg(dev, dest_node_acl,
 						iport_ptr);
 		new_reg = 1;
@@ -3883,9 +3894,10 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 	struct t10_pr_registration *pr_reg, *pr_reg_tmp;
 	struct t10_reservation *pr_tmpl = &dev->t10_pr;
 	unsigned char *buf;
-	u32 add_desc_len = 0, add_len = 0, desc_len, exp_desc_len;
+	u32 add_desc_len = 0, add_len = 0;
 	u32 off = 8; /* off into first Full Status descriptor */
 	int format_code = 0, pr_res_type = 0, pr_res_scope = 0;
+	int exp_desc_len, desc_len;
 	bool all_reg = false;
 
 	if (cmd->data_length < 8) {
@@ -3930,10 +3942,10 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 		 * Determine expected length of $FABRIC_MOD specific
 		 * TransportID full status descriptor..
 		 */
-		exp_desc_len = se_tpg->se_tpg_tfo->tpg_get_pr_transport_id_len(
-				se_tpg, se_nacl, pr_reg, &format_code);
-
-		if ((exp_desc_len + add_len) > cmd->data_length) {
+		exp_desc_len = target_get_pr_transport_id_len(se_nacl, pr_reg,
+					&format_code);
+		if (exp_desc_len < 0 ||
+		    exp_desc_len + add_len > cmd->data_length) {
 			pr_warn("SPC-3 PRIN READ_FULL_STATUS ran"
 				" out of buffer: %d\n", cmd->data_length);
 			spin_lock(&pr_tmpl->registration_lock);
@@ -3990,21 +4002,26 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 		 * IDENTIFIER field are not defined by this standard.
 		 */
 		if (!pr_reg->pr_reg_all_tg_pt) {
-			struct se_port *port = pr_reg->pr_reg_tg_pt_lun->lun_sep;
+			u16 sep_rtpi = pr_reg->tg_pt_sep_rtpi;
 
-			buf[off++] = ((port->sep_rtpi >> 8) & 0xff);
-			buf[off++] = (port->sep_rtpi & 0xff);
+			buf[off++] = ((sep_rtpi >> 8) & 0xff);
+			buf[off++] = (sep_rtpi & 0xff);
 		} else
 			off += 2; /* Skip over RELATIVE TARGET PORT IDENTIFIER */
 
+		buf[off+4] = se_tpg->proto_id;
+
 		/*
-		 * Now, have the $FABRIC_MOD fill in the protocol identifier
+		 * Now, have the $FABRIC_MOD fill in the transport ID.
 		 */
-		desc_len = se_tpg->se_tpg_tfo->tpg_get_pr_transport_id(se_tpg,
-				se_nacl, pr_reg, &format_code, &buf[off+4]);
+		desc_len = target_get_pr_transport_id(se_nacl, pr_reg,
+				&format_code, &buf[off+4]);
 
 		spin_lock(&pr_tmpl->registration_lock);
 		atomic_dec_mb(&pr_reg->pr_res_holders);
+
+		if (desc_len < 0)
+			break;
 		/*
 		 * Set the ADDITIONAL DESCRIPTOR LENGTH
 		 */
diff --git a/drivers/target/target_core_pr.h b/drivers/target/target_core_pr.h
index 749fd7bb7510..e3d26e9126a0 100644
--- a/drivers/target/target_core_pr.h
+++ b/drivers/target/target_core_pr.h
@@ -56,11 +56,11 @@ extern sense_reason_t target_scsi2_reservation_release(struct se_cmd *);
 extern sense_reason_t target_scsi2_reservation_reserve(struct se_cmd *);
 extern int core_scsi3_alloc_aptpl_registration(
 			struct t10_reservation *, u64,
-			unsigned char *, unsigned char *, u32,
-			unsigned char *, u16, u32, int, int, u8);
+			unsigned char *, unsigned char *, u64,
+			unsigned char *, u16, u64, int, int, u8);
 extern int core_scsi3_check_aptpl_registration(struct se_device *,
 			struct se_portal_group *, struct se_lun *,
-			struct se_node_acl *, u32);
+			struct se_node_acl *, u64);
 extern void core_scsi3_free_pr_reg_from_nacl(struct se_device *,
 					     struct se_node_acl *);
 extern void core_scsi3_free_all_registrations(struct se_device *);
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index ecc5eaef13d6..08e9084ee615 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -36,17 +36,15 @@
 #include <linux/module.h>
 #include <asm/unaligned.h>
 
-#include <scsi/scsi.h>
 #include <scsi/scsi_device.h>
-#include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_tcq.h>
 
 #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_internal.h"
 #include "target_core_pscsi.h"
 
 #define ISPRINT(a)  ((a >= ' ') && (a <= '~'))
@@ -56,8 +54,6 @@ static inline struct pscsi_dev_virt *PSCSI_DEV(struct se_device *dev)
 	return container_of(dev, struct pscsi_dev_virt, dev);
 }
 
-static struct se_subsystem_api pscsi_template;
-
 static sense_reason_t pscsi_execute_cmd(struct se_cmd *cmd);
 static void pscsi_req_done(struct request *, int);
 
@@ -82,7 +78,7 @@ static int pscsi_attach_hba(struct se_hba *hba, u32 host_id)
 
 	pr_debug("CORE_HBA[%d] - TCM SCSI HBA Driver %s on"
 		" Generic Target Core Stack %s\n", hba->hba_id,
-		PSCSI_VERSION, TARGET_CORE_MOD_VERSION);
+		PSCSI_VERSION, TARGET_CORE_VERSION);
 	pr_debug("CORE_HBA[%d] - Attached SCSI HBA to Generic\n",
 	       hba->hba_id);
 
@@ -581,6 +577,14 @@ static int pscsi_configure_device(struct se_device *dev)
 	return -ENODEV;
 }
 
+static void pscsi_dev_call_rcu(struct rcu_head *p)
+{
+	struct se_device *dev = container_of(p, struct se_device, rcu_head);
+	struct pscsi_dev_virt *pdv = PSCSI_DEV(dev);
+
+	kfree(pdv);
+}
+
 static void pscsi_free_device(struct se_device *dev)
 {
 	struct pscsi_dev_virt *pdv = PSCSI_DEV(dev);
@@ -612,8 +616,7 @@ static void pscsi_free_device(struct se_device *dev)
 
 		pdv->pdv_sd = NULL;
 	}
-
-	kfree(pdv);
+	call_rcu(&dev->rcu_head, pscsi_dev_call_rcu);
 }
 
 static void pscsi_transport_complete(struct se_cmd *cmd, struct scatterlist *sg,
@@ -637,12 +640,14 @@ static void pscsi_transport_complete(struct se_cmd *cmd, struct scatterlist *sg,
 	 * Hack to make sure that Write-Protect modepage is set if R/O mode is
 	 * forced.
 	 */
-	if (!cmd->se_deve || !cmd->data_length)
+	if (!cmd->data_length)
 		goto after_mode_sense;
 
 	if (((cdb[0] == MODE_SENSE) || (cdb[0] == MODE_SENSE_10)) &&
 	     (status_byte(result) << 1) == SAM_STAT_GOOD) {
-		if (cmd->se_deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY) {
+		bool read_only = target_lun_is_rdonly(cmd);
+
+		if (read_only) {
 			unsigned char *buf;
 
 			buf = transport_kmap_data_sg(cmd);
@@ -1118,27 +1123,7 @@ 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 = {
+static const struct target_backend_ops pscsi_ops = {
 	.name			= "pscsi",
 	.owner			= THIS_MODULE,
 	.transport_flags	= TRANSPORT_FLAG_PASSTHROUGH,
@@ -1154,21 +1139,17 @@ static struct se_subsystem_api pscsi_template = {
 	.show_configfs_dev_params = pscsi_show_configfs_dev_params,
 	.get_device_type	= pscsi_get_device_type,
 	.get_blocks		= pscsi_get_blocks,
+	.tb_dev_attrib_attrs	= passthrough_attrib_attrs,
 };
 
 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);
+	return transport_backend_register(&pscsi_ops);
 }
 
 static void __exit pscsi_module_exit(void)
 {
-	transport_subsystem_release(&pscsi_template);
+	target_backend_unregister(&pscsi_ops);
 }
 
 MODULE_DESCRIPTION("TCM PSCSI subsystem plugin");
diff --git a/drivers/target/target_core_pscsi.h b/drivers/target/target_core_pscsi.h
index 820d3052b775..6d2007e35df6 100644
--- a/drivers/target/target_core_pscsi.h
+++ b/drivers/target/target_core_pscsi.h
@@ -16,13 +16,13 @@
 #define PS_TIMEOUT_OTHER	(500*HZ)
 
 #include <linux/device.h>
-#include <scsi/scsi_driver.h>
-#include <scsi/scsi_device.h>
 #include <linux/kref.h>
 #include <linux/kobject.h>
 
+struct scsi_device;
+
 struct pscsi_plugin_task {
-	unsigned char pscsi_sense[SCSI_SENSE_BUFFERSIZE];
+	unsigned char pscsi_sense[TRANSPORT_SENSE_BUFFER];
 	int	pscsi_direction;
 	int	pscsi_result;
 	u32	pscsi_resid;
diff --git a/drivers/target/target_core_rd.c b/drivers/target/target_core_rd.c
index d16489b6a1a4..4703f403f31c 100644
--- a/drivers/target/target_core_rd.c
+++ b/drivers/target/target_core_rd.c
@@ -29,12 +29,10 @@
 #include <linux/timer.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
+#include <scsi/scsi_proto.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
-#include <target/target_core_backend_configfs.h>
 
 #include "target_core_rd.h"
 
@@ -43,10 +41,6 @@ static inline struct rd_dev *RD_DEV(struct se_device *dev)
 	return container_of(dev, struct rd_dev, dev);
 }
 
-/*	rd_attach_hba(): (Part of se_subsystem_api_t template)
- *
- *
- */
 static int rd_attach_hba(struct se_hba *hba, u32 host_id)
 {
 	struct rd_host *rd_host;
@@ -63,7 +57,7 @@ static int rd_attach_hba(struct se_hba *hba, u32 host_id)
 
 	pr_debug("CORE_HBA[%d] - TCM Ramdisk HBA Driver %s on"
 		" Generic Target Core Stack %s\n", hba->hba_id,
-		RD_HBA_VERSION, TARGET_CORE_MOD_VERSION);
+		RD_HBA_VERSION, TARGET_CORE_VERSION);
 
 	return 0;
 }
@@ -355,12 +349,20 @@ fail:
 	return ret;
 }
 
+static void rd_dev_call_rcu(struct rcu_head *p)
+{
+	struct se_device *dev = container_of(p, struct se_device, rcu_head);
+	struct rd_dev *rd_dev = RD_DEV(dev);
+
+	kfree(rd_dev);
+}
+
 static void rd_free_device(struct se_device *dev)
 {
 	struct rd_dev *rd_dev = RD_DEV(dev);
 
 	rd_release_device_space(rd_dev);
-	kfree(rd_dev);
+	call_rcu(&dev->rcu_head, rd_dev_call_rcu);
 }
 
 static struct rd_dev_sg_table *rd_get_sg_table(struct rd_dev *rd_dev, u32 page)
@@ -403,10 +405,7 @@ static struct rd_dev_sg_table *rd_get_prot_table(struct rd_dev *rd_dev, u32 page
 	return NULL;
 }
 
-typedef sense_reason_t (*dif_verify)(struct se_cmd *, sector_t, unsigned int,
-				     unsigned int, struct scatterlist *, int);
-
-static sense_reason_t rd_do_prot_rw(struct se_cmd *cmd, dif_verify dif_verify)
+static sense_reason_t rd_do_prot_rw(struct se_cmd *cmd, bool is_read)
 {
 	struct se_device *se_dev = cmd->se_dev;
 	struct rd_dev *dev = RD_DEV(se_dev);
@@ -466,7 +465,16 @@ static sense_reason_t rd_do_prot_rw(struct se_cmd *cmd, dif_verify dif_verify)
 
 #endif /* !CONFIG_ARCH_HAS_SG_CHAIN */
 
-	rc = dif_verify(cmd, cmd->t_task_lba, sectors, 0, prot_sg, prot_offset);
+	if (is_read)
+		rc = sbc_dif_verify(cmd, cmd->t_task_lba, sectors, 0,
+				    prot_sg, prot_offset);
+	else
+		rc = sbc_dif_verify(cmd, cmd->t_task_lba, sectors, 0,
+				    cmd->t_prot_sg, 0);
+
+	if (!rc)
+		sbc_dif_copy_prot(cmd, sectors, is_read, prot_sg, prot_offset);
+
 	if (need_to_release)
 		kfree(prot_sg);
 
@@ -512,7 +520,7 @@ rd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 
 	if (cmd->prot_type && se_dev->dev_attrib.pi_prot_type &&
 	    data_direction == DMA_TO_DEVICE) {
-		rc = rd_do_prot_rw(cmd, sbc_dif_verify_write);
+		rc = rd_do_prot_rw(cmd, false);
 		if (rc)
 			return rc;
 	}
@@ -580,7 +588,7 @@ rd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 
 	if (cmd->prot_type && se_dev->dev_attrib.pi_prot_type &&
 	    data_direction == DMA_FROM_DEVICE) {
-		rc = rd_do_prot_rw(cmd, sbc_dif_verify_read);
+		rc = rd_do_prot_rw(cmd, true);
 		if (rc)
 			return rc;
 	}
@@ -694,42 +702,7 @@ 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_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 = {
+static const struct target_backend_ops rd_mcp_ops = {
 	.name			= "rd_mcp",
 	.inquiry_prod		= "RAMDISK-MCP",
 	.inquiry_rev		= RD_MCP_VERSION,
@@ -745,25 +718,15 @@ static struct se_subsystem_api rd_mcp_template = {
 	.get_blocks		= rd_get_blocks,
 	.init_prot		= rd_init_prot,
 	.free_prot		= rd_free_prot,
+	.tb_dev_attrib_attrs	= sbc_attrib_attrs,
 };
 
 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;
-	}
-
-	return 0;
+	return transport_backend_register(&rd_mcp_ops);
 }
 
 void rd_module_exit(void)
 {
-	transport_subsystem_release(&rd_mcp_template);
+	target_backend_unregister(&rd_mcp_ops);
 }
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index 733824e3825f..e318ddbe15da 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -25,7 +25,7 @@
 #include <linux/ratelimit.h>
 #include <linux/crc-t10dif.h>
 #include <asm/unaligned.h>
-#include <scsi/scsi.h>
+#include <scsi/scsi_proto.h>
 #include <scsi/scsi_tcq.h>
 
 #include <target/target_core_base.h>
@@ -38,6 +38,7 @@
 
 static sense_reason_t
 sbc_check_prot(struct se_device *, struct se_cmd *, unsigned char *, u32, bool);
+static sense_reason_t sbc_execute_unmap(struct se_cmd *cmd);
 
 static sense_reason_t
 sbc_emulate_readcapacity(struct se_cmd *cmd)
@@ -177,6 +178,23 @@ sector_t sbc_get_write_same_sectors(struct se_cmd *cmd)
 EXPORT_SYMBOL(sbc_get_write_same_sectors);
 
 static sense_reason_t
+sbc_execute_write_same_unmap(struct se_cmd *cmd)
+{
+	struct sbc_ops *ops = cmd->protocol_data;
+	sector_t nolb = sbc_get_write_same_sectors(cmd);
+	sense_reason_t ret;
+
+	if (nolb) {
+		ret = ops->execute_unmap(cmd, cmd->t_task_lba, nolb);
+		if (ret)
+			return ret;
+	}
+
+	target_complete_cmd(cmd, GOOD);
+	return 0;
+}
+
+static sense_reason_t
 sbc_emulate_noop(struct se_cmd *cmd)
 {
 	target_complete_cmd(cmd, GOOD);
@@ -299,7 +317,7 @@ sbc_setup_write_same(struct se_cmd *cmd, unsigned char *flags, struct sbc_ops *o
 	 * translated into block discard requests within backend code.
 	 */
 	if (flags[0] & 0x08) {
-		if (!ops->execute_write_same_unmap)
+		if (!ops->execute_unmap)
 			return TCM_UNSUPPORTED_SCSI_OPCODE;
 
 		if (!dev->dev_attrib.emulate_tpws) {
@@ -307,7 +325,7 @@ sbc_setup_write_same(struct se_cmd *cmd, unsigned char *flags, struct sbc_ops *o
 			       " has emulate_tpws disabled\n");
 			return TCM_UNSUPPORTED_SCSI_OPCODE;
 		}
-		cmd->execute_cmd = ops->execute_write_same_unmap;
+		cmd->execute_cmd = sbc_execute_write_same_unmap;
 		return 0;
 	}
 	if (!ops->execute_write_same)
@@ -381,7 +399,9 @@ out:
 static sense_reason_t
 sbc_execute_rw(struct se_cmd *cmd)
 {
-	return cmd->execute_rw(cmd, cmd->t_data_sg, cmd->t_data_nents,
+	struct sbc_ops *ops = cmd->protocol_data;
+
+	return ops->execute_rw(cmd, cmd->t_data_sg, cmd->t_data_nents,
 			       cmd->data_direction);
 }
 
@@ -560,6 +580,7 @@ out:
 static sense_reason_t
 sbc_compare_and_write(struct se_cmd *cmd)
 {
+	struct sbc_ops *ops = cmd->protocol_data;
 	struct se_device *dev = cmd->se_dev;
 	sense_reason_t ret;
 	int rc;
@@ -579,7 +600,7 @@ sbc_compare_and_write(struct se_cmd *cmd)
 	 */
 	cmd->data_length = cmd->t_task_nolb * dev->dev_attrib.block_size;
 
-	ret = cmd->execute_rw(cmd, cmd->t_bidi_data_sg, cmd->t_bidi_data_nents,
+	ret = ops->execute_rw(cmd, cmd->t_bidi_data_sg, cmd->t_bidi_data_nents,
 			      DMA_FROM_DEVICE);
 	if (ret) {
 		cmd->transport_complete_callback = NULL;
@@ -738,14 +759,15 @@ static int
 sbc_check_dpofua(struct se_device *dev, struct se_cmd *cmd, unsigned char *cdb)
 {
 	if (cdb[1] & 0x10) {
-		if (!dev->dev_attrib.emulate_dpo) {
+		/* see explanation in spc_emulate_modesense */
+		if (!target_check_fua(dev)) {
 			pr_err("Got CDB: 0x%02x with DPO bit set, but device"
 			       " does not advertise support for DPO\n", cdb[0]);
 			return -EINVAL;
 		}
 	}
 	if (cdb[1] & 0x8) {
-		if (!dev->dev_attrib.emulate_fua_write || !se_dev_check_wce(dev)) {
+		if (!target_check_fua(dev)) {
 			pr_err("Got CDB: 0x%02x with FUA bit set, but device"
 			       " does not advertise support for FUA write\n",
 			       cdb[0]);
@@ -765,12 +787,13 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 	u32 sectors = 0;
 	sense_reason_t ret;
 
+	cmd->protocol_data = ops;
+
 	switch (cdb[0]) {
 	case READ_6:
 		sectors = transport_get_sectors_6(cdb);
 		cmd->t_task_lba = transport_lba_21(cdb);
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		break;
 	case READ_10:
@@ -785,7 +808,6 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 			return ret;
 
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		break;
 	case READ_12:
@@ -800,7 +822,6 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 			return ret;
 
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		break;
 	case READ_16:
@@ -815,14 +836,12 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 			return ret;
 
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		break;
 	case WRITE_6:
 		sectors = transport_get_sectors_6(cdb);
 		cmd->t_task_lba = transport_lba_21(cdb);
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		break;
 	case WRITE_10:
@@ -838,7 +857,6 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 			return ret;
 
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		break;
 	case WRITE_12:
@@ -853,7 +871,6 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 			return ret;
 
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		break;
 	case WRITE_16:
@@ -868,7 +885,6 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 			return ret;
 
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		break;
 	case XDWRITEREAD_10:
@@ -886,7 +902,6 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 		/*
 		 * Setup BIDI XOR callback to be run after I/O completion.
 		 */
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_execute_rw;
 		cmd->transport_complete_callback = &xdreadwrite_callback;
 		break;
@@ -910,7 +925,6 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 			 * Setup BIDI XOR callback to be run during after I/O
 			 * completion.
 			 */
-			cmd->execute_rw = ops->execute_rw;
 			cmd->execute_cmd = sbc_execute_rw;
 			cmd->transport_complete_callback = &xdreadwrite_callback;
 			break;
@@ -954,7 +968,6 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 		cmd->t_task_lba = get_unaligned_be64(&cdb[2]);
 		cmd->t_task_nolb = sectors;
 		cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB | SCF_COMPARE_AND_WRITE;
-		cmd->execute_rw = ops->execute_rw;
 		cmd->execute_cmd = sbc_compare_and_write;
 		cmd->transport_complete_callback = compare_and_write_callback;
 		break;
@@ -1004,7 +1017,7 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 			return TCM_UNSUPPORTED_SCSI_OPCODE;
 		}
 		size = get_unaligned_be16(&cdb[7]);
-		cmd->execute_cmd = ops->execute_unmap;
+		cmd->execute_cmd = sbc_execute_unmap;
 		break;
 	case WRITE_SAME_16:
 		sectors = transport_get_sectors_16(cdb);
@@ -1092,12 +1105,10 @@ u32 sbc_get_device_type(struct se_device *dev)
 }
 EXPORT_SYMBOL(sbc_get_device_type);
 
-sense_reason_t
-sbc_execute_unmap(struct se_cmd *cmd,
-	sense_reason_t (*do_unmap_fn)(struct se_cmd *, void *,
-				      sector_t, sector_t),
-	void *priv)
+static sense_reason_t
+sbc_execute_unmap(struct se_cmd *cmd)
 {
+	struct sbc_ops *ops = cmd->protocol_data;
 	struct se_device *dev = cmd->se_dev;
 	unsigned char *buf, *ptr = NULL;
 	sector_t lba;
@@ -1161,7 +1172,7 @@ sbc_execute_unmap(struct se_cmd *cmd,
 			goto err;
 		}
 
-		ret = do_unmap_fn(cmd, priv, lba, range);
+		ret = ops->execute_unmap(cmd, lba, range);
 		if (ret)
 			goto err;
 
@@ -1175,34 +1186,56 @@ err:
 		target_complete_cmd(cmd, GOOD);
 	return ret;
 }
-EXPORT_SYMBOL(sbc_execute_unmap);
 
 void
 sbc_dif_generate(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct se_dif_v1_tuple *sdt;
-	struct scatterlist *dsg, *psg = cmd->t_prot_sg;
+	struct scatterlist *dsg = cmd->t_data_sg, *psg;
 	sector_t sector = cmd->t_task_lba;
 	void *daddr, *paddr;
 	int i, j, offset = 0;
+	unsigned int block_size = dev->dev_attrib.block_size;
 
-	for_each_sg(cmd->t_data_sg, dsg, cmd->t_data_nents, i) {
-		daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
+	for_each_sg(cmd->t_prot_sg, psg, cmd->t_prot_nents, i) {
 		paddr = kmap_atomic(sg_page(psg)) + psg->offset;
+		daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
 
-		for (j = 0; j < dsg->length; j += dev->dev_attrib.block_size) {
+		for (j = 0; j < psg->length;
+				j += sizeof(struct se_dif_v1_tuple)) {
+			__u16 crc;
+			unsigned int avail;
+
+			if (offset >= dsg->length) {
+				offset -= dsg->length;
+				kunmap_atomic(daddr - dsg->offset);
+				dsg = sg_next(dsg);
+				if (!dsg) {
+					kunmap_atomic(paddr - psg->offset);
+					return;
+				}
+				daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
+			}
 
-			if (offset >= psg->length) {
-				kunmap_atomic(paddr);
-				psg = sg_next(psg);
-				paddr = kmap_atomic(sg_page(psg)) + psg->offset;
-				offset = 0;
+			sdt = paddr + j;
+			avail = min(block_size, dsg->length - offset);
+			crc = crc_t10dif(daddr + offset, avail);
+			if (avail < block_size) {
+				kunmap_atomic(daddr - dsg->offset);
+				dsg = sg_next(dsg);
+				if (!dsg) {
+					kunmap_atomic(paddr - psg->offset);
+					return;
+				}
+				daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
+				offset = block_size - avail;
+				crc = crc_t10dif_update(crc, daddr, offset);
+			} else {
+				offset += block_size;
 			}
 
-			sdt = paddr + offset;
-			sdt->guard_tag = cpu_to_be16(crc_t10dif(daddr + j,
-						dev->dev_attrib.block_size));
+			sdt->guard_tag = cpu_to_be16(crc);
 			if (cmd->prot_type == TARGET_DIF_TYPE1_PROT)
 				sdt->ref_tag = cpu_to_be32(sector & 0xffffffff);
 			sdt->app_tag = 0;
@@ -1215,26 +1248,23 @@ sbc_dif_generate(struct se_cmd *cmd)
 				 be32_to_cpu(sdt->ref_tag));
 
 			sector++;
-			offset += sizeof(struct se_dif_v1_tuple);
 		}
 
-		kunmap_atomic(paddr);
-		kunmap_atomic(daddr);
+		kunmap_atomic(daddr - dsg->offset);
+		kunmap_atomic(paddr - psg->offset);
 	}
 }
 
 static sense_reason_t
 sbc_dif_v1_verify(struct se_cmd *cmd, struct se_dif_v1_tuple *sdt,
-		  const void *p, sector_t sector, unsigned int ei_lba)
+		  __u16 crc, sector_t sector, unsigned int ei_lba)
 {
-	struct se_device *dev = cmd->se_dev;
-	int block_size = dev->dev_attrib.block_size;
 	__be16 csum;
 
 	if (!(cmd->prot_checks & TARGET_DIF_CHECK_GUARD))
 		goto check_ref;
 
-	csum = cpu_to_be16(crc_t10dif(p, block_size));
+	csum = cpu_to_be16(crc);
 
 	if (sdt->guard_tag != csum) {
 		pr_err("DIFv1 checksum failed on sector %llu guard tag 0x%04x"
@@ -1266,9 +1296,8 @@ check_ref:
 	return 0;
 }
 
-static void
-sbc_dif_copy_prot(struct se_cmd *cmd, unsigned int sectors, bool read,
-		  struct scatterlist *sg, int sg_off)
+void sbc_dif_copy_prot(struct se_cmd *cmd, unsigned int sectors, bool read,
+		       struct scatterlist *sg, int sg_off)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct scatterlist *psg;
@@ -1300,100 +1329,54 @@ sbc_dif_copy_prot(struct se_cmd *cmd, unsigned int sectors, bool read,
 			copied += len;
 			psg_len -= len;
 
+			kunmap_atomic(addr - sg->offset - offset);
+
 			if (offset >= sg->length) {
 				sg = sg_next(sg);
 				offset = 0;
 			}
-			kunmap_atomic(addr);
 		}
-		kunmap_atomic(paddr);
+		kunmap_atomic(paddr - psg->offset);
 	}
 }
+EXPORT_SYMBOL(sbc_dif_copy_prot);
 
 sense_reason_t
-sbc_dif_verify_write(struct se_cmd *cmd, sector_t start, unsigned int sectors,
-		     unsigned int ei_lba, struct scatterlist *sg, int sg_off)
+sbc_dif_verify(struct se_cmd *cmd, sector_t start, unsigned int sectors,
+	       unsigned int ei_lba, struct scatterlist *psg, int psg_off)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct se_dif_v1_tuple *sdt;
-	struct scatterlist *dsg, *psg = cmd->t_prot_sg;
+	struct scatterlist *dsg = cmd->t_data_sg;
 	sector_t sector = start;
 	void *daddr, *paddr;
-	int i, j, offset = 0;
+	int i;
 	sense_reason_t rc;
+	int dsg_off = 0;
+	unsigned int block_size = dev->dev_attrib.block_size;
 
-	for_each_sg(cmd->t_data_sg, dsg, cmd->t_data_nents, i) {
-		daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
+	for (; psg && sector < start + sectors; psg = sg_next(psg)) {
 		paddr = kmap_atomic(sg_page(psg)) + psg->offset;
-
-		for (j = 0; j < dsg->length; j += dev->dev_attrib.block_size) {
-
-			if (offset >= psg->length) {
-				kunmap_atomic(paddr);
-				psg = sg_next(psg);
-				paddr = kmap_atomic(sg_page(psg)) + psg->offset;
-				offset = 0;
-			}
-
-			sdt = paddr + offset;
-
-			pr_debug("DIF WRITE sector: %llu guard_tag: 0x%04x"
-				 " app_tag: 0x%04x ref_tag: %u\n",
-				 (unsigned long long)sector, sdt->guard_tag,
-				 sdt->app_tag, be32_to_cpu(sdt->ref_tag));
-
-			rc = sbc_dif_v1_verify(cmd, sdt, daddr + j, sector,
-					       ei_lba);
-			if (rc) {
-				kunmap_atomic(paddr);
-				kunmap_atomic(daddr);
-				cmd->bad_sector = sector;
-				return rc;
-			}
-
-			sector++;
-			ei_lba++;
-			offset += sizeof(struct se_dif_v1_tuple);
-		}
-
-		kunmap_atomic(paddr);
-		kunmap_atomic(daddr);
-	}
-	if (!sg)
-		return 0;
-
-	sbc_dif_copy_prot(cmd, sectors, false, sg, sg_off);
-
-	return 0;
-}
-EXPORT_SYMBOL(sbc_dif_verify_write);
-
-static sense_reason_t
-__sbc_dif_verify_read(struct se_cmd *cmd, sector_t start, unsigned int sectors,
-		      unsigned int ei_lba, struct scatterlist *sg, int sg_off)
-{
-	struct se_device *dev = cmd->se_dev;
-	struct se_dif_v1_tuple *sdt;
-	struct scatterlist *dsg, *psg = sg;
-	sector_t sector = start;
-	void *daddr, *paddr;
-	int i, j, offset = sg_off;
-	sense_reason_t rc;
-
-	for_each_sg(cmd->t_data_sg, dsg, cmd->t_data_nents, i) {
 		daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
-		paddr = kmap_atomic(sg_page(psg)) + sg->offset;
-
-		for (j = 0; j < dsg->length; j += dev->dev_attrib.block_size) {
 
-			if (offset >= psg->length) {
-				kunmap_atomic(paddr);
-				psg = sg_next(psg);
-				paddr = kmap_atomic(sg_page(psg)) + psg->offset;
-				offset = 0;
+		for (i = psg_off; i < psg->length &&
+				sector < start + sectors;
+				i += sizeof(struct se_dif_v1_tuple)) {
+			__u16 crc;
+			unsigned int avail;
+
+			if (dsg_off >= dsg->length) {
+				dsg_off -= dsg->length;
+				kunmap_atomic(daddr - dsg->offset);
+				dsg = sg_next(dsg);
+				if (!dsg) {
+					kunmap_atomic(paddr - psg->offset);
+					return 0;
+				}
+				daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
 			}
 
-			sdt = paddr + offset;
+			sdt = paddr + i;
 
 			pr_debug("DIF READ sector: %llu guard_tag: 0x%04x"
 				 " app_tag: 0x%04x ref_tag: %u\n",
@@ -1401,53 +1384,43 @@ __sbc_dif_verify_read(struct se_cmd *cmd, sector_t start, unsigned int sectors,
 				 sdt->app_tag, be32_to_cpu(sdt->ref_tag));
 
 			if (sdt->app_tag == cpu_to_be16(0xffff)) {
-				sector++;
-				offset += sizeof(struct se_dif_v1_tuple);
-				continue;
+				dsg_off += block_size;
+				goto next;
+			}
+
+			avail = min(block_size, dsg->length - dsg_off);
+			crc = crc_t10dif(daddr + dsg_off, avail);
+			if (avail < block_size) {
+				kunmap_atomic(daddr - dsg->offset);
+				dsg = sg_next(dsg);
+				if (!dsg) {
+					kunmap_atomic(paddr - psg->offset);
+					return 0;
+				}
+				daddr = kmap_atomic(sg_page(dsg)) + dsg->offset;
+				dsg_off = block_size - avail;
+				crc = crc_t10dif_update(crc, daddr, dsg_off);
+			} else {
+				dsg_off += block_size;
 			}
 
-			rc = sbc_dif_v1_verify(cmd, sdt, daddr + j, sector,
-					       ei_lba);
+			rc = sbc_dif_v1_verify(cmd, sdt, crc, sector, ei_lba);
 			if (rc) {
-				kunmap_atomic(paddr);
-				kunmap_atomic(daddr);
+				kunmap_atomic(daddr - dsg->offset);
+				kunmap_atomic(paddr - psg->offset);
 				cmd->bad_sector = sector;
 				return rc;
 			}
-
+next:
 			sector++;
 			ei_lba++;
-			offset += sizeof(struct se_dif_v1_tuple);
 		}
 
-		kunmap_atomic(paddr);
-		kunmap_atomic(daddr);
+		psg_off = 0;
+		kunmap_atomic(daddr - dsg->offset);
+		kunmap_atomic(paddr - psg->offset);
 	}
 
 	return 0;
 }
-
-sense_reason_t
-sbc_dif_read_strip(struct se_cmd *cmd)
-{
-	struct se_device *dev = cmd->se_dev;
-	u32 sectors = cmd->prot_length / dev->prot_length;
-
-	return __sbc_dif_verify_read(cmd, cmd->t_task_lba, sectors, 0,
-				     cmd->t_prot_sg, 0);
-}
-
-sense_reason_t
-sbc_dif_verify_read(struct se_cmd *cmd, sector_t start, unsigned int sectors,
-		    unsigned int ei_lba, struct scatterlist *sg, int sg_off)
-{
-	sense_reason_t rc;
-
-	rc = __sbc_dif_verify_read(cmd, start, sectors, ei_lba, sg, sg_off);
-	if (rc)
-		return rc;
-
-	sbc_dif_copy_prot(cmd, sectors, true, sg, sg_off);
-	return 0;
-}
-EXPORT_SYMBOL(sbc_dif_verify_read);
+EXPORT_SYMBOL(sbc_dif_verify);
diff --git a/drivers/target/target_core_spc.c b/drivers/target/target_core_spc.c
index 7912aa124385..b0744433315a 100644
--- a/drivers/target/target_core_spc.c
+++ b/drivers/target/target_core_spc.c
@@ -24,7 +24,8 @@
 #include <linux/module.h>
 #include <asm/unaligned.h>
 
-#include <scsi/scsi.h>
+#include <scsi/scsi_proto.h>
+#include <scsi/scsi_common.h>
 #include <scsi/scsi_tcq.h>
 
 #include <target/target_core_base.h>
@@ -37,10 +38,9 @@
 #include "target_core_ua.h"
 #include "target_core_xcopy.h"
 
-static void spc_fill_alua_data(struct se_port *port, unsigned char *buf)
+static void spc_fill_alua_data(struct se_lun *lun, unsigned char *buf)
 {
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
 
 	/*
 	 * Set SCCS for MAINTENANCE_IN + REPORT_TARGET_PORT_GROUPS.
@@ -53,17 +53,11 @@ static void spc_fill_alua_data(struct se_port *port, unsigned char *buf)
 	 *
 	 * See spc4r17 section 6.4.2 Table 135
 	 */
-	if (!port)
-		return;
-	tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
-	if (!tg_pt_gp_mem)
-		return;
-
-	spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-	tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
+	spin_lock(&lun->lun_tg_pt_gp_lock);
+	tg_pt_gp = lun->lun_tg_pt_gp;
 	if (tg_pt_gp)
 		buf[5] |= tg_pt_gp->tg_pt_gp_alua_access_type;
-	spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+	spin_unlock(&lun->lun_tg_pt_gp_lock);
 }
 
 sense_reason_t
@@ -94,7 +88,7 @@ spc_emulate_inquiry_std(struct se_cmd *cmd, unsigned char *buf)
 	/*
 	 * Enable SCCS and TPGS fields for Emulated ALUA
 	 */
-	spc_fill_alua_data(lun->lun_sep, buf);
+	spc_fill_alua_data(lun, buf);
 
 	/*
 	 * Set Third-Party Copy (3PC) bit to indicate support for EXTENDED_COPY
@@ -181,11 +175,9 @@ spc_emulate_evpd_83(struct se_cmd *cmd, unsigned char *buf)
 {
 	struct se_device *dev = cmd->se_dev;
 	struct se_lun *lun = cmd->se_lun;
-	struct se_port *port = NULL;
 	struct se_portal_group *tpg = NULL;
 	struct t10_alua_lu_gp_member *lu_gp_mem;
 	struct t10_alua_tg_pt_gp *tg_pt_gp;
-	struct t10_alua_tg_pt_gp_member *tg_pt_gp_mem;
 	unsigned char *prod = &dev->t10_wwn.model[0];
 	u32 prod_len;
 	u32 unit_serial_len, off = 0;
@@ -267,18 +259,15 @@ check_t10_vend_desc:
 	/* Header size for Designation descriptor */
 	len += (id_len + 4);
 	off += (id_len + 4);
-	/*
-	 * struct se_port is only set for INQUIRY VPD=1 through $FABRIC_MOD
-	 */
-	port = lun->lun_sep;
-	if (port) {
+
+	if (1) {
 		struct t10_alua_lu_gp *lu_gp;
 		u32 padding, scsi_name_len, scsi_target_len;
 		u16 lu_gp_id = 0;
 		u16 tg_pt_gp_id = 0;
 		u16 tpgt;
 
-		tpg = port->sep_tpg;
+		tpg = lun->lun_tpg;
 		/*
 		 * Relative target port identifer, see spc4r17
 		 * section 7.7.3.7
@@ -286,8 +275,7 @@ check_t10_vend_desc:
 		 * Get the PROTOCOL IDENTIFIER as defined by spc4r17
 		 * section 7.5.1 Table 362
 		 */
-		buf[off] =
-			(tpg->se_tpg_tfo->get_fabric_proto_ident(tpg) << 4);
+		buf[off] = tpg->proto_id << 4;
 		buf[off++] |= 0x1; /* CODE SET == Binary */
 		buf[off] = 0x80; /* Set PIV=1 */
 		/* Set ASSOCIATION == target port: 01b */
@@ -299,8 +287,8 @@ check_t10_vend_desc:
 		/* Skip over Obsolete field in RTPI payload
 		 * in Table 472 */
 		off += 2;
-		buf[off++] = ((port->sep_rtpi >> 8) & 0xff);
-		buf[off++] = (port->sep_rtpi & 0xff);
+		buf[off++] = ((lun->lun_rtpi >> 8) & 0xff);
+		buf[off++] = (lun->lun_rtpi & 0xff);
 		len += 8; /* Header size + Designation descriptor */
 		/*
 		 * Target port group identifier, see spc4r17
@@ -309,21 +297,16 @@ check_t10_vend_desc:
 		 * Get the PROTOCOL IDENTIFIER as defined by spc4r17
 		 * section 7.5.1 Table 362
 		 */
-		tg_pt_gp_mem = port->sep_alua_tg_pt_gp_mem;
-		if (!tg_pt_gp_mem)
-			goto check_lu_gp;
-
-		spin_lock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
-		tg_pt_gp = tg_pt_gp_mem->tg_pt_gp;
+		spin_lock(&lun->lun_tg_pt_gp_lock);
+		tg_pt_gp = lun->lun_tg_pt_gp;
 		if (!tg_pt_gp) {
-			spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+			spin_unlock(&lun->lun_tg_pt_gp_lock);
 			goto check_lu_gp;
 		}
 		tg_pt_gp_id = tg_pt_gp->tg_pt_gp_id;
-		spin_unlock(&tg_pt_gp_mem->tg_pt_gp_mem_lock);
+		spin_unlock(&lun->lun_tg_pt_gp_lock);
 
-		buf[off] =
-			(tpg->se_tpg_tfo->get_fabric_proto_ident(tpg) << 4);
+		buf[off] = tpg->proto_id << 4;
 		buf[off++] |= 0x1; /* CODE SET == Binary */
 		buf[off] = 0x80; /* Set PIV=1 */
 		/* Set ASSOCIATION == target port: 01b */
@@ -371,8 +354,7 @@ check_lu_gp:
 		 * section 7.5.1 Table 362
 		 */
 check_scsi_name:
-		buf[off] =
-			(tpg->se_tpg_tfo->get_fabric_proto_ident(tpg) << 4);
+		buf[off] = tpg->proto_id << 4;
 		buf[off++] |= 0x3; /* CODE SET == UTF-8 */
 		buf[off] = 0x80; /* Set PIV=1 */
 		/* Set ASSOCIATION == target port: 01b */
@@ -412,8 +394,7 @@ check_scsi_name:
 		/*
 		 * Target device designator
 		 */
-		buf[off] =
-			(tpg->se_tpg_tfo->get_fabric_proto_ident(tpg) << 4);
+		buf[off] = tpg->proto_id << 4;
 		buf[off++] |= 0x3; /* CODE SET == UTF-8 */
 		buf[off] = 0x80; /* Set PIV=1 */
 		/* Set ASSOCIATION == target device: 10b */
@@ -481,7 +462,7 @@ spc_emulate_evpd_86(struct se_cmd *cmd, unsigned char *buf)
 	buf[5] = 0x07;
 
 	/* If WriteCache emulation is enabled, set V_SUP */
-	if (se_dev_check_wce(dev))
+	if (target_check_wce(dev))
 		buf[6] = 0x01;
 	/* If an LBA map is present set R_SUP */
 	spin_lock(&cmd->se_dev->t10_alua.lba_map_lock);
@@ -698,7 +679,7 @@ static sense_reason_t
 spc_emulate_inquiry(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
-	struct se_portal_group *tpg = cmd->se_lun->lun_sep->sep_tpg;
+	struct se_portal_group *tpg = cmd->se_lun->lun_tpg;
 	unsigned char *rbuf;
 	unsigned char *cdb = cmd->t_task_cdb;
 	unsigned char *buf;
@@ -712,7 +693,7 @@ spc_emulate_inquiry(struct se_cmd *cmd)
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 	}
 
-	if (dev == tpg->tpg_virt_lun0.lun_se_dev)
+	if (dev == rcu_access_pointer(tpg->tpg_virt_lun0->lun_se_dev))
 		buf[0] = 0x3f; /* Not connected */
 	else
 		buf[0] = dev->transport->get_device_type(dev);
@@ -888,7 +869,7 @@ static int spc_modesense_caching(struct se_cmd *cmd, u8 pc, u8 *p)
 	if (pc == 1)
 		goto out;
 
-	if (se_dev_check_wce(dev))
+	if (target_check_wce(dev))
 		p[2] = 0x04; /* Write Cache Enable */
 	p[12] = 0x20; /* Disabled Read Ahead */
 
@@ -985,6 +966,7 @@ static sense_reason_t spc_emulate_modesense(struct se_cmd *cmd)
 	int length = 0;
 	int ret;
 	int i;
+	bool read_only = target_lun_is_rdonly(cmd);;
 
 	memset(buf, 0, SE_MODE_PAGE_BUF);
 
@@ -995,13 +977,15 @@ static sense_reason_t spc_emulate_modesense(struct se_cmd *cmd)
 	length = ten ? 3 : 2;
 
 	/* DEVICE-SPECIFIC PARAMETER */
-	if ((cmd->se_lun->lun_access & TRANSPORT_LUNFLAGS_READ_ONLY) ||
-	    (cmd->se_deve &&
-	     (cmd->se_deve->lun_flags & TRANSPORT_LUNFLAGS_READ_ONLY)))
+	if ((cmd->se_lun->lun_access & TRANSPORT_LUNFLAGS_READ_ONLY) || read_only)
 		spc_modesense_write_protect(&buf[length], type);
 
-	if ((se_dev_check_wce(dev)) &&
-	    (dev->dev_attrib.emulate_fua_write > 0))
+	/*
+	 * SBC only allows us to enable FUA and DPO together.  Fortunately
+	 * DPO is explicitly specified as a hint, so a noop is a perfectly
+	 * valid implementation.
+	 */
+	if (target_check_fua(dev))
 		spc_modesense_dpofua(&buf[length], type);
 
 	++length;
@@ -1211,8 +1195,9 @@ sense_reason_t spc_emulate_report_luns(struct se_cmd *cmd)
 {
 	struct se_dev_entry *deve;
 	struct se_session *sess = cmd->se_sess;
+	struct se_node_acl *nacl;
 	unsigned char *buf;
-	u32 lun_count = 0, offset = 8, i;
+	u32 lun_count = 0, offset = 8;
 
 	if (cmd->data_length < 16) {
 		pr_warn("REPORT LUNS allocation length %u too small\n",
@@ -1234,12 +1219,10 @@ sense_reason_t spc_emulate_report_luns(struct se_cmd *cmd)
 		lun_count = 1;
 		goto done;
 	}
+	nacl = sess->se_node_acl;
 
-	spin_lock_irq(&sess->se_node_acl->device_list_lock);
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		deve = sess->se_node_acl->device_list[i];
-		if (!(deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS))
-			continue;
+	rcu_read_lock();
+	hlist_for_each_entry_rcu(deve, &nacl->lun_entry_hlist, link) {
 		/*
 		 * We determine the correct LUN LIST LENGTH even once we
 		 * have reached the initial allocation length.
@@ -1252,7 +1235,7 @@ sense_reason_t spc_emulate_report_luns(struct se_cmd *cmd)
 		int_to_scsilun(deve->mapped_lun, (struct scsi_lun *)&buf[offset]);
 		offset += 8;
 	}
-	spin_unlock_irq(&sess->se_node_acl->device_list_lock);
+	rcu_read_unlock();
 
 	/*
 	 * See SPC3 r07, page 159.
diff --git a/drivers/target/target_core_stat.c b/drivers/target/target_core_stat.c
index 03538994d2f7..20ed5d2e151a 100644
--- a/drivers/target/target_core_stat.c
+++ b/drivers/target/target_core_stat.c
@@ -33,14 +33,10 @@
 #include <linux/proc_fs.h>
 #include <linux/seq_file.h>
 #include <linux/configfs.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_host.h>
 
 #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/configfs_macros.h>
 
 #include "target_core_internal.h"
@@ -107,7 +103,7 @@ static ssize_t target_stat_scsi_dev_show_attr_ports(
 	struct se_device *dev =
 		container_of(sgrps, struct se_device, dev_stat_grps);
 
-	return snprintf(page, PAGE_SIZE, "%u\n", dev->dev_port_count);
+	return snprintf(page, PAGE_SIZE, "%u\n", dev->export_count);
 }
 DEV_STAT_SCSI_DEV_ATTR_RO(ports);
 
@@ -543,20 +539,14 @@ static ssize_t target_stat_scsi_port_show_attr_inst(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	struct se_device *dev = lun->lun_se_dev;
-	struct se_hba *hba;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	hba = dev->se_hba;
-	ret = snprintf(page, PAGE_SIZE, "%u\n", hba->hba_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n", dev->hba_index);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_PORT_ATTR_RO(inst);
@@ -565,18 +555,14 @@ static ssize_t target_stat_scsi_port_show_attr_dev(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	struct se_device *dev = lun->lun_se_dev;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	ret = snprintf(page, PAGE_SIZE, "%u\n", dev->dev_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n", dev->dev_index);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_PORT_ATTR_RO(dev);
@@ -585,17 +571,14 @@ static ssize_t target_stat_scsi_port_show_attr_indx(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	ret = snprintf(page, PAGE_SIZE, "%u\n", sep->sep_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n", lun->lun_rtpi);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_PORT_ATTR_RO(indx);
@@ -604,21 +587,14 @@ static ssize_t target_stat_scsi_port_show_attr_role(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_device *dev = lun->lun_se_dev;
-	struct se_port *sep;
-	ssize_t ret;
-
-	if (!dev)
-		return -ENODEV;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	ret = snprintf(page, PAGE_SIZE, "%s%u\n", "Device", dev->dev_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%s%u\n", "Device", dev->dev_index);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_PORT_ATTR_RO(role);
@@ -627,18 +603,16 @@ static ssize_t target_stat_scsi_port_show_attr_busy_count(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev) {
+		/* FIXME: scsiPortBusyStatuses  */
+		ret = snprintf(page, PAGE_SIZE, "%u\n", 0);
 	}
-	/* FIXME: scsiPortBusyStatuses  */
-	ret = snprintf(page, PAGE_SIZE, "%u\n", 0);
-	spin_unlock(&lun->lun_sep_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_PORT_ATTR_RO(busy_count);
@@ -686,20 +660,14 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_inst(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_device *dev = lun->lun_se_dev;
-	struct se_port *sep;
-	struct se_hba *hba;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	hba = dev->se_hba;
-	ret = snprintf(page, PAGE_SIZE, "%u\n", hba->hba_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n", dev->hba_index);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(inst);
@@ -708,18 +676,14 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_dev(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_device *dev = lun->lun_se_dev;
-	struct se_port *sep;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	ret = snprintf(page, PAGE_SIZE, "%u\n", dev->dev_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n", dev->dev_index);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(dev);
@@ -728,17 +692,14 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_indx(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	ret = snprintf(page, PAGE_SIZE, "%u\n", sep->sep_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n", lun->lun_rtpi);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(indx);
@@ -747,21 +708,17 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_name(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	struct se_portal_group *tpg;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	tpg = sep->sep_tpg;
-
-	ret = snprintf(page, PAGE_SIZE, "%sPort#%u\n",
-		tpg->se_tpg_tfo->get_fabric_name(), sep->sep_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_portal_group *tpg = lun->lun_tpg;
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%sPort#%u\n",
+			tpg->se_tpg_tfo->get_fabric_name(),
+			lun->lun_rtpi);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(name);
@@ -770,22 +727,17 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_port_index(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	struct se_portal_group *tpg;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	tpg = sep->sep_tpg;
-
-	ret = snprintf(page, PAGE_SIZE, "%s%s%d\n",
-		tpg->se_tpg_tfo->tpg_get_wwn(tpg), "+t+",
-		tpg->se_tpg_tfo->tpg_get_tag(tpg));
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_portal_group *tpg = lun->lun_tpg;
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%s%s%d\n",
+			tpg->se_tpg_tfo->tpg_get_wwn(tpg), "+t+",
+			tpg->se_tpg_tfo->tpg_get_tag(tpg));
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(port_index);
@@ -794,18 +746,15 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_in_cmds(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-
-	ret = snprintf(page, PAGE_SIZE, "%llu\n", sep->sep_stats.cmd_pdus);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%lu\n",
+			       atomic_long_read(&lun->lun_stats.cmd_pdus));
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(in_cmds);
@@ -814,19 +763,15 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_write_mbytes(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-
-	ret = snprintf(page, PAGE_SIZE, "%u\n",
-			(u32)(sep->sep_stats.rx_data_octets >> 20));
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n",
+			(u32)(atomic_long_read(&lun->lun_stats.rx_data_octets) >> 20));
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(write_mbytes);
@@ -835,19 +780,15 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_read_mbytes(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-
-	ret = snprintf(page, PAGE_SIZE, "%u\n",
-			(u32)(sep->sep_stats.tx_data_octets >> 20));
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n",
+				(u32)(atomic_long_read(&lun->lun_stats.tx_data_octets) >> 20));
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(read_mbytes);
@@ -856,19 +797,16 @@ static ssize_t target_stat_scsi_tgt_port_show_attr_hs_in_cmds(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev) {
+		/* FIXME: scsiTgtPortHsInCommands */
+		ret = snprintf(page, PAGE_SIZE, "%u\n", 0);
 	}
-
-	/* FIXME: scsiTgtPortHsInCommands */
-	ret = snprintf(page, PAGE_SIZE, "%u\n", 0);
-	spin_unlock(&lun->lun_sep_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TGT_PORT_ATTR_RO(hs_in_cmds);
@@ -922,21 +860,14 @@ static ssize_t target_stat_scsi_transport_show_attr_inst(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_device *dev = lun->lun_se_dev;
-	struct se_port *sep;
-	struct se_hba *hba;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-
-	hba = dev->se_hba;
-	ret = snprintf(page, PAGE_SIZE, "%u\n", hba->hba_index);
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n", dev->hba_index);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TRANSPORT_ATTR_RO(inst);
@@ -945,21 +876,18 @@ static ssize_t target_stat_scsi_transport_show_attr_device(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	struct se_portal_group *tpg;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
+	struct se_device *dev;
+	struct se_portal_group *tpg = lun->lun_tpg;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev) {
+		/* scsiTransportType */
+		ret = snprintf(page, PAGE_SIZE, "scsiTransport%s\n",
+			       tpg->se_tpg_tfo->get_fabric_name());
 	}
-	tpg = sep->sep_tpg;
-	/* scsiTransportType */
-	ret = snprintf(page, PAGE_SIZE, "scsiTransport%s\n",
-			tpg->se_tpg_tfo->get_fabric_name());
-	spin_unlock(&lun->lun_sep_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TRANSPORT_ATTR_RO(device);
@@ -968,20 +896,16 @@ static ssize_t target_stat_scsi_transport_show_attr_indx(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_port *sep;
-	struct se_portal_group *tpg;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
-	}
-	tpg = sep->sep_tpg;
-	ret = snprintf(page, PAGE_SIZE, "%u\n",
-			tpg->se_tpg_tfo->tpg_get_inst_index(tpg));
-	spin_unlock(&lun->lun_sep_lock);
+	struct se_device *dev;
+	struct se_portal_group *tpg = lun->lun_tpg;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev)
+		ret = snprintf(page, PAGE_SIZE, "%u\n",
+			       tpg->se_tpg_tfo->tpg_get_inst_index(tpg));
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TRANSPORT_ATTR_RO(indx);
@@ -990,26 +914,22 @@ static ssize_t target_stat_scsi_transport_show_attr_dev_name(
 	struct se_port_stat_grps *pgrps, char *page)
 {
 	struct se_lun *lun = container_of(pgrps, struct se_lun, port_stat_grps);
-	struct se_device *dev = lun->lun_se_dev;
-	struct se_port *sep;
-	struct se_portal_group *tpg;
+	struct se_device *dev;
+	struct se_portal_group *tpg = lun->lun_tpg;
 	struct t10_wwn *wwn;
-	ssize_t ret;
-
-	spin_lock(&lun->lun_sep_lock);
-	sep = lun->lun_sep;
-	if (!sep) {
-		spin_unlock(&lun->lun_sep_lock);
-		return -ENODEV;
+	ssize_t ret = -ENODEV;
+
+	rcu_read_lock();
+	dev = rcu_dereference(lun->lun_se_dev);
+	if (dev) {
+		wwn = &dev->t10_wwn;
+		/* scsiTransportDevName */
+		ret = snprintf(page, PAGE_SIZE, "%s+%s\n",
+				tpg->se_tpg_tfo->tpg_get_wwn(tpg),
+				(strlen(wwn->unit_serial)) ? wwn->unit_serial :
+				wwn->vendor);
 	}
-	tpg = sep->sep_tpg;
-	wwn = &dev->t10_wwn;
-	/* scsiTransportDevName */
-	ret = snprintf(page, PAGE_SIZE, "%s+%s\n",
-			tpg->se_tpg_tfo->tpg_get_wwn(tpg),
-			(strlen(wwn->unit_serial)) ? wwn->unit_serial :
-			wwn->vendor);
-	spin_unlock(&lun->lun_sep_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_TRANSPORT_ATTR_RO(dev_name);
@@ -1085,17 +1005,17 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_inst(
 	struct se_portal_group *tpg;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	tpg = nacl->se_tpg;
 	/* scsiInstIndex */
 	ret = snprintf(page, PAGE_SIZE, "%u\n",
 			tpg->se_tpg_tfo->tpg_get_inst_index(tpg));
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(inst);
@@ -1110,16 +1030,16 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_dev(
 	struct se_lun *lun;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
-	lun = deve->se_lun;
+	lun = rcu_dereference(deve->se_lun);
 	/* scsiDeviceIndex */
-	ret = snprintf(page, PAGE_SIZE, "%u\n", lun->lun_se_dev->dev_index);
-	spin_unlock_irq(&nacl->device_list_lock);
+	ret = snprintf(page, PAGE_SIZE, "%u\n", lun->lun_index);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(dev);
@@ -1134,16 +1054,16 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_port(
 	struct se_portal_group *tpg;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	tpg = nacl->se_tpg;
 	/* scsiAuthIntrTgtPortIndex */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", tpg->se_tpg_tfo->tpg_get_tag(tpg));
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(port);
@@ -1157,15 +1077,15 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_indx(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAuthIntrIndex */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", nacl->acl_index);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(indx);
@@ -1179,15 +1099,15 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_dev_or_port(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAuthIntrDevOrPort */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", 1);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(dev_or_port);
@@ -1201,15 +1121,15 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_intr_name(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAuthIntrName */
 	ret = snprintf(page, PAGE_SIZE, "%s\n", nacl->initiatorname);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(intr_name);
@@ -1223,15 +1143,15 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_map_indx(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* FIXME: scsiAuthIntrLunMapIndex */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", 0);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(map_indx);
@@ -1245,15 +1165,15 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_att_count(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAuthIntrAttachedTimes */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", deve->attach_count);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(att_count);
@@ -1267,15 +1187,16 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_num_cmds(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAuthIntrOutCommands */
-	ret = snprintf(page, PAGE_SIZE, "%u\n", deve->total_cmds);
-	spin_unlock_irq(&nacl->device_list_lock);
+	ret = snprintf(page, PAGE_SIZE, "%lu\n",
+		       atomic_long_read(&deve->total_cmds));
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(num_cmds);
@@ -1289,15 +1210,16 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_read_mbytes(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAuthIntrReadMegaBytes */
-	ret = snprintf(page, PAGE_SIZE, "%u\n", (u32)(deve->read_bytes >> 20));
-	spin_unlock_irq(&nacl->device_list_lock);
+	ret = snprintf(page, PAGE_SIZE, "%u\n",
+		      (u32)(atomic_long_read(&deve->read_bytes) >> 20));
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(read_mbytes);
@@ -1311,15 +1233,16 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_write_mbytes(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAuthIntrWrittenMegaBytes */
-	ret = snprintf(page, PAGE_SIZE, "%u\n", (u32)(deve->write_bytes >> 20));
-	spin_unlock_irq(&nacl->device_list_lock);
+	ret = snprintf(page, PAGE_SIZE, "%u\n",
+		      (u32)(atomic_long_read(&deve->write_bytes) >> 20));
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(write_mbytes);
@@ -1333,15 +1256,15 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_hs_num_cmds(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* FIXME: scsiAuthIntrHSOutCommands */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", 0);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(hs_num_cmds);
@@ -1355,16 +1278,16 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_creation_time(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAuthIntrLastCreation */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", (u32)(((u32)deve->creation_time -
 				INITIAL_JIFFIES) * 100 / HZ));
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(creation_time);
@@ -1378,15 +1301,15 @@ static ssize_t target_stat_scsi_auth_intr_show_attr_row_status(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* FIXME: scsiAuthIntrRowStatus */
 	ret = snprintf(page, PAGE_SIZE, "Ready\n");
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_AUTH_INTR_ATTR_RO(row_status);
@@ -1451,17 +1374,17 @@ static ssize_t target_stat_scsi_att_intr_port_show_attr_inst(
 	struct se_portal_group *tpg;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	tpg = nacl->se_tpg;
 	/* scsiInstIndex */
 	ret = snprintf(page, PAGE_SIZE, "%u\n",
 			tpg->se_tpg_tfo->tpg_get_inst_index(tpg));
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_ATTR_INTR_PORT_ATTR_RO(inst);
@@ -1476,16 +1399,16 @@ static ssize_t target_stat_scsi_att_intr_port_show_attr_dev(
 	struct se_lun *lun;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
-	lun = deve->se_lun;
+	lun = rcu_dereference(deve->se_lun);
 	/* scsiDeviceIndex */
-	ret = snprintf(page, PAGE_SIZE, "%u\n", lun->lun_se_dev->dev_index);
-	spin_unlock_irq(&nacl->device_list_lock);
+	ret = snprintf(page, PAGE_SIZE, "%u\n", lun->lun_index);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_ATTR_INTR_PORT_ATTR_RO(dev);
@@ -1500,16 +1423,16 @@ static ssize_t target_stat_scsi_att_intr_port_show_attr_port(
 	struct se_portal_group *tpg;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	tpg = nacl->se_tpg;
 	/* scsiPortIndex */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", tpg->se_tpg_tfo->tpg_get_tag(tpg));
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_ATTR_INTR_PORT_ATTR_RO(port);
@@ -1549,15 +1472,15 @@ static ssize_t target_stat_scsi_att_intr_port_show_attr_port_auth_indx(
 	struct se_dev_entry *deve;
 	ssize_t ret;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[lacl->mapped_lun];
-	if (!deve->se_lun || !deve->se_lun_acl) {
-		spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, lacl->mapped_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return -ENODEV;
 	}
 	/* scsiAttIntrPortAuthIntrIdx */
 	ret = snprintf(page, PAGE_SIZE, "%u\n", nacl->acl_index);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 	return ret;
 }
 DEV_STAT_SCSI_ATTR_INTR_PORT_ATTR_RO(port_auth_indx);
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index 315ec3458eeb..5b2820312310 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -27,13 +27,10 @@
 #include <linux/spinlock.h>
 #include <linux/list.h>
 #include <linux/export.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
 
 #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"
 #include "target_core_alua.h"
@@ -117,7 +114,7 @@ void core_tmr_abort_task(
 {
 	struct se_cmd *se_cmd;
 	unsigned long flags;
-	int ref_tag;
+	u64 ref_tag;
 
 	spin_lock_irqsave(&se_sess->sess_cmd_lock, flags);
 	list_for_each_entry(se_cmd, &se_sess->sess_cmd_list, se_cmd_list) {
@@ -129,16 +126,17 @@ void core_tmr_abort_task(
 		if (se_cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)
 			continue;
 
-		ref_tag = se_cmd->se_tfo->get_task_tag(se_cmd);
+		ref_tag = se_cmd->tag;
 		if (tmr->ref_task_tag != ref_tag)
 			continue;
 
-		printk("ABORT_TASK: Found referenced %s task_tag: %u\n",
+		printk("ABORT_TASK: Found referenced %s task_tag: %llu\n",
 			se_cmd->se_tfo->get_fabric_name(), ref_tag);
 
 		spin_lock(&se_cmd->t_state_lock);
 		if (se_cmd->transport_state & CMD_T_COMPLETE) {
-			printk("ABORT_TASK: ref_tag: %u already complete, skipping\n", ref_tag);
+			printk("ABORT_TASK: ref_tag: %llu already complete,"
+			       " skipping\n", ref_tag);
 			spin_unlock(&se_cmd->t_state_lock);
 			spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 			goto out;
@@ -153,18 +151,18 @@ void core_tmr_abort_task(
 		cancel_work_sync(&se_cmd->work);
 		transport_wait_for_tasks(se_cmd);
 
-		target_put_sess_cmd(se_sess, se_cmd);
+		target_put_sess_cmd(se_cmd);
 		transport_cmd_finish_abort(se_cmd, true);
 
 		printk("ABORT_TASK: Sending TMR_FUNCTION_COMPLETE for"
-				" ref_tag: %d\n", ref_tag);
+				" ref_tag: %llu\n", ref_tag);
 		tmr->response = TMR_FUNCTION_COMPLETE;
 		return;
 	}
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 
 out:
-	printk("ABORT_TASK: Sending TMR_TASK_DOES_NOT_EXIST for ref_tag: %d\n",
+	printk("ABORT_TASK: Sending TMR_TASK_DOES_NOT_EXIST for ref_tag: %lld\n",
 			tmr->ref_task_tag);
 	tmr->response = TMR_TASK_DOES_NOT_EXIST;
 }
@@ -289,16 +287,16 @@ static void core_tmr_drain_state_list(
 		list_del(&cmd->state_list);
 
 		pr_debug("LUN_RESET: %s cmd: %p"
-			" ITT/CmdSN: 0x%08x/0x%08x, i_state: %d, t_state: %d"
+			" ITT/CmdSN: 0x%08llx/0x%08x, i_state: %d, t_state: %d"
 			"cdb: 0x%02x\n",
 			(preempt_and_abort_list) ? "Preempt" : "", cmd,
-			cmd->se_tfo->get_task_tag(cmd), 0,
+			cmd->tag, 0,
 			cmd->se_tfo->get_cmd_state(cmd), cmd->t_state,
 			cmd->t_task_cdb[0]);
-		pr_debug("LUN_RESET: ITT[0x%08x] - pr_res_key: 0x%016Lx"
+		pr_debug("LUN_RESET: ITT[0x%08llx] - pr_res_key: 0x%016Lx"
 			" -- CMD_T_ACTIVE: %d"
 			" CMD_T_STOP: %d CMD_T_SENT: %d\n",
-			cmd->se_tfo->get_task_tag(cmd), cmd->pr_res_key,
+			cmd->tag, cmd->pr_res_key,
 			(cmd->transport_state & CMD_T_ACTIVE) != 0,
 			(cmd->transport_state & CMD_T_STOP) != 0,
 			(cmd->transport_state & CMD_T_SENT) != 0);
diff --git a/drivers/target/target_core_tpg.c b/drivers/target/target_core_tpg.c
index 47f064415bf6..babde4ad841f 100644
--- a/drivers/target/target_core_tpg.c
+++ b/drivers/target/target_core_tpg.c
@@ -32,14 +32,14 @@
 #include <linux/export.h>
 #include <net/sock.h>
 #include <net/tcp.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_proto.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_backend.h>
 #include <target/target_core_fabric.h>
 
 #include "target_core_internal.h"
+#include "target_core_alua.h"
 #include "target_core_pr.h"
 
 extern struct se_device *g_lun0_dev;
@@ -47,45 +47,9 @@ extern struct se_device *g_lun0_dev;
 static DEFINE_SPINLOCK(tpg_lock);
 static LIST_HEAD(tpg_list);
 
-/*	core_clear_initiator_node_from_tpg():
- *
- *
- */
-static void core_clear_initiator_node_from_tpg(
-	struct se_node_acl *nacl,
-	struct se_portal_group *tpg)
-{
-	int i;
-	struct se_dev_entry *deve;
-	struct se_lun *lun;
-
-	spin_lock_irq(&nacl->device_list_lock);
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		deve = nacl->device_list[i];
-
-		if (!(deve->lun_flags & TRANSPORT_LUNFLAGS_INITIATOR_ACCESS))
-			continue;
-
-		if (!deve->se_lun) {
-			pr_err("%s device entries device pointer is"
-				" NULL, but Initiator has access.\n",
-				tpg->se_tpg_tfo->get_fabric_name());
-			continue;
-		}
-
-		lun = deve->se_lun;
-		spin_unlock_irq(&nacl->device_list_lock);
-		core_disable_device_list_for_node(lun, NULL, deve->mapped_lun,
-			TRANSPORT_LUNFLAGS_NO_ACCESS, nacl, tpg);
-
-		spin_lock_irq(&nacl->device_list_lock);
-	}
-	spin_unlock_irq(&nacl->device_list_lock);
-}
-
 /*	__core_tpg_get_initiator_node_acl():
  *
- *	spin_lock_bh(&tpg->acl_node_lock); must be held when calling
+ *	mutex_lock(&tpg->acl_node_mutex); must be held when calling
  */
 struct se_node_acl *__core_tpg_get_initiator_node_acl(
 	struct se_portal_group *tpg,
@@ -111,9 +75,9 @@ struct se_node_acl *core_tpg_get_initiator_node_acl(
 {
 	struct se_node_acl *acl;
 
-	spin_lock_irq(&tpg->acl_node_lock);
+	mutex_lock(&tpg->acl_node_mutex);
 	acl = __core_tpg_get_initiator_node_acl(tpg, initiatorname);
-	spin_unlock_irq(&tpg->acl_node_lock);
+	mutex_unlock(&tpg->acl_node_mutex);
 
 	return acl;
 }
@@ -125,22 +89,20 @@ EXPORT_SYMBOL(core_tpg_get_initiator_node_acl);
  */
 void core_tpg_add_node_to_devs(
 	struct se_node_acl *acl,
-	struct se_portal_group *tpg)
+	struct se_portal_group *tpg,
+	struct se_lun *lun_orig)
 {
-	int i = 0;
 	u32 lun_access = 0;
 	struct se_lun *lun;
 	struct se_device *dev;
 
-	spin_lock(&tpg->tpg_lun_lock);
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		lun = tpg->tpg_lun_list[i];
-		if (lun->lun_status != TRANSPORT_LUN_STATUS_ACTIVE)
+	mutex_lock(&tpg->tpg_lun_mutex);
+	hlist_for_each_entry_rcu(lun, &tpg->tpg_lun_hlist, link) {
+		if (lun_orig && lun != lun_orig)
 			continue;
 
-		spin_unlock(&tpg->tpg_lun_lock);
-
-		dev = lun->lun_se_dev;
+		dev = rcu_dereference_check(lun->lun_se_dev,
+					    lockdep_is_held(&tpg->tpg_lun_mutex));
 		/*
 		 * By default in LIO-Target $FABRIC_MOD,
 		 * demo_mode_write_protect is ON, or READ_ONLY;
@@ -158,7 +120,7 @@ void core_tpg_add_node_to_devs(
 				lun_access = TRANSPORT_LUNFLAGS_READ_WRITE;
 		}
 
-		pr_debug("TARGET_CORE[%s]->TPG[%u]_LUN[%u] - Adding %s"
+		pr_debug("TARGET_CORE[%s]->TPG[%u]_LUN[%llu] - Adding %s"
 			" access for LUN in Demo Mode\n",
 			tpg->se_tpg_tfo->get_fabric_name(),
 			tpg->se_tpg_tfo->tpg_get_tag(tpg), lun->unpacked_lun,
@@ -166,7 +128,7 @@ void core_tpg_add_node_to_devs(
 			"READ-WRITE" : "READ-ONLY");
 
 		core_enable_device_list_for_node(lun, NULL, lun->unpacked_lun,
-				lun_access, acl, tpg);
+						 lun_access, acl, tpg);
 		/*
 		 * Check to see if there are any existing persistent reservation
 		 * APTPL pre-registrations that need to be enabled for this dynamic
@@ -174,9 +136,8 @@ void core_tpg_add_node_to_devs(
 		 */
 		core_scsi3_check_aptpl_registration(dev, tpg, lun, acl,
 						    lun->unpacked_lun);
-		spin_lock(&tpg->tpg_lun_lock);
 	}
-	spin_unlock(&tpg->tpg_lun_lock);
+	mutex_unlock(&tpg->tpg_lun_mutex);
 }
 
 /*      core_set_queue_depth_for_node():
@@ -197,67 +158,63 @@ static int core_set_queue_depth_for_node(
 	return 0;
 }
 
-void array_free(void *array, int n)
+static struct se_node_acl *target_alloc_node_acl(struct se_portal_group *tpg,
+		const unsigned char *initiatorname)
 {
-	void **a = array;
-	int i;
+	struct se_node_acl *acl;
 
-	for (i = 0; i < n; i++)
-		kfree(a[i]);
-	kfree(a);
-}
+	acl = kzalloc(max(sizeof(*acl), tpg->se_tpg_tfo->node_acl_size),
+			GFP_KERNEL);
+	if (!acl)
+		return NULL;
 
-static void *array_zalloc(int n, size_t size, gfp_t flags)
-{
-	void **a;
-	int i;
+	INIT_LIST_HEAD(&acl->acl_list);
+	INIT_LIST_HEAD(&acl->acl_sess_list);
+	INIT_HLIST_HEAD(&acl->lun_entry_hlist);
+	kref_init(&acl->acl_kref);
+	init_completion(&acl->acl_free_comp);
+	spin_lock_init(&acl->nacl_sess_lock);
+	mutex_init(&acl->lun_entry_mutex);
+	atomic_set(&acl->acl_pr_ref_count, 0);
+	if (tpg->se_tpg_tfo->tpg_get_default_depth)
+		acl->queue_depth = tpg->se_tpg_tfo->tpg_get_default_depth(tpg);
+	else
+		acl->queue_depth = 1;
+	snprintf(acl->initiatorname, TRANSPORT_IQN_LEN, "%s", initiatorname);
+	acl->se_tpg = tpg;
+	acl->acl_index = scsi_get_new_index(SCSI_AUTH_INTR_INDEX);
 
-	a = kzalloc(n * sizeof(void*), flags);
-	if (!a)
-		return NULL;
-	for (i = 0; i < n; i++) {
-		a[i] = kzalloc(size, flags);
-		if (!a[i]) {
-			array_free(a, n);
-			return NULL;
-		}
-	}
-	return a;
+	tpg->se_tpg_tfo->set_default_node_attributes(acl);
+
+	if (core_set_queue_depth_for_node(tpg, acl) < 0)
+		goto out_free_acl;
+
+	return acl;
+
+out_free_acl:
+	kfree(acl);
+	return NULL;
 }
 
-/*      core_create_device_list_for_node():
- *
- *
- */
-static int core_create_device_list_for_node(struct se_node_acl *nacl)
+static void target_add_node_acl(struct se_node_acl *acl)
 {
-	struct se_dev_entry *deve;
-	int i;
-
-	nacl->device_list = array_zalloc(TRANSPORT_MAX_LUNS_PER_TPG,
-			sizeof(struct se_dev_entry), GFP_KERNEL);
-	if (!nacl->device_list) {
-		pr_err("Unable to allocate memory for"
-			" struct se_node_acl->device_list\n");
-		return -ENOMEM;
-	}
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		deve = nacl->device_list[i];
-
-		atomic_set(&deve->ua_count, 0);
-		atomic_set(&deve->pr_ref_count, 0);
-		spin_lock_init(&deve->ua_lock);
-		INIT_LIST_HEAD(&deve->alua_port_list);
-		INIT_LIST_HEAD(&deve->ua_list);
-	}
+	struct se_portal_group *tpg = acl->se_tpg;
 
-	return 0;
+	mutex_lock(&tpg->acl_node_mutex);
+	list_add_tail(&acl->acl_list, &tpg->acl_node_list);
+	tpg->num_node_acls++;
+	mutex_unlock(&tpg->acl_node_mutex);
+
+	pr_debug("%s_TPG[%hu] - Added %s ACL with TCQ Depth: %d for %s"
+		" Initiator Node: %s\n",
+		tpg->se_tpg_tfo->get_fabric_name(),
+		tpg->se_tpg_tfo->tpg_get_tag(tpg),
+		acl->dynamic_node_acl ? "DYNAMIC" : "",
+		acl->queue_depth,
+		tpg->se_tpg_tfo->get_fabric_name(),
+		acl->initiatorname);
 }
 
-/*	core_tpg_check_initiator_node_acl()
- *
- *
- */
 struct se_node_acl *core_tpg_check_initiator_node_acl(
 	struct se_portal_group *tpg,
 	unsigned char *initiatorname)
@@ -271,35 +228,11 @@ struct se_node_acl *core_tpg_check_initiator_node_acl(
 	if (!tpg->se_tpg_tfo->tpg_check_demo_mode(tpg))
 		return NULL;
 
-	acl =  tpg->se_tpg_tfo->tpg_alloc_fabric_acl(tpg);
+	acl = target_alloc_node_acl(tpg, initiatorname);
 	if (!acl)
 		return NULL;
-
-	INIT_LIST_HEAD(&acl->acl_list);
-	INIT_LIST_HEAD(&acl->acl_sess_list);
-	kref_init(&acl->acl_kref);
-	init_completion(&acl->acl_free_comp);
-	spin_lock_init(&acl->device_list_lock);
-	spin_lock_init(&acl->nacl_sess_lock);
-	atomic_set(&acl->acl_pr_ref_count, 0);
-	acl->queue_depth = tpg->se_tpg_tfo->tpg_get_default_depth(tpg);
-	snprintf(acl->initiatorname, TRANSPORT_IQN_LEN, "%s", initiatorname);
-	acl->se_tpg = tpg;
-	acl->acl_index = scsi_get_new_index(SCSI_AUTH_INTR_INDEX);
 	acl->dynamic_node_acl = 1;
 
-	tpg->se_tpg_tfo->set_default_node_attributes(acl);
-
-	if (core_create_device_list_for_node(acl) < 0) {
-		tpg->se_tpg_tfo->tpg_release_fabric_acl(tpg, acl);
-		return NULL;
-	}
-
-	if (core_set_queue_depth_for_node(tpg, acl) < 0) {
-		core_free_device_list_for_node(acl, tpg);
-		tpg->se_tpg_tfo->tpg_release_fabric_acl(tpg, acl);
-		return NULL;
-	}
 	/*
 	 * Here we only create demo-mode MappedLUNs from the active
 	 * TPG LUNs if the fabric is not explicitly asking for
@@ -307,18 +240,9 @@ struct se_node_acl *core_tpg_check_initiator_node_acl(
 	 */
 	if ((tpg->se_tpg_tfo->tpg_check_demo_mode_login_only == NULL) ||
 	    (tpg->se_tpg_tfo->tpg_check_demo_mode_login_only(tpg) != 1))
-		core_tpg_add_node_to_devs(acl, tpg);
-
-	spin_lock_irq(&tpg->acl_node_lock);
-	list_add_tail(&acl->acl_list, &tpg->acl_node_list);
-	tpg->num_node_acls++;
-	spin_unlock_irq(&tpg->acl_node_lock);
-
-	pr_debug("%s_TPG[%u] - Added DYNAMIC ACL with TCQ Depth: %d for %s"
-		" Initiator Node: %s\n", tpg->se_tpg_tfo->get_fabric_name(),
-		tpg->se_tpg_tfo->tpg_get_tag(tpg), acl->queue_depth,
-		tpg->se_tpg_tfo->get_fabric_name(), initiatorname);
+		core_tpg_add_node_to_devs(acl, tpg, NULL);
 
+	target_add_node_acl(acl);
 	return acl;
 }
 EXPORT_SYMBOL(core_tpg_check_initiator_node_acl);
@@ -329,40 +253,13 @@ void core_tpg_wait_for_nacl_pr_ref(struct se_node_acl *nacl)
 		cpu_relax();
 }
 
-void core_tpg_clear_object_luns(struct se_portal_group *tpg)
-{
-	int i;
-	struct se_lun *lun;
-
-	spin_lock(&tpg->tpg_lun_lock);
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		lun = tpg->tpg_lun_list[i];
-
-		if ((lun->lun_status != TRANSPORT_LUN_STATUS_ACTIVE) ||
-		    (lun->lun_se_dev == NULL))
-			continue;
-
-		spin_unlock(&tpg->tpg_lun_lock);
-		core_dev_del_lun(tpg, lun);
-		spin_lock(&tpg->tpg_lun_lock);
-	}
-	spin_unlock(&tpg->tpg_lun_lock);
-}
-EXPORT_SYMBOL(core_tpg_clear_object_luns);
-
-/*	core_tpg_add_initiator_node_acl():
- *
- *
- */
 struct se_node_acl *core_tpg_add_initiator_node_acl(
 	struct se_portal_group *tpg,
-	struct se_node_acl *se_nacl,
-	const char *initiatorname,
-	u32 queue_depth)
+	const char *initiatorname)
 {
-	struct se_node_acl *acl = NULL;
+	struct se_node_acl *acl;
 
-	spin_lock_irq(&tpg->acl_node_lock);
+	mutex_lock(&tpg->acl_node_mutex);
 	acl = __core_tpg_get_initiator_node_acl(tpg, initiatorname);
 	if (acl) {
 		if (acl->dynamic_node_acl) {
@@ -370,99 +267,42 @@ struct se_node_acl *core_tpg_add_initiator_node_acl(
 			pr_debug("%s_TPG[%u] - Replacing dynamic ACL"
 				" for %s\n", tpg->se_tpg_tfo->get_fabric_name(),
 				tpg->se_tpg_tfo->tpg_get_tag(tpg), initiatorname);
-			spin_unlock_irq(&tpg->acl_node_lock);
-			/*
-			 * Release the locally allocated struct se_node_acl
-			 * because * core_tpg_add_initiator_node_acl() returned
-			 * a pointer to an existing demo mode node ACL.
-			 */
-			if (se_nacl)
-				tpg->se_tpg_tfo->tpg_release_fabric_acl(tpg,
-							se_nacl);
-			goto done;
+			mutex_unlock(&tpg->acl_node_mutex);
+			return acl;
 		}
 
 		pr_err("ACL entry for %s Initiator"
 			" Node %s already exists for TPG %u, ignoring"
 			" request.\n",  tpg->se_tpg_tfo->get_fabric_name(),
 			initiatorname, tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		spin_unlock_irq(&tpg->acl_node_lock);
+		mutex_unlock(&tpg->acl_node_mutex);
 		return ERR_PTR(-EEXIST);
 	}
-	spin_unlock_irq(&tpg->acl_node_lock);
-
-	if (!se_nacl) {
-		pr_err("struct se_node_acl pointer is NULL\n");
-		return ERR_PTR(-EINVAL);
-	}
-	/*
-	 * For v4.x logic the se_node_acl_s is hanging off a fabric
-	 * dependent structure allocated via
-	 * struct target_core_fabric_ops->fabric_make_nodeacl()
-	 */
-	acl = se_nacl;
+	mutex_unlock(&tpg->acl_node_mutex);
 
-	INIT_LIST_HEAD(&acl->acl_list);
-	INIT_LIST_HEAD(&acl->acl_sess_list);
-	kref_init(&acl->acl_kref);
-	init_completion(&acl->acl_free_comp);
-	spin_lock_init(&acl->device_list_lock);
-	spin_lock_init(&acl->nacl_sess_lock);
-	atomic_set(&acl->acl_pr_ref_count, 0);
-	acl->queue_depth = queue_depth;
-	snprintf(acl->initiatorname, TRANSPORT_IQN_LEN, "%s", initiatorname);
-	acl->se_tpg = tpg;
-	acl->acl_index = scsi_get_new_index(SCSI_AUTH_INTR_INDEX);
-
-	tpg->se_tpg_tfo->set_default_node_attributes(acl);
-
-	if (core_create_device_list_for_node(acl) < 0) {
-		tpg->se_tpg_tfo->tpg_release_fabric_acl(tpg, acl);
+	acl = target_alloc_node_acl(tpg, initiatorname);
+	if (!acl)
 		return ERR_PTR(-ENOMEM);
-	}
-
-	if (core_set_queue_depth_for_node(tpg, acl) < 0) {
-		core_free_device_list_for_node(acl, tpg);
-		tpg->se_tpg_tfo->tpg_release_fabric_acl(tpg, acl);
-		return ERR_PTR(-EINVAL);
-	}
-
-	spin_lock_irq(&tpg->acl_node_lock);
-	list_add_tail(&acl->acl_list, &tpg->acl_node_list);
-	tpg->num_node_acls++;
-	spin_unlock_irq(&tpg->acl_node_lock);
-
-done:
-	pr_debug("%s_TPG[%hu] - Added ACL with TCQ Depth: %d for %s"
-		" Initiator Node: %s\n", tpg->se_tpg_tfo->get_fabric_name(),
-		tpg->se_tpg_tfo->tpg_get_tag(tpg), acl->queue_depth,
-		tpg->se_tpg_tfo->get_fabric_name(), initiatorname);
 
+	target_add_node_acl(acl);
 	return acl;
 }
-EXPORT_SYMBOL(core_tpg_add_initiator_node_acl);
 
-/*	core_tpg_del_initiator_node_acl():
- *
- *
- */
-int core_tpg_del_initiator_node_acl(
-	struct se_portal_group *tpg,
-	struct se_node_acl *acl,
-	int force)
+void core_tpg_del_initiator_node_acl(struct se_node_acl *acl)
 {
+	struct se_portal_group *tpg = acl->se_tpg;
 	LIST_HEAD(sess_list);
 	struct se_session *sess, *sess_tmp;
 	unsigned long flags;
 	int rc;
 
-	spin_lock_irq(&tpg->acl_node_lock);
+	mutex_lock(&tpg->acl_node_mutex);
 	if (acl->dynamic_node_acl) {
 		acl->dynamic_node_acl = 0;
 	}
 	list_del(&acl->acl_list);
 	tpg->num_node_acls--;
-	spin_unlock_irq(&tpg->acl_node_lock);
+	mutex_unlock(&tpg->acl_node_mutex);
 
 	spin_lock_irqsave(&acl->nacl_sess_lock, flags);
 	acl->acl_stop = 1;
@@ -494,7 +334,6 @@ int core_tpg_del_initiator_node_acl(
 	wait_for_completion(&acl->acl_free_comp);
 
 	core_tpg_wait_for_nacl_pr_ref(acl);
-	core_clear_initiator_node_from_tpg(acl, tpg);
 	core_free_device_list_for_node(acl, tpg);
 
 	pr_debug("%s_TPG[%hu] - Deleted ACL with TCQ Depth: %d for %s"
@@ -502,9 +341,8 @@ int core_tpg_del_initiator_node_acl(
 		tpg->se_tpg_tfo->tpg_get_tag(tpg), acl->queue_depth,
 		tpg->se_tpg_tfo->get_fabric_name(), acl->initiatorname);
 
-	return 0;
+	kfree(acl);
 }
-EXPORT_SYMBOL(core_tpg_del_initiator_node_acl);
 
 /*	core_tpg_set_initiator_node_queue_depth():
  *
@@ -521,21 +359,21 @@ int core_tpg_set_initiator_node_queue_depth(
 	unsigned long flags;
 	int dynamic_acl = 0;
 
-	spin_lock_irq(&tpg->acl_node_lock);
+	mutex_lock(&tpg->acl_node_mutex);
 	acl = __core_tpg_get_initiator_node_acl(tpg, initiatorname);
 	if (!acl) {
 		pr_err("Access Control List entry for %s Initiator"
 			" Node %s does not exists for TPG %hu, ignoring"
 			" request.\n", tpg->se_tpg_tfo->get_fabric_name(),
 			initiatorname, tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		spin_unlock_irq(&tpg->acl_node_lock);
+		mutex_unlock(&tpg->acl_node_mutex);
 		return -ENODEV;
 	}
 	if (acl->dynamic_node_acl) {
 		acl->dynamic_node_acl = 0;
 		dynamic_acl = 1;
 	}
-	spin_unlock_irq(&tpg->acl_node_lock);
+	mutex_unlock(&tpg->acl_node_mutex);
 
 	spin_lock_irqsave(&tpg->session_lock, flags);
 	list_for_each_entry(sess, &tpg->tpg_sess_list, sess_list) {
@@ -551,10 +389,10 @@ int core_tpg_set_initiator_node_queue_depth(
 				tpg->se_tpg_tfo->get_fabric_name(), initiatorname);
 			spin_unlock_irqrestore(&tpg->session_lock, flags);
 
-			spin_lock_irq(&tpg->acl_node_lock);
+			mutex_lock(&tpg->acl_node_mutex);
 			if (dynamic_acl)
 				acl->dynamic_node_acl = 1;
-			spin_unlock_irq(&tpg->acl_node_lock);
+			mutex_unlock(&tpg->acl_node_mutex);
 			return -EEXIST;
 		}
 		/*
@@ -589,10 +427,10 @@ int core_tpg_set_initiator_node_queue_depth(
 		if (init_sess)
 			tpg->se_tpg_tfo->close_session(init_sess);
 
-		spin_lock_irq(&tpg->acl_node_lock);
+		mutex_lock(&tpg->acl_node_mutex);
 		if (dynamic_acl)
 			acl->dynamic_node_acl = 1;
-		spin_unlock_irq(&tpg->acl_node_lock);
+		mutex_unlock(&tpg->acl_node_mutex);
 		return -EINVAL;
 	}
 	spin_unlock_irqrestore(&tpg->session_lock, flags);
@@ -608,10 +446,10 @@ int core_tpg_set_initiator_node_queue_depth(
 		initiatorname, tpg->se_tpg_tfo->get_fabric_name(),
 		tpg->se_tpg_tfo->tpg_get_tag(tpg));
 
-	spin_lock_irq(&tpg->acl_node_lock);
+	mutex_lock(&tpg->acl_node_mutex);
 	if (dynamic_acl)
 		acl->dynamic_node_acl = 1;
-	spin_unlock_irq(&tpg->acl_node_lock);
+	mutex_unlock(&tpg->acl_node_mutex);
 
 	return 0;
 }
@@ -647,78 +485,54 @@ static void core_tpg_lun_ref_release(struct percpu_ref *ref)
 	complete(&lun->lun_ref_comp);
 }
 
-static int core_tpg_setup_virtual_lun0(struct se_portal_group *se_tpg)
-{
-	/* Set in core_dev_setup_virtual_lun0() */
-	struct se_device *dev = g_lun0_dev;
-	struct se_lun *lun = &se_tpg->tpg_virt_lun0;
-	u32 lun_access = TRANSPORT_LUNFLAGS_READ_ONLY;
-	int ret;
-
-	lun->unpacked_lun = 0;
-	lun->lun_status = TRANSPORT_LUN_STATUS_FREE;
-	atomic_set(&lun->lun_acl_count, 0);
-	init_completion(&lun->lun_shutdown_comp);
-	INIT_LIST_HEAD(&lun->lun_acl_list);
-	spin_lock_init(&lun->lun_acl_lock);
-	spin_lock_init(&lun->lun_sep_lock);
-	init_completion(&lun->lun_ref_comp);
-
-	ret = core_tpg_add_lun(se_tpg, lun, lun_access, dev);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
 int core_tpg_register(
-	const struct target_core_fabric_ops *tfo,
 	struct se_wwn *se_wwn,
 	struct se_portal_group *se_tpg,
-	void *tpg_fabric_ptr,
-	int se_tpg_type)
+	int proto_id)
 {
-	struct se_lun *lun;
-	u32 i;
-
-	se_tpg->tpg_lun_list = array_zalloc(TRANSPORT_MAX_LUNS_PER_TPG,
-			sizeof(struct se_lun), GFP_KERNEL);
-	if (!se_tpg->tpg_lun_list) {
-		pr_err("Unable to allocate struct se_portal_group->"
-				"tpg_lun_list\n");
-		return -ENOMEM;
-	}
+	int ret;
+
+	if (!se_tpg)
+		return -EINVAL;
+	/*
+	 * For the typical case where core_tpg_register() is called by a
+	 * fabric driver from target_core_fabric_ops->fabric_make_tpg()
+	 * configfs context, use the original tf_ops pointer already saved
+	 * by target-core in target_fabric_make_wwn().
+	 *
+	 * Otherwise, for special cases like iscsi-target discovery TPGs
+	 * the caller is responsible for setting ->se_tpg_tfo ahead of
+	 * calling core_tpg_register().
+	 */
+	if (se_wwn)
+		se_tpg->se_tpg_tfo = se_wwn->wwn_tf->tf_ops;
 
-	for (i = 0; i < TRANSPORT_MAX_LUNS_PER_TPG; i++) {
-		lun = se_tpg->tpg_lun_list[i];
-		lun->unpacked_lun = i;
-		lun->lun_link_magic = SE_LUN_LINK_MAGIC;
-		lun->lun_status = TRANSPORT_LUN_STATUS_FREE;
-		atomic_set(&lun->lun_acl_count, 0);
-		init_completion(&lun->lun_shutdown_comp);
-		INIT_LIST_HEAD(&lun->lun_acl_list);
-		spin_lock_init(&lun->lun_acl_lock);
-		spin_lock_init(&lun->lun_sep_lock);
-		init_completion(&lun->lun_ref_comp);
+	if (!se_tpg->se_tpg_tfo) {
+		pr_err("Unable to locate se_tpg->se_tpg_tfo pointer\n");
+		return -EINVAL;
 	}
 
-	se_tpg->se_tpg_type = se_tpg_type;
-	se_tpg->se_tpg_fabric_ptr = tpg_fabric_ptr;
-	se_tpg->se_tpg_tfo = tfo;
+	INIT_HLIST_HEAD(&se_tpg->tpg_lun_hlist);
+	se_tpg->proto_id = proto_id;
 	se_tpg->se_tpg_wwn = se_wwn;
 	atomic_set(&se_tpg->tpg_pr_ref_count, 0);
 	INIT_LIST_HEAD(&se_tpg->acl_node_list);
 	INIT_LIST_HEAD(&se_tpg->se_tpg_node);
 	INIT_LIST_HEAD(&se_tpg->tpg_sess_list);
-	spin_lock_init(&se_tpg->acl_node_lock);
 	spin_lock_init(&se_tpg->session_lock);
-	spin_lock_init(&se_tpg->tpg_lun_lock);
-
-	if (se_tpg->se_tpg_type == TRANSPORT_TPG_TYPE_NORMAL) {
-		if (core_tpg_setup_virtual_lun0(se_tpg) < 0) {
-			array_free(se_tpg->tpg_lun_list,
-				   TRANSPORT_MAX_LUNS_PER_TPG);
-			return -ENOMEM;
+	mutex_init(&se_tpg->tpg_lun_mutex);
+	mutex_init(&se_tpg->acl_node_mutex);
+
+	if (se_tpg->proto_id >= 0) {
+		se_tpg->tpg_virt_lun0 = core_tpg_alloc_lun(se_tpg, 0);
+		if (IS_ERR(se_tpg->tpg_virt_lun0))
+			return PTR_ERR(se_tpg->tpg_virt_lun0);
+
+		ret = core_tpg_add_lun(se_tpg, se_tpg->tpg_virt_lun0,
+				TRANSPORT_LUNFLAGS_READ_ONLY, g_lun0_dev);
+		if (ret < 0) {
+			kfree(se_tpg->tpg_virt_lun0);
+			return ret;
 		}
 	}
 
@@ -726,11 +540,11 @@ int core_tpg_register(
 	list_add_tail(&se_tpg->se_tpg_node, &tpg_list);
 	spin_unlock_bh(&tpg_lock);
 
-	pr_debug("TARGET_CORE[%s]: Allocated %s struct se_portal_group for"
-		" endpoint: %s, Portal Tag: %u\n", tfo->get_fabric_name(),
-		(se_tpg->se_tpg_type == TRANSPORT_TPG_TYPE_NORMAL) ?
-		"Normal" : "Discovery", (tfo->tpg_get_wwn(se_tpg) == NULL) ?
-		"None" : tfo->tpg_get_wwn(se_tpg), tfo->tpg_get_tag(se_tpg));
+	pr_debug("TARGET_CORE[%s]: Allocated portal_group for endpoint: %s, "
+		 "Proto: %d, Portal Tag: %u\n", se_tpg->se_tpg_tfo->get_fabric_name(),
+		se_tpg->se_tpg_tfo->tpg_get_wwn(se_tpg) ?
+		se_tpg->se_tpg_tfo->tpg_get_wwn(se_tpg) : NULL,
+		se_tpg->proto_id, se_tpg->se_tpg_tfo->tpg_get_tag(se_tpg));
 
 	return 0;
 }
@@ -738,14 +552,14 @@ EXPORT_SYMBOL(core_tpg_register);
 
 int core_tpg_deregister(struct se_portal_group *se_tpg)
 {
+	const struct target_core_fabric_ops *tfo = se_tpg->se_tpg_tfo;
 	struct se_node_acl *nacl, *nacl_tmp;
+	LIST_HEAD(node_list);
 
-	pr_debug("TARGET_CORE[%s]: Deallocating %s struct se_portal_group"
-		" for endpoint: %s Portal Tag %u\n",
-		(se_tpg->se_tpg_type == TRANSPORT_TPG_TYPE_NORMAL) ?
-		"Normal" : "Discovery", se_tpg->se_tpg_tfo->get_fabric_name(),
-		se_tpg->se_tpg_tfo->tpg_get_wwn(se_tpg),
-		se_tpg->se_tpg_tfo->tpg_get_tag(se_tpg));
+	pr_debug("TARGET_CORE[%s]: Deallocating portal_group for endpoint: %s, "
+		 "Proto: %d, Portal Tag: %u\n", tfo->get_fabric_name(),
+		tfo->tpg_get_wwn(se_tpg) ? tfo->tpg_get_wwn(se_tpg) : NULL,
+		se_tpg->proto_id, tfo->tpg_get_tag(se_tpg));
 
 	spin_lock_bh(&tpg_lock);
 	list_del(&se_tpg->se_tpg_node);
@@ -753,61 +567,56 @@ int core_tpg_deregister(struct se_portal_group *se_tpg)
 
 	while (atomic_read(&se_tpg->tpg_pr_ref_count) != 0)
 		cpu_relax();
+
+	mutex_lock(&se_tpg->acl_node_mutex);
+	list_splice_init(&se_tpg->acl_node_list, &node_list);
+	mutex_unlock(&se_tpg->acl_node_mutex);
 	/*
 	 * Release any remaining demo-mode generated se_node_acl that have
 	 * not been released because of TFO->tpg_check_demo_mode_cache() == 1
 	 * in transport_deregister_session().
 	 */
-	spin_lock_irq(&se_tpg->acl_node_lock);
-	list_for_each_entry_safe(nacl, nacl_tmp, &se_tpg->acl_node_list,
-			acl_list) {
+	list_for_each_entry_safe(nacl, nacl_tmp, &node_list, acl_list) {
 		list_del(&nacl->acl_list);
 		se_tpg->num_node_acls--;
-		spin_unlock_irq(&se_tpg->acl_node_lock);
 
 		core_tpg_wait_for_nacl_pr_ref(nacl);
 		core_free_device_list_for_node(nacl, se_tpg);
-		se_tpg->se_tpg_tfo->tpg_release_fabric_acl(se_tpg, nacl);
-
-		spin_lock_irq(&se_tpg->acl_node_lock);
+		kfree(nacl);
 	}
-	spin_unlock_irq(&se_tpg->acl_node_lock);
 
-	if (se_tpg->se_tpg_type == TRANSPORT_TPG_TYPE_NORMAL)
-		core_tpg_remove_lun(se_tpg, &se_tpg->tpg_virt_lun0);
+	if (se_tpg->proto_id >= 0) {
+		core_tpg_remove_lun(se_tpg, se_tpg->tpg_virt_lun0);
+		kfree_rcu(se_tpg->tpg_virt_lun0, rcu_head);
+	}
 
-	se_tpg->se_tpg_fabric_ptr = NULL;
-	array_free(se_tpg->tpg_lun_list, TRANSPORT_MAX_LUNS_PER_TPG);
 	return 0;
 }
 EXPORT_SYMBOL(core_tpg_deregister);
 
 struct se_lun *core_tpg_alloc_lun(
 	struct se_portal_group *tpg,
-	u32 unpacked_lun)
+	u64 unpacked_lun)
 {
 	struct se_lun *lun;
 
-	if (unpacked_lun > (TRANSPORT_MAX_LUNS_PER_TPG-1)) {
-		pr_err("%s LUN: %u exceeds TRANSPORT_MAX_LUNS_PER_TPG"
-			"-1: %u for Target Portal Group: %u\n",
-			tpg->se_tpg_tfo->get_fabric_name(),
-			unpacked_lun, TRANSPORT_MAX_LUNS_PER_TPG-1,
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		return ERR_PTR(-EOVERFLOW);
-	}
-
-	spin_lock(&tpg->tpg_lun_lock);
-	lun = tpg->tpg_lun_list[unpacked_lun];
-	if (lun->lun_status == TRANSPORT_LUN_STATUS_ACTIVE) {
-		pr_err("TPG Logical Unit Number: %u is already active"
-			" on %s Target Portal Group: %u, ignoring request.\n",
-			unpacked_lun, tpg->se_tpg_tfo->get_fabric_name(),
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		spin_unlock(&tpg->tpg_lun_lock);
-		return ERR_PTR(-EINVAL);
+	lun = kzalloc(sizeof(*lun), GFP_KERNEL);
+	if (!lun) {
+		pr_err("Unable to allocate se_lun memory\n");
+		return ERR_PTR(-ENOMEM);
 	}
-	spin_unlock(&tpg->tpg_lun_lock);
+	lun->unpacked_lun = unpacked_lun;
+	lun->lun_link_magic = SE_LUN_LINK_MAGIC;
+	atomic_set(&lun->lun_acl_count, 0);
+	init_completion(&lun->lun_ref_comp);
+	INIT_LIST_HEAD(&lun->lun_deve_list);
+	INIT_LIST_HEAD(&lun->lun_dev_link);
+	atomic_set(&lun->lun_tg_pt_secondary_offline, 0);
+	spin_lock_init(&lun->lun_deve_lock);
+	mutex_init(&lun->lun_tg_pt_md_mutex);
+	INIT_LIST_HEAD(&lun->lun_tg_pt_gp_link);
+	spin_lock_init(&lun->lun_tg_pt_gp_lock);
+	lun->lun_tpg = tpg;
 
 	return lun;
 }
@@ -823,34 +632,70 @@ int core_tpg_add_lun(
 	ret = percpu_ref_init(&lun->lun_ref, core_tpg_lun_ref_release, 0,
 			      GFP_KERNEL);
 	if (ret < 0)
-		return ret;
+		goto out;
 
-	ret = core_dev_export(dev, tpg, lun);
-	if (ret < 0) {
-		percpu_ref_exit(&lun->lun_ref);
-		return ret;
-	}
+	ret = core_alloc_rtpi(lun, dev);
+	if (ret)
+		goto out_kill_ref;
+
+	if (!(dev->transport->transport_flags & TRANSPORT_FLAG_PASSTHROUGH) &&
+	    !(dev->se_hba->hba_flags & HBA_FLAGS_INTERNAL_USE))
+		target_attach_tg_pt_gp(lun, dev->t10_alua.default_tg_pt_gp);
+
+	mutex_lock(&tpg->tpg_lun_mutex);
+
+	spin_lock(&dev->se_port_lock);
+	lun->lun_index = dev->dev_index;
+	rcu_assign_pointer(lun->lun_se_dev, dev);
+	dev->export_count++;
+	list_add_tail(&lun->lun_dev_link, &dev->dev_sep_list);
+	spin_unlock(&dev->se_port_lock);
 
-	spin_lock(&tpg->tpg_lun_lock);
 	lun->lun_access = lun_access;
-	lun->lun_status = TRANSPORT_LUN_STATUS_ACTIVE;
-	spin_unlock(&tpg->tpg_lun_lock);
+	if (!(dev->se_hba->hba_flags & HBA_FLAGS_INTERNAL_USE))
+		hlist_add_head_rcu(&lun->link, &tpg->tpg_lun_hlist);
+	mutex_unlock(&tpg->tpg_lun_mutex);
 
 	return 0;
+
+out_kill_ref:
+	percpu_ref_exit(&lun->lun_ref);
+out:
+	return ret;
 }
 
 void core_tpg_remove_lun(
 	struct se_portal_group *tpg,
 	struct se_lun *lun)
 {
+	/*
+	 * rcu_dereference_raw protected by se_lun->lun_group symlink
+	 * reference to se_device->dev_group.
+	 */
+	struct se_device *dev = rcu_dereference_raw(lun->lun_se_dev);
+
 	core_clear_lun_from_tpg(lun, tpg);
+	/*
+	 * Wait for any active I/O references to percpu se_lun->lun_ref to
+	 * be released.  Also, se_lun->lun_ref is now used by PR and ALUA
+	 * logic when referencing a remote target port during ALL_TGT_PT=1
+	 * and generating UNIT_ATTENTIONs for ALUA access state transition.
+	 */
 	transport_clear_lun_ref(lun);
 
-	core_dev_unexport(lun->lun_se_dev, tpg, lun);
+	mutex_lock(&tpg->tpg_lun_mutex);
+	if (lun->lun_se_dev) {
+		target_detach_tg_pt_gp(lun);
 
-	spin_lock(&tpg->tpg_lun_lock);
-	lun->lun_status = TRANSPORT_LUN_STATUS_FREE;
-	spin_unlock(&tpg->tpg_lun_lock);
+		spin_lock(&dev->se_port_lock);
+		list_del(&lun->lun_dev_link);
+		dev->export_count--;
+		rcu_assign_pointer(lun->lun_se_dev, NULL);
+		spin_unlock(&dev->se_port_lock);
+	}
+	if (!(dev->se_hba->hba_flags & HBA_FLAGS_INTERNAL_USE))
+		hlist_del_rcu(&lun->link);
+	mutex_unlock(&tpg->tpg_lun_mutex);
 
 	percpu_ref_exit(&lun->lun_ref);
 }
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 675f2d9d1f14..ce8574b7220c 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -34,17 +34,15 @@
 #include <linux/cdrom.h>
 #include <linux/module.h>
 #include <linux/ratelimit.h>
+#include <linux/vmalloc.h>
 #include <asm/unaligned.h>
 #include <net/sock.h>
 #include <net/tcp.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
-#include <scsi/scsi_tcq.h>
+#include <scsi/scsi_proto.h>
 
 #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"
 #include "target_core_alua.h"
@@ -61,7 +59,6 @@ struct kmem_cache *t10_pr_reg_cache;
 struct kmem_cache *t10_alua_lu_gp_cache;
 struct kmem_cache *t10_alua_lu_gp_mem_cache;
 struct kmem_cache *t10_alua_tg_pt_gp_cache;
-struct kmem_cache *t10_alua_tg_pt_gp_mem_cache;
 struct kmem_cache *t10_alua_lba_map_cache;
 struct kmem_cache *t10_alua_lba_map_mem_cache;
 
@@ -120,16 +117,6 @@ int init_se_kmem_caches(void)
 				"cache failed\n");
 		goto out_free_lu_gp_mem_cache;
 	}
-	t10_alua_tg_pt_gp_mem_cache = kmem_cache_create(
-			"t10_alua_tg_pt_gp_mem_cache",
-			sizeof(struct t10_alua_tg_pt_gp_member),
-			__alignof__(struct t10_alua_tg_pt_gp_member),
-			0, NULL);
-	if (!t10_alua_tg_pt_gp_mem_cache) {
-		pr_err("kmem_cache_create() for t10_alua_tg_pt_gp_"
-				"mem_t failed\n");
-		goto out_free_tg_pt_gp_cache;
-	}
 	t10_alua_lba_map_cache = kmem_cache_create(
 			"t10_alua_lba_map_cache",
 			sizeof(struct t10_alua_lba_map),
@@ -137,7 +124,7 @@ int init_se_kmem_caches(void)
 	if (!t10_alua_lba_map_cache) {
 		pr_err("kmem_cache_create() for t10_alua_lba_map_"
 				"cache failed\n");
-		goto out_free_tg_pt_gp_mem_cache;
+		goto out_free_tg_pt_gp_cache;
 	}
 	t10_alua_lba_map_mem_cache = kmem_cache_create(
 			"t10_alua_lba_map_mem_cache",
@@ -160,8 +147,6 @@ out_free_lba_map_mem_cache:
 	kmem_cache_destroy(t10_alua_lba_map_mem_cache);
 out_free_lba_map_cache:
 	kmem_cache_destroy(t10_alua_lba_map_cache);
-out_free_tg_pt_gp_mem_cache:
-	kmem_cache_destroy(t10_alua_tg_pt_gp_mem_cache);
 out_free_tg_pt_gp_cache:
 	kmem_cache_destroy(t10_alua_tg_pt_gp_cache);
 out_free_lu_gp_mem_cache:
@@ -187,7 +172,6 @@ void release_se_kmem_caches(void)
 	kmem_cache_destroy(t10_alua_lu_gp_cache);
 	kmem_cache_destroy(t10_alua_lu_gp_mem_cache);
 	kmem_cache_destroy(t10_alua_tg_pt_gp_cache);
-	kmem_cache_destroy(t10_alua_tg_pt_gp_mem_cache);
 	kmem_cache_destroy(t10_alua_lba_map_cache);
 	kmem_cache_destroy(t10_alua_lba_map_mem_cache);
 }
@@ -280,10 +264,7 @@ int transport_alloc_session_tags(struct se_session *se_sess,
 	if (rc < 0) {
 		pr_err("Unable to init se_sess->sess_tag_pool,"
 			" tag_num: %u\n", tag_num);
-		if (is_vmalloc_addr(se_sess->sess_cmd_map))
-			vfree(se_sess->sess_cmd_map);
-		else
-			kfree(se_sess->sess_cmd_map);
+		kvfree(se_sess->sess_cmd_map);
 		se_sess->sess_cmd_map = NULL;
 		return -ENOMEM;
 	}
@@ -410,12 +391,6 @@ EXPORT_SYMBOL(target_get_session);
 
 void target_put_session(struct se_session *se_sess)
 {
-	struct se_portal_group *tpg = se_sess->se_tpg;
-
-	if (tpg->se_tpg_tfo->put_session != NULL) {
-		tpg->se_tpg_tfo->put_session(se_sess);
-		return;
-	}
 	kref_put(&se_sess->sess_kref, target_release_session);
 }
 EXPORT_SYMBOL(target_put_session);
@@ -490,10 +465,7 @@ void transport_free_session(struct se_session *se_sess)
 {
 	if (se_sess->sess_cmd_map) {
 		percpu_ida_destroy(&se_sess->sess_tag_pool);
-		if (is_vmalloc_addr(se_sess->sess_cmd_map))
-			vfree(se_sess->sess_cmd_map);
-		else
-			kfree(se_sess->sess_cmd_map);
+		kvfree(se_sess->sess_cmd_map);
 	}
 	kmem_cache_free(se_sess_cache, se_sess);
 }
@@ -505,7 +477,7 @@ void transport_deregister_session(struct se_session *se_sess)
 	const struct target_core_fabric_ops *se_tfo;
 	struct se_node_acl *se_nacl;
 	unsigned long flags;
-	bool comp_nacl = true;
+	bool comp_nacl = true, drop_nacl = false;
 
 	if (!se_tpg) {
 		transport_free_session(se_sess);
@@ -525,22 +497,22 @@ void transport_deregister_session(struct se_session *se_sess)
 	 */
 	se_nacl = se_sess->se_node_acl;
 
-	spin_lock_irqsave(&se_tpg->acl_node_lock, flags);
+	mutex_lock(&se_tpg->acl_node_mutex);
 	if (se_nacl && se_nacl->dynamic_node_acl) {
 		if (!se_tfo->tpg_check_demo_mode_cache(se_tpg)) {
 			list_del(&se_nacl->acl_list);
 			se_tpg->num_node_acls--;
-			spin_unlock_irqrestore(&se_tpg->acl_node_lock, flags);
-			core_tpg_wait_for_nacl_pr_ref(se_nacl);
-			core_free_device_list_for_node(se_nacl, se_tpg);
-			se_tfo->tpg_release_fabric_acl(se_tpg, se_nacl);
-
-			comp_nacl = false;
-			spin_lock_irqsave(&se_tpg->acl_node_lock, flags);
+			drop_nacl = true;
 		}
 	}
-	spin_unlock_irqrestore(&se_tpg->acl_node_lock, flags);
+	mutex_unlock(&se_tpg->acl_node_mutex);
 
+	if (drop_nacl) {
+		core_tpg_wait_for_nacl_pr_ref(se_nacl);
+		core_free_device_list_for_node(se_nacl, se_tpg);
+		kfree(se_nacl);
+		comp_nacl = false;
+	}
 	pr_debug("TARGET_CORE[%s]: Deregistered fabric_sess\n",
 		se_tpg->se_tpg_tfo->get_fabric_name());
 	/*
@@ -600,9 +572,8 @@ static int transport_cmd_check_stop(struct se_cmd *cmd, bool remove_from_lists,
 	 * this command for frontend exceptions.
 	 */
 	if (cmd->transport_state & CMD_T_STOP) {
-		pr_debug("%s:%d CMD_T_STOP for ITT: 0x%08x\n",
-			__func__, __LINE__,
-			cmd->se_tfo->get_task_tag(cmd));
+		pr_debug("%s:%d CMD_T_STOP for ITT: 0x%08llx\n",
+			__func__, __LINE__, cmd->tag);
 
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
@@ -1155,6 +1126,8 @@ target_cmd_size_check(struct se_cmd *cmd, unsigned int size)
 /*
  * Used by fabric modules containing a local struct se_cmd within their
  * fabric dependent per I/O descriptor.
+ *
+ * Preserves the value of @cmd->tag.
  */
 void transport_init_se_cmd(
 	struct se_cmd *cmd,
@@ -1281,11 +1254,7 @@ target_setup_cmd_from_cdb(struct se_cmd *cmd, unsigned char *cdb)
 		return ret;
 
 	cmd->se_cmd_flags |= SCF_SUPPORTED_SAM_OPCODE;
-
-	spin_lock(&cmd->se_lun->lun_sep_lock);
-	if (cmd->se_lun->lun_sep)
-		cmd->se_lun->lun_sep->sep_stats.cmd_pdus++;
-	spin_unlock(&cmd->se_lun->lun_sep_lock);
+	atomic_long_inc(&cmd->se_lun->lun_stats.cmd_pdus);
 	return 0;
 }
 EXPORT_SYMBOL(target_setup_cmd_from_cdb);
@@ -1353,11 +1322,9 @@ transport_generic_map_mem_to_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
 
 	cmd->t_data_sg = sgl;
 	cmd->t_data_nents = sgl_count;
+	cmd->t_bidi_data_sg = sgl_bidi;
+	cmd->t_bidi_data_nents = sgl_bidi_count;
 
-	if (sgl_bidi && sgl_bidi_count) {
-		cmd->t_bidi_data_sg = sgl_bidi;
-		cmd->t_bidi_data_nents = sgl_bidi_count;
-	}
 	cmd->se_cmd_flags |= SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC;
 	return 0;
 }
@@ -1382,6 +1349,8 @@ transport_generic_map_mem_to_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
  * @sgl_prot: struct scatterlist memory protection information
  * @sgl_prot_count: scatterlist count for protection information
  *
+ * Task tags are supported if the caller has set @se_cmd->tag.
+ *
  * Returns non zero to signal active I/O shutdown failure.  All other
  * setup exceptions will be returned as a SCSI CHECK_CONDITION response,
  * but still return zero here.
@@ -1390,7 +1359,7 @@ transport_generic_map_mem_to_cmd(struct se_cmd *cmd, struct scatterlist *sgl,
  * assumes internal allocation of fabric payload buffer by target-core.
  */
 int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess,
-		unsigned char *cdb, unsigned char *sense, u32 unpacked_lun,
+		unsigned char *cdb, unsigned char *sense, u64 unpacked_lun,
 		u32 data_length, int task_attr, int data_dir, int flags,
 		struct scatterlist *sgl, u32 sgl_count,
 		struct scatterlist *sgl_bidi, u32 sgl_bidi_count,
@@ -1419,7 +1388,7 @@ int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess
 	 * for fabrics using TARGET_SCF_ACK_KREF that expect a second
 	 * kref_put() to happen during fabric packet acknowledgement.
 	 */
-	ret = target_get_sess_cmd(se_sess, se_cmd, (flags & TARGET_SCF_ACK_KREF));
+	ret = target_get_sess_cmd(se_cmd, flags & TARGET_SCF_ACK_KREF);
 	if (ret)
 		return ret;
 	/*
@@ -1433,7 +1402,7 @@ int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess
 	rc = transport_lookup_cmd_lun(se_cmd, unpacked_lun);
 	if (rc) {
 		transport_send_check_condition_and_sense(se_cmd, rc, 0);
-		target_put_sess_cmd(se_sess, se_cmd);
+		target_put_sess_cmd(se_cmd);
 		return 0;
 	}
 
@@ -1450,6 +1419,7 @@ int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess
 	if (sgl_prot_count) {
 		se_cmd->t_prot_sg = sgl_prot;
 		se_cmd->t_prot_nents = sgl_prot_count;
+		se_cmd->se_cmd_flags |= SCF_PASSTHROUGH_PROT_SG_TO_MEM_NOALLOC;
 	}
 
 	/*
@@ -1513,6 +1483,8 @@ EXPORT_SYMBOL(target_submit_cmd_map_sgls);
  * @data_dir: DMA data direction
  * @flags: flags for command submission from target_sc_flags_tables
  *
+ * Task tags are supported if the caller has set @se_cmd->tag.
+ *
  * Returns non zero to signal active I/O shutdown failure.  All other
  * setup exceptions will be returned as a SCSI CHECK_CONDITION response,
  * but still return zero here.
@@ -1523,7 +1495,7 @@ EXPORT_SYMBOL(target_submit_cmd_map_sgls);
  * It also assumes interal target core SGL memory allocation.
  */
 int target_submit_cmd(struct se_cmd *se_cmd, struct se_session *se_sess,
-		unsigned char *cdb, unsigned char *sense, u32 unpacked_lun,
+		unsigned char *cdb, unsigned char *sense, u64 unpacked_lun,
 		u32 data_length, int task_attr, int data_dir, int flags)
 {
 	return target_submit_cmd_map_sgls(se_cmd, se_sess, cdb, sense,
@@ -1560,7 +1532,7 @@ static void target_complete_tmr_failure(struct work_struct *work)
  **/
 
 int target_submit_tmr(struct se_cmd *se_cmd, struct se_session *se_sess,
-		unsigned char *sense, u32 unpacked_lun,
+		unsigned char *sense, u64 unpacked_lun,
 		void *fabric_tmr_ptr, unsigned char tm_type,
 		gfp_t gfp, unsigned int tag, int flags)
 {
@@ -1584,7 +1556,7 @@ int target_submit_tmr(struct se_cmd *se_cmd, struct se_session *se_sess,
 		se_cmd->se_tmr_req->ref_task_tag = tag;
 
 	/* See target_submit_cmd for commentary */
-	ret = target_get_sess_cmd(se_sess, se_cmd, (flags & TARGET_SCF_ACK_KREF));
+	ret = target_get_sess_cmd(se_cmd, flags & TARGET_SCF_ACK_KREF);
 	if (ret) {
 		core_tmr_release_req(se_cmd->se_tmr_req);
 		return ret;
@@ -1640,9 +1612,8 @@ void transport_generic_request_failure(struct se_cmd *cmd,
 {
 	int ret = 0;
 
-	pr_debug("-----[ Storage Engine Exception for cmd: %p ITT: 0x%08x"
-		" CDB: 0x%02x\n", cmd, cmd->se_tfo->get_task_tag(cmd),
-		cmd->t_task_cdb[0]);
+	pr_debug("-----[ Storage Engine Exception for cmd: %p ITT: 0x%08llx"
+		" CDB: 0x%02x\n", cmd, cmd->tag, cmd->t_task_cdb[0]);
 	pr_debug("-----[ i_state: %d t_state: %d sense_reason: %d\n",
 		cmd->se_tfo->get_cmd_state(cmd),
 		cmd->t_state, sense_reason);
@@ -1699,13 +1670,13 @@ void transport_generic_request_failure(struct se_cmd *cmd,
 		 * See spc4r17, section 7.4.6 Control Mode Page, Table 349
 		 */
 		if (cmd->se_sess &&
-		    cmd->se_dev->dev_attrib.emulate_ua_intlck_ctrl == 2)
-			core_scsi3_ua_allocate(cmd->se_sess->se_node_acl,
-				cmd->orig_fe_lun, 0x2C,
-				ASCQ_2CH_PREVIOUS_RESERVATION_CONFLICT_STATUS);
-
+		    cmd->se_dev->dev_attrib.emulate_ua_intlck_ctrl == 2) {
+			target_ua_allocate_lun(cmd->se_sess->se_node_acl,
+					       cmd->orig_fe_lun, 0x2C,
+					ASCQ_2CH_PREVIOUS_RESERVATION_CONFLICT_STATUS);
+		}
 		trace_target_cmd_complete(cmd);
-		ret = cmd->se_tfo-> queue_status(cmd);
+		ret = cmd->se_tfo->queue_status(cmd);
 		if (ret == -EAGAIN || ret == -ENOMEM)
 			goto queue_full;
 		goto check_stop;
@@ -1766,8 +1737,8 @@ static int target_write_prot_action(struct se_cmd *cmd)
 			break;
 
 		sectors = cmd->data_length >> ilog2(cmd->se_dev->dev_attrib.block_size);
-		cmd->pi_err = sbc_dif_verify_write(cmd, cmd->t_task_lba,
-						   sectors, 0, NULL, 0);
+		cmd->pi_err = sbc_dif_verify(cmd, cmd->t_task_lba,
+					     sectors, 0, cmd->t_prot_sg, 0);
 		if (unlikely(cmd->pi_err)) {
 			spin_lock_irq(&cmd->t_state_lock);
 			cmd->transport_state &= ~(CMD_T_BUSY|CMD_T_SENT);
@@ -1850,9 +1821,8 @@ void target_execute_cmd(struct se_cmd *cmd)
 	 */
 	spin_lock_irq(&cmd->t_state_lock);
 	if (cmd->transport_state & CMD_T_STOP) {
-		pr_debug("%s:%d CMD_T_STOP for ITT: 0x%08x\n",
-			__func__, __LINE__,
-			cmd->se_tfo->get_task_tag(cmd));
+		pr_debug("%s:%d CMD_T_STOP for ITT: 0x%08llx\n",
+			__func__, __LINE__, cmd->tag);
 
 		spin_unlock_irq(&cmd->t_state_lock);
 		complete_all(&cmd->t_transport_stop_comp);
@@ -1991,16 +1961,17 @@ static void transport_handle_queue_full(
 
 static bool target_read_prot_action(struct se_cmd *cmd)
 {
-	sense_reason_t rc;
-
 	switch (cmd->prot_op) {
 	case TARGET_PROT_DIN_STRIP:
 		if (!(cmd->se_sess->sup_prot_ops & TARGET_PROT_DIN_STRIP)) {
-			rc = sbc_dif_read_strip(cmd);
-			if (rc) {
-				cmd->pi_err = rc;
+			u32 sectors = cmd->data_length >>
+				  ilog2(cmd->se_dev->dev_attrib.block_size);
+
+			cmd->pi_err = sbc_dif_verify(cmd, cmd->t_task_lba,
+						     sectors, 0, cmd->t_prot_sg,
+						     0);
+			if (cmd->pi_err)
 				return true;
-			}
 		}
 		break;
 	case TARGET_PROT_DIN_INSERT:
@@ -2079,12 +2050,8 @@ static void target_complete_ok_work(struct work_struct *work)
 queue_rsp:
 	switch (cmd->data_direction) {
 	case DMA_FROM_DEVICE:
-		spin_lock(&cmd->se_lun->lun_sep_lock);
-		if (cmd->se_lun->lun_sep) {
-			cmd->se_lun->lun_sep->sep_stats.tx_data_octets +=
-					cmd->data_length;
-		}
-		spin_unlock(&cmd->se_lun->lun_sep_lock);
+		atomic_long_add(cmd->data_length,
+				&cmd->se_lun->lun_stats.tx_data_octets);
 		/*
 		 * Perform READ_STRIP of PI using software emulation when
 		 * backend had PI enabled, if the transport will not be
@@ -2107,22 +2074,14 @@ queue_rsp:
 			goto queue_full;
 		break;
 	case DMA_TO_DEVICE:
-		spin_lock(&cmd->se_lun->lun_sep_lock);
-		if (cmd->se_lun->lun_sep) {
-			cmd->se_lun->lun_sep->sep_stats.rx_data_octets +=
-				cmd->data_length;
-		}
-		spin_unlock(&cmd->se_lun->lun_sep_lock);
+		atomic_long_add(cmd->data_length,
+				&cmd->se_lun->lun_stats.rx_data_octets);
 		/*
 		 * Check if we need to send READ payload for BIDI-COMMAND
 		 */
 		if (cmd->se_cmd_flags & SCF_BIDI) {
-			spin_lock(&cmd->se_lun->lun_sep_lock);
-			if (cmd->se_lun->lun_sep) {
-				cmd->se_lun->lun_sep->sep_stats.tx_data_octets +=
-					cmd->data_length;
-			}
-			spin_unlock(&cmd->se_lun->lun_sep_lock);
+			atomic_long_add(cmd->data_length,
+					&cmd->se_lun->lun_stats.tx_data_octets);
 			ret = cmd->se_tfo->queue_data_in(cmd);
 			if (ret == -EAGAIN || ret == -ENOMEM)
 				goto queue_full;
@@ -2179,6 +2138,12 @@ static inline void transport_reset_sgl_orig(struct se_cmd *cmd)
 
 static inline void transport_free_pages(struct se_cmd *cmd)
 {
+	if (!(cmd->se_cmd_flags & SCF_PASSTHROUGH_PROT_SG_TO_MEM_NOALLOC)) {
+		transport_free_sgl(cmd->t_prot_sg, cmd->t_prot_nents);
+		cmd->t_prot_sg = NULL;
+		cmd->t_prot_nents = 0;
+	}
+
 	if (cmd->se_cmd_flags & SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC) {
 		/*
 		 * Release special case READ buffer payload required for
@@ -2202,10 +2167,6 @@ static inline void transport_free_pages(struct se_cmd *cmd)
 	transport_free_sgl(cmd->t_bidi_data_sg, cmd->t_bidi_data_nents);
 	cmd->t_bidi_data_sg = NULL;
 	cmd->t_bidi_data_nents = 0;
-
-	transport_free_sgl(cmd->t_prot_sg, cmd->t_prot_nents);
-	cmd->t_prot_sg = NULL;
-	cmd->t_prot_nents = 0;
 }
 
 /**
@@ -2227,7 +2188,7 @@ static int transport_release_cmd(struct se_cmd *cmd)
 	 * If this cmd has been setup with target_get_sess_cmd(), drop
 	 * the kref and call ->release_cmd() in kref callback.
 	 */
-	return target_put_sess_cmd(cmd->se_sess, cmd);
+	return target_put_sess_cmd(cmd);
 }
 
 /**
@@ -2344,6 +2305,14 @@ transport_generic_new_cmd(struct se_cmd *cmd)
 	int ret = 0;
 	bool zero_flag = !(cmd->se_cmd_flags & SCF_SCSI_DATA_CDB);
 
+	if (cmd->prot_op != TARGET_PROT_NORMAL &&
+	    !(cmd->se_cmd_flags & SCF_PASSTHROUGH_PROT_SG_TO_MEM_NOALLOC)) {
+		ret = target_alloc_sgl(&cmd->t_prot_sg, &cmd->t_prot_nents,
+				       cmd->prot_length, true);
+		if (ret < 0)
+			return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+	}
+
 	/*
 	 * Determine is the TCM fabric module has already allocated physical
 	 * memory, and is directly calling transport_generic_map_mem_to_cmd()
@@ -2369,14 +2338,6 @@ transport_generic_new_cmd(struct se_cmd *cmd)
 				return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		}
 
-		if (cmd->prot_op != TARGET_PROT_NORMAL) {
-			ret = target_alloc_sgl(&cmd->t_prot_sg,
-					       &cmd->t_prot_nents,
-					       cmd->prot_length, true);
-			if (ret < 0)
-				return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-		}
-
 		ret = target_alloc_sgl(&cmd->t_data_sg, &cmd->t_data_nents,
 				       cmd->data_length, zero_flag);
 		if (ret < 0)
@@ -2471,13 +2432,12 @@ int transport_generic_free_cmd(struct se_cmd *cmd, int wait_for_tasks)
 EXPORT_SYMBOL(transport_generic_free_cmd);
 
 /* target_get_sess_cmd - Add command to active ->sess_cmd_list
- * @se_sess:	session to reference
  * @se_cmd:	command descriptor to add
  * @ack_kref:	Signal that fabric will perform an ack target_put_sess_cmd()
  */
-int target_get_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd,
-			       bool ack_kref)
+int target_get_sess_cmd(struct se_cmd *se_cmd, bool ack_kref)
 {
+	struct se_session *se_sess = se_cmd->se_sess;
 	unsigned long flags;
 	int ret = 0;
 
@@ -2499,7 +2459,7 @@ out:
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 
 	if (ret && ack_kref)
-		target_put_sess_cmd(se_sess, se_cmd);
+		target_put_sess_cmd(se_cmd);
 
 	return ret;
 }
@@ -2528,11 +2488,12 @@ static void target_release_cmd_kref(struct kref *kref)
 }
 
 /* target_put_sess_cmd - Check for active I/O shutdown via kref_put
- * @se_sess:	session to reference
  * @se_cmd:	command descriptor to drop
  */
-int target_put_sess_cmd(struct se_session *se_sess, struct se_cmd *se_cmd)
+int target_put_sess_cmd(struct se_cmd *se_cmd)
 {
+	struct se_session *se_sess = se_cmd->se_sess;
+
 	if (!se_sess) {
 		se_cmd->se_tfo->release_cmd(se_cmd);
 		return 1;
@@ -2598,31 +2559,10 @@ void target_wait_for_sess_cmds(struct se_session *se_sess)
 }
 EXPORT_SYMBOL(target_wait_for_sess_cmds);
 
-static int transport_clear_lun_ref_thread(void *p)
+void transport_clear_lun_ref(struct se_lun *lun)
 {
-	struct se_lun *lun = p;
-
 	percpu_ref_kill(&lun->lun_ref);
-
 	wait_for_completion(&lun->lun_ref_comp);
-	complete(&lun->lun_shutdown_comp);
-
-	return 0;
-}
-
-int transport_clear_lun_ref(struct se_lun *lun)
-{
-	struct task_struct *kt;
-
-	kt = kthread_run(transport_clear_lun_ref_thread, lun,
-			"tcm_cl_%u", lun->unpacked_lun);
-	if (IS_ERR(kt)) {
-		pr_err("Unable to start clear_lun thread\n");
-		return PTR_ERR(kt);
-	}
-	wait_for_completion(&lun->lun_shutdown_comp);
-
-	return 0;
 }
 
 /**
@@ -2656,10 +2596,8 @@ bool transport_wait_for_tasks(struct se_cmd *cmd)
 
 	cmd->transport_state |= CMD_T_STOP;
 
-	pr_debug("wait_for_tasks: Stopping %p ITT: 0x%08x"
-		" i_state: %d, t_state: %d, CMD_T_STOP\n",
-		cmd, cmd->se_tfo->get_task_tag(cmd),
-		cmd->se_tfo->get_cmd_state(cmd), cmd->t_state);
+	pr_debug("wait_for_tasks: Stopping %p ITT: 0x%08llx i_state: %d, t_state: %d, CMD_T_STOP\n",
+		cmd, cmd->tag, cmd->se_tfo->get_cmd_state(cmd), cmd->t_state);
 
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
@@ -2668,9 +2606,8 @@ bool transport_wait_for_tasks(struct se_cmd *cmd)
 	spin_lock_irqsave(&cmd->t_state_lock, flags);
 	cmd->transport_state &= ~(CMD_T_ACTIVE | CMD_T_STOP);
 
-	pr_debug("wait_for_tasks: Stopped wait_for_completion("
-		"&cmd->t_transport_stop_comp) for ITT: 0x%08x\n",
-		cmd->se_tfo->get_task_tag(cmd));
+	pr_debug("wait_for_tasks: Stopped wait_for_completion(&cmd->t_transport_stop_comp) for ITT: 0x%08llx\n",
+		cmd->tag);
 
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
@@ -2972,8 +2909,8 @@ int transport_check_aborted_status(struct se_cmd *cmd, int send_status)
 	if (!send_status || !(cmd->se_cmd_flags & SCF_SEND_DELAYED_TAS))
 		return 1;
 
-	pr_debug("Sending delayed SAM_STAT_TASK_ABORTED status for CDB: 0x%02x ITT: 0x%08x\n",
-		 cmd->t_task_cdb[0], cmd->se_tfo->get_task_tag(cmd));
+	pr_debug("Sending delayed SAM_STAT_TASK_ABORTED status for CDB: 0x%02x ITT: 0x%08llx\n",
+		 cmd->t_task_cdb[0], cmd->tag);
 
 	cmd->se_cmd_flags &= ~SCF_SEND_DELAYED_TAS;
 	cmd->scsi_status = SAM_STAT_TASK_ABORTED;
@@ -3012,9 +2949,8 @@ void transport_send_task_abort(struct se_cmd *cmd)
 
 	transport_lun_remove_cmd(cmd);
 
-	pr_debug("Setting SAM_STAT_TASK_ABORTED status for CDB: 0x%02x,"
-		" ITT: 0x%08x\n", cmd->t_task_cdb[0],
-		cmd->se_tfo->get_task_tag(cmd));
+	pr_debug("Setting SAM_STAT_TASK_ABORTED status for CDB: 0x%02x, ITT: 0x%08llx\n",
+		 cmd->t_task_cdb[0], cmd->tag);
 
 	trace_target_cmd_complete(cmd);
 	cmd->se_tfo->queue_status(cmd);
@@ -3040,6 +2976,11 @@ static void target_tmr_work(struct work_struct *work)
 		ret = core_tmr_lun_reset(dev, tmr, NULL, NULL);
 		tmr->response = (!ret) ? TMR_FUNCTION_COMPLETE :
 					 TMR_FUNCTION_REJECTED;
+		if (tmr->response == TMR_FUNCTION_COMPLETE) {
+			target_ua_allocate_lun(cmd->se_sess->se_node_acl,
+					       cmd->orig_fe_lun, 0x29,
+					       ASCQ_29H_BUS_DEVICE_RESET_FUNCTION_OCCURRED);
+		}
 		break;
 	case TMR_TARGET_WARM_RESET:
 		tmr->response = TMR_FUNCTION_REJECTED;
@@ -3074,3 +3015,22 @@ int transport_generic_handle_tmr(
 	return 0;
 }
 EXPORT_SYMBOL(transport_generic_handle_tmr);
+
+bool
+target_check_wce(struct se_device *dev)
+{
+	bool wce = false;
+
+	if (dev->transport->get_write_cache)
+		wce = dev->transport->get_write_cache(dev);
+	else if (dev->dev_attrib.emulate_write_cache > 0)
+		wce = true;
+
+	return wce;
+}
+
+bool
+target_check_fua(struct se_device *dev)
+{
+	return target_check_wce(dev) && dev->dev_attrib.emulate_fua_write > 0;
+}
diff --git a/drivers/target/target_core_ua.c b/drivers/target/target_core_ua.c
index 1738b1646988..be25eb807a5f 100644
--- a/drivers/target/target_core_ua.c
+++ b/drivers/target/target_core_ua.c
@@ -25,12 +25,10 @@
 
 #include <linux/slab.h>
 #include <linux/spinlock.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_proto.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 
 #include "target_core_internal.h"
 #include "target_core_alua.h"
@@ -51,9 +49,17 @@ target_scsi3_ua_check(struct se_cmd *cmd)
 	if (!nacl)
 		return 0;
 
-	deve = nacl->device_list[cmd->orig_fe_lun];
-	if (!atomic_read(&deve->ua_count))
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, cmd->orig_fe_lun);
+	if (!deve) {
+		rcu_read_unlock();
 		return 0;
+	}
+	if (!atomic_read(&deve->ua_count)) {
+		rcu_read_unlock();
+		return 0;
+	}
+	rcu_read_unlock();
 	/*
 	 * From sam4r14, section 5.14 Unit attention condition:
 	 *
@@ -80,18 +86,11 @@ target_scsi3_ua_check(struct se_cmd *cmd)
 }
 
 int core_scsi3_ua_allocate(
-	struct se_node_acl *nacl,
-	u32 unpacked_lun,
+	struct se_dev_entry *deve,
 	u8 asc,
 	u8 ascq)
 {
-	struct se_dev_entry *deve;
 	struct se_ua *ua, *ua_p, *ua_tmp;
-	/*
-	 * PASSTHROUGH OPS
-	 */
-	if (!nacl)
-		return -EINVAL;
 
 	ua = kmem_cache_zalloc(se_ua_cache, GFP_ATOMIC);
 	if (!ua) {
@@ -100,13 +99,9 @@ int core_scsi3_ua_allocate(
 	}
 	INIT_LIST_HEAD(&ua->ua_nacl_list);
 
-	ua->ua_nacl = nacl;
 	ua->ua_asc = asc;
 	ua->ua_ascq = ascq;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[unpacked_lun];
-
 	spin_lock(&deve->ua_lock);
 	list_for_each_entry_safe(ua_p, ua_tmp, &deve->ua_list, ua_nacl_list) {
 		/*
@@ -114,7 +109,6 @@ int core_scsi3_ua_allocate(
 		 */
 		if ((ua_p->ua_asc == asc) && (ua_p->ua_ascq == ascq)) {
 			spin_unlock(&deve->ua_lock);
-			spin_unlock_irq(&nacl->device_list_lock);
 			kmem_cache_free(se_ua_cache, ua);
 			return 0;
 		}
@@ -159,24 +153,40 @@ int core_scsi3_ua_allocate(
 			list_add_tail(&ua->ua_nacl_list,
 				&deve->ua_list);
 		spin_unlock(&deve->ua_lock);
-		spin_unlock_irq(&nacl->device_list_lock);
 
 		atomic_inc_mb(&deve->ua_count);
 		return 0;
 	}
 	list_add_tail(&ua->ua_nacl_list, &deve->ua_list);
 	spin_unlock(&deve->ua_lock);
-	spin_unlock_irq(&nacl->device_list_lock);
 
-	pr_debug("[%s]: Allocated UNIT ATTENTION, mapped LUN: %u, ASC:"
-		" 0x%02x, ASCQ: 0x%02x\n",
-		nacl->se_tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
+	pr_debug("Allocated UNIT ATTENTION, mapped LUN: %llu, ASC:"
+		" 0x%02x, ASCQ: 0x%02x\n", deve->mapped_lun,
 		asc, ascq);
 
 	atomic_inc_mb(&deve->ua_count);
 	return 0;
 }
 
+void target_ua_allocate_lun(struct se_node_acl *nacl,
+			    u32 unpacked_lun, u8 asc, u8 ascq)
+{
+	struct se_dev_entry *deve;
+
+	if (!nacl)
+		return;
+
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, unpacked_lun);
+	if (!deve) {
+		rcu_read_unlock();
+		return;
+	}
+
+	core_scsi3_ua_allocate(deve, asc, ascq);
+	rcu_read_unlock();
+}
+
 void core_scsi3_ua_release_all(
 	struct se_dev_entry *deve)
 {
@@ -211,10 +221,14 @@ void core_scsi3_ua_for_check_condition(
 	if (!nacl)
 		return;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[cmd->orig_fe_lun];
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, cmd->orig_fe_lun);
+	if (!deve) {
+		rcu_read_unlock();
+		return;
+	}
 	if (!atomic_read(&deve->ua_count)) {
-		spin_unlock_irq(&nacl->device_list_lock);
+		rcu_read_unlock();
 		return;
 	}
 	/*
@@ -250,10 +264,10 @@ void core_scsi3_ua_for_check_condition(
 		atomic_dec_mb(&deve->ua_count);
 	}
 	spin_unlock(&deve->ua_lock);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 
 	pr_debug("[%s]: %s UNIT ATTENTION condition with"
-		" INTLCK_CTRL: %d, mapped LUN: %u, got CDB: 0x%02x"
+		" INTLCK_CTRL: %d, mapped LUN: %llu, got CDB: 0x%02x"
 		" reported ASC: 0x%02x, ASCQ: 0x%02x\n",
 		nacl->se_tpg->se_tpg_tfo->get_fabric_name(),
 		(dev->dev_attrib.emulate_ua_intlck_ctrl != 0) ? "Reporting" :
@@ -279,10 +293,14 @@ int core_scsi3_ua_clear_for_request_sense(
 	if (!nacl)
 		return -EINVAL;
 
-	spin_lock_irq(&nacl->device_list_lock);
-	deve = nacl->device_list[cmd->orig_fe_lun];
+	rcu_read_lock();
+	deve = target_nacl_find_deve(nacl, cmd->orig_fe_lun);
+	if (!deve) {
+		rcu_read_unlock();
+		return -EINVAL;
+	}
 	if (!atomic_read(&deve->ua_count)) {
-		spin_unlock_irq(&nacl->device_list_lock);
+		rcu_read_unlock();
 		return -EPERM;
 	}
 	/*
@@ -308,10 +326,10 @@ int core_scsi3_ua_clear_for_request_sense(
 		atomic_dec_mb(&deve->ua_count);
 	}
 	spin_unlock(&deve->ua_lock);
-	spin_unlock_irq(&nacl->device_list_lock);
+	rcu_read_unlock();
 
 	pr_debug("[%s]: Released UNIT ATTENTION condition, mapped"
-		" LUN: %u, got REQUEST_SENSE reported ASC: 0x%02x,"
+		" LUN: %llu, got REQUEST_SENSE reported ASC: 0x%02x,"
 		" ASCQ: 0x%02x\n", nacl->se_tpg->se_tpg_tfo->get_fabric_name(),
 		cmd->orig_fe_lun, *asc, *ascq);
 
diff --git a/drivers/target/target_core_ua.h b/drivers/target/target_core_ua.h
index a6b56b364e7a..bd6e78ba153d 100644
--- a/drivers/target/target_core_ua.h
+++ b/drivers/target/target_core_ua.h
@@ -25,10 +25,14 @@
 
 #define ASCQ_2CH_PREVIOUS_RESERVATION_CONFLICT_STATUS		0x09
 
+#define ASCQ_3FH_INQUIRY_DATA_HAS_CHANGED			0x03
+#define ASCQ_3FH_REPORTED_LUNS_DATA_HAS_CHANGED			0x0E
+
 extern struct kmem_cache *se_ua_cache;
 
 extern sense_reason_t target_scsi3_ua_check(struct se_cmd *);
-extern int core_scsi3_ua_allocate(struct se_node_acl *, u32, u8, u8);
+extern int core_scsi3_ua_allocate(struct se_dev_entry *, u8, u8);
+extern void target_ua_allocate_lun(struct se_node_acl *, u32, u8, u8);
 extern void core_scsi3_ua_release_all(struct se_dev_entry *);
 extern void core_scsi3_ua_for_check_condition(struct se_cmd *, u8 *, u8 *);
 extern int core_scsi3_ua_clear_for_request_sense(struct se_cmd *,
diff --git a/drivers/target/target_core_user.c b/drivers/target/target_core_user.c
index 07d2996d8c1f..c448ef421ce7 100644
--- a/drivers/target/target_core_user.c
+++ b/drivers/target/target_core_user.c
@@ -1,6 +1,7 @@
 /*
  * Copyright (C) 2013 Shaohua Li <shli@kernel.org>
  * Copyright (C) 2014 Red Hat, Inc.
+ * Copyright (C) 2015 Arrikto, Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -19,16 +20,17 @@
 #include <linux/spinlock.h>
 #include <linux/module.h>
 #include <linux/idr.h>
+#include <linux/kernel.h>
 #include <linux/timer.h>
 #include <linux/parser.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
+#include <linux/vmalloc.h>
 #include <linux/uio_driver.h>
 #include <net/genetlink.h>
+#include <scsi/scsi_common.h>
+#include <scsi/scsi_proto.h>
 #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>
 
@@ -166,6 +168,11 @@ static struct tcmu_cmd *tcmu_alloc_cmd(struct se_cmd *se_cmd)
 	tcmu_cmd->tcmu_dev = udev;
 	tcmu_cmd->data_length = se_cmd->data_length;
 
+	if (se_cmd->se_cmd_flags & SCF_BIDI) {
+		BUG_ON(!(se_cmd->t_bidi_data_sg && se_cmd->t_bidi_data_nents));
+		tcmu_cmd->data_length += se_cmd->t_bidi_data_sg->length;
+	}
+
 	tcmu_cmd->deadline = jiffies + msecs_to_jiffies(TCMU_TIME_OUT);
 
 	idr_preload(GFP_KERNEL);
@@ -224,9 +231,106 @@ static inline size_t head_to_end(size_t head, size_t size)
 
 #define UPDATE_HEAD(head, used, size) smp_store_release(&head, ((head % size) + used) % size)
 
+static void alloc_and_scatter_data_area(struct tcmu_dev *udev,
+	struct scatterlist *data_sg, unsigned int data_nents,
+	struct iovec **iov, int *iov_cnt, bool copy_data)
+{
+	int i;
+	void *from, *to;
+	size_t copy_bytes;
+	struct scatterlist *sg;
+
+	for_each_sg(data_sg, sg, data_nents, i) {
+		copy_bytes = min_t(size_t, sg->length,
+				 head_to_end(udev->data_head, udev->data_size));
+		from = kmap_atomic(sg_page(sg)) + sg->offset;
+		to = (void *) udev->mb_addr + udev->data_off + udev->data_head;
+
+		if (copy_data) {
+			memcpy(to, from, copy_bytes);
+			tcmu_flush_dcache_range(to, copy_bytes);
+		}
+
+		/* Even iov_base is relative to mb_addr */
+		(*iov)->iov_len = copy_bytes;
+		(*iov)->iov_base = (void __user *) udev->data_off +
+						udev->data_head;
+		(*iov_cnt)++;
+		(*iov)++;
+
+		UPDATE_HEAD(udev->data_head, copy_bytes, udev->data_size);
+
+		/* Uh oh, we wrapped the buffer. Must split sg across 2 iovs. */
+		if (sg->length != copy_bytes) {
+			void *from_skip = from + copy_bytes;
+
+			copy_bytes = sg->length - copy_bytes;
+
+			(*iov)->iov_len = copy_bytes;
+			(*iov)->iov_base = (void __user *) udev->data_off +
+							udev->data_head;
+
+			if (copy_data) {
+				to = (void *) udev->mb_addr +
+					udev->data_off + udev->data_head;
+				memcpy(to, from_skip, copy_bytes);
+				tcmu_flush_dcache_range(to, copy_bytes);
+			}
+
+			(*iov_cnt)++;
+			(*iov)++;
+
+			UPDATE_HEAD(udev->data_head,
+				copy_bytes, udev->data_size);
+		}
+
+		kunmap_atomic(from - sg->offset);
+	}
+}
+
+static void gather_and_free_data_area(struct tcmu_dev *udev,
+	struct scatterlist *data_sg, unsigned int data_nents)
+{
+	int i;
+	void *from, *to;
+	size_t copy_bytes;
+	struct scatterlist *sg;
+
+	/* It'd be easier to look at entry's iovec again, but UAM */
+	for_each_sg(data_sg, sg, data_nents, i) {
+		copy_bytes = min_t(size_t, sg->length,
+				 head_to_end(udev->data_tail, udev->data_size));
+
+		to = kmap_atomic(sg_page(sg)) + sg->offset;
+		WARN_ON(sg->length + sg->offset > PAGE_SIZE);
+		from = (void *) udev->mb_addr +
+			udev->data_off + udev->data_tail;
+		tcmu_flush_dcache_range(from, copy_bytes);
+		memcpy(to, from, copy_bytes);
+
+		UPDATE_HEAD(udev->data_tail, copy_bytes, udev->data_size);
+
+		/* Uh oh, wrapped the data buffer for this sg's data */
+		if (sg->length != copy_bytes) {
+			void *to_skip = to + copy_bytes;
+
+			from = (void *) udev->mb_addr +
+				udev->data_off + udev->data_tail;
+			WARN_ON(udev->data_tail);
+			copy_bytes = sg->length - copy_bytes;
+			tcmu_flush_dcache_range(from, copy_bytes);
+			memcpy(to_skip, from, copy_bytes);
+
+			UPDATE_HEAD(udev->data_tail,
+				copy_bytes, udev->data_size);
+		}
+		kunmap_atomic(to - sg->offset);
+	}
+}
+
 /*
- * We can't queue a command until we have space available on the cmd ring *and* space
- * space avail on the data ring.
+ * We can't queue a command until we have space available on the cmd ring *and*
+ * space available on the data ring.
  *
  * Called with ring lock held.
  */
@@ -274,12 +378,11 @@ static int tcmu_queue_cmd_ring(struct tcmu_cmd *tcmu_cmd)
 	size_t base_command_size, command_size;
 	struct tcmu_mailbox *mb;
 	struct tcmu_cmd_entry *entry;
-	int i;
-	struct scatterlist *sg;
 	struct iovec *iov;
-	int iov_cnt = 0;
+	int iov_cnt;
 	uint32_t cmd_head;
 	uint64_t cdb_off;
+	bool copy_to_data_area;
 
 	if (test_bit(TCMU_DEV_BIT_BROKEN, &udev->flags))
 		return -EINVAL;
@@ -292,7 +395,8 @@ static int tcmu_queue_cmd_ring(struct tcmu_cmd *tcmu_cmd)
 	 * b/c size == offsetof one-past-element.
 	*/
 	base_command_size = max(offsetof(struct tcmu_cmd_entry,
-					 req.iov[se_cmd->t_data_nents + 2]),
+					 req.iov[se_cmd->t_bidi_data_nents +
+						 se_cmd->t_data_nents + 2]),
 				sizeof(struct tcmu_cmd_entry));
 	command_size = base_command_size
 		+ round_up(scsi_command_size(se_cmd->t_task_cdb), TCMU_OP_ALIGN_SIZE);
@@ -360,53 +464,20 @@ static int tcmu_queue_cmd_ring(struct tcmu_cmd *tcmu_cmd)
 	 * Fix up iovecs, and handle if allocation in data ring wrapped.
 	 */
 	iov = &entry->req.iov[0];
-	for_each_sg(se_cmd->t_data_sg, sg, se_cmd->t_data_nents, i) {
-		size_t copy_bytes = min((size_t)sg->length,
-				     head_to_end(udev->data_head, udev->data_size));
-		void *from = kmap_atomic(sg_page(sg)) + sg->offset;
-		void *to = (void *) mb + udev->data_off + udev->data_head;
-
-		if (tcmu_cmd->se_cmd->data_direction == DMA_TO_DEVICE) {
-			memcpy(to, from, copy_bytes);
-			tcmu_flush_dcache_range(to, copy_bytes);
-		}
-
-		/* Even iov_base is relative to mb_addr */
-		iov->iov_len = copy_bytes;
-		iov->iov_base = (void __user *) udev->data_off +
-						udev->data_head;
-		iov_cnt++;
-		iov++;
-
-		UPDATE_HEAD(udev->data_head, copy_bytes, udev->data_size);
-
-		/* Uh oh, we wrapped the buffer. Must split sg across 2 iovs. */
-		if (sg->length != copy_bytes) {
-			from += copy_bytes;
-			copy_bytes = sg->length - copy_bytes;
-
-			iov->iov_len = copy_bytes;
-			iov->iov_base = (void __user *) udev->data_off +
-							udev->data_head;
-
-			if (se_cmd->data_direction == DMA_TO_DEVICE) {
-				to = (void *) mb + udev->data_off + udev->data_head;
-				memcpy(to, from, copy_bytes);
-				tcmu_flush_dcache_range(to, copy_bytes);
-			}
-
-			iov_cnt++;
-			iov++;
-
-			UPDATE_HEAD(udev->data_head, copy_bytes, udev->data_size);
-		}
-
-		kunmap_atomic(from);
-	}
+	iov_cnt = 0;
+	copy_to_data_area = (se_cmd->data_direction == DMA_TO_DEVICE
+		|| se_cmd->se_cmd_flags & SCF_BIDI);
+	alloc_and_scatter_data_area(udev, se_cmd->t_data_sg,
+		se_cmd->t_data_nents, &iov, &iov_cnt, copy_to_data_area);
 	entry->req.iov_cnt = iov_cnt;
-	entry->req.iov_bidi_cnt = 0;
 	entry->req.iov_dif_cnt = 0;
 
+	/* Handle BIDI commands */
+	iov_cnt = 0;
+	alloc_and_scatter_data_area(udev, se_cmd->t_bidi_data_sg,
+		se_cmd->t_bidi_data_nents, &iov, &iov_cnt, false);
+	entry->req.iov_bidi_cnt = iov_cnt;
+
 	/* All offsets relative to mb_addr, not start of entry! */
 	cdb_off = CMDR_OFF + cmd_head + base_command_size;
 	memcpy((void *) mb + cdb_off, se_cmd->t_task_cdb, scsi_command_size(se_cmd->t_task_cdb));
@@ -479,47 +550,22 @@ static void tcmu_handle_completion(struct tcmu_cmd *cmd, struct tcmu_cmd_entry *
 			       se_cmd->scsi_sense_length);
 
 		UPDATE_HEAD(udev->data_tail, cmd->data_length, udev->data_size);
-	}
-	else if (se_cmd->data_direction == DMA_FROM_DEVICE) {
-		struct scatterlist *sg;
-		int i;
-
-		/* It'd be easier to look at entry's iovec again, but UAM */
-		for_each_sg(se_cmd->t_data_sg, sg, se_cmd->t_data_nents, i) {
-			size_t copy_bytes;
-			void *to;
-			void *from;
-
-			copy_bytes = min((size_t)sg->length,
-					 head_to_end(udev->data_tail, udev->data_size));
-
-			to = kmap_atomic(sg_page(sg)) + sg->offset;
-			WARN_ON(sg->length + sg->offset > PAGE_SIZE);
-			from = (void *) udev->mb_addr + udev->data_off + udev->data_tail;
-			tcmu_flush_dcache_range(from, copy_bytes);
-			memcpy(to, from, copy_bytes);
-
-			UPDATE_HEAD(udev->data_tail, copy_bytes, udev->data_size);
-
-			/* Uh oh, wrapped the data buffer for this sg's data */
-			if (sg->length != copy_bytes) {
-				from = (void *) udev->mb_addr + udev->data_off + udev->data_tail;
-				WARN_ON(udev->data_tail);
-				to += copy_bytes;
-				copy_bytes = sg->length - copy_bytes;
-				tcmu_flush_dcache_range(from, copy_bytes);
-				memcpy(to, from, copy_bytes);
-
-				UPDATE_HEAD(udev->data_tail, copy_bytes, udev->data_size);
-			}
-
-			kunmap_atomic(to);
-		}
-
+	} else if (se_cmd->se_cmd_flags & SCF_BIDI) {
+		/* Discard data_out buffer */
+		UPDATE_HEAD(udev->data_tail,
+			(size_t)se_cmd->t_data_sg->length, udev->data_size);
+
+		/* Get Data-In buffer */
+		gather_and_free_data_area(udev,
+			se_cmd->t_bidi_data_sg, se_cmd->t_bidi_data_nents);
+	} else if (se_cmd->data_direction == DMA_FROM_DEVICE) {
+		gather_and_free_data_area(udev,
+			se_cmd->t_data_sg, se_cmd->t_data_nents);
 	} else if (se_cmd->data_direction == DMA_TO_DEVICE) {
 		UPDATE_HEAD(udev->data_tail, cmd->data_length, udev->data_size);
-	} else {
-		pr_warn("TCMU: data direction was %d!\n", se_cmd->data_direction);
+	} else if (se_cmd->data_direction != DMA_NONE) {
+		pr_warn("TCMU: data direction was %d!\n",
+			se_cmd->data_direction);
 	}
 
 	target_complete_cmd(cmd->se_cmd, entry->rsp.scsi_status);
@@ -908,6 +954,14 @@ static int tcmu_check_pending_cmd(int id, void *p, void *data)
 	return -EINVAL;
 }
 
+static void tcmu_dev_call_rcu(struct rcu_head *p)
+{
+	struct se_device *dev = container_of(p, struct se_device, rcu_head);
+	struct tcmu_dev *udev = TCMU_DEV(dev);
+
+	kfree(udev);
+}
+
 static void tcmu_free_device(struct se_device *dev)
 {
 	struct tcmu_dev *udev = TCMU_DEV(dev);
@@ -933,8 +987,7 @@ static void tcmu_free_device(struct se_device *dev)
 		kfree(udev->uio_info.name);
 		kfree(udev->name);
 	}
-
-	kfree(udev);
+	call_rcu(&dev->rcu_head, tcmu_dev_call_rcu);
 }
 
 enum {
@@ -1052,27 +1105,7 @@ tcmu_parse_cdb(struct se_cmd *cmd)
 	return passthrough_parse_cdb(cmd, tcmu_pass_op);
 }
 
-DEF_TB_DEV_ATTRIB_RO(tcmu, hw_pi_prot_type);
-TB_DEV_ATTR_RO(tcmu, hw_pi_prot_type);
-
-DEF_TB_DEV_ATTRIB_RO(tcmu, hw_block_size);
-TB_DEV_ATTR_RO(tcmu, hw_block_size);
-
-DEF_TB_DEV_ATTRIB_RO(tcmu, hw_max_sectors);
-TB_DEV_ATTR_RO(tcmu, hw_max_sectors);
-
-DEF_TB_DEV_ATTRIB_RO(tcmu, hw_queue_depth);
-TB_DEV_ATTR_RO(tcmu, hw_queue_depth);
-
-static struct configfs_attribute *tcmu_backend_dev_attrs[] = {
-	&tcmu_dev_attrib_hw_pi_prot_type.attr,
-	&tcmu_dev_attrib_hw_block_size.attr,
-	&tcmu_dev_attrib_hw_max_sectors.attr,
-	&tcmu_dev_attrib_hw_queue_depth.attr,
-	NULL,
-};
-
-static struct se_subsystem_api tcmu_template = {
+static const struct target_backend_ops tcmu_ops = {
 	.name			= "user",
 	.inquiry_prod		= "USER",
 	.inquiry_rev		= TCMU_VERSION,
@@ -1088,11 +1121,11 @@ static struct se_subsystem_api tcmu_template = {
 	.show_configfs_dev_params = tcmu_show_configfs_dev_params,
 	.get_device_type	= sbc_get_device_type,
 	.get_blocks		= tcmu_get_blocks,
+	.tb_dev_attrib_attrs	= passthrough_attrib_attrs,
 };
 
 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);
@@ -1115,10 +1148,7 @@ 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);
+	ret = transport_backend_register(&tcmu_ops);
 	if (ret)
 		goto out_unreg_genl;
 
@@ -1136,7 +1166,7 @@ out_free_cache:
 
 static void __exit tcmu_module_exit(void)
 {
-	transport_subsystem_release(&tcmu_template);
+	target_backend_unregister(&tcmu_ops);
 	genl_unregister_family(&tcmu_genl_family);
 	root_device_unregister(tcmu_root_device);
 	kmem_cache_destroy(tcmu_cmd_cache);
diff --git a/drivers/target/target_core_xcopy.c b/drivers/target/target_core_xcopy.c
index 8fd680ac941b..4515f52546f8 100644
--- a/drivers/target/target_core_xcopy.c
+++ b/drivers/target/target_core_xcopy.c
@@ -25,14 +25,12 @@
 #include <linux/spinlock.h>
 #include <linux/list.h>
 #include <linux/configfs.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_proto.h>
 #include <asm/unaligned.h>
 
 #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"
 #include "target_core_pr.h"
@@ -349,8 +347,7 @@ struct xcopy_pt_cmd {
 	unsigned char sense_buffer[TRANSPORT_SENSE_BUFFER];
 };
 
-static struct se_port xcopy_pt_port;
-static struct se_portal_group xcopy_pt_tpg;
+struct se_portal_group xcopy_pt_tpg;
 static struct se_session xcopy_pt_sess;
 static struct se_node_acl xcopy_pt_nacl;
 
@@ -359,11 +356,6 @@ static char *xcopy_pt_get_fabric_name(void)
         return "xcopy-pt";
 }
 
-static u32 xcopy_pt_get_tag(struct se_cmd *se_cmd)
-{
-        return 0;
-}
-
 static int xcopy_pt_get_cmd_state(struct se_cmd *se_cmd)
 {
         return 0;
@@ -424,7 +416,6 @@ static int xcopy_pt_queue_status(struct se_cmd *se_cmd)
 
 static const struct target_core_fabric_ops xcopy_pt_tfo = {
 	.get_fabric_name	= xcopy_pt_get_fabric_name,
-	.get_task_tag		= xcopy_pt_get_tag,
 	.get_cmd_state		= xcopy_pt_get_cmd_state,
 	.release_cmd		= xcopy_pt_release_cmd,
 	.check_stop_free	= xcopy_pt_check_stop_free,
@@ -446,17 +437,11 @@ int target_xcopy_setup_pt(void)
 		return -ENOMEM;
 	}
 
-	memset(&xcopy_pt_port, 0, sizeof(struct se_port));
-	INIT_LIST_HEAD(&xcopy_pt_port.sep_alua_list);
-	INIT_LIST_HEAD(&xcopy_pt_port.sep_list);
-	mutex_init(&xcopy_pt_port.sep_tg_pt_md_mutex);
-
 	memset(&xcopy_pt_tpg, 0, sizeof(struct se_portal_group));
 	INIT_LIST_HEAD(&xcopy_pt_tpg.se_tpg_node);
 	INIT_LIST_HEAD(&xcopy_pt_tpg.acl_node_list);
 	INIT_LIST_HEAD(&xcopy_pt_tpg.tpg_sess_list);
 
-	xcopy_pt_port.sep_tpg = &xcopy_pt_tpg;
 	xcopy_pt_tpg.se_tpg_tfo = &xcopy_pt_tfo;
 
 	memset(&xcopy_pt_nacl, 0, sizeof(struct se_node_acl));
@@ -497,10 +482,6 @@ static void target_xcopy_setup_pt_port(
 		 */
 		if (remote_port) {
 			xpt_cmd->remote_port = remote_port;
-			pt_cmd->se_lun->lun_sep = &xcopy_pt_port;
-			pr_debug("Setup emulated remote DEST xcopy_pt_port: %p to"
-				" cmd->se_lun->lun_sep for X-COPY data PUSH\n",
-				pt_cmd->se_lun->lun_sep);
 		} else {
 			pt_cmd->se_lun = ec_cmd->se_lun;
 			pt_cmd->se_dev = ec_cmd->se_dev;
@@ -520,10 +501,6 @@ static void target_xcopy_setup_pt_port(
 		 */
 		if (remote_port) {
 			xpt_cmd->remote_port = remote_port;
-			pt_cmd->se_lun->lun_sep = &xcopy_pt_port;
-			pr_debug("Setup emulated remote SRC xcopy_pt_port: %p to"
-				" cmd->se_lun->lun_sep for X-COPY data PULL\n",
-				pt_cmd->se_lun->lun_sep);
 		} else {
 			pt_cmd->se_lun = ec_cmd->se_lun;
 			pt_cmd->se_dev = ec_cmd->se_dev;
@@ -575,6 +552,7 @@ static int target_xcopy_setup_pt_cmd(
 	xpt_cmd->xcopy_op = xop;
 	target_xcopy_setup_pt_port(xpt_cmd, xop, remote_port);
 
+	cmd->tag = 0;
 	sense_rc = target_setup_cmd_from_cdb(cmd, cdb);
 	if (sense_rc) {
 		ret = -EINVAL;
diff --git a/drivers/target/tcm_fc/tcm_fc.h b/drivers/target/tcm_fc/tcm_fc.h
index 881deb3d499a..39909dadef3e 100644
--- a/drivers/target/tcm_fc/tcm_fc.h
+++ b/drivers/target/tcm_fc/tcm_fc.h
@@ -80,8 +80,8 @@ struct ft_node_auth {
  * Node ACL for FC remote port session.
  */
 struct ft_node_acl {
-	struct ft_node_auth node_auth;
 	struct se_node_acl se_node_acl;
+	struct ft_node_auth node_auth;
 };
 
 struct ft_lun {
@@ -157,7 +157,6 @@ int ft_queue_status(struct se_cmd *);
 int ft_queue_data_in(struct se_cmd *);
 int ft_write_pending(struct se_cmd *);
 int ft_write_pending_status(struct se_cmd *);
-u32 ft_get_task_tag(struct se_cmd *);
 int ft_get_cmd_state(struct se_cmd *);
 void ft_queue_tm_resp(struct se_cmd *);
 void ft_aborted_task(struct se_cmd *);
diff --git a/drivers/target/tcm_fc/tfc_cmd.c b/drivers/target/tcm_fc/tfc_cmd.c
index edcafa4490c0..68031723e5be 100644
--- a/drivers/target/tcm_fc/tfc_cmd.c
+++ b/drivers/target/tcm_fc/tfc_cmd.c
@@ -30,17 +30,12 @@
 #include <linux/hash.h>
 #include <linux/percpu_ida.h>
 #include <asm/unaligned.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_tcq.h>
 #include <scsi/libfc.h>
 #include <scsi/fc_encode.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 
 #include "tcm_fc.h"
@@ -247,15 +242,6 @@ int ft_write_pending(struct se_cmd *se_cmd)
 	return 0;
 }
 
-u32 ft_get_task_tag(struct se_cmd *se_cmd)
-{
-	struct ft_cmd *cmd = container_of(se_cmd, struct ft_cmd, se_cmd);
-
-	if (cmd->aborted)
-		return ~0;
-	return fc_seq_exch(cmd->seq)->rxid;
-}
-
 int ft_get_cmd_state(struct se_cmd *se_cmd)
 {
 	return 0;
@@ -568,6 +554,7 @@ static void ft_send_work(struct work_struct *work)
 	}
 
 	fc_seq_exch(cmd->seq)->lp->tt.seq_set_resp(cmd->seq, ft_recv_seq, cmd);
+	cmd->se_cmd.tag = fc_seq_exch(cmd->seq)->rxid;
 	/*
 	 * Use a single se_cmd->cmd_kref as we expect to release se_cmd
 	 * directly from ft_check_stop_free callback in response path.
diff --git a/drivers/target/tcm_fc/tfc_conf.c b/drivers/target/tcm_fc/tfc_conf.c
index 65dce1345966..16670933013b 100644
--- a/drivers/target/tcm_fc/tfc_conf.c
+++ b/drivers/target/tcm_fc/tfc_conf.c
@@ -34,22 +34,15 @@
 #include <linux/kernel.h>
 #include <linux/ctype.h>
 #include <asm/unaligned.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_cmnd.h>
 #include <scsi/libfc.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 
 #include "tcm_fc.h"
 
-static const struct target_core_fabric_ops ft_fabric_ops;
-
 static LIST_HEAD(ft_wwn_list);
 DEFINE_MUTEX(ft_lport_lock);
 
@@ -198,48 +191,17 @@ static struct configfs_attribute *ft_nacl_base_attrs[] = {
  * Add ACL for an initiator.  The ACL is named arbitrarily.
  * The port_name and/or node_name are attributes.
  */
-static struct se_node_acl *ft_add_acl(
-	struct se_portal_group *se_tpg,
-	struct config_group *group,
-	const char *name)
+static int ft_init_nodeacl(struct se_node_acl *nacl, const char *name)
 {
-	struct ft_node_acl *acl;
-	struct ft_tpg *tpg;
+	struct ft_node_acl *acl =
+		container_of(nacl, struct ft_node_acl, se_node_acl);
 	u64 wwpn;
-	u32 q_depth;
-
-	pr_debug("add acl %s\n", name);
-	tpg = container_of(se_tpg, struct ft_tpg, se_tpg);
 
 	if (ft_parse_wwn(name, &wwpn, 1) < 0)
-		return ERR_PTR(-EINVAL);
+		return -EINVAL;
 
-	acl = kzalloc(sizeof(struct ft_node_acl), GFP_KERNEL);
-	if (!acl)
-		return ERR_PTR(-ENOMEM);
 	acl->node_auth.port_name = wwpn;
-
-	q_depth = 32;		/* XXX bogus default - get from tpg? */
-	return core_tpg_add_initiator_node_acl(&tpg->se_tpg,
-				&acl->se_node_acl, name, q_depth);
-}
-
-static void ft_del_acl(struct se_node_acl *se_acl)
-{
-	struct se_portal_group *se_tpg = se_acl->se_tpg;
-	struct ft_tpg *tpg;
-	struct ft_node_acl *acl = container_of(se_acl,
-				struct ft_node_acl, se_node_acl);
-
-	pr_debug("del acl %s\n",
-		config_item_name(&se_acl->acl_group.cg_item));
-
-	tpg = container_of(se_tpg, struct ft_tpg, se_tpg);
-	pr_debug("del acl %p se_acl %p tpg %p se_tpg %p\n",
-		    acl, se_acl, tpg, &tpg->se_tpg);
-
-	core_tpg_del_initiator_node_acl(&tpg->se_tpg, se_acl, 1);
-	kfree(acl);
+	return 0;
 }
 
 struct ft_node_acl *ft_acl_get(struct ft_tpg *tpg, struct fc_rport_priv *rdata)
@@ -249,7 +211,7 @@ struct ft_node_acl *ft_acl_get(struct ft_tpg *tpg, struct fc_rport_priv *rdata)
 	struct se_portal_group *se_tpg = &tpg->se_tpg;
 	struct se_node_acl *se_acl;
 
-	spin_lock_irq(&se_tpg->acl_node_lock);
+	mutex_lock(&se_tpg->acl_node_mutex);
 	list_for_each_entry(se_acl, &se_tpg->acl_node_list, acl_list) {
 		acl = container_of(se_acl, struct ft_node_acl, se_node_acl);
 		pr_debug("acl %p port_name %llx\n",
@@ -263,33 +225,10 @@ struct ft_node_acl *ft_acl_get(struct ft_tpg *tpg, struct fc_rport_priv *rdata)
 			break;
 		}
 	}
-	spin_unlock_irq(&se_tpg->acl_node_lock);
+	mutex_unlock(&se_tpg->acl_node_mutex);
 	return found;
 }
 
-static struct se_node_acl *ft_tpg_alloc_fabric_acl(struct se_portal_group *se_tpg)
-{
-	struct ft_node_acl *acl;
-
-	acl = kzalloc(sizeof(*acl), GFP_KERNEL);
-	if (!acl) {
-		pr_err("Unable to allocate struct ft_node_acl\n");
-		return NULL;
-	}
-	pr_debug("acl %p\n", acl);
-	return &acl->se_node_acl;
-}
-
-static void ft_tpg_release_fabric_acl(struct se_portal_group *se_tpg,
-				      struct se_node_acl *se_acl)
-{
-	struct ft_node_acl *acl = container_of(se_acl,
-				struct ft_node_acl, se_node_acl);
-
-	pr_debug("acl %p\n", acl);
-	kfree(acl);
-}
-
 /*
  * local_port port_group (tpg) ops.
  */
@@ -337,8 +276,7 @@ static struct se_portal_group *ft_add_tpg(
 		return NULL;
 	}
 
-	ret = core_tpg_register(&ft_fabric_ops, wwn, &tpg->se_tpg,
-				tpg, TRANSPORT_TPG_TYPE_NORMAL);
+	ret = core_tpg_register(wwn, &tpg->se_tpg, SCSI_PROTOCOL_FCP);
 	if (ret < 0) {
 		destroy_workqueue(wq);
 		kfree(tpg);
@@ -463,6 +401,11 @@ static struct configfs_attribute *ft_wwn_attrs[] = {
 	NULL,
 };
 
+static inline struct ft_tpg *ft_tpg(struct se_portal_group *se_tpg)
+{
+	return container_of(se_tpg, struct ft_tpg, se_tpg);
+}
+
 static char *ft_get_fabric_name(void)
 {
 	return "fc";
@@ -470,25 +413,16 @@ static char *ft_get_fabric_name(void)
 
 static char *ft_get_fabric_wwn(struct se_portal_group *se_tpg)
 {
-	struct ft_tpg *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->lport_wwn->name;
+	return ft_tpg(se_tpg)->lport_wwn->name;
 }
 
 static u16 ft_get_tag(struct se_portal_group *se_tpg)
 {
-	struct ft_tpg *tpg = se_tpg->se_tpg_fabric_ptr;
-
 	/*
 	 * This tag is used when forming SCSI Name identifier in EVPD=1 0x83
 	 * to represent the SCSI Target Port.
 	 */
-	return tpg->index;
-}
-
-static u32 ft_get_default_depth(struct se_portal_group *se_tpg)
-{
-	return 1;
+	return ft_tpg(se_tpg)->index;
 }
 
 static int ft_check_false(struct se_portal_group *se_tpg)
@@ -502,28 +436,20 @@ static void ft_set_default_node_attr(struct se_node_acl *se_nacl)
 
 static u32 ft_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
-	struct ft_tpg *tpg = se_tpg->se_tpg_fabric_ptr;
-
-	return tpg->index;
+	return ft_tpg(se_tpg)->index;
 }
 
 static const struct target_core_fabric_ops ft_fabric_ops = {
 	.module =			THIS_MODULE,
 	.name =				"fc",
+	.node_acl_size =		sizeof(struct ft_node_acl),
 	.get_fabric_name =		ft_get_fabric_name,
-	.get_fabric_proto_ident =	fc_get_fabric_proto_ident,
 	.tpg_get_wwn =			ft_get_fabric_wwn,
 	.tpg_get_tag =			ft_get_tag,
-	.tpg_get_default_depth =	ft_get_default_depth,
-	.tpg_get_pr_transport_id =	fc_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len =	fc_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id = fc_parse_pr_out_transport_id,
 	.tpg_check_demo_mode =		ft_check_false,
 	.tpg_check_demo_mode_cache =	ft_check_false,
 	.tpg_check_demo_mode_write_protect = ft_check_false,
 	.tpg_check_prod_mode_write_protect = ft_check_false,
-	.tpg_alloc_fabric_acl =		ft_tpg_alloc_fabric_acl,
-	.tpg_release_fabric_acl =	ft_tpg_release_fabric_acl,
 	.tpg_get_inst_index =		ft_tpg_get_inst_index,
 	.check_stop_free =		ft_check_stop_free,
 	.release_cmd =			ft_release_cmd,
@@ -534,7 +460,6 @@ static const struct target_core_fabric_ops ft_fabric_ops = {
 	.write_pending =		ft_write_pending,
 	.write_pending_status =		ft_write_pending_status,
 	.set_default_node_attributes =	ft_set_default_node_attr,
-	.get_task_tag =			ft_get_task_tag,
 	.get_cmd_state =		ft_get_cmd_state,
 	.queue_data_in =		ft_queue_data_in,
 	.queue_status =			ft_queue_status,
@@ -548,12 +473,7 @@ static const struct target_core_fabric_ops ft_fabric_ops = {
 	.fabric_drop_wwn =		&ft_del_wwn,
 	.fabric_make_tpg =		&ft_add_tpg,
 	.fabric_drop_tpg =		&ft_del_tpg,
-	.fabric_post_link =		NULL,
-	.fabric_pre_unlink =		NULL,
-	.fabric_make_np =		NULL,
-	.fabric_drop_np =		NULL,
-	.fabric_make_nodeacl =		&ft_add_acl,
-	.fabric_drop_nodeacl =		&ft_del_acl,
+	.fabric_init_nodeacl =		&ft_init_nodeacl,
 
 	.tfc_wwn_attrs			= ft_wwn_attrs,
 	.tfc_tpg_nacl_base_attrs	= ft_nacl_base_attrs,
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index 583e755d8091..4b0fedd6bd4b 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -39,16 +39,11 @@
 #include <linux/hash.h>
 #include <linux/ratelimit.h>
 #include <asm/unaligned.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_cmnd.h>
 #include <scsi/libfc.h>
 #include <scsi/fc_encode.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 
 #include "tcm_fc.h"
diff --git a/drivers/target/tcm_fc/tfc_sess.c b/drivers/target/tcm_fc/tfc_sess.c
index ccee7e332a4d..31a9e3fb98c5 100644
--- a/drivers/target/tcm_fc/tfc_sess.c
+++ b/drivers/target/tcm_fc/tfc_sess.c
@@ -32,15 +32,10 @@
 #include <linux/rculist.h>
 #include <linux/kref.h>
 #include <asm/unaligned.h>
-#include <scsi/scsi.h>
-#include <scsi/scsi_host.h>
-#include <scsi/scsi_device.h>
-#include <scsi/scsi_cmnd.h>
 #include <scsi/libfc.h>
 
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 
 #include "tcm_fc.h"
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index af40db0df58e..118938ee8552 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -42,6 +42,17 @@ config THERMAL_OF
 	  Say 'Y' here if you need to build thermal infrastructure
 	  based on device tree.
 
+config THERMAL_WRITABLE_TRIPS
+	bool "Enable writable trip points"
+	help
+	  This option allows the system integrator to choose whether
+	  trip temperatures can be changed from userspace. The
+	  writable trips need to be specified when setting up the
+	  thermal zone but the choice here takes precedence.
+
+	  Say 'Y' here if you would like to allow userspace tools to
+	  change trip temperatures.
+
 choice
 	prompt "Default Thermal governor"
 	default THERMAL_DEFAULT_GOV_STEP_WISE
@@ -71,6 +82,14 @@ config THERMAL_DEFAULT_GOV_USER_SPACE
 	  Select this if you want to let the user space manage the
 	  platform thermals.
 
+config THERMAL_DEFAULT_GOV_POWER_ALLOCATOR
+	bool "power_allocator"
+	select THERMAL_GOV_POWER_ALLOCATOR
+	help
+	  Select this if you want to control temperature based on
+	  system and device power allocation. This governor can only
+	  operate on cooling devices that implement the power API.
+
 endchoice
 
 config THERMAL_GOV_FAIR_SHARE
@@ -99,6 +118,12 @@ config THERMAL_GOV_USER_SPACE
 	help
 	  Enable this to let the user space manage the platform thermals.
 
+config THERMAL_GOV_POWER_ALLOCATOR
+	bool "Power allocator thermal governor"
+	help
+	  Enable this to manage platform thermals by dynamically
+	  allocating and limiting power to devices.
+
 config CPU_THERMAL
 	bool "generic cpu cooling support"
 	depends on CPU_FREQ
@@ -136,6 +161,14 @@ config THERMAL_EMULATION
 	  because userland can easily disable the thermal policy by simply
 	  flooding this sysfs node with low temperature values.
 
+config HISI_THERMAL
+	tristate "Hisilicon thermal driver"
+	depends on ARCH_HISI && CPU_THERMAL && OF
+	help
+	  Enable this to plug hisilicon's thermal sensor driver into the Linux
+	  thermal framework. cpufreq is used as the cooling device to throttle
+	  CPUs when the passive trip is crossed.
+
 config IMX_THERMAL
 	tristate "Temperature sensor driver for Freescale i.MX SoCs"
 	depends on CPU_THERMAL
@@ -249,9 +282,20 @@ config X86_PKG_TEMP_THERMAL
 	  two trip points which can be set by user to get notifications via thermal
 	  notification methods.
 
+config INTEL_SOC_DTS_IOSF_CORE
+	tristate
+	depends on X86
+	select IOSF_MBI
+	help
+	  This is becoming a common feature for Intel SoCs to expose the additional
+	  digital temperature sensors (DTSs) using side band interface (IOSF). This
+	  implements the common set of helper functions to register, get temperature
+	  and get/set thresholds on DTSs.
+
 config INTEL_SOC_DTS_THERMAL
 	tristate "Intel SoCs DTS thermal driver"
-	depends on X86 && IOSF_MBI
+	depends on X86
+	select INTEL_SOC_DTS_IOSF_CORE
 	help
 	  Enable this to register Intel SoCs (e.g. Bay Trail) platform digital
 	  temperature sensor (DTS). These SoCs have two additional DTSs in
@@ -261,12 +305,23 @@ config INTEL_SOC_DTS_THERMAL
 	  notification methods.The other trip is a critical trip point, which
 	  was set by the driver based on the TJ MAX temperature.
 
+config INTEL_QUARK_DTS_THERMAL
+	tristate "Intel Quark DTS thermal driver"
+	depends on X86_INTEL_QUARK
+	help
+	  Enable this to register Intel Quark SoC (e.g. X1000) platform digital
+	  temperature sensor (DTS). For X1000 SoC, it has one on-die DTS.
+	  The DTS will be registered as a thermal zone. There are two trip points:
+	  hot & critical. The critical trip point default value is set by
+	  underlying BIOS/Firmware.
+
 config INT340X_THERMAL
 	tristate "ACPI INT340X thermal drivers"
 	depends on X86 && ACPI
 	select THERMAL_GOV_USER_SPACE
 	select ACPI_THERMAL_REL
 	select ACPI_FAN
+	select INTEL_SOC_DTS_IOSF_CORE
 	help
 	  Newer laptops and tablets that use ACPI may have thermal sensors and
 	  other devices with thermal control capabilities outside the core
@@ -299,4 +354,15 @@ depends on ARCH_STI && OF
 source "drivers/thermal/st/Kconfig"
 endmenu
 
+config QCOM_SPMI_TEMP_ALARM
+	tristate "Qualcomm SPMI PMIC Temperature Alarm"
+	depends on OF && SPMI && IIO
+	select REGMAP_SPMI
+	help
+	  This enables a thermal sysfs driver for Qualcomm plug-and-play (QPNP)
+	  PMIC devices. It shows up in sysfs as a thermal sensor with multiple
+	  trip points. The temperature reported by the thermal sensor reflects the
+	  real time die temperature if an ADC is present or an estimate of the
+	  temperature based upon the over temperature stage value.
+
 endif
diff --git a/drivers/thermal/Makefile b/drivers/thermal/Makefile
index fa0dc486790f..535dfee1496f 100644
--- a/drivers/thermal/Makefile
+++ b/drivers/thermal/Makefile
@@ -14,6 +14,7 @@ thermal_sys-$(CONFIG_THERMAL_GOV_FAIR_SHARE)	+= fair_share.o
 thermal_sys-$(CONFIG_THERMAL_GOV_BANG_BANG)	+= gov_bang_bang.o
 thermal_sys-$(CONFIG_THERMAL_GOV_STEP_WISE)	+= step_wise.o
 thermal_sys-$(CONFIG_THERMAL_GOV_USER_SPACE)	+= user_space.o
+thermal_sys-$(CONFIG_THERMAL_GOV_POWER_ALLOCATOR)	+= power_allocator.o
 
 # cpufreq cooling
 thermal_sys-$(CONFIG_CPU_THERMAL)	+= cpu_cooling.o
@@ -22,6 +23,7 @@ thermal_sys-$(CONFIG_CPU_THERMAL)	+= cpu_cooling.o
 thermal_sys-$(CONFIG_CLOCK_THERMAL)	+= clock_cooling.o
 
 # platform thermal drivers
+obj-$(CONFIG_QCOM_SPMI_TEMP_ALARM)	+= qcom-spmi-temp-alarm.o
 obj-$(CONFIG_SPEAR_THERMAL)	+= spear_thermal.o
 obj-$(CONFIG_ROCKCHIP_THERMAL)	+= rockchip_thermal.o
 obj-$(CONFIG_RCAR_THERMAL)	+= rcar_thermal.o
@@ -34,8 +36,11 @@ obj-$(CONFIG_IMX_THERMAL)	+= imx_thermal.o
 obj-$(CONFIG_DB8500_CPUFREQ_COOLING)	+= db8500_cpufreq_cooling.o
 obj-$(CONFIG_INTEL_POWERCLAMP)	+= intel_powerclamp.o
 obj-$(CONFIG_X86_PKG_TEMP_THERMAL)	+= x86_pkg_temp_thermal.o
+obj-$(CONFIG_INTEL_SOC_DTS_IOSF_CORE)	+= intel_soc_dts_iosf.o
 obj-$(CONFIG_INTEL_SOC_DTS_THERMAL)	+= intel_soc_dts_thermal.o
+obj-$(CONFIG_INTEL_QUARK_DTS_THERMAL)	+= intel_quark_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
+obj-$(CONFIG_HISI_THERMAL)     += hisi_thermal.o
diff --git a/drivers/thermal/cpu_cooling.c b/drivers/thermal/cpu_cooling.c
index f65f0d109fc8..6509c61b9648 100644
--- a/drivers/thermal/cpu_cooling.c
+++ b/drivers/thermal/cpu_cooling.c
@@ -26,10 +26,13 @@
 #include <linux/thermal.h>
 #include <linux/cpufreq.h>
 #include <linux/err.h>
+#include <linux/pm_opp.h>
 #include <linux/slab.h>
 #include <linux/cpu.h>
 #include <linux/cpu_cooling.h>
 
+#include <trace/events/thermal.h>
+
 /*
  * Cooling state <-> CPUFreq frequency
  *
@@ -45,6 +48,19 @@
  */
 
 /**
+ * struct power_table - frequency to power conversion
+ * @frequency:	frequency in KHz
+ * @power:	power in mW
+ *
+ * This structure is built when the cooling device registers and helps
+ * in translating frequency to power and viceversa.
+ */
+struct power_table {
+	u32 frequency;
+	u32 power;
+};
+
+/**
  * struct cpufreq_cooling_device - data for cooling device with cpufreq
  * @id: unique integer value corresponding to each cpufreq_cooling_device
  *	registered.
@@ -58,6 +74,15 @@
  *	cpufreq frequencies.
  * @allowed_cpus: all the cpus involved for this cpufreq_cooling_device.
  * @node: list_head to link all cpufreq_cooling_device together.
+ * @last_load: load measured by the latest call to cpufreq_get_actual_power()
+ * @time_in_idle: previous reading of the absolute time that this cpu was idle
+ * @time_in_idle_timestamp: wall time of the last invocation of
+ *	get_cpu_idle_time_us()
+ * @dyn_power_table: array of struct power_table for frequency to power
+ *	conversion, sorted in ascending order.
+ * @dyn_power_table_entries: number of entries in the @dyn_power_table array
+ * @cpu_dev: the first cpu_device from @allowed_cpus that has OPPs registered
+ * @plat_get_static_power: callback to calculate the static power
  *
  * This structure is required for keeping information of each registered
  * cpufreq_cooling_device.
@@ -71,6 +96,13 @@ struct cpufreq_cooling_device {
 	unsigned int *freq_table;	/* In descending order */
 	struct cpumask allowed_cpus;
 	struct list_head node;
+	u32 last_load;
+	u64 *time_in_idle;
+	u64 *time_in_idle_timestamp;
+	struct power_table *dyn_power_table;
+	int dyn_power_table_entries;
+	struct device *cpu_dev;
+	get_static_t plat_get_static_power;
 };
 static DEFINE_IDR(cpufreq_idr);
 static DEFINE_MUTEX(cooling_cpufreq_lock);
@@ -186,23 +218,237 @@ static int cpufreq_thermal_notifier(struct notifier_block *nb,
 	unsigned long max_freq = 0;
 	struct cpufreq_cooling_device *cpufreq_dev;
 
-	if (event != CPUFREQ_ADJUST)
-		return 0;
+	switch (event) {
 
-	mutex_lock(&cooling_cpufreq_lock);
-	list_for_each_entry(cpufreq_dev, &cpufreq_dev_list, node) {
-		if (!cpumask_test_cpu(policy->cpu,
-					&cpufreq_dev->allowed_cpus))
+	case CPUFREQ_ADJUST:
+		mutex_lock(&cooling_cpufreq_lock);
+		list_for_each_entry(cpufreq_dev, &cpufreq_dev_list, node) {
+			if (!cpumask_test_cpu(policy->cpu,
+					      &cpufreq_dev->allowed_cpus))
+				continue;
+
+			max_freq = cpufreq_dev->cpufreq_val;
+
+			if (policy->max != max_freq)
+				cpufreq_verify_within_limits(policy, 0,
+							     max_freq);
+		}
+		mutex_unlock(&cooling_cpufreq_lock);
+		break;
+	default:
+		return NOTIFY_DONE;
+	}
+
+	return NOTIFY_OK;
+}
+
+/**
+ * build_dyn_power_table() - create a dynamic power to frequency table
+ * @cpufreq_device:	the cpufreq cooling device in which to store the table
+ * @capacitance: dynamic power coefficient for these cpus
+ *
+ * Build a dynamic power to frequency table for this cpu and store it
+ * in @cpufreq_device.  This table will be used in cpu_power_to_freq() and
+ * cpu_freq_to_power() to convert between power and frequency
+ * efficiently.  Power is stored in mW, frequency in KHz.  The
+ * resulting table is in ascending order.
+ *
+ * Return: 0 on success, -E* on error.
+ */
+static int build_dyn_power_table(struct cpufreq_cooling_device *cpufreq_device,
+				 u32 capacitance)
+{
+	struct power_table *power_table;
+	struct dev_pm_opp *opp;
+	struct device *dev = NULL;
+	int num_opps = 0, cpu, i, ret = 0;
+	unsigned long freq;
+
+	rcu_read_lock();
+
+	for_each_cpu(cpu, &cpufreq_device->allowed_cpus) {
+		dev = get_cpu_device(cpu);
+		if (!dev) {
+			dev_warn(&cpufreq_device->cool_dev->device,
+				 "No cpu device for cpu %d\n", cpu);
 			continue;
+		}
+
+		num_opps = dev_pm_opp_get_opp_count(dev);
+		if (num_opps > 0) {
+			break;
+		} else if (num_opps < 0) {
+			ret = num_opps;
+			goto unlock;
+		}
+	}
 
-		max_freq = cpufreq_dev->cpufreq_val;
+	if (num_opps == 0) {
+		ret = -EINVAL;
+		goto unlock;
+	}
 
-		if (policy->max != max_freq)
-			cpufreq_verify_within_limits(policy, 0, max_freq);
+	power_table = kcalloc(num_opps, sizeof(*power_table), GFP_KERNEL);
+	if (!power_table) {
+		ret = -ENOMEM;
+		goto unlock;
 	}
-	mutex_unlock(&cooling_cpufreq_lock);
 
-	return 0;
+	for (freq = 0, i = 0;
+	     opp = dev_pm_opp_find_freq_ceil(dev, &freq), !IS_ERR(opp);
+	     freq++, i++) {
+		u32 freq_mhz, voltage_mv;
+		u64 power;
+
+		freq_mhz = freq / 1000000;
+		voltage_mv = dev_pm_opp_get_voltage(opp) / 1000;
+
+		/*
+		 * Do the multiplication with MHz and millivolt so as
+		 * to not overflow.
+		 */
+		power = (u64)capacitance * freq_mhz * voltage_mv * voltage_mv;
+		do_div(power, 1000000000);
+
+		/* frequency is stored in power_table in KHz */
+		power_table[i].frequency = freq / 1000;
+
+		/* power is stored in mW */
+		power_table[i].power = power;
+	}
+
+	if (i == 0) {
+		ret = PTR_ERR(opp);
+		goto unlock;
+	}
+
+	cpufreq_device->cpu_dev = dev;
+	cpufreq_device->dyn_power_table = power_table;
+	cpufreq_device->dyn_power_table_entries = i;
+
+unlock:
+	rcu_read_unlock();
+	return ret;
+}
+
+static u32 cpu_freq_to_power(struct cpufreq_cooling_device *cpufreq_device,
+			     u32 freq)
+{
+	int i;
+	struct power_table *pt = cpufreq_device->dyn_power_table;
+
+	for (i = 1; i < cpufreq_device->dyn_power_table_entries; i++)
+		if (freq < pt[i].frequency)
+			break;
+
+	return pt[i - 1].power;
+}
+
+static u32 cpu_power_to_freq(struct cpufreq_cooling_device *cpufreq_device,
+			     u32 power)
+{
+	int i;
+	struct power_table *pt = cpufreq_device->dyn_power_table;
+
+	for (i = 1; i < cpufreq_device->dyn_power_table_entries; i++)
+		if (power < pt[i].power)
+			break;
+
+	return pt[i - 1].frequency;
+}
+
+/**
+ * get_load() - get load for a cpu since last updated
+ * @cpufreq_device:	&struct cpufreq_cooling_device for this cpu
+ * @cpu:	cpu number
+ *
+ * Return: The average load of cpu @cpu in percentage since this
+ * function was last called.
+ */
+static u32 get_load(struct cpufreq_cooling_device *cpufreq_device, int cpu)
+{
+	u32 load;
+	u64 now, now_idle, delta_time, delta_idle;
+
+	now_idle = get_cpu_idle_time(cpu, &now, 0);
+	delta_idle = now_idle - cpufreq_device->time_in_idle[cpu];
+	delta_time = now - cpufreq_device->time_in_idle_timestamp[cpu];
+
+	if (delta_time <= delta_idle)
+		load = 0;
+	else
+		load = div64_u64(100 * (delta_time - delta_idle), delta_time);
+
+	cpufreq_device->time_in_idle[cpu] = now_idle;
+	cpufreq_device->time_in_idle_timestamp[cpu] = now;
+
+	return load;
+}
+
+/**
+ * get_static_power() - calculate the static power consumed by the cpus
+ * @cpufreq_device:	struct &cpufreq_cooling_device for this cpu cdev
+ * @tz:		thermal zone device in which we're operating
+ * @freq:	frequency in KHz
+ * @power:	pointer in which to store the calculated static power
+ *
+ * Calculate the static power consumed by the cpus described by
+ * @cpu_actor running at frequency @freq.  This function relies on a
+ * platform specific function that should have been provided when the
+ * actor was registered.  If it wasn't, the static power is assumed to
+ * be negligible.  The calculated static power is stored in @power.
+ *
+ * Return: 0 on success, -E* on failure.
+ */
+static int get_static_power(struct cpufreq_cooling_device *cpufreq_device,
+			    struct thermal_zone_device *tz, unsigned long freq,
+			    u32 *power)
+{
+	struct dev_pm_opp *opp;
+	unsigned long voltage;
+	struct cpumask *cpumask = &cpufreq_device->allowed_cpus;
+	unsigned long freq_hz = freq * 1000;
+
+	if (!cpufreq_device->plat_get_static_power ||
+	    !cpufreq_device->cpu_dev) {
+		*power = 0;
+		return 0;
+	}
+
+	rcu_read_lock();
+
+	opp = dev_pm_opp_find_freq_exact(cpufreq_device->cpu_dev, freq_hz,
+					 true);
+	voltage = dev_pm_opp_get_voltage(opp);
+
+	rcu_read_unlock();
+
+	if (voltage == 0) {
+		dev_warn_ratelimited(cpufreq_device->cpu_dev,
+				     "Failed to get voltage for frequency %lu: %ld\n",
+				     freq_hz, IS_ERR(opp) ? PTR_ERR(opp) : 0);
+		return -EINVAL;
+	}
+
+	return cpufreq_device->plat_get_static_power(cpumask, tz->passive_delay,
+						     voltage, power);
+}
+
+/**
+ * get_dynamic_power() - calculate the dynamic power
+ * @cpufreq_device:	&cpufreq_cooling_device for this cdev
+ * @freq:	current frequency
+ *
+ * Return: the dynamic power consumed by the cpus described by
+ * @cpufreq_device.
+ */
+static u32 get_dynamic_power(struct cpufreq_cooling_device *cpufreq_device,
+			     unsigned long freq)
+{
+	u32 raw_cpu_power;
+
+	raw_cpu_power = cpu_freq_to_power(cpufreq_device, freq);
+	return (raw_cpu_power * cpufreq_device->last_load) / 100;
 }
 
 /* cpufreq cooling device callback functions are defined below */
@@ -280,8 +526,205 @@ static int cpufreq_set_cur_state(struct thermal_cooling_device *cdev,
 	return 0;
 }
 
+/**
+ * cpufreq_get_requested_power() - get the current power
+ * @cdev:	&thermal_cooling_device pointer
+ * @tz:		a valid thermal zone device pointer
+ * @power:	pointer in which to store the resulting power
+ *
+ * Calculate the current power consumption of the cpus in milliwatts
+ * and store it in @power.  This function should actually calculate
+ * the requested power, but it's hard to get the frequency that
+ * cpufreq would have assigned if there were no thermal limits.
+ * Instead, we calculate the current power on the assumption that the
+ * immediate future will look like the immediate past.
+ *
+ * We use the current frequency and the average load since this
+ * function was last called.  In reality, there could have been
+ * multiple opps since this function was last called and that affects
+ * the load calculation.  While it's not perfectly accurate, this
+ * simplification is good enough and works.  REVISIT this, as more
+ * complex code may be needed if experiments show that it's not
+ * accurate enough.
+ *
+ * Return: 0 on success, -E* if getting the static power failed.
+ */
+static int cpufreq_get_requested_power(struct thermal_cooling_device *cdev,
+				       struct thermal_zone_device *tz,
+				       u32 *power)
+{
+	unsigned long freq;
+	int i = 0, cpu, ret;
+	u32 static_power, dynamic_power, total_load = 0;
+	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
+	u32 *load_cpu = NULL;
+
+	cpu = cpumask_any_and(&cpufreq_device->allowed_cpus, cpu_online_mask);
+
+	/*
+	 * All the CPUs are offline, thus the requested power by
+	 * the cdev is 0
+	 */
+	if (cpu >= nr_cpu_ids) {
+		*power = 0;
+		return 0;
+	}
+
+	freq = cpufreq_quick_get(cpu);
+
+	if (trace_thermal_power_cpu_get_power_enabled()) {
+		u32 ncpus = cpumask_weight(&cpufreq_device->allowed_cpus);
+
+		load_cpu = devm_kcalloc(&cdev->device, ncpus, sizeof(*load_cpu),
+					GFP_KERNEL);
+	}
+
+	for_each_cpu(cpu, &cpufreq_device->allowed_cpus) {
+		u32 load;
+
+		if (cpu_online(cpu))
+			load = get_load(cpufreq_device, cpu);
+		else
+			load = 0;
+
+		total_load += load;
+		if (trace_thermal_power_cpu_limit_enabled() && load_cpu)
+			load_cpu[i] = load;
+
+		i++;
+	}
+
+	cpufreq_device->last_load = total_load;
+
+	dynamic_power = get_dynamic_power(cpufreq_device, freq);
+	ret = get_static_power(cpufreq_device, tz, freq, &static_power);
+	if (ret) {
+		if (load_cpu)
+			devm_kfree(&cdev->device, load_cpu);
+		return ret;
+	}
+
+	if (load_cpu) {
+		trace_thermal_power_cpu_get_power(
+			&cpufreq_device->allowed_cpus,
+			freq, load_cpu, i, dynamic_power, static_power);
+
+		devm_kfree(&cdev->device, load_cpu);
+	}
+
+	*power = static_power + dynamic_power;
+	return 0;
+}
+
+/**
+ * cpufreq_state2power() - convert a cpu cdev state to power consumed
+ * @cdev:	&thermal_cooling_device pointer
+ * @tz:		a valid thermal zone device pointer
+ * @state:	cooling device state to be converted
+ * @power:	pointer in which to store the resulting power
+ *
+ * Convert cooling device state @state into power consumption in
+ * milliwatts assuming 100% load.  Store the calculated power in
+ * @power.
+ *
+ * Return: 0 on success, -EINVAL if the cooling device state could not
+ * be converted into a frequency or other -E* if there was an error
+ * when calculating the static power.
+ */
+static int cpufreq_state2power(struct thermal_cooling_device *cdev,
+			       struct thermal_zone_device *tz,
+			       unsigned long state, u32 *power)
+{
+	unsigned int freq, num_cpus;
+	cpumask_t cpumask;
+	u32 static_power, dynamic_power;
+	int ret;
+	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
+
+	cpumask_and(&cpumask, &cpufreq_device->allowed_cpus, cpu_online_mask);
+	num_cpus = cpumask_weight(&cpumask);
+
+	/* None of our cpus are online, so no power */
+	if (num_cpus == 0) {
+		*power = 0;
+		return 0;
+	}
+
+	freq = cpufreq_device->freq_table[state];
+	if (!freq)
+		return -EINVAL;
+
+	dynamic_power = cpu_freq_to_power(cpufreq_device, freq) * num_cpus;
+	ret = get_static_power(cpufreq_device, tz, freq, &static_power);
+	if (ret)
+		return ret;
+
+	*power = static_power + dynamic_power;
+	return 0;
+}
+
+/**
+ * cpufreq_power2state() - convert power to a cooling device state
+ * @cdev:	&thermal_cooling_device pointer
+ * @tz:		a valid thermal zone device pointer
+ * @power:	power in milliwatts to be converted
+ * @state:	pointer in which to store the resulting state
+ *
+ * Calculate a cooling device state for the cpus described by @cdev
+ * that would allow them to consume at most @power mW and store it in
+ * @state.  Note that this calculation depends on external factors
+ * such as the cpu load or the current static power.  Calling this
+ * function with the same power as input can yield different cooling
+ * device states depending on those external factors.
+ *
+ * Return: 0 on success, -ENODEV if no cpus are online or -EINVAL if
+ * the calculated frequency could not be converted to a valid state.
+ * The latter should not happen unless the frequencies available to
+ * cpufreq have changed since the initialization of the cpu cooling
+ * device.
+ */
+static int cpufreq_power2state(struct thermal_cooling_device *cdev,
+			       struct thermal_zone_device *tz, u32 power,
+			       unsigned long *state)
+{
+	unsigned int cpu, cur_freq, target_freq;
+	int ret;
+	s32 dyn_power;
+	u32 last_load, normalised_power, static_power;
+	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
+
+	cpu = cpumask_any_and(&cpufreq_device->allowed_cpus, cpu_online_mask);
+
+	/* None of our cpus are online */
+	if (cpu >= nr_cpu_ids)
+		return -ENODEV;
+
+	cur_freq = cpufreq_quick_get(cpu);
+	ret = get_static_power(cpufreq_device, tz, cur_freq, &static_power);
+	if (ret)
+		return ret;
+
+	dyn_power = power - static_power;
+	dyn_power = dyn_power > 0 ? dyn_power : 0;
+	last_load = cpufreq_device->last_load ?: 1;
+	normalised_power = (dyn_power * 100) / last_load;
+	target_freq = cpu_power_to_freq(cpufreq_device, normalised_power);
+
+	*state = cpufreq_cooling_get_level(cpu, target_freq);
+	if (*state == THERMAL_CSTATE_INVALID) {
+		dev_warn_ratelimited(&cdev->device,
+				     "Failed to convert %dKHz for cpu %d into a cdev state\n",
+				     target_freq, cpu);
+		return -EINVAL;
+	}
+
+	trace_thermal_power_cpu_limit(&cpufreq_device->allowed_cpus,
+				      target_freq, *state, power);
+	return 0;
+}
+
 /* Bind cpufreq callbacks to thermal cooling device ops */
-static struct thermal_cooling_device_ops const cpufreq_cooling_ops = {
+static struct thermal_cooling_device_ops cpufreq_cooling_ops = {
 	.get_max_state = cpufreq_get_max_state,
 	.get_cur_state = cpufreq_get_cur_state,
 	.set_cur_state = cpufreq_set_cur_state,
@@ -311,6 +754,9 @@ static unsigned int find_next_max(struct cpufreq_frequency_table *table,
  * @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.
+ * @capacitance: dynamic power coefficient for these cpus
+ * @plat_static_func: function to calculate the static power consumed by these
+ *                    cpus (optional)
  *
  * This interface function registers the cpufreq cooling device with the name
  * "thermal-cpufreq-%x". This api can support multiple instances of cpufreq
@@ -322,13 +768,14 @@ static unsigned int find_next_max(struct cpufreq_frequency_table *table,
  */
 static struct thermal_cooling_device *
 __cpufreq_cooling_register(struct device_node *np,
-			   const struct cpumask *clip_cpus)
+			const struct cpumask *clip_cpus, u32 capacitance,
+			get_static_t plat_static_func)
 {
 	struct thermal_cooling_device *cool_dev;
 	struct cpufreq_cooling_device *cpufreq_dev;
 	char dev_name[THERMAL_NAME_LENGTH];
 	struct cpufreq_frequency_table *pos, *table;
-	unsigned int freq, i;
+	unsigned int freq, i, num_cpus;
 	int ret;
 
 	table = cpufreq_frequency_get_table(cpumask_first(clip_cpus));
@@ -341,6 +788,23 @@ __cpufreq_cooling_register(struct device_node *np,
 	if (!cpufreq_dev)
 		return ERR_PTR(-ENOMEM);
 
+	num_cpus = cpumask_weight(clip_cpus);
+	cpufreq_dev->time_in_idle = kcalloc(num_cpus,
+					    sizeof(*cpufreq_dev->time_in_idle),
+					    GFP_KERNEL);
+	if (!cpufreq_dev->time_in_idle) {
+		cool_dev = ERR_PTR(-ENOMEM);
+		goto free_cdev;
+	}
+
+	cpufreq_dev->time_in_idle_timestamp =
+		kcalloc(num_cpus, sizeof(*cpufreq_dev->time_in_idle_timestamp),
+			GFP_KERNEL);
+	if (!cpufreq_dev->time_in_idle_timestamp) {
+		cool_dev = ERR_PTR(-ENOMEM);
+		goto free_time_in_idle;
+	}
+
 	/* Find max levels */
 	cpufreq_for_each_valid_entry(pos, table)
 		cpufreq_dev->max_level++;
@@ -349,7 +813,7 @@ __cpufreq_cooling_register(struct device_node *np,
 					  cpufreq_dev->max_level, GFP_KERNEL);
 	if (!cpufreq_dev->freq_table) {
 		cool_dev = ERR_PTR(-ENOMEM);
-		goto free_cdev;
+		goto free_time_in_idle_timestamp;
 	}
 
 	/* max_level is an index, not a counter */
@@ -357,6 +821,20 @@ __cpufreq_cooling_register(struct device_node *np,
 
 	cpumask_copy(&cpufreq_dev->allowed_cpus, clip_cpus);
 
+	if (capacitance) {
+		cpufreq_cooling_ops.get_requested_power =
+			cpufreq_get_requested_power;
+		cpufreq_cooling_ops.state2power = cpufreq_state2power;
+		cpufreq_cooling_ops.power2state = cpufreq_power2state;
+		cpufreq_dev->plat_get_static_power = plat_static_func;
+
+		ret = build_dyn_power_table(cpufreq_dev, capacitance);
+		if (ret) {
+			cool_dev = ERR_PTR(ret);
+			goto free_table;
+		}
+	}
+
 	ret = get_idr(&cpufreq_idr, &cpufreq_dev->id);
 	if (ret) {
 		cool_dev = ERR_PTR(ret);
@@ -402,6 +880,10 @@ remove_idr:
 	release_idr(&cpufreq_idr, cpufreq_dev->id);
 free_table:
 	kfree(cpufreq_dev->freq_table);
+free_time_in_idle_timestamp:
+	kfree(cpufreq_dev->time_in_idle_timestamp);
+free_time_in_idle:
+	kfree(cpufreq_dev->time_in_idle);
 free_cdev:
 	kfree(cpufreq_dev);
 
@@ -422,7 +904,7 @@ free_cdev:
 struct thermal_cooling_device *
 cpufreq_cooling_register(const struct cpumask *clip_cpus)
 {
-	return __cpufreq_cooling_register(NULL, clip_cpus);
+	return __cpufreq_cooling_register(NULL, clip_cpus, 0, NULL);
 }
 EXPORT_SYMBOL_GPL(cpufreq_cooling_register);
 
@@ -446,11 +928,78 @@ of_cpufreq_cooling_register(struct device_node *np,
 	if (!np)
 		return ERR_PTR(-EINVAL);
 
-	return __cpufreq_cooling_register(np, clip_cpus);
+	return __cpufreq_cooling_register(np, clip_cpus, 0, NULL);
 }
 EXPORT_SYMBOL_GPL(of_cpufreq_cooling_register);
 
 /**
+ * cpufreq_power_cooling_register() - create cpufreq cooling device with power extensions
+ * @clip_cpus:	cpumask of cpus where the frequency constraints will happen
+ * @capacitance:	dynamic power coefficient for these cpus
+ * @plat_static_func:	function to calculate the static power consumed by these
+ *			cpus (optional)
+ *
+ * This interface function registers the cpufreq cooling device with
+ * the name "thermal-cpufreq-%x".  This api can support multiple
+ * instances of cpufreq cooling devices.  Using this function, the
+ * cooling device will implement the power extensions by using a
+ * simple cpu power model.  The cpus must have registered their OPPs
+ * using the OPP library.
+ *
+ * An optional @plat_static_func may be provided to calculate the
+ * static power consumed by these cpus.  If the platform's static
+ * power consumption is unknown or negligible, make it NULL.
+ *
+ * Return: a valid struct thermal_cooling_device pointer on success,
+ * on failure, it returns a corresponding ERR_PTR().
+ */
+struct thermal_cooling_device *
+cpufreq_power_cooling_register(const struct cpumask *clip_cpus, u32 capacitance,
+			       get_static_t plat_static_func)
+{
+	return __cpufreq_cooling_register(NULL, clip_cpus, capacitance,
+				plat_static_func);
+}
+EXPORT_SYMBOL(cpufreq_power_cooling_register);
+
+/**
+ * of_cpufreq_power_cooling_register() - create cpufreq cooling device with power extensions
+ * @np:	a valid struct device_node to the cooling device device tree node
+ * @clip_cpus:	cpumask of cpus where the frequency constraints will happen
+ * @capacitance:	dynamic power coefficient for these cpus
+ * @plat_static_func:	function to calculate the static power consumed by these
+ *			cpus (optional)
+ *
+ * This interface function registers the cpufreq cooling device with
+ * the name "thermal-cpufreq-%x".  This api can support multiple
+ * instances of cpufreq cooling devices.  Using this API, the cpufreq
+ * cooling device will be linked to the device tree node provided.
+ * Using this function, the cooling device will implement the power
+ * extensions by using a simple cpu power model.  The cpus must have
+ * registered their OPPs using the OPP library.
+ *
+ * An optional @plat_static_func may be provided to calculate the
+ * static power consumed by these cpus.  If the platform's static
+ * power consumption is unknown or negligible, make it NULL.
+ *
+ * Return: a valid struct thermal_cooling_device pointer on success,
+ * on failure, it returns a corresponding ERR_PTR().
+ */
+struct thermal_cooling_device *
+of_cpufreq_power_cooling_register(struct device_node *np,
+				  const struct cpumask *clip_cpus,
+				  u32 capacitance,
+				  get_static_t plat_static_func)
+{
+	if (!np)
+		return ERR_PTR(-EINVAL);
+
+	return __cpufreq_cooling_register(np, clip_cpus, capacitance,
+				plat_static_func);
+}
+EXPORT_SYMBOL(of_cpufreq_power_cooling_register);
+
+/**
  * cpufreq_cooling_unregister - function to remove cpufreq cooling device.
  * @cdev: thermal cooling device pointer.
  *
@@ -475,6 +1024,8 @@ void cpufreq_cooling_unregister(struct thermal_cooling_device *cdev)
 
 	thermal_cooling_device_unregister(cpufreq_dev->cool_dev);
 	release_idr(&cpufreq_idr, cpufreq_dev->id);
+	kfree(cpufreq_dev->time_in_idle_timestamp);
+	kfree(cpufreq_dev->time_in_idle);
 	kfree(cpufreq_dev->freq_table);
 	kfree(cpufreq_dev);
 }
diff --git a/drivers/thermal/db8500_thermal.c b/drivers/thermal/db8500_thermal.c
index 20adfbe27df1..2fb273c4baa9 100644
--- a/drivers/thermal/db8500_thermal.c
+++ b/drivers/thermal/db8500_thermal.c
@@ -76,7 +76,7 @@ static int db8500_cdev_bind(struct thermal_zone_device *thermal,
 		upper = lower = i > max_state ? max_state : i;
 
 		ret = thermal_zone_bind_cooling_device(thermal, i, cdev,
-			upper, lower);
+			upper, lower, THERMAL_WEIGHT_DEFAULT);
 
 		dev_info(&cdev->device, "%s bind to %d: %d-%s\n", cdev->type,
 			i, ret, ret ? "fail" : "succeed");
diff --git a/drivers/thermal/fair_share.c b/drivers/thermal/fair_share.c
index 6e0a3fbfae86..c2c10bbe24d6 100644
--- a/drivers/thermal/fair_share.c
+++ b/drivers/thermal/fair_share.c
@@ -59,17 +59,17 @@ static int get_trip_level(struct thermal_zone_device *tz)
 }
 
 static long get_target_state(struct thermal_zone_device *tz,
-		struct thermal_cooling_device *cdev, int weight, int level)
+		struct thermal_cooling_device *cdev, int percentage, int level)
 {
 	unsigned long max_state;
 
 	cdev->ops->get_max_state(cdev, &max_state);
 
-	return (long)(weight * level * max_state) / (100 * tz->trips);
+	return (long)(percentage * level * max_state) / (100 * tz->trips);
 }
 
 /**
- * fair_share_throttle - throttles devices asscciated with the given zone
+ * fair_share_throttle - throttles devices associated with the given zone
  * @tz - thermal_zone_device
  *
  * Throttling Logic: This uses three parameters to calculate the new
@@ -77,7 +77,7 @@ static long get_target_state(struct thermal_zone_device *tz,
  *
  * Parameters used for Throttling:
  * P1. max_state: Maximum throttle state exposed by the cooling device.
- * P2. weight[i]/100:
+ * P2. percentage[i]/100:
  *	How 'effective' the 'i'th device is, in cooling the given zone.
  * P3. cur_trip_level/max_no_of_trips:
  *	This describes the extent to which the devices should be throttled.
@@ -88,28 +88,33 @@ static long get_target_state(struct thermal_zone_device *tz,
  */
 static int fair_share_throttle(struct thermal_zone_device *tz, int trip)
 {
-	const struct thermal_zone_params *tzp;
-	struct thermal_cooling_device *cdev;
 	struct thermal_instance *instance;
-	int i;
+	int total_weight = 0;
+	int total_instance = 0;
 	int cur_trip_level = get_trip_level(tz);
 
-	if (!tz->tzp || !tz->tzp->tbp)
-		return -EINVAL;
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+		if (instance->trip != trip)
+			continue;
+
+		total_weight += instance->weight;
+		total_instance++;
+	}
 
-	tzp = tz->tzp;
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+		int percentage;
+		struct thermal_cooling_device *cdev = instance->cdev;
 
-	for (i = 0; i < tzp->num_tbps; i++) {
-		if (!tzp->tbp[i].cdev)
+		if (instance->trip != trip)
 			continue;
 
-		cdev = tzp->tbp[i].cdev;
-		instance = get_thermal_instance(tz, cdev, trip);
-		if (!instance)
-			continue;
+		if (!total_weight)
+			percentage = 100 / total_instance;
+		else
+			percentage = (instance->weight * 100) / total_weight;
 
-		instance->target = get_target_state(tz, cdev,
-					tzp->tbp[i].weight, cur_trip_level);
+		instance->target = get_target_state(tz, cdev, percentage,
+						    cur_trip_level);
 
 		instance->cdev->updated = false;
 		thermal_cdev_update(cdev);
diff --git a/drivers/thermal/hisi_thermal.c b/drivers/thermal/hisi_thermal.c
new file mode 100644
index 000000000000..d5dd357ba57c
--- /dev/null
+++ b/drivers/thermal/hisi_thermal.c
@@ -0,0 +1,421 @@
+/*
+ * Hisilicon thermal sensor driver
+ *
+ * Copyright (c) 2014-2015 Hisilicon Limited.
+ * Copyright (c) 2014-2015 Linaro Limited.
+ *
+ * Xinwei Kong <kong.kongxinwei@hisilicon.com>
+ * Leo Yan <leo.yan@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/cpufreq.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+
+#include "thermal_core.h"
+
+#define TEMP0_TH			(0x4)
+#define TEMP0_RST_TH			(0x8)
+#define TEMP0_CFG			(0xC)
+#define TEMP0_EN			(0x10)
+#define TEMP0_INT_EN			(0x14)
+#define TEMP0_INT_CLR			(0x18)
+#define TEMP0_RST_MSK			(0x1C)
+#define TEMP0_VALUE			(0x28)
+
+#define HISI_TEMP_BASE			(-60)
+#define HISI_TEMP_RESET			(100000)
+
+#define HISI_MAX_SENSORS		4
+
+struct hisi_thermal_sensor {
+	struct hisi_thermal_data *thermal;
+	struct thermal_zone_device *tzd;
+
+	long sensor_temp;
+	uint32_t id;
+	uint32_t thres_temp;
+};
+
+struct hisi_thermal_data {
+	struct mutex thermal_lock;    /* protects register data */
+	struct platform_device *pdev;
+	struct clk *clk;
+	struct hisi_thermal_sensor sensors[HISI_MAX_SENSORS];
+
+	int irq, irq_bind_sensor;
+	bool irq_enabled;
+
+	void __iomem *regs;
+};
+
+/* in millicelsius */
+static inline int _step_to_temp(int step)
+{
+	/*
+	 * Every step equals (1 * 200) / 255 celsius, and finally
+	 * need convert to millicelsius.
+	 */
+	return (HISI_TEMP_BASE + (step * 200 / 255)) * 1000;
+}
+
+static inline long _temp_to_step(long temp)
+{
+	return ((temp / 1000 - HISI_TEMP_BASE) * 255 / 200);
+}
+
+static long hisi_thermal_get_sensor_temp(struct hisi_thermal_data *data,
+					 struct hisi_thermal_sensor *sensor)
+{
+	long val;
+
+	mutex_lock(&data->thermal_lock);
+
+	/* disable interrupt */
+	writel(0x0, data->regs + TEMP0_INT_EN);
+	writel(0x1, data->regs + TEMP0_INT_CLR);
+
+	/* disable module firstly */
+	writel(0x0, data->regs + TEMP0_EN);
+
+	/* select sensor id */
+	writel((sensor->id << 12), data->regs + TEMP0_CFG);
+
+	/* enable module */
+	writel(0x1, data->regs + TEMP0_EN);
+
+	usleep_range(3000, 5000);
+
+	val = readl(data->regs + TEMP0_VALUE);
+	val = _step_to_temp(val);
+
+	mutex_unlock(&data->thermal_lock);
+
+	return val;
+}
+
+static void hisi_thermal_enable_bind_irq_sensor
+			(struct hisi_thermal_data *data)
+{
+	struct hisi_thermal_sensor *sensor;
+
+	mutex_lock(&data->thermal_lock);
+
+	sensor = &data->sensors[data->irq_bind_sensor];
+
+	/* setting the hdak time */
+	writel(0x0, data->regs + TEMP0_CFG);
+
+	/* disable module firstly */
+	writel(0x0, data->regs + TEMP0_RST_MSK);
+	writel(0x0, data->regs + TEMP0_EN);
+
+	/* select sensor id */
+	writel((sensor->id << 12), data->regs + TEMP0_CFG);
+
+	/* enable for interrupt */
+	writel(_temp_to_step(sensor->thres_temp) | 0x0FFFFFF00,
+	       data->regs + TEMP0_TH);
+
+	writel(_temp_to_step(HISI_TEMP_RESET), data->regs + TEMP0_RST_TH);
+
+	/* enable module */
+	writel(0x1, data->regs + TEMP0_RST_MSK);
+	writel(0x1, data->regs + TEMP0_EN);
+
+	writel(0x0, data->regs + TEMP0_INT_CLR);
+	writel(0x1, data->regs + TEMP0_INT_EN);
+
+	usleep_range(3000, 5000);
+
+	mutex_unlock(&data->thermal_lock);
+}
+
+static void hisi_thermal_disable_sensor(struct hisi_thermal_data *data)
+{
+	mutex_lock(&data->thermal_lock);
+
+	/* disable sensor module */
+	writel(0x0, data->regs + TEMP0_INT_EN);
+	writel(0x0, data->regs + TEMP0_RST_MSK);
+	writel(0x0, data->regs + TEMP0_EN);
+
+	mutex_unlock(&data->thermal_lock);
+}
+
+static int hisi_thermal_get_temp(void *_sensor, long *temp)
+{
+	struct hisi_thermal_sensor *sensor = _sensor;
+	struct hisi_thermal_data *data = sensor->thermal;
+
+	int sensor_id = 0, i;
+	long max_temp = 0;
+
+	*temp = hisi_thermal_get_sensor_temp(data, sensor);
+
+	sensor->sensor_temp = *temp;
+
+	for (i = 0; i < HISI_MAX_SENSORS; i++) {
+		if (data->sensors[i].sensor_temp >= max_temp) {
+			max_temp = data->sensors[i].sensor_temp;
+			sensor_id = i;
+		}
+	}
+
+	mutex_lock(&data->thermal_lock);
+	data->irq_bind_sensor = sensor_id;
+	mutex_unlock(&data->thermal_lock);
+
+	dev_dbg(&data->pdev->dev, "id=%d, irq=%d, temp=%ld, thres=%d\n",
+		sensor->id, data->irq_enabled, *temp, sensor->thres_temp);
+	/*
+	 * Bind irq to sensor for two cases:
+	 *   Reenable alarm IRQ if temperature below threshold;
+	 *   if irq has been enabled, always set it;
+	 */
+	if (data->irq_enabled) {
+		hisi_thermal_enable_bind_irq_sensor(data);
+		return 0;
+	}
+
+	if (max_temp < sensor->thres_temp) {
+		data->irq_enabled = true;
+		hisi_thermal_enable_bind_irq_sensor(data);
+		enable_irq(data->irq);
+	}
+
+	return 0;
+}
+
+static struct thermal_zone_of_device_ops hisi_of_thermal_ops = {
+	.get_temp = hisi_thermal_get_temp,
+};
+
+static irqreturn_t hisi_thermal_alarm_irq(int irq, void *dev)
+{
+	struct hisi_thermal_data *data = dev;
+
+	disable_irq_nosync(irq);
+	data->irq_enabled = false;
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t hisi_thermal_alarm_irq_thread(int irq, void *dev)
+{
+	struct hisi_thermal_data *data = dev;
+	struct hisi_thermal_sensor *sensor;
+	int i;
+
+	mutex_lock(&data->thermal_lock);
+	sensor = &data->sensors[data->irq_bind_sensor];
+
+	dev_crit(&data->pdev->dev, "THERMAL ALARM: T > %d\n",
+		 sensor->thres_temp / 1000);
+	mutex_unlock(&data->thermal_lock);
+
+	for (i = 0; i < HISI_MAX_SENSORS; i++)
+		thermal_zone_device_update(data->sensors[i].tzd);
+
+	return IRQ_HANDLED;
+}
+
+static int hisi_thermal_register_sensor(struct platform_device *pdev,
+					struct hisi_thermal_data *data,
+					struct hisi_thermal_sensor *sensor,
+					int index)
+{
+	int ret, i;
+	const struct thermal_trip *trip;
+
+	sensor->id = index;
+	sensor->thermal = data;
+
+	sensor->tzd = thermal_zone_of_sensor_register(&pdev->dev, sensor->id,
+				sensor, &hisi_of_thermal_ops);
+	if (IS_ERR(sensor->tzd)) {
+		ret = PTR_ERR(sensor->tzd);
+		dev_err(&pdev->dev, "failed to register sensor id %d: %d\n",
+			sensor->id, ret);
+		return ret;
+	}
+
+	trip = of_thermal_get_trip_points(sensor->tzd);
+
+	for (i = 0; i < of_thermal_get_ntrips(sensor->tzd); i++) {
+		if (trip[i].type == THERMAL_TRIP_PASSIVE) {
+			sensor->thres_temp = trip[i].temperature;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static const struct of_device_id of_hisi_thermal_match[] = {
+	{ .compatible = "hisilicon,tsensor" },
+	{ /* end */ }
+};
+MODULE_DEVICE_TABLE(of, of_hisi_thermal_match);
+
+static void hisi_thermal_toggle_sensor(struct hisi_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 int hisi_thermal_probe(struct platform_device *pdev)
+{
+	struct hisi_thermal_data *data;
+	struct resource *res;
+	int i;
+	int ret;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	mutex_init(&data->thermal_lock);
+	data->pdev = pdev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->regs)) {
+		dev_err(&pdev->dev, "failed to get io address\n");
+		return PTR_ERR(data->regs);
+	}
+
+	data->irq = platform_get_irq(pdev, 0);
+	if (data->irq < 0)
+		return data->irq;
+
+	ret = devm_request_threaded_irq(&pdev->dev, data->irq,
+					hisi_thermal_alarm_irq,
+					hisi_thermal_alarm_irq_thread,
+					0, "hisi_thermal", data);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to request alarm irq: %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, data);
+
+	data->clk = devm_clk_get(&pdev->dev, "thermal_clk");
+	if (IS_ERR(data->clk)) {
+		ret = PTR_ERR(data->clk);
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev,
+				"failed to get thermal clk: %d\n", ret);
+		return ret;
+	}
+
+	/* enable clock for thermal */
+	ret = clk_prepare_enable(data->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to enable thermal clk: %d\n", ret);
+		return ret;
+	}
+
+	for (i = 0; i < HISI_MAX_SENSORS; ++i) {
+		ret = hisi_thermal_register_sensor(pdev, data,
+						   &data->sensors[i], i);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"failed to register thermal sensor: %d\n", ret);
+			goto err_get_sensor_data;
+		}
+	}
+
+	hisi_thermal_enable_bind_irq_sensor(data);
+	data->irq_enabled = true;
+
+	for (i = 0; i < HISI_MAX_SENSORS; i++)
+		hisi_thermal_toggle_sensor(&data->sensors[i], true);
+
+	return 0;
+
+err_get_sensor_data:
+	clk_disable_unprepare(data->clk);
+
+	return ret;
+}
+
+static int hisi_thermal_remove(struct platform_device *pdev)
+{
+	struct hisi_thermal_data *data = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < HISI_MAX_SENSORS; i++) {
+		struct hisi_thermal_sensor *sensor = &data->sensors[i];
+
+		hisi_thermal_toggle_sensor(sensor, false);
+		thermal_zone_of_sensor_unregister(&pdev->dev, sensor->tzd);
+	}
+
+	hisi_thermal_disable_sensor(data);
+	clk_disable_unprepare(data->clk);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int hisi_thermal_suspend(struct device *dev)
+{
+	struct hisi_thermal_data *data = dev_get_drvdata(dev);
+
+	hisi_thermal_disable_sensor(data);
+	data->irq_enabled = false;
+
+	clk_disable_unprepare(data->clk);
+
+	return 0;
+}
+
+static int hisi_thermal_resume(struct device *dev)
+{
+	struct hisi_thermal_data *data = dev_get_drvdata(dev);
+
+	clk_prepare_enable(data->clk);
+
+	data->irq_enabled = true;
+	hisi_thermal_enable_bind_irq_sensor(data);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(hisi_thermal_pm_ops,
+			 hisi_thermal_suspend, hisi_thermal_resume);
+
+static struct platform_driver hisi_thermal_driver = {
+	.driver = {
+		.name		= "hisi_thermal",
+		.owner		= THIS_MODULE,
+		.pm		= &hisi_thermal_pm_ops,
+		.of_match_table = of_hisi_thermal_match,
+	},
+	.probe	= hisi_thermal_probe,
+	.remove	= hisi_thermal_remove,
+};
+
+module_platform_driver(hisi_thermal_driver);
+
+MODULE_AUTHOR("Xinwei Kong <kong.kongxinwei@hisilicon.com>");
+MODULE_AUTHOR("Leo Yan <leo.yan@linaro.org>");
+MODULE_DESCRIPTION("Hisilicon thermal driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/imx_thermal.c b/drivers/thermal/imx_thermal.c
index 2ccbc0788353..fde4c2876d14 100644
--- a/drivers/thermal/imx_thermal.c
+++ b/drivers/thermal/imx_thermal.c
@@ -306,7 +306,8 @@ static int imx_bind(struct thermal_zone_device *tz,
 
 	ret = thermal_zone_bind_cooling_device(tz, IMX_TRIP_PASSIVE, cdev,
 					       THERMAL_NO_LIMIT,
-					       THERMAL_NO_LIMIT);
+					       THERMAL_NO_LIMIT,
+					       THERMAL_WEIGHT_DEFAULT);
 	if (ret) {
 		dev_err(&tz->device,
 			"binding zone %s with cdev %s failed:%d\n",
diff --git a/drivers/thermal/int340x_thermal/processor_thermal_device.c b/drivers/thermal/int340x_thermal/processor_thermal_device.c
index 5e8d8e91ea6d..3df3dc34b124 100644
--- a/drivers/thermal/int340x_thermal/processor_thermal_device.c
+++ b/drivers/thermal/int340x_thermal/processor_thermal_device.c
@@ -16,15 +16,20 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/pci.h>
+#include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/acpi.h>
 #include <linux/thermal.h>
 #include "int340x_thermal_zone.h"
+#include "../intel_soc_dts_iosf.h"
 
 /* Broadwell-U/HSB thermal reporting device */
 #define PCI_DEVICE_ID_PROC_BDW_THERMAL	0x1603
 #define PCI_DEVICE_ID_PROC_HSB_THERMAL	0x0A03
 
+/* Skylake thermal reporting device */
+#define PCI_DEVICE_ID_PROC_SKL_THERMAL	0x1903
+
 /* Braswell thermal reporting device */
 #define PCI_DEVICE_ID_PROC_BSW_THERMAL	0x22DC
 
@@ -42,6 +47,7 @@ struct proc_thermal_device {
 	struct acpi_device *adev;
 	struct power_config power_limits[2];
 	struct int34x_thermal_zone *int340x_zone;
+	struct intel_soc_dts_sensors *soc_dts;
 };
 
 enum proc_thermal_emum_mode_type {
@@ -308,6 +314,18 @@ static int int3401_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static irqreturn_t proc_thermal_pci_msi_irq(int irq, void *devid)
+{
+	struct proc_thermal_device *proc_priv;
+	struct pci_dev *pdev = devid;
+
+	proc_priv = pci_get_drvdata(pdev);
+
+	intel_soc_dts_iosf_interrupt_handler(proc_priv->soc_dts);
+
+	return IRQ_HANDLED;
+}
+
 static int  proc_thermal_pci_probe(struct pci_dev *pdev,
 				   const struct pci_device_id *unused)
 {
@@ -334,18 +352,57 @@ static int  proc_thermal_pci_probe(struct pci_dev *pdev,
 	pci_set_drvdata(pdev, proc_priv);
 	proc_thermal_emum_mode = PROC_THERMAL_PCI;
 
+	if (pdev->device == PCI_DEVICE_ID_PROC_BSW_THERMAL) {
+		/*
+		 * Enumerate additional DTS sensors available via IOSF.
+		 * But we are not treating as a failure condition, if
+		 * there are no aux DTSs enabled or fails. This driver
+		 * already exposes sensors, which can be accessed via
+		 * ACPI/MSR. So we don't want to fail for auxiliary DTSs.
+		 */
+		proc_priv->soc_dts = intel_soc_dts_iosf_init(
+					INTEL_SOC_DTS_INTERRUPT_MSI, 2, 0);
+
+		if (proc_priv->soc_dts && pdev->irq) {
+			ret = pci_enable_msi(pdev);
+			if (!ret) {
+				ret = request_threaded_irq(pdev->irq, NULL,
+						proc_thermal_pci_msi_irq,
+						IRQF_ONESHOT, "proc_thermal",
+						pdev);
+				if (ret) {
+					intel_soc_dts_iosf_exit(
+							proc_priv->soc_dts);
+					pci_disable_msi(pdev);
+					proc_priv->soc_dts = NULL;
+				}
+			}
+		} else
+			dev_err(&pdev->dev, "No auxiliary DTSs enabled\n");
+	}
+
 	return 0;
 }
 
 static void  proc_thermal_pci_remove(struct pci_dev *pdev)
 {
-	proc_thermal_remove(pci_get_drvdata(pdev));
+	struct proc_thermal_device *proc_priv = pci_get_drvdata(pdev);
+
+	if (proc_priv->soc_dts) {
+		intel_soc_dts_iosf_exit(proc_priv->soc_dts);
+		if (pdev->irq) {
+			free_irq(pdev->irq, pdev);
+			pci_disable_msi(pdev);
+		}
+	}
+	proc_thermal_remove(proc_priv);
 	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_SKL_THERMAL)},
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_BSW_THERMAL)},
 	{ 0, },
 };
diff --git a/drivers/thermal/intel_powerclamp.c b/drivers/thermal/intel_powerclamp.c
index 725718e97a0b..5820e8513927 100644
--- a/drivers/thermal/intel_powerclamp.c
+++ b/drivers/thermal/intel_powerclamp.c
@@ -119,7 +119,7 @@ exit:
 	return ret;
 }
 
-static struct kernel_param_ops duration_ops = {
+static const struct kernel_param_ops duration_ops = {
 	.set = duration_set,
 	.get = param_get_int,
 };
@@ -167,7 +167,7 @@ exit_win:
 	return ret;
 }
 
-static struct kernel_param_ops window_size_ops = {
+static const struct kernel_param_ops window_size_ops = {
 	.set = window_size_set,
 	.get = param_get_int,
 };
@@ -697,6 +697,7 @@ static const struct x86_cpu_id intel_powerclamp_ids[] __initconst = {
 	{ X86_VENDOR_INTEL, 6, 0x4d},
 	{ X86_VENDOR_INTEL, 6, 0x4f},
 	{ X86_VENDOR_INTEL, 6, 0x56},
+	{ X86_VENDOR_INTEL, 6, 0x57},
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, intel_powerclamp_ids);
diff --git a/drivers/thermal/intel_quark_dts_thermal.c b/drivers/thermal/intel_quark_dts_thermal.c
new file mode 100644
index 000000000000..4434ec812cb7
--- /dev/null
+++ b/drivers/thermal/intel_quark_dts_thermal.c
@@ -0,0 +1,473 @@
+/*
+ * intel_quark_dts_thermal.c
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2015 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ *  WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * Contact Information:
+ *  Ong Boon Leong <boon.leong.ong@intel.com>
+ *  Intel Malaysia, Penang
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2015 Intel Corporation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in
+ *     the documentation and/or other materials provided with the
+ *     distribution.
+ *   * Neither the name of Intel Corporation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Quark DTS thermal driver is implemented by referencing
+ * intel_soc_dts_thermal.c.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/thermal.h>
+#include <asm/cpu_device_id.h>
+#include <asm/iosf_mbi.h>
+
+#define X86_FAMILY_QUARK	0x5
+#define X86_MODEL_QUARK_X1000	0x9
+
+/* DTS reset is programmed via QRK_MBI_UNIT_SOC */
+#define QRK_DTS_REG_OFFSET_RESET	0x34
+#define QRK_DTS_RESET_BIT		BIT(0)
+
+/* DTS enable is programmed via QRK_MBI_UNIT_RMU */
+#define QRK_DTS_REG_OFFSET_ENABLE	0xB0
+#define QRK_DTS_ENABLE_BIT		BIT(15)
+
+/* Temperature Register is read via QRK_MBI_UNIT_RMU */
+#define QRK_DTS_REG_OFFSET_TEMP		0xB1
+#define QRK_DTS_MASK_TEMP		0xFF
+#define QRK_DTS_OFFSET_TEMP		0
+#define QRK_DTS_OFFSET_REL_TEMP		16
+#define QRK_DTS_TEMP_BASE		50
+
+/* Programmable Trip Point Register is configured via QRK_MBI_UNIT_RMU */
+#define QRK_DTS_REG_OFFSET_PTPS		0xB2
+#define QRK_DTS_MASK_TP_THRES		0xFF
+#define QRK_DTS_SHIFT_TP		8
+#define QRK_DTS_ID_TP_CRITICAL		0
+#define QRK_DTS_SAFE_TP_THRES		105
+
+/* Thermal Sensor Register Lock */
+#define QRK_DTS_REG_OFFSET_LOCK		0x71
+#define QRK_DTS_LOCK_BIT		BIT(5)
+
+/* Quark DTS has 2 trip points: hot & catastrophic */
+#define QRK_MAX_DTS_TRIPS	2
+/* If DTS not locked, all trip points are configurable */
+#define QRK_DTS_WR_MASK_SET	0x3
+/* If DTS locked, all trip points are not configurable */
+#define QRK_DTS_WR_MASK_CLR	0
+
+#define DEFAULT_POLL_DELAY	2000
+
+struct soc_sensor_entry {
+	bool locked;
+	u32 store_ptps;
+	u32 store_dts_enable;
+	enum thermal_device_mode mode;
+	struct thermal_zone_device *tzone;
+};
+
+static struct soc_sensor_entry *soc_dts;
+
+static int polling_delay = DEFAULT_POLL_DELAY;
+module_param(polling_delay, int, 0644);
+MODULE_PARM_DESC(polling_delay,
+	"Polling interval for checking trip points (in milliseconds)");
+
+static DEFINE_MUTEX(dts_update_mutex);
+
+static int soc_dts_enable(struct thermal_zone_device *tzd)
+{
+	u32 out;
+	struct soc_sensor_entry *aux_entry = tzd->devdata;
+	int ret;
+
+	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
+					QRK_DTS_REG_OFFSET_ENABLE, &out);
+	if (ret)
+		return ret;
+
+	if (out & QRK_DTS_ENABLE_BIT) {
+		aux_entry->mode = THERMAL_DEVICE_ENABLED;
+		return 0;
+	}
+
+	if (!aux_entry->locked) {
+		out |= QRK_DTS_ENABLE_BIT;
+		ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
+					QRK_DTS_REG_OFFSET_ENABLE, out);
+		if (ret)
+			return ret;
+
+		aux_entry->mode = THERMAL_DEVICE_ENABLED;
+	} else {
+		aux_entry->mode = THERMAL_DEVICE_DISABLED;
+		pr_info("DTS is locked. Cannot enable DTS\n");
+		ret = -EPERM;
+	}
+
+	return ret;
+}
+
+static int soc_dts_disable(struct thermal_zone_device *tzd)
+{
+	u32 out;
+	struct soc_sensor_entry *aux_entry = tzd->devdata;
+	int ret;
+
+	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
+					QRK_DTS_REG_OFFSET_ENABLE, &out);
+	if (ret)
+		return ret;
+
+	if (!(out & QRK_DTS_ENABLE_BIT)) {
+		aux_entry->mode = THERMAL_DEVICE_DISABLED;
+		return 0;
+	}
+
+	if (!aux_entry->locked) {
+		out &= ~QRK_DTS_ENABLE_BIT;
+		ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
+					QRK_DTS_REG_OFFSET_ENABLE, out);
+
+		if (ret)
+			return ret;
+
+		aux_entry->mode = THERMAL_DEVICE_DISABLED;
+	} else {
+		aux_entry->mode = THERMAL_DEVICE_ENABLED;
+		pr_info("DTS is locked. Cannot disable DTS\n");
+		ret = -EPERM;
+	}
+
+	return ret;
+}
+
+static int _get_trip_temp(int trip, unsigned long *temp)
+{
+	int status;
+	u32 out;
+
+	mutex_lock(&dts_update_mutex);
+	status = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
+				QRK_DTS_REG_OFFSET_PTPS, &out);
+	mutex_unlock(&dts_update_mutex);
+
+	if (status)
+		return status;
+
+	/*
+	 * Thermal Sensor Programmable Trip Point Register has 8-bit
+	 * fields for critical (catastrophic) and hot set trip point
+	 * thresholds. The threshold value is always offset by its
+	 * temperature base (50 degree Celsius).
+	 */
+	*temp = (out >> (trip * QRK_DTS_SHIFT_TP)) & QRK_DTS_MASK_TP_THRES;
+	*temp -= QRK_DTS_TEMP_BASE;
+
+	return 0;
+}
+
+static inline int sys_get_trip_temp(struct thermal_zone_device *tzd,
+				int trip, unsigned long *temp)
+{
+	return _get_trip_temp(trip, temp);
+}
+
+static inline int sys_get_crit_temp(struct thermal_zone_device *tzd,
+				unsigned long *temp)
+{
+	return _get_trip_temp(QRK_DTS_ID_TP_CRITICAL, temp);
+}
+
+static int update_trip_temp(struct soc_sensor_entry *aux_entry,
+				int trip, unsigned long temp)
+{
+	u32 out;
+	u32 temp_out;
+	u32 store_ptps;
+	int ret;
+
+	mutex_lock(&dts_update_mutex);
+	if (aux_entry->locked) {
+		ret = -EPERM;
+		goto failed;
+	}
+
+	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
+				QRK_DTS_REG_OFFSET_PTPS, &store_ptps);
+	if (ret)
+		goto failed;
+
+	/*
+	 * Protection against unsafe trip point thresdhold value.
+	 * As Quark X1000 data-sheet does not provide any recommendation
+	 * regarding the safe trip point threshold value to use, we choose
+	 * the safe value according to the threshold value set by UEFI BIOS.
+	 */
+	if (temp > QRK_DTS_SAFE_TP_THRES)
+		temp = QRK_DTS_SAFE_TP_THRES;
+
+	/*
+	 * Thermal Sensor Programmable Trip Point Register has 8-bit
+	 * fields for critical (catastrophic) and hot set trip point
+	 * thresholds. The threshold value is always offset by its
+	 * temperature base (50 degree Celsius).
+	 */
+	temp_out = temp + QRK_DTS_TEMP_BASE;
+	out = (store_ptps & ~(QRK_DTS_MASK_TP_THRES <<
+		(trip * QRK_DTS_SHIFT_TP)));
+	out |= (temp_out & QRK_DTS_MASK_TP_THRES) <<
+		(trip * QRK_DTS_SHIFT_TP);
+
+	ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
+				QRK_DTS_REG_OFFSET_PTPS, out);
+
+failed:
+	mutex_unlock(&dts_update_mutex);
+	return ret;
+}
+
+static inline int sys_set_trip_temp(struct thermal_zone_device *tzd, int trip,
+				unsigned long temp)
+{
+	return update_trip_temp(tzd->devdata, trip, temp);
+}
+
+static int sys_get_trip_type(struct thermal_zone_device *thermal,
+		int trip, enum thermal_trip_type *type)
+{
+	if (trip)
+		*type = THERMAL_TRIP_HOT;
+	else
+		*type = THERMAL_TRIP_CRITICAL;
+
+	return 0;
+}
+
+static int sys_get_curr_temp(struct thermal_zone_device *tzd,
+				unsigned long *temp)
+{
+	u32 out;
+	int ret;
+
+	mutex_lock(&dts_update_mutex);
+	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
+					QRK_DTS_REG_OFFSET_TEMP, &out);
+	mutex_unlock(&dts_update_mutex);
+
+	if (ret)
+		return ret;
+
+	/*
+	 * Thermal Sensor Temperature Register has 8-bit field
+	 * for temperature value (offset by temperature base
+	 * 50 degree Celsius).
+	 */
+	out = (out >> QRK_DTS_OFFSET_TEMP) & QRK_DTS_MASK_TEMP;
+	*temp = out - QRK_DTS_TEMP_BASE;
+
+	return 0;
+}
+
+static int sys_get_mode(struct thermal_zone_device *tzd,
+				enum thermal_device_mode *mode)
+{
+	struct soc_sensor_entry *aux_entry = tzd->devdata;
+	*mode = aux_entry->mode;
+	return 0;
+}
+
+static int sys_set_mode(struct thermal_zone_device *tzd,
+				enum thermal_device_mode mode)
+{
+	int ret;
+
+	mutex_lock(&dts_update_mutex);
+	if (mode == THERMAL_DEVICE_ENABLED)
+		ret = soc_dts_enable(tzd);
+	else
+		ret = soc_dts_disable(tzd);
+	mutex_unlock(&dts_update_mutex);
+
+	return ret;
+}
+
+static struct thermal_zone_device_ops tzone_ops = {
+	.get_temp = sys_get_curr_temp,
+	.get_trip_temp = sys_get_trip_temp,
+	.get_trip_type = sys_get_trip_type,
+	.set_trip_temp = sys_set_trip_temp,
+	.get_crit_temp = sys_get_crit_temp,
+	.get_mode = sys_get_mode,
+	.set_mode = sys_set_mode,
+};
+
+static void free_soc_dts(struct soc_sensor_entry *aux_entry)
+{
+	if (aux_entry) {
+		if (!aux_entry->locked) {
+			mutex_lock(&dts_update_mutex);
+			iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
+					QRK_DTS_REG_OFFSET_ENABLE,
+					aux_entry->store_dts_enable);
+
+			iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
+					QRK_DTS_REG_OFFSET_PTPS,
+					aux_entry->store_ptps);
+			mutex_unlock(&dts_update_mutex);
+		}
+		thermal_zone_device_unregister(aux_entry->tzone);
+		kfree(aux_entry);
+	}
+}
+
+static struct soc_sensor_entry *alloc_soc_dts(void)
+{
+	struct soc_sensor_entry *aux_entry;
+	int err;
+	u32 out;
+	int wr_mask;
+
+	aux_entry = kzalloc(sizeof(*aux_entry), GFP_KERNEL);
+	if (!aux_entry) {
+		err = -ENOMEM;
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/* Check if DTS register is locked */
+	err = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
+					QRK_DTS_REG_OFFSET_LOCK,
+					&out);
+	if (err)
+		goto err_ret;
+
+	if (out & QRK_DTS_LOCK_BIT) {
+		aux_entry->locked = true;
+		wr_mask = QRK_DTS_WR_MASK_CLR;
+	} else {
+		aux_entry->locked = false;
+		wr_mask = QRK_DTS_WR_MASK_SET;
+	}
+
+	/* Store DTS default state if DTS registers are not locked */
+	if (!aux_entry->locked) {
+		/* Store DTS default enable for restore on exit */
+		err = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
+					QRK_DTS_REG_OFFSET_ENABLE,
+					&aux_entry->store_dts_enable);
+		if (err)
+			goto err_ret;
+
+		/* Store DTS default PTPS register for restore on exit */
+		err = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
+					QRK_DTS_REG_OFFSET_PTPS,
+					&aux_entry->store_ptps);
+		if (err)
+			goto err_ret;
+	}
+
+	aux_entry->tzone = thermal_zone_device_register("quark_dts",
+			QRK_MAX_DTS_TRIPS,
+			wr_mask,
+			aux_entry, &tzone_ops, NULL, 0, polling_delay);
+	if (IS_ERR(aux_entry->tzone)) {
+		err = PTR_ERR(aux_entry->tzone);
+		goto err_ret;
+	}
+
+	mutex_lock(&dts_update_mutex);
+	err = soc_dts_enable(aux_entry->tzone);
+	mutex_unlock(&dts_update_mutex);
+	if (err)
+		goto err_aux_status;
+
+	return aux_entry;
+
+err_aux_status:
+	thermal_zone_device_unregister(aux_entry->tzone);
+err_ret:
+	kfree(aux_entry);
+	return ERR_PTR(err);
+}
+
+static const struct x86_cpu_id qrk_thermal_ids[] __initconst  = {
+	{ X86_VENDOR_INTEL, X86_FAMILY_QUARK, X86_MODEL_QUARK_X1000 },
+	{}
+};
+MODULE_DEVICE_TABLE(x86cpu, qrk_thermal_ids);
+
+static int __init intel_quark_thermal_init(void)
+{
+	int err = 0;
+
+	if (!x86_match_cpu(qrk_thermal_ids) || !iosf_mbi_available())
+		return -ENODEV;
+
+	soc_dts = alloc_soc_dts();
+	if (IS_ERR(soc_dts)) {
+		err = PTR_ERR(soc_dts);
+		goto err_free;
+	}
+
+	return 0;
+
+err_free:
+	free_soc_dts(soc_dts);
+	return err;
+}
+
+static void __exit intel_quark_thermal_exit(void)
+{
+	free_soc_dts(soc_dts);
+}
+
+module_init(intel_quark_thermal_init)
+module_exit(intel_quark_thermal_exit)
+
+MODULE_DESCRIPTION("Intel Quark DTS Thermal Driver");
+MODULE_AUTHOR("Ong Boon Leong <boon.leong.ong@intel.com>");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/thermal/intel_soc_dts_iosf.c b/drivers/thermal/intel_soc_dts_iosf.c
new file mode 100644
index 000000000000..42e4b6ac3875
--- /dev/null
+++ b/drivers/thermal/intel_soc_dts_iosf.c
@@ -0,0 +1,478 @@
+/*
+ * intel_soc_dts_iosf.c
+ * Copyright (c) 2015, 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.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <asm/iosf_mbi.h>
+#include "intel_soc_dts_iosf.h"
+
+#define SOC_DTS_OFFSET_ENABLE		0xB0
+#define SOC_DTS_OFFSET_TEMP		0xB1
+
+#define SOC_DTS_OFFSET_PTPS		0xB2
+#define SOC_DTS_OFFSET_PTTS		0xB3
+#define SOC_DTS_OFFSET_PTTSS		0xB4
+#define SOC_DTS_OFFSET_PTMC		0x80
+#define SOC_DTS_TE_AUX0			0xB5
+#define SOC_DTS_TE_AUX1			0xB6
+
+#define SOC_DTS_AUX0_ENABLE_BIT		BIT(0)
+#define SOC_DTS_AUX1_ENABLE_BIT		BIT(1)
+#define SOC_DTS_CPU_MODULE0_ENABLE_BIT	BIT(16)
+#define SOC_DTS_CPU_MODULE1_ENABLE_BIT	BIT(17)
+#define SOC_DTS_TE_SCI_ENABLE		BIT(9)
+#define SOC_DTS_TE_SMI_ENABLE		BIT(10)
+#define SOC_DTS_TE_MSI_ENABLE		BIT(11)
+#define SOC_DTS_TE_APICA_ENABLE		BIT(14)
+#define SOC_DTS_PTMC_APIC_DEASSERT_BIT	BIT(4)
+
+/* DTS encoding for TJ MAX temperature */
+#define SOC_DTS_TJMAX_ENCODING		0x7F
+
+/* Only 2 out of 4 is allowed for OSPM */
+#define SOC_MAX_DTS_TRIPS		2
+
+/* Mask for two trips in status bits */
+#define SOC_DTS_TRIP_MASK		0x03
+
+/* DTS0 and DTS 1 */
+#define SOC_MAX_DTS_SENSORS		2
+
+static int get_tj_max(u32 *tj_max)
+{
+	u32 eax, edx;
+	u32 val;
+	int err;
+
+	err = rdmsr_safe(MSR_IA32_TEMPERATURE_TARGET, &eax, &edx);
+	if (err)
+		goto err_ret;
+	else {
+		val = (eax >> 16) & 0xff;
+		if (val)
+			*tj_max = val * 1000;
+		else {
+			err = -EINVAL;
+			goto err_ret;
+		}
+	}
+
+	return 0;
+err_ret:
+	*tj_max = 0;
+
+	return err;
+}
+
+static int sys_get_trip_temp(struct thermal_zone_device *tzd, int trip,
+			     unsigned long *temp)
+{
+	int status;
+	u32 out;
+	struct intel_soc_dts_sensor_entry *dts;
+	struct intel_soc_dts_sensors *sensors;
+
+	dts = tzd->devdata;
+	sensors = dts->sensors;
+	mutex_lock(&sensors->dts_update_lock);
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			       SOC_DTS_OFFSET_PTPS, &out);
+	mutex_unlock(&sensors->dts_update_lock);
+	if (status)
+		return status;
+
+	out = (out >> (trip * 8)) & SOC_DTS_TJMAX_ENCODING;
+	if (!out)
+		*temp = 0;
+	else
+		*temp = sensors->tj_max - out * 1000;
+
+	return 0;
+}
+
+static int update_trip_temp(struct intel_soc_dts_sensor_entry *dts,
+			    int thres_index, unsigned long temp,
+			    enum thermal_trip_type trip_type)
+{
+	int status;
+	u32 temp_out;
+	u32 out;
+	u32 store_ptps;
+	u32 store_ptmc;
+	u32 store_te_out;
+	u32 te_out;
+	u32 int_enable_bit = SOC_DTS_TE_APICA_ENABLE;
+	struct intel_soc_dts_sensors *sensors = dts->sensors;
+
+	if (sensors->intr_type == INTEL_SOC_DTS_INTERRUPT_MSI)
+		int_enable_bit |= SOC_DTS_TE_MSI_ENABLE;
+
+	temp_out = (sensors->tj_max - temp) / 1000;
+
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			       SOC_DTS_OFFSET_PTPS, &store_ptps);
+	if (status)
+		return status;
+
+	out = (store_ptps & ~(0xFF << (thres_index * 8)));
+	out |= (temp_out & 0xFF) << (thres_index * 8);
+	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+				SOC_DTS_OFFSET_PTPS, out);
+	if (status)
+		return status;
+
+	pr_debug("update_trip_temp PTPS = %x\n", out);
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			       SOC_DTS_OFFSET_PTMC, &out);
+	if (status)
+		goto err_restore_ptps;
+
+	store_ptmc = out;
+
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			       SOC_DTS_TE_AUX0 + thres_index,
+			       &te_out);
+	if (status)
+		goto err_restore_ptmc;
+
+	store_te_out = te_out;
+	/* Enable for CPU module 0 and module 1 */
+	out |= (SOC_DTS_CPU_MODULE0_ENABLE_BIT |
+					SOC_DTS_CPU_MODULE1_ENABLE_BIT);
+	if (temp) {
+		if (thres_index)
+			out |= SOC_DTS_AUX1_ENABLE_BIT;
+		else
+			out |= SOC_DTS_AUX0_ENABLE_BIT;
+		te_out |= int_enable_bit;
+	} else {
+		if (thres_index)
+			out &= ~SOC_DTS_AUX1_ENABLE_BIT;
+		else
+			out &= ~SOC_DTS_AUX0_ENABLE_BIT;
+		te_out &= ~int_enable_bit;
+	}
+	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+				SOC_DTS_OFFSET_PTMC, out);
+	if (status)
+		goto err_restore_te_out;
+
+	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+				SOC_DTS_TE_AUX0 + thres_index,
+				te_out);
+	if (status)
+		goto err_restore_te_out;
+
+	dts->trip_types[thres_index] = trip_type;
+
+	return 0;
+err_restore_te_out:
+	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+		       SOC_DTS_OFFSET_PTMC, store_te_out);
+err_restore_ptmc:
+	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+		       SOC_DTS_OFFSET_PTMC, store_ptmc);
+err_restore_ptps:
+	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+		       SOC_DTS_OFFSET_PTPS, store_ptps);
+	/* Nothing we can do if restore fails */
+
+	return status;
+}
+
+static int sys_set_trip_temp(struct thermal_zone_device *tzd, int trip,
+			     unsigned long temp)
+{
+	struct intel_soc_dts_sensor_entry *dts = tzd->devdata;
+	struct intel_soc_dts_sensors *sensors = dts->sensors;
+	int status;
+
+	if (temp > sensors->tj_max)
+		return -EINVAL;
+
+	mutex_lock(&sensors->dts_update_lock);
+	status = update_trip_temp(tzd->devdata, trip, temp,
+				  dts->trip_types[trip]);
+	mutex_unlock(&sensors->dts_update_lock);
+
+	return status;
+}
+
+static int sys_get_trip_type(struct thermal_zone_device *tzd,
+			     int trip, enum thermal_trip_type *type)
+{
+	struct intel_soc_dts_sensor_entry *dts;
+
+	dts = tzd->devdata;
+
+	*type = dts->trip_types[trip];
+
+	return 0;
+}
+
+static int sys_get_curr_temp(struct thermal_zone_device *tzd,
+			     unsigned long *temp)
+{
+	int status;
+	u32 out;
+	struct intel_soc_dts_sensor_entry *dts;
+	struct intel_soc_dts_sensors *sensors;
+
+	dts = tzd->devdata;
+	sensors = dts->sensors;
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			       SOC_DTS_OFFSET_TEMP, &out);
+	if (status)
+		return status;
+
+	out = (out & dts->temp_mask) >> dts->temp_shift;
+	out -= SOC_DTS_TJMAX_ENCODING;
+	*temp = sensors->tj_max - out * 1000;
+
+	return 0;
+}
+
+static struct thermal_zone_device_ops tzone_ops = {
+	.get_temp = sys_get_curr_temp,
+	.get_trip_temp = sys_get_trip_temp,
+	.get_trip_type = sys_get_trip_type,
+	.set_trip_temp = sys_set_trip_temp,
+};
+
+static int soc_dts_enable(int id)
+{
+	u32 out;
+	int ret;
+
+	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			    SOC_DTS_OFFSET_ENABLE, &out);
+	if (ret)
+		return ret;
+
+	if (!(out & BIT(id))) {
+		out |= BIT(id);
+		ret = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+				     SOC_DTS_OFFSET_ENABLE, out);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+static void remove_dts_thermal_zone(struct intel_soc_dts_sensor_entry *dts)
+{
+	if (dts) {
+		iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+			       SOC_DTS_OFFSET_ENABLE, dts->store_status);
+		thermal_zone_device_unregister(dts->tzone);
+	}
+}
+
+static int add_dts_thermal_zone(int id, struct intel_soc_dts_sensor_entry *dts,
+				bool notification_support, int trip_cnt,
+				int read_only_trip_cnt)
+{
+	char name[10];
+	int trip_count = 0;
+	int trip_mask = 0;
+	u32 store_ptps;
+	int ret;
+	int i;
+
+	/* Store status to restor on exit */
+	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			    SOC_DTS_OFFSET_ENABLE,
+			    &dts->store_status);
+	if (ret)
+		goto err_ret;
+
+	dts->id = id;
+	dts->temp_mask = 0x00FF << (id * 8);
+	dts->temp_shift = id * 8;
+	if (notification_support) {
+		trip_count = min(SOC_MAX_DTS_TRIPS, trip_cnt);
+		trip_mask = BIT(trip_count - read_only_trip_cnt) - 1;
+	}
+
+	/* Check if the writable trip we provide is not used by BIOS */
+	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			    SOC_DTS_OFFSET_PTPS, &store_ptps);
+	if (ret)
+		trip_mask = 0;
+	else {
+		for (i = 0; i < trip_count; ++i) {
+			if (trip_mask & BIT(i))
+				if (store_ptps & (0xff << (i * 8)))
+					trip_mask &= ~BIT(i);
+		}
+	}
+	dts->trip_mask = trip_mask;
+	dts->trip_count = trip_count;
+	snprintf(name, sizeof(name), "soc_dts%d", id);
+	dts->tzone = thermal_zone_device_register(name,
+						  trip_count,
+						  trip_mask,
+						  dts, &tzone_ops,
+						  NULL, 0, 0);
+	if (IS_ERR(dts->tzone)) {
+		ret = PTR_ERR(dts->tzone);
+		goto err_ret;
+	}
+
+	ret = soc_dts_enable(id);
+	if (ret)
+		goto err_enable;
+
+	return 0;
+err_enable:
+	thermal_zone_device_unregister(dts->tzone);
+err_ret:
+	return ret;
+}
+
+int intel_soc_dts_iosf_add_read_only_critical_trip(
+	struct intel_soc_dts_sensors *sensors, int critical_offset)
+{
+	int i, j;
+
+	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i) {
+		for (j = 0; j < sensors->soc_dts[i].trip_count; ++j) {
+			if (!(sensors->soc_dts[i].trip_mask & BIT(j))) {
+				return update_trip_temp(&sensors->soc_dts[i], j,
+					sensors->tj_max - critical_offset,
+					THERMAL_TRIP_CRITICAL);
+			}
+		}
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(intel_soc_dts_iosf_add_read_only_critical_trip);
+
+void intel_soc_dts_iosf_interrupt_handler(struct intel_soc_dts_sensors *sensors)
+{
+	u32 sticky_out;
+	int status;
+	u32 ptmc_out;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sensors->intr_notify_lock, flags);
+
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			       SOC_DTS_OFFSET_PTMC, &ptmc_out);
+	ptmc_out |= SOC_DTS_PTMC_APIC_DEASSERT_BIT;
+	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+				SOC_DTS_OFFSET_PTMC, ptmc_out);
+
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+			       SOC_DTS_OFFSET_PTTSS, &sticky_out);
+	pr_debug("status %d PTTSS %x\n", status, sticky_out);
+	if (sticky_out & SOC_DTS_TRIP_MASK) {
+		int i;
+		/* reset sticky bit */
+		status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+					SOC_DTS_OFFSET_PTTSS, sticky_out);
+		spin_unlock_irqrestore(&sensors->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(sensors->soc_dts[i].tzone);
+		}
+	} else
+		spin_unlock_irqrestore(&sensors->intr_notify_lock, flags);
+}
+EXPORT_SYMBOL_GPL(intel_soc_dts_iosf_interrupt_handler);
+
+struct intel_soc_dts_sensors *intel_soc_dts_iosf_init(
+	enum intel_soc_dts_interrupt_type intr_type, int trip_count,
+	int read_only_trip_count)
+{
+	struct intel_soc_dts_sensors *sensors;
+	bool notification;
+	u32 tj_max;
+	int ret;
+	int i;
+
+	if (!iosf_mbi_available())
+		return ERR_PTR(-ENODEV);
+
+	if (!trip_count || read_only_trip_count > trip_count)
+		return ERR_PTR(-EINVAL);
+
+	if (get_tj_max(&tj_max))
+		return ERR_PTR(-EINVAL);
+
+	sensors = kzalloc(sizeof(*sensors), GFP_KERNEL);
+	if (!sensors)
+		return ERR_PTR(-ENOMEM);
+
+	spin_lock_init(&sensors->intr_notify_lock);
+	mutex_init(&sensors->dts_update_lock);
+	sensors->intr_type = intr_type;
+	sensors->tj_max = tj_max;
+	if (intr_type == INTEL_SOC_DTS_INTERRUPT_NONE)
+		notification = false;
+	else
+		notification = true;
+	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i) {
+		sensors->soc_dts[i].sensors = sensors;
+		ret = add_dts_thermal_zone(i, &sensors->soc_dts[i],
+					   notification, trip_count,
+					   read_only_trip_count);
+		if (ret)
+			goto err_free;
+	}
+
+	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i) {
+		ret = update_trip_temp(&sensors->soc_dts[i], 0, 0,
+				       THERMAL_TRIP_PASSIVE);
+		if (ret)
+			goto err_remove_zone;
+
+		ret = update_trip_temp(&sensors->soc_dts[i], 1, 0,
+				       THERMAL_TRIP_PASSIVE);
+		if (ret)
+			goto err_remove_zone;
+	}
+
+	return sensors;
+err_remove_zone:
+	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i)
+		remove_dts_thermal_zone(&sensors->soc_dts[i]);
+
+err_free:
+	kfree(sensors);
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(intel_soc_dts_iosf_init);
+
+void intel_soc_dts_iosf_exit(struct intel_soc_dts_sensors *sensors)
+{
+	int i;
+
+	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i) {
+		update_trip_temp(&sensors->soc_dts[i], 0, 0, 0);
+		update_trip_temp(&sensors->soc_dts[i], 1, 0, 0);
+		remove_dts_thermal_zone(&sensors->soc_dts[i]);
+	}
+	kfree(sensors);
+}
+EXPORT_SYMBOL_GPL(intel_soc_dts_iosf_exit);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/intel_soc_dts_iosf.h b/drivers/thermal/intel_soc_dts_iosf.h
new file mode 100644
index 000000000000..625e37bf93dc
--- /dev/null
+++ b/drivers/thermal/intel_soc_dts_iosf.h
@@ -0,0 +1,62 @@
+/*
+ * intel_soc_dts_iosf.h
+ * Copyright (c) 2015, 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.
+ *
+ */
+
+#ifndef _INTEL_SOC_DTS_IOSF_CORE_H
+#define _INTEL_SOC_DTS_IOSF_CORE_H
+
+#include <linux/thermal.h>
+
+/* DTS0 and DTS 1 */
+#define SOC_MAX_DTS_SENSORS	2
+
+enum intel_soc_dts_interrupt_type {
+	INTEL_SOC_DTS_INTERRUPT_NONE,
+	INTEL_SOC_DTS_INTERRUPT_APIC,
+	INTEL_SOC_DTS_INTERRUPT_MSI,
+	INTEL_SOC_DTS_INTERRUPT_SCI,
+	INTEL_SOC_DTS_INTERRUPT_SMI,
+};
+
+struct intel_soc_dts_sensors;
+
+struct intel_soc_dts_sensor_entry {
+	int id;
+	u32 temp_mask;
+	u32 temp_shift;
+	u32 store_status;
+	u32 trip_mask;
+	u32 trip_count;
+	enum thermal_trip_type trip_types[2];
+	struct thermal_zone_device *tzone;
+	struct intel_soc_dts_sensors *sensors;
+};
+
+struct intel_soc_dts_sensors {
+	u32 tj_max;
+	spinlock_t intr_notify_lock;
+	struct mutex dts_update_lock;
+	enum intel_soc_dts_interrupt_type intr_type;
+	struct intel_soc_dts_sensor_entry soc_dts[SOC_MAX_DTS_SENSORS];
+};
+
+struct intel_soc_dts_sensors *intel_soc_dts_iosf_init(
+	enum intel_soc_dts_interrupt_type intr_type, int trip_count,
+	int read_only_trip_count);
+void intel_soc_dts_iosf_exit(struct intel_soc_dts_sensors *sensors);
+void intel_soc_dts_iosf_interrupt_handler(
+				struct intel_soc_dts_sensors *sensors);
+int intel_soc_dts_iosf_add_read_only_critical_trip(
+	struct intel_soc_dts_sensors *sensors, int critical_offset);
+#endif
diff --git a/drivers/thermal/intel_soc_dts_thermal.c b/drivers/thermal/intel_soc_dts_thermal.c
index 9013505e43b7..4ebb31a35a64 100644
--- a/drivers/thermal/intel_soc_dts_thermal.c
+++ b/drivers/thermal/intel_soc_dts_thermal.c
@@ -16,431 +16,54 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/module.h>
-#include <linux/slab.h>
 #include <linux/interrupt.h>
-#include <linux/thermal.h>
 #include <asm/cpu_device_id.h>
-#include <asm/iosf_mbi.h>
-
-#define SOC_DTS_OFFSET_ENABLE	0xB0
-#define SOC_DTS_OFFSET_TEMP	0xB1
-
-#define SOC_DTS_OFFSET_PTPS	0xB2
-#define SOC_DTS_OFFSET_PTTS	0xB3
-#define SOC_DTS_OFFSET_PTTSS	0xB4
-#define SOC_DTS_OFFSET_PTMC	0x80
-#define SOC_DTS_TE_AUX0		0xB5
-#define SOC_DTS_TE_AUX1		0xB6
-
-#define SOC_DTS_AUX0_ENABLE_BIT		BIT(0)
-#define SOC_DTS_AUX1_ENABLE_BIT		BIT(1)
-#define SOC_DTS_CPU_MODULE0_ENABLE_BIT	BIT(16)
-#define SOC_DTS_CPU_MODULE1_ENABLE_BIT	BIT(17)
-#define SOC_DTS_TE_SCI_ENABLE		BIT(9)
-#define SOC_DTS_TE_SMI_ENABLE		BIT(10)
-#define SOC_DTS_TE_MSI_ENABLE		BIT(11)
-#define SOC_DTS_TE_APICA_ENABLE		BIT(14)
-#define SOC_DTS_PTMC_APIC_DEASSERT_BIT	BIT(4)
-
-/* DTS encoding for TJ MAX temperature */
-#define SOC_DTS_TJMAX_ENCODING	0x7F
-
-/* IRQ 86 is a fixed APIC interrupt for BYT DTS Aux threshold notifications */
-#define BYT_SOC_DTS_APIC_IRQ	86
-
-/* Only 2 out of 4 is allowed for OSPM */
-#define SOC_MAX_DTS_TRIPS	2
-
-/* Mask for two trips in status bits */
-#define SOC_DTS_TRIP_MASK	0x03
-
-/* DTS0 and DTS 1 */
-#define SOC_MAX_DTS_SENSORS	2
+#include "intel_soc_dts_iosf.h"
 
 #define CRITICAL_OFFSET_FROM_TJ_MAX	5000
 
-struct soc_sensor_entry {
-	int id;
-	u32 tj_max;
-	u32 temp_mask;
-	u32 temp_shift;
-	u32 store_status;
-	struct thermal_zone_device *tzone;
-};
-
-static struct soc_sensor_entry *soc_dts[SOC_MAX_DTS_SENSORS];
-
 static int crit_offset = CRITICAL_OFFSET_FROM_TJ_MAX;
 module_param(crit_offset, int, 0644);
 MODULE_PARM_DESC(crit_offset,
 	"Critical Temperature offset from tj max in millidegree Celsius.");
 
-static DEFINE_MUTEX(aux_update_mutex);
-static spinlock_t intr_notify_lock;
-static int soc_dts_thres_irq;
-
-static int get_tj_max(u32 *tj_max)
-{
-	u32 eax, edx;
-	u32 val;
-	int err;
-
-	err = rdmsr_safe(MSR_IA32_TEMPERATURE_TARGET, &eax, &edx);
-	if (err)
-		goto err_ret;
-	else {
-		val = (eax >> 16) & 0xff;
-		if (val)
-			*tj_max = val * 1000;
-		else {
-			err = -EINVAL;
-			goto err_ret;
-		}
-	}
-
-	return 0;
-err_ret:
-	*tj_max = 0;
-
-	return err;
-}
-
-static int sys_get_trip_temp(struct thermal_zone_device *tzd,
-					int trip, unsigned long *temp)
-{
-	int status;
-	u32 out;
-	struct soc_sensor_entry *aux_entry;
-
-	aux_entry = tzd->devdata;
-
-	if (!trip) {
-		/* Just return the critical temp */
-		*temp = aux_entry->tj_max - crit_offset;
-		return 0;
-	}
-
-	mutex_lock(&aux_update_mutex);
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-					SOC_DTS_OFFSET_PTPS, &out);
-	mutex_unlock(&aux_update_mutex);
-	if (status)
-		return status;
-
-	out = (out >> (trip * 8)) & SOC_DTS_TJMAX_ENCODING;
-
-	if (!out)
-		*temp = 0;
-	else
-		*temp = aux_entry->tj_max - out * 1000;
-
-	return 0;
-}
-
-static int update_trip_temp(struct soc_sensor_entry *aux_entry,
-				int thres_index, unsigned long temp)
-{
-	int status;
-	u32 temp_out;
-	u32 out;
-	u32 store_ptps;
-	u32 store_ptmc;
-	u32 store_te_out;
-	u32 te_out;
-
-	u32 int_enable_bit = SOC_DTS_TE_APICA_ENABLE |
-						SOC_DTS_TE_MSI_ENABLE;
-
-	temp_out = (aux_entry->tj_max - temp) / 1000;
-
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-				SOC_DTS_OFFSET_PTPS, &store_ptps);
-	if (status)
-		return status;
-
-	out = (store_ptps & ~(0xFF << (thres_index * 8)));
-	out |= (temp_out & 0xFF) << (thres_index * 8);
-	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-				SOC_DTS_OFFSET_PTPS, out);
-	if (status)
-		return status;
-	pr_debug("update_trip_temp PTPS = %x\n", out);
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-					SOC_DTS_OFFSET_PTMC, &out);
-	if (status)
-		goto err_restore_ptps;
-
-	store_ptmc = out;
-
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-					SOC_DTS_TE_AUX0 + thres_index,
-					&te_out);
-	if (status)
-		goto err_restore_ptmc;
-
-	store_te_out = te_out;
-
-	/* Enable for CPU module 0 and module 1 */
-	out |= (SOC_DTS_CPU_MODULE0_ENABLE_BIT |
-					SOC_DTS_CPU_MODULE1_ENABLE_BIT);
-	if (temp) {
-		if (thres_index)
-			out |= SOC_DTS_AUX1_ENABLE_BIT;
-		else
-			out |= SOC_DTS_AUX0_ENABLE_BIT;
-		te_out |= int_enable_bit;
-	} else {
-		if (thres_index)
-			out &= ~SOC_DTS_AUX1_ENABLE_BIT;
-		else
-			out &= ~SOC_DTS_AUX0_ENABLE_BIT;
-		te_out &= ~int_enable_bit;
-	}
-	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-					SOC_DTS_OFFSET_PTMC, out);
-	if (status)
-		goto err_restore_te_out;
-
-	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-					SOC_DTS_TE_AUX0 + thres_index,
-					te_out);
-	if (status)
-		goto err_restore_te_out;
-
-	return 0;
-
-err_restore_te_out:
-	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-				SOC_DTS_OFFSET_PTMC, store_te_out);
-err_restore_ptmc:
-	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-				SOC_DTS_OFFSET_PTMC, store_ptmc);
-err_restore_ptps:
-	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-				SOC_DTS_OFFSET_PTPS, store_ptps);
-	/* Nothing we can do if restore fails */
-
-	return status;
-}
-
-static int sys_set_trip_temp(struct thermal_zone_device *tzd, int trip,
-							unsigned long temp)
-{
-	struct soc_sensor_entry *aux_entry = tzd->devdata;
-	int status;
-
-	if (temp > (aux_entry->tj_max - crit_offset))
-		return -EINVAL;
-
-	mutex_lock(&aux_update_mutex);
-	status = update_trip_temp(tzd->devdata, trip, temp);
-	mutex_unlock(&aux_update_mutex);
-
-	return status;
-}
-
-static int sys_get_trip_type(struct thermal_zone_device *thermal,
-		int trip, enum thermal_trip_type *type)
-{
-	if (trip)
-		*type = THERMAL_TRIP_PASSIVE;
-	else
-		*type = THERMAL_TRIP_CRITICAL;
-
-	return 0;
-}
-
-static int sys_get_curr_temp(struct thermal_zone_device *tzd,
-						unsigned long *temp)
-{
-	int status;
-	u32 out;
-	struct soc_sensor_entry *aux_entry;
-
-	aux_entry = tzd->devdata;
-
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-					SOC_DTS_OFFSET_TEMP, &out);
-	if (status)
-		return status;
-
-	out = (out & aux_entry->temp_mask) >> aux_entry->temp_shift;
-	out -= SOC_DTS_TJMAX_ENCODING;
-	*temp = aux_entry->tj_max - out * 1000;
-
-	return 0;
-}
-
-static struct thermal_zone_device_ops tzone_ops = {
-	.get_temp = sys_get_curr_temp,
-	.get_trip_temp = sys_get_trip_temp,
-	.get_trip_type = sys_get_trip_type,
-	.set_trip_temp = sys_set_trip_temp,
-};
-
-static void free_soc_dts(struct soc_sensor_entry *aux_entry)
-{
-	if (aux_entry) {
-		iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-			SOC_DTS_OFFSET_ENABLE, aux_entry->store_status);
-		thermal_zone_device_unregister(aux_entry->tzone);
-		kfree(aux_entry);
-	}
-}
-
-static int soc_dts_enable(int id)
-{
-	u32 out;
-	int ret;
-
-	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-					SOC_DTS_OFFSET_ENABLE, &out);
-	if (ret)
-		return ret;
-
-	if (!(out & BIT(id))) {
-		out |= BIT(id);
-		ret = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-					SOC_DTS_OFFSET_ENABLE, out);
-		if (ret)
-			return ret;
-	}
-
-	return ret;
-}
-
-static struct soc_sensor_entry *alloc_soc_dts(int id, u32 tj_max,
-					      bool notification_support)
-{
-	struct soc_sensor_entry *aux_entry;
-	char name[10];
-	int trip_count = 0;
-	int trip_mask = 0;
-	int err;
-
-	aux_entry = kzalloc(sizeof(*aux_entry), GFP_KERNEL);
-	if (!aux_entry) {
-		err = -ENOMEM;
-		return ERR_PTR(-ENOMEM);
-	}
-
-	/* Store status to restor on exit */
-	err = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-					SOC_DTS_OFFSET_ENABLE,
-					&aux_entry->store_status);
-	if (err)
-		goto err_ret;
-
-	aux_entry->id = id;
-	aux_entry->tj_max = tj_max;
-	aux_entry->temp_mask = 0x00FF << (id * 8);
-	aux_entry->temp_shift = id * 8;
-	if (notification_support) {
-		trip_count = SOC_MAX_DTS_TRIPS;
-		trip_mask = 0x02;
-	}
-	snprintf(name, sizeof(name), "soc_dts%d", id);
-	aux_entry->tzone = thermal_zone_device_register(name,
-							trip_count,
-							trip_mask,
-							aux_entry, &tzone_ops,
-							NULL, 0, 0);
-	if (IS_ERR(aux_entry->tzone)) {
-		err = PTR_ERR(aux_entry->tzone);
-		goto err_ret;
-	}
-
-	err = soc_dts_enable(id);
-	if (err)
-		goto err_aux_status;
-
-	return aux_entry;
-
-err_aux_status:
-	thermal_zone_device_unregister(aux_entry->tzone);
-err_ret:
-	kfree(aux_entry);
-	return ERR_PTR(err);
-}
-
-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,
-				SOC_DTS_OFFSET_PTMC, &ptmc_out);
-
-	ptmc_out |= SOC_DTS_PTMC_APIC_DEASSERT_BIT;
-	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-					SOC_DTS_OFFSET_PTMC, ptmc_out);
-
-	/* Read status here */
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-					SOC_DTS_OFFSET_PTTSS, &sticky_out);
-	pr_debug("status %d PTTSS %x\n", status, sticky_out);
-	if (sticky_out & SOC_DTS_TRIP_MASK) {
-		int i;
-		/* 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);
+/* IRQ 86 is a fixed APIC interrupt for BYT DTS Aux threshold notifications */
+#define BYT_SOC_DTS_APIC_IRQ	86
 
-}
+static int soc_dts_thres_irq;
+static struct intel_soc_dts_sensors *soc_dts;
 
 static irqreturn_t soc_irq_thread_fn(int irq, void *dev_data)
 {
-	proc_thermal_interrupt();
 	pr_debug("proc_thermal_interrupt\n");
+	intel_soc_dts_iosf_interrupt_handler(soc_dts);
 
 	return IRQ_HANDLED;
 }
 
 static const struct x86_cpu_id soc_thermal_ids[] = {
 	{ X86_VENDOR_INTEL, X86_FAMILY_ANY, 0x37, 0, BYT_SOC_DTS_APIC_IRQ},
-	{ X86_VENDOR_INTEL, X86_FAMILY_ANY, 0x4c, 0, 0},
 	{}
 };
 MODULE_DEVICE_TABLE(x86cpu, soc_thermal_ids);
 
 static int __init intel_soc_thermal_init(void)
 {
-	u32 tj_max;
 	int err = 0;
-	int i;
 	const struct x86_cpu_id *match_cpu;
 
 	match_cpu = x86_match_cpu(soc_thermal_ids);
 	if (!match_cpu)
 		return -ENODEV;
 
-	if (get_tj_max(&tj_max))
-		return -EINVAL;
-
-	soc_dts_thres_irq = (int)match_cpu->driver_data;
-
-	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i) {
-		soc_dts[i] = alloc_soc_dts(i, tj_max,
-					   soc_dts_thres_irq ? true : false);
-		if (IS_ERR(soc_dts[i])) {
-			err = PTR_ERR(soc_dts[i]);
-			goto err_free;
-		}
+	/* Create a zone with 2 trips with marked as read only */
+	soc_dts = intel_soc_dts_iosf_init(INTEL_SOC_DTS_INTERRUPT_APIC, 2, 1);
+	if (IS_ERR(soc_dts)) {
+		err = PTR_ERR(soc_dts);
+		return err;
 	}
 
-	spin_lock_init(&intr_notify_lock);
+	soc_dts_thres_irq = (int)match_cpu->driver_data;
 
 	if (soc_dts_thres_irq) {
 		err = request_threaded_irq(soc_dts_thres_irq, NULL,
@@ -449,42 +72,31 @@ static int __init intel_soc_thermal_init(void)
 					   "soc_dts", soc_dts);
 		if (err) {
 			pr_err("request_threaded_irq ret %d\n", err);
-			goto err_free;
+			goto error_irq;
 		}
 	}
 
-	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i) {
-		err = update_trip_temp(soc_dts[i], 0, tj_max - crit_offset);
-		if (err)
-			goto err_trip_temp;
-	}
+	err = intel_soc_dts_iosf_add_read_only_critical_trip(soc_dts,
+							     crit_offset);
+	if (err)
+		goto error_trips;
 
 	return 0;
 
-err_trip_temp:
-	i = SOC_MAX_DTS_SENSORS;
+error_trips:
 	if (soc_dts_thres_irq)
 		free_irq(soc_dts_thres_irq, soc_dts);
-err_free:
-	while (--i >= 0)
-		free_soc_dts(soc_dts[i]);
+error_irq:
+	intel_soc_dts_iosf_exit(soc_dts);
 
 	return err;
 }
 
 static void __exit intel_soc_thermal_exit(void)
 {
-	int i;
-
-	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i)
-		update_trip_temp(soc_dts[i], 0, 0);
-
 	if (soc_dts_thres_irq)
 		free_irq(soc_dts_thres_irq, soc_dts);
-
-	for (i = 0; i < SOC_MAX_DTS_SENSORS; ++i)
-		free_soc_dts(soc_dts[i]);
-
+	intel_soc_dts_iosf_exit(soc_dts);
 }
 
 module_init(intel_soc_thermal_init)
diff --git a/drivers/thermal/of-thermal.c b/drivers/thermal/of-thermal.c
index 668fb1bdea9e..b295b2b6c191 100644
--- a/drivers/thermal/of-thermal.c
+++ b/drivers/thermal/of-thermal.c
@@ -58,6 +58,8 @@ struct __thermal_bind_params {
  * @mode: current thermal zone device mode (enabled/disabled)
  * @passive_delay: polling interval while passive cooling is activated
  * @polling_delay: zone polling interval
+ * @slope: slope of the temperature adjustment curve
+ * @offset: offset of the temperature adjustment curve
  * @ntrips: number of trip points
  * @trips: an array of trip points (0..ntrips - 1)
  * @num_tbps: number of thermal bind params
@@ -70,6 +72,8 @@ struct __thermal_zone {
 	enum thermal_device_mode mode;
 	int passive_delay;
 	int polling_delay;
+	int slope;
+	int offset;
 
 	/* trip data */
 	int ntrips;
@@ -227,7 +231,8 @@ static int of_thermal_bind(struct thermal_zone_device *thermal,
 			ret = thermal_zone_bind_cooling_device(thermal,
 						tbp->trip_id, cdev,
 						tbp->max,
-						tbp->min);
+						tbp->min,
+						tbp->usage);
 			if (ret)
 				return ret;
 		}
@@ -581,7 +586,7 @@ static int thermal_of_populate_bind_params(struct device_node *np,
 	u32 prop;
 
 	/* Default weight. Usage is optional */
-	__tbp->usage = 0;
+	__tbp->usage = THERMAL_WEIGHT_DEFAULT;
 	ret = of_property_read_u32(np, "contribution", &prop);
 	if (ret == 0)
 		__tbp->usage = prop;
@@ -715,7 +720,7 @@ static int thermal_of_populate_trip(struct device_node *np,
  * @np parameter and fills the read data into a __thermal_zone data structure
  * and return this pointer.
  *
- * TODO: Missing properties to parse: thermal-sensor-names and coefficients
+ * TODO: Missing properties to parse: thermal-sensor-names
  *
  * Return: On success returns a valid struct __thermal_zone,
  * otherwise, it returns a corresponding ERR_PTR(). Caller must
@@ -727,7 +732,7 @@ thermal_of_build_thermal_zone(struct device_node *np)
 	struct device_node *child = NULL, *gchild;
 	struct __thermal_zone *tz;
 	int ret, i;
-	u32 prop;
+	u32 prop, coef[2];
 
 	if (!np) {
 		pr_err("no thermal zone np\n");
@@ -752,6 +757,20 @@ thermal_of_build_thermal_zone(struct device_node *np)
 	}
 	tz->polling_delay = prop;
 
+	/*
+	 * REVIST: for now, the thermal framework supports only
+	 * one sensor per thermal zone. Thus, we are considering
+	 * only the first two values as slope and offset.
+	 */
+	ret = of_property_read_u32_array(np, "coefficients", coef, 2);
+	if (ret == 0) {
+		tz->slope = coef[0];
+		tz->offset = coef[1];
+	} else {
+		tz->slope = 1;
+		tz->offset = 0;
+	}
+
 	/* trips */
 	child = of_get_child_by_name(np, "trips");
 
@@ -865,6 +884,8 @@ int __init of_parse_thermal_zones(void)
 	for_each_child_of_node(np, child) {
 		struct thermal_zone_device *zone;
 		struct thermal_zone_params *tzp;
+		int i, mask = 0;
+		u32 prop;
 
 		/* Check whether child is enabled or not */
 		if (!of_device_is_available(child))
@@ -891,8 +912,18 @@ int __init of_parse_thermal_zones(void)
 		/* No hwmon because there might be hwmon drivers registering */
 		tzp->no_hwmon = true;
 
+		if (!of_property_read_u32(child, "sustainable-power", &prop))
+			tzp->sustainable_power = prop;
+
+		for (i = 0; i < tz->ntrips; i++)
+			mask |= 1 << i;
+
+		/* these two are left for temperature drivers to use */
+		tzp->slope = tz->slope;
+		tzp->offset = tz->offset;
+
 		zone = thermal_zone_device_register(child->name, tz->ntrips,
-						    0, tz,
+						    mask, tz,
 						    ops, tzp,
 						    tz->passive_delay,
 						    tz->polling_delay);
diff --git a/drivers/thermal/power_allocator.c b/drivers/thermal/power_allocator.c
new file mode 100644
index 000000000000..4672250b329f
--- /dev/null
+++ b/drivers/thermal/power_allocator.c
@@ -0,0 +1,539 @@
+/*
+ * A power allocator to manage temperature
+ *
+ * Copyright (C) 2014 ARM Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#define pr_fmt(fmt) "Power allocator: " fmt
+
+#include <linux/rculist.h>
+#include <linux/slab.h>
+#include <linux/thermal.h>
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/thermal_power_allocator.h>
+
+#include "thermal_core.h"
+
+#define FRAC_BITS 10
+#define int_to_frac(x) ((x) << FRAC_BITS)
+#define frac_to_int(x) ((x) >> FRAC_BITS)
+
+/**
+ * mul_frac() - multiply two fixed-point numbers
+ * @x:	first multiplicand
+ * @y:	second multiplicand
+ *
+ * Return: the result of multiplying two fixed-point numbers.  The
+ * result is also a fixed-point number.
+ */
+static inline s64 mul_frac(s64 x, s64 y)
+{
+	return (x * y) >> FRAC_BITS;
+}
+
+/**
+ * div_frac() - divide two fixed-point numbers
+ * @x:	the dividend
+ * @y:	the divisor
+ *
+ * Return: the result of dividing two fixed-point numbers.  The
+ * result is also a fixed-point number.
+ */
+static inline s64 div_frac(s64 x, s64 y)
+{
+	return div_s64(x << FRAC_BITS, y);
+}
+
+/**
+ * struct power_allocator_params - parameters for the power allocator governor
+ * @err_integral:	accumulated error in the PID controller.
+ * @prev_err:	error in the previous iteration of the PID controller.
+ *		Used to calculate the derivative term.
+ * @trip_switch_on:	first passive trip point of the thermal zone.  The
+ *			governor switches on when this trip point is crossed.
+ * @trip_max_desired_temperature:	last passive trip point of the thermal
+ *					zone.  The temperature we are
+ *					controlling for.
+ */
+struct power_allocator_params {
+	s64 err_integral;
+	s32 prev_err;
+	int trip_switch_on;
+	int trip_max_desired_temperature;
+};
+
+/**
+ * pid_controller() - PID controller
+ * @tz:	thermal zone we are operating in
+ * @current_temp:	the current temperature in millicelsius
+ * @control_temp:	the target temperature in millicelsius
+ * @max_allocatable_power:	maximum allocatable power for this thermal zone
+ *
+ * This PID controller increases the available power budget so that the
+ * temperature of the thermal zone gets as close as possible to
+ * @control_temp and limits the power if it exceeds it.  k_po is the
+ * proportional term when we are overshooting, k_pu is the
+ * proportional term when we are undershooting.  integral_cutoff is a
+ * threshold below which we stop accumulating the error.  The
+ * accumulated error is only valid if the requested power will make
+ * the system warmer.  If the system is mostly idle, there's no point
+ * in accumulating positive error.
+ *
+ * Return: The power budget for the next period.
+ */
+static u32 pid_controller(struct thermal_zone_device *tz,
+			  unsigned long current_temp,
+			  unsigned long control_temp,
+			  u32 max_allocatable_power)
+{
+	s64 p, i, d, power_range;
+	s32 err, max_power_frac;
+	struct power_allocator_params *params = tz->governor_data;
+
+	max_power_frac = int_to_frac(max_allocatable_power);
+
+	err = ((s32)control_temp - (s32)current_temp);
+	err = int_to_frac(err);
+
+	/* Calculate the proportional term */
+	p = mul_frac(err < 0 ? tz->tzp->k_po : tz->tzp->k_pu, err);
+
+	/*
+	 * Calculate the integral term
+	 *
+	 * if the error is less than cut off allow integration (but
+	 * the integral is limited to max power)
+	 */
+	i = mul_frac(tz->tzp->k_i, params->err_integral);
+
+	if (err < int_to_frac(tz->tzp->integral_cutoff)) {
+		s64 i_next = i + mul_frac(tz->tzp->k_i, err);
+
+		if (abs64(i_next) < max_power_frac) {
+			i = i_next;
+			params->err_integral += err;
+		}
+	}
+
+	/*
+	 * Calculate the derivative term
+	 *
+	 * We do err - prev_err, so with a positive k_d, a decreasing
+	 * error (i.e. driving closer to the line) results in less
+	 * power being applied, slowing down the controller)
+	 */
+	d = mul_frac(tz->tzp->k_d, err - params->prev_err);
+	d = div_frac(d, tz->passive_delay);
+	params->prev_err = err;
+
+	power_range = p + i + d;
+
+	/* feed-forward the known sustainable dissipatable power */
+	power_range = tz->tzp->sustainable_power + frac_to_int(power_range);
+
+	power_range = clamp(power_range, (s64)0, (s64)max_allocatable_power);
+
+	trace_thermal_power_allocator_pid(tz, frac_to_int(err),
+					  frac_to_int(params->err_integral),
+					  frac_to_int(p), frac_to_int(i),
+					  frac_to_int(d), power_range);
+
+	return power_range;
+}
+
+/**
+ * divvy_up_power() - divvy the allocated power between the actors
+ * @req_power:	each actor's requested power
+ * @max_power:	each actor's maximum available power
+ * @num_actors:	size of the @req_power, @max_power and @granted_power's array
+ * @total_req_power: sum of @req_power
+ * @power_range:	total allocated power
+ * @granted_power:	output array: each actor's granted power
+ * @extra_actor_power:	an appropriately sized array to be used in the
+ *			function as temporary storage of the extra power given
+ *			to the actors
+ *
+ * This function divides the total allocated power (@power_range)
+ * fairly between the actors.  It first tries to give each actor a
+ * share of the @power_range according to how much power it requested
+ * compared to the rest of the actors.  For example, if only one actor
+ * requests power, then it receives all the @power_range.  If
+ * three actors each requests 1mW, each receives a third of the
+ * @power_range.
+ *
+ * If any actor received more than their maximum power, then that
+ * surplus is re-divvied among the actors based on how far they are
+ * from their respective maximums.
+ *
+ * Granted power for each actor is written to @granted_power, which
+ * should've been allocated by the calling function.
+ */
+static void divvy_up_power(u32 *req_power, u32 *max_power, int num_actors,
+			   u32 total_req_power, u32 power_range,
+			   u32 *granted_power, u32 *extra_actor_power)
+{
+	u32 extra_power, capped_extra_power;
+	int i;
+
+	/*
+	 * Prevent division by 0 if none of the actors request power.
+	 */
+	if (!total_req_power)
+		total_req_power = 1;
+
+	capped_extra_power = 0;
+	extra_power = 0;
+	for (i = 0; i < num_actors; i++) {
+		u64 req_range = req_power[i] * power_range;
+
+		granted_power[i] = DIV_ROUND_CLOSEST_ULL(req_range,
+							 total_req_power);
+
+		if (granted_power[i] > max_power[i]) {
+			extra_power += granted_power[i] - max_power[i];
+			granted_power[i] = max_power[i];
+		}
+
+		extra_actor_power[i] = max_power[i] - granted_power[i];
+		capped_extra_power += extra_actor_power[i];
+	}
+
+	if (!extra_power)
+		return;
+
+	/*
+	 * Re-divvy the reclaimed extra among actors based on
+	 * how far they are from the max
+	 */
+	extra_power = min(extra_power, capped_extra_power);
+	if (capped_extra_power > 0)
+		for (i = 0; i < num_actors; i++)
+			granted_power[i] += (extra_actor_power[i] *
+					extra_power) / capped_extra_power;
+}
+
+static int allocate_power(struct thermal_zone_device *tz,
+			  unsigned long current_temp,
+			  unsigned long control_temp)
+{
+	struct thermal_instance *instance;
+	struct power_allocator_params *params = tz->governor_data;
+	u32 *req_power, *max_power, *granted_power, *extra_actor_power;
+	u32 total_req_power, max_allocatable_power;
+	u32 total_granted_power, power_range;
+	int i, num_actors, total_weight, ret = 0;
+	int trip_max_desired_temperature = params->trip_max_desired_temperature;
+
+	mutex_lock(&tz->lock);
+
+	num_actors = 0;
+	total_weight = 0;
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+		if ((instance->trip == trip_max_desired_temperature) &&
+		    cdev_is_power_actor(instance->cdev)) {
+			num_actors++;
+			total_weight += instance->weight;
+		}
+	}
+
+	/*
+	 * We need to allocate three arrays of the same size:
+	 * req_power, max_power and granted_power.  They are going to
+	 * be needed until this function returns.  Allocate them all
+	 * in one go to simplify the allocation and deallocation
+	 * logic.
+	 */
+	BUILD_BUG_ON(sizeof(*req_power) != sizeof(*max_power));
+	BUILD_BUG_ON(sizeof(*req_power) != sizeof(*granted_power));
+	BUILD_BUG_ON(sizeof(*req_power) != sizeof(*extra_actor_power));
+	req_power = devm_kcalloc(&tz->device, num_actors * 4,
+				 sizeof(*req_power), GFP_KERNEL);
+	if (!req_power) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	max_power = &req_power[num_actors];
+	granted_power = &req_power[2 * num_actors];
+	extra_actor_power = &req_power[3 * num_actors];
+
+	i = 0;
+	total_req_power = 0;
+	max_allocatable_power = 0;
+
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+		int weight;
+		struct thermal_cooling_device *cdev = instance->cdev;
+
+		if (instance->trip != trip_max_desired_temperature)
+			continue;
+
+		if (!cdev_is_power_actor(cdev))
+			continue;
+
+		if (cdev->ops->get_requested_power(cdev, tz, &req_power[i]))
+			continue;
+
+		if (!total_weight)
+			weight = 1 << FRAC_BITS;
+		else
+			weight = instance->weight;
+
+		req_power[i] = frac_to_int(weight * req_power[i]);
+
+		if (power_actor_get_max_power(cdev, tz, &max_power[i]))
+			continue;
+
+		total_req_power += req_power[i];
+		max_allocatable_power += max_power[i];
+
+		i++;
+	}
+
+	power_range = pid_controller(tz, current_temp, control_temp,
+				     max_allocatable_power);
+
+	divvy_up_power(req_power, max_power, num_actors, total_req_power,
+		       power_range, granted_power, extra_actor_power);
+
+	total_granted_power = 0;
+	i = 0;
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+		if (instance->trip != trip_max_desired_temperature)
+			continue;
+
+		if (!cdev_is_power_actor(instance->cdev))
+			continue;
+
+		power_actor_set_power(instance->cdev, instance,
+				      granted_power[i]);
+		total_granted_power += granted_power[i];
+
+		i++;
+	}
+
+	trace_thermal_power_allocator(tz, req_power, total_req_power,
+				      granted_power, total_granted_power,
+				      num_actors, power_range,
+				      max_allocatable_power, current_temp,
+				      (s32)control_temp - (s32)current_temp);
+
+	devm_kfree(&tz->device, req_power);
+unlock:
+	mutex_unlock(&tz->lock);
+
+	return ret;
+}
+
+static int get_governor_trips(struct thermal_zone_device *tz,
+			      struct power_allocator_params *params)
+{
+	int i, ret, last_passive;
+	bool found_first_passive;
+
+	found_first_passive = false;
+	last_passive = -1;
+	ret = -EINVAL;
+
+	for (i = 0; i < tz->trips; i++) {
+		enum thermal_trip_type type;
+
+		ret = tz->ops->get_trip_type(tz, i, &type);
+		if (ret)
+			return ret;
+
+		if (!found_first_passive) {
+			if (type == THERMAL_TRIP_PASSIVE) {
+				params->trip_switch_on = i;
+				found_first_passive = true;
+			}
+		} else if (type == THERMAL_TRIP_PASSIVE) {
+			last_passive = i;
+		} else {
+			break;
+		}
+	}
+
+	if (last_passive != -1) {
+		params->trip_max_desired_temperature = last_passive;
+		ret = 0;
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static void reset_pid_controller(struct power_allocator_params *params)
+{
+	params->err_integral = 0;
+	params->prev_err = 0;
+}
+
+static void allow_maximum_power(struct thermal_zone_device *tz)
+{
+	struct thermal_instance *instance;
+	struct power_allocator_params *params = tz->governor_data;
+
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+		if ((instance->trip != params->trip_max_desired_temperature) ||
+		    (!cdev_is_power_actor(instance->cdev)))
+			continue;
+
+		instance->target = 0;
+		instance->cdev->updated = false;
+		thermal_cdev_update(instance->cdev);
+	}
+}
+
+/**
+ * power_allocator_bind() - bind the power_allocator governor to a thermal zone
+ * @tz:	thermal zone to bind it to
+ *
+ * Check that the thermal zone is valid for this governor, that is, it
+ * has two thermal trips.  If so, initialize the PID controller
+ * parameters and bind it to the thermal zone.
+ *
+ * Return: 0 on success, -EINVAL if the trips were invalid or -ENOMEM
+ * if we ran out of memory.
+ */
+static int power_allocator_bind(struct thermal_zone_device *tz)
+{
+	int ret;
+	struct power_allocator_params *params;
+	unsigned long switch_on_temp, control_temp;
+	u32 temperature_threshold;
+
+	if (!tz->tzp || !tz->tzp->sustainable_power) {
+		dev_err(&tz->device,
+			"power_allocator: missing sustainable_power\n");
+		return -EINVAL;
+	}
+
+	params = devm_kzalloc(&tz->device, sizeof(*params), GFP_KERNEL);
+	if (!params)
+		return -ENOMEM;
+
+	ret = get_governor_trips(tz, params);
+	if (ret) {
+		dev_err(&tz->device,
+			"thermal zone %s has wrong trip setup for power allocator\n",
+			tz->type);
+		goto free;
+	}
+
+	ret = tz->ops->get_trip_temp(tz, params->trip_switch_on,
+				     &switch_on_temp);
+	if (ret)
+		goto free;
+
+	ret = tz->ops->get_trip_temp(tz, params->trip_max_desired_temperature,
+				     &control_temp);
+	if (ret)
+		goto free;
+
+	temperature_threshold = control_temp - switch_on_temp;
+
+	tz->tzp->k_po = tz->tzp->k_po ?:
+		int_to_frac(tz->tzp->sustainable_power) / temperature_threshold;
+	tz->tzp->k_pu = tz->tzp->k_pu ?:
+		int_to_frac(2 * tz->tzp->sustainable_power) /
+		temperature_threshold;
+	tz->tzp->k_i = tz->tzp->k_i ?: int_to_frac(10) / 1000;
+	/*
+	 * The default for k_d and integral_cutoff is 0, so we can
+	 * leave them as they are.
+	 */
+
+	reset_pid_controller(params);
+
+	tz->governor_data = params;
+
+	return 0;
+
+free:
+	devm_kfree(&tz->device, params);
+	return ret;
+}
+
+static void power_allocator_unbind(struct thermal_zone_device *tz)
+{
+	dev_dbg(&tz->device, "Unbinding from thermal zone %d\n", tz->id);
+	devm_kfree(&tz->device, tz->governor_data);
+	tz->governor_data = NULL;
+}
+
+static int power_allocator_throttle(struct thermal_zone_device *tz, int trip)
+{
+	int ret;
+	unsigned long switch_on_temp, control_temp, current_temp;
+	struct power_allocator_params *params = tz->governor_data;
+
+	/*
+	 * We get called for every trip point but we only need to do
+	 * our calculations once
+	 */
+	if (trip != params->trip_max_desired_temperature)
+		return 0;
+
+	ret = thermal_zone_get_temp(tz, &current_temp);
+	if (ret) {
+		dev_warn(&tz->device, "Failed to get temperature: %d\n", ret);
+		return ret;
+	}
+
+	ret = tz->ops->get_trip_temp(tz, params->trip_switch_on,
+				     &switch_on_temp);
+	if (ret) {
+		dev_warn(&tz->device,
+			 "Failed to get switch on temperature: %d\n", ret);
+		return ret;
+	}
+
+	if (current_temp < switch_on_temp) {
+		tz->passive = 0;
+		reset_pid_controller(params);
+		allow_maximum_power(tz);
+		return 0;
+	}
+
+	tz->passive = 1;
+
+	ret = tz->ops->get_trip_temp(tz, params->trip_max_desired_temperature,
+				&control_temp);
+	if (ret) {
+		dev_warn(&tz->device,
+			 "Failed to get the maximum desired temperature: %d\n",
+			 ret);
+		return ret;
+	}
+
+	return allocate_power(tz, current_temp, control_temp);
+}
+
+static struct thermal_governor thermal_gov_power_allocator = {
+	.name		= "power_allocator",
+	.bind_to_tz	= power_allocator_bind,
+	.unbind_from_tz	= power_allocator_unbind,
+	.throttle	= power_allocator_throttle,
+};
+
+int thermal_gov_power_allocator_register(void)
+{
+	return thermal_register_governor(&thermal_gov_power_allocator);
+}
+
+void thermal_gov_power_allocator_unregister(void)
+{
+	thermal_unregister_governor(&thermal_gov_power_allocator);
+}
diff --git a/drivers/thermal/qcom-spmi-temp-alarm.c b/drivers/thermal/qcom-spmi-temp-alarm.c
new file mode 100644
index 000000000000..c8d27b8fb9ec
--- /dev/null
+++ b/drivers/thermal/qcom-spmi-temp-alarm.c
@@ -0,0 +1,309 @@
+/*
+ * Copyright (c) 2011-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.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/iio/consumer.h>
+#include <linux/interrupt.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/thermal.h>
+
+#define QPNP_TM_REG_TYPE		0x04
+#define QPNP_TM_REG_SUBTYPE		0x05
+#define QPNP_TM_REG_STATUS		0x08
+#define QPNP_TM_REG_SHUTDOWN_CTRL1	0x40
+#define QPNP_TM_REG_ALARM_CTRL		0x46
+
+#define QPNP_TM_TYPE			0x09
+#define QPNP_TM_SUBTYPE			0x08
+
+#define STATUS_STAGE_MASK		0x03
+
+#define SHUTDOWN_CTRL1_THRESHOLD_MASK	0x03
+
+#define ALARM_CTRL_FORCE_ENABLE		0x80
+
+/*
+ * Trip point values based on threshold control
+ * 0 = {105 C, 125 C, 145 C}
+ * 1 = {110 C, 130 C, 150 C}
+ * 2 = {115 C, 135 C, 155 C}
+ * 3 = {120 C, 140 C, 160 C}
+*/
+#define TEMP_STAGE_STEP			20000	/* Stage step: 20.000 C */
+#define TEMP_STAGE_HYSTERESIS		2000
+
+#define TEMP_THRESH_MIN			105000	/* Threshold Min: 105 C */
+#define TEMP_THRESH_STEP		5000	/* Threshold step: 5 C */
+
+#define THRESH_MIN			0
+
+/* Temperature in Milli Celsius reported during stage 0 if no ADC is present */
+#define DEFAULT_TEMP			37000
+
+struct qpnp_tm_chip {
+	struct regmap			*map;
+	struct thermal_zone_device	*tz_dev;
+	long				temp;
+	unsigned int			thresh;
+	unsigned int			stage;
+	unsigned int			prev_stage;
+	unsigned int			base;
+	struct iio_channel		*adc;
+};
+
+static int qpnp_tm_read(struct qpnp_tm_chip *chip, u16 addr, u8 *data)
+{
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(chip->map, chip->base + addr, &val);
+	if (ret < 0)
+		return ret;
+
+	*data = val;
+	return 0;
+}
+
+static int qpnp_tm_write(struct qpnp_tm_chip *chip, u16 addr, u8 data)
+{
+	return regmap_write(chip->map, chip->base + addr, data);
+}
+
+/*
+ * This function updates the internal temp value based on the
+ * current thermal stage and threshold as well as the previous stage
+ */
+static int qpnp_tm_update_temp_no_adc(struct qpnp_tm_chip *chip)
+{
+	unsigned int stage;
+	int ret;
+	u8 reg = 0;
+
+	ret = qpnp_tm_read(chip, QPNP_TM_REG_STATUS, &reg);
+	if (ret < 0)
+		return ret;
+
+	stage = reg & STATUS_STAGE_MASK;
+
+	if (stage > chip->stage) {
+		/* increasing stage, use lower bound */
+		chip->temp = (stage - 1) * TEMP_STAGE_STEP +
+			     chip->thresh * TEMP_THRESH_STEP +
+			     TEMP_STAGE_HYSTERESIS + TEMP_THRESH_MIN;
+	} else if (stage < chip->stage) {
+		/* decreasing stage, use upper bound */
+		chip->temp = stage * TEMP_STAGE_STEP +
+			     chip->thresh * TEMP_THRESH_STEP -
+			     TEMP_STAGE_HYSTERESIS + TEMP_THRESH_MIN;
+	}
+
+	chip->stage = stage;
+
+	return 0;
+}
+
+static int qpnp_tm_get_temp(void *data, long *temp)
+{
+	struct qpnp_tm_chip *chip = data;
+	int ret, mili_celsius;
+
+	if (!temp)
+		return -EINVAL;
+
+	if (IS_ERR(chip->adc)) {
+		ret = qpnp_tm_update_temp_no_adc(chip);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = iio_read_channel_processed(chip->adc, &mili_celsius);
+		if (ret < 0)
+			return ret;
+
+		chip->temp = mili_celsius;
+	}
+
+	*temp = chip->temp < 0 ? 0 : chip->temp;
+
+	return 0;
+}
+
+static const struct thermal_zone_of_device_ops qpnp_tm_sensor_ops = {
+	.get_temp = qpnp_tm_get_temp,
+};
+
+static irqreturn_t qpnp_tm_isr(int irq, void *data)
+{
+	struct qpnp_tm_chip *chip = data;
+
+	thermal_zone_device_update(chip->tz_dev);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * This function initializes the internal temp value based on only the
+ * current thermal stage and threshold. Setup threshold control and
+ * disable shutdown override.
+ */
+static int qpnp_tm_init(struct qpnp_tm_chip *chip)
+{
+	int ret;
+	u8 reg;
+
+	chip->thresh = THRESH_MIN;
+	chip->temp = DEFAULT_TEMP;
+
+	ret = qpnp_tm_read(chip, QPNP_TM_REG_STATUS, &reg);
+	if (ret < 0)
+		return ret;
+
+	chip->stage = reg & STATUS_STAGE_MASK;
+
+	if (chip->stage)
+		chip->temp = chip->thresh * TEMP_THRESH_STEP +
+			     (chip->stage - 1) * TEMP_STAGE_STEP +
+			     TEMP_THRESH_MIN;
+
+	/*
+	 * Set threshold and disable software override of stage 2 and 3
+	 * shutdowns.
+	 */
+	reg = chip->thresh & SHUTDOWN_CTRL1_THRESHOLD_MASK;
+	ret = qpnp_tm_write(chip, QPNP_TM_REG_SHUTDOWN_CTRL1, reg);
+	if (ret < 0)
+		return ret;
+
+	/* Enable the thermal alarm PMIC module in always-on mode. */
+	reg = ALARM_CTRL_FORCE_ENABLE;
+	ret = qpnp_tm_write(chip, QPNP_TM_REG_ALARM_CTRL, reg);
+
+	return ret;
+}
+
+static int qpnp_tm_probe(struct platform_device *pdev)
+{
+	struct qpnp_tm_chip *chip;
+	struct device_node *node;
+	u8 type, subtype;
+	u32 res[2];
+	int ret, irq;
+
+	node = pdev->dev.of_node;
+
+	chip = devm_kzalloc(&pdev->dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	dev_set_drvdata(&pdev->dev, chip);
+
+	chip->map = dev_get_regmap(pdev->dev.parent, NULL);
+	if (!chip->map)
+		return -ENXIO;
+
+	ret = of_property_read_u32_array(node, "reg", res, 2);
+	if (ret < 0)
+		return ret;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	/* ADC based measurements are optional */
+	chip->adc = iio_channel_get(&pdev->dev, "thermal");
+	if (PTR_ERR(chip->adc) == -EPROBE_DEFER)
+		return PTR_ERR(chip->adc);
+
+	chip->base = res[0];
+
+	ret = qpnp_tm_read(chip, QPNP_TM_REG_TYPE, &type);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "could not read type\n");
+		goto fail;
+	}
+
+	ret = qpnp_tm_read(chip, QPNP_TM_REG_SUBTYPE, &subtype);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "could not read subtype\n");
+		goto fail;
+	}
+
+	if (type != QPNP_TM_TYPE || subtype != QPNP_TM_SUBTYPE) {
+		dev_err(&pdev->dev, "invalid type 0x%02x or subtype 0x%02x\n",
+			type, subtype);
+		ret = -ENODEV;
+		goto fail;
+	}
+
+	ret = qpnp_tm_init(chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "init failed\n");
+		goto fail;
+	}
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL, qpnp_tm_isr,
+					IRQF_ONESHOT, node->name, chip);
+	if (ret < 0)
+		goto fail;
+
+	chip->tz_dev = thermal_zone_of_sensor_register(&pdev->dev, 0, chip,
+							&qpnp_tm_sensor_ops);
+	if (IS_ERR(chip->tz_dev)) {
+		dev_err(&pdev->dev, "failed to register sensor\n");
+		ret = PTR_ERR(chip->tz_dev);
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	if (!IS_ERR(chip->adc))
+		iio_channel_release(chip->adc);
+
+	return ret;
+}
+
+static int qpnp_tm_remove(struct platform_device *pdev)
+{
+	struct qpnp_tm_chip *chip = dev_get_drvdata(&pdev->dev);
+
+	thermal_zone_of_sensor_unregister(&pdev->dev, chip->tz_dev);
+	if (!IS_ERR(chip->adc))
+		iio_channel_release(chip->adc);
+
+	return 0;
+}
+
+static const struct of_device_id qpnp_tm_match_table[] = {
+	{ .compatible = "qcom,spmi-temp-alarm" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, qpnp_tm_match_table);
+
+static struct platform_driver qpnp_tm_driver = {
+	.driver = {
+		.name = "spmi-temp-alarm",
+		.of_match_table = qpnp_tm_match_table,
+	},
+	.probe  = qpnp_tm_probe,
+	.remove = qpnp_tm_remove,
+};
+module_platform_driver(qpnp_tm_driver);
+
+MODULE_ALIAS("platform:spmi-temp-alarm");
+MODULE_DESCRIPTION("QPNP PMIC Temperature Alarm driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/samsung/exynos_tmu.c b/drivers/thermal/samsung/exynos_tmu.c
index 1d30b0975651..531f4b179871 100644
--- a/drivers/thermal/samsung/exynos_tmu.c
+++ b/drivers/thermal/samsung/exynos_tmu.c
@@ -97,6 +97,32 @@
 #define EXYNOS4412_MUX_ADDR_VALUE          6
 #define EXYNOS4412_MUX_ADDR_SHIFT          20
 
+/* Exynos5433 specific registers */
+#define EXYNOS5433_TMU_REG_CONTROL1		0x024
+#define EXYNOS5433_TMU_SAMPLING_INTERVAL	0x02c
+#define EXYNOS5433_TMU_COUNTER_VALUE0		0x030
+#define EXYNOS5433_TMU_COUNTER_VALUE1		0x034
+#define EXYNOS5433_TMU_REG_CURRENT_TEMP1	0x044
+#define EXYNOS5433_THD_TEMP_RISE3_0		0x050
+#define EXYNOS5433_THD_TEMP_RISE7_4		0x054
+#define EXYNOS5433_THD_TEMP_FALL3_0		0x060
+#define EXYNOS5433_THD_TEMP_FALL7_4		0x064
+#define EXYNOS5433_TMU_REG_INTEN		0x0c0
+#define EXYNOS5433_TMU_REG_INTPEND		0x0c8
+#define EXYNOS5433_TMU_EMUL_CON			0x110
+#define EXYNOS5433_TMU_PD_DET_EN		0x130
+
+#define EXYNOS5433_TRIMINFO_SENSOR_ID_SHIFT	16
+#define EXYNOS5433_TRIMINFO_CALIB_SEL_SHIFT	23
+#define EXYNOS5433_TRIMINFO_SENSOR_ID_MASK	\
+			(0xf << EXYNOS5433_TRIMINFO_SENSOR_ID_SHIFT)
+#define EXYNOS5433_TRIMINFO_CALIB_SEL_MASK	BIT(23)
+
+#define EXYNOS5433_TRIMINFO_ONE_POINT_TRIMMING	0
+#define EXYNOS5433_TRIMINFO_TWO_POINT_TRIMMING	1
+
+#define EXYNOS5433_PD_DET_EN			1
+
 /*exynos5440 specific registers*/
 #define EXYNOS5440_TMU_S0_7_TRIM		0x000
 #define EXYNOS5440_TMU_S0_7_CTRL		0x020
@@ -484,6 +510,101 @@ out:
 	return ret;
 }
 
+static int exynos5433_tmu_initialize(struct platform_device *pdev)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+	struct exynos_tmu_platform_data *pdata = data->pdata;
+	struct thermal_zone_device *tz = data->tzd;
+	unsigned int status, trim_info;
+	unsigned int rising_threshold = 0, falling_threshold = 0;
+	unsigned long temp, temp_hist;
+	int ret = 0, threshold_code, i, sensor_id, cal_type;
+
+	status = readb(data->base + EXYNOS_TMU_REG_STATUS);
+	if (!status) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	trim_info = readl(data->base + EXYNOS_TMU_REG_TRIMINFO);
+	sanitize_temp_error(data, trim_info);
+
+	/* Read the temperature sensor id */
+	sensor_id = (trim_info & EXYNOS5433_TRIMINFO_SENSOR_ID_MASK)
+				>> EXYNOS5433_TRIMINFO_SENSOR_ID_SHIFT;
+	dev_info(&pdev->dev, "Temperature sensor ID: 0x%x\n", sensor_id);
+
+	/* Read the calibration mode */
+	writel(trim_info, data->base + EXYNOS_TMU_REG_TRIMINFO);
+	cal_type = (trim_info & EXYNOS5433_TRIMINFO_CALIB_SEL_MASK)
+				>> EXYNOS5433_TRIMINFO_CALIB_SEL_SHIFT;
+
+	switch (cal_type) {
+	case EXYNOS5433_TRIMINFO_ONE_POINT_TRIMMING:
+		pdata->cal_type = TYPE_ONE_POINT_TRIMMING;
+		break;
+	case EXYNOS5433_TRIMINFO_TWO_POINT_TRIMMING:
+		pdata->cal_type = TYPE_TWO_POINT_TRIMMING;
+		break;
+	default:
+		pdata->cal_type = TYPE_ONE_POINT_TRIMMING;
+		break;
+	};
+
+	dev_info(&pdev->dev, "Calibration type is %d-point calibration\n",
+			cal_type ?  2 : 1);
+
+	/* Write temperature code for rising and falling threshold */
+	for (i = 0; i < of_thermal_get_ntrips(tz); i++) {
+		int rising_reg_offset, falling_reg_offset;
+		int j = 0;
+
+		switch (i) {
+		case 0:
+		case 1:
+		case 2:
+		case 3:
+			rising_reg_offset = EXYNOS5433_THD_TEMP_RISE3_0;
+			falling_reg_offset = EXYNOS5433_THD_TEMP_FALL3_0;
+			j = i;
+			break;
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			rising_reg_offset = EXYNOS5433_THD_TEMP_RISE7_4;
+			falling_reg_offset = EXYNOS5433_THD_TEMP_FALL7_4;
+			j = i - 4;
+			break;
+		default:
+			continue;
+		}
+
+		/* Write temperature code for rising threshold */
+		tz->ops->get_trip_temp(tz, i, &temp);
+		temp /= MCELSIUS;
+		threshold_code = temp_to_code(data, temp);
+
+		rising_threshold = readl(data->base + rising_reg_offset);
+		rising_threshold |= (threshold_code << j * 8);
+		writel(rising_threshold, data->base + rising_reg_offset);
+
+		/* Write temperature code for falling threshold */
+		tz->ops->get_trip_hyst(tz, i, &temp_hist);
+		temp_hist = temp - (temp_hist / MCELSIUS);
+		threshold_code = temp_to_code(data, temp_hist);
+
+		falling_threshold = readl(data->base + falling_reg_offset);
+		falling_threshold &= ~(0xff << j * 8);
+		falling_threshold |= (threshold_code << j * 8);
+		writel(falling_threshold, data->base + falling_reg_offset);
+	}
+
+	data->tmu_clear_irqs(data);
+out:
+	return ret;
+}
+
 static int exynos5440_tmu_initialize(struct platform_device *pdev)
 {
 	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
@@ -643,6 +764,48 @@ static void exynos4210_tmu_control(struct platform_device *pdev, bool on)
 	writel(con, data->base + EXYNOS_TMU_REG_CONTROL);
 }
 
+static void exynos5433_tmu_control(struct platform_device *pdev, bool on)
+{
+	struct exynos_tmu_data *data = platform_get_drvdata(pdev);
+	struct thermal_zone_device *tz = data->tzd;
+	unsigned int con, interrupt_en, pd_det_en;
+
+	con = get_con_reg(data, readl(data->base + EXYNOS_TMU_REG_CONTROL));
+
+	if (on) {
+		con |= (1 << EXYNOS_TMU_CORE_EN_SHIFT);
+		interrupt_en =
+			(of_thermal_is_trip_valid(tz, 7)
+			<< EXYNOS7_TMU_INTEN_RISE7_SHIFT) |
+			(of_thermal_is_trip_valid(tz, 6)
+			<< EXYNOS7_TMU_INTEN_RISE6_SHIFT) |
+			(of_thermal_is_trip_valid(tz, 5)
+			<< EXYNOS7_TMU_INTEN_RISE5_SHIFT) |
+			(of_thermal_is_trip_valid(tz, 4)
+			<< EXYNOS7_TMU_INTEN_RISE4_SHIFT) |
+			(of_thermal_is_trip_valid(tz, 3)
+			<< EXYNOS7_TMU_INTEN_RISE3_SHIFT) |
+			(of_thermal_is_trip_valid(tz, 2)
+			<< EXYNOS7_TMU_INTEN_RISE2_SHIFT) |
+			(of_thermal_is_trip_valid(tz, 1)
+			<< EXYNOS7_TMU_INTEN_RISE1_SHIFT) |
+			(of_thermal_is_trip_valid(tz, 0)
+			<< EXYNOS7_TMU_INTEN_RISE0_SHIFT);
+
+		interrupt_en |=
+			interrupt_en << EXYNOS_TMU_INTEN_FALL0_SHIFT;
+	} else {
+		con &= ~(1 << EXYNOS_TMU_CORE_EN_SHIFT);
+		interrupt_en = 0; /* Disable all interrupts */
+	}
+
+	pd_det_en = on ? EXYNOS5433_PD_DET_EN : 0;
+
+	writel(pd_det_en, data->base + EXYNOS5433_TMU_PD_DET_EN);
+	writel(interrupt_en, data->base + EXYNOS5433_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);
@@ -770,6 +933,8 @@ static void exynos4412_tmu_set_emulation(struct exynos_tmu_data *data,
 
 	if (data->soc == SOC_ARCH_EXYNOS5260)
 		emul_con = EXYNOS5260_EMUL_CON;
+	if (data->soc == SOC_ARCH_EXYNOS5433)
+		emul_con = EXYNOS5433_TMU_EMUL_CON;
 	else if (data->soc == SOC_ARCH_EXYNOS7)
 		emul_con = EXYNOS7_TMU_REG_EMUL_CON;
 	else
@@ -882,6 +1047,9 @@ static void exynos4210_tmu_clear_irqs(struct exynos_tmu_data *data)
 	} else if (data->soc == SOC_ARCH_EXYNOS7) {
 		tmu_intstat = EXYNOS7_TMU_REG_INTPEND;
 		tmu_intclear = EXYNOS7_TMU_REG_INTPEND;
+	} else if (data->soc == SOC_ARCH_EXYNOS5433) {
+		tmu_intstat = EXYNOS5433_TMU_REG_INTPEND;
+		tmu_intclear = EXYNOS5433_TMU_REG_INTPEND;
 	} else {
 		tmu_intstat = EXYNOS_TMU_REG_INTSTAT;
 		tmu_intclear = EXYNOS_TMU_REG_INTCLEAR;
@@ -926,6 +1094,7 @@ static const struct of_device_id exynos_tmu_match[] = {
 	{ .compatible = "samsung,exynos5260-tmu", },
 	{ .compatible = "samsung,exynos5420-tmu", },
 	{ .compatible = "samsung,exynos5420-tmu-ext-triminfo", },
+	{ .compatible = "samsung,exynos5433-tmu", },
 	{ .compatible = "samsung,exynos5440-tmu", },
 	{ .compatible = "samsung,exynos7-tmu", },
 	{ /* sentinel */ },
@@ -949,6 +1118,8 @@ static int exynos_of_get_soc_type(struct device_node *np)
 	else if (of_device_is_compatible(np,
 					 "samsung,exynos5420-tmu-ext-triminfo"))
 		return SOC_ARCH_EXYNOS5420_TRIMINFO;
+	else if (of_device_is_compatible(np, "samsung,exynos5433-tmu"))
+		return SOC_ARCH_EXYNOS5433;
 	else if (of_device_is_compatible(np, "samsung,exynos5440-tmu"))
 		return SOC_ARCH_EXYNOS5440;
 	else if (of_device_is_compatible(np, "samsung,exynos7-tmu"))
@@ -1069,6 +1240,13 @@ static int exynos_map_dt_data(struct platform_device *pdev)
 		data->tmu_set_emulation = exynos4412_tmu_set_emulation;
 		data->tmu_clear_irqs = exynos4210_tmu_clear_irqs;
 		break;
+	case SOC_ARCH_EXYNOS5433:
+		data->tmu_initialize = exynos5433_tmu_initialize;
+		data->tmu_control = exynos5433_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;
@@ -1172,7 +1350,9 @@ static int exynos_tmu_probe(struct platform_device *pdev)
 		goto err_clk_sec;
 	}
 
-	if (data->soc == SOC_ARCH_EXYNOS7) {
+	switch (data->soc) {
+	case SOC_ARCH_EXYNOS5433:
+	case SOC_ARCH_EXYNOS7:
 		data->sclk = devm_clk_get(&pdev->dev, "tmu_sclk");
 		if (IS_ERR(data->sclk)) {
 			dev_err(&pdev->dev, "Failed to get sclk\n");
@@ -1184,7 +1364,10 @@ static int exynos_tmu_probe(struct platform_device *pdev)
 				goto err_clk;
 			}
 		}
-	}
+		break;
+	default:
+		break;
+	};
 
 	ret = exynos_tmu_initialize(pdev);
 	if (ret) {
diff --git a/drivers/thermal/samsung/exynos_tmu.h b/drivers/thermal/samsung/exynos_tmu.h
index 4d71ec6c9aa0..440c7140b660 100644
--- a/drivers/thermal/samsung/exynos_tmu.h
+++ b/drivers/thermal/samsung/exynos_tmu.h
@@ -33,6 +33,7 @@ enum soc_type {
 	SOC_ARCH_EXYNOS5260,
 	SOC_ARCH_EXYNOS5420,
 	SOC_ARCH_EXYNOS5420_TRIMINFO,
+	SOC_ARCH_EXYNOS5433,
 	SOC_ARCH_EXYNOS5440,
 	SOC_ARCH_EXYNOS7,
 };
diff --git a/drivers/thermal/thermal_core.c b/drivers/thermal/thermal_core.c
index 4108db7e10c1..04659bfb888b 100644
--- a/drivers/thermal/thermal_core.c
+++ b/drivers/thermal/thermal_core.c
@@ -75,6 +75,58 @@ static struct thermal_governor *__find_governor(const char *name)
 	return NULL;
 }
 
+/**
+ * bind_previous_governor() - bind the previous governor of the thermal zone
+ * @tz:		a valid pointer to a struct thermal_zone_device
+ * @failed_gov_name:	the name of the governor that failed to register
+ *
+ * Register the previous governor of the thermal zone after a new
+ * governor has failed to be bound.
+ */
+static void bind_previous_governor(struct thermal_zone_device *tz,
+				   const char *failed_gov_name)
+{
+	if (tz->governor && tz->governor->bind_to_tz) {
+		if (tz->governor->bind_to_tz(tz)) {
+			dev_err(&tz->device,
+				"governor %s failed to bind and the previous one (%s) failed to bind again, thermal zone %s has no governor\n",
+				failed_gov_name, tz->governor->name, tz->type);
+			tz->governor = NULL;
+		}
+	}
+}
+
+/**
+ * thermal_set_governor() - Switch to another governor
+ * @tz:		a valid pointer to a struct thermal_zone_device
+ * @new_gov:	pointer to the new governor
+ *
+ * Change the governor of thermal zone @tz.
+ *
+ * Return: 0 on success, an error if the new governor's bind_to_tz() failed.
+ */
+static int thermal_set_governor(struct thermal_zone_device *tz,
+				struct thermal_governor *new_gov)
+{
+	int ret = 0;
+
+	if (tz->governor && tz->governor->unbind_from_tz)
+		tz->governor->unbind_from_tz(tz);
+
+	if (new_gov && new_gov->bind_to_tz) {
+		ret = new_gov->bind_to_tz(tz);
+		if (ret) {
+			bind_previous_governor(tz, new_gov->name);
+
+			return ret;
+		}
+	}
+
+	tz->governor = new_gov;
+
+	return ret;
+}
+
 int thermal_register_governor(struct thermal_governor *governor)
 {
 	int err;
@@ -107,8 +159,15 @@ int thermal_register_governor(struct thermal_governor *governor)
 
 		name = pos->tzp->governor_name;
 
-		if (!strncasecmp(name, governor->name, THERMAL_NAME_LENGTH))
-			pos->governor = governor;
+		if (!strncasecmp(name, governor->name, THERMAL_NAME_LENGTH)) {
+			int ret;
+
+			ret = thermal_set_governor(pos, governor);
+			if (ret)
+				dev_err(&pos->device,
+					"Failed to set governor %s for thermal zone %s: %d\n",
+					governor->name, pos->type, ret);
+		}
 	}
 
 	mutex_unlock(&thermal_list_lock);
@@ -134,7 +193,7 @@ void thermal_unregister_governor(struct thermal_governor *governor)
 	list_for_each_entry(pos, &thermal_tz_list, node) {
 		if (!strncasecmp(pos->governor->name, governor->name,
 						THERMAL_NAME_LENGTH))
-			pos->governor = NULL;
+			thermal_set_governor(pos, NULL);
 	}
 
 	mutex_unlock(&thermal_list_lock);
@@ -218,7 +277,8 @@ static void print_bind_err_msg(struct thermal_zone_device *tz,
 
 static void __bind(struct thermal_zone_device *tz, int mask,
 			struct thermal_cooling_device *cdev,
-			unsigned long *limits)
+			unsigned long *limits,
+			unsigned int weight)
 {
 	int i, ret;
 
@@ -233,7 +293,8 @@ static void __bind(struct thermal_zone_device *tz, int mask,
 				upper = limits[i * 2 + 1];
 			}
 			ret = thermal_zone_bind_cooling_device(tz, i, cdev,
-							       upper, lower);
+							       upper, lower,
+							       weight);
 			if (ret)
 				print_bind_err_msg(tz, cdev, ret);
 		}
@@ -280,7 +341,8 @@ static void bind_cdev(struct thermal_cooling_device *cdev)
 				continue;
 			tzp->tbp[i].cdev = cdev;
 			__bind(pos, tzp->tbp[i].trip_mask, cdev,
-			       tzp->tbp[i].binding_limits);
+			       tzp->tbp[i].binding_limits,
+			       tzp->tbp[i].weight);
 		}
 	}
 
@@ -319,7 +381,8 @@ static void bind_tz(struct thermal_zone_device *tz)
 				continue;
 			tzp->tbp[i].cdev = pos;
 			__bind(tz, tzp->tbp[i].trip_mask, pos,
-			       tzp->tbp[i].binding_limits);
+			       tzp->tbp[i].binding_limits,
+			       tzp->tbp[i].weight);
 		}
 	}
 exit:
@@ -713,7 +776,8 @@ passive_store(struct device *dev, struct device_attribute *attr,
 				thermal_zone_bind_cooling_device(tz,
 						THERMAL_TRIPS_NONE, cdev,
 						THERMAL_NO_LIMIT,
-						THERMAL_NO_LIMIT);
+						THERMAL_NO_LIMIT,
+						THERMAL_WEIGHT_DEFAULT);
 		}
 		mutex_unlock(&thermal_list_lock);
 		if (!tz->passive_delay)
@@ -765,8 +829,9 @@ policy_store(struct device *dev, struct device_attribute *attr,
 	if (!gov)
 		goto exit;
 
-	tz->governor = gov;
-	ret = count;
+	ret = thermal_set_governor(tz, gov);
+	if (!ret)
+		ret = count;
 
 exit:
 	mutex_unlock(&tz->lock);
@@ -810,6 +875,158 @@ emul_temp_store(struct device *dev, struct device_attribute *attr,
 static DEVICE_ATTR(emul_temp, S_IWUSR, NULL, emul_temp_store);
 #endif/*CONFIG_THERMAL_EMULATION*/
 
+static ssize_t
+sustainable_power_show(struct device *dev, struct device_attribute *devattr,
+		       char *buf)
+{
+	struct thermal_zone_device *tz = to_thermal_zone(dev);
+
+	if (tz->tzp)
+		return sprintf(buf, "%u\n", tz->tzp->sustainable_power);
+	else
+		return -EIO;
+}
+
+static ssize_t
+sustainable_power_store(struct device *dev, struct device_attribute *devattr,
+			const char *buf, size_t count)
+{
+	struct thermal_zone_device *tz = to_thermal_zone(dev);
+	u32 sustainable_power;
+
+	if (!tz->tzp)
+		return -EIO;
+
+	if (kstrtou32(buf, 10, &sustainable_power))
+		return -EINVAL;
+
+	tz->tzp->sustainable_power = sustainable_power;
+
+	return count;
+}
+static DEVICE_ATTR(sustainable_power, S_IWUSR | S_IRUGO, sustainable_power_show,
+		sustainable_power_store);
+
+#define create_s32_tzp_attr(name)					\
+	static ssize_t							\
+	name##_show(struct device *dev, struct device_attribute *devattr, \
+		char *buf)						\
+	{								\
+	struct thermal_zone_device *tz = to_thermal_zone(dev);		\
+									\
+	if (tz->tzp)							\
+		return sprintf(buf, "%u\n", tz->tzp->name);		\
+	else								\
+		return -EIO;						\
+	}								\
+									\
+	static ssize_t							\
+	name##_store(struct device *dev, struct device_attribute *devattr, \
+		const char *buf, size_t count)				\
+	{								\
+		struct thermal_zone_device *tz = to_thermal_zone(dev);	\
+		s32 value;						\
+									\
+		if (!tz->tzp)						\
+			return -EIO;					\
+									\
+		if (kstrtos32(buf, 10, &value))				\
+			return -EINVAL;					\
+									\
+		tz->tzp->name = value;					\
+									\
+		return count;						\
+	}								\
+	static DEVICE_ATTR(name, S_IWUSR | S_IRUGO, name##_show, name##_store)
+
+create_s32_tzp_attr(k_po);
+create_s32_tzp_attr(k_pu);
+create_s32_tzp_attr(k_i);
+create_s32_tzp_attr(k_d);
+create_s32_tzp_attr(integral_cutoff);
+create_s32_tzp_attr(slope);
+create_s32_tzp_attr(offset);
+#undef create_s32_tzp_attr
+
+static struct device_attribute *dev_tzp_attrs[] = {
+	&dev_attr_sustainable_power,
+	&dev_attr_k_po,
+	&dev_attr_k_pu,
+	&dev_attr_k_i,
+	&dev_attr_k_d,
+	&dev_attr_integral_cutoff,
+	&dev_attr_slope,
+	&dev_attr_offset,
+};
+
+static int create_tzp_attrs(struct device *dev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dev_tzp_attrs); i++) {
+		int ret;
+		struct device_attribute *dev_attr = dev_tzp_attrs[i];
+
+		ret = device_create_file(dev, dev_attr);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * power_actor_get_max_power() - get the maximum power that a cdev can consume
+ * @cdev:	pointer to &thermal_cooling_device
+ * @tz:		a valid thermal zone device pointer
+ * @max_power:	pointer in which to store the maximum power
+ *
+ * Calculate the maximum power consumption in milliwats that the
+ * cooling device can currently consume and store it in @max_power.
+ *
+ * Return: 0 on success, -EINVAL if @cdev doesn't support the
+ * power_actor API or -E* on other error.
+ */
+int power_actor_get_max_power(struct thermal_cooling_device *cdev,
+			      struct thermal_zone_device *tz, u32 *max_power)
+{
+	if (!cdev_is_power_actor(cdev))
+		return -EINVAL;
+
+	return cdev->ops->state2power(cdev, tz, 0, max_power);
+}
+
+/**
+ * power_actor_set_power() - limit the maximum power that a cooling device can consume
+ * @cdev:	pointer to &thermal_cooling_device
+ * @instance:	thermal instance to update
+ * @power:	the power in milliwatts
+ *
+ * Set the cooling device to consume at most @power milliwatts.
+ *
+ * Return: 0 on success, -EINVAL if the cooling device does not
+ * implement the power actor API or -E* for other failures.
+ */
+int power_actor_set_power(struct thermal_cooling_device *cdev,
+			  struct thermal_instance *instance, u32 power)
+{
+	unsigned long state;
+	int ret;
+
+	if (!cdev_is_power_actor(cdev))
+		return -EINVAL;
+
+	ret = cdev->ops->power2state(cdev, instance->tz, power, &state);
+	if (ret)
+		return ret;
+
+	instance->target = state;
+	cdev->updated = false;
+	thermal_cdev_update(cdev);
+
+	return 0;
+}
+
 static DEVICE_ATTR(type, 0444, type_show, NULL);
 static DEVICE_ATTR(temp, 0444, temp_show, NULL);
 static DEVICE_ATTR(mode, 0644, mode_show, mode_store);
@@ -917,6 +1134,34 @@ static const struct attribute_group *cooling_device_attr_groups[] = {
 	NULL,
 };
 
+static ssize_t
+thermal_cooling_device_weight_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct thermal_instance *instance;
+
+	instance = container_of(attr, struct thermal_instance, weight_attr);
+
+	return sprintf(buf, "%d\n", instance->weight);
+}
+
+static ssize_t
+thermal_cooling_device_weight_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct thermal_instance *instance;
+	int ret, weight;
+
+	ret = kstrtoint(buf, 0, &weight);
+	if (ret)
+		return ret;
+
+	instance = container_of(attr, struct thermal_instance, weight_attr);
+	instance->weight = weight;
+
+	return count;
+}
 /* Device management */
 
 /**
@@ -931,6 +1176,9 @@ static const struct attribute_group *cooling_device_attr_groups[] = {
  * @lower:	the Minimum cooling state can be used for this trip point.
  *		THERMAL_NO_LIMIT means no lower limit,
  *		and the cooling device can be in cooling state 0.
+ * @weight:	The weight of the cooling device to be bound to the
+ *		thermal zone. Use THERMAL_WEIGHT_DEFAULT for the
+ *		default value
  *
  * This interface function bind a thermal cooling device to the certain trip
  * point of a thermal zone device.
@@ -941,7 +1189,8 @@ static const struct attribute_group *cooling_device_attr_groups[] = {
 int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 				     int trip,
 				     struct thermal_cooling_device *cdev,
-				     unsigned long upper, unsigned long lower)
+				     unsigned long upper, unsigned long lower,
+				     unsigned int weight)
 {
 	struct thermal_instance *dev;
 	struct thermal_instance *pos;
@@ -986,6 +1235,7 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 	dev->upper = upper;
 	dev->lower = lower;
 	dev->target = THERMAL_NO_TARGET;
+	dev->weight = weight;
 
 	result = get_idr(&tz->idr, &tz->lock, &dev->id);
 	if (result)
@@ -1006,6 +1256,16 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 	if (result)
 		goto remove_symbol_link;
 
+	sprintf(dev->weight_attr_name, "cdev%d_weight", dev->id);
+	sysfs_attr_init(&dev->weight_attr.attr);
+	dev->weight_attr.attr.name = dev->weight_attr_name;
+	dev->weight_attr.attr.mode = S_IWUSR | S_IRUGO;
+	dev->weight_attr.show = thermal_cooling_device_weight_show;
+	dev->weight_attr.store = thermal_cooling_device_weight_store;
+	result = device_create_file(&tz->device, &dev->weight_attr);
+	if (result)
+		goto remove_trip_file;
+
 	mutex_lock(&tz->lock);
 	mutex_lock(&cdev->lock);
 	list_for_each_entry(pos, &tz->thermal_instances, tz_node)
@@ -1023,6 +1283,8 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 	if (!result)
 		return 0;
 
+	device_remove_file(&tz->device, &dev->weight_attr);
+remove_trip_file:
 	device_remove_file(&tz->device, &dev->attr);
 remove_symbol_link:
 	sysfs_remove_link(&tz->device.kobj, dev->name);
@@ -1377,7 +1639,8 @@ static int create_trip_attrs(struct thermal_zone_device *tz, int mask)
 						tz->trip_temp_attrs[indx].name;
 		tz->trip_temp_attrs[indx].attr.attr.mode = S_IRUGO;
 		tz->trip_temp_attrs[indx].attr.show = trip_point_temp_show;
-		if (mask & (1 << indx)) {
+		if (IS_ENABLED(CONFIG_THERMAL_WRITABLE_TRIPS) &&
+		    mask & (1 << indx)) {
 			tz->trip_temp_attrs[indx].attr.attr.mode |= S_IWUSR;
 			tz->trip_temp_attrs[indx].attr.store =
 							trip_point_temp_store;
@@ -1454,7 +1717,7 @@ static void remove_trip_attrs(struct thermal_zone_device *tz)
 struct thermal_zone_device *thermal_zone_device_register(const char *type,
 	int trips, int mask, void *devdata,
 	struct thermal_zone_device_ops *ops,
-	const struct thermal_zone_params *tzp,
+	struct thermal_zone_params *tzp,
 	int passive_delay, int polling_delay)
 {
 	struct thermal_zone_device *tz;
@@ -1462,6 +1725,7 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
 	int result;
 	int count;
 	int passive = 0;
+	struct thermal_governor *governor;
 
 	if (type && strlen(type) >= THERMAL_NAME_LENGTH)
 		return ERR_PTR(-EINVAL);
@@ -1548,13 +1812,24 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
 	if (result)
 		goto unregister;
 
+	/* Add thermal zone params */
+	result = create_tzp_attrs(&tz->device);
+	if (result)
+		goto unregister;
+
 	/* Update 'this' zone's governor information */
 	mutex_lock(&thermal_governor_lock);
 
 	if (tz->tzp)
-		tz->governor = __find_governor(tz->tzp->governor_name);
+		governor = __find_governor(tz->tzp->governor_name);
 	else
-		tz->governor = def_governor;
+		governor = def_governor;
+
+	result = thermal_set_governor(tz, governor);
+	if (result) {
+		mutex_unlock(&thermal_governor_lock);
+		goto unregister;
+	}
 
 	mutex_unlock(&thermal_governor_lock);
 
@@ -1643,7 +1918,7 @@ void thermal_zone_device_unregister(struct thermal_zone_device *tz)
 		device_remove_file(&tz->device, &dev_attr_mode);
 	device_remove_file(&tz->device, &dev_attr_policy);
 	remove_trip_attrs(tz);
-	tz->governor = NULL;
+	thermal_set_governor(tz, NULL);
 
 	thermal_remove_hwmon_sysfs(tz);
 	release_idr(&thermal_tz_idr, &thermal_idr_lock, tz->id);
@@ -1799,7 +2074,11 @@ static int __init thermal_register_governors(void)
 	if (result)
 		return result;
 
-	return thermal_gov_user_space_register();
+	result = thermal_gov_user_space_register();
+	if (result)
+		return result;
+
+	return thermal_gov_power_allocator_register();
 }
 
 static void thermal_unregister_governors(void)
@@ -1808,6 +2087,7 @@ static void thermal_unregister_governors(void)
 	thermal_gov_fair_share_unregister();
 	thermal_gov_bang_bang_unregister();
 	thermal_gov_user_space_unregister();
+	thermal_gov_power_allocator_unregister();
 }
 
 static int __init thermal_init(void)
diff --git a/drivers/thermal/thermal_core.h b/drivers/thermal/thermal_core.h
index 8e391812e503..d7ac1fccd659 100644
--- a/drivers/thermal/thermal_core.h
+++ b/drivers/thermal/thermal_core.h
@@ -46,8 +46,11 @@ struct thermal_instance {
 	unsigned long target;	/* expected cooling state */
 	char attr_name[THERMAL_NAME_LENGTH];
 	struct device_attribute attr;
+	char weight_attr_name[THERMAL_NAME_LENGTH];
+	struct device_attribute weight_attr;
 	struct list_head tz_node; /* node in tz->thermal_instances */
 	struct list_head cdev_node; /* node in cdev->thermal_instances */
+	unsigned int weight; /* The weight of the cooling device */
 };
 
 int thermal_register_governor(struct thermal_governor *);
@@ -85,6 +88,14 @@ static inline int thermal_gov_user_space_register(void) { return 0; }
 static inline void thermal_gov_user_space_unregister(void) {}
 #endif /* CONFIG_THERMAL_GOV_USER_SPACE */
 
+#ifdef CONFIG_THERMAL_GOV_POWER_ALLOCATOR
+int thermal_gov_power_allocator_register(void);
+void thermal_gov_power_allocator_unregister(void);
+#else
+static inline int thermal_gov_power_allocator_register(void) { return 0; }
+static inline void thermal_gov_power_allocator_unregister(void) {}
+#endif /* CONFIG_THERMAL_GOV_POWER_ALLOCATOR */
+
 /* device tree support */
 #ifdef CONFIG_THERMAL_OF
 int of_parse_thermal_zones(void);
diff --git a/drivers/thermal/ti-soc-thermal/ti-bandgap.c b/drivers/thermal/ti-soc-thermal/ti-bandgap.c
index bc14dc874594..10c47c048f7a 100644
--- a/drivers/thermal/ti-soc-thermal/ti-bandgap.c
+++ b/drivers/thermal/ti-soc-thermal/ti-bandgap.c
@@ -43,6 +43,8 @@
 
 #include "ti-bandgap.h"
 
+static int ti_bandgap_force_single_read(struct ti_bandgap *bgp, int id);
+
 /***   Helper functions to access registers and their bitfields   ***/
 
 /**
@@ -103,19 +105,15 @@ do {								\
  */
 static int ti_bandgap_power(struct ti_bandgap *bgp, bool on)
 {
-	int i, ret = 0;
+	int i;
 
-	if (!TI_BANDGAP_HAS(bgp, POWER_SWITCH)) {
-		ret = -ENOTSUPP;
-		goto exit;
-	}
+	if (!TI_BANDGAP_HAS(bgp, POWER_SWITCH))
+		return -ENOTSUPP;
 
 	for (i = 0; i < bgp->conf->sensor_count; i++)
 		/* active on 0 */
 		RMW_BITS(bgp, i, temp_sensor_ctrl, bgap_tempsoff_mask, !on);
-
-exit:
-	return ret;
+	return 0;
 }
 
 /**
@@ -298,18 +296,13 @@ static
 int ti_bandgap_adc_to_mcelsius(struct ti_bandgap *bgp, int adc_val, int *t)
 {
 	const struct ti_bandgap_data *conf = bgp->conf;
-	int ret = 0;
 
 	/* look up for temperature in the table and return the temperature */
-	if (adc_val < conf->adc_start_val || adc_val > conf->adc_end_val) {
-		ret = -ERANGE;
-		goto exit;
-	}
+	if (adc_val < conf->adc_start_val || adc_val > conf->adc_end_val)
+		return -ERANGE;
 
 	*t = bgp->conf->conv_table[adc_val - conf->adc_start_val];
-
-exit:
-	return ret;
+	return 0;
 }
 
 /**
@@ -330,16 +323,14 @@ int ti_bandgap_mcelsius_to_adc(struct ti_bandgap *bgp, long temp, int *adc)
 {
 	const struct ti_bandgap_data *conf = bgp->conf;
 	const int *conv_table = bgp->conf->conv_table;
-	int high, low, mid, ret = 0;
+	int high, low, mid;
 
 	low = 0;
 	high = conf->adc_end_val - conf->adc_start_val;
 	mid = (high + low) / 2;
 
-	if (temp < conv_table[low] || temp > conv_table[high]) {
-		ret = -ERANGE;
-		goto exit;
-	}
+	if (temp < conv_table[low] || temp > conv_table[high])
+		return -ERANGE;
 
 	while (low < high) {
 		if (temp < conv_table[mid])
@@ -350,9 +341,7 @@ int ti_bandgap_mcelsius_to_adc(struct ti_bandgap *bgp, long temp, int *adc)
 	}
 
 	*adc = conf->adc_start_val + low;
-
-exit:
-	return ret;
+	return 0;
 }
 
 /**
@@ -378,13 +367,11 @@ int ti_bandgap_add_hyst(struct ti_bandgap *bgp, int adc_val, int hyst_val,
 	 */
 	ret = ti_bandgap_adc_to_mcelsius(bgp, adc_val, &temp);
 	if (ret < 0)
-		goto exit;
+		return ret;
 
 	temp += hyst_val;
 
 	ret = ti_bandgap_mcelsius_to_adc(bgp, temp, sum);
-
-exit:
 	return ret;
 }
 
@@ -542,22 +529,18 @@ exit:
  */
 static inline int ti_bandgap_validate(struct ti_bandgap *bgp, int id)
 {
-	int ret = 0;
-
 	if (!bgp || IS_ERR(bgp)) {
 		pr_err("%s: invalid bandgap pointer\n", __func__);
-		ret = -EINVAL;
-		goto exit;
+		return -EINVAL;
 	}
 
 	if ((id < 0) || (id >= bgp->conf->sensor_count)) {
 		dev_err(bgp->dev, "%s: sensor id out of range (%d)\n",
 			__func__, id);
-		ret = -ERANGE;
+		return -ERANGE;
 	}
 
-exit:
-	return ret;
+	return 0;
 }
 
 /**
@@ -585,12 +568,10 @@ static int _ti_bandgap_write_threshold(struct ti_bandgap *bgp, int id, int val,
 
 	ret = ti_bandgap_validate(bgp, id);
 	if (ret)
-		goto exit;
+		return ret;
 
-	if (!TI_BANDGAP_HAS(bgp, TALERT)) {
-		ret = -ENOTSUPP;
-		goto exit;
-	}
+	if (!TI_BANDGAP_HAS(bgp, TALERT))
+		return -ENOTSUPP;
 
 	ts_data = bgp->conf->sensors[id].ts_data;
 	tsr = bgp->conf->sensors[id].registers;
@@ -603,17 +584,15 @@ static int _ti_bandgap_write_threshold(struct ti_bandgap *bgp, int id, int val,
 	}
 
 	if (ret)
-		goto exit;
+		return ret;
 
 	ret = ti_bandgap_mcelsius_to_adc(bgp, val, &adc_val);
 	if (ret < 0)
-		goto exit;
+		return ret;
 
 	spin_lock(&bgp->lock);
 	ret = ti_bandgap_update_alert_threshold(bgp, id, adc_val, hot);
 	spin_unlock(&bgp->lock);
-
-exit:
 	return ret;
 }
 
@@ -656,7 +635,7 @@ static int _ti_bandgap_read_threshold(struct ti_bandgap *bgp, int id,
 
 	temp = ti_bandgap_readl(bgp, tsr->bgap_threshold);
 	temp = (temp & mask) >> __ffs(mask);
-	ret |= ti_bandgap_adc_to_mcelsius(bgp, temp, &temp);
+	ret = ti_bandgap_adc_to_mcelsius(bgp, temp, &temp);
 	if (ret) {
 		dev_err(bgp->dev, "failed to read thot\n");
 		ret = -EIO;
@@ -926,11 +905,17 @@ int ti_bandgap_read_temperature(struct ti_bandgap *bgp, int id,
 	if (ret)
 		return ret;
 
+	if (!TI_BANDGAP_HAS(bgp, MODE_CONFIG)) {
+		ret = ti_bandgap_force_single_read(bgp, id);
+		if (ret)
+			return ret;
+	}
+
 	spin_lock(&bgp->lock);
 	temp = ti_bandgap_read_temp(bgp, id);
 	spin_unlock(&bgp->lock);
 
-	ret |= ti_bandgap_adc_to_mcelsius(bgp, temp, &temp);
+	ret = ti_bandgap_adc_to_mcelsius(bgp, temp, &temp);
 	if (ret)
 		return -EIO;
 
@@ -991,7 +976,8 @@ void *ti_bandgap_get_sensor_data(struct ti_bandgap *bgp, int id)
 static int
 ti_bandgap_force_single_read(struct ti_bandgap *bgp, int id)
 {
-	u32 temp = 0, counter = 1000;
+	u32 counter = 1000;
+	struct temp_sensor_registers *tsr;
 
 	/* Select single conversion mode */
 	if (TI_BANDGAP_HAS(bgp, MODE_CONFIG))
@@ -999,16 +985,27 @@ ti_bandgap_force_single_read(struct ti_bandgap *bgp, int id)
 
 	/* Start of Conversion = 1 */
 	RMW_BITS(bgp, id, temp_sensor_ctrl, bgap_soc_mask, 1);
-	/* Wait until DTEMP is updated */
-	temp = ti_bandgap_read_temp(bgp, id);
 
-	while ((temp == 0) && --counter)
-		temp = ti_bandgap_read_temp(bgp, id);
-	/* REVISIT: Check correct condition for end of conversion */
+	/* Wait for EOCZ going up */
+	tsr = bgp->conf->sensors[id].registers;
+
+	while (--counter) {
+		if (ti_bandgap_readl(bgp, tsr->temp_sensor_ctrl) &
+		    tsr->bgap_eocz_mask)
+			break;
+	}
 
 	/* Start of Conversion = 0 */
 	RMW_BITS(bgp, id, temp_sensor_ctrl, bgap_soc_mask, 0);
 
+	/* Wait for EOCZ going down */
+	counter = 1000;
+	while (--counter) {
+		if (!(ti_bandgap_readl(bgp, tsr->temp_sensor_ctrl) &
+		      tsr->bgap_eocz_mask))
+			break;
+	}
+
 	return 0;
 }
 
@@ -1294,11 +1291,10 @@ int ti_bandgap_probe(struct platform_device *pdev)
 		goto free_irqs;
 	}
 
-	bgp->div_clk = clk_get(NULL,  bgp->conf->div_ck_name);
+	bgp->div_clk = clk_get(NULL, bgp->conf->div_ck_name);
 	ret = IS_ERR(bgp->div_clk);
 	if (ret) {
-		dev_err(&pdev->dev,
-			"failed to request div_ts_ck clock ref\n");
+		dev_err(&pdev->dev, "failed to request div_ts_ck clock ref\n");
 		ret = PTR_ERR(bgp->div_clk);
 		goto free_irqs;
 	}
diff --git a/drivers/thermal/ti-soc-thermal/ti-thermal-common.c b/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
index a38c1756442a..c7c5b3779dac 100644
--- a/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
+++ b/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
@@ -75,7 +75,7 @@ static inline int ti_thermal_hotspot_temperature(int t, int s, int c)
 }
 
 /* thermal zone ops */
-/* Get temperature callback function for thermal zone*/
+/* Get temperature callback function for thermal zone */
 static inline int __ti_thermal_get_temp(void *devdata, long *temp)
 {
 	struct thermal_zone_device *pcb_tz = NULL;
@@ -146,7 +146,8 @@ static int ti_thermal_bind(struct thermal_zone_device *thermal,
 	return thermal_zone_bind_cooling_device(thermal, 0, cdev,
 	/* bind with min and max states defined by cpu_cooling */
 						THERMAL_NO_LIMIT,
-						THERMAL_NO_LIMIT);
+						THERMAL_NO_LIMIT,
+						THERMAL_WEIGHT_DEFAULT);
 }
 
 /* Unbind callback functions for thermal zone */
diff --git a/drivers/thermal/x86_pkg_temp_thermal.c b/drivers/thermal/x86_pkg_temp_thermal.c
index 9ea3d9d49ffc..50d1d2cb091a 100644
--- a/drivers/thermal/x86_pkg_temp_thermal.c
+++ b/drivers/thermal/x86_pkg_temp_thermal.c
@@ -68,7 +68,7 @@ struct phy_dev_entry {
 	struct thermal_zone_device *tzone;
 };
 
-static const struct thermal_zone_params pkg_temp_tz_params = {
+static struct thermal_zone_params pkg_temp_tz_params = {
 	.no_hwmon	= true,
 };
 
diff --git a/drivers/tty/amiserial.c b/drivers/tty/amiserial.c
index b2d760055952..e53d9a512c6d 100644
--- a/drivers/tty/amiserial.c
+++ b/drivers/tty/amiserial.c
@@ -966,9 +966,7 @@ static void rs_throttle(struct tty_struct * tty)
 	struct serial_state *info = tty->driver_data;
 	unsigned long flags;
 #ifdef SERIAL_DEBUG_THROTTLE
-	char	buf[64];
-
-	printk("throttle %s: %d....\n", tty_name(tty, buf),
+	printk("throttle %s: %d....\n", tty_name(tty),
 	       tty->ldisc.chars_in_buffer(tty));
 #endif
 
@@ -991,9 +989,7 @@ static void rs_unthrottle(struct tty_struct * tty)
 	struct serial_state *info = tty->driver_data;
 	unsigned long flags;
 #ifdef SERIAL_DEBUG_THROTTLE
-	char	buf[64];
-
-	printk("unthrottle %s: %d....\n", tty_name(tty, buf),
+	printk("unthrottle %s: %d....\n", tty_name(tty),
 	       tty->ldisc.chars_in_buffer(tty));
 #endif
 
@@ -1786,7 +1782,8 @@ static int __exit amiga_serial_remove(struct platform_device *pdev)
 	struct serial_state *state = platform_get_drvdata(pdev);
 
 	/* printk("Unloading %s: version %s\n", serial_name, serial_version); */
-	if ((error = tty_unregister_driver(serial_driver)))
+	error = tty_unregister_driver(serial_driver);
+	if (error)
 		printk("SERIAL: failed to unregister serial driver (%d)\n",
 		       error);
 	put_tty_driver(serial_driver);
diff --git a/drivers/tty/cyclades.c b/drivers/tty/cyclades.c
index fd66f57390d0..87f6578c6f4a 100644
--- a/drivers/tty/cyclades.c
+++ b/drivers/tty/cyclades.c
@@ -2861,9 +2861,7 @@ static void cy_throttle(struct tty_struct *tty)
 	unsigned long flags;
 
 #ifdef CY_DEBUG_THROTTLE
-	char buf[64];
-
-	printk(KERN_DEBUG "cyc:throttle %s: %ld...ttyC%d\n", tty_name(tty, buf),
+	printk(KERN_DEBUG "cyc:throttle %s: %ld...ttyC%d\n", tty_name(tty),
 			tty->ldisc.chars_in_buffer(tty), info->line);
 #endif
 
@@ -2902,10 +2900,8 @@ static void cy_unthrottle(struct tty_struct *tty)
 	unsigned long flags;
 
 #ifdef CY_DEBUG_THROTTLE
-	char buf[64];
-
 	printk(KERN_DEBUG "cyc:unthrottle %s: %ld...ttyC%d\n",
-		tty_name(tty, buf), tty_chars_in_buffer(tty), info->line);
+		tty_name(tty), tty_chars_in_buffer(tty), info->line);
 #endif
 
 	if (serial_paranoia_check(info, tty->name, "cy_unthrottle"))
diff --git a/drivers/tty/goldfish.c b/drivers/tty/goldfish.c
index 0655fecf8240..0f82c0b146f6 100644
--- a/drivers/tty/goldfish.c
+++ b/drivers/tty/goldfish.c
@@ -59,7 +59,7 @@ static void goldfish_tty_do_write(int line, const char *buf, unsigned count)
 	struct goldfish_tty *qtty = &goldfish_ttys[line];
 	void __iomem *base = qtty->base;
 	spin_lock_irqsave(&qtty->lock, irq_flags);
-	gf_write64((u64)buf, base + GOLDFISH_TTY_DATA_PTR,
+	gf_write_ptr(buf, base + GOLDFISH_TTY_DATA_PTR,
 				base + GOLDFISH_TTY_DATA_PTR_HIGH);
 	writel(count, base + GOLDFISH_TTY_DATA_LEN);
 	writel(GOLDFISH_TTY_CMD_WRITE_BUFFER, base + GOLDFISH_TTY_CMD);
@@ -81,7 +81,7 @@ static irqreturn_t goldfish_tty_interrupt(int irq, void *dev_id)
 
 	count = tty_prepare_flip_string(&qtty->port, &buf, count);
 	spin_lock_irqsave(&qtty->lock, irq_flags);
-	gf_write64((u64)buf, base + GOLDFISH_TTY_DATA_PTR,
+	gf_write_ptr(buf, base + GOLDFISH_TTY_DATA_PTR,
 				base + GOLDFISH_TTY_DATA_PTR_HIGH);
 	writel(count, base + GOLDFISH_TTY_DATA_LEN);
 	writel(GOLDFISH_TTY_CMD_READ_BUFFER, base + GOLDFISH_TTY_CMD);
diff --git a/drivers/tty/hvc/Kconfig b/drivers/tty/hvc/Kconfig
index 8902f9b4df71..2509d057b99c 100644
--- a/drivers/tty/hvc/Kconfig
+++ b/drivers/tty/hvc/Kconfig
@@ -42,13 +42,6 @@ config HVC_RTAS
 	help
 	  IBM Console device driver which makes use of RTAS
 
-config HVC_BEAT
-	bool "Toshiba's Beat Hypervisor Console support"
-	depends on PPC_CELLEB
-	select HVC_DRIVER
-	help
-	  Toshiba's Cell Reference Set Beat Console device driver
-
 config HVC_IUCV
 	bool "z/VM IUCV Hypervisor console support (VM only)"
 	depends on S390
diff --git a/drivers/tty/hvc/Makefile b/drivers/tty/hvc/Makefile
index 4ca3723b0a3a..6a2702be76d1 100644
--- a/drivers/tty/hvc/Makefile
+++ b/drivers/tty/hvc/Makefile
@@ -4,7 +4,6 @@ obj-$(CONFIG_HVC_OLD_HVSI)	+= hvsi.o
 obj-$(CONFIG_HVC_RTAS)		+= hvc_rtas.o
 obj-$(CONFIG_HVC_TILE)		+= hvc_tile.o
 obj-$(CONFIG_HVC_DCC)		+= hvc_dcc.o
-obj-$(CONFIG_HVC_BEAT)		+= hvc_beat.o
 obj-$(CONFIG_HVC_DRIVER)	+= hvc_console.o
 obj-$(CONFIG_HVC_IRQ)		+= hvc_irq.o
 obj-$(CONFIG_HVC_XEN)		+= hvc_xen.o
diff --git a/drivers/tty/hvc/hvc_beat.c b/drivers/tty/hvc/hvc_beat.c
deleted file mode 100644
index 1560d235449e..000000000000
--- a/drivers/tty/hvc/hvc_beat.c
+++ /dev/null
@@ -1,134 +0,0 @@
-/*
- * Beat hypervisor console driver
- *
- * (C) Copyright 2006 TOSHIBA CORPORATION
- *
- * This code is based on drivers/char/hvc_rtas.c:
- * (C) Copyright IBM Corporation 2001-2005
- * (C) Copyright Red Hat, Inc. 2005
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/err.h>
-#include <linux/string.h>
-#include <linux/console.h>
-#include <asm/prom.h>
-#include <asm/hvconsole.h>
-#include <asm/firmware.h>
-
-#include "hvc_console.h"
-
-extern int64_t beat_get_term_char(uint64_t, uint64_t *, uint64_t *, uint64_t *);
-extern int64_t beat_put_term_char(uint64_t, uint64_t, uint64_t, uint64_t);
-
-struct hvc_struct *hvc_beat_dev = NULL;
-
-/* bug: only one queue is available regardless of vtermno */
-static int hvc_beat_get_chars(uint32_t vtermno, char *buf, int cnt)
-{
-	static unsigned char q[sizeof(unsigned long) * 2]
-		__attribute__((aligned(sizeof(unsigned long))));
-	static int qlen = 0;
-	u64 got;
-
-again:
-	if (qlen) {
-		if (qlen > cnt) {
-			memcpy(buf, q, cnt);
-			qlen -= cnt;
-			memmove(q + cnt, q, qlen);
-			return cnt;
-		} else {	/* qlen <= cnt */
-			int	r;
-
-			memcpy(buf, q, qlen);
-			r = qlen;
-			qlen = 0;
-			return r;
-		}
-	}
-	if (beat_get_term_char(vtermno, &got,
-		((u64 *)q), ((u64 *)q) + 1) == 0) {
-		qlen = got;
-		goto again;
-	}
-	return 0;
-}
-
-static int hvc_beat_put_chars(uint32_t vtermno, const char *buf, int cnt)
-{
-	unsigned long kb[2];
-	int rest, nlen;
-
-	for (rest = cnt; rest > 0; rest -= nlen) {
-		nlen = (rest > 16) ? 16 : rest;
-		memcpy(kb, buf, nlen);
-		beat_put_term_char(vtermno, nlen, kb[0], kb[1]);
-		buf += nlen;
-	}
-	return cnt;
-}
-
-static const struct hv_ops hvc_beat_get_put_ops = {
-	.get_chars = hvc_beat_get_chars,
-	.put_chars = hvc_beat_put_chars,
-};
-
-static int hvc_beat_useit = 1;
-
-static int hvc_beat_config(char *p)
-{
-	hvc_beat_useit = simple_strtoul(p, NULL, 0);
-	return 0;
-}
-
-static int __init hvc_beat_console_init(void)
-{
-	if (hvc_beat_useit && of_machine_is_compatible("Beat")) {
-		hvc_instantiate(0, 0, &hvc_beat_get_put_ops);
-	}
-	return 0;
-}
-
-/* temp */
-static int __init hvc_beat_init(void)
-{
-	struct hvc_struct *hp;
-
-	if (!firmware_has_feature(FW_FEATURE_BEAT))
-		return -ENODEV;
-
-	hp = hvc_alloc(0, 0, &hvc_beat_get_put_ops, 16);
-	if (IS_ERR(hp))
-		return PTR_ERR(hp);
-	hvc_beat_dev = hp;
-	return 0;
-}
-
-static void __exit hvc_beat_exit(void)
-{
-	if (hvc_beat_dev)
-		hvc_remove(hvc_beat_dev);
-}
-
-module_init(hvc_beat_init);
-module_exit(hvc_beat_exit);
-
-__setup("hvc_beat=", hvc_beat_config);
-
-console_initcall(hvc_beat_console_init);
diff --git a/drivers/tty/hvc/hvc_console.c b/drivers/tty/hvc/hvc_console.c
index 4fcec1d793a7..4e9c4cc9e1b5 100644
--- a/drivers/tty/hvc/hvc_console.c
+++ b/drivers/tty/hvc/hvc_console.c
@@ -319,7 +319,8 @@ static int hvc_install(struct tty_driver *driver, struct tty_struct *tty)
 	int rc;
 
 	/* Auto increments kref reference if found. */
-	if (!(hp = hvc_get_by_index(tty->index)))
+	hp = hvc_get_by_index(tty->index);
+	if (!hp)
 		return -ENODEV;
 
 	tty->driver_data = hp;
diff --git a/drivers/tty/hvc/hvc_iucv.c b/drivers/tty/hvc/hvc_iucv.c
index f78a87b07872..bb809cf36617 100644
--- a/drivers/tty/hvc/hvc_iucv.c
+++ b/drivers/tty/hvc/hvc_iucv.c
@@ -1345,7 +1345,7 @@ static int param_get_vmidfilter(char *buffer, const struct kernel_param *kp)
 
 #define param_check_vmidfilter(name, p) __param_check(name, p, void)
 
-static struct kernel_param_ops param_ops_vmidfilter = {
+static const struct kernel_param_ops param_ops_vmidfilter = {
 	.set = param_set_vmidfilter,
 	.get = param_get_vmidfilter,
 };
diff --git a/drivers/tty/hvc/hvc_opal.c b/drivers/tty/hvc/hvc_opal.c
index 543b234e70fc..47b54c6aefd2 100644
--- a/drivers/tty/hvc/hvc_opal.c
+++ b/drivers/tty/hvc/hvc_opal.c
@@ -29,6 +29,7 @@
 #include <linux/of.h>
 #include <linux/of_platform.h>
 #include <linux/export.h>
+#include <linux/interrupt.h>
 
 #include <asm/hvconsole.h>
 #include <asm/prom.h>
@@ -61,7 +62,6 @@ static struct hvc_opal_priv *hvc_opal_privs[MAX_NR_HVC_CONSOLES];
 /* For early boot console */
 static struct hvc_opal_priv hvc_opal_boot_priv;
 static u32 hvc_opal_boot_termno;
-static bool hvc_opal_event_registered;
 
 static const struct hv_ops hvc_opal_raw_ops = {
 	.get_chars = opal_get_chars,
@@ -162,28 +162,15 @@ static const struct hv_ops hvc_opal_hvsi_ops = {
 	.tiocmset = hvc_opal_hvsi_tiocmset,
 };
 
-static int hvc_opal_console_event(struct notifier_block *nb,
-				  unsigned long events, void *change)
-{
-	if (events & OPAL_EVENT_CONSOLE_INPUT)
-		hvc_kick();
-	return 0;
-}
-
-static struct notifier_block hvc_opal_console_nb = {
-	.notifier_call	= hvc_opal_console_event,
-};
-
 static int hvc_opal_probe(struct platform_device *dev)
 {
 	const struct hv_ops *ops;
 	struct hvc_struct *hp;
 	struct hvc_opal_priv *pv;
 	hv_protocol_t proto;
-	unsigned int termno, boot = 0;
+	unsigned int termno, irq, boot = 0;
 	const __be32 *reg;
 
-
 	if (of_device_is_compatible(dev->dev.of_node, "ibm,opal-console-raw")) {
 		proto = HV_PROTOCOL_RAW;
 		ops = &hvc_opal_raw_ops;
@@ -227,18 +214,18 @@ static int hvc_opal_probe(struct platform_device *dev)
 		dev->dev.of_node->full_name,
 		boot ? " (boot console)" : "");
 
-	/* We don't do IRQ ... */
-	hp = hvc_alloc(termno, 0, ops, MAX_VIO_PUT_CHARS);
+	irq = opal_event_request(ilog2(OPAL_EVENT_CONSOLE_INPUT));
+	if (!irq) {
+		pr_err("hvc_opal: Unable to map interrupt for device %s\n",
+			dev->dev.of_node->full_name);
+		return irq;
+	}
+
+	hp = hvc_alloc(termno, irq, ops, MAX_VIO_PUT_CHARS);
 	if (IS_ERR(hp))
 		return PTR_ERR(hp);
 	dev_set_drvdata(&dev->dev, hp);
 
-	/* ...  but we use OPAL event to kick the console */
-	if (!hvc_opal_event_registered) {
-		opal_notifier_register(&hvc_opal_console_nb);
-		hvc_opal_event_registered = true;
-	}
-
 	return 0;
 }
 
diff --git a/drivers/tty/hvc/hvc_tile.c b/drivers/tty/hvc/hvc_tile.c
index 3f6cd3102db5..9da1e842bbe9 100644
--- a/drivers/tty/hvc/hvc_tile.c
+++ b/drivers/tty/hvc/hvc_tile.c
@@ -51,7 +51,8 @@ int tile_console_write(const char *buf, int count)
 			      _SIM_CONTROL_OPERATOR_BITS));
 		return 0;
 	} else {
-		return hv_console_write((HV_VirtAddr)buf, count);
+		/* Translate 0 bytes written to EAGAIN for hvc_console_print. */
+		return hv_console_write((HV_VirtAddr)buf, count) ?: -EAGAIN;
 	}
 }
 
diff --git a/drivers/tty/hvc/hvc_xen.c b/drivers/tty/hvc/hvc_xen.c
index 7a3d146a5f0e..a9d837f83ce8 100644
--- a/drivers/tty/hvc/hvc_xen.c
+++ b/drivers/tty/hvc/hvc_xen.c
@@ -302,7 +302,7 @@ static int xen_initial_domain_console_init(void)
 static void xen_console_update_evtchn(struct xencons_info *info)
 {
 	if (xen_hvm_domain()) {
-		uint64_t v;
+		uint64_t v = 0;
 		int err;
 
 		err = hvm_get_parameter(HVM_PARAM_CONSOLE_EVTCHN, &v);
diff --git a/drivers/tty/hvc/hvcs.c b/drivers/tty/hvc/hvcs.c
index 81ff7e1bfb1a..f7ff97c0ad34 100644
--- a/drivers/tty/hvc/hvcs.c
+++ b/drivers/tty/hvc/hvcs.c
@@ -1044,8 +1044,8 @@ static int hvcs_enable_device(struct hvcs_struct *hvcsd, uint32_t unit_address,
 	 * It is possible that the vty-server was removed between the time that
 	 * the conn was registered and now.
 	 */
-	if (!(rc = request_irq(irq, &hvcs_handle_interrupt,
-				0, "ibmhvcs", hvcsd))) {
+	rc = request_irq(irq, &hvcs_handle_interrupt, 0, "ibmhvcs", hvcsd);
+	if (!rc) {
 		/*
 		 * It is possible the vty-server was removed after the irq was
 		 * requested but before we have time to enable interrupts.
diff --git a/drivers/tty/metag_da.c b/drivers/tty/metag_da.c
index 3774600741d8..9325262289f9 100644
--- a/drivers/tty/metag_da.c
+++ b/drivers/tty/metag_da.c
@@ -640,25 +640,7 @@ err_destroy_ports:
 	put_tty_driver(channel_driver);
 	return ret;
 }
-
-static void dashtty_exit(void)
-{
-	int nport;
-	struct dashtty_port *dport;
-
-	del_timer_sync(&put_timer);
-	kthread_stop(dashtty_thread);
-	del_timer_sync(&poll_timer);
-	tty_unregister_driver(channel_driver);
-	for (nport = 0; nport < NUM_TTY_CHANNELS; nport++) {
-		dport = &dashtty_ports[nport];
-		tty_port_destroy(&dport->port);
-	}
-	put_tty_driver(channel_driver);
-}
-
-module_init(dashtty_init);
-module_exit(dashtty_exit);
+device_initcall(dashtty_init);
 
 #ifdef CONFIG_DA_CONSOLE
 
diff --git a/drivers/tty/n_gsm.c b/drivers/tty/n_gsm.c
index 2c34c3249972..382d3fcba6cc 100644
--- a/drivers/tty/n_gsm.c
+++ b/drivers/tty/n_gsm.c
@@ -161,7 +161,7 @@ struct gsm_dlci {
 	struct net_device *net; /* network interface, if created */
 };
 
-/* DLCI 0, 62/63 are special or reseved see gsmtty_open */
+/* DLCI 0, 62/63 are special or reserved see gsmtty_open */
 
 #define NUM_DLCI		64
 
@@ -2274,7 +2274,6 @@ static void gsmld_receive_buf(struct tty_struct *tty, const unsigned char *cp,
 	const unsigned char *dp;
 	char *f;
 	int i;
-	char buf[64];
 	char flags = TTY_NORMAL;
 
 	if (debug & 4)
@@ -2296,7 +2295,7 @@ static void gsmld_receive_buf(struct tty_struct *tty, const unsigned char *cp,
 			break;
 		default:
 			WARN_ONCE(1, "%s: unknown flag %d\n",
-			       tty_name(tty, buf), flags);
+			       tty_name(tty), flags);
 			break;
 		}
 	}
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index cc57a3a6b02b..c9c27f69e101 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -162,6 +162,17 @@ static inline int tty_put_user(struct tty_struct *tty, unsigned char x,
 	return put_user(x, ptr);
 }
 
+static inline int tty_copy_to_user(struct tty_struct *tty,
+					void __user *to,
+					const void *from,
+					unsigned long n)
+{
+	struct n_tty_data *ldata = tty->disc_data;
+
+	tty_audit_add_data(tty, to, n, ldata->icanon);
+	return copy_to_user(to, from, n);
+}
+
 /**
  *	n_tty_kick_worker - start input worker (if required)
  *	@tty: terminal
@@ -1179,13 +1190,12 @@ static void n_tty_receive_break(struct tty_struct *tty)
 static void n_tty_receive_overrun(struct tty_struct *tty)
 {
 	struct n_tty_data *ldata = tty->disc_data;
-	char buf[64];
 
 	ldata->num_overrun++;
 	if (time_after(jiffies, ldata->overrun_time + HZ) ||
 			time_after(ldata->overrun_time, jiffies)) {
 		printk(KERN_WARNING "%s: %d input overrun(s)\n",
-			tty_name(tty, buf),
+			tty_name(tty),
 			ldata->num_overrun);
 		ldata->overrun_time = jiffies;
 		ldata->num_overrun = 0;
@@ -1460,8 +1470,6 @@ static void n_tty_receive_char_closing(struct tty_struct *tty, unsigned char c)
 static void
 n_tty_receive_char_flagged(struct tty_struct *tty, unsigned char c, char flag)
 {
-	char buf[64];
-
 	switch (flag) {
 	case TTY_BREAK:
 		n_tty_receive_break(tty);
@@ -1475,7 +1483,7 @@ n_tty_receive_char_flagged(struct tty_struct *tty, unsigned char c, char flag)
 		break;
 	default:
 		printk(KERN_ERR "%s: unknown flag %d\n",
-		       tty_name(tty, buf), flag);
+		       tty_name(tty), flag);
 		break;
 	}
 }
@@ -2070,8 +2078,8 @@ static int canon_copy_from_read_buf(struct tty_struct *tty,
 
 	size = N_TTY_BUF_SIZE - tail;
 	n = eol - tail;
-	if (n > 4096)
-		n += 4096;
+	if (n > N_TTY_BUF_SIZE)
+		n += N_TTY_BUF_SIZE;
 	n += found;
 	c = n;
 
@@ -2084,12 +2092,12 @@ static int canon_copy_from_read_buf(struct tty_struct *tty,
 		    __func__, eol, found, n, c, size, more);
 
 	if (n > size) {
-		ret = copy_to_user(*b, read_buf_addr(ldata, tail), size);
+		ret = tty_copy_to_user(tty, *b, read_buf_addr(ldata, tail), size);
 		if (ret)
 			return -EFAULT;
-		ret = copy_to_user(*b + size, ldata->read_buf, n - size);
+		ret = tty_copy_to_user(tty, *b + size, ldata->read_buf, n - size);
 	} else
-		ret = copy_to_user(*b, read_buf_addr(ldata, tail), n);
+		ret = tty_copy_to_user(tty, *b, read_buf_addr(ldata, tail), n);
 
 	if (ret)
 		return -EFAULT;
diff --git a/drivers/tty/nozomi.c b/drivers/tty/nozomi.c
index 74885af8c7bd..80f9de907563 100644
--- a/drivers/tty/nozomi.c
+++ b/drivers/tty/nozomi.c
@@ -140,8 +140,8 @@ static int debug;
 #define R_FCR		0x0000	/* Flow Control Register */
 #define R_IER		0x0004	/* Interrupt Enable Register */
 
-#define CONFIG_MAGIC	0xEFEFFEFE
-#define TOGGLE_VALID	0x0000
+#define NOZOMI_CONFIG_MAGIC	0xEFEFFEFE
+#define TOGGLE_VALID		0x0000
 
 /* Definition of interrupt tokens */
 #define MDM_DL1		0x0001
@@ -660,9 +660,9 @@ static int nozomi_read_config_table(struct nozomi *dc)
 	read_mem32((u32 *) &dc->config_table, dc->base_addr + 0,
 						sizeof(struct config_table));
 
-	if (dc->config_table.signature != CONFIG_MAGIC) {
+	if (dc->config_table.signature != NOZOMI_CONFIG_MAGIC) {
 		dev_err(&dc->pdev->dev, "ConfigTable Bad! 0x%08X != 0x%08X\n",
-			dc->config_table.signature, CONFIG_MAGIC);
+			dc->config_table.signature, NOZOMI_CONFIG_MAGIC);
 		return 0;
 	}
 
diff --git a/drivers/tty/rocket.h b/drivers/tty/rocket.h
index ec863f35f1a9..c11a9392f219 100644
--- a/drivers/tty/rocket.h
+++ b/drivers/tty/rocket.h
@@ -44,7 +44,7 @@ struct rocket_version {
 #define ROCKET_HUP_NOTIFY	0x00000004
 #define ROCKET_SPLIT_TERMIOS	0x00000008
 #define ROCKET_SPD_MASK		0x00000070
-#define ROCKET_SPD_HI		0x00000010	/* Use 56000 instead of 38400 bps */
+#define ROCKET_SPD_HI		0x00000010	/* Use 57600 instead of 38400 bps */
 #define ROCKET_SPD_VHI		0x00000020	/* Use 115200 instead of 38400 bps */
 #define ROCKET_SPD_SHI		0x00000030	/* Use 230400 instead of 38400 bps */
 #define ROCKET_SPD_WARP	        0x00000040	/* Use 460800 instead of 38400 bps */
diff --git a/drivers/tty/serial/68328serial.c b/drivers/tty/serial/68328serial.c
index 5dc9c4bfa66e..748c18f8c8cd 100644
--- a/drivers/tty/serial/68328serial.c
+++ b/drivers/tty/serial/68328serial.c
@@ -508,7 +508,8 @@ static void change_speed(struct m68k_serial *info, struct tty_struct *tty)
 	int	i;
 
 	cflag = tty->termios.c_cflag;
-	if (!(port = info->port))
+	port = info->port;
+	if (!port)
 		return;
 
 	ustcnt = uart->ustcnt;
diff --git a/drivers/tty/serial/8250/8250_core.c b/drivers/tty/serial/8250/8250_core.c
index 4506e405c8f3..37fff12dd4d0 100644
--- a/drivers/tty/serial/8250/8250_core.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -85,19 +85,6 @@ static unsigned int skip_txen_test; /* force skip of txen test at init time */
 #define BOTH_EMPTY 	(UART_LSR_TEMT | UART_LSR_THRE)
 
 
-#ifdef CONFIG_SERIAL_8250_DETECT_IRQ
-#define CONFIG_SERIAL_DETECT_IRQ 1
-#endif
-#ifdef CONFIG_SERIAL_8250_MANY_PORTS
-#define CONFIG_SERIAL_MANY_PORTS 1
-#endif
-
-/*
- * HUB6 is always on.  This will be removed once the header
- * files have been cleaned.
- */
-#define CONFIG_HUB6 1
-
 #include <asm/serial.h>
 /*
  * SERIAL_PORT_DFNS tells us about built-in ports that have no
@@ -2019,8 +2006,9 @@ EXPORT_SYMBOL_GPL(serial8250_do_set_mctrl);
 static void serial8250_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
 	if (port->set_mctrl)
-		return port->set_mctrl(port, mctrl);
-	return serial8250_do_set_mctrl(port, mctrl);
+		port->set_mctrl(port, mctrl);
+	else
+		serial8250_do_set_mctrl(port, mctrl);
 }
 
 static void serial8250_break_ctl(struct uart_port *port, int break_state)
@@ -3548,6 +3536,9 @@ static struct console univ8250_console = {
 
 static int __init univ8250_console_init(void)
 {
+	if (nr_uarts == 0)
+		return -ENODEV;
+
 	serial8250_isa_init_ports();
 	register_console(&univ8250_console);
 	return 0;
@@ -3578,7 +3569,7 @@ int __init early_serial_setup(struct uart_port *port)
 {
 	struct uart_port *p;
 
-	if (port->line >= ARRAY_SIZE(serial8250_ports))
+	if (port->line >= ARRAY_SIZE(serial8250_ports) || nr_uarts == 0)
 		return -ENODEV;
 
 	serial8250_isa_init_ports();
@@ -3850,7 +3841,6 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 		uart->port.mapbase      = up->port.mapbase;
 		uart->port.mapsize      = up->port.mapsize;
 		uart->port.private_data = up->port.private_data;
-		uart->port.fifosize	= up->port.fifosize;
 		uart->tx_loadsz		= up->tx_loadsz;
 		uart->capabilities	= up->capabilities;
 		uart->port.throttle	= up->port.throttle;
@@ -3945,6 +3935,9 @@ static int __init serial8250_init(void)
 {
 	int ret;
 
+	if (nr_uarts == 0)
+		return -ENODEV;
+
 	serial8250_isa_init_ports();
 
 	printk(KERN_INFO "Serial: 8250/16550 driver, "
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index 176f18f2e3ab..d48b50641e9a 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -377,6 +377,16 @@ static int dw8250_probe_of(struct uart_port *p,
 	return 0;
 }
 
+static bool dw8250_idma_filter(struct dma_chan *chan, void *param)
+{
+	struct device *dev = param;
+
+	if (dev != chan->device->dev->parent)
+		return false;
+
+	return true;
+}
+
 static int dw8250_probe_acpi(struct uart_8250_port *up,
 			     struct dw8250_data *data)
 {
@@ -389,8 +399,15 @@ static int dw8250_probe_acpi(struct uart_8250_port *up,
 	p->serial_out = dw8250_serial_out32;
 	p->regshift = 2;
 
-	up->dma = &data->dma;
+	/* Platforms with iDMA */
+	if (platform_get_resource_byname(to_platform_device(up->port.dev),
+					 IORESOURCE_MEM, "lpss_priv")) {
+		data->dma.rx_param = up->port.dev->parent;
+		data->dma.tx_param = up->port.dev->parent;
+		data->dma.fn = dw8250_idma_filter;
+	}
 
+	up->dma = &data->dma;
 	up->dma->rxconf.src_maxburst = p->fifosize / 4;
 	up->dma->txconf.dst_maxburst = p->fifosize / 4;
 
diff --git a/drivers/tty/serial/8250/8250_early.c b/drivers/tty/serial/8250/8250_early.c
index 6c0fd8b9d1c3..771dda29a0f8 100644
--- a/drivers/tty/serial/8250/8250_early.c
+++ b/drivers/tty/serial/8250/8250_early.c
@@ -131,7 +131,7 @@ static void __init init_port(struct earlycon_device *device)
 	serial8250_early_out(port, UART_LCR, c & ~UART_LCR_DLAB);
 }
 
-static int __init early_serial8250_setup(struct earlycon_device *device,
+int __init early_serial8250_setup(struct earlycon_device *device,
 					 const char *options)
 {
 	if (!(device->port.membase || device->port.iobase))
diff --git a/drivers/tty/serial/8250/8250_ingenic.c b/drivers/tty/serial/8250/8250_ingenic.c
new file mode 100644
index 000000000000..21bf81fe794f
--- /dev/null
+++ b/drivers/tty/serial/8250/8250_ingenic.c
@@ -0,0 +1,266 @@
+/*
+ * Copyright (C) 2010 Lars-Peter Clausen <lars@metafoo.de>
+ * Copyright (C) 2015 Imagination Technologies
+ *
+ * Ingenic SoC UART support
+ *
+ * This program is free software; you can redistribute	 it and/or modify it
+ * under  the terms of	 the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the	License, or (at your
+ * option) any later version.
+ *
+ * You should have received a copy of the  GNU General Public License along
+ * with this program; if not, write  to the Free Software Foundation, Inc.,
+ * 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/clk.h>
+#include <linux/console.h>
+#include <linux/io.h>
+#include <linux/libfdt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+#include <linux/platform_device.h>
+#include <linux/serial_8250.h>
+#include <linux/serial_core.h>
+#include <linux/serial_reg.h>
+
+struct ingenic_uart_data {
+	struct clk	*clk_module;
+	struct clk	*clk_baud;
+	int		line;
+};
+
+#define UART_FCR_UME	BIT(4)
+
+static struct earlycon_device *early_device;
+
+static uint8_t __init early_in(struct uart_port *port, int offset)
+{
+	return readl(port->membase + (offset << 2));
+}
+
+static void __init early_out(struct uart_port *port, int offset, uint8_t value)
+{
+	writel(value, port->membase + (offset << 2));
+}
+
+static void __init ingenic_early_console_putc(struct uart_port *port, int c)
+{
+	uint8_t lsr;
+
+	do {
+		lsr = early_in(port, UART_LSR);
+	} while ((lsr & UART_LSR_TEMT) == 0);
+
+	early_out(port, UART_TX, c);
+}
+
+static void __init ingenic_early_console_write(struct console *console,
+					      const char *s, unsigned int count)
+{
+	uart_console_write(&early_device->port, s, count,
+			   ingenic_early_console_putc);
+}
+
+static void __init ingenic_early_console_setup_clock(struct earlycon_device *dev)
+{
+	void *fdt = initial_boot_params;
+	const __be32 *prop;
+	int offset;
+
+	offset = fdt_path_offset(fdt, "/ext");
+	if (offset < 0)
+		return;
+
+	prop = fdt_getprop(fdt, offset, "clock-frequency", NULL);
+	if (!prop)
+		return;
+
+	dev->port.uartclk = be32_to_cpup(prop);
+}
+
+static int __init ingenic_early_console_setup(struct earlycon_device *dev,
+					      const char *opt)
+{
+	struct uart_port *port = &dev->port;
+	unsigned int baud, divisor;
+
+	if (!dev->port.membase)
+		return -ENODEV;
+
+	ingenic_early_console_setup_clock(dev);
+
+	baud = dev->baud ?: 115200;
+	divisor = DIV_ROUND_CLOSEST(port->uartclk, 16 * baud);
+
+	early_out(port, UART_IER, 0);
+	early_out(port, UART_LCR, UART_LCR_DLAB | UART_LCR_WLEN8);
+	early_out(port, UART_DLL, 0);
+	early_out(port, UART_DLM, 0);
+	early_out(port, UART_LCR, UART_LCR_WLEN8);
+	early_out(port, UART_FCR, UART_FCR_UME | UART_FCR_CLEAR_XMIT |
+			UART_FCR_CLEAR_RCVR | UART_FCR_ENABLE_FIFO);
+	early_out(port, UART_MCR, UART_MCR_RTS | UART_MCR_DTR);
+
+	early_out(port, UART_LCR, UART_LCR_DLAB | UART_LCR_WLEN8);
+	early_out(port, UART_DLL, divisor & 0xff);
+	early_out(port, UART_DLM, (divisor >> 8) & 0xff);
+	early_out(port, UART_LCR, UART_LCR_WLEN8);
+
+	early_device = dev;
+	dev->con->write = ingenic_early_console_write;
+
+	return 0;
+}
+
+EARLYCON_DECLARE(jz4740_uart, ingenic_early_console_setup);
+OF_EARLYCON_DECLARE(jz4740_uart, "ingenic,jz4740-uart",
+		    ingenic_early_console_setup);
+
+EARLYCON_DECLARE(jz4775_uart, ingenic_early_console_setup);
+OF_EARLYCON_DECLARE(jz4775_uart, "ingenic,jz4775-uart",
+		    ingenic_early_console_setup);
+
+EARLYCON_DECLARE(jz4780_uart, ingenic_early_console_setup);
+OF_EARLYCON_DECLARE(jz4780_uart, "ingenic,jz4780-uart",
+		    ingenic_early_console_setup);
+
+static void ingenic_uart_serial_out(struct uart_port *p, int offset, int value)
+{
+	switch (offset) {
+	case UART_FCR:
+		/* UART module enable */
+		value |= UART_FCR_UME;
+		break;
+
+	case UART_IER:
+		value |= (value & 0x4) << 2;
+		break;
+
+	default:
+		break;
+	}
+
+	writeb(value, p->membase + (offset << p->regshift));
+}
+
+static int ingenic_uart_probe(struct platform_device *pdev)
+{
+	struct uart_8250_port uart = {};
+	struct resource *regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	struct resource *irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	struct ingenic_uart_data *data;
+	int err, line;
+
+	if (!regs || !irq) {
+		dev_err(&pdev->dev, "no registers/irq defined\n");
+		return -EINVAL;
+	}
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	spin_lock_init(&uart.port.lock);
+	uart.port.type = PORT_16550;
+	uart.port.flags = UPF_SKIP_TEST | UPF_IOREMAP | UPF_FIXED_TYPE;
+	uart.port.iotype = UPIO_MEM;
+	uart.port.mapbase = regs->start;
+	uart.port.regshift = 2;
+	uart.port.serial_out = ingenic_uart_serial_out;
+	uart.port.irq = irq->start;
+	uart.port.dev = &pdev->dev;
+
+	/* Check for a fixed line number */
+	line = of_alias_get_id(pdev->dev.of_node, "serial");
+	if (line >= 0)
+		uart.port.line = line;
+
+	uart.port.membase = devm_ioremap(&pdev->dev, regs->start,
+					 resource_size(regs));
+	if (!uart.port.membase)
+		return -ENOMEM;
+
+	data->clk_module = devm_clk_get(&pdev->dev, "module");
+	if (IS_ERR(data->clk_module)) {
+		err = PTR_ERR(data->clk_module);
+		if (err != -EPROBE_DEFER)
+			dev_err(&pdev->dev,
+				"unable to get module clock: %d\n", err);
+		return err;
+	}
+
+	data->clk_baud = devm_clk_get(&pdev->dev, "baud");
+	if (IS_ERR(data->clk_baud)) {
+		err = PTR_ERR(data->clk_baud);
+		if (err != -EPROBE_DEFER)
+			dev_err(&pdev->dev,
+				"unable to get baud clock: %d\n", err);
+		return err;
+	}
+
+	err = clk_prepare_enable(data->clk_module);
+	if (err) {
+		dev_err(&pdev->dev, "could not enable module clock: %d\n", err);
+		goto out;
+	}
+
+	err = clk_prepare_enable(data->clk_baud);
+	if (err) {
+		dev_err(&pdev->dev, "could not enable baud clock: %d\n", err);
+		goto out_disable_moduleclk;
+	}
+	uart.port.uartclk = clk_get_rate(data->clk_baud);
+
+	data->line = serial8250_register_8250_port(&uart);
+	if (data->line < 0) {
+		err = data->line;
+		goto out_disable_baudclk;
+	}
+
+	platform_set_drvdata(pdev, data);
+	return 0;
+
+out_disable_baudclk:
+	clk_disable_unprepare(data->clk_baud);
+out_disable_moduleclk:
+	clk_disable_unprepare(data->clk_module);
+out:
+	return err;
+}
+
+static int ingenic_uart_remove(struct platform_device *pdev)
+{
+	struct ingenic_uart_data *data = platform_get_drvdata(pdev);
+
+	serial8250_unregister_port(data->line);
+	clk_disable_unprepare(data->clk_module);
+	clk_disable_unprepare(data->clk_baud);
+	return 0;
+}
+
+static const struct of_device_id of_match[] = {
+	{ .compatible = "ingenic,jz4740-uart" },
+	{ .compatible = "ingenic,jz4775-uart" },
+	{ .compatible = "ingenic,jz4780-uart" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, of_match);
+
+static struct platform_driver ingenic_uart_platform_driver = {
+	.driver = {
+		.name		= "ingenic-uart",
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match,
+	},
+	.probe			= ingenic_uart_probe,
+	.remove			= ingenic_uart_remove,
+};
+
+module_platform_driver(ingenic_uart_platform_driver);
+
+MODULE_AUTHOR("Paul Burton");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Ingenic SoC UART driver");
diff --git a/drivers/tty/serial/8250/8250_lpc18xx.c b/drivers/tty/serial/8250/8250_lpc18xx.c
new file mode 100644
index 000000000000..99cd478851ff
--- /dev/null
+++ b/drivers/tty/serial/8250/8250_lpc18xx.c
@@ -0,0 +1,230 @@
+/*
+ * Serial port driver for NXP LPC18xx/43xx UART
+ *
+ * Copyright (C) 2015 Joachim Eastwood <manabian@gmail.com>
+ *
+ * Based on 8250_mtk.c:
+ * Copyright (c) 2014 MundoReader S.L.
+ * Matthias Brugger <matthias.bgg@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/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#include "8250.h"
+
+/* Additional LPC18xx/43xx 8250 registers and bits */
+#define LPC18XX_UART_RS485CTRL		(0x04c / sizeof(u32))
+#define  LPC18XX_UART_RS485CTRL_NMMEN	BIT(0)
+#define  LPC18XX_UART_RS485CTRL_DCTRL	BIT(4)
+#define  LPC18XX_UART_RS485CTRL_OINV	BIT(5)
+#define LPC18XX_UART_RS485DLY		(0x054 / sizeof(u32))
+#define LPC18XX_UART_RS485DLY_MAX	255
+
+struct lpc18xx_uart_data {
+	struct uart_8250_dma dma;
+	struct clk *clk_uart;
+	struct clk *clk_reg;
+	int line;
+};
+
+static int lpc18xx_rs485_config(struct uart_port *port,
+				struct serial_rs485 *rs485)
+{
+	struct uart_8250_port *up = up_to_u8250p(port);
+	u32 rs485_ctrl_reg = 0;
+	u32 rs485_dly_reg = 0;
+	unsigned baud_clk;
+
+	if (rs485->flags & SER_RS485_ENABLED)
+		memset(rs485->padding, 0, sizeof(rs485->padding));
+	else
+		memset(rs485, 0, sizeof(*rs485));
+
+	rs485->flags &= SER_RS485_ENABLED | SER_RS485_RTS_ON_SEND |
+			SER_RS485_RTS_AFTER_SEND;
+
+	if (rs485->flags & SER_RS485_ENABLED) {
+		rs485_ctrl_reg |= LPC18XX_UART_RS485CTRL_NMMEN |
+				  LPC18XX_UART_RS485CTRL_DCTRL;
+
+		if (rs485->flags & SER_RS485_RTS_ON_SEND) {
+			rs485_ctrl_reg |= LPC18XX_UART_RS485CTRL_OINV;
+			rs485->flags &= ~SER_RS485_RTS_AFTER_SEND;
+		} else {
+			rs485->flags |= SER_RS485_RTS_AFTER_SEND;
+		}
+	}
+
+	if (rs485->delay_rts_after_send) {
+		baud_clk = port->uartclk / up->dl_read(up);
+		rs485_dly_reg = DIV_ROUND_UP(rs485->delay_rts_after_send
+						* baud_clk, MSEC_PER_SEC);
+
+		if (rs485_dly_reg > LPC18XX_UART_RS485DLY_MAX)
+			rs485_dly_reg = LPC18XX_UART_RS485DLY_MAX;
+
+		/* Calculate the resulting delay in ms */
+		rs485->delay_rts_after_send = (rs485_dly_reg * MSEC_PER_SEC)
+						/ baud_clk;
+	}
+
+	/* Delay RTS before send not supported */
+	rs485->delay_rts_before_send = 0;
+
+	serial_out(up, LPC18XX_UART_RS485CTRL, rs485_ctrl_reg);
+	serial_out(up, LPC18XX_UART_RS485DLY, rs485_dly_reg);
+
+	port->rs485 = *rs485;
+
+	return 0;
+}
+
+static void lpc18xx_uart_serial_out(struct uart_port *p, int offset, int value)
+{
+	/*
+	 * For DMA mode one must ensure that the UART_FCR_DMA_SELECT
+	 * bit is set when FIFO is enabled. Even if DMA is not used
+	 * setting this bit doesn't seem to affect anything.
+	 */
+	if (offset == UART_FCR && (value & UART_FCR_ENABLE_FIFO))
+		value |= UART_FCR_DMA_SELECT;
+
+	offset = offset << p->regshift;
+	writel(value, p->membase + offset);
+}
+
+static int lpc18xx_serial_probe(struct platform_device *pdev)
+{
+	struct lpc18xx_uart_data *data;
+	struct uart_8250_port uart;
+	struct resource *res;
+	int irq, ret;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "irq not found");
+		return irq;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "memory resource not found");
+		return -EINVAL;
+	}
+
+	memset(&uart, 0, sizeof(uart));
+
+	uart.port.membase = devm_ioremap(&pdev->dev, res->start,
+					 resource_size(res));
+	if (!uart.port.membase)
+		return -ENOMEM;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->clk_uart = devm_clk_get(&pdev->dev, "uartclk");
+	if (IS_ERR(data->clk_uart)) {
+		dev_err(&pdev->dev, "uart clock not found\n");
+		return PTR_ERR(data->clk_uart);
+	}
+
+	data->clk_reg = devm_clk_get(&pdev->dev, "reg");
+	if (IS_ERR(data->clk_reg)) {
+		dev_err(&pdev->dev, "reg clock not found\n");
+		return PTR_ERR(data->clk_reg);
+	}
+
+	ret = clk_prepare_enable(data->clk_reg);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to enable reg clock\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(data->clk_uart);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to enable uart clock\n");
+		goto dis_clk_reg;
+	}
+
+	ret = of_alias_get_id(pdev->dev.of_node, "serial");
+	if (ret >= 0)
+		uart.port.line = ret;
+
+	data->dma.rx_param = data;
+	data->dma.tx_param = data;
+
+	spin_lock_init(&uart.port.lock);
+	uart.port.dev = &pdev->dev;
+	uart.port.irq = irq;
+	uart.port.iotype = UPIO_MEM32;
+	uart.port.mapbase = res->start;
+	uart.port.regshift = 2;
+	uart.port.type = PORT_16550A;
+	uart.port.flags = UPF_FIXED_PORT | UPF_FIXED_TYPE | UPF_SKIP_TEST;
+	uart.port.uartclk = clk_get_rate(data->clk_uart);
+	uart.port.private_data = data;
+	uart.port.rs485_config = lpc18xx_rs485_config;
+	uart.port.serial_out = lpc18xx_uart_serial_out;
+
+	uart.dma = &data->dma;
+	uart.dma->rxconf.src_maxburst = 1;
+	uart.dma->txconf.dst_maxburst = 1;
+
+	ret = serial8250_register_8250_port(&uart);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "unable to register 8250 port\n");
+		goto dis_uart_clk;
+	}
+
+	data->line = ret;
+	platform_set_drvdata(pdev, data);
+
+	return 0;
+
+dis_uart_clk:
+	clk_disable_unprepare(data->clk_uart);
+dis_clk_reg:
+	clk_disable_unprepare(data->clk_reg);
+	return ret;
+}
+
+static int lpc18xx_serial_remove(struct platform_device *pdev)
+{
+	struct lpc18xx_uart_data *data = platform_get_drvdata(pdev);
+
+	serial8250_unregister_port(data->line);
+	clk_disable_unprepare(data->clk_uart);
+	clk_disable_unprepare(data->clk_reg);
+
+	return 0;
+}
+
+static const struct of_device_id lpc18xx_serial_match[] = {
+	{ .compatible = "nxp,lpc1850-uart" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, lpc18xx_serial_match);
+
+static struct platform_driver lpc18xx_serial_driver = {
+	.probe  = lpc18xx_serial_probe,
+	.remove = lpc18xx_serial_remove,
+	.driver = {
+		.name = "lpc18xx-uart",
+		.of_match_table = lpc18xx_serial_match,
+	},
+};
+module_platform_driver(lpc18xx_serial_driver);
+
+MODULE_AUTHOR("Joachim Eastwood <manabian@gmail.com>");
+MODULE_DESCRIPTION("Serial port driver NXP LPC18xx/43xx devices");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/8250/8250_mtk.c b/drivers/tty/serial/8250/8250_mtk.c
index 7a11fac775c4..78883ca64ddd 100644
--- a/drivers/tty/serial/8250/8250_mtk.c
+++ b/drivers/tty/serial/8250/8250_mtk.c
@@ -34,6 +34,7 @@
 struct mtk8250_data {
 	int			line;
 	struct clk		*uart_clk;
+	struct clk		*bus_clk;
 };
 
 static void
@@ -115,6 +116,36 @@ mtk8250_set_termios(struct uart_port *port, struct ktermios *termios,
 		tty_termios_encode_baud_rate(termios, baud, baud);
 }
 
+static int mtk8250_runtime_suspend(struct device *dev)
+{
+	struct mtk8250_data *data = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(data->uart_clk);
+	clk_disable_unprepare(data->bus_clk);
+
+	return 0;
+}
+
+static int mtk8250_runtime_resume(struct device *dev)
+{
+	struct mtk8250_data *data = dev_get_drvdata(dev);
+	int err;
+
+	err = clk_prepare_enable(data->uart_clk);
+	if (err) {
+		dev_warn(dev, "Can't enable clock\n");
+		return err;
+	}
+
+	err = clk_prepare_enable(data->bus_clk);
+	if (err) {
+		dev_warn(dev, "Can't enable bus clock\n");
+		return err;
+	}
+
+	return 0;
+}
+
 static void
 mtk8250_do_pm(struct uart_port *port, unsigned int state, unsigned int old)
 {
@@ -130,22 +161,24 @@ mtk8250_do_pm(struct uart_port *port, unsigned int state, unsigned int old)
 static int mtk8250_probe_of(struct platform_device *pdev, struct uart_port *p,
 			   struct mtk8250_data *data)
 {
-	int err;
-	struct device_node *np = pdev->dev.of_node;
-
-	data->uart_clk = of_clk_get(np, 0);
+	data->uart_clk = devm_clk_get(&pdev->dev, "baud");
 	if (IS_ERR(data->uart_clk)) {
-		dev_warn(&pdev->dev, "Can't get timer clock\n");
-		return PTR_ERR(data->uart_clk);
+		/*
+		 * For compatibility with older device trees try unnamed
+		 * clk when no baud clk can be found.
+		 */
+		data->uart_clk = devm_clk_get(&pdev->dev, NULL);
+		if (IS_ERR(data->uart_clk)) {
+			dev_warn(&pdev->dev, "Can't get uart clock\n");
+			return PTR_ERR(data->uart_clk);
+		}
+
+		return 0;
 	}
 
-	err = clk_prepare_enable(data->uart_clk);
-	if (err) {
-		dev_warn(&pdev->dev, "Can't prepare clock\n");
-		clk_put(data->uart_clk);
-		return err;
-	}
-	p->uartclk = clk_get_rate(data->uart_clk);
+	data->bus_clk = devm_clk_get(&pdev->dev, "bus");
+	if (IS_ERR(data->bus_clk))
+		return PTR_ERR(data->bus_clk);
 
 	return 0;
 }
@@ -190,19 +223,24 @@ static int mtk8250_probe(struct platform_device *pdev)
 	uart.port.regshift = 2;
 	uart.port.private_data = data;
 	uart.port.set_termios = mtk8250_set_termios;
+	uart.port.uartclk = clk_get_rate(data->uart_clk);
 
 	/* Disable Rate Fix function */
 	writel(0x0, uart.port.membase +
 			(MTK_UART_RATE_FIX << uart.port.regshift));
 
-	data->line = serial8250_register_8250_port(&uart);
-	if (data->line < 0)
-		return data->line;
-
 	platform_set_drvdata(pdev, data);
 
-	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
+	if (!pm_runtime_enabled(&pdev->dev)) {
+		err = mtk8250_runtime_resume(&pdev->dev);
+		if (err)
+			return err;
+	}
+
+	data->line = serial8250_register_8250_port(&uart);
+	if (data->line < 0)
+		return data->line;
 
 	return 0;
 }
@@ -214,13 +252,13 @@ static int mtk8250_remove(struct platform_device *pdev)
 	pm_runtime_get_sync(&pdev->dev);
 
 	serial8250_unregister_port(data->line);
-	if (!IS_ERR(data->uart_clk)) {
-		clk_disable_unprepare(data->uart_clk);
-		clk_put(data->uart_clk);
-	}
 
 	pm_runtime_disable(&pdev->dev);
 	pm_runtime_put_noidle(&pdev->dev);
+
+	if (!pm_runtime_status_suspended(&pdev->dev))
+		mtk8250_runtime_suspend(&pdev->dev);
+
 	return 0;
 }
 
@@ -244,28 +282,6 @@ static int mtk8250_resume(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
-#ifdef CONFIG_PM
-static int mtk8250_runtime_suspend(struct device *dev)
-{
-	struct mtk8250_data *data = dev_get_drvdata(dev);
-
-	if (!IS_ERR(data->uart_clk))
-		clk_disable_unprepare(data->uart_clk);
-
-	return 0;
-}
-
-static int mtk8250_runtime_resume(struct device *dev)
-{
-	struct mtk8250_data *data = dev_get_drvdata(dev);
-
-	if (!IS_ERR(data->uart_clk))
-		clk_prepare_enable(data->uart_clk);
-
-	return 0;
-}
-#endif
-
 static const struct dev_pm_ops mtk8250_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(mtk8250_suspend, mtk8250_resume)
 	SET_RUNTIME_PM_OPS(mtk8250_runtime_suspend, mtk8250_runtime_resume,
@@ -289,6 +305,21 @@ static struct platform_driver mtk8250_platform_driver = {
 };
 module_platform_driver(mtk8250_platform_driver);
 
+#ifdef CONFIG_SERIAL_8250_CONSOLE
+static int __init early_mtk8250_setup(struct earlycon_device *device,
+					const char *options)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->port.iotype = UPIO_MEM32;
+
+	return early_serial8250_setup(device, NULL);
+}
+
+OF_EARLYCON_DECLARE(mtk8250, "mediatek,mt6577-uart", early_mtk8250_setup);
+#endif
+
 MODULE_AUTHOR("Matthias Brugger");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Mediatek 8250 serial port driver");
diff --git a/drivers/tty/serial/8250/8250_omap.c b/drivers/tty/serial/8250/8250_omap.c
index 9289999cb7c6..d75a66c72750 100644
--- a/drivers/tty/serial/8250/8250_omap.c
+++ b/drivers/tty/serial/8250/8250_omap.c
@@ -22,6 +22,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/console.h>
 #include <linux/pm_qos.h>
+#include <linux/pm_wakeirq.h>
 #include <linux/dma-mapping.h>
 
 #include "8250.h"
@@ -98,6 +99,7 @@ struct omap8250_priv {
 	struct pm_qos_request pm_qos_request;
 	struct work_struct qos_work;
 	struct uart_8250_dma omap8250_dma;
+	spinlock_t rx_dma_lock;
 };
 
 static u32 uart_read(struct uart_8250_port *up, u32 reg)
@@ -551,39 +553,71 @@ static void omap8250_uart_qos_work(struct work_struct *work)
 	pm_qos_update_request(&priv->pm_qos_request, priv->latency);
 }
 
-static irqreturn_t omap_wake_irq(int irq, void *dev_id)
+#ifdef CONFIG_SERIAL_8250_DMA
+static int omap_8250_dma_handle_irq(struct uart_port *port);
+#endif
+
+static irqreturn_t omap8250_irq(int irq, void *dev_id)
 {
 	struct uart_port *port = dev_id;
+	struct uart_8250_port *up = up_to_u8250p(port);
+	unsigned int iir;
 	int ret;
 
-	ret = port->handle_irq(port);
-	if (ret)
-		return IRQ_HANDLED;
-	return IRQ_NONE;
+#ifdef CONFIG_SERIAL_8250_DMA
+	if (up->dma) {
+		ret = omap_8250_dma_handle_irq(port);
+		return IRQ_RETVAL(ret);
+	}
+#endif
+
+	serial8250_rpm_get(up);
+	iir = serial_port_in(port, UART_IIR);
+	ret = serial8250_handle_irq(port, iir);
+	serial8250_rpm_put(up);
+
+	return IRQ_RETVAL(ret);
 }
 
 static int omap_8250_startup(struct uart_port *port)
 {
-	struct uart_8250_port *up =
-		container_of(port, struct uart_8250_port, port);
+	struct uart_8250_port *up = up_to_u8250p(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);
+		ret = dev_pm_set_dedicated_wake_irq(port->dev, priv->wakeirq);
 		if (ret)
 			return ret;
-		disable_irq(priv->wakeirq);
 	}
 
 	pm_runtime_get_sync(port->dev);
 
-	ret = serial8250_do_startup(port);
-	if (ret)
+	up->mcr = 0;
+	serial_out(up, UART_FCR, UART_FCR_CLEAR_RCVR | UART_FCR_CLEAR_XMIT);
+
+	serial_out(up, UART_LCR, UART_LCR_WLEN8);
+
+	up->lsr_saved_flags = 0;
+	up->msr_saved_flags = 0;
+
+	if (up->dma) {
+		ret = serial8250_request_dma(up);
+		if (ret) {
+			dev_warn_ratelimited(port->dev,
+					     "failed to request DMA\n");
+			up->dma = NULL;
+		}
+	}
+
+	ret = request_irq(port->irq, omap8250_irq, IRQF_SHARED,
+			  dev_name(port->dev), port);
+	if (ret < 0)
 		goto err;
 
+	up->ier = UART_IER_RLSI | UART_IER_RDI;
+	serial_out(up, UART_IER, up->ier);
+
 #ifdef CONFIG_PM
 	up->capabilities |= UART_CAP_RPM;
 #endif
@@ -603,15 +637,13 @@ static int omap_8250_startup(struct uart_port *port)
 err:
 	pm_runtime_mark_last_busy(port->dev);
 	pm_runtime_put_autosuspend(port->dev);
-	if (priv->wakeirq)
-		free_irq(priv->wakeirq, port);
+	dev_pm_clear_wake_irq(port->dev);
 	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 uart_8250_port *up = up_to_u8250p(port);
 	struct omap8250_priv *priv = port->private_data;
 
 	flush_work(&priv->qos_work);
@@ -621,13 +653,24 @@ static void omap_8250_shutdown(struct uart_port *port)
 	pm_runtime_get_sync(port->dev);
 
 	serial_out(up, UART_OMAP_WER, 0);
-	serial8250_do_shutdown(port);
+
+	up->ier = 0;
+	serial_out(up, UART_IER, 0);
+
+	if (up->dma)
+		serial8250_release_dma(up);
+
+	/*
+	 * Disable break condition and FIFOs
+	 */
+	if (up->lcr & UART_LCR_SBC)
+		serial_out(up, UART_LCR, up->lcr & ~UART_LCR_SBC);
+	serial_out(up, UART_FCR, UART_FCR_CLEAR_RCVR | UART_FCR_CLEAR_XMIT);
 
 	pm_runtime_mark_last_busy(port->dev);
 	pm_runtime_put_autosuspend(port->dev);
-
-	if (priv->wakeirq)
-		free_irq(priv->wakeirq, port);
+	free_irq(port->irq, port);
+	dev_pm_clear_wake_irq(port->dev);
 }
 
 static void omap_8250_throttle(struct uart_port *port)
@@ -669,14 +712,21 @@ 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 omap8250_priv	*priv = p->port.private_data;
 	struct uart_8250_dma    *dma = p->dma;
 	struct tty_port         *tty_port = &p->port.state->port;
 	struct dma_tx_state     state;
 	int                     count;
+	unsigned long		flags;
 
 	dma_sync_single_for_cpu(dma->rxchan->device->dev, dma->rx_addr,
 				dma->rx_size, DMA_FROM_DEVICE);
 
+	spin_lock_irqsave(&priv->rx_dma_lock, flags);
+
+	if (!dma->rx_running)
+		goto unlock;
+
 	dma->rx_running = 0;
 	dmaengine_tx_status(dma->rxchan, dma->rx_cookie, &state);
 	dmaengine_terminate_all(dma->rxchan);
@@ -685,6 +735,9 @@ static void __dma_rx_do_complete(struct uart_8250_port *p, bool error)
 
 	tty_insert_flip_string(tty_port, dma->rx_buf, count);
 	p->port.icount.rx += count;
+unlock:
+	spin_unlock_irqrestore(&priv->rx_dma_lock, flags);
+
 	if (!error)
 		omap_8250_rx_dma(p, 0);
 
@@ -696,28 +749,45 @@ static void __dma_rx_complete(void *param)
 	__dma_rx_do_complete(param, false);
 }
 
+static void omap_8250_rx_dma_flush(struct uart_8250_port *p)
+{
+	struct omap8250_priv	*priv = p->port.private_data;
+	struct uart_8250_dma	*dma = p->dma;
+	unsigned long		flags;
+
+	spin_lock_irqsave(&priv->rx_dma_lock, flags);
+
+	if (!dma->rx_running) {
+		spin_unlock_irqrestore(&priv->rx_dma_lock, flags);
+		return;
+	}
+
+	dmaengine_pause(dma->rxchan);
+
+	spin_unlock_irqrestore(&priv->rx_dma_lock, flags);
+
+	__dma_rx_do_complete(p, true);
+}
+
 static int omap_8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 {
+	struct omap8250_priv		*priv = p->port.private_data;
 	struct uart_8250_dma            *dma = p->dma;
+	int				err = 0;
 	struct dma_async_tx_descriptor  *desc;
+	unsigned long			flags;
 
 	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);
-		}
+		omap_8250_rx_dma_flush(p);
 		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);
-		}
+		omap_8250_rx_dma_flush(p);
 		return -ETIMEDOUT;
 	case UART_IIR_RDI:
 		/*
@@ -729,24 +799,25 @@ static int omap_8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 		 * 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);
-		}
+		omap_8250_rx_dma_flush(p);
 		return -ETIMEDOUT;
 
 	default:
 		break;
 	}
 
+	spin_lock_irqsave(&priv->rx_dma_lock, flags);
+
 	if (dma->rx_running)
-		return 0;
+		goto out;
 
 	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;
+	if (!desc) {
+		err = -EBUSY;
+		goto out;
+	}
 
 	dma->rx_running = 1;
 	desc->callback = __dma_rx_complete;
@@ -758,7 +829,9 @@ static int omap_8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 				   dma->rx_size, DMA_FROM_DEVICE);
 
 	dma_async_issue_pending(dma->rxchan);
-	return 0;
+out:
+	spin_unlock_irqrestore(&priv->rx_dma_lock, flags);
+	return err;
 }
 
 static int omap_8250_tx_dma(struct uart_8250_port *p);
@@ -974,6 +1047,13 @@ static inline int omap_8250_rx_dma(struct uart_8250_port *p, unsigned int iir)
 }
 #endif
 
+static int omap8250_no_handle_irq(struct uart_port *port)
+{
+	/* IRQ has not been requested but handling irq? */
+	WARN_ONCE(1, "Unexpected irq handling before port startup\n");
+	return 0;
+}
+
 static int omap8250_probe(struct platform_device *pdev)
 {
 	struct resource *regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -1065,6 +1145,8 @@ static int omap8250_probe(struct platform_device *pdev)
 			   priv->latency);
 	INIT_WORK(&priv->qos_work, omap8250_uart_qos_work);
 
+	spin_lock_init(&priv->rx_dma_lock);
+
 	device_init_wakeup(&pdev->dev, true);
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev, -1);
@@ -1075,6 +1157,7 @@ static int omap8250_probe(struct platform_device *pdev)
 	pm_runtime_get_sync(&pdev->dev);
 
 	omap_serial_fill_features_erratas(&up, priv);
+	up.port.handle_irq = omap8250_no_handle_irq;
 #ifdef CONFIG_SERIAL_8250_DMA
 	if (pdev->dev.of_node) {
 		/*
@@ -1088,7 +1171,6 @@ static int omap8250_probe(struct platform_device *pdev)
 		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;
@@ -1129,31 +1211,6 @@ static int omap8250_remove(struct platform_device *pdev)
 	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)
 {
@@ -1180,11 +1237,6 @@ static int omap8250_suspend(struct device *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;
 }
 
@@ -1192,9 +1244,6 @@ 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;
 }
@@ -1236,7 +1285,6 @@ static int omap8250_runtime_suspend(struct device *dev)
 			return -EBUSY;
 	}
 
-	omap8250_enable_wakeup(priv, true);
 	if (up->dma)
 		omap_8250_rx_dma(up, UART_IIR_RX_TIMEOUT);
 
@@ -1257,7 +1305,6 @@ static int omap8250_runtime_resume(struct device *dev)
 		return 0;
 
 	up = serial8250_get_port(priv->line);
-	omap8250_enable_wakeup(priv, false);
 	loss_cntx = omap8250_lost_context(up);
 
 	if (loss_cntx)
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index 46bcebba54b2..e55f18b93fe7 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -1823,6 +1823,9 @@ static int pci_eg20t_init(struct pci_dev *dev)
 #endif
 }
 
+#define PCI_DEVICE_ID_EXAR_XR17V4358	0x4358
+#define PCI_DEVICE_ID_EXAR_XR17V8358	0x8358
+
 static int
 pci_xr17c154_setup(struct serial_private *priv,
 		  const struct pciserial_board *board,
@@ -1832,6 +1835,15 @@ pci_xr17c154_setup(struct serial_private *priv,
 	return pci_default_setup(priv, board, port, idx);
 }
 
+static inline int
+xr17v35x_has_slave(struct serial_private *priv)
+{
+	const int dev_id = priv->dev->device;
+
+	return ((dev_id == PCI_DEVICE_ID_EXAR_XR17V4358) ||
+	        (dev_id == PCI_DEVICE_ID_EXAR_XR17V8358));
+}
+
 static int
 pci_xr17v35x_setup(struct serial_private *priv,
 		  const struct pciserial_board *board,
@@ -1846,6 +1858,13 @@ pci_xr17v35x_setup(struct serial_private *priv,
 	port->port.flags |= UPF_EXAR_EFR;
 
 	/*
+	 * Setup the uart clock for the devices on expansion slot to
+	 * half the clock speed of the main chip (which is 125MHz)
+	 */
+	if (xr17v35x_has_slave(priv) && idx >= 8)
+		port->port.uartclk = (7812500 * 16 / 2);
+
+	/*
 	 * Setup Multipurpose Input/Output pins.
 	 */
 	if (idx == 0) {
@@ -1998,8 +2017,6 @@ pci_wch_ch38x_setup(struct serial_private *priv,
 #define PCIE_DEVICE_ID_WCH_CH382_2S1P	0x3250
 #define PCIE_DEVICE_ID_WCH_CH384_4S	0x3470
 
-#define PCI_DEVICE_ID_EXAR_XR17V8358	0x8358
-
 /* Unknown vendors/cards - this should not be in linux/pci_ids.h */
 #define PCI_SUBDEVICE_ID_UNKNOWN_0x1584	0x1584
 #define PCI_SUBDEVICE_ID_UNKNOWN_0x1588	0x1588
@@ -2524,6 +2541,13 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 	},
 	{
 		.vendor = PCI_VENDOR_ID_EXAR,
+		.device = PCI_DEVICE_ID_EXAR_XR17V4358,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.setup		= pci_xr17v35x_setup,
+	},
+	{
+		.vendor = PCI_VENDOR_ID_EXAR,
 		.device = PCI_DEVICE_ID_EXAR_XR17V8358,
 		.subvendor	= PCI_ANY_ID,
 		.subdevice	= PCI_ANY_ID,
@@ -3008,6 +3032,7 @@ enum pci_board_num_t {
 	pbn_exar_XR17V352,
 	pbn_exar_XR17V354,
 	pbn_exar_XR17V358,
+	pbn_exar_XR17V4358,
 	pbn_exar_XR17V8358,
 	pbn_exar_ibm_saturn,
 	pbn_pasemi_1682M,
@@ -3695,6 +3720,14 @@ static struct pciserial_board pci_boards[] = {
 		.reg_shift	= 0,
 		.first_offset	= 0,
 	},
+	[pbn_exar_XR17V4358] = {
+		.flags		= FL_BASE0,
+		.num_ports	= 12,
+		.base_baud	= 7812500,
+		.uart_offset	= 0x400,
+		.reg_shift	= 0,
+		.first_offset	= 0,
+	},
 	[pbn_exar_XR17V8358] = {
 		.flags		= FL_BASE0,
 		.num_ports	= 16,
@@ -5112,6 +5145,10 @@ static struct pci_device_id serial_pci_tbl[] = {
 		PCI_ANY_ID, PCI_ANY_ID,
 		0,
 		0, pbn_exar_XR17V358 },
+	{	PCI_VENDOR_ID_EXAR, PCI_DEVICE_ID_EXAR_XR17V4358,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0,
+		0, pbn_exar_XR17V4358 },
 	{	PCI_VENDOR_ID_EXAR, PCI_DEVICE_ID_EXAR_XR17V8358,
 		PCI_ANY_ID, PCI_ANY_ID,
 		0,
diff --git a/drivers/tty/serial/8250/8250_uniphier.c b/drivers/tty/serial/8250/8250_uniphier.c
new file mode 100644
index 000000000000..7d79425c2b09
--- /dev/null
+++ b/drivers/tty/serial/8250/8250_uniphier.c
@@ -0,0 +1,257 @@
+/*
+ * Copyright (C) 2015 Masahiro Yamada <yamada.masahiro@socionext.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/clk.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+
+#include "8250.h"
+
+/* Most (but not all) of UniPhier UART devices have 64-depth FIFO. */
+#define UNIPHIER_UART_DEFAULT_FIFO_SIZE	64
+
+#define UNIPHIER_UART_CHAR_FCR	3	/* Character / FIFO Control Register */
+#define UNIPHIER_UART_LCR_MCR	4	/* Line/Modem Control Register */
+#define   UNIPHIER_UART_LCR_SHIFT	8
+#define UNIPHIER_UART_DLR	9	/* Divisor Latch Register */
+
+struct uniphier8250_priv {
+	int line;
+	struct clk *clk;
+	spinlock_t atomic_write_lock;
+};
+
+/*
+ * The register map is slightly different from that of 8250.
+ * IO callbacks must be overridden for correct access to FCR, LCR, and MCR.
+ */
+static unsigned int uniphier_serial_in(struct uart_port *p, int offset)
+{
+	unsigned int valshift = 0;
+
+	switch (offset) {
+	case UART_LCR:
+		valshift = UNIPHIER_UART_LCR_SHIFT;
+		/* fall through */
+	case UART_MCR:
+		offset = UNIPHIER_UART_LCR_MCR;
+		break;
+	default:
+		break;
+	}
+
+	offset <<= p->regshift;
+
+	/*
+	 * The return value must be masked with 0xff because LCR and MCR reside
+	 * in the same register that must be accessed by 32-bit write/read.
+	 * 8 or 16 bit access to this hardware result in unexpected behavior.
+	 */
+	return (readl(p->membase + offset) >> valshift) & 0xff;
+}
+
+static void uniphier_serial_out(struct uart_port *p, int offset, int value)
+{
+	unsigned int valshift = 0;
+	bool normal = false;
+
+	switch (offset) {
+	case UART_FCR:
+		offset = UNIPHIER_UART_CHAR_FCR;
+		break;
+	case UART_LCR:
+		valshift = UNIPHIER_UART_LCR_SHIFT;
+		/* Divisor latch access bit does not exist. */
+		value &= ~(UART_LCR_DLAB << valshift);
+		/* fall through */
+	case UART_MCR:
+		offset = UNIPHIER_UART_LCR_MCR;
+		break;
+	default:
+		normal = true;
+		break;
+	}
+
+	offset <<= p->regshift;
+
+	if (normal) {
+		writel(value, p->membase + offset);
+	} else {
+		/*
+		 * Special case: two registers share the same address that
+		 * must be 32-bit accessed.  As this is not longer atomic safe,
+		 * take a lock just in case.
+		 */
+		struct uniphier8250_priv *priv = p->private_data;
+		unsigned long flags;
+		u32 tmp;
+
+		spin_lock_irqsave(&priv->atomic_write_lock, flags);
+		tmp = readl(p->membase + offset);
+		tmp &= ~(0xff << valshift);
+		tmp |= value << valshift;
+		writel(tmp, p->membase + offset);
+		spin_unlock_irqrestore(&priv->atomic_write_lock, flags);
+	}
+}
+
+/*
+ * This hardware does not have the divisor latch access bit.
+ * The divisor latch register exists at different address.
+ * Override dl_read/write callbacks.
+ */
+static int uniphier_serial_dl_read(struct uart_8250_port *up)
+{
+	return readl(up->port.membase + UNIPHIER_UART_DLR);
+}
+
+static void uniphier_serial_dl_write(struct uart_8250_port *up, int value)
+{
+	writel(value, up->port.membase + UNIPHIER_UART_DLR);
+}
+
+static int uniphier_of_serial_setup(struct device *dev, struct uart_port *port,
+				    struct uniphier8250_priv *priv)
+{
+	int ret;
+	u32 prop;
+	struct device_node *np = dev->of_node;
+
+	ret = of_alias_get_id(np, "serial");
+	if (ret < 0) {
+		dev_err(dev, "failed to get alias id\n");
+		return ret;
+	}
+	port->line = priv->line = ret;
+
+	/* Get clk rate through clk driver */
+	priv->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(priv->clk)) {
+		dev_err(dev, "failed to get clock\n");
+		return PTR_ERR(priv->clk);
+	}
+
+	ret = clk_prepare_enable(priv->clk);
+	if (ret < 0)
+		return ret;
+
+	port->uartclk = clk_get_rate(priv->clk);
+
+	/* Check for fifo size */
+	if (of_property_read_u32(np, "fifo-size", &prop) == 0)
+		port->fifosize = prop;
+	else
+		port->fifosize = UNIPHIER_UART_DEFAULT_FIFO_SIZE;
+
+	return 0;
+}
+
+static int uniphier_uart_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct uart_8250_port up;
+	struct uniphier8250_priv *priv;
+	struct resource *regs;
+	void __iomem *membase;
+	int irq;
+	int ret;
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!regs) {
+		dev_err(dev, "failed to get memory resource");
+		return -EINVAL;
+	}
+
+	membase = devm_ioremap(dev, regs->start, resource_size(regs));
+	if (!membase)
+		return -ENOMEM;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(dev, "failed to get IRQ number");
+		return irq;
+	}
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	memset(&up, 0, sizeof(up));
+
+	ret = uniphier_of_serial_setup(dev, &up.port, priv);
+	if (ret < 0)
+		return ret;
+
+	spin_lock_init(&priv->atomic_write_lock);
+
+	up.port.dev = dev;
+	up.port.private_data = priv;
+	up.port.mapbase = regs->start;
+	up.port.mapsize = resource_size(regs);
+	up.port.membase = membase;
+	up.port.irq = irq;
+
+	up.port.type = PORT_16550A;
+	up.port.iotype = UPIO_MEM32;
+	up.port.regshift = 2;
+	up.port.flags = UPF_FIXED_PORT | UPF_FIXED_TYPE;
+	up.capabilities = UART_CAP_FIFO;
+
+	up.port.serial_in = uniphier_serial_in;
+	up.port.serial_out = uniphier_serial_out;
+	up.dl_read = uniphier_serial_dl_read;
+	up.dl_write = uniphier_serial_dl_write;
+
+	ret = serial8250_register_8250_port(&up);
+	if (ret < 0) {
+		dev_err(dev, "failed to register 8250 port\n");
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, priv);
+
+	return 0;
+}
+
+static int uniphier_uart_remove(struct platform_device *pdev)
+{
+	struct uniphier8250_priv *priv = platform_get_drvdata(pdev);
+
+	serial8250_unregister_port(priv->line);
+	clk_disable_unprepare(priv->clk);
+
+	return 0;
+}
+
+static const struct of_device_id uniphier_uart_match[] = {
+	{ .compatible = "socionext,uniphier-uart" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, uniphier_uart_match);
+
+static struct platform_driver uniphier_uart_platform_driver = {
+	.probe		= uniphier_uart_probe,
+	.remove		= uniphier_uart_remove,
+	.driver = {
+		.name	= "uniphier-uart",
+		.of_match_table = uniphier_uart_match,
+	},
+};
+module_platform_driver(uniphier_uart_platform_driver);
+
+MODULE_AUTHOR("Masahiro Yamada <yamada.masahiro@socionext.com>");
+MODULE_DESCRIPTION("UniPhier UART driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index c35070356528..e1de1181b322 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -336,9 +336,33 @@ config SERIAL_8250_FINTEK
 	  LPC to 4 UART. This device has some RS485 functionality not available
 	  through the PNP driver. If unsure, say N.
 
+config SERIAL_8250_LPC18XX
+	bool "NXP LPC18xx/43xx serial port support"
+	depends on SERIAL_8250 && OF && (ARCH_LPC18XX || COMPILE_TEST)
+	default ARCH_LPC18XX
+	help
+	  If you have a LPC18xx/43xx based board and want to use the
+	  serial port, say Y to this option. If unsure, say Y.
+
 config SERIAL_8250_MT6577
 	bool "Mediatek serial port support"
 	depends on SERIAL_8250 && ARCH_MEDIATEK
 	help
 	  If you have a Mediatek based board and want to use the
 	  serial port, say Y to this option. If unsure, say N.
+
+config SERIAL_8250_UNIPHIER
+	tristate "Support for UniPhier on-chip UART"
+	depends on SERIAL_8250 && ARCH_UNIPHIER
+	help
+	  If you have a UniPhier based board and want to use the on-chip
+	  serial ports, say Y to this option. If unsure, say N.
+
+config SERIAL_8250_INGENIC
+	bool "Support for Ingenic SoC serial ports"
+	depends on SERIAL_8250_CONSOLE && OF_FLATTREE
+	select LIBFDT
+	select SERIAL_EARLYCON
+	help
+	  If you have a system using an Ingenic SoC and wish to make use of
+	  its UARTs, say Y to this option. If unsure, say N.
diff --git a/drivers/tty/serial/8250/Makefile b/drivers/tty/serial/8250/Makefile
index 31e7cdc6865c..706295913c34 100644
--- a/drivers/tty/serial/8250/Makefile
+++ b/drivers/tty/serial/8250/Makefile
@@ -22,4 +22,9 @@ 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_LPC18XX)	+= 8250_lpc18xx.o
 obj-$(CONFIG_SERIAL_8250_MT6577)	+= 8250_mtk.o
+obj-$(CONFIG_SERIAL_8250_UNIPHIER)	+= 8250_uniphier.o
+obj-$(CONFIG_SERIAL_8250_INGENIC)	+= 8250_ingenic.o
+
+CFLAGS_8250_ingenic.o += -I$(srctree)/scripts/dtc/libfdt
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index f8120c1bde14..76e65b714471 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -241,7 +241,6 @@ config SERIAL_SAMSUNG
 	tristate "Samsung SoC serial support"
 	depends on PLAT_SAMSUNG || ARCH_EXYNOS
 	select SERIAL_CORE
-	select SERIAL_EARLYCON
 	help
 	  Support for the on-chip UARTs on the Samsung S3C24XX series CPUs,
 	  providing /dev/ttySAC0, 1 and 2 (note, some machines may not
@@ -277,6 +276,7 @@ config SERIAL_SAMSUNG_CONSOLE
 	bool "Support for console on Samsung SoC serial port"
 	depends on SERIAL_SAMSUNG=y
 	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
 	help
 	  Allow selection of the S3C24XX on-board serial ports for use as
 	  an virtual console.
@@ -728,7 +728,7 @@ config SERIAL_IP22_ZILOG_CONSOLE
 
 config SERIAL_SH_SCI
 	tristate "SuperH SCI(F) serial port support"
-	depends on SUPERH || ARCH_SHMOBILE || COMPILE_TEST
+	depends on SUPERH || ARCH_SHMOBILE || H8300 || COMPILE_TEST
 	select SERIAL_CORE
 
 config SERIAL_SH_SCI_NR_UARTS
@@ -1179,15 +1179,42 @@ config SERIAL_SCCNXP_CONSOLE
 	help
 	  Support for console on SCCNXP serial ports.
 
+config SERIAL_SC16IS7XX_CORE
+        tristate
+
 config SERIAL_SC16IS7XX
-	tristate "SC16IS7xx serial support"
-	depends on I2C
-	select SERIAL_CORE
-	select REGMAP_I2C if I2C
-	help
-	  This selects support for SC16IS7xx serial ports.
-	  Supported ICs are SC16IS740, SC16IS741, SC16IS750, SC16IS752,
-	  SC16IS760 and SC16IS762.
+        tristate "SC16IS7xx serial support"
+        select SERIAL_CORE
+        depends on I2C || SPI_MASTER
+        help
+          This selects support for SC16IS7xx serial ports.
+          Supported ICs are SC16IS740, SC16IS741, SC16IS750, SC16IS752,
+          SC16IS760 and SC16IS762. Select supported buses using options below.
+
+config SERIAL_SC16IS7XX_I2C
+        bool "SC16IS7xx for I2C interface"
+        depends on SERIAL_SC16IS7XX
+        depends on I2C
+        select SERIAL_SC16IS7XX_CORE if SERIAL_SC16IS7XX
+        select REGMAP_I2C if I2C
+        default y
+        help
+          Enable SC16IS7xx driver on I2C bus,
+          If required say y, and say n to i2c if not required,
+          Enabled by default to support oldconfig.
+          You must select at least one bus for the driver to be built.
+
+config SERIAL_SC16IS7XX_SPI
+        bool "SC16IS7xx for spi interface"
+        depends on SERIAL_SC16IS7XX
+        depends on SPI_MASTER
+        select SERIAL_SC16IS7XX_CORE if SERIAL_SC16IS7XX
+        select REGMAP_SPI if SPI_MASTER
+        help
+          Enable SC16IS7xx driver on SPI bus,
+          If required say y, and say n to spi if not required,
+          This is additional support to exsisting driver.
+          You must select at least one bus for the driver to be built.
 
 config SERIAL_BFIN_SPORT
 	tristate "Blackfin SPORT emulate UART"
@@ -1349,7 +1376,7 @@ config SERIAL_ALTERA_UART_CONSOLE
 
 config SERIAL_IFX6X60
         tristate "SPI protocol driver for Infineon 6x60 modem (EXPERIMENTAL)"
-	depends on GPIOLIB && SPI
+	depends on GPIOLIB && SPI && HAS_DMA
 	help
 	  Support for the IFX6x60 modem devices on Intel MID platforms.
 
@@ -1378,14 +1405,6 @@ config SERIAL_PCH_UART_CONSOLE
 	  (the system  console is the device which receives all kernel messages and
 	  warnings and which allows logins in single user mode).
 
-config SERIAL_MSM_SMD
-	bool "Enable tty device interface for some SMD ports"
-	default n
-	depends on MSM_SMD
-	help
-	  Enables userspace clients to read and write to some streaming SMD
-	  ports via tty device interface for MSM chipset.
-
 config SERIAL_MXS_AUART
 	depends on ARCH_MXS
 	tristate "MXS AUART support"
@@ -1589,6 +1608,23 @@ config SERIAL_SPRD_CONSOLE
 	  with "earlycon" on the kernel command line. The console is
 	  enabled when early_param is processed.
 
+config SERIAL_STM32
+	tristate "STMicroelectronics STM32 serial port support"
+	select SERIAL_CORE
+	depends on ARM || COMPILE_TEST
+	help
+	  This driver is for the on-chip Serial Controller on
+	  STMicroelectronics STM32 MCUs.
+	  USART supports Rx & Tx functionality.
+	  It support all industry standard baud rates.
+
+	  If unsure, say N.
+
+config SERIAL_STM32_CONSOLE
+	bool "Support for console on STM32"
+	depends on SERIAL_STM32=y
+	select SERIAL_CORE_CONSOLE
+
 endmenu
 
 config SERIAL_MCTRL_GPIO
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index c3ac3d930b33..5ab41119b3dc 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -53,7 +53,7 @@ obj-$(CONFIG_SERIAL_SB1250_DUART) += sb1250-duart.o
 obj-$(CONFIG_ETRAX_SERIAL) += crisv10.o
 obj-$(CONFIG_SERIAL_ETRAXFS) += etraxfs-uart.o
 obj-$(CONFIG_SERIAL_SCCNXP) += sccnxp.o
-obj-$(CONFIG_SERIAL_SC16IS7XX) += sc16is7xx.o
+obj-$(CONFIG_SERIAL_SC16IS7XX_CORE) += sc16is7xx.o
 obj-$(CONFIG_SERIAL_JSM) += jsm/
 obj-$(CONFIG_SERIAL_TXX9) += serial_txx9.o
 obj-$(CONFIG_SERIAL_VR41XX) += vr41xx_siu.o
@@ -79,7 +79,6 @@ obj-$(CONFIG_SERIAL_ALTERA_JTAGUART) += altera_jtaguart.o
 obj-$(CONFIG_SERIAL_VT8500) += vt8500_serial.o
 obj-$(CONFIG_SERIAL_IFX6X60)  	+= ifx6x60.o
 obj-$(CONFIG_SERIAL_PCH_UART)	+= pch_uart.o
-obj-$(CONFIG_SERIAL_MSM_SMD)	+= msm_smd_tty.o
 obj-$(CONFIG_SERIAL_MXS_AUART) += mxs-auart.o
 obj-$(CONFIG_SERIAL_LANTIQ)	+= lantiq.o
 obj-$(CONFIG_SERIAL_XILINX_PS_UART) += xilinx_uartps.o
@@ -93,6 +92,7 @@ obj-$(CONFIG_SERIAL_FSL_LPUART)	+= fsl_lpuart.o
 obj-$(CONFIG_SERIAL_CONEXANT_DIGICOLOR)	+= digicolor-usart.o
 obj-$(CONFIG_SERIAL_MEN_Z135)	+= men_z135_uart.o
 obj-$(CONFIG_SERIAL_SPRD) += sprd_serial.o
+obj-$(CONFIG_SERIAL_STM32)	+= stm32-usart.o
 
 # GPIOLIB helpers for modem control lines
 obj-$(CONFIG_SERIAL_MCTRL_GPIO)	+= serial_mctrl_gpio.o
diff --git a/drivers/tty/serial/altera_jtaguart.c b/drivers/tty/serial/altera_jtaguart.c
index 0fefdd8931a2..32df2a0cb060 100644
--- a/drivers/tty/serial/altera_jtaguart.c
+++ b/drivers/tty/serial/altera_jtaguart.c
@@ -387,7 +387,7 @@ console_initcall(altera_jtaguart_console_init);
 
 #define	ALTERA_JTAGUART_CONSOLE	NULL
 
-#endif /* CONFIG_ALTERA_JTAGUART_CONSOLE */
+#endif /* CONFIG_SERIAL_ALTERA_JTAGUART_CONSOLE */
 
 static struct uart_driver altera_jtaguart_driver = {
 	.owner		= THIS_MODULE,
diff --git a/drivers/tty/serial/altera_uart.c b/drivers/tty/serial/altera_uart.c
index b2859fe07e14..fd87a6f574e3 100644
--- a/drivers/tty/serial/altera_uart.c
+++ b/drivers/tty/serial/altera_uart.c
@@ -493,7 +493,7 @@ console_initcall(altera_uart_console_init);
 
 #define	ALTERA_UART_CONSOLE	NULL
 
-#endif /* CONFIG_ALTERA_UART_CONSOLE */
+#endif /* CONFIG_SERIAL_ALTERA_UART_CONSOLE */
 
 /*
  *	Define the altera_uart UART driver structure.
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index 6f5a0720a8c8..50cf5b10ceed 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -58,7 +58,7 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/sizes.h>
 #include <linux/io.h>
-#include <linux/workqueue.h>
+#include <linux/acpi.h>
 
 #define UART_NR			14
 
@@ -79,6 +79,8 @@ struct vendor_data {
 	bool			oversampling;
 	bool			dma_threshold;
 	bool			cts_event_workaround;
+	bool			always_enabled;
+	bool			fixed_options;
 
 	unsigned int (*get_fifosize)(struct amba_device *dev);
 };
@@ -95,9 +97,19 @@ static struct vendor_data vendor_arm = {
 	.oversampling		= false,
 	.dma_threshold		= false,
 	.cts_event_workaround	= false,
+	.always_enabled		= false,
+	.fixed_options		= false,
 	.get_fifosize		= get_fifosize_arm,
 };
 
+static struct vendor_data vendor_sbsa = {
+	.oversampling		= false,
+	.dma_threshold		= false,
+	.cts_event_workaround	= false,
+	.always_enabled		= true,
+	.fixed_options		= true,
+};
+
 static unsigned int get_fifosize_st(struct amba_device *dev)
 {
 	return 64;
@@ -110,6 +122,8 @@ static struct vendor_data vendor_st = {
 	.oversampling		= true,
 	.dma_threshold		= true,
 	.cts_event_workaround	= true,
+	.always_enabled		= false,
+	.fixed_options		= false,
 	.get_fifosize		= get_fifosize_st,
 };
 
@@ -157,9 +171,8 @@ struct uart_amba_port {
 	unsigned int		lcrh_tx;	/* vendor-specific */
 	unsigned int		lcrh_rx;	/* vendor-specific */
 	unsigned int		old_cr;		/* state during shutdown */
-	struct delayed_work	tx_softirq_work;
 	bool			autorts;
-	unsigned int		tx_irq_seen;	/* 0=none, 1=1, 2=2 or more */
+	unsigned int		fixed_baud;	/* vendor-set fixed baud rate */
 	char			type[12];
 #ifdef CONFIG_DMA_ENGINE
 	/* DMA stuff */
@@ -1172,15 +1185,14 @@ static void pl011_stop_tx(struct uart_port *port)
 	pl011_dma_tx_stop(uap);
 }
 
-static bool pl011_tx_chars(struct uart_amba_port *uap);
+static void pl011_tx_chars(struct uart_amba_port *uap, bool from_irq);
 
 /* Start TX with programmed I/O only (no DMA) */
 static void pl011_start_tx_pio(struct uart_amba_port *uap)
 {
 	uap->im |= UART011_TXIM;
 	writew(uap->im, uap->port.membase + UART011_IMSC);
-	if (!uap->tx_irq_seen)
-		pl011_tx_chars(uap);
+	pl011_tx_chars(uap, false);
 }
 
 static void pl011_start_tx(struct uart_port *port)
@@ -1247,87 +1259,54 @@ __acquires(&uap->port.lock)
 	spin_lock(&uap->port.lock);
 }
 
-/*
- * Transmit a character
- * There must be at least one free entry in the TX FIFO to accept the char.
- *
- * Returns true if the FIFO might have space in it afterwards;
- * returns false if the FIFO definitely became full.
- */
-static bool pl011_tx_char(struct uart_amba_port *uap, unsigned char c)
+static bool pl011_tx_char(struct uart_amba_port *uap, unsigned char c,
+			  bool from_irq)
 {
+	if (unlikely(!from_irq) &&
+	    readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF)
+		return false; /* unable to transmit character */
+
 	writew(c, uap->port.membase + UART01x_DR);
 	uap->port.icount.tx++;
 
-	if (likely(uap->tx_irq_seen > 1))
-		return true;
-
-	return !(readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF);
+	return true;
 }
 
-static bool pl011_tx_chars(struct uart_amba_port *uap)
+static void pl011_tx_chars(struct uart_amba_port *uap, bool from_irq)
 {
 	struct circ_buf *xmit = &uap->port.state->xmit;
-	int count;
-
-	if (unlikely(uap->tx_irq_seen < 2))
-		/*
-		 * Initial FIFO fill level unknown: we must check TXFF
-		 * after each write, so just try to fill up the FIFO.
-		 */
-		count = uap->fifosize;
-	else /* tx_irq_seen >= 2 */
-		/*
-		 * FIFO initially at least half-empty, so we can simply
-		 * write half the FIFO without polling TXFF.
-
-		 * Note: the *first* TX IRQ can still race with
-		 * pl011_start_tx_pio(), which can result in the FIFO
-		 * being fuller than expected in that case.
-		 */
-		count = uap->fifosize >> 1;
-
-	/*
-	 * If the FIFO is full we're guaranteed a TX IRQ at some later point,
-	 * and can't transmit immediately in any case:
-	 */
-	if (unlikely(uap->tx_irq_seen < 2 &&
-		     readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF))
-		return false;
+	int count = uap->fifosize >> 1;
 
 	if (uap->port.x_char) {
-		pl011_tx_char(uap, uap->port.x_char);
+		if (!pl011_tx_char(uap, uap->port.x_char, from_irq))
+			return;
 		uap->port.x_char = 0;
 		--count;
 	}
 	if (uart_circ_empty(xmit) || uart_tx_stopped(&uap->port)) {
 		pl011_stop_tx(&uap->port);
-		goto done;
+		return;
 	}
 
 	/* If we are using DMA mode, try to send some characters. */
 	if (pl011_dma_tx_irq(uap))
-		goto done;
+		return;
 
-	while (count-- > 0 && pl011_tx_char(uap, xmit->buf[xmit->tail])) {
-		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
-		if (uart_circ_empty(xmit))
+	do {
+		if (likely(from_irq) && count-- == 0)
+			break;
+
+		if (!pl011_tx_char(uap, xmit->buf[xmit->tail], from_irq))
 			break;
-	}
+
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+	} while (!uart_circ_empty(xmit));
 
 	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(&uap->port);
 
-	if (uart_circ_empty(xmit)) {
+	if (uart_circ_empty(xmit))
 		pl011_stop_tx(&uap->port);
-		goto done;
-	}
-
-	if (unlikely(!uap->tx_irq_seen))
-		schedule_delayed_work(&uap->tx_softirq_work, uap->port.timeout);
-
-done:
-	return false;
 }
 
 static void pl011_modem_status(struct uart_amba_port *uap)
@@ -1354,26 +1333,23 @@ static void pl011_modem_status(struct uart_amba_port *uap)
 	wake_up_interruptible(&uap->port.state->port.delta_msr_wait);
 }
 
-static void pl011_tx_softirq(struct work_struct *work)
+static void check_apply_cts_event_workaround(struct uart_amba_port *uap)
 {
-	struct delayed_work *dwork = to_delayed_work(work);
-	struct uart_amba_port *uap =
-		container_of(dwork, struct uart_amba_port, tx_softirq_work);
-
-	spin_lock(&uap->port.lock);
-	while (pl011_tx_chars(uap)) ;
-	spin_unlock(&uap->port.lock);
-}
+	unsigned int dummy_read;
 
-static void pl011_tx_irq_seen(struct uart_amba_port *uap)
-{
-	if (likely(uap->tx_irq_seen > 1))
+	if (!uap->vendor->cts_event_workaround)
 		return;
 
-	uap->tx_irq_seen++;
-	if (uap->tx_irq_seen < 2)
-		/* first TX IRQ */
-		cancel_delayed_work(&uap->tx_softirq_work);
+	/* workaround to make sure that all bits are unlocked.. */
+	writew(0x00, uap->port.membase + UART011_ICR);
+
+	/*
+	 * WA: introduce 26ns(1 uart clk) delay before W1C;
+	 * single apb access will incur 2 pclk(133.12Mhz) delay,
+	 * so add 2 dummy reads
+	 */
+	dummy_read = readw(uap->port.membase + UART011_ICR);
+	dummy_read = readw(uap->port.membase + UART011_ICR);
 }
 
 static irqreturn_t pl011_int(int irq, void *dev_id)
@@ -1381,25 +1357,15 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 	struct uart_amba_port *uap = dev_id;
 	unsigned long flags;
 	unsigned int status, pass_counter = AMBA_ISR_PASS_LIMIT;
+	u16 imsc;
 	int handled = 0;
-	unsigned int dummy_read;
 
 	spin_lock_irqsave(&uap->port.lock, flags);
-	status = readw(uap->port.membase + UART011_MIS);
+	imsc = readw(uap->port.membase + UART011_IMSC);
+	status = readw(uap->port.membase + UART011_RIS) & imsc;
 	if (status) {
 		do {
-			if (uap->vendor->cts_event_workaround) {
-				/* workaround to make sure that all bits are unlocked.. */
-				writew(0x00, uap->port.membase + UART011_ICR);
-
-				/*
-				 * WA: introduce 26ns(1 uart clk) delay before W1C;
-				 * single apb access will incur 2 pclk(133.12Mhz) delay,
-				 * so add 2 dummy reads
-				 */
-				dummy_read = readw(uap->port.membase + UART011_ICR);
-				dummy_read = readw(uap->port.membase + UART011_ICR);
-			}
+			check_apply_cts_event_workaround(uap);
 
 			writew(status & ~(UART011_TXIS|UART011_RTIS|
 					  UART011_RXIS),
@@ -1414,15 +1380,13 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 			if (status & (UART011_DSRMIS|UART011_DCDMIS|
 				      UART011_CTSMIS|UART011_RIMIS))
 				pl011_modem_status(uap);
-			if (status & UART011_TXIS) {
-				pl011_tx_irq_seen(uap);
-				pl011_tx_chars(uap);
-			}
+			if (status & UART011_TXIS)
+				pl011_tx_chars(uap, true);
 
 			if (pass_counter-- == 0)
 				break;
 
-			status = readw(uap->port.membase + UART011_MIS);
+			status = readw(uap->port.membase + UART011_RIS) & imsc;
 		} while (status != 0);
 		handled = 1;
 	}
@@ -1617,6 +1581,32 @@ static void pl011_write_lcr_h(struct uart_amba_port *uap, unsigned int lcr_h)
 	}
 }
 
+static int pl011_allocate_irq(struct uart_amba_port *uap)
+{
+	writew(uap->im, uap->port.membase + UART011_IMSC);
+
+	return request_irq(uap->port.irq, pl011_int, 0, "uart-pl011", uap);
+}
+
+/*
+ * Enable interrupts, only timeouts when using DMA
+ * if initial RX DMA job failed, start in interrupt mode
+ * as well.
+ */
+static void pl011_enable_interrupts(struct uart_amba_port *uap)
+{
+	spin_lock_irq(&uap->port.lock);
+
+	/* Clear out any spuriously appearing RX interrupts */
+	writew(UART011_RTIS | UART011_RXIS,
+	       uap->port.membase + UART011_ICR);
+	uap->im = UART011_RTIM;
+	if (!pl011_dma_rx_running(uap))
+		uap->im |= UART011_RXIM;
+	writew(uap->im, uap->port.membase + UART011_IMSC);
+	spin_unlock_irq(&uap->port.lock);
+}
+
 static int pl011_startup(struct uart_port *port)
 {
 	struct uart_amba_port *uap =
@@ -1628,20 +1618,12 @@ static int pl011_startup(struct uart_port *port)
 	if (retval)
 		goto clk_dis;
 
-	writew(uap->im, uap->port.membase + UART011_IMSC);
-
-	/*
-	 * Allocate the IRQ
-	 */
-	retval = request_irq(uap->port.irq, pl011_int, 0, "uart-pl011", uap);
+	retval = pl011_allocate_irq(uap);
 	if (retval)
 		goto clk_dis;
 
 	writew(uap->vendor->ifls, uap->port.membase + UART011_IFLS);
 
-	/* Assume that TX IRQ doesn't work until we see one: */
-	uap->tx_irq_seen = 0;
-
 	spin_lock_irq(&uap->port.lock);
 
 	/* restore RTS and DTR */
@@ -1659,20 +1641,7 @@ static int pl011_startup(struct uart_port *port)
 	/* Startup DMA */
 	pl011_dma_startup(uap);
 
-	/*
-	 * Finally, enable interrupts, only timeouts when using DMA
-	 * if initial RX DMA job failed, start in interrupt mode
-	 * as well.
-	 */
-	spin_lock_irq(&uap->port.lock);
-	/* Clear out any spuriously appearing RX interrupts */
-	 writew(UART011_RTIS | UART011_RXIS,
-		uap->port.membase + UART011_ICR);
-	uap->im = UART011_RTIM;
-	if (!pl011_dma_rx_running(uap))
-		uap->im |= UART011_RXIM;
-	writew(uap->im, uap->port.membase + UART011_IMSC);
-	spin_unlock_irq(&uap->port.lock);
+	pl011_enable_interrupts(uap);
 
 	return 0;
 
@@ -1681,6 +1650,28 @@ static int pl011_startup(struct uart_port *port)
 	return retval;
 }
 
+static int sbsa_uart_startup(struct uart_port *port)
+{
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
+	int retval;
+
+	retval = pl011_hwinit(port);
+	if (retval)
+		return retval;
+
+	retval = pl011_allocate_irq(uap);
+	if (retval)
+		return retval;
+
+	/* The SBSA UART does not support any modem status lines. */
+	uap->old_status = 0;
+
+	pl011_enable_interrupts(uap);
+
+	return 0;
+}
+
 static void pl011_shutdown_channel(struct uart_amba_port *uap,
 					unsigned int lcrh)
 {
@@ -1691,36 +1682,15 @@ static void pl011_shutdown_channel(struct uart_amba_port *uap,
       writew(val, uap->port.membase + lcrh);
 }
 
-static void pl011_shutdown(struct uart_port *port)
+/*
+ * disable the port. It should not disable RTS and DTR.
+ * Also RTS and DTR state should be preserved to restore
+ * it during startup().
+ */
+static void pl011_disable_uart(struct uart_amba_port *uap)
 {
-	struct uart_amba_port *uap =
-	    container_of(port, struct uart_amba_port, port);
 	unsigned int cr;
 
-	cancel_delayed_work_sync(&uap->tx_softirq_work);
-
-	/*
-	 * disable all interrupts
-	 */
-	spin_lock_irq(&uap->port.lock);
-	uap->im = 0;
-	writew(uap->im, uap->port.membase + UART011_IMSC);
-	writew(0xffff, uap->port.membase + UART011_ICR);
-	spin_unlock_irq(&uap->port.lock);
-
-	pl011_dma_shutdown(uap);
-
-	/*
-	 * Free the interrupt
-	 */
-	free_irq(uap->port.irq, uap);
-
-	/*
-	 * disable the port
-	 * disable the port. It should not disable RTS and DTR.
-	 * Also RTS and DTR state should be preserved to restore
-	 * it during startup().
-	 */
 	uap->autorts = false;
 	spin_lock_irq(&uap->port.lock);
 	cr = readw(uap->port.membase + UART011_CR);
@@ -1736,6 +1706,32 @@ static void pl011_shutdown(struct uart_port *port)
 	pl011_shutdown_channel(uap, uap->lcrh_rx);
 	if (uap->lcrh_rx != uap->lcrh_tx)
 		pl011_shutdown_channel(uap, uap->lcrh_tx);
+}
+
+static void pl011_disable_interrupts(struct uart_amba_port *uap)
+{
+	spin_lock_irq(&uap->port.lock);
+
+	/* mask all interrupts and clear all pending ones */
+	uap->im = 0;
+	writew(uap->im, uap->port.membase + UART011_IMSC);
+	writew(0xffff, uap->port.membase + UART011_ICR);
+
+	spin_unlock_irq(&uap->port.lock);
+}
+
+static void pl011_shutdown(struct uart_port *port)
+{
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
+
+	pl011_disable_interrupts(uap);
+
+	pl011_dma_shutdown(uap);
+
+	free_irq(uap->port.irq, uap);
+
+	pl011_disable_uart(uap);
 
 	/*
 	 * Shut down the clock producer
@@ -1756,6 +1752,51 @@ static void pl011_shutdown(struct uart_port *port)
 		uap->port.ops->flush_buffer(port);
 }
 
+static void sbsa_uart_shutdown(struct uart_port *port)
+{
+	struct uart_amba_port *uap =
+		container_of(port, struct uart_amba_port, port);
+
+	pl011_disable_interrupts(uap);
+
+	free_irq(uap->port.irq, uap);
+
+	if (uap->port.ops->flush_buffer)
+		uap->port.ops->flush_buffer(port);
+}
+
+static void
+pl011_setup_status_masks(struct uart_port *port, struct ktermios *termios)
+{
+	port->read_status_mask = UART011_DR_OE | 255;
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |= UART011_DR_FE | UART011_DR_PE;
+	if (termios->c_iflag & (IGNBRK | BRKINT | PARMRK))
+		port->read_status_mask |= UART011_DR_BE;
+
+	/*
+	 * Characters to ignore
+	 */
+	port->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNPAR)
+		port->ignore_status_mask |= UART011_DR_FE | UART011_DR_PE;
+	if (termios->c_iflag & IGNBRK) {
+		port->ignore_status_mask |= UART011_DR_BE;
+		/*
+		 * If we're ignoring parity and break indicators,
+		 * ignore overruns too (for real raw support).
+		 */
+		if (termios->c_iflag & IGNPAR)
+			port->ignore_status_mask |= UART011_DR_OE;
+	}
+
+	/*
+	 * Ignore all characters if CREAD is not set.
+	 */
+	if ((termios->c_cflag & CREAD) == 0)
+		port->ignore_status_mask |= UART_DUMMY_DR_RX;
+}
+
 static void
 pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 		     struct ktermios *old)
@@ -1820,33 +1861,7 @@ pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 	 */
 	uart_update_timeout(port, termios->c_cflag, baud);
 
-	port->read_status_mask = UART011_DR_OE | 255;
-	if (termios->c_iflag & INPCK)
-		port->read_status_mask |= UART011_DR_FE | UART011_DR_PE;
-	if (termios->c_iflag & (IGNBRK | BRKINT | PARMRK))
-		port->read_status_mask |= UART011_DR_BE;
-
-	/*
-	 * Characters to ignore
-	 */
-	port->ignore_status_mask = 0;
-	if (termios->c_iflag & IGNPAR)
-		port->ignore_status_mask |= UART011_DR_FE | UART011_DR_PE;
-	if (termios->c_iflag & IGNBRK) {
-		port->ignore_status_mask |= UART011_DR_BE;
-		/*
-		 * If we're ignoring parity and break indicators,
-		 * ignore overruns too (for real raw support).
-		 */
-		if (termios->c_iflag & IGNPAR)
-			port->ignore_status_mask |= UART011_DR_OE;
-	}
-
-	/*
-	 * Ignore all characters if CREAD is not set.
-	 */
-	if ((termios->c_cflag & CREAD) == 0)
-		port->ignore_status_mask |= UART_DUMMY_DR_RX;
+	pl011_setup_status_masks(port, termios);
 
 	if (UART_ENABLE_MS(port, termios->c_cflag))
 		pl011_enable_ms(port);
@@ -1901,6 +1916,27 @@ pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
+static void
+sbsa_uart_set_termios(struct uart_port *port, struct ktermios *termios,
+		      struct ktermios *old)
+{
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
+	unsigned long flags;
+
+	tty_termios_encode_baud_rate(termios, uap->fixed_baud, uap->fixed_baud);
+
+	/* The SBSA UART only supports 8n1 without hardware flow control. */
+	termios->c_cflag &= ~(CSIZE | CSTOPB | PARENB | PARODD);
+	termios->c_cflag &= ~(CMSPAR | CRTSCTS);
+	termios->c_cflag |= CS8 | CLOCAL;
+
+	spin_lock_irqsave(&port->lock, flags);
+	uart_update_timeout(port, CS8, uap->fixed_baud);
+	pl011_setup_status_masks(port, termios);
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
 static const char *pl011_type(struct uart_port *port)
 {
 	struct uart_amba_port *uap =
@@ -1976,6 +2012,37 @@ static struct uart_ops amba_pl011_pops = {
 #endif
 };
 
+static void sbsa_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+}
+
+static unsigned int sbsa_uart_get_mctrl(struct uart_port *port)
+{
+	return 0;
+}
+
+static const struct uart_ops sbsa_uart_pops = {
+	.tx_empty	= pl011_tx_empty,
+	.set_mctrl	= sbsa_uart_set_mctrl,
+	.get_mctrl	= sbsa_uart_get_mctrl,
+	.stop_tx	= pl011_stop_tx,
+	.start_tx	= pl011_start_tx,
+	.stop_rx	= pl011_stop_rx,
+	.startup	= sbsa_uart_startup,
+	.shutdown	= sbsa_uart_shutdown,
+	.set_termios	= sbsa_uart_set_termios,
+	.type		= pl011_type,
+	.release_port	= pl011_release_port,
+	.request_port	= pl011_request_port,
+	.config_port	= pl011_config_port,
+	.verify_port	= pl011_verify_port,
+#ifdef CONFIG_CONSOLE_POLL
+	.poll_init     = pl011_hwinit,
+	.poll_get_char = pl011_get_poll_char,
+	.poll_put_char = pl011_put_poll_char,
+#endif
+};
+
 static struct uart_amba_port *amba_ports[UART_NR];
 
 #ifdef CONFIG_SERIAL_AMBA_PL011_CONSOLE
@@ -1994,7 +2061,7 @@ static void
 pl011_console_write(struct console *co, const char *s, unsigned int count)
 {
 	struct uart_amba_port *uap = amba_ports[co->index];
-	unsigned int status, old_cr, new_cr;
+	unsigned int status, old_cr = 0, new_cr;
 	unsigned long flags;
 	int locked = 1;
 
@@ -2011,10 +2078,12 @@ pl011_console_write(struct console *co, const char *s, unsigned int count)
 	/*
 	 *	First save the CR then disable the interrupts
 	 */
-	old_cr = readw(uap->port.membase + UART011_CR);
-	new_cr = old_cr & ~UART011_CR_CTSEN;
-	new_cr |= UART01x_CR_UARTEN | UART011_CR_TXE;
-	writew(new_cr, uap->port.membase + UART011_CR);
+	if (!uap->vendor->always_enabled) {
+		old_cr = readw(uap->port.membase + UART011_CR);
+		new_cr = old_cr & ~UART011_CR_CTSEN;
+		new_cr |= UART01x_CR_UARTEN | UART011_CR_TXE;
+		writew(new_cr, uap->port.membase + UART011_CR);
+	}
 
 	uart_console_write(&uap->port, s, count, pl011_console_putchar);
 
@@ -2025,7 +2094,8 @@ pl011_console_write(struct console *co, const char *s, unsigned int count)
 	do {
 		status = readw(uap->port.membase + UART01x_FR);
 	} while (status & UART01x_FR_BUSY);
-	writew(old_cr, uap->port.membase + UART011_CR);
+	if (!uap->vendor->always_enabled)
+		writew(old_cr, uap->port.membase + UART011_CR);
 
 	if (locked)
 		spin_unlock(&uap->port.lock);
@@ -2106,10 +2176,15 @@ static int __init pl011_console_setup(struct console *co, char *options)
 
 	uap->port.uartclk = clk_get_rate(uap->clk);
 
-	if (options)
-		uart_parse_options(options, &baud, &parity, &bits, &flow);
-	else
-		pl011_console_get_options(uap, &baud, &parity, &bits);
+	if (uap->vendor->fixed_options) {
+		baud = uap->fixed_baud;
+	} else {
+		if (options)
+			uart_parse_options(options,
+					   &baud, &parity, &bits, &flow);
+		else
+			pl011_console_get_options(uap, &baud, &parity, &bits);
+	}
 
 	return uart_set_options(&uap->port, co, baud, parity, bits, flow);
 }
@@ -2201,97 +2276,126 @@ static int pl011_probe_dt_alias(int index, struct device *dev)
 	return ret;
 }
 
-static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
+/* unregisters the driver also if no more ports are left */
+static void pl011_unregister_port(struct uart_amba_port *uap)
 {
-	struct uart_amba_port *uap;
-	struct vendor_data *vendor = id->data;
-	void __iomem *base;
-	int i, ret;
+	int i;
+	bool busy = false;
+
+	for (i = 0; i < ARRAY_SIZE(amba_ports); i++) {
+		if (amba_ports[i] == uap)
+			amba_ports[i] = NULL;
+		else if (amba_ports[i])
+			busy = true;
+	}
+	pl011_dma_remove(uap);
+	if (!busy)
+		uart_unregister_driver(&amba_reg);
+}
+
+static int pl011_find_free_port(void)
+{
+	int i;
 
 	for (i = 0; i < ARRAY_SIZE(amba_ports); i++)
 		if (amba_ports[i] == NULL)
-			break;
-
-	if (i == ARRAY_SIZE(amba_ports))
-		return -EBUSY;
+			return i;
 
-	uap = devm_kzalloc(&dev->dev, sizeof(struct uart_amba_port),
-			   GFP_KERNEL);
-	if (uap == NULL)
-		return -ENOMEM;
+	return -EBUSY;
+}
 
-	i = pl011_probe_dt_alias(i, &dev->dev);
+static int pl011_setup_port(struct device *dev, struct uart_amba_port *uap,
+			    struct resource *mmiobase, int index)
+{
+	void __iomem *base;
 
-	base = devm_ioremap(&dev->dev, dev->res.start,
-			    resource_size(&dev->res));
+	base = devm_ioremap_resource(dev, mmiobase);
 	if (!base)
 		return -ENOMEM;
 
-	uap->clk = devm_clk_get(&dev->dev, NULL);
-	if (IS_ERR(uap->clk))
-		return PTR_ERR(uap->clk);
+	index = pl011_probe_dt_alias(index, dev);
 
-	uap->vendor = vendor;
-	uap->lcrh_rx = vendor->lcrh_rx;
-	uap->lcrh_tx = vendor->lcrh_tx;
 	uap->old_cr = 0;
-	uap->fifosize = vendor->get_fifosize(dev);
-	uap->port.dev = &dev->dev;
-	uap->port.mapbase = dev->res.start;
+	uap->port.dev = dev;
+	uap->port.mapbase = mmiobase->start;
 	uap->port.membase = base;
 	uap->port.iotype = UPIO_MEM;
-	uap->port.irq = dev->irq[0];
 	uap->port.fifosize = uap->fifosize;
-	uap->port.ops = &amba_pl011_pops;
 	uap->port.flags = UPF_BOOT_AUTOCONF;
-	uap->port.line = i;
-	INIT_DELAYED_WORK(&uap->tx_softirq_work, pl011_tx_softirq);
+	uap->port.line = index;
 
-	/* Ensure interrupts from this UART are masked and cleared */
-	writew(0, uap->port.membase + UART011_IMSC);
-	writew(0xffff, uap->port.membase + UART011_ICR);
+	amba_ports[index] = uap;
 
-	snprintf(uap->type, sizeof(uap->type), "PL011 rev%u", amba_rev(dev));
+	return 0;
+}
 
-	amba_ports[i] = uap;
+static int pl011_register_port(struct uart_amba_port *uap)
+{
+	int ret;
 
-	amba_set_drvdata(dev, uap);
+	/* Ensure interrupts from this UART are masked and cleared */
+	writew(0, uap->port.membase + UART011_IMSC);
+	writew(0xffff, uap->port.membase + UART011_ICR);
 
 	if (!amba_reg.state) {
 		ret = uart_register_driver(&amba_reg);
 		if (ret < 0) {
-			dev_err(&dev->dev,
+			dev_err(uap->port.dev,
 				"Failed to register AMBA-PL011 driver\n");
 			return ret;
 		}
 	}
 
 	ret = uart_add_one_port(&amba_reg, &uap->port);
-	if (ret) {
-		amba_ports[i] = NULL;
-		uart_unregister_driver(&amba_reg);
-	}
+	if (ret)
+		pl011_unregister_port(uap);
 
 	return ret;
 }
 
+static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
+{
+	struct uart_amba_port *uap;
+	struct vendor_data *vendor = id->data;
+	int portnr, ret;
+
+	portnr = pl011_find_free_port();
+	if (portnr < 0)
+		return portnr;
+
+	uap = devm_kzalloc(&dev->dev, sizeof(struct uart_amba_port),
+			   GFP_KERNEL);
+	if (!uap)
+		return -ENOMEM;
+
+	uap->clk = devm_clk_get(&dev->dev, NULL);
+	if (IS_ERR(uap->clk))
+		return PTR_ERR(uap->clk);
+
+	uap->vendor = vendor;
+	uap->lcrh_rx = vendor->lcrh_rx;
+	uap->lcrh_tx = vendor->lcrh_tx;
+	uap->fifosize = vendor->get_fifosize(dev);
+	uap->port.irq = dev->irq[0];
+	uap->port.ops = &amba_pl011_pops;
+
+	snprintf(uap->type, sizeof(uap->type), "PL011 rev%u", amba_rev(dev));
+
+	ret = pl011_setup_port(&dev->dev, uap, &dev->res, portnr);
+	if (ret)
+		return ret;
+
+	amba_set_drvdata(dev, uap);
+
+	return pl011_register_port(uap);
+}
+
 static int pl011_remove(struct amba_device *dev)
 {
 	struct uart_amba_port *uap = amba_get_drvdata(dev);
-	bool busy = false;
-	int i;
 
 	uart_remove_one_port(&amba_reg, &uap->port);
-
-	for (i = 0; i < ARRAY_SIZE(amba_ports); i++)
-		if (amba_ports[i] == uap)
-			amba_ports[i] = NULL;
-		else if (amba_ports[i])
-			busy = true;
-
-	pl011_dma_remove(uap);
-	if (!busy)
-		uart_unregister_driver(&amba_reg);
+	pl011_unregister_port(uap);
 	return 0;
 }
 
@@ -2319,6 +2423,86 @@ static int pl011_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(pl011_dev_pm_ops, pl011_suspend, pl011_resume);
 
+static int sbsa_uart_probe(struct platform_device *pdev)
+{
+	struct uart_amba_port *uap;
+	struct resource *r;
+	int portnr, ret;
+	int baudrate;
+
+	/*
+	 * Check the mandatory baud rate parameter in the DT node early
+	 * so that we can easily exit with the error.
+	 */
+	if (pdev->dev.of_node) {
+		struct device_node *np = pdev->dev.of_node;
+
+		ret = of_property_read_u32(np, "current-speed", &baudrate);
+		if (ret)
+			return ret;
+	} else {
+		baudrate = 115200;
+	}
+
+	portnr = pl011_find_free_port();
+	if (portnr < 0)
+		return portnr;
+
+	uap = devm_kzalloc(&pdev->dev, sizeof(struct uart_amba_port),
+			   GFP_KERNEL);
+	if (!uap)
+		return -ENOMEM;
+
+	uap->vendor	= &vendor_sbsa;
+	uap->fifosize	= 32;
+	uap->port.irq	= platform_get_irq(pdev, 0);
+	uap->port.ops	= &sbsa_uart_pops;
+	uap->fixed_baud = baudrate;
+
+	snprintf(uap->type, sizeof(uap->type), "SBSA");
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	ret = pl011_setup_port(&pdev->dev, uap, r, portnr);
+	if (ret)
+		return ret;
+
+	platform_set_drvdata(pdev, uap);
+
+	return pl011_register_port(uap);
+}
+
+static int sbsa_uart_remove(struct platform_device *pdev)
+{
+	struct uart_amba_port *uap = platform_get_drvdata(pdev);
+
+	uart_remove_one_port(&amba_reg, &uap->port);
+	pl011_unregister_port(uap);
+	return 0;
+}
+
+static const struct of_device_id sbsa_uart_of_match[] = {
+	{ .compatible = "arm,sbsa-uart", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, sbsa_uart_of_match);
+
+static const struct acpi_device_id sbsa_uart_acpi_match[] = {
+	{ "ARMH0011", 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, sbsa_uart_acpi_match);
+
+static struct platform_driver arm_sbsa_uart_platform_driver = {
+	.probe		= sbsa_uart_probe,
+	.remove		= sbsa_uart_remove,
+	.driver	= {
+		.name	= "sbsa-uart",
+		.of_match_table = of_match_ptr(sbsa_uart_of_match),
+		.acpi_match_table = ACPI_PTR(sbsa_uart_acpi_match),
+	},
+};
+
 static struct amba_id pl011_ids[] = {
 	{
 		.id	= 0x00041011,
@@ -2349,11 +2533,14 @@ static int __init pl011_init(void)
 {
 	printk(KERN_INFO "Serial: AMBA PL011 UART driver\n");
 
+	if (platform_driver_register(&arm_sbsa_uart_platform_driver))
+		pr_warn("could not register SBSA UART platform driver\n");
 	return amba_driver_register(&pl011_driver);
 }
 
 static void __exit pl011_exit(void)
 {
+	platform_driver_unregister(&arm_sbsa_uart_platform_driver);
 	amba_driver_unregister(&pl011_driver);
 }
 
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index 27dade29646b..2a8f528153e7 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -165,6 +165,7 @@ struct atmel_uart_port {
 	struct tasklet_struct	tasklet;
 	unsigned int		irq_status;
 	unsigned int		irq_status_prev;
+	unsigned int		status_change;
 
 	struct circ_buf		rx_ring;
 
@@ -315,8 +316,7 @@ static int atmel_config_rs485(struct uart_port *port,
 	if (rs485conf->flags & SER_RS485_ENABLED) {
 		dev_dbg(port->dev, "Setting UART to RS485\n");
 		atmel_port->tx_done_mask = ATMEL_US_TXEMPTY;
-		if ((rs485conf->delay_rts_after_send) > 0)
-			UART_PUT_TTGR(port, rs485conf->delay_rts_after_send);
+		UART_PUT_TTGR(port, rs485conf->delay_rts_after_send);
 		mode |= ATMEL_US_USMODE_RS485;
 	} else {
 		dev_dbg(port->dev, "Setting UART to RS232\n");
@@ -354,8 +354,7 @@ static void atmel_set_mctrl(struct uart_port *port, u_int mctrl)
 
 	/* override mode to RS485 if needed, otherwise keep the current mode */
 	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);
+		UART_PUT_TTGR(port, port->rs485.delay_rts_after_send);
 		mode &= ~ATMEL_US_USMODE;
 		mode |= ATMEL_US_USMODE_RS485;
 	}
@@ -1177,6 +1176,9 @@ atmel_handle_status(struct uart_port *port, unsigned int pending,
 	if (pending & (ATMEL_US_RIIC | ATMEL_US_DSRIC | ATMEL_US_DCDIC
 				| ATMEL_US_CTSIC)) {
 		atmel_port->irq_status = status;
+		atmel_port->status_change = atmel_port->irq_status ^
+					    atmel_port->irq_status_prev;
+		atmel_port->irq_status_prev = status;
 		tasklet_schedule(&atmel_port->tasklet);
 	}
 }
@@ -1523,17 +1525,14 @@ static void atmel_tasklet_func(unsigned long data)
 {
 	struct uart_port *port = (struct uart_port *)data;
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
-	unsigned int status;
-	unsigned int status_change;
+	unsigned int status = atmel_port->irq_status;
+	unsigned int status_change = atmel_port->status_change;
 
 	/* The interrupt handler does not take the lock */
 	spin_lock(&port->lock);
 
 	atmel_port->schedule_tx(port);
 
-	status = atmel_port->irq_status;
-	status_change = status ^ atmel_port->irq_status_prev;
-
 	if (status_change & (ATMEL_US_RI | ATMEL_US_DSR
 				| ATMEL_US_DCD | ATMEL_US_CTS)) {
 		/* TODO: All reads to CSR will clear these interrupts! */
@@ -1548,7 +1547,7 @@ static void atmel_tasklet_func(unsigned long data)
 
 		wake_up_interruptible(&port->state->port.delta_msr_wait);
 
-		atmel_port->irq_status_prev = status;
+		atmel_port->status_change = 0;
 	}
 
 	atmel_port->schedule_rx(port);
@@ -2061,8 +2060,7 @@ static void atmel_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	/* mode */
 	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);
+		UART_PUT_TTGR(port, port->rs485.delay_rts_after_send);
 		mode |= ATMEL_US_USMODE_RS485;
 	} else if (termios->c_cflag & CRTSCTS) {
 		/* RS232 with hardware handshake (RTS/CTS) */
diff --git a/drivers/tty/serial/bfin_uart.c b/drivers/tty/serial/bfin_uart.c
index 155781ece050..ae3cf94b146b 100644
--- a/drivers/tty/serial/bfin_uart.c
+++ b/drivers/tty/serial/bfin_uart.c
@@ -74,8 +74,8 @@ static void bfin_serial_tx_chars(struct bfin_serial_port *uart);
 
 static void bfin_serial_reset_irda(struct uart_port *port);
 
-#if defined(CONFIG_SERIAL_BFIN_CTSRTS) || \
-	defined(CONFIG_SERIAL_BFIN_HARD_CTSRTS)
+#if defined(SERIAL_BFIN_CTSRTS) || \
+	defined(SERIAL_BFIN_HARD_CTSRTS)
 static unsigned int bfin_serial_get_mctrl(struct uart_port *port)
 {
 	struct bfin_serial_port *uart = (struct bfin_serial_port *)port;
@@ -110,7 +110,7 @@ static irqreturn_t bfin_serial_mctrl_cts_int(int irq, void *dev_id)
 	struct bfin_serial_port *uart = dev_id;
 	struct uart_port *uport = &uart->port;
 	unsigned int status = bfin_serial_get_mctrl(uport);
-#ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
+#ifdef SERIAL_BFIN_HARD_CTSRTS
 
 	UART_CLEAR_SCTS(uart);
 	if (uport->hw_stopped) {
@@ -700,7 +700,7 @@ static int bfin_serial_startup(struct uart_port *port)
 # endif
 #endif
 
-#ifdef CONFIG_SERIAL_BFIN_CTSRTS
+#ifdef SERIAL_BFIN_CTSRTS
 	if (uart->cts_pin >= 0) {
 		if (request_irq(gpio_to_irq(uart->cts_pin),
 			bfin_serial_mctrl_cts_int,
@@ -718,7 +718,7 @@ static int bfin_serial_startup(struct uart_port *port)
 			gpio_direction_output(uart->rts_pin, 0);
 	}
 #endif
-#ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
+#ifdef SERIAL_BFIN_HARD_CTSRTS
 	if (uart->cts_pin >= 0) {
 		if (request_irq(uart->status_irq, bfin_serial_mctrl_cts_int,
 			0, "BFIN_UART_MODEM_STATUS", uart)) {
@@ -766,13 +766,13 @@ static void bfin_serial_shutdown(struct uart_port *port)
 	free_irq(uart->tx_irq, uart);
 #endif
 
-#ifdef CONFIG_SERIAL_BFIN_CTSRTS
+#ifdef SERIAL_BFIN_CTSRTS
 	if (uart->cts_pin >= 0)
 		free_irq(gpio_to_irq(uart->cts_pin), uart);
 	if (uart->rts_pin >= 0)
 		gpio_free(uart->rts_pin);
 #endif
-#ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
+#ifdef SERIAL_BFIN_HARD_CTSRTS
 	if (uart->cts_pin >= 0)
 		free_irq(uart->status_irq, uart);
 #endif
@@ -788,7 +788,7 @@ bfin_serial_set_termios(struct uart_port *port, struct ktermios *termios,
 	unsigned int ier, lcr = 0;
 	unsigned long timeout;
 
-#ifdef CONFIG_SERIAL_BFIN_CTSRTS
+#ifdef SERIAL_BFIN_CTSRTS
 	if (old == NULL && uart->cts_pin != -1)
 		termios->c_cflag |= CRTSCTS;
 	else if (uart->cts_pin == -1)
@@ -1110,8 +1110,8 @@ bfin_serial_console_setup(struct console *co, char *options)
 	int baud = 57600;
 	int bits = 8;
 	int parity = 'n';
-# if defined(CONFIG_SERIAL_BFIN_CTSRTS) || \
-	defined(CONFIG_SERIAL_BFIN_HARD_CTSRTS)
+# if defined(SERIAL_BFIN_CTSRTS) || \
+	defined(SERIAL_BFIN_HARD_CTSRTS)
 	int flow = 'r';
 # else
 	int flow = 'n';
@@ -1322,8 +1322,8 @@ static int bfin_serial_probe(struct platform_device *pdev)
 		init_timer(&(uart->rx_dma_timer));
 #endif
 
-#if defined(CONFIG_SERIAL_BFIN_CTSRTS) || \
-	defined(CONFIG_SERIAL_BFIN_HARD_CTSRTS)
+#if defined(SERIAL_BFIN_CTSRTS) || \
+	defined(SERIAL_BFIN_HARD_CTSRTS)
 		res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 		if (res == NULL)
 			uart->cts_pin = -1;
diff --git a/drivers/tty/serial/crisv10.c b/drivers/tty/serial/crisv10.c
index 0c1825b0b41d..3e4470af5c50 100644
--- a/drivers/tty/serial/crisv10.c
+++ b/drivers/tty/serial/crisv10.c
@@ -56,10 +56,6 @@ static char *serial_version = "$Revision: 1.25 $";
 #error "RX_TIMEOUT_TICKS == 0 not allowed, use 1"
 #endif
 
-#if defined(CONFIG_ETRAX_RS485_ON_PA) && defined(CONFIG_ETRAX_RS485_ON_PORT_G)
-#error "Disable either CONFIG_ETRAX_RS485_ON_PA or CONFIG_ETRAX_RS485_ON_PORT_G"
-#endif
-
 /*
  * All of the compatibilty code so we can compile serial.c against
  * older kernels is hidden in serial_compat.h
@@ -455,30 +451,6 @@ static struct e100_serial rs_table[] = {
 static struct fast_timer fast_timers[NR_PORTS];
 #endif
 
-#ifdef CONFIG_ETRAX_SERIAL_PROC_ENTRY
-#define PROCSTAT(x) x
-struct ser_statistics_type {
-	int overrun_cnt;
-	int early_errors_cnt;
-	int ser_ints_ok_cnt;
-	int errors_cnt;
-	unsigned long int processing_flip;
-	unsigned long processing_flip_still_room;
-	unsigned long int timeout_flush_cnt;
-	int rx_dma_ints;
-	int tx_dma_ints;
-	int rx_tot;
-	int tx_tot;
-};
-
-static struct ser_statistics_type ser_stat[NR_PORTS];
-
-#else
-
-#define PROCSTAT(x)
-
-#endif /* CONFIG_ETRAX_SERIAL_PROC_ENTRY */
-
 /* RS-485 */
 #if defined(CONFIG_ETRAX_RS485)
 #ifdef CONFIG_ETRAX_FAST_TIMER
@@ -487,9 +459,6 @@ static struct fast_timer fast_timers_rs485[NR_PORTS];
 #if defined(CONFIG_ETRAX_RS485_ON_PA)
 static int rs485_pa_bit = CONFIG_ETRAX_RS485_ON_PA_BIT;
 #endif
-#if defined(CONFIG_ETRAX_RS485_ON_PORT_G)
-static int rs485_port_g_bit = CONFIG_ETRAX_RS485_ON_PORT_G_BIT;
-#endif
 #endif
 
 /* Info and macros needed for each ports extra control/status signals. */
@@ -739,10 +708,10 @@ static unsigned char dummy_ser[NR_PORTS] = {0xFF, 0xFF, 0xFF,0xFF};
     defined(CONFIG_ETRAX_SER1_DTR_RI_DSR_CD_MIXED) || \
     defined(CONFIG_ETRAX_SER2_DTR_RI_DSR_CD_MIXED) || \
     defined(CONFIG_ETRAX_SER3_DTR_RI_DSR_CD_MIXED)
-#define CONFIG_ETRAX_SERX_DTR_RI_DSR_CD_MIXED
+#define ETRAX_SERX_DTR_RI_DSR_CD_MIXED
 #endif
 
-#ifdef CONFIG_ETRAX_SERX_DTR_RI_DSR_CD_MIXED
+#ifdef ETRAX_SERX_DTR_RI_DSR_CD_MIXED
 /* The pins can be mixed on PA and PB */
 #define CONTROL_PINS_PORT_NOT_USED(line) \
   &dummy_ser[line], &dummy_ser[line], \
@@ -835,7 +804,7 @@ static const struct control_pins e100_modem_pins[NR_PORTS] =
 #endif
 	}
 };
-#else  /* CONFIG_ETRAX_SERX_DTR_RI_DSR_CD_MIXED */
+#else  /* ETRAX_SERX_DTR_RI_DSR_CD_MIXED */
 
 /* All pins are on either PA or PB for each serial port */
 #define CONTROL_PINS_PORT_NOT_USED(line) \
@@ -917,7 +886,7 @@ static const struct control_pins e100_modem_pins[NR_PORTS] =
 #endif
 	}
 };
-#endif /* !CONFIG_ETRAX_SERX_DTR_RI_DSR_CD_MIXED */
+#endif /* !ETRAX_SERX_DTR_RI_DSR_CD_MIXED */
 
 #define E100_RTS_MASK 0x20
 #define E100_CTS_MASK 0x40
@@ -1367,16 +1336,6 @@ e100_enable_rs485(struct tty_struct *tty, struct serial_rs485 *r)
 #if defined(CONFIG_ETRAX_RS485_ON_PA)
 	*R_PORT_PA_DATA = port_pa_data_shadow |= (1 << rs485_pa_bit);
 #endif
-#if defined(CONFIG_ETRAX_RS485_ON_PORT_G)
-	REG_SHADOW_SET(R_PORT_G_DATA,  port_g_data_shadow,
-		       rs485_port_g_bit, 1);
-#endif
-#if defined(CONFIG_ETRAX_RS485_LTC1387)
-	REG_SHADOW_SET(R_PORT_G_DATA, port_g_data_shadow,
-		       CONFIG_ETRAX_RS485_LTC1387_DXEN_PORT_G_BIT, 1);
-	REG_SHADOW_SET(R_PORT_G_DATA, port_g_data_shadow,
-		       CONFIG_ETRAX_RS485_LTC1387_RXEN_PORT_G_BIT, 1);
-#endif
 
 	info->rs485 = *r;
 
@@ -1676,7 +1635,8 @@ alloc_recv_buffer(unsigned int size)
 {
 	struct etrax_recv_buffer *buffer;
 
-	if (!(buffer = kmalloc(sizeof *buffer + size, GFP_ATOMIC)))
+	buffer = kmalloc(sizeof *buffer + size, GFP_ATOMIC);
+	if (!buffer)
 		return NULL;
 
 	buffer->next = NULL;
@@ -1712,7 +1672,8 @@ add_char_and_flag(struct e100_serial *info, unsigned char data, unsigned char fl
 {
 	struct etrax_recv_buffer *buffer;
 	if (info->uses_dma_in) {
-		if (!(buffer = alloc_recv_buffer(4)))
+		buffer = alloc_recv_buffer(4);
+		if (!buffer)
 			return 0;
 
 		buffer->length = 1;
@@ -1750,7 +1711,8 @@ static unsigned int handle_descr_data(struct e100_serial *info,
 
 	append_recv_buffer(info, buffer);
 
-	if (!(buffer = alloc_recv_buffer(SERIAL_DESCR_BUF_SIZE)))
+	buffer = alloc_recv_buffer(SERIAL_DESCR_BUF_SIZE);
+	if (!buffer)
 		panic("%s: Failed to allocate memory for receive buffer!\n", __func__);
 
 	descr->buf = virt_to_phys(buffer->buffer);
@@ -1841,7 +1803,6 @@ static void receive_chars_dma(struct e100_serial *info)
 		 */
 		unsigned char data = info->ioport[REG_DATA];
 
-		PROCSTAT(ser_stat[info->line].errors_cnt++);
 		DEBUG_LOG(info->line, "#dERR: s d 0x%04X\n",
 			  ((rstat & SER_ERROR_MASK) << 8) | data);
 
@@ -1867,7 +1828,8 @@ static int start_recv_dma(struct e100_serial *info)
 
 	/* Set up the receiving descriptors */
 	for (i = 0; i < SERIAL_RECV_DESCRIPTORS; i++) {
-		if (!(buffer = alloc_recv_buffer(SERIAL_DESCR_BUF_SIZE)))
+		buffer = alloc_recv_buffer(SERIAL_DESCR_BUF_SIZE);
+		if (!buffer)
 			panic("%s: Failed to allocate memory for receive buffer!\n", __func__);
 
 		descr[i].ctrl = d_int;
@@ -1943,7 +1905,6 @@ tr_interrupt(int irq, void *dev_id)
 			/* Read jiffies_usec first,
 			 * we want this time to be as late as possible
 			 */
- 			PROCSTAT(ser_stat[info->line].tx_dma_ints++);
 			info->last_tx_active_usec = GET_JIFFIES_USEC();
 			info->last_tx_active = jiffies;
 			transmit_chars_dma(info);
@@ -2022,7 +1983,6 @@ static int force_eop_if_needed(struct e100_serial *info)
 	 */
 	if (!info->forced_eop) {
 		info->forced_eop = 1;
-		PROCSTAT(ser_stat[info->line].timeout_flush_cnt++);
 		TIMERD(DEBUG_LOG(info->line, "timeout EOP %i\n", info->line));
 		FORCE_EOP(info);
 	}
@@ -2374,7 +2334,6 @@ static void handle_ser_rx_interrupt(struct e100_serial *info)
 			DEBUG_LOG(info->line, "#iERR s d %04X\n",
 			          ((rstat & SER_ERROR_MASK) << 8) | data);
 		}
-		PROCSTAT(ser_stat[info->line].early_errors_cnt++);
 	} else { /* It was a valid byte, now let the DMA do the rest */
 		unsigned long curr_time_u = GET_JIFFIES_USEC();
 		unsigned long curr_time = jiffies;
@@ -2407,7 +2366,6 @@ static void handle_ser_rx_interrupt(struct e100_serial *info)
 		DINTR2(DEBUG_LOG(info->line, "ser_rx OK %d\n", info->line));
 		info->break_detected_cnt = 0;
 
-		PROCSTAT(ser_stat[info->line].ser_ints_ok_cnt++);
 	}
 	/* Restarting the DMA never hurts */
 	*info->icmdadr = IO_STATE(R_DMA_CH6_CMD, cmd, restart);
@@ -2867,19 +2825,6 @@ change_speed(struct e100_serial *info)
 			*R_SERIAL_PRESCALE = divisor;
 			info->baud = SERIAL_PRESCALE_BASE/divisor;
 		}
-#ifdef CONFIG_ETRAX_EXTERN_PB6CLK_ENABLED
-		else if ((info->baud_base==CONFIG_ETRAX_EXTERN_PB6CLK_FREQ/8 &&
-			  info->custom_divisor == 1) ||
-			 (info->baud_base==CONFIG_ETRAX_EXTERN_PB6CLK_FREQ &&
-			  info->custom_divisor == 8)) {
-				/* ext_clk selected */
-				alt_source =
-					IO_STATE(R_ALT_SER_BAUDRATE, ser0_rec, extern) |
-					IO_STATE(R_ALT_SER_BAUDRATE, ser0_tr, extern);
-				DBAUD(printk("using external baudrate: %lu\n", CONFIG_ETRAX_EXTERN_PB6CLK_FREQ/8));
-				info->baud = CONFIG_ETRAX_EXTERN_PB6CLK_FREQ/8;
-			}
-#endif
 		else
 		{
 			/* Bad baudbase, we don't support using timer0
@@ -3216,9 +3161,7 @@ rs_throttle(struct tty_struct * tty)
 {
 	struct e100_serial *info = (struct e100_serial *)tty->driver_data;
 #ifdef SERIAL_DEBUG_THROTTLE
-	char	buf[64];
-
-	printk("throttle %s: %lu....\n", tty_name(tty, buf),
+	printk("throttle %s: %lu....\n", tty_name(tty),
 	       (unsigned long)tty->ldisc.chars_in_buffer(tty));
 #endif
 	DFLOW(DEBUG_LOG(info->line,"rs_throttle %lu\n", tty->ldisc.chars_in_buffer(tty)));
@@ -3238,9 +3181,7 @@ rs_unthrottle(struct tty_struct * tty)
 {
 	struct e100_serial *info = (struct e100_serial *)tty->driver_data;
 #ifdef SERIAL_DEBUG_THROTTLE
-	char	buf[64];
-
-	printk("unthrottle %s: %lu....\n", tty_name(tty, buf),
+	printk("unthrottle %s: %lu....\n", tty_name(tty),
 	       (unsigned long)tty->ldisc.chars_in_buffer(tty));
 #endif
 	DFLOW(DEBUG_LOG(info->line,"rs_unthrottle ldisc %d\n", tty->ldisc.chars_in_buffer(tty)));
@@ -3725,16 +3666,6 @@ rs_close(struct tty_struct *tty, struct file * filp)
 #if defined(CONFIG_ETRAX_RS485_ON_PA)
 		*R_PORT_PA_DATA = port_pa_data_shadow &= ~(1 << rs485_pa_bit);
 #endif
-#if defined(CONFIG_ETRAX_RS485_ON_PORT_G)
-		REG_SHADOW_SET(R_PORT_G_DATA, port_g_data_shadow,
-			       rs485_port_g_bit, 0);
-#endif
-#if defined(CONFIG_ETRAX_RS485_LTC1387)
-		REG_SHADOW_SET(R_PORT_G_DATA, port_g_data_shadow,
-			       CONFIG_ETRAX_RS485_LTC1387_DXEN_PORT_G_BIT, 0);
-		REG_SHADOW_SET(R_PORT_G_DATA, port_g_data_shadow,
-			       CONFIG_ETRAX_RS485_LTC1387_RXEN_PORT_G_BIT, 0);
-#endif
 	}
 #endif
 
@@ -4263,15 +4194,6 @@ static int __init rs_init(void)
 		return -EBUSY;
 	}
 #endif
-#if defined(CONFIG_ETRAX_RS485_ON_PORT_G)
-	if (cris_io_interface_allocate_pins(if_serial_0, 'g', rs485_pa_bit,
-			rs485_port_g_bit)) {
-		printk(KERN_ERR "ETRAX100LX serial: Could not allocate "
-			"RS485 pin\n");
-		put_tty_driver(driver);
-		return -EBUSY;
-	}
-#endif
 #endif
 
 	/* Initialize the tty_driver structure */
diff --git a/drivers/tty/serial/earlycon.c b/drivers/tty/serial/earlycon.c
index 6dc471e30e79..f09636083426 100644
--- a/drivers/tty/serial/earlycon.c
+++ b/drivers/tty/serial/earlycon.c
@@ -72,6 +72,7 @@ static int __init parse_options(struct earlycon_device *device, char *options)
 
 	switch (port->iotype) {
 	case UPIO_MEM32:
+	case UPIO_MEM32BE:
 		port->regshift = 2;	/* fall-through */
 	case UPIO_MEM:
 		port->mapbase = addr;
@@ -90,9 +91,11 @@ static int __init parse_options(struct earlycon_device *device, char *options)
 		strlcpy(device->options, options, length);
 	}
 
-	if (port->iotype == UPIO_MEM || port->iotype == UPIO_MEM32)
+	if (port->iotype == UPIO_MEM || port->iotype == UPIO_MEM32 ||
+	    port->iotype == UPIO_MEM32BE)
 		pr_info("Early serial console at MMIO%s 0x%llx (options '%s')\n",
-			(port->iotype == UPIO_MEM32) ? "32" : "",
+			(port->iotype == UPIO_MEM) ? "" :
+			(port->iotype == UPIO_MEM32) ? "32" : "32be",
 			(unsigned long long)port->mapbase,
 			device->options);
 	else
@@ -133,7 +136,7 @@ static int __init register_earlycon(char *buf, const struct earlycon_id *match)
  *
  *	Registers the earlycon console matching the earlycon specified
  *	in the param string @buf. Acceptable param strings are of the form
- *	   <name>,io|mmio|mmio32,<addr>,<options>
+ *	   <name>,io|mmio|mmio32|mmio32be,<addr>,<options>
  *	   <name>,0x<addr>,<options>
  *	   <name>,<options>
  *	   <name>
diff --git a/drivers/tty/serial/icom.c b/drivers/tty/serial/icom.c
index 45fc323b95e6..ffc7cb2585a6 100644
--- a/drivers/tty/serial/icom.c
+++ b/drivers/tty/serial/icom.c
@@ -1504,7 +1504,8 @@ static int icom_probe(struct pci_dev *dev,
 		return retval;
 	}
 
-	if ( (retval = pci_request_regions(dev, "icom"))) {
+	retval = pci_request_regions(dev, "icom");
+	if (retval) {
 		 dev_err(&dev->dev, "pci_request_regions FAILED\n");
 		 pci_disable_device(dev);
 		 return retval;
@@ -1512,7 +1513,8 @@ static int icom_probe(struct pci_dev *dev,
 
 	pci_set_master(dev);
 
-	if ( (retval = pci_read_config_dword(dev, PCI_COMMAND, &command_reg))) {
+	retval = pci_read_config_dword(dev, PCI_COMMAND, &command_reg);
+	if (retval) {
 		dev_err(&dev->dev, "PCI Config read FAILED\n");
 		return retval;
 	}
@@ -1556,9 +1558,8 @@ static int icom_probe(struct pci_dev *dev,
 	}
 
 	 /* save off irq and request irq line */
-	 if ( (retval = request_irq(dev->irq, icom_interrupt,
-				   IRQF_SHARED, ICOM_DRIVER_NAME,
-				   (void *) icom_adapter))) {
+	 retval = request_irq(dev->irq, icom_interrupt, IRQF_SHARED, ICOM_DRIVER_NAME, (void *)icom_adapter);
+	 if (retval) {
 		  goto probe_exit2;
 	 }
 
diff --git a/drivers/tty/serial/ifx6x60.c b/drivers/tty/serial/ifx6x60.c
index 590390970996..536a33b99be9 100644
--- a/drivers/tty/serial/ifx6x60.c
+++ b/drivers/tty/serial/ifx6x60.c
@@ -1175,7 +1175,7 @@ static int ifx_spi_spi_probe(struct spi_device *spi)
 	ret = request_irq(gpio_to_irq(ifx_dev->gpio.reset_out),
 			  ifx_spi_reset_interrupt,
 			  IRQF_TRIGGER_RISING|IRQF_TRIGGER_FALLING, DRVNAME,
-		(void *)ifx_dev);
+			  ifx_dev);
 	if (ret) {
 		dev_err(&spi->dev, "Unable to get irq %x\n",
 			gpio_to_irq(ifx_dev->gpio.reset_out));
@@ -1185,9 +1185,8 @@ static int ifx_spi_spi_probe(struct spi_device *spi)
 	ret = ifx_spi_reset(ifx_dev);
 
 	ret = request_irq(gpio_to_irq(ifx_dev->gpio.srdy),
-			  ifx_spi_srdy_interrupt,
-			  IRQF_TRIGGER_RISING, DRVNAME,
-			  (void *)ifx_dev);
+			  ifx_spi_srdy_interrupt, IRQF_TRIGGER_RISING, DRVNAME,
+			  ifx_dev);
 	if (ret) {
 		dev_err(&spi->dev, "Unable to get irq %x",
 			gpio_to_irq(ifx_dev->gpio.srdy));
@@ -1212,7 +1211,7 @@ static int ifx_spi_spi_probe(struct spi_device *spi)
 	return 0;
 
 error_ret7:
-	free_irq(gpio_to_irq(ifx_dev->gpio.reset_out), (void *)ifx_dev);
+	free_irq(gpio_to_irq(ifx_dev->gpio.reset_out), ifx_dev);
 error_ret6:
 	gpio_free(ifx_dev->gpio.srdy);
 error_ret5:
@@ -1243,8 +1242,8 @@ static int ifx_spi_spi_remove(struct spi_device *spi)
 	/* stop activity */
 	tasklet_kill(&ifx_dev->io_work_tasklet);
 	/* free irq */
-	free_irq(gpio_to_irq(ifx_dev->gpio.reset_out), (void *)ifx_dev);
-	free_irq(gpio_to_irq(ifx_dev->gpio.srdy), (void *)ifx_dev);
+	free_irq(gpio_to_irq(ifx_dev->gpio.reset_out), ifx_dev);
+	free_irq(gpio_to_irq(ifx_dev->gpio.srdy), ifx_dev);
 
 	gpio_free(ifx_dev->gpio.srdy);
 	gpio_free(ifx_dev->gpio.mrdy);
@@ -1381,7 +1380,7 @@ static void __exit ifx_spi_exit(void)
 	/* unregister */
 	tty_unregister_driver(tty_drv);
 	put_tty_driver(tty_drv);
-	spi_unregister_driver((void *)&ifx_spi_driver);
+	spi_unregister_driver(&ifx_spi_driver);
 	unregister_reboot_notifier(&ifx_modem_reboot_notifier_block);
 }
 
@@ -1420,7 +1419,7 @@ static int __init ifx_spi_init(void)
 		goto err_free_tty;
 	}
 
-	result = spi_register_driver((void *)&ifx_spi_driver);
+	result = spi_register_driver(&ifx_spi_driver);
 	if (result) {
 		pr_err("%s: spi_register_driver failed(%d)",
 			DRVNAME, result);
@@ -1436,7 +1435,7 @@ static int __init ifx_spi_init(void)
 
 	return 0;
 err_unreg_spi:
-	spi_unregister_driver((void *)&ifx_spi_driver);
+	spi_unregister_driver(&ifx_spi_driver);
 err_unreg_tty:
 	tty_unregister_driver(tty_drv);
 err_free_tty:
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index c8cfa0637128..2c90dc31bfaa 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -239,7 +239,7 @@ static struct imx_uart_data imx_uart_devdata[] = {
 	},
 };
 
-static struct platform_device_id imx_uart_devtype[] = {
+static const struct platform_device_id imx_uart_devtype[] = {
 	{
 		.name = "imx1-uart",
 		.driver_data = (kernel_ulong_t) &imx_uart_devdata[IMX1_UART],
@@ -853,7 +853,7 @@ static void imx_break_ctl(struct uart_port *port, int break_state)
 #define TXTL 2 /* reset default */
 #define RXTL 1 /* reset default */
 
-static int imx_setup_ufcr(struct imx_port *sport, unsigned int mode)
+static void imx_setup_ufcr(struct imx_port *sport, unsigned int mode)
 {
 	unsigned int val;
 
@@ -861,7 +861,6 @@ static int imx_setup_ufcr(struct imx_port *sport, unsigned int mode)
 	val = readl(sport->port.membase + UFCR) & (UFCR_RFDIV | UFCR_DCEDTE);
 	val |= TXTL << UFCR_TXTL_SHF | RXTL;
 	writel(val, sport->port.membase + UFCR);
-	return 0;
 }
 
 #define RX_BUF_SIZE	(PAGE_SIZE)
@@ -911,6 +910,14 @@ static void dma_rx_callback(void *data)
 
 	status = dmaengine_tx_status(chan, (dma_cookie_t)0, &state);
 	count = RX_BUF_SIZE - state.residue;
+
+	if (readl(sport->port.membase + USR2) & USR2_IDLE) {
+		/* In condition [3] the SDMA counted up too early */
+		count--;
+
+		writel(USR2_IDLE, sport->port.membase + USR2);
+	}
+
 	dev_dbg(sport->port.dev, "We get %d bytes.\n", count);
 
 	if (count) {
@@ -1114,6 +1121,12 @@ static int imx_startup(struct uart_port *port)
 
 	writel(temp & ~UCR4_DREN, sport->port.membase + UCR4);
 
+	/* Can we enable the DMA support? */
+	if (is_imx6q_uart(sport) && !uart_console(port) &&
+	    !sport->dma_is_inited)
+		imx_uart_dma_init(sport);
+
+	spin_lock_irqsave(&sport->port.lock, flags);
 	/* Reset fifo's and state machines */
 	i = 100;
 
@@ -1124,13 +1137,6 @@ static int imx_startup(struct uart_port *port)
 	while (!(readl(sport->port.membase + UCR2) & UCR2_SRST) && (--i > 0))
 		udelay(1);
 
-	/* Can we enable the DMA support? */
-	if (is_imx6q_uart(sport) && !uart_console(port) &&
-	    !sport->dma_is_inited)
-		imx_uart_dma_init(sport);
-
-	spin_lock_irqsave(&sport->port.lock, flags);
-
 	/*
 	 * Finally, clear and enable interrupts
 	 */
diff --git a/drivers/tty/serial/ioc3_serial.c b/drivers/tty/serial/ioc3_serial.c
index abd7ea26ed9a..27b5fefac171 100644
--- a/drivers/tty/serial/ioc3_serial.c
+++ b/drivers/tty/serial/ioc3_serial.c
@@ -2137,7 +2137,8 @@ ioc3uart_probe(struct ioc3_submodule *is, struct ioc3_driver_data *idd)
 
 	/* register port with the serial core */
 
-	if ((ret = ioc3_serial_core_attach(is, idd)))
+	ret = ioc3_serial_core_attach(is, idd);
+	if (ret)
 		goto out4;
 
 	Num_of_ioc3_cards++;
diff --git a/drivers/tty/serial/ioc4_serial.c b/drivers/tty/serial/ioc4_serial.c
index aa28209f44c1..e5c42fef69d2 100644
--- a/drivers/tty/serial/ioc4_serial.c
+++ b/drivers/tty/serial/ioc4_serial.c
@@ -1011,7 +1011,8 @@ static irqreturn_t ioc4_intr(int irq, void *arg)
 		 */
 		for (xx = 0; xx < num_intrs; xx++) {
 			intr_info = &soft->is_intr_type[intr_type].is_intr_info[xx];
-			if ((this_mir = this_ir & intr_info->sd_bits)) {
+			this_mir = this_ir & intr_info->sd_bits;
+			if (this_mir) {
 				/* Disable owned interrupts, call handler */
 				handled++;
 				write_ireg(soft, intr_info->sd_bits, IOC4_W_IEC,
@@ -2865,10 +2866,12 @@ ioc4_serial_attach_one(struct ioc4_driver_data *idd)
 
 	/* register port with the serial core - 1 rs232, 1 rs422 */
 
-	if ((ret = ioc4_serial_core_attach(idd->idd_pdev, PROTO_RS232)))
+	ret = ioc4_serial_core_attach(idd->idd_pdev, PROTO_RS232);
+	if (ret)
 		goto out4;
 
-	if ((ret = ioc4_serial_core_attach(idd->idd_pdev, PROTO_RS422)))
+	ret = ioc4_serial_core_attach(idd->idd_pdev, PROTO_RS422);
+	if (ret)
 		goto out5;
 
 	Num_of_ioc4_cards++;
diff --git a/drivers/tty/serial/kgdb_nmi.c b/drivers/tty/serial/kgdb_nmi.c
index 129dc5be6028..117df151627d 100644
--- a/drivers/tty/serial/kgdb_nmi.c
+++ b/drivers/tty/serial/kgdb_nmi.c
@@ -173,18 +173,18 @@ static int kgdb_nmi_poll_one_knock(void)
 bool kgdb_nmi_poll_knock(void)
 {
 	if (kgdb_nmi_knock < 0)
-		return 1;
+		return true;
 
 	while (1) {
 		int ret;
 
 		ret = kgdb_nmi_poll_one_knock();
 		if (ret == NO_POLL_CHAR)
-			return 0;
+			return false;
 		else if (ret == 1)
 			break;
 	}
-	return 1;
+	return true;
 }
 
 /*
diff --git a/drivers/tty/serial/mcf.c b/drivers/tty/serial/mcf.c
index a9b0ab38a68c..02eb32217685 100644
--- a/drivers/tty/serial/mcf.c
+++ b/drivers/tty/serial/mcf.c
@@ -597,7 +597,7 @@ console_initcall(mcf_console_init);
 #define	MCF_CONSOLE	NULL
 
 /****************************************************************************/
-#endif /* CONFIG_MCF_CONSOLE */
+#endif /* CONFIG_SERIAL_MCF_CONSOLE */
 /****************************************************************************/
 
 /*
diff --git a/drivers/tty/serial/meson_uart.c b/drivers/tty/serial/meson_uart.c
index 67c036702629..0fc83c962d10 100644
--- a/drivers/tty/serial/meson_uart.c
+++ b/drivers/tty/serial/meson_uart.c
@@ -370,7 +370,7 @@ static int meson_uart_verify_port(struct uart_port *port,
 static void meson_uart_release_port(struct uart_port *port)
 {
 	if (port->flags & UPF_IOREMAP) {
-		iounmap(port->membase);
+		devm_iounmap(port->dev, port->membase);
 		port->membase = NULL;
 	}
 }
diff --git a/drivers/tty/serial/mpc52xx_uart.c b/drivers/tty/serial/mpc52xx_uart.c
index 1589f17c1fca..6fc07eb9d74e 100644
--- a/drivers/tty/serial/mpc52xx_uart.c
+++ b/drivers/tty/serial/mpc52xx_uart.c
@@ -405,7 +405,7 @@ static struct psc_ops mpc5200b_psc_ops = {
 	.get_mr1 = mpc52xx_psc_get_mr1,
 };
 
-#endif /* CONFIG_MPC52xx */
+#endif /* CONFIG_PPC_MPC52xx */
 
 #ifdef CONFIG_PPC_MPC512x
 #define FIFO_512x(port) ((struct mpc512x_psc_fifo __iomem *)(PSC(port)+1))
diff --git a/drivers/tty/serial/mpsc.c b/drivers/tty/serial/mpsc.c
index 856fd5a5fa3c..82bb6d1fe23b 100644
--- a/drivers/tty/serial/mpsc.c
+++ b/drivers/tty/serial/mpsc.c
@@ -913,7 +913,8 @@ static int mpsc_make_ready(struct mpsc_port_info *pi)
 
 	if (!pi->ready) {
 		mpsc_init_hw(pi);
-		if ((rc = mpsc_alloc_ring_mem(pi)))
+		rc = mpsc_alloc_ring_mem(pi);
+		if (rc)
 			return rc;
 		mpsc_init_rings(pi);
 		pi->ready = 1;
@@ -1895,7 +1896,8 @@ static int mpsc_shared_drv_probe(struct platform_device *dev)
 	int				 rc = -ENODEV;
 
 	if (dev->id == 0) {
-		if (!(rc = mpsc_shared_map_regs(dev))) {
+		rc = mpsc_shared_map_regs(dev);
+		if (!rc) {
 			pdata = (struct mpsc_shared_pdata *)
 				dev_get_platdata(&dev->dev);
 
@@ -2081,14 +2083,16 @@ static int mpsc_drv_probe(struct platform_device *dev)
 	if (dev->id < MPSC_NUM_CTLRS) {
 		pi = &mpsc_ports[dev->id];
 
-		if (!(rc = mpsc_drv_map_regs(pi, dev))) {
+		rc = mpsc_drv_map_regs(pi, dev);
+		if (!rc) {
 			mpsc_drv_get_platform_data(pi, dev, dev->id);
 			pi->port.dev = &dev->dev;
 
-			if (!(rc = mpsc_make_ready(pi))) {
+			rc = mpsc_make_ready(pi);
+			if (!rc) {
 				spin_lock_init(&pi->tx_lock);
-				if (!(rc = uart_add_one_port(&mpsc_reg,
-								&pi->port))) {
+				rc = uart_add_one_port(&mpsc_reg, &pi->port);
+				if (!rc) {
 					rc = 0;
 				} else {
 					mpsc_release_port((struct uart_port *)
@@ -2136,9 +2140,12 @@ static int __init mpsc_drv_init(void)
 	memset(mpsc_ports, 0, sizeof(mpsc_ports));
 	memset(&mpsc_shared_regs, 0, sizeof(mpsc_shared_regs));
 
-	if (!(rc = uart_register_driver(&mpsc_reg))) {
-		if (!(rc = platform_driver_register(&mpsc_shared_driver))) {
-			if ((rc = platform_driver_register(&mpsc_driver))) {
+	rc = uart_register_driver(&mpsc_reg);
+	if (!rc) {
+		rc = platform_driver_register(&mpsc_shared_driver);
+		if (!rc) {
+			rc = platform_driver_register(&mpsc_driver);
+			if (rc) {
 				platform_driver_unregister(&mpsc_shared_driver);
 				uart_unregister_driver(&mpsc_reg);
 			}
diff --git a/drivers/tty/serial/msm_smd_tty.c b/drivers/tty/serial/msm_smd_tty.c
deleted file mode 100644
index 1238ac370bff..000000000000
--- a/drivers/tty/serial/msm_smd_tty.c
+++ /dev/null
@@ -1,232 +0,0 @@
-/*
- * Copyright (C) 2007 Google, Inc.
- * Copyright (c) 2011, Code Aurora Forum. All rights reserved.
- * Author: Brian Swetland <swetland@google.com>
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#include <linux/module.h>
-#include <linux/fs.h>
-#include <linux/cdev.h>
-#include <linux/device.h>
-#include <linux/wait.h>
-
-#include <linux/tty.h>
-#include <linux/tty_driver.h>
-#include <linux/tty_flip.h>
-
-#include <mach/msm_smd.h>
-
-#define MAX_SMD_TTYS 32
-
-struct smd_tty_info {
-	struct tty_port port;
-	smd_channel_t *ch;
-};
-
-struct smd_tty_channel_desc {
-	int id;
-	const char *name;
-};
-
-static struct smd_tty_info smd_tty[MAX_SMD_TTYS];
-
-static const struct smd_tty_channel_desc smd_default_tty_channels[] = {
-	{ .id = 0, .name = "SMD_DS" },
-	{ .id = 27, .name = "SMD_GPSNMEA" },
-};
-
-static const struct smd_tty_channel_desc *smd_tty_channels =
-		smd_default_tty_channels;
-static int smd_tty_channels_len = ARRAY_SIZE(smd_default_tty_channels);
-
-static void smd_tty_notify(void *priv, unsigned event)
-{
-	unsigned char *ptr;
-	int avail;
-	struct smd_tty_info *info = priv;
-	struct tty_struct *tty;
-
-	if (event != SMD_EVENT_DATA)
-		return;
-
-	tty = tty_port_tty_get(&info->port);
-	if (!tty)
-		return;
-
-	for (;;) {
-		if (test_bit(TTY_THROTTLED, &tty->flags))
-			break;
-		avail = smd_read_avail(info->ch);
-		if (avail == 0)
-			break;
-
-		avail = tty_prepare_flip_string(&info->port, &ptr, avail);
-
-		if (smd_read(info->ch, ptr, avail) != avail) {
-			/* shouldn't be possible since we're in interrupt
-			** context here and nobody else could 'steal' our
-			** characters.
-			*/
-			pr_err("OOPS - smd_tty_buffer mismatch?!");
-		}
-
-		tty_flip_buffer_push(&info->port);
-	}
-
-	/* XXX only when writable and necessary */
-	tty_wakeup(tty);
-	tty_kref_put(tty);
-}
-
-static int smd_tty_port_activate(struct tty_port *tport, struct tty_struct *tty)
-{
-	struct smd_tty_info *info = container_of(tport, struct smd_tty_info,
-			port);
-	int i, res = 0;
-	const char *name = NULL;
-
-	for (i = 0; i < smd_tty_channels_len; i++) {
-		if (smd_tty_channels[i].id == tty->index) {
-			name = smd_tty_channels[i].name;
-			break;
-		}
-	}
-	if (!name)
-		return -ENODEV;
-
-	if (info->ch)
-		smd_kick(info->ch);
-	else
-		res = smd_open(name, &info->ch, info, smd_tty_notify);
-
-	if (!res)
-		tty->driver_data = info;
-
-	return res;
-}
-
-static void smd_tty_port_shutdown(struct tty_port *tport)
-{
-	struct smd_tty_info *info = container_of(tport, struct smd_tty_info,
-			port);
-
-	if (info->ch) {
-		smd_close(info->ch);
-		info->ch = 0;
-	}
-}
-
-static int smd_tty_open(struct tty_struct *tty, struct file *f)
-{
-	struct smd_tty_info *info = smd_tty + tty->index;
-
-	return tty_port_open(&info->port, tty, f);
-}
-
-static void smd_tty_close(struct tty_struct *tty, struct file *f)
-{
-	struct smd_tty_info *info = tty->driver_data;
-
-	tty_port_close(&info->port, tty, f);
-}
-
-static int smd_tty_write(struct tty_struct *tty,
-			 const unsigned char *buf, int len)
-{
-	struct smd_tty_info *info = tty->driver_data;
-	int avail;
-
-	/* if we're writing to a packet channel we will
-	** never be able to write more data than there
-	** is currently space for
-	*/
-	avail = smd_write_avail(info->ch);
-	if (len > avail)
-		len = avail;
-
-	return smd_write(info->ch, buf, len);
-}
-
-static int smd_tty_write_room(struct tty_struct *tty)
-{
-	struct smd_tty_info *info = tty->driver_data;
-	return smd_write_avail(info->ch);
-}
-
-static int smd_tty_chars_in_buffer(struct tty_struct *tty)
-{
-	struct smd_tty_info *info = tty->driver_data;
-	return smd_read_avail(info->ch);
-}
-
-static void smd_tty_unthrottle(struct tty_struct *tty)
-{
-	struct smd_tty_info *info = tty->driver_data;
-	smd_kick(info->ch);
-}
-
-static const struct tty_port_operations smd_tty_port_ops = {
-	.shutdown = smd_tty_port_shutdown,
-	.activate = smd_tty_port_activate,
-};
-
-static const struct tty_operations smd_tty_ops = {
-	.open = smd_tty_open,
-	.close = smd_tty_close,
-	.write = smd_tty_write,
-	.write_room = smd_tty_write_room,
-	.chars_in_buffer = smd_tty_chars_in_buffer,
-	.unthrottle = smd_tty_unthrottle,
-};
-
-static struct tty_driver *smd_tty_driver;
-
-static int __init smd_tty_init(void)
-{
-	int ret, i;
-
-	smd_tty_driver = alloc_tty_driver(MAX_SMD_TTYS);
-	if (smd_tty_driver == 0)
-		return -ENOMEM;
-
-	smd_tty_driver->driver_name = "smd_tty_driver";
-	smd_tty_driver->name = "smd";
-	smd_tty_driver->major = 0;
-	smd_tty_driver->minor_start = 0;
-	smd_tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
-	smd_tty_driver->subtype = SERIAL_TYPE_NORMAL;
-	smd_tty_driver->init_termios = tty_std_termios;
-	smd_tty_driver->init_termios.c_iflag = 0;
-	smd_tty_driver->init_termios.c_oflag = 0;
-	smd_tty_driver->init_termios.c_cflag = B38400 | CS8 | CREAD;
-	smd_tty_driver->init_termios.c_lflag = 0;
-	smd_tty_driver->flags = TTY_DRIVER_RESET_TERMIOS |
-		TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
-	tty_set_operations(smd_tty_driver, &smd_tty_ops);
-
-	ret = tty_register_driver(smd_tty_driver);
-	if (ret)
-		return ret;
-
-	for (i = 0; i < smd_tty_channels_len; i++) {
-		struct tty_port *port = &smd_tty[smd_tty_channels[i].id].port;
-		tty_port_init(port);
-		port->ops = &smd_tty_port_ops;
-		tty_port_register_device(port, smd_tty_driver,
-				smd_tty_channels[i].id, NULL);
-	}
-
-	return 0;
-}
-
-module_init(smd_tty_init);
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index f7e5825b55ab..13cf7738fbdc 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -169,7 +169,7 @@ struct mxs_auart_port {
 	bool			ms_irq_enabled;
 };
 
-static struct platform_device_id mxs_auart_devtype[] = {
+static const struct platform_device_id mxs_auart_devtype[] = {
 	{ .name = "mxs-auart-imx23", .driver_data = IMX23_AUART },
 	{ .name = "mxs-auart-imx28", .driver_data = IMX28_AUART },
 	{ /* sentinel */ }
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index 137381e649e5..6823df99bd76 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -67,14 +67,17 @@ static int of_platform_serial_setup(struct platform_device *ofdev,
 	if (of_property_read_u32(np, "clock-frequency", &clk)) {
 
 		/* Get clk rate through clk driver if present */
-		info->clk = clk_get(&ofdev->dev, NULL);
+		info->clk = devm_clk_get(&ofdev->dev, NULL);
 		if (IS_ERR(info->clk)) {
 			dev_warn(&ofdev->dev,
 				"clk or clock-frequency not defined\n");
 			return PTR_ERR(info->clk);
 		}
 
-		clk_prepare_enable(info->clk);
+		ret = clk_prepare_enable(info->clk);
+		if (ret < 0)
+			return ret;
+
 		clk = clk_get_rate(info->clk);
 	}
 	/* If current-speed was set, then try not to change it. */
@@ -188,7 +191,6 @@ static int of_platform_serial_probe(struct platform_device *ofdev)
 	{
 		struct uart_8250_port port8250;
 		memset(&port8250, 0, sizeof(port8250));
-		port.type = port_type;
 		port8250.port = port;
 
 		if (port.fifosize)
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index 7f49172ccd86..7a2172b5e93c 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -38,6 +38,7 @@
 #include <linux/serial_core.h>
 #include <linux/irq.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_wakeirq.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
 #include <linux/gpio.h>
@@ -160,7 +161,6 @@ struct uart_omap_port {
 	unsigned long		port_activity;
 	int			context_loss_cnt;
 	u32			errata;
-	u8			wakeups_enabled;
 	u32			features;
 
 	int			rts_gpio;
@@ -209,28 +209,11 @@ static int serial_omap_get_context_loss_count(struct uart_omap_port *up)
 	return pdata->get_context_loss_count(up->dev);
 }
 
-static inline void serial_omap_enable_wakeirq(struct uart_omap_port *up,
-				       bool enable)
-{
-	if (!up->wakeirq)
-		return;
-
-	if (enable)
-		enable_irq(up->wakeirq);
-	else
-		disable_irq_nosync(up->wakeirq);
-}
-
+/* REVISIT: Remove this when omap3 boots in device tree only mode */
 static void serial_omap_enable_wakeup(struct uart_omap_port *up, bool enable)
 {
 	struct omap_uart_port_info *pdata = dev_get_platdata(up->dev);
 
-	if (enable == up->wakeups_enabled)
-		return;
-
-	serial_omap_enable_wakeirq(up, enable);
-	up->wakeups_enabled = enable;
-
 	if (!pdata || !pdata->enable_wakeup)
 		return;
 
@@ -750,13 +733,11 @@ static int serial_omap_startup(struct uart_port *port)
 
 	/* Optional wake-up IRQ */
 	if (up->wakeirq) {
-		retval = request_irq(up->wakeirq, serial_omap_irq,
-				     up->port.irqflags, up->name, up);
+		retval = dev_pm_set_dedicated_wake_irq(up->dev, up->wakeirq);
 		if (retval) {
 			free_irq(up->port.irq, up);
 			return retval;
 		}
-		disable_irq(up->wakeirq);
 	}
 
 	dev_dbg(up->port.dev, "serial_omap_startup+%d\n", up->port.line);
@@ -845,8 +826,7 @@ static void serial_omap_shutdown(struct uart_port *port)
 	pm_runtime_mark_last_busy(up->dev);
 	pm_runtime_put_autosuspend(up->dev);
 	free_irq(up->port.irq, up);
-	if (up->wakeirq)
-		free_irq(up->wakeirq, up);
+	dev_pm_clear_wake_irq(up->dev);
 }
 
 static void serial_omap_uart_qos_work(struct work_struct *work)
@@ -1139,13 +1119,6 @@ serial_omap_pm(struct uart_port *port, unsigned int state,
 	serial_out(up, UART_EFR, efr);
 	serial_out(up, UART_LCR, 0);
 
-	if (!device_may_wakeup(up->dev)) {
-		if (!state)
-			pm_runtime_forbid(up->dev);
-		else
-			pm_runtime_allow(up->dev);
-	}
-
 	pm_runtime_mark_last_busy(up->dev);
 	pm_runtime_put_autosuspend(up->dev);
 }
diff --git a/drivers/tty/serial/samsung.c b/drivers/tty/serial/samsung.c
index a0ae942d9562..67d0c213b1c7 100644
--- a/drivers/tty/serial/samsung.c
+++ b/drivers/tty/serial/samsung.c
@@ -348,7 +348,7 @@ static void s3c24xx_serial_start_next_tx(struct s3c24xx_uart_port *ourport)
 		s3c24xx_serial_start_tx_dma(ourport, count);
 }
 
-void s3c24xx_serial_start_tx(struct uart_port *port)
+static void s3c24xx_serial_start_tx(struct uart_port *port)
 {
 	struct s3c24xx_uart_port *ourport = to_ourport(port);
 	struct circ_buf *xmit = &port->state->xmit;
@@ -2337,7 +2337,7 @@ static struct s3c24xx_serial_drv_data exynos5433_serial_drv_data = {
 #define EXYNOS5433_SERIAL_DRV_DATA (kernel_ulong_t)NULL
 #endif
 
-static struct platform_device_id s3c24xx_serial_driver_ids[] = {
+static const struct platform_device_id s3c24xx_serial_driver_ids[] = {
 	{
 		.name		= "s3c2410-uart",
 		.driver_data	= S3C2410_SERIAL_DRV_DATA,
diff --git a/drivers/tty/serial/sc16is7xx.c b/drivers/tty/serial/sc16is7xx.c
index 468354ef7baa..9e6576004a42 100644
--- a/drivers/tty/serial/sc16is7xx.c
+++ b/drivers/tty/serial/sc16is7xx.c
@@ -25,6 +25,7 @@
 #include <linux/serial.h>
 #include <linux/tty.h>
 #include <linux/tty_flip.h>
+#include <linux/spi/spi.h>
 #include <linux/uaccess.h>
 
 #define SC16IS7XX_NAME			"sc16is7xx"
@@ -300,25 +301,38 @@ struct sc16is7xx_devtype {
 	int	nr_uart;
 };
 
+#define SC16IS7XX_RECONF_MD		(1 << 0)
+#define SC16IS7XX_RECONF_IER		(1 << 1)
+#define SC16IS7XX_RECONF_RS485		(1 << 2)
+
+struct sc16is7xx_one_config {
+	unsigned int			flags;
+	u8				ier_clear;
+};
+
 struct sc16is7xx_one {
 	struct uart_port		port;
-	struct work_struct		tx_work;
-	struct work_struct		md_work;
+	struct kthread_work		tx_work;
+	struct kthread_work		reg_work;
+	struct sc16is7xx_one_config	config;
 };
 
 struct sc16is7xx_port {
 	struct uart_driver		uart;
 	struct sc16is7xx_devtype	*devtype;
 	struct regmap			*regmap;
-	struct mutex			mutex;
 	struct clk			*clk;
 #ifdef CONFIG_GPIOLIB
 	struct gpio_chip		gpio;
 #endif
 	unsigned char			buf[SC16IS7XX_FIFO_SIZE];
+	struct kthread_worker		kworker;
+	struct task_struct		*kworker_task;
+	struct kthread_work		irq_work;
 	struct sc16is7xx_one		p[0];
 };
 
+#define to_sc16is7xx_port(p,e)	((container_of((p), struct sc16is7xx_port, e)))
 #define to_sc16is7xx_one(p,e)	((container_of((p), struct sc16is7xx_one, e)))
 
 static u8 sc16is7xx_port_read(struct uart_port *port, u8 reg)
@@ -615,9 +629,7 @@ static void sc16is7xx_port_irq(struct sc16is7xx_port *s, int portno)
 					       !!(msr & SC16IS7XX_MSR_CTS_BIT));
 			break;
 		case SC16IS7XX_IIR_THRI_SRC:
-			mutex_lock(&s->mutex);
 			sc16is7xx_handle_tx(port);
-			mutex_unlock(&s->mutex);
 			break;
 		default:
 			dev_err_ratelimited(port->dev,
@@ -628,81 +640,115 @@ static void sc16is7xx_port_irq(struct sc16is7xx_port *s, int portno)
 	} while (1);
 }
 
-static irqreturn_t sc16is7xx_ist(int irq, void *dev_id)
+static void sc16is7xx_ist(struct kthread_work *ws)
 {
-	struct sc16is7xx_port *s = (struct sc16is7xx_port *)dev_id;
+	struct sc16is7xx_port *s = to_sc16is7xx_port(ws, irq_work);
 	int i;
 
 	for (i = 0; i < s->uart.nr; ++i)
 		sc16is7xx_port_irq(s, i);
+}
+
+static irqreturn_t sc16is7xx_irq(int irq, void *dev_id)
+{
+	struct sc16is7xx_port *s = (struct sc16is7xx_port *)dev_id;
+
+	queue_kthread_work(&s->kworker, &s->irq_work);
 
 	return IRQ_HANDLED;
 }
 
-static void sc16is7xx_wq_proc(struct work_struct *ws)
+static void sc16is7xx_tx_proc(struct kthread_work *ws)
 {
-	struct sc16is7xx_one *one = to_sc16is7xx_one(ws, tx_work);
-	struct sc16is7xx_port *s = dev_get_drvdata(one->port.dev);
+	struct uart_port *port = &(to_sc16is7xx_one(ws, tx_work)->port);
+
+	if ((port->rs485.flags & SER_RS485_ENABLED) &&
+	    (port->rs485.delay_rts_before_send > 0))
+		msleep(port->rs485.delay_rts_before_send);
 
-	mutex_lock(&s->mutex);
-	sc16is7xx_handle_tx(&one->port);
-	mutex_unlock(&s->mutex);
+	sc16is7xx_handle_tx(port);
 }
 
-static void sc16is7xx_stop_tx(struct uart_port* port)
+static void sc16is7xx_reconf_rs485(struct uart_port *port)
 {
-	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
-	struct circ_buf *xmit = &one->port.state->xmit;
-
-	/* handle rs485 */
-	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) &&
-		    (port->rs485.delay_rts_after_send > 0))
-			mdelay(port->rs485.delay_rts_after_send);
+	const u32 mask = SC16IS7XX_EFCR_AUTO_RS485_BIT |
+			 SC16IS7XX_EFCR_RTS_INVERT_BIT;
+	u32 efcr = 0;
+	struct serial_rs485 *rs485 = &port->rs485;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&port->lock, irqflags);
+	if (rs485->flags & SER_RS485_ENABLED) {
+		efcr |=	SC16IS7XX_EFCR_AUTO_RS485_BIT;
+
+		if (rs485->flags & SER_RS485_RTS_AFTER_SEND)
+			efcr |= SC16IS7XX_EFCR_RTS_INVERT_BIT;
 	}
+	spin_unlock_irqrestore(&port->lock, irqflags);
 
-	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
-			      SC16IS7XX_IER_THRI_BIT,
-			      0);
+	sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG, mask, efcr);
 }
 
-static void sc16is7xx_stop_rx(struct uart_port* port)
+static void sc16is7xx_reg_proc(struct kthread_work *ws)
 {
+	struct sc16is7xx_one *one = to_sc16is7xx_one(ws, reg_work);
+	struct sc16is7xx_one_config config;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&one->port.lock, irqflags);
+	config = one->config;
+	memset(&one->config, 0, sizeof(one->config));
+	spin_unlock_irqrestore(&one->port.lock, irqflags);
+
+	if (config.flags & SC16IS7XX_RECONF_MD)
+		sc16is7xx_port_update(&one->port, SC16IS7XX_MCR_REG,
+				      SC16IS7XX_MCR_LOOP_BIT,
+				      (one->port.mctrl & TIOCM_LOOP) ?
+				      SC16IS7XX_MCR_LOOP_BIT : 0);
+
+	if (config.flags & SC16IS7XX_RECONF_IER)
+		sc16is7xx_port_update(&one->port, SC16IS7XX_IER_REG,
+				      config.ier_clear, 0);
+
+	if (config.flags & SC16IS7XX_RECONF_RS485)
+		sc16is7xx_reconf_rs485(&one->port);
+}
+
+static void sc16is7xx_ier_clear(struct uart_port *port, u8 bit)
+{
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
 	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
 
-	one->port.read_status_mask &= ~SC16IS7XX_LSR_DR_BIT;
-	sc16is7xx_port_update(port, SC16IS7XX_IER_REG,
-			      SC16IS7XX_LSR_DR_BIT,
-			      0);
+	one->config.flags |= SC16IS7XX_RECONF_IER;
+	one->config.ier_clear |= bit;
+	queue_kthread_work(&s->kworker, &one->reg_work);
+}
+
+static void sc16is7xx_stop_tx(struct uart_port *port)
+{
+	sc16is7xx_ier_clear(port, SC16IS7XX_IER_THRI_BIT);
+}
+
+static void sc16is7xx_stop_rx(struct uart_port *port)
+{
+	sc16is7xx_ier_clear(port, SC16IS7XX_IER_RDI_BIT);
 }
 
 static void sc16is7xx_start_tx(struct uart_port *port)
 {
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
 	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
 
-	/* handle rs485 */
-	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))
-		schedule_work(&one->tx_work);
+	queue_kthread_work(&s->kworker, &one->tx_work);
 }
 
 static unsigned int sc16is7xx_tx_empty(struct uart_port *port)
 {
-	unsigned int lvl, lsr;
+	unsigned int lsr;
 
-	lvl = sc16is7xx_port_read(port, SC16IS7XX_TXLVL_REG);
 	lsr = sc16is7xx_port_read(port, SC16IS7XX_LSR_REG);
 
-	return ((lsr & SC16IS7XX_LSR_THRE_BIT) && !lvl) ? TIOCSER_TEMT : 0;
+	return (lsr & SC16IS7XX_LSR_TEMT_BIT) ? TIOCSER_TEMT : 0;
 }
 
 static unsigned int sc16is7xx_get_mctrl(struct uart_port *port)
@@ -713,21 +759,13 @@ static unsigned int sc16is7xx_get_mctrl(struct uart_port *port)
 	return TIOCM_DSR | TIOCM_CAR;
 }
 
-static void sc16is7xx_md_proc(struct work_struct *ws)
-{
-	struct sc16is7xx_one *one = to_sc16is7xx_one(ws, md_work);
-
-	sc16is7xx_port_update(&one->port, SC16IS7XX_MCR_REG,
-			      SC16IS7XX_MCR_LOOP_BIT,
-			      (one->port.mctrl & TIOCM_LOOP) ?
-				      SC16IS7XX_MCR_LOOP_BIT : 0);
-}
-
 static void sc16is7xx_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
 	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
 
-	schedule_work(&one->md_work);
+	one->config.flags |= SC16IS7XX_RECONF_MD;
+	queue_kthread_work(&s->kworker, &one->reg_work);
 }
 
 static void sc16is7xx_break_ctl(struct uart_port *port, int break_state)
@@ -831,9 +869,8 @@ static void sc16is7xx_set_termios(struct uart_port *port,
 static int sc16is7xx_config_rs485(struct uart_port *port,
 				  struct serial_rs485 *rs485)
 {
-	const u32 mask = SC16IS7XX_EFCR_AUTO_RS485_BIT |
-			 SC16IS7XX_EFCR_RTS_INVERT_BIT;
-	u32 efcr = 0;
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+	struct sc16is7xx_one *one = to_sc16is7xx_one(port, port);
 
 	if (rs485->flags & SER_RS485_ENABLED) {
 		bool rts_during_rx, rts_during_tx;
@@ -841,21 +878,23 @@ static int sc16is7xx_config_rs485(struct uart_port *port,
 		rts_during_rx = rs485->flags & SER_RS485_RTS_AFTER_SEND;
 		rts_during_tx = rs485->flags & SER_RS485_RTS_ON_SEND;
 
-		efcr |= SC16IS7XX_EFCR_AUTO_RS485_BIT;
-
-		if (!rts_during_rx && rts_during_tx)
-			/* default */;
-		else if (rts_during_rx && !rts_during_tx)
-			efcr |= SC16IS7XX_EFCR_RTS_INVERT_BIT;
-		else
+		if (rts_during_rx == rts_during_tx)
 			dev_err(port->dev,
 				"unsupported RTS signalling on_send:%d after_send:%d - exactly one of RS485 RTS flags should be set\n",
 				rts_during_tx, rts_during_rx);
-	}
 
-	sc16is7xx_port_update(port, SC16IS7XX_EFCR_REG, mask, efcr);
+		/*
+		 * RTS signal is handled by HW, it's timing can't be influenced.
+		 * However, it's sometimes useful to delay TX even without RTS
+		 * control therefore we try to handle .delay_rts_before_send.
+		 */
+		if (rs485->delay_rts_after_send)
+			return -EINVAL;
+	}
 
 	port->rs485 = *rs485;
+	one->config.flags |= SC16IS7XX_RECONF_RS485;
+	queue_kthread_work(&s->kworker, &one->reg_work);
 
 	return 0;
 }
@@ -916,6 +955,8 @@ static int sc16is7xx_startup(struct uart_port *port)
 
 static void sc16is7xx_shutdown(struct uart_port *port)
 {
+	struct sc16is7xx_port *s = dev_get_drvdata(port->dev);
+
 	/* Disable all interrupts */
 	sc16is7xx_port_write(port, SC16IS7XX_IER_REG, 0);
 	/* Disable TX/RX */
@@ -926,6 +967,8 @@ static void sc16is7xx_shutdown(struct uart_port *port)
 			      SC16IS7XX_EFCR_TXDISABLE_BIT);
 
 	sc16is7xx_power(port, 0);
+
+	flush_kthread_worker(&s->kworker);
 }
 
 static const char *sc16is7xx_type(struct uart_port *port)
@@ -1043,6 +1086,7 @@ static int sc16is7xx_probe(struct device *dev,
 			   struct sc16is7xx_devtype *devtype,
 			   struct regmap *regmap, int irq, unsigned long flags)
 {
+	struct sched_param sched_param = { .sched_priority = MAX_RT_PRIO / 2 };
 	unsigned long freq, *pfreq = dev_get_platdata(dev);
 	int i, ret;
 	struct sc16is7xx_port *s;
@@ -1084,6 +1128,16 @@ static int sc16is7xx_probe(struct device *dev,
 		goto out_clk;
 	}
 
+	init_kthread_worker(&s->kworker);
+	init_kthread_work(&s->irq_work, sc16is7xx_ist);
+	s->kworker_task = kthread_run(kthread_worker_fn, &s->kworker,
+				      "sc16is7xx");
+	if (IS_ERR(s->kworker_task)) {
+		ret = PTR_ERR(s->kworker_task);
+		goto out_uart;
+	}
+	sched_setscheduler(s->kworker_task, SCHED_FIFO, &sched_param);
+
 #ifdef CONFIG_GPIOLIB
 	if (devtype->nr_gpio) {
 		/* Setup GPIO cotroller */
@@ -1099,12 +1153,10 @@ static int sc16is7xx_probe(struct device *dev,
 		s->gpio.can_sleep	 = 1;
 		ret = gpiochip_add(&s->gpio);
 		if (ret)
-			goto out_uart;
+			goto out_thread;
 	}
 #endif
 
-	mutex_init(&s->mutex);
-
 	for (i = 0; i < devtype->nr_uart; ++i) {
 		/* Initialize port data */
 		s->p[i].port.line	= i;
@@ -1123,10 +1175,9 @@ static int sc16is7xx_probe(struct device *dev,
 		sc16is7xx_port_write(&s->p[i].port, SC16IS7XX_EFCR_REG,
 				     SC16IS7XX_EFCR_RXDISABLE_BIT |
 				     SC16IS7XX_EFCR_TXDISABLE_BIT);
-		/* Initialize queue for start TX */
-		INIT_WORK(&s->p[i].tx_work, sc16is7xx_wq_proc);
-		/* Initialize queue for changing mode */
-		INIT_WORK(&s->p[i].md_work, sc16is7xx_md_proc);
+		/* Initialize kthread work structs */
+		init_kthread_work(&s->p[i].tx_work, sc16is7xx_tx_proc);
+		init_kthread_work(&s->p[i].reg_work, sc16is7xx_reg_proc);
 		/* Register port */
 		uart_add_one_port(&s->uart, &s->p[i].port);
 		/* Go to suspend mode */
@@ -1134,22 +1185,23 @@ static int sc16is7xx_probe(struct device *dev,
 	}
 
 	/* Setup interrupt */
-	ret = devm_request_threaded_irq(dev, irq, NULL, sc16is7xx_ist,
-					IRQF_ONESHOT | flags, dev_name(dev), s);
+	ret = devm_request_irq(dev, irq, sc16is7xx_irq,
+			       IRQF_ONESHOT | flags, dev_name(dev), s);
 	if (!ret)
 		return 0;
 
 	for (i = 0; i < s->uart.nr; i++)
 		uart_remove_one_port(&s->uart, &s->p[i].port);
 
-	mutex_destroy(&s->mutex);
-
 #ifdef CONFIG_GPIOLIB
 	if (devtype->nr_gpio)
 		gpiochip_remove(&s->gpio);
 
-out_uart:
+out_thread:
 #endif
+	kthread_stop(s->kworker_task);
+
+out_uart:
 	uart_unregister_driver(&s->uart);
 
 out_clk:
@@ -1170,13 +1222,13 @@ static int sc16is7xx_remove(struct device *dev)
 #endif
 
 	for (i = 0; i < s->uart.nr; i++) {
-		cancel_work_sync(&s->p[i].tx_work);
-		cancel_work_sync(&s->p[i].md_work);
 		uart_remove_one_port(&s->uart, &s->p[i].port);
 		sc16is7xx_power(&s->p[i].port, 0);
 	}
 
-	mutex_destroy(&s->mutex);
+	flush_kthread_worker(&s->kworker);
+	kthread_stop(s->kworker_task);
+
 	uart_unregister_driver(&s->uart);
 	if (!IS_ERR(s->clk))
 		clk_disable_unprepare(s->clk);
@@ -1204,6 +1256,75 @@ static struct regmap_config regcfg = {
 	.precious_reg = sc16is7xx_regmap_precious,
 };
 
+#ifdef CONFIG_SERIAL_SC16IS7XX_SPI
+static int sc16is7xx_spi_probe(struct spi_device *spi)
+{
+	struct sc16is7xx_devtype *devtype;
+	unsigned long flags = 0;
+	struct regmap *regmap;
+	int ret;
+
+	/* Setup SPI bus */
+	spi->bits_per_word	= 8;
+	/* only supports mode 0 on SC16IS762 */
+	spi->mode		= spi->mode ? : SPI_MODE_0;
+	spi->max_speed_hz	= spi->max_speed_hz ? : 15000000;
+	ret = spi_setup(spi);
+	if (ret)
+		return ret;
+
+	if (spi->dev.of_node) {
+		const struct of_device_id *of_id =
+			of_match_device(sc16is7xx_dt_ids, &spi->dev);
+
+		devtype = (struct sc16is7xx_devtype *)of_id->data;
+	} else {
+		const struct spi_device_id *id_entry = spi_get_device_id(spi);
+
+		devtype = (struct sc16is7xx_devtype *)id_entry->driver_data;
+		flags = IRQF_TRIGGER_FALLING;
+	}
+
+	regcfg.max_register = (0xf << SC16IS7XX_REG_SHIFT) |
+			      (devtype->nr_uart - 1);
+	regmap = devm_regmap_init_spi(spi, &regcfg);
+
+	return sc16is7xx_probe(&spi->dev, devtype, regmap, spi->irq, flags);
+}
+
+static int sc16is7xx_spi_remove(struct spi_device *spi)
+{
+	return sc16is7xx_remove(&spi->dev);
+}
+
+static const struct spi_device_id sc16is7xx_spi_id_table[] = {
+	{ "sc16is74x",	(kernel_ulong_t)&sc16is74x_devtype, },
+	{ "sc16is740",	(kernel_ulong_t)&sc16is74x_devtype, },
+	{ "sc16is741",	(kernel_ulong_t)&sc16is74x_devtype, },
+	{ "sc16is750",	(kernel_ulong_t)&sc16is750_devtype, },
+	{ "sc16is752",	(kernel_ulong_t)&sc16is752_devtype, },
+	{ "sc16is760",	(kernel_ulong_t)&sc16is760_devtype, },
+	{ "sc16is762",	(kernel_ulong_t)&sc16is762_devtype, },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(spi, sc16is7xx_spi_id_table);
+
+static struct spi_driver sc16is7xx_spi_uart_driver = {
+	.driver = {
+		.name		= SC16IS7XX_NAME,
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match_ptr(sc16is7xx_dt_ids),
+	},
+	.probe		= sc16is7xx_spi_probe,
+	.remove		= sc16is7xx_spi_remove,
+	.id_table	= sc16is7xx_spi_id_table,
+};
+
+MODULE_ALIAS("spi:sc16is7xx");
+#endif
+
+#ifdef CONFIG_SERIAL_SC16IS7XX_I2C
 static int sc16is7xx_i2c_probe(struct i2c_client *i2c,
 			       const struct i2c_device_id *id)
 {
@@ -1235,6 +1356,8 @@ static int sc16is7xx_i2c_remove(struct i2c_client *client)
 
 static const struct i2c_device_id sc16is7xx_i2c_id_table[] = {
 	{ "sc16is74x",	(kernel_ulong_t)&sc16is74x_devtype, },
+	{ "sc16is740",	(kernel_ulong_t)&sc16is74x_devtype, },
+	{ "sc16is741",	(kernel_ulong_t)&sc16is74x_devtype, },
 	{ "sc16is750",	(kernel_ulong_t)&sc16is750_devtype, },
 	{ "sc16is752",	(kernel_ulong_t)&sc16is752_devtype, },
 	{ "sc16is760",	(kernel_ulong_t)&sc16is760_devtype, },
@@ -1253,8 +1376,43 @@ static struct i2c_driver sc16is7xx_i2c_uart_driver = {
 	.remove		= sc16is7xx_i2c_remove,
 	.id_table	= sc16is7xx_i2c_id_table,
 };
-module_i2c_driver(sc16is7xx_i2c_uart_driver);
+
 MODULE_ALIAS("i2c:sc16is7xx");
+#endif
+
+static int __init sc16is7xx_init(void)
+{
+	int ret = 0;
+#ifdef CONFIG_SERIAL_SC16IS7XX_I2C
+	ret = i2c_add_driver(&sc16is7xx_i2c_uart_driver);
+	if (ret < 0) {
+		pr_err("failed to init sc16is7xx i2c --> %d\n", ret);
+		return ret;
+	}
+#endif
+
+#ifdef CONFIG_SERIAL_SC16IS7XX_SPI
+	ret = spi_register_driver(&sc16is7xx_spi_uart_driver);
+	if (ret < 0) {
+		pr_err("failed to init sc16is7xx spi --> %d\n", ret);
+		return ret;
+	}
+#endif
+	return ret;
+}
+module_init(sc16is7xx_init);
+
+static void __exit sc16is7xx_exit(void)
+{
+#ifdef CONFIG_SERIAL_SC16IS7XX_I2C
+	i2c_del_driver(&sc16is7xx_i2c_uart_driver);
+#endif
+
+#ifdef CONFIG_SERIAL_SC16IS7XX_SPI
+	spi_unregister_driver(&sc16is7xx_spi_uart_driver);
+#endif
+}
+module_exit(sc16is7xx_exit);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Jon Ringle <jringle@gridpoint.com>");
diff --git a/drivers/tty/serial/serial-tegra.c b/drivers/tty/serial/serial-tegra.c
index 1d5ea3964ee5..cf0133ae762d 100644
--- a/drivers/tty/serial/serial-tegra.c
+++ b/drivers/tty/serial/serial-tegra.c
@@ -131,8 +131,8 @@ struct tegra_uart_port {
 	struct dma_async_tx_descriptor		*rx_dma_desc;
 	dma_cookie_t				tx_cookie;
 	dma_cookie_t				rx_cookie;
-	int					tx_bytes_requested;
-	int					rx_bytes_requested;
+	unsigned int				tx_bytes_requested;
+	unsigned int				rx_bytes_requested;
 };
 
 static void tegra_uart_start_next_tx(struct tegra_uart_port *tup);
@@ -234,6 +234,22 @@ static void tegra_uart_break_ctl(struct uart_port *u, int break_ctl)
 	tup->lcr_shadow = lcr;
 }
 
+/**
+ * tegra_uart_wait_cycle_time: Wait for N UART clock periods
+ *
+ * @tup:	Tegra serial port data structure.
+ * @cycles:	Number of clock periods to wait.
+ *
+ * Tegra UARTs are clocked at 16X the baud/bit rate and hence the UART
+ * clock speed is 16X the current baud rate.
+ */
+static void tegra_uart_wait_cycle_time(struct tegra_uart_port *tup,
+				       unsigned int cycles)
+{
+	if (tup->current_baud)
+		udelay(DIV_ROUND_UP(cycles * 1000000, tup->current_baud * 16));
+}
+
 /* Wait for a symbol-time. */
 static void tegra_uart_wait_sym_time(struct tegra_uart_port *tup,
 		unsigned int syms)
@@ -263,8 +279,12 @@ static void tegra_uart_fifo_reset(struct tegra_uart_port *tup, u8 fcr_bits)
 	/* Dummy read to ensure the write is posted */
 	tegra_uart_read(tup, UART_SCR);
 
-	/* Wait for the flush to propagate. */
-	tegra_uart_wait_sym_time(tup, 1);
+	/*
+	 * For all tegra devices (up to t210), there is a hardware issue that
+	 * requires software to wait for 32 UART clock periods for the flush
+	 * to propagate, otherwise data could be lost.
+	 */
+	tegra_uart_wait_cycle_time(tup, 32);
 }
 
 static int tegra_set_baudrate(struct tegra_uart_port *tup, unsigned int baud)
@@ -388,9 +408,9 @@ static void tegra_uart_tx_dma_complete(void *args)
 	struct circ_buf *xmit = &tup->uport.state->xmit;
 	struct dma_tx_state state;
 	unsigned long flags;
-	int count;
+	unsigned int count;
 
-	dmaengine_tx_status(tup->tx_dma_chan, tup->rx_cookie, &state);
+	dmaengine_tx_status(tup->tx_dma_chan, tup->tx_cookie, &state);
 	count = tup->tx_bytes_requested - state.residue;
 	async_tx_ack(tup->tx_dma_desc);
 	spin_lock_irqsave(&tup->uport.lock, flags);
@@ -480,7 +500,7 @@ static void tegra_uart_stop_tx(struct uart_port *u)
 	struct tegra_uart_port *tup = to_tegra_uport(u);
 	struct circ_buf *xmit = &tup->uport.state->xmit;
 	struct dma_tx_state state;
-	int count;
+	unsigned int count;
 
 	if (tup->tx_in_progress != TEGRA_UART_TX_DMA)
 		return;
@@ -530,10 +550,15 @@ static void tegra_uart_handle_rx_pio(struct tegra_uart_port *tup,
 }
 
 static void tegra_uart_copy_rx_to_tty(struct tegra_uart_port *tup,
-		struct tty_port *tty, int count)
+				      struct tty_port *tty,
+				      unsigned int count)
 {
 	int copied;
 
+	/* If count is zero, then there is no data to be copied */
+	if (!count)
+		return;
+
 	tup->uport.icount.rx += count;
 	if (!tty) {
 		dev_err(tup->uport.dev, "No tty port\n");
@@ -555,21 +580,30 @@ static void tegra_uart_rx_dma_complete(void *args)
 {
 	struct tegra_uart_port *tup = args;
 	struct uart_port *u = &tup->uport;
-	int count = tup->rx_bytes_requested;
+	unsigned int count = tup->rx_bytes_requested;
 	struct tty_struct *tty = tty_port_tty_get(&tup->uport.state->port);
 	struct tty_port *port = &u->state->port;
 	unsigned long flags;
+	struct dma_tx_state state;
+	enum dma_status status;
 
-	async_tx_ack(tup->rx_dma_desc);
 	spin_lock_irqsave(&u->lock, flags);
 
+	status = dmaengine_tx_status(tup->rx_dma_chan, tup->rx_cookie, &state);
+
+	if (status == DMA_IN_PROGRESS) {
+		dev_dbg(tup->uport.dev, "RX DMA is in progress\n");
+		goto done;
+	}
+
+	async_tx_ack(tup->rx_dma_desc);
+
 	/* Deactivate flow control to stop sender */
 	if (tup->rts_active)
 		set_rts(tup, false);
 
 	/* If we are here, DMA is stopped */
-	if (count)
-		tegra_uart_copy_rx_to_tty(tup, port, count);
+	tegra_uart_copy_rx_to_tty(tup, port, count);
 
 	tegra_uart_handle_rx_pio(tup, port);
 	if (tty) {
@@ -584,6 +618,7 @@ static void tegra_uart_rx_dma_complete(void *args)
 	if (tup->rts_active)
 		set_rts(tup, true);
 
+done:
 	spin_unlock_irqrestore(&u->lock, flags);
 }
 
@@ -594,7 +629,7 @@ static void tegra_uart_handle_rx_dma(struct tegra_uart_port *tup,
 	struct tty_struct *tty = tty_port_tty_get(&tup->uport.state->port);
 	struct tty_port *port = &tup->uport.state->port;
 	struct uart_port *u = &tup->uport;
-	int count;
+	unsigned int count;
 
 	/* Deactivate flow control to stop sender */
 	if (tup->rts_active)
@@ -606,8 +641,7 @@ static void tegra_uart_handle_rx_dma(struct tegra_uart_port *tup,
 	count = tup->rx_bytes_requested - state.residue;
 
 	/* If we are here, DMA is stopped */
-	if (count)
-		tegra_uart_copy_rx_to_tty(tup, port, count);
+	tegra_uart_copy_rx_to_tty(tup, port, count);
 
 	tegra_uart_handle_rx_pio(tup, port);
 	if (tty) {
@@ -865,6 +899,16 @@ static int tegra_uart_hw_init(struct tegra_uart_port *tup)
 	tup->fcr_shadow |= TEGRA_UART_TX_TRIG_16B;
 	tegra_uart_write(tup, tup->fcr_shadow, UART_FCR);
 
+	/* Dummy read to ensure the write is posted */
+	tegra_uart_read(tup, UART_SCR);
+
+	/*
+	 * For all tegra devices (up to t210), there is a hardware issue that
+	 * requires software to wait for 3 UART clock periods after enabling
+	 * the TX fifo, otherwise data could be lost.
+	 */
+	tegra_uart_wait_cycle_time(tup, 3);
+
 	/*
 	 * Initialize the UART with default configuration
 	 * (115200, N, 8, 1) so that the receive DMA buffer may be
@@ -905,6 +949,28 @@ static int tegra_uart_hw_init(struct tegra_uart_port *tup)
 	return 0;
 }
 
+static void tegra_uart_dma_channel_free(struct tegra_uart_port *tup,
+		bool dma_to_memory)
+{
+	if (dma_to_memory) {
+		dmaengine_terminate_all(tup->rx_dma_chan);
+		dma_release_channel(tup->rx_dma_chan);
+		dma_free_coherent(tup->uport.dev, TEGRA_UART_RX_DMA_BUFFER_SIZE,
+				tup->rx_dma_buf_virt, tup->rx_dma_buf_phys);
+		tup->rx_dma_chan = NULL;
+		tup->rx_dma_buf_phys = 0;
+		tup->rx_dma_buf_virt = NULL;
+	} else {
+		dmaengine_terminate_all(tup->tx_dma_chan);
+		dma_release_channel(tup->tx_dma_chan);
+		dma_unmap_single(tup->uport.dev, tup->tx_dma_buf_phys,
+			UART_XMIT_SIZE, DMA_TO_DEVICE);
+		tup->tx_dma_chan = NULL;
+		tup->tx_dma_buf_phys = 0;
+		tup->tx_dma_buf_virt = NULL;
+	}
+}
+
 static int tegra_uart_dma_channel_allocate(struct tegra_uart_port *tup,
 			bool dma_to_memory)
 {
@@ -933,67 +999,39 @@ static int tegra_uart_dma_channel_allocate(struct tegra_uart_port *tup,
 			dma_release_channel(dma_chan);
 			return -ENOMEM;
 		}
+		dma_sconfig.src_addr = tup->uport.mapbase;
+		dma_sconfig.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+		dma_sconfig.src_maxburst = 4;
+		tup->rx_dma_chan = dma_chan;
+		tup->rx_dma_buf_virt = dma_buf;
+		tup->rx_dma_buf_phys = dma_phys;
 	} else {
 		dma_phys = dma_map_single(tup->uport.dev,
 			tup->uport.state->xmit.buf, UART_XMIT_SIZE,
 			DMA_TO_DEVICE);
+		if (dma_mapping_error(tup->uport.dev, dma_phys)) {
+			dev_err(tup->uport.dev, "dma_map_single tx failed\n");
+			dma_release_channel(dma_chan);
+			return -ENOMEM;
+		}
 		dma_buf = tup->uport.state->xmit.buf;
-	}
-
-	if (dma_to_memory) {
-		dma_sconfig.src_addr = tup->uport.mapbase;
-		dma_sconfig.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
-		dma_sconfig.src_maxburst = 4;
-	} else {
 		dma_sconfig.dst_addr = tup->uport.mapbase;
 		dma_sconfig.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
 		dma_sconfig.dst_maxburst = 16;
+		tup->tx_dma_chan = dma_chan;
+		tup->tx_dma_buf_virt = dma_buf;
+		tup->tx_dma_buf_phys = dma_phys;
 	}
 
 	ret = dmaengine_slave_config(dma_chan, &dma_sconfig);
 	if (ret < 0) {
 		dev_err(tup->uport.dev,
 			"Dma slave config failed, err = %d\n", ret);
-		goto scrub;
+		tegra_uart_dma_channel_free(tup, dma_to_memory);
+		return ret;
 	}
 
-	if (dma_to_memory) {
-		tup->rx_dma_chan = dma_chan;
-		tup->rx_dma_buf_virt = dma_buf;
-		tup->rx_dma_buf_phys = dma_phys;
-	} else {
-		tup->tx_dma_chan = dma_chan;
-		tup->tx_dma_buf_virt = dma_buf;
-		tup->tx_dma_buf_phys = dma_phys;
-	}
 	return 0;
-
-scrub:
-	dma_release_channel(dma_chan);
-	return ret;
-}
-
-static void tegra_uart_dma_channel_free(struct tegra_uart_port *tup,
-		bool dma_to_memory)
-{
-	struct dma_chan *dma_chan;
-
-	if (dma_to_memory) {
-		dma_free_coherent(tup->uport.dev, TEGRA_UART_RX_DMA_BUFFER_SIZE,
-				tup->rx_dma_buf_virt, tup->rx_dma_buf_phys);
-		dma_chan = tup->rx_dma_chan;
-		tup->rx_dma_chan = NULL;
-		tup->rx_dma_buf_phys = 0;
-		tup->rx_dma_buf_virt = NULL;
-	} else {
-		dma_unmap_single(tup->uport.dev, tup->tx_dma_buf_phys,
-			UART_XMIT_SIZE, DMA_TO_DEVICE);
-		dma_chan = tup->tx_dma_chan;
-		tup->tx_dma_chan = NULL;
-		tup->tx_dma_buf_phys = 0;
-		tup->tx_dma_buf_virt = NULL;
-	}
-	dma_release_channel(dma_chan);
 }
 
 static int tegra_uart_startup(struct uart_port *u)
@@ -1060,8 +1098,6 @@ 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)
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index 0b7bb12dfc68..7ae1592f7ec9 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -335,18 +335,29 @@ unsigned int
 uart_get_baud_rate(struct uart_port *port, struct ktermios *termios,
 		   struct ktermios *old, unsigned int min, unsigned int max)
 {
-	unsigned int try, baud, altbaud = 38400;
+	unsigned int try;
+	unsigned int baud;
+	unsigned int altbaud;
 	int hung_up = 0;
 	upf_t flags = port->flags & UPF_SPD_MASK;
 
-	if (flags == UPF_SPD_HI)
+	switch (flags) {
+	case UPF_SPD_HI:
 		altbaud = 57600;
-	else if (flags == UPF_SPD_VHI)
+		break;
+	case UPF_SPD_VHI:
 		altbaud = 115200;
-	else if (flags == UPF_SPD_SHI)
+		break;
+	case UPF_SPD_SHI:
 		altbaud = 230400;
-	else if (flags == UPF_SPD_WARP)
+		break;
+	case UPF_SPD_WARP:
 		altbaud = 460800;
+		break;
+	default:
+		altbaud = 38400;
+		break;
+	}
 
 	for (try = 0; try < 2; try++) {
 		baud = tty_termios_baud_rate(termios);
@@ -894,12 +905,10 @@ static int uart_set_info(struct tty_struct *tty, struct tty_port *port,
 			 * need to rate-limit; it's CAP_SYS_ADMIN only.
 			 */
 			if (uport->flags & UPF_SPD_MASK) {
-				char buf[64];
-
 				dev_notice(uport->dev,
 				       "%s sets custom speed on %s. This is deprecated.\n",
 				      current->comm,
-				      tty_name(port->tty, buf));
+				      tty_name(port->tty));
 			}
 			uart_change_speed(tty, state, NULL);
 		}
@@ -1816,8 +1825,8 @@ uart_get_console(struct uart_port *ports, int nr, struct console *co)
  *	@options: ptr for <options> field; NULL if not present (out)
  *
  *	Decodes earlycon kernel command line parameters of the form
- *	   earlycon=<name>,io|mmio|mmio32,<addr>,<options>
- *	   console=<name>,io|mmio|mmio32,<addr>,<options>
+ *	   earlycon=<name>,io|mmio|mmio32|mmio32be,<addr>,<options>
+ *	   console=<name>,io|mmio|mmio32|mmio32be,<addr>,<options>
  *
  *	The optional form
  *	   earlycon=<name>,0x<addr>,<options>
@@ -1835,6 +1844,9 @@ int uart_parse_earlycon(char *p, unsigned char *iotype, unsigned long *addr,
 	} else if (strncmp(p, "mmio32,", 7) == 0) {
 		*iotype = UPIO_MEM32;
 		p += 7;
+	} else if (strncmp(p, "mmio32be,", 9) == 0) {
+		*iotype = UPIO_MEM32BE;
+		p += 9;
 	} else if (strncmp(p, "io,", 3) == 0) {
 		*iotype = UPIO_PORT;
 		p += 3;
diff --git a/drivers/tty/serial/serial_ks8695.c b/drivers/tty/serial/serial_ks8695.c
index 5c79bdab985d..b4decf8787de 100644
--- a/drivers/tty/serial/serial_ks8695.c
+++ b/drivers/tty/serial/serial_ks8695.c
@@ -328,7 +328,7 @@ static int ks8695uart_startup(struct uart_port *port)
 {
 	int retval;
 
-	set_irq_flags(KS8695_IRQ_UART_TX, IRQF_VALID | IRQF_NOAUTOEN);
+	irq_modify_status(KS8695_IRQ_UART_TX, IRQ_NOREQUEST, IRQ_NOAUTOEN);
 	tx_enable(port, 0);
 	rx_enable(port, 1);
 	ms_enable(port, 1);
diff --git a/drivers/tty/serial/serial_mctrl_gpio.c b/drivers/tty/serial/serial_mctrl_gpio.c
index 0ec756c62bcf..402f7fb54133 100644
--- a/drivers/tty/serial/serial_mctrl_gpio.c
+++ b/drivers/tty/serial/serial_mctrl_gpio.c
@@ -49,13 +49,12 @@ void mctrl_gpio_set(struct mctrl_gpios *gpios, unsigned int mctrl)
 	unsigned int count = 0;
 
 	for (i = 0; i < UART_GPIO_MAX; i++)
-		if (!IS_ERR_OR_NULL(gpios->gpio[i]) &&
-		    mctrl_gpios_desc[i].dir_out) {
+		if (gpios->gpio[i] && 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);
+	gpiod_set_array_value(count, desc_array, value_array);
 }
 EXPORT_SYMBOL_GPL(mctrl_gpio_set);
 
@@ -118,7 +117,7 @@ void mctrl_gpio_free(struct device *dev, struct mctrl_gpios *gpios)
 	enum mctrl_gpio_idx i;
 
 	for (i = 0; i < UART_GPIO_MAX; i++)
-		if (!IS_ERR_OR_NULL(gpios->gpio[i]))
+		if (gpios->gpio[i])
 			devm_gpiod_put(dev, gpios->gpio[i]);
 	devm_kfree(dev, gpios);
 }
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index e7d6566fafaf..1b2f894bdc9e 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -81,10 +81,11 @@ struct sci_port {
 
 	/* Platform configuration */
 	struct plat_sci_port	*cfg;
-	int			overrun_bit;
+	unsigned int		overrun_reg;
+	unsigned int		overrun_mask;
 	unsigned int		error_mask;
 	unsigned int		sampling_rate;
-
+	resource_size_t		reg_size;
 
 	/* Break timer */
 	struct timer_list	break_timer;
@@ -168,6 +169,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= sci_reg_invalid,
 		[SCLSR]		= sci_reg_invalid,
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 
 	/*
@@ -188,6 +191,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= sci_reg_invalid,
 		[SCLSR]		= sci_reg_invalid,
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 
 	/*
@@ -207,6 +212,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= sci_reg_invalid,
 		[SCLSR]		= sci_reg_invalid,
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= { 0x30, 16 },
+		[SCPDR]		= { 0x34, 16 },
 	},
 
 	/*
@@ -226,6 +233,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= sci_reg_invalid,
 		[SCLSR]		= sci_reg_invalid,
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= { 0x30, 16 },
+		[SCPDR]		= { 0x34, 16 },
 	},
 
 	/*
@@ -246,6 +255,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= { 0x20, 16 },
 		[SCLSR]		= { 0x24, 16 },
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 
 	/*
@@ -265,6 +276,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= sci_reg_invalid,
 		[SCLSR]		= sci_reg_invalid,
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 
 	/*
@@ -284,6 +297,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= { 0x20, 16 },
 		[SCLSR]		= { 0x24, 16 },
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 
 	/*
@@ -303,6 +318,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= { 0x20, 16 },
 		[SCLSR]		= { 0x24, 16 },
 		[HSSRR]		= { 0x40, 16 },
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 
 	/*
@@ -323,6 +340,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= sci_reg_invalid,
 		[SCLSR]		= { 0x24, 16 },
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 
 	/*
@@ -343,6 +362,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= { 0x24, 16 },
 		[SCLSR]		= { 0x28, 16 },
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 
 	/*
@@ -363,6 +384,8 @@ static struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[SCSPTR]	= sci_reg_invalid,
 		[SCLSR]		= sci_reg_invalid,
 		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
 	},
 };
 
@@ -781,7 +804,7 @@ static int sci_handle_errors(struct uart_port *port)
 	struct sci_port *s = to_sci_port(port);
 
 	/* Handle overruns */
-	if (status & (1 << s->overrun_bit)) {
+	if (status & s->overrun_mask) {
 		port->icount.overrun++;
 
 		/* overrun error */
@@ -844,32 +867,17 @@ static int sci_handle_fifo_overrun(struct uart_port *port)
 	struct tty_port *tport = &port->state->port;
 	struct sci_port *s = to_sci_port(port);
 	struct plat_sci_reg *reg;
-	int copied = 0, offset;
-	u16 status, bit;
-
-	switch (port->type) {
-	case PORT_SCIF:
-	case PORT_HSCIF:
-		offset = SCLSR;
-		break;
-	case PORT_SCIFA:
-	case PORT_SCIFB:
-		offset = SCxSR;
-		break;
-	default:
-		return 0;
-	}
+	int copied = 0;
+	u16 status;
 
-	reg = sci_getreg(port, offset);
+	reg = sci_getreg(port, s->overrun_reg);
 	if (!reg->size)
 		return 0;
 
-	status = serial_port_in(port, offset);
-	bit = 1 << s->overrun_bit;
-
-	if (status & bit) {
-		status &= ~bit;
-		serial_port_out(port, offset, status);
+	status = serial_port_in(port, s->overrun_reg);
+	if (status & s->overrun_mask) {
+		status &= ~s->overrun_mask;
+		serial_port_out(port, s->overrun_reg, status);
 
 		port->icount.overrun++;
 
@@ -1021,15 +1029,11 @@ static irqreturn_t sci_mpxed_interrupt(int irq, void *ptr)
 
 	ssr_status = serial_port_in(port, SCxSR);
 	scr_status = serial_port_in(port, SCSCR);
-	switch (port->type) {
-	case PORT_SCIF:
-	case PORT_HSCIF:
-		orer_status = serial_port_in(port, SCLSR);
-		break;
-	case PORT_SCIFA:
-	case PORT_SCIFB:
+	if (s->overrun_reg == SCxSR)
 		orer_status = ssr_status;
-		break;
+	else {
+		if (sci_getreg(port, s->overrun_reg)->size)
+			orer_status = serial_port_in(port, s->overrun_reg);
 	}
 
 	err_enabled = scr_status & port_rx_irq_mask(port);
@@ -1059,7 +1063,7 @@ static irqreturn_t sci_mpxed_interrupt(int irq, void *ptr)
 		ret = sci_br_interrupt(irq, ptr);
 
 	/* Overrun Interrupt */
-	if (orer_status & (1 << s->overrun_bit))
+	if (orer_status & s->overrun_mask)
 		sci_handle_fifo_overrun(port);
 
 	return ret;
@@ -2073,23 +2077,9 @@ static const char *sci_type(struct uart_port *port)
 	return NULL;
 }
 
-static inline unsigned long sci_port_size(struct uart_port *port)
-{
-	/*
-	 * Pick an arbitrary size that encapsulates all of the base
-	 * registers by default. This can be optimized later, or derived
-	 * from platform resource data at such a time that ports begin to
-	 * behave more erratically.
-	 */
-	if (port->type == PORT_HSCIF)
-		return 96;
-	else
-		return 64;
-}
-
 static int sci_remap_port(struct uart_port *port)
 {
-	unsigned long size = sci_port_size(port);
+	struct sci_port *sport = to_sci_port(port);
 
 	/*
 	 * Nothing to do if there's already an established membase.
@@ -2098,7 +2088,7 @@ static int sci_remap_port(struct uart_port *port)
 		return 0;
 
 	if (port->flags & UPF_IOREMAP) {
-		port->membase = ioremap_nocache(port->mapbase, size);
+		port->membase = ioremap_nocache(port->mapbase, sport->reg_size);
 		if (unlikely(!port->membase)) {
 			dev_err(port->dev, "can't remap port#%d\n", port->line);
 			return -ENXIO;
@@ -2117,23 +2107,28 @@ static int sci_remap_port(struct uart_port *port)
 
 static void sci_release_port(struct uart_port *port)
 {
+	struct sci_port *sport = to_sci_port(port);
+
 	if (port->flags & UPF_IOREMAP) {
 		iounmap(port->membase);
 		port->membase = NULL;
 	}
 
-	release_mem_region(port->mapbase, sci_port_size(port));
+	release_mem_region(port->mapbase, sport->reg_size);
 }
 
 static int sci_request_port(struct uart_port *port)
 {
-	unsigned long size = sci_port_size(port);
 	struct resource *res;
+	struct sci_port *sport = to_sci_port(port);
 	int ret;
 
-	res = request_mem_region(port->mapbase, size, dev_name(port->dev));
-	if (unlikely(res == NULL))
+	res = request_mem_region(port->mapbase, sport->reg_size,
+				 dev_name(port->dev));
+	if (unlikely(res == NULL)) {
+		dev_err(port->dev, "request_mem_region failed.");
 		return -EBUSY;
+	}
 
 	ret = sci_remap_port(port);
 	if (unlikely(ret != 0)) {
@@ -2207,6 +2202,7 @@ static int sci_init_single(struct platform_device *dev,
 		return -ENOMEM;
 
 	port->mapbase = res->start;
+	sci_port->reg_size = resource_size(res);
 
 	for (i = 0; i < ARRAY_SIZE(sci_port->irqs); ++i)
 		sci_port->irqs[i] = platform_get_irq(dev, i);
@@ -2234,32 +2230,38 @@ static int sci_init_single(struct platform_device *dev,
 	switch (p->type) {
 	case PORT_SCIFB:
 		port->fifosize = 256;
-		sci_port->overrun_bit = 9;
+		sci_port->overrun_reg = SCxSR;
+		sci_port->overrun_mask = SCIFA_ORER;
 		sampling_rate = 16;
 		break;
 	case PORT_HSCIF:
 		port->fifosize = 128;
 		sampling_rate = 0;
-		sci_port->overrun_bit = 0;
+		sci_port->overrun_reg = SCLSR;
+		sci_port->overrun_mask = SCLSR_ORER;
 		break;
 	case PORT_SCIFA:
 		port->fifosize = 64;
-		sci_port->overrun_bit = 9;
+		sci_port->overrun_reg = SCxSR;
+		sci_port->overrun_mask = SCIFA_ORER;
 		sampling_rate = 16;
 		break;
 	case PORT_SCIF:
 		port->fifosize = 16;
 		if (p->regtype == SCIx_SH7705_SCIF_REGTYPE) {
-			sci_port->overrun_bit = 9;
+			sci_port->overrun_reg = SCxSR;
+			sci_port->overrun_mask = SCIFA_ORER;
 			sampling_rate = 16;
 		} else {
-			sci_port->overrun_bit = 0;
+			sci_port->overrun_reg = SCLSR;
+			sci_port->overrun_mask = SCLSR_ORER;
 			sampling_rate = 32;
 		}
 		break;
 	default:
 		port->fifosize = 1;
-		sci_port->overrun_bit = 5;
+		sci_port->overrun_reg = SCxSR;
+		sci_port->overrun_mask = SCI_ORER;
 		sampling_rate = 32;
 		break;
 	}
@@ -2305,15 +2307,11 @@ static int sci_init_single(struct platform_device *dev,
 			SCI_DEFAULT_ERROR_MASK : SCIF_DEFAULT_ERROR_MASK;
 
 	/*
-	 * Establish sensible defaults for the overrun detection, unless
-	 * the part has explicitly disabled support for it.
-	 */
-
-	/*
 	 * Make the error mask inclusive of overrun detection, if
 	 * supported.
 	 */
-	sci_port->error_mask |= 1 << sci_port->overrun_bit;
+	if (sci_port->overrun_reg == SCxSR)
+		sci_port->error_mask |= sci_port->overrun_mask;
 
 	port->type		= p->type;
 	port->flags		= UPF_FIXED_PORT | p->flags;
@@ -2536,6 +2534,12 @@ static const struct of_device_id of_sci_match[] = {
 			.regtype = SCIx_HSCIF_REGTYPE,
 		},
 	}, {
+		.compatible = "renesas,sci",
+		.data = &(const struct sci_port_info) {
+			.type = PORT_SCI,
+			.regtype = SCIx_SCI_REGTYPE,
+		},
+	}, {
 		/* Terminator */
 	},
 };
diff --git a/drivers/tty/serial/sh-sci.h b/drivers/tty/serial/sh-sci.h
index d5db81a0a430..3393f67b4e84 100644
--- a/drivers/tty/serial/sh-sci.h
+++ b/drivers/tty/serial/sh-sci.h
@@ -1,7 +1,115 @@
+#include <linux/bitops.h>
 #include <linux/serial_core.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
 
+#define SCI_MAJOR		204
+#define SCI_MINOR_START		8
+
+
+/*
+ * SCI register subset common for all port types.
+ * Not all registers will exist on all parts.
+ */
+enum {
+	SCSMR,				/* Serial Mode Register */
+	SCBRR,				/* Bit Rate Register */
+	SCSCR,				/* Serial Control Register */
+	SCxSR,				/* Serial Status Register */
+	SCFCR,				/* FIFO Control Register */
+	SCFDR,				/* FIFO Data Count Register */
+	SCxTDR,				/* Transmit (FIFO) Data Register */
+	SCxRDR,				/* Receive (FIFO) Data Register */
+	SCLSR,				/* Line Status Register */
+	SCTFDR,				/* Transmit FIFO Data Count Register */
+	SCRFDR,				/* Receive FIFO Data Count Register */
+	SCSPTR,				/* Serial Port Register */
+	HSSRR,				/* Sampling Rate Register */
+	SCPCR,				/* Serial Port Control Register */
+	SCPDR,				/* Serial Port Data Register */
+
+	SCIx_NR_REGS,
+};
+
+
+/* SCSMR (Serial Mode Register) */
+#define SCSMR_CHR	BIT(6)	/* 7-bit Character Length */
+#define SCSMR_PE	BIT(5)	/* Parity Enable */
+#define SCSMR_ODD	BIT(4)	/* Odd Parity */
+#define SCSMR_STOP	BIT(3)	/* Stop Bit Length */
+#define SCSMR_CKS	0x0003	/* Clock Select */
+
+/* Serial Control Register, SCIFA/SCIFB only bits */
+#define SCSCR_TDRQE	BIT(15)	/* Tx Data Transfer Request Enable */
+#define SCSCR_RDRQE	BIT(14)	/* Rx Data Transfer Request Enable */
+
+/* SCxSR (Serial Status Register) on SCI */
+#define SCI_TDRE	BIT(7)	/* Transmit Data Register Empty */
+#define SCI_RDRF	BIT(6)	/* Receive Data Register Full */
+#define SCI_ORER	BIT(5)	/* Overrun Error */
+#define SCI_FER		BIT(4)	/* Framing Error */
+#define SCI_PER		BIT(3)	/* Parity Error */
+#define SCI_TEND	BIT(2)	/* Transmit End */
+#define SCI_RESERVED	0x03	/* All reserved bits */
+
+#define SCI_DEFAULT_ERROR_MASK (SCI_PER | SCI_FER)
+
+#define SCI_RDxF_CLEAR	~(SCI_RESERVED | SCI_RDRF)
+#define SCI_ERROR_CLEAR	~(SCI_RESERVED | SCI_PER | SCI_FER | SCI_ORER)
+#define SCI_TDxE_CLEAR	~(SCI_RESERVED | SCI_TEND | SCI_TDRE)
+#define SCI_BREAK_CLEAR	~(SCI_RESERVED | SCI_PER | SCI_FER | SCI_ORER)
+
+/* SCxSR (Serial Status Register) on SCIF, SCIFA, SCIFB, HSCIF */
+#define SCIF_ER		BIT(7)	/* Receive Error */
+#define SCIF_TEND	BIT(6)	/* Transmission End */
+#define SCIF_TDFE	BIT(5)	/* Transmit FIFO Data Empty */
+#define SCIF_BRK	BIT(4)	/* Break Detect */
+#define SCIF_FER	BIT(3)	/* Framing Error */
+#define SCIF_PER	BIT(2)	/* Parity Error */
+#define SCIF_RDF	BIT(1)	/* Receive FIFO Data Full */
+#define SCIF_DR		BIT(0)	/* Receive Data Ready */
+/* SCIF only (optional) */
+#define SCIF_PERC	0xf000	/* Number of Parity Errors */
+#define SCIF_FERC	0x0f00	/* Number of Framing Errors */
+/*SCIFA/SCIFB and SCIF on SH7705/SH7720/SH7721 only */
+#define SCIFA_ORER	BIT(9)	/* Overrun Error */
+
+#define SCIF_DEFAULT_ERROR_MASK (SCIF_PER | SCIF_FER | SCIF_BRK | SCIF_ER)
+
+#define SCIF_RDxF_CLEAR		~(SCIF_DR | SCIF_RDF)
+#define SCIF_ERROR_CLEAR	~(SCIFA_ORER | SCIF_PER | SCIF_FER | SCIF_ER)
+#define SCIF_TDxE_CLEAR		~(SCIF_TDFE)
+#define SCIF_BREAK_CLEAR	~(SCIF_PER | SCIF_FER | SCIF_BRK)
+
+/* SCFCR (FIFO Control Register) */
+#define SCFCR_MCE	BIT(3)	/* Modem Control Enable */
+#define SCFCR_TFRST	BIT(2)	/* Transmit FIFO Data Register Reset */
+#define SCFCR_RFRST	BIT(1)	/* Receive FIFO Data Register Reset */
+#define SCFCR_LOOP	BIT(0)	/* Loopback Test */
+
+/* SCLSR (Line Status Register) on (H)SCIF */
+#define SCLSR_ORER	BIT(0)	/* Overrun Error */
+
+/* SCSPTR (Serial Port Register), optional */
+#define SCSPTR_RTSIO	BIT(7)	/* Serial Port RTS Pin Input/Output */
+#define SCSPTR_RTSDT	BIT(6)	/* Serial Port RTS Pin Data */
+#define SCSPTR_CTSIO	BIT(5)	/* Serial Port CTS Pin Input/Output */
+#define SCSPTR_CTSDT	BIT(4)	/* Serial Port CTS Pin Data */
+#define SCSPTR_SPB2IO	BIT(1)	/* Serial Port Break Input/Output */
+#define SCSPTR_SPB2DT	BIT(0)	/* Serial Port Break Data */
+
+/* HSSRR HSCIF */
+#define HSCIF_SRE	BIT(15)	/* Sampling Rate Register Enable */
+
+/* SCPCR (Serial Port Control Register), SCIFA/SCIFB only */
+#define SCPCR_RTSC	BIT(4)	/* Serial Port RTS Pin / Output Pin */
+#define SCPCR_CTSC	BIT(3)	/* Serial Port CTS Pin / Input Pin */
+
+/* SCPDR (Serial Port Data Register), SCIFA/SCIFB only */
+#define SCPDR_RTSD	BIT(4)	/* Serial Port RTS Output Pin Data */
+#define SCPDR_CTSD	BIT(3)	/* Serial Port CTS Input Pin Data */
+
+
 #define SCxSR_TEND(port)	(((port)->type == PORT_SCI) ? SCI_TEND   : SCIF_TEND)
 #define SCxSR_RDxF(port)	(((port)->type == PORT_SCI) ? SCI_RDRF   : SCIF_RDF)
 #define SCxSR_TDxE(port)	(((port)->type == PORT_SCI) ? SCI_TDRE   : SCIF_TDFE)
@@ -15,24 +123,24 @@
     defined(CONFIG_CPU_SUBTYPE_SH7720) || \
     defined(CONFIG_CPU_SUBTYPE_SH7721) || \
     defined(CONFIG_ARCH_SH73A0) || \
-    defined(CONFIG_ARCH_SH7372) || \
     defined(CONFIG_ARCH_R8A7740)
 
-# define SCxSR_RDxF_CLEAR(port)	 (serial_port_in(port, SCxSR) & 0xfffc)
-# define SCxSR_ERROR_CLEAR(port) (serial_port_in(port, SCxSR) & 0xfd73)
-# define SCxSR_TDxE_CLEAR(port)	 (serial_port_in(port, SCxSR) & 0xffdf)
-# define SCxSR_BREAK_CLEAR(port) (serial_port_in(port, SCxSR) & 0xffe3)
+# define SCxSR_RDxF_CLEAR(port) \
+	(serial_port_in(port, SCxSR) & SCIF_RDxF_CLEAR)
+# define SCxSR_ERROR_CLEAR(port) \
+	(serial_port_in(port, SCxSR) & SCIF_ERROR_CLEAR)
+# define SCxSR_TDxE_CLEAR(port) \
+	(serial_port_in(port, SCxSR) & SCIF_TDxE_CLEAR)
+# define SCxSR_BREAK_CLEAR(port) \
+	(serial_port_in(port, SCxSR) & SCIF_BREAK_CLEAR)
 #else
-# define SCxSR_RDxF_CLEAR(port)	 (((port)->type == PORT_SCI) ? 0xbc : 0x00fc)
-# define SCxSR_ERROR_CLEAR(port) (((port)->type == PORT_SCI) ? 0xc4 : 0x0073)
-# define SCxSR_TDxE_CLEAR(port)  (((port)->type == PORT_SCI) ? 0x78 : 0x00df)
-# define SCxSR_BREAK_CLEAR(port) (((port)->type == PORT_SCI) ? 0xc4 : 0x00e3)
+# define SCxSR_RDxF_CLEAR(port) \
+	((((port)->type == PORT_SCI) ? SCI_RDxF_CLEAR : SCIF_RDxF_CLEAR) & 0xff)
+# define SCxSR_ERROR_CLEAR(port) \
+	((((port)->type == PORT_SCI) ? SCI_ERROR_CLEAR : SCIF_ERROR_CLEAR) & 0xff)
+# define SCxSR_TDxE_CLEAR(port) \
+	((((port)->type == PORT_SCI) ? SCI_TDxE_CLEAR : SCIF_TDxE_CLEAR) & 0xff)
+# define SCxSR_BREAK_CLEAR(port) \
+	((((port)->type == PORT_SCI) ? SCI_BREAK_CLEAR : SCIF_BREAK_CLEAR) & 0xff)
 #endif
 
-/* SCFCR */
-#define SCFCR_RFRST 0x0002
-#define SCFCR_TFRST 0x0004
-#define SCFCR_MCE   0x0008
-
-#define SCI_MAJOR		204
-#define SCI_MINOR_START		8
diff --git a/drivers/tty/serial/sirfsoc_uart.c b/drivers/tty/serial/sirfsoc_uart.c
index 9de3eabe5737..653cdd5fb508 100644
--- a/drivers/tty/serial/sirfsoc_uart.c
+++ b/drivers/tty/serial/sirfsoc_uart.c
@@ -36,8 +36,6 @@ sirfsoc_uart_pio_rx_chars(struct uart_port *port, unsigned int max_rx_count);
 static struct uart_driver sirfsoc_uart_drv;
 
 static void sirfsoc_uart_tx_dma_complete_callback(void *param);
-static void sirfsoc_uart_start_next_rx_dma(struct uart_port *port);
-static void sirfsoc_uart_rx_dma_complete_callback(void *param);
 static const struct sirfsoc_baudrate_to_regv baudrate_to_regv[] = {
 	{4000000, 2359296},
 	{3500000, 1310721},
@@ -59,50 +57,7 @@ static const struct sirfsoc_baudrate_to_regv baudrate_to_regv[] = {
 	{9600, 1114979},
 };
 
-static struct sirfsoc_uart_port sirfsoc_uart_ports[SIRFSOC_UART_NR] = {
-	[0] = {
-		.port = {
-			.iotype		= UPIO_MEM,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 0,
-		},
-	},
-	[1] = {
-		.port = {
-			.iotype		= UPIO_MEM,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 1,
-		},
-	},
-	[2] = {
-		.port = {
-			.iotype		= UPIO_MEM,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 2,
-		},
-	},
-	[3] = {
-		.port = {
-			.iotype		= UPIO_MEM,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 3,
-		},
-	},
-	[4] = {
-		.port = {
-			.iotype		= UPIO_MEM,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 4,
-		},
-	},
-	[5] = {
-		.port = {
-			.iotype		= UPIO_MEM,
-			.flags		= UPF_BOOT_AUTOCONF,
-			.line		= 5,
-		},
-	},
-};
+static struct sirfsoc_uart_port *sirf_ports[SIRFSOC_UART_NR];
 
 static inline struct sirfsoc_uart_port *to_sirfport(struct uart_port *port)
 {
@@ -116,8 +71,7 @@ static inline unsigned int sirfsoc_uart_tx_empty(struct uart_port *port)
 	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
 	struct sirfsoc_fifo_status *ufifo_st = &sirfport->uart_reg->fifo_status;
 	reg = rd_regl(port, ureg->sirfsoc_tx_fifo_status);
-
-	return (reg & ufifo_st->ff_empty(port->line)) ? TIOCSER_TEMT : 0;
+	return (reg & ufifo_st->ff_empty(port)) ? TIOCSER_TEMT : 0;
 }
 
 static unsigned int sirfsoc_uart_get_mctrl(struct uart_port *port)
@@ -152,6 +106,26 @@ static void sirfsoc_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	unsigned int val = assert ? SIRFUART_AFC_CTRL_RX_THD : 0x0;
 	unsigned int current_val;
 
+	if (mctrl & TIOCM_LOOP) {
+		if (sirfport->uart_reg->uart_type == SIRF_REAL_UART)
+			wr_regl(port, ureg->sirfsoc_line_ctrl,
+				rd_regl(port, ureg->sirfsoc_line_ctrl) |
+				SIRFUART_LOOP_BACK);
+		else
+			wr_regl(port, ureg->sirfsoc_mode1,
+				rd_regl(port, ureg->sirfsoc_mode1) |
+				SIRFSOC_USP_LOOP_BACK_CTRL);
+	} else {
+		if (sirfport->uart_reg->uart_type == SIRF_REAL_UART)
+			wr_regl(port, ureg->sirfsoc_line_ctrl,
+				rd_regl(port, ureg->sirfsoc_line_ctrl) &
+				~SIRFUART_LOOP_BACK);
+		else
+			wr_regl(port, ureg->sirfsoc_mode1,
+				rd_regl(port, ureg->sirfsoc_mode1) &
+				~SIRFSOC_USP_LOOP_BACK_CTRL);
+	}
+
 	if (!sirfport->hw_flow_ctrl || !sirfport->ms_enabled)
 		return;
 	if (sirfport->uart_reg->uart_type == SIRF_REAL_UART) {
@@ -182,16 +156,19 @@ static void sirfsoc_uart_stop_tx(struct uart_port *port)
 				rd_regl(port, ureg->sirfsoc_int_en_reg) &
 				~uint_en->sirfsoc_txfifo_empty_en);
 			else
-				wr_regl(port, SIRFUART_INT_EN_CLR,
+				wr_regl(port, ureg->sirfsoc_int_en_clr_reg,
 				uint_en->sirfsoc_txfifo_empty_en);
 		}
 	} else {
+		if (sirfport->uart_reg->uart_type == SIRF_USP_UART)
+			wr_regl(port, ureg->sirfsoc_tx_rx_en, rd_regl(port,
+				ureg->sirfsoc_tx_rx_en) & ~SIRFUART_TX_EN);
 		if (!sirfport->is_atlas7)
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
 				rd_regl(port, ureg->sirfsoc_int_en_reg) &
 				~uint_en->sirfsoc_txfifo_empty_en);
 		else
-			wr_regl(port, SIRFUART_INT_EN_CLR,
+			wr_regl(port, ureg->sirfsoc_int_en_clr_reg,
 				uint_en->sirfsoc_txfifo_empty_en);
 	}
 }
@@ -222,7 +199,7 @@ static void sirfsoc_uart_tx_with_dma(struct sirfsoc_uart_port *sirfport)
 				rd_regl(port, ureg->sirfsoc_int_en_reg)&
 				~(uint_en->sirfsoc_txfifo_empty_en));
 	else
-		wr_regl(port, SIRFUART_INT_EN_CLR,
+		wr_regl(port, ureg->sirfsoc_int_en_clr_reg,
 				uint_en->sirfsoc_txfifo_empty_en);
 	/*
 	 * DMA requires buffer address and buffer length are both aligned with
@@ -290,8 +267,11 @@ static void sirfsoc_uart_start_tx(struct uart_port *port)
 	if (sirfport->tx_dma_chan)
 		sirfsoc_uart_tx_with_dma(sirfport);
 	else {
-		sirfsoc_uart_pio_tx_chars(sirfport,
-			SIRFSOC_UART_IO_TX_REASONABLE_CNT);
+		if (sirfport->uart_reg->uart_type == SIRF_USP_UART)
+			wr_regl(port, ureg->sirfsoc_tx_rx_en, rd_regl(port,
+				ureg->sirfsoc_tx_rx_en) | SIRFUART_TX_EN);
+		wr_regl(port, ureg->sirfsoc_tx_fifo_op, SIRFUART_FIFO_STOP);
+		sirfsoc_uart_pio_tx_chars(sirfport, port->fifosize);
 		wr_regl(port, ureg->sirfsoc_tx_fifo_op, SIRFUART_FIFO_START);
 		if (!sirfport->is_atlas7)
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
@@ -314,21 +294,25 @@ static void sirfsoc_uart_stop_rx(struct uart_port *port)
 		if (!sirfport->is_atlas7)
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
 				rd_regl(port, ureg->sirfsoc_int_en_reg) &
-				~(SIRFUART_RX_DMA_INT_EN(port, uint_en) |
+				~(SIRFUART_RX_DMA_INT_EN(uint_en,
+				sirfport->uart_reg->uart_type) |
 				uint_en->sirfsoc_rx_done_en));
 		else
-			wr_regl(port, SIRFUART_INT_EN_CLR,
-					SIRFUART_RX_DMA_INT_EN(port, uint_en)|
-					uint_en->sirfsoc_rx_done_en);
+			wr_regl(port, ureg->sirfsoc_int_en_clr_reg,
+				SIRFUART_RX_DMA_INT_EN(uint_en,
+				sirfport->uart_reg->uart_type)|
+				uint_en->sirfsoc_rx_done_en);
 		dmaengine_terminate_all(sirfport->rx_dma_chan);
 	} else {
 		if (!sirfport->is_atlas7)
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
 				rd_regl(port, ureg->sirfsoc_int_en_reg)&
-				~(SIRFUART_RX_IO_INT_EN(port, uint_en)));
+				~(SIRFUART_RX_IO_INT_EN(uint_en,
+				sirfport->uart_reg->uart_type)));
 		else
-			wr_regl(port, SIRFUART_INT_EN_CLR,
-					SIRFUART_RX_IO_INT_EN(port, uint_en));
+			wr_regl(port, ureg->sirfsoc_int_en_clr_reg,
+				SIRFUART_RX_IO_INT_EN(uint_en,
+				sirfport->uart_reg->uart_type));
 	}
 }
 
@@ -349,7 +333,7 @@ static void sirfsoc_uart_disable_ms(struct uart_port *port)
 					rd_regl(port, ureg->sirfsoc_int_en_reg)&
 					~uint_en->sirfsoc_cts_en);
 		else
-			wr_regl(port, SIRFUART_INT_EN_CLR,
+			wr_regl(port, ureg->sirfsoc_int_en_clr_reg,
 					uint_en->sirfsoc_cts_en);
 	} else
 		disable_irq(gpio_to_irq(sirfport->cts_gpio));
@@ -379,7 +363,8 @@ static void sirfsoc_uart_enable_ms(struct uart_port *port)
 	if (sirfport->uart_reg->uart_type == SIRF_REAL_UART) {
 		wr_regl(port, ureg->sirfsoc_afc_ctrl,
 				rd_regl(port, ureg->sirfsoc_afc_ctrl) |
-				SIRFUART_AFC_TX_EN | SIRFUART_AFC_RX_EN);
+				SIRFUART_AFC_TX_EN | SIRFUART_AFC_RX_EN |
+				SIRFUART_AFC_CTRL_RX_THD);
 		if (!sirfport->is_atlas7)
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
 					rd_regl(port, ureg->sirfsoc_int_en_reg)
@@ -417,7 +402,7 @@ sirfsoc_uart_pio_rx_chars(struct uart_port *port, unsigned int max_rx_count)
 	if (!tty)
 		return -ENODEV;
 	while (!(rd_regl(port, ureg->sirfsoc_rx_fifo_status) &
-					ufifo_st->ff_empty(port->line))) {
+					ufifo_st->ff_empty(port))) {
 		ch = rd_regl(port, ureg->sirfsoc_rx_fifo_data) |
 			SIRFUART_DUMMY_READ;
 		if (unlikely(uart_handle_sysrq_char(port, ch)))
@@ -444,7 +429,7 @@ sirfsoc_uart_pio_tx_chars(struct sirfsoc_uart_port *sirfport, int count)
 	unsigned int num_tx = 0;
 	while (!uart_circ_empty(xmit) &&
 		!(rd_regl(port, ureg->sirfsoc_tx_fifo_status) &
-					ufifo_st->ff_full(port->line)) &&
+					ufifo_st->ff_full(port)) &&
 		count--) {
 		wr_regl(port, ureg->sirfsoc_tx_fifo_data,
 				xmit->buf[xmit->tail]);
@@ -478,139 +463,6 @@ static void sirfsoc_uart_tx_dma_complete_callback(void *param)
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
-static void sirfsoc_uart_insert_rx_buf_to_tty(
-		struct sirfsoc_uart_port *sirfport, int count)
-{
-	struct uart_port *port = &sirfport->port;
-	struct tty_port *tport = &port->state->port;
-	int inserted;
-
-	inserted = tty_insert_flip_string(tport,
-		sirfport->rx_dma_items[sirfport->rx_completed].xmit.buf, count);
-	port->icount.rx += inserted;
-}
-
-static void sirfsoc_rx_submit_one_dma_desc(struct uart_port *port, int index)
-{
-	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
-
-	sirfport->rx_dma_items[index].xmit.tail =
-		sirfport->rx_dma_items[index].xmit.head = 0;
-	sirfport->rx_dma_items[index].desc =
-		dmaengine_prep_slave_single(sirfport->rx_dma_chan,
-		sirfport->rx_dma_items[index].dma_addr, SIRFSOC_RX_DMA_BUF_SIZE,
-		DMA_DEV_TO_MEM, DMA_PREP_INTERRUPT);
-	if (!sirfport->rx_dma_items[index].desc) {
-		dev_err(port->dev, "DMA slave single fail\n");
-		return;
-	}
-	sirfport->rx_dma_items[index].desc->callback =
-		sirfsoc_uart_rx_dma_complete_callback;
-	sirfport->rx_dma_items[index].desc->callback_param = sirfport;
-	sirfport->rx_dma_items[index].cookie =
-		dmaengine_submit(sirfport->rx_dma_items[index].desc);
-	dma_async_issue_pending(sirfport->rx_dma_chan);
-}
-
-static void sirfsoc_rx_tmo_process_tl(unsigned long param)
-{
-	struct sirfsoc_uart_port *sirfport = (struct sirfsoc_uart_port *)param;
-	struct uart_port *port = &sirfport->port;
-	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
-	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
-	struct sirfsoc_int_status *uint_st = &sirfport->uart_reg->uart_int_st;
-	unsigned int count;
-	unsigned long flags;
-	struct dma_tx_state tx_state;
-
-	spin_lock_irqsave(&port->lock, flags);
-	while (DMA_COMPLETE == dmaengine_tx_status(sirfport->rx_dma_chan,
-		sirfport->rx_dma_items[sirfport->rx_completed].cookie, &tx_state)) {
-		sirfsoc_uart_insert_rx_buf_to_tty(sirfport,
-					SIRFSOC_RX_DMA_BUF_SIZE);
-		sirfport->rx_completed++;
-		sirfport->rx_completed %= SIRFSOC_RX_LOOP_BUF_CNT;
-	}
-	count = CIRC_CNT(sirfport->rx_dma_items[sirfport->rx_issued].xmit.head,
-		sirfport->rx_dma_items[sirfport->rx_issued].xmit.tail,
-		SIRFSOC_RX_DMA_BUF_SIZE);
-	if (count > 0)
-		sirfsoc_uart_insert_rx_buf_to_tty(sirfport, count);
-	wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
-			rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) |
-			SIRFUART_IO_MODE);
-	sirfsoc_uart_pio_rx_chars(port, 4 - sirfport->rx_io_count);
-	if (sirfport->rx_io_count == 4) {
-		sirfport->rx_io_count = 0;
-		wr_regl(port, ureg->sirfsoc_int_st_reg,
-				uint_st->sirfsoc_rx_done);
-		if (!sirfport->is_atlas7)
-			wr_regl(port, ureg->sirfsoc_int_en_reg,
-				rd_regl(port, ureg->sirfsoc_int_en_reg) &
-				~(uint_en->sirfsoc_rx_done_en));
-		else
-			wr_regl(port, SIRFUART_INT_EN_CLR,
-					uint_en->sirfsoc_rx_done_en);
-		sirfsoc_uart_start_next_rx_dma(port);
-	} else {
-		wr_regl(port, ureg->sirfsoc_int_st_reg,
-				uint_st->sirfsoc_rx_done);
-		if (!sirfport->is_atlas7)
-			wr_regl(port, ureg->sirfsoc_int_en_reg,
-				rd_regl(port, ureg->sirfsoc_int_en_reg) |
-				(uint_en->sirfsoc_rx_done_en));
-		else
-			wr_regl(port, ureg->sirfsoc_int_en_reg,
-					uint_en->sirfsoc_rx_done_en);
-	}
-	spin_unlock_irqrestore(&port->lock, flags);
-	tty_flip_buffer_push(&port->state->port);
-}
-
-static void sirfsoc_uart_handle_rx_tmo(struct sirfsoc_uart_port *sirfport)
-{
-	struct uart_port *port = &sirfport->port;
-	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
-	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
-	struct dma_tx_state tx_state;
-	dmaengine_tx_status(sirfport->rx_dma_chan,
-		sirfport->rx_dma_items[sirfport->rx_issued].cookie, &tx_state);
-	dmaengine_terminate_all(sirfport->rx_dma_chan);
-	sirfport->rx_dma_items[sirfport->rx_issued].xmit.head =
-		SIRFSOC_RX_DMA_BUF_SIZE - tx_state.residue;
-	if (!sirfport->is_atlas7)
-		wr_regl(port, ureg->sirfsoc_int_en_reg,
-			rd_regl(port, ureg->sirfsoc_int_en_reg) &
-			~(uint_en->sirfsoc_rx_timeout_en));
-	else
-		wr_regl(port, SIRFUART_INT_EN_CLR,
-				uint_en->sirfsoc_rx_timeout_en);
-	tasklet_schedule(&sirfport->rx_tmo_process_tasklet);
-}
-
-static void sirfsoc_uart_handle_rx_done(struct sirfsoc_uart_port *sirfport)
-{
-	struct uart_port *port = &sirfport->port;
-	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
-	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
-	struct sirfsoc_int_status *uint_st = &sirfport->uart_reg->uart_int_st;
-
-	sirfsoc_uart_pio_rx_chars(port, 4 - sirfport->rx_io_count);
-	if (sirfport->rx_io_count == 4) {
-		sirfport->rx_io_count = 0;
-		if (!sirfport->is_atlas7)
-			wr_regl(port, ureg->sirfsoc_int_en_reg,
-				rd_regl(port, ureg->sirfsoc_int_en_reg) &
-				~(uint_en->sirfsoc_rx_done_en));
-		else
-			wr_regl(port, SIRFUART_INT_EN_CLR,
-					uint_en->sirfsoc_rx_done_en);
-		wr_regl(port, ureg->sirfsoc_int_st_reg,
-				uint_st->sirfsoc_rx_timeout);
-		sirfsoc_uart_start_next_rx_dma(port);
-	}
-}
-
 static irqreturn_t sirfsoc_uart_isr(int irq, void *dev_id)
 {
 	unsigned long intr_status;
@@ -628,20 +480,25 @@ static irqreturn_t sirfsoc_uart_isr(int irq, void *dev_id)
 	intr_status = rd_regl(port, ureg->sirfsoc_int_st_reg);
 	wr_regl(port, ureg->sirfsoc_int_st_reg, intr_status);
 	intr_status &= rd_regl(port, ureg->sirfsoc_int_en_reg);
-	if (unlikely(intr_status & (SIRFUART_ERR_INT_STAT(port, uint_st)))) {
+	if (unlikely(intr_status & (SIRFUART_ERR_INT_STAT(uint_st,
+				sirfport->uart_reg->uart_type)))) {
 		if (intr_status & uint_st->sirfsoc_rxd_brk) {
 			port->icount.brk++;
 			if (uart_handle_break(port))
 				goto recv_char;
 		}
-		if (intr_status & uint_st->sirfsoc_rx_oflow)
+		if (intr_status & uint_st->sirfsoc_rx_oflow) {
 			port->icount.overrun++;
+			flag = TTY_OVERRUN;
+		}
 		if (intr_status & uint_st->sirfsoc_frm_err) {
 			port->icount.frame++;
 			flag = TTY_FRAME;
 		}
-		if (intr_status & uint_st->sirfsoc_parity_err)
+		if (intr_status & uint_st->sirfsoc_parity_err) {
+			port->icount.parity++;
 			flag = TTY_PARITY;
+		}
 		wr_regl(port, ureg->sirfsoc_rx_fifo_op, SIRFUART_FIFO_RESET);
 		wr_regl(port, ureg->sirfsoc_rx_fifo_op, 0);
 		wr_regl(port, ureg->sirfsoc_rx_fifo_op, SIRFUART_FIFO_START);
@@ -662,15 +519,51 @@ recv_char:
 		uart_handle_cts_change(port, cts_status);
 		wake_up_interruptible(&state->port.delta_msr_wait);
 	}
-	if (sirfport->rx_dma_chan) {
-		if (intr_status & uint_st->sirfsoc_rx_timeout)
-			sirfsoc_uart_handle_rx_tmo(sirfport);
-		if (intr_status & uint_st->sirfsoc_rx_done)
-			sirfsoc_uart_handle_rx_done(sirfport);
-	} else {
-		if (intr_status & SIRFUART_RX_IO_INT_ST(uint_st))
-			sirfsoc_uart_pio_rx_chars(port,
-					SIRFSOC_UART_IO_RX_MAX_CNT);
+	if (!sirfport->rx_dma_chan &&
+		(intr_status & SIRFUART_RX_IO_INT_ST(uint_st))) {
+		/*
+		 * chip will trigger continuous RX_TIMEOUT interrupt
+		 * in RXFIFO empty and not trigger if RXFIFO recevice
+		 * data in limit time, original method use RX_TIMEOUT
+		 * will trigger lots of useless interrupt in RXFIFO
+		 * empty.RXFIFO received one byte will trigger RX_DONE
+		 * interrupt.use RX_DONE to wait for data received
+		 * into RXFIFO, use RX_THD/RX_FULL for lots data receive
+		 * and use RX_TIMEOUT for the last left data.
+		 */
+		if (intr_status & uint_st->sirfsoc_rx_done) {
+			if (!sirfport->is_atlas7) {
+				wr_regl(port, ureg->sirfsoc_int_en_reg,
+					rd_regl(port, ureg->sirfsoc_int_en_reg)
+					& ~(uint_en->sirfsoc_rx_done_en));
+				wr_regl(port, ureg->sirfsoc_int_en_reg,
+				rd_regl(port, ureg->sirfsoc_int_en_reg)
+				| (uint_en->sirfsoc_rx_timeout_en));
+			} else {
+				wr_regl(port, ureg->sirfsoc_int_en_clr_reg,
+					uint_en->sirfsoc_rx_done_en);
+				wr_regl(port, ureg->sirfsoc_int_en_reg,
+					uint_en->sirfsoc_rx_timeout_en);
+			}
+		} else {
+			if (intr_status & uint_st->sirfsoc_rx_timeout) {
+				if (!sirfport->is_atlas7) {
+					wr_regl(port, ureg->sirfsoc_int_en_reg,
+					rd_regl(port, ureg->sirfsoc_int_en_reg)
+					& ~(uint_en->sirfsoc_rx_timeout_en));
+					wr_regl(port, ureg->sirfsoc_int_en_reg,
+					rd_regl(port, ureg->sirfsoc_int_en_reg)
+					| (uint_en->sirfsoc_rx_done_en));
+				} else {
+					wr_regl(port,
+						ureg->sirfsoc_int_en_clr_reg,
+						uint_en->sirfsoc_rx_timeout_en);
+					wr_regl(port, ureg->sirfsoc_int_en_reg,
+						uint_en->sirfsoc_rx_done_en);
+				}
+			}
+			sirfsoc_uart_pio_rx_chars(port, port->fifosize);
+		}
 	}
 	spin_unlock(&port->lock);
 	tty_flip_buffer_push(&state->port);
@@ -684,10 +577,10 @@ recv_char:
 				return IRQ_HANDLED;
 			} else {
 				sirfsoc_uart_pio_tx_chars(sirfport,
-					SIRFSOC_UART_IO_TX_REASONABLE_CNT);
+						port->fifosize);
 				if ((uart_circ_empty(xmit)) &&
 				(rd_regl(port, ureg->sirfsoc_tx_fifo_status) &
-				ufifo_st->ff_empty(port->line)))
+				ufifo_st->ff_empty(port)))
 					sirfsoc_uart_stop_tx(port);
 			}
 		}
@@ -697,41 +590,8 @@ recv_char:
 	return IRQ_HANDLED;
 }
 
-static void sirfsoc_uart_rx_dma_complete_tl(unsigned long param)
-{
-	struct sirfsoc_uart_port *sirfport = (struct sirfsoc_uart_port *)param;
-	struct uart_port *port = &sirfport->port;
-	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
-	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
-	unsigned long flags;
-	struct dma_tx_state tx_state;
-	spin_lock_irqsave(&port->lock, flags);
-	while (DMA_COMPLETE == dmaengine_tx_status(sirfport->rx_dma_chan,
-			sirfport->rx_dma_items[sirfport->rx_completed].cookie, &tx_state)) {
-		sirfsoc_uart_insert_rx_buf_to_tty(sirfport,
-					SIRFSOC_RX_DMA_BUF_SIZE);
-		if (rd_regl(port, ureg->sirfsoc_int_en_reg) &
-				uint_en->sirfsoc_rx_timeout_en)
-			sirfsoc_rx_submit_one_dma_desc(port,
-					sirfport->rx_completed++);
-		else
-			sirfport->rx_completed++;
-		sirfport->rx_completed %= SIRFSOC_RX_LOOP_BUF_CNT;
-	}
-	spin_unlock_irqrestore(&port->lock, flags);
-	tty_flip_buffer_push(&port->state->port);
-}
-
 static void sirfsoc_uart_rx_dma_complete_callback(void *param)
 {
-	struct sirfsoc_uart_port *sirfport = (struct sirfsoc_uart_port *)param;
-	unsigned long flags;
-
-	spin_lock_irqsave(&sirfport->port.lock, flags);
-	sirfport->rx_issued++;
-	sirfport->rx_issued %= SIRFSOC_RX_LOOP_BUF_CNT;
-	tasklet_schedule(&sirfport->rx_dma_complete_tasklet);
-	spin_unlock_irqrestore(&sirfport->port.lock, flags);
 }
 
 /* submit rx dma task into dmaengine */
@@ -740,21 +600,36 @@ static void sirfsoc_uart_start_next_rx_dma(struct uart_port *port)
 	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
 	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
 	struct sirfsoc_int_en *uint_en = &sirfport->uart_reg->uart_int_en;
-	int i;
 	sirfport->rx_io_count = 0;
 	wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
 		rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) &
 		~SIRFUART_IO_MODE);
-	for (i = 0; i < SIRFSOC_RX_LOOP_BUF_CNT; i++)
-		sirfsoc_rx_submit_one_dma_desc(port, i);
-	sirfport->rx_completed = sirfport->rx_issued = 0;
+	sirfport->rx_dma_items.xmit.tail =
+		sirfport->rx_dma_items.xmit.head = 0;
+	sirfport->rx_dma_items.desc =
+		dmaengine_prep_dma_cyclic(sirfport->rx_dma_chan,
+		sirfport->rx_dma_items.dma_addr, SIRFSOC_RX_DMA_BUF_SIZE,
+		SIRFSOC_RX_DMA_BUF_SIZE / 2,
+		DMA_DEV_TO_MEM, DMA_PREP_INTERRUPT);
+	if (IS_ERR_OR_NULL(sirfport->rx_dma_items.desc)) {
+		dev_err(port->dev, "DMA slave single fail\n");
+		return;
+	}
+	sirfport->rx_dma_items.desc->callback =
+		sirfsoc_uart_rx_dma_complete_callback;
+	sirfport->rx_dma_items.desc->callback_param = sirfport;
+	sirfport->rx_dma_items.cookie =
+		dmaengine_submit(sirfport->rx_dma_items.desc);
+	dma_async_issue_pending(sirfport->rx_dma_chan);
 	if (!sirfport->is_atlas7)
 		wr_regl(port, ureg->sirfsoc_int_en_reg,
 				rd_regl(port, ureg->sirfsoc_int_en_reg) |
-				SIRFUART_RX_DMA_INT_EN(port, uint_en));
+				SIRFUART_RX_DMA_INT_EN(uint_en,
+				sirfport->uart_reg->uart_type));
 	else
 		wr_regl(port, ureg->sirfsoc_int_en_reg,
-			SIRFUART_RX_DMA_INT_EN(port, uint_en));
+				SIRFUART_RX_DMA_INT_EN(uint_en,
+				sirfport->uart_reg->uart_type));
 }
 
 static void sirfsoc_uart_start_rx(struct uart_port *port)
@@ -773,10 +648,12 @@ static void sirfsoc_uart_start_rx(struct uart_port *port)
 		if (!sirfport->is_atlas7)
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
 				rd_regl(port, ureg->sirfsoc_int_en_reg) |
-				SIRFUART_RX_IO_INT_EN(port, uint_en));
+				SIRFUART_RX_IO_INT_EN(uint_en,
+					sirfport->uart_reg->uart_type));
 		else
 			wr_regl(port, ureg->sirfsoc_int_en_reg,
-				SIRFUART_RX_IO_INT_EN(port, uint_en));
+				SIRFUART_RX_IO_INT_EN(uint_en,
+					sirfport->uart_reg->uart_type));
 	}
 }
 
@@ -789,7 +666,7 @@ sirfsoc_usp_calc_sample_div(unsigned long set_rate,
 	unsigned long ioclk_div = 0;
 	unsigned long temp_delta;
 
-	for (sample_div = SIRF_MIN_SAMPLE_DIV;
+	for (sample_div = SIRF_USP_MIN_SAMPLE_DIV;
 			sample_div <= SIRF_MAX_SAMPLE_DIV; sample_div++) {
 		temp_delta = ioclk_rate -
 		(ioclk_rate + (set_rate * sample_div) / 2)
@@ -910,10 +787,11 @@ static void sirfsoc_uart_set_termios(struct uart_port *port,
 					config_reg |= SIRFUART_STICK_BIT_MARK;
 				else
 					config_reg |= SIRFUART_STICK_BIT_SPACE;
-			} else if (termios->c_cflag & PARODD) {
-				config_reg |= SIRFUART_STICK_BIT_ODD;
 			} else {
-				config_reg |= SIRFUART_STICK_BIT_EVEN;
+				if (termios->c_cflag & PARODD)
+					config_reg |= SIRFUART_STICK_BIT_ODD;
+				else
+					config_reg |= SIRFUART_STICK_BIT_EVEN;
 			}
 		}
 	} else {
@@ -976,7 +854,7 @@ static void sirfsoc_uart_set_termios(struct uart_port *port,
 	wr_regl(port, ureg->sirfsoc_tx_fifo_op,
 			(txfifo_op_reg & ~SIRFUART_FIFO_START));
 	if (sirfport->uart_reg->uart_type == SIRF_REAL_UART) {
-		config_reg |= SIRFUART_RECV_TIMEOUT(port, rx_time_out);
+		config_reg |= SIRFUART_UART_RECV_TIMEOUT(rx_time_out);
 		wr_regl(port, ureg->sirfsoc_line_ctrl, config_reg);
 	} else {
 		/*tx frame ctrl*/
@@ -999,7 +877,7 @@ static void sirfsoc_uart_set_termios(struct uart_port *port,
 		wr_regl(port, ureg->sirfsoc_rx_frame_ctrl, len_val);
 		/*async param*/
 		wr_regl(port, ureg->sirfsoc_async_param_reg,
-			(SIRFUART_RECV_TIMEOUT(port, rx_time_out)) |
+			(SIRFUART_USP_RECV_TIMEOUT(rx_time_out)) |
 			(sample_div_reg & SIRFSOC_USP_ASYNC_DIV2_MASK) <<
 			SIRFSOC_USP_ASYNC_DIV2_OFFSET);
 	}
@@ -1011,6 +889,7 @@ static void sirfsoc_uart_set_termios(struct uart_port *port,
 		wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl, SIRFUART_DMA_MODE);
 	else
 		wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl, SIRFUART_IO_MODE);
+	sirfport->rx_period_time = 20000000;
 	/* Reset Rx/Tx FIFO Threshold level for proper baudrate */
 	if (set_baud < 1000000)
 		threshold_div = 1;
@@ -1032,19 +911,10 @@ 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 (sirfport->is_bt_uart) {
-			clk_prepare_enable(sirfport->clk_noc);
-			clk_prepare_enable(sirfport->clk_general);
-		}
+	if (!state)
 		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)
@@ -1053,7 +923,7 @@ static int sirfsoc_uart_startup(struct uart_port *port)
 	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
 	unsigned int index			= port->line;
 	int ret;
-	set_irq_flags(port->irq, IRQF_VALID | IRQF_NOAUTOEN);
+	irq_modify_status(port->irq, IRQ_NOREQUEST, IRQ_NOAUTOEN);
 	ret = request_irq(port->irq,
 				sirfsoc_uart_isr,
 				0,
@@ -1064,7 +934,6 @@ static int sirfsoc_uart_startup(struct uart_port *port)
 							index, port->irq);
 		goto irq_err;
 	}
-
 	/* initial hardware settings */
 	wr_regl(port, ureg->sirfsoc_tx_dma_io_ctrl,
 		rd_regl(port, ureg->sirfsoc_tx_dma_io_ctrl) |
@@ -1072,6 +941,9 @@ static int sirfsoc_uart_startup(struct uart_port *port)
 	wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
 		rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) |
 		SIRFUART_IO_MODE);
+	wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
+		rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) &
+		~SIRFUART_RX_DMA_FLUSH);
 	wr_regl(port, ureg->sirfsoc_tx_dma_io_len, 0);
 	wr_regl(port, ureg->sirfsoc_rx_dma_io_len, 0);
 	wr_regl(port, ureg->sirfsoc_tx_rx_en, SIRFUART_RX_EN | SIRFUART_TX_EN);
@@ -1080,7 +952,6 @@ static int sirfsoc_uart_startup(struct uart_port *port)
 			SIRFSOC_USP_ENDIAN_CTRL_LSBF |
 			SIRFSOC_USP_EN);
 	wr_regl(port, ureg->sirfsoc_tx_fifo_op, SIRFUART_FIFO_RESET);
-	wr_regl(port, ureg->sirfsoc_tx_fifo_op, 0);
 	wr_regl(port, ureg->sirfsoc_rx_fifo_op, SIRFUART_FIFO_RESET);
 	wr_regl(port, ureg->sirfsoc_rx_fifo_op, 0);
 	wr_regl(port, ureg->sirfsoc_tx_fifo_ctrl, SIRFUART_FIFO_THD(port));
@@ -1100,8 +971,8 @@ static int sirfsoc_uart_startup(struct uart_port *port)
 	sirfport->ms_enabled = false;
 	if (sirfport->uart_reg->uart_type == SIRF_USP_UART &&
 		sirfport->hw_flow_ctrl) {
-		set_irq_flags(gpio_to_irq(sirfport->cts_gpio),
-			IRQF_VALID | IRQF_NOAUTOEN);
+		irq_modify_status(gpio_to_irq(sirfport->cts_gpio),
+			IRQ_NOREQUEST, IRQ_NOAUTOEN);
 		ret = request_irq(gpio_to_irq(sirfport->cts_gpio),
 			sirfsoc_uart_usp_cts_handler, IRQF_TRIGGER_FALLING |
 			IRQF_TRIGGER_RISING, "usp_cts_irq", sirfport);
@@ -1110,8 +981,16 @@ static int sirfsoc_uart_startup(struct uart_port *port)
 			goto init_rx_err;
 		}
 	}
-
 	enable_irq(port->irq);
+	if (sirfport->rx_dma_chan && !sirfport->is_hrt_enabled) {
+		sirfport->is_hrt_enabled = true;
+		sirfport->rx_period_time = 20000000;
+		sirfport->rx_dma_items.xmit.tail =
+			sirfport->rx_dma_items.xmit.head = 0;
+		hrtimer_start(&sirfport->hrt,
+			ns_to_ktime(sirfport->rx_period_time),
+			HRTIMER_MODE_REL);
+	}
 
 	return 0;
 init_rx_err:
@@ -1127,7 +1006,7 @@ static void sirfsoc_uart_shutdown(struct uart_port *port)
 	if (!sirfport->is_atlas7)
 		wr_regl(port, ureg->sirfsoc_int_en_reg, 0);
 	else
-		wr_regl(port, SIRFUART_INT_EN_CLR, ~0UL);
+		wr_regl(port, ureg->sirfsoc_int_en_clr_reg, ~0UL);
 
 	free_irq(port->irq, sirfport);
 	if (sirfport->ms_enabled)
@@ -1139,6 +1018,13 @@ static void sirfsoc_uart_shutdown(struct uart_port *port)
 	}
 	if (sirfport->tx_dma_chan)
 		sirfport->tx_dma_state = TX_DMA_IDLE;
+	if (sirfport->rx_dma_chan && sirfport->is_hrt_enabled) {
+		while ((rd_regl(port, ureg->sirfsoc_rx_fifo_status) &
+			SIRFUART_RX_FIFO_MASK) > 0)
+			;
+		sirfport->is_hrt_enabled = false;
+		hrtimer_cancel(&sirfport->hrt);
+	}
 }
 
 static const char *sirfsoc_uart_type(struct uart_port *port)
@@ -1196,27 +1082,29 @@ sirfsoc_uart_console_setup(struct console *co, char *options)
 	unsigned int bits = 8;
 	unsigned int parity = 'n';
 	unsigned int flow = 'n';
-	struct uart_port *port = &sirfsoc_uart_ports[co->index].port;
-	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
-	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
+	struct sirfsoc_uart_port *sirfport;
+	struct sirfsoc_register *ureg;
 	if (co->index < 0 || co->index >= SIRFSOC_UART_NR)
-		return -EINVAL;
-
-	if (!port->mapbase)
+		co->index = 1;
+	sirfport = sirf_ports[co->index];
+	if (!sirfport)
+		return -ENODEV;
+	ureg = &sirfport->uart_reg->uart_reg;
+	if (!sirfport->port.mapbase)
 		return -ENODEV;
 
 	/* enable usp in mode1 register */
 	if (sirfport->uart_reg->uart_type == SIRF_USP_UART)
-		wr_regl(port, ureg->sirfsoc_mode1, SIRFSOC_USP_EN |
+		wr_regl(&sirfport->port, ureg->sirfsoc_mode1, SIRFSOC_USP_EN |
 				SIRFSOC_USP_ENDIAN_CTRL_LSBF);
 	if (options)
 		uart_parse_options(options, &baud, &parity, &bits, &flow);
-	port->cons = co;
+	sirfport->port.cons = co;
 
 	/* default console tx/rx transfer using io mode */
 	sirfport->rx_dma_chan = NULL;
 	sirfport->tx_dma_chan = NULL;
-	return uart_set_options(port, co, baud, parity, bits, flow);
+	return uart_set_options(&sirfport->port, co, baud, parity, bits, flow);
 }
 
 static void sirfsoc_uart_console_putchar(struct uart_port *port, int ch)
@@ -1224,8 +1112,8 @@ static void sirfsoc_uart_console_putchar(struct uart_port *port, int ch)
 	struct sirfsoc_uart_port *sirfport = to_sirfport(port);
 	struct sirfsoc_register *ureg = &sirfport->uart_reg->uart_reg;
 	struct sirfsoc_fifo_status *ufifo_st = &sirfport->uart_reg->fifo_status;
-	while (rd_regl(port,
-		ureg->sirfsoc_tx_fifo_status) & ufifo_st->ff_full(port->line))
+	while (rd_regl(port, ureg->sirfsoc_tx_fifo_status) &
+		ufifo_st->ff_full(port))
 		cpu_relax();
 	wr_regl(port, ureg->sirfsoc_tx_fifo_data, ch);
 }
@@ -1233,8 +1121,10 @@ static void sirfsoc_uart_console_putchar(struct uart_port *port, int ch)
 static void sirfsoc_uart_console_write(struct console *co, const char *s,
 							unsigned int count)
 {
-	struct uart_port *port = &sirfsoc_uart_ports[co->index].port;
-	uart_console_write(port, s, count, sirfsoc_uart_console_putchar);
+	struct sirfsoc_uart_port *sirfport = sirf_ports[co->index];
+
+	uart_console_write(&sirfport->port, s, count,
+			sirfsoc_uart_console_putchar);
 }
 
 static struct console sirfsoc_uart_console = {
@@ -1269,10 +1159,75 @@ static struct uart_driver sirfsoc_uart_drv = {
 #endif
 };
 
-static const struct of_device_id sirfsoc_uart_ids[] = {
+static enum hrtimer_restart
+	sirfsoc_uart_rx_dma_hrtimer_callback(struct hrtimer *hrt)
+{
+	struct sirfsoc_uart_port *sirfport;
+	struct uart_port *port;
+	int count, inserted;
+	struct dma_tx_state tx_state;
+	struct tty_struct *tty;
+	struct sirfsoc_register *ureg;
+	struct circ_buf *xmit;
+
+	sirfport = container_of(hrt, struct sirfsoc_uart_port, hrt);
+	port = &sirfport->port;
+	inserted = 0;
+	tty = port->state->port.tty;
+	ureg = &sirfport->uart_reg->uart_reg;
+	xmit = &sirfport->rx_dma_items.xmit;
+	dmaengine_tx_status(sirfport->rx_dma_chan,
+		sirfport->rx_dma_items.cookie, &tx_state);
+	xmit->head = SIRFSOC_RX_DMA_BUF_SIZE - tx_state.residue;
+	count = CIRC_CNT_TO_END(xmit->head, xmit->tail,
+			SIRFSOC_RX_DMA_BUF_SIZE);
+	while (count > 0) {
+		inserted = tty_insert_flip_string(tty->port,
+			(const unsigned char *)&xmit->buf[xmit->tail], count);
+		if (!inserted)
+			goto next_hrt;
+		port->icount.rx += inserted;
+		xmit->tail = (xmit->tail + inserted) &
+				(SIRFSOC_RX_DMA_BUF_SIZE - 1);
+		count = CIRC_CNT_TO_END(xmit->head, xmit->tail,
+				SIRFSOC_RX_DMA_BUF_SIZE);
+		tty_flip_buffer_push(tty->port);
+	}
+	/*
+	 * if RX DMA buffer data have all push into tty buffer, and there is
+	 * only little data(less than a dma transfer unit) left in rxfifo,
+	 * fetch it out in pio mode and switch back to dma immediately
+	 */
+	if (!inserted && !count &&
+		((rd_regl(port, ureg->sirfsoc_rx_fifo_status) &
+		SIRFUART_RX_FIFO_MASK) > 0)) {
+		/* switch to pio mode */
+		wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
+			rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) |
+			SIRFUART_IO_MODE);
+		while ((rd_regl(port, ureg->sirfsoc_rx_fifo_status) &
+			SIRFUART_RX_FIFO_MASK) > 0) {
+			if (sirfsoc_uart_pio_rx_chars(port, 16) > 0)
+				tty_flip_buffer_push(tty->port);
+		}
+		wr_regl(port, ureg->sirfsoc_rx_fifo_op, SIRFUART_FIFO_RESET);
+		wr_regl(port, ureg->sirfsoc_rx_fifo_op, 0);
+		wr_regl(port, ureg->sirfsoc_rx_fifo_op, SIRFUART_FIFO_START);
+		/* switch back to dma mode */
+		wr_regl(port, ureg->sirfsoc_rx_dma_io_ctrl,
+			rd_regl(port, ureg->sirfsoc_rx_dma_io_ctrl) &
+			~SIRFUART_IO_MODE);
+	}
+next_hrt:
+	hrtimer_forward_now(hrt, ns_to_ktime(sirfport->rx_period_time));
+	return HRTIMER_RESTART;
+}
+
+static struct of_device_id sirfsoc_uart_ids[] = {
 	{ .compatible = "sirf,prima2-uart", .data = &sirfsoc_uart,},
 	{ .compatible = "sirf,atlas7-uart", .data = &sirfsoc_uart},
 	{ .compatible = "sirf,prima2-usp-uart", .data = &sirfsoc_usp},
+	{ .compatible = "sirf,atlas7-usp-uart", .data = &sirfsoc_usp},
 	{}
 };
 MODULE_DEVICE_TABLE(of, sirfsoc_uart_ids);
@@ -1283,7 +1238,6 @@ static int sirfsoc_uart_probe(struct platform_device *pdev)
 	struct uart_port *port;
 	struct resource *res;
 	int ret;
-	int i, j;
 	struct dma_slave_config slv_cfg = {
 		.src_maxburst = 2,
 	};
@@ -1293,16 +1247,15 @@ static int sirfsoc_uart_probe(struct platform_device *pdev)
 	const struct of_device_id *match;
 
 	match = of_match_node(sirfsoc_uart_ids, pdev->dev.of_node);
-	if (of_property_read_u32(pdev->dev.of_node, "cell-index", &pdev->id)) {
-		dev_err(&pdev->dev,
-			"Unable to find cell-index in uart node.\n");
-		ret = -EFAULT;
+	sirfport = devm_kzalloc(&pdev->dev, sizeof(*sirfport), GFP_KERNEL);
+	if (!sirfport) {
+		ret = -ENOMEM;
 		goto err;
 	}
-	if (of_device_is_compatible(pdev->dev.of_node, "sirf,prima2-usp-uart"))
-		pdev->id += ((struct sirfsoc_uart_register *)
-				match->data)->uart_param.register_uart_nr;
-	sirfport = &sirfsoc_uart_ports[pdev->id];
+	sirfport->port.line = of_alias_get_id(pdev->dev.of_node, "serial");
+	sirf_ports[sirfport->port.line] = sirfport;
+	sirfport->port.iotype = UPIO_MEM;
+	sirfport->port.flags = UPF_BOOT_AUTOCONF;
 	port = &sirfport->port;
 	port->dev = &pdev->dev;
 	port->private_data = sirfport;
@@ -1310,9 +1263,12 @@ static int sirfsoc_uart_probe(struct platform_device *pdev)
 
 	sirfport->hw_flow_ctrl = of_property_read_bool(pdev->dev.of_node,
 		"sirf,uart-has-rtscts");
-	if (of_device_is_compatible(pdev->dev.of_node, "sirf,prima2-uart"))
+	if (of_device_is_compatible(pdev->dev.of_node, "sirf,prima2-uart") ||
+		of_device_is_compatible(pdev->dev.of_node, "sirf,atlas7-uart"))
 		sirfport->uart_reg->uart_type = SIRF_REAL_UART;
-	if (of_device_is_compatible(pdev->dev.of_node, "sirf,prima2-usp-uart")) {
+	if (of_device_is_compatible(pdev->dev.of_node,
+		"sirf,prima2-usp-uart") || of_device_is_compatible(
+		pdev->dev.of_node, "sirf,atlas7-usp-uart")) {
 		sirfport->uart_reg->uart_type =	SIRF_USP_UART;
 		if (!sirfport->hw_flow_ctrl)
 			goto usp_no_flow_control;
@@ -1350,7 +1306,8 @@ static int sirfsoc_uart_probe(struct platform_device *pdev)
 		gpio_direction_output(sirfport->rts_gpio, 1);
 	}
 usp_no_flow_control:
-	if (of_device_is_compatible(pdev->dev.of_node, "sirf,atlas7-uart"))
+	if (of_device_is_compatible(pdev->dev.of_node, "sirf,atlas7-uart") ||
+	    of_device_is_compatible(pdev->dev.of_node, "sirf,atlas7-usp-uart"))
 		sirfport->is_atlas7 = true;
 
 	if (of_property_read_u32(pdev->dev.of_node,
@@ -1368,12 +1325,9 @@ usp_no_flow_control:
 		ret = -EFAULT;
 		goto err;
 	}
-	tasklet_init(&sirfport->rx_dma_complete_tasklet,
-			sirfsoc_uart_rx_dma_complete_tl, (unsigned long)sirfport);
-	tasklet_init(&sirfport->rx_tmo_process_tasklet,
-			sirfsoc_rx_tmo_process_tl, (unsigned long)sirfport);
 	port->mapbase = res->start;
-	port->membase = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	port->membase = devm_ioremap(&pdev->dev,
+			res->start, resource_size(res));
 	if (!port->membase) {
 		dev_err(&pdev->dev, "Cannot remap resource.\n");
 		ret = -ENOMEM;
@@ -1393,20 +1347,6 @@ usp_no_flow_control:
 		goto err;
 	}
 	port->uartclk = clk_get_rate(sirfport->clk);
-	if (of_device_is_compatible(pdev->dev.of_node, "sirf,atlas7-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);
@@ -1419,30 +1359,32 @@ usp_no_flow_control:
 	}
 
 	sirfport->rx_dma_chan = dma_request_slave_channel(port->dev, "rx");
-	for (i = 0; sirfport->rx_dma_chan && i < SIRFSOC_RX_LOOP_BUF_CNT; i++) {
-		sirfport->rx_dma_items[i].xmit.buf =
-			dma_alloc_coherent(port->dev, SIRFSOC_RX_DMA_BUF_SIZE,
-			&sirfport->rx_dma_items[i].dma_addr, GFP_KERNEL);
-		if (!sirfport->rx_dma_items[i].xmit.buf) {
-			dev_err(port->dev, "Uart alloc bufa failed\n");
-			ret = -ENOMEM;
-			goto alloc_coherent_err;
-		}
-		sirfport->rx_dma_items[i].xmit.head =
-			sirfport->rx_dma_items[i].xmit.tail = 0;
+	sirfport->rx_dma_items.xmit.buf =
+		dma_alloc_coherent(port->dev, SIRFSOC_RX_DMA_BUF_SIZE,
+		&sirfport->rx_dma_items.dma_addr, GFP_KERNEL);
+	if (!sirfport->rx_dma_items.xmit.buf) {
+		dev_err(port->dev, "Uart alloc bufa failed\n");
+		ret = -ENOMEM;
+		goto alloc_coherent_err;
 	}
+	sirfport->rx_dma_items.xmit.head =
+		sirfport->rx_dma_items.xmit.tail = 0;
 	if (sirfport->rx_dma_chan)
 		dmaengine_slave_config(sirfport->rx_dma_chan, &slv_cfg);
 	sirfport->tx_dma_chan = dma_request_slave_channel(port->dev, "tx");
 	if (sirfport->tx_dma_chan)
 		dmaengine_slave_config(sirfport->tx_dma_chan, &tx_slv_cfg);
+	if (sirfport->rx_dma_chan) {
+		hrtimer_init(&sirfport->hrt, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+		sirfport->hrt.function = sirfsoc_uart_rx_dma_hrtimer_callback;
+		sirfport->is_hrt_enabled = false;
+	}
 
 	return 0;
 alloc_coherent_err:
-	for (j = 0; j < i; j++)
-		dma_free_coherent(port->dev, SIRFSOC_RX_DMA_BUF_SIZE,
-				sirfport->rx_dma_items[j].xmit.buf,
-				sirfport->rx_dma_items[j].dma_addr);
+	dma_free_coherent(port->dev, SIRFSOC_RX_DMA_BUF_SIZE,
+			sirfport->rx_dma_items.xmit.buf,
+			sirfport->rx_dma_items.dma_addr);
 	dma_release_channel(sirfport->rx_dma_chan);
 err:
 	return ret;
@@ -1454,13 +1396,11 @@ static int sirfsoc_uart_remove(struct platform_device *pdev)
 	struct uart_port *port = &sirfport->port;
 	uart_remove_one_port(&sirfsoc_uart_drv, port);
 	if (sirfport->rx_dma_chan) {
-		int i;
 		dmaengine_terminate_all(sirfport->rx_dma_chan);
 		dma_release_channel(sirfport->rx_dma_chan);
-		for (i = 0; i < SIRFSOC_RX_LOOP_BUF_CNT; i++)
-			dma_free_coherent(port->dev, SIRFSOC_RX_DMA_BUF_SIZE,
-					sirfport->rx_dma_items[i].xmit.buf,
-					sirfport->rx_dma_items[i].dma_addr);
+		dma_free_coherent(port->dev, SIRFSOC_RX_DMA_BUF_SIZE,
+				sirfport->rx_dma_items.xmit.buf,
+				sirfport->rx_dma_items.dma_addr);
 	}
 	if (sirfport->tx_dma_chan) {
 		dmaengine_terminate_all(sirfport->tx_dma_chan);
diff --git a/drivers/tty/serial/sirfsoc_uart.h b/drivers/tty/serial/sirfsoc_uart.h
index 727eb6b88fff..eb162b012eec 100644
--- a/drivers/tty/serial/sirfsoc_uart.h
+++ b/drivers/tty/serial/sirfsoc_uart.h
@@ -6,11 +6,11 @@
  * Licensed under GPLv2 or later.
  */
 #include <linux/bitops.h>
+#include <linux/log2.h>
+#include <linux/hrtimer.h>
 struct sirfsoc_uart_param {
 	const char *uart_name;
 	const char *port_name;
-	u32 uart_nr;
-	u32 register_uart_nr;
 };
 
 struct sirfsoc_register {
@@ -21,6 +21,7 @@ struct sirfsoc_register {
 	u32 sirfsoc_tx_rx_en;
 	u32 sirfsoc_int_en_reg;
 	u32 sirfsoc_int_st_reg;
+	u32 sirfsoc_int_en_clr_reg;
 	u32 sirfsoc_tx_dma_io_ctrl;
 	u32 sirfsoc_tx_dma_io_len;
 	u32 sirfsoc_tx_fifo_ctrl;
@@ -45,8 +46,8 @@ struct sirfsoc_register {
 	u32 sirfsoc_async_param_reg;
 };
 
-typedef u32 (*fifo_full_mask)(int line);
-typedef u32 (*fifo_empty_mask)(int line);
+typedef u32 (*fifo_full_mask)(struct uart_port *port);
+typedef u32 (*fifo_empty_mask)(struct uart_port *port);
 
 struct sirfsoc_fifo_status {
 	fifo_full_mask ff_full;
@@ -105,21 +106,20 @@ struct sirfsoc_uart_register {
 	enum sirfsoc_uart_type uart_type;
 };
 
-u32 usp_ff_full(int line)
+u32 uart_usp_ff_full_mask(struct uart_port *port)
 {
-	return 0x80;
-}
-u32 usp_ff_empty(int line)
-{
-	return 0x100;
-}
-u32 uart_ff_full(int line)
-{
-	return (line == 1) ? (0x20) : (0x80);
+	u32 full_bit;
+
+	full_bit = ilog2(port->fifosize);
+	return (1 << full_bit);
 }
-u32 uart_ff_empty(int line)
+
+u32 uart_usp_ff_empty_mask(struct uart_port *port)
 {
-	return (line == 1) ? (0x40) : (0x100);
+	u32 empty_bit;
+
+	empty_bit = ilog2(port->fifosize) + 1;
+	return (1 << empty_bit);
 }
 struct sirfsoc_uart_register sirfsoc_usp = {
 	.uart_reg = {
@@ -145,6 +145,7 @@ struct sirfsoc_uart_register sirfsoc_usp = {
 		.sirfsoc_rx_fifo_op	= 0x0130,
 		.sirfsoc_rx_fifo_status	= 0x0134,
 		.sirfsoc_rx_fifo_data	= 0x0138,
+		.sirfsoc_int_en_clr_reg = 0x140,
 	},
 	.uart_int_en = {
 		.sirfsoc_rx_done_en	= BIT(0),
@@ -177,14 +178,12 @@ struct sirfsoc_uart_register sirfsoc_usp = {
 		.sirfsoc_rxd_brk	= BIT(15),
 	},
 	.fifo_status = {
-		.ff_full		= usp_ff_full,
-		.ff_empty		= usp_ff_empty,
+		.ff_full		= uart_usp_ff_full_mask,
+		.ff_empty		= uart_usp_ff_empty_mask,
 	},
 	.uart_param = {
 		.uart_name = "ttySiRF",
 		.port_name = "sirfsoc-uart",
-		.uart_nr = 2,
-		.register_uart_nr = 3,
 	},
 };
 
@@ -195,6 +194,7 @@ struct sirfsoc_uart_register sirfsoc_uart = {
 		.sirfsoc_divisor	= 0x0050,
 		.sirfsoc_int_en_reg	= 0x0054,
 		.sirfsoc_int_st_reg	= 0x0058,
+		.sirfsoc_int_en_clr_reg	= 0x0060,
 		.sirfsoc_tx_dma_io_ctrl	= 0x0100,
 		.sirfsoc_tx_dma_io_len	= 0x0104,
 		.sirfsoc_tx_fifo_ctrl	= 0x0108,
@@ -249,14 +249,12 @@ struct sirfsoc_uart_register sirfsoc_uart = {
 		.sirfsoc_rts		= BIT(15),
 	},
 	.fifo_status = {
-		.ff_full		= uart_ff_full,
-		.ff_empty		= uart_ff_empty,
+		.ff_full		= uart_usp_ff_full_mask,
+		.ff_empty		= uart_usp_ff_empty_mask,
 	},
 	.uart_param = {
 		.uart_name = "ttySiRF",
 		.port_name = "sirfsoc_uart",
-		.uart_nr = 3,
-		.register_uart_nr = 0,
 	},
 };
 /* uart io ctrl */
@@ -296,10 +294,10 @@ struct sirfsoc_uart_register sirfsoc_uart = {
 
 #define SIRFUART_IO_MODE			BIT(0)
 #define SIRFUART_DMA_MODE			0x0
+#define SIRFUART_RX_DMA_FLUSH			0x4
 
-/* Macro Specific*/
-#define SIRFUART_INT_EN_CLR                    0x0060
 /* Baud Rate Calculation */
+#define SIRF_USP_MIN_SAMPLE_DIV			0x1
 #define SIRF_MIN_SAMPLE_DIV			0xf
 #define SIRF_MAX_SAMPLE_DIV			0x3f
 #define SIRF_IOCLK_DIV_MAX			0xffff
@@ -326,55 +324,54 @@ struct sirfsoc_uart_register sirfsoc_uart = {
 #define SIRFSOC_USP_RX_CLK_DIVISOR_OFFSET	24
 #define SIRFSOC_USP_ASYNC_DIV2_MASK		0x3f
 #define SIRFSOC_USP_ASYNC_DIV2_OFFSET		16
-
+#define SIRFSOC_USP_LOOP_BACK_CTRL		BIT(2)
 /* USP-UART Common */
 #define SIRFSOC_UART_RX_TIMEOUT(br, to)	(((br) * (((to) + 999) / 1000)) / 1000)
 #define SIRFUART_RECV_TIMEOUT_VALUE(x)	\
 				(((x) > 0xFFFF) ? 0xFFFF : ((x) & 0xFFFF))
-#define SIRFUART_RECV_TIMEOUT(port, x)	\
-		(((port)->line > 2) ? (x & 0xFFFF) : ((x) & 0xFFFF) << 16)
+#define SIRFUART_USP_RECV_TIMEOUT(x)	(x & 0xFFFF)
+#define SIRFUART_UART_RECV_TIMEOUT(x)	((x & 0xFFFF) << 16)
 
-#define SIRFUART_FIFO_THD(port)		((port->line) == 1 ? 16 : 64)
-#define SIRFUART_ERR_INT_STAT(port, unit_st)			\
+#define SIRFUART_FIFO_THD(port)		(port->fifosize >> 1)
+#define SIRFUART_ERR_INT_STAT(unit_st, uart_type)			\
 				(uint_st->sirfsoc_rx_oflow |		\
 				uint_st->sirfsoc_frm_err |		\
 				uint_st->sirfsoc_rxd_brk |		\
-		((port->line > 2) ? 0 : uint_st->sirfsoc_parity_err))
-#define SIRFUART_RX_IO_INT_EN(port, uint_en)				\
-				(uint_en->sirfsoc_rx_timeout_en |\
+				((uart_type != SIRF_REAL_UART) ? \
+				 0 : uint_st->sirfsoc_parity_err))
+#define SIRFUART_RX_IO_INT_EN(uint_en, uart_type)			\
+				(uint_en->sirfsoc_rx_done_en |\
 				 uint_en->sirfsoc_rxfifo_thd_en |\
 				 uint_en->sirfsoc_rxfifo_full_en |\
 				 uint_en->sirfsoc_frm_err_en |\
 				 uint_en->sirfsoc_rx_oflow_en |\
 				 uint_en->sirfsoc_rxd_brk_en |\
-		((port->line > 2) ? 0 : uint_en->sirfsoc_parity_err_en))
+				((uart_type != SIRF_REAL_UART) ? \
+				 0 : uint_en->sirfsoc_parity_err_en))
 #define SIRFUART_RX_IO_INT_ST(uint_st)				\
-				(uint_st->sirfsoc_rx_timeout |\
-				 uint_st->sirfsoc_rxfifo_thd |\
-				 uint_st->sirfsoc_rxfifo_full)
+				(uint_st->sirfsoc_rxfifo_thd |\
+				 uint_st->sirfsoc_rxfifo_full|\
+				 uint_st->sirfsoc_rx_done |\
+				 uint_st->sirfsoc_rx_timeout)
 #define SIRFUART_CTS_INT_ST(uint_st)	(uint_st->sirfsoc_cts)
-#define SIRFUART_RX_DMA_INT_EN(port, uint_en)				\
-				(uint_en->sirfsoc_rx_timeout_en |\
-				 uint_en->sirfsoc_frm_err_en |\
+#define SIRFUART_RX_DMA_INT_EN(uint_en, uart_type)		\
+				(uint_en->sirfsoc_frm_err_en |\
 				 uint_en->sirfsoc_rx_oflow_en |\
 				 uint_en->sirfsoc_rxd_brk_en |\
-		((port->line > 2) ? 0 : uint_en->sirfsoc_parity_err_en))
+				((uart_type != SIRF_REAL_UART) ? \
+				 0 : uint_en->sirfsoc_parity_err_en))
 /* Generic Definitions */
 #define SIRFSOC_UART_NAME			"ttySiRF"
 #define SIRFSOC_UART_MAJOR			0
 #define SIRFSOC_UART_MINOR			0
 #define SIRFUART_PORT_NAME			"sirfsoc-uart"
 #define SIRFUART_MAP_SIZE			0x200
-#define SIRFSOC_UART_NR				6
+#define SIRFSOC_UART_NR				11
 #define SIRFSOC_PORT_TYPE			0xa5
 
 /* Uart Common Use Macro*/
-#define SIRFSOC_RX_DMA_BUF_SIZE	256
+#define SIRFSOC_RX_DMA_BUF_SIZE		(1024 * 32)
 #define BYTES_TO_ALIGN(dma_addr)	((unsigned long)(dma_addr) & 0x3)
-#define LOOP_DMA_BUFA_FILL	1
-#define LOOP_DMA_BUFB_FILL	2
-#define TX_TRAN_PIO		1
-#define TX_TRAN_DMA		2
 /* Uart Fifo Level Chk */
 #define SIRFUART_TX_FIFO_SC_OFFSET	0
 #define SIRFUART_TX_FIFO_LC_OFFSET	10
@@ -389,8 +386,8 @@ struct sirfsoc_uart_register sirfsoc_uart = {
 #define SIRFUART_RX_FIFO_CHK_SC SIRFUART_TX_FIFO_CHK_SC
 #define	SIRFUART_RX_FIFO_CHK_LC SIRFUART_TX_FIFO_CHK_LC
 #define SIRFUART_RX_FIFO_CHK_HC SIRFUART_TX_FIFO_CHK_HC
+#define SIRFUART_RX_FIFO_MASK 0x7f
 /* Indicate how many buffers used */
-#define SIRFSOC_RX_LOOP_BUF_CNT		2
 
 /* For Fast Baud Rate Calculation */
 struct sirfsoc_baudrate_to_regv {
@@ -404,7 +401,7 @@ enum sirfsoc_tx_state {
 	TX_DMA_PAUSE,
 };
 
-struct sirfsoc_loop_buffer {
+struct sirfsoc_rx_buffer {
 	struct circ_buf			xmit;
 	dma_cookie_t			cookie;
 	struct dma_async_tx_descriptor	*desc;
@@ -417,10 +414,6 @@ 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 SiRFatlas7, there are SET/CLR for UART_INT_EN */
 	bool				is_atlas7;
 	struct sirfsoc_uart_register	*uart_reg;
@@ -428,17 +421,16 @@ struct sirfsoc_uart_port {
 	struct dma_chan			*tx_dma_chan;
 	dma_addr_t			tx_dma_addr;
 	struct dma_async_tx_descriptor	*tx_dma_desc;
-	struct tasklet_struct		rx_dma_complete_tasklet;
-	struct tasklet_struct		rx_tmo_process_tasklet;
 	unsigned int			rx_io_count;
 	unsigned long			transfer_size;
 	enum sirfsoc_tx_state		tx_dma_state;
 	unsigned int			cts_gpio;
 	unsigned int			rts_gpio;
 
-	struct sirfsoc_loop_buffer	rx_dma_items[SIRFSOC_RX_LOOP_BUF_CNT];
-	int				rx_completed;
-	int				rx_issued;
+	struct sirfsoc_rx_buffer	rx_dma_items;
+	struct hrtimer			hrt;
+	bool				is_hrt_enabled;
+	unsigned long			rx_period_time;
 };
 
 /* Register Access Control */
@@ -447,10 +439,6 @@ struct sirfsoc_uart_port {
 #define wr_regl(port, reg, val)		__raw_writel(val, portaddr(port, reg))
 
 /* UART Port Mask */
-#define SIRFUART_FIFOLEVEL_MASK(port)	((port->line == 1) ? (0x1f) : (0x7f))
-#define SIRFUART_FIFOFULL_MASK(port)	((port->line == 1) ? (0x20) : (0x80))
-#define SIRFUART_FIFOEMPTY_MASK(port)	((port->line == 1) ? (0x40) : (0x100))
-
-/* I/O Mode */
-#define SIRFSOC_UART_IO_RX_MAX_CNT		256
-#define SIRFSOC_UART_IO_TX_REASONABLE_CNT	256
+#define SIRFUART_FIFOLEVEL_MASK(port)	((port->fifosize - 1) & 0xFFF)
+#define SIRFUART_FIFOFULL_MASK(port)	(port->fifosize & 0xFFF)
+#define SIRFUART_FIFOEMPTY_MASK(port)	((port->fifosize & 0xFFF) << 1)
diff --git a/drivers/tty/serial/sprd_serial.c b/drivers/tty/serial/sprd_serial.c
index 582d2729f700..3866516c2926 100644
--- a/drivers/tty/serial/sprd_serial.c
+++ b/drivers/tty/serial/sprd_serial.c
@@ -716,7 +716,7 @@ static int sprd_probe(struct platform_device *pdev)
 	up->flags = UPF_BOOT_AUTOCONF;
 
 	clk = devm_clk_get(&pdev->dev, NULL);
-	if (!IS_ERR(clk))
+	if (!IS_ERR_OR_NULL(clk))
 		up->uartclk = clk_get_rate(clk);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/tty/serial/stm32-usart.c b/drivers/tty/serial/stm32-usart.c
new file mode 100644
index 000000000000..4a6eab6da63e
--- /dev/null
+++ b/drivers/tty/serial/stm32-usart.c
@@ -0,0 +1,739 @@
+/*
+ * Copyright (C) Maxime Coquelin 2015
+ * Author:  Maxime Coquelin <mcoquelin.stm32@gmail.com>
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Inspired by st-asc.c from STMicroelectronics (c)
+ */
+
+#if defined(CONFIG_SERIAL_STM32_USART_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
+#define SUPPORT_SYSRQ
+#endif
+
+#include <linux/module.h>
+#include <linux/serial.h>
+#include <linux/console.h>
+#include <linux/sysrq.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/serial_core.h>
+#include <linux/clk.h>
+
+#define DRIVER_NAME "stm32-usart"
+
+/* Register offsets */
+#define USART_SR		0x00
+#define USART_DR		0x04
+#define USART_BRR		0x08
+#define USART_CR1		0x0c
+#define USART_CR2		0x10
+#define USART_CR3		0x14
+#define USART_GTPR		0x18
+
+/* USART_SR */
+#define USART_SR_PE		BIT(0)
+#define USART_SR_FE		BIT(1)
+#define USART_SR_NF		BIT(2)
+#define USART_SR_ORE		BIT(3)
+#define USART_SR_IDLE		BIT(4)
+#define USART_SR_RXNE		BIT(5)
+#define USART_SR_TC		BIT(6)
+#define USART_SR_TXE		BIT(7)
+#define USART_SR_LBD		BIT(8)
+#define USART_SR_CTS		BIT(9)
+#define USART_SR_ERR_MASK	(USART_SR_LBD | USART_SR_ORE | \
+				 USART_SR_FE | USART_SR_PE)
+/* Dummy bits */
+#define USART_SR_DUMMY_RX	BIT(16)
+
+/* USART_DR */
+#define USART_DR_MASK		GENMASK(8, 0)
+
+/* USART_BRR */
+#define USART_BRR_DIV_F_MASK	GENMASK(3, 0)
+#define USART_BRR_DIV_M_MASK	GENMASK(15, 4)
+#define USART_BRR_DIV_M_SHIFT	4
+
+/* USART_CR1 */
+#define USART_CR1_SBK		BIT(0)
+#define USART_CR1_RWU		BIT(1)
+#define USART_CR1_RE		BIT(2)
+#define USART_CR1_TE		BIT(3)
+#define USART_CR1_IDLEIE	BIT(4)
+#define USART_CR1_RXNEIE	BIT(5)
+#define USART_CR1_TCIE		BIT(6)
+#define USART_CR1_TXEIE		BIT(7)
+#define USART_CR1_PEIE		BIT(8)
+#define USART_CR1_PS		BIT(9)
+#define USART_CR1_PCE		BIT(10)
+#define USART_CR1_WAKE		BIT(11)
+#define USART_CR1_M		BIT(12)
+#define USART_CR1_UE		BIT(13)
+#define USART_CR1_OVER8		BIT(15)
+#define USART_CR1_IE_MASK	GENMASK(8, 4)
+
+/* USART_CR2 */
+#define USART_CR2_ADD_MASK	GENMASK(3, 0)
+#define USART_CR2_LBDL		BIT(5)
+#define USART_CR2_LBDIE		BIT(6)
+#define USART_CR2_LBCL		BIT(8)
+#define USART_CR2_CPHA		BIT(9)
+#define USART_CR2_CPOL		BIT(10)
+#define USART_CR2_CLKEN		BIT(11)
+#define USART_CR2_STOP_2B	BIT(13)
+#define USART_CR2_STOP_MASK	GENMASK(13, 12)
+#define USART_CR2_LINEN		BIT(14)
+
+/* USART_CR3 */
+#define USART_CR3_EIE		BIT(0)
+#define USART_CR3_IREN		BIT(1)
+#define USART_CR3_IRLP		BIT(2)
+#define USART_CR3_HDSEL		BIT(3)
+#define USART_CR3_NACK		BIT(4)
+#define USART_CR3_SCEN		BIT(5)
+#define USART_CR3_DMAR		BIT(6)
+#define USART_CR3_DMAT		BIT(7)
+#define USART_CR3_RTSE		BIT(8)
+#define USART_CR3_CTSE		BIT(9)
+#define USART_CR3_CTSIE		BIT(10)
+#define USART_CR3_ONEBIT	BIT(11)
+
+/* USART_GTPR */
+#define USART_GTPR_PSC_MASK	GENMASK(7, 0)
+#define USART_GTPR_GT_MASK	GENMASK(15, 8)
+
+#define DRIVER_NAME "stm32-usart"
+#define STM32_SERIAL_NAME "ttyS"
+#define STM32_MAX_PORTS 6
+
+struct stm32_port {
+	struct uart_port port;
+	struct clk *clk;
+	bool hw_flow_control;
+};
+
+static struct stm32_port stm32_ports[STM32_MAX_PORTS];
+static struct uart_driver stm32_usart_driver;
+
+static void stm32_stop_tx(struct uart_port *port);
+
+static inline struct stm32_port *to_stm32_port(struct uart_port *port)
+{
+	return container_of(port, struct stm32_port, port);
+}
+
+static void stm32_set_bits(struct uart_port *port, u32 reg, u32 bits)
+{
+	u32 val;
+
+	val = readl_relaxed(port->membase + reg);
+	val |= bits;
+	writel_relaxed(val, port->membase + reg);
+}
+
+static void stm32_clr_bits(struct uart_port *port, u32 reg, u32 bits)
+{
+	u32 val;
+
+	val = readl_relaxed(port->membase + reg);
+	val &= ~bits;
+	writel_relaxed(val, port->membase + reg);
+}
+
+static void stm32_receive_chars(struct uart_port *port)
+{
+	struct tty_port *tport = &port->state->port;
+	unsigned long c;
+	u32 sr;
+	char flag;
+
+	if (port->irq_wake)
+		pm_wakeup_event(tport->tty->dev, 0);
+
+	while ((sr = readl_relaxed(port->membase + USART_SR)) & USART_SR_RXNE) {
+		sr |= USART_SR_DUMMY_RX;
+		c = readl_relaxed(port->membase + USART_DR);
+		flag = TTY_NORMAL;
+		port->icount.rx++;
+
+		if (sr & USART_SR_ERR_MASK) {
+			if (sr & USART_SR_LBD) {
+				port->icount.brk++;
+				if (uart_handle_break(port))
+					continue;
+			} else if (sr & USART_SR_ORE) {
+				port->icount.overrun++;
+			} else if (sr & USART_SR_PE) {
+				port->icount.parity++;
+			} else if (sr & USART_SR_FE) {
+				port->icount.frame++;
+			}
+
+			sr &= port->read_status_mask;
+
+			if (sr & USART_SR_LBD)
+				flag = TTY_BREAK;
+			else if (sr & USART_SR_PE)
+				flag = TTY_PARITY;
+			else if (sr & USART_SR_FE)
+				flag = TTY_FRAME;
+		}
+
+		if (uart_handle_sysrq_char(port, c))
+			continue;
+		uart_insert_char(port, sr, USART_SR_ORE, c, flag);
+	}
+
+	spin_unlock(&port->lock);
+	tty_flip_buffer_push(tport);
+	spin_lock(&port->lock);
+}
+
+static void stm32_transmit_chars(struct uart_port *port)
+{
+	struct circ_buf *xmit = &port->state->xmit;
+
+	if (port->x_char) {
+		writel_relaxed(port->x_char, port->membase + USART_DR);
+		port->x_char = 0;
+		port->icount.tx++;
+		return;
+	}
+
+	if (uart_tx_stopped(port)) {
+		stm32_stop_tx(port);
+		return;
+	}
+
+	if (uart_circ_empty(xmit)) {
+		stm32_stop_tx(port);
+		return;
+	}
+
+	writel_relaxed(xmit->buf[xmit->tail], port->membase + USART_DR);
+	xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+	port->icount.tx++;
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+
+	if (uart_circ_empty(xmit))
+		stm32_stop_tx(port);
+}
+
+static irqreturn_t stm32_interrupt(int irq, void *ptr)
+{
+	struct uart_port *port = ptr;
+	u32 sr;
+
+	spin_lock(&port->lock);
+
+	sr = readl_relaxed(port->membase + USART_SR);
+
+	if (sr & USART_SR_RXNE)
+		stm32_receive_chars(port);
+
+	if (sr & USART_SR_TXE)
+		stm32_transmit_chars(port);
+
+	spin_unlock(&port->lock);
+
+	return IRQ_HANDLED;
+}
+
+static unsigned int stm32_tx_empty(struct uart_port *port)
+{
+	return readl_relaxed(port->membase + USART_SR) & USART_SR_TXE;
+}
+
+static void stm32_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+	if ((mctrl & TIOCM_RTS) && (port->status & UPSTAT_AUTORTS))
+		stm32_set_bits(port, USART_CR3, USART_CR3_RTSE);
+	else
+		stm32_clr_bits(port, USART_CR3, USART_CR3_RTSE);
+}
+
+static unsigned int stm32_get_mctrl(struct uart_port *port)
+{
+	/* This routine is used to get signals of: DCD, DSR, RI, and CTS */
+	return TIOCM_CAR | TIOCM_DSR | TIOCM_CTS;
+}
+
+/* Transmit stop */
+static void stm32_stop_tx(struct uart_port *port)
+{
+	stm32_clr_bits(port, USART_CR1, USART_CR1_TXEIE);
+}
+
+/* There are probably characters waiting to be transmitted. */
+static void stm32_start_tx(struct uart_port *port)
+{
+	struct circ_buf *xmit = &port->state->xmit;
+
+	if (uart_circ_empty(xmit))
+		return;
+
+	stm32_set_bits(port, USART_CR1, USART_CR1_TXEIE | USART_CR1_TE);
+}
+
+/* Throttle the remote when input buffer is about to overflow. */
+static void stm32_throttle(struct uart_port *port)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+	stm32_clr_bits(port, USART_CR1, USART_CR1_RXNEIE);
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+/* Unthrottle the remote, the input buffer can now accept data. */
+static void stm32_unthrottle(struct uart_port *port)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+	stm32_set_bits(port, USART_CR1, USART_CR1_RXNEIE);
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+/* Receive stop */
+static void stm32_stop_rx(struct uart_port *port)
+{
+	stm32_clr_bits(port, USART_CR1, USART_CR1_RXNEIE);
+}
+
+/* Handle breaks - ignored by us */
+static void stm32_break_ctl(struct uart_port *port, int break_state)
+{
+}
+
+static int stm32_startup(struct uart_port *port)
+{
+	const char *name = to_platform_device(port->dev)->name;
+	u32 val;
+	int ret;
+
+	ret = request_irq(port->irq, stm32_interrupt, IRQF_NO_SUSPEND,
+			  name, port);
+	if (ret)
+		return ret;
+
+	val = USART_CR1_RXNEIE | USART_CR1_TE | USART_CR1_RE;
+	stm32_set_bits(port, USART_CR1, val);
+
+	return 0;
+}
+
+static void stm32_shutdown(struct uart_port *port)
+{
+	u32 val;
+
+	val = USART_CR1_TXEIE | USART_CR1_RXNEIE | USART_CR1_TE | USART_CR1_RE;
+	stm32_set_bits(port, USART_CR1, val);
+
+	free_irq(port->irq, port);
+}
+
+static void stm32_set_termios(struct uart_port *port, struct ktermios *termios,
+			    struct ktermios *old)
+{
+	struct stm32_port *stm32_port = to_stm32_port(port);
+	unsigned int baud;
+	u32 usartdiv, mantissa, fraction, oversampling;
+	tcflag_t cflag = termios->c_cflag;
+	u32 cr1, cr2, cr3;
+	unsigned long flags;
+
+	if (!stm32_port->hw_flow_control)
+		cflag &= ~CRTSCTS;
+
+	baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk / 8);
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	/* Stop serial port and reset value */
+	writel_relaxed(0, port->membase + USART_CR1);
+
+	cr1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE | USART_CR1_RXNEIE;
+	cr2 = 0;
+	cr3 = 0;
+
+	if (cflag & CSTOPB)
+		cr2 |= USART_CR2_STOP_2B;
+
+	if (cflag & PARENB) {
+		cr1 |= USART_CR1_PCE;
+		if ((cflag & CSIZE) == CS8)
+			cr1 |= USART_CR1_M;
+	}
+
+	if (cflag & PARODD)
+		cr1 |= USART_CR1_PS;
+
+	port->status &= ~(UPSTAT_AUTOCTS | UPSTAT_AUTORTS);
+	if (cflag & CRTSCTS) {
+		port->status |= UPSTAT_AUTOCTS | UPSTAT_AUTORTS;
+		cr3 |= USART_CR3_CTSE;
+	}
+
+	usartdiv = DIV_ROUND_CLOSEST(port->uartclk, baud);
+
+	/*
+	 * The USART supports 16 or 8 times oversampling.
+	 * By default we prefer 16 times oversampling, so that the receiver
+	 * has a better tolerance to clock deviations.
+	 * 8 times oversampling is only used to achieve higher speeds.
+	 */
+	if (usartdiv < 16) {
+		oversampling = 8;
+		stm32_set_bits(port, USART_CR1, USART_CR1_OVER8);
+	} else {
+		oversampling = 16;
+		stm32_clr_bits(port, USART_CR1, USART_CR1_OVER8);
+	}
+
+	mantissa = (usartdiv / oversampling) << USART_BRR_DIV_M_SHIFT;
+	fraction = usartdiv % oversampling;
+	writel_relaxed(mantissa | fraction, port->membase + USART_BRR);
+
+	uart_update_timeout(port, cflag, baud);
+
+	port->read_status_mask = USART_SR_ORE;
+	if (termios->c_iflag & INPCK)
+		port->read_status_mask |= USART_SR_PE | USART_SR_FE;
+	if (termios->c_iflag & (IGNBRK | BRKINT | PARMRK))
+		port->read_status_mask |= USART_SR_LBD;
+
+	/* Characters to ignore */
+	port->ignore_status_mask = 0;
+	if (termios->c_iflag & IGNPAR)
+		port->ignore_status_mask = USART_SR_PE | USART_SR_FE;
+	if (termios->c_iflag & IGNBRK) {
+		port->ignore_status_mask |= USART_SR_LBD;
+		/*
+		 * If we're ignoring parity and break indicators,
+		 * ignore overruns too (for real raw support).
+		 */
+		if (termios->c_iflag & IGNPAR)
+			port->ignore_status_mask |= USART_SR_ORE;
+	}
+
+	/* Ignore all characters if CREAD is not set */
+	if ((termios->c_cflag & CREAD) == 0)
+		port->ignore_status_mask |= USART_SR_DUMMY_RX;
+
+	writel_relaxed(cr3, port->membase + USART_CR3);
+	writel_relaxed(cr2, port->membase + USART_CR2);
+	writel_relaxed(cr1, port->membase + USART_CR1);
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static const char *stm32_type(struct uart_port *port)
+{
+	return (port->type == PORT_STM32) ? DRIVER_NAME : NULL;
+}
+
+static void stm32_release_port(struct uart_port *port)
+{
+}
+
+static int stm32_request_port(struct uart_port *port)
+{
+	return 0;
+}
+
+static void stm32_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE)
+		port->type = PORT_STM32;
+}
+
+static int
+stm32_verify_port(struct uart_port *port, struct serial_struct *ser)
+{
+	/* No user changeable parameters */
+	return -EINVAL;
+}
+
+static void stm32_pm(struct uart_port *port, unsigned int state,
+		unsigned int oldstate)
+{
+	struct stm32_port *stm32port = container_of(port,
+			struct stm32_port, port);
+	unsigned long flags = 0;
+
+	switch (state) {
+	case UART_PM_STATE_ON:
+		clk_prepare_enable(stm32port->clk);
+		break;
+	case UART_PM_STATE_OFF:
+		spin_lock_irqsave(&port->lock, flags);
+		stm32_clr_bits(port, USART_CR1, USART_CR1_UE);
+		spin_unlock_irqrestore(&port->lock, flags);
+		clk_disable_unprepare(stm32port->clk);
+		break;
+	}
+}
+
+static const struct uart_ops stm32_uart_ops = {
+	.tx_empty	= stm32_tx_empty,
+	.set_mctrl	= stm32_set_mctrl,
+	.get_mctrl	= stm32_get_mctrl,
+	.stop_tx	= stm32_stop_tx,
+	.start_tx	= stm32_start_tx,
+	.throttle	= stm32_throttle,
+	.unthrottle	= stm32_unthrottle,
+	.stop_rx	= stm32_stop_rx,
+	.break_ctl	= stm32_break_ctl,
+	.startup	= stm32_startup,
+	.shutdown	= stm32_shutdown,
+	.set_termios	= stm32_set_termios,
+	.pm		= stm32_pm,
+	.type		= stm32_type,
+	.release_port	= stm32_release_port,
+	.request_port	= stm32_request_port,
+	.config_port	= stm32_config_port,
+	.verify_port	= stm32_verify_port,
+};
+
+static int stm32_init_port(struct stm32_port *stm32port,
+			  struct platform_device *pdev)
+{
+	struct uart_port *port = &stm32port->port;
+	struct resource *res;
+	int ret;
+
+	port->iotype	= UPIO_MEM;
+	port->flags	= UPF_BOOT_AUTOCONF;
+	port->ops	= &stm32_uart_ops;
+	port->dev	= &pdev->dev;
+	port->irq	= platform_get_irq(pdev, 0);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	port->membase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(port->membase))
+		return PTR_ERR(port->membase);
+	port->mapbase = res->start;
+
+	spin_lock_init(&port->lock);
+
+	stm32port->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(stm32port->clk))
+		return PTR_ERR(stm32port->clk);
+
+	/* Ensure that clk rate is correct by enabling the clk */
+	ret = clk_prepare_enable(stm32port->clk);
+	if (ret)
+		return ret;
+
+	stm32port->port.uartclk = clk_get_rate(stm32port->clk);
+	if (!stm32port->port.uartclk)
+		ret = -EINVAL;
+
+	clk_disable_unprepare(stm32port->clk);
+
+	return ret;
+}
+
+static struct stm32_port *stm32_of_get_stm32_port(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	int id;
+
+	if (!np)
+		return NULL;
+
+	id = of_alias_get_id(np, "serial");
+	if (id < 0)
+		id = 0;
+
+	if (WARN_ON(id >= STM32_MAX_PORTS))
+		return NULL;
+
+	stm32_ports[id].hw_flow_control = of_property_read_bool(np,
+							"auto-flow-control");
+	stm32_ports[id].port.line = id;
+	return &stm32_ports[id];
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id stm32_match[] = {
+	{ .compatible = "st,stm32-usart", },
+	{ .compatible = "st,stm32-uart", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, stm32_match);
+#endif
+
+static int stm32_serial_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct stm32_port *stm32port;
+
+	stm32port = stm32_of_get_stm32_port(pdev);
+	if (!stm32port)
+		return -ENODEV;
+
+	ret = stm32_init_port(stm32port, pdev);
+	if (ret)
+		return ret;
+
+	ret = uart_add_one_port(&stm32_usart_driver, &stm32port->port);
+	if (ret)
+		return ret;
+
+	platform_set_drvdata(pdev, &stm32port->port);
+
+	return 0;
+}
+
+static int stm32_serial_remove(struct platform_device *pdev)
+{
+	struct uart_port *port = platform_get_drvdata(pdev);
+
+	return uart_remove_one_port(&stm32_usart_driver, port);
+}
+
+
+#ifdef CONFIG_SERIAL_STM32_CONSOLE
+static void stm32_console_putchar(struct uart_port *port, int ch)
+{
+	while (!(readl_relaxed(port->membase + USART_SR) & USART_SR_TXE))
+		cpu_relax();
+
+	writel_relaxed(ch, port->membase + USART_DR);
+}
+
+static void stm32_console_write(struct console *co, const char *s, unsigned cnt)
+{
+	struct uart_port *port = &stm32_ports[co->index].port;
+	unsigned long flags;
+	u32 old_cr1, new_cr1;
+	int locked = 1;
+
+	local_irq_save(flags);
+	if (port->sysrq)
+		locked = 0;
+	else if (oops_in_progress)
+		locked = spin_trylock(&port->lock);
+	else
+		spin_lock(&port->lock);
+
+	/* Save and disable interrupts */
+	old_cr1 = readl_relaxed(port->membase + USART_CR1);
+	new_cr1 = old_cr1 & ~USART_CR1_IE_MASK;
+	writel_relaxed(new_cr1, port->membase + USART_CR1);
+
+	uart_console_write(port, s, cnt, stm32_console_putchar);
+
+	/* Restore interrupt state */
+	writel_relaxed(old_cr1, port->membase + USART_CR1);
+
+	if (locked)
+		spin_unlock(&port->lock);
+	local_irq_restore(flags);
+}
+
+static int stm32_console_setup(struct console *co, char *options)
+{
+	struct stm32_port *stm32port;
+	int baud = 9600;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+
+	if (co->index >= STM32_MAX_PORTS)
+		return -ENODEV;
+
+	stm32port = &stm32_ports[co->index];
+
+	/*
+	 * This driver does not support early console initialization
+	 * (use ARM early printk support instead), so we only expect
+	 * this to be called during the uart port registration when the
+	 * driver gets probed and the port should be mapped at that point.
+	 */
+	if (stm32port->port.mapbase == 0 || stm32port->port.membase == NULL)
+		return -ENXIO;
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+
+	return uart_set_options(&stm32port->port, co, baud, parity, bits, flow);
+}
+
+static struct console stm32_console = {
+	.name		= STM32_SERIAL_NAME,
+	.device		= uart_console_device,
+	.write		= stm32_console_write,
+	.setup		= stm32_console_setup,
+	.flags		= CON_PRINTBUFFER,
+	.index		= -1,
+	.data		= &stm32_usart_driver,
+};
+
+#define STM32_SERIAL_CONSOLE (&stm32_console)
+
+#else
+#define STM32_SERIAL_CONSOLE NULL
+#endif /* CONFIG_SERIAL_STM32_CONSOLE */
+
+static struct uart_driver stm32_usart_driver = {
+	.driver_name	= DRIVER_NAME,
+	.dev_name	= STM32_SERIAL_NAME,
+	.major		= 0,
+	.minor		= 0,
+	.nr		= STM32_MAX_PORTS,
+	.cons		= STM32_SERIAL_CONSOLE,
+};
+
+static struct platform_driver stm32_serial_driver = {
+	.probe		= stm32_serial_probe,
+	.remove		= stm32_serial_remove,
+	.driver	= {
+		.name	= DRIVER_NAME,
+		.of_match_table = of_match_ptr(stm32_match),
+	},
+};
+
+static int __init usart_init(void)
+{
+	static char banner[] __initdata = "STM32 USART driver initialized";
+	int ret;
+
+	pr_info("%s\n", banner);
+
+	ret = uart_register_driver(&stm32_usart_driver);
+	if (ret)
+		return ret;
+
+	ret = platform_driver_register(&stm32_serial_driver);
+	if (ret)
+		uart_unregister_driver(&stm32_usart_driver);
+
+	return ret;
+}
+
+static void __exit usart_exit(void)
+{
+	platform_driver_unregister(&stm32_serial_driver);
+	uart_unregister_driver(&stm32_usart_driver);
+}
+
+module_init(usart_init);
+module_exit(usart_exit);
+
+MODULE_ALIAS("platform:" DRIVER_NAME);
+MODULE_DESCRIPTION("STMicroelectronics STM32 serial port driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index 3ddbac767db3..009e0dbc12d2 100644
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -1075,7 +1075,8 @@ static void cdns_uart_console_putchar(struct uart_port *port, int ch)
 	writel(ch, port->membase + CDNS_UART_FIFO_OFFSET);
 }
 
-static void cdns_early_write(struct console *con, const char *s, unsigned n)
+static void __init cdns_early_write(struct console *con, const char *s,
+				    unsigned n)
 {
 	struct earlycon_device *dev = con->data;
 
diff --git a/drivers/tty/synclink.c b/drivers/tty/synclink.c
index b7991707ffc0..2fac7123b274 100644
--- a/drivers/tty/synclink.c
+++ b/drivers/tty/synclink.c
@@ -4410,7 +4410,8 @@ static void synclink_cleanup(void)
 	printk("Unloading %s: %s\n", driver_name, driver_version);
 
 	if (serial_driver) {
-		if ((rc = tty_unregister_driver(serial_driver)))
+		rc = tty_unregister_driver(serial_driver);
+		if (rc)
 			printk("%s(%d) failed to unregister tty driver err=%d\n",
 			       __FILE__,__LINE__,rc);
 		put_tty_driver(serial_driver);
@@ -7751,7 +7752,8 @@ static int hdlcdev_open(struct net_device *dev)
 		printk("%s:hdlcdev_open(%s)\n",__FILE__,dev->name);
 
 	/* generic HDLC layer open processing */
-	if ((rc = hdlc_open(dev)))
+	rc = hdlc_open(dev);
+	if (rc)
 		return rc;
 
 	/* arbitrate between network and tty opens */
@@ -8018,7 +8020,8 @@ static int hdlcdev_init(struct mgsl_struct *info)
 
 	/* allocate and initialize network and HDLC layer objects */
 
-	if (!(dev = alloc_hdlcdev(info))) {
+	dev = alloc_hdlcdev(info);
+	if (!dev) {
 		printk(KERN_ERR "%s:hdlc device allocation failure\n",__FILE__);
 		return -ENOMEM;
 	}
@@ -8039,7 +8042,8 @@ static int hdlcdev_init(struct mgsl_struct *info)
 	hdlc->xmit   = hdlcdev_xmit;
 
 	/* register objects with HDLC layer */
-	if ((rc = register_hdlc_device(dev))) {
+	rc = register_hdlc_device(dev);
+	if (rc) {
 		printk(KERN_WARNING "%s:unable to register hdlc device\n",__FILE__);
 		free_netdev(dev);
 		return rc;
@@ -8075,7 +8079,8 @@ static int synclink_init_one (struct pci_dev *dev,
 		return -EIO;
 	}
 
-	if (!(info = mgsl_allocate_device())) {
+	info = mgsl_allocate_device();
+	if (!info) {
 		printk("can't allocate device instance data.\n");
 		return -EIO;
 	}
diff --git a/drivers/tty/synclink_gt.c b/drivers/tty/synclink_gt.c
index 0e8c39b6ccd4..0ea8eee00178 100644
--- a/drivers/tty/synclink_gt.c
+++ b/drivers/tty/synclink_gt.c
@@ -1539,7 +1539,8 @@ static int hdlcdev_open(struct net_device *dev)
 	DBGINFO(("%s hdlcdev_open\n", dev->name));
 
 	/* generic HDLC layer open processing */
-	if ((rc = hdlc_open(dev)))
+	rc = hdlc_open(dev);
+	if (rc)
 		return rc;
 
 	/* arbitrate between network and tty opens */
@@ -1803,7 +1804,8 @@ static int hdlcdev_init(struct slgt_info *info)
 
 	/* allocate and initialize network and HDLC layer objects */
 
-	if (!(dev = alloc_hdlcdev(info))) {
+	dev = alloc_hdlcdev(info);
+	if (!dev) {
 		printk(KERN_ERR "%s hdlc device alloc failure\n", info->device_name);
 		return -ENOMEM;
 	}
@@ -1824,7 +1826,8 @@ static int hdlcdev_init(struct slgt_info *info)
 	hdlc->xmit   = hdlcdev_xmit;
 
 	/* register objects with HDLC layer */
-	if ((rc = register_hdlc_device(dev))) {
+	rc = register_hdlc_device(dev);
+	if (rc) {
 		printk(KERN_WARNING "%s:unable to register hdlc device\n",__FILE__);
 		free_netdev(dev);
 		return rc;
@@ -1879,7 +1882,8 @@ static void rx_async(struct slgt_info *info)
 
 			stat = 0;
 
-			if ((status = *(p+1) & (BIT1 + BIT0))) {
+			status = *(p + 1) & (BIT1 + BIT0);
+			if (status) {
 				if (status & BIT1)
 					icount->parity++;
 				else if (status & BIT0)
@@ -3755,7 +3759,8 @@ static void slgt_cleanup(void)
 	if (serial_driver) {
 		for (info=slgt_device_list ; info != NULL ; info=info->next_device)
 			tty_unregister_device(serial_driver, info->line);
-		if ((rc = tty_unregister_driver(serial_driver)))
+		rc = tty_unregister_driver(serial_driver);
+		if (rc)
 			DBGERR(("tty_unregister_driver error=%d\n", rc));
 		put_tty_driver(serial_driver);
 	}
diff --git a/drivers/tty/synclinkmp.c b/drivers/tty/synclinkmp.c
index c3f90910fed9..08633a8139ff 100644
--- a/drivers/tty/synclinkmp.c
+++ b/drivers/tty/synclinkmp.c
@@ -1655,7 +1655,8 @@ static int hdlcdev_open(struct net_device *dev)
 		printk("%s:hdlcdev_open(%s)\n",__FILE__,dev->name);
 
 	/* generic HDLC layer open processing */
-	if ((rc = hdlc_open(dev)))
+	rc = hdlc_open(dev);
+	if (rc)
 		return rc;
 
 	/* arbitrate between network and tty opens */
@@ -1922,7 +1923,8 @@ static int hdlcdev_init(SLMP_INFO *info)
 
 	/* allocate and initialize network and HDLC layer objects */
 
-	if (!(dev = alloc_hdlcdev(info))) {
+	dev = alloc_hdlcdev(info);
+	if (!dev) {
 		printk(KERN_ERR "%s:hdlc device allocation failure\n",__FILE__);
 		return -ENOMEM;
 	}
@@ -1943,7 +1945,8 @@ static int hdlcdev_init(SLMP_INFO *info)
 	hdlc->xmit   = hdlcdev_xmit;
 
 	/* register objects with HDLC layer */
-	if ((rc = register_hdlc_device(dev))) {
+	rc = register_hdlc_device(dev);
+	if (rc) {
 		printk(KERN_WARNING "%s:unable to register hdlc device\n",__FILE__);
 		free_netdev(dev);
 		return rc;
@@ -3920,7 +3923,8 @@ static void synclinkmp_cleanup(void)
 	printk("Unloading %s %s\n", driver_name, driver_version);
 
 	if (serial_driver) {
-		if ((rc = tty_unregister_driver(serial_driver)))
+		rc = tty_unregister_driver(serial_driver);
+		if (rc)
 			printk("%s(%d) failed to unregister tty driver err=%d\n",
 			       __FILE__,__LINE__,rc);
 		put_tty_driver(serial_driver);
diff --git a/drivers/tty/sysrq.c b/drivers/tty/sysrq.c
index 843f2cdc280b..b5b427888b24 100644
--- a/drivers/tty/sysrq.c
+++ b/drivers/tty/sysrq.c
@@ -55,9 +55,6 @@
 static int __read_mostly sysrq_enabled = CONFIG_MAGIC_SYSRQ_DEFAULT_ENABLE;
 static bool __read_mostly sysrq_always_enabled;
 
-unsigned short platform_sysrq_reset_seq[] __weak = { KEY_RESERVED };
-int sysrq_reset_downtime_ms __weak;
-
 static bool sysrq_on(void)
 {
 	return sysrq_enabled || sysrq_always_enabled;
@@ -356,9 +353,11 @@ static struct sysrq_key_op sysrq_term_op = {
 
 static void moom_callback(struct work_struct *ignored)
 {
+	mutex_lock(&oom_lock);
 	if (!out_of_memory(node_zonelist(first_memory_node, GFP_KERNEL),
 			   GFP_KERNEL, 0, NULL, true))
 		pr_info("OOM request ignored because killer is disabled\n");
+	mutex_unlock(&oom_lock);
 }
 
 static DECLARE_WORK(moom_work, moom_callback);
@@ -463,6 +462,7 @@ static struct sysrq_key_op *sysrq_key_table[36] = {
 	/* v: May be registered for frame buffer console restore */
 	NULL,				/* v */
 	&sysrq_showstate_blocked_op,	/* w */
+	/* x: May be registered on mips for TLB dump */
 	/* x: May be registered on ppc/powerpc for xmon */
 	/* x: May be registered on sparc64 for global PMU dump */
 	NULL,				/* x */
@@ -569,6 +569,7 @@ void handle_sysrq(int key)
 EXPORT_SYMBOL(handle_sysrq);
 
 #ifdef CONFIG_INPUT
+static int sysrq_reset_downtime_ms;
 
 /* Simple translation table for the SysRq keys */
 static const unsigned char sysrq_xlate[KEY_CNT] =
@@ -949,23 +950,8 @@ static bool sysrq_handler_registered;
 
 static inline void sysrq_register_handler(void)
 {
-	unsigned short key;
 	int error;
-	int i;
-
-	/* First check if a __weak interface was instantiated. */
-	for (i = 0; i < ARRAY_SIZE(sysrq_reset_seq); i++) {
-		key = platform_sysrq_reset_seq[i];
-		if (key == KEY_RESERVED || key > KEY_MAX)
-			break;
-
-		sysrq_reset_seq[sysrq_reset_seq_len++] = key;
-	}
 
-	/*
-	 * DT configuration takes precedence over anything that would
-	 * have been defined via the __weak interface.
-	 */
 	sysrq_of_get_keyreset_config();
 
 	error = input_register_handler(&sysrq_handler);
@@ -1002,7 +988,7 @@ static int sysrq_reset_seq_param_set(const char *buffer,
 	return 0;
 }
 
-static struct kernel_param_ops param_ops_sysrq_reset_seq = {
+static const struct kernel_param_ops param_ops_sysrq_reset_seq = {
 	.get	= param_get_ushort,
 	.set	= sysrq_reset_seq_param_set,
 };
diff --git a/drivers/tty/tty_buffer.c b/drivers/tty/tty_buffer.c
index 2f78b77f0f81..4cf263d7dffc 100644
--- a/drivers/tty/tty_buffer.c
+++ b/drivers/tty/tty_buffer.c
@@ -286,7 +286,8 @@ static int __tty_buffer_request_room(struct tty_port *port, size_t size,
 	change = (b->flags & TTYB_NORMAL) && (~flags & TTYB_NORMAL);
 	if (change || left < size) {
 		/* This is the slow path - looking for new buffers to use */
-		if ((n = tty_buffer_alloc(port, size)) != NULL) {
+		n = tty_buffer_alloc(port, size);
+		if (n != NULL) {
 			n->flags = flags;
 			buf->tail = n;
 			b->commit = b->used;
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index e5695467598f..57fc6ee12332 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -235,7 +235,6 @@ static void tty_del_file(struct file *file)
 /**
  *	tty_name	-	return tty naming
  *	@tty: tty structure
- *	@buf: buffer for output
  *
  *	Convert a tty structure into a name. The name reflects the kernel
  *	naming policy and if udev is in use may not reflect user space
@@ -243,13 +242,11 @@ static void tty_del_file(struct file *file)
  *	Locking: none
  */
 
-char *tty_name(struct tty_struct *tty, char *buf)
+const char *tty_name(const struct tty_struct *tty)
 {
 	if (!tty) /* Hmm.  NULL pointer.  That's fun. */
-		strcpy(buf, "NULL tty");
-	else
-		strcpy(buf, tty->name);
-	return buf;
+		return "NULL tty";
+	return tty->name;
 }
 
 EXPORT_SYMBOL(tty_name);
@@ -770,8 +767,7 @@ static void do_tty_hangup(struct work_struct *work)
 void tty_hangup(struct tty_struct *tty)
 {
 #ifdef TTY_DEBUG_HANGUP
-	char	buf[64];
-	printk(KERN_DEBUG "%s hangup...\n", tty_name(tty, buf));
+	printk(KERN_DEBUG "%s hangup...\n", tty_name(tty));
 #endif
 	schedule_work(&tty->hangup_work);
 }
@@ -790,9 +786,7 @@ EXPORT_SYMBOL(tty_hangup);
 void tty_vhangup(struct tty_struct *tty)
 {
 #ifdef TTY_DEBUG_HANGUP
-	char	buf[64];
-
-	printk(KERN_DEBUG "%s vhangup...\n", tty_name(tty, buf));
+	printk(KERN_DEBUG "%s vhangup...\n", tty_name(tty));
 #endif
 	__tty_hangup(tty, 0);
 }
@@ -831,9 +825,7 @@ void tty_vhangup_self(void)
 static void tty_vhangup_session(struct tty_struct *tty)
 {
 #ifdef TTY_DEBUG_HANGUP
-	char	buf[64];
-
-	printk(KERN_DEBUG "%s vhangup session...\n", tty_name(tty, buf));
+	printk(KERN_DEBUG "%s vhangup session...\n", tty_name(tty));
 #endif
 	__tty_hangup(tty, 1);
 }
@@ -1769,7 +1761,6 @@ int tty_release(struct inode *inode, struct file *filp)
 	struct tty_struct *o_tty = NULL;
 	int	do_sleep, final;
 	int	idx;
-	char	buf[64];
 	long	timeout = 0;
 	int	once = 1;
 
@@ -1793,7 +1784,7 @@ int tty_release(struct inode *inode, struct file *filp)
 
 #ifdef TTY_DEBUG_HANGUP
 	printk(KERN_DEBUG "%s: %s (tty count=%d)...\n", __func__,
-			tty_name(tty, buf), tty->count);
+			tty_name(tty), tty->count);
 #endif
 
 	if (tty->ops->close)
@@ -1844,7 +1835,7 @@ int tty_release(struct inode *inode, struct file *filp)
 		if (once) {
 			once = 0;
 			printk(KERN_WARNING "%s: %s: read/write wait queue active!\n",
-			       __func__, tty_name(tty, buf));
+			       __func__, tty_name(tty));
 		}
 		schedule_timeout_killable(timeout);
 		if (timeout < 120 * HZ)
@@ -1856,13 +1847,13 @@ int tty_release(struct inode *inode, struct file *filp)
 	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));
+				__func__, o_tty->count, tty_name(o_tty));
 			o_tty->count = 0;
 		}
 	}
 	if (--tty->count < 0) {
 		printk(KERN_WARNING "%s: bad tty->count (%d) for %s\n",
-				__func__, tty->count, tty_name(tty, buf));
+				__func__, tty->count, tty_name(tty));
 		tty->count = 0;
 	}
 
@@ -1905,7 +1896,7 @@ int tty_release(struct inode *inode, struct file *filp)
 		return 0;
 
 #ifdef TTY_DEBUG_HANGUP
-	printk(KERN_DEBUG "%s: %s: final close\n", __func__, tty_name(tty, buf));
+	printk(KERN_DEBUG "%s: %s: final close\n", __func__, tty_name(tty));
 #endif
 	/*
 	 * Ask the line discipline code to release its structures
@@ -1916,7 +1907,8 @@ int tty_release(struct inode *inode, struct file *filp)
 	tty_flush_works(tty);
 
 #ifdef TTY_DEBUG_HANGUP
-	printk(KERN_DEBUG "%s: %s: freeing structure...\n", __func__, tty_name(tty, buf));
+	printk(KERN_DEBUG "%s: %s: freeing structure...\n", __func__,
+	       tty_name(tty));
 #endif
 	/*
 	 * The release_tty function takes care of the details of clearing
diff --git a/drivers/tty/tty_ioctl.c b/drivers/tty/tty_ioctl.c
index 8e53fe469664..5232fb60b0b1 100644
--- a/drivers/tty/tty_ioctl.c
+++ b/drivers/tty/tty_ioctl.c
@@ -211,9 +211,7 @@ int tty_unthrottle_safe(struct tty_struct *tty)
 void tty_wait_until_sent(struct tty_struct *tty, long timeout)
 {
 #ifdef TTY_DEBUG_WAIT_UNTIL_SENT
-	char buf[64];
-
-	printk(KERN_DEBUG "%s wait until sent...\n", tty_name(tty, buf));
+	printk(KERN_DEBUG "%s wait until sent...\n", tty_name(tty));
 #endif
 	if (!timeout)
 		timeout = MAX_SCHEDULE_TIMEOUT;
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index 3737f55272d2..c07fb5d9bcf9 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -22,9 +22,8 @@
 #undef LDISC_DEBUG_HANGUP
 
 #ifdef LDISC_DEBUG_HANGUP
-#define tty_ldisc_debug(tty, f, args...) ({				       \
-	char __b[64];							       \
-	printk(KERN_DEBUG "%s: %s: " f, __func__, tty_name(tty, __b), ##args); \
+#define tty_ldisc_debug(tty, f, args...) ({				  \
+	printk(KERN_DEBUG "%s: %s: " f, __func__, tty_name(tty), ##args); \
 })
 #else
 #define tty_ldisc_debug(tty, f, args...)
@@ -483,7 +482,6 @@ static void tty_ldisc_close(struct tty_struct *tty, struct tty_ldisc *ld)
 
 static void tty_ldisc_restore(struct tty_struct *tty, struct tty_ldisc *old)
 {
-	char buf[64];
 	struct tty_ldisc *new_ldisc;
 	int r;
 
@@ -504,7 +502,7 @@ static void tty_ldisc_restore(struct tty_struct *tty, struct tty_ldisc *old)
 		if (r < 0)
 			panic("Couldn't open N_TTY ldisc for "
 			      "%s --- error %d.",
-			      tty_name(tty, buf), r);
+			      tty_name(tty), r);
 	}
 }
 
diff --git a/drivers/tty/tty_ldsem.c b/drivers/tty/tty_ldsem.c
index 0ffb0cbe2823..ad7eba5ca380 100644
--- a/drivers/tty/tty_ldsem.c
+++ b/drivers/tty/tty_ldsem.c
@@ -299,7 +299,8 @@ down_write_failed(struct ld_semaphore *sem, long count, long timeout)
 		timeout = schedule_timeout(timeout);
 		raw_spin_lock_irq(&sem->wait_lock);
 		set_task_state(tsk, TASK_UNINTERRUPTIBLE);
-		if ((locked = writer_trylock(sem)))
+		locked = writer_trylock(sem);
+		if (locked)
 			break;
 	}
 
diff --git a/drivers/tty/vt/consolemap.c b/drivers/tty/vt/consolemap.c
index 59b25e039968..c8c91f0476a2 100644
--- a/drivers/tty/vt/consolemap.c
+++ b/drivers/tty/vt/consolemap.c
@@ -261,19 +261,22 @@ u16 inverse_translate(struct vc_data *conp, int glyph, int use_unicode)
 	int m;
 	if (glyph < 0 || glyph >= MAX_GLYPH)
 		return 0;
-	else if (!(p = *conp->vc_uni_pagedir_loc))
-		return glyph;
-	else if (use_unicode) {
-		if (!p->inverse_trans_unicode)
+	else {
+		p = *conp->vc_uni_pagedir_loc;
+		if (!p)
 			return glyph;
-		else
-			return p->inverse_trans_unicode[glyph];
-	} else {
-		m = inv_translate[conp->vc_num];
-		if (!p->inverse_translations[m])
-			return glyph;
-		else
-			return p->inverse_translations[m][glyph];
+		else if (use_unicode) {
+			if (!p->inverse_trans_unicode)
+				return glyph;
+			else
+				return p->inverse_trans_unicode[glyph];
+			} else {
+			m = inv_translate[conp->vc_num];
+			if (!p->inverse_translations[m])
+				return glyph;
+			else
+				return p->inverse_translations[m][glyph];
+			}
 	}
 }
 EXPORT_SYMBOL_GPL(inverse_translate);
@@ -397,7 +400,8 @@ static void con_release_unimap(struct uni_pagedir *p)
 
 	if (p == dflt) dflt = NULL;  
 	for (i = 0; i < 32; i++) {
-		if ((p1 = p->uni_pgdir[i]) != NULL) {
+		p1 = p->uni_pgdir[i];
+		if (p1 != NULL) {
 			for (j = 0; j < 32; j++)
 				kfree(p1[j]);
 			kfree(p1);
@@ -473,14 +477,16 @@ con_insert_unipair(struct uni_pagedir *p, u_short unicode, u_short fontpos)
 	int i, n;
 	u16 **p1, *p2;
 
-	if (!(p1 = p->uni_pgdir[n = unicode >> 11])) {
+	p1 = p->uni_pgdir[n = unicode >> 11];
+	if (!p1) {
 		p1 = p->uni_pgdir[n] = kmalloc(32*sizeof(u16 *), GFP_KERNEL);
 		if (!p1) return -ENOMEM;
 		for (i = 0; i < 32; i++)
 			p1[i] = NULL;
 	}
 
-	if (!(p2 = p1[n = (unicode >> 6) & 0x1f])) {
+	p2 = p1[n = (unicode >> 6) & 0x1f];
+	if (!p2) {
 		p2 = p1[n] = kmalloc(64*sizeof(u16), GFP_KERNEL);
 		if (!p2) return -ENOMEM;
 		memset(p2, 0xff, 64*sizeof(u16)); /* No glyphs for the characters (yet) */
@@ -569,10 +575,12 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 		 * entries from "p" (old) to "q" (new).
 		 */
 		l = 0;		/* unicode value */
-		for (i = 0; i < 32; i++)
-		if ((p1 = p->uni_pgdir[i]))
-			for (j = 0; j < 32; j++)
-			if ((p2 = p1[j])) {
+		for (i = 0; i < 32; i++) {
+		p1 = p->uni_pgdir[i];
+		if (p1)
+			for (j = 0; j < 32; j++) {
+			p2 = p1[j];
+			if (p2) {
 				for (k = 0; k < 64; k++, l++)
 				if (p2[k] != 0xffff) {
 					/*
@@ -593,9 +601,11 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 				/* Account for row of 64 empty entries */
 				l += 64;
 			}
+		}
 		else
 			/* Account for empty table */
 			l += 32 * 64;
+		}
 
 		/*
 		 * Finished copying font table, set vc_uni_pagedir to new table
@@ -735,10 +745,12 @@ int con_get_unimap(struct vc_data *vc, ushort ct, ushort __user *uct, struct uni
 	ect = 0;
 	if (*vc->vc_uni_pagedir_loc) {
 		p = *vc->vc_uni_pagedir_loc;
-		for (i = 0; i < 32; i++)
-		if ((p1 = p->uni_pgdir[i]))
-			for (j = 0; j < 32; j++)
-			if ((p2 = *(p1++)))
+		for (i = 0; i < 32; i++) {
+		p1 = p->uni_pgdir[i];
+		if (p1)
+			for (j = 0; j < 32; j++) {
+			p2 = *(p1++);
+			if (p2)
 				for (k = 0; k < 64; k++) {
 					if (*p2 < MAX_GLYPH && ect++ < ct) {
 						__put_user((u_short)((i<<11)+(j<<6)+k),
@@ -749,6 +761,8 @@ int con_get_unimap(struct vc_data *vc, ushort ct, ushort __user *uct, struct uni
 					}
 					p2++;
 				}
+			}
+		}
 	}
 	__put_user(ect, uct);
 	console_unlock();
diff --git a/drivers/tty/vt/keyboard.c b/drivers/tty/vt/keyboard.c
index 8a89f6e7715d..6f0336fff501 100644
--- a/drivers/tty/vt/keyboard.c
+++ b/drivers/tty/vt/keyboard.c
@@ -33,6 +33,7 @@
 #include <linux/string.h>
 #include <linux/init.h>
 #include <linux/slab.h>
+#include <linux/leds.h>
 
 #include <linux/kbd_kern.h>
 #include <linux/kbd_diacr.h>
@@ -129,7 +130,7 @@ static char rep;					/* flag telling character repeat */
 
 static int shift_state = 0;
 
-static unsigned char ledstate = 0xff;			/* undefined */
+static unsigned int ledstate = -1U;			/* undefined */
 static unsigned char ledioctl;
 
 /*
@@ -961,6 +962,122 @@ static void k_brl(struct vc_data *vc, unsigned char value, char up_flag)
 	}
 }
 
+#if IS_ENABLED(CONFIG_INPUT_LEDS) && IS_ENABLED(CONFIG_LEDS_TRIGGERS)
+
+struct kbd_led_trigger {
+	struct led_trigger trigger;
+	unsigned int mask;
+};
+
+static void kbd_led_trigger_activate(struct led_classdev *cdev)
+{
+	struct kbd_led_trigger *trigger =
+		container_of(cdev->trigger, struct kbd_led_trigger, trigger);
+
+	tasklet_disable(&keyboard_tasklet);
+	if (ledstate != -1U)
+		led_trigger_event(&trigger->trigger,
+				  ledstate & trigger->mask ?
+					LED_FULL : LED_OFF);
+	tasklet_enable(&keyboard_tasklet);
+}
+
+#define KBD_LED_TRIGGER(_led_bit, _name) {			\
+		.trigger = {					\
+			.name = _name,				\
+			.activate = kbd_led_trigger_activate,	\
+		},						\
+		.mask	= BIT(_led_bit),			\
+	}
+
+#define KBD_LOCKSTATE_TRIGGER(_led_bit, _name)		\
+	KBD_LED_TRIGGER((_led_bit) + 8, _name)
+
+static struct kbd_led_trigger kbd_led_triggers[] = {
+	KBD_LED_TRIGGER(VC_SCROLLOCK, "kbd-scrollock"),
+	KBD_LED_TRIGGER(VC_NUMLOCK,   "kbd-numlock"),
+	KBD_LED_TRIGGER(VC_CAPSLOCK,  "kbd-capslock"),
+	KBD_LED_TRIGGER(VC_KANALOCK,  "kbd-kanalock"),
+
+	KBD_LOCKSTATE_TRIGGER(VC_SHIFTLOCK,  "kbd-shiftlock"),
+	KBD_LOCKSTATE_TRIGGER(VC_ALTGRLOCK,  "kbd-altgrlock"),
+	KBD_LOCKSTATE_TRIGGER(VC_CTRLLOCK,   "kbd-ctrllock"),
+	KBD_LOCKSTATE_TRIGGER(VC_ALTLOCK,    "kbd-altlock"),
+	KBD_LOCKSTATE_TRIGGER(VC_SHIFTLLOCK, "kbd-shiftllock"),
+	KBD_LOCKSTATE_TRIGGER(VC_SHIFTRLOCK, "kbd-shiftrlock"),
+	KBD_LOCKSTATE_TRIGGER(VC_CTRLLLOCK,  "kbd-ctrlllock"),
+	KBD_LOCKSTATE_TRIGGER(VC_CTRLRLOCK,  "kbd-ctrlrlock"),
+};
+
+static void kbd_propagate_led_state(unsigned int old_state,
+				    unsigned int new_state)
+{
+	struct kbd_led_trigger *trigger;
+	unsigned int changed = old_state ^ new_state;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(kbd_led_triggers); i++) {
+		trigger = &kbd_led_triggers[i];
+
+		if (changed & trigger->mask)
+			led_trigger_event(&trigger->trigger,
+					  new_state & trigger->mask ?
+						LED_FULL : LED_OFF);
+	}
+}
+
+static int kbd_update_leds_helper(struct input_handle *handle, void *data)
+{
+	unsigned int led_state = *(unsigned int *)data;
+
+	if (test_bit(EV_LED, handle->dev->evbit))
+		kbd_propagate_led_state(~led_state, led_state);
+
+	return 0;
+}
+
+static void kbd_init_leds(void)
+{
+	int error;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(kbd_led_triggers); i++) {
+		error = led_trigger_register(&kbd_led_triggers[i].trigger);
+		if (error)
+			pr_err("error %d while registering trigger %s\n",
+			       error, kbd_led_triggers[i].trigger.name);
+	}
+}
+
+#else
+
+static int kbd_update_leds_helper(struct input_handle *handle, void *data)
+{
+	unsigned int leds = *(unsigned int *)data;
+
+	if (test_bit(EV_LED, handle->dev->evbit)) {
+		input_inject_event(handle, EV_LED, LED_SCROLLL, !!(leds & 0x01));
+		input_inject_event(handle, EV_LED, LED_NUML,    !!(leds & 0x02));
+		input_inject_event(handle, EV_LED, LED_CAPSL,   !!(leds & 0x04));
+		input_inject_event(handle, EV_SYN, SYN_REPORT, 0);
+	}
+
+	return 0;
+}
+
+static void kbd_propagate_led_state(unsigned int old_state,
+				    unsigned int new_state)
+{
+	input_handler_for_each_handle(&kbd_handler, &new_state,
+				      kbd_update_leds_helper);
+}
+
+static void kbd_init_leds(void)
+{
+}
+
+#endif
+
 /*
  * The leds display either (i) the status of NumLock, CapsLock, ScrollLock,
  * or (ii) whatever pattern of lights people want to show using KDSETLED,
@@ -968,7 +1085,7 @@ static void k_brl(struct vc_data *vc, unsigned char value, char up_flag)
  */
 static unsigned char getledstate(void)
 {
-	return ledstate;
+	return ledstate & 0xff;
 }
 
 void setledstate(struct kbd_struct *kb, unsigned int led)
@@ -995,20 +1112,6 @@ static inline unsigned char getleds(void)
 	return kb->ledflagstate;
 }
 
-static int kbd_update_leds_helper(struct input_handle *handle, void *data)
-{
-	unsigned char leds = *(unsigned char *)data;
-
-	if (test_bit(EV_LED, handle->dev->evbit)) {
-		input_inject_event(handle, EV_LED, LED_SCROLLL, !!(leds & 0x01));
-		input_inject_event(handle, EV_LED, LED_NUML,    !!(leds & 0x02));
-		input_inject_event(handle, EV_LED, LED_CAPSL,   !!(leds & 0x04));
-		input_inject_event(handle, EV_SYN, SYN_REPORT, 0);
-	}
-
-	return 0;
-}
-
 /**
  *	vt_get_leds	-	helper for braille console
  *	@console: console to read
@@ -1085,24 +1188,23 @@ void vt_kbd_con_stop(int console)
 }
 
 /*
- * This is the tasklet that updates LED state on all keyboards
- * attached to the box. The reason we use tasklet is that we
- * need to handle the scenario when keyboard handler is not
- * registered yet but we already getting updates from the VT to
- * update led state.
+ * This is the tasklet that updates LED state of LEDs using standard
+ * keyboard triggers. The reason we use tasklet is that we need to
+ * handle the scenario when keyboard handler is not registered yet
+ * but we already getting updates from the VT to update led state.
  */
 static void kbd_bh(unsigned long dummy)
 {
-	unsigned char leds;
+	unsigned int leds;
 	unsigned long flags;
-	
+
 	spin_lock_irqsave(&led_lock, flags);
 	leds = getleds();
+	leds |= (unsigned int)kbd->lockstate << 8;
 	spin_unlock_irqrestore(&led_lock, flags);
 
 	if (leds != ledstate) {
-		input_handler_for_each_handle(&kbd_handler, &leds,
-					      kbd_update_leds_helper);
+		kbd_propagate_led_state(ledstate, leds);
 		ledstate = leds;
 	}
 }
@@ -1450,7 +1552,7 @@ static void kbd_start(struct input_handle *handle)
 {
 	tasklet_disable(&keyboard_tasklet);
 
-	if (ledstate != 0xff)
+	if (ledstate != -1U)
 		kbd_update_leds_helper(handle, &ledstate);
 
 	tasklet_enable(&keyboard_tasklet);
@@ -1497,6 +1599,8 @@ int __init kbd_init(void)
 		kbd_table[i].kbdmode = default_utf8 ? VC_UNICODE : VC_XLATE;
 	}
 
+	kbd_init_leds();
+
 	error = input_register_handler(&kbd_handler);
 	if (error)
 		return error;
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 4a24eb2b0ede..8fe52989b380 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -108,6 +108,7 @@
 #define CON_DRIVER_FLAG_MODULE 1
 #define CON_DRIVER_FLAG_INIT   2
 #define CON_DRIVER_FLAG_ATTR   4
+#define CON_DRIVER_FLAG_ZOMBIE 8
 
 struct con_driver {
 	const struct consw *con;
@@ -135,6 +136,7 @@ const struct consw *conswitchp;
  */
 #define DEFAULT_BELL_PITCH	750
 #define DEFAULT_BELL_DURATION	(HZ/8)
+#define DEFAULT_CURSOR_BLINK_MS	200
 
 struct vc vc_cons [MAX_NR_CONSOLES];
 
@@ -153,6 +155,7 @@ static int set_vesa_blanking(char __user *p);
 static void set_cursor(struct vc_data *vc);
 static void hide_cursor(struct vc_data *vc);
 static void console_callback(struct work_struct *ignored);
+static void con_driver_unregister_callback(struct work_struct *ignored);
 static void blank_screen_t(unsigned long dummy);
 static void set_palette(struct vc_data *vc);
 
@@ -182,6 +185,7 @@ static int blankinterval = 10*60;
 core_param(consoleblank, blankinterval, int, 0444);
 
 static DECLARE_WORK(console_work, console_callback);
+static DECLARE_WORK(con_driver_unregister_work, con_driver_unregister_callback);
 
 /*
  * fg_console is the current virtual console,
@@ -1590,6 +1594,13 @@ static void setterm_command(struct vc_data *vc)
 		case 15: /* activate the previous console */
 			set_console(last_console);
 			break;
+		case 16: /* set cursor blink duration in msec */
+			if (vc->vc_npar >= 1 && vc->vc_par[1] >= 50 &&
+					vc->vc_par[1] <= USHRT_MAX)
+				vc->vc_cur_blink_ms = vc->vc_par[1];
+			else
+				vc->vc_cur_blink_ms = DEFAULT_CURSOR_BLINK_MS;
+			break;
 	}
 }
 
@@ -1717,6 +1728,7 @@ static void reset_terminal(struct vc_data *vc, int do_clear)
 
 	vc->vc_bell_pitch = DEFAULT_BELL_PITCH;
 	vc->vc_bell_duration = DEFAULT_BELL_DURATION;
+	vc->vc_cur_blink_ms = DEFAULT_CURSOR_BLINK_MS;
 
 	gotoxy(vc, 0, 0);
 	save_cur(vc);
@@ -3192,22 +3204,6 @@ err:
 
 
 #ifdef CONFIG_VT_HW_CONSOLE_BINDING
-static int con_is_graphics(const struct consw *csw, int first, int last)
-{
-	int i, retval = 0;
-
-	for (i = first; i <= last; i++) {
-		struct vc_data *vc = vc_cons[i].d;
-
-		if (vc && vc->vc_mode == KD_GRAPHICS) {
-			retval = 1;
-			break;
-		}
-	}
-
-	return retval;
-}
-
 /* unlocked version of unbind_con_driver() */
 int do_unbind_con_driver(const struct consw *csw, int first, int last, int deflt)
 {
@@ -3293,8 +3289,7 @@ static int vt_bind(struct con_driver *con)
 	const struct consw *defcsw = NULL, *csw = NULL;
 	int i, more = 1, first = -1, last = -1, deflt = 0;
 
- 	if (!con->con || !(con->flag & CON_DRIVER_FLAG_MODULE) ||
-	    con_is_graphics(con->con, con->first, con->last))
+ 	if (!con->con || !(con->flag & CON_DRIVER_FLAG_MODULE))
 		goto err;
 
 	csw = con->con;
@@ -3345,8 +3340,7 @@ static int vt_unbind(struct con_driver *con)
 	int i, more = 1, first = -1, last = -1, deflt = 0;
 	int ret;
 
- 	if (!con->con || !(con->flag & CON_DRIVER_FLAG_MODULE) ||
-	    con_is_graphics(con->con, con->first, con->last))
+ 	if (!con->con || !(con->flag & CON_DRIVER_FLAG_MODULE))
 		goto err;
 
 	csw = con->con;
@@ -3596,7 +3590,8 @@ static int do_register_con_driver(const struct consw *csw, int first, int last)
 	for (i = 0; i < MAX_NR_CON_DRIVER; i++) {
 		con_driver = &registered_con_driver[i];
 
-		if (con_driver->con == NULL) {
+		if (con_driver->con == NULL &&
+		    !(con_driver->flag & CON_DRIVER_FLAG_ZOMBIE)) {
 			con_driver->con = csw;
 			con_driver->desc = desc;
 			con_driver->node = i;
@@ -3658,16 +3653,20 @@ int do_unregister_con_driver(const struct consw *csw)
 		struct con_driver *con_driver = &registered_con_driver[i];
 
 		if (con_driver->con == csw) {
-			vtconsole_deinit_device(con_driver);
-			device_destroy(vtconsole_class,
-				       MKDEV(0, con_driver->node));
+			/*
+			 * Defer the removal of the sysfs entries since that
+			 * will acquire the kernfs s_active lock and we can't
+			 * acquire this lock while holding the console lock:
+			 * the unbind sysfs entry imposes already the opposite
+			 * order. Reset con already here to prevent any later
+			 * lookup to succeed and mark this slot as zombie, so
+			 * it won't get reused until we complete the removal
+			 * in the deferred work.
+			 */
 			con_driver->con = NULL;
-			con_driver->desc = NULL;
-			con_driver->dev = NULL;
-			con_driver->node = 0;
-			con_driver->flag = 0;
-			con_driver->first = 0;
-			con_driver->last = 0;
+			con_driver->flag = CON_DRIVER_FLAG_ZOMBIE;
+			schedule_work(&con_driver_unregister_work);
+
 			return 0;
 		}
 	}
@@ -3676,6 +3675,39 @@ int do_unregister_con_driver(const struct consw *csw)
 }
 EXPORT_SYMBOL_GPL(do_unregister_con_driver);
 
+static void con_driver_unregister_callback(struct work_struct *ignored)
+{
+	int i;
+
+	console_lock();
+
+	for (i = 0; i < MAX_NR_CON_DRIVER; i++) {
+		struct con_driver *con_driver = &registered_con_driver[i];
+
+		if (!(con_driver->flag & CON_DRIVER_FLAG_ZOMBIE))
+			continue;
+
+		console_unlock();
+
+		vtconsole_deinit_device(con_driver);
+		device_destroy(vtconsole_class, MKDEV(0, con_driver->node));
+
+		console_lock();
+
+		if (WARN_ON_ONCE(con_driver->con))
+			con_driver->con = NULL;
+		con_driver->desc = NULL;
+		con_driver->dev = NULL;
+		con_driver->node = 0;
+		WARN_ON_ONCE(con_driver->flag != CON_DRIVER_FLAG_ZOMBIE);
+		con_driver->flag = 0;
+		con_driver->first = 0;
+		con_driver->last = 0;
+	}
+
+	console_unlock();
+}
+
 /*
  *	If we support more console drivers, this function is used
  *	when a driver wants to take over some existing consoles
diff --git a/drivers/uio/Kconfig b/drivers/uio/Kconfig
index 8a15c323c030..48fb1d983f6c 100644
--- a/drivers/uio/Kconfig
+++ b/drivers/uio/Kconfig
@@ -126,8 +126,8 @@ config UIO_FSL_ELBC_GPCM_NETX5152
 
 config UIO_PRUSS
 	tristate "Texas Instruments PRUSS driver"
-	depends on ARCH_DAVINCI_DA850
 	select GENERIC_ALLOCATOR
+	depends on HAS_IOMEM
 	help
 	  PRUSS driver for OMAPL138/DA850/AM18XX devices
 	  PRUSS driver requires user space components, examples and user space
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index 65bf0676d54a..3257d4220d01 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -879,7 +879,8 @@ void uio_unregister_device(struct uio_info *info)
 
 	uio_dev_del_attributes(idev);
 
-	free_irq(idev->info->irq, idev);
+	if (info->irq && info->irq != UIO_IRQ_CUSTOM)
+		free_irq(info->irq, idev);
 
 	device_destroy(&uio_class, MKDEV(uio_major, idev->minor));
 
diff --git a/drivers/uio/uio_pruss.c b/drivers/uio/uio_pruss.c
index 818735bb8c3a..ca9e2fafb0b6 100644
--- a/drivers/uio/uio_pruss.c
+++ b/drivers/uio/uio_pruss.c
@@ -24,6 +24,7 @@
 #include <linux/io.h>
 #include <linux/clk.h>
 #include <linux/dma-mapping.h>
+#include <linux/sizes.h>
 #include <linux/slab.h>
 #include <linux/genalloc.h>
 
diff --git a/drivers/usb/atm/speedtch.c b/drivers/usb/atm/speedtch.c
index 0dc8c06a7b5f..0270d1312f83 100644
--- a/drivers/usb/atm/speedtch.c
+++ b/drivers/usb/atm/speedtch.c
@@ -255,7 +255,8 @@ static int speedtch_upload_firmware(struct speedtch_instance_data *instance,
 
 	usb_dbg(usbatm, "%s entered\n", __func__);
 
-	if (!(buffer = (unsigned char *)__get_free_page(GFP_KERNEL))) {
+	buffer = (unsigned char *)__get_free_page(GFP_KERNEL);
+	if (!buffer) {
 		ret = -ENOMEM;
 		usb_dbg(usbatm, "%s: no memory for buffer!\n", __func__);
 		goto out;
@@ -638,7 +639,8 @@ static void speedtch_handle_int(struct urb *int_urb)
 		goto fail;
 	}
 
-	if ((int_urb = instance->int_urb)) {
+	int_urb = instance->int_urb;
+	if (int_urb) {
 		ret = usb_submit_urb(int_urb, GFP_ATOMIC);
 		schedule_work(&instance->status_check_work);
 		if (ret < 0) {
@@ -650,7 +652,8 @@ static void speedtch_handle_int(struct urb *int_urb)
 	return;
 
 fail:
-	if ((int_urb = instance->int_urb))
+	int_urb = instance->int_urb;
+	if (int_urb)
 		mod_timer(&instance->resubmit_timer, jiffies + msecs_to_jiffies(RESUBMIT_DELAY));
 }
 
@@ -759,11 +762,13 @@ static void speedtch_release_interfaces(struct usb_device *usb_dev,
 	struct usb_interface *cur_intf;
 	int i;
 
-	for (i = 0; i < num_interfaces; i++)
-		if ((cur_intf = usb_ifnum_to_if(usb_dev, i))) {
+	for (i = 0; i < num_interfaces; i++) {
+		cur_intf = usb_ifnum_to_if(usb_dev, i);
+		if (cur_intf) {
 			usb_set_intfdata(cur_intf, NULL);
 			usb_driver_release_interface(&speedtch_usb_driver, cur_intf);
 		}
+	}
 }
 
 static int speedtch_bind(struct usbatm_data *usbatm,
@@ -787,7 +792,8 @@ static int speedtch_bind(struct usbatm_data *usbatm,
 		return -ENODEV;
 	}
 
-	if (!(data_intf = usb_ifnum_to_if(usb_dev, INTERFACE_DATA))) {
+	data_intf = usb_ifnum_to_if(usb_dev, INTERFACE_DATA);
+	if (!data_intf) {
 		usb_err(usbatm, "%s: data interface not found!\n", __func__);
 		return -ENODEV;
 	}
diff --git a/drivers/usb/atm/ueagle-atm.c b/drivers/usb/atm/ueagle-atm.c
index 888998a7fe31..a2ae88dbda78 100644
--- a/drivers/usb/atm/ueagle-atm.c
+++ b/drivers/usb/atm/ueagle-atm.c
@@ -1599,7 +1599,7 @@ static void cmvs_file_name(struct uea_softc *sc, char *const cmv_name, int ver)
 	char file_arr[] = "CMVxy.bin";
 	char *file;
 
-	kparam_block_sysfs_write(cmv_file);
+	kernel_param_lock(THIS_MODULE);
 	/* set proper name corresponding modem version and line type */
 	if (cmv_file[sc->modem_index] == NULL) {
 		if (UEA_CHIP_VERSION(sc) == ADI930)
@@ -1618,7 +1618,7 @@ static void cmvs_file_name(struct uea_softc *sc, char *const cmv_name, int ver)
 	strlcat(cmv_name, file, UEA_FW_NAME_MAX);
 	if (ver == 2)
 		strlcat(cmv_name, ".v2", UEA_FW_NAME_MAX);
-	kparam_unblock_sysfs_write(cmv_file);
+	kernel_param_unlock(THIS_MODULE);
 }
 
 static int request_cmvs_old(struct uea_softc *sc,
diff --git a/drivers/usb/atm/usbatm.c b/drivers/usb/atm/usbatm.c
index dada0146cd7f..db322d9ccb6e 100644
--- a/drivers/usb/atm/usbatm.c
+++ b/drivers/usb/atm/usbatm.c
@@ -382,7 +382,8 @@ static void usbatm_extract_one_cell(struct usbatm_data *instance, unsigned char
 		     "%s: got packet (length: %u, pdu_length: %u, vcc: 0x%p)",
 		     __func__, length, pdu_length, vcc);
 
-		if (!(skb = dev_alloc_skb(length))) {
+		skb = dev_alloc_skb(length);
+		if (!skb) {
 			if (printk_ratelimit())
 				atm_err(instance, "%s: no memory for skb (length: %u)!\n",
 					__func__, length);
@@ -816,7 +817,8 @@ static int usbatm_atm_open(struct atm_vcc *vcc)
 		goto fail;
 	}
 
-	if (!(new = kzalloc(sizeof(struct usbatm_vcc_data), GFP_KERNEL))) {
+	new = kzalloc(sizeof(struct usbatm_vcc_data), GFP_KERNEL);
+	if (!new) {
 		atm_err(instance, "%s: no memory for vcc_data!\n", __func__);
 		ret = -ENOMEM;
 		goto fail;
diff --git a/drivers/usb/atm/xusbatm.c b/drivers/usb/atm/xusbatm.c
index b3b1bb78b2ef..a87597f88a84 100644
--- a/drivers/usb/atm/xusbatm.c
+++ b/drivers/usb/atm/xusbatm.c
@@ -73,7 +73,8 @@ static int xusbatm_capture_intf(struct usbatm_data *usbatm, struct usb_device *u
 		usb_err(usbatm, "%s: failed to claim interface %2d (%d)!\n", __func__, ifnum, ret);
 		return ret;
 	}
-	if ((ret = usb_set_interface(usb_dev, ifnum, altsetting))) {
+	ret = usb_set_interface(usb_dev, ifnum, altsetting);
+	if (ret) {
 		usb_err(usbatm, "%s: altsetting %2d for interface %2d failed (%d)!\n", __func__, altsetting, ifnum, ret);
 		return ret;
 	}
@@ -128,7 +129,8 @@ static int xusbatm_bind(struct usbatm_data *usbatm,
 			rx_intf->altsetting->desc.bInterfaceNumber,
 			tx_intf->altsetting->desc.bInterfaceNumber);
 
-	if ((ret = xusbatm_capture_intf(usbatm, usb_dev, rx_intf, rx_alt, rx_intf != intf)))
+	ret = xusbatm_capture_intf(usbatm, usb_dev, rx_intf, rx_alt, rx_intf != intf);
+	if (ret)
 		return ret;
 
 	if ((tx_intf != rx_intf) && (ret = xusbatm_capture_intf(usbatm, usb_dev, tx_intf, tx_alt, tx_intf != intf))) {
diff --git a/drivers/usb/chipidea/ci_hdrc_usb2.c b/drivers/usb/chipidea/ci_hdrc_usb2.c
index 45844c951788..9eae1a16cef9 100644
--- a/drivers/usb/chipidea/ci_hdrc_usb2.c
+++ b/drivers/usb/chipidea/ci_hdrc_usb2.c
@@ -25,7 +25,7 @@ struct ci_hdrc_usb2_priv {
 	struct clk		*clk;
 };
 
-static struct ci_hdrc_platform_data ci_default_pdata = {
+static const struct ci_hdrc_platform_data ci_default_pdata = {
 	.capoffset	= DEF_CAPOFFSET,
 	.flags		= CI_HDRC_DISABLE_STREAMING,
 };
@@ -37,8 +37,10 @@ static int ci_hdrc_usb2_probe(struct platform_device *pdev)
 	struct ci_hdrc_platform_data *ci_pdata = dev_get_platdata(dev);
 	int ret;
 
-	if (!ci_pdata)
-		ci_pdata = &ci_default_pdata;
+	if (!ci_pdata) {
+		ci_pdata = devm_kmalloc(dev, sizeof(*ci_pdata), GFP_KERNEL);
+		*ci_pdata = ci_default_pdata;	/* struct copy */
+	}
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
index 21fe1a314313..6cf87b8b13a8 100644
--- a/drivers/usb/chipidea/host.c
+++ b/drivers/usb/chipidea/host.c
@@ -37,12 +37,14 @@ static int (*orig_bus_suspend)(struct usb_hcd *hcd);
 
 struct ehci_ci_priv {
 	struct regulator *reg_vbus;
+	struct ci_hdrc *ci;
 };
 
 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 ci_hdrc *ci = priv->ci;
 	struct device *dev = hcd->self.controller;
 	int ret = 0;
 	int port = HCS_N_PORTS(ehci->hcs_params);
@@ -64,6 +66,15 @@ static int ehci_ci_portpower(struct usb_hcd *hcd, int portnum, bool enable)
 			return ret;
 		}
 	}
+
+	if (enable && (ci->platdata->phy_mode == USBPHY_INTERFACE_MODE_HSIC)) {
+		/*
+		 * Marvell 28nm HSIC PHY requires forcing the port to HS mode.
+		 * As HSIC is always HS, this should be safe for others.
+		 */
+		hw_port_test_set(ci, 5);
+		hw_port_test_set(ci, 0);
+	}
 	return 0;
 };
 
@@ -112,6 +123,7 @@ static int host_start(struct ci_hdrc *ci)
 
 	priv = (struct ehci_ci_priv *)ehci->priv;
 	priv->reg_vbus = NULL;
+	priv->ci = ci;
 
 	if (ci->platdata->reg_vbus && !ci_otg_is_fsm_mode(ci)) {
 		if (ci->platdata->flags & CI_HDRC_TURN_VBUS_EARLY_ON) {
diff --git a/drivers/usb/chipidea/usbmisc_imx.c b/drivers/usb/chipidea/usbmisc_imx.c
index 140945cb124f..3cefd49ddb00 100644
--- a/drivers/usb/chipidea/usbmisc_imx.c
+++ b/drivers/usb/chipidea/usbmisc_imx.c
@@ -160,7 +160,7 @@ static int usbmisc_imx27_init(struct imx_usbmisc_data *data)
 		break;
 	default:
 		return -EINVAL;
-	};
+	}
 
 	spin_lock_irqsave(&usbmisc->lock, flags);
 	if (data->disable_oc)
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 5c8f58114677..519a77ba214c 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -46,6 +46,7 @@
 #include <linux/usb/cdc.h>
 #include <asm/byteorder.h>
 #include <asm/unaligned.h>
+#include <linux/idr.h>
 #include <linux/list.h>
 
 #include "cdc-acm.h"
@@ -56,27 +57,27 @@
 
 static struct usb_driver acm_driver;
 static struct tty_driver *acm_tty_driver;
-static struct acm *acm_table[ACM_TTY_MINORS];
 
-static DEFINE_MUTEX(acm_table_lock);
+static DEFINE_IDR(acm_minors);
+static DEFINE_MUTEX(acm_minors_lock);
 
 static void acm_tty_set_termios(struct tty_struct *tty,
 				struct ktermios *termios_old);
 
 /*
- * acm_table accessors
+ * acm_minors accessors
  */
 
 /*
- * Look up an ACM structure by index. If found and not disconnected, increment
+ * Look up an ACM structure by minor. If found and not disconnected, increment
  * its refcount and return it with its mutex held.
  */
-static struct acm *acm_get_by_index(unsigned index)
+static struct acm *acm_get_by_minor(unsigned int minor)
 {
 	struct acm *acm;
 
-	mutex_lock(&acm_table_lock);
-	acm = acm_table[index];
+	mutex_lock(&acm_minors_lock);
+	acm = idr_find(&acm_minors, minor);
 	if (acm) {
 		mutex_lock(&acm->mutex);
 		if (acm->disconnected) {
@@ -87,7 +88,7 @@ static struct acm *acm_get_by_index(unsigned index)
 			mutex_unlock(&acm->mutex);
 		}
 	}
-	mutex_unlock(&acm_table_lock);
+	mutex_unlock(&acm_minors_lock);
 	return acm;
 }
 
@@ -98,14 +99,9 @@ static int acm_alloc_minor(struct acm *acm)
 {
 	int minor;
 
-	mutex_lock(&acm_table_lock);
-	for (minor = 0; minor < ACM_TTY_MINORS; minor++) {
-		if (!acm_table[minor]) {
-			acm_table[minor] = acm;
-			break;
-		}
-	}
-	mutex_unlock(&acm_table_lock);
+	mutex_lock(&acm_minors_lock);
+	minor = idr_alloc(&acm_minors, acm, 0, ACM_TTY_MINORS, GFP_KERNEL);
+	mutex_unlock(&acm_minors_lock);
 
 	return minor;
 }
@@ -113,9 +109,9 @@ static int acm_alloc_minor(struct acm *acm)
 /* Release the minor number associated with 'acm'.  */
 static void acm_release_minor(struct acm *acm)
 {
-	mutex_lock(&acm_table_lock);
-	acm_table[acm->minor] = NULL;
-	mutex_unlock(&acm_table_lock);
+	mutex_lock(&acm_minors_lock);
+	idr_remove(&acm_minors, acm->minor);
+	mutex_unlock(&acm_minors_lock);
 }
 
 /*
@@ -497,7 +493,7 @@ static int acm_tty_install(struct tty_driver *driver, struct tty_struct *tty)
 
 	dev_dbg(tty->dev, "%s\n", __func__);
 
-	acm = acm_get_by_index(tty->index);
+	acm = acm_get_by_minor(tty->index);
 	if (!acm)
 		return -ENODEV;
 
@@ -1267,12 +1263,9 @@ skip_normal_probe:
 						!= CDC_DATA_INTERFACE_TYPE) {
 		if (control_interface->cur_altsetting->desc.bInterfaceClass
 						== CDC_DATA_INTERFACE_TYPE) {
-			struct usb_interface *t;
 			dev_dbg(&intf->dev,
 				"Your device has switched interfaces.\n");
-			t = control_interface;
-			control_interface = data_interface;
-			data_interface = t;
+			swap(control_interface, data_interface);
 		} else {
 			return -EINVAL;
 		}
@@ -1301,12 +1294,9 @@ skip_normal_probe:
 	/* workaround for switched endpoints */
 	if (!usb_endpoint_dir_in(epread)) {
 		/* descriptors are swapped */
-		struct usb_endpoint_descriptor *t;
 		dev_dbg(&intf->dev,
 			"The data interface has switched endpoints\n");
-		t = epread;
-		epread = epwrite;
-		epwrite = t;
+		swap(epread, epwrite);
 	}
 made_compressed_probe:
 	dev_dbg(&intf->dev, "interfaces are valid\n");
@@ -1316,7 +1306,7 @@ made_compressed_probe:
 		goto alloc_fail;
 
 	minor = acm_alloc_minor(acm);
-	if (minor == ACM_TTY_MINORS) {
+	if (minor < 0) {
 		dev_err(&intf->dev, "no more free acm devices\n");
 		kfree(acm);
 		return -ENODEV;
@@ -1477,6 +1467,11 @@ skip_countries:
 		goto alloc_fail8;
 	}
 
+	if (quirks & CLEAR_HALT_CONDITIONS) {
+		usb_clear_halt(usb_dev, usb_rcvbulkpipe(usb_dev, epread->bEndpointAddress));
+		usb_clear_halt(usb_dev, usb_sndbulkpipe(usb_dev, epwrite->bEndpointAddress));
+	}
+
 	return 0;
 alloc_fail8:
 	if (acm->country_codes) {
@@ -1756,6 +1751,10 @@ static const struct usb_device_id acm_ids[] = {
 	.driver_info = NO_UNION_NORMAL, /* reports zero length descriptor */
 	},
 
+	{ USB_DEVICE(0x2912, 0x0001), /* ATOL FPrint */
+	.driver_info = CLEAR_HALT_CONDITIONS,
+	},
+
 	/* Nokia S60 phones expose two ACM channels. The first is
 	 * a modem and is picked up by the standard AT-command
 	 * information below. The second is 'vendor-specific' but
diff --git a/drivers/usb/class/cdc-acm.h b/drivers/usb/class/cdc-acm.h
index ffeb3c83941f..dd9af38e7cda 100644
--- a/drivers/usb/class/cdc-acm.h
+++ b/drivers/usb/class/cdc-acm.h
@@ -19,7 +19,7 @@
  */
 
 #define ACM_TTY_MAJOR		166
-#define ACM_TTY_MINORS		32
+#define ACM_TTY_MINORS		256
 
 /*
  * Requests.
@@ -133,3 +133,4 @@ struct acm {
 #define NO_DATA_INTERFACE		BIT(4)
 #define IGNORE_DEVICE			BIT(5)
 #define QUIRK_CONTROL_LINE_STATE	BIT(6)
+#define CLEAR_HALT_CONDITIONS		BIT(7)
diff --git a/drivers/usb/class/usblp.c b/drivers/usb/class/usblp.c
index 0924ee40a966..f38e875a3fb1 100644
--- a/drivers/usb/class/usblp.c
+++ b/drivers/usb/class/usblp.c
@@ -660,7 +660,8 @@ static long usblp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		switch (cmd) {
 
 		case LPGETSTATUS:
-			if ((retval = usblp_read_status(usblp, usblp->statusbuf))) {
+			retval = usblp_read_status(usblp, usblp->statusbuf);
+			if (retval) {
 				printk_ratelimited(KERN_ERR "usblp%d:"
 					    "failed reading printer status (%d)\n",
 					    usblp->minor, retval);
@@ -693,9 +694,11 @@ static struct urb *usblp_new_writeurb(struct usblp *usblp, int transfer_length)
 	struct urb *urb;
 	char *writebuf;
 
-	if ((writebuf = kmalloc(transfer_length, GFP_KERNEL)) == NULL)
+	writebuf = kmalloc(transfer_length, GFP_KERNEL);
+	if (writebuf == NULL)
 		return NULL;
-	if ((urb = usb_alloc_urb(0, GFP_KERNEL)) == NULL) {
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (urb == NULL) {
 		kfree(writebuf);
 		return NULL;
 	}
@@ -732,7 +735,8 @@ static ssize_t usblp_write(struct file *file, const char __user *buffer, size_t
 			transfer_length = USBLP_BUF_SIZE;
 
 		rv = -ENOMEM;
-		if ((writeurb = usblp_new_writeurb(usblp, transfer_length)) == NULL)
+		writeurb = usblp_new_writeurb(usblp, transfer_length);
+		if (writeurb == NULL)
 			goto raise_urb;
 		usb_anchor_urb(writeurb, &usblp->urbs);
 
@@ -980,7 +984,8 @@ static int usblp_submit_read(struct usblp *usblp)
 	int rc;
 
 	rc = -ENOMEM;
-	if ((urb = usb_alloc_urb(0, GFP_KERNEL)) == NULL)
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (urb == NULL)
 		goto raise_urb;
 
 	usb_fill_bulk_urb(urb, usblp->dev,
diff --git a/drivers/usb/class/usbtmc.c b/drivers/usb/class/usbtmc.c
index 960bc089111b..7a11a8263171 100644
--- a/drivers/usb/class/usbtmc.c
+++ b/drivers/usb/class/usbtmc.c
@@ -109,6 +109,7 @@ struct usbtmc_ID_rigol_quirk {
 
 static const struct usbtmc_ID_rigol_quirk usbtmc_id_quirk[] = {
 	{ 0x1ab1, 0x0588 },
+	{ 0x1ab1, 0x04b0 },
 	{ 0, 0 }
 };
 
diff --git a/drivers/usb/common/Makefile b/drivers/usb/common/Makefile
index ca2f8bd0e431..6bbb3ec17018 100644
--- a/drivers/usb/common/Makefile
+++ b/drivers/usb/common/Makefile
@@ -7,3 +7,4 @@ usb-common-y			  += common.o
 usb-common-$(CONFIG_USB_LED_TRIG) += led.o
 
 obj-$(CONFIG_USB_OTG_FSM) += usb-otg-fsm.o
+obj-$(CONFIG_USB_ULPI_BUS)	+= ulpi.o
diff --git a/drivers/usb/common/ulpi.c b/drivers/usb/common/ulpi.c
new file mode 100644
index 000000000000..0e6f968e93fe
--- /dev/null
+++ b/drivers/usb/common/ulpi.c
@@ -0,0 +1,255 @@
+/**
+ * ulpi.c - USB ULPI PHY bus
+ *
+ * Copyright (C) 2015 Intel Corporation
+ *
+ * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/ulpi/interface.h>
+#include <linux/ulpi/driver.h>
+#include <linux/ulpi/regs.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/acpi.h>
+
+/* -------------------------------------------------------------------------- */
+
+int ulpi_read(struct ulpi *ulpi, u8 addr)
+{
+	return ulpi->ops->read(ulpi->ops, addr);
+}
+EXPORT_SYMBOL_GPL(ulpi_read);
+
+int ulpi_write(struct ulpi *ulpi, u8 addr, u8 val)
+{
+	return ulpi->ops->write(ulpi->ops, addr, val);
+}
+EXPORT_SYMBOL_GPL(ulpi_write);
+
+/* -------------------------------------------------------------------------- */
+
+static int ulpi_match(struct device *dev, struct device_driver *driver)
+{
+	struct ulpi_driver *drv = to_ulpi_driver(driver);
+	struct ulpi *ulpi = to_ulpi_dev(dev);
+	const struct ulpi_device_id *id;
+
+	for (id = drv->id_table; id->vendor; id++)
+		if (id->vendor == ulpi->id.vendor &&
+		    id->product == ulpi->id.product)
+			return 1;
+
+	return 0;
+}
+
+static int ulpi_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	struct ulpi *ulpi = to_ulpi_dev(dev);
+
+	if (add_uevent_var(env, "MODALIAS=ulpi:v%04xp%04x",
+			   ulpi->id.vendor, ulpi->id.product))
+		return -ENOMEM;
+	return 0;
+}
+
+static int ulpi_probe(struct device *dev)
+{
+	struct ulpi_driver *drv = to_ulpi_driver(dev->driver);
+
+	return drv->probe(to_ulpi_dev(dev));
+}
+
+static int ulpi_remove(struct device *dev)
+{
+	struct ulpi_driver *drv = to_ulpi_driver(dev->driver);
+
+	if (drv->remove)
+		drv->remove(to_ulpi_dev(dev));
+
+	return 0;
+}
+
+static struct bus_type ulpi_bus = {
+	.name = "ulpi",
+	.match = ulpi_match,
+	.uevent = ulpi_uevent,
+	.probe = ulpi_probe,
+	.remove = ulpi_remove,
+};
+
+/* -------------------------------------------------------------------------- */
+
+static ssize_t modalias_show(struct device *dev, struct device_attribute *attr,
+			     char *buf)
+{
+	struct ulpi *ulpi = to_ulpi_dev(dev);
+
+	return sprintf(buf, "ulpi:v%04xp%04x\n",
+		       ulpi->id.vendor, ulpi->id.product);
+}
+static DEVICE_ATTR_RO(modalias);
+
+static struct attribute *ulpi_dev_attrs[] = {
+	&dev_attr_modalias.attr,
+	NULL
+};
+
+static struct attribute_group ulpi_dev_attr_group = {
+	.attrs = ulpi_dev_attrs,
+};
+
+static const struct attribute_group *ulpi_dev_attr_groups[] = {
+	&ulpi_dev_attr_group,
+	NULL
+};
+
+static void ulpi_dev_release(struct device *dev)
+{
+	kfree(to_ulpi_dev(dev));
+}
+
+static struct device_type ulpi_dev_type = {
+	.name = "ulpi_device",
+	.groups = ulpi_dev_attr_groups,
+	.release = ulpi_dev_release,
+};
+
+/* -------------------------------------------------------------------------- */
+
+/**
+ * ulpi_register_driver - register a driver with the ULPI bus
+ * @drv: driver being registered
+ *
+ * Registers a driver with the ULPI bus.
+ */
+int ulpi_register_driver(struct ulpi_driver *drv)
+{
+	if (!drv->probe)
+		return -EINVAL;
+
+	drv->driver.bus = &ulpi_bus;
+
+	return driver_register(&drv->driver);
+}
+EXPORT_SYMBOL_GPL(ulpi_register_driver);
+
+/**
+ * ulpi_unregister_driver - unregister a driver with the ULPI bus
+ * @drv: driver to unregister
+ *
+ * Unregisters a driver with the ULPI bus.
+ */
+void ulpi_unregister_driver(struct ulpi_driver *drv)
+{
+	driver_unregister(&drv->driver);
+}
+EXPORT_SYMBOL_GPL(ulpi_unregister_driver);
+
+/* -------------------------------------------------------------------------- */
+
+static int ulpi_register(struct device *dev, struct ulpi *ulpi)
+{
+	int ret;
+
+	/* Test the interface */
+	ret = ulpi_write(ulpi, ULPI_SCRATCH, 0xaa);
+	if (ret < 0)
+		return ret;
+
+	ret = ulpi_read(ulpi, ULPI_SCRATCH);
+	if (ret < 0)
+		return ret;
+
+	if (ret != 0xaa)
+		return -ENODEV;
+
+	ulpi->id.vendor = ulpi_read(ulpi, ULPI_VENDOR_ID_LOW);
+	ulpi->id.vendor |= ulpi_read(ulpi, ULPI_VENDOR_ID_HIGH) << 8;
+
+	ulpi->id.product = ulpi_read(ulpi, ULPI_PRODUCT_ID_LOW);
+	ulpi->id.product |= ulpi_read(ulpi, ULPI_PRODUCT_ID_HIGH) << 8;
+
+	ulpi->dev.parent = dev;
+	ulpi->dev.bus = &ulpi_bus;
+	ulpi->dev.type = &ulpi_dev_type;
+	dev_set_name(&ulpi->dev, "%s.ulpi", dev_name(dev));
+
+	ACPI_COMPANION_SET(&ulpi->dev, ACPI_COMPANION(dev));
+
+	request_module("ulpi:v%04xp%04x", ulpi->id.vendor, ulpi->id.product);
+
+	ret = device_register(&ulpi->dev);
+	if (ret)
+		return ret;
+
+	dev_dbg(&ulpi->dev, "registered ULPI PHY: vendor %04x, product %04x\n",
+		ulpi->id.vendor, ulpi->id.product);
+
+	return 0;
+}
+
+/**
+ * ulpi_register_interface - instantiate new ULPI device
+ * @dev: USB controller's device interface
+ * @ops: ULPI register access
+ *
+ * Allocates and registers a ULPI device and an interface for it. Called from
+ * the USB controller that provides the ULPI interface.
+ */
+struct ulpi *ulpi_register_interface(struct device *dev, struct ulpi_ops *ops)
+{
+	struct ulpi *ulpi;
+	int ret;
+
+	ulpi = kzalloc(sizeof(*ulpi), GFP_KERNEL);
+	if (!ulpi)
+		return ERR_PTR(-ENOMEM);
+
+	ulpi->ops = ops;
+	ops->dev = dev;
+
+	ret = ulpi_register(dev, ulpi);
+	if (ret) {
+		kfree(ulpi);
+		return ERR_PTR(ret);
+	}
+
+	return ulpi;
+}
+EXPORT_SYMBOL_GPL(ulpi_register_interface);
+
+/**
+ * ulpi_unregister_interface - unregister ULPI interface
+ * @intrf: struct ulpi_interface
+ *
+ * Unregisters a ULPI device and it's interface that was created with
+ * ulpi_create_interface().
+ */
+void ulpi_unregister_interface(struct ulpi *ulpi)
+{
+	device_unregister(&ulpi->dev);
+}
+EXPORT_SYMBOL_GPL(ulpi_unregister_interface);
+
+/* -------------------------------------------------------------------------- */
+
+static int __init ulpi_init(void)
+{
+	return bus_register(&ulpi_bus);
+}
+module_init(ulpi_init);
+
+static void __exit ulpi_exit(void)
+{
+	bus_unregister(&ulpi_bus);
+}
+module_exit(ulpi_exit);
+
+MODULE_AUTHOR("Intel Corporation");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("USB ULPI PHY bus");
diff --git a/drivers/usb/core/Kconfig b/drivers/usb/core/Kconfig
index cc0ced08bae2..a99c89e78126 100644
--- a/drivers/usb/core/Kconfig
+++ b/drivers/usb/core/Kconfig
@@ -84,3 +84,23 @@ config USB_OTG_FSM
 	  Implements OTG Finite State Machine as specified in On-The-Go
 	  and Embedded Host Supplement to the USB Revision 2.0 Specification.
 
+config USB_ULPI_BUS
+	tristate "USB ULPI PHY interface support"
+	depends on USB_SUPPORT
+	help
+	  UTMI+ Low Pin Interface (ULPI) is specification for a commonly used
+	  USB 2.0 PHY interface. The ULPI specification defines a standard set
+	  of registers that can be used to detect the vendor and product which
+	  allows ULPI to be handled as a bus. This module is the driver for that
+	  bus.
+
+	  The ULPI interfaces (the buses) are registered by the drivers for USB
+	  controllers which support ULPI register access and have ULPI PHY
+	  attached to them. The ULPI PHY drivers themselves are normal PHY
+	  drivers.
+
+	  ULPI PHYs provide often functions such as ADP sensing/probing (OTG
+	  protocol) and USB charger detection.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called ulpi.
diff --git a/drivers/usb/core/buffer.c b/drivers/usb/core/buffer.c
index 506b969ea7fd..89f2e7765093 100644
--- a/drivers/usb/core/buffer.c
+++ b/drivers/usb/core/buffer.c
@@ -70,7 +70,7 @@ int hcd_buffer_create(struct usb_hcd *hcd)
 		size = pool_max[i];
 		if (!size)
 			continue;
-		snprintf(name, sizeof name, "buffer-%d", size);
+		snprintf(name, sizeof(name), "buffer-%d", size);
 		hcd->pool[i] = dma_pool_create(name, hcd->self.controller,
 				size, size, 0);
 		if (!hcd->pool[i]) {
@@ -95,6 +95,7 @@ void hcd_buffer_destroy(struct usb_hcd *hcd)
 
 	for (i = 0; i < HCD_BUFFER_POOLS; i++) {
 		struct dma_pool *pool = hcd->pool[i];
+
 		if (pool) {
 			dma_pool_destroy(pool);
 			hcd->pool[i] = NULL;
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index 4b0448c26810..986abde07683 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -513,7 +513,7 @@ static void async_completed(struct urb *urb)
 	snoop(&urb->dev->dev, "urb complete\n");
 	snoop_urb(urb->dev, as->userurb, urb->pipe, urb->actual_length,
 			as->status, COMPLETE, NULL, 0);
-	if ((urb->transfer_flags & URB_DIR_MASK) == USB_DIR_IN)
+	if ((urb->transfer_flags & URB_DIR_MASK) == URB_DIR_IN)
 		snoop_urb_data(urb, urb->actual_length);
 
 	if (as->status < 0 && as->bulk_addr && as->status != -ECONNRESET &&
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 45a915ccd71c..be5b2074f906 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -2691,7 +2691,8 @@ int usb_add_hcd(struct usb_hcd *hcd,
 	if ((retval = usb_register_bus(&hcd->self)) < 0)
 		goto err_register_bus;
 
-	if ((rhdev = usb_alloc_dev(NULL, &hcd->self, 0)) == NULL) {
+	rhdev = usb_alloc_dev(NULL, &hcd->self, 0);
+	if (rhdev == NULL) {
 		dev_err(hcd->self.controller, "unable to allocate root hub\n");
 		retval = -ENOMEM;
 		goto err_allocate_root_hub;
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 3b7151687776..43cb2f2e3b43 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -127,7 +127,7 @@ static int usb_device_supports_lpm(struct usb_device *udev)
 	/* USB 2.1 (and greater) devices indicate LPM support through
 	 * their USB 2.0 Extended Capabilities BOS descriptor.
 	 */
-	if (udev->speed == USB_SPEED_HIGH) {
+	if (udev->speed == USB_SPEED_HIGH || udev->speed == USB_SPEED_FULL) {
 		if (udev->bos->ext_cap &&
 			(USB_LPM_SUPPORT &
 			 le32_to_cpu(udev->bos->ext_cap->bmAttributes)))
@@ -795,7 +795,8 @@ int usb_hub_clear_tt_buffer(struct urb *urb)
 	 * since each TT has "at least two" buffers that can need it (and
 	 * there can be many TTs per hub).  even if they're uncommon.
 	 */
-	if ((clear = kmalloc (sizeof *clear, GFP_ATOMIC)) == NULL) {
+	clear = kmalloc(sizeof *clear, GFP_ATOMIC);
+	if (clear == NULL) {
 		dev_err (&udev->dev, "can't save CLEAR_TT_BUFFER state\n");
 		/* FIXME recover somehow ... RESET_TT? */
 		return -ENOMEM;
@@ -2350,6 +2351,26 @@ static void set_usb_port_removable(struct usb_device *udev)
 
 	hub = usb_hub_to_struct_hub(udev->parent);
 
+	/*
+	 * If the platform firmware has provided information about a port,
+	 * use that to determine whether it's removable.
+	 */
+	switch (hub->ports[udev->portnum - 1]->connect_type) {
+	case USB_PORT_CONNECT_TYPE_HOT_PLUG:
+		udev->removable = USB_DEVICE_REMOVABLE;
+		return;
+	case USB_PORT_CONNECT_TYPE_HARD_WIRED:
+	case USB_PORT_NOT_USED:
+		udev->removable = USB_DEVICE_FIXED;
+		return;
+	default:
+		break;
+	}
+
+	/*
+	 * Otherwise, check whether the hub knows whether a port is removable
+	 * or not
+	 */
 	wHubCharacteristics = le16_to_cpu(hub->descriptor->wHubCharacteristics);
 
 	if (!(wHubCharacteristics & HUB_CHAR_COMPOUND))
@@ -2369,21 +2390,6 @@ static void set_usb_port_removable(struct usb_device *udev)
 	else
 		udev->removable = USB_DEVICE_FIXED;
 
-	/*
-	 * Platform firmware may have populated an alternative value for
-	 * removable.  If the parent port has a known connect_type use
-	 * that instead.
-	 */
-	switch (hub->ports[udev->portnum - 1]->connect_type) {
-	case USB_PORT_CONNECT_TYPE_HOT_PLUG:
-		udev->removable = USB_DEVICE_REMOVABLE;
-		break;
-	case USB_PORT_CONNECT_TYPE_HARD_WIRED:
-		udev->removable = USB_DEVICE_FIXED;
-		break;
-	default: /* use what was set above */
-		break;
-	}
 }
 
 /**
@@ -2616,9 +2622,6 @@ static bool use_new_scheme(struct usb_device *udev, int retry)
 	return USE_NEW_SCHEME(retry);
 }
 
-static int hub_port_reset(struct usb_hub *hub, int port1,
-			struct usb_device *udev, unsigned int delay, bool warm);
-
 /* Is a USB 3.0 port in the Inactive or Compliance Mode state?
  * Port worm reset is required to recover
  */
@@ -2706,44 +2709,6 @@ static int hub_port_wait_reset(struct usb_hub *hub, int port1,
 	return 0;
 }
 
-static void hub_port_finish_reset(struct usb_hub *hub, int port1,
-			struct usb_device *udev, int *status)
-{
-	switch (*status) {
-	case 0:
-		/* TRSTRCY = 10 ms; plus some extra */
-		msleep(10 + 40);
-		if (udev) {
-			struct usb_hcd *hcd = bus_to_hcd(udev->bus);
-
-			update_devnum(udev, 0);
-			/* The xHC may think the device is already reset,
-			 * so ignore the status.
-			 */
-			if (hcd->driver->reset_device)
-				hcd->driver->reset_device(hcd, udev);
-		}
-		/* FALL THROUGH */
-	case -ENOTCONN:
-	case -ENODEV:
-		usb_clear_port_feature(hub->hdev,
-				port1, USB_PORT_FEAT_C_RESET);
-		if (hub_is_superspeed(hub->hdev)) {
-			usb_clear_port_feature(hub->hdev, port1,
-					USB_PORT_FEAT_C_BH_PORT_RESET);
-			usb_clear_port_feature(hub->hdev, port1,
-					USB_PORT_FEAT_C_PORT_LINK_STATE);
-			usb_clear_port_feature(hub->hdev, port1,
-					USB_PORT_FEAT_C_CONNECTION);
-		}
-		if (udev)
-			usb_set_device_state(udev, *status
-					? USB_STATE_NOTATTACHED
-					: USB_STATE_DEFAULT);
-		break;
-	}
-}
-
 /* Handle port reset and port warm(BH) reset (for USB3 protocol ports) */
 static int hub_port_reset(struct usb_hub *hub, int port1,
 			struct usb_device *udev, unsigned int delay, bool warm)
@@ -2767,13 +2732,10 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 		 * If the caller hasn't explicitly requested a warm reset,
 		 * double check and see if one is needed.
 		 */
-		status = hub_port_status(hub, port1,
-					&portstatus, &portchange);
-		if (status < 0)
-			goto done;
-
-		if (hub_port_warm_reset_required(hub, port1, portstatus))
-			warm = true;
+		if (hub_port_status(hub, port1, &portstatus, &portchange) == 0)
+			if (hub_port_warm_reset_required(hub, port1,
+							portstatus))
+				warm = true;
 	}
 	clear_bit(port1, hub->warm_reset_bits);
 
@@ -2799,11 +2761,19 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 
 		/* Check for disconnect or reset */
 		if (status == 0 || status == -ENOTCONN || status == -ENODEV) {
-			hub_port_finish_reset(hub, port1, udev, &status);
+			usb_clear_port_feature(hub->hdev, port1,
+					USB_PORT_FEAT_C_RESET);
 
 			if (!hub_is_superspeed(hub->hdev))
 				goto done;
 
+			usb_clear_port_feature(hub->hdev, port1,
+					USB_PORT_FEAT_C_BH_PORT_RESET);
+			usb_clear_port_feature(hub->hdev, port1,
+					USB_PORT_FEAT_C_PORT_LINK_STATE);
+			usb_clear_port_feature(hub->hdev, port1,
+					USB_PORT_FEAT_C_CONNECTION);
+
 			/*
 			 * If a USB 3.0 device migrates from reset to an error
 			 * state, re-issue the warm reset.
@@ -2836,6 +2806,26 @@ static int hub_port_reset(struct usb_hub *hub, int port1,
 	dev_err(&port_dev->dev, "Cannot enable. Maybe the USB cable is bad?\n");
 
 done:
+	if (status == 0) {
+		/* TRSTRCY = 10 ms; plus some extra */
+		msleep(10 + 40);
+		if (udev) {
+			struct usb_hcd *hcd = bus_to_hcd(udev->bus);
+
+			update_devnum(udev, 0);
+			/* The xHC may think the device is already reset,
+			 * so ignore the status.
+			 */
+			if (hcd->driver->reset_device)
+				hcd->driver->reset_device(hcd, udev);
+
+			usb_set_device_state(udev, USB_STATE_DEFAULT);
+		}
+	} else {
+		if (udev)
+			usb_set_device_state(udev, USB_STATE_NOTATTACHED);
+	}
+
 	if (!hub_is_superspeed(hub->hdev))
 		up_read(&ehci_cf_port_reset_rwsem);
 
diff --git a/drivers/usb/dwc2/Kconfig b/drivers/usb/dwc2/Kconfig
index 1bcb36ae6505..fd95ba6ec317 100644
--- a/drivers/usb/dwc2/Kconfig
+++ b/drivers/usb/dwc2/Kconfig
@@ -50,18 +50,10 @@ config USB_DWC2_DUAL_ROLE
 	  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
 	tristate "DWC2 PCI"
 	depends on PCI
 	default n
-	select USB_DWC2_PLATFORM
 	select NOP_USB_XCEIV
 	help
 	  The Designware USB2.0 PCI interface module for controllers
diff --git a/drivers/usb/dwc2/Makefile b/drivers/usb/dwc2/Makefile
index f07b425eaff3..50fdaace1e73 100644
--- a/drivers/usb/dwc2/Makefile
+++ b/drivers/usb/dwc2/Makefile
@@ -2,7 +2,7 @@ ccflags-$(CONFIG_USB_DWC2_DEBUG)	+= -DDEBUG
 ccflags-$(CONFIG_USB_DWC2_VERBOSE)	+= -DVERBOSE_DEBUG
 
 obj-$(CONFIG_USB_DWC2)			+= dwc2.o
-dwc2-y					:= core.o core_intr.o
+dwc2-y					:= core.o core_intr.o platform.o
 
 ifneq ($(filter y,$(CONFIG_USB_DWC2_HOST) $(CONFIG_USB_DWC2_DUAL_ROLE)),)
 	dwc2-y				+= hcd.o hcd_intr.o
@@ -13,6 +13,10 @@ ifneq ($(filter y,$(CONFIG_USB_DWC2_PERIPHERAL) $(CONFIG_USB_DWC2_DUAL_ROLE)),)
 	dwc2-y       			+= gadget.o
 endif
 
+ifneq ($(CONFIG_DEBUG_FS),)
+	dwc2-y				+= debugfs.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.ko will get built for host mode, peripheral mode, and dual-role
@@ -21,6 +25,3 @@ endif
 
 obj-$(CONFIG_USB_DWC2_PCI)		+= dwc2_pci.o
 dwc2_pci-y				:= pci.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 d5197d492e21..e5b546f1152e 100644
--- a/drivers/usb/dwc2/core.c
+++ b/drivers/usb/dwc2/core.c
@@ -56,6 +56,389 @@
 #include "core.h"
 #include "hcd.h"
 
+#if IS_ENABLED(CONFIG_USB_DWC2_HOST) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
+/**
+ * dwc2_backup_host_registers() - Backup controller host registers.
+ * When suspending usb bus, registers needs to be backuped
+ * if controller power is disabled once suspended.
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ */
+static int dwc2_backup_host_registers(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_hregs_backup *hr;
+	int i;
+
+	dev_dbg(hsotg->dev, "%s\n", __func__);
+
+	/* Backup Host regs */
+	hr = hsotg->hr_backup;
+	if (!hr) {
+		hr = devm_kzalloc(hsotg->dev, sizeof(*hr), GFP_KERNEL);
+		if (!hr) {
+			dev_err(hsotg->dev, "%s: can't allocate host regs\n",
+					__func__);
+			return -ENOMEM;
+		}
+
+		hsotg->hr_backup = hr;
+	}
+	hr->hcfg = readl(hsotg->regs + HCFG);
+	hr->haintmsk = readl(hsotg->regs + HAINTMSK);
+	for (i = 0; i < hsotg->core_params->host_channels; ++i)
+		hr->hcintmsk[i] = readl(hsotg->regs + HCINTMSK(i));
+
+	hr->hprt0 = readl(hsotg->regs + HPRT0);
+	hr->hfir = readl(hsotg->regs + HFIR);
+
+	return 0;
+}
+
+/**
+ * dwc2_restore_host_registers() - Restore controller host registers.
+ * When resuming usb bus, device registers needs to be restored
+ * if controller power were disabled.
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ */
+static int dwc2_restore_host_registers(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_hregs_backup *hr;
+	int i;
+
+	dev_dbg(hsotg->dev, "%s\n", __func__);
+
+	/* Restore host regs */
+	hr = hsotg->hr_backup;
+	if (!hr) {
+		dev_err(hsotg->dev, "%s: no host registers to restore\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	writel(hr->hcfg, hsotg->regs + HCFG);
+	writel(hr->haintmsk, hsotg->regs + HAINTMSK);
+
+	for (i = 0; i < hsotg->core_params->host_channels; ++i)
+		writel(hr->hcintmsk[i], hsotg->regs + HCINTMSK(i));
+
+	writel(hr->hprt0, hsotg->regs + HPRT0);
+	writel(hr->hfir, hsotg->regs + HFIR);
+
+	return 0;
+}
+#else
+static inline int dwc2_backup_host_registers(struct dwc2_hsotg *hsotg)
+{ return 0; }
+
+static inline int dwc2_restore_host_registers(struct dwc2_hsotg *hsotg)
+{ return 0; }
+#endif
+
+#if IS_ENABLED(CONFIG_USB_DWC2_PERIPHERAL) || \
+	IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
+/**
+ * dwc2_backup_device_registers() - Backup controller device registers.
+ * When suspending usb bus, registers needs to be backuped
+ * if controller power is disabled once suspended.
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ */
+static int dwc2_backup_device_registers(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_dregs_backup *dr;
+	int i;
+
+	dev_dbg(hsotg->dev, "%s\n", __func__);
+
+	/* Backup dev regs */
+	dr = hsotg->dr_backup;
+	if (!dr) {
+		dr = devm_kzalloc(hsotg->dev, sizeof(*dr), GFP_KERNEL);
+		if (!dr) {
+			dev_err(hsotg->dev, "%s: can't allocate device regs\n",
+					__func__);
+			return -ENOMEM;
+		}
+
+		hsotg->dr_backup = dr;
+	}
+
+	dr->dcfg = readl(hsotg->regs + DCFG);
+	dr->dctl = readl(hsotg->regs + DCTL);
+	dr->daintmsk = readl(hsotg->regs + DAINTMSK);
+	dr->diepmsk = readl(hsotg->regs + DIEPMSK);
+	dr->doepmsk = readl(hsotg->regs + DOEPMSK);
+
+	for (i = 0; i < hsotg->num_of_eps; i++) {
+		/* Backup IN EPs */
+		dr->diepctl[i] = readl(hsotg->regs + DIEPCTL(i));
+
+		/* Ensure DATA PID is correctly configured */
+		if (dr->diepctl[i] & DXEPCTL_DPID)
+			dr->diepctl[i] |= DXEPCTL_SETD1PID;
+		else
+			dr->diepctl[i] |= DXEPCTL_SETD0PID;
+
+		dr->dieptsiz[i] = readl(hsotg->regs + DIEPTSIZ(i));
+		dr->diepdma[i] = readl(hsotg->regs + DIEPDMA(i));
+
+		/* Backup OUT EPs */
+		dr->doepctl[i] = readl(hsotg->regs + DOEPCTL(i));
+
+		/* Ensure DATA PID is correctly configured */
+		if (dr->doepctl[i] & DXEPCTL_DPID)
+			dr->doepctl[i] |= DXEPCTL_SETD1PID;
+		else
+			dr->doepctl[i] |= DXEPCTL_SETD0PID;
+
+		dr->doeptsiz[i] = readl(hsotg->regs + DOEPTSIZ(i));
+		dr->doepdma[i] = readl(hsotg->regs + DOEPDMA(i));
+	}
+
+	return 0;
+}
+
+/**
+ * dwc2_restore_device_registers() - Restore controller device registers.
+ * When resuming usb bus, device registers needs to be restored
+ * if controller power were disabled.
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ */
+static int dwc2_restore_device_registers(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_dregs_backup *dr;
+	u32 dctl;
+	int i;
+
+	dev_dbg(hsotg->dev, "%s\n", __func__);
+
+	/* Restore dev regs */
+	dr = hsotg->dr_backup;
+	if (!dr) {
+		dev_err(hsotg->dev, "%s: no device registers to restore\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	writel(dr->dcfg, hsotg->regs + DCFG);
+	writel(dr->dctl, hsotg->regs + DCTL);
+	writel(dr->daintmsk, hsotg->regs + DAINTMSK);
+	writel(dr->diepmsk, hsotg->regs + DIEPMSK);
+	writel(dr->doepmsk, hsotg->regs + DOEPMSK);
+
+	for (i = 0; i < hsotg->num_of_eps; i++) {
+		/* Restore IN EPs */
+		writel(dr->diepctl[i], hsotg->regs + DIEPCTL(i));
+		writel(dr->dieptsiz[i], hsotg->regs + DIEPTSIZ(i));
+		writel(dr->diepdma[i], hsotg->regs + DIEPDMA(i));
+
+		/* Restore OUT EPs */
+		writel(dr->doepctl[i], hsotg->regs + DOEPCTL(i));
+		writel(dr->doeptsiz[i], hsotg->regs + DOEPTSIZ(i));
+		writel(dr->doepdma[i], hsotg->regs + DOEPDMA(i));
+	}
+
+	/* Set the Power-On Programming done bit */
+	dctl = readl(hsotg->regs + DCTL);
+	dctl |= DCTL_PWRONPRGDONE;
+	writel(dctl, hsotg->regs + DCTL);
+
+	return 0;
+}
+#else
+static inline int dwc2_backup_device_registers(struct dwc2_hsotg *hsotg)
+{ return 0; }
+
+static inline int dwc2_restore_device_registers(struct dwc2_hsotg *hsotg)
+{ return 0; }
+#endif
+
+/**
+ * dwc2_backup_global_registers() - Backup global controller registers.
+ * When suspending usb bus, registers needs to be backuped
+ * if controller power is disabled once suspended.
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ */
+static int dwc2_backup_global_registers(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_gregs_backup *gr;
+	int i;
+
+	/* Backup global regs */
+	gr = hsotg->gr_backup;
+	if (!gr) {
+		gr = devm_kzalloc(hsotg->dev, sizeof(*gr), GFP_KERNEL);
+		if (!gr) {
+			dev_err(hsotg->dev, "%s: can't allocate global regs\n",
+					__func__);
+			return -ENOMEM;
+		}
+
+		hsotg->gr_backup = gr;
+	}
+
+	gr->gotgctl = readl(hsotg->regs + GOTGCTL);
+	gr->gintmsk = readl(hsotg->regs + GINTMSK);
+	gr->gahbcfg = readl(hsotg->regs + GAHBCFG);
+	gr->gusbcfg = readl(hsotg->regs + GUSBCFG);
+	gr->grxfsiz = readl(hsotg->regs + GRXFSIZ);
+	gr->gnptxfsiz = readl(hsotg->regs + GNPTXFSIZ);
+	gr->hptxfsiz = readl(hsotg->regs + HPTXFSIZ);
+	gr->gdfifocfg = readl(hsotg->regs + GDFIFOCFG);
+	for (i = 0; i < MAX_EPS_CHANNELS; i++)
+		gr->dtxfsiz[i] = readl(hsotg->regs + DPTXFSIZN(i));
+
+	return 0;
+}
+
+/**
+ * dwc2_restore_global_registers() - Restore controller global registers.
+ * When resuming usb bus, device registers needs to be restored
+ * if controller power were disabled.
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ */
+static int dwc2_restore_global_registers(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_gregs_backup *gr;
+	int i;
+
+	dev_dbg(hsotg->dev, "%s\n", __func__);
+
+	/* Restore global regs */
+	gr = hsotg->gr_backup;
+	if (!gr) {
+		dev_err(hsotg->dev, "%s: no global registers to restore\n",
+				__func__);
+		return -EINVAL;
+	}
+
+	writel(0xffffffff, hsotg->regs + GINTSTS);
+	writel(gr->gotgctl, hsotg->regs + GOTGCTL);
+	writel(gr->gintmsk, hsotg->regs + GINTMSK);
+	writel(gr->gusbcfg, hsotg->regs + GUSBCFG);
+	writel(gr->gahbcfg, hsotg->regs + GAHBCFG);
+	writel(gr->grxfsiz, hsotg->regs + GRXFSIZ);
+	writel(gr->gnptxfsiz, hsotg->regs + GNPTXFSIZ);
+	writel(gr->hptxfsiz, hsotg->regs + HPTXFSIZ);
+	writel(gr->gdfifocfg, hsotg->regs + GDFIFOCFG);
+	for (i = 0; i < MAX_EPS_CHANNELS; i++)
+		writel(gr->dtxfsiz[i], hsotg->regs + DPTXFSIZN(i));
+
+	return 0;
+}
+
+/**
+ * dwc2_exit_hibernation() - Exit controller from Partial Power Down.
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ * @restore: Controller registers need to be restored
+ */
+int dwc2_exit_hibernation(struct dwc2_hsotg *hsotg, bool restore)
+{
+	u32 pcgcctl;
+	int ret = 0;
+
+	if (!hsotg->core_params->hibernation)
+		return -ENOTSUPP;
+
+	pcgcctl = readl(hsotg->regs + PCGCTL);
+	pcgcctl &= ~PCGCTL_STOPPCLK;
+	writel(pcgcctl, hsotg->regs + PCGCTL);
+
+	pcgcctl = readl(hsotg->regs + PCGCTL);
+	pcgcctl &= ~PCGCTL_PWRCLMP;
+	writel(pcgcctl, hsotg->regs + PCGCTL);
+
+	pcgcctl = readl(hsotg->regs + PCGCTL);
+	pcgcctl &= ~PCGCTL_RSTPDWNMODULE;
+	writel(pcgcctl, hsotg->regs + PCGCTL);
+
+	udelay(100);
+	if (restore) {
+		ret = dwc2_restore_global_registers(hsotg);
+		if (ret) {
+			dev_err(hsotg->dev, "%s: failed to restore registers\n",
+					__func__);
+			return ret;
+		}
+		if (dwc2_is_host_mode(hsotg)) {
+			ret = dwc2_restore_host_registers(hsotg);
+			if (ret) {
+				dev_err(hsotg->dev, "%s: failed to restore host registers\n",
+						__func__);
+				return ret;
+			}
+		} else {
+			ret = dwc2_restore_device_registers(hsotg);
+			if (ret) {
+				dev_err(hsotg->dev, "%s: failed to restore device registers\n",
+						__func__);
+				return ret;
+			}
+		}
+	}
+
+	return ret;
+}
+
+/**
+ * dwc2_enter_hibernation() - Put controller in Partial Power Down.
+ *
+ * @hsotg: Programming view of the DWC_otg controller
+ */
+int dwc2_enter_hibernation(struct dwc2_hsotg *hsotg)
+{
+	u32 pcgcctl;
+	int ret = 0;
+
+	if (!hsotg->core_params->hibernation)
+		return -ENOTSUPP;
+
+	/* Backup all registers */
+	ret = dwc2_backup_global_registers(hsotg);
+	if (ret) {
+		dev_err(hsotg->dev, "%s: failed to backup global registers\n",
+				__func__);
+		return ret;
+	}
+
+	if (dwc2_is_host_mode(hsotg)) {
+		ret = dwc2_backup_host_registers(hsotg);
+		if (ret) {
+			dev_err(hsotg->dev, "%s: failed to backup host registers\n",
+					__func__);
+			return ret;
+		}
+	} else {
+		ret = dwc2_backup_device_registers(hsotg);
+		if (ret) {
+			dev_err(hsotg->dev, "%s: failed to backup device registers\n",
+					__func__);
+			return ret;
+		}
+	}
+
+	/* Put the controller in low power state */
+	pcgcctl = readl(hsotg->regs + PCGCTL);
+
+	pcgcctl |= PCGCTL_PWRCLMP;
+	writel(pcgcctl, hsotg->regs + PCGCTL);
+	ndelay(20);
+
+	pcgcctl |= PCGCTL_RSTPDWNMODULE;
+	writel(pcgcctl, hsotg->regs + PCGCTL);
+	ndelay(20);
+
+	pcgcctl |= PCGCTL_STOPPCLK;
+	writel(pcgcctl, hsotg->regs + PCGCTL);
+
+	return ret;
+}
+
 /**
  * dwc2_enable_common_interrupts() - Initializes the commmon interrupts,
  * used in both device and host modes
@@ -77,8 +460,10 @@ static void dwc2_enable_common_interrupts(struct dwc2_hsotg *hsotg)
 
 	if (hsotg->core_params->dma_enable <= 0)
 		intmsk |= GINTSTS_RXFLVL;
+	if (hsotg->core_params->external_id_pin_ctl <= 0)
+		intmsk |= GINTSTS_CONIDSTSCHNG;
 
-	intmsk |= GINTSTS_CONIDSTSCHNG | GINTSTS_WKUPINT | GINTSTS_USBSUSP |
+	intmsk |= GINTSTS_WKUPINT | GINTSTS_USBSUSP |
 		  GINTSTS_SESSREQINT;
 
 	writel(intmsk, hsotg->regs + GINTMSK);
@@ -2602,6 +2987,40 @@ static void dwc2_set_param_uframe_sched(struct dwc2_hsotg *hsotg, int val)
 	hsotg->core_params->uframe_sched = val;
 }
 
+static void dwc2_set_param_external_id_pin_ctl(struct dwc2_hsotg *hsotg,
+		int val)
+{
+	if (DWC2_OUT_OF_BOUNDS(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"'%d' invalid for parameter external_id_pin_ctl\n",
+				val);
+			dev_err(hsotg->dev, "external_id_pin_ctl must be 0 or 1\n");
+		}
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting external_id_pin_ctl to %d\n", val);
+	}
+
+	hsotg->core_params->external_id_pin_ctl = val;
+}
+
+static void dwc2_set_param_hibernation(struct dwc2_hsotg *hsotg,
+		int val)
+{
+	if (DWC2_OUT_OF_BOUNDS(val, 0, 1)) {
+		if (val >= 0) {
+			dev_err(hsotg->dev,
+				"'%d' invalid for parameter hibernation\n",
+				val);
+			dev_err(hsotg->dev, "hibernation must be 0 or 1\n");
+		}
+		val = 0;
+		dev_dbg(hsotg->dev, "Setting hibernation to %d\n", val);
+	}
+
+	hsotg->core_params->hibernation = val;
+}
+
 /*
  * This function is called during module intialization to pass module parameters
  * for the DWC_otg core.
@@ -2646,6 +3065,8 @@ void dwc2_set_parameters(struct dwc2_hsotg *hsotg,
 	dwc2_set_param_ahbcfg(hsotg, params->ahbcfg);
 	dwc2_set_param_otg_ver(hsotg, params->otg_ver);
 	dwc2_set_param_uframe_sched(hsotg, params->uframe_sched);
+	dwc2_set_param_external_id_pin_ctl(hsotg, params->external_id_pin_ctl);
+	dwc2_set_param_hibernation(hsotg, params->hibernation);
 }
 
 /**
@@ -2814,6 +3235,22 @@ int dwc2_get_hwparams(struct dwc2_hsotg *hsotg)
 	return 0;
 }
 
+/*
+ * Sets all parameters to the given value.
+ *
+ * Assumes that the dwc2_core_params struct contains only integers.
+ */
+void dwc2_set_all_params(struct dwc2_core_params *params, int value)
+{
+	int *p = (int *)params;
+	size_t size = sizeof(*params) / sizeof(*p);
+	int i;
+
+	for (i = 0; i < size; i++)
+		p[i] = value;
+}
+
+
 u16 dwc2_get_otg_version(struct dwc2_hsotg *hsotg)
 {
 	return hsotg->core_params->otg_ver == 1 ? 0x0200 : 0x0103;
diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index 836c012c7707..53b8de03f102 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -331,6 +331,17 @@ enum dwc2_ep0_state {
  *                      by the driver and are ignored in this
  *                      configuration value.
  * @uframe_sched:       True to enable the microframe scheduler
+ * @external_id_pin_ctl: Specifies whether ID pin is handled externally.
+ *                      Disable CONIDSTSCHNG controller interrupt in such
+ *                      case.
+ *                      0 - No (default)
+ *                      1 - Yes
+ * @hibernation:	Specifies whether the controller support hibernation.
+ *			If hibernation is enabled, the controller will enter
+ *			hibernation in both peripheral and host mode when
+ *			needed.
+ *			0 - No (default)
+ *			1 - Yes
  *
  * The following parameters may be specified when starting the module. These
  * parameters define how the DWC_otg controller should be configured. A
@@ -368,6 +379,8 @@ struct dwc2_core_params {
 	int reload_ctl;
 	int ahbcfg;
 	int uframe_sched;
+	int external_id_pin_ctl;
+	int hibernation;
 };
 
 /**
@@ -452,6 +465,82 @@ struct dwc2_hw_params {
 #define DWC2_CTRL_BUFF_SIZE 8
 
 /**
+ * struct dwc2_gregs_backup - Holds global registers state before entering partial
+ * power down
+ * @gotgctl:		Backup of GOTGCTL register
+ * @gintmsk:		Backup of GINTMSK register
+ * @gahbcfg:		Backup of GAHBCFG register
+ * @gusbcfg:		Backup of GUSBCFG register
+ * @grxfsiz:		Backup of GRXFSIZ register
+ * @gnptxfsiz:		Backup of GNPTXFSIZ register
+ * @gi2cctl:		Backup of GI2CCTL register
+ * @hptxfsiz:		Backup of HPTXFSIZ register
+ * @gdfifocfg:		Backup of GDFIFOCFG register
+ * @dtxfsiz:		Backup of DTXFSIZ registers for each endpoint
+ * @gpwrdn:		Backup of GPWRDN register
+ */
+struct dwc2_gregs_backup {
+	u32 gotgctl;
+	u32 gintmsk;
+	u32 gahbcfg;
+	u32 gusbcfg;
+	u32 grxfsiz;
+	u32 gnptxfsiz;
+	u32 gi2cctl;
+	u32 hptxfsiz;
+	u32 pcgcctl;
+	u32 gdfifocfg;
+	u32 dtxfsiz[MAX_EPS_CHANNELS];
+	u32 gpwrdn;
+};
+
+/**
+ * struct  dwc2_dregs_backup - Holds device registers state before entering partial
+ * power down
+ * @dcfg:		Backup of DCFG register
+ * @dctl:		Backup of DCTL register
+ * @daintmsk:		Backup of DAINTMSK register
+ * @diepmsk:		Backup of DIEPMSK register
+ * @doepmsk:		Backup of DOEPMSK register
+ * @diepctl:		Backup of DIEPCTL register
+ * @dieptsiz:		Backup of DIEPTSIZ register
+ * @diepdma:		Backup of DIEPDMA register
+ * @doepctl:		Backup of DOEPCTL register
+ * @doeptsiz:		Backup of DOEPTSIZ register
+ * @doepdma:		Backup of DOEPDMA register
+ */
+struct dwc2_dregs_backup {
+	u32 dcfg;
+	u32 dctl;
+	u32 daintmsk;
+	u32 diepmsk;
+	u32 doepmsk;
+	u32 diepctl[MAX_EPS_CHANNELS];
+	u32 dieptsiz[MAX_EPS_CHANNELS];
+	u32 diepdma[MAX_EPS_CHANNELS];
+	u32 doepctl[MAX_EPS_CHANNELS];
+	u32 doeptsiz[MAX_EPS_CHANNELS];
+	u32 doepdma[MAX_EPS_CHANNELS];
+};
+
+/**
+ * struct  dwc2_hregs_backup - Holds host registers state before entering partial
+ * power down
+ * @hcfg:		Backup of HCFG register
+ * @haintmsk:		Backup of HAINTMSK register
+ * @hcintmsk:		Backup of HCINTMSK register
+ * @hptr0:		Backup of HPTR0 register
+ * @hfir:		Backup of HFIR register
+ */
+struct dwc2_hregs_backup {
+	u32 hcfg;
+	u32 haintmsk;
+	u32 hcintmsk[MAX_EPS_CHANNELS];
+	u32 hprt0;
+	u32 hfir;
+};
+
+/**
  * struct dwc2_hsotg - Holds the state of the driver, including the non-periodic
  * and periodic schedules
  *
@@ -481,6 +570,9 @@ struct dwc2_hw_params {
  *                      interrupt
  * @wkp_timer:          Timer object for handling Wakeup Detected interrupt
  * @lx_state:           Lx state of connected device
+ * @gregs_backup: Backup of global registers during suspend
+ * @dregs_backup: Backup of device registers during suspend
+ * @hregs_backup: Backup of host registers during suspend
  *
  * These are for host mode:
  *
@@ -613,11 +705,12 @@ struct dwc2_hsotg {
 	struct work_struct wf_otg;
 	struct timer_list wkp_timer;
 	enum dwc2_lx_state lx_state;
+	struct dwc2_gregs_backup *gr_backup;
+	struct dwc2_dregs_backup *dr_backup;
+	struct dwc2_hregs_backup *hr_backup;
 
 	struct dentry *debug_root;
-	struct dentry *debug_file;
-	struct dentry *debug_testmode;
-	struct dentry *debug_fifo;
+	struct debugfs_regset32 *regset;
 
 	/* DWC OTG HW Release versions */
 #define DWC2_CORE_REV_2_71a	0x4f54271a
@@ -751,6 +844,8 @@ enum dwc2_halt_status {
  * and the DWC_otg controller
  */
 extern void dwc2_core_host_init(struct dwc2_hsotg *hsotg);
+extern int dwc2_enter_hibernation(struct dwc2_hsotg *hsotg);
+extern int dwc2_exit_hibernation(struct dwc2_hsotg *hsotg, bool restore);
 
 /*
  * Host core Functions.
@@ -983,6 +1078,15 @@ extern void dwc2_set_param_ahbcfg(struct dwc2_hsotg *hsotg, int val);
 
 extern void dwc2_set_param_otg_ver(struct dwc2_hsotg *hsotg, int val);
 
+extern void dwc2_set_parameters(struct dwc2_hsotg *hsotg,
+				const struct dwc2_core_params *params);
+
+extern void dwc2_set_all_params(struct dwc2_core_params *params, int value);
+
+extern int dwc2_get_hwparams(struct dwc2_hsotg *hsotg);
+
+
+
 /*
  * Dump core registers and SPRAM
  */
@@ -1005,6 +1109,8 @@ extern void s3c_hsotg_core_init_disconnected(struct dwc2_hsotg *dwc2,
 		bool reset);
 extern void s3c_hsotg_core_connect(struct dwc2_hsotg *hsotg);
 extern void s3c_hsotg_disconnect(struct dwc2_hsotg *dwc2);
+extern int s3c_hsotg_set_test_mode(struct dwc2_hsotg *hsotg, int testmode);
+#define dwc2_is_device_connected(hsotg) (hsotg->connected)
 #else
 static inline int s3c_hsotg_remove(struct dwc2_hsotg *dwc2)
 { return 0; }
@@ -1018,6 +1124,10 @@ static inline void s3c_hsotg_core_init_disconnected(struct dwc2_hsotg *dwc2,
 		bool reset) {}
 static inline void s3c_hsotg_core_connect(struct dwc2_hsotg *hsotg) {}
 static inline void s3c_hsotg_disconnect(struct dwc2_hsotg *dwc2) {}
+static inline int s3c_hsotg_set_test_mode(struct dwc2_hsotg *hsotg,
+							int testmode)
+{ return 0; }
+#define dwc2_is_device_connected(hsotg) (0)
 #endif
 
 #if IS_ENABLED(CONFIG_USB_DWC2_HOST) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
@@ -1025,14 +1135,12 @@ 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)
+static inline int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq)
 { return 0; }
 #endif
 
diff --git a/drivers/usb/dwc2/core_intr.c b/drivers/usb/dwc2/core_intr.c
index 6cf047878dba..927be1e8b3dc 100644
--- a/drivers/usb/dwc2/core_intr.c
+++ b/drivers/usb/dwc2/core_intr.c
@@ -334,6 +334,7 @@ static void dwc2_handle_session_req_intr(struct dwc2_hsotg *hsotg)
  */
 static void dwc2_handle_wakeup_detected_intr(struct dwc2_hsotg *hsotg)
 {
+	int ret;
 	dev_dbg(hsotg->dev, "++Resume or Remote Wakeup Detected Interrupt++\n");
 	dev_dbg(hsotg->dev, "%s lxstate = %d\n", __func__, hsotg->lx_state);
 
@@ -345,6 +346,11 @@ static void dwc2_handle_wakeup_detected_intr(struct dwc2_hsotg *hsotg)
 			/* Clear Remote Wakeup Signaling */
 			dctl &= ~DCTL_RMTWKUPSIG;
 			writel(dctl, hsotg->regs + DCTL);
+			ret = dwc2_exit_hibernation(hsotg, true);
+			if (ret && (ret != -ENOTSUPP))
+				dev_err(hsotg->dev, "exit hibernation failed\n");
+
+			call_gadget(hsotg, resume);
 		}
 		/* Change to L0 state */
 		hsotg->lx_state = DWC2_L0;
@@ -397,6 +403,7 @@ static void dwc2_handle_disconnect_intr(struct dwc2_hsotg *hsotg)
 static void dwc2_handle_usb_suspend_intr(struct dwc2_hsotg *hsotg)
 {
 	u32 dsts;
+	int ret;
 
 	dev_dbg(hsotg->dev, "USB SUSPEND\n");
 
@@ -411,10 +418,43 @@ static void dwc2_handle_usb_suspend_intr(struct dwc2_hsotg *hsotg)
 			"DSTS.Suspend Status=%d HWCFG4.Power Optimize=%d\n",
 			!!(dsts & DSTS_SUSPSTS),
 			hsotg->hw_params.power_optimized);
+		if ((dsts & DSTS_SUSPSTS) && hsotg->hw_params.power_optimized) {
+			/* Ignore suspend request before enumeration */
+			if (!dwc2_is_device_connected(hsotg)) {
+				dev_dbg(hsotg->dev,
+						"ignore suspend request before enumeration\n");
+				goto clear_int;
+			}
+
+			ret = dwc2_enter_hibernation(hsotg);
+			if (ret) {
+				if (ret != -ENOTSUPP)
+					dev_err(hsotg->dev,
+							"enter hibernation failed\n");
+				goto skip_power_saving;
+			}
+
+			udelay(100);
+
+			/* Ask phy to be suspended */
+			if (!IS_ERR_OR_NULL(hsotg->uphy))
+				usb_phy_set_suspend(hsotg->uphy, true);
+skip_power_saving:
+			/*
+			 * Change to L2 (suspend) state before releasing
+			 * spinlock
+			 */
+			hsotg->lx_state = DWC2_L2;
+
+			/* Call gadget suspend callback */
+			call_gadget(hsotg, suspend);
+		}
 	} else {
 		if (hsotg->op_state == OTG_STATE_A_PERIPHERAL) {
 			dev_dbg(hsotg->dev, "a_peripheral->a_host\n");
 
+			/* Change to L2 (suspend) state */
+			hsotg->lx_state = DWC2_L2;
 			/* Clear the a_peripheral flag, back to a_host */
 			spin_unlock(&hsotg->lock);
 			dwc2_hcd_start(hsotg);
@@ -423,9 +463,7 @@ static void dwc2_handle_usb_suspend_intr(struct dwc2_hsotg *hsotg)
 		}
 	}
 
-	/* Change to L2 (suspend) state */
-	hsotg->lx_state = DWC2_L2;
-
+clear_int:
 	/* Clear interrupt */
 	writel(GINTSTS_USBSUSP, hsotg->regs + GINTSTS);
 }
@@ -522,4 +560,3 @@ out:
 	spin_unlock(&hsotg->lock);
 	return retval;
 }
-EXPORT_SYMBOL_GPL(dwc2_handle_common_intr);
diff --git a/drivers/usb/dwc2/debug.h b/drivers/usb/dwc2/debug.h
new file mode 100644
index 000000000000..12dbd1daec87
--- /dev/null
+++ b/drivers/usb/dwc2/debug.h
@@ -0,0 +1,27 @@
+/**
+ * debug.h - Designware USB2 DRD controller debug header
+ *
+ * Copyright (C) 2015 Intel Corporation
+ * Mian Yousaf Kaukab <yousaf.kaukab@intel.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the 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 "core.h"
+
+#ifdef CONFIG_DEBUG_FS
+extern int dwc2_debugfs_init(struct dwc2_hsotg *);
+extern void dwc2_debugfs_exit(struct dwc2_hsotg *);
+#else
+static inline int dwc2_debugfs_init(struct dwc2_hsotg *hsotg)
+{  return 0;  }
+static inline void dwc2_debugfs_exit(struct dwc2_hsotg *hsotg)
+{  }
+#endif
diff --git a/drivers/usb/dwc2/debugfs.c b/drivers/usb/dwc2/debugfs.c
new file mode 100644
index 000000000000..ef2ee3d9a25d
--- /dev/null
+++ b/drivers/usb/dwc2/debugfs.c
@@ -0,0 +1,771 @@
+/**
+ * debugfs.c - Designware USB2 DRD controller debugfs
+ *
+ * Copyright (C) 2015 Intel Corporation
+ * Mian Yousaf Kaukab <yousaf.kaukab@intel.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the 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 <linux/spinlock.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+
+#include "core.h"
+#include "debug.h"
+
+#if IS_ENABLED(CONFIG_USB_DWC2_PERIPHERAL) || \
+	IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
+/**
+ * testmode_write - debugfs: change usb test mode
+ * @seq: The seq file to write to.
+ * @v: Unused parameter.
+ *
+ * This debugfs entry modify the current usb test mode.
+ */
+static ssize_t testmode_write(struct file *file, const char __user *ubuf, size_t
+		count, loff_t *ppos)
+{
+	struct seq_file		*s = file->private_data;
+	struct dwc2_hsotg	*hsotg = s->private;
+	unsigned long		flags;
+	u32			testmode = 0;
+	char			buf[32];
+
+	if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+		return -EFAULT;
+
+	if (!strncmp(buf, "test_j", 6))
+		testmode = TEST_J;
+	else if (!strncmp(buf, "test_k", 6))
+		testmode = TEST_K;
+	else if (!strncmp(buf, "test_se0_nak", 12))
+		testmode = TEST_SE0_NAK;
+	else if (!strncmp(buf, "test_packet", 11))
+		testmode = TEST_PACKET;
+	else if (!strncmp(buf, "test_force_enable", 17))
+		testmode = TEST_FORCE_EN;
+	else
+		testmode = 0;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+	s3c_hsotg_set_test_mode(hsotg, testmode);
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+	return count;
+}
+
+/**
+ * testmode_show - debugfs: show usb test mode state
+ * @seq: The seq file to write to.
+ * @v: Unused parameter.
+ *
+ * This debugfs entry shows which usb test mode is currently enabled.
+ */
+static int testmode_show(struct seq_file *s, void *unused)
+{
+	struct dwc2_hsotg *hsotg = s->private;
+	unsigned long flags;
+	int dctl;
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+	dctl = readl(hsotg->regs + DCTL);
+	dctl &= DCTL_TSTCTL_MASK;
+	dctl >>= DCTL_TSTCTL_SHIFT;
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
+	switch (dctl) {
+	case 0:
+		seq_puts(s, "no test\n");
+		break;
+	case TEST_J:
+		seq_puts(s, "test_j\n");
+		break;
+	case TEST_K:
+		seq_puts(s, "test_k\n");
+		break;
+	case TEST_SE0_NAK:
+		seq_puts(s, "test_se0_nak\n");
+		break;
+	case TEST_PACKET:
+		seq_puts(s, "test_packet\n");
+		break;
+	case TEST_FORCE_EN:
+		seq_puts(s, "test_force_enable\n");
+		break;
+	default:
+		seq_printf(s, "UNKNOWN %d\n", dctl);
+	}
+
+	return 0;
+}
+
+static int testmode_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, testmode_show, inode->i_private);
+}
+
+static const struct file_operations testmode_fops = {
+	.owner		= THIS_MODULE,
+	.open		= testmode_open,
+	.write		= testmode_write,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+/**
+ * state_show - debugfs: show overall driver and device state.
+ * @seq: The seq file to write to.
+ * @v: Unused parameter.
+ *
+ * This debugfs entry shows the overall state of the hardware and
+ * some general information about each of the endpoints available
+ * to the system.
+ */
+static int state_show(struct seq_file *seq, void *v)
+{
+	struct dwc2_hsotg *hsotg = seq->private;
+	void __iomem *regs = hsotg->regs;
+	int idx;
+
+	seq_printf(seq, "DCFG=0x%08x, DCTL=0x%08x, DSTS=0x%08x\n",
+		 readl(regs + DCFG),
+		 readl(regs + DCTL),
+		 readl(regs + DSTS));
+
+	seq_printf(seq, "DIEPMSK=0x%08x, DOEPMASK=0x%08x\n",
+		   readl(regs + DIEPMSK), readl(regs + DOEPMSK));
+
+	seq_printf(seq, "GINTMSK=0x%08x, GINTSTS=0x%08x\n",
+		   readl(regs + GINTMSK),
+		   readl(regs + GINTSTS));
+
+	seq_printf(seq, "DAINTMSK=0x%08x, DAINT=0x%08x\n",
+		   readl(regs + DAINTMSK),
+		   readl(regs + DAINT));
+
+	seq_printf(seq, "GNPTXSTS=0x%08x, GRXSTSR=%08x\n",
+		   readl(regs + GNPTXSTS),
+		   readl(regs + GRXSTSR));
+
+	seq_puts(seq, "\nEndpoint status:\n");
+
+	for (idx = 0; idx < hsotg->num_of_eps; idx++) {
+		u32 in, out;
+
+		in = readl(regs + DIEPCTL(idx));
+		out = readl(regs + DOEPCTL(idx));
+
+		seq_printf(seq, "ep%d: DIEPCTL=0x%08x, DOEPCTL=0x%08x",
+			   idx, in, out);
+
+		in = readl(regs + DIEPTSIZ(idx));
+		out = readl(regs + DOEPTSIZ(idx));
+
+		seq_printf(seq, ", DIEPTSIZ=0x%08x, DOEPTSIZ=0x%08x",
+			   in, out);
+
+		seq_puts(seq, "\n");
+	}
+
+	return 0;
+}
+
+static int state_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, state_show, inode->i_private);
+}
+
+static const struct file_operations state_fops = {
+	.owner		= THIS_MODULE,
+	.open		= state_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+/**
+ * fifo_show - debugfs: show the fifo information
+ * @seq: The seq_file to write data to.
+ * @v: Unused parameter.
+ *
+ * Show the FIFO information for the overall fifo and all the
+ * periodic transmission FIFOs.
+ */
+static int fifo_show(struct seq_file *seq, void *v)
+{
+	struct dwc2_hsotg *hsotg = seq->private;
+	void __iomem *regs = hsotg->regs;
+	u32 val;
+	int idx;
+
+	seq_puts(seq, "Non-periodic FIFOs:\n");
+	seq_printf(seq, "RXFIFO: Size %d\n", readl(regs + GRXFSIZ));
+
+	val = readl(regs + GNPTXFSIZ);
+	seq_printf(seq, "NPTXFIFO: Size %d, Start 0x%08x\n",
+		   val >> FIFOSIZE_DEPTH_SHIFT,
+		   val & FIFOSIZE_DEPTH_MASK);
+
+	seq_puts(seq, "\nPeriodic TXFIFOs:\n");
+
+	for (idx = 1; idx < hsotg->num_of_eps; idx++) {
+		val = readl(regs + DPTXFSIZN(idx));
+
+		seq_printf(seq, "\tDPTXFIFO%2d: Size %d, Start 0x%08x\n", idx,
+			   val >> FIFOSIZE_DEPTH_SHIFT,
+			   val & FIFOSIZE_STARTADDR_MASK);
+	}
+
+	return 0;
+}
+
+static int fifo_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, fifo_show, inode->i_private);
+}
+
+static const struct file_operations fifo_fops = {
+	.owner		= THIS_MODULE,
+	.open		= fifo_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const char *decode_direction(int is_in)
+{
+	return is_in ? "in" : "out";
+}
+
+/**
+ * ep_show - debugfs: show the state of an endpoint.
+ * @seq: The seq_file to write data to.
+ * @v: Unused parameter.
+ *
+ * This debugfs entry shows the state of the given endpoint (one is
+ * registered for each available).
+ */
+static int ep_show(struct seq_file *seq, void *v)
+{
+	struct s3c_hsotg_ep *ep = seq->private;
+	struct dwc2_hsotg *hsotg = ep->parent;
+	struct s3c_hsotg_req *req;
+	void __iomem *regs = hsotg->regs;
+	int index = ep->index;
+	int show_limit = 15;
+	unsigned long flags;
+
+	seq_printf(seq, "Endpoint index %d, named %s,  dir %s:\n",
+		   ep->index, ep->ep.name, decode_direction(ep->dir_in));
+
+	/* first show the register state */
+
+	seq_printf(seq, "\tDIEPCTL=0x%08x, DOEPCTL=0x%08x\n",
+		   readl(regs + DIEPCTL(index)),
+		   readl(regs + DOEPCTL(index)));
+
+	seq_printf(seq, "\tDIEPDMA=0x%08x, DOEPDMA=0x%08x\n",
+		   readl(regs + DIEPDMA(index)),
+		   readl(regs + DOEPDMA(index)));
+
+	seq_printf(seq, "\tDIEPINT=0x%08x, DOEPINT=0x%08x\n",
+		   readl(regs + DIEPINT(index)),
+		   readl(regs + DOEPINT(index)));
+
+	seq_printf(seq, "\tDIEPTSIZ=0x%08x, DOEPTSIZ=0x%08x\n",
+		   readl(regs + DIEPTSIZ(index)),
+		   readl(regs + DOEPTSIZ(index)));
+
+	seq_puts(seq, "\n");
+	seq_printf(seq, "mps %d\n", ep->ep.maxpacket);
+	seq_printf(seq, "total_data=%ld\n", ep->total_data);
+
+	seq_printf(seq, "request list (%p,%p):\n",
+		   ep->queue.next, ep->queue.prev);
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
+	list_for_each_entry(req, &ep->queue, queue) {
+		if (--show_limit < 0) {
+			seq_puts(seq, "not showing more requests...\n");
+			break;
+		}
+
+		seq_printf(seq, "%c req %p: %d bytes @%p, ",
+			   req == ep->req ? '*' : ' ',
+			   req, req->req.length, req->req.buf);
+		seq_printf(seq, "%d done, res %d\n",
+			   req->req.actual, req->req.status);
+	}
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
+	return 0;
+}
+
+static int ep_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ep_show, inode->i_private);
+}
+
+static const struct file_operations ep_fops = {
+	.owner		= THIS_MODULE,
+	.open		= ep_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+/**
+ * s3c_hsotg_create_debug - create debugfs directory and files
+ * @hsotg: The driver state
+ *
+ * Create the debugfs files to allow the user to get information
+ * about the state of the system. The directory name is created
+ * with the same name as the device itself, in case we end up
+ * with multiple blocks in future systems.
+ */
+static void s3c_hsotg_create_debug(struct dwc2_hsotg *hsotg)
+{
+	struct dentry *root;
+	struct dentry *file;
+	unsigned epidx;
+
+	root = hsotg->debug_root;
+
+	/* create general state file */
+
+	file = debugfs_create_file("state", S_IRUGO, root, hsotg, &state_fops);
+	if (IS_ERR(file))
+		dev_err(hsotg->dev, "%s: failed to create state\n", __func__);
+
+	file = debugfs_create_file("testmode", S_IRUGO | S_IWUSR, root, hsotg,
+							&testmode_fops);
+	if (IS_ERR(file))
+		dev_err(hsotg->dev, "%s: failed to create testmode\n",
+				__func__);
+
+	file = debugfs_create_file("fifo", S_IRUGO, root, hsotg, &fifo_fops);
+	if (IS_ERR(file))
+		dev_err(hsotg->dev, "%s: failed to create fifo\n", __func__);
+
+	/* Create one file for each out endpoint */
+	for (epidx = 0; epidx < hsotg->num_of_eps; epidx++) {
+		struct s3c_hsotg_ep *ep;
+
+		ep = hsotg->eps_out[epidx];
+		if (ep) {
+			file = debugfs_create_file(ep->name, S_IRUGO,
+							  root, ep, &ep_fops);
+			if (IS_ERR(file))
+				dev_err(hsotg->dev, "failed to create %s debug file\n",
+					ep->name);
+		}
+	}
+	/* Create one file for each in endpoint. EP0 is handled with out eps */
+	for (epidx = 1; epidx < hsotg->num_of_eps; epidx++) {
+		struct s3c_hsotg_ep *ep;
+
+		ep = hsotg->eps_in[epidx];
+		if (ep) {
+			file = debugfs_create_file(ep->name, S_IRUGO,
+							  root, ep, &ep_fops);
+			if (IS_ERR(file))
+				dev_err(hsotg->dev, "failed to create %s debug file\n",
+					ep->name);
+		}
+	}
+}
+#else
+static inline void s3c_hsotg_create_debug(struct dwc2_hsotg *hsotg) {}
+#endif
+
+/* s3c_hsotg_delete_debug is removed as cleanup in done in dwc2_debugfs_exit */
+
+#define dump_register(nm)	\
+{				\
+	.name	= #nm,		\
+	.offset	= nm,		\
+}
+
+static const struct debugfs_reg32 dwc2_regs[] = {
+	/*
+	 * Accessing registers like this can trigger mode mismatch interrupt.
+	 * However, according to dwc2 databook, the register access, in this
+	 * case, is completed on the processor bus but is ignored by the core
+	 * and does not affect its operation.
+	 */
+	dump_register(GOTGCTL),
+	dump_register(GOTGINT),
+	dump_register(GAHBCFG),
+	dump_register(GUSBCFG),
+	dump_register(GRSTCTL),
+	dump_register(GINTSTS),
+	dump_register(GINTMSK),
+	dump_register(GRXSTSR),
+	dump_register(GRXSTSP),
+	dump_register(GRXFSIZ),
+	dump_register(GNPTXFSIZ),
+	dump_register(GNPTXSTS),
+	dump_register(GI2CCTL),
+	dump_register(GPVNDCTL),
+	dump_register(GGPIO),
+	dump_register(GUID),
+	dump_register(GSNPSID),
+	dump_register(GHWCFG1),
+	dump_register(GHWCFG2),
+	dump_register(GHWCFG3),
+	dump_register(GHWCFG4),
+	dump_register(GLPMCFG),
+	dump_register(GPWRDN),
+	dump_register(GDFIFOCFG),
+	dump_register(ADPCTL),
+	dump_register(HPTXFSIZ),
+	dump_register(DPTXFSIZN(1)),
+	dump_register(DPTXFSIZN(2)),
+	dump_register(DPTXFSIZN(3)),
+	dump_register(DPTXFSIZN(4)),
+	dump_register(DPTXFSIZN(5)),
+	dump_register(DPTXFSIZN(6)),
+	dump_register(DPTXFSIZN(7)),
+	dump_register(DPTXFSIZN(8)),
+	dump_register(DPTXFSIZN(9)),
+	dump_register(DPTXFSIZN(10)),
+	dump_register(DPTXFSIZN(11)),
+	dump_register(DPTXFSIZN(12)),
+	dump_register(DPTXFSIZN(13)),
+	dump_register(DPTXFSIZN(14)),
+	dump_register(DPTXFSIZN(15)),
+	dump_register(DCFG),
+	dump_register(DCTL),
+	dump_register(DSTS),
+	dump_register(DIEPMSK),
+	dump_register(DOEPMSK),
+	dump_register(DAINT),
+	dump_register(DAINTMSK),
+	dump_register(DTKNQR1),
+	dump_register(DTKNQR2),
+	dump_register(DTKNQR3),
+	dump_register(DTKNQR4),
+	dump_register(DVBUSDIS),
+	dump_register(DVBUSPULSE),
+	dump_register(DIEPCTL(0)),
+	dump_register(DIEPCTL(1)),
+	dump_register(DIEPCTL(2)),
+	dump_register(DIEPCTL(3)),
+	dump_register(DIEPCTL(4)),
+	dump_register(DIEPCTL(5)),
+	dump_register(DIEPCTL(6)),
+	dump_register(DIEPCTL(7)),
+	dump_register(DIEPCTL(8)),
+	dump_register(DIEPCTL(9)),
+	dump_register(DIEPCTL(10)),
+	dump_register(DIEPCTL(11)),
+	dump_register(DIEPCTL(12)),
+	dump_register(DIEPCTL(13)),
+	dump_register(DIEPCTL(14)),
+	dump_register(DIEPCTL(15)),
+	dump_register(DOEPCTL(0)),
+	dump_register(DOEPCTL(1)),
+	dump_register(DOEPCTL(2)),
+	dump_register(DOEPCTL(3)),
+	dump_register(DOEPCTL(4)),
+	dump_register(DOEPCTL(5)),
+	dump_register(DOEPCTL(6)),
+	dump_register(DOEPCTL(7)),
+	dump_register(DOEPCTL(8)),
+	dump_register(DOEPCTL(9)),
+	dump_register(DOEPCTL(10)),
+	dump_register(DOEPCTL(11)),
+	dump_register(DOEPCTL(12)),
+	dump_register(DOEPCTL(13)),
+	dump_register(DOEPCTL(14)),
+	dump_register(DOEPCTL(15)),
+	dump_register(DIEPINT(0)),
+	dump_register(DIEPINT(1)),
+	dump_register(DIEPINT(2)),
+	dump_register(DIEPINT(3)),
+	dump_register(DIEPINT(4)),
+	dump_register(DIEPINT(5)),
+	dump_register(DIEPINT(6)),
+	dump_register(DIEPINT(7)),
+	dump_register(DIEPINT(8)),
+	dump_register(DIEPINT(9)),
+	dump_register(DIEPINT(10)),
+	dump_register(DIEPINT(11)),
+	dump_register(DIEPINT(12)),
+	dump_register(DIEPINT(13)),
+	dump_register(DIEPINT(14)),
+	dump_register(DIEPINT(15)),
+	dump_register(DOEPINT(0)),
+	dump_register(DOEPINT(1)),
+	dump_register(DOEPINT(2)),
+	dump_register(DOEPINT(3)),
+	dump_register(DOEPINT(4)),
+	dump_register(DOEPINT(5)),
+	dump_register(DOEPINT(6)),
+	dump_register(DOEPINT(7)),
+	dump_register(DOEPINT(8)),
+	dump_register(DOEPINT(9)),
+	dump_register(DOEPINT(10)),
+	dump_register(DOEPINT(11)),
+	dump_register(DOEPINT(12)),
+	dump_register(DOEPINT(13)),
+	dump_register(DOEPINT(14)),
+	dump_register(DOEPINT(15)),
+	dump_register(DIEPTSIZ(0)),
+	dump_register(DIEPTSIZ(1)),
+	dump_register(DIEPTSIZ(2)),
+	dump_register(DIEPTSIZ(3)),
+	dump_register(DIEPTSIZ(4)),
+	dump_register(DIEPTSIZ(5)),
+	dump_register(DIEPTSIZ(6)),
+	dump_register(DIEPTSIZ(7)),
+	dump_register(DIEPTSIZ(8)),
+	dump_register(DIEPTSIZ(9)),
+	dump_register(DIEPTSIZ(10)),
+	dump_register(DIEPTSIZ(11)),
+	dump_register(DIEPTSIZ(12)),
+	dump_register(DIEPTSIZ(13)),
+	dump_register(DIEPTSIZ(14)),
+	dump_register(DIEPTSIZ(15)),
+	dump_register(DOEPTSIZ(0)),
+	dump_register(DOEPTSIZ(1)),
+	dump_register(DOEPTSIZ(2)),
+	dump_register(DOEPTSIZ(3)),
+	dump_register(DOEPTSIZ(4)),
+	dump_register(DOEPTSIZ(5)),
+	dump_register(DOEPTSIZ(6)),
+	dump_register(DOEPTSIZ(7)),
+	dump_register(DOEPTSIZ(8)),
+	dump_register(DOEPTSIZ(9)),
+	dump_register(DOEPTSIZ(10)),
+	dump_register(DOEPTSIZ(11)),
+	dump_register(DOEPTSIZ(12)),
+	dump_register(DOEPTSIZ(13)),
+	dump_register(DOEPTSIZ(14)),
+	dump_register(DOEPTSIZ(15)),
+	dump_register(DIEPDMA(0)),
+	dump_register(DIEPDMA(1)),
+	dump_register(DIEPDMA(2)),
+	dump_register(DIEPDMA(3)),
+	dump_register(DIEPDMA(4)),
+	dump_register(DIEPDMA(5)),
+	dump_register(DIEPDMA(6)),
+	dump_register(DIEPDMA(7)),
+	dump_register(DIEPDMA(8)),
+	dump_register(DIEPDMA(9)),
+	dump_register(DIEPDMA(10)),
+	dump_register(DIEPDMA(11)),
+	dump_register(DIEPDMA(12)),
+	dump_register(DIEPDMA(13)),
+	dump_register(DIEPDMA(14)),
+	dump_register(DIEPDMA(15)),
+	dump_register(DOEPDMA(0)),
+	dump_register(DOEPDMA(1)),
+	dump_register(DOEPDMA(2)),
+	dump_register(DOEPDMA(3)),
+	dump_register(DOEPDMA(4)),
+	dump_register(DOEPDMA(5)),
+	dump_register(DOEPDMA(6)),
+	dump_register(DOEPDMA(7)),
+	dump_register(DOEPDMA(8)),
+	dump_register(DOEPDMA(9)),
+	dump_register(DOEPDMA(10)),
+	dump_register(DOEPDMA(11)),
+	dump_register(DOEPDMA(12)),
+	dump_register(DOEPDMA(13)),
+	dump_register(DOEPDMA(14)),
+	dump_register(DOEPDMA(15)),
+	dump_register(DTXFSTS(0)),
+	dump_register(DTXFSTS(1)),
+	dump_register(DTXFSTS(2)),
+	dump_register(DTXFSTS(3)),
+	dump_register(DTXFSTS(4)),
+	dump_register(DTXFSTS(5)),
+	dump_register(DTXFSTS(6)),
+	dump_register(DTXFSTS(7)),
+	dump_register(DTXFSTS(8)),
+	dump_register(DTXFSTS(9)),
+	dump_register(DTXFSTS(10)),
+	dump_register(DTXFSTS(11)),
+	dump_register(DTXFSTS(12)),
+	dump_register(DTXFSTS(13)),
+	dump_register(DTXFSTS(14)),
+	dump_register(DTXFSTS(15)),
+	dump_register(PCGCTL),
+	dump_register(HCFG),
+	dump_register(HFIR),
+	dump_register(HFNUM),
+	dump_register(HPTXSTS),
+	dump_register(HAINT),
+	dump_register(HAINTMSK),
+	dump_register(HFLBADDR),
+	dump_register(HPRT0),
+	dump_register(HCCHAR(0)),
+	dump_register(HCCHAR(1)),
+	dump_register(HCCHAR(2)),
+	dump_register(HCCHAR(3)),
+	dump_register(HCCHAR(4)),
+	dump_register(HCCHAR(5)),
+	dump_register(HCCHAR(6)),
+	dump_register(HCCHAR(7)),
+	dump_register(HCCHAR(8)),
+	dump_register(HCCHAR(9)),
+	dump_register(HCCHAR(10)),
+	dump_register(HCCHAR(11)),
+	dump_register(HCCHAR(12)),
+	dump_register(HCCHAR(13)),
+	dump_register(HCCHAR(14)),
+	dump_register(HCCHAR(15)),
+	dump_register(HCSPLT(0)),
+	dump_register(HCSPLT(1)),
+	dump_register(HCSPLT(2)),
+	dump_register(HCSPLT(3)),
+	dump_register(HCSPLT(4)),
+	dump_register(HCSPLT(5)),
+	dump_register(HCSPLT(6)),
+	dump_register(HCSPLT(7)),
+	dump_register(HCSPLT(8)),
+	dump_register(HCSPLT(9)),
+	dump_register(HCSPLT(10)),
+	dump_register(HCSPLT(11)),
+	dump_register(HCSPLT(12)),
+	dump_register(HCSPLT(13)),
+	dump_register(HCSPLT(14)),
+	dump_register(HCSPLT(15)),
+	dump_register(HCINT(0)),
+	dump_register(HCINT(1)),
+	dump_register(HCINT(2)),
+	dump_register(HCINT(3)),
+	dump_register(HCINT(4)),
+	dump_register(HCINT(5)),
+	dump_register(HCINT(6)),
+	dump_register(HCINT(7)),
+	dump_register(HCINT(8)),
+	dump_register(HCINT(9)),
+	dump_register(HCINT(10)),
+	dump_register(HCINT(11)),
+	dump_register(HCINT(12)),
+	dump_register(HCINT(13)),
+	dump_register(HCINT(14)),
+	dump_register(HCINT(15)),
+	dump_register(HCINTMSK(0)),
+	dump_register(HCINTMSK(1)),
+	dump_register(HCINTMSK(2)),
+	dump_register(HCINTMSK(3)),
+	dump_register(HCINTMSK(4)),
+	dump_register(HCINTMSK(5)),
+	dump_register(HCINTMSK(6)),
+	dump_register(HCINTMSK(7)),
+	dump_register(HCINTMSK(8)),
+	dump_register(HCINTMSK(9)),
+	dump_register(HCINTMSK(10)),
+	dump_register(HCINTMSK(11)),
+	dump_register(HCINTMSK(12)),
+	dump_register(HCINTMSK(13)),
+	dump_register(HCINTMSK(14)),
+	dump_register(HCINTMSK(15)),
+	dump_register(HCTSIZ(0)),
+	dump_register(HCTSIZ(1)),
+	dump_register(HCTSIZ(2)),
+	dump_register(HCTSIZ(3)),
+	dump_register(HCTSIZ(4)),
+	dump_register(HCTSIZ(5)),
+	dump_register(HCTSIZ(6)),
+	dump_register(HCTSIZ(7)),
+	dump_register(HCTSIZ(8)),
+	dump_register(HCTSIZ(9)),
+	dump_register(HCTSIZ(10)),
+	dump_register(HCTSIZ(11)),
+	dump_register(HCTSIZ(12)),
+	dump_register(HCTSIZ(13)),
+	dump_register(HCTSIZ(14)),
+	dump_register(HCTSIZ(15)),
+	dump_register(HCDMA(0)),
+	dump_register(HCDMA(1)),
+	dump_register(HCDMA(2)),
+	dump_register(HCDMA(3)),
+	dump_register(HCDMA(4)),
+	dump_register(HCDMA(5)),
+	dump_register(HCDMA(6)),
+	dump_register(HCDMA(7)),
+	dump_register(HCDMA(8)),
+	dump_register(HCDMA(9)),
+	dump_register(HCDMA(10)),
+	dump_register(HCDMA(11)),
+	dump_register(HCDMA(12)),
+	dump_register(HCDMA(13)),
+	dump_register(HCDMA(14)),
+	dump_register(HCDMA(15)),
+	dump_register(HCDMAB(0)),
+	dump_register(HCDMAB(1)),
+	dump_register(HCDMAB(2)),
+	dump_register(HCDMAB(3)),
+	dump_register(HCDMAB(4)),
+	dump_register(HCDMAB(5)),
+	dump_register(HCDMAB(6)),
+	dump_register(HCDMAB(7)),
+	dump_register(HCDMAB(8)),
+	dump_register(HCDMAB(9)),
+	dump_register(HCDMAB(10)),
+	dump_register(HCDMAB(11)),
+	dump_register(HCDMAB(12)),
+	dump_register(HCDMAB(13)),
+	dump_register(HCDMAB(14)),
+	dump_register(HCDMAB(15)),
+};
+
+int dwc2_debugfs_init(struct dwc2_hsotg *hsotg)
+{
+	int			ret;
+	struct dentry		*file;
+
+	hsotg->debug_root = debugfs_create_dir(dev_name(hsotg->dev), NULL);
+	if (!hsotg->debug_root) {
+		ret = -ENOMEM;
+		goto err0;
+	}
+
+	/* Add gadget debugfs nodes */
+	s3c_hsotg_create_debug(hsotg);
+
+	hsotg->regset = devm_kzalloc(hsotg->dev, sizeof(*hsotg->regset),
+								GFP_KERNEL);
+	if (!hsotg->regset) {
+		ret = -ENOMEM;
+		goto err1;
+	}
+
+	hsotg->regset->regs = dwc2_regs;
+	hsotg->regset->nregs = ARRAY_SIZE(dwc2_regs);
+	hsotg->regset->base = hsotg->regs;
+
+	file = debugfs_create_regset32("regdump", S_IRUGO, hsotg->debug_root,
+								hsotg->regset);
+	if (!file) {
+		ret = -ENOMEM;
+		goto err1;
+	}
+
+	return 0;
+err1:
+	debugfs_remove_recursive(hsotg->debug_root);
+err0:
+	return ret;
+}
+
+void dwc2_debugfs_exit(struct dwc2_hsotg *hsotg)
+{
+	debugfs_remove_recursive(hsotg->debug_root);
+	hsotg->debug_root = NULL;
+}
diff --git a/drivers/usb/dwc2/gadget.c b/drivers/usb/dwc2/gadget.c
index 6a30887082cd..4d47b7c09238 100644
--- a/drivers/usb/dwc2/gadget.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -20,7 +20,6 @@
 #include <linux/interrupt.h>
 #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>
@@ -35,7 +34,6 @@
 #include <linux/usb/gadget.h>
 #include <linux/usb/phy.h>
 #include <linux/platform_data/s3c-hsotg.h>
-#include <linux/uaccess.h>
 
 #include "core.h"
 #include "hw.h"
@@ -792,6 +790,13 @@ static int s3c_hsotg_ep_queue(struct usb_ep *ep, struct usb_request *req,
 		ep->name, req, req->length, req->buf, req->no_interrupt,
 		req->zero, req->short_not_ok);
 
+	/* Prevent new request submission when controller is suspended */
+	if (hs->lx_state == DWC2_L2) {
+		dev_dbg(hs->dev, "%s: don't submit request while suspended\n",
+				__func__);
+		return -EAGAIN;
+	}
+
 	/* initialise status of the request */
 	INIT_LIST_HEAD(&hs_req->queue);
 	req->actual = 0;
@@ -894,7 +899,7 @@ static struct s3c_hsotg_ep *ep_from_windex(struct dwc2_hsotg *hsotg,
  * @testmode: requested usb test mode
  * Enable usb Test Mode requested by the Host.
  */
-static int s3c_hsotg_set_test_mode(struct dwc2_hsotg *hsotg, int testmode)
+int s3c_hsotg_set_test_mode(struct dwc2_hsotg *hsotg, int testmode)
 {
 	int dctl = readl(hsotg->regs + DCTL);
 
@@ -2185,7 +2190,6 @@ void s3c_hsotg_disconnect(struct dwc2_hsotg *hsotg)
 
 	call_gadget(hsotg, disconnect);
 }
-EXPORT_SYMBOL_GPL(s3c_hsotg_disconnect);
 
 /**
  * s3c_hsotg_irq_fifoempty - TX FIFO empty interrupt handler
@@ -2310,8 +2314,9 @@ void s3c_hsotg_core_init_disconnected(struct dwc2_hsotg *hsotg,
 	writel(GINTSTS_ERLYSUSP | GINTSTS_SESSREQINT |
 		GINTSTS_GOUTNAKEFF | GINTSTS_GINNAKEFF |
 		GINTSTS_CONIDSTSCHNG | GINTSTS_USBRST |
-		GINTSTS_ENUMDONE | GINTSTS_OTGINT |
-		GINTSTS_USBSUSP | GINTSTS_WKUPINT,
+		GINTSTS_RESETDET | GINTSTS_ENUMDONE |
+		GINTSTS_OTGINT | GINTSTS_USBSUSP |
+		GINTSTS_WKUPINT,
 		hsotg->regs + GINTMSK);
 
 	if (using_dma(hsotg))
@@ -2477,7 +2482,19 @@ irq_retry:
 		}
 	}
 
-	if (gintsts & GINTSTS_USBRST) {
+	if (gintsts & GINTSTS_RESETDET) {
+		dev_dbg(hsotg->dev, "%s: USBRstDet\n", __func__);
+
+		writel(GINTSTS_RESETDET, hsotg->regs + GINTSTS);
+
+		/* This event must be used only if controller is suspended */
+		if (hsotg->lx_state == DWC2_L2) {
+			dwc2_exit_hibernation(hsotg, true);
+			hsotg->lx_state = DWC2_L0;
+		}
+	}
+
+	if (gintsts & (GINTSTS_USBRST | GINTSTS_RESETDET)) {
 
 		u32 usb_status = readl(hsotg->regs + GOTGCTL);
 
@@ -2497,6 +2514,7 @@ irq_retry:
 				kill_all_requests(hsotg, hsotg->eps_out[0],
 							  -ECONNRESET);
 
+				hsotg->lx_state = DWC2_L0;
 				s3c_hsotg_core_init_disconnected(hsotg, true);
 			}
 		}
@@ -2745,7 +2763,7 @@ error:
  * s3c_hsotg_ep_disable - disable given endpoint
  * @ep: The endpoint to disable.
  */
-static int s3c_hsotg_ep_disable_force(struct usb_ep *ep, bool force)
+static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 {
 	struct s3c_hsotg_ep *hs_ep = our_ep(ep);
 	struct dwc2_hsotg *hsotg = hs_ep->parent;
@@ -2788,10 +2806,6 @@ static int s3c_hsotg_ep_disable_force(struct usb_ep *ep, bool force)
 	return 0;
 }
 
-static int s3c_hsotg_ep_disable(struct usb_ep *ep)
-{
-	return s3c_hsotg_ep_disable_force(ep, false);
-}
 /**
  * on_list - check request is on the given endpoint
  * @ep: The endpoint to check.
@@ -3187,6 +3201,14 @@ static int s3c_hsotg_vbus_session(struct usb_gadget *gadget, int is_active)
 	spin_lock_irqsave(&hsotg->lock, flags);
 
 	if (is_active) {
+		/*
+		 * If controller is hibernated, it must exit from hibernation
+		 * before being initialized
+		 */
+		if (hsotg->lx_state == DWC2_L2) {
+			dwc2_exit_hibernation(hsotg, false);
+			hsotg->lx_state = DWC2_L0;
+		}
 		/* Kill any ep0 requests as controller will be reinitialized */
 		kill_all_requests(hsotg, hsotg->eps_out[0], -ECONNRESET);
 		s3c_hsotg_core_init_disconnected(hsotg, false);
@@ -3391,404 +3413,6 @@ static void s3c_hsotg_dump(struct dwc2_hsotg *hsotg)
 #endif
 }
 
-/**
- * testmode_write - debugfs: change usb test mode
- * @seq: The seq file to write to.
- * @v: Unused parameter.
- *
- * This debugfs entry modify the current usb test mode.
- */
-static ssize_t testmode_write(struct file *file, const char __user *ubuf, size_t
-		count, loff_t *ppos)
-{
-	struct seq_file		*s = file->private_data;
-	struct dwc2_hsotg	*hsotg = s->private;
-	unsigned long		flags;
-	u32			testmode = 0;
-	char			buf[32];
-
-	if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
-		return -EFAULT;
-
-	if (!strncmp(buf, "test_j", 6))
-		testmode = TEST_J;
-	else if (!strncmp(buf, "test_k", 6))
-		testmode = TEST_K;
-	else if (!strncmp(buf, "test_se0_nak", 12))
-		testmode = TEST_SE0_NAK;
-	else if (!strncmp(buf, "test_packet", 11))
-		testmode = TEST_PACKET;
-	else if (!strncmp(buf, "test_force_enable", 17))
-		testmode = TEST_FORCE_EN;
-	else
-		testmode = 0;
-
-	spin_lock_irqsave(&hsotg->lock, flags);
-	s3c_hsotg_set_test_mode(hsotg, testmode);
-	spin_unlock_irqrestore(&hsotg->lock, flags);
-	return count;
-}
-
-/**
- * testmode_show - debugfs: show usb test mode state
- * @seq: The seq file to write to.
- * @v: Unused parameter.
- *
- * This debugfs entry shows which usb test mode is currently enabled.
- */
-static int testmode_show(struct seq_file *s, void *unused)
-{
-	struct dwc2_hsotg *hsotg = s->private;
-	unsigned long flags;
-	int dctl;
-
-	spin_lock_irqsave(&hsotg->lock, flags);
-	dctl = readl(hsotg->regs + DCTL);
-	dctl &= DCTL_TSTCTL_MASK;
-	dctl >>= DCTL_TSTCTL_SHIFT;
-	spin_unlock_irqrestore(&hsotg->lock, flags);
-
-	switch (dctl) {
-	case 0:
-		seq_puts(s, "no test\n");
-		break;
-	case TEST_J:
-		seq_puts(s, "test_j\n");
-		break;
-	case TEST_K:
-		seq_puts(s, "test_k\n");
-		break;
-	case TEST_SE0_NAK:
-		seq_puts(s, "test_se0_nak\n");
-		break;
-	case TEST_PACKET:
-		seq_puts(s, "test_packet\n");
-		break;
-	case TEST_FORCE_EN:
-		seq_puts(s, "test_force_enable\n");
-		break;
-	default:
-		seq_printf(s, "UNKNOWN %d\n", dctl);
-	}
-
-	return 0;
-}
-
-static int testmode_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, testmode_show, inode->i_private);
-}
-
-static const struct file_operations testmode_fops = {
-	.owner		= THIS_MODULE,
-	.open		= testmode_open,
-	.write		= testmode_write,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-/**
- * state_show - debugfs: show overall driver and device state.
- * @seq: The seq file to write to.
- * @v: Unused parameter.
- *
- * This debugfs entry shows the overall state of the hardware and
- * some general information about each of the endpoints available
- * to the system.
- */
-static int state_show(struct seq_file *seq, void *v)
-{
-	struct dwc2_hsotg *hsotg = seq->private;
-	void __iomem *regs = hsotg->regs;
-	int idx;
-
-	seq_printf(seq, "DCFG=0x%08x, DCTL=0x%08x, DSTS=0x%08x\n",
-		 readl(regs + DCFG),
-		 readl(regs + DCTL),
-		 readl(regs + DSTS));
-
-	seq_printf(seq, "DIEPMSK=0x%08x, DOEPMASK=0x%08x\n",
-		   readl(regs + DIEPMSK), readl(regs + DOEPMSK));
-
-	seq_printf(seq, "GINTMSK=0x%08x, GINTSTS=0x%08x\n",
-		   readl(regs + GINTMSK),
-		   readl(regs + GINTSTS));
-
-	seq_printf(seq, "DAINTMSK=0x%08x, DAINT=0x%08x\n",
-		   readl(regs + DAINTMSK),
-		   readl(regs + DAINT));
-
-	seq_printf(seq, "GNPTXSTS=0x%08x, GRXSTSR=%08x\n",
-		   readl(regs + GNPTXSTS),
-		   readl(regs + GRXSTSR));
-
-	seq_puts(seq, "\nEndpoint status:\n");
-
-	for (idx = 0; idx < hsotg->num_of_eps; idx++) {
-		u32 in, out;
-
-		in = readl(regs + DIEPCTL(idx));
-		out = readl(regs + DOEPCTL(idx));
-
-		seq_printf(seq, "ep%d: DIEPCTL=0x%08x, DOEPCTL=0x%08x",
-			   idx, in, out);
-
-		in = readl(regs + DIEPTSIZ(idx));
-		out = readl(regs + DOEPTSIZ(idx));
-
-		seq_printf(seq, ", DIEPTSIZ=0x%08x, DOEPTSIZ=0x%08x",
-			   in, out);
-
-		seq_puts(seq, "\n");
-	}
-
-	return 0;
-}
-
-static int state_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, state_show, inode->i_private);
-}
-
-static const struct file_operations state_fops = {
-	.owner		= THIS_MODULE,
-	.open		= state_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-/**
- * fifo_show - debugfs: show the fifo information
- * @seq: The seq_file to write data to.
- * @v: Unused parameter.
- *
- * Show the FIFO information for the overall fifo and all the
- * periodic transmission FIFOs.
- */
-static int fifo_show(struct seq_file *seq, void *v)
-{
-	struct dwc2_hsotg *hsotg = seq->private;
-	void __iomem *regs = hsotg->regs;
-	u32 val;
-	int idx;
-
-	seq_puts(seq, "Non-periodic FIFOs:\n");
-	seq_printf(seq, "RXFIFO: Size %d\n", readl(regs + GRXFSIZ));
-
-	val = readl(regs + GNPTXFSIZ);
-	seq_printf(seq, "NPTXFIFO: Size %d, Start 0x%08x\n",
-		   val >> FIFOSIZE_DEPTH_SHIFT,
-		   val & FIFOSIZE_DEPTH_MASK);
-
-	seq_puts(seq, "\nPeriodic TXFIFOs:\n");
-
-	for (idx = 1; idx < hsotg->num_of_eps; idx++) {
-		val = readl(regs + DPTXFSIZN(idx));
-
-		seq_printf(seq, "\tDPTXFIFO%2d: Size %d, Start 0x%08x\n", idx,
-			   val >> FIFOSIZE_DEPTH_SHIFT,
-			   val & FIFOSIZE_STARTADDR_MASK);
-	}
-
-	return 0;
-}
-
-static int fifo_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, fifo_show, inode->i_private);
-}
-
-static const struct file_operations fifo_fops = {
-	.owner		= THIS_MODULE,
-	.open		= fifo_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-
-static const char *decode_direction(int is_in)
-{
-	return is_in ? "in" : "out";
-}
-
-/**
- * ep_show - debugfs: show the state of an endpoint.
- * @seq: The seq_file to write data to.
- * @v: Unused parameter.
- *
- * This debugfs entry shows the state of the given endpoint (one is
- * registered for each available).
- */
-static int ep_show(struct seq_file *seq, void *v)
-{
-	struct s3c_hsotg_ep *ep = seq->private;
-	struct dwc2_hsotg *hsotg = ep->parent;
-	struct s3c_hsotg_req *req;
-	void __iomem *regs = hsotg->regs;
-	int index = ep->index;
-	int show_limit = 15;
-	unsigned long flags;
-
-	seq_printf(seq, "Endpoint index %d, named %s,  dir %s:\n",
-		   ep->index, ep->ep.name, decode_direction(ep->dir_in));
-
-	/* first show the register state */
-
-	seq_printf(seq, "\tDIEPCTL=0x%08x, DOEPCTL=0x%08x\n",
-		   readl(regs + DIEPCTL(index)),
-		   readl(regs + DOEPCTL(index)));
-
-	seq_printf(seq, "\tDIEPDMA=0x%08x, DOEPDMA=0x%08x\n",
-		   readl(regs + DIEPDMA(index)),
-		   readl(regs + DOEPDMA(index)));
-
-	seq_printf(seq, "\tDIEPINT=0x%08x, DOEPINT=0x%08x\n",
-		   readl(regs + DIEPINT(index)),
-		   readl(regs + DOEPINT(index)));
-
-	seq_printf(seq, "\tDIEPTSIZ=0x%08x, DOEPTSIZ=0x%08x\n",
-		   readl(regs + DIEPTSIZ(index)),
-		   readl(regs + DOEPTSIZ(index)));
-
-	seq_puts(seq, "\n");
-	seq_printf(seq, "mps %d\n", ep->ep.maxpacket);
-	seq_printf(seq, "total_data=%ld\n", ep->total_data);
-
-	seq_printf(seq, "request list (%p,%p):\n",
-		   ep->queue.next, ep->queue.prev);
-
-	spin_lock_irqsave(&hsotg->lock, flags);
-
-	list_for_each_entry(req, &ep->queue, queue) {
-		if (--show_limit < 0) {
-			seq_puts(seq, "not showing more requests...\n");
-			break;
-		}
-
-		seq_printf(seq, "%c req %p: %d bytes @%p, ",
-			   req == ep->req ? '*' : ' ',
-			   req, req->req.length, req->req.buf);
-		seq_printf(seq, "%d done, res %d\n",
-			   req->req.actual, req->req.status);
-	}
-
-	spin_unlock_irqrestore(&hsotg->lock, flags);
-
-	return 0;
-}
-
-static int ep_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, ep_show, inode->i_private);
-}
-
-static const struct file_operations ep_fops = {
-	.owner		= THIS_MODULE,
-	.open		= ep_open,
-	.read		= seq_read,
-	.llseek		= seq_lseek,
-	.release	= single_release,
-};
-
-/**
- * s3c_hsotg_create_debug - create debugfs directory and files
- * @hsotg: The driver state
- *
- * Create the debugfs files to allow the user to get information
- * about the state of the system. The directory name is created
- * with the same name as the device itself, in case we end up
- * with multiple blocks in future systems.
- */
-static void s3c_hsotg_create_debug(struct dwc2_hsotg *hsotg)
-{
-	struct dentry *root;
-	unsigned epidx;
-
-	root = debugfs_create_dir(dev_name(hsotg->dev), NULL);
-	hsotg->debug_root = root;
-	if (IS_ERR(root)) {
-		dev_err(hsotg->dev, "cannot create debug root\n");
-		return;
-	}
-
-	/* create general state file */
-
-	hsotg->debug_file = debugfs_create_file("state", S_IRUGO, root,
-						hsotg, &state_fops);
-
-	if (IS_ERR(hsotg->debug_file))
-		dev_err(hsotg->dev, "%s: failed to create state\n", __func__);
-
-	hsotg->debug_testmode = debugfs_create_file("testmode",
-					S_IRUGO | S_IWUSR, root,
-					hsotg, &testmode_fops);
-
-	if (IS_ERR(hsotg->debug_testmode))
-		dev_err(hsotg->dev, "%s: failed to create testmode\n",
-				__func__);
-
-	hsotg->debug_fifo = debugfs_create_file("fifo", S_IRUGO, root,
-						hsotg, &fifo_fops);
-
-	if (IS_ERR(hsotg->debug_fifo))
-		dev_err(hsotg->dev, "%s: failed to create fifo\n", __func__);
-
-	/* Create one file for each out endpoint */
-	for (epidx = 0; epidx < hsotg->num_of_eps; epidx++) {
-		struct s3c_hsotg_ep *ep;
-
-		ep = hsotg->eps_out[epidx];
-		if (ep) {
-			ep->debugfs = debugfs_create_file(ep->name, S_IRUGO,
-							  root, ep, &ep_fops);
-
-			if (IS_ERR(ep->debugfs))
-				dev_err(hsotg->dev, "failed to create %s debug file\n",
-					ep->name);
-		}
-	}
-	/* Create one file for each in endpoint. EP0 is handled with out eps */
-	for (epidx = 1; epidx < hsotg->num_of_eps; epidx++) {
-		struct s3c_hsotg_ep *ep;
-
-		ep = hsotg->eps_in[epidx];
-		if (ep) {
-			ep->debugfs = debugfs_create_file(ep->name, S_IRUGO,
-							  root, ep, &ep_fops);
-
-			if (IS_ERR(ep->debugfs))
-				dev_err(hsotg->dev, "failed to create %s debug file\n",
-					ep->name);
-		}
-	}
-}
-
-/**
- * s3c_hsotg_delete_debug - cleanup debugfs entries
- * @hsotg: The driver state
- *
- * Cleanup (remove) the debugfs files for use on module exit.
- */
-static void s3c_hsotg_delete_debug(struct dwc2_hsotg *hsotg)
-{
-	unsigned epidx;
-
-	for (epidx = 0; epidx < hsotg->num_of_eps; epidx++) {
-		if (hsotg->eps_in[epidx])
-			debugfs_remove(hsotg->eps_in[epidx]->debugfs);
-		if (hsotg->eps_out[epidx])
-			debugfs_remove(hsotg->eps_out[epidx]->debugfs);
-	}
-
-	debugfs_remove(hsotg->debug_file);
-	debugfs_remove(hsotg->debug_testmode);
-	debugfs_remove(hsotg->debug_fifo);
-	debugfs_remove(hsotg->debug_root);
-}
-
 #ifdef CONFIG_OF
 static void s3c_hsotg_of_probe(struct dwc2_hsotg *hsotg)
 {
@@ -3896,6 +3520,8 @@ int dwc2_gadget_init(struct dwc2_hsotg *hsotg, int irq)
 	hsotg->gadget.max_speed = USB_SPEED_HIGH;
 	hsotg->gadget.ops = &s3c_hsotg_gadget_ops;
 	hsotg->gadget.name = dev_name(dev);
+	if (hsotg->dr_mode == USB_DR_MODE_OTG)
+		hsotg->gadget.is_otg = 1;
 
 	/* reset the system */
 
@@ -4028,8 +3654,6 @@ int dwc2_gadget_init(struct dwc2_hsotg *hsotg, int irq)
 	if (ret)
 		goto err_supplies;
 
-	s3c_hsotg_create_debug(hsotg);
-
 	s3c_hsotg_dump(hsotg);
 
 	return 0;
@@ -4041,7 +3665,6 @@ err_clk:
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(dwc2_gadget_init);
 
 /**
  * s3c_hsotg_remove - remove function for hsotg driver
@@ -4050,18 +3673,19 @@ EXPORT_SYMBOL_GPL(dwc2_gadget_init);
 int s3c_hsotg_remove(struct dwc2_hsotg *hsotg)
 {
 	usb_del_gadget_udc(&hsotg->gadget);
-	s3c_hsotg_delete_debug(hsotg);
 	clk_disable_unprepare(hsotg->clk);
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(s3c_hsotg_remove);
 
 int s3c_hsotg_suspend(struct dwc2_hsotg *hsotg)
 {
 	unsigned long flags;
 	int ret = 0;
 
+	if (hsotg->lx_state != DWC2_L0)
+		return ret;
+
 	mutex_lock(&hsotg->init_mutex);
 
 	if (hsotg->driver) {
@@ -4095,13 +3719,15 @@ int s3c_hsotg_suspend(struct dwc2_hsotg *hsotg)
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(s3c_hsotg_suspend);
 
 int s3c_hsotg_resume(struct dwc2_hsotg *hsotg)
 {
 	unsigned long flags;
 	int ret = 0;
 
+	if (hsotg->lx_state == DWC2_L2)
+		return ret;
+
 	mutex_lock(&hsotg->init_mutex);
 
 	if (hsotg->driver) {
@@ -4124,4 +3750,3 @@ int s3c_hsotg_resume(struct dwc2_hsotg *hsotg)
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(s3c_hsotg_resume);
diff --git a/drivers/usb/dwc2/hcd.c b/drivers/usb/dwc2/hcd.c
index fbbbac2150a5..b10377c65064 100644
--- a/drivers/usb/dwc2/hcd.c
+++ b/drivers/usb/dwc2/hcd.c
@@ -357,12 +357,12 @@ void dwc2_hcd_stop(struct dwc2_hsotg *hsotg)
 	writel(0, hsotg->regs + HPRT0);
 }
 
+/* Caller must hold driver lock */
 static int dwc2_hcd_urb_enqueue(struct dwc2_hsotg *hsotg,
 				struct dwc2_hcd_urb *urb, void **ep_handle,
 				gfp_t mem_flags)
 {
 	struct dwc2_qtd *qtd;
-	unsigned long flags;
 	u32 intr_mask;
 	int retval;
 	int dev_speed;
@@ -413,11 +413,9 @@ static int dwc2_hcd_urb_enqueue(struct dwc2_hsotg *hsotg,
 			 */
 			return 0;
 
-		spin_lock_irqsave(&hsotg->lock, flags);
 		tr_type = dwc2_hcd_select_transactions(hsotg);
 		if (tr_type != DWC2_TRANSACTION_NONE)
 			dwc2_hcd_queue_transactions(hsotg, tr_type);
-		spin_unlock_irqrestore(&hsotg->lock, flags);
 	}
 
 	return 0;
@@ -721,9 +719,7 @@ static int dwc2_hc_setup_align_buf(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
 			/* 3072 = 3 max-size Isoc packets */
 			buf_size = 3072;
 
-		qh->dw_align_buf = dma_alloc_coherent(hsotg->dev, buf_size,
-						      &qh->dw_align_buf_dma,
-						      GFP_ATOMIC);
+		qh->dw_align_buf = kmalloc(buf_size, GFP_ATOMIC | GFP_DMA);
 		if (!qh->dw_align_buf)
 			return -ENOMEM;
 		qh->dw_align_buf_size = buf_size;
@@ -748,6 +744,15 @@ static int dwc2_hc_setup_align_buf(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
 		}
 	}
 
+	qh->dw_align_buf_dma = dma_map_single(hsotg->dev,
+			qh->dw_align_buf, qh->dw_align_buf_size,
+			chan->ep_is_in ? DMA_FROM_DEVICE : DMA_TO_DEVICE);
+	if (dma_mapping_error(hsotg->dev, qh->dw_align_buf_dma)) {
+		dev_err(hsotg->dev, "can't map align_buf\n");
+		chan->align_buf = 0;
+		return -EINVAL;
+	}
+
 	chan->align_buf = qh->dw_align_buf_dma;
 	return 0;
 }
@@ -1774,6 +1779,15 @@ static int dwc2_hcd_hub_control(struct dwc2_hsotg *hsotg, u16 typereq,
 			/* Not supported */
 			break;
 
+		case USB_PORT_FEAT_TEST:
+			hprt0 = dwc2_read_hprt0(hsotg);
+			dev_dbg(hsotg->dev,
+				"SetPortFeature - USB_PORT_FEAT_TEST\n");
+			hprt0 &= ~HPRT0_TSTCTL_MASK;
+			hprt0 |= (windex >> 8) << HPRT0_TSTCTL_SHIFT;
+			writel(hprt0, hsotg->regs + HPRT0);
+			break;
+
 		default:
 			retval = -EINVAL;
 			dev_err(hsotg->dev,
@@ -2313,6 +2327,22 @@ 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);
+
+	hsotg->lx_state = DWC2_L2;
+	return 0;
+}
+
+static int _dwc2_hcd_resume(struct usb_hcd *hcd)
+{
+	struct dwc2_hsotg *hsotg = dwc2_hcd_to_hsotg(hcd);
+
+	hsotg->lx_state = DWC2_L0;
+	return 0;
+}
+
 /* Returns the current frame number */
 static int _dwc2_hcd_get_frame_number(struct usb_hcd *hcd)
 {
@@ -2468,7 +2498,7 @@ static int _dwc2_hcd_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 				"%s: unaligned transfer with no transfer_buffer",
 				__func__);
 			retval = -EINVAL;
-			goto fail1;
+			goto fail0;
 		}
 	}
 
@@ -2496,7 +2526,6 @@ static int _dwc2_hcd_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 
 	spin_lock_irqsave(&hsotg->lock, flags);
 	retval = usb_hcd_link_urb_to_ep(hcd, urb);
-	spin_unlock_irqrestore(&hsotg->lock, flags);
 	if (retval)
 		goto fail1;
 
@@ -2505,22 +2534,22 @@ static int _dwc2_hcd_urb_enqueue(struct usb_hcd *hcd, struct urb *urb,
 		goto fail2;
 
 	if (alloc_bandwidth) {
-		spin_lock_irqsave(&hsotg->lock, flags);
 		dwc2_allocate_bus_bandwidth(hcd,
 				dwc2_hcd_get_ep_bandwidth(hsotg, ep),
 				urb);
-		spin_unlock_irqrestore(&hsotg->lock, flags);
 	}
 
+	spin_unlock_irqrestore(&hsotg->lock, flags);
+
 	return 0;
 
 fail2:
-	spin_lock_irqsave(&hsotg->lock, flags);
 	dwc2_urb->priv = NULL;
 	usb_hcd_unlink_urb_from_ep(hcd, urb);
-	spin_unlock_irqrestore(&hsotg->lock, flags);
 fail1:
+	spin_unlock_irqrestore(&hsotg->lock, flags);
 	urb->hcpriv = NULL;
+fail0:
 	kfree(dwc2_urb);
 
 	return retval;
@@ -2683,6 +2712,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,
 };
 
 /*
@@ -2748,8 +2780,6 @@ static void dwc2_hcd_free(struct dwc2_hsotg *hsotg)
 		destroy_workqueue(hsotg->wq_otg);
 	}
 
-	kfree(hsotg->core_params);
-	hsotg->core_params = NULL;
 	del_timer(&hsotg->wkp_timer);
 }
 
@@ -2762,29 +2792,12 @@ static void dwc2_hcd_release(struct dwc2_hsotg *hsotg)
 }
 
 /*
- * Sets all parameters to the given value.
- *
- * Assumes that the dwc2_core_params struct contains only integers.
- */
-void dwc2_set_all_params(struct dwc2_core_params *params, int value)
-{
-	int *p = (int *)params;
-	size_t size = sizeof(*params) / sizeof(*p);
-	int i;
-
-	for (i = 0; i < size; i++)
-		p[i] = value;
-}
-EXPORT_SYMBOL_GPL(dwc2_set_all_params);
-
-/*
  * Initializes the HCD. This function allocates memory for and initializes the
  * static parts of the usb_hcd and dwc2_hsotg structures. It also registers the
  * USB bus with the core and calls the hc_driver->start() function. It returns
  * a negative error on failure.
  */
-int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
-		  const struct dwc2_core_params *params)
+int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq)
 {
 	struct usb_hcd *hcd;
 	struct dwc2_host_chan *channel;
@@ -2797,12 +2810,6 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
 
 	dev_dbg(hsotg->dev, "DWC OTG HCD INIT\n");
 
-	/* Detect config values from hardware */
-	retval = dwc2_get_hwparams(hsotg);
-
-	if (retval)
-		return retval;
-
 	retval = -ENOMEM;
 
 	hcfg = readl(hsotg->regs + HCFG);
@@ -2821,15 +2828,6 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
 	hsotg->last_frame_num = HFNUM_MAX_FRNUM;
 #endif
 
-	hsotg->core_params = kzalloc(sizeof(*hsotg->core_params), GFP_KERNEL);
-	if (!hsotg->core_params)
-		goto error1;
-
-	dwc2_set_all_params(hsotg->core_params, -1);
-
-	/* Validate parameter values */
-	dwc2_set_parameters(hsotg, params);
-
 	/* Check if the bus driver or platform code has setup a dma_mask */
 	if (hsotg->core_params->dma_enable > 0 &&
 	    hsotg->dev->dma_mask == NULL) {
@@ -2947,6 +2945,9 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
 	/* Don't support SG list at this point */
 	hcd->self.sg_tablesize = 0;
 
+	if (!IS_ERR_OR_NULL(hsotg->uphy))
+		otg_set_host(hsotg->uphy->otg, &hcd->self);
+
 	/*
 	 * Finish generic HCD initialization and start the HCD. This function
 	 * allocates the DMA buffer pool, registers the USB bus, requests the
@@ -2979,7 +2980,6 @@ error1:
 	dev_err(hsotg->dev, "%s() FAILED, returning %d\n", __func__, retval);
 	return retval;
 }
-EXPORT_SYMBOL_GPL(dwc2_hcd_init);
 
 /*
  * Removes the HCD.
@@ -3000,6 +3000,9 @@ void dwc2_hcd_remove(struct dwc2_hsotg *hsotg)
 		return;
 	}
 
+	if (!IS_ERR_OR_NULL(hsotg->uphy))
+		otg_set_host(hsotg->uphy->otg, NULL);
+
 	usb_remove_hcd(hcd);
 	hsotg->priv = NULL;
 	dwc2_hcd_release(hsotg);
@@ -3010,4 +3013,3 @@ void dwc2_hcd_remove(struct dwc2_hsotg *hsotg)
 	kfree(hsotg->frame_num_array);
 #endif
 }
-EXPORT_SYMBOL_GPL(dwc2_hcd_remove);
diff --git a/drivers/usb/dwc2/hcd.h b/drivers/usb/dwc2/hcd.h
index e69a843d8928..7b5841c40033 100644
--- a/drivers/usb/dwc2/hcd.h
+++ b/drivers/usb/dwc2/hcd.h
@@ -451,13 +451,8 @@ static inline u8 dwc2_hcd_is_pipe_out(struct dwc2_hcd_pipe_info *pipe)
 	return !dwc2_hcd_is_pipe_in(pipe);
 }
 
-extern int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq,
-			 const struct dwc2_core_params *params);
+extern int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq);
 extern void dwc2_hcd_remove(struct dwc2_hsotg *hsotg);
-extern void dwc2_set_parameters(struct dwc2_hsotg *hsotg,
-				const struct dwc2_core_params *params);
-extern void dwc2_set_all_params(struct dwc2_core_params *params, int value);
-extern int dwc2_get_hwparams(struct dwc2_hsotg *hsotg);
 
 /* Transaction Execution Functions */
 extern enum dwc2_transaction_type dwc2_hcd_select_transactions(
diff --git a/drivers/usb/dwc2/hcd_intr.c b/drivers/usb/dwc2/hcd_intr.c
index 551ba878b003..4cc95df4262d 100644
--- a/drivers/usb/dwc2/hcd_intr.c
+++ b/drivers/usb/dwc2/hcd_intr.c
@@ -350,6 +350,9 @@ static void dwc2_port_intr(struct dwc2_hsotg *hsotg)
 		dev_vdbg(hsotg->dev,
 			 "--Port Interrupt HPRT0=0x%08x Port Connect Detected--\n",
 			 hprt0);
+		if (hsotg->lx_state != DWC2_L0)
+			usb_hcd_resume_root_hub(hsotg->priv);
+
 		hsotg->flags.b.port_connect_status_change = 1;
 		hsotg->flags.b.port_connect_status = 1;
 		hprt0_modify |= HPRT0_CONNDET;
@@ -463,10 +466,15 @@ static int dwc2_update_urb_state(struct dwc2_hsotg *hsotg,
 	}
 
 	/* Non DWORD-aligned buffer case handling */
-	if (chan->align_buf && xfer_length && chan->ep_is_in) {
+	if (chan->align_buf && xfer_length) {
 		dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
-		memcpy(urb->buf + urb->actual_length, chan->qh->dw_align_buf,
-		       xfer_length);
+		dma_unmap_single(hsotg->dev, chan->qh->dw_align_buf_dma,
+				chan->qh->dw_align_buf_size,
+				chan->ep_is_in ?
+				DMA_FROM_DEVICE : DMA_TO_DEVICE);
+		if (chan->ep_is_in)
+			memcpy(urb->buf + urb->actual_length,
+					chan->qh->dw_align_buf, xfer_length);
 	}
 
 	dev_vdbg(hsotg->dev, "urb->actual_length=%d xfer_length=%d\n",
@@ -552,13 +560,18 @@ static enum dwc2_halt_status dwc2_update_isoc_urb_state(
 					chan, chnum, qtd, halt_status, NULL);
 
 		/* Non DWORD-aligned buffer case handling */
-		if (chan->align_buf && frame_desc->actual_length &&
-		    chan->ep_is_in) {
+		if (chan->align_buf && frame_desc->actual_length) {
 			dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n",
 				 __func__);
-			memcpy(urb->buf + frame_desc->offset +
-			       qtd->isoc_split_offset, chan->qh->dw_align_buf,
-			       frame_desc->actual_length);
+			dma_unmap_single(hsotg->dev, chan->qh->dw_align_buf_dma,
+					chan->qh->dw_align_buf_size,
+					chan->ep_is_in ?
+					DMA_FROM_DEVICE : DMA_TO_DEVICE);
+			if (chan->ep_is_in)
+				memcpy(urb->buf + frame_desc->offset +
+					qtd->isoc_split_offset,
+					chan->qh->dw_align_buf,
+					frame_desc->actual_length);
 		}
 		break;
 	case DWC2_HC_XFER_FRAME_OVERRUN:
@@ -581,13 +594,18 @@ static enum dwc2_halt_status dwc2_update_isoc_urb_state(
 					chan, chnum, qtd, halt_status, NULL);
 
 		/* Non DWORD-aligned buffer case handling */
-		if (chan->align_buf && frame_desc->actual_length &&
-		    chan->ep_is_in) {
+		if (chan->align_buf && frame_desc->actual_length) {
 			dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n",
 				 __func__);
-			memcpy(urb->buf + frame_desc->offset +
-			       qtd->isoc_split_offset, chan->qh->dw_align_buf,
-			       frame_desc->actual_length);
+			dma_unmap_single(hsotg->dev, chan->qh->dw_align_buf_dma,
+					chan->qh->dw_align_buf_size,
+					chan->ep_is_in ?
+					DMA_FROM_DEVICE : DMA_TO_DEVICE);
+			if (chan->ep_is_in)
+				memcpy(urb->buf + frame_desc->offset +
+					qtd->isoc_split_offset,
+					chan->qh->dw_align_buf,
+					frame_desc->actual_length);
 		}
 
 		/* Skip whole frame */
@@ -923,6 +941,8 @@ static int dwc2_xfercomp_isoc_split_in(struct dwc2_hsotg *hsotg,
 
 	if (chan->align_buf) {
 		dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
+		dma_unmap_single(hsotg->dev, chan->qh->dw_align_buf_dma,
+				chan->qh->dw_align_buf_size, DMA_FROM_DEVICE);
 		memcpy(qtd->urb->buf + frame_desc->offset +
 		       qtd->isoc_split_offset, chan->qh->dw_align_buf, len);
 	}
@@ -1152,8 +1172,14 @@ static void dwc2_update_urb_state_abn(struct dwc2_hsotg *hsotg,
 	/* Non DWORD-aligned buffer case handling */
 	if (chan->align_buf && xfer_length && chan->ep_is_in) {
 		dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
-		memcpy(urb->buf + urb->actual_length, chan->qh->dw_align_buf,
-		       xfer_length);
+		dma_unmap_single(hsotg->dev, chan->qh->dw_align_buf_dma,
+				chan->qh->dw_align_buf_size,
+				chan->ep_is_in ?
+				DMA_FROM_DEVICE : DMA_TO_DEVICE);
+		if (chan->ep_is_in)
+			memcpy(urb->buf + urb->actual_length,
+					chan->qh->dw_align_buf,
+					xfer_length);
 	}
 
 	urb->actual_length += xfer_length;
@@ -1182,6 +1208,16 @@ static void dwc2_hc_nak_intr(struct dwc2_hsotg *hsotg,
 			     struct dwc2_host_chan *chan, int chnum,
 			     struct dwc2_qtd *qtd)
 {
+	if (!qtd) {
+		dev_dbg(hsotg->dev, "%s: qtd is NULL\n", __func__);
+		return;
+	}
+
+	if (!qtd->urb) {
+		dev_dbg(hsotg->dev, "%s: qtd->urb is NULL\n", __func__);
+		return;
+	}
+
 	if (dbg_hc(chan))
 		dev_vdbg(hsotg->dev, "--Host Channel %d Interrupt: NAK Received--\n",
 			 chnum);
diff --git a/drivers/usb/dwc2/hcd_queue.c b/drivers/usb/dwc2/hcd_queue.c
index bb97838bc6c0..9b5c36256627 100644
--- a/drivers/usb/dwc2/hcd_queue.c
+++ b/drivers/usb/dwc2/hcd_queue.c
@@ -229,11 +229,13 @@ static struct dwc2_qh *dwc2_hcd_qh_create(struct dwc2_hsotg *hsotg,
  */
 void dwc2_hcd_qh_free(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
 {
-	if (hsotg->core_params->dma_desc_enable > 0)
+	if (hsotg->core_params->dma_desc_enable > 0) {
 		dwc2_hcd_qh_free_ddma(hsotg, qh);
-	else if (qh->dw_align_buf)
-		dma_free_coherent(hsotg->dev, qh->dw_align_buf_size,
-				  qh->dw_align_buf, qh->dw_align_buf_dma);
+	} else {
+		/* kfree(NULL) is safe */
+		kfree(qh->dw_align_buf);
+		qh->dw_align_buf_dma = (dma_addr_t)0;
+	}
 	kfree(qh);
 }
 
@@ -761,6 +763,7 @@ void dwc2_hcd_qtd_init(struct dwc2_qtd *qtd, struct dwc2_hcd_urb *urb)
 
 /**
  * dwc2_hcd_qtd_add() - Adds a QTD to the QTD-list of a QH
+ *			Caller must hold driver lock.
  *
  * @hsotg:        The DWC HCD structure
  * @qtd:          The QTD to add
@@ -777,7 +780,6 @@ int dwc2_hcd_qtd_add(struct dwc2_hsotg *hsotg, struct dwc2_qtd *qtd,
 		     struct dwc2_qh **qh, gfp_t mem_flags)
 {
 	struct dwc2_hcd_urb *urb = qtd->urb;
-	unsigned long flags;
 	int allocated = 0;
 	int retval;
 
@@ -792,15 +794,12 @@ int dwc2_hcd_qtd_add(struct dwc2_hsotg *hsotg, struct dwc2_qtd *qtd,
 		allocated = 1;
 	}
 
-	spin_lock_irqsave(&hsotg->lock, flags);
-
 	retval = dwc2_hcd_qh_add(hsotg, *qh);
 	if (retval)
 		goto fail;
 
 	qtd->qh = *qh;
 	list_add_tail(&qtd->qtd_list_entry, &(*qh)->qtd_list);
-	spin_unlock_irqrestore(&hsotg->lock, flags);
 
 	return 0;
 
@@ -817,10 +816,7 @@ fail:
 					 qtd_list_entry)
 			dwc2_hcd_qtd_unlink_and_free(hsotg, qtd2, qh_tmp);
 
-		spin_unlock_irqrestore(&hsotg->lock, flags);
 		dwc2_hcd_qh_free(hsotg, qh_tmp);
-	} else {
-		spin_unlock_irqrestore(&hsotg->lock, flags);
 	}
 
 	return retval;
diff --git a/drivers/usb/dwc2/platform.c b/drivers/usb/dwc2/platform.c
index 185663e0b5f4..90935304185a 100644
--- a/drivers/usb/dwc2/platform.c
+++ b/drivers/usb/dwc2/platform.c
@@ -47,6 +47,7 @@
 
 #include "core.h"
 #include "hcd.h"
+#include "debug.h"
 
 static const char dwc2_driver_name[] = "dwc2";
 
@@ -76,6 +77,8 @@ static const struct dwc2_core_params params_bcm2835 = {
 	.reload_ctl			= 0,
 	.ahbcfg				= 0x10,
 	.uframe_sched			= 0,
+	.external_id_pin_ctl		= -1,
+	.hibernation			= -1,
 };
 
 static const struct dwc2_core_params params_rk3066 = {
@@ -104,6 +107,8 @@ static const struct dwc2_core_params params_rk3066 = {
 	.reload_ctl			= -1,
 	.ahbcfg				= 0x7, /* INCR16 */
 	.uframe_sched			= -1,
+	.external_id_pin_ctl		= -1,
+	.hibernation			= -1,
 };
 
 /**
@@ -121,6 +126,7 @@ static int dwc2_driver_remove(struct platform_device *dev)
 {
 	struct dwc2_hsotg *hsotg = platform_get_drvdata(dev);
 
+	dwc2_debugfs_exit(hsotg);
 	if (hsotg->hcd_enabled)
 		dwc2_hcd_remove(hsotg);
 	if (hsotg->gadget_enabled)
@@ -237,6 +243,21 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	spin_lock_init(&hsotg->lock);
 	mutex_init(&hsotg->init_mutex);
 
+	/* Detect config values from hardware */
+	retval = dwc2_get_hwparams(hsotg);
+	if (retval)
+		return retval;
+
+	hsotg->core_params = devm_kzalloc(&dev->dev,
+				sizeof(*hsotg->core_params), GFP_KERNEL);
+	if (!hsotg->core_params)
+		return -ENOMEM;
+
+	dwc2_set_all_params(hsotg->core_params, -1);
+
+	/* Validate parameter values */
+	dwc2_set_parameters(hsotg, params);
+
 	if (hsotg->dr_mode != USB_DR_MODE_HOST) {
 		retval = dwc2_gadget_init(hsotg, irq);
 		if (retval)
@@ -245,7 +266,7 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	}
 
 	if (hsotg->dr_mode != USB_DR_MODE_PERIPHERAL) {
-		retval = dwc2_hcd_init(hsotg, irq, params);
+		retval = dwc2_hcd_init(hsotg, irq);
 		if (retval) {
 			if (hsotg->gadget_enabled)
 				s3c_hsotg_remove(hsotg);
@@ -256,6 +277,8 @@ static int dwc2_driver_probe(struct platform_device *dev)
 
 	platform_set_drvdata(dev, hsotg);
 
+	dwc2_debugfs_init(hsotg);
+
 	return retval;
 }
 
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index 827c4f80379f..dede32e809b6 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -11,6 +11,13 @@ config USB_DWC3
 
 if USB_DWC3
 
+config USB_DWC3_ULPI
+	bool "Register ULPI PHY Interface"
+	depends on USB_ULPI_BUS=y || USB_ULPI_BUS=USB_DWC3
+	help
+	  Select this if you have ULPI type PHY attached to your DWC3
+	  controller.
+
 choice
 	bool "DWC3 Mode Selection"
 	default USB_DWC3_DUAL_ROLE if (USB && USB_GADGET)
diff --git a/drivers/usb/dwc3/Makefile b/drivers/usb/dwc3/Makefile
index 46172f47f02d..c7076e37c4ed 100644
--- a/drivers/usb/dwc3/Makefile
+++ b/drivers/usb/dwc3/Makefile
@@ -15,6 +15,10 @@ ifneq ($(filter y,$(CONFIG_USB_DWC3_GADGET) $(CONFIG_USB_DWC3_DUAL_ROLE)),)
 	dwc3-y				+= gadget.o ep0.o
 endif
 
+ifneq ($(CONFIG_USB_DWC3_ULPI),)
+	dwc3-y				+= ulpi.o
+endif
+
 ifneq ($(CONFIG_DEBUG_FS),)
 	dwc3-y				+= debugfs.o
 endif
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index 2bbab3d86fff..5c110d8e293b 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -117,6 +117,33 @@ static int dwc3_core_soft_reset(struct dwc3 *dwc)
 }
 
 /**
+ * dwc3_soft_reset - Issue soft reset
+ * @dwc: Pointer to our controller context structure
+ */
+static int dwc3_soft_reset(struct dwc3 *dwc)
+{
+	unsigned long timeout;
+	u32 reg;
+
+	timeout = jiffies + msecs_to_jiffies(500);
+	dwc3_writel(dwc->regs, DWC3_DCTL, DWC3_DCTL_CSFTRST);
+	do {
+		reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+		if (!(reg & DWC3_DCTL_CSFTRST))
+			break;
+
+		if (time_after(jiffies, timeout)) {
+			dev_err(dwc->dev, "Reset Timed Out\n");
+			return -ETIMEDOUT;
+		}
+
+		cpu_relax();
+	} while (true);
+
+	return 0;
+}
+
+/**
  * dwc3_free_one_event_buffer - Frees one event buffer
  * @dwc: Pointer to our controller context structure
  * @evt: Pointer to event buffer to be freed
@@ -367,10 +394,15 @@ 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
+ *
+ * Returns 0 on success. The USB PHY interfaces are configured but not
+ * initialized. The PHY interfaces and the PHYs get initialized together with
+ * the core in dwc3_core_init.
  */
-static void dwc3_phy_setup(struct dwc3 *dwc)
+static int dwc3_phy_setup(struct dwc3 *dwc)
 {
 	u32 reg;
+	int ret;
 
 	reg = dwc3_readl(dwc->regs, DWC3_GUSB3PIPECTL(0));
 
@@ -409,10 +441,41 @@ static void dwc3_phy_setup(struct dwc3 *dwc)
 
 	dwc3_writel(dwc->regs, DWC3_GUSB3PIPECTL(0), reg);
 
-	mdelay(100);
-
 	reg = dwc3_readl(dwc->regs, DWC3_GUSB2PHYCFG(0));
 
+	/* Select the HS PHY interface */
+	switch (DWC3_GHWPARAMS3_HSPHY_IFC(dwc->hwparams.hwparams3)) {
+	case DWC3_GHWPARAMS3_HSPHY_IFC_UTMI_ULPI:
+		if (!strncmp(dwc->hsphy_interface, "utmi", 4)) {
+			reg &= ~DWC3_GUSB2PHYCFG_ULPI_UTMI;
+			break;
+		} else if (!strncmp(dwc->hsphy_interface, "ulpi", 4)) {
+			reg |= DWC3_GUSB2PHYCFG_ULPI_UTMI;
+			dwc3_writel(dwc->regs, DWC3_GUSB2PHYCFG(0), reg);
+		} else {
+			dev_warn(dwc->dev, "HSPHY Interface not defined\n");
+
+			/* Relying on default value. */
+			if (!(reg & DWC3_GUSB2PHYCFG_ULPI_UTMI))
+				break;
+		}
+		/* FALLTHROUGH */
+	case DWC3_GHWPARAMS3_HSPHY_IFC_ULPI:
+		/* Making sure the interface and PHY are operational */
+		ret = dwc3_soft_reset(dwc);
+		if (ret)
+			return ret;
+
+		udelay(1);
+
+		ret = dwc3_ulpi_init(dwc);
+		if (ret)
+			return ret;
+		/* FALLTHROUGH */
+	default:
+		break;
+	}
+
 	/*
 	 * Above 1.94a, it is recommended to set DWC3_GUSB2PHYCFG_SUSPHY to
 	 * '0' during coreConsultant configuration. So default value will
@@ -427,7 +490,7 @@ static void dwc3_phy_setup(struct dwc3 *dwc)
 
 	dwc3_writel(dwc->regs, DWC3_GUSB2PHYCFG(0), reg);
 
-	mdelay(100);
+	return 0;
 }
 
 /**
@@ -438,7 +501,6 @@ static void dwc3_phy_setup(struct dwc3 *dwc)
  */
 static int dwc3_core_init(struct dwc3 *dwc)
 {
-	unsigned long		timeout;
 	u32			hwparams4 = dwc->hwparams.hwparams4;
 	u32			reg;
 	int			ret;
@@ -466,21 +528,9 @@ static int dwc3_core_init(struct dwc3 *dwc)
 	}
 
 	/* issue device SoftReset too */
-	timeout = jiffies + msecs_to_jiffies(500);
-	dwc3_writel(dwc->regs, DWC3_DCTL, DWC3_DCTL_CSFTRST);
-	do {
-		reg = dwc3_readl(dwc->regs, DWC3_DCTL);
-		if (!(reg & DWC3_DCTL_CSFTRST))
-			break;
-
-		if (time_after(jiffies, timeout)) {
-			dev_err(dwc->dev, "Reset Timed Out\n");
-			ret = -ETIMEDOUT;
-			goto err0;
-		}
-
-		cpu_relax();
-	} while (true);
+	ret = dwc3_soft_reset(dwc);
+	if (ret)
+		goto err0;
 
 	ret = dwc3_core_soft_reset(dwc);
 	if (ret)
@@ -555,8 +605,6 @@ 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;
@@ -836,6 +884,8 @@ static int dwc3_probe(struct platform_device *pdev)
 				"snps,tx_de_emphasis_quirk");
 		of_property_read_u8(node, "snps,tx_de_emphasis",
 				&tx_de_emphasis);
+		of_property_read_string(node, "snps,hsphy_interface",
+					&dwc->hsphy_interface);
 	} else if (pdata) {
 		dwc->maximum_speed = pdata->maximum_speed;
 		dwc->has_lpm_erratum = pdata->has_lpm_erratum;
@@ -863,6 +913,8 @@ static int dwc3_probe(struct platform_device *pdev)
 		dwc->tx_de_emphasis_quirk = pdata->tx_de_emphasis_quirk;
 		if (pdata->tx_de_emphasis)
 			tx_de_emphasis = pdata->tx_de_emphasis;
+
+		dwc->hsphy_interface = pdata->hsphy_interface;
 	}
 
 	/* default to superspeed if no maximum_speed passed */
@@ -875,12 +927,18 @@ static int dwc3_probe(struct platform_device *pdev)
 	dwc->hird_threshold = hird_threshold
 		| (dwc->is_utmi_l1_suspend << 4);
 
+	platform_set_drvdata(pdev, dwc);
+	dwc3_cache_hwparams(dwc);
+
+	ret = dwc3_phy_setup(dwc);
+	if (ret)
+		goto err0;
+
 	ret = dwc3_core_get_phy(dwc);
 	if (ret)
 		goto err0;
 
 	spin_lock_init(&dwc->lock);
-	platform_set_drvdata(pdev, dwc);
 
 	if (!dev->dma_mask) {
 		dev->dma_mask = dev->parent->dma_mask;
@@ -892,8 +950,6 @@ static int dwc3_probe(struct platform_device *pdev)
 	pm_runtime_get_sync(dev);
 	pm_runtime_forbid(dev);
 
-	dwc3_cache_hwparams(dwc);
-
 	ret = dwc3_alloc_event_buffers(dwc, DWC3_EVENT_BUFFERS_SIZE);
 	if (ret) {
 		dev_err(dwc->dev, "failed to allocate event buffers\n");
@@ -964,6 +1020,7 @@ err2:
 
 err1:
 	dwc3_free_event_buffers(dwc);
+	dwc3_ulpi_exit(dwc);
 
 err0:
 	/*
@@ -999,6 +1056,7 @@ static int dwc3_remove(struct platform_device *pdev)
 	phy_power_off(dwc->usb3_generic_phy);
 
 	dwc3_core_exit(dwc);
+	dwc3_ulpi_exit(dwc);
 
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index fdab715a0631..044778884585 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -30,6 +30,7 @@
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/otg.h>
+#include <linux/ulpi/interface.h>
 
 #include <linux/phy/phy.h>
 
@@ -173,6 +174,15 @@
 /* Global USB2 PHY Configuration Register */
 #define DWC3_GUSB2PHYCFG_PHYSOFTRST	(1 << 31)
 #define DWC3_GUSB2PHYCFG_SUSPHY		(1 << 6)
+#define DWC3_GUSB2PHYCFG_ULPI_UTMI	(1 << 4)
+
+/* Global USB2 PHY Vendor Control Register */
+#define DWC3_GUSB2PHYACC_NEWREGREQ	(1 << 25)
+#define DWC3_GUSB2PHYACC_BUSY		(1 << 23)
+#define DWC3_GUSB2PHYACC_WRITE		(1 << 22)
+#define DWC3_GUSB2PHYACC_ADDR(n)	(n << 16)
+#define DWC3_GUSB2PHYACC_EXTEND_ADDR(n)	(n << 8)
+#define DWC3_GUSB2PHYACC_DATA(n)	(n & 0xff)
 
 /* Global USB3 PIPE Control Register */
 #define DWC3_GUSB3PIPECTL_PHYSOFTRST	(1 << 31)
@@ -339,7 +349,7 @@
 #define DWC3_DGCMD_SET_ENDPOINT_NRDY	0x0c
 #define DWC3_DGCMD_RUN_SOC_BUS_LOOPBACK	0x10
 
-#define DWC3_DGCMD_STATUS(n)		(((n) >> 15) & 1)
+#define DWC3_DGCMD_STATUS(n)		(((n) >> 12) & 0x0F)
 #define DWC3_DGCMD_CMDACT		(1 << 10)
 #define DWC3_DGCMD_CMDIOC		(1 << 8)
 
@@ -355,7 +365,7 @@
 #define DWC3_DEPCMD_PARAM_SHIFT		16
 #define DWC3_DEPCMD_PARAM(x)		((x) << DWC3_DEPCMD_PARAM_SHIFT)
 #define DWC3_DEPCMD_GET_RSC_IDX(x)	(((x) >> DWC3_DEPCMD_PARAM_SHIFT) & 0x7f)
-#define DWC3_DEPCMD_STATUS(x)		(((x) >> 15) & 1)
+#define DWC3_DEPCMD_STATUS(x)		(((x) >> 12) & 0x0F)
 #define DWC3_DEPCMD_HIPRI_FORCERM	(1 << 11)
 #define DWC3_DEPCMD_CMDACT		(1 << 10)
 #define DWC3_DEPCMD_CMDIOC		(1 << 8)
@@ -652,6 +662,7 @@ struct dwc3_scratchpad_array {
  * @usb3_phy: pointer to USB3 PHY
  * @usb2_generic_phy: pointer to USB2 PHY
  * @usb3_generic_phy: pointer to USB3 PHY
+ * @ulpi: pointer to ulpi interface
  * @dcfg: saved contents of DCFG register
  * @gctl: saved contents of GCTL register
  * @isoch_delay: wValue from Set Isochronous Delay request;
@@ -673,6 +684,7 @@ struct dwc3_scratchpad_array {
  * @test_mode_nr: test feature selector
  * @lpm_nyet_threshold: LPM NYET response threshold
  * @hird_threshold: HIRD threshold
+ * @hsphy_interface: "utmi" or "ulpi"
  * @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
@@ -739,6 +751,8 @@ struct dwc3 {
 	struct phy		*usb2_generic_phy;
 	struct phy		*usb3_generic_phy;
 
+	struct ulpi		*ulpi;
+
 	void __iomem		*regs;
 	size_t			regs_size;
 
@@ -800,6 +814,8 @@ struct dwc3 {
 	u8			lpm_nyet_threshold;
 	u8			hird_threshold;
 
+	const char		*hsphy_interface;
+
 	unsigned		delayed_status:1;
 	unsigned		ep0_bounced:1;
 	unsigned		ep0_expect_in:1;
@@ -1035,4 +1051,14 @@ static inline int dwc3_gadget_resume(struct dwc3 *dwc)
 }
 #endif /* !IS_ENABLED(CONFIG_USB_DWC3_HOST) */
 
+#if IS_ENABLED(CONFIG_USB_DWC3_ULPI)
+int dwc3_ulpi_init(struct dwc3 *dwc);
+void dwc3_ulpi_exit(struct dwc3 *dwc);
+#else
+static inline int dwc3_ulpi_init(struct dwc3 *dwc)
+{ return 0; }
+static inline void dwc3_ulpi_exit(struct dwc3 *dwc)
+{ }
+#endif
+
 #endif /* __DRIVERS_USB_DWC3_CORE_H */
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index b773fb53d6a7..27e4fc896e9d 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -21,6 +21,8 @@
 #include <linux/slab.h>
 #include <linux/pci.h>
 #include <linux/platform_device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/acpi.h>
 
 #include "platform_data.h"
 
@@ -31,6 +33,15 @@
 #define PCI_DEVICE_ID_INTEL_SPTLP	0x9d30
 #define PCI_DEVICE_ID_INTEL_SPTH	0xa130
 
+static const struct acpi_gpio_params reset_gpios = { 0, 0, false };
+static const struct acpi_gpio_params cs_gpios = { 1, 0, false };
+
+static const struct acpi_gpio_mapping acpi_dwc3_byt_gpios[] = {
+	{ "reset-gpios", &reset_gpios, 1 },
+	{ "cs-gpios", &cs_gpios, 1 },
+	{ },
+};
+
 static int dwc3_pci_quirks(struct pci_dev *pdev)
 {
 	if (pdev->vendor == PCI_VENDOR_ID_AMD &&
@@ -65,6 +76,30 @@ static int dwc3_pci_quirks(struct pci_dev *pdev)
 						sizeof(pdata));
 	}
 
+	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
+	    pdev->device == PCI_DEVICE_ID_INTEL_BYT) {
+		struct gpio_desc *gpio;
+
+		acpi_dev_add_driver_gpios(ACPI_COMPANION(&pdev->dev),
+					  acpi_dwc3_byt_gpios);
+
+		/* These GPIOs will turn on the USB2 PHY */
+		gpio = gpiod_get(&pdev->dev, "cs");
+		if (!IS_ERR(gpio)) {
+			gpiod_direction_output(gpio, 0);
+			gpiod_set_value_cansleep(gpio, 1);
+			gpiod_put(gpio);
+		}
+
+		gpio = gpiod_get(&pdev->dev, "reset");
+		if (!IS_ERR(gpio)) {
+			gpiod_direction_output(gpio, 0);
+			gpiod_set_value_cansleep(gpio, 1);
+			gpiod_put(gpio);
+			usleep_range(10000, 11000);
+		}
+	}
+
 	return 0;
 }
 
@@ -128,6 +163,7 @@ err:
 
 static void dwc3_pci_remove(struct pci_dev *pci)
 {
+	acpi_dev_remove_driver_gpios(ACPI_COMPANION(&pci->dev));
 	platform_device_unregister(pci_get_drvdata(pci));
 }
 
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 8946c32047e9..333a7c0078fc 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -291,6 +291,8 @@ int dwc3_send_gadget_generic_command(struct dwc3 *dwc, unsigned cmd, u32 param)
 			dwc3_trace(trace_dwc3_gadget,
 					"Command Complete --> %d",
 					DWC3_DGCMD_STATUS(reg));
+			if (DWC3_DGCMD_STATUS(reg))
+				return -EINVAL;
 			return 0;
 		}
 
@@ -328,6 +330,8 @@ int dwc3_send_gadget_ep_cmd(struct dwc3 *dwc, unsigned ep,
 			dwc3_trace(trace_dwc3_gadget,
 					"Command Complete --> %d",
 					DWC3_DEPCMD_STATUS(reg));
+			if (DWC3_DEPCMD_STATUS(reg))
+				return -EINVAL;
 			return 0;
 		}
 
@@ -1902,12 +1906,16 @@ static void dwc3_endpoint_transfer_complete(struct dwc3 *dwc,
 {
 	unsigned		status = 0;
 	int			clean_busy;
+	u32			is_xfer_complete;
+
+	is_xfer_complete = (event->endpoint_event == DWC3_DEPEVT_XFERCOMPLETE);
 
 	if (event->status & DEPEVT_STATUS_BUSERR)
 		status = -ECONNRESET;
 
 	clean_busy = dwc3_cleanup_done_reqs(dwc, dep, event, status);
-	if (clean_busy)
+	if (clean_busy && (is_xfer_complete ||
+				usb_endpoint_xfer_isoc(dep->endpoint.desc)))
 		dep->flags &= ~DWC3_EP_BUSY;
 
 	/*
diff --git a/drivers/usb/dwc3/platform_data.h b/drivers/usb/dwc3/platform_data.h
index a2bd464be828..d3614ecbb9ca 100644
--- a/drivers/usb/dwc3/platform_data.h
+++ b/drivers/usb/dwc3/platform_data.h
@@ -45,4 +45,6 @@ struct dwc3_platform_data {
 
 	unsigned tx_de_emphasis_quirk:1;
 	unsigned tx_de_emphasis:2;
+
+	const char *hsphy_interface;
 };
diff --git a/drivers/usb/dwc3/ulpi.c b/drivers/usb/dwc3/ulpi.c
new file mode 100644
index 000000000000..ec004c6d76f2
--- /dev/null
+++ b/drivers/usb/dwc3/ulpi.c
@@ -0,0 +1,91 @@
+/**
+ * ulpi.c - DesignWare USB3 Controller's ULPI PHY interface
+ *
+ * Copyright (C) 2015 Intel Corporation
+ *
+ * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/ulpi/regs.h>
+
+#include "core.h"
+#include "io.h"
+
+#define DWC3_ULPI_ADDR(a) \
+		((a >= ULPI_EXT_VENDOR_SPECIFIC) ? \
+		DWC3_GUSB2PHYACC_ADDR(ULPI_ACCESS_EXTENDED) | \
+		DWC3_GUSB2PHYACC_EXTEND_ADDR(a) : DWC3_GUSB2PHYACC_ADDR(a))
+
+static int dwc3_ulpi_busyloop(struct dwc3 *dwc)
+{
+	unsigned count = 1000;
+	u32 reg;
+
+	while (count--) {
+		reg = dwc3_readl(dwc->regs, DWC3_GUSB2PHYACC(0));
+		if (!(reg & DWC3_GUSB2PHYACC_BUSY))
+			return 0;
+		cpu_relax();
+	}
+
+	return -ETIMEDOUT;
+}
+
+static int dwc3_ulpi_read(struct ulpi_ops *ops, u8 addr)
+{
+	struct dwc3 *dwc = dev_get_drvdata(ops->dev);
+	u32 reg;
+	int ret;
+
+	reg = DWC3_GUSB2PHYACC_NEWREGREQ | DWC3_ULPI_ADDR(addr);
+	dwc3_writel(dwc->regs, DWC3_GUSB2PHYACC(0), reg);
+
+	ret = dwc3_ulpi_busyloop(dwc);
+	if (ret)
+		return ret;
+
+	reg = dwc3_readl(dwc->regs, DWC3_GUSB2PHYACC(0));
+
+	return DWC3_GUSB2PHYACC_DATA(reg);
+}
+
+static int dwc3_ulpi_write(struct ulpi_ops *ops, u8 addr, u8 val)
+{
+	struct dwc3 *dwc = dev_get_drvdata(ops->dev);
+	u32 reg;
+
+	reg = DWC3_GUSB2PHYACC_NEWREGREQ | DWC3_ULPI_ADDR(addr);
+	reg |= DWC3_GUSB2PHYACC_WRITE | val;
+	dwc3_writel(dwc->regs, DWC3_GUSB2PHYACC(0), reg);
+
+	return dwc3_ulpi_busyloop(dwc);
+}
+
+static struct ulpi_ops dwc3_ulpi_ops = {
+	.read = dwc3_ulpi_read,
+	.write = dwc3_ulpi_write,
+};
+
+int dwc3_ulpi_init(struct dwc3 *dwc)
+{
+	/* Register the interface */
+	dwc->ulpi = ulpi_register_interface(dwc->dev, &dwc3_ulpi_ops);
+	if (IS_ERR(dwc->ulpi)) {
+		dev_err(dwc->dev, "failed to register ULPI interface");
+		return PTR_ERR(dwc->ulpi);
+	}
+
+	return 0;
+}
+
+void dwc3_ulpi_exit(struct dwc3 *dwc)
+{
+	if (dwc->ulpi) {
+		ulpi_unregister_interface(dwc->ulpi);
+		dwc->ulpi = NULL;
+	}
+}
diff --git a/drivers/usb/gadget/epautoconf.c b/drivers/usb/gadget/epautoconf.c
index 0567cca1465e..919cdfdda78b 100644
--- a/drivers/usb/gadget/epautoconf.c
+++ b/drivers/usb/gadget/epautoconf.c
@@ -258,15 +258,25 @@ struct usb_ep *usb_ep_autoconfig_ss(
 	/* First, apply chip-specific "best usage" knowledge.
 	 * This might make a good usb_gadget_ops hook ...
 	 */
-	if (gadget_is_net2280 (gadget) && type == USB_ENDPOINT_XFER_INT) {
-		/* ep-e, ep-f are PIO with only 64 byte fifos */
-		ep = find_ep (gadget, "ep-e");
-		if (ep && ep_matches(gadget, ep, desc, ep_comp))
-			goto found_ep;
-		ep = find_ep (gadget, "ep-f");
+	if (gadget_is_net2280(gadget)) {
+		char name[8];
+
+		if (type == USB_ENDPOINT_XFER_INT) {
+			/* ep-e, ep-f are PIO with only 64 byte fifos */
+			ep = find_ep(gadget, "ep-e");
+			if (ep && ep_matches(gadget, ep, desc, ep_comp))
+				goto found_ep;
+			ep = find_ep(gadget, "ep-f");
+			if (ep && ep_matches(gadget, ep, desc, ep_comp))
+				goto found_ep;
+		}
+
+		/* USB3380: use same address for usb and hardware endpoints */
+		snprintf(name, sizeof(name), "ep%d%s", usb_endpoint_num(desc),
+				usb_endpoint_dir_in(desc) ? "in" : "out");
+		ep = find_ep(gadget, name);
 		if (ep && ep_matches(gadget, ep, desc, ep_comp))
 			goto found_ep;
-
 	} else if (gadget_is_goku (gadget)) {
 		if (USB_ENDPOINT_XFER_INT == type) {
 			/* single buffering is enough */
diff --git a/drivers/usb/gadget/function/f_fs.c b/drivers/usb/gadget/function/f_fs.c
index 6bdb57069044..45b8c8b338df 100644
--- a/drivers/usb/gadget/function/f_fs.c
+++ b/drivers/usb/gadget/function/f_fs.c
@@ -315,7 +315,6 @@ static ssize_t ffs_ep0_write(struct file *file, const char __user *buf,
 				return ret;
 			}
 
-			set_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags);
 			return len;
 		}
 		break;
@@ -847,7 +846,7 @@ static ssize_t ffs_epfile_io(struct file *file, struct ffs_io_data *io_data)
 				ret = ep->status;
 				if (io_data->read && ret > 0) {
 					ret = copy_to_iter(data, ret, &io_data->data);
-					if (unlikely(iov_iter_count(&io_data->data)))
+					if (!ret)
 						ret = -EFAULT;
 				}
 			}
@@ -1463,8 +1462,7 @@ static void ffs_data_clear(struct ffs_data *ffs)
 {
 	ENTER();
 
-	if (test_and_clear_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags))
-		ffs_closed(ffs);
+	ffs_closed(ffs);
 
 	BUG_ON(ffs->gadget);
 
@@ -3422,9 +3420,13 @@ static int ffs_ready(struct ffs_data *ffs)
 	ffs_obj->desc_ready = true;
 	ffs_obj->ffs_data = ffs;
 
-	if (ffs_obj->ffs_ready_callback)
+	if (ffs_obj->ffs_ready_callback) {
 		ret = ffs_obj->ffs_ready_callback(ffs);
+		if (ret)
+			goto done;
+	}
 
+	set_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags);
 done:
 	ffs_dev_unlock();
 	return ret;
@@ -3433,6 +3435,7 @@ done:
 static void ffs_closed(struct ffs_data *ffs)
 {
 	struct ffs_dev *ffs_obj;
+	struct f_fs_opts *opts;
 
 	ENTER();
 	ffs_dev_lock();
@@ -3443,11 +3446,17 @@ static void ffs_closed(struct ffs_data *ffs)
 
 	ffs_obj->desc_ready = false;
 
-	if (ffs_obj->ffs_closed_callback)
+	if (test_and_clear_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags) &&
+	    ffs_obj->ffs_closed_callback)
 		ffs_obj->ffs_closed_callback(ffs);
 
-	if (!ffs_obj->opts || ffs_obj->opts->no_configfs
-	    || !ffs_obj->opts->func_inst.group.cg_item.ci_parent)
+	if (ffs_obj->opts)
+		opts = ffs_obj->opts;
+	else
+		goto done;
+
+	if (opts->no_configfs || !opts->func_inst.group.cg_item.ci_parent
+	    || !atomic_read(&opts->func_inst.group.cg_item.ci_kref.refcount))
 		goto done;
 
 	unregister_gadget_item(ffs_obj->opts->
diff --git a/drivers/usb/gadget/function/f_mass_storage.c b/drivers/usb/gadget/function/f_mass_storage.c
index 3cc109f3c9c8..d2259c663996 100644
--- a/drivers/usb/gadget/function/f_mass_storage.c
+++ b/drivers/usb/gadget/function/f_mass_storage.c
@@ -2936,7 +2936,7 @@ int fsg_common_create_lun(struct fsg_common *common, struct fsg_lun_config *cfg,
 	if (fsg_lun_is_open(lun)) {
 		p = "(error)";
 		if (pathbuf) {
-			p = d_path(&lun->filp->f_path, pathbuf, PATH_MAX);
+			p = file_path(lun->filp, pathbuf, PATH_MAX);
 			if (IS_ERR(p))
 				p = "(error)";
 		}
diff --git a/drivers/usb/gadget/function/f_midi.c b/drivers/usb/gadget/function/f_midi.c
index 259b656c0b3e..6316aa5b1c49 100644
--- a/drivers/usb/gadget/function/f_midi.c
+++ b/drivers/usb/gadget/function/f_midi.c
@@ -973,7 +973,13 @@ 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);
+	if (opts->id) {
+		result = strlcpy(page, opts->id, PAGE_SIZE);
+	} else {
+		page[0] = 0;
+		result = 0;
+	}
+
 	mutex_unlock(&opts->lock);
 
 	return result;
diff --git a/drivers/usb/gadget/function/f_rndis.c b/drivers/usb/gadget/function/f_rndis.c
index 829edf878dac..32985dade838 100644
--- a/drivers/usb/gadget/function/f_rndis.c
+++ b/drivers/usb/gadget/function/f_rndis.c
@@ -76,7 +76,7 @@ struct f_rndis {
 	u8				ethaddr[ETH_ALEN];
 	u32				vendorID;
 	const char			*manufacturer;
-	int				config;
+	struct rndis_params		*params;
 
 	struct usb_ep			*notify;
 	struct usb_request		*notify_req;
@@ -453,7 +453,7 @@ static void rndis_command_complete(struct usb_ep *ep, struct usb_request *req)
 
 	/* received RNDIS command from USB_CDC_SEND_ENCAPSULATED_COMMAND */
 //	spin_lock(&dev->lock);
-	status = rndis_msg_parser(rndis->config, (u8 *) req->buf);
+	status = rndis_msg_parser(rndis->params, (u8 *) req->buf);
 	if (status < 0)
 		pr_err("RNDIS command error %d, %d/%d\n",
 			status, req->actual, req->length);
@@ -499,12 +499,12 @@ rndis_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 			u32 n;
 
 			/* return the result */
-			buf = rndis_get_next_response(rndis->config, &n);
+			buf = rndis_get_next_response(rndis->params, &n);
 			if (buf) {
 				memcpy(req->buf, buf, n);
 				req->complete = rndis_response_complete;
 				req->context = rndis;
-				rndis_free_response(rndis->config, buf);
+				rndis_free_response(rndis->params, buf);
 				value = n;
 			}
 			/* else stalls ... spec says to avoid that */
@@ -597,7 +597,7 @@ static int rndis_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 		if (IS_ERR(net))
 			return PTR_ERR(net);
 
-		rndis_set_param_dev(rndis->config, net,
+		rndis_set_param_dev(rndis->params, net,
 				&rndis->port.cdc_filter);
 	} else
 		goto fail;
@@ -617,7 +617,7 @@ static void rndis_disable(struct usb_function *f)
 
 	DBG(cdev, "rndis deactivated\n");
 
-	rndis_uninit(rndis->config);
+	rndis_uninit(rndis->params);
 	gether_disconnect(&rndis->port);
 
 	usb_ep_disable(rndis->notify);
@@ -640,9 +640,9 @@ static void rndis_open(struct gether *geth)
 
 	DBG(cdev, "%s\n", __func__);
 
-	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3,
+	rndis_set_param_medium(rndis->params, RNDIS_MEDIUM_802_3,
 				bitrate(cdev->gadget) / 100);
-	rndis_signal_connect(rndis->config);
+	rndis_signal_connect(rndis->params);
 }
 
 static void rndis_close(struct gether *geth)
@@ -651,8 +651,8 @@ static void rndis_close(struct gether *geth)
 
 	DBG(geth->func.config->cdev, "%s\n", __func__);
 
-	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3, 0);
-	rndis_signal_disconnect(rndis->config);
+	rndis_set_param_medium(rndis->params, RNDIS_MEDIUM_802_3, 0);
+	rndis_signal_disconnect(rndis->params);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -796,11 +796,11 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 	rndis->port.open = rndis_open;
 	rndis->port.close = rndis_close;
 
-	rndis_set_param_medium(rndis->config, RNDIS_MEDIUM_802_3, 0);
-	rndis_set_host_mac(rndis->config, rndis->ethaddr);
+	rndis_set_param_medium(rndis->params, RNDIS_MEDIUM_802_3, 0);
+	rndis_set_host_mac(rndis->params, rndis->ethaddr);
 
 	if (rndis->manufacturer && rndis->vendorID &&
-			rndis_set_param_vendor(rndis->config, rndis->vendorID,
+			rndis_set_param_vendor(rndis->params, rndis->vendorID,
 					       rndis->manufacturer)) {
 		status = -EINVAL;
 		goto fail_free_descs;
@@ -944,7 +944,7 @@ static void rndis_free(struct usb_function *f)
 	struct f_rndis_opts *opts;
 
 	rndis = func_to_rndis(f);
-	rndis_deregister(rndis->config);
+	rndis_deregister(rndis->params);
 	opts = container_of(f->fi, struct f_rndis_opts, func_inst);
 	kfree(rndis);
 	mutex_lock(&opts->lock);
@@ -968,7 +968,7 @@ static struct usb_function *rndis_alloc(struct usb_function_instance *fi)
 {
 	struct f_rndis	*rndis;
 	struct f_rndis_opts *opts;
-	int status;
+	struct rndis_params *params;
 
 	/* allocate and initialize one new instance */
 	rndis = kzalloc(sizeof(*rndis), GFP_KERNEL);
@@ -1002,36 +1002,16 @@ static struct usb_function *rndis_alloc(struct usb_function_instance *fi)
 	rndis->port.func.disable = rndis_disable;
 	rndis->port.func.free_func = rndis_free;
 
-	status = rndis_register(rndis_response_available, rndis);
-	if (status < 0) {
+	params = rndis_register(rndis_response_available, rndis);
+	if (IS_ERR(params)) {
 		kfree(rndis);
-		return ERR_PTR(status);
+		return ERR_CAST(params);
 	}
-	rndis->config = status;
+	rndis->params = params;
 
 	return &rndis->port.func;
 }
 
-DECLARE_USB_FUNCTION(rndis, rndis_alloc_inst, rndis_alloc);
-
-static int __init rndis_mod_init(void)
-{
-	int ret;
-
-	ret = rndis_init();
-	if (ret)
-		return ret;
-
-	return usb_function_register(&rndisusb_func);
-}
-module_init(rndis_mod_init);
-
-static void __exit rndis_mod_exit(void)
-{
-	usb_function_unregister(&rndisusb_func);
-	rndis_exit();
-}
-module_exit(rndis_mod_exit);
-
+DECLARE_USB_FUNCTION_INIT(rndis, rndis_alloc_inst, rndis_alloc);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("David Brownell");
diff --git a/drivers/usb/gadget/function/f_uac1.c b/drivers/usb/gadget/function/f_uac1.c
index 9719abfb6145..7856b3394494 100644
--- a/drivers/usb/gadget/function/f_uac1.c
+++ b/drivers/usb/gadget/function/f_uac1.c
@@ -588,7 +588,10 @@ static int f_audio_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 
 	if (intf == 1) {
 		if (alt == 1) {
-			config_ep_by_speed(cdev->gadget, f, out_ep);
+			err = config_ep_by_speed(cdev->gadget, f, out_ep);
+			if (err)
+				return err;
+
 			usb_ep_enable(out_ep);
 			out_ep->driver_data = audio;
 			audio->copy_buf = f_audio_buffer_alloc(audio_buf_size);
diff --git a/drivers/usb/gadget/function/rndis.c b/drivers/usb/gadget/function/rndis.c
index 95d2324f6977..70d3917cc003 100644
--- a/drivers/usb/gadget/function/rndis.c
+++ b/drivers/usb/gadget/function/rndis.c
@@ -25,6 +25,7 @@
 #include <linux/moduleparam.h>
 #include <linux/kernel.h>
 #include <linux/errno.h>
+#include <linux/idr.h>
 #include <linux/list.h>
 #include <linux/proc_fs.h>
 #include <linux/slab.h>
@@ -57,17 +58,26 @@ MODULE_PARM_DESC (rndis_debug, "enable debugging");
 #define rndis_debug		0
 #endif
 
-#define RNDIS_MAX_CONFIGS	1
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
 
+#define	NAME_TEMPLATE "driver/rndis-%03d"
 
-static rndis_params rndis_per_dev_params[RNDIS_MAX_CONFIGS];
+#endif /* CONFIG_USB_GADGET_DEBUG_FILES */
+
+static DEFINE_IDA(rndis_ida);
 
 /* Driver Version */
 static const __le32 rndis_driver_version = cpu_to_le32(1);
 
 /* Function Prototypes */
-static rndis_resp_t *rndis_add_response(int configNr, u32 length);
+static rndis_resp_t *rndis_add_response(struct rndis_params *params,
+					u32 length);
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+
+static const struct file_operations rndis_proc_fops;
 
+#endif /* CONFIG_USB_GADGET_DEBUG_FILES */
 
 /* supported OIDs */
 static const u32 oid_supported_list[] =
@@ -160,7 +170,7 @@ static const u32 oid_supported_list[] =
 
 
 /* NDIS Functions */
-static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
+static int gen_ndis_query_resp(struct rndis_params *params, u32 OID, u8 *buf,
 			       unsigned buf_len, rndis_resp_t *r)
 {
 	int retval = -ENOTSUPP;
@@ -192,7 +202,7 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	outbuf = (__le32 *)&resp[1];
 	resp->InformationBufferOffset = cpu_to_le32(16);
 
-	net = rndis_per_dev_params[configNr].dev;
+	net = params->dev;
 	stats = dev_get_stats(net, &temp);
 
 	switch (OID) {
@@ -225,7 +235,7 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* mandatory */
 	case RNDIS_OID_GEN_MEDIA_SUPPORTED:
 		pr_debug("%s: RNDIS_OID_GEN_MEDIA_SUPPORTED\n", __func__);
-		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr].medium);
+		*outbuf = cpu_to_le32(params->medium);
 		retval = 0;
 		break;
 
@@ -233,16 +243,15 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	case RNDIS_OID_GEN_MEDIA_IN_USE:
 		pr_debug("%s: RNDIS_OID_GEN_MEDIA_IN_USE\n", __func__);
 		/* one medium, one transport... (maybe you do it better) */
-		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr].medium);
+		*outbuf = cpu_to_le32(params->medium);
 		retval = 0;
 		break;
 
 	/* mandatory */
 	case RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE:
 		pr_debug("%s: RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
-			*outbuf = cpu_to_le32(
-				rndis_per_dev_params[configNr].dev->mtu);
+		if (params->dev) {
+			*outbuf = cpu_to_le32(params->dev->mtu);
 			retval = 0;
 		}
 		break;
@@ -251,21 +260,18 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	case RNDIS_OID_GEN_LINK_SPEED:
 		if (rndis_debug > 1)
 			pr_debug("%s: RNDIS_OID_GEN_LINK_SPEED\n", __func__);
-		if (rndis_per_dev_params[configNr].media_state
-				== RNDIS_MEDIA_STATE_DISCONNECTED)
+		if (params->media_state == RNDIS_MEDIA_STATE_DISCONNECTED)
 			*outbuf = cpu_to_le32(0);
 		else
-			*outbuf = cpu_to_le32(
-				rndis_per_dev_params[configNr].speed);
+			*outbuf = cpu_to_le32(params->speed);
 		retval = 0;
 		break;
 
 	/* mandatory */
 	case RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE:
 		pr_debug("%s: RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
-			*outbuf = cpu_to_le32(
-				rndis_per_dev_params[configNr].dev->mtu);
+		if (params->dev) {
+			*outbuf = cpu_to_le32(params->dev->mtu);
 			retval = 0;
 		}
 		break;
@@ -273,9 +279,8 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* mandatory */
 	case RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE:
 		pr_debug("%s: RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
-			*outbuf = cpu_to_le32(
-				rndis_per_dev_params[configNr].dev->mtu);
+		if (params->dev) {
+			*outbuf = cpu_to_le32(params->dev->mtu);
 			retval = 0;
 		}
 		break;
@@ -283,20 +288,16 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* mandatory */
 	case RNDIS_OID_GEN_VENDOR_ID:
 		pr_debug("%s: RNDIS_OID_GEN_VENDOR_ID\n", __func__);
-		*outbuf = cpu_to_le32(
-			rndis_per_dev_params[configNr].vendorID);
+		*outbuf = cpu_to_le32(params->vendorID);
 		retval = 0;
 		break;
 
 	/* mandatory */
 	case RNDIS_OID_GEN_VENDOR_DESCRIPTION:
 		pr_debug("%s: RNDIS_OID_GEN_VENDOR_DESCRIPTION\n", __func__);
-		if (rndis_per_dev_params[configNr].vendorDescr) {
-			length = strlen(rndis_per_dev_params[configNr].
-					vendorDescr);
-			memcpy(outbuf,
-				rndis_per_dev_params[configNr].vendorDescr,
-				length);
+		if (params->vendorDescr) {
+			length = strlen(params->vendorDescr);
+			memcpy(outbuf, params->vendorDescr, length);
 		} else {
 			outbuf[0] = 0;
 		}
@@ -313,7 +314,7 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* mandatory */
 	case RNDIS_OID_GEN_CURRENT_PACKET_FILTER:
 		pr_debug("%s: RNDIS_OID_GEN_CURRENT_PACKET_FILTER\n", __func__);
-		*outbuf = cpu_to_le32(*rndis_per_dev_params[configNr].filter);
+		*outbuf = cpu_to_le32(*params->filter);
 		retval = 0;
 		break;
 
@@ -328,8 +329,7 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	case RNDIS_OID_GEN_MEDIA_CONNECT_STATUS:
 		if (rndis_debug > 1)
 			pr_debug("%s: RNDIS_OID_GEN_MEDIA_CONNECT_STATUS\n", __func__);
-		*outbuf = cpu_to_le32(rndis_per_dev_params[configNr]
-						.media_state);
+		*outbuf = cpu_to_le32(params->media_state);
 		retval = 0;
 		break;
 
@@ -409,11 +409,9 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* mandatory */
 	case RNDIS_OID_802_3_PERMANENT_ADDRESS:
 		pr_debug("%s: RNDIS_OID_802_3_PERMANENT_ADDRESS\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
+		if (params->dev) {
 			length = ETH_ALEN;
-			memcpy(outbuf,
-				rndis_per_dev_params[configNr].host_mac,
-				length);
+			memcpy(outbuf, params->host_mac, length);
 			retval = 0;
 		}
 		break;
@@ -421,11 +419,9 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	/* mandatory */
 	case RNDIS_OID_802_3_CURRENT_ADDRESS:
 		pr_debug("%s: RNDIS_OID_802_3_CURRENT_ADDRESS\n", __func__);
-		if (rndis_per_dev_params[configNr].dev) {
+		if (params->dev) {
 			length = ETH_ALEN;
-			memcpy(outbuf,
-				rndis_per_dev_params [configNr].host_mac,
-				length);
+			memcpy(outbuf, params->host_mac, length);
 			retval = 0;
 		}
 		break;
@@ -490,12 +486,11 @@ static int gen_ndis_query_resp(int configNr, u32 OID, u8 *buf,
 	return retval;
 }
 
-static int gen_ndis_set_resp(u8 configNr, u32 OID, u8 *buf, u32 buf_len,
-			     rndis_resp_t *r)
+static int gen_ndis_set_resp(struct rndis_params *params, u32 OID,
+			     u8 *buf, u32 buf_len, rndis_resp_t *r)
 {
 	rndis_set_cmplt_type *resp;
 	int i, retval = -ENOTSUPP;
-	struct rndis_params *params;
 
 	if (!r)
 		return -ENOMEM;
@@ -514,7 +509,6 @@ static int gen_ndis_set_resp(u8 configNr, u32 OID, u8 *buf, u32 buf_len,
 		}
 	}
 
-	params = &rndis_per_dev_params[configNr];
 	switch (OID) {
 	case RNDIS_OID_GEN_CURRENT_PACKET_FILTER:
 
@@ -563,16 +557,16 @@ static int gen_ndis_set_resp(u8 configNr, u32 OID, u8 *buf, u32 buf_len,
  * Response Functions
  */
 
-static int rndis_init_response(int configNr, rndis_init_msg_type *buf)
+static int rndis_init_response(struct rndis_params *params,
+			       rndis_init_msg_type *buf)
 {
 	rndis_init_cmplt_type *resp;
 	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
 
 	if (!params->dev)
 		return -ENOTSUPP;
 
-	r = rndis_add_response(configNr, sizeof(rndis_init_cmplt_type));
+	r = rndis_add_response(params, sizeof(rndis_init_cmplt_type));
 	if (!r)
 		return -ENOMEM;
 	resp = (rndis_init_cmplt_type *)r->buf;
@@ -599,11 +593,11 @@ static int rndis_init_response(int configNr, rndis_init_msg_type *buf)
 	return 0;
 }
 
-static int rndis_query_response(int configNr, rndis_query_msg_type *buf)
+static int rndis_query_response(struct rndis_params *params,
+				rndis_query_msg_type *buf)
 {
 	rndis_query_cmplt_type *resp;
 	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
 
 	/* pr_debug("%s: OID = %08X\n", __func__, cpu_to_le32(buf->OID)); */
 	if (!params->dev)
@@ -615,7 +609,7 @@ static int rndis_query_response(int configNr, rndis_query_msg_type *buf)
 	 * rndis_query_cmplt_type followed by data.
 	 * oid_supported_list is the largest data reply
 	 */
-	r = rndis_add_response(configNr,
+	r = rndis_add_response(params,
 		sizeof(oid_supported_list) + sizeof(rndis_query_cmplt_type));
 	if (!r)
 		return -ENOMEM;
@@ -624,7 +618,7 @@ static int rndis_query_response(int configNr, rndis_query_msg_type *buf)
 	resp->MessageType = cpu_to_le32(RNDIS_MSG_QUERY_C);
 	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
 
-	if (gen_ndis_query_resp(configNr, le32_to_cpu(buf->OID),
+	if (gen_ndis_query_resp(params, le32_to_cpu(buf->OID),
 			le32_to_cpu(buf->InformationBufferOffset)
 					+ 8 + (u8 *)buf,
 			le32_to_cpu(buf->InformationBufferLength),
@@ -641,14 +635,14 @@ static int rndis_query_response(int configNr, rndis_query_msg_type *buf)
 	return 0;
 }
 
-static int rndis_set_response(int configNr, rndis_set_msg_type *buf)
+static int rndis_set_response(struct rndis_params *params,
+			      rndis_set_msg_type *buf)
 {
 	u32 BufLength, BufOffset;
 	rndis_set_cmplt_type *resp;
 	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
 
-	r = rndis_add_response(configNr, sizeof(rndis_set_cmplt_type));
+	r = rndis_add_response(params, sizeof(rndis_set_cmplt_type));
 	if (!r)
 		return -ENOMEM;
 	resp = (rndis_set_cmplt_type *)r->buf;
@@ -671,7 +665,7 @@ static int rndis_set_response(int configNr, rndis_set_msg_type *buf)
 	resp->MessageType = cpu_to_le32(RNDIS_MSG_SET_C);
 	resp->MessageLength = cpu_to_le32(16);
 	resp->RequestID = buf->RequestID; /* Still LE in msg buffer */
-	if (gen_ndis_set_resp(configNr, le32_to_cpu(buf->OID),
+	if (gen_ndis_set_resp(params, le32_to_cpu(buf->OID),
 			((u8 *)buf) + 8 + BufOffset, BufLength, r))
 		resp->Status = cpu_to_le32(RNDIS_STATUS_NOT_SUPPORTED);
 	else
@@ -681,13 +675,13 @@ static int rndis_set_response(int configNr, rndis_set_msg_type *buf)
 	return 0;
 }
 
-static int rndis_reset_response(int configNr, rndis_reset_msg_type *buf)
+static int rndis_reset_response(struct rndis_params *params,
+				rndis_reset_msg_type *buf)
 {
 	rndis_reset_cmplt_type *resp;
 	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
 
-	r = rndis_add_response(configNr, sizeof(rndis_reset_cmplt_type));
+	r = rndis_add_response(params, sizeof(rndis_reset_cmplt_type));
 	if (!r)
 		return -ENOMEM;
 	resp = (rndis_reset_cmplt_type *)r->buf;
@@ -702,16 +696,15 @@ static int rndis_reset_response(int configNr, rndis_reset_msg_type *buf)
 	return 0;
 }
 
-static int rndis_keepalive_response(int configNr,
+static int rndis_keepalive_response(struct rndis_params *params,
 				    rndis_keepalive_msg_type *buf)
 {
 	rndis_keepalive_cmplt_type *resp;
 	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
 
 	/* host "should" check only in RNDIS_DATA_INITIALIZED state */
 
-	r = rndis_add_response(configNr, sizeof(rndis_keepalive_cmplt_type));
+	r = rndis_add_response(params, sizeof(rndis_keepalive_cmplt_type));
 	if (!r)
 		return -ENOMEM;
 	resp = (rndis_keepalive_cmplt_type *)r->buf;
@@ -729,17 +722,15 @@ static int rndis_keepalive_response(int configNr,
 /*
  * Device to Host Comunication
  */
-static int rndis_indicate_status_msg(int configNr, u32 status)
+static int rndis_indicate_status_msg(struct rndis_params *params, u32 status)
 {
 	rndis_indicate_status_msg_type *resp;
 	rndis_resp_t *r;
-	struct rndis_params *params = rndis_per_dev_params + configNr;
 
 	if (params->state == RNDIS_UNINITIALIZED)
 		return -ENOTSUPP;
 
-	r = rndis_add_response(configNr,
-				sizeof(rndis_indicate_status_msg_type));
+	r = rndis_add_response(params, sizeof(rndis_indicate_status_msg_type));
 	if (!r)
 		return -ENOMEM;
 	resp = (rndis_indicate_status_msg_type *)r->buf;
@@ -754,53 +745,48 @@ static int rndis_indicate_status_msg(int configNr, u32 status)
 	return 0;
 }
 
-int rndis_signal_connect(int configNr)
+int rndis_signal_connect(struct rndis_params *params)
 {
-	rndis_per_dev_params[configNr].media_state
-			= RNDIS_MEDIA_STATE_CONNECTED;
-	return rndis_indicate_status_msg(configNr,
-					  RNDIS_STATUS_MEDIA_CONNECT);
+	params->media_state = RNDIS_MEDIA_STATE_CONNECTED;
+	return rndis_indicate_status_msg(params, RNDIS_STATUS_MEDIA_CONNECT);
 }
 EXPORT_SYMBOL_GPL(rndis_signal_connect);
 
-int rndis_signal_disconnect(int configNr)
+int rndis_signal_disconnect(struct rndis_params *params)
 {
-	rndis_per_dev_params[configNr].media_state
-			= RNDIS_MEDIA_STATE_DISCONNECTED;
-	return rndis_indicate_status_msg(configNr,
-					  RNDIS_STATUS_MEDIA_DISCONNECT);
+	params->media_state = RNDIS_MEDIA_STATE_DISCONNECTED;
+	return rndis_indicate_status_msg(params, RNDIS_STATUS_MEDIA_DISCONNECT);
 }
 EXPORT_SYMBOL_GPL(rndis_signal_disconnect);
 
-void rndis_uninit(int configNr)
+void rndis_uninit(struct rndis_params *params)
 {
 	u8 *buf;
 	u32 length;
 
-	if (configNr >= RNDIS_MAX_CONFIGS)
+	if (!params)
 		return;
-	rndis_per_dev_params[configNr].state = RNDIS_UNINITIALIZED;
+	params->state = RNDIS_UNINITIALIZED;
 
 	/* drain the response queue */
-	while ((buf = rndis_get_next_response(configNr, &length)))
-		rndis_free_response(configNr, buf);
+	while ((buf = rndis_get_next_response(params, &length)))
+		rndis_free_response(params, buf);
 }
 EXPORT_SYMBOL_GPL(rndis_uninit);
 
-void rndis_set_host_mac(int configNr, const u8 *addr)
+void rndis_set_host_mac(struct rndis_params *params, const u8 *addr)
 {
-	rndis_per_dev_params[configNr].host_mac = addr;
+	params->host_mac = addr;
 }
 EXPORT_SYMBOL_GPL(rndis_set_host_mac);
 
 /*
  * Message Parser
  */
-int rndis_msg_parser(u8 configNr, u8 *buf)
+int rndis_msg_parser(struct rndis_params *params, u8 *buf)
 {
 	u32 MsgType, MsgLength;
 	__le32 *tmp;
-	struct rndis_params *params;
 
 	if (!buf)
 		return -ENOMEM;
@@ -809,9 +795,8 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 	MsgType   = get_unaligned_le32(tmp++);
 	MsgLength = get_unaligned_le32(tmp++);
 
-	if (configNr >= RNDIS_MAX_CONFIGS)
+	if (!params)
 		return -ENOTSUPP;
-	params = &rndis_per_dev_params[configNr];
 
 	/* NOTE: RNDIS is *EXTREMELY* chatty ... Windows constantly polls for
 	 * rx/tx statistics and link status, in addition to KEEPALIVE traffic
@@ -824,8 +809,7 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 		pr_debug("%s: RNDIS_MSG_INIT\n",
 			__func__);
 		params->state = RNDIS_INITIALIZED;
-		return rndis_init_response(configNr,
-					(rndis_init_msg_type *)buf);
+		return rndis_init_response(params, (rndis_init_msg_type *)buf);
 
 	case RNDIS_MSG_HALT:
 		pr_debug("%s: RNDIS_MSG_HALT\n",
@@ -838,17 +822,16 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 		return 0;
 
 	case RNDIS_MSG_QUERY:
-		return rndis_query_response(configNr,
+		return rndis_query_response(params,
 					(rndis_query_msg_type *)buf);
 
 	case RNDIS_MSG_SET:
-		return rndis_set_response(configNr,
-					(rndis_set_msg_type *)buf);
+		return rndis_set_response(params, (rndis_set_msg_type *)buf);
 
 	case RNDIS_MSG_RESET:
 		pr_debug("%s: RNDIS_MSG_RESET\n",
 			__func__);
-		return rndis_reset_response(configNr,
+		return rndis_reset_response(params,
 					(rndis_reset_msg_type *)buf);
 
 	case RNDIS_MSG_KEEPALIVE:
@@ -856,7 +839,7 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 		if (rndis_debug > 1)
 			pr_debug("%s: RNDIS_MSG_KEEPALIVE\n",
 				__func__);
-		return rndis_keepalive_response(configNr,
+		return rndis_keepalive_response(params,
 						 (rndis_keepalive_msg_type *)
 						 buf);
 
@@ -876,71 +859,131 @@ int rndis_msg_parser(u8 configNr, u8 *buf)
 }
 EXPORT_SYMBOL_GPL(rndis_msg_parser);
 
-int rndis_register(void (*resp_avail)(void *v), void *v)
+static inline int rndis_get_nr(void)
 {
-	u8 i;
+	return ida_simple_get(&rndis_ida, 0, 0, GFP_KERNEL);
+}
+
+static inline void rndis_put_nr(int nr)
+{
+	ida_simple_remove(&rndis_ida, nr);
+}
+
+struct rndis_params *rndis_register(void (*resp_avail)(void *v), void *v)
+{
+	struct rndis_params *params;
+	int i;
 
 	if (!resp_avail)
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
+
+	i = rndis_get_nr();
+	if (i < 0) {
+		pr_debug("failed\n");
+
+		return ERR_PTR(-ENODEV);
+	}
+
+	params = kzalloc(sizeof(*params), GFP_KERNEL);
+	if (!params) {
+		rndis_put_nr(i);
 
-	for (i = 0; i < RNDIS_MAX_CONFIGS; i++) {
-		if (!rndis_per_dev_params[i].used) {
-			rndis_per_dev_params[i].used = 1;
-			rndis_per_dev_params[i].resp_avail = resp_avail;
-			rndis_per_dev_params[i].v = v;
-			pr_debug("%s: configNr = %d\n", __func__, i);
-			return i;
+		return ERR_PTR(-ENOMEM);
+	}
+
+#ifdef	CONFIG_USB_GADGET_DEBUG_FILES
+	{
+		struct proc_dir_entry *proc_entry;
+		char name[20];
+
+		sprintf(name, NAME_TEMPLATE, i);
+		proc_entry = proc_create_data(name, 0660, NULL,
+					      &rndis_proc_fops, params);
+		if (!proc_entry) {
+			kfree(params);
+			rndis_put_nr(i);
+
+			return ERR_PTR(-EIO);
 		}
 	}
-	pr_debug("failed\n");
+#endif
+
+	params->confignr = i;
+	params->used = 1;
+	params->state = RNDIS_UNINITIALIZED;
+	params->media_state = RNDIS_MEDIA_STATE_DISCONNECTED;
+	params->resp_avail = resp_avail;
+	params->v = v;
+	INIT_LIST_HEAD(&(params->resp_queue));
+	pr_debug("%s: configNr = %d\n", __func__, i);
 
-	return -ENODEV;
+	return params;
 }
 EXPORT_SYMBOL_GPL(rndis_register);
 
-void rndis_deregister(int configNr)
+void rndis_deregister(struct rndis_params *params)
 {
+	int i;
+
 	pr_debug("%s:\n", __func__);
 
-	if (configNr >= RNDIS_MAX_CONFIGS) return;
-	rndis_per_dev_params[configNr].used = 0;
+	if (!params)
+		return;
+
+	i = params->confignr;
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+	{
+		char name[20];
+
+		sprintf(name, NAME_TEMPLATE, i);
+		remove_proc_entry(name, NULL);
+	}
+#endif
+
+	kfree(params);
+	rndis_put_nr(i);
 }
 EXPORT_SYMBOL_GPL(rndis_deregister);
-
-int rndis_set_param_dev(u8 configNr, struct net_device *dev, u16 *cdc_filter)
+int rndis_set_param_dev(struct rndis_params *params, struct net_device *dev,
+			u16 *cdc_filter)
 {
 	pr_debug("%s:\n", __func__);
 	if (!dev)
 		return -EINVAL;
-	if (configNr >= RNDIS_MAX_CONFIGS) return -1;
+	if (!params)
+		return -1;
 
-	rndis_per_dev_params[configNr].dev = dev;
-	rndis_per_dev_params[configNr].filter = cdc_filter;
+	params->dev = dev;
+	params->filter = cdc_filter;
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(rndis_set_param_dev);
 
-int rndis_set_param_vendor(u8 configNr, u32 vendorID, const char *vendorDescr)
+int rndis_set_param_vendor(struct rndis_params *params, u32 vendorID,
+			   const char *vendorDescr)
 {
 	pr_debug("%s:\n", __func__);
 	if (!vendorDescr) return -1;
-	if (configNr >= RNDIS_MAX_CONFIGS) return -1;
+	if (!params)
+		return -1;
 
-	rndis_per_dev_params[configNr].vendorID = vendorID;
-	rndis_per_dev_params[configNr].vendorDescr = vendorDescr;
+	params->vendorID = vendorID;
+	params->vendorDescr = vendorDescr;
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(rndis_set_param_vendor);
 
-int rndis_set_param_medium(u8 configNr, u32 medium, u32 speed)
+int rndis_set_param_medium(struct rndis_params *params, u32 medium, u32 speed)
 {
 	pr_debug("%s: %u %u\n", __func__, medium, speed);
-	if (configNr >= RNDIS_MAX_CONFIGS) return -1;
+	if (!params)
+		return -1;
 
-	rndis_per_dev_params[configNr].medium = medium;
-	rndis_per_dev_params[configNr].speed = speed;
+	params->medium = medium;
+	params->speed = speed;
 
 	return 0;
 }
@@ -961,13 +1004,12 @@ void rndis_add_hdr(struct sk_buff *skb)
 }
 EXPORT_SYMBOL_GPL(rndis_add_hdr);
 
-void rndis_free_response(int configNr, u8 *buf)
+void rndis_free_response(struct rndis_params *params, u8 *buf)
 {
 	rndis_resp_t *r;
 	struct list_head *act, *tmp;
 
-	list_for_each_safe(act, tmp,
-			&(rndis_per_dev_params[configNr].resp_queue))
+	list_for_each_safe(act, tmp, &(params->resp_queue))
 	{
 		r = list_entry(act, rndis_resp_t, list);
 		if (r && r->buf == buf) {
@@ -978,15 +1020,14 @@ void rndis_free_response(int configNr, u8 *buf)
 }
 EXPORT_SYMBOL_GPL(rndis_free_response);
 
-u8 *rndis_get_next_response(int configNr, u32 *length)
+u8 *rndis_get_next_response(struct rndis_params *params, u32 *length)
 {
 	rndis_resp_t *r;
 	struct list_head *act, *tmp;
 
 	if (!length) return NULL;
 
-	list_for_each_safe(act, tmp,
-			&(rndis_per_dev_params[configNr].resp_queue))
+	list_for_each_safe(act, tmp, &(params->resp_queue))
 	{
 		r = list_entry(act, rndis_resp_t, list);
 		if (!r->send) {
@@ -1000,7 +1041,7 @@ u8 *rndis_get_next_response(int configNr, u32 *length)
 }
 EXPORT_SYMBOL_GPL(rndis_get_next_response);
 
-static rndis_resp_t *rndis_add_response(int configNr, u32 length)
+static rndis_resp_t *rndis_add_response(struct rndis_params *params, u32 length)
 {
 	rndis_resp_t *r;
 
@@ -1012,8 +1053,7 @@ static rndis_resp_t *rndis_add_response(int configNr, u32 length)
 	r->length = length;
 	r->send = 0;
 
-	list_add_tail(&r->list,
-		&(rndis_per_dev_params[configNr].resp_queue));
+	list_add_tail(&r->list, &(params->resp_queue));
 	return r;
 }
 
@@ -1103,11 +1143,11 @@ static ssize_t rndis_proc_write(struct file *file, const char __user *buffer,
 			break;
 		case 'C':
 		case 'c':
-			rndis_signal_connect(p->confignr);
+			rndis_signal_connect(p);
 			break;
 		case 'D':
 		case 'd':
-			rndis_signal_disconnect(p->confignr);
+			rndis_signal_disconnect(p);
 			break;
 		default:
 			if (fl_speed) p->speed = speed;
@@ -1137,54 +1177,4 @@ static const struct file_operations rndis_proc_fops = {
 
 #define	NAME_TEMPLATE "driver/rndis-%03d"
 
-static struct proc_dir_entry *rndis_connect_state [RNDIS_MAX_CONFIGS];
-
 #endif /* CONFIG_USB_GADGET_DEBUG_FILES */
-
-
-int rndis_init(void)
-{
-	u8 i;
-
-	for (i = 0; i < RNDIS_MAX_CONFIGS; i++) {
-#ifdef	CONFIG_USB_GADGET_DEBUG_FILES
-		char name [20];
-
-		sprintf(name, NAME_TEMPLATE, i);
-		rndis_connect_state[i] = proc_create_data(name, 0660, NULL,
-					&rndis_proc_fops,
-					(void *)(rndis_per_dev_params + i));
-		if (!rndis_connect_state[i]) {
-			pr_debug("%s: remove entries", __func__);
-			while (i) {
-				sprintf(name, NAME_TEMPLATE, --i);
-				remove_proc_entry(name, NULL);
-			}
-			pr_debug("\n");
-			return -EIO;
-		}
-#endif
-		rndis_per_dev_params[i].confignr = i;
-		rndis_per_dev_params[i].used = 0;
-		rndis_per_dev_params[i].state = RNDIS_UNINITIALIZED;
-		rndis_per_dev_params[i].media_state
-				= RNDIS_MEDIA_STATE_DISCONNECTED;
-		INIT_LIST_HEAD(&(rndis_per_dev_params[i].resp_queue));
-	}
-
-	return 0;
-}
-
-void rndis_exit(void)
-{
-#ifdef CONFIG_USB_GADGET_DEBUG_FILES
-	u8 i;
-	char name[20];
-
-	for (i = 0; i < RNDIS_MAX_CONFIGS; i++) {
-		sprintf(name, NAME_TEMPLATE, i);
-		remove_proc_entry(name, NULL);
-	}
-#endif
-}
-
diff --git a/drivers/usb/gadget/function/rndis.h b/drivers/usb/gadget/function/rndis.h
index 0f4abb4c3775..ef92eb66d8ad 100644
--- a/drivers/usb/gadget/function/rndis.h
+++ b/drivers/usb/gadget/function/rndis.h
@@ -177,7 +177,7 @@ typedef struct rndis_resp_t
 
 typedef struct rndis_params
 {
-	u8			confignr;
+	int			confignr;
 	u8			used;
 	u16			saved_filter;
 	enum rndis_state	state;
@@ -197,24 +197,25 @@ typedef struct rndis_params
 } rndis_params;
 
 /* RNDIS Message parser and other useless functions */
-int  rndis_msg_parser (u8 configNr, u8 *buf);
-int  rndis_register(void (*resp_avail)(void *v), void *v);
-void rndis_deregister (int configNr);
-int  rndis_set_param_dev (u8 configNr, struct net_device *dev,
+int  rndis_msg_parser(struct rndis_params *params, u8 *buf);
+struct rndis_params *rndis_register(void (*resp_avail)(void *v), void *v);
+void rndis_deregister(struct rndis_params *params);
+int  rndis_set_param_dev(struct rndis_params *params, struct net_device *dev,
 			 u16 *cdc_filter);
-int  rndis_set_param_vendor (u8 configNr, u32 vendorID,
+int  rndis_set_param_vendor(struct rndis_params *params, u32 vendorID,
 			    const char *vendorDescr);
-int  rndis_set_param_medium (u8 configNr, u32 medium, u32 speed);
-void rndis_add_hdr (struct sk_buff *skb);
+int  rndis_set_param_medium(struct rndis_params *params, u32 medium,
+			     u32 speed);
+void rndis_add_hdr(struct sk_buff *skb);
 int rndis_rm_hdr(struct gether *port, struct sk_buff *skb,
 			struct sk_buff_head *list);
-u8   *rndis_get_next_response (int configNr, u32 *length);
-void rndis_free_response (int configNr, u8 *buf);
-
-void rndis_uninit (int configNr);
-int  rndis_signal_connect (int configNr);
-int  rndis_signal_disconnect (int configNr);
-int  rndis_state (int configNr);
-extern void rndis_set_host_mac (int configNr, const u8 *addr);
+u8   *rndis_get_next_response(struct rndis_params *params, u32 *length);
+void rndis_free_response(struct rndis_params *params, u8 *buf);
+
+void rndis_uninit(struct rndis_params *params);
+int  rndis_signal_connect(struct rndis_params *params);
+int  rndis_signal_disconnect(struct rndis_params *params);
+int  rndis_state(struct rndis_params *params);
+extern void rndis_set_host_mac(struct rndis_params *params, const u8 *addr);
 
 #endif  /* _LINUX_RNDIS_H */
diff --git a/drivers/usb/gadget/function/storage_common.c b/drivers/usb/gadget/function/storage_common.c
index 648f9e489b39..d62683017cf3 100644
--- a/drivers/usb/gadget/function/storage_common.c
+++ b/drivers/usb/gadget/function/storage_common.c
@@ -341,7 +341,7 @@ ssize_t fsg_show_file(struct fsg_lun *curlun, struct rw_semaphore *filesem,
 
 	down_read(filesem);
 	if (fsg_lun_is_open(curlun)) {	/* Get the complete pathname */
-		p = d_path(&curlun->filp->f_path, buf, PAGE_SIZE - 1);
+		p = file_path(curlun->filp, buf, PAGE_SIZE - 1);
 		if (IS_ERR(p))
 			rc = PTR_ERR(p);
 		else {
diff --git a/drivers/usb/gadget/function/u_rndis.h b/drivers/usb/gadget/function/u_rndis.h
index e902aa42a297..4eafd5050545 100644
--- a/drivers/usb/gadget/function/u_rndis.h
+++ b/drivers/usb/gadget/function/u_rndis.h
@@ -39,8 +39,6 @@ struct f_rndis_opts {
 	int				refcnt;
 };
 
-int rndis_init(void);
-void rndis_exit(void);
 void rndis_borrow_net(struct usb_function_instance *f, struct net_device *net);
 
 #endif /* U_RNDIS_H */
diff --git a/drivers/usb/gadget/function/uvc.h b/drivers/usb/gadget/function/uvc.h
index ebe409b9e419..7d3bb6272e06 100644
--- a/drivers/usb/gadget/function/uvc.h
+++ b/drivers/usb/gadget/function/uvc.h
@@ -56,7 +56,6 @@ struct uvc_event
 #include <linux/usb/composite.h>
 #include <linux/usb/gadget.h>
 #include <linux/videodev2.h>
-#include <linux/version.h>
 #include <media/v4l2-fh.h>
 #include <media/v4l2-device.h>
 
diff --git a/drivers/usb/gadget/legacy/g_ffs.c b/drivers/usb/gadget/legacy/g_ffs.c
index 7b9ef7e257d2..e821931c965c 100644
--- a/drivers/usb/gadget/legacy/g_ffs.c
+++ b/drivers/usb/gadget/legacy/g_ffs.c
@@ -304,8 +304,10 @@ static int functionfs_ready_callback(struct ffs_data *ffs)
 	gfs_registered = true;
 
 	ret = usb_composite_probe(&gfs_driver);
-	if (unlikely(ret < 0))
+	if (unlikely(ret < 0)) {
+		++missing_funcs;
 		gfs_registered = false;
+	}
 	
 	return ret;
 }
diff --git a/drivers/usb/gadget/legacy/inode.c b/drivers/usb/gadget/legacy/inode.c
index 2030565c6789..f454c7af489c 100644
--- a/drivers/usb/gadget/legacy/inode.c
+++ b/drivers/usb/gadget/legacy/inode.c
@@ -769,9 +769,12 @@ ep_config (struct ep_data *data, const char *buf, size_t len)
 	if (data->dev->state == STATE_DEV_UNBOUND) {
 		value = -ENOENT;
 		goto gone;
-	} else if ((ep = data->ep) == NULL) {
-		value = -ENODEV;
-		goto gone;
+	} else {
+		ep = data->ep;
+		if (ep == NULL) {
+			value = -ENODEV;
+			goto gone;
+		}
 	}
 	switch (data->dev->gadget->speed) {
 	case USB_SPEED_LOW:
diff --git a/drivers/usb/gadget/legacy/tcm_usb_gadget.c b/drivers/usb/gadget/legacy/tcm_usb_gadget.c
index f9b4882fce52..c3c48088fced 100644
--- a/drivers/usb/gadget/legacy/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/legacy/tcm_usb_gadget.c
@@ -16,12 +16,10 @@
 #include <linux/usb/composite.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/storage.h>
-#include <scsi/scsi.h>
 #include <scsi/scsi_tcq.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 #include <asm/unaligned.h>
 
@@ -29,8 +27,6 @@
 
 USB_GADGET_COMPOSITE_OPTIONS();
 
-static const struct target_core_fabric_ops usbg_ops;
-
 static inline struct f_uas *to_f_uas(struct usb_function *f)
 {
 	return container_of(f, struct f_uas, function);
@@ -1112,6 +1108,7 @@ static int usbg_submit_command(struct f_uas *fu,
 	memcpy(cmd->cmd_buf, cmd_iu->cdb, cmd_len);
 
 	cmd->tag = be16_to_cpup(&cmd_iu->tag);
+	cmd->se_cmd.tag = cmd->tag;
 	if (fu->flags & USBG_USE_STREAMS) {
 		if (cmd->tag > UASP_SS_EP_COMP_NUM_STREAMS)
 			goto err;
@@ -1245,6 +1242,7 @@ static int bot_submit_command(struct f_uas *fu,
 	cmd->unpacked_lun = cbw->Lun;
 	cmd->is_read = cbw->Flags & US_BULK_FLAG_IN ? 1 : 0;
 	cmd->data_len = le32_to_cpu(cbw->DataTransferLength);
+	cmd->se_cmd.tag = le32_to_cpu(cmd->bot_tag);
 
 	INIT_WORK(&cmd->work, bot_cmd_work);
 	ret = queue_work(tpg->workqueue, &cmd->work);
@@ -1274,23 +1272,6 @@ static char *usbg_get_fabric_name(void)
 	return "usb_gadget";
 }
 
-static u8 usbg_get_fabric_proto_ident(struct se_portal_group *se_tpg)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg,
-				struct usbg_tpg, se_tpg);
-	struct usbg_tport *tport = tpg->tport;
-	u8 proto_id;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-	default:
-		proto_id = sas_get_fabric_proto_ident(se_tpg);
-		break;
-	}
-
-	return proto_id;
-}
-
 static char *usbg_get_fabric_wwn(struct se_portal_group *se_tpg)
 {
 	struct usbg_tpg *tpg = container_of(se_tpg,
@@ -1307,97 +1288,6 @@ static u16 usbg_get_tag(struct se_portal_group *se_tpg)
 	return tpg->tport_tpgt;
 }
 
-static u32 usbg_get_default_depth(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static u32 usbg_get_pr_transport_id(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code,
-	unsigned char *buf)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg,
-				struct usbg_tpg, se_tpg);
-	struct usbg_tport *tport = tpg->tport;
-	int ret = 0;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-	default:
-		ret = sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-		break;
-	}
-
-	return ret;
-}
-
-static u32 usbg_get_pr_transport_id_len(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl,
-	struct t10_pr_registration *pr_reg,
-	int *format_code)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg,
-				struct usbg_tpg, se_tpg);
-	struct usbg_tport *tport = tpg->tport;
-	int ret = 0;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-	default:
-		ret = sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-		break;
-	}
-
-	return ret;
-}
-
-static char *usbg_parse_pr_out_transport_id(
-	struct se_portal_group *se_tpg,
-	const char *buf,
-	u32 *out_tid_len,
-	char **port_nexus_ptr)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg,
-				struct usbg_tpg, se_tpg);
-	struct usbg_tport *tport = tpg->tport;
-	char *tid = NULL;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-	default:
-		tid = sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	}
-
-	return tid;
-}
-
-static struct se_node_acl *usbg_alloc_fabric_acl(struct se_portal_group *se_tpg)
-{
-	struct usbg_nacl *nacl;
-
-	nacl = kzalloc(sizeof(struct usbg_nacl), GFP_KERNEL);
-	if (!nacl)
-		return NULL;
-
-	return &nacl->se_node_acl;
-}
-
-static void usbg_release_fabric_acl(
-	struct se_portal_group *se_tpg,
-	struct se_node_acl *se_nacl)
-{
-	struct usbg_nacl *nacl = container_of(se_nacl,
-			struct usbg_nacl, se_node_acl);
-	kfree(nacl);
-}
-
 static u32 usbg_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
 	return 1;
@@ -1448,18 +1338,6 @@ static void usbg_set_default_node_attrs(struct se_node_acl *nacl)
 	return;
 }
 
-static u32 usbg_get_task_tag(struct se_cmd *se_cmd)
-{
-	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
-			se_cmd);
-	struct f_uas *fu = cmd->fu;
-
-	if (fu->flags & USBG_IS_BOT)
-		return le32_to_cpu(cmd->bot_tag);
-	else
-		return cmd->tag;
-}
-
 static int usbg_get_cmd_state(struct se_cmd *se_cmd)
 {
 	return 0;
@@ -1489,50 +1367,11 @@ static const char *usbg_check_wwn(const char *name)
 	return n;
 }
 
-static struct se_node_acl *usbg_make_nodeacl(
-	struct se_portal_group *se_tpg,
-	struct config_group *group,
-	const char *name)
-{
-	struct se_node_acl *se_nacl, *se_nacl_new;
-	struct usbg_nacl *nacl;
-	u64 wwpn = 0;
-	u32 nexus_depth;
-	const char *wnn_name;
-
-	wnn_name = usbg_check_wwn(name);
-	if (!wnn_name)
-		return ERR_PTR(-EINVAL);
-	se_nacl_new = usbg_alloc_fabric_acl(se_tpg);
-	if (!(se_nacl_new))
-		return ERR_PTR(-ENOMEM);
-
-	nexus_depth = 1;
-	/*
-	 * se_nacl_new may be released by core_tpg_add_initiator_node_acl()
-	 * when converting a NodeACL from demo mode -> explict
-	 */
-	se_nacl = core_tpg_add_initiator_node_acl(se_tpg, se_nacl_new,
-				name, nexus_depth);
-	if (IS_ERR(se_nacl)) {
-		usbg_release_fabric_acl(se_tpg, se_nacl_new);
-		return se_nacl;
-	}
-	/*
-	 * Locate our struct usbg_nacl and set the FC Nport WWPN
-	 */
-	nacl = container_of(se_nacl, struct usbg_nacl, se_node_acl);
-	nacl->iport_wwpn = wwpn;
-	snprintf(nacl->iport_name, sizeof(nacl->iport_name), "%s", name);
-	return se_nacl;
-}
-
-static void usbg_drop_nodeacl(struct se_node_acl *se_acl)
+static int usbg_init_nodeacl(struct se_node_acl *se_nacl, const char *name)
 {
-	struct usbg_nacl *nacl = container_of(se_acl,
-				struct usbg_nacl, se_node_acl);
-	core_tpg_del_initiator_node_acl(se_acl->se_tpg, se_acl, 1);
-	kfree(nacl);
+	if (!usbg_check_wwn(name))
+		return -EINVAL;
+	return 0;
 }
 
 struct usbg_tpg *the_only_tpg_I_currently_have;
@@ -1572,8 +1411,11 @@ static struct se_portal_group *usbg_make_tpg(
 	tpg->tport = tport;
 	tpg->tport_tpgt = tpgt;
 
-	ret = core_tpg_register(&usbg_ops, wwn, &tpg->se_tpg, tpg,
-				TRANSPORT_TPG_TYPE_NORMAL);
+	/*
+	 * SPC doesn't assign a protocol identifier for USB-SCSI, so we
+	 * pretend to be SAS..
+	 */
+	ret = core_tpg_register(wwn, &tpg->se_tpg, SCSI_PROTOCOL_SAS);
 	if (ret < 0) {
 		destroy_workqueue(tpg->workqueue);
 		kfree(tpg);
@@ -1867,19 +1709,12 @@ static const struct target_core_fabric_ops usbg_ops = {
 	.module				= THIS_MODULE,
 	.name				= "usb_gadget",
 	.get_fabric_name		= usbg_get_fabric_name,
-	.get_fabric_proto_ident		= usbg_get_fabric_proto_ident,
 	.tpg_get_wwn			= usbg_get_fabric_wwn,
 	.tpg_get_tag			= usbg_get_tag,
-	.tpg_get_default_depth		= usbg_get_default_depth,
-	.tpg_get_pr_transport_id	= usbg_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= usbg_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= usbg_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= usbg_check_true,
 	.tpg_check_demo_mode_cache	= usbg_check_false,
 	.tpg_check_demo_mode_write_protect = usbg_check_false,
 	.tpg_check_prod_mode_write_protect = usbg_check_false,
-	.tpg_alloc_fabric_acl		= usbg_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= usbg_release_fabric_acl,
 	.tpg_get_inst_index		= usbg_tpg_get_inst_index,
 	.release_cmd			= usbg_release_cmd,
 	.shutdown_session		= usbg_shutdown_session,
@@ -1889,7 +1724,6 @@ static const struct target_core_fabric_ops usbg_ops = {
 	.write_pending			= usbg_send_write_request,
 	.write_pending_status		= usbg_write_pending_status,
 	.set_default_node_attributes	= usbg_set_default_node_attrs,
-	.get_task_tag			= usbg_get_task_tag,
 	.get_cmd_state			= usbg_get_cmd_state,
 	.queue_data_in			= usbg_send_read_response,
 	.queue_status			= usbg_send_status_response,
@@ -1903,10 +1737,7 @@ static const struct target_core_fabric_ops usbg_ops = {
 	.fabric_drop_tpg		= usbg_drop_tpg,
 	.fabric_post_link		= usbg_port_link,
 	.fabric_pre_unlink		= usbg_port_unlink,
-	.fabric_make_np			= NULL,
-	.fabric_drop_np			= NULL,
-	.fabric_make_nodeacl		= usbg_make_nodeacl,
-	.fabric_drop_nodeacl		= usbg_drop_nodeacl,
+	.fabric_init_nodeacl		= usbg_init_nodeacl,
 
 	.tfc_wwn_attrs			= usbg_wwn_attrs,
 	.tfc_tpg_base_attrs		= usbg_base_attrs,
diff --git a/drivers/usb/gadget/legacy/tcm_usb_gadget.h b/drivers/usb/gadget/legacy/tcm_usb_gadget.h
index 8289219925b8..0b749e1aa2f1 100644
--- a/drivers/usb/gadget/legacy/tcm_usb_gadget.h
+++ b/drivers/usb/gadget/legacy/tcm_usb_gadget.h
@@ -6,7 +6,6 @@
 #include <linux/usb/composite.h>
 #include <linux/usb/uas.h>
 #include <linux/usb/storage.h>
-#include <scsi/scsi.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 
@@ -25,15 +24,6 @@ enum {
 #define USB_G_ALT_INT_BBB       0
 #define USB_G_ALT_INT_UAS       1
 
-struct usbg_nacl {
-	/* Binary World Wide unique Port Name for SAS Initiator port */
-	u64 iport_wwpn;
-	/* ASCII formatted WWPN for Sas Initiator port */
-	char iport_name[USBG_NAMELEN];
-	/* Returned by usbg_make_nodeacl() */
-	struct se_node_acl se_node_acl;
-};
-
 struct tcm_usbg_nexus {
 	struct se_session *tvn_se_sess;
 };
@@ -53,8 +43,6 @@ struct usbg_tpg {
 };
 
 struct usbg_tport {
-	/* SCSI protocol the tport is providing */
-	u8 tport_proto_id;
 	/* Binary World Wide unique Port Name for SAS Target port */
 	u64 tport_wwpn;
 	/* ASCII formatted WWPN for SAS Target port */
diff --git a/drivers/usb/gadget/udc/atmel_usba_udc.c b/drivers/usb/gadget/udc/atmel_usba_udc.c
index 351d48550c33..4095cce05e6a 100644
--- a/drivers/usb/gadget/udc/atmel_usba_udc.c
+++ b/drivers/usb/gadget/udc/atmel_usba_udc.c
@@ -704,8 +704,8 @@ static int queue_dma(struct usba_udc *udc, struct usba_ep *ep,
 	unsigned long flags;
 	int ret;
 
-	DBG(DBG_DMA, "%s: req l/%u d/%08x %c%c%c\n",
-		ep->ep.name, req->req.length, req->req.dma,
+	DBG(DBG_DMA, "%s: req l/%u d/%pad %c%c%c\n",
+		ep->ep.name, req->req.length, &req->req.dma,
 		req->req.zero ? 'Z' : 'z',
 		req->req.short_not_ok ? 'S' : 's',
 		req->req.no_interrupt ? 'I' : 'i');
@@ -2203,7 +2203,7 @@ static int usba_udc_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int usba_udc_suspend(struct device *dev)
 {
 	struct usba_udc *udc = dev_get_drvdata(dev);
diff --git a/drivers/usb/gadget/udc/net2280.c b/drivers/usb/gadget/udc/net2280.c
index 9871b90195ad..2bee912ca65b 100644
--- a/drivers/usb/gadget/udc/net2280.c
+++ b/drivers/usb/gadget/udc/net2280.c
@@ -123,6 +123,11 @@ static char *type_string(u8 bmAttributes)
 #define valid_bit	cpu_to_le32(BIT(VALID_BIT))
 #define dma_done_ie	cpu_to_le32(BIT(DMA_DONE_INTERRUPT_ENABLE))
 
+static void ep_clear_seqnum(struct net2280_ep *ep);
+static void stop_activity(struct net2280 *dev,
+					struct usb_gadget_driver *driver);
+static void ep0_start(struct net2280 *dev);
+
 /*-------------------------------------------------------------------------*/
 static inline void enable_pciirqenb(struct net2280_ep *ep)
 {
@@ -142,7 +147,9 @@ net2280_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 {
 	struct net2280		*dev;
 	struct net2280_ep	*ep;
-	u32			max, tmp;
+	u32			max;
+	u32 tmp = 0;
+	u32 type;
 	unsigned long		flags;
 	static const u32 ep_key[9] = { 1, 0, 1, 0, 1, 1, 0, 1, 0 };
 	int ret = 0;
@@ -198,15 +205,29 @@ net2280_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 
 	/* set type, direction, address; reset fifo counters */
 	writel(BIT(FIFO_FLUSH), &ep->regs->ep_stat);
-	tmp = (desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK);
-	if (tmp == USB_ENDPOINT_XFER_INT) {
+
+	if ((dev->quirks & PLX_SUPERSPEED) && dev->enhanced_mode) {
+		tmp = readl(&ep->cfg->ep_cfg);
+		/* If USB ep number doesn't match hardware ep number */
+		if ((tmp & 0xf) != usb_endpoint_num(desc)) {
+			ret = -EINVAL;
+			spin_unlock_irqrestore(&dev->lock, flags);
+			goto print_err;
+		}
+		if (ep->is_in)
+			tmp &= ~USB3380_EP_CFG_MASK_IN;
+		else
+			tmp &= ~USB3380_EP_CFG_MASK_OUT;
+	}
+	type = (desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK);
+	if (type == USB_ENDPOINT_XFER_INT) {
 		/* erratum 0105 workaround prevents hs NYET */
 		if (dev->chiprev == 0100 &&
 				dev->gadget.speed == USB_SPEED_HIGH &&
 				!(desc->bEndpointAddress & USB_DIR_IN))
 			writel(BIT(CLEAR_NAK_OUT_PACKETS_MODE),
 				&ep->regs->ep_rsp);
-	} else if (tmp == USB_ENDPOINT_XFER_BULK) {
+	} else if (type == USB_ENDPOINT_XFER_BULK) {
 		/* catch some particularly blatant driver bugs */
 		if ((dev->gadget.speed == USB_SPEED_SUPER && max != 1024) ||
 		    (dev->gadget.speed == USB_SPEED_HIGH && max != 512) ||
@@ -216,10 +237,10 @@ net2280_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 			goto print_err;
 		}
 	}
-	ep->is_iso = (tmp == USB_ENDPOINT_XFER_ISOC);
+	ep->is_iso = (type == USB_ENDPOINT_XFER_ISOC);
 	/* Enable this endpoint */
 	if (dev->quirks & PLX_LEGACY) {
-		tmp <<= ENDPOINT_TYPE;
+		tmp |= type << ENDPOINT_TYPE;
 		tmp |= desc->bEndpointAddress;
 		/* default full fifo lines */
 		tmp |= (4 << ENDPOINT_BYTE_COUNT);
@@ -228,17 +249,17 @@ net2280_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 	} else {
 		/* In Legacy mode, only OUT endpoints are used */
 		if (dev->enhanced_mode && ep->is_in) {
-			tmp <<= IN_ENDPOINT_TYPE;
+			tmp |= type << IN_ENDPOINT_TYPE;
 			tmp |= BIT(IN_ENDPOINT_ENABLE);
-			/* Not applicable to Legacy */
-			tmp |= BIT(ENDPOINT_DIRECTION);
 		} else {
-			tmp <<= OUT_ENDPOINT_TYPE;
+			tmp |= type << OUT_ENDPOINT_TYPE;
 			tmp |= BIT(OUT_ENDPOINT_ENABLE);
 			tmp |= (ep->is_in << ENDPOINT_DIRECTION);
 		}
 
-		tmp |= usb_endpoint_num(desc);
+		tmp |= (4 << ENDPOINT_BYTE_COUNT);
+		if (!dev->enhanced_mode)
+			tmp |= usb_endpoint_num(desc);
 		tmp |= (ep->ep.maxburst << MAX_BURST_SIZE);
 	}
 
@@ -256,6 +277,8 @@ net2280_enable(struct usb_ep *_ep, const struct usb_endpoint_descriptor *desc)
 			BIT(CLEAR_NAK_OUT_PACKETS_MODE), &ep->regs->ep_rsp);
 	}
 
+	if (dev->quirks & PLX_SUPERSPEED)
+		ep_clear_seqnum(ep);
 	writel(tmp, &ep->cfg->ep_cfg);
 
 	/* enable irqs */
@@ -441,6 +464,13 @@ static void ep_reset_338x(struct net2280_regs __iomem *regs,
 	       BIT(DATA_PACKET_TRANSMITTED_INTERRUPT) |
 	       BIT(DATA_OUT_PING_TOKEN_INTERRUPT) |
 	       BIT(DATA_IN_TOKEN_INTERRUPT), &ep->regs->ep_stat);
+
+	tmp = readl(&ep->cfg->ep_cfg);
+	if (ep->is_in)
+		tmp &= ~USB3380_EP_CFG_MASK_IN;
+	else
+		tmp &= ~USB3380_EP_CFG_MASK_OUT;
+	writel(tmp, &ep->cfg->ep_cfg);
 }
 
 static void nuke(struct net2280_ep *);
@@ -1468,11 +1498,14 @@ static int net2280_pullup(struct usb_gadget *_gadget, int is_on)
 	spin_lock_irqsave(&dev->lock, flags);
 	tmp = readl(&dev->usb->usbctl);
 	dev->softconnect = (is_on != 0);
-	if (is_on)
-		tmp |= BIT(USB_DETECT_ENABLE);
-	else
-		tmp &= ~BIT(USB_DETECT_ENABLE);
-	writel(tmp, &dev->usb->usbctl);
+	if (is_on) {
+		ep0_start(dev);
+		writel(tmp | BIT(USB_DETECT_ENABLE), &dev->usb->usbctl);
+	} else {
+		writel(tmp & ~BIT(USB_DETECT_ENABLE), &dev->usb->usbctl);
+		stop_activity(dev, dev->driver);
+	}
+
 	spin_unlock_irqrestore(&dev->lock, flags);
 
 	return 0;
@@ -1860,8 +1893,8 @@ static void defect7374_enable_data_eps_zero(struct net2280 *dev)
 	tmp = ((0 << ENDPOINT_NUMBER) | BIT(ENDPOINT_DIRECTION) |
 			(2 << OUT_ENDPOINT_TYPE) | (2 << IN_ENDPOINT_TYPE) |
 			((dev->enhanced_mode) ?
-			 BIT(OUT_ENDPOINT_ENABLE) : BIT(ENDPOINT_ENABLE)) |
-			BIT(IN_ENDPOINT_ENABLE));
+			 BIT(OUT_ENDPOINT_ENABLE) | BIT(IN_ENDPOINT_ENABLE) :
+			 BIT(ENDPOINT_ENABLE)));
 
 	for (i = 1; i < 5; i++)
 		writel(tmp, &dev->ep[i].cfg->ep_cfg);
@@ -1975,9 +2008,15 @@ static void usb_reset_338x(struct net2280 *dev)
 	/* clear old dma and irq state */
 	for (tmp = 0; tmp < 4; tmp++) {
 		struct net2280_ep *ep = &dev->ep[tmp + 1];
+		struct net2280_dma_regs __iomem *dma;
 
-		if (ep->dma)
+		if (ep->dma) {
 			abort_dma(ep);
+		} else {
+			dma = &dev->dma[tmp];
+			writel(BIT(DMA_ABORT), &dma->dmastat);
+			writel(0, &dma->dmactl);
+		}
 	}
 
 	writel(~0, &dev->regs->irqstat0), writel(~0, &dev->regs->irqstat1);
@@ -2065,6 +2104,12 @@ static void usb_reinit_338x(struct net2280 *dev)
 
 		if (dev->enhanced_mode) {
 			ep->cfg = &dev->epregs[ne[i]];
+			/*
+			 * Set USB endpoint number, hardware allows same number
+			 * in both directions.
+			 */
+			 if (i > 0 && i < 5)
+				writel(ne[i], &ep->cfg->ep_cfg);
 			ep->regs = (struct net2280_ep_regs __iomem *)
 				(((void __iomem *)&dev->epregs[ne[i]]) +
 				ep_reg_addr[i]);
@@ -2874,6 +2919,26 @@ next_endpoints3:
 	return;
 }
 
+static void usb338x_handle_ep_intr(struct net2280 *dev, u32 stat0)
+{
+	u32 index;
+	u32 bit;
+
+	for (index = 0; index < ARRAY_SIZE(ep_bit); index++) {
+		bit = BIT(ep_bit[index]);
+
+		if (!stat0)
+			break;
+
+		if (!(stat0 & bit))
+			continue;
+
+		stat0 &= ~bit;
+
+		handle_ep_small(&dev->ep[index]);
+	}
+}
+
 static void handle_stat0_irqs(struct net2280 *dev, u32 stat)
 {
 	struct net2280_ep	*ep;
@@ -3098,20 +3163,31 @@ do_stall:
 #undef	w_length
 
 next_endpoints:
-	/* endpoint data irq ? */
-	scratch = stat & 0x7f;
-	stat &= ~0x7f;
-	for (num = 0; scratch; num++) {
-		u32		t;
-
-		/* do this endpoint's FIFO and queue need tending? */
-		t = BIT(num);
-		if ((scratch & t) == 0)
-			continue;
-		scratch ^= t;
+	if ((dev->quirks & PLX_SUPERSPEED) && dev->enhanced_mode) {
+		u32 mask = (BIT(ENDPOINT_0_INTERRUPT) |
+			USB3380_IRQSTAT0_EP_INTR_MASK_IN |
+			USB3380_IRQSTAT0_EP_INTR_MASK_OUT);
+
+		if (stat & mask) {
+			usb338x_handle_ep_intr(dev, stat & mask);
+			stat &= ~mask;
+		}
+	} else {
+		/* endpoint data irq ? */
+		scratch = stat & 0x7f;
+		stat &= ~0x7f;
+		for (num = 0; scratch; num++) {
+			u32		t;
+
+			/* do this endpoint's FIFO and queue need tending? */
+			t = BIT(num);
+			if ((scratch & t) == 0)
+				continue;
+			scratch ^= t;
 
-		ep = &dev->ep[num];
-		handle_ep_small(ep);
+			ep = &dev->ep[num];
+			handle_ep_small(ep);
+		}
 	}
 
 	if (stat)
diff --git a/drivers/usb/gadget/udc/s3c2410_udc.c b/drivers/usb/gadget/udc/s3c2410_udc.c
index b808951491cc..5d9aa81969b4 100644
--- a/drivers/usb/gadget/udc/s3c2410_udc.c
+++ b/drivers/usb/gadget/udc/s3c2410_udc.c
@@ -92,40 +92,38 @@ static struct s3c2410_udc_mach_info *udc_info;
 
 static uint32_t s3c2410_ticks = 0;
 
-static int dprintk(int level, const char *fmt, ...)
+__printf(2, 3)
+static void dprintk(int level, const char *fmt, ...)
 {
-	static char printk_buf[1024];
 	static long prevticks;
 	static int invocation;
+	struct va_format vaf;
 	va_list args;
-	int len;
 
 	if (level > USB_S3C2410_DEBUG_LEVEL)
-		return 0;
+		return;
+
+	va_start(args, fmt);
+
+	vaf.fmt = fmt;
+	vaf.va = &args;
 
 	if (s3c2410_ticks != prevticks) {
 		prevticks = s3c2410_ticks;
 		invocation = 0;
 	}
 
-	len = scnprintf(printk_buf,
-			sizeof(printk_buf), "%1lu.%02d USB: ",
-			prevticks, invocation++);
+	pr_debug("%1lu.%02d USB: %pV", prevticks, invocation++, &vaf);
 
-	va_start(args, fmt);
-	len = vscnprintf(printk_buf+len,
-			sizeof(printk_buf)-len, fmt, args);
 	va_end(args);
-
-	pr_debug("%s", printk_buf);
-	return len;
 }
 #else
-static int dprintk(int level, const char *fmt, ...)
+__printf(2, 3)
+static void dprintk(int level, const char *fmt, ...)
 {
-	return 0;
 }
 #endif
+
 static int s3c2410_udc_debugfs_seq_show(struct seq_file *m, void *p)
 {
 	u32 addr_reg, pwr_reg, ep_int_reg, usb_int_reg;
@@ -1487,7 +1485,7 @@ static int s3c2410_udc_pullup(struct usb_gadget *gadget, int is_on)
 
 	dprintk(DEBUG_NORMAL, "%s()\n", __func__);
 
-	s3c2410_udc_set_pullup(udc, is_on ? 0 : 1);
+	s3c2410_udc_set_pullup(udc, is_on);
 	return 0;
 }
 
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 197a6a3e613b..8afc3c1efdab 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -137,7 +137,7 @@ config XPS_USB_HCD_XILINX
 		devices only.
 
 config USB_EHCI_FSL
-	bool "Support for Freescale PPC on-chip EHCI USB controller"
+	tristate "Support for Freescale PPC on-chip EHCI USB controller"
 	depends on FSL_SOC
 	select USB_EHCI_ROOT_HUB_TT
 	select USB_FSL_MPH_DR_OF if OF
@@ -295,7 +295,7 @@ config USB_OCTEON_EHCI
 	bool "Octeon on-chip EHCI support (DEPRECATED)"
 	depends on CAVIUM_OCTEON_SOC
 	default n
-	select USB_EHCI_BIG_ENDIAN_MMIO
+	select USB_EHCI_BIG_ENDIAN_MMIO if CPU_BIG_ENDIAN
 	select USB_EHCI_HCD_PLATFORM
 	help
 	  This option is deprecated now and the driver was removed, use
@@ -568,7 +568,7 @@ config USB_OCTEON_OHCI
 	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_BIG_ENDIAN_MMIO if CPU_BIG_ENDIAN
 	select USB_OHCI_LITTLE_ENDIAN
 	select USB_OHCI_HCD_PLATFORM
 	help
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 65b0b6a58599..754efaa8ccf8 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -24,7 +24,9 @@ endif
 
 obj-$(CONFIG_USB_WHCI_HCD)	+= whci/
 
-obj-$(CONFIG_PCI)		+= pci-quirks.o
+ifneq ($(CONFIG_USB), )
+	obj-$(CONFIG_PCI)	+= pci-quirks.o
+endif
 
 obj-$(CONFIG_USB_XHCI_PCI)	+= xhci-pci.o
 obj-$(CONFIG_USB_XHCI_PLATFORM) += xhci-plat-hcd.o
@@ -70,6 +72,7 @@ obj-$(CONFIG_USB_R8A66597_HCD)	+= r8a66597-hcd.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_EHCI_FSL)	+= ehci-fsl.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-dbg.c b/drivers/usb/host/ehci-dbg.c
index 524cbf26d992..b26b96e25a13 100644
--- a/drivers/usb/host/ehci-dbg.c
+++ b/drivers/usb/host/ehci-dbg.c
@@ -628,7 +628,8 @@ static ssize_t fill_periodic_buffer(struct debug_buffer *buf)
 	unsigned		i;
 	__hc32			tag;
 
-	if (!(seen = kmalloc (DBG_SCHED_LIMIT * sizeof *seen, GFP_ATOMIC)))
+	seen = kmalloc(DBG_SCHED_LIMIT * sizeof *seen, GFP_ATOMIC);
+	if (!seen)
 		return 0;
 	seen_count = 0;
 
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index ab4eee3df97a..5352e74b92e2 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -1,6 +1,6 @@
 /*
  * Copyright 2005-2009 MontaVista Software, Inc.
- * Copyright 2008,2012      Freescale Semiconductor, Inc.
+ * Copyright 2008,2012,2015      Freescale Semiconductor, Inc.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License as published by the
@@ -24,29 +24,38 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/types.h>
 #include <linux/delay.h>
 #include <linux/pm.h>
 #include <linux/err.h>
+#include <linux/usb.h>
+#include <linux/usb/ehci_def.h>
+#include <linux/usb/hcd.h>
+#include <linux/usb/otg.h>
 #include <linux/platform_device.h>
 #include <linux/fsl_devices.h>
 
+#include "ehci.h"
 #include "ehci-fsl.h"
 
+#define DRIVER_DESC "Freescale EHCI Host controller driver"
+#define DRV_NAME "ehci-fsl"
+
+static struct hc_driver __read_mostly fsl_ehci_hc_driver;
+
 /* configure so an HC device and id are always provided */
 /* always called with process context; sleeping is OK */
 
-/**
- * usb_hcd_fsl_probe - initialize FSL-based HCDs
- * @drvier: Driver to be used for this HCD
+/*
+ * fsl_ehci_drv_probe - initialize FSL-based HCDs
  * @pdev: USB Host Controller being probed
  * Context: !in_interrupt()
  *
  * Allocates basic resources for this USB host controller.
  *
  */
-static int usb_hcd_fsl_probe(const struct hc_driver *driver,
-			     struct platform_device *pdev)
+static int fsl_ehci_drv_probe(struct platform_device *pdev)
 {
 	struct fsl_usb2_platform_data *pdata;
 	struct usb_hcd *hcd;
@@ -86,7 +95,8 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	}
 	irq = res->start;
 
-	hcd = usb_create_hcd(driver, &pdev->dev, dev_name(&pdev->dev));
+	hcd = usb_create_hcd(&fsl_ehci_hc_driver, &pdev->dev,
+				dev_name(&pdev->dev));
 	if (!hcd) {
 		retval = -ENOMEM;
 		goto err1;
@@ -159,38 +169,6 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	return retval;
 }
 
-/* may be called without controller electrically present */
-/* may be called with controller, bus, and devices active */
-
-/**
- * usb_hcd_fsl_remove - shutdown processing for FSL-based HCDs
- * @dev: USB Host Controller being removed
- * Context: !in_interrupt()
- *
- * Reverses the effect of usb_hcd_fsl_probe().
- *
- */
-static void usb_hcd_fsl_remove(struct usb_hcd *hcd,
-			       struct platform_device *pdev)
-{
-	struct fsl_usb2_platform_data *pdata = dev_get_platdata(&pdev->dev);
-
-	if (!IS_ERR_OR_NULL(hcd->usb_phy)) {
-		otg_set_host(hcd->usb_phy->otg, NULL);
-		usb_put_phy(hcd->usb_phy);
-	}
-
-	usb_remove_hcd(hcd);
-
-	/*
-	 * do platform specific un-initialization:
-	 * release iomux pins, disable clock, etc.
-	 */
-	if (pdata->exit)
-		pdata->exit(pdev);
-	usb_put_hcd(hcd);
-}
-
 static int ehci_fsl_setup_phy(struct usb_hcd *hcd,
 			       enum fsl_usb2_phy_modes phy_mode,
 			       unsigned int port_offset)
@@ -636,79 +614,77 @@ static int ehci_start_port_reset(struct usb_hcd *hcd, unsigned port)
 #define ehci_start_port_reset	NULL
 #endif /* CONFIG_USB_OTG */
 
+static struct ehci_driver_overrides ehci_fsl_overrides __initdata = {
+	.extra_priv_size = sizeof(struct ehci_fsl),
+	.reset = ehci_fsl_setup,
+};
 
-static const struct hc_driver ehci_fsl_hc_driver = {
-	.description = hcd_name,
-	.product_desc = "Freescale On-Chip EHCI Host Controller",
-	.hcd_priv_size = sizeof(struct ehci_fsl),
+/**
+ * fsl_ehci_drv_remove - shutdown processing for FSL-based HCDs
+ * @dev: USB Host Controller being removed
+ * Context: !in_interrupt()
+ *
+ * Reverses the effect of usb_hcd_fsl_probe().
+ *
+ */
 
-	/*
-	 * generic hardware linkage
-	 */
-	.irq = ehci_irq,
-	.flags = HCD_USB2 | HCD_MEMORY | HCD_BH,
+static int fsl_ehci_drv_remove(struct platform_device *pdev)
+{
+	struct fsl_usb2_platform_data *pdata = dev_get_platdata(&pdev->dev);
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 
-	/*
-	 * basic lifecycle operations
-	 */
-	.reset = ehci_fsl_setup,
-	.start = ehci_run,
-	.stop = ehci_stop,
-	.shutdown = ehci_shutdown,
+	if (!IS_ERR_OR_NULL(hcd->usb_phy)) {
+		otg_set_host(hcd->usb_phy->otg, NULL);
+		usb_put_phy(hcd->usb_phy);
+	}
 
-	/*
-	 * 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,
+	usb_remove_hcd(hcd);
 
 	/*
-	 * scheduling support
+	 * do platform specific un-initialization:
+	 * release iomux pins, disable clock, etc.
 	 */
-	.get_frame_number = ehci_get_frame,
+	if (pdata->exit)
+		pdata->exit(pdev);
+	usb_put_hcd(hcd);
 
-	/*
-	 * 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,
-	.start_port_reset = ehci_start_port_reset,
-	.relinquish_port = ehci_relinquish_port,
-	.port_handed_over = ehci_port_handed_over,
-
-	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
+	return 0;
+}
+
+static struct platform_driver ehci_fsl_driver = {
+	.probe = fsl_ehci_drv_probe,
+	.remove = fsl_ehci_drv_remove,
+	.shutdown = usb_hcd_platform_shutdown,
+	.driver = {
+		.name = "fsl-ehci",
+		.pm = EHCI_FSL_PM_OPS,
+	},
 };
 
-static int ehci_fsl_drv_probe(struct platform_device *pdev)
+static int __init ehci_fsl_init(void)
 {
 	if (usb_disabled())
 		return -ENODEV;
 
-	/* FIXME we only want one one probe() not two */
-	return usb_hcd_fsl_probe(&ehci_fsl_hc_driver, pdev);
-}
+	pr_info(DRV_NAME ": " DRIVER_DESC "\n");
 
-static int ehci_fsl_drv_remove(struct platform_device *pdev)
-{
-	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	ehci_init_driver(&fsl_ehci_hc_driver, &ehci_fsl_overrides);
 
-	/* FIXME we only want one one remove() not two */
-	usb_hcd_fsl_remove(hcd, pdev);
-	return 0;
+	fsl_ehci_hc_driver.product_desc =
+			"Freescale On-Chip EHCI Host Controller";
+	fsl_ehci_hc_driver.start_port_reset = ehci_start_port_reset;
+
+
+	return platform_driver_register(&ehci_fsl_driver);
 }
+module_init(ehci_fsl_init);
 
-MODULE_ALIAS("platform:fsl-ehci");
+static void __exit ehci_fsl_cleanup(void)
+{
+	platform_driver_unregister(&ehci_fsl_driver);
+}
+module_exit(ehci_fsl_cleanup);
 
-static struct platform_driver ehci_fsl_driver = {
-	.probe = ehci_fsl_drv_probe,
-	.remove = ehci_fsl_drv_remove,
-	.shutdown = usb_hcd_platform_shutdown,
-	.driver = {
-		.name = "fsl-ehci",
-		.pm = EHCI_FSL_PM_OPS,
-	},
-};
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index f4d88dfb26a7..c63d82c91f10 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -239,7 +239,7 @@ static void tdi_reset (struct ehci_hcd *ehci)
  * Reset a non-running (STS_HALT == 1) controller.
  * Must be called with interrupts enabled and the lock not held.
  */
-static int ehci_reset (struct ehci_hcd *ehci)
+int ehci_reset(struct ehci_hcd *ehci)
 {
 	int	retval;
 	u32	command = ehci_readl(ehci, &ehci->regs->command);
@@ -275,6 +275,7 @@ static int ehci_reset (struct ehci_hcd *ehci)
 			ehci->resuming_ports = 0;
 	return retval;
 }
+EXPORT_SYMBOL_GPL(ehci_reset);
 
 /*
  * Idle the controller (turn off the schedules).
@@ -1250,11 +1251,6 @@ MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR (DRIVER_AUTHOR);
 MODULE_LICENSE ("GPL");
 
-#ifdef CONFIG_USB_EHCI_FSL
-#include "ehci-fsl.c"
-#define	PLATFORM_DRIVER		ehci_fsl_driver
-#endif
-
 #ifdef CONFIG_USB_EHCI_SH
 #include "ehci-sh.c"
 #define PLATFORM_DRIVER		ehci_hcd_sh_driver
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 69208447d213..22abb6830dfa 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -155,7 +155,7 @@ static int ehci_port_change(struct ehci_hcd *ehci)
 	return 0;
 }
 
-static void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
+void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
 		bool suspending, bool do_wakeup)
 {
 	int		port;
@@ -220,6 +220,7 @@ static void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
 
 	spin_unlock_irq(&ehci->lock);
 }
+EXPORT_SYMBOL_GPL(ehci_adjust_port_wakeup_flags);
 
 static int ehci_bus_suspend (struct usb_hcd *hcd)
 {
diff --git a/drivers/usb/host/ehci-platform.c b/drivers/usb/host/ehci-platform.c
index d8a75a51d6d4..2593def13cea 100644
--- a/drivers/usb/host/ehci-platform.c
+++ b/drivers/usb/host/ehci-platform.c
@@ -88,15 +88,13 @@ static int ehci_platform_power_on(struct platform_device *dev)
 	}
 
 	for (phy_num = 0; phy_num < priv->num_phys; phy_num++) {
-		if (priv->phys[phy_num]) {
-			ret = phy_init(priv->phys[phy_num]);
-			if (ret)
-				goto err_exit_phy;
-			ret = phy_power_on(priv->phys[phy_num]);
-			if (ret) {
-				phy_exit(priv->phys[phy_num]);
-				goto err_exit_phy;
-			}
+		ret = phy_init(priv->phys[phy_num]);
+		if (ret)
+			goto err_exit_phy;
+		ret = phy_power_on(priv->phys[phy_num]);
+		if (ret) {
+			phy_exit(priv->phys[phy_num]);
+			goto err_exit_phy;
 		}
 	}
 
@@ -104,10 +102,8 @@ static int ehci_platform_power_on(struct platform_device *dev)
 
 err_exit_phy:
 	while (--phy_num >= 0) {
-		if (priv->phys[phy_num]) {
-			phy_power_off(priv->phys[phy_num]);
-			phy_exit(priv->phys[phy_num]);
-		}
+		phy_power_off(priv->phys[phy_num]);
+		phy_exit(priv->phys[phy_num]);
 	}
 err_disable_clks:
 	while (--clk >= 0)
@@ -123,10 +119,8 @@ static void ehci_platform_power_off(struct platform_device *dev)
 	int clk, phy_num;
 
 	for (phy_num = 0; phy_num < priv->num_phys; phy_num++) {
-		if (priv->phys[phy_num]) {
-			phy_power_off(priv->phys[phy_num]);
-			phy_exit(priv->phys[phy_num]);
-		}
+		phy_power_off(priv->phys[phy_num]);
+		phy_exit(priv->phys[phy_num]);
 	}
 
 	for (clk = EHCI_MAX_CLKS - 1; clk >= 0; clk--)
@@ -154,7 +148,6 @@ static int ehci_platform_probe(struct platform_device *dev)
 	struct usb_ehci_pdata *pdata = dev_get_platdata(&dev->dev);
 	struct ehci_platform_priv *priv;
 	struct ehci_hcd *ehci;
-	const char *phy_name;
 	int err, irq, phy_num, clk = 0;
 
 	if (usb_disabled())
@@ -202,38 +195,28 @@ static int ehci_platform_probe(struct platform_device *dev)
 					  "needs-reset-on-resume"))
 			pdata->reset_on_resume = 1;
 
+		if (of_property_read_bool(dev->dev.of_node,
+					  "has-transaction-translator"))
+			pdata->has_tt = 1;
+
 		priv->num_phys = of_count_phandle_with_args(dev->dev.of_node,
 				"phys", "#phy-cells");
-		priv->num_phys = priv->num_phys > 0 ? priv->num_phys : 1;
 
-		priv->phys = devm_kcalloc(&dev->dev, priv->num_phys,
-				sizeof(struct phy *), GFP_KERNEL);
-		if (!priv->phys)
-			return -ENOMEM;
+		if (priv->num_phys > 0) {
+			priv->phys = devm_kcalloc(&dev->dev, priv->num_phys,
+					    sizeof(struct phy *), GFP_KERNEL);
+			if (!priv->phys)
+				return -ENOMEM;
+		} else
+			priv->num_phys = 0;
 
 		for (phy_num = 0; phy_num < priv->num_phys; phy_num++) {
-				err = of_property_read_string_index(
-						dev->dev.of_node,
-						"phy-names", phy_num,
-						&phy_name);
-
-				if (err < 0) {
-					if (priv->num_phys > 1) {
-						dev_err(&dev->dev, "phy-names not provided");
-						goto err_put_hcd;
-					} else
-						phy_name = "usb";
-				}
-
-				priv->phys[phy_num] = devm_phy_get(&dev->dev,
-						phy_name);
-				if (IS_ERR(priv->phys[phy_num])) {
-					err = PTR_ERR(priv->phys[phy_num]);
-					if ((priv->num_phys > 1) ||
-					    (err == -EPROBE_DEFER))
-						goto err_put_hcd;
-					priv->phys[phy_num] = NULL;
-				}
+			priv->phys[phy_num] = devm_of_phy_get_by_index(
+					&dev->dev, dev->dev.of_node, phy_num);
+			if (IS_ERR(priv->phys[phy_num])) {
+				err = PTR_ERR(priv->phys[phy_num]);
+					goto err_put_hcd;
+			}
 		}
 
 		for (clk = 0; clk < EHCI_MAX_CLKS; clk++) {
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index ff9af29b4e9f..4031b372008e 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -304,6 +304,7 @@ struct dma_aligned_buffer {
 static void free_dma_aligned_buffer(struct urb *urb)
 {
 	struct dma_aligned_buffer *temp;
+	size_t length;
 
 	if (!(urb->transfer_flags & URB_ALIGNED_TEMP_BUFFER))
 		return;
@@ -311,9 +312,14 @@ static void free_dma_aligned_buffer(struct urb *urb)
 	temp = container_of(urb->transfer_buffer,
 		struct dma_aligned_buffer, data);
 
-	if (usb_urb_dir_in(urb))
-		memcpy(temp->old_xfer_buffer, temp->data,
-		       urb->transfer_buffer_length);
+	if (usb_urb_dir_in(urb)) {
+		if (usb_pipeisoc(urb->pipe))
+			length = urb->transfer_buffer_length;
+		else
+			length = urb->actual_length;
+
+		memcpy(temp->old_xfer_buffer, temp->data, length);
+	}
 	urb->transfer_buffer = temp->old_xfer_buffer;
 	kfree(temp->kmalloc_ptr);
 
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 52ef0844a180..f700157cd084 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -868,10 +868,13 @@ extern void	ehci_init_driver(struct hc_driver *drv,
 extern int	ehci_setup(struct usb_hcd *hcd);
 extern int	ehci_handshake(struct ehci_hcd *ehci, void __iomem *ptr,
 				u32 mask, u32 done, int usec);
+extern int	ehci_reset(struct ehci_hcd *ehci);
 
 #ifdef CONFIG_PM
 extern int	ehci_suspend(struct usb_hcd *hcd, bool do_wakeup);
 extern int	ehci_resume(struct usb_hcd *hcd, bool force_reset);
+extern void	ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
+			bool suspending, bool do_wakeup);
 #endif	/* CONFIG_PM */
 
 extern int	ehci_hub_control(struct usb_hcd	*hcd, u16 typeReq, u16 wValue,
diff --git a/drivers/usb/host/fsl-mph-dr-of.c b/drivers/usb/host/fsl-mph-dr-of.c
index 7e325e90d7d9..5e0d60035216 100644
--- a/drivers/usb/host/fsl-mph-dr-of.c
+++ b/drivers/usb/host/fsl-mph-dr-of.c
@@ -126,6 +126,8 @@ static int usb_get_ver_info(struct device_node *np)
 	/*
 	 * returns 1 for usb controller version 1.6
 	 * returns 2 for usb controller version 2.2
+	 * returns 3 for usb controller version 2.4
+	 * returns 4 for usb controller version 2.5
 	 * returns 0 otherwise
 	 */
 	if (of_device_is_compatible(np, "fsl-usb2-dr")) {
@@ -135,6 +137,8 @@ static int usb_get_ver_info(struct device_node *np)
 			ver = FSL_USB_VER_2_2;
 		else if (of_device_is_compatible(np, "fsl-usb2-dr-v2.4"))
 			ver = FSL_USB_VER_2_4;
+		else if (of_device_is_compatible(np, "fsl-usb2-dr-v2.5"))
+			ver = FSL_USB_VER_2_5;
 		else /* for previous controller versions */
 			ver = FSL_USB_VER_OLD;
 
@@ -150,6 +154,10 @@ static int usb_get_ver_info(struct device_node *np)
 			ver = FSL_USB_VER_1_6;
 		else if (of_device_is_compatible(np, "fsl-usb2-mph-v2.2"))
 			ver = FSL_USB_VER_2_2;
+		else if (of_device_is_compatible(np, "fsl-usb2-mph-v2.4"))
+			ver = FSL_USB_VER_2_4;
+		else if (of_device_is_compatible(np, "fsl-usb2-mph-v2.5"))
+			ver = FSL_USB_VER_2_5;
 		else /* for previous controller versions */
 			ver = FSL_USB_VER_OLD;
 	}
diff --git a/drivers/usb/host/fusbh200-hcd.c b/drivers/usb/host/fusbh200-hcd.c
index 00e492eaba6a..1fd8718a9f11 100644
--- a/drivers/usb/host/fusbh200-hcd.c
+++ b/drivers/usb/host/fusbh200-hcd.c
@@ -499,7 +499,8 @@ static ssize_t fill_periodic_buffer(struct debug_buffer *buf)
 	unsigned		i;
 	__hc32			tag;
 
-	if (!(seen = kmalloc (DBG_SCHED_LIMIT * sizeof *seen, GFP_ATOMIC)))
+	seen = kmalloc(DBG_SCHED_LIMIT * sizeof *seen, GFP_ATOMIC);
+	if (!seen)
 		return 0;
 	seen_count = 0;
 
diff --git a/drivers/usb/host/isp116x-hcd.c b/drivers/usb/host/isp116x-hcd.c
index 13181dcd9820..d089b3fb7a13 100644
--- a/drivers/usb/host/isp116x-hcd.c
+++ b/drivers/usb/host/isp116x-hcd.c
@@ -500,7 +500,8 @@ static void start_atl_transfers(struct isp116x *isp116x)
 	if (isp116x->periodic_count) {
 		isp116x->fmindex = index =
 		    (isp116x->fmindex + 1) & (PERIODIC_SIZE - 1);
-		if ((load = isp116x->load[index])) {
+		load = isp116x->load[index];
+		if (load) {
 			/* Bring all int transfers for this frame
 			   into the active queue */
 			isp116x->atl_active = last_ep =
diff --git a/drivers/usb/host/ohci-dbg.c b/drivers/usb/host/ohci-dbg.c
index 04f2186939d2..c3eded317495 100644
--- a/drivers/usb/host/ohci-dbg.c
+++ b/drivers/usb/host/ohci-dbg.c
@@ -491,7 +491,8 @@ static ssize_t fill_periodic_buffer(struct debug_buffer *buf)
 	char			*next;
 	unsigned		i;
 
-	if (!(seen = kmalloc (DBG_SCHED_LIMIT * sizeof *seen, GFP_ATOMIC)))
+	seen = kmalloc(DBG_SCHED_LIMIT * sizeof *seen, GFP_ATOMIC);
+	if (!seen)
 		return 0;
 	seen_count = 0;
 
@@ -506,7 +507,8 @@ static ssize_t fill_periodic_buffer(struct debug_buffer *buf)
 	/* dump a snapshot of the periodic schedule (and load) */
 	spin_lock_irqsave (&ohci->lock, flags);
 	for (i = 0; i < NUM_INTS; i++) {
-		if (!(ed = ohci->periodic [i]))
+		ed = ohci->periodic[i];
+		if (!ed)
 			continue;
 
 		temp = scnprintf (next, size, "%2d [%3d]:", i, ohci->load [i]);
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 1dab9dfbca6a..760cb57e954e 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -155,7 +155,8 @@ static int ohci_urb_enqueue (
 	int		retval = 0;
 
 	/* every endpoint has a ed, locate and maybe (re)initialize it */
-	if (! (ed = ed_get (ohci, urb->ep, urb->dev, pipe, urb->interval)))
+	ed = ed_get(ohci, urb->ep, urb->dev, pipe, urb->interval);
+	if (! ed)
 		return -ENOMEM;
 
 	/* for the private part of the URB we need the number of TDs (size) */
diff --git a/drivers/usb/host/ohci-platform.c b/drivers/usb/host/ohci-platform.c
index 185ceee52d47..c2669f185f65 100644
--- a/drivers/usb/host/ohci-platform.c
+++ b/drivers/usb/host/ohci-platform.c
@@ -57,15 +57,13 @@ static int ohci_platform_power_on(struct platform_device *dev)
 	}
 
 	for (phy_num = 0; phy_num < priv->num_phys; phy_num++) {
-		if (priv->phys[phy_num]) {
-			ret = phy_init(priv->phys[phy_num]);
-			if (ret)
-				goto err_exit_phy;
-			ret = phy_power_on(priv->phys[phy_num]);
-			if (ret) {
-				phy_exit(priv->phys[phy_num]);
-				goto err_exit_phy;
-			}
+		ret = phy_init(priv->phys[phy_num]);
+		if (ret)
+			goto err_exit_phy;
+		ret = phy_power_on(priv->phys[phy_num]);
+		if (ret) {
+			phy_exit(priv->phys[phy_num]);
+			goto err_exit_phy;
 		}
 	}
 
@@ -73,10 +71,8 @@ static int ohci_platform_power_on(struct platform_device *dev)
 
 err_exit_phy:
 	while (--phy_num >= 0) {
-		if (priv->phys[phy_num]) {
-			phy_power_off(priv->phys[phy_num]);
-			phy_exit(priv->phys[phy_num]);
-		}
+		phy_power_off(priv->phys[phy_num]);
+		phy_exit(priv->phys[phy_num]);
 	}
 err_disable_clks:
 	while (--clk >= 0)
@@ -92,10 +88,8 @@ static void ohci_platform_power_off(struct platform_device *dev)
 	int clk, phy_num;
 
 	for (phy_num = 0; phy_num < priv->num_phys; phy_num++) {
-		if (priv->phys[phy_num]) {
-			phy_power_off(priv->phys[phy_num]);
-			phy_exit(priv->phys[phy_num]);
-		}
+		phy_power_off(priv->phys[phy_num]);
+		phy_exit(priv->phys[phy_num]);
 	}
 
 	for (clk = OHCI_MAX_CLKS - 1; clk >= 0; clk--)
@@ -123,7 +117,6 @@ static int ohci_platform_probe(struct platform_device *dev)
 	struct usb_ohci_pdata *pdata = dev_get_platdata(&dev->dev);
 	struct ohci_platform_priv *priv;
 	struct ohci_hcd *ohci;
-	const char *phy_name;
 	int err, irq, phy_num, clk = 0;
 
 	if (usb_disabled())
@@ -174,36 +167,22 @@ static int ohci_platform_probe(struct platform_device *dev)
 
 		priv->num_phys = of_count_phandle_with_args(dev->dev.of_node,
 				"phys", "#phy-cells");
-		priv->num_phys = priv->num_phys > 0 ? priv->num_phys : 1;
 
-		priv->phys = devm_kcalloc(&dev->dev, priv->num_phys,
-				sizeof(struct phy *), GFP_KERNEL);
-		if (!priv->phys)
-			return -ENOMEM;
+		if (priv->num_phys > 0) {
+			priv->phys = devm_kcalloc(&dev->dev, priv->num_phys,
+					    sizeof(struct phy *), GFP_KERNEL);
+			if (!priv->phys)
+				return -ENOMEM;
+		} else
+			priv->num_phys = 0;
 
 		for (phy_num = 0; phy_num < priv->num_phys; phy_num++) {
-				err = of_property_read_string_index(
-						dev->dev.of_node,
-						"phy-names", phy_num,
-						&phy_name);
-
-				if (err < 0) {
-					if (priv->num_phys > 1) {
-						dev_err(&dev->dev, "phy-names not provided");
-						goto err_put_hcd;
-					} else
-						phy_name = "usb";
-				}
-
-				priv->phys[phy_num] = devm_phy_get(&dev->dev,
-						phy_name);
-				if (IS_ERR(priv->phys[phy_num])) {
-					err = PTR_ERR(priv->phys[phy_num]);
-					if ((priv->num_phys > 1) ||
-					    (err == -EPROBE_DEFER))
-						goto err_put_hcd;
-					priv->phys[phy_num] = NULL;
-				}
+			priv->phys[phy_num] = devm_of_phy_get_by_index(
+					&dev->dev, dev->dev.of_node, phy_num);
+			if (IS_ERR(priv->phys[phy_num])) {
+				err = PTR_ERR(priv->phys[phy_num]);
+				goto err_put_hcd;
+			}
 		}
 
 		for (clk = 0; clk < OHCI_MAX_CLKS; clk++) {
diff --git a/drivers/usb/host/ohci-q.c b/drivers/usb/host/ohci-q.c
index 1463c398d322..f7d561ed3c23 100644
--- a/drivers/usb/host/ohci-q.c
+++ b/drivers/usb/host/ohci-q.c
@@ -407,7 +407,8 @@ static struct ed *ed_get (
 
 	spin_lock_irqsave (&ohci->lock, flags);
 
-	if (!(ed = ep->hcpriv)) {
+	ed = ep->hcpriv;
+	if (!ed) {
 		struct td	*td;
 		int		is_out;
 		u32		info;
diff --git a/drivers/usb/host/ssb-hcd.c b/drivers/usb/host/ssb-hcd.c
index ffc32f4b1b1b..62b6b7804c66 100644
--- a/drivers/usb/host/ssb-hcd.c
+++ b/drivers/usb/host/ssb-hcd.c
@@ -105,7 +105,7 @@ static struct platform_device *ssb_hcd_create_pdev(struct ssb_device *dev, bool
 {
 	struct platform_device *hci_dev;
 	struct resource hci_res[2];
-	int ret = -ENOMEM;
+	int ret;
 
 	memset(hci_res, 0, sizeof(hci_res));
 
@@ -119,7 +119,7 @@ static struct platform_device *ssb_hcd_create_pdev(struct ssb_device *dev, bool
 	hci_dev = platform_device_alloc(ohci ? "ohci-platform" :
 					"ehci-platform" , 0);
 	if (!hci_dev)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
 	hci_dev->dev.parent = dev->dev;
 	hci_dev->dev.dma_mask = &hci_dev->dev.coherent_dma_mask;
@@ -166,7 +166,8 @@ static int ssb_hcd_probe(struct ssb_device *dev,
 	if (dma_set_mask_and_coherent(dev->dma_dev, DMA_BIT_MASK(32)))
 		return -EOPNOTSUPP;
 
-	usb_dev = kzalloc(sizeof(struct ssb_hcd_device), GFP_KERNEL);
+	usb_dev = devm_kzalloc(dev->dev, sizeof(struct ssb_hcd_device),
+			       GFP_KERNEL);
 	if (!usb_dev)
 		return -ENOMEM;
 
@@ -181,10 +182,8 @@ static int ssb_hcd_probe(struct ssb_device *dev,
 	start = ssb_admatch_base(tmp);
 	len = (coreid == SSB_DEV_USB20_HOST) ? 0x800 : ssb_admatch_size(tmp);
 	usb_dev->ohci_dev = ssb_hcd_create_pdev(dev, true, start, len);
-	if (IS_ERR(usb_dev->ohci_dev)) {
-		err = PTR_ERR(usb_dev->ohci_dev);
-		goto err_free_usb_dev;
-	}
+	if (IS_ERR(usb_dev->ohci_dev))
+		return PTR_ERR(usb_dev->ohci_dev);
 
 	if (coreid == SSB_DEV_USB20_HOST) {
 		start = ssb_admatch_base(tmp) + 0x800; /* ehci core offset */
@@ -200,8 +199,6 @@ static int ssb_hcd_probe(struct ssb_device *dev,
 
 err_unregister_ohci_dev:
 	platform_device_unregister(usb_dev->ohci_dev);
-err_free_usb_dev:
-	kfree(usb_dev);
 	return err;
 }
 
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 0827d7c96527..e75c565feb53 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -1184,6 +1184,10 @@ int xhci_bus_resume(struct usb_hcd *hcd)
 	struct xhci_bus_state *bus_state;
 	u32 temp;
 	unsigned long flags;
+	unsigned long port_was_suspended = 0;
+	bool need_usb2_u3_exit = false;
+	int slot_id;
+	int sret;
 
 	max_ports = xhci_get_ports(hcd, &port_array);
 	bus_state = &xhci->bus_state[hcd_index(hcd)];
@@ -1207,7 +1211,6 @@ int xhci_bus_resume(struct usb_hcd *hcd)
 		/* Check whether need resume ports. If needed
 		   resume port and disable remote wakeup */
 		u32 temp;
-		int slot_id;
 
 		temp = readl(port_array[port_index]);
 		if (DEV_SUPERSPEED(temp))
@@ -1216,39 +1219,47 @@ int xhci_bus_resume(struct usb_hcd *hcd)
 			temp &= ~(PORT_RWC_BITS | PORT_WAKE_BITS);
 		if (test_bit(port_index, &bus_state->bus_suspended) &&
 		    (temp & PORT_PLS_MASK)) {
-			if (DEV_SUPERSPEED(temp)) {
-				xhci_set_link_state(xhci, port_array,
-							port_index, XDEV_U0);
-			} else {
+			set_bit(port_index, &port_was_suspended);
+			if (!DEV_SUPERSPEED(temp)) {
 				xhci_set_link_state(xhci, port_array,
 						port_index, XDEV_RESUME);
-
-				spin_unlock_irqrestore(&xhci->lock, flags);
-				msleep(20);
-				spin_lock_irqsave(&xhci->lock, flags);
-
-				xhci_set_link_state(xhci, port_array,
-							port_index, XDEV_U0);
+				need_usb2_u3_exit = true;
 			}
-			/* wait for the port to enter U0 and report port link
-			 * state change.
-			 */
-			spin_unlock_irqrestore(&xhci->lock, flags);
-			msleep(20);
-			spin_lock_irqsave(&xhci->lock, flags);
-
-			/* Clear PLC */
-			xhci_test_and_clear_bit(xhci, port_array, port_index,
-						PORT_PLC);
-
-			slot_id = xhci_find_slot_id_by_port(hcd,
-					xhci, port_index + 1);
-			if (slot_id)
-				xhci_ring_device(xhci, slot_id);
 		} else
 			writel(temp, port_array[port_index]);
 	}
 
+	if (need_usb2_u3_exit) {
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		msleep(20);
+		spin_lock_irqsave(&xhci->lock, flags);
+	}
+
+	port_index = max_ports;
+	while (port_index--) {
+		if (!(port_was_suspended & BIT(port_index)))
+			continue;
+		/* Clear PLC to poll it later after XDEV_U0 */
+		xhci_test_and_clear_bit(xhci, port_array, port_index, PORT_PLC);
+		xhci_set_link_state(xhci, port_array, port_index, XDEV_U0);
+	}
+
+	port_index = max_ports;
+	while (port_index--) {
+		if (!(port_was_suspended & BIT(port_index)))
+			continue;
+		/* Poll and Clear PLC */
+		sret = xhci_handshake(port_array[port_index], PORT_PLC,
+				      PORT_PLC, 10 * 1000);
+		if (sret)
+			xhci_warn(xhci, "port %d resume PLC timeout\n",
+				  port_index);
+		xhci_test_and_clear_bit(xhci, port_array, port_index, PORT_PLC);
+		slot_id = xhci_find_slot_id_by_port(hcd, xhci, port_index + 1);
+		if (slot_id)
+			xhci_ring_device(xhci, slot_id);
+	}
+
 	(void) readl(&xhci->op_regs->command);
 
 	bus_state->next_statechange = jiffies + msecs_to_jiffies(5);
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index 2af32e26fafc..4a4cb1d91ac8 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -45,6 +45,13 @@ static const char hcd_name[] = "xhci_hcd";
 
 static struct hc_driver __read_mostly xhci_pci_hc_driver;
 
+static int xhci_pci_setup(struct usb_hcd *hcd);
+
+static const struct xhci_driver_overrides xhci_pci_overrides __initconst = {
+	.extra_priv_size = sizeof(struct xhci_hcd),
+	.reset = xhci_pci_setup,
+};
+
 /* called after powerup, by probe or system-pm "wakeup" */
 static int xhci_pci_reinit(struct xhci_hcd *xhci, struct pci_dev *pdev)
 {
@@ -206,7 +213,6 @@ static int xhci_pci_setup(struct usb_hcd *hcd)
 	if (!retval)
 		return retval;
 
-	kfree(xhci);
 	return retval;
 }
 
@@ -247,11 +253,6 @@ static int xhci_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		goto dealloc_usb2_hcd;
 	}
 
-	/* Set the xHCI pointer before xhci_pci_setup() (aka hcd_driver.reset)
-	 * is called by usb_add_hcd().
-	 */
-	*((struct xhci_hcd **) xhci->shared_hcd->hcd_priv) = xhci;
-
 	retval = usb_add_hcd(xhci->shared_hcd, dev->irq,
 			IRQF_SHARED);
 	if (retval)
@@ -290,8 +291,6 @@ static void xhci_pci_remove(struct pci_dev *dev)
 	/* Workaround for spurious wakeups at shutdown with HSW */
 	if (xhci->quirks & XHCI_SPURIOUS_WAKEUP)
 		pci_set_power_state(dev, PCI_D3hot);
-
-	kfree(xhci);
 }
 
 #ifdef CONFIG_PM
@@ -379,7 +378,7 @@ static struct pci_driver xhci_pci_driver = {
 
 static int __init xhci_pci_init(void)
 {
-	xhci_init_driver(&xhci_pci_hc_driver, xhci_pci_setup);
+	xhci_init_driver(&xhci_pci_hc_driver, &xhci_pci_overrides);
 #ifdef CONFIG_PM
 	xhci_pci_hc_driver.pci_suspend = xhci_pci_suspend;
 	xhci_pci_hc_driver.pci_resume = xhci_pci_resume;
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 783e819139a7..890ad9d9d329 100644
--- a/drivers/usb/host/xhci-plat.c
+++ b/drivers/usb/host/xhci-plat.c
@@ -26,6 +26,15 @@
 
 static struct hc_driver __read_mostly xhci_plat_hc_driver;
 
+static int xhci_plat_setup(struct usb_hcd *hcd);
+static int xhci_plat_start(struct usb_hcd *hcd);
+
+static const struct xhci_driver_overrides xhci_plat_overrides __initconst = {
+	.extra_priv_size = sizeof(struct xhci_hcd),
+	.reset = xhci_plat_setup,
+	.start = xhci_plat_start,
+};
+
 static void xhci_plat_quirks(struct device *dev, struct xhci_hcd *xhci)
 {
 	/*
@@ -127,31 +136,21 @@ static int xhci_plat_probe(struct platform_device *pdev)
 			goto disable_clk;
 	}
 
-	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
-	if (ret)
-		goto disable_clk;
-
 	device_wakeup_enable(hcd->self.controller);
 
-	/* USB 2.0 roothub is stored in the platform_device now. */
-	hcd = platform_get_drvdata(pdev);
 	xhci = hcd_to_xhci(hcd);
 	xhci->clk = clk;
+	xhci->main_hcd = hcd;
 	xhci->shared_hcd = usb_create_shared_hcd(driver, &pdev->dev,
 			dev_name(&pdev->dev), hcd);
 	if (!xhci->shared_hcd) {
 		ret = -ENOMEM;
-		goto dealloc_usb2_hcd;
+		goto disable_clk;
 	}
 
 	if ((node && of_property_read_bool(node, "usb3-lpm-capable")) ||
 			(pdata && pdata->usb3_lpm_capable))
 		xhci->quirks |= XHCI_LPM_SUPPORT;
-	/*
-	 * Set the xHCI pointer before xhci_plat_setup() (aka hcd_driver.reset)
-	 * is called by usb_add_hcd().
-	 */
-	*((struct xhci_hcd **) xhci->shared_hcd->hcd_priv) = xhci;
 
 	if (HCC_MAX_PSA(xhci->hcc_params) >= 4)
 		xhci->shared_hcd->can_do_streams = 1;
@@ -168,21 +167,26 @@ static int xhci_plat_probe(struct platform_device *pdev)
 			goto put_usb3_hcd;
 	}
 
-	ret = usb_add_hcd(xhci->shared_hcd, irq, IRQF_SHARED);
+	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (ret)
 		goto disable_usb_phy;
 
+	ret = usb_add_hcd(xhci->shared_hcd, irq, IRQF_SHARED);
+	if (ret)
+		goto dealloc_usb2_hcd;
+
 	return 0;
 
+
+dealloc_usb2_hcd:
+	usb_remove_hcd(hcd);
+
 disable_usb_phy:
 	usb_phy_shutdown(hcd->usb_phy);
 
 put_usb3_hcd:
 	usb_put_hcd(xhci->shared_hcd);
 
-dealloc_usb2_hcd:
-	usb_remove_hcd(hcd);
-
 disable_clk:
 	if (!IS_ERR(clk))
 		clk_disable_unprepare(clk);
@@ -201,13 +205,13 @@ static int xhci_plat_remove(struct platform_device *dev)
 
 	usb_remove_hcd(xhci->shared_hcd);
 	usb_phy_shutdown(hcd->usb_phy);
-	usb_put_hcd(xhci->shared_hcd);
 
 	usb_remove_hcd(hcd);
+	usb_put_hcd(xhci->shared_hcd);
+
 	if (!IS_ERR(clk))
 		clk_disable_unprepare(clk);
 	usb_put_hcd(hcd);
-	kfree(xhci);
 
 	return 0;
 }
@@ -271,8 +275,7 @@ MODULE_ALIAS("platform:xhci-hcd");
 
 static int __init xhci_plat_init(void)
 {
-	xhci_init_driver(&xhci_plat_hc_driver, xhci_plat_setup);
-	xhci_plat_hc_driver.start = xhci_plat_start;
+	xhci_init_driver(&xhci_plat_hc_driver, &xhci_plat_overrides);
 	return platform_driver_register(&usb_xhci_driver);
 }
 module_init(xhci_plat_init);
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 7d34cbfaf373..94416ff70810 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -1934,7 +1934,7 @@ static int process_ctrl_td(struct xhci_hcd *xhci, struct xhci_td *td,
 			td->urb->actual_length =
 				td->urb->transfer_buffer_length -
 				EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
-		else
+		else if (!td->urb_length_set)
 			td->urb->actual_length = 0;
 
 		return finish_td(xhci, td, event_trb, event, ep, status, false);
@@ -3781,8 +3781,11 @@ static int queue_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
 {
 	int reserved_trbs = xhci->cmd_ring_reserved_trbs;
 	int ret;
-	if (xhci->xhc_state & XHCI_STATE_DYING)
+
+	if (xhci->xhc_state) {
+		xhci_dbg(xhci, "xHCI dying or halted, can't queue_command\n");
 		return -ESHUTDOWN;
+	}
 
 	if (!command_must_succeed)
 		reserved_trbs++;
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index ec8ac1674854..7da0d6043d33 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -660,12 +660,6 @@ static void xhci_only_stop_hcd(struct usb_hcd *hcd)
 
 	spin_lock_irq(&xhci->lock);
 	xhci_halt(xhci);
-
-	/* The shared_hcd is going to be deallocated shortly (the USB core only
-	 * calls this function when allocation fails in usb_add_hcd(), or
-	 * usb_remove_hcd() is called).  So we need to unset xHCI's pointer.
-	 */
-	xhci->shared_hcd = NULL;
 	spin_unlock_irq(&xhci->lock);
 }
 
@@ -897,6 +891,9 @@ int xhci_suspend(struct xhci_hcd *xhci, bool do_wakeup)
 	struct usb_hcd		*hcd = xhci_to_hcd(xhci);
 	u32			command;
 
+	if (!hcd->state)
+		return 0;
+
 	if (hcd->state != HC_STATE_SUSPENDED ||
 			xhci->shared_hcd->state != HC_STATE_SUSPENDED)
 		return -EINVAL;
@@ -983,6 +980,9 @@ int xhci_resume(struct xhci_hcd *xhci, bool hibernated)
 	int			retval = 0;
 	bool			comp_timer_running = false;
 
+	if (!hcd->state)
+		return 0;
+
 	/* Wait a bit if either of the roothubs need to settle from the
 	 * transition into bus suspend.
 	 */
@@ -3682,18 +3682,21 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 {
 	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
 	unsigned long flags;
-	int ret;
+	int ret, slot_id;
 	struct xhci_command *command;
 
 	command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
 	if (!command)
 		return 0;
 
+	/* xhci->slot_id and xhci->addr_dev are not thread-safe */
+	mutex_lock(&xhci->mutex);
 	spin_lock_irqsave(&xhci->lock, flags);
 	command->completion = &xhci->addr_dev;
 	ret = xhci_queue_slot_control(xhci, command, TRB_ENABLE_SLOT, 0);
 	if (ret) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
+		mutex_unlock(&xhci->mutex);
 		xhci_dbg(xhci, "FIXME: allocate a command ring segment\n");
 		kfree(command);
 		return 0;
@@ -3702,8 +3705,10 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	wait_for_completion(command->completion);
+	slot_id = xhci->slot_id;
+	mutex_unlock(&xhci->mutex);
 
-	if (!xhci->slot_id || command->status != COMP_SUCCESS) {
+	if (!slot_id || command->status != COMP_SUCCESS) {
 		xhci_err(xhci, "Error while assigning device slot ID\n");
 		xhci_err(xhci, "Max number of devices this xHCI host supports is %u.\n",
 				HCS_MAX_SLOTS(
@@ -3728,11 +3733,11 @@ int xhci_alloc_dev(struct usb_hcd *hcd, struct usb_device *udev)
 	 * xhci_discover_or_reset_device(), which may be called as part of
 	 * mass storage driver error handling.
 	 */
-	if (!xhci_alloc_virt_device(xhci, xhci->slot_id, udev, GFP_NOIO)) {
+	if (!xhci_alloc_virt_device(xhci, slot_id, udev, GFP_NOIO)) {
 		xhci_warn(xhci, "Could not allocate xHCI USB device data structures\n");
 		goto disable_slot;
 	}
-	udev->slot_id = xhci->slot_id;
+	udev->slot_id = slot_id;
 
 #ifndef CONFIG_USB_DEFAULT_PERSIST
 	/*
@@ -3764,8 +3769,6 @@ disable_slot:
 /*
  * Issue an Address Device command and optionally send a corresponding
  * SetAddress request to the device.
- * We should be protected by the usb_address0_mutex in hub_wq's hub_port_init,
- * so we should only issue and wait on one address command at the same time.
  */
 static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 			     enum xhci_setup_dev setup)
@@ -3778,12 +3781,15 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 	struct xhci_slot_ctx *slot_ctx;
 	struct xhci_input_control_ctx *ctrl_ctx;
 	u64 temp_64;
-	struct xhci_command *command;
+	struct xhci_command *command = NULL;
+
+	mutex_lock(&xhci->mutex);
 
 	if (!udev->slot_id) {
 		xhci_dbg_trace(xhci, trace_xhci_dbg_address,
 				"Bad Slot ID %d", udev->slot_id);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out;
 	}
 
 	virt_dev = xhci->devs[udev->slot_id];
@@ -3796,7 +3802,8 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 		 */
 		xhci_warn(xhci, "Virt dev invalid for slot_id 0x%x!\n",
 			udev->slot_id);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out;
 	}
 
 	if (setup == SETUP_CONTEXT_ONLY) {
@@ -3804,13 +3811,15 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 		if (GET_SLOT_STATE(le32_to_cpu(slot_ctx->dev_state)) ==
 		    SLOT_STATE_DEFAULT) {
 			xhci_dbg(xhci, "Slot already in default state\n");
-			return 0;
+			goto out;
 		}
 	}
 
 	command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
-	if (!command)
-		return -ENOMEM;
+	if (!command) {
+		ret = -ENOMEM;
+		goto out;
+	}
 
 	command->in_ctx = virt_dev->in_ctx;
 	command->completion = &xhci->addr_dev;
@@ -3820,8 +3829,8 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 	if (!ctrl_ctx) {
 		xhci_warn(xhci, "%s: Could not get input context, bad type.\n",
 				__func__);
-		kfree(command);
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out;
 	}
 	/*
 	 * If this is the first Set Address since device plug-in or
@@ -3848,8 +3857,7 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		xhci_dbg_trace(xhci, trace_xhci_dbg_address,
 				"FIXME: allocate a command ring segment");
-		kfree(command);
-		return ret;
+		goto out;
 	}
 	xhci_ring_cmd_db(xhci);
 	spin_unlock_irqrestore(&xhci->lock, flags);
@@ -3896,10 +3904,8 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 		ret = -EINVAL;
 		break;
 	}
-	if (ret) {
-		kfree(command);
-		return ret;
-	}
+	if (ret)
+		goto out;
 	temp_64 = xhci_read_64(xhci, &xhci->op_regs->dcbaa_ptr);
 	xhci_dbg_trace(xhci, trace_xhci_dbg_address,
 			"Op regs DCBAA ptr = %#016llx", temp_64);
@@ -3932,8 +3938,10 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 	xhci_dbg_trace(xhci, trace_xhci_dbg_address,
 		       "Internal device address = %d",
 		       le32_to_cpu(slot_ctx->dev_state) & DEV_ADDR_MASK);
+out:
+	mutex_unlock(&xhci->mutex);
 	kfree(command);
-	return 0;
+	return ret;
 }
 
 int xhci_address_device(struct usb_hcd *hcd, struct usb_device *udev)
@@ -4832,10 +4840,7 @@ int xhci_gen_setup(struct usb_hcd *hcd, xhci_get_quirks_t get_quirks)
 	hcd->self.no_stop_on_short = 1;
 
 	if (usb_hcd_is_primary_hcd(hcd)) {
-		xhci = kzalloc(sizeof(struct xhci_hcd), GFP_KERNEL);
-		if (!xhci)
-			return -ENOMEM;
-		*((struct xhci_hcd **) hcd->hcd_priv) = xhci;
+		xhci = hcd_to_xhci(hcd);
 		xhci->main_hcd = hcd;
 		/* Mark the first roothub as being USB 2.0.
 		 * The xHCI driver will register the USB 3.0 roothub.
@@ -4855,6 +4860,7 @@ int xhci_gen_setup(struct usb_hcd *hcd, xhci_get_quirks_t get_quirks)
 		return 0;
 	}
 
+	mutex_init(&xhci->mutex);
 	xhci->cap_regs = hcd->regs;
 	xhci->op_regs = hcd->regs +
 		HC_LENGTH(readl(&xhci->cap_regs->hc_capbase));
@@ -4883,13 +4889,13 @@ int xhci_gen_setup(struct usb_hcd *hcd, xhci_get_quirks_t get_quirks)
 	/* Make sure the HC is halted. */
 	retval = xhci_halt(xhci);
 	if (retval)
-		goto error;
+		return retval;
 
 	xhci_dbg(xhci, "Resetting HCD\n");
 	/* Reset the internal HC memory state and registers. */
 	retval = xhci_reset(xhci);
 	if (retval)
-		goto error;
+		return retval;
 	xhci_dbg(xhci, "Reset complete\n");
 
 	/* Set dma_mask and coherent_dma_mask to 64-bits,
@@ -4904,16 +4910,13 @@ int xhci_gen_setup(struct usb_hcd *hcd, xhci_get_quirks_t get_quirks)
 	/* Initialize HCD and host controller data structures. */
 	retval = xhci_init(hcd);
 	if (retval)
-		goto error;
+		return retval;
 	xhci_dbg(xhci, "Called HCD init\n");
 
 	xhci_info(xhci, "hcc params 0x%08x hci version 0x%x quirks 0x%08x\n",
 		  xhci->hcc_params, xhci->hci_version, xhci->quirks);
 
 	return 0;
-error:
-	kfree(xhci);
-	return retval;
 }
 EXPORT_SYMBOL_GPL(xhci_gen_setup);
 
@@ -4978,11 +4981,21 @@ static const struct hc_driver xhci_hc_driver = {
 	.find_raw_port_number =	xhci_find_raw_port_number,
 };
 
-void xhci_init_driver(struct hc_driver *drv, int (*setup_fn)(struct usb_hcd *))
+void xhci_init_driver(struct hc_driver *drv,
+		      const struct xhci_driver_overrides *over)
 {
-	BUG_ON(!setup_fn);
+	BUG_ON(!over);
+
+	/* Copy the generic table to drv then apply the overrides */
 	*drv = xhci_hc_driver;
-	drv->reset = setup_fn;
+
+	if (over) {
+		drv->hcd_priv_size += over->extra_priv_size;
+		if (over->reset)
+			drv->reset = over->reset;
+		if (over->start)
+			drv->start = over->start;
+	}
 }
 EXPORT_SYMBOL_GPL(xhci_init_driver);
 
@@ -5011,4 +5024,12 @@ static int __init xhci_hcd_init(void)
 	BUILD_BUG_ON(sizeof(struct xhci_run_regs) != (8+8*128)*32/8);
 	return 0;
 }
+
+/*
+ * If an init function is provided, an exit function must also be provided
+ * to allow module unload.
+ */
+static void __exit xhci_hcd_fini(void) { }
+
 module_init(xhci_hcd_init);
+module_exit(xhci_hcd_fini);
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index ea75e8ccd3c1..31e46cc55807 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -1497,6 +1497,8 @@ struct xhci_hcd {
 	struct list_head	lpm_failed_devs;
 
 	/* slot enabling and address device helpers */
+	/* these are not thread safe so use mutex */
+	struct mutex mutex;
 	struct completion	addr_dev;
 	int slot_id;
 	/* For USB 3.0 LPM enable/disable. */
@@ -1591,10 +1593,24 @@ struct xhci_hcd {
 #define COMP_MODE_RCVRY_MSECS 2000
 };
 
+/* Platform specific overrides to generic XHCI hc_driver ops */
+struct xhci_driver_overrides {
+	size_t extra_priv_size;
+	int (*reset)(struct usb_hcd *hcd);
+	int (*start)(struct usb_hcd *hcd);
+};
+
 /* convert between an HCD pointer and the corresponding EHCI_HCD */
 static inline struct xhci_hcd *hcd_to_xhci(struct usb_hcd *hcd)
 {
-	return *((struct xhci_hcd **) (hcd->hcd_priv));
+	struct usb_hcd *primary_hcd;
+
+	if (usb_hcd_is_primary_hcd(hcd))
+		primary_hcd = hcd;
+	else
+		primary_hcd = hcd->primary_hcd;
+
+	return (struct xhci_hcd *) (primary_hcd->hcd_priv);
 }
 
 static inline struct usb_hcd *xhci_to_hcd(struct xhci_hcd *xhci)
@@ -1748,7 +1764,8 @@ int xhci_run(struct usb_hcd *hcd);
 void xhci_stop(struct usb_hcd *hcd);
 void xhci_shutdown(struct usb_hcd *hcd);
 int xhci_gen_setup(struct usb_hcd *hcd, xhci_get_quirks_t get_quirks);
-void xhci_init_driver(struct hc_driver *drv, int (*setup_fn)(struct usb_hcd *));
+void xhci_init_driver(struct hc_driver *drv,
+		      const struct xhci_driver_overrides *over);
 
 #ifdef	CONFIG_PM
 int xhci_suspend(struct xhci_hcd *xhci, bool do_wakeup);
diff --git a/drivers/usb/image/microtek.c b/drivers/usb/image/microtek.c
index 6431d08c8d9d..a4dbb0cd80da 100644
--- a/drivers/usb/image/microtek.c
+++ b/drivers/usb/image/microtek.c
@@ -635,7 +635,6 @@ static struct scsi_host_template mts_scsi_host_template = {
 	.sg_tablesize =		SG_ALL,
 	.can_queue =		1,
 	.this_id =		-1,
-	.cmd_per_lun =		1,
 	.use_clustering =	1,
 	.emulated =		1,
 	.slave_alloc =		mts_slave_alloc,
diff --git a/drivers/usb/isp1760/isp1760-udc.c b/drivers/usb/isp1760/isp1760-udc.c
index 3fc4fe770253..18ebf5b1f256 100644
--- a/drivers/usb/isp1760/isp1760-udc.c
+++ b/drivers/usb/isp1760/isp1760-udc.c
@@ -812,6 +812,8 @@ static struct usb_request *isp1760_ep_alloc_request(struct usb_ep *ep,
 	struct isp1760_request *req;
 
 	req = kzalloc(sizeof(*req), gfp_flags);
+	if (!req)
+		return NULL;
 
 	return &req->req;
 }
diff --git a/drivers/usb/misc/ldusb.c b/drivers/usb/misc/ldusb.c
index 82503a7ff6c8..cce22ff1c2eb 100644
--- a/drivers/usb/misc/ldusb.c
+++ b/drivers/usb/misc/ldusb.c
@@ -69,12 +69,6 @@
 #define USB_DEVICE_ID_LD_HYBRID		0x2090	/* USB Product ID of Automotive Hybrid */
 #define USB_DEVICE_ID_LD_HEATCONTROL	0x20A0	/* USB Product ID of Heat control */
 
-#define USB_VENDOR_ID_VERNIER		0x08f7
-#define USB_DEVICE_ID_VERNIER_GOTEMP	0x0002
-#define USB_DEVICE_ID_VERNIER_SKIP	0x0003
-#define USB_DEVICE_ID_VERNIER_CYCLOPS	0x0004
-#define USB_DEVICE_ID_VERNIER_LCSPEC	0x0006
-
 #ifdef CONFIG_USB_DYNAMIC_MINORS
 #define USB_LD_MINOR_BASE	0
 #else
@@ -115,10 +109,6 @@ static const struct usb_device_id ld_usb_table[] = {
 	{ USB_DEVICE(USB_VENDOR_ID_LD, USB_DEVICE_ID_LD_MCT) },
 	{ USB_DEVICE(USB_VENDOR_ID_LD, USB_DEVICE_ID_LD_HYBRID) },
 	{ USB_DEVICE(USB_VENDOR_ID_LD, USB_DEVICE_ID_LD_HEATCONTROL) },
-	{ USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_GOTEMP) },
-	{ USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_SKIP) },
-	{ USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_CYCLOPS) },
-	{ USB_DEVICE(USB_VENDOR_ID_VERNIER, USB_DEVICE_ID_VERNIER_LCSPEC) },
 	{ }					/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, ld_usb_table);
diff --git a/drivers/usb/misc/lvstest.c b/drivers/usb/misc/lvstest.c
index 62cb8cd08403..86b4e4b2ab9a 100644
--- a/drivers/usb/misc/lvstest.c
+++ b/drivers/usb/misc/lvstest.c
@@ -4,7 +4,7 @@
  * Test pattern generation for Link Layer Validation System Tests
  *
  * Copyright (C) 2014 ST Microelectronics
- * Pratyush Anand <pratyush.anand@st.com>
+ * Pratyush Anand <pratyush.anand@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
diff --git a/drivers/usb/misc/sisusbvga/sisusb.c b/drivers/usb/misc/sisusbvga/sisusb.c
index 022dc0008f2a..306d6852ebc7 100644
--- a/drivers/usb/misc/sisusbvga/sisusb.c
+++ b/drivers/usb/misc/sisusbvga/sisusb.c
@@ -2316,10 +2316,12 @@ sisusb_reset_text_mode(struct sisusb_usb_data *sisusb, int init)
 	/* Set mode 0x03 */
 	SiSUSBSetMode(sisusb->SiS_Pr, 0x03);
 
-	if (!(myfont = find_font("VGA8x16")))
+	myfont = find_font("VGA8x16");
+	if (!myfont)
 		return 1;
 
-	if (!(tempbuf = vmalloc(8192)))
+	tempbuf = vmalloc(8192);
+	if (!tempbuf)
 		return 1;
 
 	for (i = 0; i < 256; i++)
@@ -2342,7 +2344,8 @@ sisusb_reset_text_mode(struct sisusb_usb_data *sisusb, int init)
 
 	if (init && !sisusb->scrbuf) {
 
-		if ((tempbuf = vmalloc(8192))) {
+		tempbuf = vmalloc(8192);
+		if (tempbuf) {
 
 			i = 4096;
 			tempbufb = (u16 *)tempbuf;
@@ -2417,11 +2420,13 @@ sisusb_open(struct inode *inode, struct file *file)
 	struct usb_interface *interface;
 	int subminor = iminor(inode);
 
-	if (!(interface = usb_find_interface(&sisusb_driver, subminor))) {
+	interface = usb_find_interface(&sisusb_driver, subminor);
+	if (!interface) {
 		return -ENODEV;
 	}
 
-	if (!(sisusb = usb_get_intfdata(interface))) {
+	sisusb = usb_get_intfdata(interface);
+	if (!sisusb) {
 		return -ENODEV;
 	}
 
@@ -2488,7 +2493,8 @@ sisusb_release(struct inode *inode, struct file *file)
 {
 	struct sisusb_usb_data *sisusb;
 
-	if (!(sisusb = file->private_data))
+	sisusb = file->private_data;
+	if (!sisusb)
 		return -ENODEV;
 
 	mutex_lock(&sisusb->lock);
@@ -2520,7 +2526,8 @@ sisusb_read(struct file *file, char __user *buffer, size_t count, loff_t *ppos)
 	u16 buf16;
 	u32 buf32, address;
 
-	if (!(sisusb = file->private_data))
+	sisusb = file->private_data;
+	if (!sisusb)
 		return -ENODEV;
 
 	mutex_lock(&sisusb->lock);
@@ -2662,7 +2669,8 @@ sisusb_write(struct file *file, const char __user *buffer, size_t count,
 	u16 buf16;
 	u32 buf32, address;
 
-	if (!(sisusb = file->private_data))
+	sisusb = file->private_data;
+	if (!sisusb)
 		return -ENODEV;
 
 	mutex_lock(&sisusb->lock);
@@ -2805,7 +2813,8 @@ sisusb_lseek(struct file *file, loff_t offset, int orig)
 	struct sisusb_usb_data *sisusb;
 	loff_t ret;
 
-	if (!(sisusb = file->private_data))
+	sisusb = file->private_data;
+	if (!sisusb)
 		return -ENODEV;
 
 	mutex_lock(&sisusb->lock);
@@ -2970,7 +2979,8 @@ sisusb_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	long retval = 0;
 	u32 __user *argp = (u32 __user *)arg;
 
-	if (!(sisusb = file->private_data))
+	sisusb = file->private_data;
+	if (!sisusb)
 		return -ENODEV;
 
 	mutex_lock(&sisusb->lock);
@@ -3084,7 +3094,8 @@ static int sisusb_probe(struct usb_interface *intf,
 		dev->devnum);
 
 	/* Allocate memory for our private */
-	if (!(sisusb = kzalloc(sizeof(*sisusb), GFP_KERNEL))) {
+	sisusb = kzalloc(sizeof(*sisusb), GFP_KERNEL);
+	if (!sisusb) {
 		dev_err(&dev->dev, "Failed to allocate memory for private data\n");
 		return -ENOMEM;
 	}
@@ -3093,7 +3104,8 @@ static int sisusb_probe(struct usb_interface *intf,
 	mutex_init(&(sisusb->lock));
 
 	/* Register device */
-	if ((retval = usb_register_dev(intf, &usb_sisusb_class))) {
+	retval = usb_register_dev(intf, &usb_sisusb_class);
+	if (retval) {
 		dev_err(&sisusb->sisusb_dev->dev, "Failed to get a minor for device %d\n",
 			dev->devnum);
 		retval = -ENODEV;
@@ -3214,7 +3226,8 @@ static void sisusb_disconnect(struct usb_interface *intf)
 	struct sisusb_usb_data *sisusb;
 
 	/* This should *not* happen */
-	if (!(sisusb = usb_get_intfdata(intf)))
+	sisusb = usb_get_intfdata(intf);
+	if (!sisusb)
 		return;
 
 #ifdef INCL_SISUSB_CON
diff --git a/drivers/usb/misc/sisusbvga/sisusb_con.c b/drivers/usb/misc/sisusbvga/sisusb_con.c
index a638c4e9a947..ace343088915 100644
--- a/drivers/usb/misc/sisusbvga/sisusb_con.c
+++ b/drivers/usb/misc/sisusbvga/sisusb_con.c
@@ -169,7 +169,8 @@ sisusb_get_sisusb_lock_and_check(unsigned short console)
 	if (in_atomic())
 		return NULL;
 
-	if (!(sisusb = sisusb_get_sisusb(console)))
+	sisusb = sisusb_get_sisusb(console);
+	if (!sisusb)
 		return NULL;
 
 	mutex_lock(&sisusb->lock);
@@ -214,7 +215,8 @@ sisusbcon_init(struct vc_data *c, int init)
 	 * are set up/restored.
 	 */
 
-	if (!(sisusb = sisusb_get_sisusb(c->vc_num)))
+	sisusb = sisusb_get_sisusb(c->vc_num);
+	if (!sisusb)
 		return;
 
 	mutex_lock(&sisusb->lock);
@@ -277,7 +279,8 @@ sisusbcon_deinit(struct vc_data *c)
 	 * and others, ie not under our control.
 	 */
 
-	if (!(sisusb = sisusb_get_sisusb(c->vc_num)))
+	sisusb = sisusb_get_sisusb(c->vc_num);
+	if (!sisusb)
 		return;
 
 	mutex_lock(&sisusb->lock);
@@ -369,7 +372,8 @@ sisusbcon_putc(struct vc_data *c, int ch, int y, int x)
 	struct sisusb_usb_data *sisusb;
 	ssize_t written;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return;
 
 	/* sisusb->lock is down */
@@ -395,7 +399,8 @@ sisusbcon_putcs(struct vc_data *c, const unsigned short *s,
 	u16 *dest;
 	int i;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return;
 
 	/* sisusb->lock is down */
@@ -433,7 +438,8 @@ sisusbcon_clear(struct vc_data *c, int y, int x, int height, int width)
 	if (width <= 0 || height <= 0)
 		return;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return;
 
 	/* sisusb->lock is down */
@@ -486,7 +492,8 @@ sisusbcon_bmove(struct vc_data *c, int sy, int sx,
 	if (width <= 0 || height <= 0)
 		return;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return;
 
 	/* sisusb->lock is down */
@@ -520,7 +527,8 @@ sisusbcon_switch(struct vc_data *c)
 	 * Returnvalue != 0 naturally means the opposite.
 	 */
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return 0;
 
 	/* sisusb->lock is down */
@@ -569,7 +577,8 @@ sisusbcon_save_screen(struct vc_data *c)
 	 * buffer.
 	 */
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return;
 
 	/* sisusb->lock is down */
@@ -602,7 +611,8 @@ sisusbcon_set_palette(struct vc_data *c, unsigned char *table)
 	if (!CON_IS_VISIBLE(c))
 		return -EINVAL;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return -EINVAL;
 
 	/* sisusb->lock is down */
@@ -637,7 +647,8 @@ sisusbcon_blank(struct vc_data *c, int blank, int mode_switch)
 	ssize_t written;
 	int ret = 0;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return 0;
 
 	/* sisusb->lock is down */
@@ -721,7 +732,8 @@ sisusbcon_scrolldelta(struct vc_data *c, int lines)
 
 	/* The return value does not seem to be used */
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return 0;
 
 	/* sisusb->lock is down */
@@ -779,7 +791,8 @@ sisusbcon_cursor(struct vc_data *c, int mode)
 	struct sisusb_usb_data *sisusb;
 	int from, to, baseline;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return;
 
 	/* sisusb->lock is down */
@@ -906,7 +919,8 @@ sisusbcon_scroll(struct vc_data *c, int t, int b, int dir, int lines)
 	if (!lines)
 		return 1;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return 0;
 
 	/* sisusb->lock is down */
@@ -1018,7 +1032,8 @@ sisusbcon_set_origin(struct vc_data *c)
 	 *	screenbuffer as the origin.
 	 */
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return 0;
 
 	/* sisusb->lock is down */
@@ -1047,7 +1062,8 @@ sisusbcon_resize(struct vc_data *c, unsigned int newcols, unsigned int newrows,
 	struct sisusb_usb_data *sisusb;
 	int fh;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return -ENODEV;
 
 	fh = sisusb->current_font_height;
@@ -1286,7 +1302,8 @@ sisusbcon_font_set(struct vc_data *c, struct console_font *font,
 	if (font->width != 8 || (charcount != 256 && charcount != 512))
 		return -EINVAL;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return -ENODEV;
 
 	/* sisusb->lock is down */
@@ -1326,7 +1343,8 @@ sisusbcon_font_get(struct vc_data *c, struct console_font *font)
 {
 	struct sisusb_usb_data *sisusb;
 
-	if (!(sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num)))
+	sisusb = sisusb_get_sisusb_lock_and_check(c->vc_num);
+	if (!sisusb)
 		return -ENODEV;
 
 	/* sisusb->lock is down */
diff --git a/drivers/usb/misc/uss720.c b/drivers/usb/misc/uss720.c
index 588d62a73e1a..bbd029c9c725 100644
--- a/drivers/usb/misc/uss720.c
+++ b/drivers/usb/misc/uss720.c
@@ -714,7 +714,8 @@ static int uss720_probe(struct usb_interface *intf,
 	/*
 	 * Allocate parport interface 
 	 */
-	if (!(priv = kzalloc(sizeof(struct parport_uss720_private), GFP_KERNEL))) {
+	priv = kzalloc(sizeof(struct parport_uss720_private), GFP_KERNEL);
+	if (!priv) {
 		usb_put_dev(usbdev);
 		return -ENOMEM;
 	}
@@ -723,7 +724,8 @@ static int uss720_probe(struct usb_interface *intf,
 	kref_init(&priv->ref_count);
 	spin_lock_init(&priv->asynclock);
 	INIT_LIST_HEAD(&priv->asynclist);
-	if (!(pp = parport_register_port(0, PARPORT_IRQ_NONE, PARPORT_DMA_NONE, &parport_uss720_ops))) {
+	pp = parport_register_port(0, PARPORT_IRQ_NONE, PARPORT_DMA_NONE, &parport_uss720_ops);
+	if (!pp) {
 		printk(KERN_WARNING "uss720: could not register parport\n");
 		goto probe_abort;
 	}
diff --git a/drivers/usb/mon/mon_bin.c b/drivers/usb/mon/mon_bin.c
index 9a62e89d6dc0..3598f1a62673 100644
--- a/drivers/usb/mon/mon_bin.c
+++ b/drivers/usb/mon/mon_bin.c
@@ -675,7 +675,8 @@ static int mon_bin_open(struct inode *inode, struct file *file)
 	int rc;
 
 	mutex_lock(&mon_lock);
-	if ((mbus = mon_bus_lookup(iminor(inode))) == NULL) {
+	mbus = mon_bus_lookup(iminor(inode));
+	if (mbus == NULL) {
 		mutex_unlock(&mon_lock);
 		return -ENODEV;
 	}
@@ -1018,8 +1019,8 @@ static long mon_bin_ioctl(struct file *file, unsigned int cmd, unsigned long arg
 			return -EINVAL;
 
 		size = CHUNK_ALIGN(arg);
-		if ((vec = kzalloc(sizeof(struct mon_pgmap) * (size/CHUNK_SIZE),
-		    GFP_KERNEL)) == NULL) {
+		vec = kzalloc(sizeof(struct mon_pgmap) * (size / CHUNK_SIZE), GFP_KERNEL);
+		if (vec == NULL) {
 			ret = -ENOMEM;
 			break;
 		}
diff --git a/drivers/usb/mon/mon_main.c b/drivers/usb/mon/mon_main.c
index 10405119985c..f7c292f4891e 100644
--- a/drivers/usb/mon/mon_main.c
+++ b/drivers/usb/mon/mon_main.c
@@ -96,7 +96,8 @@ static void mon_submit(struct usb_bus *ubus, struct urb *urb)
 {
 	struct mon_bus *mbus;
 
-	if ((mbus = ubus->mon_bus) != NULL)
+	mbus = ubus->mon_bus;
+	if (mbus != NULL)
 		mon_bus_submit(mbus, urb);
 	mon_bus_submit(&mon_bus0, urb);
 }
@@ -122,7 +123,8 @@ static void mon_submit_error(struct usb_bus *ubus, struct urb *urb, int error)
 {
 	struct mon_bus *mbus;
 
-	if ((mbus = ubus->mon_bus) != NULL)
+	mbus = ubus->mon_bus;
+	if (mbus != NULL)
 		mon_bus_submit_error(mbus, urb, error);
 	mon_bus_submit_error(&mon_bus0, urb, error);
 }
@@ -148,7 +150,8 @@ static void mon_complete(struct usb_bus *ubus, struct urb *urb, int status)
 {
 	struct mon_bus *mbus;
 
-	if ((mbus = ubus->mon_bus) != NULL)
+	mbus = ubus->mon_bus;
+	if (mbus != NULL)
 		mon_bus_complete(mbus, urb, status);
 	mon_bus_complete(&mon_bus0, urb, status);
 }
@@ -280,7 +283,8 @@ static void mon_bus_init(struct usb_bus *ubus)
 {
 	struct mon_bus *mbus;
 
-	if ((mbus = kzalloc(sizeof(struct mon_bus), GFP_KERNEL)) == NULL)
+	mbus = kzalloc(sizeof(struct mon_bus), GFP_KERNEL);
+	if (mbus == NULL)
 		goto err_alloc;
 	kref_init(&mbus->ref);
 	spin_lock_init(&mbus->lock);
diff --git a/drivers/usb/mon/mon_stat.c b/drivers/usb/mon/mon_stat.c
index ebd6189a5014..5388a339cfb8 100644
--- a/drivers/usb/mon/mon_stat.c
+++ b/drivers/usb/mon/mon_stat.c
@@ -28,7 +28,8 @@ static int mon_stat_open(struct inode *inode, struct file *file)
 	struct mon_bus *mbus;
 	struct snap *sp;
 
-	if ((sp = kmalloc(sizeof(struct snap), GFP_KERNEL)) == NULL)
+	sp = kmalloc(sizeof(struct snap), GFP_KERNEL);
+	if (sp == NULL)
 		return -ENOMEM;
 
 	mbus = inode->i_private;
diff --git a/drivers/usb/musb/am35x.c b/drivers/usb/musb/am35x.c
index 220fd4d3b41c..c41fe588d14d 100644
--- a/drivers/usb/musb/am35x.c
+++ b/drivers/usb/musb/am35x.c
@@ -438,11 +438,15 @@ static void am35x_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 }
 
 static const struct musb_platform_ops am35x_ops = {
-	.quirks		= MUSB_INDEXED_EP,
+	.quirks		= MUSB_DMA_INVENTRA | MUSB_INDEXED_EP,
 	.init		= am35x_musb_init,
 	.exit		= am35x_musb_exit,
 
 	.read_fifo	= am35x_read_fifo,
+#ifdef CONFIG_USB_INVENTRA_DMA
+	.dma_init	= musbhs_dma_controller_create,
+	.dma_exit	= musbhs_dma_controller_destroy,
+#endif
 	.enable		= am35x_musb_enable,
 	.disable	= am35x_musb_disable,
 
@@ -565,7 +569,7 @@ static int am35x_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int am35x_suspend(struct device *dev)
 {
 	struct am35x_glue	*glue = dev_get_drvdata(dev);
diff --git a/drivers/usb/musb/blackfin.c b/drivers/usb/musb/blackfin.c
index 6123b748d262..310238c6b5cd 100644
--- a/drivers/usb/musb/blackfin.c
+++ b/drivers/usb/musb/blackfin.c
@@ -465,6 +465,7 @@ static int bfin_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops bfin_ops = {
+	.quirks		= MUSB_DMA_INVENTRA,
 	.init		= bfin_musb_init,
 	.exit		= bfin_musb_exit,
 
@@ -477,6 +478,10 @@ static const struct musb_platform_ops bfin_ops = {
 	.fifo_mode	= 2,
 	.read_fifo	= bfin_read_fifo,
 	.write_fifo	= bfin_write_fifo,
+#ifdef CONFIG_USB_INVENTRA_DMA
+	.dma_init	= musbhs_dma_controller_create,
+	.dma_exit	= musbhs_dma_controller_destroy,
+#endif
 	.enable		= bfin_musb_enable,
 	.disable	= bfin_musb_disable,
 
diff --git a/drivers/usb/musb/cppi_dma.c b/drivers/usb/musb/cppi_dma.c
index 904fb85d85a6..cc134109b056 100644
--- a/drivers/usb/musb/cppi_dma.c
+++ b/drivers/usb/musb/cppi_dma.c
@@ -1297,7 +1297,8 @@ irqreturn_t cppi_interrupt(int irq, void *dev_id)
 EXPORT_SYMBOL_GPL(cppi_interrupt);
 
 /* Instantiate a software object representing a DMA controller. */
-struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *mregs)
+struct dma_controller *
+cppi_dma_controller_create(struct musb *musb, void __iomem *mregs)
 {
 	struct cppi		*controller;
 	struct device		*dev = musb->controller;
@@ -1334,7 +1335,7 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *mr
 	if (irq > 0) {
 		if (request_irq(irq, cppi_interrupt, 0, "cppi-dma", musb)) {
 			dev_err(dev, "request_irq %d failed!\n", irq);
-			dma_controller_destroy(&controller->controller);
+			musb_dma_controller_destroy(&controller->controller);
 			return NULL;
 		}
 		controller->irq = irq;
@@ -1343,11 +1344,12 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *mr
 	cppi_controller_start(controller);
 	return &controller->controller;
 }
+EXPORT_SYMBOL_GPL(cppi_dma_controller_create);
 
 /*
  *  Destroy a previously-instantiated DMA controller.
  */
-void dma_controller_destroy(struct dma_controller *c)
+void cppi_dma_controller_destroy(struct dma_controller *c)
 {
 	struct cppi	*cppi;
 
@@ -1363,6 +1365,7 @@ void dma_controller_destroy(struct dma_controller *c)
 
 	kfree(cppi);
 }
+EXPORT_SYMBOL_GPL(cppi_dma_controller_destroy);
 
 /*
  * Context: controller irqlocked, endpoint selected
diff --git a/drivers/usb/musb/da8xx.c b/drivers/usb/musb/da8xx.c
index 9a9c82a4d35d..b03d3b867fca 100644
--- a/drivers/usb/musb/da8xx.c
+++ b/drivers/usb/musb/da8xx.c
@@ -458,11 +458,15 @@ static int da8xx_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops da8xx_ops = {
-	.quirks		= MUSB_INDEXED_EP,
+	.quirks		= MUSB_DMA_CPPI | MUSB_INDEXED_EP,
 	.init		= da8xx_musb_init,
 	.exit		= da8xx_musb_exit,
 
 	.fifo_mode	= 2,
+#ifdef CONFIG_USB_TI_CPPI_DMA
+	.dma_init	= cppi_dma_controller_create,
+	.dma_exit	= cppi_dma_controller_destroy,
+#endif
 	.enable		= da8xx_musb_enable,
 	.disable	= da8xx_musb_disable,
 
diff --git a/drivers/usb/musb/davinci.c b/drivers/usb/musb/davinci.c
index 3c1d9b211b51..cee61a51645e 100644
--- a/drivers/usb/musb/davinci.c
+++ b/drivers/usb/musb/davinci.c
@@ -284,7 +284,7 @@ static irqreturn_t davinci_musb_interrupt(int irq, void *__hci)
 	 * mask, state, "vector", and EOI registers.
 	 */
 	cppi = container_of(musb->dma_controller, struct cppi, controller);
-	if (is_cppi_enabled() && musb->dma_controller && !cppi->irq)
+	if (is_cppi_enabled(musb) && musb->dma_controller && !cppi->irq)
 		retval = cppi_interrupt(irq, __hci);
 
 	/* ack and handle non-CPPI interrupts */
@@ -491,9 +491,14 @@ static int davinci_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops davinci_ops = {
+	.quirks		= MUSB_DMA_CPPI,
 	.init		= davinci_musb_init,
 	.exit		= davinci_musb_exit,
 
+#ifdef CONFIG_USB_TI_CPPI_DMA
+	.dma_init	= cppi_dma_controller_create,
+	.dma_exit	= cppi_dma_controller_destroy,
+#endif
 	.enable		= davinci_musb_enable,
 	.disable	= davinci_musb_disable,
 
diff --git a/drivers/usb/musb/jz4740.c b/drivers/usb/musb/jz4740.c
index bb7b26325a74..5e5a8fa005f8 100644
--- a/drivers/usb/musb/jz4740.c
+++ b/drivers/usb/musb/jz4740.c
@@ -105,8 +105,12 @@ static int jz4740_musb_exit(struct musb *musb)
 	return 0;
 }
 
+/*
+ * DMA has not been confirmed to work with CONFIG_USB_INVENTRA_DMA,
+ * so let's not set up the dma function pointers yet.
+ */
 static const struct musb_platform_ops jz4740_musb_ops = {
-	.quirks		= MUSB_INDEXED_EP,
+	.quirks		= MUSB_DMA_INVENTRA | 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 3789b08ef67b..514a6cdaeff6 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -251,6 +251,11 @@ static u32 musb_indexed_ep_offset(u8 epnum, u16 offset)
 	return 0x10 + offset;
 }
 
+static u32 musb_default_busctl_offset(u8 epnum, u16 offset)
+{
+	return 0x80 + (0x08 * epnum) + offset;
+}
+
 static u8 musb_default_readb(const void __iomem *addr, unsigned offset)
 {
 	return __raw_readb(addr + offset);
@@ -309,7 +314,7 @@ static void musb_default_write_fifo(struct musb_hw_ep *hw_ep, u16 len,
 				index += len & ~0x03;
 			}
 			if (len & 0x02) {
-				musb_writew(fifo, 0, *(u16 *)&src[index]);
+				__raw_writew(*(u16 *)&src[index], fifo);
 				index += 2;
 			}
 		} else {
@@ -319,7 +324,7 @@ static void musb_default_write_fifo(struct musb_hw_ep *hw_ep, u16 len,
 			}
 		}
 		if (len & 0x01)
-			musb_writeb(fifo, 0, src[index]);
+			__raw_writeb(src[index], fifo);
 	} else  {
 		/* byte aligned */
 		iowrite8_rep(fifo, src, len);
@@ -351,7 +356,7 @@ static void musb_default_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 				index = len & ~0x03;
 			}
 			if (len & 0x02) {
-				*(u16 *)&dst[index] = musb_readw(fifo, 0);
+				*(u16 *)&dst[index] = __raw_readw(fifo);
 				index += 2;
 			}
 		} else {
@@ -361,7 +366,7 @@ static void musb_default_read_fifo(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 			}
 		}
 		if (len & 0x01)
-			dst[index] = musb_readb(fifo, 0);
+			dst[index] = __raw_readb(fifo);
 	} else  {
 		/* byte aligned */
 		ioread8_rep(fifo, dst, len);
@@ -389,6 +394,15 @@ EXPORT_SYMBOL_GPL(musb_readl);
 void (*musb_writel)(void __iomem *addr, unsigned offset, u32 data);
 EXPORT_SYMBOL_GPL(musb_writel);
 
+#ifndef CONFIG_MUSB_PIO_ONLY
+struct dma_controller *
+(*musb_dma_controller_create)(struct musb *musb, void __iomem *base);
+EXPORT_SYMBOL(musb_dma_controller_create);
+
+void (*musb_dma_controller_destroy)(struct dma_controller *c);
+EXPORT_SYMBOL(musb_dma_controller_destroy);
+#endif
+
 /*
  * New style IO functions
  */
@@ -1535,7 +1549,6 @@ static int musb_core_init(u16 musb_type, struct musb *musb)
 #endif
 
 		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;
 
@@ -1658,15 +1671,13 @@ void musb_dma_completion(struct musb *musb, u8 epnum, u8 transmit)
 	/* called with controller lock already held */
 
 	if (!epnum) {
-#ifndef CONFIG_USB_TUSB_OMAP_DMA
-		if (!is_cppi_enabled()) {
+		if (!is_cppi_enabled(musb)) {
 			/* endpoint 0 */
 			if (is_host_active(musb))
 				musb_h_ep0_irq(musb);
 			else
 				musb_g_ep0_irq(musb);
 		}
-#endif
 	} else {
 		/* endpoints 1..15 */
 		if (transmit) {
@@ -2021,13 +2032,7 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	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 */
+	/* Most 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;
@@ -2035,6 +2040,17 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 		musb->io.ep_offset = musb_flat_ep_offset;
 		musb->io.ep_select = musb_flat_ep_select;
 	}
+	/* And override them with platform specific ops if specified. */
+	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;
+
+	/* At least tusb6010 has its 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;
 
 	if (musb->ops->fifo_mode)
 		fifo_mode = musb->ops->fifo_mode;
@@ -2046,6 +2062,11 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	else
 		musb->io.fifo_offset = musb_default_fifo_offset;
 
+	if (musb->ops->busctl_offset)
+		musb->io.busctl_offset = musb->ops->busctl_offset;
+	else
+		musb->io.busctl_offset = musb_default_busctl_offset;
+
 	if (musb->ops->readb)
 		musb_readb = musb->ops->readb;
 	if (musb->ops->writeb)
@@ -2059,6 +2080,15 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	if (musb->ops->writel)
 		musb_writel = musb->ops->writel;
 
+#ifndef CONFIG_MUSB_PIO_ONLY
+	if (!musb->ops->dma_init || !musb->ops->dma_exit) {
+		dev_err(dev, "DMA controller not set\n");
+		goto fail2;
+	}
+	musb_dma_controller_create = musb->ops->dma_init;
+	musb_dma_controller_destroy = musb->ops->dma_exit;
+#endif
+
 	if (musb->ops->read_fifo)
 		musb->io.read_fifo = musb->ops->read_fifo;
 	else
@@ -2078,7 +2108,8 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	pm_runtime_get_sync(musb->controller);
 
 	if (use_dma && dev->dma_mask) {
-		musb->dma_controller = dma_controller_create(musb, musb->mregs);
+		musb->dma_controller =
+			musb_dma_controller_create(musb, musb->mregs);
 		if (IS_ERR(musb->dma_controller)) {
 			status = PTR_ERR(musb->dma_controller);
 			goto fail2_5;
@@ -2189,7 +2220,7 @@ fail3:
 	cancel_delayed_work_sync(&musb->finish_resume_work);
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
 	if (musb->dma_controller)
-		dma_controller_destroy(musb->dma_controller);
+		musb_dma_controller_destroy(musb->dma_controller);
 fail2_5:
 	pm_runtime_put_sync(musb->controller);
 
@@ -2248,7 +2279,7 @@ static int musb_remove(struct platform_device *pdev)
 	musb_shutdown(pdev);
 
 	if (musb->dma_controller)
-		dma_controller_destroy(musb->dma_controller);
+		musb_dma_controller_destroy(musb->dma_controller);
 
 	cancel_work_sync(&musb->irq_work);
 	cancel_delayed_work_sync(&musb->finish_resume_work);
@@ -2316,18 +2347,18 @@ static void musb_save_context(struct musb *musb)
 			musb_readb(epio, MUSB_RXINTERVAL);
 
 		musb->context.index_regs[i].txfunaddr =
-			musb_read_txfunaddr(musb_base, i);
+			musb_read_txfunaddr(musb, i);
 		musb->context.index_regs[i].txhubaddr =
-			musb_read_txhubaddr(musb_base, i);
+			musb_read_txhubaddr(musb, i);
 		musb->context.index_regs[i].txhubport =
-			musb_read_txhubport(musb_base, i);
+			musb_read_txhubport(musb, i);
 
 		musb->context.index_regs[i].rxfunaddr =
-			musb_read_rxfunaddr(musb_base, i);
+			musb_read_rxfunaddr(musb, i);
 		musb->context.index_regs[i].rxhubaddr =
-			musb_read_rxhubaddr(musb_base, i);
+			musb_read_rxhubaddr(musb, i);
 		musb->context.index_regs[i].rxhubport =
-			musb_read_rxhubport(musb_base, i);
+			musb_read_rxhubport(musb, i);
 	}
 }
 
@@ -2335,7 +2366,6 @@ static void musb_restore_context(struct musb *musb)
 {
 	int i;
 	void __iomem *musb_base = musb->mregs;
-	void __iomem *ep_target_regs;
 	void __iomem *epio;
 	u8 power;
 
@@ -2396,21 +2426,18 @@ static void musb_restore_context(struct musb *musb)
 		musb_writeb(epio, MUSB_RXINTERVAL,
 
 				musb->context.index_regs[i].rxinterval);
-		musb_write_txfunaddr(musb_base, i,
+		musb_write_txfunaddr(musb, i,
 				musb->context.index_regs[i].txfunaddr);
-		musb_write_txhubaddr(musb_base, i,
+		musb_write_txhubaddr(musb, i,
 				musb->context.index_regs[i].txhubaddr);
-		musb_write_txhubport(musb_base, i,
+		musb_write_txhubport(musb, i,
 				musb->context.index_regs[i].txhubport);
 
-		ep_target_regs =
-			musb_read_target_reg_base(i, musb_base);
-
-		musb_write_rxfunaddr(ep_target_regs,
+		musb_write_rxfunaddr(musb, i,
 				musb->context.index_regs[i].rxfunaddr);
-		musb_write_rxhubaddr(ep_target_regs,
+		musb_write_rxhubaddr(musb, i,
 				musb->context.index_regs[i].rxhubaddr);
-		musb_write_rxhubport(ep_target_regs,
+		musb_write_rxhubport(musb, i,
 				musb->context.index_regs[i].rxhubport);
 	}
 	musb_writeb(musb_base, MUSB_INDEX, musb->context.index);
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 3877249a8b2d..4b886d0f6bdf 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -67,7 +67,6 @@ struct musb_ep;
 #include "musb_dma.h"
 
 #include "musb_io.h"
-#include "musb_regs.h"
 
 #include "musb_gadget.h"
 #include <linux/usb/hcd.h>
@@ -157,6 +156,8 @@ struct musb_io;
  * @writel:	write 32 bits
  * @read_fifo:	reads the fifo
  * @write_fifo:	writes to fifo
+ * @dma_init:	platform specific dma init function
+ * @dma_exit:	platform specific dma exit function
  * @init:	turns on clocks, sets up platform-specific registers, etc
  * @exit:	undoes @init
  * @set_mode:	forcefully changes operating mode
@@ -165,6 +166,8 @@ struct musb_io;
  * @vbus_status: returns vbus status if possible
  * @set_vbus:	forces vbus status
  * @adjust_channel_params: pre check for standard dma channel_program func
+ * @pre_root_reset_end: called before the root usb port reset flag gets cleared
+ * @post_root_reset_end: called after the root usb port reset flag gets cleared
  */
 struct musb_platform_ops {
 
@@ -187,6 +190,7 @@ struct musb_platform_ops {
 	void	(*ep_select)(void __iomem *mbase, u8 epnum);
 	u16	fifo_mode;
 	u32	(*fifo_offset)(u8 epnum);
+	u32	(*busctl_offset)(u8 epnum, u16 offset);
 	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);
@@ -195,6 +199,9 @@ struct musb_platform_ops {
 	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);
+	struct dma_controller *
+		(*dma_init) (struct musb *musb, void __iomem *base);
+	void	(*dma_exit)(struct dma_controller *c);
 	int	(*set_mode)(struct musb *musb, u8 mode);
 	void	(*try_idle)(struct musb *musb, unsigned long timeout);
 	int	(*recover)(struct musb *musb);
@@ -205,6 +212,8 @@ struct musb_platform_ops {
 	int	(*adjust_channel_params)(struct dma_channel *channel,
 				u16 packet_sz, u8 *mode,
 				dma_addr_t *dma_addr, u32 *len);
+	void	(*pre_root_reset_end)(struct musb *musb);
+	void	(*post_root_reset_end)(struct musb *musb);
 };
 
 /*
@@ -241,8 +250,6 @@ struct musb_hw_ep {
 	void __iomem		*fifo_sync_va;
 #endif
 
-	void __iomem		*target_regs;
-
 	/* currently scheduled peripheral endpoint */
 	struct musb_qh		*in_qh;
 	struct musb_qh		*out_qh;
@@ -437,6 +444,9 @@ struct musb {
 #endif
 };
 
+/* This must be included after struct musb is defined */
+#include "musb_regs.h"
+
 static inline struct musb *gadget_to_musb(struct usb_gadget *g)
 {
 	return container_of(g, struct musb, g);
@@ -590,4 +600,16 @@ static inline int musb_platform_exit(struct musb *musb)
 	return musb->ops->exit(musb);
 }
 
+static inline void musb_platform_pre_root_reset_end(struct musb *musb)
+{
+	if (musb->ops->pre_root_reset_end)
+		musb->ops->pre_root_reset_end(musb);
+}
+
+static inline void musb_platform_post_root_reset_end(struct musb *musb)
+{
+	if (musb->ops->post_root_reset_end)
+		musb->ops->post_root_reset_end(musb);
+}
+
 #endif	/* __MUSB_CORE_H__ */
diff --git a/drivers/usb/musb/musb_cppi41.c b/drivers/usb/musb/musb_cppi41.c
index 8bd8c5e26921..4d1b44c232ee 100644
--- a/drivers/usb/musb/musb_cppi41.c
+++ b/drivers/usb/musb/musb_cppi41.c
@@ -678,7 +678,7 @@ err:
 	return ret;
 }
 
-void dma_controller_destroy(struct dma_controller *c)
+void cppi41_dma_controller_destroy(struct dma_controller *c)
 {
 	struct cppi41_dma_controller *controller = container_of(c,
 			struct cppi41_dma_controller, controller);
@@ -687,9 +687,10 @@ void dma_controller_destroy(struct dma_controller *c)
 	cppi41_dma_controller_stop(controller);
 	kfree(controller);
 }
+EXPORT_SYMBOL_GPL(cppi41_dma_controller_destroy);
 
-struct dma_controller *dma_controller_create(struct musb *musb,
-					void __iomem *base)
+struct dma_controller *
+cppi41_dma_controller_create(struct musb *musb, void __iomem *base)
 {
 	struct cppi41_dma_controller *controller;
 	int ret = 0;
@@ -726,3 +727,4 @@ kzalloc_fail:
 		return ERR_PTR(ret);
 	return NULL;
 }
+EXPORT_SYMBOL_GPL(cppi41_dma_controller_create);
diff --git a/drivers/usb/musb/musb_debugfs.c b/drivers/usb/musb/musb_debugfs.c
index 78a283e9ce40..9b22d946c089 100644
--- a/drivers/usb/musb/musb_debugfs.c
+++ b/drivers/usb/musb/musb_debugfs.c
@@ -191,9 +191,16 @@ static ssize_t musb_test_mode_write(struct file *file,
 {
 	struct seq_file		*s = file->private_data;
 	struct musb		*musb = s->private;
-	u8			test = 0;
+	u8			test;
 	char			buf[18];
 
+	test = musb_readb(musb->mregs, MUSB_TESTMODE);
+	if (test) {
+		dev_err(musb->controller, "Error: test mode is already set. "
+			"Please do USB Bus Reset to start a new test.\n");
+		return count;
+	}
+
 	memset(buf, 0x00, sizeof(buf));
 
 	if (copy_from_user(buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
@@ -238,6 +245,90 @@ static const struct file_operations musb_test_mode_fops = {
 	.release		= single_release,
 };
 
+static int musb_softconnect_show(struct seq_file *s, void *unused)
+{
+	struct musb	*musb = s->private;
+	u8		reg;
+	int		connect;
+
+	switch (musb->xceiv->otg->state) {
+	case OTG_STATE_A_HOST:
+	case OTG_STATE_A_WAIT_BCON:
+		reg = musb_readb(musb->mregs, MUSB_DEVCTL);
+		connect = reg & MUSB_DEVCTL_SESSION ? 1 : 0;
+		break;
+	default:
+		connect = -1;
+	}
+
+	seq_printf(s, "%d\n", connect);
+
+	return 0;
+}
+
+static int musb_softconnect_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, musb_softconnect_show, inode->i_private);
+}
+
+static ssize_t musb_softconnect_write(struct file *file,
+		const char __user *ubuf, size_t count, loff_t *ppos)
+{
+	struct seq_file		*s = file->private_data;
+	struct musb		*musb = s->private;
+	char			buf[2];
+	u8			reg;
+
+	memset(buf, 0x00, sizeof(buf));
+
+	if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+		return -EFAULT;
+
+	if (!strncmp(buf, "0", 1)) {
+		switch (musb->xceiv->otg->state) {
+		case OTG_STATE_A_HOST:
+			musb_root_disconnect(musb);
+			reg = musb_readb(musb->mregs, MUSB_DEVCTL);
+			reg &= ~MUSB_DEVCTL_SESSION;
+			musb_writeb(musb->mregs, MUSB_DEVCTL, reg);
+			break;
+		default:
+			break;
+		}
+	} else if (!strncmp(buf, "1", 1)) {
+		switch (musb->xceiv->otg->state) {
+		case OTG_STATE_A_WAIT_BCON:
+			/*
+			 * musb_save_context() called in musb_runtime_suspend()
+			 * might cache devctl with SESSION bit cleared during
+			 * soft-disconnect, so specifically set SESSION bit
+			 * here to preserve it for musb_runtime_resume().
+			 */
+			musb->context.devctl |= MUSB_DEVCTL_SESSION;
+			reg = musb_readb(musb->mregs, MUSB_DEVCTL);
+			reg |= MUSB_DEVCTL_SESSION;
+			musb_writeb(musb->mregs, MUSB_DEVCTL, reg);
+			break;
+		default:
+			break;
+		}
+	}
+
+	return count;
+}
+
+/*
+ * In host mode, connect/disconnect the bus without physically
+ * remove the devices.
+ */
+static const struct file_operations musb_softconnect_fops = {
+	.open			= musb_softconnect_open,
+	.write			= musb_softconnect_write,
+	.read			= seq_read,
+	.llseek			= seq_lseek,
+	.release		= single_release,
+};
+
 int musb_init_debugfs(struct musb *musb)
 {
 	struct dentry		*root;
@@ -264,6 +355,13 @@ int musb_init_debugfs(struct musb *musb)
 		goto err1;
 	}
 
+	file = debugfs_create_file("softconnect", S_IRUGO | S_IWUSR,
+			root, musb, &musb_softconnect_fops);
+	if (!file) {
+		ret = -ENOMEM;
+		goto err1;
+	}
+
 	musb->debugfs_root = root;
 
 	return 0;
diff --git a/drivers/usb/musb/musb_dma.h b/drivers/usb/musb/musb_dma.h
index 1d44faa86252..46357e183b4c 100644
--- a/drivers/usb/musb/musb_dma.h
+++ b/drivers/usb/musb/musb_dma.h
@@ -68,16 +68,41 @@ struct musb_hw_ep;
 #define	is_dma_capable()	(1)
 #endif
 
-#if defined(CONFIG_USB_TI_CPPI_DMA) || defined(CONFIG_USB_TI_CPPI41_DMA)
-#define	is_cppi_enabled()	1
+#ifdef CONFIG_USB_UX500_DMA
+#define musb_dma_ux500(musb)		(musb->io.quirks & MUSB_DMA_UX500)
+#else
+#define musb_dma_ux500(musb)		0
+#endif
+
+#ifdef CONFIG_USB_TI_CPPI41_DMA
+#define musb_dma_cppi41(musb)		(musb->io.quirks & MUSB_DMA_CPPI41)
+#else
+#define musb_dma_cppi41(musb)		0
+#endif
+
+#ifdef CONFIG_USB_TI_CPPI_DMA
+#define musb_dma_cppi(musb)		(musb->io.quirks & MUSB_DMA_CPPI)
 #else
-#define	is_cppi_enabled()	0
+#define musb_dma_cppi(musb)		0
 #endif
 
 #ifdef CONFIG_USB_TUSB_OMAP_DMA
-#define tusb_dma_omap()			1
+#define tusb_dma_omap(musb)		(musb->io.quirks & MUSB_DMA_TUSB_OMAP)
+#else
+#define tusb_dma_omap(musb)		0
+#endif
+
+#ifdef CONFIG_USB_INVENTRA_DMA
+#define musb_dma_inventra(musb)		(musb->io.quirks & MUSB_DMA_INVENTRA)
 #else
-#define tusb_dma_omap()			0
+#define musb_dma_inventra(musb)		0
+#endif
+
+#if defined(CONFIG_USB_TI_CPPI_DMA) || defined(CONFIG_USB_TI_CPPI41_DMA)
+#define	is_cppi_enabled(musb)		\
+	(musb_dma_cppi(musb) || musb_dma_cppi41(musb))
+#else
+#define	is_cppi_enabled(musb)	0
 #endif
 
 /* Anomaly 05000456 - USB Receive Interrupt Is Not Generated in DMA Mode 1
@@ -177,19 +202,41 @@ struct dma_controller {
 extern void musb_dma_completion(struct musb *musb, u8 epnum, u8 transmit);
 
 #ifdef CONFIG_MUSB_PIO_ONLY
-static inline struct dma_controller *dma_controller_create(struct musb *m,
-		void __iomem *io)
+static inline struct dma_controller *
+musb_dma_controller_create(struct musb *m, void __iomem *io)
 {
 	return NULL;
 }
 
-static inline void dma_controller_destroy(struct dma_controller *d) { }
+static inline void musb_dma_controller_destroy(struct dma_controller *d) { }
 
 #else
 
-extern struct dma_controller *dma_controller_create(struct musb *, void __iomem *);
+extern struct dma_controller *
+(*musb_dma_controller_create)(struct musb *, void __iomem *);
 
-extern void dma_controller_destroy(struct dma_controller *);
+extern void (*musb_dma_controller_destroy)(struct dma_controller *);
 #endif
 
+/* Platform specific DMA functions */
+extern struct dma_controller *
+musbhs_dma_controller_create(struct musb *musb, void __iomem *base);
+extern void musbhs_dma_controller_destroy(struct dma_controller *c);
+
+extern struct dma_controller *
+tusb_dma_controller_create(struct musb *musb, void __iomem *base);
+extern void tusb_dma_controller_destroy(struct dma_controller *c);
+
+extern struct dma_controller *
+cppi_dma_controller_create(struct musb *musb, void __iomem *base);
+extern void cppi_dma_controller_destroy(struct dma_controller *c);
+
+extern struct dma_controller *
+cppi41_dma_controller_create(struct musb *musb, void __iomem *base);
+extern void cppi41_dma_controller_destroy(struct dma_controller *c);
+
+extern struct dma_controller *
+ux500_dma_controller_create(struct musb *musb, void __iomem *base);
+extern void ux500_dma_controller_destroy(struct dma_controller *c);
+
 #endif	/* __MUSB_DMA_H__ */
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index 65d931a28a14..1334a3de31b8 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -634,10 +634,14 @@ static void dsps_read_fifo32(struct musb_hw_ep *hw_ep, u16 len, u8 *dst)
 }
 
 static struct musb_platform_ops dsps_ops = {
-	.quirks		= MUSB_INDEXED_EP,
+	.quirks		= MUSB_DMA_CPPI41 | MUSB_INDEXED_EP,
 	.init		= dsps_musb_init,
 	.exit		= dsps_musb_exit,
 
+#ifdef CONFIG_USB_TI_CPPI41_DMA
+	.dma_init	= cppi41_dma_controller_create,
+	.dma_exit	= cppi41_dma_controller_destroy,
+#endif
 	.enable		= dsps_musb_enable,
 	.disable	= dsps_musb_disable,
 
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 4c481cd66c77..625d482f1a97 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -366,7 +366,7 @@ static void txstate(struct musb *musb, struct musb_request *req)
 		}
 
 #endif
-		if (is_cppi_enabled()) {
+		if (is_cppi_enabled(musb)) {
 			/* program endpoint CSR first, then setup DMA */
 			csr &= ~(MUSB_TXCSR_P_UNDERRUN | MUSB_TXCSR_TXPKTRDY);
 			csr |= MUSB_TXCSR_DMAENAB | MUSB_TXCSR_DMAMODE |
@@ -402,7 +402,7 @@ static void txstate(struct musb *musb, struct musb_request *req)
 				musb_writew(epio, MUSB_TXCSR, csr);
 				/* invariant: prequest->buf is non-null */
 			}
-		} else if (tusb_dma_omap())
+		} else if (tusb_dma_omap(musb))
 			use_dma = use_dma && c->channel_program(
 					musb_ep->dma, musb_ep->packet_sz,
 					request->zero,
@@ -489,6 +489,7 @@ void musb_g_tx(struct musb *musb, u8 epnum)
 
 	if (request) {
 		u8	is_dma = 0;
+		bool	short_packet = false;
 
 		if (dma && (csr & MUSB_TXCSR_DMAENAB)) {
 			is_dma = 1;
@@ -507,15 +508,18 @@ void musb_g_tx(struct musb *musb, u8 epnum)
 		 * First, maybe a terminating short packet. Some DMA
 		 * engines might handle this by themselves.
 		 */
-		if ((request->zero && request->length
+		if ((request->zero && request->length)
 			&& (request->length % musb_ep->packet_sz == 0)
 			&& (request->actual == request->length))
-#if defined(CONFIG_USB_INVENTRA_DMA) || defined(CONFIG_USB_UX500_DMA)
-			|| (is_dma && (!dma->desired_mode ||
+				short_packet = true;
+
+		if ((musb_dma_inventra(musb) || musb_dma_ux500(musb)) &&
+			(is_dma && (!dma->desired_mode ||
 				(request->actual &
-					(musb_ep->packet_sz - 1))))
-#endif
-		) {
+					(musb_ep->packet_sz - 1)))))
+				short_packet = true;
+
+		if (short_packet) {
 			/*
 			 * On DMA completion, FIFO may not be
 			 * available yet...
@@ -595,7 +599,7 @@ static void rxstate(struct musb *musb, struct musb_request *req)
 		return;
 	}
 
-	if (is_cppi_enabled() && is_buffer_mapped(req)) {
+	if (is_cppi_enabled(musb) && is_buffer_mapped(req)) {
 		struct dma_controller	*c = musb->dma_controller;
 		struct dma_channel	*channel = musb_ep->dma;
 
@@ -772,7 +776,7 @@ static void rxstate(struct musb *musb, struct musb_request *req)
 			fifo_count = min_t(unsigned, len, fifo_count);
 
 #ifdef	CONFIG_USB_TUSB_OMAP_DMA
-			if (tusb_dma_omap() && is_buffer_mapped(req)) {
+			if (tusb_dma_omap(musb) && is_buffer_mapped(req)) {
 				struct dma_controller *c = musb->dma_controller;
 				struct dma_channel *channel = musb_ep->dma;
 				u32 dma_addr = request->dma + request->actual;
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index c3d5fc9dfb5b..26c65e66cc0f 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -181,7 +181,7 @@ static inline void musb_h_tx_dma_start(struct musb_hw_ep *ep)
 	/* NOTE: no locks here; caller should lock and select EP */
 	txcsr = musb_readw(ep->regs, MUSB_TXCSR);
 	txcsr |= MUSB_TXCSR_DMAENAB | MUSB_TXCSR_H_WZC_BITS;
-	if (is_cppi_enabled())
+	if (is_cppi_enabled(ep->musb))
 		txcsr |= MUSB_TXCSR_DMAMODE;
 	musb_writew(ep->regs, MUSB_TXCSR, txcsr);
 }
@@ -294,7 +294,7 @@ start:
 
 		if (!hw_ep->tx_channel)
 			musb_h_tx_start(hw_ep);
-		else if (is_cppi_enabled() || tusb_dma_omap())
+		else if (is_cppi_enabled(musb) || tusb_dma_omap(musb))
 			musb_h_tx_dma_start(hw_ep);
 	}
 }
@@ -555,8 +555,9 @@ musb_host_packet_rx(struct musb *musb, struct urb *urb, u8 epnum, u8 iso_err)
  * the busy/not-empty tests are basically paranoia.
  */
 static void
-musb_rx_reinit(struct musb *musb, struct musb_qh *qh, struct musb_hw_ep *ep)
+musb_rx_reinit(struct musb *musb, struct musb_qh *qh, u8 epnum)
 {
+	struct musb_hw_ep *ep = musb->endpoints + epnum;
 	u16	csr;
 
 	/* NOTE:  we know the "rx" fifo reinit never triggers for ep0.
@@ -594,10 +595,9 @@ musb_rx_reinit(struct musb *musb, struct musb_qh *qh, struct musb_hw_ep *ep)
 
 	/* target addr and (for multipoint) hub addr/port */
 	if (musb->is_multipoint) {
-		musb_write_rxfunaddr(ep->target_regs, qh->addr_reg);
-		musb_write_rxhubaddr(ep->target_regs, qh->h_addr_reg);
-		musb_write_rxhubport(ep->target_regs, qh->h_port_reg);
-
+		musb_write_rxfunaddr(musb, epnum, qh->addr_reg);
+		musb_write_rxhubaddr(musb, epnum, qh->h_addr_reg);
+		musb_write_rxhubport(musb, epnum, qh->h_port_reg);
 	} else
 		musb_writeb(musb->mregs, MUSB_FADDR, qh->addr_reg);
 
@@ -617,23 +617,22 @@ musb_rx_reinit(struct musb *musb, struct musb_qh *qh, struct musb_hw_ep *ep)
 	ep->rx_reinit = 0;
 }
 
-static bool musb_tx_dma_program(struct dma_controller *dma,
+static int musb_tx_dma_set_mode_mentor(struct dma_controller *dma,
 		struct musb_hw_ep *hw_ep, struct musb_qh *qh,
-		struct urb *urb, u32 offset, u32 length)
+		struct urb *urb, u32 offset,
+		u32 *length, u8 *mode)
 {
 	struct dma_channel	*channel = hw_ep->tx_channel;
 	void __iomem		*epio = hw_ep->regs;
 	u16			pkt_size = qh->maxpacket;
 	u16			csr;
-	u8			mode;
 
-#if defined(CONFIG_USB_INVENTRA_DMA) || defined(CONFIG_USB_UX500_DMA)
-	if (length > channel->max_len)
-		length = channel->max_len;
+	if (*length > channel->max_len)
+		*length = channel->max_len;
 
 	csr = musb_readw(epio, MUSB_TXCSR);
-	if (length > pkt_size) {
-		mode = 1;
+	if (*length > pkt_size) {
+		*mode = 1;
 		csr |= MUSB_TXCSR_DMAMODE | MUSB_TXCSR_DMAENAB;
 		/* autoset shouldn't be set in high bandwidth */
 		/*
@@ -649,15 +648,28 @@ static bool musb_tx_dma_program(struct dma_controller *dma,
 					can_bulk_split(hw_ep->musb, qh->type)))
 			csr |= MUSB_TXCSR_AUTOSET;
 	} else {
-		mode = 0;
+		*mode = 0;
 		csr &= ~(MUSB_TXCSR_AUTOSET | MUSB_TXCSR_DMAMODE);
 		csr |= MUSB_TXCSR_DMAENAB; /* against programmer's guide */
 	}
 	channel->desired_mode = mode;
 	musb_writew(epio, MUSB_TXCSR, csr);
-#else
-	if (!is_cppi_enabled() && !tusb_dma_omap())
-		return false;
+
+	return 0;
+}
+
+static int musb_tx_dma_set_mode_cppi_tusb(struct dma_controller *dma,
+					  struct musb_hw_ep *hw_ep,
+					  struct musb_qh *qh,
+					  struct urb *urb,
+					  u32 offset,
+					  u32 *length,
+					  u8 *mode)
+{
+	struct dma_channel *channel = hw_ep->tx_channel;
+
+	if (!is_cppi_enabled(hw_ep->musb) && !tusb_dma_omap(hw_ep->musb))
+		return -ENODEV;
 
 	channel->actual_len = 0;
 
@@ -665,8 +677,28 @@ static bool musb_tx_dma_program(struct dma_controller *dma,
 	 * TX uses "RNDIS" mode automatically but needs help
 	 * to identify the zero-length-final-packet case.
 	 */
-	mode = (urb->transfer_flags & URB_ZERO_PACKET) ? 1 : 0;
-#endif
+	*mode = (urb->transfer_flags & URB_ZERO_PACKET) ? 1 : 0;
+
+	return 0;
+}
+
+static bool musb_tx_dma_program(struct dma_controller *dma,
+		struct musb_hw_ep *hw_ep, struct musb_qh *qh,
+		struct urb *urb, u32 offset, u32 length)
+{
+	struct dma_channel	*channel = hw_ep->tx_channel;
+	u16			pkt_size = qh->maxpacket;
+	u8			mode;
+	int			res;
+
+	if (musb_dma_inventra(hw_ep->musb) || musb_dma_ux500(hw_ep->musb))
+		res = musb_tx_dma_set_mode_mentor(dma, hw_ep, qh, urb,
+						 offset, &length, &mode);
+	else
+		res = musb_tx_dma_set_mode_cppi_tusb(dma, hw_ep, qh, urb,
+						     offset, &length, &mode);
+	if (res)
+		return false;
 
 	qh->segsize = length;
 
@@ -678,6 +710,9 @@ static bool musb_tx_dma_program(struct dma_controller *dma,
 
 	if (!dma->channel_program(channel, pkt_size, mode,
 			urb->transfer_dma + offset, length)) {
+		void __iomem *epio = hw_ep->regs;
+		u16 csr;
+
 		dma->channel_release(channel);
 		hw_ep->tx_channel = NULL;
 
@@ -801,9 +836,9 @@ static void musb_ep_program(struct musb *musb, u8 epnum,
 
 		/* target addr and (for multipoint) hub addr/port */
 		if (musb->is_multipoint) {
-			musb_write_txfunaddr(mbase, epnum, qh->addr_reg);
-			musb_write_txhubaddr(mbase, epnum, qh->h_addr_reg);
-			musb_write_txhubport(mbase, epnum, qh->h_port_reg);
+			musb_write_txfunaddr(musb, epnum, qh->addr_reg);
+			musb_write_txhubaddr(musb, epnum, qh->h_addr_reg);
+			musb_write_txhubport(musb, epnum, qh->h_port_reg);
 /* FIXME if !epnum, do the same for RX ... */
 		} else
 			musb_writeb(mbase, MUSB_FADDR, qh->addr_reg);
@@ -875,7 +910,7 @@ finish:
 		u16	csr;
 
 		if (hw_ep->rx_reinit) {
-			musb_rx_reinit(musb, qh, hw_ep);
+			musb_rx_reinit(musb, qh, epnum);
 
 			/* init new state: toggle and NYET, maybe DMA later */
 			if (usb_gettoggle(urb->dev, qh->epnum, 0))
@@ -901,7 +936,7 @@ finish:
 
 		/* kick things off */
 
-		if ((is_cppi_enabled() || tusb_dma_omap()) && dma_channel) {
+		if ((is_cppi_enabled(musb) || tusb_dma_omap(musb)) && dma_channel) {
 			/* Candidate for DMA */
 			dma_channel->actual_len = 0L;
 			qh->segsize = len;
@@ -1441,7 +1476,7 @@ done:
 	} else if ((usb_pipeisoc(pipe) || transfer_pending) && dma) {
 		if (musb_tx_dma_program(musb->dma_controller, hw_ep, qh, urb,
 				offset, length)) {
-			if (is_cppi_enabled() || tusb_dma_omap())
+			if (is_cppi_enabled(musb) || tusb_dma_omap(musb))
 				musb_h_tx_dma_start(hw_ep);
 			return;
 		}
@@ -1498,9 +1533,47 @@ done:
 			MUSB_TXCSR_H_WZC_BITS | MUSB_TXCSR_TXPKTRDY);
 }
 
+#ifdef CONFIG_USB_TI_CPPI41_DMA
+/* Seems to set up ISO for cppi41 and not advance len. See commit c57c41d */
+static int musb_rx_dma_iso_cppi41(struct dma_controller *dma,
+				  struct musb_hw_ep *hw_ep,
+				  struct musb_qh *qh,
+				  struct urb *urb,
+				  size_t len)
+{
+	struct dma_channel *channel = hw_ep->tx_channel;
+	void __iomem *epio = hw_ep->regs;
+	dma_addr_t *buf;
+	u32 length, res;
+	u16 val;
 
-#ifdef CONFIG_USB_INVENTRA_DMA
+	buf = (void *)urb->iso_frame_desc[qh->iso_idx].offset +
+		(u32)urb->transfer_dma;
+
+	length = urb->iso_frame_desc[qh->iso_idx].length;
 
+	val = musb_readw(epio, MUSB_RXCSR);
+	val |= MUSB_RXCSR_DMAENAB;
+	musb_writew(hw_ep->regs, MUSB_RXCSR, val);
+
+	res = dma->channel_program(channel, qh->maxpacket, 0,
+				   (u32)buf, length);
+
+	return res;
+}
+#else
+static inline int musb_rx_dma_iso_cppi41(struct dma_controller *dma,
+					 struct musb_hw_ep *hw_ep,
+					 struct musb_qh *qh,
+					 struct urb *urb,
+					 size_t len)
+{
+	return false;
+}
+#endif
+
+#if defined(CONFIG_USB_INVENTRA_DMA) || defined(CONFIG_USB_UX500_DMA) || \
+	defined(CONFIG_USB_TI_CPPI41_DMA)
 /* Host side RX (IN) using Mentor DMA works as follows:
 	submit_urb ->
 		- if queue was empty, ProgramEndpoint
@@ -1535,7 +1608,194 @@ done:
  *	thus be a great candidate for using mode 1 ... for all but the
  *	last packet of one URB's transfer.
  */
+static int musb_rx_dma_inventra_cppi41(struct dma_controller *dma,
+				       struct musb_hw_ep *hw_ep,
+				       struct musb_qh *qh,
+				       struct urb *urb,
+				       size_t len)
+{
+	struct dma_channel *channel = hw_ep->rx_channel;
+	void __iomem *epio = hw_ep->regs;
+	u16 val;
+	int pipe;
+	bool done;
+
+	pipe = urb->pipe;
+
+	if (usb_pipeisoc(pipe)) {
+		struct usb_iso_packet_descriptor *d;
+
+		d = urb->iso_frame_desc + qh->iso_idx;
+		d->actual_length = len;
+
+		/* even if there was an error, we did the dma
+		 * for iso_frame_desc->length
+		 */
+		if (d->status != -EILSEQ && d->status != -EOVERFLOW)
+			d->status = 0;
+
+		if (++qh->iso_idx >= urb->number_of_packets) {
+			done = true;
+		} else {
+			/* REVISIT: Why ignore return value here? */
+			if (musb_dma_cppi41(hw_ep->musb))
+				done = musb_rx_dma_iso_cppi41(dma, hw_ep, qh,
+							      urb, len);
+			done = false;
+		}
+
+	} else  {
+		/* done if urb buffer is full or short packet is recd */
+		done = (urb->actual_length + len >=
+			urb->transfer_buffer_length
+			|| channel->actual_len < qh->maxpacket
+			|| channel->rx_packet_done);
+	}
+
+	/* send IN token for next packet, without AUTOREQ */
+	if (!done) {
+		val = musb_readw(epio, MUSB_RXCSR);
+		val |= MUSB_RXCSR_H_REQPKT;
+		musb_writew(epio, MUSB_RXCSR, MUSB_RXCSR_H_WZC_BITS | val);
+	}
+
+	return done;
+}
+
+/* Disadvantage of using mode 1:
+ *	It's basically usable only for mass storage class; essentially all
+ *	other protocols also terminate transfers on short packets.
+ *
+ * Details:
+ *	An extra IN token is sent at the end of the transfer (due to AUTOREQ)
+ *	If you try to use mode 1 for (transfer_buffer_length - 512), and try
+ *	to use the extra IN token to grab the last packet using mode 0, then
+ *	the problem is that you cannot be sure when the device will send the
+ *	last packet and RxPktRdy set. Sometimes the packet is recd too soon
+ *	such that it gets lost when RxCSR is re-set at the end of the mode 1
+ *	transfer, while sometimes it is recd just a little late so that if you
+ *	try to configure for mode 0 soon after the mode 1 transfer is
+ *	completed, you will find rxcount 0. Okay, so you might think why not
+ *	wait for an interrupt when the pkt is recd. Well, you won't get any!
+ */
+static int musb_rx_dma_in_inventra_cppi41(struct dma_controller *dma,
+					  struct musb_hw_ep *hw_ep,
+					  struct musb_qh *qh,
+					  struct urb *urb,
+					  size_t len,
+					  u8 iso_err)
+{
+	struct musb *musb = hw_ep->musb;
+	void __iomem *epio = hw_ep->regs;
+	struct dma_channel *channel = hw_ep->rx_channel;
+	u16 rx_count, val;
+	int length, pipe, done;
+	dma_addr_t buf;
+
+	rx_count = musb_readw(epio, MUSB_RXCOUNT);
+	pipe = urb->pipe;
+
+	if (usb_pipeisoc(pipe)) {
+		int d_status = 0;
+		struct usb_iso_packet_descriptor *d;
+
+		d = urb->iso_frame_desc + qh->iso_idx;
 
+		if (iso_err) {
+			d_status = -EILSEQ;
+			urb->error_count++;
+		}
+		if (rx_count > d->length) {
+			if (d_status == 0) {
+				d_status = -EOVERFLOW;
+				urb->error_count++;
+			}
+			dev_dbg(musb->controller, "** OVERFLOW %d into %d\n",
+				rx_count, d->length);
+
+			length = d->length;
+		} else
+			length = rx_count;
+		d->status = d_status;
+		buf = urb->transfer_dma + d->offset;
+	} else {
+		length = rx_count;
+		buf = urb->transfer_dma + urb->actual_length;
+	}
+
+	channel->desired_mode = 0;
+#ifdef USE_MODE1
+	/* because of the issue below, mode 1 will
+	 * only rarely behave with correct semantics.
+	 */
+	if ((urb->transfer_flags & URB_SHORT_NOT_OK)
+	    && (urb->transfer_buffer_length - urb->actual_length)
+	    > qh->maxpacket)
+		channel->desired_mode = 1;
+	if (rx_count < hw_ep->max_packet_sz_rx) {
+		length = rx_count;
+		channel->desired_mode = 0;
+	} else {
+		length = urb->transfer_buffer_length;
+	}
+#endif
+
+	/* See comments above on disadvantages of using mode 1 */
+	val = musb_readw(epio, MUSB_RXCSR);
+	val &= ~MUSB_RXCSR_H_REQPKT;
+
+	if (channel->desired_mode == 0)
+		val &= ~MUSB_RXCSR_H_AUTOREQ;
+	else
+		val |= MUSB_RXCSR_H_AUTOREQ;
+	val |= MUSB_RXCSR_DMAENAB;
+
+	/* autoclear shouldn't be set in high bandwidth */
+	if (qh->hb_mult == 1)
+		val |= MUSB_RXCSR_AUTOCLEAR;
+
+	musb_writew(epio, MUSB_RXCSR, MUSB_RXCSR_H_WZC_BITS | val);
+
+	/* REVISIT if when actual_length != 0,
+	 * transfer_buffer_length needs to be
+	 * adjusted first...
+	 */
+	done = dma->channel_program(channel, qh->maxpacket,
+				   channel->desired_mode,
+				   buf, length);
+
+	if (!done) {
+		dma->channel_release(channel);
+		hw_ep->rx_channel = NULL;
+		channel = NULL;
+		val = musb_readw(epio, MUSB_RXCSR);
+		val &= ~(MUSB_RXCSR_DMAENAB
+			 | MUSB_RXCSR_H_AUTOREQ
+			 | MUSB_RXCSR_AUTOCLEAR);
+		musb_writew(epio, MUSB_RXCSR, val);
+	}
+
+	return done;
+}
+#else
+static inline int musb_rx_dma_inventra_cppi41(struct dma_controller *dma,
+					      struct musb_hw_ep *hw_ep,
+					      struct musb_qh *qh,
+					      struct urb *urb,
+					      size_t len)
+{
+	return false;
+}
+
+static inline int musb_rx_dma_in_inventra_cppi41(struct dma_controller *dma,
+						 struct musb_hw_ep *hw_ep,
+						 struct musb_qh *qh,
+						 struct urb *urb,
+						 size_t len,
+						 u8 iso_err)
+{
+	return false;
+}
 #endif
 
 /*
@@ -1546,6 +1806,7 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 {
 	struct urb		*urb;
 	struct musb_hw_ep	*hw_ep = musb->endpoints + epnum;
+	struct dma_controller	*c = musb->dma_controller;
 	void __iomem		*epio = hw_ep->regs;
 	struct musb_qh		*qh = hw_ep->in_qh;
 	size_t			xfer_len;
@@ -1661,9 +1922,8 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 	 */
 
 	/* FIXME this is _way_ too much in-line logic for Mentor DMA */
-
-#if !defined(CONFIG_USB_INVENTRA_DMA) && !defined(CONFIG_USB_UX500_DMA)
-	if (rx_csr & MUSB_RXCSR_H_REQPKT)  {
+	if (!musb_dma_inventra(musb) && !musb_dma_ux500(musb) &&
+	    (rx_csr & MUSB_RXCSR_H_REQPKT)) {
 		/* REVISIT this happened for a while on some short reads...
 		 * the cleanup still needs investigation... looks bad...
 		 * and also duplicates dma cleanup code above ... plus,
@@ -1684,7 +1944,7 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 		musb_writew(epio, MUSB_RXCSR,
 				MUSB_RXCSR_H_WZC_BITS | rx_csr);
 	}
-#endif
+
 	if (dma && (rx_csr & MUSB_RXCSR_DMAENAB)) {
 		xfer_len = dma->actual_len;
 
@@ -1694,67 +1954,18 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 			| MUSB_RXCSR_RXPKTRDY);
 		musb_writew(hw_ep->regs, MUSB_RXCSR, val);
 
-#if defined(CONFIG_USB_INVENTRA_DMA) || defined(CONFIG_USB_UX500_DMA) || \
-	defined(CONFIG_USB_TI_CPPI41_DMA)
-		if (usb_pipeisoc(pipe)) {
-			struct usb_iso_packet_descriptor *d;
-
-			d = urb->iso_frame_desc + qh->iso_idx;
-			d->actual_length = xfer_len;
-
-			/* even if there was an error, we did the dma
-			 * for iso_frame_desc->length
-			 */
-			if (d->status != -EILSEQ && d->status != -EOVERFLOW)
-				d->status = 0;
-
-			if (++qh->iso_idx >= urb->number_of_packets) {
-				done = true;
-			} else {
-#if defined(CONFIG_USB_TI_CPPI41_DMA)
-				struct dma_controller   *c;
-				dma_addr_t *buf;
-				u32 length, ret;
-
-				c = musb->dma_controller;
-				buf = (void *)
-					urb->iso_frame_desc[qh->iso_idx].offset
-					+ (u32)urb->transfer_dma;
-
-				length =
-					urb->iso_frame_desc[qh->iso_idx].length;
-
-				val |= MUSB_RXCSR_DMAENAB;
-				musb_writew(hw_ep->regs, MUSB_RXCSR, val);
-
-				ret = c->channel_program(dma, qh->maxpacket,
-						0, (u32) buf, length);
-#endif
-				done = false;
-			}
-
-		} else  {
-			/* done if urb buffer is full or short packet is recd */
-			done = (urb->actual_length + xfer_len >=
-					urb->transfer_buffer_length
-				|| dma->actual_len < qh->maxpacket
-				|| dma->rx_packet_done);
-		}
-
-		/* send IN token for next packet, without AUTOREQ */
-		if (!done) {
-			val |= MUSB_RXCSR_H_REQPKT;
-			musb_writew(epio, MUSB_RXCSR,
-				MUSB_RXCSR_H_WZC_BITS | val);
+		if (musb_dma_inventra(musb) || musb_dma_ux500(musb) ||
+		    musb_dma_cppi41(musb)) {
+			    done = musb_rx_dma_inventra_cppi41(c, hw_ep, qh, urb, xfer_len);
+			    dev_dbg(hw_ep->musb->controller,
+				    "ep %d dma %s, rxcsr %04x, rxcount %d\n",
+				    epnum, done ? "off" : "reset",
+				    musb_readw(epio, MUSB_RXCSR),
+				    musb_readw(epio, MUSB_RXCOUNT));
+		} else {
+			done = true;
 		}
 
-		dev_dbg(musb->controller, "ep %d dma %s, rxcsr %04x, rxcount %d\n", epnum,
-			done ? "off" : "reset",
-			musb_readw(epio, MUSB_RXCSR),
-			musb_readw(epio, MUSB_RXCOUNT));
-#else
-		done = true;
-#endif
 	} else if (urb->status == -EINPROGRESS) {
 		/* if no errors, be sure a packet is ready for unloading */
 		if (unlikely(!(rx_csr & MUSB_RXCSR_RXPKTRDY))) {
@@ -1772,126 +1983,24 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 		}
 
 		/* we are expecting IN packets */
-#if defined(CONFIG_USB_INVENTRA_DMA) || defined(CONFIG_USB_UX500_DMA) || \
-	defined(CONFIG_USB_TI_CPPI41_DMA)
-		if (dma) {
-			struct dma_controller	*c;
-			u16			rx_count;
-			int			ret, length;
-			dma_addr_t		buf;
-
-			rx_count = musb_readw(epio, MUSB_RXCOUNT);
-
-			dev_dbg(musb->controller, "RX%d count %d, buffer 0x%llx len %d/%d\n",
-					epnum, rx_count,
-					(unsigned long long) urb->transfer_dma
-					+ urb->actual_length,
-					qh->offset,
-					urb->transfer_buffer_length);
-
-			c = musb->dma_controller;
-
-			if (usb_pipeisoc(pipe)) {
-				int d_status = 0;
-				struct usb_iso_packet_descriptor *d;
-
-				d = urb->iso_frame_desc + qh->iso_idx;
-
-				if (iso_err) {
-					d_status = -EILSEQ;
-					urb->error_count++;
-				}
-				if (rx_count > d->length) {
-					if (d_status == 0) {
-						d_status = -EOVERFLOW;
-						urb->error_count++;
-					}
-					dev_dbg(musb->controller, "** OVERFLOW %d into %d\n",\
-					    rx_count, d->length);
-
-					length = d->length;
-				} else
-					length = rx_count;
-				d->status = d_status;
-				buf = urb->transfer_dma + d->offset;
-			} else {
-				length = rx_count;
-				buf = urb->transfer_dma +
-						urb->actual_length;
-			}
-
-			dma->desired_mode = 0;
-#ifdef USE_MODE1
-			/* because of the issue below, mode 1 will
-			 * only rarely behave with correct semantics.
-			 */
-			if ((urb->transfer_flags &
-						URB_SHORT_NOT_OK)
-				&& (urb->transfer_buffer_length -
-						urb->actual_length)
-					> qh->maxpacket)
-				dma->desired_mode = 1;
-			if (rx_count < hw_ep->max_packet_sz_rx) {
-				length = rx_count;
-				dma->desired_mode = 0;
-			} else {
-				length = urb->transfer_buffer_length;
-			}
-#endif
-
-/* Disadvantage of using mode 1:
- *	It's basically usable only for mass storage class; essentially all
- *	other protocols also terminate transfers on short packets.
- *
- * Details:
- *	An extra IN token is sent at the end of the transfer (due to AUTOREQ)
- *	If you try to use mode 1 for (transfer_buffer_length - 512), and try
- *	to use the extra IN token to grab the last packet using mode 0, then
- *	the problem is that you cannot be sure when the device will send the
- *	last packet and RxPktRdy set. Sometimes the packet is recd too soon
- *	such that it gets lost when RxCSR is re-set at the end of the mode 1
- *	transfer, while sometimes it is recd just a little late so that if you
- *	try to configure for mode 0 soon after the mode 1 transfer is
- *	completed, you will find rxcount 0. Okay, so you might think why not
- *	wait for an interrupt when the pkt is recd. Well, you won't get any!
- */
-
-			val = musb_readw(epio, MUSB_RXCSR);
-			val &= ~MUSB_RXCSR_H_REQPKT;
-
-			if (dma->desired_mode == 0)
-				val &= ~MUSB_RXCSR_H_AUTOREQ;
+		if ((musb_dma_inventra(musb) || musb_dma_ux500(musb) ||
+		    musb_dma_cppi41(musb)) && dma) {
+			dev_dbg(hw_ep->musb->controller,
+				"RX%d count %d, buffer 0x%llx len %d/%d\n",
+				epnum, musb_readw(epio, MUSB_RXCOUNT),
+				(unsigned long long) urb->transfer_dma
+				+ urb->actual_length,
+				qh->offset,
+				urb->transfer_buffer_length);
+
+			done = musb_rx_dma_in_inventra_cppi41(c, hw_ep, qh,
+							      urb, xfer_len,
+							      iso_err);
+			if (done)
+				goto finish;
 			else
-				val |= MUSB_RXCSR_H_AUTOREQ;
-			val |= MUSB_RXCSR_DMAENAB;
-
-			/* autoclear shouldn't be set in high bandwidth */
-			if (qh->hb_mult == 1)
-				val |= MUSB_RXCSR_AUTOCLEAR;
-
-			musb_writew(epio, MUSB_RXCSR,
-				MUSB_RXCSR_H_WZC_BITS | val);
-
-			/* REVISIT if when actual_length != 0,
-			 * transfer_buffer_length needs to be
-			 * adjusted first...
-			 */
-			ret = c->channel_program(
-				dma, qh->maxpacket,
-				dma->desired_mode, buf, length);
-
-			if (!ret) {
-				c->channel_release(dma);
-				hw_ep->rx_channel = NULL;
-				dma = NULL;
-				val = musb_readw(epio, MUSB_RXCSR);
-				val &= ~(MUSB_RXCSR_DMAENAB
-					| MUSB_RXCSR_H_AUTOREQ
-					| MUSB_RXCSR_AUTOCLEAR);
-				musb_writew(epio, MUSB_RXCSR, val);
-			}
+				dev_err(musb->controller, "error: rx_dma failed\n");
 		}
-#endif	/* Mentor DMA */
 
 		if (!dma) {
 			unsigned int received_len;
@@ -2512,6 +2621,7 @@ static void musb_free_temp_buffer(struct urb *urb)
 {
 	enum dma_data_direction dir;
 	struct musb_temp_buffer *temp;
+	size_t length;
 
 	if (!(urb->transfer_flags & URB_ALIGNED_TEMP_BUFFER))
 		return;
@@ -2522,8 +2632,12 @@ static void musb_free_temp_buffer(struct urb *urb)
 			    data);
 
 	if (dir == DMA_FROM_DEVICE) {
-		memcpy(temp->old_xfer_buffer, temp->data,
-		       urb->transfer_buffer_length);
+		if (usb_pipeisoc(urb->pipe))
+			length = urb->transfer_buffer_length;
+		else
+			length = urb->actual_length;
+
+		memcpy(temp->old_xfer_buffer, temp->data, length);
 	}
 	urb->transfer_buffer = temp->old_xfer_buffer;
 	kfree(temp->kmalloc_ptr);
diff --git a/drivers/usb/musb/musb_io.h b/drivers/usb/musb/musb_io.h
index 8a57a6f4b3a6..17a80ae20674 100644
--- a/drivers/usb/musb/musb_io.h
+++ b/drivers/usb/musb/musb_io.h
@@ -47,6 +47,7 @@
  * @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
+ * @busctl_offset: platform specific function to get busctl offset
  */
 struct musb_io {
 	u32	quirks;
@@ -55,6 +56,7 @@ struct musb_io {
 	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);
+	u32	(*busctl_offset)(u8 epnum, u16 offset);
 };
 
 /* Do not add new entries here, add them the struct musb_io instead */
diff --git a/drivers/usb/musb/musb_regs.h b/drivers/usb/musb/musb_regs.h
index 11f0be07491e..cff5bcf0d00f 100644
--- a/drivers/usb/musb/musb_regs.h
+++ b/drivers/usb/musb/musb_regs.h
@@ -300,9 +300,6 @@
 #define MUSB_RXHUBADDR		0x06
 #define MUSB_RXHUBPORT		0x07
 
-#define MUSB_BUSCTL_OFFSET(_epnum, _offset) \
-	(0x80 + (8*(_epnum)) + (_offset))
-
 static inline void musb_write_txfifosz(void __iomem *mbase, u8 c_size)
 {
 	musb_writeb(mbase, MUSB_TXFIFOSZ, c_size);
@@ -364,78 +361,84 @@ static inline u16 musb_read_hwvers(void __iomem *mbase)
 	return musb_readw(mbase, MUSB_HWVERS);
 }
 
-static inline void __iomem *musb_read_target_reg_base(u8 i, void __iomem *mbase)
-{
-	return (MUSB_BUSCTL_OFFSET(i, 0) + mbase);
-}
-
-static inline void musb_write_rxfunaddr(void __iomem *ep_target_regs,
+static inline void musb_write_rxfunaddr(struct musb *musb, u8 epnum,
 		u8 qh_addr_reg)
 {
-	musb_writeb(ep_target_regs, MUSB_RXFUNCADDR, qh_addr_reg);
+	musb_writeb(musb->mregs,
+		    musb->io.busctl_offset(epnum, MUSB_RXFUNCADDR),
+		    qh_addr_reg);
 }
 
-static inline void musb_write_rxhubaddr(void __iomem *ep_target_regs,
+static inline void musb_write_rxhubaddr(struct musb *musb, u8 epnum,
 		u8 qh_h_addr_reg)
 {
-	musb_writeb(ep_target_regs, MUSB_RXHUBADDR, qh_h_addr_reg);
+	musb_writeb(musb->mregs, musb->io.busctl_offset(epnum, MUSB_RXHUBADDR),
+			qh_h_addr_reg);
 }
 
-static inline void musb_write_rxhubport(void __iomem *ep_target_regs,
+static inline void musb_write_rxhubport(struct musb *musb, u8 epnum,
 		u8 qh_h_port_reg)
 {
-	musb_writeb(ep_target_regs, MUSB_RXHUBPORT, qh_h_port_reg);
+	musb_writeb(musb->mregs, musb->io.busctl_offset(epnum, MUSB_RXHUBPORT),
+			qh_h_port_reg);
 }
 
-static inline void  musb_write_txfunaddr(void __iomem *mbase, u8 epnum,
+static inline void musb_write_txfunaddr(struct musb *musb, u8 epnum,
 		u8 qh_addr_reg)
 {
-	musb_writeb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXFUNCADDR),
-			qh_addr_reg);
+	musb_writeb(musb->mregs,
+		    musb->io.busctl_offset(epnum, MUSB_TXFUNCADDR),
+		    qh_addr_reg);
 }
 
-static inline void  musb_write_txhubaddr(void __iomem *mbase, u8 epnum,
+static inline void musb_write_txhubaddr(struct musb *musb, u8 epnum,
 		u8 qh_addr_reg)
 {
-	musb_writeb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXHUBADDR),
+	musb_writeb(musb->mregs, musb->io.busctl_offset(epnum, MUSB_TXHUBADDR),
 			qh_addr_reg);
 }
 
-static inline void  musb_write_txhubport(void __iomem *mbase, u8 epnum,
+static inline void musb_write_txhubport(struct musb *musb, u8 epnum,
 		u8 qh_h_port_reg)
 {
-	musb_writeb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXHUBPORT),
+	musb_writeb(musb->mregs, musb->io.busctl_offset(epnum, MUSB_TXHUBPORT),
 			qh_h_port_reg);
 }
 
-static inline u8 musb_read_rxfunaddr(void __iomem *mbase, u8 epnum)
+static inline u8 musb_read_rxfunaddr(struct musb *musb, u8 epnum)
 {
-	return musb_readb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_RXFUNCADDR));
+	return musb_readb(musb->mregs,
+			  musb->io.busctl_offset(epnum, MUSB_RXFUNCADDR));
 }
 
-static inline u8 musb_read_rxhubaddr(void __iomem *mbase, u8 epnum)
+static inline u8 musb_read_rxhubaddr(struct musb *musb, u8 epnum)
 {
-	return musb_readb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_RXHUBADDR));
+	return musb_readb(musb->mregs,
+			  musb->io.busctl_offset(epnum, MUSB_RXHUBADDR));
 }
 
-static inline u8 musb_read_rxhubport(void __iomem *mbase, u8 epnum)
+static inline u8 musb_read_rxhubport(struct musb *musb, u8 epnum)
 {
-	return musb_readb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_RXHUBPORT));
+	return musb_readb(musb->mregs,
+			  musb->io.busctl_offset(epnum, MUSB_RXHUBPORT));
 }
 
-static inline u8  musb_read_txfunaddr(void __iomem *mbase, u8 epnum)
+static inline u8 musb_read_txfunaddr(struct musb *musb, u8 epnum)
 {
-	return musb_readb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXFUNCADDR));
+	return musb_readb(musb->mregs,
+			  musb->io.busctl_offset(epnum, MUSB_TXFUNCADDR));
 }
 
-static inline u8  musb_read_txhubaddr(void __iomem *mbase, u8 epnum)
+static inline u8 musb_read_txhubaddr(struct musb *musb, u8 epnum)
 {
-	return musb_readb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXHUBADDR));
+	return musb_readb(musb->mregs,
+			  musb->io.busctl_offset(epnum, MUSB_TXHUBADDR));
 }
 
-static inline u8  musb_read_txhubport(void __iomem *mbase, u8 epnum)
+static inline u8 musb_read_txhubport(struct musb *musb, u8 epnum)
 {
-	return musb_readb(mbase, MUSB_BUSCTL_OFFSET(epnum, MUSB_TXHUBPORT));
+	return musb_readb(musb->mregs,
+			  musb->io.busctl_offset(epnum, MUSB_TXHUBPORT));
 }
 
 #else /* CONFIG_BLACKFIN */
@@ -556,22 +559,17 @@ static inline u16 musb_read_hwvers(void __iomem *mbase)
 	return MUSB_HWVERS_1900;
 }
 
-static inline void __iomem *musb_read_target_reg_base(u8 i, void __iomem *mbase)
-{
-	return NULL;
-}
-
-static inline void musb_write_rxfunaddr(void __iomem *ep_target_regs,
+static inline void musb_write_rxfunaddr(void __iomem *mbase, u8 epnum,
 		u8 qh_addr_req)
 {
 }
 
-static inline void musb_write_rxhubaddr(void __iomem *ep_target_regs,
+static inline void musb_write_rxhubaddr(void __iomem *mbase, u8 epnum,
 		u8 qh_h_addr_reg)
 {
 }
 
-static inline void musb_write_rxhubport(void __iomem *ep_target_regs,
+static inline void musb_write_rxhubport(void __iomem *mbase, u8 epnum,
 		u8 qh_h_port_reg)
 {
 }
diff --git a/drivers/usb/musb/musb_virthub.c b/drivers/usb/musb/musb_virthub.c
index 86c4b533e90b..30842bc195f5 100644
--- a/drivers/usb/musb/musb_virthub.c
+++ b/drivers/usb/musb/musb_virthub.c
@@ -195,8 +195,10 @@ void musb_port_reset(struct musb *musb, bool do_reset)
 				      msecs_to_jiffies(50));
 	} else {
 		dev_dbg(musb->controller, "root port reset stopped\n");
+		musb_platform_pre_root_reset_end(musb);
 		musb_writeb(mbase, MUSB_POWER,
 				power & ~MUSB_POWER_RESET);
+		musb_platform_post_root_reset_end(musb);
 
 		power = musb_readb(mbase, MUSB_POWER);
 		if (power & MUSB_POWER_HSMODE) {
diff --git a/drivers/usb/musb/musbhsdma.c b/drivers/usb/musb/musbhsdma.c
index ab7ec09a8afe..7539c3188ffc 100644
--- a/drivers/usb/musb/musbhsdma.c
+++ b/drivers/usb/musb/musbhsdma.c
@@ -357,7 +357,7 @@ done:
 	return retval;
 }
 
-void dma_controller_destroy(struct dma_controller *c)
+void musbhs_dma_controller_destroy(struct dma_controller *c)
 {
 	struct musb_dma_controller *controller = container_of(c,
 			struct musb_dma_controller, controller);
@@ -369,8 +369,10 @@ void dma_controller_destroy(struct dma_controller *c)
 
 	kfree(controller);
 }
+EXPORT_SYMBOL_GPL(musbhs_dma_controller_destroy);
 
-struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *base)
+struct dma_controller *musbhs_dma_controller_create(struct musb *musb,
+						    void __iomem *base)
 {
 	struct musb_dma_controller *controller;
 	struct device *dev = musb->controller;
@@ -398,7 +400,7 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *ba
 	if (request_irq(irq, dma_controller_irq, 0,
 			dev_name(musb->controller), &controller->controller)) {
 		dev_err(dev, "request_irq %d failed!\n", irq);
-		dma_controller_destroy(&controller->controller);
+		musb_dma_controller_destroy(&controller->controller);
 
 		return NULL;
 	}
@@ -407,3 +409,4 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *ba
 
 	return &controller->controller;
 }
+EXPORT_SYMBOL_GPL(musbhs_dma_controller_create);
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index cc752d8c7773..70f2b8a2e6cf 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -493,6 +493,11 @@ static int omap2430_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops omap2430_ops = {
+	.quirks		= MUSB_DMA_INVENTRA,
+#ifdef CONFIG_USB_INVENTRA_DMA
+	.dma_init	= musbhs_dma_controller_create,
+	.dma_exit	= musbhs_dma_controller_destroy,
+#endif
 	.init		= omap2430_musb_init,
 	.exit		= omap2430_musb_exit,
 
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index 3a5ffd575438..df7c9f46be54 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -890,7 +890,7 @@ static irqreturn_t tusb_musb_interrupt(int irq, void *__hci)
 
 		dev_dbg(musb->controller, "DMA IRQ %08x\n", dma_src);
 		real_dma_src = ~real_dma_src & dma_src;
-		if (tusb_dma_omap() && real_dma_src) {
+		if (tusb_dma_omap(musb) && real_dma_src) {
 			int	tx_source = (real_dma_src & 0xffff);
 			int	i;
 
@@ -1181,7 +1181,7 @@ static int tusb_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops tusb_ops = {
-	.quirks		= MUSB_IN_TUSB,
+	.quirks		= MUSB_DMA_TUSB_OMAP | MUSB_IN_TUSB,
 	.init		= tusb_musb_init,
 	.exit		= tusb_musb_exit,
 
@@ -1192,6 +1192,10 @@ static const struct musb_platform_ops tusb_ops = {
 	.writeb		= tusb_writeb,
 	.read_fifo	= tusb_read_fifo,
 	.write_fifo	= tusb_write_fifo,
+#ifdef CONFIG_USB_TUSB_OMAP_DMA
+	.dma_init	= tusb_dma_controller_create,
+	.dma_exit	= tusb_dma_controller_destroy,
+#endif
 	.enable		= tusb_musb_enable,
 	.disable	= tusb_musb_disable,
 
diff --git a/drivers/usb/musb/tusb6010.h b/drivers/usb/musb/tusb6010.h
index aec86c86ce32..72cdad23ced9 100644
--- a/drivers/usb/musb/tusb6010.h
+++ b/drivers/usb/musb/tusb6010.h
@@ -12,12 +12,6 @@
 #ifndef __TUSB6010_H__
 #define __TUSB6010_H__
 
-#ifdef CONFIG_USB_TUSB_OMAP_DMA
-#define tusb_dma_omap()			1
-#else
-#define tusb_dma_omap()			0
-#endif
-
 /* VLYNQ control register. 32-bit at offset 0x000 */
 #define TUSB_VLYNQ_CTRL			0x004
 
diff --git a/drivers/usb/musb/tusb6010_omap.c b/drivers/usb/musb/tusb6010_omap.c
index 3ce152c0408e..4c82077da475 100644
--- a/drivers/usb/musb/tusb6010_omap.c
+++ b/drivers/usb/musb/tusb6010_omap.c
@@ -625,7 +625,7 @@ static void tusb_omap_dma_release(struct dma_channel *channel)
 	channel = NULL;
 }
 
-void dma_controller_destroy(struct dma_controller *c)
+void tusb_dma_controller_destroy(struct dma_controller *c)
 {
 	struct tusb_omap_dma	*tusb_dma;
 	int			i;
@@ -644,8 +644,10 @@ void dma_controller_destroy(struct dma_controller *c)
 
 	kfree(tusb_dma);
 }
+EXPORT_SYMBOL_GPL(tusb_dma_controller_destroy);
 
-struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *base)
+struct dma_controller *
+tusb_dma_controller_create(struct musb *musb, void __iomem *base)
 {
 	void __iomem		*tbase = musb->ctrl_base;
 	struct tusb_omap_dma	*tusb_dma;
@@ -701,7 +703,8 @@ struct dma_controller *dma_controller_create(struct musb *musb, void __iomem *ba
 	return &tusb_dma->controller;
 
 cleanup:
-	dma_controller_destroy(&tusb_dma->controller);
+	musb_dma_controller_destroy(&tusb_dma->controller);
 out:
 	return NULL;
 }
+EXPORT_SYMBOL_GPL(tusb_dma_controller_create);
diff --git a/drivers/usb/musb/ux500.c b/drivers/usb/musb/ux500.c
index abf72728825f..39168fe9b406 100644
--- a/drivers/usb/musb/ux500.c
+++ b/drivers/usb/musb/ux500.c
@@ -188,7 +188,11 @@ static int ux500_musb_exit(struct musb *musb)
 }
 
 static const struct musb_platform_ops ux500_ops = {
-	.quirks		= MUSB_INDEXED_EP,
+	.quirks		= MUSB_DMA_UX500 | MUSB_INDEXED_EP,
+#ifdef CONFIG_USB_UX500_DMA
+	.dma_init	= ux500_dma_controller_create,
+	.dma_exit	= ux500_dma_controller_destroy,
+#endif
 	.init		= ux500_musb_init,
 	.exit		= ux500_musb_exit,
 	.fifo_mode	= 5,
@@ -338,7 +342,7 @@ static int ux500_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ux500_suspend(struct device *dev)
 {
 	struct ux500_glue	*glue = dev_get_drvdata(dev);
diff --git a/drivers/usb/musb/ux500_dma.c b/drivers/usb/musb/ux500_dma.c
index e93845c26bdb..d0b6a1cd7f62 100644
--- a/drivers/usb/musb/ux500_dma.c
+++ b/drivers/usb/musb/ux500_dma.c
@@ -359,7 +359,7 @@ static int ux500_dma_controller_start(struct ux500_dma_controller *controller)
 	return 0;
 }
 
-void dma_controller_destroy(struct dma_controller *c)
+void ux500_dma_controller_destroy(struct dma_controller *c)
 {
 	struct ux500_dma_controller *controller = container_of(c,
 			struct ux500_dma_controller, controller);
@@ -367,9 +367,10 @@ void dma_controller_destroy(struct dma_controller *c)
 	ux500_dma_controller_stop(controller);
 	kfree(controller);
 }
+EXPORT_SYMBOL_GPL(ux500_dma_controller_destroy);
 
-struct dma_controller *dma_controller_create(struct musb *musb,
-					void __iomem *base)
+struct dma_controller *
+ux500_dma_controller_create(struct musb *musb, void __iomem *base)
 {
 	struct ux500_dma_controller *controller;
 	struct platform_device *pdev = to_platform_device(musb->controller);
@@ -407,3 +408,4 @@ plat_get_fail:
 kzalloc_fail:
 	return NULL;
 }
+EXPORT_SYMBOL_GPL(ux500_dma_controller_create);
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index 2175678e674e..869c0cfcad98 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -91,7 +91,7 @@ config TWL6030_USB
 
 config USB_GPIO_VBUS
 	tristate "GPIO based peripheral-only VBUS sensing 'transceiver'"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	select USB_PHY
 	help
 	  Provides simple GPIO VBUS sensing for controllers with an
@@ -141,6 +141,7 @@ config USB_MSM_OTG
 	tristate "Qualcomm on-chip USB OTG controller support"
 	depends on (USB || USB_GADGET) && (ARCH_QCOM || COMPILE_TEST)
 	depends on RESET_CONTROLLER
+	depends on EXTCON
 	select USB_PHY
 	help
 	  Enable this to support the USB OTG transceiver on Qualcomm chips. It
@@ -186,19 +187,6 @@ config USB_RCAR_PHY
 	  To compile this driver as a module, choose M here: the
 	  module will be called phy-rcar-usb.
 
-config USB_RCAR_GEN2_PHY
-	tristate "Renesas R-Car Gen2 USB PHY support"
-	depends on ARCH_R8A7790 || ARCH_R8A7791 || COMPILE_TEST
-	select USB_PHY
-	help
-	  Say Y here to add support for the Renesas R-Car Gen2 USB PHY driver.
-	  It is typically used to control internal USB PHY for USBHS,
-	  and to configure shared USB channels 0 and 2.
-	  This driver supports R8A7790 and R8A7791.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called phy-rcar-gen2-usb.
-
 config USB_ULPI
 	bool "Generic ULPI Transceiver Driver"
 	depends on ARM || ARM64
diff --git a/drivers/usb/phy/Makefile b/drivers/usb/phy/Makefile
index 75f2bba58c84..e36ab1d46d8b 100644
--- a/drivers/usb/phy/Makefile
+++ b/drivers/usb/phy/Makefile
@@ -23,7 +23,6 @@ obj-$(CONFIG_USB_MSM_OTG)		+= phy-msm-usb.o
 obj-$(CONFIG_USB_MV_OTG)		+= phy-mv-usb.o
 obj-$(CONFIG_USB_MXS_PHY)		+= phy-mxs-usb.o
 obj-$(CONFIG_USB_RCAR_PHY)		+= phy-rcar-usb.o
-obj-$(CONFIG_USB_RCAR_GEN2_PHY)		+= phy-rcar-gen2-usb.o
 obj-$(CONFIG_USB_ULPI)			+= phy-ulpi.o
 obj-$(CONFIG_USB_ULPI_VIEWPORT)		+= phy-ulpi-viewport.o
 obj-$(CONFIG_KEYSTONE_USB_PHY)		+= phy-keystone.o
diff --git a/drivers/usb/phy/phy-ab8500-usb.c b/drivers/usb/phy/phy-ab8500-usb.c
index 7225d526df04..0c912d3950a5 100644
--- a/drivers/usb/phy/phy-ab8500-usb.c
+++ b/drivers/usb/phy/phy-ab8500-usb.c
@@ -1179,7 +1179,7 @@ static int ab8500_usb_irq_setup(struct platform_device *pdev,
 		}
 		err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 				ab8500_usb_link_status_irq,
-				IRQF_NO_SUSPEND | IRQF_SHARED,
+				IRQF_NO_SUSPEND | IRQF_SHARED | IRQF_ONESHOT,
 				"usb-link-status", ab);
 		if (err < 0) {
 			dev_err(ab->dev, "request_irq failed for link status irq\n");
@@ -1195,7 +1195,7 @@ static int ab8500_usb_irq_setup(struct platform_device *pdev,
 		}
 		err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 				ab8500_usb_disconnect_irq,
-				IRQF_NO_SUSPEND | IRQF_SHARED,
+				IRQF_NO_SUSPEND | IRQF_SHARED | IRQF_ONESHOT,
 				"usb-id-fall", ab);
 		if (err < 0) {
 			dev_err(ab->dev, "request_irq failed for ID fall irq\n");
@@ -1211,7 +1211,7 @@ static int ab8500_usb_irq_setup(struct platform_device *pdev,
 		}
 		err = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 				ab8500_usb_disconnect_irq,
-				IRQF_NO_SUSPEND | IRQF_SHARED,
+				IRQF_NO_SUSPEND | IRQF_SHARED | IRQF_ONESHOT,
 				"usb-vbus-fall", ab);
 		if (err < 0) {
 			dev_err(ab->dev, "request_irq failed for Vbus fall irq\n");
@@ -1504,7 +1504,7 @@ static int ab8500_usb_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id ab8500_usb_devtype[] = {
+static const struct platform_device_id ab8500_usb_devtype[] = {
 	{ .name = "ab8500-usb", },
 	{ .name = "ab8540-usb", },
 	{ .name = "ab9540-usb", },
diff --git a/drivers/usb/phy/phy-msm-usb.c b/drivers/usb/phy/phy-msm-usb.c
index c9156beeadef..00c49bb1bd29 100644
--- a/drivers/usb/phy/phy-msm-usb.c
+++ b/drivers/usb/phy/phy-msm-usb.c
@@ -240,8 +240,14 @@ static void ulpi_init(struct msm_otg *motg)
 static int msm_phy_notify_disconnect(struct usb_phy *phy,
 				   enum usb_device_speed speed)
 {
+	struct msm_otg *motg = container_of(phy, struct msm_otg, phy);
 	int val;
 
+	if (motg->manual_pullup) {
+		val = ULPI_MISC_A_VBUSVLDEXT | ULPI_MISC_A_VBUSVLDEXTSEL;
+		usb_phy_io_write(phy, val, ULPI_CLR(ULPI_MISC_A));
+	}
+
 	/*
 	 * Put the transceiver in non-driving mode. Otherwise host
 	 * may not detect soft-disconnection.
@@ -422,6 +428,24 @@ static int msm_phy_init(struct usb_phy *phy)
 		ulpi_write(phy, ulpi_val, ULPI_USB_INT_EN_FALL);
 	}
 
+	if (motg->manual_pullup) {
+		val = ULPI_MISC_A_VBUSVLDEXTSEL | ULPI_MISC_A_VBUSVLDEXT;
+		ulpi_write(phy, val, ULPI_SET(ULPI_MISC_A));
+
+		val = readl(USB_GENCONFIG_2);
+		val |= GENCONFIG_2_SESS_VLD_CTRL_EN;
+		writel(val, USB_GENCONFIG_2);
+
+		val = readl(USB_USBCMD);
+		val |= USBCMD_SESS_VLD_CTRL;
+		writel(val, USB_USBCMD);
+
+		val = ulpi_read(phy, ULPI_FUNC_CTRL);
+		val &= ~ULPI_FUNC_CTRL_OPMODE_MASK;
+		val |= ULPI_FUNC_CTRL_OPMODE_NORMAL;
+		ulpi_write(phy, val, ULPI_FUNC_CTRL);
+	}
+
 	if (motg->phy_number)
 		writel(readl(USB_PHY_CTRL2) | BIT(16), USB_PHY_CTRL2);
 
@@ -1436,9 +1460,42 @@ static const struct of_device_id msm_otg_dt_match[] = {
 };
 MODULE_DEVICE_TABLE(of, msm_otg_dt_match);
 
+static int msm_otg_vbus_notifier(struct notifier_block *nb, unsigned long event,
+				void *ptr)
+{
+	struct msm_usb_cable *vbus = container_of(nb, struct msm_usb_cable, nb);
+	struct msm_otg *motg = container_of(vbus, struct msm_otg, vbus);
+
+	if (event)
+		set_bit(B_SESS_VLD, &motg->inputs);
+	else
+		clear_bit(B_SESS_VLD, &motg->inputs);
+
+	schedule_work(&motg->sm_work);
+
+	return NOTIFY_DONE;
+}
+
+static int msm_otg_id_notifier(struct notifier_block *nb, unsigned long event,
+				void *ptr)
+{
+	struct msm_usb_cable *id = container_of(nb, struct msm_usb_cable, nb);
+	struct msm_otg *motg = container_of(id, struct msm_otg, id);
+
+	if (event)
+		clear_bit(ID, &motg->inputs);
+	else
+		set_bit(ID, &motg->inputs);
+
+	schedule_work(&motg->sm_work);
+
+	return NOTIFY_DONE;
+}
+
 static int msm_otg_read_dt(struct platform_device *pdev, struct msm_otg *motg)
 {
 	struct msm_otg_platform_data *pdata;
+	struct extcon_dev *ext_id, *ext_vbus;
 	const struct of_device_id *id;
 	struct device_node *node = pdev->dev.of_node;
 	struct property *prop;
@@ -1487,6 +1544,54 @@ static int msm_otg_read_dt(struct platform_device *pdev, struct msm_otg *motg)
 		motg->vdd_levels[VDD_LEVEL_MAX] = tmp[VDD_LEVEL_MAX];
 	}
 
+	motg->manual_pullup = of_property_read_bool(node, "qcom,manual-pullup");
+
+	ext_id = ERR_PTR(-ENODEV);
+	ext_vbus = ERR_PTR(-ENODEV);
+	if (of_property_read_bool(node, "extcon")) {
+
+		/* Each one of them is not mandatory */
+		ext_vbus = extcon_get_edev_by_phandle(&pdev->dev, 0);
+		if (IS_ERR(ext_vbus) && PTR_ERR(ext_vbus) != -ENODEV)
+			return PTR_ERR(ext_vbus);
+
+		ext_id = extcon_get_edev_by_phandle(&pdev->dev, 1);
+		if (IS_ERR(ext_id) && PTR_ERR(ext_id) != -ENODEV)
+			return PTR_ERR(ext_id);
+	}
+
+	if (!IS_ERR(ext_vbus)) {
+		motg->vbus.nb.notifier_call = msm_otg_vbus_notifier;
+		ret = extcon_register_interest(&motg->vbus.conn, ext_vbus->name,
+					       "USB", &motg->vbus.nb);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "register VBUS notifier failed\n");
+			return ret;
+		}
+
+		ret = extcon_get_cable_state(ext_vbus, "USB");
+		if (ret)
+			set_bit(B_SESS_VLD, &motg->inputs);
+		else
+			clear_bit(B_SESS_VLD, &motg->inputs);
+	}
+
+	if (!IS_ERR(ext_id)) {
+		motg->id.nb.notifier_call = msm_otg_id_notifier;
+		ret = extcon_register_interest(&motg->id.conn, ext_id->name,
+					       "USB-HOST", &motg->id.nb);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "register ID notifier failed\n");
+			return ret;
+		}
+
+		ret = extcon_get_cable_state(ext_id, "USB-HOST");
+		if (ret)
+			clear_bit(ID, &motg->inputs);
+		else
+			set_bit(ID, &motg->inputs);
+	}
+
 	prop = of_find_property(node, "qcom,phy-init-sequence", &len);
 	if (!prop || !len)
 		return 0;
@@ -1700,6 +1805,11 @@ static int msm_otg_remove(struct platform_device *pdev)
 	if (phy->otg->host || phy->otg->gadget)
 		return -EBUSY;
 
+	if (motg->id.conn.edev)
+		extcon_unregister_interest(&motg->id.conn);
+	if (motg->vbus.conn.edev)
+		extcon_unregister_interest(&motg->vbus.conn);
+
 	msm_otg_debugfs_cleanup();
 	cancel_delayed_work_sync(&motg->chg_work);
 	cancel_work_sync(&motg->sm_work);
diff --git a/drivers/usb/phy/phy-rcar-gen2-usb.c b/drivers/usb/phy/phy-rcar-gen2-usb.c
deleted file mode 100644
index f81800b6562a..000000000000
--- a/drivers/usb/phy/phy-rcar-gen2-usb.c
+++ /dev/null
@@ -1,246 +0,0 @@
-/*
- * Renesas R-Car Gen2 USB phy driver
- *
- * Copyright (C) 2013 Renesas Solutions Corp.
- * Copyright (C) 2013 Cogent Embedded, Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/clk.h>
-#include <linux/delay.h>
-#include <linux/io.h>
-#include <linux/module.h>
-#include <linux/platform_data/usb-rcar-gen2-phy.h>
-#include <linux/platform_device.h>
-#include <linux/spinlock.h>
-#include <linux/usb/otg.h>
-
-struct rcar_gen2_usb_phy_priv {
-	struct usb_phy phy;
-	void __iomem *base;
-	struct clk *clk;
-	spinlock_t lock;
-	int usecount;
-	u32 ugctrl2;
-};
-
-#define usb_phy_to_priv(p) container_of(p, struct rcar_gen2_usb_phy_priv, phy)
-
-/* Low Power Status register */
-#define USBHS_LPSTS_REG			0x02
-#define USBHS_LPSTS_SUSPM		(1 << 14)
-
-/* USB General control register */
-#define USBHS_UGCTRL_REG		0x80
-#define USBHS_UGCTRL_CONNECT		(1 << 2)
-#define USBHS_UGCTRL_PLLRESET		(1 << 0)
-
-/* USB General control register 2 */
-#define USBHS_UGCTRL2_REG		0x84
-#define USBHS_UGCTRL2_USB0_PCI		(1 << 4)
-#define USBHS_UGCTRL2_USB0_HS		(3 << 4)
-#define USBHS_UGCTRL2_USB2_PCI		(0 << 31)
-#define USBHS_UGCTRL2_USB2_SS		(1 << 31)
-
-/* USB General status register */
-#define USBHS_UGSTS_REG			0x88
-#define USBHS_UGSTS_LOCK		(1 << 8)
-
-/* Enable USBHS internal phy */
-static int __rcar_gen2_usbhs_phy_enable(void __iomem *base)
-{
-	u32 val;
-	int i;
-
-	/* USBHS PHY power on */
-	val = ioread32(base + USBHS_UGCTRL_REG);
-	val &= ~USBHS_UGCTRL_PLLRESET;
-	iowrite32(val, base + USBHS_UGCTRL_REG);
-
-	val = ioread16(base + USBHS_LPSTS_REG);
-	val |= USBHS_LPSTS_SUSPM;
-	iowrite16(val, base + USBHS_LPSTS_REG);
-
-	for (i = 0; i < 20; i++) {
-		val = ioread32(base + USBHS_UGSTS_REG);
-		if ((val & USBHS_UGSTS_LOCK) == USBHS_UGSTS_LOCK) {
-			val = ioread32(base + USBHS_UGCTRL_REG);
-			val |= USBHS_UGCTRL_CONNECT;
-			iowrite32(val, base + USBHS_UGCTRL_REG);
-			return 0;
-		}
-		udelay(1);
-	}
-
-	/* Timed out waiting for the PLL lock */
-	return -ETIMEDOUT;
-}
-
-/* Disable USBHS internal phy */
-static int __rcar_gen2_usbhs_phy_disable(void __iomem *base)
-{
-	u32 val;
-
-	/* USBHS PHY power off */
-	val = ioread32(base + USBHS_UGCTRL_REG);
-	val &= ~USBHS_UGCTRL_CONNECT;
-	iowrite32(val, base + USBHS_UGCTRL_REG);
-
-	val = ioread16(base + USBHS_LPSTS_REG);
-	val &= ~USBHS_LPSTS_SUSPM;
-	iowrite16(val, base + USBHS_LPSTS_REG);
-
-	val = ioread32(base + USBHS_UGCTRL_REG);
-	val |= USBHS_UGCTRL_PLLRESET;
-	iowrite32(val, base + USBHS_UGCTRL_REG);
-	return 0;
-}
-
-/* Setup USB channels */
-static void __rcar_gen2_usb_phy_init(struct rcar_gen2_usb_phy_priv *priv)
-{
-	u32 val;
-
-	clk_prepare_enable(priv->clk);
-
-	/* Set USB channels in the USBHS UGCTRL2 register */
-	val = ioread32(priv->base + USBHS_UGCTRL2_REG);
-	val &= ~(USBHS_UGCTRL2_USB0_HS | USBHS_UGCTRL2_USB2_SS);
-	val |= priv->ugctrl2;
-	iowrite32(val, priv->base + USBHS_UGCTRL2_REG);
-}
-
-/* Shutdown USB channels */
-static void __rcar_gen2_usb_phy_shutdown(struct rcar_gen2_usb_phy_priv *priv)
-{
-	__rcar_gen2_usbhs_phy_disable(priv->base);
-	clk_disable_unprepare(priv->clk);
-}
-
-static int rcar_gen2_usb_phy_set_suspend(struct usb_phy *phy, int suspend)
-{
-	struct rcar_gen2_usb_phy_priv *priv = usb_phy_to_priv(phy);
-	unsigned long flags;
-	int retval;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	retval = suspend ? __rcar_gen2_usbhs_phy_disable(priv->base) :
-			   __rcar_gen2_usbhs_phy_enable(priv->base);
-	spin_unlock_irqrestore(&priv->lock, flags);
-	return retval;
-}
-
-static int rcar_gen2_usb_phy_init(struct usb_phy *phy)
-{
-	struct rcar_gen2_usb_phy_priv *priv = usb_phy_to_priv(phy);
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	/*
-	 * Enable the clock and setup USB channels
-	 * if it's the first user
-	 */
-	if (!priv->usecount++)
-		__rcar_gen2_usb_phy_init(priv);
-	spin_unlock_irqrestore(&priv->lock, flags);
-	return 0;
-}
-
-static void rcar_gen2_usb_phy_shutdown(struct usb_phy *phy)
-{
-	struct rcar_gen2_usb_phy_priv *priv = usb_phy_to_priv(phy);
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	if (!priv->usecount) {
-		dev_warn(phy->dev, "Trying to disable phy with 0 usecount\n");
-		goto out;
-	}
-
-	/* Disable everything if it's the last user */
-	if (!--priv->usecount)
-		__rcar_gen2_usb_phy_shutdown(priv);
-out:
-	spin_unlock_irqrestore(&priv->lock, flags);
-}
-
-static int rcar_gen2_usb_phy_probe(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-	struct rcar_gen2_phy_platform_data *pdata;
-	struct rcar_gen2_usb_phy_priv *priv;
-	struct resource *res;
-	void __iomem *base;
-	struct clk *clk;
-	int retval;
-
-	pdata = dev_get_platdata(dev);
-	if (!pdata) {
-		dev_err(dev, "No platform data\n");
-		return -EINVAL;
-	}
-
-	clk = devm_clk_get(dev, "usbhs");
-	if (IS_ERR(clk)) {
-		dev_err(dev, "Can't get the clock\n");
-		return PTR_ERR(clk);
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	spin_lock_init(&priv->lock);
-	priv->clk = clk;
-	priv->base = base;
-	priv->ugctrl2 = pdata->chan0_pci ?
-			USBHS_UGCTRL2_USB0_PCI : USBHS_UGCTRL2_USB0_HS;
-	priv->ugctrl2 |= pdata->chan2_pci ?
-			USBHS_UGCTRL2_USB2_PCI : USBHS_UGCTRL2_USB2_SS;
-	priv->phy.dev = dev;
-	priv->phy.label = dev_name(dev);
-	priv->phy.init = rcar_gen2_usb_phy_init;
-	priv->phy.shutdown = rcar_gen2_usb_phy_shutdown;
-	priv->phy.set_suspend = rcar_gen2_usb_phy_set_suspend;
-
-	retval = usb_add_phy_dev(&priv->phy);
-	if (retval < 0) {
-		dev_err(dev, "Failed to add USB phy\n");
-		return retval;
-	}
-
-	platform_set_drvdata(pdev, priv);
-
-	return retval;
-}
-
-static int rcar_gen2_usb_phy_remove(struct platform_device *pdev)
-{
-	struct rcar_gen2_usb_phy_priv *priv = platform_get_drvdata(pdev);
-
-	usb_remove_phy(&priv->phy);
-
-	return 0;
-}
-
-static struct platform_driver rcar_gen2_usb_phy_driver = {
-	.driver = {
-		.name = "usb_phy_rcar_gen2",
-	},
-	.probe = rcar_gen2_usb_phy_probe,
-	.remove = rcar_gen2_usb_phy_remove,
-};
-
-module_platform_driver(rcar_gen2_usb_phy_driver);
-
-MODULE_LICENSE("GPL v2");
-MODULE_DESCRIPTION("Renesas R-Car Gen2 USB phy");
-MODULE_AUTHOR("Valentine Barshak <valentine.barshak@cogentembedded.com>");
diff --git a/drivers/usb/phy/phy-tahvo.c b/drivers/usb/phy/phy-tahvo.c
index 845f658276b1..b40d6a87d694 100644
--- a/drivers/usb/phy/phy-tahvo.c
+++ b/drivers/usb/phy/phy-tahvo.c
@@ -60,10 +60,11 @@ struct tahvo_usb {
 	struct extcon_dev	extcon;
 };
 
-static const char *tahvo_cable[] = {
-	"USB-HOST",
-	"USB",
-	NULL,
+static const unsigned int tahvo_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+
+	EXTCON_NONE,
 };
 
 static ssize_t vbus_state_show(struct device *device,
@@ -401,7 +402,8 @@ static int tahvo_usb_probe(struct platform_device *pdev)
 	dev_set_drvdata(&pdev->dev, tu);
 
 	tu->irq = platform_get_irq(pdev, 0);
-	ret = request_threaded_irq(tu->irq, NULL, tahvo_usb_vbus_interrupt, 0,
+	ret = request_threaded_irq(tu->irq, NULL, tahvo_usb_vbus_interrupt,
+				   IRQF_ONESHOT,
 				   "tahvo-vbus", tu);
 	if (ret) {
 		dev_err(&pdev->dev, "could not register tahvo-vbus irq: %d\n",
diff --git a/drivers/usb/phy/phy.c b/drivers/usb/phy/phy.c
index d1cd6b50f520..98f75d2842b7 100644
--- a/drivers/usb/phy/phy.c
+++ b/drivers/usb/phy/phy.c
@@ -22,6 +22,11 @@ static LIST_HEAD(phy_list);
 static LIST_HEAD(phy_bind_list);
 static DEFINE_SPINLOCK(phy_lock);
 
+struct phy_devm {
+	struct usb_phy *phy;
+	struct notifier_block *nb;
+};
+
 static struct usb_phy *__usb_find_phy(struct list_head *list,
 	enum usb_phy_type type)
 {
@@ -79,6 +84,15 @@ static void devm_usb_phy_release(struct device *dev, void *res)
 	usb_put_phy(phy);
 }
 
+static void devm_usb_phy_release2(struct device *dev, void *_res)
+{
+	struct phy_devm *res = _res;
+
+	if (res->nb)
+		usb_unregister_notifier(res->phy, res->nb);
+	usb_put_phy(res->phy);
+}
+
 static int devm_usb_phy_match(struct device *dev, void *res, void *match_data)
 {
 	struct usb_phy **phy = res;
@@ -153,40 +167,30 @@ err0:
 EXPORT_SYMBOL_GPL(usb_get_phy);
 
 /**
- * devm_usb_get_phy_by_phandle - find the USB PHY by phandle
+ * devm_usb_get_phy_by_node - find the USB PHY by device_node
  * @dev - device that requests this phy
- * @phandle - name of the property holding the phy phandle value
- * @index - the index of the phy
+ * @node - the device_node for the phy device.
+ * @nb - a notifier_block to register with the phy.
  *
- * Returns the phy driver associated with the given phandle value,
+ * Returns the phy driver associated with the given device_node,
  * after getting a refcount to it, -ENODEV if there is no such phy or
- * -EPROBE_DEFER if there is a phandle to the phy, but the device is
- * not yet loaded. While at that, it also associates the device with
+ * -EPROBE_DEFER if the device is not yet loaded. While at that, it
+ * also associates the device with
  * the phy using devres. On driver detach, release function is invoked
  * on the devres data, then, devres data is freed.
  *
- * For use by USB host and peripheral drivers.
+ * For use by peripheral drivers for devices related to a phy,
+ * such as a charger.
  */
-struct usb_phy *devm_usb_get_phy_by_phandle(struct device *dev,
-	const char *phandle, u8 index)
+struct  usb_phy *devm_usb_get_phy_by_node(struct device *dev,
+					  struct device_node *node,
+					  struct notifier_block *nb)
 {
-	struct usb_phy	*phy = ERR_PTR(-ENOMEM), **ptr;
+	struct usb_phy	*phy = ERR_PTR(-ENOMEM);
+	struct phy_devm	*ptr;
 	unsigned long	flags;
-	struct device_node *node;
 
-	if (!dev->of_node) {
-		dev_dbg(dev, "device does not have a device node entry\n");
-		return ERR_PTR(-EINVAL);
-	}
-
-	node = of_parse_phandle(dev->of_node, phandle, index);
-	if (!node) {
-		dev_dbg(dev, "failed to get %s phandle in %s node\n", phandle,
-			dev->of_node->full_name);
-		return ERR_PTR(-ENODEV);
-	}
-
-	ptr = devres_alloc(devm_usb_phy_release, sizeof(*ptr), GFP_KERNEL);
+	ptr = devres_alloc(devm_usb_phy_release2, sizeof(*ptr), GFP_KERNEL);
 	if (!ptr) {
 		dev_dbg(dev, "failed to allocate memory for devres\n");
 		goto err0;
@@ -205,8 +209,10 @@ struct usb_phy *devm_usb_get_phy_by_phandle(struct device *dev,
 		devres_free(ptr);
 		goto err1;
 	}
-
-	*ptr = phy;
+	if (nb)
+		usb_register_notifier(phy, nb);
+	ptr->phy = phy;
+	ptr->nb = nb;
 	devres_add(dev, ptr);
 
 	get_device(phy->dev);
@@ -215,10 +221,47 @@ err1:
 	spin_unlock_irqrestore(&phy_lock, flags);
 
 err0:
-	of_node_put(node);
 
 	return phy;
 }
+EXPORT_SYMBOL_GPL(devm_usb_get_phy_by_node);
+
+/**
+ * devm_usb_get_phy_by_phandle - find the USB PHY by phandle
+ * @dev - device that requests this phy
+ * @phandle - name of the property holding the phy phandle value
+ * @index - the index of the phy
+ *
+ * Returns the phy driver associated with the given phandle value,
+ * after getting a refcount to it, -ENODEV if there is no such phy or
+ * -EPROBE_DEFER if there is a phandle to the phy, but the device is
+ * not yet loaded. While at that, it also associates the device with
+ * the phy using devres. On driver detach, release function is invoked
+ * on the devres data, then, devres data is freed.
+ *
+ * For use by USB host and peripheral drivers.
+ */
+struct usb_phy *devm_usb_get_phy_by_phandle(struct device *dev,
+	const char *phandle, u8 index)
+{
+	struct device_node *node;
+	struct usb_phy	*phy;
+
+	if (!dev->of_node) {
+		dev_dbg(dev, "device does not have a device node entry\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	node = of_parse_phandle(dev->of_node, phandle, index);
+	if (!node) {
+		dev_dbg(dev, "failed to get %s phandle in %s node\n", phandle,
+			dev->of_node->full_name);
+		return ERR_PTR(-ENODEV);
+	}
+	phy = devm_usb_get_phy_by_node(dev, node, NULL);
+	of_node_put(node);
+	return phy;
+}
 EXPORT_SYMBOL_GPL(devm_usb_get_phy_by_phandle);
 
 /**
diff --git a/drivers/usb/renesas_usbhs/common.c b/drivers/usb/renesas_usbhs/common.c
index 0f7e850fd4aa..e8bf40808b39 100644
--- a/drivers/usb/renesas_usbhs/common.c
+++ b/drivers/usb/renesas_usbhs/common.c
@@ -466,11 +466,15 @@ static int usbhsc_drvcllbck_notify_hotplug(struct platform_device *pdev)
 static const struct of_device_id usbhs_of_match[] = {
 	{
 		.compatible = "renesas,usbhs-r8a7790",
-		.data = (void *)USBHS_TYPE_R8A7790,
+		.data = (void *)USBHS_TYPE_RCAR_GEN2,
 	},
 	{
 		.compatible = "renesas,usbhs-r8a7791",
-		.data = (void *)USBHS_TYPE_R8A7791,
+		.data = (void *)USBHS_TYPE_RCAR_GEN2,
+	},
+	{
+		.compatible = "renesas,usbhs-r8a7794",
+		.data = (void *)USBHS_TYPE_RCAR_GEN2,
 	},
 	{ },
 };
@@ -497,14 +501,8 @@ static struct renesas_usbhs_platform_info *usbhs_parse_dt(struct device *dev)
 	if (gpio > 0)
 		dparam->enable_gpio = gpio;
 
-	switch (dparam->type) {
-	case USBHS_TYPE_R8A7790:
-	case USBHS_TYPE_R8A7791:
+	if (dparam->type == USBHS_TYPE_RCAR_GEN2)
 		dparam->has_usb_dmac = 1;
-		break;
-	default:
-		break;
-	}
 
 	return info;
 }
@@ -559,8 +557,7 @@ static int usbhs_probe(struct platform_device *pdev)
 	       sizeof(struct renesas_usbhs_driver_param));
 
 	switch (priv->dparam.type) {
-	case USBHS_TYPE_R8A7790:
-	case USBHS_TYPE_R8A7791:
+	case USBHS_TYPE_RCAR_GEN2:
 		priv->pfunc = usbhs_rcar2_ops;
 		if (!priv->dparam.pipe_type) {
 			priv->dparam.pipe_type = usbhsc_new_pipe_type;
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index 8597cf9cfceb..c0f5c652d272 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -611,6 +611,8 @@ struct usbhs_pkt_handle usbhs_fifo_pio_push_handler = {
 static int usbhsf_prepare_pop(struct usbhs_pkt *pkt, int *is_done)
 {
 	struct usbhs_pipe *pipe = pkt->pipe;
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
+	struct usbhs_fifo *fifo = usbhsf_get_cfifo(priv);
 
 	if (usbhs_pipe_is_busy(pipe))
 		return 0;
@@ -624,6 +626,9 @@ static int usbhsf_prepare_pop(struct usbhs_pkt *pkt, int *is_done)
 	usbhs_pipe_data_sequence(pipe, pkt->sequence);
 	pkt->sequence = -1; /* -1 sequence will be ignored */
 
+	if (usbhs_pipe_is_dcp(pipe))
+		usbhsf_fifo_clear(pipe, fifo);
+
 	usbhs_pipe_set_trans_count_if_bulk(pipe, pkt->length);
 	usbhs_pipe_enable(pipe);
 	usbhs_pipe_running(pipe, 1);
@@ -673,7 +678,14 @@ static int usbhsf_pio_try_pop(struct usbhs_pkt *pkt, int *is_done)
 		*is_done = 1;
 		usbhsf_rx_irq_ctrl(pipe, 0);
 		usbhs_pipe_running(pipe, 0);
-		usbhs_pipe_disable(pipe);	/* disable pipe first */
+		/*
+		 * If function mode, since this controller is possible to enter
+		 * Control Write status stage at this timing, this driver
+		 * should not disable the pipe. If such a case happens, this
+		 * controller is not able to complete the status stage.
+		 */
+		if (!usbhs_mod_is_host(priv) && !usbhs_pipe_is_dcp(pipe))
+			usbhs_pipe_disable(pipe);	/* disable pipe first */
 	}
 
 	/*
@@ -1227,15 +1239,21 @@ static void usbhsf_dma_init_dt(struct device *dev, struct usbhs_fifo *fifo,
 {
 	char name[16];
 
-	snprintf(name, sizeof(name), "tx%d", channel);
-	fifo->tx_chan = dma_request_slave_channel_reason(dev, name);
-	if (IS_ERR(fifo->tx_chan))
-		fifo->tx_chan = NULL;
-
-	snprintf(name, sizeof(name), "rx%d", channel);
-	fifo->rx_chan = dma_request_slave_channel_reason(dev, name);
-	if (IS_ERR(fifo->rx_chan))
-		fifo->rx_chan = NULL;
+	/*
+	 * To avoid complex handing for DnFIFOs, the driver uses each
+	 * DnFIFO as TX or RX direction (not bi-direction).
+	 * So, the driver uses odd channels for TX, even channels for RX.
+	 */
+	snprintf(name, sizeof(name), "ch%d", channel);
+	if (channel & 1) {
+		fifo->tx_chan = dma_request_slave_channel_reason(dev, name);
+		if (IS_ERR(fifo->tx_chan))
+			fifo->tx_chan = NULL;
+	} else {
+		fifo->rx_chan = dma_request_slave_channel_reason(dev, name);
+		if (IS_ERR(fifo->rx_chan))
+			fifo->rx_chan = NULL;
+	}
 }
 
 static void usbhsf_dma_init(struct usbhs_priv *priv, struct usbhs_fifo *fifo,
diff --git a/drivers/usb/renesas_usbhs/fifo.h b/drivers/usb/renesas_usbhs/fifo.h
index 04d3f8abad9e..c7d9b86d51bf 100644
--- a/drivers/usb/renesas_usbhs/fifo.h
+++ b/drivers/usb/renesas_usbhs/fifo.h
@@ -44,10 +44,11 @@ struct usbhs_fifo_info {
 	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)))
+#define usbhs_for_each_dfifo(priv, dfifo, i)			\
+	for ((i) = 0;						\
+	     ((i) < USBHS_MAX_NUM_DFIFO) &&			\
+		     ((dfifo) = usbhsf_get_dnfifo(priv, (i)));	\
+	     (i)++)
 
 struct usbhs_pkt_handle;
 struct usbhs_pkt {
diff --git a/drivers/usb/renesas_usbhs/mod.c b/drivers/usb/renesas_usbhs/mod.c
index 9a705b15b3a1..d4be5d594896 100644
--- a/drivers/usb/renesas_usbhs/mod.c
+++ b/drivers/usb/renesas_usbhs/mod.c
@@ -218,10 +218,14 @@ static int usbhs_status_get_each_irq(struct usbhs_priv *priv,
 	/********************  spin lock ********************/
 	usbhs_lock(priv, flags);
 	state->intsts0 = usbhs_read(priv, INTSTS0);
-	state->intsts1 = usbhs_read(priv, INTSTS1);
-
 	intenb0 = usbhs_read(priv, INTENB0);
-	intenb1 = usbhs_read(priv, INTENB1);
+
+	if (usbhs_mod_is_host(priv)) {
+		state->intsts1 = usbhs_read(priv, INTSTS1);
+		intenb1 = usbhs_read(priv, INTENB1);
+	} else {
+		state->intsts1 = intenb1 = 0;
+	}
 
 	/* mask */
 	if (mod) {
@@ -275,7 +279,8 @@ static irqreturn_t usbhs_interrupt(int irq, void *data)
 	 *	   - Function :: VALID bit should 0
 	 */
 	usbhs_write(priv, INTSTS0, ~irq_state.intsts0 & INTSTS0_MAGIC);
-	usbhs_write(priv, INTSTS1, ~irq_state.intsts1 & INTSTS1_MAGIC);
+	if (usbhs_mod_is_host(priv))
+		usbhs_write(priv, INTSTS1, ~irq_state.intsts1 & INTSTS1_MAGIC);
 
 	usbhs_write(priv, BRDYSTS, ~irq_state.brdysts);
 	usbhs_write(priv, NRDYSTS, ~irq_state.nrdysts);
@@ -303,19 +308,20 @@ static irqreturn_t usbhs_interrupt(int irq, void *data)
 	if (irq_state.intsts0 & BRDY)
 		usbhs_mod_call(priv, irq_ready, priv, &irq_state);
 
-	/* INTSTS1 */
-	if (irq_state.intsts1 & ATTCH)
-		usbhs_mod_call(priv, irq_attch, priv, &irq_state);
-
-	if (irq_state.intsts1 & DTCH)
-		usbhs_mod_call(priv, irq_dtch, priv, &irq_state);
+	if (usbhs_mod_is_host(priv)) {
+		/* INTSTS1 */
+		if (irq_state.intsts1 & ATTCH)
+			usbhs_mod_call(priv, irq_attch, priv, &irq_state);
 
-	if (irq_state.intsts1 & SIGN)
-		usbhs_mod_call(priv, irq_sign, priv, &irq_state);
+		if (irq_state.intsts1 & DTCH)
+			usbhs_mod_call(priv, irq_dtch, priv, &irq_state);
 
-	if (irq_state.intsts1 & SACK)
-		usbhs_mod_call(priv, irq_sack, priv, &irq_state);
+		if (irq_state.intsts1 & SIGN)
+			usbhs_mod_call(priv, irq_sign, priv, &irq_state);
 
+		if (irq_state.intsts1 & SACK)
+			usbhs_mod_call(priv, irq_sack, priv, &irq_state);
+	}
 	return IRQ_HANDLED;
 }
 
@@ -334,7 +340,8 @@ void usbhs_irq_callback_update(struct usbhs_priv *priv, struct usbhs_mod *mod)
 	 *  - update INTSTS0
 	 */
 	usbhs_write(priv, INTENB0, 0);
-	usbhs_write(priv, INTENB1, 0);
+	if (usbhs_mod_is_host(priv))
+		usbhs_write(priv, INTENB1, 0);
 
 	usbhs_write(priv, BEMPENB, 0);
 	usbhs_write(priv, BRDYENB, 0);
@@ -368,25 +375,27 @@ void usbhs_irq_callback_update(struct usbhs_priv *priv, struct usbhs_mod *mod)
 			intenb0 |= BRDYE;
 		}
 
-		/*
-		 * INTSTS1
-		 */
-		if (mod->irq_attch)
-			intenb1 |= ATTCHE;
+		if (usbhs_mod_is_host(priv)) {
+			/*
+			 * INTSTS1
+			 */
+			if (mod->irq_attch)
+				intenb1 |= ATTCHE;
 
-		if (mod->irq_dtch)
-			intenb1 |= DTCHE;
+			if (mod->irq_dtch)
+				intenb1 |= DTCHE;
 
-		if (mod->irq_sign)
-			intenb1 |= SIGNE;
+			if (mod->irq_sign)
+				intenb1 |= SIGNE;
 
-		if (mod->irq_sack)
-			intenb1 |= SACKE;
+			if (mod->irq_sack)
+				intenb1 |= SACKE;
+		}
 	}
 
 	if (intenb0)
 		usbhs_write(priv, INTENB0, intenb0);
 
-	if (intenb1)
+	if (usbhs_mod_is_host(priv) && intenb1)
 		usbhs_write(priv, INTENB1, intenb1);
 }
diff --git a/drivers/usb/serial/Kconfig b/drivers/usb/serial/Kconfig
index b7cf1982d1d9..56ecb8b5115d 100644
--- a/drivers/usb/serial/Kconfig
+++ b/drivers/usb/serial/Kconfig
@@ -171,7 +171,7 @@ config USB_SERIAL_FTDI_SIO
 	---help---
 	  Say Y here if you want to use a FTDI SIO single port USB to serial
 	  converter device. The implementation I have is called the USC-1000.
-	  This driver has also be tested with the 245 and 232 devices.
+	  This driver has also been tested with the 245 and 232 devices.
 
 	  See <http://ftdi-usb-sio.sourceforge.net/> for more
 	  information on this driver and the device.
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index 9031750e7404..ffd739e31bfc 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -128,6 +128,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x10C4, 0x8946) }, /* Ketra N1 Wireless Interface */
 	{ USB_DEVICE(0x10C4, 0x8977) },	/* CEL MeshWorks DevKit Device */
 	{ USB_DEVICE(0x10C4, 0x8998) }, /* KCF Technologies PRN */
+	{ USB_DEVICE(0x10C4, 0x8A2A) }, /* HubZ dual ZigBee and Z-Wave dongle */
 	{ USB_DEVICE(0x10C4, 0xEA60) }, /* Silicon Labs factory default */
 	{ USB_DEVICE(0x10C4, 0xEA61) }, /* Silicon Labs factory default */
 	{ USB_DEVICE(0x10C4, 0xEA70) }, /* Silicon Labs factory default */
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 8eb68a31cab6..4c8b3b82103d 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -699,6 +699,7 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(XSENS_VID, XSENS_AWINDA_DONGLE_PID) },
 	{ USB_DEVICE(XSENS_VID, XSENS_AWINDA_STATION_PID) },
 	{ USB_DEVICE(XSENS_VID, XSENS_CONVERTER_PID) },
+	{ USB_DEVICE(XSENS_VID, XSENS_MTDEVBOARD_PID) },
 	{ USB_DEVICE(XSENS_VID, XSENS_MTW_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_OMNI1509) },
 	{ USB_DEVICE(MOBILITY_VID, MOBILITY_USB_SERIAL_PID) },
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index 4e4f46f3c89c..792e054126de 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -155,6 +155,7 @@
 #define XSENS_AWINDA_STATION_PID 0x0101
 #define XSENS_AWINDA_DONGLE_PID 0x0102
 #define XSENS_MTW_PID		0x0200	/* Xsens MTw */
+#define XSENS_MTDEVBOARD_PID	0x0300	/* Motion Tracker Development Board */
 #define XSENS_CONVERTER_PID	0xD00D	/* Xsens USB-serial converter */
 
 /* Xsens devices using FTDI VID */
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index e4473a9109cf..8ac9b55f05af 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -2301,17 +2301,14 @@ static int mos7840_port_probe(struct usb_serial_port *port)
 			goto error;
 		}
 
-		init_timer(&mos7840_port->led_timer1);
-		mos7840_port->led_timer1.function = mos7840_led_off;
+		setup_timer(&mos7840_port->led_timer1, mos7840_led_off,
+			    (unsigned long)mos7840_port);
 		mos7840_port->led_timer1.expires =
 			jiffies + msecs_to_jiffies(LED_ON_MS);
-		mos7840_port->led_timer1.data = (unsigned long)mos7840_port;
-
-		init_timer(&mos7840_port->led_timer2);
-		mos7840_port->led_timer2.function = mos7840_led_flag_off;
+		setup_timer(&mos7840_port->led_timer2, mos7840_led_flag_off,
+			    (unsigned long)mos7840_port);
 		mos7840_port->led_timer2.expires =
 			jiffies + msecs_to_jiffies(LED_OFF_MS);
-		mos7840_port->led_timer2.data = (unsigned long)mos7840_port;
 
 		/* Turn off LED */
 		mos7840_set_led_sync(port, MODEM_CONTROL_REGISTER, 0x0300);
diff --git a/drivers/usb/storage/alauda.c b/drivers/usb/storage/alauda.c
index 4b55ab66a534..171fa7d793bc 100644
--- a/drivers/usb/storage/alauda.c
+++ b/drivers/usb/storage/alauda.c
@@ -42,6 +42,9 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-alauda"
 
 MODULE_DESCRIPTION("Driver for Alauda-based card readers");
 MODULE_AUTHOR("Daniel Drake <dsd@gentoo.org>");
@@ -1232,6 +1235,8 @@ static int alauda_transport(struct scsi_cmnd *srb, struct us_data *us)
 	return USB_STOR_TRANSPORT_FAILED;
 }
 
+static struct scsi_host_template alauda_host_template;
+
 static int alauda_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -1239,7 +1244,8 @@ static int alauda_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - alauda_usb_ids) + alauda_unusual_dev_list);
+			(id - alauda_usb_ids) + alauda_unusual_dev_list,
+			&alauda_host_template);
 	if (result)
 		return result;
 
@@ -1253,7 +1259,7 @@ static int alauda_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver alauda_driver = {
-	.name =		"ums-alauda",
+	.name =		DRV_NAME,
 	.probe =	alauda_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -1266,4 +1272,4 @@ static struct usb_driver alauda_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(alauda_driver);
+module_usb_stor_driver(alauda_driver, alauda_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/cypress_atacb.c b/drivers/usb/storage/cypress_atacb.c
index b3466d1395f2..c80d3dec9a34 100644
--- a/drivers/usb/storage/cypress_atacb.c
+++ b/drivers/usb/storage/cypress_atacb.c
@@ -30,6 +30,8 @@
 #include "scsiglue.h"
 #include "debug.h"
 
+#define DRV_NAME "ums-cypress"
+
 MODULE_DESCRIPTION("SAT support for Cypress USB/ATA bridges with ATACB");
 MODULE_AUTHOR("Matthieu Castet <castet.matthieu@free.fr>");
 MODULE_LICENSE("GPL");
@@ -241,6 +243,7 @@ end:
 		srb->cmd_len = 12;
 }
 
+static struct scsi_host_template cypress_host_template;
 
 static int cypress_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
@@ -250,7 +253,8 @@ static int cypress_probe(struct usb_interface *intf,
 	struct usb_device *device;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - cypress_usb_ids) + cypress_unusual_dev_list);
+			(id - cypress_usb_ids) + cypress_unusual_dev_list,
+			&cypress_host_template);
 	if (result)
 		return result;
 
@@ -273,7 +277,7 @@ static int cypress_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver cypress_driver = {
-	.name =		"ums-cypress",
+	.name =		DRV_NAME,
 	.probe =	cypress_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -286,4 +290,4 @@ static struct usb_driver cypress_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(cypress_driver);
+module_usb_stor_driver(cypress_driver, cypress_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/datafab.c b/drivers/usb/storage/datafab.c
index 7b17c2169812..aa4f51944a4a 100644
--- a/drivers/usb/storage/datafab.c
+++ b/drivers/usb/storage/datafab.c
@@ -59,6 +59,9 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-datafab"
 
 MODULE_DESCRIPTION("Driver for Datafab USB Compact Flash reader");
 MODULE_AUTHOR("Jimmie Mayfield <mayfield+datafab@sackheads.org>");
@@ -721,6 +724,8 @@ static int datafab_transport(struct scsi_cmnd *srb, struct us_data *us)
 	return USB_STOR_TRANSPORT_FAILED;
 }
 
+static struct scsi_host_template datafab_host_template;
+
 static int datafab_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -728,7 +733,8 @@ static int datafab_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - datafab_usb_ids) + datafab_unusual_dev_list);
+			(id - datafab_usb_ids) + datafab_unusual_dev_list,
+			&datafab_host_template);
 	if (result)
 		return result;
 
@@ -742,7 +748,7 @@ static int datafab_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver datafab_driver = {
-	.name =		"ums-datafab",
+	.name =		DRV_NAME,
 	.probe =	datafab_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -755,4 +761,4 @@ static struct usb_driver datafab_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(datafab_driver);
+module_usb_stor_driver(datafab_driver, datafab_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/ene_ub6250.c b/drivers/usb/storage/ene_ub6250.c
index 56f782bef36b..f3cf4cecd2b7 100644
--- a/drivers/usb/storage/ene_ub6250.c
+++ b/drivers/usb/storage/ene_ub6250.c
@@ -28,6 +28,7 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
 
 #define SD_INIT1_FIRMWARE "ene-ub6250/sd_init1.bin"
 #define SD_INIT2_FIRMWARE "ene-ub6250/sd_init2.bin"
@@ -36,6 +37,8 @@
 #define MSP_RW_FIRMWARE "ene-ub6250/msp_rdwr.bin"
 #define MS_RW_FIRMWARE "ene-ub6250/ms_rdwr.bin"
 
+#define DRV_NAME "ums_eneub6250"
+
 MODULE_DESCRIPTION("Driver for ENE UB6250 reader");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(SD_INIT1_FIRMWARE);
@@ -2307,6 +2310,7 @@ static int ene_transport(struct scsi_cmnd *srb, struct us_data *us)
 	return 0;
 }
 
+static struct scsi_host_template ene_ub6250_host_template;
 
 static int ene_ub6250_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
@@ -2316,7 +2320,8 @@ static int ene_ub6250_probe(struct usb_interface *intf,
 	struct us_data *us;
 
 	result = usb_stor_probe1(&us, intf, id,
-		   (id - ene_ub6250_usb_ids) + ene_ub6250_unusual_dev_list);
+		   (id - ene_ub6250_usb_ids) + ene_ub6250_unusual_dev_list,
+		   &ene_ub6250_host_template);
 	if (result)
 		return result;
 
@@ -2404,7 +2409,7 @@ static int ene_ub6250_reset_resume(struct usb_interface *iface)
 #endif
 
 static struct usb_driver ene_ub6250_driver = {
-	.name =		"ums_eneub6250",
+	.name =		DRV_NAME,
 	.probe =	ene_ub6250_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -2417,4 +2422,4 @@ static struct usb_driver ene_ub6250_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(ene_ub6250_driver);
+module_usb_stor_driver(ene_ub6250_driver, ene_ub6250_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/freecom.c b/drivers/usb/storage/freecom.c
index ef16068b7087..3f2b08966b9d 100644
--- a/drivers/usb/storage/freecom.c
+++ b/drivers/usb/storage/freecom.c
@@ -34,6 +34,9 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-freecom"
 
 MODULE_DESCRIPTION("Driver for Freecom USB/IDE adaptor");
 MODULE_AUTHOR("David Brown <usb-storage@davidb.org>");
@@ -523,6 +526,8 @@ static void pdump(struct us_data *us, void *ibuffer, int length)
 }
 #endif
 
+static struct scsi_host_template freecom_host_template;
+
 static int freecom_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -530,7 +535,8 @@ static int freecom_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - freecom_usb_ids) + freecom_unusual_dev_list);
+			(id - freecom_usb_ids) + freecom_unusual_dev_list,
+			&freecom_host_template);
 	if (result)
 		return result;
 
@@ -544,7 +550,7 @@ static int freecom_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver freecom_driver = {
-	.name =		"ums-freecom",
+	.name =		DRV_NAME,
 	.probe =	freecom_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -557,4 +563,4 @@ static struct usb_driver freecom_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(freecom_driver);
+module_usb_stor_driver(freecom_driver, freecom_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/isd200.c b/drivers/usb/storage/isd200.c
index 076178645ba4..1bac215202d2 100644
--- a/drivers/usb/storage/isd200.c
+++ b/drivers/usb/storage/isd200.c
@@ -60,6 +60,8 @@
 #include "debug.h"
 #include "scsiglue.h"
 
+#define DRV_NAME "ums-isd200"
+
 MODULE_DESCRIPTION("Driver for In-System Design, Inc. ISD200 ASIC");
 MODULE_AUTHOR("Björn Stenberg <bjorn@haxx.se>");
 MODULE_LICENSE("GPL");
@@ -1537,6 +1539,8 @@ static void isd200_ata_command(struct scsi_cmnd *srb, struct us_data *us)
 	isd200_srb_set_bufflen(srb, orig_bufflen);
 }
 
+static struct scsi_host_template isd200_host_template;
+
 static int isd200_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -1544,7 +1548,8 @@ static int isd200_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - isd200_usb_ids) + isd200_unusual_dev_list);
+			(id - isd200_usb_ids) + isd200_unusual_dev_list,
+			&isd200_host_template);
 	if (result)
 		return result;
 
@@ -1556,7 +1561,7 @@ static int isd200_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver isd200_driver = {
-	.name =		"ums-isd200",
+	.name =		DRV_NAME,
 	.probe =	isd200_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -1569,4 +1574,4 @@ static struct usb_driver isd200_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(isd200_driver);
+module_usb_stor_driver(isd200_driver, isd200_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/jumpshot.c b/drivers/usb/storage/jumpshot.c
index 563078be6547..ee613e258db0 100644
--- a/drivers/usb/storage/jumpshot.c
+++ b/drivers/usb/storage/jumpshot.c
@@ -56,7 +56,9 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
 
+#define DRV_NAME "ums-jumpshot"
 
 MODULE_DESCRIPTION("Driver for Lexar \"Jumpshot\" Compact Flash reader");
 MODULE_AUTHOR("Jimmie Mayfield <mayfield+usb@sackheads.org>");
@@ -647,6 +649,8 @@ static int jumpshot_transport(struct scsi_cmnd *srb, struct us_data *us)
 	return USB_STOR_TRANSPORT_FAILED;
 }
 
+static struct scsi_host_template jumpshot_host_template;
+
 static int jumpshot_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -654,7 +658,8 @@ static int jumpshot_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - jumpshot_usb_ids) + jumpshot_unusual_dev_list);
+			(id - jumpshot_usb_ids) + jumpshot_unusual_dev_list,
+			&jumpshot_host_template);
 	if (result)
 		return result;
 
@@ -668,7 +673,7 @@ static int jumpshot_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver jumpshot_driver = {
-	.name =		"ums-jumpshot",
+	.name =		DRV_NAME,
 	.probe =	jumpshot_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -681,4 +686,4 @@ static struct usb_driver jumpshot_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(jumpshot_driver);
+module_usb_stor_driver(jumpshot_driver, jumpshot_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/karma.c b/drivers/usb/storage/karma.c
index 94d16ee5e84b..ae201e69475c 100644
--- a/drivers/usb/storage/karma.c
+++ b/drivers/usb/storage/karma.c
@@ -28,6 +28,9 @@
 #include "usb.h"
 #include "transport.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-karma"
 
 MODULE_DESCRIPTION("Driver for Rio Karma");
 MODULE_AUTHOR("Bob Copeland <me@bobcopeland.com>, Keith Bennett <keith@mcs.st-and.ac.uk>");
@@ -200,6 +203,8 @@ out:
 	return ret;
 }
 
+static struct scsi_host_template karma_host_template;
+
 static int karma_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -207,7 +212,8 @@ static int karma_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - karma_usb_ids) + karma_unusual_dev_list);
+			(id - karma_usb_ids) + karma_unusual_dev_list,
+			&karma_host_template);
 	if (result)
 		return result;
 
@@ -220,7 +226,7 @@ static int karma_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver karma_driver = {
-	.name =		"ums-karma",
+	.name =		DRV_NAME,
 	.probe =	karma_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -233,4 +239,4 @@ static struct usb_driver karma_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(karma_driver);
+module_usb_stor_driver(karma_driver, karma_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/onetouch.c b/drivers/usb/storage/onetouch.c
index 74e2aa23b045..acc3d03d8c1e 100644
--- a/drivers/usb/storage/onetouch.c
+++ b/drivers/usb/storage/onetouch.c
@@ -35,6 +35,9 @@
 #include <linux/usb/input.h>
 #include "usb.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-onetouch"
 
 MODULE_DESCRIPTION("Maxtor USB OneTouch hard drive button driver");
 MODULE_AUTHOR("Nick Sillik <n.sillik@temple.edu>");
@@ -283,6 +286,8 @@ static void onetouch_release_input(void *onetouch_)
 	}
 }
 
+static struct scsi_host_template onetouch_host_template;
+
 static int onetouch_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -290,7 +295,8 @@ static int onetouch_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - onetouch_usb_ids) + onetouch_unusual_dev_list);
+			(id - onetouch_usb_ids) + onetouch_unusual_dev_list,
+			&onetouch_host_template);
 	if (result)
 		return result;
 
@@ -301,7 +307,7 @@ static int onetouch_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver onetouch_driver = {
-	.name =		"ums-onetouch",
+	.name =		DRV_NAME,
 	.probe =	onetouch_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -314,4 +320,4 @@ static struct usb_driver onetouch_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(onetouch_driver);
+module_usb_stor_driver(onetouch_driver, onetouch_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/realtek_cr.c b/drivers/usb/storage/realtek_cr.c
index 27e4a580d2ed..20433563a601 100644
--- a/drivers/usb/storage/realtek_cr.c
+++ b/drivers/usb/storage/realtek_cr.c
@@ -39,6 +39,9 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-realtek"
 
 MODULE_DESCRIPTION("Driver for Realtek USB Card Reader");
 MODULE_AUTHOR("wwang <wei_wang@realsil.com.cn>");
@@ -1034,6 +1037,8 @@ INIT_FAIL:
 	return -EIO;
 }
 
+static struct scsi_host_template realtek_cr_host_template;
+
 static int realtek_cr_probe(struct usb_interface *intf,
 			    const struct usb_device_id *id)
 {
@@ -1044,7 +1049,8 @@ static int realtek_cr_probe(struct usb_interface *intf,
 
 	result = usb_stor_probe1(&us, intf, id,
 				 (id - realtek_cr_ids) +
-				 realtek_cr_unusual_dev_list);
+				 realtek_cr_unusual_dev_list,
+				 &realtek_cr_host_template);
 	if (result)
 		return result;
 
@@ -1054,7 +1060,7 @@ static int realtek_cr_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver realtek_cr_driver = {
-	.name = "ums-realtek",
+	.name = DRV_NAME,
 	.probe = realtek_cr_probe,
 	.disconnect = usb_stor_disconnect,
 	/* .suspend =      usb_stor_suspend, */
@@ -1070,4 +1076,4 @@ static struct usb_driver realtek_cr_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(realtek_cr_driver);
+module_usb_stor_driver(realtek_cr_driver, realtek_cr_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/scsiglue.c b/drivers/usb/storage/scsiglue.c
index 0e400f382f3a..dba51362d2e2 100644
--- a/drivers/usb/storage/scsiglue.c
+++ b/drivers/usb/storage/scsiglue.c
@@ -456,17 +456,13 @@ static int write_info(struct Scsi_Host *host, char *buffer, int length)
 	return length;
 }
 
-/* we use this macro to help us write into the buffer */
-#undef SPRINTF
-#define SPRINTF(args...) seq_printf(m, ## args)
-
 static int show_info (struct seq_file *m, struct Scsi_Host *host)
 {
 	struct us_data *us = host_to_us(host);
 	const char *string;
 
 	/* print the controller name */
-	SPRINTF("   Host scsi%d: usb-storage\n", host->host_no);
+	seq_printf(m, "   Host scsi%d: usb-storage\n", host->host_no);
 
 	/* print product, vendor, and serial number strings */
 	if (us->pusb_dev->manufacturer)
@@ -475,26 +471,26 @@ static int show_info (struct seq_file *m, struct Scsi_Host *host)
 		string = us->unusual_dev->vendorName;
 	else
 		string = "Unknown";
-	SPRINTF("       Vendor: %s\n", string);
+	seq_printf(m, "       Vendor: %s\n", string);
 	if (us->pusb_dev->product)
 		string = us->pusb_dev->product;
 	else if (us->unusual_dev->productName)
 		string = us->unusual_dev->productName;
 	else
 		string = "Unknown";
-	SPRINTF("      Product: %s\n", string);
+	seq_printf(m, "      Product: %s\n", string);
 	if (us->pusb_dev->serial)
 		string = us->pusb_dev->serial;
 	else
 		string = "None";
-	SPRINTF("Serial Number: %s\n", string);
+	seq_printf(m, "Serial Number: %s\n", string);
 
 	/* show the protocol and transport */
-	SPRINTF("     Protocol: %s\n", us->protocol_name);
-	SPRINTF("    Transport: %s\n", us->transport_name);
+	seq_printf(m, "     Protocol: %s\n", us->protocol_name);
+	seq_printf(m, "    Transport: %s\n", us->transport_name);
 
 	/* show the device flags */
-	SPRINTF("       Quirks:");
+	seq_printf(m, "       Quirks:");
 
 #define US_FLAG(name, value) \
 	if (us->fflags & value) seq_printf(m, " " #name);
@@ -540,7 +536,7 @@ static struct device_attribute *sysfs_device_attr_list[] = {
  * this defines our host template, with which we'll allocate hosts
  */
 
-struct scsi_host_template usb_stor_host_template = {
+static const struct scsi_host_template usb_stor_host_template = {
 	/* basic userland interface stuff */
 	.name =				"usb-storage",
 	.proc_name =			"usb-storage",
@@ -558,7 +554,6 @@ struct scsi_host_template usb_stor_host_template = {
 
 	/* queue commands only, only one command per LUN */
 	.can_queue =			1,
-	.cmd_per_lun =			1,
 
 	/* unknown initiator id */
 	.this_id =			-1,
@@ -592,6 +587,16 @@ struct scsi_host_template usb_stor_host_template = {
 	.module =			THIS_MODULE
 };
 
+void usb_stor_host_template_init(struct scsi_host_template *sht,
+				 const char *name, struct module *owner)
+{
+	*sht = usb_stor_host_template;
+	sht->name = name;
+	sht->proc_name = name;
+	sht->module = owner;
+}
+EXPORT_SYMBOL_GPL(usb_stor_host_template_init);
+
 /* To Report "Illegal Request: Invalid Field in CDB */
 unsigned char usb_stor_sense_invalidCDB[18] = {
 	[0]	= 0x70,			    /* current error */
diff --git a/drivers/usb/storage/scsiglue.h b/drivers/usb/storage/scsiglue.h
index ffa1cca93d2c..5494d87607fb 100644
--- a/drivers/usb/storage/scsiglue.h
+++ b/drivers/usb/storage/scsiglue.h
@@ -41,8 +41,9 @@
 
 extern void usb_stor_report_device_reset(struct us_data *us);
 extern void usb_stor_report_bus_reset(struct us_data *us);
+extern void usb_stor_host_template_init(struct scsi_host_template *sht,
+					const char *name, struct module *owner);
 
 extern unsigned char usb_stor_sense_invalidCDB[18];
-extern struct scsi_host_template usb_stor_host_template;
 
 #endif
diff --git a/drivers/usb/storage/sddr09.c b/drivers/usb/storage/sddr09.c
index 3847053d732c..b74603689b9e 100644
--- a/drivers/usb/storage/sddr09.c
+++ b/drivers/usb/storage/sddr09.c
@@ -52,6 +52,9 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-sddr09"
 
 MODULE_DESCRIPTION("Driver for SanDisk SDDR-09 SmartMedia reader");
 MODULE_AUTHOR("Andries Brouwer <aeb@cwi.nl>, Robert Baruch <autophile@starband.net>");
@@ -1738,6 +1741,8 @@ usb_stor_sddr09_init(struct us_data *us) {
 	return sddr09_common_init(us);
 }
 
+static struct scsi_host_template sddr09_host_template;
+
 static int sddr09_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -1745,7 +1750,8 @@ static int sddr09_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - sddr09_usb_ids) + sddr09_unusual_dev_list);
+			(id - sddr09_usb_ids) + sddr09_unusual_dev_list,
+			&sddr09_host_template);
 	if (result)
 		return result;
 
@@ -1766,7 +1772,7 @@ static int sddr09_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver sddr09_driver = {
-	.name =		"ums-sddr09",
+	.name =		DRV_NAME,
 	.probe =	sddr09_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -1779,4 +1785,4 @@ static struct usb_driver sddr09_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(sddr09_driver);
+module_usb_stor_driver(sddr09_driver, sddr09_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/sddr55.c b/drivers/usb/storage/sddr55.c
index aacedef9667c..e5e0a25ecd96 100644
--- a/drivers/usb/storage/sddr55.c
+++ b/drivers/usb/storage/sddr55.c
@@ -34,6 +34,9 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-sddr55"
 
 MODULE_DESCRIPTION("Driver for SanDisk SDDR-55 SmartMedia reader");
 MODULE_AUTHOR("Simon Munton");
@@ -968,6 +971,7 @@ static int sddr55_transport(struct scsi_cmnd *srb, struct us_data *us)
 	return USB_STOR_TRANSPORT_FAILED; // FIXME: sense buffer?
 }
 
+static struct scsi_host_template sddr55_host_template;
 
 static int sddr55_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
@@ -976,7 +980,8 @@ static int sddr55_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - sddr55_usb_ids) + sddr55_unusual_dev_list);
+			(id - sddr55_usb_ids) + sddr55_unusual_dev_list,
+			&sddr55_host_template);
 	if (result)
 		return result;
 
@@ -990,7 +995,7 @@ static int sddr55_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver sddr55_driver = {
-	.name =		"ums-sddr55",
+	.name =		DRV_NAME,
 	.probe =	sddr55_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -1003,4 +1008,4 @@ static struct usb_driver sddr55_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(sddr55_driver);
+module_usb_stor_driver(sddr55_driver, sddr55_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/shuttle_usbat.c b/drivers/usb/storage/shuttle_usbat.c
index 008d805c3d21..a3ec86b913a1 100644
--- a/drivers/usb/storage/shuttle_usbat.c
+++ b/drivers/usb/storage/shuttle_usbat.c
@@ -53,6 +53,9 @@
 #include "transport.h"
 #include "protocol.h"
 #include "debug.h"
+#include "scsiglue.h"
+
+#define DRV_NAME "ums-usbat"
 
 MODULE_DESCRIPTION("Driver for SCM Microsystems (a.k.a. Shuttle) USB-ATAPI cable");
 MODULE_AUTHOR("Daniel Drake <dsd@gentoo.org>, Robert Baruch <autophile@starband.net>");
@@ -1834,6 +1837,8 @@ static int init_usbat_flash(struct us_data *us)
 	return init_usbat(us, USBAT_DEV_FLASH);
 }
 
+static struct scsi_host_template usbat_host_template;
+
 static int usbat_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
@@ -1841,7 +1846,8 @@ static int usbat_probe(struct usb_interface *intf,
 	int result;
 
 	result = usb_stor_probe1(&us, intf, id,
-			(id - usbat_usb_ids) + usbat_unusual_dev_list);
+			(id - usbat_usb_ids) + usbat_unusual_dev_list,
+			&usbat_host_template);
 	if (result)
 		return result;
 
@@ -1858,7 +1864,7 @@ static int usbat_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver usbat_driver = {
-	.name =		"ums-usbat",
+	.name =		DRV_NAME,
 	.probe =	usbat_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -1871,4 +1877,4 @@ static struct usb_driver usbat_driver = {
 	.no_dynamic_id = 1,
 };
 
-module_usb_driver(usbat_driver);
+module_usb_stor_driver(usbat_driver, usbat_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/uas.c b/drivers/usb/storage/uas.c
index 6d3122afeed3..f68921909552 100644
--- a/drivers/usb/storage/uas.c
+++ b/drivers/usb/storage/uas.c
@@ -811,7 +811,6 @@ static struct scsi_host_template uas_host_template = {
 	.can_queue = 65536,	/* Is there a limit on the _host_ ? */
 	.this_id = -1,
 	.sg_tablesize = SG_NONE,
-	.cmd_per_lun = 1,	/* until we override it */
 	.skip_settle_delay = 1,
 	.use_blk_tags = 1,
 };
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index 6c10c888f35f..43576ed31ccd 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -76,6 +76,8 @@
 #include "uas-detect.h"
 #endif
 
+#define DRV_NAME "usb-storage"
+
 /* Some informational data */
 MODULE_AUTHOR("Matthew Dharm <mdharm-usb@one-eyed-alien.net>");
 MODULE_DESCRIPTION("USB Mass Storage driver for Linux");
@@ -924,7 +926,8 @@ static unsigned int usb_stor_sg_tablesize(struct usb_interface *intf)
 int usb_stor_probe1(struct us_data **pus,
 		struct usb_interface *intf,
 		const struct usb_device_id *id,
-		struct us_unusual_dev *unusual_dev)
+		struct us_unusual_dev *unusual_dev,
+		struct scsi_host_template *sht)
 {
 	struct Scsi_Host *host;
 	struct us_data *us;
@@ -936,7 +939,7 @@ int usb_stor_probe1(struct us_data **pus,
 	 * Ask the SCSI layer to allocate a host structure, with extra
 	 * space at the end for our private us_data structure.
 	 */
-	host = scsi_host_alloc(&usb_stor_host_template, sizeof(*us));
+	host = scsi_host_alloc(sht, sizeof(*us));
 	if (!host) {
 		dev_warn(&intf->dev, "Unable to allocate the scsi host\n");
 		return -ENOMEM;
@@ -1073,6 +1076,8 @@ void usb_stor_disconnect(struct usb_interface *intf)
 }
 EXPORT_SYMBOL_GPL(usb_stor_disconnect);
 
+static struct scsi_host_template usb_stor_host_template;
+
 /* The main probe routine for standard devices */
 static int storage_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
@@ -1113,7 +1118,8 @@ static int storage_probe(struct usb_interface *intf,
 			id->idVendor, id->idProduct);
 	}
 
-	result = usb_stor_probe1(&us, intf, id, unusual_dev);
+	result = usb_stor_probe1(&us, intf, id, unusual_dev,
+				 &usb_stor_host_template);
 	if (result)
 		return result;
 
@@ -1124,7 +1130,7 @@ static int storage_probe(struct usb_interface *intf,
 }
 
 static struct usb_driver usb_storage_driver = {
-	.name =		"usb-storage",
+	.name =		DRV_NAME,
 	.probe =	storage_probe,
 	.disconnect =	usb_stor_disconnect,
 	.suspend =	usb_stor_suspend,
@@ -1137,4 +1143,4 @@ static struct usb_driver usb_storage_driver = {
 	.soft_unbind =	1,
 };
 
-module_usb_driver(usb_storage_driver);
+module_usb_stor_driver(usb_storage_driver, usb_stor_host_template, DRV_NAME);
diff --git a/drivers/usb/storage/usb.h b/drivers/usb/storage/usb.h
index 307e339a9478..da0ad3241728 100644
--- a/drivers/usb/storage/usb.h
+++ b/drivers/usb/storage/usb.h
@@ -197,11 +197,25 @@ extern int usb_stor_post_reset(struct usb_interface *iface);
 extern int usb_stor_probe1(struct us_data **pus,
 		struct usb_interface *intf,
 		const struct usb_device_id *id,
-		struct us_unusual_dev *unusual_dev);
+		struct us_unusual_dev *unusual_dev,
+		struct scsi_host_template *sht);
 extern int usb_stor_probe2(struct us_data *us);
 extern void usb_stor_disconnect(struct usb_interface *intf);
 
 extern void usb_stor_adjust_quirks(struct usb_device *dev,
 		unsigned long *fflags);
 
+#define module_usb_stor_driver(__driver, __sht, __name) \
+static int __init __driver##_init(void) \
+{ \
+	usb_stor_host_template_init(&(__sht), __name, THIS_MODULE); \
+	return usb_register(&(__driver)); \
+} \
+module_init(__driver##_init); \
+static void __exit __driver##_exit(void) \
+{ \
+	usb_deregister(&(__driver)); \
+} \
+module_exit(__driver##_exit)
+
 #endif
diff --git a/drivers/vfio/Kconfig b/drivers/vfio/Kconfig
index 7d092ddc8119..454017928ed0 100644
--- a/drivers/vfio/Kconfig
+++ b/drivers/vfio/Kconfig
@@ -21,7 +21,7 @@ config VFIO_VIRQFD
 menuconfig VFIO
 	tristate "VFIO Non-Privileged userspace driver framework"
 	depends on IOMMU_API
-	select VFIO_IOMMU_TYPE1 if (X86 || S390 || ARM_SMMU)
+	select VFIO_IOMMU_TYPE1 if (X86 || S390 || ARM_SMMU || ARM_SMMU_V3)
 	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/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index e9851add6f4e..964ad572aaee 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -1056,19 +1056,21 @@ struct vfio_devices {
 static int vfio_pci_get_devs(struct pci_dev *pdev, void *data)
 {
 	struct vfio_devices *devs = data;
-	struct pci_driver *pci_drv = ACCESS_ONCE(pdev->driver);
-
-	if (pci_drv != &vfio_pci_driver)
-		return -EBUSY;
+	struct vfio_device *device;
 
 	if (devs->cur_index == devs->max_index)
 		return -ENOSPC;
 
-	devs->devices[devs->cur_index] = vfio_device_get_from_dev(&pdev->dev);
-	if (!devs->devices[devs->cur_index])
+	device = vfio_device_get_from_dev(&pdev->dev);
+	if (!device)
 		return -EINVAL;
 
-	devs->cur_index++;
+	if (pci_dev_driver(pdev) != &vfio_pci_driver) {
+		vfio_device_put(device);
+		return -EBUSY;
+	}
+
+	devs->devices[devs->cur_index++] = device;
 	return 0;
 }
 
diff --git a/drivers/vfio/platform/Kconfig b/drivers/vfio/platform/Kconfig
index 9a4403e2a36c..bb30128782aa 100644
--- a/drivers/vfio/platform/Kconfig
+++ b/drivers/vfio/platform/Kconfig
@@ -1,6 +1,6 @@
 config VFIO_PLATFORM
 	tristate "VFIO support for platform devices"
-	depends on VFIO && EVENTFD && ARM
+	depends on VFIO && EVENTFD && (ARM || ARM64)
 	select VFIO_VIRQFD
 	help
 	  Support for platform devices with VFIO. This is required to make
@@ -18,3 +18,5 @@ config VFIO_AMBA
 	  framework.
 
 	  If you don't know what to do here, say N.
+
+source "drivers/vfio/platform/reset/Kconfig"
diff --git a/drivers/vfio/platform/Makefile b/drivers/vfio/platform/Makefile
index 81de144c0eaa..9ce8afe28450 100644
--- a/drivers/vfio/platform/Makefile
+++ b/drivers/vfio/platform/Makefile
@@ -2,7 +2,9 @@
 vfio-platform-y := vfio_platform.o vfio_platform_common.o vfio_platform_irq.o
 
 obj-$(CONFIG_VFIO_PLATFORM) += vfio-platform.o
+obj-$(CONFIG_VFIO_PLATFORM) += reset/
 
 vfio-amba-y := vfio_amba.o
 
 obj-$(CONFIG_VFIO_AMBA) += vfio-amba.o
+obj-$(CONFIG_VFIO_AMBA) += reset/
diff --git a/drivers/vfio/platform/reset/Kconfig b/drivers/vfio/platform/reset/Kconfig
new file mode 100644
index 000000000000..746b96b0003b
--- /dev/null
+++ b/drivers/vfio/platform/reset/Kconfig
@@ -0,0 +1,7 @@
+config VFIO_PLATFORM_CALXEDAXGMAC_RESET
+	tristate "VFIO support for calxeda xgmac reset"
+	depends on VFIO_PLATFORM
+	help
+	  Enables the VFIO platform driver to handle reset for Calxeda xgmac
+
+	  If you don't know what to do here, say N.
diff --git a/drivers/vfio/platform/reset/Makefile b/drivers/vfio/platform/reset/Makefile
new file mode 100644
index 000000000000..2a486af9f8fa
--- /dev/null
+++ b/drivers/vfio/platform/reset/Makefile
@@ -0,0 +1,5 @@
+vfio-platform-calxedaxgmac-y := vfio_platform_calxedaxgmac.o
+
+ccflags-y += -Idrivers/vfio/platform
+
+obj-$(CONFIG_VFIO_PLATFORM_CALXEDAXGMAC_RESET) += vfio-platform-calxedaxgmac.o
diff --git a/drivers/vfio/platform/reset/vfio_platform_calxedaxgmac.c b/drivers/vfio/platform/reset/vfio_platform_calxedaxgmac.c
new file mode 100644
index 000000000000..619dc7d22082
--- /dev/null
+++ b/drivers/vfio/platform/reset/vfio_platform_calxedaxgmac.c
@@ -0,0 +1,86 @@
+/*
+ * VFIO platform driver specialized for Calxeda xgmac reset
+ * reset code is inherited from calxeda xgmac native driver
+ *
+ * Copyright 2010-2011 Calxeda, Inc.
+ * Copyright (c) 2015 Linaro Ltd.
+ *              www.linaro.org
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+
+#include "vfio_platform_private.h"
+
+#define DRIVER_VERSION  "0.1"
+#define DRIVER_AUTHOR   "Eric Auger <eric.auger@linaro.org>"
+#define DRIVER_DESC     "Reset support for Calxeda xgmac vfio platform device"
+
+#define CALXEDAXGMAC_COMPAT "calxeda,hb-xgmac"
+
+/* XGMAC Register definitions */
+#define XGMAC_CONTROL           0x00000000      /* MAC Configuration */
+
+/* DMA Control and Status Registers */
+#define XGMAC_DMA_CONTROL       0x00000f18      /* Ctrl (Operational Mode) */
+#define XGMAC_DMA_INTR_ENA      0x00000f1c      /* Interrupt Enable */
+
+/* DMA Control registe defines */
+#define DMA_CONTROL_ST          0x00002000      /* Start/Stop Transmission */
+#define DMA_CONTROL_SR          0x00000002      /* Start/Stop Receive */
+
+/* Common MAC defines */
+#define MAC_ENABLE_TX           0x00000008      /* Transmitter Enable */
+#define MAC_ENABLE_RX           0x00000004      /* Receiver Enable */
+
+static inline void xgmac_mac_disable(void __iomem *ioaddr)
+{
+	u32 value = readl(ioaddr + XGMAC_DMA_CONTROL);
+
+	value &= ~(DMA_CONTROL_ST | DMA_CONTROL_SR);
+	writel(value, ioaddr + XGMAC_DMA_CONTROL);
+
+	value = readl(ioaddr + XGMAC_CONTROL);
+	value &= ~(MAC_ENABLE_TX | MAC_ENABLE_RX);
+	writel(value, ioaddr + XGMAC_CONTROL);
+}
+
+int vfio_platform_calxedaxgmac_reset(struct vfio_platform_device *vdev)
+{
+	struct vfio_platform_region reg = vdev->regions[0];
+
+	if (!reg.ioaddr) {
+		reg.ioaddr =
+			ioremap_nocache(reg.addr, reg.size);
+		if (!reg.ioaddr)
+			return -ENOMEM;
+	}
+
+	/* disable IRQ */
+	writel(0, reg.ioaddr + XGMAC_DMA_INTR_ENA);
+
+	/* Disable the MAC core */
+	xgmac_mac_disable(reg.ioaddr);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vfio_platform_calxedaxgmac_reset);
+
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/vfio/platform/vfio_platform_common.c b/drivers/vfio/platform/vfio_platform_common.c
index abcff7a1aa66..e43efb5e92bf 100644
--- a/drivers/vfio/platform/vfio_platform_common.c
+++ b/drivers/vfio/platform/vfio_platform_common.c
@@ -25,6 +25,44 @@
 
 static DEFINE_MUTEX(driver_lock);
 
+static const struct vfio_platform_reset_combo reset_lookup_table[] = {
+	{
+		.compat = "calxeda,hb-xgmac",
+		.reset_function_name = "vfio_platform_calxedaxgmac_reset",
+		.module_name = "vfio-platform-calxedaxgmac",
+	},
+};
+
+static void vfio_platform_get_reset(struct vfio_platform_device *vdev,
+				    struct device *dev)
+{
+	const char *compat;
+	int (*reset)(struct vfio_platform_device *);
+	int ret, i;
+
+	ret = device_property_read_string(dev, "compatible", &compat);
+	if (ret)
+		return;
+
+	for (i = 0 ; i < ARRAY_SIZE(reset_lookup_table); i++) {
+		if (!strcmp(reset_lookup_table[i].compat, compat)) {
+			request_module(reset_lookup_table[i].module_name);
+			reset = __symbol_get(
+				reset_lookup_table[i].reset_function_name);
+			if (reset) {
+				vdev->reset = reset;
+				return;
+			}
+		}
+	}
+}
+
+static void vfio_platform_put_reset(struct vfio_platform_device *vdev)
+{
+	if (vdev->reset)
+		symbol_put_addr(vdev->reset);
+}
+
 static int vfio_platform_regions_init(struct vfio_platform_device *vdev)
 {
 	int cnt = 0, i;
@@ -100,6 +138,8 @@ static void vfio_platform_release(void *device_data)
 	mutex_lock(&driver_lock);
 
 	if (!(--vdev->refcnt)) {
+		if (vdev->reset)
+			vdev->reset(vdev);
 		vfio_platform_regions_cleanup(vdev);
 		vfio_platform_irq_cleanup(vdev);
 	}
@@ -127,6 +167,9 @@ static int vfio_platform_open(void *device_data)
 		ret = vfio_platform_irq_init(vdev);
 		if (ret)
 			goto err_irq;
+
+		if (vdev->reset)
+			vdev->reset(vdev);
 	}
 
 	vdev->refcnt++;
@@ -159,6 +202,8 @@ static long vfio_platform_ioctl(void *device_data,
 		if (info.argsz < minsz)
 			return -EINVAL;
 
+		if (vdev->reset)
+			vdev->flags |= VFIO_DEVICE_FLAGS_RESET;
 		info.flags = vdev->flags;
 		info.num_regions = vdev->num_regions;
 		info.num_irqs = vdev->num_irqs;
@@ -252,8 +297,12 @@ static long vfio_platform_ioctl(void *device_data,
 
 		return ret;
 
-	} else if (cmd == VFIO_DEVICE_RESET)
-		return -EINVAL;
+	} else if (cmd == VFIO_DEVICE_RESET) {
+		if (vdev->reset)
+			return vdev->reset(vdev);
+		else
+			return -EINVAL;
+	}
 
 	return -ENOTTY;
 }
@@ -502,6 +551,8 @@ int vfio_platform_probe_common(struct vfio_platform_device *vdev,
 		return ret;
 	}
 
+	vfio_platform_get_reset(vdev, dev);
+
 	mutex_init(&vdev->igate);
 
 	return 0;
@@ -513,8 +564,11 @@ struct vfio_platform_device *vfio_platform_remove_common(struct device *dev)
 	struct vfio_platform_device *vdev;
 
 	vdev = vfio_del_group_dev(dev);
-	if (vdev)
+
+	if (vdev) {
+		vfio_platform_put_reset(vdev);
 		iommu_group_put(dev->iommu_group);
+	}
 
 	return vdev;
 }
diff --git a/drivers/vfio/platform/vfio_platform_private.h b/drivers/vfio/platform/vfio_platform_private.h
index 5d31e0473406..1c9b3d59543c 100644
--- a/drivers/vfio/platform/vfio_platform_private.h
+++ b/drivers/vfio/platform/vfio_platform_private.h
@@ -67,6 +67,13 @@ struct vfio_platform_device {
 	struct resource*
 		(*get_resource)(struct vfio_platform_device *vdev, int i);
 	int	(*get_irq)(struct vfio_platform_device *vdev, int i);
+	int	(*reset)(struct vfio_platform_device *vdev);
+};
+
+struct vfio_platform_reset_combo {
+	const char *compat;
+	const char *reset_function_name;
+	const char *module_name;
 };
 
 extern int vfio_platform_probe_common(struct vfio_platform_device *vdev,
diff --git a/drivers/vfio/vfio.c b/drivers/vfio/vfio.c
index e1278fe04b1e..2fb29dfeffbd 100644
--- a/drivers/vfio/vfio.c
+++ b/drivers/vfio/vfio.c
@@ -661,18 +661,29 @@ int vfio_add_group_dev(struct device *dev,
 EXPORT_SYMBOL_GPL(vfio_add_group_dev);
 
 /**
- * Get a reference to the vfio_device for a device that is known to
- * be bound to a vfio driver.  The driver implicitly holds a
- * vfio_device reference between vfio_add_group_dev and
- * vfio_del_group_dev.  We can therefore use drvdata to increment
- * that reference from the struct device.  This additional
- * reference must be released by calling vfio_device_put.
+ * Get a reference to the vfio_device for a device.  Even if the
+ * caller thinks they own the device, they could be racing with a
+ * release call path, so we can't trust drvdata for the shortcut.
+ * Go the long way around, from the iommu_group to the vfio_group
+ * to the vfio_device.
  */
 struct vfio_device *vfio_device_get_from_dev(struct device *dev)
 {
-	struct vfio_device *device = dev_get_drvdata(dev);
+	struct iommu_group *iommu_group;
+	struct vfio_group *group;
+	struct vfio_device *device;
+
+	iommu_group = iommu_group_get(dev);
+	if (!iommu_group)
+		return NULL;
 
-	vfio_device_get(device);
+	group = vfio_group_get_from_iommu(iommu_group);
+	iommu_group_put(iommu_group);
+	if (!group)
+		return NULL;
+
+	device = vfio_group_get_device(group, dev);
+	vfio_group_put(group);
 
 	return device;
 }
diff --git a/drivers/vfio/vfio_iommu_spapr_tce.c b/drivers/vfio/vfio_iommu_spapr_tce.c
index 730b4ef3e0cc..0582b72ef377 100644
--- a/drivers/vfio/vfio_iommu_spapr_tce.c
+++ b/drivers/vfio/vfio_iommu_spapr_tce.c
@@ -19,8 +19,10 @@
 #include <linux/uaccess.h>
 #include <linux/err.h>
 #include <linux/vfio.h>
+#include <linux/vmalloc.h>
 #include <asm/iommu.h>
 #include <asm/tce.h>
+#include <asm/mmu_context.h>
 
 #define DRIVER_VERSION  "0.1"
 #define DRIVER_AUTHOR   "aik@ozlabs.ru"
@@ -29,6 +31,51 @@
 static void tce_iommu_detach_group(void *iommu_data,
 		struct iommu_group *iommu_group);
 
+static long try_increment_locked_vm(long npages)
+{
+	long ret = 0, locked, lock_limit;
+
+	if (!current || !current->mm)
+		return -ESRCH; /* process exited */
+
+	if (!npages)
+		return 0;
+
+	down_write(&current->mm->mmap_sem);
+	locked = current->mm->locked_vm + npages;
+	lock_limit = rlimit(RLIMIT_MEMLOCK) >> PAGE_SHIFT;
+	if (locked > lock_limit && !capable(CAP_IPC_LOCK))
+		ret = -ENOMEM;
+	else
+		current->mm->locked_vm += npages;
+
+	pr_debug("[%d] RLIMIT_MEMLOCK +%ld %ld/%ld%s\n", current->pid,
+			npages << PAGE_SHIFT,
+			current->mm->locked_vm << PAGE_SHIFT,
+			rlimit(RLIMIT_MEMLOCK),
+			ret ? " - exceeded" : "");
+
+	up_write(&current->mm->mmap_sem);
+
+	return ret;
+}
+
+static void decrement_locked_vm(long npages)
+{
+	if (!current || !current->mm || !npages)
+		return; /* process exited */
+
+	down_write(&current->mm->mmap_sem);
+	if (WARN_ON_ONCE(npages > current->mm->locked_vm))
+		npages = current->mm->locked_vm;
+	current->mm->locked_vm -= npages;
+	pr_debug("[%d] RLIMIT_MEMLOCK -%ld %ld/%ld\n", current->pid,
+			npages << PAGE_SHIFT,
+			current->mm->locked_vm << PAGE_SHIFT,
+			rlimit(RLIMIT_MEMLOCK));
+	up_write(&current->mm->mmap_sem);
+}
+
 /*
  * VFIO IOMMU fd for SPAPR_TCE IOMMU implementation
  *
@@ -36,6 +83,11 @@ static void tce_iommu_detach_group(void *iommu_data,
  * into DMA'ble space using the IOMMU
  */
 
+struct tce_iommu_group {
+	struct list_head next;
+	struct iommu_group *grp;
+};
+
 /*
  * The container descriptor supports only a single group per container.
  * Required by the API as the container is not supplied with the IOMMU group
@@ -43,18 +95,140 @@ static void tce_iommu_detach_group(void *iommu_data,
  */
 struct tce_container {
 	struct mutex lock;
-	struct iommu_table *tbl;
 	bool enabled;
+	bool v2;
+	unsigned long locked_pages;
+	struct iommu_table *tables[IOMMU_TABLE_GROUP_MAX_TABLES];
+	struct list_head group_list;
 };
 
+static long tce_iommu_unregister_pages(struct tce_container *container,
+		__u64 vaddr, __u64 size)
+{
+	struct mm_iommu_table_group_mem_t *mem;
+
+	if ((vaddr & ~PAGE_MASK) || (size & ~PAGE_MASK))
+		return -EINVAL;
+
+	mem = mm_iommu_find(vaddr, size >> PAGE_SHIFT);
+	if (!mem)
+		return -ENOENT;
+
+	return mm_iommu_put(mem);
+}
+
+static long tce_iommu_register_pages(struct tce_container *container,
+		__u64 vaddr, __u64 size)
+{
+	long ret = 0;
+	struct mm_iommu_table_group_mem_t *mem = NULL;
+	unsigned long entries = size >> PAGE_SHIFT;
+
+	if ((vaddr & ~PAGE_MASK) || (size & ~PAGE_MASK) ||
+			((vaddr + size) < vaddr))
+		return -EINVAL;
+
+	ret = mm_iommu_get(vaddr, entries, &mem);
+	if (ret)
+		return ret;
+
+	container->enabled = true;
+
+	return 0;
+}
+
+static long tce_iommu_userspace_view_alloc(struct iommu_table *tbl)
+{
+	unsigned long cb = _ALIGN_UP(sizeof(tbl->it_userspace[0]) *
+			tbl->it_size, PAGE_SIZE);
+	unsigned long *uas;
+	long ret;
+
+	BUG_ON(tbl->it_userspace);
+
+	ret = try_increment_locked_vm(cb >> PAGE_SHIFT);
+	if (ret)
+		return ret;
+
+	uas = vzalloc(cb);
+	if (!uas) {
+		decrement_locked_vm(cb >> PAGE_SHIFT);
+		return -ENOMEM;
+	}
+	tbl->it_userspace = uas;
+
+	return 0;
+}
+
+static void tce_iommu_userspace_view_free(struct iommu_table *tbl)
+{
+	unsigned long cb = _ALIGN_UP(sizeof(tbl->it_userspace[0]) *
+			tbl->it_size, PAGE_SIZE);
+
+	if (!tbl->it_userspace)
+		return;
+
+	vfree(tbl->it_userspace);
+	tbl->it_userspace = NULL;
+	decrement_locked_vm(cb >> PAGE_SHIFT);
+}
+
+static bool tce_page_is_contained(struct page *page, unsigned page_shift)
+{
+	/*
+	 * Check that the TCE table granularity is not bigger than the size of
+	 * a page we just found. Otherwise the hardware can get access to
+	 * a bigger memory chunk that it should.
+	 */
+	return (PAGE_SHIFT + compound_order(compound_head(page))) >= page_shift;
+}
+
+static inline bool tce_groups_attached(struct tce_container *container)
+{
+	return !list_empty(&container->group_list);
+}
+
+static long tce_iommu_find_table(struct tce_container *container,
+		phys_addr_t ioba, struct iommu_table **ptbl)
+{
+	long i;
+
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i) {
+		struct iommu_table *tbl = container->tables[i];
+
+		if (tbl) {
+			unsigned long entry = ioba >> tbl->it_page_shift;
+			unsigned long start = tbl->it_offset;
+			unsigned long end = start + tbl->it_size;
+
+			if ((start <= entry) && (entry < end)) {
+				*ptbl = tbl;
+				return i;
+			}
+		}
+	}
+
+	return -1;
+}
+
+static int tce_iommu_find_free_table(struct tce_container *container)
+{
+	int i;
+
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i) {
+		if (!container->tables[i])
+			return i;
+	}
+
+	return -ENOSPC;
+}
+
 static int tce_iommu_enable(struct tce_container *container)
 {
 	int ret = 0;
-	unsigned long locked, lock_limit, npages;
-	struct iommu_table *tbl = container->tbl;
-
-	if (!container->tbl)
-		return -ENXIO;
+	unsigned long locked;
+	struct iommu_table_group *table_group;
+	struct tce_iommu_group *tcegrp;
 
 	if (!current->mm)
 		return -ESRCH; /* process exited */
@@ -79,21 +253,38 @@ static int tce_iommu_enable(struct tce_container *container)
 	 * Also we don't have a nice way to fail on H_PUT_TCE due to ulimits,
 	 * that would effectively kill the guest at random points, much better
 	 * enforcing the limit based on the max that the guest can map.
+	 *
+	 * Unfortunately at the moment it counts whole tables, no matter how
+	 * much memory the guest has. I.e. for 4GB guest and 4 IOMMU groups
+	 * each with 2GB DMA window, 8GB will be counted here. The reason for
+	 * this is that we cannot tell here the amount of RAM used by the guest
+	 * as this information is only available from KVM and VFIO is
+	 * KVM agnostic.
+	 *
+	 * So we do not allow enabling a container without a group attached
+	 * as there is no way to know how much we should increment
+	 * the locked_vm counter.
 	 */
-	down_write(&current->mm->mmap_sem);
-	npages = (tbl->it_size << IOMMU_PAGE_SHIFT_4K) >> PAGE_SHIFT;
-	locked = current->mm->locked_vm + npages;
-	lock_limit = rlimit(RLIMIT_MEMLOCK) >> PAGE_SHIFT;
-	if (locked > lock_limit && !capable(CAP_IPC_LOCK)) {
-		pr_warn("RLIMIT_MEMLOCK (%ld) exceeded\n",
-				rlimit(RLIMIT_MEMLOCK));
-		ret = -ENOMEM;
-	} else {
+	if (!tce_groups_attached(container))
+		return -ENODEV;
 
-		current->mm->locked_vm += npages;
-		container->enabled = true;
-	}
-	up_write(&current->mm->mmap_sem);
+	tcegrp = list_first_entry(&container->group_list,
+			struct tce_iommu_group, next);
+	table_group = iommu_group_get_iommudata(tcegrp->grp);
+	if (!table_group)
+		return -ENODEV;
+
+	if (!table_group->tce32_size)
+		return -EPERM;
+
+	locked = table_group->tce32_size >> PAGE_SHIFT;
+	ret = try_increment_locked_vm(locked);
+	if (ret)
+		return ret;
+
+	container->locked_pages = locked;
+
+	container->enabled = true;
 
 	return ret;
 }
@@ -105,20 +296,17 @@ static void tce_iommu_disable(struct tce_container *container)
 
 	container->enabled = false;
 
-	if (!container->tbl || !current->mm)
+	if (!current->mm)
 		return;
 
-	down_write(&current->mm->mmap_sem);
-	current->mm->locked_vm -= (container->tbl->it_size <<
-			IOMMU_PAGE_SHIFT_4K) >> PAGE_SHIFT;
-	up_write(&current->mm->mmap_sem);
+	decrement_locked_vm(container->locked_pages);
 }
 
 static void *tce_iommu_open(unsigned long arg)
 {
 	struct tce_container *container;
 
-	if (arg != VFIO_SPAPR_TCE_IOMMU) {
+	if ((arg != VFIO_SPAPR_TCE_IOMMU) && (arg != VFIO_SPAPR_TCE_v2_IOMMU)) {
 		pr_err("tce_vfio: Wrong IOMMU type\n");
 		return ERR_PTR(-EINVAL);
 	}
@@ -128,36 +316,411 @@ static void *tce_iommu_open(unsigned long arg)
 		return ERR_PTR(-ENOMEM);
 
 	mutex_init(&container->lock);
+	INIT_LIST_HEAD_RCU(&container->group_list);
+
+	container->v2 = arg == VFIO_SPAPR_TCE_v2_IOMMU;
 
 	return container;
 }
 
+static int tce_iommu_clear(struct tce_container *container,
+		struct iommu_table *tbl,
+		unsigned long entry, unsigned long pages);
+static void tce_iommu_free_table(struct iommu_table *tbl);
+
 static void tce_iommu_release(void *iommu_data)
 {
 	struct tce_container *container = iommu_data;
+	struct iommu_table_group *table_group;
+	struct tce_iommu_group *tcegrp;
+	long i;
+
+	while (tce_groups_attached(container)) {
+		tcegrp = list_first_entry(&container->group_list,
+				struct tce_iommu_group, next);
+		table_group = iommu_group_get_iommudata(tcegrp->grp);
+		tce_iommu_detach_group(iommu_data, tcegrp->grp);
+	}
 
-	WARN_ON(container->tbl && !container->tbl->it_group);
-	tce_iommu_disable(container);
+	/*
+	 * If VFIO created a table, it was not disposed
+	 * by tce_iommu_detach_group() so do it now.
+	 */
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i) {
+		struct iommu_table *tbl = container->tables[i];
+
+		if (!tbl)
+			continue;
 
-	if (container->tbl && container->tbl->it_group)
-		tce_iommu_detach_group(iommu_data, container->tbl->it_group);
+		tce_iommu_clear(container, tbl, tbl->it_offset, tbl->it_size);
+		tce_iommu_free_table(tbl);
+	}
 
+	tce_iommu_disable(container);
 	mutex_destroy(&container->lock);
 
 	kfree(container);
 }
 
+static void tce_iommu_unuse_page(struct tce_container *container,
+		unsigned long hpa)
+{
+	struct page *page;
+
+	page = pfn_to_page(hpa >> PAGE_SHIFT);
+	put_page(page);
+}
+
+static int tce_iommu_prereg_ua_to_hpa(unsigned long tce, unsigned long size,
+		unsigned long *phpa, struct mm_iommu_table_group_mem_t **pmem)
+{
+	long ret = 0;
+	struct mm_iommu_table_group_mem_t *mem;
+
+	mem = mm_iommu_lookup(tce, size);
+	if (!mem)
+		return -EINVAL;
+
+	ret = mm_iommu_ua_to_hpa(mem, tce, phpa);
+	if (ret)
+		return -EINVAL;
+
+	*pmem = mem;
+
+	return 0;
+}
+
+static void tce_iommu_unuse_page_v2(struct iommu_table *tbl,
+		unsigned long entry)
+{
+	struct mm_iommu_table_group_mem_t *mem = NULL;
+	int ret;
+	unsigned long hpa = 0;
+	unsigned long *pua = IOMMU_TABLE_USERSPACE_ENTRY(tbl, entry);
+
+	if (!pua || !current || !current->mm)
+		return;
+
+	ret = tce_iommu_prereg_ua_to_hpa(*pua, IOMMU_PAGE_SIZE(tbl),
+			&hpa, &mem);
+	if (ret)
+		pr_debug("%s: tce %lx at #%lx was not cached, ret=%d\n",
+				__func__, *pua, entry, ret);
+	if (mem)
+		mm_iommu_mapped_dec(mem);
+
+	*pua = 0;
+}
+
+static int tce_iommu_clear(struct tce_container *container,
+		struct iommu_table *tbl,
+		unsigned long entry, unsigned long pages)
+{
+	unsigned long oldhpa;
+	long ret;
+	enum dma_data_direction direction;
+
+	for ( ; pages; --pages, ++entry) {
+		direction = DMA_NONE;
+		oldhpa = 0;
+		ret = iommu_tce_xchg(tbl, entry, &oldhpa, &direction);
+		if (ret)
+			continue;
+
+		if (direction == DMA_NONE)
+			continue;
+
+		if (container->v2) {
+			tce_iommu_unuse_page_v2(tbl, entry);
+			continue;
+		}
+
+		tce_iommu_unuse_page(container, oldhpa);
+	}
+
+	return 0;
+}
+
+static int tce_iommu_use_page(unsigned long tce, unsigned long *hpa)
+{
+	struct page *page = NULL;
+	enum dma_data_direction direction = iommu_tce_direction(tce);
+
+	if (get_user_pages_fast(tce & PAGE_MASK, 1,
+			direction != DMA_TO_DEVICE, &page) != 1)
+		return -EFAULT;
+
+	*hpa = __pa((unsigned long) page_address(page));
+
+	return 0;
+}
+
+static long tce_iommu_build(struct tce_container *container,
+		struct iommu_table *tbl,
+		unsigned long entry, unsigned long tce, unsigned long pages,
+		enum dma_data_direction direction)
+{
+	long i, ret = 0;
+	struct page *page;
+	unsigned long hpa;
+	enum dma_data_direction dirtmp;
+
+	for (i = 0; i < pages; ++i) {
+		unsigned long offset = tce & IOMMU_PAGE_MASK(tbl) & ~PAGE_MASK;
+
+		ret = tce_iommu_use_page(tce, &hpa);
+		if (ret)
+			break;
+
+		page = pfn_to_page(hpa >> PAGE_SHIFT);
+		if (!tce_page_is_contained(page, tbl->it_page_shift)) {
+			ret = -EPERM;
+			break;
+		}
+
+		hpa |= offset;
+		dirtmp = direction;
+		ret = iommu_tce_xchg(tbl, entry + i, &hpa, &dirtmp);
+		if (ret) {
+			tce_iommu_unuse_page(container, hpa);
+			pr_err("iommu_tce: %s failed ioba=%lx, tce=%lx, ret=%ld\n",
+					__func__, entry << tbl->it_page_shift,
+					tce, ret);
+			break;
+		}
+
+		if (dirtmp != DMA_NONE)
+			tce_iommu_unuse_page(container, hpa);
+
+		tce += IOMMU_PAGE_SIZE(tbl);
+	}
+
+	if (ret)
+		tce_iommu_clear(container, tbl, entry, i);
+
+	return ret;
+}
+
+static long tce_iommu_build_v2(struct tce_container *container,
+		struct iommu_table *tbl,
+		unsigned long entry, unsigned long tce, unsigned long pages,
+		enum dma_data_direction direction)
+{
+	long i, ret = 0;
+	struct page *page;
+	unsigned long hpa;
+	enum dma_data_direction dirtmp;
+
+	for (i = 0; i < pages; ++i) {
+		struct mm_iommu_table_group_mem_t *mem = NULL;
+		unsigned long *pua = IOMMU_TABLE_USERSPACE_ENTRY(tbl,
+				entry + i);
+
+		ret = tce_iommu_prereg_ua_to_hpa(tce, IOMMU_PAGE_SIZE(tbl),
+				&hpa, &mem);
+		if (ret)
+			break;
+
+		page = pfn_to_page(hpa >> PAGE_SHIFT);
+		if (!tce_page_is_contained(page, tbl->it_page_shift)) {
+			ret = -EPERM;
+			break;
+		}
+
+		/* Preserve offset within IOMMU page */
+		hpa |= tce & IOMMU_PAGE_MASK(tbl) & ~PAGE_MASK;
+		dirtmp = direction;
+
+		/* The registered region is being unregistered */
+		if (mm_iommu_mapped_inc(mem))
+			break;
+
+		ret = iommu_tce_xchg(tbl, entry + i, &hpa, &dirtmp);
+		if (ret) {
+			/* dirtmp cannot be DMA_NONE here */
+			tce_iommu_unuse_page_v2(tbl, entry + i);
+			pr_err("iommu_tce: %s failed ioba=%lx, tce=%lx, ret=%ld\n",
+					__func__, entry << tbl->it_page_shift,
+					tce, ret);
+			break;
+		}
+
+		if (dirtmp != DMA_NONE)
+			tce_iommu_unuse_page_v2(tbl, entry + i);
+
+		*pua = tce;
+
+		tce += IOMMU_PAGE_SIZE(tbl);
+	}
+
+	if (ret)
+		tce_iommu_clear(container, tbl, entry, i);
+
+	return ret;
+}
+
+static long tce_iommu_create_table(struct tce_container *container,
+			struct iommu_table_group *table_group,
+			int num,
+			__u32 page_shift,
+			__u64 window_size,
+			__u32 levels,
+			struct iommu_table **ptbl)
+{
+	long ret, table_size;
+
+	table_size = table_group->ops->get_table_size(page_shift, window_size,
+			levels);
+	if (!table_size)
+		return -EINVAL;
+
+	ret = try_increment_locked_vm(table_size >> PAGE_SHIFT);
+	if (ret)
+		return ret;
+
+	ret = table_group->ops->create_table(table_group, num,
+			page_shift, window_size, levels, ptbl);
+
+	WARN_ON(!ret && !(*ptbl)->it_ops->free);
+	WARN_ON(!ret && ((*ptbl)->it_allocated_size != table_size));
+
+	if (!ret && container->v2) {
+		ret = tce_iommu_userspace_view_alloc(*ptbl);
+		if (ret)
+			(*ptbl)->it_ops->free(*ptbl);
+	}
+
+	if (ret)
+		decrement_locked_vm(table_size >> PAGE_SHIFT);
+
+	return ret;
+}
+
+static void tce_iommu_free_table(struct iommu_table *tbl)
+{
+	unsigned long pages = tbl->it_allocated_size >> PAGE_SHIFT;
+
+	tce_iommu_userspace_view_free(tbl);
+	tbl->it_ops->free(tbl);
+	decrement_locked_vm(pages);
+}
+
+static long tce_iommu_create_window(struct tce_container *container,
+		__u32 page_shift, __u64 window_size, __u32 levels,
+		__u64 *start_addr)
+{
+	struct tce_iommu_group *tcegrp;
+	struct iommu_table_group *table_group;
+	struct iommu_table *tbl = NULL;
+	long ret, num;
+
+	num = tce_iommu_find_free_table(container);
+	if (num < 0)
+		return num;
+
+	/* Get the first group for ops::create_table */
+	tcegrp = list_first_entry(&container->group_list,
+			struct tce_iommu_group, next);
+	table_group = iommu_group_get_iommudata(tcegrp->grp);
+	if (!table_group)
+		return -EFAULT;
+
+	if (!(table_group->pgsizes & (1ULL << page_shift)))
+		return -EINVAL;
+
+	if (!table_group->ops->set_window || !table_group->ops->unset_window ||
+			!table_group->ops->get_table_size ||
+			!table_group->ops->create_table)
+		return -EPERM;
+
+	/* Create TCE table */
+	ret = tce_iommu_create_table(container, table_group, num,
+			page_shift, window_size, levels, &tbl);
+	if (ret)
+		return ret;
+
+	BUG_ON(!tbl->it_ops->free);
+
+	/*
+	 * Program the table to every group.
+	 * Groups have been tested for compatibility at the attach time.
+	 */
+	list_for_each_entry(tcegrp, &container->group_list, next) {
+		table_group = iommu_group_get_iommudata(tcegrp->grp);
+
+		ret = table_group->ops->set_window(table_group, num, tbl);
+		if (ret)
+			goto unset_exit;
+	}
+
+	container->tables[num] = tbl;
+
+	/* Return start address assigned by platform in create_table() */
+	*start_addr = tbl->it_offset << tbl->it_page_shift;
+
+	return 0;
+
+unset_exit:
+	list_for_each_entry(tcegrp, &container->group_list, next) {
+		table_group = iommu_group_get_iommudata(tcegrp->grp);
+		table_group->ops->unset_window(table_group, num);
+	}
+	tce_iommu_free_table(tbl);
+
+	return ret;
+}
+
+static long tce_iommu_remove_window(struct tce_container *container,
+		__u64 start_addr)
+{
+	struct iommu_table_group *table_group = NULL;
+	struct iommu_table *tbl;
+	struct tce_iommu_group *tcegrp;
+	int num;
+
+	num = tce_iommu_find_table(container, start_addr, &tbl);
+	if (num < 0)
+		return -EINVAL;
+
+	BUG_ON(!tbl->it_size);
+
+	/* Detach groups from IOMMUs */
+	list_for_each_entry(tcegrp, &container->group_list, next) {
+		table_group = iommu_group_get_iommudata(tcegrp->grp);
+
+		/*
+		 * SPAPR TCE IOMMU exposes the default DMA window to
+		 * the guest via dma32_window_start/size of
+		 * VFIO_IOMMU_SPAPR_TCE_GET_INFO. Some platforms allow
+		 * the userspace to remove this window, some do not so
+		 * here we check for the platform capability.
+		 */
+		if (!table_group->ops || !table_group->ops->unset_window)
+			return -EPERM;
+
+		table_group->ops->unset_window(table_group, num);
+	}
+
+	/* Free table */
+	tce_iommu_clear(container, tbl, tbl->it_offset, tbl->it_size);
+	tce_iommu_free_table(tbl);
+	container->tables[num] = NULL;
+
+	return 0;
+}
+
 static long tce_iommu_ioctl(void *iommu_data,
 				 unsigned int cmd, unsigned long arg)
 {
 	struct tce_container *container = iommu_data;
-	unsigned long minsz;
+	unsigned long minsz, ddwsz;
 	long ret;
 
 	switch (cmd) {
 	case VFIO_CHECK_EXTENSION:
 		switch (arg) {
 		case VFIO_SPAPR_TCE_IOMMU:
+		case VFIO_SPAPR_TCE_v2_IOMMU:
 			ret = 1;
 			break;
 		default:
@@ -169,9 +732,17 @@ static long tce_iommu_ioctl(void *iommu_data,
 
 	case VFIO_IOMMU_SPAPR_TCE_GET_INFO: {
 		struct vfio_iommu_spapr_tce_info info;
-		struct iommu_table *tbl = container->tbl;
+		struct tce_iommu_group *tcegrp;
+		struct iommu_table_group *table_group;
+
+		if (!tce_groups_attached(container))
+			return -ENXIO;
+
+		tcegrp = list_first_entry(&container->group_list,
+				struct tce_iommu_group, next);
+		table_group = iommu_group_get_iommudata(tcegrp->grp);
 
-		if (WARN_ON(!tbl))
+		if (!table_group)
 			return -ENXIO;
 
 		minsz = offsetofend(struct vfio_iommu_spapr_tce_info,
@@ -183,9 +754,24 @@ static long tce_iommu_ioctl(void *iommu_data,
 		if (info.argsz < minsz)
 			return -EINVAL;
 
-		info.dma32_window_start = tbl->it_offset << IOMMU_PAGE_SHIFT_4K;
-		info.dma32_window_size = tbl->it_size << IOMMU_PAGE_SHIFT_4K;
+		info.dma32_window_start = table_group->tce32_start;
+		info.dma32_window_size = table_group->tce32_size;
 		info.flags = 0;
+		memset(&info.ddw, 0, sizeof(info.ddw));
+
+		if (table_group->max_dynamic_windows_supported &&
+				container->v2) {
+			info.flags |= VFIO_IOMMU_SPAPR_INFO_DDW;
+			info.ddw.pgsizes = table_group->pgsizes;
+			info.ddw.max_dynamic_windows_supported =
+				table_group->max_dynamic_windows_supported;
+			info.ddw.levels = table_group->max_levels;
+		}
+
+		ddwsz = offsetofend(struct vfio_iommu_spapr_tce_info, ddw);
+
+		if (info.argsz >= ddwsz)
+			minsz = ddwsz;
 
 		if (copy_to_user((void __user *)arg, &info, minsz))
 			return -EFAULT;
@@ -194,13 +780,12 @@ static long tce_iommu_ioctl(void *iommu_data,
 	}
 	case VFIO_IOMMU_MAP_DMA: {
 		struct vfio_iommu_type1_dma_map param;
-		struct iommu_table *tbl = container->tbl;
-		unsigned long tce, i;
+		struct iommu_table *tbl = NULL;
+		long num;
+		enum dma_data_direction direction;
 
-		if (!tbl)
-			return -ENXIO;
-
-		BUG_ON(!tbl->it_group);
+		if (!container->enabled)
+			return -EPERM;
 
 		minsz = offsetofend(struct vfio_iommu_type1_dma_map, size);
 
@@ -214,32 +799,43 @@ static long tce_iommu_ioctl(void *iommu_data,
 				VFIO_DMA_MAP_FLAG_WRITE))
 			return -EINVAL;
 
-		if ((param.size & ~IOMMU_PAGE_MASK_4K) ||
-				(param.vaddr & ~IOMMU_PAGE_MASK_4K))
+		num = tce_iommu_find_table(container, param.iova, &tbl);
+		if (num < 0)
+			return -ENXIO;
+
+		if ((param.size & ~IOMMU_PAGE_MASK(tbl)) ||
+				(param.vaddr & ~IOMMU_PAGE_MASK(tbl)))
 			return -EINVAL;
 
 		/* iova is checked by the IOMMU API */
-		tce = param.vaddr;
-		if (param.flags & VFIO_DMA_MAP_FLAG_READ)
-			tce |= TCE_PCI_READ;
-		if (param.flags & VFIO_DMA_MAP_FLAG_WRITE)
-			tce |= TCE_PCI_WRITE;
+		if (param.flags & VFIO_DMA_MAP_FLAG_READ) {
+			if (param.flags & VFIO_DMA_MAP_FLAG_WRITE)
+				direction = DMA_BIDIRECTIONAL;
+			else
+				direction = DMA_TO_DEVICE;
+		} else {
+			if (param.flags & VFIO_DMA_MAP_FLAG_WRITE)
+				direction = DMA_FROM_DEVICE;
+			else
+				return -EINVAL;
+		}
 
-		ret = iommu_tce_put_param_check(tbl, param.iova, tce);
+		ret = iommu_tce_put_param_check(tbl, param.iova, param.vaddr);
 		if (ret)
 			return ret;
 
-		for (i = 0; i < (param.size >> IOMMU_PAGE_SHIFT_4K); ++i) {
-			ret = iommu_put_tce_user_mode(tbl,
-					(param.iova >> IOMMU_PAGE_SHIFT_4K) + i,
-					tce);
-			if (ret)
-				break;
-			tce += IOMMU_PAGE_SIZE_4K;
-		}
-		if (ret)
-			iommu_clear_tces_and_put_pages(tbl,
-					param.iova >> IOMMU_PAGE_SHIFT_4K, i);
+		if (container->v2)
+			ret = tce_iommu_build_v2(container, tbl,
+					param.iova >> tbl->it_page_shift,
+					param.vaddr,
+					param.size >> tbl->it_page_shift,
+					direction);
+		else
+			ret = tce_iommu_build(container, tbl,
+					param.iova >> tbl->it_page_shift,
+					param.vaddr,
+					param.size >> tbl->it_page_shift,
+					direction);
 
 		iommu_flush_tce(tbl);
 
@@ -247,10 +843,11 @@ static long tce_iommu_ioctl(void *iommu_data,
 	}
 	case VFIO_IOMMU_UNMAP_DMA: {
 		struct vfio_iommu_type1_dma_unmap param;
-		struct iommu_table *tbl = container->tbl;
+		struct iommu_table *tbl = NULL;
+		long num;
 
-		if (WARN_ON(!tbl))
-			return -ENXIO;
+		if (!container->enabled)
+			return -EPERM;
 
 		minsz = offsetofend(struct vfio_iommu_type1_dma_unmap,
 				size);
@@ -265,22 +862,81 @@ static long tce_iommu_ioctl(void *iommu_data,
 		if (param.flags)
 			return -EINVAL;
 
-		if (param.size & ~IOMMU_PAGE_MASK_4K)
+		num = tce_iommu_find_table(container, param.iova, &tbl);
+		if (num < 0)
+			return -ENXIO;
+
+		if (param.size & ~IOMMU_PAGE_MASK(tbl))
 			return -EINVAL;
 
 		ret = iommu_tce_clear_param_check(tbl, param.iova, 0,
-				param.size >> IOMMU_PAGE_SHIFT_4K);
+				param.size >> tbl->it_page_shift);
 		if (ret)
 			return ret;
 
-		ret = iommu_clear_tces_and_put_pages(tbl,
-				param.iova >> IOMMU_PAGE_SHIFT_4K,
-				param.size >> IOMMU_PAGE_SHIFT_4K);
+		ret = tce_iommu_clear(container, tbl,
+				param.iova >> tbl->it_page_shift,
+				param.size >> tbl->it_page_shift);
 		iommu_flush_tce(tbl);
 
 		return ret;
 	}
+	case VFIO_IOMMU_SPAPR_REGISTER_MEMORY: {
+		struct vfio_iommu_spapr_register_memory param;
+
+		if (!container->v2)
+			break;
+
+		minsz = offsetofend(struct vfio_iommu_spapr_register_memory,
+				size);
+
+		if (copy_from_user(&param, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (param.argsz < minsz)
+			return -EINVAL;
+
+		/* No flag is supported now */
+		if (param.flags)
+			return -EINVAL;
+
+		mutex_lock(&container->lock);
+		ret = tce_iommu_register_pages(container, param.vaddr,
+				param.size);
+		mutex_unlock(&container->lock);
+
+		return ret;
+	}
+	case VFIO_IOMMU_SPAPR_UNREGISTER_MEMORY: {
+		struct vfio_iommu_spapr_register_memory param;
+
+		if (!container->v2)
+			break;
+
+		minsz = offsetofend(struct vfio_iommu_spapr_register_memory,
+				size);
+
+		if (copy_from_user(&param, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (param.argsz < minsz)
+			return -EINVAL;
+
+		/* No flag is supported now */
+		if (param.flags)
+			return -EINVAL;
+
+		mutex_lock(&container->lock);
+		ret = tce_iommu_unregister_pages(container, param.vaddr,
+				param.size);
+		mutex_unlock(&container->lock);
+
+		return ret;
+	}
 	case VFIO_IOMMU_ENABLE:
+		if (container->v2)
+			break;
+
 		mutex_lock(&container->lock);
 		ret = tce_iommu_enable(container);
 		mutex_unlock(&container->lock);
@@ -288,48 +944,280 @@ static long tce_iommu_ioctl(void *iommu_data,
 
 
 	case VFIO_IOMMU_DISABLE:
+		if (container->v2)
+			break;
+
 		mutex_lock(&container->lock);
 		tce_iommu_disable(container);
 		mutex_unlock(&container->lock);
 		return 0;
-	case VFIO_EEH_PE_OP:
-		if (!container->tbl || !container->tbl->it_group)
-			return -ENODEV;
 
-		return vfio_spapr_iommu_eeh_ioctl(container->tbl->it_group,
-						  cmd, arg);
+	case VFIO_EEH_PE_OP: {
+		struct tce_iommu_group *tcegrp;
+
+		ret = 0;
+		list_for_each_entry(tcegrp, &container->group_list, next) {
+			ret = vfio_spapr_iommu_eeh_ioctl(tcegrp->grp,
+					cmd, arg);
+			if (ret)
+				return ret;
+		}
+		return ret;
+	}
+
+	case VFIO_IOMMU_SPAPR_TCE_CREATE: {
+		struct vfio_iommu_spapr_tce_create create;
+
+		if (!container->v2)
+			break;
+
+		if (!tce_groups_attached(container))
+			return -ENXIO;
+
+		minsz = offsetofend(struct vfio_iommu_spapr_tce_create,
+				start_addr);
+
+		if (copy_from_user(&create, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (create.argsz < minsz)
+			return -EINVAL;
+
+		if (create.flags)
+			return -EINVAL;
+
+		mutex_lock(&container->lock);
+
+		ret = tce_iommu_create_window(container, create.page_shift,
+				create.window_size, create.levels,
+				&create.start_addr);
+
+		mutex_unlock(&container->lock);
+
+		if (!ret && copy_to_user((void __user *)arg, &create, minsz))
+			ret = -EFAULT;
+
+		return ret;
+	}
+	case VFIO_IOMMU_SPAPR_TCE_REMOVE: {
+		struct vfio_iommu_spapr_tce_remove remove;
+
+		if (!container->v2)
+			break;
+
+		if (!tce_groups_attached(container))
+			return -ENXIO;
+
+		minsz = offsetofend(struct vfio_iommu_spapr_tce_remove,
+				start_addr);
+
+		if (copy_from_user(&remove, (void __user *)arg, minsz))
+			return -EFAULT;
+
+		if (remove.argsz < minsz)
+			return -EINVAL;
+
+		if (remove.flags)
+			return -EINVAL;
+
+		mutex_lock(&container->lock);
+
+		ret = tce_iommu_remove_window(container, remove.start_addr);
+
+		mutex_unlock(&container->lock);
+
+		return ret;
+	}
 	}
 
 	return -ENOTTY;
 }
 
+static void tce_iommu_release_ownership(struct tce_container *container,
+		struct iommu_table_group *table_group)
+{
+	int i;
+
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i) {
+		struct iommu_table *tbl = container->tables[i];
+
+		if (!tbl)
+			continue;
+
+		tce_iommu_clear(container, tbl, tbl->it_offset, tbl->it_size);
+		tce_iommu_userspace_view_free(tbl);
+		if (tbl->it_map)
+			iommu_release_ownership(tbl);
+
+		container->tables[i] = NULL;
+	}
+}
+
+static int tce_iommu_take_ownership(struct tce_container *container,
+		struct iommu_table_group *table_group)
+{
+	int i, j, rc = 0;
+
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i) {
+		struct iommu_table *tbl = table_group->tables[i];
+
+		if (!tbl || !tbl->it_map)
+			continue;
+
+		rc = tce_iommu_userspace_view_alloc(tbl);
+		if (!rc)
+			rc = iommu_take_ownership(tbl);
+
+		if (rc) {
+			for (j = 0; j < i; ++j)
+				iommu_release_ownership(
+						table_group->tables[j]);
+
+			return rc;
+		}
+	}
+
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i)
+		container->tables[i] = table_group->tables[i];
+
+	return 0;
+}
+
+static void tce_iommu_release_ownership_ddw(struct tce_container *container,
+		struct iommu_table_group *table_group)
+{
+	long i;
+
+	if (!table_group->ops->unset_window) {
+		WARN_ON_ONCE(1);
+		return;
+	}
+
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i)
+		table_group->ops->unset_window(table_group, i);
+
+	table_group->ops->release_ownership(table_group);
+}
+
+static long tce_iommu_take_ownership_ddw(struct tce_container *container,
+		struct iommu_table_group *table_group)
+{
+	long i, ret = 0;
+	struct iommu_table *tbl = NULL;
+
+	if (!table_group->ops->create_table || !table_group->ops->set_window ||
+			!table_group->ops->release_ownership) {
+		WARN_ON_ONCE(1);
+		return -EFAULT;
+	}
+
+	table_group->ops->take_ownership(table_group);
+
+	/*
+	 * If it the first group attached, check if there is
+	 * a default DMA window and create one if none as
+	 * the userspace expects it to exist.
+	 */
+	if (!tce_groups_attached(container) && !container->tables[0]) {
+		ret = tce_iommu_create_table(container,
+				table_group,
+				0, /* window number */
+				IOMMU_PAGE_SHIFT_4K,
+				table_group->tce32_size,
+				1, /* default levels */
+				&tbl);
+		if (ret)
+			goto release_exit;
+		else
+			container->tables[0] = tbl;
+	}
+
+	/* Set all windows to the new group */
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i) {
+		tbl = container->tables[i];
+
+		if (!tbl)
+			continue;
+
+		/* Set the default window to a new group */
+		ret = table_group->ops->set_window(table_group, i, tbl);
+		if (ret)
+			goto release_exit;
+	}
+
+	return 0;
+
+release_exit:
+	for (i = 0; i < IOMMU_TABLE_GROUP_MAX_TABLES; ++i)
+		table_group->ops->unset_window(table_group, i);
+
+	table_group->ops->release_ownership(table_group);
+
+	return ret;
+}
+
 static int tce_iommu_attach_group(void *iommu_data,
 		struct iommu_group *iommu_group)
 {
 	int ret;
 	struct tce_container *container = iommu_data;
-	struct iommu_table *tbl = iommu_group_get_iommudata(iommu_group);
+	struct iommu_table_group *table_group;
+	struct tce_iommu_group *tcegrp = NULL;
 
-	BUG_ON(!tbl);
 	mutex_lock(&container->lock);
 
 	/* pr_debug("tce_vfio: Attaching group #%u to iommu %p\n",
 			iommu_group_id(iommu_group), iommu_group); */
-	if (container->tbl) {
-		pr_warn("tce_vfio: Only one group per IOMMU container is allowed, existing id=%d, attaching id=%d\n",
-				iommu_group_id(container->tbl->it_group),
-				iommu_group_id(iommu_group));
-		ret = -EBUSY;
-	} else if (container->enabled) {
-		pr_err("tce_vfio: attaching group #%u to enabled container\n",
-				iommu_group_id(iommu_group));
+	table_group = iommu_group_get_iommudata(iommu_group);
+
+	if (tce_groups_attached(container) && (!table_group->ops ||
+			!table_group->ops->take_ownership ||
+			!table_group->ops->release_ownership)) {
 		ret = -EBUSY;
-	} else {
-		ret = iommu_take_ownership(tbl);
-		if (!ret)
-			container->tbl = tbl;
+		goto unlock_exit;
+	}
+
+	/* Check if new group has the same iommu_ops (i.e. compatible) */
+	list_for_each_entry(tcegrp, &container->group_list, next) {
+		struct iommu_table_group *table_group_tmp;
+
+		if (tcegrp->grp == iommu_group) {
+			pr_warn("tce_vfio: Group %d is already attached\n",
+					iommu_group_id(iommu_group));
+			ret = -EBUSY;
+			goto unlock_exit;
+		}
+		table_group_tmp = iommu_group_get_iommudata(tcegrp->grp);
+		if (table_group_tmp->ops != table_group->ops) {
+			pr_warn("tce_vfio: Group %d is incompatible with group %d\n",
+					iommu_group_id(iommu_group),
+					iommu_group_id(tcegrp->grp));
+			ret = -EPERM;
+			goto unlock_exit;
+		}
+	}
+
+	tcegrp = kzalloc(sizeof(*tcegrp), GFP_KERNEL);
+	if (!tcegrp) {
+		ret = -ENOMEM;
+		goto unlock_exit;
 	}
 
+	if (!table_group->ops || !table_group->ops->take_ownership ||
+			!table_group->ops->release_ownership)
+		ret = tce_iommu_take_ownership(container, table_group);
+	else
+		ret = tce_iommu_take_ownership_ddw(container, table_group);
+
+	if (!ret) {
+		tcegrp->grp = iommu_group;
+		list_add(&tcegrp->next, &container->group_list);
+	}
+
+unlock_exit:
+	if (ret && tcegrp)
+		kfree(tcegrp);
+
 	mutex_unlock(&container->lock);
 
 	return ret;
@@ -339,26 +1227,37 @@ static void tce_iommu_detach_group(void *iommu_data,
 		struct iommu_group *iommu_group)
 {
 	struct tce_container *container = iommu_data;
-	struct iommu_table *tbl = iommu_group_get_iommudata(iommu_group);
+	struct iommu_table_group *table_group;
+	bool found = false;
+	struct tce_iommu_group *tcegrp;
 
-	BUG_ON(!tbl);
 	mutex_lock(&container->lock);
-	if (tbl != container->tbl) {
-		pr_warn("tce_vfio: detaching group #%u, expected group is #%u\n",
-				iommu_group_id(iommu_group),
-				iommu_group_id(tbl->it_group));
-	} else {
-		if (container->enabled) {
-			pr_warn("tce_vfio: detaching group #%u from enabled container, forcing disable\n",
-					iommu_group_id(tbl->it_group));
-			tce_iommu_disable(container);
+
+	list_for_each_entry(tcegrp, &container->group_list, next) {
+		if (tcegrp->grp == iommu_group) {
+			found = true;
+			break;
 		}
+	}
 
-		/* pr_debug("tce_vfio: detaching group #%u from iommu %p\n",
-				iommu_group_id(iommu_group), iommu_group); */
-		container->tbl = NULL;
-		iommu_release_ownership(tbl);
+	if (!found) {
+		pr_warn("tce_vfio: detaching unattached group #%u\n",
+				iommu_group_id(iommu_group));
+		goto unlock_exit;
 	}
+
+	list_del(&tcegrp->next);
+	kfree(tcegrp);
+
+	table_group = iommu_group_get_iommudata(iommu_group);
+	BUG_ON(!table_group);
+
+	if (!table_group->ops || !table_group->ops->release_ownership)
+		tce_iommu_release_ownership(container, table_group);
+	else
+		tce_iommu_release_ownership_ddw(container, table_group);
+
+unlock_exit:
 	mutex_unlock(&container->lock);
 }
 
diff --git a/drivers/vfio/vfio_spapr_eeh.c b/drivers/vfio/vfio_spapr_eeh.c
index 5fa42db769ee..38edeb4729a9 100644
--- a/drivers/vfio/vfio_spapr_eeh.c
+++ b/drivers/vfio/vfio_spapr_eeh.c
@@ -85,6 +85,16 @@ long vfio_spapr_iommu_eeh_ioctl(struct iommu_group *group,
 		case VFIO_EEH_PE_CONFIGURE:
 			ret = eeh_pe_configure(pe);
 			break;
+		case VFIO_EEH_PE_INJECT_ERR:
+			minsz = offsetofend(struct vfio_eeh_pe_op, err.mask);
+			if (op.argsz < minsz)
+				return -EINVAL;
+			if (copy_from_user(&op, (void __user *)arg, minsz))
+				return -EFAULT;
+
+			ret = eeh_pe_inject_err(pe, op.err.type, op.err.func,
+						op.err.addr, op.err.mask);
+			break;
 		default:
 			ret = -EINVAL;
 		}
diff --git a/drivers/vhost/Kconfig b/drivers/vhost/Kconfig
index 017a1e8a8f6f..533eaf04f12f 100644
--- a/drivers/vhost/Kconfig
+++ b/drivers/vhost/Kconfig
@@ -32,3 +32,18 @@ config VHOST
 	---help---
 	  This option is selected by any driver which needs to access
 	  the core of vhost.
+
+config VHOST_CROSS_ENDIAN_LEGACY
+	bool "Cross-endian support for vhost"
+	default n
+	---help---
+	  This option allows vhost to support guests with a different byte
+	  ordering from host while using legacy virtio.
+
+	  Userspace programs can control the feature using the
+	  VHOST_SET_VRING_ENDIAN and VHOST_GET_VRING_ENDIAN ioctls.
+
+	  This is only useful on a few platforms (ppc64 and arm64). Since it
+	  adds some overhead, it is disabled by default.
+
+	  If unsure, say "N".
diff --git a/drivers/vhost/scsi.c b/drivers/vhost/scsi.c
index ea32b386797f..dfcc02c93648 100644
--- a/drivers/vhost/scsi.c
+++ b/drivers/vhost/scsi.c
@@ -35,13 +35,14 @@
 #include <linux/compat.h>
 #include <linux/eventfd.h>
 #include <linux/fs.h>
+#include <linux/vmalloc.h>
 #include <linux/miscdevice.h>
 #include <asm/unaligned.h>
-#include <scsi/scsi.h>
+#include <scsi/scsi_common.h>
+#include <scsi/scsi_proto.h>
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
 #include <target/target_core_fabric_configfs.h>
-#include <target/target_core_configfs.h>
 #include <target/configfs_macros.h>
 #include <linux/vhost.h>
 #include <linux/virtio_scsi.h>
@@ -115,15 +116,6 @@ struct vhost_scsi_nexus {
 	struct se_session *tvn_se_sess;
 };
 
-struct vhost_scsi_nacl {
-	/* Binary World Wide unique Port Name for Vhost Initiator port */
-	u64 iport_wwpn;
-	/* ASCII formatted WWPN for Sas Initiator port */
-	char iport_name[VHOST_SCSI_NAMELEN];
-	/* Returned by vhost_scsi_make_nodeacl() */
-	struct se_node_acl se_node_acl;
-};
-
 struct vhost_scsi_tpg {
 	/* Vhost port target portal group tag for TCM */
 	u16 tport_tpgt;
@@ -216,7 +208,6 @@ struct vhost_scsi {
 	int vs_events_nr; /* num of pending events, protected by vq->mutex */
 };
 
-static struct target_core_fabric_ops vhost_scsi_ops;
 static struct workqueue_struct *vhost_scsi_workqueue;
 
 /* Global spinlock to protect vhost_scsi TPG list for vhost IOCTL access */
@@ -297,28 +288,6 @@ static char *vhost_scsi_get_fabric_name(void)
 	return "vhost";
 }
 
-static u8 vhost_scsi_get_fabric_proto_ident(struct se_portal_group *se_tpg)
-{
-	struct vhost_scsi_tpg *tpg = container_of(se_tpg,
-				struct vhost_scsi_tpg, se_tpg);
-	struct vhost_scsi_tport *tport = tpg->tport;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_fabric_proto_ident(se_tpg);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_fabric_proto_ident(se_tpg);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_fabric_proto_ident(se_tpg);
-	default:
-		pr_err("Unknown tport_proto_id: 0x%02x, using"
-			" SAS emulation\n", tport->tport_proto_id);
-		break;
-	}
-
-	return sas_get_fabric_proto_ident(se_tpg);
-}
-
 static char *vhost_scsi_get_fabric_wwn(struct se_portal_group *se_tpg)
 {
 	struct vhost_scsi_tpg *tpg = container_of(se_tpg,
@@ -335,102 +304,6 @@ static u16 vhost_scsi_get_tpgt(struct se_portal_group *se_tpg)
 	return tpg->tport_tpgt;
 }
 
-static u32 vhost_scsi_get_default_depth(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static u32
-vhost_scsi_get_pr_transport_id(struct se_portal_group *se_tpg,
-			      struct se_node_acl *se_nacl,
-			      struct t10_pr_registration *pr_reg,
-			      int *format_code,
-			      unsigned char *buf)
-{
-	struct vhost_scsi_tpg *tpg = container_of(se_tpg,
-				struct vhost_scsi_tpg, se_tpg);
-	struct vhost_scsi_tport *tport = tpg->tport;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	default:
-		pr_err("Unknown tport_proto_id: 0x%02x, using"
-			" SAS emulation\n", tport->tport_proto_id);
-		break;
-	}
-
-	return sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-			format_code, buf);
-}
-
-static u32
-vhost_scsi_get_pr_transport_id_len(struct se_portal_group *se_tpg,
-				  struct se_node_acl *se_nacl,
-				  struct t10_pr_registration *pr_reg,
-				  int *format_code)
-{
-	struct vhost_scsi_tpg *tpg = container_of(se_tpg,
-				struct vhost_scsi_tpg, se_tpg);
-	struct vhost_scsi_tport *tport = tpg->tport;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	default:
-		pr_err("Unknown tport_proto_id: 0x%02x, using"
-			" SAS emulation\n", tport->tport_proto_id);
-		break;
-	}
-
-	return sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-			format_code);
-}
-
-static char *
-vhost_scsi_parse_pr_out_transport_id(struct se_portal_group *se_tpg,
-				    const char *buf,
-				    u32 *out_tid_len,
-				    char **port_nexus_ptr)
-{
-	struct vhost_scsi_tpg *tpg = container_of(se_tpg,
-				struct vhost_scsi_tpg, se_tpg);
-	struct vhost_scsi_tport *tport = tpg->tport;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	case SCSI_PROTOCOL_FCP:
-		return fc_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	default:
-		pr_err("Unknown tport_proto_id: 0x%02x, using"
-			" SAS emulation\n", tport->tport_proto_id);
-		break;
-	}
-
-	return sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-			port_nexus_ptr);
-}
-
 static int vhost_scsi_check_prot_fabric_only(struct se_portal_group *se_tpg)
 {
 	struct vhost_scsi_tpg *tpg = container_of(se_tpg,
@@ -439,29 +312,6 @@ static int vhost_scsi_check_prot_fabric_only(struct se_portal_group *se_tpg)
 	return tpg->tv_fabric_prot_type;
 }
 
-static struct se_node_acl *
-vhost_scsi_alloc_fabric_acl(struct se_portal_group *se_tpg)
-{
-	struct vhost_scsi_nacl *nacl;
-
-	nacl = kzalloc(sizeof(struct vhost_scsi_nacl), GFP_KERNEL);
-	if (!nacl) {
-		pr_err("Unable to allocate struct vhost_scsi_nacl\n");
-		return NULL;
-	}
-
-	return &nacl->se_node_acl;
-}
-
-static void
-vhost_scsi_release_fabric_acl(struct se_portal_group *se_tpg,
-			     struct se_node_acl *se_nacl)
-{
-	struct vhost_scsi_nacl *nacl = container_of(se_nacl,
-			struct vhost_scsi_nacl, se_node_acl);
-	kfree(nacl);
-}
-
 static u32 vhost_scsi_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
 	return 1;
@@ -519,11 +369,6 @@ static void vhost_scsi_set_default_node_attrs(struct se_node_acl *nacl)
 	return;
 }
 
-static u32 vhost_scsi_get_task_tag(struct se_cmd *se_cmd)
-{
-	return 0;
-}
-
 static int vhost_scsi_get_cmd_state(struct se_cmd *se_cmd)
 {
 	return 0;
@@ -607,7 +452,7 @@ static void vhost_scsi_free_cmd(struct vhost_scsi_cmd *cmd)
 
 static int vhost_scsi_check_stop_free(struct se_cmd *se_cmd)
 {
-	return target_put_sess_cmd(se_cmd->se_sess, se_cmd);
+	return target_put_sess_cmd(se_cmd);
 }
 
 static void
@@ -968,6 +813,7 @@ static void vhost_scsi_submission_work(struct work_struct *work)
 	}
 	tv_nexus = cmd->tvc_nexus;
 
+	se_cmd->tag = 0;
 	rc = target_submit_cmd_map_sgls(se_cmd, tv_nexus->tvn_se_sess,
 			cmd->tvc_cdb, &cmd->tvc_sense_buf[0],
 			cmd->tvc_lun, cmd->tvc_exp_data_len,
@@ -1822,50 +1668,6 @@ static void vhost_scsi_port_unlink(struct se_portal_group *se_tpg,
 	mutex_unlock(&vhost_scsi_mutex);
 }
 
-static struct se_node_acl *
-vhost_scsi_make_nodeacl(struct se_portal_group *se_tpg,
-		       struct config_group *group,
-		       const char *name)
-{
-	struct se_node_acl *se_nacl, *se_nacl_new;
-	struct vhost_scsi_nacl *nacl;
-	u64 wwpn = 0;
-	u32 nexus_depth;
-
-	/* vhost_scsi_parse_wwn(name, &wwpn, 1) < 0)
-		return ERR_PTR(-EINVAL); */
-	se_nacl_new = vhost_scsi_alloc_fabric_acl(se_tpg);
-	if (!se_nacl_new)
-		return ERR_PTR(-ENOMEM);
-
-	nexus_depth = 1;
-	/*
-	 * se_nacl_new may be released by core_tpg_add_initiator_node_acl()
-	 * when converting a NodeACL from demo mode -> explict
-	 */
-	se_nacl = core_tpg_add_initiator_node_acl(se_tpg, se_nacl_new,
-				name, nexus_depth);
-	if (IS_ERR(se_nacl)) {
-		vhost_scsi_release_fabric_acl(se_tpg, se_nacl_new);
-		return se_nacl;
-	}
-	/*
-	 * Locate our struct vhost_scsi_nacl and set the FC Nport WWPN
-	 */
-	nacl = container_of(se_nacl, struct vhost_scsi_nacl, se_node_acl);
-	nacl->iport_wwpn = wwpn;
-
-	return se_nacl;
-}
-
-static void vhost_scsi_drop_nodeacl(struct se_node_acl *se_acl)
-{
-	struct vhost_scsi_nacl *nacl = container_of(se_acl,
-				struct vhost_scsi_nacl, se_node_acl);
-	core_tpg_del_initiator_node_acl(se_acl->se_tpg, se_acl, 1);
-	kfree(nacl);
-}
-
 static void vhost_scsi_free_cmd_map_res(struct vhost_scsi_nexus *nexus,
 				       struct se_session *se_sess)
 {
@@ -2200,8 +2002,7 @@ vhost_scsi_make_tpg(struct se_wwn *wwn,
 	tpg->tport = tport;
 	tpg->tport_tpgt = tpgt;
 
-	ret = core_tpg_register(&vhost_scsi_ops, wwn,
-				&tpg->se_tpg, tpg, TRANSPORT_TPG_TYPE_NORMAL);
+	ret = core_tpg_register(wwn, &tpg->se_tpg, tport->tport_proto_id);
 	if (ret < 0) {
 		kfree(tpg);
 		return NULL;
@@ -2325,20 +2126,13 @@ static struct target_core_fabric_ops vhost_scsi_ops = {
 	.module				= THIS_MODULE,
 	.name				= "vhost",
 	.get_fabric_name		= vhost_scsi_get_fabric_name,
-	.get_fabric_proto_ident		= vhost_scsi_get_fabric_proto_ident,
 	.tpg_get_wwn			= vhost_scsi_get_fabric_wwn,
 	.tpg_get_tag			= vhost_scsi_get_tpgt,
-	.tpg_get_default_depth		= vhost_scsi_get_default_depth,
-	.tpg_get_pr_transport_id	= vhost_scsi_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= vhost_scsi_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= vhost_scsi_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= vhost_scsi_check_true,
 	.tpg_check_demo_mode_cache	= vhost_scsi_check_true,
 	.tpg_check_demo_mode_write_protect = vhost_scsi_check_false,
 	.tpg_check_prod_mode_write_protect = vhost_scsi_check_false,
 	.tpg_check_prot_fabric_only	= vhost_scsi_check_prot_fabric_only,
-	.tpg_alloc_fabric_acl		= vhost_scsi_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= vhost_scsi_release_fabric_acl,
 	.tpg_get_inst_index		= vhost_scsi_tpg_get_inst_index,
 	.release_cmd			= vhost_scsi_release_cmd,
 	.check_stop_free		= vhost_scsi_check_stop_free,
@@ -2349,7 +2143,6 @@ static struct target_core_fabric_ops vhost_scsi_ops = {
 	.write_pending			= vhost_scsi_write_pending,
 	.write_pending_status		= vhost_scsi_write_pending_status,
 	.set_default_node_attributes	= vhost_scsi_set_default_node_attrs,
-	.get_task_tag			= vhost_scsi_get_task_tag,
 	.get_cmd_state			= vhost_scsi_get_cmd_state,
 	.queue_data_in			= vhost_scsi_queue_data_in,
 	.queue_status			= vhost_scsi_queue_status,
@@ -2364,10 +2157,6 @@ static struct target_core_fabric_ops vhost_scsi_ops = {
 	.fabric_drop_tpg		= vhost_scsi_drop_tpg,
 	.fabric_post_link		= vhost_scsi_port_link,
 	.fabric_pre_unlink		= vhost_scsi_port_unlink,
-	.fabric_make_np			= NULL,
-	.fabric_drop_np			= NULL,
-	.fabric_make_nodeacl		= vhost_scsi_make_nodeacl,
-	.fabric_drop_nodeacl		= vhost_scsi_drop_nodeacl,
 
 	.tfc_wwn_attrs			= vhost_scsi_wwn_attrs,
 	.tfc_tpg_base_attrs		= vhost_scsi_tpg_attrs,
diff --git a/drivers/vhost/vhost.c b/drivers/vhost/vhost.c
index 2ee28266fd07..9e8e004bb1c3 100644
--- a/drivers/vhost/vhost.c
+++ b/drivers/vhost/vhost.c
@@ -36,6 +36,77 @@ enum {
 #define vhost_used_event(vq) ((__virtio16 __user *)&vq->avail->ring[vq->num])
 #define vhost_avail_event(vq) ((__virtio16 __user *)&vq->used->ring[vq->num])
 
+#ifdef CONFIG_VHOST_CROSS_ENDIAN_LEGACY
+static void vhost_vq_reset_user_be(struct vhost_virtqueue *vq)
+{
+	vq->user_be = !virtio_legacy_is_little_endian();
+}
+
+static long vhost_set_vring_endian(struct vhost_virtqueue *vq, int __user *argp)
+{
+	struct vhost_vring_state s;
+
+	if (vq->private_data)
+		return -EBUSY;
+
+	if (copy_from_user(&s, argp, sizeof(s)))
+		return -EFAULT;
+
+	if (s.num != VHOST_VRING_LITTLE_ENDIAN &&
+	    s.num != VHOST_VRING_BIG_ENDIAN)
+		return -EINVAL;
+
+	vq->user_be = s.num;
+
+	return 0;
+}
+
+static long vhost_get_vring_endian(struct vhost_virtqueue *vq, u32 idx,
+				   int __user *argp)
+{
+	struct vhost_vring_state s = {
+		.index = idx,
+		.num = vq->user_be
+	};
+
+	if (copy_to_user(argp, &s, sizeof(s)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static void vhost_init_is_le(struct vhost_virtqueue *vq)
+{
+	/* Note for legacy virtio: user_be is initialized at reset time
+	 * according to the host endianness. If userspace does not set an
+	 * explicit endianness, the default behavior is native endian, as
+	 * expected by legacy virtio.
+	 */
+	vq->is_le = vhost_has_feature(vq, VIRTIO_F_VERSION_1) || !vq->user_be;
+}
+#else
+static void vhost_vq_reset_user_be(struct vhost_virtqueue *vq)
+{
+}
+
+static long vhost_set_vring_endian(struct vhost_virtqueue *vq, int __user *argp)
+{
+	return -ENOIOCTLCMD;
+}
+
+static long vhost_get_vring_endian(struct vhost_virtqueue *vq, u32 idx,
+				   int __user *argp)
+{
+	return -ENOIOCTLCMD;
+}
+
+static void vhost_init_is_le(struct vhost_virtqueue *vq)
+{
+	if (vhost_has_feature(vq, VIRTIO_F_VERSION_1))
+		vq->is_le = true;
+}
+#endif /* CONFIG_VHOST_CROSS_ENDIAN_LEGACY */
+
 static void vhost_poll_func(struct file *file, wait_queue_head_t *wqh,
 			    poll_table *pt)
 {
@@ -199,6 +270,8 @@ static void vhost_vq_reset(struct vhost_dev *dev,
 	vq->call = NULL;
 	vq->log_ctx = NULL;
 	vq->memory = NULL;
+	vq->is_le = virtio_legacy_is_little_endian();
+	vhost_vq_reset_user_be(vq);
 }
 
 static int vhost_worker(void *data)
@@ -806,6 +879,12 @@ long vhost_vring_ioctl(struct vhost_dev *d, int ioctl, void __user *argp)
 		} else
 			filep = eventfp;
 		break;
+	case VHOST_SET_VRING_ENDIAN:
+		r = vhost_set_vring_endian(vq, argp);
+		break;
+	case VHOST_GET_VRING_ENDIAN:
+		r = vhost_get_vring_endian(vq, idx, argp);
+		break;
 	default:
 		r = -ENOIOCTLCMD;
 	}
@@ -1044,8 +1123,12 @@ int vhost_init_used(struct vhost_virtqueue *vq)
 {
 	__virtio16 last_used_idx;
 	int r;
-	if (!vq->private_data)
+	if (!vq->private_data) {
+		vq->is_le = virtio_legacy_is_little_endian();
 		return 0;
+	}
+
+	vhost_init_is_le(vq);
 
 	r = vhost_update_used_flags(vq);
 	if (r)
diff --git a/drivers/vhost/vhost.h b/drivers/vhost/vhost.h
index 8c1c792900ba..ce6f6da4b09f 100644
--- a/drivers/vhost/vhost.h
+++ b/drivers/vhost/vhost.h
@@ -106,6 +106,14 @@ struct vhost_virtqueue {
 	/* Log write descriptors */
 	void __user *log_base;
 	struct vhost_log *log;
+
+	/* Ring endianness. Defaults to legacy native endianness.
+	 * Set to true when starting a modern virtio device. */
+	bool is_le;
+#ifdef CONFIG_VHOST_CROSS_ENDIAN_LEGACY
+	/* Ring endianness requested by userspace for cross-endian support. */
+	bool user_be;
+#endif
 };
 
 struct vhost_dev {
@@ -173,34 +181,39 @@ static inline bool vhost_has_feature(struct vhost_virtqueue *vq, int bit)
 	return vq->acked_features & (1ULL << bit);
 }
 
+static inline bool vhost_is_little_endian(struct vhost_virtqueue *vq)
+{
+	return vq->is_le;
+}
+
 /* Memory accessors */
 static inline u16 vhost16_to_cpu(struct vhost_virtqueue *vq, __virtio16 val)
 {
-	return __virtio16_to_cpu(vhost_has_feature(vq, VIRTIO_F_VERSION_1), val);
+	return __virtio16_to_cpu(vhost_is_little_endian(vq), val);
 }
 
 static inline __virtio16 cpu_to_vhost16(struct vhost_virtqueue *vq, u16 val)
 {
-	return __cpu_to_virtio16(vhost_has_feature(vq, VIRTIO_F_VERSION_1), val);
+	return __cpu_to_virtio16(vhost_is_little_endian(vq), val);
 }
 
 static inline u32 vhost32_to_cpu(struct vhost_virtqueue *vq, __virtio32 val)
 {
-	return __virtio32_to_cpu(vhost_has_feature(vq, VIRTIO_F_VERSION_1), val);
+	return __virtio32_to_cpu(vhost_is_little_endian(vq), val);
 }
 
 static inline __virtio32 cpu_to_vhost32(struct vhost_virtqueue *vq, u32 val)
 {
-	return __cpu_to_virtio32(vhost_has_feature(vq, VIRTIO_F_VERSION_1), val);
+	return __cpu_to_virtio32(vhost_is_little_endian(vq), val);
 }
 
 static inline u64 vhost64_to_cpu(struct vhost_virtqueue *vq, __virtio64 val)
 {
-	return __virtio64_to_cpu(vhost_has_feature(vq, VIRTIO_F_VERSION_1), val);
+	return __virtio64_to_cpu(vhost_is_little_endian(vq), val);
 }
 
 static inline __virtio64 cpu_to_vhost64(struct vhost_virtqueue *vq, u64 val)
 {
-	return __cpu_to_virtio64(vhost_has_feature(vq, VIRTIO_F_VERSION_1), val);
+	return __cpu_to_virtio64(vhost_is_little_endian(vq), val);
 }
 #endif
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index 2d9923a60076..0505b796d743 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -36,14 +36,16 @@ config LCD_CORGI
 
 config LCD_L4F00242T03
 	tristate "Epson L4F00242T03 LCD"
-	depends on SPI_MASTER && GPIOLIB
+	depends on SPI_MASTER
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  SPI driver for Epson L4F00242T03. This provides basic support
 	  for init and powering the LCD up/down through a sysfs interface.
 
 config LCD_LMS283GF05
 	tristate "Samsung LMS283GF05 LCD"
-	depends on SPI_MASTER && GPIOLIB
+	depends on SPI_MASTER
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  SPI driver for Samsung LMS283GF05. This provides basic support
 	  for powering the LCD up/down through a sysfs interface.
@@ -434,7 +436,7 @@ config BACKLIGHT_AS3711
 
 config BACKLIGHT_GPIO
 	tristate "Generic GPIO based Backlight Driver"
-	depends on GPIOLIB
+	depends on GPIOLIB || COMPILE_TEST
 	help
 	  If you have a LCD backlight adjustable by GPIO, say Y to enable
 	  this driver.
diff --git a/drivers/video/backlight/da9052_bl.c b/drivers/video/backlight/da9052_bl.c
index b1943e7735a1..fd2be417aa64 100644
--- a/drivers/video/backlight/da9052_bl.c
+++ b/drivers/video/backlight/da9052_bl.c
@@ -152,7 +152,7 @@ static int da9052_backlight_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct platform_device_id da9052_wled_ids[] = {
+static const struct platform_device_id da9052_wled_ids[] = {
 	{
 		.name		= "da9052-wled1",
 		.driver_data	= DA9052_TYPE_WLED1,
diff --git a/drivers/video/backlight/gpio_backlight.c b/drivers/video/backlight/gpio_backlight.c
index 439feb2389a8..5fbbc2ebdf93 100644
--- a/drivers/video/backlight/gpio_backlight.c
+++ b/drivers/video/backlight/gpio_backlight.c
@@ -146,6 +146,8 @@ static struct of_device_id gpio_backlight_of_match[] = {
 	{ .compatible = "gpio-backlight" },
 	{ /* sentinel */ }
 };
+
+MODULE_DEVICE_TABLE(of, gpio_backlight_of_match);
 #endif
 
 static struct platform_driver gpio_backlight_driver = {
diff --git a/drivers/video/backlight/lp855x_bl.c b/drivers/video/backlight/lp855x_bl.c
index a26d3bb25650..88116b493f3b 100644
--- a/drivers/video/backlight/lp855x_bl.c
+++ b/drivers/video/backlight/lp855x_bl.c
@@ -257,21 +257,15 @@ static void lp855x_pwm_ctrl(struct lp855x *lp, int br, int max_br)
 static int lp855x_bl_update_status(struct backlight_device *bl)
 {
 	struct lp855x *lp = bl_get_data(bl);
+	int brightness = bl->props.brightness;
 
 	if (bl->props.state & (BL_CORE_SUSPENDED | BL_CORE_FBBLANK))
-		bl->props.brightness = 0;
+		brightness = 0;
 
-	if (lp->mode == PWM_BASED) {
-		int br = bl->props.brightness;
-		int max_br = bl->props.max_brightness;
-
-		lp855x_pwm_ctrl(lp, br, max_br);
-
-	} else if (lp->mode == REGISTER_BASED) {
-		u8 val = bl->props.brightness;
-
-		lp855x_write_byte(lp, lp->cfg->reg_brightness, val);
-	}
+	if (lp->mode == PWM_BASED)
+		lp855x_pwm_ctrl(lp, brightness, bl->props.max_brightness);
+	else if (lp->mode == REGISTER_BASED)
+		lp855x_write_byte(lp, lp->cfg->reg_brightness, (u8)brightness);
 
 	return 0;
 }
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index 6897f1c1bc73..eff379b234cc 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -241,7 +241,8 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	pb->dev = &pdev->dev;
 	pb->enabled = false;
 
-	pb->enable_gpio = devm_gpiod_get_optional(&pdev->dev, "enable");
+	pb->enable_gpio = devm_gpiod_get_optional(&pdev->dev, "enable",
+						  GPIOD_OUT_HIGH);
 	if (IS_ERR(pb->enable_gpio)) {
 		ret = PTR_ERR(pb->enable_gpio);
 		goto err_alloc;
@@ -263,9 +264,6 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 		pb->enable_gpio = gpio_to_desc(data->enable_gpio);
 	}
 
-	if (pb->enable_gpio)
-		gpiod_direction_output(pb->enable_gpio, 1);
-
 	pb->power_supply = devm_regulator_get(&pdev->dev, "power");
 	if (IS_ERR(pb->power_supply)) {
 		ret = PTR_ERR(pb->power_supply);
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index b97210671a81..658c34bb9076 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -402,7 +402,7 @@ static void cursor_timer_handler(unsigned long dev_addr)
 	struct fbcon_ops *ops = info->fbcon_par;
 
 	queue_work(system_power_efficient_wq, &info->queue);
-	mod_timer(&ops->cursor_timer, jiffies + HZ/5);
+	mod_timer(&ops->cursor_timer, jiffies + ops->cur_blink_jiffies);
 }
 
 static void fbcon_add_cursor_timer(struct fb_info *info)
@@ -417,7 +417,7 @@ static void fbcon_add_cursor_timer(struct fb_info *info)
 
 		init_timer(&ops->cursor_timer);
 		ops->cursor_timer.function = cursor_timer_handler;
-		ops->cursor_timer.expires = jiffies + HZ / 5;
+		ops->cursor_timer.expires = jiffies + ops->cur_blink_jiffies;
 		ops->cursor_timer.data = (unsigned long ) info;
 		add_timer(&ops->cursor_timer);
 		ops->flags |= FBCON_FLAGS_CURSOR_TIMER;
@@ -1309,6 +1309,7 @@ static void fbcon_cursor(struct vc_data *vc, int mode)
 	if (fbcon_is_inactive(vc, info) || vc->vc_deccm != 1)
 		return;
 
+	ops->cur_blink_jiffies = msecs_to_jiffies(vc->vc_cur_blink_ms);
 	if (vc->vc_cursor_type & 0x10)
 		fbcon_del_cursor_timer(info);
 	else
diff --git a/drivers/video/console/fbcon.h b/drivers/video/console/fbcon.h
index 6bd2e0c7f209..7aaa4eabbba0 100644
--- a/drivers/video/console/fbcon.h
+++ b/drivers/video/console/fbcon.h
@@ -70,6 +70,7 @@ struct fbcon_ops {
 	struct fb_cursor cursor_state;
 	struct display *p;
         int    currcon;	                /* Current VC. */
+	int    cur_blink_jiffies;
 	int    cursor_flash;
 	int    cursor_reset;
 	int    blank_state;
diff --git a/drivers/video/console/newport_con.c b/drivers/video/console/newport_con.c
index a6ab9299813c..bb4e96255974 100644
--- a/drivers/video/console/newport_con.c
+++ b/drivers/video/console/newport_con.c
@@ -687,7 +687,7 @@ static int newport_scroll(struct vc_data *vc, int t, int b, int dir,
 static void newport_bmove(struct vc_data *vc, int sy, int sx, int dy,
 			  int dx, int h, int w)
 {
-	short xs, ys, xe, ye, xoffs, yoffs, tmp;
+	short xs, ys, xe, ye, xoffs, yoffs;
 
 	xs = sx << 3;
 	xe = ((sx + w) << 3) - 1;
@@ -701,9 +701,7 @@ static void newport_bmove(struct vc_data *vc, int sy, int sx, int dy,
 	yoffs = (dy - sy) << 4;
 	if (xoffs > 0) {
 		/* move to the right, exchange starting points */
-		tmp = xe;
-		xe = xs;
-		xs = tmp;
+		swap(xe, xs);
 	}
 	newport_wait(npregs);
 	npregs->set.drawmode0 = (NPORT_DMODE0_S2S | NPORT_DMODE0_BLOCK |
diff --git a/drivers/video/fbdev/Kconfig b/drivers/video/fbdev/Kconfig
index 109462303087..2d98de535e0f 100644
--- a/drivers/video/fbdev/Kconfig
+++ b/drivers/video/fbdev/Kconfig
@@ -2326,13 +2326,6 @@ config FB_PRE_INIT_FB
 	  Select this option if display contents should be inherited as set by
 	  the bootloader.
 
-config FB_MSM
-	tristate "MSM Framebuffer support"
-	depends on FB && ARCH_MSM
-	select FB_CFB_FILLRECT
-	select FB_CFB_COPYAREA
-	select FB_CFB_IMAGEBLIT
-
 config FB_MX3
 	tristate "MX3 Framebuffer support"
 	depends on FB && MX3_IPU
@@ -2478,6 +2471,7 @@ config FB_SSD1307
 	select FB_SYS_IMAGEBLIT
 	select FB_DEFERRED_IO
 	select PWM
+	select FB_BACKLIGHT
 	help
 	  This driver implements support for the Solomon SSD1307
 	  OLED controller over I2C.
diff --git a/drivers/video/fbdev/Makefile b/drivers/video/fbdev/Makefile
index 1979afffccfe..cecea5063a80 100644
--- a/drivers/video/fbdev/Makefile
+++ b/drivers/video/fbdev/Makefile
@@ -126,7 +126,6 @@ obj-y                             += omap2/
 obj-$(CONFIG_XEN_FBDEV_FRONTEND)  += xen-fbfront.o
 obj-$(CONFIG_FB_CARMINE)          += carminefb.o
 obj-$(CONFIG_FB_MB862XX)	  += mb862xx/
-obj-$(CONFIG_FB_MSM)              += msm/
 obj-$(CONFIG_FB_NUC900)           += nuc900fb.o
 obj-$(CONFIG_FB_JZ4740)		  += jz4740_fb.o
 obj-$(CONFIG_FB_PUV3_UNIGFX)      += fb-puv3.o
diff --git a/drivers/video/fbdev/amifb.c b/drivers/video/fbdev/amifb.c
index 35f7900a0573..1d702e13aaff 100644
--- a/drivers/video/fbdev/amifb.c
+++ b/drivers/video/fbdev/amifb.c
@@ -2052,7 +2052,7 @@ static void ami_set_sprite(const struct amifb_par *par)
 {
 	copins *copl, *cops;
 	u_short hs, vs, ve;
-	u_long pl, ps, pt;
+	u_long pl, ps;
 	short mx, my;
 
 	cops = copdisplay.list[currentcop][0];
@@ -2078,7 +2078,7 @@ static void ami_set_sprite(const struct amifb_par *par)
 			if (mod2(vs)) {
 				lofsprite[1 << par->crsr.fmode] = spr2hw_ctl(vs, hs, ve);
 				shfsprite[1 << par->crsr.fmode] = spr2hw_ctl(vs + 1, hs, ve + 1);
-				pt = pl; pl = ps; ps = pt;
+				swap(pl, ps);
 			} else {
 				lofsprite[1 << par->crsr.fmode] = spr2hw_ctl(vs, hs, ve + 1);
 				shfsprite[1 << par->crsr.fmode] = spr2hw_ctl(vs + 1, hs, ve);
@@ -3705,8 +3705,8 @@ default_chipset:
 	 * access the videomem with writethrough cache
 	 */
 	info->fix.smem_start = (u_long)ZTWO_PADDR(videomemory);
-	videomemory = (u_long)ioremap_writethrough(info->fix.smem_start,
-						   info->fix.smem_len);
+	videomemory = (u_long)ioremap_wt(info->fix.smem_start,
+					 info->fix.smem_len);
 	if (!videomemory) {
 		dev_warn(&pdev->dev,
 			 "Unable to map videomem cached writethrough\n");
diff --git a/drivers/video/fbdev/atafb.c b/drivers/video/fbdev/atafb.c
index cb9ee2556850..d6ce613e12ad 100644
--- a/drivers/video/fbdev/atafb.c
+++ b/drivers/video/fbdev/atafb.c
@@ -3185,8 +3185,7 @@ int __init atafb_init(void)
 		/* Map the video memory (physical address given) to somewhere
 		 * in the kernel address space.
 		 */
-		external_screen_base = ioremap_writethrough(external_addr,
-						     external_len);
+		external_screen_base = ioremap_wt(external_addr, external_len);
 		if (external_vgaiobase)
 			external_vgaiobase =
 			  (unsigned long)ioremap(external_vgaiobase, 0x10000);
diff --git a/drivers/video/fbdev/atmel_lcdfb.c b/drivers/video/fbdev/atmel_lcdfb.c
index 94a8d04e60f9..abadc490fa1f 100644
--- a/drivers/video/fbdev/atmel_lcdfb.c
+++ b/drivers/video/fbdev/atmel_lcdfb.c
@@ -1266,7 +1266,8 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 			goto stop_clk;
 		}
 
-		info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+		info->screen_base = ioremap_wc(info->fix.smem_start,
+					       info->fix.smem_len);
 		if (!info->screen_base) {
 			ret = -ENOMEM;
 			goto release_intmem;
diff --git a/drivers/video/fbdev/aty/aty128fb.c b/drivers/video/fbdev/aty/aty128fb.c
index 0156954bf340..c42ce2fdfd44 100644
--- a/drivers/video/fbdev/aty/aty128fb.c
+++ b/drivers/video/fbdev/aty/aty128fb.c
@@ -80,10 +80,6 @@
 #include <asm/btext.h>
 #endif /* CONFIG_BOOTX_TEXT */
 
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
-
 #include <video/aty128.h>
 
 /* Debug flag */
@@ -399,10 +395,7 @@ static int default_cmode = CMODE_8;
 
 static int default_crt_on = 0;
 static int default_lcd_on = 1;
-
-#ifdef CONFIG_MTRR
 static bool mtrr = true;
-#endif
 
 #ifdef CONFIG_FB_ATY128_BACKLIGHT
 #ifdef CONFIG_PMAC_BACKLIGHT
@@ -456,9 +449,7 @@ struct aty128fb_par {
 	u32 vram_size;                      /* onboard video ram   */
 	int chip_gen;
 	const struct aty128_meminfo *mem;   /* onboard mem info    */
-#ifdef CONFIG_MTRR
-	struct { int vram; int vram_valid; } mtrr;
-#endif
+	int wc_cookie;
 	int blitter_may_be_busy;
 	int fifo_slots;                 /* free slots in FIFO (64 max) */
 
@@ -1725,12 +1716,10 @@ static int aty128fb_setup(char *options)
 #endif
 			continue;
 		}
-#ifdef CONFIG_MTRR
 		if(!strncmp(this_opt, "nomtrr", 6)) {
 			mtrr = 0;
 			continue;
 		}
-#endif
 #ifdef CONFIG_PPC_PMAC
 		/* vmode and cmode deprecated */
 		if (!strncmp(this_opt, "vmode:", 6)) {
@@ -2133,7 +2122,7 @@ static int aty128_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	par->vram_size = aty_ld_le32(CNFG_MEMSIZE) & 0x03FFFFFF;
 
 	/* Virtualize the framebuffer */
-	info->screen_base = ioremap(fb_addr, par->vram_size);
+	info->screen_base = ioremap_wc(fb_addr, par->vram_size);
 	if (!info->screen_base)
 		goto err_unmap_out;
 
@@ -2170,15 +2159,9 @@ static int aty128_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (!aty128_init(pdev, ent))
 		goto err_out;
 
-#ifdef CONFIG_MTRR
-	if (mtrr) {
-		par->mtrr.vram = mtrr_add(info->fix.smem_start,
-				par->vram_size, MTRR_TYPE_WRCOMB, 1);
-		par->mtrr.vram_valid = 1;
-		/* let there be speed */
-		printk(KERN_INFO "aty128fb: Rage128 MTRR set to ON\n");
-	}
-#endif /* CONFIG_MTRR */
+	if (mtrr)
+		par->wc_cookie = arch_phys_wc_add(info->fix.smem_start,
+						  par->vram_size);
 	return 0;
 
 err_out:
@@ -2212,11 +2195,7 @@ static void aty128_remove(struct pci_dev *pdev)
 	aty128_bl_exit(info->bl_dev);
 #endif
 
-#ifdef CONFIG_MTRR
-	if (par->mtrr.vram_valid)
-		mtrr_del(par->mtrr.vram, info->fix.smem_start,
-			 par->vram_size);
-#endif /* CONFIG_MTRR */
+	arch_phys_wc_del(par->wc_cookie);
 	iounmap(par->regbase);
 	iounmap(info->screen_base);
 
@@ -2625,8 +2604,5 @@ MODULE_DESCRIPTION("FBDev driver for ATI Rage128 / Pro cards");
 MODULE_LICENSE("GPL");
 module_param(mode_option, charp, 0);
 MODULE_PARM_DESC(mode_option, "Specify resolution as \"<xres>x<yres>[-<bpp>][@<refresh>]\" ");
-#ifdef CONFIG_MTRR
 module_param_named(nomtrr, mtrr, invbool, 0);
 MODULE_PARM_DESC(nomtrr, "bool: Disable MTRR support (0 or 1=disabled) (default=0)");
-#endif
-
diff --git a/drivers/video/fbdev/aty/radeon_base.c b/drivers/video/fbdev/aty/radeon_base.c
index 01237c8fcdc6..2bdb070707e4 100644
--- a/drivers/video/fbdev/aty/radeon_base.c
+++ b/drivers/video/fbdev/aty/radeon_base.c
@@ -85,10 +85,6 @@
 
 #endif /* CONFIG_PPC */
 
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
-
 #include <video/radeon.h>
 #include <linux/radeonfb.h>
 
@@ -271,9 +267,7 @@ static bool mirror = 0;
 static int panel_yres = 0;
 static bool force_dfp = 0;
 static bool force_measure_pll = 0;
-#ifdef CONFIG_MTRR
 static bool nomtrr = 0;
-#endif
 static bool force_sleep;
 static bool ignore_devlist;
 #ifdef CONFIG_PMAC_BACKLIGHT
@@ -2260,8 +2254,8 @@ static int radeonfb_pci_register(struct pci_dev *pdev,
 	rinfo->mapped_vram = min_t(unsigned long, MAX_MAPPED_VRAM, rinfo->video_ram);
 
 	do {
-		rinfo->fb_base = ioremap (rinfo->fb_base_phys,
-					  rinfo->mapped_vram);
+		rinfo->fb_base = ioremap_wc(rinfo->fb_base_phys,
+					    rinfo->mapped_vram);
 	} while (rinfo->fb_base == NULL &&
 		 ((rinfo->mapped_vram /= 2) >= MIN_MAPPED_VRAM));
 
@@ -2359,11 +2353,9 @@ static int radeonfb_pci_register(struct pci_dev *pdev,
 		goto err_unmap_fb;
 	}
 
-#ifdef CONFIG_MTRR
-	rinfo->mtrr_hdl = nomtrr ? -1 : mtrr_add(rinfo->fb_base_phys,
-						 rinfo->video_ram,
-						 MTRR_TYPE_WRCOMB, 1);
-#endif
+	if (!nomtrr)
+		rinfo->wc_cookie = arch_phys_wc_add(rinfo->fb_base_phys,
+						    rinfo->video_ram);
 
 	if (backlight)
 		radeonfb_bl_init(rinfo);
@@ -2428,12 +2420,7 @@ static void radeonfb_pci_unregister(struct pci_dev *pdev)
  #endif
 
 	del_timer_sync(&rinfo->lvds_timer);
-
-#ifdef CONFIG_MTRR
-	if (rinfo->mtrr_hdl >= 0)
-		mtrr_del(rinfo->mtrr_hdl, 0, 0);
-#endif
-
+	arch_phys_wc_del(rinfo->wc_cookie);
         unregister_framebuffer(info);
 
         radeonfb_bl_exit(rinfo);
@@ -2489,10 +2476,8 @@ static int __init radeonfb_setup (char *options)
 			panel_yres = simple_strtoul((this_opt+11), NULL, 0);
 		} else if (!strncmp(this_opt, "backlight:", 10)) {
 			backlight = simple_strtoul(this_opt+10, NULL, 0);
-#ifdef CONFIG_MTRR
 		} else if (!strncmp(this_opt, "nomtrr", 6)) {
 			nomtrr = 1;
-#endif
 		} else if (!strncmp(this_opt, "nomodeset", 9)) {
 			nomodeset = 1;
 		} else if (!strncmp(this_opt, "force_measure_pll", 17)) {
@@ -2552,10 +2537,8 @@ module_param(monitor_layout, charp, 0);
 MODULE_PARM_DESC(monitor_layout, "Specify monitor mapping (like XFree86)");
 module_param(force_measure_pll, bool, 0);
 MODULE_PARM_DESC(force_measure_pll, "Force measurement of PLL (debug)");
-#ifdef CONFIG_MTRR
 module_param(nomtrr, bool, 0);
 MODULE_PARM_DESC(nomtrr, "bool: disable use of MTRR registers");
-#endif
 module_param(panel_yres, int, 0);
 MODULE_PARM_DESC(panel_yres, "int: set panel yres");
 module_param(mode_option, charp, 0);
diff --git a/drivers/video/fbdev/aty/radeonfb.h b/drivers/video/fbdev/aty/radeonfb.h
index 039def41c920..5bc1944ea1a9 100644
--- a/drivers/video/fbdev/aty/radeonfb.h
+++ b/drivers/video/fbdev/aty/radeonfb.h
@@ -340,7 +340,7 @@ struct radeonfb_info {
 
 	struct pll_info		pll;
 
-	int			mtrr_hdl;
+	int			wc_cookie;
 
 	u32			save_regs[100];
 	int			asleep;
diff --git a/drivers/video/fbdev/core/Makefile b/drivers/video/fbdev/core/Makefile
index 67f28e20a892..23d86a8b7d7b 100644
--- a/drivers/video/fbdev/core/Makefile
+++ b/drivers/video/fbdev/core/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_FB_CMDLINE)          += fb_cmdline.o
 obj-$(CONFIG_FB)                  += fb.o
 fb-y                              := fbmem.o fbmon.o fbcmap.o fbsysfs.o \
                                      modedb.o fbcvt.o
+fb-$(CONFIG_FB_DEFERRED_IO)       += fb_defio.o
 fb-objs                           := $(fb-y)
 
 obj-$(CONFIG_FB_CFB_FILLRECT)  += cfbfillrect.o
@@ -14,4 +15,3 @@ obj-$(CONFIG_FB_SYS_IMAGEBLIT) += sysimgblt.o
 obj-$(CONFIG_FB_SYS_FOPS)      += fb_sys_fops.o
 obj-$(CONFIG_FB_SVGALIB)       += svgalib.o
 obj-$(CONFIG_FB_DDC)           += fb_ddc.o
-obj-$(CONFIG_FB_DEFERRED_IO)   += fb_defio.o
diff --git a/drivers/video/fbdev/core/fb_defio.c b/drivers/video/fbdev/core/fb_defio.c
index d6cab1fd9a47..3fc63c208d08 100644
--- a/drivers/video/fbdev/core/fb_defio.c
+++ b/drivers/video/fbdev/core/fb_defio.c
@@ -242,5 +242,3 @@ void fb_deferred_io_cleanup(struct fb_info *info)
 	mutex_destroy(&fbdefio->lock);
 }
 EXPORT_SYMBOL_GPL(fb_deferred_io_cleanup);
-
-MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/core/fbmon.c b/drivers/video/fbdev/core/fbmon.c
index 01ef1b953390..d787533d9c8b 100644
--- a/drivers/video/fbdev/core/fbmon.c
+++ b/drivers/video/fbdev/core/fbmon.c
@@ -1475,7 +1475,9 @@ int of_get_fb_videomode(struct device_node *np, struct fb_videomode *fb,
 	if (ret)
 		return ret;
 
-	fb_videomode_from_videomode(&vm, fb);
+	ret = fb_videomode_from_videomode(&vm, fb);
+	if (ret)
+		return ret;
 
 	pr_debug("%s: got %dx%d display mode from %s\n",
 		of_node_full_name(np), vm.hactive, vm.vactive, np->name);
diff --git a/drivers/video/fbdev/gbefb.c b/drivers/video/fbdev/gbefb.c
index 6d9ef39810c8..b63d55f481fa 100644
--- a/drivers/video/fbdev/gbefb.c
+++ b/drivers/video/fbdev/gbefb.c
@@ -22,9 +22,6 @@
 #include <linux/module.h>
 #include <linux/io.h>
 
-#ifdef CONFIG_X86
-#include <asm/mtrr.h>
-#endif
 #ifdef CONFIG_MIPS
 #include <asm/addrspace.h>
 #endif
@@ -38,6 +35,7 @@ static struct sgi_gbe *gbe;
 struct gbefb_par {
 	struct fb_var_screeninfo var;
 	struct gbe_timing_info timing;
+	int wc_cookie;
 	int valid;
 };
 
@@ -1175,8 +1173,8 @@ static int gbefb_probe(struct platform_device *p_dev)
 
 	if (gbe_mem_phys) {
 		/* memory was allocated at boot time */
-		gbe_mem = devm_ioremap_nocache(&p_dev->dev, gbe_mem_phys,
-					       gbe_mem_size);
+		gbe_mem = devm_ioremap_wc(&p_dev->dev, gbe_mem_phys,
+					  gbe_mem_size);
 		if (!gbe_mem) {
 			printk(KERN_ERR "gbefb: couldn't map framebuffer\n");
 			ret = -ENOMEM;
@@ -1187,8 +1185,8 @@ static int gbefb_probe(struct platform_device *p_dev)
 	} else {
 		/* try to allocate memory with the classical allocator
 		 * this has high chance to fail on low memory machines */
-		gbe_mem = dma_alloc_coherent(NULL, gbe_mem_size, &gbe_dma_addr,
-					     GFP_KERNEL);
+		gbe_mem = dma_alloc_writecombine(NULL, gbe_mem_size,
+						 &gbe_dma_addr, GFP_KERNEL);
 		if (!gbe_mem) {
 			printk(KERN_ERR "gbefb: couldn't allocate framebuffer memory\n");
 			ret = -ENOMEM;
@@ -1198,9 +1196,8 @@ static int gbefb_probe(struct platform_device *p_dev)
 		gbe_mem_phys = (unsigned long) gbe_dma_addr;
 	}
 
-#ifdef CONFIG_X86
-	mtrr_add(gbe_mem_phys, gbe_mem_size, MTRR_TYPE_WRCOMB, 1);
-#endif
+	par = info->par;
+	par->wc_cookie = arch_phys_wc_add(gbe_mem_phys, gbe_mem_size);
 
 	/* map framebuffer memory into tiles table */
 	for (i = 0; i < (gbe_mem_size >> TILE_SHIFT); i++)
@@ -1215,7 +1212,6 @@ static int gbefb_probe(struct platform_device *p_dev)
 	/* reset GBE */
 	gbe_reset();
 
-	par = info->par;
 	/* turn on default video mode */
 	if (fb_find_mode(&par->var, info, mode_option, NULL, 0,
 			 default_mode, 8) == 0)
@@ -1240,8 +1236,9 @@ static int gbefb_probe(struct platform_device *p_dev)
 	return 0;
 
 out_gbe_unmap:
+	arch_phys_wc_del(par->wc_cookie);
 	if (gbe_dma_addr)
-		dma_free_coherent(NULL, gbe_mem_size, gbe_mem, gbe_mem_phys);
+		dma_free_writecombine(NULL, gbe_mem_size, gbe_mem, gbe_mem_phys);
 out_tiles_free:
 	dma_free_coherent(NULL, GBE_TLB_SIZE * sizeof(uint16_t),
 			  (void *)gbe_tiles.cpu, gbe_tiles.dma);
@@ -1256,11 +1253,13 @@ out_release_framebuffer:
 static int gbefb_remove(struct platform_device* p_dev)
 {
 	struct fb_info *info = platform_get_drvdata(p_dev);
+	struct gbefb_par *par = info->par;
 
 	unregister_framebuffer(info);
 	gbe_turn_off();
+	arch_phys_wc_del(par->wc_cookie);
 	if (gbe_dma_addr)
-		dma_free_coherent(NULL, gbe_mem_size, gbe_mem, gbe_mem_phys);
+		dma_free_writecombine(NULL, gbe_mem_size, gbe_mem, gbe_mem_phys);
 	dma_free_coherent(NULL, GBE_TLB_SIZE * sizeof(uint16_t),
 			  (void *)gbe_tiles.cpu, gbe_tiles.dma);
 	release_mem_region(GBE_BASE, sizeof(struct sgi_gbe));
diff --git a/drivers/video/fbdev/geode/gxfb_core.c b/drivers/video/fbdev/geode/gxfb_core.c
index 124d7c7e2d14..ec9fc9ac23de 100644
--- a/drivers/video/fbdev/geode/gxfb_core.c
+++ b/drivers/video/fbdev/geode/gxfb_core.c
@@ -263,7 +263,8 @@ static int gxfb_map_video_memory(struct fb_info *info, struct pci_dev *dev)
 
 	info->fix.smem_start = pci_resource_start(dev, 0);
 	info->fix.smem_len = vram ? vram : gx_frame_buffer_size();
-	info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+	info->screen_base = ioremap_wc(info->fix.smem_start,
+				       info->fix.smem_len);
 	if (!info->screen_base)
 		return -ENOMEM;
 
diff --git a/drivers/video/fbdev/hpfb.c b/drivers/video/fbdev/hpfb.c
index a1b7e5fa9b09..9476d196f510 100644
--- a/drivers/video/fbdev/hpfb.c
+++ b/drivers/video/fbdev/hpfb.c
@@ -241,8 +241,8 @@ static int hpfb_init_one(unsigned long phys_base, unsigned long virt_base)
 	fb_info.fix.line_length = fb_width;
 	fb_height = (in_8(fb_regs + HPFB_FBHMSB) << 8) | in_8(fb_regs + HPFB_FBHLSB);
 	fb_info.fix.smem_len = fb_width * fb_height;
-	fb_start = (unsigned long)ioremap_writethrough(fb_info.fix.smem_start,
-						       fb_info.fix.smem_len);
+	fb_start = (unsigned long)ioremap_wt(fb_info.fix.smem_start,
+					     fb_info.fix.smem_len);
 	hpfb_defined.xres = (in_8(fb_regs + HPFB_DWMSB) << 8) | in_8(fb_regs + HPFB_DWLSB);
 	hpfb_defined.yres = (in_8(fb_regs + HPFB_DHMSB) << 8) | in_8(fb_regs + HPFB_DHLSB);
 	hpfb_defined.xres_virtual = hpfb_defined.xres;
diff --git a/drivers/video/fbdev/i810/i810.h b/drivers/video/fbdev/i810/i810.h
index 1414b73ac55b..7b1c002bfb08 100644
--- a/drivers/video/fbdev/i810/i810.h
+++ b/drivers/video/fbdev/i810/i810.h
@@ -199,7 +199,6 @@
 #define HAS_FONTCACHE               8 
 
 /* driver flags */
-#define HAS_MTRR                    1
 #define HAS_ACCELERATION            2
 #define ALWAYS_SYNC                 4
 #define LOCKUP                      8
@@ -281,7 +280,7 @@ struct i810fb_par {
 	u32 ovract;
 	u32 cur_state;
 	u32 ddc_num;
-	int mtrr_reg;
+	int wc_cookie;
 	u16 bltcntl;
 	u8 interlace;
 };
diff --git a/drivers/video/fbdev/i810/i810_main.c b/drivers/video/fbdev/i810/i810_main.c
index bb674e431741..025b882a4826 100644
--- a/drivers/video/fbdev/i810/i810_main.c
+++ b/drivers/video/fbdev/i810/i810_main.c
@@ -41,6 +41,7 @@
 #include <linux/resource.h>
 #include <linux/unistd.h>
 #include <linux/console.h>
+#include <linux/io.h>
 
 #include <asm/io.h>
 #include <asm/div64.h>
@@ -1816,7 +1817,9 @@ static void i810_init_device(struct i810fb_par *par)
 	u8 reg;
 	u8 __iomem *mmio = par->mmio_start_virtual;
 
-	if (mtrr) set_mtrr(par);
+	if (mtrr)
+		par->wc_cookie= arch_phys_wc_add((u32) par->aperture.physical,
+						 par->aperture.size);
 
 	i810_init_cursor(par);
 
@@ -1865,8 +1868,8 @@ static int i810_allocate_pci_resource(struct i810fb_par *par,
 	}
 	par->res_flags |= FRAMEBUFFER_REQ;
 
-	par->aperture.virtual = ioremap_nocache(par->aperture.physical, 
-					par->aperture.size);
+	par->aperture.virtual = ioremap_wc(par->aperture.physical,
+					   par->aperture.size);
 	if (!par->aperture.virtual) {
 		printk("i810fb_init: cannot remap framebuffer region\n");
 		return -ENODEV;
@@ -2096,7 +2099,7 @@ static void i810fb_release_resource(struct fb_info *info,
 				    struct i810fb_par *par)
 {
 	struct gtt_data *gtt = &par->i810_gtt;
-	unset_mtrr(par);
+	arch_phys_wc_del(par->wc_cookie);
 
 	i810_delete_i2c_busses(par);
 
diff --git a/drivers/video/fbdev/i810/i810_main.h b/drivers/video/fbdev/i810/i810_main.h
index a25afaa534ba..7bfaaad1d0fa 100644
--- a/drivers/video/fbdev/i810/i810_main.h
+++ b/drivers/video/fbdev/i810/i810_main.h
@@ -60,32 +60,6 @@ static inline void flush_cache(void)
 #define flush_cache() do { } while(0)
 #endif 
 
-#ifdef CONFIG_MTRR
-
-#include <asm/mtrr.h>
-
-static inline void set_mtrr(struct i810fb_par *par)
-{
-	par->mtrr_reg = mtrr_add((u32) par->aperture.physical, 
-		 par->aperture.size, MTRR_TYPE_WRCOMB, 1);
-	if (par->mtrr_reg < 0) {
-		printk(KERN_ERR "set_mtrr: unable to set MTRR\n");
-		return;
-	}
-	par->dev_flags |= HAS_MTRR;
-}
-static inline void unset_mtrr(struct i810fb_par *par)
-{
-  	if (par->dev_flags & HAS_MTRR) 
-  		mtrr_del(par->mtrr_reg, (u32) par->aperture.physical, 
-			 par->aperture.size); 
-}
-#else
-#define set_mtrr(x) printk("set_mtrr: MTRR is disabled in the kernel\n")
-
-#define unset_mtrr(x) do { } while (0)
-#endif /* CONFIG_MTRR */
-
 #ifdef CONFIG_FB_I810_GTF
 #define IS_DVT (0)
 #else
diff --git a/drivers/video/fbdev/imxfb.c b/drivers/video/fbdev/imxfb.c
index 84d1d29e532c..cee88603efc9 100644
--- a/drivers/video/fbdev/imxfb.c
+++ b/drivers/video/fbdev/imxfb.c
@@ -170,7 +170,7 @@ struct imxfb_info {
 	struct regulator	*lcd_pwr;
 };
 
-static struct platform_device_id imxfb_devtype[] = {
+static const struct platform_device_id imxfb_devtype[] = {
 	{
 		.name = "imx1-fb",
 		.driver_data = IMX1_FB,
diff --git a/drivers/video/fbdev/intelfb/intelfb.h b/drivers/video/fbdev/intelfb/intelfb.h
index 6b51175629c7..37f8339ea88c 100644
--- a/drivers/video/fbdev/intelfb/intelfb.h
+++ b/drivers/video/fbdev/intelfb/intelfb.h
@@ -285,9 +285,7 @@ struct intelfb_info {
 	/* use a gart reserved fb mem */
 	u8 fbmem_gart;
 
-	/* mtrr support */
-	int mtrr_reg;
-	u32 has_mtrr;
+	int wc_cookie;
 
 	/* heap data */
 	struct intelfb_heap_data aperture;
diff --git a/drivers/video/fbdev/intelfb/intelfbdrv.c b/drivers/video/fbdev/intelfb/intelfbdrv.c
index b847d530471a..bbec737eef30 100644
--- a/drivers/video/fbdev/intelfb/intelfbdrv.c
+++ b/drivers/video/fbdev/intelfb/intelfbdrv.c
@@ -124,10 +124,6 @@
 
 #include <asm/io.h>
 
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
-
 #include "intelfb.h"
 #include "intelfbhw.h"
 #include "../edid.h"
@@ -411,33 +407,6 @@ module_init(intelfb_init);
 module_exit(intelfb_exit);
 
 /***************************************************************
- *                     mtrr support functions                  *
- ***************************************************************/
-
-#ifdef CONFIG_MTRR
-static inline void set_mtrr(struct intelfb_info *dinfo)
-{
-	dinfo->mtrr_reg = mtrr_add(dinfo->aperture.physical,
-				   dinfo->aperture.size, MTRR_TYPE_WRCOMB, 1);
-	if (dinfo->mtrr_reg < 0) {
-		ERR_MSG("unable to set MTRR\n");
-		return;
-	}
-	dinfo->has_mtrr = 1;
-}
-static inline void unset_mtrr(struct intelfb_info *dinfo)
-{
-	if (dinfo->has_mtrr)
-		mtrr_del(dinfo->mtrr_reg, dinfo->aperture.physical,
-			 dinfo->aperture.size);
-}
-#else
-#define set_mtrr(x) WRN_MSG("MTRR is disabled in the kernel\n")
-
-#define unset_mtrr(x) do { } while (0)
-#endif /* CONFIG_MTRR */
-
-/***************************************************************
  *                        driver init / cleanup                *
  ***************************************************************/
 
@@ -456,7 +425,7 @@ static void cleanup(struct intelfb_info *dinfo)
 	if (dinfo->registered)
 		unregister_framebuffer(dinfo->info);
 
-	unset_mtrr(dinfo);
+	arch_phys_wc_del(dinfo->wc_cookie);
 
 	if (dinfo->fbmem_gart && dinfo->gtt_fb_mem) {
 		agp_unbind_memory(dinfo->gtt_fb_mem);
@@ -675,7 +644,7 @@ static int intelfb_pci_register(struct pci_dev *pdev,
 	/* Allocate memories (which aren't stolen) */
 	/* Map the fb and MMIO regions */
 	/* ioremap only up to the end of used aperture */
-	dinfo->aperture.virtual = (u8 __iomem *)ioremap_nocache
+	dinfo->aperture.virtual = (u8 __iomem *)ioremap_wc
 		(dinfo->aperture.physical, ((offset + dinfo->fb.offset) << 12)
 		 + dinfo->fb.size);
 	if (!dinfo->aperture.virtual) {
@@ -772,7 +741,8 @@ static int intelfb_pci_register(struct pci_dev *pdev,
 	agp_backend_release(bridge);
 
 	if (mtrr)
-		set_mtrr(dinfo);
+		dinfo->wc_cookie = arch_phys_wc_add(dinfo->aperture.physical,
+						    dinfo->aperture.size);
 
 	DBG_MSG("fb: 0x%x(+ 0x%x)/0x%x (0x%p)\n",
 		dinfo->fb.physical, dinfo->fb.offset, dinfo->fb.size,
diff --git a/drivers/video/fbdev/matrox/matroxfb_base.c b/drivers/video/fbdev/matrox/matroxfb_base.c
index 62539ca1cfa9..11eb094396ae 100644
--- a/drivers/video/fbdev/matrox/matroxfb_base.c
+++ b/drivers/video/fbdev/matrox/matroxfb_base.c
@@ -370,12 +370,9 @@ static void matroxfb_remove(struct matrox_fb_info *minfo, int dummy)
 	matroxfb_unregister_device(minfo);
 	unregister_framebuffer(&minfo->fbcon);
 	matroxfb_g450_shutdown(minfo);
-#ifdef CONFIG_MTRR
-	if (minfo->mtrr.vram_valid)
-		mtrr_del(minfo->mtrr.vram, minfo->video.base, minfo->video.len);
-#endif
-	mga_iounmap(minfo->mmio.vbase);
-	mga_iounmap(minfo->video.vbase);
+	arch_phys_wc_del(minfo->wc_cookie);
+	iounmap(minfo->mmio.vbase.vaddr);
+	iounmap(minfo->video.vbase.vaddr);
 	release_mem_region(minfo->video.base, minfo->video.len_maximum);
 	release_mem_region(minfo->mmio.base, 16384);
 	kfree(minfo);
@@ -591,12 +588,8 @@ static int matroxfb_decode_var(const struct matrox_fb_info *minfo,
 			unsigned int max_yres;
 
 			while (m1) {
-				int t;
-
 				while (m2 >= m1) m2 -= m1;
-				t = m1;
-				m1 = m2;
-				m2 = t;
+				swap(m1, m2);
 			}
 			m2 = linelen * PAGE_SIZE / m2;
 			*ydstorg = m2 = 0x400000 % m2;
@@ -1256,9 +1249,7 @@ static int nobios;			/* "matroxfb:nobios" */
 static int noinit = 1;			/* "matroxfb:init" */
 static int inverse;			/* "matroxfb:inverse" */
 static int sgram;			/* "matroxfb:sgram" */
-#ifdef CONFIG_MTRR
 static int mtrr = 1;			/* "matroxfb:nomtrr" */
-#endif
 static int grayscale;			/* "matroxfb:grayscale" */
 static int dev = -1;			/* "matroxfb:dev:xxxxx" */
 static unsigned int vesa = ~0;		/* "matroxfb:vesa:xxxxx" */
@@ -1717,14 +1708,17 @@ static int initMatrox2(struct matrox_fb_info *minfo, struct board *b)
 	if (mem && (mem < memsize))
 		memsize = mem;
 	err = -ENOMEM;
-	if (mga_ioremap(ctrlptr_phys, 16384, MGA_IOREMAP_MMIO, &minfo->mmio.vbase)) {
+
+	minfo->mmio.vbase.vaddr = ioremap_nocache(ctrlptr_phys, 16384);
+	if (!minfo->mmio.vbase.vaddr) {
 		printk(KERN_ERR "matroxfb: cannot ioremap(%lX, 16384), matroxfb disabled\n", ctrlptr_phys);
 		goto failVideoMR;
 	}
 	minfo->mmio.base = ctrlptr_phys;
 	minfo->mmio.len = 16384;
 	minfo->video.base = video_base_phys;
-	if (mga_ioremap(video_base_phys, memsize, MGA_IOREMAP_FB, &minfo->video.vbase)) {
+	minfo->video.vbase.vaddr = ioremap_wc(video_base_phys, memsize);
+	if (!minfo->video.vbase.vaddr) {
 		printk(KERN_ERR "matroxfb: cannot ioremap(%lX, %d), matroxfb disabled\n",
 			video_base_phys, memsize);
 		goto failCtrlIO;
@@ -1772,13 +1766,9 @@ static int initMatrox2(struct matrox_fb_info *minfo, struct board *b)
 	minfo->video.len_usable = minfo->video.len;
 	if (minfo->video.len_usable > b->base->maxdisplayable)
 		minfo->video.len_usable = b->base->maxdisplayable;
-#ifdef CONFIG_MTRR
-	if (mtrr) {
-		minfo->mtrr.vram = mtrr_add(video_base_phys, minfo->video.len, MTRR_TYPE_WRCOMB, 1);
-		minfo->mtrr.vram_valid = 1;
-		printk(KERN_INFO "matroxfb: MTRR's turned on\n");
-	}
-#endif	/* CONFIG_MTRR */
+	if (mtrr)
+		minfo->wc_cookie = arch_phys_wc_add(video_base_phys,
+						    minfo->video.len);
 
 	if (!minfo->devflags.novga)
 		request_region(0x3C0, 32, "matrox");
@@ -1947,9 +1937,9 @@ static int initMatrox2(struct matrox_fb_info *minfo, struct board *b)
 	return 0;
 failVideoIO:;
 	matroxfb_g450_shutdown(minfo);
-	mga_iounmap(minfo->video.vbase);
+	iounmap(minfo->video.vbase.vaddr);
 failCtrlIO:;
-	mga_iounmap(minfo->mmio.vbase);
+	iounmap(minfo->mmio.vbase.vaddr);
 failVideoMR:;
 	release_mem_region(video_base_phys, minfo->video.len_maximum);
 failCtrlMR:;
@@ -2443,10 +2433,8 @@ static int __init matroxfb_setup(char *options) {
 				nobios = !value;
 			else if (!strcmp(this_opt, "init"))
 				noinit = !value;
-#ifdef CONFIG_MTRR
 			else if (!strcmp(this_opt, "mtrr"))
 				mtrr = value;
-#endif
 			else if (!strcmp(this_opt, "inv24"))
 				inv24 = value;
 			else if (!strcmp(this_opt, "cross4MB"))
@@ -2515,10 +2503,8 @@ module_param(noinit, int, 0);
 MODULE_PARM_DESC(noinit, "Disables W/SG/SD-RAM and bus interface initialization (0 or 1=do not initialize) (default=0)");
 module_param(memtype, int, 0);
 MODULE_PARM_DESC(memtype, "Memory type for G200/G400 (see Documentation/fb/matroxfb.txt for explanation) (default=3 for G200, 0 for G400)");
-#ifdef CONFIG_MTRR
 module_param(mtrr, int, 0);
 MODULE_PARM_DESC(mtrr, "This speeds up video memory accesses (0=disabled or 1) (default=1)");
-#endif
 module_param(sgram, int, 0);
 MODULE_PARM_DESC(sgram, "Indicates that G100/G200/G400 has SGRAM memory (0=SDRAM, 1=SGRAM) (default=0)");
 module_param(inv24, int, 0);
diff --git a/drivers/video/fbdev/matrox/matroxfb_base.h b/drivers/video/fbdev/matrox/matroxfb_base.h
index 89a8a89a5eb2..09b02cd1eb0e 100644
--- a/drivers/video/fbdev/matrox/matroxfb_base.h
+++ b/drivers/video/fbdev/matrox/matroxfb_base.h
@@ -44,9 +44,6 @@
 
 #include <asm/io.h>
 #include <asm/unaligned.h>
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
 
 #if defined(CONFIG_PPC_PMAC)
 #include <asm/prom.h>
@@ -187,23 +184,6 @@ static inline void __iomem* vaddr_va(vaddr_t va) {
 	return va.vaddr;
 }
 
-#define MGA_IOREMAP_NORMAL	0
-#define MGA_IOREMAP_NOCACHE	1
-
-#define MGA_IOREMAP_FB		MGA_IOREMAP_NOCACHE
-#define MGA_IOREMAP_MMIO	MGA_IOREMAP_NOCACHE
-static inline int mga_ioremap(unsigned long phys, unsigned long size, int flags, vaddr_t* virt) {
-	if (flags & MGA_IOREMAP_NOCACHE)
-		virt->vaddr = ioremap_nocache(phys, size);
-	else
-		virt->vaddr = ioremap(phys, size);
-	return (virt->vaddr == NULL); /* 0, !0... 0, error_code in future */
-}
-
-static inline void mga_iounmap(vaddr_t va) {
-	iounmap(va.vaddr);
-}
-
 struct my_timming {
 	unsigned int pixclock;
 	int mnp;
@@ -449,12 +429,7 @@ struct matrox_fb_info {
 		int		plnwt;
 		int		srcorg;
 			      } capable;
-#ifdef CONFIG_MTRR
-	struct {
-		int		vram;
-		int		vram_valid;
-			      } mtrr;
-#endif
+	int			wc_cookie;
 	struct {
 		int		precise_width;
 		int		mga_24bpp_fix;
diff --git a/drivers/video/fbdev/msm/Makefile b/drivers/video/fbdev/msm/Makefile
deleted file mode 100644
index 802d6ae523fb..000000000000
--- a/drivers/video/fbdev/msm/Makefile
+++ /dev/null
@@ -1,19 +0,0 @@
-
-# core framebuffer
-#
-obj-y := msm_fb.o
-
-# MDP DMA/PPP engine
-#
-obj-y += mdp.o mdp_scale_tables.o mdp_ppp.o
-
-# MDDI interface
-#
-obj-y += mddi.o
-
-# MDDI client/panel drivers
-#
-obj-y += mddi_client_dummy.o
-obj-y += mddi_client_toshiba.o
-obj-y += mddi_client_nt35399.o
-
diff --git a/drivers/video/fbdev/msm/mddi.c b/drivers/video/fbdev/msm/mddi.c
deleted file mode 100644
index e0f8011a3c4b..000000000000
--- a/drivers/video/fbdev/msm/mddi.c
+++ /dev/null
@@ -1,821 +0,0 @@
-/*
- * MSM MDDI Transport
- *
- * Copyright (C) 2007 Google Incorporated
- * Copyright (C) 2007 QUALCOMM Incorporated
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
- * GNU General Public License for more details.
- *
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/dma-mapping.h>
-#include <linux/interrupt.h>
-#include <linux/platform_device.h>
-#include <linux/delay.h>
-#include <linux/gfp.h>
-#include <linux/spinlock.h>
-#include <linux/clk.h>
-#include <linux/io.h>
-#include <linux/sched.h>
-#include <linux/platform_data/video-msm_fb.h>
-#include "mddi_hw.h"
-
-#define FLAG_DISABLE_HIBERNATION 0x0001
-#define FLAG_HAVE_CAPS		 0x0002
-#define FLAG_HAS_VSYNC_IRQ	 0x0004
-#define FLAG_HAVE_STATUS	 0x0008
-
-#define CMD_GET_CLIENT_CAP     0x0601
-#define CMD_GET_CLIENT_STATUS  0x0602
-
-union mddi_rev {
-	unsigned char raw[MDDI_REV_BUFFER_SIZE];
-	struct mddi_rev_packet hdr;
-	struct mddi_client_status status;
-	struct mddi_client_caps caps;
-	struct mddi_register_access reg;
-};
-
-struct reg_read_info {
-	struct completion done;
-	uint32_t reg;
-	uint32_t status;
-	uint32_t result;
-};
-
-struct mddi_info {
-	uint16_t flags;
-	uint16_t version;
-	char __iomem *base;
-	int irq;
-	struct clk *clk;
-	struct msm_mddi_client_data client_data;
-
-	/* buffer for rev encap packets */
-	void *rev_data;
-	dma_addr_t rev_addr;
-	struct mddi_llentry *reg_write_data;
-	dma_addr_t reg_write_addr;
-	struct mddi_llentry *reg_read_data;
-	dma_addr_t reg_read_addr;
-	size_t rev_data_curr;
-
-	spinlock_t int_lock;
-	uint32_t int_enable;
-	uint32_t got_int;
-	wait_queue_head_t int_wait;
-
-	struct mutex reg_write_lock;
-	struct mutex reg_read_lock;
-	struct reg_read_info *reg_read;
-
-	struct mddi_client_caps caps;
-	struct mddi_client_status status;
-
-	void (*power_client)(struct msm_mddi_client_data *, int);
-
-	/* client device published to bind us to the
-	 * appropriate mddi_client driver
-	 */
-	char client_name[20];
-
-	struct platform_device client_pdev;
-};
-
-static void mddi_init_rev_encap(struct mddi_info *mddi);
-
-#define mddi_readl(r) readl(mddi->base + (MDDI_##r))
-#define mddi_writel(v, r) writel((v), mddi->base + (MDDI_##r))
-
-void mddi_activate_link(struct msm_mddi_client_data *cdata)
-{
-	struct mddi_info *mddi = container_of(cdata, struct mddi_info,
-					      client_data);
-
-	mddi_writel(MDDI_CMD_LINK_ACTIVE, CMD);
-}
-
-static void mddi_handle_link_list_done(struct mddi_info *mddi)
-{
-}
-
-static void mddi_reset_rev_encap_ptr(struct mddi_info *mddi)
-{
-	printk(KERN_INFO "mddi: resetting rev ptr\n");
-	mddi->rev_data_curr = 0;
-	mddi_writel(mddi->rev_addr, REV_PTR);
-	mddi_writel(mddi->rev_addr, REV_PTR);
-	mddi_writel(MDDI_CMD_FORCE_NEW_REV_PTR, CMD);
-}
-
-static void mddi_handle_rev_data(struct mddi_info *mddi, union mddi_rev *rev)
-{
-	int i;
-	struct reg_read_info *ri;
-
-	if ((rev->hdr.length <= MDDI_REV_BUFFER_SIZE - 2) &&
-	   (rev->hdr.length >= sizeof(struct mddi_rev_packet) - 2)) {
-
-		switch (rev->hdr.type) {
-		case TYPE_CLIENT_CAPS:
-			memcpy(&mddi->caps, &rev->caps,
-			       sizeof(struct mddi_client_caps));
-			mddi->flags |= FLAG_HAVE_CAPS;
-			wake_up(&mddi->int_wait);
-			break;
-		case TYPE_CLIENT_STATUS:
-			memcpy(&mddi->status, &rev->status,
-			       sizeof(struct mddi_client_status));
-			mddi->flags |= FLAG_HAVE_STATUS;
-			wake_up(&mddi->int_wait);
-			break;
-		case TYPE_REGISTER_ACCESS:
-			ri = mddi->reg_read;
-			if (ri == 0) {
-				printk(KERN_INFO "rev: got reg %x = %x without "
-						 " pending read\n",
-				       rev->reg.register_address,
-				       rev->reg.register_data_list);
-				break;
-			}
-			if (ri->reg != rev->reg.register_address) {
-				printk(KERN_INFO "rev: got reg %x = %x for "
-						 "wrong register, expected "
-						 "%x\n",
-				       rev->reg.register_address,
-				       rev->reg.register_data_list, ri->reg);
-				break;
-			}
-			mddi->reg_read = NULL;
-			ri->status = 0;
-			ri->result = rev->reg.register_data_list;
-			complete(&ri->done);
-			break;
-		default:
-			printk(KERN_INFO "rev: unknown reverse packet: "
-					 "len=%04x type=%04x CURR_REV_PTR=%x\n",
-			       rev->hdr.length, rev->hdr.type,
-			       mddi_readl(CURR_REV_PTR));
-			for (i = 0; i < rev->hdr.length + 2; i++) {
-				if ((i % 16) == 0)
-					printk(KERN_INFO "\n");
-				printk(KERN_INFO " %02x", rev->raw[i]);
-			}
-			printk(KERN_INFO "\n");
-			mddi_reset_rev_encap_ptr(mddi);
-		}
-	} else {
-		printk(KERN_INFO "bad rev length, %d, CURR_REV_PTR %x\n",
-		       rev->hdr.length, mddi_readl(CURR_REV_PTR));
-		mddi_reset_rev_encap_ptr(mddi);
-	}
-}
-
-static void mddi_wait_interrupt(struct mddi_info *mddi, uint32_t intmask);
-
-static void mddi_handle_rev_data_avail(struct mddi_info *mddi)
-{
-	uint32_t rev_data_count;
-	uint32_t rev_crc_err_count;
-	struct reg_read_info *ri;
-	size_t prev_offset;
-	uint16_t length;
-
-	union mddi_rev *crev = mddi->rev_data + mddi->rev_data_curr;
-
-	/* clear the interrupt */
-	mddi_writel(MDDI_INT_REV_DATA_AVAIL, INT);
-	rev_data_count = mddi_readl(REV_PKT_CNT);
-	rev_crc_err_count = mddi_readl(REV_CRC_ERR);
-	if (rev_data_count > 1)
-		printk(KERN_INFO "rev_data_count %d\n", rev_data_count);
-
-	if (rev_crc_err_count) {
-		printk(KERN_INFO "rev_crc_err_count %d, INT %x\n",
-		       rev_crc_err_count,  mddi_readl(INT));
-		ri = mddi->reg_read;
-		if (ri == 0) {
-			printk(KERN_INFO "rev: got crc error without pending "
-			       "read\n");
-		} else {
-			mddi->reg_read = NULL;
-			ri->status = -EIO;
-			ri->result = -1;
-			complete(&ri->done);
-		}
-	}
-
-	if (rev_data_count == 0)
-		return;
-
-	prev_offset = mddi->rev_data_curr;
-
-	length = *((uint8_t *)mddi->rev_data + mddi->rev_data_curr);
-	mddi->rev_data_curr++;
-	if (mddi->rev_data_curr == MDDI_REV_BUFFER_SIZE)
-		mddi->rev_data_curr = 0;
-	length += *((uint8_t *)mddi->rev_data + mddi->rev_data_curr) << 8;
-	mddi->rev_data_curr += 1 + length;
-	if (mddi->rev_data_curr >= MDDI_REV_BUFFER_SIZE)
-		mddi->rev_data_curr =
-			mddi->rev_data_curr % MDDI_REV_BUFFER_SIZE;
-
-	if (length > MDDI_REV_BUFFER_SIZE - 2) {
-		printk(KERN_INFO "mddi: rev data length greater than buffer"
-			"size\n");
-		mddi_reset_rev_encap_ptr(mddi);
-		return;
-	}
-
-	if (prev_offset + 2 + length >= MDDI_REV_BUFFER_SIZE) {
-		union mddi_rev tmprev;
-		size_t rem = MDDI_REV_BUFFER_SIZE - prev_offset;
-		memcpy(&tmprev.raw[0], mddi->rev_data + prev_offset, rem);
-		memcpy(&tmprev.raw[rem], mddi->rev_data, 2 + length - rem);
-		mddi_handle_rev_data(mddi, &tmprev);
-	} else {
-		mddi_handle_rev_data(mddi, crev);
-	}
-
-	if (prev_offset < MDDI_REV_BUFFER_SIZE / 2 &&
-	    mddi->rev_data_curr >= MDDI_REV_BUFFER_SIZE / 2) {
-		mddi_writel(mddi->rev_addr, REV_PTR);
-	}
-}
-
-static irqreturn_t mddi_isr(int irq, void *data)
-{
-	struct msm_mddi_client_data *cdata = data;
-	struct mddi_info *mddi = container_of(cdata, struct mddi_info,
-					      client_data);
-	uint32_t active, status;
-
-	spin_lock(&mddi->int_lock);
-
-	active = mddi_readl(INT);
-	status = mddi_readl(STAT);
-
-	mddi_writel(active, INT);
-
-	/* ignore any interrupts we have disabled */
-	active &= mddi->int_enable;
-
-	mddi->got_int |= active;
-	wake_up(&mddi->int_wait);
-
-	if (active & MDDI_INT_PRI_LINK_LIST_DONE) {
-		mddi->int_enable &= (~MDDI_INT_PRI_LINK_LIST_DONE);
-		mddi_handle_link_list_done(mddi);
-	}
-	if (active & MDDI_INT_REV_DATA_AVAIL)
-		mddi_handle_rev_data_avail(mddi);
-
-	if (active & ~MDDI_INT_NEED_CLEAR)
-		mddi->int_enable &= ~(active & ~MDDI_INT_NEED_CLEAR);
-
-	if (active & MDDI_INT_LINK_ACTIVE) {
-		mddi->int_enable &= (~MDDI_INT_LINK_ACTIVE);
-		mddi->int_enable |= MDDI_INT_IN_HIBERNATION;
-	}
-
-	if (active & MDDI_INT_IN_HIBERNATION) {
-		mddi->int_enable &= (~MDDI_INT_IN_HIBERNATION);
-		mddi->int_enable |= MDDI_INT_LINK_ACTIVE;
-	}
-
-	mddi_writel(mddi->int_enable, INTEN);
-	spin_unlock(&mddi->int_lock);
-
-	return IRQ_HANDLED;
-}
-
-static long mddi_wait_interrupt_timeout(struct mddi_info *mddi,
-					uint32_t intmask, int timeout)
-{
-	unsigned long irq_flags;
-
-	spin_lock_irqsave(&mddi->int_lock, irq_flags);
-	mddi->got_int &= ~intmask;
-	mddi->int_enable |= intmask;
-	mddi_writel(mddi->int_enable, INTEN);
-	spin_unlock_irqrestore(&mddi->int_lock, irq_flags);
-	return wait_event_timeout(mddi->int_wait, mddi->got_int & intmask,
-				  timeout);
-}
-
-static void mddi_wait_interrupt(struct mddi_info *mddi, uint32_t intmask)
-{
-	if (mddi_wait_interrupt_timeout(mddi, intmask, HZ/10) == 0)
-		printk(KERN_INFO "mddi_wait_interrupt %d, timeout "
-		       "waiting for %x, INT = %x, STAT = %x gotint = %x\n",
-		       current->pid, intmask, mddi_readl(INT), mddi_readl(STAT),
-		       mddi->got_int);
-}
-
-static void mddi_init_rev_encap(struct mddi_info *mddi)
-{
-	memset(mddi->rev_data, 0xee, MDDI_REV_BUFFER_SIZE);
-	mddi_writel(mddi->rev_addr, REV_PTR);
-	mddi_writel(MDDI_CMD_FORCE_NEW_REV_PTR, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-}
-
-void mddi_set_auto_hibernate(struct msm_mddi_client_data *cdata, int on)
-{
-	struct mddi_info *mddi = container_of(cdata, struct mddi_info,
-					      client_data);
-	mddi_writel(MDDI_CMD_POWERDOWN, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_IN_HIBERNATION);
-	mddi_writel(MDDI_CMD_HIBERNATE | !!on, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-}
-
-
-static uint16_t mddi_init_registers(struct mddi_info *mddi)
-{
-	mddi_writel(0x0001, VERSION);
-	mddi_writel(MDDI_HOST_BYTES_PER_SUBFRAME, BPS);
-	mddi_writel(0x0003, SPM); /* subframes per media */
-	mddi_writel(0x0005, TA1_LEN);
-	mddi_writel(MDDI_HOST_TA2_LEN, TA2_LEN);
-	mddi_writel(0x0096, DRIVE_HI);
-	/* 0x32 normal, 0x50 for Toshiba display */
-	mddi_writel(0x0050, DRIVE_LO);
-	mddi_writel(0x003C, DISP_WAKE); /* wakeup counter */
-	mddi_writel(MDDI_HOST_REV_RATE_DIV, REV_RATE_DIV);
-
-	mddi_writel(MDDI_REV_BUFFER_SIZE, REV_SIZE);
-	mddi_writel(MDDI_MAX_REV_PKT_SIZE, REV_ENCAP_SZ);
-
-	/* disable periodic rev encap */
-	mddi_writel(MDDI_CMD_PERIODIC_REV_ENCAP, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-
-	if (mddi_readl(PAD_CTL) == 0) {
-		/* If we are turning on band gap, need to wait 5us before
-		 * turning on the rest of the PAD */
-		mddi_writel(0x08000, PAD_CTL);
-		udelay(5);
-	}
-
-	/* Recommendation from PAD hw team */
-	mddi_writel(0xa850f, PAD_CTL);
-
-
-	/* Need an even number for counts */
-	mddi_writel(0x60006, DRIVER_START_CNT);
-
-	mddi_set_auto_hibernate(&mddi->client_data, 0);
-
-	mddi_writel(MDDI_CMD_DISP_IGNORE, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-
-	mddi_init_rev_encap(mddi);
-	return mddi_readl(CORE_VER) & 0xffff;
-}
-
-static void mddi_suspend(struct msm_mddi_client_data *cdata)
-{
-	struct mddi_info *mddi = container_of(cdata, struct mddi_info,
-					      client_data);
-	/* turn off the client */
-	if (mddi->power_client)
-		mddi->power_client(&mddi->client_data, 0);
-	/* turn off the link */
-	mddi_writel(MDDI_CMD_RESET, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-	/* turn off the clock */
-	clk_disable(mddi->clk);
-}
-
-static void mddi_resume(struct msm_mddi_client_data *cdata)
-{
-	struct mddi_info *mddi = container_of(cdata, struct mddi_info,
-					      client_data);
-	mddi_set_auto_hibernate(&mddi->client_data, 0);
-	/* turn on the client */
-	if (mddi->power_client)
-		mddi->power_client(&mddi->client_data, 1);
-	/* turn on the clock */
-	clk_enable(mddi->clk);
-	/* set up the local registers */
-	mddi->rev_data_curr = 0;
-	mddi_init_registers(mddi);
-	mddi_writel(mddi->int_enable, INTEN);
-	mddi_writel(MDDI_CMD_LINK_ACTIVE, CMD);
-	mddi_writel(MDDI_CMD_SEND_RTD, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-	mddi_set_auto_hibernate(&mddi->client_data, 1);
-}
-
-static int mddi_get_client_caps(struct mddi_info *mddi)
-{
-	int i, j;
-
-	/* clear any stale interrupts */
-	mddi_writel(0xffffffff, INT);
-
-	mddi->int_enable = MDDI_INT_LINK_ACTIVE |
-			   MDDI_INT_IN_HIBERNATION |
-			   MDDI_INT_PRI_LINK_LIST_DONE |
-			   MDDI_INT_REV_DATA_AVAIL |
-			   MDDI_INT_REV_OVERFLOW |
-			   MDDI_INT_REV_OVERWRITE |
-			   MDDI_INT_RTD_FAILURE;
-	mddi_writel(mddi->int_enable, INTEN);
-
-	mddi_writel(MDDI_CMD_LINK_ACTIVE, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-
-	for (j = 0; j < 3; j++) {
-		/* the toshiba vga panel does not respond to get
-		 * caps unless you SEND_RTD, but the first SEND_RTD
-		 * will fail...
-		 */
-		for (i = 0; i < 4; i++) {
-			uint32_t stat;
-
-			mddi_writel(MDDI_CMD_SEND_RTD, CMD);
-			mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-			stat = mddi_readl(STAT);
-			printk(KERN_INFO "mddi cmd send rtd: int %x, stat %x, "
-					"rtd val %x\n", mddi_readl(INT), stat,
-					mddi_readl(RTD_VAL));
-			if ((stat & MDDI_STAT_RTD_MEAS_FAIL) == 0)
-				break;
-			msleep(1);
-		}
-
-		mddi_writel(CMD_GET_CLIENT_CAP, CMD);
-		mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-		wait_event_timeout(mddi->int_wait, mddi->flags & FLAG_HAVE_CAPS,
-				   HZ / 100);
-
-		if (mddi->flags & FLAG_HAVE_CAPS)
-			break;
-		printk(KERN_INFO "mddi_init, timeout waiting for caps\n");
-	}
-	return mddi->flags & FLAG_HAVE_CAPS;
-}
-
-/* link must be active when this is called */
-int mddi_check_status(struct mddi_info *mddi)
-{
-	int ret = -1, retry = 3;
-	mutex_lock(&mddi->reg_read_lock);
-	mddi_writel(MDDI_CMD_PERIODIC_REV_ENCAP | 1, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-
-	do {
-		mddi->flags &= ~FLAG_HAVE_STATUS;
-		mddi_writel(CMD_GET_CLIENT_STATUS, CMD);
-		mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-		wait_event_timeout(mddi->int_wait,
-				   mddi->flags & FLAG_HAVE_STATUS,
-				   HZ / 100);
-
-		if (mddi->flags & FLAG_HAVE_STATUS) {
-			if (mddi->status.crc_error_count)
-				printk(KERN_INFO "mddi status: crc_error "
-					"count: %d\n",
-					mddi->status.crc_error_count);
-			else
-				ret = 0;
-			break;
-		} else
-			printk(KERN_INFO "mddi status: failed to get client "
-				"status\n");
-		mddi_writel(MDDI_CMD_SEND_RTD, CMD);
-		mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-	} while (--retry);
-
-	mddi_writel(MDDI_CMD_PERIODIC_REV_ENCAP | 0, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-	mutex_unlock(&mddi->reg_read_lock);
-	return ret;
-}
-
-
-void mddi_remote_write(struct msm_mddi_client_data *cdata, uint32_t val,
-		       uint32_t reg)
-{
-	struct mddi_info *mddi = container_of(cdata, struct mddi_info,
-					      client_data);
-	struct mddi_llentry *ll;
-	struct mddi_register_access *ra;
-
-	mutex_lock(&mddi->reg_write_lock);
-
-	ll = mddi->reg_write_data;
-
-	ra = &(ll->u.r);
-	ra->length = 14 + 4;
-	ra->type = TYPE_REGISTER_ACCESS;
-	ra->client_id = 0;
-	ra->read_write_info = MDDI_WRITE | 1;
-	ra->crc16 = 0;
-
-	ra->register_address = reg;
-	ra->register_data_list = val;
-
-	ll->flags = 1;
-	ll->header_count = 14;
-	ll->data_count = 4;
-	ll->data = mddi->reg_write_addr + offsetof(struct mddi_llentry,
-						   u.r.register_data_list);
-	ll->next = 0;
-	ll->reserved = 0;
-
-	mddi_writel(mddi->reg_write_addr, PRI_PTR);
-
-	mddi_wait_interrupt(mddi, MDDI_INT_PRI_LINK_LIST_DONE);
-	mutex_unlock(&mddi->reg_write_lock);
-}
-
-uint32_t mddi_remote_read(struct msm_mddi_client_data *cdata, uint32_t reg)
-{
-	struct mddi_info *mddi = container_of(cdata, struct mddi_info,
-					      client_data);
-	struct mddi_llentry *ll;
-	struct mddi_register_access *ra;
-	struct reg_read_info ri;
-	unsigned s;
-	int retry_count = 2;
-	unsigned long irq_flags;
-
-	mutex_lock(&mddi->reg_read_lock);
-
-	ll = mddi->reg_read_data;
-
-	ra = &(ll->u.r);
-	ra->length = 14;
-	ra->type = TYPE_REGISTER_ACCESS;
-	ra->client_id = 0;
-	ra->read_write_info = MDDI_READ | 1;
-	ra->crc16 = 0;
-
-	ra->register_address = reg;
-
-	ll->flags = 0x11;
-	ll->header_count = 14;
-	ll->data_count = 0;
-	ll->data = 0;
-	ll->next = 0;
-	ll->reserved = 0;
-
-	s = mddi_readl(STAT);
-
-	ri.reg = reg;
-	ri.status = -1;
-
-	do {
-		init_completion(&ri.done);
-		mddi->reg_read = &ri;
-		mddi_writel(mddi->reg_read_addr, PRI_PTR);
-
-		mddi_wait_interrupt(mddi, MDDI_INT_PRI_LINK_LIST_DONE);
-
-		/* Enable Periodic Reverse Encapsulation. */
-		mddi_writel(MDDI_CMD_PERIODIC_REV_ENCAP | 1, CMD);
-		mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-		if (wait_for_completion_timeout(&ri.done, HZ/10) == 0 &&
-		    !ri.done.done) {
-			printk(KERN_INFO "mddi_remote_read(%x) timeout "
-					 "(%d %d %d)\n",
-			       reg, ri.status, ri.result, ri.done.done);
-			spin_lock_irqsave(&mddi->int_lock, irq_flags);
-			mddi->reg_read = NULL;
-			spin_unlock_irqrestore(&mddi->int_lock, irq_flags);
-			ri.status = -1;
-			ri.result = -1;
-		}
-		if (ri.status == 0)
-			break;
-
-		mddi_writel(MDDI_CMD_SEND_RTD, CMD);
-		mddi_writel(MDDI_CMD_LINK_ACTIVE, CMD);
-		mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-		printk(KERN_INFO "mddi_remote_read: failed, sent "
-		       "MDDI_CMD_SEND_RTD: int %x, stat %x, rtd val %x "
-		       "curr_rev_ptr %x\n", mddi_readl(INT), mddi_readl(STAT),
-		       mddi_readl(RTD_VAL), mddi_readl(CURR_REV_PTR));
-	} while (retry_count-- > 0);
-	/* Disable Periodic Reverse Encapsulation. */
-	mddi_writel(MDDI_CMD_PERIODIC_REV_ENCAP | 0, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-	mddi->reg_read = NULL;
-	mutex_unlock(&mddi->reg_read_lock);
-	return ri.result;
-}
-
-static struct mddi_info mddi_info[2];
-
-static int mddi_clk_setup(struct platform_device *pdev, struct mddi_info *mddi,
-			  unsigned long clk_rate)
-{
-	int ret;
-
-	/* set up the clocks */
-	mddi->clk = clk_get(&pdev->dev, "mddi_clk");
-	if (IS_ERR(mddi->clk)) {
-		printk(KERN_INFO "mddi: failed to get clock\n");
-		return PTR_ERR(mddi->clk);
-	}
-	ret =  clk_enable(mddi->clk);
-	if (ret)
-		goto fail;
-	ret = clk_set_rate(mddi->clk, clk_rate);
-	if (ret)
-		goto fail;
-	return 0;
-
-fail:
-	clk_put(mddi->clk);
-	return ret;
-}
-
-static int __init mddi_rev_data_setup(struct mddi_info *mddi)
-{
-	void *dma;
-	dma_addr_t dma_addr;
-
-	/* set up dma buffer */
-	dma = dma_alloc_coherent(NULL, 0x1000, &dma_addr, GFP_KERNEL);
-	if (dma == 0)
-		return -ENOMEM;
-	mddi->rev_data = dma;
-	mddi->rev_data_curr = 0;
-	mddi->rev_addr = dma_addr;
-	mddi->reg_write_data = dma + MDDI_REV_BUFFER_SIZE;
-	mddi->reg_write_addr = dma_addr + MDDI_REV_BUFFER_SIZE;
-	mddi->reg_read_data = mddi->reg_write_data + 1;
-	mddi->reg_read_addr = mddi->reg_write_addr +
-			      sizeof(*mddi->reg_write_data);
-	return 0;
-}
-
-static int mddi_probe(struct platform_device *pdev)
-{
-	struct msm_mddi_platform_data *pdata = pdev->dev.platform_data;
-	struct mddi_info *mddi = &mddi_info[pdev->id];
-	struct resource *resource;
-	int ret, i;
-
-	resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!resource) {
-		printk(KERN_ERR "mddi: no associated mem resource!\n");
-		return -ENOMEM;
-	}
-	mddi->base = ioremap(resource->start, resource_size(resource));
-	if (!mddi->base) {
-		printk(KERN_ERR "mddi: failed to remap base!\n");
-		ret = -EINVAL;
-		goto error_ioremap;
-	}
-	resource = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!resource) {
-		printk(KERN_ERR "mddi: no associated irq resource!\n");
-		ret = -EINVAL;
-		goto error_get_irq_resource;
-	}
-	mddi->irq = resource->start;
-	printk(KERN_INFO "mddi: init() base=0x%p irq=%d\n", mddi->base,
-	       mddi->irq);
-	mddi->power_client = pdata->power_client;
-
-	mutex_init(&mddi->reg_write_lock);
-	mutex_init(&mddi->reg_read_lock);
-	spin_lock_init(&mddi->int_lock);
-	init_waitqueue_head(&mddi->int_wait);
-
-	ret = mddi_clk_setup(pdev, mddi, pdata->clk_rate);
-	if (ret) {
-		printk(KERN_ERR "mddi: failed to setup clock!\n");
-		goto error_clk_setup;
-	}
-
-	ret = mddi_rev_data_setup(mddi);
-	if (ret) {
-		printk(KERN_ERR "mddi: failed to setup rev data!\n");
-		goto error_rev_data;
-	}
-
-	mddi->int_enable = 0;
-	mddi_writel(mddi->int_enable, INTEN);
-	ret = request_irq(mddi->irq, mddi_isr, 0, "mddi",
-			  &mddi->client_data);
-	if (ret) {
-		printk(KERN_ERR "mddi: failed to request enable irq!\n");
-		goto error_request_irq;
-	}
-
-	/* turn on the mddi client bridge chip */
-	if (mddi->power_client)
-		mddi->power_client(&mddi->client_data, 1);
-
-	/* initialize the mddi registers */
-	mddi_set_auto_hibernate(&mddi->client_data, 0);
-	mddi_writel(MDDI_CMD_RESET, CMD);
-	mddi_wait_interrupt(mddi, MDDI_INT_NO_CMD_PKTS_PEND);
-	mddi->version = mddi_init_registers(mddi);
-	if (mddi->version < 0x20) {
-		printk(KERN_ERR "mddi: unsupported version 0x%x\n",
-		       mddi->version);
-		ret = -ENODEV;
-		goto error_mddi_version;
-	}
-
-	/* read the capabilities off the client */
-	if (!mddi_get_client_caps(mddi)) {
-		printk(KERN_INFO "mddi: no client found\n");
-		/* power down the panel */
-		mddi_writel(MDDI_CMD_POWERDOWN, CMD);
-		printk(KERN_INFO "mddi powerdown: stat %x\n", mddi_readl(STAT));
-		msleep(100);
-		printk(KERN_INFO "mddi powerdown: stat %x\n", mddi_readl(STAT));
-		return 0;
-	}
-	mddi_set_auto_hibernate(&mddi->client_data, 1);
-
-	if (mddi->caps.Mfr_Name == 0 && mddi->caps.Product_Code == 0)
-		pdata->fixup(&mddi->caps.Mfr_Name, &mddi->caps.Product_Code);
-
-	mddi->client_pdev.id = 0;
-	for (i = 0; i < pdata->num_clients; i++) {
-		if (pdata->client_platform_data[i].product_id ==
-		    (mddi->caps.Mfr_Name << 16 | mddi->caps.Product_Code)) {
-			mddi->client_data.private_client_data =
-				pdata->client_platform_data[i].client_data;
-			mddi->client_pdev.name =
-				pdata->client_platform_data[i].name;
-			mddi->client_pdev.id =
-				pdata->client_platform_data[i].id;
-			/* XXX: possibly set clock */
-			break;
-		}
-	}
-
-	if (i >= pdata->num_clients)
-		mddi->client_pdev.name = "mddi_c_dummy";
-	printk(KERN_INFO "mddi: registering panel %s\n",
-		mddi->client_pdev.name);
-
-	mddi->client_data.suspend = mddi_suspend;
-	mddi->client_data.resume = mddi_resume;
-	mddi->client_data.activate_link = mddi_activate_link;
-	mddi->client_data.remote_write = mddi_remote_write;
-	mddi->client_data.remote_read = mddi_remote_read;
-	mddi->client_data.auto_hibernate = mddi_set_auto_hibernate;
-	mddi->client_data.fb_resource = pdata->fb_resource;
-	if (pdev->id == 0)
-		mddi->client_data.interface_type = MSM_MDDI_PMDH_INTERFACE;
-	else if (pdev->id == 1)
-		mddi->client_data.interface_type = MSM_MDDI_EMDH_INTERFACE;
-	else {
-		printk(KERN_ERR "mddi: can not determine interface %d!\n",
-		       pdev->id);
-		ret = -EINVAL;
-		goto error_mddi_interface;
-	}
-
-	mddi->client_pdev.dev.platform_data = &mddi->client_data;
-	printk(KERN_INFO "mddi: publish: %s\n", mddi->client_name);
-	platform_device_register(&mddi->client_pdev);
-	return 0;
-
-error_mddi_interface:
-error_mddi_version:
-	free_irq(mddi->irq, 0);
-error_request_irq:
-	dma_free_coherent(NULL, 0x1000, mddi->rev_data, mddi->rev_addr);
-error_rev_data:
-error_clk_setup:
-error_get_irq_resource:
-	iounmap(mddi->base);
-error_ioremap:
-
-	printk(KERN_INFO "mddi: mddi_init() failed (%d)\n", ret);
-	return ret;
-}
-
-
-static struct platform_driver mddi_driver = {
-	.probe = mddi_probe,
-	.driver = { .name = "msm_mddi" },
-};
-
-static int __init _mddi_init(void)
-{
-	return platform_driver_register(&mddi_driver);
-}
-
-module_init(_mddi_init);
diff --git a/drivers/video/fbdev/msm/mddi_client_dummy.c b/drivers/video/fbdev/msm/mddi_client_dummy.c
deleted file mode 100644
index cdb8f69a5d88..000000000000
--- a/drivers/video/fbdev/msm/mddi_client_dummy.c
+++ /dev/null
@@ -1,85 +0,0 @@
-/* drivers/video/msm_fb/mddi_client_dummy.c
- *
- * Support for "dummy" mddi client devices which require no
- * special initialization code.
- *
- * Copyright (C) 2007 Google Incorporated
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/device.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/platform_device.h>
-
-#include <linux/platform_data/video-msm_fb.h>
-
-struct panel_info {
-	struct platform_device pdev;
-	struct msm_panel_data panel_data;
-};
-
-static int mddi_dummy_suspend(struct msm_panel_data *panel_data)
-{
-	return 0;
-}
-
-static int mddi_dummy_resume(struct msm_panel_data *panel_data)
-{
-	return 0;
-}
-
-static int mddi_dummy_blank(struct msm_panel_data *panel_data)
-{
-	return 0;
-}
-
-static int mddi_dummy_unblank(struct msm_panel_data *panel_data)
-{
-	return 0;
-}
-
-static int mddi_dummy_probe(struct platform_device *pdev)
-{
-	struct msm_mddi_client_data *client_data = pdev->dev.platform_data;
-	struct panel_info *panel =
-		devm_kzalloc(&pdev->dev, sizeof(struct panel_info), GFP_KERNEL);
-	if (!panel)
-		return -ENOMEM;
-	platform_set_drvdata(pdev, panel);
-	panel->panel_data.suspend = mddi_dummy_suspend;
-	panel->panel_data.resume = mddi_dummy_resume;
-	panel->panel_data.blank = mddi_dummy_blank;
-	panel->panel_data.unblank = mddi_dummy_unblank;
-	panel->panel_data.caps = MSMFB_CAP_PARTIAL_UPDATES;
-	panel->pdev.name = "msm_panel";
-	panel->pdev.id = pdev->id;
-	platform_device_add_resources(&panel->pdev,
-				      client_data->fb_resource, 1);
-	panel->panel_data.fb_data = client_data->private_client_data;
-	panel->pdev.dev.platform_data = &panel->panel_data;
-	return platform_device_register(&panel->pdev);
-}
-
-static struct platform_driver mddi_client_dummy = {
-	.probe = mddi_dummy_probe,
-	.driver = { .name = "mddi_c_dummy" },
-};
-
-static int __init mddi_client_dummy_init(void)
-{
-	platform_driver_register(&mddi_client_dummy);
-	return 0;
-}
-
-module_init(mddi_client_dummy_init);
-
diff --git a/drivers/video/fbdev/msm/mddi_client_nt35399.c b/drivers/video/fbdev/msm/mddi_client_nt35399.c
deleted file mode 100644
index f96df32e5509..000000000000
--- a/drivers/video/fbdev/msm/mddi_client_nt35399.c
+++ /dev/null
@@ -1,252 +0,0 @@
-/* drivers/video/msm_fb/mddi_client_nt35399.c
- *
- * Support for Novatek NT35399 MDDI client of Sapphire
- *
- * Copyright (C) 2008 HTC Incorporated
- * Author: Solomon Chiu (solomon_chiu@htc.com)
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/platform_device.h>
-#include <linux/interrupt.h>
-#include <linux/sched.h>
-#include <linux/gpio.h>
-#include <linux/slab.h>
-#include <linux/platform_data/video-msm_fb.h>
-
-static DECLARE_WAIT_QUEUE_HEAD(nt35399_vsync_wait);
-
-struct panel_info {
-	struct msm_mddi_client_data *client_data;
-	struct platform_device pdev;
-	struct msm_panel_data panel_data;
-	struct msmfb_callback *fb_callback;
-	struct work_struct panel_work;
-	struct workqueue_struct *fb_wq;
-	int nt35399_got_int;
-};
-
-static void
-nt35399_request_vsync(struct msm_panel_data *panel_data,
-		      struct msmfb_callback *callback)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	panel->fb_callback = callback;
-	if (panel->nt35399_got_int) {
-		panel->nt35399_got_int = 0;
-		client_data->activate_link(client_data); /* clears interrupt */
-	}
-}
-
-static void nt35399_wait_vsync(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	if (panel->nt35399_got_int) {
-		panel->nt35399_got_int = 0;
-		client_data->activate_link(client_data); /* clears interrupt */
-	}
-
-	if (wait_event_timeout(nt35399_vsync_wait, panel->nt35399_got_int,
-				HZ/2) == 0)
-		printk(KERN_ERR "timeout waiting for VSYNC\n");
-
-	panel->nt35399_got_int = 0;
-	/* interrupt clears when screen dma starts */
-}
-
-static int nt35399_suspend(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-	int ret;
-
-	ret = bridge_data->uninit(bridge_data, client_data);
-	if (ret) {
-		printk(KERN_INFO "mddi nt35399 client: non zero return from "
-			"uninit\n");
-		return ret;
-	}
-	client_data->suspend(client_data);
-	return 0;
-}
-
-static int nt35399_resume(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-	int ret;
-
-	client_data->resume(client_data);
-	ret = bridge_data->init(bridge_data, client_data);
-	if (ret)
-		return ret;
-	return 0;
-}
-
-static int nt35399_blank(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-
-	return bridge_data->blank(bridge_data, client_data);
-}
-
-static int nt35399_unblank(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-
-	return bridge_data->unblank(bridge_data, client_data);
-}
-
-irqreturn_t nt35399_vsync_interrupt(int irq, void *data)
-{
-	struct panel_info *panel = data;
-
-	panel->nt35399_got_int = 1;
-
-	if (panel->fb_callback) {
-		panel->fb_callback->func(panel->fb_callback);
-		panel->fb_callback = NULL;
-	}
-
-	wake_up(&nt35399_vsync_wait);
-
-	return IRQ_HANDLED;
-}
-
-static int setup_vsync(struct panel_info *panel, int init)
-{
-	int ret;
-	int gpio = 97;
-	unsigned int irq;
-
-	if (!init) {
-		ret = 0;
-		goto uninit;
-	}
-	ret = gpio_request_one(gpio, GPIOF_IN, "vsync");
-	if (ret)
-		goto err_request_gpio_failed;
-
-	ret = irq = gpio_to_irq(gpio);
-	if (ret < 0)
-		goto err_get_irq_num_failed;
-
-	ret = request_irq(irq, nt35399_vsync_interrupt, IRQF_TRIGGER_RISING,
-			  "vsync", panel);
-	if (ret)
-		goto err_request_irq_failed;
-
-	printk(KERN_INFO "vsync on gpio %d now %d\n",
-	       gpio, gpio_get_value(gpio));
-	return 0;
-
-uninit:
-	free_irq(gpio_to_irq(gpio), panel->client_data);
-err_request_irq_failed:
-err_get_irq_num_failed:
-	gpio_free(gpio);
-err_request_gpio_failed:
-	return ret;
-}
-
-static int mddi_nt35399_probe(struct platform_device *pdev)
-{
-	struct msm_mddi_client_data *client_data = pdev->dev.platform_data;
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-
-	int ret;
-
-	struct panel_info *panel = devm_kzalloc(&pdev->dev,
-						sizeof(struct panel_info),
-						GFP_KERNEL);
-
-	printk(KERN_DEBUG "%s: enter.\n", __func__);
-
-	if (!panel)
-		return -ENOMEM;
-	platform_set_drvdata(pdev, panel);
-
-	ret = setup_vsync(panel, 1);
-	if (ret) {
-		dev_err(&pdev->dev, "mddi_nt35399_setup_vsync failed\n");
-		return ret;
-	}
-
-	panel->client_data = client_data;
-	panel->panel_data.suspend = nt35399_suspend;
-	panel->panel_data.resume = nt35399_resume;
-	panel->panel_data.wait_vsync = nt35399_wait_vsync;
-	panel->panel_data.request_vsync = nt35399_request_vsync;
-	panel->panel_data.blank = nt35399_blank;
-	panel->panel_data.unblank = nt35399_unblank;
-	panel->panel_data.fb_data = &bridge_data->fb_data;
-	panel->panel_data.caps = 0;
-
-	panel->pdev.name = "msm_panel";
-	panel->pdev.id = pdev->id;
-	panel->pdev.resource = client_data->fb_resource;
-	panel->pdev.num_resources = 1;
-	panel->pdev.dev.platform_data = &panel->panel_data;
-
-	if (bridge_data->init)
-		bridge_data->init(bridge_data, client_data);
-
-	platform_device_register(&panel->pdev);
-
-	return 0;
-}
-
-static int mddi_nt35399_remove(struct platform_device *pdev)
-{
-	struct panel_info *panel = platform_get_drvdata(pdev);
-
-	setup_vsync(panel, 0);
-	return 0;
-}
-
-static struct platform_driver mddi_client_0bda_8a47 = {
-	.probe = mddi_nt35399_probe,
-	.remove = mddi_nt35399_remove,
-	.driver = { .name = "mddi_c_0bda_8a47" },
-};
-
-static int __init mddi_client_nt35399_init(void)
-{
-	return platform_driver_register(&mddi_client_0bda_8a47);
-}
-
-module_init(mddi_client_nt35399_init);
-
diff --git a/drivers/video/fbdev/msm/mddi_client_toshiba.c b/drivers/video/fbdev/msm/mddi_client_toshiba.c
deleted file mode 100644
index 061d7dfebbf3..000000000000
--- a/drivers/video/fbdev/msm/mddi_client_toshiba.c
+++ /dev/null
@@ -1,280 +0,0 @@
-/* drivers/video/msm_fb/mddi_client_toshiba.c
- *
- * Support for Toshiba TC358720XBG mddi client devices which require no
- * special initialization code.
- *
- * Copyright (C) 2007 Google Incorporated
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/platform_device.h>
-#include <linux/interrupt.h>
-#include <linux/gpio.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/platform_data/video-msm_fb.h>
-
-
-#define LCD_CONTROL_BLOCK_BASE 0x110000
-#define CMN         (LCD_CONTROL_BLOCK_BASE|0x10)
-#define INTFLG      (LCD_CONTROL_BLOCK_BASE|0x18)
-#define HCYCLE      (LCD_CONTROL_BLOCK_BASE|0x34)
-#define HDE_START   (LCD_CONTROL_BLOCK_BASE|0x3C)
-#define VPOS        (LCD_CONTROL_BLOCK_BASE|0xC0)
-#define MPLFBUF     (LCD_CONTROL_BLOCK_BASE|0x20)
-#define WAKEUP      (LCD_CONTROL_BLOCK_BASE|0x54)
-#define WSYN_DLY    (LCD_CONTROL_BLOCK_BASE|0x58)
-#define REGENB      (LCD_CONTROL_BLOCK_BASE|0x5C)
-
-#define BASE5 0x150000
-#define BASE6 0x160000
-#define BASE7 0x170000
-
-#define GPIOIEV     (BASE5 + 0x10)
-#define GPIOIE      (BASE5 + 0x14)
-#define GPIORIS     (BASE5 + 0x18)
-#define GPIOMIS     (BASE5 + 0x1C)
-#define GPIOIC      (BASE5 + 0x20)
-
-#define INTMASK     (BASE6 + 0x0C)
-#define INTMASK_VWAKEOUT (1U << 0)
-#define INTMASK_VWAKEOUT_ACTIVE_LOW (1U << 8)
-#define GPIOSEL     (BASE7 + 0x00)
-#define GPIOSEL_VWAKEINT (1U << 0)
-
-static DECLARE_WAIT_QUEUE_HEAD(toshiba_vsync_wait);
-
-struct panel_info {
-	struct msm_mddi_client_data *client_data;
-	struct platform_device pdev;
-	struct msm_panel_data panel_data;
-	struct msmfb_callback *toshiba_callback;
-	int toshiba_got_int;
-};
-
-
-static void toshiba_request_vsync(struct msm_panel_data *panel_data,
-				  struct msmfb_callback *callback)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	panel->toshiba_callback = callback;
-	if (panel->toshiba_got_int) {
-		panel->toshiba_got_int = 0;
-		client_data->activate_link(client_data);
-	}
-}
-
-static void toshiba_clear_vsync(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	client_data->activate_link(client_data);
-}
-
-static void toshiba_wait_vsync(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	if (panel->toshiba_got_int) {
-		panel->toshiba_got_int = 0;
-		client_data->activate_link(client_data); /* clears interrupt */
-	}
-	if (wait_event_timeout(toshiba_vsync_wait, panel->toshiba_got_int,
-				HZ/2) == 0)
-		printk(KERN_ERR "timeout waiting for VSYNC\n");
-	panel->toshiba_got_int = 0;
-	/* interrupt clears when screen dma starts */
-}
-
-static int toshiba_suspend(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-	int ret;
-
-	ret = bridge_data->uninit(bridge_data, client_data);
-	if (ret) {
-		printk(KERN_INFO "mddi toshiba client: non zero return from "
-			"uninit\n");
-		return ret;
-	}
-	client_data->suspend(client_data);
-	return 0;
-}
-
-static int toshiba_resume(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-	int ret;
-
-	client_data->resume(client_data);
-	ret = bridge_data->init(bridge_data, client_data);
-	if (ret)
-		return ret;
-	return 0;
-}
-
-static int toshiba_blank(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-
-	return bridge_data->blank(bridge_data, client_data);
-}
-
-static int toshiba_unblank(struct msm_panel_data *panel_data)
-{
-	struct panel_info *panel = container_of(panel_data, struct panel_info,
-						panel_data);
-	struct msm_mddi_client_data *client_data = panel->client_data;
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-
-	return bridge_data->unblank(bridge_data, client_data);
-}
-
-irqreturn_t toshiba_vsync_interrupt(int irq, void *data)
-{
-	struct panel_info *panel = data;
-
-	panel->toshiba_got_int = 1;
-	if (panel->toshiba_callback) {
-		panel->toshiba_callback->func(panel->toshiba_callback);
-		panel->toshiba_callback = 0;
-	}
-	wake_up(&toshiba_vsync_wait);
-	return IRQ_HANDLED;
-}
-
-static int setup_vsync(struct panel_info *panel,
-		       int init)
-{
-	int ret;
-	int gpio = 97;
-	unsigned int irq;
-
-	if (!init) {
-		ret = 0;
-		goto uninit;
-	}
-	ret = gpio_request_one(gpio, GPIOF_IN, "vsync");
-	if (ret)
-		goto err_request_gpio_failed;
-
-	ret = irq = gpio_to_irq(gpio);
-	if (ret < 0)
-		goto err_get_irq_num_failed;
-
-	ret = request_irq(irq, toshiba_vsync_interrupt, IRQF_TRIGGER_RISING,
-			  "vsync", panel);
-	if (ret)
-		goto err_request_irq_failed;
-	printk(KERN_INFO "vsync on gpio %d now %d\n",
-	       gpio, gpio_get_value(gpio));
-	return 0;
-
-uninit:
-	free_irq(gpio_to_irq(gpio), panel);
-err_request_irq_failed:
-err_get_irq_num_failed:
-	gpio_free(gpio);
-err_request_gpio_failed:
-	return ret;
-}
-
-static int mddi_toshiba_probe(struct platform_device *pdev)
-{
-	int ret;
-	struct msm_mddi_client_data *client_data = pdev->dev.platform_data;
-	struct msm_mddi_bridge_platform_data *bridge_data =
-		client_data->private_client_data;
-	struct panel_info *panel =
-		kzalloc(sizeof(struct panel_info), GFP_KERNEL);
-	if (!panel)
-		return -ENOMEM;
-	platform_set_drvdata(pdev, panel);
-
-	/* mddi_remote_write(mddi, 0, WAKEUP); */
-	client_data->remote_write(client_data, GPIOSEL_VWAKEINT, GPIOSEL);
-	client_data->remote_write(client_data, INTMASK_VWAKEOUT, INTMASK);
-
-	ret = setup_vsync(panel, 1);
-	if (ret) {
-		dev_err(&pdev->dev, "mddi_bridge_setup_vsync failed\n");
-		return ret;
-	}
-
-	panel->client_data = client_data;
-	panel->panel_data.suspend = toshiba_suspend;
-	panel->panel_data.resume = toshiba_resume;
-	panel->panel_data.wait_vsync = toshiba_wait_vsync;
-	panel->panel_data.request_vsync = toshiba_request_vsync;
-	panel->panel_data.clear_vsync = toshiba_clear_vsync;
-	panel->panel_data.blank = toshiba_blank;
-	panel->panel_data.unblank = toshiba_unblank;
-	panel->panel_data.fb_data =  &bridge_data->fb_data;
-	panel->panel_data.caps = MSMFB_CAP_PARTIAL_UPDATES;
-
-	panel->pdev.name = "msm_panel";
-	panel->pdev.id = pdev->id;
-	panel->pdev.resource = client_data->fb_resource;
-	panel->pdev.num_resources = 1;
-	panel->pdev.dev.platform_data = &panel->panel_data;
-	bridge_data->init(bridge_data, client_data);
-	platform_device_register(&panel->pdev);
-
-	return 0;
-}
-
-static int mddi_toshiba_remove(struct platform_device *pdev)
-{
-	struct panel_info *panel = platform_get_drvdata(pdev);
-
-	setup_vsync(panel, 0);
-	kfree(panel);
-	return 0;
-}
-
-static struct platform_driver mddi_client_d263_0000 = {
-	.probe = mddi_toshiba_probe,
-	.remove = mddi_toshiba_remove,
-	.driver = { .name = "mddi_c_d263_0000" },
-};
-
-static int __init mddi_client_toshiba_init(void)
-{
-	platform_driver_register(&mddi_client_d263_0000);
-	return 0;
-}
-
-module_init(mddi_client_toshiba_init);
-
diff --git a/drivers/video/fbdev/msm/mddi_hw.h b/drivers/video/fbdev/msm/mddi_hw.h
deleted file mode 100644
index 45cc01fc1e7f..000000000000
--- a/drivers/video/fbdev/msm/mddi_hw.h
+++ /dev/null
@@ -1,305 +0,0 @@
-/* drivers/video/msm_fb/mddi_hw.h
- *
- * MSM MDDI Hardware Registers and Structures
- *
- * Copyright (C) 2007 QUALCOMM Incorporated
- * Copyright (C) 2007 Google Incorporated
- *
- * 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 _MDDI_HW_H_
-#define _MDDI_HW_H_
-
-#include <linux/types.h>
-
-#define MDDI_CMD                0x0000
-#define MDDI_VERSION            0x0004
-#define MDDI_PRI_PTR            0x0008
-#define MDDI_SEC_PTR            0x000c
-#define MDDI_BPS                0x0010
-#define MDDI_SPM                0x0014
-#define MDDI_INT                0x0018
-#define MDDI_INTEN              0x001c
-#define MDDI_REV_PTR            0x0020
-#define MDDI_REV_SIZE           0x0024
-#define MDDI_STAT               0x0028
-#define MDDI_REV_RATE_DIV       0x002c
-#define MDDI_REV_CRC_ERR        0x0030
-#define MDDI_TA1_LEN            0x0034
-#define MDDI_TA2_LEN            0x0038
-#define MDDI_TEST_BUS           0x003c
-#define MDDI_TEST               0x0040
-#define MDDI_REV_PKT_CNT        0x0044
-#define MDDI_DRIVE_HI           0x0048
-#define MDDI_DRIVE_LO           0x004c
-#define MDDI_DISP_WAKE          0x0050
-#define MDDI_REV_ENCAP_SZ       0x0054
-#define MDDI_RTD_VAL            0x0058
-#define MDDI_PAD_CTL            0x0068
-#define MDDI_DRIVER_START_CNT   0x006c
-#define MDDI_NEXT_PRI_PTR       0x0070
-#define MDDI_NEXT_SEC_PTR       0x0074
-#define MDDI_MISR_CTL           0x0078
-#define MDDI_MISR_DATA          0x007c
-#define MDDI_SF_CNT             0x0080
-#define MDDI_MF_CNT             0x0084
-#define MDDI_CURR_REV_PTR       0x0088
-#define MDDI_CORE_VER           0x008c
-
-#define MDDI_INT_PRI_PTR_READ       0x0001
-#define MDDI_INT_SEC_PTR_READ       0x0002
-#define MDDI_INT_REV_DATA_AVAIL     0x0004
-#define MDDI_INT_DISP_REQ           0x0008
-#define MDDI_INT_PRI_UNDERFLOW      0x0010
-#define MDDI_INT_SEC_UNDERFLOW      0x0020
-#define MDDI_INT_REV_OVERFLOW       0x0040
-#define MDDI_INT_CRC_ERROR          0x0080
-#define MDDI_INT_MDDI_IN            0x0100
-#define MDDI_INT_PRI_OVERWRITE      0x0200
-#define MDDI_INT_SEC_OVERWRITE      0x0400
-#define MDDI_INT_REV_OVERWRITE      0x0800
-#define MDDI_INT_DMA_FAILURE        0x1000
-#define MDDI_INT_LINK_ACTIVE        0x2000
-#define MDDI_INT_IN_HIBERNATION     0x4000
-#define MDDI_INT_PRI_LINK_LIST_DONE 0x8000
-#define MDDI_INT_SEC_LINK_LIST_DONE 0x10000
-#define MDDI_INT_NO_CMD_PKTS_PEND   0x20000
-#define MDDI_INT_RTD_FAILURE        0x40000
-#define MDDI_INT_REV_PKT_RECEIVED   0x80000
-#define MDDI_INT_REV_PKTS_AVAIL     0x100000
-
-#define MDDI_INT_NEED_CLEAR ( \
-	MDDI_INT_REV_DATA_AVAIL | \
-	MDDI_INT_PRI_UNDERFLOW | \
-	MDDI_INT_SEC_UNDERFLOW | \
-	MDDI_INT_REV_OVERFLOW | \
-	MDDI_INT_CRC_ERROR | \
-	MDDI_INT_REV_PKT_RECEIVED)
-
-
-#define MDDI_STAT_LINK_ACTIVE        0x0001
-#define MDDI_STAT_NEW_REV_PTR        0x0002
-#define MDDI_STAT_NEW_PRI_PTR        0x0004
-#define MDDI_STAT_NEW_SEC_PTR        0x0008
-#define MDDI_STAT_IN_HIBERNATION     0x0010
-#define MDDI_STAT_PRI_LINK_LIST_DONE 0x0020
-#define MDDI_STAT_SEC_LINK_LIST_DONE 0x0040
-#define MDDI_STAT_PENDING_TIMING_PKT 0x0080
-#define MDDI_STAT_PENDING_REV_ENCAP  0x0100
-#define MDDI_STAT_PENDING_POWERDOWN  0x0200
-#define MDDI_STAT_RTD_MEAS_FAIL      0x0800
-#define MDDI_STAT_CLIENT_WAKEUP_REQ  0x1000
-
-
-#define MDDI_CMD_POWERDOWN           0x0100
-#define MDDI_CMD_POWERUP             0x0200
-#define MDDI_CMD_HIBERNATE           0x0300
-#define MDDI_CMD_RESET               0x0400
-#define MDDI_CMD_DISP_IGNORE         0x0501
-#define MDDI_CMD_DISP_LISTEN         0x0500
-#define MDDI_CMD_SEND_REV_ENCAP      0x0600
-#define MDDI_CMD_GET_CLIENT_CAP      0x0601
-#define MDDI_CMD_GET_CLIENT_STATUS   0x0602
-#define MDDI_CMD_SEND_RTD            0x0700
-#define MDDI_CMD_LINK_ACTIVE         0x0900
-#define MDDI_CMD_PERIODIC_REV_ENCAP  0x0A00
-#define MDDI_CMD_FORCE_NEW_REV_PTR   0x0C00
-
-
-
-#define MDDI_VIDEO_REV_PKT_SIZE              0x40
-#define MDDI_CLIENT_CAPABILITY_REV_PKT_SIZE  0x60
-#define MDDI_MAX_REV_PKT_SIZE                0x60
-
-/* #define MDDI_REV_BUFFER_SIZE 128 */
-#define MDDI_REV_BUFFER_SIZE (MDDI_MAX_REV_PKT_SIZE * 4)
-
-/* MDP sends 256 pixel packets, so lower value hibernates more without
- * significantly increasing latency of waiting for next subframe */
-#define MDDI_HOST_BYTES_PER_SUBFRAME  0x3C00
-#define MDDI_HOST_TA2_LEN       0x000c
-#define MDDI_HOST_REV_RATE_DIV  0x0002
-
-
-struct __attribute__((packed)) mddi_rev_packet {
-	uint16_t length;
-	uint16_t type;
-	uint16_t client_id;
-};
-
-struct __attribute__((packed)) mddi_client_status {
-	uint16_t length;
-	uint16_t type;
-	uint16_t client_id;
-	uint16_t reverse_link_request;  /* bytes needed in rev encap message */
-	uint8_t  crc_error_count;
-	uint8_t  capability_change;
-	uint16_t graphics_busy_flags;
-	uint16_t crc16;
-};
-
-struct __attribute__((packed)) mddi_client_caps {
-	uint16_t length; /* length, exclusive of this field */
-	uint16_t type; /* 66 */
-	uint16_t client_id;
-
-	uint16_t Protocol_Version;
-	uint16_t Minimum_Protocol_Version;
-	uint16_t Data_Rate_Capability;
-	uint8_t  Interface_Type_Capability;
-	uint8_t  Number_of_Alt_Displays;
-	uint16_t PostCal_Data_Rate;
-	uint16_t Bitmap_Width;
-	uint16_t Bitmap_Height;
-	uint16_t Display_Window_Width;
-	uint16_t Display_Window_Height;
-	uint32_t Color_Map_Size;
-	uint16_t Color_Map_RGB_Width;
-	uint16_t RGB_Capability;
-	uint8_t  Monochrome_Capability;
-	uint8_t  Reserved_1;
-	uint16_t Y_Cb_Cr_Capability;
-	uint16_t Bayer_Capability;
-	uint16_t Alpha_Cursor_Image_Planes;
-	uint32_t Client_Feature_Capability_Indicators;
-	uint8_t  Maximum_Video_Frame_Rate_Capability;
-	uint8_t  Minimum_Video_Frame_Rate_Capability;
-	uint16_t Minimum_Sub_frame_Rate;
-	uint16_t Audio_Buffer_Depth;
-	uint16_t Audio_Channel_Capability;
-	uint16_t Audio_Sample_Rate_Capability;
-	uint8_t  Audio_Sample_Resolution;
-	uint8_t  Mic_Audio_Sample_Resolution;
-	uint16_t Mic_Sample_Rate_Capability;
-	uint8_t  Keyboard_Data_Format;
-	uint8_t  pointing_device_data_format;
-	uint16_t content_protection_type;
-	uint16_t Mfr_Name;
-	uint16_t Product_Code;
-	uint16_t Reserved_3;
-	uint32_t Serial_Number;
-	uint8_t  Week_of_Manufacture;
-	uint8_t  Year_of_Manufacture;
-
-	uint16_t crc16;
-} mddi_client_capability_type;
-
-
-struct __attribute__((packed)) mddi_video_stream {
-	uint16_t length;
-	uint16_t type; /* 16 */
-	uint16_t client_id; /* 0 */
-
-	uint16_t video_data_format_descriptor;
-/* format of each pixel in the Pixel Data in the present stream in the
- * present packet.
- * If bits [15:13] = 000 monochrome
- * If bits [15:13] = 001 color pixels (palette).
- * If bits [15:13] = 010 color pixels in raw RGB
- * If bits [15:13] = 011 data in 4:2:2 Y Cb Cr format
- * If bits [15:13] = 100 Bayer pixels
- */
-
-	uint16_t pixel_data_attributes;
-/* interpreted as follows:
- * Bits [1:0] = 11  pixel data is displayed to both eyes
- * Bits [1:0] = 10  pixel data is routed to the left eye only.
- * Bits [1:0] = 01  pixel data is routed to the right eye only.
- * Bits [1:0] = 00  pixel data is routed to the alternate display.
- * Bit 2 is 0  Pixel Data is in the standard progressive format.
- * Bit 2 is 1  Pixel Data is in interlace format.
- * Bit 3 is 0  Pixel Data is in the standard progressive format.
- * Bit 3 is 1  Pixel Data is in alternate pixel format.
- * Bit 4 is 0  Pixel Data is to or from the display frame buffer.
- * Bit 4 is 1  Pixel Data is to or from the camera.
- * Bit 5 is 0  pixel data contains the next consecutive row of pixels.
- * Bit 5 is 1  X Left Edge, Y Top Edge, X Right Edge, Y Bottom Edge,
- *             X Start, and Y Start parameters are not defined and
- *             shall be ignored by the client.
- * Bits [7:6] = 01  Pixel data is written to the offline image buffer.
- * Bits [7:6] = 00  Pixel data is written to the buffer to refresh display.
- * Bits [7:6] = 11  Pixel data is written to all image buffers.
- * Bits [7:6] = 10  Invalid. Reserved for future use.
- * Bits 8 through 11 alternate display number.
- * Bits 12 through 14 are reserved for future use and shall be set to zero.
- * Bit 15 is 1 the row of pixels is the last row of pixels in a frame.
- */
-
-	uint16_t x_left_edge;
-	uint16_t y_top_edge;
-	/* X,Y coordinate of the top left edge of the screen window */
-
-	uint16_t x_right_edge;
-	uint16_t y_bottom_edge;
-	/* X,Y coordinate of the bottom right edge of the window being
-	 * updated. */
-
-	uint16_t x_start;
-	uint16_t y_start;
-	/* (X Start, Y Start) is the first pixel in the Pixel Data field
-	 * below. */
-
-	uint16_t pixel_count;
-	/* number of pixels in the Pixel Data field below. */
-
-	uint16_t parameter_CRC;
-	/* 16-bit CRC of all bytes from the Packet Length to the Pixel Count. */
-
-	uint16_t reserved;
-	/* 16-bit variable to make structure align on 4 byte boundary */
-};
-
-#define TYPE_VIDEO_STREAM      16
-#define TYPE_CLIENT_CAPS       66
-#define TYPE_REGISTER_ACCESS   146
-#define TYPE_CLIENT_STATUS     70
-
-struct __attribute__((packed)) mddi_register_access {
-	uint16_t length;
-	uint16_t type; /* 146 */
-	uint16_t client_id;
-
-	uint16_t read_write_info;
-	/* Bits 13:0  a 14-bit unsigned integer that specifies the number of
-	 *            32-bit Register Data List items to be transferred in the
-	 *            Register Data List field.
-	 * Bits[15:14] = 00  Write to register(s);
-	 * Bits[15:14] = 10  Read from register(s);
-	 * Bits[15:14] = 11  Response to a Read.
-	 * Bits[15:14] = 01  this value is reserved for future use. */
-#define MDDI_WRITE     (0 << 14)
-#define MDDI_READ      (2 << 14)
-#define MDDI_READ_RESP (3 << 14)
-
-	uint32_t register_address;
-	/* the register address that is to be written to or read from. */
-
-	uint16_t crc16;
-
-	uint32_t register_data_list;
-	/* list of 4-byte register data values for/from client registers */
-};
-
-struct __attribute__((packed)) mddi_llentry {
-	uint16_t flags;
-	uint16_t header_count;
-	uint16_t data_count;
-	dma_addr_t data; /* 32 bit */
-	struct mddi_llentry *next;
-	uint16_t reserved;
-	union {
-		struct mddi_video_stream v;
-		struct mddi_register_access r;
-		uint32_t _[12];
-	} u;
-};
-
-#endif
diff --git a/drivers/video/fbdev/msm/mdp.c b/drivers/video/fbdev/msm/mdp.c
deleted file mode 100644
index 113c7876c855..000000000000
--- a/drivers/video/fbdev/msm/mdp.c
+++ /dev/null
@@ -1,520 +0,0 @@
-/* drivers/video/msm_fb/mdp.c
- *
- * MSM MDP Interface (used by framebuffer core)
- *
- * Copyright (C) 2007 QUALCOMM Incorporated
- * Copyright (C) 2007 Google Incorporated
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/kernel.h>
-#include <linux/fb.h>
-#include <linux/msm_mdp.h>
-#include <linux/interrupt.h>
-#include <linux/wait.h>
-#include <linux/clk.h>
-#include <linux/file.h>
-#include <linux/major.h>
-#include <linux/slab.h>
-
-#include <linux/platform_data/video-msm_fb.h>
-#include <linux/platform_device.h>
-#include <linux/export.h>
-
-#include "mdp_hw.h"
-
-struct class *mdp_class;
-
-#define MDP_CMD_DEBUG_ACCESS_BASE (0x10000)
-
-static uint16_t mdp_default_ccs[] = {
-	0x254, 0x000, 0x331, 0x254, 0xF38, 0xE61, 0x254, 0x409, 0x000,
-	0x010, 0x080, 0x080
-};
-
-static DECLARE_WAIT_QUEUE_HEAD(mdp_dma2_waitqueue);
-static DECLARE_WAIT_QUEUE_HEAD(mdp_ppp_waitqueue);
-static struct msmfb_callback *dma_callback;
-static struct clk *clk;
-static unsigned int mdp_irq_mask;
-static DEFINE_SPINLOCK(mdp_lock);
-DEFINE_MUTEX(mdp_mutex);
-
-static int enable_mdp_irq(struct mdp_info *mdp, uint32_t mask)
-{
-	unsigned long irq_flags;
-	int ret = 0;
-
-	BUG_ON(!mask);
-
-	spin_lock_irqsave(&mdp_lock, irq_flags);
-	/* if the mask bits are already set return an error, this interrupt
-	 * is already enabled */
-	if (mdp_irq_mask & mask) {
-		printk(KERN_ERR "mdp irq already on already on %x %x\n",
-		       mdp_irq_mask, mask);
-		ret = -1;
-	}
-	/* if the mdp irq is not already enabled enable it */
-	if (!mdp_irq_mask) {
-		if (clk)
-			clk_enable(clk);
-		enable_irq(mdp->irq);
-	}
-
-	/* update the irq mask to reflect the fact that the interrupt is
-	 * enabled */
-	mdp_irq_mask |= mask;
-	spin_unlock_irqrestore(&mdp_lock, irq_flags);
-	return ret;
-}
-
-static int locked_disable_mdp_irq(struct mdp_info *mdp, uint32_t mask)
-{
-	/* this interrupt is already disabled! */
-	if (!(mdp_irq_mask & mask)) {
-		printk(KERN_ERR "mdp irq already off %x %x\n",
-		       mdp_irq_mask, mask);
-		return -1;
-	}
-	/* update the irq mask to reflect the fact that the interrupt is
-	 * disabled */
-	mdp_irq_mask &= ~(mask);
-	/* if no one is waiting on the interrupt, disable it */
-	if (!mdp_irq_mask) {
-		disable_irq_nosync(mdp->irq);
-		if (clk)
-			clk_disable(clk);
-	}
-	return 0;
-}
-
-static int disable_mdp_irq(struct mdp_info *mdp, uint32_t mask)
-{
-	unsigned long irq_flags;
-	int ret;
-
-	spin_lock_irqsave(&mdp_lock, irq_flags);
-	ret = locked_disable_mdp_irq(mdp, mask);
-	spin_unlock_irqrestore(&mdp_lock, irq_flags);
-	return ret;
-}
-
-static irqreturn_t mdp_isr(int irq, void *data)
-{
-	uint32_t status;
-	unsigned long irq_flags;
-	struct mdp_info *mdp = data;
-
-	spin_lock_irqsave(&mdp_lock, irq_flags);
-
-	status = mdp_readl(mdp, MDP_INTR_STATUS);
-	mdp_writel(mdp, status, MDP_INTR_CLEAR);
-
-	status &= mdp_irq_mask;
-	if (status & DL0_DMA2_TERM_DONE) {
-		if (dma_callback) {
-			dma_callback->func(dma_callback);
-			dma_callback = NULL;
-		}
-		wake_up(&mdp_dma2_waitqueue);
-	}
-
-	if (status & DL0_ROI_DONE)
-		wake_up(&mdp_ppp_waitqueue);
-
-	if (status)
-		locked_disable_mdp_irq(mdp, status);
-
-	spin_unlock_irqrestore(&mdp_lock, irq_flags);
-	return IRQ_HANDLED;
-}
-
-static uint32_t mdp_check_mask(uint32_t mask)
-{
-	uint32_t ret;
-	unsigned long irq_flags;
-
-	spin_lock_irqsave(&mdp_lock, irq_flags);
-	ret = mdp_irq_mask & mask;
-	spin_unlock_irqrestore(&mdp_lock, irq_flags);
-	return ret;
-}
-
-static int mdp_wait(struct mdp_info *mdp, uint32_t mask, wait_queue_head_t *wq)
-{
-	int ret = 0;
-	unsigned long irq_flags;
-
-	wait_event_timeout(*wq, !mdp_check_mask(mask), HZ);
-
-	spin_lock_irqsave(&mdp_lock, irq_flags);
-	if (mdp_irq_mask & mask) {
-		locked_disable_mdp_irq(mdp, mask);
-		printk(KERN_WARNING "timeout waiting for mdp to complete %x\n",
-		       mask);
-		ret = -ETIMEDOUT;
-	}
-	spin_unlock_irqrestore(&mdp_lock, irq_flags);
-
-	return ret;
-}
-
-void mdp_dma_wait(struct mdp_device *mdp_dev)
-{
-#define MDP_MAX_TIMEOUTS 20
-	static int timeout_count;
-	struct mdp_info *mdp = container_of(mdp_dev, struct mdp_info, mdp_dev);
-
-	if (mdp_wait(mdp, DL0_DMA2_TERM_DONE, &mdp_dma2_waitqueue) == -ETIMEDOUT)
-		timeout_count++;
-	else
-		timeout_count = 0;
-
-	if (timeout_count > MDP_MAX_TIMEOUTS) {
-		printk(KERN_ERR "mdp: dma failed %d times, somethings wrong!\n",
-		       MDP_MAX_TIMEOUTS);
-		BUG();
-	}
-}
-
-static int mdp_ppp_wait(struct mdp_info *mdp)
-{
-	return mdp_wait(mdp, DL0_ROI_DONE, &mdp_ppp_waitqueue);
-}
-
-void mdp_dma_to_mddi(struct mdp_info *mdp, uint32_t addr, uint32_t stride,
-		     uint32_t width, uint32_t height, uint32_t x, uint32_t y,
-		     struct msmfb_callback *callback)
-{
-	uint32_t dma2_cfg;
-	uint16_t ld_param = 0; /* 0=PRIM, 1=SECD, 2=EXT */
-
-	if (enable_mdp_irq(mdp, DL0_DMA2_TERM_DONE)) {
-		printk(KERN_ERR "mdp_dma_to_mddi: busy\n");
-		return;
-	}
-
-	dma_callback = callback;
-
-	dma2_cfg = DMA_PACK_TIGHT |
-		DMA_PACK_ALIGN_LSB |
-		DMA_PACK_PATTERN_RGB |
-		DMA_OUT_SEL_AHB |
-		DMA_IBUF_NONCONTIGUOUS;
-
-	dma2_cfg |= DMA_IBUF_FORMAT_RGB565;
-
-	dma2_cfg |= DMA_OUT_SEL_MDDI;
-
-	dma2_cfg |= DMA_MDDI_DMAOUT_LCD_SEL_PRIMARY;
-
-	dma2_cfg |= DMA_DITHER_EN;
-
-	/* setup size, address, and stride */
-	mdp_writel(mdp, (height << 16) | (width),
-		   MDP_CMD_DEBUG_ACCESS_BASE + 0x0184);
-	mdp_writel(mdp, addr, MDP_CMD_DEBUG_ACCESS_BASE + 0x0188);
-	mdp_writel(mdp, stride, MDP_CMD_DEBUG_ACCESS_BASE + 0x018C);
-
-	/* 666 18BPP */
-	dma2_cfg |= DMA_DSTC0G_6BITS | DMA_DSTC1B_6BITS | DMA_DSTC2R_6BITS;
-
-	/* set y & x offset and MDDI transaction parameters */
-	mdp_writel(mdp, (y << 16) | (x), MDP_CMD_DEBUG_ACCESS_BASE + 0x0194);
-	mdp_writel(mdp, ld_param, MDP_CMD_DEBUG_ACCESS_BASE + 0x01a0);
-	mdp_writel(mdp, (MDDI_VDO_PACKET_DESC << 16) | MDDI_VDO_PACKET_PRIM,
-		   MDP_CMD_DEBUG_ACCESS_BASE + 0x01a4);
-
-	mdp_writel(mdp, dma2_cfg, MDP_CMD_DEBUG_ACCESS_BASE + 0x0180);
-
-	/* start DMA2 */
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0044);
-}
-
-void mdp_dma(struct mdp_device *mdp_dev, uint32_t addr, uint32_t stride,
-	     uint32_t width, uint32_t height, uint32_t x, uint32_t y,
-	     struct msmfb_callback *callback, int interface)
-{
-	struct mdp_info *mdp = container_of(mdp_dev, struct mdp_info, mdp_dev);
-
-	if (interface == MSM_MDDI_PMDH_INTERFACE) {
-		mdp_dma_to_mddi(mdp, addr, stride, width, height, x, y,
-				callback);
-	}
-}
-
-int get_img(struct mdp_img *img, struct fb_info *info,
-	    unsigned long *start, unsigned long *len,
-	    struct file **filep)
-{
-	int ret = 0;
-	struct fd f = fdget(img->memory_id);
-	if (f.file == NULL)
-		return -1;
-
-	if (MAJOR(file_inode(f.file)->i_rdev) == FB_MAJOR) {
-		*start = info->fix.smem_start;
-		*len = info->fix.smem_len;
-	} else
-		ret = -1;
-	fdput(f);
-
-	return ret;
-}
-
-void put_img(struct file *src_file, struct file *dst_file)
-{
-}
-
-int mdp_blit(struct mdp_device *mdp_dev, struct fb_info *fb,
-	     struct mdp_blit_req *req)
-{
-	int ret;
-	unsigned long src_start = 0, src_len = 0, dst_start = 0, dst_len = 0;
-	struct mdp_info *mdp = container_of(mdp_dev, struct mdp_info, mdp_dev);
-	struct file *src_file = 0, *dst_file = 0;
-
-	/* WORKAROUND FOR HARDWARE BUG IN BG TILE FETCH */
-	if (unlikely(req->src_rect.h == 0 ||
-		     req->src_rect.w == 0)) {
-		printk(KERN_ERR "mpd_ppp: src img of zero size!\n");
-		return -EINVAL;
-	}
-	if (unlikely(req->dst_rect.h == 0 ||
-		     req->dst_rect.w == 0))
-		return -EINVAL;
-
-	/* do this first so that if this fails, the caller can always
-	 * safely call put_img */
-	if (unlikely(get_img(&req->src, fb, &src_start, &src_len, &src_file))) {
-		printk(KERN_ERR "mpd_ppp: could not retrieve src image from "
-				"memory\n");
-		return -EINVAL;
-	}
-
-	if (unlikely(get_img(&req->dst, fb, &dst_start, &dst_len, &dst_file))) {
-		printk(KERN_ERR "mpd_ppp: could not retrieve dst image from "
-				"memory\n");
-		return -EINVAL;
-	}
-	mutex_lock(&mdp_mutex);
-
-	/* transp_masking unimplemented */
-	req->transp_mask = MDP_TRANSP_NOP;
-	if (unlikely((req->transp_mask != MDP_TRANSP_NOP ||
-		      req->alpha != MDP_ALPHA_NOP ||
-		      HAS_ALPHA(req->src.format)) &&
-		     (req->flags & MDP_ROT_90 &&
-		      req->dst_rect.w <= 16 && req->dst_rect.h >= 16))) {
-		int i;
-		unsigned int tiles = req->dst_rect.h / 16;
-		unsigned int remainder = req->dst_rect.h % 16;
-		req->src_rect.w = 16*req->src_rect.w / req->dst_rect.h;
-		req->dst_rect.h = 16;
-		for (i = 0; i < tiles; i++) {
-			enable_mdp_irq(mdp, DL0_ROI_DONE);
-			ret = mdp_ppp_blit(mdp, req, src_file, src_start,
-					   src_len, dst_file, dst_start,
-					   dst_len);
-			if (ret)
-				goto err_bad_blit;
-			ret = mdp_ppp_wait(mdp);
-			if (ret)
-				goto err_wait_failed;
-			req->dst_rect.y += 16;
-			req->src_rect.x += req->src_rect.w;
-		}
-		if (!remainder)
-			goto end;
-		req->src_rect.w = remainder*req->src_rect.w / req->dst_rect.h;
-		req->dst_rect.h = remainder;
-	}
-	enable_mdp_irq(mdp, DL0_ROI_DONE);
-	ret = mdp_ppp_blit(mdp, req, src_file, src_start, src_len, dst_file,
-			   dst_start,
-			   dst_len);
-	if (ret)
-		goto err_bad_blit;
-	ret = mdp_ppp_wait(mdp);
-	if (ret)
-		goto err_wait_failed;
-end:
-	put_img(src_file, dst_file);
-	mutex_unlock(&mdp_mutex);
-	return 0;
-err_bad_blit:
-	disable_mdp_irq(mdp, DL0_ROI_DONE);
-err_wait_failed:
-	put_img(src_file, dst_file);
-	mutex_unlock(&mdp_mutex);
-	return ret;
-}
-
-void mdp_set_grp_disp(struct mdp_device *mdp_dev, unsigned disp_id)
-{
-	struct mdp_info *mdp = container_of(mdp_dev, struct mdp_info, mdp_dev);
-
-	disp_id &= 0xf;
-	mdp_writel(mdp, disp_id, MDP_FULL_BYPASS_WORD43);
-}
-
-int register_mdp_client(struct class_interface *cint)
-{
-	if (!mdp_class) {
-		pr_err("mdp: no mdp_class when registering mdp client\n");
-		return -ENODEV;
-	}
-	cint->class = mdp_class;
-	return class_interface_register(cint);
-}
-
-#include "mdp_csc_table.h"
-#include "mdp_scale_tables.h"
-
-int mdp_probe(struct platform_device *pdev)
-{
-	struct resource *resource;
-	int ret;
-	int n;
-	struct mdp_info *mdp;
-
-	resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!resource) {
-		pr_err("mdp: can not get mdp mem resource!\n");
-		return -ENOMEM;
-	}
-
-	mdp = kzalloc(sizeof(struct mdp_info), GFP_KERNEL);
-	if (!mdp)
-		return -ENOMEM;
-
-	mdp->irq = platform_get_irq(pdev, 0);
-	if (mdp->irq < 0) {
-		pr_err("mdp: can not get mdp irq\n");
-		ret = mdp->irq;
-		goto error_get_irq;
-	}
-
-	mdp->base = ioremap(resource->start, resource_size(resource));
-	if (mdp->base == 0) {
-		printk(KERN_ERR "msmfb: cannot allocate mdp regs!\n");
-		ret = -ENOMEM;
-		goto error_ioremap;
-	}
-
-	mdp->mdp_dev.dma = mdp_dma;
-	mdp->mdp_dev.dma_wait = mdp_dma_wait;
-	mdp->mdp_dev.blit = mdp_blit;
-	mdp->mdp_dev.set_grp_disp = mdp_set_grp_disp;
-
-	clk = clk_get(&pdev->dev, "mdp_clk");
-	if (IS_ERR(clk)) {
-		printk(KERN_INFO "mdp: failed to get mdp clk");
-		ret = PTR_ERR(clk);
-		goto error_get_clk;
-	}
-
-	ret = request_irq(mdp->irq, mdp_isr, 0, "msm_mdp", mdp);
-	if (ret)
-		goto error_request_irq;
-	disable_irq(mdp->irq);
-	mdp_irq_mask = 0;
-
-	/* debug interface write access */
-	mdp_writel(mdp, 1, 0x60);
-
-	mdp_writel(mdp, MDP_ANY_INTR_MASK, MDP_INTR_ENABLE);
-	mdp_writel(mdp, 1, MDP_EBI2_PORTMAP_MODE);
-
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x01f8);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x01fc);
-
-	for (n = 0; n < ARRAY_SIZE(csc_table); n++)
-		mdp_writel(mdp, csc_table[n].val, csc_table[n].reg);
-
-	/* clear up unused fg/main registers */
-	/* comp.plane 2&3 ystride */
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0120);
-
-	/* unpacked pattern */
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x012c);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0130);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0134);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0158);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x015c);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0160);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0170);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0174);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x017c);
-
-	/* comp.plane 2 & 3 */
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0114);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x0118);
-
-	/* clear unused bg registers */
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x01c8);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x01d0);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x01dc);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x01e0);
-	mdp_writel(mdp, 0, MDP_CMD_DEBUG_ACCESS_BASE + 0x01e4);
-
-	for (n = 0; n < ARRAY_SIZE(mdp_upscale_table); n++)
-		mdp_writel(mdp, mdp_upscale_table[n].val,
-		       mdp_upscale_table[n].reg);
-
-	for (n = 0; n < 9; n++)
-		mdp_writel(mdp, mdp_default_ccs[n], 0x40440 + 4 * n);
-	mdp_writel(mdp, mdp_default_ccs[9], 0x40500 + 4 * 0);
-	mdp_writel(mdp, mdp_default_ccs[10], 0x40500 + 4 * 0);
-	mdp_writel(mdp, mdp_default_ccs[11], 0x40500 + 4 * 0);
-
-	/* register mdp device */
-	mdp->mdp_dev.dev.parent = &pdev->dev;
-	mdp->mdp_dev.dev.class = mdp_class;
-	dev_set_name(&mdp->mdp_dev.dev, "mdp%d", pdev->id);
-
-	/* if you can remove the platform device you'd have to implement
-	 * this:
-	mdp_dev.release = mdp_class; */
-
-	ret = device_register(&mdp->mdp_dev.dev);
-	if (ret)
-		goto error_device_register;
-	return 0;
-
-error_device_register:
-	free_irq(mdp->irq, mdp);
-error_request_irq:
-error_get_clk:
-	iounmap(mdp->base);
-error_get_irq:
-error_ioremap:
-	kfree(mdp);
-	return ret;
-}
-
-static struct platform_driver msm_mdp_driver = {
-	.probe = mdp_probe,
-	.driver = {.name = "msm_mdp"},
-};
-
-static int __init mdp_init(void)
-{
-	mdp_class = class_create(THIS_MODULE, "msm_mdp");
-	if (IS_ERR(mdp_class)) {
-		printk(KERN_ERR "Error creating mdp class\n");
-		return PTR_ERR(mdp_class);
-	}
-	return platform_driver_register(&msm_mdp_driver);
-}
-
-subsys_initcall(mdp_init);
diff --git a/drivers/video/fbdev/msm/mdp_csc_table.h b/drivers/video/fbdev/msm/mdp_csc_table.h
deleted file mode 100644
index d1cde30ead52..000000000000
--- a/drivers/video/fbdev/msm/mdp_csc_table.h
+++ /dev/null
@@ -1,582 +0,0 @@
-/* drivers/video/msm_fb/mdp_csc_table.h
- *
- * Copyright (C) 2007 QUALCOMM Incorporated
- * Copyright (C) 2007 Google Incorporated
- *
- * 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.
- */
-
-static struct {
-	uint32_t reg;
-	uint32_t val;
-} csc_table[] = {
-	{ 0x40400, 0x83 },
-	{ 0x40404, 0x102 },
-	{ 0x40408, 0x32 },
-	{ 0x4040c, 0xffffffb5 },
-	{ 0x40410, 0xffffff6c },
-	{ 0x40414, 0xe1 },
-	{ 0x40418, 0xe1 },
-	{ 0x4041c, 0xffffff45 },
-	{ 0x40420, 0xffffffdc },
-	{ 0x40440, 0x254 },
-	{ 0x40444, 0x0 },
-	{ 0x40448, 0x331 },
-	{ 0x4044c, 0x254 },
-	{ 0x40450, 0xffffff38 },
-	{ 0x40454, 0xfffffe61 },
-	{ 0x40458, 0x254 },
-	{ 0x4045c, 0x409 },
-	{ 0x40460, 0x0 },
-	{ 0x40480, 0x5d },
-	{ 0x40484, 0x13a },
-	{ 0x40488, 0x20 },
-	{ 0x4048c, 0xffffffcd },
-	{ 0x40490, 0xffffff54 },
-	{ 0x40494, 0xe1 },
-	{ 0x40498, 0xe1 },
-	{ 0x4049c, 0xffffff35 },
-	{ 0x404a0, 0xffffffec },
-	{ 0x404c0, 0x254 },
-	{ 0x404c4, 0x0 },
-	{ 0x404c8, 0x396 },
-	{ 0x404cc, 0x254 },
-	{ 0x404d0, 0xffffff94 },
-	{ 0x404d4, 0xfffffef0 },
-	{ 0x404d8, 0x254 },
-	{ 0x404dc, 0x43a },
-	{ 0x404e0, 0x0 },
-	{ 0x40500, 0x10 },
-	{ 0x40504, 0x80 },
-	{ 0x40508, 0x80 },
-	{ 0x40540, 0x10 },
-	{ 0x40544, 0x80 },
-	{ 0x40548, 0x80 },
-	{ 0x40580, 0x10 },
-	{ 0x40584, 0xeb },
-	{ 0x40588, 0x10 },
-	{ 0x4058c, 0xf0 },
-	{ 0x405c0, 0x10 },
-	{ 0x405c4, 0xeb },
-	{ 0x405c8, 0x10 },
-	{ 0x405cc, 0xf0 },
-	{ 0x40800, 0x0 },
-	{ 0x40804, 0x151515 },
-	{ 0x40808, 0x1d1d1d },
-	{ 0x4080c, 0x232323 },
-	{ 0x40810, 0x272727 },
-	{ 0x40814, 0x2b2b2b },
-	{ 0x40818, 0x2f2f2f },
-	{ 0x4081c, 0x333333 },
-	{ 0x40820, 0x363636 },
-	{ 0x40824, 0x393939 },
-	{ 0x40828, 0x3b3b3b },
-	{ 0x4082c, 0x3e3e3e },
-	{ 0x40830, 0x404040 },
-	{ 0x40834, 0x434343 },
-	{ 0x40838, 0x454545 },
-	{ 0x4083c, 0x474747 },
-	{ 0x40840, 0x494949 },
-	{ 0x40844, 0x4b4b4b },
-	{ 0x40848, 0x4d4d4d },
-	{ 0x4084c, 0x4f4f4f },
-	{ 0x40850, 0x515151 },
-	{ 0x40854, 0x535353 },
-	{ 0x40858, 0x555555 },
-	{ 0x4085c, 0x565656 },
-	{ 0x40860, 0x585858 },
-	{ 0x40864, 0x5a5a5a },
-	{ 0x40868, 0x5b5b5b },
-	{ 0x4086c, 0x5d5d5d },
-	{ 0x40870, 0x5e5e5e },
-	{ 0x40874, 0x606060 },
-	{ 0x40878, 0x616161 },
-	{ 0x4087c, 0x636363 },
-	{ 0x40880, 0x646464 },
-	{ 0x40884, 0x666666 },
-	{ 0x40888, 0x676767 },
-	{ 0x4088c, 0x686868 },
-	{ 0x40890, 0x6a6a6a },
-	{ 0x40894, 0x6b6b6b },
-	{ 0x40898, 0x6c6c6c },
-	{ 0x4089c, 0x6e6e6e },
-	{ 0x408a0, 0x6f6f6f },
-	{ 0x408a4, 0x707070 },
-	{ 0x408a8, 0x717171 },
-	{ 0x408ac, 0x727272 },
-	{ 0x408b0, 0x747474 },
-	{ 0x408b4, 0x757575 },
-	{ 0x408b8, 0x767676 },
-	{ 0x408bc, 0x777777 },
-	{ 0x408c0, 0x787878 },
-	{ 0x408c4, 0x797979 },
-	{ 0x408c8, 0x7a7a7a },
-	{ 0x408cc, 0x7c7c7c },
-	{ 0x408d0, 0x7d7d7d },
-	{ 0x408d4, 0x7e7e7e },
-	{ 0x408d8, 0x7f7f7f },
-	{ 0x408dc, 0x808080 },
-	{ 0x408e0, 0x818181 },
-	{ 0x408e4, 0x828282 },
-	{ 0x408e8, 0x838383 },
-	{ 0x408ec, 0x848484 },
-	{ 0x408f0, 0x858585 },
-	{ 0x408f4, 0x868686 },
-	{ 0x408f8, 0x878787 },
-	{ 0x408fc, 0x888888 },
-	{ 0x40900, 0x898989 },
-	{ 0x40904, 0x8a8a8a },
-	{ 0x40908, 0x8b8b8b },
-	{ 0x4090c, 0x8c8c8c },
-	{ 0x40910, 0x8d8d8d },
-	{ 0x40914, 0x8e8e8e },
-	{ 0x40918, 0x8f8f8f },
-	{ 0x4091c, 0x8f8f8f },
-	{ 0x40920, 0x909090 },
-	{ 0x40924, 0x919191 },
-	{ 0x40928, 0x929292 },
-	{ 0x4092c, 0x939393 },
-	{ 0x40930, 0x949494 },
-	{ 0x40934, 0x959595 },
-	{ 0x40938, 0x969696 },
-	{ 0x4093c, 0x969696 },
-	{ 0x40940, 0x979797 },
-	{ 0x40944, 0x989898 },
-	{ 0x40948, 0x999999 },
-	{ 0x4094c, 0x9a9a9a },
-	{ 0x40950, 0x9b9b9b },
-	{ 0x40954, 0x9c9c9c },
-	{ 0x40958, 0x9c9c9c },
-	{ 0x4095c, 0x9d9d9d },
-	{ 0x40960, 0x9e9e9e },
-	{ 0x40964, 0x9f9f9f },
-	{ 0x40968, 0xa0a0a0 },
-	{ 0x4096c, 0xa0a0a0 },
-	{ 0x40970, 0xa1a1a1 },
-	{ 0x40974, 0xa2a2a2 },
-	{ 0x40978, 0xa3a3a3 },
-	{ 0x4097c, 0xa4a4a4 },
-	{ 0x40980, 0xa4a4a4 },
-	{ 0x40984, 0xa5a5a5 },
-	{ 0x40988, 0xa6a6a6 },
-	{ 0x4098c, 0xa7a7a7 },
-	{ 0x40990, 0xa7a7a7 },
-	{ 0x40994, 0xa8a8a8 },
-	{ 0x40998, 0xa9a9a9 },
-	{ 0x4099c, 0xaaaaaa },
-	{ 0x409a0, 0xaaaaaa },
-	{ 0x409a4, 0xababab },
-	{ 0x409a8, 0xacacac },
-	{ 0x409ac, 0xadadad },
-	{ 0x409b0, 0xadadad },
-	{ 0x409b4, 0xaeaeae },
-	{ 0x409b8, 0xafafaf },
-	{ 0x409bc, 0xafafaf },
-	{ 0x409c0, 0xb0b0b0 },
-	{ 0x409c4, 0xb1b1b1 },
-	{ 0x409c8, 0xb2b2b2 },
-	{ 0x409cc, 0xb2b2b2 },
-	{ 0x409d0, 0xb3b3b3 },
-	{ 0x409d4, 0xb4b4b4 },
-	{ 0x409d8, 0xb4b4b4 },
-	{ 0x409dc, 0xb5b5b5 },
-	{ 0x409e0, 0xb6b6b6 },
-	{ 0x409e4, 0xb6b6b6 },
-	{ 0x409e8, 0xb7b7b7 },
-	{ 0x409ec, 0xb8b8b8 },
-	{ 0x409f0, 0xb8b8b8 },
-	{ 0x409f4, 0xb9b9b9 },
-	{ 0x409f8, 0xbababa },
-	{ 0x409fc, 0xbababa },
-	{ 0x40a00, 0xbbbbbb },
-	{ 0x40a04, 0xbcbcbc },
-	{ 0x40a08, 0xbcbcbc },
-	{ 0x40a0c, 0xbdbdbd },
-	{ 0x40a10, 0xbebebe },
-	{ 0x40a14, 0xbebebe },
-	{ 0x40a18, 0xbfbfbf },
-	{ 0x40a1c, 0xc0c0c0 },
-	{ 0x40a20, 0xc0c0c0 },
-	{ 0x40a24, 0xc1c1c1 },
-	{ 0x40a28, 0xc1c1c1 },
-	{ 0x40a2c, 0xc2c2c2 },
-	{ 0x40a30, 0xc3c3c3 },
-	{ 0x40a34, 0xc3c3c3 },
-	{ 0x40a38, 0xc4c4c4 },
-	{ 0x40a3c, 0xc5c5c5 },
-	{ 0x40a40, 0xc5c5c5 },
-	{ 0x40a44, 0xc6c6c6 },
-	{ 0x40a48, 0xc6c6c6 },
-	{ 0x40a4c, 0xc7c7c7 },
-	{ 0x40a50, 0xc8c8c8 },
-	{ 0x40a54, 0xc8c8c8 },
-	{ 0x40a58, 0xc9c9c9 },
-	{ 0x40a5c, 0xc9c9c9 },
-	{ 0x40a60, 0xcacaca },
-	{ 0x40a64, 0xcbcbcb },
-	{ 0x40a68, 0xcbcbcb },
-	{ 0x40a6c, 0xcccccc },
-	{ 0x40a70, 0xcccccc },
-	{ 0x40a74, 0xcdcdcd },
-	{ 0x40a78, 0xcecece },
-	{ 0x40a7c, 0xcecece },
-	{ 0x40a80, 0xcfcfcf },
-	{ 0x40a84, 0xcfcfcf },
-	{ 0x40a88, 0xd0d0d0 },
-	{ 0x40a8c, 0xd0d0d0 },
-	{ 0x40a90, 0xd1d1d1 },
-	{ 0x40a94, 0xd2d2d2 },
-	{ 0x40a98, 0xd2d2d2 },
-	{ 0x40a9c, 0xd3d3d3 },
-	{ 0x40aa0, 0xd3d3d3 },
-	{ 0x40aa4, 0xd4d4d4 },
-	{ 0x40aa8, 0xd4d4d4 },
-	{ 0x40aac, 0xd5d5d5 },
-	{ 0x40ab0, 0xd6d6d6 },
-	{ 0x40ab4, 0xd6d6d6 },
-	{ 0x40ab8, 0xd7d7d7 },
-	{ 0x40abc, 0xd7d7d7 },
-	{ 0x40ac0, 0xd8d8d8 },
-	{ 0x40ac4, 0xd8d8d8 },
-	{ 0x40ac8, 0xd9d9d9 },
-	{ 0x40acc, 0xd9d9d9 },
-	{ 0x40ad0, 0xdadada },
-	{ 0x40ad4, 0xdbdbdb },
-	{ 0x40ad8, 0xdbdbdb },
-	{ 0x40adc, 0xdcdcdc },
-	{ 0x40ae0, 0xdcdcdc },
-	{ 0x40ae4, 0xdddddd },
-	{ 0x40ae8, 0xdddddd },
-	{ 0x40aec, 0xdedede },
-	{ 0x40af0, 0xdedede },
-	{ 0x40af4, 0xdfdfdf },
-	{ 0x40af8, 0xdfdfdf },
-	{ 0x40afc, 0xe0e0e0 },
-	{ 0x40b00, 0xe0e0e0 },
-	{ 0x40b04, 0xe1e1e1 },
-	{ 0x40b08, 0xe1e1e1 },
-	{ 0x40b0c, 0xe2e2e2 },
-	{ 0x40b10, 0xe3e3e3 },
-	{ 0x40b14, 0xe3e3e3 },
-	{ 0x40b18, 0xe4e4e4 },
-	{ 0x40b1c, 0xe4e4e4 },
-	{ 0x40b20, 0xe5e5e5 },
-	{ 0x40b24, 0xe5e5e5 },
-	{ 0x40b28, 0xe6e6e6 },
-	{ 0x40b2c, 0xe6e6e6 },
-	{ 0x40b30, 0xe7e7e7 },
-	{ 0x40b34, 0xe7e7e7 },
-	{ 0x40b38, 0xe8e8e8 },
-	{ 0x40b3c, 0xe8e8e8 },
-	{ 0x40b40, 0xe9e9e9 },
-	{ 0x40b44, 0xe9e9e9 },
-	{ 0x40b48, 0xeaeaea },
-	{ 0x40b4c, 0xeaeaea },
-	{ 0x40b50, 0xebebeb },
-	{ 0x40b54, 0xebebeb },
-	{ 0x40b58, 0xececec },
-	{ 0x40b5c, 0xececec },
-	{ 0x40b60, 0xededed },
-	{ 0x40b64, 0xededed },
-	{ 0x40b68, 0xeeeeee },
-	{ 0x40b6c, 0xeeeeee },
-	{ 0x40b70, 0xefefef },
-	{ 0x40b74, 0xefefef },
-	{ 0x40b78, 0xf0f0f0 },
-	{ 0x40b7c, 0xf0f0f0 },
-	{ 0x40b80, 0xf1f1f1 },
-	{ 0x40b84, 0xf1f1f1 },
-	{ 0x40b88, 0xf2f2f2 },
-	{ 0x40b8c, 0xf2f2f2 },
-	{ 0x40b90, 0xf2f2f2 },
-	{ 0x40b94, 0xf3f3f3 },
-	{ 0x40b98, 0xf3f3f3 },
-	{ 0x40b9c, 0xf4f4f4 },
-	{ 0x40ba0, 0xf4f4f4 },
-	{ 0x40ba4, 0xf5f5f5 },
-	{ 0x40ba8, 0xf5f5f5 },
-	{ 0x40bac, 0xf6f6f6 },
-	{ 0x40bb0, 0xf6f6f6 },
-	{ 0x40bb4, 0xf7f7f7 },
-	{ 0x40bb8, 0xf7f7f7 },
-	{ 0x40bbc, 0xf8f8f8 },
-	{ 0x40bc0, 0xf8f8f8 },
-	{ 0x40bc4, 0xf9f9f9 },
-	{ 0x40bc8, 0xf9f9f9 },
-	{ 0x40bcc, 0xfafafa },
-	{ 0x40bd0, 0xfafafa },
-	{ 0x40bd4, 0xfafafa },
-	{ 0x40bd8, 0xfbfbfb },
-	{ 0x40bdc, 0xfbfbfb },
-	{ 0x40be0, 0xfcfcfc },
-	{ 0x40be4, 0xfcfcfc },
-	{ 0x40be8, 0xfdfdfd },
-	{ 0x40bec, 0xfdfdfd },
-	{ 0x40bf0, 0xfefefe },
-	{ 0x40bf4, 0xfefefe },
-	{ 0x40bf8, 0xffffff },
-	{ 0x40bfc, 0xffffff },
-	{ 0x40c00, 0x0 },
-	{ 0x40c04, 0x0 },
-	{ 0x40c08, 0x0 },
-	{ 0x40c0c, 0x0 },
-	{ 0x40c10, 0x0 },
-	{ 0x40c14, 0x0 },
-	{ 0x40c18, 0x0 },
-	{ 0x40c1c, 0x0 },
-	{ 0x40c20, 0x0 },
-	{ 0x40c24, 0x0 },
-	{ 0x40c28, 0x0 },
-	{ 0x40c2c, 0x0 },
-	{ 0x40c30, 0x0 },
-	{ 0x40c34, 0x0 },
-	{ 0x40c38, 0x0 },
-	{ 0x40c3c, 0x0 },
-	{ 0x40c40, 0x10101 },
-	{ 0x40c44, 0x10101 },
-	{ 0x40c48, 0x10101 },
-	{ 0x40c4c, 0x10101 },
-	{ 0x40c50, 0x10101 },
-	{ 0x40c54, 0x10101 },
-	{ 0x40c58, 0x10101 },
-	{ 0x40c5c, 0x10101 },
-	{ 0x40c60, 0x10101 },
-	{ 0x40c64, 0x10101 },
-	{ 0x40c68, 0x20202 },
-	{ 0x40c6c, 0x20202 },
-	{ 0x40c70, 0x20202 },
-	{ 0x40c74, 0x20202 },
-	{ 0x40c78, 0x20202 },
-	{ 0x40c7c, 0x20202 },
-	{ 0x40c80, 0x30303 },
-	{ 0x40c84, 0x30303 },
-	{ 0x40c88, 0x30303 },
-	{ 0x40c8c, 0x30303 },
-	{ 0x40c90, 0x30303 },
-	{ 0x40c94, 0x40404 },
-	{ 0x40c98, 0x40404 },
-	{ 0x40c9c, 0x40404 },
-	{ 0x40ca0, 0x40404 },
-	{ 0x40ca4, 0x40404 },
-	{ 0x40ca8, 0x50505 },
-	{ 0x40cac, 0x50505 },
-	{ 0x40cb0, 0x50505 },
-	{ 0x40cb4, 0x50505 },
-	{ 0x40cb8, 0x60606 },
-	{ 0x40cbc, 0x60606 },
-	{ 0x40cc0, 0x60606 },
-	{ 0x40cc4, 0x70707 },
-	{ 0x40cc8, 0x70707 },
-	{ 0x40ccc, 0x70707 },
-	{ 0x40cd0, 0x70707 },
-	{ 0x40cd4, 0x80808 },
-	{ 0x40cd8, 0x80808 },
-	{ 0x40cdc, 0x80808 },
-	{ 0x40ce0, 0x90909 },
-	{ 0x40ce4, 0x90909 },
-	{ 0x40ce8, 0xa0a0a },
-	{ 0x40cec, 0xa0a0a },
-	{ 0x40cf0, 0xa0a0a },
-	{ 0x40cf4, 0xb0b0b },
-	{ 0x40cf8, 0xb0b0b },
-	{ 0x40cfc, 0xb0b0b },
-	{ 0x40d00, 0xc0c0c },
-	{ 0x40d04, 0xc0c0c },
-	{ 0x40d08, 0xd0d0d },
-	{ 0x40d0c, 0xd0d0d },
-	{ 0x40d10, 0xe0e0e },
-	{ 0x40d14, 0xe0e0e },
-	{ 0x40d18, 0xe0e0e },
-	{ 0x40d1c, 0xf0f0f },
-	{ 0x40d20, 0xf0f0f },
-	{ 0x40d24, 0x101010 },
-	{ 0x40d28, 0x101010 },
-	{ 0x40d2c, 0x111111 },
-	{ 0x40d30, 0x111111 },
-	{ 0x40d34, 0x121212 },
-	{ 0x40d38, 0x121212 },
-	{ 0x40d3c, 0x131313 },
-	{ 0x40d40, 0x131313 },
-	{ 0x40d44, 0x141414 },
-	{ 0x40d48, 0x151515 },
-	{ 0x40d4c, 0x151515 },
-	{ 0x40d50, 0x161616 },
-	{ 0x40d54, 0x161616 },
-	{ 0x40d58, 0x171717 },
-	{ 0x40d5c, 0x171717 },
-	{ 0x40d60, 0x181818 },
-	{ 0x40d64, 0x191919 },
-	{ 0x40d68, 0x191919 },
-	{ 0x40d6c, 0x1a1a1a },
-	{ 0x40d70, 0x1b1b1b },
-	{ 0x40d74, 0x1b1b1b },
-	{ 0x40d78, 0x1c1c1c },
-	{ 0x40d7c, 0x1c1c1c },
-	{ 0x40d80, 0x1d1d1d },
-	{ 0x40d84, 0x1e1e1e },
-	{ 0x40d88, 0x1f1f1f },
-	{ 0x40d8c, 0x1f1f1f },
-	{ 0x40d90, 0x202020 },
-	{ 0x40d94, 0x212121 },
-	{ 0x40d98, 0x212121 },
-	{ 0x40d9c, 0x222222 },
-	{ 0x40da0, 0x232323 },
-	{ 0x40da4, 0x242424 },
-	{ 0x40da8, 0x242424 },
-	{ 0x40dac, 0x252525 },
-	{ 0x40db0, 0x262626 },
-	{ 0x40db4, 0x272727 },
-	{ 0x40db8, 0x272727 },
-	{ 0x40dbc, 0x282828 },
-	{ 0x40dc0, 0x292929 },
-	{ 0x40dc4, 0x2a2a2a },
-	{ 0x40dc8, 0x2b2b2b },
-	{ 0x40dcc, 0x2c2c2c },
-	{ 0x40dd0, 0x2c2c2c },
-	{ 0x40dd4, 0x2d2d2d },
-	{ 0x40dd8, 0x2e2e2e },
-	{ 0x40ddc, 0x2f2f2f },
-	{ 0x40de0, 0x303030 },
-	{ 0x40de4, 0x313131 },
-	{ 0x40de8, 0x323232 },
-	{ 0x40dec, 0x333333 },
-	{ 0x40df0, 0x333333 },
-	{ 0x40df4, 0x343434 },
-	{ 0x40df8, 0x353535 },
-	{ 0x40dfc, 0x363636 },
-	{ 0x40e00, 0x373737 },
-	{ 0x40e04, 0x383838 },
-	{ 0x40e08, 0x393939 },
-	{ 0x40e0c, 0x3a3a3a },
-	{ 0x40e10, 0x3b3b3b },
-	{ 0x40e14, 0x3c3c3c },
-	{ 0x40e18, 0x3d3d3d },
-	{ 0x40e1c, 0x3e3e3e },
-	{ 0x40e20, 0x3f3f3f },
-	{ 0x40e24, 0x404040 },
-	{ 0x40e28, 0x414141 },
-	{ 0x40e2c, 0x424242 },
-	{ 0x40e30, 0x434343 },
-	{ 0x40e34, 0x444444 },
-	{ 0x40e38, 0x464646 },
-	{ 0x40e3c, 0x474747 },
-	{ 0x40e40, 0x484848 },
-	{ 0x40e44, 0x494949 },
-	{ 0x40e48, 0x4a4a4a },
-	{ 0x40e4c, 0x4b4b4b },
-	{ 0x40e50, 0x4c4c4c },
-	{ 0x40e54, 0x4d4d4d },
-	{ 0x40e58, 0x4f4f4f },
-	{ 0x40e5c, 0x505050 },
-	{ 0x40e60, 0x515151 },
-	{ 0x40e64, 0x525252 },
-	{ 0x40e68, 0x535353 },
-	{ 0x40e6c, 0x545454 },
-	{ 0x40e70, 0x565656 },
-	{ 0x40e74, 0x575757 },
-	{ 0x40e78, 0x585858 },
-	{ 0x40e7c, 0x595959 },
-	{ 0x40e80, 0x5b5b5b },
-	{ 0x40e84, 0x5c5c5c },
-	{ 0x40e88, 0x5d5d5d },
-	{ 0x40e8c, 0x5e5e5e },
-	{ 0x40e90, 0x606060 },
-	{ 0x40e94, 0x616161 },
-	{ 0x40e98, 0x626262 },
-	{ 0x40e9c, 0x646464 },
-	{ 0x40ea0, 0x656565 },
-	{ 0x40ea4, 0x666666 },
-	{ 0x40ea8, 0x686868 },
-	{ 0x40eac, 0x696969 },
-	{ 0x40eb0, 0x6a6a6a },
-	{ 0x40eb4, 0x6c6c6c },
-	{ 0x40eb8, 0x6d6d6d },
-	{ 0x40ebc, 0x6f6f6f },
-	{ 0x40ec0, 0x707070 },
-	{ 0x40ec4, 0x717171 },
-	{ 0x40ec8, 0x737373 },
-	{ 0x40ecc, 0x747474 },
-	{ 0x40ed0, 0x767676 },
-	{ 0x40ed4, 0x777777 },
-	{ 0x40ed8, 0x797979 },
-	{ 0x40edc, 0x7a7a7a },
-	{ 0x40ee0, 0x7c7c7c },
-	{ 0x40ee4, 0x7d7d7d },
-	{ 0x40ee8, 0x7f7f7f },
-	{ 0x40eec, 0x808080 },
-	{ 0x40ef0, 0x828282 },
-	{ 0x40ef4, 0x838383 },
-	{ 0x40ef8, 0x858585 },
-	{ 0x40efc, 0x868686 },
-	{ 0x40f00, 0x888888 },
-	{ 0x40f04, 0x898989 },
-	{ 0x40f08, 0x8b8b8b },
-	{ 0x40f0c, 0x8d8d8d },
-	{ 0x40f10, 0x8e8e8e },
-	{ 0x40f14, 0x909090 },
-	{ 0x40f18, 0x919191 },
-	{ 0x40f1c, 0x939393 },
-	{ 0x40f20, 0x959595 },
-	{ 0x40f24, 0x969696 },
-	{ 0x40f28, 0x989898 },
-	{ 0x40f2c, 0x9a9a9a },
-	{ 0x40f30, 0x9b9b9b },
-	{ 0x40f34, 0x9d9d9d },
-	{ 0x40f38, 0x9f9f9f },
-	{ 0x40f3c, 0xa1a1a1 },
-	{ 0x40f40, 0xa2a2a2 },
-	{ 0x40f44, 0xa4a4a4 },
-	{ 0x40f48, 0xa6a6a6 },
-	{ 0x40f4c, 0xa7a7a7 },
-	{ 0x40f50, 0xa9a9a9 },
-	{ 0x40f54, 0xababab },
-	{ 0x40f58, 0xadadad },
-	{ 0x40f5c, 0xafafaf },
-	{ 0x40f60, 0xb0b0b0 },
-	{ 0x40f64, 0xb2b2b2 },
-	{ 0x40f68, 0xb4b4b4 },
-	{ 0x40f6c, 0xb6b6b6 },
-	{ 0x40f70, 0xb8b8b8 },
-	{ 0x40f74, 0xbababa },
-	{ 0x40f78, 0xbbbbbb },
-	{ 0x40f7c, 0xbdbdbd },
-	{ 0x40f80, 0xbfbfbf },
-	{ 0x40f84, 0xc1c1c1 },
-	{ 0x40f88, 0xc3c3c3 },
-	{ 0x40f8c, 0xc5c5c5 },
-	{ 0x40f90, 0xc7c7c7 },
-	{ 0x40f94, 0xc9c9c9 },
-	{ 0x40f98, 0xcbcbcb },
-	{ 0x40f9c, 0xcdcdcd },
-	{ 0x40fa0, 0xcfcfcf },
-	{ 0x40fa4, 0xd1d1d1 },
-	{ 0x40fa8, 0xd3d3d3 },
-	{ 0x40fac, 0xd5d5d5 },
-	{ 0x40fb0, 0xd7d7d7 },
-	{ 0x40fb4, 0xd9d9d9 },
-	{ 0x40fb8, 0xdbdbdb },
-	{ 0x40fbc, 0xdddddd },
-	{ 0x40fc0, 0xdfdfdf },
-	{ 0x40fc4, 0xe1e1e1 },
-	{ 0x40fc8, 0xe3e3e3 },
-	{ 0x40fcc, 0xe5e5e5 },
-	{ 0x40fd0, 0xe7e7e7 },
-	{ 0x40fd4, 0xe9e9e9 },
-	{ 0x40fd8, 0xebebeb },
-	{ 0x40fdc, 0xeeeeee },
-	{ 0x40fe0, 0xf0f0f0 },
-	{ 0x40fe4, 0xf2f2f2 },
-	{ 0x40fe8, 0xf4f4f4 },
-	{ 0x40fec, 0xf6f6f6 },
-	{ 0x40ff0, 0xf8f8f8 },
-	{ 0x40ff4, 0xfbfbfb },
-	{ 0x40ff8, 0xfdfdfd },
-	{ 0x40ffc, 0xffffff },
-};
diff --git a/drivers/video/fbdev/msm/mdp_hw.h b/drivers/video/fbdev/msm/mdp_hw.h
deleted file mode 100644
index 35848d741001..000000000000
--- a/drivers/video/fbdev/msm/mdp_hw.h
+++ /dev/null
@@ -1,627 +0,0 @@
-/* drivers/video/msm_fb/mdp_hw.h
- *
- * Copyright (C) 2007 QUALCOMM Incorporated
- * Copyright (C) 2007 Google Incorporated
- *
- * 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 _MDP_HW_H_
-#define _MDP_HW_H_
-
-#include <linux/platform_data/video-msm_fb.h>
-
-struct mdp_info {
-	struct mdp_device mdp_dev;
-	char * __iomem base;
-	int irq;
-};
-struct mdp_blit_req;
-struct mdp_device;
-int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
-		 struct file *src_file, unsigned long src_start,
-		 unsigned long src_len, struct file *dst_file,
-		 unsigned long dst_start, unsigned long dst_len);
-#define mdp_writel(mdp, value, offset) writel(value, mdp->base + offset)
-#define mdp_readl(mdp, offset) readl(mdp->base + offset)
-
-#define MDP_SYNC_CONFIG_0                (0x00000)
-#define MDP_SYNC_CONFIG_1                (0x00004)
-#define MDP_SYNC_CONFIG_2                (0x00008)
-#define MDP_SYNC_STATUS_0                (0x0000c)
-#define MDP_SYNC_STATUS_1                (0x00010)
-#define MDP_SYNC_STATUS_2                (0x00014)
-#define MDP_SYNC_THRESH_0                (0x00018)
-#define MDP_SYNC_THRESH_1                (0x0001c)
-#define MDP_INTR_ENABLE                  (0x00020)
-#define MDP_INTR_STATUS                  (0x00024)
-#define MDP_INTR_CLEAR                   (0x00028)
-#define MDP_DISPLAY0_START               (0x00030)
-#define MDP_DISPLAY1_START               (0x00034)
-#define MDP_DISPLAY_STATUS               (0x00038)
-#define MDP_EBI2_LCD0                    (0x0003c)
-#define MDP_EBI2_LCD1                    (0x00040)
-#define MDP_DISPLAY0_ADDR                (0x00054)
-#define MDP_DISPLAY1_ADDR                (0x00058)
-#define MDP_EBI2_PORTMAP_MODE            (0x0005c)
-#define MDP_MODE                         (0x00060)
-#define MDP_TV_OUT_STATUS                (0x00064)
-#define MDP_HW_VERSION                   (0x00070)
-#define MDP_SW_RESET                     (0x00074)
-#define MDP_AXI_ERROR_MASTER_STOP        (0x00078)
-#define MDP_SEL_CLK_OR_HCLK_TEST_BUS     (0x0007c)
-#define MDP_PRIMARY_VSYNC_OUT_CTRL       (0x00080)
-#define MDP_SECONDARY_VSYNC_OUT_CTRL     (0x00084)
-#define MDP_EXTERNAL_VSYNC_OUT_CTRL      (0x00088)
-#define MDP_VSYNC_CTRL                   (0x0008c)
-#define MDP_CGC_EN                       (0x00100)
-#define MDP_CMD_STATUS                   (0x10008)
-#define MDP_PROFILE_EN                   (0x10010)
-#define MDP_PROFILE_COUNT                (0x10014)
-#define MDP_DMA_START                    (0x10044)
-#define MDP_FULL_BYPASS_WORD0            (0x10100)
-#define MDP_FULL_BYPASS_WORD1            (0x10104)
-#define MDP_COMMAND_CONFIG               (0x10104)
-#define MDP_FULL_BYPASS_WORD2            (0x10108)
-#define MDP_FULL_BYPASS_WORD3            (0x1010c)
-#define MDP_FULL_BYPASS_WORD4            (0x10110)
-#define MDP_FULL_BYPASS_WORD6            (0x10118)
-#define MDP_FULL_BYPASS_WORD7            (0x1011c)
-#define MDP_FULL_BYPASS_WORD8            (0x10120)
-#define MDP_FULL_BYPASS_WORD9            (0x10124)
-#define MDP_PPP_SOURCE_CONFIG            (0x10124)
-#define MDP_FULL_BYPASS_WORD10           (0x10128)
-#define MDP_FULL_BYPASS_WORD11           (0x1012c)
-#define MDP_FULL_BYPASS_WORD12           (0x10130)
-#define MDP_FULL_BYPASS_WORD13           (0x10134)
-#define MDP_FULL_BYPASS_WORD14           (0x10138)
-#define MDP_PPP_OPERATION_CONFIG         (0x10138)
-#define MDP_FULL_BYPASS_WORD15           (0x1013c)
-#define MDP_FULL_BYPASS_WORD16           (0x10140)
-#define MDP_FULL_BYPASS_WORD17           (0x10144)
-#define MDP_FULL_BYPASS_WORD18           (0x10148)
-#define MDP_FULL_BYPASS_WORD19           (0x1014c)
-#define MDP_FULL_BYPASS_WORD20           (0x10150)
-#define MDP_PPP_DESTINATION_CONFIG       (0x10150)
-#define MDP_FULL_BYPASS_WORD21           (0x10154)
-#define MDP_FULL_BYPASS_WORD22           (0x10158)
-#define MDP_FULL_BYPASS_WORD23           (0x1015c)
-#define MDP_FULL_BYPASS_WORD24           (0x10160)
-#define MDP_FULL_BYPASS_WORD25           (0x10164)
-#define MDP_FULL_BYPASS_WORD26           (0x10168)
-#define MDP_FULL_BYPASS_WORD27           (0x1016c)
-#define MDP_FULL_BYPASS_WORD29           (0x10174)
-#define MDP_FULL_BYPASS_WORD30           (0x10178)
-#define MDP_FULL_BYPASS_WORD31           (0x1017c)
-#define MDP_FULL_BYPASS_WORD32           (0x10180)
-#define MDP_DMA_CONFIG                   (0x10180)
-#define MDP_FULL_BYPASS_WORD33           (0x10184)
-#define MDP_FULL_BYPASS_WORD34           (0x10188)
-#define MDP_FULL_BYPASS_WORD35           (0x1018c)
-#define MDP_FULL_BYPASS_WORD37           (0x10194)
-#define MDP_FULL_BYPASS_WORD39           (0x1019c)
-#define MDP_FULL_BYPASS_WORD40           (0x101a0)
-#define MDP_FULL_BYPASS_WORD41           (0x101a4)
-#define MDP_FULL_BYPASS_WORD43           (0x101ac)
-#define MDP_FULL_BYPASS_WORD46           (0x101b8)
-#define MDP_FULL_BYPASS_WORD47           (0x101bc)
-#define MDP_FULL_BYPASS_WORD48           (0x101c0)
-#define MDP_FULL_BYPASS_WORD49           (0x101c4)
-#define MDP_FULL_BYPASS_WORD50           (0x101c8)
-#define MDP_FULL_BYPASS_WORD51           (0x101cc)
-#define MDP_FULL_BYPASS_WORD52           (0x101d0)
-#define MDP_FULL_BYPASS_WORD53           (0x101d4)
-#define MDP_FULL_BYPASS_WORD54           (0x101d8)
-#define MDP_FULL_BYPASS_WORD55           (0x101dc)
-#define MDP_FULL_BYPASS_WORD56           (0x101e0)
-#define MDP_FULL_BYPASS_WORD57           (0x101e4)
-#define MDP_FULL_BYPASS_WORD58           (0x101e8)
-#define MDP_FULL_BYPASS_WORD59           (0x101ec)
-#define MDP_FULL_BYPASS_WORD60           (0x101f0)
-#define MDP_VSYNC_THRESHOLD              (0x101f0)
-#define MDP_FULL_BYPASS_WORD61           (0x101f4)
-#define MDP_FULL_BYPASS_WORD62           (0x101f8)
-#define MDP_FULL_BYPASS_WORD63           (0x101fc)
-#define MDP_TFETCH_TEST_MODE             (0x20004)
-#define MDP_TFETCH_STATUS                (0x20008)
-#define MDP_TFETCH_TILE_COUNT            (0x20010)
-#define MDP_TFETCH_FETCH_COUNT           (0x20014)
-#define MDP_TFETCH_CONSTANT_COLOR        (0x20040)
-#define MDP_CSC_BYPASS                   (0x40004)
-#define MDP_SCALE_COEFF_LSB              (0x5fffc)
-#define MDP_TV_OUT_CTL                   (0xc0000)
-#define MDP_TV_OUT_FIR_COEFF             (0xc0004)
-#define MDP_TV_OUT_BUF_ADDR              (0xc0008)
-#define MDP_TV_OUT_CC_DATA               (0xc000c)
-#define MDP_TV_OUT_SOBEL                 (0xc0010)
-#define MDP_TV_OUT_Y_CLAMP               (0xc0018)
-#define MDP_TV_OUT_CB_CLAMP              (0xc001c)
-#define MDP_TV_OUT_CR_CLAMP              (0xc0020)
-#define MDP_TEST_MODE_CLK                (0xd0000)
-#define MDP_TEST_MISR_RESET_CLK          (0xd0004)
-#define MDP_TEST_EXPORT_MISR_CLK         (0xd0008)
-#define MDP_TEST_MISR_CURR_VAL_CLK       (0xd000c)
-#define MDP_TEST_MODE_HCLK               (0xd0100)
-#define MDP_TEST_MISR_RESET_HCLK         (0xd0104)
-#define MDP_TEST_EXPORT_MISR_HCLK        (0xd0108)
-#define MDP_TEST_MISR_CURR_VAL_HCLK      (0xd010c)
-#define MDP_TEST_MODE_DCLK               (0xd0200)
-#define MDP_TEST_MISR_RESET_DCLK         (0xd0204)
-#define MDP_TEST_EXPORT_MISR_DCLK        (0xd0208)
-#define MDP_TEST_MISR_CURR_VAL_DCLK      (0xd020c)
-#define MDP_TEST_CAPTURED_DCLK           (0xd0210)
-#define MDP_TEST_MISR_CAPT_VAL_DCLK      (0xd0214)
-#define MDP_LCDC_CTL                     (0xe0000)
-#define MDP_LCDC_HSYNC_CTL               (0xe0004)
-#define MDP_LCDC_VSYNC_CTL               (0xe0008)
-#define MDP_LCDC_ACTIVE_HCTL             (0xe000c)
-#define MDP_LCDC_ACTIVE_VCTL             (0xe0010)
-#define MDP_LCDC_BORDER_CLR              (0xe0014)
-#define MDP_LCDC_H_BLANK                 (0xe0018)
-#define MDP_LCDC_V_BLANK                 (0xe001c)
-#define MDP_LCDC_UNDERFLOW_CLR           (0xe0020)
-#define MDP_LCDC_HSYNC_SKEW              (0xe0024)
-#define MDP_LCDC_TEST_CTL                (0xe0028)
-#define MDP_LCDC_LINE_IRQ                (0xe002c)
-#define MDP_LCDC_CTL_POLARITY            (0xe0030)
-#define MDP_LCDC_DMA_CONFIG              (0xe1000)
-#define MDP_LCDC_DMA_SIZE                (0xe1004)
-#define MDP_LCDC_DMA_IBUF_ADDR           (0xe1008)
-#define MDP_LCDC_DMA_IBUF_Y_STRIDE       (0xe100c)
-
-
-#define MDP_DMA2_TERM 0x1
-#define MDP_DMA3_TERM 0x2
-#define MDP_PPP_TERM 0x3
-
-/* MDP_INTR_ENABLE */
-#define DL0_ROI_DONE           (1<<0)
-#define DL1_ROI_DONE           (1<<1)
-#define DL0_DMA2_TERM_DONE     (1<<2)
-#define DL1_DMA2_TERM_DONE     (1<<3)
-#define DL0_PPP_TERM_DONE      (1<<4)
-#define DL1_PPP_TERM_DONE      (1<<5)
-#define TV_OUT_DMA3_DONE       (1<<6)
-#define TV_ENC_UNDERRUN        (1<<7)
-#define DL0_FETCH_DONE         (1<<11)
-#define DL1_FETCH_DONE         (1<<12)
-
-#define MDP_PPP_BUSY_STATUS (DL0_ROI_DONE| \
-			   DL1_ROI_DONE| \
-			   DL0_PPP_TERM_DONE| \
-			   DL1_PPP_TERM_DONE)
-
-#define MDP_ANY_INTR_MASK (DL0_ROI_DONE| \
-			   DL1_ROI_DONE| \
-			   DL0_DMA2_TERM_DONE| \
-			   DL1_DMA2_TERM_DONE| \
-			   DL0_PPP_TERM_DONE| \
-			   DL1_PPP_TERM_DONE| \
-			   DL0_FETCH_DONE| \
-			   DL1_FETCH_DONE| \
-			   TV_ENC_UNDERRUN)
-
-#define MDP_TOP_LUMA       16
-#define MDP_TOP_CHROMA     0
-#define MDP_BOTTOM_LUMA    19
-#define MDP_BOTTOM_CHROMA  3
-#define MDP_LEFT_LUMA      22
-#define MDP_LEFT_CHROMA    6
-#define MDP_RIGHT_LUMA     25
-#define MDP_RIGHT_CHROMA   9
-
-#define CLR_G 0x0
-#define CLR_B 0x1
-#define CLR_R 0x2
-#define CLR_ALPHA 0x3
-
-#define CLR_Y  CLR_G
-#define CLR_CB CLR_B
-#define CLR_CR CLR_R
-
-/* from lsb to msb */
-#define MDP_GET_PACK_PATTERN(a, x, y, z, bit) \
-	(((a)<<(bit*3))|((x)<<(bit*2))|((y)<<bit)|(z))
-
-/* MDP_SYNC_CONFIG_0/1/2 */
-#define MDP_SYNCFG_HGT_LOC 22
-#define MDP_SYNCFG_VSYNC_EXT_EN (1<<21)
-#define MDP_SYNCFG_VSYNC_INT_EN (1<<20)
-
-/* MDP_SYNC_THRESH_0 */
-#define MDP_PRIM_BELOW_LOC 0
-#define MDP_PRIM_ABOVE_LOC 8
-
-/* MDP_{PRIMARY,SECONDARY,EXTERNAL}_VSYNC_OUT_CRL */
-#define VSYNC_PULSE_EN (1<<31)
-#define VSYNC_PULSE_INV (1<<30)
-
-/* MDP_VSYNC_CTRL */
-#define DISP0_VSYNC_MAP_VSYNC0 0
-#define DISP0_VSYNC_MAP_VSYNC1 (1<<0)
-#define DISP0_VSYNC_MAP_VSYNC2 ((1<<0)|(1<<1))
-
-#define DISP1_VSYNC_MAP_VSYNC0 0
-#define DISP1_VSYNC_MAP_VSYNC1 (1<<2)
-#define DISP1_VSYNC_MAP_VSYNC2 ((1<<2)|(1<<3))
-
-#define PRIMARY_LCD_SYNC_EN (1<<4)
-#define PRIMARY_LCD_SYNC_DISABLE 0
-
-#define SECONDARY_LCD_SYNC_EN (1<<5)
-#define SECONDARY_LCD_SYNC_DISABLE 0
-
-#define EXTERNAL_LCD_SYNC_EN (1<<6)
-#define EXTERNAL_LCD_SYNC_DISABLE 0
-
-/* MDP_VSYNC_THRESHOLD / MDP_FULL_BYPASS_WORD60 */
-#define VSYNC_THRESHOLD_ABOVE_LOC 0
-#define VSYNC_THRESHOLD_BELOW_LOC 16
-#define VSYNC_ANTI_TEAR_EN (1<<31)
-
-/* MDP_COMMAND_CONFIG / MDP_FULL_BYPASS_WORD1 */
-#define MDP_CMD_DBGBUS_EN (1<<0)
-
-/* MDP_PPP_SOURCE_CONFIG / MDP_FULL_BYPASS_WORD9&53 */
-#define PPP_SRC_C0G_8BIT ((1<<1)|(1<<0))
-#define PPP_SRC_C1B_8BIT ((1<<3)|(1<<2))
-#define PPP_SRC_C2R_8BIT ((1<<5)|(1<<4))
-#define PPP_SRC_C3A_8BIT ((1<<7)|(1<<6))
-
-#define PPP_SRC_C0G_6BIT (1<<1)
-#define PPP_SRC_C1B_6BIT (1<<3)
-#define PPP_SRC_C2R_6BIT (1<<5)
-
-#define PPP_SRC_C0G_5BIT (1<<0)
-#define PPP_SRC_C1B_5BIT (1<<2)
-#define PPP_SRC_C2R_5BIT (1<<4)
-
-#define PPP_SRC_C3ALPHA_EN (1<<8)
-
-#define PPP_SRC_BPP_1BYTES 0
-#define PPP_SRC_BPP_2BYTES (1<<9)
-#define PPP_SRC_BPP_3BYTES (1<<10)
-#define PPP_SRC_BPP_4BYTES ((1<<10)|(1<<9))
-
-#define PPP_SRC_BPP_ROI_ODD_X (1<<11)
-#define PPP_SRC_BPP_ROI_ODD_Y (1<<12)
-#define PPP_SRC_INTERLVD_2COMPONENTS (1<<13)
-#define PPP_SRC_INTERLVD_3COMPONENTS (1<<14)
-#define PPP_SRC_INTERLVD_4COMPONENTS ((1<<14)|(1<<13))
-
-
-/* RGB666 unpack format
-** TIGHT means R6+G6+B6 together
-** LOOSE means R6+2 +G6+2+ B6+2 (with MSB)
-**          or 2+R6 +2+G6 +2+B6 (with LSB)
-*/
-#define PPP_SRC_PACK_TIGHT (1<<17)
-#define PPP_SRC_PACK_LOOSE 0
-#define PPP_SRC_PACK_ALIGN_LSB 0
-#define PPP_SRC_PACK_ALIGN_MSB (1<<18)
-
-#define PPP_SRC_PLANE_INTERLVD 0
-#define PPP_SRC_PLANE_PSEUDOPLNR (1<<20)
-
-#define PPP_SRC_WMV9_MODE (1<<21)
-
-/* MDP_PPP_OPERATION_CONFIG / MDP_FULL_BYPASS_WORD14 */
-#define PPP_OP_SCALE_X_ON (1<<0)
-#define PPP_OP_SCALE_Y_ON (1<<1)
-
-#define PPP_OP_CONVERT_RGB2YCBCR 0
-#define PPP_OP_CONVERT_YCBCR2RGB (1<<2)
-#define PPP_OP_CONVERT_ON (1<<3)
-
-#define PPP_OP_CONVERT_MATRIX_PRIMARY 0
-#define PPP_OP_CONVERT_MATRIX_SECONDARY (1<<4)
-
-#define PPP_OP_LUT_C0_ON (1<<5)
-#define PPP_OP_LUT_C1_ON (1<<6)
-#define PPP_OP_LUT_C2_ON (1<<7)
-
-/* rotate or blend enable */
-#define PPP_OP_ROT_ON (1<<8)
-
-#define PPP_OP_ROT_90 (1<<9)
-#define PPP_OP_FLIP_LR (1<<10)
-#define PPP_OP_FLIP_UD (1<<11)
-
-#define PPP_OP_BLEND_ON (1<<12)
-
-#define PPP_OP_BLEND_SRCPIXEL_ALPHA 0
-#define PPP_OP_BLEND_DSTPIXEL_ALPHA (1<<13)
-#define PPP_OP_BLEND_CONSTANT_ALPHA (1<<14)
-#define PPP_OP_BLEND_SRCPIXEL_TRANSP ((1<<13)|(1<<14))
-
-#define PPP_OP_BLEND_ALPHA_BLEND_NORMAL 0
-#define PPP_OP_BLEND_ALPHA_BLEND_REVERSE (1<<15)
-
-#define PPP_OP_DITHER_EN (1<<16)
-
-#define PPP_OP_COLOR_SPACE_RGB 0
-#define PPP_OP_COLOR_SPACE_YCBCR (1<<17)
-
-#define PPP_OP_SRC_CHROMA_RGB 0
-#define PPP_OP_SRC_CHROMA_H2V1 (1<<18)
-#define PPP_OP_SRC_CHROMA_H1V2 (1<<19)
-#define PPP_OP_SRC_CHROMA_420 ((1<<18)|(1<<19))
-#define PPP_OP_SRC_CHROMA_COSITE 0
-#define PPP_OP_SRC_CHROMA_OFFSITE (1<<20)
-
-#define PPP_OP_DST_CHROMA_RGB 0
-#define PPP_OP_DST_CHROMA_H2V1 (1<<21)
-#define PPP_OP_DST_CHROMA_H1V2 (1<<22)
-#define PPP_OP_DST_CHROMA_420 ((1<<21)|(1<<22))
-#define PPP_OP_DST_CHROMA_COSITE 0
-#define PPP_OP_DST_CHROMA_OFFSITE (1<<23)
-
-#define PPP_BLEND_ALPHA_TRANSP (1<<24)
-
-#define PPP_OP_BG_CHROMA_RGB 0
-#define PPP_OP_BG_CHROMA_H2V1 (1<<25)
-#define PPP_OP_BG_CHROMA_H1V2 (1<<26)
-#define PPP_OP_BG_CHROMA_420 ((1<<25)|(1<<26))
-#define PPP_OP_BG_CHROMA_SITE_COSITE 0
-#define PPP_OP_BG_CHROMA_SITE_OFFSITE (1<<27)
-
-/* MDP_PPP_DESTINATION_CONFIG / MDP_FULL_BYPASS_WORD20 */
-#define PPP_DST_C0G_8BIT ((1<<0)|(1<<1))
-#define PPP_DST_C1B_8BIT ((1<<3)|(1<<2))
-#define PPP_DST_C2R_8BIT ((1<<5)|(1<<4))
-#define PPP_DST_C3A_8BIT ((1<<7)|(1<<6))
-
-#define PPP_DST_C0G_6BIT (1<<1)
-#define PPP_DST_C1B_6BIT (1<<3)
-#define PPP_DST_C2R_6BIT (1<<5)
-
-#define PPP_DST_C0G_5BIT (1<<0)
-#define PPP_DST_C1B_5BIT (1<<2)
-#define PPP_DST_C2R_5BIT (1<<4)
-
-#define PPP_DST_C3A_8BIT ((1<<7)|(1<<6))
-#define PPP_DST_C3ALPHA_EN (1<<8)
-
-#define PPP_DST_INTERLVD_2COMPONENTS (1<<9)
-#define PPP_DST_INTERLVD_3COMPONENTS (1<<10)
-#define PPP_DST_INTERLVD_4COMPONENTS ((1<<10)|(1<<9))
-#define PPP_DST_INTERLVD_6COMPONENTS ((1<<11)|(1<<9))
-
-#define PPP_DST_PACK_LOOSE 0
-#define PPP_DST_PACK_TIGHT (1<<13)
-#define PPP_DST_PACK_ALIGN_LSB 0
-#define PPP_DST_PACK_ALIGN_MSB (1<<14)
-
-#define PPP_DST_OUT_SEL_AXI 0
-#define PPP_DST_OUT_SEL_MDDI (1<<15)
-
-#define PPP_DST_BPP_2BYTES (1<<16)
-#define PPP_DST_BPP_3BYTES (1<<17)
-#define PPP_DST_BPP_4BYTES ((1<<17)|(1<<16))
-
-#define PPP_DST_PLANE_INTERLVD 0
-#define PPP_DST_PLANE_PLANAR (1<<18)
-#define PPP_DST_PLANE_PSEUDOPLNR (1<<19)
-
-#define PPP_DST_TO_TV (1<<20)
-
-#define PPP_DST_MDDI_PRIMARY 0
-#define PPP_DST_MDDI_SECONDARY (1<<21)
-#define PPP_DST_MDDI_EXTERNAL (1<<22)
-
-/* image configurations by image type */
-#define PPP_CFG_MDP_RGB_565(dir)       (PPP_##dir##_C2R_5BIT | \
-					PPP_##dir##_C0G_6BIT | \
-					PPP_##dir##_C1B_5BIT | \
-					PPP_##dir##_BPP_2BYTES | \
-					PPP_##dir##_INTERLVD_3COMPONENTS | \
-					PPP_##dir##_PACK_TIGHT | \
-					PPP_##dir##_PACK_ALIGN_LSB | \
-					PPP_##dir##_PLANE_INTERLVD)
-
-#define PPP_CFG_MDP_RGB_888(dir)       (PPP_##dir##_C2R_8BIT | \
-					PPP_##dir##_C0G_8BIT | \
-					PPP_##dir##_C1B_8BIT | \
-					PPP_##dir##_BPP_3BYTES | \
-					PPP_##dir##_INTERLVD_3COMPONENTS | \
-					PPP_##dir##_PACK_TIGHT | \
-					PPP_##dir##_PACK_ALIGN_LSB | \
-					PPP_##dir##_PLANE_INTERLVD)
-
-#define PPP_CFG_MDP_ARGB_8888(dir)     (PPP_##dir##_C2R_8BIT | \
-					PPP_##dir##_C0G_8BIT | \
-					PPP_##dir##_C1B_8BIT | \
-					PPP_##dir##_C3A_8BIT | \
-					PPP_##dir##_C3ALPHA_EN | \
-					PPP_##dir##_BPP_4BYTES | \
-					PPP_##dir##_INTERLVD_4COMPONENTS | \
-					PPP_##dir##_PACK_TIGHT | \
-					PPP_##dir##_PACK_ALIGN_LSB | \
-					PPP_##dir##_PLANE_INTERLVD)
-
-#define PPP_CFG_MDP_XRGB_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
-#define PPP_CFG_MDP_RGBA_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
-#define PPP_CFG_MDP_BGRA_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
-#define PPP_CFG_MDP_RGBX_8888(dir) PPP_CFG_MDP_ARGB_8888(dir)
-
-#define PPP_CFG_MDP_Y_CBCR_H2V2(dir)   (PPP_##dir##_C2R_8BIT | \
-					PPP_##dir##_C0G_8BIT | \
-					PPP_##dir##_C1B_8BIT | \
-					PPP_##dir##_C3A_8BIT | \
-					PPP_##dir##_BPP_2BYTES | \
-					PPP_##dir##_INTERLVD_2COMPONENTS | \
-					PPP_##dir##_PACK_TIGHT | \
-					PPP_##dir##_PACK_ALIGN_LSB | \
-					PPP_##dir##_PLANE_PSEUDOPLNR)
-
-#define PPP_CFG_MDP_Y_CRCB_H2V2(dir)	PPP_CFG_MDP_Y_CBCR_H2V2(dir)
-
-#define PPP_CFG_MDP_YCRYCB_H2V1(dir)   (PPP_##dir##_C2R_8BIT | \
-					PPP_##dir##_C0G_8BIT | \
-					PPP_##dir##_C1B_8BIT | \
-					PPP_##dir##_C3A_8BIT | \
-					PPP_##dir##_BPP_2BYTES | \
-					PPP_##dir##_INTERLVD_4COMPONENTS | \
-					PPP_##dir##_PACK_TIGHT | \
-					PPP_##dir##_PACK_ALIGN_LSB |\
-					PPP_##dir##_PLANE_INTERLVD)
-
-#define PPP_CFG_MDP_Y_CBCR_H2V1(dir)   (PPP_##dir##_C2R_8BIT | \
-					PPP_##dir##_C0G_8BIT | \
-					PPP_##dir##_C1B_8BIT | \
-					PPP_##dir##_C3A_8BIT | \
-					PPP_##dir##_BPP_2BYTES |   \
-					PPP_##dir##_INTERLVD_2COMPONENTS |  \
-					PPP_##dir##_PACK_TIGHT | \
-					PPP_##dir##_PACK_ALIGN_LSB | \
-					PPP_##dir##_PLANE_PSEUDOPLNR)
-
-#define PPP_CFG_MDP_Y_CRCB_H2V1(dir)	PPP_CFG_MDP_Y_CBCR_H2V1(dir)
-
-#define PPP_PACK_PATTERN_MDP_RGB_565 \
-	MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8)
-#define PPP_PACK_PATTERN_MDP_RGB_888 PPP_PACK_PATTERN_MDP_RGB_565
-#define PPP_PACK_PATTERN_MDP_XRGB_8888 \
-	MDP_GET_PACK_PATTERN(CLR_B, CLR_G, CLR_R, CLR_ALPHA, 8)
-#define PPP_PACK_PATTERN_MDP_ARGB_8888 PPP_PACK_PATTERN_MDP_XRGB_8888
-#define PPP_PACK_PATTERN_MDP_RGBA_8888 \
-	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R, 8)
-#define PPP_PACK_PATTERN_MDP_BGRA_8888 \
-	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B, 8)
-#define PPP_PACK_PATTERN_MDP_RGBX_8888 \
-	MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R, 8)
-#define PPP_PACK_PATTERN_MDP_Y_CBCR_H2V1 \
-	MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8)
-#define PPP_PACK_PATTERN_MDP_Y_CBCR_H2V2 PPP_PACK_PATTERN_MDP_Y_CBCR_H2V1
-#define PPP_PACK_PATTERN_MDP_Y_CRCB_H2V1 \
-	MDP_GET_PACK_PATTERN(0, 0, CLR_CR, CLR_CB, 8)
-#define PPP_PACK_PATTERN_MDP_Y_CRCB_H2V2 PPP_PACK_PATTERN_MDP_Y_CRCB_H2V1
-#define PPP_PACK_PATTERN_MDP_YCRYCB_H2V1 \
-	MDP_GET_PACK_PATTERN(CLR_Y, CLR_R, CLR_Y, CLR_B, 8)
-
-#define PPP_CHROMA_SAMP_MDP_RGB_565(dir) PPP_OP_##dir##_CHROMA_RGB
-#define PPP_CHROMA_SAMP_MDP_RGB_888(dir) PPP_OP_##dir##_CHROMA_RGB
-#define PPP_CHROMA_SAMP_MDP_XRGB_8888(dir) PPP_OP_##dir##_CHROMA_RGB
-#define PPP_CHROMA_SAMP_MDP_ARGB_8888(dir) PPP_OP_##dir##_CHROMA_RGB
-#define PPP_CHROMA_SAMP_MDP_RGBA_8888(dir) PPP_OP_##dir##_CHROMA_RGB
-#define PPP_CHROMA_SAMP_MDP_BGRA_8888(dir) PPP_OP_##dir##_CHROMA_RGB
-#define PPP_CHROMA_SAMP_MDP_RGBX_8888(dir) PPP_OP_##dir##_CHROMA_RGB
-#define PPP_CHROMA_SAMP_MDP_Y_CBCR_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
-#define PPP_CHROMA_SAMP_MDP_Y_CBCR_H2V2(dir) PPP_OP_##dir##_CHROMA_420
-#define PPP_CHROMA_SAMP_MDP_Y_CRCB_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
-#define PPP_CHROMA_SAMP_MDP_Y_CRCB_H2V2(dir) PPP_OP_##dir##_CHROMA_420
-#define PPP_CHROMA_SAMP_MDP_YCRYCB_H2V1(dir) PPP_OP_##dir##_CHROMA_H2V1
-
-/* Helpful array generation macros */
-#define PPP_ARRAY0(name) \
-	[MDP_RGB_565] = PPP_##name##_MDP_RGB_565,\
-	[MDP_RGB_888] = PPP_##name##_MDP_RGB_888,\
-	[MDP_XRGB_8888] = PPP_##name##_MDP_XRGB_8888,\
-	[MDP_ARGB_8888] = PPP_##name##_MDP_ARGB_8888,\
-	[MDP_RGBA_8888] = PPP_##name##_MDP_RGBA_8888,\
-	[MDP_BGRA_8888] = PPP_##name##_MDP_BGRA_8888,\
-	[MDP_RGBX_8888] = PPP_##name##_MDP_RGBX_8888,\
-	[MDP_Y_CBCR_H2V1] = PPP_##name##_MDP_Y_CBCR_H2V1,\
-	[MDP_Y_CBCR_H2V2] = PPP_##name##_MDP_Y_CBCR_H2V2,\
-	[MDP_Y_CRCB_H2V1] = PPP_##name##_MDP_Y_CRCB_H2V1,\
-	[MDP_Y_CRCB_H2V2] = PPP_##name##_MDP_Y_CRCB_H2V2,\
-	[MDP_YCRYCB_H2V1] = PPP_##name##_MDP_YCRYCB_H2V1
-
-#define PPP_ARRAY1(name, dir) \
-	[MDP_RGB_565] = PPP_##name##_MDP_RGB_565(dir),\
-	[MDP_RGB_888] = PPP_##name##_MDP_RGB_888(dir),\
-	[MDP_XRGB_8888] = PPP_##name##_MDP_XRGB_8888(dir),\
-	[MDP_ARGB_8888] = PPP_##name##_MDP_ARGB_8888(dir),\
-	[MDP_RGBA_8888] = PPP_##name##_MDP_RGBA_8888(dir),\
-	[MDP_BGRA_8888] = PPP_##name##_MDP_BGRA_8888(dir),\
-	[MDP_RGBX_8888] = PPP_##name##_MDP_RGBX_8888(dir),\
-	[MDP_Y_CBCR_H2V1] = PPP_##name##_MDP_Y_CBCR_H2V1(dir),\
-	[MDP_Y_CBCR_H2V2] = PPP_##name##_MDP_Y_CBCR_H2V2(dir),\
-	[MDP_Y_CRCB_H2V1] = PPP_##name##_MDP_Y_CRCB_H2V1(dir),\
-	[MDP_Y_CRCB_H2V2] = PPP_##name##_MDP_Y_CRCB_H2V2(dir),\
-	[MDP_YCRYCB_H2V1] = PPP_##name##_MDP_YCRYCB_H2V1(dir)
-
-#define IS_YCRCB(img) ((img == MDP_Y_CRCB_H2V2) | (img == MDP_Y_CBCR_H2V2) | \
-		       (img == MDP_Y_CRCB_H2V1) | (img == MDP_Y_CBCR_H2V1) | \
-		       (img == MDP_YCRYCB_H2V1))
-#define IS_RGB(img) ((img == MDP_RGB_565) | (img == MDP_RGB_888) | \
-		     (img == MDP_ARGB_8888) | (img == MDP_RGBA_8888) | \
-		     (img == MDP_XRGB_8888) | (img == MDP_BGRA_8888) | \
-		     (img == MDP_RGBX_8888))
-#define HAS_ALPHA(img) ((img == MDP_ARGB_8888) | (img == MDP_RGBA_8888) | \
-			(img == MDP_BGRA_8888))
-
-#define IS_PSEUDOPLNR(img) ((img == MDP_Y_CRCB_H2V2) | \
-			    (img == MDP_Y_CBCR_H2V2) | \
-			    (img == MDP_Y_CRCB_H2V1) | \
-			    (img == MDP_Y_CBCR_H2V1))
-
-/* Mappings from addr to purpose */
-#define PPP_ADDR_SRC_ROI		MDP_FULL_BYPASS_WORD2
-#define PPP_ADDR_SRC0			MDP_FULL_BYPASS_WORD3
-#define PPP_ADDR_SRC1			MDP_FULL_BYPASS_WORD4
-#define PPP_ADDR_SRC_YSTRIDE		MDP_FULL_BYPASS_WORD7
-#define PPP_ADDR_SRC_CFG		MDP_FULL_BYPASS_WORD9
-#define PPP_ADDR_SRC_PACK_PATTERN	MDP_FULL_BYPASS_WORD10
-#define PPP_ADDR_OPERATION		MDP_FULL_BYPASS_WORD14
-#define PPP_ADDR_PHASEX_INIT		MDP_FULL_BYPASS_WORD15
-#define PPP_ADDR_PHASEY_INIT		MDP_FULL_BYPASS_WORD16
-#define PPP_ADDR_PHASEX_STEP		MDP_FULL_BYPASS_WORD17
-#define PPP_ADDR_PHASEY_STEP		MDP_FULL_BYPASS_WORD18
-#define PPP_ADDR_ALPHA_TRANSP		MDP_FULL_BYPASS_WORD19
-#define PPP_ADDR_DST_CFG		MDP_FULL_BYPASS_WORD20
-#define PPP_ADDR_DST_PACK_PATTERN	MDP_FULL_BYPASS_WORD21
-#define PPP_ADDR_DST_ROI		MDP_FULL_BYPASS_WORD25
-#define PPP_ADDR_DST0			MDP_FULL_BYPASS_WORD26
-#define PPP_ADDR_DST1			MDP_FULL_BYPASS_WORD27
-#define PPP_ADDR_DST_YSTRIDE		MDP_FULL_BYPASS_WORD30
-#define PPP_ADDR_EDGE			MDP_FULL_BYPASS_WORD46
-#define PPP_ADDR_BG0			MDP_FULL_BYPASS_WORD48
-#define PPP_ADDR_BG1			MDP_FULL_BYPASS_WORD49
-#define PPP_ADDR_BG_YSTRIDE		MDP_FULL_BYPASS_WORD51
-#define PPP_ADDR_BG_CFG			MDP_FULL_BYPASS_WORD53
-#define PPP_ADDR_BG_PACK_PATTERN	MDP_FULL_BYPASS_WORD54
-
-/* MDP_DMA_CONFIG / MDP_FULL_BYPASS_WORD32 */
-#define DMA_DSTC0G_6BITS (1<<1)
-#define DMA_DSTC1B_6BITS (1<<3)
-#define DMA_DSTC2R_6BITS (1<<5)
-#define DMA_DSTC0G_5BITS (1<<0)
-#define DMA_DSTC1B_5BITS (1<<2)
-#define DMA_DSTC2R_5BITS (1<<4)
-
-#define DMA_PACK_TIGHT (1<<6)
-#define DMA_PACK_LOOSE 0
-#define DMA_PACK_ALIGN_LSB 0
-#define DMA_PACK_ALIGN_MSB (1<<7)
-#define DMA_PACK_PATTERN_RGB \
-	(MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 2)<<8)
-
-#define DMA_OUT_SEL_AHB  0
-#define DMA_OUT_SEL_MDDI (1<<14)
-#define DMA_AHBM_LCD_SEL_PRIMARY 0
-#define DMA_AHBM_LCD_SEL_SECONDARY (1<<15)
-#define DMA_IBUF_C3ALPHA_EN (1<<16)
-#define DMA_DITHER_EN (1<<17)
-
-#define DMA_MDDI_DMAOUT_LCD_SEL_PRIMARY 0
-#define DMA_MDDI_DMAOUT_LCD_SEL_SECONDARY (1<<18)
-#define DMA_MDDI_DMAOUT_LCD_SEL_EXTERNAL (1<<19)
-
-#define DMA_IBUF_FORMAT_RGB565 (1<<20)
-#define DMA_IBUF_FORMAT_RGB888_OR_ARGB8888 0
-
-#define DMA_IBUF_NONCONTIGUOUS (1<<21)
-
-/* MDDI REGISTER ? */
-#define MDDI_VDO_PACKET_DESC  0x5666
-#define MDDI_VDO_PACKET_PRIM  0xC3
-#define MDDI_VDO_PACKET_SECD  0xC0
-
-#endif
diff --git a/drivers/video/fbdev/msm/mdp_ppp.c b/drivers/video/fbdev/msm/mdp_ppp.c
deleted file mode 100644
index be6079cdfbb6..000000000000
--- a/drivers/video/fbdev/msm/mdp_ppp.c
+++ /dev/null
@@ -1,731 +0,0 @@
-/* drivers/video/msm/mdp_ppp.c
- *
- * Copyright (C) 2007 QUALCOMM Incorporated
- * Copyright (C) 2007 Google Incorporated
- *
- * 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/fb.h>
-#include <linux/file.h>
-#include <linux/delay.h>
-#include <linux/msm_mdp.h>
-#include <linux/platform_data/video-msm_fb.h>
-
-#include "mdp_hw.h"
-#include "mdp_scale_tables.h"
-
-#define DLOG(x...) do {} while (0)
-
-#define MDP_DOWNSCALE_BLUR (MDP_DOWNSCALE_MAX + 1)
-static int downscale_y_table = MDP_DOWNSCALE_MAX;
-static int downscale_x_table = MDP_DOWNSCALE_MAX;
-
-struct mdp_regs {
-	uint32_t src0;
-	uint32_t src1;
-	uint32_t dst0;
-	uint32_t dst1;
-	uint32_t src_cfg;
-	uint32_t dst_cfg;
-	uint32_t src_pack;
-	uint32_t dst_pack;
-	uint32_t src_rect;
-	uint32_t dst_rect;
-	uint32_t src_ystride;
-	uint32_t dst_ystride;
-	uint32_t op;
-	uint32_t src_bpp;
-	uint32_t dst_bpp;
-	uint32_t edge;
-	uint32_t phasex_init;
-	uint32_t phasey_init;
-	uint32_t phasex_step;
-	uint32_t phasey_step;
-};
-
-static uint32_t pack_pattern[] = {
-	PPP_ARRAY0(PACK_PATTERN)
-};
-
-static uint32_t src_img_cfg[] = {
-	PPP_ARRAY1(CFG, SRC)
-};
-
-static uint32_t dst_img_cfg[] = {
-	PPP_ARRAY1(CFG, DST)
-};
-
-static uint32_t bytes_per_pixel[] = {
-	[MDP_RGB_565] = 2,
-	[MDP_RGB_888] = 3,
-	[MDP_XRGB_8888] = 4,
-	[MDP_ARGB_8888] = 4,
-	[MDP_RGBA_8888] = 4,
-	[MDP_BGRA_8888] = 4,
-	[MDP_RGBX_8888] = 4,
-	[MDP_Y_CBCR_H2V1] = 1,
-	[MDP_Y_CBCR_H2V2] = 1,
-	[MDP_Y_CRCB_H2V1] = 1,
-	[MDP_Y_CRCB_H2V2] = 1,
-	[MDP_YCRYCB_H2V1] = 2
-};
-
-static uint32_t dst_op_chroma[] = {
-	PPP_ARRAY1(CHROMA_SAMP, DST)
-};
-
-static uint32_t src_op_chroma[] = {
-	PPP_ARRAY1(CHROMA_SAMP, SRC)
-};
-
-static uint32_t bg_op_chroma[] = {
-	PPP_ARRAY1(CHROMA_SAMP, BG)
-};
-
-static void rotate_dst_addr_x(struct mdp_blit_req *req, struct mdp_regs *regs)
-{
-	regs->dst0 += (req->dst_rect.w -
-		       min((uint32_t)16, req->dst_rect.w)) * regs->dst_bpp;
-	regs->dst1 += (req->dst_rect.w -
-		       min((uint32_t)16, req->dst_rect.w)) * regs->dst_bpp;
-}
-
-static void rotate_dst_addr_y(struct mdp_blit_req *req, struct mdp_regs *regs)
-{
-	regs->dst0 += (req->dst_rect.h -
-		       min((uint32_t)16, req->dst_rect.h)) *
-		       regs->dst_ystride;
-	regs->dst1 += (req->dst_rect.h -
-		       min((uint32_t)16, req->dst_rect.h)) *
-		       regs->dst_ystride;
-}
-
-static void blit_rotate(struct mdp_blit_req *req,
-			struct mdp_regs *regs)
-{
-	if (req->flags == MDP_ROT_NOP)
-		return;
-
-	regs->op |= PPP_OP_ROT_ON;
-	if ((req->flags & MDP_ROT_90 || req->flags & MDP_FLIP_LR) &&
-	    !(req->flags & MDP_ROT_90 && req->flags & MDP_FLIP_LR))
-		rotate_dst_addr_x(req, regs);
-	if (req->flags & MDP_ROT_90)
-		regs->op |= PPP_OP_ROT_90;
-	if (req->flags & MDP_FLIP_UD) {
-		regs->op |= PPP_OP_FLIP_UD;
-		rotate_dst_addr_y(req, regs);
-	}
-	if (req->flags & MDP_FLIP_LR)
-		regs->op |= PPP_OP_FLIP_LR;
-}
-
-static void blit_convert(struct mdp_blit_req *req, struct mdp_regs *regs)
-{
-	if (req->src.format == req->dst.format)
-		return;
-	if (IS_RGB(req->src.format) && IS_YCRCB(req->dst.format)) {
-		regs->op |= PPP_OP_CONVERT_RGB2YCBCR | PPP_OP_CONVERT_ON;
-	} else if (IS_YCRCB(req->src.format) && IS_RGB(req->dst.format)) {
-		regs->op |= PPP_OP_CONVERT_YCBCR2RGB | PPP_OP_CONVERT_ON;
-		if (req->dst.format == MDP_RGB_565)
-			regs->op |= PPP_OP_CONVERT_MATRIX_SECONDARY;
-	}
-}
-
-#define GET_BIT_RANGE(value, high, low) \
-	(((1 << (high - low + 1)) - 1) & (value >> low))
-static uint32_t transp_convert(struct mdp_blit_req *req)
-{
-	uint32_t transp = 0;
-	if (req->src.format == MDP_RGB_565) {
-		/* pad each value to 8 bits by copying the high bits into the
-		 * low end, convert RGB to RBG by switching low 2 components */
-		transp |= ((GET_BIT_RANGE(req->transp_mask, 15, 11) << 3) |
-			   (GET_BIT_RANGE(req->transp_mask, 15, 13))) << 16;
-
-		transp |= ((GET_BIT_RANGE(req->transp_mask, 4, 0) << 3) |
-			   (GET_BIT_RANGE(req->transp_mask, 4, 2))) << 8;
-
-		transp |= (GET_BIT_RANGE(req->transp_mask, 10, 5) << 2) |
-			  (GET_BIT_RANGE(req->transp_mask, 10, 9));
-	} else {
-		/* convert RGB to RBG */
-		transp |= (GET_BIT_RANGE(req->transp_mask, 15, 8)) |
-			  (GET_BIT_RANGE(req->transp_mask, 23, 16) << 16) |
-			  (GET_BIT_RANGE(req->transp_mask, 7, 0) << 8);
-	}
-	return transp;
-}
-#undef GET_BIT_RANGE
-
-static void blit_blend(struct mdp_blit_req *req, struct mdp_regs *regs)
-{
-	/* TRANSP BLEND */
-	if (req->transp_mask != MDP_TRANSP_NOP) {
-		req->transp_mask = transp_convert(req);
-		if (req->alpha != MDP_ALPHA_NOP) {
-			/* use blended transparancy mode
-			 * pixel = (src == transp) ? dst : blend
-			 * blend is combo of blend_eq_sel and
-			 * blend_alpha_sel */
-			regs->op |= PPP_OP_ROT_ON | PPP_OP_BLEND_ON |
-				PPP_OP_BLEND_ALPHA_BLEND_NORMAL |
-				PPP_OP_BLEND_CONSTANT_ALPHA |
-				PPP_BLEND_ALPHA_TRANSP;
-		} else {
-			/* simple transparancy mode
-			 * pixel = (src == transp) ? dst : src */
-			regs->op |= PPP_OP_ROT_ON | PPP_OP_BLEND_ON |
-				PPP_OP_BLEND_SRCPIXEL_TRANSP;
-		}
-	}
-
-	req->alpha &= 0xff;
-	/* ALPHA BLEND */
-	if (HAS_ALPHA(req->src.format)) {
-		regs->op |= PPP_OP_ROT_ON | PPP_OP_BLEND_ON |
-			PPP_OP_BLEND_SRCPIXEL_ALPHA;
-	} else if (req->alpha < MDP_ALPHA_NOP) {
-		/* just blend by alpha */
-		regs->op |= PPP_OP_ROT_ON | PPP_OP_BLEND_ON |
-			PPP_OP_BLEND_ALPHA_BLEND_NORMAL |
-			PPP_OP_BLEND_CONSTANT_ALPHA;
-	}
-
-	regs->op |= bg_op_chroma[req->dst.format];
-}
-
-#define ONE_HALF	(1LL << 32)
-#define ONE		(1LL << 33)
-#define TWO		(2LL << 33)
-#define THREE		(3LL << 33)
-#define FRAC_MASK (ONE - 1)
-#define INT_MASK (~FRAC_MASK)
-
-static int scale_params(uint32_t dim_in, uint32_t dim_out, uint32_t origin,
-			uint32_t *phase_init, uint32_t *phase_step)
-{
-	/* to improve precicsion calculations are done in U31.33 and converted
-	 * to U3.29 at the end */
-	int64_t k1, k2, k3, k4, tmp;
-	uint64_t n, d, os, os_p, od, od_p, oreq;
-	unsigned rpa = 0;
-	int64_t ip64, delta;
-
-	if (dim_out % 3 == 0)
-		rpa = !(dim_in % (dim_out / 3));
-
-	n = ((uint64_t)dim_out) << 34;
-	d = dim_in;
-	if (!d)
-		return -1;
-	do_div(n, d);
-	k3 = (n + 1) >> 1;
-	if ((k3 >> 4) < (1LL << 27) || (k3 >> 4) > (1LL << 31)) {
-		DLOG("crap bad scale\n");
-		return -1;
-	}
-	n = ((uint64_t)dim_in) << 34;
-	d = (uint64_t)dim_out;
-	if (!d)
-		return -1;
-	do_div(n, d);
-	k1 = (n + 1) >> 1;
-	k2 = (k1 - ONE) >> 1;
-
-	*phase_init = (int)(k2 >> 4);
-	k4 = (k3 - ONE) >> 1;
-
-	if (rpa) {
-		os = ((uint64_t)origin << 33) - ONE_HALF;
-		tmp = (dim_out * os) + ONE_HALF;
-		if (!dim_in)
-			return -1;
-		do_div(tmp, dim_in);
-		od = tmp - ONE_HALF;
-	} else {
-		os = ((uint64_t)origin << 1) - 1;
-		od = (((k3 * os) >> 1) + k4);
-	}
-
-	od_p = od & INT_MASK;
-	if (od_p != od)
-		od_p += ONE;
-
-	if (rpa) {
-		tmp = (dim_in * od_p) + ONE_HALF;
-		if (!dim_in)
-			return -1;
-		do_div(tmp, dim_in);
-		os_p = tmp - ONE_HALF;
-	} else {
-		os_p = ((k1 * (od_p >> 33)) + k2);
-	}
-
-	oreq = (os_p & INT_MASK) - ONE;
-
-	ip64 = os_p - oreq;
-	delta = ((int64_t)(origin) << 33) - oreq;
-	ip64 -= delta;
-	/* limit to valid range before the left shift */
-	delta = (ip64 & (1LL << 63)) ? 4 : -4;
-	delta <<= 33;
-	while (abs((int)(ip64 >> 33)) > 4)
-		ip64 += delta;
-	*phase_init = (int)(ip64 >> 4);
-	*phase_step = (uint32_t)(k1 >> 4);
-	return 0;
-}
-
-static void load_scale_table(const struct mdp_info *mdp,
-			     struct mdp_table_entry *table, int len)
-{
-	int i;
-	for (i = 0; i < len; i++)
-		mdp_writel(mdp, table[i].val, table[i].reg);
-}
-
-enum {
-IMG_LEFT,
-IMG_RIGHT,
-IMG_TOP,
-IMG_BOTTOM,
-};
-
-static void get_edge_info(uint32_t src, uint32_t src_coord, uint32_t dst,
-			  uint32_t *interp1, uint32_t *interp2,
-			  uint32_t *repeat1, uint32_t *repeat2) {
-	if (src > 3 * dst) {
-		*interp1 = 0;
-		*interp2 = src - 1;
-		*repeat1 = 0;
-		*repeat2 = 0;
-	} else if (src == 3 * dst) {
-		*interp1 = 0;
-		*interp2 = src;
-		*repeat1 = 0;
-		*repeat2 = 1;
-	} else if (src > dst && src < 3 * dst) {
-		*interp1 = -1;
-		*interp2 = src;
-		*repeat1 = 1;
-		*repeat2 = 1;
-	} else if (src == dst) {
-		*interp1 = -1;
-		*interp2 = src + 1;
-		*repeat1 = 1;
-		*repeat2 = 2;
-	} else {
-		*interp1 = -2;
-		*interp2 = src + 1;
-		*repeat1 = 2;
-		*repeat2 = 2;
-	}
-	*interp1 += src_coord;
-	*interp2 += src_coord;
-}
-
-static int get_edge_cond(struct mdp_blit_req *req, struct mdp_regs *regs)
-{
-	int32_t luma_interp[4];
-	int32_t luma_repeat[4];
-	int32_t chroma_interp[4];
-	int32_t chroma_bound[4];
-	int32_t chroma_repeat[4];
-	uint32_t dst_w, dst_h;
-
-	memset(&luma_interp, 0, sizeof(int32_t) * 4);
-	memset(&luma_repeat, 0, sizeof(int32_t) * 4);
-	memset(&chroma_interp, 0, sizeof(int32_t) * 4);
-	memset(&chroma_bound, 0, sizeof(int32_t) * 4);
-	memset(&chroma_repeat, 0, sizeof(int32_t) * 4);
-	regs->edge = 0;
-
-	if (req->flags & MDP_ROT_90) {
-		dst_w = req->dst_rect.h;
-		dst_h = req->dst_rect.w;
-	} else {
-		dst_w = req->dst_rect.w;
-		dst_h = req->dst_rect.h;
-	}
-
-	if (regs->op & (PPP_OP_SCALE_Y_ON | PPP_OP_SCALE_X_ON)) {
-		get_edge_info(req->src_rect.h, req->src_rect.y, dst_h,
-			      &luma_interp[IMG_TOP], &luma_interp[IMG_BOTTOM],
-			      &luma_repeat[IMG_TOP], &luma_repeat[IMG_BOTTOM]);
-		get_edge_info(req->src_rect.w, req->src_rect.x, dst_w,
-			      &luma_interp[IMG_LEFT], &luma_interp[IMG_RIGHT],
-			      &luma_repeat[IMG_LEFT], &luma_repeat[IMG_RIGHT]);
-	} else {
-		luma_interp[IMG_LEFT] = req->src_rect.x;
-		luma_interp[IMG_RIGHT] = req->src_rect.x + req->src_rect.w - 1;
-		luma_interp[IMG_TOP] = req->src_rect.y;
-		luma_interp[IMG_BOTTOM] = req->src_rect.y + req->src_rect.h - 1;
-		luma_repeat[IMG_LEFT] = 0;
-		luma_repeat[IMG_TOP] = 0;
-		luma_repeat[IMG_RIGHT] = 0;
-		luma_repeat[IMG_BOTTOM] = 0;
-	}
-
-	chroma_interp[IMG_LEFT] = luma_interp[IMG_LEFT];
-	chroma_interp[IMG_RIGHT] = luma_interp[IMG_RIGHT];
-	chroma_interp[IMG_TOP] = luma_interp[IMG_TOP];
-	chroma_interp[IMG_BOTTOM] = luma_interp[IMG_BOTTOM];
-
-	chroma_bound[IMG_LEFT] = req->src_rect.x;
-	chroma_bound[IMG_RIGHT] = req->src_rect.x + req->src_rect.w - 1;
-	chroma_bound[IMG_TOP] = req->src_rect.y;
-	chroma_bound[IMG_BOTTOM] = req->src_rect.y + req->src_rect.h - 1;
-
-	if (IS_YCRCB(req->src.format)) {
-		chroma_interp[IMG_LEFT] = chroma_interp[IMG_LEFT] >> 1;
-		chroma_interp[IMG_RIGHT] = (chroma_interp[IMG_RIGHT] + 1) >> 1;
-
-		chroma_bound[IMG_LEFT] = chroma_bound[IMG_LEFT] >> 1;
-		chroma_bound[IMG_RIGHT] = chroma_bound[IMG_RIGHT] >> 1;
-	}
-
-	if (req->src.format == MDP_Y_CBCR_H2V2 ||
-	    req->src.format == MDP_Y_CRCB_H2V2) {
-		chroma_interp[IMG_TOP] = (chroma_interp[IMG_TOP] - 1) >> 1;
-		chroma_interp[IMG_BOTTOM] = (chroma_interp[IMG_BOTTOM] + 1)
-					    >> 1;
-		chroma_bound[IMG_TOP] = (chroma_bound[IMG_TOP] + 1) >> 1;
-		chroma_bound[IMG_BOTTOM] = chroma_bound[IMG_BOTTOM] >> 1;
-	}
-
-	chroma_repeat[IMG_LEFT] = chroma_bound[IMG_LEFT] -
-				  chroma_interp[IMG_LEFT];
-	chroma_repeat[IMG_RIGHT] = chroma_interp[IMG_RIGHT] -
-				  chroma_bound[IMG_RIGHT];
-	chroma_repeat[IMG_TOP] = chroma_bound[IMG_TOP] -
-				  chroma_interp[IMG_TOP];
-	chroma_repeat[IMG_BOTTOM] = chroma_interp[IMG_BOTTOM] -
-				  chroma_bound[IMG_BOTTOM];
-
-	if (chroma_repeat[IMG_LEFT] < 0 || chroma_repeat[IMG_LEFT] > 3 ||
-	    chroma_repeat[IMG_RIGHT] < 0 || chroma_repeat[IMG_RIGHT] > 3 ||
-	    chroma_repeat[IMG_TOP] < 0 || chroma_repeat[IMG_TOP] > 3 ||
-	    chroma_repeat[IMG_BOTTOM] < 0 || chroma_repeat[IMG_BOTTOM] > 3 ||
-	    luma_repeat[IMG_LEFT] < 0 || luma_repeat[IMG_LEFT] > 3 ||
-	    luma_repeat[IMG_RIGHT] < 0 || luma_repeat[IMG_RIGHT] > 3 ||
-	    luma_repeat[IMG_TOP] < 0 || luma_repeat[IMG_TOP] > 3 ||
-	    luma_repeat[IMG_BOTTOM] < 0 || luma_repeat[IMG_BOTTOM] > 3)
-		return -1;
-
-	regs->edge |= (chroma_repeat[IMG_LEFT] & 3) << MDP_LEFT_CHROMA;
-	regs->edge |= (chroma_repeat[IMG_RIGHT] & 3) << MDP_RIGHT_CHROMA;
-	regs->edge |= (chroma_repeat[IMG_TOP] & 3) << MDP_TOP_CHROMA;
-	regs->edge |= (chroma_repeat[IMG_BOTTOM] & 3) << MDP_BOTTOM_CHROMA;
-	regs->edge |= (luma_repeat[IMG_LEFT] & 3) << MDP_LEFT_LUMA;
-	regs->edge |= (luma_repeat[IMG_RIGHT] & 3) << MDP_RIGHT_LUMA;
-	regs->edge |= (luma_repeat[IMG_TOP] & 3) << MDP_TOP_LUMA;
-	regs->edge |= (luma_repeat[IMG_BOTTOM] & 3) << MDP_BOTTOM_LUMA;
-	return 0;
-}
-
-static int blit_scale(const struct mdp_info *mdp, struct mdp_blit_req *req,
-		      struct mdp_regs *regs)
-{
-	uint32_t phase_init_x, phase_init_y, phase_step_x, phase_step_y;
-	uint32_t scale_factor_x, scale_factor_y;
-	uint32_t downscale;
-	uint32_t dst_w, dst_h;
-
-	if (req->flags & MDP_ROT_90) {
-		dst_w = req->dst_rect.h;
-		dst_h = req->dst_rect.w;
-	} else {
-		dst_w = req->dst_rect.w;
-		dst_h = req->dst_rect.h;
-	}
-	if ((req->src_rect.w == dst_w)  && (req->src_rect.h == dst_h) &&
-	    !(req->flags & MDP_BLUR)) {
-		regs->phasex_init = 0;
-		regs->phasey_init = 0;
-		regs->phasex_step = 0;
-		regs->phasey_step = 0;
-		return 0;
-	}
-
-	if (scale_params(req->src_rect.w, dst_w, 1, &phase_init_x,
-			 &phase_step_x) ||
-	    scale_params(req->src_rect.h, dst_h, 1, &phase_init_y,
-			 &phase_step_y))
-		return -1;
-
-	scale_factor_x = (dst_w * 10) / req->src_rect.w;
-	scale_factor_y = (dst_h * 10) / req->src_rect.h;
-
-	if (scale_factor_x > 8)
-		downscale = MDP_DOWNSCALE_PT8TO1;
-	else if (scale_factor_x > 6)
-		downscale = MDP_DOWNSCALE_PT6TOPT8;
-	else if (scale_factor_x > 4)
-		downscale = MDP_DOWNSCALE_PT4TOPT6;
-	else
-		downscale = MDP_DOWNSCALE_PT2TOPT4;
-	if (downscale != downscale_x_table) {
-		load_scale_table(mdp, mdp_downscale_x_table[downscale], 64);
-		downscale_x_table = downscale;
-	}
-
-	if (scale_factor_y > 8)
-		downscale = MDP_DOWNSCALE_PT8TO1;
-	else if (scale_factor_y > 6)
-		downscale = MDP_DOWNSCALE_PT6TOPT8;
-	else if (scale_factor_y > 4)
-		downscale = MDP_DOWNSCALE_PT4TOPT6;
-	else
-		downscale = MDP_DOWNSCALE_PT2TOPT4;
-	if (downscale != downscale_y_table) {
-		load_scale_table(mdp, mdp_downscale_y_table[downscale], 64);
-		downscale_y_table = downscale;
-	}
-
-	regs->phasex_init = phase_init_x;
-	regs->phasey_init = phase_init_y;
-	regs->phasex_step = phase_step_x;
-	regs->phasey_step = phase_step_y;
-	regs->op |= (PPP_OP_SCALE_Y_ON | PPP_OP_SCALE_X_ON);
-	return 0;
-
-}
-
-static void blit_blur(const struct mdp_info *mdp, struct mdp_blit_req *req,
-		      struct mdp_regs *regs)
-{
-	if (!(req->flags & MDP_BLUR))
-		return;
-
-	if (!(downscale_x_table == MDP_DOWNSCALE_BLUR &&
-	      downscale_y_table == MDP_DOWNSCALE_BLUR)) {
-		load_scale_table(mdp, mdp_gaussian_blur_table, 128);
-		downscale_x_table = MDP_DOWNSCALE_BLUR;
-		downscale_y_table = MDP_DOWNSCALE_BLUR;
-	}
-
-	regs->op |= (PPP_OP_SCALE_Y_ON | PPP_OP_SCALE_X_ON);
-}
-
-
-#define IMG_LEN(rect_h, w, rect_w, bpp) (((rect_h) * w) * bpp)
-
-#define Y_TO_CRCB_RATIO(format) \
-	((format == MDP_Y_CBCR_H2V2 || format == MDP_Y_CRCB_H2V2) ?  2 :\
-	 (format == MDP_Y_CBCR_H2V1 || format == MDP_Y_CRCB_H2V1) ?  1 : 1)
-
-static void get_len(struct mdp_img *img, struct mdp_rect *rect, uint32_t bpp,
-		    uint32_t *len0, uint32_t *len1)
-{
-	*len0 = IMG_LEN(rect->h, img->width, rect->w, bpp);
-	if (IS_PSEUDOPLNR(img->format))
-		*len1 = *len0/Y_TO_CRCB_RATIO(img->format);
-	else
-		*len1 = 0;
-}
-
-static int valid_src_dst(unsigned long src_start, unsigned long src_len,
-			 unsigned long dst_start, unsigned long dst_len,
-			 struct mdp_blit_req *req, struct mdp_regs *regs)
-{
-	unsigned long src_min_ok = src_start;
-	unsigned long src_max_ok = src_start + src_len;
-	unsigned long dst_min_ok = dst_start;
-	unsigned long dst_max_ok = dst_start + dst_len;
-	uint32_t src0_len, src1_len, dst0_len, dst1_len;
-	get_len(&req->src, &req->src_rect, regs->src_bpp, &src0_len,
-		 &src1_len);
-	get_len(&req->dst, &req->dst_rect, regs->dst_bpp, &dst0_len,
-		 &dst1_len);
-
-	if (regs->src0 < src_min_ok || regs->src0 > src_max_ok ||
-	    regs->src0 + src0_len > src_max_ok) {
-		DLOG("invalid_src %x %x %lx %lx\n", regs->src0,
-		      src0_len, src_min_ok, src_max_ok);
-		return 0;
-	}
-	if (regs->src_cfg & PPP_SRC_PLANE_PSEUDOPLNR) {
-		if (regs->src1 < src_min_ok || regs->src1 > src_max_ok ||
-		    regs->src1 + src1_len > src_max_ok) {
-			DLOG("invalid_src1");
-			return 0;
-		}
-	}
-	if (regs->dst0 < dst_min_ok || regs->dst0 > dst_max_ok ||
-	    regs->dst0 + dst0_len > dst_max_ok) {
-		DLOG("invalid_dst");
-		return 0;
-	}
-	if (regs->dst_cfg & PPP_SRC_PLANE_PSEUDOPLNR) {
-		if (regs->dst1 < dst_min_ok || regs->dst1 > dst_max_ok ||
-		    regs->dst1 + dst1_len > dst_max_ok) {
-			DLOG("invalid_dst1");
-			return 0;
-		}
-	}
-	return 1;
-}
-
-
-static void flush_imgs(struct mdp_blit_req *req, struct mdp_regs *regs,
-		       struct file *src_file, struct file *dst_file)
-{
-}
-
-static void get_chroma_addr(struct mdp_img *img, struct mdp_rect *rect,
-			    uint32_t base, uint32_t bpp, uint32_t cfg,
-			    uint32_t *addr, uint32_t *ystride)
-{
-	uint32_t compress_v = Y_TO_CRCB_RATIO(img->format);
-	uint32_t compress_h = 2;
-	uint32_t  offset;
-
-	if (IS_PSEUDOPLNR(img->format)) {
-		offset = (rect->x / compress_h) * compress_h;
-		offset += rect->y == 0 ? 0 :
-			  ((rect->y + 1) / compress_v) * img->width;
-		*addr = base + (img->width * img->height * bpp);
-		*addr += offset * bpp;
-		*ystride |= *ystride << 16;
-	} else {
-		*addr = 0;
-	}
-}
-
-static int send_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
-		     struct mdp_regs *regs, struct file *src_file,
-		     struct file *dst_file)
-{
-	mdp_writel(mdp, 1, 0x060);
-	mdp_writel(mdp, regs->src_rect, PPP_ADDR_SRC_ROI);
-	mdp_writel(mdp, regs->src0, PPP_ADDR_SRC0);
-	mdp_writel(mdp, regs->src1, PPP_ADDR_SRC1);
-	mdp_writel(mdp, regs->src_ystride, PPP_ADDR_SRC_YSTRIDE);
-	mdp_writel(mdp, regs->src_cfg, PPP_ADDR_SRC_CFG);
-	mdp_writel(mdp, regs->src_pack, PPP_ADDR_SRC_PACK_PATTERN);
-
-	mdp_writel(mdp, regs->op, PPP_ADDR_OPERATION);
-	mdp_writel(mdp, regs->phasex_init, PPP_ADDR_PHASEX_INIT);
-	mdp_writel(mdp, regs->phasey_init, PPP_ADDR_PHASEY_INIT);
-	mdp_writel(mdp, regs->phasex_step, PPP_ADDR_PHASEX_STEP);
-	mdp_writel(mdp, regs->phasey_step, PPP_ADDR_PHASEY_STEP);
-
-	mdp_writel(mdp, (req->alpha << 24) | (req->transp_mask & 0xffffff),
-	       PPP_ADDR_ALPHA_TRANSP);
-
-	mdp_writel(mdp, regs->dst_cfg, PPP_ADDR_DST_CFG);
-	mdp_writel(mdp, regs->dst_pack, PPP_ADDR_DST_PACK_PATTERN);
-	mdp_writel(mdp, regs->dst_rect, PPP_ADDR_DST_ROI);
-	mdp_writel(mdp, regs->dst0, PPP_ADDR_DST0);
-	mdp_writel(mdp, regs->dst1, PPP_ADDR_DST1);
-	mdp_writel(mdp, regs->dst_ystride, PPP_ADDR_DST_YSTRIDE);
-
-	mdp_writel(mdp, regs->edge, PPP_ADDR_EDGE);
-	if (regs->op & PPP_OP_BLEND_ON) {
-		mdp_writel(mdp, regs->dst0, PPP_ADDR_BG0);
-		mdp_writel(mdp, regs->dst1, PPP_ADDR_BG1);
-		mdp_writel(mdp, regs->dst_ystride, PPP_ADDR_BG_YSTRIDE);
-		mdp_writel(mdp, src_img_cfg[req->dst.format], PPP_ADDR_BG_CFG);
-		mdp_writel(mdp, pack_pattern[req->dst.format],
-			   PPP_ADDR_BG_PACK_PATTERN);
-	}
-	flush_imgs(req, regs, src_file, dst_file);
-	mdp_writel(mdp, 0x1000, MDP_DISPLAY0_START);
-	return 0;
-}
-
-int mdp_ppp_blit(const struct mdp_info *mdp, struct mdp_blit_req *req,
-		 struct file *src_file, unsigned long src_start, unsigned long src_len,
-		 struct file *dst_file, unsigned long dst_start, unsigned long dst_len)
-{
-	struct mdp_regs regs = {0};
-
-	if (unlikely(req->src.format >= MDP_IMGTYPE_LIMIT ||
-		     req->dst.format >= MDP_IMGTYPE_LIMIT)) {
-		printk(KERN_ERR "mpd_ppp: img is of wrong format\n");
-		return -EINVAL;
-	}
-
-	if (unlikely(req->src_rect.x > req->src.width ||
-		     req->src_rect.y > req->src.height ||
-		     req->dst_rect.x > req->dst.width ||
-		     req->dst_rect.y > req->dst.height)) {
-		printk(KERN_ERR "mpd_ppp: img rect is outside of img!\n");
-		return -EINVAL;
-	}
-
-	/* set the src image configuration */
-	regs.src_cfg = src_img_cfg[req->src.format];
-	regs.src_cfg |= (req->src_rect.x & 0x1) ? PPP_SRC_BPP_ROI_ODD_X : 0;
-	regs.src_cfg |= (req->src_rect.y & 0x1) ? PPP_SRC_BPP_ROI_ODD_Y : 0;
-	regs.src_rect = (req->src_rect.h << 16) | req->src_rect.w;
-	regs.src_pack = pack_pattern[req->src.format];
-
-	/* set the dest image configuration */
-	regs.dst_cfg = dst_img_cfg[req->dst.format] | PPP_DST_OUT_SEL_AXI;
-	regs.dst_rect = (req->dst_rect.h << 16) | req->dst_rect.w;
-	regs.dst_pack = pack_pattern[req->dst.format];
-
-	/* set src, bpp, start pixel and ystride */
-	regs.src_bpp = bytes_per_pixel[req->src.format];
-	regs.src0 = src_start + req->src.offset;
-	regs.src_ystride = req->src.width * regs.src_bpp;
-	get_chroma_addr(&req->src, &req->src_rect, regs.src0, regs.src_bpp,
-			regs.src_cfg, &regs.src1, &regs.src_ystride);
-	regs.src0 += (req->src_rect.x + (req->src_rect.y * req->src.width)) *
-		      regs.src_bpp;
-
-	/* set dst, bpp, start pixel and ystride */
-	regs.dst_bpp = bytes_per_pixel[req->dst.format];
-	regs.dst0 = dst_start + req->dst.offset;
-	regs.dst_ystride = req->dst.width * regs.dst_bpp;
-	get_chroma_addr(&req->dst, &req->dst_rect, regs.dst0, regs.dst_bpp,
-			regs.dst_cfg, &regs.dst1, &regs.dst_ystride);
-	regs.dst0 += (req->dst_rect.x + (req->dst_rect.y * req->dst.width)) *
-		      regs.dst_bpp;
-
-	if (!valid_src_dst(src_start, src_len, dst_start, dst_len, req,
-			   &regs)) {
-		printk(KERN_ERR "mpd_ppp: final src or dst location is "
-			"invalid, are you trying to make an image too large "
-			"or to place it outside the screen?\n");
-		return -EINVAL;
-	}
-
-	/* set up operation register */
-	regs.op = 0;
-	blit_rotate(req, &regs);
-	blit_convert(req, &regs);
-	if (req->flags & MDP_DITHER)
-		regs.op |= PPP_OP_DITHER_EN;
-	blit_blend(req, &regs);
-	if (blit_scale(mdp, req, &regs)) {
-		printk(KERN_ERR "mpd_ppp: error computing scale for img.\n");
-		return -EINVAL;
-	}
-	blit_blur(mdp, req, &regs);
-	regs.op |= dst_op_chroma[req->dst.format] |
-		   src_op_chroma[req->src.format];
-
-	/* if the image is YCRYCB, the x and w must be even */
-	if (unlikely(req->src.format == MDP_YCRYCB_H2V1)) {
-		req->src_rect.x = req->src_rect.x & (~0x1);
-		req->src_rect.w = req->src_rect.w & (~0x1);
-		req->dst_rect.x = req->dst_rect.x & (~0x1);
-		req->dst_rect.w = req->dst_rect.w & (~0x1);
-	}
-	if (get_edge_cond(req, &regs))
-		return -EINVAL;
-
-	send_blit(mdp, req, &regs, src_file, dst_file);
-	return 0;
-}
diff --git a/drivers/video/fbdev/msm/mdp_scale_tables.c b/drivers/video/fbdev/msm/mdp_scale_tables.c
deleted file mode 100644
index 604783b2e17c..000000000000
--- a/drivers/video/fbdev/msm/mdp_scale_tables.c
+++ /dev/null
@@ -1,766 +0,0 @@
-/* drivers/video/msm_fb/mdp_scale_tables.c
- *
- * Copyright (C) 2007 QUALCOMM Incorporated
- * Copyright (C) 2007 Google Incorporated
- *
- * 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 "mdp_scale_tables.h"
-#include "mdp_hw.h"
-
-struct mdp_table_entry mdp_upscale_table[] = {
-	{ 0x5fffc, 0x0 },
-	{ 0x50200, 0x7fc00000 },
-	{ 0x5fffc, 0xff80000d },
-	{ 0x50204, 0x7ec003f9 },
-	{ 0x5fffc, 0xfec0001c },
-	{ 0x50208, 0x7d4003f3 },
-	{ 0x5fffc, 0xfe40002b },
-	{ 0x5020c, 0x7b8003ed },
-	{ 0x5fffc, 0xfd80003c },
-	{ 0x50210, 0x794003e8 },
-	{ 0x5fffc, 0xfcc0004d },
-	{ 0x50214, 0x76c003e4 },
-	{ 0x5fffc, 0xfc40005f },
-	{ 0x50218, 0x73c003e0 },
-	{ 0x5fffc, 0xfb800071 },
-	{ 0x5021c, 0x708003de },
-	{ 0x5fffc, 0xfac00085 },
-	{ 0x50220, 0x6d0003db },
-	{ 0x5fffc, 0xfa000098 },
-	{ 0x50224, 0x698003d9 },
-	{ 0x5fffc, 0xf98000ac },
-	{ 0x50228, 0x654003d8 },
-	{ 0x5fffc, 0xf8c000c1 },
-	{ 0x5022c, 0x610003d7 },
-	{ 0x5fffc, 0xf84000d5 },
-	{ 0x50230, 0x5c8003d7 },
-	{ 0x5fffc, 0xf7c000e9 },
-	{ 0x50234, 0x580003d7 },
-	{ 0x5fffc, 0xf74000fd },
-	{ 0x50238, 0x534003d8 },
-	{ 0x5fffc, 0xf6c00112 },
-	{ 0x5023c, 0x4e8003d8 },
-	{ 0x5fffc, 0xf6800126 },
-	{ 0x50240, 0x494003da },
-	{ 0x5fffc, 0xf600013a },
-	{ 0x50244, 0x448003db },
-	{ 0x5fffc, 0xf600014d },
-	{ 0x50248, 0x3f4003dd },
-	{ 0x5fffc, 0xf5c00160 },
-	{ 0x5024c, 0x3a4003df },
-	{ 0x5fffc, 0xf5c00172 },
-	{ 0x50250, 0x354003e1 },
-	{ 0x5fffc, 0xf5c00184 },
-	{ 0x50254, 0x304003e3 },
-	{ 0x5fffc, 0xf6000195 },
-	{ 0x50258, 0x2b0003e6 },
-	{ 0x5fffc, 0xf64001a6 },
-	{ 0x5025c, 0x260003e8 },
-	{ 0x5fffc, 0xf6c001b4 },
-	{ 0x50260, 0x214003eb },
-	{ 0x5fffc, 0xf78001c2 },
-	{ 0x50264, 0x1c4003ee },
-	{ 0x5fffc, 0xf80001cf },
-	{ 0x50268, 0x17c003f1 },
-	{ 0x5fffc, 0xf90001db },
-	{ 0x5026c, 0x134003f3 },
-	{ 0x5fffc, 0xfa0001e5 },
-	{ 0x50270, 0xf0003f6 },
-	{ 0x5fffc, 0xfb4001ee },
-	{ 0x50274, 0xac003f9 },
-	{ 0x5fffc, 0xfcc001f5 },
-	{ 0x50278, 0x70003fb },
-	{ 0x5fffc, 0xfe4001fb },
-	{ 0x5027c, 0x34003fe },
-};
-
-static struct mdp_table_entry mdp_downscale_x_table_PT2TOPT4[] = {
-	{ 0x5fffc, 0x740008c },
-	{ 0x50280, 0x33800088 },
-	{ 0x5fffc, 0x800008e },
-	{ 0x50284, 0x33400084 },
-	{ 0x5fffc, 0x8400092 },
-	{ 0x50288, 0x33000080 },
-	{ 0x5fffc, 0x9000094 },
-	{ 0x5028c, 0x3300007b },
-	{ 0x5fffc, 0x9c00098 },
-	{ 0x50290, 0x32400077 },
-	{ 0x5fffc, 0xa40009b },
-	{ 0x50294, 0x32000073 },
-	{ 0x5fffc, 0xb00009d },
-	{ 0x50298,  0x31c0006f },
-	{ 0x5fffc,  0xbc000a0 },
-	{ 0x5029c,  0x3140006b },
-	{ 0x5fffc,  0xc8000a2 },
-	{ 0x502a0,  0x31000067 },
-	{ 0x5fffc,  0xd8000a5 },
-	{ 0x502a4,  0x30800062 },
-	{ 0x5fffc,  0xe4000a8 },
-	{ 0x502a8,  0x2fc0005f },
-	{ 0x5fffc,  0xec000aa },
-	{ 0x502ac,  0x2fc0005b },
-	{ 0x5fffc,  0xf8000ad },
-	{ 0x502b0,  0x2f400057 },
-	{ 0x5fffc,  0x108000b0 },
-	{ 0x502b4,  0x2e400054 },
-	{ 0x5fffc,  0x114000b2 },
-	{ 0x502b8,  0x2e000050 },
-	{ 0x5fffc,  0x124000b4 },
-	{ 0x502bc,  0x2d80004c },
-	{ 0x5fffc,  0x130000b6 },
-	{ 0x502c0,  0x2d000049 },
-	{ 0x5fffc,  0x140000b8 },
-	{ 0x502c4,  0x2c800045 },
-	{ 0x5fffc,  0x150000b9 },
-	{ 0x502c8,  0x2c000042 },
-	{ 0x5fffc,  0x15c000bd },
-	{ 0x502cc,  0x2b40003e },
-	{ 0x5fffc,  0x16c000bf },
-	{ 0x502d0,  0x2a80003b },
-	{ 0x5fffc,  0x17c000bf },
-	{ 0x502d4,  0x2a000039 },
-	{ 0x5fffc,  0x188000c2 },
-	{ 0x502d8,  0x29400036 },
-	{ 0x5fffc,  0x19c000c4 },
-	{ 0x502dc,  0x28800032 },
-	{ 0x5fffc,  0x1ac000c5 },
-	{ 0x502e0,  0x2800002f },
-	{ 0x5fffc,  0x1bc000c7 },
-	{ 0x502e4,  0x2740002c },
-	{ 0x5fffc,  0x1cc000c8 },
-	{ 0x502e8,  0x26c00029 },
-	{ 0x5fffc,  0x1dc000c9 },
-	{ 0x502ec,  0x26000027 },
-	{ 0x5fffc,  0x1ec000cc },
-	{ 0x502f0,  0x25000024 },
-	{ 0x5fffc,  0x200000cc },
-	{ 0x502f4,  0x24800021 },
-	{ 0x5fffc,  0x210000cd },
-	{ 0x502f8,  0x23800020 },
-	{ 0x5fffc,  0x220000ce },
-	{ 0x502fc,  0x2300001d },
-};
-
-static struct mdp_table_entry mdp_downscale_x_table_PT4TOPT6[] = {
-	{ 0x5fffc,  0x740008c },
-	{ 0x50280,  0x33800088 },
-	{ 0x5fffc,  0x800008e },
-	{ 0x50284,  0x33400084 },
-	{ 0x5fffc,  0x8400092 },
-	{ 0x50288,  0x33000080 },
-	{ 0x5fffc,  0x9000094 },
-	{ 0x5028c,  0x3300007b },
-	{ 0x5fffc,  0x9c00098 },
-	{ 0x50290,  0x32400077 },
-	{ 0x5fffc,  0xa40009b },
-	{ 0x50294,  0x32000073 },
-	{ 0x5fffc,  0xb00009d },
-	{ 0x50298,  0x31c0006f },
-	{ 0x5fffc,  0xbc000a0 },
-	{ 0x5029c,  0x3140006b },
-	{ 0x5fffc,  0xc8000a2 },
-	{ 0x502a0,  0x31000067 },
-	{ 0x5fffc,  0xd8000a5 },
-	{ 0x502a4,  0x30800062 },
-	{ 0x5fffc,  0xe4000a8 },
-	{ 0x502a8,  0x2fc0005f },
-	{ 0x5fffc,  0xec000aa },
-	{ 0x502ac,  0x2fc0005b },
-	{ 0x5fffc,  0xf8000ad },
-	{ 0x502b0,  0x2f400057 },
-	{ 0x5fffc,  0x108000b0 },
-	{ 0x502b4,  0x2e400054 },
-	{ 0x5fffc,  0x114000b2 },
-	{ 0x502b8,  0x2e000050 },
-	{ 0x5fffc,  0x124000b4 },
-	{ 0x502bc,  0x2d80004c },
-	{ 0x5fffc,  0x130000b6 },
-	{ 0x502c0,  0x2d000049 },
-	{ 0x5fffc,  0x140000b8 },
-	{ 0x502c4,  0x2c800045 },
-	{ 0x5fffc,  0x150000b9 },
-	{ 0x502c8,  0x2c000042 },
-	{ 0x5fffc,  0x15c000bd },
-	{ 0x502cc,  0x2b40003e },
-	{ 0x5fffc,  0x16c000bf },
-	{ 0x502d0,  0x2a80003b },
-	{ 0x5fffc,  0x17c000bf },
-	{ 0x502d4,  0x2a000039 },
-	{ 0x5fffc,  0x188000c2 },
-	{ 0x502d8,  0x29400036 },
-	{ 0x5fffc,  0x19c000c4 },
-	{ 0x502dc,  0x28800032 },
-	{ 0x5fffc,  0x1ac000c5 },
-	{ 0x502e0,  0x2800002f },
-	{ 0x5fffc,  0x1bc000c7 },
-	{ 0x502e4,  0x2740002c },
-	{ 0x5fffc,  0x1cc000c8 },
-	{ 0x502e8,  0x26c00029 },
-	{ 0x5fffc,  0x1dc000c9 },
-	{ 0x502ec,  0x26000027 },
-	{ 0x5fffc,  0x1ec000cc },
-	{ 0x502f0,  0x25000024 },
-	{ 0x5fffc,  0x200000cc },
-	{ 0x502f4,  0x24800021 },
-	{ 0x5fffc,  0x210000cd },
-	{ 0x502f8,  0x23800020 },
-	{ 0x5fffc,  0x220000ce },
-	{ 0x502fc,  0x2300001d },
-};
-
-static struct mdp_table_entry mdp_downscale_x_table_PT6TOPT8[] = {
-	{ 0x5fffc,  0xfe000070 },
-	{ 0x50280,  0x4bc00068 },
-	{ 0x5fffc,  0xfe000078 },
-	{ 0x50284,  0x4bc00060 },
-	{ 0x5fffc,  0xfe000080 },
-	{ 0x50288,  0x4b800059 },
-	{ 0x5fffc,  0xfe000089 },
-	{ 0x5028c,  0x4b000052 },
-	{ 0x5fffc,  0xfe400091 },
-	{ 0x50290,  0x4a80004b },
-	{ 0x5fffc,  0xfe40009a },
-	{ 0x50294,  0x4a000044 },
-	{ 0x5fffc,  0xfe8000a3 },
-	{ 0x50298,  0x4940003d },
-	{ 0x5fffc,  0xfec000ac },
-	{ 0x5029c,  0x48400037 },
-	{ 0x5fffc,  0xff0000b4 },
-	{ 0x502a0,  0x47800031 },
-	{ 0x5fffc,  0xff8000bd },
-	{ 0x502a4,  0x4640002b },
-	{ 0x5fffc,  0xc5 },
-	{ 0x502a8,  0x45000026 },
-	{ 0x5fffc,  0x8000ce },
-	{ 0x502ac,  0x43800021 },
-	{ 0x5fffc,  0x10000d6 },
-	{ 0x502b0,  0x4240001c },
-	{ 0x5fffc,  0x18000df },
-	{ 0x502b4,  0x40800018 },
-	{ 0x5fffc,  0x24000e6 },
-	{ 0x502b8,  0x3f000014 },
-	{ 0x5fffc,  0x30000ee },
-	{ 0x502bc,  0x3d400010 },
-	{ 0x5fffc,  0x40000f5 },
-	{ 0x502c0,  0x3b80000c },
-	{ 0x5fffc,  0x50000fc },
-	{ 0x502c4,  0x39800009 },
-	{ 0x5fffc,  0x6000102 },
-	{ 0x502c8,  0x37c00006 },
-	{ 0x5fffc,  0x7000109 },
-	{ 0x502cc,  0x35800004 },
-	{ 0x5fffc,  0x840010e },
-	{ 0x502d0,  0x33800002 },
-	{ 0x5fffc,  0x9800114 },
-	{ 0x502d4,  0x31400000 },
-	{ 0x5fffc,  0xac00119 },
-	{ 0x502d8,  0x2f4003fe },
-	{ 0x5fffc,  0xc40011e },
-	{ 0x502dc,  0x2d0003fc },
-	{ 0x5fffc,  0xdc00121 },
-	{ 0x502e0,  0x2b0003fb },
-	{ 0x5fffc,  0xf400125 },
-	{ 0x502e4,  0x28c003fa },
-	{ 0x5fffc,  0x11000128 },
-	{ 0x502e8,  0x268003f9 },
-	{ 0x5fffc,  0x12c0012a },
-	{ 0x502ec,  0x244003f9 },
-	{ 0x5fffc,  0x1480012c },
-	{ 0x502f0,  0x224003f8 },
-	{ 0x5fffc,  0x1640012e },
-	{ 0x502f4,  0x200003f8 },
-	{ 0x5fffc,  0x1800012f },
-	{ 0x502f8,  0x1e0003f8 },
-	{ 0x5fffc,  0x1a00012f },
-	{ 0x502fc,  0x1c0003f8 },
-};
-
-static struct mdp_table_entry mdp_downscale_x_table_PT8TO1[] = {
-	{ 0x5fffc,  0x0 },
-	{ 0x50280,  0x7fc00000 },
-	{ 0x5fffc,  0xff80000d },
-	{ 0x50284,  0x7ec003f9 },
-	{ 0x5fffc,  0xfec0001c },
-	{ 0x50288,  0x7d4003f3 },
-	{ 0x5fffc,  0xfe40002b },
-	{ 0x5028c,  0x7b8003ed },
-	{ 0x5fffc,  0xfd80003c },
-	{ 0x50290,  0x794003e8 },
-	{ 0x5fffc,  0xfcc0004d },
-	{ 0x50294,  0x76c003e4 },
-	{ 0x5fffc,  0xfc40005f },
-	{ 0x50298,  0x73c003e0 },
-	{ 0x5fffc,  0xfb800071 },
-	{ 0x5029c,  0x708003de },
-	{ 0x5fffc,  0xfac00085 },
-	{ 0x502a0,  0x6d0003db },
-	{ 0x5fffc,  0xfa000098 },
-	{ 0x502a4,  0x698003d9 },
-	{ 0x5fffc,  0xf98000ac },
-	{ 0x502a8,  0x654003d8 },
-	{ 0x5fffc,  0xf8c000c1 },
-	{ 0x502ac,  0x610003d7 },
-	{ 0x5fffc,  0xf84000d5 },
-	{ 0x502b0,  0x5c8003d7 },
-	{ 0x5fffc,  0xf7c000e9 },
-	{ 0x502b4,  0x580003d7 },
-	{ 0x5fffc,  0xf74000fd },
-	{ 0x502b8,  0x534003d8 },
-	{ 0x5fffc,  0xf6c00112 },
-	{ 0x502bc,  0x4e8003d8 },
-	{ 0x5fffc,  0xf6800126 },
-	{ 0x502c0,  0x494003da },
-	{ 0x5fffc,  0xf600013a },
-	{ 0x502c4,  0x448003db },
-	{ 0x5fffc,  0xf600014d },
-	{ 0x502c8,  0x3f4003dd },
-	{ 0x5fffc,  0xf5c00160 },
-	{ 0x502cc,  0x3a4003df },
-	{ 0x5fffc,  0xf5c00172 },
-	{ 0x502d0,  0x354003e1 },
-	{ 0x5fffc,  0xf5c00184 },
-	{ 0x502d4,  0x304003e3 },
-	{ 0x5fffc,  0xf6000195 },
-	{ 0x502d8,  0x2b0003e6 },
-	{ 0x5fffc,  0xf64001a6 },
-	{ 0x502dc,  0x260003e8 },
-	{ 0x5fffc,  0xf6c001b4 },
-	{ 0x502e0,  0x214003eb },
-	{ 0x5fffc,  0xf78001c2 },
-	{ 0x502e4,  0x1c4003ee },
-	{ 0x5fffc,  0xf80001cf },
-	{ 0x502e8,  0x17c003f1 },
-	{ 0x5fffc,  0xf90001db },
-	{ 0x502ec,  0x134003f3 },
-	{ 0x5fffc,  0xfa0001e5 },
-	{ 0x502f0,  0xf0003f6 },
-	{ 0x5fffc,  0xfb4001ee },
-	{ 0x502f4,  0xac003f9 },
-	{ 0x5fffc,  0xfcc001f5 },
-	{ 0x502f8,  0x70003fb },
-	{ 0x5fffc,  0xfe4001fb },
-	{ 0x502fc,  0x34003fe },
-};
-
-struct mdp_table_entry *mdp_downscale_x_table[MDP_DOWNSCALE_MAX] = {
-	[MDP_DOWNSCALE_PT2TOPT4] = mdp_downscale_x_table_PT2TOPT4,
-	[MDP_DOWNSCALE_PT4TOPT6] = mdp_downscale_x_table_PT4TOPT6,
-	[MDP_DOWNSCALE_PT6TOPT8] = mdp_downscale_x_table_PT6TOPT8,
-	[MDP_DOWNSCALE_PT8TO1]  = mdp_downscale_x_table_PT8TO1,
-};
-
-static struct mdp_table_entry mdp_downscale_y_table_PT2TOPT4[] = {
-	{ 0x5fffc,  0x740008c },
-	{ 0x50300,  0x33800088 },
-	{ 0x5fffc,  0x800008e },
-	{ 0x50304,  0x33400084 },
-	{ 0x5fffc,  0x8400092 },
-	{ 0x50308,  0x33000080 },
-	{ 0x5fffc,  0x9000094 },
-	{ 0x5030c,  0x3300007b },
-	{ 0x5fffc,  0x9c00098 },
-	{ 0x50310,  0x32400077 },
-	{ 0x5fffc,  0xa40009b },
-	{ 0x50314,  0x32000073 },
-	{ 0x5fffc,  0xb00009d },
-	{ 0x50318,  0x31c0006f },
-	{ 0x5fffc,  0xbc000a0 },
-	{ 0x5031c,  0x3140006b },
-	{ 0x5fffc,  0xc8000a2 },
-	{ 0x50320,  0x31000067 },
-	{ 0x5fffc,  0xd8000a5 },
-	{ 0x50324,  0x30800062 },
-	{ 0x5fffc,  0xe4000a8 },
-	{ 0x50328,  0x2fc0005f },
-	{ 0x5fffc,  0xec000aa },
-	{ 0x5032c,  0x2fc0005b },
-	{ 0x5fffc,  0xf8000ad },
-	{ 0x50330,  0x2f400057 },
-	{ 0x5fffc,  0x108000b0 },
-	{ 0x50334,  0x2e400054 },
-	{ 0x5fffc,  0x114000b2 },
-	{ 0x50338,  0x2e000050 },
-	{ 0x5fffc,  0x124000b4 },
-	{ 0x5033c,  0x2d80004c },
-	{ 0x5fffc,  0x130000b6 },
-	{ 0x50340,  0x2d000049 },
-	{ 0x5fffc,  0x140000b8 },
-	{ 0x50344,  0x2c800045 },
-	{ 0x5fffc,  0x150000b9 },
-	{ 0x50348,  0x2c000042 },
-	{ 0x5fffc,  0x15c000bd },
-	{ 0x5034c,  0x2b40003e },
-	{ 0x5fffc,  0x16c000bf },
-	{ 0x50350,  0x2a80003b },
-	{ 0x5fffc,  0x17c000bf },
-	{ 0x50354,  0x2a000039 },
-	{ 0x5fffc,  0x188000c2 },
-	{ 0x50358,  0x29400036 },
-	{ 0x5fffc,  0x19c000c4 },
-	{ 0x5035c,  0x28800032 },
-	{ 0x5fffc,  0x1ac000c5 },
-	{ 0x50360,  0x2800002f },
-	{ 0x5fffc,  0x1bc000c7 },
-	{ 0x50364,  0x2740002c },
-	{ 0x5fffc,  0x1cc000c8 },
-	{ 0x50368,  0x26c00029 },
-	{ 0x5fffc,  0x1dc000c9 },
-	{ 0x5036c,  0x26000027 },
-	{ 0x5fffc,  0x1ec000cc },
-	{ 0x50370,  0x25000024 },
-	{ 0x5fffc,  0x200000cc },
-	{ 0x50374,  0x24800021 },
-	{ 0x5fffc,  0x210000cd },
-	{ 0x50378,  0x23800020 },
-	{ 0x5fffc,  0x220000ce },
-	{ 0x5037c,  0x2300001d },
-};
-
-static struct mdp_table_entry mdp_downscale_y_table_PT4TOPT6[] = {
-	{ 0x5fffc,  0x740008c },
-	{ 0x50300,  0x33800088 },
-	{ 0x5fffc,  0x800008e },
-	{ 0x50304,  0x33400084 },
-	{ 0x5fffc,  0x8400092 },
-	{ 0x50308,  0x33000080 },
-	{ 0x5fffc,  0x9000094 },
-	{ 0x5030c,  0x3300007b },
-	{ 0x5fffc,  0x9c00098 },
-	{ 0x50310,  0x32400077 },
-	{ 0x5fffc,  0xa40009b },
-	{ 0x50314,  0x32000073 },
-	{ 0x5fffc,  0xb00009d },
-	{ 0x50318,  0x31c0006f },
-	{ 0x5fffc,  0xbc000a0 },
-	{ 0x5031c,  0x3140006b },
-	{ 0x5fffc,  0xc8000a2 },
-	{ 0x50320,  0x31000067 },
-	{ 0x5fffc,  0xd8000a5 },
-	{ 0x50324,  0x30800062 },
-	{ 0x5fffc,  0xe4000a8 },
-	{ 0x50328,  0x2fc0005f },
-	{ 0x5fffc,  0xec000aa },
-	{ 0x5032c,  0x2fc0005b },
-	{ 0x5fffc,  0xf8000ad },
-	{ 0x50330,  0x2f400057 },
-	{ 0x5fffc,  0x108000b0 },
-	{ 0x50334,  0x2e400054 },
-	{ 0x5fffc,  0x114000b2 },
-	{ 0x50338,  0x2e000050 },
-	{ 0x5fffc,  0x124000b4 },
-	{ 0x5033c,  0x2d80004c },
-	{ 0x5fffc,  0x130000b6 },
-	{ 0x50340,  0x2d000049 },
-	{ 0x5fffc,  0x140000b8 },
-	{ 0x50344,  0x2c800045 },
-	{ 0x5fffc,  0x150000b9 },
-	{ 0x50348,  0x2c000042 },
-	{ 0x5fffc,  0x15c000bd },
-	{ 0x5034c,  0x2b40003e },
-	{ 0x5fffc,  0x16c000bf },
-	{ 0x50350,  0x2a80003b },
-	{ 0x5fffc,  0x17c000bf },
-	{ 0x50354,  0x2a000039 },
-	{ 0x5fffc,  0x188000c2 },
-	{ 0x50358,  0x29400036 },
-	{ 0x5fffc,  0x19c000c4 },
-	{ 0x5035c,  0x28800032 },
-	{ 0x5fffc,  0x1ac000c5 },
-	{ 0x50360,  0x2800002f },
-	{ 0x5fffc,  0x1bc000c7 },
-	{ 0x50364,  0x2740002c },
-	{ 0x5fffc,  0x1cc000c8 },
-	{ 0x50368,  0x26c00029 },
-	{ 0x5fffc,  0x1dc000c9 },
-	{ 0x5036c,  0x26000027 },
-	{ 0x5fffc,  0x1ec000cc },
-	{ 0x50370,  0x25000024 },
-	{ 0x5fffc,  0x200000cc },
-	{ 0x50374,  0x24800021 },
-	{ 0x5fffc,  0x210000cd },
-	{ 0x50378,  0x23800020 },
-	{ 0x5fffc,  0x220000ce },
-	{ 0x5037c,  0x2300001d },
-};
-
-static struct mdp_table_entry mdp_downscale_y_table_PT6TOPT8[] = {
-	{ 0x5fffc,  0xfe000070 },
-	{ 0x50300,  0x4bc00068 },
-	{ 0x5fffc,  0xfe000078 },
-	{ 0x50304,  0x4bc00060 },
-	{ 0x5fffc,  0xfe000080 },
-	{ 0x50308,  0x4b800059 },
-	{ 0x5fffc,  0xfe000089 },
-	{ 0x5030c,  0x4b000052 },
-	{ 0x5fffc,  0xfe400091 },
-	{ 0x50310,  0x4a80004b },
-	{ 0x5fffc,  0xfe40009a },
-	{ 0x50314,  0x4a000044 },
-	{ 0x5fffc,  0xfe8000a3 },
-	{ 0x50318,  0x4940003d },
-	{ 0x5fffc,  0xfec000ac },
-	{ 0x5031c,  0x48400037 },
-	{ 0x5fffc,  0xff0000b4 },
-	{ 0x50320,  0x47800031 },
-	{ 0x5fffc,  0xff8000bd },
-	{ 0x50324,  0x4640002b },
-	{ 0x5fffc,  0xc5 },
-	{ 0x50328,  0x45000026 },
-	{ 0x5fffc,  0x8000ce },
-	{ 0x5032c,  0x43800021 },
-	{ 0x5fffc,  0x10000d6 },
-	{ 0x50330,  0x4240001c },
-	{ 0x5fffc,  0x18000df },
-	{ 0x50334,  0x40800018 },
-	{ 0x5fffc,  0x24000e6 },
-	{ 0x50338,  0x3f000014 },
-	{ 0x5fffc,  0x30000ee },
-	{ 0x5033c,  0x3d400010 },
-	{ 0x5fffc,  0x40000f5 },
-	{ 0x50340,  0x3b80000c },
-	{ 0x5fffc,  0x50000fc },
-	{ 0x50344,  0x39800009 },
-	{ 0x5fffc,  0x6000102 },
-	{ 0x50348,  0x37c00006 },
-	{ 0x5fffc,  0x7000109 },
-	{ 0x5034c,  0x35800004 },
-	{ 0x5fffc,  0x840010e },
-	{ 0x50350,  0x33800002 },
-	{ 0x5fffc,  0x9800114 },
-	{ 0x50354,  0x31400000 },
-	{ 0x5fffc,  0xac00119 },
-	{ 0x50358,  0x2f4003fe },
-	{ 0x5fffc,  0xc40011e },
-	{ 0x5035c,  0x2d0003fc },
-	{ 0x5fffc,  0xdc00121 },
-	{ 0x50360,  0x2b0003fb },
-	{ 0x5fffc,  0xf400125 },
-	{ 0x50364,  0x28c003fa },
-	{ 0x5fffc,  0x11000128 },
-	{ 0x50368,  0x268003f9 },
-	{ 0x5fffc,  0x12c0012a },
-	{ 0x5036c,  0x244003f9 },
-	{ 0x5fffc,  0x1480012c },
-	{ 0x50370,  0x224003f8 },
-	{ 0x5fffc,  0x1640012e },
-	{ 0x50374,  0x200003f8 },
-	{ 0x5fffc,  0x1800012f },
-	{ 0x50378,  0x1e0003f8 },
-	{ 0x5fffc,  0x1a00012f },
-	{ 0x5037c,  0x1c0003f8 },
-};
-
-static struct mdp_table_entry mdp_downscale_y_table_PT8TO1[] = {
-	{ 0x5fffc,  0x0 },
-	{ 0x50300,  0x7fc00000 },
-	{ 0x5fffc,  0xff80000d },
-	{ 0x50304,  0x7ec003f9 },
-	{ 0x5fffc,  0xfec0001c },
-	{ 0x50308,  0x7d4003f3 },
-	{ 0x5fffc,  0xfe40002b },
-	{ 0x5030c,  0x7b8003ed },
-	{ 0x5fffc,  0xfd80003c },
-	{ 0x50310,  0x794003e8 },
-	{ 0x5fffc,  0xfcc0004d },
-	{ 0x50314,  0x76c003e4 },
-	{ 0x5fffc,  0xfc40005f },
-	{ 0x50318,  0x73c003e0 },
-	{ 0x5fffc,  0xfb800071 },
-	{ 0x5031c,  0x708003de },
-	{ 0x5fffc,  0xfac00085 },
-	{ 0x50320,  0x6d0003db },
-	{ 0x5fffc,  0xfa000098 },
-	{ 0x50324,  0x698003d9 },
-	{ 0x5fffc,  0xf98000ac },
-	{ 0x50328,  0x654003d8 },
-	{ 0x5fffc,  0xf8c000c1 },
-	{ 0x5032c,  0x610003d7 },
-	{ 0x5fffc,  0xf84000d5 },
-	{ 0x50330,  0x5c8003d7 },
-	{ 0x5fffc,  0xf7c000e9 },
-	{ 0x50334,  0x580003d7 },
-	{ 0x5fffc,  0xf74000fd },
-	{ 0x50338,  0x534003d8 },
-	{ 0x5fffc,  0xf6c00112 },
-	{ 0x5033c,  0x4e8003d8 },
-	{ 0x5fffc,  0xf6800126 },
-	{ 0x50340,  0x494003da },
-	{ 0x5fffc,  0xf600013a },
-	{ 0x50344,  0x448003db },
-	{ 0x5fffc,  0xf600014d },
-	{ 0x50348,  0x3f4003dd },
-	{ 0x5fffc,  0xf5c00160 },
-	{ 0x5034c,  0x3a4003df },
-	{ 0x5fffc,  0xf5c00172 },
-	{ 0x50350,  0x354003e1 },
-	{ 0x5fffc,  0xf5c00184 },
-	{ 0x50354,  0x304003e3 },
-	{ 0x5fffc,  0xf6000195 },
-	{ 0x50358,  0x2b0003e6 },
-	{ 0x5fffc,  0xf64001a6 },
-	{ 0x5035c,  0x260003e8 },
-	{ 0x5fffc,  0xf6c001b4 },
-	{ 0x50360,  0x214003eb },
-	{ 0x5fffc,  0xf78001c2 },
-	{ 0x50364,  0x1c4003ee },
-	{ 0x5fffc,  0xf80001cf },
-	{ 0x50368,  0x17c003f1 },
-	{ 0x5fffc,  0xf90001db },
-	{ 0x5036c,  0x134003f3 },
-	{ 0x5fffc,  0xfa0001e5 },
-	{ 0x50370,  0xf0003f6 },
-	{ 0x5fffc,  0xfb4001ee },
-	{ 0x50374,  0xac003f9 },
-	{ 0x5fffc,  0xfcc001f5 },
-	{ 0x50378,  0x70003fb },
-	{ 0x5fffc,  0xfe4001fb },
-	{ 0x5037c,  0x34003fe },
-};
-
-struct mdp_table_entry *mdp_downscale_y_table[MDP_DOWNSCALE_MAX] = {
-	[MDP_DOWNSCALE_PT2TOPT4] = mdp_downscale_y_table_PT2TOPT4,
-	[MDP_DOWNSCALE_PT4TOPT6] = mdp_downscale_y_table_PT4TOPT6,
-	[MDP_DOWNSCALE_PT6TOPT8] = mdp_downscale_y_table_PT6TOPT8,
-	[MDP_DOWNSCALE_PT8TO1]  = mdp_downscale_y_table_PT8TO1,
-};
-
-struct mdp_table_entry mdp_gaussian_blur_table[] = {
-	/* max variance */
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50280, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50284, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50288, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5028c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50290, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50294, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50298, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5029c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502a0, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502a4, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502a8, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502ac, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502b0, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502b4, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502b8, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502bc, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502c0, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502c4, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502c8, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502cc, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502d0, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502d4, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502d8, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502dc, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502e0, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502e4, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502e8, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502ec, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502f0, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502f4, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502f8, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x502fc, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50300, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50304, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50308, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5030c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50310, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50314, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50318, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5031c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50320, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50324, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50328, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5032c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50330, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50334, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50338, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5033c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50340, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50344, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50348, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5034c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50350, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50354, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50358, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5035c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50360, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50364, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50368, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5036c, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50370, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50374, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x50378, 0x20000080 },
-	{ 0x5fffc, 0x20000080 },
-	{ 0x5037c, 0x20000080 },
-};
diff --git a/drivers/video/fbdev/msm/mdp_scale_tables.h b/drivers/video/fbdev/msm/mdp_scale_tables.h
deleted file mode 100644
index 34077b1af603..000000000000
--- a/drivers/video/fbdev/msm/mdp_scale_tables.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/* drivers/video/msm_fb/mdp_scale_tables.h
- *
- * Copyright (C) 2007 QUALCOMM Incorporated
- * Copyright (C) 2007 Google Incorporated
- *
- * 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 _MDP_SCALE_TABLES_H_
-#define _MDP_SCALE_TABLES_H_
-
-#include <linux/types.h>
-struct mdp_table_entry {
-	uint32_t reg;
-	uint32_t val;
-};
-
-extern struct mdp_table_entry mdp_upscale_table[64];
-
-enum {
-	MDP_DOWNSCALE_PT2TOPT4,
-	MDP_DOWNSCALE_PT4TOPT6,
-	MDP_DOWNSCALE_PT6TOPT8,
-	MDP_DOWNSCALE_PT8TO1,
-	MDP_DOWNSCALE_MAX,
-};
-
-extern struct mdp_table_entry *mdp_downscale_x_table[MDP_DOWNSCALE_MAX];
-extern struct mdp_table_entry *mdp_downscale_y_table[MDP_DOWNSCALE_MAX];
-extern struct mdp_table_entry mdp_gaussian_blur_table[];
-
-#endif
diff --git a/drivers/video/fbdev/msm/msm_fb.c b/drivers/video/fbdev/msm/msm_fb.c
deleted file mode 100644
index 2979d7e72126..000000000000
--- a/drivers/video/fbdev/msm/msm_fb.c
+++ /dev/null
@@ -1,659 +0,0 @@
-/* drivers/video/msm/msm_fb.c
- *
- * Core MSM framebuffer driver.
- *
- * Copyright (C) 2007 Google Incorporated
- *
- * 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/platform_device.h>
-#include <linux/module.h>
-#include <linux/fb.h>
-#include <linux/slab.h>
-#include <linux/delay.h>
-
-#include <linux/freezer.h>
-#include <linux/wait.h>
-#include <linux/msm_mdp.h>
-#include <linux/io.h>
-#include <linux/uaccess.h>
-#include <linux/platform_data/video-msm_fb.h>
-#include <linux/workqueue.h>
-#include <linux/clk.h>
-#include <linux/debugfs.h>
-#include <linux/dma-mapping.h>
-
-#define PRINT_FPS 0
-#define PRINT_BLIT_TIME 0
-
-#define SLEEPING 0x4
-#define UPDATING 0x3
-#define FULL_UPDATE_DONE 0x2
-#define WAKING 0x1
-#define AWAKE 0x0
-
-#define NONE 0
-#define SUSPEND_RESUME 0x1
-#define FPS 0x2
-#define BLIT_TIME 0x4
-#define SHOW_UPDATES 0x8
-
-#define DLOG(mask, fmt, args...) \
-do { \
-	if (msmfb_debug_mask & mask) \
-		printk(KERN_INFO "msmfb: "fmt, ##args); \
-} while (0)
-
-static int msmfb_debug_mask;
-module_param_named(msmfb_debug_mask, msmfb_debug_mask, int,
-		   S_IRUGO | S_IWUSR | S_IWGRP);
-
-struct mdp_device *mdp;
-
-struct msmfb_info {
-	struct fb_info *fb;
-	struct msm_panel_data *panel;
-	int xres;
-	int yres;
-	unsigned output_format;
-	unsigned yoffset;
-	unsigned frame_requested;
-	unsigned frame_done;
-	int sleeping;
-	unsigned update_frame;
-	struct {
-		int left;
-		int top;
-		int eright; /* exclusive */
-		int ebottom; /* exclusive */
-	} update_info;
-	char *black;
-
-	spinlock_t update_lock;
-	struct mutex panel_init_lock;
-	wait_queue_head_t frame_wq;
-	struct work_struct resume_work;
-	struct msmfb_callback dma_callback;
-	struct msmfb_callback vsync_callback;
-	struct hrtimer fake_vsync;
-	ktime_t vsync_request_time;
-};
-
-static int msmfb_open(struct fb_info *info, int user)
-{
-	return 0;
-}
-
-static int msmfb_release(struct fb_info *info, int user)
-{
-	return 0;
-}
-
-/* Called from dma interrupt handler, must not sleep */
-static void msmfb_handle_dma_interrupt(struct msmfb_callback *callback)
-{
-	unsigned long irq_flags;
-	struct msmfb_info *msmfb  = container_of(callback, struct msmfb_info,
-					       dma_callback);
-
-	spin_lock_irqsave(&msmfb->update_lock, irq_flags);
-	msmfb->frame_done = msmfb->frame_requested;
-	if (msmfb->sleeping == UPDATING &&
-	    msmfb->frame_done == msmfb->update_frame) {
-		DLOG(SUSPEND_RESUME, "full update completed\n");
-		schedule_work(&msmfb->resume_work);
-	}
-	spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-	wake_up(&msmfb->frame_wq);
-}
-
-static int msmfb_start_dma(struct msmfb_info *msmfb)
-{
-	uint32_t x, y, w, h;
-	unsigned addr;
-	unsigned long irq_flags;
-	uint32_t yoffset;
-	s64 time_since_request;
-	struct msm_panel_data *panel = msmfb->panel;
-
-	spin_lock_irqsave(&msmfb->update_lock, irq_flags);
-	time_since_request = ktime_to_ns(ktime_sub(ktime_get(),
-			     msmfb->vsync_request_time));
-	if (time_since_request > 20 * NSEC_PER_MSEC) {
-		uint32_t us;
-		us = do_div(time_since_request, NSEC_PER_MSEC) / NSEC_PER_USEC;
-		printk(KERN_WARNING "msmfb_start_dma %lld.%03u ms after vsync "
-			"request\n", time_since_request, us);
-	}
-	if (msmfb->frame_done == msmfb->frame_requested) {
-		spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-		return -1;
-	}
-	if (msmfb->sleeping == SLEEPING) {
-		DLOG(SUSPEND_RESUME, "tried to start dma while asleep\n");
-		spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-		return -1;
-	}
-	x = msmfb->update_info.left;
-	y = msmfb->update_info.top;
-	w = msmfb->update_info.eright - x;
-	h = msmfb->update_info.ebottom - y;
-	yoffset = msmfb->yoffset;
-	msmfb->update_info.left = msmfb->xres + 1;
-	msmfb->update_info.top = msmfb->yres + 1;
-	msmfb->update_info.eright = 0;
-	msmfb->update_info.ebottom = 0;
-	if (unlikely(w > msmfb->xres || h > msmfb->yres ||
-		     w == 0 || h == 0)) {
-		printk(KERN_INFO "invalid update: %d %d %d "
-				"%d\n", x, y, w, h);
-		msmfb->frame_done = msmfb->frame_requested;
-		goto error;
-	}
-	spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-
-	addr = ((msmfb->xres * (yoffset + y) + x) * 2);
-	mdp->dma(mdp, addr + msmfb->fb->fix.smem_start,
-		 msmfb->xres * 2, w, h, x, y, &msmfb->dma_callback,
-		 panel->interface_type);
-	return 0;
-error:
-	spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-	/* some clients need to clear their vsync interrupt */
-	if (panel->clear_vsync)
-		panel->clear_vsync(panel);
-	wake_up(&msmfb->frame_wq);
-	return 0;
-}
-
-/* Called from esync interrupt handler, must not sleep */
-static void msmfb_handle_vsync_interrupt(struct msmfb_callback *callback)
-{
-	struct msmfb_info *msmfb = container_of(callback, struct msmfb_info,
-					       vsync_callback);
-	msmfb_start_dma(msmfb);
-}
-
-static enum hrtimer_restart msmfb_fake_vsync(struct hrtimer *timer)
-{
-	struct msmfb_info *msmfb  = container_of(timer, struct msmfb_info,
-					       fake_vsync);
-	msmfb_start_dma(msmfb);
-	return HRTIMER_NORESTART;
-}
-
-static void msmfb_pan_update(struct fb_info *info, uint32_t left, uint32_t top,
-			     uint32_t eright, uint32_t ebottom,
-			     uint32_t yoffset, int pan_display)
-{
-	struct msmfb_info *msmfb = info->par;
-	struct msm_panel_data *panel = msmfb->panel;
-	unsigned long irq_flags;
-	int sleeping;
-	int retry = 1;
-
-	DLOG(SHOW_UPDATES, "update %d %d %d %d %d %d\n",
-		left, top, eright, ebottom, yoffset, pan_display);
-restart:
-	spin_lock_irqsave(&msmfb->update_lock, irq_flags);
-
-	/* if we are sleeping, on a pan_display wait 10ms (to throttle back
-	 * drawing otherwise return */
-	if (msmfb->sleeping == SLEEPING) {
-		DLOG(SUSPEND_RESUME, "drawing while asleep\n");
-		spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-		if (pan_display)
-			wait_event_interruptible_timeout(msmfb->frame_wq,
-				msmfb->sleeping != SLEEPING, HZ/10);
-		return;
-	}
-
-	sleeping = msmfb->sleeping;
-	/* on a full update, if the last frame has not completed, wait for it */
-	if ((pan_display && msmfb->frame_requested != msmfb->frame_done) ||
-			    sleeping == UPDATING) {
-		int ret;
-		spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-		ret = wait_event_interruptible_timeout(msmfb->frame_wq,
-			msmfb->frame_done == msmfb->frame_requested &&
-			msmfb->sleeping != UPDATING, 5 * HZ);
-		if (ret <= 0 && (msmfb->frame_requested != msmfb->frame_done ||
-				 msmfb->sleeping == UPDATING)) {
-			if (retry && panel->request_vsync &&
-			    (sleeping == AWAKE)) {
-				panel->request_vsync(panel,
-					&msmfb->vsync_callback);
-				retry = 0;
-				printk(KERN_WARNING "msmfb_pan_display timeout "
-					"rerequest vsync\n");
-			} else {
-				printk(KERN_WARNING "msmfb_pan_display timeout "
-					"waiting for frame start, %d %d\n",
-					msmfb->frame_requested,
-					msmfb->frame_done);
-				return;
-			}
-		}
-		goto restart;
-	}
-
-
-	msmfb->frame_requested++;
-	/* if necessary, update the y offset, if this is the
-	 * first full update on resume, set the sleeping state */
-	if (pan_display) {
-		msmfb->yoffset = yoffset;
-		if (left == 0 && top == 0 && eright == info->var.xres &&
-		    ebottom == info->var.yres) {
-			if (sleeping == WAKING) {
-				msmfb->update_frame = msmfb->frame_requested;
-				DLOG(SUSPEND_RESUME, "full update starting\n");
-				msmfb->sleeping = UPDATING;
-			}
-		}
-	}
-
-	/* set the update request */
-	if (left < msmfb->update_info.left)
-		msmfb->update_info.left = left;
-	if (top < msmfb->update_info.top)
-		msmfb->update_info.top = top;
-	if (eright > msmfb->update_info.eright)
-		msmfb->update_info.eright = eright;
-	if (ebottom > msmfb->update_info.ebottom)
-		msmfb->update_info.ebottom = ebottom;
-	DLOG(SHOW_UPDATES, "update queued %d %d %d %d %d\n",
-		msmfb->update_info.left, msmfb->update_info.top,
-		msmfb->update_info.eright, msmfb->update_info.ebottom,
-		msmfb->yoffset);
-	spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-
-	/* if the panel is all the way on wait for vsync, otherwise sleep
-	 * for 16 ms (long enough for the dma to panel) and then begin dma */
-	msmfb->vsync_request_time = ktime_get();
-	if (panel->request_vsync && (sleeping == AWAKE)) {
-		panel->request_vsync(panel, &msmfb->vsync_callback);
-	} else {
-		if (!hrtimer_active(&msmfb->fake_vsync)) {
-			hrtimer_start(&msmfb->fake_vsync,
-				      ktime_set(0, NSEC_PER_SEC/60),
-				      HRTIMER_MODE_REL);
-		}
-	}
-}
-
-static void msmfb_update(struct fb_info *info, uint32_t left, uint32_t top,
-			 uint32_t eright, uint32_t ebottom)
-{
-	msmfb_pan_update(info, left, top, eright, ebottom, 0, 0);
-}
-
-static void power_on_panel(struct work_struct *work)
-{
-	struct msmfb_info *msmfb =
-		container_of(work, struct msmfb_info, resume_work);
-	struct msm_panel_data *panel = msmfb->panel;
-	unsigned long irq_flags;
-
-	mutex_lock(&msmfb->panel_init_lock);
-	DLOG(SUSPEND_RESUME, "turning on panel\n");
-	if (msmfb->sleeping == UPDATING) {
-		if (panel->unblank(panel)) {
-			printk(KERN_INFO "msmfb: panel unblank failed,"
-			       "not starting drawing\n");
-			goto error;
-		}
-		spin_lock_irqsave(&msmfb->update_lock, irq_flags);
-		msmfb->sleeping = AWAKE;
-		wake_up(&msmfb->frame_wq);
-		spin_unlock_irqrestore(&msmfb->update_lock, irq_flags);
-	}
-error:
-	mutex_unlock(&msmfb->panel_init_lock);
-}
-
-
-static int msmfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
-{
-	if ((var->xres != info->var.xres) ||
-	    (var->yres != info->var.yres) ||
-	    (var->xres_virtual != info->var.xres_virtual) ||
-	    (var->yres_virtual != info->var.yres_virtual) ||
-	    (var->xoffset != info->var.xoffset) ||
-	    (var->bits_per_pixel != info->var.bits_per_pixel) ||
-	    (var->grayscale != info->var.grayscale))
-		 return -EINVAL;
-	return 0;
-}
-
-int msmfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
-{
-	struct msmfb_info *msmfb = info->par;
-	struct msm_panel_data *panel = msmfb->panel;
-
-	/* "UPDT" */
-	if ((panel->caps & MSMFB_CAP_PARTIAL_UPDATES) &&
-	    (var->reserved[0] == 0x54445055)) {
-		msmfb_pan_update(info, var->reserved[1] & 0xffff,
-				 var->reserved[1] >> 16,
-				 var->reserved[2] & 0xffff,
-				 var->reserved[2] >> 16, var->yoffset, 1);
-	} else {
-		msmfb_pan_update(info, 0, 0, info->var.xres, info->var.yres,
-				 var->yoffset, 1);
-	}
-	return 0;
-}
-
-static void msmfb_fillrect(struct fb_info *p, const struct fb_fillrect *rect)
-{
-	cfb_fillrect(p, rect);
-	msmfb_update(p, rect->dx, rect->dy, rect->dx + rect->width,
-		     rect->dy + rect->height);
-}
-
-static void msmfb_copyarea(struct fb_info *p, const struct fb_copyarea *area)
-{
-	cfb_copyarea(p, area);
-	msmfb_update(p, area->dx, area->dy, area->dx + area->width,
-		     area->dy + area->height);
-}
-
-static void msmfb_imageblit(struct fb_info *p, const struct fb_image *image)
-{
-	cfb_imageblit(p, image);
-	msmfb_update(p, image->dx, image->dy, image->dx + image->width,
-		     image->dy + image->height);
-}
-
-
-static int msmfb_blit(struct fb_info *info,
-		      void __user *p)
-{
-	struct mdp_blit_req req;
-	struct mdp_blit_req_list req_list;
-	int i;
-	int ret;
-
-	if (copy_from_user(&req_list, p, sizeof(req_list)))
-		return -EFAULT;
-
-	for (i = 0; i < req_list.count; i++) {
-		struct mdp_blit_req_list *list =
-			(struct mdp_blit_req_list *)p;
-		if (copy_from_user(&req, &list->req[i], sizeof(req)))
-			return -EFAULT;
-		ret = mdp->blit(mdp, info, &req);
-		if (ret)
-			return ret;
-	}
-	return 0;
-}
-
-
-DEFINE_MUTEX(mdp_ppp_lock);
-
-static int msmfb_ioctl(struct fb_info *p, unsigned int cmd, unsigned long arg)
-{
-	void __user *argp = (void __user *)arg;
-	int ret;
-
-	switch (cmd) {
-	case MSMFB_GRP_DISP:
-		mdp->set_grp_disp(mdp, arg);
-		break;
-	case MSMFB_BLIT:
-		ret = msmfb_blit(p, argp);
-		if (ret)
-			return ret;
-		break;
-	default:
-			printk(KERN_INFO "msmfb unknown ioctl: %d\n", cmd);
-			return -EINVAL;
-	}
-	return 0;
-}
-
-static struct fb_ops msmfb_ops = {
-	.owner = THIS_MODULE,
-	.fb_open = msmfb_open,
-	.fb_release = msmfb_release,
-	.fb_check_var = msmfb_check_var,
-	.fb_pan_display = msmfb_pan_display,
-	.fb_fillrect = msmfb_fillrect,
-	.fb_copyarea = msmfb_copyarea,
-	.fb_imageblit = msmfb_imageblit,
-	.fb_ioctl = msmfb_ioctl,
-};
-
-static unsigned PP[16];
-
-
-
-#define BITS_PER_PIXEL 16
-
-static void setup_fb_info(struct msmfb_info *msmfb)
-{
-	struct fb_info *fb_info = msmfb->fb;
-	int r;
-
-	/* finish setting up the fb_info struct */
-	strncpy(fb_info->fix.id, "msmfb", 16);
-	fb_info->fix.ypanstep = 1;
-
-	fb_info->fbops = &msmfb_ops;
-	fb_info->flags = FBINFO_DEFAULT;
-
-	fb_info->fix.type = FB_TYPE_PACKED_PIXELS;
-	fb_info->fix.visual = FB_VISUAL_TRUECOLOR;
-	fb_info->fix.line_length = msmfb->xres * 2;
-
-	fb_info->var.xres = msmfb->xres;
-	fb_info->var.yres = msmfb->yres;
-	fb_info->var.width = msmfb->panel->fb_data->width;
-	fb_info->var.height = msmfb->panel->fb_data->height;
-	fb_info->var.xres_virtual = msmfb->xres;
-	fb_info->var.yres_virtual = msmfb->yres * 2;
-	fb_info->var.bits_per_pixel = BITS_PER_PIXEL;
-	fb_info->var.accel_flags = 0;
-
-	fb_info->var.yoffset = 0;
-
-	if (msmfb->panel->caps & MSMFB_CAP_PARTIAL_UPDATES) {
-		/*
-		 * Set the param in the fixed screen, so userspace can't
-		 * change it. This will be used to check for the
-		 * capability.
-		 */
-		fb_info->fix.reserved[0] = 0x5444;
-		fb_info->fix.reserved[1] = 0x5055;
-
-		/*
-		 * This preloads the value so that if userspace doesn't
-		 * change it, it will be a full update
-		 */
-		fb_info->var.reserved[0] = 0x54445055;
-		fb_info->var.reserved[1] = 0;
-		fb_info->var.reserved[2] = (uint16_t)msmfb->xres |
-					   ((uint32_t)msmfb->yres << 16);
-	}
-
-	fb_info->var.red.offset = 11;
-	fb_info->var.red.length = 5;
-	fb_info->var.red.msb_right = 0;
-	fb_info->var.green.offset = 5;
-	fb_info->var.green.length = 6;
-	fb_info->var.green.msb_right = 0;
-	fb_info->var.blue.offset = 0;
-	fb_info->var.blue.length = 5;
-	fb_info->var.blue.msb_right = 0;
-
-	r = fb_alloc_cmap(&fb_info->cmap, 16, 0);
-	fb_info->pseudo_palette = PP;
-
-	PP[0] = 0;
-	for (r = 1; r < 16; r++)
-		PP[r] = 0xffffffff;
-}
-
-static int setup_fbmem(struct msmfb_info *msmfb, struct platform_device *pdev)
-{
-	struct fb_info *fb = msmfb->fb;
-	struct resource *resource;
-	unsigned long size = msmfb->xres * msmfb->yres *
-			     (BITS_PER_PIXEL >> 3) * 2;
-	unsigned char *fbram;
-
-	/* board file might have attached a resource describing an fb */
-	resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!resource)
-		return -EINVAL;
-
-	/* check the resource is large enough to fit the fb */
-	if (resource->end - resource->start < size) {
-		printk(KERN_ERR "allocated resource is too small for "
-				"fb\n");
-		return -ENOMEM;
-	}
-	fb->fix.smem_start = resource->start;
-	fb->fix.smem_len = resource_size(resource);
-	fbram = ioremap(resource->start, resource_size(resource));
-	if (fbram == NULL) {
-		printk(KERN_ERR "msmfb: cannot allocate fbram!\n");
-		return -ENOMEM;
-	}
-	fb->screen_base = fbram;
-	return 0;
-}
-
-static int msmfb_probe(struct platform_device *pdev)
-{
-	struct fb_info *fb;
-	struct msmfb_info *msmfb;
-	struct msm_panel_data *panel = pdev->dev.platform_data;
-	int ret;
-
-	if (!panel) {
-		pr_err("msmfb_probe: no platform data\n");
-		return -EINVAL;
-	}
-	if (!panel->fb_data) {
-		pr_err("msmfb_probe: no fb_data\n");
-		return -EINVAL;
-	}
-
-	fb = framebuffer_alloc(sizeof(struct msmfb_info), &pdev->dev);
-	if (!fb)
-		return -ENOMEM;
-	msmfb = fb->par;
-	msmfb->fb = fb;
-	msmfb->panel = panel;
-	msmfb->xres = panel->fb_data->xres;
-	msmfb->yres = panel->fb_data->yres;
-
-	ret = setup_fbmem(msmfb, pdev);
-	if (ret)
-		goto error_setup_fbmem;
-
-	setup_fb_info(msmfb);
-
-	spin_lock_init(&msmfb->update_lock);
-	mutex_init(&msmfb->panel_init_lock);
-	init_waitqueue_head(&msmfb->frame_wq);
-	INIT_WORK(&msmfb->resume_work, power_on_panel);
-	msmfb->black = devm_kzalloc(&pdev->dev,
-				    msmfb->fb->var.bits_per_pixel*msmfb->xres,
-				    GFP_KERNEL);
-	if (!msmfb->black) {
-		ret = -ENOMEM;
-		goto error_register_framebuffer;
-	}
-
-	printk(KERN_INFO "msmfb_probe() installing %d x %d panel\n",
-	       msmfb->xres, msmfb->yres);
-
-	msmfb->dma_callback.func = msmfb_handle_dma_interrupt;
-	msmfb->vsync_callback.func = msmfb_handle_vsync_interrupt;
-	hrtimer_init(&msmfb->fake_vsync, CLOCK_MONOTONIC,
-		     HRTIMER_MODE_REL);
-
-
-	msmfb->fake_vsync.function = msmfb_fake_vsync;
-
-	ret = register_framebuffer(fb);
-	if (ret)
-		goto error_register_framebuffer;
-
-	msmfb->sleeping = WAKING;
-
-	platform_set_drvdata(pdev, msmfb);
-
-	return 0;
-
-error_register_framebuffer:
-	iounmap(fb->screen_base);
-error_setup_fbmem:
-	framebuffer_release(msmfb->fb);
-	return ret;
-}
-
-static int msmfb_remove(struct platform_device *pdev)
-{
-	struct msmfb_info *msmfb;
-
-	msmfb = platform_get_drvdata(pdev);
-
-	unregister_framebuffer(msmfb->fb);
-	iounmap(msmfb->fb->screen_base);
-	framebuffer_release(msmfb->fb);
-
-	return 0;
-}
-
-static struct platform_driver msm_panel_driver = {
-	/* need to write remove */
-	.probe = msmfb_probe,
-	.remove = msmfb_remove,
-	.driver = {.name = "msm_panel"},
-};
-
-
-static int msmfb_add_mdp_device(struct device *dev,
-				struct class_interface *class_intf)
-{
-	/* might need locking if mulitple mdp devices */
-	if (mdp)
-		return 0;
-	mdp = container_of(dev, struct mdp_device, dev);
-	return platform_driver_register(&msm_panel_driver);
-}
-
-static void msmfb_remove_mdp_device(struct device *dev,
-				struct class_interface *class_intf)
-{
-	/* might need locking if mulitple mdp devices */
-	if (dev != &mdp->dev)
-		return;
-	platform_driver_unregister(&msm_panel_driver);
-	mdp = NULL;
-}
-
-static struct class_interface msm_fb_interface = {
-	.add_dev = &msmfb_add_mdp_device,
-	.remove_dev = &msmfb_remove_mdp_device,
-};
-
-static int __init msmfb_init(void)
-{
-	return register_mdp_client(&msm_fb_interface);
-}
-
-module_init(msmfb_init);
diff --git a/drivers/video/fbdev/mxsfb.c b/drivers/video/fbdev/mxsfb.c
index f8ac4a452f26..4e6608ceac09 100644
--- a/drivers/video/fbdev/mxsfb.c
+++ b/drivers/video/fbdev/mxsfb.c
@@ -316,6 +316,18 @@ static int mxsfb_check_var(struct fb_var_screeninfo *var,
 	return 0;
 }
 
+static inline void mxsfb_enable_axi_clk(struct mxsfb_info *host)
+{
+	if (host->clk_axi)
+		clk_prepare_enable(host->clk_axi);
+}
+
+static inline void mxsfb_disable_axi_clk(struct mxsfb_info *host)
+{
+	if (host->clk_axi)
+		clk_disable_unprepare(host->clk_axi);
+}
+
 static void mxsfb_enable_controller(struct fb_info *fb_info)
 {
 	struct mxsfb_info *host = to_imxfb_host(fb_info);
@@ -333,14 +345,13 @@ static void mxsfb_enable_controller(struct fb_info *fb_info)
 		}
 	}
 
-	if (host->clk_axi)
-		clk_prepare_enable(host->clk_axi);
-
 	if (host->clk_disp_axi)
 		clk_prepare_enable(host->clk_disp_axi);
 	clk_prepare_enable(host->clk);
 	clk_set_rate(host->clk, PICOS2KHZ(fb_info->var.pixclock) * 1000U);
 
+	mxsfb_enable_axi_clk(host);
+
 	/* if it was disabled, re-enable the mode again */
 	writel(CTRL_DOTCLK_MODE, host->base + LCDC_CTRL + REG_SET);
 
@@ -380,11 +391,11 @@ static void mxsfb_disable_controller(struct fb_info *fb_info)
 	reg = readl(host->base + LCDC_VDCTRL4);
 	writel(reg & ~VDCTRL4_SYNC_SIGNALS_ON, host->base + LCDC_VDCTRL4);
 
+	mxsfb_disable_axi_clk(host);
+
 	clk_disable_unprepare(host->clk);
 	if (host->clk_disp_axi)
 		clk_disable_unprepare(host->clk_disp_axi);
-	if (host->clk_axi)
-		clk_disable_unprepare(host->clk_axi);
 
 	host->enabled = 0;
 
@@ -421,6 +432,8 @@ static int mxsfb_set_par(struct fb_info *fb_info)
 		mxsfb_disable_controller(fb_info);
 	}
 
+	mxsfb_enable_axi_clk(host);
+
 	/* clear the FIFOs */
 	writel(CTRL1_FIFO_CLEAR, host->base + LCDC_CTRL1 + REG_SET);
 
@@ -438,6 +451,7 @@ static int mxsfb_set_par(struct fb_info *fb_info)
 		ctrl |= CTRL_SET_WORD_LENGTH(3);
 		switch (host->ld_intf_width) {
 		case STMLCDIF_8BIT:
+			mxsfb_disable_axi_clk(host);
 			dev_err(&host->pdev->dev,
 					"Unsupported LCD bus width mapping\n");
 			return -EINVAL;
@@ -451,6 +465,7 @@ static int mxsfb_set_par(struct fb_info *fb_info)
 		writel(CTRL1_SET_BYTE_PACKAGING(0x7), host->base + LCDC_CTRL1);
 		break;
 	default:
+		mxsfb_disable_axi_clk(host);
 		dev_err(&host->pdev->dev, "Unhandled color depth of %u\n",
 				fb_info->var.bits_per_pixel);
 		return -EINVAL;
@@ -504,6 +519,8 @@ static int mxsfb_set_par(struct fb_info *fb_info)
 			fb_info->fix.line_length * fb_info->var.yoffset,
 			host->base + host->devdata->next_buf);
 
+	mxsfb_disable_axi_clk(host);
+
 	if (reenable)
 		mxsfb_enable_controller(fb_info);
 
@@ -582,10 +599,14 @@ static int mxsfb_pan_display(struct fb_var_screeninfo *var,
 
 	offset = fb_info->fix.line_length * var->yoffset;
 
+	mxsfb_enable_axi_clk(host);
+
 	/* update on next VSYNC */
 	writel(fb_info->fix.smem_start + offset,
 			host->base + host->devdata->next_buf);
 
+	mxsfb_disable_axi_clk(host);
+
 	return 0;
 }
 
@@ -608,13 +629,17 @@ static int mxsfb_restore_mode(struct mxsfb_info *host,
 	unsigned line_count;
 	unsigned period;
 	unsigned long pa, fbsize;
-	int bits_per_pixel, ofs;
+	int bits_per_pixel, ofs, ret = 0;
 	u32 transfer_count, vdctrl0, vdctrl2, vdctrl3, vdctrl4, ctrl;
 
+	mxsfb_enable_axi_clk(host);
+
 	/* Only restore the mode when the controller is running */
 	ctrl = readl(host->base + LCDC_CTRL);
-	if (!(ctrl & CTRL_RUN))
-		return -EINVAL;
+	if (!(ctrl & CTRL_RUN)) {
+		ret = -EINVAL;
+		goto err;
+	}
 
 	vdctrl0 = readl(host->base + LCDC_VDCTRL0);
 	vdctrl2 = readl(host->base + LCDC_VDCTRL2);
@@ -635,7 +660,8 @@ static int mxsfb_restore_mode(struct mxsfb_info *host,
 		break;
 	case 1:
 	default:
-		return -EINVAL;
+		ret = -EINVAL;
+		goto err;
 	}
 
 	fb_info->var.bits_per_pixel = bits_per_pixel;
@@ -673,10 +699,14 @@ static int mxsfb_restore_mode(struct mxsfb_info *host,
 
 	pa = readl(host->base + host->devdata->cur_buf);
 	fbsize = fb_info->fix.line_length * vmode->yres;
-	if (pa < fb_info->fix.smem_start)
-		return -EINVAL;
-	if (pa + fbsize > fb_info->fix.smem_start + fb_info->fix.smem_len)
-		return -EINVAL;
+	if (pa < fb_info->fix.smem_start) {
+		ret = -EINVAL;
+		goto err;
+	}
+	if (pa + fbsize > fb_info->fix.smem_start + fb_info->fix.smem_len) {
+		ret = -EINVAL;
+		goto err;
+	}
 	ofs = pa - fb_info->fix.smem_start;
 	if (ofs) {
 		memmove(fb_info->screen_base, fb_info->screen_base + ofs, fbsize);
@@ -689,7 +719,11 @@ static int mxsfb_restore_mode(struct mxsfb_info *host,
 	clk_prepare_enable(host->clk);
 	host->enabled = 1;
 
-	return 0;
+err:
+	if (ret)
+		mxsfb_disable_axi_clk(host);
+
+	return ret;
 }
 
 static int mxsfb_init_fbinfo_dt(struct mxsfb_info *host,
@@ -814,7 +848,7 @@ static void mxsfb_free_videomem(struct mxsfb_info *host)
 	free_pages_exact(fb_info->screen_base, fb_info->fix.smem_len);
 }
 
-static struct platform_device_id mxsfb_devtype[] = {
+static const struct platform_device_id mxsfb_devtype[] = {
 	{
 		.name = "imx23-fb",
 		.driver_data = MXSFB_V3,
@@ -915,7 +949,9 @@ static int mxsfb_probe(struct platform_device *pdev)
 	}
 
 	if (!host->enabled) {
+		mxsfb_enable_axi_clk(host);
 		writel(0, host->base + LCDC_CTRL);
+		mxsfb_disable_axi_clk(host);
 		mxsfb_set_par(fb_info);
 		mxsfb_enable_controller(fb_info);
 	}
@@ -954,11 +990,15 @@ static void mxsfb_shutdown(struct platform_device *pdev)
 	struct fb_info *fb_info = platform_get_drvdata(pdev);
 	struct mxsfb_info *host = to_imxfb_host(fb_info);
 
+	mxsfb_enable_axi_clk(host);
+
 	/*
 	 * Force stop the LCD controller as keeping it running during reboot
 	 * might interfere with the BootROM's boot mode pads sampling.
 	 */
 	writel(CTRL_RUN, host->base + LCDC_CTRL + REG_CLR);
+
+	mxsfb_disable_axi_clk(host);
 }
 
 static struct platform_driver mxsfb_driver = {
diff --git a/drivers/video/fbdev/neofb.c b/drivers/video/fbdev/neofb.c
index 44f99a60bb9b..db023a97d1ea 100644
--- a/drivers/video/fbdev/neofb.c
+++ b/drivers/video/fbdev/neofb.c
@@ -71,11 +71,6 @@
 #include <asm/io.h>
 #include <asm/irq.h>
 #include <asm/pgtable.h>
-
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
-
 #include <video/vga.h>
 #include <video/neomagic.h>
 
@@ -1710,6 +1705,7 @@ static int neo_map_video(struct fb_info *info, struct pci_dev *dev,
 			 int video_len)
 {
 	//unsigned long addr;
+	struct neofb_par *par = info->par;
 
 	DBG("neo_map_video");
 
@@ -1723,7 +1719,7 @@ static int neo_map_video(struct fb_info *info, struct pci_dev *dev,
 	}
 
 	info->screen_base =
-	    ioremap(info->fix.smem_start, info->fix.smem_len);
+	    ioremap_wc(info->fix.smem_start, info->fix.smem_len);
 	if (!info->screen_base) {
 		printk("neofb: unable to map screen memory\n");
 		release_mem_region(info->fix.smem_start,
@@ -1733,11 +1729,8 @@ static int neo_map_video(struct fb_info *info, struct pci_dev *dev,
 		printk(KERN_INFO "neofb: mapped framebuffer at %p\n",
 		       info->screen_base);
 
-#ifdef CONFIG_MTRR
-	((struct neofb_par *)(info->par))->mtrr =
-		mtrr_add(info->fix.smem_start, pci_resource_len(dev, 0),
-				MTRR_TYPE_WRCOMB, 1);
-#endif
+	par->wc_cookie = arch_phys_wc_add(info->fix.smem_start,
+					  pci_resource_len(dev, 0));
 
 	/* Clear framebuffer, it's all white in memory after boot */
 	memset_io(info->screen_base, 0, info->fix.smem_len);
@@ -1754,16 +1747,11 @@ static int neo_map_video(struct fb_info *info, struct pci_dev *dev,
 
 static void neo_unmap_video(struct fb_info *info)
 {
-	DBG("neo_unmap_video");
+	struct neofb_par *par = info->par;
 
-#ifdef CONFIG_MTRR
-	{
-		struct neofb_par *par = info->par;
+	DBG("neo_unmap_video");
 
-		mtrr_del(par->mtrr, info->fix.smem_start,
-			 info->fix.smem_len);
-	}
-#endif
+	arch_phys_wc_del(par->wc_cookie);
 	iounmap(info->screen_base);
 	info->screen_base = NULL;
 
diff --git a/drivers/video/fbdev/nvidia/nv_type.h b/drivers/video/fbdev/nvidia/nv_type.h
index c03f7f55c76d..6ff321a36813 100644
--- a/drivers/video/fbdev/nvidia/nv_type.h
+++ b/drivers/video/fbdev/nvidia/nv_type.h
@@ -148,12 +148,7 @@ struct nvidia_par {
 	u32 forceCRTC;
 	u32 open_count;
 	u8 DDCBase;
-#ifdef CONFIG_MTRR
-	struct {
-		int vram;
-		int vram_valid;
-	} mtrr;
-#endif
+	int wc_cookie;
 	struct nvidia_i2c_chan chan[3];
 
 	volatile u32 __iomem *REGS;
diff --git a/drivers/video/fbdev/nvidia/nvidia.c b/drivers/video/fbdev/nvidia/nvidia.c
index 4273c6ee8cf6..ce7dab7299fe 100644
--- a/drivers/video/fbdev/nvidia/nvidia.c
+++ b/drivers/video/fbdev/nvidia/nvidia.c
@@ -21,9 +21,6 @@
 #include <linux/pci.h>
 #include <linux/console.h>
 #include <linux/backlight.h>
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
 #ifdef CONFIG_BOOTX_TEXT
 #include <asm/btext.h>
 #endif
@@ -76,9 +73,7 @@ static int paneltweak = 0;
 static int vram = 0;
 static int bpp = 8;
 static int reverse_i2c;
-#ifdef CONFIG_MTRR
 static bool nomtrr = false;
-#endif
 #ifdef CONFIG_PMAC_BACKLIGHT
 static int backlight = 1;
 #else
@@ -1361,7 +1356,8 @@ static int nvidiafb_probe(struct pci_dev *pd, const struct pci_device_id *ent)
 	par->ScratchBufferStart = par->FbUsableSize - par->ScratchBufferSize;
 	par->CursorStart = par->FbUsableSize + (32 * 1024);
 
-	info->screen_base = ioremap(nvidiafb_fix.smem_start, par->FbMapSize);
+	info->screen_base = ioremap_wc(nvidiafb_fix.smem_start,
+				       par->FbMapSize);
 	info->screen_size = par->FbUsableSize;
 	nvidiafb_fix.smem_len = par->RamAmountKBytes * 1024;
 
@@ -1372,20 +1368,9 @@ static int nvidiafb_probe(struct pci_dev *pd, const struct pci_device_id *ent)
 
 	par->FbStart = info->screen_base;
 
-#ifdef CONFIG_MTRR
-	if (!nomtrr) {
-		par->mtrr.vram = mtrr_add(nvidiafb_fix.smem_start,
-					  par->RamAmountKBytes * 1024,
-					  MTRR_TYPE_WRCOMB, 1);
-		if (par->mtrr.vram < 0) {
-			printk(KERN_ERR PFX "unable to setup MTRR\n");
-		} else {
-			par->mtrr.vram_valid = 1;
-			/* let there be speed */
-			printk(KERN_INFO PFX "MTRR set to ON\n");
-		}
-	}
-#endif				/* CONFIG_MTRR */
+	if (!nomtrr)
+		par->wc_cookie = arch_phys_wc_add(nvidiafb_fix.smem_start,
+						  par->RamAmountKBytes * 1024);
 
 	info->fbops = &nvidia_fb_ops;
 	info->fix = nvidiafb_fix;
@@ -1443,13 +1428,7 @@ static void nvidiafb_remove(struct pci_dev *pd)
 	unregister_framebuffer(info);
 
 	nvidia_bl_exit(par);
-
-#ifdef CONFIG_MTRR
-	if (par->mtrr.vram_valid)
-		mtrr_del(par->mtrr.vram, info->fix.smem_start,
-			 info->fix.smem_len);
-#endif				/* CONFIG_MTRR */
-
+	arch_phys_wc_del(par->wc_cookie);
 	iounmap(info->screen_base);
 	fb_destroy_modedb(info->monspecs.modedb);
 	nvidia_delete_i2c_busses(par);
@@ -1501,10 +1480,8 @@ static int nvidiafb_setup(char *options)
 			vram = simple_strtoul(this_opt+5, NULL, 0);
 		} else if (!strncmp(this_opt, "backlight:", 10)) {
 			backlight = simple_strtoul(this_opt+10, NULL, 0);
-#ifdef CONFIG_MTRR
 		} else if (!strncmp(this_opt, "nomtrr", 6)) {
 			nomtrr = true;
-#endif
 		} else if (!strncmp(this_opt, "fpdither:", 9)) {
 			fpdither = simple_strtol(this_opt+9, NULL, 0);
 		} else if (!strncmp(this_opt, "bpp:", 4)) {
@@ -1592,11 +1569,9 @@ MODULE_PARM_DESC(bpp, "pixel width in bits"
 		 "(default=8)");
 module_param(reverse_i2c, int, 0);
 MODULE_PARM_DESC(reverse_i2c, "reverse port assignment of the i2c bus");
-#ifdef CONFIG_MTRR
 module_param(nomtrr, bool, false);
 MODULE_PARM_DESC(nomtrr, "Disables MTRR support (0 or 1=disabled) "
 		 "(default=0)");
-#endif
 
 MODULE_AUTHOR("Antonino Daplas");
 MODULE_DESCRIPTION("Framebuffer driver for nVidia graphics chipset");
diff --git a/drivers/video/fbdev/omap/Kconfig b/drivers/video/fbdev/omap/Kconfig
index 18c4cb0d5690..29d250da8a3e 100644
--- a/drivers/video/fbdev/omap/Kconfig
+++ b/drivers/video/fbdev/omap/Kconfig
@@ -42,7 +42,7 @@ config FB_OMAP_LCD_MIPID
 config FB_OMAP_LCD_H3
 	bool "TPS65010 LCD controller on OMAP-H3"
 	depends on MACH_OMAP_H3
-	depends on TPS65010
+	depends on TPS65010=y
 	default y
 	help
 	  Say Y here if you want to have support for the LCD on the
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-opa362.c b/drivers/video/fbdev/omap2/displays-new/encoder-opa362.c
index 84a6b3367124..a14d993f719d 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-opa362.c
+++ b/drivers/video/fbdev/omap2/displays-new/encoder-opa362.c
@@ -201,15 +201,9 @@ static int opa362_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, ddata);
 
-	gpio = devm_gpiod_get(&pdev->dev, "enable");
-	if (IS_ERR(gpio)) {
-		if (PTR_ERR(gpio) != -ENOENT)
-			return PTR_ERR(gpio);
-
-		gpio = NULL;
-	} else {
-		gpiod_direction_output(gpio, 0);
-	}
+	gpio = devm_gpiod_get_optional(&pdev->dev, "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(gpio))
+		return PTR_ERR(gpio);
 
 	ddata->enable_gpio = gpio;
 
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c b/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
index eb8fd8140ad0..f7be3489f744 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
@@ -209,16 +209,9 @@ static int panel_dpi_probe_of(struct platform_device *pdev)
 	struct videomode vm;
 	struct gpio_desc *gpio;
 
-	gpio = devm_gpiod_get(&pdev->dev, "enable");
-
-	if (IS_ERR(gpio)) {
-		if (PTR_ERR(gpio) != -ENOENT)
-			return PTR_ERR(gpio);
-		else
-			gpio = NULL;
-	} else {
-		gpiod_direction_output(gpio, 0);
-	}
+	gpio = devm_gpiod_get_optional(&pdev->dev, "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(gpio))
+		return PTR_ERR(gpio);
 
 	ddata->enable_gpio = gpio;
 
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c b/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c
index 9974a37a11af..6a1b6a89a928 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c
@@ -285,15 +285,14 @@ static int lb035q02_probe_of(struct spi_device *spi)
 	struct omap_dss_device *in;
 	struct gpio_desc *gpio;
 
-	gpio = devm_gpiod_get(&spi->dev, "enable");
+	gpio = devm_gpiod_get(&spi->dev, "enable", GPIOD_OUT_LOW);
 	if (IS_ERR(gpio)) {
 		dev_err(&spi->dev, "failed to parse enable gpio\n");
 		return PTR_ERR(gpio);
-	} else {
-		gpiod_direction_output(gpio, 0);
-		ddata->enable_gpio = gpio;
 	}
 
+	ddata->enable_gpio = gpio;
+
 	ddata->backlight_gpio = -ENOENT;
 
 	in = omapdss_of_find_source_for_first_ep(node);
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 eae263702964..abfd1f6e3327 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
+++ b/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
@@ -268,17 +268,12 @@ static  int sharp_ls_get_gpio_of(struct device *dev, int index, int val,
 	const char *desc, struct gpio_desc **gpiod)
 {
 	struct gpio_desc *gd;
-	int r;
 
 	*gpiod = NULL;
 
-	gd = devm_gpiod_get_index(dev, desc, index);
+	gd = devm_gpiod_get_index(dev, desc, index, GPIOD_OUT_LOW);
 	if (IS_ERR(gd))
-		return PTR_ERR(gd) == -ENOENT ? 0 : PTR_ERR(gd);
-
-	r = gpiod_direction_output(gd, val);
-	if (r)
-		return r;
+		return PTR_ERR(gd);
 
 	*gpiod = gd;
 	return 0;
diff --git a/drivers/video/fbdev/omap2/dss/core.c b/drivers/video/fbdev/omap2/dss/core.c
index 16751755d433..54eeb507f9b3 100644
--- a/drivers/video/fbdev/omap2/dss/core.c
+++ b/drivers/video/fbdev/omap2/dss/core.c
@@ -50,8 +50,6 @@ static char *def_disp_name;
 module_param_named(def_disp, def_disp_name, charp, 0);
 MODULE_PARM_DESC(def_disp, "default display name");
 
-static bool dss_initialized;
-
 const char *omapdss_get_default_display_name(void)
 {
 	return core.default_display_name;
@@ -65,12 +63,6 @@ enum omapdss_version omapdss_get_version(void)
 }
 EXPORT_SYMBOL(omapdss_get_version);
 
-bool omapdss_is_initialized(void)
-{
-	return dss_initialized;
-}
-EXPORT_SYMBOL(omapdss_is_initialized);
-
 struct platform_device *dss_get_core_pdev(void)
 {
 	return core.pdev;
@@ -253,6 +245,8 @@ static struct platform_driver omap_dss_driver = {
 
 /* INIT */
 static int (*dss_output_drv_reg_funcs[])(void) __initdata = {
+	dss_init_platform_driver,
+	dispc_init_platform_driver,
 #ifdef CONFIG_OMAP2_DSS_DSI
 	dsi_init_platform_driver,
 #endif
@@ -276,32 +270,32 @@ static int (*dss_output_drv_reg_funcs[])(void) __initdata = {
 #endif
 };
 
-static void (*dss_output_drv_unreg_funcs[])(void) __exitdata = {
-#ifdef CONFIG_OMAP2_DSS_DSI
-	dsi_uninit_platform_driver,
+static void (*dss_output_drv_unreg_funcs[])(void) = {
+#ifdef CONFIG_OMAP5_DSS_HDMI
+	hdmi5_uninit_platform_driver,
 #endif
-#ifdef CONFIG_OMAP2_DSS_DPI
-	dpi_uninit_platform_driver,
+#ifdef CONFIG_OMAP4_DSS_HDMI
+	hdmi4_uninit_platform_driver,
 #endif
-#ifdef CONFIG_OMAP2_DSS_SDI
-	sdi_uninit_platform_driver,
+#ifdef CONFIG_OMAP2_DSS_VENC
+	venc_uninit_platform_driver,
 #endif
 #ifdef CONFIG_OMAP2_DSS_RFBI
 	rfbi_uninit_platform_driver,
 #endif
-#ifdef CONFIG_OMAP2_DSS_VENC
-	venc_uninit_platform_driver,
+#ifdef CONFIG_OMAP2_DSS_SDI
+	sdi_uninit_platform_driver,
 #endif
-#ifdef CONFIG_OMAP4_DSS_HDMI
-	hdmi4_uninit_platform_driver,
+#ifdef CONFIG_OMAP2_DSS_DPI
+	dpi_uninit_platform_driver,
 #endif
-#ifdef CONFIG_OMAP5_DSS_HDMI
-	hdmi5_uninit_platform_driver,
+#ifdef CONFIG_OMAP2_DSS_DSI
+	dsi_uninit_platform_driver,
 #endif
+	dispc_uninit_platform_driver,
+	dss_uninit_platform_driver,
 };
 
-static bool dss_output_drv_loaded[ARRAY_SIZE(dss_output_drv_reg_funcs)];
-
 static int __init omap_dss_init(void)
 {
 	int r;
@@ -311,35 +305,20 @@ static int __init omap_dss_init(void)
 	if (r)
 		return r;
 
-	r = dss_init_platform_driver();
-	if (r) {
-		DSSERR("Failed to initialize DSS platform driver\n");
-		goto err_dss;
-	}
-
-	r = dispc_init_platform_driver();
-	if (r) {
-		DSSERR("Failed to initialize dispc platform driver\n");
-		goto err_dispc;
-	}
-
-	/*
-	 * It's ok if the output-driver register fails. It happens, for example,
-	 * when there is no output-device (e.g. SDI for OMAP4).
-	 */
 	for (i = 0; i < ARRAY_SIZE(dss_output_drv_reg_funcs); ++i) {
 		r = dss_output_drv_reg_funcs[i]();
-		if (r == 0)
-			dss_output_drv_loaded[i] = true;
+		if (r)
+			goto err_reg;
 	}
 
-	dss_initialized = true;
-
 	return 0;
 
-err_dispc:
-	dss_uninit_platform_driver();
-err_dss:
+err_reg:
+	for (i = ARRAY_SIZE(dss_output_drv_reg_funcs) - i;
+			i < ARRAY_SIZE(dss_output_drv_reg_funcs);
+			++i)
+		dss_output_drv_unreg_funcs[i]();
+
 	platform_driver_unregister(&omap_dss_driver);
 
 	return r;
@@ -349,13 +328,8 @@ static void __exit omap_dss_exit(void)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(dss_output_drv_unreg_funcs); ++i) {
-		if (dss_output_drv_loaded[i])
-			dss_output_drv_unreg_funcs[i]();
-	}
-
-	dispc_uninit_platform_driver();
-	dss_uninit_platform_driver();
+	for (i = 0; i < ARRAY_SIZE(dss_output_drv_unreg_funcs); ++i)
+		dss_output_drv_unreg_funcs[i]();
 
 	platform_driver_unregister(&omap_dss_driver);
 }
diff --git a/drivers/video/fbdev/omap2/dss/dispc.c b/drivers/video/fbdev/omap2/dss/dispc.c
index f4fc77d9d3bf..be716c9ffb88 100644
--- a/drivers/video/fbdev/omap2/dss/dispc.c
+++ b/drivers/video/fbdev/omap2/dss/dispc.c
@@ -39,6 +39,7 @@
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
 #include <linux/of.h>
+#include <linux/component.h>
 
 #include <video/omapdss.h>
 
@@ -95,6 +96,9 @@ struct dispc_features {
 	bool mstandby_workaround:1;
 
 	bool set_max_preload:1;
+
+	/* PIXEL_INC is not added to the last pixel of a line */
+	bool last_pixel_inc_missing:1;
 };
 
 #define DISPC_MAX_NR_FIFOS 5
@@ -1741,6 +1745,15 @@ static void dispc_ovl_set_rotation_attrs(enum omap_plane plane, u8 rotation,
 			row_repeat = false;
 	}
 
+	/*
+	 * OMAP4/5 Errata i631:
+	 * NV12 in 1D mode must use ROTATION=1. Otherwise DSS will fetch extra
+	 * rows beyond the framebuffer, which may cause OCP error.
+	 */
+	if (color_mode == OMAP_DSS_COLOR_NV12 &&
+			rotation_type != OMAP_DSS_ROT_TILER)
+		vidrot = 1;
+
 	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), vidrot, 13, 12);
 	if (dss_has_feature(FEAT_ROWREPEATENABLE))
 		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane),
@@ -2154,7 +2167,7 @@ static unsigned long calc_core_clk_five_taps(unsigned long pclk,
 	if (height > out_height) {
 		unsigned int ppl = mgr_timings->x_res;
 
-		tmp = pclk * height * out_width;
+		tmp = (u64)pclk * height * out_width;
 		do_div(tmp, 2 * out_height * ppl);
 		core_clk = tmp;
 
@@ -2162,14 +2175,14 @@ static unsigned long calc_core_clk_five_taps(unsigned long pclk,
 			if (ppl == out_width)
 				return 0;
 
-			tmp = pclk * (height - 2 * out_height) * out_width;
+			tmp = (u64)pclk * (height - 2 * out_height) * out_width;
 			do_div(tmp, 2 * out_height * (ppl - out_width));
 			core_clk = max_t(u32, core_clk, tmp);
 		}
 	}
 
 	if (width > out_width) {
-		tmp = pclk * width;
+		tmp = (u64)pclk * width;
 		do_div(tmp, out_width);
 		core_clk = max_t(u32, core_clk, tmp);
 
@@ -2267,6 +2280,11 @@ static int dispc_ovl_calc_scaling_24xx(unsigned long pclk, unsigned long lclk,
 		}
 	} while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
 
+	if (error) {
+		DSSERR("failed to find scaling settings\n");
+		return -EINVAL;
+	}
+
 	if (in_width > maxsinglelinewidth) {
 		DSSERR("Cannot scale max input width exceeded");
 		return -EINVAL;
@@ -2283,7 +2301,6 @@ static int dispc_ovl_calc_scaling_34xx(unsigned long pclk, unsigned long lclk,
 {
 	int error;
 	u16 in_width, in_height;
-	int min_factor = min(*decim_x, *decim_y);
 	const int maxsinglelinewidth =
 			dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
 
@@ -2317,20 +2334,32 @@ again:
 		error = (error || in_width > maxsinglelinewidth * 2 ||
 			(in_width > maxsinglelinewidth && *five_taps) ||
 			!*core_clk || *core_clk > dispc_core_clk_rate());
-		if (error) {
-			if (*decim_x == *decim_y) {
-				*decim_x = min_factor;
-				++*decim_y;
+
+		if (!error) {
+			/* verify that we're inside the limits of scaler */
+			if (in_width / 4 > out_width)
+					error = 1;
+
+			if (*five_taps) {
+				if (in_height / 4 > out_height)
+					error = 1;
 			} else {
-				swap(*decim_x, *decim_y);
-				if (*decim_x < *decim_y)
-					++*decim_x;
+				if (in_height / 2 > out_height)
+					error = 1;
 			}
 		}
+
+		if (error)
+			++*decim_y;
 	} while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
 
-	if (check_horiz_timing_omap3(pclk, lclk, mgr_timings, pos_x, width,
-				height, out_width, out_height, *five_taps)) {
+	if (error) {
+		DSSERR("failed to find scaling settings\n");
+		return -EINVAL;
+	}
+
+	if (check_horiz_timing_omap3(pclk, lclk, mgr_timings, pos_x, in_width,
+				in_height, out_width, out_height, *five_taps)) {
 			DSSERR("horizontal timing too tight\n");
 			return -EINVAL;
 	}
@@ -2390,6 +2419,9 @@ static int dispc_ovl_calc_scaling_44xx(unsigned long pclk, unsigned long lclk,
 	return 0;
 }
 
+#define DIV_FRAC(dividend, divisor) \
+	((dividend) * 100 / (divisor) - ((dividend) / (divisor) * 100))
+
 static int dispc_ovl_calc_scaling(unsigned long pclk, unsigned long lclk,
 		enum omap_overlay_caps caps,
 		const struct omap_video_timings *mgr_timings,
@@ -2449,8 +2481,19 @@ static int dispc_ovl_calc_scaling(unsigned long pclk, unsigned long lclk,
 	if (ret)
 		return ret;
 
-	DSSDBG("required core clk rate = %lu Hz\n", core_clk);
-	DSSDBG("current core clk rate = %lu Hz\n", dispc_core_clk_rate());
+	DSSDBG("%dx%d -> %dx%d (%d.%02d x %d.%02d), decim %dx%d %dx%d (%d.%02d x %d.%02d), taps %d, req clk %lu, cur clk %lu\n",
+		width, height,
+		out_width, out_height,
+		out_width / width, DIV_FRAC(out_width, width),
+		out_height / height, DIV_FRAC(out_height, height),
+
+		decim_x, decim_y,
+		width / decim_x, height / decim_y,
+		out_width / (width / decim_x), DIV_FRAC(out_width, width / decim_x),
+		out_height / (height / decim_y), DIV_FRAC(out_height, height / decim_y),
+
+		*five_taps ? 5 : 3,
+		core_clk, dispc_core_clk_rate());
 
 	if (!core_clk || core_clk > dispc_core_clk_rate()) {
 		DSSERR("failed to set up scaling, "
@@ -2533,6 +2576,21 @@ static int dispc_ovl_setup_common(enum omap_plane plane,
 	if (paddr == 0 && rotation_type != OMAP_DSS_ROT_TILER)
 		return -EINVAL;
 
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+	case OMAP_DSS_COLOR_NV12:
+		if (in_width & 1) {
+			DSSERR("input width %d is not even for YUV format\n",
+				in_width);
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		break;
+	}
+
 	out_width = out_width == 0 ? width : out_width;
 	out_height = out_height == 0 ? height : out_height;
 
@@ -2563,6 +2621,27 @@ static int dispc_ovl_setup_common(enum omap_plane plane,
 	in_width = in_width / x_predecim;
 	in_height = in_height / y_predecim;
 
+	if (x_predecim > 1 || y_predecim > 1)
+		DSSDBG("predecimation %d x %x, new input size %d x %d\n",
+			x_predecim, y_predecim, in_width, in_height);
+
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+	case OMAP_DSS_COLOR_NV12:
+		if (in_width & 1) {
+			DSSDBG("predecimated input width is not even for YUV format\n");
+			DSSDBG("adjusting input width %d -> %d\n",
+				in_width, in_width & ~1);
+
+			in_width &= ~1;
+		}
+		break;
+
+	default:
+		break;
+	}
+
 	if (color_mode == OMAP_DSS_COLOR_YUV2 ||
 			color_mode == OMAP_DSS_COLOR_UYVY ||
 			color_mode == OMAP_DSS_COLOR_NV12)
@@ -2632,6 +2711,9 @@ static int dispc_ovl_setup_common(enum omap_plane plane,
 		dispc_ovl_set_ba1_uv(plane, p_uv_addr + offset1);
 	}
 
+	if (dispc.feat->last_pixel_inc_missing)
+		row_inc += pix_inc - 1;
+
 	dispc_ovl_set_row_inc(plane, row_inc);
 	dispc_ovl_set_pix_inc(plane, pix_inc);
 
@@ -3692,7 +3774,7 @@ static void _omap_dispc_initial_config(void)
 		dispc_init_mflag();
 }
 
-static const struct dispc_features omap24xx_dispc_feats __initconst = {
+static const struct dispc_features omap24xx_dispc_feats = {
 	.sw_start		=	5,
 	.fp_start		=	15,
 	.bp_start		=	27,
@@ -3709,9 +3791,10 @@ static const struct dispc_features omap24xx_dispc_feats __initconst = {
 	.num_fifos		=	3,
 	.no_framedone_tv	=	true,
 	.set_max_preload	=	false,
+	.last_pixel_inc_missing	=	true,
 };
 
-static const struct dispc_features omap34xx_rev1_0_dispc_feats __initconst = {
+static const struct dispc_features omap34xx_rev1_0_dispc_feats = {
 	.sw_start		=	5,
 	.fp_start		=	15,
 	.bp_start		=	27,
@@ -3729,9 +3812,10 @@ static const struct dispc_features omap34xx_rev1_0_dispc_feats __initconst = {
 	.num_fifos		=	3,
 	.no_framedone_tv	=	true,
 	.set_max_preload	=	false,
+	.last_pixel_inc_missing	=	true,
 };
 
-static const struct dispc_features omap34xx_rev3_0_dispc_feats __initconst = {
+static const struct dispc_features omap34xx_rev3_0_dispc_feats = {
 	.sw_start		=	7,
 	.fp_start		=	19,
 	.bp_start		=	31,
@@ -3749,9 +3833,10 @@ static const struct dispc_features omap34xx_rev3_0_dispc_feats __initconst = {
 	.num_fifos		=	3,
 	.no_framedone_tv	=	true,
 	.set_max_preload	=	false,
+	.last_pixel_inc_missing	=	true,
 };
 
-static const struct dispc_features omap44xx_dispc_feats __initconst = {
+static const struct dispc_features omap44xx_dispc_feats = {
 	.sw_start		=	7,
 	.fp_start		=	19,
 	.bp_start		=	31,
@@ -3771,7 +3856,7 @@ static const struct dispc_features omap44xx_dispc_feats __initconst = {
 	.set_max_preload	=	true,
 };
 
-static const struct dispc_features omap54xx_dispc_feats __initconst = {
+static const struct dispc_features omap54xx_dispc_feats = {
 	.sw_start		=	7,
 	.fp_start		=	19,
 	.bp_start		=	31,
@@ -3792,7 +3877,7 @@ static const struct dispc_features omap54xx_dispc_feats __initconst = {
 	.set_max_preload	=	true,
 };
 
-static int __init dispc_init_features(struct platform_device *pdev)
+static int dispc_init_features(struct platform_device *pdev)
 {
 	const struct dispc_features *src;
 	struct dispc_features *dst;
@@ -3882,8 +3967,9 @@ void dispc_free_irq(void *dev_id)
 EXPORT_SYMBOL(dispc_free_irq);
 
 /* DISPC HW IP initialisation */
-static int __init omap_dispchw_probe(struct platform_device *pdev)
+static int dispc_bind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	u32 rev;
 	int r = 0;
 	struct resource *dispc_mem;
@@ -3955,12 +4041,27 @@ err_runtime_get:
 	return r;
 }
 
-static int __exit omap_dispchw_remove(struct platform_device *pdev)
+static void dispc_unbind(struct device *dev, struct device *master,
+			       void *data)
 {
-	pm_runtime_disable(&pdev->dev);
+	pm_runtime_disable(dev);
 
 	dss_uninit_overlay_managers();
+}
+
+static const struct component_ops dispc_component_ops = {
+	.bind	= dispc_bind,
+	.unbind	= dispc_unbind,
+};
 
+static int dispc_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &dispc_component_ops);
+}
+
+static int dispc_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &dispc_component_ops);
 	return 0;
 }
 
@@ -4013,7 +4114,8 @@ static const struct of_device_id dispc_of_match[] = {
 };
 
 static struct platform_driver omap_dispchw_driver = {
-	.remove         = __exit_p(omap_dispchw_remove),
+	.probe		= dispc_probe,
+	.remove         = dispc_remove,
 	.driver         = {
 		.name   = "omapdss_dispc",
 		.pm	= &dispc_pm_ops,
@@ -4024,10 +4126,10 @@ static struct platform_driver omap_dispchw_driver = {
 
 int __init dispc_init_platform_driver(void)
 {
-	return platform_driver_probe(&omap_dispchw_driver, omap_dispchw_probe);
+	return platform_driver_register(&omap_dispchw_driver);
 }
 
-void __exit dispc_uninit_platform_driver(void)
+void dispc_uninit_platform_driver(void)
 {
 	platform_driver_unregister(&omap_dispchw_driver);
 }
diff --git a/drivers/video/fbdev/omap2/dss/display-sysfs.c b/drivers/video/fbdev/omap2/dss/display-sysfs.c
index 12186557a9d4..6ad0991f8259 100644
--- a/drivers/video/fbdev/omap2/dss/display-sysfs.c
+++ b/drivers/video/fbdev/omap2/dss/display-sysfs.c
@@ -324,7 +324,7 @@ int display_init_sysfs(struct platform_device *pdev)
 
 	for_each_dss_dev(dssdev) {
 		r = kobject_init_and_add(&dssdev->kobj, &display_ktype,
-			&pdev->dev.kobj, dssdev->alias);
+			&pdev->dev.kobj, "%s", dssdev->alias);
 		if (r) {
 			DSSERR("failed to create sysfs files\n");
 			omap_dss_put_device(dssdev);
diff --git a/drivers/video/fbdev/omap2/dss/dpi.c b/drivers/video/fbdev/omap2/dss/dpi.c
index f83e7b030249..fb45b6432968 100644
--- a/drivers/video/fbdev/omap2/dss/dpi.c
+++ b/drivers/video/fbdev/omap2/dss/dpi.c
@@ -32,6 +32,7 @@
 #include <linux/string.h>
 #include <linux/of.h>
 #include <linux/clk.h>
+#include <linux/component.h>
 
 #include <video/omapdss.h>
 
@@ -731,7 +732,7 @@ static void dpi_init_output(struct platform_device *pdev)
 	omapdss_register_output(out);
 }
 
-static void __exit dpi_uninit_output(struct platform_device *pdev)
+static void dpi_uninit_output(struct platform_device *pdev)
 {
 	struct dpi_data *dpi = dpi_get_data_from_pdev(pdev);
 	struct omap_dss_device *out = &dpi->output;
@@ -775,7 +776,7 @@ static void dpi_init_output_port(struct platform_device *pdev,
 	omapdss_register_output(out);
 }
 
-static void __exit dpi_uninit_output_port(struct device_node *port)
+static void dpi_uninit_output_port(struct device_node *port)
 {
 	struct dpi_data *dpi = port->data;
 	struct omap_dss_device *out = &dpi->output;
@@ -783,8 +784,9 @@ static void __exit dpi_uninit_output_port(struct device_node *port)
 	omapdss_unregister_output(out);
 }
 
-static int omap_dpi_probe(struct platform_device *pdev)
+static int dpi_bind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct dpi_data *dpi;
 
 	dpi = devm_kzalloc(&pdev->dev, sizeof(*dpi), GFP_KERNEL);
@@ -802,16 +804,32 @@ static int omap_dpi_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int __exit omap_dpi_remove(struct platform_device *pdev)
+static void dpi_unbind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	dpi_uninit_output(pdev);
+}
+
+static const struct component_ops dpi_component_ops = {
+	.bind	= dpi_bind,
+	.unbind	= dpi_unbind,
+};
 
+static int dpi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &dpi_component_ops);
+}
+
+static int dpi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &dpi_component_ops);
 	return 0;
 }
 
 static struct platform_driver omap_dpi_driver = {
-	.probe		= omap_dpi_probe,
-	.remove         = __exit_p(omap_dpi_remove),
+	.probe		= dpi_probe,
+	.remove		= dpi_remove,
 	.driver         = {
 		.name   = "omapdss_dpi",
 		.suppress_bind_attrs = true,
@@ -823,12 +841,12 @@ int __init dpi_init_platform_driver(void)
 	return platform_driver_register(&omap_dpi_driver);
 }
 
-void __exit dpi_uninit_platform_driver(void)
+void dpi_uninit_platform_driver(void)
 {
 	platform_driver_unregister(&omap_dpi_driver);
 }
 
-int __init dpi_init_port(struct platform_device *pdev, struct device_node *port)
+int dpi_init_port(struct platform_device *pdev, struct device_node *port)
 {
 	struct dpi_data *dpi;
 	struct device_node *ep;
@@ -870,7 +888,7 @@ err_datalines:
 	return r;
 }
 
-void __exit dpi_uninit_port(struct device_node *port)
+void dpi_uninit_port(struct device_node *port)
 {
 	struct dpi_data *dpi = port->data;
 
diff --git a/drivers/video/fbdev/omap2/dss/dsi.c b/drivers/video/fbdev/omap2/dss/dsi.c
index 28b0bc11669d..b3606def5b7b 100644
--- a/drivers/video/fbdev/omap2/dss/dsi.c
+++ b/drivers/video/fbdev/omap2/dss/dsi.c
@@ -40,6 +40,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/component.h>
 
 #include <video/omapdss.h>
 #include <video/mipi_display.h>
@@ -5274,8 +5275,9 @@ static int dsi_init_pll_data(struct platform_device *dsidev)
 }
 
 /* DSI1 HW IP initialisation */
-static int omap_dsihw_probe(struct platform_device *dsidev)
+static int dsi_bind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *dsidev = to_platform_device(dev);
 	u32 rev;
 	int r, i;
 	struct dsi_data *dsi;
@@ -5484,8 +5486,9 @@ err_runtime_get:
 	return r;
 }
 
-static int __exit omap_dsihw_remove(struct platform_device *dsidev)
+static void dsi_unbind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *dsidev = to_platform_device(dev);
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 
 	of_platform_depopulate(&dsidev->dev);
@@ -5502,7 +5505,21 @@ static int __exit omap_dsihw_remove(struct platform_device *dsidev)
 		regulator_disable(dsi->vdds_dsi_reg);
 		dsi->vdds_dsi_enabled = false;
 	}
+}
+
+static const struct component_ops dsi_component_ops = {
+	.bind	= dsi_bind,
+	.unbind	= dsi_unbind,
+};
 
+static int dsi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &dsi_component_ops);
+}
+
+static int dsi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &dsi_component_ops);
 	return 0;
 }
 
@@ -5569,8 +5586,8 @@ static const struct of_device_id dsi_of_match[] = {
 };
 
 static struct platform_driver omap_dsihw_driver = {
-	.probe		= omap_dsihw_probe,
-	.remove         = __exit_p(omap_dsihw_remove),
+	.probe		= dsi_probe,
+	.remove		= dsi_remove,
 	.driver         = {
 		.name   = "omapdss_dsi",
 		.pm	= &dsi_pm_ops,
@@ -5584,7 +5601,7 @@ int __init dsi_init_platform_driver(void)
 	return platform_driver_register(&omap_dsihw_driver);
 }
 
-void __exit dsi_uninit_platform_driver(void)
+void dsi_uninit_platform_driver(void)
 {
 	platform_driver_unregister(&omap_dsihw_driver);
 }
diff --git a/drivers/video/fbdev/omap2/dss/dss.c b/drivers/video/fbdev/omap2/dss/dss.c
index 7f978b6a34e8..9200a8668b49 100644
--- a/drivers/video/fbdev/omap2/dss/dss.c
+++ b/drivers/video/fbdev/omap2/dss/dss.c
@@ -39,6 +39,7 @@
 #include <linux/of.h>
 #include <linux/regulator/consumer.h>
 #include <linux/suspend.h>
+#include <linux/component.h>
 
 #include <video/omapdss.h>
 
@@ -111,6 +112,14 @@ static const char * const dss_generic_clk_source_names[] = {
 	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]	= "DSI_PLL2_HSDIV_DSI",
 };
 
+static bool dss_initialized;
+
+bool omapdss_is_initialized(void)
+{
+	return dss_initialized;
+}
+EXPORT_SYMBOL(omapdss_is_initialized);
+
 static inline void dss_write_reg(const struct dss_reg idx, u32 val)
 {
 	__raw_writel(val, dss.base + idx.idx);
@@ -811,7 +820,7 @@ static const enum omap_display_type dra7xx_ports[] = {
 	OMAP_DISPLAY_TYPE_DPI,
 };
 
-static const struct dss_features omap24xx_dss_feats __initconst = {
+static const struct dss_features omap24xx_dss_feats = {
 	/*
 	 * fck div max is really 16, but the divider range has gaps. The range
 	 * from 1 to 6 has no gaps, so let's use that as a max.
@@ -824,7 +833,7 @@ static const struct dss_features omap24xx_dss_feats __initconst = {
 	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
 };
 
-static const struct dss_features omap34xx_dss_feats __initconst = {
+static const struct dss_features omap34xx_dss_feats = {
 	.fck_div_max		=	16,
 	.dss_fck_multiplier	=	2,
 	.parent_clk_name	=	"dpll4_ck",
@@ -833,7 +842,7 @@ static const struct dss_features omap34xx_dss_feats __initconst = {
 	.num_ports		=	ARRAY_SIZE(omap34xx_ports),
 };
 
-static const struct dss_features omap3630_dss_feats __initconst = {
+static const struct dss_features omap3630_dss_feats = {
 	.fck_div_max		=	32,
 	.dss_fck_multiplier	=	1,
 	.parent_clk_name	=	"dpll4_ck",
@@ -842,7 +851,7 @@ static const struct dss_features omap3630_dss_feats __initconst = {
 	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
 };
 
-static const struct dss_features omap44xx_dss_feats __initconst = {
+static const struct dss_features omap44xx_dss_feats = {
 	.fck_div_max		=	32,
 	.dss_fck_multiplier	=	1,
 	.parent_clk_name	=	"dpll_per_x2_ck",
@@ -851,7 +860,7 @@ static const struct dss_features omap44xx_dss_feats __initconst = {
 	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
 };
 
-static const struct dss_features omap54xx_dss_feats __initconst = {
+static const struct dss_features omap54xx_dss_feats = {
 	.fck_div_max		=	64,
 	.dss_fck_multiplier	=	1,
 	.parent_clk_name	=	"dpll_per_x2_ck",
@@ -860,7 +869,7 @@ static const struct dss_features omap54xx_dss_feats __initconst = {
 	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
 };
 
-static const struct dss_features am43xx_dss_feats __initconst = {
+static const struct dss_features am43xx_dss_feats = {
 	.fck_div_max		=	0,
 	.dss_fck_multiplier	=	0,
 	.parent_clk_name	=	NULL,
@@ -869,7 +878,7 @@ static const struct dss_features am43xx_dss_feats __initconst = {
 	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
 };
 
-static const struct dss_features dra7xx_dss_feats __initconst = {
+static const struct dss_features dra7xx_dss_feats = {
 	.fck_div_max		=	64,
 	.dss_fck_multiplier	=	1,
 	.parent_clk_name	=	"dpll_per_x2_ck",
@@ -878,7 +887,7 @@ static const struct dss_features dra7xx_dss_feats __initconst = {
 	.num_ports		=	ARRAY_SIZE(dra7xx_ports),
 };
 
-static int __init dss_init_features(struct platform_device *pdev)
+static int dss_init_features(struct platform_device *pdev)
 {
 	const struct dss_features *src;
 	struct dss_features *dst;
@@ -932,7 +941,7 @@ static int __init dss_init_features(struct platform_device *pdev)
 	return 0;
 }
 
-static int __init dss_init_ports(struct platform_device *pdev)
+static int dss_init_ports(struct platform_device *pdev)
 {
 	struct device_node *parent = pdev->dev.of_node;
 	struct device_node *port;
@@ -976,7 +985,7 @@ static int __init dss_init_ports(struct platform_device *pdev)
 	return 0;
 }
 
-static void __exit dss_uninit_ports(struct platform_device *pdev)
+static void dss_uninit_ports(struct platform_device *pdev)
 {
 	struct device_node *parent = pdev->dev.of_node;
 	struct device_node *port;
@@ -1018,14 +1027,74 @@ static void __exit dss_uninit_ports(struct platform_device *pdev)
 	} while ((port = omapdss_of_get_next_port(parent, port)) != NULL);
 }
 
+static int dss_video_pll_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct regulator *pll_regulator;
+	int r;
+
+	if (!np)
+		return 0;
+
+	if (of_property_read_bool(np, "syscon-pll-ctrl")) {
+		dss.syscon_pll_ctrl = syscon_regmap_lookup_by_phandle(np,
+			"syscon-pll-ctrl");
+		if (IS_ERR(dss.syscon_pll_ctrl)) {
+			dev_err(&pdev->dev,
+				"failed to get syscon-pll-ctrl regmap\n");
+			return PTR_ERR(dss.syscon_pll_ctrl);
+		}
+
+		if (of_property_read_u32_index(np, "syscon-pll-ctrl", 1,
+				&dss.syscon_pll_ctrl_offset)) {
+			dev_err(&pdev->dev,
+				"failed to get syscon-pll-ctrl offset\n");
+			return -EINVAL;
+		}
+	}
+
+	pll_regulator = devm_regulator_get(&pdev->dev, "vdda_video");
+	if (IS_ERR(pll_regulator)) {
+		r = PTR_ERR(pll_regulator);
+
+		switch (r) {
+		case -ENOENT:
+			pll_regulator = NULL;
+			break;
+
+		case -EPROBE_DEFER:
+			return -EPROBE_DEFER;
+
+		default:
+			DSSERR("can't get DPLL VDDA regulator\n");
+			return r;
+		}
+	}
+
+	if (of_property_match_string(np, "reg-names", "pll1") >= 0) {
+		dss.video1_pll = dss_video_pll_init(pdev, 0, pll_regulator);
+		if (IS_ERR(dss.video1_pll))
+			return PTR_ERR(dss.video1_pll);
+	}
+
+	if (of_property_match_string(np, "reg-names", "pll2") >= 0) {
+		dss.video2_pll = dss_video_pll_init(pdev, 1, pll_regulator);
+		if (IS_ERR(dss.video2_pll)) {
+			dss_video_pll_uninit(dss.video1_pll);
+			return PTR_ERR(dss.video2_pll);
+		}
+	}
+
+	return 0;
+}
+
 /* DSS HW IP initialisation */
-static int __init omap_dsshw_probe(struct platform_device *pdev)
+static int dss_bind(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct resource *dss_mem;
-	struct device_node *np = pdev->dev.of_node;
 	u32 rev;
 	int r;
-	struct regulator *pll_regulator;
 
 	dss.pdev = pdev;
 
@@ -1054,6 +1123,14 @@ static int __init omap_dsshw_probe(struct platform_device *pdev)
 	if (r)
 		goto err_setup_clocks;
 
+	r = dss_video_pll_probe(pdev);
+	if (r)
+		goto err_pll_init;
+
+	r = dss_init_ports(pdev);
+	if (r)
+		goto err_init_ports;
+
 	pm_runtime_enable(&pdev->dev);
 
 	r = dss_runtime_get();
@@ -1078,86 +1155,48 @@ static int __init omap_dsshw_probe(struct platform_device *pdev)
 	dss.lcd_clk_source[0] = OMAP_DSS_CLK_SRC_FCK;
 	dss.lcd_clk_source[1] = OMAP_DSS_CLK_SRC_FCK;
 
-	dss_init_ports(pdev);
-
-	if (np && of_property_read_bool(np, "syscon-pll-ctrl")) {
-		dss.syscon_pll_ctrl = syscon_regmap_lookup_by_phandle(np,
-			"syscon-pll-ctrl");
-		if (IS_ERR(dss.syscon_pll_ctrl)) {
-			dev_err(&pdev->dev,
-				"failed to get syscon-pll-ctrl regmap\n");
-			return PTR_ERR(dss.syscon_pll_ctrl);
-		}
-
-		if (of_property_read_u32_index(np, "syscon-pll-ctrl", 1,
-				&dss.syscon_pll_ctrl_offset)) {
-			dev_err(&pdev->dev,
-				"failed to get syscon-pll-ctrl offset\n");
-			return -EINVAL;
-		}
-	}
-
-	pll_regulator = devm_regulator_get(&pdev->dev, "vdda_video");
-	if (IS_ERR(pll_regulator)) {
-		r = PTR_ERR(pll_regulator);
-
-		switch (r) {
-		case -ENOENT:
-			pll_regulator = NULL;
-			break;
-
-		case -EPROBE_DEFER:
-			return -EPROBE_DEFER;
-
-		default:
-			DSSERR("can't get DPLL VDDA regulator\n");
-			return r;
-		}
-	}
-
-	if (of_property_match_string(np, "reg-names", "pll1") >= 0) {
-		dss.video1_pll = dss_video_pll_init(pdev, 0, pll_regulator);
-		if (IS_ERR(dss.video1_pll)) {
-			r = PTR_ERR(dss.video1_pll);
-			goto err_pll_init;
-		}
-	}
-
-	if (of_property_match_string(np, "reg-names", "pll2") >= 0) {
-		dss.video2_pll = dss_video_pll_init(pdev, 1, pll_regulator);
-		if (IS_ERR(dss.video2_pll)) {
-			r = PTR_ERR(dss.video2_pll);
-			goto err_pll_init;
-		}
-	}
-
 	rev = dss_read_reg(DSS_REVISION);
 	printk(KERN_INFO "OMAP DSS rev %d.%d\n",
 			FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
 
 	dss_runtime_put();
 
+	r = component_bind_all(&pdev->dev, NULL);
+	if (r)
+		goto err_component;
+
 	dss_debugfs_create_file("dss", dss_dump_regs);
 
 	pm_set_vt_switch(0);
 
+	dss_initialized = true;
+
 	return 0;
 
-err_pll_init:
+err_component:
+err_runtime_get:
+	pm_runtime_disable(&pdev->dev);
+	dss_uninit_ports(pdev);
+err_init_ports:
 	if (dss.video1_pll)
 		dss_video_pll_uninit(dss.video1_pll);
 
 	if (dss.video2_pll)
 		dss_video_pll_uninit(dss.video2_pll);
-err_runtime_get:
-	pm_runtime_disable(&pdev->dev);
+err_pll_init:
 err_setup_clocks:
 	dss_put_clocks();
 	return r;
 }
 
-static int __exit omap_dsshw_remove(struct platform_device *pdev)
+static void dss_unbind(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
+	dss_initialized = false;
+
+	component_unbind_all(&pdev->dev, NULL);
+
 	if (dss.video1_pll)
 		dss_video_pll_uninit(dss.video1_pll);
 
@@ -1169,7 +1208,55 @@ static int __exit omap_dsshw_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 
 	dss_put_clocks();
+}
+
+static const struct component_master_ops dss_component_ops = {
+	.bind = dss_bind,
+	.unbind = dss_unbind,
+};
 
+static int dss_component_compare(struct device *dev, void *data)
+{
+	struct device *child = data;
+	return dev == child;
+}
+
+static int dss_add_child_component(struct device *dev, void *data)
+{
+	struct component_match **match = data;
+
+	/*
+	 * HACK
+	 * We don't have a working driver for rfbi, so skip it here always.
+	 * Otherwise dss will never get probed successfully, as it will wait
+	 * for rfbi to get probed.
+	 */
+	if (strstr(dev_name(dev), "rfbi"))
+		return 0;
+
+	component_match_add(dev->parent, match, dss_component_compare, dev);
+
+	return 0;
+}
+
+static int dss_probe(struct platform_device *pdev)
+{
+	struct component_match *match = NULL;
+	int r;
+
+	/* add all the child devices as components */
+	device_for_each_child(&pdev->dev, &match, dss_add_child_component);
+
+	r = component_master_add_with_match(&pdev->dev, &dss_component_ops, match);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static int dss_remove(struct platform_device *pdev)
+{
+	component_master_del(&pdev->dev, &dss_component_ops);
 	return 0;
 }
 
@@ -1215,7 +1302,8 @@ static const struct of_device_id dss_of_match[] = {
 MODULE_DEVICE_TABLE(of, dss_of_match);
 
 static struct platform_driver omap_dsshw_driver = {
-	.remove         = __exit_p(omap_dsshw_remove),
+	.probe		= dss_probe,
+	.remove		= dss_remove,
 	.driver         = {
 		.name   = "omapdss_dss",
 		.pm	= &dss_pm_ops,
@@ -1226,7 +1314,7 @@ static struct platform_driver omap_dsshw_driver = {
 
 int __init dss_init_platform_driver(void)
 {
-	return platform_driver_probe(&omap_dsshw_driver, omap_dsshw_probe);
+	return platform_driver_register(&omap_dsshw_driver);
 }
 
 void dss_uninit_platform_driver(void)
diff --git a/drivers/video/fbdev/omap2/dss/dss.h b/drivers/video/fbdev/omap2/dss/dss.h
index 4812eee2622a..2406bcdb831a 100644
--- a/drivers/video/fbdev/omap2/dss/dss.h
+++ b/drivers/video/fbdev/omap2/dss/dss.h
@@ -309,18 +309,18 @@ bool dss_div_calc(unsigned long pck, unsigned long fck_min,
 
 /* SDI */
 int sdi_init_platform_driver(void) __init;
-void sdi_uninit_platform_driver(void) __exit;
+void sdi_uninit_platform_driver(void);
 
 #ifdef CONFIG_OMAP2_DSS_SDI
-int sdi_init_port(struct platform_device *pdev, struct device_node *port) __init;
-void sdi_uninit_port(struct device_node *port) __exit;
+int sdi_init_port(struct platform_device *pdev, struct device_node *port);
+void sdi_uninit_port(struct device_node *port);
 #else
-static inline int __init sdi_init_port(struct platform_device *pdev,
+static inline int sdi_init_port(struct platform_device *pdev,
 		struct device_node *port)
 {
 	return 0;
 }
-static inline void __exit sdi_uninit_port(struct device_node *port)
+static inline void sdi_uninit_port(struct device_node *port)
 {
 }
 #endif
@@ -333,7 +333,7 @@ struct dentry;
 struct file_operations;
 
 int dsi_init_platform_driver(void) __init;
-void dsi_uninit_platform_driver(void) __exit;
+void dsi_uninit_platform_driver(void);
 
 void dsi_dump_clocks(struct seq_file *s);
 
@@ -350,25 +350,25 @@ static inline u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt)
 
 /* DPI */
 int dpi_init_platform_driver(void) __init;
-void dpi_uninit_platform_driver(void) __exit;
+void dpi_uninit_platform_driver(void);
 
 #ifdef CONFIG_OMAP2_DSS_DPI
-int dpi_init_port(struct platform_device *pdev, struct device_node *port) __init;
-void dpi_uninit_port(struct device_node *port) __exit;
+int dpi_init_port(struct platform_device *pdev, struct device_node *port);
+void dpi_uninit_port(struct device_node *port);
 #else
-static inline int __init dpi_init_port(struct platform_device *pdev,
+static inline int dpi_init_port(struct platform_device *pdev,
 		struct device_node *port)
 {
 	return 0;
 }
-static inline void __exit dpi_uninit_port(struct device_node *port)
+static inline void dpi_uninit_port(struct device_node *port)
 {
 }
 #endif
 
 /* DISPC */
 int dispc_init_platform_driver(void) __init;
-void dispc_uninit_platform_driver(void) __exit;
+void dispc_uninit_platform_driver(void);
 void dispc_dump_clocks(struct seq_file *s);
 
 void dispc_enable_sidle(void);
@@ -418,18 +418,18 @@ int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
 
 /* VENC */
 int venc_init_platform_driver(void) __init;
-void venc_uninit_platform_driver(void) __exit;
+void venc_uninit_platform_driver(void);
 
 /* HDMI */
 int hdmi4_init_platform_driver(void) __init;
-void hdmi4_uninit_platform_driver(void) __exit;
+void hdmi4_uninit_platform_driver(void);
 
 int hdmi5_init_platform_driver(void) __init;
-void hdmi5_uninit_platform_driver(void) __exit;
+void hdmi5_uninit_platform_driver(void);
 
 /* RFBI */
 int rfbi_init_platform_driver(void) __init;
-void rfbi_uninit_platform_driver(void) __exit;
+void rfbi_uninit_platform_driver(void);
 
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4.c b/drivers/video/fbdev/omap2/dss/hdmi4.c
index 916d47978f41..6d3aa3f51c20 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi4.c
@@ -32,6 +32,7 @@
 #include <linux/clk.h>
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
+#include <linux/component.h>
 #include <video/omapdss.h>
 #include <sound/omap-hdmi-audio.h>
 
@@ -229,9 +230,9 @@ static int hdmi_power_on_full(struct omap_dss_device *dssdev)
 err_mgr_enable:
 	hdmi_wp_video_stop(&hdmi.wp);
 err_vid_enable:
-err_phy_cfg:
 	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
 err_phy_pwr:
+err_phy_cfg:
 err_pll_cfg:
 	dss_pll_disable(&hdmi.pll.pll);
 err_pll_enable:
@@ -646,8 +647,9 @@ static int hdmi_audio_register(struct device *dev)
 }
 
 /* HDMI HW IP initialisation */
-static int omapdss_hdmihw_probe(struct platform_device *pdev)
+static int hdmi4_bind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	int r;
 	int irq;
 
@@ -713,8 +715,10 @@ err:
 	return r;
 }
 
-static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
+static void hdmi4_unbind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	if (hdmi.audio_pdev)
 		platform_device_unregister(hdmi.audio_pdev);
 
@@ -723,7 +727,21 @@ static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
 	hdmi_pll_uninit(&hdmi.pll);
 
 	pm_runtime_disable(&pdev->dev);
+}
+
+static const struct component_ops hdmi4_component_ops = {
+	.bind	= hdmi4_bind,
+	.unbind	= hdmi4_unbind,
+};
 
+static int hdmi4_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &hdmi4_component_ops);
+}
+
+static int hdmi4_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &hdmi4_component_ops);
 	return 0;
 }
 
@@ -756,8 +774,8 @@ static const struct of_device_id hdmi_of_match[] = {
 };
 
 static struct platform_driver omapdss_hdmihw_driver = {
-	.probe		= omapdss_hdmihw_probe,
-	.remove         = __exit_p(omapdss_hdmihw_remove),
+	.probe		= hdmi4_probe,
+	.remove		= hdmi4_remove,
 	.driver         = {
 		.name   = "omapdss_hdmi",
 		.pm	= &hdmi_pm_ops,
@@ -771,7 +789,7 @@ int __init hdmi4_init_platform_driver(void)
 	return platform_driver_register(&omapdss_hdmihw_driver);
 }
 
-void __exit hdmi4_uninit_platform_driver(void)
+void hdmi4_uninit_platform_driver(void)
 {
 	platform_driver_unregister(&omapdss_hdmihw_driver);
 }
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4_core.c b/drivers/video/fbdev/omap2/dss/hdmi4_core.c
index 7eafea5b8e19..fa72e735dad2 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4_core.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi4_core.c
@@ -654,6 +654,13 @@ static void hdmi_core_audio_infoframe_cfg(struct hdmi_core_data *core,
 	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(2), info_aud->db3);
 	sum += info_aud->db3;
 
+	/*
+	 * The OMAP HDMI IP requires to use the 8-channel channel code when
+	 * transmitting more than two channels.
+	 */
+	if (info_aud->db4_ca != 0x00)
+		info_aud->db4_ca = 0x13;
+
 	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(3), info_aud->db4_ca);
 	sum += info_aud->db4_ca;
 
@@ -795,7 +802,9 @@ int hdmi4_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
 
 	/*
 	 * the HDMI IP needs to enable four stereo channels when transmitting
-	 * more than 2 audio channels
+	 * more than 2 audio channels.  Similarly, the channel count in the
+	 * Audio InfoFrame has to match the sample_present bits (some channels
+	 * are padded with zeroes)
 	 */
 	if (channel_count == 2) {
 		audio_format.stereo_channels = HDMI_AUDIO_STEREO_ONECHANNEL;
@@ -807,6 +816,7 @@ int hdmi4_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
 				HDMI_AUDIO_I2S_SD1_EN | HDMI_AUDIO_I2S_SD2_EN |
 				HDMI_AUDIO_I2S_SD3_EN;
 		acore.layout = HDMI_AUDIO_LAYOUT_8CH;
+		audio->cea->db1_ct_cc = 7;
 	}
 
 	acore.en_spdif = false;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5.c b/drivers/video/fbdev/omap2/dss/hdmi5.c
index 3f0b34a7031a..7f875788edbc 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi5.c
@@ -37,6 +37,7 @@
 #include <linux/clk.h>
 #include <linux/gpio.h>
 #include <linux/regulator/consumer.h>
+#include <linux/component.h>
 #include <video/omapdss.h>
 #include <sound/omap-hdmi-audio.h>
 
@@ -681,8 +682,9 @@ static int hdmi_audio_register(struct device *dev)
 }
 
 /* HDMI HW IP initialisation */
-static int omapdss_hdmihw_probe(struct platform_device *pdev)
+static int hdmi5_bind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	int r;
 	int irq;
 
@@ -748,8 +750,10 @@ err:
 	return r;
 }
 
-static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
+static void hdmi5_unbind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	if (hdmi.audio_pdev)
 		platform_device_unregister(hdmi.audio_pdev);
 
@@ -758,7 +762,21 @@ static int __exit omapdss_hdmihw_remove(struct platform_device *pdev)
 	hdmi_pll_uninit(&hdmi.pll);
 
 	pm_runtime_disable(&pdev->dev);
+}
+
+static const struct component_ops hdmi5_component_ops = {
+	.bind	= hdmi5_bind,
+	.unbind	= hdmi5_unbind,
+};
 
+static int hdmi5_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &hdmi5_component_ops);
+}
+
+static int hdmi5_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &hdmi5_component_ops);
 	return 0;
 }
 
@@ -792,8 +810,8 @@ static const struct of_device_id hdmi_of_match[] = {
 };
 
 static struct platform_driver omapdss_hdmihw_driver = {
-	.probe		= omapdss_hdmihw_probe,
-	.remove         = __exit_p(omapdss_hdmihw_remove),
+	.probe		= hdmi5_probe,
+	.remove		= hdmi5_remove,
 	.driver         = {
 		.name   = "omapdss_hdmi5",
 		.pm	= &hdmi_pm_ops,
@@ -807,7 +825,7 @@ int __init hdmi5_init_platform_driver(void)
 	return platform_driver_register(&omapdss_hdmihw_driver);
 }
 
-void __exit hdmi5_uninit_platform_driver(void)
+void hdmi5_uninit_platform_driver(void)
 {
 	platform_driver_unregister(&omapdss_hdmihw_driver);
 }
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5_core.c b/drivers/video/fbdev/omap2/dss/hdmi5_core.c
index bfc0c4c297d6..8ea531d2652c 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5_core.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi5_core.c
@@ -790,7 +790,9 @@ static void hdmi5_core_audio_infoframe_cfg(struct hdmi_core_data *core,
 
 	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF1, info_aud->db2_sf_ss);
 	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF2, info_aud->db4_ca);
-	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF3, info_aud->db5_dminh_lsv);
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF3,
+	  (info_aud->db5_dminh_lsv & CEA861_AUDIO_INFOFRAME_DB5_DM_INH) >> 3 |
+	  (info_aud->db5_dminh_lsv & CEA861_AUDIO_INFOFRAME_DB5_LSV));
 }
 
 int hdmi5_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
@@ -870,6 +872,7 @@ int hdmi5_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
 	audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_TWOSAMPLES;
 	audio_format.sample_size = HDMI_AUDIO_SAMPLE_16BITS;
 	audio_format.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+	audio_format.sample_order = HDMI_AUDIO_SAMPLE_LEFT_FIRST;
 
 	/* only LPCM atm */
 	audio_format.type = HDMI_AUDIO_TYPE_LPCM;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_wp.c b/drivers/video/fbdev/omap2/dss/hdmi_wp.c
index c15377e242cc..7c544bc56fb5 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_wp.c
+++ b/drivers/video/fbdev/omap2/dss/hdmi_wp.c
@@ -110,7 +110,23 @@ int hdmi_wp_video_start(struct hdmi_wp_data *wp)
 
 void hdmi_wp_video_stop(struct hdmi_wp_data *wp)
 {
+	int i;
+
+	hdmi_write_reg(wp->base, HDMI_WP_IRQSTATUS, HDMI_IRQ_VIDEO_FRAME_DONE);
+
 	REG_FLD_MOD(wp->base, HDMI_WP_VIDEO_CFG, false, 31, 31);
+
+	for (i = 0; i < 50; ++i) {
+		u32 v;
+
+		msleep(20);
+
+		v = hdmi_read_reg(wp->base, HDMI_WP_IRQSTATUS_RAW);
+		if (v & HDMI_IRQ_VIDEO_FRAME_DONE)
+			return;
+	}
+
+	DSSERR("no HDMI FRAMEDONE when disabling output\n");
 }
 
 void hdmi_wp_video_config_format(struct hdmi_wp_data *wp,
diff --git a/drivers/video/fbdev/omap2/dss/rfbi.c b/drivers/video/fbdev/omap2/dss/rfbi.c
index 065effca9236..1525a494d057 100644
--- a/drivers/video/fbdev/omap2/dss/rfbi.c
+++ b/drivers/video/fbdev/omap2/dss/rfbi.c
@@ -36,6 +36,7 @@
 #include <linux/semaphore.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/component.h>
 
 #include <video/omapdss.h>
 #include "dss.h"
@@ -938,7 +939,7 @@ static void rfbi_init_output(struct platform_device *pdev)
 	omapdss_register_output(out);
 }
 
-static void __exit rfbi_uninit_output(struct platform_device *pdev)
+static void rfbi_uninit_output(struct platform_device *pdev)
 {
 	struct omap_dss_device *out = &rfbi.output;
 
@@ -946,8 +947,9 @@ static void __exit rfbi_uninit_output(struct platform_device *pdev)
 }
 
 /* RFBI HW IP initialisation */
-static int omap_rfbihw_probe(struct platform_device *pdev)
+static int rfbi_bind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	u32 rev;
 	struct resource *rfbi_mem;
 	struct clk *clk;
@@ -1005,8 +1007,10 @@ err_runtime_get:
 	return r;
 }
 
-static int __exit omap_rfbihw_remove(struct platform_device *pdev)
+static void rfbi_unbind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	rfbi_uninit_output(pdev);
 
 	pm_runtime_disable(&pdev->dev);
@@ -1014,6 +1018,22 @@ static int __exit omap_rfbihw_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct component_ops rfbi_component_ops = {
+	.bind	= rfbi_bind,
+	.unbind	= rfbi_unbind,
+};
+
+static int rfbi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &rfbi_component_ops);
+}
+
+static int rfbi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &rfbi_component_ops);
+	return 0;
+}
+
 static int rfbi_runtime_suspend(struct device *dev)
 {
 	dispc_runtime_put();
@@ -1038,8 +1058,8 @@ static const struct dev_pm_ops rfbi_pm_ops = {
 };
 
 static struct platform_driver omap_rfbihw_driver = {
-	.probe		= omap_rfbihw_probe,
-	.remove         = __exit_p(omap_rfbihw_remove),
+	.probe		= rfbi_probe,
+	.remove         = rfbi_remove,
 	.driver         = {
 		.name   = "omapdss_rfbi",
 		.pm	= &rfbi_pm_ops,
@@ -1052,7 +1072,7 @@ int __init rfbi_init_platform_driver(void)
 	return platform_driver_register(&omap_rfbihw_driver);
 }
 
-void __exit rfbi_uninit_platform_driver(void)
+void rfbi_uninit_platform_driver(void)
 {
 	platform_driver_unregister(&omap_rfbihw_driver);
 }
diff --git a/drivers/video/fbdev/omap2/dss/sdi.c b/drivers/video/fbdev/omap2/dss/sdi.c
index 5c2ccab5a958..5843580a1deb 100644
--- a/drivers/video/fbdev/omap2/dss/sdi.c
+++ b/drivers/video/fbdev/omap2/dss/sdi.c
@@ -27,6 +27,7 @@
 #include <linux/platform_device.h>
 #include <linux/string.h>
 #include <linux/of.h>
+#include <linux/component.h>
 
 #include <video/omapdss.h>
 #include "dss.h"
@@ -350,15 +351,17 @@ static void sdi_init_output(struct platform_device *pdev)
 	omapdss_register_output(out);
 }
 
-static void __exit sdi_uninit_output(struct platform_device *pdev)
+static void sdi_uninit_output(struct platform_device *pdev)
 {
 	struct omap_dss_device *out = &sdi.output;
 
 	omapdss_unregister_output(out);
 }
 
-static int omap_sdi_probe(struct platform_device *pdev)
+static int sdi_bind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	sdi.pdev = pdev;
 
 	sdi_init_output(pdev);
@@ -366,16 +369,32 @@ static int omap_sdi_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int __exit omap_sdi_remove(struct platform_device *pdev)
+static void sdi_unbind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	sdi_uninit_output(pdev);
+}
+
+static const struct component_ops sdi_component_ops = {
+	.bind	= sdi_bind,
+	.unbind	= sdi_unbind,
+};
 
+static int sdi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &sdi_component_ops);
+}
+
+static int sdi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &sdi_component_ops);
 	return 0;
 }
 
 static struct platform_driver omap_sdi_driver = {
-	.probe		= omap_sdi_probe,
-	.remove         = __exit_p(omap_sdi_remove),
+	.probe		= sdi_probe,
+	.remove         = sdi_remove,
 	.driver         = {
 		.name   = "omapdss_sdi",
 		.suppress_bind_attrs = true,
@@ -387,12 +406,12 @@ int __init sdi_init_platform_driver(void)
 	return platform_driver_register(&omap_sdi_driver);
 }
 
-void __exit sdi_uninit_platform_driver(void)
+void sdi_uninit_platform_driver(void)
 {
 	platform_driver_unregister(&omap_sdi_driver);
 }
 
-int __init sdi_init_port(struct platform_device *pdev, struct device_node *port)
+int sdi_init_port(struct platform_device *pdev, struct device_node *port)
 {
 	struct device_node *ep;
 	u32 datapairs;
@@ -426,7 +445,7 @@ err_datapairs:
 	return r;
 }
 
-void __exit sdi_uninit_port(struct device_node *port)
+void sdi_uninit_port(struct device_node *port)
 {
 	if (!sdi.port_initialized)
 		return;
diff --git a/drivers/video/fbdev/omap2/dss/venc.c b/drivers/video/fbdev/omap2/dss/venc.c
index ef7fd925e7f2..99ca268c1cdd 100644
--- a/drivers/video/fbdev/omap2/dss/venc.c
+++ b/drivers/video/fbdev/omap2/dss/venc.c
@@ -35,6 +35,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
 #include <linux/of.h>
+#include <linux/component.h>
 
 #include <video/omapdss.h>
 
@@ -802,7 +803,7 @@ static void venc_init_output(struct platform_device *pdev)
 	omapdss_register_output(out);
 }
 
-static void __exit venc_uninit_output(struct platform_device *pdev)
+static void venc_uninit_output(struct platform_device *pdev)
 {
 	struct omap_dss_device *out = &venc.output;
 
@@ -852,8 +853,9 @@ err:
 }
 
 /* VENC HW IP initialisation */
-static int omap_venchw_probe(struct platform_device *pdev)
+static int venc_bind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	u8 rev_id;
 	struct resource *venc_mem;
 	int r;
@@ -912,12 +914,28 @@ err_runtime_get:
 	return r;
 }
 
-static int __exit omap_venchw_remove(struct platform_device *pdev)
+static void venc_unbind(struct device *dev, struct device *master, void *data)
 {
+	struct platform_device *pdev = to_platform_device(dev);
+
 	venc_uninit_output(pdev);
 
 	pm_runtime_disable(&pdev->dev);
+}
 
+static const struct component_ops venc_component_ops = {
+	.bind	= venc_bind,
+	.unbind	= venc_unbind,
+};
+
+static int venc_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &venc_component_ops);
+}
+
+static int venc_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &venc_component_ops);
 	return 0;
 }
 
@@ -950,7 +968,6 @@ static const struct dev_pm_ops venc_pm_ops = {
 	.runtime_resume = venc_runtime_resume,
 };
 
-
 static const struct of_device_id venc_of_match[] = {
 	{ .compatible = "ti,omap2-venc", },
 	{ .compatible = "ti,omap3-venc", },
@@ -959,8 +976,8 @@ static const struct of_device_id venc_of_match[] = {
 };
 
 static struct platform_driver omap_venchw_driver = {
-	.probe		= omap_venchw_probe,
-	.remove         = __exit_p(omap_venchw_remove),
+	.probe		= venc_probe,
+	.remove		= venc_remove,
 	.driver         = {
 		.name   = "omapdss_venc",
 		.pm	= &venc_pm_ops,
@@ -974,7 +991,7 @@ int __init venc_init_platform_driver(void)
 	return platform_driver_register(&omap_venchw_driver);
 }
 
-void __exit venc_uninit_platform_driver(void)
+void venc_uninit_platform_driver(void)
 {
 	platform_driver_unregister(&omap_venchw_driver);
 }
diff --git a/drivers/video/fbdev/pm2fb.c b/drivers/video/fbdev/pm2fb.c
index 3b85b647bc10..aa8d28880912 100644
--- a/drivers/video/fbdev/pm2fb.c
+++ b/drivers/video/fbdev/pm2fb.c
@@ -38,10 +38,6 @@
 #include <linux/fb.h>
 #include <linux/init.h>
 #include <linux/pci.h>
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
-
 #include <video/permedia2.h>
 #include <video/cvisionppc.h>
 
@@ -81,10 +77,7 @@ static char *mode_option;
 static bool lowhsync;
 static bool lowvsync;
 static bool noaccel;
-/* mtrr option */
-#ifdef CONFIG_MTRR
 static bool nomtrr;
-#endif
 
 /*
  * The hardware state of the graphics card that isn't part of the
@@ -100,7 +93,7 @@ struct pm2fb_par
 	u32		mem_control;	/* MemControl reg at probe */
 	u32		boot_address;	/* BootAddress reg at probe */
 	u32		palette[16];
-	int		mtrr_handle;
+	int		wc_cookie;
 };
 
 /*
@@ -1637,21 +1630,16 @@ static int pm2fb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto err_exit_mmio;
 	}
 	info->screen_base =
-		ioremap_nocache(pm2fb_fix.smem_start, pm2fb_fix.smem_len);
+		ioremap_wc(pm2fb_fix.smem_start, pm2fb_fix.smem_len);
 	if (!info->screen_base) {
 		printk(KERN_WARNING "pm2fb: Can't ioremap smem area.\n");
 		release_mem_region(pm2fb_fix.smem_start, pm2fb_fix.smem_len);
 		goto err_exit_mmio;
 	}
 
-#ifdef CONFIG_MTRR
-	default_par->mtrr_handle = -1;
 	if (!nomtrr)
-		default_par->mtrr_handle =
-			mtrr_add(pm2fb_fix.smem_start,
-				 pm2fb_fix.smem_len,
-				 MTRR_TYPE_WRCOMB, 1);
-#endif
+		default_par->wc_cookie = arch_phys_wc_add(pm2fb_fix.smem_start,
+							  pm2fb_fix.smem_len);
 
 	info->fbops		= &pm2fb_ops;
 	info->fix		= pm2fb_fix;
@@ -1733,12 +1721,7 @@ static void pm2fb_remove(struct pci_dev *pdev)
 	struct pm2fb_par *par = info->par;
 
 	unregister_framebuffer(info);
-
-#ifdef CONFIG_MTRR
-	if (par->mtrr_handle >= 0)
-		mtrr_del(par->mtrr_handle, info->fix.smem_start,
-			 info->fix.smem_len);
-#endif /* CONFIG_MTRR */
+	arch_phys_wc_del(par->wc_cookie);
 	iounmap(info->screen_base);
 	release_mem_region(fix->smem_start, fix->smem_len);
 	iounmap(par->v_regs);
@@ -1791,10 +1774,8 @@ static int __init pm2fb_setup(char *options)
 			lowvsync = 1;
 		else if (!strncmp(this_opt, "hwcursor=", 9))
 			hwcursor = simple_strtoul(this_opt + 9, NULL, 0);
-#ifdef CONFIG_MTRR
 		else if (!strncmp(this_opt, "nomtrr", 6))
 			nomtrr = 1;
-#endif
 		else if (!strncmp(this_opt, "noaccel", 7))
 			noaccel = 1;
 		else
@@ -1847,10 +1828,8 @@ MODULE_PARM_DESC(noaccel, "Disable acceleration");
 module_param(hwcursor, int, 0644);
 MODULE_PARM_DESC(hwcursor, "Enable hardware cursor "
 			"(1=enable, 0=disable, default=1)");
-#ifdef CONFIG_MTRR
 module_param(nomtrr, bool, 0);
 MODULE_PARM_DESC(nomtrr, "Disable MTRR support (0 or 1=disabled) (default=0)");
-#endif
 
 MODULE_AUTHOR("Jim Hague <jim.hague@acm.org>");
 MODULE_DESCRIPTION("Permedia2 framebuffer device driver");
diff --git a/drivers/video/fbdev/pm3fb.c b/drivers/video/fbdev/pm3fb.c
index 77b99ed39ad0..6ff5077a2e15 100644
--- a/drivers/video/fbdev/pm3fb.c
+++ b/drivers/video/fbdev/pm3fb.c
@@ -32,9 +32,6 @@
 #include <linux/fb.h>
 #include <linux/init.h>
 #include <linux/pci.h>
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
 
 #include <video/pm3fb.h>
 
@@ -58,11 +55,7 @@
 static int hwcursor = 1;
 static char *mode_option;
 static bool noaccel;
-
-/* mtrr option */
-#ifdef CONFIG_MTRR
 static bool nomtrr;
-#endif
 
 /*
  * This structure defines the hardware state of the graphics card. Normally
@@ -76,7 +69,7 @@ struct pm3_par {
 	u32		video;		/* video flags before blanking */
 	u32		base;		/* screen base in 128 bits unit */
 	u32		palette[16];
-	int		mtrr_handle;
+	int		wc_cookie;
 };
 
 /*
@@ -1374,8 +1367,8 @@ static int pm3fb_probe(struct pci_dev *dev, const struct pci_device_id *ent)
 		printk(KERN_WARNING "pm3fb: Can't reserve smem.\n");
 		goto err_exit_mmio;
 	}
-	info->screen_base =
-		ioremap_nocache(pm3fb_fix.smem_start, pm3fb_fix.smem_len);
+	info->screen_base = ioremap_wc(pm3fb_fix.smem_start,
+				       pm3fb_fix.smem_len);
 	if (!info->screen_base) {
 		printk(KERN_WARNING "pm3fb: Can't ioremap smem area.\n");
 		release_mem_region(pm3fb_fix.smem_start, pm3fb_fix.smem_len);
@@ -1383,12 +1376,9 @@ static int pm3fb_probe(struct pci_dev *dev, const struct pci_device_id *ent)
 	}
 	info->screen_size = pm3fb_fix.smem_len;
 
-#ifdef CONFIG_MTRR
 	if (!nomtrr)
-		par->mtrr_handle = mtrr_add(pm3fb_fix.smem_start,
-						pm3fb_fix.smem_len,
-						MTRR_TYPE_WRCOMB, 1);
-#endif
+		par->wc_cookie = arch_phys_wc_add(pm3fb_fix.smem_start,
+						  pm3fb_fix.smem_len);
 	info->fbops = &pm3fb_ops;
 
 	par->video = PM3_READ_REG(par, PM3VideoControl);
@@ -1478,11 +1468,7 @@ static void pm3fb_remove(struct pci_dev *dev)
 		unregister_framebuffer(info);
 		fb_dealloc_cmap(&info->cmap);
 
-#ifdef CONFIG_MTRR
-		if (par->mtrr_handle >= 0)
-			mtrr_del(par->mtrr_handle, info->fix.smem_start,
-				 info->fix.smem_len);
-#endif /* CONFIG_MTRR */
+		arch_phys_wc_del(par->wc_cookie);
 		iounmap(info->screen_base);
 		release_mem_region(fix->smem_start, fix->smem_len);
 		iounmap(par->v_regs);
@@ -1533,10 +1519,8 @@ static int __init pm3fb_setup(char *options)
 			noaccel = 1;
 		else if (!strncmp(this_opt, "hwcursor=", 9))
 			hwcursor = simple_strtoul(this_opt + 9, NULL, 0);
-#ifdef CONFIG_MTRR
 		else if (!strncmp(this_opt, "nomtrr", 6))
 			nomtrr = 1;
-#endif
 		else
 			mode_option = this_opt;
 	}
@@ -1577,10 +1561,8 @@ MODULE_PARM_DESC(noaccel, "Disable acceleration");
 module_param(hwcursor, int, 0644);
 MODULE_PARM_DESC(hwcursor, "Enable hardware cursor "
 			"(1=enable, 0=disable, default=1)");
-#ifdef CONFIG_MTRR
 module_param(nomtrr, bool, 0);
 MODULE_PARM_DESC(nomtrr, "Disable MTRR support (0 or 1=disabled) (default=0)");
-#endif
 
 MODULE_DESCRIPTION("Permedia3 framebuffer device driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/riva/fbdev.c b/drivers/video/fbdev/riva/fbdev.c
index 294a80908c8c..f1ad2747064b 100644
--- a/drivers/video/fbdev/riva/fbdev.c
+++ b/drivers/video/fbdev/riva/fbdev.c
@@ -41,9 +41,6 @@
 #include <linux/pci.h>
 #include <linux/backlight.h>
 #include <linux/bitrev.h>
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
 #ifdef CONFIG_PMAC_BACKLIGHT
 #include <asm/machdep.h>
 #include <asm/backlight.h>
@@ -204,9 +201,7 @@ MODULE_DEVICE_TABLE(pci, rivafb_pci_tbl);
 static int flatpanel = -1; /* Autodetect later */
 static int forceCRTC = -1;
 static bool noaccel  = 0;
-#ifdef CONFIG_MTRR
 static bool nomtrr = 0;
-#endif
 #ifdef CONFIG_PMAC_BACKLIGHT
 static int backlight = 1;
 #else
@@ -2010,28 +2005,18 @@ static int rivafb_probe(struct pci_dev *pd, const struct pci_device_id *ent)
 
 	rivafb_fix.smem_len = riva_get_memlen(default_par) * 1024;
 	default_par->dclk_max = riva_get_maxdclk(default_par) * 1000;
-	info->screen_base = ioremap(rivafb_fix.smem_start,
-				    rivafb_fix.smem_len);
+	info->screen_base = ioremap_wc(rivafb_fix.smem_start,
+				       rivafb_fix.smem_len);
 	if (!info->screen_base) {
 		printk(KERN_ERR PFX "cannot ioremap FB base\n");
 		ret = -EIO;
 		goto err_iounmap_pramin;
 	}
 
-#ifdef CONFIG_MTRR
-	if (!nomtrr) {
-		default_par->mtrr.vram = mtrr_add(rivafb_fix.smem_start,
-					   	  rivafb_fix.smem_len,
-					    	  MTRR_TYPE_WRCOMB, 1);
-		if (default_par->mtrr.vram < 0) {
-			printk(KERN_ERR PFX "unable to setup MTRR\n");
-		} else {
-			default_par->mtrr.vram_valid = 1;
-			/* let there be speed */
-			printk(KERN_INFO PFX "RIVA MTRR set to ON\n");
-		}
-	}
-#endif /* CONFIG_MTRR */
+	if (!nomtrr)
+		default_par->wc_cookie =
+			arch_phys_wc_add(rivafb_fix.smem_start,
+					 rivafb_fix.smem_len);
 
 	info->fbops = &riva_fb_ops;
 	info->fix = rivafb_fix;
@@ -2105,13 +2090,7 @@ static void rivafb_remove(struct pci_dev *pd)
 	unregister_framebuffer(info);
 
 	riva_bl_exit(info);
-
-#ifdef CONFIG_MTRR
-	if (par->mtrr.vram_valid)
-		mtrr_del(par->mtrr.vram, info->fix.smem_start,
-			 info->fix.smem_len);
-#endif /* CONFIG_MTRR */
-
+	arch_phys_wc_del(par->wc_cookie);
 	iounmap(par->ctrl_base);
 	iounmap(info->screen_base);
 	if (par->riva.Architecture == NV_ARCH_03)
@@ -2150,10 +2129,8 @@ static int rivafb_setup(char *options)
 			flatpanel = 1;
 		} else if (!strncmp(this_opt, "backlight:", 10)) {
 			backlight = simple_strtoul(this_opt+10, NULL, 0);
-#ifdef CONFIG_MTRR
 		} else if (!strncmp(this_opt, "nomtrr", 6)) {
 			nomtrr = 1;
-#endif
 		} else if (!strncmp(this_opt, "strictmode", 10)) {
 			strictmode = 1;
 		} else if (!strncmp(this_opt, "noaccel", 7)) {
@@ -2209,10 +2186,8 @@ module_param(flatpanel, int, 0);
 MODULE_PARM_DESC(flatpanel, "Enables experimental flat panel support for some chipsets. (0 or 1=enabled) (default=0)");
 module_param(forceCRTC, int, 0);
 MODULE_PARM_DESC(forceCRTC, "Forces usage of a particular CRTC in case autodetection fails. (0 or 1) (default=autodetect)");
-#ifdef CONFIG_MTRR
 module_param(nomtrr, bool, 0);
 MODULE_PARM_DESC(nomtrr, "Disables MTRR support (0 or 1=disabled) (default=0)");
-#endif
 module_param(strictmode, bool, 0);
 MODULE_PARM_DESC(strictmode, "Only use video modes from EDID");
 
diff --git a/drivers/video/fbdev/riva/rivafb.h b/drivers/video/fbdev/riva/rivafb.h
index d9f107b704c6..61fd37ca490a 100644
--- a/drivers/video/fbdev/riva/rivafb.h
+++ b/drivers/video/fbdev/riva/rivafb.h
@@ -61,9 +61,7 @@ struct riva_par {
 	int FlatPanel;
 	struct pci_dev *pdev;
 	int cursor_reset;
-#ifdef CONFIG_MTRR
-	struct { int vram; int vram_valid; } mtrr;
-#endif
+	int wc_cookie;
 	struct riva_i2c_chan chan[3];
 };
 
diff --git a/drivers/video/fbdev/savage/savagefb.h b/drivers/video/fbdev/savage/savagefb.h
index 8ff4ab1cb69b..aba04afe712d 100644
--- a/drivers/video/fbdev/savage/savagefb.h
+++ b/drivers/video/fbdev/savage/savagefb.h
@@ -213,9 +213,7 @@ struct savagefb_par {
 		void   __iomem *vbase;
 		u32    pbase;
 		u32    len;
-#ifdef CONFIG_MTRR
-		int    mtrr;
-#endif
+		int    wc_cookie;
 	} video;
 
 	struct {
diff --git a/drivers/video/fbdev/savage/savagefb_driver.c b/drivers/video/fbdev/savage/savagefb_driver.c
index 4dbf45f3b21a..6c77ab09b0b2 100644
--- a/drivers/video/fbdev/savage/savagefb_driver.c
+++ b/drivers/video/fbdev/savage/savagefb_driver.c
@@ -57,10 +57,6 @@
 #include <asm/irq.h>
 #include <asm/pgtable.h>
 
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
-
 #include "savagefb.h"
 
 
@@ -1775,7 +1771,7 @@ static int savage_map_video(struct fb_info *info, int video_len)
 
 	par->video.pbase = pci_resource_start(par->pcidev, resource);
 	par->video.len   = video_len;
-	par->video.vbase = ioremap(par->video.pbase, par->video.len);
+	par->video.vbase = ioremap_wc(par->video.pbase, par->video.len);
 
 	if (!par->video.vbase) {
 		printk("savagefb: unable to map screen memory\n");
@@ -1787,11 +1783,7 @@ static int savage_map_video(struct fb_info *info, int video_len)
 	info->fix.smem_start = par->video.pbase;
 	info->fix.smem_len   = par->video.len - par->cob_size;
 	info->screen_base    = par->video.vbase;
-
-#ifdef CONFIG_MTRR
-	par->video.mtrr = mtrr_add(par->video.pbase, video_len,
-				   MTRR_TYPE_WRCOMB, 1);
-#endif
+	par->video.wc_cookie = arch_phys_wc_add(par->video.pbase, video_len);
 
 	/* Clear framebuffer, it's all white in memory after boot */
 	memset_io(par->video.vbase, 0, par->video.len);
@@ -1806,10 +1798,7 @@ static void savage_unmap_video(struct fb_info *info)
 	DBG("savage_unmap_video");
 
 	if (par->video.vbase) {
-#ifdef CONFIG_MTRR
-		mtrr_del(par->video.mtrr, par->video.pbase, par->video.len);
-#endif
-
+		arch_phys_wc_del(par->video.wc_cookie);
 		iounmap(par->video.vbase);
 		par->video.vbase = NULL;
 		info->screen_base = NULL;
diff --git a/drivers/video/fbdev/sis/sis.h b/drivers/video/fbdev/sis/sis.h
index 1987f1b7212f..ea1d1c9640bf 100644
--- a/drivers/video/fbdev/sis/sis.h
+++ b/drivers/video/fbdev/sis/sis.h
@@ -458,7 +458,7 @@ struct sis_video_info {
 
 	unsigned char	*bios_abase;
 
-	int		mtrr;
+	int		wc_cookie;
 
 	u32		sisfb_mem;
 
diff --git a/drivers/video/fbdev/sis/sis_main.c b/drivers/video/fbdev/sis/sis_main.c
index fcf610edf217..e92303823a4b 100644
--- a/drivers/video/fbdev/sis/sis_main.c
+++ b/drivers/video/fbdev/sis/sis_main.c
@@ -53,9 +53,6 @@
 #include <linux/types.h>
 #include <linux/uaccess.h>
 #include <asm/io.h>
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#endif
 
 #include "sis.h"
 #include "sis_main.h"
@@ -4130,13 +4127,13 @@ static void sisfb_post_map_vram(struct sis_video_info *ivideo,
 	if (*mapsize < (min << 20))
 		return;
 
-	ivideo->video_vbase = ioremap(ivideo->video_base, (*mapsize));
+	ivideo->video_vbase = ioremap_wc(ivideo->video_base, (*mapsize));
 
 	if(!ivideo->video_vbase) {
 		printk(KERN_ERR
 			"sisfb: Unable to map maximum video RAM for size detection\n");
 		(*mapsize) >>= 1;
-		while((!(ivideo->video_vbase = ioremap(ivideo->video_base, (*mapsize))))) {
+		while((!(ivideo->video_vbase = ioremap_wc(ivideo->video_base, (*mapsize))))) {
 			(*mapsize) >>= 1;
 			if((*mapsize) < (min << 20))
 				break;
@@ -6186,7 +6183,7 @@ static int sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto error_2;
 	}
 
-	ivideo->video_vbase = ioremap(ivideo->video_base, ivideo->video_size);
+	ivideo->video_vbase = ioremap_wc(ivideo->video_base, ivideo->video_size);
 	ivideo->SiS_Pr.VideoMemoryAddress = ivideo->video_vbase;
 	if(!ivideo->video_vbase) {
 		printk(KERN_ERR "sisfb: Fatal error: Unable to map framebuffer memory\n");
@@ -6254,8 +6251,6 @@ error_3:	vfree(ivideo->bios_abase);
 	ivideo->SiS_Pr.VideoMemoryAddress += ivideo->video_offset;
 	ivideo->SiS_Pr.VideoMemorySize = ivideo->sisfb_mem;
 
-	ivideo->mtrr = -1;
-
 	ivideo->vbflags = 0;
 	ivideo->lcddefmodeidx = DEFAULT_LCDMODE;
 	ivideo->tvdefmodeidx  = DEFAULT_TVMODE;
@@ -6443,14 +6438,8 @@ error_3:	vfree(ivideo->bios_abase);
 
 		printk(KERN_DEBUG "sisfb: Initial vbflags 0x%x\n", (int)ivideo->vbflags);
 
-#ifdef CONFIG_MTRR
-		ivideo->mtrr = mtrr_add(ivideo->video_base, ivideo->video_size,
-					MTRR_TYPE_WRCOMB, 1);
-		if(ivideo->mtrr < 0) {
-			printk(KERN_DEBUG "sisfb: Failed to add MTRRs\n");
-		}
-#endif
-
+		ivideo->wc_cookie = arch_phys_wc_add(ivideo->video_base,
+						     ivideo->video_size);
 		if(register_framebuffer(sis_fb_info) < 0) {
 			printk(KERN_ERR "sisfb: Fatal error: Failed to register framebuffer\n");
 			ret = -EINVAL;
@@ -6507,11 +6496,7 @@ static void sisfb_remove(struct pci_dev *pdev)
 
 	pci_dev_put(ivideo->nbridge);
 
-#ifdef CONFIG_MTRR
-	/* Release MTRR region */
-	if(ivideo->mtrr >= 0)
-		mtrr_del(ivideo->mtrr, ivideo->video_base, ivideo->video_size);
-#endif
+	arch_phys_wc_del(ivideo->wc_cookie);
 
 	/* If device was disabled when starting, disable
 	 * it when quitting.
diff --git a/drivers/video/fbdev/ssd1307fb.c b/drivers/video/fbdev/ssd1307fb.c
index f7ed6d9016f7..3e153c06131a 100644
--- a/drivers/video/fbdev/ssd1307fb.c
+++ b/drivers/video/fbdev/ssd1307fb.c
@@ -7,6 +7,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/backlight.h>
 #include <linux/kernel.h>
 #include <linux/i2c.h>
 #include <linux/fb.h>
@@ -38,22 +39,43 @@
 #define	SSD1307FB_SET_COM_PINS_CONFIG	0xda
 #define	SSD1307FB_SET_VCOMH		0xdb
 
+#define MAX_CONTRAST 255
+
+#define REFRESHRATE 1
+
+static u_int refreshrate = REFRESHRATE;
+module_param(refreshrate, uint, 0);
+
 struct ssd1307fb_par;
 
-struct ssd1307fb_ops {
-	int (*init)(struct ssd1307fb_par *);
-	int (*remove)(struct ssd1307fb_par *);
+struct ssd1307fb_deviceinfo {
+	u32 default_vcomh;
+	u32 default_dclk_div;
+	u32 default_dclk_frq;
+	int need_pwm;
+	int need_chargepump;
 };
 
 struct ssd1307fb_par {
+	u32 com_invdir;
+	u32 com_lrremap;
+	u32 com_offset;
+	u32 com_seq;
+	u32 contrast;
+	u32 dclk_div;
+	u32 dclk_frq;
+	struct ssd1307fb_deviceinfo *device_info;
 	struct i2c_client *client;
 	u32 height;
 	struct fb_info *info;
-	struct ssd1307fb_ops *ops;
 	u32 page_offset;
+	u32 prechargep1;
+	u32 prechargep2;
 	struct pwm_device *pwm;
 	u32 pwm_period;
 	int reset;
+	u32 seg_remap;
+	u32 vcomh;
 	u32 width;
 };
 
@@ -213,6 +235,16 @@ static ssize_t ssd1307fb_write(struct fb_info *info, const char __user *buf,
 	return count;
 }
 
+static int ssd1307fb_blank(int blank_mode, struct fb_info *info)
+{
+	struct ssd1307fb_par *par = info->par;
+
+	if (blank_mode != FB_BLANK_UNBLANK)
+		return ssd1307fb_write_cmd(par->client, SSD1307FB_DISPLAY_OFF);
+	else
+		return ssd1307fb_write_cmd(par->client, SSD1307FB_DISPLAY_ON);
+}
+
 static void ssd1307fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 {
 	struct ssd1307fb_par *par = info->par;
@@ -238,6 +270,7 @@ static struct fb_ops ssd1307fb_ops = {
 	.owner		= THIS_MODULE,
 	.fb_read	= fb_sys_read,
 	.fb_write	= ssd1307fb_write,
+	.fb_blank	= ssd1307fb_blank,
 	.fb_fillrect	= ssd1307fb_fillrect,
 	.fb_copyarea	= ssd1307fb_copyarea,
 	.fb_imageblit	= ssd1307fb_imageblit,
@@ -249,74 +282,46 @@ static void ssd1307fb_deferred_io(struct fb_info *info,
 	ssd1307fb_update_display(info->par);
 }
 
-static struct fb_deferred_io ssd1307fb_defio = {
-	.delay		= HZ,
-	.deferred_io	= ssd1307fb_deferred_io,
-};
-
-static int ssd1307fb_ssd1307_init(struct ssd1307fb_par *par)
+static int ssd1307fb_init(struct ssd1307fb_par *par)
 {
 	int ret;
+	u32 precharge, dclk, com_invdir, compins;
 
-	par->pwm = pwm_get(&par->client->dev, NULL);
-	if (IS_ERR(par->pwm)) {
-		dev_err(&par->client->dev, "Could not get PWM from device tree!\n");
-		return PTR_ERR(par->pwm);
-	}
-
-	par->pwm_period = pwm_get_period(par->pwm);
-	/* Enable the PWM */
-	pwm_config(par->pwm, par->pwm_period / 2, par->pwm_period);
-	pwm_enable(par->pwm);
-
-	dev_dbg(&par->client->dev, "Using PWM%d with a %dns period.\n",
-		par->pwm->pwm, par->pwm_period);
-
-	/* Map column 127 of the OLED to segment 0 */
-	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_SEG_REMAP_ON);
-	if (ret < 0)
-		return ret;
-
-	/* Turn on the display */
-	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_DISPLAY_ON);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-static int ssd1307fb_ssd1307_remove(struct ssd1307fb_par *par)
-{
-	pwm_disable(par->pwm);
-	pwm_put(par->pwm);
-	return 0;
-}
+	if (par->device_info->need_pwm) {
+		par->pwm = pwm_get(&par->client->dev, NULL);
+		if (IS_ERR(par->pwm)) {
+			dev_err(&par->client->dev, "Could not get PWM from device tree!\n");
+			return PTR_ERR(par->pwm);
+		}
 
-static struct ssd1307fb_ops ssd1307fb_ssd1307_ops = {
-	.init	= ssd1307fb_ssd1307_init,
-	.remove	= ssd1307fb_ssd1307_remove,
-};
+		par->pwm_period = pwm_get_period(par->pwm);
+		/* Enable the PWM */
+		pwm_config(par->pwm, par->pwm_period / 2, par->pwm_period);
+		pwm_enable(par->pwm);
 
-static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
-{
-	int ret;
+		dev_dbg(&par->client->dev, "Using PWM%d with a %dns period.\n",
+			par->pwm->pwm, par->pwm_period);
+	};
 
 	/* Set initial contrast */
 	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_CONTRAST);
 	if (ret < 0)
 		return ret;
 
-	ret = ssd1307fb_write_cmd(par->client, 0x7f);
-	if (ret < 0)
-		return ret;
-
-	/* Set COM direction */
-	ret = ssd1307fb_write_cmd(par->client, 0xc8);
+	ret = ssd1307fb_write_cmd(par->client, par->contrast);
 	if (ret < 0)
 		return ret;
 
 	/* Set segment re-map */
-	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_SEG_REMAP_ON);
+	if (par->seg_remap) {
+		ret = ssd1307fb_write_cmd(par->client, SSD1307FB_SEG_REMAP_ON);
+		if (ret < 0)
+			return ret;
+	};
+
+	/* Set COM direction */
+	com_invdir = 0xc0 | (par->com_invdir & 0x1) << 3;
+	ret = ssd1307fb_write_cmd(par->client,  com_invdir);
 	if (ret < 0)
 		return ret;
 
@@ -334,7 +339,7 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
-	ret = ssd1307fb_write_cmd(par->client, 0x20);
+	ret = ssd1307fb_write_cmd(par->client, par->com_offset);
 	if (ret < 0)
 		return ret;
 
@@ -343,7 +348,8 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
-	ret = ssd1307fb_write_cmd(par->client, 0xf0);
+	dclk = ((par->dclk_div - 1) & 0xf) | (par->dclk_frq & 0xf) << 4;
+	ret = ssd1307fb_write_cmd(par->client, dclk);
 	if (ret < 0)
 		return ret;
 
@@ -352,7 +358,8 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
-	ret = ssd1307fb_write_cmd(par->client, 0x22);
+	precharge = (par->prechargep1 & 0xf) | (par->prechargep2 & 0xf) << 4;
+	ret = ssd1307fb_write_cmd(par->client, precharge);
 	if (ret < 0)
 		return ret;
 
@@ -361,7 +368,9 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
-	ret = ssd1307fb_write_cmd(par->client, 0x22);
+	compins = 0x02 | !(par->com_seq & 0x1) << 4
+				   | (par->com_lrremap & 0x1) << 5;
+	ret = ssd1307fb_write_cmd(par->client, compins);
 	if (ret < 0)
 		return ret;
 
@@ -370,7 +379,7 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
-	ret = ssd1307fb_write_cmd(par->client, 0x49);
+	ret = ssd1307fb_write_cmd(par->client, par->vcomh);
 	if (ret < 0)
 		return ret;
 
@@ -379,7 +388,8 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
-	ret = ssd1307fb_write_cmd(par->client, 0x14);
+	ret = ssd1307fb_write_cmd(par->client,
+		(par->device_info->need_chargepump & 0x1 << 2) & 0x14);
 	if (ret < 0)
 		return ret;
 
@@ -393,6 +403,7 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
+	/* Set column range */
 	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_SET_COL_RANGE);
 	if (ret < 0)
 		return ret;
@@ -405,6 +416,7 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	if (ret < 0)
 		return ret;
 
+	/* Set page range */
 	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_SET_PAGE_RANGE);
 	if (ret < 0)
 		return ret;
@@ -426,18 +438,75 @@ static int ssd1307fb_ssd1306_init(struct ssd1307fb_par *par)
 	return 0;
 }
 
-static struct ssd1307fb_ops ssd1307fb_ssd1306_ops = {
-	.init	= ssd1307fb_ssd1306_init,
+static int ssd1307fb_update_bl(struct backlight_device *bdev)
+{
+	struct ssd1307fb_par *par = bl_get_data(bdev);
+	int ret;
+	int brightness = bdev->props.brightness;
+
+	par->contrast = brightness;
+
+	ret = ssd1307fb_write_cmd(par->client, SSD1307FB_CONTRAST);
+	if (ret < 0)
+		return ret;
+	ret = ssd1307fb_write_cmd(par->client, par->contrast);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int ssd1307fb_get_brightness(struct backlight_device *bdev)
+{
+	struct ssd1307fb_par *par = bl_get_data(bdev);
+
+	return par->contrast;
+}
+
+static int ssd1307fb_check_fb(struct backlight_device *bdev,
+				   struct fb_info *info)
+{
+	return (info->bl_dev == bdev);
+}
+
+static const struct backlight_ops ssd1307fb_bl_ops = {
+	.options	= BL_CORE_SUSPENDRESUME,
+	.update_status	= ssd1307fb_update_bl,
+	.get_brightness	= ssd1307fb_get_brightness,
+	.check_fb	= ssd1307fb_check_fb,
+};
+
+static struct ssd1307fb_deviceinfo ssd1307fb_ssd1305_deviceinfo = {
+	.default_vcomh = 0x34,
+	.default_dclk_div = 1,
+	.default_dclk_frq = 7,
+};
+
+static struct ssd1307fb_deviceinfo ssd1307fb_ssd1306_deviceinfo = {
+	.default_vcomh = 0x20,
+	.default_dclk_div = 1,
+	.default_dclk_frq = 8,
+	.need_chargepump = 1,
+};
+
+static struct ssd1307fb_deviceinfo ssd1307fb_ssd1307_deviceinfo = {
+	.default_vcomh = 0x20,
+	.default_dclk_div = 2,
+	.default_dclk_frq = 12,
+	.need_pwm = 1,
 };
 
 static const struct of_device_id ssd1307fb_of_match[] = {
 	{
+		.compatible = "solomon,ssd1305fb-i2c",
+		.data = (void *)&ssd1307fb_ssd1305_deviceinfo,
+	},
+	{
 		.compatible = "solomon,ssd1306fb-i2c",
-		.data = (void *)&ssd1307fb_ssd1306_ops,
+		.data = (void *)&ssd1307fb_ssd1306_deviceinfo,
 	},
 	{
 		.compatible = "solomon,ssd1307fb-i2c",
-		.data = (void *)&ssd1307fb_ssd1307_ops,
+		.data = (void *)&ssd1307fb_ssd1307_deviceinfo,
 	},
 	{},
 };
@@ -446,8 +515,11 @@ MODULE_DEVICE_TABLE(of, ssd1307fb_of_match);
 static int ssd1307fb_probe(struct i2c_client *client,
 			   const struct i2c_device_id *id)
 {
+	struct backlight_device *bl;
+	char bl_name[12];
 	struct fb_info *info;
 	struct device_node *node = client->dev.of_node;
+	struct fb_deferred_io *ssd1307fb_defio;
 	u32 vmem_size;
 	struct ssd1307fb_par *par;
 	u8 *vmem;
@@ -468,8 +540,8 @@ static int ssd1307fb_probe(struct i2c_client *client,
 	par->info = info;
 	par->client = client;
 
-	par->ops = (struct ssd1307fb_ops *)of_match_device(ssd1307fb_of_match,
-							   &client->dev)->data;
+	par->device_info = (struct ssd1307fb_deviceinfo *)of_match_device(
+			ssd1307fb_of_match, &client->dev)->data;
 
 	par->reset = of_get_named_gpio(client->dev.of_node,
 					 "reset-gpios", 0);
@@ -487,19 +559,51 @@ static int ssd1307fb_probe(struct i2c_client *client,
 	if (of_property_read_u32(node, "solomon,page-offset", &par->page_offset))
 		par->page_offset = 1;
 
+	if (of_property_read_u32(node, "solomon,com-offset", &par->com_offset))
+		par->com_offset = 0;
+
+	if (of_property_read_u32(node, "solomon,prechargep1", &par->prechargep1))
+		par->prechargep1 = 2;
+
+	if (of_property_read_u32(node, "solomon,prechargep2", &par->prechargep2))
+		par->prechargep2 = 2;
+
+	par->seg_remap = !of_property_read_bool(node, "solomon,segment-no-remap");
+	par->com_seq = of_property_read_bool(node, "solomon,com-seq");
+	par->com_lrremap = of_property_read_bool(node, "solomon,com-lrremap");
+	par->com_invdir = of_property_read_bool(node, "solomon,com-invdir");
+
+	par->contrast = 127;
+	par->vcomh = par->device_info->default_vcomh;
+
+	/* Setup display timing */
+	par->dclk_div = par->device_info->default_dclk_div;
+	par->dclk_frq = par->device_info->default_dclk_frq;
+
 	vmem_size = par->width * par->height / 8;
 
-	vmem = devm_kzalloc(&client->dev, vmem_size, GFP_KERNEL);
+	vmem = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
+					get_order(vmem_size));
 	if (!vmem) {
 		dev_err(&client->dev, "Couldn't allocate graphical memory.\n");
 		ret = -ENOMEM;
 		goto fb_alloc_error;
 	}
 
+	ssd1307fb_defio = devm_kzalloc(&client->dev, sizeof(struct fb_deferred_io), GFP_KERNEL);
+	if (!ssd1307fb_defio) {
+		dev_err(&client->dev, "Couldn't allocate deferred io.\n");
+		ret = -ENOMEM;
+		goto fb_alloc_error;
+	}
+
+	ssd1307fb_defio->delay = HZ / refreshrate;
+	ssd1307fb_defio->deferred_io = ssd1307fb_deferred_io;
+
 	info->fbops = &ssd1307fb_ops;
 	info->fix = ssd1307fb_fix;
 	info->fix.line_length = par->width / 8;
-	info->fbdefio = &ssd1307fb_defio;
+	info->fbdefio = ssd1307fb_defio;
 
 	info->var = ssd1307fb_var;
 	info->var.xres = par->width;
@@ -515,7 +619,7 @@ static int ssd1307fb_probe(struct i2c_client *client,
 	info->var.blue.offset = 0;
 
 	info->screen_base = (u8 __force __iomem *)vmem;
-	info->fix.smem_start = (unsigned long)vmem;
+	info->fix.smem_start = __pa(vmem);
 	info->fix.smem_len = vmem_size;
 
 	fb_deferred_io_init(info);
@@ -538,11 +642,9 @@ static int ssd1307fb_probe(struct i2c_client *client,
 	gpio_set_value(par->reset, 1);
 	udelay(4);
 
-	if (par->ops->init) {
-		ret = par->ops->init(par);
-		if (ret)
-			goto reset_oled_error;
-	}
+	ret = ssd1307fb_init(par);
+	if (ret)
+		goto reset_oled_error;
 
 	ret = register_framebuffer(info);
 	if (ret) {
@@ -550,13 +652,30 @@ static int ssd1307fb_probe(struct i2c_client *client,
 		goto panel_init_error;
 	}
 
+	snprintf(bl_name, sizeof(bl_name), "ssd1307fb%d", info->node);
+	bl = backlight_device_register(bl_name, &client->dev, par,
+				       &ssd1307fb_bl_ops, NULL);
+	if (IS_ERR(bl)) {
+		dev_err(&client->dev, "unable to register backlight device: %ld\n",
+			PTR_ERR(bl));
+		goto bl_init_error;
+	}
+
+	bl->props.brightness = par->contrast;
+	bl->props.max_brightness = MAX_CONTRAST;
+	info->bl_dev = bl;
+
 	dev_info(&client->dev, "fb%d: %s framebuffer device registered, using %d bytes of video memory\n", info->node, info->fix.id, vmem_size);
 
 	return 0;
 
+bl_init_error:
+	unregister_framebuffer(info);
 panel_init_error:
-	if (par->ops->remove)
-		par->ops->remove(par);
+	if (par->device_info->need_pwm) {
+		pwm_disable(par->pwm);
+		pwm_put(par->pwm);
+	};
 reset_oled_error:
 	fb_deferred_io_cleanup(info);
 fb_alloc_error:
@@ -569,16 +688,24 @@ static int ssd1307fb_remove(struct i2c_client *client)
 	struct fb_info *info = i2c_get_clientdata(client);
 	struct ssd1307fb_par *par = info->par;
 
+	ssd1307fb_write_cmd(par->client, SSD1307FB_DISPLAY_OFF);
+
+	backlight_device_unregister(info->bl_dev);
+
 	unregister_framebuffer(info);
-	if (par->ops->remove)
-		par->ops->remove(par);
+	if (par->device_info->need_pwm) {
+		pwm_disable(par->pwm);
+		pwm_put(par->pwm);
+	};
 	fb_deferred_io_cleanup(info);
+	__free_pages(__va(info->fix.smem_start), get_order(info->fix.smem_len));
 	framebuffer_release(info);
 
 	return 0;
 }
 
 static const struct i2c_device_id ssd1307fb_i2c_id[] = {
+	{ "ssd1305fb", 0 },
 	{ "ssd1306fb", 0 },
 	{ "ssd1307fb", 0 },
 	{ }
diff --git a/drivers/video/fbdev/stifb.c b/drivers/video/fbdev/stifb.c
index 86621fabbb8b..735355b0e023 100644
--- a/drivers/video/fbdev/stifb.c
+++ b/drivers/video/fbdev/stifb.c
@@ -121,6 +121,7 @@ static int __initdata stifb_bpp_pref[MAX_STI_ROMS];
 #define REG_3		0x0004a0
 #define REG_4		0x000600
 #define REG_6		0x000800
+#define REG_7		0x000804
 #define REG_8		0x000820
 #define REG_9		0x000a04
 #define REG_10		0x018000
@@ -135,6 +136,8 @@ static int __initdata stifb_bpp_pref[MAX_STI_ROMS];
 #define REG_21		0x200218
 #define REG_22		0x0005a0
 #define REG_23		0x0005c0
+#define REG_24		0x000808
+#define REG_25		0x000b00
 #define REG_26		0x200118
 #define REG_27		0x200308
 #define REG_32		0x21003c
@@ -429,6 +432,9 @@ ARTIST_ENABLE_DISABLE_DISPLAY(struct stifb_info *fb, int enable)
 #define SET_LENXY_START_RECFILL(fb, lenxy) \
 	WRITE_WORD(lenxy, fb, REG_9)
 
+#define SETUP_COPYAREA(fb) \
+	WRITE_BYTE(0, fb, REG_16b1)
+
 static void
 HYPER_ENABLE_DISABLE_DISPLAY(struct stifb_info *fb, int enable)
 {
@@ -1004,6 +1010,36 @@ stifb_blank(int blank_mode, struct fb_info *info)
 	return 0;
 }
 
+static void
+stifb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
+{
+	struct stifb_info *fb = container_of(info, struct stifb_info, info);
+
+	SETUP_COPYAREA(fb);
+
+	SETUP_HW(fb);
+	if (fb->info.var.bits_per_pixel == 32) {
+		WRITE_WORD(0xBBA0A000, fb, REG_10);
+
+		NGLE_REALLY_SET_IMAGE_PLANEMASK(fb, 0xffffffff);
+	} else {
+		WRITE_WORD(fb->id == S9000_ID_HCRX ? 0x13a02000 : 0x13a01000, fb, REG_10);
+
+		NGLE_REALLY_SET_IMAGE_PLANEMASK(fb, 0xff);
+	}
+
+	NGLE_QUICK_SET_IMAGE_BITMAP_OP(fb,
+		IBOvals(RopSrc, MaskAddrOffset(0),
+		BitmapExtent08, StaticReg(1),
+		DataDynamic, MaskOtc, BGx(0), FGx(0)));
+
+	WRITE_WORD(((area->sx << 16) | area->sy), fb, REG_24);
+	WRITE_WORD(((area->width << 16) | area->height), fb, REG_7);
+	WRITE_WORD(((area->dx << 16) | area->dy), fb, REG_25);
+
+	SETUP_FB(fb);
+}
+
 static void __init
 stifb_init_display(struct stifb_info *fb)
 {
@@ -1069,7 +1105,7 @@ static struct fb_ops stifb_ops = {
 	.fb_setcolreg	= stifb_setcolreg,
 	.fb_blank	= stifb_blank,
 	.fb_fillrect	= cfb_fillrect,
-	.fb_copyarea	= cfb_copyarea,
+	.fb_copyarea	= stifb_copyarea,
 	.fb_imageblit	= cfb_imageblit,
 };
 
@@ -1258,7 +1294,7 @@ static int __init stifb_init_fb(struct sti_struct *sti, int bpp_pref)
 	info->fbops = &stifb_ops;
 	info->screen_base = ioremap_nocache(REGION_BASE(fb,1), fix->smem_len);
 	info->screen_size = fix->smem_len;
-	info->flags = FBINFO_DEFAULT;
+	info->flags = FBINFO_DEFAULT | FBINFO_HWACCEL_COPYAREA;
 	info->pseudo_palette = &fb->pseudo_palette;
 
 	/* This has to be done !!! */
diff --git a/drivers/video/fbdev/tdfxfb.c b/drivers/video/fbdev/tdfxfb.c
index f761fe375f5b..621fa441a6db 100644
--- a/drivers/video/fbdev/tdfxfb.c
+++ b/drivers/video/fbdev/tdfxfb.c
@@ -78,24 +78,6 @@
 
 #define DPRINTK(a, b...) pr_debug("fb: %s: " a, __func__ , ## b)
 
-#ifdef CONFIG_MTRR
-#include <asm/mtrr.h>
-#else
-/* duplicate asm/mtrr.h defines to work on archs without mtrr */
-#define MTRR_TYPE_WRCOMB     1
-
-static inline int mtrr_add(unsigned long base, unsigned long size,
-				unsigned int type, char increment)
-{
-    return -ENODEV;
-}
-static inline int mtrr_del(int reg, unsigned long base,
-				unsigned long size)
-{
-    return -ENODEV;
-}
-#endif
-
 #define BANSHEE_MAX_PIXCLOCK 270000
 #define VOODOO3_MAX_PIXCLOCK 300000
 #define VOODOO5_MAX_PIXCLOCK 350000
@@ -167,7 +149,6 @@ static int nopan;
 static int nowrap = 1;      /* not implemented (yet) */
 static int hwcursor = 1;
 static char *mode_option;
-/* mtrr option */
 static bool nomtrr;
 
 /* -------------------------------------------------------------------------
@@ -1454,8 +1435,8 @@ static int tdfxfb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto out_err_regbase;
 	}
 
-	info->screen_base = ioremap_nocache(info->fix.smem_start,
-					    info->fix.smem_len);
+	info->screen_base = ioremap_wc(info->fix.smem_start,
+				       info->fix.smem_len);
 	if (!info->screen_base) {
 		printk(KERN_ERR "fb: Can't remap %s framebuffer.\n",
 				info->fix.id);
@@ -1473,11 +1454,9 @@ static int tdfxfb_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	printk(KERN_INFO "fb: %s memory = %dK\n", info->fix.id,
 			info->fix.smem_len >> 10);
 
-	default_par->mtrr_handle = -1;
 	if (!nomtrr)
-		default_par->mtrr_handle =
-			mtrr_add(info->fix.smem_start, info->fix.smem_len,
-				 MTRR_TYPE_WRCOMB, 1);
+		default_par->wc_cookie= arch_phys_wc_add(info->fix.smem_start,
+							 info->fix.smem_len);
 
 	info->fix.ypanstep	= nopan ? 0 : 1;
 	info->fix.ywrapstep	= nowrap ? 0 : 1;
@@ -1566,9 +1545,7 @@ out_err_iobase:
 #ifdef CONFIG_FB_3DFX_I2C
 	tdfxfb_delete_i2c_busses(default_par);
 #endif
-	if (default_par->mtrr_handle >= 0)
-		mtrr_del(default_par->mtrr_handle, info->fix.smem_start,
-			 info->fix.smem_len);
+	arch_phys_wc_del(default_par->wc_cookie);
 	release_region(pci_resource_start(pdev, 2),
 		       pci_resource_len(pdev, 2));
 out_err_screenbase:
@@ -1604,10 +1581,8 @@ static void __init tdfxfb_setup(char *options)
 			nowrap = 1;
 		} else if (!strncmp(this_opt, "hwcursor=", 9)) {
 			hwcursor = simple_strtoul(this_opt + 9, NULL, 0);
-#ifdef CONFIG_MTRR
 		} else if (!strncmp(this_opt, "nomtrr", 6)) {
 			nomtrr = 1;
-#endif
 		} else {
 			mode_option = this_opt;
 		}
@@ -1633,9 +1608,7 @@ static void tdfxfb_remove(struct pci_dev *pdev)
 #ifdef CONFIG_FB_3DFX_I2C
 	tdfxfb_delete_i2c_busses(par);
 #endif
-	if (par->mtrr_handle >= 0)
-		mtrr_del(par->mtrr_handle, info->fix.smem_start,
-			 info->fix.smem_len);
+	arch_phys_wc_del(par->wc_cookie);
 	iounmap(par->regbase_virt);
 	iounmap(info->screen_base);
 
@@ -1677,10 +1650,8 @@ MODULE_PARM_DESC(hwcursor, "Enable hardware cursor "
 			"(1=enable, 0=disable, default=1)");
 module_param(mode_option, charp, 0);
 MODULE_PARM_DESC(mode_option, "Initial video mode e.g. '648x480-8@60'");
-#ifdef CONFIG_MTRR
 module_param(nomtrr, bool, 0);
 MODULE_PARM_DESC(nomtrr, "Disable MTRR support (default: enabled)");
-#endif
 
 module_init(tdfxfb_init);
 module_exit(tdfxfb_exit);
diff --git a/drivers/video/fbdev/uvesafb.c b/drivers/video/fbdev/uvesafb.c
index d32d1c4d1b99..178ae93b7ebd 100644
--- a/drivers/video/fbdev/uvesafb.c
+++ b/drivers/video/fbdev/uvesafb.c
@@ -1977,7 +1977,7 @@ static int param_set_scroll(const char *val, const struct kernel_param *kp)
 
 	return 0;
 }
-static struct kernel_param_ops param_ops_scroll = {
+static const struct kernel_param_ops param_ops_scroll = {
 	.set = param_set_scroll,
 };
 #define param_check_scroll(name, p) __param_check(name, p, void)
diff --git a/drivers/video/fbdev/vesafb.c b/drivers/video/fbdev/vesafb.c
index d79a0ac49fc7..528fe917dd49 100644
--- a/drivers/video/fbdev/vesafb.c
+++ b/drivers/video/fbdev/vesafb.c
@@ -19,16 +19,20 @@
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/screen_info.h>
+#include <linux/io.h>
 
 #include <video/vga.h>
-#include <asm/io.h>
-#include <asm/mtrr.h>
 
 #define dac_reg	(0x3c8)
 #define dac_val	(0x3c9)
 
 /* --------------------------------------------------------------------- */
 
+struct vesafb_par {
+	u32 pseudo_palette[256];
+	int wc_cookie;
+};
+
 static struct fb_var_screeninfo vesafb_defined = {
 	.activate	= FB_ACTIVATE_NOW,
 	.height		= -1,
@@ -175,7 +179,10 @@ static int vesafb_setcolreg(unsigned regno, unsigned red, unsigned green,
 
 static void vesafb_destroy(struct fb_info *info)
 {
+	struct vesafb_par *par = info->par;
+
 	fb_dealloc_cmap(&info->cmap);
+	arch_phys_wc_del(par->wc_cookie);
 	if (info->screen_base)
 		iounmap(info->screen_base);
 	release_mem_region(info->apertures->ranges[0].base, info->apertures->ranges[0].size);
@@ -228,6 +235,7 @@ static int vesafb_setup(char *options)
 static int vesafb_probe(struct platform_device *dev)
 {
 	struct fb_info *info;
+	struct vesafb_par *par;
 	int i, err;
 	unsigned int size_vmode;
 	unsigned int size_remap;
@@ -291,14 +299,14 @@ static int vesafb_probe(struct platform_device *dev)
 		   spaces our resource handlers simply don't know about */
 	}
 
-	info = framebuffer_alloc(sizeof(u32) * 256, &dev->dev);
+	info = framebuffer_alloc(sizeof(struct vesafb_par), &dev->dev);
 	if (!info) {
 		release_mem_region(vesafb_fix.smem_start, size_total);
 		return -ENOMEM;
 	}
 	platform_set_drvdata(dev, info);
-	info->pseudo_palette = info->par;
-	info->par = NULL;
+	par = info->par;
+	info->pseudo_palette = par->pseudo_palette;
 
 	/* set vesafb aperture size for generic probing */
 	info->apertures = alloc_apertures(1);
@@ -404,60 +412,27 @@ static int vesafb_probe(struct platform_device *dev)
 	 * region already (FIXME) */
 	request_region(0x3c0, 32, "vesafb");
 
-#ifdef CONFIG_MTRR
-	if (mtrr) {
+	if (mtrr == 3) {
 		unsigned int temp_size = size_total;
-		unsigned int type = 0;
-
-		switch (mtrr) {
-		case 1:
-			type = MTRR_TYPE_UNCACHABLE;
-			break;
-		case 2:
-			type = MTRR_TYPE_WRBACK;
-			break;
-		case 3:
-			type = MTRR_TYPE_WRCOMB;
-			break;
-		case 4:
-			type = MTRR_TYPE_WRTHROUGH;
-			break;
-		default:
-			type = 0;
-			break;
-		}
 
-		if (type) {
-			int rc;
+		/* Find the largest power-of-two */
+		temp_size = roundup_pow_of_two(temp_size);
 
-			/* Find the largest power-of-two */
-			temp_size = roundup_pow_of_two(temp_size);
+		/* Try and find a power of two to add */
+		do {
+			par->wc_cookie =
+				arch_phys_wc_add(vesafb_fix.smem_start,
+						 temp_size);
+			temp_size >>= 1;
+		} while (temp_size >= PAGE_SIZE && par->wc_cookie < 0);
 
-			/* Try and find a power of two to add */
-			do {
-				rc = mtrr_add(vesafb_fix.smem_start, temp_size,
-					      type, 1);
-				temp_size >>= 1;
-			} while (temp_size >= PAGE_SIZE && rc == -EINVAL);
-		}
-	}
-#endif
-	
-	switch (mtrr) {
-	case 1: /* uncachable */
-		info->screen_base = ioremap_nocache(vesafb_fix.smem_start, vesafb_fix.smem_len);
-		break;
-	case 2: /* write-back */
-		info->screen_base = ioremap_cache(vesafb_fix.smem_start, vesafb_fix.smem_len);
-		break;
-	case 3: /* write-combining */
 		info->screen_base = ioremap_wc(vesafb_fix.smem_start, vesafb_fix.smem_len);
-		break;
-	case 4: /* write-through */
-	default:
+	} else {
+		if (mtrr && mtrr != 3)
+			WARN_ONCE(1, "Only MTRR_TYPE_WRCOMB (3) make sense\n");
 		info->screen_base = ioremap(vesafb_fix.smem_start, vesafb_fix.smem_len);
-		break;
 	}
+
 	if (!info->screen_base) {
 		printk(KERN_ERR
 		       "vesafb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
@@ -492,6 +467,7 @@ static int vesafb_probe(struct platform_device *dev)
 	fb_info(info, "%s frame buffer device\n", info->fix.id);
 	return 0;
 err:
+	arch_phys_wc_del(par->wc_cookie);
 	if (info->screen_base)
 		iounmap(info->screen_base);
 	framebuffer_release(info);
diff --git a/drivers/video/fbdev/vt8623fb.c b/drivers/video/fbdev/vt8623fb.c
index ea7f056ed5fe..8bac309c24b9 100644
--- a/drivers/video/fbdev/vt8623fb.c
+++ b/drivers/video/fbdev/vt8623fb.c
@@ -754,9 +754,9 @@ static int vt8623_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 
 	/* Prepare startup mode */
 
-	kparam_block_sysfs_write(mode_option);
+	kernel_param_lock(THIS_MODULE);
 	rc = fb_find_mode(&(info->var), info, mode_option, NULL, 0, NULL, 8);
-	kparam_unblock_sysfs_write(mode_option);
+	kernel_param_unlock(THIS_MODULE);
 	if (! ((rc == 1) || (rc == 2))) {
 		rc = -EINVAL;
 		dev_err(info->device, "mode %s not found\n", mode_option);
diff --git a/drivers/virtio/virtio_mmio.c b/drivers/virtio/virtio_mmio.c
index 7a5e60dea6c5..10189b5b627f 100644
--- a/drivers/virtio/virtio_mmio.c
+++ b/drivers/virtio/virtio_mmio.c
@@ -691,7 +691,7 @@ static int vm_cmdline_get(char *buffer, const struct kernel_param *kp)
 	return strlen(buffer) + 1;
 }
 
-static struct kernel_param_ops vm_cmdline_param_ops = {
+static const struct kernel_param_ops vm_cmdline_param_ops = {
 	.set = vm_cmdline_set,
 	.get = vm_cmdline_get,
 };
diff --git a/drivers/virtio/virtio_pci_common.c b/drivers/virtio/virtio_pci_common.c
index e894eb278d83..78f804af6c20 100644
--- a/drivers/virtio/virtio_pci_common.c
+++ b/drivers/virtio/virtio_pci_common.c
@@ -423,6 +423,7 @@ int vp_set_vq_affinity(struct virtqueue *vq, int cpu)
 		if (cpu == -1)
 			irq_set_affinity_hint(irq, NULL);
 		else {
+			cpumask_clear(mask);
 			cpumask_set_cpu(cpu, mask);
 			irq_set_affinity_hint(irq, mask);
 		}
@@ -501,18 +502,11 @@ static int virtio_pci_probe(struct pci_dev *pci_dev,
 	INIT_LIST_HEAD(&vp_dev->virtqueues);
 	spin_lock_init(&vp_dev->lock);
 
-	/* Disable MSI/MSIX to bring device to a known good state. */
-	pci_msi_off(pci_dev);
-
 	/* enable the device */
 	rc = pci_enable_device(pci_dev);
 	if (rc)
 		goto err_enable_device;
 
-	rc = pci_request_regions(pci_dev, "virtio-pci");
-	if (rc)
-		goto err_request_regions;
-
 	if (force_legacy) {
 		rc = virtio_pci_legacy_probe(vp_dev);
 		/* Also try modern mode if we can't map BAR0 (no IO space). */
@@ -542,8 +536,6 @@ err_register:
 	else
 	     virtio_pci_modern_remove(vp_dev);
 err_probe:
-	pci_release_regions(pci_dev);
-err_request_regions:
 	pci_disable_device(pci_dev);
 err_enable_device:
 	kfree(vp_dev);
@@ -561,7 +553,6 @@ static void virtio_pci_remove(struct pci_dev *pci_dev)
 	else
 		virtio_pci_modern_remove(vp_dev);
 
-	pci_release_regions(pci_dev);
 	pci_disable_device(pci_dev);
 }
 
diff --git a/drivers/virtio/virtio_pci_common.h b/drivers/virtio/virtio_pci_common.h
index 28ee4e56badf..b976d968e793 100644
--- a/drivers/virtio/virtio_pci_common.h
+++ b/drivers/virtio/virtio_pci_common.h
@@ -75,6 +75,8 @@ struct virtio_pci_device {
 	/* Multiply queue_notify_off by this value. (non-legacy mode). */
 	u32 notify_offset_multiplier;
 
+	int modern_bars;
+
 	/* Legacy only field */
 	/* the IO mapping for the PCI config space */
 	void __iomem *ioaddr;
diff --git a/drivers/virtio/virtio_pci_legacy.c b/drivers/virtio/virtio_pci_legacy.c
index 256a5278a515..48bc9797e530 100644
--- a/drivers/virtio/virtio_pci_legacy.c
+++ b/drivers/virtio/virtio_pci_legacy.c
@@ -215,6 +215,7 @@ static const struct virtio_config_ops virtio_pci_config_ops = {
 int virtio_pci_legacy_probe(struct virtio_pci_device *vp_dev)
 {
 	struct pci_dev *pci_dev = vp_dev->pci_dev;
+	int rc;
 
 	/* We only own devices >= 0x1000 and <= 0x103f: leave the rest. */
 	if (pci_dev->device < 0x1000 || pci_dev->device > 0x103f)
@@ -226,9 +227,14 @@ int virtio_pci_legacy_probe(struct virtio_pci_device *vp_dev)
 		return -ENODEV;
 	}
 
+	rc = pci_request_region(pci_dev, 0, "virtio-pci-legacy");
+	if (rc)
+		return rc;
+
+	rc = -ENOMEM;
 	vp_dev->ioaddr = pci_iomap(pci_dev, 0, 0);
 	if (!vp_dev->ioaddr)
-		return -ENOMEM;
+		goto err_iomap;
 
 	vp_dev->isr = vp_dev->ioaddr + VIRTIO_PCI_ISR;
 
@@ -246,6 +252,10 @@ int virtio_pci_legacy_probe(struct virtio_pci_device *vp_dev)
 	vp_dev->del_vq = del_vq;
 
 	return 0;
+
+err_iomap:
+	pci_release_region(pci_dev, 0);
+	return rc;
 }
 
 void virtio_pci_legacy_remove(struct virtio_pci_device *vp_dev)
@@ -253,4 +263,5 @@ void virtio_pci_legacy_remove(struct virtio_pci_device *vp_dev)
 	struct pci_dev *pci_dev = vp_dev->pci_dev;
 
 	pci_iounmap(pci_dev, vp_dev->ioaddr);
+	pci_release_region(pci_dev, 0);
 }
diff --git a/drivers/virtio/virtio_pci_modern.c b/drivers/virtio/virtio_pci_modern.c
index e88e0997a889..8e5cf194cc0b 100644
--- a/drivers/virtio/virtio_pci_modern.c
+++ b/drivers/virtio/virtio_pci_modern.c
@@ -499,7 +499,7 @@ static const struct virtio_config_ops virtio_pci_config_ops = {
  * Returns offset of the capability, or 0.
  */
 static inline int virtio_pci_find_capability(struct pci_dev *dev, u8 cfg_type,
-					     u32 ioresource_types)
+					     u32 ioresource_types, int *bars)
 {
 	int pos;
 
@@ -520,8 +520,10 @@ static inline int virtio_pci_find_capability(struct pci_dev *dev, u8 cfg_type,
 
 		if (type == cfg_type) {
 			if (pci_resource_len(dev, bar) &&
-			    pci_resource_flags(dev, bar) & ioresource_types)
+			    pci_resource_flags(dev, bar) & ioresource_types) {
+				*bars |= (1 << bar);
 				return pos;
+			}
 		}
 	}
 	return 0;
@@ -617,7 +619,8 @@ int virtio_pci_modern_probe(struct virtio_pci_device *vp_dev)
 
 	/* check for a common config: if not, use legacy mode (bar 0). */
 	common = virtio_pci_find_capability(pci_dev, VIRTIO_PCI_CAP_COMMON_CFG,
-					    IORESOURCE_IO | IORESOURCE_MEM);
+					    IORESOURCE_IO | IORESOURCE_MEM,
+					    &vp_dev->modern_bars);
 	if (!common) {
 		dev_info(&pci_dev->dev,
 			 "virtio_pci: leaving for legacy driver\n");
@@ -626,9 +629,11 @@ int virtio_pci_modern_probe(struct virtio_pci_device *vp_dev)
 
 	/* If common is there, these should be too... */
 	isr = virtio_pci_find_capability(pci_dev, VIRTIO_PCI_CAP_ISR_CFG,
-					 IORESOURCE_IO | IORESOURCE_MEM);
+					 IORESOURCE_IO | IORESOURCE_MEM,
+					 &vp_dev->modern_bars);
 	notify = virtio_pci_find_capability(pci_dev, VIRTIO_PCI_CAP_NOTIFY_CFG,
-					    IORESOURCE_IO | IORESOURCE_MEM);
+					    IORESOURCE_IO | IORESOURCE_MEM,
+					    &vp_dev->modern_bars);
 	if (!isr || !notify) {
 		dev_err(&pci_dev->dev,
 			"virtio_pci: missing capabilities %i/%i/%i\n",
@@ -640,7 +645,13 @@ int virtio_pci_modern_probe(struct virtio_pci_device *vp_dev)
 	 * device-specific configuration.
 	 */
 	device = virtio_pci_find_capability(pci_dev, VIRTIO_PCI_CAP_DEVICE_CFG,
-					    IORESOURCE_IO | IORESOURCE_MEM);
+					    IORESOURCE_IO | IORESOURCE_MEM,
+					    &vp_dev->modern_bars);
+
+	err = pci_request_selected_regions(pci_dev, vp_dev->modern_bars,
+					   "virtio-pci-modern");
+	if (err)
+		return err;
 
 	err = -EINVAL;
 	vp_dev->common = map_capability(pci_dev, common,
@@ -727,4 +738,5 @@ void virtio_pci_modern_remove(struct virtio_pci_device *vp_dev)
 		pci_iounmap(pci_dev, vp_dev->notify_base);
 	pci_iounmap(pci_dev, vp_dev->isr);
 	pci_iounmap(pci_dev, vp_dev->common);
+	pci_release_selected_regions(pci_dev, vp_dev->modern_bars);
 }
diff --git a/drivers/vme/bridges/Kconfig b/drivers/vme/bridges/Kconfig
index 9331064e0476..f6d854584906 100644
--- a/drivers/vme/bridges/Kconfig
+++ b/drivers/vme/bridges/Kconfig
@@ -9,7 +9,7 @@ config VME_CA91CX42
 
 config VME_TSI148
 	tristate "Tempe"
-	depends on VIRT_TO_BUS
+	depends on HAS_DMA
 	help
 	 If you say Y here you get support for the Tundra TSI148 VME bridge
 	 chip.
diff --git a/drivers/vme/bridges/vme_ca91cx42.c b/drivers/vme/bridges/vme_ca91cx42.c
index 18078ecbfcc6..f692efcf683f 100644
--- a/drivers/vme/bridges/vme_ca91cx42.c
+++ b/drivers/vme/bridges/vme_ca91cx42.c
@@ -1192,7 +1192,7 @@ static int ca91cx42_dma_list_exec(struct vme_dma_list *list)
 {
 	struct vme_dma_resource *ctrlr;
 	struct ca91cx42_dma_entry *entry;
-	int retval = 0;
+	int retval;
 	dma_addr_t bus_addr;
 	u32 val;
 	struct device *dev;
@@ -1245,8 +1245,18 @@ static int ca91cx42_dma_list_exec(struct vme_dma_list *list)
 
 	iowrite32(val, bridge->base + DGCS);
 
-	wait_event_interruptible(bridge->dma_queue,
-		ca91cx42_dma_busy(ctrlr->parent));
+	retval = wait_event_interruptible(bridge->dma_queue,
+					  ca91cx42_dma_busy(ctrlr->parent));
+
+	if (retval) {
+		val = ioread32(bridge->base + DGCS);
+		iowrite32(val | CA91CX42_DGCS_STOP_REQ, bridge->base + DGCS);
+		/* Wait for the operation to abort */
+		wait_event(bridge->dma_queue,
+			   ca91cx42_dma_busy(ctrlr->parent));
+		retval = -EINTR;
+		goto exit;
+	}
 
 	/*
 	 * Read status register, this register is valid until we kick off a
@@ -1259,8 +1269,10 @@ static int ca91cx42_dma_list_exec(struct vme_dma_list *list)
 
 		dev_err(dev, "ca91c042: DMA Error. DGCS=%08X\n", val);
 		val = ioread32(bridge->base + DCTL);
+		retval = -EIO;
 	}
 
+exit:
 	/* Remove list from running list */
 	mutex_lock(&ctrlr->mtx);
 	list_del(&list->list);
diff --git a/drivers/vme/bridges/vme_ca91cx42.h b/drivers/vme/bridges/vme_ca91cx42.h
index d46b12dc3b82..d54119e59d55 100644
--- a/drivers/vme/bridges/vme_ca91cx42.h
+++ b/drivers/vme/bridges/vme_ca91cx42.h
@@ -547,7 +547,7 @@ static const int CA91CX42_LINT_LM[] = { CA91CX42_LINT_LM0, CA91CX42_LINT_LM1,
 #define CA91CX42_LM_CTL_DATA		(1<<22)
 #define CA91CX42_LM_CTL_SUPR		(1<<21)
 #define CA91CX42_LM_CTL_NPRIV		(1<<20)
-#define CA91CX42_LM_CTL_AS_M		(5<<16)
+#define CA91CX42_LM_CTL_AS_M		(7<<16)
 #define CA91CX42_LM_CTL_AS_A16		0
 #define CA91CX42_LM_CTL_AS_A24		(1<<16)
 #define CA91CX42_LM_CTL_AS_A32		(1<<17)
diff --git a/drivers/vme/bridges/vme_tsi148.c b/drivers/vme/bridges/vme_tsi148.c
index 895c2a31918d..fb1e7ad272ec 100644
--- a/drivers/vme/bridges/vme_tsi148.c
+++ b/drivers/vme/bridges/vme_tsi148.c
@@ -1833,24 +1833,29 @@ static int tsi148_dma_list_add(struct vme_dma_list *list,
 	/* Add to list */
 	list_add_tail(&entry->list, &list->entries);
 
+	entry->dma_handle = dma_map_single(tsi148_bridge->parent,
+		&entry->descriptor,
+		sizeof(struct tsi148_dma_descriptor), DMA_TO_DEVICE);
+	if (dma_mapping_error(tsi148_bridge->parent, entry->dma_handle)) {
+		dev_err(tsi148_bridge->parent, "DMA mapping error\n");
+		retval = -EINVAL;
+		goto err_dma;
+	}
+
 	/* Fill out previous descriptors "Next Address" */
 	if (entry->list.prev != &list->entries) {
-		prev = list_entry(entry->list.prev, struct tsi148_dma_entry,
-			list);
-		/* We need the bus address for the pointer */
-		entry->dma_handle = dma_map_single(tsi148_bridge->parent,
-			&entry->descriptor,
-			sizeof(struct tsi148_dma_descriptor), DMA_TO_DEVICE);
-
 		reg_split((unsigned long long)entry->dma_handle, &address_high,
 			&address_low);
-		entry->descriptor.dnlau = cpu_to_be32(address_high);
-		entry->descriptor.dnlal = cpu_to_be32(address_low);
+		prev = list_entry(entry->list.prev, struct tsi148_dma_entry,
+				  list);
+		prev->descriptor.dnlau = cpu_to_be32(address_high);
+		prev->descriptor.dnlal = cpu_to_be32(address_low);
 
 	}
 
 	return 0;
 
+err_dma:
 err_dest:
 err_source:
 err_align:
@@ -1887,7 +1892,7 @@ static int tsi148_dma_busy(struct vme_bridge *tsi148_bridge, int channel)
 static int tsi148_dma_list_exec(struct vme_dma_list *list)
 {
 	struct vme_dma_resource *ctrlr;
-	int channel, retval = 0;
+	int channel, retval;
 	struct tsi148_dma_entry *entry;
 	u32 bus_addr_high, bus_addr_low;
 	u32 val, dctlreg = 0;
@@ -1921,10 +1926,6 @@ static int tsi148_dma_list_exec(struct vme_dma_list *list)
 	entry = list_first_entry(&list->entries, struct tsi148_dma_entry,
 		list);
 
-	entry->dma_handle = dma_map_single(tsi148_bridge->parent,
-		&entry->descriptor,
-		sizeof(struct tsi148_dma_descriptor), DMA_TO_DEVICE);
-
 	mutex_unlock(&ctrlr->mtx);
 
 	reg_split(entry->dma_handle, &bus_addr_high, &bus_addr_low);
@@ -1941,9 +1942,19 @@ static int tsi148_dma_list_exec(struct vme_dma_list *list)
 	iowrite32be(dctlreg | TSI148_LCSR_DCTL_DGO, bridge->base +
 		TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DCTL);
 
-	wait_event_interruptible(bridge->dma_queue[channel],
+	retval = wait_event_interruptible(bridge->dma_queue[channel],
 		tsi148_dma_busy(ctrlr->parent, channel));
 
+	if (retval) {
+		iowrite32be(dctlreg | TSI148_LCSR_DCTL_ABT, bridge->base +
+			TSI148_LCSR_DMA[channel] + TSI148_LCSR_OFFSET_DCTL);
+		/* Wait for the operation to abort */
+		wait_event(bridge->dma_queue[channel],
+			   tsi148_dma_busy(ctrlr->parent, channel));
+		retval = -EINTR;
+		goto exit;
+	}
+
 	/*
 	 * Read status register, this register is valid until we kick off a
 	 * new transfer.
@@ -1956,6 +1967,7 @@ static int tsi148_dma_list_exec(struct vme_dma_list *list)
 		retval = -EIO;
 	}
 
+exit:
 	/* Remove list from running list */
 	mutex_lock(&ctrlr->mtx);
 	list_del(&list->list);
diff --git a/drivers/vme/vme.c b/drivers/vme/vme.c
index 6bab2c4ed77c..56708915ebbe 100644
--- a/drivers/vme/vme.c
+++ b/drivers/vme/vme.c
@@ -177,8 +177,8 @@ size_t vme_get_size(struct vme_resource *resource)
 }
 EXPORT_SYMBOL(vme_get_size);
 
-static int vme_check_window(u32 aspace, unsigned long long vme_base,
-	unsigned long long size)
+int vme_check_window(u32 aspace, unsigned long long vme_base,
+		     unsigned long long size)
 {
 	int retval = 0;
 
@@ -199,10 +199,8 @@ static int vme_check_window(u32 aspace, unsigned long long vme_base,
 			retval = -EFAULT;
 		break;
 	case VME_A64:
-		/*
-		 * Any value held in an unsigned long long can be used as the
-		 * base
-		 */
+		if ((size != 0) && (vme_base > U64_MAX + 1 - size))
+			retval = -EFAULT;
 		break;
 	case VME_CRCSR:
 		if (((vme_base + size) > VME_CRCSR_MAX) ||
@@ -223,6 +221,7 @@ static int vme_check_window(u32 aspace, unsigned long long vme_base,
 
 	return retval;
 }
+EXPORT_SYMBOL(vme_check_window);
 
 /*
  * Request a slave image with specific attributes, return some unique
diff --git a/drivers/w1/masters/ds2482.c b/drivers/w1/masters/ds2482.c
index e76a9b39abb2..a674409edfb3 100644
--- a/drivers/w1/masters/ds2482.c
+++ b/drivers/w1/masters/ds2482.c
@@ -93,6 +93,7 @@ static const struct i2c_device_id ds2482_id[] = {
 	{ "ds2482", 0 },
 	{ }
 };
+MODULE_DEVICE_TABLE(i2c, ds2482_id);
 
 static struct i2c_driver ds2482_driver = {
 	.driver = {
diff --git a/drivers/w1/slaves/w1_therm.c b/drivers/w1/slaves/w1_therm.c
index 1f11a20a8ab9..2f029e8f4f95 100644
--- a/drivers/w1/slaves/w1_therm.c
+++ b/drivers/w1/slaves/w1_therm.c
@@ -59,16 +59,32 @@ MODULE_ALIAS("w1-family-" __stringify(W1_THERM_DS28EA00));
 static int w1_strong_pullup = 1;
 module_param_named(strong_pullup, w1_strong_pullup, int, 0);
 
+struct w1_therm_family_data {
+	uint8_t rom[9];
+	atomic_t refcnt;
+};
+
+/* return the address of the refcnt in the family data */
+#define THERM_REFCNT(family_data) \
+	(&((struct w1_therm_family_data*)family_data)->refcnt)
+
 static int w1_therm_add_slave(struct w1_slave *sl)
 {
-	sl->family_data = kzalloc(9, GFP_KERNEL);
+	sl->family_data = kzalloc(sizeof(struct w1_therm_family_data),
+		GFP_KERNEL);
 	if (!sl->family_data)
 		return -ENOMEM;
+	atomic_set(THERM_REFCNT(sl->family_data), 1);
 	return 0;
 }
 
 static void w1_therm_remove_slave(struct w1_slave *sl)
 {
+	int refcnt = atomic_sub_return(1, THERM_REFCNT(sl->family_data));
+	while(refcnt) {
+		msleep(1000);
+		refcnt = atomic_read(THERM_REFCNT(sl->family_data));
+	}
 	kfree(sl->family_data);
 	sl->family_data = NULL;
 }
@@ -76,13 +92,24 @@ static void w1_therm_remove_slave(struct w1_slave *sl)
 static ssize_t w1_slave_show(struct device *device,
 	struct device_attribute *attr, char *buf);
 
+static ssize_t w1_seq_show(struct device *device,
+	struct device_attribute *attr, char *buf);
+
 static DEVICE_ATTR_RO(w1_slave);
+static DEVICE_ATTR_RO(w1_seq);
 
 static struct attribute *w1_therm_attrs[] = {
 	&dev_attr_w1_slave.attr,
 	NULL,
 };
+
+static struct attribute *w1_ds28ea00_attrs[] = {
+	&dev_attr_w1_slave.attr,
+	&dev_attr_w1_seq.attr,
+	NULL,
+};
 ATTRIBUTE_GROUPS(w1_therm);
+ATTRIBUTE_GROUPS(w1_ds28ea00);
 
 static struct w1_family_ops w1_therm_fops = {
 	.add_slave	= w1_therm_add_slave,
@@ -90,6 +117,12 @@ static struct w1_family_ops w1_therm_fops = {
 	.groups		= w1_therm_groups,
 };
 
+static struct w1_family_ops w1_ds28ea00_fops = {
+	.add_slave	= w1_therm_add_slave,
+	.remove_slave	= w1_therm_remove_slave,
+	.groups		= w1_ds28ea00_groups,
+};
+
 static struct w1_family w1_therm_family_DS18S20 = {
 	.fid = W1_THERM_DS18S20,
 	.fops = &w1_therm_fops,
@@ -107,7 +140,7 @@ static struct w1_family w1_therm_family_DS1822 = {
 
 static struct w1_family w1_therm_family_DS28EA00 = {
 	.fid = W1_THERM_DS28EA00,
-	.fops = &w1_therm_fops,
+	.fops = &w1_ds28ea00_fops,
 };
 
 static struct w1_family w1_therm_family_DS1825 = {
@@ -194,13 +227,22 @@ static ssize_t w1_slave_show(struct device *device,
 	struct w1_slave *sl = dev_to_w1_slave(device);
 	struct w1_master *dev = sl->master;
 	u8 rom[9], crc, verdict, external_power;
-	int i, max_trying = 10;
+	int i, ret, max_trying = 10;
 	ssize_t c = PAGE_SIZE;
+	u8 *family_data = sl->family_data;
 
-	i = mutex_lock_interruptible(&dev->bus_mutex);
-	if (i != 0)
-		return i;
+	ret = mutex_lock_interruptible(&dev->bus_mutex);
+	if (ret != 0)
+		goto post_unlock;
+
+	if(!sl->family_data)
+	{
+		ret = -ENODEV;
+		goto pre_unlock;
+	}
 
+	/* prevent the slave from going away in sleep */
+	atomic_inc(THERM_REFCNT(family_data));
 	memset(rom, 0, sizeof(rom));
 
 	while (max_trying--) {
@@ -230,17 +272,19 @@ static ssize_t w1_slave_show(struct device *device,
 				mutex_unlock(&dev->bus_mutex);
 
 				sleep_rem = msleep_interruptible(tm);
-				if (sleep_rem != 0)
-					return -EINTR;
+				if (sleep_rem != 0) {
+					ret = -EINTR;
+					goto post_unlock;
+				}
 
-				i = mutex_lock_interruptible(&dev->bus_mutex);
-				if (i != 0)
-					return i;
+				ret = mutex_lock_interruptible(&dev->bus_mutex);
+				if (ret != 0)
+					goto post_unlock;
 			} else if (!w1_strong_pullup) {
 				sleep_rem = msleep_interruptible(tm);
 				if (sleep_rem != 0) {
-					mutex_unlock(&dev->bus_mutex);
-					return -EINTR;
+					ret = -EINTR;
+					goto pre_unlock;
 				}
 			}
 
@@ -269,19 +313,107 @@ static ssize_t w1_slave_show(struct device *device,
 	c -= snprintf(buf + PAGE_SIZE - c, c, ": crc=%02x %s\n",
 			   crc, (verdict) ? "YES" : "NO");
 	if (verdict)
-		memcpy(sl->family_data, rom, sizeof(rom));
+		memcpy(family_data, rom, sizeof(rom));
 	else
 		dev_warn(device, "Read failed CRC check\n");
 
 	for (i = 0; i < 9; ++i)
 		c -= snprintf(buf + PAGE_SIZE - c, c, "%02x ",
-			      ((u8 *)sl->family_data)[i]);
+			      ((u8 *)family_data)[i]);
 
 	c -= snprintf(buf + PAGE_SIZE - c, c, "t=%d\n",
 		w1_convert_temp(rom, sl->family->fid));
+	ret = PAGE_SIZE - c;
+
+pre_unlock:
 	mutex_unlock(&dev->bus_mutex);
 
+post_unlock:
+	atomic_dec(THERM_REFCNT(family_data));
+	return ret;
+}
+
+#define W1_42_CHAIN	0x99
+#define W1_42_CHAIN_OFF	0x3C
+#define W1_42_CHAIN_OFF_INV	0xC3
+#define W1_42_CHAIN_ON	0x5A
+#define W1_42_CHAIN_ON_INV	0xA5
+#define W1_42_CHAIN_DONE 0x96
+#define W1_42_CHAIN_DONE_INV 0x69
+#define W1_42_COND_READ	0x0F
+#define W1_42_SUCCESS_CONFIRM_BYTE 0xAA
+#define W1_42_FINISHED_BYTE 0xFF
+static ssize_t w1_seq_show(struct device *device,
+	struct device_attribute *attr, char *buf)
+{
+	struct w1_slave *sl = dev_to_w1_slave(device);
+	ssize_t c = PAGE_SIZE;
+	int rv;
+	int i;
+	u8 ack;
+	u64 rn;
+	struct w1_reg_num *reg_num;
+	int seq = 0;
+
+	mutex_lock(&sl->master->bus_mutex);
+	/* Place all devices in CHAIN state */
+	if (w1_reset_bus(sl->master))
+		goto error;
+	w1_write_8(sl->master, W1_SKIP_ROM);
+	w1_write_8(sl->master, W1_42_CHAIN);
+	w1_write_8(sl->master, W1_42_CHAIN_ON);
+	w1_write_8(sl->master, W1_42_CHAIN_ON_INV);
+	msleep(sl->master->pullup_duration);
+
+	/* check for acknowledgment */
+	ack = w1_read_8(sl->master);
+	if (ack != W1_42_SUCCESS_CONFIRM_BYTE)
+		goto error;
+
+	/* In case the bus fails to send 0xFF, limit*/
+	for (i = 0; i <= 64; i++) {
+		if (w1_reset_bus(sl->master))
+			goto error;
+
+		w1_write_8(sl->master, W1_42_COND_READ);
+		rv = w1_read_block(sl->master, (u8 *)&rn, 8);
+		reg_num = (struct w1_reg_num *) &rn;
+		if (reg_num->family == W1_42_FINISHED_BYTE)
+			break;
+		if (sl->reg_num.id == reg_num->id)
+			seq = i;
+
+		w1_write_8(sl->master, W1_42_CHAIN);
+		w1_write_8(sl->master, W1_42_CHAIN_DONE);
+		w1_write_8(sl->master, W1_42_CHAIN_DONE_INV);
+		w1_read_block(sl->master, &ack, sizeof(ack));
+
+		/* check for acknowledgment */
+		ack = w1_read_8(sl->master);
+		if (ack != W1_42_SUCCESS_CONFIRM_BYTE)
+			goto error;
+
+	}
+
+	/* Exit from CHAIN state */
+	if (w1_reset_bus(sl->master))
+		goto error;
+	w1_write_8(sl->master, W1_SKIP_ROM);
+	w1_write_8(sl->master, W1_42_CHAIN);
+	w1_write_8(sl->master, W1_42_CHAIN_OFF);
+	w1_write_8(sl->master, W1_42_CHAIN_OFF_INV);
+
+	/* check for acknowledgment */
+	ack = w1_read_8(sl->master);
+	if (ack != W1_42_SUCCESS_CONFIRM_BYTE)
+		goto error;
+	mutex_unlock(&sl->master->bus_mutex);
+
+	c -= snprintf(buf + PAGE_SIZE - c, c, "%d\n", seq);
 	return PAGE_SIZE - c;
+error:
+	mutex_unlock(&sl->master->bus_mutex);
+	return -EIO;
 }
 
 static int __init w1_therm_init(void)
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index 181f41cb960b..c9a7ff67d395 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -46,11 +46,15 @@ MODULE_AUTHOR("Evgeniy Polyakov <zbr@ioremap.net>");
 MODULE_DESCRIPTION("Driver for 1-wire Dallas network protocol.");
 
 static int w1_timeout = 10;
+static int w1_timeout_us = 0;
 int w1_max_slave_count = 64;
 int w1_max_slave_ttl = 10;
 
 module_param_named(timeout, w1_timeout, int, 0);
 MODULE_PARM_DESC(timeout, "time in seconds between automatic slave searches");
+module_param_named(timeout_us, w1_timeout_us, int, 0);
+MODULE_PARM_DESC(timeout, "time in microseconds between automatic slave"
+		          " searches");
 /* A search stops when w1_max_slave_count devices have been found in that
  * search.  The next search will start over and detect the same set of devices
  * on a static 1-wire bus.  Memory is not allocated based on this number, just
@@ -317,6 +321,14 @@ static ssize_t w1_master_attribute_show_timeout(struct device *dev, struct devic
 	return count;
 }
 
+static ssize_t w1_master_attribute_show_timeout_us(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	ssize_t count;
+	count = sprintf(buf, "%d\n", w1_timeout_us);
+	return count;
+}
+
 static ssize_t w1_master_attribute_store_max_slave_count(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
@@ -543,6 +555,7 @@ static W1_MASTER_ATTR_RO(slave_count, S_IRUGO);
 static W1_MASTER_ATTR_RW(max_slave_count, S_IRUGO | S_IWUSR | S_IWGRP);
 static W1_MASTER_ATTR_RO(attempts, S_IRUGO);
 static W1_MASTER_ATTR_RO(timeout, S_IRUGO);
+static W1_MASTER_ATTR_RO(timeout_us, S_IRUGO);
 static W1_MASTER_ATTR_RO(pointer, S_IRUGO);
 static W1_MASTER_ATTR_RW(search, S_IRUGO | S_IWUSR | S_IWGRP);
 static W1_MASTER_ATTR_RW(pullup, S_IRUGO | S_IWUSR | S_IWGRP);
@@ -556,6 +569,7 @@ static struct attribute *w1_master_default_attrs[] = {
 	&w1_master_attribute_max_slave_count.attr,
 	&w1_master_attribute_attempts.attr,
 	&w1_master_attribute_timeout.attr,
+	&w1_master_attribute_timeout_us.attr,
 	&w1_master_attribute_pointer.attr,
 	&w1_master_attribute_search.attr,
 	&w1_master_attribute_pullup.attr,
@@ -1108,7 +1122,8 @@ int w1_process(void *data)
 	/* As long as w1_timeout is only set by a module parameter the sleep
 	 * time can be calculated in jiffies once.
 	 */
-	const unsigned long jtime = msecs_to_jiffies(w1_timeout * 1000);
+	const unsigned long jtime =
+	  usecs_to_jiffies(w1_timeout * 1000000 + w1_timeout_us);
 	/* remainder if it woke up early */
 	unsigned long jremain = 0;
 
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index e5e7c5505de7..241fafde42cb 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -1,3 +1,4 @@
+
 #
 # Watchdog device configuration
 #
@@ -96,6 +97,15 @@ config DA9063_WATCHDOG
 
 	  This driver can be built as a module. The module name is da9063_wdt.
 
+config DA9062_WATCHDOG
+	tristate "Dialog DA9062 Watchdog"
+	depends on MFD_DA9062
+	select WATCHDOG_CORE
+	help
+	  Support for the watchdog in the DA9062 PMIC.
+
+	  This driver can be built as a module. The module name is da9062_wdt.
+
 config GPIO_WATCHDOG
 	tristate "Watchdog device controlled through GPIO-line"
 	depends on OF_GPIO
@@ -104,6 +114,17 @@ config GPIO_WATCHDOG
 	  If you say yes here you get support for watchdog device
 	  controlled through GPIO-line.
 
+config GPIO_WATCHDOG_ARCH_INITCALL
+	bool "Register the watchdog as early as possible"
+	depends on GPIO_WATCHDOG=y
+	help
+	  In some situations, the default initcall level (module_init)
+	  in not early enough in the boot process to avoid the watchdog
+	  to be triggered.
+	  If you say yes here, the initcall level would be raised to
+	  arch_initcall.
+	  If in doubt, say N.
+
 config MENF21BMC_WATCHDOG
 	tristate "MEN 14F021P00 BMC Watchdog"
 	depends on MFD_MENF21BMC
@@ -169,6 +190,7 @@ config AT91SAM9X_WATCHDOG
 
 config CADENCE_WATCHDOG
 	tristate "Cadence Watchdog Timer"
+	depends on HAS_IOMEM
 	select WATCHDOG_CORE
 	help
 	  Say Y here if you want to include support for the watchdog
@@ -408,7 +430,7 @@ config TS72XX_WATCHDOG
 
 config MAX63XX_WATCHDOG
 	tristate "Max63xx watchdog"
-	depends on ARM && HAS_IOMEM
+	depends on HAS_IOMEM
 	select WATCHDOG_CORE
 	help
 	  Support for memory mapped max63{69,70,71,72,73,74} watchdog timer.
@@ -470,6 +492,18 @@ config SIRFSOC_WATCHDOG
 	  Support for CSR SiRFprimaII and SiRFatlasVI watchdog. When
 	  the watchdog triggers the system will be reset.
 
+config ST_LPC_WATCHDOG
+	tristate "STMicroelectronics LPC Watchdog"
+	depends on ARCH_STI
+	depends on OF
+	select WATCHDOG_CORE
+	help
+	  Say Y here to include STMicroelectronics Low Power Controller
+	  (LPC) based Watchdog timer support.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called st_lpc_wdt.
+
 config TEGRA_WATCHDOG
 	tristate "Tegra watchdog"
 	depends on (ARCH_TEGRA || COMPILE_TEST) && HAS_IOMEM
@@ -514,6 +548,16 @@ config MEDIATEK_WATCHDOG
 	  To compile this driver as a module, choose M here: the
 	  module will be called mtk_wdt.
 
+config DIGICOLOR_WATCHDOG
+	tristate "Conexant Digicolor SoCs watchdog support"
+	depends on ARCH_DIGICOLOR
+	select WATCHDOG_CORE
+	help
+	  Say Y here to include support for the watchdog timer
+	  in Conexant Digicolor SoCs.
+	  To compile this driver as a module, choose M here: the
+	  module will be called digicolor_wdt.
+
 # AVR32 Architecture
 
 config AT32AP700X_WDT
@@ -1343,7 +1387,7 @@ config BOOKE_WDT_DEFAULT_TIMEOUT
 config MEN_A21_WDT
        tristate "MEN A21 VME CPU Carrier Board Watchdog Timer"
        select WATCHDOG_CORE
-       depends on GPIOLIB
+       depends on GPIOLIB || COMPILE_TEST
        help
         Watchdog driver for MEN A21 VMEbus CPU Carrier Boards.
 
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 5c19294d1c30..59ea9a1b8e76 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -59,11 +59,13 @@ obj-$(CONFIG_RETU_WATCHDOG) += retu_wdt.o
 obj-$(CONFIG_BCM2835_WDT) += bcm2835_wdt.o
 obj-$(CONFIG_MOXART_WDT) += moxart_wdt.o
 obj-$(CONFIG_SIRFSOC_WATCHDOG) += sirfsoc_wdt.o
+obj-$(CONFIG_ST_LPC_WATCHDOG) += st_lpc_wdt.o
 obj-$(CONFIG_QCOM_WDT) += qcom-wdt.o
 obj-$(CONFIG_BCM_KONA_WDT) += bcm_kona_wdt.o
 obj-$(CONFIG_TEGRA_WATCHDOG) += tegra_wdt.o
 obj-$(CONFIG_MESON_WATCHDOG) += meson_wdt.o
 obj-$(CONFIG_MEDIATEK_WATCHDOG) += mtk_wdt.o
+obj-$(CONFIG_DIGICOLOR_WATCHDOG) += digicolor_wdt.o
 
 # AVR32 Architecture
 obj-$(CONFIG_AT32AP700X_WDT) += at32ap700x_wdt.o
@@ -179,6 +181,7 @@ obj-$(CONFIG_XEN_WDT) += xen_wdt.o
 # Architecture Independent
 obj-$(CONFIG_DA9052_WATCHDOG) += da9052_wdt.o
 obj-$(CONFIG_DA9055_WATCHDOG) += da9055_wdt.o
+obj-$(CONFIG_DA9062_WATCHDOG) += da9062_wdt.o
 obj-$(CONFIG_DA9063_WATCHDOG) += da9063_wdt.o
 obj-$(CONFIG_GPIO_WATCHDOG)	+= gpio_wdt.o
 obj-$(CONFIG_WM831X_WATCHDOG) += wm831x_wdt.o
diff --git a/drivers/watchdog/at91sam9_wdt.c b/drivers/watchdog/at91sam9_wdt.c
index 1443b3c391de..e4698f7c5f93 100644
--- a/drivers/watchdog/at91sam9_wdt.c
+++ b/drivers/watchdog/at91sam9_wdt.c
@@ -40,9 +40,9 @@
 #define DRV_NAME "AT91SAM9 Watchdog"
 
 #define wdt_read(wdt, field) \
-	__raw_readl((wdt)->base + (field))
+	readl_relaxed((wdt)->base + (field))
 #define wdt_write(wtd, field, val) \
-	__raw_writel((val), (wdt)->base + (field))
+	writel_relaxed((val), (wdt)->base + (field))
 
 /* AT91SAM9 watchdog runs a 12bit counter @ 256Hz,
  * use this to convert a watchdog
diff --git a/drivers/watchdog/bcm2835_wdt.c b/drivers/watchdog/bcm2835_wdt.c
index 2b5a9bbf80b7..7116968dee12 100644
--- a/drivers/watchdog/bcm2835_wdt.c
+++ b/drivers/watchdog/bcm2835_wdt.c
@@ -13,20 +13,25 @@
  * option) any later version.
  */
 
+#include <linux/delay.h>
+#include <linux/reboot.h>
 #include <linux/types.h>
 #include <linux/module.h>
 #include <linux/io.h>
 #include <linux/watchdog.h>
 #include <linux/platform_device.h>
 #include <linux/of_address.h>
+#include <linux/of_platform.h>
 
 #define PM_RSTC				0x1c
+#define PM_RSTS				0x20
 #define PM_WDOG				0x24
 
 #define PM_PASSWORD			0x5a000000
 
 #define PM_WDOG_TIME_SET		0x000fffff
 #define PM_RSTC_WRCFG_CLR		0xffffffcf
+#define PM_RSTS_HADWRH_SET		0x00000040
 #define PM_RSTC_WRCFG_SET		0x00000030
 #define PM_RSTC_WRCFG_FULL_RESET	0x00000020
 #define PM_RSTC_RESET			0x00000102
@@ -37,6 +42,7 @@
 struct bcm2835_wdt {
 	void __iomem		*base;
 	spinlock_t		lock;
+	struct notifier_block	restart_handler;
 };
 
 static unsigned int heartbeat;
@@ -106,6 +112,53 @@ static struct watchdog_device bcm2835_wdt_wdd = {
 	.timeout =	WDOG_TICKS_TO_SECS(PM_WDOG_TIME_SET),
 };
 
+static int
+bcm2835_restart(struct notifier_block *this, unsigned long mode, void *cmd)
+{
+	struct bcm2835_wdt *wdt = container_of(this, struct bcm2835_wdt,
+					       restart_handler);
+	u32 val;
+
+	/* use a timeout of 10 ticks (~150us) */
+	writel_relaxed(10 | PM_PASSWORD, wdt->base + PM_WDOG);
+	val = readl_relaxed(wdt->base + PM_RSTC);
+	val &= PM_RSTC_WRCFG_CLR;
+	val |= PM_PASSWORD | PM_RSTC_WRCFG_FULL_RESET;
+	writel_relaxed(val, wdt->base + PM_RSTC);
+
+	/* No sleeping, possibly atomic. */
+	mdelay(1);
+
+	return 0;
+}
+
+/*
+ * We can't really power off, but if we do the normal reset scheme, and
+ * indicate to bootcode.bin not to reboot, then most of the chip will be
+ * powered off.
+ */
+static void bcm2835_power_off(void)
+{
+	struct device_node *np =
+		of_find_compatible_node(NULL, NULL, "brcm,bcm2835-pm-wdt");
+	struct platform_device *pdev = of_find_device_by_node(np);
+	struct bcm2835_wdt *wdt = platform_get_drvdata(pdev);
+	u32 val;
+
+	/*
+	 * We set the watchdog hard reset bit here to distinguish this reset
+	 * from the normal (full) reset. bootcode.bin will not reboot after a
+	 * hard reset.
+	 */
+	val = readl_relaxed(wdt->base + PM_RSTS);
+	val &= PM_RSTC_WRCFG_CLR;
+	val |= PM_PASSWORD | PM_RSTS_HADWRH_SET;
+	writel_relaxed(val, wdt->base + PM_RSTS);
+
+	/* Continue with normal reset mechanism */
+	bcm2835_restart(&wdt->restart_handler, REBOOT_HARD, NULL);
+}
+
 static int bcm2835_wdt_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -136,6 +189,12 @@ static int bcm2835_wdt_probe(struct platform_device *pdev)
 		return err;
 	}
 
+	wdt->restart_handler.notifier_call = bcm2835_restart;
+	wdt->restart_handler.priority = 128;
+	register_restart_handler(&wdt->restart_handler);
+	if (pm_power_off == NULL)
+		pm_power_off = bcm2835_power_off;
+
 	dev_info(dev, "Broadcom BCM2835 watchdog timer");
 	return 0;
 }
@@ -144,6 +203,9 @@ static int bcm2835_wdt_remove(struct platform_device *pdev)
 {
 	struct bcm2835_wdt *wdt = platform_get_drvdata(pdev);
 
+	unregister_restart_handler(&wdt->restart_handler);
+	if (pm_power_off == bcm2835_power_off)
+		pm_power_off = NULL;
 	watchdog_unregister_device(&bcm2835_wdt_wdd);
 	iounmap(wdt->base);
 
diff --git a/drivers/watchdog/da9062_wdt.c b/drivers/watchdog/da9062_wdt.c
new file mode 100644
index 000000000000..b3a870ce85be
--- /dev/null
+++ b/drivers/watchdog/da9062_wdt.c
@@ -0,0 +1,253 @@
+/*
+ * da9062_wdt.c - WDT device driver for DA9062
+ * Copyright (C) 2015  Dialog Semiconductor Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/watchdog.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/jiffies.h>
+#include <linux/mfd/da9062/registers.h>
+#include <linux/mfd/da9062/core.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+
+static const unsigned int wdt_timeout[] = { 0, 2, 4, 8, 16, 32, 65, 131 };
+#define DA9062_TWDSCALE_DISABLE		0
+#define DA9062_TWDSCALE_MIN		1
+#define DA9062_TWDSCALE_MAX		(ARRAY_SIZE(wdt_timeout) - 1)
+#define DA9062_WDT_MIN_TIMEOUT		wdt_timeout[DA9062_TWDSCALE_MIN]
+#define DA9062_WDT_MAX_TIMEOUT		wdt_timeout[DA9062_TWDSCALE_MAX]
+#define DA9062_WDG_DEFAULT_TIMEOUT	wdt_timeout[DA9062_TWDSCALE_MAX-1]
+#define DA9062_RESET_PROTECTION_MS	300
+
+struct da9062_watchdog {
+	struct da9062 *hw;
+	struct watchdog_device wdtdev;
+	unsigned long j_time_stamp;
+};
+
+static void da9062_set_window_start(struct da9062_watchdog *wdt)
+{
+	wdt->j_time_stamp = jiffies;
+}
+
+static void da9062_apply_window_protection(struct da9062_watchdog *wdt)
+{
+	unsigned long delay = msecs_to_jiffies(DA9062_RESET_PROTECTION_MS);
+	unsigned long timeout = wdt->j_time_stamp + delay;
+	unsigned long now = jiffies;
+	unsigned int diff_ms;
+
+	/* if time-limit has not elapsed then wait for remainder */
+	if (time_before(now, timeout)) {
+		diff_ms = jiffies_to_msecs(timeout-now);
+		dev_dbg(wdt->hw->dev,
+			"Kicked too quickly. Delaying %u msecs\n", diff_ms);
+		msleep(diff_ms);
+	}
+}
+
+static unsigned int da9062_wdt_timeout_to_sel(unsigned int secs)
+{
+	unsigned int i;
+
+	for (i = DA9062_TWDSCALE_MIN; i <= DA9062_TWDSCALE_MAX; i++) {
+		if (wdt_timeout[i] >= secs)
+			return i;
+	}
+
+	return DA9062_TWDSCALE_MAX;
+}
+
+static int da9062_reset_watchdog_timer(struct da9062_watchdog *wdt)
+{
+	int ret;
+
+	da9062_apply_window_protection(wdt);
+
+	ret = regmap_update_bits(wdt->hw->regmap,
+			   DA9062AA_CONTROL_F,
+			   DA9062AA_WATCHDOG_MASK,
+			   DA9062AA_WATCHDOG_MASK);
+
+	da9062_set_window_start(wdt);
+
+	return ret;
+}
+
+static int da9062_wdt_update_timeout_register(struct da9062_watchdog *wdt,
+					      unsigned int regval)
+{
+	struct da9062 *chip = wdt->hw;
+	int ret;
+
+	ret = da9062_reset_watchdog_timer(wdt);
+	if (ret)
+		return ret;
+
+	return regmap_update_bits(chip->regmap,
+				  DA9062AA_CONTROL_D,
+				  DA9062AA_TWDSCALE_MASK,
+				  regval);
+}
+
+static int da9062_wdt_start(struct watchdog_device *wdd)
+{
+	struct da9062_watchdog *wdt = watchdog_get_drvdata(wdd);
+	unsigned int selector;
+	int ret;
+
+	selector = da9062_wdt_timeout_to_sel(wdt->wdtdev.timeout);
+	ret = da9062_wdt_update_timeout_register(wdt, selector);
+	if (ret)
+		dev_err(wdt->hw->dev, "Watchdog failed to start (err = %d)\n",
+			ret);
+
+	return ret;
+}
+
+static int da9062_wdt_stop(struct watchdog_device *wdd)
+{
+	struct da9062_watchdog *wdt = watchdog_get_drvdata(wdd);
+	int ret;
+
+	ret = da9062_reset_watchdog_timer(wdt);
+	if (ret) {
+		dev_err(wdt->hw->dev, "Failed to ping the watchdog (err = %d)\n",
+			ret);
+		return ret;
+	}
+
+	ret = regmap_update_bits(wdt->hw->regmap,
+				 DA9062AA_CONTROL_D,
+				 DA9062AA_TWDSCALE_MASK,
+				 DA9062_TWDSCALE_DISABLE);
+	if (ret)
+		dev_err(wdt->hw->dev, "Watchdog failed to stop (err = %d)\n",
+			ret);
+
+	return ret;
+}
+
+static int da9062_wdt_ping(struct watchdog_device *wdd)
+{
+	struct da9062_watchdog *wdt = watchdog_get_drvdata(wdd);
+	int ret;
+
+	ret = da9062_reset_watchdog_timer(wdt);
+	if (ret)
+		dev_err(wdt->hw->dev, "Failed to ping the watchdog (err = %d)\n",
+			ret);
+
+	return ret;
+}
+
+static int da9062_wdt_set_timeout(struct watchdog_device *wdd,
+				  unsigned int timeout)
+{
+	struct da9062_watchdog *wdt = watchdog_get_drvdata(wdd);
+	unsigned int selector;
+	int ret;
+
+	selector = da9062_wdt_timeout_to_sel(timeout);
+	ret = da9062_wdt_update_timeout_register(wdt, selector);
+	if (ret)
+		dev_err(wdt->hw->dev, "Failed to set watchdog timeout (err = %d)\n",
+			ret);
+	else
+		wdd->timeout = wdt_timeout[selector];
+
+	return ret;
+}
+
+static const struct watchdog_info da9062_watchdog_info = {
+	.options = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING,
+	.identity = "DA9062 WDT",
+};
+
+static const struct watchdog_ops da9062_watchdog_ops = {
+	.owner = THIS_MODULE,
+	.start = da9062_wdt_start,
+	.stop = da9062_wdt_stop,
+	.ping = da9062_wdt_ping,
+	.set_timeout = da9062_wdt_set_timeout,
+};
+
+static int da9062_wdt_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct da9062 *chip;
+	struct da9062_watchdog *wdt;
+
+	chip = dev_get_drvdata(pdev->dev.parent);
+	if (!chip)
+		return -EINVAL;
+
+	wdt = devm_kzalloc(&pdev->dev, sizeof(*wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+
+	wdt->hw = chip;
+
+	wdt->wdtdev.info = &da9062_watchdog_info;
+	wdt->wdtdev.ops = &da9062_watchdog_ops;
+	wdt->wdtdev.min_timeout = DA9062_WDT_MIN_TIMEOUT;
+	wdt->wdtdev.max_timeout = DA9062_WDT_MAX_TIMEOUT;
+	wdt->wdtdev.timeout = DA9062_WDG_DEFAULT_TIMEOUT;
+	wdt->wdtdev.status = WATCHDOG_NOWAYOUT_INIT_STATUS;
+
+	watchdog_set_drvdata(&wdt->wdtdev, wdt);
+	dev_set_drvdata(&pdev->dev, wdt);
+
+	ret = watchdog_register_device(&wdt->wdtdev);
+	if (ret < 0) {
+		dev_err(wdt->hw->dev,
+			"watchdog registration failed (%d)\n", ret);
+		return ret;
+	}
+
+	da9062_set_window_start(wdt);
+
+	ret = da9062_wdt_ping(&wdt->wdtdev);
+	if (ret < 0)
+		watchdog_unregister_device(&wdt->wdtdev);
+
+	return ret;
+}
+
+static int da9062_wdt_remove(struct platform_device *pdev)
+{
+	struct da9062_watchdog *wdt = dev_get_drvdata(&pdev->dev);
+
+	watchdog_unregister_device(&wdt->wdtdev);
+	return 0;
+}
+
+static struct platform_driver da9062_wdt_driver = {
+	.probe = da9062_wdt_probe,
+	.remove = da9062_wdt_remove,
+	.driver = {
+		.name = "da9062-watchdog",
+	},
+};
+module_platform_driver(da9062_wdt_driver);
+
+MODULE_AUTHOR("S Twiss <stwiss.opensource@diasemi.com>");
+MODULE_DESCRIPTION("WDT device driver for Dialog DA9062");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:da9062-watchdog");
diff --git a/drivers/watchdog/digicolor_wdt.c b/drivers/watchdog/digicolor_wdt.c
new file mode 100644
index 000000000000..31d8e4936611
--- /dev/null
+++ b/drivers/watchdog/digicolor_wdt.c
@@ -0,0 +1,205 @@
+/*
+ * Watchdog driver for Conexant Digicolor
+ *
+ * Copyright (C) 2015 Paradox Innovation Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/watchdog.h>
+#include <linux/reboot.h>
+#include <linux/platform_device.h>
+#include <linux/of_address.h>
+
+#define TIMER_A_CONTROL		0
+#define TIMER_A_COUNT		4
+
+#define TIMER_A_ENABLE_COUNT	BIT(0)
+#define TIMER_A_ENABLE_WATCHDOG	BIT(1)
+
+struct dc_wdt {
+	void __iomem		*base;
+	struct clk		*clk;
+	struct notifier_block	restart_handler;
+	spinlock_t		lock;
+};
+
+static unsigned timeout;
+module_param(timeout, uint, 0);
+MODULE_PARM_DESC(timeout, "Watchdog timeout in seconds");
+
+static void dc_wdt_set(struct dc_wdt *wdt, u32 ticks)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&wdt->lock, flags);
+
+	writel_relaxed(0, wdt->base + TIMER_A_CONTROL);
+	writel_relaxed(ticks, wdt->base + TIMER_A_COUNT);
+	writel_relaxed(TIMER_A_ENABLE_COUNT | TIMER_A_ENABLE_WATCHDOG,
+		       wdt->base + TIMER_A_CONTROL);
+
+	spin_unlock_irqrestore(&wdt->lock, flags);
+}
+
+static int dc_restart_handler(struct notifier_block *this, unsigned long mode,
+			      void *cmd)
+{
+	struct dc_wdt *wdt = container_of(this, struct dc_wdt, restart_handler);
+
+	dc_wdt_set(wdt, 1);
+	/* wait for reset to assert... */
+	mdelay(500);
+
+	return NOTIFY_DONE;
+}
+
+static int dc_wdt_start(struct watchdog_device *wdog)
+{
+	struct dc_wdt *wdt = watchdog_get_drvdata(wdog);
+
+	dc_wdt_set(wdt, wdog->timeout * clk_get_rate(wdt->clk));
+
+	return 0;
+}
+
+static int dc_wdt_stop(struct watchdog_device *wdog)
+{
+	struct dc_wdt *wdt = watchdog_get_drvdata(wdog);
+
+	writel_relaxed(0, wdt->base + TIMER_A_CONTROL);
+
+	return 0;
+}
+
+static int dc_wdt_set_timeout(struct watchdog_device *wdog, unsigned int t)
+{
+	struct dc_wdt *wdt = watchdog_get_drvdata(wdog);
+
+	dc_wdt_set(wdt, t * clk_get_rate(wdt->clk));
+	wdog->timeout = t;
+
+	return 0;
+}
+
+static unsigned int dc_wdt_get_timeleft(struct watchdog_device *wdog)
+{
+	struct dc_wdt *wdt = watchdog_get_drvdata(wdog);
+	uint32_t count = readl_relaxed(wdt->base + TIMER_A_COUNT);
+
+	return count / clk_get_rate(wdt->clk);
+}
+
+static struct watchdog_ops dc_wdt_ops = {
+	.owner		= THIS_MODULE,
+	.start		= dc_wdt_start,
+	.stop		= dc_wdt_stop,
+	.set_timeout	= dc_wdt_set_timeout,
+	.get_timeleft	= dc_wdt_get_timeleft,
+};
+
+static struct watchdog_info dc_wdt_info = {
+	.options	= WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE
+			| WDIOF_KEEPALIVEPING,
+	.identity	= "Conexant Digicolor Watchdog",
+};
+
+static struct watchdog_device dc_wdt_wdd = {
+	.info		= &dc_wdt_info,
+	.ops		= &dc_wdt_ops,
+	.min_timeout	= 1,
+};
+
+static int dc_wdt_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct dc_wdt *wdt;
+	int ret;
+
+	wdt = devm_kzalloc(dev, sizeof(struct dc_wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, wdt);
+
+	wdt->base = of_iomap(np, 0);
+	if (!wdt->base) {
+		dev_err(dev, "Failed to remap watchdog regs");
+		return -ENODEV;
+	}
+
+	wdt->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(wdt->clk)) {
+		ret = PTR_ERR(wdt->clk);
+		goto err_iounmap;
+	}
+	dc_wdt_wdd.max_timeout = U32_MAX / clk_get_rate(wdt->clk);
+	dc_wdt_wdd.timeout = dc_wdt_wdd.max_timeout;
+
+	spin_lock_init(&wdt->lock);
+
+	watchdog_set_drvdata(&dc_wdt_wdd, wdt);
+	watchdog_init_timeout(&dc_wdt_wdd, timeout, dev);
+	ret = watchdog_register_device(&dc_wdt_wdd);
+	if (ret) {
+		dev_err(dev, "Failed to register watchdog device");
+		goto err_iounmap;
+	}
+
+	wdt->restart_handler.notifier_call = dc_restart_handler;
+	wdt->restart_handler.priority = 128;
+	ret = register_restart_handler(&wdt->restart_handler);
+	if (ret)
+		dev_warn(&pdev->dev, "cannot register restart handler\n");
+
+	return 0;
+
+err_iounmap:
+	iounmap(wdt->base);
+	return ret;
+}
+
+static int dc_wdt_remove(struct platform_device *pdev)
+{
+	struct dc_wdt *wdt = platform_get_drvdata(pdev);
+
+	unregister_restart_handler(&wdt->restart_handler);
+	watchdog_unregister_device(&dc_wdt_wdd);
+	iounmap(wdt->base);
+
+	return 0;
+}
+
+static void dc_wdt_shutdown(struct platform_device *pdev)
+{
+	dc_wdt_stop(&dc_wdt_wdd);
+}
+
+static const struct of_device_id dc_wdt_of_match[] = {
+	{ .compatible = "cnxt,cx92755-wdt", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, dc_wdt_of_match);
+
+static struct platform_driver dc_wdt_driver = {
+	.probe		= dc_wdt_probe,
+	.remove		= dc_wdt_remove,
+	.shutdown	= dc_wdt_shutdown,
+	.driver = {
+		.name =		"digicolor-wdt",
+		.of_match_table = dc_wdt_of_match,
+	},
+};
+module_platform_driver(dc_wdt_driver);
+
+MODULE_AUTHOR("Baruch Siach <baruch@tkos.co.il>");
+MODULE_DESCRIPTION("Driver for Conexant Digicolor watchdog timer");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/dw_wdt.c b/drivers/watchdog/dw_wdt.c
index d0bb9499d12c..6ea0634345e9 100644
--- a/drivers/watchdog/dw_wdt.c
+++ b/drivers/watchdog/dw_wdt.c
@@ -35,7 +35,6 @@
 #include <linux/pm.h>
 #include <linux/platform_device.h>
 #include <linux/reboot.h>
-#include <linux/spinlock.h>
 #include <linux/timer.h>
 #include <linux/uaccess.h>
 #include <linux/watchdog.h>
@@ -61,7 +60,6 @@ MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started "
 #define WDT_TIMEOUT		(HZ / 2)
 
 static struct {
-	spinlock_t		lock;
 	void __iomem		*regs;
 	struct clk		*clk;
 	unsigned long		in_use;
@@ -177,7 +175,6 @@ static int dw_wdt_open(struct inode *inode, struct file *filp)
 	/* Make sure we don't get unloaded. */
 	__module_get(THIS_MODULE);
 
-	spin_lock(&dw_wdt.lock);
 	if (!dw_wdt_is_enabled()) {
 		/*
 		 * The watchdog is not currently enabled. Set the timeout to
@@ -190,8 +187,6 @@ static int dw_wdt_open(struct inode *inode, struct file *filp)
 
 	dw_wdt_set_next_heartbeat();
 
-	spin_unlock(&dw_wdt.lock);
-
 	return nonseekable_open(inode, filp);
 }
 
@@ -220,6 +215,7 @@ static ssize_t dw_wdt_write(struct file *filp, const char __user *buf,
 	}
 
 	dw_wdt_set_next_heartbeat();
+	dw_wdt_keepalive();
 	mod_timer(&dw_wdt.timer, jiffies + WDT_TIMEOUT);
 
 	return len;
@@ -348,8 +344,6 @@ static int dw_wdt_drv_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	spin_lock_init(&dw_wdt.lock);
-
 	ret = misc_register(&dw_wdt_miscdev);
 	if (ret)
 		goto out_disable_clk;
diff --git a/drivers/watchdog/gpio_wdt.c b/drivers/watchdog/gpio_wdt.c
index cbc313d37c59..1687cc2d7122 100644
--- a/drivers/watchdog/gpio_wdt.c
+++ b/drivers/watchdog/gpio_wdt.c
@@ -267,7 +267,16 @@ static struct platform_driver gpio_wdt_driver = {
 	.probe	= gpio_wdt_probe,
 	.remove	= gpio_wdt_remove,
 };
+
+#ifdef CONFIG_GPIO_WATCHDOG_ARCH_INITCALL
+static int __init gpio_wdt_init(void)
+{
+	return platform_driver_register(&gpio_wdt_driver);
+}
+arch_initcall(gpio_wdt_init);
+#else
 module_platform_driver(gpio_wdt_driver);
+#endif
 
 MODULE_AUTHOR("Alexander Shiyan <shc_work@mail.ru>");
 MODULE_DESCRIPTION("GPIO Watchdog");
diff --git a/drivers/watchdog/hpwdt.c b/drivers/watchdog/hpwdt.c
index ada3e44f9932..286369d4f0f5 100644
--- a/drivers/watchdog/hpwdt.c
+++ b/drivers/watchdog/hpwdt.c
@@ -588,7 +588,7 @@ static long hpwdt_ioctl(struct file *file, unsigned int cmd,
 {
 	void __user *argp = (void __user *)arg;
 	int __user *p = argp;
-	int new_margin;
+	int new_margin, options;
 	int ret = -ENOTTY;
 
 	switch (cmd) {
@@ -608,6 +608,20 @@ static long hpwdt_ioctl(struct file *file, unsigned int cmd,
 		ret = 0;
 		break;
 
+	case WDIOC_SETOPTIONS:
+		ret = get_user(options, p);
+		if (ret)
+			break;
+
+		if (options & WDIOS_DISABLECARD)
+			hpwdt_stop();
+
+		if (options & WDIOS_ENABLECARD) {
+			hpwdt_start();
+			hpwdt_ping();
+		}
+		break;
+
 	case WDIOC_SETTIMEOUT:
 		ret = get_user(new_margin, p);
 		if (ret)
diff --git a/drivers/watchdog/imgpdc_wdt.c b/drivers/watchdog/imgpdc_wdt.c
index 0deaa4f971f5..0f73621827ab 100644
--- a/drivers/watchdog/imgpdc_wdt.c
+++ b/drivers/watchdog/imgpdc_wdt.c
@@ -9,6 +9,35 @@
  *
  * Based on drivers/watchdog/sunxi_wdt.c Copyright (c) 2013 Carlo Caione
  *                                                     2012 Henrik Nordstrom
+ *
+ * Notes
+ * -----
+ * The timeout value is rounded to the next power of two clock cycles.
+ * This is configured using the PDC_WDT_CONFIG register, according to this
+ * formula:
+ *
+ *     timeout = 2^(delay + 1) clock cycles
+ *
+ * Where 'delay' is the value written in PDC_WDT_CONFIG register.
+ *
+ * Therefore, the hardware only allows to program watchdog timeouts, expressed
+ * as a power of two number of watchdog clock cycles. The current implementation
+ * guarantees that the actual watchdog timeout will be _at least_ the value
+ * programmed in the imgpdg_wdt driver.
+ *
+ * The following table shows how the user-configured timeout relates
+ * to the actual hardware timeout (watchdog clock @ 40000 Hz):
+ *
+ * input timeout | WD_DELAY | actual timeout
+ * -----------------------------------
+ *      10       |   18     |  13 seconds
+ *      20       |   19     |  26 seconds
+ *      30       |   20     |  52 seconds
+ *      60       |   21     |  104 seconds
+ *
+ * Albeit coarse, this granularity would suffice most watchdog uses.
+ * If the platform allows it, the user should be able to change the watchdog
+ * clock rate and achieve a finer timeout granularity.
  */
 
 #include <linux/clk.h>
@@ -16,6 +45,7 @@
 #include <linux/log2.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/reboot.h>
 #include <linux/slab.h>
 #include <linux/watchdog.h>
 
@@ -42,7 +72,7 @@
 #define PDC_WDT_MIN_TIMEOUT		1
 #define PDC_WDT_DEF_TIMEOUT		64
 
-static int heartbeat = PDC_WDT_DEF_TIMEOUT;
+static int heartbeat;
 module_param(heartbeat, int, 0);
 MODULE_PARM_DESC(heartbeat, "Watchdog heartbeats in seconds "
 	"(default=" __MODULE_STRING(PDC_WDT_DEF_TIMEOUT) ")");
@@ -57,6 +87,7 @@ struct pdc_wdt_dev {
 	struct clk *wdt_clk;
 	struct clk *sys_clk;
 	void __iomem *base;
+	struct notifier_block restart_handler;
 };
 
 static int pdc_wdt_keepalive(struct watchdog_device *wdt_dev)
@@ -84,18 +115,24 @@ static int pdc_wdt_stop(struct watchdog_device *wdt_dev)
 	return 0;
 }
 
+static void __pdc_wdt_set_timeout(struct pdc_wdt_dev *wdt)
+{
+	unsigned long clk_rate = clk_get_rate(wdt->wdt_clk);
+	unsigned int val;
+
+	val = readl(wdt->base + PDC_WDT_CONFIG) & ~PDC_WDT_CONFIG_DELAY_MASK;
+	val |= order_base_2(wdt->wdt_dev.timeout * clk_rate) - 1;
+	writel(val, wdt->base + PDC_WDT_CONFIG);
+}
+
 static int pdc_wdt_set_timeout(struct watchdog_device *wdt_dev,
 			       unsigned int new_timeout)
 {
-	unsigned int val;
 	struct pdc_wdt_dev *wdt = watchdog_get_drvdata(wdt_dev);
-	unsigned long clk_rate = clk_get_rate(wdt->wdt_clk);
 
 	wdt->wdt_dev.timeout = new_timeout;
 
-	val = readl(wdt->base + PDC_WDT_CONFIG) & ~PDC_WDT_CONFIG_DELAY_MASK;
-	val |= order_base_2(new_timeout * clk_rate) - 1;
-	writel(val, wdt->base + PDC_WDT_CONFIG);
+	__pdc_wdt_set_timeout(wdt);
 
 	return 0;
 }
@@ -106,6 +143,8 @@ static int pdc_wdt_start(struct watchdog_device *wdt_dev)
 	unsigned int val;
 	struct pdc_wdt_dev *wdt = watchdog_get_drvdata(wdt_dev);
 
+	__pdc_wdt_set_timeout(wdt);
+
 	val = readl(wdt->base + PDC_WDT_CONFIG);
 	val |= PDC_WDT_CONFIG_ENABLE;
 	writel(val, wdt->base + PDC_WDT_CONFIG);
@@ -128,8 +167,21 @@ static const struct watchdog_ops pdc_wdt_ops = {
 	.set_timeout	= pdc_wdt_set_timeout,
 };
 
+static int pdc_wdt_restart(struct notifier_block *this, unsigned long mode,
+			   void *cmd)
+{
+	struct pdc_wdt_dev *wdt = container_of(this, struct pdc_wdt_dev,
+					       restart_handler);
+
+	/* Assert SOFT_RESET */
+	writel(0x1, wdt->base + PDC_WDT_SOFT_RESET);
+
+	return NOTIFY_OK;
+}
+
 static int pdc_wdt_probe(struct platform_device *pdev)
 {
+	u64 div;
 	int ret, val;
 	unsigned long clk_rate;
 	struct resource *res;
@@ -189,16 +241,15 @@ static int pdc_wdt_probe(struct platform_device *pdev)
 
 	pdc_wdt->wdt_dev.info = &pdc_wdt_info;
 	pdc_wdt->wdt_dev.ops = &pdc_wdt_ops;
-	pdc_wdt->wdt_dev.max_timeout = 1 << PDC_WDT_CONFIG_DELAY_MASK;
+
+	div = 1ULL << (PDC_WDT_CONFIG_DELAY_MASK + 1);
+	do_div(div, clk_rate);
+	pdc_wdt->wdt_dev.max_timeout = div;
+	pdc_wdt->wdt_dev.timeout = PDC_WDT_DEF_TIMEOUT;
 	pdc_wdt->wdt_dev.parent = &pdev->dev;
 	watchdog_set_drvdata(&pdc_wdt->wdt_dev, pdc_wdt);
 
-	ret = watchdog_init_timeout(&pdc_wdt->wdt_dev, heartbeat, &pdev->dev);
-	if (ret < 0) {
-		pdc_wdt->wdt_dev.timeout = pdc_wdt->wdt_dev.max_timeout;
-		dev_warn(&pdev->dev,
-			 "Initial timeout out of range! setting max timeout\n");
-	}
+	watchdog_init_timeout(&pdc_wdt->wdt_dev, heartbeat, &pdev->dev);
 
 	pdc_wdt_stop(&pdc_wdt->wdt_dev);
 
@@ -238,6 +289,13 @@ static int pdc_wdt_probe(struct platform_device *pdev)
 	if (ret)
 		goto disable_wdt_clk;
 
+	pdc_wdt->restart_handler.notifier_call = pdc_wdt_restart;
+	pdc_wdt->restart_handler.priority = 128;
+	ret = register_restart_handler(&pdc_wdt->restart_handler);
+	if (ret)
+		dev_warn(&pdev->dev, "failed to register restart handler: %d\n",
+			 ret);
+
 	return 0;
 
 disable_wdt_clk:
diff --git a/drivers/watchdog/imx2_wdt.c b/drivers/watchdog/imx2_wdt.c
index 5e6d808d358a..0bb1a1d1b170 100644
--- a/drivers/watchdog/imx2_wdt.c
+++ b/drivers/watchdog/imx2_wdt.c
@@ -166,6 +166,8 @@ static int imx2_wdt_set_timeout(struct watchdog_device *wdog,
 {
 	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
 
+	wdog->timeout = new_timeout;
+
 	regmap_update_bits(wdev->regmap, IMX2_WDT_WCR, IMX2_WDT_WCR_WT,
 			   WDOG_SEC_TO_COUNT(new_timeout));
 	return 0;
@@ -256,8 +258,11 @@ static int __init imx2_wdt_probe(struct platform_device *pdev)
 	wdog->ops		= &imx2_wdt_ops;
 	wdog->min_timeout	= 1;
 	wdog->max_timeout	= IMX2_WDT_MAX_TIME;
+	wdog->parent		= &pdev->dev;
 
-	clk_prepare_enable(wdev->clk);
+	ret = clk_prepare_enable(wdev->clk);
+	if (ret)
+		return ret;
 
 	regmap_read(wdev->regmap, IMX2_WDT_WRSR, &val);
 	wdog->bootstatus = val & IMX2_WDT_WRSR_TOUT ? WDIOF_CARDRESET : 0;
@@ -286,7 +291,7 @@ static int __init imx2_wdt_probe(struct platform_device *pdev)
 	ret = watchdog_register_device(wdog);
 	if (ret) {
 		dev_err(&pdev->dev, "cannot register watchdog device\n");
-		return ret;
+		goto disable_clk;
 	}
 
 	wdev->restart_handler.notifier_call = imx2_restart_handler;
@@ -299,6 +304,10 @@ static int __init imx2_wdt_probe(struct platform_device *pdev)
 		 wdog->timeout, nowayout);
 
 	return 0;
+
+disable_clk:
+	clk_disable_unprepare(wdev->clk);
+	return ret;
 }
 
 static int __exit imx2_wdt_remove(struct platform_device *pdev)
@@ -362,8 +371,11 @@ 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);
+	int ret;
 
-	clk_prepare_enable(wdev->clk);
+	ret = clk_prepare_enable(wdev->clk);
+	if (ret)
+		return ret;
 
 	if (watchdog_active(wdog) && !imx2_wdt_is_running(wdev)) {
 		/*
diff --git a/drivers/watchdog/max63xx_wdt.c b/drivers/watchdog/max63xx_wdt.c
index 08da3114accb..f36ca4be0720 100644
--- a/drivers/watchdog/max63xx_wdt.c
+++ b/drivers/watchdog/max63xx_wdt.c
@@ -39,10 +39,22 @@ static bool nowayout  = WATCHDOG_NOWAYOUT;
 #define MAX6369_WDSET	(7 << 0)
 #define MAX6369_WDI	(1 << 3)
 
-static DEFINE_SPINLOCK(io_lock);
+#define MAX6369_WDSET_DISABLED	3
 
 static int nodelay;
-static void __iomem	*wdt_base;
+
+struct max63xx_wdt {
+	struct watchdog_device wdd;
+	const struct max63xx_timeout *timeout;
+
+	/* memory mapping */
+	void __iomem *base;
+	spinlock_t lock;
+
+	/* WDI and WSET bits write access routines */
+	void (*ping)(struct max63xx_wdt *wdt);
+	void (*set)(struct max63xx_wdt *wdt, u8 set);
+};
 
 /*
  * The timeout values used are actually the absolute minimum the chip
@@ -59,25 +71,25 @@ static void __iomem	*wdt_base;
 
 /* Timeouts in second */
 struct max63xx_timeout {
-	u8 wdset;
-	u8 tdelay;
-	u8 twd;
+	const u8 wdset;
+	const u8 tdelay;
+	const u8 twd;
 };
 
-static struct max63xx_timeout max6369_table[] = {
+static const struct max63xx_timeout max6369_table[] = {
 	{ 5,  1,  1 },
 	{ 6, 10, 10 },
 	{ 7, 60, 60 },
 	{ },
 };
 
-static struct max63xx_timeout max6371_table[] = {
+static const struct max63xx_timeout max6371_table[] = {
 	{ 6, 60,  3 },
 	{ 7, 60, 60 },
 	{ },
 };
 
-static struct max63xx_timeout max6373_table[] = {
+static const struct max63xx_timeout max6373_table[] = {
 	{ 2, 60,  1 },
 	{ 5,  0,  1 },
 	{ 1,  3,  3 },
@@ -86,8 +98,6 @@ static struct max63xx_timeout max6373_table[] = {
 	{ },
 };
 
-static struct max63xx_timeout *current_timeout;
-
 static struct max63xx_timeout *
 max63xx_select_timeout(struct max63xx_timeout *table, int value)
 {
@@ -108,59 +118,32 @@ max63xx_select_timeout(struct max63xx_timeout *table, int value)
 
 static int max63xx_wdt_ping(struct watchdog_device *wdd)
 {
-	u8 val;
-
-	spin_lock(&io_lock);
+	struct max63xx_wdt *wdt = watchdog_get_drvdata(wdd);
 
-	val = __raw_readb(wdt_base);
-
-	__raw_writeb(val | MAX6369_WDI, wdt_base);
-	__raw_writeb(val & ~MAX6369_WDI, wdt_base);
-
-	spin_unlock(&io_lock);
+	wdt->ping(wdt);
 	return 0;
 }
 
 static int max63xx_wdt_start(struct watchdog_device *wdd)
 {
-	struct max63xx_timeout *entry = watchdog_get_drvdata(wdd);
-	u8 val;
+	struct max63xx_wdt *wdt = watchdog_get_drvdata(wdd);
 
-	spin_lock(&io_lock);
-
-	val = __raw_readb(wdt_base);
-	val &= ~MAX6369_WDSET;
-	val |= entry->wdset;
-	__raw_writeb(val, wdt_base);
-
-	spin_unlock(&io_lock);
+	wdt->set(wdt, wdt->timeout->wdset);
 
 	/* check for a edge triggered startup */
-	if (entry->tdelay == 0)
-		max63xx_wdt_ping(wdd);
+	if (wdt->timeout->tdelay == 0)
+		wdt->ping(wdt);
 	return 0;
 }
 
 static int max63xx_wdt_stop(struct watchdog_device *wdd)
 {
-	u8 val;
+	struct max63xx_wdt *wdt = watchdog_get_drvdata(wdd);
 
-	spin_lock(&io_lock);
-
-	val = __raw_readb(wdt_base);
-	val &= ~MAX6369_WDSET;
-	val |= 3;
-	__raw_writeb(val, wdt_base);
-
-	spin_unlock(&io_lock);
+	wdt->set(wdt, MAX6369_WDSET_DISABLED);
 	return 0;
 }
 
-static const struct watchdog_info max63xx_wdt_info = {
-	.options = WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE,
-	.identity = "max63xx Watchdog",
-};
-
 static const struct watchdog_ops max63xx_wdt_ops = {
 	.owner = THIS_MODULE,
 	.start = max63xx_wdt_start,
@@ -168,53 +151,108 @@ static const struct watchdog_ops max63xx_wdt_ops = {
 	.ping = max63xx_wdt_ping,
 };
 
-static struct watchdog_device max63xx_wdt_dev = {
-	.info = &max63xx_wdt_info,
-	.ops = &max63xx_wdt_ops,
+static const struct watchdog_info max63xx_wdt_info = {
+	.options = WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE,
+	.identity = "max63xx Watchdog",
 };
 
+static void max63xx_mmap_ping(struct max63xx_wdt *wdt)
+{
+	u8 val;
+
+	spin_lock(&wdt->lock);
+
+	val = __raw_readb(wdt->base);
+
+	__raw_writeb(val | MAX6369_WDI, wdt->base);
+	__raw_writeb(val & ~MAX6369_WDI, wdt->base);
+
+	spin_unlock(&wdt->lock);
+}
+
+static void max63xx_mmap_set(struct max63xx_wdt *wdt, u8 set)
+{
+	u8 val;
+
+	spin_lock(&wdt->lock);
+
+	val = __raw_readb(wdt->base);
+	val &= ~MAX6369_WDSET;
+	val |= set & MAX6369_WDSET;
+	__raw_writeb(val, wdt->base);
+
+	spin_unlock(&wdt->lock);
+}
+
+static int max63xx_mmap_init(struct platform_device *p, struct max63xx_wdt *wdt)
+{
+	struct resource *mem = platform_get_resource(p, IORESOURCE_MEM, 0);
+
+	wdt->base = devm_ioremap_resource(&p->dev, mem);
+	if (IS_ERR(wdt->base))
+		return PTR_ERR(wdt->base);
+
+	spin_lock_init(&wdt->lock);
+
+	wdt->ping = max63xx_mmap_ping;
+	wdt->set = max63xx_mmap_set;
+	return 0;
+}
+
 static int max63xx_wdt_probe(struct platform_device *pdev)
 {
-	struct resource	*wdt_mem;
+	struct max63xx_wdt *wdt;
 	struct max63xx_timeout *table;
+	int err;
+
+	wdt = devm_kzalloc(&pdev->dev, sizeof(*wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
 
 	table = (struct max63xx_timeout *)pdev->id_entry->driver_data;
 
 	if (heartbeat < 1 || heartbeat > MAX_HEARTBEAT)
 		heartbeat = DEFAULT_HEARTBEAT;
 
-	dev_info(&pdev->dev, "requesting %ds heartbeat\n", heartbeat);
-	current_timeout = max63xx_select_timeout(table, heartbeat);
-
-	if (!current_timeout) {
-		dev_err(&pdev->dev, "unable to satisfy heartbeat request\n");
+	wdt->timeout = max63xx_select_timeout(table, heartbeat);
+	if (!wdt->timeout) {
+		dev_err(&pdev->dev, "unable to satisfy %ds heartbeat request\n",
+			heartbeat);
 		return -EINVAL;
 	}
 
-	dev_info(&pdev->dev, "using %ds heartbeat with %ds initial delay\n",
-		 current_timeout->twd, current_timeout->tdelay);
+	err = max63xx_mmap_init(pdev, wdt);
+	if (err)
+		return err;
+
+	platform_set_drvdata(pdev, &wdt->wdd);
+	watchdog_set_drvdata(&wdt->wdd, wdt);
 
-	heartbeat = current_timeout->twd;
+	wdt->wdd.parent = &pdev->dev;
+	wdt->wdd.timeout = wdt->timeout->twd;
+	wdt->wdd.info = &max63xx_wdt_info;
+	wdt->wdd.ops = &max63xx_wdt_ops;
 
-	wdt_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	wdt_base = devm_ioremap_resource(&pdev->dev, wdt_mem);
-	if (IS_ERR(wdt_base))
-		return PTR_ERR(wdt_base);
+	watchdog_set_nowayout(&wdt->wdd, nowayout);
 
-	max63xx_wdt_dev.timeout = heartbeat;
-	watchdog_set_nowayout(&max63xx_wdt_dev, nowayout);
-	watchdog_set_drvdata(&max63xx_wdt_dev, current_timeout);
+	err = watchdog_register_device(&wdt->wdd);
+	if (err)
+		return err;
 
-	return watchdog_register_device(&max63xx_wdt_dev);
+	dev_info(&pdev->dev, "using %ds heartbeat with %ds initial delay\n",
+		 wdt->timeout->twd, wdt->timeout->tdelay);
+	return 0;
 }
 
 static int max63xx_wdt_remove(struct platform_device *pdev)
 {
-	watchdog_unregister_device(&max63xx_wdt_dev);
+	struct watchdog_device *wdd = platform_get_drvdata(pdev);
+
+	watchdog_unregister_device(wdd);
 	return 0;
 }
 
-static struct platform_device_id max63xx_id_table[] = {
+static const struct platform_device_id max63xx_id_table[] = {
 	{ "max6369_wdt", (kernel_ulong_t)max6369_table, },
 	{ "max6370_wdt", (kernel_ulong_t)max6369_table, },
 	{ "max6371_wdt", (kernel_ulong_t)max6371_table, },
diff --git a/drivers/watchdog/mena21_wdt.c b/drivers/watchdog/mena21_wdt.c
index 96dbba980579..d193a5e79c38 100644
--- a/drivers/watchdog/mena21_wdt.c
+++ b/drivers/watchdog/mena21_wdt.c
@@ -208,14 +208,15 @@ static int a21_wdt_probe(struct platform_device *pdev)
 	else if (reset == 7)
 		a21_wdt.bootstatus |= WDIOF_EXTERN2;
 
+	drv->wdt = a21_wdt;
+	dev_set_drvdata(&pdev->dev, drv);
+
 	ret = watchdog_register_device(&a21_wdt);
 	if (ret) {
 		dev_err(&pdev->dev, "Cannot register watchdog device\n");
 		goto err_register_wd;
 	}
 
-	dev_set_drvdata(&pdev->dev, drv);
-
 	dev_info(&pdev->dev, "MEN A21 watchdog timer driver enabled\n");
 
 	return 0;
diff --git a/drivers/watchdog/omap_wdt.c b/drivers/watchdog/omap_wdt.c
index 1e6be9e40577..de911c7e477c 100644
--- a/drivers/watchdog/omap_wdt.c
+++ b/drivers/watchdog/omap_wdt.c
@@ -53,7 +53,15 @@ static unsigned timer_margin;
 module_param(timer_margin, uint, 0);
 MODULE_PARM_DESC(timer_margin, "initial watchdog timeout (in seconds)");
 
+#define to_omap_wdt_dev(_wdog)	container_of(_wdog, struct omap_wdt_dev, wdog)
+
+static bool early_enable;
+module_param(early_enable, bool, 0);
+MODULE_PARM_DESC(early_enable,
+	"Watchdog is started on module insertion (default=0)");
+
 struct omap_wdt_dev {
+	struct watchdog_device wdog;
 	void __iomem    *base;          /* physical */
 	struct device   *dev;
 	bool		omap_wdt_users;
@@ -123,7 +131,7 @@ static void omap_wdt_set_timer(struct omap_wdt_dev *wdev,
 
 static int omap_wdt_start(struct watchdog_device *wdog)
 {
-	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	struct omap_wdt_dev *wdev = to_omap_wdt_dev(wdog);
 	void __iomem *base = wdev->base;
 
 	mutex_lock(&wdev->lock);
@@ -132,6 +140,13 @@ static int omap_wdt_start(struct watchdog_device *wdog)
 
 	pm_runtime_get_sync(wdev->dev);
 
+	/*
+	 * Make sure the watchdog is disabled. This is unfortunately required
+	 * because writing to various registers with the watchdog running has no
+	 * effect.
+	 */
+	omap_wdt_disable(wdev);
+
 	/* initialize prescaler */
 	while (readl_relaxed(base + OMAP_WATCHDOG_WPS) & 0x01)
 		cpu_relax();
@@ -151,7 +166,7 @@ static int omap_wdt_start(struct watchdog_device *wdog)
 
 static int omap_wdt_stop(struct watchdog_device *wdog)
 {
-	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	struct omap_wdt_dev *wdev = to_omap_wdt_dev(wdog);
 
 	mutex_lock(&wdev->lock);
 	omap_wdt_disable(wdev);
@@ -163,7 +178,7 @@ static int omap_wdt_stop(struct watchdog_device *wdog)
 
 static int omap_wdt_ping(struct watchdog_device *wdog)
 {
-	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	struct omap_wdt_dev *wdev = to_omap_wdt_dev(wdog);
 
 	mutex_lock(&wdev->lock);
 	omap_wdt_reload(wdev);
@@ -175,7 +190,7 @@ static int omap_wdt_ping(struct watchdog_device *wdog)
 static int omap_wdt_set_timeout(struct watchdog_device *wdog,
 				unsigned int timeout)
 {
-	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	struct omap_wdt_dev *wdev = to_omap_wdt_dev(wdog);
 
 	mutex_lock(&wdev->lock);
 	omap_wdt_disable(wdev);
@@ -188,6 +203,16 @@ static int omap_wdt_set_timeout(struct watchdog_device *wdog,
 	return 0;
 }
 
+static unsigned int omap_wdt_get_timeleft(struct watchdog_device *wdog)
+{
+	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	void __iomem *base = wdev->base;
+	u32 value;
+
+	value = readl_relaxed(base + OMAP_WATCHDOG_CRR);
+	return GET_WCCR_SECS(value);
+}
+
 static const struct watchdog_info omap_wdt_info = {
 	.options = WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE | WDIOF_KEEPALIVEPING,
 	.identity = "OMAP Watchdog",
@@ -199,21 +224,16 @@ static const struct watchdog_ops omap_wdt_ops = {
 	.stop		= omap_wdt_stop,
 	.ping		= omap_wdt_ping,
 	.set_timeout	= omap_wdt_set_timeout,
+	.get_timeleft	= omap_wdt_get_timeleft,
 };
 
 static int omap_wdt_probe(struct platform_device *pdev)
 {
 	struct omap_wd_timer_platform_data *pdata = dev_get_platdata(&pdev->dev);
-	struct watchdog_device *omap_wdt;
 	struct resource *res;
 	struct omap_wdt_dev *wdev;
-	u32 rs;
 	int ret;
 
-	omap_wdt = devm_kzalloc(&pdev->dev, sizeof(*omap_wdt), GFP_KERNEL);
-	if (!omap_wdt)
-		return -ENOMEM;
-
 	wdev = devm_kzalloc(&pdev->dev, sizeof(*wdev), GFP_KERNEL);
 	if (!wdev)
 		return -ENOMEM;
@@ -229,35 +249,30 @@ static int omap_wdt_probe(struct platform_device *pdev)
 	if (IS_ERR(wdev->base))
 		return PTR_ERR(wdev->base);
 
-	omap_wdt->info	      = &omap_wdt_info;
-	omap_wdt->ops	      = &omap_wdt_ops;
-	omap_wdt->min_timeout = TIMER_MARGIN_MIN;
-	omap_wdt->max_timeout = TIMER_MARGIN_MAX;
+	wdev->wdog.info = &omap_wdt_info;
+	wdev->wdog.ops = &omap_wdt_ops;
+	wdev->wdog.min_timeout = TIMER_MARGIN_MIN;
+	wdev->wdog.max_timeout = TIMER_MARGIN_MAX;
 
-	if (timer_margin >= TIMER_MARGIN_MIN &&
-	    timer_margin <= TIMER_MARGIN_MAX)
-		omap_wdt->timeout = timer_margin;
-	else
-		omap_wdt->timeout = TIMER_MARGIN_DEFAULT;
+	if (watchdog_init_timeout(&wdev->wdog, timer_margin, &pdev->dev) < 0)
+		wdev->wdog.timeout = TIMER_MARGIN_DEFAULT;
 
-	watchdog_set_drvdata(omap_wdt, wdev);
-	watchdog_set_nowayout(omap_wdt, nowayout);
+	watchdog_set_nowayout(&wdev->wdog, nowayout);
 
-	platform_set_drvdata(pdev, omap_wdt);
+	platform_set_drvdata(pdev, wdev);
 
 	pm_runtime_enable(wdev->dev);
 	pm_runtime_get_sync(wdev->dev);
 
-	if (pdata && pdata->read_reset_sources)
-		rs = pdata->read_reset_sources();
-	else
-		rs = 0;
-	omap_wdt->bootstatus = (rs & (1 << OMAP_MPU_WD_RST_SRC_ID_SHIFT)) ?
-				WDIOF_CARDRESET : 0;
+	if (pdata && pdata->read_reset_sources) {
+		u32 rs = pdata->read_reset_sources();
+		if (rs & (1 << OMAP_MPU_WD_RST_SRC_ID_SHIFT))
+			wdev->wdog.bootstatus = WDIOF_CARDRESET;
+	}
 
 	omap_wdt_disable(wdev);
 
-	ret = watchdog_register_device(omap_wdt);
+	ret = watchdog_register_device(&wdev->wdog);
 	if (ret) {
 		pm_runtime_disable(wdev->dev);
 		return ret;
@@ -265,17 +280,19 @@ static int omap_wdt_probe(struct platform_device *pdev)
 
 	pr_info("OMAP Watchdog Timer Rev 0x%02x: initial timeout %d sec\n",
 		readl_relaxed(wdev->base + OMAP_WATCHDOG_REV) & 0xFF,
-		omap_wdt->timeout);
+		wdev->wdog.timeout);
 
 	pm_runtime_put_sync(wdev->dev);
 
+	if (early_enable)
+		omap_wdt_start(&wdev->wdog);
+
 	return 0;
 }
 
 static void omap_wdt_shutdown(struct platform_device *pdev)
 {
-	struct watchdog_device *wdog = platform_get_drvdata(pdev);
-	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	struct omap_wdt_dev *wdev = platform_get_drvdata(pdev);
 
 	mutex_lock(&wdev->lock);
 	if (wdev->omap_wdt_users) {
@@ -287,11 +304,10 @@ static void omap_wdt_shutdown(struct platform_device *pdev)
 
 static int omap_wdt_remove(struct platform_device *pdev)
 {
-	struct watchdog_device *wdog = platform_get_drvdata(pdev);
-	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	struct omap_wdt_dev *wdev = platform_get_drvdata(pdev);
 
 	pm_runtime_disable(wdev->dev);
-	watchdog_unregister_device(wdog);
+	watchdog_unregister_device(&wdev->wdog);
 
 	return 0;
 }
@@ -306,8 +322,7 @@ static int omap_wdt_remove(struct platform_device *pdev)
 
 static int omap_wdt_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	struct watchdog_device *wdog = platform_get_drvdata(pdev);
-	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	struct omap_wdt_dev *wdev = platform_get_drvdata(pdev);
 
 	mutex_lock(&wdev->lock);
 	if (wdev->omap_wdt_users) {
@@ -321,8 +336,7 @@ static int omap_wdt_suspend(struct platform_device *pdev, pm_message_t state)
 
 static int omap_wdt_resume(struct platform_device *pdev)
 {
-	struct watchdog_device *wdog = platform_get_drvdata(pdev);
-	struct omap_wdt_dev *wdev = watchdog_get_drvdata(wdog);
+	struct omap_wdt_dev *wdev = platform_get_drvdata(pdev);
 
 	mutex_lock(&wdev->lock);
 	if (wdev->omap_wdt_users) {
diff --git a/drivers/watchdog/omap_wdt.h b/drivers/watchdog/omap_wdt.h
index 09b774cf75b9..42f31ec5e90d 100644
--- a/drivers/watchdog/omap_wdt.h
+++ b/drivers/watchdog/omap_wdt.h
@@ -50,5 +50,6 @@
 
 #define PTV			0	/* prescale */
 #define GET_WLDR_VAL(secs)	(0xffffffff - ((secs) * (32768/(1<<PTV))) + 1)
+#define GET_WCCR_SECS(val)	((0xffffffff - (val) + 1) / (32768/(1<<PTV)))
 
 #endif				/* _OMAP_WATCHDOG_H */
diff --git a/drivers/watchdog/st_lpc_wdt.c b/drivers/watchdog/st_lpc_wdt.c
new file mode 100644
index 000000000000..6785afdc0fca
--- /dev/null
+++ b/drivers/watchdog/st_lpc_wdt.c
@@ -0,0 +1,344 @@
+/*
+ * ST's LPC Watchdog
+ *
+ * Copyright (C) 2014 STMicroelectronics -- All Rights Reserved
+ *
+ * Author: David Paris <david.paris@st.com> for STMicroelectronics
+ *         Lee Jones <lee.jones@linaro.org> for STMicroelectronics
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public Licence
+ * as published by the Free Software Foundation; either version
+ * 2 of the Licence, or (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/watchdog.h>
+
+#include <dt-bindings/mfd/st-lpc.h>
+
+/* Low Power Alarm */
+#define LPC_LPA_LSB_OFF			0x410
+#define LPC_LPA_START_OFF		0x418
+
+/* LPC as WDT */
+#define LPC_WDT_OFF			0x510
+
+static struct watchdog_device st_wdog_dev;
+
+struct st_wdog_syscfg {
+	unsigned int reset_type_reg;
+	unsigned int reset_type_mask;
+	unsigned int enable_reg;
+	unsigned int enable_mask;
+};
+
+struct st_wdog {
+	void __iomem *base;
+	struct device *dev;
+	struct regmap *regmap;
+	struct st_wdog_syscfg *syscfg;
+	struct clk *clk;
+	unsigned long clkrate;
+	bool warm_reset;
+};
+
+static struct st_wdog_syscfg stid127_syscfg = {
+	.reset_type_reg		= 0x004,
+	.reset_type_mask	= BIT(2),
+	.enable_reg		= 0x000,
+	.enable_mask		= BIT(2),
+};
+
+static struct st_wdog_syscfg stih415_syscfg = {
+	.reset_type_reg		= 0x0B8,
+	.reset_type_mask	= BIT(6),
+	.enable_reg		= 0x0B4,
+	.enable_mask		= BIT(7),
+};
+
+static struct st_wdog_syscfg stih416_syscfg = {
+	.reset_type_reg		= 0x88C,
+	.reset_type_mask	= BIT(6),
+	.enable_reg		= 0x888,
+	.enable_mask		= BIT(7),
+};
+
+static struct st_wdog_syscfg stih407_syscfg = {
+	.enable_reg		= 0x204,
+	.enable_mask		= BIT(19),
+};
+
+static const struct of_device_id st_wdog_match[] = {
+	{
+		.compatible = "st,stih407-lpc",
+		.data = &stih407_syscfg,
+	},
+	{
+		.compatible = "st,stih416-lpc",
+		.data = &stih416_syscfg,
+	},
+	{
+		.compatible = "st,stih415-lpc",
+		.data = &stih415_syscfg,
+	},
+	{
+		.compatible = "st,stid127-lpc",
+		.data = &stid127_syscfg,
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, st_wdog_match);
+
+static void st_wdog_setup(struct st_wdog *st_wdog, bool enable)
+{
+	/* Type of watchdog reset - 0: Cold 1: Warm */
+	if (st_wdog->syscfg->reset_type_reg)
+		regmap_update_bits(st_wdog->regmap,
+				   st_wdog->syscfg->reset_type_reg,
+				   st_wdog->syscfg->reset_type_mask,
+				   st_wdog->warm_reset);
+
+	/* Mask/unmask watchdog reset */
+	regmap_update_bits(st_wdog->regmap,
+			   st_wdog->syscfg->enable_reg,
+			   st_wdog->syscfg->enable_mask,
+			   enable ? 0 : st_wdog->syscfg->enable_mask);
+}
+
+static void st_wdog_load_timer(struct st_wdog *st_wdog, unsigned int timeout)
+{
+	unsigned long clkrate = st_wdog->clkrate;
+
+	writel_relaxed(timeout * clkrate, st_wdog->base + LPC_LPA_LSB_OFF);
+	writel_relaxed(1, st_wdog->base + LPC_LPA_START_OFF);
+}
+
+static int st_wdog_start(struct watchdog_device *wdd)
+{
+	struct st_wdog *st_wdog = watchdog_get_drvdata(wdd);
+
+	writel_relaxed(1, st_wdog->base + LPC_WDT_OFF);
+
+	return 0;
+}
+
+static int st_wdog_stop(struct watchdog_device *wdd)
+{
+	struct st_wdog *st_wdog = watchdog_get_drvdata(wdd);
+
+	writel_relaxed(0, st_wdog->base + LPC_WDT_OFF);
+
+	return 0;
+}
+
+static int st_wdog_set_timeout(struct watchdog_device *wdd,
+			       unsigned int timeout)
+{
+	struct st_wdog *st_wdog = watchdog_get_drvdata(wdd);
+
+	wdd->timeout = timeout;
+	st_wdog_load_timer(st_wdog, timeout);
+
+	return 0;
+}
+
+static int st_wdog_keepalive(struct watchdog_device *wdd)
+{
+	struct st_wdog *st_wdog = watchdog_get_drvdata(wdd);
+
+	st_wdog_load_timer(st_wdog, wdd->timeout);
+
+	return 0;
+}
+
+static const struct watchdog_info st_wdog_info = {
+	.options = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE,
+	.identity = "ST LPC WDT",
+};
+
+static const struct watchdog_ops st_wdog_ops = {
+	.owner		= THIS_MODULE,
+	.start		= st_wdog_start,
+	.stop		= st_wdog_stop,
+	.ping		= st_wdog_keepalive,
+	.set_timeout	= st_wdog_set_timeout,
+};
+
+static struct watchdog_device st_wdog_dev = {
+	.info		= &st_wdog_info,
+	.ops		= &st_wdog_ops,
+};
+
+static int st_wdog_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	struct device_node *np = pdev->dev.of_node;
+	struct st_wdog *st_wdog;
+	struct regmap *regmap;
+	struct resource *res;
+	struct clk *clk;
+	void __iomem *base;
+	uint32_t mode;
+	int ret;
+
+	ret = of_property_read_u32(np, "st,lpc-mode", &mode);
+	if (ret) {
+		dev_err(&pdev->dev, "An LPC mode must be provided\n");
+		return -EINVAL;
+	}
+
+	/* LPC can either run as a Clocksource or in RTC or WDT mode */
+	if (mode != ST_LPC_MODE_WDT)
+		return -ENODEV;
+
+	st_wdog = devm_kzalloc(&pdev->dev, sizeof(*st_wdog), GFP_KERNEL);
+	if (!st_wdog)
+		return -ENOMEM;
+
+	match = of_match_device(st_wdog_match, &pdev->dev);
+	if (!match) {
+		dev_err(&pdev->dev, "Couldn't match device\n");
+		return -ENODEV;
+	}
+	st_wdog->syscfg	= (struct st_wdog_syscfg *)match->data;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	regmap = syscon_regmap_lookup_by_phandle(np, "st,syscfg");
+	if (IS_ERR(regmap)) {
+		dev_err(&pdev->dev, "No syscfg phandle specified\n");
+		return PTR_ERR(regmap);
+	}
+
+	clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "Unable to request clock\n");
+		return PTR_ERR(clk);
+	}
+
+	st_wdog->dev		= &pdev->dev;
+	st_wdog->base		= base;
+	st_wdog->clk		= clk;
+	st_wdog->regmap		= regmap;
+	st_wdog->warm_reset	= of_property_read_bool(np, "st,warm_reset");
+	st_wdog->clkrate	= clk_get_rate(st_wdog->clk);
+
+	if (!st_wdog->clkrate) {
+		dev_err(&pdev->dev, "Unable to fetch clock rate\n");
+		return -EINVAL;
+	}
+	st_wdog_dev.max_timeout = 0xFFFFFFFF / st_wdog->clkrate;
+
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to enable clock\n");
+		return ret;
+	}
+
+	watchdog_set_drvdata(&st_wdog_dev, st_wdog);
+	watchdog_set_nowayout(&st_wdog_dev, WATCHDOG_NOWAYOUT);
+
+	/* Init Watchdog timeout with value in DT */
+	ret = watchdog_init_timeout(&st_wdog_dev, 0, &pdev->dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to initialise watchdog timeout\n");
+		clk_disable_unprepare(clk);
+		return ret;
+	}
+
+	ret = watchdog_register_device(&st_wdog_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to register watchdog\n");
+		clk_disable_unprepare(clk);
+		return ret;
+	}
+
+	st_wdog_setup(st_wdog, true);
+
+	dev_info(&pdev->dev, "LPC Watchdog driver registered, reset type is %s",
+		 st_wdog->warm_reset ? "warm" : "cold");
+
+	return ret;
+}
+
+static int st_wdog_remove(struct platform_device *pdev)
+{
+	struct st_wdog *st_wdog = watchdog_get_drvdata(&st_wdog_dev);
+
+	st_wdog_setup(st_wdog, false);
+	watchdog_unregister_device(&st_wdog_dev);
+	clk_disable_unprepare(st_wdog->clk);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int st_wdog_suspend(struct device *dev)
+{
+	struct st_wdog *st_wdog = watchdog_get_drvdata(&st_wdog_dev);
+
+	if (watchdog_active(&st_wdog_dev))
+		st_wdog_stop(&st_wdog_dev);
+
+	st_wdog_setup(st_wdog, false);
+
+	clk_disable(st_wdog->clk);
+
+	return 0;
+}
+
+static int st_wdog_resume(struct device *dev)
+{
+	struct st_wdog *st_wdog = watchdog_get_drvdata(&st_wdog_dev);
+	int ret;
+
+	ret = clk_enable(st_wdog->clk);
+	if (ret) {
+		dev_err(dev, "Unable to re-enable clock\n");
+		watchdog_unregister_device(&st_wdog_dev);
+		clk_unprepare(st_wdog->clk);
+		return ret;
+	}
+
+	st_wdog_setup(st_wdog, true);
+
+	if (watchdog_active(&st_wdog_dev)) {
+		st_wdog_load_timer(st_wdog, st_wdog_dev.timeout);
+		st_wdog_start(&st_wdog_dev);
+	}
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(st_wdog_pm_ops,
+			 st_wdog_suspend,
+			 st_wdog_resume);
+
+static struct platform_driver st_wdog_driver = {
+	.driver	= {
+		.name = "st-lpc-wdt",
+		.pm = &st_wdog_pm_ops,
+		.of_match_table = st_wdog_match,
+	},
+	.probe = st_wdog_probe,
+	.remove = st_wdog_remove,
+};
+module_platform_driver(st_wdog_driver);
+
+MODULE_AUTHOR("David Paris <david.paris@st.com>");
+MODULE_DESCRIPTION("ST LPC Watchdog Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/watchdog_core.c b/drivers/watchdog/watchdog_core.c
index cec9b559647d..1a8059455413 100644
--- a/drivers/watchdog/watchdog_core.c
+++ b/drivers/watchdog/watchdog_core.c
@@ -43,6 +43,45 @@
 static DEFINE_IDA(watchdog_ida);
 static struct class *watchdog_class;
 
+/*
+ * Deferred Registration infrastructure.
+ *
+ * Sometimes watchdog drivers needs to be loaded as soon as possible,
+ * for example when it's impossible to disable it. To do so,
+ * raising the initcall level of the watchdog driver is a solution.
+ * But in such case, the miscdev is maybe not ready (subsys_initcall), and
+ * watchdog_core need miscdev to register the watchdog as a char device.
+ *
+ * The deferred registration infrastructure offer a way for the watchdog
+ * subsystem to register a watchdog properly, even before miscdev is ready.
+ */
+
+static DEFINE_MUTEX(wtd_deferred_reg_mutex);
+static LIST_HEAD(wtd_deferred_reg_list);
+static bool wtd_deferred_reg_done;
+
+static int watchdog_deferred_registration_add(struct watchdog_device *wdd)
+{
+	list_add_tail(&wdd->deferred,
+		      &wtd_deferred_reg_list);
+	return 0;
+}
+
+static void watchdog_deferred_registration_del(struct watchdog_device *wdd)
+{
+	struct list_head *p, *n;
+	struct watchdog_device *wdd_tmp;
+
+	list_for_each_safe(p, n, &wtd_deferred_reg_list) {
+		wdd_tmp = list_entry(p, struct watchdog_device,
+				     deferred);
+		if (wdd_tmp == wdd) {
+			list_del(&wdd_tmp->deferred);
+			break;
+		}
+	}
+}
+
 static void watchdog_check_min_max_timeout(struct watchdog_device *wdd)
 {
 	/*
@@ -98,17 +137,7 @@ int watchdog_init_timeout(struct watchdog_device *wdd,
 }
 EXPORT_SYMBOL_GPL(watchdog_init_timeout);
 
-/**
- * watchdog_register_device() - register a watchdog device
- * @wdd: watchdog device
- *
- * Register a watchdog device with the kernel so that the
- * watchdog timer can be accessed from userspace.
- *
- * A zero is returned on success and a negative errno code for
- * failure.
- */
-int watchdog_register_device(struct watchdog_device *wdd)
+static int __watchdog_register_device(struct watchdog_device *wdd)
 {
 	int ret, id, devno;
 
@@ -164,16 +193,33 @@ int watchdog_register_device(struct watchdog_device *wdd)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(watchdog_register_device);
 
 /**
- * watchdog_unregister_device() - unregister a watchdog device
- * @wdd: watchdog device to unregister
+ * watchdog_register_device() - register a watchdog device
+ * @wdd: watchdog device
  *
- * Unregister a watchdog device that was previously successfully
- * registered with watchdog_register_device().
+ * Register a watchdog device with the kernel so that the
+ * watchdog timer can be accessed from userspace.
+ *
+ * A zero is returned on success and a negative errno code for
+ * failure.
  */
-void watchdog_unregister_device(struct watchdog_device *wdd)
+
+int watchdog_register_device(struct watchdog_device *wdd)
+{
+	int ret;
+
+	mutex_lock(&wtd_deferred_reg_mutex);
+	if (wtd_deferred_reg_done)
+		ret = __watchdog_register_device(wdd);
+	else
+		ret = watchdog_deferred_registration_add(wdd);
+	mutex_unlock(&wtd_deferred_reg_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(watchdog_register_device);
+
+static void __watchdog_unregister_device(struct watchdog_device *wdd)
 {
 	int ret;
 	int devno;
@@ -189,8 +235,43 @@ void watchdog_unregister_device(struct watchdog_device *wdd)
 	ida_simple_remove(&watchdog_ida, wdd->id);
 	wdd->dev = NULL;
 }
+
+/**
+ * watchdog_unregister_device() - unregister a watchdog device
+ * @wdd: watchdog device to unregister
+ *
+ * Unregister a watchdog device that was previously successfully
+ * registered with watchdog_register_device().
+ */
+
+void watchdog_unregister_device(struct watchdog_device *wdd)
+{
+	mutex_lock(&wtd_deferred_reg_mutex);
+	if (wtd_deferred_reg_done)
+		__watchdog_unregister_device(wdd);
+	else
+		watchdog_deferred_registration_del(wdd);
+	mutex_unlock(&wtd_deferred_reg_mutex);
+}
+
 EXPORT_SYMBOL_GPL(watchdog_unregister_device);
 
+static int __init watchdog_deferred_registration(void)
+{
+	mutex_lock(&wtd_deferred_reg_mutex);
+	wtd_deferred_reg_done = true;
+	while (!list_empty(&wtd_deferred_reg_list)) {
+		struct watchdog_device *wdd;
+
+		wdd = list_first_entry(&wtd_deferred_reg_list,
+				       struct watchdog_device, deferred);
+		list_del(&wdd->deferred);
+		__watchdog_register_device(wdd);
+	}
+	mutex_unlock(&wtd_deferred_reg_mutex);
+	return 0;
+}
+
 static int __init watchdog_init(void)
 {
 	int err;
@@ -207,6 +288,7 @@ static int __init watchdog_init(void)
 		return err;
 	}
 
+	watchdog_deferred_registration();
 	return 0;
 }
 
@@ -217,7 +299,7 @@ static void __exit watchdog_exit(void)
 	ida_destroy(&watchdog_ida);
 }
 
-subsys_initcall(watchdog_init);
+subsys_initcall_sync(watchdog_init);
 module_exit(watchdog_exit);
 
 MODULE_AUTHOR("Alan Cox <alan@lxorguk.ukuu.org.uk>");
diff --git a/drivers/xen/events/events_base.c b/drivers/xen/events/events_base.c
index 38387950490e..96093ae369a5 100644
--- a/drivers/xen/events/events_base.c
+++ b/drivers/xen/events/events_base.c
@@ -39,8 +39,8 @@
 #include <asm/irq.h>
 #include <asm/idle.h>
 #include <asm/io_apic.h>
-#include <asm/xen/page.h>
 #include <asm/xen/pci.h>
+#include <xen/page.h>
 #endif
 #include <asm/sync_bitops.h>
 #include <asm/xen/hypercall.h>
diff --git a/drivers/xen/events/events_fifo.c b/drivers/xen/events/events_fifo.c
index 417415d738d0..ed673e1acd61 100644
--- a/drivers/xen/events/events_fifo.c
+++ b/drivers/xen/events/events_fifo.c
@@ -44,13 +44,13 @@
 #include <asm/sync_bitops.h>
 #include <asm/xen/hypercall.h>
 #include <asm/xen/hypervisor.h>
-#include <asm/xen/page.h>
 
 #include <xen/xen.h>
 #include <xen/xen-ops.h>
 #include <xen/events.h>
 #include <xen/interface/xen.h>
 #include <xen/interface/event_channel.h>
+#include <xen/page.h>
 
 #include "events_internal.h"
 
diff --git a/drivers/xen/gntdev.c b/drivers/xen/gntdev.c
index 89274850741b..67b9163db718 100644
--- a/drivers/xen/gntdev.c
+++ b/drivers/xen/gntdev.c
@@ -41,9 +41,9 @@
 #include <xen/balloon.h>
 #include <xen/gntdev.h>
 #include <xen/events.h>
+#include <xen/page.h>
 #include <asm/xen/hypervisor.h>
 #include <asm/xen/hypercall.h>
-#include <asm/xen/page.h>
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Derek G. Murray <Derek.Murray@cl.cam.ac.uk>, "
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index b1c7170e5c9e..62f591f8763c 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -138,7 +138,6 @@ static struct gnttab_free_callback *gnttab_free_callback_list;
 static int gnttab_expand(unsigned int req_entries);
 
 #define RPP (PAGE_SIZE / sizeof(grant_ref_t))
-#define SPP (PAGE_SIZE / sizeof(grant_status_t))
 
 static inline grant_ref_t *__gnttab_entry(grant_ref_t entry)
 {
diff --git a/drivers/xen/manage.c b/drivers/xen/manage.c
index 9e6a85104a20..d10effee9b9e 100644
--- a/drivers/xen/manage.c
+++ b/drivers/xen/manage.c
@@ -19,10 +19,10 @@
 #include <xen/grant_table.h>
 #include <xen/events.h>
 #include <xen/hvc-console.h>
+#include <xen/page.h>
 #include <xen/xen-ops.h>
 
 #include <asm/xen/hypercall.h>
-#include <asm/xen/page.h>
 #include <asm/xen/hypervisor.h>
 
 enum shutdown_state {
diff --git a/drivers/xen/tmem.c b/drivers/xen/tmem.c
index c4211a31612d..239738f944ba 100644
--- a/drivers/xen/tmem.c
+++ b/drivers/xen/tmem.c
@@ -17,8 +17,8 @@
 
 #include <xen/xen.h>
 #include <xen/interface/xen.h>
+#include <xen/page.h>
 #include <asm/xen/hypercall.h>
-#include <asm/xen/page.h>
 #include <asm/xen/hypervisor.h>
 #include <xen/tmem.h>
 
@@ -381,21 +381,15 @@ static int __init xen_tmem_init(void)
 #ifdef CONFIG_FRONTSWAP
 	if (tmem_enabled && frontswap) {
 		char *s = "";
-		struct frontswap_ops *old_ops;
 
 		tmem_frontswap_poolid = -1;
-		old_ops = frontswap_register_ops(&tmem_frontswap_ops);
-		if (IS_ERR(old_ops) || old_ops) {
-			if (IS_ERR(old_ops))
-				return PTR_ERR(old_ops);
-			s = " (WARNING: frontswap_ops overridden)";
-		}
+		frontswap_register_ops(&tmem_frontswap_ops);
 		pr_info("frontswap enabled, RAM provided by Xen Transcendent Memory%s\n",
 			s);
 	}
 #endif
 #ifdef CONFIG_CLEANCACHE
-	BUG_ON(sizeof(struct cleancache_filekey) != sizeof(struct tmem_oid));
+	BUILD_BUG_ON(sizeof(struct cleancache_filekey) != sizeof(struct tmem_oid));
 	if (tmem_enabled && cleancache) {
 		int err;
 
diff --git a/drivers/xen/xen-acpi-cpuhotplug.c b/drivers/xen/xen-acpi-cpuhotplug.c
index 3e62ee4b3b66..f4a369429553 100644
--- a/drivers/xen/xen-acpi-cpuhotplug.c
+++ b/drivers/xen/xen-acpi-cpuhotplug.c
@@ -46,13 +46,7 @@ static int xen_acpi_processor_enable(struct acpi_device *device)
 	unsigned long long value;
 	union acpi_object object = { 0 };
 	struct acpi_buffer buffer = { sizeof(union acpi_object), &object };
-	struct acpi_processor *pr;
-
-	pr = acpi_driver_data(device);
-	if (!pr) {
-		pr_err(PREFIX "Cannot find driver data\n");
-		return -EINVAL;
-	}
+	struct acpi_processor *pr = acpi_driver_data(device);
 
 	if (!strcmp(acpi_device_hid(device), ACPI_PROCESSOR_OBJECT_HID)) {
 		/* Declared with "Processor" statement; match ProcessorID */
@@ -77,7 +71,7 @@ static int xen_acpi_processor_enable(struct acpi_device *device)
 
 	pr->id = xen_pcpu_id(pr->acpi_id);
 
-	if ((int)pr->id < 0)
+	if (invalid_logical_cpuid(pr->id))
 		/* This cpu is not presented at hypervisor, try to hotadd it */
 		if (ACPI_FAILURE(xen_acpi_cpu_hotadd(pr))) {
 			pr_err(PREFIX "Hotadd CPU (acpi_id = %d) failed.\n",
@@ -226,7 +220,7 @@ static acpi_status xen_acpi_cpu_hotadd(struct acpi_processor *pr)
 		return AE_ERROR;
 
 	pr->id = xen_hotadd_cpu(pr);
-	if ((int)pr->id < 0)
+	if (invalid_logical_cpuid(pr->id))
 		return AE_ERROR;
 
 	/*
diff --git a/drivers/xen/xen-scsiback.c b/drivers/xen/xen-scsiback.c
index b7f51504f85a..9eeefd7cad41 100644
--- a/drivers/xen/xen-scsiback.c
+++ b/drivers/xen/xen-scsiback.c
@@ -49,14 +49,10 @@
 
 #include <generated/utsrelease.h>
 
-#include <scsi/scsi.h>
-#include <scsi/scsi_dbg.h>
-#include <scsi/scsi_eh.h>
-#include <scsi/scsi_tcq.h>
+#include <scsi/scsi_host.h> /* SG_ALL */
 
 #include <target/target_core_base.h>
 #include <target/target_core_fabric.h>
-#include <target/target_core_configfs.h>
 #include <target/target_core_fabric_configfs.h>
 
 #include <asm/hypervisor.h>
@@ -204,8 +200,6 @@ static LIST_HEAD(scsiback_free_pages);
 static DEFINE_MUTEX(scsiback_mutex);
 static LIST_HEAD(scsiback_list);
 
-static const struct target_core_fabric_ops scsiback_ops;
-
 static void scsiback_get(struct vscsibk_info *info)
 {
 	atomic_inc(&info->nr_unreplied_reqs);
@@ -400,6 +394,7 @@ static void scsiback_cmd_exec(struct vscsibk_pend *pending_req)
 	memset(se_cmd, 0, sizeof(*se_cmd));
 
 	scsiback_get(pending_req->info);
+	se_cmd->tag = pending_req->rqid;
 	rc = target_submit_cmd_map_sgls(se_cmd, sess, pending_req->cmnd,
 			pending_req->sense_buffer, pending_req->v2p->lun,
 			pending_req->data_len, 0,
@@ -866,7 +861,8 @@ static int scsiback_add_translation_entry(struct vscsibk_info *info,
 	struct list_head *head = &(info->v2p_entry_lists);
 	unsigned long flags;
 	char *lunp;
-	unsigned int lun;
+	unsigned long long unpacked_lun;
+	struct se_lun *se_lun;
 	struct scsiback_tpg *tpg_entry, *tpg = NULL;
 	char *error = "doesn't exist";
 
@@ -877,24 +873,27 @@ static int scsiback_add_translation_entry(struct vscsibk_info *info,
 	}
 	*lunp = 0;
 	lunp++;
-	if (kstrtouint(lunp, 10, &lun) || lun >= TRANSPORT_MAX_LUNS_PER_TPG) {
+	err = kstrtoull(lunp, 10, &unpacked_lun);
+	if (err < 0) {
 		pr_err("lun number not valid: %s\n", lunp);
-		return -EINVAL;
+		return err;
 	}
 
 	mutex_lock(&scsiback_mutex);
 	list_for_each_entry(tpg_entry, &scsiback_list, tv_tpg_list) {
 		if (!strcmp(phy, tpg_entry->tport->tport_name) ||
 		    !strcmp(phy, tpg_entry->param_alias)) {
-			spin_lock(&tpg_entry->se_tpg.tpg_lun_lock);
-			if (tpg_entry->se_tpg.tpg_lun_list[lun]->lun_status ==
-			    TRANSPORT_LUN_STATUS_ACTIVE) {
-				if (!tpg_entry->tpg_nexus)
-					error = "nexus undefined";
-				else
-					tpg = tpg_entry;
+			mutex_lock(&tpg_entry->se_tpg.tpg_lun_mutex);
+			hlist_for_each_entry(se_lun, &tpg_entry->se_tpg.tpg_lun_hlist, link) {
+				if (se_lun->unpacked_lun == unpacked_lun) {
+					if (!tpg_entry->tpg_nexus)
+						error = "nexus undefined";
+					else
+						tpg = tpg_entry;
+					break;
+				}
 			}
-			spin_unlock(&tpg_entry->se_tpg.tpg_lun_lock);
+			mutex_unlock(&tpg_entry->se_tpg.tpg_lun_mutex);
 			break;
 		}
 	}
@@ -906,7 +905,7 @@ static int scsiback_add_translation_entry(struct vscsibk_info *info,
 	mutex_unlock(&scsiback_mutex);
 
 	if (!tpg) {
-		pr_err("%s:%d %s\n", phy, lun, error);
+		pr_err("%s:%llu %s\n", phy, unpacked_lun, error);
 		return -ENODEV;
 	}
 
@@ -934,7 +933,7 @@ static int scsiback_add_translation_entry(struct vscsibk_info *info,
 	kref_init(&new->kref);
 	new->v = *v;
 	new->tpg = tpg;
-	new->lun = lun;
+	new->lun = unpacked_lun;
 	list_add_tail(&new->l, head);
 
 out:
@@ -1254,28 +1253,6 @@ static char *scsiback_dump_proto_id(struct scsiback_tport *tport)
 	return "Unknown";
 }
 
-static u8 scsiback_get_fabric_proto_ident(struct se_portal_group *se_tpg)
-{
-	struct scsiback_tpg *tpg = container_of(se_tpg,
-				struct scsiback_tpg, se_tpg);
-	struct scsiback_tport *tport = tpg->tport;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_fabric_proto_ident(se_tpg);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_fabric_proto_ident(se_tpg);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_fabric_proto_ident(se_tpg);
-	default:
-		pr_err("Unknown tport_proto_id: 0x%02x, using SAS emulation\n",
-			tport->tport_proto_id);
-		break;
-	}
-
-	return sas_get_fabric_proto_ident(se_tpg);
-}
-
 static char *scsiback_get_fabric_wwn(struct se_portal_group *se_tpg)
 {
 	struct scsiback_tpg *tpg = container_of(se_tpg,
@@ -1292,102 +1269,6 @@ static u16 scsiback_get_tag(struct se_portal_group *se_tpg)
 	return tpg->tport_tpgt;
 }
 
-static u32 scsiback_get_default_depth(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static u32
-scsiback_get_pr_transport_id(struct se_portal_group *se_tpg,
-			      struct se_node_acl *se_nacl,
-			      struct t10_pr_registration *pr_reg,
-			      int *format_code,
-			      unsigned char *buf)
-{
-	struct scsiback_tpg *tpg = container_of(se_tpg,
-				struct scsiback_tpg, se_tpg);
-	struct scsiback_tport *tport = tpg->tport;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-					format_code, buf);
-	default:
-		pr_err("Unknown tport_proto_id: 0x%02x, using SAS emulation\n",
-			tport->tport_proto_id);
-		break;
-	}
-
-	return sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
-			format_code, buf);
-}
-
-static u32
-scsiback_get_pr_transport_id_len(struct se_portal_group *se_tpg,
-				  struct se_node_acl *se_nacl,
-				  struct t10_pr_registration *pr_reg,
-				  int *format_code)
-{
-	struct scsiback_tpg *tpg = container_of(se_tpg,
-				struct scsiback_tpg, se_tpg);
-	struct scsiback_tport *tport = tpg->tport;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	case SCSI_PROTOCOL_FCP:
-		return fc_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-					format_code);
-	default:
-		pr_err("Unknown tport_proto_id: 0x%02x, using SAS emulation\n",
-			tport->tport_proto_id);
-		break;
-	}
-
-	return sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
-			format_code);
-}
-
-static char *
-scsiback_parse_pr_out_transport_id(struct se_portal_group *se_tpg,
-				    const char *buf,
-				    u32 *out_tid_len,
-				    char **port_nexus_ptr)
-{
-	struct scsiback_tpg *tpg = container_of(se_tpg,
-				struct scsiback_tpg, se_tpg);
-	struct scsiback_tport *tport = tpg->tport;
-
-	switch (tport->tport_proto_id) {
-	case SCSI_PROTOCOL_SAS:
-		return sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	case SCSI_PROTOCOL_FCP:
-		return fc_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	case SCSI_PROTOCOL_ISCSI:
-		return iscsi_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-					port_nexus_ptr);
-	default:
-		pr_err("Unknown tport_proto_id: 0x%02x, using SAS emulation\n",
-			tport->tport_proto_id);
-		break;
-	}
-
-	return sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
-			port_nexus_ptr);
-}
-
 static struct se_wwn *
 scsiback_make_tport(struct target_fabric_configfs *tf,
 		     struct config_group *group,
@@ -1454,19 +1335,6 @@ static void scsiback_drop_tport(struct se_wwn *wwn)
 	kfree(tport);
 }
 
-static struct se_node_acl *
-scsiback_alloc_fabric_acl(struct se_portal_group *se_tpg)
-{
-	return kzalloc(sizeof(struct se_node_acl), GFP_KERNEL);
-}
-
-static void
-scsiback_release_fabric_acl(struct se_portal_group *se_tpg,
-			     struct se_node_acl *se_nacl)
-{
-	kfree(se_nacl);
-}
-
 static u32 scsiback_tpg_get_inst_index(struct se_portal_group *se_tpg)
 {
 	return 1;
@@ -1525,14 +1393,6 @@ static void scsiback_set_default_node_attrs(struct se_node_acl *nacl)
 {
 }
 
-static u32 scsiback_get_task_tag(struct se_cmd *se_cmd)
-{
-	struct vscsibk_pend *pending_req = container_of(se_cmd,
-				struct vscsibk_pend, se_cmd);
-
-	return pending_req->rqid;
-}
-
 static int scsiback_get_cmd_state(struct se_cmd *se_cmd)
 {
 	return 0;
@@ -1901,8 +1761,7 @@ scsiback_make_tpg(struct se_wwn *wwn,
 	tpg->tport = tport;
 	tpg->tport_tpgt = tpgt;
 
-	ret = core_tpg_register(&scsiback_ops, wwn,
-				&tpg->se_tpg, tpg, TRANSPORT_TPG_TYPE_NORMAL);
+	ret = core_tpg_register(wwn, &tpg->se_tpg, tport->tport_proto_id);
 	if (ret < 0) {
 		kfree(tpg);
 		return NULL;
@@ -1947,23 +1806,15 @@ static const struct target_core_fabric_ops scsiback_ops = {
 	.module				= THIS_MODULE,
 	.name				= "xen-pvscsi",
 	.get_fabric_name		= scsiback_get_fabric_name,
-	.get_fabric_proto_ident		= scsiback_get_fabric_proto_ident,
 	.tpg_get_wwn			= scsiback_get_fabric_wwn,
 	.tpg_get_tag			= scsiback_get_tag,
-	.tpg_get_default_depth		= scsiback_get_default_depth,
-	.tpg_get_pr_transport_id	= scsiback_get_pr_transport_id,
-	.tpg_get_pr_transport_id_len	= scsiback_get_pr_transport_id_len,
-	.tpg_parse_pr_out_transport_id	= scsiback_parse_pr_out_transport_id,
 	.tpg_check_demo_mode		= scsiback_check_true,
 	.tpg_check_demo_mode_cache	= scsiback_check_true,
 	.tpg_check_demo_mode_write_protect = scsiback_check_false,
 	.tpg_check_prod_mode_write_protect = scsiback_check_false,
-	.tpg_alloc_fabric_acl		= scsiback_alloc_fabric_acl,
-	.tpg_release_fabric_acl		= scsiback_release_fabric_acl,
 	.tpg_get_inst_index		= scsiback_tpg_get_inst_index,
 	.check_stop_free		= scsiback_check_stop_free,
 	.release_cmd			= scsiback_release_cmd,
-	.put_session			= NULL,
 	.shutdown_session		= scsiback_shutdown_session,
 	.close_session			= scsiback_close_session,
 	.sess_get_index			= scsiback_sess_get_index,
@@ -1971,7 +1822,6 @@ static const struct target_core_fabric_ops scsiback_ops = {
 	.write_pending			= scsiback_write_pending,
 	.write_pending_status		= scsiback_write_pending_status,
 	.set_default_node_attributes	= scsiback_set_default_node_attrs,
-	.get_task_tag			= scsiback_get_task_tag,
 	.get_cmd_state			= scsiback_get_cmd_state,
 	.queue_data_in			= scsiback_queue_data_in,
 	.queue_status			= scsiback_queue_status,
@@ -1986,12 +1836,6 @@ static const struct target_core_fabric_ops scsiback_ops = {
 	.fabric_drop_tpg		= scsiback_drop_tpg,
 	.fabric_post_link		= scsiback_port_link,
 	.fabric_pre_unlink		= scsiback_port_unlink,
-	.fabric_make_np			= NULL,
-	.fabric_drop_np			= NULL,
-#if 0
-	.fabric_make_nodeacl		= scsiback_make_nodeacl,
-	.fabric_drop_nodeacl		= scsiback_drop_nodeacl,
-#endif
 
 	.tfc_wwn_attrs			= scsiback_wwn_attrs,
 	.tfc_tpg_base_attrs		= scsiback_tpg_attrs,
diff --git a/drivers/xen/xenbus/xenbus_client.c b/drivers/xen/xenbus/xenbus_client.c
index 96b2011d25f3..9ad327238ba9 100644
--- a/drivers/xen/xenbus/xenbus_client.c
+++ b/drivers/xen/xenbus/xenbus_client.c
@@ -37,7 +37,7 @@
 #include <linux/vmalloc.h>
 #include <linux/export.h>
 #include <asm/xen/hypervisor.h>
-#include <asm/xen/page.h>
+#include <xen/page.h>
 #include <xen/interface/xen.h>
 #include <xen/interface/event_channel.h>
 #include <xen/balloon.h>
@@ -379,16 +379,16 @@ int xenbus_grant_ring(struct xenbus_device *dev, void *vaddr,
 	int i, j;
 
 	for (i = 0; i < nr_pages; i++) {
-		unsigned long addr = (unsigned long)vaddr +
-			(PAGE_SIZE * i);
 		err = gnttab_grant_foreign_access(dev->otherend_id,
-						  virt_to_mfn(addr), 0);
+						  virt_to_mfn(vaddr), 0);
 		if (err < 0) {
 			xenbus_dev_fatal(dev, err,
 					 "granting access to ring page");
 			goto fail;
 		}
 		grefs[i] = err;
+
+		vaddr = vaddr + PAGE_SIZE;
 	}
 
 	return 0;
diff --git a/drivers/xen/xenbus/xenbus_probe.c b/drivers/xen/xenbus/xenbus_probe.c
index 5390a674b5e3..4308fb3cf7c2 100644
--- a/drivers/xen/xenbus/xenbus_probe.c
+++ b/drivers/xen/xenbus/xenbus_probe.c
@@ -742,7 +742,7 @@ static int xenbus_resume_cb(struct notifier_block *nb,
 	int err = 0;
 
 	if (xen_hvm_domain()) {
-		uint64_t v;
+		uint64_t v = 0;
 
 		err = hvm_get_parameter(HVM_PARAM_STORE_EVTCHN, &v);
 		if (!err && v)